Merge tag 'v4.5' into next

Merge with Linux 4.5 to get PROPERTY_ENTRY_INTEGER() that is needed to
fix pxa/raumfeld rotary encoder properties.

5522 files changed, 338739 insertions, 173407 deletions

diff --git a/drivers/Makefile b/drivers/Makefile
index 795d0ca714bf..8f5d076baeb0 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -106,7 +106,7 @@ obj-y				+= i2c/ media/
 obj-$(CONFIG_PPS)		+= pps/
 obj-$(CONFIG_PTP_1588_CLOCK)	+= ptp/
 obj-$(CONFIG_W1)		+= w1/
-obj-$(CONFIG_POWER_SUPPLY)	+= power/
+obj-y				+= power/
 obj-$(CONFIG_HWMON)		+= hwmon/
 obj-$(CONFIG_THERMAL)		+= thermal/
 obj-$(CONFIG_WATCHDOG)		+= watchdog/
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index 5eef4cb4f70e..82b96ee8624c 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -58,14 +58,25 @@ config ACPI_CCA_REQUIRED
 	bool
 
 config ACPI_DEBUGGER
-	bool "AML debugger interface (EXPERIMENTAL)"
+	bool "AML debugger interface"
 	select ACPI_DEBUG
 	help
-	  Enable in-kernel debugging of AML facilities: statistics, internal
-	  object dump, single step control method execution.
+	  Enable in-kernel debugging of AML facilities: statistics,
+	  internal object dump, single step control method execution.
 	  This is still under development, currently enabling this only
 	  results in the compilation of the ACPICA debugger files.
 
+if ACPI_DEBUGGER
+
+config ACPI_DEBUGGER_USER
+	tristate "Userspace debugger accessiblity"
+	depends on DEBUG_FS
+	help
+	  Export /sys/kernel/debug/acpi/acpidbg for userspace utilities
+	  to access the debugger functionalities.
+
+endif
+
 config ACPI_SLEEP
 	bool
 	depends on SUSPEND || HIBERNATION
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 675eaf337178..cb648a49543a 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -8,13 +8,13 @@ ccflags-$(CONFIG_ACPI_DEBUG)	+= -DACPI_DEBUG_OUTPUT
 #
 # ACPI Boot-Time Table Parsing
 #
-obj-y				+= tables.o
+obj-$(CONFIG_ACPI)		+= tables.o
 obj-$(CONFIG_X86)		+= blacklist.o
 
 #
 # ACPI Core Subsystem (Interpreter)
 #
-obj-y				+= acpi.o \
+obj-$(CONFIG_ACPI)		+= acpi.o \
 					acpica/
 
 # All the builtin files are in the "acpi." module_param namespace.
@@ -66,10 +66,10 @@ obj-$(CONFIG_ACPI_FAN)		+= fan.o
 obj-$(CONFIG_ACPI_VIDEO)	+= video.o
 obj-$(CONFIG_ACPI_PCI_SLOT)	+= pci_slot.o
 obj-$(CONFIG_ACPI_PROCESSOR)	+= processor.o
-obj-y				+= container.o
+obj-$(CONFIG_ACPI)		+= container.o
 obj-$(CONFIG_ACPI_THERMAL)	+= thermal.o
 obj-$(CONFIG_ACPI_NFIT)		+= nfit.o
-obj-y				+= acpi_memhotplug.o
+obj-$(CONFIG_ACPI)		+= acpi_memhotplug.o
 obj-$(CONFIG_ACPI_HOTPLUG_IOAPIC) += ioapic.o
 obj-$(CONFIG_ACPI_BATTERY)	+= battery.o
 obj-$(CONFIG_ACPI_SBS)		+= sbshc.o
@@ -79,6 +79,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_DEBUGGER_USER) += acpi_dbg.o
 
 # processor has its own "processor." module_param namespace
 processor-y			:= processor_driver.o
diff --git a/drivers/acpi/acpi_apd.c b/drivers/acpi/acpi_apd.c
index a450e7af877c..d507cf6deda0 100644
--- a/drivers/acpi/acpi_apd.c
+++ b/drivers/acpi/acpi_apd.c
@@ -51,7 +51,7 @@ struct apd_private_data {
 	const struct apd_device_desc *dev_desc;
 };
 
-#ifdef CONFIG_X86_AMD_PLATFORM_DEVICE
+#if defined(CONFIG_X86_AMD_PLATFORM_DEVICE) || defined(CONFIG_ARM64)
 #define APD_ADDR(desc)	((unsigned long)&desc)
 
 static int acpi_apd_setup(struct apd_private_data *pdata)
@@ -71,6 +71,7 @@ static int acpi_apd_setup(struct apd_private_data *pdata)
 	return 0;
 }
 
+#ifdef CONFIG_X86_AMD_PLATFORM_DEVICE
 static struct apd_device_desc cz_i2c_desc = {
 	.setup = acpi_apd_setup,
 	.fixed_clk_rate = 133000000,
@@ -80,6 +81,14 @@ static struct apd_device_desc cz_uart_desc = {
 	.setup = acpi_apd_setup,
 	.fixed_clk_rate = 48000000,
 };
+#endif
+
+#ifdef CONFIG_ARM64
+static struct apd_device_desc xgene_i2c_desc = {
+	.setup = acpi_apd_setup,
+	.fixed_clk_rate = 100000000,
+};
+#endif
 
 #else
 
@@ -132,9 +141,14 @@ static int acpi_apd_create_device(struct acpi_device *adev,
 
 static const struct acpi_device_id acpi_apd_device_ids[] = {
 	/* Generic apd devices */
+#ifdef CONFIG_X86_AMD_PLATFORM_DEVICE
 	{ "AMD0010", APD_ADDR(cz_i2c_desc) },
 	{ "AMD0020", APD_ADDR(cz_uart_desc) },
 	{ "AMD0030", },
+#endif
+#ifdef CONFIG_ARM64
+	{ "APMC0D0F", APD_ADDR(xgene_i2c_desc) },
+#endif
 	{ }
 };
 
diff --git a/drivers/acpi/acpi_dbg.c b/drivers/acpi/acpi_dbg.c
new file mode 100644
index 000000000000..15e4604efba7
--- /dev/null
+++ b/drivers/acpi/acpi_dbg.c
@@ -0,0 +1,804 @@
+/*
+ * ACPI AML interfacing support
+ *
+ * Copyright (C) 2015, Intel Corporation
+ * Authors: Lv Zheng <lv.zheng@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 DEBUG */
+#define pr_fmt(fmt) "ACPI : AML: " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/wait.h>
+#include <linux/poll.h>
+#include <linux/sched.h>
+#include <linux/kthread.h>
+#include <linux/proc_fs.h>
+#include <linux/debugfs.h>
+#include <linux/circ_buf.h>
+#include <linux/acpi.h>
+#include "internal.h"
+
+#define ACPI_AML_BUF_ALIGN	(sizeof (acpi_size))
+#define ACPI_AML_BUF_SIZE	PAGE_SIZE
+
+#define circ_count(circ) \
+	(CIRC_CNT((circ)->head, (circ)->tail, ACPI_AML_BUF_SIZE))
+#define circ_count_to_end(circ) \
+	(CIRC_CNT_TO_END((circ)->head, (circ)->tail, ACPI_AML_BUF_SIZE))
+#define circ_space(circ) \
+	(CIRC_SPACE((circ)->head, (circ)->tail, ACPI_AML_BUF_SIZE))
+#define circ_space_to_end(circ) \
+	(CIRC_SPACE_TO_END((circ)->head, (circ)->tail, ACPI_AML_BUF_SIZE))
+
+#define ACPI_AML_OPENED		0x0001
+#define ACPI_AML_CLOSED		0x0002
+#define ACPI_AML_IN_USER	0x0004 /* user space is writing cmd */
+#define ACPI_AML_IN_KERN	0x0008 /* kernel space is reading cmd */
+#define ACPI_AML_OUT_USER	0x0010 /* user space is reading log */
+#define ACPI_AML_OUT_KERN	0x0020 /* kernel space is writing log */
+#define ACPI_AML_USER		(ACPI_AML_IN_USER | ACPI_AML_OUT_USER)
+#define ACPI_AML_KERN		(ACPI_AML_IN_KERN | ACPI_AML_OUT_KERN)
+#define ACPI_AML_BUSY		(ACPI_AML_USER | ACPI_AML_KERN)
+#define ACPI_AML_OPEN		(ACPI_AML_OPENED | ACPI_AML_CLOSED)
+
+struct acpi_aml_io {
+	wait_queue_head_t wait;
+	unsigned long flags;
+	unsigned long users;
+	struct mutex lock;
+	struct task_struct *thread;
+	char out_buf[ACPI_AML_BUF_SIZE] __aligned(ACPI_AML_BUF_ALIGN);
+	struct circ_buf out_crc;
+	char in_buf[ACPI_AML_BUF_SIZE] __aligned(ACPI_AML_BUF_ALIGN);
+	struct circ_buf in_crc;
+	acpi_osd_exec_callback function;
+	void *context;
+	unsigned long usages;
+};
+
+static struct acpi_aml_io acpi_aml_io;
+static bool acpi_aml_initialized;
+static struct file *acpi_aml_active_reader;
+static struct dentry *acpi_aml_dentry;
+
+static inline bool __acpi_aml_running(void)
+{
+	return acpi_aml_io.thread ? true : false;
+}
+
+static inline bool __acpi_aml_access_ok(unsigned long flag)
+{
+	/*
+	 * The debugger interface is in opened state (OPENED && !CLOSED),
+	 * then it is allowed to access the debugger buffers from either
+	 * user space or the kernel space.
+	 * In addition, for the kernel space, only the debugger thread
+	 * (thread ID matched) is allowed to access.
+	 */
+	if (!(acpi_aml_io.flags & ACPI_AML_OPENED) ||
+	    (acpi_aml_io.flags & ACPI_AML_CLOSED) ||
+	    !__acpi_aml_running())
+		return false;
+	if ((flag & ACPI_AML_KERN) &&
+	    current != acpi_aml_io.thread)
+		return false;
+	return true;
+}
+
+static inline bool __acpi_aml_readable(struct circ_buf *circ, unsigned long flag)
+{
+	/*
+	 * Another read is not in progress and there is data in buffer
+	 * available for read.
+	 */
+	if (!(acpi_aml_io.flags & flag) && circ_count(circ))
+		return true;
+	return false;
+}
+
+static inline bool __acpi_aml_writable(struct circ_buf *circ, unsigned long flag)
+{
+	/*
+	 * Another write is not in progress and there is buffer space
+	 * available for write.
+	 */
+	if (!(acpi_aml_io.flags & flag) && circ_space(circ))
+		return true;
+	return false;
+}
+
+static inline bool __acpi_aml_busy(void)
+{
+	if (acpi_aml_io.flags & ACPI_AML_BUSY)
+		return true;
+	return false;
+}
+
+static inline bool __acpi_aml_opened(void)
+{
+	if (acpi_aml_io.flags & ACPI_AML_OPEN)
+		return true;
+	return false;
+}
+
+static inline bool __acpi_aml_used(void)
+{
+	return acpi_aml_io.usages ? true : false;
+}
+
+static inline bool acpi_aml_running(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = __acpi_aml_running();
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_busy(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = __acpi_aml_busy();
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_used(void)
+{
+	bool ret;
+
+	/*
+	 * The usage count is prepared to avoid race conditions between the
+	 * starts and the stops of the debugger thread.
+	 */
+	mutex_lock(&acpi_aml_io.lock);
+	ret = __acpi_aml_used();
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_kern_readable(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = !__acpi_aml_access_ok(ACPI_AML_IN_KERN) ||
+	      __acpi_aml_readable(&acpi_aml_io.in_crc, ACPI_AML_IN_KERN);
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_kern_writable(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = !__acpi_aml_access_ok(ACPI_AML_OUT_KERN) ||
+	      __acpi_aml_writable(&acpi_aml_io.out_crc, ACPI_AML_OUT_KERN);
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_user_readable(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = !__acpi_aml_access_ok(ACPI_AML_OUT_USER) ||
+	      __acpi_aml_readable(&acpi_aml_io.out_crc, ACPI_AML_OUT_USER);
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static bool acpi_aml_user_writable(void)
+{
+	bool ret;
+
+	mutex_lock(&acpi_aml_io.lock);
+	ret = !__acpi_aml_access_ok(ACPI_AML_IN_USER) ||
+	      __acpi_aml_writable(&acpi_aml_io.in_crc, ACPI_AML_IN_USER);
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static int acpi_aml_lock_write(struct circ_buf *circ, unsigned long flag)
+{
+	int ret = 0;
+
+	mutex_lock(&acpi_aml_io.lock);
+	if (!__acpi_aml_access_ok(flag)) {
+		ret = -EFAULT;
+		goto out;
+	}
+	if (!__acpi_aml_writable(circ, flag)) {
+		ret = -EAGAIN;
+		goto out;
+	}
+	acpi_aml_io.flags |= flag;
+out:
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static int acpi_aml_lock_read(struct circ_buf *circ, unsigned long flag)
+{
+	int ret = 0;
+
+	mutex_lock(&acpi_aml_io.lock);
+	if (!__acpi_aml_access_ok(flag)) {
+		ret = -EFAULT;
+		goto out;
+	}
+	if (!__acpi_aml_readable(circ, flag)) {
+		ret = -EAGAIN;
+		goto out;
+	}
+	acpi_aml_io.flags |= flag;
+out:
+	mutex_unlock(&acpi_aml_io.lock);
+	return ret;
+}
+
+static void acpi_aml_unlock_fifo(unsigned long flag, bool wakeup)
+{
+	mutex_lock(&acpi_aml_io.lock);
+	acpi_aml_io.flags &= ~flag;
+	if (wakeup)
+		wake_up_interruptible(&acpi_aml_io.wait);
+	mutex_unlock(&acpi_aml_io.lock);
+}
+
+static int acpi_aml_write_kern(const char *buf, int len)
+{
+	int ret;
+	struct circ_buf *crc = &acpi_aml_io.out_crc;
+	int n;
+	char *p;
+
+	ret = acpi_aml_lock_write(crc, ACPI_AML_OUT_KERN);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+	/* sync tail before inserting logs */
+	smp_mb();
+	p = &crc->buf[crc->head];
+	n = min(len, circ_space_to_end(crc));
+	memcpy(p, buf, n);
+	/* sync head after inserting logs */
+	smp_wmb();
+	crc->head = (crc->head + n) & (ACPI_AML_BUF_SIZE - 1);
+	acpi_aml_unlock_fifo(ACPI_AML_OUT_KERN, true);
+	return n;
+}
+
+static int acpi_aml_readb_kern(void)
+{
+	int ret;
+	struct circ_buf *crc = &acpi_aml_io.in_crc;
+	char *p;
+
+	ret = acpi_aml_lock_read(crc, ACPI_AML_IN_KERN);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+	/* sync head before removing cmds */
+	smp_rmb();
+	p = &crc->buf[crc->tail];
+	ret = (int)*p;
+	/* sync tail before inserting cmds */
+	smp_mb();
+	crc->tail = (crc->tail + 1) & (ACPI_AML_BUF_SIZE - 1);
+	acpi_aml_unlock_fifo(ACPI_AML_IN_KERN, true);
+	return ret;
+}
+
+/*
+ * acpi_aml_write_log() - Capture debugger output
+ * @msg: the debugger output
+ *
+ * This function should be used to implement acpi_os_printf() to filter out
+ * the debugger output and store the output into the debugger interface
+ * buffer. Return the size of stored logs or errno.
+ */
+static ssize_t acpi_aml_write_log(const char *msg)
+{
+	int ret = 0;
+	int count = 0, size = 0;
+
+	if (!acpi_aml_initialized)
+		return -ENODEV;
+	if (msg)
+		count = strlen(msg);
+	while (count > 0) {
+again:
+		ret = acpi_aml_write_kern(msg + size, count);
+		if (ret == -EAGAIN) {
+			ret = wait_event_interruptible(acpi_aml_io.wait,
+				acpi_aml_kern_writable());
+			/*
+			 * We need to retry when the condition
+			 * becomes true.
+			 */
+			if (ret == 0)
+				goto again;
+			break;
+		}
+		if (IS_ERR_VALUE(ret))
+			break;
+		size += ret;
+		count -= ret;
+	}
+	return size > 0 ? size : ret;
+}
+
+/*
+ * acpi_aml_read_cmd() - Capture debugger input
+ * @msg: the debugger input
+ * @size: the size of the debugger input
+ *
+ * This function should be used to implement acpi_os_get_line() to capture
+ * the debugger input commands and store the input commands into the
+ * debugger interface buffer. Return the size of stored commands or errno.
+ */
+static ssize_t acpi_aml_read_cmd(char *msg, size_t count)
+{
+	int ret = 0;
+	int size = 0;
+
+	/*
+	 * This is ensured by the running fact of the debugger thread
+	 * unless a bug is introduced.
+	 */
+	BUG_ON(!acpi_aml_initialized);
+	while (count > 0) {
+again:
+		/*
+		 * Check each input byte to find the end of the command.
+		 */
+		ret = acpi_aml_readb_kern();
+		if (ret == -EAGAIN) {
+			ret = wait_event_interruptible(acpi_aml_io.wait,
+				acpi_aml_kern_readable());
+			/*
+			 * We need to retry when the condition becomes
+			 * true.
+			 */
+			if (ret == 0)
+				goto again;
+		}
+		if (IS_ERR_VALUE(ret))
+			break;
+		*(msg + size) = (char)ret;
+		size++;
+		count--;
+		if (ret == '\n') {
+			/*
+			 * acpi_os_get_line() requires a zero terminated command
+			 * string.
+			 */
+			*(msg + size - 1) = '\0';
+			break;
+		}
+	}
+	return size > 0 ? size : ret;
+}
+
+static int acpi_aml_thread(void *unsed)
+{
+	acpi_osd_exec_callback function = NULL;
+	void *context;
+
+	mutex_lock(&acpi_aml_io.lock);
+	if (acpi_aml_io.function) {
+		acpi_aml_io.usages++;
+		function = acpi_aml_io.function;
+		context = acpi_aml_io.context;
+	}
+	mutex_unlock(&acpi_aml_io.lock);
+
+	if (function)
+		function(context);
+
+	mutex_lock(&acpi_aml_io.lock);
+	acpi_aml_io.usages--;
+	if (!__acpi_aml_used()) {
+		acpi_aml_io.thread = NULL;
+		wake_up(&acpi_aml_io.wait);
+	}
+	mutex_unlock(&acpi_aml_io.lock);
+
+	return 0;
+}
+
+/*
+ * acpi_aml_create_thread() - Create AML debugger thread
+ * @function: the debugger thread callback
+ * @context: the context to be passed to the debugger thread
+ *
+ * This function should be used to implement acpi_os_execute() which is
+ * used by the ACPICA debugger to create the debugger thread.
+ */
+static int acpi_aml_create_thread(acpi_osd_exec_callback function, void *context)
+{
+	struct task_struct *t;
+
+	mutex_lock(&acpi_aml_io.lock);
+	acpi_aml_io.function = function;
+	acpi_aml_io.context = context;
+	mutex_unlock(&acpi_aml_io.lock);
+
+	t = kthread_create(acpi_aml_thread, NULL, "aml");
+	if (IS_ERR(t)) {
+		pr_err("Failed to create AML debugger thread.\n");
+		return PTR_ERR(t);
+	}
+
+	mutex_lock(&acpi_aml_io.lock);
+	acpi_aml_io.thread = t;
+	acpi_set_debugger_thread_id((acpi_thread_id)(unsigned long)t);
+	wake_up_process(t);
+	mutex_unlock(&acpi_aml_io.lock);
+	return 0;
+}
+
+static int acpi_aml_wait_command_ready(bool single_step,
+				       char *buffer, size_t length)
+{
+	acpi_status status;
+
+	if (single_step)
+		acpi_os_printf("\n%1c ", ACPI_DEBUGGER_EXECUTE_PROMPT);
+	else
+		acpi_os_printf("\n%1c ", ACPI_DEBUGGER_COMMAND_PROMPT);
+
+	status = acpi_os_get_line(buffer, length, NULL);
+	if (ACPI_FAILURE(status))
+		return -EINVAL;
+	return 0;
+}
+
+static int acpi_aml_notify_command_complete(void)
+{
+	return 0;
+}
+
+static int acpi_aml_open(struct inode *inode, struct file *file)
+{
+	int ret = 0;
+	acpi_status status;
+
+	mutex_lock(&acpi_aml_io.lock);
+	/*
+	 * The debugger interface is being closed, no new user is allowed
+	 * during this period.
+	 */
+	if (acpi_aml_io.flags & ACPI_AML_CLOSED) {
+		ret = -EBUSY;
+		goto err_lock;
+	}
+	if ((file->f_flags & O_ACCMODE) != O_WRONLY) {
+		/*
+		 * Only one reader is allowed to initiate the debugger
+		 * thread.
+		 */
+		if (acpi_aml_active_reader) {
+			ret = -EBUSY;
+			goto err_lock;
+		} else {
+			pr_debug("Opening debugger reader.\n");
+			acpi_aml_active_reader = file;
+		}
+	} else {
+		/*
+		 * No writer is allowed unless the debugger thread is
+		 * ready.
+		 */
+		if (!(acpi_aml_io.flags & ACPI_AML_OPENED)) {
+			ret = -ENODEV;
+			goto err_lock;
+		}
+	}
+	if (acpi_aml_active_reader == file) {
+		pr_debug("Opening debugger interface.\n");
+		mutex_unlock(&acpi_aml_io.lock);
+
+		pr_debug("Initializing debugger thread.\n");
+		status = acpi_initialize_debugger();
+		if (ACPI_FAILURE(status)) {
+			pr_err("Failed to initialize debugger.\n");
+			ret = -EINVAL;
+			goto err_exit;
+		}
+		pr_debug("Debugger thread initialized.\n");
+
+		mutex_lock(&acpi_aml_io.lock);
+		acpi_aml_io.flags |= ACPI_AML_OPENED;
+		acpi_aml_io.out_crc.head = acpi_aml_io.out_crc.tail = 0;
+		acpi_aml_io.in_crc.head = acpi_aml_io.in_crc.tail = 0;
+		pr_debug("Debugger interface opened.\n");
+	}
+	acpi_aml_io.users++;
+err_lock:
+	if (IS_ERR_VALUE(ret)) {
+		if (acpi_aml_active_reader == file)
+			acpi_aml_active_reader = NULL;
+	}
+	mutex_unlock(&acpi_aml_io.lock);
+err_exit:
+	return ret;
+}
+
+static int acpi_aml_release(struct inode *inode, struct file *file)
+{
+	mutex_lock(&acpi_aml_io.lock);
+	acpi_aml_io.users--;
+	if (file == acpi_aml_active_reader) {
+		pr_debug("Closing debugger reader.\n");
+		acpi_aml_active_reader = NULL;
+
+		pr_debug("Closing debugger interface.\n");
+		acpi_aml_io.flags |= ACPI_AML_CLOSED;
+
+		/*
+		 * Wake up all user space/kernel space blocked
+		 * readers/writers.
+		 */
+		wake_up_interruptible(&acpi_aml_io.wait);
+		mutex_unlock(&acpi_aml_io.lock);
+		/*
+		 * Wait all user space/kernel space readers/writers to
+		 * stop so that ACPICA command loop of the debugger thread
+		 * should fail all its command line reads after this point.
+		 */
+		wait_event(acpi_aml_io.wait, !acpi_aml_busy());
+
+		/*
+		 * Then we try to terminate the debugger thread if it is
+		 * not terminated.
+		 */
+		pr_debug("Terminating debugger thread.\n");
+		acpi_terminate_debugger();
+		wait_event(acpi_aml_io.wait, !acpi_aml_used());
+		pr_debug("Debugger thread terminated.\n");
+
+		mutex_lock(&acpi_aml_io.lock);
+		acpi_aml_io.flags &= ~ACPI_AML_OPENED;
+	}
+	if (acpi_aml_io.users == 0) {
+		pr_debug("Debugger interface closed.\n");
+		acpi_aml_io.flags &= ~ACPI_AML_CLOSED;
+	}
+	mutex_unlock(&acpi_aml_io.lock);
+	return 0;
+}
+
+static int acpi_aml_read_user(char __user *buf, int len)
+{
+	int ret;
+	struct circ_buf *crc = &acpi_aml_io.out_crc;
+	int n;
+	char *p;
+
+	ret = acpi_aml_lock_read(crc, ACPI_AML_OUT_USER);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+	/* sync head before removing logs */
+	smp_rmb();
+	p = &crc->buf[crc->tail];
+	n = min(len, circ_count_to_end(crc));
+	if (copy_to_user(buf, p, n)) {
+		ret = -EFAULT;
+		goto out;
+	}
+	/* sync tail after removing logs */
+	smp_mb();
+	crc->tail = (crc->tail + n) & (ACPI_AML_BUF_SIZE - 1);
+	ret = n;
+out:
+	acpi_aml_unlock_fifo(ACPI_AML_OUT_USER, !IS_ERR_VALUE(ret));
+	return ret;
+}
+
+static ssize_t acpi_aml_read(struct file *file, char __user *buf,
+			     size_t count, loff_t *ppos)
+{
+	int ret = 0;
+	int size = 0;
+
+	if (!count)
+		return 0;
+	if (!access_ok(VERIFY_WRITE, buf, count))
+		return -EFAULT;
+
+	while (count > 0) {
+again:
+		ret = acpi_aml_read_user(buf + size, count);
+		if (ret == -EAGAIN) {
+			if (file->f_flags & O_NONBLOCK)
+				break;
+			else {
+				ret = wait_event_interruptible(acpi_aml_io.wait,
+					acpi_aml_user_readable());
+				/*
+				 * We need to retry when the condition
+				 * becomes true.
+				 */
+				if (ret == 0)
+					goto again;
+			}
+		}
+		if (IS_ERR_VALUE(ret)) {
+			if (!acpi_aml_running())
+				ret = 0;
+			break;
+		}
+		if (ret) {
+			size += ret;
+			count -= ret;
+			*ppos += ret;
+			break;
+		}
+	}
+	return size > 0 ? size : ret;
+}
+
+static int acpi_aml_write_user(const char __user *buf, int len)
+{
+	int ret;
+	struct circ_buf *crc = &acpi_aml_io.in_crc;
+	int n;
+	char *p;
+
+	ret = acpi_aml_lock_write(crc, ACPI_AML_IN_USER);
+	if (IS_ERR_VALUE(ret))
+		return ret;
+	/* sync tail before inserting cmds */
+	smp_mb();
+	p = &crc->buf[crc->head];
+	n = min(len, circ_space_to_end(crc));
+	if (copy_from_user(p, buf, n)) {
+		ret = -EFAULT;
+		goto out;
+	}
+	/* sync head after inserting cmds */
+	smp_wmb();
+	crc->head = (crc->head + n) & (ACPI_AML_BUF_SIZE - 1);
+	ret = n;
+out:
+	acpi_aml_unlock_fifo(ACPI_AML_IN_USER, !IS_ERR_VALUE(ret));
+	return n;
+}
+
+static ssize_t acpi_aml_write(struct file *file, const char __user *buf,
+			      size_t count, loff_t *ppos)
+{
+	int ret = 0;
+	int size = 0;
+
+	if (!count)
+		return 0;
+	if (!access_ok(VERIFY_READ, buf, count))
+		return -EFAULT;
+
+	while (count > 0) {
+again:
+		ret = acpi_aml_write_user(buf + size, count);
+		if (ret == -EAGAIN) {
+			if (file->f_flags & O_NONBLOCK)
+				break;
+			else {
+				ret = wait_event_interruptible(acpi_aml_io.wait,
+					acpi_aml_user_writable());
+				/*
+				 * We need to retry when the condition
+				 * becomes true.
+				 */
+				if (ret == 0)
+					goto again;
+			}
+		}
+		if (IS_ERR_VALUE(ret)) {
+			if (!acpi_aml_running())
+				ret = 0;
+			break;
+		}
+		if (ret) {
+			size += ret;
+			count -= ret;
+			*ppos += ret;
+		}
+	}
+	return size > 0 ? size : ret;
+}
+
+static unsigned int acpi_aml_poll(struct file *file, poll_table *wait)
+{
+	int masks = 0;
+
+	poll_wait(file, &acpi_aml_io.wait, wait);
+	if (acpi_aml_user_readable())
+		masks |= POLLIN | POLLRDNORM;
+	if (acpi_aml_user_writable())
+		masks |= POLLOUT | POLLWRNORM;
+
+	return masks;
+}
+
+static const struct file_operations acpi_aml_operations = {
+	.read		= acpi_aml_read,
+	.write		= acpi_aml_write,
+	.poll		= acpi_aml_poll,
+	.open		= acpi_aml_open,
+	.release	= acpi_aml_release,
+	.llseek		= generic_file_llseek,
+};
+
+static const struct acpi_debugger_ops acpi_aml_debugger = {
+	.create_thread		 = acpi_aml_create_thread,
+	.read_cmd		 = acpi_aml_read_cmd,
+	.write_log		 = acpi_aml_write_log,
+	.wait_command_ready	 = acpi_aml_wait_command_ready,
+	.notify_command_complete = acpi_aml_notify_command_complete,
+};
+
+int __init acpi_aml_init(void)
+{
+	int ret = 0;
+
+	if (!acpi_debugfs_dir) {
+		ret = -ENOENT;
+		goto err_exit;
+	}
+
+	/* Initialize AML IO interface */
+	mutex_init(&acpi_aml_io.lock);
+	init_waitqueue_head(&acpi_aml_io.wait);
+	acpi_aml_io.out_crc.buf = acpi_aml_io.out_buf;
+	acpi_aml_io.in_crc.buf = acpi_aml_io.in_buf;
+	acpi_aml_dentry = debugfs_create_file("acpidbg",
+					      S_IFREG | S_IRUGO | S_IWUSR,
+					      acpi_debugfs_dir, NULL,
+					      &acpi_aml_operations);
+	if (acpi_aml_dentry == NULL) {
+		ret = -ENODEV;
+		goto err_exit;
+	}
+	ret = acpi_register_debugger(THIS_MODULE, &acpi_aml_debugger);
+	if (ret)
+		goto err_fs;
+	acpi_aml_initialized = true;
+
+err_fs:
+	if (ret) {
+		debugfs_remove(acpi_aml_dentry);
+		acpi_aml_dentry = NULL;
+	}
+err_exit:
+	return ret;
+}
+
+void __exit acpi_aml_exit(void)
+{
+	if (acpi_aml_initialized) {
+		acpi_unregister_debugger(&acpi_aml_debugger);
+		if (acpi_aml_dentry) {
+			debugfs_remove(acpi_aml_dentry);
+			acpi_aml_dentry = NULL;
+		}
+		acpi_aml_initialized = false;
+	}
+}
+
+module_init(acpi_aml_init);
+module_exit(acpi_aml_exit);
+
+MODULE_AUTHOR("Lv Zheng");
+MODULE_DESCRIPTION("ACPI debugger userspace IO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index f9e0d09f7c66..0872d5fecb82 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -15,8 +15,10 @@
 #include <linux/clk-provider.h>
 #include <linux/err.h>
 #include <linux/io.h>
+#include <linux/mutex.h>
 #include <linux/platform_device.h>
 #include <linux/platform_data/clk-lpss.h>
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 #include <linux/delay.h>
 
@@ -26,6 +28,10 @@ ACPI_MODULE_NAME("acpi_lpss");
 
 #ifdef CONFIG_X86_INTEL_LPSS
 
+#include <asm/cpu_device_id.h>
+#include <asm/iosf_mbi.h>
+#include <asm/pmc_atom.h>
+
 #define LPSS_ADDR(desc) ((unsigned long)&desc)
 
 #define LPSS_CLK_SIZE	0x04
@@ -71,7 +77,7 @@ struct lpss_device_desc {
 	void (*setup)(struct lpss_private_data *pdata);
 };
 
-static struct lpss_device_desc lpss_dma_desc = {
+static const struct lpss_device_desc lpss_dma_desc = {
 	.flags = LPSS_CLK,
 };
 
@@ -84,6 +90,23 @@ struct lpss_private_data {
 	u32 prv_reg_ctx[LPSS_PRV_REG_COUNT];
 };
 
+/* LPSS run time quirks */
+static unsigned int lpss_quirks;
+
+/*
+ * LPSS_QUIRK_ALWAYS_POWER_ON: override power state for LPSS DMA device.
+ *
+ * The LPSS DMA controller has neither _PS0 nor _PS3 method. Moreover
+ * it can be powered off automatically whenever the last LPSS device goes down.
+ * In case of no power any access to the DMA controller will hang the system.
+ * The behaviour is reproduced on some HP laptops based on Intel BayTrail as
+ * well as on ASuS T100TA transformer.
+ *
+ * This quirk overrides power state of entire LPSS island to keep DMA powered
+ * on whenever we have at least one other device in use.
+ */
+#define LPSS_QUIRK_ALWAYS_POWER_ON	BIT(0)
+
 /* UART Component Parameter Register */
 #define LPSS_UART_CPR			0xF4
 #define LPSS_UART_CPR_AFCE		BIT(4)
@@ -196,13 +219,21 @@ static const struct lpss_device_desc bsw_i2c_dev_desc = {
 	.setup = byt_i2c_setup,
 };
 
-static struct lpss_device_desc bsw_spi_dev_desc = {
+static const struct lpss_device_desc bsw_spi_dev_desc = {
 	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX
 			| LPSS_NO_D3_DELAY,
 	.prv_offset = 0x400,
 	.setup = lpss_deassert_reset,
 };
 
+#define ICPU(model)	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_ANY, }
+
+static const struct x86_cpu_id lpss_cpu_ids[] = {
+	ICPU(0x37),	/* Valleyview, Bay Trail */
+	ICPU(0x4c),	/* Braswell, Cherry Trail */
+	{}
+};
+
 #else
 
 #define LPSS_ADDR(desc) (0UL)
@@ -574,6 +605,17 @@ static void acpi_lpss_restore_ctx(struct device *dev,
 {
 	unsigned int i;
 
+	for (i = 0; i < LPSS_PRV_REG_COUNT; i++) {
+		unsigned long offset = i * sizeof(u32);
+
+		__lpss_reg_write(pdata->prv_reg_ctx[i], pdata, offset);
+		dev_dbg(dev, "restoring 0x%08x to LPSS reg at offset 0x%02lx\n",
+			pdata->prv_reg_ctx[i], offset);
+	}
+}
+
+static void acpi_lpss_d3_to_d0_delay(struct lpss_private_data *pdata)
+{
 	/*
 	 * The following delay is needed or the subsequent write operations may
 	 * fail. The LPSS devices are actually PCI devices and the PCI spec
@@ -586,14 +628,34 @@ static void acpi_lpss_restore_ctx(struct device *dev,
 		delay = 0;
 
 	msleep(delay);
+}
 
-	for (i = 0; i < LPSS_PRV_REG_COUNT; i++) {
-		unsigned long offset = i * sizeof(u32);
+static int acpi_lpss_activate(struct device *dev)
+{
+	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
+	int ret;
 
-		__lpss_reg_write(pdata->prv_reg_ctx[i], pdata, offset);
-		dev_dbg(dev, "restoring 0x%08x to LPSS reg at offset 0x%02lx\n",
-			pdata->prv_reg_ctx[i], offset);
-	}
+	ret = acpi_dev_runtime_resume(dev);
+	if (ret)
+		return ret;
+
+	acpi_lpss_d3_to_d0_delay(pdata);
+
+	/*
+	 * This is called only on ->probe() stage where a device is either in
+	 * known state defined by BIOS or most likely powered off. Due to this
+	 * we have to deassert reset line to be sure that ->probe() will
+	 * recognize the device.
+	 */
+	if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
+		lpss_deassert_reset(pdata);
+
+	return 0;
+}
+
+static void acpi_lpss_dismiss(struct device *dev)
+{
+	acpi_dev_runtime_suspend(dev);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -621,6 +683,8 @@ static int acpi_lpss_resume_early(struct device *dev)
 	if (ret)
 		return ret;
 
+	acpi_lpss_d3_to_d0_delay(pdata);
+
 	if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
 		acpi_lpss_restore_ctx(dev, pdata);
 
@@ -628,6 +692,89 @@ static int acpi_lpss_resume_early(struct device *dev)
 }
 #endif /* CONFIG_PM_SLEEP */
 
+/* IOSF SB for LPSS island */
+#define LPSS_IOSF_UNIT_LPIOEP		0xA0
+#define LPSS_IOSF_UNIT_LPIO1		0xAB
+#define LPSS_IOSF_UNIT_LPIO2		0xAC
+
+#define LPSS_IOSF_PMCSR			0x84
+#define LPSS_PMCSR_D0			0
+#define LPSS_PMCSR_D3hot		3
+#define LPSS_PMCSR_Dx_MASK		GENMASK(1, 0)
+
+#define LPSS_IOSF_GPIODEF0		0x154
+#define LPSS_GPIODEF0_DMA1_D3		BIT(2)
+#define LPSS_GPIODEF0_DMA2_D3		BIT(3)
+#define LPSS_GPIODEF0_DMA_D3_MASK	GENMASK(3, 2)
+
+static DEFINE_MUTEX(lpss_iosf_mutex);
+
+static void lpss_iosf_enter_d3_state(void)
+{
+	u32 value1 = 0;
+	u32 mask1 = LPSS_GPIODEF0_DMA_D3_MASK;
+	u32 value2 = LPSS_PMCSR_D3hot;
+	u32 mask2 = LPSS_PMCSR_Dx_MASK;
+	/*
+	 * PMC provides an information about actual status of the LPSS devices.
+	 * Here we read the values related to LPSS power island, i.e. LPSS
+	 * devices, excluding both LPSS DMA controllers, along with SCC domain.
+	 */
+	u32 func_dis, d3_sts_0, pmc_status, pmc_mask = 0xfe000ffe;
+	int ret;
+
+	ret = pmc_atom_read(PMC_FUNC_DIS, &func_dis);
+	if (ret)
+		return;
+
+	mutex_lock(&lpss_iosf_mutex);
+
+	ret = pmc_atom_read(PMC_D3_STS_0, &d3_sts_0);
+	if (ret)
+		goto exit;
+
+	/*
+	 * Get the status of entire LPSS power island per device basis.
+	 * Shutdown both LPSS DMA controllers if and only if all other devices
+	 * are already in D3hot.
+	 */
+	pmc_status = (~(d3_sts_0 | func_dis)) & pmc_mask;
+	if (pmc_status)
+		goto exit;
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIO1, MBI_CFG_WRITE,
+			LPSS_IOSF_PMCSR, value2, mask2);
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIO2, MBI_CFG_WRITE,
+			LPSS_IOSF_PMCSR, value2, mask2);
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIOEP, MBI_CR_WRITE,
+			LPSS_IOSF_GPIODEF0, value1, mask1);
+exit:
+	mutex_unlock(&lpss_iosf_mutex);
+}
+
+static void lpss_iosf_exit_d3_state(void)
+{
+	u32 value1 = LPSS_GPIODEF0_DMA1_D3 | LPSS_GPIODEF0_DMA2_D3;
+	u32 mask1 = LPSS_GPIODEF0_DMA_D3_MASK;
+	u32 value2 = LPSS_PMCSR_D0;
+	u32 mask2 = LPSS_PMCSR_Dx_MASK;
+
+	mutex_lock(&lpss_iosf_mutex);
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIOEP, MBI_CR_WRITE,
+			LPSS_IOSF_GPIODEF0, value1, mask1);
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIO2, MBI_CFG_WRITE,
+			LPSS_IOSF_PMCSR, value2, mask2);
+
+	iosf_mbi_modify(LPSS_IOSF_UNIT_LPIO1, MBI_CFG_WRITE,
+			LPSS_IOSF_PMCSR, value2, mask2);
+
+	mutex_unlock(&lpss_iosf_mutex);
+}
+
 static int acpi_lpss_runtime_suspend(struct device *dev)
 {
 	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
@@ -640,7 +787,17 @@ static int acpi_lpss_runtime_suspend(struct device *dev)
 	if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
 		acpi_lpss_save_ctx(dev, pdata);
 
-	return acpi_dev_runtime_suspend(dev);
+	ret = acpi_dev_runtime_suspend(dev);
+
+	/*
+	 * This call must be last in the sequence, otherwise PMC will return
+	 * wrong status for devices being about to be powered off. See
+	 * lpss_iosf_enter_d3_state() for further information.
+	 */
+	if (lpss_quirks & LPSS_QUIRK_ALWAYS_POWER_ON && iosf_mbi_available())
+		lpss_iosf_enter_d3_state();
+
+	return ret;
 }
 
 static int acpi_lpss_runtime_resume(struct device *dev)
@@ -648,10 +805,19 @@ static int acpi_lpss_runtime_resume(struct device *dev)
 	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
 	int ret;
 
+	/*
+	 * This call is kept first to be in symmetry with
+	 * acpi_lpss_runtime_suspend() one.
+	 */
+	if (lpss_quirks & LPSS_QUIRK_ALWAYS_POWER_ON && iosf_mbi_available())
+		lpss_iosf_exit_d3_state();
+
 	ret = acpi_dev_runtime_resume(dev);
 	if (ret)
 		return ret;
 
+	acpi_lpss_d3_to_d0_delay(pdata);
+
 	if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
 		acpi_lpss_restore_ctx(dev, pdata);
 
@@ -660,6 +826,10 @@ static int acpi_lpss_runtime_resume(struct device *dev)
 #endif /* CONFIG_PM */
 
 static struct dev_pm_domain acpi_lpss_pm_domain = {
+#ifdef CONFIG_PM
+	.activate = acpi_lpss_activate,
+	.dismiss = acpi_lpss_dismiss,
+#endif
 	.ops = {
 #ifdef CONFIG_PM
 #ifdef CONFIG_PM_SLEEP
@@ -705,8 +875,15 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 	}
 
 	switch (action) {
+	case BUS_NOTIFY_BIND_DRIVER:
+		dev_pm_domain_set(&pdev->dev, &acpi_lpss_pm_domain);
+		break;
+	case BUS_NOTIFY_DRIVER_NOT_BOUND:
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		dev_pm_domain_set(&pdev->dev, NULL);
+		break;
 	case BUS_NOTIFY_ADD_DEVICE:
-		pdev->dev.pm_domain = &acpi_lpss_pm_domain;
+		dev_pm_domain_set(&pdev->dev, &acpi_lpss_pm_domain);
 		if (pdata->dev_desc->flags & LPSS_LTR)
 			return sysfs_create_group(&pdev->dev.kobj,
 						  &lpss_attr_group);
@@ -714,7 +891,7 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 	case BUS_NOTIFY_DEL_DEVICE:
 		if (pdata->dev_desc->flags & LPSS_LTR)
 			sysfs_remove_group(&pdev->dev.kobj, &lpss_attr_group);
-		pdev->dev.pm_domain = NULL;
+		dev_pm_domain_set(&pdev->dev, NULL);
 		break;
 	default:
 		break;
@@ -754,10 +931,19 @@ static struct acpi_scan_handler lpss_handler = {
 
 void __init acpi_lpss_init(void)
 {
-	if (!lpt_clk_init()) {
-		bus_register_notifier(&platform_bus_type, &acpi_lpss_nb);
-		acpi_scan_add_handler(&lpss_handler);
-	}
+	const struct x86_cpu_id *id;
+	int ret;
+
+	ret = lpt_clk_init();
+	if (ret)
+		return;
+
+	id = x86_match_cpu(lpss_cpu_ids);
+	if (id)
+		lpss_quirks |= LPSS_QUIRK_ALWAYS_POWER_ON;
+
+	bus_register_notifier(&platform_bus_type, &acpi_lpss_nb);
+	acpi_scan_add_handler(&lpss_handler);
 }
 
 #else
diff --git a/drivers/acpi/acpi_pnp.c b/drivers/acpi/acpi_pnp.c
index 48fc3ad13a4b..67d97c0090a2 100644
--- a/drivers/acpi/acpi_pnp.c
+++ b/drivers/acpi/acpi_pnp.c
@@ -367,7 +367,7 @@ static struct acpi_scan_handler acpi_pnp_handler = {
  */
 static int is_cmos_rtc_device(struct acpi_device *adev)
 {
-	struct acpi_device_id ids[] = {
+	static const struct acpi_device_id ids[] = {
 		{ "PNP0B00" },
 		{ "PNP0B01" },
 		{ "PNP0B02" },
diff --git a/drivers/acpi/acpi_video.c b/drivers/acpi/acpi_video.c
index 3405f7a41e25..a76f8be1bfe7 100644
--- a/drivers/acpi/acpi_video.c
+++ b/drivers/acpi/acpi_video.c
@@ -77,6 +77,13 @@ module_param(allow_duplicates, bool, 0644);
 static int disable_backlight_sysfs_if = -1;
 module_param(disable_backlight_sysfs_if, int, 0444);
 
+#define REPORT_OUTPUT_KEY_EVENTS		0x01
+#define REPORT_BRIGHTNESS_KEY_EVENTS		0x02
+static int report_key_events = -1;
+module_param(report_key_events, int, 0644);
+MODULE_PARM_DESC(report_key_events,
+	"0: none, 1: output changes, 2: brightness changes, 3: all");
+
 static bool device_id_scheme = false;
 module_param(device_id_scheme, bool, 0444);
 
@@ -85,8 +92,8 @@ module_param(only_lcd, bool, 0444);
 
 static int register_count;
 static DEFINE_MUTEX(register_count_mutex);
-static struct mutex video_list_lock;
-static struct list_head video_bus_head;
+static DEFINE_MUTEX(video_list_lock);
+static LIST_HEAD(video_bus_head);
 static int acpi_video_bus_add(struct acpi_device *device);
 static int acpi_video_bus_remove(struct acpi_device *device);
 static void acpi_video_bus_notify(struct acpi_device *device, u32 event);
@@ -412,6 +419,13 @@ static int video_enable_only_lcd(const struct dmi_system_id *d)
 	return 0;
 }
 
+static int video_set_report_key_events(const struct dmi_system_id *id)
+{
+	if (report_key_events == -1)
+		report_key_events = (uintptr_t)id->driver_data;
+	return 0;
+}
+
 static struct dmi_system_id video_dmi_table[] = {
 	/*
 	 * Broken _BQC workaround http://bugzilla.kernel.org/show_bug.cgi?id=13121
@@ -465,6 +479,15 @@ static struct dmi_system_id video_dmi_table[] = {
 	 * as brightness control does not work.
 	 */
 	{
+	 /* https://bugzilla.kernel.org/show_bug.cgi?id=21012 */
+	 .callback = video_disable_backlight_sysfs_if,
+	 .ident = "Toshiba Portege R700",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "PORTEGE R700"),
+		},
+	},
+	{
 	 /* https://bugs.freedesktop.org/show_bug.cgi?id=82634 */
 	 .callback = video_disable_backlight_sysfs_if,
 	 .ident = "Toshiba Portege R830",
@@ -473,6 +496,15 @@ static struct dmi_system_id video_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "PORTEGE R830"),
 		},
 	},
+	{
+	 /* https://bugzilla.kernel.org/show_bug.cgi?id=21012 */
+	 .callback = video_disable_backlight_sysfs_if,
+	 .ident = "Toshiba Satellite R830",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "SATELLITE R830"),
+		},
+	},
 	/*
 	 * Some machine's _DOD IDs don't have bit 31(Device ID Scheme) set
 	 * but the IDs actually follow the Device ID Scheme.
@@ -500,6 +532,24 @@ static struct dmi_system_id video_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "ESPRIMO Mobile M9410"),
 		},
 	},
+	/*
+	 * Some machines report wrong key events on the acpi-bus, suppress
+	 * key event reporting on these.  Note this is only intended to work
+	 * around events which are plain wrong. In some cases we get double
+	 * events, in this case acpi-video is considered the canonical source
+	 * and the events from the other source should be filtered. E.g.
+	 * by calling acpi_video_handles_brightness_key_presses() from the
+	 * vendor acpi/wmi driver or by using /lib/udev/hwdb.d/60-keyboard.hwdb
+	 */
+	{
+	 .callback = video_set_report_key_events,
+	 .driver_data = (void *)((uintptr_t)REPORT_OUTPUT_KEY_EVENTS),
+	 .ident = "Dell Vostro V131",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Vostro V131"),
+		},
+	},
 	{}
 };
 
@@ -1480,7 +1530,7 @@ static void acpi_video_bus_notify(struct acpi_device *device, u32 event)
 		/* Something vetoed the keypress. */
 		keycode = 0;
 
-	if (keycode) {
+	if (keycode && (report_key_events & REPORT_OUTPUT_KEY_EVENTS)) {
 		input_report_key(input, keycode, 1);
 		input_sync(input);
 		input_report_key(input, keycode, 0);
@@ -1544,7 +1594,7 @@ static void acpi_video_device_notify(acpi_handle handle, u32 event, void *data)
 
 	acpi_notifier_call_chain(device, event, 0);
 
-	if (keycode) {
+	if (keycode && (report_key_events & REPORT_BRIGHTNESS_KEY_EVENTS)) {
 		input_report_key(input, keycode, 1);
 		input_sync(input);
 		input_report_key(input, keycode, 0);
@@ -2026,9 +2076,6 @@ int acpi_video_register(void)
 		goto leave;
 	}
 
-	mutex_init(&video_list_lock);
-	INIT_LIST_HEAD(&video_bus_head);
-
 	dmi_check_system(video_dmi_table);
 
 	ret = acpi_bus_register_driver(&acpi_video_bus);
@@ -2072,6 +2119,19 @@ void acpi_video_unregister_backlight(void)
 	mutex_unlock(&register_count_mutex);
 }
 
+bool acpi_video_handles_brightness_key_presses(void)
+{
+	bool have_video_busses;
+
+	mutex_lock(&video_list_lock);
+	have_video_busses = !list_empty(&video_bus_head);
+	mutex_unlock(&video_list_lock);
+
+	return have_video_busses &&
+	       (report_key_events & REPORT_BRIGHTNESS_KEY_EVENTS);
+}
+EXPORT_SYMBOL(acpi_video_handles_brightness_key_presses);
+
 /*
  * This is kind of nasty. Hardware using Intel chipsets may require
  * the video opregion code to be run first in order to initialise
diff --git a/drivers/acpi/acpica/Makefile b/drivers/acpi/acpica/Makefile
index 885936f79542..f682374c19f4 100644
--- a/drivers/acpi/acpica/Makefile
+++ b/drivers/acpi/acpica/Makefile
@@ -50,6 +50,7 @@ acpi-y +=		\
 	exdump.o	\
 	exfield.o	\
 	exfldio.o	\
+	exmisc.o	\
 	exmutex.o	\
 	exnames.o	\
 	exoparg1.o	\
@@ -57,7 +58,6 @@ acpi-y +=		\
 	exoparg3.o	\
 	exoparg6.o	\
 	exprep.o	\
-	exmisc.o	\
 	exregion.o	\
 	exresnte.o	\
 	exresolv.o	\
@@ -66,6 +66,7 @@ acpi-y +=		\
 	exstoren.o	\
 	exstorob.o	\
 	exsystem.o	\
+	extrace.o	\
 	exutils.o
 
 acpi-y +=		\
@@ -196,7 +197,6 @@ acpi-$(ACPI_FUTURE_USAGE) +=	\
 	dbfileio.o		\
 	dbtest.o		\
 	utcache.o		\
-	utfileio.o		\
 	utprint.o		\
 	uttrack.o		\
 	utuuid.o
diff --git a/drivers/acpi/acpica/acapps.h b/drivers/acpi/acpica/acapps.h
index e4cc48fbf4ee..ca2c0607104b 100644
--- a/drivers/acpi/acpica/acapps.h
+++ b/drivers/acpi/acpica/acapps.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -44,10 +44,12 @@
 #ifndef _ACAPPS
 #define _ACAPPS
 
+#include <stdio.h>
+
 /* Common info for tool signons */
 
 #define ACPICA_NAME                 "Intel ACPI Component Architecture"
-#define ACPICA_COPYRIGHT            "Copyright (c) 2000 - 2015 Intel Corporation"
+#define ACPICA_COPYRIGHT            "Copyright (c) 2000 - 2016 Intel Corporation"
 
 #if ACPI_MACHINE_WIDTH == 64
 #define ACPI_WIDTH          "-64"
@@ -85,11 +87,40 @@
 	acpi_os_printf (description);
 
 #define ACPI_OPTION(name, description) \
-	acpi_os_printf (" %-18s%s\n", name, description);
+	acpi_os_printf (" %-20s%s\n", name, description);
+
+/* Check for unexpected exceptions */
+
+#define ACPI_CHECK_STATUS(name, status, expected) \
+	if (status != expected) \
+	{ \
+		acpi_os_printf ("Unexpected %s from %s (%s-%d)\n", \
+			acpi_format_exception (status), #name, _acpi_module_name, __LINE__); \
+	}
+
+/* Check for unexpected non-AE_OK errors */
+
+#define ACPI_CHECK_OK(name, status)   ACPI_CHECK_STATUS (name, status, AE_OK);
 
 #define FILE_SUFFIX_DISASSEMBLY     "dsl"
 #define FILE_SUFFIX_BINARY_TABLE    ".dat"	/* Needs the dot */
 
+/* acfileio */
+
+acpi_status
+ac_get_all_tables_from_file(char *filename,
+			    u8 get_only_aml_tables,
+			    struct acpi_new_table_desc **return_list_head);
+
+u8 ac_is_file_binary(FILE * file);
+
+acpi_status ac_validate_table_header(FILE * file, long table_offset);
+
+/* Values for get_only_aml_tables */
+
+#define ACPI_GET_ONLY_AML_TABLES    TRUE
+#define ACPI_GET_ALL_TABLES         FALSE
+
 /*
  * getopt
  */
@@ -107,30 +138,6 @@ extern char *acpi_gbl_optarg;
  */
 u32 cm_get_file_size(ACPI_FILE file);
 
-#ifndef ACPI_DUMP_APP
-/*
- * adisasm
- */
-acpi_status
-ad_aml_disassemble(u8 out_to_file,
-		   char *filename, char *prefix, char **out_filename);
-
-void ad_print_statistics(void);
-
-acpi_status ad_find_dsdt(u8 **dsdt_ptr, u32 *dsdt_length);
-
-void ad_dump_tables(void);
-
-acpi_status ad_get_local_tables(void);
-
-acpi_status
-ad_parse_table(struct acpi_table_header *table,
-	       acpi_owner_id * owner_id, u8 load_table, u8 external);
-
-acpi_status ad_display_tables(char *filename, struct acpi_table_header *table);
-
-acpi_status ad_display_statistics(void);
-
 /*
  * adwalk
  */
@@ -168,6 +175,5 @@ char *ad_generate_filename(char *prefix, char *table_id);
 void
 ad_write_table(struct acpi_table_header *table,
 	       u32 length, char *table_name, char *oem_table_id);
-#endif
 
 #endif				/* _ACAPPS */
diff --git a/drivers/acpi/acpica/accommon.h b/drivers/acpi/acpica/accommon.h
index a8d8092ee391..19d6ec815d12 100644
--- a/drivers/acpi/acpica/accommon.h
+++ b/drivers/acpi/acpica/accommon.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acdebug.h b/drivers/acpi/acpica/acdebug.h
index c928ba494c40..993af9eb007a 100644
--- a/drivers/acpi/acpica/acdebug.h
+++ b/drivers/acpi/acpica/acdebug.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -80,9 +80,15 @@ struct acpi_db_execute_walk {
 /*
  * dbxface - external debugger interfaces
  */
-acpi_status
-acpi_db_single_step(struct acpi_walk_state *walk_state,
-		    union acpi_parse_object *op, u32 op_type);
+ACPI_DBR_DEPENDENT_RETURN_OK(acpi_status
+			     acpi_db_single_step(struct acpi_walk_state
+						 *walk_state,
+						 union acpi_parse_object *op,
+						 u32 op_type))
+ ACPI_DBR_DEPENDENT_RETURN_VOID(void
+				acpi_db_signal_break_point(struct
+							   acpi_walk_state
+							   *walk_state))
 
 /*
  * dbcmds - debug commands and output routines
@@ -182,11 +188,15 @@ void acpi_db_display_method_info(union acpi_parse_object *op);
 
 void acpi_db_decode_and_display_object(char *target, char *output_type);
 
-void
-acpi_db_display_result_object(union acpi_operand_object *obj_desc,
-			      struct acpi_walk_state *walk_state);
+ACPI_DBR_DEPENDENT_RETURN_VOID(void
+			       acpi_db_display_result_object(union
+							     acpi_operand_object
+							     *obj_desc,
+							     struct
+							     acpi_walk_state
+							     *walk_state))
 
-acpi_status acpi_db_display_all_methods(char *display_count_arg);
+ acpi_status acpi_db_display_all_methods(char *display_count_arg);
 
 void acpi_db_display_arguments(void);
 
@@ -198,9 +208,13 @@ void acpi_db_display_calling_tree(void);
 
 void acpi_db_display_object_type(char *object_arg);
 
-void
-acpi_db_display_argument_object(union acpi_operand_object *obj_desc,
-				struct acpi_walk_state *walk_state);
+ACPI_DBR_DEPENDENT_RETURN_VOID(void
+			       acpi_db_display_argument_object(union
+							       acpi_operand_object
+							       *obj_desc,
+							       struct
+							       acpi_walk_state
+							       *walk_state))
 
 /*
  * dbexec - debugger control method execution
@@ -231,10 +245,7 @@ void acpi_db_open_debug_file(char *name);
 
 acpi_status acpi_db_load_acpi_table(char *filename);
 
-acpi_status
-acpi_db_get_table_from_file(char *filename,
-			    struct acpi_table_header **table,
-			    u8 must_be_aml_table);
+acpi_status acpi_db_load_tables(struct acpi_new_table_desc *list_head);
 
 /*
  * dbhistry - debugger HISTORY command
@@ -257,7 +268,7 @@ acpi_db_command_dispatch(char *input_buffer,
 
 void ACPI_SYSTEM_XFACE acpi_db_execute_thread(void *context);
 
-acpi_status acpi_db_user_commands(char prompt, union acpi_parse_object *op);
+acpi_status acpi_db_user_commands(void);
 
 char *acpi_db_get_next_token(char *string,
 			     char **next, acpi_object_type * return_type);
diff --git a/drivers/acpi/acpica/acdispat.h b/drivers/acpi/acpica/acdispat.h
index 7094dc89eb81..dcd48bfedb4d 100644
--- a/drivers/acpi/acpica/acdispat.h
+++ b/drivers/acpi/acpica/acdispat.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acevents.h b/drivers/acpi/acpica/acevents.h
index 228704b78657..010cf81bada9 100644
--- a/drivers/acpi/acpica/acevents.h
+++ b/drivers/acpi/acpica/acevents.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -161,6 +161,11 @@ acpi_ev_delete_gpe_handlers(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 /*
  * evhandler - Address space handling
  */
+union acpi_operand_object *acpi_ev_find_region_handler(acpi_adr_space_type
+						       space_id,
+						       union acpi_operand_object
+						       *handler_obj);
+
 u8
 acpi_ev_has_default_handler(struct acpi_namespace_node *node,
 			    acpi_adr_space_type space_id);
@@ -193,9 +198,11 @@ void
 acpi_ev_detach_region(union acpi_operand_object *region_obj,
 		      u8 acpi_ns_is_locked);
 
-acpi_status
+void acpi_ev_associate_reg_method(union acpi_operand_object *region_obj);
+
+void
 acpi_ev_execute_reg_methods(struct acpi_namespace_node *node,
-			    acpi_adr_space_type space_id);
+			    acpi_adr_space_type space_id, u32 function);
 
 acpi_status
 acpi_ev_execute_reg_method(union acpi_operand_object *region_obj, u32 function);
diff --git a/drivers/acpi/acpica/acglobal.h b/drivers/acpi/acpica/acglobal.h
index faa97604d878..55c8197036f3 100644
--- a/drivers/acpi/acpica/acglobal.h
+++ b/drivers/acpi/acpica/acglobal.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -145,6 +145,7 @@ ACPI_GLOBAL(acpi_cache_t *, acpi_gbl_operand_cache);
 
 ACPI_INIT_GLOBAL(u32, acpi_gbl_startup_flags, 0);
 ACPI_INIT_GLOBAL(u8, acpi_gbl_shutdown, TRUE);
+ACPI_INIT_GLOBAL(u8, acpi_gbl_early_initialization, TRUE);
 
 /* Global handlers */
 
@@ -164,7 +165,7 @@ ACPI_GLOBAL(u8, acpi_gbl_next_owner_id_offset);
 
 /* Initialization sequencing */
 
-ACPI_GLOBAL(u8, acpi_gbl_reg_methods_executed);
+ACPI_INIT_GLOBAL(u8, acpi_gbl_reg_methods_enabled, FALSE);
 
 /* Misc */
 
@@ -326,7 +327,6 @@ ACPI_GLOBAL(struct acpi_external_file *, acpi_gbl_external_file_list);
 #ifdef ACPI_DEBUGGER
 
 ACPI_INIT_GLOBAL(u8, acpi_gbl_abort_method, FALSE);
-ACPI_INIT_GLOBAL(u8, acpi_gbl_method_executing, FALSE);
 ACPI_INIT_GLOBAL(acpi_thread_id, acpi_gbl_db_thread_id, ACPI_INVALID_THREAD_ID);
 
 ACPI_GLOBAL(u8, acpi_gbl_db_opt_no_ini_methods);
@@ -345,7 +345,6 @@ ACPI_GLOBAL(acpi_object_type, acpi_gbl_db_arg_types[ACPI_DEBUGGER_MAX_ARGS]);
 
 /* These buffers should all be the same size */
 
-ACPI_GLOBAL(char, acpi_gbl_db_line_buf[ACPI_DB_LINE_BUFFER_SIZE]);
 ACPI_GLOBAL(char, acpi_gbl_db_parsed_buf[ACPI_DB_LINE_BUFFER_SIZE]);
 ACPI_GLOBAL(char, acpi_gbl_db_scope_buf[ACPI_DB_LINE_BUFFER_SIZE]);
 ACPI_GLOBAL(char, acpi_gbl_db_debug_filename[ACPI_DB_LINE_BUFFER_SIZE]);
@@ -360,9 +359,6 @@ ACPI_GLOBAL(u16, acpi_gbl_node_type_count_misc);
 ACPI_GLOBAL(u32, acpi_gbl_num_nodes);
 ACPI_GLOBAL(u32, acpi_gbl_num_objects);
 
-ACPI_GLOBAL(acpi_mutex, acpi_gbl_db_command_ready);
-ACPI_GLOBAL(acpi_mutex, acpi_gbl_db_command_complete);
-
 #endif				/* ACPI_DEBUGGER */
 
 /*****************************************************************************
diff --git a/drivers/acpi/acpica/achware.h b/drivers/acpi/acpica/achware.h
index 196a55244559..27addcf50c37 100644
--- a/drivers/acpi/acpica/achware.h
+++ b/drivers/acpi/acpica/achware.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acinterp.h b/drivers/acpi/acpica/acinterp.h
index e9e936e78154..bae1a35c345f 100644
--- a/drivers/acpi/acpica/acinterp.h
+++ b/drivers/acpi/acpica/acinterp.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index e1dd784d8515..e4977fac9c1d 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -219,6 +219,13 @@ struct acpi_table_list {
 #define ACPI_ROOT_ORIGIN_ALLOCATED      (1)
 #define ACPI_ROOT_ALLOW_RESIZE          (2)
 
+/* List to manage incoming ACPI tables */
+
+struct acpi_new_table_desc {
+	struct acpi_table_header *table;
+	struct acpi_new_table_desc *next;
+};
+
 /* Predefined table indexes */
 
 #define ACPI_INVALID_TABLE_INDEX        (0xFFFFFFFF)
@@ -388,7 +395,8 @@ union acpi_predefined_info {
 
 /* Return object auto-repair info */
 
-typedef acpi_status(*acpi_object_converter) (union acpi_operand_object
+typedef acpi_status(*acpi_object_converter) (struct acpi_namespace_node * scope,
+					     union acpi_operand_object
 					     *original_object,
 					     union acpi_operand_object
 					     **converted_object);
@@ -420,6 +428,7 @@ struct acpi_simple_repair_info {
 
 struct acpi_reg_walk_info {
 	acpi_adr_space_type space_id;
+	u32 function;
 	u32 reg_run_count;
 };
 
@@ -861,6 +870,7 @@ struct acpi_parse_state {
 #define ACPI_PARSEOP_CLOSING_PAREN      0x10
 #define ACPI_PARSEOP_COMPOUND           0x20
 #define ACPI_PARSEOP_ASSIGNMENT         0x40
+#define ACPI_PARSEOP_ELSEIF             0x80
 
 /*****************************************************************************
  *
diff --git a/drivers/acpi/acpica/acmacros.h b/drivers/acpi/acpica/acmacros.h
index e85366ceb15a..411c18b7d541 100644
--- a/drivers/acpi/acpica/acmacros.h
+++ b/drivers/acpi/acpica/acmacros.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -401,17 +401,6 @@
 #endif
 
 /*
- * Some code only gets executed when the debugger is built in.
- * Note that this is entirely independent of whether the
- * DEBUG_PRINT stuff (set by ACPI_DEBUG_OUTPUT) is on, or not.
- */
-#ifdef ACPI_DEBUGGER
-#define ACPI_DEBUGGER_EXEC(a)           a
-#else
-#define ACPI_DEBUGGER_EXEC(a)
-#endif
-
-/*
  * Macros used for ACPICA utilities only
  */
 
diff --git a/drivers/acpi/acpica/acnamesp.h b/drivers/acpi/acpica/acnamesp.h
index 5d261c942a0d..9684ed61284d 100644
--- a/drivers/acpi/acpica/acnamesp.h
+++ b/drivers/acpi/acpica/acnamesp.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -77,6 +77,7 @@
 /* Object is not a package element */
 
 #define ACPI_NOT_PACKAGE_ELEMENT    ACPI_UINT32_MAX
+#define ACPI_ALL_PACKAGE_ELEMENTS   (ACPI_UINT32_MAX-1)
 
 /* Always emit warning message, not dependent on node flags */
 
@@ -183,13 +184,20 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
 			  union acpi_operand_object **return_object);
 
 acpi_status
-acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
+acpi_ns_convert_to_unicode(struct acpi_namespace_node *scope,
+			   union acpi_operand_object *original_object,
 			   union acpi_operand_object **return_object);
 
 acpi_status
-acpi_ns_convert_to_resource(union acpi_operand_object *original_object,
+acpi_ns_convert_to_resource(struct acpi_namespace_node *scope,
+			    union acpi_operand_object *original_object,
 			    union acpi_operand_object **return_object);
 
+acpi_status
+acpi_ns_convert_to_reference(struct acpi_namespace_node *scope,
+			     union acpi_operand_object *original_object,
+			     union acpi_operand_object **return_object);
+
 /*
  * nsdump - Namespace dump/print utilities
  */
diff --git a/drivers/acpi/acpica/acobject.h b/drivers/acpi/acpica/acobject.h
index 0bd02c4a5f75..094b042678f7 100644
--- a/drivers/acpi/acpica/acobject.h
+++ b/drivers/acpi/acpica/acobject.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -93,9 +93,10 @@
 #define AOPOBJ_AML_CONSTANT         0x01	/* Integer is an AML constant */
 #define AOPOBJ_STATIC_POINTER       0x02	/* Data is part of an ACPI table, don't delete */
 #define AOPOBJ_DATA_VALID           0x04	/* Object is initialized and data is valid */
-#define AOPOBJ_OBJECT_INITIALIZED   0x08	/* Region is initialized, _REG was run */
-#define AOPOBJ_SETUP_COMPLETE       0x10	/* Region setup is complete */
-#define AOPOBJ_INVALID              0x20	/* Host OS won't allow a Region address */
+#define AOPOBJ_OBJECT_INITIALIZED   0x08	/* Region is initialized */
+#define AOPOBJ_REG_CONNECTED        0x10	/* _REG was run */
+#define AOPOBJ_SETUP_COMPLETE       0x20	/* Region setup is complete */
+#define AOPOBJ_INVALID              0x40	/* Host OS won't allow a Region address */
 
 /******************************************************************************
  *
diff --git a/drivers/acpi/acpica/acopcode.h b/drivers/acpi/acpica/acopcode.h
index f9acf92fa0bc..ca4bda1a60be 100644
--- a/drivers/acpi/acpica/acopcode.h
+++ b/drivers/acpi/acpica/acopcode.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -92,7 +92,7 @@
 #define ARGP_BYTELIST_OP                ARGP_LIST1 (ARGP_NAMESTRING)
 #define ARGP_CONCAT_OP                  ARGP_LIST3 (ARGP_TERMARG,    ARGP_TERMARG,       ARGP_TARGET)
 #define ARGP_CONCAT_RES_OP              ARGP_LIST3 (ARGP_TERMARG,    ARGP_TERMARG,       ARGP_TARGET)
-#define ARGP_COND_REF_OF_OP             ARGP_LIST2 (ARGP_SUPERNAME,  ARGP_SUPERNAME)
+#define ARGP_COND_REF_OF_OP             ARGP_LIST2 (ARGP_NAME_OR_REF,ARGP_TARGET)
 #define ARGP_CONNECTFIELD_OP            ARGP_LIST1 (ARGP_NAMESTRING)
 #define ARGP_CONTINUE_OP                ARG_NONE
 #define ARGP_COPY_OP                    ARGP_LIST2 (ARGP_TERMARG,    ARGP_SIMPLENAME)
@@ -152,13 +152,14 @@
 #define ARGP_NAMEPATH_OP                ARGP_LIST1 (ARGP_NAMESTRING)
 #define ARGP_NOOP_OP                    ARG_NONE
 #define ARGP_NOTIFY_OP                  ARGP_LIST2 (ARGP_SUPERNAME,  ARGP_TERMARG)
+#define ARGP_OBJECT_TYPE_OP             ARGP_LIST1 (ARGP_NAME_OR_REF)
 #define ARGP_ONE_OP                     ARG_NONE
 #define ARGP_ONES_OP                    ARG_NONE
 #define ARGP_PACKAGE_OP                 ARGP_LIST3 (ARGP_PKGLENGTH,  ARGP_BYTEDATA,      ARGP_DATAOBJLIST)
 #define ARGP_POWER_RES_OP               ARGP_LIST5 (ARGP_PKGLENGTH,  ARGP_NAME,          ARGP_BYTEDATA,  ARGP_WORDDATA,  ARGP_OBJLIST)
 #define ARGP_PROCESSOR_OP               ARGP_LIST6 (ARGP_PKGLENGTH,  ARGP_NAME,          ARGP_BYTEDATA,  ARGP_DWORDDATA, ARGP_BYTEDATA,  ARGP_OBJLIST)
 #define ARGP_QWORD_OP                   ARGP_LIST1 (ARGP_QWORDDATA)
-#define ARGP_REF_OF_OP                  ARGP_LIST1 (ARGP_SUPERNAME)
+#define ARGP_REF_OF_OP                  ARGP_LIST1 (ARGP_NAME_OR_REF)
 #define ARGP_REGION_OP                  ARGP_LIST4 (ARGP_NAME,       ARGP_BYTEDATA,      ARGP_TERMARG,   ARGP_TERMARG)
 #define ARGP_RELEASE_OP                 ARGP_LIST1 (ARGP_SUPERNAME)
 #define ARGP_RESERVEDFIELD_OP           ARGP_LIST1 (ARGP_NAMESTRING)
@@ -185,7 +186,6 @@
 #define ARGP_TO_HEX_STR_OP              ARGP_LIST2 (ARGP_TERMARG,    ARGP_TARGET)
 #define ARGP_TO_INTEGER_OP              ARGP_LIST2 (ARGP_TERMARG,    ARGP_TARGET)
 #define ARGP_TO_STRING_OP               ARGP_LIST3 (ARGP_TERMARG,    ARGP_TERMARG,       ARGP_TARGET)
-#define ARGP_TYPE_OP                    ARGP_LIST1 (ARGP_SUPERNAME)
 #define ARGP_UNLOAD_OP                  ARGP_LIST1 (ARGP_SUPERNAME)
 #define ARGP_VAR_PACKAGE_OP             ARGP_LIST3 (ARGP_PKGLENGTH,  ARGP_TERMARG,       ARGP_DATAOBJLIST)
 #define ARGP_WAIT_OP                    ARGP_LIST2 (ARGP_SUPERNAME,  ARGP_TERMARG)
@@ -223,7 +223,7 @@
 #define ARGI_BUFFER_OP                  ARGI_LIST1 (ARGI_INTEGER)
 #define ARGI_BYTE_OP                    ARGI_INVALID_OPCODE
 #define ARGI_BYTELIST_OP                ARGI_INVALID_OPCODE
-#define ARGI_CONCAT_OP                  ARGI_LIST3 (ARGI_COMPUTEDATA,ARGI_COMPUTEDATA,   ARGI_TARGETREF)
+#define ARGI_CONCAT_OP                  ARGI_LIST3 (ARGI_ANYTYPE,    ARGI_ANYTYPE,       ARGI_TARGETREF)
 #define ARGI_CONCAT_RES_OP              ARGI_LIST3 (ARGI_BUFFER,     ARGI_BUFFER,        ARGI_TARGETREF)
 #define ARGI_COND_REF_OF_OP             ARGI_LIST2 (ARGI_OBJECT_REF, ARGI_TARGETREF)
 #define ARGI_CONNECTFIELD_OP            ARGI_INVALID_OPCODE
@@ -285,6 +285,7 @@
 #define ARGI_NAMEPATH_OP                ARGI_INVALID_OPCODE
 #define ARGI_NOOP_OP                    ARG_NONE
 #define ARGI_NOTIFY_OP                  ARGI_LIST2 (ARGI_DEVICE_REF, ARGI_INTEGER)
+#define ARGI_OBJECT_TYPE_OP             ARGI_LIST1 (ARGI_ANYTYPE)
 #define ARGI_ONE_OP                     ARG_NONE
 #define ARGI_ONES_OP                    ARG_NONE
 #define ARGI_PACKAGE_OP                 ARGI_LIST1 (ARGI_INTEGER)
@@ -318,7 +319,6 @@
 #define ARGI_TO_HEX_STR_OP              ARGI_LIST2 (ARGI_COMPUTEDATA,ARGI_FIXED_TARGET)
 #define ARGI_TO_INTEGER_OP              ARGI_LIST2 (ARGI_COMPUTEDATA,ARGI_FIXED_TARGET)
 #define ARGI_TO_STRING_OP               ARGI_LIST3 (ARGI_BUFFER,     ARGI_INTEGER,       ARGI_FIXED_TARGET)
-#define ARGI_TYPE_OP                    ARGI_LIST1 (ARGI_ANYTYPE)
 #define ARGI_UNLOAD_OP                  ARGI_LIST1 (ARGI_DDBHANDLE)
 #define ARGI_VAR_PACKAGE_OP             ARGI_LIST1 (ARGI_INTEGER)
 #define ARGI_WAIT_OP                    ARGI_LIST2 (ARGI_EVENT,      ARGI_INTEGER)
diff --git a/drivers/acpi/acpica/acparser.h b/drivers/acpi/acpica/acparser.h
index 8fc8c7cea879..7da639d62416 100644
--- a/drivers/acpi/acpica/acparser.h
+++ b/drivers/acpi/acpica/acparser.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -92,7 +92,13 @@ acpi_ps_get_next_simple_arg(struct acpi_parse_state *parser_state,
 acpi_status
 acpi_ps_get_next_namepath(struct acpi_walk_state *walk_state,
 			  struct acpi_parse_state *parser_state,
-			  union acpi_parse_object *arg, u8 method_call);
+			  union acpi_parse_object *arg,
+			  u8 possible_method_call);
+
+/* Values for u8 above */
+
+#define ACPI_NOT_METHOD_CALL            FALSE
+#define ACPI_POSSIBLE_METHOD_CALL       TRUE
 
 acpi_status
 acpi_ps_get_next_arg(struct acpi_walk_state *walk_state,
diff --git a/drivers/acpi/acpica/acpredef.h b/drivers/acpi/acpica/acpredef.h
index b9474b529fcb..52f6bee52d47 100644
--- a/drivers/acpi/acpica/acpredef.h
+++ b/drivers/acpi/acpica/acpredef.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acresrc.h b/drivers/acpi/acpica/acresrc.h
index 6357efb01b93..5dd58beafa5c 100644
--- a/drivers/acpi/acpica/acresrc.h
+++ b/drivers/acpi/acpica/acresrc.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acstruct.h b/drivers/acpi/acpica/acstruct.h
index f9992dced1f9..b3b386e0b119 100644
--- a/drivers/acpi/acpica/acstruct.h
+++ b/drivers/acpi/acpica/acstruct.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/actables.h b/drivers/acpi/acpica/actables.h
index 591ea95319e2..848ad3ac938f 100644
--- a/drivers/acpi/acpica/actables.h
+++ b/drivers/acpi/acpica/actables.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/acutils.h b/drivers/acpi/acpica/acutils.h
index 8b8fef6cc32d..e43ab6f2ad7e 100644
--- a/drivers/acpi/acpica/acutils.h
+++ b/drivers/acpi/acpica/acutils.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -184,24 +184,24 @@ acpi_status acpi_ut_init_globals(void);
 
 #if defined(ACPI_DEBUG_OUTPUT) || defined(ACPI_DEBUGGER)
 
-char *acpi_ut_get_mutex_name(u32 mutex_id);
+const char *acpi_ut_get_mutex_name(u32 mutex_id);
 
 const char *acpi_ut_get_notify_name(u32 notify_value, acpi_object_type type);
 #endif
 
-char *acpi_ut_get_type_name(acpi_object_type type);
+const char *acpi_ut_get_type_name(acpi_object_type type);
 
-char *acpi_ut_get_node_name(void *object);
+const char *acpi_ut_get_node_name(void *object);
 
-char *acpi_ut_get_descriptor_name(void *object);
+const char *acpi_ut_get_descriptor_name(void *object);
 
 const char *acpi_ut_get_reference_name(union acpi_operand_object *object);
 
-char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc);
+const char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc);
 
-char *acpi_ut_get_region_name(u8 space_id);
+const char *acpi_ut_get_region_name(u8 space_id);
 
-char *acpi_ut_get_event_name(u32 event_id);
+const char *acpi_ut_get_event_name(u32 event_id);
 
 char acpi_ut_hex_to_ascii_char(u64 integer, u32 position);
 
@@ -353,14 +353,6 @@ acpi_ut_execute_power_methods(struct acpi_namespace_node *device_node,
 			      u8 method_count, u8 *out_values);
 
 /*
- * utfileio - file operations
- */
-#ifdef ACPI_APPLICATION
-acpi_status
-acpi_ut_read_table_from_file(char *filename, struct acpi_table_header **table);
-#endif
-
-/*
  * utids - device ID support
  */
 acpi_status
@@ -372,10 +364,6 @@ acpi_ut_execute_UID(struct acpi_namespace_node *device_node,
 		    struct acpi_pnp_device_id ** return_id);
 
 acpi_status
-acpi_ut_execute_SUB(struct acpi_namespace_node *device_node,
-		    struct acpi_pnp_device_id **return_id);
-
-acpi_status
 acpi_ut_execute_CID(struct acpi_namespace_node *device_node,
 		    struct acpi_pnp_device_id_list ** return_cid_list);
 
diff --git a/drivers/acpi/acpica/amlcode.h b/drivers/acpi/acpica/amlcode.h
index 883f20cfa698..ceb4f7365f7f 100644
--- a/drivers/acpi/acpica/amlcode.h
+++ b/drivers/acpi/acpica/amlcode.h
@@ -7,7 +7,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -120,7 +120,7 @@
 #define AML_CREATE_WORD_FIELD_OP    (u16) 0x8b
 #define AML_CREATE_BYTE_FIELD_OP    (u16) 0x8c
 #define AML_CREATE_BIT_FIELD_OP     (u16) 0x8d
-#define AML_TYPE_OP                 (u16) 0x8e
+#define AML_OBJECT_TYPE_OP          (u16) 0x8e
 #define AML_CREATE_QWORD_FIELD_OP   (u16) 0x8f	/* ACPI 2.0 */
 #define AML_LAND_OP                 (u16) 0x90
 #define AML_LOR_OP                  (u16) 0x91
@@ -238,7 +238,8 @@
 #define ARGP_TERMLIST               0x0F
 #define ARGP_WORDDATA               0x10
 #define ARGP_QWORDDATA              0x11
-#define ARGP_SIMPLENAME             0x12
+#define ARGP_SIMPLENAME             0x12	/* name_string | local_term | arg_term */
+#define ARGP_NAME_OR_REF            0x13	/* For object_type only */
 
 /*
  * Resolved argument types for the AML Interpreter
diff --git a/drivers/acpi/acpica/amlresrc.h b/drivers/acpi/acpica/amlresrc.h
index ee0cdd60b93d..dee6c7ea4773 100644
--- a/drivers/acpi/acpica/amlresrc.h
+++ b/drivers/acpi/acpica/amlresrc.h
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbcmds.c b/drivers/acpi/acpica/dbcmds.c
index 30414b3d7fdd..7ec62c461280 100644
--- a/drivers/acpi/acpica/dbcmds.c
+++ b/drivers/acpi/acpica/dbcmds.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -798,7 +798,7 @@ acpi_db_device_resources(acpi_handle obj_handle,
 	acpi_status status;
 
 	node = ACPI_CAST_PTR(struct acpi_namespace_node, obj_handle);
-	parent_path = acpi_ns_get_external_pathname(node);
+	parent_path = acpi_ns_get_normalized_pathname(node, TRUE);
 	if (!parent_path) {
 		return (AE_NO_MEMORY);
 	}
@@ -1131,13 +1131,8 @@ void acpi_db_trace(char *enable_arg, char *method_arg, char *once_arg)
 	u32 debug_layer = 0;
 	u32 flags = 0;
 
-	if (enable_arg) {
-		acpi_ut_strupr(enable_arg);
-	}
-
-	if (once_arg) {
-		acpi_ut_strupr(once_arg);
-	}
+	acpi_ut_strupr(enable_arg);
+	acpi_ut_strupr(once_arg);
 
 	if (method_arg) {
 		if (acpi_db_trace_method_name) {
diff --git a/drivers/acpi/acpica/dbconvert.c b/drivers/acpi/acpica/dbconvert.c
index a71632ca8a81..9fee88f1c654 100644
--- a/drivers/acpi/acpica/dbconvert.c
+++ b/drivers/acpi/acpica/dbconvert.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbdisply.c b/drivers/acpi/acpica/dbdisply.c
index 672977ec7c7d..502bb587f112 100644
--- a/drivers/acpi/acpica/dbdisply.c
+++ b/drivers/acpi/acpica/dbdisply.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -48,6 +48,7 @@
 #include "acnamesp.h"
 #include "acparser.h"
 #include "acinterp.h"
+#include "acevents.h"
 #include "acdebug.h"
 
 #define _COMPONENT          ACPI_CA_DEBUGGER
@@ -588,7 +589,7 @@ void acpi_db_display_calling_tree(void)
  *
  * FUNCTION:    acpi_db_display_object_type
  *
- * PARAMETERS:  name            - User entered NS node handle or name
+ * PARAMETERS:  object_arg      - User entered NS node handle
  *
  * RETURN:      None
  *
@@ -596,44 +597,36 @@ void acpi_db_display_calling_tree(void)
  *
  ******************************************************************************/
 
-void acpi_db_display_object_type(char *name)
+void acpi_db_display_object_type(char *object_arg)
 {
-	struct acpi_namespace_node *node;
+	acpi_size arg;
+	acpi_handle handle;
 	struct acpi_device_info *info;
 	acpi_status status;
 	u32 i;
 
-	node = acpi_db_convert_to_node(name);
-	if (!node) {
-		return;
-	}
+	arg = strtoul(object_arg, NULL, 16);
+	handle = ACPI_TO_POINTER(arg);
 
-	status = acpi_get_object_info(ACPI_CAST_PTR(acpi_handle, node), &info);
+	status = acpi_get_object_info(handle, &info);
 	if (ACPI_FAILURE(status)) {
 		acpi_os_printf("Could not get object info, %s\n",
 			       acpi_format_exception(status));
 		return;
 	}
 
-	if (info->valid & ACPI_VALID_ADR) {
-		acpi_os_printf("ADR: %8.8X%8.8X, STA: %8.8X, Flags: %X\n",
-			       ACPI_FORMAT_UINT64(info->address),
-			       info->current_status, info->flags);
-	}
-	if (info->valid & ACPI_VALID_SXDS) {
-		acpi_os_printf("S1D-%2.2X S2D-%2.2X S3D-%2.2X S4D-%2.2X\n",
-			       info->highest_dstates[0],
-			       info->highest_dstates[1],
-			       info->highest_dstates[2],
-			       info->highest_dstates[3]);
-	}
-	if (info->valid & ACPI_VALID_SXWS) {
-		acpi_os_printf
-		    ("S0W-%2.2X S1W-%2.2X S2W-%2.2X S3W-%2.2X S4W-%2.2X\n",
-		     info->lowest_dstates[0], info->lowest_dstates[1],
-		     info->lowest_dstates[2], info->lowest_dstates[3],
-		     info->lowest_dstates[4]);
-	}
+	acpi_os_printf("ADR: %8.8X%8.8X, STA: %8.8X, Flags: %X\n",
+		       ACPI_FORMAT_UINT64(info->address),
+		       info->current_status, info->flags);
+
+	acpi_os_printf("S1D-%2.2X S2D-%2.2X S3D-%2.2X S4D-%2.2X\n",
+		       info->highest_dstates[0], info->highest_dstates[1],
+		       info->highest_dstates[2], info->highest_dstates[3]);
+
+	acpi_os_printf("S0W-%2.2X S1W-%2.2X S2W-%2.2X S3W-%2.2X S4W-%2.2X\n",
+		       info->lowest_dstates[0], info->lowest_dstates[1],
+		       info->lowest_dstates[2], info->lowest_dstates[3],
+		       info->lowest_dstates[4]);
 
 	if (info->valid & ACPI_VALID_HID) {
 		acpi_os_printf("HID: %s\n", info->hardware_id.string);
@@ -643,10 +636,6 @@ void acpi_db_display_object_type(char *name)
 		acpi_os_printf("UID: %s\n", info->unique_id.string);
 	}
 
-	if (info->valid & ACPI_VALID_SUB) {
-		acpi_os_printf("SUB: %s\n", info->subsystem_id.string);
-	}
-
 	if (info->valid & ACPI_VALID_CID) {
 		for (i = 0; i < info->compatible_id_list.count; i++) {
 			acpi_os_printf("CID %u: %s\n", i,
@@ -679,6 +668,12 @@ acpi_db_display_result_object(union acpi_operand_object *obj_desc,
 			      struct acpi_walk_state *walk_state)
 {
 
+#ifndef ACPI_APPLICATION
+	if (acpi_gbl_db_thread_id != acpi_os_get_thread_id()) {
+		return;
+	}
+#endif
+
 	/* Only display if single stepping */
 
 	if (!acpi_gbl_cm_single_step) {
@@ -708,6 +703,12 @@ acpi_db_display_argument_object(union acpi_operand_object *obj_desc,
 				struct acpi_walk_state *walk_state)
 {
 
+#ifndef ACPI_APPLICATION
+	if (acpi_gbl_db_thread_id != acpi_os_get_thread_id()) {
+		return;
+	}
+#endif
+
 	if (!acpi_gbl_cm_single_step) {
 		return;
 	}
@@ -951,28 +952,25 @@ void acpi_db_display_handlers(void)
 	if (obj_desc) {
 		for (i = 0; i < ACPI_ARRAY_LENGTH(acpi_gbl_space_id_list); i++) {
 			space_id = acpi_gbl_space_id_list[i];
-			handler_obj = obj_desc->device.handler;
 
 			acpi_os_printf(ACPI_PREDEFINED_PREFIX,
 				       acpi_ut_get_region_name((u8)space_id),
 				       space_id);
 
-			while (handler_obj) {
-				if (acpi_gbl_space_id_list[i] ==
-				    handler_obj->address_space.space_id) {
-					acpi_os_printf
-					    (ACPI_HANDLER_PRESENT_STRING,
-					     (handler_obj->address_space.
-					      handler_flags &
-					      ACPI_ADDR_HANDLER_DEFAULT_INSTALLED)
-					     ? "Default" : "User",
-					     handler_obj->address_space.
-					     handler);
-
-					goto found_handler;
-				}
+			handler_obj =
+			    acpi_ev_find_region_handler(space_id,
+							obj_desc->common_notify.
+							handler);
+			if (handler_obj) {
+				acpi_os_printf(ACPI_HANDLER_PRESENT_STRING,
+					       (handler_obj->address_space.
+						handler_flags &
+						ACPI_ADDR_HANDLER_DEFAULT_INSTALLED)
+					       ? "Default" : "User",
+					       handler_obj->address_space.
+					       handler);
 
-				handler_obj = handler_obj->address_space.next;
+				goto found_handler;
 			}
 
 			/* There is no handler for this space_id */
@@ -984,7 +982,7 @@ found_handler:		;
 
 		/* Find all handlers for user-defined space_IDs */
 
-		handler_obj = obj_desc->device.handler;
+		handler_obj = obj_desc->common_notify.handler;
 		while (handler_obj) {
 			if (handler_obj->address_space.space_id >=
 			    ACPI_USER_REGION_BEGIN) {
@@ -1079,14 +1077,14 @@ acpi_db_display_non_root_handlers(acpi_handle obj_handle,
 		return (AE_OK);
 	}
 
-	pathname = acpi_ns_get_external_pathname(node);
+	pathname = acpi_ns_get_normalized_pathname(node, TRUE);
 	if (!pathname) {
 		return (AE_OK);
 	}
 
 	/* Display all handlers associated with this device */
 
-	handler_obj = obj_desc->device.handler;
+	handler_obj = obj_desc->common_notify.handler;
 	while (handler_obj) {
 		acpi_os_printf(ACPI_PREDEFINED_PREFIX,
 			       acpi_ut_get_region_name((u8)handler_obj->
diff --git a/drivers/acpi/acpica/dbexec.c b/drivers/acpi/acpica/dbexec.c
index d713e2df65b9..c814855376e2 100644
--- a/drivers/acpi/acpica/dbexec.c
+++ b/drivers/acpi/acpica/dbexec.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbfileio.c b/drivers/acpi/acpica/dbfileio.c
index d0e6b20ce82a..483287942372 100644
--- a/drivers/acpi/acpica/dbfileio.c
+++ b/drivers/acpi/acpica/dbfileio.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -46,6 +46,10 @@
 #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")
@@ -110,122 +114,31 @@ void acpi_db_open_debug_file(char *name)
 }
 #endif
 
-#ifdef ACPI_APPLICATION
-#include "acapps.h"
-
-/*******************************************************************************
- *
- * FUNCTION:    ae_local_load_table
- *
- * PARAMETERS:  table           - pointer to a buffer containing the entire
- *                                table to be loaded
- *
- * RETURN:      Status
- *
- * DESCRIPTION: This function is called to load a table from the caller's
- *              buffer. The buffer must contain an entire ACPI Table including
- *              a valid header. The header fields will be verified, and if it
- *              is determined that the table is invalid, the call will fail.
- *
- ******************************************************************************/
-
-static acpi_status ae_local_load_table(struct acpi_table_header *table)
-{
-	acpi_status status = AE_OK;
-
-	ACPI_FUNCTION_TRACE(ae_local_load_table);
-
-#if 0
-/*    struct acpi_table_desc          table_info; */
-
-	if (!table) {
-		return_ACPI_STATUS(AE_BAD_PARAMETER);
-	}
-
-	table_info.pointer = table;
-	status = acpi_tb_recognize_table(&table_info, ACPI_TABLE_ALL);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
-	/* Install the new table into the local data structures */
-
-	status = acpi_tb_init_table_descriptor(&table_info);
-	if (ACPI_FAILURE(status)) {
-		if (status == AE_ALREADY_EXISTS) {
-
-			/* Table already exists, no error */
-
-			status = AE_OK;
-		}
-
-		/* Free table allocated by acpi_tb_get_table */
-
-		acpi_tb_delete_single_table(&table_info);
-		return_ACPI_STATUS(status);
-	}
-#if (!defined (ACPI_NO_METHOD_EXECUTION) && !defined (ACPI_CONSTANT_EVAL_ONLY))
-
-	status =
-	    acpi_ns_load_table(table_info.installed_desc, acpi_gbl_root_node);
-	if (ACPI_FAILURE(status)) {
-
-		/* Uninstall table and free the buffer */
-
-		acpi_tb_delete_tables_by_type(ACPI_TABLE_ID_DSDT);
-		return_ACPI_STATUS(status);
-	}
-#endif
-#endif
-
-	return_ACPI_STATUS(status);
-}
-#endif
-
 /*******************************************************************************
  *
- * FUNCTION:    acpi_db_get_table_from_file
+ * FUNCTION:    acpi_db_load_tables
  *
- * PARAMETERS:  filename        - File where table is located
- *              return_table    - Where a pointer to the table is returned
+ * PARAMETERS:  list_head       - List of ACPI tables to load
  *
  * RETURN:      Status
  *
- * DESCRIPTION: Load an ACPI table from a file
+ * DESCRIPTION: Load ACPI tables from a previously constructed table list.
  *
  ******************************************************************************/
 
-acpi_status
-acpi_db_get_table_from_file(char *filename,
-			    struct acpi_table_header **return_table,
-			    u8 must_be_aml_file)
+acpi_status acpi_db_load_tables(struct acpi_new_table_desc *list_head)
 {
-#ifdef ACPI_APPLICATION
 	acpi_status status;
+	struct acpi_new_table_desc *table_list_head;
 	struct acpi_table_header *table;
-	u8 is_aml_table = TRUE;
-
-	status = acpi_ut_read_table_from_file(filename, &table);
-	if (ACPI_FAILURE(status)) {
-		return (status);
-	}
-
-	if (must_be_aml_file) {
-		is_aml_table = acpi_ut_is_aml_table(table);
-		if (!is_aml_table) {
-			ACPI_EXCEPTION((AE_INFO, AE_OK,
-					"Input for -e is not an AML table: "
-					"\"%4.4s\" (must be DSDT/SSDT)",
-					table->signature));
-			return (AE_TYPE);
-		}
-	}
 
-	if (is_aml_table) {
+	/* Load all ACPI tables in the list */
 
-		/* Attempt to recognize and install the table */
+	table_list_head = list_head;
+	while (table_list_head) {
+		table = table_list_head->table;
 
-		status = ae_local_load_table(table);
+		status = acpi_load_table(table);
 		if (ACPI_FAILURE(status)) {
 			if (status == AE_ALREADY_EXISTS) {
 				acpi_os_printf
@@ -239,18 +152,12 @@ acpi_db_get_table_from_file(char *filename,
 			return (status);
 		}
 
-		acpi_tb_print_table_header(0, table);
-
 		fprintf(stderr,
 			"Acpi table [%4.4s] successfully installed and loaded\n",
 			table->signature);
-	}
 
-	acpi_gbl_acpi_hardware_present = FALSE;
-	if (return_table) {
-		*return_table = table;
+		table_list_head = table_list_head->next;
 	}
 
-#endif				/* ACPI_APPLICATION */
 	return (AE_OK);
 }
diff --git a/drivers/acpi/acpica/dbhistry.c b/drivers/acpi/acpica/dbhistry.c
index 9c66a9eadd38..46bd65d38df9 100644
--- a/drivers/acpi/acpica/dbhistry.c
+++ b/drivers/acpi/acpica/dbhistry.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbinput.c b/drivers/acpi/acpica/dbinput.c
index 0480254437f1..417c02a89915 100644
--- a/drivers/acpi/acpica/dbinput.c
+++ b/drivers/acpi/acpica/dbinput.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -45,6 +45,10 @@
 #include "accommon.h"
 #include "acdebug.h"
 
+#ifdef ACPI_APPLICATION
+#include "acapps.h"
+#endif
+
 #define _COMPONENT          ACPI_CA_DEBUGGER
 ACPI_MODULE_NAME("dbinput")
 
@@ -53,8 +57,6 @@ static u32 acpi_db_get_line(char *input_buffer);
 
 static u32 acpi_db_match_command(char *user_command);
 
-static void acpi_db_single_thread(void);
-
 static void acpi_db_display_command_info(char *command, u8 display_all);
 
 static void acpi_db_display_help(char *command);
@@ -623,9 +625,7 @@ static u32 acpi_db_get_line(char *input_buffer)
 
 	/* Uppercase the actual command */
 
-	if (acpi_gbl_db_args[0]) {
-		acpi_ut_strupr(acpi_gbl_db_args[0]);
-	}
+	acpi_ut_strupr(acpi_gbl_db_args[0]);
 
 	count = i;
 	if (count) {
@@ -1050,11 +1050,17 @@ acpi_db_command_dispatch(char *input_buffer,
 		acpi_db_close_debug_file();
 		break;
 
-	case CMD_LOAD:
+	case CMD_LOAD:{
+			struct acpi_new_table_desc *list_head = NULL;
 
-		status =
-		    acpi_db_get_table_from_file(acpi_gbl_db_args[1], NULL,
-						FALSE);
+			status =
+			    ac_get_all_tables_from_file(acpi_gbl_db_args[1],
+							ACPI_GET_ALL_TABLES,
+							&list_head);
+			if (ACPI_SUCCESS(status)) {
+				acpi_db_load_tables(list_head);
+			}
+		}
 		break;
 
 	case CMD_OPEN:
@@ -1149,55 +1155,16 @@ acpi_db_command_dispatch(char *input_buffer,
 
 void ACPI_SYSTEM_XFACE acpi_db_execute_thread(void *context)
 {
-	acpi_status status = AE_OK;
-	acpi_status Mstatus;
-
-	while (status != AE_CTRL_TERMINATE && !acpi_gbl_db_terminate_loop) {
-		acpi_gbl_method_executing = FALSE;
-		acpi_gbl_step_to_next_call = FALSE;
-
-		Mstatus = acpi_os_acquire_mutex(acpi_gbl_db_command_ready,
-						ACPI_WAIT_FOREVER);
-		if (ACPI_FAILURE(Mstatus)) {
-			return;
-		}
-
-		status =
-		    acpi_db_command_dispatch(acpi_gbl_db_line_buf, NULL, NULL);
 
-		acpi_os_release_mutex(acpi_gbl_db_command_complete);
-	}
+	(void)acpi_db_user_commands();
 	acpi_gbl_db_threads_terminated = TRUE;
 }
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_db_single_thread
- *
- * PARAMETERS:  None
- *
- * RETURN:      None
- *
- * DESCRIPTION: Debugger execute thread. Waits for a command line, then
- *              simply dispatches it.
- *
- ******************************************************************************/
-
-static void acpi_db_single_thread(void)
-{
-
-	acpi_gbl_method_executing = FALSE;
-	acpi_gbl_step_to_next_call = FALSE;
-
-	(void)acpi_db_command_dispatch(acpi_gbl_db_line_buf, NULL, NULL);
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_db_user_commands
  *
- * PARAMETERS:  prompt              - User prompt (depends on mode)
- *              op                  - Current executing parse op
+ * PARAMETERS:  None
  *
  * RETURN:      None
  *
@@ -1206,7 +1173,7 @@ static void acpi_db_single_thread(void)
  *
  ******************************************************************************/
 
-acpi_status acpi_db_user_commands(char prompt, union acpi_parse_object *op)
+acpi_status acpi_db_user_commands(void)
 {
 	acpi_status status = AE_OK;
 
@@ -1216,52 +1183,31 @@ acpi_status acpi_db_user_commands(char prompt, union acpi_parse_object *op)
 
 	while (!acpi_gbl_db_terminate_loop) {
 
-		/* Force output to console until a command is entered */
-
-		acpi_db_set_output_destination(ACPI_DB_CONSOLE_OUTPUT);
-
-		/* Different prompt if method is executing */
-
-		if (!acpi_gbl_method_executing) {
-			acpi_os_printf("%1c ", ACPI_DEBUGGER_COMMAND_PROMPT);
-		} else {
-			acpi_os_printf("%1c ", ACPI_DEBUGGER_EXECUTE_PROMPT);
-		}
+		/* Wait the readiness of the command */
 
-		/* Get the user input line */
-
-		status = acpi_os_get_line(acpi_gbl_db_line_buf,
-					  ACPI_DB_LINE_BUFFER_SIZE, NULL);
+		status = acpi_os_wait_command_ready();
 		if (ACPI_FAILURE(status)) {
-			ACPI_EXCEPTION((AE_INFO, status,
-					"While parsing command line"));
-			return (status);
+			break;
 		}
 
-		/* Check for single or multithreaded debug */
+		/* Just call to the command line interpreter */
 
-		if (acpi_gbl_debugger_configuration & DEBUGGER_MULTI_THREADED) {
-			/*
-			 * Signal the debug thread that we have a command to execute,
-			 * and wait for the command to complete.
-			 */
-			acpi_os_release_mutex(acpi_gbl_db_command_ready);
-			if (ACPI_FAILURE(status)) {
-				return (status);
-			}
+		acpi_gbl_method_executing = FALSE;
+		acpi_gbl_step_to_next_call = FALSE;
 
-			status =
-			    acpi_os_acquire_mutex(acpi_gbl_db_command_complete,
-						  ACPI_WAIT_FOREVER);
-			if (ACPI_FAILURE(status)) {
-				return (status);
-			}
-		} else {
-			/* Just call to the command line interpreter */
+		(void)acpi_db_command_dispatch(acpi_gbl_db_line_buf, NULL,
+					       NULL);
+
+		/* Notify the completion of the command */
 
-			acpi_db_single_thread();
+		status = acpi_os_notify_command_complete();
+		if (ACPI_FAILURE(status)) {
+			break;
 		}
 	}
 
+	if (ACPI_FAILURE(status) && status != AE_CTRL_TERMINATE) {
+		ACPI_EXCEPTION((AE_INFO, status, "While parsing command line"));
+	}
 	return (status);
 }
diff --git a/drivers/acpi/acpica/dbmethod.c b/drivers/acpi/acpica/dbmethod.c
index 01e5a71147fd..f17a86f6b16b 100644
--- a/drivers/acpi/acpica/dbmethod.c
+++ b/drivers/acpi/acpica/dbmethod.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbnames.c b/drivers/acpi/acpica/dbnames.c
index 04ff1ebfda58..3c23b5a1079b 100644
--- a/drivers/acpi/acpica/dbnames.c
+++ b/drivers/acpi/acpica/dbnames.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -438,7 +438,7 @@ acpi_db_walk_for_predefined_names(acpi_handle obj_handle,
 		return (AE_OK);
 	}
 
-	pathname = acpi_ns_get_external_pathname(node);
+	pathname = acpi_ns_get_normalized_pathname(node, TRUE);
 	if (!pathname) {
 		return (AE_OK);
 	}
diff --git a/drivers/acpi/acpica/dbobject.c b/drivers/acpi/acpica/dbobject.c
index 116f6db8c2ed..1d59e8b6f859 100644
--- a/drivers/acpi/acpica/dbobject.c
+++ b/drivers/acpi/acpica/dbobject.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/dbstats.c b/drivers/acpi/acpica/dbstats.c
index 4ba0a20811eb..a414e1fa6f9d 100644
--- a/drivers/acpi/acpica/dbstats.c
+++ b/drivers/acpi/acpica/dbstats.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -382,6 +382,7 @@ acpi_status acpi_db_display_statistics(char *type_arg)
 				       acpi_gbl_node_type_count[i],
 				       acpi_gbl_obj_type_count[i]);
 		}
+
 		acpi_os_printf("%16.16s % 10ld% 10ld\n", "Misc/Unknown",
 			       acpi_gbl_node_type_count_misc,
 			       acpi_gbl_obj_type_count_misc);
diff --git a/drivers/acpi/acpica/dbtest.c b/drivers/acpi/acpica/dbtest.c
index 10ea8bf9b810..74aa38156cdc 100644
--- a/drivers/acpi/acpica/dbtest.c
+++ b/drivers/acpi/acpica/dbtest.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -953,7 +953,7 @@ acpi_db_evaluate_one_predefined_name(acpi_handle obj_handle,
 		return (AE_OK);
 	}
 
-	pathname = acpi_ns_get_external_pathname(node);
+	pathname = acpi_ns_get_normalized_pathname(node, TRUE);
 	if (!pathname) {
 		return (AE_OK);
 	}
diff --git a/drivers/acpi/acpica/dbutils.c b/drivers/acpi/acpica/dbutils.c
index 86790e080139..b37a2c77b86b 100644
--- a/drivers/acpi/acpica/dbutils.c
+++ b/drivers/acpi/acpica/dbutils.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -173,6 +173,7 @@ void acpi_db_dump_external_object(union acpi_object *obj_desc, u32 level)
 			if (obj_desc->buffer.length > 16) {
 				acpi_os_printf("\n");
 			}
+
 			acpi_ut_debug_dump_buffer(ACPI_CAST_PTR
 						  (u8,
 						   obj_desc->buffer.pointer),
diff --git a/drivers/acpi/acpica/dbxface.c b/drivers/acpi/acpica/dbxface.c
index 342298a6e10f..e94e0d80bc7b 100644
--- a/drivers/acpi/acpica/dbxface.c
+++ b/drivers/acpi/acpica/dbxface.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -85,46 +85,21 @@ acpi_db_start_command(struct acpi_walk_state *walk_state,
 
 	acpi_gbl_method_executing = TRUE;
 	status = AE_CTRL_TRUE;
-	while (status == AE_CTRL_TRUE) {
-		if (acpi_gbl_debugger_configuration == DEBUGGER_MULTI_THREADED) {
-
-			/* Handshake with the front-end that gets user command lines */
-
-			acpi_os_release_mutex(acpi_gbl_db_command_complete);
-
-			status =
-			    acpi_os_acquire_mutex(acpi_gbl_db_command_ready,
-						  ACPI_WAIT_FOREVER);
-			if (ACPI_FAILURE(status)) {
-				return (status);
-			}
-		} else {
-			/* Single threaded, we must get a command line ourselves */
-
-			/* Force output to console until a command is entered */
 
-			acpi_db_set_output_destination(ACPI_DB_CONSOLE_OUTPUT);
+	while (status == AE_CTRL_TRUE) {
 
-			/* Different prompt if method is executing */
+		/* Notify the completion of the command */
 
-			if (!acpi_gbl_method_executing) {
-				acpi_os_printf("%1c ",
-					       ACPI_DEBUGGER_COMMAND_PROMPT);
-			} else {
-				acpi_os_printf("%1c ",
-					       ACPI_DEBUGGER_EXECUTE_PROMPT);
-			}
+		status = acpi_os_notify_command_complete();
+		if (ACPI_FAILURE(status)) {
+			goto error_exit;
+		}
 
-			/* Get the user input line */
+		/* Wait the readiness of the command */
 
-			status = acpi_os_get_line(acpi_gbl_db_line_buf,
-						  ACPI_DB_LINE_BUFFER_SIZE,
-						  NULL);
-			if (ACPI_FAILURE(status)) {
-				ACPI_EXCEPTION((AE_INFO, status,
-						"While parsing command line"));
-				return (status);
-			}
+		status = acpi_os_wait_command_ready();
+		if (ACPI_FAILURE(status)) {
+			goto error_exit;
 		}
 
 		status =
@@ -134,11 +109,46 @@ acpi_db_start_command(struct acpi_walk_state *walk_state,
 
 	/* acpi_ut_acquire_mutex (ACPI_MTX_NAMESPACE); */
 
+error_exit:
+	if (ACPI_FAILURE(status) && status != AE_CTRL_TERMINATE) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"While parsing/handling command line"));
+	}
 	return (status);
 }
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_db_signal_break_point
+ *
+ * PARAMETERS:  walk_state      - Current walk
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Called for AML_BREAK_POINT_OP
+ *
+ ******************************************************************************/
+
+void acpi_db_signal_break_point(struct acpi_walk_state *walk_state)
+{
+
+#ifndef ACPI_APPLICATION
+	if (acpi_gbl_db_thread_id != acpi_os_get_thread_id()) {
+		return;
+	}
+#endif
+
+	/*
+	 * Set the single-step flag. This will cause the debugger (if present)
+	 * to break to the console within the AML debugger at the start of the
+	 * next AML instruction.
+	 */
+	acpi_gbl_cm_single_step = TRUE;
+	acpi_os_printf("**break** Executed AML BreakPoint opcode\n");
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_db_single_step
  *
  * PARAMETERS:  walk_state      - Current walk
@@ -420,15 +430,7 @@ acpi_status acpi_initialize_debugger(void)
 
 		/* These were created with one unit, grab it */
 
-		status = acpi_os_acquire_mutex(acpi_gbl_db_command_complete,
-					       ACPI_WAIT_FOREVER);
-		if (ACPI_FAILURE(status)) {
-			acpi_os_printf("Could not get debugger mutex\n");
-			return_ACPI_STATUS(status);
-		}
-
-		status = acpi_os_acquire_mutex(acpi_gbl_db_command_ready,
-					       ACPI_WAIT_FOREVER);
+		status = acpi_os_initialize_command_signals();
 		if (ACPI_FAILURE(status)) {
 			acpi_os_printf("Could not get debugger mutex\n");
 			return_ACPI_STATUS(status);
@@ -473,13 +475,14 @@ void acpi_terminate_debugger(void)
 	acpi_gbl_db_terminate_loop = TRUE;
 
 	if (acpi_gbl_debugger_configuration & DEBUGGER_MULTI_THREADED) {
-		acpi_os_release_mutex(acpi_gbl_db_command_ready);
 
 		/* Wait the AML Debugger threads */
 
 		while (!acpi_gbl_db_threads_terminated) {
 			acpi_os_sleep(100);
 		}
+
+		acpi_os_terminate_command_signals();
 	}
 
 	if (acpi_gbl_db_buffer) {
diff --git a/drivers/acpi/acpica/dsargs.c b/drivers/acpi/acpica/dsargs.c
index e2ab59e39162..ad0413beeeae 100644
--- a/drivers/acpi/acpica/dsargs.c
+++ b/drivers/acpi/acpica/dsargs.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -194,8 +194,8 @@ acpi_ds_get_buffer_field_arguments(union acpi_operand_object *obj_desc)
 	extra_desc = acpi_ns_get_secondary_object(obj_desc);
 	node = obj_desc->buffer_field.node;
 
-	ACPI_DEBUG_EXEC(acpi_ut_display_init_pathname(ACPI_TYPE_BUFFER_FIELD,
-						      node, NULL));
+	ACPI_DEBUG_EXEC(acpi_ut_display_init_pathname
+			(ACPI_TYPE_BUFFER_FIELD, node, NULL));
 
 	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "[%4.4s] BufferField Arg Init\n",
 			  acpi_ut_get_node_name(node)));
@@ -385,7 +385,8 @@ acpi_status acpi_ds_get_region_arguments(union acpi_operand_object *obj_desc)
 	ACPI_DEBUG_EXEC(acpi_ut_display_init_pathname
 			(ACPI_TYPE_REGION, node, NULL));
 
-	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "[%4.4s] OpRegion Arg Init at AML %p\n",
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "[%4.4s] OpRegion Arg Init at AML %p\n",
 			  acpi_ut_get_node_name(node),
 			  extra_desc->extra.aml_start));
 
diff --git a/drivers/acpi/acpica/dscontrol.c b/drivers/acpi/acpica/dscontrol.c
index 435fc16e2f83..c9a663f21ac8 100644
--- a/drivers/acpi/acpica/dscontrol.c
+++ b/drivers/acpi/acpica/dscontrol.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -47,6 +47,7 @@
 #include "amlcode.h"
 #include "acdispat.h"
 #include "acinterp.h"
+#include "acdebug.h"
 
 #define _COMPONENT          ACPI_DISPATCHER
 ACPI_MODULE_NAME("dscontrol")
@@ -348,14 +349,7 @@ acpi_ds_exec_end_control_op(struct acpi_walk_state * walk_state,
 
 	case AML_BREAK_POINT_OP:
 
-		/*
-		 * Set the single-step flag. This will cause the debugger (if present)
-		 * to break to the console within the AML debugger at the start of the
-		 * next AML instruction.
-		 */
-		ACPI_DEBUGGER_EXEC(acpi_gbl_cm_single_step = TRUE);
-		ACPI_DEBUGGER_EXEC(acpi_os_printf
-				   ("**break** Executed AML BreakPoint opcode\n"));
+		acpi_db_signal_break_point(walk_state);
 
 		/* Call to the OSL in case OS wants a piece of the action */
 
diff --git a/drivers/acpi/acpica/dsdebug.c b/drivers/acpi/acpica/dsdebug.c
index 309556efc553..56c3aadb4cba 100644
--- a/drivers/acpi/acpica/dsdebug.c
+++ b/drivers/acpi/acpica/dsdebug.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -161,6 +161,7 @@ acpi_ds_dump_method_stack(acpi_status status,
 	ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
 			  "\n**** Exception %s during execution of method ",
 			  acpi_format_exception(status)));
+
 	acpi_ds_print_node_pathname(walk_state->method_node, NULL);
 
 	/* Display stack of executing methods */
@@ -203,8 +204,8 @@ acpi_ds_dump_method_stack(acpi_status status,
 		} else {
 			/*
 			 * This method has called another method
-			 * NOTE: the method call parse subtree is already deleted at this
-			 * point, so we cannot disassemble the method invocation.
+			 * NOTE: the method call parse subtree is already deleted at
+			 * this point, so we cannot disassemble the method invocation.
 			 */
 			ACPI_DEBUG_PRINT_RAW((ACPI_DB_DISPATCH,
 					      "Call to method "));
diff --git a/drivers/acpi/acpica/dsfield.c b/drivers/acpi/acpica/dsfield.c
index 20de148594fd..6a4b603d0e83 100644
--- a/drivers/acpi/acpica/dsfield.c
+++ b/drivers/acpi/acpica/dsfield.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -106,6 +106,7 @@ acpi_ds_create_external_region(acpi_status lookup_status,
 	 * insert the name into the namespace.
 	 */
 	acpi_dm_add_op_to_external_list(op, path, ACPI_TYPE_REGION, 0, 0);
+
 	status = acpi_ns_lookup(walk_state->scope_info, path, ACPI_TYPE_REGION,
 				ACPI_IMODE_LOAD_PASS1, ACPI_NS_SEARCH_PARENT,
 				walk_state, node);
@@ -202,11 +203,10 @@ acpi_ds_create_buffer_field(union acpi_parse_object *op,
 
 		/* Enter the name_string into the namespace */
 
-		status =
-		    acpi_ns_lookup(walk_state->scope_info,
-				   arg->common.value.string, ACPI_TYPE_ANY,
-				   ACPI_IMODE_LOAD_PASS1, flags, walk_state,
-				   &node);
+		status = acpi_ns_lookup(walk_state->scope_info,
+					arg->common.value.string, ACPI_TYPE_ANY,
+					ACPI_IMODE_LOAD_PASS1, flags,
+					walk_state, &node);
 		if (ACPI_FAILURE(status)) {
 			ACPI_ERROR_NAMESPACE(arg->common.value.string, status);
 			return_ACPI_STATUS(status);
@@ -244,8 +244,8 @@ acpi_ds_create_buffer_field(union acpi_parse_object *op,
 	}
 
 	/*
-	 * Remember location in AML stream of the field unit opcode and operands --
-	 * since the buffer and index operands must be evaluated.
+	 * Remember location in AML stream of the field unit opcode and operands
+	 * -- since the buffer and index operands must be evaluated.
 	 */
 	second_desc = obj_desc->common.next_object;
 	second_desc->extra.aml_start = op->named.data;
@@ -310,8 +310,8 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 		switch (arg->common.aml_opcode) {
 		case AML_INT_RESERVEDFIELD_OP:
 
-			position = (u64) info->field_bit_position
-			    + (u64) arg->common.value.size;
+			position = (u64)info->field_bit_position +
+			    (u64)arg->common.value.size;
 
 			if (position > ACPI_UINT32_MAX) {
 				ACPI_ERROR((AE_INFO,
@@ -344,13 +344,13 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 
 			/* access_attribute (attrib_quick, attrib_byte, etc.) */
 
-			info->attribute =
-			    (u8)((arg->common.value.integer >> 8) & 0xFF);
+			info->attribute = (u8)
+			    ((arg->common.value.integer >> 8) & 0xFF);
 
 			/* access_length (for serial/buffer protocols) */
 
-			info->access_length =
-			    (u8)((arg->common.value.integer >> 16) & 0xFF);
+			info->access_length = (u8)
+			    ((arg->common.value.integer >> 16) & 0xFF);
 			break;
 
 		case AML_INT_CONNECTION_OP:
@@ -425,8 +425,8 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 
 			/* Keep track of bit position for the next field */
 
-			position = (u64) info->field_bit_position
-			    + (u64) arg->common.value.size;
+			position = (u64)info->field_bit_position +
+			    (u64)arg->common.value.size;
 
 			if (position > ACPI_UINT32_MAX) {
 				ACPI_ERROR((AE_INFO,
@@ -716,11 +716,12 @@ acpi_ds_create_bank_field(union acpi_parse_object *op,
 
 	/*
 	 * Use Info.data_register_node to store bank_field Op
-	 * It's safe because data_register_node will never be used when create bank field
-	 * We store aml_start and aml_length in the bank_field Op for late evaluation
-	 * Used in acpi_ex_prep_field_value(Info)
+	 * It's safe because data_register_node will never be used when create
+	 * bank field \we store aml_start and aml_length in the bank_field Op for
+	 * late evaluation. Used in acpi_ex_prep_field_value(Info)
 	 *
-	 * TBD: Or, should we add a field in struct acpi_create_field_info, like "void *ParentOp"?
+	 * TBD: Or, should we add a field in struct acpi_create_field_info, like
+	 * "void *ParentOp"?
 	 */
 	info.data_register_node = (struct acpi_namespace_node *)op;
 
diff --git a/drivers/acpi/acpica/dsinit.c b/drivers/acpi/acpica/dsinit.c
index 920f1b199bc6..5aa1c5feee50 100644
--- a/drivers/acpi/acpica/dsinit.c
+++ b/drivers/acpi/acpica/dsinit.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -247,7 +247,7 @@ acpi_ds_initialize_objects(u32 table_index,
 	/* Summary of objects initialized */
 
 	ACPI_DEBUG_PRINT_RAW((ACPI_DB_INIT,
-			      "Table [%4.4s:%8.8s] (id %.2X) - %4u Objects with %3u Devices, "
+			      "Table [%4.4s: %-8.8s] (id %.2X) - %4u Objects with %3u Devices, "
 			      "%3u Regions, %4u Methods (%u/%u/%u Serial/Non/Cvt)\n",
 			      table->signature, table->oem_table_id, owner_id,
 			      info.object_count, info.device_count,
diff --git a/drivers/acpi/acpica/dsmethod.c b/drivers/acpi/acpica/dsmethod.c
index bc32f3194afe..6a72047aae1c 100644
--- a/drivers/acpi/acpica/dsmethod.c
+++ b/drivers/acpi/acpica/dsmethod.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -118,10 +118,9 @@ acpi_ds_auto_serialize_method(struct acpi_namespace_node *node,
 		return_ACPI_STATUS(AE_NO_MEMORY);
 	}
 
-	status =
-	    acpi_ds_init_aml_walk(walk_state, op, node,
-				  obj_desc->method.aml_start,
-				  obj_desc->method.aml_length, NULL, 0);
+	status = acpi_ds_init_aml_walk(walk_state, op, node,
+				       obj_desc->method.aml_start,
+				       obj_desc->method.aml_length, NULL, 0);
 	if (ACPI_FAILURE(status)) {
 		acpi_ds_delete_walk_state(walk_state);
 		acpi_ps_free_op(op);
@@ -375,7 +374,8 @@ acpi_ds_begin_method_execution(struct acpi_namespace_node *method_node,
 		    && (walk_state->thread->current_sync_level >
 			obj_desc->method.mutex->mutex.sync_level)) {
 			ACPI_ERROR((AE_INFO,
-				    "Cannot acquire Mutex for method [%4.4s], current SyncLevel is too large (%u)",
+				    "Cannot acquire Mutex for method [%4.4s]"
+				    ", current SyncLevel is too large (%u)",
 				    acpi_ut_get_node_name(method_node),
 				    walk_state->thread->current_sync_level));
 
@@ -411,8 +411,19 @@ acpi_ds_begin_method_execution(struct acpi_namespace_node *method_node,
 
 				obj_desc->method.mutex->mutex.thread_id =
 				    walk_state->thread->thread_id;
-				walk_state->thread->current_sync_level =
-				    obj_desc->method.sync_level;
+
+				/*
+				 * Update the current sync_level only if this is not an auto-
+				 * serialized method. In the auto case, we have to ignore
+				 * the sync level for the method mutex (created for the
+				 * auto-serialization) because we have no idea of what the
+				 * sync level should be. Therefore, just ignore it.
+				 */
+				if (!(obj_desc->method.info_flags &
+				      ACPI_METHOD_IGNORE_SYNC_LEVEL)) {
+					walk_state->thread->current_sync_level =
+					    obj_desc->method.sync_level;
+				}
 			} else {
 				obj_desc->method.mutex->mutex.
 				    original_sync_level =
@@ -501,16 +512,18 @@ acpi_ds_call_control_method(struct acpi_thread_state *thread,
 
 	/* Init for new method, possibly wait on method mutex */
 
-	status = acpi_ds_begin_method_execution(method_node, obj_desc,
-						this_walk_state);
+	status =
+	    acpi_ds_begin_method_execution(method_node, obj_desc,
+					   this_walk_state);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
 
 	/* Begin method parse/execution. Create a new walk state */
 
-	next_walk_state = acpi_ds_create_walk_state(obj_desc->method.owner_id,
-						    NULL, obj_desc, thread);
+	next_walk_state =
+	    acpi_ds_create_walk_state(obj_desc->method.owner_id, NULL, obj_desc,
+				      thread);
 	if (!next_walk_state) {
 		status = AE_NO_MEMORY;
 		goto cleanup;
@@ -797,7 +810,8 @@ acpi_ds_terminate_control_method(union acpi_operand_object *method_desc,
 		    info_flags & ACPI_METHOD_SERIALIZED_PENDING) {
 			if (walk_state) {
 				ACPI_INFO((AE_INFO,
-					   "Marking method %4.4s as Serialized because of AE_ALREADY_EXISTS error",
+					   "Marking method %4.4s as Serialized "
+					   "because of AE_ALREADY_EXISTS error",
 					   walk_state->method_node->name.
 					   ascii));
 			}
@@ -815,6 +829,7 @@ acpi_ds_terminate_control_method(union acpi_operand_object *method_desc,
 			 */
 			method_desc->method.info_flags &=
 			    ~ACPI_METHOD_SERIALIZED_PENDING;
+
 			method_desc->method.info_flags |=
 			    (ACPI_METHOD_SERIALIZED |
 			     ACPI_METHOD_IGNORE_SYNC_LEVEL);
diff --git a/drivers/acpi/acpica/dsmthdat.c b/drivers/acpi/acpica/dsmthdat.c
index 2e4c42b377ec..45cbebaa32c0 100644
--- a/drivers/acpi/acpica/dsmthdat.c
+++ b/drivers/acpi/acpica/dsmthdat.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -99,6 +99,7 @@ void acpi_ds_method_data_init(struct acpi_walk_state *walk_state)
 	for (i = 0; i < ACPI_METHOD_NUM_ARGS; i++) {
 		ACPI_MOVE_32_TO_32(&walk_state->arguments[i].name,
 				   NAMEOF_ARG_NTE);
+
 		walk_state->arguments[i].name.integer |= (i << 24);
 		walk_state->arguments[i].descriptor_type = ACPI_DESC_TYPE_NAMED;
 		walk_state->arguments[i].type = ACPI_TYPE_ANY;
@@ -201,7 +202,7 @@ acpi_ds_method_data_init_args(union acpi_operand_object **params,
 
 	if (!params) {
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "No param list passed to method\n"));
+				  "No parameter list passed to method\n"));
 		return_ACPI_STATUS(AE_OK);
 	}
 
@@ -214,9 +215,9 @@ acpi_ds_method_data_init_args(union acpi_operand_object **params,
 		 * Store the argument in the method/walk descriptor.
 		 * Do not copy the arg in order to implement call by reference
 		 */
-		status = acpi_ds_method_data_set_value(ACPI_REFCLASS_ARG, index,
-						       params[index],
-						       walk_state);
+		status =
+		    acpi_ds_method_data_set_value(ACPI_REFCLASS_ARG, index,
+						  params[index], walk_state);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -610,11 +611,11 @@ acpi_ds_store_object_to_local(u8 type,
 			 * do the indirect store
 			 */
 			if ((ACPI_GET_DESCRIPTOR_TYPE(current_obj_desc) ==
-			     ACPI_DESC_TYPE_OPERAND)
-			    && (current_obj_desc->common.type ==
-				ACPI_TYPE_LOCAL_REFERENCE)
-			    && (current_obj_desc->reference.class ==
-				ACPI_REFCLASS_REFOF)) {
+			     ACPI_DESC_TYPE_OPERAND) &&
+			    (current_obj_desc->common.type ==
+			     ACPI_TYPE_LOCAL_REFERENCE) &&
+			    (current_obj_desc->reference.class ==
+			     ACPI_REFCLASS_REFOF)) {
 				ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
 						  "Arg (%p) is an ObjRef(Node), storing in node %p\n",
 						  new_obj_desc,
@@ -638,6 +639,7 @@ acpi_ds_store_object_to_local(u8 type,
 				if (new_obj_desc != obj_desc) {
 					acpi_ut_remove_reference(new_obj_desc);
 				}
+
 				return_ACPI_STATUS(status);
 			}
 		}
diff --git a/drivers/acpi/acpica/dsobject.c b/drivers/acpi/acpica/dsobject.c
index 2beb7fd674ae..c303e9d9266f 100644
--- a/drivers/acpi/acpica/dsobject.c
+++ b/drivers/acpi/acpica/dsobject.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -463,10 +463,10 @@ acpi_ds_build_internal_package_obj(struct acpi_walk_state *walk_state,
 						  arg->common.node);
 			}
 		} else {
-			status = acpi_ds_build_internal_object(walk_state, arg,
-							       &obj_desc->
-							       package.
-							       elements[i]);
+			status =
+			    acpi_ds_build_internal_object(walk_state, arg,
+							  &obj_desc->package.
+							  elements[i]);
 		}
 
 		if (*obj_desc_ptr) {
@@ -525,7 +525,8 @@ acpi_ds_build_internal_package_obj(struct acpi_walk_state *walk_state,
 		}
 
 		ACPI_INFO((AE_INFO,
-			   "Actual Package length (%u) is larger than NumElements field (%u), truncated",
+			   "Actual Package length (%u) is larger than "
+			   "NumElements field (%u), truncated",
 			   i, element_count));
 	} else if (i < element_count) {
 		/*
@@ -533,7 +534,8 @@ acpi_ds_build_internal_package_obj(struct acpi_walk_state *walk_state,
 		 * Note: this is not an error, the package is padded out with NULLs.
 		 */
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "Package List length (%u) smaller than NumElements count (%u), padded with null elements\n",
+				  "Package List length (%u) smaller than NumElements "
+				  "count (%u), padded with null elements\n",
 				  i, element_count));
 	}
 
@@ -584,8 +586,9 @@ acpi_ds_create_node(struct acpi_walk_state *walk_state,
 
 	/* Build an internal object for the argument(s) */
 
-	status = acpi_ds_build_internal_object(walk_state, op->common.value.arg,
-					       &obj_desc);
+	status =
+	    acpi_ds_build_internal_object(walk_state, op->common.value.arg,
+					  &obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
diff --git a/drivers/acpi/acpica/dsopcode.c b/drivers/acpi/acpica/dsopcode.c
index 81d7b9863e32..4cc9d989a114 100644
--- a/drivers/acpi/acpica/dsopcode.c
+++ b/drivers/acpi/acpica/dsopcode.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -243,8 +243,9 @@ acpi_ds_init_buffer_field(u16 aml_opcode,
 	 * For field_flags, use LOCK_RULE = 0 (NO_LOCK),
 	 * UPDATE_RULE = 0 (UPDATE_PRESERVE)
 	 */
-	status = acpi_ex_prep_common_field_object(obj_desc, field_flags, 0,
-						  bit_offset, bit_count);
+	status =
+	    acpi_ex_prep_common_field_object(obj_desc, field_flags, 0,
+					     bit_offset, bit_count);
 	if (ACPI_FAILURE(status)) {
 		goto cleanup;
 	}
@@ -330,8 +331,9 @@ acpi_ds_eval_buffer_field_operands(struct acpi_walk_state *walk_state,
 
 	/* Resolve the operands */
 
-	status = acpi_ex_resolve_operands(op->common.aml_opcode,
-					  ACPI_WALK_OPERANDS, walk_state);
+	status =
+	    acpi_ex_resolve_operands(op->common.aml_opcode, ACPI_WALK_OPERANDS,
+				     walk_state);
 	if (ACPI_FAILURE(status)) {
 		ACPI_ERROR((AE_INFO, "(%s) bad operand(s), status 0x%X",
 			    acpi_ps_get_opcode_name(op->common.aml_opcode),
@@ -414,8 +416,9 @@ acpi_ds_eval_region_operands(struct acpi_walk_state *walk_state,
 
 	/* Resolve the length and address operands to numbers */
 
-	status = acpi_ex_resolve_operands(op->common.aml_opcode,
-					  ACPI_WALK_OPERANDS, walk_state);
+	status =
+	    acpi_ex_resolve_operands(op->common.aml_opcode, ACPI_WALK_OPERANDS,
+				     walk_state);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -452,7 +455,6 @@ acpi_ds_eval_region_operands(struct acpi_walk_state *walk_state,
 	/* Now the address and length are valid for this opregion */
 
 	obj_desc->region.flags |= AOPOBJ_DATA_VALID;
-
 	return_ACPI_STATUS(status);
 }
 
@@ -510,8 +512,9 @@ acpi_ds_eval_table_region_operands(struct acpi_walk_state *walk_state,
 	 * Resolve the Signature string, oem_id string,
 	 * and oem_table_id string operands
 	 */
-	status = acpi_ex_resolve_operands(op->common.aml_opcode,
-					  ACPI_WALK_OPERANDS, walk_state);
+	status =
+	    acpi_ex_resolve_operands(op->common.aml_opcode, ACPI_WALK_OPERANDS,
+				     walk_state);
 	if (ACPI_FAILURE(status)) {
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/dsutils.c b/drivers/acpi/acpica/dsutils.c
index ebc577baeaf9..8ca9416320e0 100644
--- a/drivers/acpi/acpica/dsutils.c
+++ b/drivers/acpi/acpica/dsutils.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -245,9 +245,9 @@ acpi_ds_is_result_used(union acpi_parse_object * op,
 			 * we will use the return value
 			 */
 			if ((walk_state->control_state->common.state ==
-			     ACPI_CONTROL_PREDICATE_EXECUTING)
-			    && (walk_state->control_state->control.
-				predicate_op == op)) {
+			     ACPI_CONTROL_PREDICATE_EXECUTING) &&
+			    (walk_state->control_state->control.predicate_op ==
+			     op)) {
 				goto result_used;
 			}
 			break;
@@ -481,10 +481,9 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 
 		/* Get the entire name string from the AML stream */
 
-		status =
-		    acpi_ex_get_name_string(ACPI_TYPE_ANY,
-					    arg->common.value.buffer,
-					    &name_string, &name_length);
+		status = acpi_ex_get_name_string(ACPI_TYPE_ANY,
+						 arg->common.value.buffer,
+						 &name_string, &name_length);
 
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
@@ -503,9 +502,8 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 		 */
 		if ((walk_state->deferred_node) &&
 		    (walk_state->deferred_node->type == ACPI_TYPE_BUFFER_FIELD)
-		    && (arg_index ==
-			(u32) ((walk_state->opcode ==
-				AML_CREATE_FIELD_OP) ? 3 : 2))) {
+		    && (arg_index == (u32)
+			((walk_state->opcode == AML_CREATE_FIELD_OP) ? 3 : 2))) {
 			obj_desc =
 			    ACPI_CAST_PTR(union acpi_operand_object,
 					  walk_state->deferred_node);
@@ -522,9 +520,10 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 			op_info =
 			    acpi_ps_get_opcode_info(parent_op->common.
 						    aml_opcode);
-			if ((op_info->flags & AML_NSNODE)
-			    && (parent_op->common.aml_opcode !=
-				AML_INT_METHODCALL_OP)
+
+			if ((op_info->flags & AML_NSNODE) &&
+			    (parent_op->common.aml_opcode !=
+			     AML_INT_METHODCALL_OP)
 			    && (parent_op->common.aml_opcode != AML_REGION_OP)
 			    && (parent_op->common.aml_opcode !=
 				AML_INT_NAMEPATH_OP)) {
@@ -605,8 +604,8 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
-		ACPI_DEBUGGER_EXEC(acpi_db_display_argument_object
-				   (obj_desc, walk_state));
+
+		acpi_db_display_argument_object(obj_desc, walk_state);
 	} else {
 		/* Check for null name case */
 
@@ -633,15 +632,16 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 			return_ACPI_STATUS(AE_NOT_IMPLEMENTED);
 		}
 
-		if ((op_info->flags & AML_HAS_RETVAL)
-		    || (arg->common.flags & ACPI_PARSEOP_IN_STACK)) {
+		if ((op_info->flags & AML_HAS_RETVAL) ||
+		    (arg->common.flags & ACPI_PARSEOP_IN_STACK)) {
 			ACPI_DEBUG_PRINT((ACPI_DB_DISPATCH,
 					  "Argument previously created, already stacked\n"));
 
-			ACPI_DEBUGGER_EXEC(acpi_db_display_argument_object
-					   (walk_state->
-					    operands[walk_state->num_operands -
-						     1], walk_state));
+			acpi_db_display_argument_object(walk_state->
+							operands[walk_state->
+								 num_operands -
+								 1],
+							walk_state);
 
 			/*
 			 * Use value that was already previously returned
@@ -685,8 +685,7 @@ acpi_ds_create_operand(struct acpi_walk_state *walk_state,
 			return_ACPI_STATUS(status);
 		}
 
-		ACPI_DEBUGGER_EXEC(acpi_db_display_argument_object
-				   (obj_desc, walk_state));
+		acpi_db_display_argument_object(obj_desc, walk_state);
 	}
 
 	return_ACPI_STATUS(AE_OK);
diff --git a/drivers/acpi/acpica/dswexec.c b/drivers/acpi/acpica/dswexec.c
index df54d46225cd..402ecc590c56 100644
--- a/drivers/acpi/acpica/dswexec.c
+++ b/drivers/acpi/acpica/dswexec.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -172,14 +172,14 @@ acpi_ds_get_predicate_value(struct acpi_walk_state *walk_state,
 
 cleanup:
 
-	ACPI_DEBUG_PRINT((ACPI_DB_EXEC, "Completed a predicate eval=%X Op=%p\n",
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "Completed a predicate eval=%X Op=%p\n",
 			  walk_state->control_state->common.value,
 			  walk_state->op));
 
 	/* Break to debugger to display result */
 
-	ACPI_DEBUGGER_EXEC(acpi_db_display_result_object
-			   (local_obj_desc, walk_state));
+	acpi_db_display_result_object(local_obj_desc, walk_state);
 
 	/*
 	 * Delete the predicate result object (we know that
@@ -264,8 +264,8 @@ acpi_ds_exec_begin_op(struct acpi_walk_state *walk_state,
 	    (walk_state->control_state->common.state ==
 	     ACPI_CONTROL_CONDITIONAL_EXECUTING)) {
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "Exec predicate Op=%p State=%p\n", op,
-				  walk_state));
+				  "Exec predicate Op=%p State=%p\n",
+				  op, walk_state));
 
 		walk_state->control_state->common.state =
 		    ACPI_CONTROL_PREDICATE_EXECUTING;
@@ -386,11 +386,10 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 
 	/* Call debugger for single step support (DEBUG build only) */
 
-	ACPI_DEBUGGER_EXEC(status =
-			   acpi_db_single_step(walk_state, op, op_class));
-	ACPI_DEBUGGER_EXEC(if (ACPI_FAILURE(status)) {
-			   return_ACPI_STATUS(status);}
-	) ;
+	status = acpi_db_single_step(walk_state, op, op_class);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
 
 	/* Decode the Opcode Class */
 
@@ -502,9 +501,8 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 						  "Method Reference in a Package, Op=%p\n",
 						  op));
 
-				op->common.node =
-				    (struct acpi_namespace_node *)op->asl.value.
-				    arg->asl.node;
+				op->common.node = (struct acpi_namespace_node *)
+				    op->asl.value.arg->asl.node;
 				acpi_ut_add_reference(op->asl.value.arg->asl.
 						      node->object);
 				return_ACPI_STATUS(AE_OK);
@@ -586,8 +584,8 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 				 * Put the Node on the object stack (Contains the ACPI Name
 				 * of this object)
 				 */
-				walk_state->operands[0] =
-				    (void *)op->common.parent->common.node;
+				walk_state->operands[0] = (void *)
+				    op->common.parent->common.node;
 				walk_state->num_operands = 1;
 
 				status = acpi_ds_create_node(walk_state,
@@ -692,7 +690,8 @@ acpi_status acpi_ds_exec_end_op(struct acpi_walk_state *walk_state)
 		default:
 
 			ACPI_ERROR((AE_INFO,
-				    "Unimplemented opcode, class=0x%X type=0x%X Opcode=0x%X Op=%p",
+				    "Unimplemented opcode, class=0x%X "
+				    "type=0x%X Opcode=0x%X Op=%p",
 				    op_class, op_type, op->common.aml_opcode,
 				    op));
 
@@ -728,8 +727,8 @@ cleanup:
 
 		/* Break to debugger to display result */
 
-		ACPI_DEBUGGER_EXEC(acpi_db_display_result_object
-				   (walk_state->result_obj, walk_state));
+		acpi_db_display_result_object(walk_state->result_obj,
+					      walk_state);
 
 		/*
 		 * Delete the result op if and only if:
diff --git a/drivers/acpi/acpica/dswload.c b/drivers/acpi/acpica/dswload.c
index 097188a6b1c1..d1cedcfda1d2 100644
--- a/drivers/acpi/acpica/dswload.c
+++ b/drivers/acpi/acpica/dswload.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -476,13 +476,9 @@ acpi_status acpi_ds_load1_end_op(struct acpi_walk_state *walk_state)
 			status =
 			    acpi_ex_create_region(op->named.data,
 						  op->named.length,
-						  (acpi_adr_space_type) ((op->
-									  common.
-									  value.
-									  arg)->
-									 common.
-									 value.
-									 integer),
+						  (acpi_adr_space_type)
+						  ((op->common.value.arg)->
+						   common.value.integer),
 						  walk_state);
 			if (ACPI_FAILURE(status)) {
 				return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/dswload2.c b/drivers/acpi/acpica/dswload2.c
index e2c08cd79aca..0bac6e14170e 100644
--- a/drivers/acpi/acpica/dswload2.c
+++ b/drivers/acpi/acpica/dswload2.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -598,11 +598,10 @@ acpi_status acpi_ds_load2_end_op(struct acpi_walk_state *walk_state)
 				 * Executing a method: initialize the region and unlock
 				 * the interpreter
 				 */
-				status =
-				    acpi_ex_create_region(op->named.data,
-							  op->named.length,
-							  region_space,
-							  walk_state);
+				status = acpi_ex_create_region(op->named.data,
+							       op->named.length,
+							       region_space,
+							       walk_state);
 				if (ACPI_FAILURE(status)) {
 					return_ACPI_STATUS(status);
 				}
@@ -664,6 +663,7 @@ acpi_status acpi_ds_load2_end_op(struct acpi_walk_state *walk_state)
 								  length,
 								  walk_state);
 				}
+
 				walk_state->operands[0] = NULL;
 				walk_state->num_operands = 0;
 
diff --git a/drivers/acpi/acpica/dswscope.c b/drivers/acpi/acpica/dswscope.c
index 43b3ea40c0b6..9f32e08a07d9 100644
--- a/drivers/acpi/acpica/dswscope.c
+++ b/drivers/acpi/acpica/dswscope.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -77,6 +77,7 @@ void acpi_ds_scope_stack_clear(struct acpi_walk_state *walk_state)
 				  "Popped object type (%s)\n",
 				  acpi_ut_get_type_name(scope_info->common.
 							value)));
+
 		acpi_ut_delete_generic_state(scope_info);
 	}
 }
diff --git a/drivers/acpi/acpica/dswstate.c b/drivers/acpi/acpica/dswstate.c
index 89ac2022465e..3a26ddbaed6d 100644
--- a/drivers/acpi/acpica/dswstate.c
+++ b/drivers/acpi/acpica/dswstate.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evevent.c b/drivers/acpi/acpica/evevent.c
index bf6873f95e72..80fc0b9b11e5 100644
--- a/drivers/acpi/acpica/evevent.c
+++ b/drivers/acpi/acpica/evevent.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evglock.c b/drivers/acpi/acpica/evglock.c
index b78dc7c6d5d7..9f015782cdd3 100644
--- a/drivers/acpi/acpica/evglock.c
+++ b/drivers/acpi/acpica/evglock.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index ccf793247447..b47e62aaf654 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -92,8 +92,8 @@ acpi_ev_update_gpe_enable_mask(struct acpi_gpe_event_info *gpe_event_info)
 		ACPI_SET_BIT(gpe_register_info->enable_for_run,
 			     (u8)register_bit);
 	}
-	gpe_register_info->enable_mask = gpe_register_info->enable_for_run;
 
+	gpe_register_info->enable_mask = gpe_register_info->enable_for_run;
 	return_ACPI_STATUS(AE_OK);
 }
 
diff --git a/drivers/acpi/acpica/evgpeblk.c b/drivers/acpi/acpica/evgpeblk.c
index e0f24c504513..9275e626ed8d 100644
--- a/drivers/acpi/acpica/evgpeblk.c
+++ b/drivers/acpi/acpica/evgpeblk.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -167,6 +167,7 @@ acpi_status acpi_ev_delete_gpe_block(struct acpi_gpe_block_info *gpe_block)
 		if (gpe_block->next) {
 			gpe_block->next->previous = gpe_block->previous;
 		}
+
 		acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
 	}
 
diff --git a/drivers/acpi/acpica/evgpeinit.c b/drivers/acpi/acpica/evgpeinit.c
index ea4c0d3fca2d..9fdd8d09141b 100644
--- a/drivers/acpi/acpica/evgpeinit.c
+++ b/drivers/acpi/acpica/evgpeinit.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evgpeutil.c b/drivers/acpi/acpica/evgpeutil.c
index 3a958f3612fe..66c4b5b7cd64 100644
--- a/drivers/acpi/acpica/evgpeutil.c
+++ b/drivers/acpi/acpica/evgpeutil.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -346,6 +346,7 @@ acpi_ev_delete_gpe_handlers(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 					ACPI_FREE(notify);
 					notify = next;
 				}
+
 				gpe_event_info->dispatch.notify_list = NULL;
 				gpe_event_info->flags &=
 				    ~ACPI_GPE_DISPATCH_MASK;
diff --git a/drivers/acpi/acpica/evhandler.c b/drivers/acpi/acpica/evhandler.c
index 74e8595f5a2b..0f6be8956a99 100644
--- a/drivers/acpi/acpica/evhandler.c
+++ b/drivers/acpi/acpica/evhandler.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -159,7 +159,7 @@ acpi_ev_has_default_handler(struct acpi_namespace_node *node,
 
 	obj_desc = acpi_ns_get_attached_object(node);
 	if (obj_desc) {
-		handler_obj = obj_desc->device.handler;
+		handler_obj = obj_desc->common_notify.handler;
 
 		/* Walk the linked list of handlers for this object */
 
@@ -247,35 +247,31 @@ acpi_ev_install_handler(acpi_handle obj_handle,
 
 		/* Check if this Device already has a handler for this address space */
 
-		next_handler_obj = obj_desc->device.handler;
-		while (next_handler_obj) {
+		next_handler_obj =
+		    acpi_ev_find_region_handler(handler_obj->address_space.
+						space_id,
+						obj_desc->common_notify.
+						handler);
+		if (next_handler_obj) {
 
 			/* Found a handler, is it for the same address space? */
 
-			if (next_handler_obj->address_space.space_id ==
-			    handler_obj->address_space.space_id) {
-				ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
-						  "Found handler for region [%s] in device %p(%p) "
-						  "handler %p\n",
-						  acpi_ut_get_region_name
-						  (handler_obj->address_space.
-						   space_id), obj_desc,
-						  next_handler_obj,
-						  handler_obj));
-
-				/*
-				 * Since the object we found it on was a device, then it
-				 * means that someone has already installed a handler for
-				 * the branch of the namespace from this device on. Just
-				 * bail out telling the walk routine to not traverse this
-				 * branch. This preserves the scoping rule for handlers.
-				 */
-				return (AE_CTRL_DEPTH);
-			}
-
-			/* Walk the linked list of handlers attached to this device */
-
-			next_handler_obj = next_handler_obj->address_space.next;
+			ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
+					  "Found handler for region [%s] in device %p(%p) handler %p\n",
+					  acpi_ut_get_region_name(handler_obj->
+								  address_space.
+								  space_id),
+					  obj_desc, next_handler_obj,
+					  handler_obj));
+
+			/*
+			 * Since the object we found it on was a device, then it means
+			 * that someone has already installed a handler for the branch
+			 * of the namespace from this device on. Just bail out telling
+			 * the walk routine to not traverse this branch. This preserves
+			 * the scoping rule for handlers.
+			 */
+			return (AE_CTRL_DEPTH);
 		}
 
 		/*
@@ -309,6 +305,44 @@ acpi_ev_install_handler(acpi_handle obj_handle,
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_ev_find_region_handler
+ *
+ * PARAMETERS:  space_id        - The address space ID
+ *              handler_obj     - Head of the handler object list
+ *
+ * RETURN:      Matching handler object. NULL if space ID not matched
+ *
+ * DESCRIPTION: Search a handler object list for a match on the address
+ *              space ID.
+ *
+ ******************************************************************************/
+
+union acpi_operand_object *acpi_ev_find_region_handler(acpi_adr_space_type
+						       space_id,
+						       union acpi_operand_object
+						       *handler_obj)
+{
+
+	/* Walk the handler list for this device */
+
+	while (handler_obj) {
+
+		/* Same space_id indicates a handler is installed */
+
+		if (handler_obj->address_space.space_id == space_id) {
+			return (handler_obj);
+		}
+
+		/* Next handler object */
+
+		handler_obj = handler_obj->address_space.next;
+	}
+
+	return (NULL);
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_ev_install_space_handler
  *
  * PARAMETERS:  node            - Namespace node for the device
@@ -332,15 +366,15 @@ acpi_ev_install_space_handler(struct acpi_namespace_node * node,
 {
 	union acpi_operand_object *obj_desc;
 	union acpi_operand_object *handler_obj;
-	acpi_status status;
+	acpi_status status = AE_OK;
 	acpi_object_type type;
 	u8 flags = 0;
 
 	ACPI_FUNCTION_TRACE(ev_install_space_handler);
 
 	/*
-	 * This registration is valid for only the types below and the root. This
-	 * is where the default handlers get placed.
+	 * This registration is valid for only the types below and the root.
+	 * The root node is where the default handlers get installed.
 	 */
 	if ((node->type != ACPI_TYPE_DEVICE) &&
 	    (node->type != ACPI_TYPE_PROCESSOR) &&
@@ -407,38 +441,30 @@ acpi_ev_install_space_handler(struct acpi_namespace_node * node,
 	obj_desc = acpi_ns_get_attached_object(node);
 	if (obj_desc) {
 		/*
-		 * The attached device object already exists. Make sure the handler
-		 * is not already installed.
+		 * The attached device object already exists. Now make sure
+		 * the handler is not already installed.
 		 */
-		handler_obj = obj_desc->device.handler;
-
-		/* Walk the handler list for this device */
-
-		while (handler_obj) {
-
-			/* Same space_id indicates a handler already installed */
+		handler_obj = acpi_ev_find_region_handler(space_id,
+							  obj_desc->
+							  common_notify.
+							  handler);
 
-			if (handler_obj->address_space.space_id == space_id) {
-				if (handler_obj->address_space.handler ==
-				    handler) {
-					/*
-					 * It is (relatively) OK to attempt to install the SAME
-					 * handler twice. This can easily happen with the
-					 * PCI_Config space.
-					 */
-					status = AE_SAME_HANDLER;
-					goto unlock_and_exit;
-				} else {
-					/* A handler is already installed */
-
-					status = AE_ALREADY_EXISTS;
-				}
+		if (handler_obj) {
+			if (handler_obj->address_space.handler == handler) {
+				/*
+				 * It is (relatively) OK to attempt to install the SAME
+				 * handler twice. This can easily happen with the
+				 * PCI_Config space.
+				 */
+				status = AE_SAME_HANDLER;
 				goto unlock_and_exit;
-			}
+			} else {
+				/* A handler is already installed */
 
-			/* Walk the linked list of handlers */
+				status = AE_ALREADY_EXISTS;
+			}
 
-			handler_obj = handler_obj->address_space.next;
+			goto unlock_and_exit;
 		}
 	} else {
 		ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
@@ -477,7 +503,8 @@ acpi_ev_install_space_handler(struct acpi_namespace_node * node,
 	}
 
 	ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
-			  "Installing address handler for region %s(%X) on Device %4.4s %p(%p)\n",
+			  "Installing address handler for region %s(%X) "
+			  "on Device %4.4s %p(%p)\n",
 			  acpi_ut_get_region_name(space_id), space_id,
 			  acpi_ut_get_node_name(node), node, obj_desc));
 
@@ -506,28 +533,26 @@ acpi_ev_install_space_handler(struct acpi_namespace_node * node,
 
 	/* Install at head of Device.address_space list */
 
-	handler_obj->address_space.next = obj_desc->device.handler;
+	handler_obj->address_space.next = obj_desc->common_notify.handler;
 
 	/*
 	 * The Device object is the first reference on the handler_obj.
 	 * Each region that uses the handler adds a reference.
 	 */
-	obj_desc->device.handler = handler_obj;
+	obj_desc->common_notify.handler = handler_obj;
 
 	/*
-	 * Walk the namespace finding all of the regions this
-	 * handler will manage.
+	 * Walk the namespace finding all of the regions this handler will
+	 * manage.
 	 *
-	 * Start at the device and search the branch toward
-	 * the leaf nodes until either the leaf is encountered or
-	 * a device is detected that has an address handler of the
-	 * same type.
+	 * Start at the device and search the branch toward the leaf nodes
+	 * until either the leaf is encountered or a device is detected that
+	 * has an address handler of the same type.
 	 *
-	 * In either case, back up and search down the remainder
-	 * of the branch
+	 * In either case, back up and search down the remainder of the branch
 	 */
-	status = acpi_ns_walk_namespace(ACPI_TYPE_ANY, node, ACPI_UINT32_MAX,
-					ACPI_NS_WALK_UNLOCK,
+	status = acpi_ns_walk_namespace(ACPI_TYPE_ANY, node,
+					ACPI_UINT32_MAX, ACPI_NS_WALK_UNLOCK,
 					acpi_ev_install_handler, NULL,
 					handler_obj, NULL);
 
diff --git a/drivers/acpi/acpica/evmisc.c b/drivers/acpi/acpica/evmisc.c
index f7c9dfe7b990..c67d78c5995f 100644
--- a/drivers/acpi/acpica/evmisc.c
+++ b/drivers/acpi/acpica/evmisc.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -68,6 +68,7 @@ static void ACPI_SYSTEM_XFACE acpi_ev_notify_dispatch(void *context);
 
 u8 acpi_ev_is_notify_object(struct acpi_namespace_node *node)
 {
+
 	switch (node->type) {
 	case ACPI_TYPE_DEVICE:
 	case ACPI_TYPE_PROCESSOR:
@@ -170,8 +171,8 @@ acpi_ev_queue_notify_request(struct acpi_namespace_node * node,
 			  acpi_ut_get_notify_name(notify_value, ACPI_TYPE_ANY),
 			  node));
 
-	status = acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_ev_notify_dispatch,
-				 info);
+	status = acpi_os_execute(OSL_NOTIFY_HANDLER,
+				 acpi_ev_notify_dispatch, info);
 	if (ACPI_FAILURE(status)) {
 		acpi_ut_delete_generic_state(info);
 	}
diff --git a/drivers/acpi/acpica/evregion.c b/drivers/acpi/acpica/evregion.c
index 5ee79a16fe33..47092b4d633c 100644
--- a/drivers/acpi/acpica/evregion.c
+++ b/drivers/acpi/acpica/evregion.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -97,15 +97,12 @@ acpi_status acpi_ev_initialize_op_regions(void)
 		if (acpi_ev_has_default_handler(acpi_gbl_root_node,
 						acpi_gbl_default_address_spaces
 						[i])) {
-			status =
-			    acpi_ev_execute_reg_methods(acpi_gbl_root_node,
-							acpi_gbl_default_address_spaces
-							[i]);
+			acpi_ev_execute_reg_methods(acpi_gbl_root_node,
+						    acpi_gbl_default_address_spaces
+						    [i], ACPI_REG_CONNECT);
 		}
 	}
 
-	acpi_gbl_reg_methods_executed = TRUE;
-
 	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
 	return_ACPI_STATUS(status);
 }
@@ -127,6 +124,12 @@ acpi_status acpi_ev_initialize_op_regions(void)
  * DESCRIPTION: Dispatch an address space or operation region access to
  *              a previously installed handler.
  *
+ * NOTE: During early initialization, we always install the default region
+ * handlers for Memory, I/O and PCI_Config. This ensures that these operation
+ * region address spaces are always available as per the ACPI specification.
+ * This is especially needed in order to support the execution of
+ * module-level AML code during loading of the ACPI tables.
+ *
  ******************************************************************************/
 
 acpi_status
@@ -498,6 +501,12 @@ acpi_ev_attach_region(union acpi_operand_object *handler_obj,
 
 	ACPI_FUNCTION_TRACE(ev_attach_region);
 
+	/* Install the region's handler */
+
+	if (region_obj->region.handler) {
+		return_ACPI_STATUS(AE_ALREADY_EXISTS);
+	}
+
 	ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
 			  "Adding Region [%4.4s] %p to address handler %p [%s]\n",
 			  acpi_ut_get_node_name(region_obj->region.node),
@@ -509,17 +518,56 @@ acpi_ev_attach_region(union acpi_operand_object *handler_obj,
 
 	region_obj->region.next = handler_obj->address_space.region_list;
 	handler_obj->address_space.region_list = region_obj;
+	region_obj->region.handler = handler_obj;
+	acpi_ut_add_reference(handler_obj);
 
-	/* Install the region's handler */
+	return_ACPI_STATUS(AE_OK);
+}
 
-	if (region_obj->region.handler) {
-		return_ACPI_STATUS(AE_ALREADY_EXISTS);
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ev_associate_reg_method
+ *
+ * PARAMETERS:  region_obj          - Region object
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Find and associate _REG method to a region
+ *
+ ******************************************************************************/
+
+void acpi_ev_associate_reg_method(union acpi_operand_object *region_obj)
+{
+	acpi_name *reg_name_ptr = (acpi_name *) METHOD_NAME__REG;
+	struct acpi_namespace_node *method_node;
+	struct acpi_namespace_node *node;
+	union acpi_operand_object *region_obj2;
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(ev_associate_reg_method);
+
+	region_obj2 = acpi_ns_get_secondary_object(region_obj);
+	if (!region_obj2) {
+		return_VOID;
 	}
 
-	region_obj->region.handler = handler_obj;
-	acpi_ut_add_reference(handler_obj);
+	node = region_obj->region.node->parent;
 
-	return_ACPI_STATUS(AE_OK);
+	/* Find any "_REG" method associated with this region definition */
+
+	status =
+	    acpi_ns_search_one_scope(*reg_name_ptr, node, ACPI_TYPE_METHOD,
+				     &method_node);
+	if (ACPI_SUCCESS(status)) {
+		/*
+		 * The _REG method is optional and there can be only one per region
+		 * definition. This will be executed when the handler is attached
+		 * or removed
+		 */
+		region_obj2->extra.method_REG = method_node;
+	}
+
+	return_VOID;
 }
 
 /*******************************************************************************
@@ -550,7 +598,18 @@ acpi_ev_execute_reg_method(union acpi_operand_object *region_obj, u32 function)
 		return_ACPI_STATUS(AE_NOT_EXIST);
 	}
 
-	if (region_obj2->extra.method_REG == NULL) {
+	if (region_obj2->extra.method_REG == NULL ||
+	    region_obj->region.handler == NULL ||
+	    !acpi_gbl_reg_methods_enabled) {
+		return_ACPI_STATUS(AE_OK);
+	}
+
+	/* _REG(DISCONNECT) should be paired with _REG(CONNECT) */
+
+	if ((function == ACPI_REG_CONNECT &&
+	     region_obj->common.flags & AOPOBJ_REG_CONNECTED) ||
+	    (function == ACPI_REG_DISCONNECT &&
+	     !(region_obj->common.flags & AOPOBJ_REG_CONNECTED))) {
 		return_ACPI_STATUS(AE_OK);
 	}
 
@@ -599,6 +658,16 @@ acpi_ev_execute_reg_method(union acpi_operand_object *region_obj, u32 function)
 	status = acpi_ns_evaluate(info);
 	acpi_ut_remove_reference(args[1]);
 
+	if (ACPI_FAILURE(status)) {
+		goto cleanup2;
+	}
+
+	if (function == ACPI_REG_CONNECT) {
+		region_obj->common.flags |= AOPOBJ_REG_CONNECTED;
+	} else {
+		region_obj->common.flags &= ~AOPOBJ_REG_CONNECTED;
+	}
+
 cleanup2:
 	acpi_ut_remove_reference(args[0]);
 
@@ -613,24 +682,25 @@ cleanup1:
  *
  * PARAMETERS:  node            - Namespace node for the device
  *              space_id        - The address space ID
+ *              function        - Passed to _REG: On (1) or Off (0)
  *
- * RETURN:      Status
+ * RETURN:      None
  *
  * DESCRIPTION: Run all _REG methods for the input Space ID;
  *              Note: assumes namespace is locked, or system init time.
  *
  ******************************************************************************/
 
-acpi_status
+void
 acpi_ev_execute_reg_methods(struct acpi_namespace_node *node,
-			    acpi_adr_space_type space_id)
+			    acpi_adr_space_type space_id, u32 function)
 {
-	acpi_status status;
 	struct acpi_reg_walk_info info;
 
 	ACPI_FUNCTION_TRACE(ev_execute_reg_methods);
 
 	info.space_id = space_id;
+	info.function = function;
 	info.reg_run_count = 0;
 
 	ACPI_DEBUG_PRINT_RAW((ACPI_DB_NAMES,
@@ -643,9 +713,9 @@ acpi_ev_execute_reg_methods(struct acpi_namespace_node *node,
 	 * regions and _REG methods. (i.e. handlers must be installed for all
 	 * regions of this Space ID before we can run any _REG methods)
 	 */
-	status = acpi_ns_walk_namespace(ACPI_TYPE_ANY, node, ACPI_UINT32_MAX,
-					ACPI_NS_WALK_UNLOCK, acpi_ev_reg_run,
-					NULL, &info, NULL);
+	(void)acpi_ns_walk_namespace(ACPI_TYPE_ANY, node, ACPI_UINT32_MAX,
+				     ACPI_NS_WALK_UNLOCK, acpi_ev_reg_run, NULL,
+				     &info, NULL);
 
 	/* Special case for EC: handle "orphan" _REG methods with no region */
 
@@ -658,7 +728,7 @@ acpi_ev_execute_reg_methods(struct acpi_namespace_node *node,
 			      info.reg_run_count,
 			      acpi_ut_get_region_name(info.space_id)));
 
-	return_ACPI_STATUS(status);
+	return_VOID;
 }
 
 /*******************************************************************************
@@ -717,7 +787,7 @@ acpi_ev_reg_run(acpi_handle obj_handle,
 	}
 
 	info->reg_run_count++;
-	status = acpi_ev_execute_reg_method(obj_desc, ACPI_REG_CONNECT);
+	status = acpi_ev_execute_reg_method(obj_desc, info->function);
 	return (status);
 }
 
diff --git a/drivers/acpi/acpica/evrgnini.c b/drivers/acpi/acpica/evrgnini.c
index da323390bb70..fda869c9ad0b 100644
--- a/drivers/acpi/acpica/evrgnini.c
+++ b/drivers/acpi/acpica/evrgnini.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -507,9 +507,6 @@ acpi_ev_initialize_region(union acpi_operand_object *region_obj,
 	acpi_adr_space_type space_id;
 	struct acpi_namespace_node *node;
 	acpi_status status;
-	struct acpi_namespace_node *method_node;
-	acpi_name *reg_name_ptr = (acpi_name *) METHOD_NAME__REG;
-	union acpi_operand_object *region_obj2;
 
 	ACPI_FUNCTION_TRACE_U32(ev_initialize_region, acpi_ns_locked);
 
@@ -521,38 +518,15 @@ acpi_ev_initialize_region(union acpi_operand_object *region_obj,
 		return_ACPI_STATUS(AE_OK);
 	}
 
-	region_obj2 = acpi_ns_get_secondary_object(region_obj);
-	if (!region_obj2) {
-		return_ACPI_STATUS(AE_NOT_EXIST);
-	}
+	acpi_ev_associate_reg_method(region_obj);
+	region_obj->common.flags |= AOPOBJ_OBJECT_INITIALIZED;
 
 	node = region_obj->region.node->parent;
 	space_id = region_obj->region.space_id;
 
-	/* Setup defaults */
-
-	region_obj->region.handler = NULL;
-	region_obj2->extra.method_REG = NULL;
-	region_obj->common.flags &= ~(AOPOBJ_SETUP_COMPLETE);
-	region_obj->common.flags |= AOPOBJ_OBJECT_INITIALIZED;
-
-	/* Find any "_REG" method associated with this region definition */
-
-	status =
-	    acpi_ns_search_one_scope(*reg_name_ptr, node, ACPI_TYPE_METHOD,
-				     &method_node);
-	if (ACPI_SUCCESS(status)) {
-		/*
-		 * The _REG method is optional and there can be only one per region
-		 * definition. This will be executed when the handler is attached
-		 * or removed
-		 */
-		region_obj2->extra.method_REG = method_node;
-	}
-
 	/*
 	 * The following loop depends upon the root Node having no parent
-	 * ie: acpi_gbl_root_node->parent_entry being set to NULL
+	 * ie: acpi_gbl_root_node->Parent being set to NULL
 	 */
 	while (node) {
 
@@ -566,18 +540,10 @@ acpi_ev_initialize_region(union acpi_operand_object *region_obj,
 
 			switch (node->type) {
 			case ACPI_TYPE_DEVICE:
-
-				handler_obj = obj_desc->device.handler;
-				break;
-
 			case ACPI_TYPE_PROCESSOR:
-
-				handler_obj = obj_desc->processor.handler;
-				break;
-
 			case ACPI_TYPE_THERMAL:
 
-				handler_obj = obj_desc->thermal_zone.handler;
+				handler_obj = obj_desc->common_notify.handler;
 				break;
 
 			case ACPI_TYPE_METHOD:
@@ -602,60 +568,49 @@ acpi_ev_initialize_region(union acpi_operand_object *region_obj,
 				break;
 			}
 
-			while (handler_obj) {
-
-				/* Is this handler of the correct type? */
+			handler_obj =
+			    acpi_ev_find_region_handler(space_id, handler_obj);
+			if (handler_obj) {
 
-				if (handler_obj->address_space.space_id ==
-				    space_id) {
+				/* Found correct handler */
 
-					/* Found correct handler */
+				ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
+						  "Found handler %p for region %p in obj %p\n",
+						  handler_obj, region_obj,
+						  obj_desc));
 
-					ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
-							  "Found handler %p for region %p in obj %p\n",
-							  handler_obj,
+				status =
+				    acpi_ev_attach_region(handler_obj,
 							  region_obj,
-							  obj_desc));
+							  acpi_ns_locked);
 
+				/*
+				 * Tell all users that this region is usable by
+				 * running the _REG method
+				 */
+				if (acpi_ns_locked) {
 					status =
-					    acpi_ev_attach_region(handler_obj,
-								  region_obj,
-								  acpi_ns_locked);
-
-					/*
-					 * Tell all users that this region is usable by
-					 * running the _REG method
-					 */
-					if (acpi_ns_locked) {
-						status =
-						    acpi_ut_release_mutex
-						    (ACPI_MTX_NAMESPACE);
-						if (ACPI_FAILURE(status)) {
-							return_ACPI_STATUS
-							    (status);
-						}
+					    acpi_ut_release_mutex
+					    (ACPI_MTX_NAMESPACE);
+					if (ACPI_FAILURE(status)) {
+						return_ACPI_STATUS(status);
 					}
+				}
 
+				status =
+				    acpi_ev_execute_reg_method(region_obj,
+							       ACPI_REG_CONNECT);
+
+				if (acpi_ns_locked) {
 					status =
-					    acpi_ev_execute_reg_method
-					    (region_obj, ACPI_REG_CONNECT);
-
-					if (acpi_ns_locked) {
-						status =
-						    acpi_ut_acquire_mutex
-						    (ACPI_MTX_NAMESPACE);
-						if (ACPI_FAILURE(status)) {
-							return_ACPI_STATUS
-							    (status);
-						}
+					    acpi_ut_acquire_mutex
+					    (ACPI_MTX_NAMESPACE);
+					if (ACPI_FAILURE(status)) {
+						return_ACPI_STATUS(status);
 					}
-
-					return_ACPI_STATUS(AE_OK);
 				}
 
-				/* Try next handler in the list */
-
-				handler_obj = handler_obj->address_space.next;
+				return_ACPI_STATUS(AE_OK);
 			}
 		}
 
diff --git a/drivers/acpi/acpica/evsci.c b/drivers/acpi/acpica/evsci.c
index 0366703d2970..3b7757c9c916 100644
--- a/drivers/acpi/acpica/evsci.c
+++ b/drivers/acpi/acpica/evsci.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evxface.c b/drivers/acpi/acpica/evxface.c
index 07d22bfbaa00..e4e9260cdc57 100644
--- a/drivers/acpi/acpica/evxface.c
+++ b/drivers/acpi/acpica/evxface.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -879,9 +879,8 @@ acpi_install_gpe_handler(acpi_handle gpe_device,
 
 	ACPI_FUNCTION_TRACE(acpi_install_gpe_handler);
 
-	status =
-	    acpi_ev_install_gpe_handler(gpe_device, gpe_number, type, FALSE,
-					address, context);
+	status = acpi_ev_install_gpe_handler(gpe_device, gpe_number, type,
+					     FALSE, address, context);
 
 	return_ACPI_STATUS(status);
 }
@@ -914,8 +913,8 @@ acpi_install_gpe_raw_handler(acpi_handle gpe_device,
 
 	ACPI_FUNCTION_TRACE(acpi_install_gpe_raw_handler);
 
-	status = acpi_ev_install_gpe_handler(gpe_device, gpe_number, type, TRUE,
-					     address, context);
+	status = acpi_ev_install_gpe_handler(gpe_device, gpe_number, type,
+					     TRUE, address, context);
 
 	return_ACPI_STATUS(status);
 }
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index 10ce48e16ebf..9179e9abe3db 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 70eb47e3d724..90456714821f 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/evxfregn.c b/drivers/acpi/acpica/evxfregn.c
index f21afbab03f7..d2743067126a 100644
--- a/drivers/acpi/acpica/evxfregn.c
+++ b/drivers/acpi/acpica/evxfregn.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -112,41 +112,9 @@ acpi_install_address_space_handler(acpi_handle device,
 		goto unlock_and_exit;
 	}
 
-	/*
-	 * For the default space_IDs, (the IDs for which there are default region handlers
-	 * installed) Only execute the _REG methods if the global initialization _REG
-	 * methods have already been run (via acpi_initialize_objects). In other words,
-	 * we will defer the execution of the _REG methods for these space_IDs until
-	 * execution of acpi_initialize_objects. This is done because we need the handlers
-	 * for the default spaces (mem/io/pci/table) to be installed before we can run
-	 * any control methods (or _REG methods). There is known BIOS code that depends
-	 * on this.
-	 *
-	 * For all other space_IDs, we can safely execute the _REG methods immediately.
-	 * This means that for IDs like embedded_controller, this function should be called
-	 * only after acpi_enable_subsystem has been called.
-	 */
-	switch (space_id) {
-	case ACPI_ADR_SPACE_SYSTEM_MEMORY:
-	case ACPI_ADR_SPACE_SYSTEM_IO:
-	case ACPI_ADR_SPACE_PCI_CONFIG:
-	case ACPI_ADR_SPACE_DATA_TABLE:
-
-		if (!acpi_gbl_reg_methods_executed) {
-
-			/* We will defer execution of the _REG methods for this space */
-			goto unlock_and_exit;
-		}
-		break;
-
-	default:
-
-		break;
-	}
-
 	/* Run all _REG methods for this address space */
 
-	status = acpi_ev_execute_reg_methods(node, space_id);
+	acpi_ev_execute_reg_methods(node, space_id, ACPI_REG_CONNECT);
 
 unlock_and_exit:
 	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
@@ -215,8 +183,8 @@ acpi_remove_address_space_handler(acpi_handle device,
 
 	/* Find the address handler the user requested */
 
-	handler_obj = obj_desc->device.handler;
-	last_obj_ptr = &obj_desc->device.handler;
+	handler_obj = obj_desc->common_notify.handler;
+	last_obj_ptr = &obj_desc->common_notify.handler;
 	while (handler_obj) {
 
 		/* We have a handler, see if user requested this one */
diff --git a/drivers/acpi/acpica/exconfig.c b/drivers/acpi/acpica/exconfig.c
index b540913c11ac..011df210b7b2 100644
--- a/drivers/acpi/acpica/exconfig.c
+++ b/drivers/acpi/acpica/exconfig.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -358,8 +358,8 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		}
 
 		/*
-		 * If the Region Address and Length have not been previously evaluated,
-		 * evaluate them now and save the results.
+		 * If the Region Address and Length have not been previously
+		 * evaluated, evaluate them now and save the results.
 		 */
 		if (!(obj_desc->common.flags & AOPOBJ_DATA_VALID)) {
 			status = acpi_ds_get_region_arguments(obj_desc);
@@ -454,8 +454,8 @@ acpi_ex_load_op(union acpi_operand_object *obj_desc,
 		}
 
 		/*
-		 * Copy the table from the buffer because the buffer could be modified
-		 * or even deleted in the future
+		 * Copy the table from the buffer because the buffer could be
+		 * modified or even deleted in the future
 		 */
 		table = ACPI_ALLOCATE(length);
 		if (!table) {
diff --git a/drivers/acpi/acpica/exconvrt.c b/drivers/acpi/acpica/exconvrt.c
index 1e4c5b6dc0b0..0b9f2c13b98a 100644
--- a/drivers/acpi/acpica/exconvrt.c
+++ b/drivers/acpi/acpica/exconvrt.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -227,8 +227,8 @@ acpi_ex_convert_to_buffer(union acpi_operand_object *obj_desc,
 		/* Copy the integer to the buffer, LSB first */
 
 		new_buf = return_desc->buffer.pointer;
-		memcpy(new_buf,
-		       &obj_desc->integer.value, acpi_gbl_integer_byte_width);
+		memcpy(new_buf, &obj_desc->integer.value,
+		       acpi_gbl_integer_byte_width);
 		break;
 
 	case ACPI_TYPE_STRING:
@@ -354,9 +354,8 @@ acpi_ex_convert_to_ascii(u64 integer, u16 base, u8 *string, u8 data_width)
 
 			/* Get one hex digit, most significant digits first */
 
-			string[k] =
-			    (u8) acpi_ut_hex_to_ascii_char(integer,
-							   ACPI_MUL_4(j));
+			string[k] = (u8)
+			    acpi_ut_hex_to_ascii_char(integer, ACPI_MUL_4(j));
 			k++;
 		}
 		break;
diff --git a/drivers/acpi/acpica/excreate.c b/drivers/acpi/acpica/excreate.c
index ccb7219bdcee..bea9612e4720 100644
--- a/drivers/acpi/acpica/excreate.c
+++ b/drivers/acpi/acpica/excreate.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -189,9 +189,9 @@ acpi_status acpi_ex_create_event(struct acpi_walk_state *walk_state)
 
 	/* Attach object to the Node */
 
-	status =
-	    acpi_ns_attach_object((struct acpi_namespace_node *)walk_state->
-				  operands[0], obj_desc, ACPI_TYPE_EVENT);
+	status = acpi_ns_attach_object((struct acpi_namespace_node *)
+				       walk_state->operands[0], obj_desc,
+				       ACPI_TYPE_EVENT);
 
 cleanup:
 	/*
@@ -326,9 +326,10 @@ acpi_ex_create_region(u8 * aml_start,
 	 * Remember location in AML stream of address & length
 	 * operands since they need to be evaluated at run time.
 	 */
-	region_obj2 = obj_desc->common.next_object;
+	region_obj2 = acpi_ns_get_secondary_object(obj_desc);
 	region_obj2->extra.aml_start = aml_start;
 	region_obj2->extra.aml_length = aml_length;
+	region_obj2->extra.method_REG = NULL;
 	if (walk_state->scope_info) {
 		region_obj2->extra.scope_node =
 		    walk_state->scope_info->scope.node;
@@ -342,6 +343,10 @@ acpi_ex_create_region(u8 * aml_start,
 	obj_desc->region.address = 0;
 	obj_desc->region.length = 0;
 	obj_desc->region.node = node;
+	obj_desc->region.handler = NULL;
+	obj_desc->common.flags &=
+	    ~(AOPOBJ_SETUP_COMPLETE | AOPOBJ_REG_CONNECTED |
+	      AOPOBJ_OBJECT_INITIALIZED);
 
 	/* Install the new region object in the parent Node */
 
@@ -492,10 +497,9 @@ acpi_ex_create_method(u8 * aml_start,
 	 * Disassemble the method flags. Split off the arg_count, Serialized
 	 * flag, and sync_level for efficiency.
 	 */
-	method_flags = (u8) operand[1]->integer.value;
-
-	obj_desc->method.param_count =
-	    (u8) (method_flags & AML_METHOD_ARG_COUNT);
+	method_flags = (u8)operand[1]->integer.value;
+	obj_desc->method.param_count = (u8)
+	    (method_flags & AML_METHOD_ARG_COUNT);
 
 	/*
 	 * Get the sync_level. If method is serialized, a mutex will be
diff --git a/drivers/acpi/acpica/exdebug.c b/drivers/acpi/acpica/exdebug.c
index de92458236f5..37a509d016da 100644
--- a/drivers/acpi/acpica/exdebug.c
+++ b/drivers/acpi/acpica/exdebug.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -43,21 +43,11 @@
 
 #include <acpi/acpi.h>
 #include "accommon.h"
-#include "acnamesp.h"
 #include "acinterp.h"
-#include "acparser.h"
 
 #define _COMPONENT          ACPI_EXECUTER
 ACPI_MODULE_NAME("exdebug")
 
-static union acpi_operand_object *acpi_gbl_trace_method_object = NULL;
-
-/* Local prototypes */
-
-#ifdef ACPI_DEBUG_OUTPUT
-static const char *acpi_ex_get_trace_event_name(acpi_trace_event_type type);
-#endif
-
 #ifndef ACPI_NO_ERROR_MESSAGES
 /*******************************************************************************
  *
@@ -80,7 +70,6 @@ static const char *acpi_ex_get_trace_event_name(acpi_trace_event_type type);
  * enabled if necessary.
  *
  ******************************************************************************/
-
 void
 acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 			u32 level, u32 index)
@@ -99,20 +88,40 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 		return_VOID;
 	}
 
-	/*
-	 * We will emit the current timer value (in microseconds) with each
-	 * debug output. Only need the lower 26 bits. This allows for 67
-	 * million microseconds or 67 seconds before rollover.
-	 */
-	timer = ((u32)acpi_os_get_timer() / 10);	/* (100 nanoseconds to microseconds) */
-	timer &= 0x03FFFFFF;
+	/* Null string or newline -- don't emit the line header */
+
+	if (source_desc &&
+	    (ACPI_GET_DESCRIPTOR_TYPE(source_desc) == ACPI_DESC_TYPE_OPERAND) &&
+	    (source_desc->common.type == ACPI_TYPE_STRING)) {
+		if ((source_desc->string.length == 0) ||
+		    ((source_desc->string.length == 1) &&
+		     (*source_desc->string.pointer == '\n'))) {
+			acpi_os_printf("\n");
+			return_VOID;
+		}
+	}
 
 	/*
 	 * Print line header as long as we are not in the middle of an
 	 * object display
 	 */
 	if (!((level > 0) && index == 0)) {
-		acpi_os_printf("[ACPI Debug %.8u] %*s", timer, level, " ");
+		if (acpi_gbl_display_debug_timer) {
+			/*
+			 * We will emit the current timer value (in microseconds) with each
+			 * debug output. Only need the lower 26 bits. This allows for 67
+			 * million microseconds or 67 seconds before rollover.
+			 *
+			 * Convert 100 nanosecond units to microseconds
+			 */
+			timer = ((u32)acpi_os_get_timer() / 10);
+			timer &= 0x03FFFFFF;
+
+			acpi_os_printf("[ACPI Debug T=0x%8.8X] %*s", timer,
+				       level, " ");
+		} else {
+			acpi_os_printf("[ACPI Debug] %*s", level, " ");
+		}
 	}
 
 	/* Display the index for package output only */
@@ -127,8 +136,15 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 	}
 
 	if (ACPI_GET_DESCRIPTOR_TYPE(source_desc) == ACPI_DESC_TYPE_OPERAND) {
-		acpi_os_printf("%s ",
-			       acpi_ut_get_object_type_name(source_desc));
+
+		/* No object type prefix needed for integers and strings */
+
+		if ((source_desc->common.type != ACPI_TYPE_INTEGER) &&
+		    (source_desc->common.type != ACPI_TYPE_STRING)) {
+			acpi_os_printf("%s ",
+				       acpi_ut_get_object_type_name
+				       (source_desc));
+		}
 
 		if (!acpi_ut_valid_internal_object(source_desc)) {
 			acpi_os_printf("%p, Invalid Internal Object!\n",
@@ -137,7 +153,7 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 		}
 	} else if (ACPI_GET_DESCRIPTOR_TYPE(source_desc) ==
 		   ACPI_DESC_TYPE_NAMED) {
-		acpi_os_printf("%s: %p\n",
+		acpi_os_printf("%s (Node %p)\n",
 			       acpi_ut_get_type_name(((struct
 						       acpi_namespace_node *)
 						      source_desc)->type),
@@ -175,14 +191,12 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 
 	case ACPI_TYPE_STRING:
 
-		acpi_os_printf("[0x%.2X] \"%s\"\n",
-			       source_desc->string.length,
-			       source_desc->string.pointer);
+		acpi_os_printf("\"%s\"\n", source_desc->string.pointer);
 		break;
 
 	case ACPI_TYPE_PACKAGE:
 
-		acpi_os_printf("[Contains 0x%.2X Elements]\n",
+		acpi_os_printf("(Contains 0x%.2X Elements):\n",
 			       source_desc->package.count);
 
 		/* Output the entire contents of the package */
@@ -261,11 +275,14 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 			if (ACPI_GET_DESCRIPTOR_TYPE
 			    (source_desc->reference.object) ==
 			    ACPI_DESC_TYPE_NAMED) {
-				acpi_ex_do_debug_object(((struct
-							  acpi_namespace_node *)
+
+				/* Reference object is a namespace node */
+
+				acpi_ex_do_debug_object(ACPI_CAST_PTR
+							(union
+							 acpi_operand_object,
 							 source_desc->reference.
-							 object)->object,
-							level + 4, 0);
+							 object), level + 4, 0);
 			} else {
 				object_desc = source_desc->reference.object;
 				value = source_desc->reference.value;
@@ -293,9 +310,14 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 				case ACPI_TYPE_PACKAGE:
 
 					acpi_os_printf("Package[%u] = ", value);
-					acpi_ex_do_debug_object(*source_desc->
-								reference.where,
-								level + 4, 0);
+					if (!(*source_desc->reference.where)) {
+						acpi_os_printf
+						    ("[Uninitialized Package Element]\n");
+					} else {
+						acpi_ex_do_debug_object
+						    (*source_desc->reference.
+						     where, level + 4, 0);
+					}
 					break;
 
 				default:
@@ -311,7 +333,7 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 
 	default:
 
-		acpi_os_printf("%p\n", source_desc);
+		acpi_os_printf("(Descriptor %p)\n", source_desc);
 		break;
 	}
 
@@ -319,316 +341,3 @@ acpi_ex_do_debug_object(union acpi_operand_object *source_desc,
 	return_VOID;
 }
 #endif
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_interpreter_trace_enabled
- *
- * PARAMETERS:  name                - Whether method name should be matched,
- *                                    this should be checked before starting
- *                                    the tracer
- *
- * RETURN:      TRUE if interpreter trace is enabled.
- *
- * DESCRIPTION: Check whether interpreter trace is enabled
- *
- ******************************************************************************/
-
-static u8 acpi_ex_interpreter_trace_enabled(char *name)
-{
-
-	/* Check if tracing is enabled */
-
-	if (!(acpi_gbl_trace_flags & ACPI_TRACE_ENABLED)) {
-		return (FALSE);
-	}
-
-	/*
-	 * Check if tracing is filtered:
-	 *
-	 * 1. If the tracer is started, acpi_gbl_trace_method_object should have
-	 *    been filled by the trace starter
-	 * 2. If the tracer is not started, acpi_gbl_trace_method_name should be
-	 *    matched if it is specified
-	 * 3. If the tracer is oneshot style, acpi_gbl_trace_method_name should
-	 *    not be cleared by the trace stopper during the first match
-	 */
-	if (acpi_gbl_trace_method_object) {
-		return (TRUE);
-	}
-	if (name &&
-	    (acpi_gbl_trace_method_name &&
-	     strcmp(acpi_gbl_trace_method_name, name))) {
-		return (FALSE);
-	}
-	if ((acpi_gbl_trace_flags & ACPI_TRACE_ONESHOT) &&
-	    !acpi_gbl_trace_method_name) {
-		return (FALSE);
-	}
-
-	return (TRUE);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_get_trace_event_name
- *
- * PARAMETERS:  type            - Trace event type
- *
- * RETURN:      Trace event name.
- *
- * DESCRIPTION: Used to obtain the full trace event name.
- *
- ******************************************************************************/
-
-#ifdef ACPI_DEBUG_OUTPUT
-
-static const char *acpi_ex_get_trace_event_name(acpi_trace_event_type type)
-{
-	switch (type) {
-	case ACPI_TRACE_AML_METHOD:
-
-		return "Method";
-
-	case ACPI_TRACE_AML_OPCODE:
-
-		return "Opcode";
-
-	case ACPI_TRACE_AML_REGION:
-
-		return "Region";
-
-	default:
-
-		return "";
-	}
-}
-
-#endif
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_trace_point
- *
- * PARAMETERS:  type                - Trace event type
- *              begin               - TRUE if before execution
- *              aml                 - Executed AML address
- *              pathname            - Object path
- *
- * RETURN:      None
- *
- * DESCRIPTION: Internal interpreter execution trace.
- *
- ******************************************************************************/
-
-void
-acpi_ex_trace_point(acpi_trace_event_type type,
-		    u8 begin, u8 *aml, char *pathname)
-{
-
-	ACPI_FUNCTION_NAME(ex_trace_point);
-
-	if (pathname) {
-		ACPI_DEBUG_PRINT((ACPI_DB_TRACE_POINT,
-				  "%s %s [0x%p:%s] execution.\n",
-				  acpi_ex_get_trace_event_name(type),
-				  begin ? "Begin" : "End", aml, pathname));
-	} else {
-		ACPI_DEBUG_PRINT((ACPI_DB_TRACE_POINT,
-				  "%s %s [0x%p] execution.\n",
-				  acpi_ex_get_trace_event_name(type),
-				  begin ? "Begin" : "End", aml));
-	}
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_start_trace_method
- *
- * PARAMETERS:  method_node         - Node of the method
- *              obj_desc            - The method object
- *              walk_state          - current state, NULL if not yet executing
- *                                    a method.
- *
- * RETURN:      None
- *
- * DESCRIPTION: Start control method execution trace
- *
- ******************************************************************************/
-
-void
-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;
-
-	ACPI_FUNCTION_NAME(ex_start_trace_method);
-
-	if (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;
-		acpi_gbl_original_dbg_level = acpi_dbg_level;
-		acpi_gbl_original_dbg_layer = acpi_dbg_layer;
-		acpi_dbg_level = ACPI_TRACE_LEVEL_ALL;
-		acpi_dbg_layer = ACPI_TRACE_LAYER_ALL;
-
-		if (acpi_gbl_trace_dbg_level) {
-			acpi_dbg_level = acpi_gbl_trace_dbg_level;
-		}
-		if (acpi_gbl_trace_dbg_layer) {
-			acpi_dbg_layer = acpi_gbl_trace_dbg_layer;
-		}
-	}
-	(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,
-				 pathname);
-	}
-	if (pathname) {
-		ACPI_FREE(pathname);
-	}
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_stop_trace_method
- *
- * PARAMETERS:  method_node         - Node of the method
- *              obj_desc            - The method object
- *              walk_state          - current state, NULL if not yet executing
- *                                    a method.
- *
- * RETURN:      None
- *
- * DESCRIPTION: Stop control method execution trace
- *
- ******************************************************************************/
-
-void
-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;
-
-	ACPI_FUNCTION_NAME(ex_stop_trace_method);
-
-	if (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) {
-
-		/* Disable further tracing if type is one-shot */
-
-		if (acpi_gbl_trace_flags & ACPI_TRACE_ONESHOT) {
-			acpi_gbl_trace_method_name = NULL;
-		}
-
-		acpi_dbg_level = acpi_gbl_original_dbg_level;
-		acpi_dbg_layer = acpi_gbl_original_dbg_layer;
-		acpi_gbl_trace_method_object = NULL;
-	}
-
-	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
-
-exit_path:
-	if (pathname) {
-		ACPI_FREE(pathname);
-	}
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_start_trace_opcode
- *
- * PARAMETERS:  op                  - The parser opcode object
- *              walk_state          - current state, NULL if not yet executing
- *                                    a method.
- *
- * RETURN:      None
- *
- * DESCRIPTION: Start opcode execution trace
- *
- ******************************************************************************/
-
-void
-acpi_ex_start_trace_opcode(union acpi_parse_object *op,
-			   struct acpi_walk_state *walk_state)
-{
-
-	ACPI_FUNCTION_NAME(ex_start_trace_opcode);
-
-	if (acpi_ex_interpreter_trace_enabled(NULL) &&
-	    (acpi_gbl_trace_flags & ACPI_TRACE_OPCODE)) {
-		ACPI_TRACE_POINT(ACPI_TRACE_AML_OPCODE, TRUE,
-				 op->common.aml, op->common.aml_op_name);
-	}
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ex_stop_trace_opcode
- *
- * PARAMETERS:  op                  - The parser opcode object
- *              walk_state          - current state, NULL if not yet executing
- *                                    a method.
- *
- * RETURN:      None
- *
- * DESCRIPTION: Stop opcode execution trace
- *
- ******************************************************************************/
-
-void
-acpi_ex_stop_trace_opcode(union acpi_parse_object *op,
-			  struct acpi_walk_state *walk_state)
-{
-
-	ACPI_FUNCTION_NAME(ex_stop_trace_opcode);
-
-	if (acpi_ex_interpreter_trace_enabled(NULL) &&
-	    (acpi_gbl_trace_flags & ACPI_TRACE_OPCODE)) {
-		ACPI_TRACE_POINT(ACPI_TRACE_AML_OPCODE, FALSE,
-				 op->common.aml, op->common.aml_op_name);
-	}
-}
diff --git a/drivers/acpi/acpica/exdump.c b/drivers/acpi/acpica/exdump.c
index d836f888bb16..ee30974b245a 100644
--- a/drivers/acpi/acpica/exdump.c
+++ b/drivers/acpi/acpica/exdump.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -508,7 +508,8 @@ acpi_ex_dump_object(union acpi_operand_object *obj_desc,
 			if (next) {
 				acpi_os_printf("(%s %2.2X)",
 					       acpi_ut_get_object_type_name
-					       (next), next->common.type);
+					       (next),
+					       next->address_space.space_id);
 
 				while (next->address_space.next) {
 					if ((next->common.type ==
@@ -520,7 +521,8 @@ acpi_ex_dump_object(union acpi_operand_object *obj_desc,
 					acpi_os_printf("->%p(%s %2.2X)", next,
 						       acpi_ut_get_object_type_name
 						       (next),
-						       next->common.type);
+						       next->address_space.
+						       space_id);
 
 					if ((next == start) || (next == data)) {
 						acpi_os_printf
diff --git a/drivers/acpi/acpica/exfield.c b/drivers/acpi/acpica/exfield.c
index 61fd9c7b88bc..d5d8020a8523 100644
--- a/drivers/acpi/acpica/exfield.c
+++ b/drivers/acpi/acpica/exfield.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -167,10 +167,11 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 		    || obj_desc->field.region_obj->region.space_id ==
 		    ACPI_ADR_SPACE_IPMI)) {
 		/*
-		 * This is an SMBus, GSBus or IPMI read. We must create a buffer to hold
-		 * the data and then directly access the region handler.
+		 * This is an SMBus, GSBus or IPMI read. We must create a buffer to
+		 * hold the data and then directly access the region handler.
 		 *
-		 * Note: SMBus and GSBus protocol value is passed in upper 16-bits of Function
+		 * Note: SMBus and GSBus protocol value is passed in upper 16-bits
+		 * of Function
 		 */
 		if (obj_desc->field.region_obj->region.space_id ==
 		    ACPI_ADR_SPACE_SMBUS) {
@@ -180,17 +181,17 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 		} else if (obj_desc->field.region_obj->region.space_id ==
 			   ACPI_ADR_SPACE_GSBUS) {
 			accessor_type = obj_desc->field.attribute;
-			length = acpi_ex_get_serial_access_length(accessor_type,
-								  obj_desc->
-								  field.
-								  access_length);
+			length =
+			    acpi_ex_get_serial_access_length(accessor_type,
+							     obj_desc->field.
+							     access_length);
 
 			/*
 			 * Add additional 2 bytes for the generic_serial_bus data buffer:
 			 *
-			 *     Status;      (Byte 0 of the data buffer)
-			 *     Length;      (Byte 1 of the data buffer)
-			 *     Data[x-1];   (Bytes 2-x of the arbitrary length data buffer)
+			 *     Status;    (Byte 0 of the data buffer)
+			 *     Length;    (Byte 1 of the data buffer)
+			 *     Data[x-1]: (Bytes 2-x of the arbitrary length data buffer)
 			 */
 			length += 2;
 			function = ACPI_READ | (accessor_type << 16);
@@ -216,6 +217,7 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 							     buffer_desc->
 							     buffer.pointer),
 					       function);
+
 		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
 		goto exit;
 	}
@@ -232,6 +234,7 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 	 */
 	length =
 	    (acpi_size) ACPI_ROUND_BITS_UP_TO_BYTES(obj_desc->field.bit_length);
+
 	if (length > acpi_gbl_integer_byte_width) {
 
 		/* Field is too large for an Integer, create a Buffer instead */
@@ -273,8 +276,10 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 
 		/* Perform the write */
 
-		status = acpi_ex_access_region(obj_desc, 0,
-					       (u64 *)buffer, ACPI_READ);
+		status =
+		    acpi_ex_access_region(obj_desc, 0, (u64 *)buffer,
+					  ACPI_READ);
+
 		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
 		if (ACPI_FAILURE(status)) {
 			acpi_ut_remove_reference(buffer_desc);
@@ -366,19 +371,22 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 		    || obj_desc->field.region_obj->region.space_id ==
 		    ACPI_ADR_SPACE_IPMI)) {
 		/*
-		 * This is an SMBus, GSBus or IPMI write. We will bypass the entire field
-		 * mechanism and handoff the buffer directly to the handler. For
-		 * these address spaces, the buffer is bi-directional; on a write,
-		 * return data is returned in the same buffer.
+		 * This is an SMBus, GSBus or IPMI write. We will bypass the entire
+		 * field mechanism and handoff the buffer directly to the handler.
+		 * For these address spaces, the buffer is bi-directional; on a
+		 * write, return data is returned in the same buffer.
 		 *
 		 * Source must be a buffer of sufficient size:
-		 * ACPI_SMBUS_BUFFER_SIZE, ACPI_GSBUS_BUFFER_SIZE, or ACPI_IPMI_BUFFER_SIZE.
+		 * ACPI_SMBUS_BUFFER_SIZE, ACPI_GSBUS_BUFFER_SIZE, or
+		 * ACPI_IPMI_BUFFER_SIZE.
 		 *
-		 * Note: SMBus and GSBus protocol type is passed in upper 16-bits of Function
+		 * Note: SMBus and GSBus protocol type is passed in upper 16-bits
+		 * of Function
 		 */
 		if (source_desc->common.type != ACPI_TYPE_BUFFER) {
 			ACPI_ERROR((AE_INFO,
-				    "SMBus/IPMI/GenericSerialBus write requires Buffer, found type %s",
+				    "SMBus/IPMI/GenericSerialBus write requires "
+				    "Buffer, found type %s",
 				    acpi_ut_get_object_type_name(source_desc)));
 
 			return_ACPI_STATUS(AE_AML_OPERAND_TYPE);
@@ -392,17 +400,17 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 		} else if (obj_desc->field.region_obj->region.space_id ==
 			   ACPI_ADR_SPACE_GSBUS) {
 			accessor_type = obj_desc->field.attribute;
-			length = acpi_ex_get_serial_access_length(accessor_type,
-								  obj_desc->
-								  field.
-								  access_length);
+			length =
+			    acpi_ex_get_serial_access_length(accessor_type,
+							     obj_desc->field.
+							     access_length);
 
 			/*
 			 * Add additional 2 bytes for the generic_serial_bus data buffer:
 			 *
-			 *     Status;      (Byte 0 of the data buffer)
-			 *     Length;      (Byte 1 of the data buffer)
-			 *     Data[x-1];   (Bytes 2-x of the arbitrary length data buffer)
+			 *     Status;    (Byte 0 of the data buffer)
+			 *     Length;    (Byte 1 of the data buffer)
+			 *     Data[x-1]: (Bytes 2-x of the arbitrary length data buffer)
 			 */
 			length += 2;
 			function = ACPI_WRITE | (accessor_type << 16);
@@ -414,7 +422,8 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 
 		if (source_desc->buffer.length < length) {
 			ACPI_ERROR((AE_INFO,
-				    "SMBus/IPMI/GenericSerialBus write requires Buffer of length %u, found length %u",
+				    "SMBus/IPMI/GenericSerialBus write requires "
+				    "Buffer of length %u, found length %u",
 				    length, source_desc->buffer.length));
 
 			return_ACPI_STATUS(AE_AML_BUFFER_LIMIT);
@@ -438,8 +447,8 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 		 * Perform the write (returns status and perhaps data in the
 		 * same buffer)
 		 */
-		status = acpi_ex_access_region(obj_desc, 0,
-					       (u64 *) buffer, function);
+		status =
+		    acpi_ex_access_region(obj_desc, 0, (u64 *)buffer, function);
 		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
 
 		*result_desc = buffer_desc;
@@ -460,7 +469,7 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 		}
 
 		ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
-				  "GPIO FieldWrite [FROM]: (%s:%X), Val %.8X  [TO]:  Pin %u Bits %u\n",
+				  "GPIO FieldWrite [FROM]: (%s:%X), Val %.8X  [TO]: Pin %u Bits %u\n",
 				  acpi_ut_get_type_name(source_desc->common.
 							type),
 				  source_desc->common.type,
@@ -476,8 +485,9 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 
 		/* Perform the write */
 
-		status = acpi_ex_access_region(obj_desc, 0,
-					       (u64 *)buffer, ACPI_WRITE);
+		status =
+		    acpi_ex_access_region(obj_desc, 0, (u64 *)buffer,
+					  ACPI_WRITE);
 		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
 		return_ACPI_STATUS(status);
 	}
diff --git a/drivers/acpi/acpica/exfldio.c b/drivers/acpi/acpica/exfldio.c
index 70b7bbbb860b..f0c5ed0b7db8 100644
--- a/drivers/acpi/acpica/exfldio.c
+++ b/drivers/acpi/acpica/exfldio.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -180,7 +180,8 @@ acpi_ex_setup_region(union acpi_operand_object *obj_desc,
 			 * byte, and a field with Dword access specified.
 			 */
 			ACPI_ERROR((AE_INFO,
-				    "Field [%4.4s] access width (%u bytes) too large for region [%4.4s] (length %u)",
+				    "Field [%4.4s] access width (%u bytes) "
+				    "too large for region [%4.4s] (length %u)",
 				    acpi_ut_get_node_name(obj_desc->
 							  common_field.node),
 				    obj_desc->common_field.access_byte_width,
@@ -194,7 +195,8 @@ acpi_ex_setup_region(union acpi_operand_object *obj_desc,
 		 * exceeds region length, indicate an error
 		 */
 		ACPI_ERROR((AE_INFO,
-			    "Field [%4.4s] Base+Offset+Width %u+%u+%u is beyond end of region [%4.4s] (length %u)",
+			    "Field [%4.4s] Base+Offset+Width %u+%u+%u "
+			    "is beyond end of region [%4.4s] (length %u)",
 			    acpi_ut_get_node_name(obj_desc->common_field.node),
 			    obj_desc->common_field.base_byte_offset,
 			    field_datum_byte_offset,
@@ -638,15 +640,15 @@ acpi_ex_write_with_update_rule(union acpi_operand_object *obj_desc,
 
 			ACPI_ERROR((AE_INFO,
 				    "Unknown UpdateRule value: 0x%X",
-				    (obj_desc->common_field.
-				     field_flags &
+				    (obj_desc->common_field.field_flags &
 				     AML_FIELD_UPDATE_RULE_MASK)));
 			return_ACPI_STATUS(AE_AML_OPERAND_VALUE);
 		}
 	}
 
 	ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
-			  "Mask %8.8X%8.8X, DatumOffset %X, Width %X, Value %8.8X%8.8X, MergedValue %8.8X%8.8X\n",
+			  "Mask %8.8X%8.8X, DatumOffset %X, Width %X, "
+			  "Value %8.8X%8.8X, MergedValue %8.8X%8.8X\n",
 			  ACPI_FORMAT_UINT64(mask),
 			  field_datum_byte_offset,
 			  obj_desc->common_field.access_byte_width,
@@ -655,8 +657,9 @@ acpi_ex_write_with_update_rule(union acpi_operand_object *obj_desc,
 
 	/* Write the merged value */
 
-	status = acpi_ex_field_datum_io(obj_desc, field_datum_byte_offset,
-					&merged_value, ACPI_WRITE);
+	status =
+	    acpi_ex_field_datum_io(obj_desc, field_datum_byte_offset,
+				   &merged_value, ACPI_WRITE);
 
 	return_ACPI_STATUS(status);
 }
@@ -764,8 +767,9 @@ acpi_ex_extract_from_field(union acpi_operand_object *obj_desc,
 		/* Get next input datum from the field */
 
 		field_offset += obj_desc->common_field.access_byte_width;
-		status = acpi_ex_field_datum_io(obj_desc, field_offset,
-						&raw_datum, ACPI_READ);
+		status =
+		    acpi_ex_field_datum_io(obj_desc, field_offset, &raw_datum,
+					   ACPI_READ);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -858,6 +862,7 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 	new_buffer = NULL;
 	required_length =
 	    ACPI_ROUND_BITS_UP_TO_BYTES(obj_desc->common_field.bit_length);
+
 	/*
 	 * We must have a buffer that is at least as long as the field
 	 * we are writing to. This is because individual fields are
@@ -932,9 +937,9 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 		/* Write merged datum to the target field */
 
 		merged_datum &= mask;
-		status = acpi_ex_write_with_update_rule(obj_desc, mask,
-							merged_datum,
-							field_offset);
+		status =
+		    acpi_ex_write_with_update_rule(obj_desc, mask, merged_datum,
+						   field_offset);
 		if (ACPI_FAILURE(status)) {
 			goto exit;
 		}
@@ -990,9 +995,9 @@ acpi_ex_insert_into_field(union acpi_operand_object *obj_desc,
 	/* Write the last datum to the field */
 
 	merged_datum &= mask;
-	status = acpi_ex_write_with_update_rule(obj_desc,
-						mask, merged_datum,
-						field_offset);
+	status =
+	    acpi_ex_write_with_update_rule(obj_desc, mask, merged_datum,
+					   field_offset);
 
 exit:
 	/* Free temporary buffer if we used one */
diff --git a/drivers/acpi/acpica/exmisc.c b/drivers/acpi/acpica/exmisc.c
index d02afece0f10..db30ae43ddd8 100644
--- a/drivers/acpi/acpica/exmisc.c
+++ b/drivers/acpi/acpica/exmisc.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -98,9 +98,9 @@ acpi_ex_get_object_reference(union acpi_operand_object *obj_desc,
 
 		default:
 
-			ACPI_ERROR((AE_INFO, "Unknown Reference Class 0x%2.2X",
+			ACPI_ERROR((AE_INFO, "Invalid Reference Class 0x%2.2X",
 				    obj_desc->reference.class));
-			return_ACPI_STATUS(AE_AML_INTERNAL);
+			return_ACPI_STATUS(AE_AML_OPERAND_TYPE);
 		}
 		break;
 
@@ -247,6 +247,7 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 	union acpi_operand_object *local_operand1 = operand1;
 	union acpi_operand_object *return_desc;
 	char *new_buf;
+	const char *type_string;
 	acpi_status status;
 
 	ACPI_FUNCTION_TRACE(ex_do_concatenate);
@@ -266,9 +267,41 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 		break;
 
 	case ACPI_TYPE_STRING:
+		/*
+		 * Per the ACPI spec, Concatenate only supports int/str/buf.
+		 * However, we support all objects here as an extension.
+		 * This improves the usefulness of the Printf() macro.
+		 * 12/2015.
+		 */
+		switch (operand1->common.type) {
+		case ACPI_TYPE_INTEGER:
+		case ACPI_TYPE_STRING:
+		case ACPI_TYPE_BUFFER:
+
+			status =
+			    acpi_ex_convert_to_string(operand1, &local_operand1,
+						      ACPI_IMPLICIT_CONVERT_HEX);
+			break;
+
+		default:
+			/*
+			 * Just emit a string containing the object type.
+			 */
+			type_string =
+			    acpi_ut_get_type_name(operand1->common.type);
+
+			local_operand1 = acpi_ut_create_string_object(((acpi_size) strlen(type_string) + 9));	/* 9 For "[Object]" */
+			if (!local_operand1) {
+				status = AE_NO_MEMORY;
+				goto cleanup;
+			}
 
-		status = acpi_ex_convert_to_string(operand1, &local_operand1,
-						   ACPI_IMPLICIT_CONVERT_HEX);
+			strcpy(local_operand1->string.pointer, "[");
+			strcat(local_operand1->string.pointer, type_string);
+			strcat(local_operand1->string.pointer, " Object]");
+			status = AE_OK;
+			break;
+		}
 		break;
 
 	case ACPI_TYPE_BUFFER:
@@ -347,8 +380,7 @@ acpi_ex_do_concatenate(union acpi_operand_object *operand0,
 		/* Concatenate the strings */
 
 		strcpy(new_buf, operand0->string.pointer);
-		strcpy(new_buf + operand0->string.length,
-		       local_operand1->string.pointer);
+		strcat(new_buf, local_operand1->string.pointer);
 		break;
 
 	case ACPI_TYPE_BUFFER:
@@ -591,8 +623,9 @@ acpi_ex_do_logical_op(u16 opcode,
 
 	case ACPI_TYPE_STRING:
 
-		status = acpi_ex_convert_to_string(operand1, &local_operand1,
-						   ACPI_IMPLICIT_CONVERT_HEX);
+		status =
+		    acpi_ex_convert_to_string(operand1, &local_operand1,
+					      ACPI_IMPLICIT_CONVERT_HEX);
 		break;
 
 	case ACPI_TYPE_BUFFER:
diff --git a/drivers/acpi/acpica/exmutex.c b/drivers/acpi/acpica/exmutex.c
index 472030f2b5bb..26faa91e930c 100644
--- a/drivers/acpi/acpica/exmutex.c
+++ b/drivers/acpi/acpica/exmutex.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -185,8 +185,9 @@ acpi_ex_acquire_mutex_object(u16 timeout,
 	if (obj_desc == acpi_gbl_global_lock_mutex) {
 		status = acpi_ev_acquire_global_lock(timeout);
 	} else {
-		status = acpi_ex_system_wait_mutex(obj_desc->mutex.os_mutex,
-						   timeout);
+		status =
+		    acpi_ex_system_wait_mutex(obj_desc->mutex.os_mutex,
+					      timeout);
 	}
 
 	if (ACPI_FAILURE(status)) {
@@ -243,20 +244,30 @@ acpi_ex_acquire_mutex(union acpi_operand_object *time_desc,
 	}
 
 	/*
-	 * Current sync level must be less than or equal to the sync level of the
-	 * mutex. This mechanism provides some deadlock prevention
+	 * Current sync level must be less than or equal to the sync level
+	 * of the mutex. This mechanism provides some deadlock prevention.
 	 */
 	if (walk_state->thread->current_sync_level > obj_desc->mutex.sync_level) {
 		ACPI_ERROR((AE_INFO,
-			    "Cannot acquire Mutex [%4.4s], current SyncLevel is too large (%u)",
+			    "Cannot acquire Mutex [%4.4s], "
+			    "current SyncLevel is too large (%u)",
 			    acpi_ut_get_node_name(obj_desc->mutex.node),
 			    walk_state->thread->current_sync_level));
 		return_ACPI_STATUS(AE_AML_MUTEX_ORDER);
 	}
 
-	status = acpi_ex_acquire_mutex_object((u16) time_desc->integer.value,
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "Acquiring: Mutex SyncLevel %u, Thread SyncLevel %u, "
+			  "Depth %u TID %p\n",
+			  obj_desc->mutex.sync_level,
+			  walk_state->thread->current_sync_level,
+			  obj_desc->mutex.acquisition_depth,
+			  walk_state->thread));
+
+	status = acpi_ex_acquire_mutex_object((u16)time_desc->integer.value,
 					      obj_desc,
 					      walk_state->thread->thread_id);
+
 	if (ACPI_SUCCESS(status) && obj_desc->mutex.acquisition_depth == 1) {
 
 		/* Save Thread object, original/current sync levels */
@@ -272,6 +283,12 @@ acpi_ex_acquire_mutex(union acpi_operand_object *time_desc,
 		acpi_ex_link_mutex(obj_desc, walk_state->thread);
 	}
 
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "Acquired: Mutex SyncLevel %u, Thread SyncLevel %u, Depth %u\n",
+			  obj_desc->mutex.sync_level,
+			  walk_state->thread->current_sync_level,
+			  obj_desc->mutex.acquisition_depth));
+
 	return_ACPI_STATUS(status);
 }
 
@@ -356,9 +373,9 @@ acpi_status
 acpi_ex_release_mutex(union acpi_operand_object *obj_desc,
 		      struct acpi_walk_state *walk_state)
 {
-	acpi_status status = AE_OK;
 	u8 previous_sync_level;
 	struct acpi_thread_state *owner_thread;
+	acpi_status status = AE_OK;
 
 	ACPI_FUNCTION_TRACE(ex_release_mutex);
 
@@ -409,7 +426,8 @@ acpi_ex_release_mutex(union acpi_operand_object *obj_desc,
 	 */
 	if (obj_desc->mutex.sync_level != owner_thread->current_sync_level) {
 		ACPI_ERROR((AE_INFO,
-			    "Cannot release Mutex [%4.4s], SyncLevel mismatch: mutex %u current %u",
+			    "Cannot release Mutex [%4.4s], SyncLevel mismatch: "
+			    "mutex %u current %u",
 			    acpi_ut_get_node_name(obj_desc->mutex.node),
 			    obj_desc->mutex.sync_level,
 			    walk_state->thread->current_sync_level));
@@ -424,6 +442,15 @@ acpi_ex_release_mutex(union acpi_operand_object *obj_desc,
 	previous_sync_level =
 	    owner_thread->acquired_mutex_list->mutex.original_sync_level;
 
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "Releasing: Object SyncLevel %u, Thread SyncLevel %u, "
+			  "Prev SyncLevel %u, Depth %u TID %p\n",
+			  obj_desc->mutex.sync_level,
+			  walk_state->thread->current_sync_level,
+			  previous_sync_level,
+			  obj_desc->mutex.acquisition_depth,
+			  walk_state->thread));
+
 	status = acpi_ex_release_mutex_object(obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
@@ -436,6 +463,14 @@ acpi_ex_release_mutex(union acpi_operand_object *obj_desc,
 		owner_thread->current_sync_level = previous_sync_level;
 	}
 
+	ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
+			  "Released: Object SyncLevel %u, Thread SyncLevel, %u, "
+			  "Prev SyncLevel %u, Depth %u\n",
+			  obj_desc->mutex.sync_level,
+			  walk_state->thread->current_sync_level,
+			  previous_sync_level,
+			  obj_desc->mutex.acquisition_depth));
+
 	return_ACPI_STATUS(status);
 }
 
@@ -462,21 +497,17 @@ void acpi_ex_release_all_mutexes(struct acpi_thread_state *thread)
 	union acpi_operand_object *next = thread->acquired_mutex_list;
 	union acpi_operand_object *obj_desc;
 
-	ACPI_FUNCTION_NAME(ex_release_all_mutexes);
+	ACPI_FUNCTION_TRACE(ex_release_all_mutexes);
 
 	/* Traverse the list of owned mutexes, releasing each one */
 
 	while (next) {
 		obj_desc = next;
-		next = obj_desc->mutex.next;
-
-		obj_desc->mutex.prev = NULL;
-		obj_desc->mutex.next = NULL;
-		obj_desc->mutex.acquisition_depth = 0;
-
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "Force-releasing held mutex: %p\n",
-				  obj_desc));
+				  "Mutex [%4.4s] force-release, SyncLevel %u Depth %u\n",
+				  obj_desc->mutex.node->name.ascii,
+				  obj_desc->mutex.sync_level,
+				  obj_desc->mutex.acquisition_depth));
 
 		/* Release the mutex, special case for Global Lock */
 
@@ -489,14 +520,21 @@ void acpi_ex_release_all_mutexes(struct acpi_thread_state *thread)
 			acpi_os_release_mutex(obj_desc->mutex.os_mutex);
 		}
 
-		/* Mark mutex unowned */
-
-		obj_desc->mutex.owner_thread = NULL;
-		obj_desc->mutex.thread_id = 0;
-
 		/* Update Thread sync_level (Last mutex is the important one) */
 
 		thread->current_sync_level =
 		    obj_desc->mutex.original_sync_level;
+
+		/* Mark mutex unowned */
+
+		next = obj_desc->mutex.next;
+
+		obj_desc->mutex.prev = NULL;
+		obj_desc->mutex.next = NULL;
+		obj_desc->mutex.acquisition_depth = 0;
+		obj_desc->mutex.owner_thread = NULL;
+		obj_desc->mutex.thread_id = 0;
 	}
+
+	return_VOID;
 }
diff --git a/drivers/acpi/acpica/exnames.c b/drivers/acpi/acpica/exnames.c
index 20e87813c7d7..27c11ab5eb04 100644
--- a/drivers/acpi/acpica/exnames.c
+++ b/drivers/acpi/acpica/exnames.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -164,8 +164,8 @@ static acpi_status acpi_ex_name_segment(u8 ** in_aml_address, char *name_string)
 	ACPI_FUNCTION_TRACE(ex_name_segment);
 
 	/*
-	 * If first character is a digit, then we know that we aren't looking at a
-	 * valid name segment
+	 * If first character is a digit, then we know that we aren't looking
+	 * at a valid name segment
 	 */
 	char_buf[0] = *aml_address;
 
diff --git a/drivers/acpi/acpica/exoparg1.c b/drivers/acpi/acpica/exoparg1.c
index 77930683ab7d..4e17506a7384 100644
--- a/drivers/acpi/acpica/exoparg1.c
+++ b/drivers/acpi/acpica/exoparg1.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -484,22 +484,26 @@ acpi_status acpi_ex_opcode_1A_1T_1R(struct acpi_walk_state *walk_state)
 
 	case AML_TO_DECSTRING_OP:	/* to_decimal_string (Data, Result) */
 
-		status = acpi_ex_convert_to_string(operand[0], &return_desc,
-						   ACPI_EXPLICIT_CONVERT_DECIMAL);
+		status =
+		    acpi_ex_convert_to_string(operand[0], &return_desc,
+					      ACPI_EXPLICIT_CONVERT_DECIMAL);
 		if (return_desc == operand[0]) {
 
 			/* No conversion performed, add ref to handle return value */
+
 			acpi_ut_add_reference(return_desc);
 		}
 		break;
 
 	case AML_TO_HEXSTRING_OP:	/* to_hex_string (Data, Result) */
 
-		status = acpi_ex_convert_to_string(operand[0], &return_desc,
-						   ACPI_EXPLICIT_CONVERT_HEX);
+		status =
+		    acpi_ex_convert_to_string(operand[0], &return_desc,
+					      ACPI_EXPLICIT_CONVERT_HEX);
 		if (return_desc == operand[0]) {
 
 			/* No conversion performed, add ref to handle return value */
+
 			acpi_ut_add_reference(return_desc);
 		}
 		break;
@@ -510,17 +514,20 @@ acpi_status acpi_ex_opcode_1A_1T_1R(struct acpi_walk_state *walk_state)
 		if (return_desc == operand[0]) {
 
 			/* No conversion performed, add ref to handle return value */
+
 			acpi_ut_add_reference(return_desc);
 		}
 		break;
 
 	case AML_TO_INTEGER_OP:	/* to_integer (Data, Result) */
 
-		status = acpi_ex_convert_to_integer(operand[0], &return_desc,
-						    ACPI_ANY_BASE);
+		status =
+		    acpi_ex_convert_to_integer(operand[0], &return_desc,
+					       ACPI_ANY_BASE);
 		if (return_desc == operand[0]) {
 
 			/* No conversion performed, add ref to handle return value */
+
 			acpi_ut_add_reference(return_desc);
 		}
 		break;
@@ -679,7 +686,7 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 		status = acpi_ex_store(return_desc, operand[0], walk_state);
 		break;
 
-	case AML_TYPE_OP:	/* object_type (source_object) */
+	case AML_OBJECT_TYPE_OP:	/* object_type (source_object) */
 		/*
 		 * Note: The operand is not resolved at this point because we want to
 		 * get the associated object, not its value. For example, we don't
@@ -713,9 +720,9 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 
 		/* Get the base object */
 
-		status = acpi_ex_resolve_multiple(walk_state,
-						  operand[0], &type,
-						  &temp_desc);
+		status =
+		    acpi_ex_resolve_multiple(walk_state, operand[0], &type,
+					     &temp_desc);
 		if (ACPI_FAILURE(status)) {
 			goto cleanup;
 		}
@@ -759,8 +766,10 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 		default:
 
 			ACPI_ERROR((AE_INFO,
-				    "Operand must be Buffer/Integer/String/Package - found type %s",
+				    "Operand must be Buffer/Integer/String/Package"
+				    " - found type %s",
 				    acpi_ut_get_type_name(type)));
+
 			status = AE_AML_OPERAND_TYPE;
 			goto cleanup;
 		}
@@ -981,6 +990,7 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 						    "Unknown Index TargetType 0x%X in reference object %p",
 						    operand[0]->reference.
 						    target_type, operand[0]));
+
 					status = AE_AML_OPERAND_TYPE;
 					goto cleanup;
 				}
@@ -1050,6 +1060,7 @@ acpi_status acpi_ex_opcode_1A_0T_1R(struct acpi_walk_state *walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/exoparg2.c b/drivers/acpi/acpica/exoparg2.c
index b8944ebb1081..79ef3b6811a9 100644
--- a/drivers/acpi/acpica/exoparg2.c
+++ b/drivers/acpi/acpica/exoparg2.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -199,6 +199,7 @@ acpi_status acpi_ex_opcode_2A_2T_1R(struct acpi_walk_state *walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
@@ -299,8 +300,9 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 
 	case AML_CONCAT_OP:	/* Concatenate (Data1, Data2, Result) */
 
-		status = acpi_ex_do_concatenate(operand[0], operand[1],
-						&return_desc, walk_state);
+		status =
+		    acpi_ex_do_concatenate(operand[0], operand[1], &return_desc,
+					   walk_state);
 		break;
 
 	case AML_TO_STRING_OP:	/* to_string (Buffer, Length, Result) (ACPI 2.0) */
@@ -345,8 +347,9 @@ acpi_status acpi_ex_opcode_2A_1T_1R(struct acpi_walk_state *walk_state)
 
 		/* concatenate_res_template (Buffer, Buffer, Result) (ACPI 2.0) */
 
-		status = acpi_ex_concat_template(operand[0], operand[1],
-						 &return_desc, walk_state);
+		status =
+		    acpi_ex_concat_template(operand[0], operand[1],
+					    &return_desc, walk_state);
 		break;
 
 	case AML_INDEX_OP:	/* Index (Source Index Result) */
@@ -553,6 +556,7 @@ acpi_status acpi_ex_opcode_2A_0T_1R(struct acpi_walk_state *walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/exoparg3.c b/drivers/acpi/acpica/exoparg3.c
index fa100b3b92ee..28eb861c44eb 100644
--- a/drivers/acpi/acpica/exoparg3.c
+++ b/drivers/acpi/acpica/exoparg3.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -95,10 +95,11 @@ acpi_status acpi_ex_opcode_3A_0T_0R(struct acpi_walk_state *walk_state)
 	case AML_FATAL_OP:	/* Fatal (fatal_type fatal_code fatal_arg) */
 
 		ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-				  "FatalOp: Type %X Code %X Arg %X <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n",
-				  (u32) operand[0]->integer.value,
-				  (u32) operand[1]->integer.value,
-				  (u32) operand[2]->integer.value));
+				  "FatalOp: Type %X Code %X Arg %X "
+				  "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n",
+				  (u32)operand[0]->integer.value,
+				  (u32)operand[1]->integer.value,
+				  (u32)operand[2]->integer.value));
 
 		fatal = ACPI_ALLOCATE(sizeof(struct acpi_signal_fatal_info));
 		if (fatal) {
@@ -131,6 +132,7 @@ acpi_status acpi_ex_opcode_3A_0T_0R(struct acpi_walk_state *walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
@@ -193,7 +195,8 @@ acpi_status acpi_ex_opcode_3A_1T_1R(struct acpi_walk_state *walk_state)
 		/* Truncate request if larger than the actual String/Buffer */
 
 		else if ((index + length) > operand[0]->string.length) {
-			length = (acpi_size) operand[0]->string.length -
+			length =
+			    (acpi_size) operand[0]->string.length -
 			    (acpi_size) index;
 		}
 
@@ -237,8 +240,8 @@ acpi_status acpi_ex_opcode_3A_1T_1R(struct acpi_walk_state *walk_state)
 
 			/* We have a buffer, copy the portion requested */
 
-			memcpy(buffer, operand[0]->string.pointer + index,
-			       length);
+			memcpy(buffer,
+			       operand[0]->string.pointer + index, length);
 		}
 
 		/* Set the length of the new String/Buffer */
@@ -255,6 +258,7 @@ acpi_status acpi_ex_opcode_3A_1T_1R(struct acpi_walk_state *walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
@@ -270,12 +274,11 @@ cleanup:
 	if (ACPI_FAILURE(status) || walk_state->result_obj) {
 		acpi_ut_remove_reference(return_desc);
 		walk_state->result_obj = NULL;
-	}
+	} else {
+		/* Set the return object and exit */
 
-	/* Set the return object and exit */
-
-	else {
 		walk_state->result_obj = return_desc;
 	}
+
 	return_ACPI_STATUS(status);
 }
diff --git a/drivers/acpi/acpica/exoparg6.c b/drivers/acpi/acpica/exoparg6.c
index c930edda3f65..e2b63483857f 100644
--- a/drivers/acpi/acpica/exoparg6.c
+++ b/drivers/acpi/acpica/exoparg6.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -310,6 +310,7 @@ acpi_status acpi_ex_opcode_6A_0T_1R(struct acpi_walk_state * walk_state)
 
 		ACPI_ERROR((AE_INFO, "Unknown AML opcode 0x%X",
 			    walk_state->opcode));
+
 		status = AE_AML_BAD_OPCODE;
 		goto cleanup;
 	}
diff --git a/drivers/acpi/acpica/exprep.c b/drivers/acpi/acpica/exprep.c
index 4c2836dc825b..aed8d3459220 100644
--- a/drivers/acpi/acpica/exprep.c
+++ b/drivers/acpi/acpica/exprep.c
@@ -1,11 +1,11 @@
 /******************************************************************************
  *
- * Module Name: exprep - ACPI AML (p-code) execution - field prep utilities
+ * Module Name: exprep - ACPI AML field prep utilities
  *
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -103,8 +103,10 @@ acpi_ex_generate_access(u32 field_bit_offset,
 	/* Round Field start offset and length to "minimal" byte boundaries */
 
 	field_byte_offset = ACPI_DIV_8(ACPI_ROUND_DOWN(field_bit_offset, 8));
-	field_byte_end_offset = ACPI_DIV_8(ACPI_ROUND_UP(field_bit_length +
-							 field_bit_offset, 8));
+
+	field_byte_end_offset =
+	    ACPI_DIV_8(ACPI_ROUND_UP(field_bit_length + field_bit_offset, 8));
+
 	field_byte_length = field_byte_end_offset - field_byte_offset;
 
 	ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
@@ -159,7 +161,8 @@ acpi_ex_generate_access(u32 field_bit_offset,
 
 			if (accesses <= 1) {
 				ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
-						  "Entire field can be accessed with one operation of size %u\n",
+						  "Entire field can be accessed "
+						  "with one operation of size %u\n",
 						  access_byte_width));
 				return_VALUE(access_byte_width);
 			}
@@ -202,6 +205,7 @@ acpi_ex_generate_access(u32 field_bit_offset,
 	 */
 	ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
 			  "Cannot access field in one operation, using width 8\n"));
+
 	return_VALUE(8);
 }
 #endif				/* ACPI_UNDER_DEVELOPMENT */
@@ -281,6 +285,7 @@ acpi_ex_decode_field_access(union acpi_operand_object *obj_desc,
 		/* Invalid field access type */
 
 		ACPI_ERROR((AE_INFO, "Unknown field access type 0x%X", access));
+
 		return_UINT32(0);
 	}
 
@@ -354,8 +359,8 @@ acpi_ex_prep_common_field_object(union acpi_operand_object *obj_desc,
 	 * For all other access types (Byte, Word, Dword, Qword), the Bitwidth is
 	 * the same (equivalent) as the byte_alignment.
 	 */
-	access_bit_width = acpi_ex_decode_field_access(obj_desc, field_flags,
-						       &byte_alignment);
+	access_bit_width =
+	    acpi_ex_decode_field_access(obj_desc, field_flags, &byte_alignment);
 	if (!access_bit_width) {
 		return_ACPI_STATUS(AE_AML_OPERAND_VALUE);
 	}
@@ -595,7 +600,8 @@ acpi_status acpi_ex_prep_field_value(struct acpi_create_field_info *info)
 					  access_byte_width);
 
 		ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
-				  "IndexField: BitOff %X, Off %X, Value %X, Gran %X, Index %p, Data %p\n",
+				  "IndexField: BitOff %X, Off %X, Value %X, "
+				  "Gran %X, Index %p, Data %p\n",
 				  obj_desc->index_field.start_field_bit_offset,
 				  obj_desc->index_field.base_byte_offset,
 				  obj_desc->index_field.value,
@@ -615,8 +621,9 @@ acpi_status acpi_ex_prep_field_value(struct acpi_create_field_info *info)
 	 * Store the constructed descriptor (obj_desc) into the parent Node,
 	 * preserving the current type of that named_obj.
 	 */
-	status = acpi_ns_attach_object(info->field_node, obj_desc,
-				       acpi_ns_get_type(info->field_node));
+	status =
+	    acpi_ns_attach_object(info->field_node, obj_desc,
+				  acpi_ns_get_type(info->field_node));
 
 	ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
 			  "Set NamedObj %p [%4.4s], ObjDesc %p\n",
diff --git a/drivers/acpi/acpica/exregion.c b/drivers/acpi/acpica/exregion.c
index b4a5e44c00dd..076074daf2b6 100644
--- a/drivers/acpi/acpica/exregion.c
+++ b/drivers/acpi/acpica/exregion.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -392,7 +392,8 @@ acpi_ex_pci_config_space_handler(u32 function,
 	pci_register = (u16) (u32) address;
 
 	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
-			  "Pci-Config %u (%u) Seg(%04x) Bus(%04x) Dev(%04x) Func(%04x) Reg(%04x)\n",
+			  "Pci-Config %u (%u) Seg(%04x) Bus(%04x) "
+			  "Dev(%04x) Func(%04x) Reg(%04x)\n",
 			  function, bit_width, pci_id->segment, pci_id->bus,
 			  pci_id->device, pci_id->function, pci_register));
 
@@ -400,14 +401,16 @@ acpi_ex_pci_config_space_handler(u32 function,
 	case ACPI_READ:
 
 		*value = 0;
-		status = acpi_os_read_pci_configuration(pci_id, pci_register,
-							value, bit_width);
+		status =
+		    acpi_os_read_pci_configuration(pci_id, pci_register, value,
+						   bit_width);
 		break;
 
 	case ACPI_WRITE:
 
-		status = acpi_os_write_pci_configuration(pci_id, pci_register,
-							 *value, bit_width);
+		status =
+		    acpi_os_write_pci_configuration(pci_id, pci_register,
+						    *value, bit_width);
 		break;
 
 	default:
diff --git a/drivers/acpi/acpica/exresnte.c b/drivers/acpi/acpica/exresnte.c
index 1b372ef69308..c1e8bfb0f7f4 100644
--- a/drivers/acpi/acpica/exresnte.c
+++ b/drivers/acpi/acpica/exresnte.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -112,7 +112,7 @@ acpi_ex_resolve_node_to_value(struct acpi_namespace_node **object_ptr,
 
 	/*
 	 * Several object types require no further processing:
-	 * 1) Device/Thermal objects don't have a "real" subobject, return the Node
+	 * 1) Device/Thermal objects don't have a "real" subobject, return Node
 	 * 2) Method locals and arguments have a pseudo-Node
 	 * 3) 10/2007: Added method type to assist with Package construction.
 	 */
diff --git a/drivers/acpi/acpica/exresolv.c b/drivers/acpi/acpica/exresolv.c
index a1afe1a1e7c2..fedacf13dc36 100644
--- a/drivers/acpi/acpica/exresolv.c
+++ b/drivers/acpi/acpica/exresolv.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -217,7 +217,8 @@ acpi_ex_resolve_object_to_value(union acpi_operand_object **stack_ptr,
 					 * the package, can't dereference it
 					 */
 					ACPI_ERROR((AE_INFO,
-						    "Attempt to dereference an Index to NULL package element Idx=%p",
+						    "Attempt to dereference an Index to "
+						    "NULL package element Idx=%p",
 						    stack_desc));
 					status = AE_AML_UNINITIALIZED_ELEMENT;
 				}
@@ -361,10 +362,9 @@ acpi_ex_resolve_multiple(struct acpi_walk_state *walk_state,
 
 		if (type == ACPI_TYPE_LOCAL_ALIAS) {
 			type = ((struct acpi_namespace_node *)obj_desc)->type;
-			obj_desc =
-			    acpi_ns_get_attached_object((struct
-							 acpi_namespace_node *)
-							obj_desc);
+			obj_desc = acpi_ns_get_attached_object((struct
+								acpi_namespace_node
+								*)obj_desc);
 		}
 
 		if (!obj_desc) {
diff --git a/drivers/acpi/acpica/exresop.c b/drivers/acpi/acpica/exresop.c
index 424442d50b5e..cc2c26c46a6d 100644
--- a/drivers/acpi/acpica/exresop.c
+++ b/drivers/acpi/acpica/exresop.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -90,8 +90,8 @@ acpi_ex_check_object_type(acpi_object_type type_needed,
 		 * specification, a store to a constant is a noop.)
 		 */
 		if ((this_type == ACPI_TYPE_INTEGER) &&
-		    (((union acpi_operand_object *)object)->common.
-		     flags & AOPOBJ_AML_CONSTANT)) {
+		    (((union acpi_operand_object *)object)->common.flags &
+		     AOPOBJ_AML_CONSTANT)) {
 			return (AE_OK);
 		}
 	}
@@ -196,10 +196,10 @@ acpi_ex_resolve_operands(u16 opcode,
 			 * thus, the attached object is always the aliased namespace node
 			 */
 			if (object_type == ACPI_TYPE_LOCAL_ALIAS) {
-				obj_desc =
-				    acpi_ns_get_attached_object((struct
-								 acpi_namespace_node
-								 *)obj_desc);
+				obj_desc = acpi_ns_get_attached_object((struct
+									acpi_namespace_node
+									*)
+								       obj_desc);
 				*stack_ptr = obj_desc;
 				object_type =
 				    ((struct acpi_namespace_node *)obj_desc)->
@@ -285,8 +285,8 @@ acpi_ex_resolve_operands(u16 opcode,
 		case ARGI_REF_OR_STRING:	/* Can be a String or Reference */
 
 			if ((ACPI_GET_DESCRIPTOR_TYPE(obj_desc) ==
-			     ACPI_DESC_TYPE_OPERAND)
-			    && (obj_desc->common.type == ACPI_TYPE_STRING)) {
+			     ACPI_DESC_TYPE_OPERAND) &&
+			    (obj_desc->common.type == ACPI_TYPE_STRING)) {
 				/*
 				 * String found - the string references a named object and
 				 * must be resolved to a node
@@ -465,8 +465,9 @@ acpi_ex_resolve_operands(u16 opcode,
 			 * But we can implicitly convert from a BUFFER or INTEGER
 			 * aka - "Implicit Source Operand Conversion"
 			 */
-			status = acpi_ex_convert_to_string(obj_desc, stack_ptr,
-							   ACPI_IMPLICIT_CONVERT_HEX);
+			status =
+			    acpi_ex_convert_to_string(obj_desc, stack_ptr,
+						      ACPI_IMPLICIT_CONVERT_HEX);
 			if (ACPI_FAILURE(status)) {
 				if (status == AE_TYPE) {
 					ACPI_ERROR((AE_INFO,
@@ -597,8 +598,10 @@ acpi_ex_resolve_operands(u16 opcode,
 
 		case ARGI_REGION_OR_BUFFER:	/* Used by Load() only */
 
-			/* Need an operand of type REGION or a BUFFER (which could be a resolved region field) */
-
+			/*
+			 * Need an operand of type REGION or a BUFFER
+			 * (which could be a resolved region field)
+			 */
 			switch (obj_desc->common.type) {
 			case ACPI_TYPE_BUFFER:
 			case ACPI_TYPE_REGION:
@@ -640,9 +643,9 @@ acpi_ex_resolve_operands(u16 opcode,
 
 				if (acpi_gbl_enable_interpreter_slack) {
 					/*
-					 * Enable original behavior of Store(), allowing any and all
-					 * objects as the source operand. The ACPI spec does not
-					 * allow this, however.
+					 * Enable original behavior of Store(), allowing any
+					 * and all objects as the source operand. The ACPI
+					 * spec does not allow this, however.
 					 */
 					break;
 				}
@@ -655,7 +658,8 @@ acpi_ex_resolve_operands(u16 opcode,
 				}
 
 				ACPI_ERROR((AE_INFO,
-					    "Needed Integer/Buffer/String/Package/Ref/Ddb], found [%s] %p",
+					    "Needed Integer/Buffer/String/Package/Ref/Ddb]"
+					    ", found [%s] %p",
 					    acpi_ut_get_object_type_name
 					    (obj_desc), obj_desc));
 
@@ -678,9 +682,10 @@ acpi_ex_resolve_operands(u16 opcode,
 		 * Make sure that the original object was resolved to the
 		 * required object type (Simple cases only).
 		 */
-		status = acpi_ex_check_object_type(type_needed,
-						   (*stack_ptr)->common.type,
-						   *stack_ptr);
+		status =
+		    acpi_ex_check_object_type(type_needed,
+					      (*stack_ptr)->common.type,
+					      *stack_ptr);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
diff --git a/drivers/acpi/acpica/exstore.c b/drivers/acpi/acpica/exstore.c
index c076e9100d66..cd70cbcf6de6 100644
--- a/drivers/acpi/acpica/exstore.c
+++ b/drivers/acpi/acpica/exstore.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -467,7 +467,8 @@ acpi_ex_store_object_to_node(union acpi_operand_object *source_desc,
 		case ACPI_TYPE_THERMAL:
 
 			ACPI_ERROR((AE_INFO,
-				    "Target must be [Buffer/Integer/String/Reference], found [%s] (%4.4s)",
+				    "Target must be [Buffer/Integer/String/Reference]"
+				    ", found [%s] (%4.4s)",
 				    acpi_ut_get_type_name(node->type),
 				    node->name.ascii));
 
@@ -504,8 +505,9 @@ acpi_ex_store_object_to_node(union acpi_operand_object *source_desc,
 			 * an implicit conversion, as per the ACPI specification.
 			 * A direct store is performed instead.
 			 */
-			status = acpi_ex_store_direct_to_node(source_desc, node,
-							      walk_state);
+			status =
+			    acpi_ex_store_direct_to_node(source_desc, node,
+							 walk_state);
 			break;
 		}
 
@@ -528,8 +530,9 @@ acpi_ex_store_object_to_node(union acpi_operand_object *source_desc,
 			 * store has been performed such that the node/object type
 			 * has been changed.
 			 */
-			status = acpi_ns_attach_object(node, new_desc,
-						       new_desc->common.type);
+			status =
+			    acpi_ns_attach_object(node, new_desc,
+						  new_desc->common.type);
 
 			ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
 					  "Store type [%s] into [%s] via Convert/Attach\n",
@@ -563,8 +566,8 @@ acpi_ex_store_object_to_node(union acpi_operand_object *source_desc,
 		 * operator. (Note, for this default case, all normal
 		 * Store/Target operations exited above with an error).
 		 */
-		status = acpi_ex_store_direct_to_node(source_desc, node,
-						      walk_state);
+		status =
+		    acpi_ex_store_direct_to_node(source_desc, node, walk_state);
 		break;
 	}
 
diff --git a/drivers/acpi/acpica/exstoren.c b/drivers/acpi/acpica/exstoren.c
index d1841defa669..13bbb2b241a3 100644
--- a/drivers/acpi/acpica/exstoren.c
+++ b/drivers/acpi/acpica/exstoren.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/exstorob.c b/drivers/acpi/acpica/exstorob.c
index e1d4f4d51b97..28b724827f0f 100644
--- a/drivers/acpi/acpica/exstorob.c
+++ b/drivers/acpi/acpica/exstorob.c
@@ -1,11 +1,11 @@
 /******************************************************************************
  *
- * Module Name: exstorob - AML Interpreter object store support, store to object
+ * Module Name: exstorob - AML object store support, store to object
  *
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -203,8 +203,9 @@ acpi_ex_store_string_to_string(union acpi_operand_object *source_desc,
 			ACPI_FREE(target_desc->string.pointer);
 		}
 
-		target_desc->string.pointer = ACPI_ALLOCATE_ZEROED((acpi_size)
-								   length + 1);
+		target_desc->string.pointer =
+		    ACPI_ALLOCATE_ZEROED((acpi_size) length + 1);
+
 		if (!target_desc->string.pointer) {
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
diff --git a/drivers/acpi/acpica/exsystem.c b/drivers/acpi/acpica/exsystem.c
index 05450656fe3d..ac09c31cc70e 100644
--- a/drivers/acpi/acpica/exsystem.c
+++ b/drivers/acpi/acpica/exsystem.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -78,7 +78,6 @@ acpi_status acpi_ex_system_wait_semaphore(acpi_semaphore semaphore, u16 timeout)
 		/* We must wait, so unlock the interpreter */
 
 		acpi_ex_exit_interpreter();
-
 		status = acpi_os_wait_semaphore(semaphore, 1, timeout);
 
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
@@ -124,7 +123,6 @@ acpi_status acpi_ex_system_wait_mutex(acpi_mutex mutex, u16 timeout)
 		/* We must wait, so unlock the interpreter */
 
 		acpi_ex_exit_interpreter();
-
 		status = acpi_os_acquire_mutex(mutex, timeout);
 
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
@@ -169,8 +167,8 @@ acpi_status acpi_ex_system_do_stall(u32 how_long)
 		 * (ACPI specifies 100 usec as max, but this gives some slack in
 		 * order to support existing BIOSs)
 		 */
-		ACPI_ERROR((AE_INFO, "Time parameter is too large (%u)",
-			    how_long));
+		ACPI_ERROR((AE_INFO,
+			    "Time parameter is too large (%u)", how_long));
 		status = AE_AML_OPERAND_VALUE;
 	} else {
 		acpi_os_stall(how_long);
diff --git a/drivers/acpi/acpica/extrace.c b/drivers/acpi/acpica/extrace.c
new file mode 100644
index 000000000000..b52e84841c1a
--- /dev/null
+++ b/drivers/acpi/acpica/extrace.c
@@ -0,0 +1,377 @@
+/******************************************************************************
+ *
+ * Module Name: extrace - Support for interpreter execution tracing
+ *
+ *****************************************************************************/
+
+/*
+ * 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"
+#include "acnamesp.h"
+#include "acinterp.h"
+
+#define _COMPONENT          ACPI_EXECUTER
+ACPI_MODULE_NAME("extrace")
+
+static union acpi_operand_object *acpi_gbl_trace_method_object = NULL;
+
+/* Local prototypes */
+
+#ifdef ACPI_DEBUG_OUTPUT
+static const char *acpi_ex_get_trace_event_name(acpi_trace_event_type type);
+#endif
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_interpreter_trace_enabled
+ *
+ * PARAMETERS:  name                - Whether method name should be matched,
+ *                                    this should be checked before starting
+ *                                    the tracer
+ *
+ * RETURN:      TRUE if interpreter trace is enabled.
+ *
+ * DESCRIPTION: Check whether interpreter trace is enabled
+ *
+ ******************************************************************************/
+
+static u8 acpi_ex_interpreter_trace_enabled(char *name)
+{
+
+	/* Check if tracing is enabled */
+
+	if (!(acpi_gbl_trace_flags & ACPI_TRACE_ENABLED)) {
+		return (FALSE);
+	}
+
+	/*
+	 * Check if tracing is filtered:
+	 *
+	 * 1. If the tracer is started, acpi_gbl_trace_method_object should have
+	 *    been filled by the trace starter
+	 * 2. If the tracer is not started, acpi_gbl_trace_method_name should be
+	 *    matched if it is specified
+	 * 3. If the tracer is oneshot style, acpi_gbl_trace_method_name should
+	 *    not be cleared by the trace stopper during the first match
+	 */
+	if (acpi_gbl_trace_method_object) {
+		return (TRUE);
+	}
+
+	if (name &&
+	    (acpi_gbl_trace_method_name &&
+	     strcmp(acpi_gbl_trace_method_name, name))) {
+		return (FALSE);
+	}
+
+	if ((acpi_gbl_trace_flags & ACPI_TRACE_ONESHOT) &&
+	    !acpi_gbl_trace_method_name) {
+		return (FALSE);
+	}
+
+	return (TRUE);
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_get_trace_event_name
+ *
+ * PARAMETERS:  type            - Trace event type
+ *
+ * RETURN:      Trace event name.
+ *
+ * DESCRIPTION: Used to obtain the full trace event name.
+ *
+ ******************************************************************************/
+
+#ifdef ACPI_DEBUG_OUTPUT
+
+static const char *acpi_ex_get_trace_event_name(acpi_trace_event_type type)
+{
+
+	switch (type) {
+	case ACPI_TRACE_AML_METHOD:
+
+		return "Method";
+
+	case ACPI_TRACE_AML_OPCODE:
+
+		return "Opcode";
+
+	case ACPI_TRACE_AML_REGION:
+
+		return "Region";
+
+	default:
+
+		return "";
+	}
+}
+
+#endif
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_trace_point
+ *
+ * PARAMETERS:  type                - Trace event type
+ *              begin               - TRUE if before execution
+ *              aml                 - Executed AML address
+ *              pathname            - Object path
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Internal interpreter execution trace.
+ *
+ ******************************************************************************/
+
+void
+acpi_ex_trace_point(acpi_trace_event_type type,
+		    u8 begin, u8 *aml, char *pathname)
+{
+
+	ACPI_FUNCTION_NAME(ex_trace_point);
+
+	if (pathname) {
+		ACPI_DEBUG_PRINT((ACPI_DB_TRACE_POINT,
+				  "%s %s [0x%p:%s] execution.\n",
+				  acpi_ex_get_trace_event_name(type),
+				  begin ? "Begin" : "End", aml, pathname));
+	} else {
+		ACPI_DEBUG_PRINT((ACPI_DB_TRACE_POINT,
+				  "%s %s [0x%p] execution.\n",
+				  acpi_ex_get_trace_event_name(type),
+				  begin ? "Begin" : "End", aml));
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_start_trace_method
+ *
+ * PARAMETERS:  method_node         - Node of the method
+ *              obj_desc            - The method object
+ *              walk_state          - current state, NULL if not yet executing
+ *                                    a method.
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Start control method execution trace
+ *
+ ******************************************************************************/
+
+void
+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;
+
+	ACPI_FUNCTION_NAME(ex_start_trace_method);
+
+	if (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;
+		acpi_gbl_original_dbg_level = acpi_dbg_level;
+		acpi_gbl_original_dbg_layer = acpi_dbg_layer;
+		acpi_dbg_level = ACPI_TRACE_LEVEL_ALL;
+		acpi_dbg_layer = ACPI_TRACE_LAYER_ALL;
+
+		if (acpi_gbl_trace_dbg_level) {
+			acpi_dbg_level = acpi_gbl_trace_dbg_level;
+		}
+
+		if (acpi_gbl_trace_dbg_layer) {
+			acpi_dbg_layer = acpi_gbl_trace_dbg_layer;
+		}
+	}
+
+	(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,
+				 pathname);
+	}
+
+	if (pathname) {
+		ACPI_FREE(pathname);
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_stop_trace_method
+ *
+ * PARAMETERS:  method_node         - Node of the method
+ *              obj_desc            - The method object
+ *              walk_state          - current state, NULL if not yet executing
+ *                                    a method.
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Stop control method execution trace
+ *
+ ******************************************************************************/
+
+void
+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;
+
+	ACPI_FUNCTION_NAME(ex_stop_trace_method);
+
+	if (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) {
+
+		/* Disable further tracing if type is one-shot */
+
+		if (acpi_gbl_trace_flags & ACPI_TRACE_ONESHOT) {
+			acpi_gbl_trace_method_name = NULL;
+		}
+
+		acpi_dbg_level = acpi_gbl_original_dbg_level;
+		acpi_dbg_layer = acpi_gbl_original_dbg_layer;
+		acpi_gbl_trace_method_object = NULL;
+	}
+
+	(void)acpi_ut_release_mutex(ACPI_MTX_NAMESPACE);
+
+exit_path:
+	if (pathname) {
+		ACPI_FREE(pathname);
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_start_trace_opcode
+ *
+ * PARAMETERS:  op                  - The parser opcode object
+ *              walk_state          - current state, NULL if not yet executing
+ *                                    a method.
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Start opcode execution trace
+ *
+ ******************************************************************************/
+
+void
+acpi_ex_start_trace_opcode(union acpi_parse_object *op,
+			   struct acpi_walk_state *walk_state)
+{
+
+	ACPI_FUNCTION_NAME(ex_start_trace_opcode);
+
+	if (acpi_ex_interpreter_trace_enabled(NULL) &&
+	    (acpi_gbl_trace_flags & ACPI_TRACE_OPCODE)) {
+		ACPI_TRACE_POINT(ACPI_TRACE_AML_OPCODE, TRUE,
+				 op->common.aml, op->common.aml_op_name);
+	}
+}
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ex_stop_trace_opcode
+ *
+ * PARAMETERS:  op                  - The parser opcode object
+ *              walk_state          - current state, NULL if not yet executing
+ *                                    a method.
+ *
+ * RETURN:      None
+ *
+ * DESCRIPTION: Stop opcode execution trace
+ *
+ ******************************************************************************/
+
+void
+acpi_ex_stop_trace_opcode(union acpi_parse_object *op,
+			  struct acpi_walk_state *walk_state)
+{
+
+	ACPI_FUNCTION_NAME(ex_stop_trace_opcode);
+
+	if (acpi_ex_interpreter_trace_enabled(NULL) &&
+	    (acpi_gbl_trace_flags & ACPI_TRACE_OPCODE)) {
+		ACPI_TRACE_POINT(ACPI_TRACE_AML_OPCODE, FALSE,
+				 op->common.aml, op->common.aml_op_name);
+	}
+}
diff --git a/drivers/acpi/acpica/exutils.c b/drivers/acpi/acpica/exutils.c
index 30c3f464fda5..4d44bc1cb2ca 100644
--- a/drivers/acpi/acpica/exutils.c
+++ b/drivers/acpi/acpica/exutils.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -167,8 +167,8 @@ u8 acpi_ex_truncate_for32bit_table(union acpi_operand_object *obj_desc)
 	if ((acpi_gbl_integer_byte_width == 4) &&
 	    (obj_desc->integer.value > (u64)ACPI_UINT32_MAX)) {
 		/*
-		 * We are executing in a 32-bit ACPI table.
-		 * Truncate the value to 32 bits by zeroing out the upper 32-bit field
+		 * We are executing in a 32-bit ACPI table. Truncate
+		 * the value to 32 bits by zeroing out the upper 32-bit field
 		 */
 		obj_desc->integer.value &= (u64)ACPI_UINT32_MAX;
 		return (TRUE);
@@ -323,7 +323,8 @@ void acpi_ex_eisa_id_to_string(char *out_string, u64 compressed_id)
 
 	if (compressed_id > ACPI_UINT32_MAX) {
 		ACPI_WARNING((AE_INFO,
-			      "Expected EISAID is larger than 32 bits: 0x%8.8X%8.8X, truncating",
+			      "Expected EISAID is larger than 32 bits: "
+			      "0x%8.8X%8.8X, truncating",
 			      ACPI_FORMAT_UINT64(compressed_id)));
 	}
 
diff --git a/drivers/acpi/acpica/hwacpi.c b/drivers/acpi/acpica/hwacpi.c
index e5c5949f9081..3ebbb09030b4 100644
--- a/drivers/acpi/acpica/hwacpi.c
+++ b/drivers/acpi/acpica/hwacpi.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/hwesleep.c b/drivers/acpi/acpica/hwesleep.c
index e5599f610808..3f2fb4b31fdc 100644
--- a/drivers/acpi/acpica/hwesleep.c
+++ b/drivers/acpi/acpica/hwesleep.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -117,8 +117,8 @@ acpi_status acpi_hw_extended_sleep(u8 sleep_state)
 
 	/* Clear wake status (WAK_STS) */
 
-	status =
-	    acpi_write((u64)ACPI_X_WAKE_STATUS, &acpi_gbl_FADT.sleep_status);
+	status = acpi_write((u64)ACPI_X_WAKE_STATUS,
+			    &acpi_gbl_FADT.sleep_status);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
diff --git a/drivers/acpi/acpica/hwgpe.c b/drivers/acpi/acpica/hwgpe.c
index 73cfa5947ff3..1c4f4518611a 100644
--- a/drivers/acpi/acpica/hwgpe.c
+++ b/drivers/acpi/acpica/hwgpe.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -187,9 +187,8 @@ acpi_status acpi_hw_clear_gpe(struct acpi_gpe_event_info * gpe_event_info)
 	 */
 	register_bit = acpi_hw_get_gpe_register_bit(gpe_event_info);
 
-	status = acpi_hw_write(register_bit,
-			       &gpe_register_info->status_address);
-
+	status =
+	    acpi_hw_write(register_bit, &gpe_register_info->status_address);
 	return (status);
 }
 
@@ -297,6 +296,7 @@ acpi_hw_gpe_enable_write(u8 enable_mask,
 	acpi_status status;
 
 	gpe_register_info->enable_mask = enable_mask;
+
 	status = acpi_hw_write(enable_mask, &gpe_register_info->enable_address);
 	return (status);
 }
diff --git a/drivers/acpi/acpica/hwpci.c b/drivers/acpi/acpica/hwpci.c
index f785ea788356..3dd60c96aa07 100644
--- a/drivers/acpi/acpica/hwpci.c
+++ b/drivers/acpi/acpica/hwpci.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/hwregs.c b/drivers/acpi/acpica/hwregs.c
index 3cf77afd142c..5ba0498412fd 100644
--- a/drivers/acpi/acpica/hwregs.c
+++ b/drivers/acpi/acpica/hwregs.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/hwsleep.c b/drivers/acpi/acpica/hwsleep.c
index 7d21cae6d602..d00c9810845b 100644
--- a/drivers/acpi/acpica/hwsleep.c
+++ b/drivers/acpi/acpica/hwsleep.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -80,8 +80,8 @@ acpi_status acpi_hw_legacy_sleep(u8 sleep_state)
 
 	/* Clear wake status */
 
-	status =
-	    acpi_write_bit_register(ACPI_BITREG_WAKE_STATUS, ACPI_CLEAR_STATUS);
+	status = acpi_write_bit_register(ACPI_BITREG_WAKE_STATUS,
+					 ACPI_CLEAR_STATUS);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
diff --git a/drivers/acpi/acpica/hwtimer.c b/drivers/acpi/acpica/hwtimer.c
index 675c709a300b..04cc9406c7d8 100644
--- a/drivers/acpi/acpica/hwtimer.c
+++ b/drivers/acpi/acpica/hwtimer.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/hwvalid.c b/drivers/acpi/acpica/hwvalid.c
index 29033d71417b..ad0a745712a9 100644
--- a/drivers/acpi/acpica/hwvalid.c
+++ b/drivers/acpi/acpica/hwvalid.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/hwxface.c b/drivers/acpi/acpica/hwxface.c
index 5f97468df8ff..a01ddb393a55 100644
--- a/drivers/acpi/acpica/hwxface.c
+++ b/drivers/acpi/acpica/hwxface.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -504,11 +504,20 @@ acpi_get_sleep_type_data(u8 sleep_state, u8 *sleep_type_a, u8 *sleep_type_b)
 	 * Evaluate the \_Sx namespace object containing the register values
 	 * for this state
 	 */
-	info->relative_pathname =
-	    ACPI_CAST_PTR(char, acpi_gbl_sleep_state_names[sleep_state]);
+	info->relative_pathname = ACPI_CAST_PTR(char,
+						acpi_gbl_sleep_state_names
+						[sleep_state]);
+
 	status = acpi_ns_evaluate(info);
 	if (ACPI_FAILURE(status)) {
-		goto cleanup;
+		if (status == AE_NOT_FOUND) {
+
+			/* The _Sx states are optional, ignore NOT_FOUND */
+
+			goto final_cleanup;
+		}
+
+		goto warning_cleanup;
 	}
 
 	/* Must have a return object */
@@ -517,7 +526,7 @@ acpi_get_sleep_type_data(u8 sleep_state, u8 *sleep_type_a, u8 *sleep_type_b)
 		ACPI_ERROR((AE_INFO, "No Sleep State object returned from [%s]",
 			    info->relative_pathname));
 		status = AE_AML_NO_RETURN_VALUE;
-		goto cleanup;
+		goto warning_cleanup;
 	}
 
 	/* Return object must be of type Package */
@@ -526,7 +535,7 @@ acpi_get_sleep_type_data(u8 sleep_state, u8 *sleep_type_a, u8 *sleep_type_b)
 		ACPI_ERROR((AE_INFO,
 			    "Sleep State return object is not a Package"));
 		status = AE_AML_OPERAND_TYPE;
-		goto cleanup1;
+		goto return_value_cleanup;
 	}
 
 	/*
@@ -570,16 +579,17 @@ acpi_get_sleep_type_data(u8 sleep_state, u8 *sleep_type_a, u8 *sleep_type_b)
 		break;
 	}
 
-cleanup1:
+return_value_cleanup:
 	acpi_ut_remove_reference(info->return_object);
 
-cleanup:
+warning_cleanup:
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status,
 				"While evaluating Sleep State [%s]",
 				info->relative_pathname));
 	}
 
+final_cleanup:
 	ACPI_FREE(info);
 	return_ACPI_STATUS(status);
 }
diff --git a/drivers/acpi/acpica/hwxfsleep.c b/drivers/acpi/acpica/hwxfsleep.c
index d62a61612b3f..f76e0eab32b8 100644
--- a/drivers/acpi/acpica/hwxfsleep.c
+++ b/drivers/acpi/acpica/hwxfsleep.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -52,9 +52,9 @@ ACPI_MODULE_NAME("hwxfsleep")
 /* Local prototypes */
 #if (!ACPI_REDUCED_HARDWARE)
 static acpi_status
-acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
-				    acpi_physical_address physical_address,
-				    acpi_physical_address physical_address64);
+acpi_hw_set_firmware_waking_vector(struct acpi_table_facs *facs,
+				   acpi_physical_address physical_address,
+				   acpi_physical_address physical_address64);
 #endif
 
 static acpi_status acpi_hw_sleep_dispatch(u8 sleep_state, u32 function_id);
@@ -79,22 +79,20 @@ static struct acpi_sleep_functions acpi_sleep_dispatch[] = {
 
 /*
  * These functions are removed for the ACPI_REDUCED_HARDWARE case:
- *      acpi_set_firmware_waking_vectors
  *      acpi_set_firmware_waking_vector
- *      acpi_set_firmware_waking_vector64
  *      acpi_enter_sleep_state_s4bios
  */
 
 #if (!ACPI_REDUCED_HARDWARE)
 /*******************************************************************************
  *
- * FUNCTION:    acpi_hw_set_firmware_waking_vectors
+ * FUNCTION:    acpi_hw_set_firmware_waking_vector
  *
  * PARAMETERS:  facs                - Pointer to FACS table
  *              physical_address    - 32-bit physical address of ACPI real mode
- *                                    entry point.
+ *                                    entry point
  *              physical_address64  - 64-bit physical address of ACPI protected
- *                                    mode entry point.
+ *                                    mode entry point
  *
  * RETURN:      Status
  *
@@ -103,11 +101,11 @@ static struct acpi_sleep_functions acpi_sleep_dispatch[] = {
  ******************************************************************************/
 
 static acpi_status
-acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
-				    acpi_physical_address physical_address,
-				    acpi_physical_address physical_address64)
+acpi_hw_set_firmware_waking_vector(struct acpi_table_facs *facs,
+				   acpi_physical_address physical_address,
+				   acpi_physical_address physical_address64)
 {
-	ACPI_FUNCTION_TRACE(acpi_hw_set_firmware_waking_vectors);
+	ACPI_FUNCTION_TRACE(acpi_hw_set_firmware_waking_vector);
 
 
 	/*
@@ -140,12 +138,12 @@ acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_set_firmware_waking_vectors
+ * FUNCTION:    acpi_set_firmware_waking_vector
  *
  * PARAMETERS:  physical_address    - 32-bit physical address of ACPI real mode
- *                                    entry point.
+ *                                    entry point
  *              physical_address64  - 64-bit physical address of ACPI protected
- *                                    mode entry point.
+ *                                    mode entry point
  *
  * RETURN:      Status
  *
@@ -154,79 +152,23 @@ acpi_hw_set_firmware_waking_vectors(struct acpi_table_facs *facs,
  ******************************************************************************/
 
 acpi_status
-acpi_set_firmware_waking_vectors(acpi_physical_address physical_address,
-				 acpi_physical_address physical_address64)
+acpi_set_firmware_waking_vector(acpi_physical_address physical_address,
+				acpi_physical_address physical_address64)
 {
 
-	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vectors);
+	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector);
 
 	if (acpi_gbl_FACS) {
-		(void)acpi_hw_set_firmware_waking_vectors(acpi_gbl_FACS,
-							  physical_address,
-							  physical_address64);
+		(void)acpi_hw_set_firmware_waking_vector(acpi_gbl_FACS,
+							 physical_address,
+							 physical_address64);
 	}
 
 	return_ACPI_STATUS(AE_OK);
 }
 
-ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vectors)
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_set_firmware_waking_vector
- *
- * PARAMETERS:  physical_address    - 32-bit physical address of ACPI real mode
- *                                    entry point.
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Sets the 32-bit firmware_waking_vector field of the FACS
- *
- ******************************************************************************/
-acpi_status acpi_set_firmware_waking_vector(u32 physical_address)
-{
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector);
-
-	status = acpi_set_firmware_waking_vectors((acpi_physical_address)
-						  physical_address, 0);
-
-	return_ACPI_STATUS(status);
-}
-
 ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vector)
 
-#if ACPI_MACHINE_WIDTH == 64
-/*******************************************************************************
- *
- * FUNCTION:    acpi_set_firmware_waking_vector64
- *
- * PARAMETERS:  physical_address    - 64-bit physical address of ACPI protected
- *                                    mode entry point.
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Sets the 64-bit X_firmware_waking_vector field of the FACS, if
- *              it exists in the table. This function is intended for use with
- *              64-bit host operating systems.
- *
- ******************************************************************************/
-acpi_status acpi_set_firmware_waking_vector64(u64 physical_address)
-{
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(acpi_set_firmware_waking_vector64);
-
-	status = acpi_set_firmware_waking_vectors(0,
-						  (acpi_physical_address)
-						  physical_address);
-
-	return_ACPI_STATUS(status);
-}
-
-ACPI_EXPORT_SYMBOL(acpi_set_firmware_waking_vector64)
-#endif
 /*******************************************************************************
  *
  * FUNCTION:    acpi_enter_sleep_state_s4bios
@@ -286,6 +228,7 @@ acpi_status acpi_enter_sleep_state_s4bios(void)
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
+
 	} while (!in_value);
 
 	return_ACPI_STATUS(AE_OK);
diff --git a/drivers/acpi/acpica/nsaccess.c b/drivers/acpi/acpica/nsaccess.c
index c687b9979fb2..697af810e5ad 100644
--- a/drivers/acpi/acpica/nsaccess.c
+++ b/drivers/acpi/acpica/nsaccess.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsalloc.c b/drivers/acpi/acpica/nsalloc.c
index e107f929d9cf..c2cf73fd3918 100644
--- a/drivers/acpi/acpica/nsalloc.c
+++ b/drivers/acpi/acpica/nsalloc.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsarguments.c b/drivers/acpi/acpica/nsarguments.c
index 5d347a71bd0b..f45bff632692 100644
--- a/drivers/acpi/acpica/nsarguments.c
+++ b/drivers/acpi/acpica/nsarguments.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsconvert.c b/drivers/acpi/acpica/nsconvert.c
index da55a1c60da1..878e8fb6a64c 100644
--- a/drivers/acpi/acpica/nsconvert.c
+++ b/drivers/acpi/acpica/nsconvert.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -96,9 +96,9 @@ acpi_ns_convert_to_integer(union acpi_operand_object *original_object,
 		/* Extract each buffer byte to create the integer */
 
 		for (i = 0; i < original_object->buffer.length; i++) {
-			value |=
-			    ((u64)original_object->buffer.
-			     pointer[i] << (i * 8));
+			value |= ((u64)
+				  original_object->buffer.pointer[i] << (i *
+									 8));
 		}
 		break;
 
@@ -153,10 +153,9 @@ acpi_ns_convert_to_string(union acpi_operand_object *original_object,
 				return (AE_NO_MEMORY);
 			}
 		} else {
-			status =
-			    acpi_ex_convert_to_string(original_object,
-						      &new_object,
-						      ACPI_IMPLICIT_CONVERT_HEX);
+			status = acpi_ex_convert_to_string(original_object,
+							   &new_object,
+							   ACPI_IMPLICIT_CONVERT_HEX);
 			if (ACPI_FAILURE(status)) {
 				return (status);
 			}
@@ -244,9 +243,8 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
 
 		/* String-to-Buffer conversion. Simple data copy */
 
-		new_object =
-		    acpi_ut_create_buffer_object(original_object->string.
-						 length);
+		new_object = acpi_ut_create_buffer_object
+		    (original_object->string.length);
 		if (!new_object) {
 			return (AE_NO_MEMORY);
 		}
@@ -308,7 +306,8 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
  *
  * FUNCTION:    acpi_ns_convert_to_unicode
  *
- * PARAMETERS:  original_object     - ASCII String Object to be converted
+ * PARAMETERS:  scope               - Namespace node for the method/object
+ *              original_object     - ASCII String Object to be converted
  *              return_object       - Where the new converted object is returned
  *
  * RETURN:      Status. AE_OK if conversion was successful.
@@ -318,7 +317,8 @@ acpi_ns_convert_to_buffer(union acpi_operand_object *original_object,
  ******************************************************************************/
 
 acpi_status
-acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
+acpi_ns_convert_to_unicode(struct acpi_namespace_node * scope,
+			   union acpi_operand_object *original_object,
 			   union acpi_operand_object **return_object)
 {
 	union acpi_operand_object *new_object;
@@ -372,7 +372,8 @@ acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
  *
  * FUNCTION:    acpi_ns_convert_to_resource
  *
- * PARAMETERS:  original_object     - Object to be converted
+ * PARAMETERS:  scope               - Namespace node for the method/object
+ *              original_object     - Object to be converted
  *              return_object       - Where the new converted object is returned
  *
  * RETURN:      Status. AE_OK if conversion was successful
@@ -383,7 +384,8 @@ acpi_ns_convert_to_unicode(union acpi_operand_object *original_object,
  ******************************************************************************/
 
 acpi_status
-acpi_ns_convert_to_resource(union acpi_operand_object *original_object,
+acpi_ns_convert_to_resource(struct acpi_namespace_node * scope,
+			    union acpi_operand_object *original_object,
 			    union acpi_operand_object **return_object)
 {
 	union acpi_operand_object *new_object;
@@ -444,3 +446,78 @@ acpi_ns_convert_to_resource(union acpi_operand_object *original_object,
 	*return_object = new_object;
 	return (AE_OK);
 }
+
+/*******************************************************************************
+ *
+ * FUNCTION:    acpi_ns_convert_to_reference
+ *
+ * PARAMETERS:  scope               - Namespace node for the method/object
+ *              original_object     - Object to be converted
+ *              return_object       - Where the new converted object is returned
+ *
+ * RETURN:      Status. AE_OK if conversion was successful
+ *
+ * DESCRIPTION: Attempt to convert a Integer object to a object_reference.
+ *              Buffer.
+ *
+ ******************************************************************************/
+
+acpi_status
+acpi_ns_convert_to_reference(struct acpi_namespace_node * scope,
+			     union acpi_operand_object *original_object,
+			     union acpi_operand_object **return_object)
+{
+	union acpi_operand_object *new_object = NULL;
+	acpi_status status;
+	struct acpi_namespace_node *node;
+	union acpi_generic_state scope_info;
+	char *name;
+
+	ACPI_FUNCTION_NAME(ns_convert_to_reference);
+
+	/* Convert path into internal presentation */
+
+	status =
+	    acpi_ns_internalize_name(original_object->string.pointer, &name);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	/* Find the namespace node */
+
+	scope_info.scope.node =
+	    ACPI_CAST_PTR(struct acpi_namespace_node, scope);
+	status =
+	    acpi_ns_lookup(&scope_info, name, ACPI_TYPE_ANY, ACPI_IMODE_EXECUTE,
+			   ACPI_NS_SEARCH_PARENT | ACPI_NS_DONT_OPEN_SCOPE,
+			   NULL, &node);
+	if (ACPI_FAILURE(status)) {
+
+		/* Check if we are resolving a named reference within a package */
+
+		ACPI_ERROR_NAMESPACE(original_object->string.pointer, status);
+		goto error_exit;
+	}
+
+	/* Create and init a new internal ACPI object */
+
+	new_object = acpi_ut_create_internal_object(ACPI_TYPE_LOCAL_REFERENCE);
+	if (!new_object) {
+		status = AE_NO_MEMORY;
+		goto error_exit;
+	}
+	new_object->reference.node = node;
+	new_object->reference.object = node->object;
+	new_object->reference.class = ACPI_REFCLASS_NAME;
+
+	/*
+	 * Increase reference of the object if needed (the object is likely a
+	 * null for device nodes).
+	 */
+	acpi_ut_add_reference(node->object);
+
+error_exit:
+	ACPI_FREE(name);
+	*return_object = new_object;
+	return (AE_OK);
+}
diff --git a/drivers/acpi/acpica/nsdump.c b/drivers/acpi/acpica/nsdump.c
index 37aa5c45ca4b..af236e348294 100644
--- a/drivers/acpi/acpica/nsdump.c
+++ b/drivers/acpi/acpica/nsdump.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -539,11 +539,13 @@ acpi_ns_dump_one_object(acpi_handle obj_handle,
 				acpi_os_printf
 				    ("(Pointer to ACPI Object type %.2X [UNKNOWN])\n",
 				     obj_type);
+
 				bytes_to_dump = 32;
 			} else {
 				acpi_os_printf
 				    ("(Pointer to ACPI Object type %.2X [%s])\n",
 				     obj_type, acpi_ut_get_type_name(obj_type));
+
 				bytes_to_dump =
 				    sizeof(union acpi_operand_object);
 			}
@@ -573,6 +575,7 @@ acpi_ns_dump_one_object(acpi_handle obj_handle,
 			 */
 			bytes_to_dump = obj_desc->string.length;
 			obj_desc = (void *)obj_desc->string.pointer;
+
 			acpi_os_printf("(Buffer/String pointer %p length %X)\n",
 				       obj_desc, bytes_to_dump);
 			ACPI_DUMP_BUFFER(obj_desc, bytes_to_dump);
@@ -717,7 +720,7 @@ acpi_ns_dump_one_object_path(acpi_handle obj_handle,
 		return (AE_OK);
 	}
 
-	pathname = acpi_ns_get_external_pathname(node);
+	pathname = acpi_ns_get_normalized_pathname(node, TRUE);
 
 	path_indent = 1;
 	if (level <= max_level) {
diff --git a/drivers/acpi/acpica/nsdumpdv.c b/drivers/acpi/acpica/nsdumpdv.c
index 7dc367e6fe09..7060a5668989 100644
--- a/drivers/acpi/acpica/nsdumpdv.c
+++ b/drivers/acpi/acpica/nsdumpdv.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nseval.c b/drivers/acpi/acpica/nseval.c
index 7eba578d36f3..65d58bea4320 100644
--- a/drivers/acpi/acpica/nseval.c
+++ b/drivers/acpi/acpica/nseval.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -135,7 +135,7 @@ acpi_status acpi_ns_evaluate(struct acpi_evaluate_info *info)
 
 	/* Get the full pathname to the object, for use in warning messages */
 
-	info->full_pathname = acpi_ns_get_external_pathname(info->node);
+	info->full_pathname = acpi_ns_get_normalized_pathname(info->node, TRUE);
 	if (!info->full_pathname) {
 		return_ACPI_STATUS(AE_NO_MEMORY);
 	}
@@ -418,7 +418,8 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
 	 * Get the parent node. We cheat by using the next_object field
 	 * of the method object descriptor.
 	 */
-	parent_node = ACPI_CAST_PTR(struct acpi_namespace_node,
+	parent_node =
+	    ACPI_CAST_PTR(struct acpi_namespace_node,
 				    method_obj->method.next_object);
 	type = acpi_ns_get_type(parent_node);
 
@@ -444,9 +445,9 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
 	info->prefix_node = parent_node;
 
 	/*
-	 * Get the currently attached parent object. Add a reference, because the
-	 * ref count will be decreased when the method object is installed to
-	 * the parent node.
+	 * Get the currently attached parent object. Add a reference,
+	 * because the ref count will be decreased when the method object
+	 * is installed to the parent node.
 	 */
 	parent_obj = acpi_ns_get_attached_object(parent_node);
 	if (parent_obj) {
@@ -455,8 +456,8 @@ acpi_ns_exec_module_code(union acpi_operand_object *method_obj,
 
 	/* Install the method (module-level code) in the parent node */
 
-	status = acpi_ns_attach_object(parent_node, method_obj,
-				       ACPI_TYPE_METHOD);
+	status =
+	    acpi_ns_attach_object(parent_node, method_obj, ACPI_TYPE_METHOD);
 	if (ACPI_FAILURE(status)) {
 		goto exit;
 	}
diff --git a/drivers/acpi/acpica/nsinit.c b/drivers/acpi/acpica/nsinit.c
index b744a53618eb..bd75d46234a4 100644
--- a/drivers/acpi/acpica/nsinit.c
+++ b/drivers/acpi/acpica/nsinit.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -582,7 +582,8 @@ acpi_ns_init_one_device(acpi_handle obj_handle,
 
 		/* Ignore error and move on to next device */
 
-		char *scope_name = acpi_ns_get_external_pathname(info->node);
+		char *scope_name =
+		    acpi_ns_get_normalized_pathname(device_node, TRUE);
 
 		ACPI_EXCEPTION((AE_INFO, status, "during %s._INI execution",
 				scope_name));
diff --git a/drivers/acpi/acpica/nsload.c b/drivers/acpi/acpica/nsload.c
index 14ab83668207..75cdb8790d49 100644
--- a/drivers/acpi/acpica/nsload.c
+++ b/drivers/acpi/acpica/nsload.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -149,6 +149,23 @@ unlock:
 	ACPI_DEBUG_PRINT((ACPI_DB_INFO,
 			  "**** Completed Table Object Initialization\n"));
 
+	/*
+	 * Execute any module-level code that was detected during the table load
+	 * phase. Although illegal since ACPI 2.0, there are many machines that
+	 * contain this type of code. Each block of detected executable AML code
+	 * outside of any control method is wrapped with a temporary control
+	 * method object and placed on a global list. The methods on this list
+	 * are executed below.
+	 *
+	 * This case executes the module-level code for each table immediately
+	 * after the table has been loaded. This provides compatibility with
+	 * other ACPI implementations. Optionally, the execution can be deferred
+	 * until later, see acpi_initialize_objects.
+	 */
+	if (!acpi_gbl_group_module_level_code) {
+		acpi_ns_exec_module_code_list();
+	}
+
 	return_ACPI_STATUS(status);
 }
 
@@ -321,7 +338,6 @@ acpi_status acpi_ns_unload_namespace(acpi_handle handle)
 	/* This function does the real work */
 
 	status = acpi_ns_delete_subtree(handle);
-
 	return_ACPI_STATUS(status);
 }
 #endif
diff --git a/drivers/acpi/acpica/nsnames.c b/drivers/acpi/acpica/nsnames.c
index 8934b4eddb73..eb6e1b88a51d 100644
--- a/drivers/acpi/acpica/nsnames.c
+++ b/drivers/acpi/acpica/nsnames.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -70,7 +70,6 @@ char *acpi_ns_get_external_pathname(struct acpi_namespace_node *node)
 	ACPI_FUNCTION_TRACE_PTR(ns_get_external_pathname, node);
 
 	name_buffer = acpi_ns_get_normalized_pathname(node, FALSE);
-
 	return_PTR(name_buffer);
 }
 
@@ -93,7 +92,6 @@ acpi_size acpi_ns_get_pathname_length(struct acpi_namespace_node *node)
 	ACPI_FUNCTION_ENTRY();
 
 	size = acpi_ns_build_normalized_path(node, NULL, 0, FALSE);
-
 	return (size);
 }
 
@@ -217,6 +215,7 @@ acpi_ns_build_normalized_path(struct acpi_namespace_node *node,
 			ACPI_PATH_PUT8(full_path, path_size,
 				       AML_DUAL_NAME_PREFIX, length);
 		}
+
 		ACPI_MOVE_32_TO_32(name, &next_node->name);
 		do_no_trailing = no_trailing;
 		for (i = 0; i < 4; i++) {
@@ -228,8 +227,10 @@ acpi_ns_build_normalized_path(struct acpi_namespace_node *node,
 				ACPI_PATH_PUT8(full_path, path_size, c, length);
 			}
 		}
+
 		next_node = next_node->parent;
 	}
+
 	ACPI_PATH_PUT8(full_path, path_size, AML_ROOT_PREFIX, length);
 
 	/* Reverse the path string */
@@ -237,6 +238,7 @@ acpi_ns_build_normalized_path(struct acpi_namespace_node *node,
 	if (length <= path_size) {
 		left = full_path;
 		right = full_path + length - 1;
+
 		while (left < right) {
 			c = *left;
 			*left++ = *right;
diff --git a/drivers/acpi/acpica/nsobject.c b/drivers/acpi/acpica/nsobject.c
index 677bc9330e64..051306f0d0d6 100644
--- a/drivers/acpi/acpica/nsobject.c
+++ b/drivers/acpi/acpica/nsobject.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsparse.c b/drivers/acpi/acpica/nsparse.c
index 3736d43b18b9..f631a47724f0 100644
--- a/drivers/acpi/acpica/nsparse.c
+++ b/drivers/acpi/acpica/nsparse.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -141,8 +141,8 @@ acpi_ns_one_complete_parse(u32 pass_number,
 
 	/* Parse the AML */
 
-	ACPI_DEBUG_PRINT((ACPI_DB_PARSE, "*PARSE* pass %u parse\n",
-			  pass_number));
+	ACPI_DEBUG_PRINT((ACPI_DB_PARSE,
+			  "*PARSE* pass %u parse\n", pass_number));
 	status = acpi_ps_parse_aml(walk_state);
 
 cleanup:
@@ -181,6 +181,7 @@ acpi_ns_parse_table(u32 table_index, struct acpi_namespace_node *start_node)
 	 * 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)) {
diff --git a/drivers/acpi/acpica/nspredef.c b/drivers/acpi/acpica/nspredef.c
index 0c20980bbcf3..6d7844580b2a 100644
--- a/drivers/acpi/acpica/nspredef.c
+++ b/drivers/acpi/acpica/nspredef.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsprepkg.c b/drivers/acpi/acpica/nsprepkg.c
index 9bb251932b45..9047f2808d5b 100644
--- a/drivers/acpi/acpica/nsprepkg.c
+++ b/drivers/acpi/acpica/nsprepkg.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -233,8 +233,9 @@ acpi_ns_check_package(struct acpi_evaluate_info *info,
 
 		/* First element is the (Integer) revision */
 
-		status = acpi_ns_check_object_type(info, elements,
-						   ACPI_RTYPE_INTEGER, 0);
+		status =
+		    acpi_ns_check_object_type(info, elements,
+					      ACPI_RTYPE_INTEGER, 0);
 		if (ACPI_FAILURE(status)) {
 			return (status);
 		}
@@ -252,8 +253,9 @@ acpi_ns_check_package(struct acpi_evaluate_info *info,
 
 		/* First element is the (Integer) count of subpackages to follow */
 
-		status = acpi_ns_check_object_type(info, elements,
-						   ACPI_RTYPE_INTEGER, 0);
+		status =
+		    acpi_ns_check_object_type(info, elements,
+					      ACPI_RTYPE_INTEGER, 0);
 		if (ACPI_FAILURE(status)) {
 			return (status);
 		}
diff --git a/drivers/acpi/acpica/nsrepair.c b/drivers/acpi/acpica/nsrepair.c
index 77d8103d0094..805e36de8707 100644
--- a/drivers/acpi/acpica/nsrepair.c
+++ b/drivers/acpi/acpica/nsrepair.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -116,6 +116,11 @@ static const struct acpi_simple_repair_info acpi_object_repair_info[] = {
 	 ACPI_NOT_PACKAGE_ELEMENT,
 	 acpi_ns_convert_to_resource},
 
+	/* Object reference conversions */
+
+	{"_DEP", ACPI_RTYPE_STRING, ACPI_ALL_PACKAGE_ELEMENTS,
+	 acpi_ns_convert_to_reference},
+
 	/* Unicode conversions */
 
 	{"_MLS", ACPI_RTYPE_STRING, 1,
@@ -172,8 +177,8 @@ acpi_ns_simple_repair(struct acpi_evaluate_info *info,
 					      "Missing expected return value"));
 		}
 
-		status =
-		    predefined->object_converter(return_object, &new_object);
+		status = predefined->object_converter(info->node, return_object,
+						      &new_object);
 		if (ACPI_FAILURE(status)) {
 
 			/* A fatal error occurred during a conversion */
@@ -360,12 +365,15 @@ static const struct acpi_simple_repair_info *acpi_ns_match_simple_repair(struct
 			/* Check if we can actually repair this name/type combination */
 
 			if ((return_btype & this_name->unexpected_btypes) &&
-			    (package_index == this_name->package_index)) {
+			    (this_name->package_index ==
+			     ACPI_ALL_PACKAGE_ELEMENTS
+			     || package_index == this_name->package_index)) {
 				return (this_name);
 			}
 
 			return (NULL);
 		}
+
 		this_name++;
 	}
 
@@ -521,6 +529,7 @@ acpi_ns_remove_null_elements(struct acpi_evaluate_info *info,
 			*dest = *source;
 			dest++;
 		}
+
 		source++;
 	}
 
@@ -572,8 +581,8 @@ acpi_ns_wrap_with_package(struct acpi_evaluate_info *info,
 	ACPI_FUNCTION_NAME(ns_wrap_with_package);
 
 	/*
-	 * Create the new outer package and populate it. The new package will
-	 * have a single element, the lone sub-object.
+	 * Create the new outer package and populate it. The new
+	 * package will have a single element, the lone sub-object.
 	 */
 	pkg_obj_desc = acpi_ut_create_package_object(1);
 	if (!pkg_obj_desc) {
diff --git a/drivers/acpi/acpica/nsrepair2.c b/drivers/acpi/acpica/nsrepair2.c
index 0515a70f42a4..63edbbbf9ae4 100644
--- a/drivers/acpi/acpica/nsrepair2.c
+++ b/drivers/acpi/acpica/nsrepair2.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -225,6 +225,7 @@ static const struct acpi_repair_info *acpi_ns_match_complex_repair(struct
 		if (ACPI_COMPARE_NAME(node->name.ascii, this_name->name)) {
 			return (this_name);
 		}
+
 		this_name++;
 	}
 
@@ -301,7 +302,8 @@ acpi_ns_repair_FDE(struct acpi_evaluate_info *info,
 		/* We can only repair if we have exactly 5 BYTEs */
 
 		if (return_object->buffer.length != ACPI_FDE_BYTE_BUFFER_SIZE) {
-			ACPI_WARN_PREDEFINED((AE_INFO, info->full_pathname,
+			ACPI_WARN_PREDEFINED((AE_INFO,
+					      info->full_pathname,
 					      info->node_flags,
 					      "Incorrect return buffer length %u, expected %u",
 					      return_object->buffer.length,
@@ -321,8 +323,8 @@ acpi_ns_repair_FDE(struct acpi_evaluate_info *info,
 		/* Expand each byte to a DWORD */
 
 		byte_buffer = return_object->buffer.pointer;
-		dword_buffer =
-		    ACPI_CAST_PTR(u32, buffer_object->buffer.pointer);
+		dword_buffer = ACPI_CAST_PTR(u32,
+					     buffer_object->buffer.pointer);
 
 		for (i = 0; i < ACPI_FDE_FIELD_COUNT; i++) {
 			*dword_buffer = (u32) *byte_buffer;
@@ -461,7 +463,8 @@ acpi_ns_repair_CST(struct acpi_evaluate_info *info,
 		removing = FALSE;
 
 		if ((*outer_elements)->package.count == 0) {
-			ACPI_WARN_PREDEFINED((AE_INFO, info->full_pathname,
+			ACPI_WARN_PREDEFINED((AE_INFO,
+					      info->full_pathname,
 					      info->node_flags,
 					      "SubPackage[%u] - removing entry due to zero count",
 					      i));
@@ -471,7 +474,8 @@ acpi_ns_repair_CST(struct acpi_evaluate_info *info,
 
 		obj_desc = (*outer_elements)->package.elements[1];	/* Index1 = Type */
 		if ((u32)obj_desc->integer.value == 0) {
-			ACPI_WARN_PREDEFINED((AE_INFO, info->full_pathname,
+			ACPI_WARN_PREDEFINED((AE_INFO,
+					      info->full_pathname,
 					      info->node_flags,
 					      "SubPackage[%u] - removing entry due to invalid Type(0)",
 					      i));
@@ -538,8 +542,8 @@ acpi_ns_repair_HID(struct acpi_evaluate_info *info,
 	}
 
 	if (return_object->string.length == 0) {
-		ACPI_WARN_PREDEFINED((AE_INFO, info->full_pathname,
-				      info->node_flags,
+		ACPI_WARN_PREDEFINED((AE_INFO,
+				      info->full_pathname, info->node_flags,
 				      "Invalid zero-length _HID or _CID string"));
 
 		/* Return AE_OK anyway, let driver handle it */
@@ -710,8 +714,9 @@ acpi_ns_repair_PSS(struct acpi_evaluate_info *info,
 		elements = (*outer_elements)->package.elements;
 		obj_desc = elements[1];	/* Index1 = power_dissipation */
 
-		if ((u32) obj_desc->integer.value > previous_value) {
-			ACPI_WARN_PREDEFINED((AE_INFO, info->full_pathname,
+		if ((u32)obj_desc->integer.value > previous_value) {
+			ACPI_WARN_PREDEFINED((AE_INFO,
+					      info->full_pathname,
 					      info->node_flags,
 					      "SubPackage[%u,%u] - suspicious power dissipation values",
 					      i - 1, i));
@@ -969,6 +974,7 @@ acpi_ns_remove_element(union acpi_operand_object *obj_desc, u32 index)
 			*dest = *source;
 			dest++;
 		}
+
 		source++;
 	}
 
diff --git a/drivers/acpi/acpica/nssearch.c b/drivers/acpi/acpica/nssearch.c
index d73904013830..61036d210274 100644
--- a/drivers/acpi/acpica/nssearch.c
+++ b/drivers/acpi/acpica/nssearch.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -105,7 +105,7 @@ acpi_ns_search_one_scope(u32 target_name,
 	if (ACPI_LV_NAMES & acpi_dbg_level) {
 		char *scope_name;
 
-		scope_name = acpi_ns_get_external_pathname(parent_node);
+		scope_name = acpi_ns_get_normalized_pathname(parent_node, TRUE);
 		if (scope_name) {
 			ACPI_DEBUG_PRINT((ACPI_DB_NAMES,
 					  "Searching %s (%p) For [%4.4s] (%s)\n",
diff --git a/drivers/acpi/acpica/nsutils.c b/drivers/acpi/acpica/nsutils.c
index de325ae04ce1..c72cc62b92d0 100644
--- a/drivers/acpi/acpica/nsutils.c
+++ b/drivers/acpi/acpica/nsutils.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -173,9 +173,10 @@ void acpi_ns_get_internal_name_length(struct acpi_namestring_info *info)
 	info->fully_qualified = FALSE;
 
 	/*
-	 * For the internal name, the required length is 4 bytes per segment, plus
-	 * 1 each for root_prefix, multi_name_prefix_op, segment count, trailing null
-	 * (which is not really needed, but no there's harm in putting it there)
+	 * For the internal name, the required length is 4 bytes per segment,
+	 * plus 1 each for root_prefix, multi_name_prefix_op, segment count,
+	 * trailing null (which is not really needed, but no there's harm in
+	 * putting it there)
 	 *
 	 * strlen() + 1 covers the first name_seg, which has no path separator
 	 */
@@ -699,6 +700,7 @@ acpi_ns_get_node(struct acpi_namespace_node *prefix_node,
 		if (!prefix_node) {
 			*return_node = acpi_gbl_root_node;
 		}
+
 		return_ACPI_STATUS(AE_OK);
 	}
 
diff --git a/drivers/acpi/acpica/nswalk.c b/drivers/acpi/acpica/nswalk.c
index c68609a2bc1b..ebd731fe8e45 100644
--- a/drivers/acpi/acpica/nswalk.c
+++ b/drivers/acpi/acpica/nswalk.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/nsxfeval.c b/drivers/acpi/acpica/nsxfeval.c
index 6ee1e52b903d..a7deeaa8eddc 100644
--- a/drivers/acpi/acpica/nsxfeval.c
+++ b/drivers/acpi/acpica/nsxfeval.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -750,8 +750,8 @@ acpi_ns_get_device_callback(acpi_handle obj_handle,
 
 	/* We have a valid device, invoke the user function */
 
-	status = info->user_function(obj_handle, nesting_level, info->context,
-				     return_value);
+	status = info->user_function(obj_handle, nesting_level,
+				     info->context, return_value);
 	return (status);
 }
 
diff --git a/drivers/acpi/acpica/nsxfname.c b/drivers/acpi/acpica/nsxfname.c
index 4b4d2f43d406..285b82044e7b 100644
--- a/drivers/acpi/acpica/nsxfname.c
+++ b/drivers/acpi/acpica/nsxfname.c
@@ -6,7 +6,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -159,7 +159,7 @@ acpi_get_name(acpi_handle handle, u32 name_type, struct acpi_buffer * buffer)
 {
 	acpi_status status;
 	struct acpi_namespace_node *node;
-	char *node_name;
+	const char *node_name;
 
 	/* Parameter validation */
 
@@ -238,7 +238,6 @@ static char *acpi_ns_copy_device_id(struct acpi_pnp_device_id *dest,
 				    struct acpi_pnp_device_id *source,
 				    char *string_area)
 {
-
 	/* Create the destination PNP_DEVICE_ID */
 
 	dest->string = string_area;
@@ -263,11 +262,18 @@ static char *acpi_ns_copy_device_id(struct acpi_pnp_device_id *dest,
  *              namespace node and possibly by running several standard
  *              control methods (Such as in the case of a device.)
  *
- * For Device and Processor objects, run the Device _HID, _UID, _CID, _SUB,
- * _CLS, _STA, _ADR, _sx_w, and _sx_d methods.
+ * For Device and Processor objects, run the Device _HID, _UID, _CID, _STA,
+ * _CLS, _ADR, _sx_w, and _sx_d methods.
  *
  * Note: Allocates the return buffer, must be freed by the caller.
  *
+ * Note: This interface is intended to be used during the initial device
+ * discovery namespace traversal. Therefore, no complex methods can be
+ * executed, especially those that access operation regions. Therefore, do
+ * not add any additional methods that could cause problems in this area.
+ * this was the fate of the _SUB method which was found to cause such
+ * problems and was removed (11/2015).
+ *
  ******************************************************************************/
 
 acpi_status
@@ -279,7 +285,6 @@ acpi_get_object_info(acpi_handle handle,
 	struct acpi_pnp_device_id_list *cid_list = NULL;
 	struct acpi_pnp_device_id *hid = NULL;
 	struct acpi_pnp_device_id *uid = NULL;
-	struct acpi_pnp_device_id *sub = NULL;
 	struct acpi_pnp_device_id *cls = NULL;
 	char *next_id_string;
 	acpi_object_type type;
@@ -325,7 +330,7 @@ acpi_get_object_info(acpi_handle handle,
 	if ((type == ACPI_TYPE_DEVICE) || (type == ACPI_TYPE_PROCESSOR)) {
 		/*
 		 * Get extra info for ACPI Device/Processor objects only:
-		 * Run the Device _HID, _UID, _SUB, _CID, and _CLS methods.
+		 * Run the Device _HID, _UID, _CLS, and _CID methods.
 		 *
 		 * Note: none of these methods are required, so they may or may
 		 * not be present for this device. The Info->Valid bitfield is used
@@ -348,14 +353,6 @@ acpi_get_object_info(acpi_handle handle,
 			valid |= ACPI_VALID_UID;
 		}
 
-		/* Execute the Device._SUB method */
-
-		status = acpi_ut_execute_SUB(node, &sub);
-		if (ACPI_SUCCESS(status)) {
-			info_size += sub->length;
-			valid |= ACPI_VALID_SUB;
-		}
-
 		/* Execute the Device._CID method */
 
 		status = acpi_ut_execute_CID(node, &cid_list);
@@ -456,9 +453,8 @@ acpi_get_object_info(acpi_handle handle,
 	}
 
 	/*
-	 * Copy the HID, UID, SUB, and CIDs to the return buffer.
-	 * The variable-length strings are copied to the reserved area
-	 * at the end of the buffer.
+	 * Copy the HID, UID, and CIDs to the return buffer. The variable-length
+	 * strings are copied to the reserved area at the end of the buffer.
 	 *
 	 * For HID and CID, check if the ID is a PCI Root Bridge.
 	 */
@@ -476,11 +472,6 @@ acpi_get_object_info(acpi_handle handle,
 							uid, next_id_string);
 	}
 
-	if (sub) {
-		next_id_string = acpi_ns_copy_device_id(&info->subsystem_id,
-							sub, next_id_string);
-	}
-
 	if (cid_list) {
 		info->compatible_id_list.count = cid_list->count;
 		info->compatible_id_list.list_size = cid_list->list_size;
@@ -522,9 +513,6 @@ cleanup:
 	if (uid) {
 		ACPI_FREE(uid);
 	}
-	if (sub) {
-		ACPI_FREE(sub);
-	}
 	if (cid_list) {
 		ACPI_FREE(cid_list);
 	}
@@ -591,6 +579,7 @@ acpi_status acpi_install_method(u8 *buffer)
 	parser_state.aml += acpi_ps_get_opcode_size(opcode);
 	parser_state.pkg_end = acpi_ps_get_next_package_end(&parser_state);
 	path = acpi_ps_get_next_namestring(&parser_state);
+
 	method_flags = *parser_state.aml++;
 	aml_start = parser_state.aml;
 	aml_length = ACPI_PTR_DIFF(parser_state.pkg_end, aml_start);
diff --git a/drivers/acpi/acpica/nsxfobj.c b/drivers/acpi/acpica/nsxfobj.c
index 793383501f81..c312cd490450 100644
--- a/drivers/acpi/acpica/nsxfobj.c
+++ b/drivers/acpi/acpica/nsxfobj.c
@@ -6,7 +6,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -74,10 +74,8 @@ acpi_status acpi_get_type(acpi_handle handle, acpi_object_type * ret_type)
 		return (AE_BAD_PARAMETER);
 	}
 
-	/*
-	 * Special case for the predefined Root Node
-	 * (return type ANY)
-	 */
+	/* Special case for the predefined Root Node (return type ANY) */
+
 	if (handle == ACPI_ROOT_OBJECT) {
 		*ret_type = ACPI_TYPE_ANY;
 		return (AE_OK);
diff --git a/drivers/acpi/acpica/psargs.c b/drivers/acpi/acpica/psargs.c
index 29d8b7b01dca..d48cbed342c1 100644
--- a/drivers/acpi/acpica/psargs.c
+++ b/drivers/acpi/acpica/psargs.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -733,6 +733,7 @@ acpi_ps_get_next_arg(struct acpi_walk_state *walk_state,
 		if (!arg) {
 			return_ACPI_STATUS(AE_NO_MEMORY);
 		}
+
 		acpi_ps_get_next_simple_arg(parser_state, arg_type, arg);
 		break;
 
@@ -798,6 +799,7 @@ acpi_ps_get_next_arg(struct acpi_walk_state *walk_state,
 	case ARGP_TARGET:
 	case ARGP_SUPERNAME:
 	case ARGP_SIMPLENAME:
+	case ARGP_NAME_OR_REF:
 
 		subop = acpi_ps_peek_opcode(parser_state);
 		if (subop == 0 ||
@@ -820,11 +822,11 @@ acpi_ps_get_next_arg(struct acpi_walk_state *walk_state,
 				status =
 				    acpi_ps_get_next_namepath(walk_state,
 							      parser_state, arg,
-							      1);
+							      ACPI_POSSIBLE_METHOD_CALL);
 
 				/*
-				 * If the super_name arg of Unload is a method call,
-				 * we have restored the AML pointer, just free this Arg
+				 * If the super_name argument is a method call, we have
+				 * already restored the AML pointer, just free this Arg
 				 */
 				if (arg->common.aml_opcode ==
 				    AML_INT_METHODCALL_OP) {
@@ -835,7 +837,7 @@ acpi_ps_get_next_arg(struct acpi_walk_state *walk_state,
 				status =
 				    acpi_ps_get_next_namepath(walk_state,
 							      parser_state, arg,
-							      0);
+							      ACPI_NOT_METHOD_CALL);
 			}
 		} else {
 			/* Single complex argument, nothing returned */
diff --git a/drivers/acpi/acpica/psloop.c b/drivers/acpi/acpica/psloop.c
index 03ac8c9a67ab..6a9f5059f682 100644
--- a/drivers/acpi/acpica/psloop.c
+++ b/drivers/acpi/acpica/psloop.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -109,10 +109,10 @@ acpi_ps_get_arguments(struct acpi_walk_state *walk_state,
 
 	case AML_INT_NAMEPATH_OP:	/* AML_NAMESTRING_ARG */
 
-		status =
-		    acpi_ps_get_next_namepath(walk_state,
-					      &(walk_state->parser_state), op,
-					      1);
+		status = acpi_ps_get_next_namepath(walk_state,
+						   &(walk_state->parser_state),
+						   op,
+						   ACPI_POSSIBLE_METHOD_CALL);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
@@ -124,8 +124,8 @@ acpi_ps_get_arguments(struct acpi_walk_state *walk_state,
 		/*
 		 * Op is not a constant or string, append each argument to the Op
 		 */
-		while (GET_CURRENT_ARG_TYPE(walk_state->arg_types)
-		       && !walk_state->arg_count) {
+		while (GET_CURRENT_ARG_TYPE(walk_state->arg_types) &&
+		       !walk_state->arg_count) {
 			walk_state->aml = walk_state->parser_state.aml;
 
 			status =
diff --git a/drivers/acpi/acpica/psobject.c b/drivers/acpi/acpica/psobject.c
index e54bc2aa7a88..db0e90342e82 100644
--- a/drivers/acpi/acpica/psobject.c
+++ b/drivers/acpi/acpica/psobject.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/psopcode.c b/drivers/acpi/acpica/psopcode.c
index ed90fddf2487..8e0c97dca01f 100644
--- a/drivers/acpi/acpica/psopcode.c
+++ b/drivers/acpi/acpica/psopcode.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -185,458 +185,458 @@ const struct acpi_opcode_info acpi_gbl_aml_op_info[AML_NUM_OPCODES] = {
 /* Index           Name                 Parser Args               Interpreter Args                ObjectType                    Class                      Type                  Flags */
 
 /* 00 */ ACPI_OP("Zero", ARGP_ZERO_OP, ARGI_ZERO_OP, ACPI_TYPE_INTEGER,
-		 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
+			 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
 /* 01 */ ACPI_OP("One", ARGP_ONE_OP, ARGI_ONE_OP, ACPI_TYPE_INTEGER,
-		 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
+			 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
 /* 02 */ ACPI_OP("Alias", ARGP_ALIAS_OP, ARGI_ALIAS_OP,
-		 ACPI_TYPE_LOCAL_ALIAS, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_SIMPLE,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_LOCAL_ALIAS, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_SIMPLE,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 03 */ ACPI_OP("Name", ARGP_NAME_OP, ARGI_NAME_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_COMPLEX,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_COMPLEX,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 04 */ ACPI_OP("ByteConst", ARGP_BYTE_OP, ARGI_BYTE_OP,
-		 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_CONSTANT),
+			 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_CONSTANT),
 /* 05 */ ACPI_OP("WordConst", ARGP_WORD_OP, ARGI_WORD_OP,
-		 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_CONSTANT),
+			 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_CONSTANT),
 /* 06 */ ACPI_OP("DwordConst", ARGP_DWORD_OP, ARGI_DWORD_OP,
-		 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_CONSTANT),
+			 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_CONSTANT),
 /* 07 */ ACPI_OP("String", ARGP_STRING_OP, ARGI_STRING_OP,
-		 ACPI_TYPE_STRING, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_CONSTANT),
+			 ACPI_TYPE_STRING, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_CONSTANT),
 /* 08 */ ACPI_OP("Scope", ARGP_SCOPE_OP, ARGI_SCOPE_OP,
-		 ACPI_TYPE_LOCAL_SCOPE, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_NO_OBJ,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_LOCAL_SCOPE, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_NO_OBJ,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 09 */ ACPI_OP("Buffer", ARGP_BUFFER_OP, ARGI_BUFFER_OP,
-		 ACPI_TYPE_BUFFER, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_OBJECT,
-		 AML_HAS_ARGS | AML_DEFER | AML_CONSTANT),
+			 ACPI_TYPE_BUFFER, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_OBJECT,
+			 AML_HAS_ARGS | AML_DEFER | AML_CONSTANT),
 /* 0A */ ACPI_OP("Package", ARGP_PACKAGE_OP, ARGI_PACKAGE_OP,
-		 ACPI_TYPE_PACKAGE, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_OBJECT,
-		 AML_HAS_ARGS | AML_DEFER | AML_CONSTANT),
+			 ACPI_TYPE_PACKAGE, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_OBJECT,
+			 AML_HAS_ARGS | AML_DEFER | AML_CONSTANT),
 /* 0B */ ACPI_OP("Method", ARGP_METHOD_OP, ARGI_METHOD_OP,
-		 ACPI_TYPE_METHOD, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_COMPLEX,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED | AML_DEFER),
+			 ACPI_TYPE_METHOD, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_COMPLEX,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED | AML_DEFER),
 /* 0C */ ACPI_OP("Local0", ARGP_LOCAL0, ARGI_LOCAL0,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 0D */ ACPI_OP("Local1", ARGP_LOCAL1, ARGI_LOCAL1,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 0E */ ACPI_OP("Local2", ARGP_LOCAL2, ARGI_LOCAL2,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 0F */ ACPI_OP("Local3", ARGP_LOCAL3, ARGI_LOCAL3,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 10 */ ACPI_OP("Local4", ARGP_LOCAL4, ARGI_LOCAL4,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 11 */ ACPI_OP("Local5", ARGP_LOCAL5, ARGI_LOCAL5,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 12 */ ACPI_OP("Local6", ARGP_LOCAL6, ARGI_LOCAL6,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 13 */ ACPI_OP("Local7", ARGP_LOCAL7, ARGI_LOCAL7,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LOCAL_VARIABLE, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LOCAL_VARIABLE, 0),
 /* 14 */ ACPI_OP("Arg0", ARGP_ARG0, ARGI_ARG0,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 15 */ ACPI_OP("Arg1", ARGP_ARG1, ARGI_ARG1,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 16 */ ACPI_OP("Arg2", ARGP_ARG2, ARGI_ARG2,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 17 */ ACPI_OP("Arg3", ARGP_ARG3, ARGI_ARG3,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 18 */ ACPI_OP("Arg4", ARGP_ARG4, ARGI_ARG4,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 19 */ ACPI_OP("Arg5", ARGP_ARG5, ARGI_ARG5,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 1A */ ACPI_OP("Arg6", ARGP_ARG6, ARGI_ARG6,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_METHOD_ARGUMENT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_METHOD_ARGUMENT, 0),
 /* 1B */ ACPI_OP("Store", ARGP_STORE_OP, ARGI_STORE_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R),
 /* 1C */ ACPI_OP("RefOf", ARGP_REF_OF_OP, ARGI_REF_OF_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R),
 /* 1D */ ACPI_OP("Add", ARGP_ADD_OP, ARGI_ADD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 1E */ ACPI_OP("Concatenate", ARGP_CONCAT_OP, ARGI_CONCAT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
 /* 1F */ ACPI_OP("Subtract", ARGP_SUBTRACT_OP, ARGI_SUBTRACT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 20 */ ACPI_OP("Increment", ARGP_INCREMENT_OP, ARGI_INCREMENT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
 /* 21 */ ACPI_OP("Decrement", ARGP_DECREMENT_OP, ARGI_DECREMENT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
 /* 22 */ ACPI_OP("Multiply", ARGP_MULTIPLY_OP, ARGI_MULTIPLY_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 23 */ ACPI_OP("Divide", ARGP_DIVIDE_OP, ARGI_DIVIDE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_2T_1R,
-		 AML_FLAGS_EXEC_2A_2T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_2T_1R,
+			 AML_FLAGS_EXEC_2A_2T_1R | AML_CONSTANT),
 /* 24 */ ACPI_OP("ShiftLeft", ARGP_SHIFT_LEFT_OP, ARGI_SHIFT_LEFT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 25 */ ACPI_OP("ShiftRight", ARGP_SHIFT_RIGHT_OP, ARGI_SHIFT_RIGHT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 26 */ ACPI_OP("And", ARGP_BIT_AND_OP, ARGI_BIT_AND_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 27 */ ACPI_OP("NAnd", ARGP_BIT_NAND_OP, ARGI_BIT_NAND_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 28 */ ACPI_OP("Or", ARGP_BIT_OR_OP, ARGI_BIT_OR_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 29 */ ACPI_OP("NOr", ARGP_BIT_NOR_OP, ARGI_BIT_NOR_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 2A */ ACPI_OP("XOr", ARGP_BIT_XOR_OP, ARGI_BIT_XOR_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_MATH | AML_CONSTANT),
 /* 2B */ ACPI_OP("Not", ARGP_BIT_NOT_OP, ARGI_BIT_NOT_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 2C */ ACPI_OP("FindSetLeftBit", ARGP_FIND_SET_LEFT_BIT_OP,
-		 ARGI_FIND_SET_LEFT_BIT_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ARGI_FIND_SET_LEFT_BIT_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 2D */ ACPI_OP("FindSetRightBit", ARGP_FIND_SET_RIGHT_BIT_OP,
-		 ARGI_FIND_SET_RIGHT_BIT_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ARGI_FIND_SET_RIGHT_BIT_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 2E */ ACPI_OP("DerefOf", ARGP_DEREF_OF_OP, ARGI_DEREF_OF_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_1R, AML_FLAGS_EXEC_1A_0T_1R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_1R, AML_FLAGS_EXEC_1A_0T_1R),
 /* 2F */ ACPI_OP("Notify", ARGP_NOTIFY_OP, ARGI_NOTIFY_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_0T_0R, AML_FLAGS_EXEC_2A_0T_0R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_0T_0R, AML_FLAGS_EXEC_2A_0T_0R),
 /* 30 */ ACPI_OP("SizeOf", ARGP_SIZE_OF_OP, ARGI_SIZE_OF_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R | AML_NO_OPERAND_RESOLVE),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R | AML_NO_OPERAND_RESOLVE),
 /* 31 */ ACPI_OP("Index", ARGP_INDEX_OP, ARGI_INDEX_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R),
 /* 32 */ ACPI_OP("Match", ARGP_MATCH_OP, ARGI_MATCH_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_6A_0T_1R,
-		 AML_FLAGS_EXEC_6A_0T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_6A_0T_1R,
+			 AML_FLAGS_EXEC_6A_0T_1R | AML_CONSTANT),
 /* 33 */ ACPI_OP("CreateDWordField", ARGP_CREATE_DWORD_FIELD_OP,
-		 ARGI_CREATE_DWORD_FIELD_OP,
-		 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_CREATE),
+			 ARGI_CREATE_DWORD_FIELD_OP,
+			 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_CREATE),
 /* 34 */ ACPI_OP("CreateWordField", ARGP_CREATE_WORD_FIELD_OP,
-		 ARGI_CREATE_WORD_FIELD_OP,
-		 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_CREATE),
+			 ARGI_CREATE_WORD_FIELD_OP,
+			 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_CREATE),
 /* 35 */ ACPI_OP("CreateByteField", ARGP_CREATE_BYTE_FIELD_OP,
-		 ARGI_CREATE_BYTE_FIELD_OP,
-		 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_CREATE),
+			 ARGI_CREATE_BYTE_FIELD_OP,
+			 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_CREATE),
 /* 36 */ ACPI_OP("CreateBitField", ARGP_CREATE_BIT_FIELD_OP,
-		 ARGI_CREATE_BIT_FIELD_OP,
-		 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_CREATE),
-/* 37 */ ACPI_OP("ObjectType", ARGP_TYPE_OP, ARGI_TYPE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R | AML_NO_OPERAND_RESOLVE),
+			 ARGI_CREATE_BIT_FIELD_OP,
+			 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_CREATE),
+/* 37 */ ACPI_OP("ObjectType", ARGP_OBJECT_TYPE_OP, ARGI_OBJECT_TYPE_OP,
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R | AML_NO_OPERAND_RESOLVE),
 /* 38 */ ACPI_OP("LAnd", ARGP_LAND_OP, ARGI_LAND_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
 			 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL_NUMERIC |
 			 AML_CONSTANT),
 /* 39 */ ACPI_OP("LOr", ARGP_LOR_OP, ARGI_LOR_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
-		 	 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL_NUMERIC |
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
+			 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL_NUMERIC |
 			 AML_CONSTANT),
 /* 3A */ ACPI_OP("LNot", ARGP_LNOT_OP, ARGI_LNOT_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_1R,
-		 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_1R,
+			 AML_FLAGS_EXEC_1A_0T_1R | AML_CONSTANT),
 /* 3B */ ACPI_OP("LEqual", ARGP_LEQUAL_OP, ARGI_LEQUAL_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_0T_1R,
-		 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_0T_1R,
+			 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
 /* 3C */ ACPI_OP("LGreater", ARGP_LGREATER_OP, ARGI_LGREATER_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_0T_1R,
-		 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_0T_1R,
+			 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
 /* 3D */ ACPI_OP("LLess", ARGP_LLESS_OP, ARGI_LLESS_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
-		 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
+			 AML_FLAGS_EXEC_2A_0T_1R | AML_LOGICAL | AML_CONSTANT),
 /* 3E */ ACPI_OP("If", ARGP_IF_OP, ARGI_IF_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
+			 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
 /* 3F */ ACPI_OP("Else", ARGP_ELSE_OP, ARGI_ELSE_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
+			 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
 /* 40 */ ACPI_OP("While", ARGP_WHILE_OP, ARGI_WHILE_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
+			 AML_CLASS_CONTROL, AML_TYPE_CONTROL, AML_HAS_ARGS),
 /* 41 */ ACPI_OP("Noop", ARGP_NOOP_OP, ARGI_NOOP_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
+			 AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
 /* 42 */ ACPI_OP("Return", ARGP_RETURN_OP, ARGI_RETURN_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_CONTROL,
-		 AML_TYPE_CONTROL, AML_HAS_ARGS),
+			 ACPI_TYPE_ANY, AML_CLASS_CONTROL,
+			 AML_TYPE_CONTROL, AML_HAS_ARGS),
 /* 43 */ ACPI_OP("Break", ARGP_BREAK_OP, ARGI_BREAK_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
+			 AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
 /* 44 */ ACPI_OP("BreakPoint", ARGP_BREAK_POINT_OP, ARGI_BREAK_POINT_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
+			 ACPI_TYPE_ANY, AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
 /* 45 */ ACPI_OP("Ones", ARGP_ONES_OP, ARGI_ONES_OP, ACPI_TYPE_INTEGER,
-		 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
+			 AML_CLASS_ARGUMENT, AML_TYPE_CONSTANT, AML_CONSTANT),
 
 /* Prefixed opcodes (Two-byte opcodes with a prefix op) */
 
 /* 46 */ ACPI_OP("Mutex", ARGP_MUTEX_OP, ARGI_MUTEX_OP, ACPI_TYPE_MUTEX,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_SIMPLE,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_SIMPLE,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 47 */ ACPI_OP("Event", ARGP_EVENT_OP, ARGI_EVENT_OP, ACPI_TYPE_EVENT,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_SIMPLE,
-		 AML_NSOBJECT | AML_NSOPCODE | AML_NSNODE | AML_NAMED),
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_SIMPLE,
+			 AML_NSOBJECT | AML_NSOPCODE | AML_NSNODE | AML_NAMED),
 /* 48 */ ACPI_OP("CondRefOf", ARGP_COND_REF_OF_OP, ARGI_COND_REF_OF_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R, AML_FLAGS_EXEC_1A_1T_1R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R, AML_FLAGS_EXEC_1A_1T_1R),
 /* 49 */ ACPI_OP("CreateField", ARGP_CREATE_FIELD_OP,
-		 ARGI_CREATE_FIELD_OP, ACPI_TYPE_BUFFER_FIELD,
-		 AML_CLASS_CREATE, AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_FIELD | AML_CREATE),
+			 ARGI_CREATE_FIELD_OP, ACPI_TYPE_BUFFER_FIELD,
+			 AML_CLASS_CREATE, AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_FIELD | AML_CREATE),
 /* 4A */ ACPI_OP("Load", ARGP_LOAD_OP, ARGI_LOAD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_0R,
-		 AML_FLAGS_EXEC_1A_1T_0R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_0R,
+			 AML_FLAGS_EXEC_1A_1T_0R),
 /* 4B */ ACPI_OP("Stall", ARGP_STALL_OP, ARGI_STALL_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
-		 AML_FLAGS_EXEC_1A_0T_0R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
+			 AML_FLAGS_EXEC_1A_0T_0R),
 /* 4C */ ACPI_OP("Sleep", ARGP_SLEEP_OP, ARGI_SLEEP_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
-		 AML_FLAGS_EXEC_1A_0T_0R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
+			 AML_FLAGS_EXEC_1A_0T_0R),
 /* 4D */ ACPI_OP("Acquire", ARGP_ACQUIRE_OP, ARGI_ACQUIRE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_0T_1R, AML_FLAGS_EXEC_2A_0T_1R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_0T_1R, AML_FLAGS_EXEC_2A_0T_1R),
 /* 4E */ ACPI_OP("Signal", ARGP_SIGNAL_OP, ARGI_SIGNAL_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
 /* 4F */ ACPI_OP("Wait", ARGP_WAIT_OP, ARGI_WAIT_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
-		 AML_FLAGS_EXEC_2A_0T_1R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_0T_1R,
+			 AML_FLAGS_EXEC_2A_0T_1R),
 /* 50 */ ACPI_OP("Reset", ARGP_RESET_OP, ARGI_RESET_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
-		 AML_FLAGS_EXEC_1A_0T_0R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_0T_0R,
+			 AML_FLAGS_EXEC_1A_0T_0R),
 /* 51 */ ACPI_OP("Release", ARGP_RELEASE_OP, ARGI_RELEASE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
 /* 52 */ ACPI_OP("FromBCD", ARGP_FROM_BCD_OP, ARGI_FROM_BCD_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 53 */ ACPI_OP("ToBCD", ARGP_TO_BCD_OP, ARGI_TO_BCD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 54 */ ACPI_OP("Unload", ARGP_UNLOAD_OP, ARGI_UNLOAD_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_0T_0R, AML_FLAGS_EXEC_1A_0T_0R),
 /* 55 */ ACPI_OP("Revision", ARGP_REVISION_OP, ARGI_REVISION_OP,
-		 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
-		 AML_TYPE_CONSTANT, 0),
+			 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
+			 AML_TYPE_CONSTANT, 0),
 /* 56 */ ACPI_OP("Debug", ARGP_DEBUG_OP, ARGI_DEBUG_OP,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_CONSTANT, 0),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_CONSTANT, 0),
 /* 57 */ ACPI_OP("Fatal", ARGP_FATAL_OP, ARGI_FATAL_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_3A_0T_0R,
-		 AML_FLAGS_EXEC_3A_0T_0R),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_3A_0T_0R,
+			 AML_FLAGS_EXEC_3A_0T_0R),
 /* 58 */ ACPI_OP("OperationRegion", ARGP_REGION_OP, ARGI_REGION_OP,
-		 ACPI_TYPE_REGION, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_COMPLEX,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED | AML_DEFER),
+			 ACPI_TYPE_REGION, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_COMPLEX,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED | AML_DEFER),
 /* 59 */ ACPI_OP("Field", ARGP_FIELD_OP, ARGI_FIELD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_FIELD,
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_FIELD,
 			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
 			 AML_FIELD),
 /* 5A */ ACPI_OP("Device", ARGP_DEVICE_OP, ARGI_DEVICE_OP,
-		 ACPI_TYPE_DEVICE, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_NO_OBJ,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_DEVICE, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_NO_OBJ,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 5B */ ACPI_OP("Processor", ARGP_PROCESSOR_OP, ARGI_PROCESSOR_OP,
-		 ACPI_TYPE_PROCESSOR, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_SIMPLE,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_PROCESSOR, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_SIMPLE,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 5C */ ACPI_OP("PowerResource", ARGP_POWER_RES_OP, ARGI_POWER_RES_OP,
-		 ACPI_TYPE_POWER, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_SIMPLE,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_POWER, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_SIMPLE,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 5D */ ACPI_OP("ThermalZone", ARGP_THERMAL_ZONE_OP,
-		 ARGI_THERMAL_ZONE_OP, ACPI_TYPE_THERMAL,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_NO_OBJ,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED),
+			 ARGI_THERMAL_ZONE_OP, ACPI_TYPE_THERMAL,
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_NO_OBJ,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED),
 /* 5E */ ACPI_OP("IndexField", ARGP_INDEX_FIELD_OP, ARGI_INDEX_FIELD_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_FIELD,
-		 	 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 ACPI_TYPE_ANY, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
 			 AML_FIELD),
 /* 5F */ ACPI_OP("BankField", ARGP_BANK_FIELD_OP, ARGI_BANK_FIELD_OP,
-		 	 ACPI_TYPE_LOCAL_BANK_FIELD,
+			 ACPI_TYPE_LOCAL_BANK_FIELD,
 			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_FIELD,
-		 	 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
 			 AML_FIELD | AML_DEFER),
 
 /* Internal opcodes that map to invalid AML opcodes */
 
 /* 60 */ ACPI_OP("LNotEqual", ARGP_LNOTEQUAL_OP, ARGI_LNOTEQUAL_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
-		 AML_TYPE_BOGUS, AML_HAS_ARGS | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
+			 AML_TYPE_BOGUS, AML_HAS_ARGS | AML_CONSTANT),
 /* 61 */ ACPI_OP("LLessEqual", ARGP_LLESSEQUAL_OP, ARGI_LLESSEQUAL_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
-		 AML_TYPE_BOGUS, AML_HAS_ARGS | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
+			 AML_TYPE_BOGUS, AML_HAS_ARGS | AML_CONSTANT),
 /* 62 */ ACPI_OP("LGreaterEqual", ARGP_LGREATEREQUAL_OP,
-		 ARGI_LGREATEREQUAL_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_INTERNAL, AML_TYPE_BOGUS,
-		 AML_HAS_ARGS | AML_CONSTANT),
+			 ARGI_LGREATEREQUAL_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_INTERNAL, AML_TYPE_BOGUS,
+			 AML_HAS_ARGS | AML_CONSTANT),
 /* 63 */ ACPI_OP("-NamePath-", ARGP_NAMEPATH_OP, ARGI_NAMEPATH_OP,
-		 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_NSOBJECT | AML_NSNODE),
+			 ACPI_TYPE_LOCAL_REFERENCE, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_NSOBJECT | AML_NSNODE),
 /* 64 */ ACPI_OP("-MethodCall-", ARGP_METHODCALL_OP, ARGI_METHODCALL_OP,
-		 ACPI_TYPE_METHOD, AML_CLASS_METHOD_CALL,
-		 AML_TYPE_METHOD_CALL,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE),
+			 ACPI_TYPE_METHOD, AML_CLASS_METHOD_CALL,
+			 AML_TYPE_METHOD_CALL,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE),
 /* 65 */ ACPI_OP("-ByteList-", ARGP_BYTELIST_OP, ARGI_BYTELIST_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, 0),
+			 ACPI_TYPE_ANY, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, 0),
 /* 66 */ ACPI_OP("-ReservedField-", ARGP_RESERVEDFIELD_OP,
-		 ARGI_RESERVEDFIELD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
+			 ARGI_RESERVEDFIELD_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
 /* 67 */ ACPI_OP("-NamedField-", ARGP_NAMEDFIELD_OP, ARGI_NAMEDFIELD_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
-		 AML_TYPE_BOGUS,
-		 AML_NSOBJECT | AML_NSOPCODE | AML_NSNODE | AML_NAMED),
+			 ACPI_TYPE_ANY, AML_CLASS_INTERNAL,
+			 AML_TYPE_BOGUS,
+			 AML_NSOBJECT | AML_NSOPCODE | AML_NSNODE | AML_NAMED),
 /* 68 */ ACPI_OP("-AccessField-", ARGP_ACCESSFIELD_OP,
-		 ARGI_ACCESSFIELD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
+			 ARGI_ACCESSFIELD_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
 /* 69 */ ACPI_OP("-StaticString", ARGP_STATICSTRING_OP,
-		 ARGI_STATICSTRING_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
+			 ARGI_STATICSTRING_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_INTERNAL, AML_TYPE_BOGUS, 0),
 /* 6A */ ACPI_OP("-Return Value-", ARG_NONE, ARG_NONE, ACPI_TYPE_ANY,
-		 AML_CLASS_RETURN_VALUE, AML_TYPE_RETURN,
-		 AML_HAS_ARGS | AML_HAS_RETVAL),
+			 AML_CLASS_RETURN_VALUE, AML_TYPE_RETURN,
+			 AML_HAS_ARGS | AML_HAS_RETVAL),
 /* 6B */ ACPI_OP("-UNKNOWN_OP-", ARG_NONE, ARG_NONE, ACPI_TYPE_INVALID,
-		 AML_CLASS_UNKNOWN, AML_TYPE_BOGUS, AML_HAS_ARGS),
+			 AML_CLASS_UNKNOWN, AML_TYPE_BOGUS, AML_HAS_ARGS),
 /* 6C */ ACPI_OP("-ASCII_ONLY-", ARG_NONE, ARG_NONE, ACPI_TYPE_ANY,
-		 AML_CLASS_ASCII, AML_TYPE_BOGUS, AML_HAS_ARGS),
+			 AML_CLASS_ASCII, AML_TYPE_BOGUS, AML_HAS_ARGS),
 /* 6D */ ACPI_OP("-PREFIX_ONLY-", ARG_NONE, ARG_NONE, ACPI_TYPE_ANY,
-		 AML_CLASS_PREFIX, AML_TYPE_BOGUS, AML_HAS_ARGS),
+			 AML_CLASS_PREFIX, AML_TYPE_BOGUS, AML_HAS_ARGS),
 
 /* ACPI 2.0 opcodes */
 
 /* 6E */ ACPI_OP("QwordConst", ARGP_QWORD_OP, ARGI_QWORD_OP,
-		 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
-		 AML_TYPE_LITERAL, AML_CONSTANT),
+			 ACPI_TYPE_INTEGER, AML_CLASS_ARGUMENT,
+			 AML_TYPE_LITERAL, AML_CONSTANT),
 	/* 6F */ ACPI_OP("Package", /* Var */ ARGP_VAR_PACKAGE_OP,
 			 ARGI_VAR_PACKAGE_OP, ACPI_TYPE_PACKAGE,
 			 AML_CLASS_CREATE, AML_TYPE_CREATE_OBJECT,
 			 AML_HAS_ARGS | AML_DEFER),
 /* 70 */ ACPI_OP("ConcatenateResTemplate", ARGP_CONCAT_RES_OP,
-		 ARGI_CONCAT_RES_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
+			 ARGI_CONCAT_RES_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
 /* 71 */ ACPI_OP("Mod", ARGP_MOD_OP, ARGI_MOD_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
 /* 72 */ ACPI_OP("CreateQWordField", ARGP_CREATE_QWORD_FIELD_OP,
-		 ARGI_CREATE_QWORD_FIELD_OP,
-		 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
-		 AML_TYPE_CREATE_FIELD,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
-		 AML_DEFER | AML_CREATE),
+			 ARGI_CREATE_QWORD_FIELD_OP,
+			 ACPI_TYPE_BUFFER_FIELD, AML_CLASS_CREATE,
+			 AML_TYPE_CREATE_FIELD,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSNODE |
+			 AML_DEFER | AML_CREATE),
 /* 73 */ ACPI_OP("ToBuffer", ARGP_TO_BUFFER_OP, ARGI_TO_BUFFER_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 74 */ ACPI_OP("ToDecimalString", ARGP_TO_DEC_STR_OP,
-		 ARGI_TO_DEC_STR_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ARGI_TO_DEC_STR_OP, ACPI_TYPE_ANY,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 75 */ ACPI_OP("ToHexString", ARGP_TO_HEX_STR_OP, ARGI_TO_HEX_STR_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 76 */ ACPI_OP("ToInteger", ARGP_TO_INTEGER_OP, ARGI_TO_INTEGER_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R,
-		 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R,
+			 AML_FLAGS_EXEC_1A_1T_1R | AML_CONSTANT),
 /* 77 */ ACPI_OP("ToString", ARGP_TO_STRING_OP, ARGI_TO_STRING_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_2A_1T_1R,
-		 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_2A_1T_1R,
+			 AML_FLAGS_EXEC_2A_1T_1R | AML_CONSTANT),
 /* 78 */ ACPI_OP("CopyObject", ARGP_COPY_OP, ARGI_COPY_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_1A_1T_1R, AML_FLAGS_EXEC_1A_1T_1R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_1A_1T_1R, AML_FLAGS_EXEC_1A_1T_1R),
 /* 79 */ ACPI_OP("Mid", ARGP_MID_OP, ARGI_MID_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_3A_1T_1R,
-		 AML_FLAGS_EXEC_3A_1T_1R | AML_CONSTANT),
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_3A_1T_1R,
+			 AML_FLAGS_EXEC_3A_1T_1R | AML_CONSTANT),
 /* 7A */ ACPI_OP("Continue", ARGP_CONTINUE_OP, ARGI_CONTINUE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
+			 ACPI_TYPE_ANY, AML_CLASS_CONTROL, AML_TYPE_CONTROL, 0),
 /* 7B */ ACPI_OP("LoadTable", ARGP_LOAD_TABLE_OP, ARGI_LOAD_TABLE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
-		 AML_TYPE_EXEC_6A_0T_1R, AML_FLAGS_EXEC_6A_0T_1R),
+			 ACPI_TYPE_ANY, AML_CLASS_EXECUTE,
+			 AML_TYPE_EXEC_6A_0T_1R, AML_FLAGS_EXEC_6A_0T_1R),
 /* 7C */ ACPI_OP("DataTableRegion", ARGP_DATA_REGION_OP,
-		 ARGI_DATA_REGION_OP, ACPI_TYPE_REGION,
-		 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_COMPLEX,
-		 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
-		 AML_NSNODE | AML_NAMED | AML_DEFER),
+			 ARGI_DATA_REGION_OP, ACPI_TYPE_REGION,
+			 AML_CLASS_NAMED_OBJECT, AML_TYPE_NAMED_COMPLEX,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 AML_NSNODE | AML_NAMED | AML_DEFER),
 /* 7D */ ACPI_OP("[EvalSubTree]", ARGP_SCOPE_OP, ARGI_SCOPE_OP,
-		 ACPI_TYPE_ANY, AML_CLASS_NAMED_OBJECT,
-		 AML_TYPE_NAMED_NO_OBJ,
-		 	 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
+			 ACPI_TYPE_ANY, AML_CLASS_NAMED_OBJECT,
+			 AML_TYPE_NAMED_NO_OBJ,
+			 AML_HAS_ARGS | AML_NSOBJECT | AML_NSOPCODE |
 			 AML_NSNODE),
 
 /* ACPI 3.0 opcodes */
 
 /* 7E */ ACPI_OP("Timer", ARGP_TIMER_OP, ARGI_TIMER_OP, ACPI_TYPE_ANY,
-		 AML_CLASS_EXECUTE, AML_TYPE_EXEC_0A_0T_1R,
+			 AML_CLASS_EXECUTE, AML_TYPE_EXEC_0A_0T_1R,
 			 AML_FLAGS_EXEC_0A_0T_1R),
 
 /* ACPI 5.0 opcodes */
diff --git a/drivers/acpi/acpica/psopinfo.c b/drivers/acpi/acpica/psopinfo.c
index 58310907fa7b..cfd17a4f2e91 100644
--- a/drivers/acpi/acpica/psopinfo.c
+++ b/drivers/acpi/acpica/psopinfo.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/psparse.c b/drivers/acpi/acpica/psparse.c
index 98001d7f6f80..8038ed2aca05 100644
--- a/drivers/acpi/acpica/psparse.c
+++ b/drivers/acpi/acpica/psparse.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -526,8 +526,8 @@ acpi_status acpi_ps_parse_aml(struct acpi_walk_state *walk_state)
 			}
 
 			/*
-			 * If the transfer to the new method method call worked, a new walk
-			 * state was created -- get it
+			 * If the transfer to the new method method call worked
+			 *, a new walk state was created -- get it
 			 */
 			walk_state = acpi_ds_get_current_walk_state(thread);
 			continue;
@@ -544,8 +544,8 @@ acpi_status acpi_ps_parse_aml(struct acpi_walk_state *walk_state)
 			/* Check for possible multi-thread reentrancy problem */
 
 			if ((status == AE_ALREADY_EXISTS) &&
-			    (!(walk_state->method_desc->method.
-			       info_flags & ACPI_METHOD_SERIALIZED))) {
+			    (!(walk_state->method_desc->method.info_flags &
+			       ACPI_METHOD_SERIALIZED))) {
 				/*
 				 * Method is not serialized and tried to create an object
 				 * twice. The probable cause is that the method cannot
diff --git a/drivers/acpi/acpica/psscope.c b/drivers/acpi/acpica/psscope.c
index 9d669cc6cb62..560c3684ef43 100644
--- a/drivers/acpi/acpica/psscope.c
+++ b/drivers/acpi/acpica/psscope.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/pstree.c b/drivers/acpi/acpica/pstree.c
index cf2f2faf4f92..0288cdbda88e 100644
--- a/drivers/acpi/acpica/pstree.c
+++ b/drivers/acpi/acpica/pstree.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/psutils.c b/drivers/acpi/acpica/psutils.c
index 71d2877cd2ce..b28b0da171b6 100644
--- a/drivers/acpi/acpica/psutils.c
+++ b/drivers/acpi/acpica/psutils.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -175,8 +175,8 @@ void acpi_ps_free_op(union acpi_parse_object *op)
 	ACPI_FUNCTION_NAME(ps_free_op);
 
 	if (op->common.aml_opcode == AML_INT_RETURN_VALUE_OP) {
-		ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS, "Free retval op: %p\n",
-				  op));
+		ACPI_DEBUG_PRINT((ACPI_DB_ALLOCATIONS,
+				  "Free retval op: %p\n", op));
 	}
 
 	if (op->common.flags & ACPI_PARSEOP_GENERIC) {
diff --git a/drivers/acpi/acpica/pswalk.c b/drivers/acpi/acpica/pswalk.c
index ba5f69171288..04f98c0a7684 100644
--- a/drivers/acpi/acpica/pswalk.c
+++ b/drivers/acpi/acpica/pswalk.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -99,6 +99,7 @@ void acpi_ps_delete_parse_tree(union acpi_parse_object *subtree_root)
 		if (op == subtree_root) {
 			return_VOID;
 		}
+
 		if (next) {
 			op = next;
 		} else {
diff --git a/drivers/acpi/acpica/psxface.c b/drivers/acpi/acpica/psxface.c
index 4254805dd319..04b37fcca684 100644
--- a/drivers/acpi/acpica/psxface.c
+++ b/drivers/acpi/acpica/psxface.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsaddr.c b/drivers/acpi/acpica/rsaddr.c
index 66d406e8fe36..492d5b011f33 100644
--- a/drivers/acpi/acpica/rsaddr.c
+++ b/drivers/acpi/acpica/rsaddr.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -312,8 +312,8 @@ acpi_rs_get_address_common(struct acpi_resource *resource,
 
 	/* Validate the Resource Type */
 
-	if ((aml->address.resource_type > 2)
-	    && (aml->address.resource_type < 0xC0)) {
+	if ((aml->address.resource_type > 2) &&
+	    (aml->address.resource_type < 0xC0)) {
 		return (FALSE);
 	}
 
diff --git a/drivers/acpi/acpica/rscalc.c b/drivers/acpi/acpica/rscalc.c
index cb739a694931..2b1209d73e44 100644
--- a/drivers/acpi/acpica/rscalc.c
+++ b/drivers/acpi/acpica/rscalc.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -143,16 +143,17 @@ acpi_rs_stream_option_length(u32 resource_length,
 	ACPI_FUNCTION_ENTRY();
 
 	/*
-	 * The resource_source_index and resource_source are optional elements of some
-	 * Large-type resource descriptors.
+	 * The resource_source_index and resource_source are optional elements of
+	 * some Large-type resource descriptors.
 	 */
 
 	/*
-	 * If the length of the actual resource descriptor is greater than the ACPI
-	 * spec-defined minimum length, it means that a resource_source_index exists
-	 * and is followed by a (required) null terminated string. The string length
-	 * (including the null terminator) is the resource length minus the minimum
-	 * length, minus one byte for the resource_source_index itself.
+	 * If the length of the actual resource descriptor is greater than the
+	 * ACPI spec-defined minimum length, it means that a resource_source_index
+	 * exists and is followed by a (required) null terminated string. The
+	 * string length (including the null terminator) is the resource length
+	 * minus the minimum length, minus one byte for the resource_source_index
+	 * itself.
 	 */
 	if (resource_length > minimum_aml_resource_length) {
 
@@ -277,11 +278,11 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 			 * 16-Bit Address Resource:
 			 * Add the size of the optional resource_source info
 			 */
-			total_size = (acpi_rs_length)
-			    (total_size +
-			     acpi_rs_struct_option_length(&resource->data.
-							  address16.
-							  resource_source));
+			total_size = (acpi_rs_length) (total_size +
+						       acpi_rs_struct_option_length
+						       (&resource->data.
+							address16.
+							resource_source));
 			break;
 
 		case ACPI_RESOURCE_TYPE_ADDRESS32:
@@ -289,11 +290,11 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 			 * 32-Bit Address Resource:
 			 * Add the size of the optional resource_source info
 			 */
-			total_size = (acpi_rs_length)
-			    (total_size +
-			     acpi_rs_struct_option_length(&resource->data.
-							  address32.
-							  resource_source));
+			total_size = (acpi_rs_length) (total_size +
+						       acpi_rs_struct_option_length
+						       (&resource->data.
+							address32.
+							resource_source));
 			break;
 
 		case ACPI_RESOURCE_TYPE_ADDRESS64:
@@ -301,11 +302,11 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 			 * 64-Bit Address Resource:
 			 * Add the size of the optional resource_source info
 			 */
-			total_size = (acpi_rs_length)
-			    (total_size +
-			     acpi_rs_struct_option_length(&resource->data.
-							  address64.
-							  resource_source));
+			total_size = (acpi_rs_length) (total_size +
+						       acpi_rs_struct_option_length
+						       (&resource->data.
+							address64.
+							resource_source));
 			break;
 
 		case ACPI_RESOURCE_TYPE_EXTENDED_IRQ:
@@ -314,26 +315,28 @@ acpi_rs_get_aml_length(struct acpi_resource *resource,
 			 * Add the size of each additional optional interrupt beyond the
 			 * required 1 (4 bytes for each u32 interrupt number)
 			 */
-			total_size = (acpi_rs_length)
-			    (total_size +
-			     ((resource->data.extended_irq.interrupt_count -
-			       1) * 4) +
-			     /* Add the size of the optional resource_source info */
-			     acpi_rs_struct_option_length(&resource->data.
-							  extended_irq.
-							  resource_source));
+			total_size = (acpi_rs_length) (total_size +
+						       ((resource->data.
+							 extended_irq.
+							 interrupt_count -
+							 1) * 4) +
+						       /* Add the size of the optional resource_source info */
+						       acpi_rs_struct_option_length
+						       (&resource->data.
+							extended_irq.
+							resource_source));
 			break;
 
 		case ACPI_RESOURCE_TYPE_GPIO:
 
-			total_size =
-			    (acpi_rs_length) (total_size +
-					      (resource->data.gpio.
-					       pin_table_length * 2) +
-					      resource->data.gpio.
-					      resource_source.string_length +
-					      resource->data.gpio.
-					      vendor_length);
+			total_size = (acpi_rs_length) (total_size +
+						       (resource->data.gpio.
+							pin_table_length * 2) +
+						       resource->data.gpio.
+						       resource_source.
+						       string_length +
+						       resource->data.gpio.
+						       vendor_length);
 
 			break;
 
@@ -566,8 +569,8 @@ acpi_rs_get_list_length(u8 * aml_buffer,
 			    acpi_gbl_resource_struct_sizes[resource_index] +
 			    extra_struct_bytes;
 		}
-		buffer_size = (u32)ACPI_ROUND_UP_TO_NATIVE_WORD(buffer_size);
 
+		buffer_size = (u32)ACPI_ROUND_UP_TO_NATIVE_WORD(buffer_size);
 		*size_needed += buffer_size;
 
 		ACPI_DEBUG_PRINT((ACPI_DB_RESOURCES,
diff --git a/drivers/acpi/acpica/rscreate.c b/drivers/acpi/acpica/rscreate.c
index a5344428f3ae..12978891e842 100644
--- a/drivers/acpi/acpica/rscreate.c
+++ b/drivers/acpi/acpica/rscreate.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -81,8 +81,9 @@ acpi_buffer_to_resource(u8 *aml_buffer,
 
 	/* Get the required length for the converted resource */
 
-	status = acpi_rs_get_list_length(aml_buffer, aml_buffer_length,
-					 &list_size_needed);
+	status =
+	    acpi_rs_get_list_length(aml_buffer, aml_buffer_length,
+				    &list_size_needed);
 	if (status == AE_AML_NO_RESOURCE_END_TAG) {
 		status = AE_OK;
 	}
@@ -232,8 +233,9 @@ acpi_rs_create_pci_routing_table(union acpi_operand_object *package_object,
 
 	/* Get the required buffer length */
 
-	status = acpi_rs_get_pci_routing_table_length(package_object,
-						      &buffer_size_needed);
+	status =
+	    acpi_rs_get_pci_routing_table_length(package_object,
+						 &buffer_size_needed);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -270,9 +272,9 @@ acpi_rs_create_pci_routing_table(union acpi_operand_object *package_object,
 		user_prt = ACPI_CAST_PTR(struct acpi_pci_routing_table, buffer);
 
 		/*
-		 * Fill in the Length field with the information we have at this point.
-		 * The minus four is to subtract the size of the u8 Source[4] member
-		 * because it is added below.
+		 * Fill in the Length field with the information we have at this
+		 * point. The minus four is to subtract the size of the u8
+		 * Source[4] member because it is added below.
 		 */
 		user_prt->length = (sizeof(struct acpi_pci_routing_table) - 4);
 
@@ -345,11 +347,7 @@ acpi_rs_create_pci_routing_table(union acpi_operand_object *package_object,
 					   (u8 *) output_buffer->pointer);
 				path_buffer.pointer = user_prt->source;
 
-				status =
-				    acpi_ns_handle_to_pathname((acpi_handle)
-							       node,
-							       &path_buffer,
-							       FALSE);
+				status = acpi_ns_handle_to_pathname((acpi_handle) node, &path_buffer, FALSE);
 
 				/* +1 to include null terminator */
 
@@ -371,8 +369,8 @@ acpi_rs_create_pci_routing_table(union acpi_operand_object *package_object,
 
 			case ACPI_TYPE_INTEGER:
 				/*
-				 * If this is a number, then the Source Name is NULL, since the
-				 * entire buffer was zeroed out, we can leave this alone.
+				 * If this is a number, then the Source Name is NULL, since
+				 * the entire buffer was zeroed out, we can leave this alone.
 				 *
 				 * Add to the Length field the length of the u32 NULL
 				 */
@@ -451,9 +449,9 @@ acpi_rs_create_aml_resources(struct acpi_buffer *resource_list,
 
 	/* Get the buffer size needed for the AML byte stream */
 
-	status = acpi_rs_get_aml_length(resource_list->pointer,
-					resource_list->length,
-					&aml_size_needed);
+	status =
+	    acpi_rs_get_aml_length(resource_list->pointer,
+				   resource_list->length, &aml_size_needed);
 
 	ACPI_DEBUG_PRINT((ACPI_DB_INFO, "AmlSizeNeeded=%X, %s\n",
 			  (u32)aml_size_needed, acpi_format_exception(status)));
diff --git a/drivers/acpi/acpica/rsdump.c b/drivers/acpi/acpica/rsdump.c
index 2a09288e7c57..23a17c86d5a9 100644
--- a/drivers/acpi/acpica/rsdump.c
+++ b/drivers/acpi/acpica/rsdump.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -483,6 +483,7 @@ static void acpi_rs_dump_address_common(union acpi_resource_data *resource)
 
 static void acpi_rs_out_string(char *title, char *value)
 {
+
 	acpi_os_printf("%27s : %s", title, value);
 	if (!*value) {
 		acpi_os_printf("[NULL NAMESTRING]");
@@ -497,21 +498,25 @@ static void acpi_rs_out_integer8(char *title, u8 value)
 
 static void acpi_rs_out_integer16(char *title, u16 value)
 {
+
 	acpi_os_printf("%27s : %4.4X\n", title, value);
 }
 
 static void acpi_rs_out_integer32(char *title, u32 value)
 {
+
 	acpi_os_printf("%27s : %8.8X\n", title, value);
 }
 
 static void acpi_rs_out_integer64(char *title, u64 value)
 {
+
 	acpi_os_printf("%27s : %8.8X%8.8X\n", title, ACPI_FORMAT_UINT64(value));
 }
 
 static void acpi_rs_out_title(char *title)
 {
+
 	acpi_os_printf("%27s : ", title);
 }
 
@@ -544,6 +549,7 @@ static void acpi_rs_dump_short_byte_list(u8 length, u8 * data)
 	for (i = 0; i < length; i++) {
 		acpi_os_printf("%X ", data[i]);
 	}
+
 	acpi_os_printf("\n");
 }
 
diff --git a/drivers/acpi/acpica/rsdumpinfo.c b/drivers/acpi/acpica/rsdumpinfo.c
index b29d9ec63d1b..5c3491387f9f 100644
--- a/drivers/acpi/acpica/rsdumpinfo.c
+++ b/drivers/acpi/acpica/rsdumpinfo.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsinfo.c b/drivers/acpi/acpica/rsinfo.c
index edecfc675979..8e067cb73973 100644
--- a/drivers/acpi/acpica/rsinfo.c
+++ b/drivers/acpi/acpica/rsinfo.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsio.c b/drivers/acpi/acpica/rsio.c
index 5adba018bab0..07dfbed10d55 100644
--- a/drivers/acpi/acpica/rsio.c
+++ b/drivers/acpi/acpica/rsio.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsirq.c b/drivers/acpi/acpica/rsirq.c
index 07cfa70a475b..bc8f34590d95 100644
--- a/drivers/acpi/acpica/rsirq.c
+++ b/drivers/acpi/acpica/rsirq.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rslist.c b/drivers/acpi/acpica/rslist.c
index 50d5be2ee062..8c42dd734559 100644
--- a/drivers/acpi/acpica/rslist.c
+++ b/drivers/acpi/acpica/rslist.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -89,6 +89,7 @@ acpi_rs_convert_aml_to_resources(u8 * aml,
 	/* Get the appropriate conversion info table */
 
 	aml_resource = ACPI_CAST_PTR(union aml_resource, aml);
+
 	if (acpi_ut_get_resource_type(aml) == ACPI_RESOURCE_NAME_SERIAL_BUS) {
 		if (aml_resource->common_serial_bus.type >
 		    AML_RESOURCE_MAX_SERIALBUSTYPE) {
@@ -225,10 +226,10 @@ acpi_rs_convert_resources_to_aml(struct acpi_resource *resource,
 
 		/* Perform final sanity check on the new AML resource descriptor */
 
-		status = acpi_ut_validate_resource(NULL,
-						   ACPI_CAST_PTR(union
-								 aml_resource,
-								 aml), NULL);
+		status =
+		    acpi_ut_validate_resource(NULL,
+					      ACPI_CAST_PTR(union aml_resource,
+							    aml), NULL);
 		if (ACPI_FAILURE(status)) {
 			return_ACPI_STATUS(status);
 		}
diff --git a/drivers/acpi/acpica/rsmemory.c b/drivers/acpi/acpica/rsmemory.c
index c6b80862030e..88b53ef9105d 100644
--- a/drivers/acpi/acpica/rsmemory.c
+++ b/drivers/acpi/acpica/rsmemory.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsmisc.c b/drivers/acpi/acpica/rsmisc.c
index ac37852e0821..ce3d0b77ec89 100644
--- a/drivers/acpi/acpica/rsmisc.c
+++ b/drivers/acpi/acpica/rsmisc.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -189,8 +189,8 @@ acpi_rs_convert_aml_to_resource(struct acpi_resource *resource,
 			item_count = ACPI_GET8(source);
 			ACPI_SET8(destination, item_count);
 
-			resource->length = resource->length +
-			    (info->value * item_count);
+			resource->length =
+			    resource->length + (info->value * item_count);
 			break;
 
 		case ACPI_RSC_COUNT_GPIO_RES:
@@ -445,8 +445,8 @@ exit:
 
 		/* Round the resource struct length up to the next boundary (32 or 64) */
 
-		resource->length =
-		    (u32) ACPI_ROUND_UP_TO_NATIVE_WORD(resource->length);
+		resource->length = (u32)
+		    ACPI_ROUND_UP_TO_NATIVE_WORD(resource->length);
 	}
 	return_ACPI_STATUS(AE_OK);
 }
@@ -550,9 +550,8 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 			item_count = ACPI_GET8(source);
 			ACPI_SET8(destination, item_count);
 
-			aml_length =
-			    (u16) (aml_length +
-				   (info->value * (item_count - 1)));
+			aml_length = (u16)
+			    (aml_length + (info->value * (item_count - 1)));
 			break;
 
 		case ACPI_RSC_COUNT16:
@@ -723,11 +722,10 @@ acpi_rs_convert_resource_to_aml(struct acpi_resource *resource,
 			/*
 			 * 16-bit encoded bitmask (IRQ macro)
 			 */
-			temp16 = acpi_rs_encode_bitmask(source,
-							*ACPI_ADD_PTR(u8,
-								      resource,
-								      info->
-								      value));
+			temp16 =
+			    acpi_rs_encode_bitmask(source,
+						   *ACPI_ADD_PTR(u8, resource,
+								 info->value));
 			ACPI_MOVE_16_TO_16(destination, &temp16);
 			break;
 
diff --git a/drivers/acpi/acpica/rsserial.c b/drivers/acpi/acpica/rsserial.c
index 4c8c6fe6ea74..8a01296ac7cf 100644
--- a/drivers/acpi/acpica/rsserial.c
+++ b/drivers/acpi/acpica/rsserial.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/rsutils.c b/drivers/acpi/acpica/rsutils.c
index 9486992edbb8..cf06e49cd91c 100644
--- a/drivers/acpi/acpica/rsutils.c
+++ b/drivers/acpi/acpica/rsutils.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -221,14 +221,13 @@ acpi_rs_set_resource_length(acpi_rsdesc_size total_length,
 		ACPI_MOVE_16_TO_16(&aml->large_header.resource_length,
 				   &resource_length);
 	} else {
-		/* Small descriptor -- bits 2:0 of byte 0 contain the length */
-
+		/*
+		 * Small descriptor -- bits 2:0 of byte 0 contain the length
+		 * Clear any existing length, preserving descriptor type bits
+		 */
 		aml->small_header.descriptor_type = (u8)
-
-		    /* Clear any existing length, preserving descriptor type bits */
-		    ((aml->small_header.
-		      descriptor_type & ~ACPI_RESOURCE_NAME_SMALL_LENGTH_MASK)
-
+		    ((aml->small_header.descriptor_type &
+		      ~ACPI_RESOURCE_NAME_SMALL_LENGTH_MASK)
 		     | resource_length);
 	}
 }
@@ -333,8 +332,8 @@ acpi_rs_get_resource_source(acpi_rs_length resource_length,
 	aml_resource_source = ACPI_ADD_PTR(u8, aml, minimum_length);
 
 	/*
-	 * resource_source is present if the length of the descriptor is longer than
-	 * the minimum length.
+	 * resource_source is present if the length of the descriptor is longer
+	 * than the minimum length.
 	 *
 	 * Note: Some resource descriptors will have an additional null, so
 	 * we add 1 to the minimum length.
@@ -366,6 +365,7 @@ acpi_rs_get_resource_source(acpi_rs_length resource_length,
 		total_length =
 		    (u32)strlen(ACPI_CAST_PTR(char, &aml_resource_source[1])) +
 		    1;
+
 		total_length = (u32)ACPI_ROUND_UP_TO_NATIVE_WORD(total_length);
 
 		memset(resource_source->string_ptr, 0, total_length);
@@ -438,8 +438,8 @@ acpi_rs_set_resource_source(union aml_resource * aml,
 		 * Add the length of the string (+ 1 for null terminator) to the
 		 * final descriptor length
 		 */
-		descriptor_length +=
-		    ((acpi_rsdesc_size) resource_source->string_length + 1);
+		descriptor_length += ((acpi_rsdesc_size)
+				      resource_source->string_length + 1);
 	}
 
 	/* Return the new total length of the AML descriptor */
@@ -478,8 +478,9 @@ acpi_rs_get_prt_method_data(struct acpi_namespace_node * node,
 
 	/* Execute the method, no parameters */
 
-	status = acpi_ut_evaluate_object(node, METHOD_NAME__PRT,
-					 ACPI_BTYPE_PACKAGE, &obj_desc);
+	status =
+	    acpi_ut_evaluate_object(node, METHOD_NAME__PRT, ACPI_BTYPE_PACKAGE,
+				    &obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -527,8 +528,9 @@ acpi_rs_get_crs_method_data(struct acpi_namespace_node *node,
 
 	/* Execute the method, no parameters */
 
-	status = acpi_ut_evaluate_object(node, METHOD_NAME__CRS,
-					 ACPI_BTYPE_BUFFER, &obj_desc);
+	status =
+	    acpi_ut_evaluate_object(node, METHOD_NAME__CRS, ACPI_BTYPE_BUFFER,
+				    &obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -577,8 +579,9 @@ acpi_rs_get_prs_method_data(struct acpi_namespace_node *node,
 
 	/* Execute the method, no parameters */
 
-	status = acpi_ut_evaluate_object(node, METHOD_NAME__PRS,
-					 ACPI_BTYPE_BUFFER, &obj_desc);
+	status =
+	    acpi_ut_evaluate_object(node, METHOD_NAME__PRS, ACPI_BTYPE_BUFFER,
+				    &obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -627,8 +630,9 @@ acpi_rs_get_aei_method_data(struct acpi_namespace_node *node,
 
 	/* Execute the method, no parameters */
 
-	status = acpi_ut_evaluate_object(node, METHOD_NAME__AEI,
-					 ACPI_BTYPE_BUFFER, &obj_desc);
+	status =
+	    acpi_ut_evaluate_object(node, METHOD_NAME__AEI, ACPI_BTYPE_BUFFER,
+				    &obj_desc);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
diff --git a/drivers/acpi/acpica/rsxface.c b/drivers/acpi/acpica/rsxface.c
index 1e8cd5723326..900933be9909 100644
--- a/drivers/acpi/acpica/rsxface.c
+++ b/drivers/acpi/acpica/rsxface.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -53,7 +53,7 @@ ACPI_MODULE_NAME("rsxface")
 
 /* Local macros for 16,32-bit to 64-bit conversion */
 #define ACPI_COPY_FIELD(out, in, field)  ((out)->field = (in)->field)
-#define ACPI_COPY_ADDRESS(out, in)                      \
+#define ACPI_COPY_ADDRESS(out, in)                       \
 	ACPI_COPY_FIELD(out, in, resource_type);             \
 	ACPI_COPY_FIELD(out, in, producer_consumer);         \
 	ACPI_COPY_FIELD(out, in, decode);                    \
diff --git a/drivers/acpi/acpica/tbdata.c b/drivers/acpi/acpica/tbdata.c
index 5c9d5abf1588..7da79ce74080 100644
--- a/drivers/acpi/acpica/tbdata.c
+++ b/drivers/acpi/acpica/tbdata.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -407,6 +407,7 @@ acpi_tb_verify_temp_table(struct acpi_table_desc * table_desc, char *signature)
 					table_desc->signature.ascii : "????",
 					ACPI_FORMAT_UINT64(table_desc->
 							   address)));
+
 			goto invalidate_and_exit;
 		}
 	}
diff --git a/drivers/acpi/acpica/tbfadt.c b/drivers/acpi/acpica/tbfadt.c
index a6454f4a6fb3..a79e4f30b530 100644
--- a/drivers/acpi/acpica/tbfadt.c
+++ b/drivers/acpi/acpica/tbfadt.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/tbfind.c b/drivers/acpi/acpica/tbfind.c
index 405529d49a1a..f2d08034630e 100644
--- a/drivers/acpi/acpica/tbfind.c
+++ b/drivers/acpi/acpica/tbfind.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/tbinstal.c b/drivers/acpi/acpica/tbinstal.c
index 6319b42420c6..b661a1e013fb 100644
--- a/drivers/acpi/acpica/tbinstal.c
+++ b/drivers/acpi/acpica/tbinstal.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -337,8 +337,8 @@ acpi_tb_install_standard_table(acpi_physical_address address,
 			 * need to be unregistered when they are unloaded, and slots in the
 			 * root table list should be reused when empty.
 			 */
-			if (acpi_gbl_root_table_list.tables[i].
-			    flags & ACPI_TABLE_IS_LOADED) {
+			if (acpi_gbl_root_table_list.tables[i].flags &
+			    ACPI_TABLE_IS_LOADED) {
 
 				/* Table is still loaded, this is an error */
 
diff --git a/drivers/acpi/acpica/tbprint.c b/drivers/acpi/acpica/tbprint.c
index 709d5112fc16..fd4146d4ff49 100644
--- a/drivers/acpi/acpica/tbprint.c
+++ b/drivers/acpi/acpica/tbprint.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -76,6 +76,7 @@ static void acpi_tb_fix_string(char *string, acpi_size length)
 		if (!isprint((int)*string)) {
 			*string = '?';
 		}
+
 		string++;
 		length--;
 	}
diff --git a/drivers/acpi/acpica/tbutils.c b/drivers/acpi/acpica/tbutils.c
index d8ddef38c947..3269bef371d7 100644
--- a/drivers/acpi/acpica/tbutils.c
+++ b/drivers/acpi/acpica/tbutils.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -121,6 +121,7 @@ void acpi_tb_check_dsdt_header(void)
 		ACPI_BIOS_ERROR((AE_INFO,
 				 "The DSDT has been corrupted or replaced - "
 				 "old, new headers below"));
+
 		acpi_tb_print_table_header(0, &acpi_gbl_original_dsdt_header);
 		acpi_tb_print_table_header(0, acpi_gbl_DSDT);
 
@@ -379,7 +380,6 @@ next_table:
 	}
 
 	acpi_os_unmap_memory(table, length);
-
 	return_ACPI_STATUS(AE_OK);
 }
 
@@ -389,7 +389,7 @@ next_table:
  *
  * PARAMETERS:  signature           - Sig string to be validated
  *
- * RETURN:      TRUE if signature is correct length and has valid characters
+ * RETURN:      TRUE if signature is has 4 valid ACPI characters
  *
  * DESCRIPTION: Validate an ACPI table signature.
  *
@@ -399,12 +399,6 @@ u8 acpi_is_valid_signature(char *signature)
 {
 	u32 i;
 
-	/* Validate the signature length */
-
-	if (strlen(signature) != ACPI_NAME_SIZE) {
-		return (FALSE);
-	}
-
 	/* Validate each character in the signature */
 
 	for (i = 0; i < ACPI_NAME_SIZE; i++) {
diff --git a/drivers/acpi/acpica/tbxface.c b/drivers/acpi/acpica/tbxface.c
index 5559e2c70b15..326df65decef 100644
--- a/drivers/acpi/acpica/tbxface.c
+++ b/drivers/acpi/acpica/tbxface.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/tbxfload.c b/drivers/acpi/acpica/tbxfload.c
index 55ee14ca9418..278666e39563 100644
--- a/drivers/acpi/acpica/tbxfload.c
+++ b/drivers/acpi/acpica/tbxfload.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -191,6 +191,7 @@ acpi_status acpi_tb_load_namespace(void)
 					"(%4.4s:%8.8s) while loading table",
 					table->signature.ascii,
 					table->pointer->oem_table_id));
+
 			tables_failed++;
 
 			ACPI_DEBUG_PRINT_RAW((ACPI_DB_INIT,
@@ -206,7 +207,7 @@ acpi_status acpi_tb_load_namespace(void)
 
 	if (!tables_failed) {
 		ACPI_INFO((AE_INFO,
-			   "%u ACPI AML tables successfully acquired and loaded",
+			   "%u ACPI AML tables successfully acquired and loaded\n",
 			   tables_loaded));
 	} else {
 		ACPI_ERROR((AE_INFO,
diff --git a/drivers/acpi/acpica/tbxfroot.c b/drivers/acpi/acpica/tbxfroot.c
index fa76a3603aa1..b9a78e457d19 100644
--- a/drivers/acpi/acpica/tbxfroot.c
+++ b/drivers/acpi/acpica/tbxfroot.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utaddress.c b/drivers/acpi/acpica/utaddress.c
index 911ea8e7fe87..c986ec66a118 100644
--- a/drivers/acpi/acpica/utaddress.c
+++ b/drivers/acpi/acpica/utaddress.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -239,8 +239,9 @@ acpi_ut_check_address_range(acpi_adr_space_type space_id,
 			overlap_count++;
 			if (warn) {	/* Optional warning message */
 				pathname =
-				    acpi_ns_get_external_pathname(range_info->
-								  region_node);
+				    acpi_ns_get_normalized_pathname(range_info->
+								    region_node,
+								    TRUE);
 
 				ACPI_WARNING((AE_INFO,
 					      "%s range 0x%8.8X%8.8X-0x%8.8X%8.8X conflicts with OpRegion 0x%8.8X%8.8X-0x%8.8X%8.8X (%s)",
diff --git a/drivers/acpi/acpica/utalloc.c b/drivers/acpi/acpica/utalloc.c
index 7a4101f0685e..3dbdc3ab8b78 100644
--- a/drivers/acpi/acpica/utalloc.c
+++ b/drivers/acpi/acpica/utalloc.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utbuffer.c b/drivers/acpi/acpica/utbuffer.c
index 01c8709ca586..0cfb2b8edad5 100644
--- a/drivers/acpi/acpica/utbuffer.c
+++ b/drivers/acpi/acpica/utbuffer.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utcache.c b/drivers/acpi/acpica/utcache.c
index 0d21fbd99363..c9a720f2274a 100644
--- a/drivers/acpi/acpica/utcache.c
+++ b/drivers/acpi/acpica/utcache.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utcopy.c b/drivers/acpi/acpica/utcopy.c
index 257221d452c8..98d53e59ce55 100644
--- a/drivers/acpi/acpica/utcopy.c
+++ b/drivers/acpi/acpica/utcopy.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -257,9 +257,9 @@ acpi_ut_copy_ielement_to_eelement(u8 object_type,
 	ACPI_FUNCTION_ENTRY();
 
 	this_index = state->pkg.index;
-	target_object = (union acpi_object *)
-	    &((union acpi_object *)(state->pkg.dest_object))->package.
-	    elements[this_index];
+	target_object = (union acpi_object *)&((union acpi_object *)
+					       (state->pkg.dest_object))->
+	    package.elements[this_index];
 
 	switch (object_type) {
 	case ACPI_COPY_TYPE_SIMPLE:
@@ -348,15 +348,15 @@ acpi_ut_copy_ipackage_to_epackage(union acpi_operand_object *internal_object,
 	 * Free space begins right after the first package
 	 */
 	info.length = ACPI_ROUND_UP_TO_NATIVE_WORD(sizeof(union acpi_object));
-	info.free_space =
-	    buffer + ACPI_ROUND_UP_TO_NATIVE_WORD(sizeof(union acpi_object));
+	info.free_space = buffer +
+	    ACPI_ROUND_UP_TO_NATIVE_WORD(sizeof(union acpi_object));
 	info.object_space = 0;
 	info.num_packages = 1;
 
 	external_object->type = internal_object->common.type;
 	external_object->package.count = internal_object->package.count;
-	external_object->package.elements = ACPI_CAST_PTR(union acpi_object,
-							  info.free_space);
+	external_object->package.elements =
+	    ACPI_CAST_PTR(union acpi_object, info.free_space);
 
 	/*
 	 * Leave room for an array of ACPI_OBJECTS in the buffer
@@ -593,8 +593,8 @@ acpi_ut_copy_epackage_to_ipackage(union acpi_object *external_object,
 	package_elements = package_object->package.elements;
 
 	/*
-	 * Recursive implementation. Probably ok, since nested external packages
-	 * as parameters should be very rare.
+	 * Recursive implementation. Probably ok, since nested external
+	 * packages as parameters should be very rare.
 	 */
 	for (i = 0; i < external_object->package.count; i++) {
 		status =
@@ -649,9 +649,8 @@ acpi_ut_copy_eobject_to_iobject(union acpi_object *external_object,
 		/*
 		 * Build a simple object (no nested objects)
 		 */
-		status =
-		    acpi_ut_copy_esimple_to_isimple(external_object,
-						    internal_object);
+		status = acpi_ut_copy_esimple_to_isimple(external_object,
+							 internal_object);
 	}
 
 	return_ACPI_STATUS(status);
diff --git a/drivers/acpi/acpica/utdebug.c b/drivers/acpi/acpica/utdebug.c
index 4146229103c8..1cfc5f69b033 100644
--- a/drivers/acpi/acpica/utdebug.c
+++ b/drivers/acpi/acpica/utdebug.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utdecode.c b/drivers/acpi/acpica/utdecode.c
index ecaaaffc0788..6ba65b02550c 100644
--- a/drivers/acpi/acpica/utdecode.c
+++ b/drivers/acpi/acpica/utdecode.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -114,7 +114,7 @@ const char *acpi_gbl_region_types[ACPI_NUM_PREDEFINED_REGIONS] = {
 	"PCC"			/* 0x0A */
 };
 
-char *acpi_ut_get_region_name(u8 space_id)
+const char *acpi_ut_get_region_name(u8 space_id)
 {
 
 	if (space_id >= ACPI_USER_REGION_BEGIN) {
@@ -127,7 +127,7 @@ char *acpi_ut_get_region_name(u8 space_id)
 		return ("InvalidSpaceId");
 	}
 
-	return (ACPI_CAST_PTR(char, acpi_gbl_region_types[space_id]));
+	return (acpi_gbl_region_types[space_id]);
 }
 
 /*******************************************************************************
@@ -152,14 +152,14 @@ static const char *acpi_gbl_event_types[ACPI_NUM_FIXED_EVENTS] = {
 	"RealTimeClock",
 };
 
-char *acpi_ut_get_event_name(u32 event_id)
+const char *acpi_ut_get_event_name(u32 event_id)
 {
 
 	if (event_id > ACPI_EVENT_MAX) {
 		return ("InvalidEventID");
 	}
 
-	return (ACPI_CAST_PTR(char, acpi_gbl_event_types[event_id]));
+	return (acpi_gbl_event_types[event_id]);
 }
 
 /*******************************************************************************
@@ -180,7 +180,8 @@ char *acpi_ut_get_event_name(u32 event_id)
  *
  * The type ACPI_TYPE_ANY (Untyped) is used as a "don't care" when searching;
  * when stored in a table it really means that we have thus far seen no
- * evidence to indicate what type is actually going to be stored for this entry.
+ * evidence to indicate what type is actually going to be stored for this
+ & entry.
  */
 static const char acpi_gbl_bad_type[] = "UNDEFINED";
 
@@ -220,17 +221,17 @@ static const char *acpi_gbl_ns_type_names[] = {
 	/* 30 */ "Invalid"
 };
 
-char *acpi_ut_get_type_name(acpi_object_type type)
+const char *acpi_ut_get_type_name(acpi_object_type type)
 {
 
 	if (type > ACPI_TYPE_INVALID) {
-		return (ACPI_CAST_PTR(char, acpi_gbl_bad_type));
+		return (acpi_gbl_bad_type);
 	}
 
-	return (ACPI_CAST_PTR(char, acpi_gbl_ns_type_names[type]));
+	return (acpi_gbl_ns_type_names[type]);
 }
 
-char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc)
+const char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc)
 {
 	ACPI_FUNCTION_TRACE(ut_get_object_type_name);
 
@@ -267,7 +268,7 @@ char *acpi_ut_get_object_type_name(union acpi_operand_object *obj_desc)
  *
  ******************************************************************************/
 
-char *acpi_ut_get_node_name(void *object)
+const char *acpi_ut_get_node_name(void *object)
 {
 	struct acpi_namespace_node *node = (struct acpi_namespace_node *)object;
 
@@ -333,7 +334,7 @@ static const char *acpi_gbl_desc_type_names[] = {
 	/* 15 */ "Node"
 };
 
-char *acpi_ut_get_descriptor_name(void *object)
+const char *acpi_ut_get_descriptor_name(void *object)
 {
 
 	if (!object) {
@@ -344,10 +345,7 @@ char *acpi_ut_get_descriptor_name(void *object)
 		return ("Not a Descriptor");
 	}
 
-	return (ACPI_CAST_PTR(char,
-			      acpi_gbl_desc_type_names[ACPI_GET_DESCRIPTOR_TYPE
-						       (object)]));
-
+	return (acpi_gbl_desc_type_names[ACPI_GET_DESCRIPTOR_TYPE(object)]);
 }
 
 /*******************************************************************************
@@ -415,7 +413,7 @@ const char *acpi_ut_get_reference_name(union acpi_operand_object *object)
 
 /* Names for internal mutex objects, used for debug output */
 
-static char *acpi_gbl_mutex_names[ACPI_NUM_MUTEX] = {
+static const char *acpi_gbl_mutex_names[ACPI_NUM_MUTEX] = {
 	"ACPI_MTX_Interpreter",
 	"ACPI_MTX_Namespace",
 	"ACPI_MTX_Tables",
@@ -424,7 +422,7 @@ static char *acpi_gbl_mutex_names[ACPI_NUM_MUTEX] = {
 	"ACPI_MTX_Memory",
 };
 
-char *acpi_ut_get_mutex_name(u32 mutex_id)
+const char *acpi_ut_get_mutex_name(u32 mutex_id)
 {
 
 	if (mutex_id > ACPI_MAX_MUTEX) {
diff --git a/drivers/acpi/acpica/utdelete.c b/drivers/acpi/acpica/utdelete.c
index 1638312e3d8f..529d6c38ea7c 100644
--- a/drivers/acpi/acpica/utdelete.c
+++ b/drivers/acpi/acpica/utdelete.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -209,6 +209,7 @@ static void acpi_ut_delete_internal_obj(union acpi_operand_object *object)
 			acpi_ut_delete_object_desc(object->method.mutex);
 			object->method.mutex = NULL;
 		}
+
 		if (object->method.node) {
 			object->method.node = NULL;
 		}
@@ -515,8 +516,8 @@ acpi_ut_update_object_reference(union acpi_operand_object *object, u16 action)
 		}
 
 		/*
-		 * All sub-objects must have their reference count incremented also.
-		 * Different object types have different subobjects.
+		 * All sub-objects must have their reference count incremented
+		 * also. Different object types have different subobjects.
 		 */
 		switch (object->common.type) {
 		case ACPI_TYPE_DEVICE:
diff --git a/drivers/acpi/acpica/uterror.c b/drivers/acpi/acpica/uterror.c
index 9ef80f2828e3..475932cecf1a 100644
--- a/drivers/acpi/acpica/uterror.c
+++ b/drivers/acpi/acpica/uterror.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -217,8 +217,9 @@ acpi_ut_namespace_error(const char *module_name,
 	} else {
 		/* Convert path to external format */
 
-		status = acpi_ns_externalize_name(ACPI_UINT32_MAX,
-						  internal_name, NULL, &name);
+		status =
+		    acpi_ns_externalize_name(ACPI_UINT32_MAX, internal_name,
+					     NULL, &name);
 
 		/* Print target name */
 
@@ -271,9 +272,8 @@ acpi_ut_method_error(const char *module_name,
 	acpi_os_printf(ACPI_MSG_ERROR);
 
 	if (path) {
-		status =
-		    acpi_ns_get_node(prefix_node, path, ACPI_NS_NO_UPSEARCH,
-				     &node);
+		status = acpi_ns_get_node(prefix_node, path,
+					  ACPI_NS_NO_UPSEARCH, &node);
 		if (ACPI_FAILURE(status)) {
 			acpi_os_printf("[Could not get node by pathname]");
 		}
diff --git a/drivers/acpi/acpica/uteval.c b/drivers/acpi/acpica/uteval.c
index 6c738fa0cd42..17b9f3e6e1e1 100644
--- a/drivers/acpi/acpica/uteval.c
+++ b/drivers/acpi/acpica/uteval.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utexcep.c b/drivers/acpi/acpica/utexcep.c
index 743a0ae9fb17..695240338e00 100644
--- a/drivers/acpi/acpica/utexcep.c
+++ b/drivers/acpi/acpica/utexcep.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utfileio.c b/drivers/acpi/acpica/utfileio.c
deleted file mode 100644
index d435b7b7eb94..000000000000
--- a/drivers/acpi/acpica/utfileio.c
+++ /dev/null
@@ -1,334 +0,0 @@
-/*******************************************************************************
- *
- * Module Name: utfileio - simple file I/O routines
- *
- ******************************************************************************/
-
-/*
- * Copyright (C) 2000 - 2015, Intel Corp.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions, and the following disclaimer,
- *    without modification.
- * 2. Redistributions in binary form must reproduce at minimum a disclaimer
- *    substantially similar to the "NO WARRANTY" disclaimer below
- *    ("Disclaimer") and any redistribution must be conditioned upon
- *    including a substantially similar Disclaimer requirement for further
- *    binary redistribution.
- * 3. Neither the names of the above-listed copyright holders nor the names
- *    of any contributors may be used to endorse or promote products derived
- *    from this software without specific prior written permission.
- *
- * Alternatively, this software may be distributed under the terms of the
- * GNU General Public License ("GPL") version 2 as published by the Free
- * Software Foundation.
- *
- * NO WARRANTY
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR
- * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
- * HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
- * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
- * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGES.
- */
-
-#include <acpi/acpi.h>
-#include "accommon.h"
-#include "actables.h"
-#include "acapps.h"
-#include "errno.h"
-
-#ifdef ACPI_ASL_COMPILER
-#include "aslcompiler.h"
-#endif
-
-#define _COMPONENT          ACPI_CA_DEBUGGER
-ACPI_MODULE_NAME("utfileio")
-
-#ifdef ACPI_APPLICATION
-/* Local prototypes */
-static acpi_status
-acpi_ut_check_text_mode_corruption(u8 *table,
-				   u32 table_length, u32 file_length);
-
-static acpi_status
-acpi_ut_read_table(FILE * fp,
-		   struct acpi_table_header **table, u32 *table_length);
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ut_check_text_mode_corruption
- *
- * PARAMETERS:  table           - Table buffer
- *              table_length    - Length of table from the table header
- *              file_length     - Length of the file that contains the table
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Check table for text mode file corruption where all linefeed
- *              characters (LF) have been replaced by carriage return linefeed
- *              pairs (CR/LF).
- *
- ******************************************************************************/
-
-static acpi_status
-acpi_ut_check_text_mode_corruption(u8 *table, u32 table_length, u32 file_length)
-{
-	u32 i;
-	u32 pairs = 0;
-
-	if (table_length != file_length) {
-		ACPI_WARNING((AE_INFO,
-			      "File length (0x%X) is not the same as the table length (0x%X)",
-			      file_length, table_length));
-	}
-
-	/* Scan entire table to determine if each LF has been prefixed with a CR */
-
-	for (i = 1; i < file_length; i++) {
-		if (table[i] == 0x0A) {
-			if (table[i - 1] != 0x0D) {
-
-				/* The LF does not have a preceding CR, table not corrupted */
-
-				return (AE_OK);
-			} else {
-				/* Found a CR/LF pair */
-
-				pairs++;
-			}
-			i++;
-		}
-	}
-
-	if (!pairs) {
-		return (AE_OK);
-	}
-
-	/*
-	 * Entire table scanned, each CR is part of a CR/LF pair --
-	 * meaning that the table was treated as a text file somewhere.
-	 *
-	 * NOTE: We can't "fix" the table, because any existing CR/LF pairs in the
-	 * original table are left untouched by the text conversion process --
-	 * meaning that we cannot simply replace CR/LF pairs with LFs.
-	 */
-	acpi_os_printf("Table has been corrupted by text mode conversion\n");
-	acpi_os_printf("All LFs (%u) were changed to CR/LF pairs\n", pairs);
-	acpi_os_printf("Table cannot be repaired!\n");
-	return (AE_BAD_VALUE);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ut_read_table
- *
- * PARAMETERS:  fp              - File that contains table
- *              table           - Return value, buffer with table
- *              table_length    - Return value, length of table
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Load the DSDT from the file pointer
- *
- ******************************************************************************/
-
-static acpi_status
-acpi_ut_read_table(FILE * fp,
-		   struct acpi_table_header **table, u32 *table_length)
-{
-	struct acpi_table_header table_header;
-	u32 actual;
-	acpi_status status;
-	u32 file_size;
-	u8 standard_header = TRUE;
-	s32 count;
-
-	/* Get the file size */
-
-	file_size = cm_get_file_size(fp);
-	if (file_size == ACPI_UINT32_MAX) {
-		return (AE_ERROR);
-	}
-
-	if (file_size < 4) {
-		return (AE_BAD_HEADER);
-	}
-
-	/* Read the signature */
-
-	fseek(fp, 0, SEEK_SET);
-
-	count = fread(&table_header, 1, sizeof(struct acpi_table_header), fp);
-	if (count != sizeof(struct acpi_table_header)) {
-		acpi_os_printf("Could not read the table header\n");
-		return (AE_BAD_HEADER);
-	}
-
-	/* The RSDP table does not have standard ACPI header */
-
-	if (ACPI_VALIDATE_RSDP_SIG(table_header.signature)) {
-		*table_length = file_size;
-		standard_header = FALSE;
-	} else {
-
-#if 0
-		/* Validate the table header/length */
-
-		status = acpi_tb_validate_table_header(&table_header);
-		if (ACPI_FAILURE(status)) {
-			acpi_os_printf("Table header is invalid!\n");
-			return (status);
-		}
-#endif
-
-		/* File size must be at least as long as the Header-specified length */
-
-		if (table_header.length > file_size) {
-			acpi_os_printf
-			    ("TableHeader length [0x%X] greater than the input file size [0x%X]\n",
-			     table_header.length, file_size);
-
-#ifdef ACPI_ASL_COMPILER
-			acpi_os_printf("File is corrupt or is ASCII text -- "
-				       "it must be a binary file\n");
-#endif
-			return (AE_BAD_HEADER);
-		}
-#ifdef ACPI_OBSOLETE_CODE
-		/* We only support a limited number of table types */
-
-		if (!ACPI_COMPARE_NAME
-		    ((char *)table_header.signature, ACPI_SIG_DSDT)
-		    && !ACPI_COMPARE_NAME((char *)table_header.signature,
-					  ACPI_SIG_PSDT)
-		    && !ACPI_COMPARE_NAME((char *)table_header.signature,
-					  ACPI_SIG_SSDT)) {
-			acpi_os_printf
-			    ("Table signature [%4.4s] is invalid or not supported\n",
-			     (char *)table_header.signature);
-			ACPI_DUMP_BUFFER(&table_header,
-					 sizeof(struct acpi_table_header));
-			return (AE_ERROR);
-		}
-#endif
-
-		*table_length = table_header.length;
-	}
-
-	/* Allocate a buffer for the table */
-
-	*table = acpi_os_allocate((size_t) file_size);
-	if (!*table) {
-		acpi_os_printf
-		    ("Could not allocate memory for ACPI table %4.4s (size=0x%X)\n",
-		     table_header.signature, *table_length);
-		return (AE_NO_MEMORY);
-	}
-
-	/* Get the rest of the table */
-
-	fseek(fp, 0, SEEK_SET);
-	actual = fread(*table, 1, (size_t) file_size, fp);
-	if (actual == file_size) {
-		if (standard_header) {
-
-			/* Now validate the checksum */
-
-			status = acpi_tb_verify_checksum((void *)*table,
-							 ACPI_CAST_PTR(struct
-								       acpi_table_header,
-								       *table)->
-							 length);
-
-			if (status == AE_BAD_CHECKSUM) {
-				status =
-				    acpi_ut_check_text_mode_corruption((u8 *)
-								       *table,
-								       file_size,
-								       (*table)->
-								       length);
-				return (status);
-			}
-		}
-		return (AE_OK);
-	}
-
-	if (actual > 0) {
-		acpi_os_printf("Warning - reading table, asked for %X got %X\n",
-			       file_size, actual);
-		return (AE_OK);
-	}
-
-	acpi_os_printf("Error - could not read the table file\n");
-	acpi_os_free(*table);
-	*table = NULL;
-	*table_length = 0;
-	return (AE_ERROR);
-}
-
-/*******************************************************************************
- *
- * FUNCTION:    acpi_ut_read_table_from_file
- *
- * PARAMETERS:  filename         - File where table is located
- *              table            - Where a pointer to the table is returned
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Get an ACPI table from a file
- *
- ******************************************************************************/
-
-acpi_status
-acpi_ut_read_table_from_file(char *filename, struct acpi_table_header ** table)
-{
-	FILE *file;
-	u32 file_size;
-	u32 table_length;
-	acpi_status status = AE_ERROR;
-
-	/* Open the file, get current size */
-
-	file = fopen(filename, "rb");
-	if (!file) {
-		perror("Could not open input file");
-
-		if (errno == ENOENT) {
-			return (AE_NOT_EXIST);
-		}
-
-		return (status);
-	}
-
-	file_size = cm_get_file_size(file);
-	if (file_size == ACPI_UINT32_MAX) {
-		goto exit;
-	}
-
-	/* Get the entire file */
-
-	fprintf(stderr,
-		"Reading ACPI table from file %12s - Length %.8u (0x%06X)\n",
-		filename, file_size, file_size);
-
-	status = acpi_ut_read_table(file, table, &table_length);
-	if (ACPI_FAILURE(status)) {
-		acpi_os_printf("Could not get table from the file\n");
-	}
-
-exit:
-	fclose(file);
-	return (status);
-}
-
-#endif
diff --git a/drivers/acpi/acpica/utglobal.c b/drivers/acpi/acpica/utglobal.c
index a72685c1e819..48fffcfe9911 100644
--- a/drivers/acpi/acpica/utglobal.c
+++ b/drivers/acpi/acpica/utglobal.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/uthex.c b/drivers/acpi/acpica/uthex.c
index fda8b3def81c..4354fb800fe4 100644
--- a/drivers/acpi/acpica/uthex.c
+++ b/drivers/acpi/acpica/uthex.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -48,7 +48,7 @@
 ACPI_MODULE_NAME("uthex")
 
 /* Hex to ASCII conversion table */
-static char acpi_gbl_hex_to_ascii[] = {
+static const char acpi_gbl_hex_to_ascii[] = {
 	'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D',
 	    'E', 'F'
 };
diff --git a/drivers/acpi/acpica/utids.c b/drivers/acpi/acpica/utids.c
index 7956df1e263c..6fb4ec365272 100644
--- a/drivers/acpi/acpica/utids.c
+++ b/drivers/acpi/acpica/utids.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -127,73 +127,6 @@ cleanup:
 
 /*******************************************************************************
  *
- * FUNCTION:    acpi_ut_execute_SUB
- *
- * PARAMETERS:  device_node         - Node for the device
- *              return_id           - Where the _SUB is returned
- *
- * RETURN:      Status
- *
- * DESCRIPTION: Executes the _SUB control method that returns the subsystem
- *              ID of the device. The _SUB value is always a string containing
- *              either a valid PNP or ACPI ID.
- *
- *              NOTE: Internal function, no parameter validation
- *
- ******************************************************************************/
-
-acpi_status
-acpi_ut_execute_SUB(struct acpi_namespace_node *device_node,
-		    struct acpi_pnp_device_id **return_id)
-{
-	union acpi_operand_object *obj_desc;
-	struct acpi_pnp_device_id *sub;
-	u32 length;
-	acpi_status status;
-
-	ACPI_FUNCTION_TRACE(ut_execute_SUB);
-
-	status = acpi_ut_evaluate_object(device_node, METHOD_NAME__SUB,
-					 ACPI_BTYPE_STRING, &obj_desc);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
-	/* Get the size of the String to be returned, includes null terminator */
-
-	length = obj_desc->string.length + 1;
-
-	/* Allocate a buffer for the SUB */
-
-	sub =
-	    ACPI_ALLOCATE_ZEROED(sizeof(struct acpi_pnp_device_id) +
-				 (acpi_size) length);
-	if (!sub) {
-		status = AE_NO_MEMORY;
-		goto cleanup;
-	}
-
-	/* Area for the string starts after PNP_DEVICE_ID struct */
-
-	sub->string =
-	    ACPI_ADD_PTR(char, sub, sizeof(struct acpi_pnp_device_id));
-
-	/* Simply copy existing string */
-
-	strcpy(sub->string, obj_desc->string.pointer);
-	sub->length = length;
-	*return_id = sub;
-
-cleanup:
-
-	/* On exit, we must delete the return object */
-
-	acpi_ut_remove_reference(obj_desc);
-	return_ACPI_STATUS(status);
-}
-
-/*******************************************************************************
- *
  * FUNCTION:    acpi_ut_execute_UID
  *
  * PARAMETERS:  device_node         - Node for the device
diff --git a/drivers/acpi/acpica/utinit.c b/drivers/acpi/acpica/utinit.c
index ccd0745f011e..f91f724c487c 100644
--- a/drivers/acpi/acpica/utinit.c
+++ b/drivers/acpi/acpica/utinit.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -206,7 +206,6 @@ 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_reg_methods_executed = FALSE;
 	acpi_gbl_max_loop_iterations = 0xFFFF;
 
 	/* Hardware oriented */
diff --git a/drivers/acpi/acpica/utlock.c b/drivers/acpi/acpica/utlock.c
index 089f78bbd59b..3cd0978925ef 100644
--- a/drivers/acpi/acpica/utlock.c
+++ b/drivers/acpi/acpica/utlock.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utmath.c b/drivers/acpi/acpica/utmath.c
index f9ff100f0159..667372093de1 100644
--- a/drivers/acpi/acpica/utmath.c
+++ b/drivers/acpi/acpica/utmath.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -111,6 +111,7 @@ acpi_ut_short_divide(u64 dividend,
 	 */
 	ACPI_DIV_64_BY_32(0, dividend_ovl.part.hi, divisor,
 			  quotient.part.hi, remainder32);
+
 	ACPI_DIV_64_BY_32(remainder32, dividend_ovl.part.lo, divisor,
 			  quotient.part.lo, remainder32);
 
@@ -179,6 +180,7 @@ acpi_ut_divide(u64 in_dividend,
 		 */
 		ACPI_DIV_64_BY_32(0, dividend.part.hi, divisor.part.lo,
 				  quotient.part.hi, partial1);
+
 		ACPI_DIV_64_BY_32(partial1, dividend.part.lo, divisor.part.lo,
 				  quotient.part.lo, remainder.part.lo);
 	}
@@ -206,12 +208,12 @@ acpi_ut_divide(u64 in_dividend,
 
 		ACPI_DIV_64_BY_32(normalized_dividend.part.hi,
 				  normalized_dividend.part.lo,
-				  normalized_divisor.part.lo,
-				  quotient.part.lo, partial1);
+				  normalized_divisor.part.lo, quotient.part.lo,
+				  partial1);
 
 		/*
-		 * The quotient is always 32 bits, and simply requires adjustment.
-		 * The 64-bit remainder must be generated.
+		 * The quotient is always 32 bits, and simply requires
+		 * adjustment. The 64-bit remainder must be generated.
 		 */
 		partial1 = quotient.part.lo * divisor.part.hi;
 		partial2.full = (u64) quotient.part.lo * divisor.part.lo;
diff --git a/drivers/acpi/acpica/utmisc.c b/drivers/acpi/acpica/utmisc.c
index bd4443bdcbad..d938c27cc6cf 100644
--- a/drivers/acpi/acpica/utmisc.c
+++ b/drivers/acpi/acpica/utmisc.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -264,8 +264,8 @@ acpi_ut_walk_package_tree(union acpi_operand_object *source_object,
 		 */
 		if ((!this_source_obj) ||
 		    (ACPI_GET_DESCRIPTOR_TYPE(this_source_obj) !=
-		     ACPI_DESC_TYPE_OPERAND)
-		    || (this_source_obj->common.type != ACPI_TYPE_PACKAGE)) {
+		     ACPI_DESC_TYPE_OPERAND) ||
+		    (this_source_obj->common.type != ACPI_TYPE_PACKAGE)) {
 			status =
 			    walk_callback(ACPI_COPY_TYPE_SIMPLE,
 					  this_source_obj, state, context);
@@ -318,9 +318,10 @@ acpi_ut_walk_package_tree(union acpi_operand_object *source_object,
 			 * The callback above returned a new target package object.
 			 */
 			acpi_ut_push_generic_state(&state_list, state);
-			state = acpi_ut_create_pkg_state(this_source_obj,
-							 state->pkg.
-							 this_target_obj, 0);
+			state =
+			    acpi_ut_create_pkg_state(this_source_obj,
+						     state->pkg.this_target_obj,
+						     0);
 			if (!state) {
 
 				/* Free any stacked Update State objects */
diff --git a/drivers/acpi/acpica/utmutex.c b/drivers/acpi/acpica/utmutex.c
index ce406e39b669..15073375bd00 100644
--- a/drivers/acpi/acpica/utmutex.c
+++ b/drivers/acpi/acpica/utmutex.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -111,17 +111,6 @@ acpi_status acpi_ut_mutex_initialize(void)
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
-#ifdef ACPI_DEBUGGER
-
-	/* Debugger Support */
-
-	status = acpi_os_create_mutex(&acpi_gbl_db_command_ready);
-	if (ACPI_FAILURE(status)) {
-		return_ACPI_STATUS(status);
-	}
-
-	status = acpi_os_create_mutex(&acpi_gbl_db_command_complete);
-#endif
 
 	return_ACPI_STATUS(status);
 }
@@ -162,12 +151,6 @@ void acpi_ut_mutex_terminate(void)
 	/* Delete the reader/writer lock */
 
 	acpi_ut_delete_rw_lock(&acpi_gbl_namespace_rw_lock);
-
-#ifdef ACPI_DEBUGGER
-	acpi_os_delete_mutex(acpi_gbl_db_command_ready);
-	acpi_os_delete_mutex(acpi_gbl_db_command_complete);
-#endif
-
 	return_VOID;
 }
 
@@ -290,8 +273,9 @@ acpi_status acpi_ut_acquire_mutex(acpi_mutex_handle mutex_id)
 			  (u32)this_thread_id,
 			  acpi_ut_get_mutex_name(mutex_id)));
 
-	status = acpi_os_acquire_mutex(acpi_gbl_mutex_info[mutex_id].mutex,
-				       ACPI_WAIT_FOREVER);
+	status =
+	    acpi_os_acquire_mutex(acpi_gbl_mutex_info[mutex_id].mutex,
+				  ACPI_WAIT_FOREVER);
 	if (ACPI_SUCCESS(status)) {
 		ACPI_DEBUG_PRINT((ACPI_DB_MUTEX,
 				  "Thread %u acquired Mutex [%s]\n",
diff --git a/drivers/acpi/acpica/utnonansi.c b/drivers/acpi/acpica/utnonansi.c
index 1d5f6b17b766..c427a5cda465 100644
--- a/drivers/acpi/acpica/utnonansi.c
+++ b/drivers/acpi/acpica/utnonansi.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -282,8 +282,8 @@ acpi_status acpi_ut_strtoul64(char *string, u32 base, u64 *ret_integer)
 
 		/* Divide the digit into the correct position */
 
-		(void)acpi_ut_short_divide((dividend - (u64)this_digit),
-					   base, &quotient, NULL);
+		(void)acpi_ut_short_divide((dividend - (u64)this_digit), base,
+					   &quotient, NULL);
 
 		if (return_value > quotient) {
 			if (to_integer_op) {
diff --git a/drivers/acpi/acpica/utobject.c b/drivers/acpi/acpica/utobject.c
index 7d83efe1ea29..edad3f043ab9 100644
--- a/drivers/acpi/acpica/utobject.c
+++ b/drivers/acpi/acpica/utobject.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -112,9 +112,9 @@ union acpi_operand_object *acpi_ut_create_internal_object_dbg(const char
 
 		/* These types require a secondary object */
 
-		second_object = acpi_ut_allocate_object_desc_dbg(module_name,
-								 line_number,
-								 component_id);
+		second_object =
+		    acpi_ut_allocate_object_desc_dbg(module_name, line_number,
+						     component_id);
 		if (!second_object) {
 			acpi_ut_delete_object_desc(object);
 			return_PTR(NULL);
@@ -253,7 +253,8 @@ union acpi_operand_object *acpi_ut_create_buffer_object(acpi_size buffer_size)
 		buffer = ACPI_ALLOCATE_ZEROED(buffer_size);
 		if (!buffer) {
 			ACPI_ERROR((AE_INFO, "Could not allocate size %u",
-				    (u32) buffer_size));
+				    (u32)buffer_size));
+
 			acpi_ut_remove_reference(buffer_desc);
 			return_PTR(NULL);
 		}
@@ -305,7 +306,8 @@ union acpi_operand_object *acpi_ut_create_string_object(acpi_size string_size)
 	string = ACPI_ALLOCATE_ZEROED(string_size + 1);
 	if (!string) {
 		ACPI_ERROR((AE_INFO, "Could not allocate size %u",
-			    (u32) string_size));
+			    (u32)string_size));
+
 		acpi_ut_remove_reference(string_desc);
 		return_PTR(NULL);
 	}
@@ -649,8 +651,9 @@ acpi_ut_get_package_object_size(union acpi_operand_object *internal_object,
 	info.object_space = 0;
 	info.num_packages = 1;
 
-	status = acpi_ut_walk_package_tree(internal_object, NULL,
-					   acpi_ut_get_element_length, &info);
+	status =
+	    acpi_ut_walk_package_tree(internal_object, NULL,
+				      acpi_ut_get_element_length, &info);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
 	}
@@ -660,7 +663,8 @@ acpi_ut_get_package_object_size(union acpi_operand_object *internal_object,
 	 * just add the length of the package objects themselves.
 	 * Round up to the next machine word.
 	 */
-	info.length += ACPI_ROUND_UP_TO_NATIVE_WORD(sizeof(union acpi_object)) *
+	info.length +=
+	    ACPI_ROUND_UP_TO_NATIVE_WORD(sizeof(union acpi_object)) *
 	    (acpi_size) info.num_packages;
 
 	/* Return the total package length */
@@ -692,8 +696,8 @@ acpi_ut_get_object_size(union acpi_operand_object *internal_object,
 	ACPI_FUNCTION_ENTRY();
 
 	if ((ACPI_GET_DESCRIPTOR_TYPE(internal_object) ==
-	     ACPI_DESC_TYPE_OPERAND)
-	    && (internal_object->common.type == ACPI_TYPE_PACKAGE)) {
+	     ACPI_DESC_TYPE_OPERAND) &&
+	    (internal_object->common.type == ACPI_TYPE_PACKAGE)) {
 		status =
 		    acpi_ut_get_package_object_size(internal_object,
 						    obj_length);
diff --git a/drivers/acpi/acpica/utosi.c b/drivers/acpi/acpica/utosi.c
index 8f3d203aed79..b5cfe577fabf 100644
--- a/drivers/acpi/acpica/utosi.c
+++ b/drivers/acpi/acpica/utosi.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -269,9 +269,10 @@ acpi_status acpi_ut_remove_interface(acpi_string interface_name)
 	previous_interface = next_interface = acpi_gbl_supported_interfaces;
 	while (next_interface) {
 		if (!strcmp(interface_name, next_interface->name)) {
-
-			/* Found: name is in either the static list or was added at runtime */
-
+			/*
+			 * Found: name is in either the static list
+			 * or was added at runtime
+			 */
 			if (next_interface->flags & ACPI_OSI_DYNAMIC) {
 
 				/* Interface was added dynamically, remove and free it */
@@ -288,8 +289,8 @@ acpi_status acpi_ut_remove_interface(acpi_string interface_name)
 				ACPI_FREE(next_interface);
 			} else {
 				/*
-				 * Interface is in static list. If marked invalid, then it
-				 * does not actually exist. Else, mark it invalid.
+				 * Interface is in static list. If marked invalid, then
+				 * it does not actually exist. Else, mark it invalid.
 				 */
 				if (next_interface->flags & ACPI_OSI_INVALID) {
 					return (AE_NOT_EXIST);
diff --git a/drivers/acpi/acpica/utownerid.c b/drivers/acpi/acpica/utownerid.c
index 2959217067cb..813520ab8ca4 100644
--- a/drivers/acpi/acpica/utownerid.c
+++ b/drivers/acpi/acpica/utownerid.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -73,8 +73,8 @@ acpi_status acpi_ut_allocate_owner_id(acpi_owner_id * owner_id)
 	/* Guard against multiple allocations of ID to the same location */
 
 	if (*owner_id) {
-		ACPI_ERROR((AE_INFO, "Owner ID [0x%2.2X] already exists",
-			    *owner_id));
+		ACPI_ERROR((AE_INFO,
+			    "Owner ID [0x%2.2X] already exists", *owner_id));
 		return_ACPI_STATUS(AE_ALREADY_EXISTS);
 	}
 
@@ -87,8 +87,8 @@ acpi_status acpi_ut_allocate_owner_id(acpi_owner_id * owner_id)
 
 	/*
 	 * Find a free owner ID, cycle through all possible IDs on repeated
-	 * allocations. (ACPI_NUM_OWNERID_MASKS + 1) because first index may have
-	 * to be scanned twice.
+	 * allocations. (ACPI_NUM_OWNERID_MASKS + 1) because first index
+	 * may have to be scanned twice.
 	 */
 	for (i = 0, j = acpi_gbl_last_owner_id_index;
 	     i < (ACPI_NUM_OWNERID_MASKS + 1); i++, j++) {
@@ -141,8 +141,8 @@ acpi_status acpi_ut_allocate_owner_id(acpi_owner_id * owner_id)
 	 * they are released when a table is unloaded or a method completes
 	 * execution.
 	 *
-	 * If this error happens, there may be very deep nesting of invoked control
-	 * methods, or there may be a bug where the IDs are not released.
+	 * If this error happens, there may be very deep nesting of invoked
+	 * control methods, or there may be a bug where the IDs are not released.
 	 */
 	status = AE_OWNER_ID_LIMIT;
 	ACPI_ERROR((AE_INFO,
diff --git a/drivers/acpi/acpica/utpredef.c b/drivers/acpi/acpica/utpredef.c
index 97898ed71b4b..770a1775b264 100644
--- a/drivers/acpi/acpica/utpredef.c
+++ b/drivers/acpi/acpica/utpredef.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -225,8 +225,10 @@ const union acpi_predefined_info *acpi_ut_match_resource_name(char *name)
 {
 	const union acpi_predefined_info *this_name;
 
-	/* Quick check for a predefined name, first character must be underscore */
-
+	/*
+	 * Quick check for a predefined name, first character must
+	 * be underscore
+	 */
 	if (name[0] != '_') {
 		return (NULL);
 	}
diff --git a/drivers/acpi/acpica/utprint.c b/drivers/acpi/acpica/utprint.c
index b26297c5de49..8c218ad787cd 100644
--- a/drivers/acpi/acpica/utprint.c
+++ b/drivers/acpi/acpica/utprint.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -314,8 +314,9 @@ static char *acpi_ut_format_number(char *string,
 	if (need_prefix) {
 		string = acpi_ut_bound_string_output(string, end, '0');
 		if (base == 16) {
-			string = acpi_ut_bound_string_output(string, end,
-							     upper ? 'X' : 'x');
+			string =
+			    acpi_ut_bound_string_output(string, end,
+							upper ? 'X' : 'x');
 		}
 	}
 	if (!(type & ACPI_FORMAT_LEFT)) {
@@ -400,6 +401,7 @@ acpi_ut_vsnprintf(char *string,
 			} else {
 				break;
 			}
+
 		} while (1);
 
 		/* Process width */
@@ -429,6 +431,7 @@ acpi_ut_vsnprintf(char *string,
 				++format;
 				precision = va_arg(args, int);
 			}
+
 			if (precision < 0) {
 				precision = 0;
 			}
@@ -488,10 +491,12 @@ acpi_ut_vsnprintf(char *string,
 									' ');
 				}
 			}
+
 			for (i = 0; i < length; ++i) {
 				pos = acpi_ut_bound_string_output(pos, end, *s);
 				++s;
 			}
+
 			while (length < width--) {
 				pos =
 				    acpi_ut_bound_string_output(pos, end, ' ');
@@ -529,9 +534,9 @@ acpi_ut_vsnprintf(char *string,
 			}
 
 			p = va_arg(args, void *);
-			pos = acpi_ut_format_number(pos, end,
-						    ACPI_TO_INTEGER(p), 16,
-						    width, precision, type);
+			pos =
+			    acpi_ut_format_number(pos, end, ACPI_TO_INTEGER(p),
+						  16, width, precision, type);
 			continue;
 
 		default:
diff --git a/drivers/acpi/acpica/utresrc.c b/drivers/acpi/acpica/utresrc.c
index b3505dbc715e..1de3376da66a 100644
--- a/drivers/acpi/acpica/utresrc.c
+++ b/drivers/acpi/acpica/utresrc.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -441,8 +441,8 @@ acpi_ut_walk_aml_resources(struct acpi_walk_state *walk_state,
 		    acpi_ut_validate_resource(walk_state, aml, &resource_index);
 		if (ACPI_FAILURE(status)) {
 			/*
-			 * Exit on failure. Cannot continue because the descriptor length
-			 * may be bogus also.
+			 * Exit on failure. Cannot continue because the descriptor
+			 * length may be bogus also.
 			 */
 			return_ACPI_STATUS(status);
 		}
@@ -568,8 +568,8 @@ acpi_ut_validate_resource(struct acpi_walk_state *walk_state,
 	}
 
 	/*
-	 * Check validity of the resource type, via acpi_gbl_resource_types. Zero
-	 * indicates an invalid resource.
+	 * Check validity of the resource type, via acpi_gbl_resource_types.
+	 * Zero indicates an invalid resource.
 	 */
 	if (!acpi_gbl_resource_types[resource_index]) {
 		goto invalid_resource;
diff --git a/drivers/acpi/acpica/utstate.c b/drivers/acpi/acpica/utstate.c
index f201171c5dda..f3d4dbd5fac0 100644
--- a/drivers/acpi/acpica/utstate.c
+++ b/drivers/acpi/acpica/utstate.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -246,6 +246,7 @@ union acpi_generic_state *acpi_ut_create_pkg_state(void *internal_object,
 	state->pkg.dest_object = external_object;
 	state->pkg.index = index;
 	state->pkg.num_packages = 1;
+
 	return (state);
 }
 
@@ -279,6 +280,7 @@ union acpi_generic_state *acpi_ut_create_control_state(void)
 
 	state->common.descriptor_type = ACPI_DESC_TYPE_STATE_CONTROL;
 	state->common.state = ACPI_CONTROL_CONDITIONAL_EXECUTING;
+
 	return (state);
 }
 
@@ -304,5 +306,6 @@ void acpi_ut_delete_generic_state(union acpi_generic_state *state)
 	if (state) {
 		(void)acpi_os_release_object(acpi_gbl_state_cache, state);
 	}
+
 	return;
 }
diff --git a/drivers/acpi/acpica/utstring.c b/drivers/acpi/acpica/utstring.c
index 4ddd105d9741..0b005728db4e 100644
--- a/drivers/acpi/acpica/utstring.c
+++ b/drivers/acpi/acpica/utstring.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -135,6 +135,7 @@ void acpi_ut_print_string(char *string, u16 max_length)
 			break;
 		}
 	}
+
 	acpi_os_printf("\"");
 
 	if (i == max_length && string[i]) {
@@ -239,6 +240,14 @@ void acpi_ut_repair_name(char *name)
 
 	ACPI_FUNCTION_NAME(ut_repair_name);
 
+	/*
+	 * Special case for the root node. This can happen if we get an
+	 * error during the execution of module-level code.
+	 */
+	if (ACPI_COMPARE_NAME(name, "\\___")) {
+		return;
+	}
+
 	ACPI_MOVE_NAME(&original_name, name);
 
 	/* Check each character in the name */
diff --git a/drivers/acpi/acpica/uttrack.c b/drivers/acpi/acpica/uttrack.c
index 9a7dc8196a5d..c7c2bb8f3559 100644
--- a/drivers/acpi/acpica/uttrack.c
+++ b/drivers/acpi/acpica/uttrack.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -150,9 +150,9 @@ void *acpi_ut_allocate_and_track(acpi_size size,
 		return (NULL);
 	}
 
-	status = acpi_ut_track_allocation(allocation, size,
-					  ACPI_MEM_MALLOC, component, module,
-					  line);
+	status =
+	    acpi_ut_track_allocation(allocation, size, ACPI_MEM_MALLOC,
+				     component, module, line);
 	if (ACPI_FAILURE(status)) {
 		acpi_os_free(allocation);
 		return (NULL);
@@ -161,6 +161,7 @@ void *acpi_ut_allocate_and_track(acpi_size size,
 	acpi_gbl_global_list->total_allocated++;
 	acpi_gbl_global_list->total_size += (u32)size;
 	acpi_gbl_global_list->current_total_size += (u32)size;
+
 	if (acpi_gbl_global_list->current_total_size >
 	    acpi_gbl_global_list->max_occupied) {
 		acpi_gbl_global_list->max_occupied =
@@ -223,6 +224,7 @@ void *acpi_ut_allocate_zeroed_and_track(acpi_size size,
 	acpi_gbl_global_list->total_allocated++;
 	acpi_gbl_global_list->total_size += (u32)size;
 	acpi_gbl_global_list->current_total_size += (u32)size;
+
 	if (acpi_gbl_global_list->current_total_size >
 	    acpi_gbl_global_list->max_occupied) {
 		acpi_gbl_global_list->max_occupied =
@@ -269,8 +271,8 @@ acpi_ut_free_and_track(void *allocation,
 	acpi_gbl_global_list->total_freed++;
 	acpi_gbl_global_list->current_total_size -= debug_block->size;
 
-	status = acpi_ut_remove_allocation(debug_block,
-					   component, module, line);
+	status =
+	    acpi_ut_remove_allocation(debug_block, component, module, line);
 	if (ACPI_FAILURE(status)) {
 		ACPI_EXCEPTION((AE_INFO, status, "Could not free memory"));
 	}
@@ -525,35 +527,35 @@ void acpi_ut_dump_allocation_info(void)
 
 /*
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Current allocations",
-			  mem_list->current_count,
-			  ROUND_UP_TO_1K (mem_list->current_size)));
+		("%30s: %4d (%3d Kb)\n", "Current allocations",
+		mem_list->current_count,
+		ROUND_UP_TO_1K (mem_list->current_size)));
 
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Max concurrent allocations",
-			  mem_list->max_concurrent_count,
-			  ROUND_UP_TO_1K (mem_list->max_concurrent_size)));
+		("%30s: %4d (%3d Kb)\n", "Max concurrent allocations",
+		mem_list->max_concurrent_count,
+		ROUND_UP_TO_1K (mem_list->max_concurrent_size)));
 
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Total (all) internal objects",
-			  running_object_count,
-			  ROUND_UP_TO_1K (running_object_size)));
+		("%30s: %4d (%3d Kb)\n", "Total (all) internal objects",
+		running_object_count,
+		ROUND_UP_TO_1K (running_object_size)));
 
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Total (all) allocations",
-			  running_alloc_count,
-			  ROUND_UP_TO_1K (running_alloc_size)));
+		("%30s: %4d (%3d Kb)\n", "Total (all) allocations",
+		running_alloc_count,
+		ROUND_UP_TO_1K (running_alloc_size)));
 
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Current Nodes",
-			  acpi_gbl_current_node_count,
-			  ROUND_UP_TO_1K (acpi_gbl_current_node_size)));
+		("%30s: %4d (%3d Kb)\n", "Current Nodes",
+		acpi_gbl_current_node_count,
+		ROUND_UP_TO_1K (acpi_gbl_current_node_size)));
 
 	ACPI_DEBUG_PRINT (TRACE_ALLOCATIONS | TRACE_TABLES,
-			  ("%30s: %4d (%3d Kb)\n", "Max Nodes",
-			  acpi_gbl_max_concurrent_node_count,
-			  ROUND_UP_TO_1K ((acpi_gbl_max_concurrent_node_count *
-					 sizeof (struct acpi_namespace_node)))));
+		("%30s: %4d (%3d Kb)\n", "Max Nodes",
+		acpi_gbl_max_concurrent_node_count,
+		ROUND_UP_TO_1K ((acpi_gbl_max_concurrent_node_count *
+			sizeof (struct acpi_namespace_node)))));
 */
 	return_VOID;
 }
diff --git a/drivers/acpi/acpica/utuuid.c b/drivers/acpi/acpica/utuuid.c
index e6cab669bd9c..81088ff9d67b 100644
--- a/drivers/acpi/acpica/utuuid.c
+++ b/drivers/acpi/acpica/utuuid.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/acpi/acpica/utxface.c b/drivers/acpi/acpica/utxface.c
index f9c8f9ce1f0f..68d4673f62e6 100644
--- a/drivers/acpi/acpica/utxface.c
+++ b/drivers/acpi/acpica/utxface.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -154,7 +154,6 @@ acpi_status acpi_get_system_info(struct acpi_buffer * out_buffer)
 	 * Populate the return buffer
 	 */
 	info_ptr = (struct acpi_system_info *)out_buffer->pointer;
-
 	info_ptr->acpi_ca_version = ACPI_CA_VERSION;
 
 	/* System flags (ACPI capabilities) */
@@ -216,7 +215,6 @@ acpi_status acpi_get_statistics(struct acpi_statistics *stats)
 	/* Other counters */
 
 	stats->method_count = acpi_method_count;
-
 	return_ACPI_STATUS(AE_OK);
 }
 
diff --git a/drivers/acpi/acpica/utxferror.c b/drivers/acpi/acpica/utxferror.c
index 98d578753101..6fe59597b599 100644
--- a/drivers/acpi/acpica/utxferror.c
+++ b/drivers/acpi/acpica/utxferror.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -117,6 +117,7 @@ acpi_exception(const char *module_name,
 		acpi_os_printf(ACPI_MSG_EXCEPTION "%s, ",
 			       acpi_format_exception(status));
 	}
+
 	va_start(arg_list, format);
 	acpi_os_vprintf(format, arg_list);
 	ACPI_MSG_SUFFIX;
diff --git a/drivers/acpi/acpica/utxfinit.c b/drivers/acpi/acpica/utxfinit.c
index a7137ec28447..721b87cce908 100644
--- a/drivers/acpi/acpica/utxfinit.c
+++ b/drivers/acpi/acpica/utxfinit.c
@@ -5,7 +5,7 @@
  *****************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -147,6 +147,28 @@ acpi_status __init acpi_enable_subsystem(u32 flags)
 
 	ACPI_FUNCTION_TRACE(acpi_enable_subsystem);
 
+	/*
+	 * The early initialization phase is complete. The namespace is loaded,
+	 * and we can now support address spaces other than Memory, I/O, and
+	 * PCI_Config.
+	 */
+	acpi_gbl_early_initialization = FALSE;
+
+	/*
+	 * Install the default operation region handlers. These are the
+	 * handlers that are defined by the ACPI specification to be
+	 * "always accessible" -- namely, system_memory, system_IO, and
+	 * PCI_Config. This also means that no _REG methods need to be
+	 * run for these address spaces. We need to have these handlers
+	 * installed before any AML code can be executed, especially any
+	 * module-level code (11/2015).
+	 */
+	status = acpi_ev_install_region_handlers();
+	if (ACPI_FAILURE(status)) {
+		ACPI_EXCEPTION((AE_INFO, status,
+				"During Region initialization"));
+		return_ACPI_STATUS(status);
+	}
 #if (!ACPI_REDUCED_HARDWARE)
 
 	/* Enable ACPI mode */
@@ -175,23 +197,7 @@ acpi_status __init acpi_enable_subsystem(u32 flags)
 			return_ACPI_STATUS(status);
 		}
 	}
-#endif				/* !ACPI_REDUCED_HARDWARE */
-
-	/*
-	 * Install the default op_region handlers. These are installed unless
-	 * other handlers have already been installed via the
-	 * install_address_space_handler interface.
-	 */
-	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
-		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
-				  "[Init] Installing default address space handlers\n"));
 
-		status = acpi_ev_install_region_handlers();
-		if (ACPI_FAILURE(status)) {
-			return_ACPI_STATUS(status);
-		}
-	}
-#if (!ACPI_REDUCED_HARDWARE)
 	/*
 	 * Initialize ACPI Event handling (Fixed and General Purpose)
 	 *
@@ -261,6 +267,7 @@ acpi_status __init acpi_initialize_objects(u32 flags)
 	 * initialized, even if they contain executable AML (see the call to
 	 * acpi_ns_initialize_objects below).
 	 */
+	acpi_gbl_reg_methods_enabled = TRUE;
 	if (!(flags & ACPI_NO_ADDRESS_SPACE_INIT)) {
 		ACPI_DEBUG_PRINT((ACPI_DB_EXEC,
 				  "[Init] Executing _REG OpRegion methods\n"));
@@ -285,8 +292,14 @@ acpi_status __init acpi_initialize_objects(u32 flags)
 	 * outside of any control method is wrapped with a temporary control
 	 * method object and placed on a global list. The methods on this list
 	 * are executed below.
+	 *
+	 * This case executes the module-level code for all tables only after
+	 * 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.
 	 */
-	acpi_ns_exec_module_code_list();
+	if (acpi_gbl_group_module_level_code) {
+		acpi_ns_exec_module_code_list();
+	}
 
 	/*
 	 * Initialize the objects that remain uninitialized. This runs the
diff --git a/drivers/acpi/acpica/utxfmutex.c b/drivers/acpi/acpica/utxfmutex.c
index f2606af3364c..850de0155528 100644
--- a/drivers/acpi/acpica/utxfmutex.c
+++ b/drivers/acpi/acpica/utxfmutex.c
@@ -5,7 +5,7 @@
  ******************************************************************************/
 
 /*
- * Copyright (C) 2000 - 2015, Intel Corp.
+ * Copyright (C) 2000 - 2016, Intel Corp.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -89,9 +89,9 @@ acpi_ut_get_mutex_object(acpi_handle handle,
 
 	mutex_node = handle;
 	if (pathname != NULL) {
-		status = acpi_get_handle(handle, pathname,
-					 ACPI_CAST_PTR(acpi_handle,
-						       &mutex_node));
+		status =
+		    acpi_get_handle(handle, pathname,
+				    ACPI_CAST_PTR(acpi_handle, &mutex_node));
 		if (ACPI_FAILURE(status)) {
 			return (status);
 		}
diff --git a/drivers/acpi/apei/erst.c b/drivers/acpi/apei/erst.c
index 6682c5daf742..6e6bc1059301 100644
--- a/drivers/acpi/apei/erst.c
+++ b/drivers/acpi/apei/erst.c
@@ -32,6 +32,7 @@
 #include <linux/hardirq.h>
 #include <linux/pstore.h>
 #include <linux/vmalloc.h>
+#include <linux/mm.h> /* kvfree() */
 #include <acpi/apei.h>
 
 #include "apei-internal.h"
@@ -532,10 +533,7 @@ retry:
 			return -ENOMEM;
 		memcpy(new_entries, entries,
 		       erst_record_id_cache.len * sizeof(entries[0]));
-		if (erst_record_id_cache.size < PAGE_SIZE)
-			kfree(entries);
-		else
-			vfree(entries);
+		kvfree(entries);
 		erst_record_id_cache.entries = entries = new_entries;
 		erst_record_id_cache.size = new_size;
 	}
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index a212cefae524..891c42d1cd65 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -180,14 +180,15 @@ static void acpi_print_osc_error(acpi_handle handle,
 	int i;
 
 	if (ACPI_FAILURE(acpi_get_name(handle, ACPI_FULL_PATHNAME, &buffer)))
-		printk(KERN_DEBUG "%s\n", error);
+		printk(KERN_DEBUG "%s: %s\n", context->uuid_str, error);
 	else {
-		printk(KERN_DEBUG "%s:%s\n", (char *)buffer.pointer, error);
+		printk(KERN_DEBUG "%s (%s): %s\n",
+		       (char *)buffer.pointer, context->uuid_str, error);
 		kfree(buffer.pointer);
 	}
-	printk(KERN_DEBUG"_OSC request data:");
+	printk(KERN_DEBUG "_OSC request data:");
 	for (i = 0; i < context->cap.length; i += sizeof(u32))
-		printk("%x ", *((u32 *)(context->cap.pointer + i)));
+		printk(" %x", *((u32 *)(context->cap.pointer + i)));
 	printk("\n");
 }
 
@@ -1094,6 +1095,7 @@ static int __init acpi_init(void)
 	acpi_debugfs_init();
 	acpi_sleep_proc_init();
 	acpi_wakeup_device_init();
+	acpi_debugger_init();
 	return 0;
 }
 
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index 08a02cdc737c..cd2c3d6d40e0 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -22,6 +22,7 @@
 #include <linux/export.h>
 #include <linux/mutex.h>
 #include <linux/pm_qos.h>
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 
 #include "internal.h"
@@ -1059,7 +1060,7 @@ static void acpi_dev_pm_detach(struct device *dev, bool power_off)
 	struct acpi_device *adev = ACPI_COMPANION(dev);
 
 	if (adev && dev->pm_domain == &acpi_general_pm_domain) {
-		dev->pm_domain = NULL;
+		dev_pm_domain_set(dev, NULL);
 		acpi_remove_pm_notifier(adev);
 		if (power_off) {
 			/*
@@ -1111,7 +1112,7 @@ int acpi_dev_pm_attach(struct device *dev, bool power_on)
 		return -EBUSY;
 
 	acpi_add_pm_notifier(adev, dev, acpi_pm_notify_work_func);
-	dev->pm_domain = &acpi_general_pm_domain;
+	dev_pm_domain_set(dev, &acpi_general_pm_domain);
 	if (power_on) {
 		acpi_dev_pm_full_power(adev);
 		acpi_device_wakeup(adev, ACPI_STATE_S0, false);
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index e297a480e135..6322db64b4a4 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -339,7 +339,7 @@ static int acpi_fan_probe(struct platform_device *pdev)
 	} else {
 		result = acpi_device_update_power(device, NULL);
 		if (result) {
-			dev_err(&device->dev, "Setting initial power state\n");
+			dev_err(&device->dev, "Failed to set initial power state\n");
 			goto end;
 		}
 	}
diff --git a/drivers/acpi/gsi.c b/drivers/acpi/gsi.c
index fa4585a6914e..ee9e0f27b2bf 100644
--- a/drivers/acpi/gsi.c
+++ b/drivers/acpi/gsi.c
@@ -17,25 +17,6 @@ enum acpi_irq_model_id acpi_irq_model;
 
 static struct fwnode_handle *acpi_gsi_domain_id;
 
-static unsigned int acpi_gsi_get_irq_type(int trigger, int polarity)
-{
-	switch (polarity) {
-	case ACPI_ACTIVE_LOW:
-		return trigger == ACPI_EDGE_SENSITIVE ?
-		       IRQ_TYPE_EDGE_FALLING :
-		       IRQ_TYPE_LEVEL_LOW;
-	case ACPI_ACTIVE_HIGH:
-		return trigger == ACPI_EDGE_SENSITIVE ?
-		       IRQ_TYPE_EDGE_RISING :
-		       IRQ_TYPE_LEVEL_HIGH;
-	case ACPI_ACTIVE_BOTH:
-		if (trigger == ACPI_EDGE_SENSITIVE)
-			return IRQ_TYPE_EDGE_BOTH;
-	default:
-		return IRQ_TYPE_NONE;
-	}
-}
-
 /**
  * acpi_gsi_to_irq() - Retrieve the linux irq number for a given GSI
  * @gsi: GSI IRQ number to map
@@ -82,7 +63,7 @@ int acpi_register_gsi(struct device *dev, u32 gsi, int trigger,
 
 	fwspec.fwnode = acpi_gsi_domain_id;
 	fwspec.param[0] = gsi;
-	fwspec.param[1] = acpi_gsi_get_irq_type(trigger, polarity);
+	fwspec.param[1] = acpi_dev_get_irq_type(trigger, polarity);
 	fwspec.param_count = 2;
 
 	return irq_create_fwspec_mapping(&fwspec);
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 11d87bf67e73..1e6833a5cd44 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -86,6 +86,14 @@ bool acpi_scan_is_offline(struct acpi_device *adev, bool uevent);
 #define ACPI_STA_DEFAULT (ACPI_STA_DEVICE_PRESENT | ACPI_STA_DEVICE_ENABLED | \
 			  ACPI_STA_DEVICE_UI | ACPI_STA_DEVICE_FUNCTIONING)
 
+extern struct list_head acpi_bus_id_list;
+
+struct acpi_device_bus_id {
+	char bus_id[15];
+	unsigned int instance_no;
+	struct list_head node;
+};
+
 int acpi_device_add(struct acpi_device *device,
 		    void (*release)(struct device *));
 void acpi_init_device_object(struct acpi_device *device, acpi_handle handle,
diff --git a/drivers/acpi/nfit.c b/drivers/acpi/nfit.c
index aa45d4802707..35947ac87644 100644
--- a/drivers/acpi/nfit.c
+++ b/drivers/acpi/nfit.c
@@ -15,6 +15,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/ndctl.h>
+#include <linux/delay.h>
 #include <linux/list.h>
 #include <linux/acpi.h>
 #include <linux/sort.h>
@@ -468,37 +469,16 @@ static void nfit_mem_find_spa_bdw(struct acpi_nfit_desc *acpi_desc,
 	nfit_mem->bdw = NULL;
 }
 
-static int nfit_mem_add(struct acpi_nfit_desc *acpi_desc,
+static void nfit_mem_init_bdw(struct acpi_nfit_desc *acpi_desc,
 		struct nfit_mem *nfit_mem, struct acpi_nfit_system_address *spa)
 {
 	u16 dcr = __to_nfit_memdev(nfit_mem)->region_index;
 	struct nfit_memdev *nfit_memdev;
 	struct nfit_flush *nfit_flush;
-	struct nfit_dcr *nfit_dcr;
 	struct nfit_bdw *nfit_bdw;
 	struct nfit_idt *nfit_idt;
 	u16 idt_idx, range_index;
 
-	list_for_each_entry(nfit_dcr, &acpi_desc->dcrs, list) {
-		if (nfit_dcr->dcr->region_index != dcr)
-			continue;
-		nfit_mem->dcr = nfit_dcr->dcr;
-		break;
-	}
-
-	if (!nfit_mem->dcr) {
-		dev_dbg(acpi_desc->dev, "SPA %d missing:%s%s\n",
-				spa->range_index, __to_nfit_memdev(nfit_mem)
-				? "" : " MEMDEV", nfit_mem->dcr ? "" : " DCR");
-		return -ENODEV;
-	}
-
-	/*
-	 * We've found enough to create an nvdimm, optionally
-	 * find an associated BDW
-	 */
-	list_add(&nfit_mem->list, &acpi_desc->dimms);
-
 	list_for_each_entry(nfit_bdw, &acpi_desc->bdws, list) {
 		if (nfit_bdw->bdw->region_index != dcr)
 			continue;
@@ -507,12 +487,12 @@ static int nfit_mem_add(struct acpi_nfit_desc *acpi_desc,
 	}
 
 	if (!nfit_mem->bdw)
-		return 0;
+		return;
 
 	nfit_mem_find_spa_bdw(acpi_desc, nfit_mem);
 
 	if (!nfit_mem->spa_bdw)
-		return 0;
+		return;
 
 	range_index = nfit_mem->spa_bdw->range_index;
 	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
@@ -537,8 +517,6 @@ static int nfit_mem_add(struct acpi_nfit_desc *acpi_desc,
 		}
 		break;
 	}
-
-	return 0;
 }
 
 static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
@@ -547,7 +525,6 @@ static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
 	struct nfit_mem *nfit_mem, *found;
 	struct nfit_memdev *nfit_memdev;
 	int type = nfit_spa_type(spa);
-	u16 dcr;
 
 	switch (type) {
 	case NFIT_SPA_DCR:
@@ -558,14 +535,18 @@ static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
 	}
 
 	list_for_each_entry(nfit_memdev, &acpi_desc->memdevs, list) {
-		int rc;
+		struct nfit_dcr *nfit_dcr;
+		u32 device_handle;
+		u16 dcr;
 
 		if (nfit_memdev->memdev->range_index != spa->range_index)
 			continue;
 		found = NULL;
 		dcr = nfit_memdev->memdev->region_index;
+		device_handle = nfit_memdev->memdev->device_handle;
 		list_for_each_entry(nfit_mem, &acpi_desc->dimms, list)
-			if (__to_nfit_memdev(nfit_mem)->region_index == dcr) {
+			if (__to_nfit_memdev(nfit_mem)->device_handle
+					== device_handle) {
 				found = nfit_mem;
 				break;
 			}
@@ -578,6 +559,31 @@ static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
 			if (!nfit_mem)
 				return -ENOMEM;
 			INIT_LIST_HEAD(&nfit_mem->list);
+			list_add(&nfit_mem->list, &acpi_desc->dimms);
+		}
+
+		list_for_each_entry(nfit_dcr, &acpi_desc->dcrs, list) {
+			if (nfit_dcr->dcr->region_index != dcr)
+				continue;
+			/*
+			 * Record the control region for the dimm.  For
+			 * the ACPI 6.1 case, where there are separate
+			 * control regions for the pmem vs blk
+			 * interfaces, be sure to record the extended
+			 * blk details.
+			 */
+			if (!nfit_mem->dcr)
+				nfit_mem->dcr = nfit_dcr->dcr;
+			else if (nfit_mem->dcr->windows == 0
+					&& nfit_dcr->dcr->windows)
+				nfit_mem->dcr = nfit_dcr->dcr;
+			break;
+		}
+
+		if (dcr && !nfit_mem->dcr) {
+			dev_err(acpi_desc->dev, "SPA %d missing DCR %d\n",
+					spa->range_index, dcr);
+			return -ENODEV;
 		}
 
 		if (type == NFIT_SPA_DCR) {
@@ -594,6 +600,7 @@ static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
 				nfit_mem->idt_dcr = nfit_idt->idt;
 				break;
 			}
+			nfit_mem_init_bdw(acpi_desc, nfit_mem, spa);
 		} else {
 			/*
 			 * A single dimm may belong to multiple SPA-PM
@@ -602,13 +609,6 @@ static int nfit_mem_dcr_init(struct acpi_nfit_desc *acpi_desc,
 			 */
 			nfit_mem->memdev_pmem = nfit_memdev->memdev;
 		}
-
-		if (found)
-			continue;
-
-		rc = nfit_mem_add(acpi_desc, nfit_mem, spa);
-		if (rc)
-			return rc;
 	}
 
 	return 0;
@@ -1473,6 +1473,209 @@ static void acpi_nfit_blk_region_disable(struct nvdimm_bus *nvdimm_bus,
 	/* devm will free nfit_blk */
 }
 
+static int ars_get_cap(struct nvdimm_bus_descriptor *nd_desc,
+		struct nd_cmd_ars_cap *cmd, u64 addr, u64 length)
+{
+	cmd->address = addr;
+	cmd->length = length;
+
+	return nd_desc->ndctl(nd_desc, NULL, ND_CMD_ARS_CAP, cmd,
+			sizeof(*cmd));
+}
+
+static int ars_do_start(struct nvdimm_bus_descriptor *nd_desc,
+		struct nd_cmd_ars_start *cmd, u64 addr, u64 length)
+{
+	int rc;
+
+	cmd->address = addr;
+	cmd->length = length;
+	cmd->type = ND_ARS_PERSISTENT;
+
+	while (1) {
+		rc = nd_desc->ndctl(nd_desc, NULL, ND_CMD_ARS_START, cmd,
+				sizeof(*cmd));
+		if (rc)
+			return rc;
+		switch (cmd->status) {
+		case 0:
+			return 0;
+		case 1:
+			/* ARS unsupported, but we should never get here */
+			return 0;
+		case 6:
+			/* ARS is in progress */
+			msleep(1000);
+			break;
+		default:
+			return -ENXIO;
+		}
+	}
+}
+
+static int ars_get_status(struct nvdimm_bus_descriptor *nd_desc,
+		struct nd_cmd_ars_status *cmd, u32 size)
+{
+	int rc;
+
+	while (1) {
+		rc = nd_desc->ndctl(nd_desc, NULL, ND_CMD_ARS_STATUS, cmd,
+			size);
+		if (rc || cmd->status & 0xffff)
+			return -ENXIO;
+
+		/* Check extended status (Upper two bytes) */
+		switch (cmd->status >> 16) {
+		case 0:
+			return 0;
+		case 1:
+			/* ARS is in progress */
+			msleep(1000);
+			break;
+		case 2:
+			/* No ARS performed for the current boot */
+			return 0;
+		case 3:
+			/* TODO: error list overflow support */
+		default:
+			return -ENXIO;
+		}
+	}
+}
+
+static int ars_status_process_records(struct nvdimm_bus *nvdimm_bus,
+		struct nd_cmd_ars_status *ars_status, u64 start)
+{
+	int rc;
+	u32 i;
+
+	/*
+	 * The address field returned by ars_status should be either
+	 * less than or equal to the address we last started ARS for.
+	 * The (start, length) returned by ars_status should also have
+	 * non-zero overlap with the range we started ARS for.
+	 * If this is not the case, bail.
+	 */
+	if (ars_status->address > start ||
+			(ars_status->address + ars_status->length < start))
+		return -ENXIO;
+
+	for (i = 0; i < ars_status->num_records; i++) {
+		rc = nvdimm_bus_add_poison(nvdimm_bus,
+				ars_status->records[i].err_address,
+				ars_status->records[i].length);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static int acpi_nfit_find_poison(struct acpi_nfit_desc *acpi_desc,
+		struct nd_region_desc *ndr_desc)
+{
+	struct nvdimm_bus_descriptor *nd_desc = &acpi_desc->nd_desc;
+	struct nvdimm_bus *nvdimm_bus = acpi_desc->nvdimm_bus;
+	struct nd_cmd_ars_status *ars_status = NULL;
+	struct nd_cmd_ars_start *ars_start = NULL;
+	struct nd_cmd_ars_cap *ars_cap = NULL;
+	u64 start, len, cur, remaining;
+	u32 ars_status_size;
+	int rc;
+
+	ars_cap = kzalloc(sizeof(*ars_cap), GFP_KERNEL);
+	if (!ars_cap)
+		return -ENOMEM;
+
+	start = ndr_desc->res->start;
+	len = ndr_desc->res->end - ndr_desc->res->start + 1;
+
+	/*
+	 * If ARS is unimplemented, unsupported, or if the 'Persistent Memory
+	 * Scrub' flag in extended status is not set, skip this but continue
+	 * initialization
+	 */
+	rc = ars_get_cap(nd_desc, ars_cap, start, len);
+	if (rc == -ENOTTY) {
+		dev_dbg(acpi_desc->dev,
+			"Address Range Scrub is not implemented, won't create an error list\n");
+		rc = 0;
+		goto out;
+	}
+	if (rc)
+		goto out;
+
+	if ((ars_cap->status & 0xffff) ||
+		!(ars_cap->status >> 16 & ND_ARS_PERSISTENT)) {
+		dev_warn(acpi_desc->dev,
+			"ARS unsupported (status: 0x%x), won't create an error list\n",
+			ars_cap->status);
+		goto out;
+	}
+
+	/*
+	 * Check if a full-range ARS has been run. If so, use those results
+	 * without having to start a new ARS.
+	 */
+	ars_status_size = ars_cap->max_ars_out;
+	ars_status = kzalloc(ars_status_size, GFP_KERNEL);
+	if (!ars_status) {
+		rc = -ENOMEM;
+		goto out;
+	}
+
+	rc = ars_get_status(nd_desc, ars_status, ars_status_size);
+	if (rc)
+		goto out;
+
+	if (ars_status->address <= start &&
+		(ars_status->address + ars_status->length >= start + len)) {
+		rc = ars_status_process_records(nvdimm_bus, ars_status, start);
+		goto out;
+	}
+
+	/*
+	 * ARS_STATUS can overflow if the number of poison entries found is
+	 * greater than the maximum buffer size (ars_cap->max_ars_out)
+	 * To detect overflow, check if the length field of ars_status
+	 * is less than the length we supplied. If so, process the
+	 * error entries we got, adjust the start point, and start again
+	 */
+	ars_start = kzalloc(sizeof(*ars_start), GFP_KERNEL);
+	if (!ars_start)
+		return -ENOMEM;
+
+	cur = start;
+	remaining = len;
+	do {
+		u64 done, end;
+
+		rc = ars_do_start(nd_desc, ars_start, cur, remaining);
+		if (rc)
+			goto out;
+
+		rc = ars_get_status(nd_desc, ars_status, ars_status_size);
+		if (rc)
+			goto out;
+
+		rc = ars_status_process_records(nvdimm_bus, ars_status, cur);
+		if (rc)
+			goto out;
+
+		end = min(cur + remaining,
+			ars_status->address + ars_status->length);
+		done = end - cur;
+		cur += done;
+		remaining -= done;
+	} while (remaining);
+
+ out:
+	kfree(ars_cap);
+	kfree(ars_start);
+	kfree(ars_status);
+	return rc;
+}
+
 static int acpi_nfit_init_mapping(struct acpi_nfit_desc *acpi_desc,
 		struct nd_mapping *nd_mapping, struct nd_region_desc *ndr_desc,
 		struct acpi_nfit_memory_map *memdev,
@@ -1585,6 +1788,13 @@ static int acpi_nfit_register_region(struct acpi_nfit_desc *acpi_desc,
 
 	nvdimm_bus = acpi_desc->nvdimm_bus;
 	if (nfit_spa_type(spa) == NFIT_SPA_PM) {
+		rc = acpi_nfit_find_poison(acpi_desc, ndr_desc);
+		if (rc) {
+			dev_err(acpi_desc->dev,
+				"error while performing ARS to find poison: %d\n",
+				rc);
+			return rc;
+		}
 		if (!nvdimm_pmem_region_create(nvdimm_bus, ndr_desc))
 			return -ENOMEM;
 	} else if (nfit_spa_type(spa) == NFIT_SPA_VOLATILE) {
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 32d684af0ec7..67da6fb72274 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -220,6 +220,7 @@ void acpi_os_printf(const char *fmt, ...)
 	acpi_os_vprintf(fmt, args);
 	va_end(args);
 }
+EXPORT_SYMBOL(acpi_os_printf);
 
 void acpi_os_vprintf(const char *fmt, va_list args)
 {
@@ -234,7 +235,8 @@ void acpi_os_vprintf(const char *fmt, va_list args)
 		printk(KERN_CONT "%s", buffer);
 	}
 #else
-	printk(KERN_CONT "%s", buffer);
+	if (acpi_debugger_write_log(buffer) < 0)
+		printk(KERN_CONT "%s", buffer);
 #endif
 }
 
@@ -364,6 +366,19 @@ static void acpi_unmap(acpi_physical_address pg_off, void __iomem *vaddr)
 		iounmap(vaddr);
 }
 
+/**
+ * acpi_os_map_iomem - Get a virtual address for a given physical address range.
+ * @phys: Start of the physical address range to map.
+ * @size: Size of the physical address range to map.
+ *
+ * Look up the given physical address range in the list of existing ACPI memory
+ * mappings.  If found, get a reference to it and return a pointer to it (its
+ * virtual address).  If not found, map it, add it to that list and return a
+ * pointer to it.
+ *
+ * During early init (when acpi_gbl_permanent_mmap has not been set yet) this
+ * routine simply calls __acpi_map_table() to get the job done.
+ */
 void __iomem *__init_refok
 acpi_os_map_iomem(acpi_physical_address phys, acpi_size size)
 {
@@ -439,6 +454,20 @@ static void acpi_os_map_cleanup(struct acpi_ioremap *map)
 	}
 }
 
+/**
+ * acpi_os_unmap_iomem - Drop a memory mapping reference.
+ * @virt: Start of the address range to drop a reference to.
+ * @size: Size of the address range to drop a reference to.
+ *
+ * Look up the given virtual address range in the list of existing ACPI memory
+ * mappings, drop a reference to it and unmap it if there are no more active
+ * references to it.
+ *
+ * During early init (when acpi_gbl_permanent_mmap has not been set yet) this
+ * routine simply calls __acpi_unmap_table() to get the job done.  Since
+ * __acpi_unmap_table() is an __init function, the __ref annotation is needed
+ * here.
+ */
 void __ref acpi_os_unmap_iomem(void __iomem *virt, acpi_size size)
 {
 	struct acpi_ioremap *map;
@@ -1101,6 +1130,200 @@ static void acpi_os_execute_deferred(struct work_struct *work)
 	kfree(dpc);
 }
 
+#ifdef CONFIG_ACPI_DEBUGGER
+static struct acpi_debugger acpi_debugger;
+static bool acpi_debugger_initialized;
+
+int acpi_register_debugger(struct module *owner,
+			   const struct acpi_debugger_ops *ops)
+{
+	int ret = 0;
+
+	mutex_lock(&acpi_debugger.lock);
+	if (acpi_debugger.ops) {
+		ret = -EBUSY;
+		goto err_lock;
+	}
+
+	acpi_debugger.owner = owner;
+	acpi_debugger.ops = ops;
+
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+EXPORT_SYMBOL(acpi_register_debugger);
+
+void acpi_unregister_debugger(const struct acpi_debugger_ops *ops)
+{
+	mutex_lock(&acpi_debugger.lock);
+	if (ops == acpi_debugger.ops) {
+		acpi_debugger.ops = NULL;
+		acpi_debugger.owner = NULL;
+	}
+	mutex_unlock(&acpi_debugger.lock);
+}
+EXPORT_SYMBOL(acpi_unregister_debugger);
+
+int acpi_debugger_create_thread(acpi_osd_exec_callback function, void *context)
+{
+	int ret;
+	int (*func)(acpi_osd_exec_callback, void *);
+	struct module *owner;
+
+	if (!acpi_debugger_initialized)
+		return -ENODEV;
+	mutex_lock(&acpi_debugger.lock);
+	if (!acpi_debugger.ops) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	if (!try_module_get(acpi_debugger.owner)) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	func = acpi_debugger.ops->create_thread;
+	owner = acpi_debugger.owner;
+	mutex_unlock(&acpi_debugger.lock);
+
+	ret = func(function, context);
+
+	mutex_lock(&acpi_debugger.lock);
+	module_put(owner);
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+
+ssize_t acpi_debugger_write_log(const char *msg)
+{
+	ssize_t ret;
+	ssize_t (*func)(const char *);
+	struct module *owner;
+
+	if (!acpi_debugger_initialized)
+		return -ENODEV;
+	mutex_lock(&acpi_debugger.lock);
+	if (!acpi_debugger.ops) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	if (!try_module_get(acpi_debugger.owner)) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	func = acpi_debugger.ops->write_log;
+	owner = acpi_debugger.owner;
+	mutex_unlock(&acpi_debugger.lock);
+
+	ret = func(msg);
+
+	mutex_lock(&acpi_debugger.lock);
+	module_put(owner);
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+
+ssize_t acpi_debugger_read_cmd(char *buffer, size_t buffer_length)
+{
+	ssize_t ret;
+	ssize_t (*func)(char *, size_t);
+	struct module *owner;
+
+	if (!acpi_debugger_initialized)
+		return -ENODEV;
+	mutex_lock(&acpi_debugger.lock);
+	if (!acpi_debugger.ops) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	if (!try_module_get(acpi_debugger.owner)) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	func = acpi_debugger.ops->read_cmd;
+	owner = acpi_debugger.owner;
+	mutex_unlock(&acpi_debugger.lock);
+
+	ret = func(buffer, buffer_length);
+
+	mutex_lock(&acpi_debugger.lock);
+	module_put(owner);
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+
+int acpi_debugger_wait_command_ready(void)
+{
+	int ret;
+	int (*func)(bool, char *, size_t);
+	struct module *owner;
+
+	if (!acpi_debugger_initialized)
+		return -ENODEV;
+	mutex_lock(&acpi_debugger.lock);
+	if (!acpi_debugger.ops) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	if (!try_module_get(acpi_debugger.owner)) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	func = acpi_debugger.ops->wait_command_ready;
+	owner = acpi_debugger.owner;
+	mutex_unlock(&acpi_debugger.lock);
+
+	ret = func(acpi_gbl_method_executing,
+		   acpi_gbl_db_line_buf, ACPI_DB_LINE_BUFFER_SIZE);
+
+	mutex_lock(&acpi_debugger.lock);
+	module_put(owner);
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+
+int acpi_debugger_notify_command_complete(void)
+{
+	int ret;
+	int (*func)(void);
+	struct module *owner;
+
+	if (!acpi_debugger_initialized)
+		return -ENODEV;
+	mutex_lock(&acpi_debugger.lock);
+	if (!acpi_debugger.ops) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	if (!try_module_get(acpi_debugger.owner)) {
+		ret = -ENODEV;
+		goto err_lock;
+	}
+	func = acpi_debugger.ops->notify_command_complete;
+	owner = acpi_debugger.owner;
+	mutex_unlock(&acpi_debugger.lock);
+
+	ret = func();
+
+	mutex_lock(&acpi_debugger.lock);
+	module_put(owner);
+err_lock:
+	mutex_unlock(&acpi_debugger.lock);
+	return ret;
+}
+
+int __init acpi_debugger_init(void)
+{
+	mutex_init(&acpi_debugger.lock);
+	acpi_debugger_initialized = true;
+	return 0;
+}
+#endif
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_os_execute
@@ -1127,6 +1350,15 @@ acpi_status acpi_os_execute(acpi_execute_type type,
 			  "Scheduling function [%p(%p)] for deferred execution.\n",
 			  function, context));
 
+	if (type == OSL_DEBUGGER_MAIN_THREAD) {
+		ret = acpi_debugger_create_thread(function, context);
+		if (ret) {
+			pr_err("Call to kthread_create() failed.\n");
+			status = AE_ERROR;
+		}
+		goto out_thread;
+	}
+
 	/*
 	 * Allocate/initialize DPC structure.  Note that this memory will be
 	 * freed by the callee.  The kernel handles the work_struct list  in a
@@ -1151,11 +1383,17 @@ acpi_status acpi_os_execute(acpi_execute_type type,
 	if (type == OSL_NOTIFY_HANDLER) {
 		queue = kacpi_notify_wq;
 		INIT_WORK(&dpc->work, acpi_os_execute_deferred);
-	} else {
+	} else if (type == OSL_GPE_HANDLER) {
 		queue = kacpid_wq;
 		INIT_WORK(&dpc->work, acpi_os_execute_deferred);
+	} else {
+		pr_err("Unsupported os_execute type %d.\n", type);
+		status = AE_ERROR;
 	}
 
+	if (ACPI_FAILURE(status))
+		goto err_workqueue;
+
 	/*
 	 * On some machines, a software-initiated SMI causes corruption unless
 	 * the SMI runs on CPU 0.  An SMI can be initiated by any AML, but
@@ -1164,13 +1402,15 @@ acpi_status acpi_os_execute(acpi_execute_type type,
 	 * queueing on CPU 0.
 	 */
 	ret = queue_work_on(0, queue, &dpc->work);
-
 	if (!ret) {
 		printk(KERN_ERR PREFIX
 			  "Call to queue_work() failed.\n");
 		status = AE_ERROR;
-		kfree(dpc);
 	}
+err_workqueue:
+	if (ACPI_FAILURE(status))
+		kfree(dpc);
+out_thread:
 	return status;
 }
 EXPORT_SYMBOL(acpi_os_execute);
@@ -1358,10 +1598,39 @@ acpi_status acpi_os_get_line(char *buffer, u32 buffer_length, u32 *bytes_read)
 		chars = strlen(buffer) - 1;
 		buffer[chars] = '\0';
 	}
+#else
+	int ret;
+
+	ret = acpi_debugger_read_cmd(buffer, buffer_length);
+	if (ret < 0)
+		return AE_ERROR;
+	if (bytes_read)
+		*bytes_read = ret;
 #endif
 
 	return AE_OK;
 }
+EXPORT_SYMBOL(acpi_os_get_line);
+
+acpi_status acpi_os_wait_command_ready(void)
+{
+	int ret;
+
+	ret = acpi_debugger_wait_command_ready();
+	if (ret < 0)
+		return AE_ERROR;
+	return AE_OK;
+}
+
+acpi_status acpi_os_notify_command_complete(void)
+{
+	int ret;
+
+	ret = acpi_debugger_notify_command_complete();
+	if (ret < 0)
+		return AE_ERROR;
+	return AE_OK;
+}
 
 acpi_status acpi_os_signal(u32 function, void *info)
 {
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index c9336751e5e3..c8e169e46673 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -131,9 +131,6 @@ static void do_prt_fixups(struct acpi_prt_entry *entry,
 		quirk = &prt_quirks[i];
 
 		/* All current quirks involve link devices, not GSIs */
-		if (!prt->source)
-			continue;
-
 		if (dmi_check_system(quirk->system) &&
 		    entry->id.segment == quirk->segment &&
 		    entry->id.bus == quirk->bus &&
@@ -409,7 +406,7 @@ int acpi_pci_irq_enable(struct pci_dev *dev)
 		return 0;
 	}
 
-	if (pci_has_managed_irq(dev))
+	if (dev->irq_managed && dev->irq > 0)
 		return 0;
 
 	entry = acpi_pci_irq_lookup(dev, pin);
@@ -454,7 +451,8 @@ int acpi_pci_irq_enable(struct pci_dev *dev)
 		kfree(entry);
 		return rc;
 	}
-	pci_set_managed_irq(dev, rc);
+	dev->irq = rc;
+	dev->irq_managed = 1;
 
 	if (link)
 		snprintf(link_desc, sizeof(link_desc), " -> Link[%s]", link);
@@ -477,8 +475,16 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	u8 pin;
 
 	pin = dev->pin;
-	if (!pin || !pci_has_managed_irq(dev))
+	if (!pin || !dev->irq_managed || dev->irq <= 0)
+		return;
+
+	/* Keep IOAPIC pin configuration when suspending */
+	if (dev->dev.power.is_prepared)
 		return;
+#ifdef	CONFIG_PM
+	if (dev->dev.power.runtime_status == RPM_SUSPENDING)
+		return;
+#endif
 
 	entry = acpi_pci_irq_lookup(dev, pin);
 	if (!entry)
@@ -499,6 +505,6 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	dev_dbg(&dev->dev, "PCI INT %c disabled\n", pin_name(pin));
 	if (gsi >= 0) {
 		acpi_unregister_gsi(gsi);
-		pci_reset_managed_irq(dev);
+		dev->irq_managed = 0;
 	}
 }
diff --git a/drivers/acpi/pci_link.c b/drivers/acpi/pci_link.c
index 7c8408b946ca..ededa909df2f 100644
--- a/drivers/acpi/pci_link.c
+++ b/drivers/acpi/pci_link.c
@@ -67,12 +67,12 @@ static struct acpi_scan_handler pci_link_handler = {
  * later even the link is disable. Instead, we just repick the active irq
  */
 struct acpi_pci_link_irq {
-	u8 active;		/* Current IRQ */
+	u32 active;		/* Current IRQ */
 	u8 triggering;		/* All IRQs */
 	u8 polarity;		/* All IRQs */
 	u8 resource_type;
 	u8 possible_count;
-	u8 possible[ACPI_PCI_LINK_MAX_POSSIBLE];
+	u32 possible[ACPI_PCI_LINK_MAX_POSSIBLE];
 	u8 initialized:1;
 	u8 reserved:7;
 };
diff --git a/drivers/acpi/property.c b/drivers/acpi/property.c
index 88f4306744c0..2aee41655ce9 100644
--- a/drivers/acpi/property.c
+++ b/drivers/acpi/property.c
@@ -346,7 +346,7 @@ void acpi_free_properties(struct acpi_device *adev)
  *
  * Return: %0 if property with @name has been found (success),
  *         %-EINVAL if the arguments are invalid,
- *         %-ENODATA if the property doesn't exist,
+ *         %-EINVAL if the property doesn't exist,
  *         %-EPROTO if the property value type doesn't match @type.
  */
 static int acpi_data_get_property(struct acpi_device_data *data,
@@ -360,7 +360,7 @@ static int acpi_data_get_property(struct acpi_device_data *data,
 		return -EINVAL;
 
 	if (!data->pointer || !data->properties)
-		return -ENODATA;
+		return -EINVAL;
 
 	properties = data->properties;
 	for (i = 0; i < properties->package.count; i++) {
@@ -375,13 +375,13 @@ static int acpi_data_get_property(struct acpi_device_data *data,
 		if (!strcmp(name, propname->string.pointer)) {
 			if (type != ACPI_TYPE_ANY && propvalue->type != type)
 				return -EPROTO;
-			else if (obj)
+			if (obj)
 				*obj = propvalue;
 
 			return 0;
 		}
 	}
-	return -ENODATA;
+	return -EINVAL;
 }
 
 /**
@@ -439,7 +439,7 @@ int acpi_node_prop_get(struct fwnode_handle *fwnode, const char *propname,
  *
  * Return: %0 if array property (package) with @name has been found (success),
  *         %-EINVAL if the arguments are invalid,
- *         %-ENODATA if the property doesn't exist,
+ *         %-EINVAL if the property doesn't exist,
  *         %-EPROTO if the property is not a package or the type of its elements
  *           doesn't match @type.
  */
diff --git a/drivers/acpi/resource.c b/drivers/acpi/resource.c
index cdc5c2599beb..d02fd53042a5 100644
--- a/drivers/acpi/resource.c
+++ b/drivers/acpi/resource.c
@@ -23,6 +23,7 @@
 #include <linux/export.h>
 #include <linux/ioport.h>
 #include <linux/slab.h>
+#include <linux/irq.h>
 
 #ifdef CONFIG_X86
 #define valid_IRQ(i) (((i) != 0) && ((i) != 2))
@@ -336,6 +337,31 @@ unsigned long acpi_dev_irq_flags(u8 triggering, u8 polarity, u8 shareable)
 }
 EXPORT_SYMBOL_GPL(acpi_dev_irq_flags);
 
+/**
+ * acpi_dev_get_irq_type - Determine irq type.
+ * @triggering: Triggering type as provided by ACPI.
+ * @polarity: Interrupt polarity as provided by ACPI.
+ */
+unsigned int acpi_dev_get_irq_type(int triggering, int polarity)
+{
+	switch (polarity) {
+	case ACPI_ACTIVE_LOW:
+		return triggering == ACPI_EDGE_SENSITIVE ?
+		       IRQ_TYPE_EDGE_FALLING :
+		       IRQ_TYPE_LEVEL_LOW;
+	case ACPI_ACTIVE_HIGH:
+		return triggering == ACPI_EDGE_SENSITIVE ?
+		       IRQ_TYPE_EDGE_RISING :
+		       IRQ_TYPE_LEVEL_HIGH;
+	case ACPI_ACTIVE_BOTH:
+		if (triggering == ACPI_EDGE_SENSITIVE)
+			return IRQ_TYPE_EDGE_BOTH;
+	default:
+		return IRQ_TYPE_NONE;
+	}
+}
+EXPORT_SYMBOL_GPL(acpi_dev_get_irq_type);
+
 static void acpi_dev_irqresource_disabled(struct resource *res, u32 gsi)
 {
 	res->start = gsi;
diff --git a/drivers/acpi/sbs.c b/drivers/acpi/sbs.c
index cb3dedb1beae..ad0b13ad4bbb 100644
--- a/drivers/acpi/sbs.c
+++ b/drivers/acpi/sbs.c
@@ -417,11 +417,11 @@ static int acpi_battery_set_alarm(struct acpi_battery *battery)
 		if ((value & 0xf000) != sel) {
 			value &= 0x0fff;
 			value |= sel;
-		ret = acpi_smbus_write(sbs->hc, SMBUS_WRITE_WORD,
+			ret = acpi_smbus_write(sbs->hc, SMBUS_WRITE_WORD,
 					 ACPI_SBS_MANAGER,
 					 0x01, (u8 *)&value, 2);
-		if (ret)
-			goto end;
+			if (ret)
+				goto end;
 		}
 	}
 	ret = acpi_smbus_write(sbs->hc, SMBUS_WRITE_WORD, ACPI_SBS_BATTERY,
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 78d5f02a073b..407a3760e8de 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -39,7 +39,7 @@ static const char *dummy_hid = "device";
 
 static LIST_HEAD(acpi_dep_list);
 static DEFINE_MUTEX(acpi_dep_list_lock);
-static LIST_HEAD(acpi_bus_id_list);
+LIST_HEAD(acpi_bus_id_list);
 static DEFINE_MUTEX(acpi_scan_lock);
 static LIST_HEAD(acpi_scan_handlers_list);
 DEFINE_MUTEX(acpi_device_lock);
@@ -52,12 +52,6 @@ struct acpi_dep_data {
 	acpi_handle slave;
 };
 
-struct acpi_device_bus_id{
-	char bus_id[15];
-	unsigned int instance_no;
-	struct list_head node;
-};
-
 void acpi_scan_lock_acquire(void)
 {
 	mutex_lock(&acpi_scan_lock);
@@ -471,10 +465,24 @@ static void acpi_device_release(struct device *dev)
 
 static void acpi_device_del(struct acpi_device *device)
 {
+	struct acpi_device_bus_id *acpi_device_bus_id;
+
 	mutex_lock(&acpi_device_lock);
 	if (device->parent)
 		list_del(&device->node);
 
+	list_for_each_entry(acpi_device_bus_id, &acpi_bus_id_list, node)
+		if (!strcmp(acpi_device_bus_id->bus_id,
+			    acpi_device_hid(device))) {
+			if (acpi_device_bus_id->instance_no > 0)
+				acpi_device_bus_id->instance_no--;
+			else {
+				list_del(&acpi_device_bus_id->node);
+				kfree(acpi_device_bus_id);
+			}
+			break;
+		}
+
 	list_del(&device->wakeup_list);
 	mutex_unlock(&acpi_device_lock);
 
@@ -1461,7 +1469,7 @@ static int acpi_bus_type_and_status(acpi_handle handle, int *type,
 		*type = ACPI_BUS_TYPE_DEVICE;
 		status = acpi_bus_get_status_handle(handle, sta);
 		if (ACPI_FAILURE(status))
-			return -ENODEV;
+			*sta = 0;
 		break;
 	case ACPI_TYPE_PROCESSOR:
 		*type = ACPI_BUS_TYPE_PROCESSOR;
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 0d94621dc856..9cb975200cac 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -61,7 +61,7 @@ static int acpi_sleep_prepare(u32 acpi_state)
 	if (acpi_state == ACPI_STATE_S3) {
 		if (!acpi_wakeup_address)
 			return -EFAULT;
-		acpi_set_firmware_waking_vector(acpi_wakeup_address);
+		acpi_set_waking_vector(acpi_wakeup_address);
 
 	}
 	ACPI_FLUSH_CPU_CACHE();
@@ -410,7 +410,7 @@ static void acpi_pm_finish(void)
 	acpi_leave_sleep_state(acpi_state);
 
 	/* reset firmware waking vector */
-	acpi_set_firmware_waking_vector((acpi_physical_address) 0);
+	acpi_set_waking_vector(0);
 
 	acpi_target_sleep_state = ACPI_STATE_S0;
 
diff --git a/drivers/acpi/sleep.h b/drivers/acpi/sleep.h
index c797ffa568d5..a9cc34e663f9 100644
--- a/drivers/acpi/sleep.h
+++ b/drivers/acpi/sleep.h
@@ -6,3 +6,9 @@ extern struct list_head acpi_wakeup_device_list;
 extern struct mutex acpi_device_lock;
 
 extern void acpi_resume_power_resources(void);
+
+static inline acpi_status acpi_set_waking_vector(u32 wakeup_address)
+{
+	return acpi_set_firmware_waking_vector(
+				(acpi_physical_address)wakeup_address, 0);
+}
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index 475c9079bf85..f2f9873bb5c3 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -29,6 +29,7 @@
 #include <linux/dynamic_debug.h>
 
 #include "internal.h"
+#include "sleep.h"
 
 #define _COMPONENT		ACPI_BUS_COMPONENT
 ACPI_MODULE_NAME("utils");
@@ -709,6 +710,36 @@ bool acpi_check_dsm(acpi_handle handle, const u8 *uuid, int rev, u64 funcs)
 }
 EXPORT_SYMBOL(acpi_check_dsm);
 
+/**
+ * acpi_dev_present - Detect presence of a given ACPI device in the system.
+ * @hid: Hardware ID of the device.
+ *
+ * Return %true if the device was present at the moment of invocation.
+ * Note that if the device is pluggable, it may since have disappeared.
+ *
+ * For this function to work, acpi_bus_scan() must have been executed
+ * which happens in the subsys_initcall() subsection. Hence, do not
+ * call from a subsys_initcall() or earlier (use acpi_get_devices()
+ * instead). Calling from module_init() is fine (which is synonymous
+ * with device_initcall()).
+ */
+bool acpi_dev_present(const char *hid)
+{
+	struct acpi_device_bus_id *acpi_device_bus_id;
+	bool found = false;
+
+	mutex_lock(&acpi_device_lock);
+	list_for_each_entry(acpi_device_bus_id, &acpi_bus_id_list, node)
+		if (!strcmp(acpi_device_bus_id->bus_id, hid)) {
+			found = true;
+			break;
+		}
+	mutex_unlock(&acpi_device_lock);
+
+	return found;
+}
+EXPORT_SYMBOL(acpi_dev_present);
+
 /*
  * acpi_backlight= handling, this is done here rather then in video_detect.c
  * because __setup cannot be used in modules.
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index daaf1c4e1e0f..1316ddd92fac 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -135,14 +135,6 @@ static const struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "UL30A"),
 		},
 	},
-	{
-	.callback = video_detect_force_vendor,
-	.ident = "Dell Inspiron 5737",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 5737"),
-		},
-	},
 
 	/*
 	 * These models have a working acpi_video backlight control, and using
@@ -250,6 +242,15 @@ static const struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "XPS L521X"),
 		},
 	},
+	{
+	 /* https://bugzilla.kernel.org/show_bug.cgi?id=108971 */
+	 .callback = video_detect_force_video,
+	 .ident = "SAMSUNG 530U4E/540U4E",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "SAMSUNG ELECTRONICS CO., LTD."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "530U4E/540U4E"),
+		},
+	},
 
 	/* Non win8 machines which need native backlight nevertheless */
 	{
@@ -279,6 +280,14 @@ static const struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "MacBookPro12,1"),
 		},
 	},
+	{
+	 .callback = video_detect_force_native,
+	 .ident = "Dell Vostro V131",
+	 .matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		DMI_MATCH(DMI_PRODUCT_NAME, "Vostro V131"),
+		},
+	},
 	{ },
 };
 
diff --git a/drivers/amba/Kconfig b/drivers/amba/Kconfig
index 4a5c9d279059..294ba6f36396 100644
--- a/drivers/amba/Kconfig
+++ b/drivers/amba/Kconfig
@@ -4,7 +4,7 @@ config ARM_AMBA
 if ARM_AMBA
 
 config TEGRA_AHB
-	bool "Enable AHB driver for NVIDIA Tegra SoCs"
+	bool
 	default y if ARCH_TEGRA
 	help
 	  Adds AHB configuration functionality for NVIDIA Tegra SoCs,
diff --git a/drivers/android/binder.c b/drivers/android/binder.c
index a39e85f9efa9..7d00b7a015ea 100644
--- a/drivers/android/binder.c
+++ b/drivers/android/binder.c
@@ -2074,7 +2074,7 @@ static int binder_thread_write(struct binder_proc *proc,
 			if (get_user(cookie, (binder_uintptr_t __user *)ptr))
 				return -EFAULT;
 
-			ptr += sizeof(void *);
+			ptr += sizeof(cookie);
 			list_for_each_entry(w, &proc->delivered_death, entry) {
 				struct binder_ref_death *tmp_death = container_of(w, struct binder_ref_death, work);
 
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index 6aaa3f81755b..861643ea91b5 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -100,7 +100,7 @@ config SATA_AHCI_PLATFORM
 
 config AHCI_BRCMSTB
 	tristate "Broadcom STB AHCI SATA support"
-	depends on ARCH_BRCMSTB
+	depends on ARCH_BRCMSTB || BMIPS_GENERIC
 	help
 	  This option enables support for the AHCI SATA3 controller found on
 	  STB SoC's.
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index cdfbcc54821f..146dc0b8ec61 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -264,6 +264,26 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, 0x3b2b), board_ahci }, /* PCH RAID */
 	{ PCI_VDEVICE(INTEL, 0x3b2c), board_ahci }, /* PCH RAID */
 	{ PCI_VDEVICE(INTEL, 0x3b2f), board_ahci }, /* PCH AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b0), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b1), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b2), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b3), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b4), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b5), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b6), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19b7), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19bE), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19bF), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c0), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c1), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c2), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c3), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c4), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c5), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c6), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19c7), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19cE), board_ahci }, /* DNV AHCI */
+	{ PCI_VDEVICE(INTEL, 0x19cF), board_ahci }, /* DNV AHCI */
 	{ PCI_VDEVICE(INTEL, 0x1c02), board_ahci }, /* CPT AHCI */
 	{ PCI_VDEVICE(INTEL, 0x1c03), board_ahci }, /* CPT AHCI */
 	{ PCI_VDEVICE(INTEL, 0x1c04), board_ahci }, /* CPT RAID */
@@ -347,15 +367,21 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, 0xa107), board_ahci }, /* Sunrise Point-H RAID */
 	{ PCI_VDEVICE(INTEL, 0xa10f), board_ahci }, /* Sunrise Point-H RAID */
 	{ PCI_VDEVICE(INTEL, 0x2822), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0x2823), board_ahci }, /* Lewisburg AHCI*/
 	{ PCI_VDEVICE(INTEL, 0x2826), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0x2827), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa182), board_ahci }, /* Lewisburg AHCI*/
 	{ PCI_VDEVICE(INTEL, 0xa184), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa186), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa18e), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0xa1d2), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0xa1d6), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa202), board_ahci }, /* Lewisburg AHCI*/
 	{ PCI_VDEVICE(INTEL, 0xa204), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa206), board_ahci }, /* Lewisburg RAID*/
 	{ PCI_VDEVICE(INTEL, 0xa20e), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0xa252), board_ahci }, /* Lewisburg RAID*/
+	{ PCI_VDEVICE(INTEL, 0xa256), board_ahci }, /* Lewisburg RAID*/
 
 	/* JMicron 360/1/3/5/6, match class to avoid IDE function */
 	{ PCI_VENDOR_ID_JMICRON, PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID,
@@ -1305,16 +1331,52 @@ static inline void ahci_gtf_filter_workaround(struct ata_host *host)
 {}
 #endif
 
+#ifdef CONFIG_ARM64
 /*
- * ahci_init_msix() only implements single MSI-X support, not multiple
- * MSI-X per-port interrupts. This is needed for host controllers that only
- * have MSI-X support implemented, but no MSI or intx.
+ * Due to ERRATA#22536, ThunderX needs to handle HOST_IRQ_STAT differently.
+ * Workaround is to make sure all pending IRQs are served before leaving
+ * handler.
+ */
+static irqreturn_t ahci_thunderx_irq_handler(int irq, void *dev_instance)
+{
+	struct ata_host *host = dev_instance;
+	struct ahci_host_priv *hpriv;
+	unsigned int rc = 0;
+	void __iomem *mmio;
+	u32 irq_stat, irq_masked;
+	unsigned int handled = 1;
+
+	VPRINTK("ENTER\n");
+	hpriv = host->private_data;
+	mmio = hpriv->mmio;
+	irq_stat = readl(mmio + HOST_IRQ_STAT);
+	if (!irq_stat)
+		return IRQ_NONE;
+
+	do {
+		irq_masked = irq_stat & hpriv->port_map;
+		spin_lock(&host->lock);
+		rc = ahci_handle_port_intr(host, irq_masked);
+		if (!rc)
+			handled = 0;
+		writel(irq_stat, mmio + HOST_IRQ_STAT);
+		irq_stat = readl(mmio + HOST_IRQ_STAT);
+		spin_unlock(&host->lock);
+	} while (irq_stat);
+	VPRINTK("EXIT\n");
+
+	return IRQ_RETVAL(handled);
+}
+#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)
+			  struct ahci_host_priv *hpriv, unsigned long flags)
 {
-	int rc, nvec;
-	struct msix_entry entry = {};
+	int nvec, i, rc;
 
 	/* Do not init MSI-X if MSI is disabled for the device */
 	if (hpriv->flags & AHCI_HFLAG_NO_MSI)
@@ -1324,22 +1386,39 @@ static int ahci_init_msix(struct pci_dev *pdev, unsigned int n_ports,
 	if (nvec < 0)
 		return nvec;
 
-	if (!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;
 	}
 
-	/*
-	 * There can be more than one vector (e.g. for error detection or
-	 * hdd hotplug). Only the first vector (entry.entry = 0) is used.
-	 */
-	rc = pci_enable_msix_exact(pdev, &entry, 1);
+	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;
 
-	hpriv->irq = entry.vector;
+	if (nvec > 1)
+		hpriv->flags |= AHCI_HFLAG_MULTI_MSIX;
+	hpriv->irq = hpriv->msix[0].vector; /* for single msi-x */
 
-	return 1;
+	return nvec;
 fail:
 	dev_err(&pdev->dev,
 		"failed to enable MSI-X with error %d, # of vectors: %d\n",
@@ -1403,20 +1482,25 @@ static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
 {
 	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.
+	 */
+	nvec = ahci_init_msix(pdev, n_ports, hpriv, AHCI_HFLAG_MULTI_MSIX);
+	if (nvec >= 0)
+		return nvec;
+
 	nvec = ahci_init_msi(pdev, n_ports, hpriv);
 	if (nvec >= 0)
 		return nvec;
 
-	/*
-	 * Currently, MSI-X support only implements single IRQ mode and
-	 * exists for controllers which can't do other types of IRQ. Only
-	 * set it up if MSI fails.
-	 */
-	nvec = ahci_init_msix(pdev, n_ports, hpriv);
+	/* try single-msix */
+	nvec = ahci_init_msix(pdev, n_ports, hpriv, 0);
 	if (nvec >= 0)
 		return nvec;
 
-	/* lagacy intx interrupts */
+	/* legacy intx interrupts */
 	pci_intx(pdev, 1);
 	hpriv->irq = pdev->irq;
 
@@ -1520,6 +1604,11 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (ahci_broken_devslp(pdev))
 		hpriv->flags |= AHCI_HFLAG_NO_DEVSLP;
 
+#ifdef CONFIG_ARM64
+	if (pdev->vendor == 0x177d && pdev->device == 0xa01c)
+		hpriv->irq_handler = ahci_thunderx_irq_handler;
+#endif
+
 	/* save initial config */
 	ahci_pci_save_initial_config(pdev, hpriv);
 
@@ -1578,7 +1667,10 @@ 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 (!(hpriv->cap & HOST_CAP_SSS) || ahci_ignore_sss)
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 45586c1dbbdc..167ba7e3b92e 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -35,6 +35,7 @@
 #ifndef _AHCI_H
 #define _AHCI_H
 
+#include <linux/pci.h>
 #include <linux/clk.h>
 #include <linux/libata.h>
 #include <linux/phy/phy.h>
@@ -237,11 +238,18 @@ enum {
 	AHCI_HFLAG_DELAY_ENGINE		= (1 << 15), /* do not start engine on
 						        port start (wait until
 						        error-handling stage) */
-	AHCI_HFLAG_MULTI_MSI		= (1 << 16), /* multiple PCI MSIs */
 	AHCI_HFLAG_NO_DEVSLP		= (1 << 17), /* no device sleep */
 	AHCI_HFLAG_NO_FBS		= (1 << 18), /* no FBS */
-	AHCI_HFLAG_EDGE_IRQ		= (1 << 19), /* HOST_IRQ_STAT behaves as
-							Edge Triggered */
+
+#ifdef CONFIG_PCI_MSI
+	AHCI_HFLAG_MULTI_MSI		= (1 << 20), /* multiple PCI MSIs */
+	AHCI_HFLAG_MULTI_MSIX		= (1 << 21), /* 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 */
 
 	/* ap->flags bits */
 
@@ -308,7 +316,6 @@ struct ahci_port_priv {
 	unsigned int		ncq_saw_d2h:1;
 	unsigned int		ncq_saw_dmas:1;
 	unsigned int		ncq_saw_sdb:1;
-	atomic_t		intr_status;	/* interrupts to handle */
 	spinlock_t		lock;		/* protects parent ata_port */
 	u32 			intr_mask;	/* interrupts to enable */
 	bool			fbs_supported;	/* set iff FBS is supported */
@@ -343,6 +350,7 @@ 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 */
@@ -352,8 +360,24 @@ struct ahci_host_priv {
 	 * be overridden anytime before the host is activated.
 	 */
 	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
+
 extern int ahci_ignore_sss;
 
 extern struct device_attribute *ahci_shost_attrs[];
@@ -400,6 +424,7 @@ int ahci_reset_em(struct ata_host *host);
 void ahci_print_info(struct ata_host *host, const char *scc_s);
 int ahci_host_activate(struct ata_host *host, struct scsi_host_template *sht);
 void ahci_error_handler(struct ata_port *ap);
+u32 ahci_handle_port_intr(struct ata_host *host, u32 irq_masked);
 
 static inline void __iomem *__ahci_port_base(struct ata_host *host,
 					     unsigned int port_no)
diff --git a/drivers/ata/ahci_brcmstb.c b/drivers/ata/ahci_brcmstb.c
index 14b7305d2ba0..e87bcec0fd7c 100644
--- a/drivers/ata/ahci_brcmstb.c
+++ b/drivers/ata/ahci_brcmstb.c
@@ -52,8 +52,10 @@
   #define SATA_TOP_CTRL_2_PHY_GLOBAL_RESET		BIT(14)
  #define SATA_TOP_CTRL_PHY_OFFS				0x8
  #define SATA_TOP_MAX_PHYS				2
-#define SATA_TOP_CTRL_SATA_TP_OUT			0x1c
-#define SATA_TOP_CTRL_CLIENT_INIT_CTRL			0x20
+
+#define SATA_FIRST_PORT_CTRL				0x700
+#define SATA_NEXT_PORT_CTRL_OFFSET			0x80
+#define SATA_PORT_PCTRL6(reg_base)			(reg_base + 0x18)
 
 /* On big-endian MIPS, buses are reversed to big endian, so switch them back */
 #if defined(CONFIG_MIPS) && defined(__BIG_ENDIAN)
@@ -69,14 +71,21 @@
 	(DATA_ENDIAN << DMADESC_ENDIAN_SHIFT) |		\
 	(MMIO_ENDIAN << MMIO_ENDIAN_SHIFT))
 
+enum brcm_ahci_quirks {
+	BRCM_AHCI_QUIRK_NO_NCQ		= BIT(0),
+	BRCM_AHCI_QUIRK_SKIP_PHY_ENABLE	= BIT(1),
+};
+
 struct brcm_ahci_priv {
 	struct device *dev;
 	void __iomem *top_ctrl;
 	u32 port_mask;
+	u32 quirks;
 };
 
 static const struct ata_port_info ahci_brcm_port_info = {
-	.flags		= AHCI_FLAG_COMMON,
+	.flags		= AHCI_FLAG_COMMON | ATA_FLAG_NO_DIPM,
+	.link_flags	= ATA_LFLAG_NO_DB_DELAY,
 	.pio_mask	= ATA_PIO4,
 	.udma_mask	= ATA_UDMA6,
 	.port_ops	= &ahci_platform_ops,
@@ -107,6 +116,34 @@ static inline void brcm_sata_writereg(u32 val, void __iomem *addr)
 		writel_relaxed(val, addr);
 }
 
+static void brcm_sata_alpm_init(struct ahci_host_priv *hpriv)
+{
+	struct brcm_ahci_priv *priv = hpriv->plat_data;
+	u32 bus_ctrl, port_ctrl, host_caps;
+	int i;
+
+	/* Enable support for ALPM */
+	bus_ctrl = brcm_sata_readreg(priv->top_ctrl +
+				     SATA_TOP_CTRL_BUS_CTRL);
+	brcm_sata_writereg(bus_ctrl | OVERRIDE_HWINIT,
+			   priv->top_ctrl + SATA_TOP_CTRL_BUS_CTRL);
+	host_caps = readl(hpriv->mmio + HOST_CAP);
+	writel(host_caps | HOST_CAP_ALPM, hpriv->mmio);
+	brcm_sata_writereg(bus_ctrl, priv->top_ctrl + SATA_TOP_CTRL_BUS_CTRL);
+
+	/*
+	 * Adjust timeout to allow PLL sufficient time to lock while waking
+	 * up from slumber mode.
+	 */
+	for (i = 0, port_ctrl = SATA_FIRST_PORT_CTRL;
+	     i < SATA_TOP_MAX_PHYS;
+	     i++, port_ctrl += SATA_NEXT_PORT_CTRL_OFFSET) {
+		if (priv->port_mask & BIT(i))
+			writel(0xff1003fc,
+			       hpriv->mmio + SATA_PORT_PCTRL6(port_ctrl));
+	}
+}
+
 static void brcm_sata_phy_enable(struct brcm_ahci_priv *priv, int port)
 {
 	void __iomem *phyctrl = priv->top_ctrl + SATA_TOP_CTRL_PHY_CTRL +
@@ -114,6 +151,9 @@ static void brcm_sata_phy_enable(struct brcm_ahci_priv *priv, int port)
 	void __iomem *p;
 	u32 reg;
 
+	if (priv->quirks & BRCM_AHCI_QUIRK_SKIP_PHY_ENABLE)
+		return;
+
 	/* clear PHY_DEFAULT_POWER_STATE */
 	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_1;
 	reg = brcm_sata_readreg(p);
@@ -143,6 +183,9 @@ static void brcm_sata_phy_disable(struct brcm_ahci_priv *priv, int port)
 	void __iomem *p;
 	u32 reg;
 
+	if (priv->quirks & BRCM_AHCI_QUIRK_SKIP_PHY_ENABLE)
+		return;
+
 	/* power-off the PHY digital logic */
 	p = phyctrl + SATA_TOP_CTRL_PHY_CTRL_2;
 	reg = brcm_sata_readreg(p);
@@ -230,6 +273,7 @@ static int brcm_ahci_resume(struct device *dev)
 
 	brcm_sata_init(priv);
 	brcm_sata_phys_enable(priv);
+	brcm_sata_alpm_init(hpriv);
 	return ahci_platform_resume(dev);
 }
 #endif
@@ -256,6 +300,11 @@ static int brcm_ahci_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->top_ctrl))
 		return PTR_ERR(priv->top_ctrl);
 
+	if (of_device_is_compatible(dev->of_node, "brcm,bcm7425-ahci")) {
+		priv->quirks |= BRCM_AHCI_QUIRK_NO_NCQ;
+		priv->quirks |= BRCM_AHCI_QUIRK_SKIP_PHY_ENABLE;
+	}
+
 	brcm_sata_init(priv);
 
 	priv->port_mask = brcm_ahci_get_portmask(pdev, priv);
@@ -268,11 +317,17 @@ static int brcm_ahci_probe(struct platform_device *pdev)
 	if (IS_ERR(hpriv))
 		return PTR_ERR(hpriv);
 	hpriv->plat_data = priv;
+	hpriv->flags = AHCI_HFLAG_WAKE_BEFORE_STOP;
+
+	brcm_sata_alpm_init(hpriv);
 
 	ret = ahci_platform_enable_resources(hpriv);
 	if (ret)
 		return ret;
 
+	if (priv->quirks & BRCM_AHCI_QUIRK_NO_NCQ)
+		hpriv->flags |= AHCI_HFLAG_NO_NCQ;
+
 	ret = ahci_platform_init_host(pdev, hpriv, &ahci_brcm_port_info,
 				      &ahci_platform_sht);
 	if (ret)
@@ -300,6 +355,7 @@ static int brcm_ahci_remove(struct platform_device *pdev)
 }
 
 static const struct of_device_id ahci_of_match[] = {
+	{.compatible = "brcm,bcm7425-ahci"},
 	{.compatible = "brcm,bcm7445-ahci"},
 	{},
 };
diff --git a/drivers/ata/ahci_qoriq.c b/drivers/ata/ahci_qoriq.c
index d0f9de96e4ea..7bdee9bd8786 100644
--- a/drivers/ata/ahci_qoriq.c
+++ b/drivers/ata/ahci_qoriq.c
@@ -34,14 +34,20 @@
 
 /* port register default value */
 #define AHCI_PORT_PHY_1_CFG	0xa003fffe
-#define AHCI_PORT_PHY_2_CFG	0x28183411
-#define AHCI_PORT_PHY_3_CFG	0x0e081004
-#define AHCI_PORT_PHY_4_CFG	0x00480811
-#define AHCI_PORT_PHY_5_CFG	0x192c96a4
-#define AHCI_PORT_TRANS_CFG	0x08000025
+#define AHCI_PORT_TRANS_CFG	0x08000029
+
+/* for ls1021a */
+#define LS1021A_PORT_PHY2	0x28183414
+#define LS1021A_PORT_PHY3	0x0e080e06
+#define LS1021A_PORT_PHY4	0x064a080b
+#define LS1021A_PORT_PHY5	0x2aa86470
 
 #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,
@@ -151,16 +157,23 @@ static int ahci_qoriq_phy_init(struct ahci_host_priv *hpriv)
 	case AHCI_LS1021A:
 		writel(SATA_ECC_DISABLE, qpriv->ecc_addr);
 		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
-		writel(AHCI_PORT_PHY_2_CFG, reg_base + PORT_PHY2);
-		writel(AHCI_PORT_PHY_3_CFG, reg_base + PORT_PHY3);
-		writel(AHCI_PORT_PHY_4_CFG, reg_base + PORT_PHY4);
-		writel(AHCI_PORT_PHY_5_CFG, reg_base + PORT_PHY5);
+		writel(LS1021A_PORT_PHY2, reg_base + PORT_PHY2);
+		writel(LS1021A_PORT_PHY3, reg_base + PORT_PHY3);
+		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);
 		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);
+		break;
+
 	case AHCI_LS2080A:
 		writel(AHCI_PORT_PHY_1_CFG, reg_base + PORT_PHY1);
+		writel(AHCI_PORT_TRANS_CFG, reg_base + PORT_TRANS);
 		break;
 	}
 
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index e2c6d9e0c5ac..8e3f7faf00d3 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -548,6 +548,88 @@ softreset_retry:
 	return rc;
 }
 
+/**
+ * xgene_ahci_handle_broken_edge_irq - Handle the broken irq.
+ * @ata_host: Host that recieved the irq
+ * @irq_masked: HOST_IRQ_STAT value
+ *
+ * For hardware with broken edge trigger latch
+ * the HOST_IRQ_STAT register misses the edge interrupt
+ * when clearing of HOST_IRQ_STAT register and hardware
+ * reporting the PORT_IRQ_STAT register at the
+ * same clock cycle.
+ * As such, the algorithm below outlines the workaround.
+ *
+ * 1. Read HOST_IRQ_STAT register and save the state.
+ * 2. Clear the HOST_IRQ_STAT register.
+ * 3. Read back the HOST_IRQ_STAT register.
+ * 4. If HOST_IRQ_STAT register equals to zero, then
+ *    traverse the rest of port's PORT_IRQ_STAT register
+ *    to check if an interrupt is triggered at that point else
+ *    go to step 6.
+ * 5. If PORT_IRQ_STAT register of rest ports is not equal to zero
+ *    then update the state of HOST_IRQ_STAT saved in step 1.
+ * 6. Handle port interrupts.
+ * 7. Exit
+ */
+static int xgene_ahci_handle_broken_edge_irq(struct ata_host *host,
+					     u32 irq_masked)
+{
+	struct ahci_host_priv *hpriv = host->private_data;
+	void __iomem *port_mmio;
+	int i;
+
+	if (!readl(hpriv->mmio + HOST_IRQ_STAT)) {
+		for (i = 0; i < host->n_ports; i++) {
+			if (irq_masked & (1 << i))
+				continue;
+
+			port_mmio = ahci_port_base(host->ports[i]);
+			if (readl(port_mmio + PORT_IRQ_STAT))
+				irq_masked |= (1 << i);
+		}
+	}
+
+	return ahci_handle_port_intr(host, irq_masked);
+}
+
+static irqreturn_t xgene_ahci_irq_intr(int irq, void *dev_instance)
+{
+	struct ata_host *host = dev_instance;
+	struct ahci_host_priv *hpriv;
+	unsigned int rc = 0;
+	void __iomem *mmio;
+	u32 irq_stat, irq_masked;
+
+	VPRINTK("ENTER\n");
+
+	hpriv = host->private_data;
+	mmio = hpriv->mmio;
+
+	/* sigh.  0xffffffff is a valid return from h/w */
+	irq_stat = readl(mmio + HOST_IRQ_STAT);
+	if (!irq_stat)
+		return IRQ_NONE;
+
+	irq_masked = irq_stat & hpriv->port_map;
+
+	spin_lock(&host->lock);
+
+	/*
+	 * HOST_IRQ_STAT behaves as edge triggered latch meaning that
+	 * it should be cleared before all the port events are cleared.
+	 */
+	writel(irq_stat, mmio + HOST_IRQ_STAT);
+
+	rc = xgene_ahci_handle_broken_edge_irq(host, irq_masked);
+
+	spin_unlock(&host->lock);
+
+	VPRINTK("EXIT\n");
+
+	return IRQ_RETVAL(rc);
+}
+
 static struct ata_port_operations xgene_ahci_v1_ops = {
 	.inherits = &ahci_ops,
 	.host_stop = xgene_ahci_host_stop,
@@ -779,7 +861,8 @@ skip_clk_phy:
 		hpriv->flags = AHCI_HFLAG_NO_NCQ;
 		break;
 	case XGENE_AHCI_V2:
-		hpriv->flags |= AHCI_HFLAG_YES_FBS | AHCI_HFLAG_EDGE_IRQ;
+		hpriv->flags |= AHCI_HFLAG_YES_FBS;
+		hpriv->irq_handler = xgene_ahci_irq_intr;
 		break;
 	default:
 		break;
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 4665512dae44..85ea5142a095 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -43,6 +43,7 @@
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_cmnd.h>
 #include <linux/libata.h>
+#include <linux/pci.h>
 #include "ahci.h"
 #include "libata.h"
 
@@ -112,6 +113,7 @@ static ssize_t ahci_store_em_buffer(struct device *dev,
 				    const char *buf, size_t size);
 static ssize_t ahci_show_em_supported(struct device *dev,
 				      struct device_attribute *attr, char *buf);
+static irqreturn_t ahci_single_level_irq_intr(int irq, void *dev_instance);
 
 static DEVICE_ATTR(ahci_host_caps, S_IRUGO, ahci_show_host_caps, NULL);
 static DEVICE_ATTR(ahci_host_cap2, S_IRUGO, ahci_show_host_cap2, NULL);
@@ -495,8 +497,8 @@ void ahci_save_initial_config(struct device *dev, struct ahci_host_priv *hpriv)
 		}
 	}
 
-	/* fabricate port_map from cap.nr_ports */
-	if (!port_map) {
+	/* fabricate port_map from cap.nr_ports for < AHCI 1.3 */
+	if (!port_map && vers < 0x10300) {
 		port_map = (1 << ahci_nr_ports(cap)) - 1;
 		dev_warn(dev, "forcing PORTS_IMPL to 0x%x\n", port_map);
 
@@ -511,6 +513,9 @@ void ahci_save_initial_config(struct device *dev, struct ahci_host_priv *hpriv)
 
 	if (!hpriv->start_engine)
 		hpriv->start_engine = ahci_start_engine;
+
+	if (!hpriv->irq_handler)
+		hpriv->irq_handler = ahci_single_level_irq_intr;
 }
 EXPORT_SYMBOL_GPL(ahci_save_initial_config);
 
@@ -592,8 +597,22 @@ EXPORT_SYMBOL_GPL(ahci_start_engine);
 int ahci_stop_engine(struct ata_port *ap)
 {
 	void __iomem *port_mmio = ahci_port_base(ap);
+	struct ahci_host_priv *hpriv = ap->host->private_data;
 	u32 tmp;
 
+	/*
+	 * On some controllers, stopping a port's DMA engine while the port
+	 * is in ALPM state (partial or slumber) results in failures on
+	 * subsequent DMA engine starts.  For those controllers, put the
+	 * port back in active state before stopping its DMA engine.
+	 */
+	if ((hpriv->flags & AHCI_HFLAG_WAKE_BEFORE_STOP) &&
+	    (ap->link.lpm_policy > ATA_LPM_MAX_POWER) &&
+	    ahci_set_lpm(&ap->link, ATA_LPM_MAX_POWER, ATA_LPM_WAKE_ONLY)) {
+		dev_err(ap->host->dev, "Failed to wake up port before engine stop\n");
+		return -EIO;
+	}
+
 	tmp = readl(port_mmio + PORT_CMD);
 
 	/* check if the HBA is idle */
@@ -688,6 +707,9 @@ static int ahci_set_lpm(struct ata_link *link, enum ata_lpm_policy policy,
 	void __iomem *port_mmio = ahci_port_base(ap);
 
 	if (policy != ATA_LPM_MAX_POWER) {
+		/* wakeup flag only applies to the max power policy */
+		hints &= ~ATA_LPM_WAKE_ONLY;
+
 		/*
 		 * Disable interrupts on Phy Ready. This keeps us from
 		 * getting woken up due to spurious phy ready
@@ -703,7 +725,8 @@ static int ahci_set_lpm(struct ata_link *link, enum ata_lpm_policy policy,
 		u32 cmd = readl(port_mmio + PORT_CMD);
 
 		if (policy == ATA_LPM_MAX_POWER || !(hints & ATA_LPM_HIPM)) {
-			cmd &= ~(PORT_CMD_ASP | PORT_CMD_ALPE);
+			if (!(hints & ATA_LPM_WAKE_ONLY))
+				cmd &= ~(PORT_CMD_ASP | PORT_CMD_ALPE);
 			cmd |= PORT_CMD_ICC_ACTIVE;
 
 			writel(cmd, port_mmio + PORT_CMD);
@@ -711,6 +734,9 @@ static int ahci_set_lpm(struct ata_link *link, enum ata_lpm_policy policy,
 
 			/* wait 10ms to be sure we've come out of LPM state */
 			ata_msleep(ap, 10);
+
+			if (hints & ATA_LPM_WAKE_ONLY)
+				return 0;
 		} else {
 			cmd |= PORT_CMD_ALPE;
 			if (policy == ATA_LPM_MIN_POWER)
@@ -1142,8 +1168,7 @@ static void ahci_port_init(struct device *dev, struct ata_port *ap,
 
 	/* mark esata ports */
 	tmp = readl(port_mmio + PORT_CMD);
-	if ((tmp & PORT_CMD_HPCP) ||
-	    ((tmp & PORT_CMD_ESP) && (hpriv->cap & HOST_CAP_SXS)))
+	if ((tmp & PORT_CMD_ESP) && (hpriv->cap & HOST_CAP_SXS))
 		ap->pflags |= ATA_PFLAG_EXTERNAL;
 }
 
@@ -1804,44 +1829,27 @@ static void ahci_port_intr(struct ata_port *ap)
 	ahci_handle_port_interrupt(ap, port_mmio, status);
 }
 
-static irqreturn_t ahci_port_thread_fn(int irq, void *dev_instance)
+static irqreturn_t ahci_multi_irqs_intr_hard(int irq, void *dev_instance)
 {
 	struct ata_port *ap = dev_instance;
-	struct ahci_port_priv *pp = ap->private_data;
 	void __iomem *port_mmio = ahci_port_base(ap);
 	u32 status;
 
-	status = atomic_xchg(&pp->intr_status, 0);
-	if (!status)
-		return IRQ_NONE;
-
-	spin_lock_bh(ap->lock);
-	ahci_handle_port_interrupt(ap, port_mmio, status);
-	spin_unlock_bh(ap->lock);
-
-	return IRQ_HANDLED;
-}
-
-static irqreturn_t ahci_multi_irqs_intr(int irq, void *dev_instance)
-{
-	struct ata_port *ap = dev_instance;
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 status;
-
 	VPRINTK("ENTER\n");
 
 	status = readl(port_mmio + PORT_IRQ_STAT);
 	writel(status, port_mmio + PORT_IRQ_STAT);
 
-	atomic_or(status, &pp->intr_status);
+	spin_lock(ap->lock);
+	ahci_handle_port_interrupt(ap, port_mmio, status);
+	spin_unlock(ap->lock);
 
 	VPRINTK("EXIT\n");
 
-	return IRQ_WAKE_THREAD;
+	return IRQ_HANDLED;
 }
 
-static u32 ahci_handle_port_intr(struct ata_host *host, u32 irq_masked)
+u32 ahci_handle_port_intr(struct ata_host *host, u32 irq_masked)
 {
 	unsigned int i, handled = 0;
 
@@ -1867,43 +1875,7 @@ static u32 ahci_handle_port_intr(struct ata_host *host, u32 irq_masked)
 
 	return handled;
 }
-
-static irqreturn_t ahci_single_edge_irq_intr(int irq, void *dev_instance)
-{
-	struct ata_host *host = dev_instance;
-	struct ahci_host_priv *hpriv;
-	unsigned int rc = 0;
-	void __iomem *mmio;
-	u32 irq_stat, irq_masked;
-
-	VPRINTK("ENTER\n");
-
-	hpriv = host->private_data;
-	mmio = hpriv->mmio;
-
-	/* sigh.  0xffffffff is a valid return from h/w */
-	irq_stat = readl(mmio + HOST_IRQ_STAT);
-	if (!irq_stat)
-		return IRQ_NONE;
-
-	irq_masked = irq_stat & hpriv->port_map;
-
-	spin_lock(&host->lock);
-
-	/*
-	 * HOST_IRQ_STAT behaves as edge triggered latch meaning that
-	 * it should be cleared before all the port events are cleared.
-	 */
-	writel(irq_stat, mmio + HOST_IRQ_STAT);
-
-	rc = ahci_handle_port_intr(host, irq_masked);
-
-	spin_unlock(&host->lock);
-
-	VPRINTK("EXIT\n");
-
-	return IRQ_RETVAL(rc);
-}
+EXPORT_SYMBOL_GPL(ahci_handle_port_intr);
 
 static irqreturn_t ahci_single_level_irq_intr(int irq, void *dev_instance)
 {
@@ -2479,9 +2451,10 @@ void ahci_set_em_messages(struct ahci_host_priv *hpriv,
 }
 EXPORT_SYMBOL_GPL(ahci_set_em_messages);
 
-static int ahci_host_activate_multi_irqs(struct ata_host *host, int irq,
+static int ahci_host_activate_multi_irqs(struct ata_host *host,
 					 struct scsi_host_template *sht)
 {
+	struct ahci_host_priv *hpriv = host->private_data;
 	int i, rc;
 
 	rc = ata_host_start(host);
@@ -2493,6 +2466,7 @@ static int ahci_host_activate_multi_irqs(struct ata_host *host, int irq,
 	 */
 	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);
 
 		/* Do not receive interrupts sent by dummy ports */
 		if (!pp) {
@@ -2500,14 +2474,14 @@ static int ahci_host_activate_multi_irqs(struct ata_host *host, int irq,
 			continue;
 		}
 
-		rc = devm_request_threaded_irq(host->dev, irq + i,
-					       ahci_multi_irqs_intr,
-					       ahci_port_thread_fn, 0,
-					       pp->irq_desc, host->ports[i]);
+		rc = devm_request_irq(host->dev, irq, ahci_multi_irqs_intr_hard,
+				0, pp->irq_desc, host->ports[i]);
+
 		if (rc)
 			return rc;
-		ata_port_desc(host->ports[i], "irq %d", irq + i);
+		ata_port_desc(host->ports[i], "irq %d", irq);
 	}
+
 	return ata_host_register(host, sht);
 }
 
@@ -2528,14 +2502,18 @@ 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)
-		rc = ahci_host_activate_multi_irqs(host, irq, sht);
-	else if (hpriv->flags & AHCI_HFLAG_EDGE_IRQ)
-		rc = ata_host_activate(host, irq, ahci_single_edge_irq_intr,
-				       IRQF_SHARED, sht);
-	else
-		rc = ata_host_activate(host, irq, ahci_single_level_irq_intr,
+	if (hpriv->flags & (AHCI_HFLAG_MULTI_MSI | AHCI_HFLAG_MULTI_MSIX)) {
+		if (hpriv->irq_handler)
+			dev_warn(host->dev, "both AHCI_HFLAG_MULTI_MSI flag set \
+				 and custom irq handler implemented\n");
+
+		rc = ahci_host_activate_multi_irqs(host, sht);
+	} else {
+		rc = ata_host_activate(host, irq, hpriv->irq_handler,
 				       IRQF_SHARED, sht);
+	}
+
+
 	return rc;
 }
 EXPORT_SYMBOL_GPL(ahci_host_activate);
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index b79cb10e289e..55e257c268dd 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -50,6 +50,7 @@
 #include <linux/blkdev.h>
 #include <linux/delay.h>
 #include <linux/timer.h>
+#include <linux/time.h>
 #include <linux/interrupt.h>
 #include <linux/completion.h>
 #include <linux/suspend.h>
@@ -3597,7 +3598,8 @@ int sata_link_resume(struct ata_link *link, const unsigned long *params,
 		 * immediately after resuming.  Delay 200ms before
 		 * debouncing.
 		 */
-		ata_msleep(link->ap, 200);
+		if (!(link->flags & ATA_LFLAG_NO_DB_DELAY))
+			ata_msleep(link->ap, 200);
 
 		/* is SControl restored correctly? */
 		if ((rc = sata_scr_read(link, SCR_CONTROL, &scontrol)))
@@ -4123,6 +4125,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "SAMSUNG CD-ROM SN-124", "N001",	ATA_HORKAGE_NODMA },
 	{ "Seagate STT20000A", NULL,		ATA_HORKAGE_NODMA },
 	{ " 2GB ATA Flash Disk", "ADMA428M",	ATA_HORKAGE_NODMA },
+	{ "VRFDFC22048UCHC-TE*", NULL,		ATA_HORKAGE_NODMA },
 	/* Odd clown on sil3726/4726 PMPs */
 	{ "Config  Disk",	NULL,		ATA_HORKAGE_DISABLE },
 
@@ -6223,6 +6226,7 @@ int ata_host_activate(struct ata_host *host, int irq,
 		      struct scsi_host_template *sht)
 {
 	int i, rc;
+	char *irq_desc;
 
 	rc = ata_host_start(host);
 	if (rc)
@@ -6234,8 +6238,14 @@ int ata_host_activate(struct ata_host *host, int irq,
 		return ata_host_register(host, sht);
 	}
 
+	irq_desc = devm_kasprintf(host->dev, GFP_KERNEL, "%s[%s]",
+				  dev_driver_string(host->dev),
+				  dev_name(host->dev));
+	if (!irq_desc)
+		return -ENOMEM;
+
 	rc = devm_request_irq(host->dev, irq, irq_handler, irq_flags,
-			      dev_name(host->dev), host);
+			      irq_desc, host);
 	if (rc)
 		return rc;
 
@@ -6697,7 +6707,12 @@ void ata_msleep(struct ata_port *ap, unsigned int msecs)
 	if (owns_eh)
 		ata_eh_release(ap);
 
-	msleep(msecs);
+	if (msecs < 20) {
+		unsigned long usecs = msecs * USEC_PER_MSEC;
+		usleep_range(usecs, usecs + 50);
+	} else {
+		msleep(msecs);
+	}
 
 	if (owns_eh)
 		ata_eh_acquire(ap);
diff --git a/drivers/ata/libata-scsi.c b/drivers/ata/libata-scsi.c
index 7e959f90c020..e417e1a1d02c 100644
--- a/drivers/ata/libata-scsi.c
+++ b/drivers/ata/libata-scsi.c
@@ -675,19 +675,18 @@ static int ata_ioc32(struct ata_port *ap)
 int ata_sas_scsi_ioctl(struct ata_port *ap, struct scsi_device *scsidev,
 		     int cmd, void __user *arg)
 {
-	int val = -EINVAL, rc = -EINVAL;
+	unsigned long val;
+	int rc = -EINVAL;
 	unsigned long flags;
 
 	switch (cmd) {
-	case ATA_IOC_GET_IO32:
+	case HDIO_GET_32BIT:
 		spin_lock_irqsave(ap->lock, flags);
 		val = ata_ioc32(ap);
 		spin_unlock_irqrestore(ap->lock, flags);
-		if (copy_to_user(arg, &val, 1))
-			return -EFAULT;
-		return 0;
+		return put_user(val, (unsigned long __user *)arg);
 
-	case ATA_IOC_SET_IO32:
+	case HDIO_SET_32BIT:
 		val = (unsigned long) arg;
 		rc = 0;
 		spin_lock_irqsave(ap->lock, flags);
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index cdf6215a9a22..051b6158d1b7 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -997,12 +997,9 @@ static inline int ata_hsm_ok_in_wq(struct ata_port *ap,
 static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
 {
 	struct ata_port *ap = qc->ap;
-	unsigned long flags;
 
 	if (ap->ops->error_handler) {
 		if (in_wq) {
-			spin_lock_irqsave(ap->lock, flags);
-
 			/* EH might have kicked in while host lock is
 			 * released.
 			 */
@@ -1014,8 +1011,6 @@ static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
 				} else
 					ata_port_freeze(ap);
 			}
-
-			spin_unlock_irqrestore(ap->lock, flags);
 		} else {
 			if (likely(!(qc->err_mask & AC_ERR_HSM)))
 				ata_qc_complete(qc);
@@ -1024,10 +1019,8 @@ static void ata_hsm_qc_complete(struct ata_queued_cmd *qc, int in_wq)
 		}
 	} else {
 		if (in_wq) {
-			spin_lock_irqsave(ap->lock, flags);
 			ata_sff_irq_on(ap);
 			ata_qc_complete(qc);
-			spin_unlock_irqrestore(ap->lock, flags);
 		} else
 			ata_qc_complete(qc);
 	}
@@ -1048,9 +1041,10 @@ int ata_sff_hsm_move(struct ata_port *ap, struct ata_queued_cmd *qc,
 {
 	struct ata_link *link = qc->dev->link;
 	struct ata_eh_info *ehi = &link->eh_info;
-	unsigned long flags = 0;
 	int poll_next;
 
+	lockdep_assert_held(ap->lock);
+
 	WARN_ON_ONCE((qc->flags & ATA_QCFLAG_ACTIVE) == 0);
 
 	/* Make sure ata_sff_qc_issue() does not throw things
@@ -1112,14 +1106,6 @@ fsm_start:
 			}
 		}
 
-		/* Send the CDB (atapi) or the first data block (ata pio out).
-		 * During the state transition, interrupt handler shouldn't
-		 * be invoked before the data transfer is complete and
-		 * hsm_task_state is changed. Hence, the following locking.
-		 */
-		if (in_wq)
-			spin_lock_irqsave(ap->lock, flags);
-
 		if (qc->tf.protocol == ATA_PROT_PIO) {
 			/* PIO data out protocol.
 			 * send first data block.
@@ -1135,9 +1121,6 @@ fsm_start:
 			/* send CDB */
 			atapi_send_cdb(ap, qc);
 
-		if (in_wq)
-			spin_unlock_irqrestore(ap->lock, flags);
-
 		/* if polling, ata_sff_pio_task() handles the rest.
 		 * otherwise, interrupt handler takes over from here.
 		 */
@@ -1296,7 +1279,8 @@ fsm_start:
 		break;
 	default:
 		poll_next = 0;
-		BUG();
+		WARN(true, "ata%d: SFF host state machine in invalid state %d",
+		     ap->print_id, ap->hsm_task_state);
 	}
 
 	return poll_next;
@@ -1361,12 +1345,14 @@ static void ata_sff_pio_task(struct work_struct *work)
 	u8 status;
 	int poll_next;
 
+	spin_lock_irq(ap->lock);
+
 	BUG_ON(ap->sff_pio_task_link == NULL);
 	/* qc can be NULL if timeout occurred */
 	qc = ata_qc_from_tag(ap, link->active_tag);
 	if (!qc) {
 		ap->sff_pio_task_link = NULL;
-		return;
+		goto out_unlock;
 	}
 
 fsm_start:
@@ -1381,11 +1367,14 @@ fsm_start:
 	 */
 	status = ata_sff_busy_wait(ap, ATA_BUSY, 5);
 	if (status & ATA_BUSY) {
+		spin_unlock_irq(ap->lock);
 		ata_msleep(ap, 2);
+		spin_lock_irq(ap->lock);
+
 		status = ata_sff_busy_wait(ap, ATA_BUSY, 10);
 		if (status & ATA_BUSY) {
 			ata_sff_queue_pio_task(link, ATA_SHORT_PAUSE);
-			return;
+			goto out_unlock;
 		}
 	}
 
@@ -1402,6 +1391,8 @@ fsm_start:
 	 */
 	if (poll_next)
 		goto fsm_start;
+out_unlock:
+	spin_unlock_irq(ap->lock);
 }
 
 /**
diff --git a/drivers/ata/pata_rb532_cf.c b/drivers/ata/pata_rb532_cf.c
index 12fe0f3bb7e9..c8b6a780a290 100644
--- a/drivers/ata/pata_rb532_cf.c
+++ b/drivers/ata/pata_rb532_cf.c
@@ -32,6 +32,8 @@
 #include <linux/libata.h>
 #include <scsi/scsi_host.h>
 
+#include <asm/mach-rc32434/rb.h>
+
 #define DRV_NAME	"pata-rb532-cf"
 #define DRV_VERSION	"0.1.0"
 #define DRV_DESC	"PATA driver for RouterBOARD 532 Compact Flash"
@@ -107,6 +109,7 @@ static int rb532_pata_driver_probe(struct platform_device *pdev)
 	int gpio;
 	struct resource *res;
 	struct ata_host *ah;
+	struct cf_device *pdata;
 	struct rb532_cf_info *info;
 	int ret;
 
@@ -122,7 +125,13 @@ static int rb532_pata_driver_probe(struct platform_device *pdev)
 		return -ENOENT;
 	}
 
-	gpio = irq_to_gpio(irq);
+	pdata = dev_get_platdata(&pdev->dev);
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data specified\n");
+		return -EINVAL;
+	}
+
+	gpio = pdata->gpio_pin;
 	if (gpio < 0) {
 		dev_err(&pdev->dev, "no GPIO found for irq%d\n", irq);
 		return -ENOENT;
diff --git a/drivers/ata/sata_rcar.c b/drivers/ata/sata_rcar.c
index 8804127b108c..f72d601e300a 100644
--- a/drivers/ata/sata_rcar.c
+++ b/drivers/ata/sata_rcar.c
@@ -854,17 +854,14 @@ static struct of_device_id sata_rcar_match[] = {
 		.compatible = "renesas,sata-r8a7793",
 		.data = (void *)RCAR_GEN2_SATA
 	},
+	{
+		.compatible = "renesas,sata-r8a7795",
+		.data = (void *)RCAR_GEN2_SATA
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, sata_rcar_match);
 
-static const struct platform_device_id sata_rcar_id_table[] = {
-	{ "sata_rcar", RCAR_GEN1_SATA }, /* Deprecated by "sata-r8a7779" */
-	{ "sata-r8a7779", RCAR_GEN1_SATA },
-	{ },
-};
-MODULE_DEVICE_TABLE(platform, sata_rcar_id_table);
-
 static int sata_rcar_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id;
@@ -884,11 +881,10 @@ static int sata_rcar_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	of_id = of_match_device(sata_rcar_match, &pdev->dev);
-	if (of_id)
-		priv->type = (enum sata_rcar_type)of_id->data;
-	else
-		priv->type = platform_get_device_id(pdev)->driver_data;
+	if (!of_id)
+		return -ENODEV;
 
+	priv->type = (enum sata_rcar_type)of_id->data;
 	priv->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(priv->clk)) {
 		dev_err(&pdev->dev, "failed to get access to sata clock\n");
@@ -1018,7 +1014,6 @@ static const struct dev_pm_ops sata_rcar_pm_ops = {
 static struct platform_driver sata_rcar_driver = {
 	.probe		= sata_rcar_probe,
 	.remove		= sata_rcar_remove,
-	.id_table	= sata_rcar_id_table,
 	.driver = {
 		.name		= DRV_NAME,
 		.of_match_table	= sata_rcar_match,
diff --git a/drivers/ata/sata_sx4.c b/drivers/ata/sata_sx4.c
index fab504fd9cfd..48301cb3a316 100644
--- a/drivers/ata/sata_sx4.c
+++ b/drivers/ata/sata_sx4.c
@@ -1396,6 +1396,8 @@ static unsigned int pdc20621_dimm_init(struct ata_host *host)
 		addr = 0;
 		length = size * 1024 * 1024;
 		buf = kzalloc(ECC_ERASE_BUF_SZ, GFP_KERNEL);
+		if (!buf)
+			return 1;
 		while (addr < length) {
 			pdc20621_put_to_dimm(host, buf, addr,
 					     ECC_ERASE_BUF_SZ);
diff --git a/drivers/atm/solos-pci.c b/drivers/atm/solos-pci.c
index 3d7fb6516f74..6ac2b2b1e8de 100644
--- a/drivers/atm/solos-pci.c
+++ b/drivers/atm/solos-pci.c
@@ -347,8 +347,8 @@ static char *next_string(struct sk_buff *skb)
  */       
 static int process_status(struct solos_card *card, int port, struct sk_buff *skb)
 {
-	char *str, *end, *state_str, *snr, *attn;
-	int ver, rate_up, rate_down;
+	char *str, *state_str, *snr, *attn;
+	int ver, rate_up, rate_down, err;
 
 	if (!card->atmdev[port])
 		return -ENODEV;
@@ -357,7 +357,11 @@ static int process_status(struct solos_card *card, int port, struct sk_buff *skb
 	if (!str)
 		return -EIO;
 
-	ver = simple_strtol(str, NULL, 10);
+	err = kstrtoint(str, 10, &ver);
+	if (err) {
+		dev_warn(&card->dev->dev, "Unexpected status interrupt version\n");
+		return err;
+	}
 	if (ver < 1) {
 		dev_warn(&card->dev->dev, "Unexpected status interrupt version %d\n",
 			 ver);
@@ -373,16 +377,16 @@ static int process_status(struct solos_card *card, int port, struct sk_buff *skb
 		return 0;
 	}
 
-	rate_down = simple_strtol(str, &end, 10);
-	if (*end)
-		return -EIO;
+	err = kstrtoint(str, 10, &rate_down);
+	if (err)
+		return err;
 
 	str = next_string(skb);
 	if (!str)
 		return -EIO;
-	rate_up = simple_strtol(str, &end, 10);
-	if (*end)
-		return -EIO;
+	err = kstrtoint(str, 10, &rate_up);
+	if (err)
+		return err;
 
 	state_str = next_string(skb);
 	if (!state_str)
@@ -417,7 +421,7 @@ static int process_command(struct solos_card *card, int port, struct sk_buff *sk
 	struct solos_param *prm;
 	unsigned long flags;
 	int cmdpid;
-	int found = 0;
+	int found = 0, err;
 
 	if (skb->len < 7)
 		return 0;
@@ -428,7 +432,9 @@ static int process_command(struct solos_card *card, int port, struct sk_buff *sk
 	    skb->data[6] != '\n')
 		return 0;
 
-	cmdpid = simple_strtol(&skb->data[1], NULL, 10);
+	err = kstrtoint(&skb->data[1], 10, &cmdpid);
+	if (err)
+		return err;
 
 	spin_lock_irqsave(&card->param_queue_lock, flags);
 	list_for_each_entry(prm, &card->param_queue, list) {
@@ -519,7 +525,7 @@ struct geos_gpio_attr {
 static ssize_t geos_gpio_store(struct device *dev, struct device_attribute *attr,
 			       const char *buf, size_t count)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct geos_gpio_attr *gattr = container_of(attr, struct geos_gpio_attr, attr);
 	struct solos_card *card = pci_get_drvdata(pdev);
 	uint32_t data32;
@@ -545,7 +551,7 @@ static ssize_t geos_gpio_store(struct device *dev, struct device_attribute *attr
 static ssize_t geos_gpio_show(struct device *dev, struct device_attribute *attr,
 			      char *buf)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct geos_gpio_attr *gattr = container_of(attr, struct geos_gpio_attr, attr);
 	struct solos_card *card = pci_get_drvdata(pdev);
 	uint32_t data32;
@@ -559,7 +565,7 @@ static ssize_t geos_gpio_show(struct device *dev, struct device_attribute *attr,
 static ssize_t hardware_show(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct geos_gpio_attr *gattr = container_of(attr, struct geos_gpio_attr, attr);
 	struct solos_card *card = pci_get_drvdata(pdev);
 	uint32_t data32;
diff --git a/drivers/base/base.h b/drivers/base/base.h
index 1782f3aa386e..e05db388bd1c 100644
--- a/drivers/base/base.h
+++ b/drivers/base/base.h
@@ -131,6 +131,8 @@ extern void device_remove_groups(struct device *dev,
 extern char *make_class_name(const char *name, struct kobject *kobj);
 
 extern int devres_release_all(struct device *dev);
+extern void device_block_probing(void);
+extern void device_unblock_probing(void);
 
 /* /sys/devices directory */
 extern struct kset *devices_kset;
diff --git a/drivers/base/component.c b/drivers/base/component.c
index f748430bb654..04a1582e80bb 100644
--- a/drivers/base/component.c
+++ b/drivers/base/component.c
@@ -18,18 +18,24 @@
 #include <linux/mutex.h>
 #include <linux/slab.h>
 
+struct component;
+
+struct component_match_array {
+	void *data;
+	int (*compare)(struct device *, void *);
+	void (*release)(struct device *, void *);
+	struct component *component;
+	bool duplicate;
+};
+
 struct component_match {
 	size_t alloc;
 	size_t num;
-	struct {
-		void *data;
-		int (*fn)(struct device *, void *);
-	} compare[0];
+	struct component_match_array *compare;
 };
 
 struct master {
 	struct list_head node;
-	struct list_head components;
 	bool bound;
 
 	const struct component_master_ops *ops;
@@ -39,7 +45,6 @@ struct master {
 
 struct component {
 	struct list_head node;
-	struct list_head master_node;
 	struct master *master;
 	bool bound;
 
@@ -63,48 +68,21 @@ static struct master *__master_find(struct device *dev,
 	return NULL;
 }
 
-/* Attach an unattached component to a master. */
-static void component_attach_master(struct master *master, struct component *c)
-{
-	c->master = master;
-
-	list_add_tail(&c->master_node, &master->components);
-}
-
-/* Detach a component from a master. */
-static void component_detach_master(struct master *master, struct component *c)
-{
-	list_del(&c->master_node);
-
-	c->master = NULL;
-}
-
-/*
- * Add a component to a master, finding the component via the compare
- * function and compare data.  This is safe to call for duplicate matches
- * and will not result in the same component being added multiple times.
- */
-int component_master_add_child(struct master *master,
+static struct component *find_component(struct master *master,
 	int (*compare)(struct device *, void *), void *compare_data)
 {
 	struct component *c;
-	int ret = -ENXIO;
 
 	list_for_each_entry(c, &component_list, node) {
 		if (c->master && c->master != master)
 			continue;
 
-		if (compare(c->dev, compare_data)) {
-			if (!c->master)
-				component_attach_master(master, c);
-			ret = 0;
-			break;
-		}
+		if (compare(c->dev, compare_data))
+			return c;
 	}
 
-	return ret;
+	return NULL;
 }
-EXPORT_SYMBOL_GPL(component_master_add_child);
 
 static int find_components(struct master *master)
 {
@@ -112,39 +90,44 @@ static int find_components(struct master *master)
 	size_t i;
 	int ret = 0;
 
-	if (!match) {
-		/*
-		 * Search the list of components, looking for components that
-		 * belong to this master, and attach them to the master.
-		 */
-		return master->ops->add_components(master->dev, master);
-	}
-
 	/*
 	 * Scan the array of match functions and attach
 	 * any components which are found to this master.
 	 */
 	for (i = 0; i < match->num; i++) {
-		ret = component_master_add_child(master,
-						 match->compare[i].fn,
-						 match->compare[i].data);
-		if (ret)
+		struct component_match_array *mc = &match->compare[i];
+		struct component *c;
+
+		dev_dbg(master->dev, "Looking for component %zu\n", i);
+
+		if (match->compare[i].component)
+			continue;
+
+		c = find_component(master, mc->compare, mc->data);
+		if (!c) {
+			ret = -ENXIO;
 			break;
+		}
+
+		dev_dbg(master->dev, "found component %s, duplicate %u\n", dev_name(c->dev), !!c->master);
+
+		/* Attach this component to the master */
+		match->compare[i].duplicate = !!c->master;
+		match->compare[i].component = c;
+		c->master = master;
 	}
 	return ret;
 }
 
-/* Detach all attached components from this master */
-static void master_remove_components(struct master *master)
+/* Detach component from associated master */
+static void remove_component(struct master *master, struct component *c)
 {
-	while (!list_empty(&master->components)) {
-		struct component *c = list_first_entry(&master->components,
-					struct component, master_node);
-
-		WARN_ON(c->master != master);
+	size_t i;
 
-		component_detach_master(master, c);
-	}
+	/* Detach the component from this master. */
+	for (i = 0; i < master->match->num; i++)
+		if (master->match->compare[i].component == c)
+			master->match->compare[i].component = NULL;
 }
 
 /*
@@ -159,44 +142,32 @@ static int try_to_bring_up_master(struct master *master,
 {
 	int ret;
 
-	if (master->bound)
-		return 0;
+	dev_dbg(master->dev, "trying to bring up master\n");
 
-	/*
-	 * Search the list of components, looking for components that
-	 * belong to this master, and attach them to the master.
-	 */
 	if (find_components(master)) {
-		/* Failed to find all components */
-		ret = 0;
-		goto out;
+		dev_dbg(master->dev, "master has incomplete components\n");
+		return 0;
 	}
 
 	if (component && component->master != master) {
-		ret = 0;
-		goto out;
+		dev_dbg(master->dev, "master is not for this component (%s)\n",
+			dev_name(component->dev));
+		return 0;
 	}
 
-	if (!devres_open_group(master->dev, NULL, GFP_KERNEL)) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	if (!devres_open_group(master->dev, NULL, GFP_KERNEL))
+		return -ENOMEM;
 
 	/* Found all components */
 	ret = master->ops->bind(master->dev);
 	if (ret < 0) {
 		devres_release_group(master->dev, NULL);
 		dev_info(master->dev, "master bind failed: %d\n", ret);
-		goto out;
+		return ret;
 	}
 
 	master->bound = true;
 	return 1;
-
-out:
-	master_remove_components(master);
-
-	return ret;
 }
 
 static int try_to_bring_up_masters(struct component *component)
@@ -205,9 +176,11 @@ static int try_to_bring_up_masters(struct component *component)
 	int ret = 0;
 
 	list_for_each_entry(m, &masters, node) {
-		ret = try_to_bring_up_master(m, component);
-		if (ret != 0)
-			break;
+		if (!m->bound) {
+			ret = try_to_bring_up_master(m, component);
+			if (ret != 0)
+				break;
+		}
 	}
 
 	return ret;
@@ -220,45 +193,59 @@ static void take_down_master(struct master *master)
 		devres_release_group(master->dev, NULL);
 		master->bound = false;
 	}
+}
+
+static void component_match_release(struct device *master,
+	struct component_match *match)
+{
+	unsigned int i;
+
+	for (i = 0; i < match->num; i++) {
+		struct component_match_array *mc = &match->compare[i];
+
+		if (mc->release)
+			mc->release(master, mc->data);
+	}
 
-	master_remove_components(master);
+	kfree(match->compare);
 }
 
-static size_t component_match_size(size_t num)
+static void devm_component_match_release(struct device *dev, void *res)
 {
-	return offsetof(struct component_match, compare[num]);
+	component_match_release(dev, res);
 }
 
-static struct component_match *component_match_realloc(struct device *dev,
+static int component_match_realloc(struct device *dev,
 	struct component_match *match, size_t num)
 {
-	struct component_match *new;
+	struct component_match_array *new;
 
-	if (match && match->alloc == num)
-		return match;
+	if (match->alloc == num)
+		return 0;
 
-	new = devm_kmalloc(dev, component_match_size(num), GFP_KERNEL);
+	new = kmalloc_array(num, sizeof(*new), GFP_KERNEL);
 	if (!new)
-		return ERR_PTR(-ENOMEM);
+		return -ENOMEM;
 
-	if (match) {
-		memcpy(new, match, component_match_size(min(match->num, num)));
-		devm_kfree(dev, match);
-	} else {
-		new->num = 0;
+	if (match->compare) {
+		memcpy(new, match->compare, sizeof(*new) *
+					    min(match->num, num));
+		kfree(match->compare);
 	}
+	match->compare = new;
+	match->alloc = num;
 
-	new->alloc = num;
-
-	return new;
+	return 0;
 }
 
 /*
- * Add a component to be matched.
+ * Add a component to be matched, with a release function.
  *
  * The match array is first created or extended if necessary.
  */
-void component_match_add(struct device *dev, struct component_match **matchptr,
+void component_match_add_release(struct device *master,
+	struct component_match **matchptr,
+	void (*release)(struct device *, void *),
 	int (*compare)(struct device *, void *), void *compare_data)
 {
 	struct component_match *match = *matchptr;
@@ -266,22 +253,55 @@ void component_match_add(struct device *dev, struct component_match **matchptr,
 	if (IS_ERR(match))
 		return;
 
-	if (!match || match->num == match->alloc) {
-		size_t new_size = match ? match->alloc + 16 : 15;
+	if (!match) {
+		match = devres_alloc(devm_component_match_release,
+				     sizeof(*match), GFP_KERNEL);
+		if (!match) {
+			*matchptr = ERR_PTR(-ENOMEM);
+			return;
+		}
 
-		match = component_match_realloc(dev, match, new_size);
+		devres_add(master, match);
 
 		*matchptr = match;
+	}
+
+	if (match->num == match->alloc) {
+		size_t new_size = match ? match->alloc + 16 : 15;
+		int ret;
 
-		if (IS_ERR(match))
+		ret = component_match_realloc(master, match, new_size);
+		if (ret) {
+			*matchptr = ERR_PTR(ret);
 			return;
+		}
 	}
 
-	match->compare[match->num].fn = compare;
+	match->compare[match->num].compare = compare;
+	match->compare[match->num].release = release;
 	match->compare[match->num].data = compare_data;
+	match->compare[match->num].component = NULL;
 	match->num++;
 }
-EXPORT_SYMBOL(component_match_add);
+EXPORT_SYMBOL(component_match_add_release);
+
+static void free_master(struct master *master)
+{
+	struct component_match *match = master->match;
+	int i;
+
+	list_del(&master->node);
+
+	if (match) {
+		for (i = 0; i < match->num; i++) {
+			struct component *c = match->compare[i].component;
+			if (c)
+				c->master = NULL;
+		}
+	}
+
+	kfree(master);
+}
 
 int component_master_add_with_match(struct device *dev,
 	const struct component_master_ops *ops,
@@ -290,15 +310,10 @@ int component_master_add_with_match(struct device *dev,
 	struct master *master;
 	int ret;
 
-	if (ops->add_components && match)
-		return -EINVAL;
-
-	if (match) {
-		/* Reallocate the match array for its true size */
-		match = component_match_realloc(dev, match, match->num);
-		if (IS_ERR(match))
-			return PTR_ERR(match);
-	}
+	/* Reallocate the match array for its true size */
+	ret = component_match_realloc(dev, match, match->num);
+	if (ret)
+		return ret;
 
 	master = kzalloc(sizeof(*master), GFP_KERNEL);
 	if (!master)
@@ -307,7 +322,6 @@ int component_master_add_with_match(struct device *dev,
 	master->dev = dev;
 	master->ops = ops;
 	master->match = match;
-	INIT_LIST_HEAD(&master->components);
 
 	/* Add to the list of available masters. */
 	mutex_lock(&component_mutex);
@@ -315,24 +329,15 @@ int component_master_add_with_match(struct device *dev,
 
 	ret = try_to_bring_up_master(master, NULL);
 
-	if (ret < 0) {
-		/* Delete off the list if we weren't successful */
-		list_del(&master->node);
-		kfree(master);
-	}
+	if (ret < 0)
+		free_master(master);
+
 	mutex_unlock(&component_mutex);
 
 	return ret < 0 ? ret : 0;
 }
 EXPORT_SYMBOL_GPL(component_master_add_with_match);
 
-int component_master_add(struct device *dev,
-	const struct component_master_ops *ops)
-{
-	return component_master_add_with_match(dev, ops, NULL);
-}
-EXPORT_SYMBOL_GPL(component_master_add);
-
 void component_master_del(struct device *dev,
 	const struct component_master_ops *ops)
 {
@@ -342,9 +347,7 @@ void component_master_del(struct device *dev,
 	master = __master_find(dev, ops);
 	if (master) {
 		take_down_master(master);
-
-		list_del(&master->node);
-		kfree(master);
+		free_master(master);
 	}
 	mutex_unlock(&component_mutex);
 }
@@ -366,6 +369,7 @@ void component_unbind_all(struct device *master_dev, void *data)
 {
 	struct master *master;
 	struct component *c;
+	size_t i;
 
 	WARN_ON(!mutex_is_locked(&component_mutex));
 
@@ -373,8 +377,12 @@ void component_unbind_all(struct device *master_dev, void *data)
 	if (!master)
 		return;
 
-	list_for_each_entry_reverse(c, &master->components, master_node)
-		component_unbind(c, master, data);
+	/* Unbind components in reverse order */
+	for (i = master->match->num; i--; )
+		if (!master->match->compare[i].duplicate) {
+			c = master->match->compare[i].component;
+			component_unbind(c, master, data);
+		}
 }
 EXPORT_SYMBOL_GPL(component_unbind_all);
 
@@ -434,6 +442,7 @@ int component_bind_all(struct device *master_dev, void *data)
 {
 	struct master *master;
 	struct component *c;
+	size_t i;
 	int ret = 0;
 
 	WARN_ON(!mutex_is_locked(&component_mutex));
@@ -442,16 +451,21 @@ int component_bind_all(struct device *master_dev, void *data)
 	if (!master)
 		return -EINVAL;
 
-	list_for_each_entry(c, &master->components, master_node) {
-		ret = component_bind(c, master, data);
-		if (ret)
-			break;
-	}
+	/* Bind components in match order */
+	for (i = 0; i < master->match->num; i++)
+		if (!master->match->compare[i].duplicate) {
+			c = master->match->compare[i].component;
+			ret = component_bind(c, master, data);
+			if (ret)
+				break;
+		}
 
 	if (ret != 0) {
-		list_for_each_entry_continue_reverse(c, &master->components,
-						     master_node)
-			component_unbind(c, master, data);
+		for (; i--; )
+			if (!master->match->compare[i].duplicate) {
+				c = master->match->compare[i].component;
+				component_unbind(c, master, data);
+			}
 	}
 
 	return ret;
@@ -477,6 +491,8 @@ int component_add(struct device *dev, const struct component_ops *ops)
 
 	ret = try_to_bring_up_masters(component);
 	if (ret < 0) {
+		if (component->master)
+			remove_component(component->master, component);
 		list_del(&component->node);
 
 		kfree(component);
@@ -499,8 +515,10 @@ void component_del(struct device *dev, const struct component_ops *ops)
 			break;
 		}
 
-	if (component && component->master)
+	if (component && component->master) {
 		take_down_master(component->master);
+		remove_component(component->master, component);
+	}
 
 	mutex_unlock(&component_mutex);
 
diff --git a/drivers/base/core.c b/drivers/base/core.c
index b7d56c5ea3c6..0a8bdade53f2 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -2261,7 +2261,10 @@ void set_primary_fwnode(struct device *dev, struct fwnode_handle *fwnode)
 		if (fwnode_is_primary(fn))
 			fn = fn->secondary;
 
-		fwnode->secondary = fn;
+		if (fn) {
+			WARN_ON(fwnode->secondary);
+			fwnode->secondary = fn;
+		}
 		dev->fwnode = fwnode;
 	} else {
 		dev->fwnode = fwnode_is_primary(dev->fwnode) ?
diff --git a/drivers/base/cpu.c b/drivers/base/cpu.c
index 91bbb1959d8d..691eeea2f19a 100644
--- a/drivers/base/cpu.c
+++ b/drivers/base/cpu.c
@@ -200,7 +200,7 @@ static const struct attribute_group *hotplugable_cpu_attr_groups[] = {
 
 struct cpu_attr {
 	struct device_attribute attr;
-	const struct cpumask *const * const map;
+	const struct cpumask *const map;
 };
 
 static ssize_t show_cpus_attr(struct device *dev,
@@ -209,7 +209,7 @@ static ssize_t show_cpus_attr(struct device *dev,
 {
 	struct cpu_attr *ca = container_of(attr, struct cpu_attr, attr);
 
-	return cpumap_print_to_pagebuf(true, buf, *ca->map);
+	return cpumap_print_to_pagebuf(true, buf, ca->map);
 }
 
 #define _CPU_ATTR(name, map) \
@@ -217,9 +217,9 @@ static ssize_t show_cpus_attr(struct device *dev,
 
 /* Keep in sync with cpu_subsys_attrs */
 static struct cpu_attr cpu_attrs[] = {
-	_CPU_ATTR(online, &cpu_online_mask),
-	_CPU_ATTR(possible, &cpu_possible_mask),
-	_CPU_ATTR(present, &cpu_present_mask),
+	_CPU_ATTR(online, &__cpu_online_mask),
+	_CPU_ATTR(possible, &__cpu_possible_mask),
+	_CPU_ATTR(present, &__cpu_present_mask),
 };
 
 /*
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index a641cf3ccad6..c4da2df62e02 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -55,6 +55,13 @@ static struct workqueue_struct *deferred_wq;
 static atomic_t deferred_trigger_count = ATOMIC_INIT(0);
 
 /*
+ * In some cases, like suspend to RAM or hibernation, It might be reasonable
+ * to prohibit probing of devices as it could be unsafe.
+ * Once defer_all_probes is true all drivers probes will be forcibly deferred.
+ */
+static bool defer_all_probes;
+
+/*
  * deferred_probe_work_func() - Retry probing devices in the active list.
  */
 static void deferred_probe_work_func(struct work_struct *work)
@@ -172,6 +179,30 @@ static void driver_deferred_probe_trigger(void)
 }
 
 /**
+ * device_block_probing() - Block/defere device's probes
+ *
+ *	It will disable probing of devices and defer their probes instead.
+ */
+void device_block_probing(void)
+{
+	defer_all_probes = true;
+	/* sync with probes to avoid races. */
+	wait_for_device_probe();
+}
+
+/**
+ * device_unblock_probing() - Unblock/enable device's probes
+ *
+ *	It will restore normal behavior and trigger re-probing of deferred
+ * devices.
+ */
+void device_unblock_probing(void)
+{
+	defer_all_probes = false;
+	driver_deferred_probe_trigger();
+}
+
+/**
  * deferred_probe_initcall() - Enable probing of deferred devices
  *
  * We don't want to get in the way when the bulk of drivers are getting probed.
@@ -192,9 +223,23 @@ static int deferred_probe_initcall(void)
 }
 late_initcall(deferred_probe_initcall);
 
+/**
+ * device_is_bound() - Check if device is bound to a driver
+ * @dev: device to check
+ *
+ * Returns true if passed device has already finished probing successfully
+ * against a driver.
+ *
+ * This function must be called with the device lock held.
+ */
+bool device_is_bound(struct device *dev)
+{
+	return dev->p && klist_node_attached(&dev->p->knode_driver);
+}
+
 static void driver_bound(struct device *dev)
 {
-	if (klist_node_attached(&dev->p->knode_driver)) {
+	if (device_is_bound(dev)) {
 		printk(KERN_WARNING "%s: device %s already bound\n",
 			__func__, kobject_name(&dev->kobj));
 		return;
@@ -205,6 +250,8 @@ static void driver_bound(struct device *dev)
 
 	klist_add_tail(&dev->p->knode_driver, &dev->driver->p->klist_devices);
 
+	device_pm_check_callbacks(dev);
+
 	/*
 	 * Make sure the device is no longer in one of the deferred lists and
 	 * kick off retrying all pending devices
@@ -268,6 +315,9 @@ int device_bind_driver(struct device *dev)
 	ret = driver_sysfs_add(dev);
 	if (!ret)
 		driver_bound(dev);
+	else if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_DRIVER_NOT_BOUND, dev);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(device_bind_driver);
@@ -277,9 +327,20 @@ static DECLARE_WAIT_QUEUE_HEAD(probe_waitqueue);
 
 static int really_probe(struct device *dev, struct device_driver *drv)
 {
-	int ret = 0;
+	int ret = -EPROBE_DEFER;
 	int local_trigger_count = atomic_read(&deferred_trigger_count);
 
+	if (defer_all_probes) {
+		/*
+		 * Value of defer_all_probes can be set only by
+		 * device_defer_all_probes_enable() which, in turn, will call
+		 * wait_for_device_probe() right after that to avoid any races.
+		 */
+		dev_dbg(dev, "Driver %s force probe deferral\n", drv->name);
+		driver_deferred_probe_add(dev);
+		return ret;
+	}
+
 	atomic_inc(&probe_count);
 	pr_debug("bus: '%s': %s: probing driver %s with device %s\n",
 		 drv->bus->name, __func__, drv->name, dev_name(dev));
@@ -290,7 +351,7 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 	/* If using pinctrl, bind pins now before probing */
 	ret = pinctrl_bind_pins(dev);
 	if (ret)
-		goto probe_failed;
+		goto pinctrl_bind_failed;
 
 	if (driver_sysfs_add(dev)) {
 		printk(KERN_ERR "%s: driver_sysfs_add(%s) failed\n",
@@ -334,12 +395,17 @@ static int really_probe(struct device *dev, struct device_driver *drv)
 	goto done;
 
 probe_failed:
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_DRIVER_NOT_BOUND, dev);
+pinctrl_bind_failed:
 	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);
 
 	switch (ret) {
 	case -EPROBE_DEFER:
@@ -393,6 +459,10 @@ 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);
+
 	/* wait for the known devices to complete their probing */
 	wait_event(probe_waitqueue, atomic_read(&probe_count) == 0);
 	async_synchronize_full();
@@ -547,7 +617,7 @@ static int __device_attach(struct device *dev, bool allow_async)
 
 	device_lock(dev);
 	if (dev->driver) {
-		if (klist_node_attached(&dev->p->knode_driver)) {
+		if (device_is_bound(dev)) {
 			ret = 1;
 			goto out_unlock;
 		}
@@ -695,13 +765,14 @@ static void __device_release_driver(struct device *dev)
 		dev_set_drvdata(dev, NULL);
 		if (dev->pm_domain && dev->pm_domain->dismiss)
 			dev->pm_domain->dismiss(dev);
+		pm_runtime_reinit(dev);
 
 		klist_remove(&dev->p->knode_driver);
+		device_pm_check_callbacks(dev);
 		if (dev->bus)
 			blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
 						     BUS_NOTIFY_UNBOUND_DRIVER,
 						     dev);
-
 	}
 }
 
diff --git a/drivers/base/devtmpfs.c b/drivers/base/devtmpfs.c
index 68f03141e432..44a74cf1372c 100644
--- a/drivers/base/devtmpfs.c
+++ b/drivers/base/devtmpfs.c
@@ -215,9 +215,9 @@ static int handle_create(const char *nodename, umode_t mode, kuid_t uid,
 		newattrs.ia_uid = uid;
 		newattrs.ia_gid = gid;
 		newattrs.ia_valid = ATTR_MODE|ATTR_UID|ATTR_GID;
-		mutex_lock(&d_inode(dentry)->i_mutex);
+		inode_lock(d_inode(dentry));
 		notify_change(dentry, &newattrs, NULL);
-		mutex_unlock(&d_inode(dentry)->i_mutex);
+		inode_unlock(d_inode(dentry));
 
 		/* mark as kernel-created inode */
 		d_inode(dentry)->i_private = &thread;
@@ -244,7 +244,7 @@ static int dev_rmdir(const char *name)
 		err = -ENOENT;
 	}
 	dput(dentry);
-	mutex_unlock(&d_inode(parent.dentry)->i_mutex);
+	inode_unlock(d_inode(parent.dentry));
 	path_put(&parent);
 	return err;
 }
@@ -321,9 +321,9 @@ static int handle_remove(const char *nodename, struct device *dev)
 			newattrs.ia_mode = stat.mode & ~0777;
 			newattrs.ia_valid =
 				ATTR_UID|ATTR_GID|ATTR_MODE;
-			mutex_lock(&d_inode(dentry)->i_mutex);
+			inode_lock(d_inode(dentry));
 			notify_change(dentry, &newattrs, NULL);
-			mutex_unlock(&d_inode(dentry)->i_mutex);
+			inode_unlock(d_inode(dentry));
 			err = vfs_unlink(d_inode(parent.dentry), dentry, NULL);
 			if (!err || err == -ENOENT)
 				deleted = 1;
@@ -332,7 +332,7 @@ static int handle_remove(const char *nodename, struct device *dev)
 		err = -ENOENT;
 	}
 	dput(dentry);
-	mutex_unlock(&d_inode(parent.dentry)->i_mutex);
+	inode_unlock(d_inode(parent.dentry));
 
 	path_put(&parent);
 	if (deleted && strchr(nodename, '/'))
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index d95c5971c225..d799662f19eb 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -12,7 +12,6 @@
 #include <linux/gfp.h>
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
-#include <asm-generic/dma-coherent.h>
 
 /*
  * Managed DMA API
@@ -167,7 +166,7 @@ void dmam_free_noncoherent(struct device *dev, size_t size, void *vaddr,
 }
 EXPORT_SYMBOL(dmam_free_noncoherent);
 
-#ifdef ARCH_HAS_DMA_DECLARE_COHERENT_MEMORY
+#ifdef CONFIG_HAVE_GENERIC_DMA_COHERENT
 
 static void dmam_coherent_decl_release(struct device *dev, void *res)
 {
@@ -247,7 +246,7 @@ int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
 		    void *cpu_addr, dma_addr_t dma_addr, size_t size)
 {
 	int ret = -ENXIO;
-#ifdef CONFIG_MMU
+#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 count = PAGE_ALIGN(size) >> PAGE_SHIFT;
 	unsigned long pfn = page_to_pfn(virt_to_page(cpu_addr));
@@ -264,7 +263,7 @@ int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
 				      user_count << PAGE_SHIFT,
 				      vma->vm_page_prot);
 	}
-#endif	/* CONFIG_MMU */
+#endif	/* CONFIG_MMU && !CONFIG_ARCH_NO_COHERENT_DMA_MMAP */
 
 	return ret;
 }
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index 8524450e75bd..b9250e564ebf 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -1118,15 +1118,17 @@ static int
 _request_firmware(const struct firmware **firmware_p, const char *name,
 		  struct device *device, unsigned int opt_flags)
 {
-	struct firmware *fw;
+	struct firmware *fw = NULL;
 	long timeout;
 	int ret;
 
 	if (!firmware_p)
 		return -EINVAL;
 
-	if (!name || name[0] == '\0')
-		return -EINVAL;
+	if (!name || name[0] == '\0') {
+		ret = -EINVAL;
+		goto out;
+	}
 
 	ret = _request_firmware_prepare(&fw, name, device);
 	if (ret <= 0) /* error or already assigned */
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index 25425d3f2575..213456c2b123 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -450,8 +450,7 @@ memory_probe_store(struct device *dev, struct device_attribute *attr,
 		   const char *buf, size_t count)
 {
 	u64 phys_addr;
-	int nid;
-	int i, ret;
+	int nid, ret;
 	unsigned long pages_per_block = PAGES_PER_SECTION * sections_per_block;
 
 	ret = kstrtoull(buf, 0, &phys_addr);
@@ -461,15 +460,12 @@ memory_probe_store(struct device *dev, struct device_attribute *attr,
 	if (phys_addr & ((pages_per_block << PAGE_SHIFT) - 1))
 		return -EINVAL;
 
-	for (i = 0; i < sections_per_block; i++) {
-		nid = memory_add_physaddr_to_nid(phys_addr);
-		ret = add_memory(nid, phys_addr,
-				 PAGES_PER_SECTION << PAGE_SHIFT);
-		if (ret)
-			goto out;
+	nid = memory_add_physaddr_to_nid(phys_addr);
+	ret = add_memory(nid, phys_addr,
+			 MIN_MEMORY_BLOCK_SIZE * sections_per_block);
 
-		phys_addr += MIN_MEMORY_BLOCK_SIZE;
-	}
+	if (ret)
+		goto out;
 
 	ret = count;
 out:
@@ -618,7 +614,6 @@ static int init_memory_block(struct memory_block **memory,
 			base_memory_block_id(scn_nr) * sections_per_block;
 	mem->end_section_nr = mem->start_section_nr + sections_per_block - 1;
 	mem->state = state;
-	mem->section_count++;
 	start_pfn = section_nr_to_pfn(mem->start_section_nr);
 	mem->phys_device = arch_get_memory_phys_device(start_pfn);
 
@@ -652,6 +647,13 @@ static int add_memory_block(int base_section_nr)
 	return 0;
 }
 
+static bool is_zone_device_section(struct mem_section *ms)
+{
+	struct page *page;
+
+	page = sparse_decode_mem_map(ms->section_mem_map, __section_nr(ms));
+	return is_zone_device_page(page);
+}
 
 /*
  * need an interface for the VM to add new memory regions,
@@ -662,6 +664,9 @@ int register_new_memory(int nid, struct mem_section *section)
 	int ret = 0;
 	struct memory_block *mem;
 
+	if (is_zone_device_section(section))
+		return 0;
+
 	mutex_lock(&mem_sysfs_mutex);
 
 	mem = find_memory_block(section);
@@ -672,6 +677,7 @@ int register_new_memory(int nid, struct mem_section *section)
 		ret = init_memory_block(&mem, section, MEM_OFFLINE);
 		if (ret)
 			goto out;
+		mem->section_count++;
 	}
 
 	if (mem->section_count == sections_per_block)
@@ -692,11 +698,14 @@ unregister_memory(struct memory_block *memory)
 	device_unregister(&memory->dev);
 }
 
-static int remove_memory_block(unsigned long node_id,
+static int remove_memory_section(unsigned long node_id,
 			       struct mem_section *section, int phys_device)
 {
 	struct memory_block *mem;
 
+	if (is_zone_device_section(section))
+		return 0;
+
 	mutex_lock(&mem_sysfs_mutex);
 	mem = find_memory_block(section);
 	unregister_mem_sect_under_nodes(mem, __section_nr(section));
@@ -716,7 +725,7 @@ int unregister_memory_section(struct mem_section *section)
 	if (!present_section(section))
 		return -EINVAL;
 
-	return remove_memory_block(0, section, 0);
+	return remove_memory_section(0, section, 0);
 }
 #endif /* CONFIG_MEMORY_HOTREMOVE */
 
diff --git a/drivers/base/platform-msi.c b/drivers/base/platform-msi.c
index 5df4575b5ba7..279e53989374 100644
--- a/drivers/base/platform-msi.c
+++ b/drivers/base/platform-msi.c
@@ -24,13 +24,17 @@
 #include <linux/msi.h>
 #include <linux/slab.h>
 
-#define DEV_ID_SHIFT	24
+#define DEV_ID_SHIFT	21
+#define MAX_DEV_MSIS	(1 << (32 - DEV_ID_SHIFT))
 
 /*
  * Internal data structure containing a (made up, but unique) devid
  * and the callback to write the MSI message.
  */
 struct platform_msi_priv_data {
+	struct device		*dev;
+	void 			*host_data;
+	msi_alloc_info_t	arg;
 	irq_write_msi_msg_t	write_msg;
 	int			devid;
 };
@@ -110,39 +114,49 @@ static void platform_msi_update_chip_ops(struct msi_domain_info *info)
 		chip->irq_write_msi_msg = platform_msi_write_msg;
 }
 
-static void platform_msi_free_descs(struct device *dev)
+static void platform_msi_free_descs(struct device *dev, int base, int nvec)
 {
 	struct msi_desc *desc, *tmp;
 
 	list_for_each_entry_safe(desc, tmp, dev_to_msi_list(dev), list) {
-		list_del(&desc->list);
-		free_msi_entry(desc);
+		if (desc->platform.msi_index >= base &&
+		    desc->platform.msi_index < (base + nvec)) {
+			list_del(&desc->list);
+			free_msi_entry(desc);
+		}
 	}
 }
 
-static int platform_msi_alloc_descs(struct device *dev, int nvec,
-				    struct platform_msi_priv_data *data)
+static int platform_msi_alloc_descs_with_irq(struct device *dev, int virq,
+					     int nvec,
+					     struct platform_msi_priv_data *data)
 
 {
-	int i;
+	struct msi_desc *desc;
+	int i, base = 0;
 
-	for (i = 0; i < nvec; i++) {
-		struct msi_desc *desc;
+	if (!list_empty(dev_to_msi_list(dev))) {
+		desc = list_last_entry(dev_to_msi_list(dev),
+				       struct msi_desc, list);
+		base = desc->platform.msi_index + 1;
+	}
 
+	for (i = 0; i < nvec; i++) {
 		desc = alloc_msi_entry(dev);
 		if (!desc)
 			break;
 
 		desc->platform.msi_priv_data = data;
-		desc->platform.msi_index = i;
+		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));
 	}
 
 	if (i != nvec) {
 		/* Clean up the mess */
-		platform_msi_free_descs(dev);
+		platform_msi_free_descs(dev, base, nvec);
 
 		return -ENOMEM;
 	}
@@ -150,6 +164,13 @@ static int platform_msi_alloc_descs(struct device *dev, int nvec,
 	return 0;
 }
 
+static int platform_msi_alloc_descs(struct device *dev, int nvec,
+				    struct platform_msi_priv_data *data)
+
+{
+	return platform_msi_alloc_descs_with_irq(dev, 0, nvec, data);
+}
+
 /**
  * platform_msi_create_irq_domain - Create a platform MSI interrupt domain
  * @fwnode:		Optional fwnode of the interrupt controller
@@ -180,56 +201,75 @@ struct irq_domain *platform_msi_create_irq_domain(struct fwnode_handle *fwnode,
 	return domain;
 }
 
-/**
- * platform_msi_domain_alloc_irqs - Allocate MSI interrupts for @dev
- * @dev:		The device for which to allocate interrupts
- * @nvec:		The number of interrupts to allocate
- * @write_msi_msg:	Callback to write an interrupt message for @dev
- *
- * Returns:
- * Zero for success, or an error code in case of failure
- */
-int platform_msi_domain_alloc_irqs(struct device *dev, unsigned int nvec,
-				   irq_write_msi_msg_t write_msi_msg)
+static struct platform_msi_priv_data *
+platform_msi_alloc_priv_data(struct device *dev, unsigned int nvec,
+			     irq_write_msi_msg_t write_msi_msg)
 {
-	struct platform_msi_priv_data *priv_data;
-	int err;
-
+	struct platform_msi_priv_data *datap;
 	/*
 	 * Limit the number of interrupts to 256 per device. Should we
 	 * need to bump this up, DEV_ID_SHIFT should be adjusted
 	 * accordingly (which would impact the max number of MSI
 	 * capable devices).
 	 */
-	if (!dev->msi_domain || !write_msi_msg || !nvec ||
-	    nvec > (1 << (32 - DEV_ID_SHIFT)))
-		return -EINVAL;
+	if (!dev->msi_domain || !write_msi_msg || !nvec || nvec > MAX_DEV_MSIS)
+		return ERR_PTR(-EINVAL);
 
 	if (dev->msi_domain->bus_token != DOMAIN_BUS_PLATFORM_MSI) {
 		dev_err(dev, "Incompatible msi_domain, giving up\n");
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	}
 
 	/* Already had a helping of MSI? Greed... */
 	if (!list_empty(dev_to_msi_list(dev)))
-		return -EBUSY;
+		return ERR_PTR(-EBUSY);
+
+	datap = kzalloc(sizeof(*datap), GFP_KERNEL);
+	if (!datap)
+		return ERR_PTR(-ENOMEM);
+
+	datap->devid = ida_simple_get(&platform_msi_devid_ida,
+				      0, 1 << DEV_ID_SHIFT, GFP_KERNEL);
+	if (datap->devid < 0) {
+		int err = datap->devid;
+		kfree(datap);
+		return ERR_PTR(err);
+	}
 
-	priv_data = kzalloc(sizeof(*priv_data), GFP_KERNEL);
-	if (!priv_data)
-		return -ENOMEM;
+	datap->write_msg = write_msi_msg;
+	datap->dev = dev;
 
-	priv_data->devid = ida_simple_get(&platform_msi_devid_ida,
-					  0, 1 << DEV_ID_SHIFT, GFP_KERNEL);
-	if (priv_data->devid < 0) {
-		err = priv_data->devid;
-		goto out_free_data;
-	}
+	return datap;
+}
+
+static void platform_msi_free_priv_data(struct platform_msi_priv_data *data)
+{
+	ida_simple_remove(&platform_msi_devid_ida, data->devid);
+	kfree(data);
+}
+
+/**
+ * platform_msi_domain_alloc_irqs - Allocate MSI interrupts for @dev
+ * @dev:		The device for which to allocate interrupts
+ * @nvec:		The number of interrupts to allocate
+ * @write_msi_msg:	Callback to write an interrupt message for @dev
+ *
+ * Returns:
+ * Zero for success, or an error code in case of failure
+ */
+int platform_msi_domain_alloc_irqs(struct device *dev, unsigned int nvec,
+				   irq_write_msi_msg_t write_msi_msg)
+{
+	struct platform_msi_priv_data *priv_data;
+	int err;
 
-	priv_data->write_msg = write_msi_msg;
+	priv_data = platform_msi_alloc_priv_data(dev, nvec, write_msi_msg);
+	if (IS_ERR(priv_data))
+		return PTR_ERR(priv_data);
 
 	err = platform_msi_alloc_descs(dev, nvec, priv_data);
 	if (err)
-		goto out_free_id;
+		goto out_free_priv_data;
 
 	err = msi_domain_alloc_irqs(dev->msi_domain, dev, nvec);
 	if (err)
@@ -238,14 +278,13 @@ int platform_msi_domain_alloc_irqs(struct device *dev, unsigned int nvec,
 	return 0;
 
 out_free_desc:
-	platform_msi_free_descs(dev);
-out_free_id:
-	ida_simple_remove(&platform_msi_devid_ida, priv_data->devid);
-out_free_data:
-	kfree(priv_data);
+	platform_msi_free_descs(dev, 0, nvec);
+out_free_priv_data:
+	platform_msi_free_priv_data(priv_data);
 
 	return err;
 }
+EXPORT_SYMBOL_GPL(platform_msi_domain_alloc_irqs);
 
 /**
  * platform_msi_domain_free_irqs - Free MSI interrupts for @dev
@@ -253,18 +292,127 @@ out_free_data:
  */
 void platform_msi_domain_free_irqs(struct device *dev)
 {
-	struct msi_desc *desc;
+	if (!list_empty(dev_to_msi_list(dev))) {
+		struct msi_desc *desc;
+
+		desc = first_msi_entry(dev);
+		platform_msi_free_priv_data(desc->platform.msi_priv_data);
+	}
+
+	msi_domain_free_irqs(dev->msi_domain, dev);
+	platform_msi_free_descs(dev, 0, MAX_DEV_MSIS);
+}
+EXPORT_SYMBOL_GPL(platform_msi_domain_free_irqs);
+
+/**
+ * platform_msi_get_host_data - Query the private data associated with
+ *                              a platform-msi domain
+ * @domain:	The platform-msi domain
+ *
+ * Returns the private data provided when calling
+ * platform_msi_create_device_domain.
+ */
+void *platform_msi_get_host_data(struct irq_domain *domain)
+{
+	struct platform_msi_priv_data *data = domain->host_data;
+	return data->host_data;
+}
+
+/**
+ * platform_msi_create_device_domain - Create a platform-msi domain
+ *
+ * @dev:		The device generating the MSIs
+ * @nvec:		The number of MSIs that need to be allocated
+ * @write_msi_msg:	Callback to write an interrupt message for @dev
+ * @ops:		The hierarchy domain operations to use
+ * @host_data:		Private data associated to this domain
+ *
+ * Returns an irqdomain for @nvec interrupts
+ */
+struct irq_domain *
+platform_msi_create_device_domain(struct device *dev,
+				  unsigned int nvec,
+				  irq_write_msi_msg_t write_msi_msg,
+				  const struct irq_domain_ops *ops,
+				  void *host_data)
+{
+	struct platform_msi_priv_data *data;
+	struct irq_domain *domain;
+	int err;
+
+	data = platform_msi_alloc_priv_data(dev, nvec, write_msi_msg);
+	if (IS_ERR(data))
+		return NULL;
+
+	data->host_data = host_data;
+	domain = irq_domain_create_hierarchy(dev->msi_domain, 0, nvec,
+					     of_node_to_fwnode(dev->of_node),
+					     ops, data);
+	if (!domain)
+		goto free_priv;
 
-	desc = first_msi_entry(dev);
-	if (desc) {
-		struct platform_msi_priv_data *data;
+	err = msi_domain_prepare_irqs(domain->parent, dev, nvec, &data->arg);
+	if (err)
+		goto free_domain;
+
+	return domain;
 
-		data = desc->platform.msi_priv_data;
+free_domain:
+	irq_domain_remove(domain);
+free_priv:
+	platform_msi_free_priv_data(data);
+	return NULL;
+}
+
+/**
+ * platform_msi_domain_free - Free interrupts associated with a platform-msi
+ *                            domain
+ *
+ * @domain:	The platform-msi domain
+ * @virq:	The base irq from which to perform the free operation
+ * @nvec:	How many interrupts to free from @virq
+ */
+void platform_msi_domain_free(struct irq_domain *domain, unsigned int virq,
+			      unsigned int nvec)
+{
+	struct platform_msi_priv_data *data = domain->host_data;
+	struct msi_desc *desc;
+	for_each_msi_entry(desc, data->dev) {
+		if (WARN_ON(!desc->irq || desc->nvec_used != 1))
+			return;
+		if (!(desc->irq >= virq && desc->irq < (virq + nvec)))
+			continue;
 
-		ida_simple_remove(&platform_msi_devid_ida, data->devid);
-		kfree(data);
+		irq_domain_free_irqs_common(domain, desc->irq, 1);
 	}
+}
 
-	msi_domain_free_irqs(dev->msi_domain, dev);
-	platform_msi_free_descs(dev);
+/**
+ * platform_msi_domain_alloc - Allocate interrupts associated with
+ *			       a platform-msi domain
+ *
+ * @domain:	The platform-msi domain
+ * @virq:	The base irq from which to perform the allocate operation
+ * @nvec:	How many interrupts to free from @virq
+ *
+ * Return 0 on success, or an error code on failure. Must be called
+ * with irq_domain_mutex held (which can only be done as part of a
+ * top-level interrupt allocation).
+ */
+int platform_msi_domain_alloc(struct irq_domain *domain, unsigned int virq,
+			      unsigned int nr_irqs)
+{
+	struct platform_msi_priv_data *data = domain->host_data;
+	int err;
+
+	err = platform_msi_alloc_descs_with_irq(data->dev, virq, nr_irqs, data);
+	if (err)
+		return err;
+
+	err = msi_domain_populate_irqs(domain->parent, data->dev,
+				       virq, nr_irqs, &data->arg);
+	if (err)
+		platform_msi_domain_free(domain, virq, nr_irqs);
+
+	return err;
 }
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index 1dd6d3bf1098..f437afa17f2b 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -26,6 +26,7 @@
 #include <linux/acpi.h>
 #include <linux/clk/clk-conf.h>
 #include <linux/limits.h>
+#include <linux/property.h>
 
 #include "base.h"
 #include "power/power.h"
@@ -117,6 +118,26 @@ int platform_get_irq(struct platform_device *dev, unsigned int num)
 EXPORT_SYMBOL_GPL(platform_get_irq);
 
 /**
+ * platform_irq_count - Count the number of IRQs a platform device uses
+ * @dev: platform device
+ *
+ * Return: Number of IRQs a platform device uses or EPROBE_DEFER
+ */
+int platform_irq_count(struct platform_device *dev)
+{
+	int ret, nr = 0;
+
+	while ((ret = platform_get_irq(dev, nr)) >= 0)
+		nr++;
+
+	if (ret == -EPROBE_DEFER)
+		return ret;
+
+	return nr;
+}
+EXPORT_SYMBOL_GPL(platform_irq_count);
+
+/**
  * platform_get_resource_byname - get a resource for a device by name
  * @dev: platform device
  * @type: resource type
@@ -299,6 +320,22 @@ int platform_device_add_data(struct platform_device *pdev, const void *data,
 EXPORT_SYMBOL_GPL(platform_device_add_data);
 
 /**
+ * platform_device_add_properties - add built-in properties to a platform device
+ * @pdev: platform device to add properties to
+ * @pset: properties to add
+ *
+ * The function will take deep copy of the properties in @pset and attach
+ * the copy to the platform device. The memory associated with properties
+ * will be freed when the platform device is released.
+ */
+int platform_device_add_properties(struct platform_device *pdev,
+				   const struct property_set *pset)
+{
+	return device_add_property_set(&pdev->dev, pset);
+}
+EXPORT_SYMBOL_GPL(platform_device_add_properties);
+
+/**
  * platform_device_add - add a platform device to device hierarchy
  * @pdev: platform device we're adding
  *
@@ -409,6 +446,8 @@ void platform_device_del(struct platform_device *pdev)
 			if (r->parent)
 				release_resource(r);
 		}
+
+		device_remove_property_set(&pdev->dev);
 	}
 }
 EXPORT_SYMBOL_GPL(platform_device_del);
@@ -487,6 +526,12 @@ struct platform_device *platform_device_register_full(
 	if (ret)
 		goto err;
 
+	if (pdevinfo->pset) {
+		ret = platform_device_add_properties(pdev, pdevinfo->pset);
+		if (ret)
+			goto err;
+	}
+
 	ret = platform_device_add(pdev);
 	if (ret) {
 err:
@@ -513,10 +558,15 @@ static int platform_drv_probe(struct device *_dev)
 		return ret;
 
 	ret = dev_pm_domain_attach(_dev, true);
-	if (ret != -EPROBE_DEFER && drv->probe) {
-		ret = drv->probe(dev);
-		if (ret)
-			dev_pm_domain_detach(_dev, true);
+	if (ret != -EPROBE_DEFER) {
+		if (drv->probe) {
+			ret = drv->probe(dev);
+			if (ret)
+				dev_pm_domain_detach(_dev, true);
+		} else {
+			/* don't fail if just dev_pm_domain_attach failed */
+			ret = 0;
+		}
 	}
 
 	if (drv->prevent_deferred_probe && ret == -EPROBE_DEFER) {
@@ -552,7 +602,6 @@ static void platform_drv_shutdown(struct device *_dev)
 
 	if (drv->shutdown)
 		drv->shutdown(dev);
-	dev_pm_domain_detach(_dev, true);
 }
 
 /**
diff --git a/drivers/base/power/clock_ops.c b/drivers/base/power/clock_ops.c
index 60ee5591ee8f..272a52ebafc0 100644
--- a/drivers/base/power/clock_ops.c
+++ b/drivers/base/power/clock_ops.c
@@ -15,6 +15,7 @@
 #include <linux/clkdev.h>
 #include <linux/slab.h>
 #include <linux/err.h>
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 
 #ifdef CONFIG_PM_CLK
@@ -348,7 +349,7 @@ static int pm_clk_notify(struct notifier_block *nb,
 		if (error)
 			break;
 
-		dev->pm_domain = clknb->pm_domain;
+		dev_pm_domain_set(dev, clknb->pm_domain);
 		if (clknb->con_ids[0]) {
 			for (con_id = clknb->con_ids; *con_id; con_id++)
 				pm_clk_add(dev, *con_id);
@@ -361,7 +362,7 @@ static int pm_clk_notify(struct notifier_block *nb,
 		if (dev->pm_domain != clknb->pm_domain)
 			break;
 
-		dev->pm_domain = NULL;
+		dev_pm_domain_set(dev, NULL);
 		pm_clk_destroy(dev);
 		break;
 	}
@@ -473,6 +474,7 @@ static int pm_clk_notify(struct notifier_block *nb,
 			enable_clock(dev, NULL);
 		}
 		break;
+	case BUS_NOTIFY_DRIVER_NOT_BOUND:
 	case BUS_NOTIFY_UNBOUND_DRIVER:
 		if (clknb->con_ids[0]) {
 			for (con_id = clknb->con_ids; *con_id; con_id++)
diff --git a/drivers/base/power/common.c b/drivers/base/power/common.c
index f32b802b98f4..f6a9ad52cbbf 100644
--- a/drivers/base/power/common.c
+++ b/drivers/base/power/common.c
@@ -14,6 +14,8 @@
 #include <linux/acpi.h>
 #include <linux/pm_domain.h>
 
+#include "power.h"
+
 /**
  * dev_pm_get_subsys_data - Create or refcount power.subsys_data for device.
  * @dev: Device to handle.
@@ -112,7 +114,7 @@ EXPORT_SYMBOL_GPL(dev_pm_domain_attach);
 
 /**
  * dev_pm_domain_detach - Detach a device from its PM domain.
- * @dev: Device to attach.
+ * @dev: Device to detach.
  * @power_off: Used to indicate whether we should power off the device.
  *
  * This functions will reverse the actions from dev_pm_domain_attach() and thus
@@ -128,3 +130,25 @@ void dev_pm_domain_detach(struct device *dev, bool power_off)
 		dev->pm_domain->detach(dev, power_off);
 }
 EXPORT_SYMBOL_GPL(dev_pm_domain_detach);
+
+/**
+ * dev_pm_domain_set - Set PM domain of a device.
+ * @dev: Device whose PM domain is to be set.
+ * @pd: PM domain to be set, or NULL.
+ *
+ * Sets the PM domain the device belongs to. The PM domain of a device needs
+ * to be set before its probe finishes (it's bound to a driver).
+ *
+ * This function must be called with the device lock held.
+ */
+void dev_pm_domain_set(struct device *dev, struct dev_pm_domain *pd)
+{
+	if (dev->pm_domain == pd)
+		return;
+
+	WARN(pd && device_is_bound(dev),
+	     "PM domains can only be changed for unbound devices\n");
+	dev->pm_domain = pd;
+	device_pm_check_callbacks(dev);
+}
+EXPORT_SYMBOL_GPL(dev_pm_domain_set);
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index 65f50eccd49b..301b785f9f56 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -20,6 +20,8 @@
 #include <linux/suspend.h>
 #include <linux/export.h>
 
+#include "power.h"
+
 #define GENPD_RETRY_MAX_MS	250		/* Approximate */
 
 #define GENPD_DEV_CALLBACK(genpd, type, callback, dev)		\
@@ -160,7 +162,7 @@ static int genpd_power_off(struct generic_pm_domain *genpd, bool timed)
 
 /**
  * genpd_queue_power_off_work - Queue up the execution of genpd_poweroff().
- * @genpd: PM domait to power off.
+ * @genpd: PM domain to power off.
  *
  * Queue up the execution of genpd_poweroff() unless it's already been done
  * before.
@@ -170,16 +172,15 @@ static void genpd_queue_power_off_work(struct generic_pm_domain *genpd)
 	queue_work(pm_wq, &genpd->power_off_work);
 }
 
-static int genpd_poweron(struct generic_pm_domain *genpd);
-
 /**
- * __genpd_poweron - Restore power to a given PM domain and its masters.
+ * genpd_poweron - Restore power to a given PM domain and its masters.
  * @genpd: PM domain to power up.
+ * @depth: nesting count for lockdep.
  *
  * Restore power to @genpd and all of its masters so that it is possible to
  * resume a device belonging to it.
  */
-static int __genpd_poweron(struct generic_pm_domain *genpd)
+static int genpd_poweron(struct generic_pm_domain *genpd, unsigned int depth)
 {
 	struct gpd_link *link;
 	int ret = 0;
@@ -194,11 +195,16 @@ static int __genpd_poweron(struct generic_pm_domain *genpd)
 	 * with it.
 	 */
 	list_for_each_entry(link, &genpd->slave_links, slave_node) {
-		genpd_sd_counter_inc(link->master);
+		struct generic_pm_domain *master = link->master;
+
+		genpd_sd_counter_inc(master);
+
+		mutex_lock_nested(&master->lock, depth + 1);
+		ret = genpd_poweron(master, depth + 1);
+		mutex_unlock(&master->lock);
 
-		ret = genpd_poweron(link->master);
 		if (ret) {
-			genpd_sd_counter_dec(link->master);
+			genpd_sd_counter_dec(master);
 			goto err;
 		}
 	}
@@ -221,20 +227,6 @@ static int __genpd_poweron(struct generic_pm_domain *genpd)
 	return ret;
 }
 
-/**
- * genpd_poweron - Restore power to a given PM domain and its masters.
- * @genpd: PM domain to power up.
- */
-static int genpd_poweron(struct generic_pm_domain *genpd)
-{
-	int ret;
-
-	mutex_lock(&genpd->lock);
-	ret = __genpd_poweron(genpd);
-	mutex_unlock(&genpd->lock);
-	return ret;
-}
-
 static int genpd_save_dev(struct generic_pm_domain *genpd, struct device *dev)
 {
 	return GENPD_DEV_CALLBACK(genpd, int, save_state, dev);
@@ -482,7 +474,7 @@ static int pm_genpd_runtime_resume(struct device *dev)
 	}
 
 	mutex_lock(&genpd->lock);
-	ret = __genpd_poweron(genpd);
+	ret = genpd_poweron(genpd, 0);
 	mutex_unlock(&genpd->lock);
 
 	if (ret)
@@ -1188,10 +1180,11 @@ static struct generic_pm_domain_data *genpd_alloc_dev_data(struct device *dev,
 	}
 
 	dev->power.subsys_data->domain_data = &gpd_data->base;
-	dev->pm_domain = &genpd->domain;
 
 	spin_unlock_irq(&dev->power.lock);
 
+	dev_pm_domain_set(dev, &genpd->domain);
+
 	return gpd_data;
 
  err_free:
@@ -1205,9 +1198,10 @@ static struct generic_pm_domain_data *genpd_alloc_dev_data(struct device *dev,
 static void genpd_free_dev_data(struct device *dev,
 				struct generic_pm_domain_data *gpd_data)
 {
+	dev_pm_domain_set(dev, NULL);
+
 	spin_lock_irq(&dev->power.lock);
 
-	dev->pm_domain = NULL;
 	dev->power.subsys_data->domain_data = NULL;
 
 	spin_unlock_irq(&dev->power.lock);
@@ -1263,6 +1257,7 @@ 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.
@@ -1313,6 +1308,7 @@ 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.
@@ -1333,8 +1329,8 @@ int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
 	if (!link)
 		return -ENOMEM;
 
-	mutex_lock(&genpd->lock);
-	mutex_lock_nested(&subdomain->lock, SINGLE_DEPTH_NESTING);
+	mutex_lock(&subdomain->lock);
+	mutex_lock_nested(&genpd->lock, SINGLE_DEPTH_NESTING);
 
 	if (genpd->status == GPD_STATE_POWER_OFF
 	    &&  subdomain->status != GPD_STATE_POWER_OFF) {
@@ -1357,8 +1353,8 @@ int pm_genpd_add_subdomain(struct generic_pm_domain *genpd,
 		genpd_sd_counter_inc(genpd);
 
  out:
-	mutex_unlock(&subdomain->lock);
 	mutex_unlock(&genpd->lock);
+	mutex_unlock(&subdomain->lock);
 	if (ret)
 		kfree(link);
 	return ret;
@@ -1379,7 +1375,8 @@ int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
 	if (IS_ERR_OR_NULL(genpd) || IS_ERR_OR_NULL(subdomain))
 		return -EINVAL;
 
-	mutex_lock(&genpd->lock);
+	mutex_lock(&subdomain->lock);
+	mutex_lock_nested(&genpd->lock, SINGLE_DEPTH_NESTING);
 
 	if (!list_empty(&subdomain->slave_links) || subdomain->device_count) {
 		pr_warn("%s: unable to remove subdomain %s\n", genpd->name,
@@ -1392,22 +1389,19 @@ int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
 		if (link->slave != subdomain)
 			continue;
 
-		mutex_lock_nested(&subdomain->lock, SINGLE_DEPTH_NESTING);
-
 		list_del(&link->master_node);
 		list_del(&link->slave_node);
 		kfree(link);
 		if (subdomain->status != GPD_STATE_POWER_OFF)
 			genpd_sd_counter_dec(genpd);
 
-		mutex_unlock(&subdomain->lock);
-
 		ret = 0;
 		break;
 	}
 
 out:
 	mutex_unlock(&genpd->lock);
+	mutex_unlock(&subdomain->lock);
 
 	return ret;
 }
@@ -1812,8 +1806,10 @@ int genpd_dev_pm_attach(struct device *dev)
 
 	dev->pm_domain->detach = genpd_dev_pm_detach;
 	dev->pm_domain->sync = genpd_dev_pm_sync;
-	ret = genpd_poweron(pd);
 
+	mutex_lock(&pd->lock);
+	ret = genpd_poweron(pd, 0);
+	mutex_unlock(&pd->lock);
 out:
 	return ret ? -EPROBE_DEFER : 0;
 }
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index 1710c26ba097..6e7c3ccea24b 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -125,6 +125,7 @@ void device_pm_add(struct device *dev)
 {
 	pr_debug("PM: Adding info for %s:%s\n",
 		 dev->bus ? dev->bus->name : "No Bus", dev_name(dev));
+	device_pm_check_callbacks(dev);
 	mutex_lock(&dpm_list_mtx);
 	if (dev->parent && dev->parent->power.is_prepared)
 		dev_warn(dev, "parent %s should not be sleeping\n",
@@ -147,6 +148,7 @@ void device_pm_remove(struct device *dev)
 	mutex_unlock(&dpm_list_mtx);
 	device_wakeup_disable(dev);
 	pm_runtime_remove(dev);
+	device_pm_check_callbacks(dev);
 }
 
 /**
@@ -963,6 +965,9 @@ void dpm_complete(pm_message_t state)
 	}
 	list_splice(&list, &dpm_list);
 	mutex_unlock(&dpm_list_mtx);
+
+	/* Allow device probing and trigger re-probing of deferred devices */
+	device_unblock_probing();
 	trace_suspend_resume(TPS("dpm_complete"), state.event, false);
 }
 
@@ -1569,6 +1574,11 @@ static int device_prepare(struct device *dev, pm_message_t state)
 
 	dev->power.wakeup_path = device_may_wakeup(dev);
 
+	if (dev->power.no_pm_callbacks) {
+		ret = 1;	/* Let device go direct_complete */
+		goto unlock;
+	}
+
 	if (dev->pm_domain) {
 		info = "preparing power domain ";
 		callback = dev->pm_domain->ops.prepare;
@@ -1591,6 +1601,7 @@ static int device_prepare(struct device *dev, pm_message_t state)
 	if (callback)
 		ret = callback(dev);
 
+unlock:
 	device_unlock(dev);
 
 	if (ret < 0) {
@@ -1624,6 +1635,20 @@ int dpm_prepare(pm_message_t state)
 	trace_suspend_resume(TPS("dpm_prepare"), state.event, true);
 	might_sleep();
 
+	/*
+	 * Give a chance for the known devices to complete their probes, before
+	 * disable probing of devices. This sync point is important at least
+	 * at boot time + hibernation restore.
+	 */
+	wait_for_device_probe();
+	/*
+	 * It is unsafe if probing of devices will happen during suspend or
+	 * hibernation and system behavior will be unpredictable in this case.
+	 * So, let's prohibit device's probing here and defer their probes
+	 * instead. The normal behavior will be restored in dpm_complete().
+	 */
+	device_block_probing();
+
 	mutex_lock(&dpm_list_mtx);
 	while (!list_empty(&dpm_list)) {
 		struct device *dev = to_device(dpm_list.next);
@@ -1719,3 +1744,30 @@ void dpm_for_each_dev(void *data, void (*fn)(struct device *, void *))
 	device_pm_unlock();
 }
 EXPORT_SYMBOL_GPL(dpm_for_each_dev);
+
+static bool pm_ops_is_empty(const struct dev_pm_ops *ops)
+{
+	if (!ops)
+		return true;
+
+	return !ops->prepare &&
+	       !ops->suspend &&
+	       !ops->suspend_late &&
+	       !ops->suspend_noirq &&
+	       !ops->resume_noirq &&
+	       !ops->resume_early &&
+	       !ops->resume &&
+	       !ops->complete;
+}
+
+void device_pm_check_callbacks(struct device *dev)
+{
+	spin_lock_irq(&dev->power.lock);
+	dev->power.no_pm_callbacks =
+		(!dev->bus || pm_ops_is_empty(dev->bus->pm)) &&
+		(!dev->class || pm_ops_is_empty(dev->class->pm)) &&
+		(!dev->type || pm_ops_is_empty(dev->type->pm)) &&
+		(!dev->pm_domain || pm_ops_is_empty(&dev->pm_domain->ops)) &&
+		(!dev->driver || pm_ops_is_empty(dev->driver->pm));
+	spin_unlock_irq(&dev->power.lock);
+}
diff --git a/drivers/base/power/opp/Makefile b/drivers/base/power/opp/Makefile
index 33c1e18c41a4..19837ef04d8e 100644
--- a/drivers/base/power/opp/Makefile
+++ b/drivers/base/power/opp/Makefile
@@ -1,2 +1,3 @@
 ccflags-$(CONFIG_DEBUG_DRIVER)	:= -DDEBUG
 obj-y				+= core.o cpu.o
+obj-$(CONFIG_DEBUG_FS)		+= debugfs.o
diff --git a/drivers/base/power/opp/core.c b/drivers/base/power/opp/core.c
index b8e76f75073b..cf351d3dab1c 100644
--- a/drivers/base/power/opp/core.c
+++ b/drivers/base/power/opp/core.c
@@ -463,6 +463,7 @@ static void _kfree_list_dev_rcu(struct rcu_head *head)
 static void _remove_list_dev(struct device_list_opp *list_dev,
 			     struct device_opp *dev_opp)
 {
+	opp_debug_unregister(list_dev, dev_opp);
 	list_del(&list_dev->node);
 	call_srcu(&dev_opp->srcu_head.srcu, &list_dev->rcu_head,
 		  _kfree_list_dev_rcu);
@@ -472,6 +473,7 @@ struct device_list_opp *_add_list_dev(const struct device *dev,
 				      struct device_opp *dev_opp)
 {
 	struct device_list_opp *list_dev;
+	int ret;
 
 	list_dev = kzalloc(sizeof(*list_dev), GFP_KERNEL);
 	if (!list_dev)
@@ -481,6 +483,12 @@ struct device_list_opp *_add_list_dev(const struct device *dev,
 	list_dev->dev = dev;
 	list_add_rcu(&list_dev->node, &dev_opp->dev_list);
 
+	/* Create debugfs entries for the dev_opp */
+	ret = opp_debug_register(list_dev, dev_opp);
+	if (ret)
+		dev_err(dev, "%s: Failed to register opp debugfs (%d)\n",
+			__func__, ret);
+
 	return list_dev;
 }
 
@@ -551,6 +559,12 @@ static void _remove_device_opp(struct device_opp *dev_opp)
 	if (!list_empty(&dev_opp->opp_list))
 		return;
 
+	if (dev_opp->supported_hw)
+		return;
+
+	if (dev_opp->prop_name)
+		return;
+
 	list_dev = list_first_entry(&dev_opp->dev_list, struct device_list_opp,
 				    node);
 
@@ -596,6 +610,7 @@ static void _opp_remove(struct device_opp *dev_opp,
 	 */
 	if (notify)
 		srcu_notifier_call_chain(&dev_opp->srcu_head, OPP_EVENT_REMOVE, opp);
+	opp_debug_remove_one(opp);
 	list_del_rcu(&opp->node);
 	call_srcu(&dev_opp->srcu_head.srcu, &opp->rcu_head, _kfree_opp_rcu);
 
@@ -673,6 +688,7 @@ static int _opp_add(struct device *dev, struct dev_pm_opp *new_opp,
 {
 	struct dev_pm_opp *opp;
 	struct list_head *head = &dev_opp->opp_list;
+	int ret;
 
 	/*
 	 * Insert new OPP in order of increasing frequency and discard if
@@ -703,6 +719,11 @@ static int _opp_add(struct device *dev, struct dev_pm_opp *new_opp,
 	new_opp->dev_opp = dev_opp;
 	list_add_rcu(&new_opp->node, head);
 
+	ret = opp_debug_create_one(new_opp, dev_opp);
+	if (ret)
+		dev_err(dev, "%s: Failed to register opp to debugfs (%d)\n",
+			__func__, ret);
+
 	return 0;
 }
 
@@ -776,35 +797,49 @@ unlock:
 }
 
 /* TODO: Support multiple regulators */
-static int opp_parse_supplies(struct dev_pm_opp *opp, struct device *dev)
+static int opp_parse_supplies(struct dev_pm_opp *opp, struct device *dev,
+			      struct device_opp *dev_opp)
 {
 	u32 microvolt[3] = {0};
 	u32 val;
 	int count, ret;
+	struct property *prop = NULL;
+	char name[NAME_MAX];
+
+	/* Search for "opp-microvolt-<name>" */
+	if (dev_opp->prop_name) {
+		snprintf(name, sizeof(name), "opp-microvolt-%s",
+			 dev_opp->prop_name);
+		prop = of_find_property(opp->np, name, NULL);
+	}
 
-	/* Missing property isn't a problem, but an invalid entry is */
-	if (!of_find_property(opp->np, "opp-microvolt", NULL))
-		return 0;
+	if (!prop) {
+		/* Search for "opp-microvolt" */
+		sprintf(name, "opp-microvolt");
+		prop = of_find_property(opp->np, name, NULL);
 
-	count = of_property_count_u32_elems(opp->np, "opp-microvolt");
+		/* Missing property isn't a problem, but an invalid entry is */
+		if (!prop)
+			return 0;
+	}
+
+	count = of_property_count_u32_elems(opp->np, name);
 	if (count < 0) {
-		dev_err(dev, "%s: Invalid opp-microvolt property (%d)\n",
-			__func__, count);
+		dev_err(dev, "%s: Invalid %s property (%d)\n",
+			__func__, name, count);
 		return count;
 	}
 
 	/* There can be one or three elements here */
 	if (count != 1 && count != 3) {
-		dev_err(dev, "%s: Invalid number of elements in opp-microvolt property (%d)\n",
-			__func__, count);
+		dev_err(dev, "%s: Invalid number of elements in %s property (%d)\n",
+			__func__, name, count);
 		return -EINVAL;
 	}
 
-	ret = of_property_read_u32_array(opp->np, "opp-microvolt", microvolt,
-					 count);
+	ret = of_property_read_u32_array(opp->np, name, microvolt, count);
 	if (ret) {
-		dev_err(dev, "%s: error parsing opp-microvolt: %d\n", __func__,
-			ret);
+		dev_err(dev, "%s: error parsing %s: %d\n", __func__, name, ret);
 		return -EINVAL;
 	}
 
@@ -812,13 +847,272 @@ static int opp_parse_supplies(struct dev_pm_opp *opp, struct device *dev)
 	opp->u_volt_min = microvolt[1];
 	opp->u_volt_max = microvolt[2];
 
-	if (!of_property_read_u32(opp->np, "opp-microamp", &val))
+	/* Search for "opp-microamp-<name>" */
+	prop = NULL;
+	if (dev_opp->prop_name) {
+		snprintf(name, sizeof(name), "opp-microamp-%s",
+			 dev_opp->prop_name);
+		prop = of_find_property(opp->np, name, NULL);
+	}
+
+	if (!prop) {
+		/* Search for "opp-microamp" */
+		sprintf(name, "opp-microamp");
+		prop = of_find_property(opp->np, name, NULL);
+	}
+
+	if (prop && !of_property_read_u32(opp->np, name, &val))
 		opp->u_amp = val;
 
 	return 0;
 }
 
 /**
+ * dev_pm_opp_set_supported_hw() - Set supported platforms
+ * @dev: Device for which supported-hw has to be set.
+ * @versions: Array of hierarchy of versions to match.
+ * @count: Number of elements in the array.
+ *
+ * This is required only for the V2 bindings, and it enables a platform to
+ * specify the hierarchy of versions it supports. OPP layer will then enable
+ * OPPs, which are available for those versions, based on its 'opp-supported-hw'
+ * property.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Hence this function internally uses RCU updater strategy with mutex locks
+ * to keep the integrity of the internal data structures. Callers should ensure
+ * that this function is *NOT* called under RCU protection or in contexts where
+ * mutex cannot be locked.
+ */
+int dev_pm_opp_set_supported_hw(struct device *dev, const u32 *versions,
+				unsigned int count)
+{
+	struct device_opp *dev_opp;
+	int ret = 0;
+
+	/* Hold our list modification lock here */
+	mutex_lock(&dev_opp_list_lock);
+
+	dev_opp = _add_device_opp(dev);
+	if (!dev_opp) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	/* Make sure there are no concurrent readers while updating dev_opp */
+	WARN_ON(!list_empty(&dev_opp->opp_list));
+
+	/* Do we already have a version hierarchy associated with dev_opp? */
+	if (dev_opp->supported_hw) {
+		dev_err(dev, "%s: Already have supported hardware list\n",
+			__func__);
+		ret = -EBUSY;
+		goto err;
+	}
+
+	dev_opp->supported_hw = kmemdup(versions, count * sizeof(*versions),
+					GFP_KERNEL);
+	if (!dev_opp->supported_hw) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	dev_opp->supported_hw_count = count;
+	mutex_unlock(&dev_opp_list_lock);
+	return 0;
+
+err:
+	_remove_device_opp(dev_opp);
+unlock:
+	mutex_unlock(&dev_opp_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_set_supported_hw);
+
+/**
+ * dev_pm_opp_put_supported_hw() - Releases resources blocked for supported hw
+ * @dev: Device for which supported-hw has to be set.
+ *
+ * This is required only for the V2 bindings, and is called for a matching
+ * dev_pm_opp_set_supported_hw(). Until this is called, the device_opp structure
+ * will not be freed.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Hence this function internally uses RCU updater strategy with mutex locks
+ * to keep the integrity of the internal data structures. Callers should ensure
+ * that this function is *NOT* called under RCU protection or in contexts where
+ * mutex cannot be locked.
+ */
+void dev_pm_opp_put_supported_hw(struct device *dev)
+{
+	struct device_opp *dev_opp;
+
+	/* Hold our list modification lock here */
+	mutex_lock(&dev_opp_list_lock);
+
+	/* Check for existing list for 'dev' first */
+	dev_opp = _find_device_opp(dev);
+	if (IS_ERR(dev_opp)) {
+		dev_err(dev, "Failed to find dev_opp: %ld\n", PTR_ERR(dev_opp));
+		goto unlock;
+	}
+
+	/* Make sure there are no concurrent readers while updating dev_opp */
+	WARN_ON(!list_empty(&dev_opp->opp_list));
+
+	if (!dev_opp->supported_hw) {
+		dev_err(dev, "%s: Doesn't have supported hardware list\n",
+			__func__);
+		goto unlock;
+	}
+
+	kfree(dev_opp->supported_hw);
+	dev_opp->supported_hw = NULL;
+	dev_opp->supported_hw_count = 0;
+
+	/* Try freeing device_opp if this was the last blocking resource */
+	_remove_device_opp(dev_opp);
+
+unlock:
+	mutex_unlock(&dev_opp_list_lock);
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_put_supported_hw);
+
+/**
+ * dev_pm_opp_set_prop_name() - Set prop-extn name
+ * @dev: Device for which the regulator has to be set.
+ * @name: name to postfix to properties.
+ *
+ * This is required only for the V2 bindings, and it enables a platform to
+ * specify the extn to be used for certain property names. The properties to
+ * which the extension will apply are opp-microvolt and opp-microamp. OPP core
+ * should postfix the property name with -<name> while looking for them.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Hence this function internally uses RCU updater strategy with mutex locks
+ * to keep the integrity of the internal data structures. Callers should ensure
+ * that this function is *NOT* called under RCU protection or in contexts where
+ * mutex cannot be locked.
+ */
+int dev_pm_opp_set_prop_name(struct device *dev, const char *name)
+{
+	struct device_opp *dev_opp;
+	int ret = 0;
+
+	/* Hold our list modification lock here */
+	mutex_lock(&dev_opp_list_lock);
+
+	dev_opp = _add_device_opp(dev);
+	if (!dev_opp) {
+		ret = -ENOMEM;
+		goto unlock;
+	}
+
+	/* Make sure there are no concurrent readers while updating dev_opp */
+	WARN_ON(!list_empty(&dev_opp->opp_list));
+
+	/* Do we already have a prop-name associated with dev_opp? */
+	if (dev_opp->prop_name) {
+		dev_err(dev, "%s: Already have prop-name %s\n", __func__,
+			dev_opp->prop_name);
+		ret = -EBUSY;
+		goto err;
+	}
+
+	dev_opp->prop_name = kstrdup(name, GFP_KERNEL);
+	if (!dev_opp->prop_name) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	mutex_unlock(&dev_opp_list_lock);
+	return 0;
+
+err:
+	_remove_device_opp(dev_opp);
+unlock:
+	mutex_unlock(&dev_opp_list_lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_set_prop_name);
+
+/**
+ * dev_pm_opp_put_prop_name() - Releases resources blocked for prop-name
+ * @dev: Device for which the regulator has to be set.
+ *
+ * This is required only for the V2 bindings, and is called for a matching
+ * dev_pm_opp_set_prop_name(). Until this is called, the device_opp structure
+ * will not be freed.
+ *
+ * Locking: The internal device_opp and opp structures are RCU protected.
+ * Hence this function internally uses RCU updater strategy with mutex locks
+ * to keep the integrity of the internal data structures. Callers should ensure
+ * that this function is *NOT* called under RCU protection or in contexts where
+ * mutex cannot be locked.
+ */
+void dev_pm_opp_put_prop_name(struct device *dev)
+{
+	struct device_opp *dev_opp;
+
+	/* Hold our list modification lock here */
+	mutex_lock(&dev_opp_list_lock);
+
+	/* Check for existing list for 'dev' first */
+	dev_opp = _find_device_opp(dev);
+	if (IS_ERR(dev_opp)) {
+		dev_err(dev, "Failed to find dev_opp: %ld\n", PTR_ERR(dev_opp));
+		goto unlock;
+	}
+
+	/* Make sure there are no concurrent readers while updating dev_opp */
+	WARN_ON(!list_empty(&dev_opp->opp_list));
+
+	if (!dev_opp->prop_name) {
+		dev_err(dev, "%s: Doesn't have a prop-name\n", __func__);
+		goto unlock;
+	}
+
+	kfree(dev_opp->prop_name);
+	dev_opp->prop_name = NULL;
+
+	/* Try freeing device_opp if this was the last blocking resource */
+	_remove_device_opp(dev_opp);
+
+unlock:
+	mutex_unlock(&dev_opp_list_lock);
+}
+EXPORT_SYMBOL_GPL(dev_pm_opp_put_prop_name);
+
+static bool _opp_is_supported(struct device *dev, struct device_opp *dev_opp,
+			      struct device_node *np)
+{
+	unsigned int count = dev_opp->supported_hw_count;
+	u32 version;
+	int ret;
+
+	if (!dev_opp->supported_hw)
+		return true;
+
+	while (count--) {
+		ret = of_property_read_u32_index(np, "opp-supported-hw", count,
+						 &version);
+		if (ret) {
+			dev_warn(dev, "%s: failed to read opp-supported-hw property at index %d: %d\n",
+				 __func__, count, ret);
+			return false;
+		}
+
+		/* Both of these are bitwise masks of the versions */
+		if (!(version & dev_opp->supported_hw[count]))
+			return false;
+	}
+
+	return true;
+}
+
+/**
  * _opp_add_static_v2() - Allocate static OPPs (As per 'v2' DT bindings)
  * @dev:	device for which we do this operation
  * @np:		device node
@@ -864,6 +1158,12 @@ static int _opp_add_static_v2(struct device *dev, struct device_node *np)
 		goto free_opp;
 	}
 
+	/* Check if the OPP supports hardware's hierarchy of versions or not */
+	if (!_opp_is_supported(dev, dev_opp, np)) {
+		dev_dbg(dev, "OPP not supported by hardware: %llu\n", rate);
+		goto free_opp;
+	}
+
 	/*
 	 * Rate is defined as an unsigned long in clk API, and so casting
 	 * explicitly to its type. Must be fixed once rate is 64 bit
@@ -879,7 +1179,7 @@ static int _opp_add_static_v2(struct device *dev, struct device_node *np)
 	if (!of_property_read_u32(np, "clock-latency-ns", &val))
 		new_opp->clock_latency_ns = val;
 
-	ret = opp_parse_supplies(new_opp, dev);
+	ret = opp_parse_supplies(new_opp, dev, dev_opp);
 	if (ret)
 		goto free_opp;
 
@@ -889,12 +1189,14 @@ static int _opp_add_static_v2(struct device *dev, struct device_node *np)
 
 	/* OPP to select on device suspend */
 	if (of_property_read_bool(np, "opp-suspend")) {
-		if (dev_opp->suspend_opp)
+		if (dev_opp->suspend_opp) {
 			dev_warn(dev, "%s: Multiple suspend OPPs found (%lu %lu)\n",
 				 __func__, dev_opp->suspend_opp->rate,
 				 new_opp->rate);
-		else
+		} else {
+			new_opp->suspend = true;
 			dev_opp->suspend_opp = new_opp;
+		}
 	}
 
 	if (new_opp->clock_latency_ns > dev_opp->clock_latency_ns_max)
diff --git a/drivers/base/power/opp/cpu.c b/drivers/base/power/opp/cpu.c
index 7b445e88a0d5..9f0c15570f64 100644
--- a/drivers/base/power/opp/cpu.c
+++ b/drivers/base/power/opp/cpu.c
@@ -214,7 +214,6 @@ EXPORT_SYMBOL_GPL(dev_pm_opp_of_cpumask_add_table);
 /*
  * Works only for OPP v2 bindings.
  *
- * cpumask should be already set to mask of cpu_dev->id.
  * Returns -ENOENT if operating-points-v2 bindings aren't supported.
  */
 int dev_pm_opp_of_get_sharing_cpus(struct device *cpu_dev, cpumask_var_t cpumask)
@@ -230,6 +229,8 @@ int dev_pm_opp_of_get_sharing_cpus(struct device *cpu_dev, cpumask_var_t cpumask
 		return -ENOENT;
 	}
 
+	cpumask_set_cpu(cpu_dev->id, cpumask);
+
 	/* OPPs are shared ? */
 	if (!of_property_read_bool(np, "opp-shared"))
 		goto put_cpu_node;
diff --git a/drivers/base/power/opp/debugfs.c b/drivers/base/power/opp/debugfs.c
new file mode 100644
index 000000000000..ddfe4773e922
--- /dev/null
+++ b/drivers/base/power/opp/debugfs.c
@@ -0,0 +1,219 @@
+/*
+ * Generic OPP debugfs interface
+ *
+ * Copyright (C) 2015-2016 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/debugfs.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/limits.h>
+
+#include "opp.h"
+
+static struct dentry *rootdir;
+
+static void opp_set_dev_name(const struct device *dev, char *name)
+{
+	if (dev->parent)
+		snprintf(name, NAME_MAX, "%s-%s", dev_name(dev->parent),
+			 dev_name(dev));
+	else
+		snprintf(name, NAME_MAX, "%s", dev_name(dev));
+}
+
+void opp_debug_remove_one(struct dev_pm_opp *opp)
+{
+	debugfs_remove_recursive(opp->dentry);
+}
+
+int opp_debug_create_one(struct dev_pm_opp *opp, struct device_opp *dev_opp)
+{
+	struct dentry *pdentry = dev_opp->dentry;
+	struct dentry *d;
+	char name[25];	/* 20 chars for 64 bit value + 5 (opp:\0) */
+
+	/* Rate is unique to each OPP, use it to give opp-name */
+	snprintf(name, sizeof(name), "opp:%lu", opp->rate);
+
+	/* Create per-opp directory */
+	d = debugfs_create_dir(name, pdentry);
+	if (!d)
+		return -ENOMEM;
+
+	if (!debugfs_create_bool("available", S_IRUGO, d, &opp->available))
+		return -ENOMEM;
+
+	if (!debugfs_create_bool("dynamic", S_IRUGO, d, &opp->dynamic))
+		return -ENOMEM;
+
+	if (!debugfs_create_bool("turbo", S_IRUGO, d, &opp->turbo))
+		return -ENOMEM;
+
+	if (!debugfs_create_bool("suspend", S_IRUGO, d, &opp->suspend))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("rate_hz", S_IRUGO, d, &opp->rate))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("u_volt_target", S_IRUGO, d, &opp->u_volt))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("u_volt_min", S_IRUGO, d, &opp->u_volt_min))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("u_volt_max", S_IRUGO, d, &opp->u_volt_max))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("u_amp", S_IRUGO, d, &opp->u_amp))
+		return -ENOMEM;
+
+	if (!debugfs_create_ulong("clock_latency_ns", S_IRUGO, d,
+				  &opp->clock_latency_ns))
+		return -ENOMEM;
+
+	opp->dentry = d;
+	return 0;
+}
+
+static int device_opp_debug_create_dir(struct device_list_opp *list_dev,
+				       struct device_opp *dev_opp)
+{
+	const struct device *dev = list_dev->dev;
+	struct dentry *d;
+
+	opp_set_dev_name(dev, dev_opp->dentry_name);
+
+	/* Create device specific directory */
+	d = debugfs_create_dir(dev_opp->dentry_name, rootdir);
+	if (!d) {
+		dev_err(dev, "%s: Failed to create debugfs dir\n", __func__);
+		return -ENOMEM;
+	}
+
+	list_dev->dentry = d;
+	dev_opp->dentry = d;
+
+	return 0;
+}
+
+static int device_opp_debug_create_link(struct device_list_opp *list_dev,
+					struct device_opp *dev_opp)
+{
+	const struct device *dev = list_dev->dev;
+	char name[NAME_MAX];
+	struct dentry *d;
+
+	opp_set_dev_name(list_dev->dev, name);
+
+	/* Create device specific directory link */
+	d = debugfs_create_symlink(name, rootdir, dev_opp->dentry_name);
+	if (!d) {
+		dev_err(dev, "%s: Failed to create link\n", __func__);
+		return -ENOMEM;
+	}
+
+	list_dev->dentry = d;
+
+	return 0;
+}
+
+/**
+ * opp_debug_register - add a device opp node to the debugfs 'opp' directory
+ * @list_dev: list-dev pointer for device
+ * @dev_opp: the device-opp being added
+ *
+ * Dynamically adds device specific directory in debugfs 'opp' directory. If the
+ * device-opp is shared with other devices, then links will be created for all
+ * devices except the first.
+ *
+ * Return: 0 on success, otherwise negative error.
+ */
+int opp_debug_register(struct device_list_opp *list_dev,
+		       struct device_opp *dev_opp)
+{
+	if (!rootdir) {
+		pr_debug("%s: Uninitialized rootdir\n", __func__);
+		return -EINVAL;
+	}
+
+	if (dev_opp->dentry)
+		return device_opp_debug_create_link(list_dev, dev_opp);
+
+	return device_opp_debug_create_dir(list_dev, dev_opp);
+}
+
+static void opp_migrate_dentry(struct device_list_opp *list_dev,
+			       struct device_opp *dev_opp)
+{
+	struct device_list_opp *new_dev;
+	const struct device *dev;
+	struct dentry *dentry;
+
+	/* Look for next list-dev */
+	list_for_each_entry(new_dev, &dev_opp->dev_list, node)
+		if (new_dev != list_dev)
+			break;
+
+	/* new_dev is guaranteed to be valid here */
+	dev = new_dev->dev;
+	debugfs_remove_recursive(new_dev->dentry);
+
+	opp_set_dev_name(dev, dev_opp->dentry_name);
+
+	dentry = debugfs_rename(rootdir, list_dev->dentry, rootdir,
+				dev_opp->dentry_name);
+	if (!dentry) {
+		dev_err(dev, "%s: Failed to rename link from: %s to %s\n",
+			__func__, dev_name(list_dev->dev), dev_name(dev));
+		return;
+	}
+
+	new_dev->dentry = dentry;
+	dev_opp->dentry = dentry;
+}
+
+/**
+ * opp_debug_unregister - remove a device opp node from debugfs opp directory
+ * @list_dev: list-dev pointer for device
+ * @dev_opp: the device-opp being removed
+ *
+ * Dynamically removes device specific directory from debugfs 'opp' directory.
+ */
+void opp_debug_unregister(struct device_list_opp *list_dev,
+			  struct device_opp *dev_opp)
+{
+	if (list_dev->dentry == dev_opp->dentry) {
+		/* Move the real dentry object under another device */
+		if (!list_is_singular(&dev_opp->dev_list)) {
+			opp_migrate_dentry(list_dev, dev_opp);
+			goto out;
+		}
+		dev_opp->dentry = NULL;
+	}
+
+	debugfs_remove_recursive(list_dev->dentry);
+
+out:
+	list_dev->dentry = NULL;
+}
+
+static int __init opp_debug_init(void)
+{
+	/* Create /sys/kernel/debug/opp directory */
+	rootdir = debugfs_create_dir("opp", NULL);
+	if (!rootdir) {
+		pr_err("%s: Failed to create root directory\n", __func__);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+core_initcall(opp_debug_init);
diff --git a/drivers/base/power/opp/opp.h b/drivers/base/power/opp/opp.h
index 7366b2aa8997..690638ef36ee 100644
--- a/drivers/base/power/opp/opp.h
+++ b/drivers/base/power/opp/opp.h
@@ -17,6 +17,7 @@
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
+#include <linux/limits.h>
 #include <linux/pm_opp.h>
 #include <linux/rculist.h>
 #include <linux/rcupdate.h>
@@ -50,9 +51,10 @@ extern struct mutex dev_opp_list_lock;
  *		are protected by the dev_opp_list_lock for integrity.
  *		IMPORTANT: the opp nodes should be maintained in increasing
  *		order.
- * @dynamic:	not-created from static DT entries.
  * @available:	true/false - marks if this OPP as available or not
+ * @dynamic:	not-created from static DT entries.
  * @turbo:	true if turbo (boost) OPP
+ * @suspend:	true if suspend OPP
  * @rate:	Frequency in hertz
  * @u_volt:	Target voltage in microvolts corresponding to this OPP
  * @u_volt_min:	Minimum voltage in microvolts corresponding to this OPP
@@ -63,6 +65,7 @@ extern struct mutex dev_opp_list_lock;
  * @dev_opp:	points back to the device_opp struct this opp belongs to
  * @rcu_head:	RCU callback head used for deferred freeing
  * @np:		OPP's device node.
+ * @dentry:	debugfs dentry pointer (per opp)
  *
  * This structure stores the OPP information for a given device.
  */
@@ -72,6 +75,7 @@ struct dev_pm_opp {
 	bool available;
 	bool dynamic;
 	bool turbo;
+	bool suspend;
 	unsigned long rate;
 
 	unsigned long u_volt;
@@ -84,6 +88,10 @@ struct dev_pm_opp {
 	struct rcu_head rcu_head;
 
 	struct device_node *np;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dentry;
+#endif
 };
 
 /**
@@ -91,6 +99,7 @@ struct dev_pm_opp {
  * @node:	list node
  * @dev:	device to which the struct object belongs
  * @rcu_head:	RCU callback head used for deferred freeing
+ * @dentry:	debugfs dentry pointer (per device)
  *
  * This is an internal data structure maintaining the list of devices that are
  * managed by 'struct device_opp'.
@@ -99,6 +108,10 @@ struct device_list_opp {
 	struct list_head node;
 	const struct device *dev;
 	struct rcu_head rcu_head;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dentry;
+#endif
 };
 
 /**
@@ -113,7 +126,14 @@ struct device_list_opp {
  * @dev_list:	list of devices that share these OPPs
  * @opp_list:	list of opps
  * @np:		struct device_node pointer for opp's DT node.
+ * @clock_latency_ns_max: Max clock latency in nanoseconds.
  * @shared_opp: OPP is shared between multiple devices.
+ * @suspend_opp: Pointer to OPP to be used during device suspend.
+ * @supported_hw: Array of version number to support.
+ * @supported_hw_count: Number of elements in supported_hw array.
+ * @prop_name: A name to postfix to many DT properties, while parsing them.
+ * @dentry:	debugfs dentry pointer of the real device directory (not links).
+ * @dentry_name: Name of the real dentry.
  *
  * This is an internal data structure maintaining the link to opps attached to
  * a device. This structure is not meant to be shared to users as it is
@@ -135,6 +155,15 @@ struct device_opp {
 	unsigned long clock_latency_ns_max;
 	bool shared_opp;
 	struct dev_pm_opp *suspend_opp;
+
+	unsigned int *supported_hw;
+	unsigned int supported_hw_count;
+	const char *prop_name;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dentry;
+	char dentry_name[NAME_MAX];
+#endif
 };
 
 /* Routines internal to opp core */
@@ -143,4 +172,26 @@ struct device_list_opp *_add_list_dev(const struct device *dev,
 				      struct device_opp *dev_opp);
 struct device_node *_of_get_opp_desc_node(struct device *dev);
 
+#ifdef CONFIG_DEBUG_FS
+void opp_debug_remove_one(struct dev_pm_opp *opp);
+int opp_debug_create_one(struct dev_pm_opp *opp, struct device_opp *dev_opp);
+int opp_debug_register(struct device_list_opp *list_dev,
+		       struct device_opp *dev_opp);
+void opp_debug_unregister(struct device_list_opp *list_dev,
+			  struct device_opp *dev_opp);
+#else
+static inline void opp_debug_remove_one(struct dev_pm_opp *opp) {}
+
+static inline int opp_debug_create_one(struct dev_pm_opp *opp,
+				       struct device_opp *dev_opp)
+{ return 0; }
+static inline int opp_debug_register(struct device_list_opp *list_dev,
+				     struct device_opp *dev_opp)
+{ return 0; }
+
+static inline void opp_debug_unregister(struct device_list_opp *list_dev,
+					struct device_opp *dev_opp)
+{ }
+#endif		/* DEBUG_FS */
+
 #endif		/* __DRIVER_OPP_H__ */
diff --git a/drivers/base/power/power.h b/drivers/base/power/power.h
index 998fa6b23084..50e30e7b059d 100644
--- a/drivers/base/power/power.h
+++ b/drivers/base/power/power.h
@@ -18,6 +18,7 @@ static inline void pm_runtime_early_init(struct device *dev)
 }
 
 extern void pm_runtime_init(struct device *dev);
+extern void pm_runtime_reinit(struct device *dev);
 extern void pm_runtime_remove(struct device *dev);
 
 struct wake_irq {
@@ -84,6 +85,7 @@ static inline void pm_runtime_early_init(struct device *dev)
 }
 
 static inline void pm_runtime_init(struct device *dev) {}
+static inline void pm_runtime_reinit(struct device *dev) {}
 static inline void pm_runtime_remove(struct device *dev) {}
 
 static inline int dpm_sysfs_add(struct device *dev) { return 0; }
@@ -123,6 +125,7 @@ extern void device_pm_remove(struct device *);
 extern void device_pm_move_before(struct device *, struct device *);
 extern void device_pm_move_after(struct device *, struct device *);
 extern void device_pm_move_last(struct device *);
+extern void device_pm_check_callbacks(struct device *dev);
 
 #else /* !CONFIG_PM_SLEEP */
 
@@ -141,6 +144,8 @@ static inline void device_pm_move_after(struct device *deva,
 					struct device *devb) {}
 static inline void device_pm_move_last(struct device *dev) {}
 
+static inline void device_pm_check_callbacks(struct device *dev) {}
+
 #endif /* !CONFIG_PM_SLEEP */
 
 static inline void device_pm_init(struct device *dev)
diff --git a/drivers/base/power/runtime.c b/drivers/base/power/runtime.c
index e1a10a03df8e..4c7055009bd6 100644
--- a/drivers/base/power/runtime.c
+++ b/drivers/base/power/runtime.c
@@ -966,6 +966,30 @@ int __pm_runtime_resume(struct device *dev, int rpmflags)
 EXPORT_SYMBOL_GPL(__pm_runtime_resume);
 
 /**
+ * pm_runtime_get_if_in_use - Conditionally bump up the device's usage counter.
+ * @dev: Device to handle.
+ *
+ * Return -EINVAL if runtime PM is disabled for the device.
+ *
+ * If that's not the case and if the device's runtime PM status is RPM_ACTIVE
+ * and the runtime PM usage counter is nonzero, increment the counter and
+ * return 1.  Otherwise return 0 without changing the counter.
+ */
+int pm_runtime_get_if_in_use(struct device *dev)
+{
+	unsigned long flags;
+	int retval;
+
+	spin_lock_irqsave(&dev->power.lock, flags);
+	retval = dev->power.disable_depth > 0 ? -EINVAL :
+		dev->power.runtime_status == RPM_ACTIVE
+			&& atomic_inc_not_zero(&dev->power.usage_count);
+	spin_unlock_irqrestore(&dev->power.lock, flags);
+	return retval;
+}
+EXPORT_SYMBOL_GPL(pm_runtime_get_if_in_use);
+
+/**
  * __pm_runtime_set_status - Set runtime PM status of a device.
  * @dev: Device to handle.
  * @status: New runtime PM status of the device.
@@ -1390,18 +1414,32 @@ void pm_runtime_init(struct device *dev)
 }
 
 /**
+ * pm_runtime_reinit - Re-initialize runtime PM fields in given device object.
+ * @dev: Device object to re-initialize.
+ */
+void pm_runtime_reinit(struct device *dev)
+{
+	if (!pm_runtime_enabled(dev)) {
+		if (dev->power.runtime_status == RPM_ACTIVE)
+			pm_runtime_set_suspended(dev);
+		if (dev->power.irq_safe) {
+			spin_lock_irq(&dev->power.lock);
+			dev->power.irq_safe = 0;
+			spin_unlock_irq(&dev->power.lock);
+			if (dev->parent)
+				pm_runtime_put(dev->parent);
+		}
+	}
+}
+
+/**
  * pm_runtime_remove - Prepare for removing a device from device hierarchy.
  * @dev: Device object being removed from device hierarchy.
  */
 void pm_runtime_remove(struct device *dev)
 {
 	__pm_runtime_disable(dev, false);
-
-	/* Change the status back to 'suspended' to match the initial status. */
-	if (dev->power.runtime_status == RPM_ACTIVE)
-		pm_runtime_set_suspended(dev);
-	if (dev->power.irq_safe && dev->parent)
-		pm_runtime_put(dev->parent);
+	pm_runtime_reinit(dev);
 }
 
 /**
diff --git a/drivers/base/property.c b/drivers/base/property.c
index 1325ff225cc4..a163f2c59aa3 100644
--- a/drivers/base/property.c
+++ b/drivers/base/property.c
@@ -19,32 +19,14 @@
 #include <linux/etherdevice.h>
 #include <linux/phy.h>
 
-/**
- * device_add_property_set - Add a collection of properties to a device object.
- * @dev: Device to add properties to.
- * @pset: Collection of properties to add.
- *
- * Associate a collection of device properties represented by @pset with @dev
- * as its secondary firmware node.
- */
-void device_add_property_set(struct device *dev, struct property_set *pset)
-{
-	if (!pset)
-		return;
-
-	pset->fwnode.type = FWNODE_PDATA;
-	set_secondary_fwnode(dev, &pset->fwnode);
-}
-EXPORT_SYMBOL_GPL(device_add_property_set);
-
-static inline bool is_pset(struct fwnode_handle *fwnode)
+static inline bool is_pset_node(struct fwnode_handle *fwnode)
 {
 	return fwnode && fwnode->type == FWNODE_PDATA;
 }
 
-static inline struct property_set *to_pset(struct fwnode_handle *fwnode)
+static inline struct property_set *to_pset_node(struct fwnode_handle *fwnode)
 {
-	return is_pset(fwnode) ?
+	return is_pset_node(fwnode) ?
 		container_of(fwnode, struct property_set, fwnode) : NULL;
 }
 
@@ -63,45 +45,135 @@ static struct property_entry *pset_prop_get(struct property_set *pset,
 	return NULL;
 }
 
-static int pset_prop_read_array(struct property_set *pset, const char *name,
-				enum dev_prop_type type, void *val, size_t nval)
+static void *pset_prop_find(struct property_set *pset, const char *propname,
+			    size_t length)
 {
 	struct property_entry *prop;
-	unsigned int item_size;
+	void *pointer;
 
-	prop = pset_prop_get(pset, name);
+	prop = pset_prop_get(pset, propname);
 	if (!prop)
-		return -ENODATA;
+		return ERR_PTR(-EINVAL);
+	if (prop->is_array)
+		pointer = prop->pointer.raw_data;
+	else
+		pointer = &prop->value.raw_data;
+	if (!pointer)
+		return ERR_PTR(-ENODATA);
+	if (length > prop->length)
+		return ERR_PTR(-EOVERFLOW);
+	return pointer;
+}
+
+static int pset_prop_read_u8_array(struct property_set *pset,
+				   const char *propname,
+				   u8 *values, size_t nval)
+{
+	void *pointer;
+	size_t length = nval * sizeof(*values);
+
+	pointer = pset_prop_find(pset, propname, length);
+	if (IS_ERR(pointer))
+		return PTR_ERR(pointer);
+
+	memcpy(values, pointer, length);
+	return 0;
+}
+
+static int pset_prop_read_u16_array(struct property_set *pset,
+				    const char *propname,
+				    u16 *values, size_t nval)
+{
+	void *pointer;
+	size_t length = nval * sizeof(*values);
+
+	pointer = pset_prop_find(pset, propname, length);
+	if (IS_ERR(pointer))
+		return PTR_ERR(pointer);
+
+	memcpy(values, pointer, length);
+	return 0;
+}
+
+static int pset_prop_read_u32_array(struct property_set *pset,
+				    const char *propname,
+				    u32 *values, size_t nval)
+{
+	void *pointer;
+	size_t length = nval * sizeof(*values);
+
+	pointer = pset_prop_find(pset, propname, length);
+	if (IS_ERR(pointer))
+		return PTR_ERR(pointer);
+
+	memcpy(values, pointer, length);
+	return 0;
+}
+
+static int pset_prop_read_u64_array(struct property_set *pset,
+				    const char *propname,
+				    u64 *values, size_t nval)
+{
+	void *pointer;
+	size_t length = nval * sizeof(*values);
+
+	pointer = pset_prop_find(pset, propname, length);
+	if (IS_ERR(pointer))
+		return PTR_ERR(pointer);
+
+	memcpy(values, pointer, length);
+	return 0;
+}
+
+static int pset_prop_count_elems_of_size(struct property_set *pset,
+					 const char *propname, size_t length)
+{
+	struct property_entry *prop;
+
+	prop = pset_prop_get(pset, propname);
+	if (!prop)
+		return -EINVAL;
+
+	return prop->length / length;
+}
+
+static int pset_prop_read_string_array(struct property_set *pset,
+				       const char *propname,
+				       const char **strings, size_t nval)
+{
+	void *pointer;
+	size_t length = nval * sizeof(*strings);
+
+	pointer = pset_prop_find(pset, propname, length);
+	if (IS_ERR(pointer))
+		return PTR_ERR(pointer);
+
+	memcpy(strings, pointer, length);
+	return 0;
+}
+
+static int pset_prop_read_string(struct property_set *pset,
+				 const char *propname, const char **strings)
+{
+	struct property_entry *prop;
+	const char **pointer;
 
-	if (prop->type != type)
-		return -EPROTO;
-
-	if (!val)
-		return prop->nval;
-
-	if (prop->nval < nval)
-		return -EOVERFLOW;
-
-	switch (type) {
-	case DEV_PROP_U8:
-		item_size = sizeof(u8);
-		break;
-	case DEV_PROP_U16:
-		item_size = sizeof(u16);
-		break;
-	case DEV_PROP_U32:
-		item_size = sizeof(u32);
-		break;
-	case DEV_PROP_U64:
-		item_size = sizeof(u64);
-		break;
-	case DEV_PROP_STRING:
-		item_size = sizeof(const char *);
-		break;
-	default:
+	prop = pset_prop_get(pset, propname);
+	if (!prop)
 		return -EINVAL;
+	if (!prop->is_string)
+		return -EILSEQ;
+	if (prop->is_array) {
+		pointer = prop->pointer.str;
+		if (!pointer)
+			return -ENODATA;
+	} else {
+		pointer = &prop->value.str;
+		if (*pointer && strnlen(*pointer, prop->length) >= prop->length)
+			return -EILSEQ;
 	}
-	memcpy(val, prop->value.raw_data, nval * item_size);
+
+	*strings = *pointer;
 	return 0;
 }
 
@@ -124,6 +196,18 @@ bool device_property_present(struct device *dev, const char *propname)
 }
 EXPORT_SYMBOL_GPL(device_property_present);
 
+static bool __fwnode_property_present(struct fwnode_handle *fwnode,
+				      const char *propname)
+{
+	if (is_of_node(fwnode))
+		return of_property_read_bool(to_of_node(fwnode), propname);
+	else if (is_acpi_node(fwnode))
+		return !acpi_node_prop_get(fwnode, propname, NULL);
+	else if (is_pset_node(fwnode))
+		return !!pset_prop_get(to_pset_node(fwnode), propname);
+	return false;
+}
+
 /**
  * fwnode_property_present - check if a property of a firmware node is present
  * @fwnode: Firmware node whose property to check
@@ -131,12 +215,12 @@ EXPORT_SYMBOL_GPL(device_property_present);
  */
 bool fwnode_property_present(struct fwnode_handle *fwnode, const char *propname)
 {
-	if (is_of_node(fwnode))
-		return of_property_read_bool(to_of_node(fwnode), propname);
-	else if (is_acpi_node(fwnode))
-		return !acpi_node_prop_get(fwnode, propname, NULL);
+	bool ret;
 
-	return !!pset_prop_get(to_pset(fwnode), propname);
+	ret = __fwnode_property_present(fwnode, propname);
+	if (ret == false && fwnode && !IS_ERR_OR_NULL(fwnode->secondary))
+		ret = __fwnode_property_present(fwnode->secondary, propname);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(fwnode_property_present);
 
@@ -309,25 +393,40 @@ int device_property_match_string(struct device *dev, const char *propname,
 }
 EXPORT_SYMBOL_GPL(device_property_match_string);
 
-#define OF_DEV_PROP_READ_ARRAY(node, propname, type, val, nval) \
-	(val) ? of_property_read_##type##_array((node), (propname), (val), (nval)) \
+#define OF_DEV_PROP_READ_ARRAY(node, propname, type, val, nval)				\
+	(val) ? of_property_read_##type##_array((node), (propname), (val), (nval))	\
 	      : of_property_count_elems_of_size((node), (propname), sizeof(type))
 
-#define FWNODE_PROP_READ_ARRAY(_fwnode_, _propname_, _type_, _proptype_, _val_, _nval_) \
-({ \
-	int _ret_; \
-	if (is_of_node(_fwnode_)) \
-		_ret_ = OF_DEV_PROP_READ_ARRAY(to_of_node(_fwnode_), _propname_, \
-					       _type_, _val_, _nval_); \
-	else if (is_acpi_node(_fwnode_)) \
-		_ret_ = acpi_node_prop_read(_fwnode_, _propname_, _proptype_, \
-					    _val_, _nval_); \
-	else if (is_pset(_fwnode_)) \
-		_ret_ = pset_prop_read_array(to_pset(_fwnode_), _propname_, \
-					     _proptype_, _val_, _nval_); \
-	else \
-		_ret_ = -ENXIO; \
-	_ret_; \
+#define PSET_PROP_READ_ARRAY(node, propname, type, val, nval)				\
+	(val) ? pset_prop_read_##type##_array((node), (propname), (val), (nval))	\
+	      : pset_prop_count_elems_of_size((node), (propname), sizeof(type))
+
+#define FWNODE_PROP_READ(_fwnode_, _propname_, _type_, _proptype_, _val_, _nval_)	\
+({											\
+	int _ret_;									\
+	if (is_of_node(_fwnode_))							\
+		_ret_ = OF_DEV_PROP_READ_ARRAY(to_of_node(_fwnode_), _propname_,	\
+					       _type_, _val_, _nval_);			\
+	else if (is_acpi_node(_fwnode_))						\
+		_ret_ = acpi_node_prop_read(_fwnode_, _propname_, _proptype_,		\
+					    _val_, _nval_);				\
+	else if (is_pset_node(_fwnode_)) 						\
+		_ret_ = PSET_PROP_READ_ARRAY(to_pset_node(_fwnode_), _propname_,	\
+					     _type_, _val_, _nval_);			\
+	else										\
+		_ret_ = -ENXIO;								\
+	_ret_;										\
+})
+
+#define FWNODE_PROP_READ_ARRAY(_fwnode_, _propname_, _type_, _proptype_, _val_, _nval_)	\
+({											\
+	int _ret_;									\
+	_ret_ = FWNODE_PROP_READ(_fwnode_, _propname_, _type_, _proptype_,		\
+				 _val_, _nval_);					\
+	if (_ret_ == -EINVAL && _fwnode_ && !IS_ERR_OR_NULL(_fwnode_->secondary))	\
+		_ret_ = FWNODE_PROP_READ(_fwnode_->secondary, _propname_, _type_,	\
+				_proptype_, _val_, _nval_);				\
+	_ret_;										\
 })
 
 /**
@@ -434,6 +533,41 @@ int fwnode_property_read_u64_array(struct fwnode_handle *fwnode,
 }
 EXPORT_SYMBOL_GPL(fwnode_property_read_u64_array);
 
+static int __fwnode_property_read_string_array(struct fwnode_handle *fwnode,
+					       const char *propname,
+					       const char **val, size_t nval)
+{
+	if (is_of_node(fwnode))
+		return val ?
+			of_property_read_string_array(to_of_node(fwnode),
+						      propname, val, nval) :
+			of_property_count_strings(to_of_node(fwnode), propname);
+	else if (is_acpi_node(fwnode))
+		return acpi_node_prop_read(fwnode, propname, DEV_PROP_STRING,
+					   val, nval);
+	else if (is_pset_node(fwnode))
+		return val ?
+			pset_prop_read_string_array(to_pset_node(fwnode),
+						    propname, val, nval) :
+			pset_prop_count_elems_of_size(to_pset_node(fwnode),
+						      propname,
+						      sizeof(const char *));
+	return -ENXIO;
+}
+
+static int __fwnode_property_read_string(struct fwnode_handle *fwnode,
+					 const char *propname, const char **val)
+{
+	if (is_of_node(fwnode))
+		return of_property_read_string(to_of_node(fwnode), propname, val);
+	else if (is_acpi_node(fwnode))
+		return acpi_node_prop_read(fwnode, propname, DEV_PROP_STRING,
+					   val, 1);
+	else if (is_pset_node(fwnode))
+		return pset_prop_read_string(to_pset_node(fwnode), propname, val);
+	return -ENXIO;
+}
+
 /**
  * fwnode_property_read_string_array - return string array property of a node
  * @fwnode: Firmware node to get the property of
@@ -456,18 +590,13 @@ int fwnode_property_read_string_array(struct fwnode_handle *fwnode,
 				      const char *propname, const char **val,
 				      size_t nval)
 {
-	if (is_of_node(fwnode))
-		return val ?
-			of_property_read_string_array(to_of_node(fwnode),
-						      propname, val, nval) :
-			of_property_count_strings(to_of_node(fwnode), propname);
-	else if (is_acpi_node(fwnode))
-		return acpi_node_prop_read(fwnode, propname, DEV_PROP_STRING,
-					   val, nval);
-	else if (is_pset(fwnode))
-		return pset_prop_read_array(to_pset(fwnode), propname,
-					    DEV_PROP_STRING, val, nval);
-	return -ENXIO;
+	int ret;
+
+	ret = __fwnode_property_read_string_array(fwnode, propname, val, nval);
+	if (ret == -EINVAL && fwnode && !IS_ERR_OR_NULL(fwnode->secondary))
+		ret = __fwnode_property_read_string_array(fwnode->secondary,
+							  propname, val, nval);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(fwnode_property_read_string_array);
 
@@ -489,14 +618,13 @@ EXPORT_SYMBOL_GPL(fwnode_property_read_string_array);
 int fwnode_property_read_string(struct fwnode_handle *fwnode,
 				const char *propname, const char **val)
 {
-	if (is_of_node(fwnode))
-		return of_property_read_string(to_of_node(fwnode), propname, val);
-	else if (is_acpi_node(fwnode))
-		return acpi_node_prop_read(fwnode, propname, DEV_PROP_STRING,
-					   val, 1);
+	int ret;
 
-	return pset_prop_read_array(to_pset(fwnode), propname,
-				    DEV_PROP_STRING, val, 1);
+	ret = __fwnode_property_read_string(fwnode, propname, val);
+	if (ret == -EINVAL && fwnode && !IS_ERR_OR_NULL(fwnode->secondary))
+		ret = __fwnode_property_read_string(fwnode->secondary,
+						    propname, val);
+	return ret;
 }
 EXPORT_SYMBOL_GPL(fwnode_property_read_string);
 
@@ -525,6 +653,9 @@ int fwnode_property_match_string(struct fwnode_handle *fwnode,
 	if (nval < 0)
 		return nval;
 
+	if (nval == 0)
+		return -ENODATA;
+
 	values = kcalloc(nval, sizeof(*values), GFP_KERNEL);
 	if (!values)
 		return -ENOMEM;
@@ -547,6 +678,182 @@ out:
 EXPORT_SYMBOL_GPL(fwnode_property_match_string);
 
 /**
+ * pset_free_set - releases memory allocated for copied property set
+ * @pset: Property set to release
+ *
+ * Function takes previously copied property set and releases all the
+ * memory allocated to it.
+ */
+static void pset_free_set(struct property_set *pset)
+{
+	const struct property_entry *prop;
+	size_t i, nval;
+
+	if (!pset)
+		return;
+
+	for (prop = pset->properties; prop->name; prop++) {
+		if (prop->is_array) {
+			if (prop->is_string && prop->pointer.str) {
+				nval = prop->length / sizeof(const char *);
+				for (i = 0; i < nval; i++)
+					kfree(prop->pointer.str[i]);
+			}
+			kfree(prop->pointer.raw_data);
+		} else if (prop->is_string) {
+			kfree(prop->value.str);
+		}
+		kfree(prop->name);
+	}
+
+	kfree(pset->properties);
+	kfree(pset);
+}
+
+static int pset_copy_entry(struct property_entry *dst,
+			   const struct property_entry *src)
+{
+	const char **d, **s;
+	size_t i, nval;
+
+	dst->name = kstrdup(src->name, GFP_KERNEL);
+	if (!dst->name)
+		return -ENOMEM;
+
+	if (src->is_array) {
+		if (!src->length)
+			return -ENODATA;
+
+		if (src->is_string) {
+			nval = src->length / sizeof(const char *);
+			dst->pointer.str = kcalloc(nval, sizeof(const char *),
+						   GFP_KERNEL);
+			if (!dst->pointer.str)
+				return -ENOMEM;
+
+			d = dst->pointer.str;
+			s = src->pointer.str;
+			for (i = 0; i < nval; i++) {
+				d[i] = kstrdup(s[i], GFP_KERNEL);
+				if (!d[i] && s[i])
+					return -ENOMEM;
+			}
+		} else {
+			dst->pointer.raw_data = kmemdup(src->pointer.raw_data,
+							src->length, GFP_KERNEL);
+			if (!dst->pointer.raw_data)
+				return -ENOMEM;
+		}
+	} else if (src->is_string) {
+		dst->value.str = kstrdup(src->value.str, GFP_KERNEL);
+		if (!dst->value.str && src->value.str)
+			return -ENOMEM;
+	} else {
+		dst->value.raw_data = src->value.raw_data;
+	}
+
+	dst->length = src->length;
+	dst->is_array = src->is_array;
+	dst->is_string = src->is_string;
+
+	return 0;
+}
+
+/**
+ * pset_copy_set - copies property set
+ * @pset: Property set to copy
+ *
+ * This function takes a deep copy of the given property set and returns
+ * pointer to the copy. Call device_free_property_set() to free resources
+ * allocated in this function.
+ *
+ * Return: Pointer to the new property set or error pointer.
+ */
+static struct property_set *pset_copy_set(const struct property_set *pset)
+{
+	const struct property_entry *entry;
+	struct property_set *p;
+	size_t i, n = 0;
+
+	p = kzalloc(sizeof(*p), GFP_KERNEL);
+	if (!p)
+		return ERR_PTR(-ENOMEM);
+
+	while (pset->properties[n].name)
+		n++;
+
+	p->properties = kcalloc(n + 1, sizeof(*entry), GFP_KERNEL);
+	if (!p->properties) {
+		kfree(p);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	for (i = 0; i < n; i++) {
+		int ret = pset_copy_entry(&p->properties[i],
+					  &pset->properties[i]);
+		if (ret) {
+			pset_free_set(p);
+			return ERR_PTR(ret);
+		}
+	}
+
+	return p;
+}
+
+/**
+ * device_remove_property_set - Remove properties from a device object.
+ * @dev: Device whose properties to remove.
+ *
+ * The function removes properties previously associated to the device
+ * secondary firmware node with device_add_property_set(). Memory allocated
+ * to the properties will also be released.
+ */
+void device_remove_property_set(struct device *dev)
+{
+	struct fwnode_handle *fwnode;
+
+	fwnode = dev_fwnode(dev);
+	if (!fwnode)
+		return;
+	/*
+	 * Pick either primary or secondary node depending which one holds
+	 * the pset. If there is no real firmware node (ACPI/DT) primary
+	 * will hold the pset.
+	 */
+	if (!is_pset_node(fwnode))
+		fwnode = fwnode->secondary;
+	if (!IS_ERR(fwnode) && is_pset_node(fwnode))
+		pset_free_set(to_pset_node(fwnode));
+	set_secondary_fwnode(dev, NULL);
+}
+EXPORT_SYMBOL_GPL(device_remove_property_set);
+
+/**
+ * device_add_property_set - Add a collection of properties to a device object.
+ * @dev: Device to add properties to.
+ * @pset: Collection of properties to add.
+ *
+ * Associate a collection of device properties represented by @pset with @dev
+ * as its secondary firmware node. The function takes a copy of @pset.
+ */
+int device_add_property_set(struct device *dev, const struct property_set *pset)
+{
+	struct property_set *p;
+
+	if (!pset)
+		return -EINVAL;
+
+	p = pset_copy_set(pset);
+	if (IS_ERR(p))
+		return PTR_ERR(p);
+
+	p->fwnode.type = FWNODE_PDATA;
+	set_secondary_fwnode(dev, &p->fwnode);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(device_add_property_set);
+
+/**
  * device_get_next_child_node - Return the next child node handle for a device
  * @dev: Device to find the next child node for.
  * @child: Handle to one of the device's child nodes or a null handle.
diff --git a/drivers/base/regmap/regcache-flat.c b/drivers/base/regmap/regcache-flat.c
index 0246f44ded74..686c9e0b930e 100644
--- a/drivers/base/regmap/regcache-flat.c
+++ b/drivers/base/regmap/regcache-flat.c
@@ -21,7 +21,7 @@ static int regcache_flat_init(struct regmap *map)
 	int i;
 	unsigned int *cache;
 
-	map->cache = kzalloc(sizeof(unsigned int) * (map->max_register + 1),
+	map->cache = kcalloc(map->max_register + 1, sizeof(unsigned int),
 			     GFP_KERNEL);
 	if (!map->cache)
 		return -ENOMEM;
diff --git a/drivers/base/regmap/regcache-lzo.c b/drivers/base/regmap/regcache-lzo.c
index 736e0d378567..6f77d7319fc6 100644
--- a/drivers/base/regmap/regcache-lzo.c
+++ b/drivers/base/regmap/regcache-lzo.c
@@ -139,7 +139,7 @@ static int regcache_lzo_init(struct regmap *map)
 	ret = 0;
 
 	blkcount = regcache_lzo_block_count(map);
-	map->cache = kzalloc(blkcount * sizeof *lzo_blocks,
+	map->cache = kcalloc(blkcount, sizeof(*lzo_blocks),
 			     GFP_KERNEL);
 	if (!map->cache)
 		return -ENOMEM;
@@ -152,8 +152,8 @@ static int regcache_lzo_init(struct regmap *map)
 	 * that register.
 	 */
 	bmp_size = map->num_reg_defaults_raw;
-	sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long),
-			   GFP_KERNEL);
+	sync_bmp = kmalloc_array(BITS_TO_LONGS(bmp_size), sizeof(long),
+				 GFP_KERNEL);
 	if (!sync_bmp) {
 		ret = -ENOMEM;
 		goto err;
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index 56486d92c4e7..aa56af87d941 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -361,13 +361,14 @@ regcache_rbtree_node_alloc(struct regmap *map, unsigned int reg)
 		rbnode->base_reg = reg;
 	}
 
-	rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size,
-				GFP_KERNEL);
+	rbnode->block = kmalloc_array(rbnode->blklen, map->cache_word_size,
+				      GFP_KERNEL);
 	if (!rbnode->block)
 		goto err_free;
 
-	rbnode->cache_present = kzalloc(BITS_TO_LONGS(rbnode->blklen) *
-		sizeof(*rbnode->cache_present), GFP_KERNEL);
+	rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen),
+					sizeof(*rbnode->cache_present),
+					GFP_KERNEL);
 	if (!rbnode->cache_present)
 		goto err_free_block;
 
@@ -413,8 +414,8 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 		max = reg + max_dist;
 
 		/* look for an adjacent register to the one we are about to add */
-		for (node = rb_first(&rbtree_ctx->root); node;
-		     node = rb_next(node)) {
+		node = rbtree_ctx->root.rb_node;
+		while (node) {
 			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
 					      node);
 
@@ -425,6 +426,11 @@ static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 				new_base_reg = min(reg, base_reg);
 				new_top_reg = max(reg, top_reg);
 			} else {
+				if (max < base_reg)
+					node = node->rb_left;
+				else
+					node = node->rb_right;
+
 				continue;
 			}
 
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index 4c07802986b2..348be3a35410 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -100,15 +100,25 @@ int regcache_init(struct regmap *map, const struct regmap_config *config)
 	int i;
 	void *tmp_buf;
 
-	for (i = 0; i < config->num_reg_defaults; i++)
-		if (config->reg_defaults[i].reg % map->reg_stride)
-			return -EINVAL;
-
 	if (map->cache_type == REGCACHE_NONE) {
+		if (config->reg_defaults || config->num_reg_defaults_raw)
+			dev_warn(map->dev,
+				 "No cache used with register defaults set!\n");
+
 		map->cache_bypass = true;
 		return 0;
 	}
 
+	if (config->reg_defaults && !config->num_reg_defaults) {
+		dev_err(map->dev,
+			 "Register defaults are set without the number!\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < config->num_reg_defaults; i++)
+		if (config->reg_defaults[i].reg % map->reg_stride)
+			return -EINVAL;
+
 	for (i = 0; i < ARRAY_SIZE(cache_types); i++)
 		if (cache_types[i]->type == map->cache_type)
 			break;
@@ -138,8 +148,6 @@ int regcache_init(struct regmap *map, const struct regmap_config *config)
 	 * a copy of it.
 	 */
 	if (config->reg_defaults) {
-		if (!map->num_reg_defaults)
-			return -EINVAL;
 		tmp_buf = kmemdup(config->reg_defaults, map->num_reg_defaults *
 				  sizeof(struct reg_default), GFP_KERNEL);
 		if (!tmp_buf)
@@ -535,19 +543,30 @@ bool regcache_set_val(struct regmap *map, void *base, unsigned int idx,
 	switch (map->cache_word_size) {
 	case 1: {
 		u8 *cache = base;
+
 		cache[idx] = val;
 		break;
 	}
 	case 2: {
 		u16 *cache = base;
+
 		cache[idx] = val;
 		break;
 	}
 	case 4: {
 		u32 *cache = base;
+
+		cache[idx] = val;
+		break;
+	}
+#ifdef CONFIG_64BIT
+	case 8: {
+		u64 *cache = base;
+
 		cache[idx] = val;
 		break;
 	}
+#endif
 	default:
 		BUG();
 	}
@@ -568,16 +587,26 @@ unsigned int regcache_get_val(struct regmap *map, const void *base,
 	switch (map->cache_word_size) {
 	case 1: {
 		const u8 *cache = base;
+
 		return cache[idx];
 	}
 	case 2: {
 		const u16 *cache = base;
+
 		return cache[idx];
 	}
 	case 4: {
 		const u32 *cache = base;
+
+		return cache[idx];
+	}
+#ifdef CONFIG_64BIT
+	case 8: {
+		const u64 *cache = base;
+
 		return cache[idx];
 	}
+#endif
 	default:
 		BUG();
 	}
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
index 3f0a7e262d69..1ee3d40861c7 100644
--- a/drivers/base/regmap/regmap-debugfs.c
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -397,72 +397,39 @@ static const struct file_operations regmap_reg_ranges_fops = {
 	.llseek = default_llseek,
 };
 
-static ssize_t regmap_access_read_file(struct file *file,
-				       char __user *user_buf, size_t count,
-				       loff_t *ppos)
+static int regmap_access_show(struct seq_file *s, void *ignored)
 {
-	int reg_len, tot_len;
-	size_t buf_pos = 0;
-	loff_t p = 0;
-	ssize_t ret;
-	int i;
-	struct regmap *map = file->private_data;
-	char *buf;
-
-	if (*ppos < 0 || !count)
-		return -EINVAL;
+	struct regmap *map = s->private;
+	int i, reg_len;
 
-	buf = kmalloc(count, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	/* Calculate the length of a fixed format  */
 	reg_len = regmap_calc_reg_len(map->max_register);
-	tot_len = reg_len + 10; /* ': R W V P\n' */
 
 	for (i = 0; i <= map->max_register; i += map->reg_stride) {
 		/* Ignore registers which are neither readable nor writable */
 		if (!regmap_readable(map, i) && !regmap_writeable(map, i))
 			continue;
 
-		/* If we're in the region the user is trying to read */
-		if (p >= *ppos) {
-			/* ...but not beyond it */
-			if (buf_pos + tot_len + 1 >= count)
-				break;
-
-			/* Format the register */
-			snprintf(buf + buf_pos, count - buf_pos,
-				 "%.*x: %c %c %c %c\n",
-				 reg_len, i,
-				 regmap_readable(map, i) ? 'y' : 'n',
-				 regmap_writeable(map, i) ? 'y' : 'n',
-				 regmap_volatile(map, i) ? 'y' : 'n',
-				 regmap_precious(map, i) ? 'y' : 'n');
-
-			buf_pos += tot_len;
-		}
-		p += tot_len;
-	}
-
-	ret = buf_pos;
-
-	if (copy_to_user(user_buf, buf, buf_pos)) {
-		ret = -EFAULT;
-		goto out;
+		/* Format the register */
+		seq_printf(s, "%.*x: %c %c %c %c\n", reg_len, i,
+			   regmap_readable(map, i) ? 'y' : 'n',
+			   regmap_writeable(map, i) ? 'y' : 'n',
+			   regmap_volatile(map, i) ? 'y' : 'n',
+			   regmap_precious(map, i) ? 'y' : 'n');
 	}
 
-	*ppos += buf_pos;
+	return 0;
+}
 
-out:
-	kfree(buf);
-	return ret;
+static int access_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, regmap_access_show, inode->i_private);
 }
 
 static const struct file_operations regmap_access_fops = {
-	.open = simple_open,
-	.read = regmap_access_read_file,
-	.llseek = default_llseek,
+	.open		= access_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
 };
 
 static ssize_t regmap_cache_only_write_file(struct file *file,
diff --git a/drivers/base/regmap/regmap-irq.c b/drivers/base/regmap/regmap-irq.c
index 8d16db533527..9b0d202414d0 100644
--- a/drivers/base/regmap/regmap-irq.c
+++ b/drivers/base/regmap/regmap-irq.c
@@ -39,8 +39,11 @@ struct regmap_irq_chip_data {
 	unsigned int *mask_buf;
 	unsigned int *mask_buf_def;
 	unsigned int *wake_buf;
+	unsigned int *type_buf;
+	unsigned int *type_buf_def;
 
 	unsigned int irq_reg_stride;
+	unsigned int type_reg_stride;
 };
 
 static inline const
@@ -144,6 +147,22 @@ static void regmap_irq_sync_unlock(struct irq_data *data)
 		}
 	}
 
+	for (i = 0; i < d->chip->num_type_reg; i++) {
+		if (!d->type_buf_def[i])
+			continue;
+		reg = d->chip->type_base +
+			(i * map->reg_stride * d->type_reg_stride);
+		if (d->chip->type_invert)
+			ret = regmap_update_bits(d->map, reg,
+				d->type_buf_def[i], ~d->type_buf[i]);
+		else
+			ret = regmap_update_bits(d->map, reg,
+				d->type_buf_def[i], d->type_buf[i]);
+		if (ret != 0)
+			dev_err(d->map->dev, "Failed to sync type in %x\n",
+				reg);
+	}
+
 	if (d->chip->runtime_pm)
 		pm_runtime_put(map->dev);
 
@@ -178,6 +197,38 @@ static void regmap_irq_disable(struct irq_data *data)
 	d->mask_buf[irq_data->reg_offset / map->reg_stride] |= irq_data->mask;
 }
 
+static int regmap_irq_set_type(struct irq_data *data, unsigned int type)
+{
+	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
+	struct regmap *map = d->map;
+	const struct regmap_irq *irq_data = irq_to_regmap_irq(d, data->hwirq);
+	int reg = irq_data->type_reg_offset / map->reg_stride;
+
+	if (!(irq_data->type_rising_mask | irq_data->type_falling_mask))
+		return 0;
+
+	d->type_buf[reg] &= ~(irq_data->type_falling_mask |
+					irq_data->type_rising_mask);
+	switch (type) {
+	case IRQ_TYPE_EDGE_FALLING:
+		d->type_buf[reg] |= irq_data->type_falling_mask;
+		break;
+
+	case IRQ_TYPE_EDGE_RISING:
+		d->type_buf[reg] |= irq_data->type_rising_mask;
+		break;
+
+	case IRQ_TYPE_EDGE_BOTH:
+		d->type_buf[reg] |= (irq_data->type_falling_mask |
+					irq_data->type_rising_mask);
+		break;
+
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static int regmap_irq_set_wake(struct irq_data *data, unsigned int on)
 {
 	struct regmap_irq_chip_data *d = irq_data_get_irq_chip_data(data);
@@ -204,6 +255,7 @@ static const struct irq_chip regmap_irq_chip = {
 	.irq_bus_sync_unlock	= regmap_irq_sync_unlock,
 	.irq_disable		= regmap_irq_disable,
 	.irq_enable		= regmap_irq_enable,
+	.irq_set_type		= regmap_irq_set_type,
 	.irq_set_wake		= regmap_irq_set_wake,
 };
 
@@ -386,28 +438,40 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	if (!d)
 		return -ENOMEM;
 
-	d->status_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+	d->status_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
 				GFP_KERNEL);
 	if (!d->status_buf)
 		goto err_alloc;
 
-	d->mask_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+	d->mask_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
 			      GFP_KERNEL);
 	if (!d->mask_buf)
 		goto err_alloc;
 
-	d->mask_buf_def = kzalloc(sizeof(unsigned int) * chip->num_regs,
+	d->mask_buf_def = kcalloc(chip->num_regs, sizeof(unsigned int),
 				  GFP_KERNEL);
 	if (!d->mask_buf_def)
 		goto err_alloc;
 
 	if (chip->wake_base) {
-		d->wake_buf = kzalloc(sizeof(unsigned int) * chip->num_regs,
+		d->wake_buf = kcalloc(chip->num_regs, sizeof(unsigned int),
 				      GFP_KERNEL);
 		if (!d->wake_buf)
 			goto err_alloc;
 	}
 
+	if (chip->num_type_reg) {
+		d->type_buf_def = kcalloc(chip->num_type_reg,
+					sizeof(unsigned int), GFP_KERNEL);
+		if (!d->type_buf_def)
+			goto err_alloc;
+
+		d->type_buf = kcalloc(chip->num_type_reg, sizeof(unsigned int),
+				      GFP_KERNEL);
+		if (!d->type_buf)
+			goto err_alloc;
+	}
+
 	d->irq_chip = regmap_irq_chip;
 	d->irq_chip.name = chip->name;
 	d->irq = irq;
@@ -420,10 +484,16 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 	else
 		d->irq_reg_stride = 1;
 
+	if (chip->type_reg_stride)
+		d->type_reg_stride = chip->type_reg_stride;
+	else
+		d->type_reg_stride = 1;
+
 	if (!map->use_single_read && map->reg_stride == 1 &&
 	    d->irq_reg_stride == 1) {
-		d->status_reg_buf = kmalloc(map->format.val_bytes *
-					    chip->num_regs, GFP_KERNEL);
+		d->status_reg_buf = kmalloc_array(chip->num_regs,
+						  map->format.val_bytes,
+						  GFP_KERNEL);
 		if (!d->status_reg_buf)
 			goto err_alloc;
 	}
@@ -511,6 +581,33 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 		}
 	}
 
+	if (chip->num_type_reg) {
+		for (i = 0; i < chip->num_irqs; i++) {
+			reg = chip->irqs[i].type_reg_offset / map->reg_stride;
+			d->type_buf_def[reg] |= chip->irqs[i].type_rising_mask |
+					chip->irqs[i].type_falling_mask;
+		}
+		for (i = 0; i < chip->num_type_reg; ++i) {
+			if (!d->type_buf_def[i])
+				continue;
+
+			reg = chip->type_base +
+				(i * map->reg_stride * d->type_reg_stride);
+			if (chip->type_invert)
+				ret = regmap_update_bits(map, reg,
+					d->type_buf_def[i], 0xFF);
+			else
+				ret = regmap_update_bits(map, reg,
+					d->type_buf_def[i], 0x0);
+			if (ret != 0) {
+				dev_err(map->dev,
+					"Failed to set type in 0x%x: %x\n",
+					reg, ret);
+				goto err_alloc;
+			}
+		}
+	}
+
 	if (irq_base)
 		d->domain = irq_domain_add_legacy(map->dev->of_node,
 						  chip->num_irqs, irq_base, 0,
@@ -541,6 +638,8 @@ int regmap_add_irq_chip(struct regmap *map, int irq, int irq_flags,
 err_domain:
 	/* Should really dispose of the domain but... */
 err_alloc:
+	kfree(d->type_buf);
+	kfree(d->type_buf_def);
 	kfree(d->wake_buf);
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
@@ -564,6 +663,8 @@ void regmap_del_irq_chip(int irq, struct regmap_irq_chip_data *d)
 
 	free_irq(irq, d);
 	irq_domain_remove(d->domain);
+	kfree(d->type_buf);
+	kfree(d->type_buf_def);
 	kfree(d->wake_buf);
 	kfree(d->mask_buf_def);
 	kfree(d->mask_buf);
diff --git a/drivers/base/regmap/regmap-mmio.c b/drivers/base/regmap/regmap-mmio.c
index 426a57e41ac7..eea51569f0eb 100644
--- a/drivers/base/regmap/regmap-mmio.c
+++ b/drivers/base/regmap/regmap-mmio.c
@@ -61,6 +61,33 @@ static int regmap_mmio_regbits_check(size_t reg_bits)
 	}
 }
 
+static int regmap_mmio_get_min_stride(size_t val_bits)
+{
+	int min_stride;
+
+	switch (val_bits) {
+	case 8:
+		/* The core treats 0 as 1 */
+		min_stride = 0;
+		return 0;
+	case 16:
+		min_stride = 2;
+		break;
+	case 32:
+		min_stride = 4;
+		break;
+#ifdef CONFIG_64BIT
+	case 64:
+		min_stride = 8;
+		break;
+#endif
+	default:
+		return -EINVAL;
+	}
+
+	return min_stride;
+}
+
 static inline void regmap_mmio_count_check(size_t count, u32 offset)
 {
 	BUG_ON(count <= offset);
@@ -231,26 +258,9 @@ static struct regmap_mmio_context *regmap_mmio_gen_context(struct device *dev,
 	if (config->pad_bits)
 		return ERR_PTR(-EINVAL);
 
-	switch (config->val_bits) {
-	case 8:
-		/* The core treats 0 as 1 */
-		min_stride = 0;
-		break;
-	case 16:
-		min_stride = 2;
-		break;
-	case 32:
-		min_stride = 4;
-		break;
-#ifdef CONFIG_64BIT
-	case 64:
-		min_stride = 8;
-		break;
-#endif
-		break;
-	default:
-		return ERR_PTR(-EINVAL);
-	}
+	min_stride = regmap_mmio_get_min_stride(config->val_bits);
+	if (min_stride < 0)
+		return ERR_PTR(min_stride);
 
 	if (config->reg_stride < min_stride)
 		return ERR_PTR(-EINVAL);
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 4ac63c0e50c7..ee54e841de4a 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -245,6 +245,28 @@ static void regmap_format_32_native(void *buf, unsigned int val,
 	*(u32 *)buf = val << shift;
 }
 
+#ifdef CONFIG_64BIT
+static void regmap_format_64_be(void *buf, unsigned int val, unsigned int shift)
+{
+	__be64 *b = buf;
+
+	b[0] = cpu_to_be64((u64)val << shift);
+}
+
+static void regmap_format_64_le(void *buf, unsigned int val, unsigned int shift)
+{
+	__le64 *b = buf;
+
+	b[0] = cpu_to_le64((u64)val << shift);
+}
+
+static void regmap_format_64_native(void *buf, unsigned int val,
+				    unsigned int shift)
+{
+	*(u64 *)buf = (u64)val << shift;
+}
+#endif
+
 static void regmap_parse_inplace_noop(void *buf)
 {
 }
@@ -332,6 +354,41 @@ static unsigned int regmap_parse_32_native(const void *buf)
 	return *(u32 *)buf;
 }
 
+#ifdef CONFIG_64BIT
+static unsigned int regmap_parse_64_be(const void *buf)
+{
+	const __be64 *b = buf;
+
+	return be64_to_cpu(b[0]);
+}
+
+static unsigned int regmap_parse_64_le(const void *buf)
+{
+	const __le64 *b = buf;
+
+	return le64_to_cpu(b[0]);
+}
+
+static void regmap_parse_64_be_inplace(void *buf)
+{
+	__be64 *b = buf;
+
+	b[0] = be64_to_cpu(b[0]);
+}
+
+static void regmap_parse_64_le_inplace(void *buf)
+{
+	__le64 *b = buf;
+
+	b[0] = le64_to_cpu(b[0]);
+}
+
+static unsigned int regmap_parse_64_native(const void *buf)
+{
+	return *(u64 *)buf;
+}
+#endif
+
 static void regmap_lock_mutex(void *__map)
 {
 	struct regmap *map = __map;
@@ -712,6 +769,21 @@ struct regmap *__regmap_init(struct device *dev,
 		}
 		break;
 
+#ifdef CONFIG_64BIT
+	case 64:
+		switch (reg_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_reg = regmap_format_64_be;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_reg = regmap_format_64_native;
+			break;
+		default:
+			goto err_map;
+		}
+		break;
+#endif
+
 	default:
 		goto err_map;
 	}
@@ -771,6 +843,28 @@ struct regmap *__regmap_init(struct device *dev,
 			goto err_map;
 		}
 		break;
+#ifdef CONFIG_64BIT
+	case 64:
+		switch (val_endian) {
+		case REGMAP_ENDIAN_BIG:
+			map->format.format_val = regmap_format_64_be;
+			map->format.parse_val = regmap_parse_64_be;
+			map->format.parse_inplace = regmap_parse_64_be_inplace;
+			break;
+		case REGMAP_ENDIAN_LITTLE:
+			map->format.format_val = regmap_format_64_le;
+			map->format.parse_val = regmap_parse_64_le;
+			map->format.parse_inplace = regmap_parse_64_le_inplace;
+			break;
+		case REGMAP_ENDIAN_NATIVE:
+			map->format.format_val = regmap_format_64_native;
+			map->format.parse_val = regmap_parse_64_native;
+			break;
+		default:
+			goto err_map;
+		}
+		break;
+#endif
 	}
 
 	if (map->format.format_write) {
@@ -1513,7 +1607,7 @@ int regmap_write(struct regmap *map, unsigned int reg, unsigned int val)
 {
 	int ret;
 
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	map->lock(map->lock_arg);
@@ -1540,7 +1634,7 @@ int regmap_write_async(struct regmap *map, unsigned int reg, unsigned int val)
 {
 	int ret;
 
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	map->lock(map->lock_arg);
@@ -1714,7 +1808,7 @@ int regmap_bulk_write(struct regmap *map, unsigned int reg, const void *val,
 
 	if (map->bus && !map->format.parse_inplace)
 		return -EINVAL;
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	/*
@@ -1983,7 +2077,7 @@ static int _regmap_multi_reg_write(struct regmap *map,
 			int reg = regs[i].reg;
 			if (!map->writeable_reg(map->dev, reg))
 				return -EINVAL;
-			if (reg % map->reg_stride)
+			if (!IS_ALIGNED(reg, map->reg_stride))
 				return -EINVAL;
 		}
 
@@ -2133,7 +2227,7 @@ int regmap_raw_write_async(struct regmap *map, unsigned int reg,
 
 	if (val_len % map->format.val_bytes)
 		return -EINVAL;
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	map->lock(map->lock_arg);
@@ -2260,7 +2354,7 @@ int regmap_read(struct regmap *map, unsigned int reg, unsigned int *val)
 {
 	int ret;
 
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	map->lock(map->lock_arg);
@@ -2296,7 +2390,7 @@ int regmap_raw_read(struct regmap *map, unsigned int reg, void *val,
 		return -EINVAL;
 	if (val_len % map->format.val_bytes)
 		return -EINVAL;
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 	if (val_count == 0)
 		return -EINVAL;
@@ -2414,7 +2508,7 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 	size_t val_bytes = map->format.val_bytes;
 	bool vol = regmap_volatile_range(map, reg, val_count);
 
-	if (reg % map->reg_stride)
+	if (!IS_ALIGNED(reg, map->reg_stride))
 		return -EINVAL;
 
 	if (map->bus && map->format.parse_inplace && (vol || map->cache_type == REGCACHE_NONE)) {
@@ -2488,11 +2582,19 @@ int regmap_bulk_read(struct regmap *map, unsigned int reg, void *val,
 				 * we assume that the values are native
 				 * endian.
 				 */
+#ifdef CONFIG_64BIT
+				u64 *u64 = val;
+#endif
 				u32 *u32 = val;
 				u16 *u16 = val;
 				u8 *u8 = val;
 
 				switch (map->format.val_bytes) {
+#ifdef CONFIG_64BIT
+				case 8:
+					u64[i] = ival;
+					break;
+#endif
 				case 4:
 					u32[i] = ival;
 					break;
diff --git a/drivers/bcma/driver_gpio.c b/drivers/bcma/driver_gpio.c
index 504899a72966..98067f757fb0 100644
--- a/drivers/bcma/driver_gpio.c
+++ b/drivers/bcma/driver_gpio.c
@@ -17,14 +17,9 @@
 
 #define BCMA_GPIO_MAX_PINS	32
 
-static inline struct bcma_drv_cc *bcma_gpio_get_cc(struct gpio_chip *chip)
-{
-	return container_of(chip, struct bcma_drv_cc, gpio);
-}
-
 static int bcma_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	return !!bcma_chipco_gpio_in(cc, 1 << gpio);
 }
@@ -32,14 +27,14 @@ static int bcma_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
 static void bcma_gpio_set_value(struct gpio_chip *chip, unsigned gpio,
 				int value)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	bcma_chipco_gpio_out(cc, 1 << gpio, value ? 1 << gpio : 0);
 }
 
 static int bcma_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	bcma_chipco_gpio_outen(cc, 1 << gpio, 0);
 	return 0;
@@ -48,7 +43,7 @@ static int bcma_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 static int bcma_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 				      int value)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	bcma_chipco_gpio_outen(cc, 1 << gpio, 1 << gpio);
 	bcma_chipco_gpio_out(cc, 1 << gpio, value ? 1 << gpio : 0);
@@ -57,7 +52,7 @@ static int bcma_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 static int bcma_gpio_request(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	bcma_chipco_gpio_control(cc, 1 << gpio, 0);
 	/* clear pulldown */
@@ -70,7 +65,7 @@ static int bcma_gpio_request(struct gpio_chip *chip, unsigned gpio)
 
 static void bcma_gpio_free(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
+	struct bcma_drv_cc *cc = gpiochip_get_data(chip);
 
 	/* clear pullup */
 	bcma_chipco_gpio_pullup(cc, 1 << gpio, 0);
@@ -81,7 +76,7 @@ static void bcma_gpio_free(struct gpio_chip *chip, unsigned gpio)
 static void bcma_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(gc);
+	struct bcma_drv_cc *cc = gpiochip_get_data(gc);
 	int gpio = irqd_to_hwirq(d);
 	u32 val = bcma_chipco_gpio_in(cc, BIT(gpio));
 
@@ -92,7 +87,7 @@ static void bcma_gpio_irq_unmask(struct irq_data *d)
 static void bcma_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct bcma_drv_cc *cc = bcma_gpio_get_cc(gc);
+	struct bcma_drv_cc *cc = gpiochip_get_data(gc);
 	int gpio = irqd_to_hwirq(d);
 
 	bcma_chipco_gpio_intmask(cc, BIT(gpio), 0);
@@ -188,7 +183,7 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	chip->direction_input	= bcma_gpio_direction_input;
 	chip->direction_output	= bcma_gpio_direction_output;
 	chip->owner		= THIS_MODULE;
-	chip->dev		= bcma_bus_get_host_dev(bus);
+	chip->parent		= bcma_bus_get_host_dev(bus);
 #if IS_BUILTIN(CONFIG_OF)
 	if (cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
 		chip->of_node	= cc->core->dev.of_node;
@@ -216,7 +211,7 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	else
 		chip->base		= -1;
 
-	err = gpiochip_add(chip);
+	err = gpiochip_add_data(chip, cc);
 	if (err)
 		return err;
 
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 59d8d0d14824..c466f752b067 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -668,11 +668,36 @@ static int bcma_device_uevent(struct device *dev, struct kobj_uevent_env *env)
 			      core->id.rev, core->id.class);
 }
 
-static int __init bcma_modinit(void)
+static unsigned int bcma_bus_registered;
+
+/*
+ * If built-in, bus has to be registered early, before any driver calls
+ * bcma_driver_register.
+ * Otherwise registering driver would trigger BUG in driver_register.
+ */
+static int __init bcma_init_bus_register(void)
 {
 	int err;
 
+	if (bcma_bus_registered)
+		return 0;
+
 	err = bus_register(&bcma_bus_type);
+	if (!err)
+		bcma_bus_registered = 1;
+
+	return err;
+}
+#ifndef MODULE
+fs_initcall(bcma_init_bus_register);
+#endif
+
+/* Main initialization has to be done with SPI/mtd/NAND/SPROM available */
+static int __init bcma_modinit(void)
+{
+	int err;
+
+	err = bcma_init_bus_register();
 	if (err)
 		return err;
 
@@ -691,7 +716,7 @@ static int __init bcma_modinit(void)
 
 	return err;
 }
-fs_initcall(bcma_modinit);
+module_init(bcma_modinit);
 
 static void __exit bcma_modexit(void)
 {
diff --git a/drivers/block/aoe/aoecmd.c b/drivers/block/aoe/aoecmd.c
index ad80c85e0857..d048d2009e89 100644
--- a/drivers/block/aoe/aoecmd.c
+++ b/drivers/block/aoe/aoecmd.c
@@ -964,9 +964,9 @@ aoecmd_sleepwork(struct work_struct *work)
 		ssize = get_capacity(d->gd);
 		bd = bdget_disk(d->gd, 0);
 		if (bd) {
-			mutex_lock(&bd->bd_inode->i_mutex);
+			inode_lock(bd->bd_inode);
 			i_size_write(bd->bd_inode, (loff_t)ssize<<9);
-			mutex_unlock(&bd->bd_inode->i_mutex);
+			inode_unlock(bd->bd_inode);
 			bdput(bd);
 		}
 		spin_lock_irq(&d->lock);
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index a5880f4ab40e..cb27190e9f39 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -19,6 +19,9 @@
 #include <linux/radix-tree.h>
 #include <linux/fs.h>
 #include <linux/slab.h>
+#ifdef CONFIG_BLK_DEV_RAM_DAX
+#include <linux/pfn_t.h>
+#endif
 
 #include <asm/uaccess.h>
 
@@ -378,7 +381,7 @@ static int brd_rw_page(struct block_device *bdev, sector_t sector,
 
 #ifdef CONFIG_BLK_DEV_RAM_DAX
 static long brd_direct_access(struct block_device *bdev, sector_t sector,
-			void __pmem **kaddr, unsigned long *pfn)
+			void __pmem **kaddr, pfn_t *pfn)
 {
 	struct brd_device *brd = bdev->bd_disk->private_data;
 	struct page *page;
@@ -389,7 +392,7 @@ static long brd_direct_access(struct block_device *bdev, sector_t sector,
 	if (!page)
 		return -ENOSPC;
 	*kaddr = (void __pmem *)page_address(page);
-	*pfn = page_to_pfn(page);
+	*pfn = page_to_pfn_t(page);
 
 	return PAGE_SIZE;
 }
diff --git a/drivers/block/cciss.c b/drivers/block/cciss.c
index 0422c47261c3..63c2064689f8 100644
--- a/drivers/block/cciss.c
+++ b/drivers/block/cciss.c
@@ -514,14 +514,9 @@ cciss_proc_write(struct file *file, const char __user *buf,
 	if (!buf || length > PAGE_SIZE - 1)
 		return -EINVAL;
 
-	buffer = (char *)__get_free_page(GFP_KERNEL);
-	if (!buffer)
-		return -ENOMEM;
-
-	err = -EFAULT;
-	if (copy_from_user(buffer, buf, length))
-		goto out;
-	buffer[length] = '\0';
+	buffer = memdup_user_nul(buf, length);
+	if (IS_ERR(buffer))
+		return PTR_ERR(buffer);
 
 #ifdef CONFIG_CISS_SCSI_TAPE
 	if (strncmp(ENGAGE_SCSI, buffer, sizeof ENGAGE_SCSI - 1) == 0) {
@@ -537,8 +532,7 @@ cciss_proc_write(struct file *file, const char __user *buf,
 	/* might be nice to have "disengage" too, but it's not
 	   safely possible. (only 1 module use count, lock issues.) */
 
-out:
-	free_page((unsigned long)buffer);
+	kfree(buffer);
 	return err;
 }
 
@@ -3854,7 +3848,7 @@ static void print_cfg_table(ctlr_info_t *h)
 	       readl(&(tb->HostWrite.CoalIntDelay)));
 	dev_dbg(&h->pdev->dev, "   Coalesce Interrupt Count = 0x%x\n",
 	       readl(&(tb->HostWrite.CoalIntCount)));
-	dev_dbg(&h->pdev->dev, "   Max outstanding commands = 0x%d\n",
+	dev_dbg(&h->pdev->dev, "   Max outstanding commands = 0x%x\n",
 	       readl(&(tb->CmdsOutMax)));
 	dev_dbg(&h->pdev->dev, "   Bus Types = 0x%x\n",
 		readl(&(tb->BusTypes)));
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index b3868e7a1ffd..10459a145062 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -288,7 +288,162 @@ bool drbd_al_begin_io_prepare(struct drbd_device *device, struct drbd_interval *
 	return need_transaction;
 }
 
-static int al_write_transaction(struct drbd_device *device);
+#if (PAGE_SHIFT + 3) < (AL_EXTENT_SHIFT - BM_BLOCK_SHIFT)
+/* Currently BM_BLOCK_SHIFT, BM_EXT_SHIFT and AL_EXTENT_SHIFT
+ * are still coupled, or assume too much about their relation.
+ * Code below will not work if this is violated.
+ * Will be cleaned up with some followup patch.
+ */
+# error FIXME
+#endif
+
+static unsigned int al_extent_to_bm_page(unsigned int al_enr)
+{
+	return al_enr >>
+		/* bit to page */
+		((PAGE_SHIFT + 3) -
+		/* al extent number to bit */
+		 (AL_EXTENT_SHIFT - BM_BLOCK_SHIFT));
+}
+
+static sector_t al_tr_number_to_on_disk_sector(struct drbd_device *device)
+{
+	const unsigned int stripes = device->ldev->md.al_stripes;
+	const unsigned int stripe_size_4kB = device->ldev->md.al_stripe_size_4k;
+
+	/* transaction number, modulo on-disk ring buffer wrap around */
+	unsigned int t = device->al_tr_number % (device->ldev->md.al_size_4k);
+
+	/* ... to aligned 4k on disk block */
+	t = ((t % stripes) * stripe_size_4kB) + t/stripes;
+
+	/* ... to 512 byte sector in activity log */
+	t *= 8;
+
+	/* ... plus offset to the on disk position */
+	return device->ldev->md.md_offset + device->ldev->md.al_offset + t;
+}
+
+static int __al_write_transaction(struct drbd_device *device, struct al_transaction_on_disk *buffer)
+{
+	struct lc_element *e;
+	sector_t sector;
+	int i, mx;
+	unsigned extent_nr;
+	unsigned crc = 0;
+	int err = 0;
+
+	memset(buffer, 0, sizeof(*buffer));
+	buffer->magic = cpu_to_be32(DRBD_AL_MAGIC);
+	buffer->tr_number = cpu_to_be32(device->al_tr_number);
+
+	i = 0;
+
+	/* Even though no one can start to change this list
+	 * once we set the LC_LOCKED -- from drbd_al_begin_io(),
+	 * lc_try_lock_for_transaction() --, someone may still
+	 * be in the process of changing it. */
+	spin_lock_irq(&device->al_lock);
+	list_for_each_entry(e, &device->act_log->to_be_changed, list) {
+		if (i == AL_UPDATES_PER_TRANSACTION) {
+			i++;
+			break;
+		}
+		buffer->update_slot_nr[i] = cpu_to_be16(e->lc_index);
+		buffer->update_extent_nr[i] = cpu_to_be32(e->lc_new_number);
+		if (e->lc_number != LC_FREE)
+			drbd_bm_mark_for_writeout(device,
+					al_extent_to_bm_page(e->lc_number));
+		i++;
+	}
+	spin_unlock_irq(&device->al_lock);
+	BUG_ON(i > AL_UPDATES_PER_TRANSACTION);
+
+	buffer->n_updates = cpu_to_be16(i);
+	for ( ; i < AL_UPDATES_PER_TRANSACTION; i++) {
+		buffer->update_slot_nr[i] = cpu_to_be16(-1);
+		buffer->update_extent_nr[i] = cpu_to_be32(LC_FREE);
+	}
+
+	buffer->context_size = cpu_to_be16(device->act_log->nr_elements);
+	buffer->context_start_slot_nr = cpu_to_be16(device->al_tr_cycle);
+
+	mx = min_t(int, AL_CONTEXT_PER_TRANSACTION,
+		   device->act_log->nr_elements - device->al_tr_cycle);
+	for (i = 0; i < mx; i++) {
+		unsigned idx = device->al_tr_cycle + i;
+		extent_nr = lc_element_by_index(device->act_log, idx)->lc_number;
+		buffer->context[i] = cpu_to_be32(extent_nr);
+	}
+	for (; i < AL_CONTEXT_PER_TRANSACTION; i++)
+		buffer->context[i] = cpu_to_be32(LC_FREE);
+
+	device->al_tr_cycle += AL_CONTEXT_PER_TRANSACTION;
+	if (device->al_tr_cycle >= device->act_log->nr_elements)
+		device->al_tr_cycle = 0;
+
+	sector = al_tr_number_to_on_disk_sector(device);
+
+	crc = crc32c(0, buffer, 4096);
+	buffer->crc32c = cpu_to_be32(crc);
+
+	if (drbd_bm_write_hinted(device))
+		err = -EIO;
+	else {
+		bool write_al_updates;
+		rcu_read_lock();
+		write_al_updates = rcu_dereference(device->ldev->disk_conf)->al_updates;
+		rcu_read_unlock();
+		if (write_al_updates) {
+			if (drbd_md_sync_page_io(device, device->ldev, sector, WRITE)) {
+				err = -EIO;
+				drbd_chk_io_error(device, 1, DRBD_META_IO_ERROR);
+			} else {
+				device->al_tr_number++;
+				device->al_writ_cnt++;
+			}
+		}
+	}
+
+	return err;
+}
+
+static int al_write_transaction(struct drbd_device *device)
+{
+	struct al_transaction_on_disk *buffer;
+	int err;
+
+	if (!get_ldev(device)) {
+		drbd_err(device, "disk is %s, cannot start al transaction\n",
+			drbd_disk_str(device->state.disk));
+		return -EIO;
+	}
+
+	/* The bitmap write may have failed, causing a state change. */
+	if (device->state.disk < D_INCONSISTENT) {
+		drbd_err(device,
+			"disk is %s, cannot write al transaction\n",
+			drbd_disk_str(device->state.disk));
+		put_ldev(device);
+		return -EIO;
+	}
+
+	/* protects md_io_buffer, al_tr_cycle, ... */
+	buffer = drbd_md_get_buffer(device, __func__);
+	if (!buffer) {
+		drbd_err(device, "disk failed while waiting for md_io buffer\n");
+		put_ldev(device);
+		return -ENODEV;
+	}
+
+	err = __al_write_transaction(device, buffer);
+
+	drbd_md_put_buffer(device);
+	put_ldev(device);
+
+	return err;
+}
+
 
 void drbd_al_begin_io_commit(struct drbd_device *device)
 {
@@ -420,153 +575,6 @@ void drbd_al_complete_io(struct drbd_device *device, struct drbd_interval *i)
 	wake_up(&device->al_wait);
 }
 
-#if (PAGE_SHIFT + 3) < (AL_EXTENT_SHIFT - BM_BLOCK_SHIFT)
-/* Currently BM_BLOCK_SHIFT, BM_EXT_SHIFT and AL_EXTENT_SHIFT
- * are still coupled, or assume too much about their relation.
- * Code below will not work if this is violated.
- * Will be cleaned up with some followup patch.
- */
-# error FIXME
-#endif
-
-static unsigned int al_extent_to_bm_page(unsigned int al_enr)
-{
-	return al_enr >>
-		/* bit to page */
-		((PAGE_SHIFT + 3) -
-		/* al extent number to bit */
-		 (AL_EXTENT_SHIFT - BM_BLOCK_SHIFT));
-}
-
-static sector_t al_tr_number_to_on_disk_sector(struct drbd_device *device)
-{
-	const unsigned int stripes = device->ldev->md.al_stripes;
-	const unsigned int stripe_size_4kB = device->ldev->md.al_stripe_size_4k;
-
-	/* transaction number, modulo on-disk ring buffer wrap around */
-	unsigned int t = device->al_tr_number % (device->ldev->md.al_size_4k);
-
-	/* ... to aligned 4k on disk block */
-	t = ((t % stripes) * stripe_size_4kB) + t/stripes;
-
-	/* ... to 512 byte sector in activity log */
-	t *= 8;
-
-	/* ... plus offset to the on disk position */
-	return device->ldev->md.md_offset + device->ldev->md.al_offset + t;
-}
-
-int al_write_transaction(struct drbd_device *device)
-{
-	struct al_transaction_on_disk *buffer;
-	struct lc_element *e;
-	sector_t sector;
-	int i, mx;
-	unsigned extent_nr;
-	unsigned crc = 0;
-	int err = 0;
-
-	if (!get_ldev(device)) {
-		drbd_err(device, "disk is %s, cannot start al transaction\n",
-			drbd_disk_str(device->state.disk));
-		return -EIO;
-	}
-
-	/* The bitmap write may have failed, causing a state change. */
-	if (device->state.disk < D_INCONSISTENT) {
-		drbd_err(device,
-			"disk is %s, cannot write al transaction\n",
-			drbd_disk_str(device->state.disk));
-		put_ldev(device);
-		return -EIO;
-	}
-
-	/* protects md_io_buffer, al_tr_cycle, ... */
-	buffer = drbd_md_get_buffer(device, __func__);
-	if (!buffer) {
-		drbd_err(device, "disk failed while waiting for md_io buffer\n");
-		put_ldev(device);
-		return -ENODEV;
-	}
-
-	memset(buffer, 0, sizeof(*buffer));
-	buffer->magic = cpu_to_be32(DRBD_AL_MAGIC);
-	buffer->tr_number = cpu_to_be32(device->al_tr_number);
-
-	i = 0;
-
-	/* Even though no one can start to change this list
-	 * once we set the LC_LOCKED -- from drbd_al_begin_io(),
-	 * lc_try_lock_for_transaction() --, someone may still
-	 * be in the process of changing it. */
-	spin_lock_irq(&device->al_lock);
-	list_for_each_entry(e, &device->act_log->to_be_changed, list) {
-		if (i == AL_UPDATES_PER_TRANSACTION) {
-			i++;
-			break;
-		}
-		buffer->update_slot_nr[i] = cpu_to_be16(e->lc_index);
-		buffer->update_extent_nr[i] = cpu_to_be32(e->lc_new_number);
-		if (e->lc_number != LC_FREE)
-			drbd_bm_mark_for_writeout(device,
-					al_extent_to_bm_page(e->lc_number));
-		i++;
-	}
-	spin_unlock_irq(&device->al_lock);
-	BUG_ON(i > AL_UPDATES_PER_TRANSACTION);
-
-	buffer->n_updates = cpu_to_be16(i);
-	for ( ; i < AL_UPDATES_PER_TRANSACTION; i++) {
-		buffer->update_slot_nr[i] = cpu_to_be16(-1);
-		buffer->update_extent_nr[i] = cpu_to_be32(LC_FREE);
-	}
-
-	buffer->context_size = cpu_to_be16(device->act_log->nr_elements);
-	buffer->context_start_slot_nr = cpu_to_be16(device->al_tr_cycle);
-
-	mx = min_t(int, AL_CONTEXT_PER_TRANSACTION,
-		   device->act_log->nr_elements - device->al_tr_cycle);
-	for (i = 0; i < mx; i++) {
-		unsigned idx = device->al_tr_cycle + i;
-		extent_nr = lc_element_by_index(device->act_log, idx)->lc_number;
-		buffer->context[i] = cpu_to_be32(extent_nr);
-	}
-	for (; i < AL_CONTEXT_PER_TRANSACTION; i++)
-		buffer->context[i] = cpu_to_be32(LC_FREE);
-
-	device->al_tr_cycle += AL_CONTEXT_PER_TRANSACTION;
-	if (device->al_tr_cycle >= device->act_log->nr_elements)
-		device->al_tr_cycle = 0;
-
-	sector = al_tr_number_to_on_disk_sector(device);
-
-	crc = crc32c(0, buffer, 4096);
-	buffer->crc32c = cpu_to_be32(crc);
-
-	if (drbd_bm_write_hinted(device))
-		err = -EIO;
-	else {
-		bool write_al_updates;
-		rcu_read_lock();
-		write_al_updates = rcu_dereference(device->ldev->disk_conf)->al_updates;
-		rcu_read_unlock();
-		if (write_al_updates) {
-			if (drbd_md_sync_page_io(device, device->ldev, sector, WRITE)) {
-				err = -EIO;
-				drbd_chk_io_error(device, 1, DRBD_META_IO_ERROR);
-			} else {
-				device->al_tr_number++;
-				device->al_writ_cnt++;
-			}
-		}
-	}
-
-	drbd_md_put_buffer(device);
-	put_ldev(device);
-
-	return err;
-}
-
 static int _try_lc_del(struct drbd_device *device, struct lc_element *al_ext)
 {
 	int rv;
@@ -606,21 +614,24 @@ void drbd_al_shrink(struct drbd_device *device)
 	wake_up(&device->al_wait);
 }
 
-int drbd_initialize_al(struct drbd_device *device, void *buffer)
+int drbd_al_initialize(struct drbd_device *device, void *buffer)
 {
 	struct al_transaction_on_disk *al = buffer;
 	struct drbd_md *md = &device->ldev->md;
-	sector_t al_base = md->md_offset + md->al_offset;
 	int al_size_4k = md->al_stripes * md->al_stripe_size_4k;
 	int i;
 
-	memset(al, 0, 4096);
-	al->magic = cpu_to_be32(DRBD_AL_MAGIC);
-	al->transaction_type = cpu_to_be16(AL_TR_INITIALIZED);
-	al->crc32c = cpu_to_be32(crc32c(0, al, 4096));
+	__al_write_transaction(device, al);
+	/* There may or may not have been a pending transaction. */
+	spin_lock_irq(&device->al_lock);
+	lc_committed(device->act_log);
+	spin_unlock_irq(&device->al_lock);
 
-	for (i = 0; i < al_size_4k; i++) {
-		int err = drbd_md_sync_page_io(device, device->ldev, al_base + i * 8, WRITE);
+	/* The rest of the transactions will have an empty "updates" list, and
+	 * are written out only to provide the context, and to initialize the
+	 * on-disk ring buffer. */
+	for (i = 1; i < al_size_4k; i++) {
+		int err = __al_write_transaction(device, al);
 		if (err)
 			return err;
 	}
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index 9462d2752850..92d6fc020a65 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -24,7 +24,7 @@
 
 #define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
 
-#include <linux/bitops.h>
+#include <linux/bitmap.h>
 #include <linux/vmalloc.h>
 #include <linux/string.h>
 #include <linux/drbd.h>
@@ -364,12 +364,9 @@ static void bm_free_pages(struct page **pages, unsigned long number)
 	}
 }
 
-static void bm_vk_free(void *ptr, int v)
+static inline void bm_vk_free(void *ptr)
 {
-	if (v)
-		vfree(ptr);
-	else
-		kfree(ptr);
+	kvfree(ptr);
 }
 
 /*
@@ -379,7 +376,7 @@ static struct page **bm_realloc_pages(struct drbd_bitmap *b, unsigned long want)
 {
 	struct page **old_pages = b->bm_pages;
 	struct page **new_pages, *page;
-	unsigned int i, bytes, vmalloced = 0;
+	unsigned int i, bytes;
 	unsigned long have = b->bm_number_of_pages;
 
 	BUG_ON(have == 0 && old_pages != NULL);
@@ -401,7 +398,6 @@ static struct page **bm_realloc_pages(struct drbd_bitmap *b, unsigned long want)
 				PAGE_KERNEL);
 		if (!new_pages)
 			return NULL;
-		vmalloced = 1;
 	}
 
 	if (want >= have) {
@@ -411,7 +407,7 @@ static struct page **bm_realloc_pages(struct drbd_bitmap *b, unsigned long want)
 			page = alloc_page(GFP_NOIO | __GFP_HIGHMEM);
 			if (!page) {
 				bm_free_pages(new_pages + have, i - have);
-				bm_vk_free(new_pages, vmalloced);
+				bm_vk_free(new_pages);
 				return NULL;
 			}
 			/* we want to know which page it is
@@ -427,11 +423,6 @@ static struct page **bm_realloc_pages(struct drbd_bitmap *b, unsigned long want)
 		*/
 	}
 
-	if (vmalloced)
-		b->bm_flags |= BM_P_VMALLOCED;
-	else
-		b->bm_flags &= ~BM_P_VMALLOCED;
-
 	return new_pages;
 }
 
@@ -469,7 +460,7 @@ void drbd_bm_cleanup(struct drbd_device *device)
 	if (!expect(device->bitmap))
 		return;
 	bm_free_pages(device->bitmap->bm_pages, device->bitmap->bm_number_of_pages);
-	bm_vk_free(device->bitmap->bm_pages, (BM_P_VMALLOCED & device->bitmap->bm_flags));
+	bm_vk_free(device->bitmap->bm_pages);
 	kfree(device->bitmap);
 	device->bitmap = NULL;
 }
@@ -479,8 +470,14 @@ void drbd_bm_cleanup(struct drbd_device *device)
  * this masks out the remaining bits.
  * Returns the number of bits cleared.
  */
+#ifndef BITS_PER_PAGE
 #define BITS_PER_PAGE		(1UL << (PAGE_SHIFT + 3))
 #define BITS_PER_PAGE_MASK	(BITS_PER_PAGE - 1)
+#else
+# if BITS_PER_PAGE != (1UL << (PAGE_SHIFT + 3))
+#  error "ambiguous BITS_PER_PAGE"
+# endif
+#endif
 #define BITS_PER_LONG_MASK	(BITS_PER_LONG - 1)
 static int bm_clear_surplus(struct drbd_bitmap *b)
 {
@@ -559,21 +556,19 @@ static unsigned long bm_count_bits(struct drbd_bitmap *b)
 	unsigned long *p_addr;
 	unsigned long bits = 0;
 	unsigned long mask = (1UL << (b->bm_bits & BITS_PER_LONG_MASK)) -1;
-	int idx, i, last_word;
+	int idx, last_word;
 
 	/* all but last page */
 	for (idx = 0; idx < b->bm_number_of_pages - 1; idx++) {
 		p_addr = __bm_map_pidx(b, idx);
-		for (i = 0; i < LWPP; i++)
-			bits += hweight_long(p_addr[i]);
+		bits += bitmap_weight(p_addr, BITS_PER_PAGE);
 		__bm_unmap(p_addr);
 		cond_resched();
 	}
 	/* last (or only) page */
 	last_word = ((b->bm_bits - 1) & BITS_PER_PAGE_MASK) >> LN2_BPL;
 	p_addr = __bm_map_pidx(b, idx);
-	for (i = 0; i < last_word; i++)
-		bits += hweight_long(p_addr[i]);
+	bits += bitmap_weight(p_addr, last_word * BITS_PER_LONG);
 	p_addr[last_word] &= cpu_to_lel(mask);
 	bits += hweight_long(p_addr[last_word]);
 	/* 32bit arch, may have an unused padding long */
@@ -639,7 +634,6 @@ int drbd_bm_resize(struct drbd_device *device, sector_t capacity, int set_new_bi
 	unsigned long want, have, onpages; /* number of pages */
 	struct page **npages, **opages = NULL;
 	int err = 0, growing;
-	int opages_vmalloced;
 
 	if (!expect(b))
 		return -ENOMEM;
@@ -652,8 +646,6 @@ int drbd_bm_resize(struct drbd_device *device, sector_t capacity, int set_new_bi
 	if (capacity == b->bm_dev_capacity)
 		goto out;
 
-	opages_vmalloced = (BM_P_VMALLOCED & b->bm_flags);
-
 	if (capacity == 0) {
 		spin_lock_irq(&b->bm_lock);
 		opages = b->bm_pages;
@@ -667,7 +659,7 @@ int drbd_bm_resize(struct drbd_device *device, sector_t capacity, int set_new_bi
 		b->bm_dev_capacity = 0;
 		spin_unlock_irq(&b->bm_lock);
 		bm_free_pages(opages, onpages);
-		bm_vk_free(opages, opages_vmalloced);
+		bm_vk_free(opages);
 		goto out;
 	}
 	bits  = BM_SECT_TO_BIT(ALIGN(capacity, BM_SECT_PER_BIT));
@@ -740,7 +732,7 @@ int drbd_bm_resize(struct drbd_device *device, sector_t capacity, int set_new_bi
 
 	spin_unlock_irq(&b->bm_lock);
 	if (opages != npages)
-		bm_vk_free(opages, opages_vmalloced);
+		bm_vk_free(opages);
 	if (!growing)
 		b->bm_set = bm_count_bits(b);
 	drbd_info(device, "resync bitmap: bits=%lu words=%lu pages=%lu\n", bits, words, want);
@@ -1419,6 +1411,9 @@ static inline void bm_set_full_words_within_one_page(struct drbd_bitmap *b,
 	int bits;
 	int changed = 0;
 	unsigned long *paddr = kmap_atomic(b->bm_pages[page_nr]);
+
+	/* I think it is more cache line friendly to hweight_long then set to ~0UL,
+	 * than to first bitmap_weight() all words, then bitmap_fill() all words */
 	for (i = first_word; i < last_word; i++) {
 		bits = hweight_long(paddr[i]);
 		paddr[i] = ~0UL;
@@ -1628,8 +1623,7 @@ int drbd_bm_e_weight(struct drbd_device *device, unsigned long enr)
 		int n = e-s;
 		p_addr = bm_map_pidx(b, bm_word_to_page_idx(b, s));
 		bm = p_addr + MLPP(s);
-		while (n--)
-			count += hweight_long(*bm++);
+		count += bitmap_weight(bm, n * BITS_PER_LONG);
 		bm_unmap(p_addr);
 	} else {
 		drbd_err(device, "start offset (%d) too large in drbd_bm_e_weight\n", s);
diff --git a/drivers/block/drbd/drbd_debugfs.c b/drivers/block/drbd/drbd_debugfs.c
index 6b88a35fb048..4de95bbff486 100644
--- a/drivers/block/drbd/drbd_debugfs.c
+++ b/drivers/block/drbd/drbd_debugfs.c
@@ -434,12 +434,12 @@ static int drbd_single_open(struct file *file, int (*show)(struct seq_file *, vo
 	if (!parent || d_really_is_negative(parent))
 		goto out;
 	/* serialize with d_delete() */
-	mutex_lock(&d_inode(parent)->i_mutex);
+	inode_lock(d_inode(parent));
 	/* Make sure the object is still alive */
 	if (simple_positive(file->f_path.dentry)
 	&& kref_get_unless_zero(kref))
 		ret = 0;
-	mutex_unlock(&d_inode(parent)->i_mutex);
+	inode_unlock(d_inode(parent));
 	if (!ret) {
 		ret = single_open(file, show, data);
 		if (ret)
@@ -771,6 +771,13 @@ static int device_data_gen_id_show(struct seq_file *m, void *ignored)
 	return 0;
 }
 
+static int device_ed_gen_id_show(struct seq_file *m, void *ignored)
+{
+	struct drbd_device *device = m->private;
+	seq_printf(m, "0x%016llX\n", (unsigned long long)device->ed_uuid);
+	return 0;
+}
+
 #define drbd_debugfs_device_attr(name)						\
 static int device_ ## name ## _open(struct inode *inode, struct file *file)	\
 {										\
@@ -796,6 +803,7 @@ drbd_debugfs_device_attr(oldest_requests)
 drbd_debugfs_device_attr(act_log_extents)
 drbd_debugfs_device_attr(resync_extents)
 drbd_debugfs_device_attr(data_gen_id)
+drbd_debugfs_device_attr(ed_gen_id)
 
 void drbd_debugfs_device_add(struct drbd_device *device)
 {
@@ -839,6 +847,7 @@ void drbd_debugfs_device_add(struct drbd_device *device)
 	DCF(act_log_extents);
 	DCF(resync_extents);
 	DCF(data_gen_id);
+	DCF(ed_gen_id);
 #undef DCF
 	return;
 
@@ -854,6 +863,7 @@ void drbd_debugfs_device_cleanup(struct drbd_device *device)
 	drbd_debugfs_remove(&device->debugfs_vol_act_log_extents);
 	drbd_debugfs_remove(&device->debugfs_vol_resync_extents);
 	drbd_debugfs_remove(&device->debugfs_vol_data_gen_id);
+	drbd_debugfs_remove(&device->debugfs_vol_ed_gen_id);
 	drbd_debugfs_remove(&device->debugfs_vol);
 }
 
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index e66d453a5f2b..34bc84efc29e 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -77,13 +77,6 @@ extern int fault_devs;
 extern char usermode_helper[];
 
 
-/* I don't remember why XCPU ...
- * This is used to wake the asender,
- * and to interrupt sending the sending task
- * on disconnect.
- */
-#define DRBD_SIG SIGXCPU
-
 /* This is used to stop/restart our threads.
  * Cannot use SIGTERM nor SIGKILL, since these
  * are sent out by init on runlevel changes
@@ -292,6 +285,9 @@ struct drbd_device_work {
 
 extern int drbd_wait_misc(struct drbd_device *, struct drbd_interval *);
 
+extern void lock_all_resources(void);
+extern void unlock_all_resources(void);
+
 struct drbd_request {
 	struct drbd_work w;
 	struct drbd_device *device;
@@ -504,7 +500,6 @@ enum {
 
 	MD_NO_FUA,		/* Users wants us to not use FUA/FLUSH on meta data dev */
 
-	SUSPEND_IO,		/* suspend application io */
 	BITMAP_IO,		/* suspend application io;
 				   once no more io in flight, start bitmap io */
 	BITMAP_IO_QUEUED,       /* Started bitmap IO */
@@ -541,9 +536,6 @@ struct drbd_bitmap; /* opaque for drbd_device */
 /* definition of bits in bm_flags to be used in drbd_bm_lock
  * and drbd_bitmap_io and friends. */
 enum bm_flag {
-	/* do we need to kfree, or vfree bm_pages? */
-	BM_P_VMALLOCED = 0x10000, /* internal use only, will be masked out */
-
 	/* currently locked for bulk operation */
 	BM_LOCKED_MASK = 0xf,
 
@@ -632,12 +624,6 @@ struct bm_io_work {
 	void (*done)(struct drbd_device *device, int rv);
 };
 
-enum write_ordering_e {
-	WO_none,
-	WO_drain_io,
-	WO_bdev_flush,
-};
-
 struct fifo_buffer {
 	unsigned int head_index;
 	unsigned int size;
@@ -650,8 +636,7 @@ extern struct fifo_buffer *fifo_alloc(int fifo_size);
 enum {
 	NET_CONGESTED,		/* The data socket is congested */
 	RESOLVE_CONFLICTS,	/* Set on one node, cleared on the peer! */
-	SEND_PING,		/* whether asender should send a ping asap */
-	SIGNAL_ASENDER,		/* whether asender wants to be interrupted */
+	SEND_PING,
 	GOT_PING_ACK,		/* set when we receive a ping_ack packet, ping_wait gets woken */
 	CONN_WD_ST_CHG_REQ,	/* A cluster wide state change on the connection is active */
 	CONN_WD_ST_CHG_OKAY,
@@ -670,6 +655,8 @@ enum {
 	DEVICE_WORK_PENDING,	/* tell worker that some device has pending work */
 };
 
+enum which_state { NOW, OLD = NOW, NEW };
+
 struct drbd_resource {
 	char *name;
 #ifdef CONFIG_DEBUG_FS
@@ -755,7 +742,8 @@ struct drbd_connection {
 	unsigned long last_reconnect_jif;
 	struct drbd_thread receiver;
 	struct drbd_thread worker;
-	struct drbd_thread asender;
+	struct drbd_thread ack_receiver;
+	struct workqueue_struct *ack_sender;
 
 	/* cached pointers,
 	 * so we can look up the oldest pending requests more quickly.
@@ -774,6 +762,8 @@ struct drbd_connection {
 	struct drbd_thread_timing_details r_timing_details[DRBD_THREAD_DETAILS_HIST];
 
 	struct {
+		unsigned long last_sent_barrier_jif;
+
 		/* whether this sender thread
 		 * has processed a single write yet. */
 		bool seen_any_write_yet;
@@ -788,6 +778,17 @@ struct drbd_connection {
 	} send;
 };
 
+static inline bool has_net_conf(struct drbd_connection *connection)
+{
+	bool has_net_conf;
+
+	rcu_read_lock();
+	has_net_conf = rcu_dereference(connection->net_conf);
+	rcu_read_unlock();
+
+	return has_net_conf;
+}
+
 void __update_timing_details(
 		struct drbd_thread_timing_details *tdp,
 		unsigned int *cb_nr,
@@ -811,6 +812,7 @@ struct drbd_peer_device {
 	struct list_head peer_devices;
 	struct drbd_device *device;
 	struct drbd_connection *connection;
+	struct work_struct send_acks_work;
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *debugfs_peer_dev;
 #endif
@@ -829,6 +831,7 @@ struct drbd_device {
 	struct dentry *debugfs_vol_act_log_extents;
 	struct dentry *debugfs_vol_resync_extents;
 	struct dentry *debugfs_vol_data_gen_id;
+	struct dentry *debugfs_vol_ed_gen_id;
 #endif
 
 	unsigned int vnr;	/* volume number within the connection */
@@ -873,6 +876,7 @@ struct drbd_device {
 	atomic_t rs_pending_cnt; /* RS request/data packets on the wire */
 	atomic_t unacked_cnt;	 /* Need to send replies for */
 	atomic_t local_cnt;	 /* Waiting for local completion */
+	atomic_t suspend_cnt;
 
 	/* Interval tree of pending local requests */
 	struct rb_root read_requests;
@@ -1020,6 +1024,12 @@ static inline struct drbd_peer_device *first_peer_device(struct drbd_device *dev
 	return list_first_entry_or_null(&device->peer_devices, struct drbd_peer_device, peer_devices);
 }
 
+static inline struct drbd_peer_device *
+conn_peer_device(struct drbd_connection *connection, int volume_number)
+{
+	return idr_find(&connection->peer_devices, volume_number);
+}
+
 #define for_each_resource(resource, _resources) \
 	list_for_each_entry(resource, _resources, resources)
 
@@ -1113,7 +1123,7 @@ extern int drbd_send_ov_request(struct drbd_peer_device *, sector_t sector, int
 extern int drbd_send_bitmap(struct drbd_device *device);
 extern void drbd_send_sr_reply(struct drbd_peer_device *, enum drbd_state_rv retcode);
 extern void conn_send_sr_reply(struct drbd_connection *connection, enum drbd_state_rv retcode);
-extern void drbd_free_ldev(struct drbd_backing_dev *ldev);
+extern void drbd_backing_dev_free(struct drbd_device *device, struct drbd_backing_dev *ldev);
 extern void drbd_device_cleanup(struct drbd_device *device);
 void drbd_print_uuids(struct drbd_device *device, const char *text);
 
@@ -1424,7 +1434,7 @@ extern struct bio_set *drbd_md_io_bio_set;
 /* to allocate from that set */
 extern struct bio *bio_alloc_drbd(gfp_t gfp_mask);
 
-extern rwlock_t global_state_lock;
+extern struct mutex resources_mutex;
 
 extern int conn_lowest_minor(struct drbd_connection *connection);
 extern enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsigned int minor);
@@ -1454,6 +1464,9 @@ extern int is_valid_ar_handle(struct drbd_request *, sector_t);
 
 
 /* drbd_nl.c */
+
+extern struct mutex notification_mutex;
+
 extern void drbd_suspend_io(struct drbd_device *device);
 extern void drbd_resume_io(struct drbd_device *device);
 extern char *ppsize(char *buf, unsigned long long size);
@@ -1536,7 +1549,9 @@ extern void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req);
 
 /* drbd_receiver.c */
 extern int drbd_receiver(struct drbd_thread *thi);
-extern int drbd_asender(struct drbd_thread *thi);
+extern int drbd_ack_receiver(struct drbd_thread *thi);
+extern void drbd_send_ping_wf(struct work_struct *ws);
+extern void drbd_send_acks_wf(struct work_struct *ws);
 extern bool drbd_rs_c_min_rate_throttle(struct drbd_device *device);
 extern bool drbd_rs_should_slow_down(struct drbd_device *device, sector_t sector,
 		bool throttle_if_app_is_waiting);
@@ -1649,7 +1664,7 @@ extern int __drbd_change_sync(struct drbd_device *device, sector_t sector, int s
 #define drbd_rs_failed_io(device, sector, size) \
 	__drbd_change_sync(device, sector, size, RECORD_RS_FAILED)
 extern void drbd_al_shrink(struct drbd_device *device);
-extern int drbd_initialize_al(struct drbd_device *, void *);
+extern int drbd_al_initialize(struct drbd_device *, void *);
 
 /* drbd_nl.c */
 /* state info broadcast */
@@ -1668,6 +1683,29 @@ struct sib_info {
 };
 void drbd_bcast_event(struct drbd_device *device, const struct sib_info *sib);
 
+extern void notify_resource_state(struct sk_buff *,
+				  unsigned int,
+				  struct drbd_resource *,
+				  struct resource_info *,
+				  enum drbd_notification_type);
+extern void notify_device_state(struct sk_buff *,
+				unsigned int,
+				struct drbd_device *,
+				struct device_info *,
+				enum drbd_notification_type);
+extern void notify_connection_state(struct sk_buff *,
+				    unsigned int,
+				    struct drbd_connection *,
+				    struct connection_info *,
+				    enum drbd_notification_type);
+extern void notify_peer_device_state(struct sk_buff *,
+				     unsigned int,
+				     struct drbd_peer_device *,
+				     struct peer_device_info *,
+				     enum drbd_notification_type);
+extern void notify_helper(enum drbd_notification_type, struct drbd_device *,
+			  struct drbd_connection *, const char *, int);
+
 /*
  * inline helper functions
  *************************/
@@ -1694,19 +1732,6 @@ static inline int drbd_peer_req_has_active_page(struct drbd_peer_request *peer_r
 	return 0;
 }
 
-static inline enum drbd_state_rv
-_drbd_set_state(struct drbd_device *device, union drbd_state ns,
-		enum chg_state_flags flags, struct completion *done)
-{
-	enum drbd_state_rv rv;
-
-	read_lock(&global_state_lock);
-	rv = __drbd_set_state(device, ns, flags, done);
-	read_unlock(&global_state_lock);
-
-	return rv;
-}
-
 static inline union drbd_state drbd_read_state(struct drbd_device *device)
 {
 	struct drbd_resource *resource = device->resource;
@@ -1937,16 +1962,21 @@ drbd_device_post_work(struct drbd_device *device, int work_bit)
 
 extern void drbd_flush_workqueue(struct drbd_work_queue *work_queue);
 
-static inline void wake_asender(struct drbd_connection *connection)
+/* To get the ack_receiver out of the blocking network stack,
+ * so it can change its sk_rcvtimeo from idle- to ping-timeout,
+ * and send a ping, we need to send a signal.
+ * Which signal we send is irrelevant. */
+static inline void wake_ack_receiver(struct drbd_connection *connection)
 {
-	if (test_bit(SIGNAL_ASENDER, &connection->flags))
-		force_sig(DRBD_SIG, connection->asender.task);
+	struct task_struct *task = connection->ack_receiver.task;
+	if (task && get_t_state(&connection->ack_receiver) == RUNNING)
+		force_sig(SIGXCPU, task);
 }
 
 static inline void request_ping(struct drbd_connection *connection)
 {
 	set_bit(SEND_PING, &connection->flags);
-	wake_asender(connection);
+	wake_ack_receiver(connection);
 }
 
 extern void *conn_prepare_command(struct drbd_connection *, struct drbd_socket *);
@@ -2230,7 +2260,7 @@ static inline bool may_inc_ap_bio(struct drbd_device *device)
 
 	if (drbd_suspended(device))
 		return false;
-	if (test_bit(SUSPEND_IO, &device->flags))
+	if (atomic_read(&device->suspend_cnt))
 		return false;
 
 	/* to avoid potential deadlock or bitmap corruption,
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 74d97f4bac34..5b43dfb79819 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -117,6 +117,7 @@ module_param_string(usermode_helper, usermode_helper, sizeof(usermode_helper), 0
  */
 struct idr drbd_devices;
 struct list_head drbd_resources;
+struct mutex resources_mutex;
 
 struct kmem_cache *drbd_request_cache;
 struct kmem_cache *drbd_ee_cache;	/* peer requests */
@@ -1435,8 +1436,8 @@ static int we_should_drop_the_connection(struct drbd_connection *connection, str
 	/* long elapsed = (long)(jiffies - device->last_received); */
 
 	drop_it =   connection->meta.socket == sock
-		|| !connection->asender.task
-		|| get_t_state(&connection->asender) != RUNNING
+		|| !connection->ack_receiver.task
+		|| get_t_state(&connection->ack_receiver) != RUNNING
 		|| connection->cstate < C_WF_REPORT_PARAMS;
 
 	if (drop_it)
@@ -1793,15 +1794,6 @@ int drbd_send(struct drbd_connection *connection, struct socket *sock,
 		drbd_update_congested(connection);
 	}
 	do {
-		/* STRANGE
-		 * tcp_sendmsg does _not_ use its size parameter at all ?
-		 *
-		 * -EAGAIN on timeout, -EINTR on signal.
-		 */
-/* THINK
- * do we need to block DRBD_SIG if sock == &meta.socket ??
- * otherwise wake_asender() might interrupt some send_*Ack !
- */
 		rv = kernel_sendmsg(sock, &msg, &iov, 1, size);
 		if (rv == -EAGAIN) {
 			if (we_should_drop_the_connection(connection, sock))
@@ -2000,7 +1992,7 @@ void drbd_device_cleanup(struct drbd_device *device)
 		drbd_bm_cleanup(device);
 	}
 
-	drbd_free_ldev(device->ldev);
+	drbd_backing_dev_free(device, device->ldev);
 	device->ldev = NULL;
 
 	clear_bit(AL_SUSPENDED, &device->flags);
@@ -2179,7 +2171,7 @@ void drbd_destroy_device(struct kref *kref)
 	if (device->this_bdev)
 		bdput(device->this_bdev);
 
-	drbd_free_ldev(device->ldev);
+	drbd_backing_dev_free(device, device->ldev);
 	device->ldev = NULL;
 
 	drbd_release_all_peer_reqs(device);
@@ -2563,7 +2555,7 @@ int set_resource_options(struct drbd_resource *resource, struct res_opts *res_op
 		cpumask_copy(resource->cpu_mask, new_cpu_mask);
 		for_each_connection_rcu(connection, resource) {
 			connection->receiver.reset_cpu_mask = 1;
-			connection->asender.reset_cpu_mask = 1;
+			connection->ack_receiver.reset_cpu_mask = 1;
 			connection->worker.reset_cpu_mask = 1;
 		}
 	}
@@ -2590,7 +2582,7 @@ struct drbd_resource *drbd_create_resource(const char *name)
 	kref_init(&resource->kref);
 	idr_init(&resource->devices);
 	INIT_LIST_HEAD(&resource->connections);
-	resource->write_ordering = WO_bdev_flush;
+	resource->write_ordering = WO_BDEV_FLUSH;
 	list_add_tail_rcu(&resource->resources, &drbd_resources);
 	mutex_init(&resource->conf_update);
 	mutex_init(&resource->adm_mutex);
@@ -2652,8 +2644,8 @@ struct drbd_connection *conn_create(const char *name, struct res_opts *res_opts)
 	connection->receiver.connection = connection;
 	drbd_thread_init(resource, &connection->worker, drbd_worker, "worker");
 	connection->worker.connection = connection;
-	drbd_thread_init(resource, &connection->asender, drbd_asender, "asender");
-	connection->asender.connection = connection;
+	drbd_thread_init(resource, &connection->ack_receiver, drbd_ack_receiver, "ack_recv");
+	connection->ack_receiver.connection = connection;
 
 	kref_init(&connection->kref);
 
@@ -2702,8 +2694,8 @@ static int init_submitter(struct drbd_device *device)
 {
 	/* opencoded create_singlethread_workqueue(),
 	 * to be able to say "drbd%d", ..., minor */
-	device->submit.wq = alloc_workqueue("drbd%u_submit",
-			WQ_UNBOUND | WQ_MEM_RECLAIM, 1, device->minor);
+	device->submit.wq =
+		alloc_ordered_workqueue("drbd%u_submit", WQ_MEM_RECLAIM, device->minor);
 	if (!device->submit.wq)
 		return -ENOMEM;
 
@@ -2820,6 +2812,7 @@ enum drbd_ret_code drbd_create_device(struct drbd_config_context *adm_ctx, unsig
 			goto out_idr_remove_from_resource;
 		}
 		kref_get(&connection->kref);
+		INIT_WORK(&peer_device->send_acks_work, drbd_send_acks_wf);
 	}
 
 	if (init_submitter(device)) {
@@ -2923,7 +2916,7 @@ static int __init drbd_init(void)
 	drbd_proc = NULL; /* play safe for drbd_cleanup */
 	idr_init(&drbd_devices);
 
-	rwlock_init(&global_state_lock);
+	mutex_init(&resources_mutex);
 	INIT_LIST_HEAD(&drbd_resources);
 
 	err = drbd_genl_register();
@@ -2971,18 +2964,6 @@ fail:
 	return err;
 }
 
-void drbd_free_ldev(struct drbd_backing_dev *ldev)
-{
-	if (ldev == NULL)
-		return;
-
-	blkdev_put(ldev->backing_bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
-	blkdev_put(ldev->md_bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
-
-	kfree(ldev->disk_conf);
-	kfree(ldev);
-}
-
 static void drbd_free_one_sock(struct drbd_socket *ds)
 {
 	struct socket *s;
@@ -3277,6 +3258,10 @@ int drbd_md_read(struct drbd_device *device, struct drbd_backing_dev *bdev)
 	 * and read it. */
 	bdev->md.meta_dev_idx = bdev->disk_conf->meta_dev_idx;
 	bdev->md.md_offset = drbd_md_ss(bdev);
+	/* Even for (flexible or indexed) external meta data,
+	 * initially restrict us to the 4k superblock for now.
+	 * Affects the paranoia out-of-range access check in drbd_md_sync_page_io(). */
+	bdev->md.md_size_sect = 8;
 
 	if (drbd_md_sync_page_io(device, bdev, bdev->md.md_offset, READ)) {
 		/* NOTE: can't do normal error processing here as this is
@@ -3578,7 +3563,9 @@ void drbd_queue_bitmap_io(struct drbd_device *device,
 
 	spin_lock_irq(&device->resource->req_lock);
 	set_bit(BITMAP_IO, &device->flags);
-	if (atomic_read(&device->ap_bio_cnt) == 0) {
+	/* don't wait for pending application IO if the caller indicates that
+	 * application IO does not conflict anyways. */
+	if (flags == BM_LOCKED_CHANGE_ALLOWED || atomic_read(&device->ap_bio_cnt) == 0) {
 		if (!test_and_set_bit(BITMAP_IO_QUEUED, &device->flags))
 			drbd_queue_work(&first_peer_device(device)->connection->sender_work,
 					&device->bm_io_work.w);
@@ -3746,6 +3733,27 @@ int drbd_wait_misc(struct drbd_device *device, struct drbd_interval *i)
 	return 0;
 }
 
+void lock_all_resources(void)
+{
+	struct drbd_resource *resource;
+	int __maybe_unused i = 0;
+
+	mutex_lock(&resources_mutex);
+	local_irq_disable();
+	for_each_resource(resource, &drbd_resources)
+		spin_lock_nested(&resource->req_lock, i++);
+}
+
+void unlock_all_resources(void)
+{
+	struct drbd_resource *resource;
+
+	for_each_resource(resource, &drbd_resources)
+		spin_unlock(&resource->req_lock);
+	local_irq_enable();
+	mutex_unlock(&resources_mutex);
+}
+
 #ifdef CONFIG_DRBD_FAULT_INJECTION
 /* Fault insertion support including random number generator shamelessly
  * stolen from kernel/rcutorture.c */
diff --git a/drivers/block/drbd/drbd_nl.c b/drivers/block/drbd/drbd_nl.c
index e80cbefbc2b5..c055c5e12f24 100644
--- a/drivers/block/drbd/drbd_nl.c
+++ b/drivers/block/drbd/drbd_nl.c
@@ -36,6 +36,7 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
+#include "drbd_state_change.h"
 #include <asm/unaligned.h>
 #include <linux/drbd_limits.h>
 #include <linux/kthread.h>
@@ -75,11 +76,24 @@ int drbd_adm_get_status(struct sk_buff *skb, struct genl_info *info);
 int drbd_adm_get_timeout_type(struct sk_buff *skb, struct genl_info *info);
 /* .dumpit */
 int drbd_adm_get_status_all(struct sk_buff *skb, struct netlink_callback *cb);
+int drbd_adm_dump_resources(struct sk_buff *skb, struct netlink_callback *cb);
+int drbd_adm_dump_devices(struct sk_buff *skb, struct netlink_callback *cb);
+int drbd_adm_dump_devices_done(struct netlink_callback *cb);
+int drbd_adm_dump_connections(struct sk_buff *skb, struct netlink_callback *cb);
+int drbd_adm_dump_connections_done(struct netlink_callback *cb);
+int drbd_adm_dump_peer_devices(struct sk_buff *skb, struct netlink_callback *cb);
+int drbd_adm_dump_peer_devices_done(struct netlink_callback *cb);
+int drbd_adm_get_initial_state(struct sk_buff *skb, struct netlink_callback *cb);
 
 #include <linux/drbd_genl_api.h>
 #include "drbd_nla.h"
 #include <linux/genl_magic_func.h>
 
+static atomic_t drbd_genl_seq = ATOMIC_INIT(2); /* two. */
+static atomic_t notify_genl_seq = ATOMIC_INIT(2); /* two. */
+
+DEFINE_MUTEX(notification_mutex);
+
 /* used blkdev_get_by_path, to claim our meta data device(s) */
 static char *drbd_m_holder = "Hands off! this is DRBD's meta data device.";
 
@@ -349,6 +363,7 @@ int drbd_khelper(struct drbd_device *device, char *cmd)
 	sib.sib_reason = SIB_HELPER_PRE;
 	sib.helper_name = cmd;
 	drbd_bcast_event(device, &sib);
+	notify_helper(NOTIFY_CALL, device, connection, cmd, 0);
 	ret = call_usermodehelper(usermode_helper, argv, envp, UMH_WAIT_PROC);
 	if (ret)
 		drbd_warn(device, "helper command: %s %s %s exit code %u (0x%x)\n",
@@ -361,6 +376,7 @@ int drbd_khelper(struct drbd_device *device, char *cmd)
 	sib.sib_reason = SIB_HELPER_POST;
 	sib.helper_exit_code = ret;
 	drbd_bcast_event(device, &sib);
+	notify_helper(NOTIFY_RESPONSE, device, connection, cmd, ret);
 
 	if (current == connection->worker.task)
 		clear_bit(CALLBACK_PENDING, &connection->flags);
@@ -388,6 +404,7 @@ static int conn_khelper(struct drbd_connection *connection, char *cmd)
 
 	drbd_info(connection, "helper command: %s %s %s\n", usermode_helper, cmd, resource_name);
 	/* TODO: conn_bcast_event() ?? */
+	notify_helper(NOTIFY_CALL, NULL, connection, cmd, 0);
 
 	ret = call_usermodehelper(usermode_helper, argv, envp, UMH_WAIT_PROC);
 	if (ret)
@@ -399,6 +416,7 @@ static int conn_khelper(struct drbd_connection *connection, char *cmd)
 			  usermode_helper, cmd, resource_name,
 			  (ret >> 8) & 0xff, ret);
 	/* TODO: conn_bcast_event() ?? */
+	notify_helper(NOTIFY_RESPONSE, NULL, connection, cmd, ret);
 
 	if (ret < 0) /* Ignore any ERRNOs we got. */
 		ret = 0;
@@ -847,9 +865,11 @@ char *ppsize(char *buf, unsigned long long size)
  * and can be long lived.
  * This changes an device->flag, is triggered by drbd internals,
  * and should be short-lived. */
+/* It needs to be a counter, since multiple threads might
+   independently suspend and resume IO. */
 void drbd_suspend_io(struct drbd_device *device)
 {
-	set_bit(SUSPEND_IO, &device->flags);
+	atomic_inc(&device->suspend_cnt);
 	if (drbd_suspended(device))
 		return;
 	wait_event(device->misc_wait, !atomic_read(&device->ap_bio_cnt));
@@ -857,8 +877,8 @@ void drbd_suspend_io(struct drbd_device *device)
 
 void drbd_resume_io(struct drbd_device *device)
 {
-	clear_bit(SUSPEND_IO, &device->flags);
-	wake_up(&device->misc_wait);
+	if (atomic_dec_and_test(&device->suspend_cnt))
+		wake_up(&device->misc_wait);
 }
 
 /**
@@ -871,27 +891,32 @@ void drbd_resume_io(struct drbd_device *device)
 enum determine_dev_size
 drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct resize_parms *rs) __must_hold(local)
 {
-	sector_t prev_first_sect, prev_size; /* previous meta location */
-	sector_t la_size_sect, u_size;
+	struct md_offsets_and_sizes {
+		u64 last_agreed_sect;
+		u64 md_offset;
+		s32 al_offset;
+		s32 bm_offset;
+		u32 md_size_sect;
+
+		u32 al_stripes;
+		u32 al_stripe_size_4k;
+	} prev;
+	sector_t u_size, size;
 	struct drbd_md *md = &device->ldev->md;
-	u32 prev_al_stripe_size_4k;
-	u32 prev_al_stripes;
-	sector_t size;
 	char ppb[10];
 	void *buffer;
 
 	int md_moved, la_size_changed;
 	enum determine_dev_size rv = DS_UNCHANGED;
 
-	/* race:
-	 * application request passes inc_ap_bio,
-	 * but then cannot get an AL-reference.
-	 * this function later may wait on ap_bio_cnt == 0. -> deadlock.
+	/* We may change the on-disk offsets of our meta data below.  Lock out
+	 * anything that may cause meta data IO, to avoid acting on incomplete
+	 * layout changes or scribbling over meta data that is in the process
+	 * of being moved.
 	 *
-	 * to avoid that:
-	 * Suspend IO right here.
-	 * still lock the act_log to not trigger ASSERTs there.
-	 */
+	 * Move is not exactly correct, btw, currently we have all our meta
+	 * data in core memory, to "move" it we just write it all out, there
+	 * are no reads. */
 	drbd_suspend_io(device);
 	buffer = drbd_md_get_buffer(device, __func__); /* Lock meta-data IO */
 	if (!buffer) {
@@ -899,19 +924,17 @@ drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct
 		return DS_ERROR;
 	}
 
-	/* no wait necessary anymore, actually we could assert that */
-	wait_event(device->al_wait, lc_try_lock(device->act_log));
-
-	prev_first_sect = drbd_md_first_sector(device->ldev);
-	prev_size = device->ldev->md.md_size_sect;
-	la_size_sect = device->ldev->md.la_size_sect;
+	/* remember current offset and sizes */
+	prev.last_agreed_sect = md->la_size_sect;
+	prev.md_offset = md->md_offset;
+	prev.al_offset = md->al_offset;
+	prev.bm_offset = md->bm_offset;
+	prev.md_size_sect = md->md_size_sect;
+	prev.al_stripes = md->al_stripes;
+	prev.al_stripe_size_4k = md->al_stripe_size_4k;
 
 	if (rs) {
 		/* rs is non NULL if we should change the AL layout only */
-
-		prev_al_stripes = md->al_stripes;
-		prev_al_stripe_size_4k = md->al_stripe_size_4k;
-
 		md->al_stripes = rs->al_stripes;
 		md->al_stripe_size_4k = rs->al_stripe_size / 4;
 		md->al_size_4k = (u64)rs->al_stripes * rs->al_stripe_size / 4;
@@ -924,7 +947,7 @@ drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct
 	rcu_read_unlock();
 	size = drbd_new_dev_size(device, device->ldev, u_size, flags & DDSF_FORCED);
 
-	if (size < la_size_sect) {
+	if (size < prev.last_agreed_sect) {
 		if (rs && u_size == 0) {
 			/* Remove "rs &&" later. This check should always be active, but
 			   right now the receiver expects the permissive behavior */
@@ -945,30 +968,29 @@ drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct
 		err = drbd_bm_resize(device, size, !(flags & DDSF_NO_RESYNC));
 		if (unlikely(err)) {
 			/* currently there is only one error: ENOMEM! */
-			size = drbd_bm_capacity(device)>>1;
+			size = drbd_bm_capacity(device);
 			if (size == 0) {
 				drbd_err(device, "OUT OF MEMORY! "
 				    "Could not allocate bitmap!\n");
 			} else {
 				drbd_err(device, "BM resizing failed. "
-				    "Leaving size unchanged at size = %lu KB\n",
-				    (unsigned long)size);
+				    "Leaving size unchanged\n");
 			}
 			rv = DS_ERROR;
 		}
 		/* racy, see comments above. */
 		drbd_set_my_capacity(device, size);
-		device->ldev->md.la_size_sect = size;
+		md->la_size_sect = size;
 		drbd_info(device, "size = %s (%llu KB)\n", ppsize(ppb, size>>1),
 		     (unsigned long long)size>>1);
 	}
 	if (rv <= DS_ERROR)
 		goto err_out;
 
-	la_size_changed = (la_size_sect != device->ldev->md.la_size_sect);
+	la_size_changed = (prev.last_agreed_sect != md->la_size_sect);
 
-	md_moved = prev_first_sect != drbd_md_first_sector(device->ldev)
-		|| prev_size	   != device->ldev->md.md_size_sect;
+	md_moved = prev.md_offset    != md->md_offset
+		|| prev.md_size_sect != md->md_size_sect;
 
 	if (la_size_changed || md_moved || rs) {
 		u32 prev_flags;
@@ -977,20 +999,29 @@ drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct
 		 * Clear the timer, to avoid scary "timer expired!" messages,
 		 * "Superblock" is written out at least twice below, anyways. */
 		del_timer(&device->md_sync_timer);
-		drbd_al_shrink(device); /* All extents inactive. */
 
+		/* We won't change the "al-extents" setting, we just may need
+		 * to move the on-disk location of the activity log ringbuffer.
+		 * Lock for transaction is good enough, it may well be "dirty"
+		 * or even "starving". */
+		wait_event(device->al_wait, lc_try_lock_for_transaction(device->act_log));
+
+		/* mark current on-disk bitmap and activity log as unreliable */
 		prev_flags = md->flags;
-		md->flags &= ~MDF_PRIMARY_IND;
+		md->flags |= MDF_FULL_SYNC | MDF_AL_DISABLED;
 		drbd_md_write(device, buffer);
 
+		drbd_al_initialize(device, buffer);
+
 		drbd_info(device, "Writing the whole bitmap, %s\n",
 			 la_size_changed && md_moved ? "size changed and md moved" :
 			 la_size_changed ? "size changed" : "md moved");
 		/* next line implicitly does drbd_suspend_io()+drbd_resume_io() */
 		drbd_bitmap_io(device, md_moved ? &drbd_bm_write_all : &drbd_bm_write,
 			       "size changed", BM_LOCKED_MASK);
-		drbd_initialize_al(device, buffer);
 
+		/* on-disk bitmap and activity log is authoritative again
+		 * (unless there was an IO error meanwhile...) */
 		md->flags = prev_flags;
 		drbd_md_write(device, buffer);
 
@@ -999,20 +1030,22 @@ drbd_determine_dev_size(struct drbd_device *device, enum dds_flags flags, struct
 				  md->al_stripes, md->al_stripe_size_4k * 4);
 	}
 
-	if (size > la_size_sect)
-		rv = la_size_sect ? DS_GREW : DS_GREW_FROM_ZERO;
-	if (size < la_size_sect)
+	if (size > prev.last_agreed_sect)
+		rv = prev.last_agreed_sect ? DS_GREW : DS_GREW_FROM_ZERO;
+	if (size < prev.last_agreed_sect)
 		rv = DS_SHRUNK;
 
 	if (0) {
 	err_out:
-		if (rs) {
-			md->al_stripes = prev_al_stripes;
-			md->al_stripe_size_4k = prev_al_stripe_size_4k;
-			md->al_size_4k = (u64)prev_al_stripes * prev_al_stripe_size_4k;
-
-			drbd_md_set_sector_offsets(device, device->ldev);
-		}
+		/* restore previous offset and sizes */
+		md->la_size_sect = prev.last_agreed_sect;
+		md->md_offset = prev.md_offset;
+		md->al_offset = prev.al_offset;
+		md->bm_offset = prev.bm_offset;
+		md->md_size_sect = prev.md_size_sect;
+		md->al_stripes = prev.al_stripes;
+		md->al_stripe_size_4k = prev.al_stripe_size_4k;
+		md->al_size_4k = (u64)prev.al_stripes * prev.al_stripe_size_4k;
 	}
 	lc_unlock(device->act_log);
 	wake_up(&device->al_wait);
@@ -1115,8 +1148,7 @@ static int drbd_check_al_size(struct drbd_device *device, struct disk_conf *dc)
 		lc_destroy(n);
 		return -EBUSY;
 	} else {
-		if (t)
-			lc_destroy(t);
+		lc_destroy(t);
 	}
 	drbd_md_mark_dirty(device); /* we changed device->act_log->nr_elemens */
 	return 0;
@@ -1151,21 +1183,20 @@ static void drbd_setup_queue_param(struct drbd_device *device, struct drbd_backi
 	if (b) {
 		struct drbd_connection *connection = first_peer_device(device)->connection;
 
+		blk_queue_max_discard_sectors(q, DRBD_MAX_DISCARD_SECTORS);
+
 		if (blk_queue_discard(b) &&
 		    (connection->cstate < C_CONNECTED || connection->agreed_features & FF_TRIM)) {
-			/* For now, don't allow more than one activity log extent worth of data
-			 * to be discarded in one go. We may need to rework drbd_al_begin_io()
-			 * to allow for even larger discard ranges */
-			blk_queue_max_discard_sectors(q, DRBD_MAX_DISCARD_SECTORS);
-
+			/* We don't care, stacking below should fix it for the local device.
+			 * Whether or not it is a suitable granularity on the remote device
+			 * is not our problem, really. If you care, you need to
+			 * use devices with similar topology on all peers. */
+			q->limits.discard_granularity = 512;
 			queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
-			/* REALLY? Is stacking secdiscard "legal"? */
-			if (blk_queue_secdiscard(b))
-				queue_flag_set_unlocked(QUEUE_FLAG_SECDISCARD, q);
 		} else {
 			blk_queue_max_discard_sectors(q, 0);
 			queue_flag_clear_unlocked(QUEUE_FLAG_DISCARD, q);
-			queue_flag_clear_unlocked(QUEUE_FLAG_SECDISCARD, q);
+			q->limits.discard_granularity = 0;
 		}
 
 		blk_queue_stack_limits(q, b);
@@ -1177,6 +1208,12 @@ static void drbd_setup_queue_param(struct drbd_device *device, struct drbd_backi
 			q->backing_dev_info.ra_pages = b->backing_dev_info.ra_pages;
 		}
 	}
+	/* To avoid confusion, if this queue does not support discard, clear
+	 * max_discard_sectors, which is what lsblk -D reports to the user.  */
+	if (!blk_queue_discard(q)) {
+		blk_queue_max_discard_sectors(q, 0);
+		q->limits.discard_granularity = 0;
+	}
 }
 
 void drbd_reconsider_max_bio_size(struct drbd_device *device, struct drbd_backing_dev *bdev)
@@ -1241,8 +1278,8 @@ static void conn_reconfig_done(struct drbd_connection *connection)
 		connection->cstate == C_STANDALONE;
 	spin_unlock_irq(&connection->resource->req_lock);
 	if (stop_threads) {
-		/* asender is implicitly stopped by receiver
-		 * in conn_disconnect() */
+		/* ack_receiver thread and ack_sender workqueue are implicitly
+		 * stopped by receiver in conn_disconnect() */
 		drbd_thread_stop(&connection->receiver);
 		drbd_thread_stop(&connection->worker);
 	}
@@ -1389,13 +1426,13 @@ int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 		goto fail_unlock;
 	}
 
-	write_lock_irq(&global_state_lock);
+	lock_all_resources();
 	retcode = drbd_resync_after_valid(device, new_disk_conf->resync_after);
 	if (retcode == NO_ERROR) {
 		rcu_assign_pointer(device->ldev->disk_conf, new_disk_conf);
 		drbd_resync_after_changed(device);
 	}
-	write_unlock_irq(&global_state_lock);
+	unlock_all_resources();
 
 	if (retcode != NO_ERROR)
 		goto fail_unlock;
@@ -1418,7 +1455,7 @@ int drbd_adm_disk_opts(struct sk_buff *skb, struct genl_info *info)
 		set_bit(MD_NO_FUA, &device->flags);
 
 	if (write_ordering_changed(old_disk_conf, new_disk_conf))
-		drbd_bump_write_ordering(device->resource, NULL, WO_bdev_flush);
+		drbd_bump_write_ordering(device->resource, NULL, WO_BDEV_FLUSH);
 
 	drbd_md_sync(device);
 
@@ -1449,6 +1486,88 @@ success:
 	return 0;
 }
 
+static struct block_device *open_backing_dev(struct drbd_device *device,
+		const char *bdev_path, void *claim_ptr, bool do_bd_link)
+{
+	struct block_device *bdev;
+	int err = 0;
+
+	bdev = blkdev_get_by_path(bdev_path,
+				  FMODE_READ | FMODE_WRITE | FMODE_EXCL, claim_ptr);
+	if (IS_ERR(bdev)) {
+		drbd_err(device, "open(\"%s\") failed with %ld\n",
+				bdev_path, PTR_ERR(bdev));
+		return bdev;
+	}
+
+	if (!do_bd_link)
+		return bdev;
+
+	err = bd_link_disk_holder(bdev, device->vdisk);
+	if (err) {
+		blkdev_put(bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
+		drbd_err(device, "bd_link_disk_holder(\"%s\", ...) failed with %d\n",
+				bdev_path, err);
+		bdev = ERR_PTR(err);
+	}
+	return bdev;
+}
+
+static int open_backing_devices(struct drbd_device *device,
+		struct disk_conf *new_disk_conf,
+		struct drbd_backing_dev *nbc)
+{
+	struct block_device *bdev;
+
+	bdev = open_backing_dev(device, new_disk_conf->backing_dev, device, true);
+	if (IS_ERR(bdev))
+		return ERR_OPEN_DISK;
+	nbc->backing_bdev = bdev;
+
+	/*
+	 * meta_dev_idx >= 0: external fixed size, possibly multiple
+	 * drbd sharing one meta device.  TODO in that case, paranoia
+	 * check that [md_bdev, meta_dev_idx] is not yet used by some
+	 * other drbd minor!  (if you use drbd.conf + drbdadm, that
+	 * should check it for you already; but if you don't, or
+	 * someone fooled it, we need to double check here)
+	 */
+	bdev = open_backing_dev(device, new_disk_conf->meta_dev,
+		/* claim ptr: device, if claimed exclusively; shared drbd_m_holder,
+		 * if potentially shared with other drbd minors */
+			(new_disk_conf->meta_dev_idx < 0) ? (void*)device : (void*)drbd_m_holder,
+		/* avoid double bd_claim_by_disk() for the same (source,target) tuple,
+		 * as would happen with internal metadata. */
+			(new_disk_conf->meta_dev_idx != DRBD_MD_INDEX_FLEX_INT &&
+			 new_disk_conf->meta_dev_idx != DRBD_MD_INDEX_INTERNAL));
+	if (IS_ERR(bdev))
+		return ERR_OPEN_MD_DISK;
+	nbc->md_bdev = bdev;
+	return NO_ERROR;
+}
+
+static void close_backing_dev(struct drbd_device *device, struct block_device *bdev,
+	bool do_bd_unlink)
+{
+	if (!bdev)
+		return;
+	if (do_bd_unlink)
+		bd_unlink_disk_holder(bdev, device->vdisk);
+	blkdev_put(bdev, FMODE_READ | FMODE_WRITE | FMODE_EXCL);
+}
+
+void drbd_backing_dev_free(struct drbd_device *device, struct drbd_backing_dev *ldev)
+{
+	if (ldev == NULL)
+		return;
+
+	close_backing_dev(device, ldev->md_bdev, ldev->md_bdev != ldev->backing_bdev);
+	close_backing_dev(device, ldev->backing_bdev, true);
+
+	kfree(ldev->disk_conf);
+	kfree(ldev);
+}
+
 int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 {
 	struct drbd_config_context adm_ctx;
@@ -1462,7 +1581,6 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	sector_t min_md_device_sectors;
 	struct drbd_backing_dev *nbc = NULL; /* new_backing_conf */
 	struct disk_conf *new_disk_conf = NULL;
-	struct block_device *bdev;
 	struct lru_cache *resync_lru = NULL;
 	struct fifo_buffer *new_plan = NULL;
 	union drbd_state ns, os;
@@ -1478,7 +1596,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	device = adm_ctx.device;
 	mutex_lock(&adm_ctx.resource->adm_mutex);
 	peer_device = first_peer_device(device);
-	connection = peer_device ? peer_device->connection : NULL;
+	connection = peer_device->connection;
 	conn_reconfig_start(connection);
 
 	/* if you want to reconfigure, please tear down first */
@@ -1539,12 +1657,6 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		goto fail;
 	}
 
-	write_lock_irq(&global_state_lock);
-	retcode = drbd_resync_after_valid(device, new_disk_conf->resync_after);
-	write_unlock_irq(&global_state_lock);
-	if (retcode != NO_ERROR)
-		goto fail;
-
 	rcu_read_lock();
 	nc = rcu_dereference(connection->net_conf);
 	if (nc) {
@@ -1556,35 +1668,9 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	}
 	rcu_read_unlock();
 
-	bdev = blkdev_get_by_path(new_disk_conf->backing_dev,
-				  FMODE_READ | FMODE_WRITE | FMODE_EXCL, device);
-	if (IS_ERR(bdev)) {
-		drbd_err(device, "open(\"%s\") failed with %ld\n", new_disk_conf->backing_dev,
-			PTR_ERR(bdev));
-		retcode = ERR_OPEN_DISK;
-		goto fail;
-	}
-	nbc->backing_bdev = bdev;
-
-	/*
-	 * meta_dev_idx >= 0: external fixed size, possibly multiple
-	 * drbd sharing one meta device.  TODO in that case, paranoia
-	 * check that [md_bdev, meta_dev_idx] is not yet used by some
-	 * other drbd minor!  (if you use drbd.conf + drbdadm, that
-	 * should check it for you already; but if you don't, or
-	 * someone fooled it, we need to double check here)
-	 */
-	bdev = blkdev_get_by_path(new_disk_conf->meta_dev,
-				  FMODE_READ | FMODE_WRITE | FMODE_EXCL,
-				  (new_disk_conf->meta_dev_idx < 0) ?
-				  (void *)device : (void *)drbd_m_holder);
-	if (IS_ERR(bdev)) {
-		drbd_err(device, "open(\"%s\") failed with %ld\n", new_disk_conf->meta_dev,
-			PTR_ERR(bdev));
-		retcode = ERR_OPEN_MD_DISK;
+	retcode = open_backing_devices(device, new_disk_conf, nbc);
+	if (retcode != NO_ERROR)
 		goto fail;
-	}
-	nbc->md_bdev = bdev;
 
 	if ((nbc->backing_bdev == nbc->md_bdev) !=
 	    (new_disk_conf->meta_dev_idx == DRBD_MD_INDEX_INTERNAL ||
@@ -1707,6 +1793,13 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 		goto force_diskless_dec;
 	}
 
+	lock_all_resources();
+	retcode = drbd_resync_after_valid(device, new_disk_conf->resync_after);
+	if (retcode != NO_ERROR) {
+		unlock_all_resources();
+		goto force_diskless_dec;
+	}
+
 	/* Reset the "barriers don't work" bits here, then force meta data to
 	 * be written, to ensure we determine if barriers are supported. */
 	if (new_disk_conf->md_flushes)
@@ -1727,7 +1820,9 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 	new_disk_conf = NULL;
 	new_plan = NULL;
 
-	drbd_bump_write_ordering(device->resource, device->ldev, WO_bdev_flush);
+	drbd_resync_after_changed(device);
+	drbd_bump_write_ordering(device->resource, device->ldev, WO_BDEV_FLUSH);
+	unlock_all_resources();
 
 	if (drbd_md_test_flag(device->ldev, MDF_CRASHED_PRIMARY))
 		set_bit(CRASHED_PRIMARY, &device->flags);
@@ -1875,12 +1970,8 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
  fail:
 	conn_reconfig_done(connection);
 	if (nbc) {
-		if (nbc->backing_bdev)
-			blkdev_put(nbc->backing_bdev,
-				   FMODE_READ | FMODE_WRITE | FMODE_EXCL);
-		if (nbc->md_bdev)
-			blkdev_put(nbc->md_bdev,
-				   FMODE_READ | FMODE_WRITE | FMODE_EXCL);
+		close_backing_dev(device, nbc->md_bdev, nbc->md_bdev != nbc->backing_bdev);
+		close_backing_dev(device, nbc->backing_bdev, true);
 		kfree(nbc);
 	}
 	kfree(new_disk_conf);
@@ -1895,6 +1986,7 @@ int drbd_adm_attach(struct sk_buff *skb, struct genl_info *info)
 static int adm_detach(struct drbd_device *device, int force)
 {
 	enum drbd_state_rv retcode;
+	void *buffer;
 	int ret;
 
 	if (force) {
@@ -1905,13 +1997,16 @@ static int adm_detach(struct drbd_device *device, int force)
 	}
 
 	drbd_suspend_io(device); /* so no-one is stuck in drbd_al_begin_io */
-	drbd_md_get_buffer(device, __func__); /* make sure there is no in-flight meta-data IO */
-	retcode = drbd_request_state(device, NS(disk, D_FAILED));
-	drbd_md_put_buffer(device);
+	buffer = drbd_md_get_buffer(device, __func__); /* make sure there is no in-flight meta-data IO */
+	if (buffer) {
+		retcode = drbd_request_state(device, NS(disk, D_FAILED));
+		drbd_md_put_buffer(device);
+	} else /* already <= D_FAILED */
+		retcode = SS_NOTHING_TO_DO;
 	/* D_FAILED will transition to DISKLESS. */
+	drbd_resume_io(device);
 	ret = wait_event_interruptible(device->misc_wait,
 			device->state.disk != D_FAILED);
-	drbd_resume_io(device);
 	if ((int)retcode == (int)SS_IS_DISKLESS)
 		retcode = SS_NOTHING_TO_DO;
 	if (ret)
@@ -2245,8 +2340,31 @@ int drbd_adm_net_opts(struct sk_buff *skb, struct genl_info *info)
 	return 0;
 }
 
+static void connection_to_info(struct connection_info *info,
+			       struct drbd_connection *connection)
+{
+	info->conn_connection_state = connection->cstate;
+	info->conn_role = conn_highest_peer(connection);
+}
+
+static void peer_device_to_info(struct peer_device_info *info,
+				struct drbd_peer_device *peer_device)
+{
+	struct drbd_device *device = peer_device->device;
+
+	info->peer_repl_state =
+		max_t(enum drbd_conns, C_WF_REPORT_PARAMS, device->state.conn);
+	info->peer_disk_state = device->state.pdsk;
+	info->peer_resync_susp_user = device->state.user_isp;
+	info->peer_resync_susp_peer = device->state.peer_isp;
+	info->peer_resync_susp_dependency = device->state.aftr_isp;
+}
+
 int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 {
+	struct connection_info connection_info;
+	enum drbd_notification_type flags;
+	unsigned int peer_devices = 0;
 	struct drbd_config_context adm_ctx;
 	struct drbd_peer_device *peer_device;
 	struct net_conf *old_net_conf, *new_net_conf = NULL;
@@ -2347,6 +2465,22 @@ int drbd_adm_connect(struct sk_buff *skb, struct genl_info *info)
 	connection->peer_addr_len = nla_len(adm_ctx.peer_addr);
 	memcpy(&connection->peer_addr, nla_data(adm_ctx.peer_addr), connection->peer_addr_len);
 
+	idr_for_each_entry(&connection->peer_devices, peer_device, i) {
+		peer_devices++;
+	}
+
+	connection_to_info(&connection_info, connection);
+	flags = (peer_devices--) ? NOTIFY_CONTINUES : 0;
+	mutex_lock(&notification_mutex);
+	notify_connection_state(NULL, 0, connection, &connection_info, NOTIFY_CREATE | flags);
+	idr_for_each_entry(&connection->peer_devices, peer_device, i) {
+		struct peer_device_info peer_device_info;
+
+		peer_device_to_info(&peer_device_info, peer_device);
+		flags = (peer_devices--) ? NOTIFY_CONTINUES : 0;
+		notify_peer_device_state(NULL, 0, peer_device, &peer_device_info, NOTIFY_CREATE | flags);
+	}
+	mutex_unlock(&notification_mutex);
 	mutex_unlock(&adm_ctx.resource->conf_update);
 
 	rcu_read_lock();
@@ -2428,6 +2562,8 @@ static enum drbd_state_rv conn_try_disconnect(struct drbd_connection *connection
 			drbd_err(connection,
 				"unexpected rv2=%d in conn_try_disconnect()\n",
 				rv2);
+		/* Unlike in DRBD 9, the state engine has generated
+		 * NOTIFY_DESTROY events before clearing connection->net_conf. */
 	}
 	return rv;
 }
@@ -2585,6 +2721,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 		mutex_unlock(&device->resource->conf_update);
 		synchronize_rcu();
 		kfree(old_disk_conf);
+		new_disk_conf = NULL;
 	}
 
 	ddsf = (rs.resize_force ? DDSF_FORCED : 0) | (rs.no_resync ? DDSF_NO_RESYNC : 0);
@@ -2618,6 +2755,7 @@ int drbd_adm_resize(struct sk_buff *skb, struct genl_info *info)
 
  fail_ldev:
 	put_ldev(device);
+	kfree(new_disk_conf);
 	goto fail;
 }
 
@@ -2855,7 +2993,30 @@ int drbd_adm_resume_io(struct sk_buff *skb, struct genl_info *info)
 	mutex_lock(&adm_ctx.resource->adm_mutex);
 	device = adm_ctx.device;
 	if (test_bit(NEW_CUR_UUID, &device->flags)) {
-		drbd_uuid_new_current(device);
+		if (get_ldev_if_state(device, D_ATTACHING)) {
+			drbd_uuid_new_current(device);
+			put_ldev(device);
+		} else {
+			/* This is effectively a multi-stage "forced down".
+			 * The NEW_CUR_UUID bit is supposedly only set, if we
+			 * lost the replication connection, and are configured
+			 * to freeze IO and wait for some fence-peer handler.
+			 * So we still don't have a replication connection.
+			 * And now we don't have a local disk either.  After
+			 * resume, we will fail all pending and new IO, because
+			 * we don't have any data anymore.  Which means we will
+			 * eventually be able to terminate all users of this
+			 * device, and then take it down.  By bumping the
+			 * "effective" data uuid, we make sure that you really
+			 * need to tear down before you reconfigure, we will
+			 * the refuse to re-connect or re-attach (because no
+			 * matching real data uuid exists).
+			 */
+			u64 val;
+			get_random_bytes(&val, sizeof(u64));
+			drbd_set_ed_uuid(device, val);
+			drbd_warn(device, "Resumed without access to data; please tear down before attempting to re-configure.\n");
+		}
 		clear_bit(NEW_CUR_UUID, &device->flags);
 	}
 	drbd_suspend_io(device);
@@ -2910,6 +3071,486 @@ nla_put_failure:
 }
 
 /*
+ * The generic netlink dump callbacks are called outside the genl_lock(), so
+ * they cannot use the simple attribute parsing code which uses global
+ * attribute tables.
+ */
+static struct nlattr *find_cfg_context_attr(const struct nlmsghdr *nlh, int attr)
+{
+	const unsigned hdrlen = GENL_HDRLEN + GENL_MAGIC_FAMILY_HDRSZ;
+	const int maxtype = ARRAY_SIZE(drbd_cfg_context_nl_policy) - 1;
+	struct nlattr *nla;
+
+	nla = nla_find(nlmsg_attrdata(nlh, hdrlen), nlmsg_attrlen(nlh, hdrlen),
+		       DRBD_NLA_CFG_CONTEXT);
+	if (!nla)
+		return NULL;
+	return drbd_nla_find_nested(maxtype, nla, __nla_type(attr));
+}
+
+static void resource_to_info(struct resource_info *, struct drbd_resource *);
+
+int drbd_adm_dump_resources(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct drbd_genlmsghdr *dh;
+	struct drbd_resource *resource;
+	struct resource_info resource_info;
+	struct resource_statistics resource_statistics;
+	int err;
+
+	rcu_read_lock();
+	if (cb->args[0]) {
+		for_each_resource_rcu(resource, &drbd_resources)
+			if (resource == (struct drbd_resource *)cb->args[0])
+				goto found_resource;
+		err = 0;  /* resource was probably deleted */
+		goto out;
+	}
+	resource = list_entry(&drbd_resources,
+			      struct drbd_resource, resources);
+
+found_resource:
+	list_for_each_entry_continue_rcu(resource, &drbd_resources, resources) {
+		goto put_result;
+	}
+	err = 0;
+	goto out;
+
+put_result:
+	dh = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
+			cb->nlh->nlmsg_seq, &drbd_genl_family,
+			NLM_F_MULTI, DRBD_ADM_GET_RESOURCES);
+	err = -ENOMEM;
+	if (!dh)
+		goto out;
+	dh->minor = -1U;
+	dh->ret_code = NO_ERROR;
+	err = nla_put_drbd_cfg_context(skb, resource, NULL, NULL);
+	if (err)
+		goto out;
+	err = res_opts_to_skb(skb, &resource->res_opts, !capable(CAP_SYS_ADMIN));
+	if (err)
+		goto out;
+	resource_to_info(&resource_info, resource);
+	err = resource_info_to_skb(skb, &resource_info, !capable(CAP_SYS_ADMIN));
+	if (err)
+		goto out;
+	resource_statistics.res_stat_write_ordering = resource->write_ordering;
+	err = resource_statistics_to_skb(skb, &resource_statistics, !capable(CAP_SYS_ADMIN));
+	if (err)
+		goto out;
+	cb->args[0] = (long)resource;
+	genlmsg_end(skb, dh);
+	err = 0;
+
+out:
+	rcu_read_unlock();
+	if (err)
+		return err;
+	return skb->len;
+}
+
+static void device_to_statistics(struct device_statistics *s,
+				 struct drbd_device *device)
+{
+	memset(s, 0, sizeof(*s));
+	s->dev_upper_blocked = !may_inc_ap_bio(device);
+	if (get_ldev(device)) {
+		struct drbd_md *md = &device->ldev->md;
+		u64 *history_uuids = (u64 *)s->history_uuids;
+		struct request_queue *q;
+		int n;
+
+		spin_lock_irq(&md->uuid_lock);
+		s->dev_current_uuid = md->uuid[UI_CURRENT];
+		BUILD_BUG_ON(sizeof(s->history_uuids) < UI_HISTORY_END - UI_HISTORY_START + 1);
+		for (n = 0; n < UI_HISTORY_END - UI_HISTORY_START + 1; n++)
+			history_uuids[n] = md->uuid[UI_HISTORY_START + n];
+		for (; n < HISTORY_UUIDS; n++)
+			history_uuids[n] = 0;
+		s->history_uuids_len = HISTORY_UUIDS;
+		spin_unlock_irq(&md->uuid_lock);
+
+		s->dev_disk_flags = md->flags;
+		q = bdev_get_queue(device->ldev->backing_bdev);
+		s->dev_lower_blocked =
+			bdi_congested(&q->backing_dev_info,
+				      (1 << WB_async_congested) |
+				      (1 << WB_sync_congested));
+		put_ldev(device);
+	}
+	s->dev_size = drbd_get_capacity(device->this_bdev);
+	s->dev_read = device->read_cnt;
+	s->dev_write = device->writ_cnt;
+	s->dev_al_writes = device->al_writ_cnt;
+	s->dev_bm_writes = device->bm_writ_cnt;
+	s->dev_upper_pending = atomic_read(&device->ap_bio_cnt);
+	s->dev_lower_pending = atomic_read(&device->local_cnt);
+	s->dev_al_suspended = test_bit(AL_SUSPENDED, &device->flags);
+	s->dev_exposed_data_uuid = device->ed_uuid;
+}
+
+static int put_resource_in_arg0(struct netlink_callback *cb, int holder_nr)
+{
+	if (cb->args[0]) {
+		struct drbd_resource *resource =
+			(struct drbd_resource *)cb->args[0];
+		kref_put(&resource->kref, drbd_destroy_resource);
+	}
+
+	return 0;
+}
+
+int drbd_adm_dump_devices_done(struct netlink_callback *cb) {
+	return put_resource_in_arg0(cb, 7);
+}
+
+static void device_to_info(struct device_info *, struct drbd_device *);
+
+int drbd_adm_dump_devices(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct nlattr *resource_filter;
+	struct drbd_resource *resource;
+	struct drbd_device *uninitialized_var(device);
+	int minor, err, retcode;
+	struct drbd_genlmsghdr *dh;
+	struct device_info device_info;
+	struct device_statistics device_statistics;
+	struct idr *idr_to_search;
+
+	resource = (struct drbd_resource *)cb->args[0];
+	if (!cb->args[0] && !cb->args[1]) {
+		resource_filter = find_cfg_context_attr(cb->nlh, T_ctx_resource_name);
+		if (resource_filter) {
+			retcode = ERR_RES_NOT_KNOWN;
+			resource = drbd_find_resource(nla_data(resource_filter));
+			if (!resource)
+				goto put_result;
+			cb->args[0] = (long)resource;
+		}
+	}
+
+	rcu_read_lock();
+	minor = cb->args[1];
+	idr_to_search = resource ? &resource->devices : &drbd_devices;
+	device = idr_get_next(idr_to_search, &minor);
+	if (!device) {
+		err = 0;
+		goto out;
+	}
+	idr_for_each_entry_continue(idr_to_search, device, minor) {
+		retcode = NO_ERROR;
+		goto put_result;  /* only one iteration */
+	}
+	err = 0;
+	goto out;  /* no more devices */
+
+put_result:
+	dh = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
+			cb->nlh->nlmsg_seq, &drbd_genl_family,
+			NLM_F_MULTI, DRBD_ADM_GET_DEVICES);
+	err = -ENOMEM;
+	if (!dh)
+		goto out;
+	dh->ret_code = retcode;
+	dh->minor = -1U;
+	if (retcode == NO_ERROR) {
+		dh->minor = device->minor;
+		err = nla_put_drbd_cfg_context(skb, device->resource, NULL, device);
+		if (err)
+			goto out;
+		if (get_ldev(device)) {
+			struct disk_conf *disk_conf =
+				rcu_dereference(device->ldev->disk_conf);
+
+			err = disk_conf_to_skb(skb, disk_conf, !capable(CAP_SYS_ADMIN));
+			put_ldev(device);
+			if (err)
+				goto out;
+		}
+		device_to_info(&device_info, device);
+		err = device_info_to_skb(skb, &device_info, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+
+		device_to_statistics(&device_statistics, device);
+		err = device_statistics_to_skb(skb, &device_statistics, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+		cb->args[1] = minor + 1;
+	}
+	genlmsg_end(skb, dh);
+	err = 0;
+
+out:
+	rcu_read_unlock();
+	if (err)
+		return err;
+	return skb->len;
+}
+
+int drbd_adm_dump_connections_done(struct netlink_callback *cb)
+{
+	return put_resource_in_arg0(cb, 6);
+}
+
+enum { SINGLE_RESOURCE, ITERATE_RESOURCES };
+
+int drbd_adm_dump_connections(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct nlattr *resource_filter;
+	struct drbd_resource *resource = NULL, *next_resource;
+	struct drbd_connection *uninitialized_var(connection);
+	int err = 0, retcode;
+	struct drbd_genlmsghdr *dh;
+	struct connection_info connection_info;
+	struct connection_statistics connection_statistics;
+
+	rcu_read_lock();
+	resource = (struct drbd_resource *)cb->args[0];
+	if (!cb->args[0]) {
+		resource_filter = find_cfg_context_attr(cb->nlh, T_ctx_resource_name);
+		if (resource_filter) {
+			retcode = ERR_RES_NOT_KNOWN;
+			resource = drbd_find_resource(nla_data(resource_filter));
+			if (!resource)
+				goto put_result;
+			cb->args[0] = (long)resource;
+			cb->args[1] = SINGLE_RESOURCE;
+		}
+	}
+	if (!resource) {
+		if (list_empty(&drbd_resources))
+			goto out;
+		resource = list_first_entry(&drbd_resources, struct drbd_resource, resources);
+		kref_get(&resource->kref);
+		cb->args[0] = (long)resource;
+		cb->args[1] = ITERATE_RESOURCES;
+	}
+
+    next_resource:
+	rcu_read_unlock();
+	mutex_lock(&resource->conf_update);
+	rcu_read_lock();
+	if (cb->args[2]) {
+		for_each_connection_rcu(connection, resource)
+			if (connection == (struct drbd_connection *)cb->args[2])
+				goto found_connection;
+		/* connection was probably deleted */
+		goto no_more_connections;
+	}
+	connection = list_entry(&resource->connections, struct drbd_connection, connections);
+
+found_connection:
+	list_for_each_entry_continue_rcu(connection, &resource->connections, connections) {
+		if (!has_net_conf(connection))
+			continue;
+		retcode = NO_ERROR;
+		goto put_result;  /* only one iteration */
+	}
+
+no_more_connections:
+	if (cb->args[1] == ITERATE_RESOURCES) {
+		for_each_resource_rcu(next_resource, &drbd_resources) {
+			if (next_resource == resource)
+				goto found_resource;
+		}
+		/* resource was probably deleted */
+	}
+	goto out;
+
+found_resource:
+	list_for_each_entry_continue_rcu(next_resource, &drbd_resources, resources) {
+		mutex_unlock(&resource->conf_update);
+		kref_put(&resource->kref, drbd_destroy_resource);
+		resource = next_resource;
+		kref_get(&resource->kref);
+		cb->args[0] = (long)resource;
+		cb->args[2] = 0;
+		goto next_resource;
+	}
+	goto out;  /* no more resources */
+
+put_result:
+	dh = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
+			cb->nlh->nlmsg_seq, &drbd_genl_family,
+			NLM_F_MULTI, DRBD_ADM_GET_CONNECTIONS);
+	err = -ENOMEM;
+	if (!dh)
+		goto out;
+	dh->ret_code = retcode;
+	dh->minor = -1U;
+	if (retcode == NO_ERROR) {
+		struct net_conf *net_conf;
+
+		err = nla_put_drbd_cfg_context(skb, resource, connection, NULL);
+		if (err)
+			goto out;
+		net_conf = rcu_dereference(connection->net_conf);
+		if (net_conf) {
+			err = net_conf_to_skb(skb, net_conf, !capable(CAP_SYS_ADMIN));
+			if (err)
+				goto out;
+		}
+		connection_to_info(&connection_info, connection);
+		err = connection_info_to_skb(skb, &connection_info, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+		connection_statistics.conn_congested = test_bit(NET_CONGESTED, &connection->flags);
+		err = connection_statistics_to_skb(skb, &connection_statistics, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+		cb->args[2] = (long)connection;
+	}
+	genlmsg_end(skb, dh);
+	err = 0;
+
+out:
+	rcu_read_unlock();
+	if (resource)
+		mutex_unlock(&resource->conf_update);
+	if (err)
+		return err;
+	return skb->len;
+}
+
+enum mdf_peer_flag {
+	MDF_PEER_CONNECTED =	1 << 0,
+	MDF_PEER_OUTDATED =	1 << 1,
+	MDF_PEER_FENCING =	1 << 2,
+	MDF_PEER_FULL_SYNC =	1 << 3,
+};
+
+static void peer_device_to_statistics(struct peer_device_statistics *s,
+				      struct drbd_peer_device *peer_device)
+{
+	struct drbd_device *device = peer_device->device;
+
+	memset(s, 0, sizeof(*s));
+	s->peer_dev_received = device->recv_cnt;
+	s->peer_dev_sent = device->send_cnt;
+	s->peer_dev_pending = atomic_read(&device->ap_pending_cnt) +
+			      atomic_read(&device->rs_pending_cnt);
+	s->peer_dev_unacked = atomic_read(&device->unacked_cnt);
+	s->peer_dev_out_of_sync = drbd_bm_total_weight(device) << (BM_BLOCK_SHIFT - 9);
+	s->peer_dev_resync_failed = device->rs_failed << (BM_BLOCK_SHIFT - 9);
+	if (get_ldev(device)) {
+		struct drbd_md *md = &device->ldev->md;
+
+		spin_lock_irq(&md->uuid_lock);
+		s->peer_dev_bitmap_uuid = md->uuid[UI_BITMAP];
+		spin_unlock_irq(&md->uuid_lock);
+		s->peer_dev_flags =
+			(drbd_md_test_flag(device->ldev, MDF_CONNECTED_IND) ?
+				MDF_PEER_CONNECTED : 0) +
+			(drbd_md_test_flag(device->ldev, MDF_CONSISTENT) &&
+			 !drbd_md_test_flag(device->ldev, MDF_WAS_UP_TO_DATE) ?
+				MDF_PEER_OUTDATED : 0) +
+			/* FIXME: MDF_PEER_FENCING? */
+			(drbd_md_test_flag(device->ldev, MDF_FULL_SYNC) ?
+				MDF_PEER_FULL_SYNC : 0);
+		put_ldev(device);
+	}
+}
+
+int drbd_adm_dump_peer_devices_done(struct netlink_callback *cb)
+{
+	return put_resource_in_arg0(cb, 9);
+}
+
+int drbd_adm_dump_peer_devices(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct nlattr *resource_filter;
+	struct drbd_resource *resource;
+	struct drbd_device *uninitialized_var(device);
+	struct drbd_peer_device *peer_device = NULL;
+	int minor, err, retcode;
+	struct drbd_genlmsghdr *dh;
+	struct idr *idr_to_search;
+
+	resource = (struct drbd_resource *)cb->args[0];
+	if (!cb->args[0] && !cb->args[1]) {
+		resource_filter = find_cfg_context_attr(cb->nlh, T_ctx_resource_name);
+		if (resource_filter) {
+			retcode = ERR_RES_NOT_KNOWN;
+			resource = drbd_find_resource(nla_data(resource_filter));
+			if (!resource)
+				goto put_result;
+		}
+		cb->args[0] = (long)resource;
+	}
+
+	rcu_read_lock();
+	minor = cb->args[1];
+	idr_to_search = resource ? &resource->devices : &drbd_devices;
+	device = idr_find(idr_to_search, minor);
+	if (!device) {
+next_device:
+		minor++;
+		cb->args[2] = 0;
+		device = idr_get_next(idr_to_search, &minor);
+		if (!device) {
+			err = 0;
+			goto out;
+		}
+	}
+	if (cb->args[2]) {
+		for_each_peer_device(peer_device, device)
+			if (peer_device == (struct drbd_peer_device *)cb->args[2])
+				goto found_peer_device;
+		/* peer device was probably deleted */
+		goto next_device;
+	}
+	/* Make peer_device point to the list head (not the first entry). */
+	peer_device = list_entry(&device->peer_devices, struct drbd_peer_device, peer_devices);
+
+found_peer_device:
+	list_for_each_entry_continue_rcu(peer_device, &device->peer_devices, peer_devices) {
+		if (!has_net_conf(peer_device->connection))
+			continue;
+		retcode = NO_ERROR;
+		goto put_result;  /* only one iteration */
+	}
+	goto next_device;
+
+put_result:
+	dh = genlmsg_put(skb, NETLINK_CB(cb->skb).portid,
+			cb->nlh->nlmsg_seq, &drbd_genl_family,
+			NLM_F_MULTI, DRBD_ADM_GET_PEER_DEVICES);
+	err = -ENOMEM;
+	if (!dh)
+		goto out;
+	dh->ret_code = retcode;
+	dh->minor = -1U;
+	if (retcode == NO_ERROR) {
+		struct peer_device_info peer_device_info;
+		struct peer_device_statistics peer_device_statistics;
+
+		dh->minor = minor;
+		err = nla_put_drbd_cfg_context(skb, device->resource, peer_device->connection, device);
+		if (err)
+			goto out;
+		peer_device_to_info(&peer_device_info, peer_device);
+		err = peer_device_info_to_skb(skb, &peer_device_info, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+		peer_device_to_statistics(&peer_device_statistics, peer_device);
+		err = peer_device_statistics_to_skb(skb, &peer_device_statistics, !capable(CAP_SYS_ADMIN));
+		if (err)
+			goto out;
+		cb->args[1] = minor;
+		cb->args[2] = (long)peer_device;
+	}
+	genlmsg_end(skb, dh);
+	err = 0;
+
+out:
+	rcu_read_unlock();
+	if (err)
+		return err;
+	return skb->len;
+}
+/*
  * Return the connection of @resource if @resource has exactly one connection.
  */
 static struct drbd_connection *the_only_connection(struct drbd_resource *resource)
@@ -3414,8 +4055,18 @@ drbd_check_resource_name(struct drbd_config_context *adm_ctx)
 	return NO_ERROR;
 }
 
+static void resource_to_info(struct resource_info *info,
+			     struct drbd_resource *resource)
+{
+	info->res_role = conn_highest_role(first_connection(resource));
+	info->res_susp = resource->susp;
+	info->res_susp_nod = resource->susp_nod;
+	info->res_susp_fen = resource->susp_fen;
+}
+
 int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 {
+	struct drbd_connection *connection;
 	struct drbd_config_context adm_ctx;
 	enum drbd_ret_code retcode;
 	struct res_opts res_opts;
@@ -3449,13 +4100,33 @@ int drbd_adm_new_resource(struct sk_buff *skb, struct genl_info *info)
 	}
 
 	/* not yet safe for genl_family.parallel_ops */
-	if (!conn_create(adm_ctx.resource_name, &res_opts))
+	mutex_lock(&resources_mutex);
+	connection = conn_create(adm_ctx.resource_name, &res_opts);
+	mutex_unlock(&resources_mutex);
+
+	if (connection) {
+		struct resource_info resource_info;
+
+		mutex_lock(&notification_mutex);
+		resource_to_info(&resource_info, connection->resource);
+		notify_resource_state(NULL, 0, connection->resource,
+				      &resource_info, NOTIFY_CREATE);
+		mutex_unlock(&notification_mutex);
+	} else
 		retcode = ERR_NOMEM;
+
 out:
 	drbd_adm_finish(&adm_ctx, info, retcode);
 	return 0;
 }
 
+static void device_to_info(struct device_info *info,
+			   struct drbd_device *device)
+{
+	info->dev_disk_state = device->state.disk;
+}
+
+
 int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 {
 	struct drbd_config_context adm_ctx;
@@ -3490,6 +4161,36 @@ int drbd_adm_new_minor(struct sk_buff *skb, struct genl_info *info)
 
 	mutex_lock(&adm_ctx.resource->adm_mutex);
 	retcode = drbd_create_device(&adm_ctx, dh->minor);
+	if (retcode == NO_ERROR) {
+		struct drbd_device *device;
+		struct drbd_peer_device *peer_device;
+		struct device_info info;
+		unsigned int peer_devices = 0;
+		enum drbd_notification_type flags;
+
+		device = minor_to_device(dh->minor);
+		for_each_peer_device(peer_device, device) {
+			if (!has_net_conf(peer_device->connection))
+				continue;
+			peer_devices++;
+		}
+
+		device_to_info(&info, device);
+		mutex_lock(&notification_mutex);
+		flags = (peer_devices--) ? NOTIFY_CONTINUES : 0;
+		notify_device_state(NULL, 0, device, &info, NOTIFY_CREATE | flags);
+		for_each_peer_device(peer_device, device) {
+			struct peer_device_info peer_device_info;
+
+			if (!has_net_conf(peer_device->connection))
+				continue;
+			peer_device_to_info(&peer_device_info, peer_device);
+			flags = (peer_devices--) ? NOTIFY_CONTINUES : 0;
+			notify_peer_device_state(NULL, 0, peer_device, &peer_device_info,
+						 NOTIFY_CREATE | flags);
+		}
+		mutex_unlock(&notification_mutex);
+	}
 	mutex_unlock(&adm_ctx.resource->adm_mutex);
 out:
 	drbd_adm_finish(&adm_ctx, info, retcode);
@@ -3498,13 +4199,35 @@ out:
 
 static enum drbd_ret_code adm_del_minor(struct drbd_device *device)
 {
+	struct drbd_peer_device *peer_device;
+
 	if (device->state.disk == D_DISKLESS &&
 	    /* no need to be device->state.conn == C_STANDALONE &&
 	     * we may want to delete a minor from a live replication group.
 	     */
 	    device->state.role == R_SECONDARY) {
+		struct drbd_connection *connection =
+			first_connection(device->resource);
+
 		_drbd_request_state(device, NS(conn, C_WF_REPORT_PARAMS),
 				    CS_VERBOSE + CS_WAIT_COMPLETE);
+
+		/* If the state engine hasn't stopped the sender thread yet, we
+		 * need to flush the sender work queue before generating the
+		 * DESTROY events here. */
+		if (get_t_state(&connection->worker) == RUNNING)
+			drbd_flush_workqueue(&connection->sender_work);
+
+		mutex_lock(&notification_mutex);
+		for_each_peer_device(peer_device, device) {
+			if (!has_net_conf(peer_device->connection))
+				continue;
+			notify_peer_device_state(NULL, 0, peer_device, NULL,
+						 NOTIFY_DESTROY | NOTIFY_CONTINUES);
+		}
+		notify_device_state(NULL, 0, device, NULL, NOTIFY_DESTROY);
+		mutex_unlock(&notification_mutex);
+
 		drbd_delete_device(device);
 		return NO_ERROR;
 	} else
@@ -3541,7 +4264,16 @@ static int adm_del_resource(struct drbd_resource *resource)
 	if (!idr_is_empty(&resource->devices))
 		return ERR_RES_IN_USE;
 
+	/* The state engine has stopped the sender thread, so we don't
+	 * need to flush the sender work queue before generating the
+	 * DESTROY event here. */
+	mutex_lock(&notification_mutex);
+	notify_resource_state(NULL, 0, resource, NULL, NOTIFY_DESTROY);
+	mutex_unlock(&notification_mutex);
+
+	mutex_lock(&resources_mutex);
 	list_del_rcu(&resource->resources);
+	mutex_unlock(&resources_mutex);
 	/* Make sure all threads have actually stopped: state handling only
 	 * does drbd_thread_stop_nowait(). */
 	list_for_each_entry(connection, &resource->connections, connections)
@@ -3637,7 +4369,6 @@ finish:
 
 void drbd_bcast_event(struct drbd_device *device, const struct sib_info *sib)
 {
-	static atomic_t drbd_genl_seq = ATOMIC_INIT(2); /* two. */
 	struct sk_buff *msg;
 	struct drbd_genlmsghdr *d_out;
 	unsigned seq;
@@ -3658,7 +4389,7 @@ void drbd_bcast_event(struct drbd_device *device, const struct sib_info *sib)
 	if (nla_put_status_info(msg, device, sib))
 		goto nla_put_failure;
 	genlmsg_end(msg, d_out);
-	err = drbd_genl_multicast_events(msg, 0);
+	err = drbd_genl_multicast_events(msg, GFP_NOWAIT);
 	/* msg has been consumed or freed in netlink_broadcast() */
 	if (err && err != -ESRCH)
 		goto failed;
@@ -3672,3 +4403,405 @@ failed:
 			"Event seq:%u sib_reason:%u\n",
 			err, seq, sib->sib_reason);
 }
+
+static int nla_put_notification_header(struct sk_buff *msg,
+				       enum drbd_notification_type type)
+{
+	struct drbd_notification_header nh = {
+		.nh_type = type,
+	};
+
+	return drbd_notification_header_to_skb(msg, &nh, true);
+}
+
+void notify_resource_state(struct sk_buff *skb,
+			   unsigned int seq,
+			   struct drbd_resource *resource,
+			   struct resource_info *resource_info,
+			   enum drbd_notification_type type)
+{
+	struct resource_statistics resource_statistics;
+	struct drbd_genlmsghdr *dh;
+	bool multicast = false;
+	int err;
+
+	if (!skb) {
+		seq = atomic_inc_return(&notify_genl_seq);
+		skb = genlmsg_new(NLMSG_GOODSIZE, GFP_NOIO);
+		err = -ENOMEM;
+		if (!skb)
+			goto failed;
+		multicast = true;
+	}
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_RESOURCE_STATE);
+	if (!dh)
+		goto nla_put_failure;
+	dh->minor = -1U;
+	dh->ret_code = NO_ERROR;
+	if (nla_put_drbd_cfg_context(skb, resource, NULL, NULL) ||
+	    nla_put_notification_header(skb, type) ||
+	    ((type & ~NOTIFY_FLAGS) != NOTIFY_DESTROY &&
+	     resource_info_to_skb(skb, resource_info, true)))
+		goto nla_put_failure;
+	resource_statistics.res_stat_write_ordering = resource->write_ordering;
+	err = resource_statistics_to_skb(skb, &resource_statistics, !capable(CAP_SYS_ADMIN));
+	if (err)
+		goto nla_put_failure;
+	genlmsg_end(skb, dh);
+	if (multicast) {
+		err = drbd_genl_multicast_events(skb, GFP_NOWAIT);
+		/* skb has been consumed or freed in netlink_broadcast() */
+		if (err && err != -ESRCH)
+			goto failed;
+	}
+	return;
+
+nla_put_failure:
+	nlmsg_free(skb);
+failed:
+	drbd_err(resource, "Error %d while broadcasting event. Event seq:%u\n",
+			err, seq);
+}
+
+void notify_device_state(struct sk_buff *skb,
+			 unsigned int seq,
+			 struct drbd_device *device,
+			 struct device_info *device_info,
+			 enum drbd_notification_type type)
+{
+	struct device_statistics device_statistics;
+	struct drbd_genlmsghdr *dh;
+	bool multicast = false;
+	int err;
+
+	if (!skb) {
+		seq = atomic_inc_return(&notify_genl_seq);
+		skb = genlmsg_new(NLMSG_GOODSIZE, GFP_NOIO);
+		err = -ENOMEM;
+		if (!skb)
+			goto failed;
+		multicast = true;
+	}
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_DEVICE_STATE);
+	if (!dh)
+		goto nla_put_failure;
+	dh->minor = device->minor;
+	dh->ret_code = NO_ERROR;
+	if (nla_put_drbd_cfg_context(skb, device->resource, NULL, device) ||
+	    nla_put_notification_header(skb, type) ||
+	    ((type & ~NOTIFY_FLAGS) != NOTIFY_DESTROY &&
+	     device_info_to_skb(skb, device_info, true)))
+		goto nla_put_failure;
+	device_to_statistics(&device_statistics, device);
+	device_statistics_to_skb(skb, &device_statistics, !capable(CAP_SYS_ADMIN));
+	genlmsg_end(skb, dh);
+	if (multicast) {
+		err = drbd_genl_multicast_events(skb, GFP_NOWAIT);
+		/* skb has been consumed or freed in netlink_broadcast() */
+		if (err && err != -ESRCH)
+			goto failed;
+	}
+	return;
+
+nla_put_failure:
+	nlmsg_free(skb);
+failed:
+	drbd_err(device, "Error %d while broadcasting event. Event seq:%u\n",
+		 err, seq);
+}
+
+void notify_connection_state(struct sk_buff *skb,
+			     unsigned int seq,
+			     struct drbd_connection *connection,
+			     struct connection_info *connection_info,
+			     enum drbd_notification_type type)
+{
+	struct connection_statistics connection_statistics;
+	struct drbd_genlmsghdr *dh;
+	bool multicast = false;
+	int err;
+
+	if (!skb) {
+		seq = atomic_inc_return(&notify_genl_seq);
+		skb = genlmsg_new(NLMSG_GOODSIZE, GFP_NOIO);
+		err = -ENOMEM;
+		if (!skb)
+			goto failed;
+		multicast = true;
+	}
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_CONNECTION_STATE);
+	if (!dh)
+		goto nla_put_failure;
+	dh->minor = -1U;
+	dh->ret_code = NO_ERROR;
+	if (nla_put_drbd_cfg_context(skb, connection->resource, connection, NULL) ||
+	    nla_put_notification_header(skb, type) ||
+	    ((type & ~NOTIFY_FLAGS) != NOTIFY_DESTROY &&
+	     connection_info_to_skb(skb, connection_info, true)))
+		goto nla_put_failure;
+	connection_statistics.conn_congested = test_bit(NET_CONGESTED, &connection->flags);
+	connection_statistics_to_skb(skb, &connection_statistics, !capable(CAP_SYS_ADMIN));
+	genlmsg_end(skb, dh);
+	if (multicast) {
+		err = drbd_genl_multicast_events(skb, GFP_NOWAIT);
+		/* skb has been consumed or freed in netlink_broadcast() */
+		if (err && err != -ESRCH)
+			goto failed;
+	}
+	return;
+
+nla_put_failure:
+	nlmsg_free(skb);
+failed:
+	drbd_err(connection, "Error %d while broadcasting event. Event seq:%u\n",
+		 err, seq);
+}
+
+void notify_peer_device_state(struct sk_buff *skb,
+			      unsigned int seq,
+			      struct drbd_peer_device *peer_device,
+			      struct peer_device_info *peer_device_info,
+			      enum drbd_notification_type type)
+{
+	struct peer_device_statistics peer_device_statistics;
+	struct drbd_resource *resource = peer_device->device->resource;
+	struct drbd_genlmsghdr *dh;
+	bool multicast = false;
+	int err;
+
+	if (!skb) {
+		seq = atomic_inc_return(&notify_genl_seq);
+		skb = genlmsg_new(NLMSG_GOODSIZE, GFP_NOIO);
+		err = -ENOMEM;
+		if (!skb)
+			goto failed;
+		multicast = true;
+	}
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_PEER_DEVICE_STATE);
+	if (!dh)
+		goto nla_put_failure;
+	dh->minor = -1U;
+	dh->ret_code = NO_ERROR;
+	if (nla_put_drbd_cfg_context(skb, resource, peer_device->connection, peer_device->device) ||
+	    nla_put_notification_header(skb, type) ||
+	    ((type & ~NOTIFY_FLAGS) != NOTIFY_DESTROY &&
+	     peer_device_info_to_skb(skb, peer_device_info, true)))
+		goto nla_put_failure;
+	peer_device_to_statistics(&peer_device_statistics, peer_device);
+	peer_device_statistics_to_skb(skb, &peer_device_statistics, !capable(CAP_SYS_ADMIN));
+	genlmsg_end(skb, dh);
+	if (multicast) {
+		err = drbd_genl_multicast_events(skb, GFP_NOWAIT);
+		/* skb has been consumed or freed in netlink_broadcast() */
+		if (err && err != -ESRCH)
+			goto failed;
+	}
+	return;
+
+nla_put_failure:
+	nlmsg_free(skb);
+failed:
+	drbd_err(peer_device, "Error %d while broadcasting event. Event seq:%u\n",
+		 err, seq);
+}
+
+void notify_helper(enum drbd_notification_type type,
+		   struct drbd_device *device, struct drbd_connection *connection,
+		   const char *name, int status)
+{
+	struct drbd_resource *resource = device ? device->resource : connection->resource;
+	struct drbd_helper_info helper_info;
+	unsigned int seq = atomic_inc_return(&notify_genl_seq);
+	struct sk_buff *skb = NULL;
+	struct drbd_genlmsghdr *dh;
+	int err;
+
+	strlcpy(helper_info.helper_name, name, sizeof(helper_info.helper_name));
+	helper_info.helper_name_len = min(strlen(name), sizeof(helper_info.helper_name));
+	helper_info.helper_status = status;
+
+	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_NOIO);
+	err = -ENOMEM;
+	if (!skb)
+		goto fail;
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_HELPER);
+	if (!dh)
+		goto fail;
+	dh->minor = device ? device->minor : -1;
+	dh->ret_code = NO_ERROR;
+	mutex_lock(&notification_mutex);
+	if (nla_put_drbd_cfg_context(skb, resource, connection, device) ||
+	    nla_put_notification_header(skb, type) ||
+	    drbd_helper_info_to_skb(skb, &helper_info, true))
+		goto unlock_fail;
+	genlmsg_end(skb, dh);
+	err = drbd_genl_multicast_events(skb, GFP_NOWAIT);
+	skb = NULL;
+	/* skb has been consumed or freed in netlink_broadcast() */
+	if (err && err != -ESRCH)
+		goto unlock_fail;
+	mutex_unlock(&notification_mutex);
+	return;
+
+unlock_fail:
+	mutex_unlock(&notification_mutex);
+fail:
+	nlmsg_free(skb);
+	drbd_err(resource, "Error %d while broadcasting event. Event seq:%u\n",
+		 err, seq);
+}
+
+static void notify_initial_state_done(struct sk_buff *skb, unsigned int seq)
+{
+	struct drbd_genlmsghdr *dh;
+	int err;
+
+	err = -EMSGSIZE;
+	dh = genlmsg_put(skb, 0, seq, &drbd_genl_family, 0, DRBD_INITIAL_STATE_DONE);
+	if (!dh)
+		goto nla_put_failure;
+	dh->minor = -1U;
+	dh->ret_code = NO_ERROR;
+	if (nla_put_notification_header(skb, NOTIFY_EXISTS))
+		goto nla_put_failure;
+	genlmsg_end(skb, dh);
+	return;
+
+nla_put_failure:
+	nlmsg_free(skb);
+	pr_err("Error %d sending event. Event seq:%u\n", err, seq);
+}
+
+static void free_state_changes(struct list_head *list)
+{
+	while (!list_empty(list)) {
+		struct drbd_state_change *state_change =
+			list_first_entry(list, struct drbd_state_change, list);
+		list_del(&state_change->list);
+		forget_state_change(state_change);
+	}
+}
+
+static unsigned int notifications_for_state_change(struct drbd_state_change *state_change)
+{
+	return 1 +
+	       state_change->n_connections +
+	       state_change->n_devices +
+	       state_change->n_devices * state_change->n_connections;
+}
+
+static int get_initial_state(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct drbd_state_change *state_change = (struct drbd_state_change *)cb->args[0];
+	unsigned int seq = cb->args[2];
+	unsigned int n;
+	enum drbd_notification_type flags = 0;
+
+	/* There is no need for taking notification_mutex here: it doesn't
+	   matter if the initial state events mix with later state chage
+	   events; we can always tell the events apart by the NOTIFY_EXISTS
+	   flag. */
+
+	cb->args[5]--;
+	if (cb->args[5] == 1) {
+		notify_initial_state_done(skb, seq);
+		goto out;
+	}
+	n = cb->args[4]++;
+	if (cb->args[4] < cb->args[3])
+		flags |= NOTIFY_CONTINUES;
+	if (n < 1) {
+		notify_resource_state_change(skb, seq, state_change->resource,
+					     NOTIFY_EXISTS | flags);
+		goto next;
+	}
+	n--;
+	if (n < state_change->n_connections) {
+		notify_connection_state_change(skb, seq, &state_change->connections[n],
+					       NOTIFY_EXISTS | flags);
+		goto next;
+	}
+	n -= state_change->n_connections;
+	if (n < state_change->n_devices) {
+		notify_device_state_change(skb, seq, &state_change->devices[n],
+					   NOTIFY_EXISTS | flags);
+		goto next;
+	}
+	n -= state_change->n_devices;
+	if (n < state_change->n_devices * state_change->n_connections) {
+		notify_peer_device_state_change(skb, seq, &state_change->peer_devices[n],
+						NOTIFY_EXISTS | flags);
+		goto next;
+	}
+
+next:
+	if (cb->args[4] == cb->args[3]) {
+		struct drbd_state_change *next_state_change =
+			list_entry(state_change->list.next,
+				   struct drbd_state_change, list);
+		cb->args[0] = (long)next_state_change;
+		cb->args[3] = notifications_for_state_change(next_state_change);
+		cb->args[4] = 0;
+	}
+out:
+	return skb->len;
+}
+
+int drbd_adm_get_initial_state(struct sk_buff *skb, struct netlink_callback *cb)
+{
+	struct drbd_resource *resource;
+	LIST_HEAD(head);
+
+	if (cb->args[5] >= 1) {
+		if (cb->args[5] > 1)
+			return get_initial_state(skb, cb);
+		if (cb->args[0]) {
+			struct drbd_state_change *state_change =
+				(struct drbd_state_change *)cb->args[0];
+
+			/* connect list to head */
+			list_add(&head, &state_change->list);
+			free_state_changes(&head);
+		}
+		return 0;
+	}
+
+	cb->args[5] = 2;  /* number of iterations */
+	mutex_lock(&resources_mutex);
+	for_each_resource(resource, &drbd_resources) {
+		struct drbd_state_change *state_change;
+
+		state_change = remember_old_state(resource, GFP_KERNEL);
+		if (!state_change) {
+			if (!list_empty(&head))
+				free_state_changes(&head);
+			mutex_unlock(&resources_mutex);
+			return -ENOMEM;
+		}
+		copy_old_to_new_state_change(state_change);
+		list_add_tail(&state_change->list, &head);
+		cb->args[5] += notifications_for_state_change(state_change);
+	}
+	mutex_unlock(&resources_mutex);
+
+	if (!list_empty(&head)) {
+		struct drbd_state_change *state_change =
+			list_entry(head.next, struct drbd_state_change, list);
+		cb->args[0] = (long)state_change;
+		cb->args[3] = notifications_for_state_change(state_change);
+		list_del(&head);  /* detach list from head */
+	}
+
+	cb->args[2] = cb->nlh->nlmsg_seq;
+	return get_initial_state(skb, cb);
+}
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 3b10fa6cb039..6537b25db9c1 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -245,9 +245,9 @@ static int drbd_seq_show(struct seq_file *seq, void *v)
 	char wp;
 
 	static char write_ordering_chars[] = {
-		[WO_none] = 'n',
-		[WO_drain_io] = 'd',
-		[WO_bdev_flush] = 'f',
+		[WO_NONE] = 'n',
+		[WO_DRAIN_IO] = 'd',
+		[WO_BDEV_FLUSH] = 'f',
 	};
 
 	seq_printf(seq, "version: " REL_VERSION " (api:%d/proto:%d-%d)\n%s\n",
diff --git a/drivers/block/drbd/drbd_protocol.h b/drivers/block/drbd/drbd_protocol.h
index 2da9104a3851..ef9245363dcc 100644
--- a/drivers/block/drbd/drbd_protocol.h
+++ b/drivers/block/drbd/drbd_protocol.h
@@ -23,7 +23,7 @@ enum drbd_packet {
 	P_AUTH_RESPONSE	      = 0x11,
 	P_STATE_CHG_REQ	      = 0x12,
 
-	/* asender (meta socket */
+	/* (meta socket) */
 	P_PING		      = 0x13,
 	P_PING_ACK	      = 0x14,
 	P_RECV_ACK	      = 0x15, /* Used in protocol B */
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index b4b5680ac6ad..1957fe8601dc 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -215,7 +215,7 @@ static void reclaim_finished_net_peer_reqs(struct drbd_device *device,
 	}
 }
 
-static void drbd_kick_lo_and_reclaim_net(struct drbd_device *device)
+static void drbd_reclaim_net_peer_reqs(struct drbd_device *device)
 {
 	LIST_HEAD(reclaimed);
 	struct drbd_peer_request *peer_req, *t;
@@ -223,11 +223,30 @@ static void drbd_kick_lo_and_reclaim_net(struct drbd_device *device)
 	spin_lock_irq(&device->resource->req_lock);
 	reclaim_finished_net_peer_reqs(device, &reclaimed);
 	spin_unlock_irq(&device->resource->req_lock);
-
 	list_for_each_entry_safe(peer_req, t, &reclaimed, w.list)
 		drbd_free_net_peer_req(device, peer_req);
 }
 
+static void conn_reclaim_net_peer_reqs(struct drbd_connection *connection)
+{
+	struct drbd_peer_device *peer_device;
+	int vnr;
+
+	rcu_read_lock();
+	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
+		struct drbd_device *device = peer_device->device;
+		if (!atomic_read(&device->pp_in_use_by_net))
+			continue;
+
+		kref_get(&device->kref);
+		rcu_read_unlock();
+		drbd_reclaim_net_peer_reqs(device);
+		kref_put(&device->kref, drbd_destroy_device);
+		rcu_read_lock();
+	}
+	rcu_read_unlock();
+}
+
 /**
  * drbd_alloc_pages() - Returns @number pages, retries forever (or until signalled)
  * @device:	DRBD device.
@@ -265,10 +284,15 @@ struct page *drbd_alloc_pages(struct drbd_peer_device *peer_device, unsigned int
 	if (atomic_read(&device->pp_in_use) < mxb)
 		page = __drbd_alloc_pages(device, number);
 
+	/* Try to keep the fast path fast, but occasionally we need
+	 * to reclaim the pages we lended to the network stack. */
+	if (page && atomic_read(&device->pp_in_use_by_net) > 512)
+		drbd_reclaim_net_peer_reqs(device);
+
 	while (page == NULL) {
 		prepare_to_wait(&drbd_pp_wait, &wait, TASK_INTERRUPTIBLE);
 
-		drbd_kick_lo_and_reclaim_net(device);
+		drbd_reclaim_net_peer_reqs(device);
 
 		if (atomic_read(&device->pp_in_use) < mxb) {
 			page = __drbd_alloc_pages(device, number);
@@ -1099,7 +1123,15 @@ randomize:
 		return 0;
 	}
 
-	drbd_thread_start(&connection->asender);
+	drbd_thread_start(&connection->ack_receiver);
+	/* opencoded create_singlethread_workqueue(),
+	 * to be able to use format string arguments */
+	connection->ack_sender =
+		alloc_ordered_workqueue("drbd_as_%s", WQ_MEM_RECLAIM, connection->resource->name);
+	if (!connection->ack_sender) {
+		drbd_err(connection, "Failed to create workqueue ack_sender\n");
+		return 0;
+	}
 
 	mutex_lock(&connection->resource->conf_update);
 	/* The discard_my_data flag is a single-shot modifier to the next
@@ -1178,7 +1210,7 @@ static void drbd_flush(struct drbd_connection *connection)
 	struct drbd_peer_device *peer_device;
 	int vnr;
 
-	if (connection->resource->write_ordering >= WO_bdev_flush) {
+	if (connection->resource->write_ordering >= WO_BDEV_FLUSH) {
 		rcu_read_lock();
 		idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 			struct drbd_device *device = peer_device->device;
@@ -1203,7 +1235,7 @@ static void drbd_flush(struct drbd_connection *connection)
 				/* would rather check on EOPNOTSUPP, but that is not reliable.
 				 * don't try again for ANY return value != 0
 				 * if (rv == -EOPNOTSUPP) */
-				drbd_bump_write_ordering(connection->resource, NULL, WO_drain_io);
+				drbd_bump_write_ordering(connection->resource, NULL, WO_DRAIN_IO);
 			}
 			put_ldev(device);
 			kref_put(&device->kref, drbd_destroy_device);
@@ -1299,10 +1331,10 @@ max_allowed_wo(struct drbd_backing_dev *bdev, enum write_ordering_e wo)
 
 	dc = rcu_dereference(bdev->disk_conf);
 
-	if (wo == WO_bdev_flush && !dc->disk_flushes)
-		wo = WO_drain_io;
-	if (wo == WO_drain_io && !dc->disk_drain)
-		wo = WO_none;
+	if (wo == WO_BDEV_FLUSH && !dc->disk_flushes)
+		wo = WO_DRAIN_IO;
+	if (wo == WO_DRAIN_IO && !dc->disk_drain)
+		wo = WO_NONE;
 
 	return wo;
 }
@@ -1319,13 +1351,13 @@ void drbd_bump_write_ordering(struct drbd_resource *resource, struct drbd_backin
 	enum write_ordering_e pwo;
 	int vnr;
 	static char *write_ordering_str[] = {
-		[WO_none] = "none",
-		[WO_drain_io] = "drain",
-		[WO_bdev_flush] = "flush",
+		[WO_NONE] = "none",
+		[WO_DRAIN_IO] = "drain",
+		[WO_BDEV_FLUSH] = "flush",
 	};
 
 	pwo = resource->write_ordering;
-	if (wo != WO_bdev_flush)
+	if (wo != WO_BDEV_FLUSH)
 		wo = min(pwo, wo);
 	rcu_read_lock();
 	idr_for_each_entry(&resource->devices, device, vnr) {
@@ -1343,7 +1375,7 @@ void drbd_bump_write_ordering(struct drbd_resource *resource, struct drbd_backin
 	rcu_read_unlock();
 
 	resource->write_ordering = wo;
-	if (pwo != resource->write_ordering || wo == WO_bdev_flush)
+	if (pwo != resource->write_ordering || wo == WO_BDEV_FLUSH)
 		drbd_info(resource, "Method to ensure write ordering: %s\n", write_ordering_str[resource->write_ordering]);
 }
 
@@ -1380,7 +1412,7 @@ int drbd_submit_peer_request(struct drbd_device *device,
 	if (peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) {
 		/* wait for all pending IO completions, before we start
 		 * zeroing things out. */
-		conn_wait_active_ee_empty(first_peer_device(device)->connection);
+		conn_wait_active_ee_empty(peer_req->peer_device->connection);
 		/* add it to the active list now,
 		 * so we can find it to present it in debugfs */
 		peer_req->submit_jif = jiffies;
@@ -1508,12 +1540,6 @@ static void conn_wait_active_ee_empty(struct drbd_connection *connection)
 	rcu_read_unlock();
 }
 
-static struct drbd_peer_device *
-conn_peer_device(struct drbd_connection *connection, int volume_number)
-{
-	return idr_find(&connection->peer_devices, volume_number);
-}
-
 static int receive_Barrier(struct drbd_connection *connection, struct packet_info *pi)
 {
 	int rv;
@@ -1533,7 +1559,7 @@ static int receive_Barrier(struct drbd_connection *connection, struct packet_inf
 	 * Therefore we must send the barrier_ack after the barrier request was
 	 * completed. */
 	switch (connection->resource->write_ordering) {
-	case WO_none:
+	case WO_NONE:
 		if (rv == FE_RECYCLED)
 			return 0;
 
@@ -1546,8 +1572,8 @@ static int receive_Barrier(struct drbd_connection *connection, struct packet_inf
 			drbd_warn(connection, "Allocation of an epoch failed, slowing down\n");
 			/* Fall through */
 
-	case WO_bdev_flush:
-	case WO_drain_io:
+	case WO_BDEV_FLUSH:
+	case WO_DRAIN_IO:
 		conn_wait_active_ee_empty(connection);
 		drbd_flush(connection);
 
@@ -1752,7 +1778,7 @@ static int recv_dless_read(struct drbd_peer_device *peer_device, struct drbd_req
 }
 
 /*
- * e_end_resync_block() is called in asender context via
+ * e_end_resync_block() is called in ack_sender context via
  * drbd_finish_peer_reqs().
  */
 static int e_end_resync_block(struct drbd_work *w, int unused)
@@ -1926,7 +1952,7 @@ static void restart_conflicting_writes(struct drbd_device *device,
 }
 
 /*
- * e_end_block() is called in asender context via drbd_finish_peer_reqs().
+ * e_end_block() is called in ack_sender context via drbd_finish_peer_reqs().
  */
 static int e_end_block(struct drbd_work *w, int cancel)
 {
@@ -1966,7 +1992,7 @@ static int e_end_block(struct drbd_work *w, int cancel)
 	} else
 		D_ASSERT(device, drbd_interval_empty(&peer_req->i));
 
-	drbd_may_finish_epoch(first_peer_device(device)->connection, peer_req->epoch, EV_PUT + (cancel ? EV_CLEANUP : 0));
+	drbd_may_finish_epoch(peer_device->connection, peer_req->epoch, EV_PUT + (cancel ? EV_CLEANUP : 0));
 
 	return err;
 }
@@ -2098,7 +2124,7 @@ static int wait_for_and_update_peer_seq(struct drbd_peer_device *peer_device, co
 		}
 
 		rcu_read_lock();
-		tp = rcu_dereference(first_peer_device(device)->connection->net_conf)->two_primaries;
+		tp = rcu_dereference(peer_device->connection->net_conf)->two_primaries;
 		rcu_read_unlock();
 
 		if (!tp)
@@ -2217,7 +2243,7 @@ static int handle_write_conflicts(struct drbd_device *device,
 			peer_req->w.cb = superseded ? e_send_superseded :
 						   e_send_retry_write;
 			list_add_tail(&peer_req->w.list, &device->done_ee);
-			wake_asender(connection);
+			queue_work(connection->ack_sender, &peer_req->peer_device->send_acks_work);
 
 			err = -ENOENT;
 			goto out;
@@ -2364,7 +2390,7 @@ static int receive_Data(struct drbd_connection *connection, struct packet_info *
 	if (dp_flags & DP_SEND_RECEIVE_ACK) {
 		/* I really don't like it that the receiver thread
 		 * sends on the msock, but anyways */
-		drbd_send_ack(first_peer_device(device), P_RECV_ACK, peer_req);
+		drbd_send_ack(peer_device, P_RECV_ACK, peer_req);
 	}
 
 	if (tp) {
@@ -4056,7 +4082,7 @@ static int receive_state(struct drbd_connection *connection, struct packet_info
 	os = ns = drbd_read_state(device);
 	spin_unlock_irq(&device->resource->req_lock);
 
-	/* If some other part of the code (asender thread, timeout)
+	/* If some other part of the code (ack_receiver thread, timeout)
 	 * already decided to close the connection again,
 	 * we must not "re-establish" it here. */
 	if (os.conn <= C_TEAR_DOWN)
@@ -4661,8 +4687,12 @@ static void conn_disconnect(struct drbd_connection *connection)
 	 */
 	conn_request_state(connection, NS(conn, C_NETWORK_FAILURE), CS_HARD);
 
-	/* asender does not clean up anything. it must not interfere, either */
-	drbd_thread_stop(&connection->asender);
+	/* ack_receiver does not clean up anything. it must not interfere, either */
+	drbd_thread_stop(&connection->ack_receiver);
+	if (connection->ack_sender) {
+		destroy_workqueue(connection->ack_sender);
+		connection->ack_sender = NULL;
+	}
 	drbd_free_sock(connection);
 
 	rcu_read_lock();
@@ -5431,49 +5461,39 @@ static int got_skip(struct drbd_connection *connection, struct packet_info *pi)
 	return 0;
 }
 
-static int connection_finish_peer_reqs(struct drbd_connection *connection)
+struct meta_sock_cmd {
+	size_t pkt_size;
+	int (*fn)(struct drbd_connection *connection, struct packet_info *);
+};
+
+static void set_rcvtimeo(struct drbd_connection *connection, bool ping_timeout)
 {
-	struct drbd_peer_device *peer_device;
-	int vnr, not_empty = 0;
+	long t;
+	struct net_conf *nc;
 
-	do {
-		clear_bit(SIGNAL_ASENDER, &connection->flags);
-		flush_signals(current);
+	rcu_read_lock();
+	nc = rcu_dereference(connection->net_conf);
+	t = ping_timeout ? nc->ping_timeo : nc->ping_int;
+	rcu_read_unlock();
 
-		rcu_read_lock();
-		idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
-			struct drbd_device *device = peer_device->device;
-			kref_get(&device->kref);
-			rcu_read_unlock();
-			if (drbd_finish_peer_reqs(device)) {
-				kref_put(&device->kref, drbd_destroy_device);
-				return 1;
-			}
-			kref_put(&device->kref, drbd_destroy_device);
-			rcu_read_lock();
-		}
-		set_bit(SIGNAL_ASENDER, &connection->flags);
+	t *= HZ;
+	if (ping_timeout)
+		t /= 10;
 
-		spin_lock_irq(&connection->resource->req_lock);
-		idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
-			struct drbd_device *device = peer_device->device;
-			not_empty = !list_empty(&device->done_ee);
-			if (not_empty)
-				break;
-		}
-		spin_unlock_irq(&connection->resource->req_lock);
-		rcu_read_unlock();
-	} while (not_empty);
+	connection->meta.socket->sk->sk_rcvtimeo = t;
+}
 
-	return 0;
+static void set_ping_timeout(struct drbd_connection *connection)
+{
+	set_rcvtimeo(connection, 1);
 }
 
-struct asender_cmd {
-	size_t pkt_size;
-	int (*fn)(struct drbd_connection *connection, struct packet_info *);
-};
+static void set_idle_timeout(struct drbd_connection *connection)
+{
+	set_rcvtimeo(connection, 0);
+}
 
-static struct asender_cmd asender_tbl[] = {
+static struct meta_sock_cmd ack_receiver_tbl[] = {
 	[P_PING]	    = { 0, got_Ping },
 	[P_PING_ACK]	    = { 0, got_PingAck },
 	[P_RECV_ACK]	    = { sizeof(struct p_block_ack), got_BlockAck },
@@ -5493,64 +5513,40 @@ static struct asender_cmd asender_tbl[] = {
 	[P_RETRY_WRITE]	    = { sizeof(struct p_block_ack), got_BlockAck },
 };
 
-int drbd_asender(struct drbd_thread *thi)
+int drbd_ack_receiver(struct drbd_thread *thi)
 {
 	struct drbd_connection *connection = thi->connection;
-	struct asender_cmd *cmd = NULL;
+	struct meta_sock_cmd *cmd = NULL;
 	struct packet_info pi;
+	unsigned long pre_recv_jif;
 	int rv;
 	void *buf    = connection->meta.rbuf;
 	int received = 0;
 	unsigned int header_size = drbd_header_size(connection);
 	int expect   = header_size;
 	bool ping_timeout_active = false;
-	struct net_conf *nc;
-	int ping_timeo, tcp_cork, ping_int;
 	struct sched_param param = { .sched_priority = 2 };
 
 	rv = sched_setscheduler(current, SCHED_RR, &param);
 	if (rv < 0)
-		drbd_err(connection, "drbd_asender: ERROR set priority, ret=%d\n", rv);
+		drbd_err(connection, "drbd_ack_receiver: ERROR set priority, ret=%d\n", rv);
 
 	while (get_t_state(thi) == RUNNING) {
 		drbd_thread_current_set_cpu(thi);
 
-		rcu_read_lock();
-		nc = rcu_dereference(connection->net_conf);
-		ping_timeo = nc->ping_timeo;
-		tcp_cork = nc->tcp_cork;
-		ping_int = nc->ping_int;
-		rcu_read_unlock();
+		conn_reclaim_net_peer_reqs(connection);
 
 		if (test_and_clear_bit(SEND_PING, &connection->flags)) {
 			if (drbd_send_ping(connection)) {
 				drbd_err(connection, "drbd_send_ping has failed\n");
 				goto reconnect;
 			}
-			connection->meta.socket->sk->sk_rcvtimeo = ping_timeo * HZ / 10;
+			set_ping_timeout(connection);
 			ping_timeout_active = true;
 		}
 
-		/* TODO: conditionally cork; it may hurt latency if we cork without
-		   much to send */
-		if (tcp_cork)
-			drbd_tcp_cork(connection->meta.socket);
-		if (connection_finish_peer_reqs(connection)) {
-			drbd_err(connection, "connection_finish_peer_reqs() failed\n");
-			goto reconnect;
-		}
-		/* but unconditionally uncork unless disabled */
-		if (tcp_cork)
-			drbd_tcp_uncork(connection->meta.socket);
-
-		/* short circuit, recv_msg would return EINTR anyways. */
-		if (signal_pending(current))
-			continue;
-
+		pre_recv_jif = jiffies;
 		rv = drbd_recv_short(connection->meta.socket, buf, expect-received, 0);
-		clear_bit(SIGNAL_ASENDER, &connection->flags);
-
-		flush_signals(current);
 
 		/* Note:
 		 * -EINTR	 (on meta) we got a signal
@@ -5562,7 +5558,6 @@ int drbd_asender(struct drbd_thread *thi)
 		 * rv <  expected: "woken" by signal during receive
 		 * rv == 0	 : "connection shut down by peer"
 		 */
-received_more:
 		if (likely(rv > 0)) {
 			received += rv;
 			buf	 += rv;
@@ -5584,8 +5579,7 @@ received_more:
 		} else if (rv == -EAGAIN) {
 			/* If the data socket received something meanwhile,
 			 * that is good enough: peer is still alive. */
-			if (time_after(connection->last_received,
-				jiffies - connection->meta.socket->sk->sk_rcvtimeo))
+			if (time_after(connection->last_received, pre_recv_jif))
 				continue;
 			if (ping_timeout_active) {
 				drbd_err(connection, "PingAck did not arrive in time.\n");
@@ -5594,6 +5588,10 @@ received_more:
 			set_bit(SEND_PING, &connection->flags);
 			continue;
 		} else if (rv == -EINTR) {
+			/* maybe drbd_thread_stop(): the while condition will notice.
+			 * maybe woken for send_ping: we'll send a ping above,
+			 * and change the rcvtimeo */
+			flush_signals(current);
 			continue;
 		} else {
 			drbd_err(connection, "sock_recvmsg returned %d\n", rv);
@@ -5603,8 +5601,8 @@ received_more:
 		if (received == expect && cmd == NULL) {
 			if (decode_header(connection, connection->meta.rbuf, &pi))
 				goto reconnect;
-			cmd = &asender_tbl[pi.cmd];
-			if (pi.cmd >= ARRAY_SIZE(asender_tbl) || !cmd->fn) {
+			cmd = &ack_receiver_tbl[pi.cmd];
+			if (pi.cmd >= ARRAY_SIZE(ack_receiver_tbl) || !cmd->fn) {
 				drbd_err(connection, "Unexpected meta packet %s (0x%04x)\n",
 					 cmdname(pi.cmd), pi.cmd);
 				goto disconnect;
@@ -5627,9 +5625,8 @@ received_more:
 
 			connection->last_received = jiffies;
 
-			if (cmd == &asender_tbl[P_PING_ACK]) {
-				/* restore idle timeout */
-				connection->meta.socket->sk->sk_rcvtimeo = ping_int * HZ;
+			if (cmd == &ack_receiver_tbl[P_PING_ACK]) {
+				set_idle_timeout(connection);
 				ping_timeout_active = false;
 			}
 
@@ -5638,11 +5635,6 @@ received_more:
 			expect	 = header_size;
 			cmd	 = NULL;
 		}
-		if (test_bit(SEND_PING, &connection->flags))
-			continue;
-		rv = drbd_recv_short(connection->meta.socket, buf, expect-received, MSG_DONTWAIT);
-		if (rv > 0)
-			goto received_more;
 	}
 
 	if (0) {
@@ -5654,9 +5646,41 @@ reconnect:
 disconnect:
 		conn_request_state(connection, NS(conn, C_DISCONNECTING), CS_HARD);
 	}
-	clear_bit(SIGNAL_ASENDER, &connection->flags);
 
-	drbd_info(connection, "asender terminated\n");
+	drbd_info(connection, "ack_receiver terminated\n");
 
 	return 0;
 }
+
+void drbd_send_acks_wf(struct work_struct *ws)
+{
+	struct drbd_peer_device *peer_device =
+		container_of(ws, struct drbd_peer_device, send_acks_work);
+	struct drbd_connection *connection = peer_device->connection;
+	struct drbd_device *device = peer_device->device;
+	struct net_conf *nc;
+	int tcp_cork, err;
+
+	rcu_read_lock();
+	nc = rcu_dereference(connection->net_conf);
+	tcp_cork = nc->tcp_cork;
+	rcu_read_unlock();
+
+	if (tcp_cork)
+		drbd_tcp_cork(connection->meta.socket);
+
+	err = drbd_finish_peer_reqs(device);
+	kref_put(&device->kref, drbd_destroy_device);
+	/* get is in drbd_endio_write_sec_final(). That is necessary to keep the
+	   struct work_struct send_acks_work alive, which is in the peer_device object */
+
+	if (err) {
+		conn_request_state(connection, NS(conn, C_NETWORK_FAILURE), CS_HARD);
+		return;
+	}
+
+	if (tcp_cork)
+		drbd_tcp_uncork(connection->meta.socket);
+
+	return;
+}
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index 3ae2c0086563..2255dcfebd2b 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -453,12 +453,12 @@ static void mod_rq_state(struct drbd_request *req, struct bio_and_error *m,
 		kref_get(&req->kref); /* wait for the DONE */
 
 	if (!(s & RQ_NET_SENT) && (set & RQ_NET_SENT)) {
-		/* potentially already completed in the asender thread */
+		/* potentially already completed in the ack_receiver thread */
 		if (!(s & RQ_NET_DONE)) {
 			atomic_add(req->i.size >> 9, &device->ap_in_flight);
 			set_if_null_req_not_net_done(peer_device, req);
 		}
-		if (s & RQ_NET_PENDING)
+		if (req->rq_state & RQ_NET_PENDING)
 			set_if_null_req_ack_pending(peer_device, req);
 	}
 
@@ -1095,6 +1095,24 @@ static bool do_remote_read(struct drbd_request *req)
 	return false;
 }
 
+bool drbd_should_do_remote(union drbd_dev_state s)
+{
+	return s.pdsk == D_UP_TO_DATE ||
+		(s.pdsk >= D_INCONSISTENT &&
+		 s.conn >= C_WF_BITMAP_T &&
+		 s.conn < C_AHEAD);
+	/* Before proto 96 that was >= CONNECTED instead of >= C_WF_BITMAP_T.
+	   That is equivalent since before 96 IO was frozen in the C_WF_BITMAP*
+	   states. */
+}
+
+static bool drbd_should_send_out_of_sync(union drbd_dev_state s)
+{
+	return s.conn == C_AHEAD || s.conn == C_WF_BITMAP_S;
+	/* pdsk = D_INCONSISTENT as a consequence. Protocol 96 check not necessary
+	   since we enter state C_AHEAD only if proto >= 96 */
+}
+
 /* returns number of connections (== 1, for drbd 8.4)
  * expected to actually write this data,
  * which does NOT include those that we are L_AHEAD for. */
@@ -1149,7 +1167,6 @@ drbd_submit_req_private_bio(struct drbd_request *req)
 	 * stable storage, and this is a WRITE, we may not even submit
 	 * this bio. */
 	if (get_ldev(device)) {
-		req->pre_submit_jif = jiffies;
 		if (drbd_insert_fault(device,
 				      rw == WRITE ? DRBD_FAULT_DT_WR
 				    : rw == READ  ? DRBD_FAULT_DT_RD
@@ -1293,6 +1310,7 @@ static void drbd_send_and_submit(struct drbd_device *device, struct drbd_request
 			&device->pending_master_completion[rw == WRITE]);
 	if (req->private_bio) {
 		/* needs to be marked within the same spinlock */
+		req->pre_submit_jif = jiffies;
 		list_add_tail(&req->req_pending_local,
 			&device->pending_completion[rw == WRITE]);
 		_req_mod(req, TO_BE_SUBMITTED);
@@ -1513,6 +1531,78 @@ blk_qc_t drbd_make_request(struct request_queue *q, struct bio *bio)
 	return BLK_QC_T_NONE;
 }
 
+static bool net_timeout_reached(struct drbd_request *net_req,
+		struct drbd_connection *connection,
+		unsigned long now, unsigned long ent,
+		unsigned int ko_count, unsigned int timeout)
+{
+	struct drbd_device *device = net_req->device;
+
+	if (!time_after(now, net_req->pre_send_jif + ent))
+		return false;
+
+	if (time_in_range(now, connection->last_reconnect_jif, connection->last_reconnect_jif + ent))
+		return false;
+
+	if (net_req->rq_state & RQ_NET_PENDING) {
+		drbd_warn(device, "Remote failed to finish a request within %ums > ko-count (%u) * timeout (%u * 0.1s)\n",
+			jiffies_to_msecs(now - net_req->pre_send_jif), ko_count, timeout);
+		return true;
+	}
+
+	/* We received an ACK already (or are using protocol A),
+	 * but are waiting for the epoch closing barrier ack.
+	 * Check if we sent the barrier already.  We should not blame the peer
+	 * for being unresponsive, if we did not even ask it yet. */
+	if (net_req->epoch == connection->send.current_epoch_nr) {
+		drbd_warn(device,
+			"We did not send a P_BARRIER for %ums > ko-count (%u) * timeout (%u * 0.1s); drbd kernel thread blocked?\n",
+			jiffies_to_msecs(now - net_req->pre_send_jif), ko_count, timeout);
+		return false;
+	}
+
+	/* Worst case: we may have been blocked for whatever reason, then
+	 * suddenly are able to send a lot of requests (and epoch separating
+	 * barriers) in quick succession.
+	 * The timestamp of the net_req may be much too old and not correspond
+	 * to the sending time of the relevant unack'ed barrier packet, so
+	 * would trigger a spurious timeout.  The latest barrier packet may
+	 * have a too recent timestamp to trigger the timeout, potentially miss
+	 * a timeout.  Right now we don't have a place to conveniently store
+	 * these timestamps.
+	 * But in this particular situation, the application requests are still
+	 * completed to upper layers, DRBD should still "feel" responsive.
+	 * No need yet to kill this connection, it may still recover.
+	 * If not, eventually we will have queued enough into the network for
+	 * us to block. From that point of view, the timestamp of the last sent
+	 * barrier packet is relevant enough.
+	 */
+	if (time_after(now, connection->send.last_sent_barrier_jif + ent)) {
+		drbd_warn(device, "Remote failed to answer a P_BARRIER (sent at %lu jif; now=%lu jif) within %ums > ko-count (%u) * timeout (%u * 0.1s)\n",
+			connection->send.last_sent_barrier_jif, now,
+			jiffies_to_msecs(now - connection->send.last_sent_barrier_jif), ko_count, timeout);
+		return true;
+	}
+	return false;
+}
+
+/* A request is considered timed out, if
+ * - we have some effective timeout from the configuration,
+ *   with some state restrictions applied,
+ * - the oldest request is waiting for a response from the network
+ *   resp. the local disk,
+ * - the oldest request is in fact older than the effective timeout,
+ * - the connection was established (resp. disk was attached)
+ *   for longer than the timeout already.
+ * Note that for 32bit jiffies and very stable connections/disks,
+ * we may have a wrap around, which is catched by
+ *   !time_in_range(now, last_..._jif, last_..._jif + timeout).
+ *
+ * Side effect: once per 32bit wrap-around interval, which means every
+ * ~198 days with 250 HZ, we have a window where the timeout would need
+ * to expire twice (worst case) to become effective. Good enough.
+ */
+
 void request_timer_fn(unsigned long data)
 {
 	struct drbd_device *device = (struct drbd_device *) data;
@@ -1522,11 +1612,14 @@ void request_timer_fn(unsigned long data)
 	unsigned long oldest_submit_jif;
 	unsigned long ent = 0, dt = 0, et, nt; /* effective timeout = ko_count * timeout */
 	unsigned long now;
+	unsigned int ko_count = 0, timeout = 0;
 
 	rcu_read_lock();
 	nc = rcu_dereference(connection->net_conf);
-	if (nc && device->state.conn >= C_WF_REPORT_PARAMS)
-		ent = nc->timeout * HZ/10 * nc->ko_count;
+	if (nc && device->state.conn >= C_WF_REPORT_PARAMS) {
+		ko_count = nc->ko_count;
+		timeout = nc->timeout;
+	}
 
 	if (get_ldev(device)) { /* implicit state.disk >= D_INCONSISTENT */
 		dt = rcu_dereference(device->ldev->disk_conf)->disk_timeout * HZ / 10;
@@ -1534,6 +1627,8 @@ void request_timer_fn(unsigned long data)
 	}
 	rcu_read_unlock();
 
+
+	ent = timeout * HZ/10 * ko_count;
 	et = min_not_zero(dt, ent);
 
 	if (!et)
@@ -1545,11 +1640,22 @@ void request_timer_fn(unsigned long data)
 	spin_lock_irq(&device->resource->req_lock);
 	req_read = list_first_entry_or_null(&device->pending_completion[0], struct drbd_request, req_pending_local);
 	req_write = list_first_entry_or_null(&device->pending_completion[1], struct drbd_request, req_pending_local);
-	req_peer = connection->req_not_net_done;
+
 	/* maybe the oldest request waiting for the peer is in fact still
-	 * blocking in tcp sendmsg */
-	if (!req_peer && connection->req_next && connection->req_next->pre_send_jif)
-		req_peer = connection->req_next;
+	 * blocking in tcp sendmsg.  That's ok, though, that's handled via the
+	 * socket send timeout, requesting a ping, and bumping ko-count in
+	 * we_should_drop_the_connection().
+	 */
+
+	/* check the oldest request we did successfully sent,
+	 * but which is still waiting for an ACK. */
+	req_peer = connection->req_ack_pending;
+
+	/* if we don't have such request (e.g. protocoll A)
+	 * check the oldest requests which is still waiting on its epoch
+	 * closing barrier ack. */
+	if (!req_peer)
+		req_peer = connection->req_not_net_done;
 
 	/* evaluate the oldest peer request only in one timer! */
 	if (req_peer && req_peer->device != device)
@@ -1566,28 +1672,9 @@ void request_timer_fn(unsigned long data)
 		: req_write ? req_write->pre_submit_jif
 		: req_read ? req_read->pre_submit_jif : now;
 
-	/* The request is considered timed out, if
-	 * - we have some effective timeout from the configuration,
-	 *   with above state restrictions applied,
-	 * - the oldest request is waiting for a response from the network
-	 *   resp. the local disk,
-	 * - the oldest request is in fact older than the effective timeout,
-	 * - the connection was established (resp. disk was attached)
-	 *   for longer than the timeout already.
-	 * Note that for 32bit jiffies and very stable connections/disks,
-	 * we may have a wrap around, which is catched by
-	 *   !time_in_range(now, last_..._jif, last_..._jif + timeout).
-	 *
-	 * Side effect: once per 32bit wrap-around interval, which means every
-	 * ~198 days with 250 HZ, we have a window where the timeout would need
-	 * to expire twice (worst case) to become effective. Good enough.
-	 */
-	if (ent && req_peer &&
-		 time_after(now, req_peer->pre_send_jif + ent) &&
-		!time_in_range(now, connection->last_reconnect_jif, connection->last_reconnect_jif + ent)) {
-		drbd_warn(device, "Remote failed to finish a request within ko-count * timeout\n");
+	if (ent && req_peer && net_timeout_reached(req_peer, connection, now, ent, ko_count, timeout))
 		_conn_request_state(connection, NS(conn, C_TIMEOUT), CS_VERBOSE | CS_HARD);
-	}
+
 	if (dt && oldest_submit_jif != now &&
 		 time_after(now, oldest_submit_jif + dt) &&
 		!time_in_range(now, device->last_reattach_jif, device->last_reattach_jif + dt)) {
diff --git a/drivers/block/drbd/drbd_req.h b/drivers/block/drbd/drbd_req.h
index 9f6a04080e9f..bb2ef78165e5 100644
--- a/drivers/block/drbd/drbd_req.h
+++ b/drivers/block/drbd/drbd_req.h
@@ -331,21 +331,6 @@ static inline int req_mod(struct drbd_request *req,
 	return rv;
 }
 
-static inline bool drbd_should_do_remote(union drbd_dev_state s)
-{
-	return s.pdsk == D_UP_TO_DATE ||
-		(s.pdsk >= D_INCONSISTENT &&
-		 s.conn >= C_WF_BITMAP_T &&
-		 s.conn < C_AHEAD);
-	/* Before proto 96 that was >= CONNECTED instead of >= C_WF_BITMAP_T.
-	   That is equivalent since before 96 IO was frozen in the C_WF_BITMAP*
-	   states. */
-}
-static inline bool drbd_should_send_out_of_sync(union drbd_dev_state s)
-{
-	return s.conn == C_AHEAD || s.conn == C_WF_BITMAP_S;
-	/* pdsk = D_INCONSISTENT as a consequence. Protocol 96 check not necessary
-	   since we enter state C_AHEAD only if proto >= 96 */
-}
+extern bool drbd_should_do_remote(union drbd_dev_state);
 
 #endif
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index 2d7dd269b6a8..5a7ef7873b67 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -29,6 +29,7 @@
 #include "drbd_int.h"
 #include "drbd_protocol.h"
 #include "drbd_req.h"
+#include "drbd_state_change.h"
 
 struct after_state_chg_work {
 	struct drbd_work w;
@@ -37,6 +38,7 @@ struct after_state_chg_work {
 	union drbd_state ns;
 	enum chg_state_flags flags;
 	struct completion *done;
+	struct drbd_state_change *state_change;
 };
 
 enum sanitize_state_warnings {
@@ -48,9 +50,248 @@ enum sanitize_state_warnings {
 	IMPLICITLY_UPGRADED_PDSK,
 };
 
+static void count_objects(struct drbd_resource *resource,
+			  unsigned int *n_devices,
+			  unsigned int *n_connections)
+{
+	struct drbd_device *device;
+	struct drbd_connection *connection;
+	int vnr;
+
+	*n_devices = 0;
+	*n_connections = 0;
+
+	idr_for_each_entry(&resource->devices, device, vnr)
+		(*n_devices)++;
+	for_each_connection(connection, resource)
+		(*n_connections)++;
+}
+
+static struct drbd_state_change *alloc_state_change(unsigned int n_devices, unsigned int n_connections, gfp_t gfp)
+{
+	struct drbd_state_change *state_change;
+	unsigned int size, n;
+
+	size = sizeof(struct drbd_state_change) +
+	       n_devices * sizeof(struct drbd_device_state_change) +
+	       n_connections * sizeof(struct drbd_connection_state_change) +
+	       n_devices * n_connections * sizeof(struct drbd_peer_device_state_change);
+	state_change = kmalloc(size, gfp);
+	if (!state_change)
+		return NULL;
+	state_change->n_devices = n_devices;
+	state_change->n_connections = n_connections;
+	state_change->devices = (void *)(state_change + 1);
+	state_change->connections = (void *)&state_change->devices[n_devices];
+	state_change->peer_devices = (void *)&state_change->connections[n_connections];
+	state_change->resource->resource = NULL;
+	for (n = 0; n < n_devices; n++)
+		state_change->devices[n].device = NULL;
+	for (n = 0; n < n_connections; n++)
+		state_change->connections[n].connection = NULL;
+	return state_change;
+}
+
+struct drbd_state_change *remember_old_state(struct drbd_resource *resource, gfp_t gfp)
+{
+	struct drbd_state_change *state_change;
+	struct drbd_device *device;
+	unsigned int n_devices;
+	struct drbd_connection *connection;
+	unsigned int n_connections;
+	int vnr;
+
+	struct drbd_device_state_change *device_state_change;
+	struct drbd_peer_device_state_change *peer_device_state_change;
+	struct drbd_connection_state_change *connection_state_change;
+
+	/* Caller holds req_lock spinlock.
+	 * No state, no device IDR, no connections lists can change. */
+	count_objects(resource, &n_devices, &n_connections);
+	state_change = alloc_state_change(n_devices, n_connections, gfp);
+	if (!state_change)
+		return NULL;
+
+	kref_get(&resource->kref);
+	state_change->resource->resource = resource;
+	state_change->resource->role[OLD] =
+		conn_highest_role(first_connection(resource));
+	state_change->resource->susp[OLD] = resource->susp;
+	state_change->resource->susp_nod[OLD] = resource->susp_nod;
+	state_change->resource->susp_fen[OLD] = resource->susp_fen;
+
+	connection_state_change = state_change->connections;
+	for_each_connection(connection, resource) {
+		kref_get(&connection->kref);
+		connection_state_change->connection = connection;
+		connection_state_change->cstate[OLD] =
+			connection->cstate;
+		connection_state_change->peer_role[OLD] =
+			conn_highest_peer(connection);
+		connection_state_change++;
+	}
+
+	device_state_change = state_change->devices;
+	peer_device_state_change = state_change->peer_devices;
+	idr_for_each_entry(&resource->devices, device, vnr) {
+		kref_get(&device->kref);
+		device_state_change->device = device;
+		device_state_change->disk_state[OLD] = device->state.disk;
+
+		/* The peer_devices for each device have to be enumerated in
+		   the order of the connections. We may not use for_each_peer_device() here. */
+		for_each_connection(connection, resource) {
+			struct drbd_peer_device *peer_device;
+
+			peer_device = conn_peer_device(connection, device->vnr);
+			peer_device_state_change->peer_device = peer_device;
+			peer_device_state_change->disk_state[OLD] =
+				device->state.pdsk;
+			peer_device_state_change->repl_state[OLD] =
+				max_t(enum drbd_conns,
+				      C_WF_REPORT_PARAMS, device->state.conn);
+			peer_device_state_change->resync_susp_user[OLD] =
+				device->state.user_isp;
+			peer_device_state_change->resync_susp_peer[OLD] =
+				device->state.peer_isp;
+			peer_device_state_change->resync_susp_dependency[OLD] =
+				device->state.aftr_isp;
+			peer_device_state_change++;
+		}
+		device_state_change++;
+	}
+
+	return state_change;
+}
+
+static void remember_new_state(struct drbd_state_change *state_change)
+{
+	struct drbd_resource_state_change *resource_state_change;
+	struct drbd_resource *resource;
+	unsigned int n;
+
+	if (!state_change)
+		return;
+
+	resource_state_change = &state_change->resource[0];
+	resource = resource_state_change->resource;
+
+	resource_state_change->role[NEW] =
+		conn_highest_role(first_connection(resource));
+	resource_state_change->susp[NEW] = resource->susp;
+	resource_state_change->susp_nod[NEW] = resource->susp_nod;
+	resource_state_change->susp_fen[NEW] = resource->susp_fen;
+
+	for (n = 0; n < state_change->n_devices; n++) {
+		struct drbd_device_state_change *device_state_change =
+			&state_change->devices[n];
+		struct drbd_device *device = device_state_change->device;
+
+		device_state_change->disk_state[NEW] = device->state.disk;
+	}
+
+	for (n = 0; n < state_change->n_connections; n++) {
+		struct drbd_connection_state_change *connection_state_change =
+			&state_change->connections[n];
+		struct drbd_connection *connection =
+			connection_state_change->connection;
+
+		connection_state_change->cstate[NEW] = connection->cstate;
+		connection_state_change->peer_role[NEW] =
+			conn_highest_peer(connection);
+	}
+
+	for (n = 0; n < state_change->n_devices * state_change->n_connections; n++) {
+		struct drbd_peer_device_state_change *peer_device_state_change =
+			&state_change->peer_devices[n];
+		struct drbd_device *device =
+			peer_device_state_change->peer_device->device;
+		union drbd_dev_state state = device->state;
+
+		peer_device_state_change->disk_state[NEW] = state.pdsk;
+		peer_device_state_change->repl_state[NEW] =
+			max_t(enum drbd_conns, C_WF_REPORT_PARAMS, state.conn);
+		peer_device_state_change->resync_susp_user[NEW] =
+			state.user_isp;
+		peer_device_state_change->resync_susp_peer[NEW] =
+			state.peer_isp;
+		peer_device_state_change->resync_susp_dependency[NEW] =
+			state.aftr_isp;
+	}
+}
+
+void copy_old_to_new_state_change(struct drbd_state_change *state_change)
+{
+	struct drbd_resource_state_change *resource_state_change = &state_change->resource[0];
+	unsigned int n_device, n_connection, n_peer_device, n_peer_devices;
+
+#define OLD_TO_NEW(x) \
+	(x[NEW] = x[OLD])
+
+	OLD_TO_NEW(resource_state_change->role);
+	OLD_TO_NEW(resource_state_change->susp);
+	OLD_TO_NEW(resource_state_change->susp_nod);
+	OLD_TO_NEW(resource_state_change->susp_fen);
+
+	for (n_connection = 0; n_connection < state_change->n_connections; n_connection++) {
+		struct drbd_connection_state_change *connection_state_change =
+				&state_change->connections[n_connection];
+
+		OLD_TO_NEW(connection_state_change->peer_role);
+		OLD_TO_NEW(connection_state_change->cstate);
+	}
+
+	for (n_device = 0; n_device < state_change->n_devices; n_device++) {
+		struct drbd_device_state_change *device_state_change =
+			&state_change->devices[n_device];
+
+		OLD_TO_NEW(device_state_change->disk_state);
+	}
+
+	n_peer_devices = state_change->n_devices * state_change->n_connections;
+	for (n_peer_device = 0; n_peer_device < n_peer_devices; n_peer_device++) {
+		struct drbd_peer_device_state_change *p =
+			&state_change->peer_devices[n_peer_device];
+
+		OLD_TO_NEW(p->disk_state);
+		OLD_TO_NEW(p->repl_state);
+		OLD_TO_NEW(p->resync_susp_user);
+		OLD_TO_NEW(p->resync_susp_peer);
+		OLD_TO_NEW(p->resync_susp_dependency);
+	}
+
+#undef OLD_TO_NEW
+}
+
+void forget_state_change(struct drbd_state_change *state_change)
+{
+	unsigned int n;
+
+	if (!state_change)
+		return;
+
+	if (state_change->resource->resource)
+		kref_put(&state_change->resource->resource->kref, drbd_destroy_resource);
+	for (n = 0; n < state_change->n_devices; n++) {
+		struct drbd_device *device = state_change->devices[n].device;
+
+		if (device)
+			kref_put(&device->kref, drbd_destroy_device);
+	}
+	for (n = 0; n < state_change->n_connections; n++) {
+		struct drbd_connection *connection =
+			state_change->connections[n].connection;
+
+		if (connection)
+			kref_put(&connection->kref, drbd_destroy_connection);
+	}
+	kfree(state_change);
+}
+
 static int w_after_state_ch(struct drbd_work *w, int unused);
 static void after_state_ch(struct drbd_device *device, union drbd_state os,
-			   union drbd_state ns, enum chg_state_flags flags);
+			   union drbd_state ns, enum chg_state_flags flags,
+			   struct drbd_state_change *);
 static enum drbd_state_rv is_valid_state(struct drbd_device *, union drbd_state);
 static enum drbd_state_rv is_valid_soft_transition(union drbd_state, union drbd_state, struct drbd_connection *);
 static enum drbd_state_rv is_valid_transition(union drbd_state os, union drbd_state ns);
@@ -93,6 +334,7 @@ static enum drbd_role max_role(enum drbd_role role1, enum drbd_role role2)
 		return R_SECONDARY;
 	return R_UNKNOWN;
 }
+
 static enum drbd_role min_role(enum drbd_role role1, enum drbd_role role2)
 {
 	if (role1 == R_UNKNOWN || role2 == R_UNKNOWN)
@@ -937,7 +1179,7 @@ void drbd_resume_al(struct drbd_device *device)
 		drbd_info(device, "Resumed AL updates\n");
 }
 
-/* helper for __drbd_set_state */
+/* helper for _drbd_set_state */
 static void set_ov_position(struct drbd_device *device, enum drbd_conns cs)
 {
 	if (first_peer_device(device)->connection->agreed_pro_version < 90)
@@ -965,17 +1207,17 @@ static void set_ov_position(struct drbd_device *device, enum drbd_conns cs)
 }
 
 /**
- * __drbd_set_state() - Set a new DRBD state
+ * _drbd_set_state() - Set a new DRBD state
  * @device:	DRBD device.
  * @ns:		new state.
  * @flags:	Flags
  * @done:	Optional completion, that will get completed after the after_state_ch() finished
  *
- * Caller needs to hold req_lock, and global_state_lock. Do not call directly.
+ * Caller needs to hold req_lock. Do not call directly.
  */
 enum drbd_state_rv
-__drbd_set_state(struct drbd_device *device, union drbd_state ns,
-	         enum chg_state_flags flags, struct completion *done)
+_drbd_set_state(struct drbd_device *device, union drbd_state ns,
+	        enum chg_state_flags flags, struct completion *done)
 {
 	struct drbd_peer_device *peer_device = first_peer_device(device);
 	struct drbd_connection *connection = peer_device ? peer_device->connection : NULL;
@@ -983,6 +1225,7 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 	enum drbd_state_rv rv = SS_SUCCESS;
 	enum sanitize_state_warnings ssw;
 	struct after_state_chg_work *ascw;
+	struct drbd_state_change *state_change;
 
 	os = drbd_read_state(device);
 
@@ -1037,6 +1280,9 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 	if (!is_sync_state(os.conn) && is_sync_state(ns.conn))
 		clear_bit(RS_DONE, &device->flags);
 
+	/* FIXME: Have any flags been set earlier in this function already? */
+	state_change = remember_old_state(device->resource, GFP_ATOMIC);
+
 	/* changes to local_cnt and device flags should be visible before
 	 * changes to state, which again should be visible before anything else
 	 * depending on that change happens. */
@@ -1047,6 +1293,8 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 	device->resource->susp_fen = ns.susp_fen;
 	smp_wmb();
 
+	remember_new_state(state_change);
+
 	/* put replicated vs not-replicated requests in seperate epochs */
 	if (drbd_should_do_remote((union drbd_dev_state)os.i) !=
 	    drbd_should_do_remote((union drbd_dev_state)ns.i))
@@ -1184,6 +1432,7 @@ __drbd_set_state(struct drbd_device *device, union drbd_state ns,
 		ascw->w.cb = w_after_state_ch;
 		ascw->device = device;
 		ascw->done = done;
+		ascw->state_change = state_change;
 		drbd_queue_work(&connection->sender_work,
 				&ascw->w);
 	} else {
@@ -1199,7 +1448,8 @@ static int w_after_state_ch(struct drbd_work *w, int unused)
 		container_of(w, struct after_state_chg_work, w);
 	struct drbd_device *device = ascw->device;
 
-	after_state_ch(device, ascw->os, ascw->ns, ascw->flags);
+	after_state_ch(device, ascw->os, ascw->ns, ascw->flags, ascw->state_change);
+	forget_state_change(ascw->state_change);
 	if (ascw->flags & CS_WAIT_COMPLETE)
 		complete(ascw->done);
 	kfree(ascw);
@@ -1234,7 +1484,7 @@ int drbd_bitmap_io_from_worker(struct drbd_device *device,
 	D_ASSERT(device, current == first_peer_device(device)->connection->worker.task);
 
 	/* open coded non-blocking drbd_suspend_io(device); */
-	set_bit(SUSPEND_IO, &device->flags);
+	atomic_inc(&device->suspend_cnt);
 
 	drbd_bm_lock(device, why, flags);
 	rv = io_fn(device);
@@ -1245,6 +1495,139 @@ int drbd_bitmap_io_from_worker(struct drbd_device *device,
 	return rv;
 }
 
+void notify_resource_state_change(struct sk_buff *skb,
+				  unsigned int seq,
+				  struct drbd_resource_state_change *resource_state_change,
+				  enum drbd_notification_type type)
+{
+	struct drbd_resource *resource = resource_state_change->resource;
+	struct resource_info resource_info = {
+		.res_role = resource_state_change->role[NEW],
+		.res_susp = resource_state_change->susp[NEW],
+		.res_susp_nod = resource_state_change->susp_nod[NEW],
+		.res_susp_fen = resource_state_change->susp_fen[NEW],
+	};
+
+	notify_resource_state(skb, seq, resource, &resource_info, type);
+}
+
+void notify_connection_state_change(struct sk_buff *skb,
+				    unsigned int seq,
+				    struct drbd_connection_state_change *connection_state_change,
+				    enum drbd_notification_type type)
+{
+	struct drbd_connection *connection = connection_state_change->connection;
+	struct connection_info connection_info = {
+		.conn_connection_state = connection_state_change->cstate[NEW],
+		.conn_role = connection_state_change->peer_role[NEW],
+	};
+
+	notify_connection_state(skb, seq, connection, &connection_info, type);
+}
+
+void notify_device_state_change(struct sk_buff *skb,
+				unsigned int seq,
+				struct drbd_device_state_change *device_state_change,
+				enum drbd_notification_type type)
+{
+	struct drbd_device *device = device_state_change->device;
+	struct device_info device_info = {
+		.dev_disk_state = device_state_change->disk_state[NEW],
+	};
+
+	notify_device_state(skb, seq, device, &device_info, type);
+}
+
+void notify_peer_device_state_change(struct sk_buff *skb,
+				     unsigned int seq,
+				     struct drbd_peer_device_state_change *p,
+				     enum drbd_notification_type type)
+{
+	struct drbd_peer_device *peer_device = p->peer_device;
+	struct peer_device_info peer_device_info = {
+		.peer_repl_state = p->repl_state[NEW],
+		.peer_disk_state = p->disk_state[NEW],
+		.peer_resync_susp_user = p->resync_susp_user[NEW],
+		.peer_resync_susp_peer = p->resync_susp_peer[NEW],
+		.peer_resync_susp_dependency = p->resync_susp_dependency[NEW],
+	};
+
+	notify_peer_device_state(skb, seq, peer_device, &peer_device_info, type);
+}
+
+static void broadcast_state_change(struct drbd_state_change *state_change)
+{
+	struct drbd_resource_state_change *resource_state_change = &state_change->resource[0];
+	bool resource_state_has_changed;
+	unsigned int n_device, n_connection, n_peer_device, n_peer_devices;
+	void (*last_func)(struct sk_buff *, unsigned int, void *,
+			  enum drbd_notification_type) = NULL;
+	void *uninitialized_var(last_arg);
+
+#define HAS_CHANGED(state) ((state)[OLD] != (state)[NEW])
+#define FINAL_STATE_CHANGE(type) \
+	({ if (last_func) \
+		last_func(NULL, 0, last_arg, type); \
+	})
+#define REMEMBER_STATE_CHANGE(func, arg, type) \
+	({ FINAL_STATE_CHANGE(type | NOTIFY_CONTINUES); \
+	   last_func = (typeof(last_func))func; \
+	   last_arg = arg; \
+	 })
+
+	mutex_lock(&notification_mutex);
+
+	resource_state_has_changed =
+	    HAS_CHANGED(resource_state_change->role) ||
+	    HAS_CHANGED(resource_state_change->susp) ||
+	    HAS_CHANGED(resource_state_change->susp_nod) ||
+	    HAS_CHANGED(resource_state_change->susp_fen);
+
+	if (resource_state_has_changed)
+		REMEMBER_STATE_CHANGE(notify_resource_state_change,
+				      resource_state_change, NOTIFY_CHANGE);
+
+	for (n_connection = 0; n_connection < state_change->n_connections; n_connection++) {
+		struct drbd_connection_state_change *connection_state_change =
+				&state_change->connections[n_connection];
+
+		if (HAS_CHANGED(connection_state_change->peer_role) ||
+		    HAS_CHANGED(connection_state_change->cstate))
+			REMEMBER_STATE_CHANGE(notify_connection_state_change,
+					      connection_state_change, NOTIFY_CHANGE);
+	}
+
+	for (n_device = 0; n_device < state_change->n_devices; n_device++) {
+		struct drbd_device_state_change *device_state_change =
+			&state_change->devices[n_device];
+
+		if (HAS_CHANGED(device_state_change->disk_state))
+			REMEMBER_STATE_CHANGE(notify_device_state_change,
+					      device_state_change, NOTIFY_CHANGE);
+	}
+
+	n_peer_devices = state_change->n_devices * state_change->n_connections;
+	for (n_peer_device = 0; n_peer_device < n_peer_devices; n_peer_device++) {
+		struct drbd_peer_device_state_change *p =
+			&state_change->peer_devices[n_peer_device];
+
+		if (HAS_CHANGED(p->disk_state) ||
+		    HAS_CHANGED(p->repl_state) ||
+		    HAS_CHANGED(p->resync_susp_user) ||
+		    HAS_CHANGED(p->resync_susp_peer) ||
+		    HAS_CHANGED(p->resync_susp_dependency))
+			REMEMBER_STATE_CHANGE(notify_peer_device_state_change,
+					      p, NOTIFY_CHANGE);
+	}
+
+	FINAL_STATE_CHANGE(NOTIFY_CHANGE);
+	mutex_unlock(&notification_mutex);
+
+#undef HAS_CHANGED
+#undef FINAL_STATE_CHANGE
+#undef REMEMBER_STATE_CHANGE
+}
+
 /**
  * after_state_ch() - Perform after state change actions that may sleep
  * @device:	DRBD device.
@@ -1253,13 +1636,16 @@ int drbd_bitmap_io_from_worker(struct drbd_device *device,
  * @flags:	Flags
  */
 static void after_state_ch(struct drbd_device *device, union drbd_state os,
-			   union drbd_state ns, enum chg_state_flags flags)
+			   union drbd_state ns, enum chg_state_flags flags,
+			   struct drbd_state_change *state_change)
 {
 	struct drbd_resource *resource = device->resource;
 	struct drbd_peer_device *peer_device = first_peer_device(device);
 	struct drbd_connection *connection = peer_device ? peer_device->connection : NULL;
 	struct sib_info sib;
 
+	broadcast_state_change(state_change);
+
 	sib.sib_reason = SIB_STATE_CHANGE;
 	sib.os = os;
 	sib.ns = ns;
@@ -1377,7 +1763,7 @@ static void after_state_ch(struct drbd_device *device, union drbd_state os,
 	}
 
 	if (ns.pdsk < D_INCONSISTENT && get_ldev(device)) {
-		if (os.peer == R_SECONDARY && ns.peer == R_PRIMARY &&
+		if (os.peer != R_PRIMARY && ns.peer == R_PRIMARY &&
 		    device->ldev->md.uuid[UI_BITMAP] == 0 && ns.disk >= D_UP_TO_DATE) {
 			drbd_uuid_new_current(device);
 			drbd_send_uuids(peer_device);
@@ -1444,7 +1830,7 @@ static void after_state_ch(struct drbd_device *device, union drbd_state os,
 	if (os.disk != D_FAILED && ns.disk == D_FAILED) {
 		enum drbd_io_error_p eh = EP_PASS_ON;
 		int was_io_error = 0;
-		/* corresponding get_ldev was in __drbd_set_state, to serialize
+		/* corresponding get_ldev was in _drbd_set_state, to serialize
 		 * our cleanup here with the transition to D_DISKLESS.
 		 * But is is still not save to dreference ldev here, since
 		 * we might come from an failed Attach before ldev was set. */
@@ -1455,6 +1841,10 @@ static void after_state_ch(struct drbd_device *device, union drbd_state os,
 
 			was_io_error = test_and_clear_bit(WAS_IO_ERROR, &device->flags);
 
+			/* Intentionally call this handler first, before drbd_send_state().
+			 * See: 2932204 drbd: call local-io-error handler early
+			 * People may chose to hard-reset the box from this handler.
+			 * It is useful if this looks like a "regular node crash". */
 			if (was_io_error && eh == EP_CALL_HELPER)
 				drbd_khelper(device, "local-io-error");
 
@@ -1572,6 +1962,7 @@ struct after_conn_state_chg_work {
 	union drbd_state ns_max; /* new, max state, over all devices */
 	enum chg_state_flags flags;
 	struct drbd_connection *connection;
+	struct drbd_state_change *state_change;
 };
 
 static int w_after_conn_state_ch(struct drbd_work *w, int unused)
@@ -1584,6 +1975,8 @@ static int w_after_conn_state_ch(struct drbd_work *w, int unused)
 	struct drbd_peer_device *peer_device;
 	int vnr;
 
+	broadcast_state_change(acscw->state_change);
+	forget_state_change(acscw->state_change);
 	kfree(acscw);
 
 	/* Upon network configuration, we need to start the receiver */
@@ -1593,6 +1986,13 @@ static int w_after_conn_state_ch(struct drbd_work *w, int unused)
 	if (oc == C_DISCONNECTING && ns_max.conn == C_STANDALONE) {
 		struct net_conf *old_conf;
 
+		mutex_lock(&notification_mutex);
+		idr_for_each_entry(&connection->peer_devices, peer_device, vnr)
+			notify_peer_device_state(NULL, 0, peer_device, NULL,
+						 NOTIFY_DESTROY | NOTIFY_CONTINUES);
+		notify_connection_state(NULL, 0, connection, NULL, NOTIFY_DESTROY);
+		mutex_unlock(&notification_mutex);
+
 		mutex_lock(&connection->resource->conf_update);
 		old_conf = connection->net_conf;
 		connection->my_addr_len = 0;
@@ -1759,7 +2159,7 @@ conn_set_state(struct drbd_connection *connection, union drbd_state mask, union
 		if (flags & CS_IGN_OUTD_FAIL && ns.disk == D_OUTDATED && os.disk < D_OUTDATED)
 			ns.disk = os.disk;
 
-		rv = __drbd_set_state(device, ns, flags, NULL);
+		rv = _drbd_set_state(device, ns, flags, NULL);
 		if (rv < SS_SUCCESS)
 			BUG();
 
@@ -1823,6 +2223,7 @@ _conn_request_state(struct drbd_connection *connection, union drbd_state mask, u
 	enum drbd_conns oc = connection->cstate;
 	union drbd_state ns_max, ns_min, os;
 	bool have_mutex = false;
+	struct drbd_state_change *state_change;
 
 	if (mask.conn) {
 		rv = is_valid_conn_transition(oc, val.conn);
@@ -1868,10 +2269,12 @@ _conn_request_state(struct drbd_connection *connection, union drbd_state mask, u
 			goto abort;
 	}
 
+	state_change = remember_old_state(connection->resource, GFP_ATOMIC);
 	conn_old_common_state(connection, &os, &flags);
 	flags |= CS_DC_SUSP;
 	conn_set_state(connection, mask, val, &ns_min, &ns_max, flags);
 	conn_pr_state_change(connection, os, ns_max, flags);
+	remember_new_state(state_change);
 
 	acscw = kmalloc(sizeof(*acscw), GFP_ATOMIC);
 	if (acscw) {
@@ -1882,6 +2285,7 @@ _conn_request_state(struct drbd_connection *connection, union drbd_state mask, u
 		acscw->w.cb = w_after_conn_state_ch;
 		kref_get(&connection->kref);
 		acscw->connection = connection;
+		acscw->state_change = state_change;
 		drbd_queue_work(&connection->sender_work, &acscw->w);
 	} else {
 		drbd_err(connection, "Could not kmalloc an acscw\n");
diff --git a/drivers/block/drbd/drbd_state.h b/drivers/block/drbd/drbd_state.h
index 7f53c40823cd..bd989536f888 100644
--- a/drivers/block/drbd/drbd_state.h
+++ b/drivers/block/drbd/drbd_state.h
@@ -122,9 +122,9 @@ extern enum drbd_state_rv
 _drbd_request_state_holding_state_mutex(struct drbd_device *, union drbd_state,
 					union drbd_state, enum chg_state_flags);
 
-extern enum drbd_state_rv __drbd_set_state(struct drbd_device *, union drbd_state,
-					   enum chg_state_flags,
-					   struct completion *done);
+extern enum drbd_state_rv _drbd_set_state(struct drbd_device *, union drbd_state,
+					  enum chg_state_flags,
+					  struct completion *done);
 extern void print_st_err(struct drbd_device *, union drbd_state,
 			union drbd_state, int);
 
diff --git a/drivers/block/drbd/drbd_state_change.h b/drivers/block/drbd/drbd_state_change.h
new file mode 100644
index 000000000000..9e503a1a0bfb
--- /dev/null
+++ b/drivers/block/drbd/drbd_state_change.h
@@ -0,0 +1,63 @@
+#ifndef DRBD_STATE_CHANGE_H
+#define DRBD_STATE_CHANGE_H
+
+struct drbd_resource_state_change {
+	struct drbd_resource *resource;
+	enum drbd_role role[2];
+	bool susp[2];
+	bool susp_nod[2];
+	bool susp_fen[2];
+};
+
+struct drbd_device_state_change {
+	struct drbd_device *device;
+	enum drbd_disk_state disk_state[2];
+};
+
+struct drbd_connection_state_change {
+	struct drbd_connection *connection;
+	enum drbd_conns cstate[2];  /* drbd9: enum drbd_conn_state */
+	enum drbd_role peer_role[2];
+};
+
+struct drbd_peer_device_state_change {
+	struct drbd_peer_device *peer_device;
+	enum drbd_disk_state disk_state[2];
+	enum drbd_conns repl_state[2];  /* drbd9: enum drbd_repl_state */
+	bool resync_susp_user[2];
+	bool resync_susp_peer[2];
+	bool resync_susp_dependency[2];
+};
+
+struct drbd_state_change {
+	struct list_head list;
+	unsigned int n_devices;
+	unsigned int n_connections;
+	struct drbd_resource_state_change resource[1];
+	struct drbd_device_state_change *devices;
+	struct drbd_connection_state_change *connections;
+	struct drbd_peer_device_state_change *peer_devices;
+};
+
+extern struct drbd_state_change *remember_old_state(struct drbd_resource *, gfp_t);
+extern void copy_old_to_new_state_change(struct drbd_state_change *);
+extern void forget_state_change(struct drbd_state_change *);
+
+extern void notify_resource_state_change(struct sk_buff *,
+					 unsigned int,
+					 struct drbd_resource_state_change *,
+					 enum drbd_notification_type type);
+extern void notify_connection_state_change(struct sk_buff *,
+					   unsigned int,
+					   struct drbd_connection_state_change *,
+					   enum drbd_notification_type type);
+extern void notify_device_state_change(struct sk_buff *,
+				       unsigned int,
+				       struct drbd_device_state_change *,
+				       enum drbd_notification_type type);
+extern void notify_peer_device_state_change(struct sk_buff *,
+					    unsigned int,
+					    struct drbd_peer_device_state_change *,
+					    enum drbd_notification_type type);
+
+#endif  /* DRBD_STATE_CHANGE_H */
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 5578c1477ba6..eff716c27b1f 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -55,13 +55,6 @@ static int make_resync_request(struct drbd_device *, int);
  *
  */
 
-
-/* About the global_state_lock
-   Each state transition on an device holds a read lock. In case we have
-   to evaluate the resync after dependencies, we grab a write lock, because
-   we need stable states on all devices for that.  */
-rwlock_t global_state_lock;
-
 /* used for synchronous meta data and bitmap IO
  * submitted by drbd_md_sync_page_io()
  */
@@ -120,6 +113,7 @@ void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(l
 	unsigned long flags = 0;
 	struct drbd_peer_device *peer_device = peer_req->peer_device;
 	struct drbd_device *device = peer_device->device;
+	struct drbd_connection *connection = peer_device->connection;
 	struct drbd_interval i;
 	int do_wake;
 	u64 block_id;
@@ -152,6 +146,12 @@ void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(l
 	 * ((peer_req->flags & (EE_WAS_ERROR|EE_IS_TRIM)) == EE_WAS_ERROR) */
 	if (peer_req->flags & EE_WAS_ERROR)
 		__drbd_chk_io_error(device, DRBD_WRITE_ERROR);
+
+	if (connection->cstate >= C_WF_REPORT_PARAMS) {
+		kref_get(&device->kref); /* put is in drbd_send_acks_wf() */
+		if (!queue_work(connection->ack_sender, &peer_device->send_acks_work))
+			kref_put(&device->kref, drbd_destroy_device);
+	}
 	spin_unlock_irqrestore(&device->resource->req_lock, flags);
 
 	if (block_id == ID_SYNCER)
@@ -163,7 +163,6 @@ void drbd_endio_write_sec_final(struct drbd_peer_request *peer_req) __releases(l
 	if (do_al_complete_io)
 		drbd_al_complete_io(device, &i);
 
-	wake_asender(peer_device->connection);
 	put_ldev(device);
 }
 
@@ -195,6 +194,12 @@ void drbd_peer_request_endio(struct bio *bio)
 	}
 }
 
+void drbd_panic_after_delayed_completion_of_aborted_request(struct drbd_device *device)
+{
+	panic("drbd%u %s/%u potential random memory corruption caused by delayed completion of aborted local request\n",
+		device->minor, device->resource->name, device->vnr);
+}
+
 /* read, readA or write requests on R_PRIMARY coming from drbd_make_request
  */
 void drbd_request_endio(struct bio *bio)
@@ -238,7 +243,7 @@ void drbd_request_endio(struct bio *bio)
 			drbd_emerg(device, "delayed completion of aborted local request; disk-timeout may be too aggressive\n");
 
 		if (!bio->bi_error)
-			panic("possible random memory corruption caused by delayed completion of aborted local request\n");
+			drbd_panic_after_delayed_completion_of_aborted_request(device);
 	}
 
 	/* to avoid recursion in __req_mod */
@@ -1291,6 +1296,7 @@ static int drbd_send_barrier(struct drbd_connection *connection)
 	p->barrier = connection->send.current_epoch_nr;
 	p->pad = 0;
 	connection->send.current_epoch_writes = 0;
+	connection->send.last_sent_barrier_jif = jiffies;
 
 	return conn_send_command(connection, sock, P_BARRIER, sizeof(*p), NULL, 0);
 }
@@ -1315,6 +1321,7 @@ static void re_init_if_first_write(struct drbd_connection *connection, unsigned
 		connection->send.seen_any_write_yet = true;
 		connection->send.current_epoch_nr = epoch;
 		connection->send.current_epoch_writes = 0;
+		connection->send.last_sent_barrier_jif = jiffies;
 	}
 }
 
@@ -1456,70 +1463,73 @@ static int _drbd_may_sync_now(struct drbd_device *device)
 }
 
 /**
- * _drbd_pause_after() - Pause resync on all devices that may not resync now
+ * drbd_pause_after() - Pause resync on all devices that may not resync now
  * @device:	DRBD device.
  *
  * Called from process context only (admin command and after_state_ch).
  */
-static int _drbd_pause_after(struct drbd_device *device)
+static bool drbd_pause_after(struct drbd_device *device)
 {
+	bool changed = false;
 	struct drbd_device *odev;
-	int i, rv = 0;
+	int i;
 
 	rcu_read_lock();
 	idr_for_each_entry(&drbd_devices, odev, i) {
 		if (odev->state.conn == C_STANDALONE && odev->state.disk == D_DISKLESS)
 			continue;
-		if (!_drbd_may_sync_now(odev))
-			rv |= (__drbd_set_state(_NS(odev, aftr_isp, 1), CS_HARD, NULL)
-			       != SS_NOTHING_TO_DO);
+		if (!_drbd_may_sync_now(odev) &&
+		    _drbd_set_state(_NS(odev, aftr_isp, 1),
+				    CS_HARD, NULL) != SS_NOTHING_TO_DO)
+			changed = true;
 	}
 	rcu_read_unlock();
 
-	return rv;
+	return changed;
 }
 
 /**
- * _drbd_resume_next() - Resume resync on all devices that may resync now
+ * drbd_resume_next() - Resume resync on all devices that may resync now
  * @device:	DRBD device.
  *
  * Called from process context only (admin command and worker).
  */
-static int _drbd_resume_next(struct drbd_device *device)
+static bool drbd_resume_next(struct drbd_device *device)
 {
+	bool changed = false;
 	struct drbd_device *odev;
-	int i, rv = 0;
+	int i;
 
 	rcu_read_lock();
 	idr_for_each_entry(&drbd_devices, odev, i) {
 		if (odev->state.conn == C_STANDALONE && odev->state.disk == D_DISKLESS)
 			continue;
 		if (odev->state.aftr_isp) {
-			if (_drbd_may_sync_now(odev))
-				rv |= (__drbd_set_state(_NS(odev, aftr_isp, 0),
-							CS_HARD, NULL)
-				       != SS_NOTHING_TO_DO) ;
+			if (_drbd_may_sync_now(odev) &&
+			    _drbd_set_state(_NS(odev, aftr_isp, 0),
+					    CS_HARD, NULL) != SS_NOTHING_TO_DO)
+				changed = true;
 		}
 	}
 	rcu_read_unlock();
-	return rv;
+	return changed;
 }
 
 void resume_next_sg(struct drbd_device *device)
 {
-	write_lock_irq(&global_state_lock);
-	_drbd_resume_next(device);
-	write_unlock_irq(&global_state_lock);
+	lock_all_resources();
+	drbd_resume_next(device);
+	unlock_all_resources();
 }
 
 void suspend_other_sg(struct drbd_device *device)
 {
-	write_lock_irq(&global_state_lock);
-	_drbd_pause_after(device);
-	write_unlock_irq(&global_state_lock);
+	lock_all_resources();
+	drbd_pause_after(device);
+	unlock_all_resources();
 }
 
-/* caller must hold global_state_lock */
+/* caller must lock_all_resources() */
 enum drbd_ret_code drbd_resync_after_valid(struct drbd_device *device, int o_minor)
 {
 	struct drbd_device *odev;
@@ -1557,15 +1567,15 @@ enum drbd_ret_code drbd_resync_after_valid(struct drbd_device *device, int o_min
 	}
 }
 
-/* caller must hold global_state_lock */
+/* caller must lock_all_resources() */
 void drbd_resync_after_changed(struct drbd_device *device)
 {
-	int changes;
+	int changed;
 
 	do {
-		changes  = _drbd_pause_after(device);
-		changes |= _drbd_resume_next(device);
-	} while (changes);
+		changed  = drbd_pause_after(device);
+		changed |= drbd_resume_next(device);
+	} while (changed);
 }
 
 void drbd_rs_controller_reset(struct drbd_device *device)
@@ -1685,19 +1695,14 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	} else {
 		mutex_lock(device->state_mutex);
 	}
-	clear_bit(B_RS_H_DONE, &device->flags);
 
-	/* req_lock: serialize with drbd_send_and_submit() and others
-	 * global_state_lock: for stable sync-after dependencies */
-	spin_lock_irq(&device->resource->req_lock);
-	write_lock(&global_state_lock);
+	lock_all_resources();
+	clear_bit(B_RS_H_DONE, &device->flags);
 	/* Did some connection breakage or IO error race with us? */
 	if (device->state.conn < C_CONNECTED
 	|| !get_ldev_if_state(device, D_NEGOTIATING)) {
-		write_unlock(&global_state_lock);
-		spin_unlock_irq(&device->resource->req_lock);
-		mutex_unlock(device->state_mutex);
-		return;
+		unlock_all_resources();
+		goto out;
 	}
 
 	ns = drbd_read_state(device);
@@ -1711,7 +1716,7 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 	else /* side == C_SYNC_SOURCE */
 		ns.pdsk = D_INCONSISTENT;
 
-	r = __drbd_set_state(device, ns, CS_VERBOSE, NULL);
+	r = _drbd_set_state(device, ns, CS_VERBOSE, NULL);
 	ns = drbd_read_state(device);
 
 	if (ns.conn < C_CONNECTED)
@@ -1732,7 +1737,7 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 			device->rs_mark_left[i] = tw;
 			device->rs_mark_time[i] = now;
 		}
-		_drbd_pause_after(device);
+		drbd_pause_after(device);
 		/* Forget potentially stale cached per resync extent bit-counts.
 		 * Open coded drbd_rs_cancel_all(device), we already have IRQs
 		 * disabled, and know the disk state is ok. */
@@ -1742,8 +1747,7 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 		device->resync_wenr = LC_FREE;
 		spin_unlock(&device->al_lock);
 	}
-	write_unlock(&global_state_lock);
-	spin_unlock_irq(&device->resource->req_lock);
+	unlock_all_resources();
 
 	if (r == SS_SUCCESS) {
 		wake_up(&device->al_wait); /* for lc_reset() above */
@@ -1807,6 +1811,7 @@ void drbd_start_resync(struct drbd_device *device, enum drbd_conns side)
 		drbd_md_sync(device);
 	}
 	put_ldev(device);
+out:
 	mutex_unlock(device->state_mutex);
 }
 
@@ -1836,7 +1841,7 @@ static void drbd_ldev_destroy(struct drbd_device *device)
 	device->act_log = NULL;
 
 	__acquire(local);
-	drbd_free_ldev(device->ldev);
+	drbd_backing_dev_free(device, device->ldev);
 	device->ldev = NULL;
 	__release(local);
 
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 331363e7de0f..84708a5f8c52 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -866,7 +866,7 @@ static void set_fdc(int drive)
 }
 
 /* locks the driver */
-static int lock_fdc(int drive, bool interruptible)
+static int lock_fdc(int drive)
 {
 	if (WARN(atomic_read(&usage_count) == 0,
 		 "Trying to lock fdc while usage count=0\n"))
@@ -2173,7 +2173,7 @@ static int do_format(int drive, struct format_descr *tmp_format_req)
 {
 	int ret;
 
-	if (lock_fdc(drive, true))
+	if (lock_fdc(drive))
 		return -EINTR;
 
 	set_floppy(drive);
@@ -2960,7 +2960,7 @@ static int user_reset_fdc(int drive, int arg, bool interruptible)
 {
 	int ret;
 
-	if (lock_fdc(drive, interruptible))
+	if (lock_fdc(drive))
 		return -EINTR;
 
 	if (arg == FD_RESET_ALWAYS)
@@ -3243,7 +3243,7 @@ static int set_geometry(unsigned int cmd, struct floppy_struct *g,
 		if (!capable(CAP_SYS_ADMIN))
 			return -EPERM;
 		mutex_lock(&open_lock);
-		if (lock_fdc(drive, true)) {
+		if (lock_fdc(drive)) {
 			mutex_unlock(&open_lock);
 			return -EINTR;
 		}
@@ -3263,7 +3263,7 @@ static int set_geometry(unsigned int cmd, struct floppy_struct *g,
 	} else {
 		int oldStretch;
 
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		if (cmd != FDDEFPRM) {
 			/* notice a disk change immediately, else
@@ -3349,7 +3349,7 @@ static int get_floppy_geometry(int drive, int type, struct floppy_struct **g)
 	if (type)
 		*g = &floppy_type[type];
 	else {
-		if (lock_fdc(drive, false))
+		if (lock_fdc(drive))
 			return -EINTR;
 		if (poll_drive(false, 0) == -EINTR)
 			return -EINTR;
@@ -3433,7 +3433,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		if (UDRS->fd_ref != 1)
 			/* somebody else has this drive open */
 			return -EBUSY;
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 
 		/* do the actual eject. Fails on
@@ -3445,7 +3445,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		process_fd_request();
 		return ret;
 	case FDCLRPRM:
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		current_type[drive] = NULL;
 		floppy_sizes[drive] = MAX_DISK_SIZE << 1;
@@ -3467,7 +3467,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		UDP->flags &= ~FTD_MSG;
 		return 0;
 	case FDFMTBEG:
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		if (poll_drive(true, FD_RAW_NEED_DISK) == -EINTR)
 			return -EINTR;
@@ -3484,7 +3484,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		return do_format(drive, &inparam.f);
 	case FDFMTEND:
 	case FDFLUSH:
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		return invalidate_drive(bdev);
 	case FDSETEMSGTRESH:
@@ -3507,7 +3507,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		outparam = UDP;
 		break;
 	case FDPOLLDRVSTAT:
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		if (poll_drive(true, FD_RAW_NEED_DISK) == -EINTR)
 			return -EINTR;
@@ -3530,7 +3530,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 	case FDRAWCMD:
 		if (type)
 			return -EINVAL;
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		set_floppy(drive);
 		i = raw_cmd_ioctl(cmd, (void __user *)param);
@@ -3539,7 +3539,7 @@ static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
 		process_fd_request();
 		return i;
 	case FDTWADDLE:
-		if (lock_fdc(drive, true))
+		if (lock_fdc(drive))
 			return -EINTR;
 		twaddle();
 		process_fd_request();
@@ -3585,7 +3585,7 @@ static void __init config_types(void)
 		unsigned int type = UDP->cmos;
 		struct floppy_drive_params *params;
 		const char *name = NULL;
-		static char temparea[32];
+		char temparea[32];
 
 		if (type < ARRAY_SIZE(default_drive_params)) {
 			params = &default_drive_params[type].params;
@@ -3596,7 +3596,8 @@ static void __init config_types(void)
 				allowed_drive_mask &= ~(1 << drive);
 		} else {
 			params = &default_drive_params[0].params;
-			sprintf(temparea, "unknown type %d (usb?)", type);
+			snprintf(temparea, sizeof(temparea),
+				 "unknown type %d (usb?)", type);
 			name = temparea;
 		}
 		if (name) {
@@ -3662,6 +3663,11 @@ static int floppy_open(struct block_device *bdev, fmode_t mode)
 
 	opened_bdev[drive] = bdev;
 
+	if (!(mode & (FMODE_READ|FMODE_WRITE))) {
+		res = -EINVAL;
+		goto out;
+	}
+
 	res = -ENXIO;
 
 	if (!floppy_track_buffer) {
@@ -3705,21 +3711,20 @@ static int floppy_open(struct block_device *bdev, fmode_t mode)
 	if (UFDCS->rawcmd == 1)
 		UFDCS->rawcmd = 2;
 
-	if (!(mode & FMODE_NDELAY)) {
-		if (mode & (FMODE_READ|FMODE_WRITE)) {
-			UDRS->last_checked = 0;
-			clear_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags);
-			check_disk_change(bdev);
-			if (test_bit(FD_DISK_CHANGED_BIT, &UDRS->flags))
-				goto out;
-			if (test_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags))
-				goto out;
-		}
-		res = -EROFS;
-		if ((mode & FMODE_WRITE) &&
-		    !test_bit(FD_DISK_WRITABLE_BIT, &UDRS->flags))
-			goto out;
-	}
+	UDRS->last_checked = 0;
+	clear_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags);
+	check_disk_change(bdev);
+	if (test_bit(FD_DISK_CHANGED_BIT, &UDRS->flags))
+		goto out;
+	if (test_bit(FD_OPEN_SHOULD_FAIL_BIT, &UDRS->flags))
+		goto out;
+
+	res = -EROFS;
+
+	if ((mode & FMODE_WRITE) &&
+			!test_bit(FD_DISK_WRITABLE_BIT, &UDRS->flags))
+		goto out;
+
 	mutex_unlock(&open_lock);
 	mutex_unlock(&floppy_mutex);
 	return 0;
@@ -3747,7 +3752,8 @@ static unsigned int floppy_check_events(struct gendisk *disk,
 		return DISK_EVENT_MEDIA_CHANGE;
 
 	if (time_after(jiffies, UDRS->last_checked + UDP->checkfreq)) {
-		lock_fdc(drive, false);
+		if (lock_fdc(drive))
+			return -EINTR;
 		poll_drive(false, 0);
 		process_fd_request();
 	}
@@ -3846,7 +3852,9 @@ static int floppy_revalidate(struct gendisk *disk)
 			 "VFS: revalidate called on non-open device.\n"))
 			return -EFAULT;
 
-		lock_fdc(drive, false);
+		res = lock_fdc(drive);
+		if (res)
+			return res;
 		cf = (test_bit(FD_DISK_CHANGED_BIT, &UDRS->flags) ||
 		      test_bit(FD_VERIFY_BIT, &UDRS->flags));
 		if (!(cf || test_bit(drive, &fake_change) || drive_no_geom(drive))) {
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index 3457ac8c03e2..9b180dbbd03c 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -104,9 +104,9 @@
 /* Device instance number, incremented each time a device is probed. */
 static int instance;
 
-struct list_head online_list;
-struct list_head removing_list;
-spinlock_t dev_lock;
+static struct list_head online_list;
+static struct list_head removing_list;
+static spinlock_t dev_lock;
 
 /*
  * Global variable used to hold the major block device number
@@ -173,7 +173,7 @@ static struct mtip_cmd *mtip_get_int_command(struct driver_data *dd)
 {
 	struct request *rq;
 
-	rq = blk_mq_alloc_request(dd->queue, 0, __GFP_RECLAIM, true);
+	rq = blk_mq_alloc_request(dd->queue, 0, BLK_MQ_REQ_RESERVED);
 	return blk_mq_rq_to_pdu(rq);
 }
 
@@ -2029,13 +2029,10 @@ static int exec_drive_taskfile(struct driver_data *dd,
 	}
 
 	if (taskout) {
-		outbuf = kzalloc(taskout, GFP_KERNEL);
-		if (outbuf == NULL) {
-			err = -ENOMEM;
-			goto abort;
-		}
-		if (copy_from_user(outbuf, buf + outtotal, taskout)) {
-			err = -EFAULT;
+		outbuf = memdup_user(buf + outtotal, taskout);
+		if (IS_ERR(outbuf)) {
+			err = PTR_ERR(outbuf);
+			outbuf = NULL;
 			goto abort;
 		}
 		outbuf_dma = pci_map_single(dd->pdev,
@@ -2050,14 +2047,10 @@ static int exec_drive_taskfile(struct driver_data *dd,
 	}
 
 	if (taskin) {
-		inbuf = kzalloc(taskin, GFP_KERNEL);
-		if (inbuf == NULL) {
-			err = -ENOMEM;
-			goto abort;
-		}
-
-		if (copy_from_user(inbuf, buf + intotal, taskin)) {
-			err = -EFAULT;
+		inbuf = memdup_user(buf + intotal, taskin);
+		if (IS_ERR(inbuf)) {
+			err = PTR_ERR(inbuf);
+			inbuf = NULL;
 			goto abort;
 		}
 		inbuf_dma = pci_map_single(dd->pdev,
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index 93b3f99b6865..e4c5cc107934 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -827,6 +827,7 @@ static const struct block_device_operations nbd_fops =
 {
 	.owner =	THIS_MODULE,
 	.ioctl =	nbd_ioctl,
+	.compat_ioctl =	nbd_ioctl,
 };
 
 #if IS_ENABLED(CONFIG_DEBUG_FS)
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index 09e3c0d87ecc..64a7b5971b57 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -436,9 +436,8 @@ static void null_del_dev(struct nullb *nullb)
 static void null_lnvm_end_io(struct request *rq, int error)
 {
 	struct nvm_rq *rqd = rq->end_io_data;
-	struct nvm_dev *dev = rqd->dev;
 
-	dev->mt->end_io(rqd, error);
+	nvm_end_io(rqd, error);
 
 	blk_put_request(rq);
 }
@@ -449,7 +448,7 @@ static int null_lnvm_submit_io(struct nvm_dev *dev, struct nvm_rq *rqd)
 	struct request *rq;
 	struct bio *bio = rqd->bio;
 
-	rq = blk_mq_alloc_request(q, bio_rw(bio), GFP_KERNEL, 0);
+	rq = blk_mq_alloc_request(q, bio_rw(bio), 0);
 	if (IS_ERR(rq))
 		return -ENOMEM;
 
@@ -479,7 +478,7 @@ static int null_lnvm_id(struct nvm_dev *dev, struct nvm_id *id)
 	id->ver_id = 0x1;
 	id->vmnt = 0;
 	id->cgrps = 1;
-	id->cap = 0x3;
+	id->cap = 0x2;
 	id->dom = 0x1;
 
 	id->ppaf.blk_offset = 0;
@@ -495,17 +494,17 @@ static int null_lnvm_id(struct nvm_dev *dev, struct nvm_id *id)
 	id->ppaf.ch_offset = 56;
 	id->ppaf.ch_len = 8;
 
-	do_div(size, bs); /* convert size to pages */
-	do_div(size, 256); /* concert size to pgs pr blk */
+	sector_div(size, bs); /* convert size to pages */
+	size >>= 8; /* concert size to pgs pr blk */
 	grp = &id->groups[0];
 	grp->mtype = 0;
 	grp->fmtype = 0;
 	grp->num_ch = 1;
 	grp->num_pg = 256;
 	blksize = size;
-	do_div(size, (1 << 16));
+	size >>= 16;
 	grp->num_lun = size + 1;
-	do_div(blksize, grp->num_lun);
+	sector_div(blksize, grp->num_lun);
 	grp->num_blk = blksize;
 	grp->num_pln = 1;
 
@@ -708,9 +707,7 @@ static int null_add_dev(void)
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, nullb->q);
 	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, nullb->q);
 
-
 	mutex_lock(&lock);
-	list_add_tail(&nullb->list, &nullb_list);
 	nullb->index = nullb_indexes++;
 	mutex_unlock(&lock);
 
@@ -744,6 +741,10 @@ static int null_add_dev(void)
 	strncpy(disk->disk_name, nullb->disk_name, DISK_NAME_LEN);
 
 	add_disk(disk);
+
+	mutex_lock(&lock);
+	list_add_tail(&nullb->list, &nullb_list);
+	mutex_unlock(&lock);
 done:
 	return 0;
 
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 81ea69fee7ca..4a876785b68c 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -5185,8 +5185,7 @@ static int rbd_dev_probe_parent(struct rbd_device *rbd_dev, int depth)
 
 out_err:
 	rbd_dev_unparent(rbd_dev);
-	if (parent)
-		rbd_dev_destroy(parent);
+	rbd_dev_destroy(parent);
 	return ret;
 }
 
diff --git a/drivers/block/rsxx/core.c b/drivers/block/rsxx/core.c
index d8b2488aaade..34997df132e2 100644
--- a/drivers/block/rsxx/core.c
+++ b/drivers/block/rsxx/core.c
@@ -203,14 +203,11 @@ static ssize_t rsxx_cram_write(struct file *fp, const char __user *ubuf,
 	char *buf;
 	ssize_t st;
 
-	buf = kzalloc(cnt, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
+	buf = memdup_user(ubuf, cnt);
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	st = copy_from_user(buf, ubuf, cnt);
-	if (!st)
-		st = rsxx_creg_write(card, CREG_ADD_CRAM + (u32)*ppos, cnt,
-				     buf, 1);
+	st = rsxx_creg_write(card, CREG_ADD_CRAM + (u32)*ppos, cnt, buf, 1);
 	kfree(buf);
 	if (st)
 		return st;
diff --git a/drivers/block/sx8.c b/drivers/block/sx8.c
index 59c91d49b14b..ba4bfe933276 100644
--- a/drivers/block/sx8.c
+++ b/drivers/block/sx8.c
@@ -23,7 +23,7 @@
 #include <linux/workqueue.h>
 #include <linux/bitops.h>
 #include <linux/delay.h>
-#include <linux/time.h>
+#include <linux/ktime.h>
 #include <linux/hdreg.h>
 #include <linux/dma-mapping.h>
 #include <linux/completion.h>
@@ -671,16 +671,15 @@ static int carm_send_special (struct carm_host *host, carm_sspc_t func)
 static unsigned int carm_fill_sync_time(struct carm_host *host,
 					unsigned int idx, void *mem)
 {
-	struct timeval tv;
 	struct carm_msg_sync_time *st = mem;
 
-	do_gettimeofday(&tv);
+	time64_t tv = ktime_get_real_seconds();
 
 	memset(st, 0, sizeof(*st));
 	st->type	= CARM_MSG_MISC;
 	st->subtype	= MISC_SET_TIME;
 	st->handle	= cpu_to_le32(TAG_ENCODE(idx));
-	st->timestamp	= cpu_to_le32(tv.tv_sec);
+	st->timestamp	= cpu_to_le32(tv);
 
 	return sizeof(struct carm_msg_sync_time);
 }
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 41fb1a917b17..4809c1501d7e 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -84,6 +84,16 @@ MODULE_PARM_DESC(max_persistent_grants,
                  "Maximum number of grants to map persistently");
 
 /*
+ * Maximum number of rings/queues blkback supports, allow as many queues as there
+ * are CPUs if user has not specified a value.
+ */
+unsigned int xenblk_max_queues;
+module_param_named(max_queues, xenblk_max_queues, uint, 0644);
+MODULE_PARM_DESC(max_queues,
+		 "Maximum number of hardware queues per virtual disk." \
+		 "By default it is the number of online CPUs.");
+
+/*
  * Maximum order of pages to be used for the shared ring between front and
  * backend, 4KB page granularity is used.
  */
@@ -113,71 +123,71 @@ module_param(log_stats, int, 0644);
 /* Number of free pages to remove on each call to gnttab_free_pages */
 #define NUM_BATCH_FREE_PAGES 10
 
-static inline int get_free_page(struct xen_blkif *blkif, struct page **page)
+static inline int get_free_page(struct xen_blkif_ring *ring, struct page **page)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&blkif->free_pages_lock, flags);
-	if (list_empty(&blkif->free_pages)) {
-		BUG_ON(blkif->free_pages_num != 0);
-		spin_unlock_irqrestore(&blkif->free_pages_lock, flags);
+	spin_lock_irqsave(&ring->free_pages_lock, flags);
+	if (list_empty(&ring->free_pages)) {
+		BUG_ON(ring->free_pages_num != 0);
+		spin_unlock_irqrestore(&ring->free_pages_lock, flags);
 		return gnttab_alloc_pages(1, page);
 	}
-	BUG_ON(blkif->free_pages_num == 0);
-	page[0] = list_first_entry(&blkif->free_pages, struct page, lru);
+	BUG_ON(ring->free_pages_num == 0);
+	page[0] = list_first_entry(&ring->free_pages, struct page, lru);
 	list_del(&page[0]->lru);
-	blkif->free_pages_num--;
-	spin_unlock_irqrestore(&blkif->free_pages_lock, flags);
+	ring->free_pages_num--;
+	spin_unlock_irqrestore(&ring->free_pages_lock, flags);
 
 	return 0;
 }
 
-static inline void put_free_pages(struct xen_blkif *blkif, struct page **page,
+static inline void put_free_pages(struct xen_blkif_ring *ring, struct page **page,
                                   int num)
 {
 	unsigned long flags;
 	int i;
 
-	spin_lock_irqsave(&blkif->free_pages_lock, flags);
+	spin_lock_irqsave(&ring->free_pages_lock, flags);
 	for (i = 0; i < num; i++)
-		list_add(&page[i]->lru, &blkif->free_pages);
-	blkif->free_pages_num += num;
-	spin_unlock_irqrestore(&blkif->free_pages_lock, flags);
+		list_add(&page[i]->lru, &ring->free_pages);
+	ring->free_pages_num += num;
+	spin_unlock_irqrestore(&ring->free_pages_lock, flags);
 }
 
-static inline void shrink_free_pagepool(struct xen_blkif *blkif, int num)
+static inline void shrink_free_pagepool(struct xen_blkif_ring *ring, int num)
 {
 	/* Remove requested pages in batches of NUM_BATCH_FREE_PAGES */
 	struct page *page[NUM_BATCH_FREE_PAGES];
 	unsigned int num_pages = 0;
 	unsigned long flags;
 
-	spin_lock_irqsave(&blkif->free_pages_lock, flags);
-	while (blkif->free_pages_num > num) {
-		BUG_ON(list_empty(&blkif->free_pages));
-		page[num_pages] = list_first_entry(&blkif->free_pages,
+	spin_lock_irqsave(&ring->free_pages_lock, flags);
+	while (ring->free_pages_num > num) {
+		BUG_ON(list_empty(&ring->free_pages));
+		page[num_pages] = list_first_entry(&ring->free_pages,
 		                                   struct page, lru);
 		list_del(&page[num_pages]->lru);
-		blkif->free_pages_num--;
+		ring->free_pages_num--;
 		if (++num_pages == NUM_BATCH_FREE_PAGES) {
-			spin_unlock_irqrestore(&blkif->free_pages_lock, flags);
+			spin_unlock_irqrestore(&ring->free_pages_lock, flags);
 			gnttab_free_pages(num_pages, page);
-			spin_lock_irqsave(&blkif->free_pages_lock, flags);
+			spin_lock_irqsave(&ring->free_pages_lock, flags);
 			num_pages = 0;
 		}
 	}
-	spin_unlock_irqrestore(&blkif->free_pages_lock, flags);
+	spin_unlock_irqrestore(&ring->free_pages_lock, flags);
 	if (num_pages != 0)
 		gnttab_free_pages(num_pages, page);
 }
 
 #define vaddr(page) ((unsigned long)pfn_to_kaddr(page_to_pfn(page)))
 
-static int do_block_io_op(struct xen_blkif *blkif);
-static int dispatch_rw_block_io(struct xen_blkif *blkif,
+static int do_block_io_op(struct xen_blkif_ring *ring);
+static int dispatch_rw_block_io(struct xen_blkif_ring *ring,
 				struct blkif_request *req,
 				struct pending_req *pending_req);
-static void make_response(struct xen_blkif *blkif, u64 id,
+static void make_response(struct xen_blkif_ring *ring, u64 id,
 			  unsigned short op, int st);
 
 #define foreach_grant_safe(pos, n, rbtree, node) \
@@ -190,7 +200,7 @@ static void make_response(struct xen_blkif *blkif, u64 id,
 
 /*
  * We don't need locking around the persistent grant helpers
- * because blkback uses a single-thread for each backed, so we
+ * because blkback uses a single-thread for each backend, so we
  * can be sure that this functions will never be called recursively.
  *
  * The only exception to that is put_persistent_grant, that can be called
@@ -198,19 +208,20 @@ static void make_response(struct xen_blkif *blkif, u64 id,
  * bit operations to modify the flags of a persistent grant and to count
  * the number of used grants.
  */
-static int add_persistent_gnt(struct xen_blkif *blkif,
+static int add_persistent_gnt(struct xen_blkif_ring *ring,
 			       struct persistent_gnt *persistent_gnt)
 {
 	struct rb_node **new = NULL, *parent = NULL;
 	struct persistent_gnt *this;
+	struct xen_blkif *blkif = ring->blkif;
 
-	if (blkif->persistent_gnt_c >= xen_blkif_max_pgrants) {
+	if (ring->persistent_gnt_c >= xen_blkif_max_pgrants) {
 		if (!blkif->vbd.overflow_max_grants)
 			blkif->vbd.overflow_max_grants = 1;
 		return -EBUSY;
 	}
 	/* Figure out where to put new node */
-	new = &blkif->persistent_gnts.rb_node;
+	new = &ring->persistent_gnts.rb_node;
 	while (*new) {
 		this = container_of(*new, struct persistent_gnt, node);
 
@@ -229,19 +240,19 @@ static int add_persistent_gnt(struct xen_blkif *blkif,
 	set_bit(PERSISTENT_GNT_ACTIVE, persistent_gnt->flags);
 	/* Add new node and rebalance tree. */
 	rb_link_node(&(persistent_gnt->node), parent, new);
-	rb_insert_color(&(persistent_gnt->node), &blkif->persistent_gnts);
-	blkif->persistent_gnt_c++;
-	atomic_inc(&blkif->persistent_gnt_in_use);
+	rb_insert_color(&(persistent_gnt->node), &ring->persistent_gnts);
+	ring->persistent_gnt_c++;
+	atomic_inc(&ring->persistent_gnt_in_use);
 	return 0;
 }
 
-static struct persistent_gnt *get_persistent_gnt(struct xen_blkif *blkif,
+static struct persistent_gnt *get_persistent_gnt(struct xen_blkif_ring *ring,
 						 grant_ref_t gref)
 {
 	struct persistent_gnt *data;
 	struct rb_node *node = NULL;
 
-	node = blkif->persistent_gnts.rb_node;
+	node = ring->persistent_gnts.rb_node;
 	while (node) {
 		data = container_of(node, struct persistent_gnt, node);
 
@@ -255,24 +266,24 @@ static struct persistent_gnt *get_persistent_gnt(struct xen_blkif *blkif,
 				return NULL;
 			}
 			set_bit(PERSISTENT_GNT_ACTIVE, data->flags);
-			atomic_inc(&blkif->persistent_gnt_in_use);
+			atomic_inc(&ring->persistent_gnt_in_use);
 			return data;
 		}
 	}
 	return NULL;
 }
 
-static void put_persistent_gnt(struct xen_blkif *blkif,
+static void put_persistent_gnt(struct xen_blkif_ring *ring,
                                struct persistent_gnt *persistent_gnt)
 {
 	if(!test_bit(PERSISTENT_GNT_ACTIVE, persistent_gnt->flags))
 		pr_alert_ratelimited("freeing a grant already unused\n");
 	set_bit(PERSISTENT_GNT_WAS_ACTIVE, persistent_gnt->flags);
 	clear_bit(PERSISTENT_GNT_ACTIVE, persistent_gnt->flags);
-	atomic_dec(&blkif->persistent_gnt_in_use);
+	atomic_dec(&ring->persistent_gnt_in_use);
 }
 
-static void free_persistent_gnts(struct xen_blkif *blkif, struct rb_root *root,
+static void free_persistent_gnts(struct xen_blkif_ring *ring, struct rb_root *root,
                                  unsigned int num)
 {
 	struct gnttab_unmap_grant_ref unmap[BLKIF_MAX_SEGMENTS_PER_REQUEST];
@@ -303,7 +314,7 @@ static void free_persistent_gnts(struct xen_blkif *blkif, struct rb_root *root,
 			unmap_data.count = segs_to_unmap;
 			BUG_ON(gnttab_unmap_refs_sync(&unmap_data));
 
-			put_free_pages(blkif, pages, segs_to_unmap);
+			put_free_pages(ring, pages, segs_to_unmap);
 			segs_to_unmap = 0;
 		}
 
@@ -320,15 +331,15 @@ void xen_blkbk_unmap_purged_grants(struct work_struct *work)
 	struct page *pages[BLKIF_MAX_SEGMENTS_PER_REQUEST];
 	struct persistent_gnt *persistent_gnt;
 	int segs_to_unmap = 0;
-	struct xen_blkif *blkif = container_of(work, typeof(*blkif), persistent_purge_work);
+	struct xen_blkif_ring *ring = container_of(work, typeof(*ring), persistent_purge_work);
 	struct gntab_unmap_queue_data unmap_data;
 
 	unmap_data.pages = pages;
 	unmap_data.unmap_ops = unmap;
 	unmap_data.kunmap_ops = NULL;
 
-	while(!list_empty(&blkif->persistent_purge_list)) {
-		persistent_gnt = list_first_entry(&blkif->persistent_purge_list,
+	while(!list_empty(&ring->persistent_purge_list)) {
+		persistent_gnt = list_first_entry(&ring->persistent_purge_list,
 		                                  struct persistent_gnt,
 		                                  remove_node);
 		list_del(&persistent_gnt->remove_node);
@@ -343,7 +354,7 @@ void xen_blkbk_unmap_purged_grants(struct work_struct *work)
 		if (++segs_to_unmap == BLKIF_MAX_SEGMENTS_PER_REQUEST) {
 			unmap_data.count = segs_to_unmap;
 			BUG_ON(gnttab_unmap_refs_sync(&unmap_data));
-			put_free_pages(blkif, pages, segs_to_unmap);
+			put_free_pages(ring, pages, segs_to_unmap);
 			segs_to_unmap = 0;
 		}
 		kfree(persistent_gnt);
@@ -351,11 +362,11 @@ void xen_blkbk_unmap_purged_grants(struct work_struct *work)
 	if (segs_to_unmap > 0) {
 		unmap_data.count = segs_to_unmap;
 		BUG_ON(gnttab_unmap_refs_sync(&unmap_data));
-		put_free_pages(blkif, pages, segs_to_unmap);
+		put_free_pages(ring, pages, segs_to_unmap);
 	}
 }
 
-static void purge_persistent_gnt(struct xen_blkif *blkif)
+static void purge_persistent_gnt(struct xen_blkif_ring *ring)
 {
 	struct persistent_gnt *persistent_gnt;
 	struct rb_node *n;
@@ -363,23 +374,23 @@ static void purge_persistent_gnt(struct xen_blkif *blkif)
 	bool scan_used = false, clean_used = false;
 	struct rb_root *root;
 
-	if (blkif->persistent_gnt_c < xen_blkif_max_pgrants ||
-	    (blkif->persistent_gnt_c == xen_blkif_max_pgrants &&
-	    !blkif->vbd.overflow_max_grants)) {
-		return;
+	if (ring->persistent_gnt_c < xen_blkif_max_pgrants ||
+	    (ring->persistent_gnt_c == xen_blkif_max_pgrants &&
+	    !ring->blkif->vbd.overflow_max_grants)) {
+		goto out;
 	}
 
-	if (work_busy(&blkif->persistent_purge_work)) {
+	if (work_busy(&ring->persistent_purge_work)) {
 		pr_alert_ratelimited("Scheduled work from previous purge is still busy, cannot purge list\n");
-		return;
+		goto out;
 	}
 
 	num_clean = (xen_blkif_max_pgrants / 100) * LRU_PERCENT_CLEAN;
-	num_clean = blkif->persistent_gnt_c - xen_blkif_max_pgrants + num_clean;
-	num_clean = min(blkif->persistent_gnt_c, num_clean);
+	num_clean = ring->persistent_gnt_c - xen_blkif_max_pgrants + num_clean;
+	num_clean = min(ring->persistent_gnt_c, num_clean);
 	if ((num_clean == 0) ||
-	    (num_clean > (blkif->persistent_gnt_c - atomic_read(&blkif->persistent_gnt_in_use))))
-		return;
+	    (num_clean > (ring->persistent_gnt_c - atomic_read(&ring->persistent_gnt_in_use))))
+		goto out;
 
 	/*
 	 * At this point, we can assure that there will be no calls
@@ -394,8 +405,8 @@ static void purge_persistent_gnt(struct xen_blkif *blkif)
 
 	pr_debug("Going to purge %u persistent grants\n", num_clean);
 
-	BUG_ON(!list_empty(&blkif->persistent_purge_list));
-	root = &blkif->persistent_gnts;
+	BUG_ON(!list_empty(&ring->persistent_purge_list));
+	root = &ring->persistent_gnts;
 purge_list:
 	foreach_grant_safe(persistent_gnt, n, root, node) {
 		BUG_ON(persistent_gnt->handle ==
@@ -414,7 +425,7 @@ purge_list:
 
 		rb_erase(&persistent_gnt->node, root);
 		list_add(&persistent_gnt->remove_node,
-		         &blkif->persistent_purge_list);
+			 &ring->persistent_purge_list);
 		if (--num_clean == 0)
 			goto finished;
 	}
@@ -435,30 +446,32 @@ finished:
 		goto purge_list;
 	}
 
-	blkif->persistent_gnt_c -= (total - num_clean);
-	blkif->vbd.overflow_max_grants = 0;
+	ring->persistent_gnt_c -= (total - num_clean);
+	ring->blkif->vbd.overflow_max_grants = 0;
 
 	/* We can defer this work */
-	schedule_work(&blkif->persistent_purge_work);
+	schedule_work(&ring->persistent_purge_work);
 	pr_debug("Purged %u/%u\n", (total - num_clean), total);
+
+out:
 	return;
 }
 
 /*
  * Retrieve from the 'pending_reqs' a free pending_req structure to be used.
  */
-static struct pending_req *alloc_req(struct xen_blkif *blkif)
+static struct pending_req *alloc_req(struct xen_blkif_ring *ring)
 {
 	struct pending_req *req = NULL;
 	unsigned long flags;
 
-	spin_lock_irqsave(&blkif->pending_free_lock, flags);
-	if (!list_empty(&blkif->pending_free)) {
-		req = list_entry(blkif->pending_free.next, struct pending_req,
+	spin_lock_irqsave(&ring->pending_free_lock, flags);
+	if (!list_empty(&ring->pending_free)) {
+		req = list_entry(ring->pending_free.next, struct pending_req,
 				 free_list);
 		list_del(&req->free_list);
 	}
-	spin_unlock_irqrestore(&blkif->pending_free_lock, flags);
+	spin_unlock_irqrestore(&ring->pending_free_lock, flags);
 	return req;
 }
 
@@ -466,17 +479,17 @@ static struct pending_req *alloc_req(struct xen_blkif *blkif)
  * Return the 'pending_req' structure back to the freepool. We also
  * wake up the thread if it was waiting for a free page.
  */
-static void free_req(struct xen_blkif *blkif, struct pending_req *req)
+static void free_req(struct xen_blkif_ring *ring, struct pending_req *req)
 {
 	unsigned long flags;
 	int was_empty;
 
-	spin_lock_irqsave(&blkif->pending_free_lock, flags);
-	was_empty = list_empty(&blkif->pending_free);
-	list_add(&req->free_list, &blkif->pending_free);
-	spin_unlock_irqrestore(&blkif->pending_free_lock, flags);
+	spin_lock_irqsave(&ring->pending_free_lock, flags);
+	was_empty = list_empty(&ring->pending_free);
+	list_add(&req->free_list, &ring->pending_free);
+	spin_unlock_irqrestore(&ring->pending_free_lock, flags);
 	if (was_empty)
-		wake_up(&blkif->pending_free_wq);
+		wake_up(&ring->pending_free_wq);
 }
 
 /*
@@ -556,10 +569,10 @@ abort:
 /*
  * Notification from the guest OS.
  */
-static void blkif_notify_work(struct xen_blkif *blkif)
+static void blkif_notify_work(struct xen_blkif_ring *ring)
 {
-	blkif->waiting_reqs = 1;
-	wake_up(&blkif->wq);
+	ring->waiting_reqs = 1;
+	wake_up(&ring->wq);
 }
 
 irqreturn_t xen_blkif_be_int(int irq, void *dev_id)
@@ -572,31 +585,33 @@ irqreturn_t xen_blkif_be_int(int irq, void *dev_id)
  * SCHEDULER FUNCTIONS
  */
 
-static void print_stats(struct xen_blkif *blkif)
+static void print_stats(struct xen_blkif_ring *ring)
 {
 	pr_info("(%s): oo %3llu  |  rd %4llu  |  wr %4llu  |  f %4llu"
 		 "  |  ds %4llu | pg: %4u/%4d\n",
-		 current->comm, blkif->st_oo_req,
-		 blkif->st_rd_req, blkif->st_wr_req,
-		 blkif->st_f_req, blkif->st_ds_req,
-		 blkif->persistent_gnt_c,
+		 current->comm, ring->st_oo_req,
+		 ring->st_rd_req, ring->st_wr_req,
+		 ring->st_f_req, ring->st_ds_req,
+		 ring->persistent_gnt_c,
 		 xen_blkif_max_pgrants);
-	blkif->st_print = jiffies + msecs_to_jiffies(10 * 1000);
-	blkif->st_rd_req = 0;
-	blkif->st_wr_req = 0;
-	blkif->st_oo_req = 0;
-	blkif->st_ds_req = 0;
+	ring->st_print = jiffies + msecs_to_jiffies(10 * 1000);
+	ring->st_rd_req = 0;
+	ring->st_wr_req = 0;
+	ring->st_oo_req = 0;
+	ring->st_ds_req = 0;
 }
 
 int xen_blkif_schedule(void *arg)
 {
-	struct xen_blkif *blkif = arg;
+	struct xen_blkif_ring *ring = arg;
+	struct xen_blkif *blkif = ring->blkif;
 	struct xen_vbd *vbd = &blkif->vbd;
 	unsigned long timeout;
 	int ret;
 
 	xen_blkif_get(blkif);
 
+	set_freezable();
 	while (!kthread_should_stop()) {
 		if (try_to_freeze())
 			continue;
@@ -606,50 +621,50 @@ int xen_blkif_schedule(void *arg)
 		timeout = msecs_to_jiffies(LRU_INTERVAL);
 
 		timeout = wait_event_interruptible_timeout(
-			blkif->wq,
-			blkif->waiting_reqs || kthread_should_stop(),
+			ring->wq,
+			ring->waiting_reqs || kthread_should_stop(),
 			timeout);
 		if (timeout == 0)
 			goto purge_gnt_list;
 		timeout = wait_event_interruptible_timeout(
-			blkif->pending_free_wq,
-			!list_empty(&blkif->pending_free) ||
+			ring->pending_free_wq,
+			!list_empty(&ring->pending_free) ||
 			kthread_should_stop(),
 			timeout);
 		if (timeout == 0)
 			goto purge_gnt_list;
 
-		blkif->waiting_reqs = 0;
+		ring->waiting_reqs = 0;
 		smp_mb(); /* clear flag *before* checking for work */
 
-		ret = do_block_io_op(blkif);
+		ret = do_block_io_op(ring);
 		if (ret > 0)
-			blkif->waiting_reqs = 1;
+			ring->waiting_reqs = 1;
 		if (ret == -EACCES)
-			wait_event_interruptible(blkif->shutdown_wq,
+			wait_event_interruptible(ring->shutdown_wq,
 						 kthread_should_stop());
 
 purge_gnt_list:
 		if (blkif->vbd.feature_gnt_persistent &&
-		    time_after(jiffies, blkif->next_lru)) {
-			purge_persistent_gnt(blkif);
-			blkif->next_lru = jiffies + msecs_to_jiffies(LRU_INTERVAL);
+		    time_after(jiffies, ring->next_lru)) {
+			purge_persistent_gnt(ring);
+			ring->next_lru = jiffies + msecs_to_jiffies(LRU_INTERVAL);
 		}
 
 		/* Shrink if we have more than xen_blkif_max_buffer_pages */
-		shrink_free_pagepool(blkif, xen_blkif_max_buffer_pages);
+		shrink_free_pagepool(ring, xen_blkif_max_buffer_pages);
 
-		if (log_stats && time_after(jiffies, blkif->st_print))
-			print_stats(blkif);
+		if (log_stats && time_after(jiffies, ring->st_print))
+			print_stats(ring);
 	}
 
 	/* Drain pending purge work */
-	flush_work(&blkif->persistent_purge_work);
+	flush_work(&ring->persistent_purge_work);
 
 	if (log_stats)
-		print_stats(blkif);
+		print_stats(ring);
 
-	blkif->xenblkd = NULL;
+	ring->xenblkd = NULL;
 	xen_blkif_put(blkif);
 
 	return 0;
@@ -658,22 +673,22 @@ purge_gnt_list:
 /*
  * Remove persistent grants and empty the pool of free pages
  */
-void xen_blkbk_free_caches(struct xen_blkif *blkif)
+void xen_blkbk_free_caches(struct xen_blkif_ring *ring)
 {
 	/* Free all persistent grant pages */
-	if (!RB_EMPTY_ROOT(&blkif->persistent_gnts))
-		free_persistent_gnts(blkif, &blkif->persistent_gnts,
-			blkif->persistent_gnt_c);
+	if (!RB_EMPTY_ROOT(&ring->persistent_gnts))
+		free_persistent_gnts(ring, &ring->persistent_gnts,
+			ring->persistent_gnt_c);
 
-	BUG_ON(!RB_EMPTY_ROOT(&blkif->persistent_gnts));
-	blkif->persistent_gnt_c = 0;
+	BUG_ON(!RB_EMPTY_ROOT(&ring->persistent_gnts));
+	ring->persistent_gnt_c = 0;
 
 	/* Since we are shutting down remove all pages from the buffer */
-	shrink_free_pagepool(blkif, 0 /* All */);
+	shrink_free_pagepool(ring, 0 /* All */);
 }
 
 static unsigned int xen_blkbk_unmap_prepare(
-	struct xen_blkif *blkif,
+	struct xen_blkif_ring *ring,
 	struct grant_page **pages,
 	unsigned int num,
 	struct gnttab_unmap_grant_ref *unmap_ops,
@@ -683,7 +698,7 @@ static unsigned int xen_blkbk_unmap_prepare(
 
 	for (i = 0; i < num; i++) {
 		if (pages[i]->persistent_gnt != NULL) {
-			put_persistent_gnt(blkif, pages[i]->persistent_gnt);
+			put_persistent_gnt(ring, pages[i]->persistent_gnt);
 			continue;
 		}
 		if (pages[i]->handle == BLKBACK_INVALID_HANDLE)
@@ -700,17 +715,18 @@ static unsigned int xen_blkbk_unmap_prepare(
 
 static void xen_blkbk_unmap_and_respond_callback(int result, struct gntab_unmap_queue_data *data)
 {
-	struct pending_req* pending_req = (struct pending_req*) (data->data);
-	struct xen_blkif *blkif = pending_req->blkif;
+	struct pending_req *pending_req = (struct pending_req *)(data->data);
+	struct xen_blkif_ring *ring = pending_req->ring;
+	struct xen_blkif *blkif = ring->blkif;
 
 	/* BUG_ON used to reproduce existing behaviour,
 	   but is this the best way to deal with this? */
 	BUG_ON(result);
 
-	put_free_pages(blkif, data->pages, data->count);
-	make_response(blkif, pending_req->id,
+	put_free_pages(ring, data->pages, data->count);
+	make_response(ring, pending_req->id,
 		      pending_req->operation, pending_req->status);
-	free_req(blkif, pending_req);
+	free_req(ring, pending_req);
 	/*
 	 * Make sure the request is freed before releasing blkif,
 	 * or there could be a race between free_req and the
@@ -723,7 +739,7 @@ static void xen_blkbk_unmap_and_respond_callback(int result, struct gntab_unmap_
 	 * pending_free_wq if there's a drain going on, but it has
 	 * to be taken into account if the current model is changed.
 	 */
-	if (atomic_dec_and_test(&blkif->inflight) && atomic_read(&blkif->drain)) {
+	if (atomic_dec_and_test(&ring->inflight) && atomic_read(&blkif->drain)) {
 		complete(&blkif->drain_complete);
 	}
 	xen_blkif_put(blkif);
@@ -732,11 +748,11 @@ static void xen_blkbk_unmap_and_respond_callback(int result, struct gntab_unmap_
 static void xen_blkbk_unmap_and_respond(struct pending_req *req)
 {
 	struct gntab_unmap_queue_data* work = &req->gnttab_unmap_data;
-	struct xen_blkif *blkif = req->blkif;
+	struct xen_blkif_ring *ring = req->ring;
 	struct grant_page **pages = req->segments;
 	unsigned int invcount;
 
-	invcount = xen_blkbk_unmap_prepare(blkif, pages, req->nr_segs,
+	invcount = xen_blkbk_unmap_prepare(ring, pages, req->nr_segs,
 					   req->unmap, req->unmap_pages);
 
 	work->data = req;
@@ -757,7 +773,7 @@ static void xen_blkbk_unmap_and_respond(struct pending_req *req)
  * of hypercalls, but since this is only used in error paths there's
  * no real need.
  */
-static void xen_blkbk_unmap(struct xen_blkif *blkif,
+static void xen_blkbk_unmap(struct xen_blkif_ring *ring,
                             struct grant_page *pages[],
                             int num)
 {
@@ -768,20 +784,20 @@ static void xen_blkbk_unmap(struct xen_blkif *blkif,
 
 	while (num) {
 		unsigned int batch = min(num, BLKIF_MAX_SEGMENTS_PER_REQUEST);
-		
-		invcount = xen_blkbk_unmap_prepare(blkif, pages, batch,
+
+		invcount = xen_blkbk_unmap_prepare(ring, pages, batch,
 						   unmap, unmap_pages);
 		if (invcount) {
 			ret = gnttab_unmap_refs(unmap, NULL, unmap_pages, invcount);
 			BUG_ON(ret);
-			put_free_pages(blkif, unmap_pages, invcount);
+			put_free_pages(ring, unmap_pages, invcount);
 		}
 		pages += batch;
 		num -= batch;
 	}
 }
 
-static int xen_blkbk_map(struct xen_blkif *blkif,
+static int xen_blkbk_map(struct xen_blkif_ring *ring,
 			 struct grant_page *pages[],
 			 int num, bool ro)
 {
@@ -794,6 +810,7 @@ static int xen_blkbk_map(struct xen_blkif *blkif,
 	int ret = 0;
 	int last_map = 0, map_until = 0;
 	int use_persistent_gnts;
+	struct xen_blkif *blkif = ring->blkif;
 
 	use_persistent_gnts = (blkif->vbd.feature_gnt_persistent);
 
@@ -806,10 +823,11 @@ again:
 	for (i = map_until; i < num; i++) {
 		uint32_t flags;
 
-		if (use_persistent_gnts)
+		if (use_persistent_gnts) {
 			persistent_gnt = get_persistent_gnt(
-				blkif,
+				ring,
 				pages[i]->gref);
+		}
 
 		if (persistent_gnt) {
 			/*
@@ -819,7 +837,7 @@ again:
 			pages[i]->page = persistent_gnt->page;
 			pages[i]->persistent_gnt = persistent_gnt;
 		} else {
-			if (get_free_page(blkif, &pages[i]->page))
+			if (get_free_page(ring, &pages[i]->page))
 				goto out_of_memory;
 			addr = vaddr(pages[i]->page);
 			pages_to_gnt[segs_to_map] = pages[i]->page;
@@ -852,7 +870,7 @@ again:
 			BUG_ON(new_map_idx >= segs_to_map);
 			if (unlikely(map[new_map_idx].status != 0)) {
 				pr_debug("invalid buffer -- could not remap it\n");
-				put_free_pages(blkif, &pages[seg_idx]->page, 1);
+				put_free_pages(ring, &pages[seg_idx]->page, 1);
 				pages[seg_idx]->handle = BLKBACK_INVALID_HANDLE;
 				ret |= 1;
 				goto next;
@@ -862,7 +880,7 @@ again:
 			continue;
 		}
 		if (use_persistent_gnts &&
-		    blkif->persistent_gnt_c < xen_blkif_max_pgrants) {
+		    ring->persistent_gnt_c < xen_blkif_max_pgrants) {
 			/*
 			 * We are using persistent grants, the grant is
 			 * not mapped but we might have room for it.
@@ -880,7 +898,7 @@ again:
 			persistent_gnt->gnt = map[new_map_idx].ref;
 			persistent_gnt->handle = map[new_map_idx].handle;
 			persistent_gnt->page = pages[seg_idx]->page;
-			if (add_persistent_gnt(blkif,
+			if (add_persistent_gnt(ring,
 			                       persistent_gnt)) {
 				kfree(persistent_gnt);
 				persistent_gnt = NULL;
@@ -888,7 +906,7 @@ again:
 			}
 			pages[seg_idx]->persistent_gnt = persistent_gnt;
 			pr_debug("grant %u added to the tree of persistent grants, using %u/%u\n",
-				 persistent_gnt->gnt, blkif->persistent_gnt_c,
+				 persistent_gnt->gnt, ring->persistent_gnt_c,
 				 xen_blkif_max_pgrants);
 			goto next;
 		}
@@ -913,7 +931,7 @@ next:
 
 out_of_memory:
 	pr_alert("%s: out of memory\n", __func__);
-	put_free_pages(blkif, pages_to_gnt, segs_to_map);
+	put_free_pages(ring, pages_to_gnt, segs_to_map);
 	return -ENOMEM;
 }
 
@@ -921,7 +939,7 @@ static int xen_blkbk_map_seg(struct pending_req *pending_req)
 {
 	int rc;
 
-	rc = xen_blkbk_map(pending_req->blkif, pending_req->segments,
+	rc = xen_blkbk_map(pending_req->ring, pending_req->segments,
 			   pending_req->nr_segs,
 	                   (pending_req->operation != BLKIF_OP_READ));
 
@@ -934,7 +952,7 @@ static int xen_blkbk_parse_indirect(struct blkif_request *req,
 				    struct phys_req *preq)
 {
 	struct grant_page **pages = pending_req->indirect_pages;
-	struct xen_blkif *blkif = pending_req->blkif;
+	struct xen_blkif_ring *ring = pending_req->ring;
 	int indirect_grefs, rc, n, nseg, i;
 	struct blkif_request_segment *segments = NULL;
 
@@ -945,7 +963,7 @@ static int xen_blkbk_parse_indirect(struct blkif_request *req,
 	for (i = 0; i < indirect_grefs; i++)
 		pages[i]->gref = req->u.indirect.indirect_grefs[i];
 
-	rc = xen_blkbk_map(blkif, pages, indirect_grefs, true);
+	rc = xen_blkbk_map(ring, pages, indirect_grefs, true);
 	if (rc)
 		goto unmap;
 
@@ -977,15 +995,16 @@ static int xen_blkbk_parse_indirect(struct blkif_request *req,
 unmap:
 	if (segments)
 		kunmap_atomic(segments);
-	xen_blkbk_unmap(blkif, pages, indirect_grefs);
+	xen_blkbk_unmap(ring, pages, indirect_grefs);
 	return rc;
 }
 
-static int dispatch_discard_io(struct xen_blkif *blkif,
+static int dispatch_discard_io(struct xen_blkif_ring *ring,
 				struct blkif_request *req)
 {
 	int err = 0;
 	int status = BLKIF_RSP_OKAY;
+	struct xen_blkif *blkif = ring->blkif;
 	struct block_device *bdev = blkif->vbd.bdev;
 	unsigned long secure;
 	struct phys_req preq;
@@ -1002,7 +1021,7 @@ static int dispatch_discard_io(struct xen_blkif *blkif,
 			preq.sector_number + preq.nr_sects, blkif->vbd.pdevice);
 		goto fail_response;
 	}
-	blkif->st_ds_req++;
+	ring->st_ds_req++;
 
 	secure = (blkif->vbd.discard_secure &&
 		 (req->u.discard.flag & BLKIF_DISCARD_SECURE)) ?
@@ -1018,26 +1037,28 @@ fail_response:
 	} else if (err)
 		status = BLKIF_RSP_ERROR;
 
-	make_response(blkif, req->u.discard.id, req->operation, status);
+	make_response(ring, req->u.discard.id, req->operation, status);
 	xen_blkif_put(blkif);
 	return err;
 }
 
-static int dispatch_other_io(struct xen_blkif *blkif,
+static int dispatch_other_io(struct xen_blkif_ring *ring,
 			     struct blkif_request *req,
 			     struct pending_req *pending_req)
 {
-	free_req(blkif, pending_req);
-	make_response(blkif, req->u.other.id, req->operation,
+	free_req(ring, pending_req);
+	make_response(ring, req->u.other.id, req->operation,
 		      BLKIF_RSP_EOPNOTSUPP);
 	return -EIO;
 }
 
-static void xen_blk_drain_io(struct xen_blkif *blkif)
+static void xen_blk_drain_io(struct xen_blkif_ring *ring)
 {
+	struct xen_blkif *blkif = ring->blkif;
+
 	atomic_set(&blkif->drain, 1);
 	do {
-		if (atomic_read(&blkif->inflight) == 0)
+		if (atomic_read(&ring->inflight) == 0)
 			break;
 		wait_for_completion_interruptible_timeout(
 				&blkif->drain_complete, HZ);
@@ -1058,12 +1079,12 @@ static void __end_block_io_op(struct pending_req *pending_req, int error)
 	if ((pending_req->operation == BLKIF_OP_FLUSH_DISKCACHE) &&
 	    (error == -EOPNOTSUPP)) {
 		pr_debug("flush diskcache op failed, not supported\n");
-		xen_blkbk_flush_diskcache(XBT_NIL, pending_req->blkif->be, 0);
+		xen_blkbk_flush_diskcache(XBT_NIL, pending_req->ring->blkif->be, 0);
 		pending_req->status = BLKIF_RSP_EOPNOTSUPP;
 	} else if ((pending_req->operation == BLKIF_OP_WRITE_BARRIER) &&
 		    (error == -EOPNOTSUPP)) {
 		pr_debug("write barrier op failed, not supported\n");
-		xen_blkbk_barrier(XBT_NIL, pending_req->blkif->be, 0);
+		xen_blkbk_barrier(XBT_NIL, pending_req->ring->blkif->be, 0);
 		pending_req->status = BLKIF_RSP_EOPNOTSUPP;
 	} else if (error) {
 		pr_debug("Buffer not up-to-date at end of operation,"
@@ -1097,9 +1118,9 @@ static void end_block_io_op(struct bio *bio)
  * and transmute  it to the block API to hand it over to the proper block disk.
  */
 static int
-__do_block_io_op(struct xen_blkif *blkif)
+__do_block_io_op(struct xen_blkif_ring *ring)
 {
-	union blkif_back_rings *blk_rings = &blkif->blk_rings;
+	union blkif_back_rings *blk_rings = &ring->blk_rings;
 	struct blkif_request req;
 	struct pending_req *pending_req;
 	RING_IDX rc, rp;
@@ -1112,7 +1133,7 @@ __do_block_io_op(struct xen_blkif *blkif)
 	if (RING_REQUEST_PROD_OVERFLOW(&blk_rings->common, rp)) {
 		rc = blk_rings->common.rsp_prod_pvt;
 		pr_warn("Frontend provided bogus ring requests (%d - %d = %d). Halting ring processing on dev=%04x\n",
-			rp, rc, rp - rc, blkif->vbd.pdevice);
+			rp, rc, rp - rc, ring->blkif->vbd.pdevice);
 		return -EACCES;
 	}
 	while (rc != rp) {
@@ -1125,14 +1146,14 @@ __do_block_io_op(struct xen_blkif *blkif)
 			break;
 		}
 
-		pending_req = alloc_req(blkif);
+		pending_req = alloc_req(ring);
 		if (NULL == pending_req) {
-			blkif->st_oo_req++;
+			ring->st_oo_req++;
 			more_to_do = 1;
 			break;
 		}
 
-		switch (blkif->blk_protocol) {
+		switch (ring->blkif->blk_protocol) {
 		case BLKIF_PROTOCOL_NATIVE:
 			memcpy(&req, RING_GET_REQUEST(&blk_rings->native, rc), sizeof(req));
 			break;
@@ -1156,16 +1177,16 @@ __do_block_io_op(struct xen_blkif *blkif)
 		case BLKIF_OP_WRITE_BARRIER:
 		case BLKIF_OP_FLUSH_DISKCACHE:
 		case BLKIF_OP_INDIRECT:
-			if (dispatch_rw_block_io(blkif, &req, pending_req))
+			if (dispatch_rw_block_io(ring, &req, pending_req))
 				goto done;
 			break;
 		case BLKIF_OP_DISCARD:
-			free_req(blkif, pending_req);
-			if (dispatch_discard_io(blkif, &req))
+			free_req(ring, pending_req);
+			if (dispatch_discard_io(ring, &req))
 				goto done;
 			break;
 		default:
-			if (dispatch_other_io(blkif, &req, pending_req))
+			if (dispatch_other_io(ring, &req, pending_req))
 				goto done;
 			break;
 		}
@@ -1178,13 +1199,13 @@ done:
 }
 
 static int
-do_block_io_op(struct xen_blkif *blkif)
+do_block_io_op(struct xen_blkif_ring *ring)
 {
-	union blkif_back_rings *blk_rings = &blkif->blk_rings;
+	union blkif_back_rings *blk_rings = &ring->blk_rings;
 	int more_to_do;
 
 	do {
-		more_to_do = __do_block_io_op(blkif);
+		more_to_do = __do_block_io_op(ring);
 		if (more_to_do)
 			break;
 
@@ -1197,7 +1218,7 @@ do_block_io_op(struct xen_blkif *blkif)
  * Transmutation of the 'struct blkif_request' to a proper 'struct bio'
  * and call the 'submit_bio' to pass it to the underlying storage.
  */
-static int dispatch_rw_block_io(struct xen_blkif *blkif,
+static int dispatch_rw_block_io(struct xen_blkif_ring *ring,
 				struct blkif_request *req,
 				struct pending_req *pending_req)
 {
@@ -1225,17 +1246,17 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 
 	switch (req_operation) {
 	case BLKIF_OP_READ:
-		blkif->st_rd_req++;
+		ring->st_rd_req++;
 		operation = READ;
 		break;
 	case BLKIF_OP_WRITE:
-		blkif->st_wr_req++;
+		ring->st_wr_req++;
 		operation = WRITE_ODIRECT;
 		break;
 	case BLKIF_OP_WRITE_BARRIER:
 		drain = true;
 	case BLKIF_OP_FLUSH_DISKCACHE:
-		blkif->st_f_req++;
+		ring->st_f_req++;
 		operation = WRITE_FLUSH;
 		break;
 	default:
@@ -1260,7 +1281,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 
 	preq.nr_sects      = 0;
 
-	pending_req->blkif     = blkif;
+	pending_req->ring      = ring;
 	pending_req->id        = req->u.rw.id;
 	pending_req->operation = req_operation;
 	pending_req->status    = BLKIF_RSP_OKAY;
@@ -1287,12 +1308,12 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 			goto fail_response;
 	}
 
-	if (xen_vbd_translate(&preq, blkif, operation) != 0) {
+	if (xen_vbd_translate(&preq, ring->blkif, operation) != 0) {
 		pr_debug("access denied: %s of [%llu,%llu] on dev=%04x\n",
 			 operation == READ ? "read" : "write",
 			 preq.sector_number,
 			 preq.sector_number + preq.nr_sects,
-			 blkif->vbd.pdevice);
+			 ring->blkif->vbd.pdevice);
 		goto fail_response;
 	}
 
@@ -1304,7 +1325,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 		if (((int)preq.sector_number|(int)seg[i].nsec) &
 		    ((bdev_logical_block_size(preq.bdev) >> 9) - 1)) {
 			pr_debug("Misaligned I/O request from domain %d\n",
-				 blkif->domid);
+				 ring->blkif->domid);
 			goto fail_response;
 		}
 	}
@@ -1313,7 +1334,7 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 	 * issue the WRITE_FLUSH.
 	 */
 	if (drain)
-		xen_blk_drain_io(pending_req->blkif);
+		xen_blk_drain_io(pending_req->ring);
 
 	/*
 	 * If we have failed at this point, we need to undo the M2P override,
@@ -1328,8 +1349,8 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 	 * This corresponding xen_blkif_put is done in __end_block_io_op, or
 	 * below (in "!bio") if we are handling a BLKIF_OP_DISCARD.
 	 */
-	xen_blkif_get(blkif);
-	atomic_inc(&blkif->inflight);
+	xen_blkif_get(ring->blkif);
+	atomic_inc(&ring->inflight);
 
 	for (i = 0; i < nseg; i++) {
 		while ((bio == NULL) ||
@@ -1377,19 +1398,19 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 	blk_finish_plug(&plug);
 
 	if (operation == READ)
-		blkif->st_rd_sect += preq.nr_sects;
+		ring->st_rd_sect += preq.nr_sects;
 	else if (operation & WRITE)
-		blkif->st_wr_sect += preq.nr_sects;
+		ring->st_wr_sect += preq.nr_sects;
 
 	return 0;
 
  fail_flush:
-	xen_blkbk_unmap(blkif, pending_req->segments,
+	xen_blkbk_unmap(ring, pending_req->segments,
 	                pending_req->nr_segs);
  fail_response:
 	/* Haven't submitted any bio's yet. */
-	make_response(blkif, req->u.rw.id, req_operation, BLKIF_RSP_ERROR);
-	free_req(blkif, pending_req);
+	make_response(ring, req->u.rw.id, req_operation, BLKIF_RSP_ERROR);
+	free_req(ring, pending_req);
 	msleep(1); /* back off a bit */
 	return -EIO;
 
@@ -1407,21 +1428,22 @@ static int dispatch_rw_block_io(struct xen_blkif *blkif,
 /*
  * Put a response on the ring on how the operation fared.
  */
-static void make_response(struct xen_blkif *blkif, u64 id,
+static void make_response(struct xen_blkif_ring *ring, u64 id,
 			  unsigned short op, int st)
 {
 	struct blkif_response  resp;
 	unsigned long     flags;
-	union blkif_back_rings *blk_rings = &blkif->blk_rings;
+	union blkif_back_rings *blk_rings;
 	int notify;
 
 	resp.id        = id;
 	resp.operation = op;
 	resp.status    = st;
 
-	spin_lock_irqsave(&blkif->blk_ring_lock, flags);
+	spin_lock_irqsave(&ring->blk_ring_lock, flags);
+	blk_rings = &ring->blk_rings;
 	/* Place on the response ring for the relevant domain. */
-	switch (blkif->blk_protocol) {
+	switch (ring->blkif->blk_protocol) {
 	case BLKIF_PROTOCOL_NATIVE:
 		memcpy(RING_GET_RESPONSE(&blk_rings->native, blk_rings->native.rsp_prod_pvt),
 		       &resp, sizeof(resp));
@@ -1439,9 +1461,9 @@ static void make_response(struct xen_blkif *blkif, u64 id,
 	}
 	blk_rings->common.rsp_prod_pvt++;
 	RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&blk_rings->common, notify);
-	spin_unlock_irqrestore(&blkif->blk_ring_lock, flags);
+	spin_unlock_irqrestore(&ring->blk_ring_lock, flags);
 	if (notify)
-		notify_remote_via_irq(blkif->irq);
+		notify_remote_via_irq(ring->irq);
 }
 
 static int __init xen_blkif_init(void)
@@ -1457,6 +1479,9 @@ static int __init xen_blkif_init(void)
 		xen_blkif_max_ring_order = XENBUS_MAX_RING_GRANT_ORDER;
 	}
 
+	if (xenblk_max_queues == 0)
+		xenblk_max_queues = num_online_cpus();
+
 	rc = xen_blkif_interface_init();
 	if (rc)
 		goto failed_init;
diff --git a/drivers/block/xen-blkback/common.h b/drivers/block/xen-blkback/common.h
index c929ae22764c..dea61f6ab8cb 100644
--- a/drivers/block/xen-blkback/common.h
+++ b/drivers/block/xen-blkback/common.h
@@ -46,6 +46,7 @@
 #include <xen/interface/io/protocols.h>
 
 extern unsigned int xen_blkif_max_ring_order;
+extern unsigned int xenblk_max_queues;
 /*
  * This is the maximum number of segments that would be allowed in indirect
  * requests. This value will also be passed to the frontend.
@@ -269,68 +270,79 @@ struct persistent_gnt {
 	struct list_head remove_node;
 };
 
-struct xen_blkif {
-	/* Unique identifier for this interface. */
-	domid_t			domid;
-	unsigned int		handle;
+/* Per-ring information. */
+struct xen_blkif_ring {
 	/* Physical parameters of the comms window. */
 	unsigned int		irq;
-	/* Comms information. */
-	enum blkif_protocol	blk_protocol;
 	union blkif_back_rings	blk_rings;
 	void			*blk_ring;
-	/* The VBD attached to this interface. */
-	struct xen_vbd		vbd;
-	/* Back pointer to the backend_info. */
-	struct backend_info	*be;
 	/* Private fields. */
 	spinlock_t		blk_ring_lock;
-	atomic_t		refcnt;
 
 	wait_queue_head_t	wq;
-	/* for barrier (drain) requests */
-	struct completion	drain_complete;
-	atomic_t		drain;
 	atomic_t		inflight;
-	/* One thread per one blkif. */
+	/* One thread per blkif ring. */
 	struct task_struct	*xenblkd;
 	unsigned int		waiting_reqs;
 
-	/* tree to store persistent grants */
+	/* List of all 'pending_req' available */
+	struct list_head	pending_free;
+	/* And its spinlock. */
+	spinlock_t		pending_free_lock;
+	wait_queue_head_t	pending_free_wq;
+
+	/* Tree to store persistent grants. */
+	spinlock_t		pers_gnts_lock;
 	struct rb_root		persistent_gnts;
 	unsigned int		persistent_gnt_c;
 	atomic_t		persistent_gnt_in_use;
 	unsigned long           next_lru;
 
-	/* used by the kworker that offload work from the persistent purge */
+	/* Statistics. */
+	unsigned long		st_print;
+	unsigned long long	st_rd_req;
+	unsigned long long	st_wr_req;
+	unsigned long long	st_oo_req;
+	unsigned long long	st_f_req;
+	unsigned long long	st_ds_req;
+	unsigned long long	st_rd_sect;
+	unsigned long long	st_wr_sect;
+
+	/* Used by the kworker that offload work from the persistent purge. */
 	struct list_head	persistent_purge_list;
 	struct work_struct	persistent_purge_work;
 
-	/* buffer of free pages to map grant refs */
+	/* Buffer of free pages to map grant refs. */
 	spinlock_t		free_pages_lock;
 	int			free_pages_num;
 	struct list_head	free_pages;
 
-	/* List of all 'pending_req' available */
-	struct list_head	pending_free;
-	/* And its spinlock. */
-	spinlock_t		pending_free_lock;
-	wait_queue_head_t	pending_free_wq;
-
-	/* statistics */
-	unsigned long		st_print;
-	unsigned long long			st_rd_req;
-	unsigned long long			st_wr_req;
-	unsigned long long			st_oo_req;
-	unsigned long long			st_f_req;
-	unsigned long long			st_ds_req;
-	unsigned long long			st_rd_sect;
-	unsigned long long			st_wr_sect;
-
 	struct work_struct	free_work;
 	/* Thread shutdown wait queue. */
 	wait_queue_head_t	shutdown_wq;
-	unsigned int nr_ring_pages;
+	struct xen_blkif 	*blkif;
+};
+
+struct xen_blkif {
+	/* Unique identifier for this interface. */
+	domid_t			domid;
+	unsigned int		handle;
+	/* Comms information. */
+	enum blkif_protocol	blk_protocol;
+	/* The VBD attached to this interface. */
+	struct xen_vbd		vbd;
+	/* Back pointer to the backend_info. */
+	struct backend_info	*be;
+	atomic_t		refcnt;
+	/* for barrier (drain) requests */
+	struct completion	drain_complete;
+	atomic_t		drain;
+
+	struct work_struct	free_work;
+	unsigned int 		nr_ring_pages;
+	/* All rings for this device. */
+	struct xen_blkif_ring	*rings;
+	unsigned int		nr_rings;
 };
 
 struct seg_buf {
@@ -352,7 +364,7 @@ struct grant_page {
  * response queued for it, with the saved 'id' passed back.
  */
 struct pending_req {
-	struct xen_blkif	*blkif;
+	struct xen_blkif_ring   *ring;
 	u64			id;
 	int			nr_segs;
 	atomic_t		pendcnt;
@@ -394,7 +406,7 @@ int xen_blkif_xenbus_init(void);
 irqreturn_t xen_blkif_be_int(int irq, void *dev_id);
 int xen_blkif_schedule(void *arg);
 int xen_blkif_purge_persistent(void *arg);
-void xen_blkbk_free_caches(struct xen_blkif *blkif);
+void xen_blkbk_free_caches(struct xen_blkif_ring *ring);
 
 int xen_blkbk_flush_diskcache(struct xenbus_transaction xbt,
 			      struct backend_info *be, int state);
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index f53cff42f8da..876763f7f13e 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -86,9 +86,11 @@ static void xen_update_blkif_status(struct xen_blkif *blkif)
 {
 	int err;
 	char name[BLKBACK_NAME_LEN];
+	struct xen_blkif_ring *ring;
+	int i;
 
 	/* Not ready to connect? */
-	if (!blkif->irq || !blkif->vbd.bdev)
+	if (!blkif->rings || !blkif->rings[0].irq || !blkif->vbd.bdev)
 		return;
 
 	/* Already connected? */
@@ -113,13 +115,55 @@ static void xen_update_blkif_status(struct xen_blkif *blkif)
 	}
 	invalidate_inode_pages2(blkif->vbd.bdev->bd_inode->i_mapping);
 
-	blkif->xenblkd = kthread_run(xen_blkif_schedule, blkif, "%s", name);
-	if (IS_ERR(blkif->xenblkd)) {
-		err = PTR_ERR(blkif->xenblkd);
-		blkif->xenblkd = NULL;
-		xenbus_dev_error(blkif->be->dev, err, "start xenblkd");
-		return;
+	for (i = 0; i < blkif->nr_rings; i++) {
+		ring = &blkif->rings[i];
+		ring->xenblkd = kthread_run(xen_blkif_schedule, ring, "%s-%d", name, i);
+		if (IS_ERR(ring->xenblkd)) {
+			err = PTR_ERR(ring->xenblkd);
+			ring->xenblkd = NULL;
+			xenbus_dev_fatal(blkif->be->dev, err,
+					"start %s-%d xenblkd", name, i);
+			goto out;
+		}
+	}
+	return;
+
+out:
+	while (--i >= 0) {
+		ring = &blkif->rings[i];
+		kthread_stop(ring->xenblkd);
+	}
+	return;
+}
+
+static int xen_blkif_alloc_rings(struct xen_blkif *blkif)
+{
+	unsigned int r;
+
+	blkif->rings = kzalloc(blkif->nr_rings * sizeof(struct xen_blkif_ring), GFP_KERNEL);
+	if (!blkif->rings)
+		return -ENOMEM;
+
+	for (r = 0; r < blkif->nr_rings; r++) {
+		struct xen_blkif_ring *ring = &blkif->rings[r];
+
+		spin_lock_init(&ring->blk_ring_lock);
+		init_waitqueue_head(&ring->wq);
+		INIT_LIST_HEAD(&ring->pending_free);
+		INIT_LIST_HEAD(&ring->persistent_purge_list);
+		INIT_WORK(&ring->persistent_purge_work, xen_blkbk_unmap_purged_grants);
+		spin_lock_init(&ring->free_pages_lock);
+		INIT_LIST_HEAD(&ring->free_pages);
+
+		spin_lock_init(&ring->pending_free_lock);
+		init_waitqueue_head(&ring->pending_free_wq);
+		init_waitqueue_head(&ring->shutdown_wq);
+		ring->blkif = blkif;
+		ring->st_print = jiffies;
+		xen_blkif_get(blkif);
 	}
+
+	return 0;
 }
 
 static struct xen_blkif *xen_blkif_alloc(domid_t domid)
@@ -133,41 +177,25 @@ static struct xen_blkif *xen_blkif_alloc(domid_t domid)
 		return ERR_PTR(-ENOMEM);
 
 	blkif->domid = domid;
-	spin_lock_init(&blkif->blk_ring_lock);
 	atomic_set(&blkif->refcnt, 1);
-	init_waitqueue_head(&blkif->wq);
 	init_completion(&blkif->drain_complete);
-	atomic_set(&blkif->drain, 0);
-	blkif->st_print = jiffies;
-	blkif->persistent_gnts.rb_node = NULL;
-	spin_lock_init(&blkif->free_pages_lock);
-	INIT_LIST_HEAD(&blkif->free_pages);
-	INIT_LIST_HEAD(&blkif->persistent_purge_list);
-	blkif->free_pages_num = 0;
-	atomic_set(&blkif->persistent_gnt_in_use, 0);
-	atomic_set(&blkif->inflight, 0);
-	INIT_WORK(&blkif->persistent_purge_work, xen_blkbk_unmap_purged_grants);
-
-	INIT_LIST_HEAD(&blkif->pending_free);
 	INIT_WORK(&blkif->free_work, xen_blkif_deferred_free);
-	spin_lock_init(&blkif->pending_free_lock);
-	init_waitqueue_head(&blkif->pending_free_wq);
-	init_waitqueue_head(&blkif->shutdown_wq);
 
 	return blkif;
 }
 
-static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t *gref,
+static int xen_blkif_map(struct xen_blkif_ring *ring, grant_ref_t *gref,
 			 unsigned int nr_grefs, unsigned int evtchn)
 {
 	int err;
+	struct xen_blkif *blkif = ring->blkif;
 
 	/* Already connected through? */
-	if (blkif->irq)
+	if (ring->irq)
 		return 0;
 
 	err = xenbus_map_ring_valloc(blkif->be->dev, gref, nr_grefs,
-				     &blkif->blk_ring);
+				     &ring->blk_ring);
 	if (err < 0)
 		return err;
 
@@ -175,24 +203,24 @@ static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t *gref,
 	case BLKIF_PROTOCOL_NATIVE:
 	{
 		struct blkif_sring *sring;
-		sring = (struct blkif_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.native, sring,
+		sring = (struct blkif_sring *)ring->blk_ring;
+		BACK_RING_INIT(&ring->blk_rings.native, sring,
 			       XEN_PAGE_SIZE * nr_grefs);
 		break;
 	}
 	case BLKIF_PROTOCOL_X86_32:
 	{
 		struct blkif_x86_32_sring *sring_x86_32;
-		sring_x86_32 = (struct blkif_x86_32_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.x86_32, sring_x86_32,
+		sring_x86_32 = (struct blkif_x86_32_sring *)ring->blk_ring;
+		BACK_RING_INIT(&ring->blk_rings.x86_32, sring_x86_32,
 			       XEN_PAGE_SIZE * nr_grefs);
 		break;
 	}
 	case BLKIF_PROTOCOL_X86_64:
 	{
 		struct blkif_x86_64_sring *sring_x86_64;
-		sring_x86_64 = (struct blkif_x86_64_sring *)blkif->blk_ring;
-		BACK_RING_INIT(&blkif->blk_rings.x86_64, sring_x86_64,
+		sring_x86_64 = (struct blkif_x86_64_sring *)ring->blk_ring;
+		BACK_RING_INIT(&ring->blk_rings.x86_64, sring_x86_64,
 			       XEN_PAGE_SIZE * nr_grefs);
 		break;
 	}
@@ -202,13 +230,13 @@ static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t *gref,
 
 	err = bind_interdomain_evtchn_to_irqhandler(blkif->domid, evtchn,
 						    xen_blkif_be_int, 0,
-						    "blkif-backend", blkif);
+						    "blkif-backend", ring);
 	if (err < 0) {
-		xenbus_unmap_ring_vfree(blkif->be->dev, blkif->blk_ring);
-		blkif->blk_rings.common.sring = NULL;
+		xenbus_unmap_ring_vfree(blkif->be->dev, ring->blk_ring);
+		ring->blk_rings.common.sring = NULL;
 		return err;
 	}
-	blkif->irq = err;
+	ring->irq = err;
 
 	return 0;
 }
@@ -216,50 +244,69 @@ static int xen_blkif_map(struct xen_blkif *blkif, grant_ref_t *gref,
 static int xen_blkif_disconnect(struct xen_blkif *blkif)
 {
 	struct pending_req *req, *n;
-	int i = 0, j;
+	unsigned int j, r;
 
-	if (blkif->xenblkd) {
-		kthread_stop(blkif->xenblkd);
-		wake_up(&blkif->shutdown_wq);
-		blkif->xenblkd = NULL;
-	}
+	for (r = 0; r < blkif->nr_rings; r++) {
+		struct xen_blkif_ring *ring = &blkif->rings[r];
+		unsigned int i = 0;
 
-	/* The above kthread_stop() guarantees that at this point we
-	 * don't have any discard_io or other_io requests. So, checking
-	 * for inflight IO is enough.
-	 */
-	if (atomic_read(&blkif->inflight) > 0)
-		return -EBUSY;
+		if (ring->xenblkd) {
+			kthread_stop(ring->xenblkd);
+			wake_up(&ring->shutdown_wq);
+			ring->xenblkd = NULL;
+		}
 
-	if (blkif->irq) {
-		unbind_from_irqhandler(blkif->irq, blkif);
-		blkif->irq = 0;
-	}
+		/* The above kthread_stop() guarantees that at this point we
+		 * don't have any discard_io or other_io requests. So, checking
+		 * for inflight IO is enough.
+		 */
+		if (atomic_read(&ring->inflight) > 0)
+			return -EBUSY;
 
-	if (blkif->blk_rings.common.sring) {
-		xenbus_unmap_ring_vfree(blkif->be->dev, blkif->blk_ring);
-		blkif->blk_rings.common.sring = NULL;
-	}
+		if (ring->irq) {
+			unbind_from_irqhandler(ring->irq, ring);
+			ring->irq = 0;
+		}
 
-	/* Remove all persistent grants and the cache of ballooned pages. */
-	xen_blkbk_free_caches(blkif);
+		if (ring->blk_rings.common.sring) {
+			xenbus_unmap_ring_vfree(blkif->be->dev, ring->blk_ring);
+			ring->blk_rings.common.sring = NULL;
+		}
 
-	/* Check that there is no request in use */
-	list_for_each_entry_safe(req, n, &blkif->pending_free, free_list) {
-		list_del(&req->free_list);
+		/* Remove all persistent grants and the cache of ballooned pages. */
+		xen_blkbk_free_caches(ring);
 
-		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++)
-			kfree(req->segments[j]);
+		/* Check that there is no request in use */
+		list_for_each_entry_safe(req, n, &ring->pending_free, free_list) {
+			list_del(&req->free_list);
 
-		for (j = 0; j < MAX_INDIRECT_PAGES; j++)
-			kfree(req->indirect_pages[j]);
+			for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++)
+				kfree(req->segments[j]);
 
-		kfree(req);
-		i++;
-	}
+			for (j = 0; j < MAX_INDIRECT_PAGES; j++)
+				kfree(req->indirect_pages[j]);
+
+			kfree(req);
+			i++;
+		}
 
-	WARN_ON(i != (XEN_BLKIF_REQS_PER_PAGE * blkif->nr_ring_pages));
+		BUG_ON(atomic_read(&ring->persistent_gnt_in_use) != 0);
+		BUG_ON(!list_empty(&ring->persistent_purge_list));
+		BUG_ON(!RB_EMPTY_ROOT(&ring->persistent_gnts));
+		BUG_ON(!list_empty(&ring->free_pages));
+		BUG_ON(ring->free_pages_num != 0);
+		BUG_ON(ring->persistent_gnt_c != 0);
+		WARN_ON(i != (XEN_BLKIF_REQS_PER_PAGE * blkif->nr_ring_pages));
+		xen_blkif_put(blkif);
+	}
 	blkif->nr_ring_pages = 0;
+	/*
+	 * blkif->rings was allocated in connect_ring, so we should free it in
+	 * here.
+	 */
+	kfree(blkif->rings);
+	blkif->rings = NULL;
+	blkif->nr_rings = 0;
 
 	return 0;
 }
@@ -271,13 +318,6 @@ static void xen_blkif_free(struct xen_blkif *blkif)
 	xen_vbd_free(&blkif->vbd);
 
 	/* Make sure everything is drained before shutting down */
-	BUG_ON(blkif->persistent_gnt_c != 0);
-	BUG_ON(atomic_read(&blkif->persistent_gnt_in_use) != 0);
-	BUG_ON(blkif->free_pages_num != 0);
-	BUG_ON(!list_empty(&blkif->persistent_purge_list));
-	BUG_ON(!list_empty(&blkif->free_pages));
-	BUG_ON(!RB_EMPTY_ROOT(&blkif->persistent_gnts));
-
 	kmem_cache_free(xen_blkif_cachep, blkif);
 }
 
@@ -296,25 +336,38 @@ int __init xen_blkif_interface_init(void)
  *  sysfs interface for VBD I/O requests
  */
 
-#define VBD_SHOW(name, format, args...)					\
+#define VBD_SHOW_ALLRING(name, format)					\
 	static ssize_t show_##name(struct device *_dev,			\
 				   struct device_attribute *attr,	\
 				   char *buf)				\
 	{								\
 		struct xenbus_device *dev = to_xenbus_device(_dev);	\
 		struct backend_info *be = dev_get_drvdata(&dev->dev);	\
+		struct xen_blkif *blkif = be->blkif;			\
+		unsigned int i;						\
+		unsigned long long result = 0;				\
 									\
-		return sprintf(buf, format, ##args);			\
+		if (!blkif->rings)				\
+			goto out;					\
+									\
+		for (i = 0; i < blkif->nr_rings; i++) {		\
+			struct xen_blkif_ring *ring = &blkif->rings[i];	\
+									\
+			result += ring->st_##name;			\
+		}							\
+									\
+out:									\
+		return sprintf(buf, format, result);			\
 	}								\
 	static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL)
 
-VBD_SHOW(oo_req,  "%llu\n", be->blkif->st_oo_req);
-VBD_SHOW(rd_req,  "%llu\n", be->blkif->st_rd_req);
-VBD_SHOW(wr_req,  "%llu\n", be->blkif->st_wr_req);
-VBD_SHOW(f_req,  "%llu\n", be->blkif->st_f_req);
-VBD_SHOW(ds_req,  "%llu\n", be->blkif->st_ds_req);
-VBD_SHOW(rd_sect, "%llu\n", be->blkif->st_rd_sect);
-VBD_SHOW(wr_sect, "%llu\n", be->blkif->st_wr_sect);
+VBD_SHOW_ALLRING(oo_req,  "%llu\n");
+VBD_SHOW_ALLRING(rd_req,  "%llu\n");
+VBD_SHOW_ALLRING(wr_req,  "%llu\n");
+VBD_SHOW_ALLRING(f_req,  "%llu\n");
+VBD_SHOW_ALLRING(ds_req,  "%llu\n");
+VBD_SHOW_ALLRING(rd_sect, "%llu\n");
+VBD_SHOW_ALLRING(wr_sect, "%llu\n");
 
 static struct attribute *xen_vbdstat_attrs[] = {
 	&dev_attr_oo_req.attr,
@@ -332,6 +385,18 @@ static struct attribute_group xen_vbdstat_group = {
 	.attrs = xen_vbdstat_attrs,
 };
 
+#define VBD_SHOW(name, format, args...)					\
+	static ssize_t show_##name(struct device *_dev,			\
+				   struct device_attribute *attr,	\
+				   char *buf)				\
+	{								\
+		struct xenbus_device *dev = to_xenbus_device(_dev);	\
+		struct backend_info *be = dev_get_drvdata(&dev->dev);	\
+									\
+		return sprintf(buf, format, ##args);			\
+	}								\
+	static DEVICE_ATTR(name, S_IRUGO, show_##name, NULL)
+
 VBD_SHOW(physical_device, "%x:%x\n", be->major, be->minor);
 VBD_SHOW(mode, "%s\n", be->mode);
 
@@ -440,11 +505,11 @@ static int xen_blkbk_remove(struct xenbus_device *dev)
 
 	dev_set_drvdata(&dev->dev, NULL);
 
-	if (be->blkif) {
+	if (be->blkif)
 		xen_blkif_disconnect(be->blkif);
-		xen_blkif_put(be->blkif);
-	}
 
+	/* Put the reference we set in xen_blkif_alloc(). */
+	xen_blkif_put(be->blkif);
 	kfree(be->mode);
 	kfree(be);
 	return 0;
@@ -553,6 +618,12 @@ static int xen_blkbk_probe(struct xenbus_device *dev,
 		goto fail;
 	}
 
+	/* Multi-queue: advertise how many queues are supported by us.*/
+	err = xenbus_printf(XBT_NIL, dev->nodename,
+			    "multi-queue-max-queues", "%u", xenblk_max_queues);
+	if (err)
+		pr_warn("Error writing multi-queue-max-queues\n");
+
 	/* setup back pointer */
 	be->blkif->be = be;
 
@@ -708,8 +779,14 @@ static void frontend_changed(struct xenbus_device *dev,
 		}
 
 		err = connect_ring(be);
-		if (err)
+		if (err) {
+			/*
+			 * Clean up so that memory resources can be used by
+			 * other devices. connect_ring reported already error.
+			 */
+			xen_blkif_disconnect(be->blkif);
 			break;
+		}
 		xen_update_blkif_status(be->blkif);
 		break;
 
@@ -825,50 +902,43 @@ again:
 	xenbus_transaction_end(xbt, 1);
 }
 
-
-static int connect_ring(struct backend_info *be)
+/*
+ * Each ring may have multi pages, depends on "ring-page-order".
+ */
+static int read_per_ring_refs(struct xen_blkif_ring *ring, const char *dir)
 {
-	struct xenbus_device *dev = be->dev;
 	unsigned int ring_ref[XENBUS_MAX_RING_GRANTS];
-	unsigned int evtchn, nr_grefs, ring_page_order;
-	unsigned int pers_grants;
-	char protocol[64] = "";
 	struct pending_req *req, *n;
 	int err, i, j;
+	struct xen_blkif *blkif = ring->blkif;
+	struct xenbus_device *dev = blkif->be->dev;
+	unsigned int ring_page_order, nr_grefs, evtchn;
 
-	pr_debug("%s %s\n", __func__, dev->otherend);
-
-	err = xenbus_scanf(XBT_NIL, dev->otherend, "event-channel", "%u",
+	err = xenbus_scanf(XBT_NIL, dir, "event-channel", "%u",
 			  &evtchn);
 	if (err != 1) {
 		err = -EINVAL;
-		xenbus_dev_fatal(dev, err, "reading %s/event-channel",
-				 dev->otherend);
+		xenbus_dev_fatal(dev, err, "reading %s/event-channel", dir);
 		return err;
 	}
-	pr_info("event-channel %u\n", evtchn);
 
 	err = xenbus_scanf(XBT_NIL, dev->otherend, "ring-page-order", "%u",
 			  &ring_page_order);
 	if (err != 1) {
-		err = xenbus_scanf(XBT_NIL, dev->otherend, "ring-ref",
-				  "%u", &ring_ref[0]);
+		err = xenbus_scanf(XBT_NIL, dir, "ring-ref", "%u", &ring_ref[0]);
 		if (err != 1) {
 			err = -EINVAL;
-			xenbus_dev_fatal(dev, err, "reading %s/ring-ref",
-					 dev->otherend);
+			xenbus_dev_fatal(dev, err, "reading %s/ring-ref", dir);
 			return err;
 		}
 		nr_grefs = 1;
-		pr_info("%s:using single page: ring-ref %d\n", dev->otherend,
-			ring_ref[0]);
 	} else {
 		unsigned int i;
 
 		if (ring_page_order > xen_blkif_max_ring_order) {
 			err = -EINVAL;
 			xenbus_dev_fatal(dev, err, "%s/request %d ring page order exceed max:%d",
-					 dev->otherend, ring_page_order,
+					 dir, ring_page_order,
 					 xen_blkif_max_ring_order);
 			return err;
 		}
@@ -878,52 +948,23 @@ static int connect_ring(struct backend_info *be)
 			char ring_ref_name[RINGREF_NAME_LEN];
 
 			snprintf(ring_ref_name, RINGREF_NAME_LEN, "ring-ref%u", i);
-			err = xenbus_scanf(XBT_NIL, dev->otherend, ring_ref_name,
+			err = xenbus_scanf(XBT_NIL, dir, ring_ref_name,
 					   "%u", &ring_ref[i]);
 			if (err != 1) {
 				err = -EINVAL;
 				xenbus_dev_fatal(dev, err, "reading %s/%s",
-						 dev->otherend, ring_ref_name);
+						 dir, ring_ref_name);
 				return err;
 			}
-			pr_info("ring-ref%u: %u\n", i, ring_ref[i]);
 		}
 	}
-
-	be->blkif->blk_protocol = BLKIF_PROTOCOL_DEFAULT;
-	err = xenbus_gather(XBT_NIL, dev->otherend, "protocol",
-			    "%63s", protocol, NULL);
-	if (err)
-		strcpy(protocol, "unspecified, assuming default");
-	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_NATIVE))
-		be->blkif->blk_protocol = BLKIF_PROTOCOL_NATIVE;
-	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_X86_32))
-		be->blkif->blk_protocol = BLKIF_PROTOCOL_X86_32;
-	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_X86_64))
-		be->blkif->blk_protocol = BLKIF_PROTOCOL_X86_64;
-	else {
-		xenbus_dev_fatal(dev, err, "unknown fe protocol %s", protocol);
-		return -1;
-	}
-	err = xenbus_gather(XBT_NIL, dev->otherend,
-			    "feature-persistent", "%u",
-			    &pers_grants, NULL);
-	if (err)
-		pers_grants = 0;
-
-	be->blkif->vbd.feature_gnt_persistent = pers_grants;
-	be->blkif->vbd.overflow_max_grants = 0;
-	be->blkif->nr_ring_pages = nr_grefs;
-
-	pr_info("ring-pages:%d, event-channel %d, protocol %d (%s) %s\n",
-		nr_grefs, evtchn, be->blkif->blk_protocol, protocol,
-		pers_grants ? "persistent grants" : "");
+	blkif->nr_ring_pages = nr_grefs;
 
 	for (i = 0; i < nr_grefs * XEN_BLKIF_REQS_PER_PAGE; i++) {
 		req = kzalloc(sizeof(*req), GFP_KERNEL);
 		if (!req)
 			goto fail;
-		list_add_tail(&req->free_list, &be->blkif->pending_free);
+		list_add_tail(&req->free_list, &ring->pending_free);
 		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
 			req->segments[j] = kzalloc(sizeof(*req->segments[0]), GFP_KERNEL);
 			if (!req->segments[j])
@@ -938,7 +979,7 @@ static int connect_ring(struct backend_info *be)
 	}
 
 	/* Map the shared frame, irq etc. */
-	err = xen_blkif_map(be->blkif, ring_ref, nr_grefs, evtchn);
+	err = xen_blkif_map(ring, ring_ref, nr_grefs, evtchn);
 	if (err) {
 		xenbus_dev_fatal(dev, err, "mapping ring-ref port %u", evtchn);
 		return err;
@@ -947,7 +988,7 @@ static int connect_ring(struct backend_info *be)
 	return 0;
 
 fail:
-	list_for_each_entry_safe(req, n, &be->blkif->pending_free, free_list) {
+	list_for_each_entry_safe(req, n, &ring->pending_free, free_list) {
 		list_del(&req->free_list);
 		for (j = 0; j < MAX_INDIRECT_SEGMENTS; j++) {
 			if (!req->segments[j])
@@ -962,6 +1003,93 @@ fail:
 		kfree(req);
 	}
 	return -ENOMEM;
+
+}
+
+static int connect_ring(struct backend_info *be)
+{
+	struct xenbus_device *dev = be->dev;
+	unsigned int pers_grants;
+	char protocol[64] = "";
+	int err, i;
+	char *xspath;
+	size_t xspathsize;
+	const size_t xenstore_path_ext_size = 11; /* sufficient for "/queue-NNN" */
+	unsigned int requested_num_queues = 0;
+
+	pr_debug("%s %s\n", __func__, dev->otherend);
+
+	be->blkif->blk_protocol = BLKIF_PROTOCOL_DEFAULT;
+	err = xenbus_gather(XBT_NIL, dev->otherend, "protocol",
+			    "%63s", protocol, NULL);
+	if (err)
+		strcpy(protocol, "unspecified, assuming default");
+	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_NATIVE))
+		be->blkif->blk_protocol = BLKIF_PROTOCOL_NATIVE;
+	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_X86_32))
+		be->blkif->blk_protocol = BLKIF_PROTOCOL_X86_32;
+	else if (0 == strcmp(protocol, XEN_IO_PROTO_ABI_X86_64))
+		be->blkif->blk_protocol = BLKIF_PROTOCOL_X86_64;
+	else {
+		xenbus_dev_fatal(dev, err, "unknown fe protocol %s", protocol);
+		return -ENOSYS;
+	}
+	err = xenbus_gather(XBT_NIL, dev->otherend,
+			    "feature-persistent", "%u",
+			    &pers_grants, NULL);
+	if (err)
+		pers_grants = 0;
+
+	be->blkif->vbd.feature_gnt_persistent = pers_grants;
+	be->blkif->vbd.overflow_max_grants = 0;
+
+	/*
+	 * Read the number of hardware queues from frontend.
+	 */
+	err = xenbus_scanf(XBT_NIL, dev->otherend, "multi-queue-num-queues",
+			   "%u", &requested_num_queues);
+	if (err < 0) {
+		requested_num_queues = 1;
+	} else {
+		if (requested_num_queues > xenblk_max_queues
+		    || requested_num_queues == 0) {
+			/* Buggy or malicious guest. */
+			xenbus_dev_fatal(dev, err,
+					"guest requested %u queues, exceeding the maximum of %u.",
+					requested_num_queues, xenblk_max_queues);
+			return -ENOSYS;
+		}
+	}
+	be->blkif->nr_rings = requested_num_queues;
+	if (xen_blkif_alloc_rings(be->blkif))
+		return -ENOMEM;
+
+	pr_info("%s: using %d queues, protocol %d (%s) %s\n", dev->nodename,
+		 be->blkif->nr_rings, be->blkif->blk_protocol, protocol,
+		 pers_grants ? "persistent grants" : "");
+
+	if (be->blkif->nr_rings == 1)
+		return read_per_ring_refs(&be->blkif->rings[0], dev->otherend);
+	else {
+		xspathsize = strlen(dev->otherend) + xenstore_path_ext_size;
+		xspath = kmalloc(xspathsize, GFP_KERNEL);
+		if (!xspath) {
+			xenbus_dev_fatal(dev, -ENOMEM, "reading ring references");
+			return -ENOMEM;
+		}
+
+		for (i = 0; i < be->blkif->nr_rings; i++) {
+			memset(xspath, 0, xspathsize);
+			snprintf(xspath, xspathsize, "%s/queue-%u", dev->otherend, i);
+			err = read_per_ring_refs(&be->blkif->rings[i], xspath);
+			if (err) {
+				kfree(xspath);
+				return err;
+			}
+		}
+		kfree(xspath);
+	}
+	return 0;
 }
 
 static const struct xenbus_device_id xen_blkbk_ids[] = {
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 2fee2eef988d..83eb9e6bf8b0 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -60,6 +60,20 @@
 
 #include <asm/xen/hypervisor.h>
 
+/*
+ * The minimal size of segment supported by the block framework is PAGE_SIZE.
+ * When Linux is using a different page size than Xen, it may not be possible
+ * to put all the data in a single segment.
+ * This can happen when the backend doesn't support indirect descriptor and
+ * therefore the maximum amount of data that a request can carry is
+ * BLKIF_MAX_SEGMENTS_PER_REQUEST * XEN_PAGE_SIZE = 44KB
+ *
+ * Note that we only support one extra request. So the Linux page size
+ * should be <= ( 2 * BLKIF_MAX_SEGMENTS_PER_REQUEST * XEN_PAGE_SIZE) =
+ * 88KB.
+ */
+#define HAS_EXTRA_REQ (BLKIF_MAX_SEGMENTS_PER_REQUEST < XEN_PFN_PER_PAGE)
+
 enum blkif_state {
 	BLKIF_STATE_DISCONNECTED,
 	BLKIF_STATE_CONNECTED,
@@ -72,6 +86,13 @@ struct grant {
 	struct list_head node;
 };
 
+enum blk_req_status {
+	REQ_WAITING,
+	REQ_DONE,
+	REQ_ERROR,
+	REQ_EOPNOTSUPP,
+};
+
 struct blk_shadow {
 	struct blkif_request req;
 	struct request *request;
@@ -79,6 +100,14 @@ struct blk_shadow {
 	struct grant **indirect_grants;
 	struct scatterlist *sg;
 	unsigned int num_sg;
+	enum blk_req_status status;
+
+	#define NO_ASSOCIATED_ID ~0UL
+	/*
+	 * Id of the sibling if we ever need 2 requests when handling a
+	 * block I/O request
+	 */
+	unsigned long associated_id;
 };
 
 struct split_bio {
@@ -99,6 +128,10 @@ static unsigned int xen_blkif_max_segments = 32;
 module_param_named(max, xen_blkif_max_segments, int, S_IRUGO);
 MODULE_PARM_DESC(max, "Maximum amount of segments in indirect requests (default is 32)");
 
+static unsigned int xen_blkif_max_queues = 4;
+module_param_named(max_queues, xen_blkif_max_queues, uint, S_IRUGO);
+MODULE_PARM_DESC(max_queues, "Maximum number of hardware queues/rings used per virtual disk");
+
 /*
  * Maximum order of pages to be used for the shared ring between front and
  * backend, 4KB page granularity is used.
@@ -114,10 +147,35 @@ MODULE_PARM_DESC(max_ring_page_order, "Maximum order of pages to be used for the
 	__CONST_RING_SIZE(blkif, XEN_PAGE_SIZE * XENBUS_MAX_RING_GRANTS)
 
 /*
- * ring-ref%i i=(-1UL) would take 11 characters + 'ring-ref' is 8, so 19
- * characters are enough. Define to 20 to keep consist with backend.
+ * ring-ref%u i=(-1UL) would take 11 characters + 'ring-ref' is 8, so 19
+ * characters are enough. Define to 20 to keep consistent with backend.
  */
 #define RINGREF_NAME_LEN (20)
+/*
+ * queue-%u would take 7 + 10(UINT_MAX) = 17 characters.
+ */
+#define QUEUE_NAME_LEN (17)
+
+/*
+ *  Per-ring info.
+ *  Every blkfront device can associate with one or more blkfront_ring_info,
+ *  depending on how many hardware queues/rings to be used.
+ */
+struct blkfront_ring_info {
+	/* Lock to protect data in every ring buffer. */
+	spinlock_t ring_lock;
+	struct blkif_front_ring ring;
+	unsigned int ring_ref[XENBUS_MAX_RING_GRANTS];
+	unsigned int evtchn, irq;
+	struct work_struct work;
+	struct gnttab_free_callback callback;
+	struct blk_shadow shadow[BLK_MAX_RING_SIZE];
+	struct list_head indirect_pages;
+	struct list_head grants;
+	unsigned int persistent_gnts_c;
+	unsigned long shadow_free;
+	struct blkfront_info *dev_info;
+};
 
 /*
  * We have one of these per vbd, whether ide, scsi or 'other'.  They
@@ -126,25 +184,15 @@ MODULE_PARM_DESC(max_ring_page_order, "Maximum order of pages to be used for the
  */
 struct blkfront_info
 {
-	spinlock_t io_lock;
 	struct mutex mutex;
 	struct xenbus_device *xbdev;
 	struct gendisk *gd;
 	int vdevice;
 	blkif_vdev_t handle;
 	enum blkif_state connected;
-	int ring_ref[XENBUS_MAX_RING_GRANTS];
+	/* Number of pages per ring buffer. */
 	unsigned int nr_ring_pages;
-	struct blkif_front_ring ring;
-	unsigned int evtchn, irq;
 	struct request_queue *rq;
-	struct work_struct work;
-	struct gnttab_free_callback callback;
-	struct blk_shadow shadow[BLK_MAX_RING_SIZE];
-	struct list_head grants;
-	struct list_head indirect_pages;
-	unsigned int persistent_gnts_c;
-	unsigned long shadow_free;
 	unsigned int feature_flush;
 	unsigned int feature_discard:1;
 	unsigned int feature_secdiscard:1;
@@ -155,6 +203,8 @@ struct blkfront_info
 	unsigned int max_indirect_segments;
 	int is_ready;
 	struct blk_mq_tag_set tag_set;
+	struct blkfront_ring_info *rinfo;
+	unsigned int nr_rings;
 };
 
 static unsigned int nr_minors;
@@ -198,38 +248,40 @@ static DEFINE_SPINLOCK(minor_lock);
 
 #define GREFS(_psegs)	((_psegs) * GRANTS_PER_PSEG)
 
-static int blkfront_setup_indirect(struct blkfront_info *info);
-static int blkfront_gather_backend_features(struct blkfront_info *info);
+static int blkfront_setup_indirect(struct blkfront_ring_info *rinfo);
+static void blkfront_gather_backend_features(struct blkfront_info *info);
 
-static int get_id_from_freelist(struct blkfront_info *info)
+static int get_id_from_freelist(struct blkfront_ring_info *rinfo)
 {
-	unsigned long free = info->shadow_free;
-	BUG_ON(free >= BLK_RING_SIZE(info));
-	info->shadow_free = info->shadow[free].req.u.rw.id;
-	info->shadow[free].req.u.rw.id = 0x0fffffee; /* debug */
+	unsigned long free = rinfo->shadow_free;
+
+	BUG_ON(free >= BLK_RING_SIZE(rinfo->dev_info));
+	rinfo->shadow_free = rinfo->shadow[free].req.u.rw.id;
+	rinfo->shadow[free].req.u.rw.id = 0x0fffffee; /* debug */
 	return free;
 }
 
-static int add_id_to_freelist(struct blkfront_info *info,
-			       unsigned long id)
+static int add_id_to_freelist(struct blkfront_ring_info *rinfo,
+			      unsigned long id)
 {
-	if (info->shadow[id].req.u.rw.id != id)
+	if (rinfo->shadow[id].req.u.rw.id != id)
 		return -EINVAL;
-	if (info->shadow[id].request == NULL)
+	if (rinfo->shadow[id].request == NULL)
 		return -EINVAL;
-	info->shadow[id].req.u.rw.id  = info->shadow_free;
-	info->shadow[id].request = NULL;
-	info->shadow_free = id;
+	rinfo->shadow[id].req.u.rw.id  = rinfo->shadow_free;
+	rinfo->shadow[id].request = NULL;
+	rinfo->shadow_free = id;
 	return 0;
 }
 
-static int fill_grant_buffer(struct blkfront_info *info, int num)
+static int fill_grant_buffer(struct blkfront_ring_info *rinfo, int num)
 {
+	struct blkfront_info *info = rinfo->dev_info;
 	struct page *granted_page;
 	struct grant *gnt_list_entry, *n;
 	int i = 0;
 
-	while(i < num) {
+	while (i < num) {
 		gnt_list_entry = kzalloc(sizeof(struct grant), GFP_NOIO);
 		if (!gnt_list_entry)
 			goto out_of_memory;
@@ -244,7 +296,7 @@ static int fill_grant_buffer(struct blkfront_info *info, int num)
 		}
 
 		gnt_list_entry->gref = GRANT_INVALID_REF;
-		list_add(&gnt_list_entry->node, &info->grants);
+		list_add(&gnt_list_entry->node, &rinfo->grants);
 		i++;
 	}
 
@@ -252,7 +304,7 @@ static int fill_grant_buffer(struct blkfront_info *info, int num)
 
 out_of_memory:
 	list_for_each_entry_safe(gnt_list_entry, n,
-	                         &info->grants, node) {
+	                         &rinfo->grants, node) {
 		list_del(&gnt_list_entry->node);
 		if (info->feature_persistent)
 			__free_page(gnt_list_entry->page);
@@ -263,17 +315,17 @@ out_of_memory:
 	return -ENOMEM;
 }
 
-static struct grant *get_free_grant(struct blkfront_info *info)
+static struct grant *get_free_grant(struct blkfront_ring_info *rinfo)
 {
 	struct grant *gnt_list_entry;
 
-	BUG_ON(list_empty(&info->grants));
-	gnt_list_entry = list_first_entry(&info->grants, struct grant,
+	BUG_ON(list_empty(&rinfo->grants));
+	gnt_list_entry = list_first_entry(&rinfo->grants, struct grant,
 					  node);
 	list_del(&gnt_list_entry->node);
 
 	if (gnt_list_entry->gref != GRANT_INVALID_REF)
-		info->persistent_gnts_c--;
+		rinfo->persistent_gnts_c--;
 
 	return gnt_list_entry;
 }
@@ -289,9 +341,10 @@ static inline void grant_foreign_access(const struct grant *gnt_list_entry,
 
 static struct grant *get_grant(grant_ref_t *gref_head,
 			       unsigned long gfn,
-			       struct blkfront_info *info)
+			       struct blkfront_ring_info *rinfo)
 {
-	struct grant *gnt_list_entry = get_free_grant(info);
+	struct grant *gnt_list_entry = get_free_grant(rinfo);
+	struct blkfront_info *info = rinfo->dev_info;
 
 	if (gnt_list_entry->gref != GRANT_INVALID_REF)
 		return gnt_list_entry;
@@ -312,9 +365,10 @@ static struct grant *get_grant(grant_ref_t *gref_head,
 }
 
 static struct grant *get_indirect_grant(grant_ref_t *gref_head,
-					struct blkfront_info *info)
+					struct blkfront_ring_info *rinfo)
 {
-	struct grant *gnt_list_entry = get_free_grant(info);
+	struct grant *gnt_list_entry = get_free_grant(rinfo);
+	struct blkfront_info *info = rinfo->dev_info;
 
 	if (gnt_list_entry->gref != GRANT_INVALID_REF)
 		return gnt_list_entry;
@@ -326,8 +380,8 @@ static struct grant *get_indirect_grant(grant_ref_t *gref_head,
 		struct page *indirect_page;
 
 		/* Fetch a pre-allocated page to use for indirect grefs */
-		BUG_ON(list_empty(&info->indirect_pages));
-		indirect_page = list_first_entry(&info->indirect_pages,
+		BUG_ON(list_empty(&rinfo->indirect_pages));
+		indirect_page = list_first_entry(&rinfo->indirect_pages,
 						 struct page, lru);
 		list_del(&indirect_page->lru);
 		gnt_list_entry->page = indirect_page;
@@ -403,8 +457,8 @@ static void xlbd_release_minors(unsigned int minor, unsigned int nr)
 
 static void blkif_restart_queue_callback(void *arg)
 {
-	struct blkfront_info *info = (struct blkfront_info *)arg;
-	schedule_work(&info->work);
+	struct blkfront_ring_info *rinfo = (struct blkfront_ring_info *)arg;
+	schedule_work(&rinfo->work);
 }
 
 static int blkif_getgeo(struct block_device *bd, struct hd_geometry *hg)
@@ -456,16 +510,33 @@ static int blkif_ioctl(struct block_device *bdev, fmode_t mode,
 	return 0;
 }
 
-static int blkif_queue_discard_req(struct request *req)
+static unsigned long blkif_ring_get_request(struct blkfront_ring_info *rinfo,
+					    struct request *req,
+					    struct blkif_request **ring_req)
+{
+	unsigned long id;
+
+	*ring_req = RING_GET_REQUEST(&rinfo->ring, rinfo->ring.req_prod_pvt);
+	rinfo->ring.req_prod_pvt++;
+
+	id = get_id_from_freelist(rinfo);
+	rinfo->shadow[id].request = req;
+	rinfo->shadow[id].status = REQ_WAITING;
+	rinfo->shadow[id].associated_id = NO_ASSOCIATED_ID;
+
+	(*ring_req)->u.rw.id = id;
+
+	return id;
+}
+
+static int blkif_queue_discard_req(struct request *req, struct blkfront_ring_info *rinfo)
 {
-	struct blkfront_info *info = req->rq_disk->private_data;
+	struct blkfront_info *info = rinfo->dev_info;
 	struct blkif_request *ring_req;
 	unsigned long id;
 
 	/* Fill out a communications ring structure. */
-	ring_req = RING_GET_REQUEST(&info->ring, info->ring.req_prod_pvt);
-	id = get_id_from_freelist(info);
-	info->shadow[id].request = req;
+	id = blkif_ring_get_request(rinfo, req, &ring_req);
 
 	ring_req->operation = BLKIF_OP_DISCARD;
 	ring_req->u.discard.nr_sectors = blk_rq_sectors(req);
@@ -476,10 +547,8 @@ static int blkif_queue_discard_req(struct request *req)
 	else
 		ring_req->u.discard.flag = 0;
 
-	info->ring.req_prod_pvt++;
-
 	/* Keep a private copy so we can reissue requests when recovering. */
-	info->shadow[id].req = *ring_req;
+	rinfo->shadow[id].req = *ring_req;
 
 	return 0;
 }
@@ -487,7 +556,7 @@ static int blkif_queue_discard_req(struct request *req)
 struct setup_rw_req {
 	unsigned int grant_idx;
 	struct blkif_request_segment *segments;
-	struct blkfront_info *info;
+	struct blkfront_ring_info *rinfo;
 	struct blkif_request *ring_req;
 	grant_ref_t gref_head;
 	unsigned int id;
@@ -495,6 +564,9 @@ struct setup_rw_req {
 	bool need_copy;
 	unsigned int bvec_off;
 	char *bvec_data;
+
+	bool require_extra_req;
+	struct blkif_request *extra_ring_req;
 };
 
 static void blkif_setup_rw_req_grant(unsigned long gfn, unsigned int offset,
@@ -507,8 +579,24 @@ static void blkif_setup_rw_req_grant(unsigned long gfn, unsigned int offset,
 	/* Convenient aliases */
 	unsigned int grant_idx = setup->grant_idx;
 	struct blkif_request *ring_req = setup->ring_req;
-	struct blkfront_info *info = setup->info;
-	struct blk_shadow *shadow = &info->shadow[setup->id];
+	struct blkfront_ring_info *rinfo = setup->rinfo;
+	/*
+	 * We always use the shadow of the first request to store the list
+	 * of grant associated to the block I/O request. This made the
+	 * completion more easy to handle even if the block I/O request is
+	 * split.
+	 */
+	struct blk_shadow *shadow = &rinfo->shadow[setup->id];
+
+	if (unlikely(setup->require_extra_req &&
+		     grant_idx >= BLKIF_MAX_SEGMENTS_PER_REQUEST)) {
+		/*
+		 * We are using the second request, setup grant_idx
+		 * to be the index of the segment array.
+		 */
+		grant_idx -= BLKIF_MAX_SEGMENTS_PER_REQUEST;
+		ring_req = setup->extra_ring_req;
+	}
 
 	if ((ring_req->operation == BLKIF_OP_INDIRECT) &&
 	    (grant_idx % GRANTS_PER_INDIRECT_FRAME == 0)) {
@@ -516,15 +604,19 @@ static void blkif_setup_rw_req_grant(unsigned long gfn, unsigned int offset,
 			kunmap_atomic(setup->segments);
 
 		n = grant_idx / GRANTS_PER_INDIRECT_FRAME;
-		gnt_list_entry = get_indirect_grant(&setup->gref_head, info);
+		gnt_list_entry = get_indirect_grant(&setup->gref_head, rinfo);
 		shadow->indirect_grants[n] = gnt_list_entry;
 		setup->segments = kmap_atomic(gnt_list_entry->page);
 		ring_req->u.indirect.indirect_grefs[n] = gnt_list_entry->gref;
 	}
 
-	gnt_list_entry = get_grant(&setup->gref_head, gfn, info);
+	gnt_list_entry = get_grant(&setup->gref_head, gfn, rinfo);
 	ref = gnt_list_entry->gref;
-	shadow->grants_used[grant_idx] = gnt_list_entry;
+	/*
+	 * All the grants are stored in the shadow of the first
+	 * request. Therefore we have to use the global index.
+	 */
+	shadow->grants_used[setup->grant_idx] = gnt_list_entry;
 
 	if (setup->need_copy) {
 		void *shared_data;
@@ -566,16 +658,36 @@ static void blkif_setup_rw_req_grant(unsigned long gfn, unsigned int offset,
 	(setup->grant_idx)++;
 }
 
-static int blkif_queue_rw_req(struct request *req)
+static void blkif_setup_extra_req(struct blkif_request *first,
+				  struct blkif_request *second)
 {
-	struct blkfront_info *info = req->rq_disk->private_data;
-	struct blkif_request *ring_req;
-	unsigned long id;
+	uint16_t nr_segments = first->u.rw.nr_segments;
+
+	/*
+	 * The second request is only present when the first request uses
+	 * all its segments. It's always the continuity of the first one.
+	 */
+	first->u.rw.nr_segments = BLKIF_MAX_SEGMENTS_PER_REQUEST;
+
+	second->u.rw.nr_segments = nr_segments - BLKIF_MAX_SEGMENTS_PER_REQUEST;
+	second->u.rw.sector_number = first->u.rw.sector_number +
+		(BLKIF_MAX_SEGMENTS_PER_REQUEST * XEN_PAGE_SIZE) / 512;
+
+	second->u.rw.handle = first->u.rw.handle;
+	second->operation = first->operation;
+}
+
+static int blkif_queue_rw_req(struct request *req, struct blkfront_ring_info *rinfo)
+{
+	struct blkfront_info *info = rinfo->dev_info;
+	struct blkif_request *ring_req, *extra_ring_req = NULL;
+	unsigned long id, extra_id = NO_ASSOCIATED_ID;
+	bool require_extra_req = false;
 	int i;
 	struct setup_rw_req setup = {
 		.grant_idx = 0,
 		.segments = NULL,
-		.info = info,
+		.rinfo = rinfo,
 		.need_copy = rq_data_dir(req) && info->feature_persistent,
 	};
 
@@ -584,7 +696,6 @@ static int blkif_queue_rw_req(struct request *req)
 	 * existing persistent grants, or if we have to get new grants,
 	 * as there are not sufficiently many free.
 	 */
-	bool new_persistent_gnts;
 	struct scatterlist *sg;
 	int num_sg, max_grefs, num_grant;
 
@@ -596,41 +707,36 @@ static int blkif_queue_rw_req(struct request *req)
 		 */
 		max_grefs += INDIRECT_GREFS(max_grefs);
 
-	/* Check if we have enough grants to allocate a requests */
-	if (info->persistent_gnts_c < max_grefs) {
-		new_persistent_gnts = 1;
-		if (gnttab_alloc_grant_references(
-		    max_grefs - info->persistent_gnts_c,
-		    &setup.gref_head) < 0) {
+	/*
+	 * We have to reserve 'max_grefs' grants because persistent
+	 * grants are shared by all rings.
+	 */
+	if (max_grefs > 0)
+		if (gnttab_alloc_grant_references(max_grefs, &setup.gref_head) < 0) {
 			gnttab_request_free_callback(
-				&info->callback,
+				&rinfo->callback,
 				blkif_restart_queue_callback,
-				info,
+				rinfo,
 				max_grefs);
 			return 1;
 		}
-	} else
-		new_persistent_gnts = 0;
 
 	/* Fill out a communications ring structure. */
-	ring_req = RING_GET_REQUEST(&info->ring, info->ring.req_prod_pvt);
-	id = get_id_from_freelist(info);
-	info->shadow[id].request = req;
+	id = blkif_ring_get_request(rinfo, req, &ring_req);
 
-	BUG_ON(info->max_indirect_segments == 0 &&
-	       GREFS(req->nr_phys_segments) > BLKIF_MAX_SEGMENTS_PER_REQUEST);
-	BUG_ON(info->max_indirect_segments &&
-	       GREFS(req->nr_phys_segments) > info->max_indirect_segments);
-
-	num_sg = blk_rq_map_sg(req->q, req, info->shadow[id].sg);
+	num_sg = blk_rq_map_sg(req->q, req, rinfo->shadow[id].sg);
 	num_grant = 0;
 	/* Calculate the number of grant used */
-	for_each_sg(info->shadow[id].sg, sg, num_sg, i)
+	for_each_sg(rinfo->shadow[id].sg, sg, num_sg, i)
 	       num_grant += gnttab_count_grant(sg->offset, sg->length);
 
-	ring_req->u.rw.id = id;
-	info->shadow[id].num_sg = num_sg;
-	if (num_grant > BLKIF_MAX_SEGMENTS_PER_REQUEST) {
+	require_extra_req = info->max_indirect_segments == 0 &&
+		num_grant > BLKIF_MAX_SEGMENTS_PER_REQUEST;
+	BUG_ON(!HAS_EXTRA_REQ && require_extra_req);
+
+	rinfo->shadow[id].num_sg = num_sg;
+	if (num_grant > BLKIF_MAX_SEGMENTS_PER_REQUEST &&
+	    likely(!require_extra_req)) {
 		/*
 		 * The indirect operation can only be a BLKIF_OP_READ or
 		 * BLKIF_OP_WRITE
@@ -670,11 +776,31 @@ static int blkif_queue_rw_req(struct request *req)
 			}
 		}
 		ring_req->u.rw.nr_segments = num_grant;
+		if (unlikely(require_extra_req)) {
+			extra_id = blkif_ring_get_request(rinfo, req,
+							  &extra_ring_req);
+			/*
+			 * Only the first request contains the scatter-gather
+			 * list.
+			 */
+			rinfo->shadow[extra_id].num_sg = 0;
+
+			blkif_setup_extra_req(ring_req, extra_ring_req);
+
+			/* Link the 2 requests together */
+			rinfo->shadow[extra_id].associated_id = id;
+			rinfo->shadow[id].associated_id = extra_id;
+		}
 	}
 
 	setup.ring_req = ring_req;
 	setup.id = id;
-	for_each_sg(info->shadow[id].sg, sg, num_sg, i) {
+
+	setup.require_extra_req = require_extra_req;
+	if (unlikely(require_extra_req))
+		setup.extra_ring_req = extra_ring_req;
+
+	for_each_sg(rinfo->shadow[id].sg, sg, num_sg, i) {
 		BUG_ON(sg->offset + sg->length > PAGE_SIZE);
 
 		if (setup.need_copy) {
@@ -694,12 +820,12 @@ static int blkif_queue_rw_req(struct request *req)
 	if (setup.segments)
 		kunmap_atomic(setup.segments);
 
-	info->ring.req_prod_pvt++;
-
 	/* Keep a private copy so we can reissue requests when recovering. */
-	info->shadow[id].req = *ring_req;
+	rinfo->shadow[id].req = *ring_req;
+	if (unlikely(require_extra_req))
+		rinfo->shadow[extra_id].req = *extra_ring_req;
 
-	if (new_persistent_gnts)
+	if (max_grefs > 0)
 		gnttab_free_grant_references(setup.gref_head);
 
 	return 0;
@@ -711,27 +837,25 @@ static int blkif_queue_rw_req(struct request *req)
  *
  * @req: a request struct
  */
-static int blkif_queue_request(struct request *req)
+static int blkif_queue_request(struct request *req, struct blkfront_ring_info *rinfo)
 {
-	struct blkfront_info *info = req->rq_disk->private_data;
-
-	if (unlikely(info->connected != BLKIF_STATE_CONNECTED))
+	if (unlikely(rinfo->dev_info->connected != BLKIF_STATE_CONNECTED))
 		return 1;
 
 	if (unlikely(req->cmd_flags & (REQ_DISCARD | REQ_SECURE)))
-		return blkif_queue_discard_req(req);
+		return blkif_queue_discard_req(req, rinfo);
 	else
-		return blkif_queue_rw_req(req);
+		return blkif_queue_rw_req(req, rinfo);
 }
 
-static inline void flush_requests(struct blkfront_info *info)
+static inline void flush_requests(struct blkfront_ring_info *rinfo)
 {
 	int notify;
 
-	RING_PUSH_REQUESTS_AND_CHECK_NOTIFY(&info->ring, notify);
+	RING_PUSH_REQUESTS_AND_CHECK_NOTIFY(&rinfo->ring, notify);
 
 	if (notify)
-		notify_remote_via_irq(info->irq);
+		notify_remote_via_irq(rinfo->irq);
 }
 
 static inline bool blkif_request_flush_invalid(struct request *req,
@@ -745,38 +869,50 @@ static inline bool blkif_request_flush_invalid(struct request *req,
 }
 
 static int blkif_queue_rq(struct blk_mq_hw_ctx *hctx,
-			   const struct blk_mq_queue_data *qd)
+			  const struct blk_mq_queue_data *qd)
 {
-	struct blkfront_info *info = qd->rq->rq_disk->private_data;
+	unsigned long flags;
+	struct blkfront_ring_info *rinfo = (struct blkfront_ring_info *)hctx->driver_data;
 
 	blk_mq_start_request(qd->rq);
-	spin_lock_irq(&info->io_lock);
-	if (RING_FULL(&info->ring))
+	spin_lock_irqsave(&rinfo->ring_lock, flags);
+	if (RING_FULL(&rinfo->ring))
 		goto out_busy;
 
-	if (blkif_request_flush_invalid(qd->rq, info))
+	if (blkif_request_flush_invalid(qd->rq, rinfo->dev_info))
 		goto out_err;
 
-	if (blkif_queue_request(qd->rq))
+	if (blkif_queue_request(qd->rq, rinfo))
 		goto out_busy;
 
-	flush_requests(info);
-	spin_unlock_irq(&info->io_lock);
+	flush_requests(rinfo);
+	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 	return BLK_MQ_RQ_QUEUE_OK;
 
 out_err:
-	spin_unlock_irq(&info->io_lock);
+	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 	return BLK_MQ_RQ_QUEUE_ERROR;
 
 out_busy:
-	spin_unlock_irq(&info->io_lock);
+	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 	blk_mq_stop_hw_queue(hctx);
 	return BLK_MQ_RQ_QUEUE_BUSY;
 }
 
+static int blk_mq_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
+			    unsigned int index)
+{
+	struct blkfront_info *info = (struct blkfront_info *)data;
+
+	BUG_ON(info->nr_rings <= index);
+	hctx->driver_data = &info->rinfo[index];
+	return 0;
+}
+
 static struct blk_mq_ops blkfront_mq_ops = {
 	.queue_rq = blkif_queue_rq,
 	.map_queue = blk_mq_map_queue,
+	.init_hctx = blk_mq_init_hctx,
 };
 
 static int xlvbd_init_blk_queue(struct gendisk *gd, u16 sector_size,
@@ -788,19 +924,28 @@ static int xlvbd_init_blk_queue(struct gendisk *gd, u16 sector_size,
 
 	memset(&info->tag_set, 0, sizeof(info->tag_set));
 	info->tag_set.ops = &blkfront_mq_ops;
-	info->tag_set.nr_hw_queues = 1;
-	info->tag_set.queue_depth =  BLK_RING_SIZE(info);
+	info->tag_set.nr_hw_queues = info->nr_rings;
+	if (HAS_EXTRA_REQ && info->max_indirect_segments == 0) {
+		/*
+		 * When indirect descriptior is not supported, the I/O request
+		 * will be split between multiple request in the ring.
+		 * To avoid problems when sending the request, divide by
+		 * 2 the depth of the queue.
+		 */
+		info->tag_set.queue_depth =  BLK_RING_SIZE(info) / 2;
+	} else
+		info->tag_set.queue_depth = BLK_RING_SIZE(info);
 	info->tag_set.numa_node = NUMA_NO_NODE;
 	info->tag_set.flags = BLK_MQ_F_SHOULD_MERGE | BLK_MQ_F_SG_MERGE;
 	info->tag_set.cmd_size = 0;
 	info->tag_set.driver_data = info;
 
 	if (blk_mq_alloc_tag_set(&info->tag_set))
-		return -1;
+		return -EINVAL;
 	rq = blk_mq_init_queue(&info->tag_set);
 	if (IS_ERR(rq)) {
 		blk_mq_free_tag_set(&info->tag_set);
-		return -1;
+		return PTR_ERR(rq);
 	}
 
 	queue_flag_set_unlocked(QUEUE_FLAG_VIRT, rq);
@@ -1028,7 +1173,7 @@ static int xlvbd_alloc_gendisk(blkif_sector_t capacity,
 
 static void xlvbd_release_gendisk(struct blkfront_info *info)
 {
-	unsigned int minor, nr_minors;
+	unsigned int minor, nr_minors, i;
 
 	if (info->rq == NULL)
 		return;
@@ -1036,11 +1181,15 @@ static void xlvbd_release_gendisk(struct blkfront_info *info)
 	/* No more blkif_request(). */
 	blk_mq_stop_hw_queues(info->rq);
 
-	/* No more gnttab callback work. */
-	gnttab_cancel_free_callback(&info->callback);
+	for (i = 0; i < info->nr_rings; i++) {
+		struct blkfront_ring_info *rinfo = &info->rinfo[i];
 
-	/* Flush gnttab callback work. Must be done with no locks held. */
-	flush_work(&info->work);
+		/* No more gnttab callback work. */
+		gnttab_cancel_free_callback(&rinfo->callback);
+
+		/* Flush gnttab callback work. Must be done with no locks held. */
+		flush_work(&rinfo->work);
+	}
 
 	del_gendisk(info->gd);
 
@@ -1056,88 +1205,87 @@ static void xlvbd_release_gendisk(struct blkfront_info *info)
 	info->gd = NULL;
 }
 
-/* Must be called with io_lock holded */
-static void kick_pending_request_queues(struct blkfront_info *info)
+/* Already hold rinfo->ring_lock. */
+static inline void kick_pending_request_queues_locked(struct blkfront_ring_info *rinfo)
 {
-	if (!RING_FULL(&info->ring))
-		blk_mq_start_stopped_hw_queues(info->rq, true);
+	if (!RING_FULL(&rinfo->ring))
+		blk_mq_start_stopped_hw_queues(rinfo->dev_info->rq, true);
 }
 
-static void blkif_restart_queue(struct work_struct *work)
+static void kick_pending_request_queues(struct blkfront_ring_info *rinfo)
 {
-	struct blkfront_info *info = container_of(work, struct blkfront_info, work);
+	unsigned long flags;
 
-	spin_lock_irq(&info->io_lock);
-	if (info->connected == BLKIF_STATE_CONNECTED)
-		kick_pending_request_queues(info);
-	spin_unlock_irq(&info->io_lock);
+	spin_lock_irqsave(&rinfo->ring_lock, flags);
+	kick_pending_request_queues_locked(rinfo);
+	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 }
 
-static void blkif_free(struct blkfront_info *info, int suspend)
+static void blkif_restart_queue(struct work_struct *work)
 {
-	struct grant *persistent_gnt;
-	struct grant *n;
-	int i, j, segs;
+	struct blkfront_ring_info *rinfo = container_of(work, struct blkfront_ring_info, work);
 
-	/* Prevent new requests being issued until we fix things up. */
-	spin_lock_irq(&info->io_lock);
-	info->connected = suspend ?
-		BLKIF_STATE_SUSPENDED : BLKIF_STATE_DISCONNECTED;
-	/* No more blkif_request(). */
-	if (info->rq)
-		blk_mq_stop_hw_queues(info->rq);
+	if (rinfo->dev_info->connected == BLKIF_STATE_CONNECTED)
+		kick_pending_request_queues(rinfo);
+}
 
-	/* Remove all persistent grants */
-	if (!list_empty(&info->grants)) {
-		list_for_each_entry_safe(persistent_gnt, n,
-		                         &info->grants, node) {
-			list_del(&persistent_gnt->node);
-			if (persistent_gnt->gref != GRANT_INVALID_REF) {
-				gnttab_end_foreign_access(persistent_gnt->gref,
-				                          0, 0UL);
-				info->persistent_gnts_c--;
-			}
-			if (info->feature_persistent)
-				__free_page(persistent_gnt->page);
-			kfree(persistent_gnt);
-		}
-	}
-	BUG_ON(info->persistent_gnts_c != 0);
+static void blkif_free_ring(struct blkfront_ring_info *rinfo)
+{
+	struct grant *persistent_gnt, *n;
+	struct blkfront_info *info = rinfo->dev_info;
+	int i, j, segs;
 
 	/*
 	 * Remove indirect pages, this only happens when using indirect
 	 * descriptors but not persistent grants
 	 */
-	if (!list_empty(&info->indirect_pages)) {
+	if (!list_empty(&rinfo->indirect_pages)) {
 		struct page *indirect_page, *n;
 
 		BUG_ON(info->feature_persistent);
-		list_for_each_entry_safe(indirect_page, n, &info->indirect_pages, lru) {
+		list_for_each_entry_safe(indirect_page, n, &rinfo->indirect_pages, lru) {
 			list_del(&indirect_page->lru);
 			__free_page(indirect_page);
 		}
 	}
 
+	/* Remove all persistent grants. */
+	if (!list_empty(&rinfo->grants)) {
+		list_for_each_entry_safe(persistent_gnt, n,
+					 &rinfo->grants, node) {
+			list_del(&persistent_gnt->node);
+			if (persistent_gnt->gref != GRANT_INVALID_REF) {
+				gnttab_end_foreign_access(persistent_gnt->gref,
+							  0, 0UL);
+				rinfo->persistent_gnts_c--;
+			}
+			if (info->feature_persistent)
+				__free_page(persistent_gnt->page);
+			kfree(persistent_gnt);
+		}
+	}
+	BUG_ON(rinfo->persistent_gnts_c != 0);
+
 	for (i = 0; i < BLK_RING_SIZE(info); i++) {
 		/*
 		 * Clear persistent grants present in requests already
 		 * on the shared ring
 		 */
-		if (!info->shadow[i].request)
+		if (!rinfo->shadow[i].request)
 			goto free_shadow;
 
-		segs = info->shadow[i].req.operation == BLKIF_OP_INDIRECT ?
-		       info->shadow[i].req.u.indirect.nr_segments :
-		       info->shadow[i].req.u.rw.nr_segments;
+		segs = rinfo->shadow[i].req.operation == BLKIF_OP_INDIRECT ?
+		       rinfo->shadow[i].req.u.indirect.nr_segments :
+		       rinfo->shadow[i].req.u.rw.nr_segments;
 		for (j = 0; j < segs; j++) {
-			persistent_gnt = info->shadow[i].grants_used[j];
+			persistent_gnt = rinfo->shadow[i].grants_used[j];
 			gnttab_end_foreign_access(persistent_gnt->gref, 0, 0UL);
 			if (info->feature_persistent)
 				__free_page(persistent_gnt->page);
 			kfree(persistent_gnt);
 		}
 
-		if (info->shadow[i].req.operation != BLKIF_OP_INDIRECT)
+		if (rinfo->shadow[i].req.operation != BLKIF_OP_INDIRECT)
 			/*
 			 * If this is not an indirect operation don't try to
 			 * free indirect segments
@@ -1145,42 +1293,59 @@ static void blkif_free(struct blkfront_info *info, int suspend)
 			goto free_shadow;
 
 		for (j = 0; j < INDIRECT_GREFS(segs); j++) {
-			persistent_gnt = info->shadow[i].indirect_grants[j];
+			persistent_gnt = rinfo->shadow[i].indirect_grants[j];
 			gnttab_end_foreign_access(persistent_gnt->gref, 0, 0UL);
 			__free_page(persistent_gnt->page);
 			kfree(persistent_gnt);
 		}
 
 free_shadow:
-		kfree(info->shadow[i].grants_used);
-		info->shadow[i].grants_used = NULL;
-		kfree(info->shadow[i].indirect_grants);
-		info->shadow[i].indirect_grants = NULL;
-		kfree(info->shadow[i].sg);
-		info->shadow[i].sg = NULL;
+		kfree(rinfo->shadow[i].grants_used);
+		rinfo->shadow[i].grants_used = NULL;
+		kfree(rinfo->shadow[i].indirect_grants);
+		rinfo->shadow[i].indirect_grants = NULL;
+		kfree(rinfo->shadow[i].sg);
+		rinfo->shadow[i].sg = NULL;
 	}
 
 	/* No more gnttab callback work. */
-	gnttab_cancel_free_callback(&info->callback);
-	spin_unlock_irq(&info->io_lock);
+	gnttab_cancel_free_callback(&rinfo->callback);
 
 	/* Flush gnttab callback work. Must be done with no locks held. */
-	flush_work(&info->work);
+	flush_work(&rinfo->work);
 
 	/* Free resources associated with old device channel. */
 	for (i = 0; i < info->nr_ring_pages; i++) {
-		if (info->ring_ref[i] != GRANT_INVALID_REF) {
-			gnttab_end_foreign_access(info->ring_ref[i], 0, 0);
-			info->ring_ref[i] = GRANT_INVALID_REF;
+		if (rinfo->ring_ref[i] != GRANT_INVALID_REF) {
+			gnttab_end_foreign_access(rinfo->ring_ref[i], 0, 0);
+			rinfo->ring_ref[i] = GRANT_INVALID_REF;
 		}
 	}
-	free_pages((unsigned long)info->ring.sring, get_order(info->nr_ring_pages * PAGE_SIZE));
-	info->ring.sring = NULL;
+	free_pages((unsigned long)rinfo->ring.sring, get_order(info->nr_ring_pages * PAGE_SIZE));
+	rinfo->ring.sring = NULL;
+
+	if (rinfo->irq)
+		unbind_from_irqhandler(rinfo->irq, rinfo);
+	rinfo->evtchn = rinfo->irq = 0;
+}
+
+static void blkif_free(struct blkfront_info *info, int suspend)
+{
+	unsigned int i;
+
+	/* Prevent new requests being issued until we fix things up. */
+	info->connected = suspend ?
+		BLKIF_STATE_SUSPENDED : BLKIF_STATE_DISCONNECTED;
+	/* No more blkif_request(). */
+	if (info->rq)
+		blk_mq_stop_hw_queues(info->rq);
 
-	if (info->irq)
-		unbind_from_irqhandler(info->irq, info);
-	info->evtchn = info->irq = 0;
+	for (i = 0; i < info->nr_rings; i++)
+		blkif_free_ring(&info->rinfo[i]);
 
+	kfree(info->rinfo);
+	info->rinfo = NULL;
+	info->nr_rings = 0;
 }
 
 struct copy_from_grant {
@@ -1209,19 +1374,93 @@ static void blkif_copy_from_grant(unsigned long gfn, unsigned int offset,
 	kunmap_atomic(shared_data);
 }
 
-static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
+static enum blk_req_status blkif_rsp_to_req_status(int rsp)
+{
+	switch (rsp)
+	{
+	case BLKIF_RSP_OKAY:
+		return REQ_DONE;
+	case BLKIF_RSP_EOPNOTSUPP:
+		return REQ_EOPNOTSUPP;
+	case BLKIF_RSP_ERROR:
+		/* Fallthrough. */
+	default:
+		return REQ_ERROR;
+	}
+}
+
+/*
+ * Get the final status of the block request based on two ring response
+ */
+static int blkif_get_final_status(enum blk_req_status s1,
+				  enum blk_req_status s2)
+{
+	BUG_ON(s1 == REQ_WAITING);
+	BUG_ON(s2 == REQ_WAITING);
+
+	if (s1 == REQ_ERROR || s2 == REQ_ERROR)
+		return BLKIF_RSP_ERROR;
+	else if (s1 == REQ_EOPNOTSUPP || s2 == REQ_EOPNOTSUPP)
+		return BLKIF_RSP_EOPNOTSUPP;
+	return BLKIF_RSP_OKAY;
+}
+
+static bool blkif_completion(unsigned long *id,
+			     struct blkfront_ring_info *rinfo,
 			     struct blkif_response *bret)
 {
 	int i = 0;
 	struct scatterlist *sg;
 	int num_sg, num_grant;
+	struct blkfront_info *info = rinfo->dev_info;
+	struct blk_shadow *s = &rinfo->shadow[*id];
 	struct copy_from_grant data = {
-		.s = s,
 		.grant_idx = 0,
 	};
 
 	num_grant = s->req.operation == BLKIF_OP_INDIRECT ?
 		s->req.u.indirect.nr_segments : s->req.u.rw.nr_segments;
+
+	/* The I/O request may be split in two. */
+	if (unlikely(s->associated_id != NO_ASSOCIATED_ID)) {
+		struct blk_shadow *s2 = &rinfo->shadow[s->associated_id];
+
+		/* Keep the status of the current response in shadow. */
+		s->status = blkif_rsp_to_req_status(bret->status);
+
+		/* Wait the second response if not yet here. */
+		if (s2->status == REQ_WAITING)
+			return 0;
+
+		bret->status = blkif_get_final_status(s->status,
+						      s2->status);
+
+		/*
+		 * All the grants is stored in the first shadow in order
+		 * to make the completion code simpler.
+		 */
+		num_grant += s2->req.u.rw.nr_segments;
+
+		/*
+		 * The two responses may not come in order. Only the
+		 * first request will store the scatter-gather list.
+		 */
+		if (s2->num_sg != 0) {
+			/* Update "id" with the ID of the first response. */
+			*id = s->associated_id;
+			s = s2;
+		}
+
+		/*
+		 * We don't need anymore the second request, so recycling
+		 * it now.
+		 */
+		if (add_id_to_freelist(rinfo, s->associated_id))
+			WARN(1, "%s: can't recycle the second part (id = %ld) of the request\n",
+			     info->gd->disk_name, s->associated_id);
+	}
+
+	data.s = s;
 	num_sg = s->num_sg;
 
 	if (bret->operation == BLKIF_OP_READ && info->feature_persistent) {
@@ -1252,8 +1491,8 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 			if (!info->feature_persistent)
 				pr_alert_ratelimited("backed has not unmapped grant: %u\n",
 						     s->grants_used[i]->gref);
-			list_add(&s->grants_used[i]->node, &info->grants);
-			info->persistent_gnts_c++;
+			list_add(&s->grants_used[i]->node, &rinfo->grants);
+			rinfo->persistent_gnts_c++;
 		} else {
 			/*
 			 * If the grant is not mapped by the backend we end the
@@ -1263,7 +1502,7 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 			 */
 			gnttab_end_foreign_access(s->grants_used[i]->gref, 0, 0UL);
 			s->grants_used[i]->gref = GRANT_INVALID_REF;
-			list_add_tail(&s->grants_used[i]->node, &info->grants);
+			list_add_tail(&s->grants_used[i]->node, &rinfo->grants);
 		}
 	}
 	if (s->req.operation == BLKIF_OP_INDIRECT) {
@@ -1272,8 +1511,8 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 				if (!info->feature_persistent)
 					pr_alert_ratelimited("backed has not unmapped grant: %u\n",
 							     s->indirect_grants[i]->gref);
-				list_add(&s->indirect_grants[i]->node, &info->grants);
-				info->persistent_gnts_c++;
+				list_add(&s->indirect_grants[i]->node, &rinfo->grants);
+				rinfo->persistent_gnts_c++;
 			} else {
 				struct page *indirect_page;
 
@@ -1284,13 +1523,15 @@ static void blkif_completion(struct blk_shadow *s, struct blkfront_info *info,
 				 */
 				if (!info->feature_persistent) {
 					indirect_page = s->indirect_grants[i]->page;
-					list_add(&indirect_page->lru, &info->indirect_pages);
+					list_add(&indirect_page->lru, &rinfo->indirect_pages);
 				}
 				s->indirect_grants[i]->gref = GRANT_INVALID_REF;
-				list_add_tail(&s->indirect_grants[i]->node, &info->grants);
+				list_add_tail(&s->indirect_grants[i]->node, &rinfo->grants);
 			}
 		}
 	}
+
+	return 1;
 }
 
 static irqreturn_t blkif_interrupt(int irq, void *dev_id)
@@ -1299,24 +1540,22 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 	struct blkif_response *bret;
 	RING_IDX i, rp;
 	unsigned long flags;
-	struct blkfront_info *info = (struct blkfront_info *)dev_id;
+	struct blkfront_ring_info *rinfo = (struct blkfront_ring_info *)dev_id;
+	struct blkfront_info *info = rinfo->dev_info;
 	int error;
 
-	spin_lock_irqsave(&info->io_lock, flags);
-
-	if (unlikely(info->connected != BLKIF_STATE_CONNECTED)) {
-		spin_unlock_irqrestore(&info->io_lock, flags);
+	if (unlikely(info->connected != BLKIF_STATE_CONNECTED))
 		return IRQ_HANDLED;
-	}
 
+	spin_lock_irqsave(&rinfo->ring_lock, flags);
  again:
-	rp = info->ring.sring->rsp_prod;
+	rp = rinfo->ring.sring->rsp_prod;
 	rmb(); /* Ensure we see queued responses up to 'rp'. */
 
-	for (i = info->ring.rsp_cons; i != rp; i++) {
+	for (i = rinfo->ring.rsp_cons; i != rp; i++) {
 		unsigned long id;
 
-		bret = RING_GET_RESPONSE(&info->ring, i);
+		bret = RING_GET_RESPONSE(&rinfo->ring, i);
 		id   = bret->id;
 		/*
 		 * The backend has messed up and given us an id that we would
@@ -1330,12 +1569,18 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 			 * the id is busted. */
 			continue;
 		}
-		req  = info->shadow[id].request;
+		req  = rinfo->shadow[id].request;
 
-		if (bret->operation != BLKIF_OP_DISCARD)
-			blkif_completion(&info->shadow[id], info, bret);
+		if (bret->operation != BLKIF_OP_DISCARD) {
+			/*
+			 * We may need to wait for an extra response if the
+			 * I/O request is split in 2
+			 */
+			if (!blkif_completion(&id, rinfo, bret))
+				continue;
+		}
 
-		if (add_id_to_freelist(info, id)) {
+		if (add_id_to_freelist(rinfo, id)) {
 			WARN(1, "%s: response to %s (id %ld) couldn't be recycled!\n",
 			     info->gd->disk_name, op_name(bret->operation), id);
 			continue;
@@ -1364,7 +1609,7 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 				error = -EOPNOTSUPP;
 			}
 			if (unlikely(bret->status == BLKIF_RSP_ERROR &&
-				     info->shadow[id].req.u.rw.nr_segments == 0)) {
+				     rinfo->shadow[id].req.u.rw.nr_segments == 0)) {
 				printk(KERN_WARNING "blkfront: %s: empty %s op failed\n",
 				       info->gd->disk_name, op_name(bret->operation));
 				error = -EOPNOTSUPP;
@@ -1389,34 +1634,35 @@ static irqreturn_t blkif_interrupt(int irq, void *dev_id)
 		}
 	}
 
-	info->ring.rsp_cons = i;
+	rinfo->ring.rsp_cons = i;
 
-	if (i != info->ring.req_prod_pvt) {
+	if (i != rinfo->ring.req_prod_pvt) {
 		int more_to_do;
-		RING_FINAL_CHECK_FOR_RESPONSES(&info->ring, more_to_do);
+		RING_FINAL_CHECK_FOR_RESPONSES(&rinfo->ring, more_to_do);
 		if (more_to_do)
 			goto again;
 	} else
-		info->ring.sring->rsp_event = i + 1;
+		rinfo->ring.sring->rsp_event = i + 1;
 
-	kick_pending_request_queues(info);
+	kick_pending_request_queues_locked(rinfo);
 
-	spin_unlock_irqrestore(&info->io_lock, flags);
+	spin_unlock_irqrestore(&rinfo->ring_lock, flags);
 
 	return IRQ_HANDLED;
 }
 
 
 static int setup_blkring(struct xenbus_device *dev,
-			 struct blkfront_info *info)
+			 struct blkfront_ring_info *rinfo)
 {
 	struct blkif_sring *sring;
 	int err, i;
+	struct blkfront_info *info = rinfo->dev_info;
 	unsigned long ring_size = info->nr_ring_pages * XEN_PAGE_SIZE;
 	grant_ref_t gref[XENBUS_MAX_RING_GRANTS];
 
 	for (i = 0; i < info->nr_ring_pages; i++)
-		info->ring_ref[i] = GRANT_INVALID_REF;
+		rinfo->ring_ref[i] = GRANT_INVALID_REF;
 
 	sring = (struct blkif_sring *)__get_free_pages(GFP_NOIO | __GFP_HIGH,
 						       get_order(ring_size));
@@ -1425,29 +1671,29 @@ static int setup_blkring(struct xenbus_device *dev,
 		return -ENOMEM;
 	}
 	SHARED_RING_INIT(sring);
-	FRONT_RING_INIT(&info->ring, sring, ring_size);
+	FRONT_RING_INIT(&rinfo->ring, sring, ring_size);
 
-	err = xenbus_grant_ring(dev, info->ring.sring, info->nr_ring_pages, gref);
+	err = xenbus_grant_ring(dev, rinfo->ring.sring, info->nr_ring_pages, gref);
 	if (err < 0) {
 		free_pages((unsigned long)sring, get_order(ring_size));
-		info->ring.sring = NULL;
+		rinfo->ring.sring = NULL;
 		goto fail;
 	}
 	for (i = 0; i < info->nr_ring_pages; i++)
-		info->ring_ref[i] = gref[i];
+		rinfo->ring_ref[i] = gref[i];
 
-	err = xenbus_alloc_evtchn(dev, &info->evtchn);
+	err = xenbus_alloc_evtchn(dev, &rinfo->evtchn);
 	if (err)
 		goto fail;
 
-	err = bind_evtchn_to_irqhandler(info->evtchn, blkif_interrupt, 0,
-					"blkif", info);
+	err = bind_evtchn_to_irqhandler(rinfo->evtchn, blkif_interrupt, 0,
+					"blkif", rinfo);
 	if (err <= 0) {
 		xenbus_dev_fatal(dev, err,
 				 "bind_evtchn_to_irqhandler failed");
 		goto fail;
 	}
-	info->irq = err;
+	rinfo->irq = err;
 
 	return 0;
 fail:
@@ -1455,6 +1701,53 @@ fail:
 	return err;
 }
 
+/*
+ * Write out per-ring/queue nodes including ring-ref and event-channel, and each
+ * ring buffer may have multi pages depending on ->nr_ring_pages.
+ */
+static int write_per_ring_nodes(struct xenbus_transaction xbt,
+				struct blkfront_ring_info *rinfo, const char *dir)
+{
+	int err;
+	unsigned int i;
+	const char *message = NULL;
+	struct blkfront_info *info = rinfo->dev_info;
+
+	if (info->nr_ring_pages == 1) {
+		err = xenbus_printf(xbt, dir, "ring-ref", "%u", rinfo->ring_ref[0]);
+		if (err) {
+			message = "writing ring-ref";
+			goto abort_transaction;
+		}
+	} else {
+		for (i = 0; i < info->nr_ring_pages; i++) {
+			char ring_ref_name[RINGREF_NAME_LEN];
+
+			snprintf(ring_ref_name, RINGREF_NAME_LEN, "ring-ref%u", i);
+			err = xenbus_printf(xbt, dir, ring_ref_name,
+					    "%u", rinfo->ring_ref[i]);
+			if (err) {
+				message = "writing ring-ref";
+				goto abort_transaction;
+			}
+		}
+	}
+
+	err = xenbus_printf(xbt, dir, "event-channel", "%u", rinfo->evtchn);
+	if (err) {
+		message = "writing event-channel";
+		goto abort_transaction;
+	}
+
+	return 0;
+
+abort_transaction:
+	xenbus_transaction_end(xbt, 1);
+	if (message)
+		xenbus_dev_fatal(info->xbdev, err, "%s", message);
+
+	return err;
+}
 
 /* Common code used when first setting up, and when resuming. */
 static int talk_to_blkback(struct xenbus_device *dev,
@@ -1462,8 +1755,8 @@ static int talk_to_blkback(struct xenbus_device *dev,
 {
 	const char *message = NULL;
 	struct xenbus_transaction xbt;
-	int err, i;
-	unsigned int max_page_order = 0;
+	int err;
+	unsigned int i, max_page_order = 0;
 	unsigned int ring_page_order = 0;
 
 	err = xenbus_scanf(XBT_NIL, info->xbdev->otherend,
@@ -1475,10 +1768,14 @@ static int talk_to_blkback(struct xenbus_device *dev,
 		info->nr_ring_pages = 1 << ring_page_order;
 	}
 
-	/* Create shared ring, alloc event channel. */
-	err = setup_blkring(dev, info);
-	if (err)
-		goto out;
+	for (i = 0; i < info->nr_rings; i++) {
+		struct blkfront_ring_info *rinfo = &info->rinfo[i];
+
+		/* Create shared ring, alloc event channel. */
+		err = setup_blkring(dev, rinfo);
+		if (err)
+			goto destroy_blkring;
+	}
 
 again:
 	err = xenbus_transaction_start(&xbt);
@@ -1487,38 +1784,49 @@ again:
 		goto destroy_blkring;
 	}
 
-	if (info->nr_ring_pages == 1) {
-		err = xenbus_printf(xbt, dev->nodename,
-				    "ring-ref", "%u", info->ring_ref[0]);
+	if (info->nr_ring_pages > 1) {
+		err = xenbus_printf(xbt, dev->nodename, "ring-page-order", "%u",
+				    ring_page_order);
 		if (err) {
-			message = "writing ring-ref";
+			message = "writing ring-page-order";
 			goto abort_transaction;
 		}
+	}
+
+	/* We already got the number of queues/rings in _probe */
+	if (info->nr_rings == 1) {
+		err = write_per_ring_nodes(xbt, &info->rinfo[0], dev->nodename);
+		if (err)
+			goto destroy_blkring;
 	} else {
-		err = xenbus_printf(xbt, dev->nodename,
-				    "ring-page-order", "%u", ring_page_order);
+		char *path;
+		size_t pathsize;
+
+		err = xenbus_printf(xbt, dev->nodename, "multi-queue-num-queues", "%u",
+				    info->nr_rings);
 		if (err) {
-			message = "writing ring-page-order";
+			message = "writing multi-queue-num-queues";
 			goto abort_transaction;
 		}
 
-		for (i = 0; i < info->nr_ring_pages; i++) {
-			char ring_ref_name[RINGREF_NAME_LEN];
+		pathsize = strlen(dev->nodename) + QUEUE_NAME_LEN;
+		path = kmalloc(pathsize, GFP_KERNEL);
+		if (!path) {
+			err = -ENOMEM;
+			message = "ENOMEM while writing ring references";
+			goto abort_transaction;
+		}
 
-			snprintf(ring_ref_name, RINGREF_NAME_LEN, "ring-ref%u", i);
-			err = xenbus_printf(xbt, dev->nodename, ring_ref_name,
-					    "%u", info->ring_ref[i]);
+		for (i = 0; i < info->nr_rings; i++) {
+			memset(path, 0, pathsize);
+			snprintf(path, pathsize, "%s/queue-%u", dev->nodename, i);
+			err = write_per_ring_nodes(xbt, &info->rinfo[i], path);
 			if (err) {
-				message = "writing ring-ref";
-				goto abort_transaction;
+				kfree(path);
+				goto destroy_blkring;
 			}
 		}
-	}
-	err = xenbus_printf(xbt, dev->nodename,
-			    "event-channel", "%u", info->evtchn);
-	if (err) {
-		message = "writing event-channel";
-		goto abort_transaction;
+		kfree(path);
 	}
 	err = xenbus_printf(xbt, dev->nodename, "protocol", "%s",
 			    XEN_IO_PROTO_ABI_NATIVE);
@@ -1540,9 +1848,14 @@ again:
 		goto destroy_blkring;
 	}
 
-	for (i = 0; i < BLK_RING_SIZE(info); i++)
-		info->shadow[i].req.u.rw.id = i+1;
-	info->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
+	for (i = 0; i < info->nr_rings; i++) {
+		unsigned int j;
+		struct blkfront_ring_info *rinfo = &info->rinfo[i];
+
+		for (j = 0; j < BLK_RING_SIZE(info); j++)
+			rinfo->shadow[j].req.u.rw.id = j + 1;
+		rinfo->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
+	}
 	xenbus_switch_state(dev, XenbusStateInitialised);
 
 	return 0;
@@ -1553,10 +1866,50 @@ again:
 		xenbus_dev_fatal(dev, err, "%s", message);
  destroy_blkring:
 	blkif_free(info, 0);
- out:
+
+	kfree(info);
+	dev_set_drvdata(&dev->dev, NULL);
+
 	return err;
 }
 
+static int negotiate_mq(struct blkfront_info *info)
+{
+	unsigned int backend_max_queues = 0;
+	int err;
+	unsigned int i;
+
+	BUG_ON(info->nr_rings);
+
+	/* Check if backend supports multiple queues. */
+	err = xenbus_scanf(XBT_NIL, info->xbdev->otherend,
+			   "multi-queue-max-queues", "%u", &backend_max_queues);
+	if (err < 0)
+		backend_max_queues = 1;
+
+	info->nr_rings = min(backend_max_queues, xen_blkif_max_queues);
+	/* We need at least one ring. */
+	if (!info->nr_rings)
+		info->nr_rings = 1;
+
+	info->rinfo = kzalloc(sizeof(struct blkfront_ring_info) * info->nr_rings, GFP_KERNEL);
+	if (!info->rinfo) {
+		xenbus_dev_fatal(info->xbdev, -ENOMEM, "allocating ring_info structure");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < info->nr_rings; i++) {
+		struct blkfront_ring_info *rinfo;
+
+		rinfo = &info->rinfo[i];
+		INIT_LIST_HEAD(&rinfo->indirect_pages);
+		INIT_LIST_HEAD(&rinfo->grants);
+		rinfo->dev_info = info;
+		INIT_WORK(&rinfo->work, blkif_restart_queue);
+		spin_lock_init(&rinfo->ring_lock);
+	}
+	return 0;
+}
 /**
  * Entry point to this code when a new device is created.  Allocate the basic
  * structures and the ring buffer for communication with the backend, and
@@ -1617,15 +1970,16 @@ static int blkfront_probe(struct xenbus_device *dev,
 		return -ENOMEM;
 	}
 
-	mutex_init(&info->mutex);
-	spin_lock_init(&info->io_lock);
 	info->xbdev = dev;
+	err = negotiate_mq(info);
+	if (err) {
+		kfree(info);
+		return err;
+	}
+
+	mutex_init(&info->mutex);
 	info->vdevice = vdevice;
-	INIT_LIST_HEAD(&info->grants);
-	INIT_LIST_HEAD(&info->indirect_pages);
-	info->persistent_gnts_c = 0;
 	info->connected = BLKIF_STATE_DISCONNECTED;
-	INIT_WORK(&info->work, blkif_restart_queue);
 
 	/* Front end dir is a number, which is used as the id. */
 	info->handle = simple_strtoul(strrchr(dev->nodename, '/')+1, NULL, 0);
@@ -1649,7 +2003,7 @@ static void split_bio_end(struct bio *bio)
 
 static int blkif_recover(struct blkfront_info *info)
 {
-	int i;
+	unsigned int i, r_index;
 	struct request *req, *n;
 	struct blk_shadow *copy;
 	int rc;
@@ -1660,64 +2014,73 @@ static int blkif_recover(struct blkfront_info *info)
 	struct split_bio *split_bio;
 	struct list_head requests;
 
-	/* Stage 1: Make a safe copy of the shadow state. */
-	copy = kmemdup(info->shadow, sizeof(info->shadow),
-		       GFP_NOIO | __GFP_REPEAT | __GFP_HIGH);
-	if (!copy)
-		return -ENOMEM;
-
-	/* Stage 2: Set up free list. */
-	memset(&info->shadow, 0, sizeof(info->shadow));
-	for (i = 0; i < BLK_RING_SIZE(info); i++)
-		info->shadow[i].req.u.rw.id = i+1;
-	info->shadow_free = info->ring.req_prod_pvt;
-	info->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
-
-	rc = blkfront_gather_backend_features(info);
-	if (rc) {
-		kfree(copy);
-		return rc;
-	}
-
+	blkfront_gather_backend_features(info);
 	segs = info->max_indirect_segments ? : BLKIF_MAX_SEGMENTS_PER_REQUEST;
 	blk_queue_max_segments(info->rq, segs);
 	bio_list_init(&bio_list);
 	INIT_LIST_HEAD(&requests);
-	for (i = 0; i < BLK_RING_SIZE(info); i++) {
-		/* Not in use? */
-		if (!copy[i].request)
-			continue;
 
-		/*
-		 * Get the bios in the request so we can re-queue them.
-		 */
-		if (copy[i].request->cmd_flags &
-		    (REQ_FLUSH | REQ_FUA | REQ_DISCARD | REQ_SECURE)) {
+	for (r_index = 0; r_index < info->nr_rings; r_index++) {
+		struct blkfront_ring_info *rinfo;
+
+		rinfo = &info->rinfo[r_index];
+		/* Stage 1: Make a safe copy of the shadow state. */
+		copy = kmemdup(rinfo->shadow, sizeof(rinfo->shadow),
+			       GFP_NOIO | __GFP_REPEAT | __GFP_HIGH);
+		if (!copy)
+			return -ENOMEM;
+
+		/* Stage 2: Set up free list. */
+		memset(&rinfo->shadow, 0, sizeof(rinfo->shadow));
+		for (i = 0; i < BLK_RING_SIZE(info); i++)
+			rinfo->shadow[i].req.u.rw.id = i+1;
+		rinfo->shadow_free = rinfo->ring.req_prod_pvt;
+		rinfo->shadow[BLK_RING_SIZE(info)-1].req.u.rw.id = 0x0fffffff;
+
+		rc = blkfront_setup_indirect(rinfo);
+		if (rc) {
+			kfree(copy);
+			return rc;
+		}
+
+		for (i = 0; i < BLK_RING_SIZE(info); i++) {
+			/* Not in use? */
+			if (!copy[i].request)
+				continue;
+
 			/*
-			 * Flush operations don't contain bios, so
-			 * we need to requeue the whole request
+			 * Get the bios in the request so we can re-queue them.
 			 */
-			list_add(&copy[i].request->queuelist, &requests);
-			continue;
+			if (copy[i].request->cmd_flags &
+			    (REQ_FLUSH | REQ_FUA | REQ_DISCARD | REQ_SECURE)) {
+				/*
+				 * Flush operations don't contain bios, so
+				 * we need to requeue the whole request
+				 */
+				list_add(&copy[i].request->queuelist, &requests);
+				continue;
+			}
+			merge_bio.head = copy[i].request->bio;
+			merge_bio.tail = copy[i].request->biotail;
+			bio_list_merge(&bio_list, &merge_bio);
+			copy[i].request->bio = NULL;
+			blk_end_request_all(copy[i].request, 0);
 		}
-		merge_bio.head = copy[i].request->bio;
-		merge_bio.tail = copy[i].request->biotail;
-		bio_list_merge(&bio_list, &merge_bio);
-		copy[i].request->bio = NULL;
-		blk_end_request_all(copy[i].request, 0);
-	}
-
-	kfree(copy);
 
+		kfree(copy);
+	}
 	xenbus_switch_state(info->xbdev, XenbusStateConnected);
 
-	spin_lock_irq(&info->io_lock);
-
 	/* Now safe for us to use the shared ring */
 	info->connected = BLKIF_STATE_CONNECTED;
 
-	/* Kick any other new requests queued since we resumed */
-	kick_pending_request_queues(info);
+	for (r_index = 0; r_index < info->nr_rings; r_index++) {
+		struct blkfront_ring_info *rinfo;
+
+		rinfo = &info->rinfo[r_index];
+		/* Kick any other new requests queued since we resumed */
+		kick_pending_request_queues(rinfo);
+	}
 
 	list_for_each_entry_safe(req, n, &requests, queuelist) {
 		/* Requeue pending requests (flush or discard) */
@@ -1725,7 +2088,6 @@ static int blkif_recover(struct blkfront_info *info)
 		BUG_ON(req->nr_phys_segments > segs);
 		blk_mq_requeue_request(req);
 	}
-	spin_unlock_irq(&info->io_lock);
 	blk_mq_kick_requeue_list(info->rq);
 
 	while ((bio = bio_list_pop(&bio_list)) != NULL) {
@@ -1773,12 +2135,16 @@ static int blkif_recover(struct blkfront_info *info)
 static int blkfront_resume(struct xenbus_device *dev)
 {
 	struct blkfront_info *info = dev_get_drvdata(&dev->dev);
-	int err;
+	int err = 0;
 
 	dev_dbg(&dev->dev, "blkfront_resume: %s\n", dev->nodename);
 
 	blkif_free(info, info->connected == BLKIF_STATE_CONNECTED);
 
+	err = negotiate_mq(info);
+	if (err)
+		return err;
+
 	err = talk_to_blkback(dev, info);
 
 	/*
@@ -1790,8 +2156,7 @@ static int blkfront_resume(struct xenbus_device *dev)
 	return err;
 }
 
-static void
-blkfront_closing(struct blkfront_info *info)
+static void blkfront_closing(struct blkfront_info *info)
 {
 	struct xenbus_device *xbdev = info->xbdev;
 	struct block_device *bdev = NULL;
@@ -1851,18 +2216,29 @@ static void blkfront_setup_discard(struct blkfront_info *info)
 		info->feature_secdiscard = !!discard_secure;
 }
 
-static int blkfront_setup_indirect(struct blkfront_info *info)
+static int blkfront_setup_indirect(struct blkfront_ring_info *rinfo)
 {
 	unsigned int psegs, grants;
 	int err, i;
+	struct blkfront_info *info = rinfo->dev_info;
 
-	if (info->max_indirect_segments == 0)
-		grants = BLKIF_MAX_SEGMENTS_PER_REQUEST;
+	if (info->max_indirect_segments == 0) {
+		if (!HAS_EXTRA_REQ)
+			grants = BLKIF_MAX_SEGMENTS_PER_REQUEST;
+		else {
+			/*
+			 * When an extra req is required, the maximum
+			 * grants supported is related to the size of the
+			 * Linux block segment.
+			 */
+			grants = GRANTS_PER_PSEG;
+		}
+	}
 	else
 		grants = info->max_indirect_segments;
 	psegs = grants / GRANTS_PER_PSEG;
 
-	err = fill_grant_buffer(info,
+	err = fill_grant_buffer(rinfo,
 				(grants + INDIRECT_GREFS(grants)) * BLK_RING_SIZE(info));
 	if (err)
 		goto out_of_memory;
@@ -1875,31 +2251,31 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 		 */
 		int num = INDIRECT_GREFS(grants) * BLK_RING_SIZE(info);
 
-		BUG_ON(!list_empty(&info->indirect_pages));
+		BUG_ON(!list_empty(&rinfo->indirect_pages));
 		for (i = 0; i < num; i++) {
 			struct page *indirect_page = alloc_page(GFP_NOIO);
 			if (!indirect_page)
 				goto out_of_memory;
-			list_add(&indirect_page->lru, &info->indirect_pages);
+			list_add(&indirect_page->lru, &rinfo->indirect_pages);
 		}
 	}
 
 	for (i = 0; i < BLK_RING_SIZE(info); i++) {
-		info->shadow[i].grants_used = kzalloc(
-			sizeof(info->shadow[i].grants_used[0]) * grants,
+		rinfo->shadow[i].grants_used = kzalloc(
+			sizeof(rinfo->shadow[i].grants_used[0]) * grants,
 			GFP_NOIO);
-		info->shadow[i].sg = kzalloc(sizeof(info->shadow[i].sg[0]) * psegs, GFP_NOIO);
+		rinfo->shadow[i].sg = kzalloc(sizeof(rinfo->shadow[i].sg[0]) * psegs, GFP_NOIO);
 		if (info->max_indirect_segments)
-			info->shadow[i].indirect_grants = kzalloc(
-				sizeof(info->shadow[i].indirect_grants[0]) *
+			rinfo->shadow[i].indirect_grants = kzalloc(
+				sizeof(rinfo->shadow[i].indirect_grants[0]) *
 				INDIRECT_GREFS(grants),
 				GFP_NOIO);
-		if ((info->shadow[i].grants_used == NULL) ||
-			(info->shadow[i].sg == NULL) ||
+		if ((rinfo->shadow[i].grants_used == NULL) ||
+			(rinfo->shadow[i].sg == NULL) ||
 		     (info->max_indirect_segments &&
-		     (info->shadow[i].indirect_grants == NULL)))
+		     (rinfo->shadow[i].indirect_grants == NULL)))
 			goto out_of_memory;
-		sg_init_table(info->shadow[i].sg, psegs);
+		sg_init_table(rinfo->shadow[i].sg, psegs);
 	}
 
 
@@ -1907,16 +2283,16 @@ static int blkfront_setup_indirect(struct blkfront_info *info)
 
 out_of_memory:
 	for (i = 0; i < BLK_RING_SIZE(info); i++) {
-		kfree(info->shadow[i].grants_used);
-		info->shadow[i].grants_used = NULL;
-		kfree(info->shadow[i].sg);
-		info->shadow[i].sg = NULL;
-		kfree(info->shadow[i].indirect_grants);
-		info->shadow[i].indirect_grants = NULL;
-	}
-	if (!list_empty(&info->indirect_pages)) {
+		kfree(rinfo->shadow[i].grants_used);
+		rinfo->shadow[i].grants_used = NULL;
+		kfree(rinfo->shadow[i].sg);
+		rinfo->shadow[i].sg = NULL;
+		kfree(rinfo->shadow[i].indirect_grants);
+		rinfo->shadow[i].indirect_grants = NULL;
+	}
+	if (!list_empty(&rinfo->indirect_pages)) {
 		struct page *indirect_page, *n;
-		list_for_each_entry_safe(indirect_page, n, &info->indirect_pages, lru) {
+		list_for_each_entry_safe(indirect_page, n, &rinfo->indirect_pages, lru) {
 			list_del(&indirect_page->lru);
 			__free_page(indirect_page);
 		}
@@ -1927,7 +2303,7 @@ out_of_memory:
 /*
  * Gather all backend feature-*
  */
-static int blkfront_gather_backend_features(struct blkfront_info *info)
+static void blkfront_gather_backend_features(struct blkfront_info *info)
 {
 	int err;
 	int barrier, flush, discard, persistent;
@@ -1982,8 +2358,6 @@ static int blkfront_gather_backend_features(struct blkfront_info *info)
 	else
 		info->max_indirect_segments = min(indirect_segments,
 						  xen_blkif_max_segments);
-
-	return blkfront_setup_indirect(info);
 }
 
 /*
@@ -1996,7 +2370,7 @@ static void blkfront_connect(struct blkfront_info *info)
 	unsigned long sector_size;
 	unsigned int physical_sector_size;
 	unsigned int binfo;
-	int err;
+	int err, i;
 
 	switch (info->connected) {
 	case BLKIF_STATE_CONNECTED:
@@ -2053,11 +2427,15 @@ static void blkfront_connect(struct blkfront_info *info)
 	if (err != 1)
 		physical_sector_size = sector_size;
 
-	err = blkfront_gather_backend_features(info);
-	if (err) {
-		xenbus_dev_fatal(info->xbdev, err, "setup_indirect at %s",
-				 info->xbdev->otherend);
-		return;
+	blkfront_gather_backend_features(info);
+	for (i = 0; i < info->nr_rings; i++) {
+		err = blkfront_setup_indirect(&info->rinfo[i]);
+		if (err) {
+			xenbus_dev_fatal(info->xbdev, err, "setup_indirect at %s",
+					 info->xbdev->otherend);
+			blkif_free(info, 0);
+			break;
+		}
 	}
 
 	err = xlvbd_alloc_gendisk(sectors, info, binfo, sector_size,
@@ -2071,10 +2449,9 @@ static void blkfront_connect(struct blkfront_info *info)
 	xenbus_switch_state(info->xbdev, XenbusStateConnected);
 
 	/* Kick pending requests. */
-	spin_lock_irq(&info->io_lock);
 	info->connected = BLKIF_STATE_CONNECTED;
-	kick_pending_request_queues(info);
-	spin_unlock_irq(&info->io_lock);
+	for (i = 0; i < info->nr_rings; i++)
+		kick_pending_request_queues(&info->rinfo[i]);
 
 	add_disk(info->gd);
 
@@ -2095,11 +2472,8 @@ static void blkback_changed(struct xenbus_device *dev,
 	case XenbusStateInitWait:
 		if (dev->state != XenbusStateInitialising)
 			break;
-		if (talk_to_blkback(dev, info)) {
-			kfree(info);
-			dev_set_drvdata(&dev->dev, NULL);
+		if (talk_to_blkback(dev, info))
 			break;
-		}
 	case XenbusStateInitialising:
 	case XenbusStateInitialised:
 	case XenbusStateReconfiguring:
@@ -2108,6 +2482,10 @@ static void blkback_changed(struct xenbus_device *dev,
 		break;
 
 	case XenbusStateConnected:
+		if (dev->state != XenbusStateInitialised) {
+			if (talk_to_blkback(dev, info))
+				break;
+		}
 		blkfront_connect(info);
 		break;
 
@@ -2281,6 +2659,7 @@ static struct xenbus_driver blkfront_driver = {
 static int __init xlblk_init(void)
 {
 	int ret;
+	int nr_cpus = num_online_cpus();
 
 	if (!xen_domain())
 		return -ENODEV;
@@ -2288,7 +2667,13 @@ static int __init xlblk_init(void)
 	if (xen_blkif_max_ring_order > XENBUS_MAX_RING_GRANT_ORDER) {
 		pr_info("Invalid max_ring_order (%d), will use default max: %d.\n",
 			xen_blkif_max_ring_order, XENBUS_MAX_RING_GRANT_ORDER);
-		xen_blkif_max_ring_order = 0;
+		xen_blkif_max_ring_order = XENBUS_MAX_RING_GRANT_ORDER;
+	}
+
+	if (xen_blkif_max_queues > nr_cpus) {
+		pr_info("Invalid max_queues (%d), will use default max: %d.\n",
+			xen_blkif_max_queues, nr_cpus);
+		xen_blkif_max_queues = nr_cpus;
 	}
 
 	if (!xen_has_pv_disk_devices())
diff --git a/drivers/block/zram/zcomp.c b/drivers/block/zram/zcomp.c
index 5cb13ca3a3ac..3ef42e563bb5 100644
--- a/drivers/block/zram/zcomp.c
+++ b/drivers/block/zram/zcomp.c
@@ -74,18 +74,18 @@ static void zcomp_strm_free(struct zcomp *comp, struct zcomp_strm *zstrm)
  * allocate new zcomp_strm structure with ->private initialized by
  * backend, return NULL on error
  */
-static struct zcomp_strm *zcomp_strm_alloc(struct zcomp *comp)
+static struct zcomp_strm *zcomp_strm_alloc(struct zcomp *comp, gfp_t flags)
 {
-	struct zcomp_strm *zstrm = kmalloc(sizeof(*zstrm), GFP_KERNEL);
+	struct zcomp_strm *zstrm = kmalloc(sizeof(*zstrm), flags);
 	if (!zstrm)
 		return NULL;
 
-	zstrm->private = comp->backend->create();
+	zstrm->private = comp->backend->create(flags);
 	/*
 	 * allocate 2 pages. 1 for compressed data, plus 1 extra for the
 	 * case when compressed size is larger than the original one
 	 */
-	zstrm->buffer = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO, 1);
+	zstrm->buffer = (void *)__get_free_pages(flags | __GFP_ZERO, 1);
 	if (!zstrm->private || !zstrm->buffer) {
 		zcomp_strm_free(comp, zstrm);
 		zstrm = NULL;
@@ -120,8 +120,16 @@ static struct zcomp_strm *zcomp_strm_multi_find(struct zcomp *comp)
 		/* allocate new zstrm stream */
 		zs->avail_strm++;
 		spin_unlock(&zs->strm_lock);
-
-		zstrm = zcomp_strm_alloc(comp);
+		/*
+		 * This function can be called in swapout/fs write path
+		 * so we can't use GFP_FS|IO. And it assumes we already
+		 * have at least one stream in zram initialization so we
+		 * don't do best effort to allocate more stream in here.
+		 * A default stream will work well without further multiple
+		 * streams. That's why we use NORETRY | NOWARN.
+		 */
+		zstrm = zcomp_strm_alloc(comp, GFP_NOIO | __GFP_NORETRY |
+					__GFP_NOWARN);
 		if (!zstrm) {
 			spin_lock(&zs->strm_lock);
 			zs->avail_strm--;
@@ -209,7 +217,7 @@ static int zcomp_strm_multi_create(struct zcomp *comp, int max_strm)
 	zs->max_strm = max_strm;
 	zs->avail_strm = 1;
 
-	zstrm = zcomp_strm_alloc(comp);
+	zstrm = zcomp_strm_alloc(comp, GFP_KERNEL);
 	if (!zstrm) {
 		kfree(zs);
 		return -ENOMEM;
@@ -259,7 +267,7 @@ static int zcomp_strm_single_create(struct zcomp *comp)
 
 	comp->stream = zs;
 	mutex_init(&zs->strm_lock);
-	zs->zstrm = zcomp_strm_alloc(comp);
+	zs->zstrm = zcomp_strm_alloc(comp, GFP_KERNEL);
 	if (!zs->zstrm) {
 		kfree(zs);
 		return -ENOMEM;
diff --git a/drivers/block/zram/zcomp.h b/drivers/block/zram/zcomp.h
index 46e2b9f8f1f0..b7d2a4bcae54 100644
--- a/drivers/block/zram/zcomp.h
+++ b/drivers/block/zram/zcomp.h
@@ -33,7 +33,7 @@ struct zcomp_backend {
 	int (*decompress)(const unsigned char *src, size_t src_len,
 			unsigned char *dst);
 
-	void *(*create)(void);
+	void *(*create)(gfp_t flags);
 	void (*destroy)(void *private);
 
 	const char *name;
diff --git a/drivers/block/zram/zcomp_lz4.c b/drivers/block/zram/zcomp_lz4.c
index f2afb7e988c3..0110086accba 100644
--- a/drivers/block/zram/zcomp_lz4.c
+++ b/drivers/block/zram/zcomp_lz4.c
@@ -10,17 +10,26 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/lz4.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
 
 #include "zcomp_lz4.h"
 
-static void *zcomp_lz4_create(void)
+static void *zcomp_lz4_create(gfp_t flags)
 {
-	return kzalloc(LZ4_MEM_COMPRESS, GFP_KERNEL);
+	void *ret;
+
+	ret = kmalloc(LZ4_MEM_COMPRESS, flags);
+	if (!ret)
+		ret = __vmalloc(LZ4_MEM_COMPRESS,
+				flags | __GFP_HIGHMEM,
+				PAGE_KERNEL);
+	return ret;
 }
 
 static void zcomp_lz4_destroy(void *private)
 {
-	kfree(private);
+	kvfree(private);
 }
 
 static int zcomp_lz4_compress(const unsigned char *src, unsigned char *dst,
diff --git a/drivers/block/zram/zcomp_lzo.c b/drivers/block/zram/zcomp_lzo.c
index da1bc47d588e..ed7a1f0549ec 100644
--- a/drivers/block/zram/zcomp_lzo.c
+++ b/drivers/block/zram/zcomp_lzo.c
@@ -10,17 +10,26 @@
 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/lzo.h>
+#include <linux/vmalloc.h>
+#include <linux/mm.h>
 
 #include "zcomp_lzo.h"
 
-static void *lzo_create(void)
+static void *lzo_create(gfp_t flags)
 {
-	return kzalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
+	void *ret;
+
+	ret = kmalloc(LZO1X_MEM_COMPRESS, flags);
+	if (!ret)
+		ret = __vmalloc(LZO1X_MEM_COMPRESS,
+				flags | __GFP_HIGHMEM,
+				PAGE_KERNEL);
+	return ret;
 }
 
 static void lzo_destroy(void *private)
 {
-	kfree(private);
+	kvfree(private);
 }
 
 static int lzo_compress(const unsigned char *src, unsigned char *dst,
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index 47915d736f8d..370c2f76016d 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -1325,7 +1325,6 @@ static int zram_remove(struct zram *zram)
 
 	pr_info("Removed device: %s\n", zram->disk->disk_name);
 
-	idr_remove(&zram_index_idr, zram->disk->first_minor);
 	blk_cleanup_queue(zram->disk->queue);
 	del_gendisk(zram->disk);
 	put_disk(zram->disk);
@@ -1367,10 +1366,12 @@ static ssize_t hot_remove_store(struct class *class,
 	mutex_lock(&zram_index_mutex);
 
 	zram = idr_find(&zram_index_idr, dev_id);
-	if (zram)
+	if (zram) {
 		ret = zram_remove(zram);
-	else
+		idr_remove(&zram_index_idr, dev_id);
+	} else {
 		ret = -ENODEV;
+	}
 
 	mutex_unlock(&zram_index_mutex);
 	return ret ? ret : count;
diff --git a/drivers/bluetooth/bcm203x.c b/drivers/bluetooth/bcm203x.c
index 364f82b34d03..5b0ef7bbe8ac 100644
--- a/drivers/bluetooth/bcm203x.c
+++ b/drivers/bluetooth/bcm203x.c
@@ -178,10 +178,8 @@ static int bcm203x_probe(struct usb_interface *intf, const struct usb_device_id
 		return -ENODEV;
 
 	data = devm_kzalloc(&intf->dev, sizeof(*data), GFP_KERNEL);
-	if (!data) {
-		BT_ERR("Can't allocate memory for data structure");
+	if (!data)
 		return -ENOMEM;
-	}
 
 	data->udev  = udev;
 	data->state = BCM203X_LOAD_MINIDRV;
diff --git a/drivers/bluetooth/bfusb.c b/drivers/bluetooth/bfusb.c
index 616ec2ac1b22..3bf4ec60e073 100644
--- a/drivers/bluetooth/bfusb.c
+++ b/drivers/bluetooth/bfusb.c
@@ -324,7 +324,7 @@ static inline int bfusb_recv_block(struct bfusb_data *data, int hdr, unsigned ch
 			return -ENOMEM;
 		}
 
-		bt_cb(skb)->pkt_type = pkt_type;
+		hci_skb_pkt_type(skb) = pkt_type;
 
 		data->reassembly = skb;
 	} else {
@@ -469,9 +469,10 @@ static int bfusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	unsigned char buf[3];
 	int sent = 0, size, count;
 
-	BT_DBG("hdev %p skb %p type %d len %d", hdev, skb, bt_cb(skb)->pkt_type, skb->len);
+	BT_DBG("hdev %p skb %p type %d len %d", hdev, skb,
+	       hci_skb_pkt_type(skb), skb->len);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
@@ -484,7 +485,7 @@ static int bfusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	}
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
 	count = skb->len;
 
@@ -635,10 +636,8 @@ static int bfusb_probe(struct usb_interface *intf, const struct usb_device_id *i
 
 	/* Initialize control structure and load firmware */
 	data = devm_kzalloc(&intf->dev, sizeof(struct bfusb_data), GFP_KERNEL);
-	if (!data) {
-		BT_ERR("Can't allocate memory for control structure");
-		goto done;
-	}
+	if (!data)
+		return -ENOMEM;
 
 	data->udev = udev;
 	data->bulk_in_ep    = bulk_in_ep->desc.bEndpointAddress;
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index 36fa1c958c74..c0b3b5576992 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -261,7 +261,7 @@ static void bluecard_write_wakeup(struct bluecard_info *info)
 		if (!skb)
 			break;
 
-		if (bt_cb(skb)->pkt_type & 0x80) {
+		if (hci_skb_pkt_type(skb) & 0x80) {
 			/* Disable RTS */
 			info->ctrl_reg |= REG_CONTROL_RTS;
 			outb(info->ctrl_reg, iobase + REG_CONTROL);
@@ -279,13 +279,13 @@ static void bluecard_write_wakeup(struct bluecard_info *info)
 		/* Mark the buffer as dirty */
 		clear_bit(ready_bit, &(info->tx_state));
 
-		if (bt_cb(skb)->pkt_type & 0x80) {
+		if (hci_skb_pkt_type(skb) & 0x80) {
 			DECLARE_WAIT_QUEUE_HEAD_ONSTACK(wq);
 			DEFINE_WAIT(wait);
 
 			unsigned char baud_reg;
 
-			switch (bt_cb(skb)->pkt_type) {
+			switch (hci_skb_pkt_type(skb)) {
 			case PKT_BAUD_RATE_460800:
 				baud_reg = REG_CONTROL_BAUD_RATE_460800;
 				break;
@@ -402,9 +402,9 @@ static void bluecard_receive(struct bluecard_info *info,
 
 		if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-			bt_cb(info->rx_skb)->pkt_type = buf[i];
+			hci_skb_pkt_type(info->rx_skb) = buf[i];
 
-			switch (bt_cb(info->rx_skb)->pkt_type) {
+			switch (hci_skb_pkt_type(info->rx_skb)) {
 
 			case 0x00:
 				/* init packet */
@@ -436,7 +436,8 @@ static void bluecard_receive(struct bluecard_info *info,
 
 			default:
 				/* unknown packet */
-				BT_ERR("Unknown HCI packet with type 0x%02x received", bt_cb(info->rx_skb)->pkt_type);
+				BT_ERR("Unknown HCI packet with type 0x%02x received",
+				       hci_skb_pkt_type(info->rx_skb));
 				info->hdev->stat.err_rx++;
 
 				kfree_skb(info->rx_skb);
@@ -578,21 +579,21 @@ static int bluecard_hci_set_baud_rate(struct hci_dev *hdev, int baud)
 	switch (baud) {
 	case 460800:
 		cmd[4] = 0x00;
-		bt_cb(skb)->pkt_type = PKT_BAUD_RATE_460800;
+		hci_skb_pkt_type(skb) = PKT_BAUD_RATE_460800;
 		break;
 	case 230400:
 		cmd[4] = 0x01;
-		bt_cb(skb)->pkt_type = PKT_BAUD_RATE_230400;
+		hci_skb_pkt_type(skb) = PKT_BAUD_RATE_230400;
 		break;
 	case 115200:
 		cmd[4] = 0x02;
-		bt_cb(skb)->pkt_type = PKT_BAUD_RATE_115200;
+		hci_skb_pkt_type(skb) = PKT_BAUD_RATE_115200;
 		break;
 	case 57600:
 		/* Fall through... */
 	default:
 		cmd[4] = 0x03;
-		bt_cb(skb)->pkt_type = PKT_BAUD_RATE_57600;
+		hci_skb_pkt_type(skb) = PKT_BAUD_RATE_57600;
 		break;
 	}
 
@@ -660,7 +661,7 @@ static int bluecard_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct bluecard_info *info = hci_get_drvdata(hdev);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
@@ -673,7 +674,7 @@ static int bluecard_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	}
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&(info->txq), skb);
 
 	bluecard_write_wakeup(info);
diff --git a/drivers/bluetooth/bpa10x.c b/drivers/bluetooth/bpa10x.c
index 49c397e21b39..fd6b53e9bbf2 100644
--- a/drivers/bluetooth/bpa10x.c
+++ b/drivers/bluetooth/bpa10x.c
@@ -295,9 +295,9 @@ static int bpa10x_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 		return -ENOMEM;
 
 	/* Prepend skb with frame type */
-	*skb_push(skb, 1) = bt_cb(skb)->pkt_type;
+	*skb_push(skb, 1) = hci_skb_pkt_type(skb);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		dr = kmalloc(sizeof(*dr), GFP_ATOMIC);
 		if (!dr) {
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 5803aaed958f..8165ef2fe877 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -246,10 +246,10 @@ static void bt3c_receive(struct bt3c_info *info)
 
 		if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-			bt_cb(info->rx_skb)->pkt_type = inb(iobase + DATA_L);
+			hci_skb_pkt_type(info->rx_skb) = inb(iobase + DATA_L);
 			inb(iobase + DATA_H);
 
-			switch (bt_cb(info->rx_skb)->pkt_type) {
+			switch (hci_skb_pkt_type(info->rx_skb)) {
 
 			case HCI_EVENT_PKT:
 				info->rx_state = RECV_WAIT_EVENT_HEADER;
@@ -268,7 +268,8 @@ static void bt3c_receive(struct bt3c_info *info)
 
 			default:
 				/* Unknown packet */
-				BT_ERR("Unknown HCI packet with type 0x%02x received", bt_cb(info->rx_skb)->pkt_type);
+				BT_ERR("Unknown HCI packet with type 0x%02x received",
+				       hci_skb_pkt_type(info->rx_skb));
 				info->hdev->stat.err_rx++;
 
 				kfree_skb(info->rx_skb);
@@ -411,7 +412,7 @@ static int bt3c_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	struct bt3c_info *info = hci_get_drvdata(hdev);
 	unsigned long flags;
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
@@ -424,7 +425,7 @@ static int bt3c_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	}
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&(info->txq), skb);
 
 	spin_lock_irqsave(&(info->lock), flags);
diff --git a/drivers/bluetooth/btintel.c b/drivers/bluetooth/btintel.c
index 1f13e617bf56..fce154855718 100644
--- a/drivers/bluetooth/btintel.c
+++ b/drivers/bluetooth/btintel.c
@@ -73,6 +73,48 @@ int btintel_check_bdaddr(struct hci_dev *hdev)
 }
 EXPORT_SYMBOL_GPL(btintel_check_bdaddr);
 
+int btintel_enter_mfg(struct hci_dev *hdev)
+{
+	const u8 param[] = { 0x01, 0x00 };
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_CMD_TIMEOUT);
+	if (IS_ERR(skb)) {
+		bt_dev_err(hdev, "Entering manufacturer mode failed (%ld)",
+			   PTR_ERR(skb));
+		return PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btintel_enter_mfg);
+
+int btintel_exit_mfg(struct hci_dev *hdev, bool reset, bool patched)
+{
+	u8 param[] = { 0x00, 0x00 };
+	struct sk_buff *skb;
+
+	/* The 2nd command parameter specifies the manufacturing exit method:
+	 * 0x00: Just disable the manufacturing mode (0x00).
+	 * 0x01: Disable manufacturing mode and reset with patches deactivated.
+	 * 0x02: Disable manufacturing mode and reset with patches activated.
+	 */
+	if (reset)
+		param[1] |= patched ? 0x02 : 0x01;
+
+	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_CMD_TIMEOUT);
+	if (IS_ERR(skb)) {
+		bt_dev_err(hdev, "Exiting manufacturer mode failed (%ld)",
+			   PTR_ERR(skb));
+		return PTR_ERR(skb);
+	}
+	kfree_skb(skb);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btintel_exit_mfg);
+
 int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 {
 	struct sk_buff *skb;
@@ -126,37 +168,19 @@ EXPORT_SYMBOL_GPL(btintel_set_diag);
 
 int btintel_set_diag_mfg(struct hci_dev *hdev, bool enable)
 {
-	struct sk_buff *skb;
-	u8 param[2];
-	int err;
-
-	param[0] = 0x01;
-	param[1] = 0x00;
-
-	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		err = PTR_ERR(skb);
-		BT_ERR("%s: Entering Intel manufacturer mode failed (%d)",
-		       hdev->name, err);
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	int err, ret;
 
-	err = btintel_set_diag(hdev, enable);
+	err = btintel_enter_mfg(hdev);
+	if (err)
+		return err;
 
-	param[0] = 0x00;
-	param[1] = 0x00;
+	ret = btintel_set_diag(hdev, enable);
 
-	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		err = PTR_ERR(skb);
-		BT_ERR("%s: Leaving Intel manufacturer mode failed (%d)",
-		       hdev->name, err);
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	err = btintel_exit_mfg(hdev, false, false);
+	if (err)
+		return err;
 
-	return err;
+	return ret;
 }
 EXPORT_SYMBOL_GPL(btintel_set_diag_mfg);
 
@@ -309,39 +333,46 @@ EXPORT_SYMBOL_GPL(btintel_set_event_mask);
 
 int btintel_set_event_mask_mfg(struct hci_dev *hdev, bool debug)
 {
-	struct sk_buff *skb;
-	u8 param[2];
-	int err;
+	int err, ret;
 
-	param[0] = 0x01;
-	param[1] = 0x00;
+	err = btintel_enter_mfg(hdev);
+	if (err)
+		return err;
 
-	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		err = PTR_ERR(skb);
-		BT_ERR("%s: Entering Intel manufacturer mode failed (%d)",
-		       hdev->name, err);
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	ret = btintel_set_event_mask(hdev, debug);
 
-	err = btintel_set_event_mask(hdev, debug);
+	err = btintel_exit_mfg(hdev, false, false);
+	if (err)
+		return err;
 
-	param[0] = 0x00;
-	param[1] = 0x00;
+	return ret;
+}
+EXPORT_SYMBOL_GPL(btintel_set_event_mask_mfg);
 
-	skb = __hci_cmd_sync(hdev, 0xfc11, 2, param, HCI_INIT_TIMEOUT);
+int btintel_read_version(struct hci_dev *hdev, struct intel_version *ver)
+{
+	struct sk_buff *skb;
+
+	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_CMD_TIMEOUT);
 	if (IS_ERR(skb)) {
-		err = PTR_ERR(skb);
-		BT_ERR("%s: Leaving Intel manufacturer mode failed (%d)",
-		       hdev->name, err);
+		bt_dev_err(hdev, "Reading Intel version information failed (%ld)",
+			   PTR_ERR(skb));
 		return PTR_ERR(skb);
 	}
+
+	if (skb->len != sizeof(*ver)) {
+		bt_dev_err(hdev, "Intel version event size mismatch");
+		kfree_skb(skb);
+		return -EILSEQ;
+	}
+
+	memcpy(ver, skb->data, sizeof(*ver));
+
 	kfree_skb(skb);
 
-	return err;
+	return 0;
 }
-EXPORT_SYMBOL_GPL(btintel_set_event_mask_mfg);
+EXPORT_SYMBOL_GPL(btintel_read_version);
 
 /* ------- REGMAP IBT SUPPORT ------- */
 
diff --git a/drivers/bluetooth/btintel.h b/drivers/bluetooth/btintel.h
index 07e58e05a7fa..1e8955aaafed 100644
--- a/drivers/bluetooth/btintel.h
+++ b/drivers/bluetooth/btintel.h
@@ -72,6 +72,8 @@ struct intel_secure_send_result {
 #if IS_ENABLED(CONFIG_BT_INTEL)
 
 int btintel_check_bdaddr(struct hci_dev *hdev);
+int btintel_enter_mfg(struct hci_dev *hdev);
+int btintel_exit_mfg(struct hci_dev *hdev, bool reset, bool patched);
 int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr);
 int btintel_set_diag(struct hci_dev *hdev, bool enable);
 int btintel_set_diag_mfg(struct hci_dev *hdev, bool enable);
@@ -83,6 +85,7 @@ int btintel_secure_send(struct hci_dev *hdev, u8 fragment_type, u32 plen,
 int btintel_load_ddc_config(struct hci_dev *hdev, const char *ddc_name);
 int btintel_set_event_mask(struct hci_dev *hdev, bool debug);
 int btintel_set_event_mask_mfg(struct hci_dev *hdev, bool debug);
+int btintel_read_version(struct hci_dev *hdev, struct intel_version *ver);
 
 struct regmap *btintel_regmap_init(struct hci_dev *hdev, u16 opcode_read,
 				   u16 opcode_write);
@@ -94,6 +97,16 @@ static inline int btintel_check_bdaddr(struct hci_dev *hdev)
 	return -EOPNOTSUPP;
 }
 
+static inline int btintel_enter_mfg(struct hci_dev *hdev)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline int btintel_exit_mfg(struct hci_dev *hdev, bool reset, bool patched)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline int btintel_set_bdaddr(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 {
 	return -EOPNOTSUPP;
@@ -140,6 +153,12 @@ static inline int btintel_set_event_mask_mfg(struct hci_dev *hdev, bool debug)
 	return -EOPNOTSUPP;
 }
 
+static inline int btintel_read_version(struct hci_dev *hdev,
+				       struct intel_version *ver)
+{
+	return -EOPNOTSUPP;
+}
+
 static inline struct regmap *btintel_regmap_init(struct hci_dev *hdev,
 						 u16 opcode_read,
 						 u16 opcode_write)
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
index 27a9aac25583..05904732e6f1 100644
--- a/drivers/bluetooth/btmrvl_drv.h
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -89,6 +89,7 @@ struct btmrvl_adapter {
 	wait_queue_head_t event_hs_wait_q;
 	u8 cmd_complete;
 	bool is_suspended;
+	bool is_suspending;
 };
 
 struct btmrvl_private {
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index 6af917331962..f25a825a693f 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -196,7 +196,7 @@ static int btmrvl_send_sync_cmd(struct btmrvl_private *priv, u16 opcode,
 	if (len)
 		memcpy(skb_put(skb, len), param, len);
 
-	bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+	hci_skb_pkt_type(skb) = MRVL_VENDOR_PKT;
 
 	skb_queue_head(&priv->adapter->tx_queue, skb);
 
@@ -387,7 +387,7 @@ static int btmrvl_tx_pkt(struct btmrvl_private *priv, struct sk_buff *skb)
 	skb->data[0] = (skb->len & 0x0000ff);
 	skb->data[1] = (skb->len & 0x00ff00) >> 8;
 	skb->data[2] = (skb->len & 0xff0000) >> 16;
-	skb->data[3] = bt_cb(skb)->pkt_type;
+	skb->data[3] = hci_skb_pkt_type(skb);
 
 	if (priv->hw_host_to_card)
 		ret = priv->hw_host_to_card(priv, skb->data, skb->len);
@@ -434,9 +434,14 @@ static int btmrvl_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct btmrvl_private *priv = hci_get_drvdata(hdev);
 
-	BT_DBG("type=%d, len=%d", skb->pkt_type, skb->len);
+	BT_DBG("type=%d, len=%d", hci_skb_pkt_type(skb), skb->len);
 
-	switch (bt_cb(skb)->pkt_type) {
+	if (priv->adapter->is_suspending || priv->adapter->is_suspended) {
+		BT_ERR("%s: Device is suspending or suspended", __func__);
+		return -EBUSY;
+	}
+
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
@@ -452,7 +457,8 @@ static int btmrvl_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 
 	skb_queue_tail(&priv->adapter->tx_queue, skb);
 
-	wake_up_interruptible(&priv->main_thread.wait_q);
+	if (!priv->adapter->is_suspended)
+		wake_up_interruptible(&priv->main_thread.wait_q);
 
 	return 0;
 }
@@ -543,7 +549,7 @@ static int btmrvl_setup(struct hci_dev *hdev)
 	if (ret)
 		return ret;
 
-	priv->btmrvl_dev.gpio_gap = 0xffff;
+	priv->btmrvl_dev.gpio_gap = 0xfffe;
 
 	btmrvl_check_device_tree(priv);
 
@@ -643,7 +649,8 @@ static int btmrvl_service_main_thread(void *data)
 		if (adapter->ps_state == PS_SLEEP)
 			continue;
 
-		if (!priv->btmrvl_dev.tx_dnld_rdy)
+		if (!priv->btmrvl_dev.tx_dnld_rdy ||
+		    priv->adapter->is_suspended)
 			continue;
 
 		skb = skb_dequeue(&adapter->tx_queue);
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 71ea2a3af293..6ed8acfcfa9c 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -698,7 +698,7 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 	case HCI_ACLDATA_PKT:
 	case HCI_SCODATA_PKT:
 	case HCI_EVENT_PKT:
-		bt_cb(skb)->pkt_type = type;
+		hci_skb_pkt_type(skb) = type;
 		skb_put(skb, buf_len);
 		skb_pull(skb, SDIO_HEADER_LEN);
 
@@ -713,7 +713,7 @@ static int btmrvl_sdio_card_to_host(struct btmrvl_private *priv)
 		break;
 
 	case MRVL_VENDOR_PKT:
-		bt_cb(skb)->pkt_type = HCI_VENDOR_PKT;
+		hci_skb_pkt_type(skb) = HCI_VENDOR_PKT;
 		skb_put(skb, buf_len);
 		skb_pull(skb, SDIO_HEADER_LEN);
 
@@ -1112,7 +1112,8 @@ static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
 	 */
 	if (btmrvl_sdio_verify_fw_download(card, pollnum)) {
 		BT_ERR("FW failed to be active in time!");
-		return -ETIMEDOUT;
+		ret = -ETIMEDOUT;
+		goto done;
 	}
 
 	sdio_release_host(card->func);
@@ -1544,10 +1545,10 @@ static int btmrvl_sdio_suspend(struct device *dev)
 	}
 
 	priv = card->priv;
+	priv->adapter->is_suspending = true;
 	hcidev = priv->btmrvl_dev.hcidev;
 	BT_DBG("%s: SDIO suspend", hcidev->name);
 	hci_suspend_dev(hcidev);
-	skb_queue_purge(&priv->adapter->tx_queue);
 
 	if (priv->adapter->hs_state != HS_ACTIVATED) {
 		if (btmrvl_enable_hs(priv)) {
@@ -1556,6 +1557,7 @@ static int btmrvl_sdio_suspend(struct device *dev)
 		}
 	}
 
+	priv->adapter->is_suspending = false;
 	priv->adapter->is_suspended = true;
 
 	/* We will keep the power when hs enabled successfully */
diff --git a/drivers/bluetooth/btsdio.c b/drivers/bluetooth/btsdio.c
index 7b624423a7e8..2b05661e3818 100644
--- a/drivers/bluetooth/btsdio.c
+++ b/drivers/bluetooth/btsdio.c
@@ -86,7 +86,7 @@ static int btsdio_tx_packet(struct btsdio_data *data, struct sk_buff *skb)
 	skb->data[0] = (skb->len & 0x0000ff);
 	skb->data[1] = (skb->len & 0x00ff00) >> 8;
 	skb->data[2] = (skb->len & 0xff0000) >> 16;
-	skb->data[3] = bt_cb(skb)->pkt_type;
+	skb->data[3] = hci_skb_pkt_type(skb);
 
 	err = sdio_writesb(data->func, REG_TDAT, skb->data, skb->len);
 	if (err < 0) {
@@ -158,7 +158,7 @@ static int btsdio_rx_packet(struct btsdio_data *data)
 
 	data->hdev->stat.byte_rx += len;
 
-	bt_cb(skb)->pkt_type = hdr[3];
+	hci_skb_pkt_type(skb) = hdr[3];
 
 	err = hci_recv_frame(data->hdev, skb);
 	if (err < 0)
@@ -252,7 +252,7 @@ static int btsdio_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 
 	BT_DBG("%s", hdev->name);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index bb8e4025fb9e..9624b29f8349 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -200,9 +200,9 @@ static void btuart_receive(struct btuart_info *info)
 
 		if (info->rx_state == RECV_WAIT_PACKET_TYPE) {
 
-			bt_cb(info->rx_skb)->pkt_type = inb(iobase + UART_RX);
+			hci_skb_pkt_type(info->rx_skb) = inb(iobase + UART_RX);
 
-			switch (bt_cb(info->rx_skb)->pkt_type) {
+			switch (hci_skb_pkt_type(info->rx_skb)) {
 
 			case HCI_EVENT_PKT:
 				info->rx_state = RECV_WAIT_EVENT_HEADER;
@@ -221,7 +221,8 @@ static void btuart_receive(struct btuart_info *info)
 
 			default:
 				/* Unknown packet */
-				BT_ERR("Unknown HCI packet with type 0x%02x received", bt_cb(info->rx_skb)->pkt_type);
+				BT_ERR("Unknown HCI packet with type 0x%02x received",
+				       hci_skb_pkt_type(info->rx_skb));
 				info->hdev->stat.err_rx++;
 
 				kfree_skb(info->rx_skb);
@@ -424,7 +425,7 @@ static int btuart_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct btuart_info *info = hci_get_drvdata(hdev);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		break;
@@ -437,7 +438,7 @@ static int btuart_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	}
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&(info->txq), skb);
 
 	btuart_write_wakeup(info);
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 92f0ee388f9e..a191e318fab8 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -153,6 +153,10 @@ static const struct usb_device_id btusb_table[] = {
 	{ USB_VENDOR_AND_INTERFACE_INFO(0x13d3, 0xff, 0x01, 0x01),
 	  .driver_info = BTUSB_BCM_PATCHRAM },
 
+	/* Toshiba Corp - Broadcom based */
+	{ USB_VENDOR_AND_INTERFACE_INFO(0x0930, 0xff, 0x01, 0x01),
+	  .driver_info = BTUSB_BCM_PATCHRAM },
+
 	/* Intel Bluetooth USB Bootloader (RAM module) */
 	{ USB_DEVICE(0x8087, 0x0a5a),
 	  .driver_info = BTUSB_INTEL_BOOT | BTUSB_BROKEN_ISOC },
@@ -437,22 +441,22 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count)
 				break;
 			}
 
-			bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
-			bt_cb(skb)->expect = HCI_EVENT_HDR_SIZE;
+			hci_skb_pkt_type(skb) = HCI_EVENT_PKT;
+			hci_skb_expect(skb) = HCI_EVENT_HDR_SIZE;
 		}
 
-		len = min_t(uint, bt_cb(skb)->expect, count);
+		len = min_t(uint, hci_skb_expect(skb), count);
 		memcpy(skb_put(skb, len), buffer, len);
 
 		count -= len;
 		buffer += len;
-		bt_cb(skb)->expect -= len;
+		hci_skb_expect(skb) -= len;
 
 		if (skb->len == HCI_EVENT_HDR_SIZE) {
 			/* Complete event header */
-			bt_cb(skb)->expect = hci_event_hdr(skb)->plen;
+			hci_skb_expect(skb) = hci_event_hdr(skb)->plen;
 
-			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+			if (skb_tailroom(skb) < hci_skb_expect(skb)) {
 				kfree_skb(skb);
 				skb = NULL;
 
@@ -461,7 +465,7 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count)
 			}
 		}
 
-		if (bt_cb(skb)->expect == 0) {
+		if (!hci_skb_expect(skb)) {
 			/* Complete frame */
 			data->recv_event(data->hdev, skb);
 			skb = NULL;
@@ -492,24 +496,24 @@ static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count)
 				break;
 			}
 
-			bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
-			bt_cb(skb)->expect = HCI_ACL_HDR_SIZE;
+			hci_skb_pkt_type(skb) = HCI_ACLDATA_PKT;
+			hci_skb_expect(skb) = HCI_ACL_HDR_SIZE;
 		}
 
-		len = min_t(uint, bt_cb(skb)->expect, count);
+		len = min_t(uint, hci_skb_expect(skb), count);
 		memcpy(skb_put(skb, len), buffer, len);
 
 		count -= len;
 		buffer += len;
-		bt_cb(skb)->expect -= len;
+		hci_skb_expect(skb) -= len;
 
 		if (skb->len == HCI_ACL_HDR_SIZE) {
 			__le16 dlen = hci_acl_hdr(skb)->dlen;
 
 			/* Complete ACL header */
-			bt_cb(skb)->expect = __le16_to_cpu(dlen);
+			hci_skb_expect(skb) = __le16_to_cpu(dlen);
 
-			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+			if (skb_tailroom(skb) < hci_skb_expect(skb)) {
 				kfree_skb(skb);
 				skb = NULL;
 
@@ -518,7 +522,7 @@ static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count)
 			}
 		}
 
-		if (bt_cb(skb)->expect == 0) {
+		if (!hci_skb_expect(skb)) {
 			/* Complete frame */
 			hci_recv_frame(data->hdev, skb);
 			skb = NULL;
@@ -549,22 +553,22 @@ static int btusb_recv_isoc(struct btusb_data *data, void *buffer, int count)
 				break;
 			}
 
-			bt_cb(skb)->pkt_type = HCI_SCODATA_PKT;
-			bt_cb(skb)->expect = HCI_SCO_HDR_SIZE;
+			hci_skb_pkt_type(skb) = HCI_SCODATA_PKT;
+			hci_skb_expect(skb) = HCI_SCO_HDR_SIZE;
 		}
 
-		len = min_t(uint, bt_cb(skb)->expect, count);
+		len = min_t(uint, hci_skb_expect(skb), count);
 		memcpy(skb_put(skb, len), buffer, len);
 
 		count -= len;
 		buffer += len;
-		bt_cb(skb)->expect -= len;
+		hci_skb_expect(skb) -= len;
 
 		if (skb->len == HCI_SCO_HDR_SIZE) {
 			/* Complete SCO header */
-			bt_cb(skb)->expect = hci_sco_hdr(skb)->dlen;
+			hci_skb_expect(skb) = hci_sco_hdr(skb)->dlen;
 
-			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+			if (skb_tailroom(skb) < hci_skb_expect(skb)) {
 				kfree_skb(skb);
 				skb = NULL;
 
@@ -573,7 +577,7 @@ static int btusb_recv_isoc(struct btusb_data *data, void *buffer, int count)
 			}
 		}
 
-		if (bt_cb(skb)->expect == 0) {
+		if (!hci_skb_expect(skb)) {
 			/* Complete frame */
 			hci_recv_frame(data->hdev, skb);
 			skb = NULL;
@@ -1257,7 +1261,7 @@ static int btusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 
 	BT_DBG("%s", hdev->name);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		urb = alloc_ctrl_urb(hdev, skb);
 		if (IS_ERR(urb))
@@ -1642,13 +1646,8 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	struct sk_buff *skb;
 	const struct firmware *fw;
 	const u8 *fw_ptr;
-	int disable_patch;
-	struct intel_version *ver;
-
-	const u8 mfg_enable[] = { 0x01, 0x00 };
-	const u8 mfg_disable[] = { 0x00, 0x00 };
-	const u8 mfg_reset_deactivate[] = { 0x00, 0x01 };
-	const u8 mfg_reset_activate[] = { 0x00, 0x02 };
+	int disable_patch, err;
+	struct intel_version ver;
 
 	BT_DBG("%s", hdev->name);
 
@@ -1674,35 +1673,22 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	 * The returned information are hardware variant and revision plus
 	 * firmware variant, revision and build number.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s reading Intel fw version command failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-
-	if (skb->len != sizeof(*ver)) {
-		BT_ERR("%s Intel version event length mismatch", hdev->name);
-		kfree_skb(skb);
-		return -EIO;
-	}
-
-	ver = (struct intel_version *)skb->data;
+	err = btintel_read_version(hdev, &ver);
+	if (err)
+		return err;
 
 	BT_INFO("%s: read Intel version: %02x%02x%02x%02x%02x%02x%02x%02x%02x",
-		hdev->name, ver->hw_platform, ver->hw_variant,
-		ver->hw_revision, ver->fw_variant,  ver->fw_revision,
-		ver->fw_build_num, ver->fw_build_ww, ver->fw_build_yy,
-		ver->fw_patch_num);
+		hdev->name, ver.hw_platform, ver.hw_variant, ver.hw_revision,
+		ver.fw_variant,  ver.fw_revision, ver.fw_build_num,
+		ver.fw_build_ww, ver.fw_build_yy, ver.fw_patch_num);
 
 	/* fw_patch_num indicates the version of patch the device currently
 	 * have. If there is no patch data in the device, it is always 0x00.
 	 * So, if it is other than 0x00, no need to patch the device again.
 	 */
-	if (ver->fw_patch_num) {
+	if (ver.fw_patch_num) {
 		BT_INFO("%s: Intel device is already patched. patch num: %02x",
-			hdev->name, ver->fw_patch_num);
-		kfree_skb(skb);
+			hdev->name, ver.fw_patch_num);
 		goto complete;
 	}
 
@@ -1712,31 +1698,21 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	 * If no patch file is found, allow the device to operate without
 	 * a patch.
 	 */
-	fw = btusb_setup_intel_get_fw(hdev, ver);
-	if (!fw) {
-		kfree_skb(skb);
+	fw = btusb_setup_intel_get_fw(hdev, &ver);
+	if (!fw)
 		goto complete;
-	}
 	fw_ptr = fw->data;
 
-	kfree_skb(skb);
-
-	/* This Intel specific command enables the manufacturer mode of the
-	 * controller.
-	 *
+	/* Enable the manufacturer mode of the controller.
 	 * Only while this mode is enabled, the driver can download the
 	 * firmware patch data and configuration parameters.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc11, 2, mfg_enable, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s entering Intel manufacturer mode failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
+	err = btintel_enter_mfg(hdev);
+	if (err) {
 		release_firmware(fw);
-		return PTR_ERR(skb);
+		return err;
 	}
 
-	kfree_skb(skb);
-
 	disable_patch = 1;
 
 	/* The firmware data file consists of list of Intel specific HCI
@@ -1776,14 +1752,9 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 	/* Patching completed successfully and disable the manufacturer mode
 	 * with reset and activate the downloaded firmware patches.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_activate),
-			     mfg_reset_activate, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	err = btintel_exit_mfg(hdev, true, true);
+	if (err)
+		return err;
 
 	BT_INFO("%s: Intel Bluetooth firmware patch completed and activated",
 		hdev->name);
@@ -1792,14 +1763,9 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 
 exit_mfg_disable:
 	/* Disable the manufacturer mode without reset */
-	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_disable), mfg_disable,
-			     HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	err = btintel_exit_mfg(hdev, false, false);
+	if (err)
+		return err;
 
 	BT_INFO("%s: Intel Bluetooth firmware patch completed", hdev->name);
 
@@ -1811,14 +1777,9 @@ exit_mfg_deactivate:
 	/* Patching failed. Disable the manufacturer mode with reset and
 	 * deactivate the downloaded firmware patches.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_deactivate),
-			     mfg_reset_deactivate, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-	kfree_skb(skb);
+	err = btintel_exit_mfg(hdev, true, false);
+	if (err)
+		return err;
 
 	BT_INFO("%s: Intel Bluetooth firmware patch completed and deactivated",
 		hdev->name);
@@ -1853,7 +1814,7 @@ static int inject_cmd_complete(struct hci_dev *hdev, __u16 opcode)
 
 	*skb_put(skb, 1) = 0x00;
 
-	bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
+	hci_skb_pkt_type(skb) = HCI_EVENT_PKT;
 
 	return hci_recv_frame(hdev, skb);
 }
@@ -1945,7 +1906,7 @@ static int btusb_send_frame_intel(struct hci_dev *hdev, struct sk_buff *skb)
 
 	BT_DBG("%s", hdev->name);
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		if (test_bit(BTUSB_BOOTLOADER, &data->flags)) {
 			struct hci_command_hdr *cmd = (void *)skb->data;
@@ -2005,7 +1966,7 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 					  0x00, 0x08, 0x04, 0x00 };
 	struct btusb_data *data = hci_get_drvdata(hdev);
 	struct sk_buff *skb;
-	struct intel_version *ver;
+	struct intel_version ver;
 	struct intel_boot_params *params;
 	const struct firmware *fw;
 	const u8 *fw_ptr;
@@ -2023,28 +1984,16 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	 * is in bootloader mode or if it already has operational firmware
 	 * loaded.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		BT_ERR("%s: Reading Intel version information failed (%ld)",
-		       hdev->name, PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-
-	if (skb->len != sizeof(*ver)) {
-		BT_ERR("%s: Intel version event size mismatch", hdev->name);
-		kfree_skb(skb);
-		return -EILSEQ;
-	}
-
-	ver = (struct intel_version *)skb->data;
+	err = btintel_read_version(hdev, &ver);
+	if (err)
+		return err;
 
 	/* The hardware platform number has a fixed value of 0x37 and
 	 * for now only accept this single value.
 	 */
-	if (ver->hw_platform != 0x37) {
+	if (ver.hw_platform != 0x37) {
 		BT_ERR("%s: Unsupported Intel hardware platform (%u)",
-		       hdev->name, ver->hw_platform);
-		kfree_skb(skb);
+		       hdev->name, ver.hw_platform);
 		return -EINVAL;
 	}
 
@@ -2053,14 +2002,13 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	 * put in place to ensure correct forward compatibility options
 	 * when newer hardware variants come along.
 	 */
-	if (ver->hw_variant != 0x0b) {
+	if (ver.hw_variant != 0x0b) {
 		BT_ERR("%s: Unsupported Intel hardware variant (%u)",
-		       hdev->name, ver->hw_variant);
-		kfree_skb(skb);
+		       hdev->name, ver.hw_variant);
 		return -EINVAL;
 	}
 
-	btintel_version_info(hdev, ver);
+	btintel_version_info(hdev, &ver);
 
 	/* The firmware variant determines if the device is in bootloader
 	 * mode or is running operational firmware. The value 0x06 identifies
@@ -2075,8 +2023,7 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	 * It is not possible to use the Secure Boot Parameters in this
 	 * case since that command is only available in bootloader mode.
 	 */
-	if (ver->fw_variant == 0x23) {
-		kfree_skb(skb);
+	if (ver.fw_variant == 0x23) {
 		clear_bit(BTUSB_BOOTLOADER, &data->flags);
 		btintel_check_bdaddr(hdev);
 		return 0;
@@ -2085,15 +2032,12 @@ static int btusb_setup_intel_new(struct hci_dev *hdev)
 	/* If the device is not in bootloader mode, then the only possible
 	 * choice is to return an error and abort the device initialization.
 	 */
-	if (ver->fw_variant != 0x06) {
+	if (ver.fw_variant != 0x06) {
 		BT_ERR("%s: Unsupported Intel firmware variant (%u)",
-		       hdev->name, ver->fw_variant);
-		kfree_skb(skb);
+		       hdev->name, ver.fw_variant);
 		return -ENODEV;
 	}
 
-	kfree_skb(skb);
-
 	/* Read the secure boot parameters to identify the operating
 	 * details of the bootloader.
 	 */
diff --git a/drivers/bluetooth/btwilink.c b/drivers/bluetooth/btwilink.c
index 57eb935aedc7..24a652f9252b 100644
--- a/drivers/bluetooth/btwilink.c
+++ b/drivers/bluetooth/btwilink.c
@@ -249,10 +249,10 @@ static int ti_st_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	hst = hci_get_drvdata(hdev);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
-	BT_DBG("%s: type %d len %d", hdev->name, bt_cb(skb)->pkt_type,
-			skb->len);
+	BT_DBG("%s: type %d len %d", hdev->name, hci_skb_pkt_type(skb),
+	       skb->len);
 
 	/* Insert skb to shared transport layer's transmit queue.
 	 * Freeing skb memory is taken care in shared transport layer,
@@ -268,7 +268,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, bt_cb(skb)->pkt_type);
+	ti_st_tx_complete(hst, hci_skb_pkt_type(skb));
 
 	return 0;
 }
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 5026f66fac88..6317c6f323bf 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -239,7 +239,7 @@ static void dtl1_receive(struct dtl1_info *info)
 				info->rx_count = nsh->len + (nsh->len & 0x0001);
 				break;
 			case RECV_WAIT_DATA:
-				bt_cb(info->rx_skb)->pkt_type = nsh->type;
+				hci_skb_pkt_type(info->rx_skb) = nsh->type;
 
 				/* remove PAD byte if it exists */
 				if (nsh->len & 0x0001) {
@@ -250,7 +250,7 @@ static void dtl1_receive(struct dtl1_info *info)
 				/* remove NSH */
 				skb_pull(info->rx_skb, NSHL);
 
-				switch (bt_cb(info->rx_skb)->pkt_type) {
+				switch (hci_skb_pkt_type(info->rx_skb)) {
 				case 0x80:
 					/* control data for the Nokia Card */
 					dtl1_control(info, info->rx_skb);
@@ -259,12 +259,13 @@ static void dtl1_receive(struct dtl1_info *info)
 				case 0x83:
 				case 0x84:
 					/* send frame to the HCI layer */
-					bt_cb(info->rx_skb)->pkt_type &= 0x0f;
+					hci_skb_pkt_type(info->rx_skb) &= 0x0f;
 					hci_recv_frame(info->hdev, info->rx_skb);
 					break;
 				default:
 					/* unknown packet */
-					BT_ERR("Unknown HCI packet with type 0x%02x received", bt_cb(info->rx_skb)->pkt_type);
+					BT_ERR("Unknown HCI packet with type 0x%02x received",
+					       hci_skb_pkt_type(info->rx_skb));
 					kfree_skb(info->rx_skb);
 					break;
 				}
@@ -386,7 +387,7 @@ static int dtl1_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 	struct sk_buff *s;
 	struct nsh nsh;
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		hdev->stat.cmd_tx++;
 		nsh.type = 0x81;
diff --git a/drivers/bluetooth/hci_ath.c b/drivers/bluetooth/hci_ath.c
index d776dfd51478..0ccf6bf01ed4 100644
--- a/drivers/bluetooth/hci_ath.c
+++ b/drivers/bluetooth/hci_ath.c
@@ -205,7 +205,7 @@ static int ath_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 {
 	struct ath_struct *ath = hu->priv;
 
-	if (bt_cb(skb)->pkt_type == HCI_SCODATA_PKT) {
+	if (hci_skb_pkt_type(skb) == HCI_SCODATA_PKT) {
 		kfree_skb(skb);
 		return 0;
 	}
@@ -213,7 +213,7 @@ static int ath_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	/* Update power management enable flag with parameters of
 	 * HCI sleep enable vendor specific HCI command.
 	 */
-	if (bt_cb(skb)->pkt_type == HCI_COMMAND_PKT) {
+	if (hci_skb_pkt_type(skb) == HCI_COMMAND_PKT) {
 		struct hci_command_hdr *hdr = (void *)skb->data;
 
 		if (__le16_to_cpu(hdr->opcode) == HCI_OP_ATH_SLEEP)
@@ -223,7 +223,7 @@ static int ath_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	BT_DBG("hu %p skb %p", hu, skb);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
 	skb_queue_tail(&ath->txq, skb);
 	set_bit(HCI_UART_SENDING, &hu->tx_state);
diff --git a/drivers/bluetooth/hci_bcm.c b/drivers/bluetooth/hci_bcm.c
index cb852cc750b7..5f3de181e744 100644
--- a/drivers/bluetooth/hci_bcm.c
+++ b/drivers/bluetooth/hci_bcm.c
@@ -472,7 +472,7 @@ static int bcm_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	bt_dev_dbg(hu->hdev, "hu %p skb %p", hu, skb);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&bcm->txq, skb);
 
 	return 0;
@@ -814,8 +814,16 @@ static const struct hci_uart_proto bcm_proto = {
 
 #ifdef CONFIG_ACPI
 static const struct acpi_device_id bcm_acpi_match[] = {
+	{ "BCM2E1A", 0 },
 	{ "BCM2E39", 0 },
+	{ "BCM2E3A", 0 },
+	{ "BCM2E3D", 0 },
+	{ "BCM2E3F", 0 },
+	{ "BCM2E40", 0 },
+	{ "BCM2E64", 0 },
+	{ "BCM2E65", 0 },
 	{ "BCM2E67", 0 },
+	{ "BCM2E7B", 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, bcm_acpi_match);
diff --git a/drivers/bluetooth/hci_bcsp.c b/drivers/bluetooth/hci_bcsp.c
index d0b615a932d1..064f2fefad62 100644
--- a/drivers/bluetooth/hci_bcsp.c
+++ b/drivers/bluetooth/hci_bcsp.c
@@ -155,7 +155,7 @@ static int bcsp_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 		return 0;
 	}
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_ACLDATA_PKT:
 	case HCI_COMMAND_PKT:
 		skb_queue_tail(&bcsp->rel, skb);
@@ -231,7 +231,7 @@ static struct sk_buff *bcsp_prepare_pkt(struct bcsp_struct *bcsp, u8 *data,
 	if (!nskb)
 		return NULL;
 
-	bt_cb(nskb)->pkt_type = pkt_type;
+	hci_skb_pkt_type(nskb) = pkt_type;
 
 	bcsp_slip_msgdelim(nskb);
 
@@ -291,7 +291,10 @@ static struct sk_buff *bcsp_dequeue(struct hci_uart *hu)
 
 	skb = skb_dequeue(&bcsp->unrel);
 	if (skb != NULL) {
-		struct sk_buff *nskb = bcsp_prepare_pkt(bcsp, skb->data, skb->len, bt_cb(skb)->pkt_type);
+		struct sk_buff *nskb;
+
+		nskb = bcsp_prepare_pkt(bcsp, skb->data, skb->len,
+					hci_skb_pkt_type(skb));
 		if (nskb) {
 			kfree_skb(skb);
 			return nskb;
@@ -310,8 +313,10 @@ static struct sk_buff *bcsp_dequeue(struct hci_uart *hu)
 	if (bcsp->unack.qlen < BCSP_TXWINSIZE) {
 		skb = skb_dequeue(&bcsp->rel);
 		if (skb != NULL) {
-			struct sk_buff *nskb = bcsp_prepare_pkt(bcsp, skb->data, skb->len,
-								bt_cb(skb)->pkt_type);
+			struct sk_buff *nskb;
+
+			nskb = bcsp_prepare_pkt(bcsp, skb->data, skb->len,
+						hci_skb_pkt_type(skb));
 			if (nskb) {
 				__skb_queue_tail(&bcsp->unack, skb);
 				mod_timer(&bcsp->tbcsp, jiffies + HZ / 4);
@@ -412,7 +417,7 @@ static void bcsp_handle_le_pkt(struct hci_uart *hu)
 		if (!nskb)
 			return;
 		memcpy(skb_put(nskb, 4), conf_rsp_pkt, 4);
-		bt_cb(nskb)->pkt_type = BCSP_LE_PKT;
+		hci_skb_pkt_type(nskb) = BCSP_LE_PKT;
 
 		skb_queue_head(&bcsp->unrel, nskb);
 		hci_uart_tx_wakeup(hu);
@@ -494,14 +499,14 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
 	bcsp_pkt_cull(bcsp);
 	if ((bcsp->rx_skb->data[1] & 0x0f) == 6 &&
 			bcsp->rx_skb->data[0] & 0x80) {
-		bt_cb(bcsp->rx_skb)->pkt_type = HCI_ACLDATA_PKT;
+		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) {
-		bt_cb(bcsp->rx_skb)->pkt_type = HCI_EVENT_PKT;
+		hci_skb_pkt_type(bcsp->rx_skb) = HCI_EVENT_PKT;
 		pass_up = 1;
 	} else if ((bcsp->rx_skb->data[1] & 0x0f) == 7) {
-		bt_cb(bcsp->rx_skb)->pkt_type = HCI_SCODATA_PKT;
+		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)) {
@@ -523,7 +528,7 @@ static void bcsp_complete_rx_pkt(struct hci_uart *hu)
 				hdr.evt = 0xff;
 				hdr.plen = bcsp->rx_skb->len;
 				memcpy(skb_push(bcsp->rx_skb, HCI_EVENT_HDR_SIZE), &hdr, HCI_EVENT_HDR_SIZE);
-				bt_cb(bcsp->rx_skb)->pkt_type = HCI_EVENT_PKT;
+				hci_skb_pkt_type(bcsp->rx_skb) = HCI_EVENT_PKT;
 
 				hci_recv_frame(hu->hdev, bcsp->rx_skb);
 			} else {
diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index a6fce48da0fb..635597b6e168 100644
--- a/drivers/bluetooth/hci_h4.c
+++ b/drivers/bluetooth/hci_h4.c
@@ -108,7 +108,7 @@ static int h4_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	BT_DBG("hu %p skb %p", hu, skb);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&h4->txq, skb);
 
 	return 0;
@@ -184,8 +184,8 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
 				if (!skb)
 					return ERR_PTR(-ENOMEM);
 
-				bt_cb(skb)->pkt_type = (&pkts[i])->type;
-				bt_cb(skb)->expect = (&pkts[i])->hlen;
+				hci_skb_pkt_type(skb) = (&pkts[i])->type;
+				hci_skb_expect(skb) = (&pkts[i])->hlen;
 				break;
 			}
 
@@ -197,18 +197,18 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
 			buffer += 1;
 		}
 
-		len = min_t(uint, bt_cb(skb)->expect - skb->len, count);
+		len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
 		memcpy(skb_put(skb, len), buffer, len);
 
 		count -= len;
 		buffer += len;
 
 		/* Check for partial packet */
-		if (skb->len < bt_cb(skb)->expect)
+		if (skb->len < hci_skb_expect(skb))
 			continue;
 
 		for (i = 0; i < pkts_count; i++) {
-			if (bt_cb(skb)->pkt_type == (&pkts[i])->type)
+			if (hci_skb_pkt_type(skb) == (&pkts[i])->type)
 				break;
 		}
 
@@ -228,7 +228,7 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
 			case 1:
 				/* Single octet variable length */
 				dlen = skb->data[(&pkts[i])->loff];
-				bt_cb(skb)->expect += dlen;
+				hci_skb_expect(skb) += dlen;
 
 				if (skb_tailroom(skb) < dlen) {
 					kfree_skb(skb);
@@ -239,7 +239,7 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb,
 				/* Double octet variable length */
 				dlen = get_unaligned_le16(skb->data +
 							  (&pkts[i])->loff);
-				bt_cb(skb)->expect += dlen;
+				hci_skb_expect(skb) += dlen;
 
 				if (skb_tailroom(skb) < dlen) {
 					kfree_skb(skb);
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index abee2216fdeb..0879d64b1caf 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -51,7 +51,7 @@
 #define H5_HDR_CRC(hdr)		(((hdr)[0] >> 6) & 0x01)
 #define H5_HDR_RELIABLE(hdr)	(((hdr)[0] >> 7) & 0x01)
 #define H5_HDR_PKT_TYPE(hdr)	((hdr)[1] & 0x0f)
-#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0xff) + ((hdr)[2] << 4))
+#define H5_HDR_LEN(hdr)		((((hdr)[1] >> 4) & 0x0f) + ((hdr)[2] << 4))
 
 #define SLIP_DELIMITER	0xc0
 #define SLIP_ESC	0xdb
@@ -107,7 +107,7 @@ static void h5_link_control(struct hci_uart *hu, const void *data, size_t len)
 	if (!nskb)
 		return;
 
-	bt_cb(nskb)->pkt_type = HCI_3WIRE_LINK_PKT;
+	hci_skb_pkt_type(nskb) = HCI_3WIRE_LINK_PKT;
 
 	memcpy(skb_put(nskb, len), data, len);
 
@@ -116,18 +116,14 @@ static void h5_link_control(struct hci_uart *hu, const void *data, size_t len)
 
 static u8 h5_cfg_field(struct h5 *h5)
 {
-	u8 field = 0;
-
 	/* Sliding window size (first 3 bits) */
-	field |= (h5->tx_win & 7);
-
-	return field;
+	return h5->tx_win & 0x07;
 }
 
 static void h5_timed_event(unsigned long arg)
 {
 	const unsigned char sync_req[] = { 0x01, 0x7e };
-	unsigned char conf_req[] = { 0x03, 0xfc, 0x01 };
+	unsigned char conf_req[3] = { 0x03, 0xfc };
 	struct hci_uart *hu = (struct hci_uart *)arg;
 	struct h5 *h5 = hu->priv;
 	struct sk_buff *skb;
@@ -285,7 +281,7 @@ static void h5_handle_internal_rx(struct hci_uart *hu)
 	struct h5 *h5 = hu->priv;
 	const unsigned char sync_req[] = { 0x01, 0x7e };
 	const unsigned char sync_rsp[] = { 0x02, 0x7d };
-	unsigned char conf_req[] = { 0x03, 0xfc, 0x01 };
+	unsigned char conf_req[3] = { 0x03, 0xfc };
 	const unsigned char conf_rsp[] = { 0x04, 0x7b };
 	const unsigned char wakeup_req[] = { 0x05, 0xfa };
 	const unsigned char woken_req[] = { 0x06, 0xf9 };
@@ -317,7 +313,7 @@ static void h5_handle_internal_rx(struct hci_uart *hu)
 		h5_link_control(hu, conf_req, 3);
 	} else if (memcmp(data, conf_rsp, 2) == 0) {
 		if (H5_HDR_LEN(hdr) > 2)
-			h5->tx_win = (data[2] & 7);
+			h5->tx_win = (data[2] & 0x07);
 		BT_DBG("Three-wire init complete. tx_win %u", h5->tx_win);
 		h5->state = H5_ACTIVE;
 		hci_uart_init_ready(hu);
@@ -360,7 +356,7 @@ static void h5_complete_rx_pkt(struct hci_uart *hu)
 	case HCI_EVENT_PKT:
 	case HCI_ACLDATA_PKT:
 	case HCI_SCODATA_PKT:
-		bt_cb(h5->rx_skb)->pkt_type = H5_HDR_PKT_TYPE(hdr);
+		hci_skb_pkt_type(h5->rx_skb) = H5_HDR_PKT_TYPE(hdr);
 
 		/* Remove Three-wire header */
 		skb_pull(h5->rx_skb, 4);
@@ -562,7 +558,7 @@ static int h5_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 		return 0;
 	}
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_ACLDATA_PKT:
 	case HCI_COMMAND_PKT:
 		skb_queue_tail(&h5->rel, skb);
@@ -573,7 +569,7 @@ static int h5_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 		break;
 
 	default:
-		BT_ERR("Unknown packet type %u", bt_cb(skb)->pkt_type);
+		BT_ERR("Unknown packet type %u", hci_skb_pkt_type(skb));
 		kfree_skb(skb);
 		break;
 	}
@@ -642,7 +638,7 @@ static struct sk_buff *h5_prepare_pkt(struct hci_uart *hu, u8 pkt_type,
 	if (!nskb)
 		return NULL;
 
-	bt_cb(nskb)->pkt_type = pkt_type;
+	hci_skb_pkt_type(nskb) = pkt_type;
 
 	h5_slip_delim(nskb);
 
@@ -697,7 +693,7 @@ static struct sk_buff *h5_dequeue(struct hci_uart *hu)
 
 	skb = skb_dequeue(&h5->unrel);
 	if (skb) {
-		nskb = h5_prepare_pkt(hu, bt_cb(skb)->pkt_type,
+		nskb = h5_prepare_pkt(hu, hci_skb_pkt_type(skb),
 				      skb->data, skb->len);
 		if (nskb) {
 			kfree_skb(skb);
@@ -715,7 +711,7 @@ static struct sk_buff *h5_dequeue(struct hci_uart *hu)
 
 	skb = skb_dequeue(&h5->rel);
 	if (skb) {
-		nskb = h5_prepare_pkt(hu, bt_cb(skb)->pkt_type,
+		nskb = h5_prepare_pkt(hu, hci_skb_pkt_type(skb),
 				      skb->data, skb->len);
 		if (nskb) {
 			__skb_queue_tail(&h5->unack, skb);
diff --git a/drivers/bluetooth/hci_intel.c b/drivers/bluetooth/hci_intel.c
index 4a414a5a3165..3d63ea37bd4c 100644
--- a/drivers/bluetooth/hci_intel.c
+++ b/drivers/bluetooth/hci_intel.c
@@ -186,7 +186,7 @@ static int intel_lpm_suspend(struct hci_uart *hu)
 	}
 
 	memcpy(skb_put(skb, sizeof(suspend)), suspend, sizeof(suspend));
-	bt_cb(skb)->pkt_type = HCI_LPM_PKT;
+	hci_skb_pkt_type(skb) = HCI_LPM_PKT;
 
 	set_bit(STATE_LPM_TRANSACTION, &intel->flags);
 
@@ -230,7 +230,7 @@ static int intel_lpm_resume(struct hci_uart *hu)
 		return -ENOMEM;
 	}
 
-	bt_cb(skb)->pkt_type = HCI_LPM_WAKE_PKT;
+	hci_skb_pkt_type(skb) = HCI_LPM_WAKE_PKT;
 
 	set_bit(STATE_LPM_TRANSACTION, &intel->flags);
 
@@ -272,7 +272,7 @@ static int intel_lpm_host_wake(struct hci_uart *hu)
 
 	memcpy(skb_put(skb, sizeof(lpm_resume_ack)), lpm_resume_ack,
 	       sizeof(lpm_resume_ack));
-	bt_cb(skb)->pkt_type = HCI_LPM_PKT;
+	hci_skb_pkt_type(skb) = HCI_LPM_PKT;
 
 	/* LPM flow is a priority, enqueue packet at list head */
 	skb_queue_head(&intel->txq, skb);
@@ -467,7 +467,7 @@ static int inject_cmd_complete(struct hci_dev *hdev, __u16 opcode)
 
 	*skb_put(skb, 1) = 0x00;
 
-	bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
+	hci_skb_pkt_type(skb) = HCI_EVENT_PKT;
 
 	return hci_recv_frame(hdev, skb);
 }
@@ -502,7 +502,7 @@ static int intel_set_baudrate(struct hci_uart *hu, unsigned int speed)
 	/* Device will not accept speed change if Intel version has not been
 	 * previously requested.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
+	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_CMD_TIMEOUT);
 	if (IS_ERR(skb)) {
 		bt_dev_err(hdev, "Reading Intel version information failed (%ld)",
 			   PTR_ERR(skb));
@@ -517,7 +517,7 @@ static int intel_set_baudrate(struct hci_uart *hu, unsigned int speed)
 	}
 
 	memcpy(skb_put(skb, sizeof(speed_cmd)), speed_cmd, sizeof(speed_cmd));
-	bt_cb(skb)->pkt_type = HCI_COMMAND_PKT;
+	hci_skb_pkt_type(skb) = HCI_COMMAND_PKT;
 
 	hci_uart_set_flow_control(hu, true);
 
@@ -542,7 +542,7 @@ static int intel_setup(struct hci_uart *hu)
 	struct intel_device *idev = NULL;
 	struct hci_dev *hdev = hu->hdev;
 	struct sk_buff *skb;
-	struct intel_version *ver;
+	struct intel_version ver;
 	struct intel_boot_params *params;
 	struct list_head *p;
 	const struct firmware *fw;
@@ -590,35 +590,16 @@ static int intel_setup(struct hci_uart *hu)
 	 * is in bootloader mode or if it already has operational firmware
 	 * loaded.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
-	if (IS_ERR(skb)) {
-		bt_dev_err(hdev, "Reading Intel version information failed (%ld)",
-			   PTR_ERR(skb));
-		return PTR_ERR(skb);
-	}
-
-	if (skb->len != sizeof(*ver)) {
-		bt_dev_err(hdev, "Intel version event size mismatch");
-		kfree_skb(skb);
-		return -EILSEQ;
-	}
-
-	ver = (struct intel_version *)skb->data;
-	if (ver->status) {
-		bt_dev_err(hdev, "Intel version command failure (%02x)",
-			   ver->status);
-		err = -bt_to_errno(ver->status);
-		kfree_skb(skb);
+	 err = btintel_read_version(hdev, &ver);
+	 if (err)
 		return err;
-	}
 
 	/* The hardware platform number has a fixed value of 0x37 and
 	 * for now only accept this single value.
 	 */
-	if (ver->hw_platform != 0x37) {
+	if (ver.hw_platform != 0x37) {
 		bt_dev_err(hdev, "Unsupported Intel hardware platform (%u)",
-			   ver->hw_platform);
-		kfree_skb(skb);
+			   ver.hw_platform);
 		return -EINVAL;
 	}
 
@@ -627,14 +608,13 @@ static int intel_setup(struct hci_uart *hu)
 	 * put in place to ensure correct forward compatibility options
 	 * when newer hardware variants come along.
 	 */
-	if (ver->hw_variant != 0x0b) {
+	if (ver.hw_variant != 0x0b) {
 		bt_dev_err(hdev, "Unsupported Intel hardware variant (%u)",
-			   ver->hw_variant);
-		kfree_skb(skb);
+			   ver.hw_variant);
 		return -EINVAL;
 	}
 
-	btintel_version_info(hdev, ver);
+	btintel_version_info(hdev, &ver);
 
 	/* The firmware variant determines if the device is in bootloader
 	 * mode or is running operational firmware. The value 0x06 identifies
@@ -649,8 +629,7 @@ static int intel_setup(struct hci_uart *hu)
 	 * It is not possible to use the Secure Boot Parameters in this
 	 * case since that command is only available in bootloader mode.
 	 */
-	if (ver->fw_variant == 0x23) {
-		kfree_skb(skb);
+	if (ver.fw_variant == 0x23) {
 		clear_bit(STATE_BOOTLOADER, &intel->flags);
 		btintel_check_bdaddr(hdev);
 		return 0;
@@ -659,19 +638,16 @@ static int intel_setup(struct hci_uart *hu)
 	/* If the device is not in bootloader mode, then the only possible
 	 * choice is to return an error and abort the device initialization.
 	 */
-	if (ver->fw_variant != 0x06) {
+	if (ver.fw_variant != 0x06) {
 		bt_dev_err(hdev, "Unsupported Intel firmware variant (%u)",
-			   ver->fw_variant);
-		kfree_skb(skb);
+			   ver.fw_variant);
 		return -ENODEV;
 	}
 
-	kfree_skb(skb);
-
 	/* Read the secure boot parameters to identify the operating
 	 * details of the bootloader.
 	 */
-	skb = __hci_cmd_sync(hdev, 0xfc0d, 0, NULL, HCI_INIT_TIMEOUT);
+	skb = __hci_cmd_sync(hdev, 0xfc0d, 0, NULL, HCI_CMD_TIMEOUT);
 	if (IS_ERR(skb)) {
 		bt_dev_err(hdev, "Reading Intel boot parameters failed (%ld)",
 			   PTR_ERR(skb));
@@ -881,7 +857,7 @@ done:
 	set_bit(STATE_BOOTING, &intel->flags);
 
 	skb = __hci_cmd_sync(hdev, 0xfc01, sizeof(reset_param), reset_param,
-			     HCI_INIT_TIMEOUT);
+			     HCI_CMD_TIMEOUT);
 	if (IS_ERR(skb))
 		return PTR_ERR(skb);
 
@@ -1126,7 +1102,7 @@ static struct sk_buff *intel_dequeue(struct hci_uart *hu)
 		return skb;
 
 	if (test_bit(STATE_BOOTLOADER, &intel->flags) &&
-	    (bt_cb(skb)->pkt_type == HCI_COMMAND_PKT)) {
+	    (hci_skb_pkt_type(skb) == HCI_COMMAND_PKT)) {
 		struct hci_command_hdr *cmd = (void *)skb->data;
 		__u16 opcode = le16_to_cpu(cmd->opcode);
 
@@ -1140,7 +1116,7 @@ static struct sk_buff *intel_dequeue(struct hci_uart *hu)
 	}
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
 	return skb;
 }
diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 96bcec5598c2..73202624133b 100644
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -162,7 +162,7 @@ restart:
 			break;
 		}
 
-		hci_uart_tx_complete(hu, bt_cb(skb)->pkt_type);
+		hci_uart_tx_complete(hu, hci_skb_pkt_type(skb));
 		kfree_skb(skb);
 	}
 
@@ -248,7 +248,8 @@ static int hci_uart_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_uart *hu = hci_get_drvdata(hdev);
 
-	BT_DBG("%s: type %d len %d", hdev->name, bt_cb(skb)->pkt_type, skb->len);
+	BT_DBG("%s: type %d len %d", hdev->name, hci_skb_pkt_type(skb),
+	       skb->len);
 
 	hu->proto->enqueue(hu, skb);
 
@@ -461,13 +462,7 @@ static int hci_uart_tty_open(struct tty_struct *tty)
 	INIT_WORK(&hu->init_ready, hci_uart_init_work);
 	INIT_WORK(&hu->write_work, hci_uart_write_work);
 
-	/* Flush any pending characters in the driver and line discipline. */
-
-	/* FIXME: why is this needed. Note don't use ldisc_ref here as the
-	   open path is before the ldisc is referencable */
-
-	if (tty->ldisc->ops->flush_buffer)
-		tty->ldisc->ops->flush_buffer(tty);
+	/* Flush any pending characters in the driver */
 	tty_driver_flush_buffer(tty);
 
 	return 0;
diff --git a/drivers/bluetooth/hci_ll.c b/drivers/bluetooth/hci_ll.c
index 9ee24b075f79..02692fe30279 100644
--- a/drivers/bluetooth/hci_ll.c
+++ b/drivers/bluetooth/hci_ll.c
@@ -307,7 +307,7 @@ static int ll_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	BT_DBG("hu %p skb %p", hu, skb);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
 	/* lock hcill state */
 	spin_lock_irqsave(&ll->hcill_lock, flags);
@@ -493,7 +493,7 @@ static int ll_recv(struct hci_uart *hu, const void *data, int count)
 			return -ENOMEM;
 		}
 
-		bt_cb(ll->rx_skb)->pkt_type = type;
+		hci_skb_pkt_type(ll->rx_skb) = type;
 	}
 
 	return count;
diff --git a/drivers/bluetooth/hci_qca.c b/drivers/bluetooth/hci_qca.c
index 71325e443e46..683c2b642057 100644
--- a/drivers/bluetooth/hci_qca.c
+++ b/drivers/bluetooth/hci_qca.c
@@ -678,7 +678,7 @@ static int qca_enqueue(struct hci_uart *hu, struct sk_buff *skb)
 	       qca->tx_ibs_state);
 
 	/* Prepend skb with frame type */
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 
 	/* Don't go to sleep in middle of patch download or
 	 * Out-Of-Band(GPIOs control) sleep is selected.
@@ -873,7 +873,7 @@ static int qca_set_baudrate(struct hci_dev *hdev, uint8_t baudrate)
 
 	/* Assign commands to change baudrate and packet type. */
 	memcpy(skb_put(skb, sizeof(cmd)), cmd, sizeof(cmd));
-	bt_cb(skb)->pkt_type = HCI_COMMAND_PKT;
+	hci_skb_pkt_type(skb) = HCI_COMMAND_PKT;
 
 	skb_queue_tail(&qca->txq, skb);
 	hci_uart_tx_wakeup(hu);
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index ed888e302bc3..80783dcb7f57 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -80,7 +80,7 @@ static int vhci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct vhci_data *data = hci_get_drvdata(hdev);
 
-	memcpy(skb_push(skb, 1), &bt_cb(skb)->pkt_type, 1);
+	memcpy(skb_push(skb, 1), &hci_skb_pkt_type(skb), 1);
 	skb_queue_tail(&data->readq, skb);
 
 	wake_up_interruptible(&data->read_wait);
@@ -140,7 +140,7 @@ static int vhci_create_device(struct vhci_data *data, __u8 opcode)
 		return -EBUSY;
 	}
 
-	bt_cb(skb)->pkt_type = HCI_VENDOR_PKT;
+	hci_skb_pkt_type(skb) = HCI_VENDOR_PKT;
 
 	*skb_put(skb, 1) = 0xff;
 	*skb_put(skb, 1) = opcode;
@@ -183,7 +183,7 @@ static inline ssize_t vhci_get_user(struct vhci_data *data,
 			return -ENODEV;
 		}
 
-		bt_cb(skb)->pkt_type = pkt_type;
+		hci_skb_pkt_type(skb) = pkt_type;
 
 		ret = hci_recv_frame(data->hdev, skb);
 		break;
@@ -234,7 +234,7 @@ static inline ssize_t vhci_put_user(struct vhci_data *data,
 
 	data->hdev->stat.byte_tx += len;
 
-	switch (bt_cb(skb)->pkt_type) {
+	switch (hci_skb_pkt_type(skb)) {
 	case HCI_COMMAND_PKT:
 		data->hdev->stat.cmd_tx++;
 		break;
diff --git a/drivers/bus/Kconfig b/drivers/bus/Kconfig
index 116b363b7987..9a92c072a485 100644
--- a/drivers/bus/Kconfig
+++ b/drivers/bus/Kconfig
@@ -131,6 +131,14 @@ config SUNXI_RSB
 	  with various RSB based devices, such as AXP223, AXP8XX PMICs,
 	  and AC100/AC200 ICs.
 
+config UNIPHIER_SYSTEM_BUS
+	tristate "UniPhier System Bus driver"
+	depends on ARCH_UNIPHIER && OF
+	default y
+	help
+	  Support for UniPhier System Bus, a simple external bus.  This is
+	  needed to use on-board devices connected to UniPhier SoCs.
+
 config VEXPRESS_CONFIG
 	bool "Versatile Express configuration bus"
 	default y if ARCH_VEXPRESS
diff --git a/drivers/bus/Makefile b/drivers/bus/Makefile
index fcb9f9794a1f..ccff007ee7e8 100644
--- a/drivers/bus/Makefile
+++ b/drivers/bus/Makefile
@@ -17,4 +17,5 @@ obj-$(CONFIG_OMAP_INTERCONNECT)	+= omap_l3_smx.o omap_l3_noc.o
 obj-$(CONFIG_OMAP_OCP2SCP)	+= omap-ocp2scp.o
 obj-$(CONFIG_SUNXI_RSB)		+= sunxi-rsb.o
 obj-$(CONFIG_SIMPLE_PM_BUS)	+= simple-pm-bus.o
+obj-$(CONFIG_UNIPHIER_SYSTEM_BUS)	+= uniphier-system-bus.o
 obj-$(CONFIG_VEXPRESS_CONFIG)	+= vexpress-config.o
diff --git a/drivers/bus/uniphier-system-bus.c b/drivers/bus/uniphier-system-bus.c
new file mode 100644
index 000000000000..834a2aeaf27a
--- /dev/null
+++ b/drivers/bus/uniphier-system-bus.c
@@ -0,0 +1,281 @@
+/*
+ * Copyright (C) 2015 Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/io.h>
+#include <linux/log2.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+/* System Bus Controller registers */
+#define UNIPHIER_SBC_BASE	0x100	/* base address of bank0 space */
+#define    UNIPHIER_SBC_BASE_BE		BIT(0)	/* bank_enable */
+#define UNIPHIER_SBC_CTRL0	0x200	/* timing parameter 0 of bank0 */
+#define UNIPHIER_SBC_CTRL1	0x204	/* timing parameter 1 of bank0 */
+#define UNIPHIER_SBC_CTRL2	0x208	/* timing parameter 2 of bank0 */
+#define UNIPHIER_SBC_CTRL3	0x20c	/* timing parameter 3 of bank0 */
+#define UNIPHIER_SBC_CTRL4	0x300	/* timing parameter 4 of bank0 */
+
+#define UNIPHIER_SBC_STRIDE	0x10	/* register stride to next bank */
+#define UNIPHIER_SBC_NR_BANKS	8	/* number of banks (chip select) */
+#define UNIPHIER_SBC_BASE_DUMMY	0xffffffff	/* data to squash bank 0, 1 */
+
+struct uniphier_system_bus_bank {
+	u32 base;
+	u32 end;
+};
+
+struct uniphier_system_bus_priv {
+	struct device *dev;
+	void __iomem *membase;
+	struct uniphier_system_bus_bank bank[UNIPHIER_SBC_NR_BANKS];
+};
+
+static int uniphier_system_bus_add_bank(struct uniphier_system_bus_priv *priv,
+					int bank, u32 addr, u64 paddr, u32 size)
+{
+	u64 end, mask;
+
+	dev_dbg(priv->dev,
+		"range found: bank = %d, addr = %08x, paddr = %08llx, size = %08x\n",
+		bank, addr, paddr, size);
+
+	if (bank >= ARRAY_SIZE(priv->bank)) {
+		dev_err(priv->dev, "unsupported bank number %d\n", bank);
+		return -EINVAL;
+	}
+
+	if (priv->bank[bank].base || priv->bank[bank].end) {
+		dev_err(priv->dev,
+			"range for bank %d has already been specified\n", bank);
+		return -EINVAL;
+	}
+
+	if (paddr > U32_MAX) {
+		dev_err(priv->dev, "base address %llx is too high\n", paddr);
+		return -EINVAL;
+	}
+
+	end = paddr + size;
+
+	if (addr > paddr) {
+		dev_err(priv->dev,
+			"base %08x cannot be mapped to %08llx of parent\n",
+			addr, paddr);
+		return -EINVAL;
+	}
+	paddr -= addr;
+
+	paddr = round_down(paddr, 0x00020000);
+	end = round_up(end, 0x00020000);
+
+	if (end > U32_MAX) {
+		dev_err(priv->dev, "end address %08llx is too high\n", end);
+		return -EINVAL;
+	}
+	mask = paddr ^ (end - 1);
+	mask = roundup_pow_of_two(mask);
+
+	paddr = round_down(paddr, mask);
+	end = round_up(end, mask);
+
+	priv->bank[bank].base = paddr;
+	priv->bank[bank].end = end;
+
+	dev_dbg(priv->dev, "range added: bank = %d, addr = %08x, end = %08x\n",
+		bank, priv->bank[bank].base, priv->bank[bank].end);
+
+	return 0;
+}
+
+static int uniphier_system_bus_check_overlap(
+				const struct uniphier_system_bus_priv *priv)
+{
+	int i, j;
+
+	for (i = 0; i < ARRAY_SIZE(priv->bank); i++) {
+		for (j = i + 1; j < ARRAY_SIZE(priv->bank); j++) {
+			if (priv->bank[i].end > priv->bank[j].base ||
+			    priv->bank[i].base < priv->bank[j].end) {
+				dev_err(priv->dev,
+					"region overlap between bank%d and bank%d\n",
+					i, j);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static void uniphier_system_bus_check_boot_swap(
+					struct uniphier_system_bus_priv *priv)
+{
+	void __iomem *base_reg = priv->membase + UNIPHIER_SBC_BASE;
+	int is_swapped;
+
+	is_swapped = !(readl(base_reg) & UNIPHIER_SBC_BASE_BE);
+
+	dev_dbg(priv->dev, "Boot Swap: %s\n", is_swapped ? "on" : "off");
+
+	/*
+	 * If BOOT_SWAP was asserted on power-on-reset, the CS0 and CS1 are
+	 * swapped.  In this case, bank0 and bank1 should be swapped as well.
+	 */
+	if (is_swapped)
+		swap(priv->bank[0], priv->bank[1]);
+}
+
+static void uniphier_system_bus_set_reg(
+				const struct uniphier_system_bus_priv *priv)
+{
+	void __iomem *base_reg = priv->membase + UNIPHIER_SBC_BASE;
+	u32 base, end, mask, val;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(priv->bank); i++) {
+		base = priv->bank[i].base;
+		end = priv->bank[i].end;
+
+		if (base == end) {
+			/*
+			 * If SBC_BASE0 or SBC_BASE1 is set to zero, the access
+			 * to anywhere in the system bus space is routed to
+			 * bank 0 (if boot swap if off) or bank 1 (if boot swap
+			 * if on).  It means that CPUs cannot get access to
+			 * bank 2 or later.  In other words, bank 0/1 cannot
+			 * be disabled even if its bank_enable bits is cleared.
+			 * This seems odd, but it is how this hardware goes.
+			 * As a workaround, dummy data (0xffffffff) should be
+			 * set when the bank 0/1 is unused.  As for bank 2 and
+			 * later, they can be simply disable by clearing the
+			 * bank_enable bit.
+			 */
+			if (i < 2)
+				val = UNIPHIER_SBC_BASE_DUMMY;
+			else
+				val = 0;
+		} else {
+			mask = base ^ (end - 1);
+
+			val = base & 0xfffe0000;
+			val |= (~mask >> 16) & 0xfffe;
+			val |= UNIPHIER_SBC_BASE_BE;
+		}
+		dev_dbg(priv->dev, "SBC_BASE[%d] = 0x%08x\n", i, val);
+
+		writel(val, base_reg + UNIPHIER_SBC_STRIDE * i);
+	}
+}
+
+static int uniphier_system_bus_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct uniphier_system_bus_priv *priv;
+	struct resource *regs;
+	const __be32 *ranges;
+	u32 cells, addr, size;
+	u64 paddr;
+	int pna, bank, rlen, rone, ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->membase = devm_ioremap_resource(dev, regs);
+	if (IS_ERR(priv->membase))
+		return PTR_ERR(priv->membase);
+
+	priv->dev = dev;
+
+	pna = of_n_addr_cells(dev->of_node);
+
+	ret = of_property_read_u32(dev->of_node, "#address-cells", &cells);
+	if (ret) {
+		dev_err(dev, "failed to get #address-cells\n");
+		return ret;
+	}
+	if (cells != 2) {
+		dev_err(dev, "#address-cells must be 2\n");
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32(dev->of_node, "#size-cells", &cells);
+	if (ret) {
+		dev_err(dev, "failed to get #size-cells\n");
+		return ret;
+	}
+	if (cells != 1) {
+		dev_err(dev, "#size-cells must be 1\n");
+		return -EINVAL;
+	}
+
+	ranges = of_get_property(dev->of_node, "ranges", &rlen);
+	if (!ranges) {
+		dev_err(dev, "failed to get ranges property\n");
+		return -ENOENT;
+	}
+
+	rlen /= sizeof(*ranges);
+	rone = pna + 2;
+
+	for (; rlen >= rone; rlen -= rone) {
+		bank = be32_to_cpup(ranges++);
+		addr = be32_to_cpup(ranges++);
+		paddr = of_translate_address(dev->of_node, ranges);
+		if (paddr == OF_BAD_ADDR)
+			return -EINVAL;
+		ranges += pna;
+		size = be32_to_cpup(ranges++);
+
+		ret = uniphier_system_bus_add_bank(priv, bank, addr,
+						   paddr, size);
+		if (ret)
+			return ret;
+	}
+
+	ret = uniphier_system_bus_check_overlap(priv);
+	if (ret)
+		return ret;
+
+	uniphier_system_bus_check_boot_swap(priv);
+
+	uniphier_system_bus_set_reg(priv);
+
+	/* Now, the bus is configured.  Populate platform_devices below it */
+	return of_platform_populate(dev->of_node, of_default_bus_match_table,
+				    NULL, dev);
+}
+
+static const struct of_device_id uniphier_system_bus_match[] = {
+	{ .compatible = "socionext,uniphier-system-bus" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, uniphier_system_bus_match);
+
+static struct platform_driver uniphier_system_bus_driver = {
+	.probe		= uniphier_system_bus_probe,
+	.driver = {
+		.name	= "uniphier-system-bus",
+		.of_match_table = uniphier_system_bus_match,
+	},
+};
+module_platform_driver(uniphier_system_bus_driver);
+
+MODULE_AUTHOR("Masahiro Yamada <yamada.masahiro@socionext.com>");
+MODULE_DESCRIPTION("UniPhier System Bus driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/bus/vexpress-config.c b/drivers/bus/vexpress-config.c
index 6575c0fe6a4e..c3cb76b363c6 100644
--- a/drivers/bus/vexpress-config.c
+++ b/drivers/bus/vexpress-config.c
@@ -192,8 +192,10 @@ static int __init vexpress_config_init(void)
 	/* Need the config devices early, before the "normal" devices... */
 	for_each_compatible_node(node, NULL, "arm,vexpress,config-bus") {
 		err = vexpress_config_populate(node);
-		if (err)
+		if (err) {
+			of_node_put(node);
 			break;
+		}
 	}
 
 	return err;
diff --git a/drivers/cdrom/cdrom.c b/drivers/cdrom/cdrom.c
index c206ccda899b..1b257ea9776a 100644
--- a/drivers/cdrom/cdrom.c
+++ b/drivers/cdrom/cdrom.c
@@ -3186,15 +3186,11 @@ static noinline int mmc_ioctl_dvd_read_struct(struct cdrom_device_info *cdi,
 	if (!CDROM_CAN(CDC_DVD))
 		return -ENOSYS;
 
-	s = kmalloc(size, GFP_KERNEL);
-	if (!s)
-		return -ENOMEM;
+	s = memdup_user(arg, size);
+	if (IS_ERR(s))
+		return PTR_ERR(s);
 
 	cd_dbg(CD_DO_IOCTL, "entering DVD_READ_STRUCT\n");
-	if (copy_from_user(s, arg, size)) {
-		kfree(s);
-		return -EFAULT;
-	}
 
 	ret = dvd_read_struct(cdi, s, cgc);
 	if (ret)
diff --git a/drivers/char/generic_nvram.c b/drivers/char/generic_nvram.c
index 6c4f4b5a9dd3..073db9558379 100644
--- a/drivers/char/generic_nvram.c
+++ b/drivers/char/generic_nvram.c
@@ -20,6 +20,7 @@
 #include <linux/fcntl.h>
 #include <linux/init.h>
 #include <linux/mutex.h>
+#include <linux/pagemap.h>
 #include <asm/uaccess.h>
 #include <asm/nvram.h>
 #ifdef CONFIG_PPC_PMAC
@@ -33,24 +34,8 @@ static ssize_t nvram_len;
 
 static loff_t nvram_llseek(struct file *file, loff_t offset, int origin)
 {
-	switch (origin) {
-	case 0:
-		break;
-	case 1:
-		offset += file->f_pos;
-		break;
-	case 2:
-		offset += nvram_len;
-		break;
-	default:
-		offset = -1;
-	}
-	if (offset < 0)
-		return -EINVAL;
-
-	file->f_pos = offset;
-
-	return file->f_pos;
+	return generic_file_llseek_size(file, offset, origin,
+					MAX_LFS_FILESIZE, nvram_len);
 }
 
 static ssize_t read_nvram(struct file *file, char __user *buf,
diff --git a/drivers/char/hpet.c b/drivers/char/hpet.c
index 240b6cf1d97c..be54e5331a45 100644
--- a/drivers/char/hpet.c
+++ b/drivers/char/hpet.c
@@ -42,7 +42,7 @@
 /*
  * The High Precision Event Timer driver.
  * This driver is closely modelled after the rtc.c driver.
- * http://www.intel.com/hardwaredesign/hpetspec_1.pdf
+ * See HPET spec revision 1.
  */
 #define	HPET_USER_FREQ	(64)
 #define	HPET_DRIFT	(500)
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index dbf22719462f..ff00331bff49 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -372,6 +372,7 @@ config HW_RANDOM_XGENE
 config HW_RANDOM_STM32
 	tristate "STMicroelectronics STM32 random number generator"
 	depends on HW_RANDOM && (ARCH_STM32 || COMPILE_TEST)
+	depends on HAS_IOMEM
 	help
 	  This driver provides kernel-side support for the Random Number
 	  Generator hardware found on STM32 microcontrollers.
diff --git a/drivers/char/hw_random/core.c b/drivers/char/hw_random/core.c
index 6f497aa1b276..9203f2d130c0 100644
--- a/drivers/char/hw_random/core.c
+++ b/drivers/char/hw_random/core.c
@@ -238,7 +238,10 @@ static ssize_t rng_dev_read(struct file *filp, char __user *buf,
 			goto out;
 		}
 
-		mutex_lock(&reading_mutex);
+		if (mutex_lock_interruptible(&reading_mutex)) {
+			err = -ERESTARTSYS;
+			goto out_put;
+		}
 		if (!data_avail) {
 			bytes_read = rng_get_data(rng, rng_buffer,
 				rng_buffer_size(),
@@ -288,6 +291,7 @@ out:
 
 out_unlock_reading:
 	mutex_unlock(&reading_mutex);
+out_put:
 	put_rng(rng);
 	goto out;
 }
diff --git a/drivers/char/hw_random/omap3-rom-rng.c b/drivers/char/hw_random/omap3-rom-rng.c
index a405cdcd8dd2..8da14f1a1f56 100644
--- a/drivers/char/hw_random/omap3-rom-rng.c
+++ b/drivers/char/hw_random/omap3-rom-rng.c
@@ -17,7 +17,7 @@
 #include <linux/init.h>
 #include <linux/random.h>
 #include <linux/hw_random.h>
-#include <linux/timer.h>
+#include <linux/workqueue.h>
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/platform_device.h>
@@ -29,11 +29,11 @@
 /* param1: ptr, param2: count, param3: flag */
 static u32 (*omap3_rom_rng_call)(u32, u32, u32);
 
-static struct timer_list idle_timer;
+static struct delayed_work idle_work;
 static int rng_idle;
 static struct clk *rng_clk;
 
-static void omap3_rom_rng_idle(unsigned long data)
+static void omap3_rom_rng_idle(struct work_struct *work)
 {
 	int r;
 
@@ -51,7 +51,7 @@ static int omap3_rom_rng_get_random(void *buf, unsigned int count)
 	u32 r;
 	u32 ptr;
 
-	del_timer_sync(&idle_timer);
+	cancel_delayed_work_sync(&idle_work);
 	if (rng_idle) {
 		clk_prepare_enable(rng_clk);
 		r = omap3_rom_rng_call(0, 0, RNG_GEN_PRNG_HW_INIT);
@@ -65,7 +65,7 @@ static int omap3_rom_rng_get_random(void *buf, unsigned int count)
 
 	ptr = virt_to_phys(buf);
 	r = omap3_rom_rng_call(ptr, count, RNG_GEN_HW);
-	mod_timer(&idle_timer, jiffies + msecs_to_jiffies(500));
+	schedule_delayed_work(&idle_work, msecs_to_jiffies(500));
 	if (r != 0)
 		return -EINVAL;
 	return 0;
@@ -102,7 +102,7 @@ static int omap3_rom_rng_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	setup_timer(&idle_timer, omap3_rom_rng_idle, 0);
+	INIT_DELAYED_WORK(&idle_work, omap3_rom_rng_idle);
 	rng_clk = devm_clk_get(&pdev->dev, "ick");
 	if (IS_ERR(rng_clk)) {
 		pr_err("unable to get RNG clock\n");
@@ -118,6 +118,7 @@ static int omap3_rom_rng_probe(struct platform_device *pdev)
 
 static int omap3_rom_rng_remove(struct platform_device *pdev)
 {
+	cancel_delayed_work_sync(&idle_work);
 	hwrng_unregister(&omap3_rom_rng_ops);
 	clk_disable_unprepare(rng_clk);
 	return 0;
diff --git a/drivers/char/hw_random/via-rng.c b/drivers/char/hw_random/via-rng.c
index 0c98a9d51a24..44ce80606944 100644
--- a/drivers/char/hw_random/via-rng.c
+++ b/drivers/char/hw_random/via-rng.c
@@ -140,7 +140,7 @@ static int via_rng_init(struct hwrng *rng)
 	 * RNG configuration like it used to be the case in this
 	 * register */
 	if ((c->x86 == 6) && (c->x86_model >= 0x0f)) {
-		if (!cpu_has_xstore_enabled) {
+		if (!boot_cpu_has(X86_FEATURE_XSTORE_EN)) {
 			pr_err(PFX "can't enable hardware RNG "
 				"if XSTORE is not enabled\n");
 			return -ENODEV;
@@ -200,8 +200,9 @@ static int __init mod_init(void)
 {
 	int err;
 
-	if (!cpu_has_xstore)
+	if (!boot_cpu_has(X86_FEATURE_XSTORE))
 		return -ENODEV;
+
 	pr_info("VIA RNG detected\n");
 	err = hwrng_register(&via_rng);
 	if (err) {
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index e3536da05c88..94fb407d8561 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -472,9 +472,10 @@ static DEFINE_MUTEX(smi_watchers_mutex);
 #define ipmi_get_stat(intf, stat) \
 	((unsigned int) atomic_read(&(intf)->stats[IPMI_STAT_ ## stat]))
 
-static char *addr_src_to_str[] = { "invalid", "hotmod", "hardcoded", "SPMI",
-				   "ACPI", "SMBIOS", "PCI",
-				   "device-tree", "default" };
+static const char * const addr_src_to_str[] = {
+	"invalid", "hotmod", "hardcoded", "SPMI", "ACPI", "SMBIOS", "PCI",
+	"device-tree", "default"
+};
 
 const char *ipmi_addr_src_to_str(enum ipmi_addr_src src)
 {
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 4cc72fa017c7..7fddd8696211 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -68,6 +68,7 @@
 #include <linux/of_platform.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
+#include <linux/acpi.h>
 
 #ifdef CONFIG_PARISC
 #include <asm/hardware.h>	/* for register_parisc_driver() stuff */
@@ -105,7 +106,8 @@ enum si_intf_state {
 enum si_type {
     SI_KCS, SI_SMIC, SI_BT
 };
-static char *si_to_str[] = { "kcs", "smic", "bt" };
+
+static const char * const si_to_str[] = { "kcs", "smic", "bt" };
 
 #define DEVICE_NAME "ipmi_si"
 
@@ -1341,7 +1343,7 @@ static unsigned int num_slave_addrs;
 
 #define IPMI_IO_ADDR_SPACE  0
 #define IPMI_MEM_ADDR_SPACE 1
-static char *addr_space_to_str[] = { "i/o", "mem" };
+static const char * const addr_space_to_str[] = { "i/o", "mem" };
 
 static int hotmod_handler(const char *val, struct kernel_param *kp);
 
@@ -1723,27 +1725,31 @@ static int mem_setup(struct smi_info *info)
  */
 enum hotmod_op { HM_ADD, HM_REMOVE };
 struct hotmod_vals {
-	char *name;
-	int  val;
+	const char *name;
+	const int  val;
 };
-static struct hotmod_vals hotmod_ops[] = {
+
+static const struct hotmod_vals hotmod_ops[] = {
 	{ "add",	HM_ADD },
 	{ "remove",	HM_REMOVE },
 	{ NULL }
 };
-static struct hotmod_vals hotmod_si[] = {
+
+static const struct hotmod_vals hotmod_si[] = {
 	{ "kcs",	SI_KCS },
 	{ "smic",	SI_SMIC },
 	{ "bt",		SI_BT },
 	{ NULL }
 };
-static struct hotmod_vals hotmod_as[] = {
+
+static const struct hotmod_vals hotmod_as[] = {
 	{ "mem",	IPMI_MEM_ADDR_SPACE },
 	{ "i/o",	IPMI_IO_ADDR_SPACE },
 	{ NULL }
 };
 
-static int parse_str(struct hotmod_vals *v, int *val, char *name, char **curr)
+static int parse_str(const struct hotmod_vals *v, int *val, char *name,
+		     char **curr)
 {
 	char *s;
 	int  i;
@@ -2049,8 +2055,6 @@ static int hardcode_find_bmc(void)
 
 #ifdef CONFIG_ACPI
 
-#include <linux/acpi.h>
-
 /*
  * Once we get an ACPI failure, we don't try any more, because we go
  * through the tables sequentially.  Once we don't find a table, there
@@ -2554,7 +2558,6 @@ static void ipmi_pci_remove(struct pci_dev *pdev)
 {
 	struct smi_info *info = pci_get_drvdata(pdev);
 	cleanup_one_si(info);
-	pci_disable_device(pdev);
 }
 
 static const struct pci_device_id ipmi_pci_devices[] = {
@@ -2870,7 +2873,7 @@ static int ipmi_parisc_remove(struct parisc_device *dev)
 	return 0;
 }
 
-static struct parisc_device_id ipmi_parisc_tbl[] = {
+static const struct parisc_device_id ipmi_parisc_tbl[] = {
 	{ HPHW_MC, HVERSION_REV_ANY_ID, 0x004, 0xC0 },
 	{ 0, }
 };
@@ -3444,8 +3447,8 @@ static inline void wait_for_timer_and_thread(struct smi_info *smi_info)
 
 static const struct ipmi_default_vals
 {
-	int type;
-	int port;
+	const int type;
+	const int port;
 } ipmi_defaults[] =
 {
 	{ .type = SI_KCS, .port = 0xca2 },
diff --git a/drivers/char/ipmi/ipmi_ssif.c b/drivers/char/ipmi/ipmi_ssif.c
index 90e624662257..5f1c3d08ba65 100644
--- a/drivers/char/ipmi/ipmi_ssif.c
+++ b/drivers/char/ipmi/ipmi_ssif.c
@@ -1959,7 +1959,6 @@ MODULE_DEVICE_TABLE(i2c, ssif_id);
 static struct i2c_driver ssif_i2c_driver = {
 	.class		= I2C_CLASS_HWMON,
 	.driver		= {
-		.owner			= THIS_MODULE,
 		.name			= DEVICE_NAME
 	},
 	.probe		= ssif_probe,
diff --git a/drivers/char/mbcs.c b/drivers/char/mbcs.c
index e5d3e3f7a49b..67d426470e53 100644
--- a/drivers/char/mbcs.c
+++ b/drivers/char/mbcs.c
@@ -26,6 +26,7 @@
 #include <linux/uio.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/pagemap.h>
 #include <asm/io.h>
 #include <asm/uaccess.h>
 #include <asm/pgtable.h>
@@ -451,31 +452,8 @@ mbcs_sram_write(struct file * fp, const char __user *buf, size_t len, loff_t * o
 
 static loff_t mbcs_sram_llseek(struct file * filp, loff_t off, int whence)
 {
-	loff_t newpos;
-
-	switch (whence) {
-	case SEEK_SET:
-		newpos = off;
-		break;
-
-	case SEEK_CUR:
-		newpos = filp->f_pos + off;
-		break;
-
-	case SEEK_END:
-		newpos = MBCS_SRAM_SIZE + off;
-		break;
-
-	default:		/* can't happen */
-		return -EINVAL;
-	}
-
-	if (newpos < 0)
-		return -EINVAL;
-
-	filp->f_pos = newpos;
-
-	return newpos;
+	return generic_file_llseek_size(filp, off, whence, MAX_LFS_FILESIZE,
+					MBCS_SRAM_SIZE);
 }
 
 static uint64_t mbcs_pioaddr(struct mbcs_soft *soft, uint64_t offset)
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 6b1721f978c2..4f6f94c43412 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -689,7 +689,7 @@ static loff_t memory_lseek(struct file *file, loff_t offset, int orig)
 {
 	loff_t ret;
 
-	mutex_lock(&file_inode(file)->i_mutex);
+	inode_lock(file_inode(file));
 	switch (orig) {
 	case SEEK_CUR:
 		offset += file->f_pos;
@@ -706,7 +706,7 @@ static loff_t memory_lseek(struct file *file, loff_t offset, int orig)
 	default:
 		ret = -EINVAL;
 	}
-	mutex_unlock(&file_inode(file)->i_mutex);
+	inode_unlock(file_inode(file));
 	return ret;
 }
 
diff --git a/drivers/char/mspec.c b/drivers/char/mspec.c
index f1d7fa45c275..f3f92d5fcda0 100644
--- a/drivers/char/mspec.c
+++ b/drivers/char/mspec.c
@@ -93,14 +93,11 @@ struct vma_data {
 	spinlock_t lock;	/* Serialize access to this structure. */
 	int count;		/* Number of pages allocated. */
 	enum mspec_page_type type; /* Type of pages allocated. */
-	int flags;		/* See VMD_xxx below. */
 	unsigned long vm_start;	/* Original (unsplit) base. */
 	unsigned long vm_end;	/* Original (unsplit) end. */
 	unsigned long maddr[0];	/* Array of MSPEC addresses. */
 };
 
-#define VMD_VMALLOCED 0x1	/* vmalloc'd rather than kmalloc'd */
-
 /* used on shub2 to clear FOP cache in the HUB */
 static unsigned long scratch_page[MAX_NUMNODES];
 #define SH2_AMO_CACHE_ENTRIES	4
@@ -185,10 +182,7 @@ mspec_close(struct vm_area_struct *vma)
 			       "failed to zero page %ld\n", my_page);
 	}
 
-	if (vdata->flags & VMD_VMALLOCED)
-		vfree(vdata);
-	else
-		kfree(vdata);
+	kvfree(vdata);
 }
 
 /*
@@ -256,7 +250,7 @@ mspec_mmap(struct file *file, struct vm_area_struct *vma,
 					enum mspec_page_type type)
 {
 	struct vma_data *vdata;
-	int pages, vdata_size, flags = 0;
+	int pages, vdata_size;
 
 	if (vma->vm_pgoff != 0)
 		return -EINVAL;
@@ -271,16 +265,13 @@ mspec_mmap(struct file *file, struct vm_area_struct *vma,
 	vdata_size = sizeof(struct vma_data) + pages * sizeof(long);
 	if (vdata_size <= PAGE_SIZE)
 		vdata = kzalloc(vdata_size, GFP_KERNEL);
-	else {
+	else
 		vdata = vzalloc(vdata_size);
-		flags = VMD_VMALLOCED;
-	}
 	if (!vdata)
 		return -ENOMEM;
 
 	vdata->vm_start = vma->vm_start;
 	vdata->vm_end = vma->vm_end;
-	vdata->flags = flags;
 	vdata->type = type;
 	spin_lock_init(&vdata->lock);
 	atomic_set(&vdata->refcnt, 1);
diff --git a/drivers/char/nvram.c b/drivers/char/nvram.c
index 97c2d8d433d6..01292328a456 100644
--- a/drivers/char/nvram.c
+++ b/drivers/char/nvram.c
@@ -110,6 +110,7 @@
 #include <linux/io.h>
 #include <linux/uaccess.h>
 #include <linux/mutex.h>
+#include <linux/pagemap.h>
 
 
 static DEFINE_MUTEX(nvram_mutex);
@@ -213,21 +214,8 @@ void nvram_set_checksum(void)
 
 static loff_t nvram_llseek(struct file *file, loff_t offset, int origin)
 {
-	switch (origin) {
-	case 0:
-		/* nothing to do */
-		break;
-	case 1:
-		offset += file->f_pos;
-		break;
-	case 2:
-		offset += NVRAM_BYTES;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return (offset >= 0) ? (file->f_pos = offset) : -EINVAL;
+	return generic_file_llseek_size(file, offset, origin, MAX_LFS_FILESIZE,
+					NVRAM_BYTES);
 }
 
 static ssize_t nvram_read(struct file *file, char __user *buf,
diff --git a/drivers/char/nwflash.c b/drivers/char/nwflash.c
index e371480d3639..dbe598de9b74 100644
--- a/drivers/char/nwflash.c
+++ b/drivers/char/nwflash.c
@@ -277,36 +277,7 @@ static loff_t flash_llseek(struct file *file, loff_t offset, int orig)
 		printk(KERN_DEBUG "flash_llseek: offset=0x%X, orig=0x%X.\n",
 		       (unsigned int) offset, orig);
 
-	switch (orig) {
-	case 0:
-		if (offset < 0) {
-			ret = -EINVAL;
-			break;
-		}
-
-		if ((unsigned int) offset > gbFlashSize) {
-			ret = -EINVAL;
-			break;
-		}
-
-		file->f_pos = (unsigned int) offset;
-		ret = file->f_pos;
-		break;
-	case 1:
-		if ((file->f_pos + offset) > gbFlashSize) {
-			ret = -EINVAL;
-			break;
-		}
-		if ((file->f_pos + offset) < 0) {
-			ret = -EINVAL;
-			break;
-		}
-		file->f_pos += offset;
-		ret = file->f_pos;
-		break;
-	default:
-		ret = -EINVAL;
-	}
+	ret = no_seek_end_llseek_size(file, offset, orig, gbFlashSize);
 	mutex_unlock(&flash_mutex);
 	return ret;
 }
diff --git a/drivers/char/ps3flash.c b/drivers/char/ps3flash.c
index 0b311fa277ef..b526dc15c271 100644
--- a/drivers/char/ps3flash.c
+++ b/drivers/char/ps3flash.c
@@ -290,9 +290,9 @@ static int ps3flash_fsync(struct file *file, loff_t start, loff_t end, int datas
 {
 	struct inode *inode = file_inode(file);
 	int err;
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	err = ps3flash_writeback(ps3flash_dev);
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 	return err;
 }
 
diff --git a/drivers/char/random.c b/drivers/char/random.c
index d0da5d852d41..b583e5336630 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -1819,6 +1819,28 @@ unsigned int get_random_int(void)
 EXPORT_SYMBOL(get_random_int);
 
 /*
+ * Same as get_random_int(), but returns unsigned long.
+ */
+unsigned long get_random_long(void)
+{
+	__u32 *hash;
+	unsigned long ret;
+
+	if (arch_get_random_long(&ret))
+		return ret;
+
+	hash = get_cpu_var(get_random_int_hash);
+
+	hash[0] += current->pid + jiffies + random_get_entropy();
+	md5_transform(hash, random_int_secret);
+	ret = *(unsigned long *)hash;
+	put_cpu_var(get_random_int_hash);
+
+	return ret;
+}
+EXPORT_SYMBOL(get_random_long);
+
+/*
  * randomize_range() returns a start address such that
  *
  *    [...... <range> .....]
diff --git a/drivers/char/raw.c b/drivers/char/raw.c
index 60316fbaf295..9b9809b709a5 100644
--- a/drivers/char/raw.c
+++ b/drivers/char/raw.c
@@ -71,7 +71,7 @@ static int raw_open(struct inode *inode, struct file *filp)
 	err = -ENODEV;
 	if (!bdev)
 		goto out;
-	igrab(bdev->bd_inode);
+	bdgrab(bdev);
 	err = blkdev_get(bdev, filp->f_mode | FMODE_EXCL, raw_open);
 	if (err)
 		goto out;
diff --git a/drivers/char/tpm/tpm-interface.c b/drivers/char/tpm/tpm-interface.c
index c50637db3a8a..e2fa89c88304 100644
--- a/drivers/char/tpm/tpm-interface.c
+++ b/drivers/char/tpm/tpm-interface.c
@@ -310,10 +310,12 @@ unsigned long tpm_calc_ordinal_duration(struct tpm_chip *chip,
 {
 	int duration_idx = TPM_UNDEFINED;
 	int duration = 0;
-	u8 category = (ordinal >> 24) & 0xFF;
 
-	if ((category == TPM_PROTECTED_COMMAND && ordinal < TPM_MAX_ORDINAL) ||
-	    (category == TPM_CONNECTION_COMMAND && ordinal < TSC_MAX_ORDINAL))
+	/*
+	 * We only have a duration table for protected commands, where the upper
+	 * 16 bits are 0. For the few other ordinals the fallback will be used.
+	 */
+	if (ordinal < TPM_MAX_ORDINAL)
 		duration_idx = tpm_ordinal_duration[ordinal];
 
 	if (duration_idx != TPM_UNDEFINED)
@@ -501,6 +503,21 @@ int tpm_get_timeouts(struct tpm_chip *chip)
 	struct duration_t *duration_cap;
 	ssize_t rc;
 
+	if (chip->flags & TPM_CHIP_FLAG_TPM2) {
+		/* Fixed timeouts for TPM2 */
+		chip->vendor.timeout_a = msecs_to_jiffies(TPM2_TIMEOUT_A);
+		chip->vendor.timeout_b = msecs_to_jiffies(TPM2_TIMEOUT_B);
+		chip->vendor.timeout_c = msecs_to_jiffies(TPM2_TIMEOUT_C);
+		chip->vendor.timeout_d = msecs_to_jiffies(TPM2_TIMEOUT_D);
+		chip->vendor.duration[TPM_SHORT] =
+		    msecs_to_jiffies(TPM2_DURATION_SHORT);
+		chip->vendor.duration[TPM_MEDIUM] =
+		    msecs_to_jiffies(TPM2_DURATION_MEDIUM);
+		chip->vendor.duration[TPM_LONG] =
+		    msecs_to_jiffies(TPM2_DURATION_LONG);
+		return 0;
+	}
+
 	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);
diff --git a/drivers/char/tpm/tpm.h b/drivers/char/tpm/tpm.h
index a4257a32964f..542a80cbfd9c 100644
--- a/drivers/char/tpm/tpm.h
+++ b/drivers/char/tpm/tpm.h
@@ -83,16 +83,20 @@ enum tpm2_structures {
 };
 
 enum tpm2_return_codes {
-	TPM2_RC_INITIALIZE	= 0x0100,
-	TPM2_RC_TESTING		= 0x090A,
+	TPM2_RC_HASH		= 0x0083, /* RC_FMT1 */
+	TPM2_RC_INITIALIZE	= 0x0100, /* RC_VER1 */
 	TPM2_RC_DISABLED	= 0x0120,
+	TPM2_RC_TESTING		= 0x090A, /* RC_WARN */
 };
 
 enum tpm2_algorithms {
 	TPM2_ALG_SHA1		= 0x0004,
 	TPM2_ALG_KEYEDHASH	= 0x0008,
 	TPM2_ALG_SHA256		= 0x000B,
-	TPM2_ALG_NULL		= 0x0010
+	TPM2_ALG_SHA384		= 0x000C,
+	TPM2_ALG_SHA512		= 0x000D,
+	TPM2_ALG_NULL		= 0x0010,
+	TPM2_ALG_SM3_256	= 0x0012,
 };
 
 enum tpm2_command_codes {
@@ -138,7 +142,6 @@ struct tpm_vendor_specific {
 	unsigned long base;		/* TPM base address */
 
 	int irq;
-	int probed_irq;
 
 	int region_size;
 	int have_region;
diff --git a/drivers/char/tpm/tpm2-cmd.c b/drivers/char/tpm/tpm2-cmd.c
index c12130485fc1..45a634016f95 100644
--- a/drivers/char/tpm/tpm2-cmd.c
+++ b/drivers/char/tpm/tpm2-cmd.c
@@ -16,6 +16,7 @@
  */
 
 #include "tpm.h"
+#include <crypto/hash_info.h>
 #include <keys/trusted-type.h>
 
 enum tpm2_object_attributes {
@@ -104,6 +105,19 @@ struct tpm2_cmd {
 	union tpm2_cmd_params	params;
 } __packed;
 
+struct tpm2_hash {
+	unsigned int crypto_id;
+	unsigned int tpm_id;
+};
+
+static struct tpm2_hash tpm2_hash_map[] = {
+	{HASH_ALGO_SHA1, TPM2_ALG_SHA1},
+	{HASH_ALGO_SHA256, TPM2_ALG_SHA256},
+	{HASH_ALGO_SHA384, TPM2_ALG_SHA384},
+	{HASH_ALGO_SHA512, TPM2_ALG_SHA512},
+	{HASH_ALGO_SM3_256, TPM2_ALG_SM3_256},
+};
+
 /*
  * Array with one entry per ordinal defining the maximum amount
  * of time the chip could take to return the result. The values
@@ -429,8 +443,20 @@ int tpm2_seal_trusted(struct tpm_chip *chip,
 {
 	unsigned int blob_len;
 	struct tpm_buf buf;
+	u32 hash;
+	int i;
 	int rc;
 
+	for (i = 0; i < ARRAY_SIZE(tpm2_hash_map); i++) {
+		if (options->hash == tpm2_hash_map[i].crypto_id) {
+			hash = tpm2_hash_map[i].tpm_id;
+			break;
+		}
+	}
+
+	if (i == ARRAY_SIZE(tpm2_hash_map))
+		return -EINVAL;
+
 	rc = tpm_buf_init(&buf, TPM2_ST_SESSIONS, TPM2_CC_CREATE);
 	if (rc)
 		return rc;
@@ -452,12 +478,26 @@ int tpm2_seal_trusted(struct tpm_chip *chip,
 	tpm_buf_append_u8(&buf, payload->migratable);
 
 	/* public */
-	tpm_buf_append_u16(&buf, 14);
+	if (options->policydigest)
+		tpm_buf_append_u16(&buf, 14 + options->digest_len);
+	else
+		tpm_buf_append_u16(&buf, 14);
 
 	tpm_buf_append_u16(&buf, TPM2_ALG_KEYEDHASH);
-	tpm_buf_append_u16(&buf, TPM2_ALG_SHA256);
-	tpm_buf_append_u32(&buf, TPM2_ATTR_USER_WITH_AUTH);
-	tpm_buf_append_u16(&buf, 0); /* policy digest size */
+	tpm_buf_append_u16(&buf, hash);
+
+	/* policy */
+	if (options->policydigest) {
+		tpm_buf_append_u32(&buf, 0);
+		tpm_buf_append_u16(&buf, options->digest_len);
+		tpm_buf_append(&buf, options->policydigest,
+			       options->digest_len);
+	} else {
+		tpm_buf_append_u32(&buf, TPM2_ATTR_USER_WITH_AUTH);
+		tpm_buf_append_u16(&buf, 0);
+	}
+
+	/* public parameters */
 	tpm_buf_append_u16(&buf, TPM2_ALG_NULL);
 	tpm_buf_append_u16(&buf, 0);
 
@@ -488,8 +528,12 @@ int tpm2_seal_trusted(struct tpm_chip *chip,
 out:
 	tpm_buf_destroy(&buf);
 
-	if (rc > 0)
-		rc = -EPERM;
+	if (rc > 0) {
+		if ((rc & TPM2_RC_HASH) == TPM2_RC_HASH)
+			rc = -EINVAL;
+		else
+			rc = -EPERM;
+	}
 
 	return rc;
 }
@@ -583,7 +627,9 @@ static int tpm2_unseal(struct tpm_chip *chip,
 		return rc;
 
 	tpm_buf_append_u32(&buf, blob_handle);
-	tpm2_buf_append_auth(&buf, TPM2_RS_PW,
+	tpm2_buf_append_auth(&buf,
+			     options->policyhandle ?
+			     options->policyhandle : TPM2_RS_PW,
 			     NULL /* nonce */, 0,
 			     0 /* session_attributes */,
 			     options->blobauth /* hmac */,
diff --git a/drivers/char/tpm/tpm_crb.c b/drivers/char/tpm/tpm_crb.c
index 4bb9727c1047..8342cf51ffdc 100644
--- a/drivers/char/tpm/tpm_crb.c
+++ b/drivers/char/tpm/tpm_crb.c
@@ -284,17 +284,9 @@ static int crb_acpi_add(struct acpi_device *device)
 
 	chip->vendor.priv = priv;
 
-	/* Default timeouts and durations */
-	chip->vendor.timeout_a = msecs_to_jiffies(TPM2_TIMEOUT_A);
-	chip->vendor.timeout_b = msecs_to_jiffies(TPM2_TIMEOUT_B);
-	chip->vendor.timeout_c = msecs_to_jiffies(TPM2_TIMEOUT_C);
-	chip->vendor.timeout_d = msecs_to_jiffies(TPM2_TIMEOUT_D);
-	chip->vendor.duration[TPM_SHORT] =
-		msecs_to_jiffies(TPM2_DURATION_SHORT);
-	chip->vendor.duration[TPM_MEDIUM] =
-		msecs_to_jiffies(TPM2_DURATION_MEDIUM);
-	chip->vendor.duration[TPM_LONG] =
-		msecs_to_jiffies(TPM2_DURATION_LONG);
+	rc = tpm_get_timeouts(chip);
+	if (rc)
+		return rc;
 
 	chip->acpi_dev_handle = device->handle;
 
diff --git a/drivers/char/tpm/tpm_ibmvtpm.c b/drivers/char/tpm/tpm_ibmvtpm.c
index 3e6a22658b63..b0a9a9e34241 100644
--- a/drivers/char/tpm/tpm_ibmvtpm.c
+++ b/drivers/char/tpm/tpm_ibmvtpm.c
@@ -90,7 +90,7 @@ static int tpm_ibmvtpm_recv(struct tpm_chip *chip, u8 *buf, size_t count)
 		return 0;
 	}
 
-	sig = wait_event_interruptible(ibmvtpm->wq, ibmvtpm->res_len != 0);
+	sig = wait_event_interruptible(ibmvtpm->wq, !ibmvtpm->tpm_processing_cmd);
 	if (sig)
 		return -EINTR;
 
@@ -125,7 +125,7 @@ static int tpm_ibmvtpm_send(struct tpm_chip *chip, u8 *buf, size_t count)
 	struct ibmvtpm_dev *ibmvtpm;
 	struct ibmvtpm_crq crq;
 	__be64 *word = (__be64 *)&crq;
-	int rc;
+	int rc, sig;
 
 	ibmvtpm = (struct ibmvtpm_dev *)TPM_VPRIV(chip);
 
@@ -141,18 +141,35 @@ static int tpm_ibmvtpm_send(struct tpm_chip *chip, u8 *buf, size_t count)
 		return -EIO;
 	}
 
+	if (ibmvtpm->tpm_processing_cmd) {
+		dev_info(ibmvtpm->dev,
+		         "Need to wait for TPM to finish\n");
+		/* wait for previous command to finish */
+		sig = wait_event_interruptible(ibmvtpm->wq, !ibmvtpm->tpm_processing_cmd);
+		if (sig)
+			return -EINTR;
+	}
+
 	spin_lock(&ibmvtpm->rtce_lock);
+	ibmvtpm->res_len = 0;
 	memcpy((void *)ibmvtpm->rtce_buf, (void *)buf, count);
 	crq.valid = (u8)IBMVTPM_VALID_CMD;
 	crq.msg = (u8)VTPM_TPM_COMMAND;
 	crq.len = cpu_to_be16(count);
 	crq.data = cpu_to_be32(ibmvtpm->rtce_dma_handle);
 
+	/*
+	 * set the processing flag before the Hcall, since we may get the
+	 * result (interrupt) before even being able to check rc.
+	 */
+	ibmvtpm->tpm_processing_cmd = true;
+
 	rc = ibmvtpm_send_crq(ibmvtpm->vdev, be64_to_cpu(word[0]),
 			      be64_to_cpu(word[1]));
 	if (rc != H_SUCCESS) {
 		dev_err(ibmvtpm->dev, "tpm_ibmvtpm_send failed rc=%d\n", rc);
 		rc = 0;
+		ibmvtpm->tpm_processing_cmd = false;
 	} else
 		rc = count;
 
@@ -515,6 +532,7 @@ static void ibmvtpm_crq_process(struct ibmvtpm_crq *crq,
 		case VTPM_TPM_COMMAND_RES:
 			/* len of the data in rtce buffer */
 			ibmvtpm->res_len = be16_to_cpu(crq->len);
+			ibmvtpm->tpm_processing_cmd = false;
 			wake_up_interruptible(&ibmvtpm->wq);
 			return;
 		default:
diff --git a/drivers/char/tpm/tpm_ibmvtpm.h b/drivers/char/tpm/tpm_ibmvtpm.h
index 6af92890518f..91dfe766d080 100644
--- a/drivers/char/tpm/tpm_ibmvtpm.h
+++ b/drivers/char/tpm/tpm_ibmvtpm.h
@@ -45,6 +45,7 @@ struct ibmvtpm_dev {
 	wait_queue_head_t wq;
 	u16 res_len;
 	u32 vtpm_version;
+	bool tpm_processing_cmd;
 };
 
 #define CRQ_RES_BUF_SIZE	PAGE_SIZE
diff --git a/drivers/char/tpm/tpm_tis.c b/drivers/char/tpm/tpm_tis.c
index 65f7eecc45b0..8a3509cb10da 100644
--- a/drivers/char/tpm/tpm_tis.c
+++ b/drivers/char/tpm/tpm_tis.c
@@ -401,7 +401,7 @@ static void disable_interrupts(struct tpm_chip *chip)
 	iowrite32(intmask,
 		  chip->vendor.iobase +
 		  TPM_INT_ENABLE(chip->vendor.locality));
-	free_irq(chip->vendor.irq, chip);
+	devm_free_irq(chip->pdev, chip->vendor.irq, chip);
 	chip->vendor.irq = 0;
 }
 
@@ -461,11 +461,8 @@ static int tpm_tis_send(struct tpm_chip *chip, u8 *buf, size_t len)
 	chip->vendor.irq = irq;
 	if (!priv->irq_tested)
 		msleep(1);
-	if (!priv->irq_tested) {
+	if (!priv->irq_tested)
 		disable_interrupts(chip);
-		dev_err(chip->pdev,
-			FW_BUG "TPM interrupt not working, polling instead\n");
-	}
 	priv->irq_tested = true;
 	return rc;
 }
@@ -570,26 +567,6 @@ static const struct tpm_class_ops tpm_tis = {
 	.req_canceled = tpm_tis_req_canceled,
 };
 
-static irqreturn_t tis_int_probe(int irq, void *dev_id)
-{
-	struct tpm_chip *chip = dev_id;
-	u32 interrupt;
-
-	interrupt = ioread32(chip->vendor.iobase +
-			     TPM_INT_STATUS(chip->vendor.locality));
-
-	if (interrupt == 0)
-		return IRQ_NONE;
-
-	chip->vendor.probed_irq = irq;
-
-	/* Clear interrupts handled with TPM_EOI */
-	iowrite32(interrupt,
-		  chip->vendor.iobase +
-		  TPM_INT_STATUS(chip->vendor.locality));
-	return IRQ_HANDLED;
-}
-
 static irqreturn_t tis_int_handler(int dummy, void *dev_id)
 {
 	struct tpm_chip *chip = dev_id;
@@ -622,6 +599,84 @@ static irqreturn_t tis_int_handler(int dummy, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+/* 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.
+ */
+static int tpm_tis_probe_irq_single(struct tpm_chip *chip, u32 intmask,
+				    int flags, int irq)
+{
+	struct priv_data *priv = chip->vendor.priv;
+	u8 original_int_vec;
+
+	if (devm_request_irq(chip->pdev, irq, tis_int_handler, flags,
+			     chip->devname, chip) != 0) {
+		dev_info(chip->pdev, "Unable to request irq: %d for probe\n",
+			 irq);
+		return -1;
+	}
+	chip->vendor.irq = irq;
+
+	original_int_vec = ioread8(chip->vendor.iobase +
+				   TPM_INT_VECTOR(chip->vendor.locality));
+	iowrite8(irq,
+		 chip->vendor.iobase + TPM_INT_VECTOR(chip->vendor.locality));
+
+	/* Clear all existing */
+	iowrite32(ioread32(chip->vendor.iobase +
+			   TPM_INT_STATUS(chip->vendor.locality)),
+		  chip->vendor.iobase + TPM_INT_STATUS(chip->vendor.locality));
+
+	/* Turn on */
+	iowrite32(intmask | TPM_GLOBAL_INT_ENABLE,
+		  chip->vendor.iobase + TPM_INT_ENABLE(chip->vendor.locality));
+
+	priv->irq_tested = false;
+
+	/* 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);
+
+	/* tpm_tis_send will either confirm the interrupt is working or it
+	 * will call disable_irq which undoes all of the above.
+	 */
+	if (!chip->vendor.irq) {
+		iowrite8(original_int_vec,
+			 chip->vendor.iobase +
+			     TPM_INT_VECTOR(chip->vendor.locality));
+		return 1;
+	}
+
+	return 0;
+}
+
+/* Try to find the IRQ the TPM is using. This is for legacy x86 systems that
+ * do not have ACPI/etc. We typically expect the interrupt to be declared if
+ * present.
+ */
+static void tpm_tis_probe_irq(struct tpm_chip *chip, u32 intmask)
+{
+	u8 original_int_vec;
+	int i;
+
+	original_int_vec = ioread8(chip->vendor.iobase +
+				   TPM_INT_VECTOR(chip->vendor.locality));
+
+	if (!original_int_vec) {
+		if (IS_ENABLED(CONFIG_X86))
+			for (i = 3; i <= 15; i++)
+				if (!tpm_tis_probe_irq_single(chip, intmask, 0,
+							      i))
+					return;
+	} else if (!tpm_tis_probe_irq_single(chip, intmask, 0,
+					     original_int_vec))
+		return;
+}
+
 static bool interrupts = true;
 module_param(interrupts, bool, 0444);
 MODULE_PARM_DESC(interrupts, "Enable interrupts");
@@ -644,8 +699,7 @@ static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
 			acpi_handle acpi_dev_handle)
 {
 	u32 vendor, intfcaps, intmask;
-	int rc, i, irq_s, irq_e, probe;
-	int irq_r = -1;
+	int rc, probe;
 	struct tpm_chip *chip;
 	struct priv_data *priv;
 
@@ -677,6 +731,15 @@ static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
 		goto out_err;
 	}
 
+	/* Take control of the TPM's interrupt hardware and shut it off */
+	intmask = ioread32(chip->vendor.iobase +
+			   TPM_INT_ENABLE(chip->vendor.locality));
+	intmask |= TPM_INTF_CMD_READY_INT | TPM_INTF_LOCALITY_CHANGE_INT |
+		   TPM_INTF_DATA_AVAIL_INT | TPM_INTF_STS_VALID_INT;
+	intmask &= ~TPM_GLOBAL_INT_ENABLE;
+	iowrite32(intmask,
+		  chip->vendor.iobase + TPM_INT_ENABLE(chip->vendor.locality));
+
 	if (request_locality(chip, 0) != 0) {
 		rc = -ENODEV;
 		goto out_err;
@@ -731,126 +794,31 @@ static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
 	if (intfcaps & TPM_INTF_DATA_AVAIL_INT)
 		dev_dbg(dev, "\tData Avail Int Support\n");
 
+	/* Very early on issue a command to the TPM in polling mode to make
+	 * sure it works. May as well use that command to set the proper
+	 *  timeouts for the driver.
+	 */
+	if (tpm_get_timeouts(chip)) {
+		dev_err(dev, "Could not get TPM timeouts and durations\n");
+		rc = -ENODEV;
+		goto out_err;
+	}
+
 	/* INTERRUPT Setup */
 	init_waitqueue_head(&chip->vendor.read_queue);
 	init_waitqueue_head(&chip->vendor.int_queue);
-
-	intmask =
-	    ioread32(chip->vendor.iobase +
-		     TPM_INT_ENABLE(chip->vendor.locality));
-
-	intmask |= TPM_INTF_CMD_READY_INT
-	    | TPM_INTF_LOCALITY_CHANGE_INT | TPM_INTF_DATA_AVAIL_INT
-	    | TPM_INTF_STS_VALID_INT;
-
-	iowrite32(intmask,
-		  chip->vendor.iobase +
-		  TPM_INT_ENABLE(chip->vendor.locality));
-	if (interrupts)
-		chip->vendor.irq = tpm_info->irq;
-	if (interrupts && !chip->vendor.irq) {
-		irq_s =
-		    ioread8(chip->vendor.iobase +
-			    TPM_INT_VECTOR(chip->vendor.locality));
-		irq_r = irq_s;
-		if (irq_s) {
-			irq_e = irq_s;
-		} else {
-			irq_s = 3;
-			irq_e = 15;
-		}
-
-		for (i = irq_s; i <= irq_e && chip->vendor.irq == 0; i++) {
-			iowrite8(i, chip->vendor.iobase +
-				 TPM_INT_VECTOR(chip->vendor.locality));
-			if (devm_request_irq
-			    (dev, i, tis_int_probe, IRQF_SHARED,
-			     chip->devname, chip) != 0) {
-				dev_info(chip->pdev,
-					 "Unable to request irq: %d for probe\n",
-					 i);
-				continue;
-			}
-
-			/* Clear all existing */
-			iowrite32(ioread32
-				  (chip->vendor.iobase +
-				   TPM_INT_STATUS(chip->vendor.locality)),
-				  chip->vendor.iobase +
-				  TPM_INT_STATUS(chip->vendor.locality));
-
-			/* Turn on */
-			iowrite32(intmask | TPM_GLOBAL_INT_ENABLE,
-				  chip->vendor.iobase +
-				  TPM_INT_ENABLE(chip->vendor.locality));
-
-			chip->vendor.probed_irq = 0;
-
-			/* Generate Interrupts */
-			if (chip->flags & TPM_CHIP_FLAG_TPM2)
-				tpm2_gen_interrupt(chip);
-			else
-				tpm_gen_interrupt(chip);
-
-			chip->vendor.irq = chip->vendor.probed_irq;
-
-			/* free_irq will call into tis_int_probe;
-			   clear all irqs we haven't seen while doing
-			   tpm_gen_interrupt */
-			iowrite32(ioread32
-				  (chip->vendor.iobase +
-				   TPM_INT_STATUS(chip->vendor.locality)),
-				  chip->vendor.iobase +
-				  TPM_INT_STATUS(chip->vendor.locality));
-
-			/* Turn off */
-			iowrite32(intmask,
-				  chip->vendor.iobase +
-				  TPM_INT_ENABLE(chip->vendor.locality));
-
-			devm_free_irq(dev, i, chip);
-		}
+	if (interrupts) {
+		if (tpm_info->irq) {
+			tpm_tis_probe_irq_single(chip, intmask, IRQF_SHARED,
+						 tpm_info->irq);
+			if (!chip->vendor.irq)
+				dev_err(chip->pdev, FW_BUG
+					"TPM interrupt not working, polling instead\n");
+		} else
+			tpm_tis_probe_irq(chip, intmask);
 	}
-	if (chip->vendor.irq) {
-		iowrite8(chip->vendor.irq,
-			 chip->vendor.iobase +
-			 TPM_INT_VECTOR(chip->vendor.locality));
-		if (devm_request_irq
-		    (dev, chip->vendor.irq, tis_int_handler, IRQF_SHARED,
-		     chip->devname, chip) != 0) {
-			dev_info(chip->pdev,
-				 "Unable to request irq: %d for use\n",
-				 chip->vendor.irq);
-			chip->vendor.irq = 0;
-		} else {
-			/* Clear all existing */
-			iowrite32(ioread32
-				  (chip->vendor.iobase +
-				   TPM_INT_STATUS(chip->vendor.locality)),
-				  chip->vendor.iobase +
-				  TPM_INT_STATUS(chip->vendor.locality));
-
-			/* Turn on */
-			iowrite32(intmask | TPM_GLOBAL_INT_ENABLE,
-				  chip->vendor.iobase +
-				  TPM_INT_ENABLE(chip->vendor.locality));
-		}
-	} else if (irq_r != -1)
-		iowrite8(irq_r, chip->vendor.iobase +
-			 TPM_INT_VECTOR(chip->vendor.locality));
 
 	if (chip->flags & TPM_CHIP_FLAG_TPM2) {
-		chip->vendor.timeout_a = msecs_to_jiffies(TPM2_TIMEOUT_A);
-		chip->vendor.timeout_b = msecs_to_jiffies(TPM2_TIMEOUT_B);
-		chip->vendor.timeout_c = msecs_to_jiffies(TPM2_TIMEOUT_C);
-		chip->vendor.timeout_d = msecs_to_jiffies(TPM2_TIMEOUT_D);
-		chip->vendor.duration[TPM_SHORT] =
-			msecs_to_jiffies(TPM2_DURATION_SHORT);
-		chip->vendor.duration[TPM_MEDIUM] =
-			msecs_to_jiffies(TPM2_DURATION_MEDIUM);
-		chip->vendor.duration[TPM_LONG] =
-			msecs_to_jiffies(TPM2_DURATION_LONG);
-
 		rc = tpm2_do_selftest(chip);
 		if (rc == TPM2_RC_INITIALIZE) {
 			dev_warn(dev, "Firmware has not started TPM\n");
@@ -866,12 +834,6 @@ static int tpm_tis_init(struct device *dev, struct tpm_info *tpm_info,
 			goto out_err;
 		}
 	} else {
-		if (tpm_get_timeouts(chip)) {
-			dev_err(dev, "Could not get TPM timeouts and durations\n");
-			rc = -ENODEV;
-			goto out_err;
-		}
-
 		if (tpm_do_selftest(chip)) {
 			dev_err(dev, "TPM self test failed\n");
 			rc = -ENODEV;
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index c3e3a02f7f1f..eca8e019e005 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -116,6 +116,12 @@ config COMMON_CLK_CDCE925
 	  Given a target output frequency, the driver will set the PLL and
 	  divider to best approximate the desired output.
 
+config COMMON_CLK_CS2000_CP
+	tristate "Clock driver for CS2000 Fractional-N Clock Synthesizer & Clock Multiplier"
+	depends on I2C
+	help
+	  If you say yes here you get support for the CS2000 clock multiplier.
+
 config COMMON_CLK_S2MPS11
 	tristate "Clock driver for S2MPS1X/S5M8767 MFD"
 	depends on MFD_SEC_CORE
@@ -161,6 +167,12 @@ config COMMON_CLK_KEYSTONE
           Supports clock drivers for Keystone based SOCs. These SOCs have local
 	  a power sleep control module that gate the clock to the IPs and PLLs.
 
+config COMMON_CLK_NXP
+	def_bool COMMON_CLK && (ARCH_LPC18XX || ARCH_LPC32XX)
+	select REGMAP_MMIO if ARCH_LPC32XX
+	---help---
+	  Support for clock providers on NXP platforms.
+
 config COMMON_CLK_PALMAS
 	tristate "Clock driver for TI Palmas devices"
 	depends on MFD_PALMAS
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 820714c72d36..bae4be6501df 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_MACH_ASM9260)		+= clk-asm9260.o
 obj-$(CONFIG_COMMON_CLK_AXI_CLKGEN)	+= clk-axi-clkgen.o
 obj-$(CONFIG_ARCH_AXXIA)		+= clk-axm5516.o
 obj-$(CONFIG_COMMON_CLK_CDCE706)	+= clk-cdce706.o
+obj-$(CONFIG_COMMON_CLK_CS2000_CP)	+= clk-cs2000-cp.o
 obj-$(CONFIG_ARCH_CLPS711X)		+= clk-clps711x.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.o
@@ -42,6 +43,7 @@ obj-$(CONFIG_COMMON_CLK_SI514)		+= clk-si514.o
 obj-$(CONFIG_COMMON_CLK_SI570)		+= clk-si570.o
 obj-$(CONFIG_COMMON_CLK_CDCE925)	+= clk-cdce925.o
 obj-$(CONFIG_ARCH_STM32)		+= clk-stm32f4.o
+obj-$(CONFIG_ARCH_TANGO)		+= clk-tango4.o
 obj-$(CONFIG_CLK_TWL6040)		+= clk-twl6040.o
 obj-$(CONFIG_ARCH_U300)			+= clk-u300.o
 obj-$(CONFIG_ARCH_VT8500)		+= clk-vt8500.o
@@ -62,13 +64,14 @@ endif
 obj-$(CONFIG_PLAT_ORION)		+= mvebu/
 obj-$(CONFIG_ARCH_MESON)		+= meson/
 obj-$(CONFIG_ARCH_MXS)			+= mxs/
-obj-$(CONFIG_ARCH_LPC18XX)		+= nxp/
 obj-$(CONFIG_MACH_PISTACHIO)		+= pistachio/
+obj-$(CONFIG_COMMON_CLK_NXP)		+= nxp/
 obj-$(CONFIG_COMMON_CLK_PXA)		+= pxa/
 obj-$(CONFIG_COMMON_CLK_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
 obj-$(CONFIG_COMMON_CLK_SAMSUNG)	+= samsung/
 obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= shmobile/
+obj-$(CONFIG_ARCH_RENESAS)		+= shmobile/
 obj-$(CONFIG_ARCH_SIRF)			+= sirf/
 obj-$(CONFIG_ARCH_SOCFPGA)		+= socfpga/
 obj-$(CONFIG_PLAT_SPEAR)		+= spear/
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
index d0d5076a9b94..6f99a530ead6 100644
--- a/drivers/clk/at91/clk-slow.c
+++ b/drivers/clk/at91/clk-slow.c
@@ -10,7 +10,6 @@
  *
  */
 
-#include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
 #include <linux/slab.h>
@@ -72,8 +71,6 @@ struct clk_sam9x5_slow {
 
 #define to_clk_sam9x5_slow(hw) container_of(hw, struct clk_sam9x5_slow, hw)
 
-static struct clk *slow_clk;
-
 static int clk_slow_osc_prepare(struct clk_hw *hw)
 {
 	struct clk_slow_osc *osc = to_clk_slow_osc(hw);
@@ -360,8 +357,6 @@ at91_clk_register_sam9x5_slow(void __iomem *sckcr,
 	clk = clk_register(NULL, &slowck->hw);
 	if (IS_ERR(clk))
 		kfree(slowck);
-	else
-		slow_clk = clk;
 
 	return clk;
 }
@@ -433,8 +428,6 @@ at91_clk_register_sam9260_slow(struct at91_pmc *pmc,
 	clk = clk_register(NULL, &slowck->hw);
 	if (IS_ERR(clk))
 		kfree(slowck);
-	else
-		slow_clk = clk;
 
 	return clk;
 }
@@ -462,25 +455,3 @@ void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 }
-
-/*
- * FIXME: All slow clk users are not properly claiming it (get + prepare +
- * enable) before using it.
- * If all users properly claiming this clock decide that they don't need it
- * anymore (or are removed), it is disabled while faulty users are still
- * requiring it, and the system hangs.
- * Prevent this clock from being disabled until all users are properly
- * requesting it.
- * Once this is done we should remove this function and the slow_clk variable.
- */
-static int __init of_at91_clk_slow_retain(void)
-{
-	if (!slow_clk)
-		return 0;
-
-	__clk_get(slow_clk);
-	clk_prepare_enable(slow_clk);
-
-	return 0;
-}
-arch_initcall(of_at91_clk_slow_retain);
diff --git a/drivers/clk/bcm/Kconfig b/drivers/clk/bcm/Kconfig
index 85260fb96b36..f2878459199a 100644
--- a/drivers/clk/bcm/Kconfig
+++ b/drivers/clk/bcm/Kconfig
@@ -1,3 +1,13 @@
+config CLK_BCM_63XX
+	bool "Broadcom BCM63xx clock support"
+	depends on ARCH_BCM_63XX || COMPILE_TEST
+	depends on COMMON_CLK
+	select COMMON_CLK_IPROC
+	default ARCH_BCM_63XX
+	help
+	  Enable common clock framework support for Broadcom BCM63xx DSL SoCs
+	  based on the ARM architecture
+
 config CLK_BCM_KONA
 	bool "Broadcom Kona CCU clock support"
 	depends on ARCH_BCM_MOBILE || COMPILE_TEST
diff --git a/drivers/clk/bcm/Makefile b/drivers/clk/bcm/Makefile
index 3fc95060d875..1d79bd2c36f0 100644
--- a/drivers/clk/bcm/Makefile
+++ b/drivers/clk/bcm/Makefile
@@ -1,9 +1,11 @@
+obj-$(CONFIG_CLK_BCM_63XX)	+= clk-bcm63xx.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-kona-setup.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm281xx.o
 obj-$(CONFIG_CLK_BCM_KONA)	+= clk-bcm21664.o
 obj-$(CONFIG_COMMON_CLK_IPROC)	+= clk-iproc-armpll.o clk-iproc-pll.o clk-iproc-asiu.o
 obj-$(CONFIG_ARCH_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
diff --git a/drivers/clk/bcm/clk-bcm2835-aux.c b/drivers/clk/bcm/clk-bcm2835-aux.c
new file mode 100644
index 000000000000..e4f89e28b5ec
--- /dev/null
+++ b/drivers/clk/bcm/clk-bcm2835-aux.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2015 Broadcom
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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/clk-provider.h>
+#include <linux/clk/bcm2835.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <dt-bindings/clock/bcm2835-aux.h>
+
+#define BCM2835_AUXIRQ		0x00
+#define BCM2835_AUXENB		0x04
+
+static int bcm2835_aux_clk_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct clk_onecell_data *onecell;
+	const char *parent;
+	struct clk *parent_clk;
+	struct resource *res;
+	void __iomem *reg, *gate;
+
+	parent_clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(parent_clk))
+		return PTR_ERR(parent_clk);
+	parent = __clk_get_name(parent_clk);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	reg = devm_ioremap_resource(dev, res);
+	if (!reg)
+		return -ENODEV;
+
+	onecell = devm_kmalloc(dev, sizeof(*onecell), 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;
+
+	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->clks[BCM2835_AUX_CLOCK_SPI2] =
+		clk_register_gate(dev, "aux_spi2", parent, 0, gate, 2, 0, NULL);
+
+	of_clk_add_provider(pdev->dev.of_node, of_clk_src_onecell_get, onecell);
+
+	return 0;
+}
+
+static const struct of_device_id bcm2835_aux_clk_of_match[] = {
+	{ .compatible = "brcm,bcm2835-aux", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcm2835_aux_clk_of_match);
+
+static struct platform_driver bcm2835_aux_clk_driver = {
+	.driver = {
+		.name = "bcm2835-aux-clk",
+		.of_match_table = bcm2835_aux_clk_of_match,
+	},
+	.probe          = bcm2835_aux_clk_probe,
+};
+builtin_platform_driver(bcm2835_aux_clk_driver);
+
+MODULE_AUTHOR("Eric Anholt <eric@anholt.net>");
+MODULE_DESCRIPTION("BCM2835 auxiliary peripheral clock driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clk/bcm/clk-bcm2835.c b/drivers/clk/bcm/clk-bcm2835.c
index 39bf5820297e..015e687ffabe 100644
--- a/drivers/clk/bcm/clk-bcm2835.c
+++ b/drivers/clk/bcm/clk-bcm2835.c
@@ -807,6 +807,16 @@ static const struct bcm2835_clock_data bcm2835_clock_emmc_data = {
 	.frac_bits = 8,
 };
 
+static const struct bcm2835_clock_data bcm2835_clock_pwm_data = {
+	.name = "pwm",
+	.num_mux_parents = ARRAY_SIZE(bcm2835_clock_per_parents),
+	.parents = bcm2835_clock_per_parents,
+	.ctl_reg = CM_PWMCTL,
+	.div_reg = CM_PWMDIV,
+	.int_bits = 12,
+	.frac_bits = 12,
+};
+
 struct bcm2835_pll {
 	struct clk_hw hw;
 	struct bcm2835_cprman *cprman;
@@ -1148,22 +1158,24 @@ static int bcm2835_clock_is_on(struct clk_hw *hw)
 
 static u32 bcm2835_clock_choose_div(struct clk_hw *hw,
 				    unsigned long rate,
-				    unsigned long parent_rate)
+				    unsigned long parent_rate,
+				    bool round_up)
 {
 	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
 	const struct bcm2835_clock_data *data = clock->data;
-	u32 unused_frac_mask = GENMASK(CM_DIV_FRAC_BITS - data->frac_bits, 0);
+	u32 unused_frac_mask =
+		GENMASK(CM_DIV_FRAC_BITS - data->frac_bits, 0) >> 1;
 	u64 temp = (u64)parent_rate << CM_DIV_FRAC_BITS;
+	u64 rem;
 	u32 div;
 
-	do_div(temp, rate);
+	rem = do_div(temp, rate);
 	div = temp;
 
-	/* Round and mask off the unused bits */
-	if (unused_frac_mask != 0) {
-		div += unused_frac_mask >> 1;
-		div &= ~unused_frac_mask;
-	}
+	/* Round up and mask off the unused bits */
+	if (round_up && ((div & unused_frac_mask) != 0 || rem != 0))
+		div += unused_frac_mask + 1;
+	div &= ~unused_frac_mask;
 
 	/* Clamp to the limits. */
 	div = max(div, unused_frac_mask + 1);
@@ -1197,16 +1209,6 @@ static long bcm2835_clock_rate_from_divisor(struct bcm2835_clock *clock,
 	return temp;
 }
 
-static long bcm2835_clock_round_rate(struct clk_hw *hw,
-				     unsigned long rate,
-				     unsigned long *parent_rate)
-{
-	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
-	u32 div = bcm2835_clock_choose_div(hw, rate, *parent_rate);
-
-	return bcm2835_clock_rate_from_divisor(clock, *parent_rate, div);
-}
-
 static unsigned long bcm2835_clock_get_rate(struct clk_hw *hw,
 					    unsigned long parent_rate)
 {
@@ -1271,20 +1273,82 @@ static int bcm2835_clock_set_rate(struct clk_hw *hw,
 	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
 	struct bcm2835_cprman *cprman = clock->cprman;
 	const struct bcm2835_clock_data *data = clock->data;
-	u32 div = bcm2835_clock_choose_div(hw, rate, parent_rate);
+	u32 div = bcm2835_clock_choose_div(hw, rate, parent_rate, false);
 
 	cprman_write(cprman, data->div_reg, div);
 
 	return 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;
+	unsigned long rate, best_rate = 0;
+	unsigned long prate, best_prate = 0;
+	size_t i;
+	u32 div;
+
+	/*
+	 * Select parent clock that results in the closest but lower rate
+	 */
+	for (i = 0; i < clk_hw_get_num_parents(hw); ++i) {
+		parent = clk_hw_get_parent_by_index(hw, i);
+		if (!parent)
+			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);
+		if (rate > best_rate && rate <= req->rate) {
+			best_parent = parent;
+			best_prate = prate;
+			best_rate = rate;
+		}
+	}
+
+	if (!best_parent)
+		return -EINVAL;
+
+	req->best_parent_hw = best_parent;
+	req->best_parent_rate = best_prate;
+
+	req->rate = best_rate;
+
+	return 0;
+}
+
+static int bcm2835_clock_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
+	struct bcm2835_cprman *cprman = clock->cprman;
+	const struct bcm2835_clock_data *data = clock->data;
+	u8 src = (index << CM_SRC_SHIFT) & CM_SRC_MASK;
+
+	cprman_write(cprman, data->ctl_reg, src);
+	return 0;
+}
+
+static u8 bcm2835_clock_get_parent(struct clk_hw *hw)
+{
+	struct bcm2835_clock *clock = bcm2835_clock_from_hw(hw);
+	struct bcm2835_cprman *cprman = clock->cprman;
+	const struct bcm2835_clock_data *data = clock->data;
+	u32 src = cprman_read(cprman, data->ctl_reg);
+
+	return (src & CM_SRC_MASK) >> CM_SRC_SHIFT;
+}
+
+
 static const struct clk_ops bcm2835_clock_clk_ops = {
 	.is_prepared = bcm2835_clock_is_on,
 	.prepare = bcm2835_clock_on,
 	.unprepare = bcm2835_clock_off,
 	.recalc_rate = bcm2835_clock_get_rate,
 	.set_rate = bcm2835_clock_set_rate,
-	.round_rate = bcm2835_clock_round_rate,
+	.determine_rate = bcm2835_clock_determine_rate,
+	.set_parent = bcm2835_clock_set_parent,
+	.get_parent = bcm2835_clock_get_parent,
 };
 
 static int bcm2835_vpu_clock_is_on(struct clk_hw *hw)
@@ -1300,7 +1364,9 @@ static const struct clk_ops bcm2835_vpu_clock_clk_ops = {
 	.is_prepared = bcm2835_vpu_clock_is_on,
 	.recalc_rate = bcm2835_clock_get_rate,
 	.set_rate = bcm2835_clock_set_rate,
-	.round_rate = bcm2835_clock_round_rate,
+	.determine_rate = bcm2835_clock_determine_rate,
+	.set_parent = bcm2835_clock_set_parent,
+	.get_parent = bcm2835_clock_get_parent,
 };
 
 static struct clk *bcm2835_register_pll(struct bcm2835_cprman *cprman,
@@ -1394,45 +1460,23 @@ static struct clk *bcm2835_register_clock(struct bcm2835_cprman *cprman,
 {
 	struct bcm2835_clock *clock;
 	struct clk_init_data init;
-	const char *parent;
+	const char *parents[1 << CM_SRC_BITS];
+	size_t i;
 
 	/*
-	 * Most of the clock generators have a mux field, so we
-	 * instantiate a generic mux as our parent to handle it.
+	 * Replace our "xosc" references with the oscillator's
+	 * actual name.
 	 */
-	if (data->num_mux_parents) {
-		const char *parents[1 << CM_SRC_BITS];
-		int i;
-
-		parent = devm_kasprintf(cprman->dev, GFP_KERNEL,
-					"mux_%s", data->name);
-		if (!parent)
-			return NULL;
-
-		/*
-		 * Replace our "xosc" references with the oscillator's
-		 * actual name.
-		 */
-		for (i = 0; i < data->num_mux_parents; i++) {
-			if (strcmp(data->parents[i], "xosc") == 0)
-				parents[i] = cprman->osc_name;
-			else
-				parents[i] = data->parents[i];
-		}
-
-		clk_register_mux(cprman->dev, parent,
-				 parents, data->num_mux_parents,
-				 CLK_SET_RATE_PARENT,
-				 cprman->regs + data->ctl_reg,
-				 CM_SRC_SHIFT, CM_SRC_BITS,
-				 0, &cprman->regs_lock);
-	} else {
-		parent = data->parents[0];
+	for (i = 0; i < data->num_mux_parents; i++) {
+		if (strcmp(data->parents[i], "xosc") == 0)
+			parents[i] = cprman->osc_name;
+		else
+			parents[i] = data->parents[i];
 	}
 
 	memset(&init, 0, sizeof(init));
-	init.parent_names = &parent;
-	init.num_parents = 1;
+	init.parent_names = parents;
+	init.num_parents = data->num_mux_parents;
 	init.name = data->name;
 	init.flags = CLK_IGNORE_UNUSED;
 
@@ -1550,6 +1594,9 @@ static int bcm2835_clk_probe(struct platform_device *pdev)
 				  cprman->regs + CM_PERIICTL, CM_GATE_BIT,
 				  0, &cprman->regs_lock);
 
+	clks[BCM2835_CLOCK_PWM] =
+		bcm2835_register_clock(cprman, &bcm2835_clock_pwm_data);
+
 	return of_clk_add_provider(dev->of_node, of_clk_src_onecell_get,
 				   &cprman->onecell);
 }
diff --git a/drivers/clk/bcm/clk-bcm63xx.c b/drivers/clk/bcm/clk-bcm63xx.c
new file mode 100644
index 000000000000..fbc17ae5ff2b
--- /dev/null
+++ b/drivers/clk/bcm/clk-bcm63xx.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/init.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include "clk-iproc.h"
+
+static void __init bcm63138_armpll_init(struct device_node *node)
+{
+	iproc_armpll_setup(node);
+}
+CLK_OF_DECLARE(bcm63138_armpll, "brcm,bcm63138-armpll", bcm63138_armpll_init);
diff --git a/drivers/clk/clk-cs2000-cp.c b/drivers/clk/clk-cs2000-cp.c
new file mode 100644
index 000000000000..7379de8dc894
--- /dev/null
+++ b/drivers/clk/clk-cs2000-cp.c
@@ -0,0 +1,510 @@
+/*
+ * CS2000  --  CIRRUS LOGIC Fractional-N Clock Synthesizer & Clock Multiplier
+ *
+ * Copyright (C) 2015 Renesas Electronics Corporation
+ * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/i2c.h>
+#include <linux/of_device.h>
+#include <linux/module.h>
+
+#define CH_MAX 4
+#define RATIO_REG_SIZE 4
+
+#define DEVICE_ID	0x1
+#define DEVICE_CTRL	0x2
+#define DEVICE_CFG1	0x3
+#define DEVICE_CFG2	0x4
+#define GLOBAL_CFG	0x5
+#define Ratio_Add(x, nth)	(6 + (x * 4) + (nth))
+#define Ratio_Val(x, nth)	((x >> (24 - (8 * nth))) & 0xFF)
+#define Val_Ratio(x, nth)	((x & 0xFF) << (24 - (8 * nth)))
+#define FUNC_CFG1	0x16
+#define FUNC_CFG2	0x17
+
+/* DEVICE_ID */
+#define REVISION_MASK	(0x7)
+#define REVISION_B2_B3	(0x4)
+#define REVISION_C1	(0x6)
+
+/* DEVICE_CTRL */
+#define PLL_UNLOCK	(1 << 7)
+
+/* DEVICE_CFG1 */
+#define RSEL(x)		(((x) & 0x3) << 3)
+#define RSEL_MASK	RSEL(0x3)
+#define ENDEV1		(0x1)
+
+/* GLOBAL_CFG */
+#define ENDEV2		(0x1)
+
+#define CH_SIZE_ERR(ch)		((ch < 0) || (ch >= CH_MAX))
+#define hw_to_priv(_hw)		container_of(_hw, struct cs2000_priv, hw)
+#define priv_to_client(priv)	(priv->client)
+#define priv_to_dev(priv)	(&(priv_to_client(priv)->dev))
+
+#define CLK_IN	0
+#define REF_CLK	1
+#define CLK_MAX 2
+
+struct cs2000_priv {
+	struct clk_hw hw;
+	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[] = {
+	{ .compatible = "cirrus,cs2000-cp", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, cs2000_of_match);
+
+static const struct i2c_device_id cs2000_id[] = {
+	{ "cs2000-cp", },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, cs2000_id);
+
+#define cs2000_read(priv, addr) \
+	i2c_smbus_read_byte_data(priv_to_client(priv), addr)
+#define cs2000_write(priv, addr, val) \
+	i2c_smbus_write_byte_data(priv_to_client(priv), addr, val)
+
+static int cs2000_bset(struct cs2000_priv *priv, u8 addr, u8 mask, u8 val)
+{
+	s32 data;
+
+	data = cs2000_read(priv, addr);
+	if (data < 0)
+		return data;
+
+	data &= ~mask;
+	data |= (val & mask);
+
+	return cs2000_write(priv, addr, data);
+}
+
+static int cs2000_enable_dev_config(struct cs2000_priv *priv, bool enable)
+{
+	int ret;
+
+	ret = cs2000_bset(priv, DEVICE_CFG1, ENDEV1,
+			  enable ? ENDEV1 : 0);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_bset(priv, GLOBAL_CFG,  ENDEV2,
+			  enable ? ENDEV2 : 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int cs2000_clk_in_bound_rate(struct cs2000_priv *priv,
+				    u32 rate_in)
+{
+	u32 val;
+
+	if (rate_in >= 32000000 && rate_in < 56000000)
+		val = 0x0;
+	else if (rate_in >= 16000000 && rate_in < 28000000)
+		val = 0x1;
+	else if (rate_in >= 8000000 && rate_in < 14000000)
+		val = 0x2;
+	else
+		return -EINVAL;
+
+	return cs2000_bset(priv, FUNC_CFG1, 0x3 << 3, val << 3);
+}
+
+static int cs2000_wait_pll_lock(struct cs2000_priv *priv)
+{
+	struct device *dev = priv_to_dev(priv);
+	s32 val;
+	unsigned int i;
+
+	for (i = 0; i < 256; i++) {
+		val = cs2000_read(priv, DEVICE_CTRL);
+		if (val < 0)
+			return val;
+		if (!(val & PLL_UNLOCK))
+			return 0;
+		udelay(1);
+	}
+
+	dev_err(dev, "pll lock failed\n");
+
+	return -ETIMEDOUT;
+}
+
+static int cs2000_clk_out_enable(struct cs2000_priv *priv, bool enable)
+{
+	/* enable both AUX_OUT, CLK_OUT */
+	return cs2000_write(priv, DEVICE_CTRL, enable ? 0 : 0x3);
+}
+
+static u32 cs2000_rate_to_ratio(u32 rate_in, u32 rate_out)
+{
+	u64 ratio;
+
+	/*
+	 * ratio = rate_out / rate_in * 2^20
+	 *
+	 * To avoid over flow, rate_out is u64.
+	 * The result should be u32.
+	 */
+	ratio = (u64)rate_out << 20;
+	do_div(ratio, rate_in);
+
+	return ratio;
+}
+
+static unsigned long cs2000_ratio_to_rate(u32 ratio, u32 rate_in)
+{
+	u64 rate_out;
+
+	/*
+	 * ratio = rate_out / rate_in * 2^20
+	 *
+	 * To avoid over flow, rate_out is u64.
+	 * The result should be u32 or unsigned long.
+	 */
+
+	rate_out = (u64)ratio * rate_in;
+	return rate_out >> 20;
+}
+
+static int cs2000_ratio_set(struct cs2000_priv *priv,
+			    int ch, u32 rate_in, u32 rate_out)
+{
+	u32 val;
+	unsigned int i;
+	int ret;
+
+	if (CH_SIZE_ERR(ch))
+		return -EINVAL;
+
+	val = cs2000_rate_to_ratio(rate_in, rate_out);
+	for (i = 0; i < RATIO_REG_SIZE; i++) {
+		ret = cs2000_write(priv,
+				   Ratio_Add(ch, i),
+				   Ratio_Val(val, i));
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static u32 cs2000_ratio_get(struct cs2000_priv *priv, int ch)
+{
+	s32 tmp;
+	u32 val;
+	unsigned int i;
+
+	val = 0;
+	for (i = 0; i < RATIO_REG_SIZE; i++) {
+		tmp = cs2000_read(priv, Ratio_Add(ch, i));
+		if (tmp < 0)
+			return 0;
+
+		val |= Val_Ratio(tmp, i);
+	}
+
+	return val;
+}
+
+static int cs2000_ratio_select(struct cs2000_priv *priv, int ch)
+{
+	int ret;
+
+	if (CH_SIZE_ERR(ch))
+		return -EINVAL;
+
+	/*
+	 * FIXME
+	 *
+	 * this driver supports static ratio mode only at this point.
+	 */
+	ret = cs2000_bset(priv, DEVICE_CFG1, RSEL_MASK, RSEL(ch));
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_write(priv, DEVICE_CFG2, 0x0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static unsigned long cs2000_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct cs2000_priv *priv = hw_to_priv(hw);
+	int ch = 0; /* it uses ch0 only at this point */
+	u32 ratio;
+
+	ratio = cs2000_ratio_get(priv, ch);
+
+	return cs2000_ratio_to_rate(ratio, parent_rate);
+}
+
+static long cs2000_round_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long *parent_rate)
+{
+	u32 ratio;
+
+	ratio = cs2000_rate_to_ratio(*parent_rate, rate);
+
+	return cs2000_ratio_to_rate(ratio, *parent_rate);
+}
+
+static int __cs2000_set_rate(struct cs2000_priv *priv, int ch,
+			     unsigned long rate, unsigned long parent_rate)
+
+{
+	int ret;
+
+	ret = cs2000_clk_in_bound_rate(priv, parent_rate);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_ratio_set(priv, ch, parent_rate, rate);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_ratio_select(priv, ch);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int cs2000_set_rate(struct clk_hw *hw,
+			   unsigned long rate, unsigned long parent_rate)
+{
+	struct cs2000_priv *priv = hw_to_priv(hw);
+	int ch = 0; /* it uses ch0 only at this point */
+
+	return __cs2000_set_rate(priv, ch, rate, parent_rate);
+}
+
+static int cs2000_enable(struct clk_hw *hw)
+{
+	struct cs2000_priv *priv = hw_to_priv(hw);
+	int ret;
+
+	ret = cs2000_enable_dev_config(priv, true);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_clk_out_enable(priv, true);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_wait_pll_lock(priv);
+	if (ret < 0)
+		return ret;
+
+	return ret;
+}
+
+static void cs2000_disable(struct clk_hw *hw)
+{
+	struct cs2000_priv *priv = hw_to_priv(hw);
+
+	cs2000_enable_dev_config(priv, false);
+
+	cs2000_clk_out_enable(priv, false);
+}
+
+static u8 cs2000_get_parent(struct clk_hw *hw)
+{
+	/* always return REF_CLK */
+	return REF_CLK;
+}
+
+static const struct clk_ops cs2000_ops = {
+	.get_parent	= cs2000_get_parent,
+	.recalc_rate	= cs2000_recalc_rate,
+	.round_rate	= cs2000_round_rate,
+	.set_rate	= cs2000_set_rate,
+	.prepare	= cs2000_enable,
+	.unprepare	= cs2000_disable,
+};
+
+static int cs2000_clk_get(struct cs2000_priv *priv)
+{
+	struct i2c_client *client = priv_to_client(priv);
+	struct device *dev = &client->dev;
+	struct clk *clk_in, *ref_clk;
+
+	clk_in = devm_clk_get(dev, "clk_in");
+	/* not yet provided */
+	if (IS_ERR(clk_in))
+		return -EPROBE_DEFER;
+
+	ref_clk = devm_clk_get(dev, "ref_clk");
+	/* not yet provided */
+	if (IS_ERR(ref_clk))
+		return -EPROBE_DEFER;
+
+	priv->clk_in	= clk_in;
+	priv->ref_clk	= ref_clk;
+
+	return 0;
+}
+
+static int cs2000_clk_register(struct cs2000_priv *priv)
+{
+	struct device *dev = priv_to_dev(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;
+	int ret;
+
+	of_property_read_string(np, "clock-output-names", &name);
+
+	/*
+	 * set default rate as 1/1.
+	 * otherwise .set_rate which setup ratio
+	 * is never called if user requests 1/1 rate
+	 */
+	rate = clk_get_rate(priv->ref_clk);
+	ret = __cs2000_set_rate(priv, ch, rate, rate);
+	if (ret < 0)
+		return ret;
+
+	parent_names[CLK_IN]	= __clk_get_name(priv->clk_in);
+	parent_names[REF_CLK]	= __clk_get_name(priv->ref_clk);
+
+	init.name		= name;
+	init.ops		= &cs2000_ops;
+	init.flags		= CLK_SET_RATE_GATE;
+	init.parent_names	= parent_names;
+	init.num_parents	= ARRAY_SIZE(parent_names);
+
+	priv->hw.init = &init;
+
+	clk = clk_register(dev, &priv->hw);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	ret = of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	if (ret < 0) {
+		clk_unregister(clk);
+		return ret;
+	}
+
+	priv->clk_out = clk;
+
+	return 0;
+}
+
+static int cs2000_version_print(struct cs2000_priv *priv)
+{
+	struct i2c_client *client = priv_to_client(priv);
+	struct device *dev = &client->dev;
+	s32 val;
+	const char *revision;
+
+	val = cs2000_read(priv, DEVICE_ID);
+	if (val < 0)
+		return val;
+
+	/* CS2000 should be 0x0 */
+	if (val >> 3)
+		return -EIO;
+
+	switch (val & REVISION_MASK) {
+	case REVISION_B2_B3:
+		revision = "B2 / B3";
+		break;
+	case REVISION_C1:
+		revision = "C1";
+		break;
+	default:
+		return -EIO;
+	}
+
+	dev_info(dev, "revision - %s\n", revision);
+
+	return 0;
+}
+
+static int cs2000_remove(struct i2c_client *client)
+{
+	struct cs2000_priv *priv = i2c_get_clientdata(client);
+	struct device *dev = &client->dev;
+	struct device_node *np = dev->of_node;
+
+	of_clk_del_provider(np);
+
+	clk_unregister(priv->clk_out);
+
+	return 0;
+}
+
+static int cs2000_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct cs2000_priv *priv;
+	struct device *dev = &client->dev;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->client = client;
+	i2c_set_clientdata(client, priv);
+
+	ret = cs2000_clk_get(priv);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_clk_register(priv);
+	if (ret < 0)
+		return ret;
+
+	ret = cs2000_version_print(priv);
+	if (ret < 0)
+		goto probe_err;
+
+	return 0;
+
+probe_err:
+	cs2000_remove(client);
+
+	return ret;
+}
+
+static struct i2c_driver cs2000_driver = {
+	.driver = {
+		.name = "cs2000-cp",
+		.of_match_table = cs2000_of_match,
+	},
+	.probe		= cs2000_probe,
+	.remove		= cs2000_remove,
+	.id_table	= cs2000_id,
+};
+
+module_i2c_driver(cs2000_driver);
+
+MODULE_DESCRIPTION("CS2000-CP driver");
+MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c
index 3ace102a2a0a..ded3ff4b91b9 100644
--- a/drivers/clk/clk-divider.c
+++ b/drivers/clk/clk-divider.c
@@ -32,13 +32,14 @@
 
 #define div_mask(width)	((1 << (width)) - 1)
 
-static unsigned int _get_table_maxdiv(const struct clk_div_table *table)
+static unsigned int _get_table_maxdiv(const struct clk_div_table *table,
+				      u8 width)
 {
-	unsigned int maxdiv = 0;
+	unsigned int maxdiv = 0, mask = div_mask(width);
 	const struct clk_div_table *clkt;
 
 	for (clkt = table; clkt->div; clkt++)
-		if (clkt->div > maxdiv)
+		if (clkt->div > maxdiv && clkt->val <= mask)
 			maxdiv = clkt->div;
 	return maxdiv;
 }
@@ -62,7 +63,7 @@ static unsigned int _get_maxdiv(const struct clk_div_table *table, u8 width,
 	if (flags & CLK_DIVIDER_POWER_OF_TWO)
 		return 1 << div_mask(width);
 	if (table)
-		return _get_table_maxdiv(table);
+		return _get_table_maxdiv(table, width);
 	return div_mask(width) + 1;
 }
 
diff --git a/drivers/clk/clk-gpio.c b/drivers/clk/clk-gpio.c
index 335322dc403f..7b09a265d79f 100644
--- a/drivers/clk/clk-gpio.c
+++ b/drivers/clk/clk-gpio.c
@@ -264,8 +264,8 @@ static struct clk *of_clk_gpio_gate_delayed_register_get(const char *name,
 		const char * const *parent_names, u8 num_parents,
 		unsigned gpio, bool active_low)
 {
-	return clk_register_gpio_gate(NULL, name, parent_names[0],
-			gpio, active_low, 0);
+	return clk_register_gpio_gate(NULL, name, parent_names ?
+			parent_names[0] : NULL, gpio, active_low, 0);
 }
 
 static struct clk *of_clk_gpio_mux_delayed_register_get(const char *name,
@@ -287,18 +287,26 @@ static void __init of_gpio_clk_setup(struct device_node *node,
 	const char **parent_names;
 	int i, num_parents;
 
-	data = kzalloc(sizeof(*data), GFP_KERNEL);
-	if (!data)
-		return;
-
 	num_parents = of_clk_get_parent_count(node);
+	if (num_parents < 0)
+		num_parents = 0;
 
-	parent_names = kcalloc(num_parents, sizeof(char *), GFP_KERNEL);
-	if (!parent_names)
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
 		return;
 
-	for (i = 0; i < num_parents; i++)
-		parent_names[i] = of_clk_get_parent_name(node, i);
+	if (num_parents) {
+		parent_names = kcalloc(num_parents, sizeof(char *), GFP_KERNEL);
+		if (!parent_names) {
+			kfree(data);
+			return;
+		}
+
+		for (i = 0; i < num_parents; i++)
+			parent_names[i] = of_clk_get_parent_name(node, i);
+	} else {
+		parent_names = NULL;
+	}
 
 	data->num_parents = num_parents;
 	data->parent_names = parent_names;
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index 7129c86a79db..5ed03c8a8df9 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -71,10 +71,9 @@ static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
 	u32 val;
 	unsigned long flags = 0;
 
-	if (mux->table)
+	if (mux->table) {
 		index = mux->table[index];
-
-	else {
+	} else {
 		if (mux->flags & CLK_MUX_INDEX_BIT)
 			index = 1 << index;
 
diff --git a/drivers/clk/clk-scpi.c b/drivers/clk/clk-scpi.c
index cd0f2726f5e0..89e9ca78bb94 100644
--- a/drivers/clk/clk-scpi.c
+++ b/drivers/clk/clk-scpi.c
@@ -299,7 +299,7 @@ static int scpi_clocks_probe(struct platform_device *pdev)
 	/* Add the virtual cpufreq device */
 	cpufreq_dev = platform_device_register_simple("scpi-cpufreq",
 						      -1, NULL, 0);
-	if (!cpufreq_dev)
+	if (IS_ERR(cpufreq_dev))
 		pr_warn("unable to register cpufreq device");
 
 	return 0;
diff --git a/drivers/clk/clk-si5351.c b/drivers/clk/clk-si5351.c
index e346b223199d..850316ac8831 100644
--- a/drivers/clk/clk-si5351.c
+++ b/drivers/clk/clk-si5351.c
@@ -1091,6 +1091,13 @@ static int si5351_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
 	si5351_set_bits(hwdata->drvdata, SI5351_CLK0_CTRL + hwdata->num,
 			SI5351_CLK_POWERDOWN, 0);
 
+	/*
+	 * Do a pll soft reset on both plls, needed in some cases to get
+	 * all outputs running.
+	 */
+	si5351_reg_write(hwdata->drvdata, SI5351_PLL_RESET,
+			 SI5351_PLL_RESET_A | SI5351_PLL_RESET_B);
+
 	dev_dbg(&hwdata->drvdata->client->dev,
 		"%s - %s: rdiv = %u, parent_rate = %lu, rate = %lu\n",
 		__func__, clk_hw_get_name(hw), (1 << rdiv),
diff --git a/drivers/clk/clk-tango4.c b/drivers/clk/clk-tango4.c
new file mode 100644
index 000000000000..004ab7dfcfe3
--- /dev/null
+++ b/drivers/clk/clk-tango4.c
@@ -0,0 +1,61 @@
+#include <linux/kernel.h>
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/init.h>
+#include <linux/io.h>
+
+static struct clk *out[2];
+static struct clk_onecell_data clk_data = { out, 2 };
+
+#define SYSCLK_CTRL	0x20
+#define CPUCLK_CTRL	0x24
+#define LEGACY_DIV	0x3c
+
+#define PLL_N(val)	(((val) >>  0) & 0x7f)
+#define PLL_K(val)	(((val) >> 13) & 0x7)
+#define PLL_M(val)	(((val) >> 16) & 0x7)
+#define DIV_INDEX(val)	(((val) >>  8) & 0xf)
+
+static void __init make_pll(int idx, const char *parent, void __iomem *base)
+{
+	char name[8];
+	u32 val, mul, div;
+
+	sprintf(name, "pll%d", idx);
+	val = readl_relaxed(base + idx*8);
+	mul =  PLL_N(val) + 1;
+	div = (PLL_M(val) + 1) << PLL_K(val);
+	clk_register_fixed_factor(NULL, name, parent, 0, mul, div);
+}
+
+static int __init get_div(void __iomem *base)
+{
+	u8 sysclk_tab[16] = { 2, 4, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4 };
+	int idx = DIV_INDEX(readl_relaxed(base + LEGACY_DIV));
+
+	return sysclk_tab[idx];
+}
+
+static void __init tango4_clkgen_setup(struct device_node *np)
+{
+	int div, ret;
+	void __iomem *base = of_iomap(np, 0);
+	const char *parent = of_clk_get_parent_name(np, 0);
+
+	if (!base)
+		panic("%s: invalid address\n", np->full_name);
+
+	make_pll(0, parent, base);
+	make_pll(1, parent, base);
+
+	out[0] = clk_register_divider(NULL, "cpuclk", "pll0", 0,
+			base + CPUCLK_CTRL, 8, 8, CLK_DIVIDER_ONE_BASED, NULL);
+
+	div = readl_relaxed(base + SYSCLK_CTRL) & BIT(23) ? get_div(base) : 4;
+	out[1] = clk_register_fixed_factor(NULL, "sysclk", "pll1", 0, 1, div);
+
+	ret = of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	if (IS_ERR(out[0]) || IS_ERR(out[1]) || ret < 0)
+		panic("%s: clk registration failed\n", np->full_name);
+}
+CLK_OF_DECLARE(tango4_clkgen, "sigma,tango4-clkgen", tango4_clkgen_setup);
diff --git a/drivers/clk/clk-xgene.c b/drivers/clk/clk-xgene.c
index 27c0da29eca3..10224b01b97c 100644
--- a/drivers/clk/clk-xgene.c
+++ b/drivers/clk/clk-xgene.c
@@ -351,7 +351,8 @@ static int xgene_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 		/* Set new divider */
 		data = xgene_clk_read(pclk->param.divider_reg +
 				pclk->param.reg_divider_offset);
-		data &= ~((1 << pclk->param.reg_divider_width) - 1);
+		data &= ~((1 << pclk->param.reg_divider_width) - 1)
+				<< pclk->param.reg_divider_shift;
 		data |= divider;
 		xgene_clk_write(data, pclk->param.divider_reg +
 					pclk->param.reg_divider_offset);
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index f13c3f4228d4..b4db67a446c8 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -1443,6 +1443,15 @@ static void clk_change_rate(struct clk_core *core)
 	else if (core->parent)
 		best_parent_rate = core->parent->rate;
 
+	if (core->flags & CLK_SET_RATE_UNGATE) {
+		unsigned long flags;
+
+		clk_core_prepare(core);
+		flags = clk_enable_lock();
+		clk_core_enable(core);
+		clk_enable_unlock(flags);
+	}
+
 	if (core->new_parent && core->new_parent != core->parent) {
 		old_parent = __clk_set_parent_before(core, core->new_parent);
 		trace_clk_set_parent(core, core->new_parent);
@@ -1469,6 +1478,15 @@ static void clk_change_rate(struct clk_core *core)
 
 	core->rate = clk_recalc(core, best_parent_rate);
 
+	if (core->flags & CLK_SET_RATE_UNGATE) {
+		unsigned long flags;
+
+		flags = clk_enable_lock();
+		clk_core_disable(core);
+		clk_enable_unlock(flags);
+		clk_core_unprepare(core);
+	}
+
 	if (core->notifier_count && old_rate != core->rate)
 		__clk_notify(core, POST_RATE_CHANGE, old_rate, core->rate);
 
@@ -1944,7 +1962,7 @@ bool clk_is_match(const struct clk *p, const struct clk *q)
 	if (p == q)
 		return true;
 
-	/* true if clk->core pointers match. Avoid derefing garbage */
+	/* true if clk->core pointers match. Avoid dereferencing garbage */
 	if (!IS_ERR_OR_NULL(p) && !IS_ERR_OR_NULL(q))
 		if (p->core == q->core)
 			return true;
@@ -2482,7 +2500,7 @@ struct clk *__clk_create_clk(struct clk_hw *hw, const char *dev_id,
 	struct clk *clk;
 
 	/* This is to allow this function to be chained to others */
-	if (!hw || IS_ERR(hw))
+	if (IS_ERR_OR_NULL(hw))
 		return (struct clk *) hw;
 
 	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
@@ -2806,10 +2824,9 @@ void __clk_put(struct clk *clk)
  * re-enter into the clk framework by calling any top-level clk APIs;
  * this will cause a nested prepare_lock mutex.
  *
- * In all notification cases cases (pre, post and abort rate change) the
- * original clock rate is passed to the callback via struct
- * clk_notifier_data.old_rate and the new frequency is passed via struct
- * clk_notifier_data.new_rate.
+ * In all notification cases (pre, post and abort rate change) the original
+ * clock rate is passed to the callback via struct clk_notifier_data.old_rate
+ * and the new frequency is passed via struct clk_notifier_data.new_rate.
  *
  * clk_notifier_register() must be called from non-atomic context.
  * Returns -EINVAL if called with null arguments, -ENOMEM upon
@@ -3062,9 +3079,6 @@ const char *of_clk_get_parent_name(struct device_node *np, int index)
 	int count;
 	struct clk *clk;
 
-	if (index < 0)
-		return NULL;
-
 	rc = of_parse_phandle_with_args(np, "clocks", "#clock-cells", index,
 					&clkspec);
 	if (rc)
@@ -3083,6 +3097,9 @@ const char *of_clk_get_parent_name(struct device_node *np, int index)
 		}
 		count++;
 	}
+	/* We went off the end of 'clock-indices' without finding it */
+	if (prop && !vp)
+		return NULL;
 
 	if (of_property_read_string_index(clkspec.np, "clock-output-names",
 					  index,
diff --git a/drivers/clk/imx/clk-imx25.c b/drivers/clk/imx/clk-imx25.c
index c4c141cab444..23686f756b5e 100644
--- a/drivers/clk/imx/clk-imx25.c
+++ b/drivers/clk/imx/clk-imx25.c
@@ -96,13 +96,11 @@ static struct clk ** const uart_clks[] __initconst = {
 	NULL
 };
 
-static int __init __mx25_clocks_init(unsigned long osc_rate,
-				     void __iomem *ccm_base)
+static int __init __mx25_clocks_init(void __iomem *ccm_base)
 {
 	BUG_ON(!ccm_base);
 
 	clk[dummy] = imx_clk_fixed("dummy", 0);
-	clk[osc] = imx_clk_fixed("osc", osc_rate);
 	clk[mpll] = imx_clk_pllv1(IMX_PLLV1_IMX25, "mpll", "osc", ccm(CCM_MPCTL));
 	clk[upll] = imx_clk_pllv1(IMX_PLLV1_IMX25, "upll", "osc", ccm(CCM_UPCTL));
 	clk[mpll_cpu_3_4] = imx_clk_fixed_factor("mpll_cpu_3_4", "mpll", 3, 4);
@@ -250,22 +248,10 @@ static int __init __mx25_clocks_init(unsigned long osc_rate,
 
 static void __init mx25_clocks_init_dt(struct device_node *np)
 {
-	struct device_node *refnp;
-	unsigned long osc_rate = 24000000;
 	void __iomem *ccm;
 
-	/* retrieve the freqency of fixed clocks from device tree */
-	for_each_compatible_node(refnp, NULL, "fixed-clock") {
-		u32 rate;
-		if (of_property_read_u32(refnp, "clock-frequency", &rate))
-			continue;
-
-		if (of_device_is_compatible(refnp, "fsl,imx-osc"))
-			osc_rate = rate;
-	}
-
 	ccm = of_iomap(np, 0);
-	__mx25_clocks_init(osc_rate, ccm);
+	__mx25_clocks_init(ccm);
 
 	clk_data.clks = clk;
 	clk_data.clk_num = ARRAY_SIZE(clk);
diff --git a/drivers/clk/imx/clk-imx51-imx53.c b/drivers/clk/imx/clk-imx51-imx53.c
index c6770348d2ab..29d4c44ef356 100644
--- a/drivers/clk/imx/clk-imx51-imx53.c
+++ b/drivers/clk/imx/clk-imx51-imx53.c
@@ -519,10 +519,10 @@ static void __init mx53_clocks_init(struct device_node *np)
 						mx53_ldb_di0_sel, ARRAY_SIZE(mx53_ldb_di0_sel), CLK_SET_RATE_PARENT);
 	clk[IMX5_CLK_LDB_DI0_GATE]	= imx_clk_gate2("ldb_di0_gate", "ldb_di0_div", MXC_CCM_CCGR6, 28);
 	clk[IMX5_CLK_LDB_DI1_GATE]	= imx_clk_gate2("ldb_di1_gate", "ldb_di1_div", MXC_CCM_CCGR6, 30);
-	clk[IMX5_CLK_IPU_DI0_SEL]	= imx_clk_mux("ipu_di0_sel", MXC_CCM_CSCMR2, 26, 3,
-						mx53_ipu_di0_sel, ARRAY_SIZE(mx53_ipu_di0_sel));
-	clk[IMX5_CLK_IPU_DI1_SEL]	= imx_clk_mux("ipu_di1_sel", MXC_CCM_CSCMR2, 29, 3,
-						mx53_ipu_di1_sel, ARRAY_SIZE(mx53_ipu_di1_sel));
+	clk[IMX5_CLK_IPU_DI0_SEL]	= imx_clk_mux_flags("ipu_di0_sel", MXC_CCM_CSCMR2, 26, 3,
+						mx53_ipu_di0_sel, ARRAY_SIZE(mx53_ipu_di0_sel), CLK_SET_RATE_PARENT);
+	clk[IMX5_CLK_IPU_DI1_SEL]	= imx_clk_mux_flags("ipu_di1_sel", MXC_CCM_CSCMR2, 29, 3,
+						mx53_ipu_di1_sel, ARRAY_SIZE(mx53_ipu_di1_sel), CLK_SET_RATE_PARENT);
 	clk[IMX5_CLK_TVE_EXT_SEL]	= imx_clk_mux_flags("tve_ext_sel", MXC_CCM_CSCMR1, 6, 1,
 						mx53_tve_ext_sel, ARRAY_SIZE(mx53_tve_ext_sel), CLK_SET_RATE_PARENT);
 	clk[IMX5_CLK_TVE_GATE]		= imx_clk_gate2("tve_gate", "tve_pred", MXC_CCM_CCGR2, 30);
diff --git a/drivers/clk/imx/clk-imx6q.c b/drivers/clk/imx/clk-imx6q.c
index c1935081d34a..f0efc6feeec2 100644
--- a/drivers/clk/imx/clk-imx6q.c
+++ b/drivers/clk/imx/clk-imx6q.c
@@ -70,7 +70,8 @@ static const char *cko_sels[] = { "cko1", "cko2", };
 static const char *lvds_sels[] = {
 	"dummy", "dummy", "dummy", "dummy", "dummy", "dummy",
 	"pll4_audio", "pll5_video", "pll8_mlb", "enet_ref",
-	"pcie_ref_125m", "sata_ref_100m",
+	"pcie_ref_125m", "sata_ref_100m",  "usbphy1", "usbphy2",
+	"dummy", "dummy", "dummy", "dummy", "osc",
 };
 static const char *pll_bypass_src_sels[] = { "osc", "lvds1_in", "lvds2_in", "dummy", };
 static const char *pll1_bypass_sels[] = { "pll1", "pll1_bypass_src", };
diff --git a/drivers/clk/imx/clk-imx6ul.c b/drivers/clk/imx/clk-imx6ul.c
index 01718d05e952..08692d74b884 100644
--- a/drivers/clk/imx/clk-imx6ul.c
+++ b/drivers/clk/imx/clk-imx6ul.c
@@ -399,9 +399,7 @@ static void __init imx6ul_clocks_init(struct device_node *ccm_node)
 	/* mask handshake of mmdc */
 	writel_relaxed(BM_CCM_CCDR_MMDC_CH0_MASK, base + CCDR);
 
-	for (i = 0; i < ARRAY_SIZE(clks); i++)
-		if (IS_ERR(clks[i]))
-			pr_err("i.MX6UL clk %d: register failed with %ld\n", i, PTR_ERR(clks[i]));
+	imx_check_clocks(clks, ARRAY_SIZE(clks));
 
 	clk_data.clks = clks;
 	clk_data.clk_num = ARRAY_SIZE(clks);
diff --git a/drivers/clk/imx/clk-imx7d.c b/drivers/clk/imx/clk-imx7d.c
index 448ef321948b..fbb6a8c8653d 100644
--- a/drivers/clk/imx/clk-imx7d.c
+++ b/drivers/clk/imx/clk-imx7d.c
@@ -833,10 +833,13 @@ static void __init imx7d_clocks_init(struct device_node *ccm_node)
 
 	clks[IMX7D_GPT_3M_CLK] = imx_clk_fixed_factor("gpt_3m", "osc", 1, 8);
 
-	for (i = 0; i < ARRAY_SIZE(clks); i++)
-		if (IS_ERR(clks[i]))
-			pr_err("i.MX7D clk %d: register failed with %ld\n",
-					i, PTR_ERR(clks[i]));
+	clks[IMX7D_CLK_ARM] = imx_clk_cpu("arm", "arm_a7_root_clk",
+					 clks[IMX7D_ARM_A7_ROOT_CLK],
+					 clks[IMX7D_ARM_A7_ROOT_SRC],
+					 clks[IMX7D_PLL_ARM_MAIN_CLK],
+					 clks[IMX7D_PLL_SYS_MAIN_CLK]);
+
+	imx_check_clocks(clks, ARRAY_SIZE(clks));
 
 	clk_data.clks = clks;
 	clk_data.clk_num = ARRAY_SIZE(clks);
diff --git a/drivers/clk/imx/clk-pllv3.c b/drivers/clk/imx/clk-pllv3.c
index 6addf8f58b97..c05c43d56a94 100644
--- a/drivers/clk/imx/clk-pllv3.c
+++ b/drivers/clk/imx/clk-pllv3.c
@@ -97,6 +97,16 @@ static void clk_pllv3_unprepare(struct clk_hw *hw)
 	writel_relaxed(val, pll->base);
 }
 
+static int clk_pllv3_is_prepared(struct clk_hw *hw)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+
+	if (readl_relaxed(pll->base) & BM_PLL_LOCK)
+		return 1;
+
+	return 0;
+}
+
 static unsigned long clk_pllv3_recalc_rate(struct clk_hw *hw,
 					   unsigned long parent_rate)
 {
@@ -139,6 +149,7 @@ static int clk_pllv3_set_rate(struct clk_hw *hw, unsigned long rate,
 static const struct clk_ops clk_pllv3_ops = {
 	.prepare	= clk_pllv3_prepare,
 	.unprepare	= clk_pllv3_unprepare,
+	.is_prepared	= clk_pllv3_is_prepared,
 	.recalc_rate	= clk_pllv3_recalc_rate,
 	.round_rate	= clk_pllv3_round_rate,
 	.set_rate	= clk_pllv3_set_rate,
@@ -193,6 +204,7 @@ static int clk_pllv3_sys_set_rate(struct clk_hw *hw, unsigned long rate,
 static const struct clk_ops clk_pllv3_sys_ops = {
 	.prepare	= clk_pllv3_prepare,
 	.unprepare	= clk_pllv3_unprepare,
+	.is_prepared	= clk_pllv3_is_prepared,
 	.recalc_rate	= clk_pllv3_sys_recalc_rate,
 	.round_rate	= clk_pllv3_sys_round_rate,
 	.set_rate	= clk_pllv3_sys_set_rate,
@@ -265,6 +277,7 @@ static int clk_pllv3_av_set_rate(struct clk_hw *hw, unsigned long rate,
 static const struct clk_ops clk_pllv3_av_ops = {
 	.prepare	= clk_pllv3_prepare,
 	.unprepare	= clk_pllv3_unprepare,
+	.is_prepared	= clk_pllv3_is_prepared,
 	.recalc_rate	= clk_pllv3_av_recalc_rate,
 	.round_rate	= clk_pllv3_av_round_rate,
 	.set_rate	= clk_pllv3_av_set_rate,
@@ -279,6 +292,7 @@ static unsigned long clk_pllv3_enet_recalc_rate(struct clk_hw *hw,
 static const struct clk_ops clk_pllv3_enet_ops = {
 	.prepare	= clk_pllv3_prepare,
 	.unprepare	= clk_pllv3_unprepare,
+	.is_prepared	= clk_pllv3_is_prepared,
 	.recalc_rate	= clk_pllv3_enet_recalc_rate,
 };
 
diff --git a/drivers/clk/mmp/clk-mmp2.c b/drivers/clk/mmp/clk-mmp2.c
index 71fd29348f28..38931dbd1eff 100644
--- a/drivers/clk/mmp/clk-mmp2.c
+++ b/drivers/clk/mmp/clk-mmp2.c
@@ -17,8 +17,6 @@
 #include <linux/delay.h>
 #include <linux/err.h>
 
-#include <mach/addr-map.h>
-
 #include "clk.h"
 
 #define APBC_RTC	0x0
@@ -74,7 +72,8 @@ static const char *sdh_parent[] = {"pll1_4", "pll2", "usb_pll", "pll1"};
 static const char *disp_parent[] = {"pll1", "pll1_16", "pll2", "vctcxo"};
 static const char *ccic_parent[] = {"pll1_2", "pll1_16", "vctcxo"};
 
-void __init mmp2_clk_init(void)
+void __init mmp2_clk_init(phys_addr_t mpmu_phys, phys_addr_t apmu_phys,
+			  phys_addr_t apbc_phys)
 {
 	struct clk *clk;
 	struct clk *vctcxo;
@@ -82,19 +81,19 @@ void __init mmp2_clk_init(void)
 	void __iomem *apmu_base;
 	void __iomem *apbc_base;
 
-	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	mpmu_base = ioremap(mpmu_phys, SZ_4K);
 	if (mpmu_base == NULL) {
 		pr_err("error to ioremap MPMU base\n");
 		return;
 	}
 
-	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	apmu_base = ioremap(apmu_phys, SZ_4K);
 	if (apmu_base == NULL) {
 		pr_err("error to ioremap APMU base\n");
 		return;
 	}
 
-	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	apbc_base = ioremap(apbc_phys, SZ_4K);
 	if (apbc_base == NULL) {
 		pr_err("error to ioremap APBC base\n");
 		return;
diff --git a/drivers/clk/mmp/clk-pxa168.c b/drivers/clk/mmp/clk-pxa168.c
index 75244915df05..0dd83fb950c9 100644
--- a/drivers/clk/mmp/clk-pxa168.c
+++ b/drivers/clk/mmp/clk-pxa168.c
@@ -17,8 +17,6 @@
 #include <linux/delay.h>
 #include <linux/err.h>
 
-#include <mach/addr-map.h>
-
 #include "clk.h"
 
 #define APBC_RTC	0x28
@@ -67,7 +65,8 @@ static const char *disp_parent[] = {"pll1_2", "pll1_12"};
 static const char *ccic_parent[] = {"pll1_2", "pll1_12"};
 static const char *ccic_phy_parent[] = {"pll1_6", "pll1_12"};
 
-void __init pxa168_clk_init(void)
+void __init pxa168_clk_init(phys_addr_t mpmu_phys, phys_addr_t apmu_phys,
+			    phys_addr_t apbc_phys)
 {
 	struct clk *clk;
 	struct clk *uart_pll;
@@ -75,19 +74,19 @@ void __init pxa168_clk_init(void)
 	void __iomem *apmu_base;
 	void __iomem *apbc_base;
 
-	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	mpmu_base = ioremap(mpmu_phys, SZ_4K);
 	if (mpmu_base == NULL) {
 		pr_err("error to ioremap MPMU base\n");
 		return;
 	}
 
-	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	apmu_base = ioremap(apmu_phys, SZ_4K);
 	if (apmu_base == NULL) {
 		pr_err("error to ioremap APMU base\n");
 		return;
 	}
 
-	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	apbc_base = ioremap(apbc_phys, SZ_4K);
 	if (apbc_base == NULL) {
 		pr_err("error to ioremap APBC base\n");
 		return;
diff --git a/drivers/clk/mmp/clk-pxa910.c b/drivers/clk/mmp/clk-pxa910.c
index 37ba04ba1368..e1d2ce22cdf1 100644
--- a/drivers/clk/mmp/clk-pxa910.c
+++ b/drivers/clk/mmp/clk-pxa910.c
@@ -17,8 +17,6 @@
 #include <linux/delay.h>
 #include <linux/err.h>
 
-#include <mach/addr-map.h>
-
 #include "clk.h"
 
 #define APBC_RTC	0x28
@@ -65,7 +63,8 @@ static const char *disp_parent[] = {"pll1_2", "pll1_12"};
 static const char *ccic_parent[] = {"pll1_2", "pll1_12"};
 static const char *ccic_phy_parent[] = {"pll1_6", "pll1_12"};
 
-void __init pxa910_clk_init(void)
+void __init pxa910_clk_init(phys_addr_t mpmu_phys, phys_addr_t apmu_phys,
+			    phys_addr_t apbc_phys, phys_addr_t apbcp_phys)
 {
 	struct clk *clk;
 	struct clk *uart_pll;
@@ -74,25 +73,25 @@ void __init pxa910_clk_init(void)
 	void __iomem *apbcp_base;
 	void __iomem *apbc_base;
 
-	mpmu_base = ioremap(APB_PHYS_BASE + 0x50000, SZ_4K);
+	mpmu_base = ioremap(mpmu_phys, SZ_4K);
 	if (mpmu_base == NULL) {
 		pr_err("error to ioremap MPMU base\n");
 		return;
 	}
 
-	apmu_base = ioremap(AXI_PHYS_BASE + 0x82800, SZ_4K);
+	apmu_base = ioremap(apmu_phys, SZ_4K);
 	if (apmu_base == NULL) {
 		pr_err("error to ioremap APMU base\n");
 		return;
 	}
 
-	apbcp_base = ioremap(APB_PHYS_BASE + 0x3b000, SZ_4K);
+	apbcp_base = ioremap(apbcp_phys, SZ_4K);
 	if (apbcp_base == NULL) {
 		pr_err("error to ioremap APBC extension base\n");
 		return;
 	}
 
-	apbc_base = ioremap(APB_PHYS_BASE + 0x15000, SZ_4K);
+	apbc_base = ioremap(apbc_phys, SZ_4K);
 	if (apbc_base == NULL) {
 		pr_err("error to ioremap APBC base\n");
 		return;
diff --git a/drivers/clk/mvebu/Makefile b/drivers/clk/mvebu/Makefile
index 645ac7ea3565..8866115486f7 100644
--- a/drivers/clk/mvebu/Makefile
+++ b/drivers/clk/mvebu/Makefile
@@ -7,6 +7,6 @@ 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_XP_CLK)	+= armada-xp.o
-obj-$(CONFIG_DOVE_CLK)		+= dove.o
+obj-$(CONFIG_DOVE_CLK)		+= dove.o dove-divider.o
 obj-$(CONFIG_KIRKWOOD_CLK)	+= kirkwood.o
 obj-$(CONFIG_ORION_CLK)		+= orion.o
diff --git a/drivers/clk/mvebu/dove-divider.c b/drivers/clk/mvebu/dove-divider.c
new file mode 100644
index 000000000000..3e0b52daa35f
--- /dev/null
+++ b/drivers/clk/mvebu/dove-divider.c
@@ -0,0 +1,262 @@
+/*
+ * Marvell Dove PMU Core PLL divider driver
+ *
+ * Cleaned up by substantially rewriting, and converted to DT by
+ * Russell King.  Origin is not known.
+ */
+#include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+#include "dove-divider.h"
+
+struct dove_clk {
+	const char *name;
+	struct clk_hw hw;
+	void __iomem *base;
+	spinlock_t *lock;
+	u8 div_bit_start;
+	u8 div_bit_end;
+	u8 div_bit_load;
+	u8 div_bit_size;
+	u32 *divider_table;
+};
+
+enum {
+	DIV_CTRL0 = 0,
+	DIV_CTRL1 = 4,
+	DIV_CTRL1_N_RESET_MASK = BIT(10),
+};
+
+#define to_dove_clk(hw) container_of(hw, struct dove_clk, hw)
+
+static void dove_load_divider(void __iomem *base, u32 val, u32 mask, u32 load)
+{
+	u32 v;
+
+	v = readl_relaxed(base + DIV_CTRL1) | DIV_CTRL1_N_RESET_MASK;
+	writel_relaxed(v, base + DIV_CTRL1);
+
+	v = (readl_relaxed(base + DIV_CTRL0) & ~(mask | load)) | val;
+	writel_relaxed(v, base + DIV_CTRL0);
+	writel_relaxed(v | load, base + DIV_CTRL0);
+	ndelay(250);
+	writel_relaxed(v, base + DIV_CTRL0);
+}
+
+static unsigned int dove_get_divider(struct dove_clk *dc)
+{
+	unsigned int divider;
+	u32 val;
+
+	val = readl_relaxed(dc->base + DIV_CTRL0);
+	val >>= dc->div_bit_start;
+
+	divider = val & ~(~0 << dc->div_bit_size);
+
+	if (dc->divider_table)
+		divider = dc->divider_table[divider];
+
+	return divider;
+}
+
+static int dove_calc_divider(const struct dove_clk *dc, unsigned long rate,
+			     unsigned long parent_rate, bool set)
+{
+	unsigned int divider, max;
+
+	divider = DIV_ROUND_CLOSEST(parent_rate, rate);
+
+	if (dc->divider_table) {
+		unsigned int i;
+
+		for (i = 0; dc->divider_table[i]; i++)
+			if (divider == dc->divider_table[i]) {
+				divider = i;
+				break;
+			}
+
+		if (!dc->divider_table[i])
+			return -EINVAL;
+	} else {
+		max = 1 << dc->div_bit_size;
+
+		if (set && (divider == 0 || divider >= max))
+			return -EINVAL;
+		if (divider >= max)
+			divider = max - 1;
+		else if (divider == 0)
+			divider = 1;
+	}
+
+	return divider;
+}
+
+static unsigned long dove_recalc_rate(struct clk_hw *hw, unsigned long parent)
+{
+	struct dove_clk *dc = to_dove_clk(hw);
+	unsigned int divider = dove_get_divider(dc);
+	unsigned long rate = DIV_ROUND_CLOSEST(parent, divider);
+
+	pr_debug("%s(): %s divider=%u parent=%lu rate=%lu\n",
+		 __func__, dc->name, divider, parent, rate);
+
+	return rate;
+}
+
+static long dove_round_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long *parent)
+{
+	struct dove_clk *dc = to_dove_clk(hw);
+	unsigned long parent_rate = *parent;
+	int divider;
+
+	divider = dove_calc_divider(dc, rate, parent_rate, false);
+	if (divider < 0)
+		return divider;
+
+	rate = DIV_ROUND_CLOSEST(parent_rate, divider);
+
+	pr_debug("%s(): %s divider=%u parent=%lu rate=%lu\n",
+		 __func__, dc->name, divider, parent_rate, rate);
+
+	return rate;
+}
+
+static int dove_set_clock(struct clk_hw *hw, unsigned long rate,
+			  unsigned long parent_rate)
+{
+	struct dove_clk *dc = to_dove_clk(hw);
+	u32 mask, load, div;
+	int divider;
+
+	divider = dove_calc_divider(dc, rate, parent_rate, true);
+	if (divider < 0)
+		return divider;
+
+	pr_debug("%s(): %s divider=%u parent=%lu rate=%lu\n",
+		 __func__, dc->name, divider, parent_rate, rate);
+
+	div = (u32)divider << dc->div_bit_start;
+	mask = ~(~0 << dc->div_bit_size) << dc->div_bit_start;
+	load = BIT(dc->div_bit_load);
+
+	spin_lock(dc->lock);
+	dove_load_divider(dc->base, div, mask, load);
+	spin_unlock(dc->lock);
+
+	return 0;
+}
+
+static const struct clk_ops dove_divider_ops = {
+	.set_rate	= dove_set_clock,
+	.round_rate	= dove_round_rate,
+	.recalc_rate	= dove_recalc_rate,
+};
+
+static struct clk *clk_register_dove_divider(struct device *dev,
+	struct dove_clk *dc, const char **parent_names, size_t num_parents,
+	void __iomem *base)
+{
+	char name[32];
+	struct clk_init_data init = {
+		.name = name,
+		.ops = &dove_divider_ops,
+		.parent_names = parent_names,
+		.num_parents = num_parents,
+	};
+
+	strlcpy(name, dc->name, sizeof(name));
+
+	dc->hw.init = &init;
+	dc->base = base;
+	dc->div_bit_size = dc->div_bit_end - dc->div_bit_start + 1;
+
+	return clk_register(dev, &dc->hw);
+}
+
+static DEFINE_SPINLOCK(dove_divider_lock);
+
+static u32 axi_divider[] = {-1, 2, 1, 3, 4, 6, 5, 7, 8, 10, 9, 0};
+
+static struct dove_clk dove_hw_clocks[4] = {
+	{
+		.name = "axi",
+		.lock = &dove_divider_lock,
+		.div_bit_start = 1,
+		.div_bit_end = 6,
+		.div_bit_load = 7,
+		.divider_table = axi_divider,
+	}, {
+		.name = "gpu",
+		.lock = &dove_divider_lock,
+		.div_bit_start = 8,
+		.div_bit_end = 13,
+		.div_bit_load = 14,
+	}, {
+		.name = "vmeta",
+		.lock = &dove_divider_lock,
+		.div_bit_start = 15,
+		.div_bit_end = 20,
+		.div_bit_load = 21,
+	}, {
+		.name = "lcd",
+		.lock = &dove_divider_lock,
+		.div_bit_start = 22,
+		.div_bit_end = 27,
+		.div_bit_load = 28,
+	},
+};
+
+static const char *core_pll[] = {
+	"core-pll",
+};
+
+static int dove_divider_init(struct device *dev, void __iomem *base,
+	struct clk **clks)
+{
+	struct clk *clk;
+	int i;
+
+	/*
+	 * Create the core PLL clock.  We treat this as a fixed rate
+	 * clock as we don't know any better, and documentation is sparse.
+	 */
+	clk = clk_register_fixed_rate(dev, core_pll[0], NULL, CLK_IS_ROOT,
+				      2000000000UL);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	for (i = 0; i < ARRAY_SIZE(dove_hw_clocks); i++)
+		clks[i] = clk_register_dove_divider(dev, &dove_hw_clocks[i],
+						    core_pll,
+						    ARRAY_SIZE(core_pll), base);
+
+	return 0;
+}
+
+static struct clk *dove_divider_clocks[4];
+
+static struct clk_onecell_data dove_divider_data = {
+	.clks = dove_divider_clocks,
+	.clk_num = ARRAY_SIZE(dove_divider_clocks),
+};
+
+void __init dove_divider_clk_init(struct device_node *np)
+{
+	void __iomem *base;
+
+	base = of_iomap(np, 0);
+	if (WARN_ON(!base))
+		return;
+
+	if (WARN_ON(dove_divider_init(NULL, base, dove_divider_clocks))) {
+		iounmap(base);
+		return;
+	}
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &dove_divider_data);
+}
diff --git a/drivers/clk/mvebu/dove-divider.h b/drivers/clk/mvebu/dove-divider.h
new file mode 100644
index 000000000000..4f2f718deb8e
--- /dev/null
+++ b/drivers/clk/mvebu/dove-divider.h
@@ -0,0 +1,6 @@
+#ifndef DOVE_DIVIDER_H
+#define DOVE_DIVIDER_H
+
+void __init dove_divider_clk_init(struct device_node *np);
+
+#endif
diff --git a/drivers/clk/mvebu/dove.c b/drivers/clk/mvebu/dove.c
index b8c2424ac926..59fad9546c84 100644
--- a/drivers/clk/mvebu/dove.c
+++ b/drivers/clk/mvebu/dove.c
@@ -17,6 +17,7 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include "common.h"
+#include "dove-divider.h"
 
 /*
  * Core Clocks
@@ -184,9 +185,14 @@ static void __init dove_clk_init(struct device_node *np)
 {
 	struct device_node *cgnp =
 		of_find_compatible_node(NULL, NULL, "marvell,dove-gating-clock");
+	struct device_node *ddnp =
+		of_find_compatible_node(NULL, NULL, "marvell,dove-divider-clock");
 
 	mvebu_coreclk_setup(np, &dove_coreclks);
 
+	if (ddnp)
+		dove_divider_clk_init(ddnp);
+
 	if (cgnp)
 		mvebu_clk_gating_setup(cgnp, dove_gating_desc);
 }
diff --git a/drivers/clk/nxp/Makefile b/drivers/clk/nxp/Makefile
index 7f608b0ad7b4..607bd48c6563 100644
--- a/drivers/clk/nxp/Makefile
+++ b/drivers/clk/nxp/Makefile
@@ -1,2 +1,3 @@
 obj-$(CONFIG_ARCH_LPC18XX)	+= clk-lpc18xx-cgu.o
 obj-$(CONFIG_ARCH_LPC18XX)	+= clk-lpc18xx-ccu.o
+obj-$(CONFIG_ARCH_LPC32XX)	+= clk-lpc32xx.o
diff --git a/drivers/clk/nxp/clk-lpc32xx.c b/drivers/clk/nxp/clk-lpc32xx.c
new file mode 100644
index 000000000000..10dd0fdaa474
--- /dev/null
+++ b/drivers/clk/nxp/clk-lpc32xx.c
@@ -0,0 +1,1569 @@
+/*
+ * Copyright 2015 Vladimir Zapolskiy <vz@mleia.com>
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/of_address.h>
+#include <linux/regmap.h>
+
+#include <dt-bindings/clock/lpc32xx-clock.h>
+
+#undef pr_fmt
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
+/* Common bitfield definitions for x397 PLL (lock), USB PLL and HCLK PLL */
+#define PLL_CTRL_ENABLE			BIT(16)
+#define PLL_CTRL_BYPASS			BIT(15)
+#define PLL_CTRL_DIRECT			BIT(14)
+#define PLL_CTRL_FEEDBACK		BIT(13)
+#define PLL_CTRL_POSTDIV		(BIT(12)|BIT(11))
+#define PLL_CTRL_PREDIV			(BIT(10)|BIT(9))
+#define PLL_CTRL_FEEDDIV		(0xFF << 1)
+#define PLL_CTRL_LOCK			BIT(0)
+
+/* Clock registers on System Control Block */
+#define LPC32XX_CLKPWR_DEBUG_CTRL	0x00
+#define LPC32XX_CLKPWR_USB_DIV		0x1C
+#define LPC32XX_CLKPWR_HCLKDIV_CTRL	0x40
+#define LPC32XX_CLKPWR_PWR_CTRL		0x44
+#define LPC32XX_CLKPWR_PLL397_CTRL	0x48
+#define LPC32XX_CLKPWR_OSC_CTRL		0x4C
+#define LPC32XX_CLKPWR_SYSCLK_CTRL	0x50
+#define LPC32XX_CLKPWR_LCDCLK_CTRL	0x54
+#define LPC32XX_CLKPWR_HCLKPLL_CTRL	0x58
+#define LPC32XX_CLKPWR_ADCCLK_CTRL1	0x60
+#define LPC32XX_CLKPWR_USB_CTRL		0x64
+#define LPC32XX_CLKPWR_SSP_CTRL		0x78
+#define LPC32XX_CLKPWR_I2S_CTRL		0x7C
+#define LPC32XX_CLKPWR_MS_CTRL		0x80
+#define LPC32XX_CLKPWR_MACCLK_CTRL	0x90
+#define LPC32XX_CLKPWR_TEST_CLK_CTRL	0xA4
+#define LPC32XX_CLKPWR_I2CCLK_CTRL	0xAC
+#define LPC32XX_CLKPWR_KEYCLK_CTRL	0xB0
+#define LPC32XX_CLKPWR_ADCCLK_CTRL	0xB4
+#define LPC32XX_CLKPWR_PWMCLK_CTRL	0xB8
+#define LPC32XX_CLKPWR_TIMCLK_CTRL	0xBC
+#define LPC32XX_CLKPWR_TIMCLK_CTRL1	0xC0
+#define LPC32XX_CLKPWR_SPI_CTRL		0xC4
+#define LPC32XX_CLKPWR_FLASHCLK_CTRL	0xC8
+#define LPC32XX_CLKPWR_UART3_CLK_CTRL	0xD0
+#define LPC32XX_CLKPWR_UART4_CLK_CTRL	0xD4
+#define LPC32XX_CLKPWR_UART5_CLK_CTRL	0xD8
+#define LPC32XX_CLKPWR_UART6_CLK_CTRL	0xDC
+#define LPC32XX_CLKPWR_IRDA_CLK_CTRL	0xE0
+#define LPC32XX_CLKPWR_UART_CLK_CTRL	0xE4
+#define LPC32XX_CLKPWR_DMA_CLK_CTRL	0xE8
+
+/* Clock registers on USB controller */
+#define LPC32XX_USB_CLK_CTRL		0xF4
+#define LPC32XX_USB_CLK_STS		0xF8
+
+static struct regmap_config lpc32xx_scb_regmap_config = {
+	.reg_bits = 32,
+	.val_bits = 32,
+	.reg_stride = 4,
+	.val_format_endian = REGMAP_ENDIAN_LITTLE,
+	.max_register = 0x114,
+	.fast_io = true,
+};
+
+static struct regmap *clk_regmap;
+static void __iomem *usb_clk_vbase;
+
+enum {
+	LPC32XX_USB_CLK_OTG = LPC32XX_USB_CLK_HOST + 1,
+	LPC32XX_USB_CLK_AHB,
+
+	LPC32XX_USB_CLK_MAX = LPC32XX_USB_CLK_AHB + 1,
+};
+
+enum {
+	/* Start from the last defined clock in dt bindings */
+	LPC32XX_CLK_ADC_DIV = LPC32XX_CLK_ADC + 1,
+	LPC32XX_CLK_ADC_RTC,
+	LPC32XX_CLK_TEST1,
+	LPC32XX_CLK_TEST2,
+
+	/* System clocks, PLL 397x and HCLK PLL clocks */
+	LPC32XX_CLK_OSC,
+	LPC32XX_CLK_SYS,
+	LPC32XX_CLK_PLL397X,
+	LPC32XX_CLK_HCLK_PLL,
+	LPC32XX_CLK_HCLK_DIV_PERIPH,
+	LPC32XX_CLK_HCLK_DIV,
+	LPC32XX_CLK_HCLK,
+	LPC32XX_CLK_PERIPH,
+	LPC32XX_CLK_ARM,
+	LPC32XX_CLK_ARM_VFP,
+
+	/* USB clocks */
+	LPC32XX_CLK_USB_PLL,
+	LPC32XX_CLK_USB_DIV,
+	LPC32XX_CLK_USB,
+
+	/* Only one control PWR_CTRL[10] for both muxes */
+	LPC32XX_CLK_PERIPH_HCLK_MUX,
+	LPC32XX_CLK_PERIPH_ARM_MUX,
+
+	/* Only one control PWR_CTRL[2] for all three muxes */
+	LPC32XX_CLK_SYSCLK_PERIPH_MUX,
+	LPC32XX_CLK_SYSCLK_HCLK_MUX,
+	LPC32XX_CLK_SYSCLK_ARM_MUX,
+
+	/* Two clock sources external to the driver */
+	LPC32XX_CLK_XTAL_32K,
+	LPC32XX_CLK_XTAL,
+
+	/* Renumbered USB clocks, may have a parent from SCB table */
+	LPC32XX_CLK_USB_OFFSET,
+	LPC32XX_CLK_USB_I2C = LPC32XX_USB_CLK_I2C + LPC32XX_CLK_USB_OFFSET,
+	LPC32XX_CLK_USB_DEV = LPC32XX_USB_CLK_DEVICE + LPC32XX_CLK_USB_OFFSET,
+	LPC32XX_CLK_USB_HOST = LPC32XX_USB_CLK_HOST + LPC32XX_CLK_USB_OFFSET,
+	LPC32XX_CLK_USB_OTG = LPC32XX_USB_CLK_OTG + LPC32XX_CLK_USB_OFFSET,
+	LPC32XX_CLK_USB_AHB = LPC32XX_USB_CLK_AHB + LPC32XX_CLK_USB_OFFSET,
+
+	/* Stub for composite clocks */
+	LPC32XX_CLK__NULL,
+
+	/* Subclocks of composite clocks, clocks above are for CCF */
+	LPC32XX_CLK_PWM1_MUX,
+	LPC32XX_CLK_PWM1_DIV,
+	LPC32XX_CLK_PWM1_GATE,
+	LPC32XX_CLK_PWM2_MUX,
+	LPC32XX_CLK_PWM2_DIV,
+	LPC32XX_CLK_PWM2_GATE,
+	LPC32XX_CLK_UART3_MUX,
+	LPC32XX_CLK_UART3_DIV,
+	LPC32XX_CLK_UART3_GATE,
+	LPC32XX_CLK_UART4_MUX,
+	LPC32XX_CLK_UART4_DIV,
+	LPC32XX_CLK_UART4_GATE,
+	LPC32XX_CLK_UART5_MUX,
+	LPC32XX_CLK_UART5_DIV,
+	LPC32XX_CLK_UART5_GATE,
+	LPC32XX_CLK_UART6_MUX,
+	LPC32XX_CLK_UART6_DIV,
+	LPC32XX_CLK_UART6_GATE,
+	LPC32XX_CLK_TEST1_MUX,
+	LPC32XX_CLK_TEST1_GATE,
+	LPC32XX_CLK_TEST2_MUX,
+	LPC32XX_CLK_TEST2_GATE,
+	LPC32XX_CLK_USB_DIV_DIV,
+	LPC32XX_CLK_USB_DIV_GATE,
+	LPC32XX_CLK_SD_DIV,
+	LPC32XX_CLK_SD_GATE,
+	LPC32XX_CLK_LCD_DIV,
+	LPC32XX_CLK_LCD_GATE,
+
+	LPC32XX_CLK_HW_MAX,
+	LPC32XX_CLK_MAX = LPC32XX_CLK_SYSCLK_ARM_MUX + 1,
+	LPC32XX_CLK_CCF_MAX = LPC32XX_CLK_USB_AHB + 1,
+};
+
+static struct clk *clk[LPC32XX_CLK_MAX];
+static struct clk_onecell_data clk_data = {
+	.clks = clk,
+	.clk_num = LPC32XX_CLK_MAX,
+};
+
+static struct clk *usb_clk[LPC32XX_USB_CLK_MAX];
+static struct clk_onecell_data usb_clk_data = {
+	.clks = usb_clk,
+	.clk_num = LPC32XX_USB_CLK_MAX,
+};
+
+#define LPC32XX_CLK_PARENTS_MAX			5
+
+struct clk_proto_t {
+	const char *name;
+	const u8 parents[LPC32XX_CLK_PARENTS_MAX];
+	u8 num_parents;
+	unsigned long flags;
+};
+
+#define CLK_PREFIX(LITERAL)		LPC32XX_CLK_ ## LITERAL
+#define NUMARGS(...)	(sizeof((int[]){__VA_ARGS__})/sizeof(int))
+
+#define LPC32XX_CLK_DEFINE(_idx, _name, _flags, ...)		\
+	[CLK_PREFIX(_idx)] = {					\
+		.name = _name,					\
+		.flags = _flags,				\
+		.parents = { __VA_ARGS__ },			\
+		.num_parents = NUMARGS(__VA_ARGS__),		\
+	 }
+
+static const struct clk_proto_t clk_proto[LPC32XX_CLK_CCF_MAX] __initconst = {
+	LPC32XX_CLK_DEFINE(XTAL, "xtal", 0x0),
+	LPC32XX_CLK_DEFINE(XTAL_32K, "xtal_32k", 0x0),
+
+	LPC32XX_CLK_DEFINE(RTC, "rtc", 0x0, LPC32XX_CLK_XTAL_32K),
+	LPC32XX_CLK_DEFINE(OSC, "osc", CLK_IGNORE_UNUSED, LPC32XX_CLK_XTAL),
+	LPC32XX_CLK_DEFINE(SYS, "sys", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_OSC, LPC32XX_CLK_PLL397X),
+	LPC32XX_CLK_DEFINE(PLL397X, "pll_397x", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_RTC),
+	LPC32XX_CLK_DEFINE(HCLK_PLL, "hclk_pll", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYS),
+	LPC32XX_CLK_DEFINE(HCLK_DIV_PERIPH, "hclk_div_periph",
+		CLK_IGNORE_UNUSED, LPC32XX_CLK_HCLK_PLL),
+	LPC32XX_CLK_DEFINE(HCLK_DIV, "hclk_div", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_HCLK_PLL),
+	LPC32XX_CLK_DEFINE(HCLK, "hclk", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_PERIPH_HCLK_MUX),
+	LPC32XX_CLK_DEFINE(PERIPH, "pclk", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYSCLK_PERIPH_MUX),
+	LPC32XX_CLK_DEFINE(ARM, "arm", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_PERIPH_ARM_MUX),
+
+	LPC32XX_CLK_DEFINE(PERIPH_HCLK_MUX, "periph_hclk_mux",
+		CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYSCLK_HCLK_MUX, LPC32XX_CLK_SYSCLK_PERIPH_MUX),
+	LPC32XX_CLK_DEFINE(PERIPH_ARM_MUX, "periph_arm_mux", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYSCLK_ARM_MUX, LPC32XX_CLK_SYSCLK_PERIPH_MUX),
+	LPC32XX_CLK_DEFINE(SYSCLK_PERIPH_MUX, "sysclk_periph_mux",
+		CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYS, LPC32XX_CLK_HCLK_DIV_PERIPH),
+	LPC32XX_CLK_DEFINE(SYSCLK_HCLK_MUX, "sysclk_hclk_mux",
+		CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYS, LPC32XX_CLK_HCLK_DIV),
+	LPC32XX_CLK_DEFINE(SYSCLK_ARM_MUX, "sysclk_arm_mux", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_SYS, LPC32XX_CLK_HCLK_PLL),
+
+	LPC32XX_CLK_DEFINE(ARM_VFP, "vfp9", CLK_IGNORE_UNUSED,
+		LPC32XX_CLK_ARM),
+	LPC32XX_CLK_DEFINE(USB_PLL, "usb_pll",
+		CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT, LPC32XX_CLK_USB_DIV),
+	LPC32XX_CLK_DEFINE(USB_DIV, "usb_div", 0x0, LPC32XX_CLK_OSC),
+	LPC32XX_CLK_DEFINE(USB, "usb", 0x0, LPC32XX_CLK_USB_PLL),
+	LPC32XX_CLK_DEFINE(DMA, "dma", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(MLC, "mlc", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(SLC, "slc", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(LCD, "lcd", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(MAC, "mac", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(SD, "sd", 0x0, LPC32XX_CLK_ARM),
+	LPC32XX_CLK_DEFINE(DDRAM, "ddram", CLK_GET_RATE_NOCACHE,
+		LPC32XX_CLK_SYSCLK_ARM_MUX),
+	LPC32XX_CLK_DEFINE(SSP0, "ssp0", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(SSP1, "ssp1", 0x0, LPC32XX_CLK_HCLK),
+
+	/*
+	 * CLK_GET_RATE_NOCACHE is needed, if UART clock is disabled, its
+	 * divider register does not contain information about selected rate.
+	 */
+	LPC32XX_CLK_DEFINE(UART3, "uart3", CLK_GET_RATE_NOCACHE,
+		LPC32XX_CLK_PERIPH, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(UART4, "uart4", CLK_GET_RATE_NOCACHE,
+		LPC32XX_CLK_PERIPH, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(UART5, "uart5", CLK_GET_RATE_NOCACHE,
+		LPC32XX_CLK_PERIPH, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(UART6, "uart6", CLK_GET_RATE_NOCACHE,
+		LPC32XX_CLK_PERIPH, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(IRDA, "irda", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(I2C1, "i2c1", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(I2C2, "i2c2", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(TIMER0, "timer0", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(TIMER1, "timer1", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(TIMER2, "timer2", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(TIMER3, "timer3", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(TIMER4, "timer4", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(TIMER5, "timer5", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(WDOG, "watchdog", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(I2S0, "i2s0", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(I2S1, "i2s1", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(SPI1, "spi1", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(SPI2, "spi2", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(MCPWM, "mcpwm", 0x0, LPC32XX_CLK_HCLK),
+	LPC32XX_CLK_DEFINE(HSTIMER, "hstimer", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(KEY, "key", 0x0, LPC32XX_CLK_RTC),
+	LPC32XX_CLK_DEFINE(PWM1, "pwm1", 0x0,
+		LPC32XX_CLK_RTC, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(PWM2, "pwm2", 0x0,
+		LPC32XX_CLK_RTC, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(ADC, "adc", 0x0,
+		LPC32XX_CLK_ADC_RTC, LPC32XX_CLK_ADC_DIV),
+	LPC32XX_CLK_DEFINE(ADC_DIV, "adc_div", 0x0, LPC32XX_CLK_PERIPH),
+	LPC32XX_CLK_DEFINE(ADC_RTC, "adc_rtc", 0x0, LPC32XX_CLK_RTC),
+	LPC32XX_CLK_DEFINE(TEST1, "test1", 0x0,
+		LPC32XX_CLK_PERIPH, LPC32XX_CLK_RTC, LPC32XX_CLK_OSC),
+	LPC32XX_CLK_DEFINE(TEST2, "test2", 0x0,
+		LPC32XX_CLK_HCLK, LPC32XX_CLK_PERIPH, LPC32XX_CLK_USB,
+		LPC32XX_CLK_OSC, LPC32XX_CLK_PLL397X),
+
+	/* USB controller clocks */
+	LPC32XX_CLK_DEFINE(USB_AHB, "usb_ahb", 0x0, LPC32XX_CLK_USB),
+	LPC32XX_CLK_DEFINE(USB_OTG, "usb_otg", 0x0, LPC32XX_CLK_USB_AHB),
+	LPC32XX_CLK_DEFINE(USB_I2C, "usb_i2c", 0x0, LPC32XX_CLK_USB_AHB),
+	LPC32XX_CLK_DEFINE(USB_DEV, "usb_dev", 0x0, LPC32XX_CLK_USB_OTG),
+	LPC32XX_CLK_DEFINE(USB_HOST, "usb_host", 0x0, LPC32XX_CLK_USB_OTG),
+};
+
+struct lpc32xx_clk {
+	struct clk_hw hw;
+	u32 reg;
+	u32 enable;
+	u32 enable_mask;
+	u32 disable;
+	u32 disable_mask;
+	u32 busy;
+	u32 busy_mask;
+};
+
+enum clk_pll_mode {
+	PLL_UNKNOWN,
+	PLL_DIRECT,
+	PLL_BYPASS,
+	PLL_DIRECT_BYPASS,
+	PLL_INTEGER,
+	PLL_NON_INTEGER,
+};
+
+struct lpc32xx_pll_clk {
+	struct clk_hw hw;
+	u32 reg;
+	u32 enable;
+	unsigned long m_div;
+	unsigned long n_div;
+	unsigned long p_div;
+	enum clk_pll_mode mode;
+};
+
+struct lpc32xx_usb_clk {
+	struct clk_hw hw;
+	u32 ctrl_enable;
+	u32 ctrl_disable;
+	u32 ctrl_mask;
+	u32 enable;
+	u32 busy;
+};
+
+struct lpc32xx_clk_mux {
+	struct clk_hw	hw;
+	u32		reg;
+	u32		mask;
+	u8		shift;
+	u32		*table;
+	u8		flags;
+};
+
+struct lpc32xx_clk_div {
+	struct clk_hw	hw;
+	u32		reg;
+	u8		shift;
+	u8		width;
+	const struct clk_div_table	*table;
+	u8		flags;
+};
+
+struct lpc32xx_clk_gate {
+	struct clk_hw	hw;
+	u32		reg;
+	u8		bit_idx;
+	u8		flags;
+};
+
+#define to_lpc32xx_clk(_hw)	container_of(_hw, struct lpc32xx_clk, hw)
+#define to_lpc32xx_pll_clk(_hw)	container_of(_hw, struct lpc32xx_pll_clk, hw)
+#define to_lpc32xx_usb_clk(_hw)	container_of(_hw, struct lpc32xx_usb_clk, hw)
+#define to_lpc32xx_mux(_hw)	container_of(_hw, struct lpc32xx_clk_mux, hw)
+#define to_lpc32xx_div(_hw)	container_of(_hw, struct lpc32xx_clk_div, hw)
+#define to_lpc32xx_gate(_hw)	container_of(_hw, struct lpc32xx_clk_gate, hw)
+
+static inline bool pll_is_valid(u64 val0, u64 val1, u64 min, u64 max)
+{
+	return (val0 >= (val1 * min) && val0 <= (val1 * max));
+}
+
+static inline u32 lpc32xx_usb_clk_read(struct lpc32xx_usb_clk *clk)
+{
+	return readl(usb_clk_vbase + LPC32XX_USB_CLK_STS);
+}
+
+static inline void lpc32xx_usb_clk_write(struct lpc32xx_usb_clk *clk, u32 val)
+{
+	writel(val, usb_clk_vbase + LPC32XX_USB_CLK_CTRL);
+}
+
+static int clk_mask_enable(struct clk_hw *hw)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+
+	if (clk->busy_mask && (val & clk->busy_mask) == clk->busy)
+		return -EBUSY;
+
+	return regmap_update_bits(clk_regmap, clk->reg,
+				  clk->enable_mask, clk->enable);
+}
+
+static void clk_mask_disable(struct clk_hw *hw)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+
+	regmap_update_bits(clk_regmap, clk->reg,
+			   clk->disable_mask, clk->disable);
+}
+
+static int clk_mask_is_enabled(struct clk_hw *hw)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+
+	return ((val & clk->enable_mask) == clk->enable);
+}
+
+static const struct clk_ops clk_mask_ops = {
+	.enable = clk_mask_enable,
+	.disable = clk_mask_disable,
+	.is_enabled = clk_mask_is_enabled,
+};
+
+static int clk_pll_enable(struct clk_hw *hw)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	u32 val, count;
+
+	regmap_update_bits(clk_regmap, clk->reg, clk->enable, clk->enable);
+
+	for (count = 0; count < 1000; count++) {
+		regmap_read(clk_regmap, clk->reg, &val);
+		if (val & PLL_CTRL_LOCK)
+			break;
+	}
+
+	if (val & PLL_CTRL_LOCK)
+		return 0;
+
+	return -ETIMEDOUT;
+}
+
+static void clk_pll_disable(struct clk_hw *hw)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+
+	regmap_update_bits(clk_regmap, clk->reg, clk->enable, 0x0);
+}
+
+static int clk_pll_is_enabled(struct clk_hw *hw)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	u32 val;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+
+	val &= clk->enable | PLL_CTRL_LOCK;
+	if (val == (clk->enable | PLL_CTRL_LOCK))
+		return 1;
+
+	return 0;
+}
+
+static unsigned long clk_pll_397x_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	return parent_rate * 397;
+}
+
+static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	bool is_direct, is_bypass, is_feedback;
+	unsigned long rate, cco_rate, ref_rate;
+	u32 val;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	is_direct = val & PLL_CTRL_DIRECT;
+	is_bypass = val & PLL_CTRL_BYPASS;
+	is_feedback = val & PLL_CTRL_FEEDBACK;
+
+	clk->m_div = ((val & PLL_CTRL_FEEDDIV) >> 1) + 1;
+	clk->n_div = ((val & PLL_CTRL_PREDIV) >> 9) + 1;
+	clk->p_div = ((val & PLL_CTRL_POSTDIV) >> 11) + 1;
+
+	if (is_direct && is_bypass) {
+		clk->p_div = 0;
+		clk->mode = PLL_DIRECT_BYPASS;
+		return parent_rate;
+	}
+	if (is_bypass) {
+		clk->mode = PLL_BYPASS;
+		return parent_rate / (1 << clk->p_div);
+	}
+	if (is_direct) {
+		clk->p_div = 0;
+		clk->mode = PLL_DIRECT;
+	}
+
+	ref_rate = parent_rate / clk->n_div;
+	rate = cco_rate = ref_rate * clk->m_div;
+
+	if (!is_direct) {
+		if (is_feedback) {
+			cco_rate *= (1 << clk->p_div);
+			clk->mode = PLL_INTEGER;
+		} else {
+			rate /= (1 << clk->p_div);
+			clk->mode = PLL_NON_INTEGER;
+		}
+	}
+
+	pr_debug("%s: %lu: 0x%x: %d/%d/%d, %lu/%lu/%d => %lu\n",
+		 clk_hw_get_name(hw),
+		 parent_rate, val, is_direct, is_bypass, is_feedback,
+		 clk->n_div, clk->m_div, (1 << clk->p_div), rate);
+
+	if (clk_pll_is_enabled(hw) &&
+	    !(pll_is_valid(parent_rate, 1, 1000000, 20000000)
+	      && pll_is_valid(cco_rate, 1, 156000000, 320000000)
+	      && pll_is_valid(ref_rate, 1, 1000000, 27000000)))
+		pr_err("%s: PLL clocks are not in valid ranges: %lu/%lu/%lu",
+		       clk_hw_get_name(hw),
+		       parent_rate, cco_rate, ref_rate);
+
+	return rate;
+}
+
+static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+			    unsigned long parent_rate)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	u32 val;
+	unsigned long new_rate;
+
+	/* Validate PLL clock parameters computed on round rate stage */
+	switch (clk->mode) {
+	case PLL_DIRECT:
+		val = PLL_CTRL_DIRECT;
+		val |= (clk->m_div - 1) << 1;
+		val |= (clk->n_div - 1) << 9;
+		new_rate = (parent_rate * clk->m_div) / clk->n_div;
+		break;
+	case PLL_BYPASS:
+		val = PLL_CTRL_BYPASS;
+		val |= (clk->p_div - 1) << 11;
+		new_rate = parent_rate / (1 << (clk->p_div));
+		break;
+	case PLL_DIRECT_BYPASS:
+		val = PLL_CTRL_DIRECT | PLL_CTRL_BYPASS;
+		new_rate = parent_rate;
+		break;
+	case PLL_INTEGER:
+		val = PLL_CTRL_FEEDBACK;
+		val |= (clk->m_div - 1) << 1;
+		val |= (clk->n_div - 1) << 9;
+		val |= (clk->p_div - 1) << 11;
+		new_rate = (parent_rate * clk->m_div) / clk->n_div;
+		break;
+	case PLL_NON_INTEGER:
+		val = 0x0;
+		val |= (clk->m_div - 1) << 1;
+		val |= (clk->n_div - 1) << 9;
+		val |= (clk->p_div - 1) << 11;
+		new_rate = (parent_rate * clk->m_div) /
+				(clk->n_div * (1 << clk->p_div));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Sanity check that round rate is equal to the requested one */
+	if (new_rate != rate)
+		return -EINVAL;
+
+	return regmap_update_bits(clk_regmap, clk->reg, 0x1FFFF, val);
+}
+
+static long clk_hclk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *parent_rate)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	u64 m_i, m, n, p, o = rate, i = *parent_rate, d = (u64)rate << 6;
+	int p_i, n_i;
+
+	pr_debug("%s: %lu/%lu\n", clk_hw_get_name(hw), *parent_rate, rate);
+
+	if (rate > 266500000)
+		return -EINVAL;
+
+	/* Have to check all 20 possibilities to find the minimal M */
+	for (p_i = 4; p_i >= 0; p_i--) {
+		for (n_i = 4; n_i > 0; n_i--) {
+			m_i = div64_u64(o * n_i * (1 << p_i), i);
+
+			/* Check for valid PLL parameter constraints */
+			if (!(m_i && m_i <= 256
+			      && pll_is_valid(i, n_i, 1000000, 27000000)
+			      && pll_is_valid(i * m_i * (1 << p_i), n_i,
+					      156000000, 320000000)))
+				continue;
+
+			/* Store some intermediate valid parameters */
+			if (o * n_i * (1 << p_i) - i * m_i <= d) {
+				m = m_i;
+				n = n_i;
+				p = p_i;
+				d = o * n_i * (1 << p_i) - i * m_i;
+			}
+		}
+	}
+
+	if (d == (u64)rate << 6) {
+		pr_err("%s: %lu: no valid PLL parameters are found\n",
+		       clk_hw_get_name(hw), rate);
+		return -EINVAL;
+	}
+
+	clk->m_div = m;
+	clk->n_div = n;
+	clk->p_div = p;
+
+	/* Set only direct or non-integer mode of PLL */
+	if (!p)
+		clk->mode = PLL_DIRECT;
+	else
+		clk->mode = PLL_NON_INTEGER;
+
+	o = div64_u64(i * m, n * (1 << p));
+
+	if (!d)
+		pr_debug("%s: %lu: found exact match: %llu/%llu/%llu\n",
+			 clk_hw_get_name(hw), rate, m, n, p);
+	else
+		pr_debug("%s: %lu: found closest: %llu/%llu/%llu - %llu\n",
+			 clk_hw_get_name(hw), rate, m, n, p, o);
+
+	return o;
+}
+
+static long clk_usb_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				   unsigned long *parent_rate)
+{
+	struct lpc32xx_pll_clk *clk = to_lpc32xx_pll_clk(hw);
+	struct clk_hw *usb_div_hw, *osc_hw;
+	u64 d_i, n_i, m, o;
+
+	pr_debug("%s: %lu/%lu\n", clk_hw_get_name(hw), *parent_rate, rate);
+
+	/*
+	 * The only supported USB clock is 48MHz, with PLL internal constraints
+	 * on Fclkin, Fcco and Fref this implies that Fcco must be 192MHz
+	 * and post-divider must be 4, this slightly simplifies calculation of
+	 * USB divider, USB PLL N and M parameters.
+	 */
+	if (rate != 48000000)
+		return -EINVAL;
+
+	/* USB divider clock */
+	usb_div_hw = clk_hw_get_parent_by_index(hw, 0);
+	if (!usb_div_hw)
+		return -EINVAL;
+
+	/* Main oscillator clock */
+	osc_hw = clk_hw_get_parent_by_index(usb_div_hw, 0);
+	if (!osc_hw)
+		return -EINVAL;
+	o = clk_hw_get_rate(osc_hw);	/* must be in range 1..20 MHz */
+
+	/* Check if valid USB divider and USB PLL parameters exists */
+	for (d_i = 16; d_i >= 1; d_i--) {
+		for (n_i = 1; n_i <= 4; n_i++) {
+			m = div64_u64(192000000 * d_i * n_i, o);
+			if (!(m && m <= 256
+			      && m * o == 192000000 * d_i * n_i
+			      && pll_is_valid(o, d_i, 1000000, 20000000)
+			      && pll_is_valid(o, d_i * n_i, 1000000, 27000000)))
+				continue;
+
+			clk->n_div = n_i;
+			clk->m_div = m;
+			clk->p_div = 2;
+			clk->mode = PLL_NON_INTEGER;
+			*parent_rate = div64_u64(o, d_i);
+
+			return rate;
+		}
+	}
+
+	return -EINVAL;
+}
+
+#define LPC32XX_DEFINE_PLL_OPS(_name, _rc, _sr, _rr)			\
+	static const struct clk_ops clk_ ##_name ## _ops = {		\
+		.enable = clk_pll_enable,				\
+		.disable = clk_pll_disable,				\
+		.is_enabled = clk_pll_is_enabled,			\
+		.recalc_rate = _rc,					\
+		.set_rate = _sr,					\
+		.round_rate = _rr,					\
+	}
+
+LPC32XX_DEFINE_PLL_OPS(pll_397x, clk_pll_397x_recalc_rate, NULL, NULL);
+LPC32XX_DEFINE_PLL_OPS(hclk_pll, clk_pll_recalc_rate,
+		       clk_pll_set_rate, clk_hclk_pll_round_rate);
+LPC32XX_DEFINE_PLL_OPS(usb_pll,  clk_pll_recalc_rate,
+		       clk_pll_set_rate, clk_usb_pll_round_rate);
+
+static int clk_ddram_is_enabled(struct clk_hw *hw)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	val &= clk->enable_mask | clk->busy_mask;
+
+	return (val == (BIT(7) | BIT(0)) ||
+		val == (BIT(8) | BIT(1)));
+}
+
+static int clk_ddram_enable(struct clk_hw *hw)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val, hclk_div;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	hclk_div = val & clk->busy_mask;
+
+	/*
+	 * DDRAM clock must be 2 times higher than HCLK,
+	 * this implies DDRAM clock can not be enabled,
+	 * if HCLK clock rate is equal to ARM clock rate
+	 */
+	if (hclk_div == 0x0 || hclk_div == (BIT(1) | BIT(0)))
+		return -EINVAL;
+
+	return regmap_update_bits(clk_regmap, clk->reg,
+				  clk->enable_mask, hclk_div << 7);
+}
+
+static unsigned long clk_ddram_recalc_rate(struct clk_hw *hw,
+					   unsigned long parent_rate)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val;
+
+	if (!clk_ddram_is_enabled(hw))
+		return 0;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	val &= clk->enable_mask;
+
+	return parent_rate / (val >> 7);
+}
+
+static const struct clk_ops clk_ddram_ops = {
+	.enable = clk_ddram_enable,
+	.disable = clk_mask_disable,
+	.is_enabled = clk_ddram_is_enabled,
+	.recalc_rate = clk_ddram_recalc_rate,
+};
+
+static unsigned long lpc32xx_clk_uart_recalc_rate(struct clk_hw *hw,
+						  unsigned long parent_rate)
+{
+	struct lpc32xx_clk *clk = to_lpc32xx_clk(hw);
+	u32 val, x, y;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	x = (val & 0xFF00) >> 8;
+	y = val & 0xFF;
+
+	if (x && y)
+		return (parent_rate * x) / y;
+	else
+		return 0;
+}
+
+static const struct clk_ops lpc32xx_uart_div_ops = {
+	.recalc_rate = lpc32xx_clk_uart_recalc_rate,
+};
+
+static const struct clk_div_table clk_hclk_div_table[] = {
+	{ .val = 0, .div = 1 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ },
+};
+
+static u32 test1_mux_table[] = { 0, 1, 2, };
+static u32 test2_mux_table[] = { 0, 1, 2, 5, 7, };
+
+static int clk_usb_enable(struct clk_hw *hw)
+{
+	struct lpc32xx_usb_clk *clk = to_lpc32xx_usb_clk(hw);
+	u32 val, ctrl_val, count;
+
+	pr_debug("%s: 0x%x\n", clk_hw_get_name(hw), clk->enable);
+
+	if (clk->ctrl_mask) {
+		regmap_read(clk_regmap, LPC32XX_CLKPWR_USB_CTRL, &ctrl_val);
+		regmap_update_bits(clk_regmap, LPC32XX_CLKPWR_USB_CTRL,
+				   clk->ctrl_mask, clk->ctrl_enable);
+	}
+
+	val = lpc32xx_usb_clk_read(clk);
+	if (clk->busy && (val & clk->busy) == clk->busy) {
+		if (clk->ctrl_mask)
+			regmap_write(clk_regmap, LPC32XX_CLKPWR_USB_CTRL,
+				     ctrl_val);
+		return -EBUSY;
+	}
+
+	val |= clk->enable;
+	lpc32xx_usb_clk_write(clk, val);
+
+	for (count = 0; count < 1000; count++) {
+		val = lpc32xx_usb_clk_read(clk);
+		if ((val & clk->enable) == clk->enable)
+			break;
+	}
+
+	if ((val & clk->enable) == clk->enable)
+		return 0;
+
+	if (clk->ctrl_mask)
+		regmap_write(clk_regmap, LPC32XX_CLKPWR_USB_CTRL, ctrl_val);
+
+	return -ETIMEDOUT;
+}
+
+static void clk_usb_disable(struct clk_hw *hw)
+{
+	struct lpc32xx_usb_clk *clk = to_lpc32xx_usb_clk(hw);
+	u32 val = lpc32xx_usb_clk_read(clk);
+
+	val &= ~clk->enable;
+	lpc32xx_usb_clk_write(clk, val);
+
+	if (clk->ctrl_mask)
+		regmap_update_bits(clk_regmap, LPC32XX_CLKPWR_USB_CTRL,
+				   clk->ctrl_mask, clk->ctrl_disable);
+}
+
+static int clk_usb_is_enabled(struct clk_hw *hw)
+{
+	struct lpc32xx_usb_clk *clk = to_lpc32xx_usb_clk(hw);
+	u32 ctrl_val, val;
+
+	if (clk->ctrl_mask) {
+		regmap_read(clk_regmap, LPC32XX_CLKPWR_USB_CTRL, &ctrl_val);
+		if ((ctrl_val & clk->ctrl_mask) != clk->ctrl_enable)
+			return 0;
+	}
+
+	val = lpc32xx_usb_clk_read(clk);
+
+	return ((val & clk->enable) == clk->enable);
+}
+
+static unsigned long clk_usb_i2c_recalc_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	return clk_get_rate(clk[LPC32XX_CLK_PERIPH]);
+}
+
+static const struct clk_ops clk_usb_ops = {
+	.enable = clk_usb_enable,
+	.disable = clk_usb_disable,
+	.is_enabled = clk_usb_is_enabled,
+};
+
+static const struct clk_ops clk_usb_i2c_ops = {
+	.enable = clk_usb_enable,
+	.disable = clk_usb_disable,
+	.is_enabled = clk_usb_is_enabled,
+	.recalc_rate = clk_usb_i2c_recalc_rate,
+};
+
+static int clk_gate_enable(struct clk_hw *hw)
+{
+	struct lpc32xx_clk_gate *clk = to_lpc32xx_gate(hw);
+	u32 mask = BIT(clk->bit_idx);
+	u32 val = (clk->flags & CLK_GATE_SET_TO_DISABLE ? 0x0 : mask);
+
+	return regmap_update_bits(clk_regmap, clk->reg, mask, val);
+}
+
+static void clk_gate_disable(struct clk_hw *hw)
+{
+	struct lpc32xx_clk_gate *clk = to_lpc32xx_gate(hw);
+	u32 mask = BIT(clk->bit_idx);
+	u32 val = (clk->flags & CLK_GATE_SET_TO_DISABLE ? mask : 0x0);
+
+	regmap_update_bits(clk_regmap, clk->reg, mask, val);
+}
+
+static int clk_gate_is_enabled(struct clk_hw *hw)
+{
+	struct lpc32xx_clk_gate *clk = to_lpc32xx_gate(hw);
+	u32 val;
+	bool is_set;
+
+	regmap_read(clk_regmap, clk->reg, &val);
+	is_set = val & BIT(clk->bit_idx);
+
+	return (clk->flags & CLK_GATE_SET_TO_DISABLE ? !is_set : is_set);
+}
+
+static const struct clk_ops lpc32xx_clk_gate_ops = {
+	.enable = clk_gate_enable,
+	.disable = clk_gate_disable,
+	.is_enabled = clk_gate_is_enabled,
+};
+
+#define div_mask(width)	((1 << (width)) - 1)
+
+static unsigned int _get_table_div(const struct clk_div_table *table,
+							unsigned int val)
+{
+	const struct clk_div_table *clkt;
+
+	for (clkt = table; clkt->div; clkt++)
+		if (clkt->val == val)
+			return clkt->div;
+	return 0;
+}
+
+static unsigned int _get_div(const struct clk_div_table *table,
+			     unsigned int val, unsigned long flags, u8 width)
+{
+	if (flags & CLK_DIVIDER_ONE_BASED)
+		return val;
+	if (table)
+		return _get_table_div(table, val);
+	return val + 1;
+}
+
+static unsigned long clk_divider_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct lpc32xx_clk_div *divider = to_lpc32xx_div(hw);
+	unsigned int val;
+
+	regmap_read(clk_regmap, divider->reg, &val);
+
+	val >>= divider->shift;
+	val &= div_mask(divider->width);
+
+	return divider_recalc_rate(hw, parent_rate, val, divider->table,
+				   divider->flags);
+}
+
+static long clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long *prate)
+{
+	struct lpc32xx_clk_div *divider = to_lpc32xx_div(hw);
+	unsigned int bestdiv;
+
+	/* if read only, just return current value */
+	if (divider->flags & CLK_DIVIDER_READ_ONLY) {
+		regmap_read(clk_regmap, divider->reg, &bestdiv);
+		bestdiv >>= divider->shift;
+		bestdiv &= div_mask(divider->width);
+		bestdiv = _get_div(divider->table, bestdiv, divider->flags,
+			divider->width);
+		return DIV_ROUND_UP(*prate, bestdiv);
+	}
+
+	return divider_round_rate(hw, rate, prate, divider->table,
+				  divider->width, divider->flags);
+}
+
+static int clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct lpc32xx_clk_div *divider = to_lpc32xx_div(hw);
+	unsigned int value;
+
+	value = divider_get_val(rate, parent_rate, divider->table,
+				divider->width, divider->flags);
+
+	return regmap_update_bits(clk_regmap, divider->reg,
+				  div_mask(divider->width) << divider->shift,
+				  value << divider->shift);
+}
+
+static const struct clk_ops lpc32xx_clk_divider_ops = {
+	.recalc_rate = clk_divider_recalc_rate,
+	.round_rate = clk_divider_round_rate,
+	.set_rate = clk_divider_set_rate,
+};
+
+static u8 clk_mux_get_parent(struct clk_hw *hw)
+{
+	struct lpc32xx_clk_mux *mux = to_lpc32xx_mux(hw);
+	u32 num_parents = clk_hw_get_num_parents(hw);
+	u32 val;
+
+	regmap_read(clk_regmap, mux->reg, &val);
+	val >>= mux->shift;
+	val &= mux->mask;
+
+	if (mux->table) {
+		u32 i;
+
+		for (i = 0; i < num_parents; i++)
+			if (mux->table[i] == val)
+				return i;
+		return -EINVAL;
+	}
+
+	if (val >= num_parents)
+		return -EINVAL;
+
+	return val;
+}
+
+static int clk_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct lpc32xx_clk_mux *mux = to_lpc32xx_mux(hw);
+
+	if (mux->table)
+		index = mux->table[index];
+
+	return regmap_update_bits(clk_regmap, mux->reg,
+			  mux->mask << mux->shift, index << mux->shift);
+}
+
+static const struct clk_ops lpc32xx_clk_mux_ro_ops = {
+	.get_parent = clk_mux_get_parent,
+};
+
+static const struct clk_ops lpc32xx_clk_mux_ops = {
+	.get_parent = clk_mux_get_parent,
+	.set_parent = clk_mux_set_parent,
+	.determine_rate = __clk_mux_determine_rate,
+};
+
+enum lpc32xx_clk_type {
+	CLK_FIXED,
+	CLK_MUX,
+	CLK_DIV,
+	CLK_GATE,
+	CLK_COMPOSITE,
+	CLK_LPC32XX,
+	CLK_LPC32XX_PLL,
+	CLK_LPC32XX_USB,
+};
+
+struct clk_hw_proto0 {
+	const struct clk_ops *ops;
+	union {
+		struct lpc32xx_pll_clk pll;
+		struct lpc32xx_clk clk;
+		struct lpc32xx_usb_clk usb_clk;
+		struct lpc32xx_clk_mux mux;
+		struct lpc32xx_clk_div div;
+		struct lpc32xx_clk_gate gate;
+	};
+};
+
+struct clk_hw_proto1 {
+	struct clk_hw_proto0 *mux;
+	struct clk_hw_proto0 *div;
+	struct clk_hw_proto0 *gate;
+};
+
+struct clk_hw_proto {
+	enum lpc32xx_clk_type type;
+
+	union {
+		struct clk_fixed_rate f;
+		struct clk_hw_proto0 hw0;
+		struct clk_hw_proto1 hw1;
+	};
+};
+
+#define LPC32XX_DEFINE_FIXED(_idx, _rate, _flags)			\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_FIXED,						\
+	{								\
+		.f = {							\
+			.fixed_rate = (_rate),				\
+			.flags = (_flags),				\
+		},							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_PLL(_idx, _name, _reg, _enable)			\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_LPC32XX_PLL,					\
+	{								\
+		.hw0 = {						\
+			.ops = &clk_ ##_name ## _ops,			\
+			{						\
+				.pll = {				\
+					.reg = LPC32XX_CLKPWR_ ## _reg,	\
+					.enable = (_enable),		\
+				},					\
+			},						\
+		},							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_MUX(_idx, _reg, _shift, _mask, _table, _flags)	\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_MUX,						\
+	{								\
+		.hw0 = {						\
+			.ops = (_flags & CLK_MUX_READ_ONLY ?		\
+				&lpc32xx_clk_mux_ro_ops :		\
+				&lpc32xx_clk_mux_ops),			\
+			{						\
+				.mux = {				\
+					.reg = LPC32XX_CLKPWR_ ## _reg,	\
+					.mask = (_mask),		\
+					.shift = (_shift),		\
+					.table = (_table),		\
+					.flags = (_flags),		\
+				},					\
+			},						\
+		},							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_DIV(_idx, _reg, _shift, _width, _table, _flags)	\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_DIV,						\
+	{								\
+		.hw0 = {						\
+			.ops = &lpc32xx_clk_divider_ops,		\
+			{						\
+				.div = {				\
+					.reg = LPC32XX_CLKPWR_ ## _reg,	\
+					.shift = (_shift),		\
+					.width = (_width),		\
+					.table = (_table),		\
+					.flags = (_flags),		\
+				 },					\
+			},						\
+		 },							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_GATE(_idx, _reg, _bit, _flags)			\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_GATE,						\
+	{								\
+		.hw0 = {						\
+			.ops = &lpc32xx_clk_gate_ops,			\
+			{						\
+				.gate = {				\
+					.reg = LPC32XX_CLKPWR_ ## _reg,	\
+					.bit_idx = (_bit),		\
+					.flags = (_flags),		\
+				},					\
+			},						\
+		},							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_CLK(_idx, _reg, _e, _em, _d, _dm, _b, _bm, _ops)	\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_LPC32XX,						\
+	{								\
+		.hw0 = {						\
+			.ops = &(_ops),					\
+			{						\
+				.clk = {				\
+					.reg = LPC32XX_CLKPWR_ ## _reg,	\
+					.enable = (_e),			\
+					.enable_mask = (_em),		\
+					.disable = (_d),		\
+					.disable_mask = (_dm),		\
+					.busy = (_b),			\
+					.busy_mask = (_bm),		\
+				},					\
+			},						\
+		},							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_USB(_idx, _ce, _cd, _cm, _e, _b, _ops)		\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_LPC32XX_USB,					\
+	{								\
+		.hw0 = {						\
+			.ops = &(_ops),					\
+			{						\
+				.usb_clk = {				\
+					.ctrl_enable = (_ce),		\
+					.ctrl_disable = (_cd),		\
+					.ctrl_mask = (_cm),		\
+					.enable = (_e),			\
+					.busy = (_b),			\
+				}					\
+			},						\
+		}							\
+	},								\
+}
+
+#define LPC32XX_DEFINE_COMPOSITE(_idx, _mux, _div, _gate)		\
+[CLK_PREFIX(_idx)] = {							\
+	.type = CLK_COMPOSITE,						\
+	{								\
+		.hw1 = {						\
+		.mux = (CLK_PREFIX(_mux) == LPC32XX_CLK__NULL ? NULL :	\
+			&clk_hw_proto[CLK_PREFIX(_mux)].hw0),		\
+		.div = (CLK_PREFIX(_div) == LPC32XX_CLK__NULL ? NULL :	\
+			&clk_hw_proto[CLK_PREFIX(_div)].hw0),		\
+		.gate = (CLK_PREFIX(_gate) == LPC32XX_CLK__NULL ? NULL :\
+			 &clk_hw_proto[CLK_PREFIX(_gate)].hw0),		\
+		},							\
+	},								\
+}
+
+static struct clk_hw_proto clk_hw_proto[LPC32XX_CLK_HW_MAX] = {
+	LPC32XX_DEFINE_FIXED(RTC, 32768, 0),
+	LPC32XX_DEFINE_PLL(PLL397X, pll_397x, HCLKPLL_CTRL, BIT(1)),
+	LPC32XX_DEFINE_PLL(HCLK_PLL, hclk_pll, HCLKPLL_CTRL, PLL_CTRL_ENABLE),
+	LPC32XX_DEFINE_PLL(USB_PLL, usb_pll, USB_CTRL, PLL_CTRL_ENABLE),
+	LPC32XX_DEFINE_GATE(OSC, OSC_CTRL, 0, CLK_GATE_SET_TO_DISABLE),
+	LPC32XX_DEFINE_GATE(USB, USB_CTRL, 18, 0),
+
+	LPC32XX_DEFINE_DIV(HCLK_DIV_PERIPH, HCLKDIV_CTRL, 2, 5, NULL,
+			   CLK_DIVIDER_READ_ONLY),
+	LPC32XX_DEFINE_DIV(HCLK_DIV, HCLKDIV_CTRL, 0, 2, clk_hclk_div_table,
+			   CLK_DIVIDER_READ_ONLY),
+
+	/* Register 3 read-only muxes with a single control PWR_CTRL[2] */
+	LPC32XX_DEFINE_MUX(SYSCLK_PERIPH_MUX, PWR_CTRL, 2, 0x1, NULL,
+			   CLK_MUX_READ_ONLY),
+	LPC32XX_DEFINE_MUX(SYSCLK_HCLK_MUX, PWR_CTRL, 2, 0x1, NULL,
+			   CLK_MUX_READ_ONLY),
+	LPC32XX_DEFINE_MUX(SYSCLK_ARM_MUX, PWR_CTRL, 2, 0x1, NULL,
+			   CLK_MUX_READ_ONLY),
+	/* Register 2 read-only muxes with a single control PWR_CTRL[10] */
+	LPC32XX_DEFINE_MUX(PERIPH_HCLK_MUX, PWR_CTRL, 10, 0x1, NULL,
+			   CLK_MUX_READ_ONLY),
+	LPC32XX_DEFINE_MUX(PERIPH_ARM_MUX, PWR_CTRL, 10, 0x1, NULL,
+			   CLK_MUX_READ_ONLY),
+
+	/* 3 always on gates with a single control PWR_CTRL[0] same as OSC */
+	LPC32XX_DEFINE_GATE(PERIPH, PWR_CTRL, 0, CLK_GATE_SET_TO_DISABLE),
+	LPC32XX_DEFINE_GATE(HCLK, PWR_CTRL, 0, CLK_GATE_SET_TO_DISABLE),
+	LPC32XX_DEFINE_GATE(ARM, PWR_CTRL, 0, CLK_GATE_SET_TO_DISABLE),
+
+	LPC32XX_DEFINE_GATE(ARM_VFP, DEBUG_CTRL, 4, 0),
+	LPC32XX_DEFINE_GATE(DMA, DMA_CLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_CLK(DDRAM, HCLKDIV_CTRL, 0x0, BIT(8) | BIT(7),
+		   0x0, BIT(8) | BIT(7), 0x0, BIT(1) | BIT(0), clk_ddram_ops),
+
+	LPC32XX_DEFINE_GATE(TIMER0, TIMCLK_CTRL1, 2, 0),
+	LPC32XX_DEFINE_GATE(TIMER1, TIMCLK_CTRL1, 3, 0),
+	LPC32XX_DEFINE_GATE(TIMER2, TIMCLK_CTRL1, 4, 0),
+	LPC32XX_DEFINE_GATE(TIMER3, TIMCLK_CTRL1, 5, 0),
+	LPC32XX_DEFINE_GATE(TIMER4, TIMCLK_CTRL1, 0, 0),
+	LPC32XX_DEFINE_GATE(TIMER5, TIMCLK_CTRL1, 1, 0),
+
+	LPC32XX_DEFINE_GATE(SSP0, SSP_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(SSP1, SSP_CTRL, 1, 0),
+	LPC32XX_DEFINE_GATE(SPI1, SPI_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(SPI2, SPI_CTRL, 4, 0),
+	LPC32XX_DEFINE_GATE(I2S0, I2S_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(I2S1, I2S_CTRL, 1, 0),
+	LPC32XX_DEFINE_GATE(I2C1, I2CCLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(I2C2, I2CCLK_CTRL, 1, 0),
+	LPC32XX_DEFINE_GATE(WDOG, TIMCLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(HSTIMER, TIMCLK_CTRL, 1, 0),
+
+	LPC32XX_DEFINE_GATE(KEY, KEYCLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_GATE(MCPWM, TIMCLK_CTRL1, 6, 0),
+
+	LPC32XX_DEFINE_MUX(PWM1_MUX, PWMCLK_CTRL, 1, 0x1, NULL, 0),
+	LPC32XX_DEFINE_DIV(PWM1_DIV, PWMCLK_CTRL, 4, 4, NULL,
+			   CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO),
+	LPC32XX_DEFINE_GATE(PWM1_GATE, PWMCLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_COMPOSITE(PWM1, PWM1_MUX, PWM1_DIV, PWM1_GATE),
+
+	LPC32XX_DEFINE_MUX(PWM2_MUX, PWMCLK_CTRL, 3, 0x1, NULL, 0),
+	LPC32XX_DEFINE_DIV(PWM2_DIV, PWMCLK_CTRL, 8, 4, NULL,
+			   CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO),
+	LPC32XX_DEFINE_GATE(PWM2_GATE, PWMCLK_CTRL, 2, 0),
+	LPC32XX_DEFINE_COMPOSITE(PWM2, PWM2_MUX, PWM2_DIV, PWM2_GATE),
+
+	LPC32XX_DEFINE_MUX(UART3_MUX, UART3_CLK_CTRL, 16, 0x1, NULL, 0),
+	LPC32XX_DEFINE_CLK(UART3_DIV, UART3_CLK_CTRL,
+			   0, 0, 0, 0, 0, 0, lpc32xx_uart_div_ops),
+	LPC32XX_DEFINE_GATE(UART3_GATE, UART_CLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_COMPOSITE(UART3, UART3_MUX, UART3_DIV, UART3_GATE),
+
+	LPC32XX_DEFINE_MUX(UART4_MUX, UART4_CLK_CTRL, 16, 0x1, NULL, 0),
+	LPC32XX_DEFINE_CLK(UART4_DIV, UART4_CLK_CTRL,
+			   0, 0, 0, 0, 0, 0, lpc32xx_uart_div_ops),
+	LPC32XX_DEFINE_GATE(UART4_GATE, UART_CLK_CTRL, 1, 0),
+	LPC32XX_DEFINE_COMPOSITE(UART4, UART4_MUX, UART4_DIV, UART4_GATE),
+
+	LPC32XX_DEFINE_MUX(UART5_MUX, UART5_CLK_CTRL, 16, 0x1, NULL, 0),
+	LPC32XX_DEFINE_CLK(UART5_DIV, UART5_CLK_CTRL,
+			   0, 0, 0, 0, 0, 0, lpc32xx_uart_div_ops),
+	LPC32XX_DEFINE_GATE(UART5_GATE, UART_CLK_CTRL, 2, 0),
+	LPC32XX_DEFINE_COMPOSITE(UART5, UART5_MUX, UART5_DIV, UART5_GATE),
+
+	LPC32XX_DEFINE_MUX(UART6_MUX, UART6_CLK_CTRL, 16, 0x1, NULL, 0),
+	LPC32XX_DEFINE_CLK(UART6_DIV, UART6_CLK_CTRL,
+			   0, 0, 0, 0, 0, 0, lpc32xx_uart_div_ops),
+	LPC32XX_DEFINE_GATE(UART6_GATE, UART_CLK_CTRL, 3, 0),
+	LPC32XX_DEFINE_COMPOSITE(UART6, UART6_MUX, UART6_DIV, UART6_GATE),
+
+	LPC32XX_DEFINE_CLK(IRDA, IRDA_CLK_CTRL,
+			   0, 0, 0, 0, 0, 0, lpc32xx_uart_div_ops),
+
+	LPC32XX_DEFINE_MUX(TEST1_MUX, TEST_CLK_CTRL, 5, 0x3,
+			   test1_mux_table, 0),
+	LPC32XX_DEFINE_GATE(TEST1_GATE, TEST_CLK_CTRL, 4, 0),
+	LPC32XX_DEFINE_COMPOSITE(TEST1, TEST1_MUX, _NULL, TEST1_GATE),
+
+	LPC32XX_DEFINE_MUX(TEST2_MUX, TEST_CLK_CTRL, 1, 0x7,
+			   test2_mux_table, 0),
+	LPC32XX_DEFINE_GATE(TEST2_GATE, TEST_CLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_COMPOSITE(TEST2, TEST2_MUX, _NULL, TEST2_GATE),
+
+	LPC32XX_DEFINE_MUX(SYS, SYSCLK_CTRL, 0, 0x1, NULL, CLK_MUX_READ_ONLY),
+
+	LPC32XX_DEFINE_DIV(USB_DIV_DIV, USB_DIV, 0, 4, NULL, 0),
+	LPC32XX_DEFINE_GATE(USB_DIV_GATE, USB_CTRL, 17, 0),
+	LPC32XX_DEFINE_COMPOSITE(USB_DIV, _NULL, USB_DIV_DIV, USB_DIV_GATE),
+
+	LPC32XX_DEFINE_DIV(SD_DIV, MS_CTRL, 0, 4, NULL,
+			   CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO),
+	LPC32XX_DEFINE_CLK(SD_GATE, MS_CTRL, BIT(5) | BIT(9), BIT(5) | BIT(9),
+			   0x0, BIT(5) | BIT(9), 0x0, 0x0, clk_mask_ops),
+	LPC32XX_DEFINE_COMPOSITE(SD, _NULL, SD_DIV, SD_GATE),
+
+	LPC32XX_DEFINE_DIV(LCD_DIV, LCDCLK_CTRL, 0, 5, NULL, 0),
+	LPC32XX_DEFINE_GATE(LCD_GATE, LCDCLK_CTRL, 5, 0),
+	LPC32XX_DEFINE_COMPOSITE(LCD, _NULL, LCD_DIV, LCD_GATE),
+
+	LPC32XX_DEFINE_CLK(MAC, MACCLK_CTRL,
+			   BIT(2) | BIT(1) | BIT(0), BIT(2) | BIT(1) | BIT(0),
+			   BIT(2) | BIT(1) | BIT(0), BIT(2) | BIT(1) | BIT(0),
+			   0x0, 0x0, clk_mask_ops),
+	LPC32XX_DEFINE_CLK(SLC, FLASHCLK_CTRL,
+			   BIT(2) | BIT(0), BIT(2) | BIT(0), 0x0,
+			   BIT(0), BIT(1), BIT(2) | BIT(1), clk_mask_ops),
+	LPC32XX_DEFINE_CLK(MLC, FLASHCLK_CTRL,
+			   BIT(1), BIT(2) | BIT(1), 0x0, BIT(1),
+			   BIT(2) | BIT(0), BIT(2) | BIT(0), clk_mask_ops),
+	/*
+	 * ADC/TS clock unfortunately cannot be registered as a composite one
+	 * due to a different connection of gate, div and mux, e.g. gating it
+	 * won't mean that the clock is off, if peripheral clock is its parent:
+	 *
+	 * rtc-->[gate]-->|     |
+	 *                | mux |--> adc/ts
+	 * pclk-->[div]-->|     |
+	 *
+	 * Constraints:
+	 * ADC --- resulting clock must be <= 4.5 MHz
+	 * TS  --- resulting clock must be <= 400 KHz
+	 */
+	LPC32XX_DEFINE_DIV(ADC_DIV, ADCCLK_CTRL1, 0, 8, NULL, 0),
+	LPC32XX_DEFINE_GATE(ADC_RTC, ADCCLK_CTRL, 0, 0),
+	LPC32XX_DEFINE_MUX(ADC, ADCCLK_CTRL1, 8, 0x1, NULL, 0),
+
+	/* USB controller clocks */
+	LPC32XX_DEFINE_USB(USB_AHB,
+			   BIT(24), 0x0, BIT(24), BIT(4), 0, clk_usb_ops),
+	LPC32XX_DEFINE_USB(USB_OTG,
+			   0x0, 0x0, 0x0, BIT(3), 0, clk_usb_ops),
+	LPC32XX_DEFINE_USB(USB_I2C,
+			   0x0, BIT(23), BIT(23), BIT(2), 0, clk_usb_i2c_ops),
+	LPC32XX_DEFINE_USB(USB_DEV,
+			   BIT(22), 0x0, BIT(22), BIT(1), BIT(0), clk_usb_ops),
+	LPC32XX_DEFINE_USB(USB_HOST,
+			   BIT(21), 0x0, BIT(21), BIT(0), BIT(1), clk_usb_ops),
+};
+
+static struct clk * __init lpc32xx_clk_register(u32 id)
+{
+	const struct clk_proto_t *lpc32xx_clk = &clk_proto[id];
+	struct clk_hw_proto *clk_hw = &clk_hw_proto[id];
+	const char *parents[LPC32XX_CLK_PARENTS_MAX];
+	struct clk *clk;
+	unsigned int i;
+
+	for (i = 0; i < lpc32xx_clk->num_parents; i++)
+		parents[i] = clk_proto[lpc32xx_clk->parents[i]].name;
+
+	pr_debug("%s: derived from '%s', clock type %d\n", lpc32xx_clk->name,
+		 parents[0], clk_hw->type);
+
+	switch (clk_hw->type) {
+	case CLK_LPC32XX:
+	case CLK_LPC32XX_PLL:
+	case CLK_LPC32XX_USB:
+	case CLK_MUX:
+	case CLK_DIV:
+	case CLK_GATE:
+	{
+		struct clk_init_data clk_init = {
+			.name = lpc32xx_clk->name,
+			.parent_names = parents,
+			.num_parents = lpc32xx_clk->num_parents,
+			.flags = lpc32xx_clk->flags,
+			.ops = clk_hw->hw0.ops,
+		};
+		struct clk_hw *hw;
+
+		if (clk_hw->type == CLK_LPC32XX)
+			hw = &clk_hw->hw0.clk.hw;
+		else if (clk_hw->type == CLK_LPC32XX_PLL)
+			hw = &clk_hw->hw0.pll.hw;
+		else if (clk_hw->type == CLK_LPC32XX_USB)
+			hw = &clk_hw->hw0.usb_clk.hw;
+		else if (clk_hw->type == CLK_MUX)
+			hw = &clk_hw->hw0.mux.hw;
+		else if (clk_hw->type == CLK_DIV)
+			hw = &clk_hw->hw0.div.hw;
+		else if (clk_hw->type == CLK_GATE)
+			hw = &clk_hw->hw0.gate.hw;
+
+		hw->init = &clk_init;
+		clk = clk_register(NULL, hw);
+		break;
+	}
+	case CLK_COMPOSITE:
+	{
+		struct clk_hw *mux_hw = NULL, *div_hw = NULL, *gate_hw = NULL;
+		const struct clk_ops *mops = NULL, *dops = NULL, *gops = NULL;
+		struct clk_hw_proto0 *mux0, *div0, *gate0;
+
+		mux0 = clk_hw->hw1.mux;
+		div0 = clk_hw->hw1.div;
+		gate0 = clk_hw->hw1.gate;
+		if (mux0) {
+			mops = mux0->ops;
+			mux_hw = &mux0->clk.hw;
+		}
+		if (div0) {
+			dops = div0->ops;
+			div_hw = &div0->clk.hw;
+		}
+		if (gate0) {
+			gops = gate0->ops;
+			gate_hw = &gate0->clk.hw;
+		}
+
+		clk = clk_register_composite(NULL, lpc32xx_clk->name,
+				parents, lpc32xx_clk->num_parents,
+				mux_hw, mops, div_hw, dops,
+				gate_hw, gops, lpc32xx_clk->flags);
+		break;
+	}
+	case CLK_FIXED:
+	{
+		struct clk_fixed_rate *fixed = &clk_hw->f;
+
+		clk = clk_register_fixed_rate(NULL, lpc32xx_clk->name,
+			parents[0], fixed->flags, fixed->fixed_rate);
+		break;
+	}
+	default:
+		clk = ERR_PTR(-EINVAL);
+	}
+
+	return clk;
+}
+
+static void __init lpc32xx_clk_init(struct device_node *np)
+{
+	unsigned int i;
+	struct clk *clk_osc, *clk_32k;
+	void __iomem *base = NULL;
+
+	/* Ensure that parent clocks are available and valid */
+	clk_32k = of_clk_get_by_name(np, clk_proto[LPC32XX_CLK_XTAL_32K].name);
+	if (IS_ERR(clk_32k)) {
+		pr_err("failed to find external 32KHz clock: %ld\n",
+		       PTR_ERR(clk_32k));
+		return;
+	}
+	if (clk_get_rate(clk_32k) != 32768) {
+		pr_err("invalid clock rate of external 32KHz oscillator");
+		return;
+	}
+
+	clk_osc = of_clk_get_by_name(np, clk_proto[LPC32XX_CLK_XTAL].name);
+	if (IS_ERR(clk_osc)) {
+		pr_err("failed to find external main oscillator clock: %ld\n",
+		       PTR_ERR(clk_osc));
+		return;
+	}
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_err("failed to map system control block registers\n");
+		return;
+	}
+
+	clk_regmap = regmap_init_mmio(NULL, base, &lpc32xx_scb_regmap_config);
+	if (IS_ERR(clk_regmap)) {
+		pr_err("failed to regmap system control block: %ld\n",
+			PTR_ERR(clk_regmap));
+		return;
+	}
+
+	for (i = 0; i < LPC32XX_CLK_MAX; i++) {
+		clk[i] = lpc32xx_clk_register(i);
+		if (IS_ERR(clk[i])) {
+			pr_err("failed to register %s clock: %ld\n",
+				clk_proto[i].name, PTR_ERR(clk[i]));
+			clk[i] = NULL;
+		}
+	}
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+
+	/* For 13MHz osc valid output range of PLL is from 156MHz to 266.5MHz */
+	clk_set_rate(clk[LPC32XX_CLK_HCLK_PLL], 208000000);
+
+	/* Set 48MHz rate of USB PLL clock */
+	clk_set_rate(clk[LPC32XX_CLK_USB_PLL], 48000000);
+
+	/* These two clocks must be always on independently on consumers */
+	clk_prepare_enable(clk[LPC32XX_CLK_ARM]);
+	clk_prepare_enable(clk[LPC32XX_CLK_HCLK]);
+
+	/* Enable ARM VFP by default */
+	clk_prepare_enable(clk[LPC32XX_CLK_ARM_VFP]);
+
+	/* Disable enabled by default clocks for NAND MLC and SLC */
+	clk_mask_disable(&clk_hw_proto[LPC32XX_CLK_SLC].hw0.clk.hw);
+	clk_mask_disable(&clk_hw_proto[LPC32XX_CLK_MLC].hw0.clk.hw);
+}
+CLK_OF_DECLARE(lpc32xx_clk, "nxp,lpc3220-clk", lpc32xx_clk_init);
+
+static void __init lpc32xx_usb_clk_init(struct device_node *np)
+{
+	unsigned int i;
+
+	usb_clk_vbase = of_iomap(np, 0);
+	if (!usb_clk_vbase) {
+		pr_err("failed to map address range\n");
+		return;
+	}
+
+	for (i = 0; i < LPC32XX_USB_CLK_MAX; i++) {
+		usb_clk[i] = lpc32xx_clk_register(i + LPC32XX_CLK_USB_OFFSET);
+		if (IS_ERR(usb_clk[i])) {
+			pr_err("failed to register %s clock: %ld\n",
+				clk_proto[i].name, PTR_ERR(usb_clk[i]));
+			usb_clk[i] = NULL;
+		}
+	}
+
+	of_clk_add_provider(np, of_clk_src_onecell_get, &usb_clk_data);
+}
+CLK_OF_DECLARE(lpc32xx_usb_clk, "nxp,lpc3220-usb-clk", lpc32xx_usb_clk_init);
diff --git a/drivers/clk/pxa/clk-pxa25x.c b/drivers/clk/pxa/clk-pxa25x.c
index 542e45ef5087..b7747229db9a 100644
--- a/drivers/clk/pxa/clk-pxa25x.c
+++ b/drivers/clk/pxa/clk-pxa25x.c
@@ -17,7 +17,6 @@
 #include <linux/clkdev.h>
 #include <linux/io.h>
 #include <linux/of.h>
-#include <mach/pxa25x.h>
 #include <mach/pxa2xx-regs.h>
 
 #include <dt-bindings/clock/pxa-clock.h>
diff --git a/drivers/clk/qcom/Kconfig b/drivers/clk/qcom/Kconfig
index ee4c83aab4f4..b552eceec2be 100644
--- a/drivers/clk/qcom/Kconfig
+++ b/drivers/clk/qcom/Kconfig
@@ -106,3 +106,20 @@ config MSM_MMCC_8974
 	  Support for the multimedia clock controller on msm8974 devices.
 	  Say Y if you want to support multimedia devices such as display,
 	  graphics, video encode/decode, camera, etc.
+
+config MSM_GCC_8996
+	tristate "MSM8996 Global Clock Controller"
+	depends on COMMON_CLK_QCOM
+	help
+	  Support for the global clock controller on msm8996 devices.
+	  Say Y if you want to use peripheral devices such as UART, SPI,
+	  i2c, USB, UFS, SD/eMMC, PCIe, etc.
+
+config MSM_MMCC_8996
+	tristate "MSM8996 Multimedia Clock Controller"
+	select MSM_GCC_8996
+	depends on COMMON_CLK_QCOM
+	help
+	  Support for the multimedia clock controller on msm8996 devices.
+	  Say Y if you want to support multimedia devices such as display,
+	  graphics, video encode/decode, camera, etc.
diff --git a/drivers/clk/qcom/Makefile b/drivers/clk/qcom/Makefile
index fe6252349e55..dc4280b85db1 100644
--- a/drivers/clk/qcom/Makefile
+++ b/drivers/clk/qcom/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_COMMON_CLK_QCOM) += clk-qcom.o
 
 clk-qcom-y += common.o
 clk-qcom-y += clk-regmap.o
+clk-qcom-y += clk-alpha-pll.o
 clk-qcom-y += clk-pll.o
 clk-qcom-y += clk-rcg.o
 clk-qcom-y += clk-rcg2.o
@@ -20,5 +21,7 @@ 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_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-alpha-pll.c b/drivers/clk/qcom/clk-alpha-pll.c
new file mode 100644
index 000000000000..e6a03eaf7a93
--- /dev/null
+++ b/drivers/clk/qcom/clk-alpha-pll.c
@@ -0,0 +1,355 @@
+/*
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * 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/export.h>
+#include <linux/clk-provider.h>
+#include <linux/regmap.h>
+#include <linux/delay.h>
+
+#include "clk-alpha-pll.h"
+
+#define PLL_MODE		0x00
+# define PLL_OUTCTRL		BIT(0)
+# define PLL_BYPASSNL		BIT(1)
+# define PLL_RESET_N		BIT(2)
+# define PLL_LOCK_COUNT_SHIFT	8
+# define PLL_LOCK_COUNT_MASK	0x3f
+# define PLL_BIAS_COUNT_SHIFT	14
+# define PLL_BIAS_COUNT_MASK	0x3f
+# define PLL_VOTE_FSM_ENA	BIT(20)
+# define PLL_VOTE_FSM_RESET	BIT(21)
+# define PLL_ACTIVE_FLAG	BIT(30)
+# define PLL_LOCK_DET		BIT(31)
+
+#define PLL_L_VAL		0x04
+#define PLL_ALPHA_VAL		0x08
+#define PLL_ALPHA_VAL_U		0x0c
+
+#define PLL_USER_CTL		0x10
+# define PLL_POST_DIV_SHIFT	8
+# define PLL_POST_DIV_MASK	0xf
+# define PLL_ALPHA_EN		BIT(24)
+# define PLL_VCO_SHIFT		20
+# define PLL_VCO_MASK		0x3
+
+#define PLL_USER_CTL_U		0x14
+
+#define PLL_CONFIG_CTL		0x18
+#define PLL_TEST_CTL		0x1c
+#define PLL_TEST_CTL_U		0x20
+#define PLL_STATUS		0x24
+
+/*
+ * Even though 40 bits are present, use only 32 for ease of calculation.
+ */
+#define ALPHA_REG_BITWIDTH	40
+#define ALPHA_BITWIDTH		32
+
+#define to_clk_alpha_pll(_hw) container_of(to_clk_regmap(_hw), \
+					   struct clk_alpha_pll, clkr)
+
+#define to_clk_alpha_pll_postdiv(_hw) container_of(to_clk_regmap(_hw), \
+					   struct clk_alpha_pll_postdiv, clkr)
+
+static int wait_for_pll(struct clk_alpha_pll *pll)
+{
+	u32 val, mask, off;
+	int count;
+	int ret;
+	const char *name = clk_hw_get_name(&pll->clkr.hw);
+
+	off = pll->offset;
+	ret = regmap_read(pll->clkr.regmap, off + PLL_MODE, &val);
+	if (ret)
+		return ret;
+
+	if (val & PLL_VOTE_FSM_ENA)
+		mask = PLL_ACTIVE_FLAG;
+	else
+		mask = PLL_LOCK_DET;
+
+	/* Wait for pll to enable. */
+	for (count = 100; count > 0; count--) {
+		ret = regmap_read(pll->clkr.regmap, off + PLL_MODE, &val);
+		if (ret)
+			return ret;
+		if ((val & mask) == mask)
+			return 0;
+
+		udelay(1);
+	}
+
+	WARN(1, "%s didn't enable after voting for it!\n", name);
+	return -ETIMEDOUT;
+}
+
+static int clk_alpha_pll_enable(struct clk_hw *hw)
+{
+	int ret;
+	struct clk_alpha_pll *pll = to_clk_alpha_pll(hw);
+	u32 val, mask, off;
+
+	off = pll->offset;
+
+	mask = PLL_OUTCTRL | PLL_RESET_N | PLL_BYPASSNL;
+	ret = regmap_read(pll->clkr.regmap, off + PLL_MODE, &val);
+	if (ret)
+		return ret;
+
+	/* If in FSM mode, just vote for it */
+	if (val & PLL_VOTE_FSM_ENA) {
+		ret = clk_enable_regmap(hw);
+		if (ret)
+			return ret;
+		return wait_for_pll(pll);
+	}
+
+	/* Skip if already enabled */
+	if ((val & mask) == mask)
+		return 0;
+
+	ret = regmap_update_bits(pll->clkr.regmap, off + PLL_MODE,
+				 PLL_BYPASSNL, PLL_BYPASSNL);
+	if (ret)
+		return ret;
+
+	/*
+	 * H/W requires a 5us delay between disabling the bypass and
+	 * de-asserting the reset.
+	 */
+	mb();
+	udelay(5);
+
+	ret = regmap_update_bits(pll->clkr.regmap, off + PLL_MODE,
+				 PLL_RESET_N, PLL_RESET_N);
+	if (ret)
+		return ret;
+
+	ret = wait_for_pll(pll);
+	if (ret)
+		return ret;
+
+	ret = regmap_update_bits(pll->clkr.regmap, off + PLL_MODE,
+				 PLL_OUTCTRL, PLL_OUTCTRL);
+
+	/* Ensure that the write above goes through before returning. */
+	mb();
+	return ret;
+}
+
+static void clk_alpha_pll_disable(struct clk_hw *hw)
+{
+	int ret;
+	struct clk_alpha_pll *pll = to_clk_alpha_pll(hw);
+	u32 val, mask, off;
+
+	off = pll->offset;
+
+	ret = regmap_read(pll->clkr.regmap, off + PLL_MODE, &val);
+	if (ret)
+		return;
+
+	/* If in FSM mode, just unvote it */
+	if (val & PLL_VOTE_FSM_ENA) {
+		clk_disable_regmap(hw);
+		return;
+	}
+
+	mask = PLL_OUTCTRL;
+	regmap_update_bits(pll->clkr.regmap, off + PLL_MODE, mask, 0);
+
+	/* Delay of 2 output clock ticks required until output is disabled */
+	mb();
+	udelay(1);
+
+	mask = PLL_RESET_N | PLL_BYPASSNL;
+	regmap_update_bits(pll->clkr.regmap, off + PLL_MODE, mask, 0);
+}
+
+static unsigned long alpha_pll_calc_rate(u64 prate, u32 l, u32 a)
+{
+	return (prate * l) + ((prate * a) >> ALPHA_BITWIDTH);
+}
+
+static unsigned long
+alpha_pll_round_rate(unsigned long rate, unsigned long prate, u32 *l, u64 *a)
+{
+	u64 remainder;
+	u64 quotient;
+
+	quotient = rate;
+	remainder = do_div(quotient, prate);
+	*l = quotient;
+
+	if (!remainder) {
+		*a = 0;
+		return rate;
+	}
+
+	/* Upper ALPHA_BITWIDTH bits of Alpha */
+	quotient = remainder << ALPHA_BITWIDTH;
+	remainder = do_div(quotient, prate);
+
+	if (remainder)
+		quotient++;
+
+	*a = quotient;
+	return alpha_pll_calc_rate(prate, *l, *a);
+}
+
+static const struct pll_vco *
+alpha_pll_find_vco(const struct clk_alpha_pll *pll, unsigned long rate)
+{
+	const struct pll_vco *v = pll->vco_table;
+	const struct pll_vco *end = v + pll->num_vco;
+
+	for (; v < end; v++)
+		if (rate >= v->min_freq && rate <= v->max_freq)
+			return v;
+
+	return NULL;
+}
+
+static unsigned long
+clk_alpha_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	u32 l, low, high, ctl;
+	u64 a = 0, prate = parent_rate;
+	struct clk_alpha_pll *pll = to_clk_alpha_pll(hw);
+	u32 off = pll->offset;
+
+	regmap_read(pll->clkr.regmap, off + PLL_L_VAL, &l);
+
+	regmap_read(pll->clkr.regmap, off + PLL_USER_CTL, &ctl);
+	if (ctl & PLL_ALPHA_EN) {
+		regmap_read(pll->clkr.regmap, off + PLL_ALPHA_VAL, &low);
+		regmap_read(pll->clkr.regmap, off + PLL_ALPHA_VAL_U, &high);
+		a = (u64)high << 32 | low;
+		a >>= ALPHA_REG_BITWIDTH - ALPHA_BITWIDTH;
+	}
+
+	return alpha_pll_calc_rate(prate, l, a);
+}
+
+static int clk_alpha_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long prate)
+{
+	struct clk_alpha_pll *pll = to_clk_alpha_pll(hw);
+	const struct pll_vco *vco;
+	u32 l, off = pll->offset;
+	u64 a;
+
+	rate = alpha_pll_round_rate(rate, prate, &l, &a);
+	vco = alpha_pll_find_vco(pll, rate);
+	if (!vco) {
+		pr_err("alpha pll not in a valid vco range\n");
+		return -EINVAL;
+	}
+
+	a <<= (ALPHA_REG_BITWIDTH - ALPHA_BITWIDTH);
+
+	regmap_write(pll->clkr.regmap, off + PLL_L_VAL, l);
+	regmap_write(pll->clkr.regmap, off + PLL_ALPHA_VAL, a);
+	regmap_write(pll->clkr.regmap, off + PLL_ALPHA_VAL_U, a >> 32);
+
+	regmap_update_bits(pll->clkr.regmap, off + PLL_USER_CTL,
+			   PLL_VCO_MASK << PLL_VCO_SHIFT,
+			   vco->val << PLL_VCO_SHIFT);
+
+	regmap_update_bits(pll->clkr.regmap, off + PLL_USER_CTL, PLL_ALPHA_EN,
+			   PLL_ALPHA_EN);
+
+	return 0;
+}
+
+static long clk_alpha_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long *prate)
+{
+	struct clk_alpha_pll *pll = to_clk_alpha_pll(hw);
+	u32 l;
+	u64 a;
+	unsigned long min_freq, max_freq;
+
+	rate = alpha_pll_round_rate(rate, *prate, &l, &a);
+	if (alpha_pll_find_vco(pll, rate))
+		return rate;
+
+	min_freq = pll->vco_table[0].min_freq;
+	max_freq = pll->vco_table[pll->num_vco - 1].max_freq;
+
+	return clamp(rate, min_freq, max_freq);
+}
+
+const struct clk_ops clk_alpha_pll_ops = {
+	.enable = clk_alpha_pll_enable,
+	.disable = clk_alpha_pll_disable,
+	.recalc_rate = clk_alpha_pll_recalc_rate,
+	.round_rate = clk_alpha_pll_round_rate,
+	.set_rate = clk_alpha_pll_set_rate,
+};
+EXPORT_SYMBOL_GPL(clk_alpha_pll_ops);
+
+static unsigned long
+clk_alpha_pll_postdiv_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct clk_alpha_pll_postdiv *pll = to_clk_alpha_pll_postdiv(hw);
+	u32 ctl;
+
+	regmap_read(pll->clkr.regmap, pll->offset + PLL_USER_CTL, &ctl);
+
+	ctl >>= PLL_POST_DIV_SHIFT;
+	ctl &= PLL_POST_DIV_MASK;
+
+	return parent_rate >> fls(ctl);
+}
+
+static const struct clk_div_table clk_alpha_div_table[] = {
+	{ 0x0, 1 },
+	{ 0x1, 2 },
+	{ 0x3, 4 },
+	{ 0x7, 8 },
+	{ 0xf, 16 },
+	{ }
+};
+
+static long
+clk_alpha_pll_postdiv_round_rate(struct clk_hw *hw, unsigned long rate,
+				 unsigned long *prate)
+{
+	struct clk_alpha_pll_postdiv *pll = to_clk_alpha_pll_postdiv(hw);
+
+	return divider_round_rate(hw, rate, prate, clk_alpha_div_table,
+				  pll->width, CLK_DIVIDER_POWER_OF_TWO);
+}
+
+static int clk_alpha_pll_postdiv_set_rate(struct clk_hw *hw, unsigned long rate,
+					  unsigned long parent_rate)
+{
+	struct clk_alpha_pll_postdiv *pll = to_clk_alpha_pll_postdiv(hw);
+	int div;
+
+	/* 16 -> 0xf, 8 -> 0x7, 4 -> 0x3, 2 -> 0x1, 1 -> 0x0 */
+	div = DIV_ROUND_UP_ULL((u64)parent_rate, rate) - 1;
+
+	return regmap_update_bits(pll->clkr.regmap, pll->offset + PLL_USER_CTL,
+				  PLL_POST_DIV_MASK << PLL_POST_DIV_SHIFT,
+				  div << PLL_POST_DIV_SHIFT);
+}
+
+const struct clk_ops clk_alpha_pll_postdiv_ops = {
+	.recalc_rate = clk_alpha_pll_postdiv_recalc_rate,
+	.round_rate = clk_alpha_pll_postdiv_round_rate,
+	.set_rate = clk_alpha_pll_postdiv_set_rate,
+};
+EXPORT_SYMBOL_GPL(clk_alpha_pll_postdiv_ops);
diff --git a/drivers/clk/qcom/clk-alpha-pll.h b/drivers/clk/qcom/clk-alpha-pll.h
new file mode 100644
index 000000000000..90ce2016e1a0
--- /dev/null
+++ b/drivers/clk/qcom/clk-alpha-pll.h
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __QCOM_CLK_ALPHA_PLL_H__
+#define __QCOM_CLK_ALPHA_PLL_H__
+
+#include <linux/clk-provider.h>
+#include "clk-regmap.h"
+
+struct pll_vco {
+	unsigned long min_freq;
+	unsigned long max_freq;
+	u32 val;
+};
+
+/**
+ * struct clk_alpha_pll - phase locked loop (PLL)
+ * @offset: base address of registers
+ * @vco_table: array of VCO settings
+ * @clkr: regmap clock handle
+ */
+struct clk_alpha_pll {
+	u32 offset;
+
+	const struct pll_vco *vco_table;
+	size_t num_vco;
+
+	struct clk_regmap clkr;
+};
+
+/**
+ * struct clk_alpha_pll_postdiv - phase locked loop (PLL) post-divider
+ * @offset: base address of registers
+ * @width: width of post-divider
+ * @clkr: regmap clock handle
+ */
+struct clk_alpha_pll_postdiv {
+	u32 offset;
+	u8 width;
+
+	struct clk_regmap clkr;
+};
+
+extern const struct clk_ops clk_alpha_pll_ops;
+extern const struct clk_ops clk_alpha_pll_postdiv_ops;
+
+#endif
diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index 4b1e94bdf29e..b904c335cda4 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -178,5 +178,6 @@ extern const struct clk_ops clk_edp_pixel_ops;
 extern const struct clk_ops clk_byte_ops;
 extern const struct clk_ops clk_byte2_ops;
 extern const struct clk_ops clk_pixel_ops;
+extern const struct clk_ops clk_gfx3d_ops;
 
 #endif
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index b544bb302f79..a071bba8018c 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -723,3 +723,90 @@ const struct clk_ops clk_pixel_ops = {
 	.determine_rate = clk_pixel_determine_rate,
 };
 EXPORT_SYMBOL_GPL(clk_pixel_ops);
+
+static int clk_gfx3d_determine_rate(struct clk_hw *hw,
+				    struct clk_rate_request *req)
+{
+	struct clk_rate_request parent_req = { };
+	struct clk_hw *p2, *p8, *p9, *xo;
+	unsigned long p9_rate;
+	int ret;
+
+	xo = clk_hw_get_parent_by_index(hw, 0);
+	if (req->rate == clk_hw_get_rate(xo)) {
+		req->best_parent_hw = xo;
+		return 0;
+	}
+
+	p9 = clk_hw_get_parent_by_index(hw, 2);
+	p2 = clk_hw_get_parent_by_index(hw, 3);
+	p8 = clk_hw_get_parent_by_index(hw, 4);
+
+	/* PLL9 is a fixed rate PLL */
+	p9_rate = clk_hw_get_rate(p9);
+
+	parent_req.rate = req->rate = min(req->rate, p9_rate);
+	if (req->rate == p9_rate) {
+		req->rate = req->best_parent_rate = p9_rate;
+		req->best_parent_hw = p9;
+		return 0;
+	}
+
+	if (req->best_parent_hw == p9) {
+		/* Are we going back to a previously used rate? */
+		if (clk_hw_get_rate(p8) == req->rate)
+			req->best_parent_hw = p8;
+		else
+			req->best_parent_hw = p2;
+	} else if (req->best_parent_hw == p8) {
+		req->best_parent_hw = p2;
+	} else {
+		req->best_parent_hw = p8;
+	}
+
+	ret = __clk_determine_rate(req->best_parent_hw, &parent_req);
+	if (ret)
+		return ret;
+
+	req->rate = req->best_parent_rate = parent_req.rate;
+
+	return 0;
+}
+
+static int clk_gfx3d_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate, u8 index)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	u32 cfg;
+	int ret;
+
+	/* Just mux it, we don't use the division or m/n hardware */
+	cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
+	ret = regmap_write(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, cfg);
+	if (ret)
+		return ret;
+
+	return update_config(rcg);
+}
+
+static int clk_gfx3d_set_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long parent_rate)
+{
+	/*
+	 * We should never get here; clk_gfx3d_determine_rate() should always
+	 * make us use a different parent than what we're currently using, so
+	 * clk_gfx3d_set_rate_and_parent() should always be called.
+	 */
+	return 0;
+}
+
+const struct clk_ops clk_gfx3d_ops = {
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_parent = clk_rcg2_set_parent,
+	.recalc_rate = clk_rcg2_recalc_rate,
+	.set_rate = clk_gfx3d_set_rate,
+	.set_rate_and_parent = clk_gfx3d_set_rate_and_parent,
+	.determine_rate = clk_gfx3d_determine_rate,
+};
+EXPORT_SYMBOL_GPL(clk_gfx3d_ops);
diff --git a/drivers/clk/qcom/common.c b/drivers/clk/qcom/common.c
index 8fa477293ae0..c112ebaba70d 100644
--- a/drivers/clk/qcom/common.c
+++ b/drivers/clk/qcom/common.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/clk-provider.h>
 #include <linux/reset-controller.h>
+#include <linux/of.h>
 
 #include "common.h"
 #include "clk-rcg.h"
@@ -88,6 +89,92 @@ static void qcom_cc_gdsc_unregister(void *data)
 	gdsc_unregister(data);
 }
 
+/*
+ * Backwards compatibility with old DTs. Register a pass-through factor 1/1
+ * clock to translate 'path' clk into 'name' clk and regsiter the 'path'
+ * clk as a fixed rate clock if it isn't present.
+ */
+static int _qcom_cc_register_board_clk(struct device *dev, const char *path,
+				       const char *name, unsigned long rate,
+				       bool add_factor)
+{
+	struct device_node *node = NULL;
+	struct device_node *clocks_node;
+	struct clk_fixed_factor *factor;
+	struct clk_fixed_rate *fixed;
+	struct clk *clk;
+	struct clk_init_data init_data = { };
+
+	clocks_node = of_find_node_by_path("/clocks");
+	if (clocks_node)
+		node = of_find_node_by_name(clocks_node, path);
+	of_node_put(clocks_node);
+
+	if (!node) {
+		fixed = devm_kzalloc(dev, sizeof(*fixed), GFP_KERNEL);
+		if (!fixed)
+			return -EINVAL;
+
+		fixed->fixed_rate = rate;
+		fixed->hw.init = &init_data;
+
+		init_data.name = path;
+		init_data.flags = CLK_IS_ROOT;
+		init_data.ops = &clk_fixed_rate_ops;
+
+		clk = devm_clk_register(dev, &fixed->hw);
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+	}
+	of_node_put(node);
+
+	if (add_factor) {
+		factor = devm_kzalloc(dev, sizeof(*factor), GFP_KERNEL);
+		if (!factor)
+			return -EINVAL;
+
+		factor->mult = factor->div = 1;
+		factor->hw.init = &init_data;
+
+		init_data.name = name;
+		init_data.parent_names = &path;
+		init_data.num_parents = 1;
+		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);
+	}
+
+	return 0;
+}
+
+int qcom_cc_register_board_clk(struct device *dev, const char *path,
+			       const char *name, unsigned long rate)
+{
+	bool add_factor = true;
+	struct device_node *node;
+
+	/* The RPM clock driver will add the factor clock if present */
+	if (IS_ENABLED(CONFIG_QCOM_RPMCC)) {
+		node = of_find_compatible_node(NULL, NULL, "qcom,rpmcc");
+		if (of_device_is_available(node))
+			add_factor = false;
+		of_node_put(node);
+	}
+
+	return _qcom_cc_register_board_clk(dev, path, name, rate, add_factor);
+}
+EXPORT_SYMBOL_GPL(qcom_cc_register_board_clk);
+
+int qcom_cc_register_sleep_clk(struct device *dev)
+{
+	return _qcom_cc_register_board_clk(dev, "sleep_clk", "sleep_clk_src",
+					   32768, true);
+}
+EXPORT_SYMBOL_GPL(qcom_cc_register_sleep_clk);
+
 int qcom_cc_really_probe(struct platform_device *pdev,
 			 const struct qcom_cc_desc *desc, struct regmap *regmap)
 {
diff --git a/drivers/clk/qcom/common.h b/drivers/clk/qcom/common.h
index 7c1fba3ebc03..ae9bdeb21f29 100644
--- a/drivers/clk/qcom/common.h
+++ b/drivers/clk/qcom/common.h
@@ -37,6 +37,10 @@ extern const struct freq_tbl *qcom_find_freq(const struct freq_tbl *f,
 extern int qcom_find_src_index(struct clk_hw *hw, const struct parent_map *map,
 			       u8 src);
 
+extern int qcom_cc_register_board_clk(struct device *dev, const char *path,
+				      const char *name, unsigned long rate);
+extern int qcom_cc_register_sleep_clk(struct device *dev);
+
 extern struct regmap *qcom_cc_map(struct platform_device *pdev,
 				  const struct qcom_cc_desc *desc);
 extern int qcom_cc_really_probe(struct platform_device *pdev,
diff --git a/drivers/clk/qcom/gcc-apq8084.c b/drivers/clk/qcom/gcc-apq8084.c
index 1567c3a79534..070037a29ea5 100644
--- a/drivers/clk/qcom/gcc-apq8084.c
+++ b/drivers/clk/qcom/gcc-apq8084.c
@@ -3607,18 +3607,16 @@ MODULE_DEVICE_TABLE(of, gcc_apq8084_match_table);
 
 static int gcc_apq8084_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
+	int ret;
 	struct device *dev = &pdev->dev;
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "xo", NULL, CLK_IS_ROOT, 19200000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "xo_board", "xo", 19200000);
+	if (ret)
+		return ret;
 
-	clk = clk_register_fixed_rate(dev, "sleep_clk_src", NULL,
-				      CLK_IS_ROOT, 32768);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_sleep_clk(dev);
+	if (ret)
+		return ret;
 
 	return qcom_cc_probe(pdev, &gcc_apq8084_desc);
 }
diff --git a/drivers/clk/qcom/gcc-ipq806x.c b/drivers/clk/qcom/gcc-ipq806x.c
index 16fc64c082a5..dd5402bac620 100644
--- a/drivers/clk/qcom/gcc-ipq806x.c
+++ b/drivers/clk/qcom/gcc-ipq806x.c
@@ -3023,19 +3023,17 @@ MODULE_DEVICE_TABLE(of, gcc_ipq806x_match_table);
 
 static int gcc_ipq806x_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
 	struct device *dev = &pdev->dev;
 	struct regmap *regmap;
 	int ret;
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 25000000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "cxo_board", "cxo", 19200000);
+	if (ret)
+		return ret;
 
-	clk = clk_register_fixed_rate(dev, "pxo", NULL, CLK_IS_ROOT, 25000000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "pxo_board", "pxo", 27000000);
+	if (ret)
+		return ret;
 
 	ret = qcom_cc_probe(pdev, &gcc_ipq806x_desc);
 	if (ret)
diff --git a/drivers/clk/qcom/gcc-msm8660.c b/drivers/clk/qcom/gcc-msm8660.c
index f110bb5a1df3..ad413036f7c7 100644
--- a/drivers/clk/qcom/gcc-msm8660.c
+++ b/drivers/clk/qcom/gcc-msm8660.c
@@ -2720,17 +2720,16 @@ MODULE_DEVICE_TABLE(of, gcc_msm8660_match_table);
 
 static int gcc_msm8660_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
+	int ret;
 	struct device *dev = &pdev->dev;
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 19200000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "cxo_board", "cxo", 19200000);
+	if (ret)
+		return ret;
 
-	clk = clk_register_fixed_rate(dev, "pxo", NULL, CLK_IS_ROOT, 27000000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "pxo_board", "pxo", 27000000);
+	if (ret)
+		return ret;
 
 	return qcom_cc_probe(pdev, &gcc_msm8660_desc);
 }
diff --git a/drivers/clk/qcom/gcc-msm8916.c b/drivers/clk/qcom/gcc-msm8916.c
index d0a0313d6bef..8cc9b2868b41 100644
--- a/drivers/clk/qcom/gcc-msm8916.c
+++ b/drivers/clk/qcom/gcc-msm8916.c
@@ -3356,18 +3356,16 @@ MODULE_DEVICE_TABLE(of, gcc_msm8916_match_table);
 
 static int gcc_msm8916_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
+	int ret;
 	struct device *dev = &pdev->dev;
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "xo", NULL, CLK_IS_ROOT, 19200000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "xo_board", "xo", 19200000);
+	if (ret)
+		return ret;
 
-	clk = clk_register_fixed_rate(dev, "sleep_clk_src", NULL,
-				      CLK_IS_ROOT, 32768);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_sleep_clk(dev);
+	if (ret)
+		return ret;
 
 	return qcom_cc_probe(pdev, &gcc_msm8916_desc);
 }
diff --git a/drivers/clk/qcom/gcc-msm8960.c b/drivers/clk/qcom/gcc-msm8960.c
index 66c18bc97857..983dd7dc89a7 100644
--- a/drivers/clk/qcom/gcc-msm8960.c
+++ b/drivers/clk/qcom/gcc-msm8960.c
@@ -3503,7 +3503,6 @@ MODULE_DEVICE_TABLE(of, gcc_msm8960_match_table);
 
 static int gcc_msm8960_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
 	struct device *dev = &pdev->dev;
 	const struct of_device_id *match;
 	struct platform_device *tsens;
@@ -3513,14 +3512,13 @@ static int gcc_msm8960_probe(struct platform_device *pdev)
 	if (!match)
 		return -EINVAL;
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "cxo", NULL, CLK_IS_ROOT, 19200000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "cxo_board", "cxo", 19200000);
+	if (ret)
+		return ret;
 
-	clk = clk_register_fixed_rate(dev, "pxo", NULL, CLK_IS_ROOT, 27000000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "pxo_board", "pxo", 27000000);
+	if (ret)
+		return ret;
 
 	ret = qcom_cc_probe(pdev, match->data);
 	if (ret)
diff --git a/drivers/clk/qcom/gcc-msm8974.c b/drivers/clk/qcom/gcc-msm8974.c
index 28abb8f8f293..335952db309b 100644
--- a/drivers/clk/qcom/gcc-msm8974.c
+++ b/drivers/clk/qcom/gcc-msm8974.c
@@ -2717,7 +2717,7 @@ static void msm8974_pro_clock_override(void)
 
 static int gcc_msm8974_probe(struct platform_device *pdev)
 {
-	struct clk *clk;
+	int ret;
 	struct device *dev = &pdev->dev;
 	bool pro;
 	const struct of_device_id *id;
@@ -2730,16 +2730,13 @@ static int gcc_msm8974_probe(struct platform_device *pdev)
 	if (pro)
 		msm8974_pro_clock_override();
 
-	/* Temporary until RPM clocks supported */
-	clk = clk_register_fixed_rate(dev, "xo", NULL, CLK_IS_ROOT, 19200000);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_board_clk(dev, "xo_board", "xo", 19200000);
+	if (ret)
+		return ret;
 
-	/* Should move to DT node? */
-	clk = clk_register_fixed_rate(dev, "sleep_clk_src", NULL,
-				      CLK_IS_ROOT, 32768);
-	if (IS_ERR(clk))
-		return PTR_ERR(clk);
+	ret = qcom_cc_register_sleep_clk(dev);
+	if (ret)
+		return ret;
 
 	return qcom_cc_probe(pdev, &gcc_msm8974_desc);
 }
diff --git a/drivers/clk/qcom/gcc-msm8996.c b/drivers/clk/qcom/gcc-msm8996.c
new file mode 100644
index 000000000000..16d7c323db49
--- /dev/null
+++ b/drivers/clk/qcom/gcc-msm8996.c
@@ -0,0 +1,3422 @@
+/*
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * 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-msm8996.h>
+
+#include "common.h"
+#include "clk-regmap.h"
+#include "clk-alpha-pll.h"
+#include "clk-rcg.h"
+#include "clk-branch.h"
+#include "reset.h"
+
+#define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n) }
+
+enum {
+	P_XO,
+	P_GPLL0,
+	P_GPLL2,
+	P_GPLL3,
+	P_GPLL1,
+	P_GPLL2_EARLY,
+	P_GPLL0_EARLY_DIV,
+	P_SLEEP_CLK,
+	P_GPLL4,
+	P_AUD_REF_CLK,
+	P_GPLL1_EARLY_DIV
+};
+
+static const struct parent_map gcc_sleep_clk_map[] = {
+	{ P_SLEEP_CLK, 5 }
+};
+
+static const char * const gcc_sleep_clk[] = {
+	"sleep_clk"
+};
+
+static const struct parent_map gcc_xo_gpll0_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 }
+};
+
+static const char * const gcc_xo_gpll0[] = {
+	"xo",
+	"gpll0"
+};
+
+static const struct parent_map gcc_xo_sleep_clk_map[] = {
+	{ P_XO, 0 },
+	{ P_SLEEP_CLK, 5 }
+};
+
+static const char * const gcc_xo_sleep_clk[] = {
+	"xo",
+	"sleep_clk"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll4_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL4, 5 }
+};
+
+static const char * const gcc_xo_gpll0_gpll4[] = {
+	"xo",
+	"gpll0",
+	"gpll4"
+};
+
+static const struct parent_map gcc_xo_gpll0_aud_ref_clk_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_AUD_REF_CLK, 2 }
+};
+
+static const char * const gcc_xo_gpll0_aud_ref_clk[] = {
+	"xo",
+	"gpll0",
+	"aud_ref_clk"
+};
+
+static const struct parent_map gcc_xo_gpll0_sleep_clk_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_SLEEP_CLK, 5 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_sleep_clk_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"sleep_clk",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll4_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL4, 5 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll4_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll4",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll2_gpll3_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL2, 2 },
+	{ P_GPLL3, 3 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll2_gpll3_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll2",
+	"gpll3",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll1_early_div_gpll1_gpll4_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL1_EARLY_DIV, 3 },
+	{ P_GPLL1, 4 },
+	{ P_GPLL4, 5 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll1_early_div_gpll1_gpll4_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll1_early_div",
+	"gpll1",
+	"gpll4",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll2_early_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL2, 2 },
+	{ P_GPLL3, 3 },
+	{ P_GPLL1, 4 },
+	{ P_GPLL2_EARLY, 5 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll2_early_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll2",
+	"gpll3",
+	"gpll1",
+	"gpll2_early",
+	"gpll0_early_div"
+};
+
+static const struct parent_map gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll4_gpll0_early_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 1 },
+	{ P_GPLL2, 2 },
+	{ P_GPLL3, 3 },
+	{ P_GPLL1, 4 },
+	{ P_GPLL4, 5 },
+	{ P_GPLL0_EARLY_DIV, 6 }
+};
+
+static const char * const gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll4_gpll0_early_div[] = {
+	"xo",
+	"gpll0",
+	"gpll2",
+	"gpll3",
+	"gpll1",
+	"gpll4",
+	"gpll0_early_div"
+};
+
+static struct clk_fixed_factor xo = {
+	.mult = 1,
+	.div = 1,
+	.hw.init = &(struct clk_init_data){
+		.name = "xo",
+		.parent_names = (const char *[]){ "xo_board" },
+		.num_parents = 1,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_alpha_pll gpll0_early = {
+	.offset = 0x00000,
+	.clkr = {
+		.enable_reg = 0x52000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpll0_early",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_alpha_pll_ops,
+		},
+	},
+};
+
+static struct clk_fixed_factor gpll0_early_div = {
+	.mult = 1,
+	.div = 2,
+	.hw.init = &(struct clk_init_data){
+		.name = "gpll0_early_div",
+		.parent_names = (const char *[]){ "gpll0_early" },
+		.num_parents = 1,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv gpll0 = {
+	.offset = 0x00000,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gpll0",
+		.parent_names = (const char *[]){ "gpll0_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+	},
+};
+
+static struct clk_alpha_pll gpll4_early = {
+	.offset = 0x77000,
+	.clkr = {
+		.enable_reg = 0x52000,
+		.enable_mask = BIT(4),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpll4_early",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_alpha_pll_ops,
+		},
+	},
+};
+
+static struct clk_alpha_pll_postdiv gpll4 = {
+	.offset = 0x77000,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gpll4",
+		.parent_names = (const char *[]){ "gpll4_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_system_noc_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(50000000, P_GPLL0_EARLY_DIV, 6, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(240000000, P_GPLL0, 2.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 system_noc_clk_src = {
+	.cmd_rcgr = 0x0401c,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll2_early_gpll0_early_div_map,
+	.freq_tbl = ftbl_system_noc_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "system_noc_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll2_gpll3_gpll1_gpll2_early_gpll0_early_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_config_noc_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(37500000, P_GPLL0, 16, 0, 0),
+	F(75000000, P_GPLL0, 8, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 config_noc_clk_src = {
+	.cmd_rcgr = 0x0500c,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_config_noc_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "config_noc_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_periph_noc_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(37500000, P_GPLL0, 16, 0, 0),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(75000000, P_GPLL0, 8, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 periph_noc_clk_src = {
+	.cmd_rcgr = 0x06014,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_periph_noc_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "periph_noc_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_usb30_master_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(120000000, P_GPLL0, 5, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 usb30_master_clk_src = {
+	.cmd_rcgr = 0x0f014,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll0_early_div_map,
+	.freq_tbl = ftbl_usb30_master_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "usb30_master_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll0_early_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_usb30_mock_utmi_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 usb30_mock_utmi_clk_src = {
+	.cmd_rcgr = 0x0f028,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll0_early_div_map,
+	.freq_tbl = ftbl_usb30_mock_utmi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "usb30_mock_utmi_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll0_early_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_usb3_phy_aux_clk_src[] = {
+	F(1200000, P_XO, 16, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 usb3_phy_aux_clk_src = {
+	.cmd_rcgr = 0x5000c,
+	.hid_width = 5,
+	.parent_map = gcc_xo_sleep_clk_map,
+	.freq_tbl = ftbl_usb3_phy_aux_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "usb3_phy_aux_clk_src",
+		.parent_names = gcc_xo_sleep_clk,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_usb20_master_clk_src[] = {
+	F(120000000, P_GPLL0, 5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 usb20_master_clk_src = {
+	.cmd_rcgr = 0x12010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll0_early_div_map,
+	.freq_tbl = ftbl_usb20_master_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "usb20_master_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll0_early_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 usb20_mock_utmi_clk_src = {
+	.cmd_rcgr = 0x12024,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll0_early_div_map,
+	.freq_tbl = ftbl_usb30_mock_utmi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "usb20_mock_utmi_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll0_early_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_sdcc1_apps_clk_src[] = {
+	F(144000, P_XO, 16, 3, 25),
+	F(400000, P_XO, 12, 1, 4),
+	F(20000000, P_GPLL0, 15, 1, 2),
+	F(25000000, P_GPLL0, 12, 1, 2),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(96000000, P_GPLL4, 4, 0, 0),
+	F(192000000, P_GPLL4, 2, 0, 0),
+	F(384000000, P_GPLL4, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 sdcc1_apps_clk_src = {
+	.cmd_rcgr = 0x13010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll4_gpll0_early_div_map,
+	.freq_tbl = ftbl_sdcc1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "sdcc1_apps_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll4_gpll0_early_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 sdcc1_ice_core_clk_src = {
+	.cmd_rcgr = 0x13024,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll4_gpll0_early_div_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "sdcc1_ice_core_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll4_gpll0_early_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_sdcc2_apps_clk_src[] = {
+	F(144000, P_XO, 16, 3, 25),
+	F(400000, P_XO, 12, 1, 4),
+	F(20000000, P_GPLL0, 15, 1, 2),
+	F(25000000, P_GPLL0, 12, 1, 2),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 sdcc2_apps_clk_src = {
+	.cmd_rcgr = 0x14010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll4_map,
+	.freq_tbl = ftbl_sdcc2_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "sdcc2_apps_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll4,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 sdcc3_apps_clk_src = {
+	.cmd_rcgr = 0x15010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll4_map,
+	.freq_tbl = ftbl_sdcc2_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "sdcc3_apps_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll4,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_sdcc4_apps_clk_src[] = {
+	F(144000, P_XO, 16, 3, 25),
+	F(400000, P_XO, 12, 1, 4),
+	F(20000000, P_GPLL0, 15, 1, 2),
+	F(25000000, P_GPLL0, 12, 1, 2),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 sdcc4_apps_clk_src = {
+	.cmd_rcgr = 0x16010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_sdcc4_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "sdcc4_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_blsp1_qup1_spi_apps_clk_src[] = {
+	F(960000, P_XO, 10, 1, 2),
+	F(4800000, P_XO, 4, 0, 0),
+	F(9600000, P_XO, 2, 0, 0),
+	F(15000000, P_GPLL0, 10, 1, 4),
+	F(19200000, P_XO, 1, 0, 0),
+	F(25000000, P_GPLL0, 12, 1, 2),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 blsp1_qup1_spi_apps_clk_src = {
+	.cmd_rcgr = 0x1900c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup1_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_blsp1_qup1_i2c_apps_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 blsp1_qup1_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x19020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup1_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_blsp1_uart1_apps_clk_src[] = {
+	F(3686400, P_GPLL0, 1, 96, 15625),
+	F(7372800, P_GPLL0, 1, 192, 15625),
+	F(14745600, P_GPLL0, 1, 384, 15625),
+	F(16000000, P_GPLL0, 5, 2, 15),
+	F(19200000, P_XO, 1, 0, 0),
+	F(24000000, P_GPLL0, 5, 1, 5),
+	F(32000000, P_GPLL0, 1, 4, 75),
+	F(40000000, P_GPLL0, 15, 0, 0),
+	F(46400000, P_GPLL0, 1, 29, 375),
+	F(48000000, P_GPLL0, 12.5, 0, 0),
+	F(51200000, P_GPLL0, 1, 32, 375),
+	F(56000000, P_GPLL0, 1, 7, 75),
+	F(58982400, P_GPLL0, 1, 1536, 15625),
+	F(60000000, P_GPLL0, 10, 0, 0),
+	F(63157895, P_GPLL0, 9.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 blsp1_uart1_apps_clk_src = {
+	.cmd_rcgr = 0x1a00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart1_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup2_spi_apps_clk_src = {
+	.cmd_rcgr = 0x1b00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup2_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup2_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x1b020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup2_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_uart2_apps_clk_src = {
+	.cmd_rcgr = 0x1c00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart2_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup3_spi_apps_clk_src = {
+	.cmd_rcgr = 0x1d00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup3_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup3_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x1d020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup3_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_uart3_apps_clk_src = {
+	.cmd_rcgr = 0x1e00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart3_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup4_spi_apps_clk_src = {
+	.cmd_rcgr = 0x1f00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup4_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup4_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x1f020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup4_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_uart4_apps_clk_src = {
+	.cmd_rcgr = 0x2000c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart4_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup5_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2100c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup5_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup5_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x21020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup5_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_uart5_apps_clk_src = {
+	.cmd_rcgr = 0x2200c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart5_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup6_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2300c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup6_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_qup6_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x23020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_qup6_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp1_uart6_apps_clk_src = {
+	.cmd_rcgr = 0x2400c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp1_uart6_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup1_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2600c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup1_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup1_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x26020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup1_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart1_apps_clk_src = {
+	.cmd_rcgr = 0x2700c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart1_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup2_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2800c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup2_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup2_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x28020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup2_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart2_apps_clk_src = {
+	.cmd_rcgr = 0x2900c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart2_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup3_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2a00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup3_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup3_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x2a020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup3_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart3_apps_clk_src = {
+	.cmd_rcgr = 0x2b00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart3_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup4_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2c00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup4_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup4_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x2c020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup4_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart4_apps_clk_src = {
+	.cmd_rcgr = 0x2d00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart4_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup5_spi_apps_clk_src = {
+	.cmd_rcgr = 0x2e00c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup5_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup5_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x2e020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup5_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart5_apps_clk_src = {
+	.cmd_rcgr = 0x2f00c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart5_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup6_spi_apps_clk_src = {
+	.cmd_rcgr = 0x3000c,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_spi_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup6_spi_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_qup6_i2c_apps_clk_src = {
+	.cmd_rcgr = 0x30020,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_qup1_i2c_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_qup6_i2c_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 blsp2_uart6_apps_clk_src = {
+	.cmd_rcgr = 0x3100c,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_blsp1_uart1_apps_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "blsp2_uart6_apps_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_pdm2_clk_src[] = {
+	F(60000000, P_GPLL0, 10, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 pdm2_clk_src = {
+	.cmd_rcgr = 0x33010,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_pdm2_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pdm2_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_tsif_ref_clk_src[] = {
+	F(105495, P_XO, 1, 1, 182),
+	{ }
+};
+
+static struct clk_rcg2 tsif_ref_clk_src = {
+	.cmd_rcgr = 0x36010,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_aud_ref_clk_map,
+	.freq_tbl = ftbl_tsif_ref_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "tsif_ref_clk_src",
+		.parent_names = gcc_xo_gpll0_aud_ref_clk,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 gcc_sleep_clk_src = {
+	.cmd_rcgr = 0x43014,
+	.hid_width = 5,
+	.parent_map = gcc_sleep_clk_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gcc_sleep_clk_src",
+		.parent_names = gcc_sleep_clk,
+		.num_parents = 1,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 hmss_rbcpr_clk_src = {
+	.cmd_rcgr = 0x48040,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_usb30_mock_utmi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "hmss_rbcpr_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 hmss_gpll0_clk_src = {
+	.cmd_rcgr = 0x48058,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "hmss_gpll0_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_gp1_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 gp1_clk_src = {
+	.cmd_rcgr = 0x64004,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_sleep_clk_gpll0_early_div_map,
+	.freq_tbl = ftbl_gp1_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gp1_clk_src",
+		.parent_names = gcc_xo_gpll0_sleep_clk_gpll0_early_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 gp2_clk_src = {
+	.cmd_rcgr = 0x65004,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_sleep_clk_gpll0_early_div_map,
+	.freq_tbl = ftbl_gp1_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gp2_clk_src",
+		.parent_names = gcc_xo_gpll0_sleep_clk_gpll0_early_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 gp3_clk_src = {
+	.cmd_rcgr = 0x66004,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_sleep_clk_gpll0_early_div_map,
+	.freq_tbl = ftbl_gp1_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gp3_clk_src",
+		.parent_names = gcc_xo_gpll0_sleep_clk_gpll0_early_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_pcie_aux_clk_src[] = {
+	F(1010526, P_XO, 1, 1, 19),
+	{ }
+};
+
+static struct clk_rcg2 pcie_aux_clk_src = {
+	.cmd_rcgr = 0x6c000,
+	.mnd_width = 16,
+	.hid_width = 5,
+	.parent_map = gcc_xo_sleep_clk_map,
+	.freq_tbl = ftbl_pcie_aux_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pcie_aux_clk_src",
+		.parent_names = gcc_xo_sleep_clk,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_ufs_axi_clk_src[] = {
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(240000000, P_GPLL0, 2.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 ufs_axi_clk_src = {
+	.cmd_rcgr = 0x75024,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.freq_tbl = ftbl_ufs_axi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "ufs_axi_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 ufs_ice_core_clk_src = {
+	.cmd_rcgr = 0x76014,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "ufs_ice_core_clk_src",
+		.parent_names = gcc_xo_gpll0,
+		.num_parents = 2,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 qspi_ser_clk_src = {
+	.cmd_rcgr = 0x8b00c,
+	.hid_width = 5,
+	.parent_map = gcc_xo_gpll0_gpll1_early_div_gpll1_gpll4_gpll0_early_div_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "qspi_ser_clk_src",
+		.parent_names = gcc_xo_gpll0_gpll1_early_div_gpll1_gpll4_gpll0_early_div,
+		.num_parents = 6,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_branch gcc_sys_noc_usb3_axi_clk = {
+	.halt_reg = 0x0f03c,
+	.clkr = {
+		.enable_reg = 0x0f03c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sys_noc_usb3_axi_clk",
+			.parent_names = (const char *[]){ "usb30_master_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sys_noc_ufs_axi_clk = {
+	.halt_reg = 0x75038,
+	.clkr = {
+		.enable_reg = 0x75038,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sys_noc_ufs_axi_clk",
+			.parent_names = (const char *[]){ "ufs_axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_periph_noc_usb20_ahb_clk = {
+	.halt_reg = 0x6010,
+	.clkr = {
+		.enable_reg = 0x6010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_periph_noc_usb20_ahb_clk",
+			.parent_names = (const char *[]){ "usb20_master_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_mmss_noc_cfg_ahb_clk = {
+	.halt_reg = 0x9008,
+	.clkr = {
+		.enable_reg = 0x9008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_mmss_noc_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_mmss_bimc_gfx_clk = {
+	.halt_reg = 0x9010,
+	.clkr = {
+		.enable_reg = 0x9010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_mmss_bimc_gfx_clk",
+			.flags = CLK_SET_RATE_PARENT | CLK_IS_ROOT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb30_master_clk = {
+	.halt_reg = 0x0f008,
+	.clkr = {
+		.enable_reg = 0x0f008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb30_master_clk",
+			.parent_names = (const char *[]){ "usb30_master_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb30_sleep_clk = {
+	.halt_reg = 0x0f00c,
+	.clkr = {
+		.enable_reg = 0x0f00c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb30_sleep_clk",
+			.parent_names = (const char *[]){ "gcc_sleep_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb30_mock_utmi_clk = {
+	.halt_reg = 0x0f010,
+	.clkr = {
+		.enable_reg = 0x0f010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb30_mock_utmi_clk",
+			.parent_names = (const char *[]){ "usb30_mock_utmi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb3_phy_aux_clk = {
+	.halt_reg = 0x50000,
+	.clkr = {
+		.enable_reg = 0x50000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb3_phy_aux_clk",
+			.parent_names = (const char *[]){ "usb3_phy_aux_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb3_phy_pipe_clk = {
+	.halt_reg = 0x50004,
+	.clkr = {
+		.enable_reg = 0x50004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb3_phy_pipe_clk",
+			.parent_names = (const char *[]){ "usb3_phy_pipe_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb20_master_clk = {
+	.halt_reg = 0x12004,
+	.clkr = {
+		.enable_reg = 0x12004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb20_master_clk",
+			.parent_names = (const char *[]){ "usb20_master_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb20_sleep_clk = {
+	.halt_reg = 0x12008,
+	.clkr = {
+		.enable_reg = 0x12008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb20_sleep_clk",
+			.parent_names = (const char *[]){ "gcc_sleep_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb20_mock_utmi_clk = {
+	.halt_reg = 0x1200c,
+	.clkr = {
+		.enable_reg = 0x1200c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb20_mock_utmi_clk",
+			.parent_names = (const char *[]){ "usb20_mock_utmi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb_phy_cfg_ahb2phy_clk = {
+	.halt_reg = 0x6a004,
+	.clkr = {
+		.enable_reg = 0x6a004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb_phy_cfg_ahb2phy_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc1_apps_clk = {
+	.halt_reg = 0x13004,
+	.clkr = {
+		.enable_reg = 0x13004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc1_apps_clk",
+			.parent_names = (const char *[]){ "sdcc1_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc1_ahb_clk = {
+	.halt_reg = 0x13008,
+	.clkr = {
+		.enable_reg = 0x13008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc1_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc1_ice_core_clk = {
+	.halt_reg = 0x13038,
+	.clkr = {
+		.enable_reg = 0x13038,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc1_ice_core_clk",
+			.parent_names = (const char *[]){ "sdcc1_ice_core_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc2_apps_clk = {
+	.halt_reg = 0x14004,
+	.clkr = {
+		.enable_reg = 0x14004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc2_apps_clk",
+			.parent_names = (const char *[]){ "sdcc2_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc2_ahb_clk = {
+	.halt_reg = 0x14008,
+	.clkr = {
+		.enable_reg = 0x14008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc2_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc3_apps_clk = {
+	.halt_reg = 0x15004,
+	.clkr = {
+		.enable_reg = 0x15004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc3_apps_clk",
+			.parent_names = (const char *[]){ "sdcc3_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc3_ahb_clk = {
+	.halt_reg = 0x15008,
+	.clkr = {
+		.enable_reg = 0x15008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc3_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc4_apps_clk = {
+	.halt_reg = 0x16004,
+	.clkr = {
+		.enable_reg = 0x16004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc4_apps_clk",
+			.parent_names = (const char *[]){ "sdcc4_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_sdcc4_ahb_clk = {
+	.halt_reg = 0x16008,
+	.clkr = {
+		.enable_reg = 0x16008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_sdcc4_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_ahb_clk = {
+	.halt_reg = 0x17004,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(17),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_sleep_clk = {
+	.halt_reg = 0x17008,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(16),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_sleep_clk",
+			.parent_names = (const char *[]){ "gcc_sleep_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup1_spi_apps_clk = {
+	.halt_reg = 0x19004,
+	.clkr = {
+		.enable_reg = 0x19004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup1_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup1_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup1_i2c_apps_clk = {
+	.halt_reg = 0x19008,
+	.clkr = {
+		.enable_reg = 0x19008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup1_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup1_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart1_apps_clk = {
+	.halt_reg = 0x1a004,
+	.clkr = {
+		.enable_reg = 0x1a004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart1_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart1_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup2_spi_apps_clk = {
+	.halt_reg = 0x1b004,
+	.clkr = {
+		.enable_reg = 0x1b004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup2_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup2_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup2_i2c_apps_clk = {
+	.halt_reg = 0x1b008,
+	.clkr = {
+		.enable_reg = 0x1b008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup2_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup2_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart2_apps_clk = {
+	.halt_reg = 0x1c004,
+	.clkr = {
+		.enable_reg = 0x1c004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart2_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart2_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup3_spi_apps_clk = {
+	.halt_reg = 0x1d004,
+	.clkr = {
+		.enable_reg = 0x1d004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup3_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup3_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup3_i2c_apps_clk = {
+	.halt_reg = 0x1d008,
+	.clkr = {
+		.enable_reg = 0x1d008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup3_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup3_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart3_apps_clk = {
+	.halt_reg = 0x1e004,
+	.clkr = {
+		.enable_reg = 0x1e004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart3_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart3_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup4_spi_apps_clk = {
+	.halt_reg = 0x1f004,
+	.clkr = {
+		.enable_reg = 0x1f004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup4_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup4_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup4_i2c_apps_clk = {
+	.halt_reg = 0x1f008,
+	.clkr = {
+		.enable_reg = 0x1f008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup4_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup4_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart4_apps_clk = {
+	.halt_reg = 0x20004,
+	.clkr = {
+		.enable_reg = 0x20004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart4_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart4_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup5_spi_apps_clk = {
+	.halt_reg = 0x21004,
+	.clkr = {
+		.enable_reg = 0x21004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup5_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup5_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup5_i2c_apps_clk = {
+	.halt_reg = 0x21008,
+	.clkr = {
+		.enable_reg = 0x21008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup5_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup5_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart5_apps_clk = {
+	.halt_reg = 0x22004,
+	.clkr = {
+		.enable_reg = 0x22004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart5_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart5_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup6_spi_apps_clk = {
+	.halt_reg = 0x23004,
+	.clkr = {
+		.enable_reg = 0x23004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup6_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup6_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_qup6_i2c_apps_clk = {
+	.halt_reg = 0x23008,
+	.clkr = {
+		.enable_reg = 0x23008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_qup6_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_qup6_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp1_uart6_apps_clk = {
+	.halt_reg = 0x24004,
+	.clkr = {
+		.enable_reg = 0x24004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp1_uart6_apps_clk",
+			.parent_names = (const char *[]){ "blsp1_uart6_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_ahb_clk = {
+	.halt_reg = 0x25004,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(15),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_sleep_clk = {
+	.halt_reg = 0x25008,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(14),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_sleep_clk",
+			.parent_names = (const char *[]){ "gcc_sleep_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup1_spi_apps_clk = {
+	.halt_reg = 0x26004,
+	.clkr = {
+		.enable_reg = 0x26004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup1_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup1_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup1_i2c_apps_clk = {
+	.halt_reg = 0x26008,
+	.clkr = {
+		.enable_reg = 0x26008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup1_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup1_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart1_apps_clk = {
+	.halt_reg = 0x27004,
+	.clkr = {
+		.enable_reg = 0x27004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart1_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart1_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup2_spi_apps_clk = {
+	.halt_reg = 0x28004,
+	.clkr = {
+		.enable_reg = 0x28004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup2_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup2_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup2_i2c_apps_clk = {
+	.halt_reg = 0x28008,
+	.clkr = {
+		.enable_reg = 0x28008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup2_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup2_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart2_apps_clk = {
+	.halt_reg = 0x29004,
+	.clkr = {
+		.enable_reg = 0x29004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart2_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart2_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup3_spi_apps_clk = {
+	.halt_reg = 0x2a004,
+	.clkr = {
+		.enable_reg = 0x2a004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup3_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup3_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup3_i2c_apps_clk = {
+	.halt_reg = 0x2a008,
+	.clkr = {
+		.enable_reg = 0x2a008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup3_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup3_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart3_apps_clk = {
+	.halt_reg = 0x2b004,
+	.clkr = {
+		.enable_reg = 0x2b004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart3_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart3_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup4_spi_apps_clk = {
+	.halt_reg = 0x2c004,
+	.clkr = {
+		.enable_reg = 0x2c004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup4_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup4_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup4_i2c_apps_clk = {
+	.halt_reg = 0x2c008,
+	.clkr = {
+		.enable_reg = 0x2c008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup4_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup4_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart4_apps_clk = {
+	.halt_reg = 0x2d004,
+	.clkr = {
+		.enable_reg = 0x2d004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart4_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart4_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup5_spi_apps_clk = {
+	.halt_reg = 0x2e004,
+	.clkr = {
+		.enable_reg = 0x2e004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup5_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup5_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup5_i2c_apps_clk = {
+	.halt_reg = 0x2e008,
+	.clkr = {
+		.enable_reg = 0x2e008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup5_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup5_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart5_apps_clk = {
+	.halt_reg = 0x2f004,
+	.clkr = {
+		.enable_reg = 0x2f004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart5_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart5_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup6_spi_apps_clk = {
+	.halt_reg = 0x30004,
+	.clkr = {
+		.enable_reg = 0x30004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup6_spi_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup6_spi_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_qup6_i2c_apps_clk = {
+	.halt_reg = 0x30008,
+	.clkr = {
+		.enable_reg = 0x30008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_qup6_i2c_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_qup6_i2c_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_blsp2_uart6_apps_clk = {
+	.halt_reg = 0x31004,
+	.clkr = {
+		.enable_reg = 0x31004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_blsp2_uart6_apps_clk",
+			.parent_names = (const char *[]){ "blsp2_uart6_apps_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pdm_ahb_clk = {
+	.halt_reg = 0x33004,
+	.clkr = {
+		.enable_reg = 0x33004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pdm_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pdm2_clk = {
+	.halt_reg = 0x3300c,
+	.clkr = {
+		.enable_reg = 0x3300c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pdm2_clk",
+			.parent_names = (const char *[]){ "pdm2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_prng_ahb_clk = {
+	.halt_reg = 0x34004,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(13),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_prng_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_tsif_ahb_clk = {
+	.halt_reg = 0x36004,
+	.clkr = {
+		.enable_reg = 0x36004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_tsif_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_tsif_ref_clk = {
+	.halt_reg = 0x36008,
+	.clkr = {
+		.enable_reg = 0x36008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_tsif_ref_clk",
+			.parent_names = (const char *[]){ "tsif_ref_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_tsif_inactivity_timers_clk = {
+	.halt_reg = 0x3600c,
+	.clkr = {
+		.enable_reg = 0x3600c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_tsif_inactivity_timers_clk",
+			.parent_names = (const char *[]){ "gcc_sleep_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_boot_rom_ahb_clk = {
+	.halt_reg = 0x38004,
+	.halt_check = BRANCH_HALT_VOTED,
+	.clkr = {
+		.enable_reg = 0x52004,
+		.enable_mask = BIT(10),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_boot_rom_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_bimc_gfx_clk = {
+	.halt_reg = 0x46018,
+	.clkr = {
+		.enable_reg = 0x46018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_bimc_gfx_clk",
+			.flags = CLK_SET_RATE_PARENT | CLK_IS_ROOT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_hmss_rbcpr_clk = {
+	.halt_reg = 0x4800c,
+	.clkr = {
+		.enable_reg = 0x4800c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_hmss_rbcpr_clk",
+			.parent_names = (const char *[]){ "hmss_rbcpr_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_gp1_clk = {
+	.halt_reg = 0x64000,
+	.clkr = {
+		.enable_reg = 0x64000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_gp1_clk",
+			.parent_names = (const char *[]){ "gp1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_gp2_clk = {
+	.halt_reg = 0x65000,
+	.clkr = {
+		.enable_reg = 0x65000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_gp2_clk",
+			.parent_names = (const char *[]){ "gp2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_gp3_clk = {
+	.halt_reg = 0x66000,
+	.clkr = {
+		.enable_reg = 0x66000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_gp3_clk",
+			.parent_names = (const char *[]){ "gp3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_0_slv_axi_clk = {
+	.halt_reg = 0x6b008,
+	.clkr = {
+		.enable_reg = 0x6b008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_0_slv_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_0_mstr_axi_clk = {
+	.halt_reg = 0x6b00c,
+	.clkr = {
+		.enable_reg = 0x6b00c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_0_mstr_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_0_cfg_ahb_clk = {
+	.halt_reg = 0x6b010,
+	.clkr = {
+		.enable_reg = 0x6b010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_0_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_0_aux_clk = {
+	.halt_reg = 0x6b014,
+	.clkr = {
+		.enable_reg = 0x6b014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_0_aux_clk",
+			.parent_names = (const char *[]){ "pcie_aux_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_0_pipe_clk = {
+	.halt_reg = 0x6b018,
+	.clkr = {
+		.enable_reg = 0x6b018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_0_pipe_clk",
+			.parent_names = (const char *[]){ "pcie_0_pipe_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_1_slv_axi_clk = {
+	.halt_reg = 0x6d008,
+	.clkr = {
+		.enable_reg = 0x6d008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_1_slv_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_1_mstr_axi_clk = {
+	.halt_reg = 0x6d00c,
+	.clkr = {
+		.enable_reg = 0x6d00c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_1_mstr_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_1_cfg_ahb_clk = {
+	.halt_reg = 0x6d010,
+	.clkr = {
+		.enable_reg = 0x6d010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_1_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_1_aux_clk = {
+	.halt_reg = 0x6d014,
+	.clkr = {
+		.enable_reg = 0x6d014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_1_aux_clk",
+			.parent_names = (const char *[]){ "pcie_aux_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_1_pipe_clk = {
+	.halt_reg = 0x6d018,
+	.clkr = {
+		.enable_reg = 0x6d018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_1_pipe_clk",
+			.parent_names = (const char *[]){ "pcie_1_pipe_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_2_slv_axi_clk = {
+	.halt_reg = 0x6e008,
+	.clkr = {
+		.enable_reg = 0x6e008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_2_slv_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_2_mstr_axi_clk = {
+	.halt_reg = 0x6e00c,
+	.clkr = {
+		.enable_reg = 0x6e00c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_2_mstr_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_2_cfg_ahb_clk = {
+	.halt_reg = 0x6e010,
+	.clkr = {
+		.enable_reg = 0x6e010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_2_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_2_aux_clk = {
+	.halt_reg = 0x6e014,
+	.clkr = {
+		.enable_reg = 0x6e014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_2_aux_clk",
+			.parent_names = (const char *[]){ "pcie_aux_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_2_pipe_clk = {
+	.halt_reg = 0x6e108,
+	.clkr = {
+		.enable_reg = 0x6e108,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_2_pipe_clk",
+			.parent_names = (const char *[]){ "pcie_2_pipe_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_phy_cfg_ahb_clk = {
+	.halt_reg = 0x6f004,
+	.clkr = {
+		.enable_reg = 0x6f004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_phy_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_phy_aux_clk = {
+	.halt_reg = 0x6f008,
+	.clkr = {
+		.enable_reg = 0x6f008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_phy_aux_clk",
+			.parent_names = (const char *[]){ "pcie_aux_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_axi_clk = {
+	.halt_reg = 0x75008,
+	.clkr = {
+		.enable_reg = 0x75008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_axi_clk",
+			.parent_names = (const char *[]){ "ufs_axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_ahb_clk = {
+	.halt_reg = 0x7500c,
+	.clkr = {
+		.enable_reg = 0x7500c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_fixed_factor ufs_tx_cfg_clk_src = {
+	.mult = 1,
+	.div = 16,
+	.hw.init = &(struct clk_init_data){
+		.name = "ufs_tx_cfg_clk_src",
+		.parent_names = (const char *[]){ "ufs_axi_clk_src" },
+		.num_parents = 1,
+		.flags = CLK_SET_RATE_PARENT,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_branch gcc_ufs_tx_cfg_clk = {
+	.halt_reg = 0x75010,
+	.clkr = {
+		.enable_reg = 0x75010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_tx_cfg_clk",
+			.parent_names = (const char *[]){ "ufs_tx_cfg_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_fixed_factor ufs_rx_cfg_clk_src = {
+	.mult = 1,
+	.div = 16,
+	.hw.init = &(struct clk_init_data){
+		.name = "ufs_rx_cfg_clk_src",
+		.parent_names = (const char *[]){ "ufs_axi_clk_src" },
+		.num_parents = 1,
+		.flags = CLK_SET_RATE_PARENT,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_branch gcc_ufs_rx_cfg_clk = {
+	.halt_reg = 0x75014,
+	.clkr = {
+		.enable_reg = 0x75014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_rx_cfg_clk",
+			.parent_names = (const char *[]){ "ufs_rx_cfg_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_tx_symbol_0_clk = {
+	.halt_reg = 0x75018,
+	.clkr = {
+		.enable_reg = 0x75018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_tx_symbol_0_clk",
+			.parent_names = (const char *[]){ "ufs_tx_symbol_0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_rx_symbol_0_clk = {
+	.halt_reg = 0x7501c,
+	.clkr = {
+		.enable_reg = 0x7501c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_rx_symbol_0_clk",
+			.parent_names = (const char *[]){ "ufs_rx_symbol_0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_rx_symbol_1_clk = {
+	.halt_reg = 0x75020,
+	.clkr = {
+		.enable_reg = 0x75020,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_rx_symbol_1_clk",
+			.parent_names = (const char *[]){ "ufs_rx_symbol_1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_fixed_factor ufs_ice_core_postdiv_clk_src = {
+	.mult = 1,
+	.div = 2,
+	.hw.init = &(struct clk_init_data){
+		.name = "ufs_ice_core_postdiv_clk_src",
+		.parent_names = (const char *[]){ "ufs_ice_core_clk_src" },
+		.num_parents = 1,
+		.flags = CLK_SET_RATE_PARENT,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct clk_branch gcc_ufs_unipro_core_clk = {
+	.halt_reg = 0x7600c,
+	.clkr = {
+		.enable_reg = 0x7600c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_unipro_core_clk",
+			.parent_names = (const char *[]){ "ufs_ice_core_postdiv_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_ice_core_clk = {
+	.halt_reg = 0x76010,
+	.clkr = {
+		.enable_reg = 0x76010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_ice_core_clk",
+			.parent_names = (const char *[]){ "ufs_ice_core_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_sys_clk_core_clk = {
+	.halt_check = BRANCH_HALT_DELAY,
+	.clkr = {
+		.enable_reg = 0x76030,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_sys_clk_core_clk",
+			.ops = &clk_branch2_ops,
+			.flags = CLK_IS_ROOT,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_tx_symbol_clk_core_clk = {
+	.halt_check = BRANCH_HALT_DELAY,
+	.clkr = {
+		.enable_reg = 0x76034,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_tx_symbol_clk_core_clk",
+			.ops = &clk_branch2_ops,
+			.flags = CLK_IS_ROOT,
+		},
+	},
+};
+
+static struct clk_branch gcc_aggre0_snoc_axi_clk = {
+	.halt_reg = 0x81008,
+	.clkr = {
+		.enable_reg = 0x81008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_aggre0_snoc_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_aggre0_cnoc_ahb_clk = {
+	.halt_reg = 0x8100c,
+	.clkr = {
+		.enable_reg = 0x8100c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_aggre0_cnoc_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_smmu_aggre0_axi_clk = {
+	.halt_reg = 0x81014,
+	.clkr = {
+		.enable_reg = 0x81014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_smmu_aggre0_axi_clk",
+			.parent_names = (const char *[]){ "system_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_smmu_aggre0_ahb_clk = {
+	.halt_reg = 0x81018,
+	.clkr = {
+		.enable_reg = 0x81018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_smmu_aggre0_ahb_clk",
+			.parent_names = (const char *[]){ "config_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_aggre1_pnoc_ahb_clk = {
+	.halt_reg = 0x82014,
+	.clkr = {
+		.enable_reg = 0x82014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_aggre1_pnoc_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_aggre2_ufs_axi_clk = {
+	.halt_reg = 0x83014,
+	.clkr = {
+		.enable_reg = 0x83014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_aggre2_ufs_axi_clk",
+			.parent_names = (const char *[]){ "ufs_axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_aggre2_usb3_axi_clk = {
+	.halt_reg = 0x83018,
+	.clkr = {
+		.enable_reg = 0x83018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_aggre2_usb3_axi_clk",
+			.parent_names = (const char *[]){ "usb30_master_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_qspi_ahb_clk = {
+	.halt_reg = 0x8b004,
+	.clkr = {
+		.enable_reg = 0x8b004,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_qspi_ahb_clk",
+			.parent_names = (const char *[]){ "periph_noc_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_qspi_ser_clk = {
+	.halt_reg = 0x8b008,
+	.clkr = {
+		.enable_reg = 0x8b008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_qspi_ser_clk",
+			.parent_names = (const char *[]){ "qspi_ser_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_usb3_clkref_clk = {
+	.halt_reg = 0x8800C,
+	.clkr = {
+		.enable_reg = 0x8800C,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_usb3_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_hdmi_clkref_clk = {
+	.halt_reg = 0x88000,
+	.clkr = {
+		.enable_reg = 0x88000,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_hdmi_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_ufs_clkref_clk = {
+	.halt_reg = 0x88008,
+	.clkr = {
+		.enable_reg = 0x88008,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_ufs_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_pcie_clkref_clk = {
+	.halt_reg = 0x88010,
+	.clkr = {
+		.enable_reg = 0x88010,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_pcie_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_rx2_usb2_clkref_clk = {
+	.halt_reg = 0x88014,
+	.clkr = {
+		.enable_reg = 0x88014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_rx2_usb2_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gcc_rx1_usb2_clkref_clk = {
+	.halt_reg = 0x88018,
+	.clkr = {
+		.enable_reg = 0x88018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gcc_rx1_usb2_clkref_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_hw *gcc_msm8996_hws[] = {
+	&xo.hw,
+	&gpll0_early_div.hw,
+	&ufs_tx_cfg_clk_src.hw,
+	&ufs_rx_cfg_clk_src.hw,
+	&ufs_ice_core_postdiv_clk_src.hw,
+};
+
+static struct clk_regmap *gcc_msm8996_clocks[] = {
+	[GPLL0_EARLY] = &gpll0_early.clkr,
+	[GPLL0] = &gpll0.clkr,
+	[GPLL4_EARLY] = &gpll4_early.clkr,
+	[GPLL4] = &gpll4.clkr,
+	[SYSTEM_NOC_CLK_SRC] = &system_noc_clk_src.clkr,
+	[CONFIG_NOC_CLK_SRC] = &config_noc_clk_src.clkr,
+	[PERIPH_NOC_CLK_SRC] = &periph_noc_clk_src.clkr,
+	[USB30_MASTER_CLK_SRC] = &usb30_master_clk_src.clkr,
+	[USB30_MOCK_UTMI_CLK_SRC] = &usb30_mock_utmi_clk_src.clkr,
+	[USB3_PHY_AUX_CLK_SRC] = &usb3_phy_aux_clk_src.clkr,
+	[USB20_MASTER_CLK_SRC] = &usb20_master_clk_src.clkr,
+	[USB20_MOCK_UTMI_CLK_SRC] = &usb20_mock_utmi_clk_src.clkr,
+	[SDCC1_APPS_CLK_SRC] = &sdcc1_apps_clk_src.clkr,
+	[SDCC1_ICE_CORE_CLK_SRC] = &sdcc1_ice_core_clk_src.clkr,
+	[SDCC2_APPS_CLK_SRC] = &sdcc2_apps_clk_src.clkr,
+	[SDCC3_APPS_CLK_SRC] = &sdcc3_apps_clk_src.clkr,
+	[SDCC4_APPS_CLK_SRC] = &sdcc4_apps_clk_src.clkr,
+	[BLSP1_QUP1_SPI_APPS_CLK_SRC] = &blsp1_qup1_spi_apps_clk_src.clkr,
+	[BLSP1_QUP1_I2C_APPS_CLK_SRC] = &blsp1_qup1_i2c_apps_clk_src.clkr,
+	[BLSP1_UART1_APPS_CLK_SRC] = &blsp1_uart1_apps_clk_src.clkr,
+	[BLSP1_QUP2_SPI_APPS_CLK_SRC] = &blsp1_qup2_spi_apps_clk_src.clkr,
+	[BLSP1_QUP2_I2C_APPS_CLK_SRC] = &blsp1_qup2_i2c_apps_clk_src.clkr,
+	[BLSP1_UART2_APPS_CLK_SRC] = &blsp1_uart2_apps_clk_src.clkr,
+	[BLSP1_QUP3_SPI_APPS_CLK_SRC] = &blsp1_qup3_spi_apps_clk_src.clkr,
+	[BLSP1_QUP3_I2C_APPS_CLK_SRC] = &blsp1_qup3_i2c_apps_clk_src.clkr,
+	[BLSP1_UART3_APPS_CLK_SRC] = &blsp1_uart3_apps_clk_src.clkr,
+	[BLSP1_QUP4_SPI_APPS_CLK_SRC] = &blsp1_qup4_spi_apps_clk_src.clkr,
+	[BLSP1_QUP4_I2C_APPS_CLK_SRC] = &blsp1_qup4_i2c_apps_clk_src.clkr,
+	[BLSP1_UART4_APPS_CLK_SRC] = &blsp1_uart4_apps_clk_src.clkr,
+	[BLSP1_QUP5_SPI_APPS_CLK_SRC] = &blsp1_qup5_spi_apps_clk_src.clkr,
+	[BLSP1_QUP5_I2C_APPS_CLK_SRC] = &blsp1_qup5_i2c_apps_clk_src.clkr,
+	[BLSP1_UART5_APPS_CLK_SRC] = &blsp1_uart5_apps_clk_src.clkr,
+	[BLSP1_QUP6_SPI_APPS_CLK_SRC] = &blsp1_qup6_spi_apps_clk_src.clkr,
+	[BLSP1_QUP6_I2C_APPS_CLK_SRC] = &blsp1_qup6_i2c_apps_clk_src.clkr,
+	[BLSP1_UART6_APPS_CLK_SRC] = &blsp1_uart6_apps_clk_src.clkr,
+	[BLSP2_QUP1_SPI_APPS_CLK_SRC] = &blsp2_qup1_spi_apps_clk_src.clkr,
+	[BLSP2_QUP1_I2C_APPS_CLK_SRC] = &blsp2_qup1_i2c_apps_clk_src.clkr,
+	[BLSP2_UART1_APPS_CLK_SRC] = &blsp2_uart1_apps_clk_src.clkr,
+	[BLSP2_QUP2_SPI_APPS_CLK_SRC] = &blsp2_qup2_spi_apps_clk_src.clkr,
+	[BLSP2_QUP2_I2C_APPS_CLK_SRC] = &blsp2_qup2_i2c_apps_clk_src.clkr,
+	[BLSP2_UART2_APPS_CLK_SRC] = &blsp2_uart2_apps_clk_src.clkr,
+	[BLSP2_QUP3_SPI_APPS_CLK_SRC] = &blsp2_qup3_spi_apps_clk_src.clkr,
+	[BLSP2_QUP3_I2C_APPS_CLK_SRC] = &blsp2_qup3_i2c_apps_clk_src.clkr,
+	[BLSP2_UART3_APPS_CLK_SRC] = &blsp2_uart3_apps_clk_src.clkr,
+	[BLSP2_QUP4_SPI_APPS_CLK_SRC] = &blsp2_qup4_spi_apps_clk_src.clkr,
+	[BLSP2_QUP4_I2C_APPS_CLK_SRC] = &blsp2_qup4_i2c_apps_clk_src.clkr,
+	[BLSP2_UART4_APPS_CLK_SRC] = &blsp2_uart4_apps_clk_src.clkr,
+	[BLSP2_QUP5_SPI_APPS_CLK_SRC] = &blsp2_qup5_spi_apps_clk_src.clkr,
+	[BLSP2_QUP5_I2C_APPS_CLK_SRC] = &blsp2_qup5_i2c_apps_clk_src.clkr,
+	[BLSP2_UART5_APPS_CLK_SRC] = &blsp2_uart5_apps_clk_src.clkr,
+	[BLSP2_QUP6_SPI_APPS_CLK_SRC] = &blsp2_qup6_spi_apps_clk_src.clkr,
+	[BLSP2_QUP6_I2C_APPS_CLK_SRC] = &blsp2_qup6_i2c_apps_clk_src.clkr,
+	[BLSP2_UART6_APPS_CLK_SRC] = &blsp2_uart6_apps_clk_src.clkr,
+	[PDM2_CLK_SRC] = &pdm2_clk_src.clkr,
+	[TSIF_REF_CLK_SRC] = &tsif_ref_clk_src.clkr,
+	[GCC_SLEEP_CLK_SRC] = &gcc_sleep_clk_src.clkr,
+	[HMSS_RBCPR_CLK_SRC] = &hmss_rbcpr_clk_src.clkr,
+	[HMSS_GPLL0_CLK_SRC] = &hmss_gpll0_clk_src.clkr,
+	[GP1_CLK_SRC] = &gp1_clk_src.clkr,
+	[GP2_CLK_SRC] = &gp2_clk_src.clkr,
+	[GP3_CLK_SRC] = &gp3_clk_src.clkr,
+	[PCIE_AUX_CLK_SRC] = &pcie_aux_clk_src.clkr,
+	[UFS_AXI_CLK_SRC] = &ufs_axi_clk_src.clkr,
+	[UFS_ICE_CORE_CLK_SRC] = &ufs_ice_core_clk_src.clkr,
+	[QSPI_SER_CLK_SRC] = &qspi_ser_clk_src.clkr,
+	[GCC_SYS_NOC_USB3_AXI_CLK] = &gcc_sys_noc_usb3_axi_clk.clkr,
+	[GCC_SYS_NOC_UFS_AXI_CLK] = &gcc_sys_noc_ufs_axi_clk.clkr,
+	[GCC_PERIPH_NOC_USB20_AHB_CLK] = &gcc_periph_noc_usb20_ahb_clk.clkr,
+	[GCC_MMSS_NOC_CFG_AHB_CLK] = &gcc_mmss_noc_cfg_ahb_clk.clkr,
+	[GCC_MMSS_BIMC_GFX_CLK] = &gcc_mmss_bimc_gfx_clk.clkr,
+	[GCC_USB30_MASTER_CLK] = &gcc_usb30_master_clk.clkr,
+	[GCC_USB30_SLEEP_CLK] = &gcc_usb30_sleep_clk.clkr,
+	[GCC_USB30_MOCK_UTMI_CLK] = &gcc_usb30_mock_utmi_clk.clkr,
+	[GCC_USB3_PHY_AUX_CLK] = &gcc_usb3_phy_aux_clk.clkr,
+	[GCC_USB3_PHY_PIPE_CLK] = &gcc_usb3_phy_pipe_clk.clkr,
+	[GCC_USB20_MASTER_CLK] = &gcc_usb20_master_clk.clkr,
+	[GCC_USB20_SLEEP_CLK] = &gcc_usb20_sleep_clk.clkr,
+	[GCC_USB20_MOCK_UTMI_CLK] = &gcc_usb20_mock_utmi_clk.clkr,
+	[GCC_USB_PHY_CFG_AHB2PHY_CLK] = &gcc_usb_phy_cfg_ahb2phy_clk.clkr,
+	[GCC_SDCC1_APPS_CLK] = &gcc_sdcc1_apps_clk.clkr,
+	[GCC_SDCC1_AHB_CLK] = &gcc_sdcc1_ahb_clk.clkr,
+	[GCC_SDCC1_ICE_CORE_CLK] = &gcc_sdcc1_ice_core_clk.clkr,
+	[GCC_SDCC2_APPS_CLK] = &gcc_sdcc2_apps_clk.clkr,
+	[GCC_SDCC2_AHB_CLK] = &gcc_sdcc2_ahb_clk.clkr,
+	[GCC_SDCC3_APPS_CLK] = &gcc_sdcc3_apps_clk.clkr,
+	[GCC_SDCC3_AHB_CLK] = &gcc_sdcc3_ahb_clk.clkr,
+	[GCC_SDCC4_APPS_CLK] = &gcc_sdcc4_apps_clk.clkr,
+	[GCC_SDCC4_AHB_CLK] = &gcc_sdcc4_ahb_clk.clkr,
+	[GCC_BLSP1_AHB_CLK] = &gcc_blsp1_ahb_clk.clkr,
+	[GCC_BLSP1_SLEEP_CLK] = &gcc_blsp1_sleep_clk.clkr,
+	[GCC_BLSP1_QUP1_SPI_APPS_CLK] = &gcc_blsp1_qup1_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP1_I2C_APPS_CLK] = &gcc_blsp1_qup1_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART1_APPS_CLK] = &gcc_blsp1_uart1_apps_clk.clkr,
+	[GCC_BLSP1_QUP2_SPI_APPS_CLK] = &gcc_blsp1_qup2_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP2_I2C_APPS_CLK] = &gcc_blsp1_qup2_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART2_APPS_CLK] = &gcc_blsp1_uart2_apps_clk.clkr,
+	[GCC_BLSP1_QUP3_SPI_APPS_CLK] = &gcc_blsp1_qup3_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP3_I2C_APPS_CLK] = &gcc_blsp1_qup3_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART3_APPS_CLK] = &gcc_blsp1_uart3_apps_clk.clkr,
+	[GCC_BLSP1_QUP4_SPI_APPS_CLK] = &gcc_blsp1_qup4_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP4_I2C_APPS_CLK] = &gcc_blsp1_qup4_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART4_APPS_CLK] = &gcc_blsp1_uart4_apps_clk.clkr,
+	[GCC_BLSP1_QUP5_SPI_APPS_CLK] = &gcc_blsp1_qup5_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP5_I2C_APPS_CLK] = &gcc_blsp1_qup5_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART5_APPS_CLK] = &gcc_blsp1_uart5_apps_clk.clkr,
+	[GCC_BLSP1_QUP6_SPI_APPS_CLK] = &gcc_blsp1_qup6_spi_apps_clk.clkr,
+	[GCC_BLSP1_QUP6_I2C_APPS_CLK] = &gcc_blsp1_qup6_i2c_apps_clk.clkr,
+	[GCC_BLSP1_UART6_APPS_CLK] = &gcc_blsp1_uart6_apps_clk.clkr,
+	[GCC_BLSP2_AHB_CLK] = &gcc_blsp2_ahb_clk.clkr,
+	[GCC_BLSP2_SLEEP_CLK] = &gcc_blsp2_sleep_clk.clkr,
+	[GCC_BLSP2_QUP1_SPI_APPS_CLK] = &gcc_blsp2_qup1_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP1_I2C_APPS_CLK] = &gcc_blsp2_qup1_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART1_APPS_CLK] = &gcc_blsp2_uart1_apps_clk.clkr,
+	[GCC_BLSP2_QUP2_SPI_APPS_CLK] = &gcc_blsp2_qup2_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP2_I2C_APPS_CLK] = &gcc_blsp2_qup2_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART2_APPS_CLK] = &gcc_blsp2_uart2_apps_clk.clkr,
+	[GCC_BLSP2_QUP3_SPI_APPS_CLK] = &gcc_blsp2_qup3_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP3_I2C_APPS_CLK] = &gcc_blsp2_qup3_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART3_APPS_CLK] = &gcc_blsp2_uart3_apps_clk.clkr,
+	[GCC_BLSP2_QUP4_SPI_APPS_CLK] = &gcc_blsp2_qup4_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP4_I2C_APPS_CLK] = &gcc_blsp2_qup4_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART4_APPS_CLK] = &gcc_blsp2_uart4_apps_clk.clkr,
+	[GCC_BLSP2_QUP5_SPI_APPS_CLK] = &gcc_blsp2_qup5_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP5_I2C_APPS_CLK] = &gcc_blsp2_qup5_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART5_APPS_CLK] = &gcc_blsp2_uart5_apps_clk.clkr,
+	[GCC_BLSP2_QUP6_SPI_APPS_CLK] = &gcc_blsp2_qup6_spi_apps_clk.clkr,
+	[GCC_BLSP2_QUP6_I2C_APPS_CLK] = &gcc_blsp2_qup6_i2c_apps_clk.clkr,
+	[GCC_BLSP2_UART6_APPS_CLK] = &gcc_blsp2_uart6_apps_clk.clkr,
+	[GCC_PDM_AHB_CLK] = &gcc_pdm_ahb_clk.clkr,
+	[GCC_PDM2_CLK] = &gcc_pdm2_clk.clkr,
+	[GCC_PRNG_AHB_CLK] = &gcc_prng_ahb_clk.clkr,
+	[GCC_TSIF_AHB_CLK] = &gcc_tsif_ahb_clk.clkr,
+	[GCC_TSIF_REF_CLK] = &gcc_tsif_ref_clk.clkr,
+	[GCC_TSIF_INACTIVITY_TIMERS_CLK] = &gcc_tsif_inactivity_timers_clk.clkr,
+	[GCC_BOOT_ROM_AHB_CLK] = &gcc_boot_rom_ahb_clk.clkr,
+	[GCC_BIMC_GFX_CLK] = &gcc_bimc_gfx_clk.clkr,
+	[GCC_HMSS_RBCPR_CLK] = &gcc_hmss_rbcpr_clk.clkr,
+	[GCC_GP1_CLK] = &gcc_gp1_clk.clkr,
+	[GCC_GP2_CLK] = &gcc_gp2_clk.clkr,
+	[GCC_GP3_CLK] = &gcc_gp3_clk.clkr,
+	[GCC_PCIE_0_SLV_AXI_CLK] = &gcc_pcie_0_slv_axi_clk.clkr,
+	[GCC_PCIE_0_MSTR_AXI_CLK] = &gcc_pcie_0_mstr_axi_clk.clkr,
+	[GCC_PCIE_0_CFG_AHB_CLK] = &gcc_pcie_0_cfg_ahb_clk.clkr,
+	[GCC_PCIE_0_AUX_CLK] = &gcc_pcie_0_aux_clk.clkr,
+	[GCC_PCIE_0_PIPE_CLK] = &gcc_pcie_0_pipe_clk.clkr,
+	[GCC_PCIE_1_SLV_AXI_CLK] = &gcc_pcie_1_slv_axi_clk.clkr,
+	[GCC_PCIE_1_MSTR_AXI_CLK] = &gcc_pcie_1_mstr_axi_clk.clkr,
+	[GCC_PCIE_1_CFG_AHB_CLK] = &gcc_pcie_1_cfg_ahb_clk.clkr,
+	[GCC_PCIE_1_AUX_CLK] = &gcc_pcie_1_aux_clk.clkr,
+	[GCC_PCIE_1_PIPE_CLK] = &gcc_pcie_1_pipe_clk.clkr,
+	[GCC_PCIE_2_SLV_AXI_CLK] = &gcc_pcie_2_slv_axi_clk.clkr,
+	[GCC_PCIE_2_MSTR_AXI_CLK] = &gcc_pcie_2_mstr_axi_clk.clkr,
+	[GCC_PCIE_2_CFG_AHB_CLK] = &gcc_pcie_2_cfg_ahb_clk.clkr,
+	[GCC_PCIE_2_AUX_CLK] = &gcc_pcie_2_aux_clk.clkr,
+	[GCC_PCIE_2_PIPE_CLK] = &gcc_pcie_2_pipe_clk.clkr,
+	[GCC_PCIE_PHY_CFG_AHB_CLK] = &gcc_pcie_phy_cfg_ahb_clk.clkr,
+	[GCC_PCIE_PHY_AUX_CLK] = &gcc_pcie_phy_aux_clk.clkr,
+	[GCC_UFS_AXI_CLK] = &gcc_ufs_axi_clk.clkr,
+	[GCC_UFS_AHB_CLK] = &gcc_ufs_ahb_clk.clkr,
+	[GCC_UFS_TX_CFG_CLK] = &gcc_ufs_tx_cfg_clk.clkr,
+	[GCC_UFS_RX_CFG_CLK] = &gcc_ufs_rx_cfg_clk.clkr,
+	[GCC_UFS_TX_SYMBOL_0_CLK] = &gcc_ufs_tx_symbol_0_clk.clkr,
+	[GCC_UFS_RX_SYMBOL_0_CLK] = &gcc_ufs_rx_symbol_0_clk.clkr,
+	[GCC_UFS_RX_SYMBOL_1_CLK] = &gcc_ufs_rx_symbol_1_clk.clkr,
+	[GCC_UFS_UNIPRO_CORE_CLK] = &gcc_ufs_unipro_core_clk.clkr,
+	[GCC_UFS_ICE_CORE_CLK] = &gcc_ufs_ice_core_clk.clkr,
+	[GCC_UFS_SYS_CLK_CORE_CLK] = &gcc_ufs_sys_clk_core_clk.clkr,
+	[GCC_UFS_TX_SYMBOL_CLK_CORE_CLK] = &gcc_ufs_tx_symbol_clk_core_clk.clkr,
+	[GCC_AGGRE0_SNOC_AXI_CLK] = &gcc_aggre0_snoc_axi_clk.clkr,
+	[GCC_AGGRE0_CNOC_AHB_CLK] = &gcc_aggre0_cnoc_ahb_clk.clkr,
+	[GCC_SMMU_AGGRE0_AXI_CLK] = &gcc_smmu_aggre0_axi_clk.clkr,
+	[GCC_SMMU_AGGRE0_AHB_CLK] = &gcc_smmu_aggre0_ahb_clk.clkr,
+	[GCC_AGGRE1_PNOC_AHB_CLK] = &gcc_aggre1_pnoc_ahb_clk.clkr,
+	[GCC_AGGRE2_UFS_AXI_CLK] = &gcc_aggre2_ufs_axi_clk.clkr,
+	[GCC_AGGRE2_USB3_AXI_CLK] = &gcc_aggre2_usb3_axi_clk.clkr,
+	[GCC_QSPI_AHB_CLK] = &gcc_qspi_ahb_clk.clkr,
+	[GCC_QSPI_SER_CLK] = &gcc_qspi_ser_clk.clkr,
+	[GCC_USB3_CLKREF_CLK] = &gcc_usb3_clkref_clk.clkr,
+	[GCC_HDMI_CLKREF_CLK] = &gcc_hdmi_clkref_clk.clkr,
+	[GCC_UFS_CLKREF_CLK] = &gcc_ufs_clkref_clk.clkr,
+	[GCC_PCIE_CLKREF_CLK] = &gcc_pcie_clkref_clk.clkr,
+	[GCC_RX2_USB2_CLKREF_CLK] = &gcc_rx2_usb2_clkref_clk.clkr,
+	[GCC_RX1_USB2_CLKREF_CLK] = &gcc_rx1_usb2_clkref_clk.clkr,
+};
+
+static const struct qcom_reset_map gcc_msm8996_resets[] = {
+	[GCC_SYSTEM_NOC_BCR] = { 0x4000 },
+	[GCC_CONFIG_NOC_BCR] = { 0x5000 },
+	[GCC_PERIPH_NOC_BCR] = { 0x6000 },
+	[GCC_IMEM_BCR] = { 0x8000 },
+	[GCC_MMSS_BCR] = { 0x9000 },
+	[GCC_PIMEM_BCR] = { 0x0a000 },
+	[GCC_QDSS_BCR] = { 0x0c000 },
+	[GCC_USB_30_BCR] = { 0x0f000 },
+	[GCC_USB_20_BCR] = { 0x12000 },
+	[GCC_QUSB2PHY_PRIM_BCR] = { 0x12038 },
+	[GCC_QUSB2PHY_SEC_BCR] = { 0x1203c },
+	[GCC_USB_PHY_CFG_AHB2PHY_BCR] = { 0x6a000 },
+	[GCC_SDCC1_BCR] = { 0x13000 },
+	[GCC_SDCC2_BCR] = { 0x14000 },
+	[GCC_SDCC3_BCR] = { 0x15000 },
+	[GCC_SDCC4_BCR] = { 0x16000 },
+	[GCC_BLSP1_BCR] = { 0x17000 },
+	[GCC_BLSP1_QUP1_BCR] = { 0x19000 },
+	[GCC_BLSP1_UART1_BCR] = { 0x1a000 },
+	[GCC_BLSP1_QUP2_BCR] = { 0x1b000 },
+	[GCC_BLSP1_UART2_BCR] = { 0x1c000 },
+	[GCC_BLSP1_QUP3_BCR] = { 0x1d000 },
+	[GCC_BLSP1_UART3_BCR] = { 0x1e000 },
+	[GCC_BLSP1_QUP4_BCR] = { 0x1f000 },
+	[GCC_BLSP1_UART4_BCR] = { 0x20000 },
+	[GCC_BLSP1_QUP5_BCR] = { 0x21000 },
+	[GCC_BLSP1_UART5_BCR] = { 0x22000 },
+	[GCC_BLSP1_QUP6_BCR] = { 0x23000 },
+	[GCC_BLSP1_UART6_BCR] = { 0x24000 },
+	[GCC_BLSP2_BCR] = { 0x25000 },
+	[GCC_BLSP2_QUP1_BCR] = { 0x26000 },
+	[GCC_BLSP2_UART1_BCR] = { 0x27000 },
+	[GCC_BLSP2_QUP2_BCR] = { 0x28000 },
+	[GCC_BLSP2_UART2_BCR] = { 0x29000 },
+	[GCC_BLSP2_QUP3_BCR] = { 0x2a000 },
+	[GCC_BLSP2_UART3_BCR] = { 0x2b000 },
+	[GCC_BLSP2_QUP4_BCR] = { 0x2c000 },
+	[GCC_BLSP2_UART4_BCR] = { 0x2d000 },
+	[GCC_BLSP2_QUP5_BCR] = { 0x2e000 },
+	[GCC_BLSP2_UART5_BCR] = { 0x2f000 },
+	[GCC_BLSP2_QUP6_BCR] = { 0x30000 },
+	[GCC_BLSP2_UART6_BCR] = { 0x31000 },
+	[GCC_PDM_BCR] = { 0x33000 },
+	[GCC_PRNG_BCR] = { 0x34000 },
+	[GCC_TSIF_BCR] = { 0x36000 },
+	[GCC_TCSR_BCR] = { 0x37000 },
+	[GCC_BOOT_ROM_BCR] = { 0x38000 },
+	[GCC_MSG_RAM_BCR] = { 0x39000 },
+	[GCC_TLMM_BCR] = { 0x3a000 },
+	[GCC_MPM_BCR] = { 0x3b000 },
+	[GCC_SEC_CTRL_BCR] = { 0x3d000 },
+	[GCC_SPMI_BCR] = { 0x3f000 },
+	[GCC_SPDM_BCR] = { 0x40000 },
+	[GCC_CE1_BCR] = { 0x41000 },
+	[GCC_BIMC_BCR] = { 0x44000 },
+	[GCC_SNOC_BUS_TIMEOUT0_BCR] = { 0x49000 },
+	[GCC_SNOC_BUS_TIMEOUT2_BCR] = { 0x49008 },
+	[GCC_SNOC_BUS_TIMEOUT1_BCR] = { 0x49010 },
+	[GCC_SNOC_BUS_TIMEOUT3_BCR] = { 0x49018 },
+	[GCC_SNOC_BUS_TIMEOUT_EXTREF_BCR] = { 0x49020 },
+	[GCC_PNOC_BUS_TIMEOUT0_BCR] = { 0x4a000 },
+	[GCC_PNOC_BUS_TIMEOUT1_BCR] = { 0x4a008 },
+	[GCC_PNOC_BUS_TIMEOUT2_BCR] = { 0x4a010 },
+	[GCC_PNOC_BUS_TIMEOUT3_BCR] = { 0x4a018 },
+	[GCC_PNOC_BUS_TIMEOUT4_BCR] = { 0x4a020 },
+	[GCC_CNOC_BUS_TIMEOUT0_BCR] = { 0x4b000 },
+	[GCC_CNOC_BUS_TIMEOUT1_BCR] = { 0x4b008 },
+	[GCC_CNOC_BUS_TIMEOUT2_BCR] = { 0x4b010 },
+	[GCC_CNOC_BUS_TIMEOUT3_BCR] = { 0x4b018 },
+	[GCC_CNOC_BUS_TIMEOUT4_BCR] = { 0x4b020 },
+	[GCC_CNOC_BUS_TIMEOUT5_BCR] = { 0x4b028 },
+	[GCC_CNOC_BUS_TIMEOUT6_BCR] = { 0x4b030 },
+	[GCC_CNOC_BUS_TIMEOUT7_BCR] = { 0x4b038 },
+	[GCC_CNOC_BUS_TIMEOUT8_BCR] = { 0x80000 },
+	[GCC_CNOC_BUS_TIMEOUT9_BCR] = { 0x80008 },
+	[GCC_CNOC_BUS_TIMEOUT_EXTREF_BCR] = { 0x80010 },
+	[GCC_APB2JTAG_BCR] = { 0x4c000 },
+	[GCC_RBCPR_CX_BCR] = { 0x4e000 },
+	[GCC_RBCPR_MX_BCR] = { 0x4f000 },
+	[GCC_PCIE_0_BCR] = { 0x6b000 },
+	[GCC_PCIE_0_PHY_BCR] = { 0x6c01c },
+	[GCC_PCIE_1_BCR] = { 0x6d000 },
+	[GCC_PCIE_1_PHY_BCR] = { 0x6d038 },
+	[GCC_PCIE_2_BCR] = { 0x6e000 },
+	[GCC_PCIE_2_PHY_BCR] = { 0x6e038 },
+	[GCC_PCIE_PHY_BCR] = { 0x6f000 },
+	[GCC_DCD_BCR] = { 0x70000 },
+	[GCC_OBT_ODT_BCR] = { 0x73000 },
+	[GCC_UFS_BCR] = { 0x75000 },
+	[GCC_SSC_BCR] = { 0x63000 },
+	[GCC_VS_BCR] = { 0x7a000 },
+	[GCC_AGGRE0_NOC_BCR] = { 0x81000 },
+	[GCC_AGGRE1_NOC_BCR] = { 0x82000 },
+	[GCC_AGGRE2_NOC_BCR] = { 0x83000 },
+	[GCC_DCC_BCR] = { 0x84000 },
+	[GCC_IPA_BCR] = { 0x89000 },
+	[GCC_QSPI_BCR] = { 0x8b000 },
+	[GCC_SKL_BCR] = { 0x8c000 },
+	[GCC_MSMPU_BCR] = { 0x8d000 },
+	[GCC_MSS_Q6_BCR] = { 0x8e000 },
+	[GCC_QREFS_VBG_CAL_BCR] = { 0x88020 },
+};
+
+static const struct regmap_config gcc_msm8996_regmap_config = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0x8f010,
+	.fast_io	= true,
+};
+
+static const struct qcom_cc_desc gcc_msm8996_desc = {
+	.config = &gcc_msm8996_regmap_config,
+	.clks = gcc_msm8996_clocks,
+	.num_clks = ARRAY_SIZE(gcc_msm8996_clocks),
+	.resets = gcc_msm8996_resets,
+	.num_resets = ARRAY_SIZE(gcc_msm8996_resets),
+};
+
+static const struct of_device_id gcc_msm8996_match_table[] = {
+	{ .compatible = "qcom,gcc-msm8996" },
+	{ }
+};
+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;
+	struct regmap *regmap;
+
+	regmap = qcom_cc_map(pdev, &gcc_msm8996_desc);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	/*
+	 * Set the HMSS_AHB_CLK_SLEEP_ENA bit to allow the hmss_ahb_clk to be
+	 * turned off by hardware during certain apps low power modes.
+	 */
+	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);
+	}
+
+	return qcom_cc_really_probe(pdev, &gcc_msm8996_desc, regmap);
+}
+
+static struct platform_driver gcc_msm8996_driver = {
+	.probe		= gcc_msm8996_probe,
+	.driver		= {
+		.name	= "gcc-msm8996",
+		.of_match_table = gcc_msm8996_match_table,
+	},
+};
+
+static int __init gcc_msm8996_init(void)
+{
+	return platform_driver_register(&gcc_msm8996_driver);
+}
+core_initcall(gcc_msm8996_init);
+
+static void __exit gcc_msm8996_exit(void)
+{
+	platform_driver_unregister(&gcc_msm8996_driver);
+}
+module_exit(gcc_msm8996_exit);
+
+MODULE_DESCRIPTION("QCOM GCC MSM8996 Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:gcc-msm8996");
diff --git a/drivers/clk/qcom/mmcc-msm8996.c b/drivers/clk/qcom/mmcc-msm8996.c
new file mode 100644
index 000000000000..064f3eaa39d0
--- /dev/null
+++ b/drivers/clk/qcom/mmcc-msm8996.c
@@ -0,0 +1,3217 @@
+/*x
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * 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 <linux/clk.h>
+
+#include <dt-bindings/clock/qcom,mmcc-msm8996.h>
+
+#include "common.h"
+#include "clk-regmap.h"
+#include "clk-regmap-divider.h"
+#include "clk-alpha-pll.h"
+#include "clk-rcg.h"
+#include "clk-branch.h"
+#include "reset.h"
+
+#define F(f, s, h, m, n) { (f), (s), (2 * (h) - 1), (m), (n) }
+
+enum {
+	P_XO,
+	P_MMPLL0,
+	P_GPLL0,
+	P_GPLL0_DIV,
+	P_MMPLL1,
+	P_MMPLL9,
+	P_MMPLL2,
+	P_MMPLL8,
+	P_MMPLL3,
+	P_DSI0PLL,
+	P_DSI1PLL,
+	P_MMPLL5,
+	P_HDMIPLL,
+	P_DSI0PLL_BYTE,
+	P_DSI1PLL_BYTE,
+	P_MMPLL4,
+};
+
+static const struct parent_map mmss_xo_hdmi_map[] = {
+	{ P_XO, 0 },
+	{ P_HDMIPLL, 1 }
+};
+
+static const char * const mmss_xo_hdmi[] = {
+	"xo",
+	"hdmipll"
+};
+
+static const struct parent_map mmss_xo_dsi0pll_dsi1pll_map[] = {
+	{ P_XO, 0 },
+	{ P_DSI0PLL, 1 },
+	{ P_DSI1PLL, 2 }
+};
+
+static const char * const mmss_xo_dsi0pll_dsi1pll[] = {
+	"xo",
+	"dsi0pll",
+	"dsi1pll"
+};
+
+static const struct parent_map mmss_xo_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_gpll0_gpll0_div[] = {
+	"xo",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_dsibyte_map[] = {
+	{ P_XO, 0 },
+	{ P_DSI0PLL_BYTE, 1 },
+	{ P_DSI1PLL_BYTE, 2 }
+};
+
+static const char * const mmss_xo_dsibyte[] = {
+	"xo",
+	"dsi0pllbyte",
+	"dsi1pllbyte"
+};
+
+static const struct parent_map mmss_xo_mmpll0_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL1, 2 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll1",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL3, 3 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll3",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll5_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL5, 2 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll5_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll5",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL4, 3 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll4",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL9, 2 },
+	{ P_MMPLL2, 3 },
+	{ P_MMPLL8, 4 },
+	{ P_GPLL0, 5 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0[] = {
+	"xo",
+	"mmpll0",
+	"mmpll9",
+	"mmpll2",
+	"mmpll8",
+	"gpll0"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL9, 2 },
+	{ P_MMPLL2, 3 },
+	{ P_MMPLL8, 4 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll9",
+	"mmpll2",
+	"mmpll8",
+	"gpll0",
+	"gpll0_div"
+};
+
+static const struct parent_map mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map[] = {
+	{ P_XO, 0 },
+	{ P_MMPLL0, 1 },
+	{ P_MMPLL1, 2 },
+	{ P_MMPLL4, 3 },
+	{ P_MMPLL3, 4 },
+	{ P_GPLL0, 5 },
+	{ P_GPLL0_DIV, 6 }
+};
+
+static const char * const mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div[] = {
+	"xo",
+	"mmpll0",
+	"mmpll1",
+	"mmpll4",
+	"mmpll3",
+	"gpll0",
+	"gpll0_div"
+};
+
+static struct clk_fixed_factor gpll0_div = {
+	.mult = 1,
+	.div = 2,
+	.hw.init = &(struct clk_init_data){
+		.name = "gpll0_div",
+		.parent_names = (const char *[]){ "gpll0" },
+		.num_parents = 1,
+		.ops = &clk_fixed_factor_ops,
+	},
+};
+
+static struct pll_vco mmpll_p_vco[] = {
+	{ 250000000, 500000000, 3 },
+	{ 500000000, 1000000000, 2 },
+	{ 1000000000, 1500000000, 1 },
+	{ 1500000000, 2000000000, 0 },
+};
+
+static struct pll_vco mmpll_gfx_vco[] = {
+	{ 400000000, 1000000000, 2 },
+	{ 1000000000, 1500000000, 1 },
+	{ 1500000000, 2000000000, 0 },
+};
+
+static struct pll_vco mmpll_t_vco[] = {
+	{ 500000000, 1500000000, 0 },
+};
+
+static struct clk_alpha_pll mmpll0_early = {
+	.offset = 0x0,
+	.vco_table = mmpll_p_vco,
+	.num_vco = ARRAY_SIZE(mmpll_p_vco),
+	.clkr = {
+		.enable_reg = 0x100,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmpll0_early",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_alpha_pll_ops,
+		},
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll0 = {
+	.offset = 0x0,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll0",
+		.parent_names = (const char *[]){ "mmpll0_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll1_early = {
+	.offset = 0x30,
+	.vco_table = mmpll_p_vco,
+	.num_vco = ARRAY_SIZE(mmpll_p_vco),
+	.clkr = {
+		.enable_reg = 0x100,
+		.enable_mask = BIT(1),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmpll1_early",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_alpha_pll_ops,
+		}
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll1 = {
+	.offset = 0x30,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll1",
+		.parent_names = (const char *[]){ "mmpll1_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll2_early = {
+	.offset = 0x4100,
+	.vco_table = mmpll_gfx_vco,
+	.num_vco = ARRAY_SIZE(mmpll_gfx_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll2_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll2 = {
+	.offset = 0x4100,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll2",
+		.parent_names = (const char *[]){ "mmpll2_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll3_early = {
+	.offset = 0x60,
+	.vco_table = mmpll_p_vco,
+	.num_vco = ARRAY_SIZE(mmpll_p_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll3_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll3 = {
+	.offset = 0x60,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll3",
+		.parent_names = (const char *[]){ "mmpll3_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll4_early = {
+	.offset = 0x90,
+	.vco_table = mmpll_t_vco,
+	.num_vco = ARRAY_SIZE(mmpll_t_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll4_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll4 = {
+	.offset = 0x90,
+	.width = 2,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll4",
+		.parent_names = (const char *[]){ "mmpll4_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll5_early = {
+	.offset = 0xc0,
+	.vco_table = mmpll_p_vco,
+	.num_vco = ARRAY_SIZE(mmpll_p_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll5_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll5 = {
+	.offset = 0xc0,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll5",
+		.parent_names = (const char *[]){ "mmpll5_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll8_early = {
+	.offset = 0x4130,
+	.vco_table = mmpll_gfx_vco,
+	.num_vco = ARRAY_SIZE(mmpll_gfx_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll8_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll8 = {
+	.offset = 0x4130,
+	.width = 4,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll8",
+		.parent_names = (const char *[]){ "mmpll8_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_alpha_pll mmpll9_early = {
+	.offset = 0x4200,
+	.vco_table = mmpll_t_vco,
+	.num_vco = ARRAY_SIZE(mmpll_t_vco),
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll9_early",
+		.parent_names = (const char *[]){ "xo" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_ops,
+	},
+};
+
+static struct clk_alpha_pll_postdiv mmpll9 = {
+	.offset = 0x4200,
+	.width = 2,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mmpll9",
+		.parent_names = (const char *[]){ "mmpll9_early" },
+		.num_parents = 1,
+		.ops = &clk_alpha_pll_postdiv_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static const struct freq_tbl ftbl_ahb_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(40000000, P_GPLL0_DIV, 7.5, 0, 0),
+	F(80000000, P_MMPLL0, 10, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 ahb_clk_src = {
+	.cmd_rcgr = 0x5000,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_ahb_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "ahb_clk_src",
+		.parent_names = mmss_xo_mmpll0_gpll0_gpll0_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_axi_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(75000000, P_GPLL0_DIV, 4, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(171430000, P_GPLL0, 3.5, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(320000000, P_MMPLL0, 2.5, 0, 0),
+	F(400000000, P_MMPLL0, 2, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 axi_clk_src = {
+	.cmd_rcgr = 0x5040,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_axi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "axi_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 maxi_clk_src = {
+	.cmd_rcgr = 0x5090,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_axi_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "maxi_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 gfx3d_clk_src = {
+	.cmd_rcgr = 0x4000,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "gfx3d_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0,
+		.num_parents = 6,
+		.ops = &clk_gfx3d_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static const struct freq_tbl ftbl_rbbmtimer_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 rbbmtimer_clk_src = {
+	.cmd_rcgr = 0x4090,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_rbbmtimer_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "rbbmtimer_clk_src",
+		.parent_names = mmss_xo_mmpll0_gpll0_gpll0_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 isense_clk_src = {
+	.cmd_rcgr = 0x4010,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_gpll0_div_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "isense_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll9_mmpll2_mmpll8_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_rbcpr_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 rbcpr_clk_src = {
+	.cmd_rcgr = 0x4060,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_rbcpr_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "rbcpr_clk_src",
+		.parent_names = mmss_xo_mmpll0_gpll0_gpll0_div,
+		.num_parents = 4,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_video_core_clk_src[] = {
+	F(75000000, P_GPLL0_DIV, 4, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	F(346666667, P_MMPLL3, 3, 0, 0),
+	F(520000000, P_MMPLL3, 2, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 video_core_clk_src = {
+	.cmd_rcgr = 0x1000,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_video_core_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "video_core_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 video_subcore0_clk_src = {
+	.cmd_rcgr = 0x1060,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_video_core_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "video_subcore0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 video_subcore1_clk_src = {
+	.cmd_rcgr = 0x1080,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_video_core_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "video_subcore1_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll3_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 pclk0_clk_src = {
+	.cmd_rcgr = 0x2000,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsi0pll_dsi1pll_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pclk0_clk_src",
+		.parent_names = mmss_xo_dsi0pll_dsi1pll,
+		.num_parents = 3,
+		.ops = &clk_pixel_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_rcg2 pclk1_clk_src = {
+	.cmd_rcgr = 0x2020,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsi0pll_dsi1pll_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pclk1_clk_src",
+		.parent_names = mmss_xo_dsi0pll_dsi1pll,
+		.num_parents = 3,
+		.ops = &clk_pixel_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static const struct freq_tbl ftbl_mdp_clk_src[] = {
+	F(85714286, P_GPLL0, 7, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	F(171428571, P_GPLL0, 3.5, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(275000000, P_MMPLL5, 3, 0, 0),
+	F(300000000, P_GPLL0, 2, 0, 0),
+	F(330000000, P_MMPLL5, 2.5, 0, 0),
+	F(412500000, P_MMPLL5, 2, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 mdp_clk_src = {
+	.cmd_rcgr = 0x2040,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll5_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mdp_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mdp_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll5_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct freq_tbl extpclk_freq_tbl[] = {
+	{ .src = P_HDMIPLL },
+	{ }
+};
+
+static struct clk_rcg2 extpclk_clk_src = {
+	.cmd_rcgr = 0x2060,
+	.hid_width = 5,
+	.parent_map = mmss_xo_hdmi_map,
+	.freq_tbl = extpclk_freq_tbl,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "extpclk_clk_src",
+		.parent_names = mmss_xo_hdmi,
+		.num_parents = 2,
+		.ops = &clk_byte_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct freq_tbl ftbl_mdss_vsync_clk[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 vsync_clk_src = {
+	.cmd_rcgr = 0x2080,
+	.hid_width = 5,
+	.parent_map = mmss_xo_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mdss_vsync_clk,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "vsync_clk_src",
+		.parent_names = mmss_xo_gpll0_gpll0_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct freq_tbl ftbl_mdss_hdmi_clk[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 hdmi_clk_src = {
+	.cmd_rcgr = 0x2100,
+	.hid_width = 5,
+	.parent_map = mmss_xo_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mdss_hdmi_clk,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "hdmi_clk_src",
+		.parent_names = mmss_xo_gpll0_gpll0_div,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 byte0_clk_src = {
+	.cmd_rcgr = 0x2120,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsibyte_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "byte0_clk_src",
+		.parent_names = mmss_xo_dsibyte,
+		.num_parents = 3,
+		.ops = &clk_byte2_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct clk_rcg2 byte1_clk_src = {
+	.cmd_rcgr = 0x2140,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsibyte_map,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "byte1_clk_src",
+		.parent_names = mmss_xo_dsibyte,
+		.num_parents = 3,
+		.ops = &clk_byte2_ops,
+		.flags = CLK_SET_RATE_PARENT,
+	},
+};
+
+static struct freq_tbl ftbl_mdss_esc0_1_clk[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 esc0_clk_src = {
+	.cmd_rcgr = 0x2160,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsibyte_map,
+	.freq_tbl = ftbl_mdss_esc0_1_clk,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "esc0_clk_src",
+		.parent_names = mmss_xo_dsibyte,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 esc1_clk_src = {
+	.cmd_rcgr = 0x2180,
+	.hid_width = 5,
+	.parent_map = mmss_xo_dsibyte_map,
+	.freq_tbl = ftbl_mdss_esc0_1_clk,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "esc1_clk_src",
+		.parent_names = mmss_xo_dsibyte,
+		.num_parents = 3,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_camss_gp0_clk_src[] = {
+	F(10000, P_XO, 16, 1, 120),
+	F(24000, P_XO, 16, 1, 50),
+	F(6000000, P_GPLL0_DIV, 10, 1, 5),
+	F(12000000, P_GPLL0_DIV, 1, 1, 25),
+	F(13000000, P_GPLL0_DIV, 2, 13, 150),
+	F(24000000, P_GPLL0_DIV, 1, 2, 25),
+	{ }
+};
+
+static struct clk_rcg2 camss_gp0_clk_src = {
+	.cmd_rcgr = 0x3420,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_camss_gp0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "camss_gp0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 camss_gp1_clk_src = {
+	.cmd_rcgr = 0x3450,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_camss_gp0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "camss_gp1_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_mclk0_clk_src[] = {
+	F(4800000, P_XO, 4, 0, 0),
+	F(6000000, P_GPLL0_DIV, 10, 1, 5),
+	F(8000000, P_GPLL0_DIV, 1, 2, 75),
+	F(9600000, P_XO, 2, 0, 0),
+	F(16666667, P_GPLL0_DIV, 2, 1, 9),
+	F(19200000, P_XO, 1, 0, 0),
+	F(24000000, P_GPLL0_DIV, 1, 2, 25),
+	F(33333333, P_GPLL0_DIV, 1, 1, 9),
+	F(48000000, P_GPLL0, 1, 2, 25),
+	F(66666667, P_GPLL0, 1, 1, 9),
+	{ }
+};
+
+static struct clk_rcg2 mclk0_clk_src = {
+	.cmd_rcgr = 0x3360,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mclk0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mclk0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 mclk1_clk_src = {
+	.cmd_rcgr = 0x3390,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mclk0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mclk1_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 mclk2_clk_src = {
+	.cmd_rcgr = 0x33c0,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mclk0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mclk2_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 mclk3_clk_src = {
+	.cmd_rcgr = 0x33f0,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_mclk0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "mclk3_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_cci_clk_src[] = {
+	F(19200000, P_XO, 1, 0, 0),
+	F(37500000, P_GPLL0, 16, 0, 0),
+	F(50000000, P_GPLL0, 12, 0, 0),
+	F(100000000, P_GPLL0, 6, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 cci_clk_src = {
+	.cmd_rcgr = 0x3300,
+	.mnd_width = 8,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_cci_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "cci_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_csi0phytimer_clk_src[] = {
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(266666667, P_MMPLL0, 3, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 csi0phytimer_clk_src = {
+	.cmd_rcgr = 0x3000,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0phytimer_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi0phytimer_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csi1phytimer_clk_src = {
+	.cmd_rcgr = 0x3030,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0phytimer_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi1phytimer_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csi2phytimer_clk_src = {
+	.cmd_rcgr = 0x3060,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0phytimer_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi2phytimer_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_csiphy0_3p_clk_src[] = {
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(320000000, P_MMPLL4, 3, 0, 0),
+	F(384000000, P_MMPLL4, 2.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 csiphy0_3p_clk_src = {
+	.cmd_rcgr = 0x3240,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csiphy0_3p_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csiphy0_3p_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csiphy1_3p_clk_src = {
+	.cmd_rcgr = 0x3260,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csiphy0_3p_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csiphy1_3p_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csiphy2_3p_clk_src = {
+	.cmd_rcgr = 0x3280,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csiphy0_3p_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csiphy2_3p_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_jpeg0_clk_src[] = {
+	F(75000000, P_GPLL0_DIV, 4, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	F(228571429, P_MMPLL0, 3.5, 0, 0),
+	F(266666667, P_MMPLL0, 3, 0, 0),
+	F(320000000, P_MMPLL0, 2.5, 0, 0),
+	F(480000000, P_MMPLL4, 2, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 jpeg0_clk_src = {
+	.cmd_rcgr = 0x3500,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_jpeg0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "jpeg0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_jpeg2_clk_src[] = {
+	F(75000000, P_GPLL0_DIV, 4, 0, 0),
+	F(150000000, P_GPLL0, 4, 0, 0),
+	F(228571429, P_MMPLL0, 3.5, 0, 0),
+	F(266666667, P_MMPLL0, 3, 0, 0),
+	F(320000000, P_MMPLL0, 2.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 jpeg2_clk_src = {
+	.cmd_rcgr = 0x3540,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_jpeg2_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "jpeg2_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 jpeg_dma_clk_src = {
+	.cmd_rcgr = 0x3560,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_jpeg0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "jpeg_dma_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_vfe0_clk_src[] = {
+	F(75000000, P_GPLL0_DIV, 4, 0, 0),
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(300000000, P_GPLL0, 2, 0, 0),
+	F(320000000, P_MMPLL0, 2.5, 0, 0),
+	F(480000000, P_MMPLL4, 2, 0, 0),
+	F(600000000, P_GPLL0, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 vfe0_clk_src = {
+	.cmd_rcgr = 0x3600,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_vfe0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "vfe0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 vfe1_clk_src = {
+	.cmd_rcgr = 0x3620,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_vfe0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "vfe1_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_cpp_clk_src[] = {
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(320000000, P_MMPLL0, 2.5, 0, 0),
+	F(480000000, P_MMPLL4, 2, 0, 0),
+	F(640000000, P_MMPLL4, 1.5, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 cpp_clk_src = {
+	.cmd_rcgr = 0x3640,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_cpp_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "cpp_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_csi0_clk_src[] = {
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(266666667, P_MMPLL0, 3, 0, 0),
+	F(480000000, P_MMPLL4, 2, 0, 0),
+	F(600000000, P_GPLL0, 1, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 csi0_clk_src = {
+	.cmd_rcgr = 0x3090,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi0_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csi1_clk_src = {
+	.cmd_rcgr = 0x3100,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi1_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csi2_clk_src = {
+	.cmd_rcgr = 0x3160,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi2_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_rcg2 csi3_clk_src = {
+	.cmd_rcgr = 0x31c0,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_csi0_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "csi3_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll1_mmpll4_mmpll3_gpll0_gpll0_div,
+		.num_parents = 7,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static const struct freq_tbl ftbl_fd_core_clk_src[] = {
+	F(100000000, P_GPLL0_DIV, 3, 0, 0),
+	F(200000000, P_GPLL0, 3, 0, 0),
+	F(400000000, P_MMPLL0, 2, 0, 0),
+	{ }
+};
+
+static struct clk_rcg2 fd_core_clk_src = {
+	.cmd_rcgr = 0x3b00,
+	.hid_width = 5,
+	.parent_map = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div_map,
+	.freq_tbl = ftbl_fd_core_clk_src,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "fd_core_clk_src",
+		.parent_names = mmss_xo_mmpll0_mmpll4_gpll0_gpll0_div,
+		.num_parents = 5,
+		.ops = &clk_rcg2_ops,
+	},
+};
+
+static struct clk_branch mmss_mmagic_ahb_clk = {
+	.halt_reg = 0x5024,
+	.clkr = {
+		.enable_reg = 0x5024,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_mmagic_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_mmagic_cfg_ahb_clk = {
+	.halt_reg = 0x5054,
+	.clkr = {
+		.enable_reg = 0x5054,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_mmagic_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_misc_ahb_clk = {
+	.halt_reg = 0x5018,
+	.clkr = {
+		.enable_reg = 0x5018,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_misc_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_misc_cxo_clk = {
+	.halt_reg = 0x5014,
+	.clkr = {
+		.enable_reg = 0x5014,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_misc_cxo_clk",
+			.parent_names = (const char *[]){ "xo" },
+			.num_parents = 1,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_mmagic_axi_clk = {
+	.halt_reg = 0x506c,
+	.clkr = {
+		.enable_reg = 0x506c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_mmagic_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_mmagic_maxi_clk = {
+	.halt_reg = 0x5074,
+	.clkr = {
+		.enable_reg = 0x5074,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_mmagic_maxi_clk",
+			.parent_names = (const char *[]){ "maxi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_camss_axi_clk = {
+	.halt_reg = 0x3c44,
+	.clkr = {
+		.enable_reg = 0x3c44,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_camss_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_camss_noc_cfg_ahb_clk = {
+	.halt_reg = 0x3c48,
+	.clkr = {
+		.enable_reg = 0x3c48,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_camss_noc_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "gcc_mmss_noc_cfg_ahb_clk" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_vfe_ahb_clk = {
+	.halt_reg = 0x3c04,
+	.clkr = {
+		.enable_reg = 0x3c04,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_vfe_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_vfe_axi_clk = {
+	.halt_reg = 0x3c08,
+	.clkr = {
+		.enable_reg = 0x3c08,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_vfe_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_cpp_ahb_clk = {
+	.halt_reg = 0x3c14,
+	.clkr = {
+		.enable_reg = 0x3c14,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_cpp_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_cpp_axi_clk = {
+	.halt_reg = 0x3c18,
+	.clkr = {
+		.enable_reg = 0x3c18,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_cpp_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_jpeg_ahb_clk = {
+	.halt_reg = 0x3c24,
+	.clkr = {
+		.enable_reg = 0x3c24,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_jpeg_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_jpeg_axi_clk = {
+	.halt_reg = 0x3c28,
+	.clkr = {
+		.enable_reg = 0x3c28,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_jpeg_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_mdss_axi_clk = {
+	.halt_reg = 0x2474,
+	.clkr = {
+		.enable_reg = 0x2474,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_mdss_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_mdss_noc_cfg_ahb_clk = {
+	.halt_reg = 0x2478,
+	.clkr = {
+		.enable_reg = 0x2478,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_mdss_noc_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "gcc_mmss_noc_cfg_ahb_clk" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_rot_ahb_clk = {
+	.halt_reg = 0x2444,
+	.clkr = {
+		.enable_reg = 0x2444,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_rot_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_rot_axi_clk = {
+	.halt_reg = 0x2448,
+	.clkr = {
+		.enable_reg = 0x2448,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_rot_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_mdp_ahb_clk = {
+	.halt_reg = 0x2454,
+	.clkr = {
+		.enable_reg = 0x2454,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_mdp_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_mdp_axi_clk = {
+	.halt_reg = 0x2458,
+	.clkr = {
+		.enable_reg = 0x2458,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_mdp_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_video_axi_clk = {
+	.halt_reg = 0x1194,
+	.clkr = {
+		.enable_reg = 0x1194,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_video_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_video_noc_cfg_ahb_clk = {
+	.halt_reg = 0x1198,
+	.clkr = {
+		.enable_reg = 0x1198,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_video_noc_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "gcc_mmss_noc_cfg_ahb_clk" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_video_ahb_clk = {
+	.halt_reg = 0x1174,
+	.clkr = {
+		.enable_reg = 0x1174,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_video_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch smmu_video_axi_clk = {
+	.halt_reg = 0x1178,
+	.clkr = {
+		.enable_reg = 0x1178,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "smmu_video_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_bimc_axi_clk = {
+	.halt_reg = 0x5294,
+	.clkr = {
+		.enable_reg = 0x5294,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_bimc_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmagic_bimc_noc_cfg_ahb_clk = {
+	.halt_reg = 0x5298,
+	.clkr = {
+		.enable_reg = 0x5298,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmagic_bimc_noc_cfg_ahb_clk",
+			.parent_names = (const char *[]){ "gcc_mmss_noc_cfg_ahb_clk" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gpu_gx_gfx3d_clk = {
+	.halt_reg = 0x4028,
+	.clkr = {
+		.enable_reg = 0x4028,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpu_gx_gfx3d_clk",
+			.parent_names = (const char *[]){ "gfx3d_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gpu_gx_rbbmtimer_clk = {
+	.halt_reg = 0x40b0,
+	.clkr = {
+		.enable_reg = 0x40b0,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpu_gx_rbbmtimer_clk",
+			.parent_names = (const char *[]){ "rbbmtimer_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gpu_ahb_clk = {
+	.halt_reg = 0x403c,
+	.clkr = {
+		.enable_reg = 0x403c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpu_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch gpu_aon_isense_clk = {
+	.halt_reg = 0x4044,
+	.clkr = {
+		.enable_reg = 0x4044,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "gpu_aon_isense_clk",
+			.parent_names = (const char *[]){ "isense_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch vmem_maxi_clk = {
+	.halt_reg = 0x1204,
+	.clkr = {
+		.enable_reg = 0x1204,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "vmem_maxi_clk",
+			.parent_names = (const char *[]){ "maxi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch vmem_ahb_clk = {
+	.halt_reg = 0x1208,
+	.clkr = {
+		.enable_reg = 0x1208,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "vmem_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_rbcpr_clk = {
+	.halt_reg = 0x4084,
+	.clkr = {
+		.enable_reg = 0x4084,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_rbcpr_clk",
+			.parent_names = (const char *[]){ "rbcpr_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mmss_rbcpr_ahb_clk = {
+	.halt_reg = 0x4088,
+	.clkr = {
+		.enable_reg = 0x4088,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mmss_rbcpr_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_core_clk = {
+	.halt_reg = 0x1028,
+	.clkr = {
+		.enable_reg = 0x1028,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_core_clk",
+			.parent_names = (const char *[]){ "video_core_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_axi_clk = {
+	.halt_reg = 0x1034,
+	.clkr = {
+		.enable_reg = 0x1034,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_maxi_clk = {
+	.halt_reg = 0x1038,
+	.clkr = {
+		.enable_reg = 0x1038,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_maxi_clk",
+			.parent_names = (const char *[]){ "maxi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_ahb_clk = {
+	.halt_reg = 0x1030,
+	.clkr = {
+		.enable_reg = 0x1030,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_subcore0_clk = {
+	.halt_reg = 0x1048,
+	.clkr = {
+		.enable_reg = 0x1048,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_subcore0_clk",
+			.parent_names = (const char *[]){ "video_subcore0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch video_subcore1_clk = {
+	.halt_reg = 0x104c,
+	.clkr = {
+		.enable_reg = 0x104c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "video_subcore1_clk",
+			.parent_names = (const char *[]){ "video_subcore1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_ahb_clk = {
+	.halt_reg = 0x2308,
+	.clkr = {
+		.enable_reg = 0x2308,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_hdmi_ahb_clk = {
+	.halt_reg = 0x230c,
+	.clkr = {
+		.enable_reg = 0x230c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_hdmi_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_axi_clk = {
+	.halt_reg = 0x2310,
+	.clkr = {
+		.enable_reg = 0x2310,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_pclk0_clk = {
+	.halt_reg = 0x2314,
+	.clkr = {
+		.enable_reg = 0x2314,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_pclk0_clk",
+			.parent_names = (const char *[]){ "pclk0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_pclk1_clk = {
+	.halt_reg = 0x2318,
+	.clkr = {
+		.enable_reg = 0x2318,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_pclk1_clk",
+			.parent_names = (const char *[]){ "pclk1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_mdp_clk = {
+	.halt_reg = 0x231c,
+	.clkr = {
+		.enable_reg = 0x231c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_mdp_clk",
+			.parent_names = (const char *[]){ "mdp_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_extpclk_clk = {
+	.halt_reg = 0x2324,
+	.clkr = {
+		.enable_reg = 0x2324,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_extpclk_clk",
+			.parent_names = (const char *[]){ "extpclk_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_vsync_clk = {
+	.halt_reg = 0x2328,
+	.clkr = {
+		.enable_reg = 0x2328,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_vsync_clk",
+			.parent_names = (const char *[]){ "vsync_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_hdmi_clk = {
+	.halt_reg = 0x2338,
+	.clkr = {
+		.enable_reg = 0x2338,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_hdmi_clk",
+			.parent_names = (const char *[]){ "hdmi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_byte0_clk = {
+	.halt_reg = 0x233c,
+	.clkr = {
+		.enable_reg = 0x233c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_byte0_clk",
+			.parent_names = (const char *[]){ "byte0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_byte1_clk = {
+	.halt_reg = 0x2340,
+	.clkr = {
+		.enable_reg = 0x2340,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_byte1_clk",
+			.parent_names = (const char *[]){ "byte1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_esc0_clk = {
+	.halt_reg = 0x2344,
+	.clkr = {
+		.enable_reg = 0x2344,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_esc0_clk",
+			.parent_names = (const char *[]){ "esc0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch mdss_esc1_clk = {
+	.halt_reg = 0x2348,
+	.clkr = {
+		.enable_reg = 0x2348,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "mdss_esc1_clk",
+			.parent_names = (const char *[]){ "esc1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_top_ahb_clk = {
+	.halt_reg = 0x3484,
+	.clkr = {
+		.enable_reg = 0x3484,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_top_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_ahb_clk = {
+	.halt_reg = 0x348c,
+	.clkr = {
+		.enable_reg = 0x348c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_micro_ahb_clk = {
+	.halt_reg = 0x3494,
+	.clkr = {
+		.enable_reg = 0x3494,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_micro_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_gp0_clk = {
+	.halt_reg = 0x3444,
+	.clkr = {
+		.enable_reg = 0x3444,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_gp0_clk",
+			.parent_names = (const char *[]){ "camss_gp0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_gp1_clk = {
+	.halt_reg = 0x3474,
+	.clkr = {
+		.enable_reg = 0x3474,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_gp1_clk",
+			.parent_names = (const char *[]){ "camss_gp1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_mclk0_clk = {
+	.halt_reg = 0x3384,
+	.clkr = {
+		.enable_reg = 0x3384,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_mclk0_clk",
+			.parent_names = (const char *[]){ "mclk0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_mclk1_clk = {
+	.halt_reg = 0x33b4,
+	.clkr = {
+		.enable_reg = 0x33b4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_mclk1_clk",
+			.parent_names = (const char *[]){ "mclk1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_mclk2_clk = {
+	.halt_reg = 0x33e4,
+	.clkr = {
+		.enable_reg = 0x33e4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_mclk2_clk",
+			.parent_names = (const char *[]){ "mclk2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_mclk3_clk = {
+	.halt_reg = 0x3414,
+	.clkr = {
+		.enable_reg = 0x3414,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_mclk3_clk",
+			.parent_names = (const char *[]){ "mclk3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cci_clk = {
+	.halt_reg = 0x3344,
+	.clkr = {
+		.enable_reg = 0x3344,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cci_clk",
+			.parent_names = (const char *[]){ "cci_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cci_ahb_clk = {
+	.halt_reg = 0x3348,
+	.clkr = {
+		.enable_reg = 0x3348,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cci_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0phytimer_clk = {
+	.halt_reg = 0x3024,
+	.clkr = {
+		.enable_reg = 0x3024,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0phytimer_clk",
+			.parent_names = (const char *[]){ "csi0phytimer_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1phytimer_clk = {
+	.halt_reg = 0x3054,
+	.clkr = {
+		.enable_reg = 0x3054,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1phytimer_clk",
+			.parent_names = (const char *[]){ "csi1phytimer_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2phytimer_clk = {
+	.halt_reg = 0x3084,
+	.clkr = {
+		.enable_reg = 0x3084,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2phytimer_clk",
+			.parent_names = (const char *[]){ "csi2phytimer_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csiphy0_3p_clk = {
+	.halt_reg = 0x3234,
+	.clkr = {
+		.enable_reg = 0x3234,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csiphy0_3p_clk",
+			.parent_names = (const char *[]){ "csiphy0_3p_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csiphy1_3p_clk = {
+	.halt_reg = 0x3254,
+	.clkr = {
+		.enable_reg = 0x3254,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csiphy1_3p_clk",
+			.parent_names = (const char *[]){ "csiphy1_3p_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csiphy2_3p_clk = {
+	.halt_reg = 0x3274,
+	.clkr = {
+		.enable_reg = 0x3274,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csiphy2_3p_clk",
+			.parent_names = (const char *[]){ "csiphy2_3p_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_jpeg0_clk = {
+	.halt_reg = 0x35a8,
+	.clkr = {
+		.enable_reg = 0x35a8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_jpeg0_clk",
+			.parent_names = (const char *[]){ "jpeg0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_jpeg2_clk = {
+	.halt_reg = 0x35b0,
+	.clkr = {
+		.enable_reg = 0x35b0,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_jpeg2_clk",
+			.parent_names = (const char *[]){ "jpeg2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_jpeg_dma_clk = {
+	.halt_reg = 0x35c0,
+	.clkr = {
+		.enable_reg = 0x35c0,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_jpeg_dma_clk",
+			.parent_names = (const char *[]){ "jpeg_dma_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_jpeg_ahb_clk = {
+	.halt_reg = 0x35b4,
+	.clkr = {
+		.enable_reg = 0x35b4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_jpeg_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_jpeg_axi_clk = {
+	.halt_reg = 0x35b8,
+	.clkr = {
+		.enable_reg = 0x35b8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_jpeg_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe_ahb_clk = {
+	.halt_reg = 0x36b8,
+	.clkr = {
+		.enable_reg = 0x36b8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe_axi_clk = {
+	.halt_reg = 0x36bc,
+	.clkr = {
+		.enable_reg = 0x36bc,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe0_clk = {
+	.halt_reg = 0x36a8,
+	.clkr = {
+		.enable_reg = 0x36a8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe0_clk",
+			.parent_names = (const char *[]){ "vfe0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe0_stream_clk = {
+	.halt_reg = 0x3720,
+	.clkr = {
+		.enable_reg = 0x3720,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe0_stream_clk",
+			.parent_names = (const char *[]){ "vfe0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe0_ahb_clk = {
+	.halt_reg = 0x3668,
+	.clkr = {
+		.enable_reg = 0x3668,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe0_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe1_clk = {
+	.halt_reg = 0x36ac,
+	.clkr = {
+		.enable_reg = 0x36ac,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe1_clk",
+			.parent_names = (const char *[]){ "vfe1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe1_stream_clk = {
+	.halt_reg = 0x3724,
+	.clkr = {
+		.enable_reg = 0x3724,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe1_stream_clk",
+			.parent_names = (const char *[]){ "vfe1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_vfe1_ahb_clk = {
+	.halt_reg = 0x3678,
+	.clkr = {
+		.enable_reg = 0x3678,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_vfe1_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi_vfe0_clk = {
+	.halt_reg = 0x3704,
+	.clkr = {
+		.enable_reg = 0x3704,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi_vfe0_clk",
+			.parent_names = (const char *[]){ "vfe0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi_vfe1_clk = {
+	.halt_reg = 0x3714,
+	.clkr = {
+		.enable_reg = 0x3714,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi_vfe1_clk",
+			.parent_names = (const char *[]){ "vfe1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cpp_vbif_ahb_clk = {
+	.halt_reg = 0x36c8,
+	.clkr = {
+		.enable_reg = 0x36c8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cpp_vbif_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cpp_axi_clk = {
+	.halt_reg = 0x36c4,
+	.clkr = {
+		.enable_reg = 0x36c4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cpp_axi_clk",
+			.parent_names = (const char *[]){ "axi_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cpp_clk = {
+	.halt_reg = 0x36b0,
+	.clkr = {
+		.enable_reg = 0x36b0,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cpp_clk",
+			.parent_names = (const char *[]){ "cpp_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_cpp_ahb_clk = {
+	.halt_reg = 0x36b4,
+	.clkr = {
+		.enable_reg = 0x36b4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_cpp_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0_clk = {
+	.halt_reg = 0x30b4,
+	.clkr = {
+		.enable_reg = 0x30b4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0_clk",
+			.parent_names = (const char *[]){ "csi0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0_ahb_clk = {
+	.halt_reg = 0x30bc,
+	.clkr = {
+		.enable_reg = 0x30bc,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0phy_clk = {
+	.halt_reg = 0x30c4,
+	.clkr = {
+		.enable_reg = 0x30c4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0phy_clk",
+			.parent_names = (const char *[]){ "csi0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0rdi_clk = {
+	.halt_reg = 0x30d4,
+	.clkr = {
+		.enable_reg = 0x30d4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0rdi_clk",
+			.parent_names = (const char *[]){ "csi0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi0pix_clk = {
+	.halt_reg = 0x30e4,
+	.clkr = {
+		.enable_reg = 0x30e4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi0pix_clk",
+			.parent_names = (const char *[]){ "csi0_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1_clk = {
+	.halt_reg = 0x3124,
+	.clkr = {
+		.enable_reg = 0x3124,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1_clk",
+			.parent_names = (const char *[]){ "csi1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1_ahb_clk = {
+	.halt_reg = 0x3128,
+	.clkr = {
+		.enable_reg = 0x3128,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1phy_clk = {
+	.halt_reg = 0x3134,
+	.clkr = {
+		.enable_reg = 0x3134,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1phy_clk",
+			.parent_names = (const char *[]){ "csi1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1rdi_clk = {
+	.halt_reg = 0x3144,
+	.clkr = {
+		.enable_reg = 0x3144,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1rdi_clk",
+			.parent_names = (const char *[]){ "csi1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi1pix_clk = {
+	.halt_reg = 0x3154,
+	.clkr = {
+		.enable_reg = 0x3154,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi1pix_clk",
+			.parent_names = (const char *[]){ "csi1_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2_clk = {
+	.halt_reg = 0x3184,
+	.clkr = {
+		.enable_reg = 0x3184,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2_clk",
+			.parent_names = (const char *[]){ "csi2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2_ahb_clk = {
+	.halt_reg = 0x3188,
+	.clkr = {
+		.enable_reg = 0x3188,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2phy_clk = {
+	.halt_reg = 0x3194,
+	.clkr = {
+		.enable_reg = 0x3194,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2phy_clk",
+			.parent_names = (const char *[]){ "csi2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2rdi_clk = {
+	.halt_reg = 0x31a4,
+	.clkr = {
+		.enable_reg = 0x31a4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2rdi_clk",
+			.parent_names = (const char *[]){ "csi2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi2pix_clk = {
+	.halt_reg = 0x31b4,
+	.clkr = {
+		.enable_reg = 0x31b4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi2pix_clk",
+			.parent_names = (const char *[]){ "csi2_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi3_clk = {
+	.halt_reg = 0x31e4,
+	.clkr = {
+		.enable_reg = 0x31e4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi3_clk",
+			.parent_names = (const char *[]){ "csi3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi3_ahb_clk = {
+	.halt_reg = 0x31e8,
+	.clkr = {
+		.enable_reg = 0x31e8,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi3_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi3phy_clk = {
+	.halt_reg = 0x31f4,
+	.clkr = {
+		.enable_reg = 0x31f4,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi3phy_clk",
+			.parent_names = (const char *[]){ "csi3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi3rdi_clk = {
+	.halt_reg = 0x3204,
+	.clkr = {
+		.enable_reg = 0x3204,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi3rdi_clk",
+			.parent_names = (const char *[]){ "csi3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_csi3pix_clk = {
+	.halt_reg = 0x3214,
+	.clkr = {
+		.enable_reg = 0x3214,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_csi3pix_clk",
+			.parent_names = (const char *[]){ "csi3_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch camss_ispif_ahb_clk = {
+	.halt_reg = 0x3224,
+	.clkr = {
+		.enable_reg = 0x3224,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "camss_ispif_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch fd_core_clk = {
+	.halt_reg = 0x3b68,
+	.clkr = {
+		.enable_reg = 0x3b68,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "fd_core_clk",
+			.parent_names = (const char *[]){ "fd_core_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch fd_core_uar_clk = {
+	.halt_reg = 0x3b6c,
+	.clkr = {
+		.enable_reg = 0x3b6c,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "fd_core_uar_clk",
+			.parent_names = (const char *[]){ "fd_core_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_branch fd_ahb_clk = {
+	.halt_reg = 0x3ba74,
+	.clkr = {
+		.enable_reg = 0x3ba74,
+		.enable_mask = BIT(0),
+		.hw.init = &(struct clk_init_data){
+			.name = "fd_ahb_clk",
+			.parent_names = (const char *[]){ "ahb_clk_src" },
+			.num_parents = 1,
+			.flags = CLK_SET_RATE_PARENT,
+			.ops = &clk_branch2_ops,
+		},
+	},
+};
+
+static struct clk_hw *mmcc_msm8996_hws[] = {
+	&gpll0_div.hw,
+};
+
+static struct clk_regmap *mmcc_msm8996_clocks[] = {
+	[MMPLL0_EARLY] = &mmpll0_early.clkr,
+	[MMPLL0_PLL] = &mmpll0.clkr,
+	[MMPLL1_EARLY] = &mmpll1_early.clkr,
+	[MMPLL1_PLL] = &mmpll1.clkr,
+	[MMPLL2_EARLY] = &mmpll2_early.clkr,
+	[MMPLL2_PLL] = &mmpll2.clkr,
+	[MMPLL3_EARLY] = &mmpll3_early.clkr,
+	[MMPLL3_PLL] = &mmpll3.clkr,
+	[MMPLL4_EARLY] = &mmpll4_early.clkr,
+	[MMPLL4_PLL] = &mmpll4.clkr,
+	[MMPLL5_EARLY] = &mmpll5_early.clkr,
+	[MMPLL5_PLL] = &mmpll5.clkr,
+	[MMPLL8_EARLY] = &mmpll8_early.clkr,
+	[MMPLL8_PLL] = &mmpll8.clkr,
+	[MMPLL9_EARLY] = &mmpll9_early.clkr,
+	[MMPLL9_PLL] = &mmpll9.clkr,
+	[AHB_CLK_SRC] = &ahb_clk_src.clkr,
+	[AXI_CLK_SRC] = &axi_clk_src.clkr,
+	[MAXI_CLK_SRC] = &maxi_clk_src.clkr,
+	[GFX3D_CLK_SRC] = &gfx3d_clk_src.clkr,
+	[RBBMTIMER_CLK_SRC] = &rbbmtimer_clk_src.clkr,
+	[ISENSE_CLK_SRC] = &isense_clk_src.clkr,
+	[RBCPR_CLK_SRC] = &rbcpr_clk_src.clkr,
+	[VIDEO_CORE_CLK_SRC] = &video_core_clk_src.clkr,
+	[VIDEO_SUBCORE0_CLK_SRC] = &video_subcore0_clk_src.clkr,
+	[VIDEO_SUBCORE1_CLK_SRC] = &video_subcore1_clk_src.clkr,
+	[PCLK0_CLK_SRC] = &pclk0_clk_src.clkr,
+	[PCLK1_CLK_SRC] = &pclk1_clk_src.clkr,
+	[MDP_CLK_SRC] = &mdp_clk_src.clkr,
+	[EXTPCLK_CLK_SRC] = &extpclk_clk_src.clkr,
+	[VSYNC_CLK_SRC] = &vsync_clk_src.clkr,
+	[HDMI_CLK_SRC] = &hdmi_clk_src.clkr,
+	[BYTE0_CLK_SRC] = &byte0_clk_src.clkr,
+	[BYTE1_CLK_SRC] = &byte1_clk_src.clkr,
+	[ESC0_CLK_SRC] = &esc0_clk_src.clkr,
+	[ESC1_CLK_SRC] = &esc1_clk_src.clkr,
+	[CAMSS_GP0_CLK_SRC] = &camss_gp0_clk_src.clkr,
+	[CAMSS_GP1_CLK_SRC] = &camss_gp1_clk_src.clkr,
+	[MCLK0_CLK_SRC] = &mclk0_clk_src.clkr,
+	[MCLK1_CLK_SRC] = &mclk1_clk_src.clkr,
+	[MCLK2_CLK_SRC] = &mclk2_clk_src.clkr,
+	[MCLK3_CLK_SRC] = &mclk3_clk_src.clkr,
+	[CCI_CLK_SRC] = &cci_clk_src.clkr,
+	[CSI0PHYTIMER_CLK_SRC] = &csi0phytimer_clk_src.clkr,
+	[CSI1PHYTIMER_CLK_SRC] = &csi1phytimer_clk_src.clkr,
+	[CSI2PHYTIMER_CLK_SRC] = &csi2phytimer_clk_src.clkr,
+	[CSIPHY0_3P_CLK_SRC] = &csiphy0_3p_clk_src.clkr,
+	[CSIPHY1_3P_CLK_SRC] = &csiphy1_3p_clk_src.clkr,
+	[CSIPHY2_3P_CLK_SRC] = &csiphy2_3p_clk_src.clkr,
+	[JPEG0_CLK_SRC] = &jpeg0_clk_src.clkr,
+	[JPEG2_CLK_SRC] = &jpeg2_clk_src.clkr,
+	[JPEG_DMA_CLK_SRC] = &jpeg_dma_clk_src.clkr,
+	[VFE0_CLK_SRC] = &vfe0_clk_src.clkr,
+	[VFE1_CLK_SRC] = &vfe1_clk_src.clkr,
+	[CPP_CLK_SRC] = &cpp_clk_src.clkr,
+	[CSI0_CLK_SRC] = &csi0_clk_src.clkr,
+	[CSI1_CLK_SRC] = &csi1_clk_src.clkr,
+	[CSI2_CLK_SRC] = &csi2_clk_src.clkr,
+	[CSI3_CLK_SRC] = &csi3_clk_src.clkr,
+	[FD_CORE_CLK_SRC] = &fd_core_clk_src.clkr,
+	[MMSS_MMAGIC_AHB_CLK] = &mmss_mmagic_ahb_clk.clkr,
+	[MMSS_MMAGIC_CFG_AHB_CLK] = &mmss_mmagic_cfg_ahb_clk.clkr,
+	[MMSS_MISC_AHB_CLK] = &mmss_misc_ahb_clk.clkr,
+	[MMSS_MISC_CXO_CLK] = &mmss_misc_cxo_clk.clkr,
+	[MMSS_MMAGIC_AXI_CLK] = &mmss_mmagic_axi_clk.clkr,
+	[MMSS_MMAGIC_MAXI_CLK] = &mmss_mmagic_maxi_clk.clkr,
+	[MMAGIC_CAMSS_AXI_CLK] = &mmagic_camss_axi_clk.clkr,
+	[MMAGIC_CAMSS_NOC_CFG_AHB_CLK] = &mmagic_camss_noc_cfg_ahb_clk.clkr,
+	[SMMU_VFE_AHB_CLK] = &smmu_vfe_ahb_clk.clkr,
+	[SMMU_VFE_AXI_CLK] = &smmu_vfe_axi_clk.clkr,
+	[SMMU_CPP_AHB_CLK] = &smmu_cpp_ahb_clk.clkr,
+	[SMMU_CPP_AXI_CLK] = &smmu_cpp_axi_clk.clkr,
+	[SMMU_JPEG_AHB_CLK] = &smmu_jpeg_ahb_clk.clkr,
+	[SMMU_JPEG_AXI_CLK] = &smmu_jpeg_axi_clk.clkr,
+	[MMAGIC_MDSS_AXI_CLK] = &mmagic_mdss_axi_clk.clkr,
+	[MMAGIC_MDSS_NOC_CFG_AHB_CLK] = &mmagic_mdss_noc_cfg_ahb_clk.clkr,
+	[SMMU_ROT_AHB_CLK] = &smmu_rot_ahb_clk.clkr,
+	[SMMU_ROT_AXI_CLK] = &smmu_rot_axi_clk.clkr,
+	[SMMU_MDP_AHB_CLK] = &smmu_mdp_ahb_clk.clkr,
+	[SMMU_MDP_AXI_CLK] = &smmu_mdp_axi_clk.clkr,
+	[MMAGIC_VIDEO_AXI_CLK] = &mmagic_video_axi_clk.clkr,
+	[MMAGIC_VIDEO_NOC_CFG_AHB_CLK] = &mmagic_video_noc_cfg_ahb_clk.clkr,
+	[SMMU_VIDEO_AHB_CLK] = &smmu_video_ahb_clk.clkr,
+	[SMMU_VIDEO_AXI_CLK] = &smmu_video_axi_clk.clkr,
+	[MMAGIC_BIMC_AXI_CLK] = &mmagic_bimc_axi_clk.clkr,
+	[MMAGIC_BIMC_NOC_CFG_AHB_CLK] = &mmagic_bimc_noc_cfg_ahb_clk.clkr,
+	[GPU_GX_GFX3D_CLK] = &gpu_gx_gfx3d_clk.clkr,
+	[GPU_GX_RBBMTIMER_CLK] = &gpu_gx_rbbmtimer_clk.clkr,
+	[GPU_AHB_CLK] = &gpu_ahb_clk.clkr,
+	[GPU_AON_ISENSE_CLK] = &gpu_aon_isense_clk.clkr,
+	[VMEM_MAXI_CLK] = &vmem_maxi_clk.clkr,
+	[VMEM_AHB_CLK] = &vmem_ahb_clk.clkr,
+	[MMSS_RBCPR_CLK] = &mmss_rbcpr_clk.clkr,
+	[MMSS_RBCPR_AHB_CLK] = &mmss_rbcpr_ahb_clk.clkr,
+	[VIDEO_CORE_CLK] = &video_core_clk.clkr,
+	[VIDEO_AXI_CLK] = &video_axi_clk.clkr,
+	[VIDEO_MAXI_CLK] = &video_maxi_clk.clkr,
+	[VIDEO_AHB_CLK] = &video_ahb_clk.clkr,
+	[VIDEO_SUBCORE0_CLK] = &video_subcore0_clk.clkr,
+	[VIDEO_SUBCORE1_CLK] = &video_subcore1_clk.clkr,
+	[MDSS_AHB_CLK] = &mdss_ahb_clk.clkr,
+	[MDSS_HDMI_AHB_CLK] = &mdss_hdmi_ahb_clk.clkr,
+	[MDSS_AXI_CLK] = &mdss_axi_clk.clkr,
+	[MDSS_PCLK0_CLK] = &mdss_pclk0_clk.clkr,
+	[MDSS_PCLK1_CLK] = &mdss_pclk1_clk.clkr,
+	[MDSS_MDP_CLK] = &mdss_mdp_clk.clkr,
+	[MDSS_EXTPCLK_CLK] = &mdss_extpclk_clk.clkr,
+	[MDSS_VSYNC_CLK] = &mdss_vsync_clk.clkr,
+	[MDSS_HDMI_CLK] = &mdss_hdmi_clk.clkr,
+	[MDSS_BYTE0_CLK] = &mdss_byte0_clk.clkr,
+	[MDSS_BYTE1_CLK] = &mdss_byte1_clk.clkr,
+	[MDSS_ESC0_CLK] = &mdss_esc0_clk.clkr,
+	[MDSS_ESC1_CLK] = &mdss_esc1_clk.clkr,
+	[CAMSS_TOP_AHB_CLK] = &camss_top_ahb_clk.clkr,
+	[CAMSS_AHB_CLK] = &camss_ahb_clk.clkr,
+	[CAMSS_MICRO_AHB_CLK] = &camss_micro_ahb_clk.clkr,
+	[CAMSS_GP0_CLK] = &camss_gp0_clk.clkr,
+	[CAMSS_GP1_CLK] = &camss_gp1_clk.clkr,
+	[CAMSS_MCLK0_CLK] = &camss_mclk0_clk.clkr,
+	[CAMSS_MCLK1_CLK] = &camss_mclk1_clk.clkr,
+	[CAMSS_MCLK2_CLK] = &camss_mclk2_clk.clkr,
+	[CAMSS_MCLK3_CLK] = &camss_mclk3_clk.clkr,
+	[CAMSS_CCI_CLK] = &camss_cci_clk.clkr,
+	[CAMSS_CCI_AHB_CLK] = &camss_cci_ahb_clk.clkr,
+	[CAMSS_CSI0PHYTIMER_CLK] = &camss_csi0phytimer_clk.clkr,
+	[CAMSS_CSI1PHYTIMER_CLK] = &camss_csi1phytimer_clk.clkr,
+	[CAMSS_CSI2PHYTIMER_CLK] = &camss_csi2phytimer_clk.clkr,
+	[CAMSS_CSIPHY0_3P_CLK] = &camss_csiphy0_3p_clk.clkr,
+	[CAMSS_CSIPHY1_3P_CLK] = &camss_csiphy1_3p_clk.clkr,
+	[CAMSS_CSIPHY2_3P_CLK] = &camss_csiphy2_3p_clk.clkr,
+	[CAMSS_JPEG0_CLK] = &camss_jpeg0_clk.clkr,
+	[CAMSS_JPEG2_CLK] = &camss_jpeg2_clk.clkr,
+	[CAMSS_JPEG_DMA_CLK] = &camss_jpeg_dma_clk.clkr,
+	[CAMSS_JPEG_AHB_CLK] = &camss_jpeg_ahb_clk.clkr,
+	[CAMSS_JPEG_AXI_CLK] = &camss_jpeg_axi_clk.clkr,
+	[CAMSS_VFE_AHB_CLK] = &camss_vfe_ahb_clk.clkr,
+	[CAMSS_VFE_AXI_CLK] = &camss_vfe_axi_clk.clkr,
+	[CAMSS_VFE0_CLK] = &camss_vfe0_clk.clkr,
+	[CAMSS_VFE0_STREAM_CLK] = &camss_vfe0_stream_clk.clkr,
+	[CAMSS_VFE0_AHB_CLK] = &camss_vfe0_ahb_clk.clkr,
+	[CAMSS_VFE1_CLK] = &camss_vfe1_clk.clkr,
+	[CAMSS_VFE1_STREAM_CLK] = &camss_vfe1_stream_clk.clkr,
+	[CAMSS_VFE1_AHB_CLK] = &camss_vfe1_ahb_clk.clkr,
+	[CAMSS_CSI_VFE0_CLK] = &camss_csi_vfe0_clk.clkr,
+	[CAMSS_CSI_VFE1_CLK] = &camss_csi_vfe1_clk.clkr,
+	[CAMSS_CPP_VBIF_AHB_CLK] = &camss_cpp_vbif_ahb_clk.clkr,
+	[CAMSS_CPP_AXI_CLK] = &camss_cpp_axi_clk.clkr,
+	[CAMSS_CPP_CLK] = &camss_cpp_clk.clkr,
+	[CAMSS_CPP_AHB_CLK] = &camss_cpp_ahb_clk.clkr,
+	[CAMSS_CSI0_CLK] = &camss_csi0_clk.clkr,
+	[CAMSS_CSI0_AHB_CLK] = &camss_csi0_ahb_clk.clkr,
+	[CAMSS_CSI0PHY_CLK] = &camss_csi0phy_clk.clkr,
+	[CAMSS_CSI0RDI_CLK] = &camss_csi0rdi_clk.clkr,
+	[CAMSS_CSI0PIX_CLK] = &camss_csi0pix_clk.clkr,
+	[CAMSS_CSI1_CLK] = &camss_csi1_clk.clkr,
+	[CAMSS_CSI1_AHB_CLK] = &camss_csi1_ahb_clk.clkr,
+	[CAMSS_CSI1PHY_CLK] = &camss_csi1phy_clk.clkr,
+	[CAMSS_CSI1RDI_CLK] = &camss_csi1rdi_clk.clkr,
+	[CAMSS_CSI1PIX_CLK] = &camss_csi1pix_clk.clkr,
+	[CAMSS_CSI2_CLK] = &camss_csi2_clk.clkr,
+	[CAMSS_CSI2_AHB_CLK] = &camss_csi2_ahb_clk.clkr,
+	[CAMSS_CSI2PHY_CLK] = &camss_csi2phy_clk.clkr,
+	[CAMSS_CSI2RDI_CLK] = &camss_csi2rdi_clk.clkr,
+	[CAMSS_CSI2PIX_CLK] = &camss_csi2pix_clk.clkr,
+	[CAMSS_CSI3_CLK] = &camss_csi3_clk.clkr,
+	[CAMSS_CSI3_AHB_CLK] = &camss_csi3_ahb_clk.clkr,
+	[CAMSS_CSI3PHY_CLK] = &camss_csi3phy_clk.clkr,
+	[CAMSS_CSI3RDI_CLK] = &camss_csi3rdi_clk.clkr,
+	[CAMSS_CSI3PIX_CLK] = &camss_csi3pix_clk.clkr,
+	[CAMSS_ISPIF_AHB_CLK] = &camss_ispif_ahb_clk.clkr,
+	[FD_CORE_CLK] = &fd_core_clk.clkr,
+	[FD_CORE_UAR_CLK] = &fd_core_uar_clk.clkr,
+	[FD_AHB_CLK] = &fd_ahb_clk.clkr,
+};
+
+static const struct qcom_reset_map mmcc_msm8996_resets[] = {
+	[MMAGICAHB_BCR] = { 0x5020 },
+	[MMAGIC_CFG_BCR] = { 0x5050 },
+	[MISC_BCR] = { 0x5010 },
+	[BTO_BCR] = { 0x5030 },
+	[MMAGICAXI_BCR] = { 0x5060 },
+	[MMAGICMAXI_BCR] = { 0x5070 },
+	[DSA_BCR] = { 0x50a0 },
+	[MMAGIC_CAMSS_BCR] = { 0x3c40 },
+	[THROTTLE_CAMSS_BCR] = { 0x3c30 },
+	[SMMU_VFE_BCR] = { 0x3c00 },
+	[SMMU_CPP_BCR] = { 0x3c10 },
+	[SMMU_JPEG_BCR] = { 0x3c20 },
+	[MMAGIC_MDSS_BCR] = { 0x2470 },
+	[THROTTLE_MDSS_BCR] = { 0x2460 },
+	[SMMU_ROT_BCR] = { 0x2440 },
+	[SMMU_MDP_BCR] = { 0x2450 },
+	[MMAGIC_VIDEO_BCR] = { 0x1190 },
+	[THROTTLE_VIDEO_BCR] = { 0x1180 },
+	[SMMU_VIDEO_BCR] = { 0x1170 },
+	[MMAGIC_BIMC_BCR] = { 0x5290 },
+	[GPU_GX_BCR] = { 0x4020 },
+	[GPU_BCR] = { 0x4030 },
+	[GPU_AON_BCR] = { 0x4040 },
+	[VMEM_BCR] = { 0x1200 },
+	[MMSS_RBCPR_BCR] = { 0x4080 },
+	[VIDEO_BCR] = { 0x1020 },
+	[MDSS_BCR] = { 0x2300 },
+	[CAMSS_TOP_BCR] = { 0x3480 },
+	[CAMSS_AHB_BCR] = { 0x3488 },
+	[CAMSS_MICRO_BCR] = { 0x3490 },
+	[CAMSS_CCI_BCR] = { 0x3340 },
+	[CAMSS_PHY0_BCR] = { 0x3020 },
+	[CAMSS_PHY1_BCR] = { 0x3050 },
+	[CAMSS_PHY2_BCR] = { 0x3080 },
+	[CAMSS_CSIPHY0_3P_BCR] = { 0x3230 },
+	[CAMSS_CSIPHY1_3P_BCR] = { 0x3250 },
+	[CAMSS_CSIPHY2_3P_BCR] = { 0x3270 },
+	[CAMSS_JPEG_BCR] = { 0x35a0 },
+	[CAMSS_VFE_BCR] = { 0x36a0 },
+	[CAMSS_VFE0_BCR] = { 0x3660 },
+	[CAMSS_VFE1_BCR] = { 0x3670 },
+	[CAMSS_CSI_VFE0_BCR] = { 0x3700 },
+	[CAMSS_CSI_VFE1_BCR] = { 0x3710 },
+	[CAMSS_CPP_TOP_BCR] = { 0x36c0 },
+	[CAMSS_CPP_BCR] = { 0x36d0 },
+	[CAMSS_CSI0_BCR] = { 0x30b0 },
+	[CAMSS_CSI0RDI_BCR] = { 0x30d0 },
+	[CAMSS_CSI0PIX_BCR] = { 0x30e0 },
+	[CAMSS_CSI1_BCR] = { 0x3120 },
+	[CAMSS_CSI1RDI_BCR] = { 0x3140 },
+	[CAMSS_CSI1PIX_BCR] = { 0x3150 },
+	[CAMSS_CSI2_BCR] = { 0x3180 },
+	[CAMSS_CSI2RDI_BCR] = { 0x31a0 },
+	[CAMSS_CSI2PIX_BCR] = { 0x31b0 },
+	[CAMSS_CSI3_BCR] = { 0x31e0 },
+	[CAMSS_CSI3RDI_BCR] = { 0x3200 },
+	[CAMSS_CSI3PIX_BCR] = { 0x3210 },
+	[CAMSS_ISPIF_BCR] = { 0x3220 },
+	[FD_BCR] = { 0x3b60 },
+	[MMSS_SPDM_RM_BCR] = { 0x300 },
+};
+
+static const struct regmap_config mmcc_msm8996_regmap_config = {
+	.reg_bits	= 32,
+	.reg_stride	= 4,
+	.val_bits	= 32,
+	.max_register	= 0xb008,
+	.fast_io	= true,
+};
+
+static const struct qcom_cc_desc mmcc_msm8996_desc = {
+	.config = &mmcc_msm8996_regmap_config,
+	.clks = mmcc_msm8996_clocks,
+	.num_clks = ARRAY_SIZE(mmcc_msm8996_clocks),
+	.resets = mmcc_msm8996_resets,
+	.num_resets = ARRAY_SIZE(mmcc_msm8996_resets),
+};
+
+static const struct of_device_id mmcc_msm8996_match_table[] = {
+	{ .compatible = "qcom,mmcc-msm8996" },
+	{ }
+};
+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;
+	struct regmap *regmap;
+
+	regmap = qcom_cc_map(pdev, &mmcc_msm8996_desc);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	/* Disable the AHB DCD */
+	regmap_update_bits(regmap, 0x50d8, BIT(31), 0);
+	/* Disable the NoC FSM for mmss_mmagic_cfg_ahb_clk */
+	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);
+	}
+
+	return qcom_cc_really_probe(pdev, &mmcc_msm8996_desc, regmap);
+}
+
+static struct platform_driver mmcc_msm8996_driver = {
+	.probe		= mmcc_msm8996_probe,
+	.driver		= {
+		.name	= "mmcc-msm8996",
+		.of_match_table = mmcc_msm8996_match_table,
+	},
+};
+module_platform_driver(mmcc_msm8996_driver);
+
+MODULE_DESCRIPTION("QCOM MMCC MSM8996 Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:mmcc-msm8996");
diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile
index b27edd6c8183..80b9a379beb4 100644
--- a/drivers/clk/rockchip/Makefile
+++ b/drivers/clk/rockchip/Makefile
@@ -10,6 +10,8 @@ obj-y	+= clk-inverter.o
 obj-y	+= clk-mmc-phase.o
 obj-$(CONFIG_RESET_CONTROLLER)	+= softrst.o
 
+obj-y	+= clk-rk3036.o
 obj-y	+= clk-rk3188.o
+obj-y	+= clk-rk3228.o
 obj-y	+= clk-rk3288.o
 obj-y	+= clk-rk3368.o
diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
index 330870a6d8bf..d07374f48caf 100644
--- a/drivers/clk/rockchip/clk-cpu.c
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -242,8 +242,8 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
 	struct clk *clk, *cclk;
 	int ret;
 
-	if (num_parents != 2) {
-		pr_err("%s: needs two parent clocks\n", __func__);
+	if (num_parents < 2) {
+		pr_err("%s: needs at least two parent clocks\n", __func__);
 		return ERR_PTR(-EINVAL);
 	}
 
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index 4881eb8a1576..b7e66c9dd9f2 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -2,6 +2,9 @@
  * Copyright (c) 2014 MundoReader S.L.
  * Author: Heiko Stuebner <heiko@sntech.de>
  *
+ * Copyright (c) 2015 Rockchip Electronics Co. Ltd.
+ * Author: Xing Zheng <zhengxing@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
@@ -19,6 +22,7 @@
 #include <linux/delay.h>
 #include <linux/clk-provider.h>
 #include <linux/regmap.h>
+#include <linux/clk.h>
 #include "clk.h"
 
 #define PLL_MODE_MASK		0x3
@@ -108,6 +112,252 @@ static int rockchip_pll_wait_lock(struct rockchip_clk_pll *pll)
 }
 
 /**
+ * PLL used in RK3036
+ */
+
+#define RK3036_PLLCON(i)			(i * 0x4)
+#define RK3036_PLLCON0_FBDIV_MASK		0xfff
+#define RK3036_PLLCON0_FBDIV_SHIFT		0
+#define RK3036_PLLCON0_POSTDIV1_MASK		0x7
+#define RK3036_PLLCON0_POSTDIV1_SHIFT		12
+#define RK3036_PLLCON1_REFDIV_MASK		0x3f
+#define RK3036_PLLCON1_REFDIV_SHIFT		0
+#define RK3036_PLLCON1_POSTDIV2_MASK		0x7
+#define RK3036_PLLCON1_POSTDIV2_SHIFT		6
+#define RK3036_PLLCON1_DSMPD_MASK		0x1
+#define RK3036_PLLCON1_DSMPD_SHIFT		12
+#define RK3036_PLLCON2_FRAC_MASK		0xffffff
+#define RK3036_PLLCON2_FRAC_SHIFT		0
+
+#define RK3036_PLLCON1_PWRDOWN			(1 << 13)
+
+static void rockchip_rk3036_pll_get_params(struct rockchip_clk_pll *pll,
+					struct rockchip_pll_rate_table *rate)
+{
+	u32 pllcon;
+
+	pllcon = readl_relaxed(pll->reg_base + RK3036_PLLCON(0));
+	rate->fbdiv = ((pllcon >> RK3036_PLLCON0_FBDIV_SHIFT)
+				& RK3036_PLLCON0_FBDIV_MASK);
+	rate->postdiv1 = ((pllcon >> RK3036_PLLCON0_POSTDIV1_SHIFT)
+				& RK3036_PLLCON0_POSTDIV1_MASK);
+
+	pllcon = readl_relaxed(pll->reg_base + RK3036_PLLCON(1));
+	rate->refdiv = ((pllcon >> RK3036_PLLCON1_REFDIV_SHIFT)
+				& RK3036_PLLCON1_REFDIV_MASK);
+	rate->postdiv2 = ((pllcon >> RK3036_PLLCON1_POSTDIV2_SHIFT)
+				& RK3036_PLLCON1_POSTDIV2_MASK);
+	rate->dsmpd = ((pllcon >> RK3036_PLLCON1_DSMPD_SHIFT)
+				& RK3036_PLLCON1_DSMPD_MASK);
+
+	pllcon = readl_relaxed(pll->reg_base + RK3036_PLLCON(2));
+	rate->frac = ((pllcon >> RK3036_PLLCON2_FRAC_SHIFT)
+				& RK3036_PLLCON2_FRAC_MASK);
+}
+
+static unsigned long rockchip_rk3036_pll_recalc_rate(struct clk_hw *hw,
+						     unsigned long prate)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	struct rockchip_pll_rate_table cur;
+	u64 rate64 = prate;
+
+	rockchip_rk3036_pll_get_params(pll, &cur);
+
+	rate64 *= cur.fbdiv;
+	do_div(rate64, cur.refdiv);
+
+	if (cur.dsmpd == 0) {
+		/* fractional mode */
+		u64 frac_rate64 = prate * cur.frac;
+
+		do_div(frac_rate64, cur.refdiv);
+		rate64 += frac_rate64 >> 24;
+	}
+
+	do_div(rate64, cur.postdiv1);
+	do_div(rate64, cur.postdiv2);
+
+	return (unsigned long)rate64;
+}
+
+static int rockchip_rk3036_pll_set_params(struct rockchip_clk_pll *pll,
+				const struct rockchip_pll_rate_table *rate)
+{
+	const struct clk_ops *pll_mux_ops = pll->pll_mux_ops;
+	struct clk_mux *pll_mux = &pll->pll_mux;
+	struct rockchip_pll_rate_table cur;
+	u32 pllcon;
+	int rate_change_remuxed = 0;
+	int cur_parent;
+	int ret;
+
+	pr_debug("%s: rate settings for %lu fbdiv: %d, postdiv1: %d, refdiv: %d, postdiv2: %d, dsmpd: %d, frac: %d\n",
+		__func__, rate->rate, rate->fbdiv, rate->postdiv1, rate->refdiv,
+		rate->postdiv2, rate->dsmpd, rate->frac);
+
+	rockchip_rk3036_pll_get_params(pll, &cur);
+	cur.rate = 0;
+
+	cur_parent = pll_mux_ops->get_parent(&pll_mux->hw);
+	if (cur_parent == PLL_MODE_NORM) {
+		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_SLOW);
+		rate_change_remuxed = 1;
+	}
+
+	/* update pll values */
+	writel_relaxed(HIWORD_UPDATE(rate->fbdiv, RK3036_PLLCON0_FBDIV_MASK,
+					  RK3036_PLLCON0_FBDIV_SHIFT) |
+		       HIWORD_UPDATE(rate->postdiv1, RK3036_PLLCON0_POSTDIV1_MASK,
+					     RK3036_PLLCON0_POSTDIV1_SHIFT),
+		       pll->reg_base + RK3036_PLLCON(0));
+
+	writel_relaxed(HIWORD_UPDATE(rate->refdiv, RK3036_PLLCON1_REFDIV_MASK,
+						   RK3036_PLLCON1_REFDIV_SHIFT) |
+		       HIWORD_UPDATE(rate->postdiv2, RK3036_PLLCON1_POSTDIV2_MASK,
+						     RK3036_PLLCON1_POSTDIV2_SHIFT) |
+		       HIWORD_UPDATE(rate->dsmpd, RK3036_PLLCON1_DSMPD_MASK,
+						  RK3036_PLLCON1_DSMPD_SHIFT),
+		       pll->reg_base + RK3036_PLLCON(1));
+
+	/* GPLL CON2 is not HIWORD_MASK */
+	pllcon = readl_relaxed(pll->reg_base + RK3036_PLLCON(2));
+	pllcon &= ~(RK3036_PLLCON2_FRAC_MASK << RK3036_PLLCON2_FRAC_SHIFT);
+	pllcon |= rate->frac << RK3036_PLLCON2_FRAC_SHIFT;
+	writel_relaxed(pllcon, pll->reg_base + RK3036_PLLCON(2));
+
+	/* wait for the pll to lock */
+	ret = rockchip_pll_wait_lock(pll);
+	if (ret) {
+		pr_warn("%s: pll update unsucessful, trying to restore old params\n",
+			__func__);
+		rockchip_rk3036_pll_set_params(pll, &cur);
+	}
+
+	if (rate_change_remuxed)
+		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_NORM);
+
+	return ret;
+}
+
+static int rockchip_rk3036_pll_set_rate(struct clk_hw *hw, unsigned long drate,
+					unsigned long prate)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	const struct rockchip_pll_rate_table *rate;
+	unsigned long old_rate = rockchip_rk3036_pll_recalc_rate(hw, prate);
+	struct regmap *grf = rockchip_clk_get_grf();
+
+	if (IS_ERR(grf)) {
+		pr_debug("%s: grf regmap not available, aborting rate change\n",
+			 __func__);
+		return PTR_ERR(grf);
+	}
+
+	pr_debug("%s: changing %s from %lu to %lu with a parent rate of %lu\n",
+		 __func__, __clk_get_name(hw->clk), old_rate, drate, prate);
+
+	/* Get required rate settings from table */
+	rate = rockchip_get_pll_settings(pll, drate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for pll clk %s\n", __func__,
+			drate, __clk_get_name(hw->clk));
+		return -EINVAL;
+	}
+
+	return rockchip_rk3036_pll_set_params(pll, rate);
+}
+
+static int rockchip_rk3036_pll_enable(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+
+	writel(HIWORD_UPDATE(0, RK3036_PLLCON1_PWRDOWN, 0),
+	       pll->reg_base + RK3036_PLLCON(1));
+
+	return 0;
+}
+
+static void rockchip_rk3036_pll_disable(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+
+	writel(HIWORD_UPDATE(RK3036_PLLCON1_PWRDOWN,
+			     RK3036_PLLCON1_PWRDOWN, 0),
+	       pll->reg_base + RK3036_PLLCON(1));
+}
+
+static int rockchip_rk3036_pll_is_enabled(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	u32 pllcon = readl(pll->reg_base + RK3036_PLLCON(1));
+
+	return !(pllcon & RK3036_PLLCON1_PWRDOWN);
+}
+
+static void rockchip_rk3036_pll_init(struct clk_hw *hw)
+{
+	struct rockchip_clk_pll *pll = to_rockchip_clk_pll(hw);
+	const struct rockchip_pll_rate_table *rate;
+	struct rockchip_pll_rate_table cur;
+	unsigned long drate;
+
+	if (!(pll->flags & ROCKCHIP_PLL_SYNC_RATE))
+		return;
+
+	drate = clk_hw_get_rate(hw);
+	rate = rockchip_get_pll_settings(pll, drate);
+
+	/* when no rate setting for the current rate, rely on clk_set_rate */
+	if (!rate)
+		return;
+
+	rockchip_rk3036_pll_get_params(pll, &cur);
+
+	pr_debug("%s: pll %s@%lu: Hz\n", __func__, __clk_get_name(hw->clk),
+		 drate);
+	pr_debug("old - fbdiv: %d, postdiv1: %d, refdiv: %d, postdiv2: %d, dsmpd: %d, frac: %d\n",
+		 cur.fbdiv, cur.postdiv1, cur.refdiv, cur.postdiv2,
+		 cur.dsmpd, cur.frac);
+	pr_debug("new - fbdiv: %d, postdiv1: %d, refdiv: %d, postdiv2: %d, dsmpd: %d, frac: %d\n",
+		 rate->fbdiv, rate->postdiv1, rate->refdiv, rate->postdiv2,
+		 rate->dsmpd, rate->frac);
+
+	if (rate->fbdiv != cur.fbdiv || rate->postdiv1 != cur.postdiv1 ||
+		rate->refdiv != cur.refdiv || rate->postdiv2 != cur.postdiv2 ||
+		rate->dsmpd != cur.dsmpd || rate->frac != cur.frac) {
+		struct clk *parent = clk_get_parent(hw->clk);
+
+		if (!parent) {
+			pr_warn("%s: parent of %s not available\n",
+				__func__, __clk_get_name(hw->clk));
+			return;
+		}
+
+		pr_debug("%s: pll %s: rate params do not match rate table, adjusting\n",
+			 __func__, __clk_get_name(hw->clk));
+		rockchip_rk3036_pll_set_params(pll, rate);
+	}
+}
+
+static const struct clk_ops rockchip_rk3036_pll_clk_norate_ops = {
+	.recalc_rate = rockchip_rk3036_pll_recalc_rate,
+	.enable = rockchip_rk3036_pll_enable,
+	.disable = rockchip_rk3036_pll_disable,
+	.is_enabled = rockchip_rk3036_pll_is_enabled,
+};
+
+static const struct clk_ops rockchip_rk3036_pll_clk_ops = {
+	.recalc_rate = rockchip_rk3036_pll_recalc_rate,
+	.round_rate = rockchip_pll_round_rate,
+	.set_rate = rockchip_rk3036_pll_set_rate,
+	.enable = rockchip_rk3036_pll_enable,
+	.disable = rockchip_rk3036_pll_disable,
+	.is_enabled = rockchip_rk3036_pll_is_enabled,
+	.init = rockchip_rk3036_pll_init,
+};
+
+/**
  * PLL used in RK3066, RK3188 and RK3288
  */
 
@@ -376,7 +626,7 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	pll_mux->lock = lock;
 	pll_mux->hw.init = &init;
 
-	if (pll_type == pll_rk3066)
+	if (pll_type == pll_rk3036 || pll_type == pll_rk3066)
 		pll_mux->flags |= CLK_MUX_HIWORD_MASK;
 
 	/* the actual muxing is xin24m, pll-output, xin32k */
@@ -421,6 +671,12 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	}
 
 	switch (pll_type) {
+	case pll_rk3036:
+		if (!pll->rate_table)
+			init.ops = &rockchip_rk3036_pll_clk_norate_ops;
+		else
+			init.ops = &rockchip_rk3036_pll_clk_ops;
+		break;
 	case pll_rk3066:
 		if (!pll->rate_table)
 			init.ops = &rockchip_rk3066_pll_clk_norate_ops;
diff --git a/drivers/clk/rockchip/clk-rk3036.c b/drivers/clk/rockchip/clk-rk3036.c
new file mode 100644
index 000000000000..bc7fbac83ab7
--- /dev/null
+++ b/drivers/clk/rockchip/clk-rk3036.c
@@ -0,0 +1,493 @@
+/*
+ * Copyright (c) 2014 MundoReader S.L.
+ * Author: Heiko Stuebner <heiko@sntech.de>
+ *
+ * Copyright (c) 2015 Rockchip Electronics Co. Ltd.
+ * Author: Xing Zheng <zhengxing@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/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+#include <dt-bindings/clock/rk3036-cru.h>
+#include "clk.h"
+
+#define RK3036_GRF_SOC_STATUS0	0x14c
+
+enum rk3036_plls {
+	apll, dpll, gpll,
+};
+
+static struct rockchip_pll_rate_table rk3036_pll_rates[] = {
+	/* _mhz, _refdiv, _fbdiv, _postdiv1, _postdiv2, _dsmpd, _frac */
+	RK3036_PLL_RATE(1608000000, 1, 67, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1584000000, 1, 66, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1560000000, 1, 65, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1536000000, 1, 64, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1512000000, 1, 63, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1488000000, 1, 62, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1464000000, 1, 61, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1440000000, 1, 60, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1416000000, 1, 59, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1392000000, 1, 58, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1368000000, 1, 57, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1344000000, 1, 56, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1320000000, 1, 55, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1296000000, 1, 54, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1272000000, 1, 53, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1248000000, 1, 52, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1200000000, 1, 50, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1188000000, 2, 99, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1104000000, 1, 46, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1100000000, 12, 550, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1008000000, 1, 84, 2, 1, 1, 0),
+	RK3036_PLL_RATE(1000000000, 6, 500, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 984000000, 1, 82, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 960000000, 1, 80, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 936000000, 1, 78, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 912000000, 1, 76, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 900000000, 4, 300, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 888000000, 1, 74, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 864000000, 1, 72, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 840000000, 1, 70, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 816000000, 1, 68, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 800000000, 6, 400, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 700000000, 6, 350, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 696000000, 1, 58, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 600000000, 1, 75, 3, 1, 1, 0),
+	RK3036_PLL_RATE( 594000000, 2, 99, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 504000000, 1, 63, 3, 1, 1, 0),
+	RK3036_PLL_RATE( 500000000, 6, 250, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 408000000, 1, 68, 2, 2, 1, 0),
+	RK3036_PLL_RATE( 312000000, 1, 52, 2, 2, 1, 0),
+	RK3036_PLL_RATE( 216000000, 1, 72, 4, 2, 1, 0),
+	RK3036_PLL_RATE(  96000000, 1, 64, 4, 4, 1, 0),
+	{ /* sentinel */ },
+};
+
+#define RK3036_DIV_CPU_MASK		0x1f
+#define RK3036_DIV_CPU_SHIFT		8
+
+#define RK3036_DIV_PERI_MASK		0xf
+#define RK3036_DIV_PERI_SHIFT		0
+#define RK3036_DIV_ACLK_MASK		0x7
+#define RK3036_DIV_ACLK_SHIFT		4
+#define RK3036_DIV_HCLK_MASK		0x3
+#define RK3036_DIV_HCLK_SHIFT		8
+#define RK3036_DIV_PCLK_MASK		0x7
+#define RK3036_DIV_PCLK_SHIFT		12
+
+#define RK3036_CLKSEL1(_core_periph_div)					\
+	{									\
+		.reg = RK2928_CLKSEL_CON(1),					\
+		.val = HIWORD_UPDATE(_core_periph_div, RK3036_DIV_PERI_MASK,	\
+				RK3036_DIV_PERI_SHIFT)				\
+	}
+
+#define RK3036_CPUCLK_RATE(_prate, _core_periph_div)			\
+	{								\
+		.prate = _prate,					\
+		.divs = {						\
+			RK3036_CLKSEL1(_core_periph_div),		\
+		},							\
+	}
+
+static struct rockchip_cpuclk_rate_table rk3036_cpuclk_rates[] __initdata = {
+	RK3036_CPUCLK_RATE(816000000, 4),
+	RK3036_CPUCLK_RATE(600000000, 4),
+	RK3036_CPUCLK_RATE(312000000, 4),
+};
+
+static const struct rockchip_cpuclk_reg_data rk3036_cpuclk_data = {
+	.core_reg = RK2928_CLKSEL_CON(0),
+	.div_core_shift = 0,
+	.div_core_mask = 0x1f,
+	.mux_core_shift = 7,
+};
+
+PNAME(mux_pll_p)		= { "xin24m", "xin24m" };
+
+PNAME(mux_armclk_p)		= { "apll", "gpll_armclk" };
+PNAME(mux_busclk_p)		= { "apll", "dpll_cpu", "gpll_cpu" };
+PNAME(mux_ddrphy_p)		= { "dpll_ddr", "gpll_ddr" };
+PNAME(mux_pll_src_3plls_p)	= { "apll", "dpll", "gpll" };
+PNAME(mux_timer_p)		= { "xin24m", "pclk_peri_src" };
+
+PNAME(mux_pll_src_apll_dpll_gpll_usb480m_p)	= { "apll", "dpll", "gpll" "usb480m" };
+
+PNAME(mux_mmc_src_p)	= { "apll", "dpll", "gpll", "xin24m" };
+PNAME(mux_i2s_pre_p)	= { "i2s_src", "i2s_frac", "ext_i2s", "xin12m" };
+PNAME(mux_i2s_clkout_p)	= { "i2s_pre", "xin12m" };
+PNAME(mux_spdif_p)	= { "spdif_src", "spdif_frac", "xin12m" };
+PNAME(mux_uart0_p)	= { "uart0_src", "uart0_frac", "xin24m" };
+PNAME(mux_uart1_p)	= { "uart1_src", "uart1_frac", "xin24m" };
+PNAME(mux_uart2_p)	= { "uart2_src", "uart2_frac", "xin24m" };
+PNAME(mux_mac_p)	= { "mac_pll_src", "rmii_clkin" };
+PNAME(mux_dclk_p)	= { "dclk_lcdc", "dclk_cru" };
+
+static struct rockchip_pll_clock rk3036_pll_clks[] __initdata = {
+	[apll] = PLL(pll_rk3036, PLL_APLL, "apll", mux_pll_p, 0, RK2928_PLL_CON(0),
+		     RK2928_MODE_CON, 0, 5, 0, rk3036_pll_rates),
+	[dpll] = PLL(pll_rk3036, PLL_DPLL, "dpll", mux_pll_p, 0, RK2928_PLL_CON(4),
+		     RK2928_MODE_CON, 4, 4, 0, NULL),
+	[gpll] = PLL(pll_rk3036, PLL_GPLL, "gpll", mux_pll_p, 0, RK2928_PLL_CON(12),
+		     RK2928_MODE_CON, 12, 6, ROCKCHIP_PLL_SYNC_RATE, rk3036_pll_rates),
+};
+
+#define MFLAGS CLK_MUX_HIWORD_MASK
+#define DFLAGS CLK_DIVIDER_HIWORD_MASK
+#define GFLAGS (CLK_GATE_HIWORD_MASK | CLK_GATE_SET_TO_DISABLE)
+
+static struct rockchip_clk_branch rk3036_uart0_fracmux __initdata =
+	MUX(SCLK_UART0, "sclk_uart0", mux_uart0_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(13), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3036_uart1_fracmux __initdata =
+	MUX(SCLK_UART1, "sclk_uart1", mux_uart1_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(14), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3036_uart2_fracmux __initdata =
+	MUX(SCLK_UART2, "sclk_uart2", mux_uart2_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(15), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3036_i2s_fracmux __initdata =
+	MUX(0, "i2s_pre", mux_i2s_pre_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3036_spdif_fracmux __initdata =
+	MUX(SCLK_SPDIF, "sclk_spdif", mux_spdif_p, 0,
+			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3036_clk_branches[] __initdata = {
+	/*
+	 * Clock-Architecture Diagram 1
+	 */
+
+	GATE(0, "gpll_armclk", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 6, GFLAGS),
+
+	/*
+	 * Clock-Architecture Diagram 2
+	 */
+
+	GATE(0, "dpll_ddr", "dpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 2, GFLAGS),
+	GATE(0, "gpll_ddr", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 8, GFLAGS),
+	COMPOSITE_NOGATE(0, "ddrphy2x", mux_ddrphy_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(26), 8, 1, MFLAGS, 0, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
+
+	COMPOSITE_NOMUX(0, "pclk_dbg", "armclk", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 0, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(0), 7, GFLAGS),
+	COMPOSITE_NOMUX(0, "aclk_core_pre", "armclk", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 4, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(0), 7, GFLAGS),
+
+	GATE(0, "dpll_cpu", "dpll", 0, RK2928_CLKGATE_CON(10), 8, GFLAGS),
+	GATE(0, "gpll_cpu", "gpll", 0, RK2928_CLKGATE_CON(0), 1, GFLAGS),
+	COMPOSITE_NOGATE(0, "aclk_cpu_src", mux_busclk_p, 0,
+			RK2928_CLKSEL_CON(0), 14, 2, MFLAGS, 8, 5, DFLAGS),
+	GATE(ACLK_CPU, "aclk_cpu", "aclk_cpu_src", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 3, GFLAGS),
+	COMPOSITE_NOMUX(PCLK_CPU, "pclk_cpu", "aclk_cpu_src", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 12, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(0), 5, GFLAGS),
+	COMPOSITE_NOMUX(HCLK_CPU, "hclk_cpu", "aclk_cpu_src", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(0), 4, GFLAGS),
+
+	COMPOSITE(0, "aclk_peri_src", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(10), 14, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(2), 0, GFLAGS),
+
+	GATE(ACLK_PERI, "aclk_peri", "aclk_peri_src", 0,
+			RK2928_CLKGATE_CON(2), 1, GFLAGS),
+	DIV(0, "pclk_peri_src", "aclk_peri_src", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(10), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
+	GATE(PCLK_PERI, "pclk_peri", "pclk_peri_src", 0,
+			RK2928_CLKGATE_CON(2), 3, GFLAGS),
+	DIV(0, "hclk_peri_src", "aclk_peri_src", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(10), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
+	GATE(HCLK_PERI, "hclk_peri", "hclk_peri_src", 0,
+			RK2928_CLKGATE_CON(2), 2, GFLAGS),
+
+	COMPOSITE_NODIV(SCLK_TIMER0, "sclk_timer0", mux_timer_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(2), 4, 1, MFLAGS,
+			RK2928_CLKGATE_CON(1), 0, GFLAGS),
+	COMPOSITE_NODIV(SCLK_TIMER1, "sclk_timer1", mux_timer_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(2), 5, 1, MFLAGS,
+			RK2928_CLKGATE_CON(1), 1, GFLAGS),
+	COMPOSITE_NODIV(SCLK_TIMER2, "sclk_timer2", mux_timer_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(2), 6, 1, MFLAGS,
+			RK2928_CLKGATE_CON(2), 4, GFLAGS),
+	COMPOSITE_NODIV(SCLK_TIMER3, "sclk_timer3", mux_timer_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(2), 7, 1, MFLAGS,
+			RK2928_CLKGATE_CON(2), 5, GFLAGS),
+
+	MUX(0, "uart_pll_clk", mux_pll_src_apll_dpll_gpll_usb480m_p, 0,
+			RK2928_CLKSEL_CON(13), 10, 2, MFLAGS),
+	COMPOSITE_NOMUX(0, "uart0_src", "uart_pll_clk", 0,
+			RK2928_CLKSEL_CON(13), 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(1), 8, GFLAGS),
+	COMPOSITE_NOMUX(0, "uart1_src", "uart_pll_clk", 0,
+			RK2928_CLKSEL_CON(14), 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(1), 10, GFLAGS),
+	COMPOSITE_NOMUX(0, "uart2_src", "uart_pll_clk", 0,
+			RK2928_CLKSEL_CON(15), 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(1), 12, GFLAGS),
+	COMPOSITE_FRACMUX(0, "uart0_frac", "uart0_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(17), 0,
+			RK2928_CLKGATE_CON(1), 9, GFLAGS,
+			&rk3036_uart0_fracmux),
+	COMPOSITE_FRACMUX(0, "uart1_frac", "uart1_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(18), 0,
+			RK2928_CLKGATE_CON(1), 11, GFLAGS,
+			&rk3036_uart1_fracmux),
+	COMPOSITE_FRACMUX(0, "uart2_frac", "uart2_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(19), 0,
+			RK2928_CLKGATE_CON(1), 13, GFLAGS,
+			&rk3036_uart2_fracmux),
+
+	COMPOSITE(0, "aclk_vcodec", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(32), 14, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 11, GFLAGS),
+
+	COMPOSITE(0, "aclk_hvec", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(20), 0, 2, MFLAGS, 2, 5, DFLAGS,
+			RK2928_CLKGATE_CON(10), 6, GFLAGS),
+
+	COMPOSITE(0, "aclk_disp1_pre", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(31), 14, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 4, GFLAGS),
+	COMPOSITE(0, "hclk_disp_pre", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(30), 14, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(0), 11, GFLAGS),
+	COMPOSITE(SCLK_LCDC, "dclk_lcdc", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(28), 0, 2, MFLAGS, 8, 8, DFLAGS,
+			RK2928_CLKGATE_CON(3), 2, GFLAGS),
+
+	COMPOSITE_NODIV(0, "sclk_sdmmc_src", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(12), 8, 2, MFLAGS,
+			RK2928_CLKGATE_CON(2), 11, GFLAGS),
+	DIV(SCLK_SDMMC, "sclk_sdmmc", "sclk_sdmmc_src", 0,
+			RK2928_CLKSEL_CON(11), 0, 7, DFLAGS),
+
+	COMPOSITE_NODIV(0, "sclk_sdio_src", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(12), 10, 2, MFLAGS,
+			RK2928_CLKGATE_CON(2), 13, GFLAGS),
+	DIV(SCLK_SDIO, "sclk_sdio", "sclk_sdio_src", 0,
+			RK2928_CLKSEL_CON(11), 8, 7, DFLAGS),
+
+	COMPOSITE(SCLK_EMMC, "sclk_emmc", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(12), 12, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(2), 14, GFLAGS),
+
+	MMC(SCLK_SDMMC_DRV,    "sdmmc_drv",    "sclk_sdmmc", RK3036_SDMMC_CON0, 1),
+	MMC(SCLK_SDMMC_SAMPLE, "sdmmc_sample", "sclk_sdmmc", RK3036_SDMMC_CON1, 0),
+
+	MMC(SCLK_SDIO_DRV,     "sdio_drv",     "sclk_sdio",  RK3036_SDIO_CON0, 1),
+	MMC(SCLK_SDIO_SAMPLE,  "sdio_sample",  "sclk_sdio",  RK3036_SDIO_CON1, 0),
+
+	MMC(SCLK_EMMC_DRV,     "emmc_drv",     "sclk_emmc",  RK3036_EMMC_CON0,  1),
+	MMC(SCLK_EMMC_SAMPLE,  "emmc_sample",  "sclk_emmc",  RK3036_EMMC_CON1,  0),
+
+	COMPOSITE(0, "i2s_src", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(3), 14, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(0), 9, GFLAGS),
+	COMPOSITE_FRACMUX(0, "i2s_frac", "i2s_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(7), 0,
+			RK2928_CLKGATE_CON(0), 10, GFLAGS,
+			&rk3036_i2s_fracmux),
+	COMPOSITE_NODIV(SCLK_I2S_OUT, "i2s_clkout", mux_i2s_clkout_p, 0,
+			RK2928_CLKSEL_CON(3), 12, 1, MFLAGS,
+			RK2928_CLKGATE_CON(0), 13, GFLAGS),
+	GATE(SCLK_I2S, "sclk_i2s", "i2s_pre", CLK_SET_RATE_PARENT,
+			RK2928_CLKGATE_CON(0), 14, GFLAGS),
+
+	COMPOSITE(0, "spdif_src", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(5), 10, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(2), 10, GFLAGS),
+	COMPOSITE_FRACMUX(0, "spdif_frac", "spdif_src", 0,
+			RK2928_CLKSEL_CON(9), 0,
+			RK2928_CLKGATE_CON(2), 12, GFLAGS,
+			&rk3036_spdif_fracmux),
+
+	GATE(SCLK_OTGPHY0, "sclk_otgphy0", "xin12m", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(1), 5, GFLAGS),
+
+	COMPOSITE(SCLK_GPU, "sclk_gpu", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(34), 8, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 13, GFLAGS),
+
+	COMPOSITE(SCLK_SPI, "sclk_spi", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(25), 8, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(2), 9, GFLAGS),
+
+	COMPOSITE(SCLK_NANDC, "sclk_nandc", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(16), 8, 2, MFLAGS, 10, 5, DFLAGS,
+			RK2928_CLKGATE_CON(10), 4, GFLAGS),
+
+	COMPOSITE(SCLK_SFC, "sclk_sfc", mux_pll_src_apll_dpll_gpll_usb480m_p, 0,
+			RK2928_CLKSEL_CON(16), 0, 2, MFLAGS, 2, 5, DFLAGS,
+			RK2928_CLKGATE_CON(10), 5, GFLAGS),
+
+	COMPOSITE_NOGATE(0, "mac_pll_src", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(21), 0, 2, MFLAGS, 9, 5, DFLAGS),
+	MUX(SCLK_MACREF, "mac_clk_ref", mux_mac_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(21), 3, 1, MFLAGS),
+
+	COMPOSITE_NOMUX(SCLK_MAC, "mac_clk", "mac_clk_ref", 0,
+			RK2928_CLKSEL_CON(21), 4, 5, DFLAGS,
+			RK2928_CLKGATE_CON(2), 6, GFLAGS),
+
+	MUX(SCLK_HDMI, "dclk_hdmi", mux_dclk_p, 0,
+			RK2928_CLKSEL_CON(31), 0, 1, MFLAGS),
+
+	/*
+	 * Clock-Architecture Diagram 3
+	 */
+
+	/* aclk_cpu gates */
+	GATE(0, "sclk_intmem", "aclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 12, GFLAGS),
+	GATE(0, "aclk_strc_sys", "aclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 10, GFLAGS),
+
+	/* hclk_cpu gates */
+	GATE(HCLK_ROM, "hclk_rom", "hclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 6, GFLAGS),
+
+	/* pclk_cpu gates */
+	GATE(PCLK_GRF, "pclk_grf", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 4, GFLAGS),
+	GATE(PCLK_DDRUPCTL, "pclk_ddrupctl", "pclk_cpu", 0, RK2928_CLKGATE_CON(5), 7, GFLAGS),
+	GATE(PCLK_ACODEC, "pclk_acodec", "pclk_cpu", 0, RK2928_CLKGATE_CON(5), 14, GFLAGS),
+	GATE(PCLK_HDMI, "pclk_hdmi", "pclk_cpu", 0, RK2928_CLKGATE_CON(3), 8, GFLAGS),
+
+	/* aclk_vio gates */
+	GATE(ACLK_VIO, "aclk_vio", "aclk_disp1_pre", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(6), 13, GFLAGS),
+	GATE(ACLK_LCDC, "aclk_lcdc", "aclk_disp1_pre", 0, RK2928_CLKGATE_CON(9), 6, GFLAGS),
+
+	GATE(HCLK_VIO_BUS, "hclk_vio_bus", "hclk_disp_pre", 0, RK2928_CLKGATE_CON(6), 12, GFLAGS),
+	GATE(HCLK_LCDC, "hclk_lcdc", "hclk_disp_pre", 0, RK2928_CLKGATE_CON(9), 5, GFLAGS),
+
+	/* hclk_video gates */
+	GATE(HCLK_VCODEC, "hclk_vcodec", "hclk_disp_pre", 0, RK2928_CLKGATE_CON(3), 12, GFLAGS),
+
+	/* xin24m gates */
+	GATE(SCLK_PVTM_CORE, "sclk_pvtm_core", "xin24m", 0, RK2928_CLKGATE_CON(10), 0, GFLAGS),
+	GATE(SCLK_PVTM_GPU, "sclk_pvtm_gpu", "xin24m", 0, RK2928_CLKGATE_CON(10), 1, GFLAGS),
+
+	/* aclk_peri gates */
+	GATE(0, "aclk_peri_axi_matrix", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 3, GFLAGS),
+	GATE(0, "aclk_cpu_peri", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 2, GFLAGS),
+	GATE(ACLK_DMAC2, "aclk_dmac2", "aclk_peri", 0, RK2928_CLKGATE_CON(5), 1, GFLAGS),
+	GATE(0, "aclk_peri_niu", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(9), 15, GFLAGS),
+
+	/* hclk_peri gates */
+	GATE(0, "hclk_peri_matrix", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 0, GFLAGS),
+	GATE(0, "hclk_usb_peri", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(9), 13, GFLAGS),
+	GATE(0, "hclk_peri_arbi", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(9), 14, GFLAGS),
+	GATE(HCLK_NANDC, "hclk_nandc", "hclk_peri", 0, RK2928_CLKGATE_CON(5), 9, GFLAGS),
+	GATE(HCLK_SDMMC, "hclk_sdmmc", "hclk_peri", 0, RK2928_CLKGATE_CON(5), 10, GFLAGS),
+	GATE(HCLK_SDIO, "hclk_sdio", "hclk_peri", 0, RK2928_CLKGATE_CON(5), 11, GFLAGS),
+	GATE(HCLK_EMMC, "hclk_emmc", "hclk_peri", 0, RK2928_CLKGATE_CON(7), 0, GFLAGS),
+	GATE(HCLK_OTG0, "hclk_otg0", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 13, GFLAGS),
+	GATE(HCLK_OTG1, "hclk_otg1", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(7), 3, GFLAGS),
+	GATE(HCLK_I2S, "hclk_i2s", "hclk_peri", 0, RK2928_CLKGATE_CON(7), 2, GFLAGS),
+	GATE(0, "hclk_sfc", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(3), 14, GFLAGS),
+	GATE(0, "hclk_mac", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(3), 15, GFLAGS),
+
+	/* pclk_peri gates */
+	GATE(0, "pclk_peri_matrix", "pclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(4), 1, GFLAGS),
+	GATE(0, "pclk_efuse", "pclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(5), 2, GFLAGS),
+	GATE(PCLK_TIMER, "pclk_timer", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 7, GFLAGS),
+	GATE(PCLK_PWM, "pclk_pwm", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 10, GFLAGS),
+	GATE(PCLK_SPI, "pclk_spi", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 12, GFLAGS),
+	GATE(PCLK_WDT, "pclk_wdt", "pclk_peri", 0, RK2928_CLKGATE_CON(7), 15, GFLAGS),
+	GATE(PCLK_UART0, "pclk_uart0", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 0, GFLAGS),
+	GATE(PCLK_UART1, "pclk_uart1", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 1, GFLAGS),
+	GATE(PCLK_UART2, "pclk_uart2", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 2, GFLAGS),
+	GATE(PCLK_I2C0, "pclk_i2c0", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 4, GFLAGS),
+	GATE(PCLK_I2C1, "pclk_i2c1", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 5, GFLAGS),
+	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 6, GFLAGS),
+	GATE(PCLK_GPIO0, "pclk_gpio0", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 9, GFLAGS),
+	GATE(PCLK_GPIO1, "pclk_gpio1", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 10, GFLAGS),
+	GATE(PCLK_GPIO2, "pclk_gpio2", "pclk_peri", 0, RK2928_CLKGATE_CON(8), 11, GFLAGS),
+};
+
+static const char *const rk3036_critical_clocks[] __initconst = {
+	"aclk_cpu",
+	"aclk_peri",
+	"hclk_peri",
+	"pclk_peri",
+};
+
+static void __init rk3036_clk_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	struct clk *clk;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: could not map cru region\n", __func__);
+		return;
+	}
+
+	rockchip_clk_init(np, reg_base, CLK_NR_CLKS);
+
+	/* xin12m is created by an cru-internal divider */
+	clk = clk_register_fixed_factor(NULL, "xin12m", "xin24m", 0, 1, 2);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock xin12m: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "usb480m", "xin24m", 0, 20, 1);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock usb480m: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "ddrphy", "ddrphy2x", 0, 1, 2);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock ddrphy: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "hclk_vcodec_pre",
+					"aclk_vcodec", 0, 1, 4);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock hclk_vcodec_pre: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "sclk_macref_out",
+					"hclk_peri_src", 0, 1, 2);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock sclk_macref_out: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	rockchip_clk_register_plls(rk3036_pll_clks,
+				   ARRAY_SIZE(rk3036_pll_clks),
+				   RK3036_GRF_SOC_STATUS0);
+	rockchip_clk_register_branches(rk3036_clk_branches,
+				  ARRAY_SIZE(rk3036_clk_branches));
+	rockchip_clk_protect_critical(rk3036_critical_clocks,
+				      ARRAY_SIZE(rk3036_critical_clocks));
+
+	rockchip_clk_register_armclk(ARMCLK, "armclk",
+			mux_armclk_p, ARRAY_SIZE(mux_armclk_p),
+			&rk3036_cpuclk_data, rk3036_cpuclk_rates,
+			ARRAY_SIZE(rk3036_cpuclk_rates));
+
+	rockchip_register_softrst(np, 9, reg_base + RK2928_SOFTRST_CON(0),
+				  ROCKCHIP_SOFTRST_HIWORD_MASK);
+
+	rockchip_register_restart_notifier(RK2928_GLB_SRST_FST, NULL);
+}
+CLK_OF_DECLARE(rk3036_cru, "rockchip,rk3036-cru", rk3036_clk_init);
diff --git a/drivers/clk/rockchip/clk-rk3188.c b/drivers/clk/rockchip/clk-rk3188.c
index abb47608713b..7f7444cbf6fc 100644
--- a/drivers/clk/rockchip/clk-rk3188.c
+++ b/drivers/clk/rockchip/clk-rk3188.c
@@ -247,6 +247,30 @@ static struct clk_div_table div_core_peri_t[] = {
 	{ /* sentinel */ },
 };
 
+static struct rockchip_clk_branch common_hsadc_out_fracmux __initdata =
+	MUX(0, "sclk_hsadc_out", mux_sclk_hsadc_p, 0,
+			RK2928_CLKSEL_CON(22), 4, 2, MFLAGS);
+
+static struct rockchip_clk_branch common_spdif_fracmux __initdata =
+	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch common_uart0_fracmux __initdata =
+	MUX(SCLK_UART0, "sclk_uart0", mux_sclk_uart0_p, 0,
+			RK2928_CLKSEL_CON(13), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch common_uart1_fracmux __initdata =
+	MUX(SCLK_UART1, "sclk_uart1", mux_sclk_uart1_p, 0,
+			RK2928_CLKSEL_CON(14), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch common_uart2_fracmux __initdata =
+	MUX(SCLK_UART2, "sclk_uart2", mux_sclk_uart2_p, 0,
+			RK2928_CLKSEL_CON(15), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch common_uart3_fracmux __initdata =
+	MUX(SCLK_UART3, "sclk_uart3", mux_sclk_uart3_p, 0,
+			RK2928_CLKSEL_CON(16), 8, 2, MFLAGS);
+
 static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	/*
 	 * Clock-Architecture Diagram 2
@@ -335,11 +359,10 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	COMPOSITE(0, "hsadc_src", mux_pll_src_gpll_cpll_p, 0,
 			RK2928_CLKSEL_CON(22), 0, 1, MFLAGS, 8, 8, DFLAGS,
 			RK2928_CLKGATE_CON(2), 6, GFLAGS),
-	COMPOSITE_FRAC(0, "hsadc_frac", "hsadc_src", 0,
+	COMPOSITE_FRACMUX(0, "hsadc_frac", "hsadc_src", 0,
 			RK2928_CLKSEL_CON(23), 0,
-			RK2928_CLKGATE_CON(2), 7, GFLAGS),
-	MUX(0, "sclk_hsadc_out", mux_sclk_hsadc_p, 0,
-			RK2928_CLKSEL_CON(22), 4, 2, MFLAGS),
+			RK2928_CLKGATE_CON(2), 7, GFLAGS,
+			&common_hsadc_out_fracmux),
 	INVERTER(SCLK_HSADC, "sclk_hsadc", "sclk_hsadc_out",
 			RK2928_CLKSEL_CON(22), 7, IFLAGS),
 
@@ -350,11 +373,10 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "spdif_pre", "i2s_src", 0,
 			RK2928_CLKSEL_CON(5), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(0), 13, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_frac", "spdif_pre", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "spdif_frac", "spdif_pll", CLK_SET_RATE_PARENT,
 			RK2928_CLKSEL_CON(9), 0,
-			RK2928_CLKGATE_CON(0), 14, GFLAGS),
-	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, CLK_SET_RATE_PARENT,
-			RK2928_CLKSEL_CON(5), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(0), 14, GFLAGS,
+			&common_spdif_fracmux),
 
 	/*
 	 * Clock-Architecture Diagram 4
@@ -385,35 +407,31 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "uart0_pre", "uart_src", 0,
 			RK2928_CLKSEL_CON(13), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(1), 8, GFLAGS),
-	COMPOSITE_FRAC(0, "uart0_frac", "uart0_pre", 0,
+	COMPOSITE_FRACMUX(0, "uart0_frac", "uart0_pre", 0,
 			RK2928_CLKSEL_CON(17), 0,
-			RK2928_CLKGATE_CON(1), 9, GFLAGS),
-	MUX(SCLK_UART0, "sclk_uart0", mux_sclk_uart0_p, 0,
-			RK2928_CLKSEL_CON(13), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(1), 9, GFLAGS,
+			&common_uart0_fracmux),
 	COMPOSITE_NOMUX(0, "uart1_pre", "uart_src", 0,
 			RK2928_CLKSEL_CON(14), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(1), 10, GFLAGS),
-	COMPOSITE_FRAC(0, "uart1_frac", "uart1_pre", 0,
+	COMPOSITE_FRACMUX(0, "uart1_frac", "uart1_pre", 0,
 			RK2928_CLKSEL_CON(18), 0,
-			RK2928_CLKGATE_CON(1), 11, GFLAGS),
-	MUX(SCLK_UART1, "sclk_uart1", mux_sclk_uart1_p, 0,
-			RK2928_CLKSEL_CON(14), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(1), 11, GFLAGS,
+			&common_uart1_fracmux),
 	COMPOSITE_NOMUX(0, "uart2_pre", "uart_src", 0,
 			RK2928_CLKSEL_CON(15), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(1), 12, GFLAGS),
-	COMPOSITE_FRAC(0, "uart2_frac", "uart2_pre", 0,
+	COMPOSITE_FRACMUX(0, "uart2_frac", "uart2_pre", 0,
 			RK2928_CLKSEL_CON(19), 0,
-			RK2928_CLKGATE_CON(1), 13, GFLAGS),
-	MUX(SCLK_UART2, "sclk_uart2", mux_sclk_uart2_p, 0,
-			RK2928_CLKSEL_CON(15), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(1), 13, GFLAGS,
+			&common_uart2_fracmux),
 	COMPOSITE_NOMUX(0, "uart3_pre", "uart_src", 0,
 			RK2928_CLKSEL_CON(16), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(1), 14, GFLAGS),
-	COMPOSITE_FRAC(0, "uart3_frac", "uart3_pre", 0,
+	COMPOSITE_FRACMUX(0, "uart3_frac", "uart3_pre", 0,
 			RK2928_CLKSEL_CON(20), 0,
-			RK2928_CLKGATE_CON(1), 15, GFLAGS),
-	MUX(SCLK_UART3, "sclk_uart3", mux_sclk_uart3_p, 0,
-			RK2928_CLKSEL_CON(16), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(1), 15, GFLAGS,
+			&common_uart3_fracmux),
 
 	GATE(SCLK_JTAG, "jtag", "ext_jtag", 0, RK2928_CLKGATE_CON(1), 3, GFLAGS),
 
@@ -523,6 +541,18 @@ static struct clk_div_table div_aclk_cpu_t[] = {
 	{ /* sentinel */ },
 };
 
+static struct rockchip_clk_branch rk3066a_i2s0_fracmux __initdata =
+	MUX(SCLK_I2S0, "sclk_i2s0", mux_sclk_i2s0_p, 0,
+			RK2928_CLKSEL_CON(2), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3066a_i2s1_fracmux __initdata =
+	MUX(SCLK_I2S1, "sclk_i2s1", mux_sclk_i2s1_p, 0,
+			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3066a_i2s2_fracmux __initdata =
+	MUX(SCLK_I2S2, "sclk_i2s2", mux_sclk_i2s2_p, 0,
+			RK2928_CLKSEL_CON(4), 8, 2, MFLAGS);
+
 static struct rockchip_clk_branch rk3066a_clk_branches[] __initdata = {
 	DIVTBL(0, "aclk_cpu_pre", "armclk", 0,
 			RK2928_CLKSEL_CON(1), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY, div_aclk_cpu_t),
@@ -584,27 +614,24 @@ static struct rockchip_clk_branch rk3066a_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "i2s0_pre", "i2s_src", 0,
 			RK2928_CLKSEL_CON(2), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(0), 7, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s0_frac", "i2s0_pre", 0,
+	COMPOSITE_FRACMUX(0, "i2s0_frac", "i2s0_pre", 0,
 			RK2928_CLKSEL_CON(6), 0,
-			RK2928_CLKGATE_CON(0), 8, GFLAGS),
-	MUX(SCLK_I2S0, "sclk_i2s0", mux_sclk_i2s0_p, 0,
-			RK2928_CLKSEL_CON(2), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(0), 8, GFLAGS,
+			&rk3066a_i2s0_fracmux),
 	COMPOSITE_NOMUX(0, "i2s1_pre", "i2s_src", 0,
 			RK2928_CLKSEL_CON(3), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(0), 9, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s1_frac", "i2s1_pre", 0,
+	COMPOSITE_FRACMUX(0, "i2s1_frac", "i2s1_pre", 0,
 			RK2928_CLKSEL_CON(7), 0,
-			RK2928_CLKGATE_CON(0), 10, GFLAGS),
-	MUX(SCLK_I2S1, "sclk_i2s1", mux_sclk_i2s1_p, 0,
-			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(0), 10, GFLAGS,
+			&rk3066a_i2s1_fracmux),
 	COMPOSITE_NOMUX(0, "i2s2_pre", "i2s_src", 0,
 			RK2928_CLKSEL_CON(4), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(0), 11, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s2_frac", "i2s2_pre", 0,
+	COMPOSITE_FRACMUX(0, "i2s2_frac", "i2s2_pre", 0,
 			RK2928_CLKSEL_CON(8), 0,
-			RK2928_CLKGATE_CON(0), 12, GFLAGS),
-	MUX(SCLK_I2S2, "sclk_i2s2", mux_sclk_i2s2_p, 0,
-			RK2928_CLKSEL_CON(4), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(0), 12, GFLAGS,
+			&rk3066a_i2s2_fracmux),
 
 	GATE(HCLK_I2S1, "hclk_i2s1", "hclk_cpu", 0, RK2928_CLKGATE_CON(7), 3, GFLAGS),
 	GATE(HCLK_I2S2, "hclk_i2s2", "hclk_cpu", 0, RK2928_CLKGATE_CON(7), 4, GFLAGS),
@@ -638,6 +665,10 @@ static struct clk_div_table div_rk3188_aclk_core_t[] = {
 PNAME(mux_hsicphy_p)		= { "sclk_otgphy0", "sclk_otgphy1",
 				    "gpll", "cpll" };
 
+static struct rockchip_clk_branch rk3188_i2s0_fracmux __initdata =
+	MUX(SCLK_I2S0, "sclk_i2s0", mux_sclk_i2s0_p, 0,
+			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS);
+
 static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX_DIVTBL(0, "aclk_core", "armclk", CLK_IGNORE_UNUSED,
 			RK2928_CLKSEL_CON(1), 3, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
@@ -691,11 +722,10 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "i2s0_pre", "i2s_src", 0,
 			RK2928_CLKSEL_CON(3), 0, 7, DFLAGS,
 			RK2928_CLKGATE_CON(0), 9, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s0_frac", "i2s0_pre", 0,
+	COMPOSITE_FRACMUX(0, "i2s0_frac", "i2s0_pre", 0,
 			RK2928_CLKSEL_CON(7), 0,
-			RK2928_CLKGATE_CON(0), 10, GFLAGS),
-	MUX(SCLK_I2S0, "sclk_i2s0", mux_sclk_i2s0_p, 0,
-			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS),
+			RK2928_CLKGATE_CON(0), 10, GFLAGS,
+			&rk3188_i2s0_fracmux),
 
 	GATE(0, "hclk_imem0", "hclk_cpu", 0, RK2928_CLKGATE_CON(4), 14, GFLAGS),
 	GATE(0, "hclk_imem1", "hclk_cpu", 0, RK2928_CLKGATE_CON(4), 15, GFLAGS),
@@ -750,7 +780,7 @@ static void __init rk3188_common_clk_init(struct device_node *np)
 	rockchip_register_softrst(np, 9, reg_base + RK2928_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
 
-	rockchip_register_restart_notifier(RK2928_GLB_SRST_FST);
+	rockchip_register_restart_notifier(RK2928_GLB_SRST_FST, NULL);
 }
 
 static void __init rk3066a_clk_init(struct device_node *np)
diff --git a/drivers/clk/rockchip/clk-rk3228.c b/drivers/clk/rockchip/clk-rk3228.c
new file mode 100644
index 000000000000..981a50205339
--- /dev/null
+++ b/drivers/clk/rockchip/clk-rk3228.c
@@ -0,0 +1,678 @@
+/*
+ * Copyright (c) 2015 Rockchip Electronics Co. Ltd.
+ * Author: Xing Zheng <zhengxing@rock-chips.com>
+ *         Jeffy Chen <jeffy.chen@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/syscore_ops.h>
+#include <dt-bindings/clock/rk3228-cru.h>
+#include "clk.h"
+
+#define RK3228_GRF_SOC_STATUS0	0x480
+
+enum rk3228_plls {
+	apll, dpll, cpll, gpll,
+};
+
+static struct rockchip_pll_rate_table rk3228_pll_rates[] = {
+	/* _mhz, _refdiv, _fbdiv, _postdiv1, _postdiv2, _dsmpd, _frac */
+	RK3036_PLL_RATE(1608000000, 1, 67, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1584000000, 1, 66, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1560000000, 1, 65, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1536000000, 1, 64, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1512000000, 1, 63, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1488000000, 1, 62, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1464000000, 1, 61, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1440000000, 1, 60, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1416000000, 1, 59, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1392000000, 1, 58, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1368000000, 1, 57, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1344000000, 1, 56, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1320000000, 1, 55, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1296000000, 1, 54, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1272000000, 1, 53, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1248000000, 1, 52, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1200000000, 1, 50, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1188000000, 2, 99, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1104000000, 1, 46, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1100000000, 12, 550, 1, 1, 1, 0),
+	RK3036_PLL_RATE(1008000000, 1, 84, 2, 1, 1, 0),
+	RK3036_PLL_RATE(1000000000, 6, 500, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 984000000, 1, 82, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 960000000, 1, 80, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 936000000, 1, 78, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 912000000, 1, 76, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 900000000, 4, 300, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 888000000, 1, 74, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 864000000, 1, 72, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 840000000, 1, 70, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 816000000, 1, 68, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 800000000, 6, 400, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 700000000, 6, 350, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 696000000, 1, 58, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 600000000, 1, 75, 3, 1, 1, 0),
+	RK3036_PLL_RATE( 594000000, 2, 99, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 504000000, 1, 63, 3, 1, 1, 0),
+	RK3036_PLL_RATE( 500000000, 6, 250, 2, 1, 1, 0),
+	RK3036_PLL_RATE( 408000000, 1, 68, 2, 2, 1, 0),
+	RK3036_PLL_RATE( 312000000, 1, 52, 2, 2, 1, 0),
+	RK3036_PLL_RATE( 216000000, 1, 72, 4, 2, 1, 0),
+	RK3036_PLL_RATE(  96000000, 1, 64, 4, 4, 1, 0),
+	{ /* sentinel */ },
+};
+
+#define RK3228_DIV_CPU_MASK		0x1f
+#define RK3228_DIV_CPU_SHIFT		8
+
+#define RK3228_DIV_PERI_MASK		0xf
+#define RK3228_DIV_PERI_SHIFT		0
+#define RK3228_DIV_ACLK_MASK		0x7
+#define RK3228_DIV_ACLK_SHIFT		4
+#define RK3228_DIV_HCLK_MASK		0x3
+#define RK3228_DIV_HCLK_SHIFT		8
+#define RK3228_DIV_PCLK_MASK		0x7
+#define RK3228_DIV_PCLK_SHIFT		12
+
+#define RK3228_CLKSEL1(_core_peri_div)					\
+	{									\
+		.reg = RK2928_CLKSEL_CON(1),					\
+		.val = HIWORD_UPDATE(_core_peri_div, RK3228_DIV_PERI_MASK,	\
+				RK3228_DIV_PERI_SHIFT)				\
+	}
+
+#define RK3228_CPUCLK_RATE(_prate, _core_peri_div)			\
+	{								\
+		.prate = _prate,					\
+		.divs = {						\
+			RK3228_CLKSEL1(_core_peri_div),		\
+		},							\
+	}
+
+static struct rockchip_cpuclk_rate_table rk3228_cpuclk_rates[] __initdata = {
+	RK3228_CPUCLK_RATE(816000000, 4),
+	RK3228_CPUCLK_RATE(600000000, 4),
+	RK3228_CPUCLK_RATE(312000000, 4),
+};
+
+static const struct rockchip_cpuclk_reg_data rk3228_cpuclk_data = {
+	.core_reg = RK2928_CLKSEL_CON(0),
+	.div_core_shift = 0,
+	.div_core_mask = 0x1f,
+	.mux_core_shift = 6,
+};
+
+PNAME(mux_pll_p)		= { "clk_24m", "xin24m" };
+
+PNAME(mux_ddrphy_p)		= { "dpll_ddr", "gpll_ddr", "apll_ddr" };
+PNAME(mux_armclk_p)		= { "apll_core", "gpll_core", "dpll_core" };
+PNAME(mux_usb480m_phy_p)	= { "usb480m_phy0", "usb480m_phy1" };
+PNAME(mux_usb480m_p)		= { "usb480m_phy", "xin24m" };
+PNAME(mux_hdmiphy_p)		= { "hdmiphy_phy", "xin24m" };
+PNAME(mux_aclk_cpu_src_p)	= { "cpll_aclk_cpu", "gpll_aclk_cpu", "hdmiphy_aclk_cpu" };
+
+PNAME(mux_pll_src_4plls_p)	= { "cpll", "gpll", "hdmiphy" "usb480m" };
+PNAME(mux_pll_src_3plls_p)	= { "cpll", "gpll", "hdmiphy" };
+PNAME(mux_pll_src_2plls_p)	= { "cpll", "gpll" };
+PNAME(mux_sclk_hdmi_cec_p)	= { "cpll", "gpll", "xin24m" };
+PNAME(mux_aclk_peri_src_p)	= { "cpll_peri", "gpll_peri", "hdmiphy_peri" };
+PNAME(mux_mmc_src_p)		= { "cpll", "gpll", "xin24m", "usb480m" };
+PNAME(mux_pll_src_cpll_gpll_usb480m_p)	= { "cpll", "gpll", "usb480m" };
+
+PNAME(mux_sclk_rga_p)		= { "gpll", "cpll", "sclk_rga_src" };
+
+PNAME(mux_sclk_vop_src_p)	= { "gpll_vop", "cpll_vop" };
+PNAME(mux_dclk_vop_p)		= { "hdmiphy", "sclk_vop_pre" };
+
+PNAME(mux_i2s0_p)		= { "i2s0_src", "i2s0_frac", "ext_i2s", "xin12m" };
+PNAME(mux_i2s1_pre_p)		= { "i2s1_src", "i2s1_frac", "ext_i2s", "xin12m" };
+PNAME(mux_i2s_out_p)		= { "i2s1_pre", "xin12m" };
+PNAME(mux_i2s2_p)		= { "i2s2_src", "i2s2_frac", "xin12m" };
+PNAME(mux_sclk_spdif_p)		= { "sclk_spdif_src", "spdif_frac", "xin12m" };
+
+PNAME(mux_aclk_gpu_pre_p)	= { "cpll_gpu", "gpll_gpu", "hdmiphy_gpu", "usb480m_gpu" };
+
+PNAME(mux_uart0_p)		= { "uart0_src", "uart0_frac", "xin24m" };
+PNAME(mux_uart1_p)		= { "uart1_src", "uart1_frac", "xin24m" };
+PNAME(mux_uart2_p)		= { "uart2_src", "uart2_frac", "xin24m" };
+
+PNAME(mux_sclk_macphy_50m_p)	= { "ext_gmac", "phy_50m_out" };
+PNAME(mux_sclk_gmac_pre_p)	= { "sclk_gmac_src", "sclk_macphy_50m" };
+PNAME(mux_sclk_macphy_p)	= { "sclk_gmac_src", "ext_gmac" };
+
+static struct rockchip_pll_clock rk3228_pll_clks[] __initdata = {
+	[apll] = PLL(pll_rk3036, PLL_APLL, "apll", mux_pll_p, 0, RK2928_PLL_CON(0),
+		     RK2928_MODE_CON, 0, 7, 0, rk3228_pll_rates),
+	[dpll] = PLL(pll_rk3036, PLL_DPLL, "dpll", mux_pll_p, 0, RK2928_PLL_CON(3),
+		     RK2928_MODE_CON, 4, 6, 0, NULL),
+	[cpll] = PLL(pll_rk3036, PLL_CPLL, "cpll", mux_pll_p, 0, RK2928_PLL_CON(6),
+		     RK2928_MODE_CON, 8, 8, 0, NULL),
+	[gpll] = PLL(pll_rk3036, PLL_GPLL, "gpll", mux_pll_p, 0, RK2928_PLL_CON(9),
+		     RK2928_MODE_CON, 12, 9, ROCKCHIP_PLL_SYNC_RATE, rk3228_pll_rates),
+};
+
+#define MFLAGS CLK_MUX_HIWORD_MASK
+#define DFLAGS CLK_DIVIDER_HIWORD_MASK
+#define GFLAGS (CLK_GATE_HIWORD_MASK | CLK_GATE_SET_TO_DISABLE)
+
+static struct rockchip_clk_branch rk3228_clk_branches[] __initdata = {
+	/*
+	 * Clock-Architecture Diagram 1
+	 */
+
+	DIV(0, "clk_24m", "xin24m", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(4), 8, 5, DFLAGS),
+
+	/* PD_DDR */
+	GATE(0, "apll_ddr", "apll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 2, GFLAGS),
+	GATE(0, "dpll_ddr", "dpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 2, GFLAGS),
+	GATE(0, "gpll_ddr", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 2, GFLAGS),
+	COMPOSITE(0, "ddrphy4x", mux_ddrphy_p, CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(26), 8, 2, MFLAGS, 0, 3, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
+			RK2928_CLKGATE_CON(7), 1, GFLAGS),
+	GATE(0, "ddrc", "ddrphy_pre", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(8), 5, GFLAGS),
+	GATE(0, "ddrphy", "ddrphy_pre", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(7), 0, GFLAGS),
+
+	/* PD_CORE */
+	GATE(0, "dpll_core", "dpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 6, GFLAGS),
+	GATE(0, "apll_core", "apll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 6, GFLAGS),
+	GATE(0, "gpll_core", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 6, GFLAGS),
+	COMPOSITE_NOMUX(0, "pclk_dbg", "armclk", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 0, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(4), 1, GFLAGS),
+	COMPOSITE_NOMUX(0, "armcore", "armclk", CLK_IGNORE_UNUSED,
+			RK2928_CLKSEL_CON(1), 4, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(4), 0, GFLAGS),
+
+	/* PD_MISC */
+	MUX(0, "hdmiphy", mux_hdmiphy_p, CLK_SET_RATE_PARENT,
+			RK2928_MISC_CON, 13, 1, MFLAGS),
+	MUX(0, "usb480m_phy", mux_usb480m_phy_p, CLK_SET_RATE_PARENT,
+			RK2928_MISC_CON, 14, 1, MFLAGS),
+	MUX(0, "usb480m", mux_usb480m_p, CLK_SET_RATE_PARENT,
+			RK2928_MISC_CON, 15, 1, MFLAGS),
+
+	/* PD_BUS */
+	GATE(0, "hdmiphy_aclk_cpu", "hdmiphy", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 1, GFLAGS),
+	GATE(0, "gpll_aclk_cpu", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 1, GFLAGS),
+	GATE(0, "cpll_aclk_cpu", "cpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(0), 1, GFLAGS),
+	COMPOSITE_NOGATE(0, "aclk_cpu_src", mux_aclk_cpu_src_p, 0,
+			RK2928_CLKSEL_CON(0), 13, 2, MFLAGS, 8, 5, DFLAGS),
+	GATE(ARMCLK, "aclk_cpu", "aclk_cpu_src", 0,
+			RK2928_CLKGATE_CON(6), 0, GFLAGS),
+	COMPOSITE_NOMUX(0, "hclk_cpu", "aclk_cpu_src", 0,
+			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS,
+			RK2928_CLKGATE_CON(6), 1, GFLAGS),
+	COMPOSITE_NOMUX(0, "pclk_bus_src", "aclk_cpu_src", 0,
+			RK2928_CLKSEL_CON(1), 12, 3, DFLAGS,
+			RK2928_CLKGATE_CON(6), 2, GFLAGS),
+	GATE(0, "pclk_cpu", "pclk_bus_src", 0,
+			RK2928_CLKGATE_CON(6), 3, GFLAGS),
+	GATE(0, "pclk_phy_pre", "pclk_bus_src", 0,
+			RK2928_CLKGATE_CON(6), 4, GFLAGS),
+	GATE(0, "pclk_ddr_pre", "pclk_bus_src", 0,
+			RK2928_CLKGATE_CON(6), 13, GFLAGS),
+
+	/* PD_VIDEO */
+	COMPOSITE(0, "aclk_vpu_pre", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(32), 5, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 11, GFLAGS),
+	GATE(0, "hclk_vpu_src", "aclk_vpu_pre", 0,
+			RK2928_CLKGATE_CON(4), 4, GFLAGS),
+
+	COMPOSITE(0, "aclk_rkvdec_pre", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(28), 6, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 2, GFLAGS),
+	GATE(0, "hclk_rkvdec_src", "aclk_rkvdec_pre", 0,
+			RK2928_CLKGATE_CON(4), 5, GFLAGS),
+
+	COMPOSITE(0, "sclk_vdec_cabac", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(28), 14, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 3, GFLAGS),
+
+	COMPOSITE(0, "sclk_vdec_core", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(34), 13, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 4, GFLAGS),
+
+	/* PD_VIO */
+	COMPOSITE(0, "aclk_iep_pre", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(31), 5, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 0, GFLAGS),
+	DIV(0, "hclk_vio_pre", "aclk_iep_pre", 0,
+			RK2928_CLKSEL_CON(2), 0, 5, DFLAGS),
+
+	COMPOSITE(0, "aclk_hdcp_pre", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(31), 13, 2, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 4, GFLAGS),
+
+	MUX(0, "sclk_rga_src", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(33), 13, 2, MFLAGS),
+	COMPOSITE_NOMUX(0, "aclk_rga_pre", "sclk_rga_src", 0,
+			RK2928_CLKSEL_CON(33), 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 2, GFLAGS),
+	COMPOSITE(0, "sclk_rga", mux_sclk_rga_p, 0,
+			RK2928_CLKSEL_CON(22), 5, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(3), 6, GFLAGS),
+
+	COMPOSITE(0, "aclk_vop_pre", mux_pll_src_4plls_p, 0,
+			RK2928_CLKSEL_CON(33), 5, 2, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 1, GFLAGS),
+
+	COMPOSITE(0, "sclk_hdcp", mux_pll_src_3plls_p, 0,
+			RK2928_CLKSEL_CON(23), 14, 2, MFLAGS, 8, 6, DFLAGS,
+			RK2928_CLKGATE_CON(3), 5, GFLAGS),
+
+	GATE(0, "sclk_hdmi_hdcp", "xin24m", 0,
+			RK2928_CLKGATE_CON(3), 7, GFLAGS),
+
+	COMPOSITE(0, "sclk_hdmi_cec", mux_sclk_hdmi_cec_p, 0,
+			RK2928_CLKSEL_CON(21), 14, 2, MFLAGS, 0, 14, DFLAGS,
+			RK2928_CLKGATE_CON(3), 8, GFLAGS),
+
+	/* PD_PERI */
+	GATE(0, "cpll_peri", "cpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(2), 0, GFLAGS),
+	GATE(0, "gpll_peri", "gpll", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(2), 0, GFLAGS),
+	GATE(0, "hdmiphy_peri", "hdmiphy", CLK_IGNORE_UNUSED,
+			RK2928_CLKGATE_CON(2), 0, GFLAGS),
+	COMPOSITE_NOGATE(0, "aclk_peri_src", mux_aclk_peri_src_p, 0,
+			RK2928_CLKSEL_CON(10), 10, 2, MFLAGS, 0, 5, DFLAGS),
+	COMPOSITE_NOMUX(PCLK_PERI, "pclk_peri", "aclk_peri_src", 0,
+			RK2928_CLKSEL_CON(10), 12, 3, DFLAGS,
+			RK2928_CLKGATE_CON(5), 2, GFLAGS),
+	COMPOSITE_NOMUX(HCLK_PERI, "hclk_peri", "aclk_peri_src", 0,
+			RK2928_CLKSEL_CON(10), 8, 2, DFLAGS,
+			RK2928_CLKGATE_CON(5), 1, GFLAGS),
+	GATE(ACLK_PERI, "aclk_peri", "aclk_peri_src", 0,
+			RK2928_CLKGATE_CON(5), 0, GFLAGS),
+
+	GATE(SCLK_TIMER0, "sclk_timer0", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 5, GFLAGS),
+	GATE(SCLK_TIMER1, "sclk_timer1", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 6, GFLAGS),
+	GATE(SCLK_TIMER2, "sclk_timer2", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 7, GFLAGS),
+	GATE(SCLK_TIMER3, "sclk_timer3", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 8, GFLAGS),
+	GATE(SCLK_TIMER4, "sclk_timer4", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 9, GFLAGS),
+	GATE(SCLK_TIMER5, "sclk_timer5", "xin24m", 0,
+			RK2928_CLKGATE_CON(6), 10, GFLAGS),
+
+	COMPOSITE(0, "sclk_crypto", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(24), 5, 1, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(2), 7, GFLAGS),
+
+	COMPOSITE(0, "sclk_tsp", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(22), 15, 1, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(2), 6, GFLAGS),
+
+	GATE(0, "sclk_hsadc", "ext_hsadc", 0,
+			RK3288_CLKGATE_CON(10), 12, GFLAGS),
+
+	COMPOSITE(0, "sclk_wifi", mux_pll_src_cpll_gpll_usb480m_p, 0,
+			RK2928_CLKSEL_CON(23), 5, 2, MFLAGS, 0, 6, DFLAGS,
+			RK2928_CLKGATE_CON(2), 15, GFLAGS),
+
+	COMPOSITE(SCLK_SDMMC, "sclk_sdmmc0", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(11), 8, 2, MFLAGS, 0, 8, DFLAGS,
+			RK2928_CLKGATE_CON(2), 11, GFLAGS),
+
+	COMPOSITE_NODIV(0, "sclk_sdio_src", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(11), 10, 2, MFLAGS,
+			RK2928_CLKGATE_CON(2), 13, GFLAGS),
+	DIV(SCLK_SDIO, "sclk_sdio", "sclk_sdio_src", 0,
+			RK2928_CLKSEL_CON(12), 0, 8, DFLAGS),
+
+	COMPOSITE_NODIV(0, "sclk_emmc_src", mux_mmc_src_p, 0,
+			RK2928_CLKSEL_CON(11), 12, 2, MFLAGS,
+			RK2928_CLKGATE_CON(2), 14, GFLAGS),
+	DIV(SCLK_EMMC, "sclk_emmc", "sclk_emmc_src", 0,
+			RK2928_CLKSEL_CON(12), 8, 8, DFLAGS),
+
+	/*
+	 * Clock-Architecture Diagram 2
+	 */
+
+	GATE(0, "gpll_vop", "gpll", 0,
+			RK2928_CLKGATE_CON(3), 1, GFLAGS),
+	GATE(0, "cpll_vop", "cpll", 0,
+			RK2928_CLKGATE_CON(3), 1, GFLAGS),
+	MUX(0, "sclk_vop_src", mux_sclk_vop_src_p, 0,
+			RK2928_CLKSEL_CON(27), 0, 1, MFLAGS),
+	DIV(0, "dclk_hdmiphy", "sclk_vop_src", 0,
+			RK2928_CLKSEL_CON(29), 0, 3, DFLAGS),
+	DIV(0, "sclk_vop_pre", "sclk_vop_src", 0,
+			RK2928_CLKSEL_CON(27), 8, 8, DFLAGS),
+	MUX(0, "dclk_vop", mux_dclk_vop_p, 0,
+			RK2928_CLKSEL_CON(27), 1, 1, MFLAGS),
+
+	COMPOSITE(0, "i2s0_src", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(9), 15, 1, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(0), 3, GFLAGS),
+	COMPOSITE_FRAC(0, "i2s0_frac", "i2s0_src", CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(8), 0,
+			RK3288_CLKGATE_CON(0), 4, GFLAGS),
+	COMPOSITE_NODIV(SCLK_I2S0, "sclk_i2s0", mux_i2s0_p, 0,
+			RK2928_CLKSEL_CON(9), 8, 2, MFLAGS,
+			RK2928_CLKGATE_CON(0), 5, GFLAGS),
+
+	COMPOSITE(0, "i2s1_src", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(3), 15, 1, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(0), 10, GFLAGS),
+	COMPOSITE_FRAC(0, "i2s1_frac", "i2s1_src", CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(7), 0,
+			RK3288_CLKGATE_CON(0), 11, GFLAGS),
+	MUX(0, "i2s1_pre", mux_i2s1_pre_p, 0,
+			RK2928_CLKSEL_CON(3), 8, 2, MFLAGS),
+	GATE(SCLK_I2S1, "sclk_i2s1", "i2s1_pre", 0,
+			RK2928_CLKGATE_CON(0), 14, GFLAGS),
+	COMPOSITE_NODIV(SCLK_I2S_OUT, "i2s_out", mux_i2s_out_p, 0,
+			RK2928_CLKSEL_CON(3), 12, 1, MFLAGS,
+			RK2928_CLKGATE_CON(0), 13, GFLAGS),
+
+	COMPOSITE(0, "i2s2_src", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(16), 15, 1, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(0), 7, GFLAGS),
+	COMPOSITE_FRAC(0, "i2s2_frac", "i2s2_src", CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(30), 0,
+			RK3288_CLKGATE_CON(0), 8, GFLAGS),
+	COMPOSITE_NODIV(SCLK_I2S2, "sclk_i2s2", mux_i2s2_p, 0,
+			RK2928_CLKSEL_CON(16), 8, 2, MFLAGS,
+			RK2928_CLKGATE_CON(0), 9, GFLAGS),
+
+	COMPOSITE(0, "sclk_spdif_src", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(6), 15, 1, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(2), 10, GFLAGS),
+	COMPOSITE_FRAC(0, "spdif_frac", "sclk_spdif_src", CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(20), 0,
+			RK3288_CLKGATE_CON(2), 12, GFLAGS),
+	MUX(SCLK_SPDIF, "sclk_spdif", mux_sclk_spdif_p, 0,
+			RK2928_CLKSEL_CON(6), 8, 2, MFLAGS),
+
+	GATE(0, "jtag", "ext_jtag", 0,
+			RK2928_CLKGATE_CON(1), 3, GFLAGS),
+
+	GATE(0, "sclk_otgphy0", "xin24m", 0,
+			RK2928_CLKGATE_CON(1), 5, GFLAGS),
+	GATE(0, "sclk_otgphy1", "xin24m", 0,
+			RK2928_CLKGATE_CON(1), 6, GFLAGS),
+
+	COMPOSITE_NOMUX(0, "sclk_tsadc", "xin24m", 0,
+			RK2928_CLKSEL_CON(24), 6, 10, DFLAGS,
+			RK2928_CLKGATE_CON(2), 8, GFLAGS),
+
+	GATE(0, "cpll_gpu", "cpll", 0,
+			RK2928_CLKGATE_CON(3), 13, GFLAGS),
+	GATE(0, "gpll_gpu", "gpll", 0,
+			RK2928_CLKGATE_CON(3), 13, GFLAGS),
+	GATE(0, "hdmiphy_gpu", "hdmiphy", 0,
+			RK2928_CLKGATE_CON(3), 13, GFLAGS),
+	GATE(0, "usb480m_gpu", "usb480m", 0,
+			RK2928_CLKGATE_CON(3), 13, GFLAGS),
+	COMPOSITE_NOGATE(0, "aclk_gpu_pre", mux_aclk_gpu_pre_p, 0,
+			RK2928_CLKSEL_CON(34), 5, 2, MFLAGS, 0, 5, DFLAGS),
+
+	COMPOSITE(SCLK_SPI0, "sclk_spi0", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(25), 8, 1, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(2), 9, GFLAGS),
+
+	/* PD_UART */
+	COMPOSITE(0, "uart0_src", mux_pll_src_cpll_gpll_usb480m_p, 0,
+			RK2928_CLKSEL_CON(13), 12, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(1), 8, GFLAGS),
+	COMPOSITE(0, "uart1_src", mux_pll_src_cpll_gpll_usb480m_p, 0,
+			RK2928_CLKSEL_CON(14), 12, 2, MFLAGS, 0, 7, DFLAGS,
+			RK2928_CLKGATE_CON(1), 10, GFLAGS),
+	COMPOSITE(0, "uart2_src", mux_pll_src_cpll_gpll_usb480m_p,
+			0, RK2928_CLKSEL_CON(15), 12, 2,
+			MFLAGS, 0, 7, DFLAGS, RK2928_CLKGATE_CON(1), 12, GFLAGS),
+	COMPOSITE_FRAC(0, "uart0_frac", "uart0_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(17), 0,
+			RK2928_CLKGATE_CON(1), 9, GFLAGS),
+	COMPOSITE_FRAC(0, "uart1_frac", "uart1_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(18), 0,
+			RK2928_CLKGATE_CON(1), 11, GFLAGS),
+	COMPOSITE_FRAC(0, "uart2_frac", "uart2_src", CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(19), 0,
+			RK2928_CLKGATE_CON(1), 13, GFLAGS),
+	MUX(SCLK_UART0, "sclk_uart0", mux_uart0_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(13), 8, 2, MFLAGS),
+	MUX(SCLK_UART1, "sclk_uart1", mux_uart1_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(14), 8, 2, MFLAGS),
+	MUX(SCLK_UART2, "sclk_uart2", mux_uart2_p, CLK_SET_RATE_PARENT,
+			RK2928_CLKSEL_CON(15), 8, 2, MFLAGS),
+
+	COMPOSITE(SCLK_NANDC, "sclk_nandc", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(2), 14, 1, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 0, GFLAGS),
+
+	COMPOSITE(0, "sclk_gmac_src", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(5), 7, 1, MFLAGS, 0, 5, DFLAGS,
+			RK2928_CLKGATE_CON(1), 7, GFLAGS),
+	MUX(0, "sclk_macphy_50m", mux_sclk_macphy_50m_p, 0,
+			RK2928_CLKSEL_CON(29), 10, 1, MFLAGS),
+	MUX(0, "sclk_gmac_pre", mux_sclk_gmac_pre_p, 0,
+			RK2928_CLKSEL_CON(5), 5, 1, MFLAGS),
+	GATE(0, "sclk_mac_refout", "sclk_gmac_pre", 0,
+			RK2928_CLKGATE_CON(5), 4, GFLAGS),
+	GATE(0, "sclk_mac_ref", "sclk_gmac_pre", 0,
+			RK2928_CLKGATE_CON(5), 3, GFLAGS),
+	GATE(0, "sclk_mac_rx", "sclk_gmac_pre", 0,
+			RK2928_CLKGATE_CON(5), 5, GFLAGS),
+	GATE(0, "sclk_mac_tx", "sclk_gmac_pre", 0,
+			RK2928_CLKGATE_CON(5), 6, GFLAGS),
+	COMPOSITE(0, "sclk_macphy", mux_sclk_macphy_p, 0,
+			RK2928_CLKSEL_CON(29), 12, 1, MFLAGS, 8, 2, DFLAGS,
+			RK2928_CLKGATE_CON(5), 7, GFLAGS),
+	COMPOSITE(0, "sclk_gmac_out", mux_pll_src_2plls_p, 0,
+			RK2928_CLKSEL_CON(5), 15, 1, MFLAGS, 8, 5, DFLAGS,
+			RK2928_CLKGATE_CON(2), 2, GFLAGS),
+
+	/*
+	 * Clock-Architecture Diagram 3
+	 */
+
+	/* PD_VOP */
+	GATE(0, "aclk_rga", "aclk_rga_pre", 0, RK2928_CLKGATE_CON(13), 0, GFLAGS),
+	GATE(0, "aclk_rga_noc", "aclk_rga_pre", 0, RK2928_CLKGATE_CON(13), 11, GFLAGS),
+	GATE(0, "aclk_iep", "aclk_iep_pre", 0, RK2928_CLKGATE_CON(13), 2, GFLAGS),
+	GATE(0, "aclk_iep_noc", "aclk_iep_pre", 0, RK2928_CLKGATE_CON(13), 9, GFLAGS),
+
+	GATE(0, "aclk_vop", "aclk_vop_pre", 0, RK2928_CLKGATE_CON(13), 5, GFLAGS),
+	GATE(0, "aclk_vop_noc", "aclk_vop_pre", 0, RK2928_CLKGATE_CON(13), 12, GFLAGS),
+
+	GATE(0, "aclk_hdcp", "aclk_hdcp_pre", 0, RK2928_CLKGATE_CON(14), 10, GFLAGS),
+	GATE(0, "aclk_hdcp_noc", "aclk_hdcp_pre", 0, RK2928_CLKGATE_CON(13), 10, GFLAGS),
+
+	GATE(0, "hclk_rga", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 1, GFLAGS),
+	GATE(0, "hclk_iep", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 3, GFLAGS),
+	GATE(0, "hclk_vop", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 6, GFLAGS),
+	GATE(0, "hclk_vio_ahb_arbi", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 7, GFLAGS),
+	GATE(0, "hclk_vio_noc", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 8, GFLAGS),
+	GATE(0, "hclk_vop_noc", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(13), 13, GFLAGS),
+	GATE(0, "hclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 7, GFLAGS),
+	GATE(0, "hclk_hdcp_mmu", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 12, GFLAGS),
+	GATE(0, "pclk_hdmi_ctrl", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 6, GFLAGS),
+	GATE(0, "pclk_vio_h2p", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 8, GFLAGS),
+	GATE(0, "pclk_hdcp", "hclk_vio_pre", 0, RK2928_CLKGATE_CON(14), 11, GFLAGS),
+
+	/* PD_PERI */
+	GATE(0, "aclk_peri_noc", "aclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 0, GFLAGS),
+	GATE(0, "aclk_gmac", "aclk_peri", 0, RK2928_CLKGATE_CON(11), 4, GFLAGS),
+
+	GATE(HCLK_SDMMC, "hclk_sdmmc", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 0, GFLAGS),
+	GATE(HCLK_SDIO, "hclk_sdio", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 1, GFLAGS),
+	GATE(HCLK_EMMC, "hclk_emmc", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 2, GFLAGS),
+	GATE(HCLK_NANDC, "hclk_nandc", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 3, GFLAGS),
+	GATE(0, "hclk_host0", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 6, GFLAGS),
+	GATE(0, "hclk_host0_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 7, GFLAGS),
+	GATE(0, "hclk_host1", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 8, GFLAGS),
+	GATE(0, "hclk_host1_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 9, GFLAGS),
+	GATE(0, "hclk_host2", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 10, GFLAGS),
+	GATE(0, "hclk_otg", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 12, GFLAGS),
+	GATE(0, "hclk_otg_pmu", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 13, GFLAGS),
+	GATE(0, "hclk_host2_arb", "hclk_peri", 0, RK2928_CLKGATE_CON(11), 14, GFLAGS),
+	GATE(0, "hclk_peri_noc", "hclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 1, GFLAGS),
+
+	GATE(0, "pclk_gmac", "pclk_peri", 0, RK2928_CLKGATE_CON(11), 5, GFLAGS),
+	GATE(0, "pclk_peri_noc", "pclk_peri", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(12), 2, GFLAGS),
+
+	/* PD_GPU */
+	GATE(0, "aclk_gpu", "aclk_gpu_pre", 0, RK2928_CLKGATE_CON(13), 14, GFLAGS),
+	GATE(0, "aclk_gpu_noc", "aclk_gpu_pre", 0, RK2928_CLKGATE_CON(13), 15, GFLAGS),
+
+	/* PD_BUS */
+	GATE(0, "sclk_initmem_mbist", "aclk_cpu", 0, RK2928_CLKGATE_CON(8), 1, GFLAGS),
+	GATE(0, "aclk_initmem", "aclk_cpu", 0, RK2928_CLKGATE_CON(8), 0, GFLAGS),
+	GATE(ACLK_DMAC, "aclk_dmac_bus", "aclk_cpu", 0, RK2928_CLKGATE_CON(8), 2, GFLAGS),
+	GATE(0, "aclk_bus_noc", "aclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(10), 1, GFLAGS),
+
+	GATE(0, "hclk_rom", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 3, GFLAGS),
+	GATE(0, "hclk_i2s0_8ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 7, GFLAGS),
+	GATE(0, "hclk_i2s1_8ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 8, GFLAGS),
+	GATE(0, "hclk_i2s2_2ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 9, GFLAGS),
+	GATE(0, "hclk_spdif_8ch", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 10, GFLAGS),
+	GATE(0, "hclk_tsp", "hclk_cpu", 0, RK2928_CLKGATE_CON(10), 11, GFLAGS),
+	GATE(0, "hclk_crypto_mst", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 11, GFLAGS),
+	GATE(0, "hclk_crypto_slv", "hclk_cpu", 0, RK2928_CLKGATE_CON(8), 12, GFLAGS),
+
+	GATE(0, "pclk_ddrupctl", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(8), 4, GFLAGS),
+	GATE(0, "pclk_ddrmon", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(8), 6, GFLAGS),
+	GATE(0, "pclk_msch_noc", "pclk_ddr_pre", 0, RK2928_CLKGATE_CON(10), 2, GFLAGS),
+
+	GATE(0, "pclk_efuse_1024", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 13, GFLAGS),
+	GATE(0, "pclk_efuse_256", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 14, GFLAGS),
+	GATE(PCLK_I2C0, "pclk_i2c0", "pclk_cpu", 0, RK2928_CLKGATE_CON(8), 15, GFLAGS),
+	GATE(PCLK_I2C1, "pclk_i2c1", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 0, GFLAGS),
+	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 1, GFLAGS),
+	GATE(PCLK_I2C3, "pclk_i2c3", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 2, GFLAGS),
+	GATE(PCLK_TIMER, "pclk_timer0", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 4, GFLAGS),
+	GATE(0, "pclk_stimer", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 5, GFLAGS),
+	GATE(PCLK_SPI0, "pclk_spi0", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 6, GFLAGS),
+	GATE(PCLK_PWM, "pclk_rk_pwm", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 7, GFLAGS),
+	GATE(PCLK_GPIO0, "pclk_gpio0", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 8, GFLAGS),
+	GATE(PCLK_GPIO1, "pclk_gpio1", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 9, GFLAGS),
+	GATE(PCLK_GPIO2, "pclk_gpio2", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 10, GFLAGS),
+	GATE(PCLK_GPIO3, "pclk_gpio3", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 11, GFLAGS),
+	GATE(PCLK_UART0, "pclk_uart0", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 12, GFLAGS),
+	GATE(PCLK_UART1, "pclk_uart1", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 13, GFLAGS),
+	GATE(PCLK_UART2, "pclk_uart2", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 14, GFLAGS),
+	GATE(0, "pclk_tsadc", "pclk_cpu", 0, RK2928_CLKGATE_CON(9), 15, GFLAGS),
+	GATE(PCLK_GRF, "pclk_grf", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(10), 0, GFLAGS),
+	GATE(0, "pclk_cru", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(10), 1, GFLAGS),
+	GATE(0, "pclk_sgrf", "pclk_cpu", CLK_IGNORE_UNUSED, RK2928_CLKGATE_CON(10), 2, GFLAGS),
+	GATE(0, "pclk_sim", "pclk_cpu", 0, RK2928_CLKGATE_CON(10), 3, GFLAGS),
+
+	GATE(0, "pclk_ddrphy", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 3, GFLAGS),
+	GATE(0, "pclk_acodecphy", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 5, GFLAGS),
+	GATE(0, "pclk_hdmiphy", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 7, GFLAGS),
+	GATE(0, "pclk_vdacphy", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 8, GFLAGS),
+	GATE(0, "pclk_phy_noc", "pclk_phy_pre", 0, RK2928_CLKGATE_CON(10), 9, GFLAGS),
+
+	GATE(0, "aclk_vpu", "aclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 0, GFLAGS),
+	GATE(0, "aclk_vpu_noc", "aclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 4, GFLAGS),
+	GATE(0, "aclk_rkvdec", "aclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 2, GFLAGS),
+	GATE(0, "aclk_rkvdec_noc", "aclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 6, GFLAGS),
+	GATE(0, "hclk_vpu", "hclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 1, GFLAGS),
+	GATE(0, "hclk_vpu_noc", "hclk_vpu_pre", 0, RK2928_CLKGATE_CON(15), 5, GFLAGS),
+	GATE(0, "hclk_rkvdec", "hclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 3, GFLAGS),
+	GATE(0, "hclk_rkvdec_noc", "hclk_rkvdec_pre", 0, RK2928_CLKGATE_CON(15), 7, GFLAGS),
+
+	/* PD_MMC */
+	MMC(SCLK_SDMMC_DRV,    "sdmmc_drv",    "sclk_sdmmc", RK3228_SDMMC_CON0, 1),
+	MMC(SCLK_SDMMC_SAMPLE, "sdmmc_sample", "sclk_sdmmc", RK3228_SDMMC_CON1, 1),
+
+	MMC(SCLK_SDIO_DRV,     "sdio_drv",     "sclk_sdio",  RK3228_SDIO_CON0,  1),
+	MMC(SCLK_SDIO_SAMPLE,  "sdio_sample",  "sclk_sdio",  RK3228_SDIO_CON1,  1),
+
+	MMC(SCLK_EMMC_DRV,     "emmc_drv",     "sclk_emmc",  RK3228_EMMC_CON0,  1),
+	MMC(SCLK_EMMC_SAMPLE,  "emmc_sample",  "sclk_emmc",  RK3228_EMMC_CON1,  1),
+};
+
+static const char *const rk3228_critical_clocks[] __initconst = {
+	"aclk_cpu",
+	"aclk_peri",
+	"hclk_peri",
+	"pclk_peri",
+};
+
+static void __init rk3228_clk_init(struct device_node *np)
+{
+	void __iomem *reg_base;
+	struct clk *clk;
+
+	reg_base = of_iomap(np, 0);
+	if (!reg_base) {
+		pr_err("%s: could not map cru region\n", __func__);
+		return;
+	}
+
+	rockchip_clk_init(np, reg_base, CLK_NR_CLKS);
+
+	/* xin12m is created by an cru-internal divider */
+	clk = clk_register_fixed_factor(NULL, "xin12m", "xin24m", 0, 1, 2);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock xin12m: %ld\n",
+				__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "ddrphy_pre", "ddrphy4x", 0, 1, 4);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock ddrphy_pre: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "hclk_vpu_pre",
+					"hclk_vpu_src", 0, 1, 4);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock hclk_vpu_pre: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	clk = clk_register_fixed_factor(NULL, "hclk_rkvdec_pre",
+					"hclk_rkvdec_src", 0, 1, 4);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock hclk_rkvdec_pre: %ld\n",
+			__func__, PTR_ERR(clk));
+
+	rockchip_clk_register_plls(rk3228_pll_clks,
+				   ARRAY_SIZE(rk3228_pll_clks),
+				   RK3228_GRF_SOC_STATUS0);
+	rockchip_clk_register_branches(rk3228_clk_branches,
+				  ARRAY_SIZE(rk3228_clk_branches));
+	rockchip_clk_protect_critical(rk3228_critical_clocks,
+				      ARRAY_SIZE(rk3228_critical_clocks));
+
+	rockchip_clk_register_armclk(ARMCLK, "armclk",
+			mux_armclk_p, ARRAY_SIZE(mux_armclk_p),
+			&rk3228_cpuclk_data, rk3228_cpuclk_rates,
+			ARRAY_SIZE(rk3228_cpuclk_rates));
+
+	rockchip_register_softrst(np, 9, reg_base + RK2928_SOFTRST_CON(0),
+				  ROCKCHIP_SOFTRST_HIWORD_MASK);
+
+	rockchip_register_restart_notifier(RK3228_GLB_SRST_FST, NULL);
+}
+CLK_OF_DECLARE(rk3228_cru, "rockchip,rk3228-cru", rk3228_clk_init);
diff --git a/drivers/clk/rockchip/clk-rk3288.c b/drivers/clk/rockchip/clk-rk3288.c
index 9040878e3e2b..984fc187d12e 100644
--- a/drivers/clk/rockchip/clk-rk3288.c
+++ b/drivers/clk/rockchip/clk-rk3288.c
@@ -225,6 +225,38 @@ static struct clk_div_table div_hclk_cpu_t[] = {
 #define GFLAGS (CLK_GATE_HIWORD_MASK | CLK_GATE_SET_TO_DISABLE)
 #define IFLAGS ROCKCHIP_INVERTER_HIWORD_MASK
 
+static struct rockchip_clk_branch rk3288_i2s_fracmux __initdata =
+	MUX(0, "i2s_pre", mux_i2s_pre_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(4), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_spdif_fracmux __initdata =
+	MUX(0, "spdif_mux", mux_spdif_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(5), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_spdif_8ch_fracmux __initdata =
+	MUX(0, "spdif_8ch_mux", mux_spdif_8ch_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(40), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_uart0_fracmux __initdata =
+	MUX(SCLK_UART0, "sclk_uart0", mux_uart0_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(13), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_uart1_fracmux __initdata =
+	MUX(SCLK_UART1, "sclk_uart1", mux_uart1_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(14), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_uart2_fracmux __initdata =
+	MUX(SCLK_UART2, "sclk_uart2", mux_uart2_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(15), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_uart3_fracmux __initdata =
+	MUX(SCLK_UART3, "sclk_uart3", mux_uart3_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(16), 8, 2, MFLAGS);
+
+static struct rockchip_clk_branch rk3288_uart4_fracmux __initdata =
+	MUX(SCLK_UART4, "sclk_uart4", mux_uart4_p, CLK_SET_RATE_PARENT,
+			RK3288_CLKSEL_CON(3), 8, 2, MFLAGS);
+
 static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	/*
 	 * Clock-Architecture Diagram 1
@@ -295,7 +327,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKGATE_CON(0), 4, GFLAGS),
 	GATE(0, "c2c_host", "aclk_cpu_src", 0,
 			RK3288_CLKGATE_CON(13), 8, GFLAGS),
-	COMPOSITE_NOMUX(0, "crypto", "aclk_cpu_pre", 0,
+	COMPOSITE_NOMUX(SCLK_CRYPTO, "crypto", "aclk_cpu_pre", 0,
 			RK3288_CLKSEL_CON(26), 6, 2, DFLAGS,
 			RK3288_CLKGATE_CON(5), 4, GFLAGS),
 	GATE(0, "aclk_bus_2pmu", "aclk_cpu_pre", CLK_IGNORE_UNUSED,
@@ -304,11 +336,10 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "i2s_src", mux_pll_src_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(4), 15, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(4), 1, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s_frac", "i2s_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "i2s_frac", "i2s_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(8), 0,
-			RK3288_CLKGATE_CON(4), 2, GFLAGS),
-	MUX(0, "i2s_pre", mux_i2s_pre_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(4), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(4), 2, GFLAGS,
+			&rk3288_i2s_fracmux),
 	COMPOSITE_NODIV(SCLK_I2S0_OUT, "i2s0_clkout", mux_i2s_clkout_p, 0,
 			RK3288_CLKSEL_CON(4), 12, 1, MFLAGS,
 			RK3288_CLKGATE_CON(4), 0, GFLAGS),
@@ -317,23 +348,23 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 
 	MUX(0, "spdif_src", mux_pll_src_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(5), 15, 1, MFLAGS),
-	COMPOSITE_NOMUX(0, "spdif_pre", "spdif_src", 0,
+	COMPOSITE_NOMUX(0, "spdif_pre", "spdif_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(5), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(4), 4, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_frac", "spdif_src", 0,
+	COMPOSITE_FRACMUX(0, "spdif_frac", "spdif_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(9), 0,
-			RK3288_CLKGATE_CON(4), 5, GFLAGS),
-	COMPOSITE_NODIV(SCLK_SPDIF, "sclk_spdif", mux_spdif_p, 0,
-			RK3288_CLKSEL_CON(5), 8, 2, MFLAGS,
+			RK3288_CLKGATE_CON(4), 5, GFLAGS,
+			&rk3288_spdif_fracmux),
+	GATE(SCLK_SPDIF, "sclk_spdif", "spdif_mux", CLK_SET_RATE_PARENT,
 			RK3288_CLKGATE_CON(4), 6, GFLAGS),
-	COMPOSITE_NOMUX(0, "spdif_8ch_pre", "spdif_src", 0,
+	COMPOSITE_NOMUX(0, "spdif_8ch_pre", "spdif_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(40), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(4), 7, GFLAGS),
-	COMPOSITE_FRAC(0, "spdif_8ch_frac", "spdif_8ch_pre", 0,
+	COMPOSITE_FRACMUX(0, "spdif_8ch_frac", "spdif_8ch_pre", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(41), 0,
-			RK3288_CLKGATE_CON(4), 8, GFLAGS),
-	COMPOSITE_NODIV(SCLK_SPDIF8CH, "sclk_spdif_8ch", mux_spdif_8ch_p, 0,
-			RK3288_CLKSEL_CON(40), 8, 2, MFLAGS,
+			RK3288_CLKGATE_CON(4), 8, GFLAGS,
+			&rk3288_spdif_8ch_fracmux),
+	GATE(SCLK_SPDIF8CH, "sclk_spdif_8ch", "spdif_8ch_mux", CLK_SET_RATE_PARENT,
 			RK3288_CLKGATE_CON(4), 9, GFLAGS),
 
 	GATE(0, "sclk_acc_efuse", "xin24m", 0,
@@ -536,45 +567,40 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "uart0_src", mux_pll_src_cpll_gll_usb_npll_p, 0,
 			RK3288_CLKSEL_CON(13), 13, 2, MFLAGS, 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(1), 8, GFLAGS),
-	COMPOSITE_FRAC(0, "uart0_frac", "uart0_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "uart0_frac", "uart0_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(17), 0,
-			RK3288_CLKGATE_CON(1), 9, GFLAGS),
-	MUX(SCLK_UART0, "sclk_uart0", mux_uart0_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(13), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(1), 9, GFLAGS,
+			&rk3288_uart0_fracmux),
 	MUX(0, "uart_src", mux_pll_src_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(13), 15, 1, MFLAGS),
 	COMPOSITE_NOMUX(0, "uart1_src", "uart_src", 0,
 			RK3288_CLKSEL_CON(14), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(1), 10, GFLAGS),
-	COMPOSITE_FRAC(0, "uart1_frac", "uart1_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "uart1_frac", "uart1_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(18), 0,
-			RK3288_CLKGATE_CON(1), 11, GFLAGS),
-	MUX(SCLK_UART1, "sclk_uart1", mux_uart1_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(14), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(1), 11, GFLAGS,
+			&rk3288_uart1_fracmux),
 	COMPOSITE_NOMUX(0, "uart2_src", "uart_src", 0,
 			RK3288_CLKSEL_CON(15), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(1), 12, GFLAGS),
-	COMPOSITE_FRAC(0, "uart2_frac", "uart2_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "uart2_frac", "uart2_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(19), 0,
-			RK3288_CLKGATE_CON(1), 13, GFLAGS),
-	MUX(SCLK_UART2, "sclk_uart2", mux_uart2_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(15), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(1), 13, GFLAGS,
+			&rk3288_uart2_fracmux),
 	COMPOSITE_NOMUX(0, "uart3_src", "uart_src", 0,
 			RK3288_CLKSEL_CON(16), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(1), 14, GFLAGS),
-	COMPOSITE_FRAC(0, "uart3_frac", "uart3_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "uart3_frac", "uart3_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(20), 0,
-			RK3288_CLKGATE_CON(1), 15, GFLAGS),
-	MUX(SCLK_UART3, "sclk_uart3", mux_uart3_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(16), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(1), 15, GFLAGS,
+			&rk3288_uart3_fracmux),
 	COMPOSITE_NOMUX(0, "uart4_src", "uart_src", 0,
 			RK3288_CLKSEL_CON(3), 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(2), 12, GFLAGS),
-	COMPOSITE_FRAC(0, "uart4_frac", "uart4_src", CLK_SET_RATE_PARENT,
+	COMPOSITE_FRACMUX(0, "uart4_frac", "uart4_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(7), 0,
-			RK3288_CLKGATE_CON(2), 13, GFLAGS),
-	MUX(SCLK_UART4, "sclk_uart4", mux_uart4_p, CLK_SET_RATE_PARENT,
-			RK3288_CLKSEL_CON(3), 8, 2, MFLAGS),
+			RK3288_CLKGATE_CON(2), 13, GFLAGS,
+			&rk3288_uart4_fracmux),
 
 	COMPOSITE(0, "mac_pll_src", mux_pll_src_npll_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(21), 0, 2, MFLAGS, 8, 5, DFLAGS,
@@ -644,10 +670,10 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(PCLK_PUBL0, "pclk_publ0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 15, GFLAGS),
 	GATE(PCLK_DDRUPCTL1, "pclk_ddrupctl1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 0, GFLAGS),
 	GATE(PCLK_PUBL1, "pclk_publ1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 1, GFLAGS),
-	GATE(0, "pclk_efuse_1024", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 2, GFLAGS),
+	GATE(PCLK_EFUSE1024, "pclk_efuse_1024", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 2, GFLAGS),
 	GATE(PCLK_TZPC, "pclk_tzpc", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 3, GFLAGS),
 	GATE(PCLK_UART2, "pclk_uart2", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 9, GFLAGS),
-	GATE(0, "pclk_efuse_256", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 10, GFLAGS),
+	GATE(PCLK_EFUSE256, "pclk_efuse_256", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 10, GFLAGS),
 	GATE(PCLK_RKPWM, "pclk_rkpwm", "pclk_cpu", CLK_IGNORE_UNUSED, RK3288_CLKGATE_CON(11), 11, GFLAGS),
 
 	/* ddrctrl [DDR Controller PHY clock] gates */
@@ -709,7 +735,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(SCLK_LCDC_PWM1, "sclk_lcdc_pwm1", "xin24m", 0, RK3288_CLKGATE_CON(13), 11, GFLAGS),
 	GATE(SCLK_PVTM_CORE, "sclk_pvtm_core", "xin24m", 0, RK3288_CLKGATE_CON(5), 9, GFLAGS),
 	GATE(SCLK_PVTM_GPU, "sclk_pvtm_gpu", "xin24m", 0, RK3288_CLKGATE_CON(5), 10, GFLAGS),
-	GATE(0, "sclk_mipidsi_24m", "xin24m", 0, RK3288_CLKGATE_CON(5), 15, GFLAGS),
+	GATE(SCLK_MIPIDSI_24M, "sclk_mipidsi_24m", "xin24m", 0, RK3288_CLKGATE_CON(5), 15, GFLAGS),
 
 	/* sclk_gpu gates */
 	GATE(ACLK_GPU, "aclk_gpu", "sclk_gpu", 0, RK3288_CLKGATE_CON(18), 0, GFLAGS),
@@ -783,10 +809,10 @@ static const char *const rk3288_critical_clocks[] __initconst = {
 	"pclk_pd_pmu",
 };
 
-#ifdef CONFIG_PM_SLEEP
 static void __iomem *rk3288_cru_base;
 
-/* Some CRU registers will be reset in maskrom when the system
+/*
+ * Some CRU registers will be reset in maskrom when the system
  * wakes up from fastboot.
  * So save them before suspend, restore them after resume.
  */
@@ -840,33 +866,27 @@ static void rk3288_clk_resume(void)
 	}
 }
 
+static void rk3288_clk_shutdown(void)
+{
+	writel_relaxed(0xf3030000, rk3288_cru_base + RK3288_MODE_CON);
+}
+
 static struct syscore_ops rk3288_clk_syscore_ops = {
 	.suspend = rk3288_clk_suspend,
 	.resume = rk3288_clk_resume,
 };
 
-static void rk3288_clk_sleep_init(void __iomem *reg_base)
-{
-	rk3288_cru_base = reg_base;
-	register_syscore_ops(&rk3288_clk_syscore_ops);
-}
-
-#else /* CONFIG_PM_SLEEP */
-static void rk3288_clk_sleep_init(void __iomem *reg_base) {}
-#endif
-
 static void __init rk3288_clk_init(struct device_node *np)
 {
-	void __iomem *reg_base;
 	struct clk *clk;
 
-	reg_base = of_iomap(np, 0);
-	if (!reg_base) {
+	rk3288_cru_base = of_iomap(np, 0);
+	if (!rk3288_cru_base) {
 		pr_err("%s: could not map cru region\n", __func__);
 		return;
 	}
 
-	rockchip_clk_init(np, reg_base, CLK_NR_CLKS);
+	rockchip_clk_init(np, rk3288_cru_base, CLK_NR_CLKS);
 
 	/* xin12m is created by an cru-internal divider */
 	clk = clk_register_fixed_factor(NULL, "xin12m", "xin24m", 0, 1, 2);
@@ -907,10 +927,12 @@ static void __init rk3288_clk_init(struct device_node *np)
 			&rk3288_cpuclk_data, rk3288_cpuclk_rates,
 			ARRAY_SIZE(rk3288_cpuclk_rates));
 
-	rockchip_register_softrst(np, 12, reg_base + RK3288_SOFTRST_CON(0),
+	rockchip_register_softrst(np, 12,
+				  rk3288_cru_base + RK3288_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
 
-	rockchip_register_restart_notifier(RK3288_GLB_SRST_FST);
-	rk3288_clk_sleep_init(reg_base);
+	rockchip_register_restart_notifier(RK3288_GLB_SRST_FST,
+					   rk3288_clk_shutdown);
+	register_syscore_ops(&rk3288_clk_syscore_ops);
 }
 CLK_OF_DECLARE(rk3288_cru, "rockchip,rk3288-cru", rk3288_clk_init);
diff --git a/drivers/clk/rockchip/clk-rk3368.c b/drivers/clk/rockchip/clk-rk3368.c
index 7e6b783e6eee..21f3ea909fab 100644
--- a/drivers/clk/rockchip/clk-rk3368.c
+++ b/drivers/clk/rockchip/clk-rk3368.c
@@ -184,13 +184,13 @@ static const struct rockchip_cpuclk_reg_data rk3368_cpuclkl_data = {
 
 #define RK3368_CLKSEL0(_offs, _aclkm)					\
 	{								\
-		.reg = RK3288_CLKSEL_CON(0 + _offs),			\
+		.reg = RK3368_CLKSEL_CON(0 + _offs),			\
 		.val = HIWORD_UPDATE(_aclkm, RK3368_DIV_ACLKM_MASK,	\
 				RK3368_DIV_ACLKM_SHIFT),		\
 	}
 #define RK3368_CLKSEL1(_offs, _atclk, _pdbg)				\
 	{								\
-		.reg = RK3288_CLKSEL_CON(1 + _offs),			\
+		.reg = RK3368_CLKSEL_CON(1 + _offs),			\
 		.val = HIWORD_UPDATE(_atclk, RK3368_DIV_ATCLK_MASK,	\
 				RK3368_DIV_ATCLK_SHIFT) |		\
 		       HIWORD_UPDATE(_pdbg, RK3368_DIV_PCLK_DBG_MASK,	\
@@ -780,13 +780,13 @@ static struct rockchip_clk_branch rk3368_clk_branches[] __initdata = {
 	GATE(PCLK_TSADC, "pclk_tsadc", "pclk_peri", 0, RK3368_CLKGATE_CON(20), 0, GFLAGS),
 
 	/* pclk_pd_alive gates */
-	GATE(PCLK_TIMER1, "pclk_timer1", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(14), 8, GFLAGS),
-	GATE(PCLK_TIMER0, "pclk_timer0", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(14), 7, GFLAGS),
-	GATE(0, "pclk_alive_niu", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(14), 12, GFLAGS),
-	GATE(PCLK_GRF, "pclk_grf", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(14), 11, GFLAGS),
-	GATE(PCLK_GPIO3, "pclk_gpio3", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(14), 3, GFLAGS),
-	GATE(PCLK_GPIO2, "pclk_gpio2", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(14), 2, GFLAGS),
-	GATE(PCLK_GPIO1, "pclk_gpio1", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(14), 1, GFLAGS),
+	GATE(PCLK_TIMER1, "pclk_timer1", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(22), 13, GFLAGS),
+	GATE(PCLK_TIMER0, "pclk_timer0", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(22), 12, GFLAGS),
+	GATE(0, "pclk_alive_niu", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(22), 9, GFLAGS),
+	GATE(PCLK_GRF, "pclk_grf", "pclk_pd_alive", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(22), 8, GFLAGS),
+	GATE(PCLK_GPIO3, "pclk_gpio3", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(22), 3, GFLAGS),
+	GATE(PCLK_GPIO2, "pclk_gpio2", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(22), 2, GFLAGS),
+	GATE(PCLK_GPIO1, "pclk_gpio1", "pclk_pd_alive", 0, RK3368_CLKGATE_CON(22), 1, GFLAGS),
 
 	/*
 	 * pclk_vio gates
@@ -796,12 +796,12 @@ static struct rockchip_clk_branch rk3368_clk_branches[] __initdata = {
 	GATE(0, "pclk_dphytx", "hclk_vio", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(14), 8, GFLAGS),
 
 	/* pclk_pd_pmu gates */
-	GATE(PCLK_PMUGRF, "pclk_pmugrf", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(17), 0, GFLAGS),
-	GATE(PCLK_GPIO0, "pclk_gpio0", "pclk_pd_pmu", 0, RK3368_CLKGATE_CON(17), 4, GFLAGS),
-	GATE(PCLK_SGRF, "pclk_sgrf", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(17), 3, GFLAGS),
-	GATE(0, "pclk_pmu_noc", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(17), 2, GFLAGS),
-	GATE(0, "pclk_intmem1", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(17), 1, GFLAGS),
-	GATE(PCLK_PMU, "pclk_pmu", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(17), 2, GFLAGS),
+	GATE(PCLK_PMUGRF, "pclk_pmugrf", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(23), 5, GFLAGS),
+	GATE(PCLK_GPIO0, "pclk_gpio0", "pclk_pd_pmu", 0, RK3368_CLKGATE_CON(23), 4, GFLAGS),
+	GATE(PCLK_SGRF, "pclk_sgrf", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(23), 3, GFLAGS),
+	GATE(0, "pclk_pmu_noc", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(23), 2, GFLAGS),
+	GATE(0, "pclk_intmem1", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(23), 1, GFLAGS),
+	GATE(PCLK_PMU, "pclk_pmu", "pclk_pd_pmu", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(23), 0, GFLAGS),
 
 	/* timer gates */
 	GATE(0, "sclk_timer15", "xin24m", CLK_IGNORE_UNUSED, RK3368_CLKGATE_CON(24), 11, GFLAGS),
@@ -819,6 +819,13 @@ static struct rockchip_clk_branch rk3368_clk_branches[] __initdata = {
 };
 
 static const char *const rk3368_critical_clocks[] __initconst = {
+	"aclk_bus",
+	"aclk_peri",
+	/*
+	 * pwm1 supplies vdd_logic on a lot of boards, is currently unhandled
+	 * but needs to stay enabled there (including its parents) at all times.
+	 */
+	"pclk_pwm1",
 	"pclk_pd_pmu",
 };
 
@@ -882,6 +889,6 @@ static void __init rk3368_clk_init(struct device_node *np)
 	rockchip_register_softrst(np, 15, reg_base + RK3368_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
 
-	rockchip_register_restart_notifier(RK3368_GLB_SRST_FST);
+	rockchip_register_restart_notifier(RK3368_GLB_SRST_FST, NULL);
 }
 CLK_OF_DECLARE(rk3368_cru, "rockchip,rk3368-cru", rk3368_clk_init);
diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index be6c7fd8315d..d9a0b5d4d47f 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -102,22 +102,82 @@ static struct clk *rockchip_clk_register_branch(const char *name,
 	return clk;
 }
 
+struct rockchip_clk_frac {
+	struct notifier_block			clk_nb;
+	struct clk_fractional_divider		div;
+	struct clk_gate				gate;
+
+	struct clk_mux				mux;
+	const struct clk_ops			*mux_ops;
+	int					mux_frac_idx;
+
+	bool					rate_change_remuxed;
+	int					rate_change_idx;
+};
+
+#define to_rockchip_clk_frac_nb(nb) \
+			container_of(nb, struct rockchip_clk_frac, clk_nb)
+
+static int rockchip_clk_frac_notifier_cb(struct notifier_block *nb,
+					 unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct rockchip_clk_frac *frac = to_rockchip_clk_frac_nb(nb);
+	struct clk_mux *frac_mux = &frac->mux;
+	int ret = 0;
+
+	pr_debug("%s: event %lu, old_rate %lu, new_rate: %lu\n",
+		 __func__, event, ndata->old_rate, ndata->new_rate);
+	if (event == PRE_RATE_CHANGE) {
+		frac->rate_change_idx = frac->mux_ops->get_parent(&frac_mux->hw);
+		if (frac->rate_change_idx != frac->mux_frac_idx) {
+			frac->mux_ops->set_parent(&frac_mux->hw, frac->mux_frac_idx);
+			frac->rate_change_remuxed = 1;
+		}
+	} else if (event == POST_RATE_CHANGE) {
+		/*
+		 * The POST_RATE_CHANGE notifier runs directly after the
+		 * divider clock is set in clk_change_rate, so we'll have
+		 * remuxed back to the original parent before clk_change_rate
+		 * reaches the mux itself.
+		 */
+		if (frac->rate_change_remuxed) {
+			frac->mux_ops->set_parent(&frac_mux->hw, frac->rate_change_idx);
+			frac->rate_change_remuxed = 0;
+		}
+	}
+
+	return notifier_from_errno(ret);
+}
+
 static struct clk *rockchip_clk_register_frac_branch(const char *name,
 		const char *const *parent_names, u8 num_parents,
 		void __iomem *base, int muxdiv_offset, u8 div_flags,
 		int gate_offset, u8 gate_shift, u8 gate_flags,
-		unsigned long flags, spinlock_t *lock)
+		unsigned long flags, struct rockchip_clk_branch *child,
+		spinlock_t *lock)
 {
+	struct rockchip_clk_frac *frac;
 	struct clk *clk;
 	struct clk_gate *gate = NULL;
 	struct clk_fractional_divider *div = NULL;
 	const struct clk_ops *div_ops = NULL, *gate_ops = NULL;
 
-	if (gate_offset >= 0) {
-		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
-		if (!gate)
-			return ERR_PTR(-ENOMEM);
+	if (muxdiv_offset < 0)
+		return ERR_PTR(-EINVAL);
+
+	if (child && child->branch_type != branch_mux) {
+		pr_err("%s: fractional child clock for %s can only be a mux\n",
+		       __func__, name);
+		return ERR_PTR(-EINVAL);
+	}
 
+	frac = kzalloc(sizeof(*frac), GFP_KERNEL);
+	if (!frac)
+		return ERR_PTR(-ENOMEM);
+
+	if (gate_offset >= 0) {
+		gate = &frac->gate;
 		gate->flags = gate_flags;
 		gate->reg = base + gate_offset;
 		gate->bit_idx = gate_shift;
@@ -125,13 +185,7 @@ static struct clk *rockchip_clk_register_frac_branch(const char *name,
 		gate_ops = &clk_gate_ops;
 	}
 
-	if (muxdiv_offset < 0)
-		return ERR_PTR(-EINVAL);
-
-	div = kzalloc(sizeof(*div), GFP_KERNEL);
-	if (!div)
-		return ERR_PTR(-ENOMEM);
-
+	div = &frac->div;
 	div->flags = div_flags;
 	div->reg = base + muxdiv_offset;
 	div->mshift = 16;
@@ -147,7 +201,61 @@ static struct clk *rockchip_clk_register_frac_branch(const char *name,
 				     NULL, NULL,
 				     &div->hw, div_ops,
 				     gate ? &gate->hw : NULL, gate_ops,
-				     flags);
+				     flags | CLK_SET_RATE_UNGATE);
+	if (IS_ERR(clk)) {
+		kfree(frac);
+		return clk;
+	}
+
+	if (child) {
+		struct clk_mux *frac_mux = &frac->mux;
+		struct clk_init_data init;
+		struct clk *mux_clk;
+		int i, ret;
+
+		frac->mux_frac_idx = -1;
+		for (i = 0; i < child->num_parents; i++) {
+			if (!strcmp(name, child->parent_names[i])) {
+				pr_debug("%s: found fractional parent in mux at pos %d\n",
+					 __func__, i);
+				frac->mux_frac_idx = i;
+				break;
+			}
+		}
+
+		frac->mux_ops = &clk_mux_ops;
+		frac->clk_nb.notifier_call = rockchip_clk_frac_notifier_cb;
+
+		frac_mux->reg = base + child->muxdiv_offset;
+		frac_mux->shift = child->mux_shift;
+		frac_mux->mask = BIT(child->mux_width) - 1;
+		frac_mux->flags = child->mux_flags;
+		frac_mux->lock = lock;
+		frac_mux->hw.init = &init;
+
+		init.name = child->name;
+		init.flags = child->flags | CLK_SET_RATE_PARENT;
+		init.ops = frac->mux_ops;
+		init.parent_names = child->parent_names;
+		init.num_parents = child->num_parents;
+
+		mux_clk = clk_register(NULL, &frac_mux->hw);
+		if (IS_ERR(mux_clk))
+			return clk;
+
+		rockchip_clk_add_lookup(mux_clk, child->id);
+
+		/* notifier on the fraction divider to catch rate changes */
+		if (frac->mux_frac_idx >= 0) {
+			ret = clk_notifier_register(clk, &frac->clk_nb);
+			if (ret)
+				pr_err("%s: failed to register clock notifier for %s\n",
+						__func__, name);
+		} else {
+			pr_warn("%s: could not find %s as parent of %s, rate changes may not work\n",
+				__func__, name, child->name);
+		}
+	}
 
 	return clk;
 }
@@ -251,7 +359,8 @@ void __init rockchip_clk_register_branches(
 				list->parent_names, list->num_parents,
 				reg_base, list->muxdiv_offset, list->div_flags,
 				list->gate_offset, list->gate_shift,
-				list->gate_flags, flags, &clk_lock);
+				list->gate_flags, flags, list->child,
+				&clk_lock);
 			break;
 		case branch_gate:
 			flags |= CLK_SET_RATE_PARENT;
@@ -341,9 +450,13 @@ void __init rockchip_clk_protect_critical(const char *const clocks[],
 }
 
 static unsigned int reg_restart;
+static void (*cb_restart)(void);
 static int rockchip_restart_notify(struct notifier_block *this,
 				   unsigned long mode, void *cmd)
 {
+	if (cb_restart)
+		cb_restart();
+
 	writel(0xfdb9, reg_base + reg_restart);
 	return NOTIFY_DONE;
 }
@@ -353,11 +466,12 @@ static struct notifier_block rockchip_restart_handler = {
 	.priority = 128,
 };
 
-void __init rockchip_register_restart_notifier(unsigned int reg)
+void __init rockchip_register_restart_notifier(unsigned int reg, void (*cb)(void))
 {
 	int ret;
 
 	reg_restart = reg;
+	cb_restart = cb;
 	ret = register_restart_handler(&rockchip_restart_handler);
 	if (ret)
 		pr_err("%s: cannot register restart handler, %d\n",
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index dc8ecb2673b7..ff8bd23a93ec 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -2,6 +2,9 @@
  * Copyright (c) 2014 MundoReader S.L.
  * Author: Heiko Stuebner <heiko@sntech.de>
  *
+ * Copyright (c) 2015 Rockchip Electronics Co. Ltd.
+ * Author: Xing Zheng <zhengxing@rock-chips.com>
+ *
  * based on
  *
  * samsung/clk.h
@@ -30,7 +33,7 @@ struct clk;
 #define HIWORD_UPDATE(val, mask, shift) \
 		((val) << (shift) | (mask) << ((shift) + 16))
 
-/* register positions shared by RK2928, RK3066 and RK3188 */
+/* register positions shared by RK2928, RK3036, RK3066, RK3188 and RK3228 */
 #define RK2928_PLL_CON(x)		((x) * 0x4)
 #define RK2928_MODE_CON		0x40
 #define RK2928_CLKSEL_CON(x)	((x) * 0x4 + 0x44)
@@ -40,6 +43,22 @@ struct clk;
 #define RK2928_SOFTRST_CON(x)	((x) * 0x4 + 0x110)
 #define RK2928_MISC_CON		0x134
 
+#define RK3036_SDMMC_CON0		0x144
+#define RK3036_SDMMC_CON1		0x148
+#define RK3036_SDIO_CON0		0x14c
+#define RK3036_SDIO_CON1		0x150
+#define RK3036_EMMC_CON0		0x154
+#define RK3036_EMMC_CON1		0x158
+
+#define RK3228_GLB_SRST_FST		0x1f0
+#define RK3228_GLB_SRST_SND		0x1f4
+#define RK3228_SDMMC_CON0		0x1c0
+#define RK3228_SDMMC_CON1		0x1c4
+#define RK3228_SDIO_CON0		0x1c8
+#define RK3228_SDIO_CON1		0x1cc
+#define RK3228_EMMC_CON0		0x1d8
+#define RK3228_EMMC_CON1		0x1dc
+
 #define RK3288_PLL_CON(x)		RK2928_PLL_CON(x)
 #define RK3288_MODE_CON			0x50
 #define RK3288_CLKSEL_CON(x)		((x) * 0x4 + 0x60)
@@ -74,9 +93,22 @@ struct clk;
 #define RK3368_EMMC_CON1		0x41c
 
 enum rockchip_pll_type {
+	pll_rk3036,
 	pll_rk3066,
 };
 
+#define RK3036_PLL_RATE(_rate, _refdiv, _fbdiv, _postdiv1,	\
+			_postdiv2, _dsmpd, _frac)		\
+{								\
+	.rate	= _rate##U,					\
+	.fbdiv = _fbdiv,					\
+	.postdiv1 = _postdiv1,					\
+	.refdiv = _refdiv,					\
+	.postdiv2 = _postdiv2,					\
+	.dsmpd = _dsmpd,					\
+	.frac = _frac,						\
+}
+
 #define RK3066_PLL_RATE(_rate, _nr, _nf, _no)	\
 {						\
 	.rate	= _rate##U,			\
@@ -101,6 +133,13 @@ struct rockchip_pll_rate_table {
 	unsigned int nf;
 	unsigned int no;
 	unsigned int nb;
+	/* for RK3036 */
+	unsigned int fbdiv;
+	unsigned int postdiv1;
+	unsigned int refdiv;
+	unsigned int postdiv2;
+	unsigned int dsmpd;
+	unsigned int frac;
 };
 
 /**
@@ -235,6 +274,7 @@ struct rockchip_clk_branch {
 	int				gate_offset;
 	u8				gate_shift;
 	u8				gate_flags;
+	struct rockchip_clk_branch	*child;
 };
 
 #define COMPOSITE(_id, cname, pnames, f, mo, ms, mw, mf, ds, dw,\
@@ -369,6 +409,24 @@ struct rockchip_clk_branch {
 		.gate_flags	= gf,				\
 	}
 
+#define COMPOSITE_FRACMUX(_id, cname, pname, f, mo, df, go, gs, gf, ch) \
+	{							\
+		.id		= _id,				\
+		.branch_type	= branch_fraction_divider,	\
+		.name		= cname,			\
+		.parent_names	= (const char *[]){ pname },	\
+		.num_parents	= 1,				\
+		.flags		= f,				\
+		.muxdiv_offset	= mo,				\
+		.div_shift	= 16,				\
+		.div_width	= 16,				\
+		.div_flags	= df,				\
+		.gate_offset	= go,				\
+		.gate_shift	= gs,				\
+		.gate_flags	= gf,				\
+		.child		= ch,				\
+	}
+
 #define MUX(_id, cname, pnames, f, o, s, w, mf)			\
 	{							\
 		.id		= _id,				\
@@ -464,7 +522,7 @@ void rockchip_clk_register_armclk(unsigned int lookup_id, const char *name,
 			const struct rockchip_cpuclk_rate_table *rates,
 			int nrates);
 void rockchip_clk_protect_critical(const char *const clocks[], int nclocks);
-void rockchip_register_restart_notifier(unsigned int reg);
+void rockchip_register_restart_notifier(unsigned int reg, void (*cb)(void));
 
 #define ROCKCHIP_SOFTRST_HIWORD_MASK	BIT(0)
 
diff --git a/drivers/clk/samsung/clk-cpu.c b/drivers/clk/samsung/clk-cpu.c
index 2fe37f708dc7..813003d6ce09 100644
--- a/drivers/clk/samsung/clk-cpu.c
+++ b/drivers/clk/samsung/clk-cpu.c
@@ -148,6 +148,7 @@ static int exynos_cpuclk_pre_rate_change(struct clk_notifier_data *ndata,
 	unsigned long alt_prate = clk_get_rate(cpuclk->alt_parent);
 	unsigned long alt_div = 0, alt_div_mask = DIV_MASK;
 	unsigned long div0, div1 = 0, mux_reg;
+	unsigned long flags;
 
 	/* find out the divider values to use for clock data */
 	while ((cfg_data->prate * 1000) != ndata->new_rate) {
@@ -156,7 +157,7 @@ static int exynos_cpuclk_pre_rate_change(struct clk_notifier_data *ndata,
 		cfg_data++;
 	}
 
-	spin_lock(cpuclk->lock);
+	spin_lock_irqsave(cpuclk->lock, flags);
 
 	/*
 	 * For the selected PLL clock frequency, get the pre-defined divider
@@ -212,7 +213,7 @@ static int exynos_cpuclk_pre_rate_change(struct clk_notifier_data *ndata,
 				DIV_MASK_ALL);
 	}
 
-	spin_unlock(cpuclk->lock);
+	spin_unlock_irqrestore(cpuclk->lock, flags);
 	return 0;
 }
 
@@ -223,6 +224,7 @@ static int exynos_cpuclk_post_rate_change(struct clk_notifier_data *ndata,
 	const struct exynos_cpuclk_cfg_data *cfg_data = cpuclk->cfg;
 	unsigned long div = 0, div_mask = DIV_MASK;
 	unsigned long mux_reg;
+	unsigned long flags;
 
 	/* find out the divider values to use for clock data */
 	if (cpuclk->flags & CLK_CPU_NEEDS_DEBUG_ALT_DIV) {
@@ -233,7 +235,7 @@ static int exynos_cpuclk_post_rate_change(struct clk_notifier_data *ndata,
 		}
 	}
 
-	spin_lock(cpuclk->lock);
+	spin_lock_irqsave(cpuclk->lock, flags);
 
 	/* select mout_apll as the alternate parent */
 	mux_reg = readl(base + E4210_SRC_CPU);
@@ -246,7 +248,7 @@ static int exynos_cpuclk_post_rate_change(struct clk_notifier_data *ndata,
 	}
 
 	exynos_set_safe_div(base, div, div_mask);
-	spin_unlock(cpuclk->lock);
+	spin_unlock_irqrestore(cpuclk->lock, flags);
 	return 0;
 }
 
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index 7f370d3e0983..ac03e4fe2871 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -1024,6 +1024,7 @@ static struct samsung_gate_clock exynos4_gate_clks[] __initdata = {
 			0, 0),
 	GATE(CLK_AC97, "ac97", "aclk100", GATE_IP_PERIL, 27,
 			0, 0),
+	GATE(CLK_SSS, "sss", "aclk133", GATE_IP_DMC, 4, 0, 0),
 	GATE(CLK_PPMUDMC0, "ppmudmc0", "aclk133", GATE_IP_DMC, 8, 0, 0),
 	GATE(CLK_PPMUDMC1, "ppmudmc1", "aclk133", GATE_IP_DMC, 9, 0, 0),
 	GATE(CLK_PPMUCPU, "ppmucpu", "aclk133", GATE_IP_DMC, 10, 0, 0),
diff --git a/drivers/clk/samsung/clk-exynos5420.c b/drivers/clk/samsung/clk-exynos5420.c
index 389af3c15ec4..d048dedd8b72 100644
--- a/drivers/clk/samsung/clk-exynos5420.c
+++ b/drivers/clk/samsung/clk-exynos5420.c
@@ -18,6 +18,7 @@
 #include <linux/syscore_ops.h>
 
 #include "clk.h"
+#include "clk-cpu.h"
 
 #define APLL_LOCK		0x0
 #define APLL_CON0		0x100
@@ -616,9 +617,11 @@ static struct samsung_mux_clock exynos5x_mux_clks[] __initdata = {
 	MUX(0, "mout_mspll_kfc", mout_mspll_cpu_p, SRC_TOP7, 8, 2),
 	MUX(0, "mout_mspll_cpu", mout_mspll_cpu_p, SRC_TOP7, 12, 2),
 
-	MUX(0, "mout_apll", mout_apll_p, SRC_CPU, 0, 1),
+	MUX_F(0, "mout_apll", mout_apll_p, SRC_CPU, 0, 1,
+	      CLK_SET_RATE_PARENT | CLK_RECALC_NEW_RATES, 0),
 	MUX(0, "mout_cpu", mout_cpu_p, SRC_CPU, 16, 1),
-	MUX(0, "mout_kpll", mout_kpll_p, SRC_KFC, 0, 1),
+	MUX_F(0, "mout_kpll", mout_kpll_p, SRC_KFC, 0, 1,
+	      CLK_SET_RATE_PARENT | CLK_RECALC_NEW_RATES, 0),
 	MUX(0, "mout_kfc", mout_kfc_p, SRC_KFC, 16, 1),
 
 	MUX(0, "mout_aclk200", mout_group1_p, SRC_TOP0, 8, 2),
@@ -677,8 +680,8 @@ static struct samsung_mux_clock exynos5x_mux_clks[] __initdata = {
 			SRC_TOP5, 20, 1),
 	MUX(CLK_MOUT_USER_ACLK300_DISP1, "mout_user_aclk300_disp1",
 			mout_user_aclk300_disp1_p, SRC_TOP5, 24, 1),
-	MUX(0, "mout_user_aclk300_gscl", mout_user_aclk300_gscl_p,
-			SRC_TOP5, 28, 1),
+	MUX(CLK_MOUT_USER_ACLK300_GSCL, "mout_user_aclk300_gscl",
+			mout_user_aclk300_gscl_p, SRC_TOP5, 28, 1),
 
 	MUX(0, "mout_sclk_mpll", mout_mpll_p, SRC_TOP6, 0, 1),
 	MUX(CLK_MOUT_VPLL, "mout_sclk_vpll", mout_vpll_p, SRC_TOP6, 4, 1),
@@ -729,8 +732,8 @@ static struct samsung_mux_clock exynos5x_mux_clks[] __initdata = {
 			SRC_TOP12, 20, 1),
 	MUX(CLK_MOUT_SW_ACLK300, "mout_sw_aclk300_disp1",
 			mout_sw_aclk300_disp1_p, SRC_TOP12, 24, 1),
-	MUX(0, "mout_sw_aclk300_gscl", mout_sw_aclk300_gscl_p,
-			SRC_TOP12, 28, 1),
+	MUX(CLK_MOUT_SW_ACLK300_GSCL, "mout_sw_aclk300_gscl",
+			mout_sw_aclk300_gscl_p, SRC_TOP12, 28, 1),
 
 	/* DISP1 Block */
 	MUX(0, "mout_mipi1", mout_group2_p, SRC_DISP10, 16, 3),
@@ -926,7 +929,7 @@ static struct samsung_gate_clock exynos5x_gate_clks[] __initdata = {
 			GATE_BUS_TOP, 13, 0, 0),
 	GATE(0, "aclk166", "mout_user_aclk166",
 			GATE_BUS_TOP, 14, CLK_IGNORE_UNUSED, 0),
-	GATE(0, "aclk333", "mout_aclk333",
+	GATE(0, "aclk333", "mout_user_aclk333",
 			GATE_BUS_TOP, 15, CLK_IGNORE_UNUSED, 0),
 	GATE(0, "aclk400_isp", "mout_user_aclk400_isp",
 			GATE_BUS_TOP, 16, 0, 0),
@@ -1246,6 +1249,74 @@ static struct samsung_pll_clock exynos5x_plls[nr_plls] __initdata = {
 		KPLL_CON0, NULL),
 };
 
+#define E5420_EGL_DIV0(apll, pclk_dbg, atb, cpud)			\
+		((((apll) << 24) | ((pclk_dbg) << 20) | ((atb) << 16) |	\
+		 ((cpud) << 4)))
+
+static const struct exynos_cpuclk_cfg_data exynos5420_eglclk_d[] __initconst = {
+	{ 1800000, E5420_EGL_DIV0(3, 7, 7, 4), },
+	{ 1700000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1600000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1500000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1400000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1300000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1200000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1100000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1000000, E5420_EGL_DIV0(3, 6, 6, 2), },
+	{  900000, E5420_EGL_DIV0(3, 6, 6, 2), },
+	{  800000, E5420_EGL_DIV0(3, 5, 5, 2), },
+	{  700000, E5420_EGL_DIV0(3, 5, 5, 2), },
+	{  600000, E5420_EGL_DIV0(3, 4, 4, 2), },
+	{  500000, E5420_EGL_DIV0(3, 3, 3, 2), },
+	{  400000, E5420_EGL_DIV0(3, 3, 3, 2), },
+	{  300000, E5420_EGL_DIV0(3, 3, 3, 2), },
+	{  200000, E5420_EGL_DIV0(3, 3, 3, 2), },
+	{  0 },
+};
+
+static const struct exynos_cpuclk_cfg_data exynos5800_eglclk_d[] __initconst = {
+	{ 2000000, E5420_EGL_DIV0(3, 7, 7, 4), },
+	{ 1900000, E5420_EGL_DIV0(3, 7, 7, 4), },
+	{ 1800000, E5420_EGL_DIV0(3, 7, 7, 4), },
+	{ 1700000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1600000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1500000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1400000, E5420_EGL_DIV0(3, 7, 7, 3), },
+	{ 1300000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1200000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1100000, E5420_EGL_DIV0(3, 7, 7, 2), },
+	{ 1000000, E5420_EGL_DIV0(3, 7, 6, 2), },
+	{  900000, E5420_EGL_DIV0(3, 7, 6, 2), },
+	{  800000, E5420_EGL_DIV0(3, 7, 5, 2), },
+	{  700000, E5420_EGL_DIV0(3, 7, 5, 2), },
+	{  600000, E5420_EGL_DIV0(3, 7, 4, 2), },
+	{  500000, E5420_EGL_DIV0(3, 7, 3, 2), },
+	{  400000, E5420_EGL_DIV0(3, 7, 3, 2), },
+	{  300000, E5420_EGL_DIV0(3, 7, 3, 2), },
+	{  200000, E5420_EGL_DIV0(3, 7, 3, 2), },
+	{  0 },
+};
+
+#define E5420_KFC_DIV(kpll, pclk, aclk)					\
+		((((kpll) << 24) | ((pclk) << 20) | ((aclk) << 4)))
+
+static const struct exynos_cpuclk_cfg_data exynos5420_kfcclk_d[] __initconst = {
+	{ 1400000, E5420_KFC_DIV(3, 5, 3), }, /* for Exynos5800 */
+	{ 1300000, E5420_KFC_DIV(3, 5, 2), },
+	{ 1200000, E5420_KFC_DIV(3, 5, 2), },
+	{ 1100000, E5420_KFC_DIV(3, 5, 2), },
+	{ 1000000, E5420_KFC_DIV(3, 5, 2), },
+	{  900000, E5420_KFC_DIV(3, 5, 2), },
+	{  800000, E5420_KFC_DIV(3, 5, 2), },
+	{  700000, E5420_KFC_DIV(3, 4, 2), },
+	{  600000, E5420_KFC_DIV(3, 4, 2), },
+	{  500000, E5420_KFC_DIV(3, 4, 2), },
+	{  400000, E5420_KFC_DIV(3, 3, 2), },
+	{  300000, E5420_KFC_DIV(3, 3, 2), },
+	{  200000, E5420_KFC_DIV(3, 3, 2), },
+	{  0 },
+};
+
 static const struct of_device_id ext_clk_match[] __initconst = {
 	{ .compatible = "samsung,exynos5420-oscclk", .data = (void *)0, },
 	{ },
@@ -1310,6 +1381,19 @@ static void __init exynos5x_clk_init(struct device_node *np,
 				ARRAY_SIZE(exynos5800_gate_clks));
 	}
 
+	if (soc == EXYNOS5420) {
+		exynos_register_cpu_clock(ctx, CLK_ARM_CLK, "armclk",
+			mout_cpu_p[0], mout_cpu_p[1], 0x200,
+			exynos5420_eglclk_d, ARRAY_SIZE(exynos5420_eglclk_d), 0);
+	} else {
+		exynos_register_cpu_clock(ctx, CLK_ARM_CLK, "armclk",
+			mout_cpu_p[0], mout_cpu_p[1], 0x200,
+			exynos5800_eglclk_d, ARRAY_SIZE(exynos5800_eglclk_d), 0);
+	}
+	exynos_register_cpu_clock(ctx, CLK_KFC_CLK, "kfcclk",
+		mout_kfc_p[0], mout_kfc_p[1], 0x28200,
+		exynos5420_kfcclk_d, ARRAY_SIZE(exynos5420_kfcclk_d), 0);
+
 	exynos5420_clk_sleep_init();
 
 	samsung_clk_of_add_provider(np, ctx);
diff --git a/drivers/clk/samsung/clk-s3c2410-dclk.c b/drivers/clk/samsung/clk-s3c2410-dclk.c
index e9eb935d7616..ec6fb14d951c 100644
--- a/drivers/clk/samsung/clk-s3c2410-dclk.c
+++ b/drivers/clk/samsung/clk-s3c2410-dclk.c
@@ -77,12 +77,11 @@ static u8 s3c24xx_clkout_get_parent(struct clk_hw *hw)
 static int s3c24xx_clkout_set_parent(struct clk_hw *hw, u8 index)
 {
 	struct s3c24xx_clkout *clkout = to_s3c24xx_clkout(hw);
-	int ret = 0;
 
 	s3c2410_modify_misccr((clkout->mask << clkout->shift),
 			      (index << clkout->shift));
 
-	return ret;
+	return 0;
 }
 
 static const struct clk_ops s3c24xx_clkout_ops = {
diff --git a/drivers/clk/shmobile/Makefile b/drivers/clk/shmobile/Makefile
index 97c71c885e4f..7e2579b30326 100644
--- a/drivers/clk/shmobile/Makefile
+++ b/drivers/clk/shmobile/Makefile
@@ -1,13 +1,13 @@
 obj-$(CONFIG_ARCH_EMEV2)		+= clk-emev2.o
-obj-$(CONFIG_ARCH_R7S72100)		+= clk-rz.o
-obj-$(CONFIG_ARCH_R8A73A4)		+= clk-r8a73a4.o
-obj-$(CONFIG_ARCH_R8A7740)		+= clk-r8a7740.o
-obj-$(CONFIG_ARCH_R8A7778)		+= clk-r8a7778.o
-obj-$(CONFIG_ARCH_R8A7779)		+= clk-r8a7779.o
-obj-$(CONFIG_ARCH_R8A7790)		+= clk-rcar-gen2.o
-obj-$(CONFIG_ARCH_R8A7791)		+= clk-rcar-gen2.o
-obj-$(CONFIG_ARCH_R8A7793)		+= clk-rcar-gen2.o
-obj-$(CONFIG_ARCH_R8A7794)		+= clk-rcar-gen2.o
-obj-$(CONFIG_ARCH_SH73A0)		+= clk-sh73a0.o
-obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= clk-div6.o
-obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= clk-mstp.o
+obj-$(CONFIG_ARCH_R7S72100)		+= clk-rz.o clk-mstp.o
+obj-$(CONFIG_ARCH_R8A73A4)		+= clk-r8a73a4.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7740)		+= clk-r8a7740.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7778)		+= clk-r8a7778.o clk-mstp.o
+obj-$(CONFIG_ARCH_R8A7779)		+= clk-r8a7779.o clk-mstp.o
+obj-$(CONFIG_ARCH_R8A7790)		+= clk-rcar-gen2.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7791)		+= clk-rcar-gen2.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7793)		+= clk-rcar-gen2.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7794)		+= clk-rcar-gen2.o clk-mstp.o clk-div6.o
+obj-$(CONFIG_ARCH_R8A7795)		+= renesas-cpg-mssr.o \
+					   r8a7795-cpg-mssr.o clk-div6.o
+obj-$(CONFIG_ARCH_SH73A0)		+= clk-sh73a0.o clk-mstp.o clk-div6.o
diff --git a/drivers/clk/shmobile/clk-div6.c b/drivers/clk/shmobile/clk-div6.c
index b4c8d6746f68..999994769450 100644
--- a/drivers/clk/shmobile/clk-div6.c
+++ b/drivers/clk/shmobile/clk-div6.c
@@ -18,6 +18,8 @@
 #include <linux/of_address.h>
 #include <linux/slab.h>
 
+#include "clk-div6.h"
+
 #define CPG_DIV6_CKSTP		BIT(8)
 #define CPG_DIV6_DIV(d)		((d) & 0x3f)
 #define CPG_DIV6_DIV_MASK	0x3f
@@ -172,67 +174,44 @@ static const struct clk_ops cpg_div6_clock_ops = {
 	.set_rate = cpg_div6_clock_set_rate,
 };
 
-static void __init cpg_div6_clock_init(struct device_node *np)
+
+/**
+ * cpg_div6_register - Register a DIV6 clock
+ * @name: Name of the DIV6 clock
+ * @num_parents: Number of parent clocks of the DIV6 clock (1, 4, or 8)
+ * @parent_names: Array containing the names of the parent clocks
+ * @reg: Mapped register used to control the DIV6 clock
+ */
+struct clk * __init cpg_div6_register(const char *name,
+				      unsigned int num_parents,
+				      const char **parent_names,
+				      void __iomem *reg)
 {
-	unsigned int num_parents, valid_parents;
-	const char **parent_names;
+	unsigned int valid_parents;
 	struct clk_init_data init;
 	struct div6_clock *clock;
-	const char *name;
 	struct clk *clk;
 	unsigned int i;
-	int ret;
 
 	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
 	if (!clock)
-		return;
+		return ERR_PTR(-ENOMEM);
 
-	num_parents = of_clk_get_parent_count(np);
-	if (num_parents < 1) {
-		pr_err("%s: no parent found for %s DIV6 clock\n",
-		       __func__, np->name);
-		return;
+	clock->parents = kmalloc_array(num_parents, sizeof(*clock->parents),
+				       GFP_KERNEL);
+	if (!clock->parents) {
+		clk = ERR_PTR(-ENOMEM);
+		goto free_clock;
 	}
 
-	clock->parents = kmalloc_array(num_parents, sizeof(*clock->parents),
-		GFP_KERNEL);
-	parent_names = kmalloc_array(num_parents, sizeof(*parent_names),
-				GFP_KERNEL);
-	if (!parent_names)
-		return;
+	clock->reg = reg;
 
-	/* Remap the clock register and read the divisor. Disabling the
-	 * clock overwrites the divisor, so we need to cache its value for the
-	 * enable operation.
+	/*
+	 * Read the divisor. Disabling the clock overwrites the divisor, so we
+	 * need to cache its value for the enable operation.
 	 */
-	clock->reg = of_iomap(np, 0);
-	if (clock->reg == NULL) {
-		pr_err("%s: failed to map %s DIV6 clock register\n",
-		       __func__, np->name);
-		goto error;
-	}
-
 	clock->div = (clk_readl(clock->reg) & CPG_DIV6_DIV_MASK) + 1;
 
-	/* Parse the DT properties. */
-	ret = of_property_read_string(np, "clock-output-names", &name);
-	if (ret < 0) {
-		pr_err("%s: failed to get %s DIV6 clock output name\n",
-		       __func__, np->name);
-		goto error;
-	}
-
-
-	for (i = 0, valid_parents = 0; i < num_parents; i++) {
-		const char *name = of_clk_get_parent_name(np, i);
-
-		if (name) {
-			parent_names[valid_parents] = name;
-			clock->parents[valid_parents] = i;
-			valid_parents++;
-		}
-	}
-
 	switch (num_parents) {
 	case 1:
 		/* fixed parent clock */
@@ -250,8 +229,18 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 		break;
 	default:
 		pr_err("%s: invalid number of parents for DIV6 clock %s\n",
-		       __func__, np->name);
-		goto error;
+		       __func__, name);
+		clk = ERR_PTR(-EINVAL);
+		goto free_parents;
+	}
+
+	/* Filter out invalid parents */
+	for (i = 0, valid_parents = 0; i < num_parents; i++) {
+		if (parent_names[i]) {
+			parent_names[valid_parents] = parent_names[i];
+			clock->parents[valid_parents] = i;
+			valid_parents++;
+		}
 	}
 
 	/* Register the clock. */
@@ -264,6 +253,53 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 	clock->hw.init = &init;
 
 	clk = clk_register(NULL, &clock->hw);
+	if (IS_ERR(clk))
+		goto free_parents;
+
+	return clk;
+
+free_parents:
+	kfree(clock->parents);
+free_clock:
+	kfree(clock);
+	return clk;
+}
+
+static void __init cpg_div6_clock_init(struct device_node *np)
+{
+	unsigned int num_parents;
+	const char **parent_names;
+	const char *clk_name = np->name;
+	void __iomem *reg;
+	struct clk *clk;
+	unsigned int i;
+
+	num_parents = of_clk_get_parent_count(np);
+	if (num_parents < 1) {
+		pr_err("%s: no parent found for %s DIV6 clock\n",
+		       __func__, np->name);
+		return;
+	}
+
+	parent_names = kmalloc_array(num_parents, sizeof(*parent_names),
+				GFP_KERNEL);
+	if (!parent_names)
+		return;
+
+	reg = of_iomap(np, 0);
+	if (reg == NULL) {
+		pr_err("%s: failed to map %s DIV6 clock register\n",
+		       __func__, np->name);
+		goto error;
+	}
+
+	/* Parse the DT properties. */
+	of_property_read_string(np, "clock-output-names", &clk_name);
+
+	for (i = 0; i < num_parents; i++)
+		parent_names[i] = of_clk_get_parent_name(np, i);
+
+	clk = cpg_div6_register(clk_name, num_parents, parent_names, reg);
 	if (IS_ERR(clk)) {
 		pr_err("%s: failed to register %s DIV6 clock (%ld)\n",
 		       __func__, np->name, PTR_ERR(clk));
@@ -276,9 +312,8 @@ static void __init cpg_div6_clock_init(struct device_node *np)
 	return;
 
 error:
-	if (clock->reg)
-		iounmap(clock->reg);
+	if (reg)
+		iounmap(reg);
 	kfree(parent_names);
-	kfree(clock);
 }
 CLK_OF_DECLARE(cpg_div6_clk, "renesas,cpg-div6-clock", cpg_div6_clock_init);
diff --git a/drivers/clk/shmobile/clk-div6.h b/drivers/clk/shmobile/clk-div6.h
new file mode 100644
index 000000000000..9a85a95188da
--- /dev/null
+++ b/drivers/clk/shmobile/clk-div6.h
@@ -0,0 +1,7 @@
+#ifndef __SHMOBILE_CLK_DIV6_H__
+#define __SHMOBILE_CLK_DIV6_H__
+
+struct clk *cpg_div6_register(const char *name, unsigned int num_parents,
+			      const char **parent_names, void __iomem *reg);
+
+#endif
diff --git a/drivers/clk/shmobile/clk-rcar-gen2.c b/drivers/clk/shmobile/clk-rcar-gen2.c
index 745496f7ee9c..841977240305 100644
--- a/drivers/clk/shmobile/clk-rcar-gen2.c
+++ b/drivers/clk/shmobile/clk-rcar-gen2.c
@@ -115,7 +115,7 @@ static int cpg_z_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 	 *
 	 * Using experimental measurements, it seems that no more than
 	 * ~10 iterations are needed, independently of the CPU rate.
-	 * Since this value might be dependant of external xtal rate, pll1
+	 * Since this value might be dependent on external xtal rate, pll1
 	 * rate or even the other emulation clocks rate, use 1000 as a
 	 * "super" safe value.
 	 */
@@ -262,7 +262,7 @@ static struct clk * __init cpg_adsp_clk_register(struct rcar_gen2_cpg *cpg)
  * 1  1  0	30 / 2		x172/2	x208/2	x106
  * 1  1  1	30 / 2		x172/2	x208/2	x88
  *
- * *1 :	Table 7.6 indicates VCO ouput (PLLx = VCO/2)
+ * *1 :	Table 7.6 indicates VCO output (PLLx = VCO/2)
  */
 #define CPG_PLL_CONFIG_INDEX(md)	((((md) & BIT(14)) >> 12) | \
 					 (((md) & BIT(13)) >> 12) | \
diff --git a/drivers/clk/shmobile/r8a7795-cpg-mssr.c b/drivers/clk/shmobile/r8a7795-cpg-mssr.c
new file mode 100644
index 000000000000..13e994772dfd
--- /dev/null
+++ b/drivers/clk/shmobile/r8a7795-cpg-mssr.c
@@ -0,0 +1,383 @@
+/*
+ * r8a7795 Clock Pulse Generator / Module Standby and Software Reset
+ *
+ * Copyright (C) 2015 Glider bvba
+ *
+ * Based on clk-rcar-gen3.c
+ *
+ * Copyright (C) 2015 Renesas Electronics Corp.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/bug.h>
+#include <linux/clk-provider.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+
+#include <dt-bindings/clock/r8a7795-cpg-mssr.h>
+
+#include "renesas-cpg-mssr.h"
+
+
+enum clk_ids {
+	/* Core Clock Outputs exported to DT */
+	LAST_DT_CORE_CLK = R8A7795_CLK_OSC,
+
+	/* External Input Clocks */
+	CLK_EXTAL,
+	CLK_EXTALR,
+
+	/* Internal Core Clocks */
+	CLK_MAIN,
+	CLK_PLL0,
+	CLK_PLL1,
+	CLK_PLL2,
+	CLK_PLL3,
+	CLK_PLL4,
+	CLK_PLL1_DIV2,
+	CLK_PLL1_DIV4,
+	CLK_S0,
+	CLK_S1,
+	CLK_S2,
+	CLK_S3,
+	CLK_SDSRC,
+	CLK_SSPSRC,
+
+	/* Module Clocks */
+	MOD_CLK_BASE
+};
+
+enum r8a7795_clk_types {
+	CLK_TYPE_GEN3_MAIN = CLK_TYPE_CUSTOM,
+	CLK_TYPE_GEN3_PLL0,
+	CLK_TYPE_GEN3_PLL1,
+	CLK_TYPE_GEN3_PLL2,
+	CLK_TYPE_GEN3_PLL3,
+	CLK_TYPE_GEN3_PLL4,
+};
+
+static const struct cpg_core_clk r8a7795_core_clks[] __initconst = {
+	/* External Clock Inputs */
+	DEF_INPUT("extal",  CLK_EXTAL),
+	DEF_INPUT("extalr", CLK_EXTALR),
+
+	/* Internal Core Clocks */
+	DEF_BASE(".main",       CLK_MAIN, CLK_TYPE_GEN3_MAIN, CLK_EXTAL),
+	DEF_BASE(".pll0",       CLK_PLL0, CLK_TYPE_GEN3_PLL0, CLK_MAIN),
+	DEF_BASE(".pll1",       CLK_PLL1, CLK_TYPE_GEN3_PLL1, CLK_MAIN),
+	DEF_BASE(".pll2",       CLK_PLL2, CLK_TYPE_GEN3_PLL2, CLK_MAIN),
+	DEF_BASE(".pll3",       CLK_PLL3, CLK_TYPE_GEN3_PLL3, CLK_MAIN),
+	DEF_BASE(".pll4",       CLK_PLL4, CLK_TYPE_GEN3_PLL4, CLK_MAIN),
+
+	DEF_FIXED(".pll1_div2", CLK_PLL1_DIV2,     CLK_PLL1,       2, 1),
+	DEF_FIXED(".pll1_div4", CLK_PLL1_DIV4,     CLK_PLL1_DIV2,  2, 1),
+	DEF_FIXED(".s0",        CLK_S0,            CLK_PLL1_DIV2,  2, 1),
+	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),
+
+	/* Core Clock Outputs */
+	DEF_FIXED("ztr",        R8A7795_CLK_ZTR,   CLK_PLL1_DIV2,  6, 1),
+	DEF_FIXED("ztrd2",      R8A7795_CLK_ZTRD2, CLK_PLL1_DIV2, 12, 1),
+	DEF_FIXED("zt",         R8A7795_CLK_ZT,    CLK_PLL1_DIV2,  4, 1),
+	DEF_FIXED("zx",         R8A7795_CLK_ZX,    CLK_PLL1_DIV2,  2, 1),
+	DEF_FIXED("s0d1",       R8A7795_CLK_S0D1,  CLK_S0,         1, 1),
+	DEF_FIXED("s0d4",       R8A7795_CLK_S0D4,  CLK_S0,         4, 1),
+	DEF_FIXED("s1d1",       R8A7795_CLK_S1D1,  CLK_S1,         1, 1),
+	DEF_FIXED("s1d2",       R8A7795_CLK_S1D2,  CLK_S1,         2, 1),
+	DEF_FIXED("s1d4",       R8A7795_CLK_S1D4,  CLK_S1,         4, 1),
+	DEF_FIXED("s2d1",       R8A7795_CLK_S2D1,  CLK_S2,         1, 1),
+	DEF_FIXED("s2d2",       R8A7795_CLK_S2D2,  CLK_S2,         2, 1),
+	DEF_FIXED("s2d4",       R8A7795_CLK_S2D4,  CLK_S2,         4, 1),
+	DEF_FIXED("s3d1",       R8A7795_CLK_S3D1,  CLK_S3,         1, 1),
+	DEF_FIXED("s3d2",       R8A7795_CLK_S3D2,  CLK_S3,         2, 1),
+	DEF_FIXED("s3d4",       R8A7795_CLK_S3D4,  CLK_S3,         4, 1),
+	DEF_FIXED("cl",         R8A7795_CLK_CL,    CLK_PLL1_DIV2, 48, 1),
+	DEF_FIXED("cp",         R8A7795_CLK_CP,    CLK_EXTAL,      2, 1),
+
+	DEF_DIV6P1("mso",       R8A7795_CLK_MSO,   CLK_PLL1_DIV4, 0x014),
+	DEF_DIV6P1("hdmi",      R8A7795_CLK_HDMI,  CLK_PLL1_DIV2, 0x250),
+};
+
+static const struct mssr_mod_clk r8a7795_mod_clks[] __initconst = {
+	DEF_MOD("scif5",		 202,	R8A7795_CLK_S3D4),
+	DEF_MOD("scif4",		 203,	R8A7795_CLK_S3D4),
+	DEF_MOD("scif3",		 204,	R8A7795_CLK_S3D4),
+	DEF_MOD("scif1",		 206,	R8A7795_CLK_S3D4),
+	DEF_MOD("scif0",		 207,	R8A7795_CLK_S3D4),
+	DEF_MOD("msiof3",		 208,	R8A7795_CLK_MSO),
+	DEF_MOD("msiof2",		 209,	R8A7795_CLK_MSO),
+	DEF_MOD("msiof1",		 210,	R8A7795_CLK_MSO),
+	DEF_MOD("msiof0",		 211,	R8A7795_CLK_MSO),
+	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("scif2",		 310,	R8A7795_CLK_S3D4),
+	DEF_MOD("pcie1",		 318,	R8A7795_CLK_S3D1),
+	DEF_MOD("pcie0",		 319,	R8A7795_CLK_S3D1),
+	DEF_MOD("intc-ap",		 408,	R8A7795_CLK_S3D1),
+	DEF_MOD("audmac0",		 502,	R8A7795_CLK_S3D4),
+	DEF_MOD("audmac1",		 501,	R8A7795_CLK_S3D4),
+	DEF_MOD("hscif4",		 516,	R8A7795_CLK_S3D1),
+	DEF_MOD("hscif3",		 517,	R8A7795_CLK_S3D1),
+	DEF_MOD("hscif2",		 518,	R8A7795_CLK_S3D1),
+	DEF_MOD("hscif1",		 519,	R8A7795_CLK_S3D1),
+	DEF_MOD("hscif0",		 520,	R8A7795_CLK_S3D1),
+	DEF_MOD("vspd3",		 620,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspd2",		 621,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspd1",		 622,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspd0",		 623,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspbc",		 624,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspbd",		 626,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspi2",		 629,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspi1",		 630,	R8A7795_CLK_S2D1),
+	DEF_MOD("vspi0",		 631,	R8A7795_CLK_S2D1),
+	DEF_MOD("ehci2",		 701,	R8A7795_CLK_S3D4),
+	DEF_MOD("ehci1",		 702,	R8A7795_CLK_S3D4),
+	DEF_MOD("ehci0",		 703,	R8A7795_CLK_S3D4),
+	DEF_MOD("hsusb",		 704,	R8A7795_CLK_S3D4),
+	DEF_MOD("du3",			 721,	R8A7795_CLK_S2D1),
+	DEF_MOD("du2",			 722,	R8A7795_CLK_S2D1),
+	DEF_MOD("du1",			 723,	R8A7795_CLK_S2D1),
+	DEF_MOD("du0",			 724,	R8A7795_CLK_S2D1),
+	DEF_MOD("hdmi1",		 728,	R8A7795_CLK_HDMI),
+	DEF_MOD("hdmi0",		 729,	R8A7795_CLK_HDMI),
+	DEF_MOD("etheravb",		 812,	R8A7795_CLK_S3D2),
+	DEF_MOD("sata0",		 815,	R8A7795_CLK_S3D2),
+	DEF_MOD("gpio7",		 905,	R8A7795_CLK_CP),
+	DEF_MOD("gpio6",		 906,	R8A7795_CLK_CP),
+	DEF_MOD("gpio5",		 907,	R8A7795_CLK_CP),
+	DEF_MOD("gpio4",		 908,	R8A7795_CLK_CP),
+	DEF_MOD("gpio3",		 909,	R8A7795_CLK_CP),
+	DEF_MOD("gpio2",		 910,	R8A7795_CLK_CP),
+	DEF_MOD("gpio1",		 911,	R8A7795_CLK_CP),
+	DEF_MOD("gpio0",		 912,	R8A7795_CLK_CP),
+	DEF_MOD("i2c6",			 918,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c5",			 919,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c4",			 927,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c3",			 928,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c2",			 929,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c1",			 930,	R8A7795_CLK_S3D2),
+	DEF_MOD("i2c0",			 931,	R8A7795_CLK_S3D2),
+	DEF_MOD("ssi-all",		1005,	R8A7795_CLK_S3D4),
+	DEF_MOD("ssi9",			1006,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi8",			1007,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi7",			1008,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi6",			1009,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi5",			1010,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi4",			1011,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi3",			1012,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi2",			1013,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi1",			1014,	MOD_CLK_ID(1005)),
+	DEF_MOD("ssi0",			1015,	MOD_CLK_ID(1005)),
+	DEF_MOD("scu-all",		1017,	R8A7795_CLK_S3D4),
+	DEF_MOD("scu-dvc1",		1018,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-dvc0",		1019,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-ctu1-mix1",	1020,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-ctu0-mix0",	1021,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src9",		1022,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src8",		1023,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src7",		1024,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src6",		1025,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src5",		1026,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src4",		1027,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src3",		1028,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src2",		1029,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src1",		1030,	MOD_CLK_ID(1017)),
+	DEF_MOD("scu-src0",		1031,	MOD_CLK_ID(1017)),
+};
+
+static const unsigned int r8a7795_crit_mod_clks[] __initconst = {
+	MOD_CLK_ID(408),	/* INTC-AP (GIC) */
+};
+
+
+#define CPG_PLL0CR	0x00d8
+#define CPG_PLL2CR	0x002c
+#define CPG_PLL4CR	0x01f4
+
+/*
+ * CPG Clock Data
+ */
+
+/*
+ *   MD		EXTAL		PLL0	PLL1	PLL2	PLL3	PLL4
+ * 14 13 19 17	(MHz)
+ *-------------------------------------------------------------------
+ * 0  0  0  0	16.66 x 1	x180	x192	x144	x192	x144
+ * 0  0  0  1	16.66 x 1	x180	x192	x144	x128	x144
+ * 0  0  1  0	Prohibited setting
+ * 0  0  1  1	16.66 x 1	x180	x192	x144	x192	x144
+ * 0  1  0  0	20    x 1	x150	x160	x120	x160	x120
+ * 0  1  0  1	20    x 1	x150	x160	x120	x106	x120
+ * 0  1  1  0	Prohibited setting
+ * 0  1  1  1	20    x 1	x150	x160	x120	x160	x120
+ * 1  0  0  0	25    x 1	x120	x128	x96	x128	x96
+ * 1  0  0  1	25    x 1	x120	x128	x96	x84	x96
+ * 1  0  1  0	Prohibited setting
+ * 1  0  1  1	25    x 1	x120	x128	x96	x128	x96
+ * 1  1  0  0	33.33 / 2	x180	x192	x144	x192	x144
+ * 1  1  0  1	33.33 / 2	x180	x192	x144	x128	x144
+ * 1  1  1  0	Prohibited setting
+ * 1  1  1  1	33.33 / 2	x180	x192	x144	x192	x144
+ */
+#define CPG_PLL_CONFIG_INDEX(md)	((((md) & BIT(14)) >> 11) | \
+					 (((md) & BIT(13)) >> 11) | \
+					 (((md) & BIT(19)) >> 18) | \
+					 (((md) & BIT(17)) >> 17))
+
+struct cpg_pll_config {
+	unsigned int extal_div;
+	unsigned int pll1_mult;
+	unsigned int pll3_mult;
+};
+
+static const struct cpg_pll_config cpg_pll_configs[16] __initconst = {
+	/* EXTAL div	PLL1 mult	PLL3 mult */
+	{ 1,		192,		192,	},
+	{ 1,		192,		128,	},
+	{ 0, /* Prohibited setting */		},
+	{ 1,		192,		192,	},
+	{ 1,		160,		160,	},
+	{ 1,		160,		106,	},
+	{ 0, /* Prohibited setting */		},
+	{ 1,		160,		160,	},
+	{ 1,		128,		128,	},
+	{ 1,		128,		84,	},
+	{ 0, /* Prohibited setting */		},
+	{ 1,		128,		128,	},
+	{ 2,		192,		192,	},
+	{ 2,		192,		128,	},
+	{ 0, /* Prohibited setting */		},
+	{ 2,		192,		192,	},
+};
+
+static const struct cpg_pll_config *cpg_pll_config __initdata;
+
+static
+struct clk * __init r8a7795_cpg_clk_register(struct device *dev,
+					     const struct cpg_core_clk *core,
+					     const struct cpg_mssr_info *info,
+					     struct clk **clks,
+					     void __iomem *base)
+{
+	const struct clk *parent;
+	unsigned int mult = 1;
+	unsigned int div = 1;
+	u32 value;
+
+	parent = clks[core->parent];
+	if (IS_ERR(parent))
+		return ERR_CAST(parent);
+
+	switch (core->type) {
+	case CLK_TYPE_GEN3_MAIN:
+		div = cpg_pll_config->extal_div;
+		break;
+
+	case CLK_TYPE_GEN3_PLL0:
+		/*
+		 * PLL0 is a configurable multiplier clock. Register it as a
+		 * fixed factor clock for now as there's no generic multiplier
+		 * clock implementation and we currently have no need to change
+		 * the multiplier value.
+		 */
+		value = readl(base + CPG_PLL0CR);
+		mult = (((value >> 24) & 0x7f) + 1) * 2;
+		break;
+
+	case CLK_TYPE_GEN3_PLL1:
+		mult = cpg_pll_config->pll1_mult;
+		break;
+
+	case CLK_TYPE_GEN3_PLL2:
+		/*
+		 * PLL2 is a configurable multiplier clock. Register it as a
+		 * fixed factor clock for now as there's no generic multiplier
+		 * clock implementation and we currently have no need to change
+		 * the multiplier value.
+		 */
+		value = readl(base + CPG_PLL2CR);
+		mult = (((value >> 24) & 0x7f) + 1) * 2;
+		break;
+
+	case CLK_TYPE_GEN3_PLL3:
+		mult = cpg_pll_config->pll3_mult;
+		break;
+
+	case CLK_TYPE_GEN3_PLL4:
+		/*
+		 * PLL4 is a configurable multiplier clock. Register it as a
+		 * fixed factor clock for now as there's no generic multiplier
+		 * clock implementation and we currently have no need to change
+		 * the multiplier value.
+		 */
+		value = readl(base + CPG_PLL4CR);
+		mult = (((value >> 24) & 0x7f) + 1) * 2;
+		break;
+
+	default:
+		return ERR_PTR(-EINVAL);
+	}
+
+	return clk_register_fixed_factor(NULL, core->name,
+					 __clk_get_name(parent), 0, mult, div);
+}
+
+/*
+ * Reset register definitions.
+ */
+#define MODEMR	0xe6160060
+
+static u32 rcar_gen3_read_mode_pins(void)
+{
+	void __iomem *modemr = ioremap_nocache(MODEMR, 4);
+	u32 mode;
+
+	BUG_ON(!modemr);
+	mode = ioread32(modemr);
+	iounmap(modemr);
+
+	return mode;
+}
+
+static int __init r8a7795_cpg_mssr_init(struct device *dev)
+{
+	u32 cpg_mode = rcar_gen3_read_mode_pins();
+
+	cpg_pll_config = &cpg_pll_configs[CPG_PLL_CONFIG_INDEX(cpg_mode)];
+	if (!cpg_pll_config->extal_div) {
+		dev_err(dev, "Prohibited setting (cpg_mode=0x%x)\n", cpg_mode);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct cpg_mssr_info r8a7795_cpg_mssr_info __initconst = {
+	/* Core Clocks */
+	.core_clks = r8a7795_core_clks,
+	.num_core_clks = ARRAY_SIZE(r8a7795_core_clks),
+	.last_dt_core_clk = LAST_DT_CORE_CLK,
+	.num_total_core_clks = MOD_CLK_BASE,
+
+	/* Module Clocks */
+	.mod_clks = r8a7795_mod_clks,
+	.num_mod_clks = ARRAY_SIZE(r8a7795_mod_clks),
+	.num_hw_mod_clks = 12 * 32,
+
+	/* Critical Module Clocks */
+	.crit_mod_clks = r8a7795_crit_mod_clks,
+	.num_crit_mod_clks = ARRAY_SIZE(r8a7795_crit_mod_clks),
+
+	/* Callbacks */
+	.init = r8a7795_cpg_mssr_init,
+	.cpg_clk_register = r8a7795_cpg_clk_register,
+};
diff --git a/drivers/clk/shmobile/renesas-cpg-mssr.c b/drivers/clk/shmobile/renesas-cpg-mssr.c
new file mode 100644
index 000000000000..9a4d888164bb
--- /dev/null
+++ b/drivers/clk/shmobile/renesas-cpg-mssr.c
@@ -0,0 +1,596 @@
+/*
+ * Renesas Clock Pulse Generator / Module Standby and Software Reset
+ *
+ * Copyright (C) 2015 Glider bvba
+ *
+ * Based on clk-mstp.c, clk-rcar-gen2.c, and clk-rcar-gen3.c
+ *
+ * Copyright (C) 2013 Ideas On Board SPRL
+ * Copyright (C) 2015 Renesas Electronics Corp.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/device.h>
+#include <linux/init.h>
+#include <linux/mod_devicetable.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_clock.h>
+#include <linux/pm_domain.h>
+#include <linux/slab.h>
+
+#include <dt-bindings/clock/renesas-cpg-mssr.h>
+
+#include "renesas-cpg-mssr.h"
+#include "clk-div6.h"
+
+#ifdef DEBUG
+#define WARN_DEBUG(x)	do { } while (0)
+#else
+#define WARN_DEBUG(x)	WARN_ON(x)
+#endif
+
+
+/*
+ * Module Standby and Software Reset register offets.
+ *
+ * If the registers exist, these are valid for SH-Mobile, R-Mobile,
+ * R-Car Gen 2, and R-Car Gen 3.
+ * These are NOT valid for R-Car Gen1 and RZ/A1!
+ */
+
+/*
+ * Module Stop Status Register offsets
+ */
+
+static const u16 mstpsr[] = {
+	0x030, 0x038, 0x040, 0x048, 0x04C, 0x03C, 0x1C0, 0x1C4,
+	0x9A0, 0x9A4, 0x9A8, 0x9AC,
+};
+
+#define	MSTPSR(i)	mstpsr[i]
+
+
+/*
+ * System Module Stop Control Register offsets
+ */
+
+static const u16 smstpcr[] = {
+	0x130, 0x134, 0x138, 0x13C, 0x140, 0x144, 0x148, 0x14C,
+	0x990, 0x994, 0x998, 0x99C,
+};
+
+#define	SMSTPCR(i)	smstpcr[i]
+
+
+/*
+ * Software Reset Register offsets
+ */
+
+static const u16 srcr[] = {
+	0x0A0, 0x0A8, 0x0B0, 0x0B8, 0x0BC, 0x0C4, 0x1C8, 0x1CC,
+	0x920, 0x924, 0x928, 0x92C,
+};
+
+#define	SRCR(i)		srcr[i]
+
+
+/* Realtime Module Stop Control Register offsets */
+#define RMSTPCR(i)	(smstpcr[i] - 0x20)
+
+/* Modem Module Stop Control Register offsets (r8a73a4) */
+#define MMSTPCR(i)	(smstpcr[i] + 0x20)
+
+/* Software Reset Clearing Register offsets */
+#define	SRSTCLR(i)	(0x940 + (i) * 4)
+
+
+/**
+ * Clock Pulse Generator / Module Standby and Software Reset Private Data
+ *
+ * @dev: CPG/MSSR device
+ * @base: CPG/MSSR register block base address
+ * @mstp_lock: protects writes to SMSTPCR
+ * @clks: Array containing all Core and Module Clocks
+ * @num_core_clks: Number of Core Clocks in clks[]
+ * @num_mod_clks: Number of Module Clocks in clks[]
+ * @last_dt_core_clk: ID of the last Core Clock exported to DT
+ */
+struct cpg_mssr_priv {
+	struct device *dev;
+	void __iomem *base;
+	spinlock_t mstp_lock;
+
+	struct clk **clks;
+	unsigned int num_core_clks;
+	unsigned int num_mod_clks;
+	unsigned int last_dt_core_clk;
+};
+
+
+/**
+ * struct mstp_clock - MSTP gating clock
+ * @hw: handle between common and hardware-specific interfaces
+ * @index: MSTP clock number
+ * @priv: CPG/MSSR private data
+ */
+struct mstp_clock {
+	struct clk_hw hw;
+	u32 index;
+	struct cpg_mssr_priv *priv;
+};
+
+#define to_mstp_clock(_hw) container_of(_hw, struct mstp_clock, hw)
+
+static int cpg_mstp_clock_endisable(struct clk_hw *hw, bool enable)
+{
+	struct mstp_clock *clock = to_mstp_clock(hw);
+	struct cpg_mssr_priv *priv = clock->priv;
+	unsigned int reg = clock->index / 32;
+	unsigned int bit = clock->index % 32;
+	struct device *dev = priv->dev;
+	u32 bitmask = BIT(bit);
+	unsigned long flags;
+	unsigned int i;
+	u32 value;
+
+	dev_dbg(dev, "MSTP %u%02u/%pC %s\n", reg, bit, hw->clk,
+		enable ? "ON" : "OFF");
+	spin_lock_irqsave(&priv->mstp_lock, flags);
+
+	value = clk_readl(priv->base + SMSTPCR(reg));
+	if (enable)
+		value &= ~bitmask;
+	else
+		value |= bitmask;
+	clk_writel(value, priv->base + SMSTPCR(reg));
+
+	spin_unlock_irqrestore(&priv->mstp_lock, flags);
+
+	if (!enable)
+		return 0;
+
+	for (i = 1000; i > 0; --i) {
+		if (!(clk_readl(priv->base + MSTPSR(reg)) &
+		      bitmask))
+			break;
+		cpu_relax();
+	}
+
+	if (!i) {
+		dev_err(dev, "Failed to enable SMSTP %p[%d]\n",
+			priv->base + SMSTPCR(reg), bit);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int cpg_mstp_clock_enable(struct clk_hw *hw)
+{
+	return cpg_mstp_clock_endisable(hw, true);
+}
+
+static void cpg_mstp_clock_disable(struct clk_hw *hw)
+{
+	cpg_mstp_clock_endisable(hw, false);
+}
+
+static int cpg_mstp_clock_is_enabled(struct clk_hw *hw)
+{
+	struct mstp_clock *clock = to_mstp_clock(hw);
+	struct cpg_mssr_priv *priv = clock->priv;
+	u32 value;
+
+	value = clk_readl(priv->base + MSTPSR(clock->index / 32));
+
+	return !(value & BIT(clock->index % 32));
+}
+
+static const struct clk_ops cpg_mstp_clock_ops = {
+	.enable = cpg_mstp_clock_enable,
+	.disable = cpg_mstp_clock_disable,
+	.is_enabled = cpg_mstp_clock_is_enabled,
+};
+
+static
+struct clk *cpg_mssr_clk_src_twocell_get(struct of_phandle_args *clkspec,
+					 void *data)
+{
+	unsigned int clkidx = clkspec->args[1];
+	struct cpg_mssr_priv *priv = data;
+	struct device *dev = priv->dev;
+	unsigned int idx;
+	const char *type;
+	struct clk *clk;
+
+	switch (clkspec->args[0]) {
+	case CPG_CORE:
+		type = "core";
+		if (clkidx > priv->last_dt_core_clk) {
+			dev_err(dev, "Invalid %s clock index %u\n", type,
+			       clkidx);
+			return ERR_PTR(-EINVAL);
+		}
+		clk = priv->clks[clkidx];
+		break;
+
+	case CPG_MOD:
+		type = "module";
+		idx = MOD_CLK_PACK(clkidx);
+		if (clkidx % 100 > 31 || idx >= priv->num_mod_clks) {
+			dev_err(dev, "Invalid %s clock index %u\n", type,
+				clkidx);
+			return ERR_PTR(-EINVAL);
+		}
+		clk = priv->clks[priv->num_core_clks + idx];
+		break;
+
+	default:
+		dev_err(dev, "Invalid CPG clock type %u\n", clkspec->args[0]);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (IS_ERR(clk))
+		dev_err(dev, "Cannot get %s clock %u: %ld", type, clkidx,
+		       PTR_ERR(clk));
+	else
+		dev_dbg(dev, "clock (%u, %u) is %pC at %pCr Hz\n",
+			clkspec->args[0], clkspec->args[1], clk, clk);
+	return clk;
+}
+
+static void __init cpg_mssr_register_core_clk(const struct cpg_core_clk *core,
+					      const struct cpg_mssr_info *info,
+					      struct cpg_mssr_priv *priv)
+{
+	struct clk *clk = NULL, *parent;
+	struct device *dev = priv->dev;
+	unsigned int id = core->id;
+	const char *parent_name;
+
+	WARN_DEBUG(id >= priv->num_core_clks);
+	WARN_DEBUG(PTR_ERR(priv->clks[id]) != -ENOENT);
+
+	switch (core->type) {
+	case CLK_TYPE_IN:
+		clk = of_clk_get_by_name(priv->dev->of_node, core->name);
+		break;
+
+	case CLK_TYPE_FF:
+	case CLK_TYPE_DIV6P1:
+		WARN_DEBUG(core->parent >= priv->num_core_clks);
+		parent = priv->clks[core->parent];
+		if (IS_ERR(parent)) {
+			clk = parent;
+			goto fail;
+		}
+
+		parent_name = __clk_get_name(parent);
+		if (core->type == CLK_TYPE_FF) {
+			clk = clk_register_fixed_factor(NULL, core->name,
+							parent_name, 0,
+							core->mult, core->div);
+		} else {
+			clk = cpg_div6_register(core->name, 1, &parent_name,
+						priv->base + core->offset);
+		}
+		break;
+
+	default:
+		if (info->cpg_clk_register)
+			clk = info->cpg_clk_register(dev, core, info,
+						     priv->clks, priv->base);
+		else
+			dev_err(dev, "%s has unsupported core clock type %u\n",
+				core->name, core->type);
+		break;
+	}
+
+	if (IS_ERR_OR_NULL(clk))
+		goto fail;
+
+	dev_dbg(dev, "Core clock %pC at %pCr Hz\n", clk, clk);
+	priv->clks[id] = clk;
+	return;
+
+fail:
+	dev_err(dev, "Failed to register %s clock %s: %ld\n", "core,",
+		core->name, PTR_ERR(clk));
+}
+
+static void __init cpg_mssr_register_mod_clk(const struct mssr_mod_clk *mod,
+					     const struct cpg_mssr_info *info,
+					     struct cpg_mssr_priv *priv)
+{
+	struct mstp_clock *clock = NULL;
+	struct device *dev = priv->dev;
+	unsigned int id = mod->id;
+	struct clk_init_data init;
+	struct clk *parent, *clk;
+	const char *parent_name;
+	unsigned int i;
+
+	WARN_DEBUG(id < priv->num_core_clks);
+	WARN_DEBUG(id >= priv->num_core_clks + priv->num_mod_clks);
+	WARN_DEBUG(mod->parent >= priv->num_core_clks + priv->num_mod_clks);
+	WARN_DEBUG(PTR_ERR(priv->clks[id]) != -ENOENT);
+
+	parent = priv->clks[mod->parent];
+	if (IS_ERR(parent)) {
+		clk = parent;
+		goto fail;
+	}
+
+	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
+	if (!clock) {
+		clk = ERR_PTR(-ENOMEM);
+		goto fail;
+	}
+
+	init.name = mod->name;
+	init.ops = &cpg_mstp_clock_ops;
+	init.flags = CLK_IS_BASIC | CLK_SET_RATE_PARENT;
+	for (i = 0; i < info->num_crit_mod_clks; i++)
+		if (id == info->crit_mod_clks[i]) {
+#ifdef CLK_ENABLE_HAND_OFF
+			dev_dbg(dev, "MSTP %s setting CLK_ENABLE_HAND_OFF\n",
+				mod->name);
+			init.flags |= CLK_ENABLE_HAND_OFF;
+			break;
+#else
+			dev_dbg(dev, "Ignoring MSTP %s to prevent disabling\n",
+				mod->name);
+			return;
+#endif
+		}
+
+	parent_name = __clk_get_name(parent);
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	clock->index = id - priv->num_core_clks;
+	clock->priv = priv;
+	clock->hw.init = &init;
+
+	clk = clk_register(NULL, &clock->hw);
+	if (IS_ERR(clk))
+		goto fail;
+
+	dev_dbg(dev, "Module clock %pC at %pCr Hz\n", clk, clk);
+	priv->clks[id] = clk;
+	return;
+
+fail:
+	dev_err(dev, "Failed to register %s clock %s: %ld\n", "module,",
+		mod->name, PTR_ERR(clk));
+	kfree(clock);
+}
+
+
+#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
+struct cpg_mssr_clk_domain {
+	struct generic_pm_domain genpd;
+	struct device_node *np;
+	unsigned int num_core_pm_clks;
+	unsigned int core_pm_clks[0];
+};
+
+static bool cpg_mssr_is_pm_clk(const struct of_phandle_args *clkspec,
+			       struct cpg_mssr_clk_domain *pd)
+{
+	unsigned int i;
+
+	if (clkspec->np != pd->np || clkspec->args_count != 2)
+		return false;
+
+	switch (clkspec->args[0]) {
+	case CPG_CORE:
+		for (i = 0; i < pd->num_core_pm_clks; i++)
+			if (clkspec->args[1] == pd->core_pm_clks[i])
+				return true;
+		return false;
+
+	case CPG_MOD:
+		return true;
+
+	default:
+		return false;
+	}
+}
+
+static int cpg_mssr_attach_dev(struct generic_pm_domain *genpd,
+			       struct device *dev)
+{
+	struct cpg_mssr_clk_domain *pd =
+		container_of(genpd, struct cpg_mssr_clk_domain, genpd);
+	struct device_node *np = dev->of_node;
+	struct of_phandle_args clkspec;
+	struct clk *clk;
+	int i = 0;
+	int error;
+
+	while (!of_parse_phandle_with_args(np, "clocks", "#clock-cells", i,
+					   &clkspec)) {
+		if (cpg_mssr_is_pm_clk(&clkspec, pd))
+			goto found;
+
+		of_node_put(clkspec.np);
+		i++;
+	}
+
+	return 0;
+
+found:
+	clk = of_clk_get_from_provider(&clkspec);
+	of_node_put(clkspec.np);
+
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	error = pm_clk_create(dev);
+	if (error) {
+		dev_err(dev, "pm_clk_create failed %d\n", error);
+		goto fail_put;
+	}
+
+	error = pm_clk_add_clk(dev, clk);
+	if (error) {
+		dev_err(dev, "pm_clk_add_clk %pC failed %d\n", clk, error);
+		goto fail_destroy;
+	}
+
+	return 0;
+
+fail_destroy:
+	pm_clk_destroy(dev);
+fail_put:
+	clk_put(clk);
+	return error;
+}
+
+static void cpg_mssr_detach_dev(struct generic_pm_domain *genpd,
+				struct device *dev)
+{
+	if (!list_empty(&dev->power.subsys_data->clock_list))
+		pm_clk_destroy(dev);
+}
+
+static int __init cpg_mssr_add_clk_domain(struct device *dev,
+					  const unsigned int *core_pm_clks,
+					  unsigned int num_core_pm_clks)
+{
+	struct device_node *np = dev->of_node;
+	struct generic_pm_domain *genpd;
+	struct cpg_mssr_clk_domain *pd;
+	size_t pm_size = num_core_pm_clks * sizeof(core_pm_clks[0]);
+
+	pd = devm_kzalloc(dev, sizeof(*pd) + pm_size, GFP_KERNEL);
+	if (!pd)
+		return -ENOMEM;
+
+	pd->np = np;
+	pd->num_core_pm_clks = num_core_pm_clks;
+	memcpy(pd->core_pm_clks, core_pm_clks, pm_size);
+
+	genpd = &pd->genpd;
+	genpd->name = np->name;
+	genpd->flags = GENPD_FLAG_PM_CLK;
+	pm_genpd_init(genpd, &simple_qos_governor, false);
+	genpd->attach_dev = cpg_mssr_attach_dev;
+	genpd->detach_dev = cpg_mssr_detach_dev;
+
+	of_genpd_add_provider_simple(np, genpd);
+	return 0;
+}
+#else
+static inline int cpg_mssr_add_clk_domain(struct device *dev,
+					  const unsigned int *core_pm_clks,
+					  unsigned int num_core_pm_clks)
+{
+	return 0;
+}
+#endif /* !CONFIG_PM_GENERIC_DOMAINS_OF */
+
+
+static const struct of_device_id cpg_mssr_match[] = {
+#ifdef CONFIG_ARCH_R8A7795
+	{
+		.compatible = "renesas,r8a7795-cpg-mssr",
+		.data = &r8a7795_cpg_mssr_info,
+	},
+#endif
+	{ /* sentinel */ }
+};
+
+static void cpg_mssr_del_clk_provider(void *data)
+{
+	of_clk_del_provider(data);
+}
+
+static int __init cpg_mssr_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	const struct cpg_mssr_info *info;
+	struct cpg_mssr_priv *priv;
+	unsigned int nclks, i;
+	struct resource *res;
+	struct clk **clks;
+	int error;
+
+	info = of_match_node(cpg_mssr_match, np)->data;
+	if (info->init) {
+		error = info->init(dev);
+		if (error)
+			return error;
+	}
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = dev;
+	spin_lock_init(&priv->mstp_lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	nclks = info->num_total_core_clks + info->num_hw_mod_clks;
+	clks = devm_kmalloc_array(dev, nclks, sizeof(*clks), GFP_KERNEL);
+	if (!clks)
+		return -ENOMEM;
+
+	priv->clks = clks;
+	priv->num_core_clks = info->num_total_core_clks;
+	priv->num_mod_clks = info->num_hw_mod_clks;
+	priv->last_dt_core_clk = info->last_dt_core_clk;
+
+	for (i = 0; i < nclks; i++)
+		clks[i] = ERR_PTR(-ENOENT);
+
+	for (i = 0; i < info->num_core_clks; i++)
+		cpg_mssr_register_core_clk(&info->core_clks[i], info, priv);
+
+	for (i = 0; i < info->num_mod_clks; i++)
+		cpg_mssr_register_mod_clk(&info->mod_clks[i], info, priv);
+
+	error = of_clk_add_provider(np, cpg_mssr_clk_src_twocell_get, priv);
+	if (error)
+		return error;
+
+	devm_add_action(dev, cpg_mssr_del_clk_provider, np);
+
+	error = cpg_mssr_add_clk_domain(dev, info->core_pm_clks,
+					info->num_core_pm_clks);
+	if (error)
+		return error;
+
+	return 0;
+}
+
+static struct platform_driver cpg_mssr_driver = {
+	.driver		= {
+		.name	= "renesas-cpg-mssr",
+		.of_match_table = cpg_mssr_match,
+	},
+};
+
+static int __init cpg_mssr_init(void)
+{
+	return platform_driver_probe(&cpg_mssr_driver, cpg_mssr_probe);
+}
+
+subsys_initcall(cpg_mssr_init);
+
+MODULE_DESCRIPTION("Renesas CPG/MSSR Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/clk/shmobile/renesas-cpg-mssr.h b/drivers/clk/shmobile/renesas-cpg-mssr.h
new file mode 100644
index 000000000000..e09f03cbf086
--- /dev/null
+++ b/drivers/clk/shmobile/renesas-cpg-mssr.h
@@ -0,0 +1,132 @@
+/*
+ * Renesas Clock Pulse Generator / Module Standby and Software Reset
+ *
+ * Copyright (C) 2015 Glider bvba
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#ifndef __CLK_RENESAS_CPG_MSSR_H__
+#define __CLK_RENESAS_CPG_MSSR_H__
+
+    /*
+     * Definitions of CPG Core Clocks
+     *
+     * These include:
+     *   - Clock outputs exported to DT
+     *   - External input clocks
+     *   - Internal CPG clocks
+     */
+
+struct cpg_core_clk {
+	/* Common */
+	const char *name;
+	unsigned int id;
+	unsigned int type;
+	/* Depending on type */
+	unsigned int parent;	/* Core Clocks only */
+	unsigned int div;
+	unsigned int mult;
+	unsigned int offset;
+};
+
+enum clk_types {
+	/* Generic */
+	CLK_TYPE_IN,		/* External Clock Input */
+	CLK_TYPE_FF,		/* Fixed Factor Clock */
+	CLK_TYPE_DIV6P1,	/* DIV6 Clock with 1 parent clock */
+
+	/* Custom definitions start here */
+	CLK_TYPE_CUSTOM,
+};
+
+#define DEF_TYPE(_name, _id, _type...)	\
+	{ .name = _name, .id = _id, .type = _type }
+#define DEF_BASE(_name, _id, _type, _parent...)	\
+	DEF_TYPE(_name, _id, _type, .parent = _parent)
+
+#define DEF_INPUT(_name, _id) \
+	DEF_TYPE(_name, _id, CLK_TYPE_IN)
+#define DEF_FIXED(_name, _id, _parent, _div, _mult)	\
+	DEF_BASE(_name, _id, CLK_TYPE_FF, _parent, .div = _div, .mult = _mult)
+#define DEF_DIV6P1(_name, _id, _parent, _offset)	\
+	DEF_BASE(_name, _id, CLK_TYPE_DIV6P1, _parent, .offset = _offset)
+
+
+    /*
+     * Definitions of Module Clocks
+     */
+
+struct mssr_mod_clk {
+	const char *name;
+	unsigned int id;
+	unsigned int parent;	/* Add MOD_CLK_BASE for Module Clocks */
+};
+
+/* Convert from sparse base-100 to packed index space */
+#define MOD_CLK_PACK(x)	((x) - ((x) / 100) * (100 - 32))
+
+#define MOD_CLK_ID(x)	(MOD_CLK_BASE + MOD_CLK_PACK(x))
+
+#define DEF_MOD(_name, _mod, _parent...)	\
+	{ .name = _name, .id = MOD_CLK_ID(_mod), .parent = _parent }
+
+
+struct device_node;
+
+    /**
+     * SoC-specific CPG/MSSR Description
+     *
+     * @core_clks: Array of Core Clock definitions
+     * @num_core_clks: Number of entries in core_clks[]
+     * @last_dt_core_clk: ID of the last Core Clock exported to DT
+     * @num_total_core_clks: Total number of Core Clocks (exported + internal)
+     *
+     * @mod_clks: Array of Module Clock definitions
+     * @num_mod_clks: Number of entries in mod_clks[]
+     * @num_hw_mod_clks: Number of Module Clocks supported by the hardware
+     *
+     * @crit_mod_clks: Array with Module Clock IDs of critical clocks that
+     *                 should not be disabled without a knowledgeable driver
+     * @num_crit_mod_clks: Number of entries in crit_mod_clks[]
+     *
+     * @core_pm_clks: Array with IDs of Core Clocks that are suitable for Power
+     *                Management, in addition to Module Clocks
+     * @num_core_pm_clks: Number of entries in core_pm_clks[]
+     *
+     * @init: Optional callback to perform SoC-specific initialization
+     * @cpg_clk_register: Optional callback to handle special Core Clock types
+     */
+
+struct cpg_mssr_info {
+	/* Core Clocks */
+	const struct cpg_core_clk *core_clks;
+	unsigned int num_core_clks;
+	unsigned int last_dt_core_clk;
+	unsigned int num_total_core_clks;
+
+	/* Module Clocks */
+	const struct mssr_mod_clk *mod_clks;
+	unsigned int num_mod_clks;
+	unsigned int num_hw_mod_clks;
+
+	/* Critical Module Clocks that should not be disabled */
+	const unsigned int *crit_mod_clks;
+	unsigned int num_crit_mod_clks;
+
+	/* Core Clocks suitable for PM, in addition to the Module Clocks */
+	const unsigned int *core_pm_clks;
+	unsigned int num_core_pm_clks;
+
+	/* Callbacks */
+	int (*init)(struct device *dev);
+	struct clk *(*cpg_clk_register)(struct device *dev,
+					const struct cpg_core_clk *core,
+					const struct cpg_mssr_info *info,
+					struct clk **clks, void __iomem *base);
+};
+
+extern const struct cpg_mssr_info r8a7795_cpg_mssr_info;
+#endif
diff --git a/drivers/clk/st/clkgen-fsyn.c b/drivers/clk/st/clkgen-fsyn.c
index 576cd0354d48..ccb324d97160 100644
--- a/drivers/clk/st/clkgen-fsyn.c
+++ b/drivers/clk/st/clkgen-fsyn.c
@@ -549,19 +549,20 @@ static int clk_fs660c32_vco_get_params(unsigned long input,
 	return 0;
 }
 
-static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw, unsigned long rate
-		, unsigned long *prate)
+static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw,
+					   unsigned long rate,
+					   unsigned long *prate)
 {
 	struct stm_fs params;
 
-	if (!clk_fs660c32_vco_get_params(*prate, rate, &params))
-		clk_fs660c32_vco_get_rate(*prate, &params, &rate);
+	if (clk_fs660c32_vco_get_params(*prate, rate, &params))
+		return rate;
 
-	pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
+	clk_fs660c32_vco_get_rate(*prate, &params, &rate);
+
+	pr_debug("%s: %s new rate %ld [ndiv=%u]\n",
 		 __func__, clk_hw_get_name(hw),
-		 rate, (unsigned int)params.sdiv,
-		 (unsigned int)params.mdiv,
-		 (unsigned int)params.pe, (unsigned int)params.nsdiv);
+		 rate, (unsigned int)params.ndiv);
 
 	return rate;
 }
diff --git a/drivers/clk/sunxi/Makefile b/drivers/clk/sunxi/Makefile
index cb4c299214ce..3fd7901d48e4 100644
--- a/drivers/clk/sunxi/Makefile
+++ b/drivers/clk/sunxi/Makefile
@@ -7,14 +7,19 @@ obj-y += clk-a10-codec.o
 obj-y += clk-a10-hosc.o
 obj-y += clk-a10-mod1.o
 obj-y += clk-a10-pll2.o
+obj-y += clk-a10-ve.o
 obj-y += clk-a20-gmac.o
 obj-y += clk-mod0.o
 obj-y += clk-simple-gates.o
+obj-y += clk-sun8i-bus-gates.o
 obj-y += clk-sun8i-mbus.o
 obj-y += clk-sun9i-core.o
 obj-y += clk-sun9i-mmc.o
 obj-y += clk-usb.o
 
+obj-$(CONFIG_MACH_SUN9I) += clk-sun8i-apb0.o
+obj-$(CONFIG_MACH_SUN9I) += clk-sun9i-cpus.o
+
 obj-$(CONFIG_MFD_SUN6I_PRCM) += \
 	clk-sun6i-ar100.o clk-sun6i-apb0.o clk-sun6i-apb0-gates.o \
 	clk-sun8i-apb0.o
diff --git a/drivers/clk/sunxi/clk-a10-ve.c b/drivers/clk/sunxi/clk-a10-ve.c
new file mode 100644
index 000000000000..044c1717b762
--- /dev/null
+++ b/drivers/clk/sunxi/clk-a10-ve.c
@@ -0,0 +1,171 @@
+/*
+ * Copyright 2015 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/reset-controller.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+static DEFINE_SPINLOCK(ve_lock);
+
+#define SUN4I_VE_ENABLE		31
+#define SUN4I_VE_DIVIDER_SHIFT	16
+#define SUN4I_VE_DIVIDER_WIDTH	3
+#define SUN4I_VE_RESET		0
+
+/**
+ * sunxi_ve_reset... - reset bit in ve clk registers handling
+ */
+
+struct ve_reset_data {
+	void __iomem			*reg;
+	spinlock_t			*lock;
+	struct reset_controller_dev	rcdev;
+};
+
+static int sunxi_ve_reset_assert(struct reset_controller_dev *rcdev,
+				 unsigned long id)
+{
+	struct ve_reset_data *data = container_of(rcdev,
+						  struct ve_reset_data,
+						  rcdev);
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(data->lock, flags);
+
+	reg = readl(data->reg);
+	writel(reg & ~BIT(SUN4I_VE_RESET), data->reg);
+
+	spin_unlock_irqrestore(data->lock, flags);
+
+	return 0;
+}
+
+static int sunxi_ve_reset_deassert(struct reset_controller_dev *rcdev,
+				   unsigned long id)
+{
+	struct ve_reset_data *data = container_of(rcdev,
+						  struct ve_reset_data,
+						  rcdev);
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(data->lock, flags);
+
+	reg = readl(data->reg);
+	writel(reg | BIT(SUN4I_VE_RESET), data->reg);
+
+	spin_unlock_irqrestore(data->lock, flags);
+
+	return 0;
+}
+
+static int sunxi_ve_of_xlate(struct reset_controller_dev *rcdev,
+			     const struct of_phandle_args *reset_spec)
+{
+	if (WARN_ON(reset_spec->args_count != 0))
+		return -EINVAL;
+
+	return 0;
+}
+
+static struct reset_control_ops sunxi_ve_reset_ops = {
+	.assert		= sunxi_ve_reset_assert,
+	.deassert	= sunxi_ve_reset_deassert,
+};
+
+static void __init sun4i_ve_clk_setup(struct device_node *node)
+{
+	struct clk *clk;
+	struct clk_divider *div;
+	struct clk_gate *gate;
+	struct ve_reset_data *reset_data;
+	const char *parent;
+	const char *clk_name = node->name;
+	void __iomem *reg;
+	int err;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg))
+		return;
+
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		goto err_unmap;
+
+	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+	if (!gate)
+		goto err_free_div;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+	parent = of_clk_get_parent_name(node, 0);
+
+	gate->reg = reg;
+	gate->bit_idx = SUN4I_VE_ENABLE;
+	gate->lock = &ve_lock;
+
+	div->reg = reg;
+	div->shift = SUN4I_VE_DIVIDER_SHIFT;
+	div->width = SUN4I_VE_DIVIDER_WIDTH;
+	div->lock = &ve_lock;
+
+	clk = clk_register_composite(NULL, clk_name, &parent, 1,
+				     NULL, NULL,
+				     &div->hw, &clk_divider_ops,
+				     &gate->hw, &clk_gate_ops,
+				     CLK_SET_RATE_PARENT);
+	if (IS_ERR(clk))
+		goto err_free_gate;
+
+	err = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (err)
+		goto err_unregister_clk;
+
+	reset_data = kzalloc(sizeof(*reset_data), GFP_KERNEL);
+	if (!reset_data)
+		goto err_del_provider;
+
+	reset_data->reg = reg;
+	reset_data->lock = &ve_lock;
+	reset_data->rcdev.nr_resets = 1;
+	reset_data->rcdev.ops = &sunxi_ve_reset_ops;
+	reset_data->rcdev.of_node = node;
+	reset_data->rcdev.of_xlate = sunxi_ve_of_xlate;
+	reset_data->rcdev.of_reset_n_cells = 0;
+	err = reset_controller_register(&reset_data->rcdev);
+	if (err)
+		goto err_free_reset;
+
+	return;
+
+err_free_reset:
+	kfree(reset_data);
+err_del_provider:
+	of_clk_del_provider(node);
+err_unregister_clk:
+	clk_unregister(clk);
+err_free_gate:
+	kfree(gate);
+err_free_div:
+	kfree(div);
+err_unmap:
+	iounmap(reg);
+}
+CLK_OF_DECLARE(sun4i_ve, "allwinner,sun4i-a10-ve-clk",
+	       sun4i_ve_clk_setup);
diff --git a/drivers/clk/sunxi/clk-simple-gates.c b/drivers/clk/sunxi/clk-simple-gates.c
index 0214c6548afd..f4da52b5ca0e 100644
--- a/drivers/clk/sunxi/clk-simple-gates.c
+++ b/drivers/clk/sunxi/clk-simple-gates.c
@@ -140,6 +140,8 @@ CLK_OF_DECLARE(sun9i_a80_apb0, "allwinner,sun9i-a80-apb0-gates-clk",
 	       sunxi_simple_gates_init);
 CLK_OF_DECLARE(sun9i_a80_apb1, "allwinner,sun9i-a80-apb1-gates-clk",
 	       sunxi_simple_gates_init);
+CLK_OF_DECLARE(sun9i_a80_apbs, "allwinner,sun9i-a80-apbs-gates-clk",
+	       sunxi_simple_gates_init);
 
 static const int sun4i_a10_ahb_critical_clocks[] __initconst = {
 	14,	/* ahb_sdram */
@@ -158,3 +160,15 @@ CLK_OF_DECLARE(sun5i_a13_ahb, "allwinner,sun5i-a13-ahb-gates-clk",
 	       sun4i_a10_ahb_init);
 CLK_OF_DECLARE(sun7i_a20_ahb, "allwinner,sun7i-a20-ahb-gates-clk",
 	       sun4i_a10_ahb_init);
+
+static const int sun4i_a10_dram_critical_clocks[] __initconst = {
+	15,	/* dram_output */
+};
+
+static void __init sun4i_a10_dram_init(struct device_node *node)
+{
+	sunxi_simple_gates_setup(node, sun4i_a10_dram_critical_clocks,
+				 ARRAY_SIZE(sun4i_a10_dram_critical_clocks));
+}
+CLK_OF_DECLARE(sun4i_a10_dram, "allwinner,sun4i-a10-dram-gates-clk",
+	       sun4i_a10_dram_init);
diff --git a/drivers/clk/sunxi/clk-sun8i-apb0.c b/drivers/clk/sunxi/clk-sun8i-apb0.c
index 7ae5d2c2cde1..7ba61103a6f5 100644
--- a/drivers/clk/sunxi/clk-sun8i-apb0.c
+++ b/drivers/clk/sunxi/clk-sun8i-apb0.c
@@ -17,13 +17,77 @@
 #include <linux/clk-provider.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/platform_device.h>
 
+static struct clk *sun8i_a23_apb0_register(struct device_node *node,
+					   void __iomem *reg)
+{
+	const char *clk_name = node->name;
+	const char *clk_parent;
+	struct clk *clk;
+	int ret;
+
+	clk_parent = of_clk_get_parent_name(node, 0);
+	if (!clk_parent)
+		return ERR_PTR(-EINVAL);
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	/* The A23 APB0 clock is a standard 2 bit wide divider clock */
+	clk = clk_register_divider(NULL, clk_name, clk_parent, 0, reg,
+				   0, 2, CLK_DIVIDER_POWER_OF_TWO, NULL);
+	if (IS_ERR(clk))
+		return clk;
+
+	ret = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (ret)
+		goto err_unregister;
+
+	return clk;
+
+err_unregister:
+	clk_unregister_divider(clk);
+
+	return ERR_PTR(ret);
+}
+
+static void sun8i_a23_apb0_setup(struct device_node *node)
+{
+	void __iomem *reg;
+	struct resource res;
+	struct clk *clk;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg)) {
+		/*
+		 * This happens with clk nodes instantiated through mfd,
+		 * as those do not have their resources assigned in the
+		 * device tree. Do not print an error in this case.
+		 */
+		if (PTR_ERR(reg) != -EINVAL)
+			pr_err("Could not get registers for a23-apb0-clk\n");
+
+		return;
+	}
+
+	clk = sun8i_a23_apb0_register(node, reg);
+	if (IS_ERR(clk))
+		goto err_unmap;
+
+	return;
+
+err_unmap:
+	iounmap(reg);
+	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);
+
 static int sun8i_a23_apb0_clk_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	const char *clk_name = np->name;
-	const char *clk_parent;
 	struct resource *r;
 	void __iomem *reg;
 	struct clk *clk;
@@ -33,19 +97,11 @@ static int sun8i_a23_apb0_clk_probe(struct platform_device *pdev)
 	if (IS_ERR(reg))
 		return PTR_ERR(reg);
 
-	clk_parent = of_clk_get_parent_name(np, 0);
-	if (!clk_parent)
-		return -EINVAL;
-
-	of_property_read_string(np, "clock-output-names", &clk_name);
-
-	/* The A23 APB0 clock is a standard 2 bit wide divider clock */
-	clk = clk_register_divider(&pdev->dev, clk_name, clk_parent, 0, reg,
-				   0, 2, CLK_DIVIDER_POWER_OF_TWO, NULL);
+	clk = sun8i_a23_apb0_register(np, reg);
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
-	return of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	return 0;
 }
 
 static const struct of_device_id sun8i_a23_apb0_clk_dt_ids[] = {
diff --git a/drivers/clk/sunxi/clk-sun8i-bus-gates.c b/drivers/clk/sunxi/clk-sun8i-bus-gates.c
new file mode 100644
index 000000000000..e32d18ba252b
--- /dev/null
+++ b/drivers/clk/sunxi/clk-sun8i-bus-gates.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (C) 2015 Jens Kuske <jenskuske@gmail.com>
+ *
+ * Based on clk-simple-gates.c, which is:
+ * Copyright 2015 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.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+static DEFINE_SPINLOCK(gates_lock);
+
+static void __init sun8i_h3_bus_gates_init(struct device_node *node)
+{
+	static const char * const names[] = { "ahb1", "ahb2", "apb1", "apb2" };
+	enum { AHB1, AHB2, APB1, APB2, PARENT_MAX } clk_parent;
+	const char *parents[PARENT_MAX];
+	struct clk_onecell_data *clk_data;
+	const char *clk_name;
+	struct property *prop;
+	struct resource res;
+	void __iomem *clk_reg;
+	void __iomem *reg;
+	const __be32 *p;
+	int number, i;
+	u8 clk_bit;
+	int index;
+
+	reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(reg))
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(names); i++) {
+		int idx = of_property_match_string(node, "clock-names",
+						   names[i]);
+		if (idx < 0)
+			return;
+
+		parents[i] = of_clk_get_parent_name(node, idx);
+	}
+
+	clk_data = kmalloc(sizeof(struct clk_onecell_data), GFP_KERNEL);
+	if (!clk_data)
+		goto err_unmap;
+
+	number = of_property_count_u32_elems(node, "clock-indices");
+	of_property_read_u32_index(node, "clock-indices", number - 1, &number);
+
+	clk_data->clks = kcalloc(number + 1, sizeof(struct clk *), GFP_KERNEL);
+	if (!clk_data->clks)
+		goto err_free_data;
+
+	i = 0;
+	of_property_for_each_u32(node, "clock-indices", prop, p, index) {
+		of_property_read_string_index(node, "clock-output-names",
+					      i, &clk_name);
+
+		if (index == 17 || (index >= 29 && index <= 31))
+			clk_parent = AHB2;
+		else if (index <= 63 || index >= 128)
+			clk_parent = AHB1;
+		else if (index >= 64 && index <= 95)
+			clk_parent = APB1;
+		else if (index >= 96 && index <= 127)
+			clk_parent = APB2;
+
+		clk_reg = reg + 4 * (index / 32);
+		clk_bit = index % 32;
+
+		clk_data->clks[index] = clk_register_gate(NULL, clk_name,
+							  parents[clk_parent],
+							  0, clk_reg, clk_bit,
+							  0, &gates_lock);
+		i++;
+
+		if (IS_ERR(clk_data->clks[index])) {
+			WARN_ON(true);
+			continue;
+		}
+	}
+
+	clk_data->clk_num = number + 1;
+	of_clk_add_provider(node, of_clk_src_onecell_get, clk_data);
+
+	return;
+
+err_free_data:
+	kfree(clk_data);
+err_unmap:
+	iounmap(reg);
+	of_address_to_resource(node, 0, &res);
+	release_mem_region(res.start, resource_size(&res));
+}
+
+CLK_OF_DECLARE(sun8i_h3_bus_gates, "allwinner,sun8i-h3-bus-gates-clk",
+	       sun8i_h3_bus_gates_init);
diff --git a/drivers/clk/sunxi/clk-sun9i-cpus.c b/drivers/clk/sunxi/clk-sun9i-cpus.c
new file mode 100644
index 000000000000..7626d2194b96
--- /dev/null
+++ b/drivers/clk/sunxi/clk-sun9i-cpus.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (C) 2015 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * Allwinner A80 CPUS clock driver
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+
+static DEFINE_SPINLOCK(sun9i_a80_cpus_lock);
+
+/**
+ * sun9i_a80_cpus_clk_setup() - Setup function for a80 cpus composite clk
+ */
+
+#define SUN9I_CPUS_MAX_PARENTS		4
+#define SUN9I_CPUS_MUX_PARENT_PLL4	3
+#define SUN9I_CPUS_MUX_SHIFT		16
+#define SUN9I_CPUS_MUX_MASK		GENMASK(17, 16)
+#define SUN9I_CPUS_MUX_GET_PARENT(reg)	((reg & SUN9I_CPUS_MUX_MASK) >> \
+						SUN9I_CPUS_MUX_SHIFT)
+
+#define SUN9I_CPUS_DIV_SHIFT		4
+#define SUN9I_CPUS_DIV_MASK		GENMASK(5, 4)
+#define SUN9I_CPUS_DIV_GET(reg)		((reg & SUN9I_CPUS_DIV_MASK) >> \
+						SUN9I_CPUS_DIV_SHIFT)
+#define SUN9I_CPUS_DIV_SET(reg, div)	((reg & ~SUN9I_CPUS_DIV_MASK) | \
+						(div << SUN9I_CPUS_DIV_SHIFT))
+#define SUN9I_CPUS_PLL4_DIV_SHIFT	8
+#define SUN9I_CPUS_PLL4_DIV_MASK	GENMASK(12, 8)
+#define SUN9I_CPUS_PLL4_DIV_GET(reg)	((reg & SUN9I_CPUS_PLL4_DIV_MASK) >> \
+						SUN9I_CPUS_PLL4_DIV_SHIFT)
+#define SUN9I_CPUS_PLL4_DIV_SET(reg, div) ((reg & ~SUN9I_CPUS_PLL4_DIV_MASK) | \
+						(div << SUN9I_CPUS_PLL4_DIV_SHIFT))
+
+struct sun9i_a80_cpus_clk {
+	struct clk_hw hw;
+	void __iomem *reg;
+};
+
+#define to_sun9i_a80_cpus_clk(_hw) container_of(_hw, struct sun9i_a80_cpus_clk, hw)
+
+static unsigned long sun9i_a80_cpus_clk_recalc_rate(struct clk_hw *hw,
+						    unsigned long parent_rate)
+{
+	struct sun9i_a80_cpus_clk *cpus = to_sun9i_a80_cpus_clk(hw);
+	unsigned long rate;
+	u32 reg;
+
+	/* Fetch the register value */
+	reg = readl(cpus->reg);
+
+	/* apply pre-divider first if parent is pll4 */
+	if (SUN9I_CPUS_MUX_GET_PARENT(reg) == SUN9I_CPUS_MUX_PARENT_PLL4)
+		parent_rate /= SUN9I_CPUS_PLL4_DIV_GET(reg) + 1;
+
+	/* clk divider */
+	rate = parent_rate / (SUN9I_CPUS_DIV_GET(reg) + 1);
+
+	return rate;
+}
+
+static long sun9i_a80_cpus_clk_round(unsigned long rate, u8 *divp, u8 *pre_divp,
+				     u8 parent, unsigned long parent_rate)
+{
+	u8 div, pre_div = 1;
+
+	/*
+	 * clock can only divide, so we will never be able to achieve
+	 * frequencies higher than the parent frequency
+	 */
+	if (parent_rate && rate > parent_rate)
+		rate = parent_rate;
+
+	div = DIV_ROUND_UP(parent_rate, rate);
+
+	/* calculate pre-divider if parent is pll4 */
+	if (parent == SUN9I_CPUS_MUX_PARENT_PLL4 && div > 4) {
+		/* pre-divider is 1 ~ 32 */
+		if (div < 32) {
+			pre_div = div;
+			div = 1;
+		} else if (div < 64) {
+			pre_div = DIV_ROUND_UP(div, 2);
+			div = 2;
+		} else if (div < 96) {
+			pre_div = DIV_ROUND_UP(div, 3);
+			div = 3;
+		} else {
+			pre_div = DIV_ROUND_UP(div, 4);
+			div = 4;
+		}
+	}
+
+	/* we were asked to pass back divider values */
+	if (divp) {
+		*divp = div - 1;
+		*pre_divp = pre_div - 1;
+	}
+
+	return parent_rate / pre_div / div;
+}
+
+static int sun9i_a80_cpus_clk_determine_rate(struct clk_hw *clk,
+					     struct clk_rate_request *req)
+{
+	struct clk_hw *parent, *best_parent = NULL;
+	int i, num_parents;
+	unsigned long parent_rate, best = 0, child_rate, best_child_rate = 0;
+	unsigned long rate = req->rate;
+
+	/* find the parent that can help provide the fastest rate <= rate */
+	num_parents = clk_hw_get_num_parents(clk);
+	for (i = 0; i < num_parents; i++) {
+		parent = clk_hw_get_parent_by_index(clk, i);
+		if (!parent)
+			continue;
+		if (clk_hw_get_flags(clk) & CLK_SET_RATE_PARENT)
+			parent_rate = clk_hw_round_rate(parent, rate);
+		else
+			parent_rate = clk_hw_get_rate(parent);
+
+		child_rate = sun9i_a80_cpus_clk_round(rate, NULL, NULL, i,
+						      parent_rate);
+
+		if (child_rate <= rate && child_rate > best_child_rate) {
+			best_parent = parent;
+			best = parent_rate;
+			best_child_rate = child_rate;
+		}
+	}
+
+	if (!best_parent)
+		return -EINVAL;
+
+	req->best_parent_hw = best_parent;
+	req->best_parent_rate = best;
+	req->rate = best_child_rate;
+
+	return 0;
+}
+
+static int sun9i_a80_cpus_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+				       unsigned long parent_rate)
+{
+	struct sun9i_a80_cpus_clk *cpus = to_sun9i_a80_cpus_clk(hw);
+	unsigned long flags;
+	u8 div, pre_div, parent;
+	u32 reg;
+
+	spin_lock_irqsave(&sun9i_a80_cpus_lock, flags);
+
+	reg = readl(cpus->reg);
+
+	/* need to know which parent is used to apply pre-divider */
+	parent = SUN9I_CPUS_MUX_GET_PARENT(reg);
+	sun9i_a80_cpus_clk_round(rate, &div, &pre_div, parent, parent_rate);
+
+	reg = SUN9I_CPUS_DIV_SET(reg, div);
+	reg = SUN9I_CPUS_PLL4_DIV_SET(reg, pre_div);
+	writel(reg, cpus->reg);
+
+	spin_unlock_irqrestore(&sun9i_a80_cpus_lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops sun9i_a80_cpus_clk_ops = {
+	.determine_rate	= sun9i_a80_cpus_clk_determine_rate,
+	.recalc_rate	= sun9i_a80_cpus_clk_recalc_rate,
+	.set_rate	= sun9i_a80_cpus_clk_set_rate,
+};
+
+static void sun9i_a80_cpus_setup(struct device_node *node)
+{
+	const char *clk_name = node->name;
+	const char *parents[SUN9I_CPUS_MAX_PARENTS];
+	struct resource res;
+	struct sun9i_a80_cpus_clk *cpus;
+	struct clk_mux *mux;
+	struct clk *clk;
+	int ret;
+
+	cpus = kzalloc(sizeof(*cpus), GFP_KERNEL);
+	if (!cpus)
+		return;
+
+	cpus->reg = of_io_request_and_map(node, 0, of_node_full_name(node));
+	if (IS_ERR(cpus->reg))
+		goto err_free_cpus;
+
+	of_property_read_string(node, "clock-output-names", &clk_name);
+
+	/* we have a mux, we will have >1 parents */
+	ret = of_clk_parent_fill(node, parents, SUN9I_CPUS_MAX_PARENTS);
+
+	mux = kzalloc(sizeof(*mux), GFP_KERNEL);
+	if (!mux)
+		goto err_unmap;
+
+	/* set up clock properties */
+	mux->reg = cpus->reg;
+	mux->shift = SUN9I_CPUS_MUX_SHIFT;
+	/* un-shifted mask is what mux_clk expects */
+	mux->mask = SUN9I_CPUS_MUX_MASK >> SUN9I_CPUS_MUX_SHIFT;
+	mux->lock = &sun9i_a80_cpus_lock;
+
+	clk = clk_register_composite(NULL, clk_name, parents, ret,
+				     &mux->hw, &clk_mux_ops,
+				     &cpus->hw, &sun9i_a80_cpus_clk_ops,
+				     NULL, NULL, 0);
+	if (IS_ERR(clk))
+		goto err_free_mux;
+
+	ret = of_clk_add_provider(node, of_clk_src_simple_get, clk);
+	if (ret)
+		goto err_unregister;
+
+	return;
+
+err_unregister:
+	clk_unregister(clk);
+err_free_mux:
+	kfree(mux);
+err_unmap:
+	iounmap(cpus->reg);
+	of_address_to_resource(node, 0, &res);
+	release_mem_region(res.start, resource_size(&res));
+err_free_cpus:
+	kfree(cpus);
+}
+CLK_OF_DECLARE(sun9i_a80_cpus, "allwinner,sun9i-a80-cpus-clk",
+	       sun9i_a80_cpus_setup);
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
index 9c79af0c03b2..5ba2188ee99c 100644
--- a/drivers/clk/sunxi/clk-sunxi.c
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -778,6 +778,10 @@ static const struct mux_data sun6i_a31_ahb1_mux_data __initconst = {
 	.shift = 12,
 };
 
+static const struct mux_data sun8i_h3_ahb2_mux_data __initconst = {
+	.shift = 0,
+};
+
 static void __init sunxi_mux_clk_setup(struct device_node *node,
 				       struct mux_data *data)
 {
@@ -1130,6 +1134,7 @@ static const struct of_device_id clk_divs_match[] __initconst = {
 static const struct of_device_id clk_mux_match[] __initconst = {
 	{.compatible = "allwinner,sun4i-a10-cpu-clk", .data = &sun4i_cpu_mux_data,},
 	{.compatible = "allwinner,sun6i-a31-ahb1-mux-clk", .data = &sun6i_a31_ahb1_mux_data,},
+	{.compatible = "allwinner,sun8i-h3-ahb2-clk", .data = &sun8i_h3_ahb2_mux_data,},
 	{}
 };
 
@@ -1212,6 +1217,7 @@ CLK_OF_DECLARE(sun6i_a31_clk_init, "allwinner,sun6i-a31", sun6i_init_clocks);
 CLK_OF_DECLARE(sun6i_a31s_clk_init, "allwinner,sun6i-a31s", sun6i_init_clocks);
 CLK_OF_DECLARE(sun8i_a23_clk_init, "allwinner,sun8i-a23", sun6i_init_clocks);
 CLK_OF_DECLARE(sun8i_a33_clk_init, "allwinner,sun8i-a33", sun6i_init_clocks);
+CLK_OF_DECLARE(sun8i_h3_clk_init, "allwinner,sun8i-h3", sun6i_init_clocks);
 
 static void __init sun9i_init_clocks(struct device_node *node)
 {
diff --git a/drivers/clk/sunxi/clk-usb.c b/drivers/clk/sunxi/clk-usb.c
index 1a72cd672839..67b8e38f4ee9 100644
--- a/drivers/clk/sunxi/clk-usb.c
+++ b/drivers/clk/sunxi/clk-usb.c
@@ -243,3 +243,15 @@ static void __init sun9i_a80_usb_phy_setup(struct device_node *node)
 	sunxi_usb_clk_setup(node, &sun9i_a80_usb_phy_data, &a80_usb_phy_lock);
 }
 CLK_OF_DECLARE(sun9i_a80_usb_phy, "allwinner,sun9i-a80-usb-phy-clk", sun9i_a80_usb_phy_setup);
+
+static const struct usb_clk_data sun8i_h3_usb_clk_data __initconst = {
+	.clk_mask =  BIT(19) | BIT(18) | BIT(17) | BIT(16) |
+		     BIT(11) | BIT(10) | BIT(9) | BIT(8),
+	.reset_mask = BIT(3) | BIT(2) | BIT(1) | BIT(0),
+};
+
+static void __init sun8i_h3_usb_setup(struct device_node *node)
+{
+	sunxi_usb_clk_setup(node, &sun8i_h3_usb_clk_data, &sun4i_a10_usb_lock);
+}
+CLK_OF_DECLARE(sun8i_h3_usb, "allwinner,sun8i-h3-usb-clk", sun8i_h3_usb_setup);
diff --git a/drivers/clk/tegra/Makefile b/drivers/clk/tegra/Makefile
index 826c325dc2e8..97984c503bbb 100644
--- a/drivers/clk/tegra/Makefile
+++ b/drivers/clk/tegra/Makefile
@@ -20,3 +20,4 @@ obj-$(CONFIG_ARCH_TEGRA_124_SOC)	+= clk-tegra124.o
 obj-$(CONFIG_ARCH_TEGRA_124_SOC)	+= clk-tegra124-dfll-fcpu.o
 obj-$(CONFIG_ARCH_TEGRA_132_SOC)	+= clk-tegra124.o
 obj-y					+= cvb.o
+obj-$(CONFIG_ARCH_TEGRA_210_SOC)	+= clk-tegra210.o
diff --git a/drivers/clk/tegra/clk-divider.c b/drivers/clk/tegra/clk-divider.c
index 48c83efda4cf..16e0aee14773 100644
--- a/drivers/clk/tegra/clk-divider.c
+++ b/drivers/clk/tegra/clk-divider.c
@@ -32,7 +32,7 @@
 static int get_div(struct tegra_clk_frac_div *divider, unsigned long rate,
 		   unsigned long parent_rate)
 {
-	s64 divider_ux1 = parent_rate;
+	u64 divider_ux1 = parent_rate;
 	u8 flags = divider->flags;
 	int mul;
 
@@ -54,7 +54,7 @@ static int get_div(struct tegra_clk_frac_div *divider, unsigned long rate,
 
 	divider_ux1 -= mul;
 
-	if (divider_ux1 < 0)
+	if ((s64)divider_ux1 < 0)
 		return 0;
 
 	if (divider_ux1 > get_max_div(divider))
diff --git a/drivers/clk/tegra/clk-emc.c b/drivers/clk/tegra/clk-emc.c
index e1fe8f35d45c..74e7544f861b 100644
--- a/drivers/clk/tegra/clk-emc.c
+++ b/drivers/clk/tegra/clk-emc.c
@@ -450,8 +450,10 @@ static int load_timings_from_dt(struct tegra_clk_emc *tegra,
 		struct emc_timing *timing = tegra->timings + (i++);
 
 		err = load_one_timing_from_dt(tegra, timing, child);
-		if (err)
+		if (err) {
+			of_node_put(child);
 			return err;
+		}
 
 		timing->ram_code = ram_code;
 	}
@@ -499,9 +501,9 @@ struct clk *tegra_clk_register_emc(void __iomem *base, struct device_node *np,
 		 * fuses until the apbmisc driver is loaded.
 		 */
 		err = load_timings_from_dt(tegra, node, node_ram_code);
+		of_node_put(node);
 		if (err)
 			return ERR_PTR(err);
-		of_node_put(node);
 		break;
 	}
 
diff --git a/drivers/clk/tegra/clk-id.h b/drivers/clk/tegra/clk-id.h
index 60738cc954cb..62ea38187b71 100644
--- a/drivers/clk/tegra/clk-id.h
+++ b/drivers/clk/tegra/clk-id.h
@@ -11,8 +11,10 @@ enum clk_id {
 	tegra_clk_afi,
 	tegra_clk_amx,
 	tegra_clk_amx1,
+	tegra_clk_apb2ape,
 	tegra_clk_apbdma,
 	tegra_clk_apbif,
+	tegra_clk_ape,
 	tegra_clk_audio0,
 	tegra_clk_audio0_2x,
 	tegra_clk_audio0_mux,
@@ -38,6 +40,7 @@ enum clk_id {
 	tegra_clk_cile,
 	tegra_clk_clk_32k,
 	tegra_clk_clk72Mhz,
+	tegra_clk_clk72Mhz_8,
 	tegra_clk_clk_m,
 	tegra_clk_clk_m_div2,
 	tegra_clk_clk_m_div4,
@@ -51,17 +54,21 @@ enum clk_id {
 	tegra_clk_cml1,
 	tegra_clk_csi,
 	tegra_clk_csite,
+	tegra_clk_csite_8,
 	tegra_clk_csus,
 	tegra_clk_cve,
 	tegra_clk_dam0,
 	tegra_clk_dam1,
 	tegra_clk_dam2,
 	tegra_clk_d_audio,
+	tegra_clk_dbgapb,
 	tegra_clk_dds,
 	tegra_clk_dfll_ref,
 	tegra_clk_dfll_soc,
 	tegra_clk_disp1,
+	tegra_clk_disp1_8,
 	tegra_clk_disp2,
+	tegra_clk_disp2_8,
 	tegra_clk_dp2,
 	tegra_clk_dpaux,
 	tegra_clk_dsialp,
@@ -71,6 +78,7 @@ enum clk_id {
 	tegra_clk_dtv,
 	tegra_clk_emc,
 	tegra_clk_entropy,
+	tegra_clk_entropy_8,
 	tegra_clk_epp,
 	tegra_clk_epp_8,
 	tegra_clk_extern1,
@@ -85,12 +93,16 @@ enum clk_id {
 	tegra_clk_gr3d_8,
 	tegra_clk_hclk,
 	tegra_clk_hda,
+	tegra_clk_hda_8,
 	tegra_clk_hda2codec_2x,
+	tegra_clk_hda2codec_2x_8,
 	tegra_clk_hda2hdmi,
 	tegra_clk_hdmi,
 	tegra_clk_hdmi_audio,
 	tegra_clk_host1x,
 	tegra_clk_host1x_8,
+	tegra_clk_host1x_9,
+	tegra_clk_hsic_trk,
 	tegra_clk_i2c1,
 	tegra_clk_i2c2,
 	tegra_clk_i2c3,
@@ -110,11 +122,14 @@ enum clk_id {
 	tegra_clk_i2s4_sync,
 	tegra_clk_isp,
 	tegra_clk_isp_8,
+	tegra_clk_isp_9,
 	tegra_clk_ispb,
 	tegra_clk_kbc,
 	tegra_clk_kfuse,
 	tegra_clk_la,
+	tegra_clk_maud,
 	tegra_clk_mipi,
+	tegra_clk_mipibif,
 	tegra_clk_mipi_cal,
 	tegra_clk_mpe,
 	tegra_clk_mselect,
@@ -124,15 +139,24 @@ enum clk_id {
 	tegra_clk_ndspeed,
 	tegra_clk_ndspeed_8,
 	tegra_clk_nor,
+	tegra_clk_nvdec,
+	tegra_clk_nvenc,
+	tegra_clk_nvjpg,
 	tegra_clk_owr,
+	tegra_clk_owr_8,
 	tegra_clk_pcie,
 	tegra_clk_pclk,
 	tegra_clk_pll_a,
 	tegra_clk_pll_a_out0,
+	tegra_clk_pll_a1,
 	tegra_clk_pll_c,
 	tegra_clk_pll_c2,
 	tegra_clk_pll_c3,
 	tegra_clk_pll_c4,
+	tegra_clk_pll_c4_out0,
+	tegra_clk_pll_c4_out1,
+	tegra_clk_pll_c4_out2,
+	tegra_clk_pll_c4_out3,
 	tegra_clk_pll_c_out1,
 	tegra_clk_pll_d,
 	tegra_clk_pll_d2,
@@ -140,19 +164,29 @@ enum clk_id {
 	tegra_clk_pll_d_out0,
 	tegra_clk_pll_dp,
 	tegra_clk_pll_e_out0,
+	tegra_clk_pll_g_ref,
 	tegra_clk_pll_m,
 	tegra_clk_pll_m_out1,
+	tegra_clk_pll_mb,
 	tegra_clk_pll_p,
 	tegra_clk_pll_p_out1,
 	tegra_clk_pll_p_out2,
 	tegra_clk_pll_p_out2_int,
 	tegra_clk_pll_p_out3,
 	tegra_clk_pll_p_out4,
+	tegra_clk_pll_p_out4_cpu,
 	tegra_clk_pll_p_out5,
+	tegra_clk_pll_p_out_hsio,
+	tegra_clk_pll_p_out_xusb,
+	tegra_clk_pll_p_out_cpu,
+	tegra_clk_pll_p_out_adsp,
 	tegra_clk_pll_ref,
 	tegra_clk_pll_re_out,
 	tegra_clk_pll_re_vco,
 	tegra_clk_pll_u,
+	tegra_clk_pll_u_out,
+	tegra_clk_pll_u_out1,
+	tegra_clk_pll_u_out2,
 	tegra_clk_pll_u_12m,
 	tegra_clk_pll_u_480m,
 	tegra_clk_pll_u_48m,
@@ -160,53 +194,80 @@ enum clk_id {
 	tegra_clk_pll_x,
 	tegra_clk_pll_x_out0,
 	tegra_clk_pwm,
+	tegra_clk_qspi,
 	tegra_clk_rtc,
 	tegra_clk_sata,
+	tegra_clk_sata_8,
 	tegra_clk_sata_cold,
 	tegra_clk_sata_oob,
+	tegra_clk_sata_oob_8,
 	tegra_clk_sbc1,
 	tegra_clk_sbc1_8,
+	tegra_clk_sbc1_9,
 	tegra_clk_sbc2,
 	tegra_clk_sbc2_8,
+	tegra_clk_sbc2_9,
 	tegra_clk_sbc3,
 	tegra_clk_sbc3_8,
+	tegra_clk_sbc3_9,
 	tegra_clk_sbc4,
 	tegra_clk_sbc4_8,
+	tegra_clk_sbc4_9,
 	tegra_clk_sbc5,
 	tegra_clk_sbc5_8,
 	tegra_clk_sbc6,
 	tegra_clk_sbc6_8,
 	tegra_clk_sclk,
+	tegra_clk_sdmmc_legacy,
 	tegra_clk_sdmmc1,
 	tegra_clk_sdmmc1_8,
+	tegra_clk_sdmmc1_9,
 	tegra_clk_sdmmc2,
 	tegra_clk_sdmmc2_8,
+	tegra_clk_sdmmc2_9,
 	tegra_clk_sdmmc3,
 	tegra_clk_sdmmc3_8,
+	tegra_clk_sdmmc3_9,
 	tegra_clk_sdmmc4,
 	tegra_clk_sdmmc4_8,
+	tegra_clk_sdmmc4_9,
 	tegra_clk_se,
 	tegra_clk_soc_therm,
+	tegra_clk_soc_therm_8,
 	tegra_clk_sor0,
 	tegra_clk_sor0_lvds,
+	tegra_clk_sor1,
+	tegra_clk_sor1_brick,
+	tegra_clk_sor1_src,
 	tegra_clk_spdif,
 	tegra_clk_spdif_2x,
 	tegra_clk_spdif_in,
+	tegra_clk_spdif_in_8,
 	tegra_clk_spdif_in_sync,
 	tegra_clk_spdif_mux,
 	tegra_clk_spdif_out,
 	tegra_clk_timer,
 	tegra_clk_trace,
 	tegra_clk_tsec,
+	tegra_clk_tsec_8,
+	tegra_clk_tsecb,
 	tegra_clk_tsensor,
 	tegra_clk_tvdac,
 	tegra_clk_tvo,
 	tegra_clk_uarta,
+	tegra_clk_uarta_8,
 	tegra_clk_uartb,
+	tegra_clk_uartb_8,
 	tegra_clk_uartc,
+	tegra_clk_uartc_8,
 	tegra_clk_uartd,
+	tegra_clk_uartd_8,
 	tegra_clk_uarte,
+	tegra_clk_uarte_8,
+	tegra_clk_uartape,
 	tegra_clk_usb2,
+	tegra_clk_usb2_hsic_trk,
+	tegra_clk_usb2_trk,
 	tegra_clk_usb3,
 	tegra_clk_usbd,
 	tegra_clk_vcp,
@@ -216,22 +277,35 @@ enum clk_id {
 	tegra_clk_vi,
 	tegra_clk_vi_8,
 	tegra_clk_vi_9,
+	tegra_clk_vi_10,
+	tegra_clk_vi_i2c,
 	tegra_clk_vic03,
+	tegra_clk_vic03_8,
 	tegra_clk_vim2_clk,
 	tegra_clk_vimclk_sync,
 	tegra_clk_vi_sensor,
-	tegra_clk_vi_sensor2,
 	tegra_clk_vi_sensor_8,
+	tegra_clk_vi_sensor_9,
+	tegra_clk_vi_sensor2,
+	tegra_clk_vi_sensor2_8,
 	tegra_clk_xusb_dev,
 	tegra_clk_xusb_dev_src,
+	tegra_clk_xusb_dev_src_8,
 	tegra_clk_xusb_falcon_src,
+	tegra_clk_xusb_falcon_src_8,
 	tegra_clk_xusb_fs_src,
+	tegra_clk_xusb_gate,
 	tegra_clk_xusb_host,
 	tegra_clk_xusb_host_src,
+	tegra_clk_xusb_host_src_8,
 	tegra_clk_xusb_hs_src,
+	tegra_clk_xusb_hs_src_4,
 	tegra_clk_xusb_ss,
 	tegra_clk_xusb_ss_src,
+	tegra_clk_xusb_ss_src_8,
 	tegra_clk_xusb_ss_div2,
+	tegra_clk_xusb_ssp_src,
+	tegra_clk_sclk_mux,
 	tegra_clk_max,
 };
 
diff --git a/drivers/clk/tegra/clk-pll.c b/drivers/clk/tegra/clk-pll.c
index d6d4ecb88e94..6ac3f843e7ca 100644
--- a/drivers/clk/tegra/clk-pll.c
+++ b/drivers/clk/tegra/clk-pll.c
@@ -65,6 +65,7 @@
 #define PLLE_BASE_DIVN_WIDTH 8
 #define PLLE_BASE_DIVM_SHIFT 0
 #define PLLE_BASE_DIVM_WIDTH 8
+#define PLLE_BASE_ENABLE BIT(31)
 
 #define PLLE_MISC_SETUP_BASE_SHIFT 16
 #define PLLE_MISC_SETUP_BASE_MASK (0xffff << PLLE_MISC_SETUP_BASE_SHIFT)
@@ -85,15 +86,21 @@
 #define PLLE_SS_DISABLE (PLLE_SS_CNTL_BYPASS_SS | PLLE_SS_CNTL_INTERP_RESET |\
 				PLLE_SS_CNTL_SSC_BYP)
 #define PLLE_SS_MAX_MASK 0x1ff
-#define PLLE_SS_MAX_VAL 0x25
+#define PLLE_SS_MAX_VAL_TEGRA114 0x25
+#define PLLE_SS_MAX_VAL_TEGRA210 0x21
 #define PLLE_SS_INC_MASK (0xff << 16)
 #define PLLE_SS_INC_VAL (0x1 << 16)
 #define PLLE_SS_INCINTRV_MASK (0x3f << 24)
-#define PLLE_SS_INCINTRV_VAL (0x20 << 24)
+#define PLLE_SS_INCINTRV_VAL_TEGRA114 (0x20 << 24)
+#define PLLE_SS_INCINTRV_VAL_TEGRA210 (0x23 << 24)
 #define PLLE_SS_COEFFICIENTS_MASK \
 	(PLLE_SS_MAX_MASK | PLLE_SS_INC_MASK | PLLE_SS_INCINTRV_MASK)
-#define PLLE_SS_COEFFICIENTS_VAL \
-	(PLLE_SS_MAX_VAL | PLLE_SS_INC_VAL | PLLE_SS_INCINTRV_VAL)
+#define PLLE_SS_COEFFICIENTS_VAL_TEGRA114 \
+	(PLLE_SS_MAX_VAL_TEGRA114 | PLLE_SS_INC_VAL |\
+	 PLLE_SS_INCINTRV_VAL_TEGRA114)
+#define PLLE_SS_COEFFICIENTS_VAL_TEGRA210 \
+	(PLLE_SS_MAX_VAL_TEGRA210 | PLLE_SS_INC_VAL |\
+	 PLLE_SS_INCINTRV_VAL_TEGRA210)
 
 #define PLLE_AUX_PLLP_SEL	BIT(2)
 #define PLLE_AUX_USE_LOCKDET	BIT(3)
@@ -102,6 +109,7 @@
 #define PLLE_AUX_SEQ_ENABLE	BIT(24)
 #define PLLE_AUX_SEQ_START_STATE BIT(25)
 #define PLLE_AUX_PLLRE_SEL	BIT(28)
+#define PLLE_AUX_SS_SEQ_INCLUDE	BIT(31)
 
 #define XUSBIO_PLL_CFG0		0x51c
 #define XUSBIO_PLL_CFG0_PADPLL_RESET_SWCTL	BIT(0)
@@ -187,17 +195,23 @@
 #define pll_readl_base(p) pll_readl(p->params->base_reg, p)
 #define pll_readl_misc(p) pll_readl(p->params->misc_reg, p)
 #define pll_override_readl(offset, p) readl_relaxed(p->pmc + offset)
+#define pll_readl_sdm_din(p) pll_readl(p->params->sdm_din_reg, p)
+#define pll_readl_sdm_ctrl(p) pll_readl(p->params->sdm_ctrl_reg, p)
 
 #define pll_writel(val, offset, p) writel_relaxed(val, p->clk_base + offset)
 #define pll_writel_base(val, p) pll_writel(val, p->params->base_reg, p)
 #define pll_writel_misc(val, p) pll_writel(val, p->params->misc_reg, p)
 #define pll_override_writel(val, offset, p) writel(val, p->pmc + offset)
+#define pll_writel_sdm_din(val, p) pll_writel(val, p->params->sdm_din_reg, p)
+#define pll_writel_sdm_ctrl(val, p) pll_writel(val, p->params->sdm_ctrl_reg, p)
 
 #define mask(w) ((1 << (w)) - 1)
 #define divm_mask(p) mask(p->params->div_nmp->divm_width)
 #define divn_mask(p) mask(p->params->div_nmp->divn_width)
 #define divp_mask(p) (p->params->flags & TEGRA_PLLU ? PLLU_POST_DIVP_MASK :\
 		      mask(p->params->div_nmp->divp_width))
+#define sdm_din_mask(p) p->params->sdm_din_mask
+#define sdm_en_mask(p) p->params->sdm_ctrl_en_mask
 
 #define divm_shift(p) (p)->params->div_nmp->divm_shift
 #define divn_shift(p) (p)->params->div_nmp->divn_shift
@@ -211,6 +225,9 @@
 #define divn_max(p) (divn_mask(p))
 #define divp_max(p) (1 << (divp_mask(p)))
 
+#define sdin_din_to_data(din)	((u16)((din) ? : 0xFFFFU))
+#define sdin_data_to_din(dat)	(((dat) == 0xFFFFU) ? 0 : (s16)dat)
+
 static struct div_nmp default_nmp = {
 	.divn_shift = PLL_BASE_DIVN_SHIFT,
 	.divn_width = PLL_BASE_DIVN_WIDTH,
@@ -269,6 +286,11 @@ static int clk_pll_wait_for_lock(struct tegra_clk_pll *pll)
 	return -1;
 }
 
+int tegra_pll_wait_for_lock(struct tegra_clk_pll *pll)
+{
+	return clk_pll_wait_for_lock(pll);
+}
+
 static int clk_pll_is_enabled(struct clk_hw *hw)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
@@ -290,6 +312,19 @@ static void _clk_pll_enable(struct clk_hw *hw)
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
 	u32 val;
 
+	if (pll->params->iddq_reg) {
+		val = pll_readl(pll->params->iddq_reg, pll);
+		val &= ~BIT(pll->params->iddq_bit_idx);
+		pll_writel(val, pll->params->iddq_reg, pll);
+		udelay(2);
+	}
+
+	if (pll->params->reset_reg) {
+		val = pll_readl(pll->params->reset_reg, pll);
+		val &= ~BIT(pll->params->reset_bit_idx);
+		pll_writel(val, pll->params->reset_reg, pll);
+	}
+
 	clk_pll_enable_lock(pll);
 
 	val = pll_readl_base(pll);
@@ -321,6 +356,19 @@ static void _clk_pll_disable(struct clk_hw *hw)
 		val &= ~PMC_PLLP_WB0_OVERRIDE_PLLM_ENABLE;
 		writel_relaxed(val, pll->pmc + PMC_PLLP_WB0_OVERRIDE);
 	}
+
+	if (pll->params->reset_reg) {
+		val = pll_readl(pll->params->reset_reg, pll);
+		val |= BIT(pll->params->reset_bit_idx);
+		pll_writel(val, pll->params->reset_reg, pll);
+	}
+
+	if (pll->params->iddq_reg) {
+		val = pll_readl(pll->params->iddq_reg, pll);
+		val |= BIT(pll->params->iddq_bit_idx);
+		pll_writel(val, pll->params->iddq_reg, pll);
+		udelay(2);
+	}
 }
 
 static int clk_pll_enable(struct clk_hw *hw)
@@ -359,7 +407,7 @@ static void clk_pll_disable(struct clk_hw *hw)
 static int _p_div_to_hw(struct clk_hw *hw, u8 p_div)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
+	const struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
 
 	if (p_tohw) {
 		while (p_tohw->pdiv) {
@@ -372,10 +420,15 @@ static int _p_div_to_hw(struct clk_hw *hw, u8 p_div)
 	return -EINVAL;
 }
 
+int tegra_pll_p_div_to_hw(struct tegra_clk_pll *pll, u8 p_div)
+{
+	return _p_div_to_hw(&pll->hw, p_div);
+}
+
 static int _hw_to_p_div(struct clk_hw *hw, u8 p_div_hw)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
+	const struct pdiv_map *p_tohw = pll->params->pdiv_tohw;
 
 	if (p_tohw) {
 		while (p_tohw->pdiv) {
@@ -395,6 +448,7 @@ static int _get_table_rate(struct clk_hw *hw,
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
 	struct tegra_clk_pll_freq_table *sel;
+	int p;
 
 	for (sel = pll->params->freq_table; sel->input_rate != 0; sel++)
 		if (sel->input_rate == parent_rate &&
@@ -404,12 +458,21 @@ static int _get_table_rate(struct clk_hw *hw,
 	if (sel->input_rate == 0)
 		return -EINVAL;
 
+	if (pll->params->pdiv_tohw) {
+		p = _p_div_to_hw(hw, sel->p);
+		if (p < 0)
+			return p;
+	} else {
+		p = ilog2(sel->p);
+	}
+
 	cfg->input_rate = sel->input_rate;
 	cfg->output_rate = sel->output_rate;
 	cfg->m = sel->m;
 	cfg->n = sel->n;
-	cfg->p = sel->p;
+	cfg->p = p;
 	cfg->cpcon = sel->cpcon;
+	cfg->sdm_data = sel->sdm_data;
 
 	return 0;
 }
@@ -439,7 +502,7 @@ static int _calc_rate(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 		/*
 		 * PLL_P_OUT1 rate is not listed in PLLA table
 		 */
-		cfreq = parent_rate/(parent_rate/1000000);
+		cfreq = parent_rate / (parent_rate / 1000000);
 		break;
 	default:
 		pr_err("%s Unexpected reference rate %lu\n",
@@ -476,6 +539,42 @@ static int _calc_rate(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 	return 0;
 }
 
+/*
+ * SDM (Sigma Delta Modulator) divisor is 16-bit 2's complement signed number
+ * within (-2^12 ... 2^12-1) range. Represented in PLL data structure as
+ * unsigned 16-bit value, with "0" divisor mapped to 0xFFFF. Data "0" is used
+ * to indicate that SDM is disabled.
+ *
+ * Effective ndiv value when SDM is enabled: ndiv + 1/2 + sdm_din/2^13
+ */
+static void clk_pll_set_sdm_data(struct clk_hw *hw,
+				 struct tegra_clk_pll_freq_table *cfg)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	u32 val;
+	bool enabled;
+
+	if (!pll->params->sdm_din_reg)
+		return;
+
+	if (cfg->sdm_data) {
+		val = pll_readl_sdm_din(pll) & (~sdm_din_mask(pll));
+		val |= sdin_data_to_din(cfg->sdm_data) & sdm_din_mask(pll);
+		pll_writel_sdm_din(val, pll);
+	}
+
+	val = pll_readl_sdm_ctrl(pll);
+	enabled = (val & sdm_en_mask(pll));
+
+	if (cfg->sdm_data == 0 && enabled)
+		val &= ~pll->params->sdm_ctrl_en_mask;
+
+	if (cfg->sdm_data != 0 && !enabled)
+		val |= pll->params->sdm_ctrl_en_mask;
+
+	pll_writel_sdm_ctrl(val, pll);
+}
+
 static void _update_pll_mnp(struct tegra_clk_pll *pll,
 			    struct tegra_clk_pll_freq_table *cfg)
 {
@@ -483,7 +582,7 @@ static void _update_pll_mnp(struct tegra_clk_pll *pll,
 	struct tegra_clk_pll_params *params = pll->params;
 	struct div_nmp *div_nmp = params->div_nmp;
 
-	if ((params->flags & TEGRA_PLLM) &&
+	if ((params->flags & (TEGRA_PLLM | TEGRA_PLLMB)) &&
 		(pll_override_readl(PMC_PLLP_WB0_OVERRIDE, pll) &
 			PMC_PLLP_WB0_OVERRIDE_PLLM_OVERRIDE)) {
 		val = pll_override_readl(params->pmc_divp_reg, pll);
@@ -508,6 +607,8 @@ static void _update_pll_mnp(struct tegra_clk_pll *pll,
 		       (cfg->p << divp_shift(pll));
 
 		pll_writel_base(val, pll);
+
+		clk_pll_set_sdm_data(&pll->hw, cfg);
 	}
 }
 
@@ -518,7 +619,7 @@ static void _get_pll_mnp(struct tegra_clk_pll *pll,
 	struct tegra_clk_pll_params *params = pll->params;
 	struct div_nmp *div_nmp = params->div_nmp;
 
-	if ((params->flags & TEGRA_PLLM) &&
+	if ((params->flags & (TEGRA_PLLM | TEGRA_PLLMB)) &&
 		(pll_override_readl(PMC_PLLP_WB0_OVERRIDE, pll) &
 			PMC_PLLP_WB0_OVERRIDE_PLLM_OVERRIDE)) {
 		val = pll_override_readl(params->pmc_divp_reg, pll);
@@ -533,6 +634,14 @@ static void _get_pll_mnp(struct tegra_clk_pll *pll,
 		cfg->m = (val >> div_nmp->divm_shift) & divm_mask(pll);
 		cfg->n = (val >> div_nmp->divn_shift) & divn_mask(pll);
 		cfg->p = (val >> div_nmp->divp_shift) & divp_mask(pll);
+
+		if (pll->params->sdm_din_reg) {
+			if (sdm_en_mask(pll) & pll_readl_sdm_ctrl(pll)) {
+				val = pll_readl_sdm_din(pll);
+				val &= sdm_din_mask(pll);
+				cfg->sdm_data = sdin_din_to_data(val);
+			}
+		}
 	}
 }
 
@@ -560,16 +669,51 @@ static void _update_pll_cpcon(struct tegra_clk_pll *pll,
 	pll_writel_misc(val, pll);
 }
 
+static void pll_clk_start_ss(struct tegra_clk_pll *pll)
+{
+	if (pll->params->defaults_set && pll->params->ssc_ctrl_reg) {
+		u32 val = pll_readl(pll->params->ssc_ctrl_reg, pll);
+
+		val |= pll->params->ssc_ctrl_en_mask;
+		pll_writel(val, pll->params->ssc_ctrl_reg, pll);
+	}
+}
+
+static void pll_clk_stop_ss(struct tegra_clk_pll *pll)
+{
+	if (pll->params->defaults_set && pll->params->ssc_ctrl_reg) {
+		u32 val = pll_readl(pll->params->ssc_ctrl_reg, pll);
+
+		val &= ~pll->params->ssc_ctrl_en_mask;
+		pll_writel(val, pll->params->ssc_ctrl_reg, pll);
+	}
+}
+
 static int _program_pll(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 			unsigned long rate)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct tegra_clk_pll_freq_table old_cfg;
 	int state, ret = 0;
 
 	state = clk_pll_is_enabled(hw);
 
-	if (state)
+	_get_pll_mnp(pll, &old_cfg);
+
+	if (state && pll->params->defaults_set && pll->params->dyn_ramp &&
+			(cfg->m == old_cfg.m) && (cfg->p == old_cfg.p)) {
+		ret = pll->params->dyn_ramp(pll, cfg);
+		if (!ret)
+			return 0;
+	}
+
+	if (state) {
+		pll_clk_stop_ss(pll);
 		_clk_pll_disable(hw);
+	}
+
+	if (!pll->params->defaults_set && pll->params->set_defaults)
+		pll->params->set_defaults(pll);
 
 	_update_pll_mnp(pll, cfg);
 
@@ -579,6 +723,7 @@ static int _program_pll(struct clk_hw *hw, struct tegra_clk_pll_freq_table *cfg,
 	if (state) {
 		_clk_pll_enable(hw);
 		ret = clk_pll_wait_for_lock(pll);
+		pll_clk_start_ss(pll);
 	}
 
 	return ret;
@@ -603,7 +748,7 @@ static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	}
 
 	if (_get_table_rate(hw, &cfg, rate, parent_rate) &&
-	    _calc_rate(hw, &cfg, rate, parent_rate)) {
+	    pll->params->calc_rate(hw, &cfg, rate, parent_rate)) {
 		pr_err("%s: Failed to set %s rate %lu\n", __func__,
 		       clk_hw_get_name(hw), rate);
 		WARN_ON(1);
@@ -613,8 +758,11 @@ static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 		spin_lock_irqsave(pll->lock, flags);
 
 	_get_pll_mnp(pll, &old_cfg);
+	if (pll->params->flags & TEGRA_PLL_VCO_OUT)
+		cfg.p = old_cfg.p;
 
-	if (old_cfg.m != cfg.m || old_cfg.n != cfg.n || old_cfg.p != cfg.p)
+	if (old_cfg.m != cfg.m || old_cfg.n != cfg.n || old_cfg.p != cfg.p ||
+		old_cfg.sdm_data != cfg.sdm_data)
 		ret = _program_pll(hw, &cfg, rate);
 
 	if (pll->lock)
@@ -629,15 +777,15 @@ static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
 	struct tegra_clk_pll_freq_table cfg;
 
-	if (pll->params->flags & TEGRA_PLL_FIXED)
+	if (pll->params->flags & TEGRA_PLL_FIXED) {
+		/* PLLM/MB are used for memory; we do not change rate */
+		if (pll->params->flags & (TEGRA_PLLM | TEGRA_PLLMB))
+			return clk_hw_get_rate(hw);
 		return pll->params->fixed_rate;
-
-	/* PLLM is used for memory; we do not change rate */
-	if (pll->params->flags & TEGRA_PLLM)
-		return clk_hw_get_rate(hw);
+	}
 
 	if (_get_table_rate(hw, &cfg, rate, *prate) &&
-	    _calc_rate(hw, &cfg, rate, *prate))
+	    pll->params->calc_rate(hw, &cfg, rate, *prate))
 		return -EINVAL;
 
 	return cfg.output_rate;
@@ -658,6 +806,7 @@ static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
 		return parent_rate;
 
 	if ((pll->params->flags & TEGRA_PLL_FIXED) &&
+	    !(pll->params->flags & (TEGRA_PLLM | TEGRA_PLLMB)) &&
 			!(val & PLL_BASE_OVERRIDE)) {
 		struct tegra_clk_pll_freq_table sel;
 		if (_get_table_rate(hw, &sel, pll->params->fixed_rate,
@@ -671,12 +820,20 @@ static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
 
 	_get_pll_mnp(pll, &cfg);
 
-	pdiv = _hw_to_p_div(hw, cfg.p);
-	if (pdiv < 0) {
-		WARN_ON(1);
+	if (pll->params->flags & TEGRA_PLL_VCO_OUT) {
 		pdiv = 1;
+	} else {
+		pdiv = _hw_to_p_div(hw, cfg.p);
+		if (pdiv < 0) {
+			WARN(1, "Clock %s has invalid pdiv value : 0x%x\n",
+			     clk_hw_get_name(hw), cfg.p);
+			pdiv = 1;
+		}
 	}
 
+	if (pll->params->set_gain)
+		pll->params->set_gain(&cfg);
+
 	cfg.m *= pdiv;
 
 	rate *= cfg.n;
@@ -729,7 +886,7 @@ static int clk_plle_training(struct tegra_clk_pll *pll)
 static int clk_plle_enable(struct clk_hw *hw)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned long input_rate = clk_get_rate(clk_get_parent(hw->clk));
+	unsigned long input_rate = clk_hw_get_rate(clk_hw_get_parent(hw));
 	struct tegra_clk_pll_freq_table sel;
 	u32 val;
 	int err;
@@ -816,19 +973,65 @@ const struct clk_ops tegra_clk_plle_ops = {
 	.enable = clk_plle_enable,
 };
 
-#if defined(CONFIG_ARCH_TEGRA_114_SOC) || \
-	defined(CONFIG_ARCH_TEGRA_124_SOC) || \
-	defined(CONFIG_ARCH_TEGRA_132_SOC)
-
 static int _pll_fixed_mdiv(struct tegra_clk_pll_params *pll_params,
 			   unsigned long parent_rate)
 {
+	u16 mdiv = parent_rate / pll_params->cf_min;
+
+	if (pll_params->flags & TEGRA_MDIV_NEW)
+		return (!pll_params->mdiv_default ? mdiv :
+			min(mdiv, pll_params->mdiv_default));
+
+	if (pll_params->mdiv_default)
+		return pll_params->mdiv_default;
+
 	if (parent_rate > pll_params->cf_max)
 		return 2;
 	else
 		return 1;
 }
 
+static int _calc_dynamic_ramp_rate(struct clk_hw *hw,
+				struct tegra_clk_pll_freq_table *cfg,
+				unsigned long rate, unsigned long parent_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned int p;
+	int p_div;
+
+	if (!rate)
+		return -EINVAL;
+
+	p = DIV_ROUND_UP(pll->params->vco_min, rate);
+	cfg->m = _pll_fixed_mdiv(pll->params, parent_rate);
+	cfg->output_rate = rate * p;
+	cfg->n = cfg->output_rate * cfg->m / parent_rate;
+	cfg->input_rate = parent_rate;
+
+	p_div = _p_div_to_hw(hw, p);
+	if (p_div < 0)
+		return p_div;
+
+	cfg->p = p_div;
+
+	if (cfg->n > divn_max(pll) || cfg->output_rate > pll->params->vco_max)
+		return -EINVAL;
+
+	return 0;
+}
+
+#if defined(CONFIG_ARCH_TEGRA_114_SOC) || \
+	defined(CONFIG_ARCH_TEGRA_124_SOC) || \
+	defined(CONFIG_ARCH_TEGRA_132_SOC) || \
+	defined(CONFIG_ARCH_TEGRA_210_SOC)
+
+u16 tegra_pll_get_fixed_mdiv(struct clk_hw *hw, unsigned long input_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+
+	return (u16)_pll_fixed_mdiv(pll->params, input_rate);
+}
+
 static unsigned long _clip_vco_min(unsigned long vco_min,
 				   unsigned long parent_rate)
 {
@@ -871,86 +1074,12 @@ static int _setup_dynamic_ramp(struct tegra_clk_pll_params *pll_params,
 	return 0;
 }
 
-static int clk_pll_iddq_enable(struct clk_hw *hw)
-{
-	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned long flags = 0;
-
-	u32 val;
-	int ret;
-
-	if (pll->lock)
-		spin_lock_irqsave(pll->lock, flags);
-
-	val = pll_readl(pll->params->iddq_reg, pll);
-	val &= ~BIT(pll->params->iddq_bit_idx);
-	pll_writel(val, pll->params->iddq_reg, pll);
-	udelay(2);
-
-	_clk_pll_enable(hw);
-
-	ret = clk_pll_wait_for_lock(pll);
-
-	if (pll->lock)
-		spin_unlock_irqrestore(pll->lock, flags);
-
-	return 0;
-}
-
-static void clk_pll_iddq_disable(struct clk_hw *hw)
-{
-	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned long flags = 0;
-	u32 val;
-
-	if (pll->lock)
-		spin_lock_irqsave(pll->lock, flags);
-
-	_clk_pll_disable(hw);
-
-	val = pll_readl(pll->params->iddq_reg, pll);
-	val |= BIT(pll->params->iddq_bit_idx);
-	pll_writel(val, pll->params->iddq_reg, pll);
-	udelay(2);
-
-	if (pll->lock)
-		spin_unlock_irqrestore(pll->lock, flags);
-}
-
-static int _calc_dynamic_ramp_rate(struct clk_hw *hw,
-				struct tegra_clk_pll_freq_table *cfg,
-				unsigned long rate, unsigned long parent_rate)
-{
-	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned int p;
-	int p_div;
-
-	if (!rate)
-		return -EINVAL;
-
-	p = DIV_ROUND_UP(pll->params->vco_min, rate);
-	cfg->m = _pll_fixed_mdiv(pll->params, parent_rate);
-	cfg->output_rate = rate * p;
-	cfg->n = cfg->output_rate * cfg->m / parent_rate;
-
-	p_div = _p_div_to_hw(hw, p);
-	if (p_div < 0)
-		return p_div;
-	else
-		cfg->p = p_div;
-
-	if (cfg->n > divn_max(pll) || cfg->output_rate > pll->params->vco_max)
-		return -EINVAL;
-
-	return 0;
-}
-
 static int _pll_ramp_calc_pll(struct clk_hw *hw,
 			      struct tegra_clk_pll_freq_table *cfg,
 			      unsigned long rate, unsigned long parent_rate)
 {
 	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	int err = 0, p_div;
+	int err = 0;
 
 	err = _get_table_rate(hw, cfg, rate, parent_rate);
 	if (err < 0)
@@ -961,11 +1090,6 @@ static int _pll_ramp_calc_pll(struct clk_hw *hw,
 			err = -EINVAL;
 			goto out;
 		}
-		p_div = _p_div_to_hw(hw, cfg->p);
-		if (p_div < 0)
-			return p_div;
-		else
-			cfg->p = p_div;
 	}
 
 	if (cfg->p >  pll->params->max_p)
@@ -991,6 +1115,8 @@ static int clk_pllxc_set_rate(struct clk_hw *hw, unsigned long rate,
 		spin_lock_irqsave(pll->lock, flags);
 
 	_get_pll_mnp(pll, &old_cfg);
+	if (pll->params->flags & TEGRA_PLL_VCO_OUT)
+		cfg.p = old_cfg.p;
 
 	if (old_cfg.m != cfg.m || old_cfg.n != cfg.n || old_cfg.p != cfg.p)
 		ret = _program_pll(hw, &cfg, rate);
@@ -1004,6 +1130,7 @@ static int clk_pllxc_set_rate(struct clk_hw *hw, unsigned long rate,
 static long clk_pll_ramp_round_rate(struct clk_hw *hw, unsigned long rate,
 				unsigned long *prate)
 {
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
 	struct tegra_clk_pll_freq_table cfg;
 	int ret, p_div;
 	u64 output_rate = *prate;
@@ -1016,46 +1143,15 @@ static long clk_pll_ramp_round_rate(struct clk_hw *hw, unsigned long rate,
 	if (p_div < 0)
 		return p_div;
 
+	if (pll->params->set_gain)
+		pll->params->set_gain(&cfg);
+
 	output_rate *= cfg.n;
 	do_div(output_rate, cfg.m * p_div);
 
 	return output_rate;
 }
 
-static int clk_pllm_set_rate(struct clk_hw *hw, unsigned long rate,
-				unsigned long parent_rate)
-{
-	struct tegra_clk_pll_freq_table cfg;
-	struct tegra_clk_pll *pll = to_clk_pll(hw);
-	unsigned long flags = 0;
-	int state, ret = 0;
-
-	if (pll->lock)
-		spin_lock_irqsave(pll->lock, flags);
-
-	state = clk_pll_is_enabled(hw);
-	if (state) {
-		if (rate != clk_get_rate(hw->clk)) {
-			pr_err("%s: Cannot change active PLLM\n", __func__);
-			ret = -EINVAL;
-			goto out;
-		}
-		goto out;
-	}
-
-	ret = _pll_ramp_calc_pll(hw, &cfg, rate, parent_rate);
-	if (ret < 0)
-		goto out;
-
-	_update_pll_mnp(pll, &cfg);
-
-out:
-	if (pll->lock)
-		spin_unlock_irqrestore(pll->lock, flags);
-
-	return ret;
-}
-
 static void _pllcx_strobe(struct tegra_clk_pll *pll)
 {
 	u32 val;
@@ -1288,7 +1384,7 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	u32 val;
 	int ret;
 	unsigned long flags = 0;
-	unsigned long input_rate = clk_get_rate(clk_get_parent(hw->clk));
+	unsigned long input_rate = clk_hw_get_rate(clk_hw_get_parent(hw));
 
 	if (_get_table_rate(hw, &sel, pll->params->fixed_rate, input_rate))
 		return -EINVAL;
@@ -1311,7 +1407,7 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	val |= PLLE_MISC_IDDQ_SW_CTRL;
 	val &= ~PLLE_MISC_IDDQ_SW_VALUE;
 	val |= PLLE_MISC_PLLE_PTS;
-	val |= PLLE_MISC_VREG_BG_CTRL_MASK | PLLE_MISC_VREG_CTRL_MASK;
+	val &= ~(PLLE_MISC_VREG_BG_CTRL_MASK | PLLE_MISC_VREG_CTRL_MASK);
 	pll_writel_misc(val, pll);
 	udelay(5);
 
@@ -1338,7 +1434,7 @@ static int clk_plle_tegra114_enable(struct clk_hw *hw)
 	val = pll_readl(PLLE_SS_CTRL, pll);
 	val &= ~(PLLE_SS_CNTL_CENTER | PLLE_SS_CNTL_INVERT);
 	val &= ~PLLE_SS_COEFFICIENTS_MASK;
-	val |= PLLE_SS_COEFFICIENTS_VAL;
+	val |= PLLE_SS_COEFFICIENTS_VAL_TEGRA114;
 	pll_writel(val, PLLE_SS_CTRL, pll);
 	val &= ~(PLLE_SS_CNTL_SSC_BYP | PLLE_SS_CNTL_BYPASS_SS);
 	pll_writel(val, PLLE_SS_CTRL, pll);
@@ -1445,6 +1541,17 @@ static struct clk *_tegra_clk_register_pll(struct tegra_clk_pll *pll,
 	init.parent_names = (parent_name ? &parent_name : NULL);
 	init.num_parents = (parent_name ? 1 : 0);
 
+	/* Default to _calc_rate if unspecified */
+	if (!pll->params->calc_rate) {
+		if (pll->params->flags & TEGRA_PLLM)
+			pll->params->calc_rate = _calc_dynamic_ramp_rate;
+		else
+			pll->params->calc_rate = _calc_rate;
+	}
+
+	if (pll->params->set_defaults)
+		pll->params->set_defaults(pll);
+
 	/* Data in .init is copied by clk_register(), so stack variable OK */
 	pll->hw.init = &init;
 
@@ -1460,7 +1567,7 @@ struct clk *tegra_clk_register_pll(const char *name, const char *parent_name,
 	struct clk *clk;
 
 	pll_params->flags |= TEGRA_PLL_BYPASS;
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1490,8 +1597,7 @@ struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 	struct tegra_clk_pll *pll;
 	struct clk *clk;
 
-	pll_params->flags |= TEGRA_PLL_LOCK_MISC | TEGRA_PLL_BYPASS;
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
+	pll_params->flags |= TEGRA_PLL_BYPASS;
 
 	if (!pll_params->div_nmp)
 		pll_params->div_nmp = &pll_e_nmp;
@@ -1510,25 +1616,17 @@ struct clk *tegra_clk_register_plle(const char *name, const char *parent_name,
 
 #if defined(CONFIG_ARCH_TEGRA_114_SOC) || \
 	defined(CONFIG_ARCH_TEGRA_124_SOC) || \
-	defined(CONFIG_ARCH_TEGRA_132_SOC)
+	defined(CONFIG_ARCH_TEGRA_132_SOC) || \
+	defined(CONFIG_ARCH_TEGRA_210_SOC)
 static const struct clk_ops tegra_clk_pllxc_ops = {
 	.is_enabled = clk_pll_is_enabled,
-	.enable = clk_pll_iddq_enable,
-	.disable = clk_pll_iddq_disable,
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
 	.recalc_rate = clk_pll_recalc_rate,
 	.round_rate = clk_pll_ramp_round_rate,
 	.set_rate = clk_pllxc_set_rate,
 };
 
-static const struct clk_ops tegra_clk_pllm_ops = {
-	.is_enabled = clk_pll_is_enabled,
-	.enable = clk_pll_iddq_enable,
-	.disable = clk_pll_iddq_disable,
-	.recalc_rate = clk_pll_recalc_rate,
-	.round_rate = clk_pll_ramp_round_rate,
-	.set_rate = clk_pllm_set_rate,
-};
-
 static const struct clk_ops tegra_clk_pllc_ops = {
 	.is_enabled = clk_pll_is_enabled,
 	.enable = clk_pllc_enable,
@@ -1540,8 +1638,8 @@ static const struct clk_ops tegra_clk_pllc_ops = {
 
 static const struct clk_ops tegra_clk_pllre_ops = {
 	.is_enabled = clk_pll_is_enabled,
-	.enable = clk_pll_iddq_enable,
-	.disable = clk_pll_iddq_disable,
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
 	.recalc_rate = clk_pllre_recalc_rate,
 	.round_rate = clk_pllre_round_rate,
 	.set_rate = clk_pllre_set_rate,
@@ -1564,7 +1662,6 @@ struct clk *tegra_clk_register_pllxc(const char *name, const char *parent_name,
 	struct tegra_clk_pll *pll;
 	struct clk *clk, *parent;
 	unsigned long parent_rate;
-	int err;
 	u32 val, val_iddq;
 
 	parent = __clk_lookup(parent_name);
@@ -1581,21 +1678,33 @@ struct clk *tegra_clk_register_pllxc(const char *name, const char *parent_name,
 
 	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
 
-	err = _setup_dynamic_ramp(pll_params, clk_base, parent_rate);
-	if (err)
-		return ERR_PTR(err);
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
 
-	val = readl_relaxed(clk_base + pll_params->base_reg);
-	val_iddq = readl_relaxed(clk_base + pll_params->iddq_reg);
+	/*
+	 * If the pll has a set_defaults callback, it will take care of
+	 * configuring dynamic ramping and setting IDDQ in that path.
+	 */
+	if (!pll_params->set_defaults) {
+		int err;
 
-	if (val & PLL_BASE_ENABLE)
-		WARN_ON(val_iddq & BIT(pll_params->iddq_bit_idx));
-	else {
-		val_iddq |= BIT(pll_params->iddq_bit_idx);
-		writel_relaxed(val_iddq, clk_base + pll_params->iddq_reg);
+		err = _setup_dynamic_ramp(pll_params, clk_base, parent_rate);
+		if (err)
+			return ERR_PTR(err);
+
+		val = readl_relaxed(clk_base + pll_params->base_reg);
+		val_iddq = readl_relaxed(clk_base + pll_params->iddq_reg);
+
+		if (val & PLL_BASE_ENABLE)
+			WARN_ON(val_iddq & BIT(pll_params->iddq_bit_idx));
+		else {
+			val_iddq |= BIT(pll_params->iddq_bit_idx);
+			writel_relaxed(val_iddq,
+				       clk_base + pll_params->iddq_reg);
+		}
 	}
 
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1618,10 +1727,12 @@ struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
 	struct tegra_clk_pll *pll;
 	struct clk *clk;
 
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE | TEGRA_PLL_LOCK_MISC;
-
 	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
 
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1630,7 +1741,8 @@ struct clk *tegra_clk_register_pllre(const char *name, const char *parent_name,
 
 	val = pll_readl_base(pll);
 	if (val & PLL_BASE_ENABLE)
-		WARN_ON(val & pll_params->iddq_bit_idx);
+		WARN_ON(readl_relaxed(clk_base + pll_params->iddq_reg) &
+				BIT(pll_params->iddq_bit_idx));
 	else {
 		int m;
 
@@ -1678,15 +1790,18 @@ struct clk *tegra_clk_register_pllm(const char *name, const char *parent_name,
 
 	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
 
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
 	pll_params->flags |= TEGRA_PLL_BYPASS;
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
 	pll_params->flags |= TEGRA_PLLM;
 	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
 
 	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
-				      &tegra_clk_pllm_ops);
+				      &tegra_clk_pll_ops);
 	if (IS_ERR(clk))
 		kfree(pll);
 
@@ -1700,7 +1815,7 @@ struct clk *tegra_clk_register_pllc(const char *name, const char *parent_name,
 			  spinlock_t *lock)
 {
 	struct clk *parent, *clk;
-	struct pdiv_map *p_tohw = pll_params->pdiv_tohw;
+	const struct pdiv_map *p_tohw = pll_params->pdiv_tohw;
 	struct tegra_clk_pll *pll;
 	struct tegra_clk_pll_freq_table cfg;
 	unsigned long parent_rate;
@@ -1777,7 +1892,6 @@ struct clk *tegra_clk_register_plle_tegra114(const char *name,
 	struct clk *clk;
 	u32 val, val_aux;
 
-	pll_params->flags |= TEGRA_PLL_HAS_LOCK_ENABLE;
 	pll = _tegra_init_pll(clk_base, NULL, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1810,8 +1924,8 @@ struct clk *tegra_clk_register_plle_tegra114(const char *name,
 #if defined(CONFIG_ARCH_TEGRA_124_SOC) || defined(CONFIG_ARCH_TEGRA_132_SOC)
 static const struct clk_ops tegra_clk_pllss_ops = {
 	.is_enabled = clk_pll_is_enabled,
-	.enable = clk_pll_iddq_enable,
-	.disable = clk_pll_iddq_disable,
+	.enable = clk_pll_enable,
+	.disable = clk_pll_disable,
 	.recalc_rate = clk_pll_recalc_rate,
 	.round_rate = clk_pll_ramp_round_rate,
 	.set_rate = clk_pllxc_set_rate,
@@ -1826,7 +1940,7 @@ struct clk *tegra_clk_register_pllss(const char *name, const char *parent_name,
 	struct clk *clk, *parent;
 	struct tegra_clk_pll_freq_table cfg;
 	unsigned long parent_rate;
-	u32 val;
+	u32 val, val_iddq;
 	int i;
 
 	if (!pll_params->div_nmp)
@@ -1839,7 +1953,6 @@ struct clk *tegra_clk_register_pllss(const char *name, const char *parent_name,
 		return ERR_PTR(-EINVAL);
 	}
 
-	pll_params->flags = TEGRA_PLL_HAS_LOCK_ENABLE | TEGRA_PLL_USE_LOCK;
 	pll = _tegra_init_pll(clk_base, NULL, pll_params, lock);
 	if (IS_ERR(pll))
 		return ERR_CAST(pll);
@@ -1874,6 +1987,345 @@ struct clk *tegra_clk_register_pllss(const char *name, const char *parent_name,
 	pll_writel(PLLSS_CTRL1_DEFAULT, pll_params->ext_misc_reg[2], pll);
 
 	val = pll_readl_base(pll);
+	val_iddq = readl_relaxed(clk_base + pll_params->iddq_reg);
+	if (val & PLL_BASE_ENABLE) {
+		if (val_iddq & BIT(pll_params->iddq_bit_idx)) {
+			WARN(1, "%s is on but IDDQ set\n", name);
+			kfree(pll);
+			return ERR_PTR(-EINVAL);
+		}
+	} else {
+		val_iddq |= BIT(pll_params->iddq_bit_idx);
+		writel_relaxed(val_iddq, clk_base + pll_params->iddq_reg);
+	}
+
+	val &= ~PLLSS_LOCK_OVERRIDE;
+	pll_writel_base(val, pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+					&tegra_clk_pllss_ops);
+
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+#endif
+
+#if defined(CONFIG_ARCH_TEGRA_210_SOC)
+static int clk_plle_tegra210_enable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct tegra_clk_pll_freq_table sel;
+	u32 val;
+	int ret = 0;
+	unsigned long flags = 0;
+	unsigned long input_rate = clk_hw_get_rate(clk_hw_get_parent(hw));
+
+	if (_get_table_rate(hw, &sel, pll->params->fixed_rate, input_rate))
+		return -EINVAL;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	val = pll_readl(pll->params->aux_reg, pll);
+	if (val & PLLE_AUX_SEQ_ENABLE)
+		goto out;
+
+	val = pll_readl_base(pll);
+	val &= ~BIT(30); /* Disable lock override */
+	pll_writel_base(val, pll);
+
+	val = pll_readl_misc(pll);
+	val |= PLLE_MISC_LOCK_ENABLE;
+	val |= PLLE_MISC_IDDQ_SW_CTRL;
+	val &= ~PLLE_MISC_IDDQ_SW_VALUE;
+	val |= PLLE_MISC_PLLE_PTS;
+	val &= ~(PLLE_MISC_VREG_BG_CTRL_MASK | PLLE_MISC_VREG_CTRL_MASK);
+	pll_writel_misc(val, pll);
+	udelay(5);
+
+	val = pll_readl(PLLE_SS_CTRL, pll);
+	val |= PLLE_SS_DISABLE;
+	pll_writel(val, PLLE_SS_CTRL, pll);
+
+	val = pll_readl_base(pll);
+	val &= ~(divp_mask_shifted(pll) | divn_mask_shifted(pll) |
+		 divm_mask_shifted(pll));
+	val &= ~(PLLE_BASE_DIVCML_MASK << PLLE_BASE_DIVCML_SHIFT);
+	val |= sel.m << divm_shift(pll);
+	val |= sel.n << divn_shift(pll);
+	val |= sel.cpcon << PLLE_BASE_DIVCML_SHIFT;
+	pll_writel_base(val, pll);
+	udelay(1);
+
+	val = pll_readl_base(pll);
+	val |= PLLE_BASE_ENABLE;
+	pll_writel_base(val, pll);
+
+	ret = clk_pll_wait_for_lock(pll);
+
+	if (ret < 0)
+		goto out;
+
+	val = pll_readl(PLLE_SS_CTRL, pll);
+	val &= ~(PLLE_SS_CNTL_CENTER | PLLE_SS_CNTL_INVERT);
+	val &= ~PLLE_SS_COEFFICIENTS_MASK;
+	val |= PLLE_SS_COEFFICIENTS_VAL_TEGRA210;
+	pll_writel(val, PLLE_SS_CTRL, pll);
+	val &= ~(PLLE_SS_CNTL_SSC_BYP | PLLE_SS_CNTL_BYPASS_SS);
+	pll_writel(val, PLLE_SS_CTRL, pll);
+	udelay(1);
+	val &= ~PLLE_SS_CNTL_INTERP_RESET;
+	pll_writel(val, PLLE_SS_CTRL, pll);
+	udelay(1);
+
+	val = pll_readl_misc(pll);
+	val &= ~PLLE_MISC_IDDQ_SW_CTRL;
+	pll_writel_misc(val, pll);
+
+	val = pll_readl(pll->params->aux_reg, pll);
+	val |= (PLLE_AUX_USE_LOCKDET | PLLE_AUX_SS_SEQ_INCLUDE);
+	val &= ~(PLLE_AUX_ENABLE_SWCTL | PLLE_AUX_SS_SWCTL);
+	pll_writel(val, pll->params->aux_reg, pll);
+	udelay(1);
+	val |= PLLE_AUX_SEQ_ENABLE;
+	pll_writel(val, pll->params->aux_reg, pll);
+
+out:
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static void clk_plle_tegra210_disable(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	unsigned long flags = 0;
+	u32 val;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	/* If PLLE HW sequencer is enabled, SW should not disable PLLE */
+	val = pll_readl(pll->params->aux_reg, pll);
+	if (val & PLLE_AUX_SEQ_ENABLE)
+		goto out;
+
+	val = pll_readl_base(pll);
+	val &= ~PLLE_BASE_ENABLE;
+	pll_writel_base(val, pll);
+
+	val = pll_readl(pll->params->aux_reg, pll);
+	val |= PLLE_AUX_ENABLE_SWCTL | PLLE_AUX_SS_SWCTL;
+	pll_writel(val, pll->params->aux_reg, pll);
+
+	val = pll_readl_misc(pll);
+	val |= PLLE_MISC_IDDQ_SW_CTRL | PLLE_MISC_IDDQ_SW_VALUE;
+	pll_writel_misc(val, pll);
+	udelay(1);
+
+out:
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+}
+
+static int clk_plle_tegra210_is_enabled(struct clk_hw *hw)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	u32 val;
+
+	val = pll_readl_base(pll);
+
+	return val & PLLE_BASE_ENABLE ? 1 : 0;
+}
+
+static const struct clk_ops tegra_clk_plle_tegra210_ops = {
+	.is_enabled =  clk_plle_tegra210_is_enabled,
+	.enable = clk_plle_tegra210_enable,
+	.disable = clk_plle_tegra210_disable,
+	.recalc_rate = clk_pll_recalc_rate,
+};
+
+struct clk *tegra_clk_register_plle_tegra210(const char *name,
+				const char *parent_name,
+				void __iomem *clk_base, unsigned long flags,
+				struct tegra_clk_pll_params *pll_params,
+				spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk;
+	u32 val, val_aux;
+
+	pll = _tegra_init_pll(clk_base, NULL, pll_params, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	/* ensure parent is set to pll_re_vco */
+
+	val = pll_readl_base(pll);
+	val_aux = pll_readl(pll_params->aux_reg, pll);
+
+	if (val & PLLE_BASE_ENABLE) {
+		if ((val_aux & PLLE_AUX_PLLRE_SEL) ||
+			(val_aux & PLLE_AUX_PLLP_SEL))
+			WARN(1, "pll_e enabled with unsupported parent %s\n",
+			  (val_aux & PLLE_AUX_PLLP_SEL) ? "pllp_out0" :
+					"pll_re_vco");
+	} else {
+		val_aux &= ~(PLLE_AUX_PLLRE_SEL | PLLE_AUX_PLLP_SEL);
+		pll_writel(val_aux, pll_params->aux_reg, pll);
+	}
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_plle_tegra210_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllc_tegra210(const char *name,
+			const char *parent_name, void __iomem *clk_base,
+			void __iomem *pmc, unsigned long flags,
+			struct tegra_clk_pll_params *pll_params,
+			spinlock_t *lock)
+{
+	struct clk *parent, *clk;
+	const struct pdiv_map *p_tohw = pll_params->pdiv_tohw;
+	struct tegra_clk_pll *pll;
+	unsigned long parent_rate;
+
+	if (!p_tohw)
+		return ERR_PTR(-EINVAL);
+
+	parent = __clk_lookup(parent_name);
+	if (!parent) {
+		WARN(1, "parent clk %s of %s must be registered first\n",
+			name, parent_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	parent_rate = clk_get_rate(parent);
+
+	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
+
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
+	pll_params->flags |= TEGRA_PLL_BYPASS;
+	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pll_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllxc_tegra210(const char *name,
+			const char *parent_name, void __iomem *clk_base,
+			void __iomem *pmc, unsigned long flags,
+			struct tegra_clk_pll_params *pll_params,
+			spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk, *parent;
+	unsigned long parent_rate;
+
+	parent = __clk_lookup(parent_name);
+	if (!parent) {
+		WARN(1, "parent clk %s of %s must be registered first\n",
+			name, parent_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (!pll_params->pdiv_tohw)
+		return ERR_PTR(-EINVAL);
+
+	parent_rate = clk_get_rate(parent);
+
+	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
+
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
+	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pll_ops);
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllss_tegra210(const char *name,
+				const char *parent_name, void __iomem *clk_base,
+				unsigned long flags,
+				struct tegra_clk_pll_params *pll_params,
+				spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk, *parent;
+	struct tegra_clk_pll_freq_table cfg;
+	unsigned long parent_rate;
+	u32 val;
+	int i;
+
+	if (!pll_params->div_nmp)
+		return ERR_PTR(-EINVAL);
+
+	parent = __clk_lookup(parent_name);
+	if (!parent) {
+		WARN(1, "parent clk %s of %s must be registered first\n",
+			name, parent_name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	pll = _tegra_init_pll(clk_base, NULL, pll_params, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	val = pll_readl_base(pll);
+	val &= ~PLLSS_REF_SRC_SEL_MASK;
+	pll_writel_base(val, pll);
+
+	parent_rate = clk_get_rate(parent);
+
+	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
+
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
+	/* initialize PLL to minimum rate */
+
+	cfg.m = _pll_fixed_mdiv(pll_params, parent_rate);
+	cfg.n = cfg.m * pll_params->vco_min / parent_rate;
+
+	for (i = 0; pll_params->pdiv_tohw[i].pdiv; i++)
+		;
+	if (!i) {
+		kfree(pll);
+		return ERR_PTR(-EINVAL);
+	}
+
+	cfg.p = pll_params->pdiv_tohw[i-1].hw_val;
+
+	_update_pll_mnp(pll, &cfg);
+
+	pll_writel_misc(PLLSS_MISC_DEFAULT, pll);
+
+	val = pll_readl_base(pll);
 	if (val & PLL_BASE_ENABLE) {
 		if (val & BIT(pll_params->iddq_bit_idx)) {
 			WARN(1, "%s is on but IDDQ set\n", name);
@@ -1887,8 +2339,50 @@ struct clk *tegra_clk_register_pllss(const char *name, const char *parent_name,
 	pll_writel_base(val, pll);
 
 	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
-					&tegra_clk_pllss_ops);
+					&tegra_clk_pll_ops);
+
+	if (IS_ERR(clk))
+		kfree(pll);
+
+	return clk;
+}
+
+struct clk *tegra_clk_register_pllmb(const char *name, const char *parent_name,
+			  void __iomem *clk_base, void __iomem *pmc,
+			  unsigned long flags,
+			  struct tegra_clk_pll_params *pll_params,
+			  spinlock_t *lock)
+{
+	struct tegra_clk_pll *pll;
+	struct clk *clk, *parent;
+	unsigned long parent_rate;
 
+	if (!pll_params->pdiv_tohw)
+		return ERR_PTR(-EINVAL);
+
+	parent = __clk_lookup(parent_name);
+	if (!parent) {
+		WARN(1, "parent clk %s of %s must be registered first\n",
+			parent_name, name);
+		return ERR_PTR(-EINVAL);
+	}
+
+	parent_rate = clk_get_rate(parent);
+
+	pll_params->vco_min = _clip_vco_min(pll_params->vco_min, parent_rate);
+
+	if (pll_params->adjust_vco)
+		pll_params->vco_min = pll_params->adjust_vco(pll_params,
+							     parent_rate);
+
+	pll_params->flags |= TEGRA_PLL_BYPASS;
+	pll_params->flags |= TEGRA_PLLMB;
+	pll = _tegra_init_pll(clk_base, pmc, pll_params, lock);
+	if (IS_ERR(pll))
+		return ERR_CAST(pll);
+
+	clk = _tegra_clk_register_pll(pll, name, parent_name, flags,
+				      &tegra_clk_pll_ops);
 	if (IS_ERR(clk))
 		kfree(pll);
 
diff --git a/drivers/clk/tegra/clk-tegra-periph.c b/drivers/clk/tegra/clk-tegra-periph.c
index cb6ab830941d..ea2b9cbf9e70 100644
--- a/drivers/clk/tegra/clk-tegra-periph.c
+++ b/drivers/clk/tegra/clk-tegra-periph.c
@@ -124,6 +124,20 @@
 #define CLK_SOURCE_HDMI_AUDIO 0x668
 #define CLK_SOURCE_VIC03 0x678
 #define CLK_SOURCE_CLK72MHZ 0x66c
+#define CLK_SOURCE_DBGAPB 0x718
+#define CLK_SOURCE_NVENC 0x6a0
+#define CLK_SOURCE_NVDEC 0x698
+#define CLK_SOURCE_NVJPG 0x69c
+#define CLK_SOURCE_APE 0x6c0
+#define CLK_SOURCE_SOR1 0x410
+#define CLK_SOURCE_SDMMC_LEGACY 0x694
+#define CLK_SOURCE_QSPI 0x6c4
+#define CLK_SOURCE_VI_I2C 0x6c8
+#define CLK_SOURCE_MIPIBIF 0x660
+#define CLK_SOURCE_UARTAPE 0x710
+#define CLK_SOURCE_TSECB 0x6d8
+#define CLK_SOURCE_MAUD 0x6d4
+#define CLK_SOURCE_USB2_HSIC_TRK 0x6cc
 
 #define MASK(x) (BIT(x) - 1)
 
@@ -182,6 +196,13 @@
 			TEGRA_DIVIDER_ROUND_UP, _clk_num, 0, _clk_id,\
 			_parents##_idx, 0, NULL)
 
+#define UART8(_name, _parents, _offset,\
+			     _clk_num, _clk_id)			\
+	TEGRA_INIT_DATA_TABLE(_name, NULL, NULL, _parents, _offset,\
+			29, MASK(3), 0, 0, 16, 1, TEGRA_DIVIDER_UART| \
+			TEGRA_DIVIDER_ROUND_UP, _clk_num, 0, _clk_id,\
+			_parents##_idx, 0, NULL)
+
 #define I2C(_name, _parents, _offset,\
 			     _clk_num, _clk_id)			\
 	TEGRA_INIT_DATA_TABLE(_name, NULL, NULL, _parents, _offset,\
@@ -221,8 +242,21 @@
 		.flags = _flags						\
 	}
 
+#define DIV8(_name, _parent_name, _offset, _clk_id, _flags)		\
+	{								\
+		.name = _name,						\
+		.clk_id = _clk_id,					\
+		.p.parent_name = _parent_name,				\
+		.periph = TEGRA_CLK_PERIPH(0, 0, 0, 0, 8, 1,		\
+				TEGRA_DIVIDER_ROUND_UP, 0, 0,		\
+				NULL, NULL),				\
+		.offset = _offset,					\
+		.flags = _flags,					\
+	}
+
 #define PLLP_BASE 0xa0
 #define PLLP_MISC 0xac
+#define PLLP_MISC1 0x680
 #define PLLP_OUTA 0xa4
 #define PLLP_OUTB 0xa8
 #define PLLP_OUTC 0x67c
@@ -234,6 +268,7 @@ static DEFINE_SPINLOCK(PLLP_OUTA_lock);
 static DEFINE_SPINLOCK(PLLP_OUTB_lock);
 static DEFINE_SPINLOCK(PLLP_OUTC_lock);
 static DEFINE_SPINLOCK(sor0_lock);
+static DEFINE_SPINLOCK(sor1_lock);
 
 #define MUX_I2S_SPDIF(_id)						\
 static const char *mux_pllaout0_##_id##_2x_pllp_clkm[] = { "pll_a_out0", \
@@ -285,6 +320,68 @@ static u32 mux_pllp_clkm_idx[] = {
 	[0] = 0, [1] = 3,
 };
 
+static const char *mux_pllp_clkm_2[] = {
+	"pll_p", "clk_m"
+};
+static u32 mux_pllp_clkm_2_idx[] = {
+	[0] = 2, [1] = 6,
+};
+
+static const char *mux_pllc2_c_c3_pllp_plla1_clkm[] = {
+	"pll_c2", "pll_c", "pll_c3", "pll_p", "pll_a1", "clk_m"
+};
+static u32 mux_pllc2_c_c3_pllp_plla1_clkm_idx[] = {
+	[0] = 1, [1] = 2, [2] = 3, [3] = 4, [4] = 6, [5] = 7,
+};
+
+static const char *
+mux_pllc4_out1_pllc_pllc4_out2_pllp_clkm_plla_pllc4_out0[] = {
+	"pll_c4_out1", "pll_c", "pll_c4_out2", "pll_p", "clk_m",
+	"pll_a_out0", "pll_c4_out0"
+};
+static u32 mux_pllc4_out1_pllc_pllc4_out2_pllp_clkm_plla_pllc4_out0_idx[] = {
+	[0] = 0, [1] = 2, [2] = 3, [3] = 4, [4] = 5, [5] = 6, [6] = 7,
+};
+
+static const char *mux_pllc_pllp_plla[] = {
+	"pll_c", "pll_p", "pll_a_out0"
+};
+static u32 mux_pllc_pllp_plla_idx[] = {
+	[0] = 1, [1] = 2, [2] = 3,
+};
+
+static const char *mux_clkm_pllc_pllp_plla[] = {
+	"clk_m", "pll_c", "pll_p", "pll_a_out0"
+};
+#define mux_clkm_pllc_pllp_plla_idx NULL
+
+static const char *mux_pllc_pllp_plla1_pllc2_c3_clkm[] = {
+	"pll_c", "pll_p", "pll_a1", "pll_c2", "pll_c3", "clk_m"
+};
+static u32 mux_pllc_pllp_plla1_pllc2_c3_clkm_idx[] = {
+	[0] = 1, [1] = 2, [2] = 3, [3] = 4, [4] = 5, [5] = 6,
+};
+
+static const char *mux_pllc2_c_c3_pllp_clkm_plla1_pllc4[] = {
+	"pll_c2", "pll_c", "pll_c3", "pll_p", "clk_m", "pll_a1", "pll_c4_out0",
+};
+static u32 mux_pllc2_c_c3_pllp_clkm_plla1_pllc4_idx[] = {
+	[0] = 1, [1] = 2, [2] = 3, [3] = 4, [4] = 5, [5] = 6, [6] = 7,
+};
+
+static const char *mux_pllc_pllp_plla1_pllc2_c3_clkm_pllc4[] = {
+	"pll_c", "pll_p", "pll_a1", "pll_c2", "pll_c3", "clk_m", "pll_c4_out0",
+};
+#define mux_pllc_pllp_plla1_pllc2_c3_clkm_pllc4_idx \
+	mux_pllc2_c_c3_pllp_clkm_plla1_pllc4_idx
+
+static const char *
+mux_plla_pllc4_out0_pllc_pllc4_out1_pllp_pllc4_out2_clkm[] = {
+	"pll_a_out0", "pll_c4_out0", "pll_c", "pll_c4_out1", "pll_p",
+	"pll_c4_out2", "clk_m"
+};
+#define mux_plla_pllc4_out0_pllc_pllc4_out1_pllp_pllc4_out2_clkm_idx NULL
+
 static const char *mux_pllm_pllc2_c_c3_pllp_plla[] = {
 	"pll_m", "pll_c2", "pll_c", "pll_c3", "pll_p", "pll_a_out0"
 };
@@ -302,12 +399,93 @@ static const char *mux_pllm_pllc_pllp_plla[] = {
 #define mux_pllm_pllc_pllp_plla_idx mux_pllp_pllc_pllm_clkm_idx
 
 static const char *mux_pllp_pllc_clkm[] = {
-	"pll_p", "pll_c", "pll_m"
+	"pll_p", "pll_c", "clk_m"
 };
 static u32 mux_pllp_pllc_clkm_idx[] = {
 	[0] = 0, [1] = 1, [2] = 3,
 };
 
+static const char *mux_pllp_pllc_clkm_1[] = {
+	"pll_p", "pll_c", "clk_m"
+};
+static u32 mux_pllp_pllc_clkm_1_idx[] = {
+	[0] = 0, [1] = 2, [2] = 5,
+};
+
+static const char *mux_pllp_pllc_plla_clkm[] = {
+	"pll_p", "pll_c", "pll_a_out0", "clk_m"
+};
+static u32 mux_pllp_pllc_plla_clkm_idx[] = {
+	[0] = 0, [1] = 2, [2] = 4, [3] = 6,
+};
+
+static const char *mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2[] = {
+	"pll_p", "pll_c", "pll_c4_out0", "pll_c4_out1", "clk_m", "pll_c4_out2"
+};
+static u32 mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2_idx[] = {
+	[0] = 0, [1] = 2, [2] = 3, [3] = 5, [4] = 6, [5] = 7,
+};
+
+static const char *
+mux_pllp_pllc_pllc_out1_pllc4_out2_pllc4_out1_clkm_pllc4_out0[] = {
+	"pll_p", "pll_c_out1", "pll_c", "pll_c4_out2", "pll_c4_out1",
+	"clk_m", "pll_c4_out0"
+};
+static u32
+mux_pllp_pllc_pllc_out1_pllc4_out2_pllc4_out1_clkm_pllc4_out0_idx[] = {
+	[0] = 0, [1] = 1, [2] = 2, [3] = 4, [4] = 5, [5] = 6, [6] = 7,
+};
+
+static const char *mux_pllp_pllc4_out2_pllc4_out1_clkm_pllc4_out0[] = {
+	"pll_p", "pll_c4_out2", "pll_c4_out1", "clk_m", "pll_c4_out0"
+};
+static u32 mux_pllp_pllc4_out2_pllc4_out1_clkm_pllc4_out0_idx[] = {
+	[0] = 0, [1] = 3, [2] = 4, [3] = 6, [4] = 7,
+};
+
+static const char *mux_pllp_clkm_pllc4_out2_out1_out0_lj[] = {
+	"pll_p",
+	"pll_c4_out2", "pll_c4_out0",	/* LJ input */
+	"pll_c4_out2", "pll_c4_out1",
+	"pll_c4_out1",			/* LJ input */
+	"clk_m", "pll_c4_out0"
+};
+#define mux_pllp_clkm_pllc4_out2_out1_out0_lj_idx NULL
+
+static const char *mux_pllp_pllc2_c_c3_clkm[] = {
+	"pll_p", "pll_c2", "pll_c", "pll_c3", "clk_m"
+};
+static u32 mux_pllp_pllc2_c_c3_clkm_idx[] = {
+	[0] = 0, [1] = 1, [2] = 2, [3] = 3, [4] = 6,
+};
+
+static const char *mux_pllp_clkm_clk32_plle[] = {
+	"pll_p", "clk_m", "clk_32k", "pll_e"
+};
+static u32 mux_pllp_clkm_clk32_plle_idx[] = {
+	[0] = 0, [1] = 2, [2] = 4, [3] = 6,
+};
+
+static const char *mux_pllp_pllp_out3_clkm_clk32k_plla[] = {
+	"pll_p", "pll_p_out3", "clk_m", "clk_32k", "pll_a_out0"
+};
+#define mux_pllp_pllp_out3_clkm_clk32k_plla_idx NULL
+
+static const char *mux_pllp_out3_clkm_pllp_pllc4[] = {
+	"pll_p_out3", "clk_m", "pll_p", "pll_c4_out0", "pll_c4_out1",
+	"pll_c4_out2"
+};
+static u32 mux_pllp_out3_clkm_pllp_pllc4_idx[] = {
+	[0] = 0, [1] = 3, [2] = 4, [3] = 5, [4] = 6, [5] = 7,
+};
+
+static const char *mux_clkm_pllp_pllre[] = {
+	"clk_m", "pll_p_out_xusb", "pll_re_out"
+};
+static u32 mux_clkm_pllp_pllre_idx[] = {
+	[0] = 0, [1] = 1, [2] = 5,
+};
+
 static const char *mux_pllp_pllc_clkm_clk32[] = {
 	"pll_p", "pll_c", "clk_m", "clk_32k"
 };
@@ -332,6 +510,11 @@ static u32 mux_clkm_48M_pllp_480M_idx[] = {
 	[0] = 0, [1] = 2, [2] = 4, [3] = 6,
 };
 
+static const char *mux_clkm_pllre_clk32_480M[] = {
+	"clk_m", "pll_re_out", "clk_32k", "pll_u_480M"
+};
+#define mux_clkm_pllre_clk32_480M_idx NULL
+
 static const char *mux_clkm_pllre_clk32_480M_pllc_ref[] = {
 	"clk_m", "pll_re_out", "clk_32k", "pll_u_480M", "pll_c", "pll_ref"
 };
@@ -339,10 +522,27 @@ static u32 mux_clkm_pllre_clk32_480M_pllc_ref_idx[] = {
 	[0] = 0, [1] = 1, [2] = 3, [3] = 3, [4] = 4, [5] = 7,
 };
 
-static const char *mux_ss_60M[] = {
+static const char *mux_pllp_out3_pllp_pllc_clkm[] = {
+	"pll_p_out3", "pll_p", "pll_c", "clk_m"
+};
+static u32 mux_pllp_out3_pllp_pllc_clkm_idx[] = {
+	[0] = 0, [1] = 1, [2] = 2, [3] = 6,
+};
+
+static const char *mux_ss_div2_60M[] = {
 	"xusb_ss_div2", "pll_u_60M"
 };
-#define mux_ss_60M_idx NULL
+#define mux_ss_div2_60M_idx NULL
+
+static const char *mux_ss_div2_60M_ss[] = {
+	"xusb_ss_div2", "pll_u_60M", "xusb_ss_src"
+};
+#define mux_ss_div2_60M_ss_idx NULL
+
+static const char *mux_ss_clkm[] = {
+	"xusb_ss_src", "clk_m"
+};
+#define mux_ss_clkm_idx NULL
 
 static const char *mux_d_audio_clk[] = {
 	"pll_a_out0", "pll_p", "clk_m", "spdif_in_sync", "i2s0_sync",
@@ -386,6 +586,32 @@ static u32 mux_pllm_pllc2_c_c3_pllp_plla_pllc4_idx[] = {
 	[0] = 0, [1] = 1, [2] = 2, [3] = 3, [4] = 4, [5] = 6, [6] = 7,
 };
 
+/* SOR1 mux'es */
+static const char *mux_pllp_plld_plld2_clkm[] = {
+	"pll_p", "pll_d_out0", "pll_d2_out0", "clk_m"
+};
+static u32 mux_pllp_plld_plld2_clkm_idx[] = {
+	[0] = 0, [1] = 2, [2] = 5, [3] = 6
+};
+
+static const char *mux_plldp_sor1_src[] = {
+	"pll_dp", "clk_sor1_src"
+};
+#define mux_plldp_sor1_src_idx NULL
+
+static const char *mux_clkm_sor1_brick_sor1_src[] = {
+	"clk_m", "sor1_brick", "sor1_src", "sor1_brick"
+};
+#define mux_clkm_sor1_brick_sor1_src_idx NULL
+
+static const char *mux_pllp_pllre_clkm[] = {
+	"pll_p", "pll_re_out1", "clk_m"
+};
+
+static u32 mux_pllp_pllre_clkm_idx[] = {
+	[0] = 0, [1] = 2, [2] = 3,
+};
+
 static const char *mux_clkm_plldp_sor0lvds[] = {
 	"clk_m", "pll_dp", "sor0_lvds",
 };
@@ -401,6 +627,7 @@ static struct tegra_periph_init_data periph_clks[] = {
 	I2C("i2c3", mux_pllp_clkm, CLK_SOURCE_I2C3, 67, tegra_clk_i2c3),
 	I2C("i2c4", mux_pllp_clkm, CLK_SOURCE_I2C4, 103, tegra_clk_i2c4),
 	I2C("i2c5", mux_pllp_clkm, CLK_SOURCE_I2C5, 47, tegra_clk_i2c5),
+	I2C("i2c6", mux_pllp_clkm, CLK_SOURCE_I2C6, 166, tegra_clk_i2c6),
 	INT("vde", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde),
 	INT("vi", mux_pllm_pllc_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi),
 	INT("epp", mux_pllm_pllc_pllp_plla, CLK_SOURCE_EPP, 19, 0, tegra_clk_epp),
@@ -411,14 +638,19 @@ static struct tegra_periph_init_data periph_clks[] = {
 	INT8("vde", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_VDE, 61, 0, tegra_clk_vde_8),
 	INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_8),
 	INT8("vi", mux_pllm_pllc2_c_c3_pllp_plla_pllc4, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_9),
+	INT8("vi", mux_pllc2_c_c3_pllp_clkm_plla1_pllc4, CLK_SOURCE_VI, 20, 0, tegra_clk_vi_10),
 	INT8("epp", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_EPP, 19, 0, tegra_clk_epp_8),
 	INT8("msenc", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_MSENC, 91, TEGRA_PERIPH_WAR_1005168, tegra_clk_msenc),
 	INT8("tsec", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_TSEC, 83, 0, tegra_clk_tsec),
+	INT("tsec", mux_pllp_pllc_clkm, CLK_SOURCE_TSEC, 83, 0, tegra_clk_tsec_8),
 	INT8("host1x", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_HOST1X, 28, 0, tegra_clk_host1x_8),
+	INT8("host1x", mux_pllc4_out1_pllc_pllc4_out2_pllp_clkm_plla_pllc4_out0, CLK_SOURCE_HOST1X, 28, 0, tegra_clk_host1x_9),
 	INT8("se", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SE, 127, TEGRA_PERIPH_ON_APB, tegra_clk_se),
+	INT8("se", mux_pllp_pllc2_c_c3_clkm, CLK_SOURCE_SE, 127, TEGRA_PERIPH_ON_APB, tegra_clk_se),
 	INT8("2d", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_2D, 21, 0, tegra_clk_gr2d_8),
 	INT8("3d", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_3D, 24, 0, tegra_clk_gr3d_8),
 	INT8("vic03", mux_pllm_pllc_pllp_plla_pllc2_c3_clkm, CLK_SOURCE_VIC03, 178, 0, tegra_clk_vic03),
+	INT8("vic03", mux_pllc_pllp_plla1_pllc2_c3_clkm, CLK_SOURCE_VIC03, 178, 0, tegra_clk_vic03_8),
 	INT_FLAGS("mselect", mux_pllp_clkm, CLK_SOURCE_MSELECT, 99, 0, tegra_clk_mselect, CLK_IGNORE_UNUSED),
 	MUX("i2s0", mux_pllaout0_audio0_2x_pllp_clkm, CLK_SOURCE_I2S0, 30, TEGRA_PERIPH_ON_APB, tegra_clk_i2s0),
 	MUX("i2s1", mux_pllaout0_audio1_2x_pllp_clkm, CLK_SOURCE_I2S1, 11, TEGRA_PERIPH_ON_APB, tegra_clk_i2s1),
@@ -427,22 +659,31 @@ static struct tegra_periph_init_data periph_clks[] = {
 	MUX("i2s4", mux_pllaout0_audio4_2x_pllp_clkm, CLK_SOURCE_I2S4, 102, TEGRA_PERIPH_ON_APB, tegra_clk_i2s4),
 	MUX("spdif_out", mux_pllaout0_audio_2x_pllp_clkm, CLK_SOURCE_SPDIF_OUT, 10, TEGRA_PERIPH_ON_APB, tegra_clk_spdif_out),
 	MUX("spdif_in", mux_pllp_pllc_pllm, CLK_SOURCE_SPDIF_IN, 10, TEGRA_PERIPH_ON_APB, tegra_clk_spdif_in),
+	MUX8("spdif_in", mux_pllp_pllc_clkm_1, CLK_SOURCE_SPDIF_IN, 10, TEGRA_PERIPH_ON_APB, tegra_clk_spdif_in_8),
 	MUX("pwm", mux_pllp_pllc_clk32_clkm, CLK_SOURCE_PWM, 17, TEGRA_PERIPH_ON_APB, tegra_clk_pwm),
 	MUX("adx", mux_plla_pllc_pllp_clkm, CLK_SOURCE_ADX, 154, TEGRA_PERIPH_ON_APB, tegra_clk_adx),
 	MUX("amx", mux_plla_pllc_pllp_clkm, CLK_SOURCE_AMX, 153, TEGRA_PERIPH_ON_APB, tegra_clk_amx),
 	MUX("hda", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_HDA, 125, TEGRA_PERIPH_ON_APB, tegra_clk_hda),
+	MUX("hda", mux_pllp_pllc_clkm, CLK_SOURCE_HDA, 125, TEGRA_PERIPH_ON_APB, tegra_clk_hda_8),
 	MUX("hda2codec_2x", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_HDA2CODEC_2X, 111, TEGRA_PERIPH_ON_APB, tegra_clk_hda2codec_2x),
+	MUX8("hda2codec_2x", mux_pllp_pllc_plla_clkm, CLK_SOURCE_HDA2CODEC_2X, 111, TEGRA_PERIPH_ON_APB, tegra_clk_hda2codec_2x_8),
 	MUX("vfir", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_VFIR, 7, TEGRA_PERIPH_ON_APB, tegra_clk_vfir),
 	MUX("sdmmc1", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC1, 14, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc1),
 	MUX("sdmmc2", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC2, 9, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc2),
 	MUX("sdmmc3", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC3, 69, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc3),
 	MUX("sdmmc4", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SDMMC4, 15, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc4),
+	MUX8("sdmmc1", mux_pllp_pllc4_out2_pllc4_out1_clkm_pllc4_out0, CLK_SOURCE_SDMMC1, 14, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc1_9),
+	MUX8("sdmmc2", mux_pllp_clkm_pllc4_out2_out1_out0_lj, CLK_SOURCE_SDMMC2, 9, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc2_9),
+	MUX8("sdmmc3", mux_pllp_pllc4_out2_pllc4_out1_clkm_pllc4_out0, CLK_SOURCE_SDMMC3, 69, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc3_9),
+	MUX8("sdmmc4", mux_pllp_clkm_pllc4_out2_out1_out0_lj, CLK_SOURCE_SDMMC4, 15, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc4_9),
 	MUX("la", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_LA, 76, TEGRA_PERIPH_ON_APB, tegra_clk_la),
 	MUX("trace", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_TRACE, 77, TEGRA_PERIPH_ON_APB, tegra_clk_trace),
 	MUX("owr", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_OWR, 71, TEGRA_PERIPH_ON_APB, tegra_clk_owr),
+	MUX("owr", mux_pllp_pllc_clkm, CLK_SOURCE_OWR, 71, TEGRA_PERIPH_ON_APB, tegra_clk_owr_8),
 	MUX("nor", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_NOR, 42, 0, tegra_clk_nor),
 	MUX("mipi", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_MIPI, 50, TEGRA_PERIPH_ON_APB, tegra_clk_mipi),
 	MUX("vi_sensor", mux_pllm_pllc_pllp_plla, CLK_SOURCE_VI_SENSOR, 20, TEGRA_PERIPH_NO_RESET, tegra_clk_vi_sensor),
+	MUX("vi_sensor", mux_pllc_pllp_plla, CLK_SOURCE_VI_SENSOR, 20, TEGRA_PERIPH_NO_RESET, tegra_clk_vi_sensor_9),
 	MUX("cilab", mux_pllp_pllc_clkm, CLK_SOURCE_CILAB, 144, 0, tegra_clk_cilab),
 	MUX("cilcd", mux_pllp_pllc_clkm, CLK_SOURCE_CILCD, 145, 0, tegra_clk_cilcd),
 	MUX("cile", mux_pllp_pllc_clkm, CLK_SOURCE_CILE, 146, 0, tegra_clk_cile),
@@ -465,10 +706,13 @@ static struct tegra_periph_init_data periph_clks[] = {
 	MUX("ndflash", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_NDFLASH, 13, TEGRA_PERIPH_ON_APB, tegra_clk_ndflash),
 	MUX("ndspeed", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_NDSPEED, 80, TEGRA_PERIPH_ON_APB, tegra_clk_ndspeed),
 	MUX("sata_oob", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SATA_OOB, 123, TEGRA_PERIPH_ON_APB, tegra_clk_sata_oob),
+	MUX("sata_oob", mux_pllp_pllc_clkm, CLK_SOURCE_SATA_OOB, 123, TEGRA_PERIPH_ON_APB, tegra_clk_sata_oob_8),
 	MUX("sata", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_SATA, 124, TEGRA_PERIPH_ON_APB, tegra_clk_sata),
+	MUX("sata", mux_pllp_pllc_clkm, CLK_SOURCE_SATA, 124, TEGRA_PERIPH_ON_APB, tegra_clk_sata_8),
 	MUX("adx1", mux_plla_pllc_pllp_clkm, CLK_SOURCE_ADX1, 180, TEGRA_PERIPH_ON_APB, tegra_clk_adx1),
 	MUX("amx1", mux_plla_pllc_pllp_clkm, CLK_SOURCE_AMX1, 185, TEGRA_PERIPH_ON_APB, tegra_clk_amx1),
 	MUX("vi_sensor2", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI_SENSOR2, 165, TEGRA_PERIPH_NO_RESET, tegra_clk_vi_sensor2),
+	MUX("vi_sensor2", mux_pllc_pllp_plla, CLK_SOURCE_VI_SENSOR2, 165, TEGRA_PERIPH_NO_RESET, tegra_clk_vi_sensor2_8),
 	MUX8("sdmmc1", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SDMMC1, 14, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc1_8),
 	MUX8("sdmmc2", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SDMMC2, 9, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc2_8),
 	MUX8("sdmmc3", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SDMMC3, 69, TEGRA_PERIPH_ON_APB, tegra_clk_sdmmc3_8),
@@ -479,6 +723,10 @@ static struct tegra_periph_init_data periph_clks[] = {
 	MUX8("sbc4", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SBC4, 68, TEGRA_PERIPH_ON_APB, tegra_clk_sbc4_8),
 	MUX8("sbc5", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SBC5, 104, TEGRA_PERIPH_ON_APB, tegra_clk_sbc5_8),
 	MUX8("sbc6", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_SBC6, 105, TEGRA_PERIPH_ON_APB, tegra_clk_sbc6_8),
+	MUX("sbc1", mux_pllp_pllc_clkm, CLK_SOURCE_SBC1, 41, TEGRA_PERIPH_ON_APB, tegra_clk_sbc1_9),
+	MUX("sbc2", mux_pllp_pllc_clkm, CLK_SOURCE_SBC2, 44, TEGRA_PERIPH_ON_APB, tegra_clk_sbc2_9),
+	MUX("sbc3", mux_pllp_pllc_clkm, CLK_SOURCE_SBC3, 46, TEGRA_PERIPH_ON_APB, tegra_clk_sbc3_9),
+	MUX("sbc4", mux_pllp_pllc_clkm, CLK_SOURCE_SBC4, 68, TEGRA_PERIPH_ON_APB, tegra_clk_sbc4_9),
 	MUX8("ndflash", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_NDFLASH, 13, TEGRA_PERIPH_ON_APB, tegra_clk_ndflash_8),
 	MUX8("ndspeed", mux_pllp_pllc2_c_c3_pllm_clkm, CLK_SOURCE_NDSPEED, 80, TEGRA_PERIPH_ON_APB, tegra_clk_ndspeed_8),
 	MUX8("hdmi", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_HDMI, 51, 0, tegra_clk_hdmi),
@@ -486,27 +734,59 @@ static struct tegra_periph_init_data periph_clks[] = {
 	MUX8("extern2", mux_plla_clk32_pllp_clkm_plle, CLK_SOURCE_EXTERN2, 121, 0, tegra_clk_extern2),
 	MUX8("extern3", mux_plla_clk32_pllp_clkm_plle, CLK_SOURCE_EXTERN3, 122, 0, tegra_clk_extern3),
 	MUX8("soc_therm", mux_pllm_pllc_pllp_plla, CLK_SOURCE_SOC_THERM, 78, TEGRA_PERIPH_ON_APB, tegra_clk_soc_therm),
+	MUX8("soc_therm", mux_clkm_pllc_pllp_plla, CLK_SOURCE_SOC_THERM, 78, TEGRA_PERIPH_ON_APB, tegra_clk_soc_therm_8),
 	MUX8("vi_sensor", mux_pllm_pllc2_c_c3_pllp_plla, CLK_SOURCE_VI_SENSOR, 164, TEGRA_PERIPH_NO_RESET, tegra_clk_vi_sensor_8),
 	MUX8("isp", mux_pllm_pllc_pllp_plla_clkm_pllc4, CLK_SOURCE_ISP, 23, TEGRA_PERIPH_ON_APB, tegra_clk_isp_8),
+	MUX8("isp", mux_pllc_pllp_plla1_pllc2_c3_clkm_pllc4, CLK_SOURCE_ISP, 23, TEGRA_PERIPH_ON_APB, tegra_clk_isp_9),
 	MUX8("entropy", mux_pllp_clkm1, CLK_SOURCE_ENTROPY, 149,  0, tegra_clk_entropy),
+	MUX8("entropy", mux_pllp_clkm_clk32_plle, CLK_SOURCE_ENTROPY, 149,  0, tegra_clk_entropy_8),
 	MUX8("hdmi_audio", mux_pllp3_pllc_clkm, CLK_SOURCE_HDMI_AUDIO, 176, TEGRA_PERIPH_NO_RESET, tegra_clk_hdmi_audio),
 	MUX8("clk72mhz", mux_pllp3_pllc_clkm, CLK_SOURCE_CLK72MHZ, 177, TEGRA_PERIPH_NO_RESET, tegra_clk_clk72Mhz),
+	MUX8("clk72mhz", mux_pllp_out3_pllp_pllc_clkm, CLK_SOURCE_CLK72MHZ, 177, TEGRA_PERIPH_NO_RESET, tegra_clk_clk72Mhz_8),
 	MUX8_NOGATE_LOCK("sor0_lvds", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_SOR0, tegra_clk_sor0_lvds, &sor0_lock),
 	MUX_FLAGS("csite", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_CSITE, 73, TEGRA_PERIPH_ON_APB, tegra_clk_csite, CLK_IGNORE_UNUSED),
+	MUX_FLAGS("csite", mux_pllp_pllre_clkm, CLK_SOURCE_CSITE, 73, TEGRA_PERIPH_ON_APB, tegra_clk_csite_8, CLK_IGNORE_UNUSED),
 	NODIV("disp1", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_DISP1, 29, 7, 27, 0, tegra_clk_disp1, NULL),
+	NODIV("disp1", mux_pllp_plld_plld2_clkm, CLK_SOURCE_DISP1, 29, 7, 27, 0, tegra_clk_disp1_8, NULL),
 	NODIV("disp2", mux_pllp_pllm_plld_plla_pllc_plld2_clkm, CLK_SOURCE_DISP2, 29, 7, 26, 0, tegra_clk_disp2, NULL),
+	NODIV("disp2", mux_pllp_plld_plld2_clkm, CLK_SOURCE_DISP2, 29, 7, 26, 0, tegra_clk_disp2_8, NULL),
 	NODIV("sor0", mux_clkm_plldp_sor0lvds, CLK_SOURCE_SOR0, 14, 3, 182, 0, tegra_clk_sor0, &sor0_lock),
 	UART("uarta", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTA, 6, tegra_clk_uarta),
 	UART("uartb", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTB, 7, tegra_clk_uartb),
 	UART("uartc", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTC, 55, tegra_clk_uartc),
 	UART("uartd", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTD, 65, tegra_clk_uartd),
 	UART("uarte", mux_pllp_pllc_pllm_clkm, CLK_SOURCE_UARTE, 66, tegra_clk_uarte),
+	UART8("uarta", mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2, CLK_SOURCE_UARTA, 6, tegra_clk_uarta_8),
+	UART8("uartb", mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2, CLK_SOURCE_UARTB, 7, tegra_clk_uartb_8),
+	UART8("uartc", mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2, CLK_SOURCE_UARTC, 55, tegra_clk_uartc_8),
+	UART8("uartd", mux_pllp_pllc_pllc4_out0_pllc4_out1_clkm_pllc4_out2, CLK_SOURCE_UARTD, 65, tegra_clk_uartd_8),
 	XUSB("xusb_host_src", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_HOST_SRC, 143, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_host_src),
+	XUSB("xusb_host_src", mux_clkm_pllp_pllre, CLK_SOURCE_XUSB_HOST_SRC, 143, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_host_src_8),
 	XUSB("xusb_falcon_src", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_FALCON_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_falcon_src),
+	XUSB("xusb_falcon_src", mux_clkm_pllp_pllre, CLK_SOURCE_XUSB_FALCON_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_falcon_src_8),
 	XUSB("xusb_fs_src", mux_clkm_48M_pllp_480M, CLK_SOURCE_XUSB_FS_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_fs_src),
 	XUSB("xusb_ss_src", mux_clkm_pllre_clk32_480M_pllc_ref, CLK_SOURCE_XUSB_SS_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_ss_src),
-	NODIV("xusb_hs_src", mux_ss_60M, CLK_SOURCE_XUSB_SS_SRC, 25, MASK(1), 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_hs_src, NULL),
+	XUSB("xusb_ss_src", mux_clkm_pllre_clk32_480M, CLK_SOURCE_XUSB_SS_SRC, 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_ss_src_8),
+	NODIV("xusb_hs_src", mux_ss_div2_60M, CLK_SOURCE_XUSB_SS_SRC, 25, MASK(1), 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_hs_src, NULL),
+	NODIV("xusb_hs_src", mux_ss_div2_60M_ss, CLK_SOURCE_XUSB_SS_SRC, 25, MASK(2), 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_hs_src_4, NULL),
+	NODIV("xusb_ssp_src", mux_ss_clkm, CLK_SOURCE_XUSB_SS_SRC, 24, MASK(1), 143, TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_ssp_src, NULL),
 	XUSB("xusb_dev_src", mux_clkm_pllp_pllc_pllre, CLK_SOURCE_XUSB_DEV_SRC, 95, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_dev_src),
+	XUSB("xusb_dev_src", mux_clkm_pllp_pllre, CLK_SOURCE_XUSB_DEV_SRC, 95, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_xusb_dev_src_8),
+	MUX8("dbgapb", mux_pllp_clkm_2, CLK_SOURCE_DBGAPB, 185, TEGRA_PERIPH_NO_RESET, tegra_clk_dbgapb),
+	MUX8("nvenc", mux_pllc2_c_c3_pllp_plla1_clkm, CLK_SOURCE_NVENC, 219, 0, tegra_clk_nvenc),
+	MUX8("nvdec", mux_pllc2_c_c3_pllp_plla1_clkm, CLK_SOURCE_NVDEC, 194, 0, tegra_clk_nvdec),
+	MUX8("nvjpg", mux_pllc2_c_c3_pllp_plla1_clkm, CLK_SOURCE_NVJPG, 195, 0, tegra_clk_nvjpg),
+	MUX8("ape", mux_plla_pllc4_out0_pllc_pllc4_out1_pllp_pllc4_out2_clkm, CLK_SOURCE_APE, 198, TEGRA_PERIPH_ON_APB, tegra_clk_ape),
+	MUX8_NOGATE_LOCK("sor1_src", mux_pllp_plld_plld2_clkm, CLK_SOURCE_SOR1, tegra_clk_sor1_src, &sor1_lock),
+	NODIV("sor1_brick", mux_plldp_sor1_src, CLK_SOURCE_SOR1, 14, MASK(1), 183, 0, tegra_clk_sor1_brick, &sor1_lock),
+	NODIV("sor1", mux_clkm_sor1_brick_sor1_src, CLK_SOURCE_SOR1, 15, MASK(1), 183, 0, tegra_clk_sor1, &sor1_lock),
+	MUX8("sdmmc_legacy", mux_pllp_out3_clkm_pllp_pllc4, CLK_SOURCE_SDMMC_LEGACY, 193, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_sdmmc_legacy),
+	MUX8("qspi", mux_pllp_pllc_pllc_out1_pllc4_out2_pllc4_out1_clkm_pllc4_out0, CLK_SOURCE_QSPI, 211, TEGRA_PERIPH_ON_APB, tegra_clk_qspi),
+	I2C("vii2c", mux_pllp_pllc_clkm, CLK_SOURCE_VI_I2C, 208, tegra_clk_vi_i2c),
+	MUX("mipibif", mux_pllp_clkm, CLK_SOURCE_MIPIBIF, 173, TEGRA_PERIPH_ON_APB, tegra_clk_mipibif),
+	MUX("uartape", mux_pllp_pllc_clkm, CLK_SOURCE_UARTAPE, 212, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_uartape),
+	MUX8("tsecb", mux_pllp_pllc2_c_c3_clkm, CLK_SOURCE_TSECB, 206, 0, tegra_clk_tsecb),
+	MUX8("maud", mux_pllp_pllp_out3_clkm_clk32k_plla, CLK_SOURCE_MAUD, 202, TEGRA_PERIPH_ON_APB | TEGRA_PERIPH_NO_RESET, tegra_clk_maud),
 };
 
 static struct tegra_periph_init_data gate_clks[] = {
@@ -543,6 +823,17 @@ static struct tegra_periph_init_data gate_clks[] = {
 	GATE("pcie", "clk_m", 70, 0, tegra_clk_pcie, 0),
 	GATE("dpaux", "clk_m", 181, 0, tegra_clk_dpaux, 0),
 	GATE("gpu", "pll_ref", 184, 0, tegra_clk_gpu, 0),
+	GATE("pllg_ref", "pll_ref", 189, 0, tegra_clk_pll_g_ref, 0),
+	GATE("hsic_trk", "usb2_hsic_trk", 209, TEGRA_PERIPH_NO_RESET, tegra_clk_hsic_trk, 0),
+	GATE("usb2_trk", "usb2_hsic_trk", 210, TEGRA_PERIPH_NO_RESET, tegra_clk_usb2_trk, 0),
+	GATE("xusb_gate", "osc", 143, 0, tegra_clk_xusb_gate, 0),
+	GATE("pll_p_out_cpu", "pll_p", 223, 0, tegra_clk_pll_p_out_cpu, 0),
+	GATE("pll_p_out_adsp", "pll_p", 187, 0, tegra_clk_pll_p_out_adsp, 0),
+	GATE("apb2ape", "clk_m", 107, 0, tegra_clk_apb2ape, 0),
+};
+
+static struct tegra_periph_init_data div_clks[] = {
+	DIV8("usb2_hsic_trk", "osc", CLK_SOURCE_USB2_HSIC_TRK, tegra_clk_usb2_hsic_trk, 0),
 };
 
 struct pll_out_data {
@@ -633,6 +924,33 @@ static void __init gate_clk_init(void __iomem *clk_base,
 	}
 }
 
+static void __init div_clk_init(void __iomem *clk_base,
+				struct tegra_clk *tegra_clks)
+{
+	int i;
+	struct clk *clk;
+	struct clk **dt_clk;
+
+	for (i = 0; i < ARRAY_SIZE(div_clks); i++) {
+		struct tegra_periph_init_data *data;
+
+		data = div_clks + i;
+
+		dt_clk = tegra_lookup_dt_id(data->clk_id, tegra_clks);
+		if (!dt_clk)
+			continue;
+
+		clk = tegra_clk_register_divider(data->name,
+				data->p.parent_name, clk_base + data->offset,
+				data->flags, data->periph.divider.flags,
+				data->periph.divider.shift,
+				data->periph.divider.width,
+				data->periph.divider.frac_width,
+				data->periph.divider.lock);
+		*dt_clk = clk;
+	}
+}
+
 static void __init init_pllp(void __iomem *clk_base, void __iomem *pmc_base,
 				struct tegra_clk *tegra_clks,
 				struct tegra_clk_pll_params *pll_params)
@@ -669,6 +987,51 @@ static void __init init_pllp(void __iomem *clk_base, void __iomem *pmc_base,
 				data->lock);
 		*dt_clk = clk;
 	}
+
+	dt_clk = tegra_lookup_dt_id(tegra_clk_pll_p_out_cpu,
+			tegra_clks);
+	if (dt_clk) {
+		/*
+		 * Tegra210 has control on enabling/disabling PLLP branches to
+		 * CPU, register a gate clock "pll_p_out_cpu" for this gating
+		 * function and parent "pll_p_out4" to it, so when we are
+		 * re-parenting CPU off from "pll_p_out4" the PLLP branching to
+		 * CPU can be disabled automatically.
+		 */
+		clk = tegra_clk_register_divider("pll_p_out4_div",
+				"pll_p_out_cpu", clk_base + PLLP_OUTB, 0, 0, 24,
+				8, 1, &PLLP_OUTB_lock);
+
+		dt_clk = tegra_lookup_dt_id(tegra_clk_pll_p_out4_cpu, tegra_clks);
+		if (dt_clk) {
+			clk = tegra_clk_register_pll_out("pll_p_out4",
+					"pll_p_out4_div", clk_base + PLLP_OUTB,
+					17, 16, CLK_IGNORE_UNUSED |
+					CLK_SET_RATE_PARENT, 0,
+					&PLLP_OUTB_lock);
+			*dt_clk = clk;
+		}
+	}
+
+	dt_clk = tegra_lookup_dt_id(tegra_clk_pll_p_out_hsio, tegra_clks);
+	if (dt_clk) {
+		/* PLLP_OUT_HSIO */
+		clk = clk_register_gate(NULL, "pll_p_out_hsio", "pll_p",
+				CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED,
+				clk_base + PLLP_MISC1, 29, 0, NULL);
+		*dt_clk = clk;
+	}
+
+	dt_clk = tegra_lookup_dt_id(tegra_clk_pll_p_out_xusb, tegra_clks);
+	if (dt_clk) {
+		/* PLLP_OUT_XUSB */
+		clk = clk_register_gate(NULL, "pll_p_out_xusb",
+				"pll_p_out_hsio", CLK_SET_RATE_PARENT |
+				CLK_IGNORE_UNUSED, clk_base + PLLP_MISC1, 28, 0,
+				NULL);
+		clk_register_clkdev(clk, "pll_p_out_xusb", NULL);
+		*dt_clk = clk;
+	}
 }
 
 void __init tegra_periph_clk_init(void __iomem *clk_base,
@@ -678,4 +1041,5 @@ void __init tegra_periph_clk_init(void __iomem *clk_base,
 	init_pllp(clk_base, pmc_base, tegra_clks, pll_params);
 	periph_clk_init(clk_base, tegra_clks);
 	gate_clk_init(clk_base, tegra_clks);
+	div_clk_init(clk_base, tegra_clks);
 }
diff --git a/drivers/clk/tegra/clk-tegra-super-gen4.c b/drivers/clk/tegra/clk-tegra-super-gen4.c
index 5b1d723932c5..474de0f0c26d 100644
--- a/drivers/clk/tegra/clk-tegra-super-gen4.c
+++ b/drivers/clk/tegra/clk-tegra-super-gen4.c
@@ -34,9 +34,25 @@
 #define CCLKLP_BURST_POLICY 0x370
 #define SCLK_BURST_POLICY 0x028
 #define SYSTEM_CLK_RATE 0x030
+#define SCLK_DIVIDER 0x2c
 
 static DEFINE_SPINLOCK(sysrate_lock);
 
+enum tegra_super_gen {
+	gen4 = 4,
+	gen5,
+};
+
+struct tegra_super_gen_info {
+	enum tegra_super_gen gen;
+	const char **sclk_parents;
+	const char **cclk_g_parents;
+	const char **cclk_lp_parents;
+	int num_sclk_parents;
+	int num_cclk_g_parents;
+	int num_cclk_lp_parents;
+};
+
 static const char *sclk_parents[] = { "clk_m", "pll_c_out1", "pll_p_out4",
 			       "pll_p", "pll_p_out2", "unused",
 			       "clk_32k", "pll_m_out1" };
@@ -51,21 +67,81 @@ static const char *cclk_lp_parents[] = { "clk_m", "pll_c", "clk_32k", "pll_m",
 					 "pll_p", "pll_p_out4", "unused",
 					 "unused", "pll_x", "pll_x_out0" };
 
+static const struct tegra_super_gen_info tegra_super_gen_info_gen4 = {
+	.gen = gen4,
+	.sclk_parents = sclk_parents,
+	.cclk_g_parents = cclk_g_parents,
+	.cclk_lp_parents = cclk_lp_parents,
+	.num_sclk_parents = ARRAY_SIZE(sclk_parents),
+	.num_cclk_g_parents = ARRAY_SIZE(cclk_g_parents),
+	.num_cclk_lp_parents = ARRAY_SIZE(cclk_lp_parents),
+};
+
+static const char *sclk_parents_gen5[] = { "clk_m", "pll_c_out1", "pll_c4_out3",
+			       "pll_p", "pll_p_out2", "pll_c4_out1",
+			       "clk_32k", "pll_c4_out2" };
+
+static const char *cclk_g_parents_gen5[] = { "clk_m", "unused", "clk_32k", "unused",
+					"pll_p", "pll_p_out4", "unused",
+					"unused", "pll_x", "unused", "unused",
+					"unused", "unused", "unused", "unused",
+					"dfllCPU_out" };
+
+static const char *cclk_lp_parents_gen5[] = { "clk_m", "unused", "clk_32k", "unused",
+					"pll_p", "pll_p_out4", "unused",
+					"unused", "pll_x", "unused", "unused",
+					"unused", "unused", "unused", "unused",
+					"dfllCPU_out" };
+
+static const struct tegra_super_gen_info tegra_super_gen_info_gen5 = {
+	.gen = gen5,
+	.sclk_parents = sclk_parents_gen5,
+	.cclk_g_parents = cclk_g_parents_gen5,
+	.cclk_lp_parents = cclk_lp_parents_gen5,
+	.num_sclk_parents = ARRAY_SIZE(sclk_parents_gen5),
+	.num_cclk_g_parents = ARRAY_SIZE(cclk_g_parents_gen5),
+	.num_cclk_lp_parents = ARRAY_SIZE(cclk_lp_parents_gen5),
+};
+
 static void __init tegra_sclk_init(void __iomem *clk_base,
-				struct tegra_clk *tegra_clks)
+				struct tegra_clk *tegra_clks,
+				const struct tegra_super_gen_info *gen_info)
 {
 	struct clk *clk;
 	struct clk **dt_clk;
 
-	/* SCLK */
-	dt_clk = tegra_lookup_dt_id(tegra_clk_sclk, tegra_clks);
+	/* SCLK_MUX */
+	dt_clk = tegra_lookup_dt_id(tegra_clk_sclk_mux, tegra_clks);
 	if (dt_clk) {
-		clk = tegra_clk_register_super_mux("sclk", sclk_parents,
-						ARRAY_SIZE(sclk_parents),
+		clk = tegra_clk_register_super_mux("sclk_mux",
+						gen_info->sclk_parents,
+						gen_info->num_sclk_parents,
 						CLK_SET_RATE_PARENT,
 						clk_base + SCLK_BURST_POLICY,
 						0, 4, 0, 0, NULL);
 		*dt_clk = clk;
+
+
+		/* SCLK */
+		dt_clk = tegra_lookup_dt_id(tegra_clk_sclk, tegra_clks);
+		if (dt_clk) {
+			clk = clk_register_divider(NULL, "sclk", "sclk_mux", 0,
+						clk_base + SCLK_DIVIDER, 0, 8,
+						0, &sysrate_lock);
+			*dt_clk = clk;
+		}
+	} else {
+		/* SCLK */
+		dt_clk = tegra_lookup_dt_id(tegra_clk_sclk, tegra_clks);
+		if (dt_clk) {
+			clk = tegra_clk_register_super_mux("sclk",
+						gen_info->sclk_parents,
+						gen_info->num_sclk_parents,
+						CLK_SET_RATE_PARENT,
+						clk_base + SCLK_BURST_POLICY,
+						0, 4, 0, 0, NULL);
+			*dt_clk = clk;
+		}
 	}
 
 	/* HCLK */
@@ -95,10 +171,11 @@ static void __init tegra_sclk_init(void __iomem *clk_base,
 	*dt_clk = clk;
 }
 
-void __init tegra_super_clk_gen4_init(void __iomem *clk_base,
+static void __init tegra_super_clk_init(void __iomem *clk_base,
 				void __iomem *pmc_base,
 				struct tegra_clk *tegra_clks,
-				struct tegra_clk_pll_params *params)
+				struct tegra_clk_pll_params *params,
+				const struct tegra_super_gen_info *gen_info)
 {
 	struct clk *clk;
 	struct clk **dt_clk;
@@ -106,28 +183,50 @@ void __init tegra_super_clk_gen4_init(void __iomem *clk_base,
 	/* CCLKG */
 	dt_clk = tegra_lookup_dt_id(tegra_clk_cclk_g, tegra_clks);
 	if (dt_clk) {
-		clk = tegra_clk_register_super_mux("cclk_g", cclk_g_parents,
-					ARRAY_SIZE(cclk_g_parents),
+		if (gen_info->gen == gen5) {
+			clk = tegra_clk_register_super_mux("cclk_g",
+					gen_info->cclk_g_parents,
+					gen_info->num_cclk_g_parents,
+					CLK_SET_RATE_PARENT,
+					clk_base + CCLKG_BURST_POLICY,
+					0, 4, 8, 0, NULL);
+		} else {
+			clk = tegra_clk_register_super_mux("cclk_g",
+					gen_info->cclk_g_parents,
+					gen_info->num_cclk_g_parents,
 					CLK_SET_RATE_PARENT,
 					clk_base + CCLKG_BURST_POLICY,
 					0, 4, 0, 0, NULL);
+		}
 		*dt_clk = clk;
 	}
 
 	/* CCLKLP */
 	dt_clk = tegra_lookup_dt_id(tegra_clk_cclk_lp, tegra_clks);
 	if (dt_clk) {
-		clk = tegra_clk_register_super_mux("cclk_lp", cclk_lp_parents,
-					ARRAY_SIZE(cclk_lp_parents),
+		if (gen_info->gen == gen5) {
+			clk = tegra_clk_register_super_mux("cclk_lp",
+					gen_info->cclk_lp_parents,
+					gen_info->num_cclk_lp_parents,
+					CLK_SET_RATE_PARENT,
+					clk_base + CCLKLP_BURST_POLICY,
+					0, 4, 8, 0, NULL);
+		} else {
+			clk = tegra_clk_register_super_mux("cclk_lp",
+					gen_info->cclk_lp_parents,
+					gen_info->num_cclk_lp_parents,
 					CLK_SET_RATE_PARENT,
 					clk_base + CCLKLP_BURST_POLICY,
 					TEGRA_DIVIDER_2, 4, 8, 9, NULL);
+		}
 		*dt_clk = clk;
 	}
 
-	tegra_sclk_init(clk_base, tegra_clks);
+	tegra_sclk_init(clk_base, tegra_clks, gen_info);
 
-#if defined(CONFIG_ARCH_TEGRA_114_SOC) || defined(CONFIG_ARCH_TEGRA_124_SOC)
+#if defined(CONFIG_ARCH_TEGRA_114_SOC) || \
+    defined(CONFIG_ARCH_TEGRA_124_SOC) || \
+    defined(CONFIG_ARCH_TEGRA_210_SOC)
 	/* PLLX */
 	dt_clk = tegra_lookup_dt_id(tegra_clk_pll_x, tegra_clks);
 	if (!dt_clk)
@@ -148,3 +247,20 @@ void __init tegra_super_clk_gen4_init(void __iomem *clk_base,
 #endif
 }
 
+void __init tegra_super_clk_gen4_init(void __iomem *clk_base,
+				void __iomem *pmc_base,
+				struct tegra_clk *tegra_clks,
+				struct tegra_clk_pll_params *params)
+{
+	tegra_super_clk_init(clk_base, pmc_base, tegra_clks, params,
+			     &tegra_super_gen_info_gen4);
+}
+
+void __init tegra_super_clk_gen5_init(void __iomem *clk_base,
+				void __iomem *pmc_base,
+				struct tegra_clk *tegra_clks,
+				struct tegra_clk_pll_params *params)
+{
+	tegra_super_clk_init(clk_base, pmc_base, tegra_clks, params,
+			     &tegra_super_gen_info_gen5);
+}
diff --git a/drivers/clk/tegra/clk-tegra114.c b/drivers/clk/tegra/clk-tegra114.c
index b7d03e9add97..4a24aa4bbdea 100644
--- a/drivers/clk/tegra/clk-tegra114.c
+++ b/drivers/clk/tegra/clk-tegra114.c
@@ -182,40 +182,40 @@ static struct div_nmp pllxc_nmp = {
 	.divp_width = 4,
 };
 
-static struct pdiv_map pllxc_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 3, .hw_val = 2 },
-	{ .pdiv = 4, .hw_val = 3 },
-	{ .pdiv = 5, .hw_val = 4 },
-	{ .pdiv = 6, .hw_val = 5 },
-	{ .pdiv = 8, .hw_val = 6 },
-	{ .pdiv = 10, .hw_val = 7 },
-	{ .pdiv = 12, .hw_val = 8 },
-	{ .pdiv = 16, .hw_val = 9 },
+static const struct pdiv_map pllxc_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
 	{ .pdiv = 12, .hw_val = 10 },
 	{ .pdiv = 16, .hw_val = 11 },
 	{ .pdiv = 20, .hw_val = 12 },
 	{ .pdiv = 24, .hw_val = 13 },
 	{ .pdiv = 32, .hw_val = 14 },
-	{ .pdiv = 0, .hw_val = 0 },
+	{ .pdiv =  0, .hw_val =  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 624000000, 104, 0, 2},
-	{ 12000000, 600000000, 100, 0, 2},
-	{ 13000000, 600000000,  92, 0, 2},	/* actual: 598.0 MHz */
-	{ 16800000, 600000000,  71, 0, 2},	/* actual: 596.4 MHz */
-	{ 19200000, 600000000,  62, 0, 2},	/* actual: 595.2 MHz */
-	{ 26000000, 600000000,  92, 1, 2},	/* actual: 598.0 MHz */
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 624000000, 104, 1, 2, 0 },
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_c_params = {
 	.input_min = 12000000,
 	.input_max = 800000000,
 	.cf_min = 12000000,
-	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.cf_max = 19200000, /* s/w policy, h/w capability 50 MHz */
 	.vco_min = 600000000,
 	.vco_max = 1400000000,
 	.base_reg = PLLC_BASE,
@@ -232,7 +232,7 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.pdiv_tohw = pllxc_p,
 	.div_nmp = &pllxc_nmp,
 	.freq_table = pll_c_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct div_nmp pllcx_nmp = {
@@ -244,22 +244,22 @@ static struct div_nmp pllcx_nmp = {
 	.divp_width = 3,
 };
 
-static struct pdiv_map pllc_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 4, .hw_val = 3 },
-	{ .pdiv = 8, .hw_val = 5 },
+static const struct pdiv_map pllc_p[] = {
+	{ .pdiv =  1, .hw_val = 0 },
+	{ .pdiv =  2, .hw_val = 1 },
+	{ .pdiv =  4, .hw_val = 3 },
+	{ .pdiv =  8, .hw_val = 5 },
 	{ .pdiv = 16, .hw_val = 7 },
-	{ .pdiv = 0, .hw_val = 0 },
+	{ .pdiv =  0, .hw_val = 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_cx_freq_table[] = {
-	{12000000, 600000000, 100, 0, 2},
-	{13000000, 600000000, 92, 0, 2},	/* actual: 598.0 MHz */
-	{16800000, 600000000, 71, 0, 2},	/* actual: 596.4 MHz */
-	{19200000, 600000000, 62, 0, 2},	/* actual: 595.2 MHz */
-	{26000000, 600000000, 92, 1, 2},	/* actual: 598.0 MHz */
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_c2_params = {
@@ -318,26 +318,26 @@ static struct div_nmp pllm_nmp = {
 	.override_divp_shift = 27,
 };
 
-static struct pdiv_map pllm_p[] = {
+static const struct pdiv_map pllm_p[] = {
 	{ .pdiv = 1, .hw_val = 0 },
 	{ .pdiv = 2, .hw_val = 1 },
 	{ .pdiv = 0, .hw_val = 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{12000000, 800000000, 66, 0, 1},	/* actual: 792.0 MHz */
-	{13000000, 800000000, 61, 0, 1},	/* actual: 793.0 MHz */
-	{16800000, 800000000, 47, 0, 1},	/* actual: 789.6 MHz */
-	{19200000, 800000000, 41, 0, 1},	/* actual: 787.2 MHz */
-	{26000000, 800000000, 61, 1, 1},	/* actual: 793.0 MHz */
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 800000000, 66, 1, 1, 0 }, /* actual: 792.0 MHz */
+	{ 13000000, 800000000, 61, 1, 1, 0 }, /* actual: 793.0 MHz */
+	{ 16800000, 800000000, 47, 1, 1, 0 }, /* actual: 789.6 MHz */
+	{ 19200000, 800000000, 41, 1, 1, 0 }, /* actual: 787.2 MHz */
+	{ 26000000, 800000000, 61, 2, 1, 0 }, /* actual: 793.0 MHz */
+	{        0,         0,  0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_m_params = {
 	.input_min = 12000000,
 	.input_max = 500000000,
 	.cf_min = 12000000,
-	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.cf_max = 19200000, /* s/w policy, h/w capability 50 MHz */
 	.vco_min = 400000000,
 	.vco_max = 1066000000,
 	.base_reg = PLLM_BASE,
@@ -351,7 +351,8 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.pmc_divnm_reg = PMC_PLLM_WB0_OVERRIDE,
 	.pmc_divp_reg = PMC_PLLM_WB0_OVERRIDE_2,
 	.freq_table = pll_m_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE |
+		 TEGRA_PLL_FIXED,
 };
 
 static struct div_nmp pllp_nmp = {
@@ -364,12 +365,12 @@ static struct div_nmp pllp_nmp = {
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{12000000, 216000000, 432, 12, 1, 8},
-	{13000000, 216000000, 432, 13, 1, 8},
-	{16800000, 216000000, 360, 14, 1, 8},
-	{19200000, 216000000, 360, 16, 1, 8},
-	{26000000, 216000000, 432, 26, 1, 8},
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 216000000, 432, 12, 2, 8 },
+	{ 13000000, 216000000, 432, 13, 2, 8 },
+	{ 16800000, 216000000, 360, 14, 2, 8 },
+	{ 19200000, 216000000, 360, 16, 2, 8 },
+	{ 26000000, 216000000, 432, 26, 2, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_p_params = {
@@ -386,19 +387,19 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.lock_delay = 300,
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_p_freq_table,
-	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate = 408000000,
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{9600000, 282240000, 147, 5, 0, 4},
-	{9600000, 368640000, 192, 5, 0, 4},
-	{9600000, 240000000, 200, 8, 0, 8},
-
-	{28800000, 282240000, 245, 25, 0, 8},
-	{28800000, 368640000, 320, 25, 0, 8},
-	{28800000, 240000000, 200, 24, 0, 8},
-	{0, 0, 0, 0, 0, 0},
+	{  9600000, 282240000, 147,  5, 1, 4 },
+	{  9600000, 368640000, 192,  5, 1, 4 },
+	{  9600000, 240000000, 200,  8, 1, 8 },
+	{ 28800000, 282240000, 245, 25, 1, 8 },
+	{ 28800000, 368640000, 320, 25, 1, 8 },
+	{ 28800000, 240000000, 200, 24, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 
@@ -416,28 +417,26 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.lock_delay = 300,
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_a_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{12000000, 216000000, 864, 12, 2, 12},
-	{13000000, 216000000, 864, 13, 2, 12},
-	{16800000, 216000000, 720, 14, 2, 12},
-	{19200000, 216000000, 720, 16, 2, 12},
-	{26000000, 216000000, 864, 26, 2, 12},
-
-	{12000000, 594000000, 594, 12, 0, 12},
-	{13000000, 594000000, 594, 13, 0, 12},
-	{16800000, 594000000, 495, 14, 0, 12},
-	{19200000, 594000000, 495, 16, 0, 12},
-	{26000000, 594000000, 594, 26, 0, 12},
-
-	{12000000, 1000000000, 1000, 12, 0, 12},
-	{13000000, 1000000000, 1000, 13, 0, 12},
-	{19200000, 1000000000, 625, 12, 0, 12},
-	{26000000, 1000000000, 1000, 26, 0, 12},
-
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000,  216000000,  864, 12, 4, 12 },
+	{ 13000000,  216000000,  864, 13, 4, 12 },
+	{ 16800000,  216000000,  720, 14, 4, 12 },
+	{ 19200000,  216000000,  720, 16, 4, 12 },
+	{ 26000000,  216000000,  864, 26, 4, 12 },
+	{ 12000000,  594000000,  594, 12, 1, 12 },
+	{ 13000000,  594000000,  594, 13, 1, 12 },
+	{ 16800000,  594000000,  495, 14, 1, 12 },
+	{ 19200000,  594000000,  495, 16, 1, 12 },
+	{ 26000000,  594000000,  594, 26, 1, 12 },
+	{ 12000000, 1000000000, 1000, 12, 1, 12 },
+	{ 13000000, 1000000000, 1000, 13, 1, 12 },
+	{ 19200000, 1000000000,  625, 12, 1, 12 },
+	{ 26000000, 1000000000, 1000, 26, 1, 12 },
+	{        0,          0,    0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_params pll_d_params = {
@@ -455,7 +454,7 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_d_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_d2_params = {
@@ -473,10 +472,10 @@ static struct tegra_clk_pll_params pll_d2_params = {
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_d_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
-static struct pdiv_map pllu_p[] = {
+static const struct pdiv_map pllu_p[] = {
 	{ .pdiv = 1, .hw_val = 1 },
 	{ .pdiv = 2, .hw_val = 0 },
 	{ .pdiv = 0, .hw_val = 0 },
@@ -492,12 +491,12 @@ static struct div_nmp pllu_nmp = {
 };
 
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{12000000, 480000000, 960, 12, 0, 12},
-	{13000000, 480000000, 960, 13, 0, 12},
-	{16800000, 480000000, 400, 7, 0, 5},
-	{19200000, 480000000, 200, 4, 0, 3},
-	{26000000, 480000000, 960, 26, 0, 12},
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 480000000, 960, 12, 2, 12 },
+	{ 13000000, 480000000, 960, 13, 2, 12 },
+	{ 16800000, 480000000, 400,  7, 2,  5 },
+	{ 19200000, 480000000, 200,  4, 2,  3 },
+	{ 26000000, 480000000, 960, 26, 2, 12 },
+	{        0,         0,   0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_params pll_u_params = {
@@ -516,25 +515,24 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.div_nmp = &pllu_nmp,
 	.freq_table = pll_u_freq_table,
 	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1 GHz */
-	{12000000, 1000000000, 83, 0, 1},	/* actual: 996.0 MHz */
-	{13000000, 1000000000, 76, 0, 1},	/* actual: 988.0 MHz */
-	{16800000, 1000000000, 59, 0, 1},	/* actual: 991.2 MHz */
-	{19200000, 1000000000, 52, 0, 1},	/* actual: 998.4 MHz */
-	{26000000, 1000000000, 76, 1, 1},	/* actual: 988.0 MHz */
-
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 1000000000, 83, 1, 1, 0 }, /* actual: 996.0 MHz */
+	{ 13000000, 1000000000, 76, 1, 1, 0 }, /* actual: 988.0 MHz */
+	{ 16800000, 1000000000, 59, 1, 1, 0 }, /* actual: 991.2 MHz */
+	{ 19200000, 1000000000, 52, 1, 1, 0 }, /* actual: 998.4 MHz */
+	{ 26000000, 1000000000, 76, 2, 1, 0 }, /* actual: 988.0 MHz */
+	{        0,          0,  0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
 	.input_min = 12000000,
 	.input_max = 800000000,
 	.cf_min = 12000000,
-	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.cf_max = 19200000, /* s/w policy, h/w capability 50 MHz */
 	.vco_min = 700000000,
 	.vco_max = 2400000000U,
 	.base_reg = PLLX_BASE,
@@ -551,15 +549,34 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.pdiv_tohw = pllxc_p,
 	.div_nmp = &pllxc_nmp,
 	.freq_table = pll_x_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
 	/* PLLE special case: use cpcon field to store cml divider value */
-	{336000000, 100000000, 100, 21, 16, 11},
-	{312000000, 100000000, 200, 26, 24, 13},
-	{12000000, 100000000, 200,  1,  24, 13},
-	{0, 0, 0, 0, 0, 0},
+	{ 336000000, 100000000, 100, 21, 16, 11 },
+	{ 312000000, 100000000, 200, 26, 24, 13 },
+	{  12000000, 100000000, 200,  1, 24, 13 },
+	{         0,         0,   0,  0,  0,  0 },
+};
+
+static const struct pdiv_map plle_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
+	{ .pdiv = 12, .hw_val = 10 },
+	{ .pdiv = 16, .hw_val = 11 },
+	{ .pdiv = 20, .hw_val = 12 },
+	{ .pdiv = 24, .hw_val = 13 },
+	{ .pdiv = 32, .hw_val = 14 },
+	{ .pdiv =  0, .hw_val =  0 }
 };
 
 static struct div_nmp plle_nmp = {
@@ -584,9 +601,10 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
+	.pdiv_tohw = plle_p,
 	.div_nmp = &plle_nmp,
 	.freq_table = pll_e_freq_table,
-	.flags = TEGRA_PLL_FIXED,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate = 100000000,
 };
 
@@ -614,18 +632,19 @@ static struct tegra_clk_pll_params pll_re_vco_params = {
 	.iddq_reg = PLLRE_MISC,
 	.iddq_bit_idx = PLLRE_IDDQ_BIT,
 	.div_nmp = &pllre_nmp,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE |
+		 TEGRA_PLL_LOCK_MISC,
 };
 
 /* possible OSC frequencies in Hz */
 static unsigned long tegra114_input_freq[] = {
-	[0] = 13000000,
-	[1] = 16800000,
-	[4] = 19200000,
-	[5] = 38400000,
-	[8] = 12000000,
-	[9] = 48000000,
-	[12] = 260000000,
+	[ 0] = 13000000,
+	[ 1] = 16800000,
+	[ 4] = 19200000,
+	[ 5] = 38400000,
+	[ 8] = 12000000,
+	[ 9] = 48000000,
+	[12] = 26000000,
 };
 
 #define MASK(x) (BIT(x) - 1)
@@ -644,21 +663,27 @@ struct utmi_clk_param {
 };
 
 static const struct utmi_clk_param utmi_parameters[] = {
-	{.osc_frequency = 13000000, .enable_delay_count = 0x02,
-	 .stable_count = 0x33, .active_delay_count = 0x05,
-	 .xtal_freq_count = 0x7F},
-	{.osc_frequency = 19200000, .enable_delay_count = 0x03,
-	 .stable_count = 0x4B, .active_delay_count = 0x06,
-	 .xtal_freq_count = 0xBB},
-	{.osc_frequency = 12000000, .enable_delay_count = 0x02,
-	 .stable_count = 0x2F, .active_delay_count = 0x04,
-	 .xtal_freq_count = 0x76},
-	{.osc_frequency = 26000000, .enable_delay_count = 0x04,
-	 .stable_count = 0x66, .active_delay_count = 0x09,
-	 .xtal_freq_count = 0xFE},
-	{.osc_frequency = 16800000, .enable_delay_count = 0x03,
-	 .stable_count = 0x41, .active_delay_count = 0x0A,
-	 .xtal_freq_count = 0xA4},
+	{
+		.osc_frequency = 13000000, .enable_delay_count = 0x02,
+		.stable_count = 0x33, .active_delay_count = 0x05,
+		.xtal_freq_count = 0x7f
+	}, {
+		.osc_frequency = 19200000, .enable_delay_count = 0x03,
+		.stable_count = 0x4b, .active_delay_count = 0x06,
+		.xtal_freq_count = 0xbb
+	}, {
+		.osc_frequency = 12000000, .enable_delay_count = 0x02,
+		.stable_count = 0x2f, .active_delay_count = 0x04,
+		.xtal_freq_count = 0x76
+	}, {
+		.osc_frequency = 26000000, .enable_delay_count = 0x04,
+		.stable_count = 0x66, .active_delay_count = 0x09,
+		.xtal_freq_count = 0xfe
+	}, {
+		.osc_frequency = 16800000, .enable_delay_count = 0x03,
+		.stable_count = 0x41, .active_delay_count = 0x0a,
+		.xtal_freq_count = 0xa4
+	},
 };
 
 /* peripheral mux definitions */
@@ -965,8 +990,8 @@ static void __init tegra114_fixed_clk_init(void __iomem *clk_base)
 
 static __init void tegra114_utmi_param_configure(void __iomem *clk_base)
 {
+	unsigned int i;
 	u32 reg;
-	int i;
 
 	for (i = 0; i < ARRAY_SIZE(utmi_parameters); i++) {
 		if (osc_freq == utmi_parameters[i].osc_frequency)
@@ -1173,7 +1198,7 @@ static __init void tegra114_periph_clk_init(void __iomem *clk_base,
 {
 	struct clk *clk;
 	struct tegra_periph_init_data *data;
-	int i;
+	unsigned int i;
 
 	/* xusb_ss_div2 */
 	clk = clk_register_fixed_factor(NULL, "xusb_ss_div2", "xusb_ss_src", 0,
@@ -1278,7 +1303,7 @@ static struct tegra_cpu_car_ops tegra114_cpu_car_ops = {
 
 static const struct of_device_id pmc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra114-pmc" },
-	{},
+	{ },
 };
 
 /*
@@ -1286,37 +1311,37 @@ static const struct of_device_id pmc_match[] __initconst = {
  * breaks
  */
 static struct tegra_clk_init_table init_table[] __initdata = {
-	{TEGRA114_CLK_UARTA, TEGRA114_CLK_PLL_P, 408000000, 0},
-	{TEGRA114_CLK_UARTB, TEGRA114_CLK_PLL_P, 408000000, 0},
-	{TEGRA114_CLK_UARTC, TEGRA114_CLK_PLL_P, 408000000, 0},
-	{TEGRA114_CLK_UARTD, TEGRA114_CLK_PLL_P, 408000000, 0},
-	{TEGRA114_CLK_PLL_A, TEGRA114_CLK_CLK_MAX, 564480000, 1},
-	{TEGRA114_CLK_PLL_A_OUT0, TEGRA114_CLK_CLK_MAX, 11289600, 1},
-	{TEGRA114_CLK_EXTERN1, TEGRA114_CLK_PLL_A_OUT0, 0, 1},
-	{TEGRA114_CLK_CLK_OUT_1_MUX, TEGRA114_CLK_EXTERN1, 0, 1},
-	{TEGRA114_CLK_CLK_OUT_1, TEGRA114_CLK_CLK_MAX, 0, 1},
-	{TEGRA114_CLK_I2S0, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA114_CLK_I2S1, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA114_CLK_I2S2, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA114_CLK_I2S3, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA114_CLK_I2S4, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA114_CLK_HOST1X, TEGRA114_CLK_PLL_P, 136000000, 0},
-	{TEGRA114_CLK_DFLL_SOC, TEGRA114_CLK_PLL_P, 51000000, 1},
-	{TEGRA114_CLK_DFLL_REF, TEGRA114_CLK_PLL_P, 51000000, 1},
-	{TEGRA114_CLK_DISP1, TEGRA114_CLK_PLL_P, 0, 0},
-	{TEGRA114_CLK_DISP2, TEGRA114_CLK_PLL_P, 0, 0},
-	{TEGRA114_CLK_GR2D, TEGRA114_CLK_PLL_C2, 300000000, 0},
-	{TEGRA114_CLK_GR3D, TEGRA114_CLK_PLL_C2, 300000000, 0},
-	{TEGRA114_CLK_DSIALP, TEGRA114_CLK_PLL_P, 68000000, 0},
-	{TEGRA114_CLK_DSIBLP, TEGRA114_CLK_PLL_P, 68000000, 0},
-	{TEGRA114_CLK_PLL_RE_VCO, TEGRA114_CLK_CLK_MAX, 612000000, 0},
-	{TEGRA114_CLK_XUSB_SS_SRC, TEGRA114_CLK_PLL_RE_OUT, 122400000, 0},
-	{TEGRA114_CLK_XUSB_FS_SRC, TEGRA114_CLK_PLL_U_48M, 48000000, 0},
-	{TEGRA114_CLK_XUSB_HS_SRC, TEGRA114_CLK_XUSB_SS_DIV2, 61200000, 0},
-	{TEGRA114_CLK_XUSB_FALCON_SRC, TEGRA114_CLK_PLL_P, 204000000, 0},
-	{TEGRA114_CLK_XUSB_HOST_SRC, TEGRA114_CLK_PLL_P, 102000000, 0},
-	/* This MUST be the last entry. */
-	{TEGRA114_CLK_CLK_MAX, TEGRA114_CLK_CLK_MAX, 0, 0},
+	{ TEGRA114_CLK_UARTA, TEGRA114_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA114_CLK_UARTB, TEGRA114_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA114_CLK_UARTC, TEGRA114_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA114_CLK_UARTD, TEGRA114_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA114_CLK_PLL_A, TEGRA114_CLK_CLK_MAX, 564480000, 1 },
+	{ TEGRA114_CLK_PLL_A_OUT0, TEGRA114_CLK_CLK_MAX, 11289600, 1 },
+	{ TEGRA114_CLK_EXTERN1, TEGRA114_CLK_PLL_A_OUT0, 0, 1 },
+	{ TEGRA114_CLK_CLK_OUT_1_MUX, TEGRA114_CLK_EXTERN1, 0, 1 },
+	{ TEGRA114_CLK_CLK_OUT_1, TEGRA114_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA114_CLK_I2S0, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA114_CLK_I2S1, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA114_CLK_I2S2, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA114_CLK_I2S3, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA114_CLK_I2S4, TEGRA114_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA114_CLK_HOST1X, TEGRA114_CLK_PLL_P, 136000000, 0 },
+	{ TEGRA114_CLK_DFLL_SOC, TEGRA114_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA114_CLK_DFLL_REF, TEGRA114_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA114_CLK_DISP1, TEGRA114_CLK_PLL_P, 0, 0 },
+	{ TEGRA114_CLK_DISP2, TEGRA114_CLK_PLL_P, 0, 0 },
+	{ TEGRA114_CLK_GR2D, TEGRA114_CLK_PLL_C2, 300000000, 0 },
+	{ TEGRA114_CLK_GR3D, TEGRA114_CLK_PLL_C2, 300000000, 0 },
+	{ TEGRA114_CLK_DSIALP, TEGRA114_CLK_PLL_P, 68000000, 0 },
+	{ TEGRA114_CLK_DSIBLP, TEGRA114_CLK_PLL_P, 68000000, 0 },
+	{ TEGRA114_CLK_PLL_RE_VCO, TEGRA114_CLK_CLK_MAX, 612000000, 0 },
+	{ TEGRA114_CLK_XUSB_SS_SRC, TEGRA114_CLK_PLL_RE_OUT, 122400000, 0 },
+	{ TEGRA114_CLK_XUSB_FS_SRC, TEGRA114_CLK_PLL_U_48M, 48000000, 0 },
+	{ TEGRA114_CLK_XUSB_HS_SRC, TEGRA114_CLK_XUSB_SS_DIV2, 61200000, 0 },
+	{ TEGRA114_CLK_XUSB_FALCON_SRC, TEGRA114_CLK_PLL_P, 204000000, 0 },
+	{ TEGRA114_CLK_XUSB_HOST_SRC, TEGRA114_CLK_PLL_P, 102000000, 0 },
+	/* must be the last entry */
+	{ TEGRA114_CLK_CLK_MAX, TEGRA114_CLK_CLK_MAX, 0, 0 },
 };
 
 static void __init tegra114_clock_apply_init_table(void)
diff --git a/drivers/clk/tegra/clk-tegra124.c b/drivers/clk/tegra/clk-tegra124.c
index 87975f7adddc..1627258292d2 100644
--- a/drivers/clk/tegra/clk-tegra124.c
+++ b/drivers/clk/tegra/clk-tegra124.c
@@ -150,13 +150,13 @@ static DEFINE_SPINLOCK(emc_lock);
 
 /* possible OSC frequencies in Hz */
 static unsigned long tegra124_input_freq[] = {
-	[0] = 13000000,
-	[1] = 16800000,
-	[4] = 19200000,
-	[5] = 38400000,
-	[8] = 12000000,
-	[9] = 48000000,
-	[12] = 260000000,
+	[ 0] = 13000000,
+	[ 1] = 16800000,
+	[ 4] = 19200000,
+	[ 5] = 38400000,
+	[ 8] = 12000000,
+	[ 9] = 48000000,
+	[12] = 26000000,
 };
 
 static struct div_nmp pllxc_nmp = {
@@ -168,33 +168,33 @@ static struct div_nmp pllxc_nmp = {
 	.divp_width = 4,
 };
 
-static struct pdiv_map pllxc_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 3, .hw_val = 2 },
-	{ .pdiv = 4, .hw_val = 3 },
-	{ .pdiv = 5, .hw_val = 4 },
-	{ .pdiv = 6, .hw_val = 5 },
-	{ .pdiv = 8, .hw_val = 6 },
-	{ .pdiv = 10, .hw_val = 7 },
-	{ .pdiv = 12, .hw_val = 8 },
-	{ .pdiv = 16, .hw_val = 9 },
+static const struct pdiv_map pllxc_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
 	{ .pdiv = 12, .hw_val = 10 },
 	{ .pdiv = 16, .hw_val = 11 },
 	{ .pdiv = 20, .hw_val = 12 },
 	{ .pdiv = 24, .hw_val = 13 },
 	{ .pdiv = 32, .hw_val = 14 },
-	{ .pdiv = 0, .hw_val = 0 },
+	{ .pdiv =  0, .hw_val =  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1 GHz */
-	{12000000, 1000000000, 83, 0, 1},	/* actual: 996.0 MHz */
-	{13000000, 1000000000, 76, 0, 1},	/* actual: 988.0 MHz */
-	{16800000, 1000000000, 59, 0, 1},	/* actual: 991.2 MHz */
-	{19200000, 1000000000, 52, 0, 1},	/* actual: 998.4 MHz */
-	{26000000, 1000000000, 76, 1, 1},	/* actual: 988.0 MHz */
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 1000000000, 83, 1, 1, 0 }, /* actual: 996.0 MHz */
+	{ 13000000, 1000000000, 76, 1, 1, 0 }, /* actual: 988.0 MHz */
+	{ 16800000, 1000000000, 59, 1, 1, 0 }, /* actual: 991.2 MHz */
+	{ 19200000, 1000000000, 52, 1, 1, 0 }, /* actual: 998.4 MHz */
+	{ 26000000, 1000000000, 76, 2, 1, 0 }, /* actual: 988.0 MHz */
+	{        0,          0,  0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
@@ -218,24 +218,24 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.pdiv_tohw = pllxc_p,
 	.div_nmp = &pllxc_nmp,
 	.freq_table = pll_x_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 624000000, 104, 1, 2},
-	{ 12000000, 600000000, 100, 1, 2},
-	{ 13000000, 600000000,  92, 1, 2},	/* actual: 598.0 MHz */
-	{ 16800000, 600000000,  71, 1, 2},	/* actual: 596.4 MHz */
-	{ 19200000, 600000000,  62, 1, 2},	/* actual: 595.2 MHz */
-	{ 26000000, 600000000,  92, 2, 2},	/* actual: 598.0 MHz */
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 624000000, 104, 1, 2, 0 },
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_c_params = {
 	.input_min = 12000000,
 	.input_max = 800000000,
 	.cf_min = 12000000,
-	.cf_max = 19200000,	/* s/w policy, h/w capability 50 MHz */
+	.cf_max = 19200000, /* s/w policy, h/w capability 50 MHz */
 	.vco_min = 600000000,
 	.vco_max = 1400000000,
 	.base_reg = PLLC_BASE,
@@ -252,7 +252,7 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.pdiv_tohw = pllxc_p,
 	.div_nmp = &pllxc_nmp,
 	.freq_table = pll_c_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct div_nmp pllcx_nmp = {
@@ -264,25 +264,25 @@ static struct div_nmp pllcx_nmp = {
 	.divp_width = 3,
 };
 
-static struct pdiv_map pllc_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 3, .hw_val = 2 },
-	{ .pdiv = 4, .hw_val = 3 },
-	{ .pdiv = 6, .hw_val = 4 },
-	{ .pdiv = 8, .hw_val = 5 },
+static const struct pdiv_map pllc_p[] = {
+	{ .pdiv =  1, .hw_val = 0 },
+	{ .pdiv =  2, .hw_val = 1 },
+	{ .pdiv =  3, .hw_val = 2 },
+	{ .pdiv =  4, .hw_val = 3 },
+	{ .pdiv =  6, .hw_val = 4 },
+	{ .pdiv =  8, .hw_val = 5 },
 	{ .pdiv = 12, .hw_val = 6 },
 	{ .pdiv = 16, .hw_val = 7 },
-	{ .pdiv = 0, .hw_val = 0 },
+	{ .pdiv =  0, .hw_val = 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_cx_freq_table[] = {
-	{12000000, 600000000, 100, 1, 2},
-	{13000000, 600000000, 92, 1, 2},	/* actual: 598.0 MHz */
-	{16800000, 600000000, 71, 1, 2},	/* actual: 596.4 MHz */
-	{19200000, 600000000, 62, 1, 2},	/* actual: 595.2 MHz */
-	{26000000, 600000000, 92, 2, 2},	/* actual: 598.0 MHz */
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_c2_params = {
@@ -338,32 +338,32 @@ static struct div_nmp pllss_nmp = {
 	.divp_width = 4,
 };
 
-static struct pdiv_map pll12g_ssd_esd_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 3, .hw_val = 2 },
-	{ .pdiv = 4, .hw_val = 3 },
-	{ .pdiv = 5, .hw_val = 4 },
-	{ .pdiv = 6, .hw_val = 5 },
-	{ .pdiv = 8, .hw_val = 6 },
-	{ .pdiv = 10, .hw_val = 7 },
-	{ .pdiv = 12, .hw_val = 8 },
-	{ .pdiv = 16, .hw_val = 9 },
+static const struct pdiv_map pll12g_ssd_esd_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
 	{ .pdiv = 12, .hw_val = 10 },
 	{ .pdiv = 16, .hw_val = 11 },
 	{ .pdiv = 20, .hw_val = 12 },
 	{ .pdiv = 24, .hw_val = 13 },
 	{ .pdiv = 32, .hw_val = 14 },
-	{ .pdiv = 0, .hw_val = 0 },
+	{ .pdiv =  0, .hw_val =  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_c4_freq_table[] = {
-	{ 12000000, 600000000, 100, 1, 1},
-	{ 13000000, 600000000,  92, 1, 1},      /* actual: 598.0 MHz */
-	{ 16800000, 600000000,  71, 1, 1},      /* actual: 596.4 MHz */
-	{ 19200000, 600000000,  62, 1, 1},      /* actual: 595.2 MHz */
-	{ 26000000, 600000000,  92, 2, 1},      /* actual: 598.0 MHz */
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_c4_params = {
@@ -386,21 +386,35 @@ static struct tegra_clk_pll_params pll_c4_params = {
 	.ext_misc_reg[1] = 0x5b0,
 	.ext_misc_reg[2] = 0x5b4,
 	.freq_table = pll_c4_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
-static struct pdiv_map pllm_p[] = {
-	{ .pdiv = 1, .hw_val = 0 },
-	{ .pdiv = 2, .hw_val = 1 },
-	{ .pdiv = 0, .hw_val = 0 },
+static const struct pdiv_map pllm_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
+	{ .pdiv = 12, .hw_val = 10 },
+	{ .pdiv = 16, .hw_val = 11 },
+	{ .pdiv = 20, .hw_val = 12 },
+	{ .pdiv = 24, .hw_val = 13 },
+	{ .pdiv = 32, .hw_val = 14 },
+	{ .pdiv =  0, .hw_val =  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{12000000, 800000000, 66, 1, 1},	/* actual: 792.0 MHz */
-	{13000000, 800000000, 61, 1, 1},	/* actual: 793.0 MHz */
-	{16800000, 800000000, 47, 1, 1},	/* actual: 789.6 MHz */
-	{19200000, 800000000, 41, 1, 1},	/* actual: 787.2 MHz */
-	{26000000, 800000000, 61, 2, 1},	/* actual: 793.0 MHz */
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 800000000, 66, 1, 1, 0 }, /* actual: 792.0 MHz */
+	{ 13000000, 800000000, 61, 1, 1, 0 }, /* actual: 793.0 MHz */
+	{ 16800000, 800000000, 47, 1, 1, 0 }, /* actual: 789.6 MHz */
+	{ 19200000, 800000000, 41, 1, 1, 0 }, /* actual: 787.2 MHz */
+	{ 26000000, 800000000, 61, 2, 1, 0 }, /* actual: 793.0 MHz */
+	{        0,         0,  0, 0, 0, 0},
 };
 
 static struct div_nmp pllm_nmp = {
@@ -427,22 +441,41 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.lock_mask = PLL_BASE_LOCK,
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
-	.max_p = 2,
+	.max_p = 5,
 	.pdiv_tohw = pllm_p,
 	.div_nmp = &pllm_nmp,
 	.pmc_divnm_reg = PMC_PLLM_WB0_OVERRIDE,
 	.pmc_divp_reg = PMC_PLLM_WB0_OVERRIDE_2,
 	.freq_table = pll_m_freq_table,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
 	/* PLLE special case: use cpcon field to store cml divider value */
-	{336000000, 100000000, 100, 21, 16, 11},
-	{312000000, 100000000, 200, 26, 24, 13},
-	{13000000,  100000000, 200, 1,  26, 13},
-	{12000000,  100000000, 200, 1,  24, 13},
-	{0, 0, 0, 0, 0, 0},
+	{ 336000000, 100000000, 100, 21, 16, 11 },
+	{ 312000000, 100000000, 200, 26, 24, 13 },
+	{  13000000, 100000000, 200,  1, 26, 13 },
+	{  12000000, 100000000, 200,  1, 24, 13 },
+	{         0,         0,   0,  0,  0,  0 },
+};
+
+static const struct pdiv_map plle_p[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv = 10, .hw_val =  7 },
+	{ .pdiv = 12, .hw_val =  8 },
+	{ .pdiv = 16, .hw_val =  9 },
+	{ .pdiv = 12, .hw_val = 10 },
+	{ .pdiv = 16, .hw_val = 11 },
+	{ .pdiv = 20, .hw_val = 12 },
+	{ .pdiv = 24, .hw_val = 13 },
+	{ .pdiv = 32, .hw_val = 14 },
+	{ .pdiv =  1, .hw_val =  0 },
 };
 
 static struct div_nmp plle_nmp = {
@@ -467,9 +500,10 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
+	.pdiv_tohw = plle_p,
 	.div_nmp = &plle_nmp,
 	.freq_table = pll_e_freq_table,
-	.flags = TEGRA_PLL_FIXED,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate = 100000000,
 };
 
@@ -507,7 +541,8 @@ static struct tegra_clk_pll_params pll_re_vco_params = {
 	.iddq_reg = PLLRE_MISC,
 	.iddq_bit_idx = PLLRE_IDDQ_BIT,
 	.div_nmp = &pllre_nmp,
-	.flags = TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE |
+		 TEGRA_PLL_LOCK_MISC,
 };
 
 static struct div_nmp pllp_nmp = {
@@ -520,12 +555,12 @@ static struct div_nmp pllp_nmp = {
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{12000000, 408000000, 408, 12, 0, 8},
-	{13000000, 408000000, 408, 13, 0, 8},
-	{16800000, 408000000, 340, 14, 0, 8},
-	{19200000, 408000000, 340, 16, 0, 8},
-	{26000000, 408000000, 408, 26, 0, 8},
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 408000000, 408, 12, 1, 8 },
+	{ 13000000, 408000000, 408, 13, 1, 8 },
+	{ 16800000, 408000000, 340, 14, 1, 8 },
+	{ 19200000, 408000000, 340, 16, 1, 8 },
+	{ 26000000, 408000000, 408, 26, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_p_params = {
@@ -543,18 +578,18 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_p_freq_table,
 	.fixed_rate = 408000000,
-	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{9600000, 282240000, 147, 5, 0, 4},
-	{9600000, 368640000, 192, 5, 0, 4},
-	{9600000, 240000000, 200, 8, 0, 8},
-
-	{28800000, 282240000, 245, 25, 0, 8},
-	{28800000, 368640000, 320, 25, 0, 8},
-	{28800000, 240000000, 200, 24, 0, 8},
-	{0, 0, 0, 0, 0, 0},
+	{  9600000, 282240000, 147,  5, 1, 4 },
+	{  9600000, 368640000, 192,  5, 1, 4 },
+	{  9600000, 240000000, 200,  8, 1, 8 },
+	{ 28800000, 282240000, 245, 25, 1, 8 },
+	{ 28800000, 368640000, 320, 25, 1, 8 },
+	{ 28800000, 240000000, 200, 24, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_a_params = {
@@ -571,7 +606,8 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.lock_delay = 300,
 	.div_nmp = &pllp_nmp,
 	.freq_table = pll_a_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct div_nmp plld_nmp = {
@@ -584,24 +620,21 @@ static struct div_nmp plld_nmp = {
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{12000000, 216000000, 864, 12, 4, 12},
-	{13000000, 216000000, 864, 13, 4, 12},
-	{16800000, 216000000, 720, 14, 4, 12},
-	{19200000, 216000000, 720, 16, 4, 12},
-	{26000000, 216000000, 864, 26, 4, 12},
-
-	{12000000, 594000000, 594, 12, 1, 12},
-	{13000000, 594000000, 594, 13, 1, 12},
-	{16800000, 594000000, 495, 14, 1, 12},
-	{19200000, 594000000, 495, 16, 1, 12},
-	{26000000, 594000000, 594, 26, 1, 12},
-
-	{12000000, 1000000000, 1000, 12, 1, 12},
-	{13000000, 1000000000, 1000, 13, 1, 12},
-	{19200000, 1000000000, 625, 12, 1, 12},
-	{26000000, 1000000000, 1000, 26, 1, 12},
-
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000,  216000000,  864, 12, 4, 12 },
+	{ 13000000,  216000000,  864, 13, 4, 12 },
+	{ 16800000,  216000000,  720, 14, 4, 12 },
+	{ 19200000,  216000000,  720, 16, 4, 12 },
+	{ 26000000,  216000000,  864, 26, 4, 12 },
+	{ 12000000,  594000000,  594, 12, 1, 12 },
+	{ 13000000,  594000000,  594, 13, 1, 12 },
+	{ 16800000,  594000000,  495, 14, 1, 12 },
+	{ 19200000,  594000000,  495, 16, 1, 12 },
+	{ 26000000,  594000000,  594, 26, 1, 12 },
+	{ 12000000, 1000000000, 1000, 12, 1, 12 },
+	{ 13000000, 1000000000, 1000, 13, 1, 12 },
+	{ 19200000, 1000000000,  625, 12, 1, 12 },
+	{ 26000000, 1000000000, 1000, 26, 1, 12 },
+	{        0,          0,    0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_params pll_d_params = {
@@ -619,16 +652,16 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.div_nmp = &plld_nmp,
 	.freq_table = pll_d_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table tegra124_pll_d2_freq_table[] = {
-	{ 12000000, 594000000,  99, 1, 2},
-	{ 13000000, 594000000,  91, 1, 2},      /* actual: 591.5 MHz */
-	{ 16800000, 594000000,  71, 1, 2},      /* actual: 596.4 MHz */
-	{ 19200000, 594000000,  62, 1, 2},      /* actual: 595.2 MHz */
-	{ 26000000, 594000000,  91, 2, 2},      /* actual: 591.5 MHz */
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 594000000, 99, 1, 2, 0 },
+	{ 13000000, 594000000, 91, 1, 2, 0 }, /* actual: 591.5 MHz */
+	{ 16800000, 594000000, 71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 594000000, 62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 594000000, 91, 2, 2, 0 }, /* actual: 591.5 MHz */
+	{        0,         0,  0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params tegra124_pll_d2_params = {
@@ -652,15 +685,16 @@ static struct tegra_clk_pll_params tegra124_pll_d2_params = {
 	.ext_misc_reg[2] = 0x578,
 	.max_p = 15,
 	.freq_table = tegra124_pll_d2_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_freq_table pll_dp_freq_table[] = {
-	{ 12000000, 600000000, 100, 1, 1},
-	{ 13000000, 600000000,  92, 1, 1},      /* actual: 598.0 MHz */
-	{ 16800000, 600000000,  71, 1, 1},      /* actual: 596.4 MHz */
-	{ 19200000, 600000000,  62, 1, 1},      /* actual: 595.2 MHz */
-	{ 26000000, 600000000,  92, 2, 1},      /* actual: 598.0 MHz */
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 600000000, 100, 1, 2, 0 },
+	{ 13000000, 600000000,  92, 1, 2, 0 }, /* actual: 598.0 MHz */
+	{ 16800000, 600000000,  71, 1, 2, 0 }, /* actual: 596.4 MHz */
+	{ 19200000, 600000000,  62, 1, 2, 0 }, /* actual: 595.2 MHz */
+	{ 26000000, 600000000,  92, 2, 2, 0 }, /* actual: 598.0 MHz */
+	{        0,         0,   0, 0, 0, 0 },
 };
 
 static struct tegra_clk_pll_params pll_dp_params = {
@@ -684,9 +718,10 @@ static struct tegra_clk_pll_params pll_dp_params = {
 	.ext_misc_reg[2] = 0x5a0,
 	.max_p = 5,
 	.freq_table = pll_dp_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
-static struct pdiv_map pllu_p[] = {
+static const struct pdiv_map pllu_p[] = {
 	{ .pdiv = 1, .hw_val = 1 },
 	{ .pdiv = 2, .hw_val = 0 },
 	{ .pdiv = 0, .hw_val = 0 },
@@ -702,12 +737,12 @@ static struct div_nmp pllu_nmp = {
 };
 
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{12000000, 480000000, 960, 12, 2, 12},
-	{13000000, 480000000, 960, 13, 2, 12},
-	{16800000, 480000000, 400, 7, 2, 5},
-	{19200000, 480000000, 200, 4, 2, 3},
-	{26000000, 480000000, 960, 26, 2, 12},
-	{0, 0, 0, 0, 0, 0},
+	{ 12000000, 480000000, 960, 12, 2, 12 },
+	{ 13000000, 480000000, 960, 13, 2, 12 },
+	{ 16800000, 480000000, 400,  7, 2,  5 },
+	{ 19200000, 480000000, 200,  4, 2,  3 },
+	{ 26000000, 480000000, 960, 26, 2, 12 },
+	{        0,         0,   0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_params pll_u_params = {
@@ -726,7 +761,7 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.div_nmp = &pllu_nmp,
 	.freq_table = pll_u_freq_table,
 	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 struct utmi_clk_param {
@@ -743,21 +778,27 @@ struct utmi_clk_param {
 };
 
 static const struct utmi_clk_param utmi_parameters[] = {
-	{.osc_frequency = 13000000, .enable_delay_count = 0x02,
-	 .stable_count = 0x33, .active_delay_count = 0x05,
-	 .xtal_freq_count = 0x7F},
-	{.osc_frequency = 19200000, .enable_delay_count = 0x03,
-	 .stable_count = 0x4B, .active_delay_count = 0x06,
-	 .xtal_freq_count = 0xBB},
-	{.osc_frequency = 12000000, .enable_delay_count = 0x02,
-	 .stable_count = 0x2F, .active_delay_count = 0x04,
-	 .xtal_freq_count = 0x76},
-	{.osc_frequency = 26000000, .enable_delay_count = 0x04,
-	 .stable_count = 0x66, .active_delay_count = 0x09,
-	 .xtal_freq_count = 0xFE},
-	{.osc_frequency = 16800000, .enable_delay_count = 0x03,
-	 .stable_count = 0x41, .active_delay_count = 0x0A,
-	 .xtal_freq_count = 0xA4},
+	{
+		.osc_frequency = 13000000, .enable_delay_count = 0x02,
+		.stable_count = 0x33, .active_delay_count = 0x05,
+		.xtal_freq_count = 0x7f
+	}, {
+		.osc_frequency = 19200000, .enable_delay_count = 0x03,
+		.stable_count = 0x4b, .active_delay_count = 0x06,
+		.xtal_freq_count = 0xbb
+	}, {
+		.osc_frequency = 12000000, .enable_delay_count = 0x02,
+		.stable_count = 0x2f, .active_delay_count = 0x04,
+		.xtal_freq_count = 0x76
+	}, {
+		.osc_frequency = 26000000, .enable_delay_count = 0x04,
+		.stable_count = 0x66, .active_delay_count = 0x09,
+		.xtal_freq_count = 0xfe
+	}, {
+		.osc_frequency = 16800000, .enable_delay_count = 0x03,
+		.stable_count = 0x41, .active_delay_count = 0x0a,
+		.xtal_freq_count = 0xa4
+	},
 };
 
 static struct tegra_clk tegra124_clks[tegra_clk_max] __initdata = {
@@ -1024,8 +1065,8 @@ static struct clk **clks;
 
 static void tegra124_utmi_param_configure(void __iomem *clk_base)
 {
+	unsigned int i;
 	u32 reg;
-	int i;
 
 	for (i = 0; i < ARRAY_SIZE(utmi_parameters); i++) {
 		if (osc_freq == utmi_parameters[i].osc_frequency)
@@ -1356,65 +1397,65 @@ static struct tegra_cpu_car_ops tegra124_cpu_car_ops = {
 
 static const struct of_device_id pmc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra124-pmc" },
-	{},
+	{ },
 };
 
 static struct tegra_clk_init_table common_init_table[] __initdata = {
-	{TEGRA124_CLK_UARTA, TEGRA124_CLK_PLL_P, 408000000, 0},
-	{TEGRA124_CLK_UARTB, TEGRA124_CLK_PLL_P, 408000000, 0},
-	{TEGRA124_CLK_UARTC, TEGRA124_CLK_PLL_P, 408000000, 0},
-	{TEGRA124_CLK_UARTD, TEGRA124_CLK_PLL_P, 408000000, 0},
-	{TEGRA124_CLK_PLL_A, TEGRA124_CLK_CLK_MAX, 564480000, 1},
-	{TEGRA124_CLK_PLL_A_OUT0, TEGRA124_CLK_CLK_MAX, 11289600, 1},
-	{TEGRA124_CLK_EXTERN1, TEGRA124_CLK_PLL_A_OUT0, 0, 1},
-	{TEGRA124_CLK_CLK_OUT_1_MUX, TEGRA124_CLK_EXTERN1, 0, 1},
-	{TEGRA124_CLK_CLK_OUT_1, TEGRA124_CLK_CLK_MAX, 0, 1},
-	{TEGRA124_CLK_I2S0, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA124_CLK_I2S1, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA124_CLK_I2S2, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA124_CLK_I2S3, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA124_CLK_I2S4, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA124_CLK_VDE, TEGRA124_CLK_PLL_P, 0, 0},
-	{TEGRA124_CLK_HOST1X, TEGRA124_CLK_PLL_P, 136000000, 1},
-	{TEGRA124_CLK_DSIALP, TEGRA124_CLK_PLL_P, 68000000, 0},
-	{TEGRA124_CLK_DSIBLP, TEGRA124_CLK_PLL_P, 68000000, 0},
-	{TEGRA124_CLK_SCLK, TEGRA124_CLK_PLL_P_OUT2, 102000000, 1},
-	{TEGRA124_CLK_DFLL_SOC, TEGRA124_CLK_PLL_P, 51000000, 1},
-	{TEGRA124_CLK_DFLL_REF, TEGRA124_CLK_PLL_P, 51000000, 1},
-	{TEGRA124_CLK_PLL_C, TEGRA124_CLK_CLK_MAX, 768000000, 0},
-	{TEGRA124_CLK_PLL_C_OUT1, TEGRA124_CLK_CLK_MAX, 100000000, 0},
-	{TEGRA124_CLK_SBC4, TEGRA124_CLK_PLL_P, 12000000, 1},
-	{TEGRA124_CLK_TSEC, TEGRA124_CLK_PLL_C3, 0, 0},
-	{TEGRA124_CLK_MSENC, TEGRA124_CLK_PLL_C3, 0, 0},
-	{TEGRA124_CLK_PLL_RE_VCO, TEGRA124_CLK_CLK_MAX, 672000000, 0},
-	{TEGRA124_CLK_XUSB_SS_SRC, TEGRA124_CLK_PLL_U_480M, 120000000, 0},
-	{TEGRA124_CLK_XUSB_FS_SRC, TEGRA124_CLK_PLL_U_48M, 48000000, 0},
-	{TEGRA124_CLK_XUSB_HS_SRC, TEGRA124_CLK_PLL_U_60M, 60000000, 0},
-	{TEGRA124_CLK_XUSB_FALCON_SRC, TEGRA124_CLK_PLL_RE_OUT, 224000000, 0},
-	{TEGRA124_CLK_XUSB_HOST_SRC, TEGRA124_CLK_PLL_RE_OUT, 112000000, 0},
-	{TEGRA124_CLK_SATA, TEGRA124_CLK_PLL_P, 104000000, 0},
-	{TEGRA124_CLK_SATA_OOB, TEGRA124_CLK_PLL_P, 204000000, 0},
-	{TEGRA124_CLK_MSELECT, TEGRA124_CLK_CLK_MAX, 0, 1},
-	{TEGRA124_CLK_CSITE, TEGRA124_CLK_CLK_MAX, 0, 1},
-	{TEGRA124_CLK_TSENSOR, TEGRA124_CLK_CLK_M, 400000, 0},
-	/* This MUST be the last entry. */
-	{TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0},
+	{ TEGRA124_CLK_UARTA, TEGRA124_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA124_CLK_UARTB, TEGRA124_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA124_CLK_UARTC, TEGRA124_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA124_CLK_UARTD, TEGRA124_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA124_CLK_PLL_A, TEGRA124_CLK_CLK_MAX, 564480000, 1 },
+	{ TEGRA124_CLK_PLL_A_OUT0, TEGRA124_CLK_CLK_MAX, 11289600, 1 },
+	{ TEGRA124_CLK_EXTERN1, TEGRA124_CLK_PLL_A_OUT0, 0, 1 },
+	{ TEGRA124_CLK_CLK_OUT_1_MUX, TEGRA124_CLK_EXTERN1, 0, 1 },
+	{ TEGRA124_CLK_CLK_OUT_1, TEGRA124_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA124_CLK_I2S0, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA124_CLK_I2S1, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA124_CLK_I2S2, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA124_CLK_I2S3, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA124_CLK_I2S4, TEGRA124_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA124_CLK_VDE, TEGRA124_CLK_PLL_P, 0, 0 },
+	{ TEGRA124_CLK_HOST1X, TEGRA124_CLK_PLL_P, 136000000, 1 },
+	{ TEGRA124_CLK_DSIALP, TEGRA124_CLK_PLL_P, 68000000, 0 },
+	{ TEGRA124_CLK_DSIBLP, TEGRA124_CLK_PLL_P, 68000000, 0 },
+	{ TEGRA124_CLK_SCLK, TEGRA124_CLK_PLL_P_OUT2, 102000000, 1 },
+	{ TEGRA124_CLK_DFLL_SOC, TEGRA124_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA124_CLK_DFLL_REF, TEGRA124_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA124_CLK_PLL_C, TEGRA124_CLK_CLK_MAX, 768000000, 0 },
+	{ TEGRA124_CLK_PLL_C_OUT1, TEGRA124_CLK_CLK_MAX, 100000000, 0 },
+	{ TEGRA124_CLK_SBC4, TEGRA124_CLK_PLL_P, 12000000, 1 },
+	{ TEGRA124_CLK_TSEC, TEGRA124_CLK_PLL_C3, 0, 0 },
+	{ TEGRA124_CLK_MSENC, TEGRA124_CLK_PLL_C3, 0, 0 },
+	{ TEGRA124_CLK_PLL_RE_VCO, TEGRA124_CLK_CLK_MAX, 672000000, 0 },
+	{ TEGRA124_CLK_XUSB_SS_SRC, TEGRA124_CLK_PLL_U_480M, 120000000, 0 },
+	{ TEGRA124_CLK_XUSB_FS_SRC, TEGRA124_CLK_PLL_U_48M, 48000000, 0 },
+	{ TEGRA124_CLK_XUSB_HS_SRC, TEGRA124_CLK_PLL_U_60M, 60000000, 0 },
+	{ TEGRA124_CLK_XUSB_FALCON_SRC, TEGRA124_CLK_PLL_RE_OUT, 224000000, 0 },
+	{ TEGRA124_CLK_XUSB_HOST_SRC, TEGRA124_CLK_PLL_RE_OUT, 112000000, 0 },
+	{ TEGRA124_CLK_SATA, TEGRA124_CLK_PLL_P, 104000000, 0 },
+	{ TEGRA124_CLK_SATA_OOB, TEGRA124_CLK_PLL_P, 204000000, 0 },
+	{ TEGRA124_CLK_MSELECT, TEGRA124_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA124_CLK_CSITE, TEGRA124_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA124_CLK_TSENSOR, TEGRA124_CLK_CLK_M, 400000, 0 },
+	/* must be the last entry */
+	{ TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0 },
 };
 
 static struct tegra_clk_init_table tegra124_init_table[] __initdata = {
-	{TEGRA124_CLK_SOC_THERM, TEGRA124_CLK_PLL_P, 51000000, 0},
-	{TEGRA124_CLK_CCLK_G, TEGRA124_CLK_CLK_MAX, 0, 1},
-	{TEGRA124_CLK_HDA, TEGRA124_CLK_PLL_P, 102000000, 0},
-	{TEGRA124_CLK_HDA2CODEC_2X, TEGRA124_CLK_PLL_P, 48000000, 0},
-	/* This MUST be the last entry. */
-	{TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0},
+	{ TEGRA124_CLK_SOC_THERM, TEGRA124_CLK_PLL_P, 51000000, 0 },
+	{ TEGRA124_CLK_CCLK_G, TEGRA124_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA124_CLK_HDA, TEGRA124_CLK_PLL_P, 102000000, 0 },
+	{ TEGRA124_CLK_HDA2CODEC_2X, TEGRA124_CLK_PLL_P, 48000000, 0 },
+	/* must be the last entry */
+	{ TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0 },
 };
 
 /* Tegra132 requires the SOC_THERM clock to remain active */
 static struct tegra_clk_init_table tegra132_init_table[] __initdata = {
-	{TEGRA124_CLK_SOC_THERM, TEGRA124_CLK_PLL_P, 51000000, 1},
-	/* This MUST be the last entry. */
-	{TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0},
+	{ TEGRA124_CLK_SOC_THERM, TEGRA124_CLK_PLL_P, 51000000, 1 },
+	/* must be the last entry */
+	{ TEGRA124_CLK_CLK_MAX, TEGRA124_CLK_CLK_MAX, 0, 0 },
 };
 
 static struct tegra_audio_clk_info tegra124_audio_plls[] = {
diff --git a/drivers/clk/tegra/clk-tegra20.c b/drivers/clk/tegra/clk-tegra20.c
index bf004f0e4f65..7a48e986c4c9 100644
--- a/drivers/clk/tegra/clk-tegra20.c
+++ b/drivers/clk/tegra/clk-tegra20.c
@@ -166,126 +166,120 @@ static DEFINE_SPINLOCK(emc_lock);
 static struct clk **clks;
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 600000000, 600, 12, 0, 8 },
-	{ 13000000, 600000000, 600, 13, 0, 8 },
-	{ 19200000, 600000000, 500, 16, 0, 6 },
-	{ 26000000, 600000000, 600, 26, 0, 8 },
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 600000000, 600, 12, 1, 8 },
+	{ 13000000, 600000000, 600, 13, 1, 8 },
+	{ 19200000, 600000000, 500, 16, 1, 6 },
+	{ 26000000, 600000000, 600, 26, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{ 12000000, 666000000, 666, 12, 0, 8},
-	{ 13000000, 666000000, 666, 13, 0, 8},
-	{ 19200000, 666000000, 555, 16, 0, 8},
-	{ 26000000, 666000000, 666, 26, 0, 8},
-	{ 12000000, 600000000, 600, 12, 0, 8},
-	{ 13000000, 600000000, 600, 13, 0, 8},
-	{ 19200000, 600000000, 375, 12, 0, 6},
-	{ 26000000, 600000000, 600, 26, 0, 8},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 666000000, 666, 12, 1, 8 },
+	{ 13000000, 666000000, 666, 13, 1, 8 },
+	{ 19200000, 666000000, 555, 16, 1, 8 },
+	{ 26000000, 666000000, 666, 26, 1, 8 },
+	{ 12000000, 600000000, 600, 12, 1, 8 },
+	{ 13000000, 600000000, 600, 13, 1, 8 },
+	{ 19200000, 600000000, 375, 12, 1, 6 },
+	{ 26000000, 600000000, 600, 26, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{ 12000000, 216000000, 432, 12, 1, 8},
-	{ 13000000, 216000000, 432, 13, 1, 8},
-	{ 19200000, 216000000, 90,   4, 1, 1},
-	{ 26000000, 216000000, 432, 26, 1, 8},
-	{ 12000000, 432000000, 432, 12, 0, 8},
-	{ 13000000, 432000000, 432, 13, 0, 8},
-	{ 19200000, 432000000, 90,   4, 0, 1},
-	{ 26000000, 432000000, 432, 26, 0, 8},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 216000000, 432, 12, 2, 8 },
+	{ 13000000, 216000000, 432, 13, 2, 8 },
+	{ 19200000, 216000000,  90,  4, 2, 1 },
+	{ 26000000, 216000000, 432, 26, 2, 8 },
+	{ 12000000, 432000000, 432, 12, 1, 8 },
+	{ 13000000, 432000000, 432, 13, 1, 8 },
+	{ 19200000, 432000000,  90,  4, 1, 1 },
+	{ 26000000, 432000000, 432, 26, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{ 28800000, 56448000, 49, 25, 0, 1},
-	{ 28800000, 73728000, 64, 25, 0, 1},
-	{ 28800000, 24000000,  5,  6, 0, 1},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 28800000, 56448000, 49, 25, 1, 1 },
+	{ 28800000, 73728000, 64, 25, 1, 1 },
+	{ 28800000, 24000000,  5,  6, 1, 1 },
+	{        0,        0,  0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{ 12000000, 216000000, 216, 12, 0, 4},
-	{ 13000000, 216000000, 216, 13, 0, 4},
-	{ 19200000, 216000000, 135, 12, 0, 3},
-	{ 26000000, 216000000, 216, 26, 0, 4},
-
-	{ 12000000, 594000000, 594, 12, 0, 8},
-	{ 13000000, 594000000, 594, 13, 0, 8},
-	{ 19200000, 594000000, 495, 16, 0, 8},
-	{ 26000000, 594000000, 594, 26, 0, 8},
-
-	{ 12000000, 1000000000, 1000, 12, 0, 12},
-	{ 13000000, 1000000000, 1000, 13, 0, 12},
-	{ 19200000, 1000000000, 625,  12, 0, 8},
-	{ 26000000, 1000000000, 1000, 26, 0, 12},
-
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000,  216000000,  216, 12, 1,  4 },
+	{ 13000000,  216000000,  216, 13, 1,  4 },
+	{ 19200000,  216000000,  135, 12, 1,  3 },
+	{ 26000000,  216000000,  216, 26, 1,  4 },
+	{ 12000000,  594000000,  594, 12, 1,  8 },
+	{ 13000000,  594000000,  594, 13, 1,  8 },
+	{ 19200000,  594000000,  495, 16, 1,  8 },
+	{ 26000000,  594000000,  594, 26, 1,  8 },
+	{ 12000000, 1000000000, 1000, 12, 1, 12 },
+	{ 13000000, 1000000000, 1000, 13, 1, 12 },
+	{ 19200000, 1000000000,  625, 12, 1,  8 },
+	{ 26000000, 1000000000, 1000, 26, 1, 12 },
+	{        0,          0,    0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{ 12000000, 480000000, 960, 12, 0, 0},
-	{ 13000000, 480000000, 960, 13, 0, 0},
-	{ 19200000, 480000000, 200, 4,  0, 0},
-	{ 26000000, 480000000, 960, 26, 0, 0},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 480000000, 960, 12, 1, 0 },
+	{ 13000000, 480000000, 960, 13, 1, 0 },
+	{ 19200000, 480000000, 200,  4, 1, 0 },
+	{ 26000000, 480000000, 960, 26, 1, 0 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1 GHz */
-	{ 12000000, 1000000000, 1000, 12, 0, 12},
-	{ 13000000, 1000000000, 1000, 13, 0, 12},
-	{ 19200000, 1000000000, 625,  12, 0, 8},
-	{ 26000000, 1000000000, 1000, 26, 0, 12},
-
+	{ 12000000, 1000000000, 1000, 12, 1, 12 },
+	{ 13000000, 1000000000, 1000, 13, 1, 12 },
+	{ 19200000, 1000000000,  625, 12, 1,  8 },
+	{ 26000000, 1000000000, 1000, 26, 1, 12 },
 	/* 912 MHz */
-	{ 12000000, 912000000,  912,  12, 0, 12},
-	{ 13000000, 912000000,  912,  13, 0, 12},
-	{ 19200000, 912000000,  760,  16, 0, 8},
-	{ 26000000, 912000000,  912,  26, 0, 12},
-
+	{ 12000000,  912000000,  912, 12, 1, 12 },
+	{ 13000000,  912000000,  912, 13, 1, 12 },
+	{ 19200000,  912000000,  760, 16, 1,  8 },
+	{ 26000000,  912000000,  912, 26, 1, 12 },
 	/* 816 MHz */
-	{ 12000000, 816000000,  816,  12, 0, 12},
-	{ 13000000, 816000000,  816,  13, 0, 12},
-	{ 19200000, 816000000,  680,  16, 0, 8},
-	{ 26000000, 816000000,  816,  26, 0, 12},
-
+	{ 12000000,  816000000,  816, 12, 1, 12 },
+	{ 13000000,  816000000,  816, 13, 1, 12 },
+	{ 19200000,  816000000,  680, 16, 1,  8 },
+	{ 26000000,  816000000,  816, 26, 1, 12 },
 	/* 760 MHz */
-	{ 12000000, 760000000,  760,  12, 0, 12},
-	{ 13000000, 760000000,  760,  13, 0, 12},
-	{ 19200000, 760000000,  950,  24, 0, 8},
-	{ 26000000, 760000000,  760,  26, 0, 12},
-
+	{ 12000000,  760000000,  760, 12, 1, 12 },
+	{ 13000000,  760000000,  760, 13, 1, 12 },
+	{ 19200000,  760000000,  950, 24, 1,  8 },
+	{ 26000000,  760000000,  760, 26, 1, 12 },
 	/* 750 MHz */
-	{ 12000000, 750000000,  750,  12, 0, 12},
-	{ 13000000, 750000000,  750,  13, 0, 12},
-	{ 19200000, 750000000,  625,  16, 0, 8},
-	{ 26000000, 750000000,  750,  26, 0, 12},
-
+	{ 12000000,  750000000,  750, 12, 1, 12 },
+	{ 13000000,  750000000,  750, 13, 1, 12 },
+	{ 19200000,  750000000,  625, 16, 1,  8 },
+	{ 26000000,  750000000,  750, 26, 1, 12 },
 	/* 608 MHz */
-	{ 12000000, 608000000,  608,  12, 0, 12},
-	{ 13000000, 608000000,  608,  13, 0, 12},
-	{ 19200000, 608000000,  380,  12, 0, 8},
-	{ 26000000, 608000000,  608,  26, 0, 12},
-
+	{ 12000000,  608000000,  608, 12, 1, 12 },
+	{ 13000000,  608000000,  608, 13, 1, 12 },
+	{ 19200000,  608000000,  380, 12, 1,  8 },
+	{ 26000000,  608000000,  608, 26, 1, 12 },
 	/* 456 MHz */
-	{ 12000000, 456000000,  456,  12, 0, 12},
-	{ 13000000, 456000000,  456,  13, 0, 12},
-	{ 19200000, 456000000,  380,  16, 0, 8},
-	{ 26000000, 456000000,  456,  26, 0, 12},
-
+	{ 12000000,  456000000,  456, 12, 1, 12 },
+	{ 13000000,  456000000,  456, 13, 1, 12 },
+	{ 19200000,  456000000,  380, 16, 1,  8 },
+	{ 26000000,  456000000,  456, 26, 1, 12 },
 	/* 312 MHz */
-	{ 12000000, 312000000,  312,  12, 0, 12},
-	{ 13000000, 312000000,  312,  13, 0, 12},
-	{ 19200000, 312000000,  260,  16, 0, 8},
-	{ 26000000, 312000000,  312,  26, 0, 12},
+	{ 12000000,  312000000,  312, 12, 1, 12 },
+	{ 13000000,  312000000,  312, 13, 1, 12 },
+	{ 19200000,  312000000,  260, 16, 1,  8 },
+	{ 26000000,  312000000,  312, 26, 1, 12 },
+	{        0,          0,    0,  0, 0,  0 },
+};
 
-	{ 0, 0, 0, 0, 0, 0 },
+static const struct pdiv_map plle_p[] = {
+	{ .pdiv = 1, .hw_val = 1 },
+	{ .pdiv = 0, .hw_val = 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
-	{ 12000000, 100000000,  200,  24, 0, 0 },
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 100000000, 200, 24, 1, 0 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 /* PLL parameters */
@@ -302,7 +296,7 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_c_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_m_params = {
@@ -318,7 +312,7 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_m_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_p_params = {
@@ -334,7 +328,8 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_p_freq_table,
-	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_CPCON |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate =  216000000,
 };
 
@@ -351,7 +346,7 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_a_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_d_params = {
@@ -367,10 +362,10 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.lock_enable_bit_idx = PLLDU_MISC_LOCK_ENABLE,
 	.lock_delay = 1000,
 	.freq_table = pll_d_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
-static struct pdiv_map pllu_p[] = {
+static const struct pdiv_map pllu_p[] = {
 	{ .pdiv = 1, .hw_val = 1 },
 	{ .pdiv = 2, .hw_val = 0 },
 	{ .pdiv = 0, .hw_val = 0 },
@@ -390,7 +385,7 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.lock_delay = 1000,
 	.pdiv_tohw = pllu_p,
 	.freq_table = pll_u_freq_table,
-	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
@@ -406,7 +401,7 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_x_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_e_params = {
@@ -421,8 +416,10 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 0,
+	.pdiv_tohw = plle_p,
 	.freq_table = pll_e_freq_table,
-	.flags = TEGRA_PLL_FIXED,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_LOCK_MISC |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate = 100000000,
 };
 
@@ -733,9 +730,9 @@ static void tegra20_super_clk_init(void)
 	clks[TEGRA20_CLK_TWD] = clk;
 }
 
-static const char *audio_parents[] = {"spdif_in", "i2s1", "i2s2", "unused",
-				      "pll_a_out0", "unused", "unused",
-				      "unused"};
+static const char *audio_parents[] = { "spdif_in", "i2s1", "i2s2", "unused",
+				       "pll_a_out0", "unused", "unused",
+				       "unused" };
 
 static void __init tegra20_audio_clk_init(void)
 {
@@ -759,19 +756,18 @@ static void __init tegra20_audio_clk_init(void)
 				    CLK_SET_RATE_PARENT, 89,
 				    periph_clk_enb_refcnt);
 	clks[TEGRA20_CLK_AUDIO_2X] = clk;
-
 }
 
-static const char *i2s1_parents[] = {"pll_a_out0", "audio_2x", "pll_p",
-				     "clk_m"};
-static const char *i2s2_parents[] = {"pll_a_out0", "audio_2x", "pll_p",
-				     "clk_m"};
-static const char *pwm_parents[] = {"pll_p", "pll_c", "audio", "clk_m",
-				    "clk_32k"};
-static const char *mux_pllpcm_clkm[] = {"pll_p", "pll_c", "pll_m", "clk_m"};
-static const char *mux_pllpdc_clkm[] = {"pll_p", "pll_d_out0", "pll_c",
-					"clk_m"};
-static const char *mux_pllmcp_clkm[] = {"pll_m", "pll_c", "pll_p", "clk_m"};
+static const char *i2s1_parents[] = { "pll_a_out0", "audio_2x", "pll_p",
+				      "clk_m" };
+static const char *i2s2_parents[] = { "pll_a_out0", "audio_2x", "pll_p",
+				      "clk_m" };
+static const char *pwm_parents[] = { "pll_p", "pll_c", "audio", "clk_m",
+				     "clk_32k" };
+static const char *mux_pllpcm_clkm[] = { "pll_p", "pll_c", "pll_m", "clk_m" };
+static const char *mux_pllpdc_clkm[] = { "pll_p", "pll_d_out0", "pll_c",
+					 "clk_m" };
+static const char *mux_pllmcp_clkm[] = { "pll_m", "pll_c", "pll_p", "clk_m" };
 
 static struct tegra_periph_init_data tegra_periph_clk_list[] = {
 	TEGRA_INIT_DATA_MUX("i2s1", i2s1_parents,     CLK_SOURCE_I2S1,   11, TEGRA_PERIPH_ON_APB, TEGRA20_CLK_I2S1),
@@ -802,7 +798,7 @@ static void __init tegra20_periph_clk_init(void)
 {
 	struct tegra_periph_init_data *data;
 	struct clk *clk;
-	int i;
+	unsigned int i;
 
 	/* ac97 */
 	clk = tegra_clk_register_periph_gate("ac97", "pll_a_out0",
@@ -1025,44 +1021,45 @@ static struct tegra_cpu_car_ops tegra20_cpu_car_ops = {
 };
 
 static struct tegra_clk_init_table init_table[] __initdata = {
-	{TEGRA20_CLK_PLL_P, TEGRA20_CLK_CLK_MAX, 216000000, 1},
-	{TEGRA20_CLK_PLL_P_OUT1, TEGRA20_CLK_CLK_MAX, 28800000, 1},
-	{TEGRA20_CLK_PLL_P_OUT2, TEGRA20_CLK_CLK_MAX, 48000000, 1},
-	{TEGRA20_CLK_PLL_P_OUT3, TEGRA20_CLK_CLK_MAX, 72000000, 1},
-	{TEGRA20_CLK_PLL_P_OUT4, TEGRA20_CLK_CLK_MAX, 24000000, 1},
-	{TEGRA20_CLK_PLL_C, TEGRA20_CLK_CLK_MAX, 600000000, 1},
-	{TEGRA20_CLK_PLL_C_OUT1, TEGRA20_CLK_CLK_MAX, 120000000, 1},
-	{TEGRA20_CLK_SCLK, TEGRA20_CLK_PLL_C_OUT1, 0, 1},
-	{TEGRA20_CLK_HCLK, TEGRA20_CLK_CLK_MAX, 0, 1},
-	{TEGRA20_CLK_PCLK, TEGRA20_CLK_CLK_MAX, 60000000, 1},
-	{TEGRA20_CLK_CSITE, TEGRA20_CLK_CLK_MAX, 0, 1},
-	{TEGRA20_CLK_EMC, TEGRA20_CLK_CLK_MAX, 0, 1},
-	{TEGRA20_CLK_CCLK, TEGRA20_CLK_CLK_MAX, 0, 1},
-	{TEGRA20_CLK_UARTA, TEGRA20_CLK_PLL_P, 0, 0},
-	{TEGRA20_CLK_UARTB, TEGRA20_CLK_PLL_P, 0, 0},
-	{TEGRA20_CLK_UARTC, TEGRA20_CLK_PLL_P, 0, 0},
-	{TEGRA20_CLK_UARTD, TEGRA20_CLK_PLL_P, 0, 0},
-	{TEGRA20_CLK_UARTE, TEGRA20_CLK_PLL_P, 0, 0},
-	{TEGRA20_CLK_PLL_A, TEGRA20_CLK_CLK_MAX, 56448000, 1},
-	{TEGRA20_CLK_PLL_A_OUT0, TEGRA20_CLK_CLK_MAX, 11289600, 1},
-	{TEGRA20_CLK_CDEV1, TEGRA20_CLK_CLK_MAX, 0, 1},
-	{TEGRA20_CLK_BLINK, TEGRA20_CLK_CLK_MAX, 32768, 1},
-	{TEGRA20_CLK_I2S1, TEGRA20_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA20_CLK_I2S2, TEGRA20_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA20_CLK_SDMMC1, TEGRA20_CLK_PLL_P, 48000000, 0},
-	{TEGRA20_CLK_SDMMC3, TEGRA20_CLK_PLL_P, 48000000, 0},
-	{TEGRA20_CLK_SDMMC4, TEGRA20_CLK_PLL_P, 48000000, 0},
-	{TEGRA20_CLK_SPI, TEGRA20_CLK_PLL_P, 20000000, 0},
-	{TEGRA20_CLK_SBC1, TEGRA20_CLK_PLL_P, 100000000, 0},
-	{TEGRA20_CLK_SBC2, TEGRA20_CLK_PLL_P, 100000000, 0},
-	{TEGRA20_CLK_SBC3, TEGRA20_CLK_PLL_P, 100000000, 0},
-	{TEGRA20_CLK_SBC4, TEGRA20_CLK_PLL_P, 100000000, 0},
-	{TEGRA20_CLK_HOST1X, TEGRA20_CLK_PLL_C, 150000000, 0},
-	{TEGRA20_CLK_DISP1, TEGRA20_CLK_PLL_P, 600000000, 0},
-	{TEGRA20_CLK_DISP2, TEGRA20_CLK_PLL_P, 600000000, 0},
-	{TEGRA20_CLK_GR2D, TEGRA20_CLK_PLL_C, 300000000, 0},
-	{TEGRA20_CLK_GR3D, TEGRA20_CLK_PLL_C, 300000000, 0},
-	{TEGRA20_CLK_CLK_MAX, TEGRA20_CLK_CLK_MAX, 0, 0}, /* This MUST be the last entry */
+	{ TEGRA20_CLK_PLL_P, TEGRA20_CLK_CLK_MAX, 216000000, 1 },
+	{ TEGRA20_CLK_PLL_P_OUT1, TEGRA20_CLK_CLK_MAX, 28800000, 1 },
+	{ TEGRA20_CLK_PLL_P_OUT2, TEGRA20_CLK_CLK_MAX, 48000000, 1 },
+	{ TEGRA20_CLK_PLL_P_OUT3, TEGRA20_CLK_CLK_MAX, 72000000, 1 },
+	{ TEGRA20_CLK_PLL_P_OUT4, TEGRA20_CLK_CLK_MAX, 24000000, 1 },
+	{ TEGRA20_CLK_PLL_C, TEGRA20_CLK_CLK_MAX, 600000000, 1 },
+	{ TEGRA20_CLK_PLL_C_OUT1, TEGRA20_CLK_CLK_MAX, 120000000, 1 },
+	{ TEGRA20_CLK_SCLK, TEGRA20_CLK_PLL_C_OUT1, 0, 1 },
+	{ TEGRA20_CLK_HCLK, TEGRA20_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA20_CLK_PCLK, TEGRA20_CLK_CLK_MAX, 60000000, 1 },
+	{ TEGRA20_CLK_CSITE, TEGRA20_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA20_CLK_EMC, TEGRA20_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA20_CLK_CCLK, TEGRA20_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA20_CLK_UARTA, TEGRA20_CLK_PLL_P, 0, 0 },
+	{ TEGRA20_CLK_UARTB, TEGRA20_CLK_PLL_P, 0, 0 },
+	{ TEGRA20_CLK_UARTC, TEGRA20_CLK_PLL_P, 0, 0 },
+	{ TEGRA20_CLK_UARTD, TEGRA20_CLK_PLL_P, 0, 0 },
+	{ TEGRA20_CLK_UARTE, TEGRA20_CLK_PLL_P, 0, 0 },
+	{ TEGRA20_CLK_PLL_A, TEGRA20_CLK_CLK_MAX, 56448000, 1 },
+	{ TEGRA20_CLK_PLL_A_OUT0, TEGRA20_CLK_CLK_MAX, 11289600, 1 },
+	{ TEGRA20_CLK_CDEV1, TEGRA20_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA20_CLK_BLINK, TEGRA20_CLK_CLK_MAX, 32768, 1 },
+	{ TEGRA20_CLK_I2S1, TEGRA20_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA20_CLK_I2S2, TEGRA20_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA20_CLK_SDMMC1, TEGRA20_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA20_CLK_SDMMC3, TEGRA20_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA20_CLK_SDMMC4, TEGRA20_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA20_CLK_SPI, TEGRA20_CLK_PLL_P, 20000000, 0 },
+	{ TEGRA20_CLK_SBC1, TEGRA20_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA20_CLK_SBC2, TEGRA20_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA20_CLK_SBC3, TEGRA20_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA20_CLK_SBC4, TEGRA20_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA20_CLK_HOST1X, TEGRA20_CLK_PLL_C, 150000000, 0 },
+	{ TEGRA20_CLK_DISP1, TEGRA20_CLK_PLL_P, 600000000, 0 },
+	{ TEGRA20_CLK_DISP2, TEGRA20_CLK_PLL_P, 600000000, 0 },
+	{ TEGRA20_CLK_GR2D, TEGRA20_CLK_PLL_C, 300000000, 0 },
+	{ TEGRA20_CLK_GR3D, TEGRA20_CLK_PLL_C, 300000000, 0 },
+	/* must be the last entry */
+	{ TEGRA20_CLK_CLK_MAX, TEGRA20_CLK_CLK_MAX, 0, 0 },
 };
 
 static void __init tegra20_clock_apply_init_table(void)
@@ -1076,16 +1073,17 @@ static void __init tegra20_clock_apply_init_table(void)
  * table under two names.
  */
 static struct tegra_clk_duplicate tegra_clk_duplicates[] = {
-	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,   "utmip-pad",    NULL),
-	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,   "tegra-ehci.0", NULL),
-	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,   "tegra-otg",    NULL),
-	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_CCLK,   NULL,           "cpu"),
-	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_CLK_MAX, NULL, NULL), /* Must be the last entry */
+	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,    "utmip-pad",     NULL),
+	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,    "tegra-ehci.0",  NULL),
+	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_USBD,    "tegra-otg",     NULL),
+	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_CCLK,    NULL,           "cpu"),
+	/* must be the last entry */
+	TEGRA_CLK_DUPLICATE(TEGRA20_CLK_CLK_MAX, NULL,            NULL),
 };
 
 static const struct of_device_id pmc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra20-pmc" },
-	{},
+	{ },
 };
 
 static void __init tegra20_clock_init(struct device_node *np)
diff --git a/drivers/clk/tegra/clk-tegra210.c b/drivers/clk/tegra/clk-tegra210.c
new file mode 100644
index 000000000000..637041fd53ad
--- /dev/null
+++ b/drivers/clk/tegra/clk-tegra210.c
@@ -0,0 +1,2840 @@
+/*
+ * Copyright (c) 2012-2014 NVIDIA CORPORATION.  All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+#include <linux/clk/tegra.h>
+#include <dt-bindings/clock/tegra210-car.h>
+
+#include "clk.h"
+#include "clk-id.h"
+
+/*
+ * TEGRA210_CAR_BANK_COUNT: the number of peripheral clock register
+ * banks present in the Tegra210 CAR IP block.  The banks are
+ * identified by single letters, e.g.: L, H, U, V, W, X, Y.  See
+ * periph_regs[] in drivers/clk/tegra/clk.c
+ */
+#define TEGRA210_CAR_BANK_COUNT			7
+
+#define CLK_SOURCE_CSITE 0x1d4
+#define CLK_SOURCE_EMC 0x19c
+
+#define PLLC_BASE 0x80
+#define PLLC_OUT 0x84
+#define PLLC_MISC0 0x88
+#define PLLC_MISC1 0x8c
+#define PLLC_MISC2 0x5d0
+#define PLLC_MISC3 0x5d4
+
+#define PLLC2_BASE 0x4e8
+#define PLLC2_MISC0 0x4ec
+#define PLLC2_MISC1 0x4f0
+#define PLLC2_MISC2 0x4f4
+#define PLLC2_MISC3 0x4f8
+
+#define PLLC3_BASE 0x4fc
+#define PLLC3_MISC0 0x500
+#define PLLC3_MISC1 0x504
+#define PLLC3_MISC2 0x508
+#define PLLC3_MISC3 0x50c
+
+#define PLLM_BASE 0x90
+#define PLLM_MISC1 0x98
+#define PLLM_MISC2 0x9c
+#define PLLP_BASE 0xa0
+#define PLLP_MISC0 0xac
+#define PLLP_MISC1 0x680
+#define PLLA_BASE 0xb0
+#define PLLA_MISC0 0xbc
+#define PLLA_MISC1 0xb8
+#define PLLA_MISC2 0x5d8
+#define PLLD_BASE 0xd0
+#define PLLD_MISC0 0xdc
+#define PLLD_MISC1 0xd8
+#define PLLU_BASE 0xc0
+#define PLLU_OUTA 0xc4
+#define PLLU_MISC0 0xcc
+#define PLLU_MISC1 0xc8
+#define PLLX_BASE 0xe0
+#define PLLX_MISC0 0xe4
+#define PLLX_MISC1 0x510
+#define PLLX_MISC2 0x514
+#define PLLX_MISC3 0x518
+#define PLLX_MISC4 0x5f0
+#define PLLX_MISC5 0x5f4
+#define PLLE_BASE 0xe8
+#define PLLE_MISC0 0xec
+#define PLLD2_BASE 0x4b8
+#define PLLD2_MISC0 0x4bc
+#define PLLD2_MISC1 0x570
+#define PLLD2_MISC2 0x574
+#define PLLD2_MISC3 0x578
+#define PLLE_AUX 0x48c
+#define PLLRE_BASE 0x4c4
+#define PLLRE_MISC0 0x4c8
+#define PLLDP_BASE 0x590
+#define PLLDP_MISC 0x594
+
+#define PLLC4_BASE 0x5a4
+#define PLLC4_MISC0 0x5a8
+#define PLLC4_OUT 0x5e4
+#define PLLMB_BASE 0x5e8
+#define PLLMB_MISC1 0x5ec
+#define PLLA1_BASE 0x6a4
+#define PLLA1_MISC0 0x6a8
+#define PLLA1_MISC1 0x6ac
+#define PLLA1_MISC2 0x6b0
+#define PLLA1_MISC3 0x6b4
+
+#define PLLU_IDDQ_BIT 31
+#define PLLCX_IDDQ_BIT 27
+#define PLLRE_IDDQ_BIT 24
+#define PLLA_IDDQ_BIT 25
+#define PLLD_IDDQ_BIT 20
+#define PLLSS_IDDQ_BIT 18
+#define PLLM_IDDQ_BIT 5
+#define PLLMB_IDDQ_BIT 17
+#define PLLXP_IDDQ_BIT 3
+
+#define PLLCX_RESET_BIT 30
+
+#define PLL_BASE_LOCK BIT(27)
+#define PLLCX_BASE_LOCK BIT(26)
+#define PLLE_MISC_LOCK BIT(11)
+#define PLLRE_MISC_LOCK BIT(27)
+
+#define PLL_MISC_LOCK_ENABLE 18
+#define PLLC_MISC_LOCK_ENABLE 24
+#define PLLDU_MISC_LOCK_ENABLE 22
+#define PLLU_MISC_LOCK_ENABLE 29
+#define PLLE_MISC_LOCK_ENABLE 9
+#define PLLRE_MISC_LOCK_ENABLE 30
+#define PLLSS_MISC_LOCK_ENABLE 30
+#define PLLP_MISC_LOCK_ENABLE 18
+#define PLLM_MISC_LOCK_ENABLE 4
+#define PLLMB_MISC_LOCK_ENABLE 16
+#define PLLA_MISC_LOCK_ENABLE 28
+#define PLLU_MISC_LOCK_ENABLE 29
+#define PLLD_MISC_LOCK_ENABLE 18
+
+#define PLLA_SDM_DIN_MASK 0xffff
+#define PLLA_SDM_EN_MASK BIT(26)
+
+#define PLLD_SDM_EN_MASK BIT(16)
+
+#define PLLD2_SDM_EN_MASK BIT(31)
+#define PLLD2_SSC_EN_MASK BIT(30)
+
+#define PLLDP_SS_CFG	0x598
+#define PLLDP_SDM_EN_MASK BIT(31)
+#define PLLDP_SSC_EN_MASK BIT(30)
+#define PLLDP_SS_CTRL1	0x59c
+#define PLLDP_SS_CTRL2	0x5a0
+
+#define PMC_PLLM_WB0_OVERRIDE 0x1dc
+#define PMC_PLLM_WB0_OVERRIDE_2 0x2b0
+
+#define UTMIP_PLL_CFG2 0x488
+#define UTMIP_PLL_CFG2_STABLE_COUNT(x) (((x) & 0xfff) << 6)
+#define UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(x) (((x) & 0x3f) << 18)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN BIT(0)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERUP BIT(1)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN BIT(2)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERUP BIT(3)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERDOWN BIT(4)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_C_POWERUP BIT(5)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_D_POWERDOWN BIT(24)
+#define UTMIP_PLL_CFG2_FORCE_PD_SAMP_D_POWERUP BIT(25)
+
+#define UTMIP_PLL_CFG1 0x484
+#define UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(x) (((x) & 0x1f) << 27)
+#define UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(x) (((x) & 0xfff) << 0)
+#define UTMIP_PLL_CFG1_FORCE_PLLU_POWERUP BIT(17)
+#define UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN BIT(16)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP BIT(15)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN BIT(14)
+#define UTMIP_PLL_CFG1_FORCE_PLL_ACTIVE_POWERDOWN BIT(12)
+
+#define UTMIPLL_HW_PWRDN_CFG0			0x52c
+#define UTMIPLL_HW_PWRDN_CFG0_UTMIPLL_LOCK	BIT(31)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_START_STATE	BIT(25)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE	BIT(24)
+#define UTMIPLL_HW_PWRDN_CFG0_IDDQ_PD_INCLUDE	BIT(7)
+#define UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET	BIT(6)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_RESET_INPUT_VALUE	BIT(5)
+#define UTMIPLL_HW_PWRDN_CFG0_SEQ_IN_SWCTL	BIT(4)
+#define UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL	BIT(2)
+#define UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE	BIT(1)
+#define UTMIPLL_HW_PWRDN_CFG0_IDDQ_SWCTL	BIT(0)
+
+#define PLLU_HW_PWRDN_CFG0			0x530
+#define PLLU_HW_PWRDN_CFG0_IDDQ_PD_INCLUDE	BIT(28)
+#define PLLU_HW_PWRDN_CFG0_SEQ_ENABLE		BIT(24)
+#define PLLU_HW_PWRDN_CFG0_USE_SWITCH_DETECT	BIT(7)
+#define PLLU_HW_PWRDN_CFG0_USE_LOCKDET		BIT(6)
+#define PLLU_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL	BIT(2)
+#define PLLU_HW_PWRDN_CFG0_CLK_SWITCH_SWCTL	BIT(0)
+
+#define XUSB_PLL_CFG0				0x534
+#define XUSB_PLL_CFG0_UTMIPLL_LOCK_DLY		0x3ff
+#define XUSB_PLL_CFG0_PLLU_LOCK_DLY_MASK	(0x3ff << 14)
+
+#define SPARE_REG0 0x55c
+#define CLK_M_DIVISOR_SHIFT 2
+#define CLK_M_DIVISOR_MASK 0x3
+
+/*
+ * SDM fractional divisor is 16-bit 2's complement signed number within
+ * (-2^12 ... 2^12-1) range. Represented in PLL data structure as unsigned
+ * 16-bit value, with "0" divisor mapped to 0xFFFF. Data "0" is used to
+ * indicate that SDM is disabled.
+ *
+ * Effective ndiv value when SDM is enabled: ndiv + 1/2 + sdm_din/2^13
+ */
+#define PLL_SDM_COEFF BIT(13)
+#define sdin_din_to_data(din)	((u16)((din) ? : 0xFFFFU))
+#define sdin_data_to_din(dat)	(((dat) == 0xFFFFU) ? 0 : (s16)dat)
+
+/* Tegra CPU clock and reset control regs */
+#define CLK_RST_CONTROLLER_CPU_CMPLX_STATUS	0x470
+
+#ifdef CONFIG_PM_SLEEP
+static struct cpu_clk_suspend_context {
+	u32 clk_csite_src;
+} tegra210_cpu_clk_sctx;
+#endif
+
+static void __iomem *clk_base;
+static void __iomem *pmc_base;
+
+static unsigned long osc_freq;
+static unsigned long pll_ref_freq;
+
+static DEFINE_SPINLOCK(pll_d_lock);
+static DEFINE_SPINLOCK(pll_e_lock);
+static DEFINE_SPINLOCK(pll_re_lock);
+static DEFINE_SPINLOCK(pll_u_lock);
+static DEFINE_SPINLOCK(emc_lock);
+
+/* possible OSC frequencies in Hz */
+static unsigned long tegra210_input_freq[] = {
+	[5] = 38400000,
+	[8] = 12000000,
+};
+
+static const char *mux_pllmcp_clkm[] = {
+	"pll_m", "pll_c", "pll_p", "clk_m", "pll_m_ud", "pll_mb", "pll_mb",
+	"pll_p",
+};
+#define mux_pllmcp_clkm_idx NULL
+
+#define PLL_ENABLE			(1 << 30)
+
+#define PLLCX_MISC1_IDDQ		(1 << 27)
+#define PLLCX_MISC0_RESET		(1 << 30)
+
+#define PLLCX_MISC0_DEFAULT_VALUE	0x40080000
+#define PLLCX_MISC0_WRITE_MASK		0x400ffffb
+#define PLLCX_MISC1_DEFAULT_VALUE	0x08000000
+#define PLLCX_MISC1_WRITE_MASK		0x08003cff
+#define PLLCX_MISC2_DEFAULT_VALUE	0x1f720f05
+#define PLLCX_MISC2_WRITE_MASK		0xffffff17
+#define PLLCX_MISC3_DEFAULT_VALUE	0x000000c4
+#define PLLCX_MISC3_WRITE_MASK		0x00ffffff
+
+/* PLLA */
+#define PLLA_BASE_IDDQ			(1 << 25)
+#define PLLA_BASE_LOCK			(1 << 27)
+
+#define PLLA_MISC0_LOCK_ENABLE		(1 << 28)
+#define PLLA_MISC0_LOCK_OVERRIDE	(1 << 27)
+
+#define PLLA_MISC2_EN_SDM		(1 << 26)
+#define PLLA_MISC2_EN_DYNRAMP		(1 << 25)
+
+#define PLLA_MISC0_DEFAULT_VALUE	0x12000020
+#define PLLA_MISC0_WRITE_MASK		0x7fffffff
+#define PLLA_MISC2_DEFAULT_VALUE	0x0
+#define PLLA_MISC2_WRITE_MASK		0x06ffffff
+
+/* PLLD */
+#define PLLD_MISC0_EN_SDM		(1 << 16)
+#define PLLD_MISC0_LOCK_OVERRIDE	(1 << 17)
+#define PLLD_MISC0_LOCK_ENABLE		(1 << 18)
+#define PLLD_MISC0_IDDQ			(1 << 20)
+#define PLLD_MISC0_DSI_CLKENABLE	(1 << 21)
+
+#define PLLD_MISC0_DEFAULT_VALUE	0x00140000
+#define PLLD_MISC0_WRITE_MASK		0x3ff7ffff
+#define PLLD_MISC1_DEFAULT_VALUE	0x20
+#define PLLD_MISC1_WRITE_MASK		0x00ffffff
+
+/* PLLD2 and PLLDP  and PLLC4 */
+#define PLLDSS_BASE_LOCK		(1 << 27)
+#define PLLDSS_BASE_LOCK_OVERRIDE	(1 << 24)
+#define PLLDSS_BASE_IDDQ		(1 << 18)
+#define PLLDSS_BASE_REF_SEL_SHIFT	25
+#define PLLDSS_BASE_REF_SEL_MASK	(0x3 << PLLDSS_BASE_REF_SEL_SHIFT)
+
+#define PLLDSS_MISC0_LOCK_ENABLE	(1 << 30)
+
+#define PLLDSS_MISC1_CFG_EN_SDM		(1 << 31)
+#define PLLDSS_MISC1_CFG_EN_SSC		(1 << 30)
+
+#define PLLD2_MISC0_DEFAULT_VALUE	0x40000020
+#define PLLD2_MISC1_CFG_DEFAULT_VALUE	0x10000000
+#define PLLD2_MISC2_CTRL1_DEFAULT_VALUE	0x0
+#define PLLD2_MISC3_CTRL2_DEFAULT_VALUE	0x0
+
+#define PLLDP_MISC0_DEFAULT_VALUE	0x40000020
+#define PLLDP_MISC1_CFG_DEFAULT_VALUE	0xc0000000
+#define PLLDP_MISC2_CTRL1_DEFAULT_VALUE	0xf400f0da
+#define PLLDP_MISC3_CTRL2_DEFAULT_VALUE	0x2004f400
+
+#define PLLDSS_MISC0_WRITE_MASK		0x47ffffff
+#define PLLDSS_MISC1_CFG_WRITE_MASK	0xf8000000
+#define PLLDSS_MISC2_CTRL1_WRITE_MASK	0xffffffff
+#define PLLDSS_MISC3_CTRL2_WRITE_MASK	0xffffffff
+
+#define PLLC4_MISC0_DEFAULT_VALUE	0x40000000
+
+/* PLLRE */
+#define PLLRE_MISC0_LOCK_ENABLE		(1 << 30)
+#define PLLRE_MISC0_LOCK_OVERRIDE	(1 << 29)
+#define PLLRE_MISC0_LOCK		(1 << 27)
+#define PLLRE_MISC0_IDDQ		(1 << 24)
+
+#define PLLRE_BASE_DEFAULT_VALUE	0x0
+#define PLLRE_MISC0_DEFAULT_VALUE	0x41000000
+
+#define PLLRE_BASE_DEFAULT_MASK		0x1c000000
+#define PLLRE_MISC0_WRITE_MASK		0x67ffffff
+
+/* PLLX */
+#define PLLX_USE_DYN_RAMP		1
+#define PLLX_BASE_LOCK			(1 << 27)
+
+#define PLLX_MISC0_FO_G_DISABLE		(0x1 << 28)
+#define PLLX_MISC0_LOCK_ENABLE		(0x1 << 18)
+
+#define PLLX_MISC2_DYNRAMP_STEPB_SHIFT	24
+#define PLLX_MISC2_DYNRAMP_STEPB_MASK	(0xFF << PLLX_MISC2_DYNRAMP_STEPB_SHIFT)
+#define PLLX_MISC2_DYNRAMP_STEPA_SHIFT	16
+#define PLLX_MISC2_DYNRAMP_STEPA_MASK	(0xFF << PLLX_MISC2_DYNRAMP_STEPA_SHIFT)
+#define PLLX_MISC2_NDIV_NEW_SHIFT	8
+#define PLLX_MISC2_NDIV_NEW_MASK	(0xFF << PLLX_MISC2_NDIV_NEW_SHIFT)
+#define PLLX_MISC2_LOCK_OVERRIDE	(0x1 << 4)
+#define PLLX_MISC2_DYNRAMP_DONE		(0x1 << 2)
+#define PLLX_MISC2_EN_DYNRAMP		(0x1 << 0)
+
+#define PLLX_MISC3_IDDQ			(0x1 << 3)
+
+#define PLLX_MISC0_DEFAULT_VALUE	PLLX_MISC0_LOCK_ENABLE
+#define PLLX_MISC0_WRITE_MASK		0x10c40000
+#define PLLX_MISC1_DEFAULT_VALUE	0x20
+#define PLLX_MISC1_WRITE_MASK		0x00ffffff
+#define PLLX_MISC2_DEFAULT_VALUE	0x0
+#define PLLX_MISC2_WRITE_MASK		0xffffff11
+#define PLLX_MISC3_DEFAULT_VALUE	PLLX_MISC3_IDDQ
+#define PLLX_MISC3_WRITE_MASK		0x01ff0f0f
+#define PLLX_MISC4_DEFAULT_VALUE	0x0
+#define PLLX_MISC4_WRITE_MASK		0x8000ffff
+#define PLLX_MISC5_DEFAULT_VALUE	0x0
+#define PLLX_MISC5_WRITE_MASK		0x0000ffff
+
+#define PLLX_HW_CTRL_CFG		0x548
+#define PLLX_HW_CTRL_CFG_SWCTRL		(0x1 << 0)
+
+/* PLLMB */
+#define PLLMB_BASE_LOCK			(1 << 27)
+
+#define PLLMB_MISC1_LOCK_OVERRIDE	(1 << 18)
+#define PLLMB_MISC1_IDDQ		(1 << 17)
+#define PLLMB_MISC1_LOCK_ENABLE		(1 << 16)
+
+#define PLLMB_MISC1_DEFAULT_VALUE	0x00030000
+#define PLLMB_MISC1_WRITE_MASK		0x0007ffff
+
+/* PLLP */
+#define PLLP_BASE_OVERRIDE		(1 << 28)
+#define PLLP_BASE_LOCK			(1 << 27)
+
+#define PLLP_MISC0_LOCK_ENABLE		(1 << 18)
+#define PLLP_MISC0_LOCK_OVERRIDE	(1 << 17)
+#define PLLP_MISC0_IDDQ			(1 << 3)
+
+#define PLLP_MISC1_HSIO_EN_SHIFT	29
+#define PLLP_MISC1_HSIO_EN		(1 << PLLP_MISC1_HSIO_EN_SHIFT)
+#define PLLP_MISC1_XUSB_EN_SHIFT	28
+#define PLLP_MISC1_XUSB_EN		(1 << PLLP_MISC1_XUSB_EN_SHIFT)
+
+#define PLLP_MISC0_DEFAULT_VALUE	0x00040008
+#define PLLP_MISC1_DEFAULT_VALUE	0x0
+
+#define PLLP_MISC0_WRITE_MASK		0xdc6000f
+#define PLLP_MISC1_WRITE_MASK		0x70ffffff
+
+/* PLLU */
+#define PLLU_BASE_LOCK			(1 << 27)
+#define PLLU_BASE_OVERRIDE		(1 << 24)
+#define PLLU_BASE_CLKENABLE_USB		(1 << 21)
+#define PLLU_BASE_CLKENABLE_HSIC	(1 << 22)
+#define PLLU_BASE_CLKENABLE_ICUSB	(1 << 23)
+#define PLLU_BASE_CLKENABLE_48M		(1 << 25)
+#define PLLU_BASE_CLKENABLE_ALL		(PLLU_BASE_CLKENABLE_USB |\
+					 PLLU_BASE_CLKENABLE_HSIC |\
+					 PLLU_BASE_CLKENABLE_ICUSB |\
+					 PLLU_BASE_CLKENABLE_48M)
+
+#define PLLU_MISC0_IDDQ			(1 << 31)
+#define PLLU_MISC0_LOCK_ENABLE		(1 << 29)
+#define PLLU_MISC1_LOCK_OVERRIDE	(1 << 0)
+
+#define PLLU_MISC0_DEFAULT_VALUE	0xa0000000
+#define PLLU_MISC1_DEFAULT_VALUE	0x0
+
+#define PLLU_MISC0_WRITE_MASK		0xbfffffff
+#define PLLU_MISC1_WRITE_MASK		0x00000007
+
+static inline void _pll_misc_chk_default(void __iomem *base,
+					struct tegra_clk_pll_params *params,
+					u8 misc_num, u32 default_val, u32 mask)
+{
+	u32 boot_val = readl_relaxed(base + params->ext_misc_reg[misc_num]);
+
+	boot_val &= mask;
+	default_val &= mask;
+	if (boot_val != default_val) {
+		pr_warn("boot misc%d 0x%x: expected 0x%x\n",
+			misc_num, boot_val, default_val);
+		pr_warn(" (comparison mask = 0x%x)\n", mask);
+		params->defaults_set = false;
+	}
+}
+
+/*
+ * PLLCX: PLLC, PLLC2, PLLC3, PLLA1
+ * Hybrid PLLs with dynamic ramp. Dynamic ramp is allowed for any transition
+ * that changes NDIV only, while PLL is already locked.
+ */
+static void pllcx_check_defaults(struct tegra_clk_pll_params *params)
+{
+	u32 default_val;
+
+	default_val = PLLCX_MISC0_DEFAULT_VALUE & (~PLLCX_MISC0_RESET);
+	_pll_misc_chk_default(clk_base, params, 0, default_val,
+			PLLCX_MISC0_WRITE_MASK);
+
+	default_val = PLLCX_MISC1_DEFAULT_VALUE & (~PLLCX_MISC1_IDDQ);
+	_pll_misc_chk_default(clk_base, params, 1, default_val,
+			PLLCX_MISC1_WRITE_MASK);
+
+	default_val = PLLCX_MISC2_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, params, 2, default_val,
+			PLLCX_MISC2_WRITE_MASK);
+
+	default_val = PLLCX_MISC3_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, params, 3, default_val,
+			PLLCX_MISC3_WRITE_MASK);
+}
+
+static void tegra210_pllcx_set_defaults(const char *name,
+					struct tegra_clk_pll *pllcx)
+{
+	pllcx->params->defaults_set = true;
+
+	if (readl_relaxed(clk_base + pllcx->params->base_reg) &
+			PLL_ENABLE) {
+		/* PLL is ON: only check if defaults already set */
+		pllcx_check_defaults(pllcx->params);
+		pr_warn("%s already enabled. Postponing set full defaults\n",
+			name);
+		return;
+	}
+
+	/* Defaults assert PLL reset, and set IDDQ */
+	writel_relaxed(PLLCX_MISC0_DEFAULT_VALUE,
+			clk_base + pllcx->params->ext_misc_reg[0]);
+	writel_relaxed(PLLCX_MISC1_DEFAULT_VALUE,
+			clk_base + pllcx->params->ext_misc_reg[1]);
+	writel_relaxed(PLLCX_MISC2_DEFAULT_VALUE,
+			clk_base + pllcx->params->ext_misc_reg[2]);
+	writel_relaxed(PLLCX_MISC3_DEFAULT_VALUE,
+			clk_base + pllcx->params->ext_misc_reg[3]);
+	udelay(1);
+}
+
+static void _pllc_set_defaults(struct tegra_clk_pll *pllcx)
+{
+	tegra210_pllcx_set_defaults("PLL_C", pllcx);
+}
+
+static void _pllc2_set_defaults(struct tegra_clk_pll *pllcx)
+{
+	tegra210_pllcx_set_defaults("PLL_C2", pllcx);
+}
+
+static void _pllc3_set_defaults(struct tegra_clk_pll *pllcx)
+{
+	tegra210_pllcx_set_defaults("PLL_C3", pllcx);
+}
+
+static void _plla1_set_defaults(struct tegra_clk_pll *pllcx)
+{
+	tegra210_pllcx_set_defaults("PLL_A1", pllcx);
+}
+
+/*
+ * PLLA
+ * PLL with dynamic ramp and fractional SDM. Dynamic ramp is not used.
+ * Fractional SDM is allowed to provide exact audio rates.
+ */
+static void tegra210_plla_set_defaults(struct tegra_clk_pll *plla)
+{
+	u32 mask;
+	u32 val = readl_relaxed(clk_base + plla->params->base_reg);
+
+	plla->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		if (val & PLLA_BASE_IDDQ) {
+			pr_warn("PLL_A boot enabled with IDDQ set\n");
+			plla->params->defaults_set = false;
+		}
+
+		pr_warn("PLL_A already enabled. Postponing set full defaults\n");
+
+		val = PLLA_MISC0_DEFAULT_VALUE;	/* ignore lock enable */
+		mask = PLLA_MISC0_LOCK_ENABLE | PLLA_MISC0_LOCK_OVERRIDE;
+		_pll_misc_chk_default(clk_base, plla->params, 0, val,
+				~mask & PLLA_MISC0_WRITE_MASK);
+
+		val = PLLA_MISC2_DEFAULT_VALUE; /* ignore all but control bit */
+		_pll_misc_chk_default(clk_base, plla->params, 2, val,
+				PLLA_MISC2_EN_DYNRAMP);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + plla->params->ext_misc_reg[0]);
+		val &= ~mask;
+		val |= PLLA_MISC0_DEFAULT_VALUE & mask;
+		writel_relaxed(val, clk_base + plla->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect, disable dynamic ramp and SDM */
+	val |= PLLA_BASE_IDDQ;
+	writel_relaxed(val, clk_base + plla->params->base_reg);
+	writel_relaxed(PLLA_MISC0_DEFAULT_VALUE,
+			clk_base + plla->params->ext_misc_reg[0]);
+	writel_relaxed(PLLA_MISC2_DEFAULT_VALUE,
+			clk_base + plla->params->ext_misc_reg[2]);
+	udelay(1);
+}
+
+/*
+ * PLLD
+ * PLL with fractional SDM.
+ */
+static void tegra210_plld_set_defaults(struct tegra_clk_pll *plld)
+{
+	u32 val;
+	u32 mask = 0xffff;
+
+	plld->params->defaults_set = true;
+
+	if (readl_relaxed(clk_base + plld->params->base_reg) &
+			PLL_ENABLE) {
+		pr_warn("PLL_D already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		val = PLLD_MISC1_DEFAULT_VALUE;
+		_pll_misc_chk_default(clk_base, plld->params, 1,
+				val, PLLD_MISC1_WRITE_MASK);
+
+		/* ignore lock, DSI and SDM controls, make sure IDDQ not set */
+		val = PLLD_MISC0_DEFAULT_VALUE & (~PLLD_MISC0_IDDQ);
+		mask |= PLLD_MISC0_DSI_CLKENABLE | PLLD_MISC0_LOCK_ENABLE |
+			PLLD_MISC0_LOCK_OVERRIDE | PLLD_MISC0_EN_SDM;
+		_pll_misc_chk_default(clk_base, plld->params, 0, val,
+				~mask & PLLD_MISC0_WRITE_MASK);
+
+		/* Enable lock detect */
+		mask = PLLD_MISC0_LOCK_ENABLE | PLLD_MISC0_LOCK_OVERRIDE;
+		val = readl_relaxed(clk_base + plld->params->ext_misc_reg[0]);
+		val &= ~mask;
+		val |= PLLD_MISC0_DEFAULT_VALUE & mask;
+		writel_relaxed(val, clk_base + plld->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	val = readl_relaxed(clk_base + plld->params->ext_misc_reg[0]);
+	val &= PLLD_MISC0_DSI_CLKENABLE;
+	val |= PLLD_MISC0_DEFAULT_VALUE;
+	/* set IDDQ, enable lock detect, disable SDM */
+	writel_relaxed(val, clk_base + plld->params->ext_misc_reg[0]);
+	writel_relaxed(PLLD_MISC1_DEFAULT_VALUE, clk_base +
+			plld->params->ext_misc_reg[1]);
+	udelay(1);
+}
+
+/*
+ * PLLD2, PLLDP
+ * PLL with fractional SDM and Spread Spectrum (SDM is a must if SSC is used).
+ */
+static void plldss_defaults(const char *pll_name, struct tegra_clk_pll *plldss,
+		u32 misc0_val, u32 misc1_val, u32 misc2_val, u32 misc3_val)
+{
+	u32 default_val;
+	u32 val = readl_relaxed(clk_base + plldss->params->base_reg);
+
+	plldss->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		pr_warn("%s already enabled. Postponing set full defaults\n",
+			 pll_name);
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		if (val & PLLDSS_BASE_IDDQ) {
+			pr_warn("plldss boot enabled with IDDQ set\n");
+			plldss->params->defaults_set = false;
+		}
+
+		/* ignore lock enable */
+		default_val = misc0_val;
+		_pll_misc_chk_default(clk_base, plldss->params, 0, default_val,
+				     PLLDSS_MISC0_WRITE_MASK &
+				     (~PLLDSS_MISC0_LOCK_ENABLE));
+
+		/*
+		 * If SSC is used, check all settings, otherwise just confirm
+		 * that SSC is not used on boot as well. Do nothing when using
+		 * this function for PLLC4 that has only MISC0.
+		 */
+		if (plldss->params->ssc_ctrl_en_mask) {
+			default_val = misc1_val;
+			_pll_misc_chk_default(clk_base, plldss->params, 1,
+				default_val, PLLDSS_MISC1_CFG_WRITE_MASK);
+			default_val = misc2_val;
+			_pll_misc_chk_default(clk_base, plldss->params, 2,
+				default_val, PLLDSS_MISC2_CTRL1_WRITE_MASK);
+			default_val = misc3_val;
+			_pll_misc_chk_default(clk_base, plldss->params, 3,
+				default_val, PLLDSS_MISC3_CTRL2_WRITE_MASK);
+		} else if (plldss->params->ext_misc_reg[1]) {
+			default_val = misc1_val;
+			_pll_misc_chk_default(clk_base, plldss->params, 1,
+				default_val, PLLDSS_MISC1_CFG_WRITE_MASK &
+				(~PLLDSS_MISC1_CFG_EN_SDM));
+		}
+
+		/* Enable lock detect */
+		if (val & PLLDSS_BASE_LOCK_OVERRIDE) {
+			val &= ~PLLDSS_BASE_LOCK_OVERRIDE;
+			writel_relaxed(val, clk_base +
+					plldss->params->base_reg);
+		}
+
+		val = readl_relaxed(clk_base + plldss->params->ext_misc_reg[0]);
+		val &= ~PLLDSS_MISC0_LOCK_ENABLE;
+		val |= misc0_val & PLLDSS_MISC0_LOCK_ENABLE;
+		writel_relaxed(val, clk_base + plldss->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect, configure SDM/SSC  */
+	val |= PLLDSS_BASE_IDDQ;
+	val &= ~PLLDSS_BASE_LOCK_OVERRIDE;
+	writel_relaxed(val, clk_base + plldss->params->base_reg);
+
+	/* When using this function for PLLC4 exit here */
+	if (!plldss->params->ext_misc_reg[1]) {
+		writel_relaxed(misc0_val, clk_base +
+				plldss->params->ext_misc_reg[0]);
+		udelay(1);
+		return;
+	}
+
+	writel_relaxed(misc0_val, clk_base +
+			plldss->params->ext_misc_reg[0]);
+	/* if SSC used set by 1st enable */
+	writel_relaxed(misc1_val & (~PLLDSS_MISC1_CFG_EN_SSC),
+			clk_base + plldss->params->ext_misc_reg[1]);
+	writel_relaxed(misc2_val, clk_base + plldss->params->ext_misc_reg[2]);
+	writel_relaxed(misc3_val, clk_base + plldss->params->ext_misc_reg[3]);
+	udelay(1);
+}
+
+static void tegra210_plld2_set_defaults(struct tegra_clk_pll *plld2)
+{
+	plldss_defaults("PLL_D2", plld2, PLLD2_MISC0_DEFAULT_VALUE,
+			PLLD2_MISC1_CFG_DEFAULT_VALUE,
+			PLLD2_MISC2_CTRL1_DEFAULT_VALUE,
+			PLLD2_MISC3_CTRL2_DEFAULT_VALUE);
+}
+
+static void tegra210_plldp_set_defaults(struct tegra_clk_pll *plldp)
+{
+	plldss_defaults("PLL_DP", plldp, PLLDP_MISC0_DEFAULT_VALUE,
+			PLLDP_MISC1_CFG_DEFAULT_VALUE,
+			PLLDP_MISC2_CTRL1_DEFAULT_VALUE,
+			PLLDP_MISC3_CTRL2_DEFAULT_VALUE);
+}
+
+/*
+ * PLLC4
+ * Base and misc0 layout is the same as PLLD2/PLLDP, but no SDM/SSC support.
+ * VCO is exposed to the clock tree via fixed 1/3 and 1/5 dividers.
+ */
+static void tegra210_pllc4_set_defaults(struct tegra_clk_pll *pllc4)
+{
+	plldss_defaults("PLL_C4", pllc4, PLLC4_MISC0_DEFAULT_VALUE, 0, 0, 0);
+}
+
+/*
+ * PLLRE
+ * VCO is exposed to the clock tree directly along with post-divider output
+ */
+static void tegra210_pllre_set_defaults(struct tegra_clk_pll *pllre)
+{
+	u32 mask;
+	u32 val = readl_relaxed(clk_base + pllre->params->base_reg);
+
+	pllre->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		pr_warn("PLL_RE already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		val &= PLLRE_BASE_DEFAULT_MASK;
+		if (val != PLLRE_BASE_DEFAULT_VALUE) {
+			pr_warn("pllre boot base 0x%x : expected 0x%x\n",
+				val, PLLRE_BASE_DEFAULT_VALUE);
+			pr_warn("(comparison mask = 0x%x)\n",
+				PLLRE_BASE_DEFAULT_MASK);
+			pllre->params->defaults_set = false;
+		}
+
+		/* Ignore lock enable */
+		val = PLLRE_MISC0_DEFAULT_VALUE & (~PLLRE_MISC0_IDDQ);
+		mask = PLLRE_MISC0_LOCK_ENABLE | PLLRE_MISC0_LOCK_OVERRIDE;
+		_pll_misc_chk_default(clk_base, pllre->params, 0, val,
+				~mask & PLLRE_MISC0_WRITE_MASK);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + pllre->params->ext_misc_reg[0]);
+		val &= ~mask;
+		val |= PLLRE_MISC0_DEFAULT_VALUE & mask;
+		writel_relaxed(val, clk_base + pllre->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect */
+	val &= ~PLLRE_BASE_DEFAULT_MASK;
+	val |= PLLRE_BASE_DEFAULT_VALUE & PLLRE_BASE_DEFAULT_MASK;
+	writel_relaxed(val, clk_base + pllre->params->base_reg);
+	writel_relaxed(PLLRE_MISC0_DEFAULT_VALUE,
+			clk_base + pllre->params->ext_misc_reg[0]);
+	udelay(1);
+}
+
+static void pllx_get_dyn_steps(struct clk_hw *hw, u32 *step_a, u32 *step_b)
+{
+	unsigned long input_rate;
+
+	/* cf rate */
+	if (!IS_ERR_OR_NULL(hw->clk))
+		input_rate = clk_hw_get_rate(clk_hw_get_parent(hw));
+	else
+		input_rate = 38400000;
+
+	input_rate /= tegra_pll_get_fixed_mdiv(hw, input_rate);
+
+	switch (input_rate) {
+	case 12000000:
+	case 12800000:
+	case 13000000:
+		*step_a = 0x2B;
+		*step_b = 0x0B;
+		return;
+	case 19200000:
+		*step_a = 0x12;
+		*step_b = 0x08;
+		return;
+	case 38400000:
+		*step_a = 0x04;
+		*step_b = 0x05;
+		return;
+	default:
+		pr_err("%s: Unexpected reference rate %lu\n",
+			__func__, input_rate);
+		BUG();
+	}
+}
+
+static void pllx_check_defaults(struct tegra_clk_pll *pll)
+{
+	u32 default_val;
+
+	default_val = PLLX_MISC0_DEFAULT_VALUE;
+	/* ignore lock enable */
+	_pll_misc_chk_default(clk_base, pll->params, 0, default_val,
+			PLLX_MISC0_WRITE_MASK & (~PLLX_MISC0_LOCK_ENABLE));
+
+	default_val = PLLX_MISC1_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, pll->params, 1, default_val,
+			PLLX_MISC1_WRITE_MASK);
+
+	/* ignore all but control bit */
+	default_val = PLLX_MISC2_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, pll->params, 2,
+			default_val, PLLX_MISC2_EN_DYNRAMP);
+
+	default_val = PLLX_MISC3_DEFAULT_VALUE & (~PLLX_MISC3_IDDQ);
+	_pll_misc_chk_default(clk_base, pll->params, 3, default_val,
+			PLLX_MISC3_WRITE_MASK);
+
+	default_val = PLLX_MISC4_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, pll->params, 4, default_val,
+			PLLX_MISC4_WRITE_MASK);
+
+	default_val = PLLX_MISC5_DEFAULT_VALUE;
+	_pll_misc_chk_default(clk_base, pll->params, 5, default_val,
+			PLLX_MISC5_WRITE_MASK);
+}
+
+static void tegra210_pllx_set_defaults(struct tegra_clk_pll *pllx)
+{
+	u32 val;
+	u32 step_a, step_b;
+
+	pllx->params->defaults_set = true;
+
+	/* Get ready dyn ramp state machine settings */
+	pllx_get_dyn_steps(&pllx->hw, &step_a, &step_b);
+	val = PLLX_MISC2_DEFAULT_VALUE & (~PLLX_MISC2_DYNRAMP_STEPA_MASK) &
+		(~PLLX_MISC2_DYNRAMP_STEPB_MASK);
+	val |= step_a << PLLX_MISC2_DYNRAMP_STEPA_SHIFT;
+	val |= step_b << PLLX_MISC2_DYNRAMP_STEPB_SHIFT;
+
+	if (readl_relaxed(clk_base + pllx->params->base_reg) & PLL_ENABLE) {
+		pr_warn("PLL_X already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		pllx_check_defaults(pllx);
+
+		/* Configure dyn ramp, disable lock override */
+		writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[2]);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + pllx->params->ext_misc_reg[0]);
+		val &= ~PLLX_MISC0_LOCK_ENABLE;
+		val |= PLLX_MISC0_DEFAULT_VALUE & PLLX_MISC0_LOCK_ENABLE;
+		writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* Enable lock detect and CPU output */
+	writel_relaxed(PLLX_MISC0_DEFAULT_VALUE, clk_base +
+			pllx->params->ext_misc_reg[0]);
+
+	/* Setup */
+	writel_relaxed(PLLX_MISC1_DEFAULT_VALUE, clk_base +
+			pllx->params->ext_misc_reg[1]);
+
+	/* Configure dyn ramp state machine, disable lock override */
+	writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[2]);
+
+	/* Set IDDQ */
+	writel_relaxed(PLLX_MISC3_DEFAULT_VALUE, clk_base +
+			pllx->params->ext_misc_reg[3]);
+
+	/* Disable SDM */
+	writel_relaxed(PLLX_MISC4_DEFAULT_VALUE, clk_base +
+			pllx->params->ext_misc_reg[4]);
+	writel_relaxed(PLLX_MISC5_DEFAULT_VALUE, clk_base +
+			pllx->params->ext_misc_reg[5]);
+	udelay(1);
+}
+
+/* PLLMB */
+static void tegra210_pllmb_set_defaults(struct tegra_clk_pll *pllmb)
+{
+	u32 mask, val = readl_relaxed(clk_base + pllmb->params->base_reg);
+
+	pllmb->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		pr_warn("PLL_MB already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		val = PLLMB_MISC1_DEFAULT_VALUE & (~PLLMB_MISC1_IDDQ);
+		mask = PLLMB_MISC1_LOCK_ENABLE | PLLMB_MISC1_LOCK_OVERRIDE;
+		_pll_misc_chk_default(clk_base, pllmb->params, 0, val,
+				~mask & PLLMB_MISC1_WRITE_MASK);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + pllmb->params->ext_misc_reg[0]);
+		val &= ~mask;
+		val |= PLLMB_MISC1_DEFAULT_VALUE & mask;
+		writel_relaxed(val, clk_base + pllmb->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect */
+	writel_relaxed(PLLMB_MISC1_DEFAULT_VALUE,
+			clk_base + pllmb->params->ext_misc_reg[0]);
+	udelay(1);
+}
+
+/*
+ * PLLP
+ * VCO is exposed to the clock tree directly along with post-divider output.
+ * Both VCO and post-divider output rates are fixed at 408MHz and 204MHz,
+ * respectively.
+ */
+static void pllp_check_defaults(struct tegra_clk_pll *pll, bool enabled)
+{
+	u32 val, mask;
+
+	/* Ignore lock enable (will be set), make sure not in IDDQ if enabled */
+	val = PLLP_MISC0_DEFAULT_VALUE & (~PLLP_MISC0_IDDQ);
+	mask = PLLP_MISC0_LOCK_ENABLE | PLLP_MISC0_LOCK_OVERRIDE;
+	if (!enabled)
+		mask |= PLLP_MISC0_IDDQ;
+	_pll_misc_chk_default(clk_base, pll->params, 0, val,
+			~mask & PLLP_MISC0_WRITE_MASK);
+
+	/* Ignore branch controls */
+	val = PLLP_MISC1_DEFAULT_VALUE;
+	mask = PLLP_MISC1_HSIO_EN | PLLP_MISC1_XUSB_EN;
+	_pll_misc_chk_default(clk_base, pll->params, 1, val,
+			~mask & PLLP_MISC1_WRITE_MASK);
+}
+
+static void tegra210_pllp_set_defaults(struct tegra_clk_pll *pllp)
+{
+	u32 mask;
+	u32 val = readl_relaxed(clk_base + pllp->params->base_reg);
+
+	pllp->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		pr_warn("PLL_P already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		pllp_check_defaults(pllp, true);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + pllp->params->ext_misc_reg[0]);
+		mask = PLLP_MISC0_LOCK_ENABLE | PLLP_MISC0_LOCK_OVERRIDE;
+		val &= ~mask;
+		val |= PLLP_MISC0_DEFAULT_VALUE & mask;
+		writel_relaxed(val, clk_base + pllp->params->ext_misc_reg[0]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect */
+	writel_relaxed(PLLP_MISC0_DEFAULT_VALUE,
+			clk_base + pllp->params->ext_misc_reg[0]);
+
+	/* Preserve branch control */
+	val = readl_relaxed(clk_base + pllp->params->ext_misc_reg[1]);
+	mask = PLLP_MISC1_HSIO_EN | PLLP_MISC1_XUSB_EN;
+	val &= mask;
+	val |= ~mask & PLLP_MISC1_DEFAULT_VALUE;
+	writel_relaxed(val, clk_base + pllp->params->ext_misc_reg[1]);
+	udelay(1);
+}
+
+/*
+ * PLLU
+ * VCO is exposed to the clock tree directly along with post-divider output.
+ * Both VCO and post-divider output rates are fixed at 480MHz and 240MHz,
+ * respectively.
+ */
+static void pllu_check_defaults(struct tegra_clk_pll *pll, bool hw_control)
+{
+	u32 val, mask;
+
+	/* Ignore lock enable (will be set) and IDDQ if under h/w control */
+	val = PLLU_MISC0_DEFAULT_VALUE & (~PLLU_MISC0_IDDQ);
+	mask = PLLU_MISC0_LOCK_ENABLE | (hw_control ? PLLU_MISC0_IDDQ : 0);
+	_pll_misc_chk_default(clk_base, pll->params, 0, val,
+			~mask & PLLU_MISC0_WRITE_MASK);
+
+	val = PLLU_MISC1_DEFAULT_VALUE;
+	mask = PLLU_MISC1_LOCK_OVERRIDE;
+	_pll_misc_chk_default(clk_base, pll->params, 1, val,
+			~mask & PLLU_MISC1_WRITE_MASK);
+}
+
+static void tegra210_pllu_set_defaults(struct tegra_clk_pll *pllu)
+{
+	u32 val = readl_relaxed(clk_base + pllu->params->base_reg);
+
+	pllu->params->defaults_set = true;
+
+	if (val & PLL_ENABLE) {
+		pr_warn("PLL_U already enabled. Postponing set full defaults\n");
+
+		/*
+		 * PLL is ON: check if defaults already set, then set those
+		 * that can be updated in flight.
+		 */
+		pllu_check_defaults(pllu, false);
+
+		/* Enable lock detect */
+		val = readl_relaxed(clk_base + pllu->params->ext_misc_reg[0]);
+		val &= ~PLLU_MISC0_LOCK_ENABLE;
+		val |= PLLU_MISC0_DEFAULT_VALUE & PLLU_MISC0_LOCK_ENABLE;
+		writel_relaxed(val, clk_base + pllu->params->ext_misc_reg[0]);
+
+		val = readl_relaxed(clk_base + pllu->params->ext_misc_reg[1]);
+		val &= ~PLLU_MISC1_LOCK_OVERRIDE;
+		val |= PLLU_MISC1_DEFAULT_VALUE & PLLU_MISC1_LOCK_OVERRIDE;
+		writel_relaxed(val, clk_base + pllu->params->ext_misc_reg[1]);
+		udelay(1);
+
+		return;
+	}
+
+	/* set IDDQ, enable lock detect */
+	writel_relaxed(PLLU_MISC0_DEFAULT_VALUE,
+			clk_base + pllu->params->ext_misc_reg[0]);
+	writel_relaxed(PLLU_MISC1_DEFAULT_VALUE,
+			clk_base + pllu->params->ext_misc_reg[1]);
+	udelay(1);
+}
+
+#define mask(w) ((1 << (w)) - 1)
+#define divm_mask(p) mask(p->params->div_nmp->divm_width)
+#define divn_mask(p) mask(p->params->div_nmp->divn_width)
+#define divp_mask(p) (p->params->flags & TEGRA_PLLU ? PLLU_POST_DIVP_MASK :\
+		      mask(p->params->div_nmp->divp_width))
+
+#define divm_shift(p) ((p)->params->div_nmp->divm_shift)
+#define divn_shift(p) ((p)->params->div_nmp->divn_shift)
+#define divp_shift(p) ((p)->params->div_nmp->divp_shift)
+
+#define divm_mask_shifted(p) (divm_mask(p) << divm_shift(p))
+#define divn_mask_shifted(p) (divn_mask(p) << divn_shift(p))
+#define divp_mask_shifted(p) (divp_mask(p) << divp_shift(p))
+
+#define PLL_LOCKDET_DELAY 2	/* Lock detection safety delays */
+static int tegra210_wait_for_mask(struct tegra_clk_pll *pll,
+				  u32 reg, u32 mask)
+{
+	int i;
+	u32 val = 0;
+
+	for (i = 0; i < pll->params->lock_delay / PLL_LOCKDET_DELAY + 1; i++) {
+		udelay(PLL_LOCKDET_DELAY);
+		val = readl_relaxed(clk_base + reg);
+		if ((val & mask) == mask) {
+			udelay(PLL_LOCKDET_DELAY);
+			return 0;
+		}
+	}
+	return -ETIMEDOUT;
+}
+
+static int tegra210_pllx_dyn_ramp(struct tegra_clk_pll *pllx,
+		struct tegra_clk_pll_freq_table *cfg)
+{
+	u32 val, base, ndiv_new_mask;
+
+	ndiv_new_mask = (divn_mask(pllx) >> pllx->params->div_nmp->divn_shift)
+			 << PLLX_MISC2_NDIV_NEW_SHIFT;
+
+	val = readl_relaxed(clk_base + pllx->params->ext_misc_reg[2]);
+	val &= (~ndiv_new_mask);
+	val |= cfg->n << PLLX_MISC2_NDIV_NEW_SHIFT;
+	writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[2]);
+	udelay(1);
+
+	val = readl_relaxed(clk_base + pllx->params->ext_misc_reg[2]);
+	val |= PLLX_MISC2_EN_DYNRAMP;
+	writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[2]);
+	udelay(1);
+
+	tegra210_wait_for_mask(pllx, pllx->params->ext_misc_reg[2],
+			       PLLX_MISC2_DYNRAMP_DONE);
+
+	base = readl_relaxed(clk_base + pllx->params->base_reg) &
+		(~divn_mask_shifted(pllx));
+	base |= cfg->n << pllx->params->div_nmp->divn_shift;
+	writel_relaxed(base, clk_base + pllx->params->base_reg);
+	udelay(1);
+
+	val &= ~PLLX_MISC2_EN_DYNRAMP;
+	writel_relaxed(val, clk_base + pllx->params->ext_misc_reg[2]);
+	udelay(1);
+
+	pr_debug("%s: dynamic ramp to m = %u n = %u p = %u, Fout = %lu kHz\n",
+		 __clk_get_name(pllx->hw.clk), cfg->m, cfg->n, cfg->p,
+		 cfg->input_rate / cfg->m * cfg->n /
+		 pllx->params->pdiv_tohw[cfg->p].pdiv / 1000);
+
+	return 0;
+}
+
+/*
+ * Common configuration for PLLs with fixed input divider policy:
+ * - always set fixed M-value based on the reference rate
+ * - always set P-value value 1:1 for output rates above VCO minimum, and
+ *   choose minimum necessary P-value for output rates below VCO maximum
+ * - calculate N-value based on selected M and P
+ * - calculate SDM_DIN fractional part
+ */
+static int tegra210_pll_fixed_mdiv_cfg(struct clk_hw *hw,
+			       struct tegra_clk_pll_freq_table *cfg,
+			       unsigned long rate, unsigned long input_rate)
+{
+	struct tegra_clk_pll *pll = to_clk_pll(hw);
+	struct tegra_clk_pll_params *params = pll->params;
+	int p;
+	unsigned long cf, p_rate;
+	u32 pdiv;
+
+	if (!rate)
+		return -EINVAL;
+
+	if (!(params->flags & TEGRA_PLL_VCO_OUT)) {
+		p = DIV_ROUND_UP(params->vco_min, rate);
+		p = params->round_p_to_pdiv(p, &pdiv);
+	} else {
+		p = rate >= params->vco_min ? 1 : -EINVAL;
+	}
+
+	if (IS_ERR_VALUE(p))
+		return -EINVAL;
+
+	cfg->m = tegra_pll_get_fixed_mdiv(hw, input_rate);
+	cfg->p = p;
+
+	/* Store P as HW value, as that is what is expected */
+	cfg->p = tegra_pll_p_div_to_hw(pll, cfg->p);
+
+	p_rate = rate * p;
+	if (p_rate > params->vco_max)
+		p_rate = params->vco_max;
+	cf = input_rate / cfg->m;
+	cfg->n = p_rate / cf;
+
+	cfg->sdm_data = 0;
+	if (params->sdm_ctrl_reg) {
+		unsigned long rem = p_rate - cf * cfg->n;
+		/* If ssc is enabled SDM enabled as well, even for integer n */
+		if (rem || params->ssc_ctrl_reg) {
+			u64 s = rem * PLL_SDM_COEFF;
+
+			do_div(s, cf);
+			s -= PLL_SDM_COEFF / 2;
+			cfg->sdm_data = sdin_din_to_data(s);
+		}
+	}
+
+	cfg->input_rate = input_rate;
+	cfg->output_rate = rate;
+
+	return 0;
+}
+
+/*
+ * clk_pll_set_gain - set gain to m, n to calculate correct VCO rate
+ *
+ * @cfg: struct tegra_clk_pll_freq_table * cfg
+ *
+ * For Normal mode:
+ *     Fvco = Fref * NDIV / MDIV
+ *
+ * For fractional mode:
+ *     Fvco = Fref * (NDIV + 0.5 + SDM_DIN / PLL_SDM_COEFF) / MDIV
+ */
+static void tegra210_clk_pll_set_gain(struct tegra_clk_pll_freq_table *cfg)
+{
+	cfg->n = cfg->n * PLL_SDM_COEFF + PLL_SDM_COEFF/2 +
+			sdin_data_to_din(cfg->sdm_data);
+	cfg->m *= PLL_SDM_COEFF;
+}
+
+static unsigned long
+tegra210_clk_adjust_vco_min(struct tegra_clk_pll_params *params,
+			    unsigned long parent_rate)
+{
+	unsigned long vco_min = params->vco_min;
+
+	params->vco_min += DIV_ROUND_UP(parent_rate, PLL_SDM_COEFF);
+	vco_min = min(vco_min, params->vco_min);
+
+	return vco_min;
+}
+
+static struct div_nmp pllx_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+};
+/*
+ * PLL post divider maps - two types: quasi-linear and exponential
+ * post divider.
+ */
+#define PLL_QLIN_PDIV_MAX	16
+static const struct pdiv_map pll_qlin_pdiv_to_hw[] = {
+	{ .pdiv =  1, .hw_val =  0 },
+	{ .pdiv =  2, .hw_val =  1 },
+	{ .pdiv =  3, .hw_val =  2 },
+	{ .pdiv =  4, .hw_val =  3 },
+	{ .pdiv =  5, .hw_val =  4 },
+	{ .pdiv =  6, .hw_val =  5 },
+	{ .pdiv =  8, .hw_val =  6 },
+	{ .pdiv =  9, .hw_val =  7 },
+	{ .pdiv = 10, .hw_val =  8 },
+	{ .pdiv = 12, .hw_val =  9 },
+	{ .pdiv = 15, .hw_val = 10 },
+	{ .pdiv = 16, .hw_val = 11 },
+	{ .pdiv = 18, .hw_val = 12 },
+	{ .pdiv = 20, .hw_val = 13 },
+	{ .pdiv = 24, .hw_val = 14 },
+	{ .pdiv = 30, .hw_val = 15 },
+	{ .pdiv = 32, .hw_val = 16 },
+};
+
+static u32 pll_qlin_p_to_pdiv(u32 p, u32 *pdiv)
+{
+	int i;
+
+	if (p) {
+		for (i = 0; i <= PLL_QLIN_PDIV_MAX; i++) {
+			if (p <= pll_qlin_pdiv_to_hw[i].pdiv) {
+				if (pdiv)
+					*pdiv = i;
+				return pll_qlin_pdiv_to_hw[i].pdiv;
+			}
+		}
+	}
+
+	return -EINVAL;
+}
+
+#define PLL_EXPO_PDIV_MAX	7
+static const struct pdiv_map pll_expo_pdiv_to_hw[] = {
+	{ .pdiv =   1, .hw_val = 0 },
+	{ .pdiv =   2, .hw_val = 1 },
+	{ .pdiv =   4, .hw_val = 2 },
+	{ .pdiv =   8, .hw_val = 3 },
+	{ .pdiv =  16, .hw_val = 4 },
+	{ .pdiv =  32, .hw_val = 5 },
+	{ .pdiv =  64, .hw_val = 6 },
+	{ .pdiv = 128, .hw_val = 7 },
+};
+
+static u32 pll_expo_p_to_pdiv(u32 p, u32 *pdiv)
+{
+	if (p) {
+		u32 i = fls(p);
+
+		if (i == ffs(p))
+			i--;
+
+		if (i <= PLL_EXPO_PDIV_MAX) {
+			if (pdiv)
+				*pdiv = i;
+			return 1 << i;
+		}
+	}
+	return -EINVAL;
+}
+
+static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
+	/* 1 GHz */
+	{ 12000000, 1000000000, 166, 1, 1, 0 }, /* actual: 996.0 MHz */
+	{ 13000000, 1000000000, 153, 1, 1, 0 }, /* actual: 994.0 MHz */
+	{ 38400000, 1000000000, 156, 3, 1, 0 }, /* actual: 998.4 MHz */
+	{        0,          0,   0, 0, 0, 0 },
+};
+
+static struct tegra_clk_pll_params pll_x_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 38400000,
+	.vco_min = 1350000000,
+	.vco_max = 3000000000UL,
+	.base_reg = PLLX_BASE,
+	.misc_reg = PLLX_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.ext_misc_reg[0] = PLLX_MISC0,
+	.ext_misc_reg[1] = PLLX_MISC1,
+	.ext_misc_reg[2] = PLLX_MISC2,
+	.ext_misc_reg[3] = PLLX_MISC3,
+	.ext_misc_reg[4] = PLLX_MISC4,
+	.ext_misc_reg[5] = PLLX_MISC5,
+	.iddq_reg = PLLX_MISC3,
+	.iddq_bit_idx = PLLXP_IDDQ_BIT,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.mdiv_default = 2,
+	.dyn_ramp_reg = PLLX_MISC2,
+	.stepa_shift = 16,
+	.stepb_shift = 24,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllx_nmp,
+	.freq_table = pll_x_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
+	.dyn_ramp = tegra210_pllx_dyn_ramp,
+	.set_defaults = tegra210_pllx_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct div_nmp pllc_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 10,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_freq_table pll_cx_freq_table[] = {
+	{ 12000000, 510000000, 85, 1, 1, 0 },
+	{ 13000000, 510000000, 78, 1, 1, 0 }, /* actual: 507.0 MHz */
+	{ 38400000, 510000000, 79, 3, 1, 0 }, /* actual: 505.6 MHz */
+	{        0,         0,  0, 0, 0, 0 },
+};
+
+static struct tegra_clk_pll_params pll_c_params = {
+	.input_min = 12000000,
+	.input_max = 700000000,
+	.cf_min = 12000000,
+	.cf_max = 50000000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLC_BASE,
+	.misc_reg = PLLC_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLC_MISC1,
+	.iddq_bit_idx = PLLCX_IDDQ_BIT,
+	.reset_reg = PLLC_MISC0,
+	.reset_bit_idx = PLLCX_RESET_BIT,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLC_MISC0,
+	.ext_misc_reg[1] = PLLC_MISC1,
+	.ext_misc_reg[2] = PLLC_MISC2,
+	.ext_misc_reg[3] = PLLC_MISC3,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.mdiv_default = 3,
+	.div_nmp = &pllc_nmp,
+	.freq_table = pll_cx_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.set_defaults = _pllc_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct div_nmp pllcx_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 10,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_params pll_c2_params = {
+	.input_min = 12000000,
+	.input_max = 700000000,
+	.cf_min = 12000000,
+	.cf_max = 50000000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLC2_BASE,
+	.misc_reg = PLLC2_MISC0,
+	.iddq_reg = PLLC2_MISC1,
+	.iddq_bit_idx = PLLCX_IDDQ_BIT,
+	.reset_reg = PLLC2_MISC0,
+	.reset_bit_idx = PLLCX_RESET_BIT,
+	.lock_mask = PLLCX_BASE_LOCK,
+	.lock_delay = 300,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.mdiv_default = 3,
+	.div_nmp = &pllcx_nmp,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLC2_MISC0,
+	.ext_misc_reg[1] = PLLC2_MISC1,
+	.ext_misc_reg[2] = PLLC2_MISC2,
+	.ext_misc_reg[3] = PLLC2_MISC3,
+	.freq_table = pll_cx_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.set_defaults = _pllc2_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct tegra_clk_pll_params pll_c3_params = {
+	.input_min = 12000000,
+	.input_max = 700000000,
+	.cf_min = 12000000,
+	.cf_max = 50000000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLC3_BASE,
+	.misc_reg = PLLC3_MISC0,
+	.lock_mask = PLLCX_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLC3_MISC1,
+	.iddq_bit_idx = PLLCX_IDDQ_BIT,
+	.reset_reg = PLLC3_MISC0,
+	.reset_bit_idx = PLLCX_RESET_BIT,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.mdiv_default = 3,
+	.div_nmp = &pllcx_nmp,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLC3_MISC0,
+	.ext_misc_reg[1] = PLLC3_MISC1,
+	.ext_misc_reg[2] = PLLC3_MISC2,
+	.ext_misc_reg[3] = PLLC3_MISC3,
+	.freq_table = pll_cx_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.set_defaults = _pllc3_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct div_nmp pllss_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 19,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_freq_table pll_c4_vco_freq_table[] = {
+	{ 12000000, 600000000, 50, 1, 0, 0 },
+	{ 13000000, 600000000, 46, 1, 0, 0 }, /* actual: 598.0 MHz */
+	{ 38400000, 600000000, 62, 4, 0, 0 }, /* actual: 595.2 MHz */
+	{        0,         0,  0, 0, 0, 0 },
+};
+
+static const struct clk_div_table pll_vco_post_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 =  8 },
+	{ .val =  7, .div = 10 },
+	{ .val =  8, .div = 12 },
+	{ .val =  9, .div = 16 },
+	{ .val = 10, .div = 12 },
+	{ .val = 11, .div = 16 },
+	{ .val = 12, .div = 20 },
+	{ .val = 13, .div = 24 },
+	{ .val = 14, .div = 32 },
+	{ .val =  0, .div =  0 },
+};
+
+static struct tegra_clk_pll_params pll_c4_vco_params = {
+	.input_min = 9600000,
+	.input_max = 800000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 500000000,
+	.vco_max = 1080000000,
+	.base_reg = PLLC4_BASE,
+	.misc_reg = PLLC4_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLC4_MISC0,
+	.iddq_reg = PLLC4_BASE,
+	.iddq_bit_idx = PLLSS_IDDQ_BIT,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.mdiv_default = 3,
+	.div_nmp = &pllss_nmp,
+	.freq_table = pll_c4_vco_freq_table,
+	.set_defaults = tegra210_pllc4_set_defaults,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_VCO_OUT,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
+	{ 12000000,  800000000,  66, 1, 0, 0 }, /* actual: 792.0 MHz */
+	{ 13000000,  800000000,  61, 1, 0, 0 }, /* actual: 793.0 MHz */
+	{ 38400000,  297600000,  93, 4, 2, 0 },
+	{ 38400000,  400000000, 125, 4, 2, 0 },
+	{ 38400000,  532800000, 111, 4, 1, 0 },
+	{ 38400000,  665600000, 104, 3, 1, 0 },
+	{ 38400000,  800000000, 125, 3, 1, 0 },
+	{ 38400000,  931200000,  97, 4, 0, 0 },
+	{ 38400000, 1065600000, 111, 4, 0, 0 },
+	{ 38400000, 1200000000, 125, 4, 0, 0 },
+	{ 38400000, 1331200000, 104, 3, 0, 0 },
+	{ 38400000, 1459200000,  76, 2, 0, 0 },
+	{ 38400000, 1600000000, 125, 3, 0, 0 },
+	{        0,          0,   0, 0, 0, 0 },
+};
+
+static struct div_nmp pllm_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.override_divm_shift = 0,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.override_divn_shift = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+	.override_divp_shift = 27,
+};
+
+static struct tegra_clk_pll_params pll_m_params = {
+	.input_min = 9600000,
+	.input_max = 500000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 800000000,
+	.vco_max = 1866000000,
+	.base_reg = PLLM_BASE,
+	.misc_reg = PLLM_MISC2,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_enable_bit_idx = PLLM_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.iddq_reg = PLLM_MISC2,
+	.iddq_bit_idx = PLLM_IDDQ_BIT,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLM_MISC2,
+	.ext_misc_reg[1] = PLLM_MISC1,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllm_nmp,
+	.pmc_divnm_reg = PMC_PLLM_WB0_OVERRIDE,
+	.pmc_divp_reg = PMC_PLLM_WB0_OVERRIDE_2,
+	.freq_table = pll_m_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct tegra_clk_pll_params pll_mb_params = {
+	.input_min = 9600000,
+	.input_max = 500000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 800000000,
+	.vco_max = 1866000000,
+	.base_reg = PLLMB_BASE,
+	.misc_reg = PLLMB_MISC1,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLMB_MISC1,
+	.iddq_bit_idx = PLLMB_IDDQ_BIT,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLMB_MISC1,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllm_nmp,
+	.freq_table = pll_m_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.set_defaults = tegra210_pllmb_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+
+static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
+	/* PLLE special case: use cpcon field to store cml divider value */
+	{ 672000000, 100000000, 125, 42, 0, 13 },
+	{ 624000000, 100000000, 125, 39, 0, 13 },
+	{ 336000000, 100000000, 125, 21, 0, 13 },
+	{ 312000000, 100000000, 200, 26, 0, 14 },
+	{  38400000, 100000000, 125,  2, 0, 14 },
+	{  12000000, 100000000, 200,  1, 0, 14 },
+	{         0,         0,   0,  0, 0,  0 },
+};
+
+static struct div_nmp plle_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 24,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_params pll_e_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 38400000,
+	.vco_min = 1600000000,
+	.vco_max = 2500000000U,
+	.base_reg = PLLE_BASE,
+	.misc_reg = PLLE_MISC0,
+	.aux_reg = PLLE_AUX,
+	.lock_mask = PLLE_MISC_LOCK,
+	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
+	.lock_delay = 300,
+	.div_nmp = &plle_nmp,
+	.freq_table = pll_e_freq_table,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_LOCK_MISC | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
+	.fixed_rate = 100000000,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct tegra_clk_pll_freq_table pll_re_vco_freq_table[] = {
+	{ 12000000, 672000000, 56, 1, 0, 0 },
+	{ 13000000, 672000000, 51, 1, 0, 0 }, /* actual: 663.0 MHz */
+	{ 38400000, 672000000, 70, 4, 0, 0 },
+	{        0,         0,  0, 0, 0, 0 },
+};
+
+static struct div_nmp pllre_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 16,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_params pll_re_vco_params = {
+	.input_min = 9600000,
+	.input_max = 800000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 350000000,
+	.vco_max = 700000000,
+	.base_reg = PLLRE_BASE,
+	.misc_reg = PLLRE_MISC0,
+	.lock_mask = PLLRE_MISC_LOCK,
+	.lock_delay = 300,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.ext_misc_reg[0] = PLLRE_MISC0,
+	.iddq_reg = PLLRE_MISC0,
+	.iddq_bit_idx = PLLRE_IDDQ_BIT,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllre_nmp,
+	.freq_table = pll_re_vco_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_PLL_LOCK_MISC | TEGRA_PLL_VCO_OUT,
+	.set_defaults = tegra210_pllre_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct div_nmp pllp_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 10,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
+	{ 12000000, 408000000, 34, 1, 0, 0 },
+	{ 38400000, 408000000, 85, 8, 0, 0 }, /* cf = 4.8MHz, allowed exception */
+	{        0,         0,  0, 0, 0, 0 },
+};
+
+static struct tegra_clk_pll_params pll_p_params = {
+	.input_min = 9600000,
+	.input_max = 800000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 350000000,
+	.vco_max = 700000000,
+	.base_reg = PLLP_BASE,
+	.misc_reg = PLLP_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLP_MISC0,
+	.iddq_bit_idx = PLLXP_IDDQ_BIT,
+	.ext_misc_reg[0] = PLLP_MISC0,
+	.ext_misc_reg[1] = PLLP_MISC1,
+	.div_nmp = &pllp_nmp,
+	.freq_table = pll_p_freq_table,
+	.fixed_rate = 408000000,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_USE_LOCK | TEGRA_PLL_VCO_OUT,
+	.set_defaults = tegra210_pllp_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct tegra_clk_pll_params pll_a1_params = {
+	.input_min = 12000000,
+	.input_max = 700000000,
+	.cf_min = 12000000,
+	.cf_max = 50000000,
+	.vco_min = 600000000,
+	.vco_max = 1200000000,
+	.base_reg = PLLA1_BASE,
+	.misc_reg = PLLA1_MISC0,
+	.lock_mask = PLLCX_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLA1_MISC0,
+	.iddq_bit_idx = PLLCX_IDDQ_BIT,
+	.reset_reg = PLLA1_MISC0,
+	.reset_bit_idx = PLLCX_RESET_BIT,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllc_nmp,
+	.ext_misc_reg[0] = PLLA1_MISC0,
+	.ext_misc_reg[1] = PLLA1_MISC1,
+	.ext_misc_reg[2] = PLLA1_MISC2,
+	.ext_misc_reg[3] = PLLA1_MISC3,
+	.freq_table = pll_cx_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.set_defaults = _plla1_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+static struct div_nmp plla_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
+	{ 12000000, 282240000, 47, 1, 1, 1, 0xf148 }, /* actual: 282240234 */
+	{ 12000000, 368640000, 61, 1, 1, 1, 0xfe15 }, /* actual: 368640381 */
+	{ 12000000, 240000000, 60, 1, 2, 1,      0 },
+	{ 13000000, 282240000, 43, 1, 1, 1, 0xfd7d }, /* actual: 282239807 */
+	{ 13000000, 368640000, 56, 1, 1, 1, 0x06d8 }, /* actual: 368640137 */
+	{ 13000000, 240000000, 55, 1, 2, 1,      0 }, /* actual: 238.3 MHz */
+	{ 38400000, 282240000, 44, 3, 1, 1, 0xf333 }, /* actual: 282239844 */
+	{ 38400000, 368640000, 57, 3, 1, 1, 0x0333 }, /* actual: 368639844 */
+	{ 38400000, 240000000, 75, 3, 3, 1,      0 },
+	{        0,         0,  0, 0, 0, 0,      0 },
+};
+
+static struct tegra_clk_pll_params pll_a_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 19200000,
+	.vco_min = 500000000,
+	.vco_max = 1000000000,
+	.base_reg = PLLA_BASE,
+	.misc_reg = PLLA_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.iddq_reg = PLLA_BASE,
+	.iddq_bit_idx = PLLA_IDDQ_BIT,
+	.div_nmp = &plla_nmp,
+	.sdm_din_reg = PLLA_MISC1,
+	.sdm_din_mask = PLLA_SDM_DIN_MASK,
+	.sdm_ctrl_reg = PLLA_MISC2,
+	.sdm_ctrl_en_mask = PLLA_SDM_EN_MASK,
+	.ext_misc_reg[0] = PLLA_MISC0,
+	.ext_misc_reg[1] = PLLA_MISC1,
+	.ext_misc_reg[2] = PLLA_MISC2,
+	.freq_table = pll_a_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK | TEGRA_MDIV_NEW,
+	.set_defaults = tegra210_plla_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+	.set_gain = tegra210_clk_pll_set_gain,
+	.adjust_vco = tegra210_clk_adjust_vco_min,
+};
+
+static struct div_nmp plld_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 11,
+	.divn_width = 8,
+	.divp_shift = 20,
+	.divp_width = 3,
+};
+
+static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
+	{ 12000000, 594000000, 99, 1, 1, 0,      0 },
+	{ 13000000, 594000000, 91, 1, 1, 0, 0xfc4f }, /* actual: 594000183 */
+	{ 38400000, 594000000, 30, 1, 1, 0, 0x0e00 },
+	{        0,         0,  0, 0, 0, 0,      0 },
+};
+
+static struct tegra_clk_pll_params pll_d_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 38400000,
+	.vco_min = 750000000,
+	.vco_max = 1500000000,
+	.base_reg = PLLD_BASE,
+	.misc_reg = PLLD_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 1000,
+	.iddq_reg = PLLD_MISC0,
+	.iddq_bit_idx = PLLD_IDDQ_BIT,
+	.round_p_to_pdiv = pll_expo_p_to_pdiv,
+	.pdiv_tohw = pll_expo_pdiv_to_hw,
+	.div_nmp = &plld_nmp,
+	.sdm_din_reg = PLLD_MISC0,
+	.sdm_din_mask = PLLA_SDM_DIN_MASK,
+	.sdm_ctrl_reg = PLLD_MISC0,
+	.sdm_ctrl_en_mask = PLLD_SDM_EN_MASK,
+	.ext_misc_reg[0] = PLLD_MISC0,
+	.ext_misc_reg[1] = PLLD_MISC1,
+	.freq_table = pll_d_freq_table,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.mdiv_default = 1,
+	.set_defaults = tegra210_plld_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+	.set_gain = tegra210_clk_pll_set_gain,
+	.adjust_vco = tegra210_clk_adjust_vco_min,
+};
+
+static struct tegra_clk_pll_freq_table tegra210_pll_d2_freq_table[] = {
+	{ 12000000, 594000000, 99, 1, 1, 0, 0xf000 },
+	{ 13000000, 594000000, 91, 1, 1, 0, 0xfc4f }, /* actual: 594000183 */
+	{ 38400000, 594000000, 30, 1, 1, 0, 0x0e00 },
+	{        0,         0,  0, 0, 0, 0,      0 },
+};
+
+/* s/w policy, always tegra_pll_ref */
+static struct tegra_clk_pll_params pll_d2_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 38400000,
+	.vco_min = 750000000,
+	.vco_max = 1500000000,
+	.base_reg = PLLD2_BASE,
+	.misc_reg = PLLD2_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLD2_BASE,
+	.iddq_bit_idx = PLLSS_IDDQ_BIT,
+	.sdm_din_reg = PLLD2_MISC3,
+	.sdm_din_mask = PLLA_SDM_DIN_MASK,
+	.sdm_ctrl_reg = PLLD2_MISC1,
+	.sdm_ctrl_en_mask = PLLD2_SDM_EN_MASK,
+	.ssc_ctrl_reg = PLLD2_MISC1,
+	.ssc_ctrl_en_mask = PLLD2_SSC_EN_MASK,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllss_nmp,
+	.ext_misc_reg[0] = PLLD2_MISC0,
+	.ext_misc_reg[1] = PLLD2_MISC1,
+	.ext_misc_reg[2] = PLLD2_MISC2,
+	.ext_misc_reg[3] = PLLD2_MISC3,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.mdiv_default = 1,
+	.freq_table = tegra210_pll_d2_freq_table,
+	.set_defaults = tegra210_plld2_set_defaults,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+	.set_gain = tegra210_clk_pll_set_gain,
+	.adjust_vco = tegra210_clk_adjust_vco_min,
+};
+
+static struct tegra_clk_pll_freq_table pll_dp_freq_table[] = {
+	{ 12000000, 270000000, 90, 1, 3, 0, 0xf000 },
+	{ 13000000, 270000000, 83, 1, 3, 0, 0xf000 }, /* actual: 269.8 MHz */
+	{ 38400000, 270000000, 28, 1, 3, 0, 0xf400 },
+	{        0,         0,  0, 0, 0, 0,      0 },
+};
+
+static struct tegra_clk_pll_params pll_dp_params = {
+	.input_min = 12000000,
+	.input_max = 800000000,
+	.cf_min = 12000000,
+	.cf_max = 38400000,
+	.vco_min = 750000000,
+	.vco_max = 1500000000,
+	.base_reg = PLLDP_BASE,
+	.misc_reg = PLLDP_MISC,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 300,
+	.iddq_reg = PLLDP_BASE,
+	.iddq_bit_idx = PLLSS_IDDQ_BIT,
+	.sdm_din_reg = PLLDP_SS_CTRL2,
+	.sdm_din_mask = PLLA_SDM_DIN_MASK,
+	.sdm_ctrl_reg = PLLDP_SS_CFG,
+	.sdm_ctrl_en_mask = PLLDP_SDM_EN_MASK,
+	.ssc_ctrl_reg = PLLDP_SS_CFG,
+	.ssc_ctrl_en_mask = PLLDP_SSC_EN_MASK,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllss_nmp,
+	.ext_misc_reg[0] = PLLDP_MISC,
+	.ext_misc_reg[1] = PLLDP_SS_CFG,
+	.ext_misc_reg[2] = PLLDP_SS_CTRL1,
+	.ext_misc_reg[3] = PLLDP_SS_CTRL2,
+	.max_p = PLL_QLIN_PDIV_MAX,
+	.mdiv_default = 1,
+	.freq_table = pll_dp_freq_table,
+	.set_defaults = tegra210_plldp_set_defaults,
+	.flags = TEGRA_PLL_USE_LOCK,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+	.set_gain = tegra210_clk_pll_set_gain,
+	.adjust_vco = tegra210_clk_adjust_vco_min,
+};
+
+static struct div_nmp pllu_nmp = {
+	.divm_shift = 0,
+	.divm_width = 8,
+	.divn_shift = 8,
+	.divn_width = 8,
+	.divp_shift = 16,
+	.divp_width = 5,
+};
+
+static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
+	{ 12000000, 480000000, 40, 1, 0, 0 },
+	{ 13000000, 480000000, 36, 1, 0, 0 }, /* actual: 468.0 MHz */
+	{ 38400000, 480000000, 25, 2, 0, 0 },
+	{        0,         0,  0, 0, 0, 0 },
+};
+
+static struct tegra_clk_pll_params pll_u_vco_params = {
+	.input_min = 9600000,
+	.input_max = 800000000,
+	.cf_min = 9600000,
+	.cf_max = 19200000,
+	.vco_min = 350000000,
+	.vco_max = 700000000,
+	.base_reg = PLLU_BASE,
+	.misc_reg = PLLU_MISC0,
+	.lock_mask = PLL_BASE_LOCK,
+	.lock_delay = 1000,
+	.iddq_reg = PLLU_MISC0,
+	.iddq_bit_idx = PLLU_IDDQ_BIT,
+	.ext_misc_reg[0] = PLLU_MISC0,
+	.ext_misc_reg[1] = PLLU_MISC1,
+	.round_p_to_pdiv = pll_qlin_p_to_pdiv,
+	.pdiv_tohw = pll_qlin_pdiv_to_hw,
+	.div_nmp = &pllu_nmp,
+	.freq_table = pll_u_freq_table,
+	.flags = TEGRA_PLLU | TEGRA_PLL_USE_LOCK | TEGRA_PLL_VCO_OUT,
+	.set_defaults = tegra210_pllu_set_defaults,
+	.calc_rate = tegra210_pll_fixed_mdiv_cfg,
+};
+
+struct utmi_clk_param {
+	/* Oscillator Frequency in KHz */
+	u32 osc_frequency;
+	/* UTMIP PLL Enable Delay Count  */
+	u8 enable_delay_count;
+	/* UTMIP PLL Stable count */
+	u16 stable_count;
+	/*  UTMIP PLL Active delay count */
+	u8 active_delay_count;
+	/* UTMIP PLL Xtal frequency count */
+	u16 xtal_freq_count;
+};
+
+static const struct utmi_clk_param utmi_parameters[] = {
+	{
+		.osc_frequency = 38400000, .enable_delay_count = 0x0,
+		.stable_count = 0x0, .active_delay_count = 0x6,
+		.xtal_freq_count = 0x80
+	}, {
+		.osc_frequency = 13000000, .enable_delay_count = 0x02,
+		.stable_count = 0x33, .active_delay_count = 0x05,
+		.xtal_freq_count = 0x7f
+	}, {
+		.osc_frequency = 19200000, .enable_delay_count = 0x03,
+		.stable_count = 0x4b, .active_delay_count = 0x06,
+		.xtal_freq_count = 0xbb
+	}, {
+		.osc_frequency = 12000000, .enable_delay_count = 0x02,
+		.stable_count = 0x2f, .active_delay_count = 0x08,
+		.xtal_freq_count = 0x76
+	}, {
+		.osc_frequency = 26000000, .enable_delay_count = 0x04,
+		.stable_count = 0x66, .active_delay_count = 0x09,
+		.xtal_freq_count = 0xfe
+	}, {
+		.osc_frequency = 16800000, .enable_delay_count = 0x03,
+		.stable_count = 0x41, .active_delay_count = 0x0a,
+		.xtal_freq_count = 0xa4
+	},
+};
+
+static struct tegra_clk tegra210_clks[tegra_clk_max] __initdata = {
+	[tegra_clk_ispb] = { .dt_id = TEGRA210_CLK_ISPB, .present = true },
+	[tegra_clk_rtc] = { .dt_id = TEGRA210_CLK_RTC, .present = true },
+	[tegra_clk_timer] = { .dt_id = TEGRA210_CLK_TIMER, .present = true },
+	[tegra_clk_uarta_8] = { .dt_id = TEGRA210_CLK_UARTA, .present = true },
+	[tegra_clk_sdmmc2_9] = { .dt_id = TEGRA210_CLK_SDMMC2, .present = true },
+	[tegra_clk_i2s1] = { .dt_id = TEGRA210_CLK_I2S1, .present = true },
+	[tegra_clk_i2c1] = { .dt_id = TEGRA210_CLK_I2C1, .present = true },
+	[tegra_clk_sdmmc1_9] = { .dt_id = TEGRA210_CLK_SDMMC1, .present = true },
+	[tegra_clk_sdmmc4_9] = { .dt_id = TEGRA210_CLK_SDMMC4, .present = true },
+	[tegra_clk_pwm] = { .dt_id = TEGRA210_CLK_PWM, .present = true },
+	[tegra_clk_i2s2] = { .dt_id = TEGRA210_CLK_I2S2, .present = true },
+	[tegra_clk_usbd] = { .dt_id = TEGRA210_CLK_USBD, .present = true },
+	[tegra_clk_isp_9] = { .dt_id = TEGRA210_CLK_ISP, .present = true },
+	[tegra_clk_disp2_8] = { .dt_id = TEGRA210_CLK_DISP2, .present = true },
+	[tegra_clk_disp1_8] = { .dt_id = TEGRA210_CLK_DISP1, .present = true },
+	[tegra_clk_host1x_9] = { .dt_id = TEGRA210_CLK_HOST1X, .present = true },
+	[tegra_clk_i2s0] = { .dt_id = TEGRA210_CLK_I2S0, .present = true },
+	[tegra_clk_apbdma] = { .dt_id = TEGRA210_CLK_APBDMA, .present = true },
+	[tegra_clk_kfuse] = { .dt_id = TEGRA210_CLK_KFUSE, .present = true },
+	[tegra_clk_sbc1_9] = { .dt_id = TEGRA210_CLK_SBC1, .present = true },
+	[tegra_clk_sbc2_9] = { .dt_id = TEGRA210_CLK_SBC2, .present = true },
+	[tegra_clk_sbc3_9] = { .dt_id = TEGRA210_CLK_SBC3, .present = true },
+	[tegra_clk_i2c5] = { .dt_id = TEGRA210_CLK_I2C5, .present = true },
+	[tegra_clk_csi] = { .dt_id = TEGRA210_CLK_CSI, .present = true },
+	[tegra_clk_i2c2] = { .dt_id = TEGRA210_CLK_I2C2, .present = true },
+	[tegra_clk_uartc_8] = { .dt_id = TEGRA210_CLK_UARTC, .present = true },
+	[tegra_clk_mipi_cal] = { .dt_id = TEGRA210_CLK_MIPI_CAL, .present = true },
+	[tegra_clk_emc] = { .dt_id = TEGRA210_CLK_EMC, .present = true },
+	[tegra_clk_usb2] = { .dt_id = TEGRA210_CLK_USB2, .present = true },
+	[tegra_clk_bsev] = { .dt_id = TEGRA210_CLK_BSEV, .present = true },
+	[tegra_clk_uartd_8] = { .dt_id = TEGRA210_CLK_UARTD, .present = true },
+	[tegra_clk_i2c3] = { .dt_id = TEGRA210_CLK_I2C3, .present = true },
+	[tegra_clk_sbc4_9] = { .dt_id = TEGRA210_CLK_SBC4, .present = true },
+	[tegra_clk_sdmmc3_9] = { .dt_id = TEGRA210_CLK_SDMMC3, .present = true },
+	[tegra_clk_pcie] = { .dt_id = TEGRA210_CLK_PCIE, .present = true },
+	[tegra_clk_owr_8] = { .dt_id = TEGRA210_CLK_OWR, .present = true },
+	[tegra_clk_afi] = { .dt_id = TEGRA210_CLK_AFI, .present = true },
+	[tegra_clk_csite_8] = { .dt_id = TEGRA210_CLK_CSITE, .present = true },
+	[tegra_clk_soc_therm_8] = { .dt_id = TEGRA210_CLK_SOC_THERM, .present = true },
+	[tegra_clk_dtv] = { .dt_id = TEGRA210_CLK_DTV, .present = true },
+	[tegra_clk_i2cslow] = { .dt_id = TEGRA210_CLK_I2CSLOW, .present = true },
+	[tegra_clk_tsec_8] = { .dt_id = TEGRA210_CLK_TSEC, .present = true },
+	[tegra_clk_xusb_host] = { .dt_id = TEGRA210_CLK_XUSB_HOST, .present = true },
+	[tegra_clk_csus] = { .dt_id = TEGRA210_CLK_CSUS, .present = true },
+	[tegra_clk_mselect] = { .dt_id = TEGRA210_CLK_MSELECT, .present = true },
+	[tegra_clk_tsensor] = { .dt_id = TEGRA210_CLK_TSENSOR, .present = true },
+	[tegra_clk_i2s3] = { .dt_id = TEGRA210_CLK_I2S3, .present = true },
+	[tegra_clk_i2s4] = { .dt_id = TEGRA210_CLK_I2S4, .present = true },
+	[tegra_clk_i2c4] = { .dt_id = TEGRA210_CLK_I2C4, .present = true },
+	[tegra_clk_d_audio] = { .dt_id = TEGRA210_CLK_D_AUDIO, .present = true },
+	[tegra_clk_hda2codec_2x_8] = { .dt_id = TEGRA210_CLK_HDA2CODEC_2X, .present = true },
+	[tegra_clk_spdif_2x] = { .dt_id = TEGRA210_CLK_SPDIF_2X, .present = true },
+	[tegra_clk_actmon] = { .dt_id = TEGRA210_CLK_ACTMON, .present = true },
+	[tegra_clk_extern1] = { .dt_id = TEGRA210_CLK_EXTERN1, .present = true },
+	[tegra_clk_extern2] = { .dt_id = TEGRA210_CLK_EXTERN2, .present = true },
+	[tegra_clk_extern3] = { .dt_id = TEGRA210_CLK_EXTERN3, .present = true },
+	[tegra_clk_sata_oob_8] = { .dt_id = TEGRA210_CLK_SATA_OOB, .present = true },
+	[tegra_clk_sata_8] = { .dt_id = TEGRA210_CLK_SATA, .present = true },
+	[tegra_clk_hda_8] = { .dt_id = TEGRA210_CLK_HDA, .present = true },
+	[tegra_clk_hda2hdmi] = { .dt_id = TEGRA210_CLK_HDA2HDMI, .present = true },
+	[tegra_clk_cilab] = { .dt_id = TEGRA210_CLK_CILAB, .present = true },
+	[tegra_clk_cilcd] = { .dt_id = TEGRA210_CLK_CILCD, .present = true },
+	[tegra_clk_cile] = { .dt_id = TEGRA210_CLK_CILE, .present = true },
+	[tegra_clk_dsialp] = { .dt_id = TEGRA210_CLK_DSIALP, .present = true },
+	[tegra_clk_dsiblp] = { .dt_id = TEGRA210_CLK_DSIBLP, .present = true },
+	[tegra_clk_entropy_8] = { .dt_id = TEGRA210_CLK_ENTROPY, .present = true },
+	[tegra_clk_xusb_ss] = { .dt_id = TEGRA210_CLK_XUSB_SS, .present = true },
+	[tegra_clk_i2c6] = { .dt_id = TEGRA210_CLK_I2C6, .present = true },
+	[tegra_clk_vim2_clk] = { .dt_id = TEGRA210_CLK_VIM2_CLK, .present = true },
+	[tegra_clk_clk72Mhz_8] = { .dt_id = TEGRA210_CLK_CLK72MHZ, .present = true },
+	[tegra_clk_vic03_8] = { .dt_id = TEGRA210_CLK_VIC03, .present = true },
+	[tegra_clk_dpaux] = { .dt_id = TEGRA210_CLK_DPAUX, .present = true },
+	[tegra_clk_sor0] = { .dt_id = TEGRA210_CLK_SOR0, .present = true },
+	[tegra_clk_sor0_lvds] = { .dt_id = TEGRA210_CLK_SOR0_LVDS, .present = true },
+	[tegra_clk_gpu] = { .dt_id = TEGRA210_CLK_GPU, .present = true },
+	[tegra_clk_pll_g_ref] = { .dt_id = TEGRA210_CLK_PLL_G_REF, .present = true, },
+	[tegra_clk_uartb_8] = { .dt_id = TEGRA210_CLK_UARTB, .present = true },
+	[tegra_clk_vfir] = { .dt_id = TEGRA210_CLK_VFIR, .present = true },
+	[tegra_clk_spdif_in_8] = { .dt_id = TEGRA210_CLK_SPDIF_IN, .present = true },
+	[tegra_clk_spdif_out] = { .dt_id = TEGRA210_CLK_SPDIF_OUT, .present = true },
+	[tegra_clk_vi_10] = { .dt_id = TEGRA210_CLK_VI, .present = true },
+	[tegra_clk_vi_sensor_8] = { .dt_id = TEGRA210_CLK_VI_SENSOR, .present = true },
+	[tegra_clk_fuse] = { .dt_id = TEGRA210_CLK_FUSE, .present = true },
+	[tegra_clk_fuse_burn] = { .dt_id = TEGRA210_CLK_FUSE_BURN, .present = true },
+	[tegra_clk_clk_32k] = { .dt_id = TEGRA210_CLK_CLK_32K, .present = true },
+	[tegra_clk_clk_m] = { .dt_id = TEGRA210_CLK_CLK_M, .present = true },
+	[tegra_clk_clk_m_div2] = { .dt_id = TEGRA210_CLK_CLK_M_DIV2, .present = true },
+	[tegra_clk_clk_m_div4] = { .dt_id = TEGRA210_CLK_CLK_M_DIV4, .present = true },
+	[tegra_clk_pll_ref] = { .dt_id = TEGRA210_CLK_PLL_REF, .present = true },
+	[tegra_clk_pll_c] = { .dt_id = TEGRA210_CLK_PLL_C, .present = true },
+	[tegra_clk_pll_c_out1] = { .dt_id = TEGRA210_CLK_PLL_C_OUT1, .present = true },
+	[tegra_clk_pll_c2] = { .dt_id = TEGRA210_CLK_PLL_C2, .present = true },
+	[tegra_clk_pll_c3] = { .dt_id = TEGRA210_CLK_PLL_C3, .present = true },
+	[tegra_clk_pll_m] = { .dt_id = TEGRA210_CLK_PLL_M, .present = true },
+	[tegra_clk_pll_m_out1] = { .dt_id = TEGRA210_CLK_PLL_M_OUT1, .present = true },
+	[tegra_clk_pll_p] = { .dt_id = TEGRA210_CLK_PLL_P, .present = true },
+	[tegra_clk_pll_p_out1] = { .dt_id = TEGRA210_CLK_PLL_P_OUT1, .present = true },
+	[tegra_clk_pll_p_out3] = { .dt_id = TEGRA210_CLK_PLL_P_OUT3, .present = true },
+	[tegra_clk_pll_p_out4_cpu] = { .dt_id = TEGRA210_CLK_PLL_P_OUT4, .present = true },
+	[tegra_clk_pll_p_out_hsio] = { .dt_id = TEGRA210_CLK_PLL_P_OUT_HSIO, .present = true },
+	[tegra_clk_pll_p_out_xusb] = { .dt_id = TEGRA210_CLK_PLL_P_OUT_XUSB, .present = true },
+	[tegra_clk_pll_p_out_cpu] = { .dt_id = TEGRA210_CLK_PLL_P_OUT_CPU, .present = true },
+	[tegra_clk_pll_p_out_adsp] = { .dt_id = TEGRA210_CLK_PLL_P_OUT_ADSP, .present = true },
+	[tegra_clk_pll_a] = { .dt_id = TEGRA210_CLK_PLL_A, .present = true },
+	[tegra_clk_pll_a_out0] = { .dt_id = TEGRA210_CLK_PLL_A_OUT0, .present = true },
+	[tegra_clk_pll_d] = { .dt_id = TEGRA210_CLK_PLL_D, .present = true },
+	[tegra_clk_pll_d_out0] = { .dt_id = TEGRA210_CLK_PLL_D_OUT0, .present = true },
+	[tegra_clk_pll_d2] = { .dt_id = TEGRA210_CLK_PLL_D2, .present = true },
+	[tegra_clk_pll_d2_out0] = { .dt_id = TEGRA210_CLK_PLL_D2_OUT0, .present = true },
+	[tegra_clk_pll_u] = { .dt_id = TEGRA210_CLK_PLL_U, .present = true },
+	[tegra_clk_pll_u_out] = { .dt_id = TEGRA210_CLK_PLL_U_OUT, .present = true },
+	[tegra_clk_pll_u_out1] = { .dt_id = TEGRA210_CLK_PLL_U_OUT1, .present = true },
+	[tegra_clk_pll_u_out2] = { .dt_id = TEGRA210_CLK_PLL_U_OUT2, .present = true },
+	[tegra_clk_pll_u_480m] = { .dt_id = TEGRA210_CLK_PLL_U_480M, .present = true },
+	[tegra_clk_pll_u_60m] = { .dt_id = TEGRA210_CLK_PLL_U_60M, .present = true },
+	[tegra_clk_pll_u_48m] = { .dt_id = TEGRA210_CLK_PLL_U_48M, .present = true },
+	[tegra_clk_pll_x] = { .dt_id = TEGRA210_CLK_PLL_X, .present = true },
+	[tegra_clk_pll_x_out0] = { .dt_id = TEGRA210_CLK_PLL_X_OUT0, .present = true },
+	[tegra_clk_pll_re_vco] = { .dt_id = TEGRA210_CLK_PLL_RE_VCO, .present = true },
+	[tegra_clk_pll_re_out] = { .dt_id = TEGRA210_CLK_PLL_RE_OUT, .present = true },
+	[tegra_clk_spdif_in_sync] = { .dt_id = TEGRA210_CLK_SPDIF_IN_SYNC, .present = true },
+	[tegra_clk_i2s0_sync] = { .dt_id = TEGRA210_CLK_I2S0_SYNC, .present = true },
+	[tegra_clk_i2s1_sync] = { .dt_id = TEGRA210_CLK_I2S1_SYNC, .present = true },
+	[tegra_clk_i2s2_sync] = { .dt_id = TEGRA210_CLK_I2S2_SYNC, .present = true },
+	[tegra_clk_i2s3_sync] = { .dt_id = TEGRA210_CLK_I2S3_SYNC, .present = true },
+	[tegra_clk_i2s4_sync] = { .dt_id = TEGRA210_CLK_I2S4_SYNC, .present = true },
+	[tegra_clk_vimclk_sync] = { .dt_id = TEGRA210_CLK_VIMCLK_SYNC, .present = true },
+	[tegra_clk_audio0] = { .dt_id = TEGRA210_CLK_AUDIO0, .present = true },
+	[tegra_clk_audio1] = { .dt_id = TEGRA210_CLK_AUDIO1, .present = true },
+	[tegra_clk_audio2] = { .dt_id = TEGRA210_CLK_AUDIO2, .present = true },
+	[tegra_clk_audio3] = { .dt_id = TEGRA210_CLK_AUDIO3, .present = true },
+	[tegra_clk_audio4] = { .dt_id = TEGRA210_CLK_AUDIO4, .present = true },
+	[tegra_clk_spdif] = { .dt_id = TEGRA210_CLK_SPDIF, .present = true },
+	[tegra_clk_clk_out_1] = { .dt_id = TEGRA210_CLK_CLK_OUT_1, .present = true },
+	[tegra_clk_clk_out_2] = { .dt_id = TEGRA210_CLK_CLK_OUT_2, .present = true },
+	[tegra_clk_clk_out_3] = { .dt_id = TEGRA210_CLK_CLK_OUT_3, .present = true },
+	[tegra_clk_blink] = { .dt_id = TEGRA210_CLK_BLINK, .present = true },
+	[tegra_clk_xusb_gate] = { .dt_id = TEGRA210_CLK_XUSB_GATE, .present = true },
+	[tegra_clk_xusb_host_src_8] = { .dt_id = TEGRA210_CLK_XUSB_HOST_SRC, .present = true },
+	[tegra_clk_xusb_falcon_src_8] = { .dt_id = TEGRA210_CLK_XUSB_FALCON_SRC, .present = true },
+	[tegra_clk_xusb_fs_src] = { .dt_id = TEGRA210_CLK_XUSB_FS_SRC, .present = true },
+	[tegra_clk_xusb_ss_src_8] = { .dt_id = TEGRA210_CLK_XUSB_SS_SRC, .present = true },
+	[tegra_clk_xusb_ss_div2] = { .dt_id = TEGRA210_CLK_XUSB_SS_DIV2, .present = true },
+	[tegra_clk_xusb_dev_src_8] = { .dt_id = TEGRA210_CLK_XUSB_DEV_SRC, .present = true },
+	[tegra_clk_xusb_dev] = { .dt_id = TEGRA210_CLK_XUSB_DEV, .present = true },
+	[tegra_clk_xusb_hs_src_4] = { .dt_id = TEGRA210_CLK_XUSB_HS_SRC, .present = true },
+	[tegra_clk_xusb_ssp_src] = { .dt_id = TEGRA210_CLK_XUSB_SSP_SRC, .present = true },
+	[tegra_clk_usb2_hsic_trk] = { .dt_id = TEGRA210_CLK_USB2_HSIC_TRK, .present = true },
+	[tegra_clk_hsic_trk] = { .dt_id = TEGRA210_CLK_HSIC_TRK, .present = true },
+	[tegra_clk_usb2_trk] = { .dt_id = TEGRA210_CLK_USB2_TRK, .present = true },
+	[tegra_clk_sclk] = { .dt_id = TEGRA210_CLK_SCLK, .present = true },
+	[tegra_clk_sclk_mux] = { .dt_id = TEGRA210_CLK_SCLK_MUX, .present = true },
+	[tegra_clk_hclk] = { .dt_id = TEGRA210_CLK_HCLK, .present = true },
+	[tegra_clk_pclk] = { .dt_id = TEGRA210_CLK_PCLK, .present = true },
+	[tegra_clk_cclk_g] = { .dt_id = TEGRA210_CLK_CCLK_G, .present = true },
+	[tegra_clk_cclk_lp] = { .dt_id = TEGRA210_CLK_CCLK_LP, .present = true },
+	[tegra_clk_dfll_ref] = { .dt_id = TEGRA210_CLK_DFLL_REF, .present = true },
+	[tegra_clk_dfll_soc] = { .dt_id = TEGRA210_CLK_DFLL_SOC, .present = true },
+	[tegra_clk_vi_sensor2_8] = { .dt_id = TEGRA210_CLK_VI_SENSOR2, .present = true },
+	[tegra_clk_pll_p_out5] = { .dt_id = TEGRA210_CLK_PLL_P_OUT5, .present = true },
+	[tegra_clk_pll_c4] = { .dt_id = TEGRA210_CLK_PLL_C4, .present = true },
+	[tegra_clk_pll_dp] = { .dt_id = TEGRA210_CLK_PLL_DP, .present = true },
+	[tegra_clk_audio0_mux] = { .dt_id = TEGRA210_CLK_AUDIO0_MUX, .present = true },
+	[tegra_clk_audio1_mux] = { .dt_id = TEGRA210_CLK_AUDIO1_MUX, .present = true },
+	[tegra_clk_audio2_mux] = { .dt_id = TEGRA210_CLK_AUDIO2_MUX, .present = true },
+	[tegra_clk_audio3_mux] = { .dt_id = TEGRA210_CLK_AUDIO3_MUX, .present = true },
+	[tegra_clk_audio4_mux] = { .dt_id = TEGRA210_CLK_AUDIO4_MUX, .present = true },
+	[tegra_clk_spdif_mux] = { .dt_id = TEGRA210_CLK_SPDIF_MUX, .present = true },
+	[tegra_clk_clk_out_1_mux] = { .dt_id = TEGRA210_CLK_CLK_OUT_1_MUX, .present = true },
+	[tegra_clk_clk_out_2_mux] = { .dt_id = TEGRA210_CLK_CLK_OUT_2_MUX, .present = true },
+	[tegra_clk_clk_out_3_mux] = { .dt_id = TEGRA210_CLK_CLK_OUT_3_MUX, .present = true },
+	[tegra_clk_maud] = { .dt_id = TEGRA210_CLK_MAUD, .present = true },
+	[tegra_clk_mipibif] = { .dt_id = TEGRA210_CLK_MIPIBIF, .present = true },
+	[tegra_clk_qspi] = { .dt_id = TEGRA210_CLK_QSPI, .present = true },
+	[tegra_clk_sdmmc_legacy] = { .dt_id = TEGRA210_CLK_SDMMC_LEGACY, .present = true },
+	[tegra_clk_tsecb] = { .dt_id = TEGRA210_CLK_TSECB, .present = true },
+	[tegra_clk_uartape] = { .dt_id = TEGRA210_CLK_UARTAPE, .present = true },
+	[tegra_clk_vi_i2c] = { .dt_id = TEGRA210_CLK_VI_I2C, .present = true },
+	[tegra_clk_ape] = { .dt_id = TEGRA210_CLK_APE, .present = true },
+	[tegra_clk_dbgapb] = { .dt_id = TEGRA210_CLK_DBGAPB, .present = true },
+	[tegra_clk_nvdec] = { .dt_id = TEGRA210_CLK_NVDEC, .present = true },
+	[tegra_clk_nvenc] = { .dt_id = TEGRA210_CLK_NVENC, .present = true },
+	[tegra_clk_nvjpg] = { .dt_id = TEGRA210_CLK_NVJPG, .present = true },
+	[tegra_clk_pll_c4_out0] = { .dt_id = TEGRA210_CLK_PLL_C4_OUT0, .present = true },
+	[tegra_clk_pll_c4_out1] = { .dt_id = TEGRA210_CLK_PLL_C4_OUT1, .present = true },
+	[tegra_clk_pll_c4_out2] = { .dt_id = TEGRA210_CLK_PLL_C4_OUT2, .present = true },
+	[tegra_clk_pll_c4_out3] = { .dt_id = TEGRA210_CLK_PLL_C4_OUT3, .present = true },
+	[tegra_clk_apb2ape] = { .dt_id = TEGRA210_CLK_APB2APE, .present = true },
+};
+
+static struct tegra_devclk devclks[] __initdata = {
+	{ .con_id = "clk_m", .dt_id = TEGRA210_CLK_CLK_M },
+	{ .con_id = "pll_ref", .dt_id = TEGRA210_CLK_PLL_REF },
+	{ .con_id = "clk_32k", .dt_id = TEGRA210_CLK_CLK_32K },
+	{ .con_id = "clk_m_div2", .dt_id = TEGRA210_CLK_CLK_M_DIV2 },
+	{ .con_id = "clk_m_div4", .dt_id = TEGRA210_CLK_CLK_M_DIV4 },
+	{ .con_id = "pll_c", .dt_id = TEGRA210_CLK_PLL_C },
+	{ .con_id = "pll_c_out1", .dt_id = TEGRA210_CLK_PLL_C_OUT1 },
+	{ .con_id = "pll_c2", .dt_id = TEGRA210_CLK_PLL_C2 },
+	{ .con_id = "pll_c3", .dt_id = TEGRA210_CLK_PLL_C3 },
+	{ .con_id = "pll_p", .dt_id = TEGRA210_CLK_PLL_P },
+	{ .con_id = "pll_p_out1", .dt_id = TEGRA210_CLK_PLL_P_OUT1 },
+	{ .con_id = "pll_p_out2", .dt_id = TEGRA210_CLK_PLL_P_OUT2 },
+	{ .con_id = "pll_p_out3", .dt_id = TEGRA210_CLK_PLL_P_OUT3 },
+	{ .con_id = "pll_p_out4", .dt_id = TEGRA210_CLK_PLL_P_OUT4 },
+	{ .con_id = "pll_m", .dt_id = TEGRA210_CLK_PLL_M },
+	{ .con_id = "pll_m_out1", .dt_id = TEGRA210_CLK_PLL_M_OUT1 },
+	{ .con_id = "pll_x", .dt_id = TEGRA210_CLK_PLL_X },
+	{ .con_id = "pll_x_out0", .dt_id = TEGRA210_CLK_PLL_X_OUT0 },
+	{ .con_id = "pll_u", .dt_id = TEGRA210_CLK_PLL_U },
+	{ .con_id = "pll_u_out", .dt_id = TEGRA210_CLK_PLL_U_OUT },
+	{ .con_id = "pll_u_out1", .dt_id = TEGRA210_CLK_PLL_U_OUT1 },
+	{ .con_id = "pll_u_out2", .dt_id = TEGRA210_CLK_PLL_U_OUT2 },
+	{ .con_id = "pll_u_480M", .dt_id = TEGRA210_CLK_PLL_U_480M },
+	{ .con_id = "pll_u_60M", .dt_id = TEGRA210_CLK_PLL_U_60M },
+	{ .con_id = "pll_u_48M", .dt_id = TEGRA210_CLK_PLL_U_48M },
+	{ .con_id = "pll_d", .dt_id = TEGRA210_CLK_PLL_D },
+	{ .con_id = "pll_d_out0", .dt_id = TEGRA210_CLK_PLL_D_OUT0 },
+	{ .con_id = "pll_d2", .dt_id = TEGRA210_CLK_PLL_D2 },
+	{ .con_id = "pll_d2_out0", .dt_id = TEGRA210_CLK_PLL_D2_OUT0 },
+	{ .con_id = "pll_a", .dt_id = TEGRA210_CLK_PLL_A },
+	{ .con_id = "pll_a_out0", .dt_id = TEGRA210_CLK_PLL_A_OUT0 },
+	{ .con_id = "pll_re_vco", .dt_id = TEGRA210_CLK_PLL_RE_VCO },
+	{ .con_id = "pll_re_out", .dt_id = TEGRA210_CLK_PLL_RE_OUT },
+	{ .con_id = "spdif_in_sync", .dt_id = TEGRA210_CLK_SPDIF_IN_SYNC },
+	{ .con_id = "i2s0_sync", .dt_id = TEGRA210_CLK_I2S0_SYNC },
+	{ .con_id = "i2s1_sync", .dt_id = TEGRA210_CLK_I2S1_SYNC },
+	{ .con_id = "i2s2_sync", .dt_id = TEGRA210_CLK_I2S2_SYNC },
+	{ .con_id = "i2s3_sync", .dt_id = TEGRA210_CLK_I2S3_SYNC },
+	{ .con_id = "i2s4_sync", .dt_id = TEGRA210_CLK_I2S4_SYNC },
+	{ .con_id = "vimclk_sync", .dt_id = TEGRA210_CLK_VIMCLK_SYNC },
+	{ .con_id = "audio0", .dt_id = TEGRA210_CLK_AUDIO0 },
+	{ .con_id = "audio1", .dt_id = TEGRA210_CLK_AUDIO1 },
+	{ .con_id = "audio2", .dt_id = TEGRA210_CLK_AUDIO2 },
+	{ .con_id = "audio3", .dt_id = TEGRA210_CLK_AUDIO3 },
+	{ .con_id = "audio4", .dt_id = TEGRA210_CLK_AUDIO4 },
+	{ .con_id = "spdif", .dt_id = TEGRA210_CLK_SPDIF },
+	{ .con_id = "spdif_2x", .dt_id = TEGRA210_CLK_SPDIF_2X },
+	{ .con_id = "extern1", .dev_id = "clk_out_1", .dt_id = TEGRA210_CLK_EXTERN1 },
+	{ .con_id = "extern2", .dev_id = "clk_out_2", .dt_id = TEGRA210_CLK_EXTERN2 },
+	{ .con_id = "extern3", .dev_id = "clk_out_3", .dt_id = TEGRA210_CLK_EXTERN3 },
+	{ .con_id = "blink", .dt_id = TEGRA210_CLK_BLINK },
+	{ .con_id = "cclk_g", .dt_id = TEGRA210_CLK_CCLK_G },
+	{ .con_id = "cclk_lp", .dt_id = TEGRA210_CLK_CCLK_LP },
+	{ .con_id = "sclk", .dt_id = TEGRA210_CLK_SCLK },
+	{ .con_id = "hclk", .dt_id = TEGRA210_CLK_HCLK },
+	{ .con_id = "pclk", .dt_id = TEGRA210_CLK_PCLK },
+	{ .con_id = "fuse", .dt_id = TEGRA210_CLK_FUSE },
+	{ .dev_id = "rtc-tegra", .dt_id = TEGRA210_CLK_RTC },
+	{ .dev_id = "timer", .dt_id = TEGRA210_CLK_TIMER },
+	{ .con_id = "pll_c4_out0", .dt_id = TEGRA210_CLK_PLL_C4_OUT0 },
+	{ .con_id = "pll_c4_out1", .dt_id = TEGRA210_CLK_PLL_C4_OUT1 },
+	{ .con_id = "pll_c4_out2", .dt_id = TEGRA210_CLK_PLL_C4_OUT2 },
+	{ .con_id = "pll_c4_out3", .dt_id = TEGRA210_CLK_PLL_C4_OUT3 },
+	{ .con_id = "dpaux", .dt_id = TEGRA210_CLK_DPAUX },
+	{ .con_id = "sor0", .dt_id = TEGRA210_CLK_SOR0 },
+};
+
+static struct tegra_audio_clk_info tegra210_audio_plls[] = {
+	{ "pll_a", &pll_a_params, tegra_clk_pll_a, "pll_ref" },
+	{ "pll_a1", &pll_a1_params, tegra_clk_pll_a1, "pll_ref" },
+};
+
+static struct clk **clks;
+
+static void tegra210_utmi_param_configure(void __iomem *clk_base)
+{
+	u32 reg;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(utmi_parameters); i++) {
+		if (osc_freq == utmi_parameters[i].osc_frequency)
+			break;
+	}
+
+	if (i >= ARRAY_SIZE(utmi_parameters)) {
+		pr_err("%s: Unexpected oscillator freq %lu\n", __func__,
+		       osc_freq);
+		return;
+	}
+
+	reg = readl_relaxed(clk_base + PLLU_HW_PWRDN_CFG0);
+	reg |= PLLU_HW_PWRDN_CFG0_IDDQ_PD_INCLUDE |
+	       PLLU_HW_PWRDN_CFG0_USE_SWITCH_DETECT |
+	       PLLU_HW_PWRDN_CFG0_USE_LOCKDET;
+	reg &= ~(PLLU_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL |
+		  PLLU_HW_PWRDN_CFG0_CLK_SWITCH_SWCTL);
+	writel_relaxed(reg, clk_base + PLLU_HW_PWRDN_CFG0);
+
+	reg = readl_relaxed(clk_base + PLLU_HW_PWRDN_CFG0);
+	reg |= PLLU_HW_PWRDN_CFG0_SEQ_ENABLE;
+	writel_relaxed(reg, clk_base + PLLU_HW_PWRDN_CFG0);
+	udelay(1);
+
+	reg = readl_relaxed(clk_base + PLLU_BASE);
+	reg &= ~PLLU_BASE_CLKENABLE_USB;
+	writel_relaxed(reg, clk_base + PLLU_BASE);
+
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_IDDQ_OVERRIDE;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+
+	udelay(10);
+
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG2);
+
+	/* Program UTMIP PLL stable and active counts */
+	/* [FIXME] arclk_rst.h says WRONG! This should be 1ms -> 0x50 Check! */
+	reg &= ~UTMIP_PLL_CFG2_STABLE_COUNT(~0);
+	reg |= UTMIP_PLL_CFG2_STABLE_COUNT(utmi_parameters[i].stable_count);
+
+	reg &= ~UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(~0);
+
+	reg |= UTMIP_PLL_CFG2_ACTIVE_DLY_COUNT(utmi_parameters[i].
+					    active_delay_count);
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG2);
+
+	/* Program UTMIP PLL delay and oscillator frequency counts */
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG1);
+	reg &= ~UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(~0);
+
+	reg |= UTMIP_PLL_CFG1_ENABLE_DLY_COUNT(utmi_parameters[i].
+					    enable_delay_count);
+
+	reg &= ~UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(~0);
+	reg |= UTMIP_PLL_CFG1_XTAL_FREQ_COUNT(utmi_parameters[i].
+					   xtal_freq_count);
+
+	reg |= UTMIP_PLL_CFG1_FORCE_PLLU_POWERDOWN;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
+
+	/* Remove power downs from UTMIP PLL control bits */
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG1);
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	reg |= UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
+	udelay(1);
+
+	/* Enable samplers for SNPS, XUSB_HOST, XUSB_DEV */
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG2);
+	reg |= UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERUP;
+	reg |= UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERUP;
+	reg |= UTMIP_PLL_CFG2_FORCE_PD_SAMP_D_POWERUP;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_A_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_B_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG2_FORCE_PD_SAMP_D_POWERDOWN;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG2);
+
+	/* Setup HW control of UTMIPLL */
+	reg = readl_relaxed(clk_base + UTMIP_PLL_CFG1);
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERDOWN;
+	reg &= ~UTMIP_PLL_CFG1_FORCE_PLL_ENABLE_POWERUP;
+	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
+
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_USE_LOCKDET;
+	reg &= ~UTMIPLL_HW_PWRDN_CFG0_CLK_ENABLE_SWCTL;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+
+	udelay(1);
+
+	reg = readl_relaxed(clk_base + XUSB_PLL_CFG0);
+	reg &= ~XUSB_PLL_CFG0_UTMIPLL_LOCK_DLY;
+	writel_relaxed(reg, clk_base + XUSB_PLL_CFG0);
+
+	udelay(1);
+
+	/* Enable HW control UTMIPLL */
+	reg = readl_relaxed(clk_base + UTMIPLL_HW_PWRDN_CFG0);
+	reg |= UTMIPLL_HW_PWRDN_CFG0_SEQ_ENABLE;
+	writel_relaxed(reg, clk_base + UTMIPLL_HW_PWRDN_CFG0);
+}
+
+static __init void tegra210_periph_clk_init(void __iomem *clk_base,
+					    void __iomem *pmc_base)
+{
+	struct clk *clk;
+
+	/* xusb_ss_div2 */
+	clk = clk_register_fixed_factor(NULL, "xusb_ss_div2", "xusb_ss_src", 0,
+					1, 2);
+	clks[TEGRA210_CLK_XUSB_SS_DIV2] = clk;
+
+	/* pll_d_dsi_out */
+	clk = clk_register_gate(NULL, "pll_d_dsi_out", "pll_d_out0", 0,
+				clk_base + PLLD_MISC0, 21, 0, &pll_d_lock);
+	clks[TEGRA210_CLK_PLL_D_DSI_OUT] = clk;
+
+	/* dsia */
+	clk = tegra_clk_register_periph_gate("dsia", "pll_d_dsi_out", 0,
+					     clk_base, 0, 48,
+					     periph_clk_enb_refcnt);
+	clks[TEGRA210_CLK_DSIA] = clk;
+
+	/* dsib */
+	clk = tegra_clk_register_periph_gate("dsib", "pll_d_dsi_out", 0,
+					     clk_base, 0, 82,
+					     periph_clk_enb_refcnt);
+	clks[TEGRA210_CLK_DSIB] = clk;
+
+	/* emc mux */
+	clk = clk_register_mux(NULL, "emc_mux", mux_pllmcp_clkm,
+			       ARRAY_SIZE(mux_pllmcp_clkm), 0,
+			       clk_base + CLK_SOURCE_EMC,
+			       29, 3, 0, &emc_lock);
+
+	clk = tegra_clk_register_mc("mc", "emc_mux", clk_base + CLK_SOURCE_EMC,
+				    &emc_lock);
+	clks[TEGRA210_CLK_MC] = clk;
+
+	/* cml0 */
+	clk = clk_register_gate(NULL, "cml0", "pll_e", 0, clk_base + PLLE_AUX,
+				0, 0, &pll_e_lock);
+	clk_register_clkdev(clk, "cml0", NULL);
+	clks[TEGRA210_CLK_CML0] = clk;
+
+	/* cml1 */
+	clk = clk_register_gate(NULL, "cml1", "pll_e", 0, clk_base + PLLE_AUX,
+				1, 0, &pll_e_lock);
+	clk_register_clkdev(clk, "cml1", NULL);
+	clks[TEGRA210_CLK_CML1] = clk;
+
+	tegra_periph_clk_init(clk_base, pmc_base, tegra210_clks, &pll_p_params);
+}
+
+static void __init tegra210_pll_init(void __iomem *clk_base,
+				     void __iomem *pmc)
+{
+	u32 val;
+	struct clk *clk;
+
+	/* PLLC */
+	clk = tegra_clk_register_pllxc_tegra210("pll_c", "pll_ref", clk_base,
+			pmc, 0, &pll_c_params, NULL);
+	if (!WARN_ON(IS_ERR(clk)))
+		clk_register_clkdev(clk, "pll_c", NULL);
+	clks[TEGRA210_CLK_PLL_C] = clk;
+
+	/* PLLC_OUT1 */
+	clk = tegra_clk_register_divider("pll_c_out1_div", "pll_c",
+			clk_base + PLLC_OUT, 0, TEGRA_DIVIDER_ROUND_UP,
+			8, 8, 1, NULL);
+	clk = tegra_clk_register_pll_out("pll_c_out1", "pll_c_out1_div",
+				clk_base + PLLC_OUT, 1, 0,
+				CLK_SET_RATE_PARENT, 0, NULL);
+	clk_register_clkdev(clk, "pll_c_out1", NULL);
+	clks[TEGRA210_CLK_PLL_C_OUT1] = clk;
+
+	/* PLLC_UD */
+	clk = clk_register_fixed_factor(NULL, "pll_c_ud", "pll_c",
+					CLK_SET_RATE_PARENT, 1, 1);
+	clk_register_clkdev(clk, "pll_c_ud", NULL);
+	clks[TEGRA210_CLK_PLL_C_UD] = clk;
+
+	/* PLLC2 */
+	clk = tegra_clk_register_pllc_tegra210("pll_c2", "pll_ref", clk_base,
+			     pmc, 0, &pll_c2_params, NULL);
+	clk_register_clkdev(clk, "pll_c2", NULL);
+	clks[TEGRA210_CLK_PLL_C2] = clk;
+
+	/* PLLC3 */
+	clk = tegra_clk_register_pllc_tegra210("pll_c3", "pll_ref", clk_base,
+			     pmc, 0, &pll_c3_params, NULL);
+	clk_register_clkdev(clk, "pll_c3", NULL);
+	clks[TEGRA210_CLK_PLL_C3] = clk;
+
+	/* PLLM */
+	clk = tegra_clk_register_pllm("pll_m", "osc", clk_base, pmc,
+			     CLK_SET_RATE_GATE, &pll_m_params, NULL);
+	clk_register_clkdev(clk, "pll_m", NULL);
+	clks[TEGRA210_CLK_PLL_M] = clk;
+
+	/* PLLMB */
+	clk = tegra_clk_register_pllmb("pll_mb", "osc", clk_base, pmc,
+			     CLK_SET_RATE_GATE, &pll_mb_params, NULL);
+	clk_register_clkdev(clk, "pll_mb", NULL);
+	clks[TEGRA210_CLK_PLL_MB] = clk;
+
+	clk_register_clkdev(clk, "pll_m_out1", NULL);
+	clks[TEGRA210_CLK_PLL_M_OUT1] = clk;
+
+	/* PLLM_UD */
+	clk = clk_register_fixed_factor(NULL, "pll_m_ud", "pll_m",
+					CLK_SET_RATE_PARENT, 1, 1);
+	clk_register_clkdev(clk, "pll_m_ud", NULL);
+	clks[TEGRA210_CLK_PLL_M_UD] = clk;
+
+	/* PLLU_VCO */
+	val = readl(clk_base + pll_u_vco_params.base_reg);
+	val &= ~PLLU_BASE_OVERRIDE; /* disable PLLU_OVERRIDE */
+	writel(val, clk_base + pll_u_vco_params.base_reg);
+
+	clk = tegra_clk_register_pllre("pll_u_vco", "pll_ref", clk_base, pmc,
+			    0, &pll_u_vco_params, &pll_u_lock, pll_ref_freq);
+	clk_register_clkdev(clk, "pll_u_vco", NULL);
+	clks[TEGRA210_CLK_PLL_U] = clk;
+
+	/* PLLU_OUT */
+	clk = clk_register_divider_table(NULL, "pll_u_out", "pll_u_vco", 0,
+					 clk_base + PLLU_BASE, 16, 4, 0,
+					 pll_vco_post_div_table, NULL);
+	clk_register_clkdev(clk, "pll_u_out", NULL);
+	clks[TEGRA210_CLK_PLL_U_OUT] = clk;
+
+	/* PLLU_OUT1 */
+	clk = tegra_clk_register_divider("pll_u_out1_div", "pll_u_out",
+				clk_base + PLLU_OUTA, 0,
+				TEGRA_DIVIDER_ROUND_UP,
+				8, 8, 1, &pll_u_lock);
+	clk = tegra_clk_register_pll_out("pll_u_out1", "pll_u_out1_div",
+				clk_base + PLLU_OUTA, 1, 0,
+				CLK_SET_RATE_PARENT, 0, &pll_u_lock);
+	clk_register_clkdev(clk, "pll_u_out1", NULL);
+	clks[TEGRA210_CLK_PLL_U_OUT1] = clk;
+
+	/* PLLU_OUT2 */
+	clk = tegra_clk_register_divider("pll_u_out2_div", "pll_u_out",
+				clk_base + PLLU_OUTA, 0,
+				TEGRA_DIVIDER_ROUND_UP,
+				24, 8, 1, &pll_u_lock);
+	clk = tegra_clk_register_pll_out("pll_u_out2", "pll_u_out2_div",
+				clk_base + PLLU_OUTA, 17, 16,
+				CLK_SET_RATE_PARENT, 0, &pll_u_lock);
+	clk_register_clkdev(clk, "pll_u_out2", NULL);
+	clks[TEGRA210_CLK_PLL_U_OUT2] = clk;
+
+	tegra210_utmi_param_configure(clk_base);
+
+	/* PLLU_480M */
+	clk = clk_register_gate(NULL, "pll_u_480M", "pll_u_vco",
+				CLK_SET_RATE_PARENT, clk_base + PLLU_BASE,
+				22, 0, &pll_u_lock);
+	clk_register_clkdev(clk, "pll_u_480M", NULL);
+	clks[TEGRA210_CLK_PLL_U_480M] = clk;
+
+	/* PLLU_60M */
+	clk = clk_register_gate(NULL, "pll_u_60M", "pll_u_out2",
+				CLK_SET_RATE_PARENT, clk_base + PLLU_BASE,
+				23, 0, NULL);
+	clk_register_clkdev(clk, "pll_u_60M", NULL);
+	clks[TEGRA210_CLK_PLL_U_60M] = clk;
+
+	/* PLLU_48M */
+	clk = clk_register_gate(NULL, "pll_u_48M", "pll_u_out1",
+				CLK_SET_RATE_PARENT, clk_base + PLLU_BASE,
+				25, 0, NULL);
+	clk_register_clkdev(clk, "pll_u_48M", NULL);
+	clks[TEGRA210_CLK_PLL_U_48M] = clk;
+
+	/* PLLD */
+	clk = tegra_clk_register_pll("pll_d", "pll_ref", clk_base, pmc, 0,
+			    &pll_d_params, &pll_d_lock);
+	clk_register_clkdev(clk, "pll_d", NULL);
+	clks[TEGRA210_CLK_PLL_D] = clk;
+
+	/* PLLD_OUT0 */
+	clk = clk_register_fixed_factor(NULL, "pll_d_out0", "pll_d",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll_d_out0", NULL);
+	clks[TEGRA210_CLK_PLL_D_OUT0] = clk;
+
+	/* PLLRE */
+	clk = tegra_clk_register_pllre("pll_re_vco", "pll_ref", clk_base, pmc,
+			     0, &pll_re_vco_params, &pll_re_lock, pll_ref_freq);
+	clk_register_clkdev(clk, "pll_re_vco", NULL);
+	clks[TEGRA210_CLK_PLL_RE_VCO] = clk;
+
+	clk = clk_register_divider_table(NULL, "pll_re_out", "pll_re_vco", 0,
+					 clk_base + PLLRE_BASE, 16, 5, 0,
+					 pll_vco_post_div_table, &pll_re_lock);
+	clk_register_clkdev(clk, "pll_re_out", NULL);
+	clks[TEGRA210_CLK_PLL_RE_OUT] = clk;
+
+	/* PLLE */
+	clk = tegra_clk_register_plle_tegra210("pll_e", "pll_ref",
+				      clk_base, 0, &pll_e_params, NULL);
+	clk_register_clkdev(clk, "pll_e", NULL);
+	clks[TEGRA210_CLK_PLL_E] = clk;
+
+	/* PLLC4 */
+	clk = tegra_clk_register_pllre("pll_c4_vco", "pll_ref", clk_base, pmc,
+			     0, &pll_c4_vco_params, NULL, pll_ref_freq);
+	clk_register_clkdev(clk, "pll_c4_vco", NULL);
+	clks[TEGRA210_CLK_PLL_C4] = clk;
+
+	/* PLLC4_OUT0 */
+	clk = clk_register_divider_table(NULL, "pll_c4_out0", "pll_c4_vco", 0,
+					 clk_base + PLLC4_BASE, 19, 4, 0,
+					 pll_vco_post_div_table, NULL);
+	clk_register_clkdev(clk, "pll_c4_out0", NULL);
+	clks[TEGRA210_CLK_PLL_C4_OUT0] = clk;
+
+	/* PLLC4_OUT1 */
+	clk = clk_register_fixed_factor(NULL, "pll_c4_out1", "pll_c4_vco",
+					CLK_SET_RATE_PARENT, 1, 3);
+	clk_register_clkdev(clk, "pll_c4_out1", NULL);
+	clks[TEGRA210_CLK_PLL_C4_OUT1] = clk;
+
+	/* PLLC4_OUT2 */
+	clk = clk_register_fixed_factor(NULL, "pll_c4_out2", "pll_c4_vco",
+					CLK_SET_RATE_PARENT, 1, 5);
+	clk_register_clkdev(clk, "pll_c4_out2", NULL);
+	clks[TEGRA210_CLK_PLL_C4_OUT2] = clk;
+
+	/* PLLC4_OUT3 */
+	clk = tegra_clk_register_divider("pll_c4_out3_div", "pll_c4_out0",
+			clk_base + PLLC4_OUT, 0, TEGRA_DIVIDER_ROUND_UP,
+			8, 8, 1, NULL);
+	clk = tegra_clk_register_pll_out("pll_c4_out3", "pll_c4_out3_div",
+				clk_base + PLLC4_OUT, 1, 0,
+				CLK_SET_RATE_PARENT, 0, NULL);
+	clk_register_clkdev(clk, "pll_c4_out3", NULL);
+	clks[TEGRA210_CLK_PLL_C4_OUT3] = clk;
+
+	/* PLLDP */
+	clk = tegra_clk_register_pllss_tegra210("pll_dp", "pll_ref", clk_base,
+					0, &pll_dp_params, NULL);
+	clk_register_clkdev(clk, "pll_dp", NULL);
+	clks[TEGRA210_CLK_PLL_DP] = clk;
+
+	/* PLLD2 */
+	clk = tegra_clk_register_pllss_tegra210("pll_d2", "pll_ref", clk_base,
+					0, &pll_d2_params, NULL);
+	clk_register_clkdev(clk, "pll_d2", NULL);
+	clks[TEGRA210_CLK_PLL_D2] = clk;
+
+	/* PLLD2_OUT0 */
+	clk = clk_register_fixed_factor(NULL, "pll_d2_out0", "pll_d2",
+					CLK_SET_RATE_PARENT, 1, 1);
+	clk_register_clkdev(clk, "pll_d2_out0", NULL);
+	clks[TEGRA210_CLK_PLL_D2_OUT0] = clk;
+
+	/* PLLP_OUT2 */
+	clk = clk_register_fixed_factor(NULL, "pll_p_out2", "pll_p",
+					CLK_SET_RATE_PARENT, 1, 2);
+	clk_register_clkdev(clk, "pll_p_out2", NULL);
+	clks[TEGRA210_CLK_PLL_P_OUT2] = clk;
+
+}
+
+/* Tegra210 CPU clock and reset control functions */
+static void tegra210_wait_cpu_in_reset(u32 cpu)
+{
+	unsigned int reg;
+
+	do {
+		reg = readl(clk_base + CLK_RST_CONTROLLER_CPU_CMPLX_STATUS);
+		cpu_relax();
+	} while (!(reg & (1 << cpu)));  /* check CPU been reset or not */
+}
+
+static void tegra210_disable_cpu_clock(u32 cpu)
+{
+	/* flow controller would take care in the power sequence. */
+}
+
+#ifdef CONFIG_PM_SLEEP
+static void tegra210_cpu_clock_suspend(void)
+{
+	/* switch coresite to clk_m, save off original source */
+	tegra210_cpu_clk_sctx.clk_csite_src =
+				readl(clk_base + CLK_SOURCE_CSITE);
+	writel(3 << 30, clk_base + CLK_SOURCE_CSITE);
+}
+
+static void tegra210_cpu_clock_resume(void)
+{
+	writel(tegra210_cpu_clk_sctx.clk_csite_src,
+				clk_base + CLK_SOURCE_CSITE);
+}
+#endif
+
+static struct tegra_cpu_car_ops tegra210_cpu_car_ops = {
+	.wait_for_reset	= tegra210_wait_cpu_in_reset,
+	.disable_clock	= tegra210_disable_cpu_clock,
+#ifdef CONFIG_PM_SLEEP
+	.suspend	= tegra210_cpu_clock_suspend,
+	.resume		= tegra210_cpu_clock_resume,
+#endif
+};
+
+static const struct of_device_id pmc_match[] __initconst = {
+	{ .compatible = "nvidia,tegra210-pmc" },
+	{ },
+};
+
+static struct tegra_clk_init_table init_table[] __initdata = {
+	{ TEGRA210_CLK_UARTA, TEGRA210_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA210_CLK_UARTB, TEGRA210_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA210_CLK_UARTC, TEGRA210_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA210_CLK_UARTD, TEGRA210_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA210_CLK_PLL_A, TEGRA210_CLK_CLK_MAX, 564480000, 1 },
+	{ TEGRA210_CLK_PLL_A_OUT0, TEGRA210_CLK_CLK_MAX, 11289600, 1 },
+	{ TEGRA210_CLK_EXTERN1, TEGRA210_CLK_PLL_A_OUT0, 0, 1 },
+	{ TEGRA210_CLK_CLK_OUT_1_MUX, TEGRA210_CLK_EXTERN1, 0, 1 },
+	{ TEGRA210_CLK_CLK_OUT_1, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA210_CLK_I2S0, TEGRA210_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA210_CLK_I2S1, TEGRA210_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA210_CLK_I2S2, TEGRA210_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA210_CLK_I2S3, TEGRA210_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA210_CLK_I2S4, TEGRA210_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA210_CLK_HOST1X, TEGRA210_CLK_PLL_P, 136000000, 1 },
+	{ TEGRA210_CLK_SCLK_MUX, TEGRA210_CLK_PLL_P, 0, 1 },
+	{ TEGRA210_CLK_SCLK, TEGRA210_CLK_CLK_MAX, 102000000, 1 },
+	{ TEGRA210_CLK_DFLL_SOC, TEGRA210_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA210_CLK_DFLL_REF, TEGRA210_CLK_PLL_P, 51000000, 1 },
+	{ TEGRA210_CLK_SBC4, TEGRA210_CLK_PLL_P, 12000000, 1 },
+	{ TEGRA210_CLK_PLL_RE_VCO, TEGRA210_CLK_CLK_MAX, 672000000, 1 },
+	{ TEGRA210_CLK_XUSB_GATE, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA210_CLK_XUSB_SS_SRC, TEGRA210_CLK_PLL_U_480M, 120000000, 0 },
+	{ TEGRA210_CLK_XUSB_FS_SRC, TEGRA210_CLK_PLL_U_48M, 48000000, 0 },
+	{ TEGRA210_CLK_XUSB_HS_SRC, TEGRA210_CLK_XUSB_SS_SRC, 120000000, 0 },
+	{ TEGRA210_CLK_XUSB_SSP_SRC, TEGRA210_CLK_XUSB_SS_SRC, 120000000, 0 },
+	{ TEGRA210_CLK_XUSB_FALCON_SRC, TEGRA210_CLK_PLL_P_OUT_XUSB, 204000000, 0 },
+	{ TEGRA210_CLK_XUSB_HOST_SRC, TEGRA210_CLK_PLL_P_OUT_XUSB, 102000000, 0 },
+	{ TEGRA210_CLK_XUSB_DEV_SRC, TEGRA210_CLK_PLL_P_OUT_XUSB, 102000000, 0 },
+	{ TEGRA210_CLK_SATA, TEGRA210_CLK_PLL_P, 104000000, 0 },
+	{ TEGRA210_CLK_SATA_OOB, TEGRA210_CLK_PLL_P, 204000000, 0 },
+	{ TEGRA210_CLK_EMC, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA210_CLK_MSELECT, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA210_CLK_CSITE, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA210_CLK_TSENSOR, TEGRA210_CLK_CLK_M, 400000, 0 },
+	{ TEGRA210_CLK_I2C1, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_I2C2, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_I2C3, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_I2C4, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_I2C5, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_I2C6, TEGRA210_CLK_PLL_P, 0, 0 },
+	{ TEGRA210_CLK_PLL_DP, TEGRA210_CLK_CLK_MAX, 270000000, 0 },
+	{ TEGRA210_CLK_SOC_THERM, TEGRA210_CLK_PLL_P, 51000000, 0 },
+	{ TEGRA210_CLK_CCLK_G, TEGRA210_CLK_CLK_MAX, 0, 1 },
+	/* This MUST be the last entry. */
+	{ TEGRA210_CLK_CLK_MAX, TEGRA210_CLK_CLK_MAX, 0, 0 },
+};
+
+/**
+ * tegra210_clock_apply_init_table - initialize clocks on Tegra210 SoCs
+ *
+ * Program an initial clock rate and enable or disable clocks needed
+ * by the rest of the kernel, for Tegra210 SoCs.  It is intended to be
+ * called by assigning a pointer to it to tegra_clk_apply_init_table -
+ * this will be called as an arch_initcall.  No return value.
+ */
+static void __init tegra210_clock_apply_init_table(void)
+{
+	tegra_init_from_table(init_table, clks, TEGRA210_CLK_CLK_MAX);
+}
+
+/**
+ * tegra210_clock_init - Tegra210-specific clock initialization
+ * @np: struct device_node * of the DT node for the SoC CAR IP block
+ *
+ * Register most SoC clocks for the Tegra210 system-on-chip.  Intended
+ * to be called by the OF init code when a DT node with the
+ * "nvidia,tegra210-car" string is encountered, and declared with
+ * CLK_OF_DECLARE.  No return value.
+ */
+static void __init tegra210_clock_init(struct device_node *np)
+{
+	struct device_node *node;
+	u32 value, clk_m_div;
+
+	clk_base = of_iomap(np, 0);
+	if (!clk_base) {
+		pr_err("ioremap tegra210 CAR failed\n");
+		return;
+	}
+
+	node = of_find_matching_node(NULL, pmc_match);
+	if (!node) {
+		pr_err("Failed to find pmc node\n");
+		WARN_ON(1);
+		return;
+	}
+
+	pmc_base = of_iomap(node, 0);
+	if (!pmc_base) {
+		pr_err("Can't map pmc registers\n");
+		WARN_ON(1);
+		return;
+	}
+
+	clks = tegra_clk_init(clk_base, TEGRA210_CLK_CLK_MAX,
+			      TEGRA210_CAR_BANK_COUNT);
+	if (!clks)
+		return;
+
+	value = clk_readl(clk_base + SPARE_REG0) >> CLK_M_DIVISOR_SHIFT;
+	clk_m_div = (value & CLK_M_DIVISOR_MASK) + 1;
+
+	if (tegra_osc_clk_init(clk_base, tegra210_clks, tegra210_input_freq,
+			       ARRAY_SIZE(tegra210_input_freq), clk_m_div,
+			       &osc_freq, &pll_ref_freq) < 0)
+		return;
+
+	tegra_fixed_clk_init(tegra210_clks);
+	tegra210_pll_init(clk_base, pmc_base);
+	tegra210_periph_clk_init(clk_base, pmc_base);
+	tegra_audio_clk_init(clk_base, pmc_base, tegra210_clks,
+			     tegra210_audio_plls,
+			     ARRAY_SIZE(tegra210_audio_plls));
+	tegra_pmc_clk_init(pmc_base, tegra210_clks);
+
+	/* For Tegra210, PLLD is the only source for DSIA & DSIB */
+	value = clk_readl(clk_base + PLLD_BASE);
+	value &= ~BIT(25);
+	clk_writel(value, clk_base + PLLD_BASE);
+
+	tegra_clk_apply_init_table = tegra210_clock_apply_init_table;
+
+	tegra_super_clk_gen5_init(clk_base, pmc_base, tegra210_clks,
+				  &pll_x_params);
+	tegra_add_of_provider(np);
+	tegra_register_devclks(devclks, ARRAY_SIZE(devclks));
+
+	tegra_cpu_car_ops = &tegra210_cpu_car_ops;
+}
+CLK_OF_DECLARE(tegra210, "nvidia,tegra210-car", tegra210_clock_init);
diff --git a/drivers/clk/tegra/clk-tegra30.c b/drivers/clk/tegra/clk-tegra30.c
index b90db615c29e..0478565cf292 100644
--- a/drivers/clk/tegra/clk-tegra30.c
+++ b/drivers/clk/tegra/clk-tegra30.c
@@ -224,188 +224,192 @@ struct utmi_clk_param {
 };
 
 static const struct utmi_clk_param utmi_parameters[] = {
-/*	OSC_FREQUENCY, ENABLE_DLY, STABLE_CNT, ACTIVE_DLY, XTAL_FREQ_CNT */
-	{13000000,     0x02,       0x33,       0x05,       0x7F},
-	{19200000,     0x03,       0x4B,       0x06,       0xBB},
-	{12000000,     0x02,       0x2F,       0x04,       0x76},
-	{26000000,     0x04,       0x66,       0x09,       0xFE},
-	{16800000,     0x03,       0x41,       0x0A,       0xA4},
+	{
+		.osc_frequency = 13000000, .enable_delay_count = 0x02,
+		.stable_count = 0x33, .active_delay_count = 0x05,
+		.xtal_freq_count = 0x7f
+	}, {
+		.osc_frequency = 19200000, .enable_delay_count = 0x03,
+		.stable_count = 0x4b, .active_delay_count = 0x06,
+		.xtal_freq_count = 0xbb
+	}, {
+		.osc_frequency = 12000000, .enable_delay_count = 0x02,
+		.stable_count = 0x2f, .active_delay_count = 0x04,
+		.xtal_freq_count = 0x76
+	}, {
+		.osc_frequency = 26000000, .enable_delay_count = 0x04,
+		.stable_count = 0x66, .active_delay_count = 0x09,
+		.xtal_freq_count = 0xfe
+	}, {
+		.osc_frequency = 16800000, .enable_delay_count = 0x03,
+		.stable_count = 0x41, .active_delay_count = 0x0a,
+		.xtal_freq_count = 0xa4
+	},
 };
 
 static struct tegra_clk_pll_freq_table pll_c_freq_table[] = {
-	{ 12000000, 1040000000, 520,  6, 0, 8},
-	{ 13000000, 1040000000, 480,  6, 0, 8},
-	{ 16800000, 1040000000, 495,  8, 0, 8},	/* actual: 1039.5 MHz */
-	{ 19200000, 1040000000, 325,  6, 0, 6},
-	{ 26000000, 1040000000, 520, 13, 0, 8},
-
-	{ 12000000, 832000000, 416,  6, 0, 8},
-	{ 13000000, 832000000, 832, 13, 0, 8},
-	{ 16800000, 832000000, 396,  8, 0, 8},	/* actual: 831.6 MHz */
-	{ 19200000, 832000000, 260,  6, 0, 8},
-	{ 26000000, 832000000, 416, 13, 0, 8},
-
-	{ 12000000, 624000000, 624, 12, 0, 8},
-	{ 13000000, 624000000, 624, 13, 0, 8},
-	{ 16800000, 600000000, 520, 14, 0, 8},
-	{ 19200000, 624000000, 520, 16, 0, 8},
-	{ 26000000, 624000000, 624, 26, 0, 8},
-
-	{ 12000000, 600000000, 600, 12, 0, 8},
-	{ 13000000, 600000000, 600, 13, 0, 8},
-	{ 16800000, 600000000, 500, 14, 0, 8},
-	{ 19200000, 600000000, 375, 12, 0, 6},
-	{ 26000000, 600000000, 600, 26, 0, 8},
-
-	{ 12000000, 520000000, 520, 12, 0, 8},
-	{ 13000000, 520000000, 520, 13, 0, 8},
-	{ 16800000, 520000000, 495, 16, 0, 8},	/* actual: 519.75 MHz */
-	{ 19200000, 520000000, 325, 12, 0, 6},
-	{ 26000000, 520000000, 520, 26, 0, 8},
-
-	{ 12000000, 416000000, 416, 12, 0, 8},
-	{ 13000000, 416000000, 416, 13, 0, 8},
-	{ 16800000, 416000000, 396, 16, 0, 8},	/* actual: 415.8 MHz */
-	{ 19200000, 416000000, 260, 12, 0, 6},
-	{ 26000000, 416000000, 416, 26, 0, 8},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 1040000000, 520,  6, 1, 8 },
+	{ 13000000, 1040000000, 480,  6, 1, 8 },
+	{ 16800000, 1040000000, 495,  8, 1, 8 }, /* actual: 1039.5 MHz */
+	{ 19200000, 1040000000, 325,  6, 1, 6 },
+	{ 26000000, 1040000000, 520, 13, 1, 8 },
+	{ 12000000,  832000000, 416,  6, 1, 8 },
+	{ 13000000,  832000000, 832, 13, 1, 8 },
+	{ 16800000,  832000000, 396,  8, 1, 8 }, /* actual: 831.6 MHz */
+	{ 19200000,  832000000, 260,  6, 1, 8 },
+	{ 26000000,  832000000, 416, 13, 1, 8 },
+	{ 12000000,  624000000, 624, 12, 1, 8 },
+	{ 13000000,  624000000, 624, 13, 1, 8 },
+	{ 16800000,  600000000, 520, 14, 1, 8 },
+	{ 19200000,  624000000, 520, 16, 1, 8 },
+	{ 26000000,  624000000, 624, 26, 1, 8 },
+	{ 12000000,  600000000, 600, 12, 1, 8 },
+	{ 13000000,  600000000, 600, 13, 1, 8 },
+	{ 16800000,  600000000, 500, 14, 1, 8 },
+	{ 19200000,  600000000, 375, 12, 1, 6 },
+	{ 26000000,  600000000, 600, 26, 1, 8 },
+	{ 12000000,  520000000, 520, 12, 1, 8 },
+	{ 13000000,  520000000, 520, 13, 1, 8 },
+	{ 16800000,  520000000, 495, 16, 1, 8 }, /* actual: 519.75 MHz */
+	{ 19200000,  520000000, 325, 12, 1, 6 },
+	{ 26000000,  520000000, 520, 26, 1, 8 },
+	{ 12000000,  416000000, 416, 12, 1, 8 },
+	{ 13000000,  416000000, 416, 13, 1, 8 },
+	{ 16800000,  416000000, 396, 16, 1, 8 }, /* actual: 415.8 MHz */
+	{ 19200000,  416000000, 260, 12, 1, 6 },
+	{ 26000000,  416000000, 416, 26, 1, 8 },
+	{        0,          0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_m_freq_table[] = {
-	{ 12000000, 666000000, 666, 12, 0, 8},
-	{ 13000000, 666000000, 666, 13, 0, 8},
-	{ 16800000, 666000000, 555, 14, 0, 8},
-	{ 19200000, 666000000, 555, 16, 0, 8},
-	{ 26000000, 666000000, 666, 26, 0, 8},
-	{ 12000000, 600000000, 600, 12, 0, 8},
-	{ 13000000, 600000000, 600, 13, 0, 8},
-	{ 16800000, 600000000, 500, 14, 0, 8},
-	{ 19200000, 600000000, 375, 12, 0, 6},
-	{ 26000000, 600000000, 600, 26, 0, 8},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 666000000, 666, 12, 1, 8 },
+	{ 13000000, 666000000, 666, 13, 1, 8 },
+	{ 16800000, 666000000, 555, 14, 1, 8 },
+	{ 19200000, 666000000, 555, 16, 1, 8 },
+	{ 26000000, 666000000, 666, 26, 1, 8 },
+	{ 12000000, 600000000, 600, 12, 1, 8 },
+	{ 13000000, 600000000, 600, 13, 1, 8 },
+	{ 16800000, 600000000, 500, 14, 1, 8 },
+	{ 19200000, 600000000, 375, 12, 1, 6 },
+	{ 26000000, 600000000, 600, 26, 1, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_p_freq_table[] = {
-	{ 12000000, 216000000, 432, 12, 1, 8},
-	{ 13000000, 216000000, 432, 13, 1, 8},
-	{ 16800000, 216000000, 360, 14, 1, 8},
-	{ 19200000, 216000000, 360, 16, 1, 8},
-	{ 26000000, 216000000, 432, 26, 1, 8},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 216000000, 432, 12, 2, 8 },
+	{ 13000000, 216000000, 432, 13, 2, 8 },
+	{ 16800000, 216000000, 360, 14, 2, 8 },
+	{ 19200000, 216000000, 360, 16, 2, 8 },
+	{ 26000000, 216000000, 432, 26, 2, 8 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_a_freq_table[] = {
-	{ 9600000, 564480000, 294, 5, 0, 4},
-	{ 9600000, 552960000, 288, 5, 0, 4},
-	{ 9600000, 24000000,  5,   2, 0, 1},
-
-	{ 28800000, 56448000, 49, 25, 0, 1},
-	{ 28800000, 73728000, 64, 25, 0, 1},
-	{ 28800000, 24000000,  5,  6, 0, 1},
-	{ 0, 0, 0, 0, 0, 0 },
+	{  9600000, 564480000, 294,  5, 1, 4 },
+	{  9600000, 552960000, 288,  5, 1, 4 },
+	{  9600000,  24000000,   5,  2, 1, 1 },
+	{ 28800000,  56448000,  49, 25, 1, 1 },
+	{ 28800000,  73728000,  64, 25, 1, 1 },
+	{ 28800000,  24000000,   5,  6, 1, 1 },
+	{        0,         0,   0,  0, 0, 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_d_freq_table[] = {
-	{ 12000000, 216000000, 216, 12, 0, 4},
-	{ 13000000, 216000000, 216, 13, 0, 4},
-	{ 16800000, 216000000, 180, 14, 0, 4},
-	{ 19200000, 216000000, 180, 16, 0, 4},
-	{ 26000000, 216000000, 216, 26, 0, 4},
-
-	{ 12000000, 594000000, 594, 12, 0, 8},
-	{ 13000000, 594000000, 594, 13, 0, 8},
-	{ 16800000, 594000000, 495, 14, 0, 8},
-	{ 19200000, 594000000, 495, 16, 0, 8},
-	{ 26000000, 594000000, 594, 26, 0, 8},
-
-	{ 12000000, 1000000000, 1000, 12, 0, 12},
-	{ 13000000, 1000000000, 1000, 13, 0, 12},
-	{ 19200000, 1000000000, 625,  12, 0, 8},
-	{ 26000000, 1000000000, 1000, 26, 0, 12},
-
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000,  216000000,  216, 12, 1,  4 },
+	{ 13000000,  216000000,  216, 13, 1,  4 },
+	{ 16800000,  216000000,  180, 14, 1,  4 },
+	{ 19200000,  216000000,  180, 16, 1,  4 },
+	{ 26000000,  216000000,  216, 26, 1,  4 },
+	{ 12000000,  594000000,  594, 12, 1,  8 },
+	{ 13000000,  594000000,  594, 13, 1,  8 },
+	{ 16800000,  594000000,  495, 14, 1,  8 },
+	{ 19200000,  594000000,  495, 16, 1,  8 },
+	{ 26000000,  594000000,  594, 26, 1,  8 },
+	{ 12000000, 1000000000, 1000, 12, 1, 12 },
+	{ 13000000, 1000000000, 1000, 13, 1, 12 },
+	{ 19200000, 1000000000,  625, 12, 1,  8 },
+	{ 26000000, 1000000000, 1000, 26, 1, 12 },
+	{        0,          0,    0,  0, 0,  0 },
 };
 
-static struct pdiv_map pllu_p[] = {
+static const struct pdiv_map pllu_p[] = {
 	{ .pdiv = 1, .hw_val = 1 },
 	{ .pdiv = 2, .hw_val = 0 },
 	{ .pdiv = 0, .hw_val = 0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_u_freq_table[] = {
-	{ 12000000, 480000000, 960, 12, 0, 12},
-	{ 13000000, 480000000, 960, 13, 0, 12},
-	{ 16800000, 480000000, 400, 7,  0, 5},
-	{ 19200000, 480000000, 200, 4,  0, 3},
-	{ 26000000, 480000000, 960, 26, 0, 12},
-	{ 0, 0, 0, 0, 0, 0 },
+	{ 12000000, 480000000, 960, 12, 1, 12 },
+	{ 13000000, 480000000, 960, 13, 1, 12 },
+	{ 16800000, 480000000, 400,  7, 1,  5 },
+	{ 19200000, 480000000, 200,  4, 1,  3 },
+	{ 26000000, 480000000, 960, 26, 1, 12 },
+	{        0,         0,   0,  0, 0,  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_x_freq_table[] = {
 	/* 1.7 GHz */
-	{ 12000000, 1700000000, 850,  6,  0, 8},
-	{ 13000000, 1700000000, 915,  7,  0, 8},	/* actual: 1699.2 MHz */
-	{ 16800000, 1700000000, 708,  7,  0, 8},	/* actual: 1699.2 MHz */
-	{ 19200000, 1700000000, 885,  10, 0, 8},	/* actual: 1699.2 MHz */
-	{ 26000000, 1700000000, 850,  13, 0, 8},
-
+	{ 12000000, 1700000000, 850,   6, 1, 8 },
+	{ 13000000, 1700000000, 915,   7, 1, 8 }, /* actual: 1699.2 MHz */
+	{ 16800000, 1700000000, 708,   7, 1, 8 }, /* actual: 1699.2 MHz */
+	{ 19200000, 1700000000, 885,  10, 1, 8 }, /* actual: 1699.2 MHz */
+	{ 26000000, 1700000000, 850,  13, 1, 8 },
 	/* 1.6 GHz */
-	{ 12000000, 1600000000, 800,  6,  0, 8},
-	{ 13000000, 1600000000, 738,  6,  0, 8},	/* actual: 1599.0 MHz */
-	{ 16800000, 1600000000, 857,  9,  0, 8},	/* actual: 1599.7 MHz */
-	{ 19200000, 1600000000, 500,  6,  0, 8},
-	{ 26000000, 1600000000, 800,  13, 0, 8},
-
+	{ 12000000, 1600000000, 800,   6, 1, 8 },
+	{ 13000000, 1600000000, 738,   6, 1, 8 }, /* actual: 1599.0 MHz */
+	{ 16800000, 1600000000, 857,   9, 1, 8 }, /* actual: 1599.7 MHz */
+	{ 19200000, 1600000000, 500,   6, 1, 8 },
+	{ 26000000, 1600000000, 800,  13, 1, 8 },
 	/* 1.5 GHz */
-	{ 12000000, 1500000000, 750,  6,  0, 8},
-	{ 13000000, 1500000000, 923,  8,  0, 8},	/* actual: 1499.8 MHz */
-	{ 16800000, 1500000000, 625,  7,  0, 8},
-	{ 19200000, 1500000000, 625,  8,  0, 8},
-	{ 26000000, 1500000000, 750,  13, 0, 8},
-
+	{ 12000000, 1500000000, 750,   6, 1, 8 },
+	{ 13000000, 1500000000, 923,   8, 1, 8 }, /* actual: 1499.8 MHz */
+	{ 16800000, 1500000000, 625,   7, 1, 8 },
+	{ 19200000, 1500000000, 625,   8, 1, 8 },
+	{ 26000000, 1500000000, 750,  13, 1, 8 },
 	/* 1.4 GHz */
-	{ 12000000, 1400000000, 700,  6,  0, 8},
-	{ 13000000, 1400000000, 969,  9,  0, 8},	/* actual: 1399.7 MHz */
-	{ 16800000, 1400000000, 1000, 12, 0, 8},
-	{ 19200000, 1400000000, 875,  12, 0, 8},
-	{ 26000000, 1400000000, 700,  13, 0, 8},
-
+	{ 12000000, 1400000000,  700,  6, 1, 8 },
+	{ 13000000, 1400000000,  969,  9, 1, 8 }, /* actual: 1399.7 MHz */
+	{ 16800000, 1400000000, 1000, 12, 1, 8 },
+	{ 19200000, 1400000000,  875, 12, 1, 8 },
+	{ 26000000, 1400000000,  700, 13, 1, 8 },
 	/* 1.3 GHz */
-	{ 12000000, 1300000000, 975,  9,  0, 8},
-	{ 13000000, 1300000000, 1000, 10, 0, 8},
-	{ 16800000, 1300000000, 928,  12, 0, 8},	/* actual: 1299.2 MHz */
-	{ 19200000, 1300000000, 812,  12, 0, 8},	/* actual: 1299.2 MHz */
-	{ 26000000, 1300000000, 650,  13, 0, 8},
-
+	{ 12000000, 1300000000,  975,  9, 1, 8 },
+	{ 13000000, 1300000000, 1000, 10, 1, 8 },
+	{ 16800000, 1300000000,  928, 12, 1, 8 }, /* actual: 1299.2 MHz */
+	{ 19200000, 1300000000,  812, 12, 1, 8 }, /* actual: 1299.2 MHz */
+	{ 26000000, 1300000000,  650, 13, 1, 8 },
 	/* 1.2 GHz */
-	{ 12000000, 1200000000, 1000, 10, 0, 8},
-	{ 13000000, 1200000000, 923,  10, 0, 8},	/* actual: 1199.9 MHz */
-	{ 16800000, 1200000000, 1000, 14, 0, 8},
-	{ 19200000, 1200000000, 1000, 16, 0, 8},
-	{ 26000000, 1200000000, 600,  13, 0, 8},
-
+	{ 12000000, 1200000000, 1000, 10, 1, 8 },
+	{ 13000000, 1200000000,  923, 10, 1, 8 }, /* actual: 1199.9 MHz */
+	{ 16800000, 1200000000, 1000, 14, 1, 8 },
+	{ 19200000, 1200000000, 1000, 16, 1, 8 },
+	{ 26000000, 1200000000,  600, 13, 1, 8 },
 	/* 1.1 GHz */
-	{ 12000000, 1100000000, 825,  9,  0, 8},
-	{ 13000000, 1100000000, 846,  10, 0, 8},	/* actual: 1099.8 MHz */
-	{ 16800000, 1100000000, 982,  15, 0, 8},	/* actual: 1099.8 MHz */
-	{ 19200000, 1100000000, 859,  15, 0, 8},	/* actual: 1099.5 MHz */
-	{ 26000000, 1100000000, 550,  13, 0, 8},
-
+	{ 12000000, 1100000000, 825,   9, 1, 8 },
+	{ 13000000, 1100000000, 846,  10, 1, 8 }, /* actual: 1099.8 MHz */
+	{ 16800000, 1100000000, 982,  15, 1, 8 }, /* actual: 1099.8 MHz */
+	{ 19200000, 1100000000, 859,  15, 1, 8 }, /* actual: 1099.5 MHz */
+	{ 26000000, 1100000000, 550,  13, 1, 8 },
 	/* 1 GHz */
-	{ 12000000, 1000000000, 1000, 12, 0, 8},
-	{ 13000000, 1000000000, 1000, 13, 0, 8},
-	{ 16800000, 1000000000, 833,  14, 0, 8},	/* actual: 999.6 MHz */
-	{ 19200000, 1000000000, 625,  12, 0, 8},
-	{ 26000000, 1000000000, 1000, 26, 0, 8},
+	{ 12000000, 1000000000, 1000, 12, 1, 8 },
+	{ 13000000, 1000000000, 1000, 13, 1, 8 },
+	{ 16800000, 1000000000,  833, 14, 1, 8 }, /* actual: 999.6 MHz */
+	{ 19200000, 1000000000,  625, 12, 1, 8 },
+	{ 26000000, 1000000000, 1000, 26, 1, 8 },
+	{        0,          0,    0,  0, 0, 0 },
+};
 
-	{ 0, 0, 0, 0, 0, 0 },
+static const struct pdiv_map plle_p[] = {
+	{ .pdiv = 18, .hw_val = 18 },
+	{ .pdiv = 24, .hw_val = 24 },
+	{ .pdiv =  0, .hw_val =  0 },
 };
 
 static struct tegra_clk_pll_freq_table pll_e_freq_table[] = {
 	/* PLLE special case: use cpcon field to store cml divider value */
-	{ 12000000,  100000000, 150, 1,  18, 11},
-	{ 216000000, 100000000, 200, 18, 24, 13},
-	{ 0, 0, 0, 0, 0, 0 },
+	{  12000000, 100000000, 150,  1, 18, 11 },
+	{ 216000000, 100000000, 200, 18, 24, 13 },
+	{         0,         0,   0,  0,  0,  0 },
 };
 
 /* PLL parameters */
@@ -422,7 +426,8 @@ static struct tegra_clk_pll_params pll_c_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_c_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct div_nmp pllm_nmp = {
@@ -454,7 +459,8 @@ static struct tegra_clk_pll_params pll_m_params = {
 	.pmc_divp_reg = PMC_PLLM_WB0_OVERRIDE,
 	.freq_table = pll_m_freq_table,
 	.flags = TEGRA_PLLM | TEGRA_PLL_HAS_CPCON |
-		 TEGRA_PLL_SET_DCCON | TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_SET_DCCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE | TEGRA_PLL_FIXED,
 };
 
 static struct tegra_clk_pll_params pll_p_params = {
@@ -470,7 +476,8 @@ static struct tegra_clk_pll_params pll_p_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_p_freq_table,
-	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_FIXED | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 	.fixed_rate = 408000000,
 };
 
@@ -487,7 +494,8 @@ static struct tegra_clk_pll_params pll_a_params = {
 	.lock_enable_bit_idx = PLL_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
 	.freq_table = pll_a_freq_table,
-	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK,
+	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_USE_LOCK |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_d_params = {
@@ -504,8 +512,7 @@ static struct tegra_clk_pll_params pll_d_params = {
 	.lock_delay = 1000,
 	.freq_table = pll_d_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
-
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_d2_params = {
@@ -522,7 +529,7 @@ static struct tegra_clk_pll_params pll_d2_params = {
 	.lock_delay = 1000,
 	.freq_table = pll_d_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_u_params = {
@@ -539,7 +546,8 @@ static struct tegra_clk_pll_params pll_u_params = {
 	.lock_delay = 1000,
 	.pdiv_tohw = pllu_p,
 	.freq_table = pll_u_freq_table,
-	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON,
+	.flags = TEGRA_PLLU | TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_LFCON |
+		 TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_x_params = {
@@ -556,7 +564,7 @@ static struct tegra_clk_pll_params pll_x_params = {
 	.lock_delay = 300,
 	.freq_table = pll_x_freq_table,
 	.flags = TEGRA_PLL_HAS_CPCON | TEGRA_PLL_SET_DCCON |
-		 TEGRA_PLL_USE_LOCK,
+		 TEGRA_PLL_USE_LOCK | TEGRA_PLL_HAS_LOCK_ENABLE,
 };
 
 static struct tegra_clk_pll_params pll_e_params = {
@@ -571,19 +579,21 @@ static struct tegra_clk_pll_params pll_e_params = {
 	.lock_mask = PLLE_MISC_LOCK,
 	.lock_enable_bit_idx = PLLE_MISC_LOCK_ENABLE,
 	.lock_delay = 300,
+	.pdiv_tohw = plle_p,
 	.freq_table = pll_e_freq_table,
-	.flags = TEGRA_PLLE_CONFIGURE | TEGRA_PLL_FIXED,
+	.flags = TEGRA_PLLE_CONFIGURE | TEGRA_PLL_FIXED |
+		 TEGRA_PLL_HAS_LOCK_ENABLE | TEGRA_PLL_LOCK_MISC,
 	.fixed_rate = 100000000,
 };
 
 static unsigned long tegra30_input_freq[] = {
-	[0] = 13000000,
-	[1] = 16800000,
-	[4] = 19200000,
-	[5] = 38400000,
-	[8] = 12000000,
-	[9] = 48000000,
-	[12] = 260000000,
+	[ 0] = 13000000,
+	[ 1] = 16800000,
+	[ 4] = 19200000,
+	[ 5] = 38400000,
+	[ 8] = 12000000,
+	[ 9] = 48000000,
+	[12] = 26000000,
 };
 
 static struct tegra_devclk devclks[] __initdata = {
@@ -861,13 +871,12 @@ static struct tegra_clk tegra30_clks[tegra_clk_max] __initdata = {
 	[tegra_clk_pll_p_out4] = { .dt_id = TEGRA30_CLK_PLL_P_OUT4, .present = true },
 	[tegra_clk_pll_a] = { .dt_id = TEGRA30_CLK_PLL_A, .present = true },
 	[tegra_clk_pll_a_out0] = { .dt_id = TEGRA30_CLK_PLL_A_OUT0, .present = true },
-
 };
 
 static void tegra30_utmi_param_configure(void)
 {
+	unsigned int i;
 	u32 reg;
-	int i;
 
 	for (i = 0; i < ARRAY_SIZE(utmi_parameters); i++) {
 		if (input_freq == utmi_parameters[i].osc_frequency)
@@ -917,7 +926,7 @@ static void tegra30_utmi_param_configure(void)
 	writel_relaxed(reg, clk_base + UTMIP_PLL_CFG1);
 }
 
-static const char *pll_e_parents[] = {"pll_ref", "pll_p"};
+static const char *pll_e_parents[] = { "pll_ref", "pll_p" };
 
 static void __init tegra30_pll_init(void)
 {
@@ -925,7 +934,7 @@ static void __init tegra30_pll_init(void)
 
 	/* PLLC */
 	clk = tegra_clk_register_pll("pll_c", "pll_ref", clk_base, pmc_base, 0,
-				&pll_c_params, NULL);
+				     &pll_c_params, NULL);
 	clks[TEGRA30_CLK_PLL_C] = clk;
 
 	/* PLLC_OUT1 */
@@ -1135,7 +1144,7 @@ static void __init tegra30_periph_clk_init(void)
 {
 	struct tegra_periph_init_data *data;
 	struct clk *clk;
-	int i;
+	unsigned int i;
 
 	/* dsia */
 	clk = tegra_clk_register_periph_gate("dsia", "pll_d_out0", 0, clk_base,
@@ -1224,7 +1233,6 @@ static void tegra30_cpu_out_of_reset(u32 cpu)
 	wmb();
 }
 
-
 static void tegra30_enable_cpu_clock(u32 cpu)
 {
 	unsigned int reg;
@@ -1237,7 +1245,6 @@ static void tegra30_enable_cpu_clock(u32 cpu)
 
 static void tegra30_disable_cpu_clock(u32 cpu)
 {
-
 	unsigned int reg;
 
 	reg = readl(clk_base + TEGRA_CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
@@ -1268,7 +1275,7 @@ static void tegra30_cpu_clock_suspend(void)
 	/* switch coresite to clk_m, save off original source */
 	tegra30_cpu_clk_sctx.clk_csite_src =
 				readl(clk_base + CLK_RESET_SOURCE_CSITE);
-	writel(3<<30, clk_base + CLK_RESET_SOURCE_CSITE);
+	writel(3 << 30, clk_base + CLK_RESET_SOURCE_CSITE);
 
 	tegra30_cpu_clk_sctx.cpu_burst =
 				readl(clk_base + CLK_RESET_CCLK_BURST);
@@ -1335,44 +1342,45 @@ static struct tegra_cpu_car_ops tegra30_cpu_car_ops = {
 };
 
 static struct tegra_clk_init_table init_table[] __initdata = {
-	{TEGRA30_CLK_UARTA, TEGRA30_CLK_PLL_P, 408000000, 0},
-	{TEGRA30_CLK_UARTB, TEGRA30_CLK_PLL_P, 408000000, 0},
-	{TEGRA30_CLK_UARTC, TEGRA30_CLK_PLL_P, 408000000, 0},
-	{TEGRA30_CLK_UARTD, TEGRA30_CLK_PLL_P, 408000000, 0},
-	{TEGRA30_CLK_UARTE, TEGRA30_CLK_PLL_P, 408000000, 0},
-	{TEGRA30_CLK_PLL_A, TEGRA30_CLK_CLK_MAX, 564480000, 1},
-	{TEGRA30_CLK_PLL_A_OUT0, TEGRA30_CLK_CLK_MAX, 11289600, 1},
-	{TEGRA30_CLK_EXTERN1, TEGRA30_CLK_PLL_A_OUT0, 0, 1},
-	{TEGRA30_CLK_CLK_OUT_1_MUX, TEGRA30_CLK_EXTERN1, 0, 0},
-	{TEGRA30_CLK_CLK_OUT_1, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_BLINK, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_I2S0, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA30_CLK_I2S1, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA30_CLK_I2S2, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA30_CLK_I2S3, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA30_CLK_I2S4, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0},
-	{TEGRA30_CLK_SDMMC1, TEGRA30_CLK_PLL_P, 48000000, 0},
-	{TEGRA30_CLK_SDMMC2, TEGRA30_CLK_PLL_P, 48000000, 0},
-	{TEGRA30_CLK_SDMMC3, TEGRA30_CLK_PLL_P, 48000000, 0},
-	{TEGRA30_CLK_PLL_M, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_PCLK, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_CSITE, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_EMC, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_MSELECT, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_SBC1, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_SBC2, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_SBC3, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_SBC4, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_SBC5, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_SBC6, TEGRA30_CLK_PLL_P, 100000000, 0},
-	{TEGRA30_CLK_HOST1X, TEGRA30_CLK_PLL_C, 150000000, 0},
-	{TEGRA30_CLK_DISP1, TEGRA30_CLK_PLL_P, 600000000, 0},
-	{TEGRA30_CLK_DISP2, TEGRA30_CLK_PLL_P, 600000000, 0},
-	{TEGRA30_CLK_TWD, TEGRA30_CLK_CLK_MAX, 0, 1},
-	{TEGRA30_CLK_GR2D, TEGRA30_CLK_PLL_C, 300000000, 0},
-	{TEGRA30_CLK_GR3D, TEGRA30_CLK_PLL_C, 300000000, 0},
-	{TEGRA30_CLK_GR3D2, TEGRA30_CLK_PLL_C, 300000000, 0},
-	{TEGRA30_CLK_CLK_MAX, TEGRA30_CLK_CLK_MAX, 0, 0}, /* This MUST be the last entry. */
+	{ TEGRA30_CLK_UARTA, TEGRA30_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA30_CLK_UARTB, TEGRA30_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA30_CLK_UARTC, TEGRA30_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA30_CLK_UARTD, TEGRA30_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA30_CLK_UARTE, TEGRA30_CLK_PLL_P, 408000000, 0 },
+	{ TEGRA30_CLK_PLL_A, TEGRA30_CLK_CLK_MAX, 564480000, 1 },
+	{ TEGRA30_CLK_PLL_A_OUT0, TEGRA30_CLK_CLK_MAX, 11289600, 1 },
+	{ TEGRA30_CLK_EXTERN1, TEGRA30_CLK_PLL_A_OUT0, 0, 1 },
+	{ TEGRA30_CLK_CLK_OUT_1_MUX, TEGRA30_CLK_EXTERN1, 0, 0 },
+	{ TEGRA30_CLK_CLK_OUT_1, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_BLINK, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_I2S0, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA30_CLK_I2S1, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA30_CLK_I2S2, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA30_CLK_I2S3, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA30_CLK_I2S4, TEGRA30_CLK_PLL_A_OUT0, 11289600, 0 },
+	{ TEGRA30_CLK_SDMMC1, TEGRA30_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA30_CLK_SDMMC2, TEGRA30_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA30_CLK_SDMMC3, TEGRA30_CLK_PLL_P, 48000000, 0 },
+	{ TEGRA30_CLK_PLL_M, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_PCLK, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_CSITE, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_EMC, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_MSELECT, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_SBC1, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_SBC2, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_SBC3, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_SBC4, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_SBC5, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_SBC6, TEGRA30_CLK_PLL_P, 100000000, 0 },
+	{ TEGRA30_CLK_HOST1X, TEGRA30_CLK_PLL_C, 150000000, 0 },
+	{ TEGRA30_CLK_DISP1, TEGRA30_CLK_PLL_P, 600000000, 0 },
+	{ TEGRA30_CLK_DISP2, TEGRA30_CLK_PLL_P, 600000000, 0 },
+	{ TEGRA30_CLK_TWD, TEGRA30_CLK_CLK_MAX, 0, 1 },
+	{ TEGRA30_CLK_GR2D, TEGRA30_CLK_PLL_C, 300000000, 0 },
+	{ TEGRA30_CLK_GR3D, TEGRA30_CLK_PLL_C, 300000000, 0 },
+	{ TEGRA30_CLK_GR3D2, TEGRA30_CLK_PLL_C, 300000000, 0 },
+	/* must be the last entry */
+	{ TEGRA30_CLK_CLK_MAX, TEGRA30_CLK_CLK_MAX, 0, 0 },
 };
 
 static void __init tegra30_clock_apply_init_table(void)
@@ -1397,12 +1405,13 @@ static struct tegra_clk_duplicate tegra_clk_duplicates[] = {
 	TEGRA_CLK_DUPLICATE(TEGRA30_CLK_CML1, "tegra_sata_cml", NULL),
 	TEGRA_CLK_DUPLICATE(TEGRA30_CLK_CML0, "tegra_pcie", "cml"),
 	TEGRA_CLK_DUPLICATE(TEGRA30_CLK_VCP, "nvavp", "vcp"),
-	TEGRA_CLK_DUPLICATE(TEGRA30_CLK_CLK_MAX, NULL, NULL), /* MUST be the last entry */
+	/* must be the last entry */
+	TEGRA_CLK_DUPLICATE(TEGRA30_CLK_CLK_MAX, NULL, NULL),
 };
 
 static const struct of_device_id pmc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra30-pmc" },
-	{},
+	{ },
 };
 
 static struct tegra_audio_clk_info tegra30_audio_plls[] = {
@@ -1441,7 +1450,6 @@ static void __init tegra30_clock_init(struct device_node *np)
 			       NULL) < 0)
 		return;
 
-
 	tegra_fixed_clk_init(tegra30_clks);
 	tegra30_pll_init();
 	tegra30_super_clk_init();
diff --git a/drivers/clk/tegra/clk.h b/drivers/clk/tegra/clk.h
index 5d2678914160..4dbcfaec576a 100644
--- a/drivers/clk/tegra/clk.h
+++ b/drivers/clk/tegra/clk.h
@@ -110,14 +110,16 @@ struct clk *tegra_clk_register_mc(const char *name, const char *parent_name,
  * @m:			input divider
  * @p:			post divider
  * @cpcon:		charge pump current
+ * @sdm_data:		fraction divider setting (0 = disabled)
  */
 struct tegra_clk_pll_freq_table {
 	unsigned long	input_rate;
 	unsigned long	output_rate;
-	u16		n;
+	u32		n;
 	u16		m;
 	u8		p;
 	u8		cpcon;
+	u16		sdm_data;
 };
 
 /**
@@ -156,6 +158,10 @@ struct div_nmp {
 	u8		override_divp_shift;
 };
 
+#define MAX_PLL_MISC_REG_COUNT	6
+
+struct tegra_clk_pll;
+
 /**
  * struct tegra_clk_pll_params - PLL parameters
  *
@@ -172,6 +178,14 @@ struct div_nmp {
  * @lock_enable_bit_idx:	Bit index to enable PLL lock
  * @iddq_reg:			PLL IDDQ register offset
  * @iddq_bit_idx:		Bit index to enable PLL IDDQ
+ * @reset_reg:			Register offset of where RESET bit is
+ * @reset_bit_idx:		Shift of reset bit in reset_reg
+ * @sdm_din_reg:		Register offset where SDM settings are
+ * @sdm_din_mask:		Mask of SDM divider bits
+ * @sdm_ctrl_reg:		Register offset where SDM enable is
+ * @sdm_ctrl_en_mask:		Mask of SDM enable bit
+ * @ssc_ctrl_reg:		Register offset where SSC settings are
+ * @ssc_ctrl_en_mask:		Mask of SSC enable bit
  * @aux_reg:			AUX register offset
  * @dyn_ramp_reg:		Dynamic ramp control register offset
  * @ext_misc_reg:		Miscellaneous control register offsets
@@ -182,10 +196,27 @@ struct div_nmp {
  * @stepb_shift:		Dynamic ramp step B field shift
  * @lock_delay:			Delay in us if PLL lock is not used
  * @max_p:			maximum value for the p divider
+ * @defaults_set:		Boolean signaling all reg defaults for PLL set.
  * @pdiv_tohw:			mapping of p divider to register values
  * @div_nmp:			offsets and widths on n, m and p fields
  * @freq_table:			array of frequencies supported by PLL
  * @fixed_rate:			PLL rate if it is fixed
+ * @mdiv_default:		Default value for fixed mdiv for this PLL
+ * @round_p_to_pdiv:		Callback used to round p to the closed pdiv
+ * @set_gain:			Callback to adjust N div for SDM enabled
+ *				PLL's based on fractional divider value.
+ * @calc_rate:			Callback used to change how out of table
+ *				rates (dividers and multipler) are calculated.
+ * @adjust_vco:			Callback to adjust the programming range of the
+ *				divider range (if SDM is present)
+ * @set_defaults:		Callback which will try to initialize PLL
+ *				registers to sane default values. This is first
+ *				tried during PLL registration, but if the PLL
+ *				is already enabled, it will be done the first
+ *				time the rate is changed while the PLL is
+ *				disabled.
+ * @dyn_ramp:			Callback which can be used to define a custom
+ *				dynamic ramp function for a given PLL.
  *
  * Flags:
  * TEGRA_PLL_USE_LOCK - This flag indicated to use lock bits for
@@ -207,6 +238,11 @@ struct div_nmp {
  *     base register.
  * TEGRA_PLL_BYPASS - PLL has bypass bit
  * TEGRA_PLL_HAS_LOCK_ENABLE - PLL has bit to enable lock monitoring
+ * TEGRA_MDIV_NEW - Switch to new method for calculating fixed mdiv
+ *     it may be more accurate (especially if SDM present)
+ * TEGRA_PLLMB - PLLMB has should be treated similar to PLLM. This
+ *     flag indicated that it is PLLMB.
+ * TEGRA_PLL_VCO_OUT - Used to indicate that the PLL has a VCO output
  */
 struct tegra_clk_pll_params {
 	unsigned long	input_min;
@@ -223,9 +259,17 @@ struct tegra_clk_pll_params {
 	u32		lock_enable_bit_idx;
 	u32		iddq_reg;
 	u32		iddq_bit_idx;
+	u32		reset_reg;
+	u32		reset_bit_idx;
+	u32		sdm_din_reg;
+	u32		sdm_din_mask;
+	u32		sdm_ctrl_reg;
+	u32		sdm_ctrl_en_mask;
+	u32		ssc_ctrl_reg;
+	u32		ssc_ctrl_en_mask;
 	u32		aux_reg;
 	u32		dyn_ramp_reg;
-	u32		ext_misc_reg[3];
+	u32		ext_misc_reg[MAX_PLL_MISC_REG_COUNT];
 	u32		pmc_divnm_reg;
 	u32		pmc_divp_reg;
 	u32		flags;
@@ -233,10 +277,22 @@ struct tegra_clk_pll_params {
 	int		stepb_shift;
 	int		lock_delay;
 	int		max_p;
-	struct pdiv_map *pdiv_tohw;
+	bool		defaults_set;
+	const struct pdiv_map *pdiv_tohw;
 	struct div_nmp	*div_nmp;
 	struct tegra_clk_pll_freq_table	*freq_table;
 	unsigned long	fixed_rate;
+	u16		mdiv_default;
+	u32	(*round_p_to_pdiv)(u32 p, u32 *pdiv);
+	void	(*set_gain)(struct tegra_clk_pll_freq_table *cfg);
+	int	(*calc_rate)(struct clk_hw *hw,
+			struct tegra_clk_pll_freq_table *cfg,
+			unsigned long rate, unsigned long parent_rate);
+	unsigned long	(*adjust_vco)(struct tegra_clk_pll_params *pll_params,
+				unsigned long parent_rate);
+	void	(*set_defaults)(struct tegra_clk_pll *pll);
+	int	(*dyn_ramp)(struct tegra_clk_pll *pll,
+			struct tegra_clk_pll_freq_table *cfg);
 };
 
 #define TEGRA_PLL_USE_LOCK BIT(0)
@@ -250,6 +306,9 @@ struct tegra_clk_pll_params {
 #define TEGRA_PLL_LOCK_MISC BIT(8)
 #define TEGRA_PLL_BYPASS BIT(9)
 #define TEGRA_PLL_HAS_LOCK_ENABLE BIT(10)
+#define TEGRA_MDIV_NEW BIT(11)
+#define TEGRA_PLLMB BIT(12)
+#define TEGRA_PLL_VCO_OUT BIT(13)
 
 /**
  * struct tegra_clk_pll - Tegra PLL clock
@@ -303,6 +362,12 @@ struct clk *tegra_clk_register_pllxc(const char *name, const char *parent_name,
 			    struct tegra_clk_pll_params *pll_params,
 			    spinlock_t *lock);
 
+struct clk *tegra_clk_register_pllxc_tegra210(const char *name,
+			const char *parent_name, void __iomem *clk_base,
+			void __iomem *pmc, unsigned long flags,
+			struct tegra_clk_pll_params *pll_params,
+			spinlock_t *lock);
+
 struct clk *tegra_clk_register_pllm(const char *name, const char *parent_name,
 			   void __iomem *clk_base, void __iomem *pmc,
 			   unsigned long flags,
@@ -327,11 +392,35 @@ struct clk *tegra_clk_register_plle_tegra114(const char *name,
 				struct tegra_clk_pll_params *pll_params,
 				spinlock_t *lock);
 
+struct clk *tegra_clk_register_plle_tegra210(const char *name,
+				const char *parent_name,
+				void __iomem *clk_base, unsigned long flags,
+				struct tegra_clk_pll_params *pll_params,
+				spinlock_t *lock);
+
+struct clk *tegra_clk_register_pllc_tegra210(const char *name,
+				const char *parent_name, void __iomem *clk_base,
+				void __iomem *pmc, unsigned long flags,
+				struct tegra_clk_pll_params *pll_params,
+				spinlock_t *lock);
+
+struct clk *tegra_clk_register_pllss_tegra210(const char *name,
+				const char *parent_name, void __iomem *clk_base,
+				unsigned long flags,
+				struct tegra_clk_pll_params *pll_params,
+				spinlock_t *lock);
+
 struct clk *tegra_clk_register_pllss(const char *name, const char *parent_name,
 			   void __iomem *clk_base, unsigned long flags,
 			   struct tegra_clk_pll_params *pll_params,
 			   spinlock_t *lock);
 
+struct clk *tegra_clk_register_pllmb(const char *name, const char *parent_name,
+			   void __iomem *clk_base, void __iomem *pmc,
+			   unsigned long flags,
+			   struct tegra_clk_pll_params *pll_params,
+			   spinlock_t *lock);
+
 /**
  * struct tegra_clk_pll_out - PLL divider down clock
  *
@@ -653,6 +742,9 @@ int tegra_osc_clk_init(void __iomem *clk_base, struct tegra_clk *clks,
 void tegra_super_clk_gen4_init(void __iomem *clk_base,
 			void __iomem *pmc_base, struct tegra_clk *tegra_clks,
 			struct tegra_clk_pll_params *pll_params);
+void tegra_super_clk_gen5_init(void __iomem *clk_base,
+			void __iomem *pmc_base, struct tegra_clk *tegra_clks,
+			struct tegra_clk_pll_params *pll_params);
 
 #ifdef CONFIG_TEGRA_CLK_EMC
 struct clk *tegra_clk_register_emc(void __iomem *base, struct device_node *np,
@@ -674,5 +766,8 @@ void tegra114_clock_deassert_dfll_dvco_reset(void);
 
 typedef void (*tegra_clk_apply_init_table_func)(void);
 extern tegra_clk_apply_init_table_func tegra_clk_apply_init_table;
+int tegra_pll_wait_for_lock(struct tegra_clk_pll *pll);
+u16 tegra_pll_get_fixed_mdiv(struct clk_hw *hw, unsigned long input_rate);
+int tegra_pll_p_div_to_hw(struct tegra_clk_pll *pll, u8 p_div);
 
 #endif /* TEGRA_CLK_H */
diff --git a/drivers/clk/ti/apll.c b/drivers/clk/ti/apll.c
index f3eab6e79027..b336a8c11e2a 100644
--- a/drivers/clk/ti/apll.c
+++ b/drivers/clk/ti/apll.c
@@ -323,7 +323,7 @@ static void omap2_apll_deny_idle(struct clk_hw_omap *clk)
 	omap2_apll_set_autoidle(clk, OMAP2_APLL_AUTOIDLE_DISABLE);
 }
 
-static struct clk_hw_omap_ops omap2_apll_hwops = {
+static const struct clk_hw_omap_ops omap2_apll_hwops = {
 	.allow_idle	= &omap2_apll_allow_idle,
 	.deny_idle	= &omap2_apll_deny_idle,
 };
diff --git a/drivers/clk/ti/clk-814x.c b/drivers/clk/ti/clk-814x.c
index e172920798ea..9e85fcc74cc9 100644
--- a/drivers/clk/ti/clk-814x.c
+++ b/drivers/clk/ti/clk-814x.c
@@ -14,10 +14,14 @@ static struct ti_dt_clk dm814_clks[] = {
 	DT_CLK(NULL, "devosc_ck", "devosc_ck"),
 	DT_CLK(NULL, "mpu_ck", "mpu_ck"),
 	DT_CLK(NULL, "sysclk4_ck", "sysclk4_ck"),
+	DT_CLK(NULL, "sysclk5_ck", "sysclk5_ck"),
 	DT_CLK(NULL, "sysclk6_ck", "sysclk6_ck"),
+	DT_CLK(NULL, "sysclk8_ck", "sysclk8_ck"),
 	DT_CLK(NULL, "sysclk10_ck", "sysclk10_ck"),
 	DT_CLK(NULL, "sysclk18_ck", "sysclk18_ck"),
 	DT_CLK(NULL, "timer_sys_ck", "devosc_ck"),
+	DT_CLK(NULL, "timer1_fck", "timer1_fck"),
+	DT_CLK(NULL, "timer2_fck", "timer2_fck"),
 	DT_CLK(NULL, "cpsw_125mhz_gclk", "cpsw_125mhz_gclk"),
 	DT_CLK(NULL, "cpsw_cpts_rft_clk", "cpsw_cpts_rft_clk"),
 	{ .node_name = NULL },
diff --git a/drivers/clk/ti/dpll3xxx.c b/drivers/clk/ti/dpll3xxx.c
index f4dec00fb684..cc739291a3ce 100644
--- a/drivers/clk/ti/dpll3xxx.c
+++ b/drivers/clk/ti/dpll3xxx.c
@@ -305,8 +305,9 @@ static void _lookup_sddiv(struct clk_hw_omap *clk, u8 *sd_div, u16 m, u8 n)
 static int omap3_noncore_dpll_program(struct clk_hw_omap *clk, u16 freqsel)
 {
 	struct dpll_data *dd = clk->dpll_data;
-	u8 dco, sd_div;
+	u8 dco, sd_div, ai = 0;
 	u32 v;
+	bool errata_i810;
 
 	/* 3430 ES2 TRM: 4.7.6.9 DPLL Programming Sequence */
 	_omap3_noncore_dpll_bypass(clk);
@@ -350,6 +351,25 @@ static int omap3_noncore_dpll_program(struct clk_hw_omap *clk, u16 freqsel)
 		v |= sd_div << __ffs(dd->sddiv_mask);
 	}
 
+	/*
+	 * Errata i810 - DPLL controller can get stuck while transitioning
+	 * to a power saving state. Software must ensure the DPLL can not
+	 * transition to a low power state while changing M/N values.
+	 * Easiest way to accomplish this is to prevent DPLL autoidle
+	 * before doing the M/N re-program.
+	 */
+	errata_i810 = ti_clk_get_features()->flags & TI_CLK_ERRATA_I810;
+
+	if (errata_i810) {
+		ai = omap3_dpll_autoidle_read(clk);
+		if (ai) {
+			omap3_dpll_deny_idle(clk);
+
+			/* OCP barrier */
+			omap3_dpll_autoidle_read(clk);
+		}
+	}
+
 	ti_clk_ll_ops->clk_writel(v, dd->mult_div1_reg);
 
 	/* Set 4X multiplier and low-power mode */
@@ -379,6 +399,9 @@ static int omap3_noncore_dpll_program(struct clk_hw_omap *clk, u16 freqsel)
 
 	_omap3_noncore_dpll_lock(clk);
 
+	if (errata_i810 && ai)
+		omap3_dpll_allow_idle(clk);
+
 	return 0;
 }
 
@@ -437,7 +460,8 @@ int omap3_noncore_dpll_enable(struct clk_hw *hw)
 
 	parent = clk_hw_get_parent(hw);
 
-	if (clk_hw_get_rate(hw) == clk_get_rate(dd->clk_bypass)) {
+	if (clk_hw_get_rate(hw) ==
+	    clk_hw_get_rate(__clk_get_hw(dd->clk_bypass))) {
 		WARN_ON(parent != __clk_get_hw(dd->clk_bypass));
 		r = _omap3_noncore_dpll_bypass(clk);
 	} else {
diff --git a/drivers/clk/versatile/Kconfig b/drivers/clk/versatile/Kconfig
index fc50b6264bed..a6da2aa09f83 100644
--- a/drivers/clk/versatile/Kconfig
+++ b/drivers/clk/versatile/Kconfig
@@ -1,6 +1,9 @@
 config COMMON_CLK_VERSATILE
 	bool "Clock driver for ARM Reference designs"
-	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || ARCH_VEXPRESS || ARM64 || COMPILE_TEST
+	depends on ARCH_INTEGRATOR || ARCH_REALVIEW || \
+		ARCH_VERSATILE || ARCH_VEXPRESS || ARM64 || \
+		COMPILE_TEST
+	select REGMAP_MMIO
 	---help---
           Supports clocking on ARM Reference designs:
 	  - Integrator/AP and Integrator/CP
diff --git a/drivers/clk/versatile/clk-icst.c b/drivers/clk/versatile/clk-icst.c
index 08c5ee976879..3bca438ecd19 100644
--- a/drivers/clk/versatile/clk-icst.c
+++ b/drivers/clk/versatile/clk-icst.c
@@ -3,7 +3,7 @@
  * We wrap the custom interface from <asm/hardware/icst.h> into the generic
  * clock framework.
  *
- * Copyright (C) 2012 Linus Walleij
+ * Copyright (C) 2012-2015 Linus Walleij
  *
  * This 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,9 +19,14 @@
 #include <linux/err.h>
 #include <linux/clk-provider.h>
 #include <linux/io.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
 
 #include "clk-icst.h"
 
+/* Magic unlocking token used on all Versatile boards */
+#define VERSATILE_LOCK_VAL	0xA05F
+
 /**
  * struct clk_icst - ICST VCO clock wrapper
  * @hw: corresponding clock hardware entry
@@ -32,8 +37,9 @@
  */
 struct clk_icst {
 	struct clk_hw hw;
-	void __iomem *vcoreg;
-	void __iomem *lockreg;
+	struct regmap *map;
+	u32 vcoreg_off;
+	u32 lockreg_off;
 	struct icst_params *params;
 	unsigned long rate;
 };
@@ -41,53 +47,70 @@ struct clk_icst {
 #define to_icst(_hw) container_of(_hw, struct clk_icst, hw)
 
 /**
- * vco_get() - get ICST VCO settings from a certain register
- * @vcoreg: register containing the VCO settings
+ * vco_get() - get ICST VCO settings from a certain ICST
+ * @icst: the ICST clock to get
+ * @vco: the VCO struct to return the value in
  */
-static struct icst_vco vco_get(void __iomem *vcoreg)
+static int vco_get(struct clk_icst *icst, struct icst_vco *vco)
 {
 	u32 val;
-	struct icst_vco vco;
+	int ret;
 
-	val = readl(vcoreg);
-	vco.v = val & 0x1ff;
-	vco.r = (val >> 9) & 0x7f;
-	vco.s = (val >> 16) & 03;
-	return vco;
+	ret = regmap_read(icst->map, icst->vcoreg_off, &val);
+	if (ret)
+		return ret;
+	vco->v = val & 0x1ff;
+	vco->r = (val >> 9) & 0x7f;
+	vco->s = (val >> 16) & 03;
+	return 0;
 }
 
 /**
  * vco_set() - commit changes to an ICST VCO
- * @locreg: register to poke to unlock the VCO for writing
- * @vcoreg: register containing the VCO settings
- * @vco: ICST VCO parameters to commit
+ * @icst: the ICST clock to set
+ * @vco: the VCO struct to set the changes from
  */
-static void vco_set(void __iomem *lockreg,
-			void __iomem *vcoreg,
-			struct icst_vco vco)
+static int vco_set(struct clk_icst *icst, struct icst_vco vco)
 {
 	u32 val;
+	int ret;
 
-	val = readl(vcoreg) & ~0x7ffff;
+	ret = regmap_read(icst->map, icst->vcoreg_off, &val);
+	if (ret)
+		return ret;
+
+	/* Mask the 18 bits used by the VCO */
+	val &= ~0x7ffff;
 	val |= vco.v | (vco.r << 9) | (vco.s << 16);
 
 	/* This magic unlocks the VCO so it can be controlled */
-	writel(0xa05f, lockreg);
-	writel(val, vcoreg);
+	ret = regmap_write(icst->map, icst->lockreg_off, VERSATILE_LOCK_VAL);
+	if (ret)
+		return ret;
+	ret = regmap_write(icst->map, icst->vcoreg_off, val);
+	if (ret)
+		return ret;
 	/* This locks the VCO again */
-	writel(0, lockreg);
+	ret = regmap_write(icst->map, icst->lockreg_off, 0);
+	if (ret)
+		return ret;
+	return 0;
 }
 
-
 static unsigned long icst_recalc_rate(struct clk_hw *hw,
 				      unsigned long parent_rate)
 {
 	struct clk_icst *icst = to_icst(hw);
 	struct icst_vco vco;
+	int ret;
 
 	if (parent_rate)
 		icst->params->ref = parent_rate;
-	vco = vco_get(icst->vcoreg);
+	ret = vco_get(icst, &vco);
+	if (ret) {
+		pr_err("ICST: could not get VCO setting\n");
+		return 0;
+	}
 	icst->rate = icst_hz(icst->params, vco);
 	return icst->rate;
 }
@@ -112,8 +135,7 @@ static int icst_set_rate(struct clk_hw *hw, unsigned long rate,
 		icst->params->ref = parent_rate;
 	vco = icst_hz_to_vco(icst->params, rate);
 	icst->rate = icst_hz(icst->params, vco);
-	vco_set(icst->lockreg, icst->vcoreg, vco);
-	return 0;
+	return vco_set(icst, vco);
 }
 
 static const struct clk_ops icst_ops = {
@@ -122,11 +144,11 @@ static const struct clk_ops icst_ops = {
 	.set_rate = icst_set_rate,
 };
 
-struct clk *icst_clk_register(struct device *dev,
-			const struct clk_icst_desc *desc,
-			const char *name,
-			const char *parent_name,
-			void __iomem *base)
+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 clk *clk;
 	struct clk_icst *icst;
@@ -151,10 +173,11 @@ struct clk *icst_clk_register(struct device *dev,
 	init.flags = CLK_IS_ROOT;
 	init.parent_names = (parent_name ? &parent_name : NULL);
 	init.num_parents = (parent_name ? 1 : 0);
+	icst->map = map;
 	icst->hw.init = &init;
 	icst->params = pclone;
-	icst->vcoreg = base + desc->vco_offset;
-	icst->lockreg = base + desc->lock_offset;
+	icst->vcoreg_off = desc->vco_offset;
+	icst->lockreg_off = desc->lock_offset;
 
 	clk = clk_register(dev, &icst->hw);
 	if (IS_ERR(clk)) {
@@ -164,4 +187,112 @@ struct clk *icst_clk_register(struct device *dev,
 
 	return clk;
 }
+
+struct clk *icst_clk_register(struct device *dev,
+			const struct clk_icst_desc *desc,
+			const char *name,
+			const char *parent_name,
+			void __iomem *base)
+{
+	struct regmap_config icst_regmap_conf = {
+		.reg_bits = 32,
+		.val_bits = 32,
+		.reg_stride = 4,
+	};
+	struct regmap *map;
+
+	map = regmap_init_mmio(dev, base, &icst_regmap_conf);
+	if (IS_ERR(map)) {
+		pr_err("could not initialize ICST regmap\n");
+		return ERR_CAST(map);
+	}
+	return icst_clk_setup(dev, desc, name, parent_name, map);
+}
 EXPORT_SYMBOL_GPL(icst_clk_register);
+
+#ifdef CONFIG_OF
+/*
+ * In a device tree, an memory-mapped ICST clock appear as a child
+ * of a syscon node. Assume this and probe it only as a child of a
+ * syscon.
+ */
+
+static const struct icst_params icst525_params = {
+	.vco_max	= ICST525_VCO_MAX_5V,
+	.vco_min	= ICST525_VCO_MIN,
+	.vd_min		= 8,
+	.vd_max		= 263,
+	.rd_min		= 3,
+	.rd_max		= 65,
+	.s2div		= icst525_s2div,
+	.idx2s		= icst525_idx2s,
+};
+
+static const struct icst_params icst307_params = {
+	.vco_max	= ICST307_VCO_MAX,
+	.vco_min	= ICST307_VCO_MIN,
+	.vd_min		= 4 + 8,
+	.vd_max		= 511 + 8,
+	.rd_min		= 1 + 2,
+	.rd_max		= 127 + 2,
+	.s2div		= icst307_s2div,
+	.idx2s		= icst307_idx2s,
+};
+
+static void __init of_syscon_icst_setup(struct device_node *np)
+{
+	struct device_node *parent;
+	struct regmap *map;
+	struct clk_icst_desc icst_desc;
+	const char *name = np->name;
+	const char *parent_name;
+	struct clk *regclk;
+
+	/* We do not release this reference, we are using it perpetually */
+	parent = of_get_parent(np);
+	if (!parent) {
+		pr_err("no parent node for syscon ICST clock\n");
+		return;
+	}
+	map = syscon_node_to_regmap(parent);
+	if (IS_ERR(map)) {
+		pr_err("no regmap for syscon ICST clock parent\n");
+		return;
+	}
+
+	if (of_property_read_u32(np, "vco-offset", &icst_desc.vco_offset)) {
+		pr_err("no VCO register offset for ICST clock\n");
+		return;
+	}
+	if (of_property_read_u32(np, "lock-offset", &icst_desc.lock_offset)) {
+		pr_err("no lock register offset for ICST clock\n");
+		return;
+	}
+
+	if (of_device_is_compatible(np, "arm,syscon-icst525"))
+		icst_desc.params = &icst525_params;
+	else if (of_device_is_compatible(np, "arm,syscon-icst307"))
+		icst_desc.params = &icst307_params;
+	else {
+		pr_err("unknown ICST clock %s\n", name);
+		return;
+	}
+
+	/* 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);
+	if (IS_ERR(regclk)) {
+		pr_err("error setting up syscon ICST clock %s\n", name);
+		return;
+	}
+	of_clk_add_provider(np, of_clk_src_simple_get, regclk);
+	pr_debug("registered syscon ICST clock %s\n", name);
+}
+
+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);
+
+#endif
diff --git a/drivers/clk/versatile/clk-realview.c b/drivers/clk/versatile/clk-realview.c
index 86f70997d59d..bd4dd2463e23 100644
--- a/drivers/clk/versatile/clk-realview.c
+++ b/drivers/clk/versatile/clk-realview.c
@@ -11,11 +11,15 @@
 #include <linux/io.h>
 #include <linux/clk-provider.h>
 
-#include <mach/hardware.h>
-#include <mach/platform.h>
-
 #include "clk-icst.h"
 
+#define REALVIEW_SYS_OSC0_OFFSET             0x0C
+#define REALVIEW_SYS_OSC1_OFFSET             0x10
+#define REALVIEW_SYS_OSC2_OFFSET             0x14
+#define REALVIEW_SYS_OSC3_OFFSET             0x18
+#define REALVIEW_SYS_OSC4_OFFSET             0x1C	/* OSC1 for RealView/AB */
+#define REALVIEW_SYS_LOCK_OFFSET             0x20
+
 /*
  * Implementation of the ARM RealView clock trees.
  */
diff --git a/drivers/clk/versatile/clk-sp810.c b/drivers/clk/versatile/clk-sp810.c
index a1cdef6b0f90..e78755e0ef78 100644
--- a/drivers/clk/versatile/clk-sp810.c
+++ b/drivers/clk/versatile/clk-sp810.c
@@ -95,13 +95,12 @@ static void __init clk_sp810_of_setup(struct device_node *node)
 	int i;
 	bool deprecated;
 
-	if (!sp810) {
-		pr_err("Failed to allocate memory for SP810!\n");
+	if (!sp810)
 		return;
-	}
 
 	if (of_clk_parent_fill(node, parent_names, num) != num) {
 		pr_warn("Failed to obtain parent clocks for SP810!\n");
+		kfree(sp810);
 		return;
 	}
 
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index 2eb5f0efae90..33db7406c0e2 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -28,10 +28,18 @@ config CLKSRC_MMIO
 	bool
 
 config DIGICOLOR_TIMER
-	bool
+	bool "Digicolor timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	select CLKSRC_MMIO
+	depends on HAS_IOMEM
+	help
+	  Enables the support for the digicolor timer driver.
 
 config DW_APB_TIMER
-	bool
+	bool "DW APB timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	help
+	  Enables the support for the dw_apb timer.
 
 config DW_APB_TIMER_OF
 	bool
@@ -39,47 +47,81 @@ config DW_APB_TIMER_OF
 	select CLKSRC_OF
 
 config ROCKCHIP_TIMER
-	bool
+	bool "Rockchip timer driver" if COMPILE_TEST
+	depends on ARM || ARM64
 	select CLKSRC_OF
+	help
+	  Enables the support for the rockchip timer driver.
 
 config ARMADA_370_XP_TIMER
-	bool
+	bool "Armada 370 and XP timer driver" if COMPILE_TEST
+	depends on ARM
 	select CLKSRC_OF
+	select CLKSRC_MMIO
+	help
+	  Enables the support for the Armada 370 and XP timer driver.
 
 config MESON6_TIMER
-	bool
+	bool "Meson6 timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
 	select CLKSRC_MMIO
+	help
+	  Enables the support for the Meson6 timer driver.
 
 config ORION_TIMER
+	bool "Orion timer driver" if COMPILE_TEST
+	depends on ARM
 	select CLKSRC_OF
 	select CLKSRC_MMIO
-	bool
+	help
+	  Enables the support for the Orion timer driver
 
 config SUN4I_TIMER
+	bool "Sun4i timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	select CLKSRC_MMIO
-	bool
+	help
+	  Enables support for the Sun4i timer.
 
 config SUN5I_HSTIMER
+	bool "Sun5i timer driver" if COMPILE_TEST
 	select CLKSRC_MMIO
-	bool
+	depends on COMMON_CLK
+	help
+	  Enables support the Sun5i timer.
 
 config TEGRA_TIMER
-	bool
+	bool "Tegra timer driver" if COMPILE_TEST
+	select CLKSRC_MMIO
+	depends on ARM
+	help
+	  Enables support for the Tegra driver.
 
 config VT8500_TIMER
-	bool
+	bool "VT8500 timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
+	help
+	  Enables support for the VT8500 driver.
 
 config CADENCE_TTC_TIMER
-	bool
+	bool "Cadence TTC timer driver" if COMPILE_TEST
+	depends on COMMON_CLK
+	help
+	  Enables support for the cadence ttc driver.
 
 config ASM9260_TIMER
-	bool
+	bool "ASM9260 timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
 	select CLKSRC_MMIO
 	select CLKSRC_OF
+	help
+	  Enables support for the ASM9260 timer.
 
 config CLKSRC_NOMADIK_MTU
-	bool
-	depends on (ARCH_NOMADIK || ARCH_U8500)
+	bool "Nomakdik clocksource driver" if COMPILE_TEST
+	depends on ARM
 	select CLKSRC_MMIO
 	help
 	  Support for Multi Timer Unit. MTU provides access
@@ -93,9 +135,9 @@ config CLKSRC_NOMADIK_MTU_SCHED_CLOCK
 	  Use the Multi Timer Unit as the sched_clock.
 
 config CLKSRC_DBX500_PRCMU
-	bool "Clocksource PRCMU Timer"
-	depends on UX500_SOC_DB8500
-	default y
+	bool "Clocksource PRCMU Timer" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	help
 	  Use the always on PRCMU Timer as clocksource
 
@@ -116,13 +158,19 @@ config CLKSRC_EFM32
 	  event device.
 
 config CLKSRC_LPC32XX
-	bool
+	bool "Clocksource for LPC32XX" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
 	select CLKSRC_MMIO
 	select CLKSRC_OF
+	help
+	  Support for the LPC32XX clocksource.
 
 config CLKSRC_PISTACHIO
-	bool
+	bool "Clocksource for Pistachio SoC" if COMPILE_TEST
+	depends on HAS_IOMEM
 	select CLKSRC_OF
+	help
+	  Enables the clocksource for the Pistachio SoC.
 
 config CLKSRC_TI_32K
 	bool "Texas Instruments 32.768 Hz Clocksource" if COMPILE_TEST
@@ -199,13 +247,15 @@ config CLKSRC_METAG_GENERIC
 	  This option enables support for the Meta per-thread timers.
 
 config CLKSRC_EXYNOS_MCT
-	def_bool y if ARCH_EXYNOS
-	depends on !ARM64
+	bool "Exynos multi core timer driver" if COMPILE_TEST
+	depends on ARM
 	help
 	  Support for Multi Core Timer controller on Exynos SoCs.
 
 config CLKSRC_SAMSUNG_PWM
-	bool
+	bool "PWM timer drvier for Samsung S3C, S5P" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
 	help
 	  This is a new clocksource driver for the PWM timer found in
 	  Samsung S3C, S5P and Exynos SoCs, replacing an earlier driver
@@ -213,12 +263,16 @@ config CLKSRC_SAMSUNG_PWM
 	  needed only on systems that do not have the Exynos MCT available.
 
 config FSL_FTM_TIMER
-	bool
+	bool "Freescale FlexTimer Module driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
+	select CLKSRC_MMIO
 	help
 	  Support for Freescale FlexTimer Module (FTM) timer.
 
 config VF_PIT_TIMER
 	bool
+	select CLKSRC_MMIO
 	help
 	  Support for Period Interrupt Timer on Freescale Vybrid Family SoCs.
 
@@ -226,9 +280,12 @@ config SYS_SUPPORTS_SH_CMT
         bool
 
 config MTK_TIMER
+	bool "Mediatek timer driver" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
 	select CLKSRC_OF
 	select CLKSRC_MMIO
-	bool
+	help
+	  Support for Mediatek timer driver.
 
 config SYS_SUPPORTS_SH_MTU2
         bool
@@ -279,7 +336,12 @@ config EM_TIMER_STI
 	  such as EMEV2 from former NEC Electronics.
 
 config CLKSRC_QCOM
-	bool
+	bool "Qualcomm MSM timer" if COMPILE_TEST
+	depends on ARM
+	select CLKSRC_OF
+	help
+	  This enables the clocksource and the per CPU clockevent driver for the
+	  Qualcomm SoCs.
 
 config CLKSRC_VERSATILE
 	bool "ARM Versatile (Express) reference platforms clock source"
@@ -298,21 +360,41 @@ config CLKSRC_MIPS_GIC
 	select CLKSRC_OF
 
 config CLKSRC_TANGO_XTAL
-	bool
+	bool "Clocksource for Tango SoC" if COMPILE_TEST
+	depends on ARM
 	select CLKSRC_OF
+	select CLKSRC_MMIO
+	help
+	  This enables the clocksource for Tango SoC
 
 config CLKSRC_PXA
-	def_bool y if ARCH_PXA || ARCH_SA1100
-	select CLKSRC_OF if OF
+	bool "Clocksource for PXA or SA-11x0 platform" if COMPILE_TEST
+	depends on GENERIC_CLOCKEVENTS
+	depends on HAS_IOMEM
+	select CLKSRC_MMIO
 	help
 	  This enables OST0 support available on PXA and SA-11x0
 	  platforms.
 
+config H8300_TMR8
+        bool "Clockevent timer for the H8300 platform" if COMPILE_TEST
+        depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
+	help
+	  This enables the 8 bits timer for the H8300 platform.
+
 config H8300_TMR16
-        bool
+        bool "Clockevent timer for the H83069 platform" if COMPILE_TEST
+        depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
+	help
+	  This enables the 16 bits timer for the H8300 platform with the
+	  H83069 cpu.
 
 config H8300_TPU
-        bool
+        bool "Clocksource for the H8300 platform" if COMPILE_TEST
+        depends on GENERIC_CLOCKEVENTS && HAS_IOMEM
+	help
+	  This enables the clocksource for the H8300 platform with the
+	  H8S2678 cpu.
 
 config CLKSRC_IMX_GPT
 	bool "Clocksource using i.MX GPT" if COMPILE_TEST
@@ -320,9 +402,10 @@ config CLKSRC_IMX_GPT
 	select CLKSRC_MMIO
 
 config CLKSRC_ST_LPC
-	bool
-	depends on ARCH_STI
+	bool "Low power clocksource found in the LPC" if COMPILE_TEST
 	select CLKSRC_OF if OF
+	depends on HAS_IOMEM
+	select CLKSRC_MMIO
 	help
 	  Enable this option to use the Low Power controller timer
 	  as clocksource.
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 56bd16e77ae3..dc2b8997f6e6 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -60,7 +60,7 @@ obj-$(CONFIG_CLKSRC_MIPS_GIC)		+= mips-gic-timer.o
 obj-$(CONFIG_CLKSRC_TANGO_XTAL)		+= tango_xtal.o
 obj-$(CONFIG_CLKSRC_IMX_GPT)		+= timer-imx-gpt.o
 obj-$(CONFIG_ASM9260_TIMER)		+= asm9260_timer.o
-obj-$(CONFIG_H8300)			+= h8300_timer8.o
+obj-$(CONFIG_H8300_TMR8)		+= h8300_timer8.o
 obj-$(CONFIG_H8300_TMR16)		+= h8300_timer16.o
 obj-$(CONFIG_H8300_TPU)			+= h8300_tpu.o
 obj-$(CONFIG_CLKSRC_ST_LPC)		+= clksrc_st_lpc.o
diff --git a/drivers/clocksource/acpi_pm.c b/drivers/clocksource/acpi_pm.c
index 6eab88985670..28037d0b8dcd 100644
--- a/drivers/clocksource/acpi_pm.c
+++ b/drivers/clocksource/acpi_pm.c
@@ -109,10 +109,8 @@ static void acpi_pm_check_blacklist(struct pci_dev *dev)
 
 	/* the bug has been fixed in PIIX4M */
 	if (dev->revision < 3) {
-		printk(KERN_WARNING "* Found PM-Timer Bug on the chipset."
-		       " Due to workarounds for a bug,\n"
-		       "* this clock source is slow. Consider trying"
-		       " other clock sources\n");
+		pr_warn("* Found PM-Timer Bug on the chipset. Due to workarounds for a bug,\n"
+			"* this clock source is slow. Consider trying other clock sources\n");
 
 		acpi_pm_need_workaround();
 	}
@@ -125,12 +123,9 @@ static void acpi_pm_check_graylist(struct pci_dev *dev)
 	if (acpi_pm_good)
 		return;
 
-	printk(KERN_WARNING "* The chipset may have PM-Timer Bug. Due to"
-	       " workarounds for a bug,\n"
-	       "* this clock source is slow. If you are sure your timer"
-	       " does not have\n"
-	       "* this bug, please use \"acpi_pm_good\" to disable the"
-	       " workaround\n");
+	pr_warn("* The chipset may have PM-Timer Bug. Due to workarounds for a bug,\n"
+		"* this clock source is slow. If you are sure your timer does not have\n"
+		"* this bug, please use \"acpi_pm_good\" to disable the workaround\n");
 
 	acpi_pm_need_workaround();
 }
@@ -162,8 +157,7 @@ static int verify_pmtmr_rate(void)
 	/* Check that the PMTMR delta is within 5% of what we expect */
 	if (delta < (PMTMR_EXPECTED_RATE * 19) / 20 ||
 	    delta > (PMTMR_EXPECTED_RATE * 21) / 20) {
-		printk(KERN_INFO "PM-Timer running at invalid rate: %lu%% "
-			"of normal - aborting.\n",
+		pr_info("PM-Timer running at invalid rate: %lu%% of normal - aborting.\n",
 			100UL * delta / PMTMR_EXPECTED_RATE);
 		return -1;
 	}
@@ -199,15 +193,14 @@ static int __init init_acpi_pm_clocksource(void)
 				break;
 			if ((value2 < value1) && ((value2) < 0xFFF))
 				break;
-			printk(KERN_INFO "PM-Timer had inconsistent results:"
-			       " %#llx, %#llx - aborting.\n",
-			       value1, value2);
+			pr_info("PM-Timer had inconsistent results: %#llx, %#llx - aborting.\n",
+				value1, value2);
 			pmtmr_ioport = 0;
 			return -EINVAL;
 		}
 		if (i == ACPI_PM_READ_CHECKS) {
-			printk(KERN_INFO "PM-Timer failed consistency check "
-			       " (%#llx) - aborting.\n", value1);
+			pr_info("PM-Timer failed consistency check  (%#llx) - aborting.\n",
+				value1);
 			pmtmr_ioport = 0;
 			return -ENODEV;
 		}
diff --git a/drivers/clocksource/arm_global_timer.c b/drivers/clocksource/arm_global_timer.c
index a2cb6fae9295..d189d8cb69f7 100644
--- a/drivers/clocksource/arm_global_timer.c
+++ b/drivers/clocksource/arm_global_timer.c
@@ -99,17 +99,17 @@ static void gt_compare_set(unsigned long delta, int periodic)
 
 	counter += delta;
 	ctrl = GT_CONTROL_TIMER_ENABLE;
-	writel(ctrl, gt_base + GT_CONTROL);
-	writel(lower_32_bits(counter), gt_base + GT_COMP0);
-	writel(upper_32_bits(counter), gt_base + GT_COMP1);
+	writel_relaxed(ctrl, gt_base + GT_CONTROL);
+	writel_relaxed(lower_32_bits(counter), gt_base + GT_COMP0);
+	writel_relaxed(upper_32_bits(counter), gt_base + GT_COMP1);
 
 	if (periodic) {
-		writel(delta, gt_base + GT_AUTO_INC);
+		writel_relaxed(delta, gt_base + GT_AUTO_INC);
 		ctrl |= GT_CONTROL_AUTO_INC;
 	}
 
 	ctrl |= GT_CONTROL_COMP_ENABLE | GT_CONTROL_IRQ_ENABLE;
-	writel(ctrl, gt_base + GT_CONTROL);
+	writel_relaxed(ctrl, gt_base + GT_CONTROL);
 }
 
 static int gt_clockevent_shutdown(struct clock_event_device *evt)
@@ -195,12 +195,23 @@ static cycle_t gt_clocksource_read(struct clocksource *cs)
 	return gt_counter_read();
 }
 
+static void gt_resume(struct clocksource *cs)
+{
+	unsigned long ctrl;
+
+	ctrl = readl(gt_base + GT_CONTROL);
+	if (!(ctrl & GT_CONTROL_TIMER_ENABLE))
+		/* re-enable timer on resume */
+		writel(GT_CONTROL_TIMER_ENABLE, gt_base + GT_CONTROL);
+}
+
 static struct clocksource gt_clocksource = {
 	.name	= "arm_global_timer",
 	.rating	= 300,
 	.read	= gt_clocksource_read,
 	.mask	= CLOCKSOURCE_MASK(64),
 	.flags	= CLOCK_SOURCE_IS_CONTINUOUS,
+	.resume = gt_resume,
 };
 
 #ifdef CONFIG_CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
diff --git a/drivers/clocksource/clksrc-dbx500-prcmu.c b/drivers/clocksource/clksrc-dbx500-prcmu.c
index b375106844d8..dfad6eb99662 100644
--- a/drivers/clocksource/clksrc-dbx500-prcmu.c
+++ b/drivers/clocksource/clksrc-dbx500-prcmu.c
@@ -12,8 +12,9 @@
  * power domain.  We use the Timer 4 for our always-on clock
  * source on DB8500.
  */
+#include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/clockchips.h>
-#include <linux/clksrc-dbx500-prcmu.h>
 #include <linux/sched_clock.h>
 
 #define RATE_32K		32768
@@ -63,9 +64,9 @@ static u64 notrace dbx500_prcmu_sched_clock_read(void)
 
 #endif
 
-void __init clksrc_dbx500_prcmu_init(void __iomem *base)
+static void __init clksrc_dbx500_prcmu_init(struct device_node *node)
 {
-	clksrc_dbx500_timer_base = base;
+	clksrc_dbx500_timer_base = of_iomap(node, 0);
 
 	/*
 	 * The A9 sub system expects the timer to be configured as
@@ -85,3 +86,5 @@ void __init clksrc_dbx500_prcmu_init(void __iomem *base)
 #endif
 	clocksource_register_hz(&clocksource_dbx500_prcmu, RATE_32K);
 }
+CLOCKSOURCE_OF_DECLARE(dbx500_prcmu, "stericsson,db8500-prcmu-timer-4",
+		       clksrc_dbx500_prcmu_init);
diff --git a/drivers/clocksource/dw_apb_timer.c b/drivers/clocksource/dw_apb_timer.c
index c76c75006ea6..63345260244d 100644
--- a/drivers/clocksource/dw_apb_timer.c
+++ b/drivers/clocksource/dw_apb_timer.c
@@ -49,20 +49,31 @@ clocksource_to_dw_apb_clocksource(struct clocksource *cs)
 	return container_of(cs, struct dw_apb_clocksource, cs);
 }
 
-static unsigned long apbt_readl(struct dw_apb_timer *timer, unsigned long offs)
+static inline u32 apbt_readl(struct dw_apb_timer *timer, unsigned long offs)
 {
 	return readl(timer->base + offs);
 }
 
-static void apbt_writel(struct dw_apb_timer *timer, unsigned long val,
-		 unsigned long offs)
+static inline void apbt_writel(struct dw_apb_timer *timer, u32 val,
+			unsigned long offs)
 {
 	writel(val, timer->base + offs);
 }
 
+static inline u32 apbt_readl_relaxed(struct dw_apb_timer *timer, unsigned long offs)
+{
+	return readl_relaxed(timer->base + offs);
+}
+
+static inline void apbt_writel_relaxed(struct dw_apb_timer *timer, u32 val,
+			unsigned long offs)
+{
+	writel_relaxed(val, timer->base + offs);
+}
+
 static void apbt_disable_int(struct dw_apb_timer *timer)
 {
-	unsigned long ctrl = apbt_readl(timer, APBTMR_N_CONTROL);
+	u32 ctrl = apbt_readl(timer, APBTMR_N_CONTROL);
 
 	ctrl |= APBTMR_CONTROL_INT;
 	apbt_writel(timer, ctrl, APBTMR_N_CONTROL);
@@ -81,7 +92,7 @@ void dw_apb_clockevent_pause(struct dw_apb_clock_event_device *dw_ced)
 
 static void apbt_eoi(struct dw_apb_timer *timer)
 {
-	apbt_readl(timer, APBTMR_N_EOI);
+	apbt_readl_relaxed(timer, APBTMR_N_EOI);
 }
 
 static irqreturn_t dw_apb_clockevent_irq(int irq, void *data)
@@ -103,7 +114,7 @@ static irqreturn_t dw_apb_clockevent_irq(int irq, void *data)
 
 static void apbt_enable_int(struct dw_apb_timer *timer)
 {
-	unsigned long ctrl = apbt_readl(timer, APBTMR_N_CONTROL);
+	u32 ctrl = apbt_readl(timer, APBTMR_N_CONTROL);
 	/* clear pending intr */
 	apbt_readl(timer, APBTMR_N_EOI);
 	ctrl &= ~APBTMR_CONTROL_INT;
@@ -113,7 +124,7 @@ static void apbt_enable_int(struct dw_apb_timer *timer)
 static int apbt_shutdown(struct clock_event_device *evt)
 {
 	struct dw_apb_clock_event_device *dw_ced = ced_to_dw_apb_ced(evt);
-	unsigned long ctrl;
+	u32 ctrl;
 
 	pr_debug("%s CPU %d state=shutdown\n", __func__,
 		 cpumask_first(evt->cpumask));
@@ -127,7 +138,7 @@ static int apbt_shutdown(struct clock_event_device *evt)
 static int apbt_set_oneshot(struct clock_event_device *evt)
 {
 	struct dw_apb_clock_event_device *dw_ced = ced_to_dw_apb_ced(evt);
-	unsigned long ctrl;
+	u32 ctrl;
 
 	pr_debug("%s CPU %d state=oneshot\n", __func__,
 		 cpumask_first(evt->cpumask));
@@ -160,7 +171,7 @@ static int apbt_set_periodic(struct clock_event_device *evt)
 {
 	struct dw_apb_clock_event_device *dw_ced = ced_to_dw_apb_ced(evt);
 	unsigned long period = DIV_ROUND_UP(dw_ced->timer.freq, HZ);
-	unsigned long ctrl;
+	u32 ctrl;
 
 	pr_debug("%s CPU %d state=periodic\n", __func__,
 		 cpumask_first(evt->cpumask));
@@ -196,17 +207,17 @@ static int apbt_resume(struct clock_event_device *evt)
 static int apbt_next_event(unsigned long delta,
 			   struct clock_event_device *evt)
 {
-	unsigned long ctrl;
+	u32 ctrl;
 	struct dw_apb_clock_event_device *dw_ced = ced_to_dw_apb_ced(evt);
 
 	/* Disable timer */
-	ctrl = apbt_readl(&dw_ced->timer, APBTMR_N_CONTROL);
+	ctrl = apbt_readl_relaxed(&dw_ced->timer, APBTMR_N_CONTROL);
 	ctrl &= ~APBTMR_CONTROL_ENABLE;
-	apbt_writel(&dw_ced->timer, ctrl, APBTMR_N_CONTROL);
+	apbt_writel_relaxed(&dw_ced->timer, ctrl, APBTMR_N_CONTROL);
 	/* write new count */
-	apbt_writel(&dw_ced->timer, delta, APBTMR_N_LOAD_COUNT);
+	apbt_writel_relaxed(&dw_ced->timer, delta, APBTMR_N_LOAD_COUNT);
 	ctrl |= APBTMR_CONTROL_ENABLE;
-	apbt_writel(&dw_ced->timer, ctrl, APBTMR_N_CONTROL);
+	apbt_writel_relaxed(&dw_ced->timer, ctrl, APBTMR_N_CONTROL);
 
 	return 0;
 }
@@ -323,7 +334,7 @@ void dw_apb_clocksource_start(struct dw_apb_clocksource *dw_cs)
 	 * start count down from 0xffff_ffff. this is done by toggling the
 	 * enable bit then load initial load count to ~0.
 	 */
-	unsigned long ctrl = apbt_readl(&dw_cs->timer, APBTMR_N_CONTROL);
+	u32 ctrl = apbt_readl(&dw_cs->timer, APBTMR_N_CONTROL);
 
 	ctrl &= ~APBTMR_CONTROL_ENABLE;
 	apbt_writel(&dw_cs->timer, ctrl, APBTMR_N_CONTROL);
@@ -338,11 +349,12 @@ void dw_apb_clocksource_start(struct dw_apb_clocksource *dw_cs)
 
 static cycle_t __apbt_read_clocksource(struct clocksource *cs)
 {
-	unsigned long current_count;
+	u32 current_count;
 	struct dw_apb_clocksource *dw_cs =
 		clocksource_to_dw_apb_clocksource(cs);
 
-	current_count = apbt_readl(&dw_cs->timer, APBTMR_N_CURRENT_VALUE);
+	current_count = apbt_readl_relaxed(&dw_cs->timer,
+					APBTMR_N_CURRENT_VALUE);
 
 	return (cycle_t)~current_count;
 }
diff --git a/drivers/clocksource/dw_apb_timer_of.c b/drivers/clocksource/dw_apb_timer_of.c
index a19a3f619cc7..860843cef572 100644
--- a/drivers/clocksource/dw_apb_timer_of.c
+++ b/drivers/clocksource/dw_apb_timer_of.c
@@ -16,6 +16,7 @@
  * 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/delay.h>
 #include <linux/dw_apb_timer.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -130,6 +131,17 @@ static void __init init_sched_clock(void)
 	sched_clock_register(read_sched_clock, 32, sched_rate);
 }
 
+#ifdef CONFIG_ARM
+static unsigned long dw_apb_delay_timer_read(void)
+{
+	return ~readl_relaxed(sched_io_base);
+}
+
+static struct delay_timer dw_apb_delay_timer = {
+	.read_current_timer	= dw_apb_delay_timer_read,
+};
+#endif
+
 static int num_called;
 static void __init dw_apb_timer_init(struct device_node *timer)
 {
@@ -142,6 +154,10 @@ static void __init dw_apb_timer_init(struct device_node *timer)
 		pr_debug("%s: found clocksource timer\n", __func__);
 		add_clocksource(timer);
 		init_sched_clock();
+#ifdef CONFIG_ARM
+		dw_apb_delay_timer.freq = sched_rate;
+		register_current_timer_delay(&dw_apb_delay_timer);
+#endif
 		break;
 	default:
 		break;
diff --git a/drivers/clocksource/h8300_timer16.c b/drivers/clocksource/h8300_timer16.c
index 0e076c6fc006..75c44079b345 100644
--- a/drivers/clocksource/h8300_timer16.c
+++ b/drivers/clocksource/h8300_timer16.c
@@ -4,85 +4,56 @@
  *  Copyright 2015 Yoshinori Sato <ysato@users.sourcefoge.jp>
  */
 
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/param.h>
-#include <linux/string.h>
-#include <linux/slab.h>
 #include <linux/interrupt.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
 #include <linux/clocksource.h>
-#include <linux/module.h>
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/of.h>
-
-#include <asm/segment.h>
-#include <asm/irq.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 
 #define TSTR	0
-#define TSNC	1
-#define TMDR	2
-#define TOLR	3
-#define TISRA	4
-#define TISRB	5
 #define TISRC	6
 
 #define TCR	0
-#define TIOR	1
 #define TCNT	2
-#define GRA	4
-#define GRB	6
-
-#define FLAG_REPROGRAM (1 << 0)
-#define FLAG_SKIPEVENT (1 << 1)
-#define FLAG_IRQCONTEXT (1 << 2)
-#define FLAG_STARTED (1 << 3)
 
-#define ONESHOT  0
-#define PERIODIC 1
-
-#define RELATIVE 0
-#define ABSOLUTE 1
+#define bset(b, a) iowrite8(ioread8(a) | (1 << (b)), (a))
+#define bclr(b, a) iowrite8(ioread8(a) & ~(1 << (b)), (a))
 
 struct timer16_priv {
-	struct platform_device *pdev;
 	struct clocksource cs;
-	struct irqaction irqaction;
 	unsigned long total_cycles;
-	unsigned long mapbase;
-	unsigned long mapcommon;
-	unsigned long flags;
-	unsigned short gra;
+	void __iomem *mapbase;
+	void __iomem *mapcommon;
 	unsigned short cs_enabled;
 	unsigned char enb;
-	unsigned char imfa;
-	unsigned char imiea;
 	unsigned char ovf;
-	raw_spinlock_t lock;
-	struct clk *clk;
+	unsigned char ovie;
 };
 
 static unsigned long timer16_get_counter(struct timer16_priv *p)
 {
-	unsigned long v1, v2, v3;
-	int o1, o2;
+	unsigned short v1, v2, v3;
+	unsigned char  o1, o2;
 
-	o1 = ctrl_inb(p->mapcommon + TISRC) & p->ovf;
+	o1 = ioread8(p->mapcommon + TISRC) & p->ovf;
 
 	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
 	do {
 		o2 = o1;
-		v1 = ctrl_inw(p->mapbase + TCNT);
-		v2 = ctrl_inw(p->mapbase + TCNT);
-		v3 = ctrl_inw(p->mapbase + TCNT);
-		o1 = ctrl_inb(p->mapcommon + TISRC) & p->ovf;
+		v1 = ioread16be(p->mapbase + TCNT);
+		v2 = ioread16be(p->mapbase + TCNT);
+		v3 = ioread16be(p->mapbase + TCNT);
+		o1 = ioread8(p->mapcommon + TISRC) & p->ovf;
 	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
 			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
-	v2 |= 0x10000;
-	return v2;
+	if (likely(!o1))
+		return v2;
+	else
+		return v2 + 0x10000;
 }
 
 
@@ -90,8 +61,7 @@ static irqreturn_t timer16_interrupt(int irq, void *dev_id)
 {
 	struct timer16_priv *p = (struct timer16_priv *)dev_id;
 
-	ctrl_outb(ctrl_inb(p->mapcommon + TISRA) & ~p->imfa,
-		  p->mapcommon + TISRA);
+	bclr(p->ovf, p->mapcommon + TISRC);
 	p->total_cycles += 0x10000;
 
 	return IRQ_HANDLED;
@@ -105,13 +75,10 @@ static inline struct timer16_priv *cs_to_priv(struct clocksource *cs)
 static cycle_t timer16_clocksource_read(struct clocksource *cs)
 {
 	struct timer16_priv *p = cs_to_priv(cs);
-	unsigned long flags, raw;
-	unsigned long value;
+	unsigned long raw, value;
 
-	raw_spin_lock_irqsave(&p->lock, flags);
 	value = p->total_cycles;
 	raw = timer16_get_counter(p);
-	raw_spin_unlock_irqrestore(&p->lock, flags);
 
 	return value + raw;
 }
@@ -123,10 +90,10 @@ static int timer16_enable(struct clocksource *cs)
 	WARN_ON(p->cs_enabled);
 
 	p->total_cycles = 0;
-	ctrl_outw(0x0000, p->mapbase + TCNT);
-	ctrl_outb(0x83, p->mapbase + TCR);
-	ctrl_outb(ctrl_inb(p->mapcommon + TSTR) | p->enb,
-		  p->mapcommon + TSTR);
+	iowrite16be(0x0000, p->mapbase + TCNT);
+	iowrite8(0x83, p->mapbase + TCR);
+	bset(p->ovie, p->mapcommon + TISRC);
+	bset(p->enb, p->mapcommon + TSTR);
 
 	p->cs_enabled = true;
 	return 0;
@@ -138,116 +105,83 @@ static void timer16_disable(struct clocksource *cs)
 
 	WARN_ON(!p->cs_enabled);
 
-	ctrl_outb(ctrl_inb(p->mapcommon + TSTR) & ~p->enb,
-		  p->mapcommon + TSTR);
+	bclr(p->ovie, p->mapcommon + TISRC);
+	bclr(p->enb, p->mapcommon + TSTR);
 
 	p->cs_enabled = false;
 }
 
+static struct timer16_priv timer16_priv = {
+	.cs = {
+		.name = "h8300_16timer",
+		.rating = 200,
+		.read = timer16_clocksource_read,
+		.enable = timer16_enable,
+		.disable = timer16_disable,
+		.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8),
+		.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+	},
+};
+
 #define REG_CH   0
 #define REG_COMM 1
 
-static int timer16_setup(struct timer16_priv *p, struct platform_device *pdev)
+static void __init h8300_16timer_init(struct device_node *node)
 {
-	struct resource *res[2];
+	void __iomem *base[2];
 	int ret, irq;
 	unsigned int ch;
+	struct clk *clk;
 
-	p->pdev = pdev;
-
-	res[REG_CH] = platform_get_resource(p->pdev,
-					    IORESOURCE_MEM, REG_CH);
-	res[REG_COMM] = platform_get_resource(p->pdev,
-					      IORESOURCE_MEM, REG_COMM);
-	if (!res[REG_CH] || !res[REG_COMM]) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		return -ENXIO;
-	}
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		return irq;
+	clk = of_clk_get(node, 0);
+	if (IS_ERR(clk)) {
+		pr_err("failed to get clock for clocksource\n");
+		return;
 	}
 
-	p->clk = clk_get(&p->pdev->dev, "fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "can't get clk\n");
-		return PTR_ERR(p->clk);
+	base[REG_CH] = of_iomap(node, 0);
+	if (!base[REG_CH]) {
+		pr_err("failed to map registers for clocksource\n");
+		goto free_clk;
 	}
-	of_property_read_u32(p->pdev->dev.of_node, "renesas,channel", &ch);
-
-	p->pdev = pdev;
-	p->mapbase = res[REG_CH]->start;
-	p->mapcommon = res[REG_COMM]->start;
-	p->enb = 1 << ch;
-	p->imfa = 1 << ch;
-	p->imiea = 1 << (4 + ch);
-	p->cs.name = pdev->name;
-	p->cs.rating = 200;
-	p->cs.read = timer16_clocksource_read;
-	p->cs.enable = timer16_enable;
-	p->cs.disable = timer16_disable;
-	p->cs.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
-	p->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
-	ret = request_irq(irq, timer16_interrupt,
-			  IRQF_TIMER, pdev->name, p);
-	if (ret < 0) {
-		dev_err(&p->pdev->dev, "failed to request irq %d\n", irq);
-		return ret;
+	base[REG_COMM] = of_iomap(node, 1);
+	if (!base[REG_COMM]) {
+		pr_err("failed to map registers for clocksource\n");
+		goto unmap_ch;
 	}
 
-	clocksource_register_hz(&p->cs, clk_get_rate(p->clk) / 8);
-
-	return 0;
-}
-
-static int timer16_probe(struct platform_device *pdev)
-{
-	struct timer16_priv *p = platform_get_drvdata(pdev);
-
-	if (p) {
-		dev_info(&pdev->dev, "kept as earlytimer\n");
-		return 0;
+	irq = irq_of_parse_and_map(node, 0);
+	if (!irq) {
+		pr_err("failed to get irq for clockevent\n");
+		goto unmap_comm;
 	}
 
-	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
-	if (!p)
-		return -ENOMEM;
+	of_property_read_u32(node, "renesas,channel", &ch);
 
-	return timer16_setup(p, pdev);
-}
-
-static int timer16_remove(struct platform_device *pdev)
-{
-	return -EBUSY;
-}
+	timer16_priv.mapbase = base[REG_CH];
+	timer16_priv.mapcommon = base[REG_COMM];
+	timer16_priv.enb = ch;
+	timer16_priv.ovf = ch;
+	timer16_priv.ovie = 4 + ch;
 
-static const struct of_device_id timer16_of_table[] = {
-	{ .compatible = "renesas,16bit-timer" },
-	{ }
-};
-static struct platform_driver timer16_driver = {
-	.probe		= timer16_probe,
-	.remove		= timer16_remove,
-	.driver		= {
-		.name	= "h8300h-16timer",
-		.of_match_table = of_match_ptr(timer16_of_table),
+	ret = request_irq(irq, timer16_interrupt,
+			  IRQF_TIMER, timer16_priv.cs.name, &timer16_priv);
+	if (ret < 0) {
+		pr_err("failed to request irq %d of clocksource\n", irq);
+		goto unmap_comm;
 	}
-};
 
-static int __init timer16_init(void)
-{
-	return platform_driver_register(&timer16_driver);
-}
+	clocksource_register_hz(&timer16_priv.cs,
+				clk_get_rate(clk) / 8);
+	return;
 
-static void __exit timer16_exit(void)
-{
-	platform_driver_unregister(&timer16_driver);
+unmap_comm:
+	iounmap(base[REG_COMM]);
+unmap_ch:
+	iounmap(base[REG_CH]);
+free_clk:
+	clk_put(clk);
 }
 
-subsys_initcall(timer16_init);
-module_exit(timer16_exit);
-MODULE_AUTHOR("Yoshinori Sato");
-MODULE_DESCRIPTION("H8/300H 16bit Timer Driver");
-MODULE_LICENSE("GPL v2");
+CLOCKSOURCE_OF_DECLARE(h8300_16bit, "renesas,16bit-timer", h8300_16timer_init);
diff --git a/drivers/clocksource/h8300_timer8.c b/drivers/clocksource/h8300_timer8.c
index 44375d8b9bc4..c151941e1956 100644
--- a/drivers/clocksource/h8300_timer8.c
+++ b/drivers/clocksource/h8300_timer8.c
@@ -8,19 +8,15 @@
  */
 
 #include <linux/errno.h>
-#include <linux/sched.h>
 #include <linux/kernel.h>
 #include <linux/interrupt.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
 #include <linux/clockchips.h>
-#include <linux/module.h>
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/of.h>
-
-#include <asm/irq.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 
 #define _8TCR	0
 #define _8TCSR	2
@@ -28,126 +24,74 @@
 #define TCORB	6
 #define _8TCNT	8
 
-#define FLAG_REPROGRAM (1 << 0)
-#define FLAG_SKIPEVENT (1 << 1)
-#define FLAG_IRQCONTEXT (1 << 2)
+#define CMIEA	6
+#define CMFA	6
+
 #define FLAG_STARTED (1 << 3)
 
-#define ONESHOT  0
-#define PERIODIC 1
+#define SCALE 64
 
-#define RELATIVE 0
-#define ABSOLUTE 1
+#define bset(b, a) iowrite8(ioread8(a) | (1 << (b)), (a))
+#define bclr(b, a) iowrite8(ioread8(a) & ~(1 << (b)), (a))
 
 struct timer8_priv {
-	struct platform_device *pdev;
 	struct clock_event_device ced;
-	struct irqaction irqaction;
-	unsigned long mapbase;
-	raw_spinlock_t lock;
+	void __iomem *mapbase;
 	unsigned long flags;
 	unsigned int rate;
-	unsigned int tcora;
-	struct clk *pclk;
 };
 
-static unsigned long timer8_get_counter(struct timer8_priv *p)
-{
-	unsigned long v1, v2, v3;
-	int o1, o2;
-
-	o1 = ctrl_inb(p->mapbase + _8TCSR) & 0x20;
-
-	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
-	do {
-		o2 = o1;
-		v1 = ctrl_inw(p->mapbase + _8TCNT);
-		v2 = ctrl_inw(p->mapbase + _8TCNT);
-		v3 = ctrl_inw(p->mapbase + _8TCNT);
-		o1 = ctrl_inb(p->mapbase + _8TCSR) & 0x20;
-	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
-			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
-
-	v2 |= o1 << 10;
-	return v2;
-}
-
 static irqreturn_t timer8_interrupt(int irq, void *dev_id)
 {
 	struct timer8_priv *p = dev_id;
 
-	ctrl_outb(ctrl_inb(p->mapbase + _8TCSR) & ~0x40,
-		  p->mapbase + _8TCSR);
-	p->flags |= FLAG_IRQCONTEXT;
-	ctrl_outw(p->tcora, p->mapbase + TCORA);
-	if (!(p->flags & FLAG_SKIPEVENT)) {
-		if (clockevent_state_oneshot(&p->ced))
-			ctrl_outw(0x0000, p->mapbase + _8TCR);
-		p->ced.event_handler(&p->ced);
-	}
-	p->flags &= ~(FLAG_SKIPEVENT | FLAG_IRQCONTEXT);
+	if (clockevent_state_oneshot(&p->ced))
+		iowrite16be(0x0000, p->mapbase + _8TCR);
+
+	p->ced.event_handler(&p->ced);
+
+	bclr(CMFA, p->mapbase + _8TCSR);
 
 	return IRQ_HANDLED;
 }
 
 static void timer8_set_next(struct timer8_priv *p, unsigned long delta)
 {
-	unsigned long flags;
-	unsigned long now;
-
-	raw_spin_lock_irqsave(&p->lock, flags);
 	if (delta >= 0x10000)
-		dev_warn(&p->pdev->dev, "delta out of range\n");
-	now = timer8_get_counter(p);
-	p->tcora = delta;
-	ctrl_outb(ctrl_inb(p->mapbase + _8TCR) | 0x40, p->mapbase + _8TCR);
-	if (delta > now)
-		ctrl_outw(delta, p->mapbase + TCORA);
-	else
-		ctrl_outw(now + 1, p->mapbase + TCORA);
-
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+		pr_warn("delta out of range\n");
+	bclr(CMIEA, p->mapbase + _8TCR);
+	iowrite16be(delta, p->mapbase + TCORA);
+	iowrite16be(0x0000, p->mapbase + _8TCNT);
+	bclr(CMFA, p->mapbase + _8TCSR);
+	bset(CMIEA, p->mapbase + _8TCR);
 }
 
 static int timer8_enable(struct timer8_priv *p)
 {
-	p->rate = clk_get_rate(p->pclk) / 64;
-	ctrl_outw(0xffff, p->mapbase + TCORA);
-	ctrl_outw(0x0000, p->mapbase + _8TCNT);
-	ctrl_outw(0x0c02, p->mapbase + _8TCR);
+	iowrite16be(0xffff, p->mapbase + TCORA);
+	iowrite16be(0x0000, p->mapbase + _8TCNT);
+	iowrite16be(0x0c02, p->mapbase + _8TCR);
 
 	return 0;
 }
 
 static int timer8_start(struct timer8_priv *p)
 {
-	int ret = 0;
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&p->lock, flags);
-
-	if (!(p->flags & FLAG_STARTED))
-		ret = timer8_enable(p);
+	int ret;
 
-	if (ret)
-		goto out;
-	p->flags |= FLAG_STARTED;
+	if ((p->flags & FLAG_STARTED))
+		return 0;
 
- out:
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	ret = timer8_enable(p);
+	if (!ret)
+		p->flags |= FLAG_STARTED;
 
 	return ret;
 }
 
 static void timer8_stop(struct timer8_priv *p)
 {
-	unsigned long flags;
-
-	raw_spin_lock_irqsave(&p->lock, flags);
-
-	ctrl_outw(0x0000, p->mapbase + _8TCR);
-
-	raw_spin_unlock_irqrestore(&p->lock, flags);
+	iowrite16be(0x0000, p->mapbase + _8TCR);
 }
 
 static inline struct timer8_priv *ced_to_priv(struct clock_event_device *ced)
@@ -155,7 +99,7 @@ static inline struct timer8_priv *ced_to_priv(struct clock_event_device *ced)
 	return container_of(ced, struct timer8_priv, ced);
 }
 
-static void timer8_clock_event_start(struct timer8_priv *p, int periodic)
+static void timer8_clock_event_start(struct timer8_priv *p, unsigned long delta)
 {
 	struct clock_event_device *ced = &p->ced;
 
@@ -166,7 +110,7 @@ static void timer8_clock_event_start(struct timer8_priv *p, int periodic)
 	ced->max_delta_ns = clockevent_delta2ns(0xffff, ced);
 	ced->min_delta_ns = clockevent_delta2ns(0x0001, ced);
 
-	timer8_set_next(p, periodic?(p->rate + HZ/2) / HZ:0x10000);
+	timer8_set_next(p, delta);
 }
 
 static int timer8_clock_event_shutdown(struct clock_event_device *ced)
@@ -179,9 +123,9 @@ static int timer8_clock_event_periodic(struct clock_event_device *ced)
 {
 	struct timer8_priv *p = ced_to_priv(ced);
 
-	dev_info(&p->pdev->dev, "used for periodic clock events\n");
+	pr_info("%s: used for periodic clock events\n", ced->name);
 	timer8_stop(p);
-	timer8_clock_event_start(p, PERIODIC);
+	timer8_clock_event_start(p, (p->rate + HZ/2) / HZ);
 
 	return 0;
 }
@@ -190,9 +134,9 @@ static int timer8_clock_event_oneshot(struct clock_event_device *ced)
 {
 	struct timer8_priv *p = ced_to_priv(ced);
 
-	dev_info(&p->pdev->dev, "used for oneshot clock events\n");
+	pr_info("%s: used for oneshot clock events\n", ced->name);
 	timer8_stop(p);
-	timer8_clock_event_start(p, ONESHOT);
+	timer8_clock_event_start(p, 0x10000);
 
 	return 0;
 }
@@ -208,110 +152,64 @@ static int timer8_clock_event_next(unsigned long delta,
 	return 0;
 }
 
-static int timer8_setup(struct timer8_priv *p,
-			struct platform_device *pdev)
+static struct timer8_priv timer8_priv = {
+	.ced = {
+		.name = "h8300_8timer",
+		.features = CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+		.rating = 200,
+		.set_next_event = timer8_clock_event_next,
+		.set_state_shutdown = timer8_clock_event_shutdown,
+		.set_state_periodic = timer8_clock_event_periodic,
+		.set_state_oneshot = timer8_clock_event_oneshot,
+	},
+};
+
+static void __init h8300_8timer_init(struct device_node *node)
 {
-	struct resource *res;
+	void __iomem *base;
 	int irq;
-	int ret;
+	struct clk *clk;
 
-	p->pdev = pdev;
-
-	res = platform_get_resource(p->pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		return -ENXIO;
+	clk = of_clk_get(node, 0);
+	if (IS_ERR(clk)) {
+		pr_err("failed to get clock for clockevent\n");
+		return;
 	}
 
-	irq = platform_get_irq(p->pdev, 0);
-	if (irq < 0) {
-		dev_err(&p->pdev->dev, "failed to get irq\n");
-		return -ENXIO;
+	base = of_iomap(node, 0);
+	if (!base) {
+		pr_err("failed to map registers for clockevent\n");
+		goto free_clk;
 	}
 
-	p->mapbase = res->start;
-
-	p->irqaction.name = dev_name(&p->pdev->dev);
-	p->irqaction.handler = timer8_interrupt;
-	p->irqaction.dev_id = p;
-	p->irqaction.flags = IRQF_TIMER;
-
-	p->pclk = clk_get(&p->pdev->dev, "fck");
-	if (IS_ERR(p->pclk)) {
-		dev_err(&p->pdev->dev, "can't get clk\n");
-		return PTR_ERR(p->pclk);
+	irq = irq_of_parse_and_map(node, 0);
+	if (!irq) {
+		pr_err("failed to get irq for clockevent\n");
+		goto unmap_reg;
 	}
 
-	p->ced.name = pdev->name;
-	p->ced.features = CLOCK_EVT_FEAT_PERIODIC |
-		CLOCK_EVT_FEAT_ONESHOT;
-	p->ced.rating = 200;
-	p->ced.cpumask = cpumask_of(0);
-	p->ced.set_next_event = timer8_clock_event_next;
-	p->ced.set_state_shutdown = timer8_clock_event_shutdown;
-	p->ced.set_state_periodic = timer8_clock_event_periodic;
-	p->ced.set_state_oneshot = timer8_clock_event_oneshot;
-
-	ret = setup_irq(irq, &p->irqaction);
-	if (ret < 0) {
-		dev_err(&p->pdev->dev,
-			"failed to request irq %d\n", irq);
-		return ret;
-	}
-	clockevents_register_device(&p->ced);
-	platform_set_drvdata(pdev, p);
+	timer8_priv.mapbase = base;
 
-	return 0;
-}
-
-static int timer8_probe(struct platform_device *pdev)
-{
-	struct timer8_priv *p = platform_get_drvdata(pdev);
-
-	if (p) {
-		dev_info(&pdev->dev, "kept as earlytimer\n");
-		return 0;
+	timer8_priv.rate = clk_get_rate(clk) / SCALE;
+	if (!timer8_priv.rate) {
+		pr_err("Failed to get rate for the clocksource\n");
+		goto unmap_reg;
 	}
 
-	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
-	if (!p)
-		return -ENOMEM;
-
-	return timer8_setup(p, pdev);
-}
-
-static int timer8_remove(struct platform_device *pdev)
-{
-	return -EBUSY;
-}
-
-static const struct of_device_id timer8_of_table[] __maybe_unused = {
-	{ .compatible = "renesas,8bit-timer" },
-	{ }
-};
-
-MODULE_DEVICE_TABLE(of, timer8_of_table);
-static struct platform_driver timer8_driver = {
-	.probe		= timer8_probe,
-	.remove		= timer8_remove,
-	.driver		= {
-		.name	= "h8300-8timer",
-		.of_match_table = of_match_ptr(timer8_of_table),
+	if (request_irq(irq, timer8_interrupt, IRQF_TIMER,
+			timer8_priv.ced.name, &timer8_priv) < 0) {
+		pr_err("failed to request irq %d for clockevent\n", irq);
+		goto unmap_reg;
 	}
-};
 
-static int __init timer8_init(void)
-{
-	return platform_driver_register(&timer8_driver);
-}
+	clockevents_config_and_register(&timer8_priv.ced,
+					timer8_priv.rate, 1, 0x0000ffff);
 
-static void __exit timer8_exit(void)
-{
-	platform_driver_unregister(&timer8_driver);
+	return;
+unmap_reg:
+	iounmap(base);
+free_clk:
+	clk_put(clk);
 }
 
-subsys_initcall(timer8_init);
-module_exit(timer8_exit);
-MODULE_AUTHOR("Yoshinori Sato");
-MODULE_DESCRIPTION("H8/300 8bit Timer Driver");
-MODULE_LICENSE("GPL v2");
+CLOCKSOURCE_OF_DECLARE(h8300_8bit, "renesas,8bit-timer", h8300_8timer_init);
diff --git a/drivers/clocksource/h8300_tpu.c b/drivers/clocksource/h8300_tpu.c
index 5487410bfabb..d4c1a287c262 100644
--- a/drivers/clocksource/h8300_tpu.c
+++ b/drivers/clocksource/h8300_tpu.c
@@ -1,42 +1,30 @@
 /*
- *  H8/300 TPU Driver
+ *  H8S TPU Driver
  *
  *  Copyright 2015 Yoshinori Sato <ysato@users.sourcefoge.jp>
  *
  */
 
 #include <linux/errno.h>
-#include <linux/sched.h>
 #include <linux/kernel.h>
-#include <linux/interrupt.h>
 #include <linux/init.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
 #include <linux/clocksource.h>
-#include <linux/module.h>
 #include <linux/clk.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 
-#include <asm/irq.h>
+#define TCR	0x0
+#define TSR	0x5
+#define TCNT	0x6
 
-#define TCR	0
-#define TMDR	1
-#define TIOR	2
-#define TER	4
-#define TSR	5
-#define TCNT	6
-#define TGRA	8
-#define TGRB	10
-#define TGRC	12
-#define TGRD	14
+#define TCFV	0x10
 
 struct tpu_priv {
-	struct platform_device *pdev;
 	struct clocksource cs;
-	struct clk *clk;
-	unsigned long mapbase1;
-	unsigned long mapbase2;
+	void __iomem *mapbase1;
+	void __iomem *mapbase2;
 	raw_spinlock_t lock;
 	unsigned int cs_enabled;
 };
@@ -45,8 +33,8 @@ static inline unsigned long read_tcnt32(struct tpu_priv *p)
 {
 	unsigned long tcnt;
 
-	tcnt = ctrl_inw(p->mapbase1 + TCNT) << 16;
-	tcnt |= ctrl_inw(p->mapbase2 + TCNT);
+	tcnt = ioread16be(p->mapbase1 + TCNT) << 16;
+	tcnt |= ioread16be(p->mapbase2 + TCNT);
 	return tcnt;
 }
 
@@ -55,7 +43,7 @@ static int tpu_get_counter(struct tpu_priv *p, unsigned long long *val)
 	unsigned long v1, v2, v3;
 	int o1, o2;
 
-	o1 = ctrl_inb(p->mapbase1 + TSR) & 0x10;
+	o1 = ioread8(p->mapbase1 + TSR) & TCFV;
 
 	/* Make sure the timer value is stable. Stolen from acpi_pm.c */
 	do {
@@ -63,7 +51,7 @@ static int tpu_get_counter(struct tpu_priv *p, unsigned long long *val)
 		v1 = read_tcnt32(p);
 		v2 = read_tcnt32(p);
 		v3 = read_tcnt32(p);
-		o1 = ctrl_inb(p->mapbase1 + TSR) & 0x10;
+		o1 = ioread8(p->mapbase1 + TSR) & TCFV;
 	} while (unlikely((o1 != o2) || (v1 > v2 && v1 < v3)
 			  || (v2 > v3 && v2 < v1) || (v3 > v1 && v3 < v2)));
 
@@ -96,10 +84,10 @@ static int tpu_clocksource_enable(struct clocksource *cs)
 
 	WARN_ON(p->cs_enabled);
 
-	ctrl_outw(0, p->mapbase1 + TCNT);
-	ctrl_outw(0, p->mapbase2 + TCNT);
-	ctrl_outb(0x0f, p->mapbase1 + TCR);
-	ctrl_outb(0x03, p->mapbase2 + TCR);
+	iowrite16be(0, p->mapbase1 + TCNT);
+	iowrite16be(0, p->mapbase2 + TCNT);
+	iowrite8(0x0f, p->mapbase1 + TCR);
+	iowrite8(0x03, p->mapbase2 + TCR);
 
 	p->cs_enabled = true;
 	return 0;
@@ -111,96 +99,59 @@ static void tpu_clocksource_disable(struct clocksource *cs)
 
 	WARN_ON(!p->cs_enabled);
 
-	ctrl_outb(0, p->mapbase1 + TCR);
-	ctrl_outb(0, p->mapbase2 + TCR);
+	iowrite8(0, p->mapbase1 + TCR);
+	iowrite8(0, p->mapbase2 + TCR);
 	p->cs_enabled = false;
 }
 
+static struct tpu_priv tpu_priv = {
+	.cs = {
+		.name = "H8S_TPU",
+		.rating = 200,
+		.read = tpu_clocksource_read,
+		.enable = tpu_clocksource_enable,
+		.disable = tpu_clocksource_disable,
+		.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8),
+		.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+	},
+};
+
 #define CH_L 0
 #define CH_H 1
 
-static int __init tpu_setup(struct tpu_priv *p, struct platform_device *pdev)
+static void __init h8300_tpu_init(struct device_node *node)
 {
-	struct resource *res[2];
-
-	p->pdev = pdev;
+	void __iomem *base[2];
+	struct clk *clk;
 
-	res[CH_L] = platform_get_resource(p->pdev, IORESOURCE_MEM, CH_L);
-	res[CH_H] = platform_get_resource(p->pdev, IORESOURCE_MEM, CH_H);
-	if (!res[CH_L] || !res[CH_H]) {
-		dev_err(&p->pdev->dev, "failed to get I/O memory\n");
-		return -ENXIO;
+	clk = of_clk_get(node, 0);
+	if (IS_ERR(clk)) {
+		pr_err("failed to get clock for clocksource\n");
+		return;
 	}
 
-	p->clk = clk_get(&p->pdev->dev, "fck");
-	if (IS_ERR(p->clk)) {
-		dev_err(&p->pdev->dev, "can't get clk\n");
-		return PTR_ERR(p->clk);
+	base[CH_L] = of_iomap(node, CH_L);
+	if (!base[CH_L]) {
+		pr_err("failed to map registers for clocksource\n");
+		goto free_clk;
 	}
-
-	p->mapbase1 = res[CH_L]->start;
-	p->mapbase2 = res[CH_H]->start;
-
-	p->cs.name = pdev->name;
-	p->cs.rating = 200;
-	p->cs.read = tpu_clocksource_read;
-	p->cs.enable = tpu_clocksource_enable;
-	p->cs.disable = tpu_clocksource_disable;
-	p->cs.mask = CLOCKSOURCE_MASK(sizeof(unsigned long) * 8);
-	p->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
-	clocksource_register_hz(&p->cs, clk_get_rate(p->clk) / 64);
-	platform_set_drvdata(pdev, p);
-
-	return 0;
-}
-
-static int tpu_probe(struct platform_device *pdev)
-{
-	struct tpu_priv *p = platform_get_drvdata(pdev);
-
-	if (p) {
-		dev_info(&pdev->dev, "kept as earlytimer\n");
-		return 0;
+	base[CH_H] = of_iomap(node, CH_H);
+	if (!base[CH_H]) {
+		pr_err("failed to map registers for clocksource\n");
+		goto unmap_L;
 	}
 
-	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
-	if (!p)
-		return -ENOMEM;
+	tpu_priv.mapbase1 = base[CH_L];
+	tpu_priv.mapbase2 = base[CH_H];
 
-	return tpu_setup(p, pdev);
-}
-
-static int tpu_remove(struct platform_device *pdev)
-{
-	return -EBUSY;
-}
-
-static const struct of_device_id tpu_of_table[] = {
-	{ .compatible = "renesas,tpu" },
-	{ }
-};
+	clocksource_register_hz(&tpu_priv.cs, clk_get_rate(clk) / 64);
 
-static struct platform_driver tpu_driver = {
-	.probe		= tpu_probe,
-	.remove		= tpu_remove,
-	.driver		= {
-		.name	= "h8s-tpu",
-		.of_match_table = of_match_ptr(tpu_of_table),
-	}
-};
-
-static int __init tpu_init(void)
-{
-	return platform_driver_register(&tpu_driver);
-}
+	return;
 
-static void __exit tpu_exit(void)
-{
-	platform_driver_unregister(&tpu_driver);
+unmap_L:
+	iounmap(base[CH_H]);
+free_clk:
+	clk_put(clk);
 }
 
-subsys_initcall(tpu_init);
-module_exit(tpu_exit);
-MODULE_AUTHOR("Yoshinori Sato");
-MODULE_DESCRIPTION("H8S Timer Pulse Unit Driver");
-MODULE_LICENSE("GPL v2");
+CLOCKSOURCE_OF_DECLARE(h8300_tpu, "renesas,tpu", h8300_tpu_init);
diff --git a/drivers/clocksource/mtk_timer.c b/drivers/clocksource/mtk_timer.c
index fbfc74685e6a..d67bc356488f 100644
--- a/drivers/clocksource/mtk_timer.c
+++ b/drivers/clocksource/mtk_timer.c
@@ -16,6 +16,8 @@
  * GNU General Public License for more details.
  */
 
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
 #include <linux/clk.h>
 #include <linux/clockchips.h>
 #include <linux/interrupt.h>
@@ -187,10 +189,8 @@ static void __init mtk_timer_init(struct device_node *node)
 	struct clk *clk;
 
 	evt = kzalloc(sizeof(*evt), GFP_KERNEL);
-	if (!evt) {
-		pr_warn("Can't allocate mtk clock event driver struct");
+	if (!evt)
 		return;
-	}
 
 	evt->dev.name = "mtk_tick";
 	evt->dev.rating = 300;
@@ -204,31 +204,31 @@ static void __init mtk_timer_init(struct device_node *node)
 
 	evt->gpt_base = of_io_request_and_map(node, 0, "mtk-timer");
 	if (IS_ERR(evt->gpt_base)) {
-		pr_warn("Can't get resource\n");
-		return;
+		pr_err("Can't get resource\n");
+		goto err_kzalloc;
 	}
 
 	evt->dev.irq = irq_of_parse_and_map(node, 0);
 	if (evt->dev.irq <= 0) {
-		pr_warn("Can't parse IRQ");
+		pr_err("Can't parse IRQ\n");
 		goto err_mem;
 	}
 
 	clk = of_clk_get(node, 0);
 	if (IS_ERR(clk)) {
-		pr_warn("Can't get timer clock");
+		pr_err("Can't get timer clock\n");
 		goto err_irq;
 	}
 
 	if (clk_prepare_enable(clk)) {
-		pr_warn("Can't prepare clock");
+		pr_err("Can't prepare clock\n");
 		goto err_clk_put;
 	}
 	rate = clk_get_rate(clk);
 
 	if (request_irq(evt->dev.irq, mtk_timer_interrupt,
 			IRQF_TIMER | IRQF_IRQPOLL, "mtk_timer", evt)) {
-		pr_warn("failed to setup irq %d\n", evt->dev.irq);
+		pr_err("failed to setup irq %d\n", evt->dev.irq);
 		goto err_clk_disable;
 	}
 
@@ -260,5 +260,7 @@ err_mem:
 	iounmap(evt->gpt_base);
 	of_address_to_resource(node, 0, &res);
 	release_mem_region(res.start, resource_size(&res));
+err_kzalloc:
+	kfree(evt);
 }
 CLOCKSOURCE_OF_DECLARE(mtk_mt6577, "mediatek,mt6577-timer", mtk_timer_init);
diff --git a/drivers/clocksource/rockchip_timer.c b/drivers/clocksource/rockchip_timer.c
index d3c1742ded1a..8c77a529d0d4 100644
--- a/drivers/clocksource/rockchip_timer.c
+++ b/drivers/clocksource/rockchip_timer.c
@@ -17,16 +17,16 @@
 
 #define TIMER_NAME "rk_timer"
 
-#define TIMER_LOAD_COUNT0 0x00
-#define TIMER_LOAD_COUNT1 0x04
-#define TIMER_CONTROL_REG 0x10
-#define TIMER_INT_STATUS 0x18
+#define TIMER_LOAD_COUNT0	0x00
+#define TIMER_LOAD_COUNT1	0x04
+#define TIMER_CONTROL_REG	0x10
+#define TIMER_INT_STATUS	0x18
 
-#define TIMER_DISABLE 0x0
-#define TIMER_ENABLE 0x1
-#define TIMER_MODE_FREE_RUNNING (0 << 1)
-#define TIMER_MODE_USER_DEFINED_COUNT (1 << 1)
-#define TIMER_INT_UNMASK (1 << 2)
+#define TIMER_DISABLE		0x0
+#define TIMER_ENABLE		0x1
+#define TIMER_MODE_FREE_RUNNING			(0 << 1)
+#define TIMER_MODE_USER_DEFINED_COUNT		(1 << 1)
+#define TIMER_INT_UNMASK			(1 << 2)
 
 struct bc_timer {
 	struct clock_event_device ce;
@@ -49,14 +49,12 @@ static inline void __iomem *rk_base(struct clock_event_device *ce)
 static inline void rk_timer_disable(struct clock_event_device *ce)
 {
 	writel_relaxed(TIMER_DISABLE, rk_base(ce) + TIMER_CONTROL_REG);
-	dsb();
 }
 
 static inline void rk_timer_enable(struct clock_event_device *ce, u32 flags)
 {
 	writel_relaxed(TIMER_ENABLE | TIMER_INT_UNMASK | flags,
 		       rk_base(ce) + TIMER_CONTROL_REG);
-	dsb();
 }
 
 static void rk_timer_update_counter(unsigned long cycles,
@@ -64,13 +62,11 @@ static void rk_timer_update_counter(unsigned long cycles,
 {
 	writel_relaxed(cycles, rk_base(ce) + TIMER_LOAD_COUNT0);
 	writel_relaxed(0, rk_base(ce) + TIMER_LOAD_COUNT1);
-	dsb();
 }
 
 static void rk_timer_interrupt_clear(struct clock_event_device *ce)
 {
 	writel_relaxed(1, rk_base(ce) + TIMER_INT_STATUS);
-	dsb();
 }
 
 static inline int rk_timer_set_next_event(unsigned long cycles,
@@ -173,4 +169,5 @@ static void __init rk_timer_init(struct device_node *np)
 
 	clockevents_config_and_register(ce, bc_timer.freq, 1, UINT_MAX);
 }
+
 CLOCKSOURCE_OF_DECLARE(rk_timer, "rockchip,rk3288-timer", rk_timer_init);
diff --git a/drivers/clocksource/tango_xtal.c b/drivers/clocksource/tango_xtal.c
index d297b30d2bc0..2bcecafdeaea 100644
--- a/drivers/clocksource/tango_xtal.c
+++ b/drivers/clocksource/tango_xtal.c
@@ -19,19 +19,6 @@ static u64 notrace read_sched_clock(void)
 	return read_xtal_counter();
 }
 
-static cycle_t read_clocksource(struct clocksource *cs)
-{
-	return read_xtal_counter();
-}
-
-static struct clocksource tango_xtal = {
-	.name	= "tango-xtal",
-	.rating	= 350,
-	.read	= read_clocksource,
-	.mask	= CLOCKSOURCE_MASK(32),
-	.flags	= CLOCK_SOURCE_IS_CONTINUOUS,
-};
-
 static void __init tango_clocksource_init(struct device_node *np)
 {
 	struct clk *clk;
@@ -53,8 +40,9 @@ static void __init tango_clocksource_init(struct device_node *np)
 	delay_timer.freq = xtal_freq;
 	delay_timer.read_current_timer = read_xtal_counter;
 
-	ret = clocksource_register_hz(&tango_xtal, xtal_freq);
-	if (ret != 0) {
+	ret = clocksource_mmio_init(xtal_in_cnt, "tango-xtal", xtal_freq, 350,
+				    32, clocksource_mmio_readl_up);
+	if (!ret) {
 		pr_err("%s: registration failed\n", np->full_name);
 		return;
 	}
diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index 6ee91401918e..4da2af9694a2 100644
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -98,7 +98,8 @@ static int tc_shutdown(struct clock_event_device *d)
 
 	__raw_writel(0xff, regs + ATMEL_TC_REG(2, IDR));
 	__raw_writel(ATMEL_TC_CLKDIS, regs + ATMEL_TC_REG(2, CCR));
-	clk_disable(tcd->clk);
+	if (!clockevent_state_detached(d))
+		clk_disable(tcd->clk);
 
 	return 0;
 }
diff --git a/drivers/clocksource/tegra20_timer.c b/drivers/clocksource/tegra20_timer.c
index 6ebda1177e79..38333aba3055 100644
--- a/drivers/clocksource/tegra20_timer.c
+++ b/drivers/clocksource/tegra20_timer.c
@@ -96,7 +96,8 @@ static struct clock_event_device tegra_clockevent = {
 	.name			= "timer0",
 	.rating			= 300,
 	.features		= CLOCK_EVT_FEAT_ONESHOT |
-				  CLOCK_EVT_FEAT_PERIODIC,
+				  CLOCK_EVT_FEAT_PERIODIC |
+				  CLOCK_EVT_FEAT_DYNIRQ,
 	.set_next_event		= tegra_timer_set_next_event,
 	.set_state_shutdown	= tegra_timer_shutdown,
 	.set_state_periodic	= tegra_timer_set_periodic,
diff --git a/drivers/clocksource/time-lpc32xx.c b/drivers/clocksource/time-lpc32xx.c
index a1c06a2bc77c..1316876b487a 100644
--- a/drivers/clocksource/time-lpc32xx.c
+++ b/drivers/clocksource/time-lpc32xx.c
@@ -125,7 +125,7 @@ static int __init lpc32xx_clocksource_init(struct device_node *np)
 
 	clk = of_clk_get_by_name(np, "timerclk");
 	if (IS_ERR(clk)) {
-		pr_err("clock get failed (%lu)\n", PTR_ERR(clk));
+		pr_err("clock get failed (%ld)\n", PTR_ERR(clk));
 		return PTR_ERR(clk);
 	}
 
@@ -184,7 +184,7 @@ static int __init lpc32xx_clockevent_init(struct device_node *np)
 
 	clk = of_clk_get_by_name(np, "timerclk");
 	if (IS_ERR(clk)) {
-		pr_err("clock get failed (%lu)\n", PTR_ERR(clk));
+		pr_err("clock get failed (%ld)\n", PTR_ERR(clk));
 		return PTR_ERR(clk);
 	}
 
diff --git a/drivers/clocksource/time-pistachio.c b/drivers/clocksource/time-pistachio.c
index bba679900054..3269d9ef7a18 100644
--- a/drivers/clocksource/time-pistachio.c
+++ b/drivers/clocksource/time-pistachio.c
@@ -84,7 +84,7 @@ pistachio_clocksource_read_cycles(struct clocksource *cs)
 	counter = gpt_readl(pcs->base, TIMER_CURRENT_VALUE, 0);
 	raw_spin_unlock_irqrestore(&pcs->lock, flags);
 
-	return ~(cycle_t)counter;
+	return (cycle_t)~counter;
 }
 
 static u64 notrace pistachio_read_sched_clock(void)
diff --git a/drivers/clocksource/timer-sun5i.c b/drivers/clocksource/timer-sun5i.c
index bca9573e036a..24c83f9efd87 100644
--- a/drivers/clocksource/timer-sun5i.c
+++ b/drivers/clocksource/timer-sun5i.c
@@ -152,13 +152,6 @@ 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)
 {
@@ -217,13 +210,8 @@ static int __init sun5i_setup_clocksource(struct device_node *node,
 	writel(TIMER_CTL_ENABLE | TIMER_CTL_RELOAD,
 	       base + TIMER_CTL_REG(1));
 
-	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);
+	ret = clocksource_mmio_init(base + TIMER_CNTVAL_LO_REG(1), node->name,
+				    rate, 340, 32, clocksource_mmio_readl_down);
 	if (ret) {
 		pr_err("Couldn't register clock source.\n");
 		goto err_remove_notifier;
diff --git a/drivers/clocksource/vt8500_timer.c b/drivers/clocksource/vt8500_timer.c
index a92e94b40b5b..ddb409274f45 100644
--- a/drivers/clocksource/vt8500_timer.c
+++ b/drivers/clocksource/vt8500_timer.c
@@ -30,7 +30,6 @@
 #include <linux/clocksource.h>
 #include <linux/clockchips.h>
 #include <linux/delay.h>
-#include <asm/mach/time.h>
 
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -50,6 +49,8 @@
 
 #define msecs_to_loops(t) (loops_per_jiffy / 1000 * HZ * t)
 
+#define MIN_OSCR_DELTA		16
+
 static void __iomem *regbase;
 
 static cycle_t vt8500_timer_read(struct clocksource *cs)
@@ -80,7 +81,7 @@ static int vt8500_timer_set_next_event(unsigned long cycles,
 		cpu_relax();
 	writel((unsigned long)alarm, regbase + TIMER_MATCH_VAL);
 
-	if ((signed)(alarm - clocksource.read(&clocksource)) <= 16)
+	if ((signed)(alarm - clocksource.read(&clocksource)) <= MIN_OSCR_DELTA)
 		return -ETIME;
 
 	writel(1, regbase + TIMER_IER_VAL);
@@ -151,7 +152,7 @@ static void __init vt8500_timer_init(struct device_node *np)
 		pr_err("%s: setup_irq failed for %s\n", __func__,
 							clockevent.name);
 	clockevents_config_and_register(&clockevent, VT8500_TIMER_HZ,
-					4, 0xf0000000);
+					MIN_OSCR_DELTA * 2, 0xf0000000);
 }
 
 CLOCKSOURCE_OF_DECLARE(vt8500, "via,vt8500-timer", vt8500_timer_init);
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index 659879a56dba..f93511031177 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -296,6 +296,7 @@ endif
 config QORIQ_CPUFREQ
 	tristate "CPU frequency scaling driver for Freescale QorIQ SoCs"
 	depends on OF && COMMON_CLK && (PPC_E500MC || ARM)
+	depends on !CPU_THERMAL || THERMAL
 	select CLK_QORIQ
 	help
 	  This adds the CPUFreq driver support for Freescale QorIQ SoCs
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index b1f8a73e5a94..14b1f9393b05 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -6,6 +6,8 @@
 config ARM_BIG_LITTLE_CPUFREQ
 	tristate "Generic ARM big LITTLE CPUfreq driver"
 	depends on (ARM_CPU_TOPOLOGY || ARM64) && HAVE_CLK
+	# if CPU_THERMAL is on and THERMAL=m, ARM_BIT_LITTLE_CPUFREQ cannot be =y
+	depends on !CPU_THERMAL || THERMAL
 	select PM_OPP
 	help
 	  This enables the Generic CPUfreq driver for ARM big.LITTLE platforms.
@@ -82,10 +84,10 @@ config ARM_KIRKWOOD_CPUFREQ
 	  SoCs.
 
 config ARM_MT8173_CPUFREQ
-	bool "Mediatek MT8173 CPUFreq support"
+	tristate "Mediatek MT8173 CPUFreq support"
 	depends on ARCH_MEDIATEK && REGULATOR
 	depends on ARM64 || (ARM_CPU_TOPOLOGY && COMPILE_TEST)
-	depends on !CPU_THERMAL || THERMAL=y
+	depends on !CPU_THERMAL || THERMAL
 	select PM_OPP
 	help
 	  This adds the CPUFreq driver support for Mediatek MT8173 SoC.
@@ -217,6 +219,16 @@ config ARM_SPEAR_CPUFREQ
 	help
 	  This adds the CPUFreq driver support for SPEAr SOCs.
 
+config ARM_STI_CPUFREQ
+	tristate "STi CPUFreq support"
+	depends on SOC_STIH407
+	help
+	  This driver uses the generic OPP framework to match the running
+	  platform with a predefined set of suitable values.  If not provided
+	  we will fall-back so safe-values contained in Device Tree.  Enable
+	  this config option if you wish to add CPUFreq support for STi based
+	  SoCs.
+
 config ARM_TEGRA20_CPUFREQ
 	bool "Tegra20 CPUFreq support"
 	depends on ARCH_TEGRA
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index c0af1a1281c8..9e63fb1b09f8 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -73,6 +73,7 @@ obj-$(CONFIG_ARM_SA1100_CPUFREQ)	+= sa1100-cpufreq.o
 obj-$(CONFIG_ARM_SA1110_CPUFREQ)	+= sa1110-cpufreq.o
 obj-$(CONFIG_ARM_SCPI_CPUFREQ)		+= scpi-cpufreq.o
 obj-$(CONFIG_ARM_SPEAR_CPUFREQ)		+= spear-cpufreq.o
+obj-$(CONFIG_ARM_STI_CPUFREQ)		+= sti-cpufreq.o
 obj-$(CONFIG_ARM_TEGRA20_CPUFREQ)	+= tegra20-cpufreq.o
 obj-$(CONFIG_ARM_TEGRA124_CPUFREQ)	+= tegra124-cpufreq.o
 obj-$(CONFIG_ARM_VEXPRESS_SPC_CPUFREQ)	+= vexpress-spc-cpufreq.o
diff --git a/drivers/cpufreq/acpi-cpufreq.c b/drivers/cpufreq/acpi-cpufreq.c
index cec1ee2d2f74..51eef87bbc37 100644
--- a/drivers/cpufreq/acpi-cpufreq.c
+++ b/drivers/cpufreq/acpi-cpufreq.c
@@ -135,7 +135,7 @@ static void boost_set_msrs(bool enable, const struct cpumask *cpumask)
 	wrmsr_on_cpus(cpumask, msr_addr, msrs);
 }
 
-static int _store_boost(int val)
+static int set_boost(int val)
 {
 	get_online_cpus();
 	boost_set_msrs(val, cpu_online_mask);
@@ -158,29 +158,24 @@ static ssize_t show_freqdomain_cpus(struct cpufreq_policy *policy, char *buf)
 cpufreq_freq_attr_ro(freqdomain_cpus);
 
 #ifdef CONFIG_X86_ACPI_CPUFREQ_CPB
-static ssize_t store_boost(const char *buf, size_t count)
+static ssize_t store_cpb(struct cpufreq_policy *policy, const char *buf,
+			 size_t count)
 {
 	int ret;
-	unsigned long val = 0;
+	unsigned int val = 0;
 
-	if (!acpi_cpufreq_driver.boost_supported)
+	if (!acpi_cpufreq_driver.set_boost)
 		return -EINVAL;
 
-	ret = kstrtoul(buf, 10, &val);
-	if (ret || (val > 1))
+	ret = kstrtouint(buf, 10, &val);
+	if (ret || val > 1)
 		return -EINVAL;
 
-	_store_boost((int) val);
+	set_boost(val);
 
 	return count;
 }
 
-static ssize_t store_cpb(struct cpufreq_policy *policy, const char *buf,
-			 size_t count)
-{
-	return store_boost(buf, count);
-}
-
 static ssize_t show_cpb(struct cpufreq_policy *policy, char *buf)
 {
 	return sprintf(buf, "%u\n", acpi_cpufreq_driver.boost_enabled);
@@ -905,7 +900,6 @@ static struct cpufreq_driver acpi_cpufreq_driver = {
 	.resume		= acpi_cpufreq_resume,
 	.name		= "acpi-cpufreq",
 	.attr		= acpi_cpufreq_attr,
-	.set_boost      = _store_boost,
 };
 
 static void __init acpi_cpufreq_boost_init(void)
@@ -916,7 +910,7 @@ static void __init acpi_cpufreq_boost_init(void)
 		if (!msrs)
 			return;
 
-		acpi_cpufreq_driver.boost_supported = true;
+		acpi_cpufreq_driver.set_boost = set_boost;
 		acpi_cpufreq_driver.boost_enabled = boost_state(0);
 
 		cpu_notifier_register_begin();
diff --git a/drivers/cpufreq/arm_big_little.c b/drivers/cpufreq/arm_big_little.c
index c5d256caa664..c251247ae661 100644
--- a/drivers/cpufreq/arm_big_little.c
+++ b/drivers/cpufreq/arm_big_little.c
@@ -23,6 +23,7 @@
 #include <linux/cpu.h>
 #include <linux/cpufreq.h>
 #include <linux/cpumask.h>
+#include <linux/cpu_cooling.h>
 #include <linux/export.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
@@ -55,6 +56,7 @@ static bool bL_switching_enabled;
 #define ACTUAL_FREQ(cluster, freq)  ((cluster == A7_CLUSTER) ? freq << 1 : freq)
 #define VIRT_FREQ(cluster, freq)    ((cluster == A7_CLUSTER) ? freq >> 1 : freq)
 
+static struct thermal_cooling_device *cdev[MAX_CLUSTERS];
 static struct cpufreq_arm_bL_ops *arm_bL_ops;
 static struct clk *clk[MAX_CLUSTERS];
 static struct cpufreq_frequency_table *freq_table[MAX_CLUSTERS + 1];
@@ -493,6 +495,12 @@ static int bL_cpufreq_init(struct cpufreq_policy *policy)
 static int bL_cpufreq_exit(struct cpufreq_policy *policy)
 {
 	struct device *cpu_dev;
+	int cur_cluster = cpu_to_cluster(policy->cpu);
+
+	if (cur_cluster < MAX_CLUSTERS) {
+		cpufreq_cooling_unregister(cdev[cur_cluster]);
+		cdev[cur_cluster] = NULL;
+	}
 
 	cpu_dev = get_cpu_device(policy->cpu);
 	if (!cpu_dev) {
@@ -507,6 +515,38 @@ static int bL_cpufreq_exit(struct cpufreq_policy *policy)
 	return 0;
 }
 
+static void bL_cpufreq_ready(struct cpufreq_policy *policy)
+{
+	struct device *cpu_dev = get_cpu_device(policy->cpu);
+	int cur_cluster = cpu_to_cluster(policy->cpu);
+	struct device_node *np;
+
+	/* Do not register a cpu_cooling device if we are in IKS mode */
+	if (cur_cluster >= MAX_CLUSTERS)
+		return;
+
+	np = of_node_get(cpu_dev->of_node);
+	if (WARN_ON(!np))
+		return;
+
+	if (of_find_property(np, "#cooling-cells", NULL)) {
+		u32 power_coefficient = 0;
+
+		of_property_read_u32(np, "dynamic-power-coefficient",
+				     &power_coefficient);
+
+		cdev[cur_cluster] = of_cpufreq_power_cooling_register(np,
+				policy->related_cpus, power_coefficient, NULL);
+		if (IS_ERR(cdev[cur_cluster])) {
+			dev_err(cpu_dev,
+				"running cpufreq without cooling device: %ld\n",
+				PTR_ERR(cdev[cur_cluster]));
+			cdev[cur_cluster] = NULL;
+		}
+	}
+	of_node_put(np);
+}
+
 static struct cpufreq_driver bL_cpufreq_driver = {
 	.name			= "arm-big-little",
 	.flags			= CPUFREQ_STICKY |
@@ -517,6 +557,7 @@ static struct cpufreq_driver bL_cpufreq_driver = {
 	.get			= bL_cpufreq_get_rate,
 	.init			= bL_cpufreq_init,
 	.exit			= bL_cpufreq_exit,
+	.ready			= bL_cpufreq_ready,
 	.attr			= cpufreq_generic_attr,
 };
 
diff --git a/drivers/cpufreq/blackfin-cpufreq.c b/drivers/cpufreq/blackfin-cpufreq.c
index a9f8e5bd0716..12e97d8a9db0 100644
--- a/drivers/cpufreq/blackfin-cpufreq.c
+++ b/drivers/cpufreq/blackfin-cpufreq.c
@@ -112,7 +112,7 @@ static unsigned int bfin_getfreq_khz(unsigned int cpu)
 }
 
 #ifdef CONFIG_BF60x
-unsigned long cpu_set_cclk(int cpu, unsigned long new)
+static int cpu_set_cclk(int cpu, unsigned long new)
 {
 	struct clk *clk;
 	int ret;
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
index 90d64081ddb3..0ca74d070058 100644
--- a/drivers/cpufreq/cpufreq-dt.c
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -50,7 +50,8 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
 	struct private_data *priv = policy->driver_data;
 	struct device *cpu_dev = priv->cpu_dev;
 	struct regulator *cpu_reg = priv->cpu_reg;
-	unsigned long volt = 0, volt_old = 0, tol = 0;
+	unsigned long volt = 0, tol = 0;
+	int volt_old = 0;
 	unsigned int old_freq, new_freq;
 	long freq_Hz, freq_exact;
 	int ret;
@@ -83,7 +84,7 @@ static int set_target(struct cpufreq_policy *policy, unsigned int index)
 			opp_freq / 1000, volt);
 	}
 
-	dev_dbg(cpu_dev, "%u MHz, %ld mV --> %u MHz, %ld mV\n",
+	dev_dbg(cpu_dev, "%u MHz, %d mV --> %u MHz, %ld mV\n",
 		old_freq / 1000, (volt_old > 0) ? volt_old / 1000 : -1,
 		new_freq / 1000, volt ? volt / 1000 : -1);
 
@@ -141,15 +142,16 @@ static int allocate_resources(int cpu, struct device **cdev,
 
 try_again:
 	cpu_reg = regulator_get_optional(cpu_dev, reg);
-	if (IS_ERR(cpu_reg)) {
+	ret = PTR_ERR_OR_ZERO(cpu_reg);
+	if (ret) {
 		/*
 		 * If cpu's regulator supply node is present, but regulator is
 		 * not yet registered, we should try defering probe.
 		 */
-		if (PTR_ERR(cpu_reg) == -EPROBE_DEFER) {
+		if (ret == -EPROBE_DEFER) {
 			dev_dbg(cpu_dev, "cpu%d regulator not ready, retry\n",
 				cpu);
-			return -EPROBE_DEFER;
+			return ret;
 		}
 
 		/* Try with "cpu-supply" */
@@ -158,18 +160,16 @@ try_again:
 			goto try_again;
 		}
 
-		dev_dbg(cpu_dev, "no regulator for cpu%d: %ld\n",
-			cpu, PTR_ERR(cpu_reg));
+		dev_dbg(cpu_dev, "no regulator for cpu%d: %d\n", cpu, ret);
 	}
 
 	cpu_clk = clk_get(cpu_dev, NULL);
-	if (IS_ERR(cpu_clk)) {
+	ret = PTR_ERR_OR_ZERO(cpu_clk);
+	if (ret) {
 		/* put regulator */
 		if (!IS_ERR(cpu_reg))
 			regulator_put(cpu_reg);
 
-		ret = PTR_ERR(cpu_clk);
-
 		/*
 		 * If cpu's clk node is present, but clock is not yet
 		 * registered, we should try defering probe.
@@ -407,8 +407,13 @@ static void cpufreq_ready(struct cpufreq_policy *policy)
 	 * thermal DT code takes care of matching them.
 	 */
 	if (of_find_property(np, "#cooling-cells", NULL)) {
-		priv->cdev = of_cpufreq_cooling_register(np,
-							 policy->related_cpus);
+		u32 power_coefficient = 0;
+
+		of_property_read_u32(np, "dynamic-power-coefficient",
+				     &power_coefficient);
+
+		priv->cdev = of_cpufreq_power_cooling_register(np,
+				policy->related_cpus, power_coefficient, NULL);
 		if (IS_ERR(priv->cdev)) {
 			dev_err(priv->cpu_dev,
 				"running cpufreq without cooling device: %ld\n",
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index 8412ce5f93a7..e979ec78b695 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -48,11 +48,11 @@ static struct cpufreq_policy *next_policy(struct cpufreq_policy *policy,
 					  bool active)
 {
 	do {
-		policy = list_next_entry(policy, policy_list);
-
 		/* No more policies in the list */
-		if (&policy->policy_list == &cpufreq_policy_list)
+		if (list_is_last(&policy->policy_list, &cpufreq_policy_list))
 			return NULL;
+
+		policy = list_next_entry(policy, policy_list);
 	} while (!suitable_policy(policy, active));
 
 	return policy;
@@ -2330,29 +2330,15 @@ int cpufreq_boost_trigger_state(int state)
 	return ret;
 }
 
-int cpufreq_boost_supported(void)
+static bool cpufreq_boost_supported(void)
 {
-	if (likely(cpufreq_driver))
-		return cpufreq_driver->boost_supported;
-
-	return 0;
+	return likely(cpufreq_driver) && cpufreq_driver->set_boost;
 }
-EXPORT_SYMBOL_GPL(cpufreq_boost_supported);
 
 static int create_boost_sysfs_file(void)
 {
 	int ret;
 
-	if (!cpufreq_boost_supported())
-		return 0;
-
-	/*
-	 * Check if driver provides function to enable boost -
-	 * if not, use cpufreq_boost_set_sw as default
-	 */
-	if (!cpufreq_driver->set_boost)
-		cpufreq_driver->set_boost = cpufreq_boost_set_sw;
-
 	ret = sysfs_create_file(cpufreq_global_kobject, &boost.attr);
 	if (ret)
 		pr_err("%s: cannot register global BOOST sysfs file\n",
@@ -2375,7 +2361,7 @@ int cpufreq_enable_boost_support(void)
 	if (cpufreq_boost_supported())
 		return 0;
 
-	cpufreq_driver->boost_supported = true;
+	cpufreq_driver->set_boost = cpufreq_boost_set_sw;
 
 	/* This will get removed on driver unregister */
 	return create_boost_sysfs_file();
@@ -2435,9 +2421,11 @@ int cpufreq_register_driver(struct cpufreq_driver *driver_data)
 	if (driver_data->setpolicy)
 		driver_data->flags |= CPUFREQ_CONST_LOOPS;
 
-	ret = create_boost_sysfs_file();
-	if (ret)
-		goto err_null_driver;
+	if (cpufreq_boost_supported()) {
+		ret = create_boost_sysfs_file();
+		if (ret)
+			goto err_null_driver;
+	}
 
 	ret = subsys_interface_register(&cpufreq_interface);
 	if (ret)
diff --git a/drivers/cpufreq/cpufreq_conservative.c b/drivers/cpufreq/cpufreq_conservative.c
index 1fa1deb6e91f..606ad74abe6e 100644
--- a/drivers/cpufreq/cpufreq_conservative.c
+++ b/drivers/cpufreq/cpufreq_conservative.c
@@ -115,13 +115,13 @@ static void cs_check_cpu(int cpu, unsigned int load)
 	}
 }
 
-static unsigned int cs_dbs_timer(struct cpu_dbs_info *cdbs,
-				 struct dbs_data *dbs_data, bool modify_all)
+static unsigned int cs_dbs_timer(struct cpufreq_policy *policy, bool modify_all)
 {
+	struct dbs_data *dbs_data = policy->governor_data;
 	struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 
 	if (modify_all)
-		dbs_check_cpu(dbs_data, cdbs->shared->policy->cpu);
+		dbs_check_cpu(dbs_data, policy->cpu);
 
 	return delay_for_sampling_rate(cs_tuners->sampling_rate);
 }
diff --git a/drivers/cpufreq/cpufreq_governor.c b/drivers/cpufreq/cpufreq_governor.c
index b260576ddb12..e0d111024d48 100644
--- a/drivers/cpufreq/cpufreq_governor.c
+++ b/drivers/cpufreq/cpufreq_governor.c
@@ -84,6 +84,9 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 			(cur_wall_time - j_cdbs->prev_cpu_wall);
 		j_cdbs->prev_cpu_wall = cur_wall_time;
 
+		if (cur_idle_time < j_cdbs->prev_cpu_idle)
+			cur_idle_time = j_cdbs->prev_cpu_idle;
+
 		idle_time = (unsigned int)
 			(cur_idle_time - j_cdbs->prev_cpu_idle);
 		j_cdbs->prev_cpu_idle = cur_idle_time;
@@ -158,47 +161,55 @@ void dbs_check_cpu(struct dbs_data *dbs_data, int cpu)
 }
 EXPORT_SYMBOL_GPL(dbs_check_cpu);
 
-static inline void __gov_queue_work(int cpu, struct dbs_data *dbs_data,
-		unsigned int delay)
-{
-	struct cpu_dbs_info *cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
-
-	mod_delayed_work_on(cpu, system_wq, &cdbs->dwork, delay);
-}
-
-void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
-		unsigned int delay, bool all_cpus)
+void gov_add_timers(struct cpufreq_policy *policy, unsigned int delay)
 {
-	int i;
+	struct dbs_data *dbs_data = policy->governor_data;
+	struct cpu_dbs_info *cdbs;
+	int cpu;
 
-	if (!all_cpus) {
-		/*
-		 * Use raw_smp_processor_id() to avoid preemptible warnings.
-		 * We know that this is only called with all_cpus == false from
-		 * works that have been queued with *_work_on() functions and
-		 * those works are canceled during CPU_DOWN_PREPARE so they
-		 * can't possibly run on any other CPU.
-		 */
-		__gov_queue_work(raw_smp_processor_id(), dbs_data, delay);
-	} else {
-		for_each_cpu(i, policy->cpus)
-			__gov_queue_work(i, dbs_data, delay);
+	for_each_cpu(cpu, policy->cpus) {
+		cdbs = dbs_data->cdata->get_cpu_cdbs(cpu);
+		cdbs->timer.expires = jiffies + delay;
+		add_timer_on(&cdbs->timer, cpu);
 	}
 }
-EXPORT_SYMBOL_GPL(gov_queue_work);
+EXPORT_SYMBOL_GPL(gov_add_timers);
 
-static inline void gov_cancel_work(struct dbs_data *dbs_data,
-		struct cpufreq_policy *policy)
+static inline void gov_cancel_timers(struct cpufreq_policy *policy)
 {
+	struct dbs_data *dbs_data = policy->governor_data;
 	struct cpu_dbs_info *cdbs;
 	int i;
 
 	for_each_cpu(i, policy->cpus) {
 		cdbs = dbs_data->cdata->get_cpu_cdbs(i);
-		cancel_delayed_work_sync(&cdbs->dwork);
+		del_timer_sync(&cdbs->timer);
 	}
 }
 
+void gov_cancel_work(struct cpu_common_dbs_info *shared)
+{
+	/* Tell dbs_timer_handler() to skip queuing up work items. */
+	atomic_inc(&shared->skip_work);
+	/*
+	 * If dbs_timer_handler() is already running, it may not notice the
+	 * incremented skip_work, so wait for it to complete to prevent its work
+	 * item from being queued up after the cancel_work_sync() below.
+	 */
+	gov_cancel_timers(shared->policy);
+	/*
+	 * In case dbs_timer_handler() managed to run and spawn a work item
+	 * before the timers have been canceled, wait for that work item to
+	 * complete and then cancel all of the timers set up by it.  If
+	 * dbs_timer_handler() runs again at that point, it will see the
+	 * positive value of skip_work and won't spawn any more work items.
+	 */
+	cancel_work_sync(&shared->work);
+	gov_cancel_timers(shared->policy);
+	atomic_set(&shared->skip_work, 0);
+}
+EXPORT_SYMBOL_GPL(gov_cancel_work);
+
 /* Will return if we need to evaluate cpu load again or not */
 static bool need_load_eval(struct cpu_common_dbs_info *shared,
 			   unsigned int sampling_rate)
@@ -217,29 +228,21 @@ static bool need_load_eval(struct cpu_common_dbs_info *shared,
 	return true;
 }
 
-static void dbs_timer(struct work_struct *work)
+static void dbs_work_handler(struct work_struct *work)
 {
-	struct cpu_dbs_info *cdbs = container_of(work, struct cpu_dbs_info,
-						 dwork.work);
-	struct cpu_common_dbs_info *shared = cdbs->shared;
+	struct cpu_common_dbs_info *shared = container_of(work, struct
+					cpu_common_dbs_info, work);
 	struct cpufreq_policy *policy;
 	struct dbs_data *dbs_data;
 	unsigned int sampling_rate, delay;
-	bool modify_all = true;
-
-	mutex_lock(&shared->timer_mutex);
+	bool eval_load;
 
 	policy = shared->policy;
-
-	/*
-	 * Governor might already be disabled and there is no point continuing
-	 * with the work-handler.
-	 */
-	if (!policy)
-		goto unlock;
-
 	dbs_data = policy->governor_data;
 
+	/* Kill all timers */
+	gov_cancel_timers(policy);
+
 	if (dbs_data->cdata->governor == GOV_CONSERVATIVE) {
 		struct cs_dbs_tuners *cs_tuners = dbs_data->tuners;
 
@@ -250,14 +253,37 @@ static void dbs_timer(struct work_struct *work)
 		sampling_rate = od_tuners->sampling_rate;
 	}
 
-	if (!need_load_eval(cdbs->shared, sampling_rate))
-		modify_all = false;
-
-	delay = dbs_data->cdata->gov_dbs_timer(cdbs, dbs_data, modify_all);
-	gov_queue_work(dbs_data, policy, delay, modify_all);
+	eval_load = need_load_eval(shared, sampling_rate);
 
-unlock:
+	/*
+	 * Make sure cpufreq_governor_limits() isn't evaluating load in
+	 * parallel.
+	 */
+	mutex_lock(&shared->timer_mutex);
+	delay = dbs_data->cdata->gov_dbs_timer(policy, eval_load);
 	mutex_unlock(&shared->timer_mutex);
+
+	atomic_dec(&shared->skip_work);
+
+	gov_add_timers(policy, delay);
+}
+
+static void dbs_timer_handler(unsigned long data)
+{
+	struct cpu_dbs_info *cdbs = (struct cpu_dbs_info *)data;
+	struct cpu_common_dbs_info *shared = cdbs->shared;
+
+	/*
+	 * Timer handler may not be allowed to queue the work at the moment,
+	 * because:
+	 * - Another timer handler has done that
+	 * - We are stopping the governor
+	 * - Or we are updating the sampling rate of the ondemand governor
+	 */
+	if (atomic_inc_return(&shared->skip_work) > 1)
+		atomic_dec(&shared->skip_work);
+	else
+		queue_work(system_wq, &shared->work);
 }
 
 static void set_sampling_rate(struct dbs_data *dbs_data,
@@ -287,6 +313,9 @@ static int alloc_common_dbs_info(struct cpufreq_policy *policy,
 	for_each_cpu(j, policy->related_cpus)
 		cdata->get_cpu_cdbs(j)->shared = shared;
 
+	mutex_init(&shared->timer_mutex);
+	atomic_set(&shared->skip_work, 0);
+	INIT_WORK(&shared->work, dbs_work_handler);
 	return 0;
 }
 
@@ -297,6 +326,8 @@ static void free_common_dbs_info(struct cpufreq_policy *policy,
 	struct cpu_common_dbs_info *shared = cdbs->shared;
 	int j;
 
+	mutex_destroy(&shared->timer_mutex);
+
 	for_each_cpu(j, policy->cpus)
 		cdata->get_cpu_cdbs(j)->shared = NULL;
 
@@ -356,16 +387,18 @@ static int cpufreq_governor_init(struct cpufreq_policy *policy,
 	if (!have_governor_per_policy())
 		cdata->gdbs_data = dbs_data;
 
+	policy->governor_data = dbs_data;
+
 	ret = sysfs_create_group(get_governor_parent_kobj(policy),
 				 get_sysfs_attr(dbs_data));
 	if (ret)
 		goto reset_gdbs_data;
 
-	policy->governor_data = dbs_data;
-
 	return 0;
 
 reset_gdbs_data:
+	policy->governor_data = NULL;
+
 	if (!have_governor_per_policy())
 		cdata->gdbs_data = NULL;
 	cdata->exit(dbs_data, !policy->governor->initialized);
@@ -386,16 +419,19 @@ static int cpufreq_governor_exit(struct cpufreq_policy *policy,
 	if (!cdbs->shared || cdbs->shared->policy)
 		return -EBUSY;
 
-	policy->governor_data = NULL;
 	if (!--dbs_data->usage_count) {
 		sysfs_remove_group(get_governor_parent_kobj(policy),
 				   get_sysfs_attr(dbs_data));
 
+		policy->governor_data = NULL;
+
 		if (!have_governor_per_policy())
 			cdata->gdbs_data = NULL;
 
 		cdata->exit(dbs_data, policy->governor->initialized == 1);
 		kfree(dbs_data);
+	} else {
+		policy->governor_data = NULL;
 	}
 
 	free_common_dbs_info(policy, cdata);
@@ -433,7 +469,6 @@ static int cpufreq_governor_start(struct cpufreq_policy *policy,
 
 	shared->policy = policy;
 	shared->time_stamp = ktime_get();
-	mutex_init(&shared->timer_mutex);
 
 	for_each_cpu(j, policy->cpus) {
 		struct cpu_dbs_info *j_cdbs = cdata->get_cpu_cdbs(j);
@@ -450,7 +485,9 @@ static int cpufreq_governor_start(struct cpufreq_policy *policy,
 		if (ignore_nice)
 			j_cdbs->prev_cpu_nice = kcpustat_cpu(j).cpustat[CPUTIME_NICE];
 
-		INIT_DEFERRABLE_WORK(&j_cdbs->dwork, dbs_timer);
+		__setup_timer(&j_cdbs->timer, dbs_timer_handler,
+			      (unsigned long)j_cdbs,
+			      TIMER_DEFERRABLE | TIMER_IRQSAFE);
 	}
 
 	if (cdata->governor == GOV_CONSERVATIVE) {
@@ -468,8 +505,7 @@ static int cpufreq_governor_start(struct cpufreq_policy *policy,
 		od_ops->powersave_bias_init_cpu(cpu);
 	}
 
-	gov_queue_work(dbs_data, policy, delay_for_sampling_rate(sampling_rate),
-		       true);
+	gov_add_timers(policy, delay_for_sampling_rate(sampling_rate));
 	return 0;
 }
 
@@ -483,18 +519,9 @@ static int cpufreq_governor_stop(struct cpufreq_policy *policy,
 	if (!shared || !shared->policy)
 		return -EBUSY;
 
-	/*
-	 * Work-handler must see this updated, as it should not proceed any
-	 * further after governor is disabled. And so timer_mutex is taken while
-	 * updating this value.
-	 */
-	mutex_lock(&shared->timer_mutex);
+	gov_cancel_work(shared);
 	shared->policy = NULL;
-	mutex_unlock(&shared->timer_mutex);
-
-	gov_cancel_work(dbs_data, policy);
 
-	mutex_destroy(&shared->timer_mutex);
 	return 0;
 }
 
diff --git a/drivers/cpufreq/cpufreq_governor.h b/drivers/cpufreq/cpufreq_governor.h
index 5621bb03e874..91e767a058a7 100644
--- a/drivers/cpufreq/cpufreq_governor.h
+++ b/drivers/cpufreq/cpufreq_governor.h
@@ -17,6 +17,7 @@
 #ifndef _CPUFREQ_GOVERNOR_H
 #define _CPUFREQ_GOVERNOR_H
 
+#include <linux/atomic.h>
 #include <linux/cpufreq.h>
 #include <linux/kernel_stat.h>
 #include <linux/module.h>
@@ -132,12 +133,14 @@ static void *get_cpu_dbs_info_s(int cpu)				\
 struct cpu_common_dbs_info {
 	struct cpufreq_policy *policy;
 	/*
-	 * percpu mutex that serializes governor limit change with dbs_timer
-	 * invocation. We do not want dbs_timer to run when user is changing
-	 * the governor or limits.
+	 * Per policy mutex that serializes load evaluation from limit-change
+	 * and work-handler.
 	 */
 	struct mutex timer_mutex;
+
 	ktime_t time_stamp;
+	atomic_t skip_work;
+	struct work_struct work;
 };
 
 /* Per cpu structures */
@@ -152,7 +155,7 @@ struct cpu_dbs_info {
 	 * wake-up from idle.
 	 */
 	unsigned int prev_load;
-	struct delayed_work dwork;
+	struct timer_list timer;
 	struct cpu_common_dbs_info *shared;
 };
 
@@ -209,8 +212,7 @@ struct common_dbs_data {
 
 	struct cpu_dbs_info *(*get_cpu_cdbs)(int cpu);
 	void *(*get_cpu_dbs_info_s)(int cpu);
-	unsigned int (*gov_dbs_timer)(struct cpu_dbs_info *cdbs,
-				      struct dbs_data *dbs_data,
+	unsigned int (*gov_dbs_timer)(struct cpufreq_policy *policy,
 				      bool modify_all);
 	void (*gov_check_cpu)(int cpu, unsigned int load);
 	int (*init)(struct dbs_data *dbs_data, bool notify);
@@ -269,11 +271,11 @@ static ssize_t show_sampling_rate_min_gov_pol				\
 
 extern struct mutex cpufreq_governor_lock;
 
+void gov_add_timers(struct cpufreq_policy *policy, unsigned int delay);
+void gov_cancel_work(struct cpu_common_dbs_info *shared);
 void dbs_check_cpu(struct dbs_data *dbs_data, int cpu);
 int cpufreq_governor_dbs(struct cpufreq_policy *policy,
 		struct common_dbs_data *cdata, unsigned int event);
-void gov_queue_work(struct dbs_data *dbs_data, struct cpufreq_policy *policy,
-		unsigned int delay, bool all_cpus);
 void od_register_powersave_bias_handler(unsigned int (*f)
 		(struct cpufreq_policy *, unsigned int, unsigned int),
 		unsigned int powersave_bias);
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index 03ac6ce54042..eae51070c034 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -191,10 +191,9 @@ static void od_check_cpu(int cpu, unsigned int load)
 	}
 }
 
-static unsigned int od_dbs_timer(struct cpu_dbs_info *cdbs,
-				 struct dbs_data *dbs_data, bool modify_all)
+static unsigned int od_dbs_timer(struct cpufreq_policy *policy, bool modify_all)
 {
-	struct cpufreq_policy *policy = cdbs->shared->policy;
+	struct dbs_data *dbs_data = policy->governor_data;
 	unsigned int cpu = policy->cpu;
 	struct od_cpu_dbs_info_s *dbs_info = &per_cpu(od_cpu_dbs_info,
 			cpu);
@@ -247,40 +246,66 @@ static void update_sampling_rate(struct dbs_data *dbs_data,
 		unsigned int new_rate)
 {
 	struct od_dbs_tuners *od_tuners = dbs_data->tuners;
+	struct cpumask cpumask;
 	int cpu;
 
 	od_tuners->sampling_rate = new_rate = max(new_rate,
 			dbs_data->min_sampling_rate);
 
-	for_each_online_cpu(cpu) {
+	/*
+	 * Lock governor so that governor start/stop can't execute in parallel.
+	 */
+	mutex_lock(&od_dbs_cdata.mutex);
+
+	cpumask_copy(&cpumask, cpu_online_mask);
+
+	for_each_cpu(cpu, &cpumask) {
 		struct cpufreq_policy *policy;
 		struct od_cpu_dbs_info_s *dbs_info;
+		struct cpu_dbs_info *cdbs;
+		struct cpu_common_dbs_info *shared;
 		unsigned long next_sampling, appointed_at;
 
-		policy = cpufreq_cpu_get(cpu);
-		if (!policy)
-			continue;
-		if (policy->governor != &cpufreq_gov_ondemand) {
-			cpufreq_cpu_put(policy);
-			continue;
-		}
 		dbs_info = &per_cpu(od_cpu_dbs_info, cpu);
-		cpufreq_cpu_put(policy);
+		cdbs = &dbs_info->cdbs;
+		shared = cdbs->shared;
 
-		if (!delayed_work_pending(&dbs_info->cdbs.dwork))
+		/*
+		 * A valid shared and shared->policy means governor hasn't
+		 * stopped or exited yet.
+		 */
+		if (!shared || !shared->policy)
+			continue;
+
+		policy = shared->policy;
+
+		/* clear all CPUs of this policy */
+		cpumask_andnot(&cpumask, &cpumask, policy->cpus);
+
+		/*
+		 * Update sampling rate for CPUs whose policy is governed by
+		 * dbs_data. In case of governor_per_policy, only a single
+		 * policy will be governed by dbs_data, otherwise there can be
+		 * multiple policies that are governed by the same dbs_data.
+		 */
+		if (dbs_data != policy->governor_data)
 			continue;
 
+		/*
+		 * Checking this for any CPU should be fine, timers for all of
+		 * them are scheduled together.
+		 */
 		next_sampling = jiffies + usecs_to_jiffies(new_rate);
-		appointed_at = dbs_info->cdbs.dwork.timer.expires;
+		appointed_at = dbs_info->cdbs.timer.expires;
 
 		if (time_before(next_sampling, appointed_at)) {
-			cancel_delayed_work_sync(&dbs_info->cdbs.dwork);
-
-			gov_queue_work(dbs_data, policy,
-				       usecs_to_jiffies(new_rate), true);
+			gov_cancel_work(shared);
+			gov_add_timers(policy, usecs_to_jiffies(new_rate));
 
 		}
 	}
+
+	mutex_unlock(&od_dbs_cdata.mutex);
 }
 
 static ssize_t store_sampling_rate(struct dbs_data *dbs_data, const char *buf,
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index 98fb8821382d..cd83d477e32d 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -66,6 +66,7 @@ static inline int ceiling_fp(int32_t x)
 
 struct sample {
 	int32_t core_pct_busy;
+	int32_t busy_scaled;
 	u64 aperf;
 	u64 mperf;
 	u64 tsc;
@@ -112,6 +113,7 @@ struct cpudata {
 	u64	prev_aperf;
 	u64	prev_mperf;
 	u64	prev_tsc;
+	u64	prev_cummulative_iowait;
 	struct sample sample;
 };
 
@@ -133,6 +135,7 @@ struct pstate_funcs {
 	int (*get_scaling)(void);
 	void (*set)(struct cpudata*, int pstate);
 	void (*get_vid)(struct cpudata *);
+	int32_t (*get_target_pstate)(struct cpudata *);
 };
 
 struct cpu_defaults {
@@ -140,6 +143,9 @@ struct cpu_defaults {
 	struct pstate_funcs funcs;
 };
 
+static inline int32_t get_target_pstate_use_performance(struct cpudata *cpu);
+static inline int32_t get_target_pstate_use_cpu_load(struct cpudata *cpu);
+
 static struct pstate_adjust_policy pid_params;
 static struct pstate_funcs pstate_funcs;
 static int hwp_active;
@@ -738,6 +744,7 @@ static struct cpu_defaults core_params = {
 		.get_turbo = core_get_turbo_pstate,
 		.get_scaling = core_get_scaling,
 		.set = core_set_pstate,
+		.get_target_pstate = get_target_pstate_use_performance,
 	},
 };
 
@@ -758,6 +765,7 @@ static struct cpu_defaults silvermont_params = {
 		.set = atom_set_pstate,
 		.get_scaling = silvermont_get_scaling,
 		.get_vid = atom_get_vid,
+		.get_target_pstate = get_target_pstate_use_cpu_load,
 	},
 };
 
@@ -778,6 +786,7 @@ static struct cpu_defaults airmont_params = {
 		.set = atom_set_pstate,
 		.get_scaling = airmont_get_scaling,
 		.get_vid = atom_get_vid,
+		.get_target_pstate = get_target_pstate_use_cpu_load,
 	},
 };
 
@@ -797,6 +806,7 @@ static struct cpu_defaults knl_params = {
 		.get_turbo = knl_get_turbo_pstate,
 		.get_scaling = core_get_scaling,
 		.set = core_set_pstate,
+		.get_target_pstate = get_target_pstate_use_performance,
 	},
 };
 
@@ -882,12 +892,11 @@ static inline void intel_pstate_sample(struct cpudata *cpu)
 	local_irq_save(flags);
 	rdmsrl(MSR_IA32_APERF, aperf);
 	rdmsrl(MSR_IA32_MPERF, mperf);
-	if (cpu->prev_mperf == mperf) {
+	tsc = rdtsc();
+	if ((cpu->prev_mperf == mperf) || (cpu->prev_tsc == tsc)) {
 		local_irq_restore(flags);
 		return;
 	}
-
-	tsc = rdtsc();
 	local_irq_restore(flags);
 
 	cpu->last_sample_time = cpu->sample.time;
@@ -922,7 +931,43 @@ static inline void intel_pstate_set_sample_time(struct cpudata *cpu)
 	mod_timer_pinned(&cpu->timer, jiffies + delay);
 }
 
-static inline int32_t intel_pstate_get_scaled_busy(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;
+
+	cummulative_iowait = get_cpu_iowait_time_us(cpu->cpu, &now);
+
+	/*
+	 * 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);
+
+	mperf = cpu->sample.mperf + delta_iowait_mperf;
+	cpu->prev_cummulative_iowait = cummulative_iowait;
+
+
+	/*
+	 * 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.
+	 */
+	cpu_load = div64_u64(int_tofp(100) * mperf, sample->tsc);
+	cpu->sample.busy_scaled = cpu_load;
+
+	return cpu->pstate.current_pstate - pid_calc(&cpu->pid, cpu_load);
+}
+
+static inline int32_t get_target_pstate_use_performance(struct cpudata *cpu)
 {
 	int32_t core_busy, max_pstate, current_pstate, sample_ratio;
 	s64 duration_us;
@@ -960,30 +1005,24 @@ static inline int32_t intel_pstate_get_scaled_busy(struct cpudata *cpu)
 		core_busy = mul_fp(core_busy, sample_ratio);
 	}
 
-	return core_busy;
+	cpu->sample.busy_scaled = core_busy;
+	return cpu->pstate.current_pstate - pid_calc(&cpu->pid, core_busy);
 }
 
 static inline void intel_pstate_adjust_busy_pstate(struct cpudata *cpu)
 {
-	int32_t busy_scaled;
-	struct _pid *pid;
-	signed int ctl;
-	int from;
+	int from, target_pstate;
 	struct sample *sample;
 
 	from = cpu->pstate.current_pstate;
 
-	pid = &cpu->pid;
-	busy_scaled = intel_pstate_get_scaled_busy(cpu);
+	target_pstate = pstate_funcs.get_target_pstate(cpu);
 
-	ctl = pid_calc(pid, busy_scaled);
-
-	/* Negative values of ctl increase the pstate and vice versa */
-	intel_pstate_set_pstate(cpu, cpu->pstate.current_pstate - ctl, true);
+	intel_pstate_set_pstate(cpu, target_pstate, true);
 
 	sample = &cpu->sample;
 	trace_pstate_sample(fp_toint(sample->core_pct_busy),
-		fp_toint(busy_scaled),
+		fp_toint(sample->busy_scaled),
 		from,
 		cpu->pstate.current_pstate,
 		sample->mperf,
@@ -1237,6 +1276,8 @@ static void copy_cpu_funcs(struct pstate_funcs *funcs)
 	pstate_funcs.get_scaling = funcs->get_scaling;
 	pstate_funcs.set       = funcs->set;
 	pstate_funcs.get_vid   = funcs->get_vid;
+	pstate_funcs.get_target_pstate = funcs->get_target_pstate;
+
 }
 
 #if IS_ENABLED(CONFIG_ACPI)
diff --git a/drivers/cpufreq/mt8173-cpufreq.c b/drivers/cpufreq/mt8173-cpufreq.c
index 83001dc5b646..2058e6d292ce 100644
--- a/drivers/cpufreq/mt8173-cpufreq.c
+++ b/drivers/cpufreq/mt8173-cpufreq.c
@@ -17,6 +17,7 @@
 #include <linux/cpu_cooling.h>
 #include <linux/cpufreq.h>
 #include <linux/cpumask.h>
+#include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm_opp.h>
@@ -41,16 +42,35 @@
  * the original PLL becomes stable at target frequency.
  */
 struct mtk_cpu_dvfs_info {
+	struct cpumask cpus;
 	struct device *cpu_dev;
 	struct regulator *proc_reg;
 	struct regulator *sram_reg;
 	struct clk *cpu_clk;
 	struct clk *inter_clk;
 	struct thermal_cooling_device *cdev;
+	struct list_head list_head;
 	int intermediate_voltage;
 	bool need_voltage_tracking;
 };
 
+static LIST_HEAD(dvfs_info_list);
+
+static struct mtk_cpu_dvfs_info *mtk_cpu_dvfs_info_lookup(int cpu)
+{
+	struct mtk_cpu_dvfs_info *info;
+	struct list_head *list;
+
+	list_for_each(list, &dvfs_info_list) {
+		info = list_entry(list, struct mtk_cpu_dvfs_info, list_head);
+
+		if (cpumask_test_cpu(cpu, &info->cpus))
+			return info;
+	}
+
+	return NULL;
+}
+
 static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
 					int new_vproc)
 {
@@ -59,7 +79,10 @@ static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
 	int old_vproc, old_vsram, new_vsram, vsram, vproc, ret;
 
 	old_vproc = regulator_get_voltage(proc_reg);
-	old_vsram = regulator_get_voltage(sram_reg);
+	if (old_vproc < 0) {
+		pr_err("%s: invalid Vproc value: %d\n", __func__, old_vproc);
+		return old_vproc;
+	}
 	/* Vsram should not exceed the maximum allowed voltage of SoC. */
 	new_vsram = min(new_vproc + MIN_VOLT_SHIFT, MAX_VOLT_LIMIT);
 
@@ -72,7 +95,17 @@ static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
 		 */
 		do {
 			old_vsram = regulator_get_voltage(sram_reg);
+			if (old_vsram < 0) {
+				pr_err("%s: invalid Vsram value: %d\n",
+				       __func__, old_vsram);
+				return old_vsram;
+			}
 			old_vproc = regulator_get_voltage(proc_reg);
+			if (old_vproc < 0) {
+				pr_err("%s: invalid Vproc value: %d\n",
+				       __func__, old_vproc);
+				return old_vproc;
+			}
 
 			vsram = min(new_vsram, old_vproc + MAX_VOLT_SHIFT);
 
@@ -117,7 +150,17 @@ static int mtk_cpufreq_voltage_tracking(struct mtk_cpu_dvfs_info *info,
 		 */
 		do {
 			old_vproc = regulator_get_voltage(proc_reg);
+			if (old_vproc < 0) {
+				pr_err("%s: invalid Vproc value: %d\n",
+				       __func__, old_vproc);
+				return old_vproc;
+			}
 			old_vsram = regulator_get_voltage(sram_reg);
+			if (old_vsram < 0) {
+				pr_err("%s: invalid Vsram value: %d\n",
+				       __func__, old_vsram);
+				return old_vsram;
+			}
 
 			vproc = max(new_vproc, old_vsram - MAX_VOLT_SHIFT);
 			ret = regulator_set_voltage(proc_reg, vproc,
@@ -185,6 +228,10 @@ static int mtk_cpufreq_set_target(struct cpufreq_policy *policy,
 
 	old_freq_hz = clk_get_rate(cpu_clk);
 	old_vproc = regulator_get_voltage(info->proc_reg);
+	if (old_vproc < 0) {
+		pr_err("%s: invalid Vproc value: %d\n", __func__, old_vproc);
+		return old_vproc;
+	}
 
 	freq_hz = freq_table[index].frequency * 1000;
 
@@ -344,7 +391,15 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 	/* Both presence and absence of sram regulator are valid cases. */
 	sram_reg = regulator_get_exclusive(cpu_dev, "sram");
 
-	ret = dev_pm_opp_of_add_table(cpu_dev);
+	/* Get OPP-sharing information from "operating-points-v2" bindings */
+	ret = dev_pm_opp_of_get_sharing_cpus(cpu_dev, &info->cpus);
+	if (ret) {
+		pr_err("failed to get OPP-sharing information for cpu%d\n",
+		       cpu);
+		goto out_free_resources;
+	}
+
+	ret = dev_pm_opp_of_cpumask_add_table(&info->cpus);
 	if (ret) {
 		pr_warn("no OPP table for cpu%d\n", cpu);
 		goto out_free_resources;
@@ -378,7 +433,7 @@ static int mtk_cpu_dvfs_info_init(struct mtk_cpu_dvfs_info *info, int cpu)
 	return 0;
 
 out_free_opp_table:
-	dev_pm_opp_of_remove_table(cpu_dev);
+	dev_pm_opp_of_cpumask_remove_table(&info->cpus);
 
 out_free_resources:
 	if (!IS_ERR(proc_reg))
@@ -404,7 +459,7 @@ static void mtk_cpu_dvfs_info_release(struct mtk_cpu_dvfs_info *info)
 	if (!IS_ERR(info->inter_clk))
 		clk_put(info->inter_clk);
 
-	dev_pm_opp_of_remove_table(info->cpu_dev);
+	dev_pm_opp_of_cpumask_remove_table(&info->cpus);
 }
 
 static int mtk_cpufreq_init(struct cpufreq_policy *policy)
@@ -413,22 +468,18 @@ static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 	struct cpufreq_frequency_table *freq_table;
 	int ret;
 
-	info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-
-	ret = mtk_cpu_dvfs_info_init(info, policy->cpu);
-	if (ret) {
-		pr_err("%s failed to initialize dvfs info for cpu%d\n",
-		       __func__, policy->cpu);
-		goto out_free_dvfs_info;
+	info = mtk_cpu_dvfs_info_lookup(policy->cpu);
+	if (!info) {
+		pr_err("dvfs info for cpu%d is not initialized.\n",
+		       policy->cpu);
+		return -EINVAL;
 	}
 
 	ret = dev_pm_opp_init_cpufreq_table(info->cpu_dev, &freq_table);
 	if (ret) {
 		pr_err("failed to init cpufreq table for cpu%d: %d\n",
 		       policy->cpu, ret);
-		goto out_release_dvfs_info;
+		return ret;
 	}
 
 	ret = cpufreq_table_validate_and_show(policy, freq_table);
@@ -437,8 +488,7 @@ static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 		goto out_free_cpufreq_table;
 	}
 
-	/* CPUs in the same cluster share a clock and power domain. */
-	cpumask_copy(policy->cpus, &cpu_topology[policy->cpu].core_sibling);
+	cpumask_copy(policy->cpus, &info->cpus);
 	policy->driver_data = info;
 	policy->clk = info->cpu_clk;
 
@@ -446,13 +496,6 @@ static int mtk_cpufreq_init(struct cpufreq_policy *policy)
 
 out_free_cpufreq_table:
 	dev_pm_opp_free_cpufreq_table(info->cpu_dev, &freq_table);
-
-out_release_dvfs_info:
-	mtk_cpu_dvfs_info_release(info);
-
-out_free_dvfs_info:
-	kfree(info);
-
 	return ret;
 }
 
@@ -462,14 +505,13 @@ static int mtk_cpufreq_exit(struct cpufreq_policy *policy)
 
 	cpufreq_cooling_unregister(info->cdev);
 	dev_pm_opp_free_cpufreq_table(info->cpu_dev, &policy->freq_table);
-	mtk_cpu_dvfs_info_release(info);
-	kfree(info);
 
 	return 0;
 }
 
 static struct cpufreq_driver mt8173_cpufreq_driver = {
-	.flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
+	.flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK |
+		 CPUFREQ_HAVE_GOVERNOR_PER_POLICY,
 	.verify = cpufreq_generic_frequency_table_verify,
 	.target_index = mtk_cpufreq_set_target,
 	.get = cpufreq_generic_get,
@@ -482,11 +524,47 @@ static struct cpufreq_driver mt8173_cpufreq_driver = {
 
 static int mt8173_cpufreq_probe(struct platform_device *pdev)
 {
-	int ret;
+	struct mtk_cpu_dvfs_info *info;
+	struct list_head *list, *tmp;
+	int cpu, ret;
+
+	for_each_possible_cpu(cpu) {
+		info = mtk_cpu_dvfs_info_lookup(cpu);
+		if (info)
+			continue;
+
+		info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+		if (!info) {
+			ret = -ENOMEM;
+			goto release_dvfs_info_list;
+		}
+
+		ret = mtk_cpu_dvfs_info_init(info, cpu);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to initialize dvfs info for cpu%d\n",
+				cpu);
+			goto release_dvfs_info_list;
+		}
+
+		list_add(&info->list_head, &dvfs_info_list);
+	}
 
 	ret = cpufreq_register_driver(&mt8173_cpufreq_driver);
-	if (ret)
-		pr_err("failed to register mtk cpufreq driver\n");
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register mtk cpufreq driver\n");
+		goto release_dvfs_info_list;
+	}
+
+	return 0;
+
+release_dvfs_info_list:
+	list_for_each_safe(list, tmp, &dvfs_info_list) {
+		info = list_entry(list, struct mtk_cpu_dvfs_info, list_head);
+
+		mtk_cpu_dvfs_info_release(info);
+		list_del(list);
+	}
 
 	return ret;
 }
diff --git a/drivers/cpufreq/pcc-cpufreq.c b/drivers/cpufreq/pcc-cpufreq.c
index 2a0d58959acf..808a320e9d5d 100644
--- a/drivers/cpufreq/pcc-cpufreq.c
+++ b/drivers/cpufreq/pcc-cpufreq.c
@@ -555,6 +555,8 @@ static int pcc_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	policy->min = policy->cpuinfo.min_freq =
 		ioread32(&pcch_hdr->minimum_frequency) * 1000;
 
+	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+
 	pr_debug("init: policy->max is %d, policy->min is %d\n",
 		policy->max, policy->min);
 out:
diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index cb501386eb6e..547890fd9572 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -586,7 +586,7 @@ static int __init powernv_cpufreq_init(void)
 	int rc = 0;
 
 	/* Don't probe on pseries (guest) platforms */
-	if (!firmware_has_feature(FW_FEATURE_OPALv3))
+	if (!firmware_has_feature(FW_FEATURE_OPAL))
 		return -ENODEV;
 
 	/* Discover pstates from device tree and init */
diff --git a/drivers/cpufreq/pxa2xx-cpufreq.c b/drivers/cpufreq/pxa2xx-cpufreq.c
index 1d99c97defa9..096377232747 100644
--- a/drivers/cpufreq/pxa2xx-cpufreq.c
+++ b/drivers/cpufreq/pxa2xx-cpufreq.c
@@ -202,7 +202,7 @@ static void __init pxa_cpufreq_init_voltages(void)
 	}
 }
 #else
-static int pxa_cpufreq_change_voltage(struct pxa_freqs *pxa_freq)
+static int pxa_cpufreq_change_voltage(const struct pxa_freqs *pxa_freq)
 {
 	return 0;
 }
diff --git a/drivers/cpufreq/qoriq-cpufreq.c b/drivers/cpufreq/qoriq-cpufreq.c
index 358f0752c31e..b23e525a7af3 100644
--- a/drivers/cpufreq/qoriq-cpufreq.c
+++ b/drivers/cpufreq/qoriq-cpufreq.c
@@ -12,6 +12,7 @@
 
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
+#include <linux/cpu_cooling.h>
 #include <linux/errno.h>
 #include <linux/init.h>
 #include <linux/kernel.h>
@@ -33,6 +34,7 @@
 struct cpu_data {
 	struct clk **pclk;
 	struct cpufreq_frequency_table *table;
+	struct thermal_cooling_device *cdev;
 };
 
 /**
@@ -321,6 +323,27 @@ static int qoriq_cpufreq_target(struct cpufreq_policy *policy,
 	return clk_set_parent(policy->clk, parent);
 }
 
+
+static void qoriq_cpufreq_ready(struct cpufreq_policy *policy)
+{
+	struct cpu_data *cpud = policy->driver_data;
+	struct device_node *np = of_get_cpu_node(policy->cpu, NULL);
+
+	if (of_find_property(np, "#cooling-cells", NULL)) {
+		cpud->cdev = of_cpufreq_cooling_register(np,
+							 policy->related_cpus);
+
+		if (IS_ERR(cpud->cdev)) {
+			pr_err("Failed to register cooling device cpu%d: %ld\n",
+					policy->cpu, PTR_ERR(cpud->cdev));
+
+			cpud->cdev = NULL;
+		}
+	}
+
+	of_node_put(np);
+}
+
 static struct cpufreq_driver qoriq_cpufreq_driver = {
 	.name		= "qoriq_cpufreq",
 	.flags		= CPUFREQ_CONST_LOOPS,
@@ -329,6 +352,7 @@ static struct cpufreq_driver qoriq_cpufreq_driver = {
 	.verify		= cpufreq_generic_frequency_table_verify,
 	.target_index	= qoriq_cpufreq_target,
 	.get		= cpufreq_generic_get,
+	.ready		= qoriq_cpufreq_ready,
 	.attr		= cpufreq_generic_attr,
 };
 
diff --git a/drivers/cpufreq/sti-cpufreq.c b/drivers/cpufreq/sti-cpufreq.c
new file mode 100644
index 000000000000..a9c659f58974
--- /dev/null
+++ b/drivers/cpufreq/sti-cpufreq.c
@@ -0,0 +1,294 @@
+/*
+ * Match running platform with pre-defined OPP values for CPUFreq
+ *
+ * Author: Ajit Pal Singh <ajitpal.singh@st.com>
+ *         Lee Jones <lee.jones@linaro.org>
+ *
+ * Copyright (C) 2015 STMicroelectronics (R&D) Limited
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the version 2 of the GNU General Public License as
+ * published by the Free Software Foundation
+ */
+
+#include <linux/cpu.h>
+#include <linux/io.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/pm_opp.h>
+#include <linux/regmap.h>
+
+#define VERSION_ELEMENTS	3
+#define MAX_PCODE_NAME_LEN	7
+
+#define VERSION_SHIFT		28
+#define HW_INFO_INDEX		1
+#define MAJOR_ID_INDEX		1
+#define MINOR_ID_INDEX		2
+
+/*
+ * Only match on "suitable for ALL versions" entries
+ *
+ * This will be used with the BIT() macro.  It sets the
+ * top bit of a 32bit value and is equal to 0x80000000.
+ */
+#define DEFAULT_VERSION		31
+
+enum {
+	PCODE = 0,
+	SUBSTRATE,
+	DVFS_MAX_REGFIELDS,
+};
+
+/**
+ * ST CPUFreq Driver Data
+ *
+ * @cpu_node		CPU's OF node
+ * @syscfg_eng		Engineering Syscon register map
+ * @regmap		Syscon register map
+ */
+static struct sti_cpufreq_ddata {
+	struct device *cpu;
+	struct regmap *syscfg_eng;
+	struct regmap *syscfg;
+} ddata;
+
+static int sti_cpufreq_fetch_major(void) {
+	struct device_node *np = ddata.cpu->of_node;
+	struct device *dev = ddata.cpu;
+	unsigned int major_offset;
+	unsigned int socid;
+	int ret;
+
+	ret = of_property_read_u32_index(np, "st,syscfg",
+					 MAJOR_ID_INDEX, &major_offset);
+	if (ret) {
+		dev_err(dev, "No major number offset provided in %s [%d]\n",
+			np->full_name, ret);
+		return ret;
+	}
+
+	ret = regmap_read(ddata.syscfg, major_offset, &socid);
+	if (ret) {
+		dev_err(dev, "Failed to read major number from syscon [%d]\n",
+			ret);
+		return ret;
+	}
+
+	return ((socid >> VERSION_SHIFT) & 0xf) + 1;
+}
+
+static int sti_cpufreq_fetch_minor(void)
+{
+	struct device *dev = ddata.cpu;
+	struct device_node *np = dev->of_node;
+	unsigned int minor_offset;
+	unsigned int minid;
+	int ret;
+
+	ret = of_property_read_u32_index(np, "st,syscfg-eng",
+					 MINOR_ID_INDEX, &minor_offset);
+	if (ret) {
+		dev_err(dev,
+			"No minor number offset provided %s [%d]\n",
+			np->full_name, ret);
+		return ret;
+	}
+
+	ret = regmap_read(ddata.syscfg_eng, minor_offset, &minid);
+	if (ret) {
+		dev_err(dev,
+			"Failed to read the minor number from syscon [%d]\n",
+			ret);
+		return ret;
+	}
+
+	return minid & 0xf;
+}
+
+static int sti_cpufreq_fetch_regmap_field(const struct reg_field *reg_fields,
+					  int hw_info_offset, int field)
+{
+	struct regmap_field *regmap_field;
+	struct reg_field reg_field = reg_fields[field];
+	struct device *dev = ddata.cpu;
+	unsigned int value;
+	int ret;
+
+	reg_field.reg = hw_info_offset;
+	regmap_field = devm_regmap_field_alloc(dev,
+					       ddata.syscfg_eng,
+					       reg_field);
+	if (IS_ERR(regmap_field)) {
+		dev_err(dev, "Failed to allocate reg field\n");
+		return PTR_ERR(regmap_field);
+	}
+
+	ret = regmap_field_read(regmap_field, &value);
+	if (ret) {
+		dev_err(dev, "Failed to read %s code\n",
+			field ? "SUBSTRATE" : "PCODE");
+		return ret;
+	}
+
+	return value;
+}
+
+static const struct reg_field sti_stih407_dvfs_regfields[DVFS_MAX_REGFIELDS] = {
+	[PCODE]		= REG_FIELD(0, 16, 19),
+	[SUBSTRATE]	= REG_FIELD(0, 0, 2),
+};
+
+static const struct reg_field *sti_cpufreq_match(void)
+{
+	if (of_machine_is_compatible("st,stih407") ||
+	    of_machine_is_compatible("st,stih410"))
+		return sti_stih407_dvfs_regfields;
+
+	return NULL;
+}
+
+static int sti_cpufreq_set_opp_info(void)
+{
+	struct device *dev = ddata.cpu;
+	struct device_node *np = dev->of_node;
+	const struct reg_field *reg_fields;
+	unsigned int hw_info_offset;
+	unsigned int version[VERSION_ELEMENTS];
+	int pcode, substrate, major, minor;
+	int ret;
+	char name[MAX_PCODE_NAME_LEN];
+
+	reg_fields = sti_cpufreq_match();
+	if (!reg_fields) {
+		dev_err(dev, "This SoC doesn't support voltage scaling");
+		return -ENODEV;
+	}
+
+	ret = of_property_read_u32_index(np, "st,syscfg-eng",
+					 HW_INFO_INDEX, &hw_info_offset);
+	if (ret) {
+		dev_warn(dev, "Failed to read HW info offset from DT\n");
+		substrate = DEFAULT_VERSION;
+		pcode = 0;
+		goto use_defaults;
+	}
+
+	pcode = sti_cpufreq_fetch_regmap_field(reg_fields,
+					       hw_info_offset,
+					       PCODE);
+	if (pcode < 0) {
+		dev_warn(dev, "Failed to obtain process code\n");
+		/* Use default pcode */
+		pcode = 0;
+	}
+
+	substrate = sti_cpufreq_fetch_regmap_field(reg_fields,
+						   hw_info_offset,
+						   SUBSTRATE);
+	if (substrate) {
+		dev_warn(dev, "Failed to obtain substrate code\n");
+		/* Use default substrate */
+		substrate = DEFAULT_VERSION;
+	}
+
+use_defaults:
+	major = sti_cpufreq_fetch_major();
+	if (major < 0) {
+		dev_err(dev, "Failed to obtain major version\n");
+		/* Use default major number */
+		major = DEFAULT_VERSION;
+	}
+
+	minor = sti_cpufreq_fetch_minor();
+	if (minor < 0) {
+		dev_err(dev, "Failed to obtain minor version\n");
+		/* Use default minor number */
+		minor = DEFAULT_VERSION;
+	}
+
+	snprintf(name, MAX_PCODE_NAME_LEN, "pcode%d", pcode);
+
+	ret = dev_pm_opp_set_prop_name(dev, name);
+	if (ret) {
+		dev_err(dev, "Failed to set prop name\n");
+		return ret;
+	}
+
+	version[0] = BIT(major);
+	version[1] = BIT(minor);
+	version[2] = BIT(substrate);
+
+	ret = dev_pm_opp_set_supported_hw(dev, version, VERSION_ELEMENTS);
+	if (ret) {
+		dev_err(dev, "Failed to set supported hardware\n");
+		return ret;
+	}
+
+	dev_dbg(dev, "pcode: %d major: %d minor: %d substrate: %d\n",
+		pcode, major, minor, substrate);
+	dev_dbg(dev, "version[0]: %x version[1]: %x version[2]: %x\n",
+		version[0], version[1], version[2]);
+
+	return 0;
+}
+
+static int sti_cpufreq_fetch_syscon_regsiters(void)
+{
+	struct device *dev = ddata.cpu;
+	struct device_node *np = dev->of_node;
+
+	ddata.syscfg = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(ddata.syscfg)) {
+		dev_err(dev,  "\"st,syscfg\" not supplied\n");
+		return PTR_ERR(ddata.syscfg);
+	}
+
+	ddata.syscfg_eng = syscon_regmap_lookup_by_phandle(np, "st,syscfg-eng");
+	if (IS_ERR(ddata.syscfg_eng)) {
+		dev_err(dev, "\"st,syscfg-eng\" not supplied\n");
+		return PTR_ERR(ddata.syscfg_eng);
+	}
+
+	return 0;
+}
+
+static int sti_cpufreq_init(void)
+{
+	int ret;
+
+	ddata.cpu = get_cpu_device(0);
+	if (!ddata.cpu) {
+		dev_err(ddata.cpu, "Failed to get device for CPU0\n");
+		goto skip_voltage_scaling;
+	}
+
+	if (!of_get_property(ddata.cpu->of_node, "operating-points-v2", NULL)) {
+		dev_err(ddata.cpu, "OPP-v2 not supported\n");
+		goto skip_voltage_scaling;
+	}
+
+	ret = sti_cpufreq_fetch_syscon_regsiters();
+	if (ret)
+		goto skip_voltage_scaling;
+
+	ret = sti_cpufreq_set_opp_info();
+	if (!ret)
+		goto register_cpufreq_dt;
+
+skip_voltage_scaling:
+	dev_err(ddata.cpu, "Not doing voltage scaling\n");
+
+register_cpufreq_dt:
+	platform_device_register_simple("cpufreq-dt", -1, NULL, 0);
+
+	return 0;
+}
+module_init(sti_cpufreq_init);
+
+MODULE_DESCRIPTION("STMicroelectronics CPUFreq/OPP driver");
+MODULE_AUTHOR("Ajitpal Singh <ajitpal.singh@st.com>");
+MODULE_AUTHOR("Lee Jones <lee.jones@linaro.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index 8c7930b5a65f..7e48eb5bf0a7 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -19,11 +19,9 @@ config CPU_IDLE_MULTIPLE_DRIVERS
 
 config CPU_IDLE_GOV_LADDER
 	bool "Ladder governor (for periodic timer tick)"
-	default y
 
 config CPU_IDLE_GOV_MENU
 	bool "Menu governor (for tickless system)"
-	default y
 
 config DT_IDLE_STATES
 	bool
diff --git a/drivers/cpuidle/coupled.c b/drivers/cpuidle/coupled.c
index 344058f8501a..d5657d50ac40 100644
--- a/drivers/cpuidle/coupled.c
+++ b/drivers/cpuidle/coupled.c
@@ -119,7 +119,6 @@ struct cpuidle_coupled {
 
 #define CPUIDLE_COUPLED_NOT_IDLE	(-1)
 
-static DEFINE_MUTEX(cpuidle_coupled_lock);
 static DEFINE_PER_CPU(struct call_single_data, cpuidle_coupled_poke_cb);
 
 /*
diff --git a/drivers/cpuidle/cpuidle-clps711x.c b/drivers/cpuidle/cpuidle-clps711x.c
index 18a7f7380508..66a9f231ec41 100644
--- a/drivers/cpuidle/cpuidle-clps711x.c
+++ b/drivers/cpuidle/cpuidle-clps711x.c
@@ -12,7 +12,7 @@
 #include <linux/cpuidle.h>
 #include <linux/err.h>
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/platform_device.h>
 
 #define CLPS711X_CPUIDLE_NAME	"clps711x-cpuidle"
@@ -56,8 +56,4 @@ static struct platform_driver clps711x_cpuidle_driver = {
 		.name	= CLPS711X_CPUIDLE_NAME,
 	},
 };
-module_platform_driver_probe(clps711x_cpuidle_driver, clps711x_cpuidle_probe);
-
-MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
-MODULE_DESCRIPTION("CLPS711X CPU idle driver");
-MODULE_LICENSE("GPL");
+builtin_platform_driver_probe(clps711x_cpuidle_driver, clps711x_cpuidle_probe);
diff --git a/drivers/cpuidle/cpuidle-exynos.c b/drivers/cpuidle/cpuidle-exynos.c
index b5f0a9cc8185..00cd129b10a4 100644
--- a/drivers/cpuidle/cpuidle-exynos.c
+++ b/drivers/cpuidle/cpuidle-exynos.c
@@ -14,7 +14,7 @@
 #include <linux/cpuidle.h>
 #include <linux/cpu_pm.h>
 #include <linux/export.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
 #include <linux/platform_data/cpuidle-exynos.h>
@@ -142,5 +142,4 @@ static struct platform_driver exynos_cpuidle_driver = {
 		.name = "exynos_cpuidle",
 	},
 };
-
-module_platform_driver(exynos_cpuidle_driver);
+builtin_platform_driver(exynos_cpuidle_driver);
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index 845bafcfa792..e12dc30d8864 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -264,7 +264,7 @@ static int powernv_idle_probe(void)
 	if (cpuidle_disable != IDLE_NO_OVERRIDE)
 		return -ENODEV;
 
-	if (firmware_has_feature(FW_FEATURE_OPALv3)) {
+	if (firmware_has_feature(FW_FEATURE_OPAL)) {
 		cpuidle_state_table = powernv_states;
 		/* Device tree can indicate more idle states */
 		max_idle_state = powernv_add_idle_states();
diff --git a/drivers/cpuidle/cpuidle-ux500.c b/drivers/cpuidle/cpuidle-ux500.c
index 8bf895c0017d..7941a090bea6 100644
--- a/drivers/cpuidle/cpuidle-ux500.c
+++ b/drivers/cpuidle/cpuidle-ux500.c
@@ -9,7 +9,7 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/cpuidle.h>
 #include <linux/spinlock.h>
 #include <linux/atomic.h>
@@ -124,5 +124,4 @@ static struct platform_driver dbx500_cpuidle_plat_driver = {
 	},
 	.probe = dbx500_cpuidle_probe,
 };
-
-module_platform_driver(dbx500_cpuidle_plat_driver);
+builtin_platform_driver(dbx500_cpuidle_plat_driver);
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index 17a6dc0e2111..f996efc56605 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -79,9 +79,9 @@ static int find_deepest_state(struct cpuidle_driver *drv,
 			      bool freeze)
 {
 	unsigned int latency_req = 0;
-	int i, ret = -ENXIO;
+	int i, ret = 0;
 
-	for (i = 0; i < drv->state_count; i++) {
+	for (i = 1; i < drv->state_count; i++) {
 		struct cpuidle_state *s = &drv->states[i];
 		struct cpuidle_state_usage *su = &dev->states_usage[i];
 
@@ -153,7 +153,7 @@ int cpuidle_enter_freeze(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	 * be frozen safely.
 	 */
 	index = find_deepest_state(drv, dev, UINT_MAX, 0, true);
-	if (index >= 0)
+	if (index > 0)
 		enter_freeze_proper(drv, dev, index);
 
 	return index;
@@ -243,7 +243,7 @@ int cpuidle_enter_state(struct cpuidle_device *dev, struct cpuidle_driver *drv,
  * @drv: the cpuidle driver
  * @dev: the cpuidle device
  *
- * Returns the index of the idle state.
+ * Returns the index of the idle state.  The return value must not be negative.
  */
 int cpuidle_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
diff --git a/drivers/cpuidle/governors/ladder.c b/drivers/cpuidle/governors/ladder.c
index 401c0106ed34..63bd5a403e22 100644
--- a/drivers/cpuidle/governors/ladder.c
+++ b/drivers/cpuidle/governors/ladder.c
@@ -17,6 +17,7 @@
 #include <linux/pm_qos.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
+#include <linux/tick.h>
 
 #include <asm/io.h>
 #include <asm/uaccess.h>
@@ -184,6 +185,14 @@ static struct cpuidle_governor ladder_governor = {
  */
 static int __init init_ladder(void)
 {
+	/*
+	 * When NO_HZ is disabled, or when booting with nohz=off, the ladder
+	 * governor is better so give it a higher rating than the menu
+	 * governor.
+	 */
+	if (!tick_nohz_enabled)
+		ladder_governor.rating = 25;
+
 	return cpuidle_register_governor(&ladder_governor);
 }
 
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index 22e4463d1787..0742b3296673 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -294,8 +294,6 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 		data->needs_update = 0;
 	}
 
-	data->last_state_idx = CPUIDLE_DRIVER_STATE_START - 1;
-
 	/* Special case when user has set very strict latency requirement */
 	if (unlikely(latency_req == 0))
 		return 0;
@@ -326,20 +324,25 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	if (latency_req > interactivity_req)
 		latency_req = interactivity_req;
 
-	/*
-	 * We want to default to C1 (hlt), not to busy polling
-	 * unless the timer is happening really really soon.
-	 */
-	if (data->next_timer_us > 5 &&
-	    !drv->states[CPUIDLE_DRIVER_STATE_START].disabled &&
-		dev->states_usage[CPUIDLE_DRIVER_STATE_START].disable == 0)
+	if (CPUIDLE_DRIVER_STATE_START > 0) {
+		data->last_state_idx = CPUIDLE_DRIVER_STATE_START - 1;
+		/*
+		 * We want to default to C1 (hlt), not to busy polling
+		 * unless the timer is happening really really soon.
+		 */
+		if (interactivity_req > 20 &&
+		    !drv->states[CPUIDLE_DRIVER_STATE_START].disabled &&
+			dev->states_usage[CPUIDLE_DRIVER_STATE_START].disable == 0)
+			data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
+	} else {
 		data->last_state_idx = CPUIDLE_DRIVER_STATE_START;
+	}
 
 	/*
 	 * Find the idle state with the lowest power while satisfying
 	 * our constraints.
 	 */
-	for (i = CPUIDLE_DRIVER_STATE_START; i < drv->state_count; i++) {
+	for (i = data->last_state_idx + 1; i < drv->state_count; i++) {
 		struct cpuidle_state *s = &drv->states[i];
 		struct cpuidle_state_usage *su = &dev->states_usage[i];
 
@@ -404,8 +407,10 @@ static void menu_update(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 	measured_us = cpuidle_get_last_residency(dev);
 
 	/* Deduct exit latency */
-	if (measured_us > target->exit_latency)
+	if (measured_us > 2 * target->exit_latency)
 		measured_us -= target->exit_latency;
+	else
+		measured_us /= 2;
 
 	/* Make sure our coefficients do not exceed unity */
 	if (measured_us > data->next_timer_us)
diff --git a/drivers/crypto/Kconfig b/drivers/crypto/Kconfig
index 2569e043317e..07d494276aad 100644
--- a/drivers/crypto/Kconfig
+++ b/drivers/crypto/Kconfig
@@ -194,6 +194,9 @@ config CRYPTO_DEV_NIAGARA2
        select CRYPTO_DES
        select CRYPTO_BLKCIPHER
        select CRYPTO_HASH
+       select CRYPTO_MD5
+       select CRYPTO_SHA1
+       select CRYPTO_SHA256
        depends on SPARC64
        help
 	  Each core of a Niagara2 processor contains a Stream
@@ -378,10 +381,11 @@ config CRYPTO_DEV_BFIN_CRC
 
 config CRYPTO_DEV_ATMEL_AES
 	tristate "Support for Atmel AES hw accelerator"
-	depends on ARCH_AT91
+	depends on HAS_DMA
+	depends on AT_XDMAC || AT_HDMAC || COMPILE_TEST
 	select CRYPTO_AES
+	select CRYPTO_AEAD
 	select CRYPTO_BLKCIPHER
-	select AT_HDMAC
 	help
 	  Some Atmel processors have AES hw accelerator.
 	  Select this if you want to use the Atmel module for
@@ -498,4 +502,15 @@ config CRYPTO_DEV_SUN4I_SS
 	  To compile this driver as a module, choose M here: the module
 	  will be called sun4i-ss.
 
+config CRYPTO_DEV_ROCKCHIP
+	tristate "Rockchip's Cryptographic Engine driver"
+	depends on OF && ARCH_ROCKCHIP
+	select CRYPTO_AES
+	select CRYPTO_DES
+	select CRYPTO_BLKCIPHER
+
+	help
+	  This driver interfaces with the hardware crypto accelerator.
+	  Supporting cbc/ecb chainmode, and aes/des/des3_ede cipher mode.
+
 endif # CRYPTO_HW
diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index c3ced6fbd1b8..713de9d11148 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -29,3 +29,4 @@ obj-$(CONFIG_CRYPTO_DEV_QAT) += qat/
 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/
diff --git a/drivers/crypto/amcc/crypto4xx_core.c b/drivers/crypto/amcc/crypto4xx_core.c
index 58a630e55d5d..62134c8a2260 100644
--- a/drivers/crypto/amcc/crypto4xx_core.c
+++ b/drivers/crypto/amcc/crypto4xx_core.c
@@ -781,6 +781,10 @@ u32 crypto4xx_build_pd(struct crypto_async_request *req,
 
 	/* figure how many gd is needed */
 	num_gd = sg_nents_for_len(src, datalen);
+	if ((int)num_gd < 0) {
+		dev_err(dev->core_dev->device, "Invalid number of src SG.\n");
+		return -EINVAL;
+	}
 	if (num_gd == 1)
 		num_gd = 0;
 
diff --git a/drivers/crypto/atmel-aes-regs.h b/drivers/crypto/atmel-aes-regs.h
index 2786bb1a5aa0..6c2951bb70b1 100644
--- a/drivers/crypto/atmel-aes-regs.h
+++ b/drivers/crypto/atmel-aes-regs.h
@@ -9,6 +9,7 @@
 #define	AES_MR			0x04
 #define AES_MR_CYPHER_DEC		(0 << 0)
 #define AES_MR_CYPHER_ENC		(1 << 0)
+#define AES_MR_GTAGEN			(1 << 1)
 #define	AES_MR_DUALBUFF			(1 << 3)
 #define AES_MR_PROCDLY_MASK		(0xF << 4)
 #define AES_MR_PROCDLY_OFFSET	4
@@ -26,6 +27,7 @@
 #define AES_MR_OPMOD_OFB		(0x2 << 12)
 #define AES_MR_OPMOD_CFB		(0x3 << 12)
 #define AES_MR_OPMOD_CTR		(0x4 << 12)
+#define AES_MR_OPMOD_GCM		(0x5 << 12)
 #define AES_MR_LOD				(0x1 << 15)
 #define AES_MR_CFBS_MASK		(0x7 << 16)
 #define AES_MR_CFBS_128b		(0x0 << 16)
@@ -44,6 +46,7 @@
 #define	AES_ISR		0x1C
 #define AES_INT_DATARDY		(1 << 0)
 #define AES_INT_URAD		(1 << 8)
+#define AES_INT_TAGRDY		(1 << 16)
 #define AES_ISR_URAT_MASK	(0xF << 12)
 #define AES_ISR_URAT_IDR_WR_PROC	(0x0 << 12)
 #define AES_ISR_URAT_ODR_RD_PROC	(0x1 << 12)
@@ -57,6 +60,13 @@
 #define AES_ODATAR(x)	(0x50 + ((x) * 0x04))
 #define AES_IVR(x)		(0x60 + ((x) * 0x04))
 
+#define AES_AADLENR	0x70
+#define AES_CLENR	0x74
+#define AES_GHASHR(x)	(0x78 + ((x) * 0x04))
+#define AES_TAGR(x)	(0x88 + ((x) * 0x04))
+#define AES_CTRR	0x98
+#define AES_GCMHR(x)	(0x9c + ((x) * 0x04))
+
 #define AES_HW_VERSION	0xFC
 
 #endif /* __ATMEL_AES_REGS_H__ */
diff --git a/drivers/crypto/atmel-aes.c b/drivers/crypto/atmel-aes.c
index fb16d812c8f5..3eb3f1279fb7 100644
--- a/drivers/crypto/atmel-aes.c
+++ b/drivers/crypto/atmel-aes.c
@@ -33,68 +33,118 @@
 #include <linux/of_device.h>
 #include <linux/delay.h>
 #include <linux/crypto.h>
-#include <linux/cryptohash.h>
 #include <crypto/scatterwalk.h>
 #include <crypto/algapi.h>
 #include <crypto/aes.h>
-#include <crypto/hash.h>
-#include <crypto/internal/hash.h>
+#include <crypto/internal/aead.h>
 #include <linux/platform_data/crypto-atmel.h>
 #include <dt-bindings/dma/at91.h>
 #include "atmel-aes-regs.h"
 
+#define ATMEL_AES_PRIORITY	300
+
+#define ATMEL_AES_BUFFER_ORDER	2
+#define ATMEL_AES_BUFFER_SIZE	(PAGE_SIZE << ATMEL_AES_BUFFER_ORDER)
+
 #define CFB8_BLOCK_SIZE		1
 #define CFB16_BLOCK_SIZE	2
 #define CFB32_BLOCK_SIZE	4
 #define CFB64_BLOCK_SIZE	8
 
+#define SIZE_IN_WORDS(x)	((x) >> 2)
+
 /* AES flags */
-#define AES_FLAGS_MODE_MASK	0x03ff
-#define AES_FLAGS_ENCRYPT	BIT(0)
-#define AES_FLAGS_CBC		BIT(1)
-#define AES_FLAGS_CFB		BIT(2)
-#define AES_FLAGS_CFB8		BIT(3)
-#define AES_FLAGS_CFB16		BIT(4)
-#define AES_FLAGS_CFB32		BIT(5)
-#define AES_FLAGS_CFB64		BIT(6)
-#define AES_FLAGS_CFB128	BIT(7)
-#define AES_FLAGS_OFB		BIT(8)
-#define AES_FLAGS_CTR		BIT(9)
-
-#define AES_FLAGS_INIT		BIT(16)
-#define AES_FLAGS_DMA		BIT(17)
-#define AES_FLAGS_BUSY		BIT(18)
-#define AES_FLAGS_FAST		BIT(19)
+/* Reserve bits [18:16] [14:12] [1:0] for mode (same as for AES_MR) */
+#define AES_FLAGS_ENCRYPT	AES_MR_CYPHER_ENC
+#define AES_FLAGS_GTAGEN	AES_MR_GTAGEN
+#define AES_FLAGS_OPMODE_MASK	(AES_MR_OPMOD_MASK | AES_MR_CFBS_MASK)
+#define AES_FLAGS_ECB		AES_MR_OPMOD_ECB
+#define AES_FLAGS_CBC		AES_MR_OPMOD_CBC
+#define AES_FLAGS_OFB		AES_MR_OPMOD_OFB
+#define AES_FLAGS_CFB128	(AES_MR_OPMOD_CFB | AES_MR_CFBS_128b)
+#define AES_FLAGS_CFB64		(AES_MR_OPMOD_CFB | AES_MR_CFBS_64b)
+#define AES_FLAGS_CFB32		(AES_MR_OPMOD_CFB | AES_MR_CFBS_32b)
+#define AES_FLAGS_CFB16		(AES_MR_OPMOD_CFB | AES_MR_CFBS_16b)
+#define AES_FLAGS_CFB8		(AES_MR_OPMOD_CFB | AES_MR_CFBS_8b)
+#define AES_FLAGS_CTR		AES_MR_OPMOD_CTR
+#define AES_FLAGS_GCM		AES_MR_OPMOD_GCM
+
+#define AES_FLAGS_MODE_MASK	(AES_FLAGS_OPMODE_MASK |	\
+				 AES_FLAGS_ENCRYPT |		\
+				 AES_FLAGS_GTAGEN)
+
+#define AES_FLAGS_INIT		BIT(2)
+#define AES_FLAGS_BUSY		BIT(3)
+#define AES_FLAGS_DUMP_REG	BIT(4)
+
+#define AES_FLAGS_PERSISTENT	(AES_FLAGS_INIT | AES_FLAGS_BUSY)
 
 #define ATMEL_AES_QUEUE_LENGTH	50
 
-#define ATMEL_AES_DMA_THRESHOLD		16
+#define ATMEL_AES_DMA_THRESHOLD		256
 
 
 struct atmel_aes_caps {
-	bool	has_dualbuff;
-	bool	has_cfb64;
-	u32		max_burst_size;
+	bool			has_dualbuff;
+	bool			has_cfb64;
+	bool			has_ctr32;
+	bool			has_gcm;
+	u32			max_burst_size;
 };
 
 struct atmel_aes_dev;
 
+
+typedef int (*atmel_aes_fn_t)(struct atmel_aes_dev *);
+
+
+struct atmel_aes_base_ctx {
+	struct atmel_aes_dev	*dd;
+	atmel_aes_fn_t		start;
+	int			keylen;
+	u32			key[AES_KEYSIZE_256 / sizeof(u32)];
+	u16			block_size;
+};
+
 struct atmel_aes_ctx {
-	struct atmel_aes_dev *dd;
+	struct atmel_aes_base_ctx	base;
+};
+
+struct atmel_aes_ctr_ctx {
+	struct atmel_aes_base_ctx	base;
+
+	u32			iv[AES_BLOCK_SIZE / sizeof(u32)];
+	size_t			offset;
+	struct scatterlist	src[2];
+	struct scatterlist	dst[2];
+};
 
-	int		keylen;
-	u32		key[AES_KEYSIZE_256 / sizeof(u32)];
+struct atmel_aes_gcm_ctx {
+	struct atmel_aes_base_ctx	base;
 
-	u16		block_size;
+	struct scatterlist	src[2];
+	struct scatterlist	dst[2];
+
+	u32			j0[AES_BLOCK_SIZE / sizeof(u32)];
+	u32			tag[AES_BLOCK_SIZE / sizeof(u32)];
+	u32			ghash[AES_BLOCK_SIZE / sizeof(u32)];
+	size_t			textlen;
+
+	const u32		*ghash_in;
+	u32			*ghash_out;
+	atmel_aes_fn_t		ghash_resume;
 };
 
 struct atmel_aes_reqctx {
-	unsigned long mode;
+	unsigned long		mode;
 };
 
 struct atmel_aes_dma {
-	struct dma_chan			*chan;
-	struct dma_slave_config dma_conf;
+	struct dma_chan		*chan;
+	struct scatterlist	*sg;
+	int			nents;
+	unsigned int		remainder;
+	unsigned int		sg_len;
 };
 
 struct atmel_aes_dev {
@@ -102,13 +152,18 @@ struct atmel_aes_dev {
 	unsigned long		phys_base;
 	void __iomem		*io_base;
 
-	struct atmel_aes_ctx	*ctx;
+	struct crypto_async_request	*areq;
+	struct atmel_aes_base_ctx	*ctx;
+
+	bool			is_async;
+	atmel_aes_fn_t		resume;
+	atmel_aes_fn_t		cpu_transfer_complete;
+
 	struct device		*dev;
 	struct clk		*iclk;
-	int	irq;
+	int			irq;
 
 	unsigned long		flags;
-	int	err;
 
 	spinlock_t		lock;
 	struct crypto_queue	queue;
@@ -116,33 +171,21 @@ struct atmel_aes_dev {
 	struct tasklet_struct	done_task;
 	struct tasklet_struct	queue_task;
 
-	struct ablkcipher_request	*req;
-	size_t	total;
+	size_t			total;
+	size_t			datalen;
+	u32			*data;
 
-	struct scatterlist	*in_sg;
-	unsigned int		nb_in_sg;
-	size_t				in_offset;
-	struct scatterlist	*out_sg;
-	unsigned int		nb_out_sg;
-	size_t				out_offset;
+	struct atmel_aes_dma	src;
+	struct atmel_aes_dma	dst;
 
-	size_t	bufcnt;
-	size_t	buflen;
-	size_t	dma_size;
-
-	void	*buf_in;
-	int		dma_in;
-	dma_addr_t	dma_addr_in;
-	struct atmel_aes_dma	dma_lch_in;
-
-	void	*buf_out;
-	int		dma_out;
-	dma_addr_t	dma_addr_out;
-	struct atmel_aes_dma	dma_lch_out;
+	size_t			buflen;
+	void			*buf;
+	struct scatterlist	aligned_sg;
+	struct scatterlist	*real_dst;
 
 	struct atmel_aes_caps	caps;
 
-	u32	hw_version;
+	u32			hw_version;
 };
 
 struct atmel_aes_drv {
@@ -155,71 +198,129 @@ static struct atmel_aes_drv atmel_aes = {
 	.lock = __SPIN_LOCK_UNLOCKED(atmel_aes.lock),
 };
 
-static int atmel_aes_sg_length(struct ablkcipher_request *req,
-			struct scatterlist *sg)
+#ifdef VERBOSE_DEBUG
+static const char *atmel_aes_reg_name(u32 offset, char *tmp, size_t sz)
 {
-	unsigned int total = req->nbytes;
-	int sg_nb;
-	unsigned int len;
-	struct scatterlist *sg_list;
-
-	sg_nb = 0;
-	sg_list = sg;
-	total = req->nbytes;
+	switch (offset) {
+	case AES_CR:
+		return "CR";
+
+	case AES_MR:
+		return "MR";
+
+	case AES_ISR:
+		return "ISR";
+
+	case AES_IMR:
+		return "IMR";
+
+	case AES_IER:
+		return "IER";
+
+	case AES_IDR:
+		return "IDR";
+
+	case AES_KEYWR(0):
+	case AES_KEYWR(1):
+	case AES_KEYWR(2):
+	case AES_KEYWR(3):
+	case AES_KEYWR(4):
+	case AES_KEYWR(5):
+	case AES_KEYWR(6):
+	case AES_KEYWR(7):
+		snprintf(tmp, sz, "KEYWR[%u]", (offset - AES_KEYWR(0)) >> 2);
+		break;
 
-	while (total) {
-		len = min(sg_list->length, total);
+	case AES_IDATAR(0):
+	case AES_IDATAR(1):
+	case AES_IDATAR(2):
+	case AES_IDATAR(3):
+		snprintf(tmp, sz, "IDATAR[%u]", (offset - AES_IDATAR(0)) >> 2);
+		break;
 
-		sg_nb++;
-		total -= len;
+	case AES_ODATAR(0):
+	case AES_ODATAR(1):
+	case AES_ODATAR(2):
+	case AES_ODATAR(3):
+		snprintf(tmp, sz, "ODATAR[%u]", (offset - AES_ODATAR(0)) >> 2);
+		break;
 
-		sg_list = sg_next(sg_list);
-		if (!sg_list)
-			total = 0;
-	}
+	case AES_IVR(0):
+	case AES_IVR(1):
+	case AES_IVR(2):
+	case AES_IVR(3):
+		snprintf(tmp, sz, "IVR[%u]", (offset - AES_IVR(0)) >> 2);
+		break;
 
-	return sg_nb;
-}
+	case AES_AADLENR:
+		return "AADLENR";
 
-static int atmel_aes_sg_copy(struct scatterlist **sg, size_t *offset,
-			void *buf, size_t buflen, size_t total, int out)
-{
-	unsigned int count, off = 0;
+	case AES_CLENR:
+		return "CLENR";
 
-	while (buflen && total) {
-		count = min((*sg)->length - *offset, total);
-		count = min(count, buflen);
+	case AES_GHASHR(0):
+	case AES_GHASHR(1):
+	case AES_GHASHR(2):
+	case AES_GHASHR(3):
+		snprintf(tmp, sz, "GHASHR[%u]", (offset - AES_GHASHR(0)) >> 2);
+		break;
 
-		if (!count)
-			return off;
+	case AES_TAGR(0):
+	case AES_TAGR(1):
+	case AES_TAGR(2):
+	case AES_TAGR(3):
+		snprintf(tmp, sz, "TAGR[%u]", (offset - AES_TAGR(0)) >> 2);
+		break;
 
-		scatterwalk_map_and_copy(buf + off, *sg, *offset, count, out);
+	case AES_CTRR:
+		return "CTRR";
 
-		off += count;
-		buflen -= count;
-		*offset += count;
-		total -= count;
+	case AES_GCMHR(0):
+	case AES_GCMHR(1):
+	case AES_GCMHR(2):
+	case AES_GCMHR(3):
+		snprintf(tmp, sz, "GCMHR[%u]", (offset - AES_GCMHR(0)) >> 2);
+		break;
 
-		if (*offset == (*sg)->length) {
-			*sg = sg_next(*sg);
-			if (*sg)
-				*offset = 0;
-			else
-				total = 0;
-		}
+	default:
+		snprintf(tmp, sz, "0x%02x", offset);
+		break;
 	}
 
-	return off;
+	return tmp;
 }
+#endif /* VERBOSE_DEBUG */
+
+/* Shared functions */
 
 static inline u32 atmel_aes_read(struct atmel_aes_dev *dd, u32 offset)
 {
-	return readl_relaxed(dd->io_base + offset);
+	u32 value = readl_relaxed(dd->io_base + offset);
+
+#ifdef VERBOSE_DEBUG
+	if (dd->flags & AES_FLAGS_DUMP_REG) {
+		char tmp[16];
+
+		dev_vdbg(dd->dev, "read 0x%08x from %s\n", value,
+			 atmel_aes_reg_name(offset, tmp, sizeof(tmp)));
+	}
+#endif /* VERBOSE_DEBUG */
+
+	return value;
 }
 
 static inline void atmel_aes_write(struct atmel_aes_dev *dd,
 					u32 offset, u32 value)
 {
+#ifdef VERBOSE_DEBUG
+	if (dd->flags & AES_FLAGS_DUMP_REG) {
+		char tmp[16];
+
+		dev_vdbg(dd->dev, "write 0x%08x into %s\n", value,
+			 atmel_aes_reg_name(offset, tmp));
+	}
+#endif /* VERBOSE_DEBUG */
+
 	writel_relaxed(value, dd->io_base + offset);
 }
 
@@ -231,13 +332,50 @@ static void atmel_aes_read_n(struct atmel_aes_dev *dd, u32 offset,
 }
 
 static void atmel_aes_write_n(struct atmel_aes_dev *dd, u32 offset,
-					u32 *value, int count)
+			      const u32 *value, int count)
 {
 	for (; count--; value++, offset += 4)
 		atmel_aes_write(dd, offset, *value);
 }
 
-static struct atmel_aes_dev *atmel_aes_find_dev(struct atmel_aes_ctx *ctx)
+static inline void atmel_aes_read_block(struct atmel_aes_dev *dd, u32 offset,
+					u32 *value)
+{
+	atmel_aes_read_n(dd, offset, value, SIZE_IN_WORDS(AES_BLOCK_SIZE));
+}
+
+static inline void atmel_aes_write_block(struct atmel_aes_dev *dd, u32 offset,
+					 const u32 *value)
+{
+	atmel_aes_write_n(dd, offset, value, SIZE_IN_WORDS(AES_BLOCK_SIZE));
+}
+
+static inline int atmel_aes_wait_for_data_ready(struct atmel_aes_dev *dd,
+						atmel_aes_fn_t resume)
+{
+	u32 isr = atmel_aes_read(dd, AES_ISR);
+
+	if (unlikely(isr & AES_INT_DATARDY))
+		return resume(dd);
+
+	dd->resume = resume;
+	atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
+	return -EINPROGRESS;
+}
+
+static inline size_t atmel_aes_padlen(size_t len, size_t block_size)
+{
+	len &= block_size - 1;
+	return len ? block_size - len : 0;
+}
+
+static inline struct aead_request *
+aead_request_cast(struct crypto_async_request *req)
+{
+	return container_of(req, struct aead_request, base);
+}
+
+static struct atmel_aes_dev *atmel_aes_find_dev(struct atmel_aes_base_ctx *ctx)
 {
 	struct atmel_aes_dev *aes_dd = NULL;
 	struct atmel_aes_dev *tmp;
@@ -262,7 +400,7 @@ static int atmel_aes_hw_init(struct atmel_aes_dev *dd)
 {
 	int err;
 
-	err = clk_prepare_enable(dd->iclk);
+	err = clk_enable(dd->iclk);
 	if (err)
 		return err;
 
@@ -270,7 +408,6 @@ static int atmel_aes_hw_init(struct atmel_aes_dev *dd)
 		atmel_aes_write(dd, AES_CR, AES_CR_SWRST);
 		atmel_aes_write(dd, AES_MR, 0xE << AES_MR_CKEY_OFFSET);
 		dd->flags |= AES_FLAGS_INIT;
-		dd->err = 0;
 	}
 
 	return 0;
@@ -281,552 +418,643 @@ static inline unsigned int atmel_aes_get_version(struct atmel_aes_dev *dd)
 	return atmel_aes_read(dd, AES_HW_VERSION) & 0x00000fff;
 }
 
-static void atmel_aes_hw_version_init(struct atmel_aes_dev *dd)
+static int atmel_aes_hw_version_init(struct atmel_aes_dev *dd)
 {
-	atmel_aes_hw_init(dd);
+	int err;
+
+	err = atmel_aes_hw_init(dd);
+	if (err)
+		return err;
 
 	dd->hw_version = atmel_aes_get_version(dd);
 
-	dev_info(dd->dev,
-			"version: 0x%x\n", dd->hw_version);
+	dev_info(dd->dev, "version: 0x%x\n", dd->hw_version);
 
-	clk_disable_unprepare(dd->iclk);
+	clk_disable(dd->iclk);
+	return 0;
 }
 
-static void atmel_aes_finish_req(struct atmel_aes_dev *dd, int err)
+static inline void atmel_aes_set_mode(struct atmel_aes_dev *dd,
+				      const struct atmel_aes_reqctx *rctx)
 {
-	struct ablkcipher_request *req = dd->req;
-
-	clk_disable_unprepare(dd->iclk);
-	dd->flags &= ~AES_FLAGS_BUSY;
+	/* Clear all but persistent flags and set request flags. */
+	dd->flags = (dd->flags & AES_FLAGS_PERSISTENT) | rctx->mode;
+}
 
-	req->base.complete(&req->base, err);
+static inline bool atmel_aes_is_encrypt(const struct atmel_aes_dev *dd)
+{
+	return (dd->flags & AES_FLAGS_ENCRYPT);
 }
 
-static void atmel_aes_dma_callback(void *data)
+static inline int atmel_aes_complete(struct atmel_aes_dev *dd, int err)
 {
-	struct atmel_aes_dev *dd = data;
+	clk_disable(dd->iclk);
+	dd->flags &= ~AES_FLAGS_BUSY;
+
+	if (dd->is_async)
+		dd->areq->complete(dd->areq, err);
+
+	tasklet_schedule(&dd->queue_task);
 
-	/* dma_lch_out - completed */
-	tasklet_schedule(&dd->done_task);
+	return err;
 }
 
-static int atmel_aes_crypt_dma(struct atmel_aes_dev *dd,
-		dma_addr_t dma_addr_in, dma_addr_t dma_addr_out, int length)
+static void atmel_aes_write_ctrl(struct atmel_aes_dev *dd, bool use_dma,
+				 const u32 *iv)
 {
-	struct scatterlist sg[2];
-	struct dma_async_tx_descriptor	*in_desc, *out_desc;
+	u32 valmr = 0;
 
-	dd->dma_size = length;
+	/* MR register must be set before IV registers */
+	if (dd->ctx->keylen == AES_KEYSIZE_128)
+		valmr |= AES_MR_KEYSIZE_128;
+	else if (dd->ctx->keylen == AES_KEYSIZE_192)
+		valmr |= AES_MR_KEYSIZE_192;
+	else
+		valmr |= AES_MR_KEYSIZE_256;
 
-	dma_sync_single_for_device(dd->dev, dma_addr_in, length,
-				   DMA_TO_DEVICE);
-	dma_sync_single_for_device(dd->dev, dma_addr_out, length,
-				   DMA_FROM_DEVICE);
+	valmr |= dd->flags & AES_FLAGS_MODE_MASK;
 
-	if (dd->flags & AES_FLAGS_CFB8) {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_1_BYTE;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_1_BYTE;
-	} else if (dd->flags & AES_FLAGS_CFB16) {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_2_BYTES;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_2_BYTES;
+	if (use_dma) {
+		valmr |= AES_MR_SMOD_IDATAR0;
+		if (dd->caps.has_dualbuff)
+			valmr |= AES_MR_DUALBUFF;
 	} else {
-		dd->dma_lch_in.dma_conf.dst_addr_width =
-			DMA_SLAVE_BUSWIDTH_4_BYTES;
-		dd->dma_lch_out.dma_conf.src_addr_width =
-			DMA_SLAVE_BUSWIDTH_4_BYTES;
+		valmr |= AES_MR_SMOD_AUTO;
 	}
 
-	if (dd->flags & (AES_FLAGS_CFB8 | AES_FLAGS_CFB16 |
-			AES_FLAGS_CFB32 | AES_FLAGS_CFB64)) {
-		dd->dma_lch_in.dma_conf.src_maxburst = 1;
-		dd->dma_lch_in.dma_conf.dst_maxburst = 1;
-		dd->dma_lch_out.dma_conf.src_maxburst = 1;
-		dd->dma_lch_out.dma_conf.dst_maxburst = 1;
-	} else {
-		dd->dma_lch_in.dma_conf.src_maxburst = dd->caps.max_burst_size;
-		dd->dma_lch_in.dma_conf.dst_maxburst = dd->caps.max_burst_size;
-		dd->dma_lch_out.dma_conf.src_maxburst = dd->caps.max_burst_size;
-		dd->dma_lch_out.dma_conf.dst_maxburst = dd->caps.max_burst_size;
-	}
+	atmel_aes_write(dd, AES_MR, valmr);
 
-	dmaengine_slave_config(dd->dma_lch_in.chan, &dd->dma_lch_in.dma_conf);
-	dmaengine_slave_config(dd->dma_lch_out.chan, &dd->dma_lch_out.dma_conf);
+	atmel_aes_write_n(dd, AES_KEYWR(0), dd->ctx->key,
+			  SIZE_IN_WORDS(dd->ctx->keylen));
 
-	dd->flags |= AES_FLAGS_DMA;
+	if (iv && (valmr & AES_MR_OPMOD_MASK) != AES_MR_OPMOD_ECB)
+		atmel_aes_write_block(dd, AES_IVR(0), iv);
+}
 
-	sg_init_table(&sg[0], 1);
-	sg_dma_address(&sg[0]) = dma_addr_in;
-	sg_dma_len(&sg[0]) = length;
 
-	sg_init_table(&sg[1], 1);
-	sg_dma_address(&sg[1]) = dma_addr_out;
-	sg_dma_len(&sg[1]) = length;
+/* CPU transfer */
 
-	in_desc = dmaengine_prep_slave_sg(dd->dma_lch_in.chan, &sg[0],
-				1, DMA_MEM_TO_DEV,
-				DMA_PREP_INTERRUPT  |  DMA_CTRL_ACK);
-	if (!in_desc)
-		return -EINVAL;
+static int atmel_aes_cpu_transfer(struct atmel_aes_dev *dd)
+{
+	int err = 0;
+	u32 isr;
 
-	out_desc = dmaengine_prep_slave_sg(dd->dma_lch_out.chan, &sg[1],
-				1, DMA_DEV_TO_MEM,
-				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-	if (!out_desc)
-		return -EINVAL;
+	for (;;) {
+		atmel_aes_read_block(dd, AES_ODATAR(0), dd->data);
+		dd->data += 4;
+		dd->datalen -= AES_BLOCK_SIZE;
 
-	out_desc->callback = atmel_aes_dma_callback;
-	out_desc->callback_param = dd;
+		if (dd->datalen < AES_BLOCK_SIZE)
+			break;
 
-	dmaengine_submit(out_desc);
-	dma_async_issue_pending(dd->dma_lch_out.chan);
+		atmel_aes_write_block(dd, AES_IDATAR(0), dd->data);
 
-	dmaengine_submit(in_desc);
-	dma_async_issue_pending(dd->dma_lch_in.chan);
+		isr = atmel_aes_read(dd, AES_ISR);
+		if (!(isr & AES_INT_DATARDY)) {
+			dd->resume = atmel_aes_cpu_transfer;
+			atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
+			return -EINPROGRESS;
+		}
+	}
 
-	return 0;
+	if (!sg_copy_from_buffer(dd->real_dst, sg_nents(dd->real_dst),
+				 dd->buf, dd->total))
+		err = -EINVAL;
+
+	if (err)
+		return atmel_aes_complete(dd, err);
+
+	return dd->cpu_transfer_complete(dd);
 }
 
-static int atmel_aes_crypt_cpu_start(struct atmel_aes_dev *dd)
+static int atmel_aes_cpu_start(struct atmel_aes_dev *dd,
+			       struct scatterlist *src,
+			       struct scatterlist *dst,
+			       size_t len,
+			       atmel_aes_fn_t resume)
 {
-	dd->flags &= ~AES_FLAGS_DMA;
+	size_t padlen = atmel_aes_padlen(len, AES_BLOCK_SIZE);
 
-	dma_sync_single_for_cpu(dd->dev, dd->dma_addr_in,
-				dd->dma_size, DMA_TO_DEVICE);
-	dma_sync_single_for_cpu(dd->dev, dd->dma_addr_out,
-				dd->dma_size, DMA_FROM_DEVICE);
-
-	/* use cache buffers */
-	dd->nb_in_sg = atmel_aes_sg_length(dd->req, dd->in_sg);
-	if (!dd->nb_in_sg)
+	if (unlikely(len == 0))
 		return -EINVAL;
 
-	dd->nb_out_sg = atmel_aes_sg_length(dd->req, dd->out_sg);
-	if (!dd->nb_out_sg)
-		return -EINVAL;
+	sg_copy_to_buffer(src, sg_nents(src), dd->buf, len);
 
-	dd->bufcnt = sg_copy_to_buffer(dd->in_sg, dd->nb_in_sg,
-					dd->buf_in, dd->total);
+	dd->total = len;
+	dd->real_dst = dst;
+	dd->cpu_transfer_complete = resume;
+	dd->datalen = len + padlen;
+	dd->data = (u32 *)dd->buf;
+	atmel_aes_write_block(dd, AES_IDATAR(0), dd->data);
+	return atmel_aes_wait_for_data_ready(dd, atmel_aes_cpu_transfer);
+}
 
-	if (!dd->bufcnt)
-		return -EINVAL;
 
-	dd->total -= dd->bufcnt;
+/* DMA transfer */
 
-	atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
-	atmel_aes_write_n(dd, AES_IDATAR(0), (u32 *) dd->buf_in,
-				dd->bufcnt >> 2);
+static void atmel_aes_dma_callback(void *data);
 
-	return 0;
+static bool atmel_aes_check_aligned(struct atmel_aes_dev *dd,
+				    struct scatterlist *sg,
+				    size_t len,
+				    struct atmel_aes_dma *dma)
+{
+	int nents;
+
+	if (!IS_ALIGNED(len, dd->ctx->block_size))
+		return false;
+
+	for (nents = 0; sg; sg = sg_next(sg), ++nents) {
+		if (!IS_ALIGNED(sg->offset, sizeof(u32)))
+			return false;
+
+		if (len <= sg->length) {
+			if (!IS_ALIGNED(len, dd->ctx->block_size))
+				return false;
+
+			dma->nents = nents+1;
+			dma->remainder = sg->length - len;
+			sg->length = len;
+			return true;
+		}
+
+		if (!IS_ALIGNED(sg->length, dd->ctx->block_size))
+			return false;
+
+		len -= sg->length;
+	}
+
+	return false;
 }
 
-static int atmel_aes_crypt_dma_start(struct atmel_aes_dev *dd)
+static inline void atmel_aes_restore_sg(const struct atmel_aes_dma *dma)
 {
-	int err, fast = 0, in, out;
-	size_t count;
-	dma_addr_t addr_in, addr_out;
+	struct scatterlist *sg = dma->sg;
+	int nents = dma->nents;
 
-	if ((!dd->in_offset) && (!dd->out_offset)) {
-		/* check for alignment */
-		in = IS_ALIGNED((u32)dd->in_sg->offset, sizeof(u32)) &&
-			IS_ALIGNED(dd->in_sg->length, dd->ctx->block_size);
-		out = IS_ALIGNED((u32)dd->out_sg->offset, sizeof(u32)) &&
-			IS_ALIGNED(dd->out_sg->length, dd->ctx->block_size);
-		fast = in && out;
+	if (!dma->remainder)
+		return;
 
-		if (sg_dma_len(dd->in_sg) != sg_dma_len(dd->out_sg))
-			fast = 0;
-	}
+	while (--nents > 0 && sg)
+		sg = sg_next(sg);
 
+	if (!sg)
+		return;
 
-	if (fast)  {
-		count = min(dd->total, sg_dma_len(dd->in_sg));
-		count = min(count, sg_dma_len(dd->out_sg));
+	sg->length += dma->remainder;
+}
 
-		err = dma_map_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
-		if (!err) {
-			dev_err(dd->dev, "dma_map_sg() error\n");
-			return -EINVAL;
-		}
+static int atmel_aes_map(struct atmel_aes_dev *dd,
+			 struct scatterlist *src,
+			 struct scatterlist *dst,
+			 size_t len)
+{
+	bool src_aligned, dst_aligned;
+	size_t padlen;
+
+	dd->total = len;
+	dd->src.sg = src;
+	dd->dst.sg = dst;
+	dd->real_dst = dst;
 
-		err = dma_map_sg(dd->dev, dd->out_sg, 1,
-				DMA_FROM_DEVICE);
-		if (!err) {
-			dev_err(dd->dev, "dma_map_sg() error\n");
-			dma_unmap_sg(dd->dev, dd->in_sg, 1,
-				DMA_TO_DEVICE);
-			return -EINVAL;
+	src_aligned = atmel_aes_check_aligned(dd, src, len, &dd->src);
+	if (src == dst)
+		dst_aligned = src_aligned;
+	else
+		dst_aligned = atmel_aes_check_aligned(dd, dst, len, &dd->dst);
+	if (!src_aligned || !dst_aligned) {
+		padlen = atmel_aes_padlen(len, dd->ctx->block_size);
+
+		if (dd->buflen < len + padlen)
+			return -ENOMEM;
+
+		if (!src_aligned) {
+			sg_copy_to_buffer(src, sg_nents(src), dd->buf, len);
+			dd->src.sg = &dd->aligned_sg;
+			dd->src.nents = 1;
+			dd->src.remainder = 0;
 		}
 
-		addr_in = sg_dma_address(dd->in_sg);
-		addr_out = sg_dma_address(dd->out_sg);
+		if (!dst_aligned) {
+			dd->dst.sg = &dd->aligned_sg;
+			dd->dst.nents = 1;
+			dd->dst.remainder = 0;
+		}
 
-		dd->flags |= AES_FLAGS_FAST;
+		sg_init_table(&dd->aligned_sg, 1);
+		sg_set_buf(&dd->aligned_sg, dd->buf, len + padlen);
+	}
 
+	if (dd->src.sg == dd->dst.sg) {
+		dd->src.sg_len = dma_map_sg(dd->dev, dd->src.sg, dd->src.nents,
+					    DMA_BIDIRECTIONAL);
+		dd->dst.sg_len = dd->src.sg_len;
+		if (!dd->src.sg_len)
+			return -EFAULT;
 	} else {
-		dma_sync_single_for_cpu(dd->dev, dd->dma_addr_in,
-					dd->dma_size, DMA_TO_DEVICE);
+		dd->src.sg_len = dma_map_sg(dd->dev, dd->src.sg, dd->src.nents,
+					    DMA_TO_DEVICE);
+		if (!dd->src.sg_len)
+			return -EFAULT;
+
+		dd->dst.sg_len = dma_map_sg(dd->dev, dd->dst.sg, dd->dst.nents,
+					    DMA_FROM_DEVICE);
+		if (!dd->dst.sg_len) {
+			dma_unmap_sg(dd->dev, dd->src.sg, dd->src.nents,
+				     DMA_TO_DEVICE);
+			return -EFAULT;
+		}
+	}
 
-		/* use cache buffers */
-		count = atmel_aes_sg_copy(&dd->in_sg, &dd->in_offset,
-				dd->buf_in, dd->buflen, dd->total, 0);
+	return 0;
+}
 
-		addr_in = dd->dma_addr_in;
-		addr_out = dd->dma_addr_out;
+static void atmel_aes_unmap(struct atmel_aes_dev *dd)
+{
+	if (dd->src.sg == dd->dst.sg) {
+		dma_unmap_sg(dd->dev, dd->src.sg, dd->src.nents,
+			     DMA_BIDIRECTIONAL);
 
-		dd->flags &= ~AES_FLAGS_FAST;
-	}
+		if (dd->src.sg != &dd->aligned_sg)
+			atmel_aes_restore_sg(&dd->src);
+	} else {
+		dma_unmap_sg(dd->dev, dd->dst.sg, dd->dst.nents,
+			     DMA_FROM_DEVICE);
 
-	dd->total -= count;
+		if (dd->dst.sg != &dd->aligned_sg)
+			atmel_aes_restore_sg(&dd->dst);
 
-	err = atmel_aes_crypt_dma(dd, addr_in, addr_out, count);
+		dma_unmap_sg(dd->dev, dd->src.sg, dd->src.nents,
+			     DMA_TO_DEVICE);
 
-	if (err && (dd->flags & AES_FLAGS_FAST)) {
-		dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
-		dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_TO_DEVICE);
+		if (dd->src.sg != &dd->aligned_sg)
+			atmel_aes_restore_sg(&dd->src);
 	}
 
-	return err;
+	if (dd->dst.sg == &dd->aligned_sg)
+		sg_copy_from_buffer(dd->real_dst, sg_nents(dd->real_dst),
+				    dd->buf, dd->total);
 }
 
-static int atmel_aes_write_ctrl(struct atmel_aes_dev *dd)
+static int atmel_aes_dma_transfer_start(struct atmel_aes_dev *dd,
+					enum dma_slave_buswidth addr_width,
+					enum dma_transfer_direction dir,
+					u32 maxburst)
 {
+	struct dma_async_tx_descriptor *desc;
+	struct dma_slave_config config;
+	dma_async_tx_callback callback;
+	struct atmel_aes_dma *dma;
 	int err;
-	u32 valcr = 0, valmr = 0;
 
-	err = atmel_aes_hw_init(dd);
+	memset(&config, 0, sizeof(config));
+	config.direction = dir;
+	config.src_addr_width = addr_width;
+	config.dst_addr_width = addr_width;
+	config.src_maxburst = maxburst;
+	config.dst_maxburst = maxburst;
+
+	switch (dir) {
+	case DMA_MEM_TO_DEV:
+		dma = &dd->src;
+		callback = NULL;
+		config.dst_addr = dd->phys_base + AES_IDATAR(0);
+		break;
+
+	case DMA_DEV_TO_MEM:
+		dma = &dd->dst;
+		callback = atmel_aes_dma_callback;
+		config.src_addr = dd->phys_base + AES_ODATAR(0);
+		break;
 
+	default:
+		return -EINVAL;
+	}
+
+	err = dmaengine_slave_config(dma->chan, &config);
 	if (err)
 		return err;
 
-	/* MR register must be set before IV registers */
-	if (dd->ctx->keylen == AES_KEYSIZE_128)
-		valmr |= AES_MR_KEYSIZE_128;
-	else if (dd->ctx->keylen == AES_KEYSIZE_192)
-		valmr |= AES_MR_KEYSIZE_192;
-	else
-		valmr |= AES_MR_KEYSIZE_256;
+	desc = dmaengine_prep_slave_sg(dma->chan, dma->sg, dma->sg_len, dir,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc)
+		return -ENOMEM;
 
-	if (dd->flags & AES_FLAGS_CBC) {
-		valmr |= AES_MR_OPMOD_CBC;
-	} else if (dd->flags & AES_FLAGS_CFB) {
-		valmr |= AES_MR_OPMOD_CFB;
-		if (dd->flags & AES_FLAGS_CFB8)
-			valmr |= AES_MR_CFBS_8b;
-		else if (dd->flags & AES_FLAGS_CFB16)
-			valmr |= AES_MR_CFBS_16b;
-		else if (dd->flags & AES_FLAGS_CFB32)
-			valmr |= AES_MR_CFBS_32b;
-		else if (dd->flags & AES_FLAGS_CFB64)
-			valmr |= AES_MR_CFBS_64b;
-		else if (dd->flags & AES_FLAGS_CFB128)
-			valmr |= AES_MR_CFBS_128b;
-	} else if (dd->flags & AES_FLAGS_OFB) {
-		valmr |= AES_MR_OPMOD_OFB;
-	} else if (dd->flags & AES_FLAGS_CTR) {
-		valmr |= AES_MR_OPMOD_CTR;
-	} else {
-		valmr |= AES_MR_OPMOD_ECB;
-	}
+	desc->callback = callback;
+	desc->callback_param = dd;
+	dmaengine_submit(desc);
+	dma_async_issue_pending(dma->chan);
 
-	if (dd->flags & AES_FLAGS_ENCRYPT)
-		valmr |= AES_MR_CYPHER_ENC;
+	return 0;
+}
 
-	if (dd->total > ATMEL_AES_DMA_THRESHOLD) {
-		valmr |= AES_MR_SMOD_IDATAR0;
-		if (dd->caps.has_dualbuff)
-			valmr |= AES_MR_DUALBUFF;
-	} else {
-		valmr |= AES_MR_SMOD_AUTO;
+static void atmel_aes_dma_transfer_stop(struct atmel_aes_dev *dd,
+					enum dma_transfer_direction dir)
+{
+	struct atmel_aes_dma *dma;
+
+	switch (dir) {
+	case DMA_MEM_TO_DEV:
+		dma = &dd->src;
+		break;
+
+	case DMA_DEV_TO_MEM:
+		dma = &dd->dst;
+		break;
+
+	default:
+		return;
 	}
 
-	atmel_aes_write(dd, AES_CR, valcr);
-	atmel_aes_write(dd, AES_MR, valmr);
+	dmaengine_terminate_all(dma->chan);
+}
 
-	atmel_aes_write_n(dd, AES_KEYWR(0), dd->ctx->key,
-						dd->ctx->keylen >> 2);
+static int atmel_aes_dma_start(struct atmel_aes_dev *dd,
+			       struct scatterlist *src,
+			       struct scatterlist *dst,
+			       size_t len,
+			       atmel_aes_fn_t resume)
+{
+	enum dma_slave_buswidth addr_width;
+	u32 maxburst;
+	int err;
 
-	if (((dd->flags & AES_FLAGS_CBC) || (dd->flags & AES_FLAGS_CFB) ||
-	   (dd->flags & AES_FLAGS_OFB) || (dd->flags & AES_FLAGS_CTR)) &&
-	   dd->req->info) {
-		atmel_aes_write_n(dd, AES_IVR(0), dd->req->info, 4);
+	switch (dd->ctx->block_size) {
+	case CFB8_BLOCK_SIZE:
+		addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+		maxburst = 1;
+		break;
+
+	case CFB16_BLOCK_SIZE:
+		addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+		maxburst = 1;
+		break;
+
+	case CFB32_BLOCK_SIZE:
+	case CFB64_BLOCK_SIZE:
+		addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		maxburst = 1;
+		break;
+
+	case AES_BLOCK_SIZE:
+		addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+		maxburst = dd->caps.max_burst_size;
+		break;
+
+	default:
+		err = -EINVAL;
+		goto exit;
 	}
 
-	return 0;
+	err = atmel_aes_map(dd, src, dst, len);
+	if (err)
+		goto exit;
+
+	dd->resume = resume;
+
+	/* Set output DMA transfer first */
+	err = atmel_aes_dma_transfer_start(dd, addr_width, DMA_DEV_TO_MEM,
+					   maxburst);
+	if (err)
+		goto unmap;
+
+	/* Then set input DMA transfer */
+	err = atmel_aes_dma_transfer_start(dd, addr_width, DMA_MEM_TO_DEV,
+					   maxburst);
+	if (err)
+		goto output_transfer_stop;
+
+	return -EINPROGRESS;
+
+output_transfer_stop:
+	atmel_aes_dma_transfer_stop(dd, DMA_DEV_TO_MEM);
+unmap:
+	atmel_aes_unmap(dd);
+exit:
+	return atmel_aes_complete(dd, err);
+}
+
+static void atmel_aes_dma_stop(struct atmel_aes_dev *dd)
+{
+	atmel_aes_dma_transfer_stop(dd, DMA_MEM_TO_DEV);
+	atmel_aes_dma_transfer_stop(dd, DMA_DEV_TO_MEM);
+	atmel_aes_unmap(dd);
+}
+
+static void atmel_aes_dma_callback(void *data)
+{
+	struct atmel_aes_dev *dd = data;
+
+	atmel_aes_dma_stop(dd);
+	dd->is_async = true;
+	(void)dd->resume(dd);
 }
 
 static int atmel_aes_handle_queue(struct atmel_aes_dev *dd,
-			       struct ablkcipher_request *req)
+				  struct crypto_async_request *new_areq)
 {
-	struct crypto_async_request *async_req, *backlog;
-	struct atmel_aes_ctx *ctx;
-	struct atmel_aes_reqctx *rctx;
+	struct crypto_async_request *areq, *backlog;
+	struct atmel_aes_base_ctx *ctx;
 	unsigned long flags;
 	int err, ret = 0;
 
 	spin_lock_irqsave(&dd->lock, flags);
-	if (req)
-		ret = ablkcipher_enqueue_request(&dd->queue, req);
+	if (new_areq)
+		ret = crypto_enqueue_request(&dd->queue, new_areq);
 	if (dd->flags & AES_FLAGS_BUSY) {
 		spin_unlock_irqrestore(&dd->lock, flags);
 		return ret;
 	}
 	backlog = crypto_get_backlog(&dd->queue);
-	async_req = crypto_dequeue_request(&dd->queue);
-	if (async_req)
+	areq = crypto_dequeue_request(&dd->queue);
+	if (areq)
 		dd->flags |= AES_FLAGS_BUSY;
 	spin_unlock_irqrestore(&dd->lock, flags);
 
-	if (!async_req)
+	if (!areq)
 		return ret;
 
 	if (backlog)
 		backlog->complete(backlog, -EINPROGRESS);
 
-	req = ablkcipher_request_cast(async_req);
-
-	/* assign new request to device */
-	dd->req = req;
-	dd->total = req->nbytes;
-	dd->in_offset = 0;
-	dd->in_sg = req->src;
-	dd->out_offset = 0;
-	dd->out_sg = req->dst;
+	ctx = crypto_tfm_ctx(areq->tfm);
 
-	rctx = ablkcipher_request_ctx(req);
-	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
-	rctx->mode &= AES_FLAGS_MODE_MASK;
-	dd->flags = (dd->flags & ~AES_FLAGS_MODE_MASK) | rctx->mode;
+	dd->areq = areq;
 	dd->ctx = ctx;
-	ctx->dd = dd;
+	dd->is_async = (areq != new_areq);
 
-	err = atmel_aes_write_ctrl(dd);
-	if (!err) {
-		if (dd->total > ATMEL_AES_DMA_THRESHOLD)
-			err = atmel_aes_crypt_dma_start(dd);
-		else
-			err = atmel_aes_crypt_cpu_start(dd);
-	}
-	if (err) {
-		/* aes_task will not finish it, so do it here */
-		atmel_aes_finish_req(dd, err);
-		tasklet_schedule(&dd->queue_task);
-	}
-
-	return ret;
+	err = ctx->start(dd);
+	return (dd->is_async) ? ret : err;
 }
 
-static int atmel_aes_crypt_dma_stop(struct atmel_aes_dev *dd)
-{
-	int err = -EINVAL;
-	size_t count;
 
-	if (dd->flags & AES_FLAGS_DMA) {
-		err = 0;
-		if  (dd->flags & AES_FLAGS_FAST) {
-			dma_unmap_sg(dd->dev, dd->out_sg, 1, DMA_FROM_DEVICE);
-			dma_unmap_sg(dd->dev, dd->in_sg, 1, DMA_TO_DEVICE);
-		} else {
-			dma_sync_single_for_cpu(dd->dev, dd->dma_addr_out,
-				dd->dma_size, DMA_FROM_DEVICE);
-
-			/* copy data */
-			count = atmel_aes_sg_copy(&dd->out_sg, &dd->out_offset,
-				dd->buf_out, dd->buflen, dd->dma_size, 1);
-			if (count != dd->dma_size) {
-				err = -EINVAL;
-				pr_err("not all data converted: %u\n", count);
-			}
-		}
-	}
+/* AES async block ciphers */
 
-	return err;
+static int atmel_aes_transfer_complete(struct atmel_aes_dev *dd)
+{
+	return atmel_aes_complete(dd, 0);
 }
 
-
-static int atmel_aes_buff_init(struct atmel_aes_dev *dd)
+static int atmel_aes_start(struct atmel_aes_dev *dd)
 {
-	int err = -ENOMEM;
-
-	dd->buf_in = (void *)__get_free_pages(GFP_KERNEL, 0);
-	dd->buf_out = (void *)__get_free_pages(GFP_KERNEL, 0);
-	dd->buflen = PAGE_SIZE;
-	dd->buflen &= ~(AES_BLOCK_SIZE - 1);
-
-	if (!dd->buf_in || !dd->buf_out) {
-		dev_err(dd->dev, "unable to alloc pages.\n");
-		goto err_alloc;
-	}
+	struct ablkcipher_request *req = ablkcipher_request_cast(dd->areq);
+	struct atmel_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	bool use_dma = (req->nbytes >= ATMEL_AES_DMA_THRESHOLD ||
+			dd->ctx->block_size != AES_BLOCK_SIZE);
+	int err;
 
-	/* MAP here */
-	dd->dma_addr_in = dma_map_single(dd->dev, dd->buf_in,
-					dd->buflen, DMA_TO_DEVICE);
-	if (dma_mapping_error(dd->dev, dd->dma_addr_in)) {
-		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
-		err = -EINVAL;
-		goto err_map_in;
-	}
+	atmel_aes_set_mode(dd, rctx);
 
-	dd->dma_addr_out = dma_map_single(dd->dev, dd->buf_out,
-					dd->buflen, DMA_FROM_DEVICE);
-	if (dma_mapping_error(dd->dev, dd->dma_addr_out)) {
-		dev_err(dd->dev, "dma %d bytes error\n", dd->buflen);
-		err = -EINVAL;
-		goto err_map_out;
-	}
+	err = atmel_aes_hw_init(dd);
+	if (err)
+		return atmel_aes_complete(dd, err);
 
-	return 0;
+	atmel_aes_write_ctrl(dd, use_dma, req->info);
+	if (use_dma)
+		return atmel_aes_dma_start(dd, req->src, req->dst, req->nbytes,
+					   atmel_aes_transfer_complete);
 
-err_map_out:
-	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
-		DMA_TO_DEVICE);
-err_map_in:
-err_alloc:
-	free_page((unsigned long)dd->buf_out);
-	free_page((unsigned long)dd->buf_in);
-	if (err)
-		pr_err("error: %d\n", err);
-	return err;
+	return atmel_aes_cpu_start(dd, req->src, req->dst, req->nbytes,
+				   atmel_aes_transfer_complete);
 }
 
-static void atmel_aes_buff_cleanup(struct atmel_aes_dev *dd)
+static inline struct atmel_aes_ctr_ctx *
+atmel_aes_ctr_ctx_cast(struct atmel_aes_base_ctx *ctx)
 {
-	dma_unmap_single(dd->dev, dd->dma_addr_out, dd->buflen,
-			 DMA_FROM_DEVICE);
-	dma_unmap_single(dd->dev, dd->dma_addr_in, dd->buflen,
-		DMA_TO_DEVICE);
-	free_page((unsigned long)dd->buf_out);
-	free_page((unsigned long)dd->buf_in);
+	return container_of(ctx, struct atmel_aes_ctr_ctx, base);
 }
 
-static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
+static int atmel_aes_ctr_transfer(struct atmel_aes_dev *dd)
 {
-	struct atmel_aes_ctx *ctx = crypto_ablkcipher_ctx(
-			crypto_ablkcipher_reqtfm(req));
-	struct atmel_aes_reqctx *rctx = ablkcipher_request_ctx(req);
-	struct atmel_aes_dev *dd;
-
-	if (mode & AES_FLAGS_CFB8) {
-		if (!IS_ALIGNED(req->nbytes, CFB8_BLOCK_SIZE)) {
-			pr_err("request size is not exact amount of CFB8 blocks\n");
-			return -EINVAL;
-		}
-		ctx->block_size = CFB8_BLOCK_SIZE;
-	} else if (mode & AES_FLAGS_CFB16) {
-		if (!IS_ALIGNED(req->nbytes, CFB16_BLOCK_SIZE)) {
-			pr_err("request size is not exact amount of CFB16 blocks\n");
-			return -EINVAL;
-		}
-		ctx->block_size = CFB16_BLOCK_SIZE;
-	} else if (mode & AES_FLAGS_CFB32) {
-		if (!IS_ALIGNED(req->nbytes, CFB32_BLOCK_SIZE)) {
-			pr_err("request size is not exact amount of CFB32 blocks\n");
-			return -EINVAL;
-		}
-		ctx->block_size = CFB32_BLOCK_SIZE;
-	} else if (mode & AES_FLAGS_CFB64) {
-		if (!IS_ALIGNED(req->nbytes, CFB64_BLOCK_SIZE)) {
-			pr_err("request size is not exact amount of CFB64 blocks\n");
-			return -EINVAL;
+	struct atmel_aes_ctr_ctx *ctx = atmel_aes_ctr_ctx_cast(dd->ctx);
+	struct ablkcipher_request *req = ablkcipher_request_cast(dd->areq);
+	struct scatterlist *src, *dst;
+	u32 ctr, blocks;
+	size_t datalen;
+	bool use_dma, fragmented = false;
+
+	/* Check for transfer completion. */
+	ctx->offset += dd->total;
+	if (ctx->offset >= req->nbytes)
+		return atmel_aes_transfer_complete(dd);
+
+	/* Compute data length. */
+	datalen = req->nbytes - ctx->offset;
+	blocks = DIV_ROUND_UP(datalen, AES_BLOCK_SIZE);
+	ctr = be32_to_cpu(ctx->iv[3]);
+	if (dd->caps.has_ctr32) {
+		/* Check 32bit counter overflow. */
+		u32 start = ctr;
+		u32 end = start + blocks - 1;
+
+		if (end < start) {
+			ctr |= 0xffffffff;
+			datalen = AES_BLOCK_SIZE * -start;
+			fragmented = true;
 		}
-		ctx->block_size = CFB64_BLOCK_SIZE;
 	} else {
-		if (!IS_ALIGNED(req->nbytes, AES_BLOCK_SIZE)) {
-			pr_err("request size is not exact amount of AES blocks\n");
-			return -EINVAL;
+		/* Check 16bit counter overflow. */
+		u16 start = ctr & 0xffff;
+		u16 end = start + (u16)blocks - 1;
+
+		if (blocks >> 16 || end < start) {
+			ctr |= 0xffff;
+			datalen = AES_BLOCK_SIZE * (0x10000-start);
+			fragmented = true;
 		}
-		ctx->block_size = AES_BLOCK_SIZE;
+	}
+	use_dma = (datalen >= ATMEL_AES_DMA_THRESHOLD);
+
+	/* Jump to offset. */
+	src = scatterwalk_ffwd(ctx->src, req->src, ctx->offset);
+	dst = ((req->src == req->dst) ? src :
+	       scatterwalk_ffwd(ctx->dst, req->dst, ctx->offset));
+
+	/* Configure hardware. */
+	atmel_aes_write_ctrl(dd, use_dma, ctx->iv);
+	if (unlikely(fragmented)) {
+		/*
+		 * Increment the counter manually to cope with the hardware
+		 * counter overflow.
+		 */
+		ctx->iv[3] = cpu_to_be32(ctr);
+		crypto_inc((u8 *)ctx->iv, AES_BLOCK_SIZE);
 	}
 
-	dd = atmel_aes_find_dev(ctx);
-	if (!dd)
-		return -ENODEV;
-
-	rctx->mode = mode;
+	if (use_dma)
+		return atmel_aes_dma_start(dd, src, dst, datalen,
+					   atmel_aes_ctr_transfer);
 
-	return atmel_aes_handle_queue(dd, req);
+	return atmel_aes_cpu_start(dd, src, dst, datalen,
+				   atmel_aes_ctr_transfer);
 }
 
-static bool atmel_aes_filter(struct dma_chan *chan, void *slave)
+static int atmel_aes_ctr_start(struct atmel_aes_dev *dd)
 {
-	struct at_dma_slave	*sl = slave;
+	struct atmel_aes_ctr_ctx *ctx = atmel_aes_ctr_ctx_cast(dd->ctx);
+	struct ablkcipher_request *req = ablkcipher_request_cast(dd->areq);
+	struct atmel_aes_reqctx *rctx = ablkcipher_request_ctx(req);
+	int err;
 
-	if (sl && sl->dma_dev == chan->device->dev) {
-		chan->private = sl;
-		return true;
-	} else {
-		return false;
-	}
+	atmel_aes_set_mode(dd, rctx);
+
+	err = atmel_aes_hw_init(dd);
+	if (err)
+		return atmel_aes_complete(dd, err);
+
+	memcpy(ctx->iv, req->info, AES_BLOCK_SIZE);
+	ctx->offset = 0;
+	dd->total = 0;
+	return atmel_aes_ctr_transfer(dd);
 }
 
-static int atmel_aes_dma_init(struct atmel_aes_dev *dd,
-	struct crypto_platform_data *pdata)
+static int atmel_aes_crypt(struct ablkcipher_request *req, unsigned long mode)
 {
-	int err = -ENOMEM;
-	dma_cap_mask_t mask;
+	struct atmel_aes_base_ctx *ctx;
+	struct atmel_aes_reqctx *rctx;
+	struct atmel_aes_dev *dd;
 
-	dma_cap_zero(mask);
-	dma_cap_set(DMA_SLAVE, mask);
+	ctx = crypto_ablkcipher_ctx(crypto_ablkcipher_reqtfm(req));
+	switch (mode & AES_FLAGS_OPMODE_MASK) {
+	case AES_FLAGS_CFB8:
+		ctx->block_size = CFB8_BLOCK_SIZE;
+		break;
 
-	/* Try to grab 2 DMA channels */
-	dd->dma_lch_in.chan = dma_request_slave_channel_compat(mask,
-			atmel_aes_filter, &pdata->dma_slave->rxdata, dd->dev, "tx");
-	if (!dd->dma_lch_in.chan)
-		goto err_dma_in;
+	case AES_FLAGS_CFB16:
+		ctx->block_size = CFB16_BLOCK_SIZE;
+		break;
 
-	dd->dma_lch_in.dma_conf.direction = DMA_MEM_TO_DEV;
-	dd->dma_lch_in.dma_conf.dst_addr = dd->phys_base +
-		AES_IDATAR(0);
-	dd->dma_lch_in.dma_conf.src_maxburst = dd->caps.max_burst_size;
-	dd->dma_lch_in.dma_conf.src_addr_width =
-		DMA_SLAVE_BUSWIDTH_4_BYTES;
-	dd->dma_lch_in.dma_conf.dst_maxburst = dd->caps.max_burst_size;
-	dd->dma_lch_in.dma_conf.dst_addr_width =
-		DMA_SLAVE_BUSWIDTH_4_BYTES;
-	dd->dma_lch_in.dma_conf.device_fc = false;
-
-	dd->dma_lch_out.chan = dma_request_slave_channel_compat(mask,
-			atmel_aes_filter, &pdata->dma_slave->txdata, dd->dev, "rx");
-	if (!dd->dma_lch_out.chan)
-		goto err_dma_out;
+	case AES_FLAGS_CFB32:
+		ctx->block_size = CFB32_BLOCK_SIZE;
+		break;
 
-	dd->dma_lch_out.dma_conf.direction = DMA_DEV_TO_MEM;
-	dd->dma_lch_out.dma_conf.src_addr = dd->phys_base +
-		AES_ODATAR(0);
-	dd->dma_lch_out.dma_conf.src_maxburst = dd->caps.max_burst_size;
-	dd->dma_lch_out.dma_conf.src_addr_width =
-		DMA_SLAVE_BUSWIDTH_4_BYTES;
-	dd->dma_lch_out.dma_conf.dst_maxburst = dd->caps.max_burst_size;
-	dd->dma_lch_out.dma_conf.dst_addr_width =
-		DMA_SLAVE_BUSWIDTH_4_BYTES;
-	dd->dma_lch_out.dma_conf.device_fc = false;
+	case AES_FLAGS_CFB64:
+		ctx->block_size = CFB64_BLOCK_SIZE;
+		break;
 
-	return 0;
+	default:
+		ctx->block_size = AES_BLOCK_SIZE;
+		break;
+	}
 
-err_dma_out:
-	dma_release_channel(dd->dma_lch_in.chan);
-err_dma_in:
-	dev_warn(dd->dev, "no DMA channel available\n");
-	return err;
-}
+	dd = atmel_aes_find_dev(ctx);
+	if (!dd)
+		return -ENODEV;
 
-static void atmel_aes_dma_cleanup(struct atmel_aes_dev *dd)
-{
-	dma_release_channel(dd->dma_lch_in.chan);
-	dma_release_channel(dd->dma_lch_out.chan);
+	rctx = ablkcipher_request_ctx(req);
+	rctx->mode = mode;
+
+	return atmel_aes_handle_queue(dd, &req->base);
 }
 
 static int atmel_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 			   unsigned int keylen)
 {
-	struct atmel_aes_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct atmel_aes_base_ctx *ctx = crypto_ablkcipher_ctx(tfm);
 
-	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
-		   keylen != AES_KEYSIZE_256) {
+	if (keylen != AES_KEYSIZE_128 &&
+	    keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256) {
 		crypto_ablkcipher_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
 		return -EINVAL;
 	}
@@ -839,115 +1067,110 @@ static int atmel_aes_setkey(struct crypto_ablkcipher *tfm, const u8 *key,
 
 static int atmel_aes_ecb_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT);
+	return atmel_aes_crypt(req, AES_FLAGS_ECB | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_ecb_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		0);
+	return atmel_aes_crypt(req, AES_FLAGS_ECB);
 }
 
 static int atmel_aes_cbc_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CBC);
+	return atmel_aes_crypt(req, AES_FLAGS_CBC | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cbc_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CBC);
+	return atmel_aes_crypt(req, AES_FLAGS_CBC);
 }
 
 static int atmel_aes_ofb_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_OFB);
+	return atmel_aes_crypt(req, AES_FLAGS_OFB | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_ofb_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_OFB);
+	return atmel_aes_crypt(req, AES_FLAGS_OFB);
 }
 
 static int atmel_aes_cfb_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB128);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB128 | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cfb_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB | AES_FLAGS_CFB128);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB128);
 }
 
 static int atmel_aes_cfb64_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB64);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB64 | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cfb64_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB | AES_FLAGS_CFB64);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB64);
 }
 
 static int atmel_aes_cfb32_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB32);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB32 | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cfb32_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB | AES_FLAGS_CFB32);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB32);
 }
 
 static int atmel_aes_cfb16_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CFB | AES_FLAGS_CFB16);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB16 | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cfb16_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB | AES_FLAGS_CFB16);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB16);
 }
 
 static int atmel_aes_cfb8_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT |	AES_FLAGS_CFB | AES_FLAGS_CFB8);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB8 | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_cfb8_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CFB | AES_FLAGS_CFB8);
+	return atmel_aes_crypt(req, AES_FLAGS_CFB8);
 }
 
 static int atmel_aes_ctr_encrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_ENCRYPT | AES_FLAGS_CTR);
+	return atmel_aes_crypt(req, AES_FLAGS_CTR | AES_FLAGS_ENCRYPT);
 }
 
 static int atmel_aes_ctr_decrypt(struct ablkcipher_request *req)
 {
-	return atmel_aes_crypt(req,
-		AES_FLAGS_CTR);
+	return atmel_aes_crypt(req, AES_FLAGS_CTR);
 }
 
 static int atmel_aes_cra_init(struct crypto_tfm *tfm)
 {
+	struct atmel_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
 	tfm->crt_ablkcipher.reqsize = sizeof(struct atmel_aes_reqctx);
+	ctx->base.start = atmel_aes_start;
+
+	return 0;
+}
+
+static int atmel_aes_ctr_cra_init(struct crypto_tfm *tfm)
+{
+	struct atmel_aes_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	tfm->crt_ablkcipher.reqsize = sizeof(struct atmel_aes_reqctx);
+	ctx->base.start = atmel_aes_ctr_start;
 
 	return 0;
 }
@@ -960,7 +1183,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "ecb(aes)",
 	.cra_driver_name	= "atmel-ecb-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -980,7 +1203,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "cbc(aes)",
 	.cra_driver_name	= "atmel-cbc-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1001,7 +1224,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "ofb(aes)",
 	.cra_driver_name	= "atmel-ofb-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1022,7 +1245,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "cfb(aes)",
 	.cra_driver_name	= "atmel-cfb-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= AES_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1043,7 +1266,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "cfb32(aes)",
 	.cra_driver_name	= "atmel-cfb32-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB32_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1064,7 +1287,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "cfb16(aes)",
 	.cra_driver_name	= "atmel-cfb16-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB16_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1085,7 +1308,7 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "cfb8(aes)",
 	.cra_driver_name	= "atmel-cfb8-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB8_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1106,14 +1329,14 @@ static struct crypto_alg aes_algs[] = {
 {
 	.cra_name		= "ctr(aes)",
 	.cra_driver_name	= "atmel-ctr-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
-	.cra_blocksize		= AES_BLOCK_SIZE,
-	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
+	.cra_blocksize		= 1,
+	.cra_ctxsize		= sizeof(struct atmel_aes_ctr_ctx),
 	.cra_alignmask		= 0xf,
 	.cra_type		= &crypto_ablkcipher_type,
 	.cra_module		= THIS_MODULE,
-	.cra_init		= atmel_aes_cra_init,
+	.cra_init		= atmel_aes_ctr_cra_init,
 	.cra_exit		= atmel_aes_cra_exit,
 	.cra_u.ablkcipher = {
 		.min_keysize	= AES_MIN_KEY_SIZE,
@@ -1129,7 +1352,7 @@ static struct crypto_alg aes_algs[] = {
 static struct crypto_alg aes_cfb64_alg = {
 	.cra_name		= "cfb64(aes)",
 	.cra_driver_name	= "atmel-cfb64-aes",
-	.cra_priority		= 100,
+	.cra_priority		= ATMEL_AES_PRIORITY,
 	.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER | CRYPTO_ALG_ASYNC,
 	.cra_blocksize		= CFB64_BLOCK_SIZE,
 	.cra_ctxsize		= sizeof(struct atmel_aes_ctx),
@@ -1148,53 +1371,496 @@ static struct crypto_alg aes_cfb64_alg = {
 	}
 };
 
-static void atmel_aes_queue_task(unsigned long data)
+
+/* gcm aead functions */
+
+static int atmel_aes_gcm_ghash(struct atmel_aes_dev *dd,
+			       const u32 *data, size_t datalen,
+			       const u32 *ghash_in, u32 *ghash_out,
+			       atmel_aes_fn_t resume);
+static int atmel_aes_gcm_ghash_init(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_ghash_finalize(struct atmel_aes_dev *dd);
+
+static int atmel_aes_gcm_start(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_process(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_length(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_data(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_tag_init(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_tag(struct atmel_aes_dev *dd);
+static int atmel_aes_gcm_finalize(struct atmel_aes_dev *dd);
+
+static inline struct atmel_aes_gcm_ctx *
+atmel_aes_gcm_ctx_cast(struct atmel_aes_base_ctx *ctx)
 {
-	struct atmel_aes_dev *dd = (struct atmel_aes_dev *)data;
+	return container_of(ctx, struct atmel_aes_gcm_ctx, base);
+}
 
-	atmel_aes_handle_queue(dd, NULL);
+static int atmel_aes_gcm_ghash(struct atmel_aes_dev *dd,
+			       const u32 *data, size_t datalen,
+			       const u32 *ghash_in, u32 *ghash_out,
+			       atmel_aes_fn_t resume)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+
+	dd->data = (u32 *)data;
+	dd->datalen = datalen;
+	ctx->ghash_in = ghash_in;
+	ctx->ghash_out = ghash_out;
+	ctx->ghash_resume = resume;
+
+	atmel_aes_write_ctrl(dd, false, NULL);
+	return atmel_aes_wait_for_data_ready(dd, atmel_aes_gcm_ghash_init);
 }
 
-static void atmel_aes_done_task(unsigned long data)
+static int atmel_aes_gcm_ghash_init(struct atmel_aes_dev *dd)
 {
-	struct atmel_aes_dev *dd = (struct atmel_aes_dev *) data;
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+
+	/* Set the data length. */
+	atmel_aes_write(dd, AES_AADLENR, dd->total);
+	atmel_aes_write(dd, AES_CLENR, 0);
+
+	/* If needed, overwrite the GCM Intermediate Hash Word Registers */
+	if (ctx->ghash_in)
+		atmel_aes_write_block(dd, AES_GHASHR(0), ctx->ghash_in);
+
+	return atmel_aes_gcm_ghash_finalize(dd);
+}
+
+static int atmel_aes_gcm_ghash_finalize(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	u32 isr;
+
+	/* Write data into the Input Data Registers. */
+	while (dd->datalen > 0) {
+		atmel_aes_write_block(dd, AES_IDATAR(0), dd->data);
+		dd->data += 4;
+		dd->datalen -= AES_BLOCK_SIZE;
+
+		isr = atmel_aes_read(dd, AES_ISR);
+		if (!(isr & AES_INT_DATARDY)) {
+			dd->resume = atmel_aes_gcm_ghash_finalize;
+			atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
+			return -EINPROGRESS;
+		}
+	}
+
+	/* Read the computed hash from GHASHRx. */
+	atmel_aes_read_block(dd, AES_GHASHR(0), ctx->ghash_out);
+
+	return ctx->ghash_resume(dd);
+}
+
+
+static int atmel_aes_gcm_start(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	struct atmel_aes_reqctx *rctx = aead_request_ctx(req);
+	size_t ivsize = crypto_aead_ivsize(tfm);
+	size_t datalen, padlen;
+	const void *iv = req->iv;
+	u8 *data = dd->buf;
 	int err;
 
-	if (!(dd->flags & AES_FLAGS_DMA)) {
-		atmel_aes_read_n(dd, AES_ODATAR(0), (u32 *) dd->buf_out,
-				dd->bufcnt >> 2);
+	atmel_aes_set_mode(dd, rctx);
 
-		if (sg_copy_from_buffer(dd->out_sg, dd->nb_out_sg,
-			dd->buf_out, dd->bufcnt))
-			err = 0;
-		else
-			err = -EINVAL;
+	err = atmel_aes_hw_init(dd);
+	if (err)
+		return atmel_aes_complete(dd, err);
 
-		goto cpu_end;
+	if (likely(ivsize == 12)) {
+		memcpy(ctx->j0, iv, ivsize);
+		ctx->j0[3] = cpu_to_be32(1);
+		return atmel_aes_gcm_process(dd);
 	}
 
-	err = atmel_aes_crypt_dma_stop(dd);
+	padlen = atmel_aes_padlen(ivsize, AES_BLOCK_SIZE);
+	datalen = ivsize + padlen + AES_BLOCK_SIZE;
+	if (datalen > dd->buflen)
+		return atmel_aes_complete(dd, -EINVAL);
 
-	err = dd->err ? : err;
+	memcpy(data, iv, ivsize);
+	memset(data + ivsize, 0, padlen + sizeof(u64));
+	((u64 *)(data + datalen))[-1] = cpu_to_be64(ivsize * 8);
 
-	if (dd->total && !err) {
-		if (dd->flags & AES_FLAGS_FAST) {
-			dd->in_sg = sg_next(dd->in_sg);
-			dd->out_sg = sg_next(dd->out_sg);
-			if (!dd->in_sg || !dd->out_sg)
-				err = -EINVAL;
+	return atmel_aes_gcm_ghash(dd, (const u32 *)data, datalen,
+				   NULL, ctx->j0, atmel_aes_gcm_process);
+}
+
+static int atmel_aes_gcm_process(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	bool enc = atmel_aes_is_encrypt(dd);
+	u32 authsize;
+
+	/* Compute text length. */
+	authsize = crypto_aead_authsize(tfm);
+	ctx->textlen = req->cryptlen - (enc ? 0 : authsize);
+
+	/*
+	 * According to tcrypt test suite, the GCM Automatic Tag Generation
+	 * fails when both the message and its associated data are empty.
+	 */
+	if (likely(req->assoclen != 0 || ctx->textlen != 0))
+		dd->flags |= AES_FLAGS_GTAGEN;
+
+	atmel_aes_write_ctrl(dd, false, NULL);
+	return atmel_aes_wait_for_data_ready(dd, atmel_aes_gcm_length);
+}
+
+static int atmel_aes_gcm_length(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	u32 j0_lsw, *j0 = ctx->j0;
+	size_t padlen;
+
+	/* Write incr32(J0) into IV. */
+	j0_lsw = j0[3];
+	j0[3] = cpu_to_be32(be32_to_cpu(j0[3]) + 1);
+	atmel_aes_write_block(dd, AES_IVR(0), j0);
+	j0[3] = j0_lsw;
+
+	/* Set aad and text lengths. */
+	atmel_aes_write(dd, AES_AADLENR, req->assoclen);
+	atmel_aes_write(dd, AES_CLENR, ctx->textlen);
+
+	/* Check whether AAD are present. */
+	if (unlikely(req->assoclen == 0)) {
+		dd->datalen = 0;
+		return atmel_aes_gcm_data(dd);
+	}
+
+	/* Copy assoc data and add padding. */
+	padlen = atmel_aes_padlen(req->assoclen, AES_BLOCK_SIZE);
+	if (unlikely(req->assoclen + padlen > dd->buflen))
+		return atmel_aes_complete(dd, -EINVAL);
+	sg_copy_to_buffer(req->src, sg_nents(req->src), dd->buf, req->assoclen);
+
+	/* Write assoc data into the Input Data register. */
+	dd->data = (u32 *)dd->buf;
+	dd->datalen = req->assoclen + padlen;
+	return atmel_aes_gcm_data(dd);
+}
+
+static int atmel_aes_gcm_data(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	bool use_dma = (ctx->textlen >= ATMEL_AES_DMA_THRESHOLD);
+	struct scatterlist *src, *dst;
+	u32 isr, mr;
+
+	/* Write AAD first. */
+	while (dd->datalen > 0) {
+		atmel_aes_write_block(dd, AES_IDATAR(0), dd->data);
+		dd->data += 4;
+		dd->datalen -= AES_BLOCK_SIZE;
+
+		isr = atmel_aes_read(dd, AES_ISR);
+		if (!(isr & AES_INT_DATARDY)) {
+			dd->resume = atmel_aes_gcm_data;
+			atmel_aes_write(dd, AES_IER, AES_INT_DATARDY);
+			return -EINPROGRESS;
 		}
-		if (!err)
-			err = atmel_aes_crypt_dma_start(dd);
-		if (!err)
-			return; /* DMA started. Not fininishing. */
 	}
 
-cpu_end:
-	atmel_aes_finish_req(dd, err);
+	/* GMAC only. */
+	if (unlikely(ctx->textlen == 0))
+		return atmel_aes_gcm_tag_init(dd);
+
+	/* Prepare src and dst scatter lists to transfer cipher/plain texts */
+	src = scatterwalk_ffwd(ctx->src, req->src, req->assoclen);
+	dst = ((req->src == req->dst) ? src :
+	       scatterwalk_ffwd(ctx->dst, req->dst, req->assoclen));
+
+	if (use_dma) {
+		/* Update the Mode Register for DMA transfers. */
+		mr = atmel_aes_read(dd, AES_MR);
+		mr &= ~(AES_MR_SMOD_MASK | AES_MR_DUALBUFF);
+		mr |= AES_MR_SMOD_IDATAR0;
+		if (dd->caps.has_dualbuff)
+			mr |= AES_MR_DUALBUFF;
+		atmel_aes_write(dd, AES_MR, mr);
+
+		return atmel_aes_dma_start(dd, src, dst, ctx->textlen,
+					   atmel_aes_gcm_tag_init);
+	}
+
+	return atmel_aes_cpu_start(dd, src, dst, ctx->textlen,
+				   atmel_aes_gcm_tag_init);
+}
+
+static int atmel_aes_gcm_tag_init(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	u64 *data = dd->buf;
+
+	if (likely(dd->flags & AES_FLAGS_GTAGEN)) {
+		if (!(atmel_aes_read(dd, AES_ISR) & AES_INT_TAGRDY)) {
+			dd->resume = atmel_aes_gcm_tag_init;
+			atmel_aes_write(dd, AES_IER, AES_INT_TAGRDY);
+			return -EINPROGRESS;
+		}
+
+		return atmel_aes_gcm_finalize(dd);
+	}
+
+	/* Read the GCM Intermediate Hash Word Registers. */
+	atmel_aes_read_block(dd, AES_GHASHR(0), ctx->ghash);
+
+	data[0] = cpu_to_be64(req->assoclen * 8);
+	data[1] = cpu_to_be64(ctx->textlen * 8);
+
+	return atmel_aes_gcm_ghash(dd, (const u32 *)data, AES_BLOCK_SIZE,
+				   ctx->ghash, ctx->ghash, atmel_aes_gcm_tag);
+}
+
+static int atmel_aes_gcm_tag(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	unsigned long flags;
+
+	/*
+	 * Change mode to CTR to complete the tag generation.
+	 * Use J0 as Initialization Vector.
+	 */
+	flags = dd->flags;
+	dd->flags &= ~(AES_FLAGS_OPMODE_MASK | AES_FLAGS_GTAGEN);
+	dd->flags |= AES_FLAGS_CTR;
+	atmel_aes_write_ctrl(dd, false, ctx->j0);
+	dd->flags = flags;
+
+	atmel_aes_write_block(dd, AES_IDATAR(0), ctx->ghash);
+	return atmel_aes_wait_for_data_ready(dd, atmel_aes_gcm_finalize);
+}
+
+static int atmel_aes_gcm_finalize(struct atmel_aes_dev *dd)
+{
+	struct atmel_aes_gcm_ctx *ctx = atmel_aes_gcm_ctx_cast(dd->ctx);
+	struct aead_request *req = aead_request_cast(dd->areq);
+	struct crypto_aead *tfm = crypto_aead_reqtfm(req);
+	bool enc = atmel_aes_is_encrypt(dd);
+	u32 offset, authsize, itag[4], *otag = ctx->tag;
+	int err;
+
+	/* Read the computed tag. */
+	if (likely(dd->flags & AES_FLAGS_GTAGEN))
+		atmel_aes_read_block(dd, AES_TAGR(0), ctx->tag);
+	else
+		atmel_aes_read_block(dd, AES_ODATAR(0), ctx->tag);
+
+	offset = req->assoclen + ctx->textlen;
+	authsize = crypto_aead_authsize(tfm);
+	if (enc) {
+		scatterwalk_map_and_copy(otag, req->dst, offset, authsize, 1);
+		err = 0;
+	} else {
+		scatterwalk_map_and_copy(itag, req->src, offset, authsize, 0);
+		err = crypto_memneq(itag, otag, authsize) ? -EBADMSG : 0;
+	}
+
+	return atmel_aes_complete(dd, err);
+}
+
+static int atmel_aes_gcm_crypt(struct aead_request *req,
+			       unsigned long mode)
+{
+	struct atmel_aes_base_ctx *ctx;
+	struct atmel_aes_reqctx *rctx;
+	struct atmel_aes_dev *dd;
+
+	ctx = crypto_aead_ctx(crypto_aead_reqtfm(req));
+	ctx->block_size = AES_BLOCK_SIZE;
+
+	dd = atmel_aes_find_dev(ctx);
+	if (!dd)
+		return -ENODEV;
+
+	rctx = aead_request_ctx(req);
+	rctx->mode = AES_FLAGS_GCM | mode;
+
+	return atmel_aes_handle_queue(dd, &req->base);
+}
+
+static int atmel_aes_gcm_setkey(struct crypto_aead *tfm, const u8 *key,
+				unsigned int keylen)
+{
+	struct atmel_aes_base_ctx *ctx = crypto_aead_ctx(tfm);
+
+	if (keylen != AES_KEYSIZE_256 &&
+	    keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_128) {
+		crypto_aead_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	memcpy(ctx->key, key, keylen);
+	ctx->keylen = keylen;
+
+	return 0;
+}
+
+static int atmel_aes_gcm_setauthsize(struct crypto_aead *tfm,
+				     unsigned int authsize)
+{
+	/* Same as crypto_gcm_authsize() from crypto/gcm.c */
+	switch (authsize) {
+	case 4:
+	case 8:
+	case 12:
+	case 13:
+	case 14:
+	case 15:
+	case 16:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int atmel_aes_gcm_encrypt(struct aead_request *req)
+{
+	return atmel_aes_gcm_crypt(req, AES_FLAGS_ENCRYPT);
+}
+
+static int atmel_aes_gcm_decrypt(struct aead_request *req)
+{
+	return atmel_aes_gcm_crypt(req, 0);
+}
+
+static int atmel_aes_gcm_init(struct crypto_aead *tfm)
+{
+	struct atmel_aes_gcm_ctx *ctx = crypto_aead_ctx(tfm);
+
+	crypto_aead_set_reqsize(tfm, sizeof(struct atmel_aes_reqctx));
+	ctx->base.start = atmel_aes_gcm_start;
+
+	return 0;
+}
+
+static void atmel_aes_gcm_exit(struct crypto_aead *tfm)
+{
+
+}
+
+static struct aead_alg aes_gcm_alg = {
+	.setkey		= atmel_aes_gcm_setkey,
+	.setauthsize	= atmel_aes_gcm_setauthsize,
+	.encrypt	= atmel_aes_gcm_encrypt,
+	.decrypt	= atmel_aes_gcm_decrypt,
+	.init		= atmel_aes_gcm_init,
+	.exit		= atmel_aes_gcm_exit,
+	.ivsize		= 12,
+	.maxauthsize	= AES_BLOCK_SIZE,
+
+	.base = {
+		.cra_name		= "gcm(aes)",
+		.cra_driver_name	= "atmel-gcm-aes",
+		.cra_priority		= ATMEL_AES_PRIORITY,
+		.cra_flags		= CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= 1,
+		.cra_ctxsize		= sizeof(struct atmel_aes_gcm_ctx),
+		.cra_alignmask		= 0xf,
+		.cra_module		= THIS_MODULE,
+	},
+};
+
+
+/* Probe functions */
+
+static int atmel_aes_buff_init(struct atmel_aes_dev *dd)
+{
+	dd->buf = (void *)__get_free_pages(GFP_KERNEL, ATMEL_AES_BUFFER_ORDER);
+	dd->buflen = ATMEL_AES_BUFFER_SIZE;
+	dd->buflen &= ~(AES_BLOCK_SIZE - 1);
+
+	if (!dd->buf) {
+		dev_err(dd->dev, "unable to alloc pages.\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void atmel_aes_buff_cleanup(struct atmel_aes_dev *dd)
+{
+	free_page((unsigned long)dd->buf);
+}
+
+static bool atmel_aes_filter(struct dma_chan *chan, void *slave)
+{
+	struct at_dma_slave	*sl = slave;
+
+	if (sl && sl->dma_dev == chan->device->dev) {
+		chan->private = sl;
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static int atmel_aes_dma_init(struct atmel_aes_dev *dd,
+			      struct crypto_platform_data *pdata)
+{
+	struct at_dma_slave *slave;
+	int err = -ENOMEM;
+	dma_cap_mask_t mask;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	/* Try to grab 2 DMA channels */
+	slave = &pdata->dma_slave->rxdata;
+	dd->src.chan = dma_request_slave_channel_compat(mask, atmel_aes_filter,
+							slave, dd->dev, "tx");
+	if (!dd->src.chan)
+		goto err_dma_in;
+
+	slave = &pdata->dma_slave->txdata;
+	dd->dst.chan = dma_request_slave_channel_compat(mask, atmel_aes_filter,
+							slave, dd->dev, "rx");
+	if (!dd->dst.chan)
+		goto err_dma_out;
+
+	return 0;
+
+err_dma_out:
+	dma_release_channel(dd->src.chan);
+err_dma_in:
+	dev_warn(dd->dev, "no DMA channel available\n");
+	return err;
+}
+
+static void atmel_aes_dma_cleanup(struct atmel_aes_dev *dd)
+{
+	dma_release_channel(dd->dst.chan);
+	dma_release_channel(dd->src.chan);
+}
+
+static void atmel_aes_queue_task(unsigned long data)
+{
+	struct atmel_aes_dev *dd = (struct atmel_aes_dev *)data;
+
 	atmel_aes_handle_queue(dd, NULL);
 }
 
+static void atmel_aes_done_task(unsigned long data)
+{
+	struct atmel_aes_dev *dd = (struct atmel_aes_dev *)data;
+
+	dd->is_async = true;
+	(void)dd->resume(dd);
+}
+
 static irqreturn_t atmel_aes_irq(int irq, void *dev_id)
 {
 	struct atmel_aes_dev *aes_dd = dev_id;
@@ -1217,10 +1883,14 @@ static void atmel_aes_unregister_algs(struct atmel_aes_dev *dd)
 {
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
-		crypto_unregister_alg(&aes_algs[i]);
+	if (dd->caps.has_gcm)
+		crypto_unregister_aead(&aes_gcm_alg);
+
 	if (dd->caps.has_cfb64)
 		crypto_unregister_alg(&aes_cfb64_alg);
+
+	for (i = 0; i < ARRAY_SIZE(aes_algs); i++)
+		crypto_unregister_alg(&aes_algs[i]);
 }
 
 static int atmel_aes_register_algs(struct atmel_aes_dev *dd)
@@ -1239,8 +1909,16 @@ static int atmel_aes_register_algs(struct atmel_aes_dev *dd)
 			goto err_aes_cfb64_alg;
 	}
 
+	if (dd->caps.has_gcm) {
+		err = crypto_register_aead(&aes_gcm_alg);
+		if (err)
+			goto err_aes_gcm_alg;
+	}
+
 	return 0;
 
+err_aes_gcm_alg:
+	crypto_unregister_alg(&aes_cfb64_alg);
 err_aes_cfb64_alg:
 	i = ARRAY_SIZE(aes_algs);
 err_aes_algs:
@@ -1254,13 +1932,24 @@ static void atmel_aes_get_cap(struct atmel_aes_dev *dd)
 {
 	dd->caps.has_dualbuff = 0;
 	dd->caps.has_cfb64 = 0;
+	dd->caps.has_ctr32 = 0;
+	dd->caps.has_gcm = 0;
 	dd->caps.max_burst_size = 1;
 
 	/* keep only major version number */
 	switch (dd->hw_version & 0xff0) {
+	case 0x500:
+		dd->caps.has_dualbuff = 1;
+		dd->caps.has_cfb64 = 1;
+		dd->caps.has_ctr32 = 1;
+		dd->caps.has_gcm = 1;
+		dd->caps.max_burst_size = 4;
+		break;
 	case 0x200:
 		dd->caps.has_dualbuff = 1;
 		dd->caps.has_cfb64 = 1;
+		dd->caps.has_ctr32 = 1;
+		dd->caps.has_gcm = 1;
 		dd->caps.max_burst_size = 4;
 		break;
 	case 0x130:
@@ -1402,7 +2091,13 @@ static int atmel_aes_probe(struct platform_device *pdev)
 		goto res_err;
 	}
 
-	atmel_aes_hw_version_init(aes_dd);
+	err = clk_prepare(aes_dd->iclk);
+	if (err)
+		goto res_err;
+
+	err = atmel_aes_hw_version_init(aes_dd);
+	if (err)
+		goto iclk_unprepare;
 
 	atmel_aes_get_cap(aes_dd);
 
@@ -1423,8 +2118,8 @@ static int atmel_aes_probe(struct platform_device *pdev)
 		goto err_algs;
 
 	dev_info(dev, "Atmel AES - Using %s, %s for DMA transfers\n",
-			dma_chan_name(aes_dd->dma_lch_in.chan),
-			dma_chan_name(aes_dd->dma_lch_out.chan));
+			dma_chan_name(aes_dd->src.chan),
+			dma_chan_name(aes_dd->dst.chan));
 
 	return 0;
 
@@ -1436,6 +2131,8 @@ err_algs:
 err_aes_dma:
 	atmel_aes_buff_cleanup(aes_dd);
 err_aes_buff:
+iclk_unprepare:
+	clk_unprepare(aes_dd->iclk);
 res_err:
 	tasklet_kill(&aes_dd->done_task);
 	tasklet_kill(&aes_dd->queue_task);
@@ -1462,6 +2159,9 @@ static int atmel_aes_remove(struct platform_device *pdev)
 	tasklet_kill(&aes_dd->queue_task);
 
 	atmel_aes_dma_cleanup(aes_dd);
+	atmel_aes_buff_cleanup(aes_dd);
+
+	clk_unprepare(aes_dd->iclk);
 
 	return 0;
 }
diff --git a/drivers/crypto/atmel-sha.c b/drivers/crypto/atmel-sha.c
index 660d8c06540b..8bf9914d4d15 100644
--- a/drivers/crypto/atmel-sha.c
+++ b/drivers/crypto/atmel-sha.c
@@ -755,7 +755,6 @@ static int atmel_sha_finish(struct ahash_request *req)
 {
 	struct atmel_sha_reqctx *ctx = ahash_request_ctx(req);
 	struct atmel_sha_dev *dd = ctx->dd;
-	int err = 0;
 
 	if (ctx->digcnt[0] || ctx->digcnt[1])
 		atmel_sha_copy_ready_hash(req);
@@ -763,7 +762,7 @@ static int atmel_sha_finish(struct ahash_request *req)
 	dev_dbg(dd->dev, "digcnt: 0x%llx 0x%llx, bufcnt: %d\n", ctx->digcnt[1],
 		ctx->digcnt[0], ctx->bufcnt);
 
-	return err;
+	return 0;
 }
 
 static void atmel_sha_finish_req(struct ahash_request *req, int err)
@@ -783,7 +782,7 @@ static void atmel_sha_finish_req(struct ahash_request *req, int err)
 	dd->flags &= ~(SHA_FLAGS_BUSY | SHA_FLAGS_FINAL | SHA_FLAGS_CPU |
 			SHA_FLAGS_DMA_READY | SHA_FLAGS_OUTPUT_READY);
 
-	clk_disable_unprepare(dd->iclk);
+	clk_disable(dd->iclk);
 
 	if (req->base.complete)
 		req->base.complete(&req->base, err);
@@ -796,7 +795,7 @@ static int atmel_sha_hw_init(struct atmel_sha_dev *dd)
 {
 	int err;
 
-	err = clk_prepare_enable(dd->iclk);
+	err = clk_enable(dd->iclk);
 	if (err)
 		return err;
 
@@ -823,7 +822,7 @@ static void atmel_sha_hw_version_init(struct atmel_sha_dev *dd)
 	dev_info(dd->dev,
 			"version: 0x%x\n", dd->hw_version);
 
-	clk_disable_unprepare(dd->iclk);
+	clk_disable(dd->iclk);
 }
 
 static int atmel_sha_handle_queue(struct atmel_sha_dev *dd,
@@ -1411,6 +1410,10 @@ static int atmel_sha_probe(struct platform_device *pdev)
 		goto res_err;
 	}
 
+	err = clk_prepare(sha_dd->iclk);
+	if (err)
+		goto res_err;
+
 	atmel_sha_hw_version_init(sha_dd);
 
 	atmel_sha_get_cap(sha_dd);
@@ -1422,12 +1425,12 @@ static int atmel_sha_probe(struct platform_device *pdev)
 			if (IS_ERR(pdata)) {
 				dev_err(&pdev->dev, "platform data not available\n");
 				err = PTR_ERR(pdata);
-				goto res_err;
+				goto iclk_unprepare;
 			}
 		}
 		if (!pdata->dma_slave) {
 			err = -ENXIO;
-			goto res_err;
+			goto iclk_unprepare;
 		}
 		err = atmel_sha_dma_init(sha_dd, pdata);
 		if (err)
@@ -1458,6 +1461,8 @@ err_algs:
 	if (sha_dd->caps.has_dma)
 		atmel_sha_dma_cleanup(sha_dd);
 err_sha_dma:
+iclk_unprepare:
+	clk_unprepare(sha_dd->iclk);
 res_err:
 	tasklet_kill(&sha_dd->done_task);
 sha_dd_err:
@@ -1484,12 +1489,7 @@ static int atmel_sha_remove(struct platform_device *pdev)
 	if (sha_dd->caps.has_dma)
 		atmel_sha_dma_cleanup(sha_dd);
 
-	iounmap(sha_dd->io_base);
-
-	clk_put(sha_dd->iclk);
-
-	if (sha_dd->irq >= 0)
-		free_irq(sha_dd->irq, sha_dd);
+	clk_unprepare(sha_dd->iclk);
 
 	return 0;
 }
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index 49106ea42887..5845d4a08797 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -803,6 +803,10 @@ static int ahash_update_ctx(struct ahash_request *req)
 	if (to_hash) {
 		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;
+		}
 		sec4_sg_src_index = 1 + (*buflen ? 1 : 0);
 		sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
 				 sizeof(struct sec4_sg_entry);
@@ -1002,6 +1006,10 @@ static int ahash_finup_ctx(struct ahash_request *req)
 	int sh_len;
 
 	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;
+	}
 	sec4_sg_src_index = 1 + (buflen ? 1 : 0);
 	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
 			 sizeof(struct sec4_sg_entry);
@@ -1086,6 +1094,10 @@ static int ahash_digest(struct ahash_request *req)
 	int sh_len;
 
 	src_nents = sg_count(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);
 
@@ -1234,6 +1246,10 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 	if (to_hash) {
 		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;
+		}
 		sec4_sg_bytes = (1 + src_nents) *
 				sizeof(struct sec4_sg_entry);
 
@@ -1342,6 +1358,10 @@ static int ahash_finup_no_ctx(struct ahash_request *req)
 	int ret = 0;
 
 	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;
+	}
 	sec4_sg_src_index = 2;
 	sec4_sg_bytes = (sec4_sg_src_index + src_nents) *
 			 sizeof(struct sec4_sg_entry);
@@ -1430,6 +1450,10 @@ static int ahash_update_first(struct ahash_request *req)
 
 	if (to_hash) {
 		src_nents = sg_count(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);
 
@@ -1572,7 +1596,7 @@ static int ahash_export(struct ahash_request *req, void *out)
 		len = state->buflen_1;
 	} else {
 		buf = state->buf_0;
-		len = state->buflen_1;
+		len = state->buflen_0;
 	}
 
 	memcpy(export->buf, buf, len);
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 8abb4bc548cc..69d4a1326fee 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -534,8 +534,8 @@ static int caam_probe(struct platform_device *pdev)
 	 * long pointers in master configuration register
 	 */
 	clrsetbits_32(&ctrl->mcr, MCFGR_AWCACHE_MASK, MCFGR_AWCACHE_CACH |
-		      MCFGR_WDENABLE | (sizeof(dma_addr_t) == sizeof(u64) ?
-					MCFGR_LONG_PTR : 0));
+		      MCFGR_AWCACHE_BUFF | MCFGR_WDENABLE |
+		      (sizeof(dma_addr_t) == sizeof(u64) ? MCFGR_LONG_PTR : 0));
 
 	/*
 	 *  Read the Compile Time paramters and SCFGR to determine
diff --git a/drivers/crypto/ccp/Kconfig b/drivers/crypto/ccp/Kconfig
index 3cd8481065f8..6e37845abf8f 100644
--- a/drivers/crypto/ccp/Kconfig
+++ b/drivers/crypto/ccp/Kconfig
@@ -3,6 +3,8 @@ config CRYPTO_DEV_CCP_DD
 	depends on CRYPTO_DEV_CCP
 	default m
 	select HW_RANDOM
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
 	help
 	  Provides the interface to use the AMD Cryptographic Coprocessor
 	  which can be used to offload encryption operations such as SHA,
diff --git a/drivers/crypto/ccp/ccp-ops.c b/drivers/crypto/ccp/ccp-ops.c
index c6e883b296a9..6613aee79b87 100644
--- a/drivers/crypto/ccp/ccp-ops.c
+++ b/drivers/crypto/ccp/ccp-ops.c
@@ -152,32 +152,6 @@ static const __be32 ccp_sha256_init[CCP_SHA_CTXSIZE / sizeof(__be32)] = {
 	cpu_to_be32(SHA256_H6), cpu_to_be32(SHA256_H7),
 };
 
-/* 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.
- */
-static const u8 ccp_sha1_zero[CCP_SHA_CTXSIZE] = {
-	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d,
-	0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90,
-	0xaf, 0xd8, 0x07, 0x09, 0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-};
-
-static const u8 ccp_sha224_zero[CCP_SHA_CTXSIZE] = {
-	0xd1, 0x4a, 0x02, 0x8c, 0x2a, 0x3a, 0x2b, 0xc9,
-	0x47, 0x61, 0x02, 0xbb, 0x28, 0x82, 0x34, 0xc4,
-	0x15, 0xa2, 0xb0, 0x1f, 0x82, 0x8e, 0xa6, 0x2a,
-	0xc5, 0xb3, 0xe4, 0x2f, 0x00, 0x00, 0x00, 0x00,
-};
-
-static const u8 ccp_sha256_zero[CCP_SHA_CTXSIZE] = {
-	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14,
-	0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
-	0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c,
-	0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55,
-};
-
 static u32 ccp_addr_lo(struct ccp_dma_info *info)
 {
 	return lower_32_bits(info->address + info->offset);
@@ -1391,18 +1365,21 @@ static int ccp_run_sha_cmd(struct ccp_cmd_queue *cmd_q, struct ccp_cmd *cmd)
 		if (sha->msg_bits)
 			return -EINVAL;
 
-		/* A sha operation for a message with a total length of zero,
-		 * return known result.
+		/* 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 = ccp_sha1_zero;
+			sha_zero = sha1_zero_message_hash;
 			break;
 		case CCP_SHA_TYPE_224:
-			sha_zero = ccp_sha224_zero;
+			sha_zero = sha224_zero_message_hash;
 			break;
 		case CCP_SHA_TYPE_256:
-			sha_zero = ccp_sha256_zero;
+			sha_zero = sha256_zero_message_hash;
 			break;
 		default:
 			return -EINVAL;
diff --git a/drivers/crypto/ccp/ccp-pci.c b/drivers/crypto/ccp/ccp-pci.c
index 6ade02f04f91..7690467c42f8 100644
--- a/drivers/crypto/ccp/ccp-pci.c
+++ b/drivers/crypto/ccp/ccp-pci.c
@@ -44,7 +44,7 @@ static int ccp_get_msix_irqs(struct ccp_device *ccp)
 {
 	struct ccp_pci *ccp_pci = ccp->dev_specific;
 	struct device *dev = ccp->dev;
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct msix_entry msix_entry[MSIX_VECTORS];
 	unsigned int name_len = sizeof(ccp_pci->msix[0].name) - 1;
 	int v, ret;
@@ -86,7 +86,7 @@ e_irq:
 static int ccp_get_msi_irq(struct ccp_device *ccp)
 {
 	struct device *dev = ccp->dev;
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	int ret;
 
 	ret = pci_enable_msi(pdev);
@@ -133,7 +133,7 @@ static void ccp_free_irqs(struct ccp_device *ccp)
 {
 	struct ccp_pci *ccp_pci = ccp->dev_specific;
 	struct device *dev = ccp->dev;
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 
 	if (ccp_pci->msix_count) {
 		while (ccp_pci->msix_count--)
@@ -149,7 +149,7 @@ static void ccp_free_irqs(struct ccp_device *ccp)
 static int ccp_find_mmio_area(struct ccp_device *ccp)
 {
 	struct device *dev = ccp->dev;
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	resource_size_t io_len;
 	unsigned long io_flags;
 
diff --git a/drivers/crypto/ccp/ccp-platform.c b/drivers/crypto/ccp/ccp-platform.c
index 01b50cb4c982..66dd7c9d08c3 100644
--- a/drivers/crypto/ccp/ccp-platform.c
+++ b/drivers/crypto/ccp/ccp-platform.c
@@ -35,8 +35,7 @@ struct ccp_platform {
 static int ccp_get_irq(struct ccp_device *ccp)
 {
 	struct device *dev = ccp->dev;
-	struct platform_device *pdev = container_of(dev,
-					struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	int ret;
 
 	ret = platform_get_irq(pdev, 0);
@@ -78,8 +77,7 @@ static void ccp_free_irqs(struct ccp_device *ccp)
 static struct resource *ccp_find_mmio_area(struct ccp_device *ccp)
 {
 	struct device *dev = ccp->dev;
-	struct platform_device *pdev = container_of(dev,
-					struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct resource *ior;
 
 	ior = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/crypto/hifn_795x.c b/drivers/crypto/hifn_795x.c
index ca5c71ab4b4d..eee2c7e6c299 100644
--- a/drivers/crypto/hifn_795x.c
+++ b/drivers/crypto/hifn_795x.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 program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
 #include <linux/kernel.h>
@@ -36,14 +32,6 @@
 #include <crypto/algapi.h>
 #include <crypto/des.h>
 
-//#define HIFN_DEBUG
-
-#ifdef HIFN_DEBUG
-#define dprintk(f, a...) 	printk(f, ##a)
-#else
-#define dprintk(f, a...)	do {} while (0)
-#endif
-
 static char hifn_pll_ref[sizeof("extNNN")] = "ext";
 module_param_string(hifn_pll_ref, hifn_pll_ref, sizeof(hifn_pll_ref), 0444);
 MODULE_PARM_DESC(hifn_pll_ref,
@@ -79,12 +67,12 @@ static atomic_t hifn_dev_number;
 
 /* DMA registres */
 
-#define HIFN_DMA_CRA 			0x0C	/* DMA Command Ring Address */
-#define HIFN_DMA_SDRA 			0x1C	/* DMA Source Data Ring Address */
+#define HIFN_DMA_CRA			0x0C	/* DMA Command Ring Address */
+#define HIFN_DMA_SDRA			0x1C	/* DMA Source Data Ring Address */
 #define HIFN_DMA_RRA			0x2C	/* DMA Result Ring Address */
 #define HIFN_DMA_DDRA			0x3C	/* DMA Destination Data Ring Address */
 #define HIFN_DMA_STCTL			0x40	/* DMA Status and Control */
-#define HIFN_DMA_INTREN 		0x44	/* DMA Interrupt Enable */
+#define HIFN_DMA_INTREN			0x44	/* DMA Interrupt Enable */
 #define HIFN_DMA_CFG1			0x48	/* DMA Configuration #1 */
 #define HIFN_DMA_CFG2			0x6C	/* DMA Configuration #2 */
 #define HIFN_CHIP_ID			0x98	/* Chip ID */
@@ -358,10 +346,10 @@ static atomic_t hifn_dev_number;
 #define HIFN_NAMESIZE			32
 #define HIFN_MAX_RESULT_ORDER		5
 
-#define	HIFN_D_CMD_RSIZE		24*1
-#define	HIFN_D_SRC_RSIZE		80*1
-#define	HIFN_D_DST_RSIZE		80*1
-#define	HIFN_D_RES_RSIZE		24*1
+#define	HIFN_D_CMD_RSIZE		(24 * 1)
+#define	HIFN_D_SRC_RSIZE		(80 * 1)
+#define	HIFN_D_DST_RSIZE		(80 * 1)
+#define	HIFN_D_RES_RSIZE		(24 * 1)
 
 #define HIFN_D_DST_DALIGN		4
 
@@ -386,17 +374,16 @@ static atomic_t hifn_dev_number;
 #define	HIFN_MAX_RESULT			(8 + 4 + 4 + 20 + 4)
 #define HIFN_USED_RESULT		12
 
-struct hifn_desc
-{
+struct hifn_desc {
 	volatile __le32		l;
 	volatile __le32		p;
 };
 
 struct hifn_dma {
-	struct hifn_desc	cmdr[HIFN_D_CMD_RSIZE+1];
-	struct hifn_desc	srcr[HIFN_D_SRC_RSIZE+1];
-	struct hifn_desc	dstr[HIFN_D_DST_RSIZE+1];
-	struct hifn_desc	resr[HIFN_D_RES_RSIZE+1];
+	struct hifn_desc	cmdr[HIFN_D_CMD_RSIZE + 1];
+	struct hifn_desc	srcr[HIFN_D_SRC_RSIZE + 1];
+	struct hifn_desc	dstr[HIFN_D_DST_RSIZE + 1];
+	struct hifn_desc	resr[HIFN_D_RES_RSIZE + 1];
 
 	u8			command_bufs[HIFN_D_CMD_RSIZE][HIFN_MAX_COMMAND];
 	u8			result_bufs[HIFN_D_CMD_RSIZE][HIFN_MAX_RESULT];
@@ -410,16 +397,15 @@ struct hifn_dma {
 	int			cmdk, srck, dstk, resk;
 };
 
-#define HIFN_FLAG_CMD_BUSY	(1<<0)
-#define HIFN_FLAG_SRC_BUSY	(1<<1)
-#define HIFN_FLAG_DST_BUSY	(1<<2)
-#define HIFN_FLAG_RES_BUSY	(1<<3)
-#define HIFN_FLAG_OLD_KEY	(1<<4)
+#define HIFN_FLAG_CMD_BUSY	(1 << 0)
+#define HIFN_FLAG_SRC_BUSY	(1 << 1)
+#define HIFN_FLAG_DST_BUSY	(1 << 2)
+#define HIFN_FLAG_RES_BUSY	(1 << 3)
+#define HIFN_FLAG_OLD_KEY	(1 << 4)
 
 #define HIFN_DEFAULT_ACTIVE_NUM	5
 
-struct hifn_device
-{
+struct hifn_device {
 	char			name[HIFN_NAMESIZE];
 
 	int			irq;
@@ -432,7 +418,7 @@ struct hifn_device
 
 	u32			dmareg;
 
-	void 			*sa[HIFN_D_RES_RSIZE];
+	void			*sa[HIFN_D_RES_RSIZE];
 
 	spinlock_t		lock;
 
@@ -447,7 +433,7 @@ struct hifn_device
 
 	struct tasklet_struct	tasklet;
 
-	struct crypto_queue 	queue;
+	struct crypto_queue	queue;
 	struct list_head	alg_list;
 
 	unsigned int		pk_clk_freq;
@@ -468,8 +454,7 @@ struct hifn_device
 #define	HIFN_D_JUMP			0x40000000
 #define	HIFN_D_VALID			0x80000000
 
-struct hifn_base_command
-{
+struct hifn_base_command {
 	volatile __le16		masks;
 	volatile __le16		session_num;
 	volatile __le16		total_source_count;
@@ -491,12 +476,11 @@ struct hifn_base_command
 /*
  * Structure to help build up the command data structure.
  */
-struct hifn_crypt_command
-{
-	volatile __le16 		masks;
-	volatile __le16 		header_skip;
-	volatile __le16 		source_count;
-	volatile __le16 		reserved;
+struct hifn_crypt_command {
+	volatile __le16		masks;
+	volatile __le16		header_skip;
+	volatile __le16		source_count;
+	volatile __le16		reserved;
 };
 
 #define	HIFN_CRYPT_CMD_ALG_MASK		0x0003		/* algorithm: */
@@ -522,12 +506,11 @@ struct hifn_crypt_command
 /*
  * Structure to help build up the command data structure.
  */
-struct hifn_mac_command
-{
-	volatile __le16 	masks;
-	volatile __le16 	header_skip;
-	volatile __le16 	source_count;
-	volatile __le16 	reserved;
+struct hifn_mac_command {
+	volatile __le16	masks;
+	volatile __le16	header_skip;
+	volatile __le16	source_count;
+	volatile __le16	reserved;
 };
 
 #define	HIFN_MAC_CMD_ALG_MASK		0x0001
@@ -551,12 +534,11 @@ struct hifn_mac_command
 #define	HIFN_MAC_CMD_POS_IPSEC		0x0200
 #define	HIFN_MAC_CMD_NEW_KEY		0x0800
 
-struct hifn_comp_command
-{
-	volatile __le16 	masks;
-	volatile __le16 	header_skip;
-	volatile __le16 	source_count;
-	volatile __le16 	reserved;
+struct hifn_comp_command {
+	volatile __le16		masks;
+	volatile __le16		header_skip;
+	volatile __le16		source_count;
+	volatile __le16		reserved;
 };
 
 #define	HIFN_COMP_CMD_SRCLEN_M		0xc000
@@ -570,12 +552,11 @@ struct hifn_comp_command
 #define	HIFN_COMP_CMD_ALG_MPPC		0x0001	/*   MPPC */
 #define	HIFN_COMP_CMD_ALG_LZS		0x0000	/*   LZS */
 
-struct hifn_base_result
-{
-	volatile __le16 	flags;
-	volatile __le16 	session;
-	volatile __le16 	src_cnt;		/* 15:0 of source count */
-	volatile __le16 	dst_cnt;		/* 15:0 of dest count */
+struct hifn_base_result {
+	volatile __le16		flags;
+	volatile __le16		session;
+	volatile __le16		src_cnt;		/* 15:0 of source count */
+	volatile __le16		dst_cnt;		/* 15:0 of dest count */
 };
 
 #define	HIFN_BASE_RES_DSTOVERRUN	0x0200	/* destination overrun */
@@ -584,8 +565,7 @@ struct hifn_base_result
 #define	HIFN_BASE_RES_DSTLEN_M		0x3000	/* 17:16 of dest count */
 #define	HIFN_BASE_RES_DSTLEN_S		12
 
-struct hifn_comp_result
-{
+struct hifn_comp_result {
 	volatile __le16		flags;
 	volatile __le16		crc;
 };
@@ -596,18 +576,16 @@ struct hifn_comp_result
 #define	HIFN_COMP_RES_ENDMARKER		0x0002	/* LZS: end marker seen */
 #define	HIFN_COMP_RES_SRC_NOTZERO	0x0001	/* source expired */
 
-struct hifn_mac_result
-{
-	volatile __le16 	flags;
-	volatile __le16 	reserved;
+struct hifn_mac_result {
+	volatile __le16		flags;
+	volatile __le16		reserved;
 	/* followed by 0, 6, 8, or 10 u16's of the MAC, then crypt */
 };
 
 #define	HIFN_MAC_RES_MISCOMPARE		0x0002	/* compare failed */
 #define	HIFN_MAC_RES_SRC_NOTZERO	0x0001	/* source expired */
 
-struct hifn_crypt_result
-{
+struct hifn_crypt_result {
 	volatile __le16		flags;
 	volatile __le16		reserved;
 };
@@ -622,11 +600,10 @@ struct hifn_crypt_result
 #define	HIFN_POLL_SCALAR	0x0
 #endif
 
-#define	HIFN_MAX_SEGLEN 	0xffff		/* maximum dma segment len */
+#define	HIFN_MAX_SEGLEN		0xffff		/* maximum dma segment len */
 #define	HIFN_MAX_DMALEN		0x3ffff		/* maximum dma length */
 
-struct hifn_crypto_alg
-{
+struct hifn_crypto_alg {
 	struct list_head	entry;
 	struct crypto_alg	alg;
 	struct hifn_device	*dev;
@@ -634,24 +611,21 @@ struct hifn_crypto_alg
 
 #define ASYNC_SCATTERLIST_CACHE	16
 
-#define ASYNC_FLAGS_MISALIGNED	(1<<0)
+#define ASYNC_FLAGS_MISALIGNED	(1 << 0)
 
-struct hifn_cipher_walk
-{
+struct hifn_cipher_walk {
 	struct scatterlist	cache[ASYNC_SCATTERLIST_CACHE];
 	u32			flags;
 	int			num;
 };
 
-struct hifn_context
-{
+struct hifn_context {
 	u8			key[HIFN_MAX_CRYPT_KEY_LENGTH];
 	struct hifn_device	*dev;
 	unsigned int		keysize;
 };
 
-struct hifn_request_context
-{
+struct hifn_request_context {
 	u8			*iv;
 	unsigned int		ivsize;
 	u8			op, type, mode, unused;
@@ -693,7 +667,7 @@ static void hifn_wait_puc(struct hifn_device *dev)
 	int i;
 	u32 ret;
 
-	for (i=10000; i > 0; --i) {
+	for (i = 10000; i > 0; --i) {
 		ret = hifn_read_0(dev, HIFN_0_PUCTRL);
 		if (!(ret & HIFN_PUCTRL_RESET))
 			break;
@@ -702,7 +676,7 @@ static void hifn_wait_puc(struct hifn_device *dev)
 	}
 
 	if (!i)
-		dprintk("%s: Failed to reset PUC unit.\n", dev->name);
+		dev_err(&dev->pdev->dev, "Failed to reset PUC unit.\n");
 }
 
 static void hifn_reset_puc(struct hifn_device *dev)
@@ -749,13 +723,12 @@ static void hifn_reset_dma(struct hifn_device *dev, int full)
 	hifn_reset_puc(dev);
 }
 
-static u32 hifn_next_signature(u_int32_t a, u_int cnt)
+static u32 hifn_next_signature(u32 a, u_int cnt)
 {
 	int i;
 	u32 v;
 
 	for (i = 0; i < cnt; i++) {
-
 		/* get the parity */
 		v = a & 0x80080125;
 		v ^= v >> 16;
@@ -846,33 +819,28 @@ static int hifn_init_pubrng(struct hifn_device *dev)
 	hifn_write_1(dev, HIFN_1_PUB_RESET, hifn_read_1(dev, HIFN_1_PUB_RESET) |
 			HIFN_PUBRST_RESET);
 
-	for (i=100; i > 0; --i) {
+	for (i = 100; i > 0; --i) {
 		mdelay(1);
 
 		if ((hifn_read_1(dev, HIFN_1_PUB_RESET) & HIFN_PUBRST_RESET) == 0)
 			break;
 	}
 
-	if (!i)
-		dprintk("Chip %s: Failed to initialise public key engine.\n",
-				dev->name);
-	else {
+	if (!i) {
+		dev_err(&dev->pdev->dev, "Failed to initialise public key engine.\n");
+	} else {
 		hifn_write_1(dev, HIFN_1_PUB_IEN, HIFN_PUBIEN_DONE);
 		dev->dmareg |= HIFN_DMAIER_PUBDONE;
 		hifn_write_1(dev, HIFN_1_DMA_IER, dev->dmareg);
 
-		dprintk("Chip %s: Public key engine has been successfully "
-				"initialised.\n", dev->name);
+		dev_dbg(&dev->pdev->dev, "Public key engine has been successfully initialised.\n");
 	}
 
-	/*
-	 * Enable RNG engine.
-	 */
+	/* Enable RNG engine. */
 
 	hifn_write_1(dev, HIFN_1_RNG_CONFIG,
 			hifn_read_1(dev, HIFN_1_RNG_CONFIG) | HIFN_RNGCFG_ENA);
-	dprintk("Chip %s: RNG engine has been successfully initialised.\n",
-			dev->name);
+	dev_dbg(&dev->pdev->dev, "RNG engine has been successfully initialised.\n");
 
 #ifdef CONFIG_CRYPTO_DEV_HIFN_795X_RNG
 	/* First value must be discarded */
@@ -896,8 +864,8 @@ static int hifn_enable_crypto(struct hifn_device *dev)
 		}
 	}
 
-	if (offtbl == NULL) {
-		dprintk("Chip %s: Unknown card!\n", dev->name);
+	if (!offtbl) {
+		dev_err(&dev->pdev->dev, "Unknown card!\n");
 		return -ENODEV;
 	}
 
@@ -912,7 +880,7 @@ static int hifn_enable_crypto(struct hifn_device *dev)
 	hifn_write_1(dev, HIFN_1_UNLOCK_SECRET2, 0);
 	mdelay(1);
 
-	for (i=0; i<12; ++i) {
+	for (i = 0; i < 12; ++i) {
 		addr = hifn_next_signature(addr, offtbl[i] + 0x101);
 		hifn_write_1(dev, HIFN_1_UNLOCK_SECRET2, addr);
 
@@ -920,7 +888,7 @@ static int hifn_enable_crypto(struct hifn_device *dev)
 	}
 	hifn_write_1(dev, HIFN_1_DMA_CNFG, dmacfg);
 
-	dprintk("Chip %s: %s.\n", dev->name, pci_name(dev->pdev));
+	dev_dbg(&dev->pdev->dev, "%s %s.\n", dev->name, pci_name(dev->pdev));
 
 	return 0;
 }
@@ -931,16 +899,14 @@ static void hifn_init_dma(struct hifn_device *dev)
 	u32 dptr = dev->desc_dma;
 	int i;
 
-	for (i=0; i<HIFN_D_CMD_RSIZE; ++i)
+	for (i = 0; i < HIFN_D_CMD_RSIZE; ++i)
 		dma->cmdr[i].p = __cpu_to_le32(dptr +
 				offsetof(struct hifn_dma, command_bufs[i][0]));
-	for (i=0; i<HIFN_D_RES_RSIZE; ++i)
+	for (i = 0; i < HIFN_D_RES_RSIZE; ++i)
 		dma->resr[i].p = __cpu_to_le32(dptr +
 				offsetof(struct hifn_dma, result_bufs[i][0]));
 
-	/*
-	 * Setup LAST descriptors.
-	 */
+	/* Setup LAST descriptors. */
 	dma->cmdr[HIFN_D_CMD_RSIZE].p = __cpu_to_le32(dptr +
 			offsetof(struct hifn_dma, cmdr[0]));
 	dma->srcr[HIFN_D_SRC_RSIZE].p = __cpu_to_le32(dptr +
@@ -960,7 +926,7 @@ static void hifn_init_dma(struct hifn_device *dev)
  * to calculate the optimal multiplier. For PCI we assume 66MHz, since that
  * allows us to operate without the risk of overclocking the chip. If it
  * actually uses 33MHz, the chip will operate at half the speed, this can be
- * overriden by specifying the frequency as module parameter (pci33).
+ * overridden by specifying the frequency as module parameter (pci33).
  *
  * Unfortunately the PCI clock is not very suitable since the HIFN needs a
  * stable clock and the PCI clock frequency may vary, so the default is the
@@ -984,9 +950,8 @@ static void hifn_init_pll(struct hifn_device *dev)
 		freq = simple_strtoul(hifn_pll_ref + 3, NULL, 10);
 	else {
 		freq = 66;
-		printk(KERN_INFO "hifn795x: assuming %uMHz clock speed, "
-				 "override with hifn_pll_ref=%.3s<frequency>\n",
-		       freq, hifn_pll_ref);
+		dev_info(&dev->pdev->dev, "assuming %uMHz clock speed, override with hifn_pll_ref=%.3s<frequency>\n",
+			 freq, hifn_pll_ref);
 	}
 
 	m = HIFN_PLL_FCK_MAX / freq;
@@ -1174,17 +1139,17 @@ static int hifn_setup_cmd_desc(struct hifn_device *dev,
 
 	mask = 0;
 	switch (rctx->op) {
-		case ACRYPTO_OP_DECRYPT:
-			mask = HIFN_BASE_CMD_CRYPT | HIFN_BASE_CMD_DECODE;
-			break;
-		case ACRYPTO_OP_ENCRYPT:
-			mask = HIFN_BASE_CMD_CRYPT;
-			break;
-		case ACRYPTO_OP_HMAC:
-			mask = HIFN_BASE_CMD_MAC;
-			break;
-		default:
-			goto err_out;
+	case ACRYPTO_OP_DECRYPT:
+		mask = HIFN_BASE_CMD_CRYPT | HIFN_BASE_CMD_DECODE;
+		break;
+	case ACRYPTO_OP_ENCRYPT:
+		mask = HIFN_BASE_CMD_CRYPT;
+		break;
+	case ACRYPTO_OP_HMAC:
+		mask = HIFN_BASE_CMD_MAC;
+		break;
+	default:
+		goto err_out;
 	}
 
 	buf_pos += hifn_setup_base_command(dev, buf_pos, nbytes,
@@ -1199,53 +1164,53 @@ static int hifn_setup_cmd_desc(struct hifn_device *dev,
 			md |= HIFN_CRYPT_CMD_NEW_IV;
 
 		switch (rctx->mode) {
-			case ACRYPTO_MODE_ECB:
-				md |= HIFN_CRYPT_CMD_MODE_ECB;
-				break;
-			case ACRYPTO_MODE_CBC:
-				md |= HIFN_CRYPT_CMD_MODE_CBC;
-				break;
-			case ACRYPTO_MODE_CFB:
-				md |= HIFN_CRYPT_CMD_MODE_CFB;
-				break;
-			case ACRYPTO_MODE_OFB:
-				md |= HIFN_CRYPT_CMD_MODE_OFB;
-				break;
-			default:
-				goto err_out;
+		case ACRYPTO_MODE_ECB:
+			md |= HIFN_CRYPT_CMD_MODE_ECB;
+			break;
+		case ACRYPTO_MODE_CBC:
+			md |= HIFN_CRYPT_CMD_MODE_CBC;
+			break;
+		case ACRYPTO_MODE_CFB:
+			md |= HIFN_CRYPT_CMD_MODE_CFB;
+			break;
+		case ACRYPTO_MODE_OFB:
+			md |= HIFN_CRYPT_CMD_MODE_OFB;
+			break;
+		default:
+			goto err_out;
 		}
 
 		switch (rctx->type) {
-			case ACRYPTO_TYPE_AES_128:
-				if (ctx->keysize != 16)
-					goto err_out;
-				md |= HIFN_CRYPT_CMD_KSZ_128 |
-					HIFN_CRYPT_CMD_ALG_AES;
-				break;
-			case ACRYPTO_TYPE_AES_192:
-				if (ctx->keysize != 24)
-					goto err_out;
-				md |= HIFN_CRYPT_CMD_KSZ_192 |
-					HIFN_CRYPT_CMD_ALG_AES;
-				break;
-			case ACRYPTO_TYPE_AES_256:
-				if (ctx->keysize != 32)
-					goto err_out;
-				md |= HIFN_CRYPT_CMD_KSZ_256 |
-					HIFN_CRYPT_CMD_ALG_AES;
-				break;
-			case ACRYPTO_TYPE_3DES:
-				if (ctx->keysize != 24)
-					goto err_out;
-				md |= HIFN_CRYPT_CMD_ALG_3DES;
-				break;
-			case ACRYPTO_TYPE_DES:
-				if (ctx->keysize != 8)
-					goto err_out;
-				md |= HIFN_CRYPT_CMD_ALG_DES;
-				break;
-			default:
+		case ACRYPTO_TYPE_AES_128:
+			if (ctx->keysize != 16)
+				goto err_out;
+			md |= HIFN_CRYPT_CMD_KSZ_128 |
+				HIFN_CRYPT_CMD_ALG_AES;
+			break;
+		case ACRYPTO_TYPE_AES_192:
+			if (ctx->keysize != 24)
 				goto err_out;
+			md |= HIFN_CRYPT_CMD_KSZ_192 |
+				HIFN_CRYPT_CMD_ALG_AES;
+			break;
+		case ACRYPTO_TYPE_AES_256:
+			if (ctx->keysize != 32)
+				goto err_out;
+			md |= HIFN_CRYPT_CMD_KSZ_256 |
+				HIFN_CRYPT_CMD_ALG_AES;
+			break;
+		case ACRYPTO_TYPE_3DES:
+			if (ctx->keysize != 24)
+				goto err_out;
+			md |= HIFN_CRYPT_CMD_ALG_3DES;
+			break;
+		case ACRYPTO_TYPE_DES:
+			if (ctx->keysize != 8)
+				goto err_out;
+			md |= HIFN_CRYPT_CMD_ALG_DES;
+			break;
+		default:
+			goto err_out;
 		}
 
 		buf_pos += hifn_setup_crypto_command(dev, buf_pos,
@@ -1265,8 +1230,9 @@ static int hifn_setup_cmd_desc(struct hifn_device *dev,
 			HIFN_D_VALID | HIFN_D_LAST |
 			HIFN_D_MASKDONEIRQ | HIFN_D_JUMP);
 		dma->cmdi = 0;
-	} else
-		dma->cmdr[dma->cmdi-1].l |= __cpu_to_le32(HIFN_D_VALID);
+	} else {
+		dma->cmdr[dma->cmdi - 1].l |= __cpu_to_le32(HIFN_D_VALID);
+	}
 
 	if (!(dev->flags & HIFN_FLAG_CMD_BUSY)) {
 		hifn_write_1(dev, HIFN_1_DMA_CSR, HIFN_DMACSR_C_CTRL_ENA);
@@ -1424,7 +1390,7 @@ static int hifn_cipher_walk_init(struct hifn_cipher_walk *w,
 	sg_init_table(w->cache, num);
 
 	w->num = 0;
-	for (i=0; i<num; ++i) {
+	for (i = 0; i < num; ++i) {
 		struct page *page = alloc_page(gfp_flags);
 		struct scatterlist *s;
 
@@ -1444,7 +1410,7 @@ static void hifn_cipher_walk_exit(struct hifn_cipher_walk *w)
 {
 	int i;
 
-	for (i=0; i<w->num; ++i) {
+	for (i = 0; i < w->num; ++i) {
 		struct scatterlist *s = &w->cache[i];
 
 		__free_page(sg_page(s));
@@ -1471,8 +1437,8 @@ static int ablkcipher_add(unsigned int *drestp, struct scatterlist *dst,
 		drest -= copy;
 		nbytes -= copy;
 
-		dprintk("%s: copy: %u, size: %u, drest: %u, nbytes: %u.\n",
-				__func__, copy, size, drest, nbytes);
+		pr_debug("%s: copy: %u, size: %u, drest: %u, nbytes: %u.\n",
+			 __func__, copy, size, drest, nbytes);
 
 		dst++;
 		idx++;
@@ -1499,8 +1465,8 @@ static int hifn_cipher_walk(struct ablkcipher_request *req,
 
 		dst = &req->dst[idx];
 
-		dprintk("\n%s: dlen: %u, doff: %u, offset: %u, nbytes: %u.\n",
-			__func__, dst->length, dst->offset, offset, nbytes);
+		pr_debug("\n%s: dlen: %u, doff: %u, offset: %u, nbytes: %u.\n",
+			 __func__, dst->length, dst->offset, offset, nbytes);
 
 		if (!IS_ALIGNED(dst->offset, HIFN_D_DST_DALIGN) ||
 		    !IS_ALIGNED(dst->length, HIFN_D_DST_DALIGN) ||
@@ -1525,10 +1491,10 @@ static int hifn_cipher_walk(struct ablkcipher_request *req,
 				 * to put there additional blocksized chunk,
 				 * so we mark that page as containing only
 				 * blocksize aligned chunks:
-				 * 	t->length = (slen & ~(HIFN_D_DST_DALIGN - 1));
+				 *	t->length = (slen & ~(HIFN_D_DST_DALIGN - 1));
 				 * and increase number of bytes to be processed
 				 * in next chunk:
-				 * 	nbytes += diff;
+				 *	nbytes += diff;
 				 */
 				nbytes += diff;
 
@@ -1536,14 +1502,13 @@ static int hifn_cipher_walk(struct ablkcipher_request *req,
 				 * Temporary of course...
 				 * Kick author if you will catch this one.
 				 */
-				printk(KERN_ERR "%s: dlen: %u, nbytes: %u,"
-					"slen: %u, offset: %u.\n",
-					__func__, dlen, nbytes, slen, offset);
-				printk(KERN_ERR "%s: please contact author to fix this "
-					"issue, generally you should not catch "
-					"this path under any condition but who "
-					"knows how did you use crypto code.\n"
-					"Thank you.\n",	__func__);
+				pr_err("%s: dlen: %u, nbytes: %u, slen: %u, offset: %u.\n",
+				       __func__, dlen, nbytes, slen, offset);
+				pr_err("%s: please contact author to fix this "
+				       "issue, generally you should not catch "
+				       "this path under any condition but who "
+				       "knows how did you use crypto code.\n"
+				       "Thank you.\n",	__func__);
 				BUG();
 			} else {
 				copy += diff + nbytes;
@@ -1630,70 +1595,16 @@ err_out:
 	spin_unlock_irqrestore(&dev->lock, flags);
 err_out_exit:
 	if (err) {
-		printk("%s: iv: %p [%d], key: %p [%d], mode: %u, op: %u, "
-				"type: %u, err: %d.\n",
-			dev->name, rctx->iv, rctx->ivsize,
-			ctx->key, ctx->keysize,
-			rctx->mode, rctx->op, rctx->type, err);
+		dev_info(&dev->pdev->dev, "iv: %p [%d], key: %p [%d], mode: %u, op: %u, "
+			 "type: %u, err: %d.\n",
+			 rctx->iv, rctx->ivsize,
+			 ctx->key, ctx->keysize,
+			 rctx->mode, rctx->op, rctx->type, err);
 	}
 
 	return err;
 }
 
-static int hifn_test(struct hifn_device *dev, int encdec, u8 snum)
-{
-	int n, err;
-	u8 src[16];
-	struct hifn_context ctx;
-	struct hifn_request_context rctx;
-	u8 fips_aes_ecb_from_zero[16] = {
-		0x66, 0xE9, 0x4B, 0xD4,
-		0xEF, 0x8A, 0x2C, 0x3B,
-		0x88, 0x4C, 0xFA, 0x59,
-		0xCA, 0x34, 0x2B, 0x2E};
-	struct scatterlist sg;
-
-	memset(src, 0, sizeof(src));
-	memset(ctx.key, 0, sizeof(ctx.key));
-
-	ctx.dev = dev;
-	ctx.keysize = 16;
-	rctx.ivsize = 0;
-	rctx.iv = NULL;
-	rctx.op = (encdec)?ACRYPTO_OP_ENCRYPT:ACRYPTO_OP_DECRYPT;
-	rctx.mode = ACRYPTO_MODE_ECB;
-	rctx.type = ACRYPTO_TYPE_AES_128;
-	rctx.walk.cache[0].length = 0;
-
-	sg_init_one(&sg, &src, sizeof(src));
-
-	err = hifn_setup_dma(dev, &ctx, &rctx, &sg, &sg, sizeof(src), NULL);
-	if (err)
-		goto err_out;
-
-	dev->started = 0;
-	msleep(200);
-
-	dprintk("%s: decoded: ", dev->name);
-	for (n=0; n<sizeof(src); ++n)
-		dprintk("%02x ", src[n]);
-	dprintk("\n");
-	dprintk("%s: FIPS   : ", dev->name);
-	for (n=0; n<sizeof(fips_aes_ecb_from_zero); ++n)
-		dprintk("%02x ", fips_aes_ecb_from_zero[n]);
-	dprintk("\n");
-
-	if (!memcmp(src, fips_aes_ecb_from_zero, sizeof(fips_aes_ecb_from_zero))) {
-		printk(KERN_INFO "%s: AES 128 ECB test has been successfully "
-				"passed.\n", dev->name);
-		return 0;
-	}
-
-err_out:
-	printk(KERN_INFO "%s: AES 128 ECB test has been failed.\n", dev->name);
-	return -1;
-}
-
 static int hifn_start_device(struct hifn_device *dev)
 {
 	int err;
@@ -1739,8 +1650,8 @@ static int ablkcipher_get(void *saddr, unsigned int *srestp, unsigned int offset
 		saddr += copy;
 		offset = 0;
 
-		dprintk("%s: copy: %u, size: %u, srest: %u, nbytes: %u.\n",
-				__func__, copy, size, srest, nbytes);
+		pr_debug("%s: copy: %u, size: %u, srest: %u, nbytes: %u.\n",
+			 __func__, copy, size, srest, nbytes);
 
 		dst++;
 		idx++;
@@ -1760,7 +1671,8 @@ static inline void hifn_complete_sa(struct hifn_device *dev, int i)
 	dev->sa[i] = NULL;
 	dev->started--;
 	if (dev->started < 0)
-		printk("%s: started: %d.\n", __func__, dev->started);
+		dev_info(&dev->pdev->dev, "%s: started: %d.\n", __func__,
+			 dev->started);
 	spin_unlock_irqrestore(&dev->lock, flags);
 	BUG_ON(dev->started < 0);
 }
@@ -1779,7 +1691,7 @@ static void hifn_process_ready(struct ablkcipher_request *req, int error)
 			t = &rctx->walk.cache[idx];
 			dst = &req->dst[idx];
 
-			dprintk("\n%s: sg_page(t): %p, t->length: %u, "
+			pr_debug("\n%s: sg_page(t): %p, t->length: %u, "
 				"sg_page(dst): %p, dst->length: %u, "
 				"nbytes: %u.\n",
 				__func__, sg_page(t), t->length,
@@ -1815,9 +1727,8 @@ static void hifn_clear_rings(struct hifn_device *dev, int error)
 	struct hifn_dma *dma = (struct hifn_dma *)dev->desc_virt;
 	int i, u;
 
-	dprintk("%s: ring cleanup 1: i: %d.%d.%d.%d, u: %d.%d.%d.%d, "
+	dev_dbg(&dev->pdev->dev, "ring cleanup 1: i: %d.%d.%d.%d, u: %d.%d.%d.%d, "
 			"k: %d.%d.%d.%d.\n",
-			dev->name,
 			dma->cmdi, dma->srci, dma->dsti, dma->resi,
 			dma->cmdu, dma->srcu, dma->dstu, dma->resu,
 			dma->cmdk, dma->srck, dma->dstk, dma->resk);
@@ -1870,9 +1781,8 @@ static void hifn_clear_rings(struct hifn_device *dev, int error)
 	}
 	dma->dstk = i; dma->dstu = u;
 
-	dprintk("%s: ring cleanup 2: i: %d.%d.%d.%d, u: %d.%d.%d.%d, "
+	dev_dbg(&dev->pdev->dev, "ring cleanup 2: i: %d.%d.%d.%d, u: %d.%d.%d.%d, "
 			"k: %d.%d.%d.%d.\n",
-			dev->name,
 			dma->cmdi, dma->srci, dma->dsti, dma->resi,
 			dma->cmdu, dma->srcu, dma->dstu, dma->resu,
 			dma->cmdk, dma->srck, dma->dstk, dma->resk);
@@ -1921,21 +1831,22 @@ static void hifn_work(struct work_struct *work)
 			int i;
 			struct hifn_dma *dma = (struct hifn_dma *)dev->desc_virt;
 
-			printk("%s: r: %08x, active: %d, started: %d, "
-				"success: %lu: qlen: %u/%u, reset: %d.\n",
-				dev->name, r, dev->active, dev->started,
-				dev->success, dev->queue.qlen, dev->queue.max_qlen,
-				reset);
+			dev_info(&dev->pdev->dev,
+				 "r: %08x, active: %d, started: %d, "
+				 "success: %lu: qlen: %u/%u, reset: %d.\n",
+				 r, dev->active, dev->started,
+				 dev->success, dev->queue.qlen, dev->queue.max_qlen,
+				 reset);
 
-			printk("%s: res: ", __func__);
-			for (i=0; i<HIFN_D_RES_RSIZE; ++i) {
-				printk("%x.%p ", dma->resr[i].l, dev->sa[i]);
+			dev_info(&dev->pdev->dev, "%s: res: ", __func__);
+			for (i = 0; i < HIFN_D_RES_RSIZE; ++i) {
+				pr_info("%x.%p ", dma->resr[i].l, dev->sa[i]);
 				if (dev->sa[i]) {
 					hifn_process_ready(dev->sa[i], -ENODEV);
 					hifn_complete_sa(dev, i);
 				}
 			}
-			printk("\n");
+			pr_info("\n");
 
 			hifn_reset_dma(dev, 1);
 			hifn_stop_device(dev);
@@ -1957,9 +1868,9 @@ static irqreturn_t hifn_interrupt(int irq, void *data)
 
 	dmacsr = hifn_read_1(dev, HIFN_1_DMA_CSR);
 
-	dprintk("%s: 1 dmacsr: %08x, dmareg: %08x, res: %08x [%d], "
+	dev_dbg(&dev->pdev->dev, "1 dmacsr: %08x, dmareg: %08x, res: %08x [%d], "
 			"i: %d.%d.%d.%d, u: %d.%d.%d.%d.\n",
-		dev->name, dmacsr, dev->dmareg, dmacsr & dev->dmareg, dma->cmdi,
+		dmacsr, dev->dmareg, dmacsr & dev->dmareg, dma->cmdi,
 		dma->cmdi, dma->srci, dma->dsti, dma->resi,
 		dma->cmdu, dma->srcu, dma->dstu, dma->resu);
 
@@ -1978,9 +1889,9 @@ static irqreturn_t hifn_interrupt(int irq, void *data)
 	if (restart) {
 		u32 puisr = hifn_read_0(dev, HIFN_0_PUISR);
 
-		printk(KERN_WARNING "%s: overflow: r: %d, d: %d, puisr: %08x, d: %u.\n",
-			dev->name, !!(dmacsr & HIFN_DMACSR_R_OVER),
-			!!(dmacsr & HIFN_DMACSR_D_OVER),
+		dev_warn(&dev->pdev->dev, "overflow: r: %d, d: %d, puisr: %08x, d: %u.\n",
+			 !!(dmacsr & HIFN_DMACSR_R_OVER),
+			 !!(dmacsr & HIFN_DMACSR_D_OVER),
 			puisr, !!(puisr & HIFN_PUISR_DSTOVER));
 		if (!!(puisr & HIFN_PUISR_DSTOVER))
 			hifn_write_0(dev, HIFN_0_PUISR, HIFN_PUISR_DSTOVER);
@@ -1991,18 +1902,18 @@ static irqreturn_t hifn_interrupt(int irq, void *data)
 	restart = dmacsr & (HIFN_DMACSR_C_ABORT | HIFN_DMACSR_S_ABORT |
 			HIFN_DMACSR_D_ABORT | HIFN_DMACSR_R_ABORT);
 	if (restart) {
-		printk(KERN_WARNING "%s: abort: c: %d, s: %d, d: %d, r: %d.\n",
-			dev->name, !!(dmacsr & HIFN_DMACSR_C_ABORT),
-			!!(dmacsr & HIFN_DMACSR_S_ABORT),
-			!!(dmacsr & HIFN_DMACSR_D_ABORT),
-			!!(dmacsr & HIFN_DMACSR_R_ABORT));
+		dev_warn(&dev->pdev->dev, "abort: c: %d, s: %d, d: %d, r: %d.\n",
+			 !!(dmacsr & HIFN_DMACSR_C_ABORT),
+			 !!(dmacsr & HIFN_DMACSR_S_ABORT),
+			 !!(dmacsr & HIFN_DMACSR_D_ABORT),
+			 !!(dmacsr & HIFN_DMACSR_R_ABORT));
 		hifn_reset_dma(dev, 1);
 		hifn_init_dma(dev);
 		hifn_init_registers(dev);
 	}
 
 	if ((dmacsr & HIFN_DMACSR_C_WAIT) && (dma->cmdu == 0)) {
-		dprintk("%s: wait on command.\n", dev->name);
+		dev_dbg(&dev->pdev->dev, "wait on command.\n");
 		dev->dmareg &= ~(HIFN_DMAIER_C_WAIT);
 		hifn_write_1(dev, HIFN_1_DMA_IER, dev->dmareg);
 	}
@@ -2020,19 +1931,19 @@ static void hifn_flush(struct hifn_device *dev)
 	struct hifn_dma *dma = (struct hifn_dma *)dev->desc_virt;
 	int i;
 
-	for (i=0; i<HIFN_D_RES_RSIZE; ++i) {
+	for (i = 0; i < HIFN_D_RES_RSIZE; ++i) {
 		struct hifn_desc *d = &dma->resr[i];
 
 		if (dev->sa[i]) {
 			hifn_process_ready(dev->sa[i],
-				(d->l & __cpu_to_le32(HIFN_D_VALID))?-ENODEV:0);
+				(d->l & __cpu_to_le32(HIFN_D_VALID)) ? -ENODEV : 0);
 			hifn_complete_sa(dev, i);
 		}
 	}
 
 	spin_lock_irqsave(&dev->lock, flags);
 	while ((async_req = crypto_dequeue_request(&dev->queue))) {
-		req = container_of(async_req, struct ablkcipher_request, base);
+		req = ablkcipher_request_cast(async_req);
 		spin_unlock_irqrestore(&dev->lock, flags);
 
 		hifn_process_ready(req, -ENODEV);
@@ -2057,7 +1968,7 @@ static int hifn_setkey(struct crypto_ablkcipher *cipher, const u8 *key,
 	if (len == HIFN_DES_KEY_LENGTH) {
 		u32 tmp[DES_EXPKEY_WORDS];
 		int ret = des_ekey(tmp, key);
-		
+
 		if (unlikely(ret == 0) && (tfm->crt_flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
 			tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
 			return -EINVAL;
@@ -2151,7 +2062,7 @@ static int hifn_process_queue(struct hifn_device *dev)
 		if (backlog)
 			backlog->complete(backlog, -EINPROGRESS);
 
-		req = container_of(async_req, struct ablkcipher_request, base);
+		req = ablkcipher_request_cast(async_req);
 
 		err = hifn_handle_req(req);
 		if (err)
@@ -2298,9 +2209,7 @@ static inline int hifn_encrypt_3des_ofb(struct ablkcipher_request *req)
 			ACRYPTO_TYPE_3DES, ACRYPTO_MODE_OFB);
 }
 
-/*
- * 3DES decryption functions.
- */
+/* 3DES decryption functions. */
 static inline int hifn_decrypt_3des_ecb(struct ablkcipher_request *req)
 {
 	return hifn_setup_crypto(req, ACRYPTO_OP_DECRYPT,
@@ -2322,8 +2231,7 @@ static inline int hifn_decrypt_3des_ofb(struct ablkcipher_request *req)
 			ACRYPTO_TYPE_3DES, ACRYPTO_MODE_OFB);
 }
 
-struct hifn_alg_template
-{
+struct hifn_alg_template {
 	char name[CRYPTO_MAX_ALG_NAME];
 	char drv_name[CRYPTO_MAX_ALG_NAME];
 	unsigned int bsize;
@@ -2483,7 +2391,7 @@ static int hifn_alg_alloc(struct hifn_device *dev, struct hifn_alg_template *t)
 	struct hifn_crypto_alg *alg;
 	int err;
 
-	alg = kzalloc(sizeof(struct hifn_crypto_alg), GFP_KERNEL);
+	alg = kzalloc(sizeof(*alg), GFP_KERNEL);
 	if (!alg)
 		return -ENOMEM;
 
@@ -2530,7 +2438,7 @@ static int hifn_register_alg(struct hifn_device *dev)
 {
 	int i, err;
 
-	for (i=0; i<ARRAY_SIZE(hifn_alg_templates); ++i) {
+	for (i = 0; i < ARRAY_SIZE(hifn_alg_templates); ++i) {
 		err = hifn_alg_alloc(dev, &hifn_alg_templates[i]);
 		if (err)
 			goto err_out_exit;
@@ -2575,7 +2483,7 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto err_out_disable_pci_device;
 
 	snprintf(name, sizeof(name), "hifn%d",
-			atomic_inc_return(&hifn_dev_number)-1);
+			atomic_inc_return(&hifn_dev_number) - 1);
 
 	err = pci_request_regions(pdev, name);
 	if (err)
@@ -2584,8 +2492,7 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (pci_resource_len(pdev, 0) < HIFN_BAR0_SIZE ||
 	    pci_resource_len(pdev, 1) < HIFN_BAR1_SIZE ||
 	    pci_resource_len(pdev, 2) < HIFN_BAR2_SIZE) {
-		dprintk("%s: Broken hardware - I/O regions are too small.\n",
-				pci_name(pdev));
+		dev_err(&pdev->dev, "Broken hardware - I/O regions are too small.\n");
 		err = -ENODEV;
 		goto err_out_free_regions;
 	}
@@ -2602,7 +2509,7 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	snprintf(dev->name, sizeof(dev->name), "%s", name);
 	spin_lock_init(&dev->lock);
 
-	for (i=0; i<3; ++i) {
+	for (i = 0; i < 3; ++i) {
 		unsigned long addr, size;
 
 		addr = pci_resource_start(pdev, i);
@@ -2618,7 +2525,7 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	dev->desc_virt = pci_zalloc_consistent(pdev, sizeof(struct hifn_dma),
 					       &dev->desc_dma);
 	if (!dev->desc_virt) {
-		dprintk("Failed to allocate descriptor rings.\n");
+		dev_err(&pdev->dev, "Failed to allocate descriptor rings.\n");
 		err = -ENOMEM;
 		goto err_out_unmap_bars;
 	}
@@ -2626,7 +2533,7 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	dev->pdev = pdev;
 	dev->irq = pdev->irq;
 
-	for (i=0; i<HIFN_D_RES_RSIZE; ++i)
+	for (i = 0; i < HIFN_D_RES_RSIZE; ++i)
 		dev->sa[i] = NULL;
 
 	pci_set_drvdata(pdev, dev);
@@ -2637,7 +2544,8 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	err = request_irq(dev->irq, hifn_interrupt, IRQF_SHARED, dev->name, dev);
 	if (err) {
-		dprintk("Failed to request IRQ%d: err: %d.\n", dev->irq, err);
+		dev_err(&pdev->dev, "Failed to request IRQ%d: err: %d.\n",
+			dev->irq, err);
 		dev->irq = 0;
 		goto err_out_free_desc;
 	}
@@ -2646,10 +2554,6 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (err)
 		goto err_out_free_irq;
 
-	err = hifn_test(dev, 1, 0);
-	if (err)
-		goto err_out_stop_device;
-
 	err = hifn_register_rng(dev);
 	if (err)
 		goto err_out_stop_device;
@@ -2661,9 +2565,9 @@ static int hifn_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	INIT_DELAYED_WORK(&dev->work, hifn_work);
 	schedule_delayed_work(&dev->work, HZ);
 
-	dprintk("HIFN crypto accelerator card at %s has been "
-			"successfully registered as %s.\n",
-			pci_name(pdev), dev->name);
+	dev_dbg(&pdev->dev, "HIFN crypto accelerator card at %s has been "
+		"successfully registered as %s.\n",
+		pci_name(pdev), dev->name);
 
 	return 0;
 
@@ -2680,7 +2584,7 @@ err_out_free_desc:
 			dev->desc_virt, dev->desc_dma);
 
 err_out_unmap_bars:
-	for (i=0; i<3; ++i)
+	for (i = 0; i < 3; ++i)
 		if (dev->bar[i])
 			iounmap(dev->bar[i]);
 
@@ -2715,7 +2619,7 @@ static void hifn_remove(struct pci_dev *pdev)
 
 		pci_free_consistent(pdev, sizeof(struct hifn_dma),
 				dev->desc_virt, dev->desc_dma);
-		for (i=0; i<3; ++i)
+		for (i = 0; i < 3; ++i)
 			if (dev->bar[i])
 				iounmap(dev->bar[i]);
 
@@ -2750,8 +2654,7 @@ static int __init hifn_init(void)
 
 	if (strncmp(hifn_pll_ref, "ext", 3) &&
 	    strncmp(hifn_pll_ref, "pci", 3)) {
-		printk(KERN_ERR "hifn795x: invalid hifn_pll_ref clock, "
-				"must be pci or ext");
+		pr_err("hifn795x: invalid hifn_pll_ref clock, must be pci or ext");
 		return -EINVAL;
 	}
 
@@ -2763,22 +2666,21 @@ static int __init hifn_init(void)
 	if (hifn_pll_ref[3] != '\0') {
 		freq = simple_strtoul(hifn_pll_ref + 3, NULL, 10);
 		if (freq < 20 || freq > 100) {
-			printk(KERN_ERR "hifn795x: invalid hifn_pll_ref "
-					"frequency, must be in the range "
-					"of 20-100");
+			pr_err("hifn795x: invalid hifn_pll_ref frequency, must"
+			       "be in the range of 20-100");
 			return -EINVAL;
 		}
 	}
 
 	err = pci_register_driver(&hifn_pci_driver);
 	if (err < 0) {
-		dprintk("Failed to register PCI driver for %s device.\n",
-				hifn_pci_driver.name);
+		pr_err("Failed to register PCI driver for %s device.\n",
+		       hifn_pci_driver.name);
 		return -ENODEV;
 	}
 
-	printk(KERN_INFO "Driver for HIFN 795x crypto accelerator chip "
-			"has been successfully registered.\n");
+	pr_info("Driver for HIFN 795x crypto accelerator chip "
+		"has been successfully registered.\n");
 
 	return 0;
 }
@@ -2787,8 +2689,8 @@ static void __exit hifn_fini(void)
 {
 	pci_unregister_driver(&hifn_pci_driver);
 
-	printk(KERN_INFO "Driver for HIFN 795x crypto accelerator chip "
-			"has been successfully unregistered.\n");
+	pr_info("Driver for HIFN 795x crypto accelerator chip "
+		"has been successfully unregistered.\n");
 }
 
 module_init(hifn_init);
diff --git a/drivers/crypto/ixp4xx_crypto.c b/drivers/crypto/ixp4xx_crypto.c
index 8f2790353281..e52496a172d0 100644
--- a/drivers/crypto/ixp4xx_crypto.c
+++ b/drivers/crypto/ixp4xx_crypto.c
@@ -510,10 +510,8 @@ npe_error:
 	printk(KERN_ERR "%s not responding\n", npe_name(npe_c));
 	ret = -EIO;
 err:
-	if (ctx_pool)
-		dma_pool_destroy(ctx_pool);
-	if (buffer_pool)
-		dma_pool_destroy(buffer_pool);
+	dma_pool_destroy(ctx_pool);
+	dma_pool_destroy(buffer_pool);
 	npe_release(npe_c);
 	return ret;
 }
diff --git a/drivers/crypto/marvell/cesa.c b/drivers/crypto/marvell/cesa.c
index 0643e3366e33..c0656e7f37b5 100644
--- a/drivers/crypto/marvell/cesa.c
+++ b/drivers/crypto/marvell/cesa.c
@@ -306,7 +306,7 @@ static int mv_cesa_dev_dma_init(struct mv_cesa_dev *cesa)
 		return -ENOMEM;
 
 	dma->padding_pool = dmam_pool_create("cesa_padding", dev, 72, 1, 0);
-	if (!dma->cache_pool)
+	if (!dma->padding_pool)
 		return -ENOMEM;
 
 	cesa->dma = dma;
diff --git a/drivers/crypto/marvell/cipher.c b/drivers/crypto/marvell/cipher.c
index 6edae64bb387..dcf1fceb9336 100644
--- a/drivers/crypto/marvell/cipher.c
+++ b/drivers/crypto/marvell/cipher.c
@@ -401,7 +401,15 @@ static int mv_cesa_ablkcipher_req_init(struct ablkcipher_request *req,
 		return -EINVAL;
 
 	creq->src_nents = sg_nents_for_len(req->src, req->nbytes);
+	if (creq->src_nents < 0) {
+		dev_err(cesa_dev->dev, "Invalid number of src SG");
+		return creq->src_nents;
+	}
 	creq->dst_nents = sg_nents_for_len(req->dst, req->nbytes);
+	if (creq->dst_nents < 0) {
+		dev_err(cesa_dev->dev, "Invalid number of dst SG");
+		return creq->dst_nents;
+	}
 
 	mv_cesa_update_op_cfg(tmpl, CESA_SA_DESC_CFG_OP_CRYPT_ONLY,
 			      CESA_SA_DESC_CFG_OP_MSK);
diff --git a/drivers/crypto/marvell/hash.c b/drivers/crypto/marvell/hash.c
index 6ec55b4a087b..683cca9ac3c4 100644
--- a/drivers/crypto/marvell/hash.c
+++ b/drivers/crypto/marvell/hash.c
@@ -712,6 +712,10 @@ static int mv_cesa_ahash_req_init(struct ahash_request *req, bool *cached)
 		creq->req.base.type = CESA_STD_REQ;
 
 	creq->src_nents = sg_nents_for_len(req->src, req->nbytes);
+	if (creq->src_nents < 0) {
+		dev_err(cesa_dev->dev, "Invalid number of src SG");
+		return creq->src_nents;
+	}
 
 	ret = mv_cesa_ahash_cache_req(req, cached);
 	if (ret)
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 5450880abb7b..b85a7a7dbf63 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -241,7 +241,7 @@ static inline bool n2_should_run_async(struct spu_queue *qp, int this_len)
 
 struct n2_ahash_alg {
 	struct list_head	entry;
-	const char		*hash_zero;
+	const u8		*hash_zero;
 	const u32		*hash_init;
 	u8			hw_op_hashsz;
 	u8			digest_size;
@@ -1267,7 +1267,7 @@ static LIST_HEAD(cipher_algs);
 
 struct n2_hash_tmpl {
 	const char	*name;
-	const char	*hash_zero;
+	const u8	*hash_zero;
 	const u32	*hash_init;
 	u8		hw_op_hashsz;
 	u8		digest_size;
@@ -1276,40 +1276,19 @@ struct n2_hash_tmpl {
 	u8		hmac_type;
 };
 
-static const char md5_zero[MD5_DIGEST_SIZE] = {
-	0xd4, 0x1d, 0x8c, 0xd9, 0x8f, 0x00, 0xb2, 0x04,
-	0xe9, 0x80, 0x09, 0x98, 0xec, 0xf8, 0x42, 0x7e,
-};
 static const u32 md5_init[MD5_HASH_WORDS] = {
 	cpu_to_le32(MD5_H0),
 	cpu_to_le32(MD5_H1),
 	cpu_to_le32(MD5_H2),
 	cpu_to_le32(MD5_H3),
 };
-static const char sha1_zero[SHA1_DIGEST_SIZE] = {
-	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d, 0x32,
-	0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90, 0xaf, 0xd8,
-	0x07, 0x09
-};
 static const u32 sha1_init[SHA1_DIGEST_SIZE / 4] = {
 	SHA1_H0, SHA1_H1, SHA1_H2, SHA1_H3, SHA1_H4,
 };
-static const char sha256_zero[SHA256_DIGEST_SIZE] = {
-	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14, 0x9a,
-	0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24, 0x27, 0xae,
-	0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c, 0xa4, 0x95, 0x99,
-	0x1b, 0x78, 0x52, 0xb8, 0x55
-};
 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 char sha224_zero[SHA224_DIGEST_SIZE] = {
-	0xd1, 0x4a, 0x02, 0x8c, 0x2a, 0x3a, 0x2b, 0xc9, 0x47,
-	0x61, 0x02, 0xbb, 0x28, 0x82, 0x34, 0xc4, 0x15, 0xa2,
-	0xb0, 0x1f, 0x82, 0x8e, 0xa6, 0x2a, 0xc5, 0xb3, 0xe4,
-	0x2f
-};
 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,
@@ -1317,7 +1296,7 @@ static const u32 sha224_init[SHA256_DIGEST_SIZE / 4] = {
 
 static const struct n2_hash_tmpl hash_tmpls[] = {
 	{ .name		= "md5",
-	  .hash_zero	= md5_zero,
+	  .hash_zero	= md5_zero_message_hash,
 	  .hash_init	= md5_init,
 	  .auth_type	= AUTH_TYPE_MD5,
 	  .hmac_type	= AUTH_TYPE_HMAC_MD5,
@@ -1325,7 +1304,7 @@ static const struct n2_hash_tmpl hash_tmpls[] = {
 	  .digest_size	= MD5_DIGEST_SIZE,
 	  .block_size	= MD5_HMAC_BLOCK_SIZE },
 	{ .name		= "sha1",
-	  .hash_zero	= sha1_zero,
+	  .hash_zero	= sha1_zero_message_hash,
 	  .hash_init	= sha1_init,
 	  .auth_type	= AUTH_TYPE_SHA1,
 	  .hmac_type	= AUTH_TYPE_HMAC_SHA1,
@@ -1333,7 +1312,7 @@ static const struct n2_hash_tmpl hash_tmpls[] = {
 	  .digest_size	= SHA1_DIGEST_SIZE,
 	  .block_size	= SHA1_BLOCK_SIZE },
 	{ .name		= "sha256",
-	  .hash_zero	= sha256_zero,
+	  .hash_zero	= sha256_zero_message_hash,
 	  .hash_init	= sha256_init,
 	  .auth_type	= AUTH_TYPE_SHA256,
 	  .hmac_type	= AUTH_TYPE_HMAC_SHA256,
@@ -1341,7 +1320,7 @@ static const struct n2_hash_tmpl hash_tmpls[] = {
 	  .digest_size	= SHA256_DIGEST_SIZE,
 	  .block_size	= SHA256_BLOCK_SIZE },
 	{ .name		= "sha224",
-	  .hash_zero	= sha224_zero,
+	  .hash_zero	= sha224_zero_message_hash,
 	  .hash_init	= sha224_init,
 	  .auth_type	= AUTH_TYPE_SHA256,
 	  .hmac_type	= AUTH_TYPE_RESERVED,
@@ -2243,22 +2222,19 @@ static struct platform_driver n2_mau_driver = {
 	.remove		=	n2_mau_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&n2_crypto_driver,
+	&n2_mau_driver,
+};
+
 static int __init n2_init(void)
 {
-	int err = platform_driver_register(&n2_crypto_driver);
-
-	if (!err) {
-		err = platform_driver_register(&n2_mau_driver);
-		if (err)
-			platform_driver_unregister(&n2_crypto_driver);
-	}
-	return err;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit n2_exit(void)
 {
-	platform_driver_unregister(&n2_mau_driver);
-	platform_driver_unregister(&n2_crypto_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_init(n2_init);
diff --git a/drivers/crypto/nx/nx-842-powernv.c b/drivers/crypto/nx/nx-842-powernv.c
index 9ef51fafdbff..1710f80a09ec 100644
--- a/drivers/crypto/nx/nx-842-powernv.c
+++ b/drivers/crypto/nx/nx-842-powernv.c
@@ -442,6 +442,14 @@ static int nx842_powernv_function(const unsigned char *in, unsigned int inlen,
 			     (unsigned int)ccw,
 			     (unsigned int)be32_to_cpu(crb->ccw));
 
+	/*
+	 * NX842 coprocessor sets 3rd bit in CR register with XER[S0].
+	 * XER[S0] is the integer summary overflow bit which is nothing
+	 * to do NX. Since this bit can be set with other return values,
+	 * mask this bit.
+	 */
+	ret &= ~ICSWX_XERS0;
+
 	switch (ret) {
 	case ICSWX_INITIATED:
 		ret = wait_for_csb(wmem, csb);
@@ -454,10 +462,6 @@ static int nx842_powernv_function(const unsigned char *in, unsigned int inlen,
 		pr_err_ratelimited("ICSWX rejected\n");
 		ret = -EPROTO;
 		break;
-	default:
-		pr_err_ratelimited("Invalid ICSWX return code %x\n", ret);
-		ret = -EPROTO;
-		break;
 	}
 
 	if (!ret)
@@ -525,7 +529,6 @@ static int nx842_powernv_decompress(const unsigned char *in, unsigned int inlen,
 static int __init nx842_powernv_probe(struct device_node *dn)
 {
 	struct nx842_coproc *coproc;
-	struct property *ct_prop, *ci_prop;
 	unsigned int ct, ci;
 	int chip_id;
 
@@ -534,18 +537,16 @@ static int __init nx842_powernv_probe(struct device_node *dn)
 		pr_err("ibm,chip-id missing\n");
 		return -EINVAL;
 	}
-	ct_prop = of_find_property(dn, "ibm,842-coprocessor-type", NULL);
-	if (!ct_prop) {
+
+	if (of_property_read_u32(dn, "ibm,842-coprocessor-type", &ct)) {
 		pr_err("ibm,842-coprocessor-type missing\n");
 		return -EINVAL;
 	}
-	ct = be32_to_cpu(*(unsigned int *)ct_prop->value);
-	ci_prop = of_find_property(dn, "ibm,842-coprocessor-instance", NULL);
-	if (!ci_prop) {
+
+	if (of_property_read_u32(dn, "ibm,842-coprocessor-instance", &ci)) {
 		pr_err("ibm,842-coprocessor-instance missing\n");
 		return -EINVAL;
 	}
-	ci = be32_to_cpu(*(unsigned int *)ci_prop->value);
 
 	coproc = kmalloc(sizeof(*coproc), GFP_KERNEL);
 	if (!coproc)
diff --git a/drivers/crypto/omap-aes.c b/drivers/crypto/omap-aes.c
index eba23147c0ee..dd355bd19474 100644
--- a/drivers/crypto/omap-aes.c
+++ b/drivers/crypto/omap-aes.c
@@ -539,8 +539,6 @@ static void omap_aes_finish_req(struct omap_aes_dev *dd, int err)
 
 static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
 {
-	int err = 0;
-
 	pr_debug("total: %d\n", dd->total);
 
 	omap_aes_dma_stop(dd);
@@ -548,7 +546,7 @@ static int omap_aes_crypt_dma_stop(struct omap_aes_dev *dd)
 	dmaengine_terminate_all(dd->dma_lch_in);
 	dmaengine_terminate_all(dd->dma_lch_out);
 
-	return err;
+	return 0;
 }
 
 static int omap_aes_check_aligned(struct scatterlist *sg, int total)
diff --git a/drivers/crypto/omap-des.c b/drivers/crypto/omap-des.c
index 0a70e46d5416..dd7b93f2f94c 100644
--- a/drivers/crypto/omap-des.c
+++ b/drivers/crypto/omap-des.c
@@ -527,8 +527,6 @@ static void omap_des_finish_req(struct omap_des_dev *dd, int err)
 
 static int omap_des_crypt_dma_stop(struct omap_des_dev *dd)
 {
-	int err = 0;
-
 	pr_debug("total: %d\n", dd->total);
 
 	omap_des_dma_stop(dd);
@@ -536,7 +534,7 @@ static int omap_des_crypt_dma_stop(struct omap_des_dev *dd)
 	dmaengine_terminate_all(dd->dma_lch_in);
 	dmaengine_terminate_all(dd->dma_lch_out);
 
-	return err;
+	return 0;
 }
 
 static int omap_des_copy_needed(struct scatterlist *sg)
@@ -1086,6 +1084,7 @@ static int omap_des_probe(struct platform_device *pdev)
 	dd->phys_base = res->start;
 
 	pm_runtime_enable(dev);
+	pm_runtime_irq_safe(dev);
 	err = pm_runtime_get_sync(dev);
 	if (err < 0) {
 		pm_runtime_put_noidle(dev);
diff --git a/drivers/crypto/padlock-aes.c b/drivers/crypto/padlock-aes.c
index da2d6777bd09..441e86b23571 100644
--- a/drivers/crypto/padlock-aes.c
+++ b/drivers/crypto/padlock-aes.c
@@ -238,7 +238,7 @@ static inline void ecb_crypt(const u8 *in, u8 *out, u32 *key,
 	/* Padlock in ECB mode fetches at least ecb_fetch_bytes of data.
 	 * We could avoid some copying here but it's probably not worth it.
 	 */
-	if (unlikely(((unsigned long)in & ~PAGE_MASK) + ecb_fetch_bytes > PAGE_SIZE)) {
+	if (unlikely(offset_in_page(in) + ecb_fetch_bytes > PAGE_SIZE)) {
 		ecb_crypt_copy(in, out, key, cword, count);
 		return;
 	}
@@ -250,7 +250,7 @@ static inline u8 *cbc_crypt(const u8 *in, u8 *out, u32 *key,
 			    u8 *iv, struct cword *cword, int count)
 {
 	/* Padlock in CBC mode fetches at least cbc_fetch_bytes of data. */
-	if (unlikely(((unsigned long)in & ~PAGE_MASK) + cbc_fetch_bytes > PAGE_SIZE))
+	if (unlikely(offset_in_page(in) + cbc_fetch_bytes > PAGE_SIZE))
 		return cbc_crypt_copy(in, out, key, iv, cword, count);
 
 	return rep_xcrypt_cbc(in, out, key, iv, cword, count);
@@ -515,7 +515,7 @@ static int __init padlock_init(void)
 	if (!x86_match_cpu(padlock_cpu_id))
 		return -ENODEV;
 
-	if (!cpu_has_xcrypt_enabled) {
+	if (!boot_cpu_has(X86_FEATURE_XCRYPT_EN)) {
 		printk(KERN_NOTICE PFX "VIA PadLock detected, but not enabled. Hmm, strange...\n");
 		return -ENODEV;
 	}
diff --git a/drivers/crypto/padlock-sha.c b/drivers/crypto/padlock-sha.c
index 4e154c9b9206..8c5f90647b7a 100644
--- a/drivers/crypto/padlock-sha.c
+++ b/drivers/crypto/padlock-sha.c
@@ -540,7 +540,7 @@ static int __init padlock_init(void)
 	struct shash_alg *sha1;
 	struct shash_alg *sha256;
 
-	if (!x86_match_cpu(padlock_sha_ids) || !cpu_has_phe_enabled)
+	if (!x86_match_cpu(padlock_sha_ids) || !boot_cpu_has(X86_FEATURE_PHE_EN))
 		return -ENODEV;
 
 	/* Register the newly added algorithm module if on *
diff --git a/drivers/crypto/picoxcell_crypto.c b/drivers/crypto/picoxcell_crypto.c
index 615da961c4d8..3b1c7ecf078f 100644
--- a/drivers/crypto/picoxcell_crypto.c
+++ b/drivers/crypto/picoxcell_crypto.c
@@ -272,12 +272,6 @@ static unsigned spacc_load_ctx(struct spacc_generic_ctx *ctx,
 	return indx;
 }
 
-/* Count the number of scatterlist entries in a scatterlist. */
-static inline int sg_count(struct scatterlist *sg_list, int nbytes)
-{
-	return sg_nents_for_len(sg_list, nbytes);
-}
-
 static inline void ddt_set(struct spacc_ddt *ddt, dma_addr_t phys, size_t len)
 {
 	ddt->p = phys;
@@ -295,12 +289,17 @@ static struct spacc_ddt *spacc_sg_to_ddt(struct spacc_engine *engine,
 					 enum dma_data_direction dir,
 					 dma_addr_t *ddt_phys)
 {
-	unsigned nents, mapped_ents;
+	unsigned mapped_ents;
 	struct scatterlist *cur;
 	struct spacc_ddt *ddt;
 	int i;
+	int nents;
 
-	nents = sg_count(payload, nbytes);
+	nents = sg_nents_for_len(payload, nbytes);
+	if (nents < 0) {
+		dev_err(engine->dev, "Invalid numbers of SG.\n");
+		return NULL;
+	}
 	mapped_ents = dma_map_sg(engine->dev, payload, nents, dir);
 
 	if (mapped_ents + 1 > MAX_DDT_LEN)
@@ -328,7 +327,7 @@ static int spacc_aead_make_ddts(struct aead_request *areq)
 	struct spacc_engine *engine = req->engine;
 	struct spacc_ddt *src_ddt, *dst_ddt;
 	unsigned total;
-	unsigned int src_nents, dst_nents;
+	int src_nents, dst_nents;
 	struct scatterlist *cur;
 	int i, dst_ents, src_ents;
 
@@ -336,13 +335,21 @@ static int spacc_aead_make_ddts(struct aead_request *areq)
 	if (req->is_encrypt)
 		total += crypto_aead_authsize(aead);
 
-	src_nents = sg_count(areq->src, total);
+	src_nents = sg_nents_for_len(areq->src, total);
+	if (src_nents < 0) {
+		dev_err(engine->dev, "Invalid numbers of src SG.\n");
+		return src_nents;
+	}
 	if (src_nents + 1 > MAX_DDT_LEN)
 		return -E2BIG;
 
 	dst_nents = 0;
 	if (areq->src != areq->dst) {
-		dst_nents = sg_count(areq->dst, total);
+		dst_nents = sg_nents_for_len(areq->dst, total);
+		if (dst_nents < 0) {
+			dev_err(engine->dev, "Invalid numbers of dst SG.\n");
+			return dst_nents;
+		}
 		if (src_nents + 1 > MAX_DDT_LEN)
 			return -E2BIG;
 	}
@@ -422,13 +429,22 @@ static void spacc_aead_free_ddts(struct spacc_req *req)
 			 (req->is_encrypt ? crypto_aead_authsize(aead) : 0);
 	struct spacc_aead_ctx *aead_ctx = crypto_aead_ctx(aead);
 	struct spacc_engine *engine = aead_ctx->generic.engine;
-	unsigned nents = sg_count(areq->src, total);
+	int nents = sg_nents_for_len(areq->src, total);
+
+	/* sg_nents_for_len should not fail since it works when mapping sg */
+	if (unlikely(nents < 0)) {
+		dev_err(engine->dev, "Invalid numbers of src SG.\n");
+		return;
+	}
 
 	if (areq->src != areq->dst) {
 		dma_unmap_sg(engine->dev, areq->src, nents, DMA_TO_DEVICE);
-		dma_unmap_sg(engine->dev, areq->dst,
-			     sg_count(areq->dst, total),
-			     DMA_FROM_DEVICE);
+		nents = sg_nents_for_len(areq->dst, total);
+		if (unlikely(nents < 0)) {
+			dev_err(engine->dev, "Invalid numbers of dst SG.\n");
+			return;
+		}
+		dma_unmap_sg(engine->dev, areq->dst, nents, DMA_FROM_DEVICE);
 	} else
 		dma_unmap_sg(engine->dev, areq->src, nents, DMA_BIDIRECTIONAL);
 
@@ -440,7 +456,12 @@ static void spacc_free_ddt(struct spacc_req *req, struct spacc_ddt *ddt,
 			   dma_addr_t ddt_addr, struct scatterlist *payload,
 			   unsigned nbytes, enum dma_data_direction dir)
 {
-	unsigned nents = sg_count(payload, nbytes);
+	int nents = sg_nents_for_len(payload, nbytes);
+
+	if (nents < 0) {
+		dev_err(req->engine->dev, "Invalid numbers of SG.\n");
+		return;
+	}
 
 	dma_unmap_sg(req->engine->dev, payload, nents, dir);
 	dma_pool_free(req->engine->req_pool, ddt, ddt_addr);
@@ -835,8 +856,7 @@ static int spacc_ablk_need_fallback(struct spacc_req *req)
 
 static void spacc_ablk_complete(struct spacc_req *req)
 {
-	struct ablkcipher_request *ablk_req =
-		container_of(req->req, struct ablkcipher_request, base);
+	struct ablkcipher_request *ablk_req = ablkcipher_request_cast(req->req);
 
 	if (ablk_req->src != ablk_req->dst) {
 		spacc_free_ddt(req, req->src_ddt, req->src_addr, ablk_req->src,
diff --git a/drivers/crypto/qat/Kconfig b/drivers/crypto/qat/Kconfig
index eefccf7b8be7..85b44e577684 100644
--- a/drivers/crypto/qat/Kconfig
+++ b/drivers/crypto/qat/Kconfig
@@ -22,6 +22,28 @@ config CRYPTO_DEV_QAT_DH895xCC
 	  To compile this as a module, choose M here: the module
 	  will be called qat_dh895xcc.
 
+config CRYPTO_DEV_QAT_C3XXX
+	tristate "Support for Intel(R) C3XXX"
+	depends on X86 && PCI
+	select CRYPTO_DEV_QAT
+	help
+	  Support for Intel(R) C3xxx with Intel(R) QuickAssist Technology
+	  for accelerating crypto and compression workloads.
+
+	  To compile this as a module, choose M here: the module
+	  will be called qat_c3xxx.
+
+config CRYPTO_DEV_QAT_C62X
+	tristate "Support for Intel(R) C62X"
+	depends on X86 && PCI
+	select CRYPTO_DEV_QAT
+	help
+	  Support for Intel(R) C62x with Intel(R) QuickAssist Technology
+	  for accelerating crypto and compression workloads.
+
+	  To compile this as a module, choose M here: the module
+	  will be called qat_c62x.
+
 config CRYPTO_DEV_QAT_DH895xCCVF
 	tristate "Support for Intel(R) DH895xCC Virtual Function"
 	depends on X86 && PCI
@@ -34,3 +56,27 @@ config CRYPTO_DEV_QAT_DH895xCCVF
 
 	  To compile this as a module, choose M here: the module
 	  will be called qat_dh895xccvf.
+
+config CRYPTO_DEV_QAT_C3XXXVF
+	tristate "Support for Intel(R) C3XXX Virtual Function"
+	depends on X86 && PCI
+	select PCI_IOV
+	select CRYPTO_DEV_QAT
+	help
+	  Support for Intel(R) C3xxx with Intel(R) QuickAssist Technology
+	  Virtual Function for accelerating crypto and compression workloads.
+
+	  To compile this as a module, choose M here: the module
+	  will be called qat_c3xxxvf.
+
+config CRYPTO_DEV_QAT_C62XVF
+	tristate "Support for Intel(R) C62X Virtual Function"
+	depends on X86 && PCI
+	select PCI_IOV
+	select CRYPTO_DEV_QAT
+	help
+	  Support for Intel(R) C62x with Intel(R) QuickAssist Technology
+	  Virtual Function for accelerating crypto and compression workloads.
+
+	  To compile this as a module, choose M here: the module
+	  will be called qat_c62xvf.
diff --git a/drivers/crypto/qat/Makefile b/drivers/crypto/qat/Makefile
index a3ce0b70e32f..8265106f1c8e 100644
--- a/drivers/crypto/qat/Makefile
+++ b/drivers/crypto/qat/Makefile
@@ -1,3 +1,7 @@
 obj-$(CONFIG_CRYPTO_DEV_QAT) += qat_common/
 obj-$(CONFIG_CRYPTO_DEV_QAT_DH895xCC) += qat_dh895xcc/
+obj-$(CONFIG_CRYPTO_DEV_QAT_C3XXX) += qat_c3xxx/
+obj-$(CONFIG_CRYPTO_DEV_QAT_C62X) += qat_c62x/
 obj-$(CONFIG_CRYPTO_DEV_QAT_DH895xCCVF) += qat_dh895xccvf/
+obj-$(CONFIG_CRYPTO_DEV_QAT_C3XXXVF) += qat_c3xxxvf/
+obj-$(CONFIG_CRYPTO_DEV_QAT_C62XVF) += qat_c62xvf/
diff --git a/drivers/crypto/qat/qat_c3xxx/Makefile b/drivers/crypto/qat/qat_c3xxx/Makefile
new file mode 100644
index 000000000000..8f5fd4838a96
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxx/Makefile
@@ -0,0 +1,3 @@
+ccflags-y := -I$(src)/../qat_common
+obj-$(CONFIG_CRYPTO_DEV_QAT_C3XXX) += qat_c3xxx.o
+qat_c3xxx-objs := adf_drv.o adf_c3xxx_hw_data.o
diff --git a/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.c b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.c
new file mode 100644
index 000000000000..c5bd5a9abc4d
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.c
@@ -0,0 +1,238 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in
+	  the documentation and/or other materials provided with the
+	  distribution.
+	* Neither the name of Intel Corporation nor the names of its
+	  contributors may be used to endorse or promote products derived
+	  from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_pf2vf_msg.h>
+#include "adf_c3xxx_hw_data.h"
+
+/* Worker thread to service arbiter mappings based on dev SKUs */
+static const u32 thrd_to_arb_map_6_me_sku[] = {
+	0x12222AAA, 0x11222AAA, 0x12222AAA,
+	0x11222AAA, 0x12222AAA, 0x11222AAA
+};
+
+static struct adf_hw_device_class c3xxx_class = {
+	.name = ADF_C3XXX_DEVICE_NAME,
+	.type = DEV_C3XXX,
+	.instances = 0
+};
+
+static u32 get_accel_mask(u32 fuse)
+{
+	return (~fuse) >> ADF_C3XXX_ACCELERATORS_REG_OFFSET &
+		ADF_C3XXX_ACCELERATORS_MASK;
+}
+
+static u32 get_ae_mask(u32 fuse)
+{
+	return (~fuse) & ADF_C3XXX_ACCELENGINES_MASK;
+}
+
+static u32 get_num_accels(struct adf_hw_device_data *self)
+{
+	u32 i, ctr = 0;
+
+	if (!self || !self->accel_mask)
+		return 0;
+
+	for (i = 0; i < ADF_C3XXX_MAX_ACCELERATORS; i++) {
+		if (self->accel_mask & (1 << i))
+			ctr++;
+	}
+	return ctr;
+}
+
+static u32 get_num_aes(struct adf_hw_device_data *self)
+{
+	u32 i, ctr = 0;
+
+	if (!self || !self->ae_mask)
+		return 0;
+
+	for (i = 0; i < ADF_C3XXX_MAX_ACCELENGINES; i++) {
+		if (self->ae_mask & (1 << i))
+			ctr++;
+	}
+	return ctr;
+}
+
+static u32 get_misc_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXX_PMISC_BAR;
+}
+
+static u32 get_etr_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXX_ETR_BAR;
+}
+
+static u32 get_sram_bar_id(struct adf_hw_device_data *self)
+{
+	return 0;
+}
+
+static enum dev_sku_info get_sku(struct adf_hw_device_data *self)
+{
+	int aes = get_num_aes(self);
+
+	if (aes == 6)
+		return DEV_SKU_4;
+
+	return DEV_SKU_UNKNOWN;
+}
+
+static void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
+				    u32 const **arb_map_config)
+{
+	switch (accel_dev->accel_pci_dev.sku) {
+	case DEV_SKU_4:
+		*arb_map_config = thrd_to_arb_map_6_me_sku;
+		break;
+	default:
+		dev_err(&GET_DEV(accel_dev),
+			"The configuration doesn't match any SKU");
+		*arb_map_config = NULL;
+	}
+}
+
+static u32 get_pf2vf_offset(u32 i)
+{
+	return ADF_C3XXX_PF2VF_OFFSET(i);
+}
+
+static u32 get_vintmsk_offset(u32 i)
+{
+	return ADF_C3XXX_VINTMSK_OFFSET(i);
+}
+
+static void adf_enable_error_correction(struct adf_accel_dev *accel_dev)
+{
+	struct adf_hw_device_data *hw_device = accel_dev->hw_device;
+	struct adf_bar *misc_bar = &GET_BARS(accel_dev)[ADF_C3XXX_PMISC_BAR];
+	void __iomem *csr = misc_bar->virt_addr;
+	unsigned int val, i;
+
+	/* Enable Accel Engine error detection & correction */
+	for (i = 0; i < hw_device->get_num_aes(hw_device); i++) {
+		val = ADF_CSR_RD(csr, ADF_C3XXX_AE_CTX_ENABLES(i));
+		val |= ADF_C3XXX_ENABLE_AE_ECC_ERR;
+		ADF_CSR_WR(csr, ADF_C3XXX_AE_CTX_ENABLES(i), val);
+		val = ADF_CSR_RD(csr, ADF_C3XXX_AE_MISC_CONTROL(i));
+		val |= ADF_C3XXX_ENABLE_AE_ECC_PARITY_CORR;
+		ADF_CSR_WR(csr, ADF_C3XXX_AE_MISC_CONTROL(i), val);
+	}
+
+	/* Enable shared memory error detection & correction */
+	for (i = 0; i < hw_device->get_num_accels(hw_device); i++) {
+		val = ADF_CSR_RD(csr, ADF_C3XXX_UERRSSMSH(i));
+		val |= ADF_C3XXX_ERRSSMSH_EN;
+		ADF_CSR_WR(csr, ADF_C3XXX_UERRSSMSH(i), val);
+		val = ADF_CSR_RD(csr, ADF_C3XXX_CERRSSMSH(i));
+		val |= ADF_C3XXX_ERRSSMSH_EN;
+		ADF_CSR_WR(csr, ADF_C3XXX_CERRSSMSH(i), val);
+	}
+}
+
+static void adf_enable_ints(struct adf_accel_dev *accel_dev)
+{
+	void __iomem *addr;
+
+	addr = (&GET_BARS(accel_dev)[ADF_C3XXX_PMISC_BAR])->virt_addr;
+
+	/* Enable bundle and misc interrupts */
+	ADF_CSR_WR(addr, ADF_C3XXX_SMIAPF0_MASK_OFFSET,
+		   ADF_C3XXX_SMIA0_MASK);
+	ADF_CSR_WR(addr, ADF_C3XXX_SMIAPF1_MASK_OFFSET,
+		   ADF_C3XXX_SMIA1_MASK);
+}
+
+static int adf_pf_enable_vf2pf_comms(struct adf_accel_dev *accel_dev)
+{
+	return 0;
+}
+
+void adf_init_hw_data_c3xxx(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class = &c3xxx_class;
+	hw_data->instance_id = c3xxx_class.instances++;
+	hw_data->num_banks = ADF_C3XXX_ETR_MAX_BANKS;
+	hw_data->num_accel = ADF_C3XXX_MAX_ACCELERATORS;
+	hw_data->num_logical_accel = 1;
+	hw_data->num_engines = ADF_C3XXX_MAX_ACCELENGINES;
+	hw_data->tx_rx_gap = ADF_C3XXX_RX_RINGS_OFFSET;
+	hw_data->tx_rings_mask = ADF_C3XXX_TX_RINGS_MASK;
+	hw_data->alloc_irq = adf_isr_resource_alloc;
+	hw_data->free_irq = adf_isr_resource_free;
+	hw_data->enable_error_correction = adf_enable_error_correction;
+	hw_data->get_accel_mask = get_accel_mask;
+	hw_data->get_ae_mask = get_ae_mask;
+	hw_data->get_num_accels = get_num_accels;
+	hw_data->get_num_aes = get_num_aes;
+	hw_data->get_sram_bar_id = get_sram_bar_id;
+	hw_data->get_etr_bar_id = get_etr_bar_id;
+	hw_data->get_misc_bar_id = get_misc_bar_id;
+	hw_data->get_pf2vf_offset = get_pf2vf_offset;
+	hw_data->get_vintmsk_offset = get_vintmsk_offset;
+	hw_data->get_sku = get_sku;
+	hw_data->fw_name = ADF_C3XXX_FW;
+	hw_data->fw_mmp_name = ADF_C3XXX_MMP;
+	hw_data->init_admin_comms = adf_init_admin_comms;
+	hw_data->exit_admin_comms = adf_exit_admin_comms;
+	hw_data->disable_iov = adf_disable_sriov;
+	hw_data->send_admin_init = adf_send_admin_init;
+	hw_data->init_arb = adf_init_arb;
+	hw_data->exit_arb = adf_exit_arb;
+	hw_data->get_arb_mapping = adf_get_arbiter_mapping;
+	hw_data->enable_ints = adf_enable_ints;
+	hw_data->enable_vf2pf_comms = adf_pf_enable_vf2pf_comms;
+	hw_data->min_iov_compat_ver = ADF_PFVF_COMPATIBILITY_VERSION;
+}
+
+void adf_clean_hw_data_c3xxx(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class->instances--;
+}
diff --git a/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h
new file mode 100644
index 000000000000..2f2681d3458a
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxx/adf_c3xxx_hw_data.h
@@ -0,0 +1,83 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef ADF_C3XXX_HW_DATA_H_
+#define ADF_C3XXX_HW_DATA_H_
+
+/* PCIe configuration space */
+#define ADF_C3XXX_PMISC_BAR 0
+#define ADF_C3XXX_ETR_BAR 1
+#define ADF_C3XXX_RX_RINGS_OFFSET 8
+#define ADF_C3XXX_TX_RINGS_MASK 0xFF
+#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_ACCELENGINES_MASK 0x3F
+#define ADF_C3XXX_ETR_MAX_BANKS 16
+#define ADF_C3XXX_SMIAPF0_MASK_OFFSET (0x3A000 + 0x28)
+#define ADF_C3XXX_SMIAPF1_MASK_OFFSET (0x3A000 + 0x30)
+#define ADF_C3XXX_SMIA0_MASK 0xFFFF
+#define ADF_C3XXX_SMIA1_MASK 0x1
+/* Error detection and correction */
+#define ADF_C3XXX_AE_CTX_ENABLES(i) (i * 0x1000 + 0x20818)
+#define ADF_C3XXX_AE_MISC_CONTROL(i) (i * 0x1000 + 0x20960)
+#define ADF_C3XXX_ENABLE_AE_ECC_ERR BIT(28)
+#define ADF_C3XXX_ENABLE_AE_ECC_PARITY_CORR (BIT(24) | BIT(12))
+#define ADF_C3XXX_UERRSSMSH(i) (i * 0x4000 + 0x18)
+#define ADF_C3XXX_CERRSSMSH(i) (i * 0x4000 + 0x10)
+#define ADF_C3XXX_ERRSSMSH_EN BIT(3)
+
+#define ADF_C3XXX_PF2VF_OFFSET(i)	(0x3A000 + 0x280 + ((i) * 0x04))
+#define ADF_C3XXX_VINTMSK_OFFSET(i)	(0x3A000 + 0x200 + ((i) * 0x04))
+
+/* Firmware Binary */
+#define ADF_C3XXX_FW "qat_c3xxx.bin"
+#define ADF_C3XXX_MMP "qat_c3xxx_mmp.bin"
+
+void adf_init_hw_data_c3xxx(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_c3xxx(struct adf_hw_device_data *hw_data);
+#endif
diff --git a/drivers/crypto/qat/qat_c3xxx/adf_drv.c b/drivers/crypto/qat/qat_c3xxx/adf_drv.c
new file mode 100644
index 000000000000..e13bd08ddd1e
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxx/adf_drv.c
@@ -0,0 +1,335 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_cfg.h>
+#include "adf_c3xxx_hw_data.h"
+
+#define ADF_SYSTEM_DEVICE(device_id) \
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
+
+static const struct pci_device_id adf_pci_tbl[] = {
+	ADF_SYSTEM_DEVICE(ADF_C3XXX_PCI_DEVICE_ID),
+	{0,}
+};
+MODULE_DEVICE_TABLE(pci, adf_pci_tbl);
+
+static int adf_probe(struct pci_dev *dev, const struct pci_device_id *ent);
+static void adf_remove(struct pci_dev *dev);
+
+static struct pci_driver adf_driver = {
+	.id_table = adf_pci_tbl,
+	.name = ADF_C3XXX_DEVICE_NAME,
+	.probe = adf_probe,
+	.remove = adf_remove,
+	.sriov_configure = adf_sriov_configure,
+};
+
+static void adf_cleanup_pci_dev(struct adf_accel_dev *accel_dev)
+{
+	pci_release_regions(accel_dev->accel_pci_dev.pci_dev);
+	pci_disable_device(accel_dev->accel_pci_dev.pci_dev);
+}
+
+static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
+{
+	struct adf_accel_pci *accel_pci_dev = &accel_dev->accel_pci_dev;
+	int i;
+
+	for (i = 0; i < ADF_PCI_MAX_BARS; i++) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i];
+
+		if (bar->virt_addr)
+			pci_iounmap(accel_pci_dev->pci_dev, bar->virt_addr);
+	}
+
+	if (accel_dev->hw_device) {
+		switch (accel_pci_dev->pci_dev->device) {
+		case ADF_C3XXX_PCI_DEVICE_ID:
+			adf_clean_hw_data_c3xxx(accel_dev->hw_device);
+			break;
+		default:
+			break;
+		}
+		kfree(accel_dev->hw_device);
+		accel_dev->hw_device = NULL;
+	}
+	adf_cfg_dev_remove(accel_dev);
+	debugfs_remove(accel_dev->debugfs_dir);
+	adf_devmgr_rm_dev(accel_dev, NULL);
+}
+
+static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct adf_accel_dev *accel_dev;
+	struct adf_accel_pci *accel_pci_dev;
+	struct adf_hw_device_data *hw_data;
+	char name[ADF_DEVICE_NAME_LENGTH];
+	unsigned int i, bar_nr;
+	int ret, bar_mask;
+
+	switch (ent->device) {
+	case ADF_C3XXX_PCI_DEVICE_ID:
+		break;
+	default:
+		dev_err(&pdev->dev, "Invalid device 0x%x.\n", ent->device);
+		return -ENODEV;
+	}
+
+	if (num_possible_nodes() > 1 && dev_to_node(&pdev->dev) < 0) {
+		/* If the accelerator is connected to a node with no memory
+		 * there is no point in using the accelerator since the remote
+		 * memory transaction will be very slow. */
+		dev_err(&pdev->dev, "Invalid NUMA configuration.\n");
+		return -EINVAL;
+	}
+
+	accel_dev = kzalloc_node(sizeof(*accel_dev), GFP_KERNEL,
+				 dev_to_node(&pdev->dev));
+	if (!accel_dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&accel_dev->crypto_list);
+	accel_pci_dev = &accel_dev->accel_pci_dev;
+	accel_pci_dev->pci_dev = pdev;
+
+	/* Add accel device to accel table.
+	 * This should be called before adf_cleanup_accel is called */
+	if (adf_devmgr_add_dev(accel_dev, NULL)) {
+		dev_err(&pdev->dev, "Failed to add new accelerator device.\n");
+		kfree(accel_dev);
+		return -EFAULT;
+	}
+
+	accel_dev->owner = THIS_MODULE;
+	/* Allocate and configure device configuration structure */
+	hw_data = kzalloc_node(sizeof(*hw_data), GFP_KERNEL,
+			       dev_to_node(&pdev->dev));
+	if (!hw_data) {
+		ret = -ENOMEM;
+		goto out_err;
+	}
+
+	accel_dev->hw_device = hw_data;
+	adf_init_hw_data_c3xxx(accel_dev->hw_device);
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &accel_pci_dev->revid);
+	pci_read_config_dword(pdev, ADF_DEVICE_FUSECTL_OFFSET,
+			      &hw_data->fuses);
+
+	/* Get Accelerators and Accelerators Engines masks */
+	hw_data->accel_mask = hw_data->get_accel_mask(hw_data->fuses);
+	hw_data->ae_mask = hw_data->get_ae_mask(hw_data->fuses);
+	accel_pci_dev->sku = hw_data->get_sku(hw_data);
+	/* If the device has no acceleration engines then ignore it. */
+	if (!hw_data->accel_mask || !hw_data->ae_mask ||
+	    ((~hw_data->ae_mask) & 0x01)) {
+		dev_err(&pdev->dev, "No acceleration units found");
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* Create dev top level debugfs entry */
+	snprintf(name, sizeof(name), "%s%s_%02x:%02d.%02d",
+		 ADF_DEVICE_NAME_PREFIX, hw_data->dev_class->name,
+		 pdev->bus->number, PCI_SLOT(pdev->devfn),
+		 PCI_FUNC(pdev->devfn));
+
+	accel_dev->debugfs_dir = debugfs_create_dir(name, NULL);
+	if (!accel_dev->debugfs_dir) {
+		dev_err(&pdev->dev, "Could not create debugfs dir %s\n", name);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	/* Create device configuration table */
+	ret = adf_cfg_dev_add(accel_dev);
+	if (ret)
+		goto out_err;
+
+	/* enable PCI device */
+	if (pci_enable_device(pdev)) {
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* set dma identifier */
+	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if ((pci_set_dma_mask(pdev, DMA_BIT_MASK(32)))) {
+			dev_err(&pdev->dev, "No usable DMA configuration\n");
+			ret = -EFAULT;
+			goto out_err_disable;
+		} else {
+			pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		}
+
+	} else {
+		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	}
+
+	if (pci_request_regions(pdev, ADF_C3XXX_DEVICE_NAME)) {
+		ret = -EFAULT;
+		goto out_err_disable;
+	}
+
+	/* Read accelerator capabilities mask */
+	pci_read_config_dword(pdev, ADF_DEVICE_LEGFUSE_OFFSET,
+			      &hw_data->accel_capabilities_mask);
+
+	/* Find and map all the device's BARS */
+	i = 0;
+	bar_mask = pci_select_bars(pdev, IORESOURCE_MEM);
+	for_each_set_bit(bar_nr, (const unsigned long *)&bar_mask,
+			 ADF_PCI_MAX_BARS * 2) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i++];
+
+		bar->base_addr = pci_resource_start(pdev, bar_nr);
+		if (!bar->base_addr)
+			break;
+		bar->size = pci_resource_len(pdev, bar_nr);
+		bar->virt_addr = pci_iomap(accel_pci_dev->pci_dev, bar_nr, 0);
+		if (!bar->virt_addr) {
+			dev_err(&pdev->dev, "Failed to map BAR %d\n", bar_nr);
+			ret = -EFAULT;
+			goto out_err_free_reg;
+		}
+	}
+	pci_set_master(pdev);
+
+	if (adf_enable_aer(accel_dev, &adf_driver)) {
+		dev_err(&pdev->dev, "Failed to enable aer\n");
+		ret = -EFAULT;
+		goto out_err_free_reg;
+	}
+
+	if (pci_save_state(pdev)) {
+		dev_err(&pdev->dev, "Failed to save pci state\n");
+		ret = -ENOMEM;
+		goto out_err_free_reg;
+	}
+
+	ret = qat_crypto_dev_config(accel_dev);
+	if (ret)
+		goto out_err_free_reg;
+
+	ret = adf_dev_init(accel_dev);
+	if (ret)
+		goto out_err_dev_shutdown;
+
+	ret = adf_dev_start(accel_dev);
+	if (ret)
+		goto out_err_dev_stop;
+
+	return ret;
+
+out_err_dev_stop:
+	adf_dev_stop(accel_dev);
+out_err_dev_shutdown:
+	adf_dev_shutdown(accel_dev);
+out_err_free_reg:
+	pci_release_regions(accel_pci_dev->pci_dev);
+out_err_disable:
+	pci_disable_device(accel_pci_dev->pci_dev);
+out_err:
+	adf_cleanup_accel(accel_dev);
+	kfree(accel_dev);
+	return ret;
+}
+
+static void adf_remove(struct pci_dev *pdev)
+{
+	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
+
+	if (!accel_dev) {
+		pr_err("QAT: Driver removal failed\n");
+		return;
+	}
+	if (adf_dev_stop(accel_dev))
+		dev_err(&GET_DEV(accel_dev), "Failed to stop QAT accel dev\n");
+
+	adf_dev_shutdown(accel_dev);
+	adf_disable_aer(accel_dev);
+	adf_cleanup_accel(accel_dev);
+	adf_cleanup_pci_dev(accel_dev);
+	kfree(accel_dev);
+}
+
+static int __init adfdrv_init(void)
+{
+	request_module("intel_qat");
+
+	if (pci_register_driver(&adf_driver)) {
+		pr_err("QAT: Driver initialization failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void __exit adfdrv_release(void)
+{
+	pci_unregister_driver(&adf_driver);
+}
+
+module_init(adfdrv_init);
+module_exit(adfdrv_release);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel");
+MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/qat/qat_c3xxxvf/Makefile b/drivers/crypto/qat/qat_c3xxxvf/Makefile
new file mode 100644
index 000000000000..16d178e2eaa2
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxxvf/Makefile
@@ -0,0 +1,3 @@
+ccflags-y := -I$(src)/../qat_common
+obj-$(CONFIG_CRYPTO_DEV_QAT_C3XXXVF) += qat_c3xxxvf.o
+qat_c3xxxvf-objs := adf_drv.o adf_c3xxxvf_hw_data.o
diff --git a/drivers/crypto/qat/qat_c3xxxvf/adf_c3xxxvf_hw_data.c b/drivers/crypto/qat/qat_c3xxxvf/adf_c3xxxvf_hw_data.c
new file mode 100644
index 000000000000..1af321c2ce1a
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxxvf/adf_c3xxxvf_hw_data.c
@@ -0,0 +1,173 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2015 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2015 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <adf_accel_devices.h>
+#include <adf_pf2vf_msg.h>
+#include <adf_common_drv.h>
+#include "adf_c3xxxvf_hw_data.h"
+
+static struct adf_hw_device_class c3xxxiov_class = {
+	.name = ADF_C3XXXVF_DEVICE_NAME,
+	.type = DEV_C3XXXVF,
+	.instances = 0
+};
+
+static u32 get_accel_mask(u32 fuse)
+{
+	return ADF_C3XXXIOV_ACCELERATORS_MASK;
+}
+
+static u32 get_ae_mask(u32 fuse)
+{
+	return ADF_C3XXXIOV_ACCELENGINES_MASK;
+}
+
+static u32 get_num_accels(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXXIOV_MAX_ACCELERATORS;
+}
+
+static u32 get_num_aes(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXXIOV_MAX_ACCELENGINES;
+}
+
+static u32 get_misc_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXXIOV_PMISC_BAR;
+}
+
+static u32 get_etr_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C3XXXIOV_ETR_BAR;
+}
+
+static enum dev_sku_info get_sku(struct adf_hw_device_data *self)
+{
+	return DEV_SKU_VF;
+}
+
+static u32 get_pf2vf_offset(u32 i)
+{
+	return ADF_C3XXXIOV_PF2VF_OFFSET;
+}
+
+static u32 get_vintmsk_offset(u32 i)
+{
+	return ADF_C3XXXIOV_VINTMSK_OFFSET;
+}
+
+static int adf_vf_int_noop(struct adf_accel_dev *accel_dev)
+{
+	return 0;
+}
+
+static void adf_vf_void_noop(struct adf_accel_dev *accel_dev)
+{
+}
+
+static int adf_vf2pf_init(struct adf_accel_dev *accel_dev)
+{
+	u32 msg = (ADF_VF2PF_MSGORIGIN_SYSTEM |
+		(ADF_VF2PF_MSGTYPE_INIT << ADF_VF2PF_MSGTYPE_SHIFT));
+
+	if (adf_iov_putmsg(accel_dev, msg, 0)) {
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to send Init event to PF\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void adf_vf2pf_shutdown(struct adf_accel_dev *accel_dev)
+{
+	u32 msg = (ADF_VF2PF_MSGORIGIN_SYSTEM |
+	    (ADF_VF2PF_MSGTYPE_SHUTDOWN << ADF_VF2PF_MSGTYPE_SHIFT));
+
+	if (adf_iov_putmsg(accel_dev, msg, 0))
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to send Shutdown event to PF\n");
+}
+
+void adf_init_hw_data_c3xxxiov(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class = &c3xxxiov_class;
+	hw_data->num_banks = ADF_C3XXXIOV_ETR_MAX_BANKS;
+	hw_data->num_accel = ADF_C3XXXIOV_MAX_ACCELERATORS;
+	hw_data->num_logical_accel = 1;
+	hw_data->num_engines = ADF_C3XXXIOV_MAX_ACCELENGINES;
+	hw_data->tx_rx_gap = ADF_C3XXXIOV_RX_RINGS_OFFSET;
+	hw_data->tx_rings_mask = ADF_C3XXXIOV_TX_RINGS_MASK;
+	hw_data->alloc_irq = adf_vf_isr_resource_alloc;
+	hw_data->free_irq = adf_vf_isr_resource_free;
+	hw_data->enable_error_correction = adf_vf_void_noop;
+	hw_data->init_admin_comms = adf_vf_int_noop;
+	hw_data->exit_admin_comms = adf_vf_void_noop;
+	hw_data->send_admin_init = adf_vf2pf_init;
+	hw_data->init_arb = adf_vf_int_noop;
+	hw_data->exit_arb = adf_vf_void_noop;
+	hw_data->disable_iov = adf_vf2pf_shutdown;
+	hw_data->get_accel_mask = get_accel_mask;
+	hw_data->get_ae_mask = get_ae_mask;
+	hw_data->get_num_accels = get_num_accels;
+	hw_data->get_num_aes = get_num_aes;
+	hw_data->get_etr_bar_id = get_etr_bar_id;
+	hw_data->get_misc_bar_id = get_misc_bar_id;
+	hw_data->get_pf2vf_offset = get_pf2vf_offset;
+	hw_data->get_vintmsk_offset = get_vintmsk_offset;
+	hw_data->get_sku = get_sku;
+	hw_data->enable_ints = adf_vf_void_noop;
+	hw_data->enable_vf2pf_comms = adf_enable_vf2pf_comms;
+	hw_data->min_iov_compat_ver = ADF_PFVF_COMPATIBILITY_VERSION;
+	hw_data->dev_class->instances++;
+	adf_devmgr_update_class_index(hw_data);
+}
+
+void adf_clean_hw_data_c3xxxiov(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class->instances--;
+	adf_devmgr_update_class_index(hw_data);
+}
diff --git a/drivers/crypto/qat/qat_dh895xccvf/adf_drv.h b/drivers/crypto/qat/qat_c3xxxvf/adf_c3xxxvf_hw_data.h
index e270e4a63d14..934f216acf39 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/adf_drv.h
+++ b/drivers/crypto/qat/qat_c3xxxvf/adf_c3xxxvf_hw_data.h
@@ -3,7 +3,7 @@
   redistributing this file, you may do so under either license.
 
   GPL LICENSE SUMMARY
-  Copyright(c) 2014 Intel Corporation.
+  Copyright(c) 2015 Intel Corporation.
   This program is free software; you can redistribute it and/or modify
   it under the terms of version 2 of the GNU General Public License as
   published by the Free Software Foundation.
@@ -17,7 +17,7 @@
   qat-linux@intel.com
 
   BSD LICENSE
-  Copyright(c) 2014 Intel Corporation.
+  Copyright(c) 2015 Intel Corporation.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
@@ -44,14 +44,21 @@
   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
-#ifndef ADF_DH895xVF_DRV_H_
-#define ADF_DH895xVF_DRV_H_
-#include <adf_accel_devices.h>
-#include <adf_transport.h>
-
-void adf_init_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data);
-void adf_clean_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data);
-int adf_vf_isr_resource_alloc(struct adf_accel_dev *accel_dev);
-void adf_vf_isr_resource_free(struct adf_accel_dev *accel_dev);
-void adf_update_ring_arb_enable(struct adf_etr_ring_data *ring);
+#ifndef ADF_C3XXXVF_HW_DATA_H_
+#define ADF_C3XXXVF_HW_DATA_H_
+
+#define ADF_C3XXXIOV_PMISC_BAR 1
+#define ADF_C3XXXIOV_ACCELERATORS_MASK 0x1
+#define ADF_C3XXXIOV_ACCELENGINES_MASK 0x1
+#define ADF_C3XXXIOV_MAX_ACCELERATORS 1
+#define ADF_C3XXXIOV_MAX_ACCELENGINES 1
+#define ADF_C3XXXIOV_RX_RINGS_OFFSET 8
+#define ADF_C3XXXIOV_TX_RINGS_MASK 0xFF
+#define ADF_C3XXXIOV_ETR_BAR 0
+#define ADF_C3XXXIOV_ETR_MAX_BANKS 1
+#define ADF_C3XXXIOV_PF2VF_OFFSET	0x200
+#define ADF_C3XXXIOV_VINTMSK_OFFSET	0x208
+
+void adf_init_hw_data_c3xxxiov(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_c3xxxiov(struct adf_hw_device_data *hw_data);
 #endif
diff --git a/drivers/crypto/qat/qat_c3xxxvf/adf_drv.c b/drivers/crypto/qat/qat_c3xxxvf/adf_drv.c
new file mode 100644
index 000000000000..1ac4ae90e072
--- /dev/null
+++ b/drivers/crypto/qat/qat_c3xxxvf/adf_drv.c
@@ -0,0 +1,305 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_cfg.h>
+#include "adf_c3xxxvf_hw_data.h"
+
+#define ADF_SYSTEM_DEVICE(device_id) \
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
+
+static const struct pci_device_id adf_pci_tbl[] = {
+	ADF_SYSTEM_DEVICE(ADF_C3XXXIOV_PCI_DEVICE_ID),
+	{0,}
+};
+MODULE_DEVICE_TABLE(pci, adf_pci_tbl);
+
+static int adf_probe(struct pci_dev *dev, const struct pci_device_id *ent);
+static void adf_remove(struct pci_dev *dev);
+
+static struct pci_driver adf_driver = {
+	.id_table = adf_pci_tbl,
+	.name = ADF_C3XXXVF_DEVICE_NAME,
+	.probe = adf_probe,
+	.remove = adf_remove,
+};
+
+static void adf_cleanup_pci_dev(struct adf_accel_dev *accel_dev)
+{
+	pci_release_regions(accel_dev->accel_pci_dev.pci_dev);
+	pci_disable_device(accel_dev->accel_pci_dev.pci_dev);
+}
+
+static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
+{
+	struct adf_accel_pci *accel_pci_dev = &accel_dev->accel_pci_dev;
+	struct adf_accel_dev *pf;
+	int i;
+
+	for (i = 0; i < ADF_PCI_MAX_BARS; i++) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i];
+
+		if (bar->virt_addr)
+			pci_iounmap(accel_pci_dev->pci_dev, bar->virt_addr);
+	}
+
+	if (accel_dev->hw_device) {
+		switch (accel_pci_dev->pci_dev->device) {
+		case ADF_C3XXXIOV_PCI_DEVICE_ID:
+			adf_clean_hw_data_c3xxxiov(accel_dev->hw_device);
+			break;
+		default:
+			break;
+		}
+		kfree(accel_dev->hw_device);
+		accel_dev->hw_device = NULL;
+	}
+	adf_cfg_dev_remove(accel_dev);
+	debugfs_remove(accel_dev->debugfs_dir);
+	pf = adf_devmgr_pci_to_accel_dev(accel_pci_dev->pci_dev->physfn);
+	adf_devmgr_rm_dev(accel_dev, pf);
+}
+
+static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct adf_accel_dev *accel_dev;
+	struct adf_accel_dev *pf;
+	struct adf_accel_pci *accel_pci_dev;
+	struct adf_hw_device_data *hw_data;
+	char name[ADF_DEVICE_NAME_LENGTH];
+	unsigned int i, bar_nr;
+	int ret, bar_mask;
+
+	switch (ent->device) {
+	case ADF_C3XXXIOV_PCI_DEVICE_ID:
+		break;
+	default:
+		dev_err(&pdev->dev, "Invalid device 0x%x.\n", ent->device);
+		return -ENODEV;
+	}
+
+	accel_dev = kzalloc_node(sizeof(*accel_dev), GFP_KERNEL,
+				 dev_to_node(&pdev->dev));
+	if (!accel_dev)
+		return -ENOMEM;
+
+	accel_dev->is_vf = true;
+	pf = adf_devmgr_pci_to_accel_dev(pdev->physfn);
+	accel_pci_dev = &accel_dev->accel_pci_dev;
+	accel_pci_dev->pci_dev = pdev;
+
+	/* Add accel device to accel table */
+	if (adf_devmgr_add_dev(accel_dev, pf)) {
+		dev_err(&pdev->dev, "Failed to add new accelerator device.\n");
+		kfree(accel_dev);
+		return -EFAULT;
+	}
+	INIT_LIST_HEAD(&accel_dev->crypto_list);
+
+	accel_dev->owner = THIS_MODULE;
+	/* Allocate and configure device configuration structure */
+	hw_data = kzalloc_node(sizeof(*hw_data), GFP_KERNEL,
+			       dev_to_node(&pdev->dev));
+	if (!hw_data) {
+		ret = -ENOMEM;
+		goto out_err;
+	}
+	accel_dev->hw_device = hw_data;
+	adf_init_hw_data_c3xxxiov(accel_dev->hw_device);
+
+	/* Get Accelerators and Accelerators Engines masks */
+	hw_data->accel_mask = hw_data->get_accel_mask(hw_data->fuses);
+	hw_data->ae_mask = hw_data->get_ae_mask(hw_data->fuses);
+	accel_pci_dev->sku = hw_data->get_sku(hw_data);
+
+	/* Create dev top level debugfs entry */
+	snprintf(name, sizeof(name), "%s%s_%02x:%02d.%02d",
+		 ADF_DEVICE_NAME_PREFIX, hw_data->dev_class->name,
+		 pdev->bus->number, PCI_SLOT(pdev->devfn),
+		 PCI_FUNC(pdev->devfn));
+
+	accel_dev->debugfs_dir = debugfs_create_dir(name, NULL);
+	if (!accel_dev->debugfs_dir) {
+		dev_err(&pdev->dev, "Could not create debugfs dir %s\n", name);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	/* Create device configuration table */
+	ret = adf_cfg_dev_add(accel_dev);
+	if (ret)
+		goto out_err;
+
+	/* enable PCI device */
+	if (pci_enable_device(pdev)) {
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* set dma identifier */
+	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if ((pci_set_dma_mask(pdev, DMA_BIT_MASK(32)))) {
+			dev_err(&pdev->dev, "No usable DMA configuration\n");
+			ret = -EFAULT;
+			goto out_err_disable;
+		} else {
+			pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		}
+
+	} else {
+		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	}
+
+	if (pci_request_regions(pdev, ADF_C3XXXVF_DEVICE_NAME)) {
+		ret = -EFAULT;
+		goto out_err_disable;
+	}
+
+	/* Find and map all the device's BARS */
+	i = 0;
+	bar_mask = pci_select_bars(pdev, IORESOURCE_MEM);
+	for_each_set_bit(bar_nr, (const unsigned long *)&bar_mask,
+			 ADF_PCI_MAX_BARS * 2) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i++];
+
+		bar->base_addr = pci_resource_start(pdev, bar_nr);
+		if (!bar->base_addr)
+			break;
+		bar->size = pci_resource_len(pdev, bar_nr);
+		bar->virt_addr = pci_iomap(accel_pci_dev->pci_dev, bar_nr, 0);
+		if (!bar->virt_addr) {
+			dev_err(&pdev->dev, "Failed to map BAR %d\n", bar_nr);
+			ret = -EFAULT;
+			goto out_err_free_reg;
+		}
+	}
+	pci_set_master(pdev);
+	/* Completion for VF2PF request/response message exchange */
+	init_completion(&accel_dev->vf.iov_msg_completion);
+
+	ret = qat_crypto_dev_config(accel_dev);
+	if (ret)
+		goto out_err_free_reg;
+
+	ret = adf_dev_init(accel_dev);
+	if (ret)
+		goto out_err_dev_shutdown;
+
+	ret = adf_dev_start(accel_dev);
+	if (ret)
+		goto out_err_dev_stop;
+
+	return ret;
+
+out_err_dev_stop:
+	adf_dev_stop(accel_dev);
+out_err_dev_shutdown:
+	adf_dev_shutdown(accel_dev);
+out_err_free_reg:
+	pci_release_regions(accel_pci_dev->pci_dev);
+out_err_disable:
+	pci_disable_device(accel_pci_dev->pci_dev);
+out_err:
+	adf_cleanup_accel(accel_dev);
+	kfree(accel_dev);
+	return ret;
+}
+
+static void adf_remove(struct pci_dev *pdev)
+{
+	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
+
+	if (!accel_dev) {
+		pr_err("QAT: Driver removal failed\n");
+		return;
+	}
+	if (adf_dev_stop(accel_dev))
+		dev_err(&GET_DEV(accel_dev), "Failed to stop QAT accel dev\n");
+
+	adf_dev_shutdown(accel_dev);
+	adf_cleanup_accel(accel_dev);
+	adf_cleanup_pci_dev(accel_dev);
+	kfree(accel_dev);
+}
+
+static int __init adfdrv_init(void)
+{
+	request_module("intel_qat");
+
+	if (pci_register_driver(&adf_driver)) {
+		pr_err("QAT: Driver initialization failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void __exit adfdrv_release(void)
+{
+	pci_unregister_driver(&adf_driver);
+	adf_clean_vf_map(true);
+}
+
+module_init(adfdrv_init);
+module_exit(adfdrv_release);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel");
+MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/qat/qat_c62x/Makefile b/drivers/crypto/qat/qat_c62x/Makefile
new file mode 100644
index 000000000000..bd75ace59b76
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62x/Makefile
@@ -0,0 +1,3 @@
+ccflags-y := -I$(src)/../qat_common
+obj-$(CONFIG_CRYPTO_DEV_QAT_C62X) += qat_c62x.o
+qat_c62x-objs := adf_drv.o adf_c62x_hw_data.o
diff --git a/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.c b/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.c
new file mode 100644
index 000000000000..879e04cae714
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.c
@@ -0,0 +1,248 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in
+	  the documentation and/or other materials provided with the
+	  distribution.
+	* Neither the name of Intel Corporation nor the names of its
+	  contributors may be used to endorse or promote products derived
+	  from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_pf2vf_msg.h>
+#include "adf_c62x_hw_data.h"
+
+/* Worker thread to service arbiter mappings based on dev SKUs */
+static const u32 thrd_to_arb_map_8_me_sku[] = {
+	0x12222AAA, 0x11222AAA, 0x12222AAA, 0x11222AAA, 0x12222AAA,
+	0x11222AAA, 0x12222AAA, 0x11222AAA, 0, 0
+};
+
+static const u32 thrd_to_arb_map_10_me_sku[] = {
+	0x12222AAA, 0x11222AAA, 0x12222AAA, 0x11222AAA, 0x12222AAA,
+	0x11222AAA, 0x12222AAA, 0x11222AAA, 0x12222AAA, 0x11222AAA
+};
+
+static struct adf_hw_device_class c62x_class = {
+	.name = ADF_C62X_DEVICE_NAME,
+	.type = DEV_C62X,
+	.instances = 0
+};
+
+static u32 get_accel_mask(u32 fuse)
+{
+	return (~fuse) >> ADF_C62X_ACCELERATORS_REG_OFFSET &
+			  ADF_C62X_ACCELERATORS_MASK;
+}
+
+static u32 get_ae_mask(u32 fuse)
+{
+	return (~fuse) & ADF_C62X_ACCELENGINES_MASK;
+}
+
+static u32 get_num_accels(struct adf_hw_device_data *self)
+{
+	u32 i, ctr = 0;
+
+	if (!self || !self->accel_mask)
+		return 0;
+
+	for (i = 0; i < ADF_C62X_MAX_ACCELERATORS; i++) {
+		if (self->accel_mask & (1 << i))
+			ctr++;
+	}
+	return ctr;
+}
+
+static u32 get_num_aes(struct adf_hw_device_data *self)
+{
+	u32 i, ctr = 0;
+
+	if (!self || !self->ae_mask)
+		return 0;
+
+	for (i = 0; i < ADF_C62X_MAX_ACCELENGINES; i++) {
+		if (self->ae_mask & (1 << i))
+			ctr++;
+	}
+	return ctr;
+}
+
+static u32 get_misc_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C62X_PMISC_BAR;
+}
+
+static u32 get_etr_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C62X_ETR_BAR;
+}
+
+static u32 get_sram_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C62X_SRAM_BAR;
+}
+
+static enum dev_sku_info get_sku(struct adf_hw_device_data *self)
+{
+	int aes = get_num_aes(self);
+
+	if (aes == 8)
+		return DEV_SKU_2;
+	else if (aes == 10)
+		return DEV_SKU_4;
+
+	return DEV_SKU_UNKNOWN;
+}
+
+static void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
+				    u32 const **arb_map_config)
+{
+	switch (accel_dev->accel_pci_dev.sku) {
+	case DEV_SKU_2:
+		*arb_map_config = thrd_to_arb_map_8_me_sku;
+		break;
+	case DEV_SKU_4:
+		*arb_map_config = thrd_to_arb_map_10_me_sku;
+		break;
+	default:
+		dev_err(&GET_DEV(accel_dev),
+			"The configuration doesn't match any SKU");
+		*arb_map_config = NULL;
+	}
+}
+
+static u32 get_pf2vf_offset(u32 i)
+{
+	return ADF_C62X_PF2VF_OFFSET(i);
+}
+
+static u32 get_vintmsk_offset(u32 i)
+{
+	return ADF_C62X_VINTMSK_OFFSET(i);
+}
+
+static void adf_enable_error_correction(struct adf_accel_dev *accel_dev)
+{
+	struct adf_hw_device_data *hw_device = accel_dev->hw_device;
+	struct adf_bar *misc_bar = &GET_BARS(accel_dev)[ADF_C62X_PMISC_BAR];
+	void __iomem *csr = misc_bar->virt_addr;
+	unsigned int val, i;
+
+	/* Enable Accel Engine error detection & correction */
+	for (i = 0; i < hw_device->get_num_aes(hw_device); i++) {
+		val = ADF_CSR_RD(csr, ADF_C62X_AE_CTX_ENABLES(i));
+		val |= ADF_C62X_ENABLE_AE_ECC_ERR;
+		ADF_CSR_WR(csr, ADF_C62X_AE_CTX_ENABLES(i), val);
+		val = ADF_CSR_RD(csr, ADF_C62X_AE_MISC_CONTROL(i));
+		val |= ADF_C62X_ENABLE_AE_ECC_PARITY_CORR;
+		ADF_CSR_WR(csr, ADF_C62X_AE_MISC_CONTROL(i), val);
+	}
+
+	/* Enable shared memory error detection & correction */
+	for (i = 0; i < hw_device->get_num_accels(hw_device); i++) {
+		val = ADF_CSR_RD(csr, ADF_C62X_UERRSSMSH(i));
+		val |= ADF_C62X_ERRSSMSH_EN;
+		ADF_CSR_WR(csr, ADF_C62X_UERRSSMSH(i), val);
+		val = ADF_CSR_RD(csr, ADF_C62X_CERRSSMSH(i));
+		val |= ADF_C62X_ERRSSMSH_EN;
+		ADF_CSR_WR(csr, ADF_C62X_CERRSSMSH(i), val);
+	}
+}
+
+static void adf_enable_ints(struct adf_accel_dev *accel_dev)
+{
+	void __iomem *addr;
+
+	addr = (&GET_BARS(accel_dev)[ADF_C62X_PMISC_BAR])->virt_addr;
+
+	/* Enable bundle and misc interrupts */
+	ADF_CSR_WR(addr, ADF_C62X_SMIAPF0_MASK_OFFSET,
+		   ADF_C62X_SMIA0_MASK);
+	ADF_CSR_WR(addr, ADF_C62X_SMIAPF1_MASK_OFFSET,
+		   ADF_C62X_SMIA1_MASK);
+}
+
+static int adf_pf_enable_vf2pf_comms(struct adf_accel_dev *accel_dev)
+{
+	return 0;
+}
+
+void adf_init_hw_data_c62x(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class = &c62x_class;
+	hw_data->instance_id = c62x_class.instances++;
+	hw_data->num_banks = ADF_C62X_ETR_MAX_BANKS;
+	hw_data->num_accel = ADF_C62X_MAX_ACCELERATORS;
+	hw_data->num_logical_accel = 1;
+	hw_data->num_engines = ADF_C62X_MAX_ACCELENGINES;
+	hw_data->tx_rx_gap = ADF_C62X_RX_RINGS_OFFSET;
+	hw_data->tx_rings_mask = ADF_C62X_TX_RINGS_MASK;
+	hw_data->alloc_irq = adf_isr_resource_alloc;
+	hw_data->free_irq = adf_isr_resource_free;
+	hw_data->enable_error_correction = adf_enable_error_correction;
+	hw_data->get_accel_mask = get_accel_mask;
+	hw_data->get_ae_mask = get_ae_mask;
+	hw_data->get_num_accels = get_num_accels;
+	hw_data->get_num_aes = get_num_aes;
+	hw_data->get_sram_bar_id = get_sram_bar_id;
+	hw_data->get_etr_bar_id = get_etr_bar_id;
+	hw_data->get_misc_bar_id = get_misc_bar_id;
+	hw_data->get_pf2vf_offset = get_pf2vf_offset;
+	hw_data->get_vintmsk_offset = get_vintmsk_offset;
+	hw_data->get_sku = get_sku;
+	hw_data->fw_name = ADF_C62X_FW;
+	hw_data->fw_mmp_name = ADF_C62X_MMP;
+	hw_data->init_admin_comms = adf_init_admin_comms;
+	hw_data->exit_admin_comms = adf_exit_admin_comms;
+	hw_data->disable_iov = adf_disable_sriov;
+	hw_data->send_admin_init = adf_send_admin_init;
+	hw_data->init_arb = adf_init_arb;
+	hw_data->exit_arb = adf_exit_arb;
+	hw_data->get_arb_mapping = adf_get_arbiter_mapping;
+	hw_data->enable_ints = adf_enable_ints;
+	hw_data->enable_vf2pf_comms = adf_pf_enable_vf2pf_comms;
+	hw_data->min_iov_compat_ver = ADF_PFVF_COMPATIBILITY_VERSION;
+}
+
+void adf_clean_hw_data_c62x(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class->instances--;
+}
diff --git a/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.h b/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.h
new file mode 100644
index 000000000000..17a8a32d5c63
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62x/adf_c62x_hw_data.h
@@ -0,0 +1,84 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef ADF_C62X_HW_DATA_H_
+#define ADF_C62X_HW_DATA_H_
+
+/* PCIe configuration space */
+#define ADF_C62X_SRAM_BAR 0
+#define ADF_C62X_PMISC_BAR 1
+#define ADF_C62X_ETR_BAR 2
+#define ADF_C62X_RX_RINGS_OFFSET 8
+#define ADF_C62X_TX_RINGS_MASK 0xFF
+#define ADF_C62X_MAX_ACCELERATORS 5
+#define ADF_C62X_MAX_ACCELENGINES 10
+#define ADF_C62X_ACCELERATORS_REG_OFFSET 16
+#define ADF_C62X_ACCELERATORS_MASK 0x1F
+#define ADF_C62X_ACCELENGINES_MASK 0x3FF
+#define ADF_C62X_ETR_MAX_BANKS 16
+#define ADF_C62X_SMIAPF0_MASK_OFFSET (0x3A000 + 0x28)
+#define ADF_C62X_SMIAPF1_MASK_OFFSET (0x3A000 + 0x30)
+#define ADF_C62X_SMIA0_MASK 0xFFFF
+#define ADF_C62X_SMIA1_MASK 0x1
+/* Error detection and correction */
+#define ADF_C62X_AE_CTX_ENABLES(i) (i * 0x1000 + 0x20818)
+#define ADF_C62X_AE_MISC_CONTROL(i) (i * 0x1000 + 0x20960)
+#define ADF_C62X_ENABLE_AE_ECC_ERR BIT(28)
+#define ADF_C62X_ENABLE_AE_ECC_PARITY_CORR (BIT(24) | BIT(12))
+#define ADF_C62X_UERRSSMSH(i) (i * 0x4000 + 0x18)
+#define ADF_C62X_CERRSSMSH(i) (i * 0x4000 + 0x10)
+#define ADF_C62X_ERRSSMSH_EN BIT(3)
+
+#define ADF_C62X_PF2VF_OFFSET(i)	(0x3A000 + 0x280 + ((i) * 0x04))
+#define ADF_C62X_VINTMSK_OFFSET(i)	(0x3A000 + 0x200 + ((i) * 0x04))
+
+/* Firmware Binary */
+#define ADF_C62X_FW "qat_c62x.bin"
+#define ADF_C62X_MMP "qat_c62x_mmp.bin"
+
+void adf_init_hw_data_c62x(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_c62x(struct adf_hw_device_data *hw_data);
+#endif
diff --git a/drivers/crypto/qat/qat_c62x/adf_drv.c b/drivers/crypto/qat/qat_c62x/adf_drv.c
new file mode 100644
index 000000000000..512c56509718
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62x/adf_drv.c
@@ -0,0 +1,335 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_cfg.h>
+#include "adf_c62x_hw_data.h"
+
+#define ADF_SYSTEM_DEVICE(device_id) \
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
+
+static const struct pci_device_id adf_pci_tbl[] = {
+	ADF_SYSTEM_DEVICE(ADF_C62X_PCI_DEVICE_ID),
+	{0,}
+};
+MODULE_DEVICE_TABLE(pci, adf_pci_tbl);
+
+static int adf_probe(struct pci_dev *dev, const struct pci_device_id *ent);
+static void adf_remove(struct pci_dev *dev);
+
+static struct pci_driver adf_driver = {
+	.id_table = adf_pci_tbl,
+	.name = ADF_C62X_DEVICE_NAME,
+	.probe = adf_probe,
+	.remove = adf_remove,
+	.sriov_configure = adf_sriov_configure,
+};
+
+static void adf_cleanup_pci_dev(struct adf_accel_dev *accel_dev)
+{
+	pci_release_regions(accel_dev->accel_pci_dev.pci_dev);
+	pci_disable_device(accel_dev->accel_pci_dev.pci_dev);
+}
+
+static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
+{
+	struct adf_accel_pci *accel_pci_dev = &accel_dev->accel_pci_dev;
+	int i;
+
+	for (i = 0; i < ADF_PCI_MAX_BARS; i++) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i];
+
+		if (bar->virt_addr)
+			pci_iounmap(accel_pci_dev->pci_dev, bar->virt_addr);
+	}
+
+	if (accel_dev->hw_device) {
+		switch (accel_pci_dev->pci_dev->device) {
+		case ADF_C62X_PCI_DEVICE_ID:
+			adf_clean_hw_data_c62x(accel_dev->hw_device);
+			break;
+		default:
+			break;
+		}
+		kfree(accel_dev->hw_device);
+		accel_dev->hw_device = NULL;
+	}
+	adf_cfg_dev_remove(accel_dev);
+	debugfs_remove(accel_dev->debugfs_dir);
+	adf_devmgr_rm_dev(accel_dev, NULL);
+}
+
+static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct adf_accel_dev *accel_dev;
+	struct adf_accel_pci *accel_pci_dev;
+	struct adf_hw_device_data *hw_data;
+	char name[ADF_DEVICE_NAME_LENGTH];
+	unsigned int i, bar_nr;
+	int ret, bar_mask;
+
+	switch (ent->device) {
+	case ADF_C62X_PCI_DEVICE_ID:
+		break;
+	default:
+		dev_err(&pdev->dev, "Invalid device 0x%x.\n", ent->device);
+		return -ENODEV;
+	}
+
+	if (num_possible_nodes() > 1 && dev_to_node(&pdev->dev) < 0) {
+		/* If the accelerator is connected to a node with no memory
+		 * there is no point in using the accelerator since the remote
+		 * memory transaction will be very slow. */
+		dev_err(&pdev->dev, "Invalid NUMA configuration.\n");
+		return -EINVAL;
+	}
+
+	accel_dev = kzalloc_node(sizeof(*accel_dev), GFP_KERNEL,
+				 dev_to_node(&pdev->dev));
+	if (!accel_dev)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&accel_dev->crypto_list);
+	accel_pci_dev = &accel_dev->accel_pci_dev;
+	accel_pci_dev->pci_dev = pdev;
+
+	/* Add accel device to accel table.
+	 * This should be called before adf_cleanup_accel is called */
+	if (adf_devmgr_add_dev(accel_dev, NULL)) {
+		dev_err(&pdev->dev, "Failed to add new accelerator device.\n");
+		kfree(accel_dev);
+		return -EFAULT;
+	}
+
+	accel_dev->owner = THIS_MODULE;
+	/* Allocate and configure device configuration structure */
+	hw_data = kzalloc_node(sizeof(*hw_data), GFP_KERNEL,
+			       dev_to_node(&pdev->dev));
+	if (!hw_data) {
+		ret = -ENOMEM;
+		goto out_err;
+	}
+
+	accel_dev->hw_device = hw_data;
+	adf_init_hw_data_c62x(accel_dev->hw_device);
+	pci_read_config_byte(pdev, PCI_REVISION_ID, &accel_pci_dev->revid);
+	pci_read_config_dword(pdev, ADF_DEVICE_FUSECTL_OFFSET,
+			      &hw_data->fuses);
+
+	/* Get Accelerators and Accelerators Engines masks */
+	hw_data->accel_mask = hw_data->get_accel_mask(hw_data->fuses);
+	hw_data->ae_mask = hw_data->get_ae_mask(hw_data->fuses);
+	accel_pci_dev->sku = hw_data->get_sku(hw_data);
+	/* If the device has no acceleration engines then ignore it. */
+	if (!hw_data->accel_mask || !hw_data->ae_mask ||
+	    ((~hw_data->ae_mask) & 0x01)) {
+		dev_err(&pdev->dev, "No acceleration units found");
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* Create dev top level debugfs entry */
+	snprintf(name, sizeof(name), "%s%s_%02x:%02d.%02d",
+		 ADF_DEVICE_NAME_PREFIX, hw_data->dev_class->name,
+		 pdev->bus->number, PCI_SLOT(pdev->devfn),
+		 PCI_FUNC(pdev->devfn));
+
+	accel_dev->debugfs_dir = debugfs_create_dir(name, NULL);
+	if (!accel_dev->debugfs_dir) {
+		dev_err(&pdev->dev, "Could not create debugfs dir %s\n", name);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	/* Create device configuration table */
+	ret = adf_cfg_dev_add(accel_dev);
+	if (ret)
+		goto out_err;
+
+	/* enable PCI device */
+	if (pci_enable_device(pdev)) {
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* set dma identifier */
+	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if ((pci_set_dma_mask(pdev, DMA_BIT_MASK(32)))) {
+			dev_err(&pdev->dev, "No usable DMA configuration\n");
+			ret = -EFAULT;
+			goto out_err_disable;
+		} else {
+			pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		}
+
+	} else {
+		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	}
+
+	if (pci_request_regions(pdev, ADF_C62X_DEVICE_NAME)) {
+		ret = -EFAULT;
+		goto out_err_disable;
+	}
+
+	/* Read accelerator capabilities mask */
+	pci_read_config_dword(pdev, ADF_DEVICE_LEGFUSE_OFFSET,
+			      &hw_data->accel_capabilities_mask);
+
+	/* Find and map all the device's BARS */
+	i = 0;
+	bar_mask = pci_select_bars(pdev, IORESOURCE_MEM);
+	for_each_set_bit(bar_nr, (const unsigned long *)&bar_mask,
+			 ADF_PCI_MAX_BARS * 2) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i++];
+
+		bar->base_addr = pci_resource_start(pdev, bar_nr);
+		if (!bar->base_addr)
+			break;
+		bar->size = pci_resource_len(pdev, bar_nr);
+		bar->virt_addr = pci_iomap(accel_pci_dev->pci_dev, bar_nr, 0);
+		if (!bar->virt_addr) {
+			dev_err(&pdev->dev, "Failed to map BAR %d\n", bar_nr);
+			ret = -EFAULT;
+			goto out_err_free_reg;
+		}
+	}
+	pci_set_master(pdev);
+
+	if (adf_enable_aer(accel_dev, &adf_driver)) {
+		dev_err(&pdev->dev, "Failed to enable aer\n");
+		ret = -EFAULT;
+		goto out_err_free_reg;
+	}
+
+	if (pci_save_state(pdev)) {
+		dev_err(&pdev->dev, "Failed to save pci state\n");
+		ret = -ENOMEM;
+		goto out_err_free_reg;
+	}
+
+	ret = qat_crypto_dev_config(accel_dev);
+	if (ret)
+		goto out_err_free_reg;
+
+	ret = adf_dev_init(accel_dev);
+	if (ret)
+		goto out_err_dev_shutdown;
+
+	ret = adf_dev_start(accel_dev);
+	if (ret)
+		goto out_err_dev_stop;
+
+	return ret;
+
+out_err_dev_stop:
+	adf_dev_stop(accel_dev);
+out_err_dev_shutdown:
+	adf_dev_shutdown(accel_dev);
+out_err_free_reg:
+	pci_release_regions(accel_pci_dev->pci_dev);
+out_err_disable:
+	pci_disable_device(accel_pci_dev->pci_dev);
+out_err:
+	adf_cleanup_accel(accel_dev);
+	kfree(accel_dev);
+	return ret;
+}
+
+static void adf_remove(struct pci_dev *pdev)
+{
+	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
+
+	if (!accel_dev) {
+		pr_err("QAT: Driver removal failed\n");
+		return;
+	}
+	if (adf_dev_stop(accel_dev))
+		dev_err(&GET_DEV(accel_dev), "Failed to stop QAT accel dev\n");
+
+	adf_dev_shutdown(accel_dev);
+	adf_disable_aer(accel_dev);
+	adf_cleanup_accel(accel_dev);
+	adf_cleanup_pci_dev(accel_dev);
+	kfree(accel_dev);
+}
+
+static int __init adfdrv_init(void)
+{
+	request_module("intel_qat");
+
+	if (pci_register_driver(&adf_driver)) {
+		pr_err("QAT: Driver initialization failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void __exit adfdrv_release(void)
+{
+	pci_unregister_driver(&adf_driver);
+}
+
+module_init(adfdrv_init);
+module_exit(adfdrv_release);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel");
+MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/qat/qat_c62xvf/Makefile b/drivers/crypto/qat/qat_c62xvf/Makefile
new file mode 100644
index 000000000000..ecd708c213b2
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62xvf/Makefile
@@ -0,0 +1,3 @@
+ccflags-y := -I$(src)/../qat_common
+obj-$(CONFIG_CRYPTO_DEV_QAT_C62XVF) += qat_c62xvf.o
+qat_c62xvf-objs := adf_drv.o adf_c62xvf_hw_data.o
diff --git a/drivers/crypto/qat/qat_c62xvf/adf_c62xvf_hw_data.c b/drivers/crypto/qat/qat_c62xvf/adf_c62xvf_hw_data.c
new file mode 100644
index 000000000000..baf4b509c892
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62xvf/adf_c62xvf_hw_data.c
@@ -0,0 +1,173 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2015 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2015 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <adf_accel_devices.h>
+#include <adf_pf2vf_msg.h>
+#include <adf_common_drv.h>
+#include "adf_c62xvf_hw_data.h"
+
+static struct adf_hw_device_class c62xiov_class = {
+	.name = ADF_C62XVF_DEVICE_NAME,
+	.type = DEV_C62XVF,
+	.instances = 0
+};
+
+static u32 get_accel_mask(u32 fuse)
+{
+	return ADF_C62XIOV_ACCELERATORS_MASK;
+}
+
+static u32 get_ae_mask(u32 fuse)
+{
+	return ADF_C62XIOV_ACCELENGINES_MASK;
+}
+
+static u32 get_num_accels(struct adf_hw_device_data *self)
+{
+	return ADF_C62XIOV_MAX_ACCELERATORS;
+}
+
+static u32 get_num_aes(struct adf_hw_device_data *self)
+{
+	return ADF_C62XIOV_MAX_ACCELENGINES;
+}
+
+static u32 get_misc_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C62XIOV_PMISC_BAR;
+}
+
+static u32 get_etr_bar_id(struct adf_hw_device_data *self)
+{
+	return ADF_C62XIOV_ETR_BAR;
+}
+
+static enum dev_sku_info get_sku(struct adf_hw_device_data *self)
+{
+	return DEV_SKU_VF;
+}
+
+static u32 get_pf2vf_offset(u32 i)
+{
+	return ADF_C62XIOV_PF2VF_OFFSET;
+}
+
+static u32 get_vintmsk_offset(u32 i)
+{
+	return ADF_C62XIOV_VINTMSK_OFFSET;
+}
+
+static int adf_vf_int_noop(struct adf_accel_dev *accel_dev)
+{
+	return 0;
+}
+
+static void adf_vf_void_noop(struct adf_accel_dev *accel_dev)
+{
+}
+
+static int adf_vf2pf_init(struct adf_accel_dev *accel_dev)
+{
+	u32 msg = (ADF_VF2PF_MSGORIGIN_SYSTEM |
+		(ADF_VF2PF_MSGTYPE_INIT << ADF_VF2PF_MSGTYPE_SHIFT));
+
+	if (adf_iov_putmsg(accel_dev, msg, 0)) {
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to send Init event to PF\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void adf_vf2pf_shutdown(struct adf_accel_dev *accel_dev)
+{
+	u32 msg = (ADF_VF2PF_MSGORIGIN_SYSTEM |
+	    (ADF_VF2PF_MSGTYPE_SHUTDOWN << ADF_VF2PF_MSGTYPE_SHIFT));
+
+	if (adf_iov_putmsg(accel_dev, msg, 0))
+		dev_err(&GET_DEV(accel_dev),
+			"Failed to send Shutdown event to PF\n");
+}
+
+void adf_init_hw_data_c62xiov(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class = &c62xiov_class;
+	hw_data->num_banks = ADF_C62XIOV_ETR_MAX_BANKS;
+	hw_data->num_accel = ADF_C62XIOV_MAX_ACCELERATORS;
+	hw_data->num_logical_accel = 1;
+	hw_data->num_engines = ADF_C62XIOV_MAX_ACCELENGINES;
+	hw_data->tx_rx_gap = ADF_C62XIOV_RX_RINGS_OFFSET;
+	hw_data->tx_rings_mask = ADF_C62XIOV_TX_RINGS_MASK;
+	hw_data->alloc_irq = adf_vf_isr_resource_alloc;
+	hw_data->free_irq = adf_vf_isr_resource_free;
+	hw_data->enable_error_correction = adf_vf_void_noop;
+	hw_data->init_admin_comms = adf_vf_int_noop;
+	hw_data->exit_admin_comms = adf_vf_void_noop;
+	hw_data->send_admin_init = adf_vf2pf_init;
+	hw_data->init_arb = adf_vf_int_noop;
+	hw_data->exit_arb = adf_vf_void_noop;
+	hw_data->disable_iov = adf_vf2pf_shutdown;
+	hw_data->get_accel_mask = get_accel_mask;
+	hw_data->get_ae_mask = get_ae_mask;
+	hw_data->get_num_accels = get_num_accels;
+	hw_data->get_num_aes = get_num_aes;
+	hw_data->get_etr_bar_id = get_etr_bar_id;
+	hw_data->get_misc_bar_id = get_misc_bar_id;
+	hw_data->get_pf2vf_offset = get_pf2vf_offset;
+	hw_data->get_vintmsk_offset = get_vintmsk_offset;
+	hw_data->get_sku = get_sku;
+	hw_data->enable_ints = adf_vf_void_noop;
+	hw_data->enable_vf2pf_comms = adf_enable_vf2pf_comms;
+	hw_data->min_iov_compat_ver = ADF_PFVF_COMPATIBILITY_VERSION;
+	hw_data->dev_class->instances++;
+	adf_devmgr_update_class_index(hw_data);
+}
+
+void adf_clean_hw_data_c62xiov(struct adf_hw_device_data *hw_data)
+{
+	hw_data->dev_class->instances--;
+	adf_devmgr_update_class_index(hw_data);
+}
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_drv.h b/drivers/crypto/qat/qat_c62xvf/adf_c62xvf_hw_data.h
index 85ff245bd1d8..a28d83e77422 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_drv.h
+++ b/drivers/crypto/qat/qat_c62xvf/adf_c62xvf_hw_data.h
@@ -3,7 +3,7 @@
   redistributing this file, you may do so under either license.
 
   GPL LICENSE SUMMARY
-  Copyright(c) 2014 Intel Corporation.
+  Copyright(c) 2015 Intel Corporation.
   This program is free software; you can redistribute it and/or modify
   it under the terms of version 2 of the GNU General Public License as
   published by the Free Software Foundation.
@@ -17,7 +17,7 @@
   qat-linux@intel.com
 
   BSD LICENSE
-  Copyright(c) 2014 Intel Corporation.
+  Copyright(c) 2015 Intel Corporation.
   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions
   are met:
@@ -44,15 +44,21 @@
   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
-#ifndef ADF_DH895x_DRV_H_
-#define ADF_DH895x_DRV_H_
-#include <adf_accel_devices.h>
-#include <adf_transport.h>
-
-void adf_init_hw_data_dh895xcc(struct adf_hw_device_data *hw_data);
-void adf_clean_hw_data_dh895xcc(struct adf_hw_device_data *hw_data);
-int adf_isr_resource_alloc(struct adf_accel_dev *accel_dev);
-void adf_isr_resource_free(struct adf_accel_dev *accel_dev);
-void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
-			     uint32_t const **arb_map_config);
+#ifndef ADF_C62XVF_HW_DATA_H_
+#define ADF_C62XVF_HW_DATA_H_
+
+#define ADF_C62XIOV_PMISC_BAR 1
+#define ADF_C62XIOV_ACCELERATORS_MASK 0x1
+#define ADF_C62XIOV_ACCELENGINES_MASK 0x1
+#define ADF_C62XIOV_MAX_ACCELERATORS 1
+#define ADF_C62XIOV_MAX_ACCELENGINES 1
+#define ADF_C62XIOV_RX_RINGS_OFFSET 8
+#define ADF_C62XIOV_TX_RINGS_MASK 0xFF
+#define ADF_C62XIOV_ETR_BAR 0
+#define ADF_C62XIOV_ETR_MAX_BANKS 1
+#define ADF_C62XIOV_PF2VF_OFFSET	0x200
+#define ADF_C62XIOV_VINTMSK_OFFSET	0x208
+
+void adf_init_hw_data_c62xiov(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_c62xiov(struct adf_hw_device_data *hw_data);
 #endif
diff --git a/drivers/crypto/qat/qat_c62xvf/adf_drv.c b/drivers/crypto/qat/qat_c62xvf/adf_drv.c
new file mode 100644
index 000000000000..d2e4b928f3be
--- /dev/null
+++ b/drivers/crypto/qat/qat_c62xvf/adf_drv.c
@@ -0,0 +1,305 @@
+/*
+  This file is provided under a dual BSD/GPLv2 license.  When using or
+  redistributing this file, you may do so under either license.
+
+  GPL LICENSE SUMMARY
+  Copyright(c) 2014 Intel Corporation.
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of version 2 of the GNU General Public License as
+  published by the Free Software Foundation.
+
+  This program is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  General Public License for more details.
+
+  Contact Information:
+  qat-linux@intel.com
+
+  BSD LICENSE
+  Copyright(c) 2014 Intel Corporation.
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions
+  are met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in
+      the documentation and/or other materials provided with the
+      distribution.
+    * Neither the name of Intel Corporation nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/errno.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/platform_device.h>
+#include <linux/workqueue.h>
+#include <linux/io.h>
+#include <adf_accel_devices.h>
+#include <adf_common_drv.h>
+#include <adf_cfg.h>
+#include "adf_c62xvf_hw_data.h"
+
+#define ADF_SYSTEM_DEVICE(device_id) \
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
+
+static const struct pci_device_id adf_pci_tbl[] = {
+	ADF_SYSTEM_DEVICE(ADF_C62XIOV_PCI_DEVICE_ID),
+	{0,}
+};
+MODULE_DEVICE_TABLE(pci, adf_pci_tbl);
+
+static int adf_probe(struct pci_dev *dev, const struct pci_device_id *ent);
+static void adf_remove(struct pci_dev *dev);
+
+static struct pci_driver adf_driver = {
+	.id_table = adf_pci_tbl,
+	.name = ADF_C62XVF_DEVICE_NAME,
+	.probe = adf_probe,
+	.remove = adf_remove,
+};
+
+static void adf_cleanup_pci_dev(struct adf_accel_dev *accel_dev)
+{
+	pci_release_regions(accel_dev->accel_pci_dev.pci_dev);
+	pci_disable_device(accel_dev->accel_pci_dev.pci_dev);
+}
+
+static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
+{
+	struct adf_accel_pci *accel_pci_dev = &accel_dev->accel_pci_dev;
+	struct adf_accel_dev *pf;
+	int i;
+
+	for (i = 0; i < ADF_PCI_MAX_BARS; i++) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i];
+
+		if (bar->virt_addr)
+			pci_iounmap(accel_pci_dev->pci_dev, bar->virt_addr);
+	}
+
+	if (accel_dev->hw_device) {
+		switch (accel_pci_dev->pci_dev->device) {
+		case ADF_C62XIOV_PCI_DEVICE_ID:
+			adf_clean_hw_data_c62xiov(accel_dev->hw_device);
+			break;
+		default:
+			break;
+		}
+		kfree(accel_dev->hw_device);
+		accel_dev->hw_device = NULL;
+	}
+	adf_cfg_dev_remove(accel_dev);
+	debugfs_remove(accel_dev->debugfs_dir);
+	pf = adf_devmgr_pci_to_accel_dev(accel_pci_dev->pci_dev->physfn);
+	adf_devmgr_rm_dev(accel_dev, pf);
+}
+
+static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct adf_accel_dev *accel_dev;
+	struct adf_accel_dev *pf;
+	struct adf_accel_pci *accel_pci_dev;
+	struct adf_hw_device_data *hw_data;
+	char name[ADF_DEVICE_NAME_LENGTH];
+	unsigned int i, bar_nr;
+	int ret, bar_mask;
+
+	switch (ent->device) {
+	case ADF_C62XIOV_PCI_DEVICE_ID:
+		break;
+	default:
+		dev_err(&pdev->dev, "Invalid device 0x%x.\n", ent->device);
+		return -ENODEV;
+	}
+
+	accel_dev = kzalloc_node(sizeof(*accel_dev), GFP_KERNEL,
+				 dev_to_node(&pdev->dev));
+	if (!accel_dev)
+		return -ENOMEM;
+
+	accel_dev->is_vf = true;
+	pf = adf_devmgr_pci_to_accel_dev(pdev->physfn);
+	accel_pci_dev = &accel_dev->accel_pci_dev;
+	accel_pci_dev->pci_dev = pdev;
+
+	/* Add accel device to accel table */
+	if (adf_devmgr_add_dev(accel_dev, pf)) {
+		dev_err(&pdev->dev, "Failed to add new accelerator device.\n");
+		kfree(accel_dev);
+		return -EFAULT;
+	}
+	INIT_LIST_HEAD(&accel_dev->crypto_list);
+
+	accel_dev->owner = THIS_MODULE;
+	/* Allocate and configure device configuration structure */
+	hw_data = kzalloc_node(sizeof(*hw_data), GFP_KERNEL,
+			       dev_to_node(&pdev->dev));
+	if (!hw_data) {
+		ret = -ENOMEM;
+		goto out_err;
+	}
+	accel_dev->hw_device = hw_data;
+	adf_init_hw_data_c62xiov(accel_dev->hw_device);
+
+	/* Get Accelerators and Accelerators Engines masks */
+	hw_data->accel_mask = hw_data->get_accel_mask(hw_data->fuses);
+	hw_data->ae_mask = hw_data->get_ae_mask(hw_data->fuses);
+	accel_pci_dev->sku = hw_data->get_sku(hw_data);
+
+	/* Create dev top level debugfs entry */
+	snprintf(name, sizeof(name), "%s%s_%02x:%02d.%02d",
+		 ADF_DEVICE_NAME_PREFIX, hw_data->dev_class->name,
+		 pdev->bus->number, PCI_SLOT(pdev->devfn),
+		 PCI_FUNC(pdev->devfn));
+
+	accel_dev->debugfs_dir = debugfs_create_dir(name, NULL);
+	if (!accel_dev->debugfs_dir) {
+		dev_err(&pdev->dev, "Could not create debugfs dir %s\n", name);
+		ret = -EINVAL;
+		goto out_err;
+	}
+
+	/* Create device configuration table */
+	ret = adf_cfg_dev_add(accel_dev);
+	if (ret)
+		goto out_err;
+
+	/* enable PCI device */
+	if (pci_enable_device(pdev)) {
+		ret = -EFAULT;
+		goto out_err;
+	}
+
+	/* set dma identifier */
+	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if ((pci_set_dma_mask(pdev, DMA_BIT_MASK(32)))) {
+			dev_err(&pdev->dev, "No usable DMA configuration\n");
+			ret = -EFAULT;
+			goto out_err_disable;
+		} else {
+			pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		}
+
+	} else {
+		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	}
+
+	if (pci_request_regions(pdev, ADF_C62XVF_DEVICE_NAME)) {
+		ret = -EFAULT;
+		goto out_err_disable;
+	}
+
+	/* Find and map all the device's BARS */
+	i = 0;
+	bar_mask = pci_select_bars(pdev, IORESOURCE_MEM);
+	for_each_set_bit(bar_nr, (const unsigned long *)&bar_mask,
+			 ADF_PCI_MAX_BARS * 2) {
+		struct adf_bar *bar = &accel_pci_dev->pci_bars[i++];
+
+		bar->base_addr = pci_resource_start(pdev, bar_nr);
+		if (!bar->base_addr)
+			break;
+		bar->size = pci_resource_len(pdev, bar_nr);
+		bar->virt_addr = pci_iomap(accel_pci_dev->pci_dev, bar_nr, 0);
+		if (!bar->virt_addr) {
+			dev_err(&pdev->dev, "Failed to map BAR %d\n", bar_nr);
+			ret = -EFAULT;
+			goto out_err_free_reg;
+		}
+	}
+	pci_set_master(pdev);
+	/* Completion for VF2PF request/response message exchange */
+	init_completion(&accel_dev->vf.iov_msg_completion);
+
+	ret = qat_crypto_dev_config(accel_dev);
+	if (ret)
+		goto out_err_free_reg;
+
+	ret = adf_dev_init(accel_dev);
+	if (ret)
+		goto out_err_dev_shutdown;
+
+	ret = adf_dev_start(accel_dev);
+	if (ret)
+		goto out_err_dev_stop;
+
+	return ret;
+
+out_err_dev_stop:
+	adf_dev_stop(accel_dev);
+out_err_dev_shutdown:
+	adf_dev_shutdown(accel_dev);
+out_err_free_reg:
+	pci_release_regions(accel_pci_dev->pci_dev);
+out_err_disable:
+	pci_disable_device(accel_pci_dev->pci_dev);
+out_err:
+	adf_cleanup_accel(accel_dev);
+	kfree(accel_dev);
+	return ret;
+}
+
+static void adf_remove(struct pci_dev *pdev)
+{
+	struct adf_accel_dev *accel_dev = adf_devmgr_pci_to_accel_dev(pdev);
+
+	if (!accel_dev) {
+		pr_err("QAT: Driver removal failed\n");
+		return;
+	}
+	if (adf_dev_stop(accel_dev))
+		dev_err(&GET_DEV(accel_dev), "Failed to stop QAT accel dev\n");
+
+	adf_dev_shutdown(accel_dev);
+	adf_cleanup_accel(accel_dev);
+	adf_cleanup_pci_dev(accel_dev);
+	kfree(accel_dev);
+}
+
+static int __init adfdrv_init(void)
+{
+	request_module("intel_qat");
+
+	if (pci_register_driver(&adf_driver)) {
+		pr_err("QAT: Driver initialization failed\n");
+		return -EFAULT;
+	}
+	return 0;
+}
+
+static void __exit adfdrv_release(void)
+{
+	pci_unregister_driver(&adf_driver);
+	adf_clean_vf_map(true);
+}
+
+module_init(adfdrv_init);
+module_exit(adfdrv_release);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("Intel");
+MODULE_DESCRIPTION("Intel(R) QuickAssist Technology");
+MODULE_VERSION(ADF_DRV_VERSION);
diff --git a/drivers/crypto/qat/qat_common/Makefile b/drivers/crypto/qat/qat_common/Makefile
index 9e9e196c6d51..29c7c53d2845 100644
--- a/drivers/crypto/qat/qat_common/Makefile
+++ b/drivers/crypto/qat/qat_common/Makefile
@@ -4,10 +4,12 @@ $(obj)/qat_rsaprivkey-asn1.o: $(obj)/qat_rsaprivkey-asn1.c \
 			      $(obj)/qat_rsaprivkey-asn1.h
 
 clean-files += qat_rsapubkey-asn1.c qat_rsapubkey-asn1.h
-clean-files += qat_rsaprivkey-asn1.c qat_rsapvivkey-asn1.h
+clean-files += qat_rsaprivkey-asn1.c qat_rsaprivkey-asn1.h
 
 obj-$(CONFIG_CRYPTO_DEV_QAT) += intel_qat.o
 intel_qat-objs := adf_cfg.o \
+	adf_isr.o \
+	adf_vf_isr.o \
 	adf_ctl_drv.o \
 	adf_dev_mgr.o \
 	adf_init.o \
diff --git a/drivers/crypto/qat/qat_common/adf_accel_devices.h b/drivers/crypto/qat/qat_common/adf_accel_devices.h
index ca853d50b4b7..f96d427e502c 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_devices.h
+++ b/drivers/crypto/qat/qat_common/adf_accel_devices.h
@@ -55,8 +55,20 @@
 
 #define ADF_DH895XCC_DEVICE_NAME "dh895xcc"
 #define ADF_DH895XCCVF_DEVICE_NAME "dh895xccvf"
+#define ADF_C62X_DEVICE_NAME "c62x"
+#define ADF_C62XVF_DEVICE_NAME "c62xvf"
+#define ADF_C3XXX_DEVICE_NAME "c3xxx"
+#define ADF_C3XXXVF_DEVICE_NAME "c3xxxvf"
 #define ADF_DH895XCC_PCI_DEVICE_ID 0x435
 #define ADF_DH895XCCIOV_PCI_DEVICE_ID 0x443
+#define ADF_C62X_PCI_DEVICE_ID 0x37c8
+#define ADF_C62XIOV_PCI_DEVICE_ID 0x37c9
+#define ADF_C3XXX_PCI_DEVICE_ID 0x19e2
+#define ADF_C3XXXIOV_PCI_DEVICE_ID 0x19e3
+#define ADF_ERRSOU3 (0x3A000 + 0x0C)
+#define ADF_ERRSOU5 (0x3A000 + 0xD8)
+#define ADF_DEVICE_FUSECTL_OFFSET 0x40
+#define ADF_DEVICE_LEGFUSE_OFFSET 0x4C
 #define ADF_PCI_MAX_BARS 3
 #define ADF_DEVICE_NAME_LENGTH 32
 #define ADF_ETR_MAX_RINGS_PER_BANK 16
@@ -168,11 +180,11 @@ struct adf_hw_device_data {
 	const char *fw_mmp_name;
 	uint32_t fuses;
 	uint32_t accel_capabilities_mask;
+	uint32_t instance_id;
 	uint16_t accel_mask;
 	uint16_t ae_mask;
 	uint16_t tx_rings_mask;
 	uint8_t tx_rx_gap;
-	uint8_t instance_id;
 	uint8_t num_banks;
 	uint8_t num_accel;
 	uint8_t num_logical_accel;
@@ -239,6 +251,6 @@ struct adf_accel_dev {
 		} vf;
 	};
 	bool is_vf;
-	uint8_t accel_id;
+	u32 accel_id;
 } __packed;
 #endif
diff --git a/drivers/crypto/qat/qat_common/adf_accel_engine.c b/drivers/crypto/qat/qat_common/adf_accel_engine.c
index 20b08bdcb146..a42fc42704be 100644
--- a/drivers/crypto/qat/qat_common/adf_accel_engine.c
+++ b/drivers/crypto/qat/qat_common/adf_accel_engine.c
@@ -78,9 +78,12 @@ int adf_ae_fw_load(struct adf_accel_dev *accel_dev)
 	uof_addr = (void *)loader_data->uof_fw->data;
 	mmp_size = loader_data->mmp_fw->size;
 	mmp_addr = (void *)loader_data->mmp_fw->data;
-	qat_uclo_wr_mimage(loader_data->fw_loader, mmp_addr, mmp_size);
-	if (qat_uclo_map_uof_obj(loader_data->fw_loader, uof_addr, uof_size)) {
-		dev_err(&GET_DEV(accel_dev), "Failed to map UOF\n");
+	if (qat_uclo_wr_mimage(loader_data->fw_loader, mmp_addr, mmp_size)) {
+		dev_err(&GET_DEV(accel_dev), "Failed to load MMP\n");
+		goto out_err;
+	}
+	if (qat_uclo_map_obj(loader_data->fw_loader, uof_addr, uof_size)) {
+		dev_err(&GET_DEV(accel_dev), "Failed to map FW\n");
 		goto out_err;
 	}
 	if (qat_uclo_wr_all_uimage(loader_data->fw_loader)) {
diff --git a/drivers/crypto/qat/qat_common/adf_admin.c b/drivers/crypto/qat/qat_common/adf_admin.c
index 147d755fed97..eb557f69e367 100644
--- a/drivers/crypto/qat/qat_common/adf_admin.c
+++ b/drivers/crypto/qat/qat_common/adf_admin.c
@@ -51,6 +51,7 @@
 #include <linux/pci.h>
 #include <linux/dma-mapping.h>
 #include "adf_accel_devices.h"
+#include "adf_common_drv.h"
 #include "icp_qat_fw_init_admin.h"
 
 /* Admin Messages Registers */
@@ -234,7 +235,8 @@ int adf_init_admin_comms(struct adf_accel_dev *accel_dev)
 	struct adf_bar *pmisc =
 		&GET_BARS(accel_dev)[hw_data->get_misc_bar_id(hw_data)];
 	void __iomem *csr = pmisc->virt_addr;
-	void __iomem *mailbox = csr + ADF_DH895XCC_MAILBOX_BASE_OFFSET;
+	void __iomem *mailbox = (void __iomem *)((uintptr_t)csr +
+				 ADF_DH895XCC_MAILBOX_BASE_OFFSET);
 	u64 reg_val;
 
 	admin = kzalloc_node(sizeof(*accel_dev->admin), GFP_KERNEL,
diff --git a/drivers/crypto/qat/qat_common/adf_aer.c b/drivers/crypto/qat/qat_common/adf_aer.c
index 0a5ca0ba5d64..e78a1d7d88fc 100644
--- a/drivers/crypto/qat/qat_common/adf_aer.c
+++ b/drivers/crypto/qat/qat_common/adf_aer.c
@@ -82,7 +82,7 @@ struct adf_reset_dev_data {
 	struct work_struct reset_work;
 };
 
-static void adf_dev_restore(struct adf_accel_dev *accel_dev)
+void adf_dev_restore(struct adf_accel_dev *accel_dev)
 {
 	struct pci_dev *pdev = accel_to_pci_dev(accel_dev);
 	struct pci_dev *parent = pdev->bus->self;
@@ -197,7 +197,7 @@ static void adf_resume(struct pci_dev *pdev)
 	dev_info(&pdev->dev, "Device is up and runnig\n");
 }
 
-static struct pci_error_handlers adf_err_handler = {
+static const struct pci_error_handlers adf_err_handler = {
 	.error_detected = adf_error_detected,
 	.slot_reset = adf_slot_reset,
 	.resume = adf_resume,
diff --git a/drivers/crypto/qat/qat_common/adf_cfg_common.h b/drivers/crypto/qat/qat_common/adf_cfg_common.h
index c697fb1cdfb5..8c4f6573ce59 100644
--- a/drivers/crypto/qat/qat_common/adf_cfg_common.h
+++ b/drivers/crypto/qat/qat_common/adf_cfg_common.h
@@ -72,12 +72,16 @@ enum adf_device_type {
 	DEV_UNKNOWN = 0,
 	DEV_DH895XCC,
 	DEV_DH895XCCVF,
+	DEV_C62X,
+	DEV_C62XVF,
+	DEV_C3XXX,
+	DEV_C3XXXVF
 };
 
 struct adf_dev_status_info {
 	enum adf_device_type type;
-	uint8_t accel_id;
-	uint8_t instance_id;
+	u32 accel_id;
+	u32 instance_id;
 	uint8_t num_ae;
 	uint8_t num_accel;
 	uint8_t num_logical_accel;
diff --git a/drivers/crypto/qat/qat_common/adf_common_drv.h b/drivers/crypto/qat/qat_common/adf_common_drv.h
index 3f76bd495bcb..0e82ce3c383e 100644
--- a/drivers/crypto/qat/qat_common/adf_common_drv.h
+++ b/drivers/crypto/qat/qat_common/adf_common_drv.h
@@ -54,7 +54,7 @@
 #include "icp_qat_hal.h"
 
 #define ADF_MAJOR_VERSION	0
-#define ADF_MINOR_VERSION	2
+#define ADF_MINOR_VERSION	6
 #define ADF_BUILD_VERSION	0
 #define ADF_DRV_VERSION		__stringify(ADF_MAJOR_VERSION) "." \
 				__stringify(ADF_MINOR_VERSION) "." \
@@ -106,8 +106,6 @@ int adf_dev_start(struct adf_accel_dev *accel_dev);
 int adf_dev_stop(struct adf_accel_dev *accel_dev);
 void adf_dev_shutdown(struct adf_accel_dev *accel_dev);
 
-void adf_enable_pf2vf_interrupts(struct adf_accel_dev *accel_dev);
-void adf_disable_pf2vf_interrupts(struct adf_accel_dev *accel_dev);
 int adf_iov_putmsg(struct adf_accel_dev *accel_dev, u32 msg, u8 vf_nr);
 void adf_pf2vf_notify_restarting(struct adf_accel_dev *accel_dev);
 int adf_enable_vf2pf_comms(struct adf_accel_dev *accel_dev);
@@ -143,6 +141,7 @@ int adf_ae_stop(struct adf_accel_dev *accel_dev);
 
 int adf_enable_aer(struct adf_accel_dev *accel_dev, struct pci_driver *adf);
 void adf_disable_aer(struct adf_accel_dev *accel_dev);
+void adf_dev_restore(struct adf_accel_dev *accel_dev);
 int adf_init_aer(void);
 void adf_exit_aer(void);
 int adf_init_admin_comms(struct adf_accel_dev *accel_dev);
@@ -159,6 +158,7 @@ int adf_init_etr_data(struct adf_accel_dev *accel_dev);
 void adf_cleanup_etr_data(struct adf_accel_dev *accel_dev);
 int qat_crypto_register(void);
 int qat_crypto_unregister(void);
+int qat_crypto_dev_config(struct adf_accel_dev *accel_dev);
 struct qat_crypto_instance *qat_crypto_get_instance_node(int node);
 void qat_crypto_put_instance(struct qat_crypto_instance *inst);
 void qat_alg_callback(void *resp);
@@ -168,6 +168,11 @@ void qat_algs_unregister(void);
 int qat_asym_algs_register(void);
 void qat_asym_algs_unregister(void);
 
+int adf_isr_resource_alloc(struct adf_accel_dev *accel_dev);
+void adf_isr_resource_free(struct adf_accel_dev *accel_dev);
+int adf_vf_isr_resource_alloc(struct adf_accel_dev *accel_dev);
+void adf_vf_isr_resource_free(struct adf_accel_dev *accel_dev);
+
 int qat_hal_init(struct adf_accel_dev *accel_dev);
 void qat_hal_deinit(struct icp_qat_fw_loader_handle *handle);
 void qat_hal_start(struct icp_qat_fw_loader_handle *handle, unsigned char ae,
@@ -178,6 +183,8 @@ void qat_hal_reset(struct icp_qat_fw_loader_handle *handle);
 int qat_hal_clr_reset(struct icp_qat_fw_loader_handle *handle);
 void qat_hal_set_live_ctx(struct icp_qat_fw_loader_handle *handle,
 			  unsigned char ae, unsigned int ctx_mask);
+int qat_hal_check_ae_active(struct icp_qat_fw_loader_handle *handle,
+			    unsigned int ae);
 int qat_hal_set_ae_lm_mode(struct icp_qat_fw_loader_handle *handle,
 			   unsigned char ae, enum icp_qat_uof_regtype lm_type,
 			   unsigned char mode);
@@ -216,10 +223,10 @@ int qat_hal_wr_lm(struct icp_qat_fw_loader_handle *handle,
 		  unsigned char ae, unsigned short lm_addr, unsigned int value);
 int qat_uclo_wr_all_uimage(struct icp_qat_fw_loader_handle *handle);
 void qat_uclo_del_uof_obj(struct icp_qat_fw_loader_handle *handle);
-int qat_uclo_map_uof_obj(struct icp_qat_fw_loader_handle *handle,
-			 void *addr_ptr, int mem_size);
-void qat_uclo_wr_mimage(struct icp_qat_fw_loader_handle *handle,
-			void *addr_ptr, int mem_size);
+int qat_uclo_wr_mimage(struct icp_qat_fw_loader_handle *handle, void *addr_ptr,
+		       int mem_size);
+int qat_uclo_map_obj(struct icp_qat_fw_loader_handle *handle,
+		     void *addr_ptr, int mem_size);
 #if defined(CONFIG_PCI_IOV)
 int adf_sriov_configure(struct pci_dev *pdev, int numvfs);
 void adf_disable_sriov(struct adf_accel_dev *accel_dev);
@@ -227,6 +234,8 @@ void adf_disable_vf2pf_interrupts(struct adf_accel_dev *accel_dev,
 				  uint32_t vf_mask);
 void adf_enable_vf2pf_interrupts(struct adf_accel_dev *accel_dev,
 				 uint32_t vf_mask);
+void adf_enable_pf2vf_interrupts(struct adf_accel_dev *accel_dev);
+void adf_disable_pf2vf_interrupts(struct adf_accel_dev *accel_dev);
 #else
 static inline int adf_sriov_configure(struct pci_dev *pdev, int numvfs)
 {
@@ -236,5 +245,13 @@ static inline int adf_sriov_configure(struct pci_dev *pdev, int numvfs)
 static inline void adf_disable_sriov(struct adf_accel_dev *accel_dev)
 {
 }
+
+static inline void adf_enable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
+{
+}
+
+static inline void adf_disable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
+{
+}
 #endif
 #endif
diff --git a/drivers/crypto/qat/qat_common/adf_ctl_drv.c b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
index 473d36d91644..5c897e6e7994 100644
--- a/drivers/crypto/qat/qat_common/adf_ctl_drv.c
+++ b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
@@ -255,12 +255,9 @@ out:
 
 static int adf_ctl_is_device_in_use(int id)
 {
-	struct list_head *itr, *head = adf_devmgr_get_head();
-
-	list_for_each(itr, head) {
-		struct adf_accel_dev *dev =
-				list_entry(itr, struct adf_accel_dev, list);
+	struct adf_accel_dev *dev;
 
+	list_for_each_entry(dev, adf_devmgr_get_head(), list) {
 		if (id == dev->accel_id || id == ADF_CFG_ALL_DEVICES) {
 			if (adf_devmgr_in_reset(dev) || adf_dev_in_use(dev)) {
 				dev_info(&GET_DEV(dev),
@@ -275,12 +272,10 @@ static int adf_ctl_is_device_in_use(int id)
 
 static int adf_ctl_stop_devices(uint32_t id)
 {
-	struct list_head *itr, *head = adf_devmgr_get_head();
+	struct adf_accel_dev *accel_dev;
 	int ret = 0;
 
-	list_for_each(itr, head) {
-		struct adf_accel_dev *accel_dev =
-				list_entry(itr, struct adf_accel_dev, list);
+	list_for_each_entry_reverse(accel_dev, adf_devmgr_get_head(), list) {
 		if (id == accel_dev->accel_id || id == ADF_CFG_ALL_DEVICES) {
 			if (!adf_dev_started(accel_dev))
 				continue;
@@ -342,12 +337,10 @@ static int adf_ctl_ioctl_dev_start(struct file *fp, unsigned int cmd,
 	if (ret)
 		return ret;
 
+	ret = -ENODEV;
 	accel_dev = adf_devmgr_get_dev_by_id(ctl_data->device_id);
-	if (!accel_dev) {
-		pr_err("QAT: Device %d not found\n", ctl_data->device_id);
-		ret = -ENODEV;
+	if (!accel_dev)
 		goto out;
-	}
 
 	if (!adf_dev_started(accel_dev)) {
 		dev_info(&GET_DEV(accel_dev),
diff --git a/drivers/crypto/qat/qat_common/adf_dev_mgr.c b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
index 8dfdb8f90797..b3ebb25f9ca7 100644
--- a/drivers/crypto/qat/qat_common/adf_dev_mgr.c
+++ b/drivers/crypto/qat/qat_common/adf_dev_mgr.c
@@ -53,6 +53,7 @@ static LIST_HEAD(accel_table);
 static LIST_HEAD(vfs_table);
 static DEFINE_MUTEX(table_lock);
 static uint32_t num_devices;
+static u8 id_map[ADF_MAX_DEVICES];
 
 struct vf_id_map {
 	u32 bdf;
@@ -116,8 +117,10 @@ void adf_clean_vf_map(bool vf)
 	mutex_lock(&table_lock);
 	list_for_each_safe(ptr, tmp, &vfs_table) {
 		map = list_entry(ptr, struct vf_id_map, list);
-		if (map->bdf != -1)
+		if (map->bdf != -1) {
+			id_map[map->id] = 0;
 			num_devices--;
+		}
 
 		if (vf && map->bdf == -1)
 			continue;
@@ -154,6 +157,19 @@ void adf_devmgr_update_class_index(struct adf_hw_device_data *hw_data)
 }
 EXPORT_SYMBOL_GPL(adf_devmgr_update_class_index);
 
+static unsigned int adf_find_free_id(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < ADF_MAX_DEVICES; i++) {
+		if (!id_map[i]) {
+			id_map[i] = 1;
+			return i;
+		}
+	}
+	return ADF_MAX_DEVICES + 1;
+}
+
 /**
  * adf_devmgr_add_dev() - Add accel_dev to the acceleration framework
  * @accel_dev:  Pointer to acceleration device.
@@ -194,8 +210,12 @@ int adf_devmgr_add_dev(struct adf_accel_dev *accel_dev,
 		}
 
 		list_add_tail(&accel_dev->list, &accel_table);
-		accel_dev->accel_id = num_devices++;
-
+		accel_dev->accel_id = adf_find_free_id();
+		if (accel_dev->accel_id > ADF_MAX_DEVICES) {
+			ret = -EFAULT;
+			goto unlock;
+		}
+		num_devices++;
 		map = kzalloc(sizeof(*map), GFP_KERNEL);
 		if (!map) {
 			ret = -ENOMEM;
@@ -236,8 +256,13 @@ int adf_devmgr_add_dev(struct adf_accel_dev *accel_dev,
 			ret = -ENOMEM;
 			goto unlock;
 		}
-
-		accel_dev->accel_id = num_devices++;
+		accel_dev->accel_id = adf_find_free_id();
+		if (accel_dev->accel_id > ADF_MAX_DEVICES) {
+			kfree(map);
+			ret = -EFAULT;
+			goto unlock;
+		}
+		num_devices++;
 		list_add_tail(&accel_dev->list, &accel_table);
 		map->bdf = adf_get_vf_num(accel_dev);
 		map->id = accel_dev->accel_id;
@@ -271,6 +296,7 @@ void adf_devmgr_rm_dev(struct adf_accel_dev *accel_dev,
 {
 	mutex_lock(&table_lock);
 	if (!accel_dev->is_vf || (accel_dev->is_vf && !pf)) {
+		id_map[accel_dev->accel_id] = 0;
 		num_devices--;
 	} else if (accel_dev->is_vf && pf) {
 		struct vf_id_map *map, *next;
diff --git a/drivers/crypto/qat/qat_common/adf_hw_arbiter.c b/drivers/crypto/qat/qat_common/adf_hw_arbiter.c
index 6849422e04bb..f267d9e42e0b 100644
--- a/drivers/crypto/qat/qat_common/adf_hw_arbiter.c
+++ b/drivers/crypto/qat/qat_common/adf_hw_arbiter.c
@@ -45,6 +45,7 @@
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #include "adf_accel_devices.h"
+#include "adf_common_drv.h"
 #include "adf_transport_internal.h"
 
 #define ADF_ARB_NUM 4
@@ -124,19 +125,12 @@ int adf_init_arb(struct adf_accel_dev *accel_dev)
 }
 EXPORT_SYMBOL_GPL(adf_init_arb);
 
-/**
- * adf_update_ring_arb() - update ring arbitration rgister
- * @accel_dev:  Pointer to ring data.
- *
- * Function enables or disables rings for/from arbitration.
- */
 void adf_update_ring_arb(struct adf_etr_ring_data *ring)
 {
 	WRITE_CSR_ARB_RINGSRVARBEN(ring->bank->csr_addr,
 				   ring->bank->bank_number,
 				   ring->bank->ring_mask & 0xFF);
 }
-EXPORT_SYMBOL_GPL(adf_update_ring_arb);
 
 void adf_exit_arb(struct adf_accel_dev *accel_dev)
 {
diff --git a/drivers/crypto/qat/qat_common/adf_init.c b/drivers/crypto/qat/qat_common/adf_init.c
index d873eeecc363..ef5575e4a215 100644
--- a/drivers/crypto/qat/qat_common/adf_init.c
+++ b/drivers/crypto/qat/qat_common/adf_init.c
@@ -62,15 +62,6 @@ static void adf_service_add(struct service_hndl *service)
 	mutex_unlock(&service_lock);
 }
 
-/**
- * adf_service_register() - Register acceleration service in the accel framework
- * @service:    Pointer to the service
- *
- * Function adds the acceleration service to the acceleration framework.
- * To be used by QAT device specific drivers.
- *
- * Return: 0 on success, error code otherwise.
- */
 int adf_service_register(struct service_hndl *service)
 {
 	service->init_status = 0;
@@ -78,7 +69,6 @@ int adf_service_register(struct service_hndl *service)
 	adf_service_add(service);
 	return 0;
 }
-EXPORT_SYMBOL_GPL(adf_service_register);
 
 static void adf_service_remove(struct service_hndl *service)
 {
@@ -87,15 +77,6 @@ static void adf_service_remove(struct service_hndl *service)
 	mutex_unlock(&service_lock);
 }
 
-/**
- * adf_service_unregister() - Unregister acceleration service from the framework
- * @service:    Pointer to the service
- *
- * Function remove the acceleration service from the acceleration framework.
- * To be used by QAT device specific drivers.
- *
- * Return: 0 on success, error code otherwise.
- */
 int adf_service_unregister(struct service_hndl *service)
 {
 	if (service->init_status || service->start_status) {
@@ -105,7 +86,6 @@ int adf_service_unregister(struct service_hndl *service)
 	adf_service_remove(service);
 	return 0;
 }
-EXPORT_SYMBOL_GPL(adf_service_unregister);
 
 /**
  * adf_dev_init() - Init data structures and services for the given accel device
@@ -366,6 +346,7 @@ void adf_dev_shutdown(struct adf_accel_dev *accel_dev)
 
 	hw_data->disable_iov(accel_dev);
 	adf_cleanup_etr_data(accel_dev);
+	adf_dev_restore(accel_dev);
 }
 EXPORT_SYMBOL_GPL(adf_dev_shutdown);
 
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c b/drivers/crypto/qat/qat_common/adf_isr.c
index 5570f78795c1..b81f79acc4ea 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
+++ b/drivers/crypto/qat/qat_common/adf_isr.c
@@ -51,15 +51,13 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/interrupt.h>
-#include <adf_accel_devices.h>
-#include <adf_common_drv.h>
-#include <adf_cfg.h>
-#include <adf_cfg_strings.h>
-#include <adf_cfg_common.h>
-#include <adf_transport_access_macros.h>
-#include <adf_transport_internal.h>
-#include "adf_drv.h"
-#include "adf_dh895xcc_hw_data.h"
+#include "adf_accel_devices.h"
+#include "adf_common_drv.h"
+#include "adf_cfg.h"
+#include "adf_cfg_strings.h"
+#include "adf_cfg_common.h"
+#include "adf_transport_access_macros.h"
+#include "adf_transport_internal.h"
 
 static int adf_enable_msix(struct adf_accel_dev *accel_dev)
 {
@@ -109,14 +107,16 @@ static irqreturn_t adf_msix_isr_ae(int irq, void *dev_ptr)
 #ifdef CONFIG_PCI_IOV
 	/* If SR-IOV is enabled (vf_info is non-NULL), check for VF->PF ints */
 	if (accel_dev->pf.vf_info) {
-		void __iomem *pmisc_bar_addr =
-		    (&GET_BARS(accel_dev)[ADF_DH895XCC_PMISC_BAR])->virt_addr;
+		struct adf_hw_device_data *hw_data = accel_dev->hw_device;
+		struct adf_bar *pmisc =
+			&GET_BARS(accel_dev)[hw_data->get_misc_bar_id(hw_data)];
+		void __iomem *pmisc_bar_addr = pmisc->virt_addr;
 		u32 vf_mask;
 
 		/* Get the interrupt sources triggered by VFs */
-		vf_mask = ((ADF_CSR_RD(pmisc_bar_addr, ADF_DH895XCC_ERRSOU5) &
+		vf_mask = ((ADF_CSR_RD(pmisc_bar_addr, ADF_ERRSOU5) &
 			    0x0000FFFF) << 16) |
-			  ((ADF_CSR_RD(pmisc_bar_addr, ADF_DH895XCC_ERRSOU3) &
+			  ((ADF_CSR_RD(pmisc_bar_addr, ADF_ERRSOU3) &
 			    0x01FFFE00) >> 9);
 
 		if (vf_mask) {
@@ -301,6 +301,12 @@ static void adf_cleanup_bh(struct adf_accel_dev *accel_dev)
 	}
 }
 
+/**
+ * adf_vf_isr_resource_free() - Free IRQ for acceleration device
+ * @accel_dev:  Pointer to acceleration device.
+ *
+ * Function frees interrupts for acceleration device.
+ */
 void adf_isr_resource_free(struct adf_accel_dev *accel_dev)
 {
 	adf_free_irqs(accel_dev);
@@ -308,7 +314,16 @@ void adf_isr_resource_free(struct adf_accel_dev *accel_dev)
 	adf_disable_msix(&accel_dev->accel_pci_dev);
 	adf_isr_free_msix_entry_table(accel_dev);
 }
-
+EXPORT_SYMBOL_GPL(adf_isr_resource_free);
+
+/**
+ * adf_vf_isr_resource_alloc() - Allocate IRQ for acceleration device
+ * @accel_dev:  Pointer to acceleration device.
+ *
+ * Function allocates interrupts for acceleration device.
+ *
+ * Return: 0 on success, error code otherwise.
+ */
 int adf_isr_resource_alloc(struct adf_accel_dev *accel_dev)
 {
 	int ret;
@@ -330,3 +345,4 @@ err_out:
 	adf_isr_resource_free(accel_dev);
 	return -EFAULT;
 }
+EXPORT_SYMBOL_GPL(adf_isr_resource_alloc);
diff --git a/drivers/crypto/qat/qat_common/adf_pf2vf_msg.c b/drivers/crypto/qat/qat_common/adf_pf2vf_msg.c
index 5fdbad809343..b3875fdf6cd7 100644
--- a/drivers/crypto/qat/qat_common/adf_pf2vf_msg.c
+++ b/drivers/crypto/qat/qat_common/adf_pf2vf_msg.c
@@ -45,8 +45,6 @@
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <linux/pci.h>
-#include <linux/mutex.h>
 #include <linux/delay.h>
 #include "adf_accel_devices.h"
 #include "adf_common_drv.h"
@@ -58,12 +56,6 @@
 #define ADF_DH895XCC_ERRMSK5	(ADF_DH895XCC_EP_OFFSET + 0xDC)
 #define ADF_DH895XCC_ERRMSK5_VF2PF_U_MASK(vf_mask) (vf_mask >> 16)
 
-/**
- * adf_enable_pf2vf_interrupts() - Enable PF to VF interrupts
- * @accel_dev:  Pointer to acceleration device.
- *
- * Function enables PF to VF interrupts
- */
 void adf_enable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
 {
 	struct adf_accel_pci *pci_info = &accel_dev->accel_pci_dev;
@@ -73,14 +65,7 @@ void adf_enable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
 
 	ADF_CSR_WR(pmisc_bar_addr, hw_data->get_vintmsk_offset(0), 0x0);
 }
-EXPORT_SYMBOL_GPL(adf_enable_pf2vf_interrupts);
 
-/**
- * adf_disable_pf2vf_interrupts() - Disable PF to VF interrupts
- * @accel_dev:  Pointer to acceleration device.
- *
- * Function disables PF to VF interrupts
- */
 void adf_disable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
 {
 	struct adf_accel_pci *pci_info = &accel_dev->accel_pci_dev;
@@ -90,7 +75,6 @@ void adf_disable_pf2vf_interrupts(struct adf_accel_dev *accel_dev)
 
 	ADF_CSR_WR(pmisc_bar_addr, hw_data->get_vintmsk_offset(0), 0x2);
 }
-EXPORT_SYMBOL_GPL(adf_disable_pf2vf_interrupts);
 
 void adf_enable_vf2pf_interrupts(struct adf_accel_dev *accel_dev,
 				 u32 vf_mask)
@@ -116,12 +100,6 @@ void adf_enable_vf2pf_interrupts(struct adf_accel_dev *accel_dev,
 	}
 }
 
-/**
- * adf_disable_pf2vf_interrupts() - Disable VF to PF interrupts
- * @accel_dev:  Pointer to acceleration device.
- *
- * Function disables VF to PF interrupts
- */
 void adf_disable_vf2pf_interrupts(struct adf_accel_dev *accel_dev, u32 vf_mask)
 {
 	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
@@ -144,7 +122,6 @@ void adf_disable_vf2pf_interrupts(struct adf_accel_dev *accel_dev, u32 vf_mask)
 		ADF_CSR_WR(pmisc_addr, ADF_DH895XCC_ERRMSK5, reg);
 	}
 }
-EXPORT_SYMBOL_GPL(adf_disable_vf2pf_interrupts);
 
 static int __adf_iov_putmsg(struct adf_accel_dev *accel_dev, u32 msg, u8 vf_nr)
 {
diff --git a/drivers/crypto/qat/qat_common/adf_transport.c b/drivers/crypto/qat/qat_common/adf_transport.c
index 3865ae8d96d9..57d2622728a5 100644
--- a/drivers/crypto/qat/qat_common/adf_transport.c
+++ b/drivers/crypto/qat/qat_common/adf_transport.c
@@ -122,7 +122,7 @@ int adf_send_message(struct adf_etr_ring_data *ring, uint32_t *msg)
 		return -EAGAIN;
 	}
 	spin_lock_bh(&ring->lock);
-	memcpy(ring->base_addr + ring->tail, msg,
+	memcpy((void *)((uintptr_t)ring->base_addr + ring->tail), msg,
 	       ADF_MSG_SIZE_TO_BYTES(ring->msg_size));
 
 	ring->tail = adf_modulo(ring->tail +
@@ -137,23 +137,22 @@ int adf_send_message(struct adf_etr_ring_data *ring, uint32_t *msg)
 static int adf_handle_response(struct adf_etr_ring_data *ring)
 {
 	uint32_t msg_counter = 0;
-	uint32_t *msg = (uint32_t *)(ring->base_addr + ring->head);
+	uint32_t *msg = (uint32_t *)((uintptr_t)ring->base_addr + ring->head);
 
 	while (*msg != ADF_RING_EMPTY_SIG) {
 		ring->callback((uint32_t *)msg);
+		atomic_dec(ring->inflights);
 		*msg = ADF_RING_EMPTY_SIG;
 		ring->head = adf_modulo(ring->head +
 					ADF_MSG_SIZE_TO_BYTES(ring->msg_size),
 					ADF_RING_SIZE_MODULO(ring->ring_size));
 		msg_counter++;
-		msg = (uint32_t *)(ring->base_addr + ring->head);
+		msg = (uint32_t *)((uintptr_t)ring->base_addr + ring->head);
 	}
-	if (msg_counter > 0) {
+	if (msg_counter > 0)
 		WRITE_CSR_RING_HEAD(ring->bank->csr_addr,
 				    ring->bank->bank_number,
 				    ring->ring_number, ring->head);
-		atomic_sub(msg_counter, ring->inflights);
-	}
 	return 0;
 }
 
@@ -342,27 +341,15 @@ static void adf_ring_response_handler(struct adf_etr_bank_data *bank)
 	}
 }
 
-/**
- * adf_response_handler() - Bottom half handler response handler
- * @bank_addr:  Address of a ring bank for with the BH was scheduled.
- *
- * Function is the bottom half handler for the response from acceleration
- * device. There is one handler for every ring bank. Function checks all
- * communication rings in the bank.
- * To be used by QAT device specific drivers.
- *
- * Return: void
- */
-void adf_response_handler(unsigned long bank_addr)
+void adf_response_handler(uintptr_t bank_addr)
 {
 	struct adf_etr_bank_data *bank = (void *)bank_addr;
 
-	/* Handle all the responses nad reenable IRQs */
+	/* Handle all the responses and reenable IRQs */
 	adf_ring_response_handler(bank);
 	WRITE_CSR_INT_FLAG_AND_COL(bank->csr_addr, bank->bank_number,
 				   bank->irq_mask);
 }
-EXPORT_SYMBOL_GPL(adf_response_handler);
 
 static inline int adf_get_cfg_int(struct adf_accel_dev *accel_dev,
 				  const char *section, const char *format,
@@ -447,6 +434,7 @@ static int adf_init_bank(struct adf_accel_dev *accel_dev,
 		goto err;
 	}
 
+	WRITE_CSR_INT_FLAG(csr_addr, bank_num, ADF_BANK_INT_FLAG_CLEAR_MASK);
 	WRITE_CSR_INT_SRCSEL(csr_addr, bank_num);
 	return 0;
 err:
diff --git a/drivers/crypto/qat/qat_common/adf_transport_access_macros.h b/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
index 6ad7e4e1edca..80e02a2a0a09 100644
--- a/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
+++ b/drivers/crypto/qat/qat_common/adf_transport_access_macros.h
@@ -50,12 +50,14 @@
 #include "adf_accel_devices.h"
 #define ADF_BANK_INT_SRC_SEL_MASK_0 0x4444444CUL
 #define ADF_BANK_INT_SRC_SEL_MASK_X 0x44444444UL
+#define ADF_BANK_INT_FLAG_CLEAR_MASK 0xFFFF
 #define ADF_RING_CSR_RING_CONFIG 0x000
 #define ADF_RING_CSR_RING_LBASE 0x040
 #define ADF_RING_CSR_RING_UBASE 0x080
 #define ADF_RING_CSR_RING_HEAD 0x0C0
 #define ADF_RING_CSR_RING_TAIL 0x100
 #define ADF_RING_CSR_E_STAT 0x14C
+#define ADF_RING_CSR_INT_FLAG	0x170
 #define ADF_RING_CSR_INT_SRCSEL 0x174
 #define ADF_RING_CSR_INT_SRCSEL_2 0x178
 #define ADF_RING_CSR_INT_COL_EN 0x17C
@@ -144,6 +146,9 @@ do { \
 #define WRITE_CSR_RING_TAIL(csr_base_addr, bank, ring, value) \
 	ADF_CSR_WR(csr_base_addr, (ADF_RING_BUNDLE_SIZE * bank) + \
 		ADF_RING_CSR_RING_TAIL + (ring << 2), value)
+#define WRITE_CSR_INT_FLAG(csr_base_addr, bank, value) \
+	ADF_CSR_WR(csr_base_addr, (ADF_RING_BUNDLE_SIZE * (bank)) + \
+			ADF_RING_CSR_INT_FLAG, value)
 #define WRITE_CSR_INT_SRCSEL(csr_base_addr, bank) \
 do { \
 	ADF_CSR_WR(csr_base_addr, (ADF_RING_BUNDLE_SIZE * bank) + \
diff --git a/drivers/crypto/qat/qat_common/adf_transport_internal.h b/drivers/crypto/qat/qat_common/adf_transport_internal.h
index a4869627fd57..bb883368ac01 100644
--- a/drivers/crypto/qat/qat_common/adf_transport_internal.h
+++ b/drivers/crypto/qat/qat_common/adf_transport_internal.h
@@ -91,7 +91,7 @@ struct adf_etr_data {
 	struct dentry *debug;
 };
 
-void adf_response_handler(unsigned long bank_addr);
+void adf_response_handler(uintptr_t bank_addr);
 #ifdef CONFIG_DEBUG_FS
 #include <linux/debugfs.h>
 int adf_bank_debugfs_add(struct adf_etr_bank_data *bank);
diff --git a/drivers/crypto/qat/qat_dh895xccvf/adf_isr.c b/drivers/crypto/qat/qat_common/adf_vf_isr.c
index 87c5d8adb125..09427b3d4d55 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/adf_isr.c
+++ b/drivers/crypto/qat/qat_common/adf_vf_isr.c
@@ -51,16 +51,18 @@
 #include <linux/slab.h>
 #include <linux/errno.h>
 #include <linux/interrupt.h>
-#include <adf_accel_devices.h>
-#include <adf_common_drv.h>
-#include <adf_cfg.h>
-#include <adf_cfg_strings.h>
-#include <adf_cfg_common.h>
-#include <adf_transport_access_macros.h>
-#include <adf_transport_internal.h>
-#include <adf_pf2vf_msg.h>
-#include "adf_drv.h"
-#include "adf_dh895xccvf_hw_data.h"
+#include "adf_accel_devices.h"
+#include "adf_common_drv.h"
+#include "adf_cfg.h"
+#include "adf_cfg_strings.h"
+#include "adf_cfg_common.h"
+#include "adf_transport_access_macros.h"
+#include "adf_transport_internal.h"
+#include "adf_pf2vf_msg.h"
+
+#define ADF_VINTSOU_OFFSET	0x204
+#define ADF_VINTSOU_BUN		BIT(0)
+#define ADF_VINTSOU_PF2VF	BIT(1)
 
 static int adf_enable_msi(struct adf_accel_dev *accel_dev)
 {
@@ -91,12 +93,14 @@ static void adf_disable_msi(struct adf_accel_dev *accel_dev)
 static void adf_pf2vf_bh_handler(void *data)
 {
 	struct adf_accel_dev *accel_dev = data;
-	void __iomem *pmisc_bar_addr =
-		(&GET_BARS(accel_dev)[ADF_DH895XCCIOV_PMISC_BAR])->virt_addr;
+	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
+	struct adf_bar *pmisc =
+			&GET_BARS(accel_dev)[hw_data->get_misc_bar_id(hw_data)];
+	void __iomem *pmisc_bar_addr = pmisc->virt_addr;
 	u32 msg;
 
 	/* Read the message from PF */
-	msg = ADF_CSR_RD(pmisc_bar_addr, ADF_DH895XCCIOV_PF2VF_OFFSET);
+	msg = ADF_CSR_RD(pmisc_bar_addr, hw_data->get_pf2vf_offset(0));
 
 	if (!(msg & ADF_PF2VF_MSGORIGIN_SYSTEM))
 		/* Ignore legacy non-system (non-kernel) PF2VF messages */
@@ -124,8 +128,8 @@ static void adf_pf2vf_bh_handler(void *data)
 	}
 
 	/* To ack, clear the PF2VFINT bit */
-	msg &= ~ADF_DH895XCC_PF2VF_PF2VFINT;
-	ADF_CSR_WR(pmisc_bar_addr, ADF_DH895XCCIOV_PF2VF_OFFSET, msg);
+	msg &= ~BIT(0);
+	ADF_CSR_WR(pmisc_bar_addr, hw_data->get_pf2vf_offset(0), msg);
 
 	/* Re-enable PF2VF interrupts */
 	adf_enable_pf2vf_interrupts(accel_dev);
@@ -155,15 +159,17 @@ static void adf_cleanup_pf2vf_bh(struct adf_accel_dev *accel_dev)
 static irqreturn_t adf_isr(int irq, void *privdata)
 {
 	struct adf_accel_dev *accel_dev = privdata;
-	void __iomem *pmisc_bar_addr =
-		(&GET_BARS(accel_dev)[ADF_DH895XCCIOV_PMISC_BAR])->virt_addr;
+	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
+	struct adf_bar *pmisc =
+			&GET_BARS(accel_dev)[hw_data->get_misc_bar_id(hw_data)];
+	void __iomem *pmisc_bar_addr = pmisc->virt_addr;
 	u32 v_int;
 
 	/* Read VF INT source CSR to determine the source of VF interrupt */
-	v_int = ADF_CSR_RD(pmisc_bar_addr, ADF_DH895XCCIOV_VINTSOU_OFFSET);
+	v_int = ADF_CSR_RD(pmisc_bar_addr, ADF_VINTSOU_OFFSET);
 
 	/* Check for PF2VF interrupt */
-	if (v_int & ADF_DH895XCC_VINTSOU_PF2VF) {
+	if (v_int & ADF_VINTSOU_PF2VF) {
 		/* Disable PF to VF interrupt */
 		adf_disable_pf2vf_interrupts(accel_dev);
 
@@ -173,7 +179,7 @@ static irqreturn_t adf_isr(int irq, void *privdata)
 	}
 
 	/* Check bundle interrupt */
-	if (v_int & ADF_DH895XCC_VINTSOU_BUN) {
+	if (v_int & ADF_VINTSOU_BUN) {
 		struct adf_etr_data *etr_data = accel_dev->transport;
 		struct adf_etr_bank_data *bank = &etr_data->banks[0];
 
@@ -226,6 +232,12 @@ static void adf_cleanup_bh(struct adf_accel_dev *accel_dev)
 	tasklet_kill(&priv_data->banks[0].resp_handler);
 }
 
+/**
+ * adf_vf_isr_resource_free() - Free IRQ for acceleration device
+ * @accel_dev:  Pointer to acceleration device.
+ *
+ * Function frees interrupts for acceleration device virtual function.
+ */
 void adf_vf_isr_resource_free(struct adf_accel_dev *accel_dev)
 {
 	struct pci_dev *pdev = accel_to_pci_dev(accel_dev);
@@ -236,7 +248,16 @@ void adf_vf_isr_resource_free(struct adf_accel_dev *accel_dev)
 	adf_cleanup_pf2vf_bh(accel_dev);
 	adf_disable_msi(accel_dev);
 }
-
+EXPORT_SYMBOL_GPL(adf_vf_isr_resource_free);
+
+/**
+ * adf_vf_isr_resource_alloc() - Allocate IRQ for acceleration device
+ * @accel_dev:  Pointer to acceleration device.
+ *
+ * Function allocates interrupts for acceleration device virtual function.
+ *
+ * Return: 0 on success, error code otherwise.
+ */
 int adf_vf_isr_resource_alloc(struct adf_accel_dev *accel_dev)
 {
 	if (adf_enable_msi(accel_dev))
@@ -256,3 +277,4 @@ err_out:
 	adf_vf_isr_resource_free(accel_dev);
 	return -EFAULT;
 }
+EXPORT_SYMBOL_GPL(adf_vf_isr_resource_alloc);
diff --git a/drivers/crypto/qat/qat_common/icp_qat_fw_loader_handle.h b/drivers/crypto/qat/qat_common/icp_qat_fw_loader_handle.h
index 5e1aa40c0404..2ffef3e4fd68 100644
--- a/drivers/crypto/qat/qat_common/icp_qat_fw_loader_handle.h
+++ b/drivers/crypto/qat/qat_common/icp_qat_fw_loader_handle.h
@@ -68,11 +68,21 @@ struct icp_qat_fw_loader_hal_handle {
 
 struct icp_qat_fw_loader_handle {
 	struct icp_qat_fw_loader_hal_handle *hal_handle;
+	struct pci_dev *pci_dev;
 	void *obj_handle;
+	void *sobj_handle;
+	bool fw_auth;
 	void __iomem *hal_sram_addr_v;
 	void __iomem *hal_cap_g_ctl_csr_addr_v;
 	void __iomem *hal_cap_ae_xfer_csr_addr_v;
 	void __iomem *hal_cap_ae_local_csr_addr_v;
 	void __iomem *hal_ep_csr_addr_v;
 };
+
+struct icp_firml_dram_desc {
+	void __iomem *dram_base_addr;
+	void *dram_base_addr_v;
+	dma_addr_t dram_bus_addr;
+	u64 dram_size;
+};
 #endif
diff --git a/drivers/crypto/qat/qat_common/icp_qat_hal.h b/drivers/crypto/qat/qat_common/icp_qat_hal.h
index 85b6d241ea82..7187917533d0 100644
--- a/drivers/crypto/qat/qat_common/icp_qat_hal.h
+++ b/drivers/crypto/qat/qat_common/icp_qat_hal.h
@@ -81,6 +81,31 @@ enum hal_ae_csr {
 	LOCAL_CSR_STATUS = 0x180,
 };
 
+enum fcu_csr {
+	FCU_CONTROL           = 0x8c0,
+	FCU_STATUS            = 0x8c4,
+	FCU_STATUS1           = 0x8c8,
+	FCU_DRAM_ADDR_LO      = 0x8cc,
+	FCU_DRAM_ADDR_HI      = 0x8d0,
+	FCU_RAMBASE_ADDR_HI   = 0x8d4,
+	FCU_RAMBASE_ADDR_LO   = 0x8d8
+};
+
+enum fcu_cmd {
+	FCU_CTRL_CMD_NOOP  = 0,
+	FCU_CTRL_CMD_AUTH  = 1,
+	FCU_CTRL_CMD_LOAD  = 2,
+	FCU_CTRL_CMD_START = 3
+};
+
+enum fcu_sts {
+	FCU_STS_NO_STS    = 0,
+	FCU_STS_VERI_DONE = 1,
+	FCU_STS_LOAD_DONE = 2,
+	FCU_STS_VERI_FAIL = 3,
+	FCU_STS_LOAD_FAIL = 4,
+	FCU_STS_BUSY      = 5
+};
 #define UA_ECS                      (0x1 << 31)
 #define ACS_ABO_BITPOS              31
 #define ACS_ACNO                    0x7
@@ -98,6 +123,13 @@ enum hal_ae_csr {
 #define LCS_STATUS          (0x1)
 #define MMC_SHARE_CS_BITPOS         2
 #define GLOBAL_CSR                0xA00
+#define FCU_CTRL_AE_POS     0x8
+#define FCU_AUTH_STS_MASK   0x7
+#define FCU_STS_DONE_POS    0x9
+#define FCU_STS_AUTHFWLD_POS 0X8
+#define FCU_LOADED_AE_POS   0x16
+#define FW_AUTH_WAIT_PERIOD 10
+#define FW_AUTH_MAX_RETRY   300
 
 #define SET_CAP_CSR(handle, csr, val) \
 	ADF_CSR_WR(handle->hal_cap_g_ctl_csr_addr_v, csr, val)
@@ -106,14 +138,14 @@ enum hal_ae_csr {
 #define SET_GLB_CSR(handle, csr, val) SET_CAP_CSR(handle, csr + GLOBAL_CSR, val)
 #define GET_GLB_CSR(handle, csr) GET_CAP_CSR(handle, GLOBAL_CSR + csr)
 #define AE_CSR(handle, ae) \
-	(handle->hal_cap_ae_local_csr_addr_v + \
+	((char __iomem *)handle->hal_cap_ae_local_csr_addr_v + \
 	((ae & handle->hal_handle->ae_mask) << 12))
 #define AE_CSR_ADDR(handle, ae, csr) (AE_CSR(handle, ae) + (0x3ff & csr))
 #define SET_AE_CSR(handle, ae, csr, val) \
 	ADF_CSR_WR(AE_CSR_ADDR(handle, ae, csr), 0, val)
 #define GET_AE_CSR(handle, ae, csr) ADF_CSR_RD(AE_CSR_ADDR(handle, ae, csr), 0)
 #define AE_XFER(handle, ae) \
-	(handle->hal_cap_ae_xfer_csr_addr_v + \
+	((char __iomem *)handle->hal_cap_ae_xfer_csr_addr_v + \
 	((ae & handle->hal_handle->ae_mask) << 12))
 #define AE_XFER_ADDR(handle, ae, reg) (AE_XFER(handle, ae) + \
 	((reg & 0xff) << 2))
@@ -121,5 +153,4 @@ enum hal_ae_csr {
 	ADF_CSR_WR(AE_XFER_ADDR(handle, ae, reg), 0, val)
 #define SRAM_WRITE(handle, addr, val) \
 	ADF_CSR_WR(handle->hal_sram_addr_v, addr, val)
-#define SRAM_READ(handle, addr) ADF_CSR_RD(handle->hal_sram_addr_v, addr)
 #endif
diff --git a/drivers/crypto/qat/qat_common/icp_qat_uclo.h b/drivers/crypto/qat/qat_common/icp_qat_uclo.h
index 2132a8cbc4ec..d97db990955d 100644
--- a/drivers/crypto/qat/qat_common/icp_qat_uclo.h
+++ b/drivers/crypto/qat/qat_common/icp_qat_uclo.h
@@ -47,32 +47,55 @@
 #ifndef __ICP_QAT_UCLO_H__
 #define __ICP_QAT_UCLO_H__
 
-#define ICP_QAT_AC_C_CPU_TYPE     0x00400000
+#define ICP_QAT_AC_895XCC_DEV_TYPE 0x00400000
+#define ICP_QAT_AC_C62X_DEV_TYPE   0x01000000
+#define ICP_QAT_AC_C3XXX_DEV_TYPE  0x02000000
 #define ICP_QAT_UCLO_MAX_AE       12
 #define ICP_QAT_UCLO_MAX_CTX      8
 #define ICP_QAT_UCLO_MAX_UIMAGE   (ICP_QAT_UCLO_MAX_AE * ICP_QAT_UCLO_MAX_CTX)
 #define ICP_QAT_UCLO_MAX_USTORE   0x4000
 #define ICP_QAT_UCLO_MAX_XFER_REG 128
 #define ICP_QAT_UCLO_MAX_GPR_REG  128
-#define ICP_QAT_UCLO_MAX_NN_REG   128
 #define ICP_QAT_UCLO_MAX_LMEM_REG 1024
 #define ICP_QAT_UCLO_AE_ALL_CTX   0xff
 #define ICP_QAT_UOF_OBJID_LEN     8
 #define ICP_QAT_UOF_FID 0xc6c2
 #define ICP_QAT_UOF_MAJVER 0x4
 #define ICP_QAT_UOF_MINVER 0x11
-#define ICP_QAT_UOF_NN_MODE_NOTCARE   0xff
 #define ICP_QAT_UOF_OBJS        "UOF_OBJS"
 #define ICP_QAT_UOF_STRT        "UOF_STRT"
-#define ICP_QAT_UOF_GTID        "UOF_GTID"
 #define ICP_QAT_UOF_IMAG        "UOF_IMAG"
 #define ICP_QAT_UOF_IMEM        "UOF_IMEM"
-#define ICP_QAT_UOF_MSEG        "UOF_MSEG"
 #define ICP_QAT_UOF_LOCAL_SCOPE     1
 #define ICP_QAT_UOF_INIT_EXPR               0
 #define ICP_QAT_UOF_INIT_REG                1
 #define ICP_QAT_UOF_INIT_REG_CTX            2
 #define ICP_QAT_UOF_INIT_EXPR_ENDIAN_SWAP   3
+#define ICP_QAT_SUOF_OBJ_ID_LEN             8
+#define ICP_QAT_SUOF_FID  0x53554f46
+#define ICP_QAT_SUOF_MAJVER 0x0
+#define ICP_QAT_SUOF_MINVER 0x1
+#define ICP_QAT_SIMG_AE_INIT_SEQ_LEN    (50 * sizeof(unsigned long long))
+#define ICP_QAT_SIMG_AE_INSTS_LEN       (0x4000 * sizeof(unsigned long long))
+#define ICP_QAT_CSS_FWSK_MODULUS_LEN    256
+#define ICP_QAT_CSS_FWSK_EXPONENT_LEN   4
+#define ICP_QAT_CSS_FWSK_PAD_LEN        252
+#define ICP_QAT_CSS_FWSK_PUB_LEN   (ICP_QAT_CSS_FWSK_MODULUS_LEN + \
+				    ICP_QAT_CSS_FWSK_EXPONENT_LEN + \
+				    ICP_QAT_CSS_FWSK_PAD_LEN)
+#define ICP_QAT_CSS_SIGNATURE_LEN   256
+#define ICP_QAT_CSS_AE_IMG_LEN     (sizeof(struct icp_qat_simg_ae_mode) + \
+				    ICP_QAT_SIMG_AE_INIT_SEQ_LEN +         \
+				    ICP_QAT_SIMG_AE_INSTS_LEN)
+#define ICP_QAT_CSS_AE_SIMG_LEN    (sizeof(struct icp_qat_css_hdr) + \
+				    ICP_QAT_CSS_FWSK_PUB_LEN + \
+				    ICP_QAT_CSS_SIGNATURE_LEN + \
+				    ICP_QAT_CSS_AE_IMG_LEN)
+#define ICP_QAT_AE_IMG_OFFSET	   (sizeof(struct icp_qat_css_hdr) + \
+				    ICP_QAT_CSS_FWSK_MODULUS_LEN + \
+				    ICP_QAT_CSS_FWSK_EXPONENT_LEN + \
+				    ICP_QAT_CSS_SIGNATURE_LEN)
+#define ICP_QAT_CSS_MAX_IMAGE_LEN   0x40000
 
 #define ICP_QAT_CTX_MODE(ae_mode) ((ae_mode) & 0xf)
 #define ICP_QAT_NN_MODE(ae_mode) (((ae_mode) >> 0x4) & 0xf)
@@ -112,6 +135,11 @@ enum icp_qat_uof_regtype {
 	ICP_NEIGH_REL,
 };
 
+enum icp_qat_css_fwtype {
+	CSS_AE_FIRMWARE = 0,
+	CSS_MMP_FIRMWARE = 1
+};
+
 struct icp_qat_uclo_page {
 	struct icp_qat_uclo_encap_page *encap_page;
 	struct icp_qat_uclo_region *region;
@@ -235,7 +263,7 @@ struct icp_qat_uof_filechunkhdr {
 };
 
 struct icp_qat_uof_objhdr {
-	unsigned int cpu_type;
+	unsigned int ac_dev_type;
 	unsigned short min_cpu_ver;
 	unsigned short max_cpu_ver;
 	short max_chunks;
@@ -326,7 +354,7 @@ struct icp_qat_uof_image {
 	unsigned int img_name;
 	unsigned int ae_assigned;
 	unsigned int ctx_assigned;
-	unsigned int cpu_type;
+	unsigned int ac_dev_type;
 	unsigned int entry_address;
 	unsigned int fill_pattern[2];
 	unsigned int reloadable_size;
@@ -374,4 +402,127 @@ struct icp_qat_uof_batch_init {
 	unsigned int size;
 	struct icp_qat_uof_batch_init *next;
 };
+
+struct icp_qat_suof_img_hdr {
+	char          *simg_buf;
+	unsigned long simg_len;
+	char          *css_header;
+	char          *css_key;
+	char          *css_signature;
+	char          *css_simg;
+	unsigned long simg_size;
+	unsigned int  ae_num;
+	unsigned int  ae_mask;
+	unsigned int  fw_type;
+	unsigned long simg_name;
+	unsigned long appmeta_data;
+};
+
+struct icp_qat_suof_img_tbl {
+	unsigned int num_simgs;
+	struct icp_qat_suof_img_hdr *simg_hdr;
+};
+
+struct icp_qat_suof_handle {
+	unsigned int  file_id;
+	unsigned int  check_sum;
+	char          min_ver;
+	char          maj_ver;
+	char          fw_type;
+	char          *suof_buf;
+	unsigned int  suof_size;
+	char          *sym_str;
+	unsigned int  sym_size;
+	struct icp_qat_suof_img_tbl img_table;
+};
+
+struct icp_qat_fw_auth_desc {
+	unsigned int   img_len;
+	unsigned int   reserved;
+	unsigned int   css_hdr_high;
+	unsigned int   css_hdr_low;
+	unsigned int   img_high;
+	unsigned int   img_low;
+	unsigned int   signature_high;
+	unsigned int   signature_low;
+	unsigned int   fwsk_pub_high;
+	unsigned int   fwsk_pub_low;
+	unsigned int   img_ae_mode_data_high;
+	unsigned int   img_ae_mode_data_low;
+	unsigned int   img_ae_init_data_high;
+	unsigned int   img_ae_init_data_low;
+	unsigned int   img_ae_insts_high;
+	unsigned int   img_ae_insts_low;
+};
+
+struct icp_qat_auth_chunk {
+	struct icp_qat_fw_auth_desc fw_auth_desc;
+	u64 chunk_size;
+	u64 chunk_bus_addr;
+};
+
+struct icp_qat_css_hdr {
+	unsigned int module_type;
+	unsigned int header_len;
+	unsigned int header_ver;
+	unsigned int module_id;
+	unsigned int module_vendor;
+	unsigned int date;
+	unsigned int size;
+	unsigned int key_size;
+	unsigned int module_size;
+	unsigned int exponent_size;
+	unsigned int fw_type;
+	unsigned int reserved[21];
+};
+
+struct icp_qat_simg_ae_mode {
+	unsigned int     file_id;
+	unsigned short   maj_ver;
+	unsigned short   min_ver;
+	unsigned int     dev_type;
+	unsigned short   devmax_ver;
+	unsigned short   devmin_ver;
+	unsigned int     ae_mask;
+	unsigned int     ctx_enables;
+	char             fw_type;
+	char             ctx_mode;
+	char             nn_mode;
+	char             lm0_mode;
+	char             lm1_mode;
+	char             scs_mode;
+	char             lm2_mode;
+	char             lm3_mode;
+	char             tindex_mode;
+	unsigned char    reserved[7];
+	char             simg_name[256];
+	char             appmeta_data[256];
+};
+
+struct icp_qat_suof_filehdr {
+	unsigned int     file_id;
+	unsigned int     check_sum;
+	char             min_ver;
+	char             maj_ver;
+	char             fw_type;
+	char             reserved;
+	unsigned short   max_chunks;
+	unsigned short   num_chunks;
+};
+
+struct icp_qat_suof_chunk_hdr {
+	char chunk_id[ICP_QAT_SUOF_OBJ_ID_LEN];
+	u64 offset;
+	u64 size;
+};
+
+struct icp_qat_suof_strtable {
+	unsigned int tab_length;
+	unsigned int strings;
+};
+
+struct icp_qat_suof_objhdr {
+	unsigned int img_length;
+	unsigned int reserved;
+};
 #endif
diff --git a/drivers/crypto/qat/qat_common/qat_crypto.c b/drivers/crypto/qat/qat_common/qat_crypto.c
index 9cab15497f04..3852d31ce0a4 100644
--- a/drivers/crypto/qat/qat_common/qat_crypto.c
+++ b/drivers/crypto/qat/qat_common/qat_crypto.c
@@ -49,6 +49,7 @@
 #include "adf_accel_devices.h"
 #include "adf_common_drv.h"
 #include "adf_transport.h"
+#include "adf_transport_access_macros.h"
 #include "adf_cfg.h"
 #include "adf_cfg_strings.h"
 #include "qat_crypto.h"
@@ -66,13 +67,10 @@ void qat_crypto_put_instance(struct qat_crypto_instance *inst)
 
 static int qat_crypto_free_instances(struct adf_accel_dev *accel_dev)
 {
-	struct qat_crypto_instance *inst;
-	struct list_head *list_ptr, *tmp;
+	struct qat_crypto_instance *inst, *tmp;
 	int i;
 
-	list_for_each_safe(list_ptr, tmp, &accel_dev->crypto_list) {
-		inst = list_entry(list_ptr, struct qat_crypto_instance, list);
-
+	list_for_each_entry_safe(inst, tmp, &accel_dev->crypto_list, list) {
 		for (i = 0; i < atomic_read(&inst->refctr); i++)
 			qat_crypto_put_instance(inst);
 
@@ -88,7 +86,7 @@ static int qat_crypto_free_instances(struct adf_accel_dev *accel_dev)
 		if (inst->pke_rx)
 			adf_remove_ring(inst->pke_rx);
 
-		list_del(list_ptr);
+		list_del(&inst->list);
 		kfree(inst);
 	}
 	return 0;
@@ -96,17 +94,13 @@ static int qat_crypto_free_instances(struct adf_accel_dev *accel_dev)
 
 struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 {
-	struct adf_accel_dev *accel_dev = NULL;
-	struct qat_crypto_instance *inst = NULL;
-	struct list_head *itr;
+	struct adf_accel_dev *accel_dev = NULL, *tmp_dev;
+	struct qat_crypto_instance *inst = NULL, *tmp_inst;
 	unsigned long best = ~0;
 
-	list_for_each(itr, adf_devmgr_get_head()) {
-		struct adf_accel_dev *tmp_dev;
+	list_for_each_entry(tmp_dev, adf_devmgr_get_head(), list) {
 		unsigned long ctr;
 
-		tmp_dev = list_entry(itr, struct adf_accel_dev, list);
-
 		if ((node == dev_to_node(&GET_DEV(tmp_dev)) ||
 		     dev_to_node(&GET_DEV(tmp_dev)) < 0) &&
 		    adf_dev_started(tmp_dev) &&
@@ -118,19 +112,16 @@ struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 			}
 		}
 	}
-	if (!accel_dev)
-		pr_info("QAT: Could not find a device on node %d\n", node);
-
-	/* Get any started device */
-	list_for_each(itr, adf_devmgr_get_head()) {
-		struct adf_accel_dev *tmp_dev;
 
-		tmp_dev = list_entry(itr, struct adf_accel_dev, list);
-
-		if (adf_dev_started(tmp_dev) &&
-		    !list_empty(&tmp_dev->crypto_list)) {
-			accel_dev = tmp_dev;
-			break;
+	if (!accel_dev) {
+		pr_info("QAT: Could not find a device on node %d\n", node);
+		/* Get any started device */
+		list_for_each_entry(tmp_dev, adf_devmgr_get_head(), list) {
+			if (adf_dev_started(tmp_dev) &&
+			    !list_empty(&tmp_dev->crypto_list)) {
+				accel_dev = tmp_dev;
+				break;
+			}
 		}
 	}
 
@@ -138,11 +129,9 @@ struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 		return NULL;
 
 	best = ~0;
-	list_for_each(itr, &accel_dev->crypto_list) {
-		struct qat_crypto_instance *tmp_inst;
+	list_for_each_entry(tmp_inst, &accel_dev->crypto_list, list) {
 		unsigned long ctr;
 
-		tmp_inst = list_entry(itr, struct qat_crypto_instance, list);
 		ctr = atomic_read(&tmp_inst->refctr);
 		if (best > ctr) {
 			inst = tmp_inst;
@@ -159,6 +148,97 @@ struct qat_crypto_instance *qat_crypto_get_instance_node(int node)
 	return inst;
 }
 
+/**
+ * qat_crypto_dev_config() - create dev config required to create crypto inst.
+ *
+ * @accel_dev: Pointer to acceleration device.
+ *
+ * Function creates device configuration required to create crypto instances
+ *
+ * Return: 0 on success, error code otherwise.
+ */
+int qat_crypto_dev_config(struct adf_accel_dev *accel_dev)
+{
+	int cpus = num_online_cpus();
+	int banks = GET_MAX_BANKS(accel_dev);
+	int instances = min(cpus, banks);
+	char key[ADF_CFG_MAX_KEY_LEN_IN_BYTES];
+	int i;
+	unsigned long val;
+
+	if (adf_cfg_section_add(accel_dev, ADF_KERNEL_SEC))
+		goto err;
+	if (adf_cfg_section_add(accel_dev, "Accelerator0"))
+		goto err;
+	for (i = 0; i < instances; i++) {
+		val = i;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_BANK_NUM, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_ETRMGR_CORE_AFFINITY,
+			 i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_SIZE, i);
+		val = 128;
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = 512;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_SIZE, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = 0;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_TX, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = 2;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_TX, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = 8;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_RX, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = 10;
+		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_RX, i);
+		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+						key, (void *)&val, ADF_DEC))
+			goto err;
+
+		val = ADF_COALESCING_DEF_TIME;
+		snprintf(key, sizeof(key), ADF_ETRMGR_COALESCE_TIMER_FORMAT, i);
+		if (adf_cfg_add_key_value_param(accel_dev, "Accelerator0",
+						key, (void *)&val, ADF_DEC))
+			goto err;
+	}
+
+	val = i;
+	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
+					ADF_NUM_CY, (void *)&val, ADF_DEC))
+		goto err;
+
+	set_bit(ADF_STATUS_CONFIGURED, &accel_dev->status);
+	return 0;
+err:
+	dev_err(&GET_DEV(accel_dev), "Failed to start QAT accel dev\n");
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(qat_crypto_dev_config);
+
 static int qat_crypto_create_instances(struct adf_accel_dev *accel_dev)
 {
 	int i;
diff --git a/drivers/crypto/qat/qat_common/qat_hal.c b/drivers/crypto/qat/qat_common/qat_hal.c
index 380e761801a7..1e480f140663 100644
--- a/drivers/crypto/qat/qat_common/qat_hal.c
+++ b/drivers/crypto/qat/qat_common/qat_hal.c
@@ -45,21 +45,22 @@
   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #include <linux/slab.h>
+#include <linux/delay.h>
 
 #include "adf_accel_devices.h"
 #include "adf_common_drv.h"
 #include "icp_qat_hal.h"
 #include "icp_qat_uclo.h"
 
-#define BAD_REGADDR               0xffff
-#define MAX_RETRY_TIMES           10000
-#define INIT_CTX_ARB_VALUE        0x0
+#define BAD_REGADDR	       0xffff
+#define MAX_RETRY_TIMES	   10000
+#define INIT_CTX_ARB_VALUE	0x0
 #define INIT_CTX_ENABLE_VALUE     0x0
-#define INIT_PC_VALUE             0x0
+#define INIT_PC_VALUE	     0x0
 #define INIT_WAKEUP_EVENTS_VALUE  0x1
 #define INIT_SIG_EVENTS_VALUE     0x1
 #define INIT_CCENABLE_VALUE       0x2000
-#define RST_CSR_QAT_LSB           20
+#define RST_CSR_QAT_LSB	   20
 #define RST_CSR_AE_LSB		  0
 #define MC_TIMESTAMP_ENABLE       (0x1 << 7)
 
@@ -185,7 +186,7 @@ static int qat_hal_wait_cycles(struct icp_qat_fw_loader_handle *handle,
 		if (elapsed_cycles >= 8 && !(csr & (1 << ACS_ABO_BITPOS)))
 			return 0;
 	}
-	if (!times) {
+	if (times < 0) {
 		pr_err("QAT: wait_num_cycles time out\n");
 		return -EFAULT;
 	}
@@ -388,12 +389,9 @@ static int qat_hal_check_ae_alive(struct icp_qat_fw_loader_handle *handle)
 {
 	unsigned int base_cnt, cur_cnt;
 	unsigned char ae;
-	unsigned int times = MAX_RETRY_TIMES;
+	int times = MAX_RETRY_TIMES;
 
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
-		if (!(handle->hal_handle->ae_mask & (1 << ae)))
-			continue;
-
 		qat_hal_rd_ae_csr(handle, ae, PROFILE_COUNT,
 				  (unsigned int *)&base_cnt);
 		base_cnt &= 0xffff;
@@ -404,7 +402,7 @@ static int qat_hal_check_ae_alive(struct icp_qat_fw_loader_handle *handle)
 			cur_cnt &= 0xffff;
 		} while (times-- && (cur_cnt == base_cnt));
 
-		if (!times) {
+		if (times < 0) {
 			pr_err("QAT: AE%d is inactive!!\n", ae);
 			return -EFAULT;
 		}
@@ -413,6 +411,20 @@ static int qat_hal_check_ae_alive(struct icp_qat_fw_loader_handle *handle)
 	return 0;
 }
 
+int qat_hal_check_ae_active(struct icp_qat_fw_loader_handle *handle,
+			    unsigned int ae)
+{
+	unsigned int enable = 0, active = 0;
+
+	qat_hal_rd_ae_csr(handle, ae, CTX_ENABLES, &enable);
+	qat_hal_rd_ae_csr(handle, ae, ACTIVE_CTX_STATUS, &active);
+	if ((enable & (0xff << CE_ENABLE_BITPOS)) ||
+	    (active & (1 << ACS_ABO_BITPOS)))
+		return 1;
+	else
+		return 0;
+}
+
 static void qat_hal_reset_timestamp(struct icp_qat_fw_loader_handle *handle)
 {
 	unsigned int misc_ctl;
@@ -425,8 +437,6 @@ static void qat_hal_reset_timestamp(struct icp_qat_fw_loader_handle *handle)
 			    (~MC_TIMESTAMP_ENABLE));
 
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
-		if (!(handle->hal_handle->ae_mask & (1 << ae)))
-			continue;
 		qat_hal_wr_ae_csr(handle, ae, TIMESTAMP_LOW, 0);
 		qat_hal_wr_ae_csr(handle, ae, TIMESTAMP_HIGH, 0);
 	}
@@ -440,9 +450,14 @@ static void qat_hal_reset_timestamp(struct icp_qat_fw_loader_handle *handle)
 #define ESRAM_AUTO_INIT_CSR_OFFSET 0xC1C
 static int qat_hal_init_esram(struct icp_qat_fw_loader_handle *handle)
 {
-	void __iomem *csr_addr = handle->hal_ep_csr_addr_v +
-				 ESRAM_AUTO_INIT_CSR_OFFSET;
-	unsigned int csr_val, times = 30;
+	void __iomem *csr_addr =
+			(void __iomem *)((uintptr_t)handle->hal_ep_csr_addr_v +
+			ESRAM_AUTO_INIT_CSR_OFFSET);
+	unsigned int csr_val;
+	int times = 30;
+
+	if (handle->pci_dev->device == ADF_C3XXX_PCI_DEVICE_ID)
+		return 0;
 
 	csr_val = ADF_CSR_RD(csr_addr, 0);
 	if ((csr_val & ESRAM_AUTO_TINIT) && (csr_val & ESRAM_AUTO_TINIT_DONE))
@@ -456,7 +471,7 @@ static int qat_hal_init_esram(struct icp_qat_fw_loader_handle *handle)
 		qat_hal_wait_cycles(handle, 0, ESRAM_AUTO_INIT_USED_CYCLES, 0);
 		csr_val = ADF_CSR_RD(csr_addr, 0);
 	} while (!(csr_val & ESRAM_AUTO_TINIT_DONE) && times--);
-	if ((!times)) {
+	if ((times < 0)) {
 		pr_err("QAT: Fail to init eSram!\n");
 		return -EFAULT;
 	}
@@ -493,8 +508,6 @@ int qat_hal_clr_reset(struct icp_qat_fw_loader_handle *handle)
 
 	/* Set undefined power-up/reset states to reasonable default values */
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
-		if (!(handle->hal_handle->ae_mask & (1 << ae)))
-			continue;
 		qat_hal_wr_ae_csr(handle, ae, CTX_ENABLES,
 				  INIT_CTX_ENABLE_VALUE);
 		qat_hal_wr_indr_csr(handle, ae, ICP_QAT_UCLO_AE_ALL_CTX,
@@ -598,25 +611,31 @@ static void qat_hal_enable_ctx(struct icp_qat_fw_loader_handle *handle,
 	qat_hal_wr_ae_csr(handle, ae, CTX_ENABLES, ctx);
 }
 
-static int qat_hal_clear_gpr(struct icp_qat_fw_loader_handle *handle)
+static void qat_hal_clear_xfer(struct icp_qat_fw_loader_handle *handle)
 {
 	unsigned char ae;
-	unsigned int ctx_mask = ICP_QAT_UCLO_AE_ALL_CTX;
-	int times = MAX_RETRY_TIMES;
-	unsigned int csr_val = 0;
 	unsigned short reg;
-	unsigned int savctx = 0;
-	int ret = 0;
 
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
-		if (!(handle->hal_handle->ae_mask & (1 << ae)))
-			continue;
 		for (reg = 0; reg < ICP_QAT_UCLO_MAX_GPR_REG; reg++) {
 			qat_hal_init_rd_xfer(handle, ae, 0, ICP_SR_RD_ABS,
 					     reg, 0);
 			qat_hal_init_rd_xfer(handle, ae, 0, ICP_DR_RD_ABS,
 					     reg, 0);
 		}
+	}
+}
+
+static int qat_hal_clear_gpr(struct icp_qat_fw_loader_handle *handle)
+{
+	unsigned char ae;
+	unsigned int ctx_mask = ICP_QAT_UCLO_AE_ALL_CTX;
+	int times = MAX_RETRY_TIMES;
+	unsigned int csr_val = 0;
+	unsigned int savctx = 0;
+	int ret = 0;
+
+	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
 		qat_hal_rd_ae_csr(handle, ae, AE_MISC_CONTROL, &csr_val);
 		csr_val &= ~(1 << MMC_SHARE_CS_BITPOS);
 		qat_hal_wr_ae_csr(handle, ae, AE_MISC_CONTROL, csr_val);
@@ -638,14 +657,12 @@ static int qat_hal_clear_gpr(struct icp_qat_fw_loader_handle *handle)
 		qat_hal_enable_ctx(handle, ae, ctx_mask);
 	}
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
-		if (!(handle->hal_handle->ae_mask & (1 << ae)))
-			continue;
 		/* wait for AE to finish */
 		do {
 			ret = qat_hal_wait_cycles(handle, ae, 20, 1);
 		} while (ret && times--);
 
-		if (!times) {
+		if (times < 0) {
 			pr_err("QAT: clear GPR of AE %d failed", ae);
 			return -EINVAL;
 		}
@@ -667,10 +684,10 @@ static int qat_hal_clear_gpr(struct icp_qat_fw_loader_handle *handle)
 	return 0;
 }
 
-#define ICP_DH895XCC_AE_OFFSET      0x20000
-#define ICP_DH895XCC_CAP_OFFSET     (ICP_DH895XCC_AE_OFFSET + 0x10000)
+#define ICP_QAT_AE_OFFSET	0x20000
+#define ICP_QAT_CAP_OFFSET       (ICP_QAT_AE_OFFSET + 0x10000)
 #define LOCAL_TO_XFER_REG_OFFSET    0x800
-#define ICP_DH895XCC_EP_OFFSET      0x3a000
+#define ICP_QAT_EP_OFFSET	0x3a000
 int qat_hal_init(struct adf_accel_dev *accel_dev)
 {
 	unsigned char ae;
@@ -680,22 +697,32 @@ int qat_hal_init(struct adf_accel_dev *accel_dev)
 	struct adf_hw_device_data *hw_data = accel_dev->hw_device;
 	struct adf_bar *misc_bar =
 			&pci_info->pci_bars[hw_data->get_misc_bar_id(hw_data)];
-	struct adf_bar *sram_bar =
-			&pci_info->pci_bars[hw_data->get_sram_bar_id(hw_data)];
+	struct adf_bar *sram_bar;
 
 	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
 	if (!handle)
 		return -ENOMEM;
 
-	handle->hal_cap_g_ctl_csr_addr_v = misc_bar->virt_addr +
-						ICP_DH895XCC_CAP_OFFSET;
-	handle->hal_cap_ae_xfer_csr_addr_v = misc_bar->virt_addr +
-						ICP_DH895XCC_AE_OFFSET;
-	handle->hal_ep_csr_addr_v = misc_bar->virt_addr +
-				    ICP_DH895XCC_EP_OFFSET;
+	handle->hal_cap_g_ctl_csr_addr_v =
+		(void __iomem *)((uintptr_t)misc_bar->virt_addr +
+				 ICP_QAT_CAP_OFFSET);
+	handle->hal_cap_ae_xfer_csr_addr_v =
+		(void __iomem *)((uintptr_t)misc_bar->virt_addr +
+				 ICP_QAT_AE_OFFSET);
+	handle->hal_ep_csr_addr_v =
+		(void __iomem *)((uintptr_t)misc_bar->virt_addr +
+				 ICP_QAT_EP_OFFSET);
 	handle->hal_cap_ae_local_csr_addr_v =
-		handle->hal_cap_ae_xfer_csr_addr_v + LOCAL_TO_XFER_REG_OFFSET;
-	handle->hal_sram_addr_v = sram_bar->virt_addr;
+		(void __iomem *)((uintptr_t)handle->hal_cap_ae_xfer_csr_addr_v +
+				 LOCAL_TO_XFER_REG_OFFSET);
+	handle->pci_dev = pci_info->pci_dev;
+	if (handle->pci_dev->device != ADF_C3XXX_PCI_DEVICE_ID) {
+		sram_bar =
+			&pci_info->pci_bars[hw_data->get_sram_bar_id(hw_data)];
+		handle->hal_sram_addr_v = sram_bar->virt_addr;
+	}
+	handle->fw_auth = (handle->pci_dev->device ==
+			   ADF_DH895XCC_PCI_DEVICE_ID) ? false : true;
 	handle->hal_handle = kzalloc(sizeof(*handle->hal_handle), GFP_KERNEL);
 	if (!handle->hal_handle)
 		goto out_hal_handle;
@@ -723,14 +750,16 @@ int qat_hal_init(struct adf_accel_dev *accel_dev)
 		dev_err(&GET_DEV(accel_dev), "qat_hal_clr_reset error\n");
 		goto out_err;
 	}
-	if (qat_hal_clear_gpr(handle))
-		goto out_err;
+	qat_hal_clear_xfer(handle);
+	if (!handle->fw_auth) {
+		if (qat_hal_clear_gpr(handle))
+			goto out_err;
+	}
+
 	/* Set SIGNATURE_ENABLE[0] to 0x1 in order to enable ALU_OUT csr */
 	for (ae = 0; ae < handle->hal_handle->ae_max_num; ae++) {
 		unsigned int csr_val = 0;
 
-		if (!(hw_data->ae_mask & (1 << ae)))
-			continue;
 		qat_hal_rd_ae_csr(handle, ae, SIGNATURE_ENABLE, &csr_val);
 		csr_val |= 0x1;
 		qat_hal_wr_ae_csr(handle, ae, SIGNATURE_ENABLE, csr_val);
@@ -756,15 +785,31 @@ void qat_hal_deinit(struct icp_qat_fw_loader_handle *handle)
 void qat_hal_start(struct icp_qat_fw_loader_handle *handle, unsigned char ae,
 		   unsigned int ctx_mask)
 {
-	qat_hal_put_wakeup_event(handle, ae, (~ctx_mask) &
+	int retry = 0;
+	unsigned int fcu_sts = 0;
+
+	if (handle->fw_auth) {
+		SET_CAP_CSR(handle, FCU_CONTROL, FCU_CTRL_CMD_START);
+		do {
+			msleep(FW_AUTH_WAIT_PERIOD);
+			fcu_sts = GET_CAP_CSR(handle, FCU_STATUS);
+			if (((fcu_sts >> FCU_STS_DONE_POS) & 0x1))
+				return;
+		} while (retry++ < FW_AUTH_MAX_RETRY);
+		pr_err("QAT: start error (AE 0x%x FCU_STS = 0x%x)\n", ae,
+		       fcu_sts);
+	} else {
+		qat_hal_put_wakeup_event(handle, ae, (~ctx_mask) &
 				 ICP_QAT_UCLO_AE_ALL_CTX, 0x10000);
-	qat_hal_enable_ctx(handle, ae, ctx_mask);
+		qat_hal_enable_ctx(handle, ae, ctx_mask);
+	}
 }
 
 void qat_hal_stop(struct icp_qat_fw_loader_handle *handle, unsigned char ae,
 		  unsigned int ctx_mask)
 {
-	qat_hal_disable_ctx(handle, ae, ctx_mask);
+	if (!handle->fw_auth)
+		qat_hal_disable_ctx(handle, ae, ctx_mask);
 }
 
 void qat_hal_set_pc(struct icp_qat_fw_loader_handle *handle,
diff --git a/drivers/crypto/qat/qat_common/qat_uclo.c b/drivers/crypto/qat/qat_common/qat_uclo.c
index c48f181e8941..25d15f19c2b3 100644
--- a/drivers/crypto/qat/qat_common/qat_uclo.c
+++ b/drivers/crypto/qat/qat_common/qat_uclo.c
@@ -47,7 +47,7 @@
 #include <linux/slab.h>
 #include <linux/ctype.h>
 #include <linux/kernel.h>
-
+#include <linux/delay.h>
 #include "adf_accel_devices.h"
 #include "adf_common_drv.h"
 #include "icp_qat_uclo.h"
@@ -119,10 +119,10 @@ static char *qat_uclo_get_string(struct icp_qat_uof_strtable *str_table,
 {
 	if ((!str_table->table_len) || (str_offset > str_table->table_len))
 		return NULL;
-	return (char *)(((unsigned long)(str_table->strings)) + str_offset);
+	return (char *)(((uintptr_t)(str_table->strings)) + str_offset);
 }
 
-static int qat_uclo_check_format(struct icp_qat_uof_filehdr *hdr)
+static int qat_uclo_check_uof_format(struct icp_qat_uof_filehdr *hdr)
 {
 	int maj = hdr->maj_ver & 0xff;
 	int min = hdr->min_ver & 0xff;
@@ -139,6 +139,31 @@ static int qat_uclo_check_format(struct icp_qat_uof_filehdr *hdr)
 	return 0;
 }
 
+static int qat_uclo_check_suof_format(struct icp_qat_suof_filehdr *suof_hdr)
+{
+	int maj = suof_hdr->maj_ver & 0xff;
+	int min = suof_hdr->min_ver & 0xff;
+
+	if (suof_hdr->file_id != ICP_QAT_SUOF_FID) {
+		pr_err("QAT: invalid header 0x%x\n", suof_hdr->file_id);
+		return -EINVAL;
+	}
+	if (suof_hdr->fw_type != 0) {
+		pr_err("QAT: unsupported firmware type\n");
+		return -EINVAL;
+	}
+	if (suof_hdr->num_chunks <= 0x1) {
+		pr_err("QAT: SUOF chunk amount is incorrect\n");
+		return -EINVAL;
+	}
+	if (maj != ICP_QAT_SUOF_MAJVER || min != ICP_QAT_SUOF_MINVER) {
+		pr_err("QAT: bad SUOF version, major 0x%x, minor 0x%x\n",
+		       maj, min);
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static void qat_uclo_wr_sram_by_words(struct icp_qat_fw_loader_handle *handle,
 				      unsigned int addr, unsigned int *val,
 				      unsigned int num_in_bytes)
@@ -275,7 +300,7 @@ static int qat_uclo_create_batch_init_list(struct icp_qat_fw_loader_handle
 	unsigned int i, flag = 0;
 
 	mem_val_attr =
-		(struct icp_qat_uof_memvar_attr *)((unsigned long)init_mem +
+		(struct icp_qat_uof_memvar_attr *)((uintptr_t)init_mem +
 		sizeof(struct icp_qat_uof_initmem));
 
 	init_header = *init_tab_base;
@@ -425,8 +450,8 @@ static int qat_uclo_init_memory(struct icp_qat_fw_loader_handle *handle)
 			if (qat_uclo_init_ae_memory(handle, initmem))
 				return -EINVAL;
 		}
-		initmem = (struct icp_qat_uof_initmem *)((unsigned long)(
-			(unsigned long)initmem +
+		initmem = (struct icp_qat_uof_initmem *)((uintptr_t)(
+			(uintptr_t)initmem +
 			sizeof(struct icp_qat_uof_initmem)) +
 			(sizeof(struct icp_qat_uof_memvar_attr) *
 			initmem->val_attr_num));
@@ -454,7 +479,7 @@ static void *qat_uclo_find_chunk(struct icp_qat_uof_objhdr *obj_hdr,
 	int i;
 	struct icp_qat_uof_chunkhdr *chunk_hdr =
 	    (struct icp_qat_uof_chunkhdr *)
-	    ((unsigned long)obj_hdr + sizeof(struct icp_qat_uof_objhdr));
+	    ((uintptr_t)obj_hdr + sizeof(struct icp_qat_uof_objhdr));
 
 	for (i = 0; i < obj_hdr->num_chunks; i++) {
 		if ((cur < (void *)&chunk_hdr[i]) &&
@@ -596,7 +621,7 @@ static void qat_uclo_map_image_page(struct icp_qat_uof_encap_obj
 	page->uwblock = (struct icp_qat_uclo_encap_uwblock *)uwblock;
 	for (i = 0; i < uword_block_tab->entry_num; i++)
 		page->uwblock[i].micro_words =
-		(unsigned long)encap_uof_obj->beg_uof + uwblock[i].uword_offset;
+		(uintptr_t)encap_uof_obj->beg_uof + uwblock[i].uword_offset;
 }
 
 static int qat_uclo_map_uimage(struct icp_qat_uclo_objhandle *obj_handle,
@@ -697,7 +722,7 @@ qat_uclo_map_str_table(struct icp_qat_uclo_objhdr *obj_hdr,
 		memcpy(&str_table->table_len, obj_hdr->file_buff +
 		       chunk_hdr->offset, sizeof(str_table->table_len));
 		hdr_size = (char *)&str_table->strings - (char *)str_table;
-		str_table->strings = (unsigned long)obj_hdr->file_buff +
+		str_table->strings = (uintptr_t)obj_hdr->file_buff +
 					chunk_hdr->offset + hdr_size;
 		return str_table;
 	}
@@ -721,13 +746,31 @@ qat_uclo_map_initmem_table(struct icp_qat_uof_encap_obj *encap_uof_obj,
 	}
 }
 
+static unsigned int
+qat_uclo_get_dev_type(struct icp_qat_fw_loader_handle *handle)
+{
+	switch (handle->pci_dev->device) {
+	case ADF_DH895XCC_PCI_DEVICE_ID:
+		return ICP_QAT_AC_895XCC_DEV_TYPE;
+	case ADF_C62X_PCI_DEVICE_ID:
+		return ICP_QAT_AC_C62X_DEV_TYPE;
+	case ADF_C3XXX_PCI_DEVICE_ID:
+		return ICP_QAT_AC_C3XXX_DEV_TYPE;
+	default:
+		pr_err("QAT: unsupported device 0x%x\n",
+		       handle->pci_dev->device);
+		return 0;
+	}
+}
+
 static int qat_uclo_check_uof_compat(struct icp_qat_uclo_objhandle *obj_handle)
 {
 	unsigned int maj_ver, prod_type = obj_handle->prod_type;
 
-	if (!(prod_type & obj_handle->encap_uof_obj.obj_hdr->cpu_type)) {
-		pr_err("QAT: UOF type 0x%x not match with cur platform 0x%x\n",
-		       obj_handle->encap_uof_obj.obj_hdr->cpu_type, prod_type);
+	if (!(prod_type & obj_handle->encap_uof_obj.obj_hdr->ac_dev_type)) {
+		pr_err("QAT: UOF type 0x%x doesn't match with platform 0x%x\n",
+		       obj_handle->encap_uof_obj.obj_hdr->ac_dev_type,
+		       prod_type);
 		return -EINVAL;
 	}
 	maj_ver = obj_handle->prod_rev & 0xff;
@@ -932,7 +975,7 @@ static int qat_uclo_parse_uof_obj(struct icp_qat_fw_loader_handle *handle)
 	obj_handle->encap_uof_obj.obj_hdr = (struct icp_qat_uof_objhdr *)
 					     obj_handle->obj_hdr->file_buff;
 	obj_handle->uword_in_bytes = 6;
-	obj_handle->prod_type = ICP_QAT_AC_C_CPU_TYPE;
+	obj_handle->prod_type = qat_uclo_get_dev_type(handle);
 	obj_handle->prod_rev = PID_MAJOR_REV |
 			(PID_MINOR_REV & handle->hal_handle->revision_id);
 	if (qat_uclo_check_uof_compat(obj_handle)) {
@@ -969,23 +1012,435 @@ out_err:
 	return -EFAULT;
 }
 
-void qat_uclo_wr_mimage(struct icp_qat_fw_loader_handle *handle,
-			void *addr_ptr, int mem_size)
+static int qat_uclo_map_suof_file_hdr(struct icp_qat_fw_loader_handle *handle,
+				      struct icp_qat_suof_filehdr *suof_ptr,
+				      int suof_size)
 {
-	qat_uclo_wr_sram_by_words(handle, 0, addr_ptr, ALIGN(mem_size, 4));
+	unsigned int check_sum = 0;
+	unsigned int min_ver_offset = 0;
+	struct icp_qat_suof_handle *suof_handle = handle->sobj_handle;
+
+	suof_handle->file_id = ICP_QAT_SUOF_FID;
+	suof_handle->suof_buf = (char *)suof_ptr;
+	suof_handle->suof_size = suof_size;
+	min_ver_offset = suof_size - offsetof(struct icp_qat_suof_filehdr,
+					      min_ver);
+	check_sum = qat_uclo_calc_str_checksum((char *)&suof_ptr->min_ver,
+					       min_ver_offset);
+	if (check_sum != suof_ptr->check_sum) {
+		pr_err("QAT: incorrect SUOF checksum\n");
+		return -EINVAL;
+	}
+	suof_handle->check_sum = suof_ptr->check_sum;
+	suof_handle->min_ver = suof_ptr->min_ver;
+	suof_handle->maj_ver = suof_ptr->maj_ver;
+	suof_handle->fw_type = suof_ptr->fw_type;
+	return 0;
 }
 
-int qat_uclo_map_uof_obj(struct icp_qat_fw_loader_handle *handle,
-			 void *addr_ptr, int mem_size)
+static void qat_uclo_map_simg(struct icp_qat_suof_handle *suof_handle,
+			      struct icp_qat_suof_img_hdr *suof_img_hdr,
+			      struct icp_qat_suof_chunk_hdr *suof_chunk_hdr)
 {
-	struct icp_qat_uof_filehdr *filehdr;
-	struct icp_qat_uclo_objhandle *objhdl;
+	struct icp_qat_simg_ae_mode *ae_mode;
+	struct icp_qat_suof_objhdr *suof_objhdr;
+
+	suof_img_hdr->simg_buf  = (suof_handle->suof_buf +
+				   suof_chunk_hdr->offset +
+				   sizeof(*suof_objhdr));
+	suof_img_hdr->simg_len = ((struct icp_qat_suof_objhdr *)(uintptr_t)
+				  (suof_handle->suof_buf +
+				   suof_chunk_hdr->offset))->img_length;
+
+	suof_img_hdr->css_header = suof_img_hdr->simg_buf;
+	suof_img_hdr->css_key = (suof_img_hdr->css_header +
+				 sizeof(struct icp_qat_css_hdr));
+	suof_img_hdr->css_signature = suof_img_hdr->css_key +
+				      ICP_QAT_CSS_FWSK_MODULUS_LEN +
+				      ICP_QAT_CSS_FWSK_EXPONENT_LEN;
+	suof_img_hdr->css_simg = suof_img_hdr->css_signature +
+				 ICP_QAT_CSS_SIGNATURE_LEN;
+
+	ae_mode = (struct icp_qat_simg_ae_mode *)(suof_img_hdr->css_simg);
+	suof_img_hdr->ae_mask = ae_mode->ae_mask;
+	suof_img_hdr->simg_name = (unsigned long)&ae_mode->simg_name;
+	suof_img_hdr->appmeta_data = (unsigned long)&ae_mode->appmeta_data;
+	suof_img_hdr->fw_type = ae_mode->fw_type;
+}
 
-	BUILD_BUG_ON(ICP_QAT_UCLO_MAX_AE >=
-		     (sizeof(handle->hal_handle->ae_mask) * 8));
+static void
+qat_uclo_map_suof_symobjs(struct icp_qat_suof_handle *suof_handle,
+			  struct icp_qat_suof_chunk_hdr *suof_chunk_hdr)
+{
+	char **sym_str = (char **)&suof_handle->sym_str;
+	unsigned int *sym_size = &suof_handle->sym_size;
+	struct icp_qat_suof_strtable *str_table_obj;
+
+	*sym_size = *(unsigned int *)(uintptr_t)
+		   (suof_chunk_hdr->offset + suof_handle->suof_buf);
+	*sym_str = (char *)(uintptr_t)
+		   (suof_handle->suof_buf + suof_chunk_hdr->offset +
+		   sizeof(str_table_obj->tab_length));
+}
 
-	if (!handle || !addr_ptr || mem_size < 24)
+static int qat_uclo_check_simg_compat(struct icp_qat_fw_loader_handle *handle,
+				      struct icp_qat_suof_img_hdr *img_hdr)
+{
+	struct icp_qat_simg_ae_mode *img_ae_mode = NULL;
+	unsigned int prod_rev, maj_ver, prod_type;
+
+	prod_type = qat_uclo_get_dev_type(handle);
+	img_ae_mode = (struct icp_qat_simg_ae_mode *)img_hdr->css_simg;
+	prod_rev = PID_MAJOR_REV |
+			 (PID_MINOR_REV & handle->hal_handle->revision_id);
+	if (img_ae_mode->dev_type != prod_type) {
+		pr_err("QAT: incompatible product type %x\n",
+		       img_ae_mode->dev_type);
 		return -EINVAL;
+	}
+	maj_ver = prod_rev & 0xff;
+	if ((maj_ver > img_ae_mode->devmax_ver) ||
+	    (maj_ver < img_ae_mode->devmin_ver)) {
+		pr_err("QAT: incompatible device majver 0x%x\n", maj_ver);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void qat_uclo_del_suof(struct icp_qat_fw_loader_handle *handle)
+{
+	struct icp_qat_suof_handle *sobj_handle = handle->sobj_handle;
+
+	kfree(sobj_handle->img_table.simg_hdr);
+	sobj_handle->img_table.simg_hdr = NULL;
+	kfree(handle->sobj_handle);
+	handle->sobj_handle = NULL;
+}
+
+static void qat_uclo_tail_img(struct icp_qat_suof_img_hdr *suof_img_hdr,
+			      unsigned int img_id, unsigned int num_simgs)
+{
+	struct icp_qat_suof_img_hdr img_header;
+
+	if (img_id != num_simgs - 1) {
+		memcpy(&img_header, &suof_img_hdr[num_simgs - 1],
+		       sizeof(*suof_img_hdr));
+		memcpy(&suof_img_hdr[num_simgs - 1], &suof_img_hdr[img_id],
+		       sizeof(*suof_img_hdr));
+		memcpy(&suof_img_hdr[img_id], &img_header,
+		       sizeof(*suof_img_hdr));
+	}
+}
+
+static int qat_uclo_map_suof(struct icp_qat_fw_loader_handle *handle,
+			     struct icp_qat_suof_filehdr *suof_ptr,
+			     int suof_size)
+{
+	struct icp_qat_suof_handle *suof_handle = handle->sobj_handle;
+	struct icp_qat_suof_chunk_hdr *suof_chunk_hdr = NULL;
+	struct icp_qat_suof_img_hdr *suof_img_hdr = NULL;
+	int ret = 0, ae0_img = ICP_QAT_UCLO_MAX_AE;
+	unsigned int i = 0;
+	struct icp_qat_suof_img_hdr img_header;
+
+	if (!suof_ptr || (suof_size == 0)) {
+		pr_err("QAT: input parameter SUOF pointer/size is NULL\n");
+		return -EINVAL;
+	}
+	if (qat_uclo_check_suof_format(suof_ptr))
+		return -EINVAL;
+	ret = qat_uclo_map_suof_file_hdr(handle, suof_ptr, suof_size);
+	if (ret)
+		return ret;
+	suof_chunk_hdr = (struct icp_qat_suof_chunk_hdr *)
+			 ((uintptr_t)suof_ptr + sizeof(*suof_ptr));
+
+	qat_uclo_map_suof_symobjs(suof_handle, suof_chunk_hdr);
+	suof_handle->img_table.num_simgs = suof_ptr->num_chunks - 1;
+
+	if (suof_handle->img_table.num_simgs != 0) {
+		suof_img_hdr = kzalloc(suof_handle->img_table.num_simgs *
+				       sizeof(img_header), GFP_KERNEL);
+		if (!suof_img_hdr)
+			return -ENOMEM;
+		suof_handle->img_table.simg_hdr = suof_img_hdr;
+	}
+
+	for (i = 0; i < suof_handle->img_table.num_simgs; i++) {
+		qat_uclo_map_simg(handle->sobj_handle, &suof_img_hdr[i],
+				  &suof_chunk_hdr[1 + i]);
+		ret = qat_uclo_check_simg_compat(handle,
+						 &suof_img_hdr[i]);
+		if (ret)
+			return ret;
+		if ((suof_img_hdr[i].ae_mask & 0x1) != 0)
+			ae0_img = i;
+	}
+	qat_uclo_tail_img(suof_img_hdr, ae0_img,
+			  suof_handle->img_table.num_simgs);
+	return 0;
+}
+
+#define ADD_ADDR(high, low)  ((((uint64_t)high) << 32) + low)
+#define BITS_IN_DWORD 32
+
+static int qat_uclo_auth_fw(struct icp_qat_fw_loader_handle *handle,
+			    struct icp_qat_fw_auth_desc *desc)
+{
+	unsigned int fcu_sts, retry = 0;
+	u64 bus_addr;
+
+	bus_addr = ADD_ADDR(desc->css_hdr_high, desc->css_hdr_low)
+			   - sizeof(struct icp_qat_auth_chunk);
+	SET_CAP_CSR(handle, FCU_DRAM_ADDR_HI, (bus_addr >> BITS_IN_DWORD));
+	SET_CAP_CSR(handle, FCU_DRAM_ADDR_LO, bus_addr);
+	SET_CAP_CSR(handle, FCU_CONTROL, FCU_CTRL_CMD_AUTH);
+
+	do {
+		msleep(FW_AUTH_WAIT_PERIOD);
+		fcu_sts = GET_CAP_CSR(handle, FCU_STATUS);
+		if ((fcu_sts & FCU_AUTH_STS_MASK) == FCU_STS_VERI_FAIL)
+			goto auth_fail;
+		if (((fcu_sts >> FCU_STS_AUTHFWLD_POS) & 0x1))
+			if ((fcu_sts & FCU_AUTH_STS_MASK) == FCU_STS_VERI_DONE)
+				return 0;
+	} while (retry++ < FW_AUTH_MAX_RETRY);
+auth_fail:
+	pr_err("QAT: authentication error (FCU_STATUS = 0x%x),retry = %d\n",
+	       fcu_sts & FCU_AUTH_STS_MASK, retry);
+	return -EINVAL;
+}
+
+static int qat_uclo_simg_alloc(struct icp_qat_fw_loader_handle *handle,
+			       struct icp_firml_dram_desc *dram_desc,
+			       unsigned int size)
+{
+	void *vptr;
+	dma_addr_t ptr;
+
+	vptr = dma_alloc_coherent(&handle->pci_dev->dev,
+				  size, &ptr, GFP_KERNEL);
+	if (!vptr)
+		return -ENOMEM;
+	dram_desc->dram_base_addr_v = vptr;
+	dram_desc->dram_bus_addr = ptr;
+	dram_desc->dram_size = size;
+	return 0;
+}
+
+static void qat_uclo_simg_free(struct icp_qat_fw_loader_handle *handle,
+			       struct icp_firml_dram_desc *dram_desc)
+{
+	dma_free_coherent(&handle->pci_dev->dev,
+			  (size_t)(dram_desc->dram_size),
+			  (dram_desc->dram_base_addr_v),
+			  dram_desc->dram_bus_addr);
+	memset(dram_desc, 0, sizeof(*dram_desc));
+}
+
+static void qat_uclo_ummap_auth_fw(struct icp_qat_fw_loader_handle *handle,
+				   struct icp_qat_fw_auth_desc **desc)
+{
+	struct icp_firml_dram_desc dram_desc;
+
+	dram_desc.dram_base_addr_v = *desc;
+	dram_desc.dram_bus_addr = ((struct icp_qat_auth_chunk *)
+				   (*desc))->chunk_bus_addr;
+	dram_desc.dram_size = ((struct icp_qat_auth_chunk *)
+			       (*desc))->chunk_size;
+	qat_uclo_simg_free(handle, &dram_desc);
+}
+
+static int qat_uclo_map_auth_fw(struct icp_qat_fw_loader_handle *handle,
+				char *image, unsigned int size,
+				struct icp_qat_fw_auth_desc **desc)
+{
+	struct icp_qat_css_hdr *css_hdr = (struct icp_qat_css_hdr *)image;
+	struct icp_qat_fw_auth_desc *auth_desc;
+	struct icp_qat_auth_chunk *auth_chunk;
+	u64 virt_addr,  bus_addr, virt_base;
+	unsigned int length, simg_offset = sizeof(*auth_chunk);
+	struct icp_firml_dram_desc img_desc;
+
+	if (size > (ICP_QAT_AE_IMG_OFFSET + ICP_QAT_CSS_MAX_IMAGE_LEN)) {
+		pr_err("QAT: error, input image size overflow %d\n", size);
+		return -EINVAL;
+	}
+	length = (css_hdr->fw_type == CSS_AE_FIRMWARE) ?
+		 ICP_QAT_CSS_AE_SIMG_LEN + simg_offset :
+		 size + ICP_QAT_CSS_FWSK_PAD_LEN + simg_offset;
+	if (qat_uclo_simg_alloc(handle, &img_desc, length)) {
+		pr_err("QAT: error, allocate continuous dram fail\n");
+		return -ENOMEM;
+	}
+
+	auth_chunk = img_desc.dram_base_addr_v;
+	auth_chunk->chunk_size = img_desc.dram_size;
+	auth_chunk->chunk_bus_addr = img_desc.dram_bus_addr;
+	virt_base = (uintptr_t)img_desc.dram_base_addr_v + simg_offset;
+	bus_addr  = img_desc.dram_bus_addr + simg_offset;
+	auth_desc = img_desc.dram_base_addr_v;
+	auth_desc->css_hdr_high = (unsigned int)(bus_addr >> BITS_IN_DWORD);
+	auth_desc->css_hdr_low = (unsigned int)bus_addr;
+	virt_addr = virt_base;
+
+	memcpy((void *)(uintptr_t)virt_addr, image, sizeof(*css_hdr));
+	/* pub key */
+	bus_addr = ADD_ADDR(auth_desc->css_hdr_high, auth_desc->css_hdr_low) +
+			   sizeof(*css_hdr);
+	virt_addr = virt_addr + sizeof(*css_hdr);
+
+	auth_desc->fwsk_pub_high = (unsigned int)(bus_addr >> BITS_IN_DWORD);
+	auth_desc->fwsk_pub_low = (unsigned int)bus_addr;
+
+	memcpy((void *)(uintptr_t)virt_addr,
+	       (void *)(image + sizeof(*css_hdr)),
+	       ICP_QAT_CSS_FWSK_MODULUS_LEN);
+	/* padding */
+	memset((void *)(uintptr_t)(virt_addr + ICP_QAT_CSS_FWSK_MODULUS_LEN),
+	       0, ICP_QAT_CSS_FWSK_PAD_LEN);
+
+	/* exponent */
+	memcpy((void *)(uintptr_t)(virt_addr + ICP_QAT_CSS_FWSK_MODULUS_LEN +
+	       ICP_QAT_CSS_FWSK_PAD_LEN),
+	       (void *)(image + sizeof(*css_hdr) +
+			ICP_QAT_CSS_FWSK_MODULUS_LEN),
+	       sizeof(unsigned int));
+
+	/* signature */
+	bus_addr = ADD_ADDR(auth_desc->fwsk_pub_high,
+			    auth_desc->fwsk_pub_low) +
+		   ICP_QAT_CSS_FWSK_PUB_LEN;
+	virt_addr = virt_addr + ICP_QAT_CSS_FWSK_PUB_LEN;
+	auth_desc->signature_high = (unsigned int)(bus_addr >> BITS_IN_DWORD);
+	auth_desc->signature_low = (unsigned int)bus_addr;
+
+	memcpy((void *)(uintptr_t)virt_addr,
+	       (void *)(image + sizeof(*css_hdr) +
+	       ICP_QAT_CSS_FWSK_MODULUS_LEN +
+	       ICP_QAT_CSS_FWSK_EXPONENT_LEN),
+	       ICP_QAT_CSS_SIGNATURE_LEN);
+
+	bus_addr = ADD_ADDR(auth_desc->signature_high,
+			    auth_desc->signature_low) +
+		   ICP_QAT_CSS_SIGNATURE_LEN;
+	virt_addr += ICP_QAT_CSS_SIGNATURE_LEN;
+
+	auth_desc->img_high = (unsigned int)(bus_addr >> BITS_IN_DWORD);
+	auth_desc->img_low = (unsigned int)bus_addr;
+	auth_desc->img_len = size - ICP_QAT_AE_IMG_OFFSET;
+	memcpy((void *)(uintptr_t)virt_addr,
+	       (void *)(image + ICP_QAT_AE_IMG_OFFSET),
+	       auth_desc->img_len);
+	virt_addr = virt_base;
+	/* AE firmware */
+	if (((struct icp_qat_css_hdr *)(uintptr_t)virt_addr)->fw_type ==
+	    CSS_AE_FIRMWARE) {
+		auth_desc->img_ae_mode_data_high = auth_desc->img_high;
+		auth_desc->img_ae_mode_data_low = auth_desc->img_low;
+		bus_addr = ADD_ADDR(auth_desc->img_ae_mode_data_high,
+				    auth_desc->img_ae_mode_data_low) +
+			   sizeof(struct icp_qat_simg_ae_mode);
+
+		auth_desc->img_ae_init_data_high = (unsigned int)
+						 (bus_addr >> BITS_IN_DWORD);
+		auth_desc->img_ae_init_data_low = (unsigned int)bus_addr;
+		bus_addr += ICP_QAT_SIMG_AE_INIT_SEQ_LEN;
+		auth_desc->img_ae_insts_high = (unsigned int)
+					     (bus_addr >> BITS_IN_DWORD);
+		auth_desc->img_ae_insts_low = (unsigned int)bus_addr;
+	} else {
+		auth_desc->img_ae_insts_high = auth_desc->img_high;
+		auth_desc->img_ae_insts_low = auth_desc->img_low;
+	}
+	*desc = auth_desc;
+	return 0;
+}
+
+static int qat_uclo_load_fw(struct icp_qat_fw_loader_handle *handle,
+			    struct icp_qat_fw_auth_desc *desc)
+{
+	unsigned int i;
+	unsigned int fcu_sts;
+	struct icp_qat_simg_ae_mode *virt_addr;
+	unsigned int fcu_loaded_ae_pos = FCU_LOADED_AE_POS;
+
+	virt_addr = (void *)((uintptr_t)desc +
+		     sizeof(struct icp_qat_auth_chunk) +
+		     sizeof(struct icp_qat_css_hdr) +
+		     ICP_QAT_CSS_FWSK_PUB_LEN +
+		     ICP_QAT_CSS_SIGNATURE_LEN);
+	for (i = 0; i < handle->hal_handle->ae_max_num; i++) {
+		int retry = 0;
+
+		if (!((virt_addr->ae_mask >> i) & 0x1))
+			continue;
+		if (qat_hal_check_ae_active(handle, i)) {
+			pr_err("QAT: AE %d is active\n", i);
+			return -EINVAL;
+		}
+		SET_CAP_CSR(handle, FCU_CONTROL,
+			    (FCU_CTRL_CMD_LOAD | (i << FCU_CTRL_AE_POS)));
+
+		do {
+			msleep(FW_AUTH_WAIT_PERIOD);
+			fcu_sts = GET_CAP_CSR(handle, FCU_STATUS);
+			if (((fcu_sts & FCU_AUTH_STS_MASK) ==
+			    FCU_STS_LOAD_DONE) &&
+			    ((fcu_sts >> fcu_loaded_ae_pos) & (1 << i)))
+				break;
+		} while (retry++ < FW_AUTH_MAX_RETRY);
+		if (retry > FW_AUTH_MAX_RETRY) {
+			pr_err("QAT: firmware load failed timeout %x\n", retry);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+static int qat_uclo_map_suof_obj(struct icp_qat_fw_loader_handle *handle,
+				 void *addr_ptr, int mem_size)
+{
+	struct icp_qat_suof_handle *suof_handle;
+
+	suof_handle = kzalloc(sizeof(*suof_handle), GFP_KERNEL);
+	if (!suof_handle)
+		return -ENOMEM;
+	handle->sobj_handle = suof_handle;
+	if (qat_uclo_map_suof(handle, addr_ptr, mem_size)) {
+		qat_uclo_del_suof(handle);
+		pr_err("QAT: map SUOF failed\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int qat_uclo_wr_mimage(struct icp_qat_fw_loader_handle *handle,
+		       void *addr_ptr, int mem_size)
+{
+	struct icp_qat_fw_auth_desc *desc = NULL;
+	int status = 0;
+
+	if (handle->fw_auth) {
+		if (!qat_uclo_map_auth_fw(handle, addr_ptr, mem_size, &desc))
+			status = qat_uclo_auth_fw(handle, desc);
+		qat_uclo_ummap_auth_fw(handle, &desc);
+	} else {
+		if (handle->pci_dev->device == ADF_C3XXX_PCI_DEVICE_ID) {
+			pr_err("QAT: C3XXX doesn't support unsigned MMP\n");
+			return -EINVAL;
+		}
+		qat_uclo_wr_sram_by_words(handle, 0, addr_ptr, mem_size);
+	}
+	return status;
+}
+
+static int qat_uclo_map_uof_obj(struct icp_qat_fw_loader_handle *handle,
+				void *addr_ptr, int mem_size)
+{
+	struct icp_qat_uof_filehdr *filehdr;
+	struct icp_qat_uclo_objhandle *objhdl;
+
 	objhdl = kzalloc(sizeof(*objhdl), GFP_KERNEL);
 	if (!objhdl)
 		return -ENOMEM;
@@ -993,7 +1448,7 @@ int qat_uclo_map_uof_obj(struct icp_qat_fw_loader_handle *handle,
 	if (!objhdl->obj_buf)
 		goto out_objbuf_err;
 	filehdr = (struct icp_qat_uof_filehdr *)objhdl->obj_buf;
-	if (qat_uclo_check_format(filehdr))
+	if (qat_uclo_check_uof_format(filehdr))
 		goto out_objhdr_err;
 	objhdl->obj_hdr = qat_uclo_map_chunk((char *)objhdl->obj_buf, filehdr,
 					     ICP_QAT_UOF_OBJS);
@@ -1016,11 +1471,27 @@ out_objbuf_err:
 	return -ENOMEM;
 }
 
+int qat_uclo_map_obj(struct icp_qat_fw_loader_handle *handle,
+		     void *addr_ptr, int mem_size)
+{
+	BUILD_BUG_ON(ICP_QAT_UCLO_MAX_AE >=
+		     (sizeof(handle->hal_handle->ae_mask) * 8));
+
+	if (!handle || !addr_ptr || mem_size < 24)
+		return -EINVAL;
+
+	return (handle->fw_auth) ?
+			qat_uclo_map_suof_obj(handle, addr_ptr, mem_size) :
+			qat_uclo_map_uof_obj(handle, addr_ptr, mem_size);
+}
+
 void qat_uclo_del_uof_obj(struct icp_qat_fw_loader_handle *handle)
 {
 	struct icp_qat_uclo_objhandle *obj_handle = handle->obj_handle;
 	unsigned int a;
 
+	if (handle->sobj_handle)
+		qat_uclo_del_suof(handle);
 	if (!obj_handle)
 		return;
 
@@ -1055,7 +1526,7 @@ static void qat_uclo_fill_uwords(struct icp_qat_uclo_objhandle *obj_handle,
 		    encap_page->uwblock[i].words_num - 1) {
 			raddr -= encap_page->uwblock[i].start_addr;
 			raddr *= obj_handle->uword_in_bytes;
-			memcpy(&uwrd, (void *)(((unsigned long)
+			memcpy(&uwrd, (void *)(((uintptr_t)
 			       encap_page->uwblock[i].micro_words) + raddr),
 			       obj_handle->uword_in_bytes);
 			uwrd = uwrd & 0xbffffffffffull;
@@ -1147,7 +1618,33 @@ static void qat_uclo_wr_uimage_page(struct icp_qat_fw_loader_handle *handle,
 	}
 }
 
-int qat_uclo_wr_all_uimage(struct icp_qat_fw_loader_handle *handle)
+static int qat_uclo_wr_suof_img(struct icp_qat_fw_loader_handle *handle)
+{
+	unsigned int i;
+	struct icp_qat_fw_auth_desc *desc = NULL;
+	struct icp_qat_suof_handle *sobj_handle = handle->sobj_handle;
+	struct icp_qat_suof_img_hdr *simg_hdr = sobj_handle->img_table.simg_hdr;
+
+	for (i = 0; i < sobj_handle->img_table.num_simgs; i++) {
+		if (qat_uclo_map_auth_fw(handle,
+					 (char *)simg_hdr[i].simg_buf,
+					 (unsigned int)
+					 (simg_hdr[i].simg_len),
+					 &desc))
+			goto wr_err;
+		if (qat_uclo_auth_fw(handle, desc))
+			goto wr_err;
+		if (qat_uclo_load_fw(handle, desc))
+			goto wr_err;
+		qat_uclo_ummap_auth_fw(handle, &desc);
+	}
+	return 0;
+wr_err:
+	qat_uclo_ummap_auth_fw(handle, &desc);
+	return -EINVAL;
+}
+
+static int qat_uclo_wr_uof_img(struct icp_qat_fw_loader_handle *handle)
 {
 	struct icp_qat_uclo_objhandle *obj_handle = handle->obj_handle;
 	unsigned int i;
@@ -1164,3 +1661,9 @@ int qat_uclo_wr_all_uimage(struct icp_qat_fw_loader_handle *handle)
 	}
 	return 0;
 }
+
+int qat_uclo_wr_all_uimage(struct icp_qat_fw_loader_handle *handle)
+{
+	return (handle->fw_auth) ? qat_uclo_wr_suof_img(handle) :
+				   qat_uclo_wr_uof_img(handle);
+}
diff --git a/drivers/crypto/qat/qat_dh895xcc/Makefile b/drivers/crypto/qat/qat_dh895xcc/Makefile
index 8c79c543740f..180a00ed7f89 100644
--- a/drivers/crypto/qat/qat_dh895xcc/Makefile
+++ b/drivers/crypto/qat/qat_dh895xcc/Makefile
@@ -1,5 +1,3 @@
 ccflags-y := -I$(src)/../qat_common
 obj-$(CONFIG_CRYPTO_DEV_QAT_DH895xCC) += qat_dh895xcc.o
-qat_dh895xcc-objs := adf_drv.o \
-		adf_isr.o \
-		adf_dh895xcc_hw_data.o
+qat_dh895xcc-objs := adf_drv.o adf_dh895xcc_hw_data.o
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
index ff54257eced4..6e1d5e185526 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.c
@@ -48,7 +48,6 @@
 #include <adf_pf2vf_msg.h>
 #include <adf_common_drv.h>
 #include "adf_dh895xcc_hw_data.h"
-#include "adf_drv.h"
 
 /* Worker thread to service arbiter mappings based on dev SKUs */
 static const uint32_t thrd_to_arb_map_sku4[] = {
@@ -143,8 +142,8 @@ static enum dev_sku_info get_sku(struct adf_hw_device_data *self)
 	return DEV_SKU_UNKNOWN;
 }
 
-void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
-			     uint32_t const **arb_map_config)
+static void adf_get_arbiter_mapping(struct adf_accel_dev *accel_dev,
+				    u32 const **arb_map_config)
 {
 	switch (accel_dev->accel_pci_dev.sku) {
 	case DEV_SKU_1:
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
index 88dffb297346..092f7353ed23 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
@@ -53,7 +53,6 @@
 #define ADF_DH895XCC_ETR_BAR 2
 #define ADF_DH895XCC_RX_RINGS_OFFSET 8
 #define ADF_DH895XCC_TX_RINGS_MASK 0xFF
-#define ADF_DH895XCC_FUSECTL_OFFSET 0x40
 #define ADF_DH895XCC_FUSECTL_SKU_MASK 0x300000
 #define ADF_DH895XCC_FUSECTL_SKU_SHIFT 20
 #define ADF_DH895XCC_FUSECTL_SKU_1 0x0
@@ -65,7 +64,6 @@
 #define ADF_DH895XCC_ACCELERATORS_REG_OFFSET 13
 #define ADF_DH895XCC_ACCELERATORS_MASK 0x3F
 #define ADF_DH895XCC_ACCELENGINES_MASK 0xFFF
-#define ADF_DH895XCC_LEGFUSE_OFFSET 0x4C
 #define ADF_DH895XCC_ETR_MAX_BANKS 32
 #define ADF_DH895XCC_SMIAPF0_MASK_OFFSET (0x3A000 + 0x28)
 #define ADF_DH895XCC_SMIAPF1_MASK_OFFSET (0x3A000 + 0x30)
@@ -80,11 +78,12 @@
 #define ADF_DH895XCC_CERRSSMSH(i) (i * 0x4000 + 0x10)
 #define ADF_DH895XCC_ERRSSMSH_EN BIT(3)
 
-#define ADF_DH895XCC_ERRSOU3	(0x3A000 + 0x00C)
-#define ADF_DH895XCC_ERRSOU5	(0x3A000 + 0x0D8)
 #define ADF_DH895XCC_PF2VF_OFFSET(i)	(0x3A000 + 0x280 + ((i) * 0x04))
 #define ADF_DH895XCC_VINTMSK_OFFSET(i)	(0x3A000 + 0x200 + ((i) * 0x04))
 /* FW names */
 #define ADF_DH895XCC_FW "qat_895xcc.bin"
-#define ADF_DH895XCC_MMP "qat_mmp.bin"
+#define ADF_DH895XCC_MMP "qat_895xcc_mmp.bin"
+
+void adf_init_hw_data_dh895xcc(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_dh895xcc(struct adf_hw_device_data *hw_data);
 #endif
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
index f8dd14f232c8..a8c4b92a7cbd 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_drv.c
@@ -60,11 +60,7 @@
 #include <adf_accel_devices.h>
 #include <adf_common_drv.h>
 #include <adf_cfg.h>
-#include <adf_transport_access_macros.h>
 #include "adf_dh895xcc_hw_data.h"
-#include "adf_drv.h"
-
-static const char adf_driver_name[] = ADF_DH895XCC_DEVICE_NAME;
 
 #define ADF_SYSTEM_DEVICE(device_id) \
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
@@ -80,7 +76,7 @@ static void adf_remove(struct pci_dev *dev);
 
 static struct pci_driver adf_driver = {
 	.id_table = adf_pci_tbl,
-	.name = adf_driver_name,
+	.name = ADF_DH895XCC_DEVICE_NAME,
 	.probe = adf_probe,
 	.remove = adf_remove,
 	.sriov_configure = adf_sriov_configure,
@@ -120,87 +116,6 @@ static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
 	adf_devmgr_rm_dev(accel_dev, NULL);
 }
 
-static int adf_dev_configure(struct adf_accel_dev *accel_dev)
-{
-	int cpus = num_online_cpus();
-	int banks = GET_MAX_BANKS(accel_dev);
-	int instances = min(cpus, banks);
-	char key[ADF_CFG_MAX_KEY_LEN_IN_BYTES];
-	int i;
-	unsigned long val;
-
-	if (adf_cfg_section_add(accel_dev, ADF_KERNEL_SEC))
-		goto err;
-	if (adf_cfg_section_add(accel_dev, "Accelerator0"))
-		goto err;
-	for (i = 0; i < instances; i++) {
-		val = i;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_BANK_NUM, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_ETRMGR_CORE_AFFINITY,
-			 i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_SIZE, i);
-		val = 128;
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = 512;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_SIZE, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = 0;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_TX, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = 2;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_TX, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = 8;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_RX, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = 10;
-		snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_RX, i);
-		if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-						key, (void *)&val, ADF_DEC))
-			goto err;
-
-		val = ADF_COALESCING_DEF_TIME;
-		snprintf(key, sizeof(key), ADF_ETRMGR_COALESCE_TIMER_FORMAT, i);
-		if (adf_cfg_add_key_value_param(accel_dev, "Accelerator0",
-						key, (void *)&val, ADF_DEC))
-			goto err;
-	}
-
-	val = i;
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					ADF_NUM_CY, (void *)&val, ADF_DEC))
-		goto err;
-
-	set_bit(ADF_STATUS_CONFIGURED, &accel_dev->status);
-	return 0;
-err:
-	dev_err(&GET_DEV(accel_dev), "Failed to start QAT accel dev\n");
-	return -EINVAL;
-}
-
 static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct adf_accel_dev *accel_dev;
@@ -253,15 +168,9 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	accel_dev->hw_device = hw_data;
-	switch (ent->device) {
-	case ADF_DH895XCC_PCI_DEVICE_ID:
-		adf_init_hw_data_dh895xcc(accel_dev->hw_device);
-		break;
-	default:
-		return -ENODEV;
-	}
+	adf_init_hw_data_dh895xcc(accel_dev->hw_device);
 	pci_read_config_byte(pdev, PCI_REVISION_ID, &accel_pci_dev->revid);
-	pci_read_config_dword(pdev, ADF_DH895XCC_FUSECTL_OFFSET,
+	pci_read_config_dword(pdev, ADF_DEVICE_FUSECTL_OFFSET,
 			      &hw_data->fuses);
 
 	/* Get Accelerators and Accelerators Engines masks */
@@ -316,13 +225,13 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
 	}
 
-	if (pci_request_regions(pdev, adf_driver_name)) {
+	if (pci_request_regions(pdev, ADF_DH895XCC_DEVICE_NAME)) {
 		ret = -EFAULT;
 		goto out_err_disable;
 	}
 
 	/* Read accelerator capabilities mask */
-	pci_read_config_dword(pdev, ADF_DH895XCC_LEGFUSE_OFFSET,
+	pci_read_config_dword(pdev, ADF_DEVICE_LEGFUSE_OFFSET,
 			      &hw_data->accel_capabilities_mask);
 
 	/* Find and map all the device's BARS */
@@ -357,7 +266,7 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_err_free_reg;
 	}
 
-	ret = adf_dev_configure(accel_dev);
+	ret = qat_crypto_dev_config(accel_dev);
 	if (ret)
 		goto out_err_free_reg;
 
diff --git a/drivers/crypto/qat/qat_dh895xccvf/Makefile b/drivers/crypto/qat/qat_dh895xccvf/Makefile
index 85399fcbbad4..5c3ccf8267eb 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/Makefile
+++ b/drivers/crypto/qat/qat_dh895xccvf/Makefile
@@ -1,5 +1,3 @@
 ccflags-y := -I$(src)/../qat_common
 obj-$(CONFIG_CRYPTO_DEV_QAT_DH895xCCVF) += qat_dh895xccvf.o
-qat_dh895xccvf-objs := adf_drv.o \
-		adf_isr.o \
-		adf_dh895xccvf_hw_data.o
+qat_dh895xccvf-objs := adf_drv.o adf_dh895xccvf_hw_data.o
diff --git a/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.c b/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.c
index a9a27eff41fb..dc04ab68d24d 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.c
+++ b/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.c
@@ -48,7 +48,6 @@
 #include <adf_pf2vf_msg.h>
 #include <adf_common_drv.h>
 #include "adf_dh895xccvf_hw_data.h"
-#include "adf_drv.h"
 
 static struct adf_hw_device_class dh895xcciov_class = {
 	.name = ADF_DH895XCCVF_DEVICE_NAME,
@@ -136,7 +135,6 @@ static void adf_vf2pf_shutdown(struct adf_accel_dev *accel_dev)
 void adf_init_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data)
 {
 	hw_data->dev_class = &dh895xcciov_class;
-	hw_data->instance_id = dh895xcciov_class.instances++;
 	hw_data->num_banks = ADF_DH895XCCIOV_ETR_MAX_BANKS;
 	hw_data->num_accel = ADF_DH895XCCIOV_MAX_ACCELERATORS;
 	hw_data->num_logical_accel = 1;
@@ -164,9 +162,12 @@ void adf_init_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data)
 	hw_data->enable_ints = adf_vf_void_noop;
 	hw_data->enable_vf2pf_comms = adf_enable_vf2pf_comms;
 	hw_data->min_iov_compat_ver = ADF_PFVF_COMPATIBILITY_VERSION;
+	hw_data->dev_class->instances++;
+	adf_devmgr_update_class_index(hw_data);
 }
 
 void adf_clean_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data)
 {
 	hw_data->dev_class->instances--;
+	adf_devmgr_update_class_index(hw_data);
 }
diff --git a/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.h b/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.h
index 8f6babfef629..6ddc19bd4410 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.h
+++ b/drivers/crypto/qat/qat_dh895xccvf/adf_dh895xccvf_hw_data.h
@@ -56,13 +56,9 @@
 #define ADF_DH895XCCIOV_TX_RINGS_MASK 0xFF
 #define ADF_DH895XCCIOV_ETR_BAR 0
 #define ADF_DH895XCCIOV_ETR_MAX_BANKS 1
-
 #define ADF_DH895XCCIOV_PF2VF_OFFSET	0x200
-#define ADF_DH895XCC_PF2VF_PF2VFINT	BIT(0)
-
-#define ADF_DH895XCCIOV_VINTSOU_OFFSET	0x204
-#define ADF_DH895XCC_VINTSOU_BUN	BIT(0)
-#define ADF_DH895XCC_VINTSOU_PF2VF	BIT(1)
-
 #define ADF_DH895XCCIOV_VINTMSK_OFFSET	0x208
+
+void adf_init_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data);
+void adf_clean_hw_data_dh895xcciov(struct adf_hw_device_data *hw_data);
 #endif
diff --git a/drivers/crypto/qat/qat_dh895xccvf/adf_drv.c b/drivers/crypto/qat/qat_dh895xccvf/adf_drv.c
index 789426f21882..f8cc4bf0a50c 100644
--- a/drivers/crypto/qat/qat_dh895xccvf/adf_drv.c
+++ b/drivers/crypto/qat/qat_dh895xccvf/adf_drv.c
@@ -60,11 +60,7 @@
 #include <adf_accel_devices.h>
 #include <adf_common_drv.h>
 #include <adf_cfg.h>
-#include <adf_transport_access_macros.h>
 #include "adf_dh895xccvf_hw_data.h"
-#include "adf_drv.h"
-
-static const char adf_driver_name[] = ADF_DH895XCCVF_DEVICE_NAME;
 
 #define ADF_SYSTEM_DEVICE(device_id) \
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, device_id)}
@@ -80,7 +76,7 @@ static void adf_remove(struct pci_dev *dev);
 
 static struct pci_driver adf_driver = {
 	.id_table = adf_pci_tbl,
-	.name = adf_driver_name,
+	.name = ADF_DH895XCCVF_DEVICE_NAME,
 	.probe = adf_probe,
 	.remove = adf_remove,
 };
@@ -121,83 +117,6 @@ static void adf_cleanup_accel(struct adf_accel_dev *accel_dev)
 	adf_devmgr_rm_dev(accel_dev, pf);
 }
 
-static int adf_dev_configure(struct adf_accel_dev *accel_dev)
-{
-	char key[ADF_CFG_MAX_KEY_LEN_IN_BYTES];
-	unsigned long val, bank = 0;
-
-	if (adf_cfg_section_add(accel_dev, ADF_KERNEL_SEC))
-		goto err;
-	if (adf_cfg_section_add(accel_dev, "Accelerator0"))
-		goto err;
-
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_BANK_NUM, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC, key,
-					(void *)&bank, ADF_DEC))
-		goto err;
-
-	val = bank;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_ETRMGR_CORE_AFFINITY, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC, key,
-					(void *)&val, ADF_DEC))
-		goto err;
-
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_SIZE, 0);
-
-	val = 128;
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC, key,
-					(void *)&val, ADF_DEC))
-		goto err;
-
-	val = 512;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_SIZE, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					key, (void *)&val, ADF_DEC))
-		goto err;
-
-	val = 0;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_TX, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					key, (void *)&val, ADF_DEC))
-		goto err;
-
-	val = 2;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_TX, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					key, (void *)&val, ADF_DEC))
-		goto err;
-
-	val = 8;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_ASYM_RX, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					key, (void *)&val, ADF_DEC))
-		goto err;
-
-	val = 10;
-	snprintf(key, sizeof(key), ADF_CY "%d" ADF_RING_SYM_RX, 0);
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					key, (void *)&val, ADF_DEC))
-			goto err;
-
-	val = ADF_COALESCING_DEF_TIME;
-	snprintf(key, sizeof(key), ADF_ETRMGR_COALESCE_TIMER_FORMAT,
-		 (int)bank);
-	if (adf_cfg_add_key_value_param(accel_dev, "Accelerator0",
-					key, (void *)&val, ADF_DEC))
-		goto err;
-
-	val = 1;
-	if (adf_cfg_add_key_value_param(accel_dev, ADF_KERNEL_SEC,
-					ADF_NUM_CY, (void *)&val, ADF_DEC))
-		goto err;
-
-	set_bit(ADF_STATUS_CONFIGURED, &accel_dev->status);
-	return 0;
-err:
-	dev_err(&GET_DEV(accel_dev), "Failed to configure QAT accel dev\n");
-	return -EINVAL;
-}
-
 static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct adf_accel_dev *accel_dev;
@@ -243,14 +162,7 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_err;
 	}
 	accel_dev->hw_device = hw_data;
-	switch (ent->device) {
-	case ADF_DH895XCCIOV_PCI_DEVICE_ID:
-		adf_init_hw_data_dh895xcciov(accel_dev->hw_device);
-		break;
-	default:
-		ret = -ENODEV;
-		goto out_err;
-	}
+	adf_init_hw_data_dh895xcciov(accel_dev->hw_device);
 
 	/* Get Accelerators and Accelerators Engines masks */
 	hw_data->accel_mask = hw_data->get_accel_mask(hw_data->fuses);
@@ -295,7 +207,7 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
 	}
 
-	if (pci_request_regions(pdev, adf_driver_name)) {
+	if (pci_request_regions(pdev, ADF_DH895XCCVF_DEVICE_NAME)) {
 		ret = -EFAULT;
 		goto out_err_disable;
 	}
@@ -322,7 +234,7 @@ static int adf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* Completion for VF2PF request/response message exchange */
 	init_completion(&accel_dev->vf.iov_msg_completion);
 
-	ret = adf_dev_configure(accel_dev);
+	ret = qat_crypto_dev_config(accel_dev);
 	if (ret)
 		goto out_err_free_reg;
 
diff --git a/drivers/crypto/qce/ablkcipher.c b/drivers/crypto/qce/ablkcipher.c
index 2c0d63d48747..dbcbbe242bd6 100644
--- a/drivers/crypto/qce/ablkcipher.c
+++ b/drivers/crypto/qce/ablkcipher.c
@@ -83,6 +83,14 @@ qce_ablkcipher_async_req_handle(struct crypto_async_request *async_req)
 		rctx->dst_nents = sg_nents_for_len(req->dst, req->nbytes);
 	else
 		rctx->dst_nents = rctx->src_nents;
+	if (rctx->src_nents < 0) {
+		dev_err(qce->dev, "Invalid numbers of src SG.\n");
+		return rctx->src_nents;
+	}
+	if (rctx->dst_nents < 0) {
+		dev_err(qce->dev, "Invalid numbers of dst SG.\n");
+		return -rctx->dst_nents;
+	}
 
 	rctx->dst_nents += 1;
 
diff --git a/drivers/crypto/qce/sha.c b/drivers/crypto/qce/sha.c
index 0c9973ec80eb..47e114ac09d0 100644
--- a/drivers/crypto/qce/sha.c
+++ b/drivers/crypto/qce/sha.c
@@ -92,6 +92,11 @@ static int qce_ahash_async_req_handle(struct crypto_async_request *async_req)
 	}
 
 	rctx->src_nents = sg_nents_for_len(req->src, req->nbytes);
+	if (rctx->src_nents < 0) {
+		dev_err(qce->dev, "Invalid numbers of src SG.\n");
+		return rctx->src_nents;
+	}
+
 	ret = dma_map_sg(qce->dev, req->src, rctx->src_nents, DMA_TO_DEVICE);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/crypto/rockchip/Makefile b/drivers/crypto/rockchip/Makefile
new file mode 100644
index 000000000000..7051c6c715f3
--- /dev/null
+++ b/drivers/crypto/rockchip/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_CRYPTO_DEV_ROCKCHIP) += rk_crypto.o
+rk_crypto-objs := rk3288_crypto.o \
+		  rk3288_crypto_ablkcipher.o \
diff --git a/drivers/crypto/rockchip/rk3288_crypto.c b/drivers/crypto/rockchip/rk3288_crypto.c
new file mode 100644
index 000000000000..da9c73dce4af
--- /dev/null
+++ b/drivers/crypto/rockchip/rk3288_crypto.c
@@ -0,0 +1,394 @@
+/*
+ * Crypto acceleration support for Rockchip RK3288
+ *
+ * Copyright (c) 2015, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author: Zain Wang <zain.wang@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.
+ *
+ * Some ideas are from marvell-cesa.c and s5p-sss.c driver.
+ */
+
+#include "rk3288_crypto.h"
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/crypto.h>
+#include <linux/reset.h>
+
+static int rk_crypto_enable_clk(struct rk_crypto_info *dev)
+{
+	int err;
+
+	err = clk_prepare_enable(dev->sclk);
+	if (err) {
+		dev_err(dev->dev, "[%s:%d], Couldn't enable clock sclk\n",
+			__func__, __LINE__);
+		goto err_return;
+	}
+	err = clk_prepare_enable(dev->aclk);
+	if (err) {
+		dev_err(dev->dev, "[%s:%d], Couldn't enable clock aclk\n",
+			__func__, __LINE__);
+		goto err_aclk;
+	}
+	err = clk_prepare_enable(dev->hclk);
+	if (err) {
+		dev_err(dev->dev, "[%s:%d], Couldn't enable clock hclk\n",
+			__func__, __LINE__);
+		goto err_hclk;
+	}
+	err = clk_prepare_enable(dev->dmaclk);
+	if (err) {
+		dev_err(dev->dev, "[%s:%d], Couldn't enable clock dmaclk\n",
+			__func__, __LINE__);
+		goto err_dmaclk;
+	}
+	return err;
+err_dmaclk:
+	clk_disable_unprepare(dev->hclk);
+err_hclk:
+	clk_disable_unprepare(dev->aclk);
+err_aclk:
+	clk_disable_unprepare(dev->sclk);
+err_return:
+	return err;
+}
+
+static void rk_crypto_disable_clk(struct rk_crypto_info *dev)
+{
+	clk_disable_unprepare(dev->dmaclk);
+	clk_disable_unprepare(dev->hclk);
+	clk_disable_unprepare(dev->aclk);
+	clk_disable_unprepare(dev->sclk);
+}
+
+static int check_alignment(struct scatterlist *sg_src,
+			   struct scatterlist *sg_dst,
+			   int align_mask)
+{
+	int in, out, align;
+
+	in = IS_ALIGNED((uint32_t)sg_src->offset, 4) &&
+	     IS_ALIGNED((uint32_t)sg_src->length, align_mask);
+	if (!sg_dst)
+		return in;
+	out = IS_ALIGNED((uint32_t)sg_dst->offset, 4) &&
+	      IS_ALIGNED((uint32_t)sg_dst->length, align_mask);
+	align = in && out;
+
+	return (align && (sg_src->length == sg_dst->length));
+}
+
+static int rk_load_data(struct rk_crypto_info *dev,
+			struct scatterlist *sg_src,
+			struct scatterlist *sg_dst)
+{
+	unsigned int count;
+
+	dev->aligned = dev->aligned ?
+		check_alignment(sg_src, sg_dst, dev->align_size) :
+		dev->aligned;
+	if (dev->aligned) {
+		count = min(dev->left_bytes, sg_src->length);
+		dev->left_bytes -= count;
+
+		if (!dma_map_sg(dev->dev, sg_src, 1, DMA_TO_DEVICE)) {
+			dev_err(dev->dev, "[%s:%d] dma_map_sg(src)  error\n",
+				__func__, __LINE__);
+			return -EINVAL;
+		}
+		dev->addr_in = sg_dma_address(sg_src);
+
+		if (sg_dst) {
+			if (!dma_map_sg(dev->dev, sg_dst, 1, DMA_FROM_DEVICE)) {
+				dev_err(dev->dev,
+					"[%s:%d] dma_map_sg(dst)  error\n",
+					__func__, __LINE__);
+				dma_unmap_sg(dev->dev, sg_src, 1,
+					     DMA_TO_DEVICE);
+				return -EINVAL;
+			}
+			dev->addr_out = sg_dma_address(sg_dst);
+		}
+	} else {
+		count = (dev->left_bytes > PAGE_SIZE) ?
+			PAGE_SIZE : dev->left_bytes;
+
+		if (!sg_pcopy_to_buffer(dev->first, dev->nents,
+					dev->addr_vir, count,
+					dev->total - dev->left_bytes)) {
+			dev_err(dev->dev, "[%s:%d] pcopy err\n",
+				__func__, __LINE__);
+			return -EINVAL;
+		}
+		dev->left_bytes -= count;
+		sg_init_one(&dev->sg_tmp, dev->addr_vir, count);
+		if (!dma_map_sg(dev->dev, &dev->sg_tmp, 1, DMA_TO_DEVICE)) {
+			dev_err(dev->dev, "[%s:%d] dma_map_sg(sg_tmp)  error\n",
+				__func__, __LINE__);
+			return -ENOMEM;
+		}
+		dev->addr_in = sg_dma_address(&dev->sg_tmp);
+
+		if (sg_dst) {
+			if (!dma_map_sg(dev->dev, &dev->sg_tmp, 1,
+					DMA_FROM_DEVICE)) {
+				dev_err(dev->dev,
+					"[%s:%d] dma_map_sg(sg_tmp)  error\n",
+					__func__, __LINE__);
+				dma_unmap_sg(dev->dev, &dev->sg_tmp, 1,
+					     DMA_TO_DEVICE);
+				return -ENOMEM;
+			}
+			dev->addr_out = sg_dma_address(&dev->sg_tmp);
+		}
+	}
+	dev->count = count;
+	return 0;
+}
+
+static void rk_unload_data(struct rk_crypto_info *dev)
+{
+	struct scatterlist *sg_in, *sg_out;
+
+	sg_in = dev->aligned ? dev->sg_src : &dev->sg_tmp;
+	dma_unmap_sg(dev->dev, sg_in, 1, DMA_TO_DEVICE);
+
+	if (dev->sg_dst) {
+		sg_out = dev->aligned ? dev->sg_dst : &dev->sg_tmp;
+		dma_unmap_sg(dev->dev, sg_out, 1, DMA_FROM_DEVICE);
+	}
+}
+
+static irqreturn_t rk_crypto_irq_handle(int irq, void *dev_id)
+{
+	struct rk_crypto_info *dev  = platform_get_drvdata(dev_id);
+	u32 interrupt_status;
+	int err = 0;
+
+	spin_lock(&dev->lock);
+	interrupt_status = CRYPTO_READ(dev, RK_CRYPTO_INTSTS);
+	CRYPTO_WRITE(dev, RK_CRYPTO_INTSTS, interrupt_status);
+	if (interrupt_status & 0x0a) {
+		dev_warn(dev->dev, "DMA Error\n");
+		err = -EFAULT;
+	} else if (interrupt_status & 0x05) {
+		err = dev->update(dev);
+	}
+	if (err)
+		dev->complete(dev, err);
+	spin_unlock(&dev->lock);
+	return IRQ_HANDLED;
+}
+
+static void rk_crypto_tasklet_cb(unsigned long data)
+{
+	struct rk_crypto_info *dev = (struct rk_crypto_info *)data;
+	struct crypto_async_request *async_req, *backlog;
+	unsigned long flags;
+	int err = 0;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	backlog   = crypto_get_backlog(&dev->queue);
+	async_req = crypto_dequeue_request(&dev->queue);
+	spin_unlock_irqrestore(&dev->lock, flags);
+	if (!async_req) {
+		dev_err(dev->dev, "async_req is NULL !!\n");
+		return;
+	}
+	if (backlog) {
+		backlog->complete(backlog, -EINPROGRESS);
+		backlog = NULL;
+	}
+
+	if (crypto_tfm_alg_type(async_req->tfm) == CRYPTO_ALG_TYPE_ABLKCIPHER)
+		dev->ablk_req = ablkcipher_request_cast(async_req);
+	err = dev->start(dev);
+	if (err)
+		dev->complete(dev, err);
+}
+
+static struct rk_crypto_tmp *rk_cipher_algs[] = {
+	&rk_ecb_aes_alg,
+	&rk_cbc_aes_alg,
+	&rk_ecb_des_alg,
+	&rk_cbc_des_alg,
+	&rk_ecb_des3_ede_alg,
+	&rk_cbc_des3_ede_alg,
+};
+
+static int rk_crypto_register(struct rk_crypto_info *crypto_info)
+{
+	unsigned int i, k;
+	int err = 0;
+
+	for (i = 0; i < ARRAY_SIZE(rk_cipher_algs); i++) {
+		rk_cipher_algs[i]->dev = crypto_info;
+		err = crypto_register_alg(&rk_cipher_algs[i]->alg);
+		if (err)
+			goto err_cipher_algs;
+	}
+	return 0;
+
+err_cipher_algs:
+	for (k = 0; k < i; k++)
+		crypto_unregister_alg(&rk_cipher_algs[k]->alg);
+	return err;
+}
+
+static void rk_crypto_unregister(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(rk_cipher_algs); i++)
+		crypto_unregister_alg(&rk_cipher_algs[i]->alg);
+}
+
+static void rk_crypto_action(void *data)
+{
+	struct rk_crypto_info *crypto_info = data;
+
+	reset_control_assert(crypto_info->rst);
+}
+
+static const struct of_device_id crypto_of_id_table[] = {
+	{ .compatible = "rockchip,rk3288-crypto" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, crypto_of_id_table);
+
+static int rk_crypto_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	struct rk_crypto_info *crypto_info;
+	int err = 0;
+
+	crypto_info = devm_kzalloc(&pdev->dev,
+				   sizeof(*crypto_info), GFP_KERNEL);
+	if (!crypto_info) {
+		err = -ENOMEM;
+		goto err_crypto;
+	}
+
+	crypto_info->rst = devm_reset_control_get(dev, "crypto-rst");
+	if (IS_ERR(crypto_info->rst)) {
+		err = PTR_ERR(crypto_info->rst);
+		goto err_crypto;
+	}
+
+	reset_control_assert(crypto_info->rst);
+	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);
+		goto err_crypto;
+	}
+
+	spin_lock_init(&crypto_info->lock);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	crypto_info->reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(crypto_info->reg)) {
+		err = PTR_ERR(crypto_info->reg);
+		goto err_crypto;
+	}
+
+	crypto_info->aclk = devm_clk_get(&pdev->dev, "aclk");
+	if (IS_ERR(crypto_info->aclk)) {
+		err = PTR_ERR(crypto_info->aclk);
+		goto err_crypto;
+	}
+
+	crypto_info->hclk = devm_clk_get(&pdev->dev, "hclk");
+	if (IS_ERR(crypto_info->hclk)) {
+		err = PTR_ERR(crypto_info->hclk);
+		goto err_crypto;
+	}
+
+	crypto_info->sclk = devm_clk_get(&pdev->dev, "sclk");
+	if (IS_ERR(crypto_info->sclk)) {
+		err = PTR_ERR(crypto_info->sclk);
+		goto err_crypto;
+	}
+
+	crypto_info->dmaclk = devm_clk_get(&pdev->dev, "apb_pclk");
+	if (IS_ERR(crypto_info->dmaclk)) {
+		err = PTR_ERR(crypto_info->dmaclk);
+		goto err_crypto;
+	}
+
+	crypto_info->irq = platform_get_irq(pdev, 0);
+	if (crypto_info->irq < 0) {
+		dev_warn(crypto_info->dev,
+			 "control Interrupt is not available.\n");
+		err = crypto_info->irq;
+		goto err_crypto;
+	}
+
+	err = devm_request_irq(&pdev->dev, crypto_info->irq,
+			       rk_crypto_irq_handle, IRQF_SHARED,
+			       "rk-crypto", pdev);
+
+	if (err) {
+		dev_err(crypto_info->dev, "irq request failed.\n");
+		goto err_crypto;
+	}
+
+	crypto_info->dev = &pdev->dev;
+	platform_set_drvdata(pdev, crypto_info);
+
+	tasklet_init(&crypto_info->crypto_tasklet,
+		     rk_crypto_tasklet_cb, (unsigned long)crypto_info);
+	crypto_init_queue(&crypto_info->queue, 50);
+
+	crypto_info->enable_clk = rk_crypto_enable_clk;
+	crypto_info->disable_clk = rk_crypto_disable_clk;
+	crypto_info->load_data = rk_load_data;
+	crypto_info->unload_data = rk_unload_data;
+
+	err = rk_crypto_register(crypto_info);
+	if (err) {
+		dev_err(dev, "err in register alg");
+		goto err_register_alg;
+	}
+
+	dev_info(dev, "Crypto Accelerator successfully registered\n");
+	return 0;
+
+err_register_alg:
+	tasklet_kill(&crypto_info->crypto_tasklet);
+err_crypto:
+	return err;
+}
+
+static int rk_crypto_remove(struct platform_device *pdev)
+{
+	struct rk_crypto_info *crypto_tmp = platform_get_drvdata(pdev);
+
+	rk_crypto_unregister();
+	tasklet_kill(&crypto_tmp->crypto_tasklet);
+	return 0;
+}
+
+static struct platform_driver crypto_driver = {
+	.probe		= rk_crypto_probe,
+	.remove		= rk_crypto_remove,
+	.driver		= {
+		.name	= "rk3288-crypto",
+		.of_match_table	= crypto_of_id_table,
+	},
+};
+
+module_platform_driver(crypto_driver);
+
+MODULE_AUTHOR("Zain Wang <zain.wang@rock-chips.com>");
+MODULE_DESCRIPTION("Support for Rockchip's cryptographic engine");
+MODULE_LICENSE("GPL");
diff --git a/drivers/crypto/rockchip/rk3288_crypto.h b/drivers/crypto/rockchip/rk3288_crypto.h
new file mode 100644
index 000000000000..e499c2c6c903
--- /dev/null
+++ b/drivers/crypto/rockchip/rk3288_crypto.h
@@ -0,0 +1,216 @@
+#ifndef __RK3288_CRYPTO_H__
+#define __RK3288_CRYPTO_H__
+
+#include <crypto/aes.h>
+#include <crypto/des.h>
+#include <crypto/algapi.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+
+#define _SBF(v, f)			((v) << (f))
+
+/* Crypto control registers*/
+#define RK_CRYPTO_INTSTS		0x0000
+#define RK_CRYPTO_PKA_DONE_INT		BIT(5)
+#define RK_CRYPTO_HASH_DONE_INT		BIT(4)
+#define RK_CRYPTO_HRDMA_ERR_INT		BIT(3)
+#define RK_CRYPTO_HRDMA_DONE_INT	BIT(2)
+#define RK_CRYPTO_BCDMA_ERR_INT		BIT(1)
+#define RK_CRYPTO_BCDMA_DONE_INT	BIT(0)
+
+#define RK_CRYPTO_INTENA		0x0004
+#define RK_CRYPTO_PKA_DONE_ENA		BIT(5)
+#define RK_CRYPTO_HASH_DONE_ENA		BIT(4)
+#define RK_CRYPTO_HRDMA_ERR_ENA		BIT(3)
+#define RK_CRYPTO_HRDMA_DONE_ENA	BIT(2)
+#define RK_CRYPTO_BCDMA_ERR_ENA		BIT(1)
+#define RK_CRYPTO_BCDMA_DONE_ENA	BIT(0)
+
+#define RK_CRYPTO_CTRL			0x0008
+#define RK_CRYPTO_WRITE_MASK		_SBF(0xFFFF, 16)
+#define RK_CRYPTO_TRNG_FLUSH		BIT(9)
+#define RK_CRYPTO_TRNG_START		BIT(8)
+#define RK_CRYPTO_PKA_FLUSH		BIT(7)
+#define RK_CRYPTO_HASH_FLUSH		BIT(6)
+#define RK_CRYPTO_BLOCK_FLUSH		BIT(5)
+#define RK_CRYPTO_PKA_START		BIT(4)
+#define RK_CRYPTO_HASH_START		BIT(3)
+#define RK_CRYPTO_BLOCK_START		BIT(2)
+#define RK_CRYPTO_TDES_START		BIT(1)
+#define RK_CRYPTO_AES_START		BIT(0)
+
+#define RK_CRYPTO_CONF			0x000c
+/* HASH Receive DMA Address Mode:   fix | increment */
+#define RK_CRYPTO_HR_ADDR_MODE		BIT(8)
+/* Block Transmit DMA Address Mode: fix | increment */
+#define RK_CRYPTO_BT_ADDR_MODE		BIT(7)
+/* Block Receive DMA Address Mode:  fix | increment */
+#define RK_CRYPTO_BR_ADDR_MODE		BIT(6)
+#define RK_CRYPTO_BYTESWAP_HRFIFO	BIT(5)
+#define RK_CRYPTO_BYTESWAP_BTFIFO	BIT(4)
+#define RK_CRYPTO_BYTESWAP_BRFIFO	BIT(3)
+/* AES = 0 OR DES = 1 */
+#define RK_CRYPTO_DESSEL				BIT(2)
+#define RK_CYYPTO_HASHINSEL_INDEPENDENT_SOURCE		_SBF(0x00, 0)
+#define RK_CYYPTO_HASHINSEL_BLOCK_CIPHER_INPUT		_SBF(0x01, 0)
+#define RK_CYYPTO_HASHINSEL_BLOCK_CIPHER_OUTPUT		_SBF(0x02, 0)
+
+/* Block Receiving DMA Start Address Register */
+#define RK_CRYPTO_BRDMAS		0x0010
+/* Block Transmitting DMA Start Address Register */
+#define RK_CRYPTO_BTDMAS		0x0014
+/* Block Receiving DMA Length Register */
+#define RK_CRYPTO_BRDMAL		0x0018
+/* Hash Receiving DMA Start Address Register */
+#define RK_CRYPTO_HRDMAS		0x001c
+/* Hash Receiving DMA Length Register */
+#define RK_CRYPTO_HRDMAL		0x0020
+
+/* AES registers */
+#define RK_CRYPTO_AES_CTRL			  0x0080
+#define RK_CRYPTO_AES_BYTESWAP_CNT	BIT(11)
+#define RK_CRYPTO_AES_BYTESWAP_KEY	BIT(10)
+#define RK_CRYPTO_AES_BYTESWAP_IV	BIT(9)
+#define RK_CRYPTO_AES_BYTESWAP_DO	BIT(8)
+#define RK_CRYPTO_AES_BYTESWAP_DI	BIT(7)
+#define RK_CRYPTO_AES_KEY_CHANGE	BIT(6)
+#define RK_CRYPTO_AES_ECB_MODE		_SBF(0x00, 4)
+#define RK_CRYPTO_AES_CBC_MODE		_SBF(0x01, 4)
+#define RK_CRYPTO_AES_CTR_MODE		_SBF(0x02, 4)
+#define RK_CRYPTO_AES_128BIT_key	_SBF(0x00, 2)
+#define RK_CRYPTO_AES_192BIT_key	_SBF(0x01, 2)
+#define RK_CRYPTO_AES_256BIT_key	_SBF(0x02, 2)
+/* Slave = 0 / fifo = 1 */
+#define RK_CRYPTO_AES_FIFO_MODE		BIT(1)
+/* Encryption = 0 , Decryption = 1 */
+#define RK_CRYPTO_AES_DEC		BIT(0)
+
+#define RK_CRYPTO_AES_STS		0x0084
+#define RK_CRYPTO_AES_DONE		BIT(0)
+
+/* AES Input Data 0-3 Register */
+#define RK_CRYPTO_AES_DIN_0		0x0088
+#define RK_CRYPTO_AES_DIN_1		0x008c
+#define RK_CRYPTO_AES_DIN_2		0x0090
+#define RK_CRYPTO_AES_DIN_3		0x0094
+
+/* AES output Data 0-3 Register */
+#define RK_CRYPTO_AES_DOUT_0		0x0098
+#define RK_CRYPTO_AES_DOUT_1		0x009c
+#define RK_CRYPTO_AES_DOUT_2		0x00a0
+#define RK_CRYPTO_AES_DOUT_3		0x00a4
+
+/* AES IV Data 0-3 Register */
+#define RK_CRYPTO_AES_IV_0		0x00a8
+#define RK_CRYPTO_AES_IV_1		0x00ac
+#define RK_CRYPTO_AES_IV_2		0x00b0
+#define RK_CRYPTO_AES_IV_3		0x00b4
+
+/* AES Key Data 0-3 Register */
+#define RK_CRYPTO_AES_KEY_0		0x00b8
+#define RK_CRYPTO_AES_KEY_1		0x00bc
+#define RK_CRYPTO_AES_KEY_2		0x00c0
+#define RK_CRYPTO_AES_KEY_3		0x00c4
+#define RK_CRYPTO_AES_KEY_4		0x00c8
+#define RK_CRYPTO_AES_KEY_5		0x00cc
+#define RK_CRYPTO_AES_KEY_6		0x00d0
+#define RK_CRYPTO_AES_KEY_7		0x00d4
+
+/* des/tdes */
+#define RK_CRYPTO_TDES_CTRL		0x0100
+#define RK_CRYPTO_TDES_BYTESWAP_KEY	BIT(8)
+#define RK_CRYPTO_TDES_BYTESWAP_IV	BIT(7)
+#define RK_CRYPTO_TDES_BYTESWAP_DO	BIT(6)
+#define RK_CRYPTO_TDES_BYTESWAP_DI	BIT(5)
+/* 0: ECB, 1: CBC */
+#define RK_CRYPTO_TDES_CHAINMODE_CBC	BIT(4)
+/* TDES Key Mode, 0 : EDE, 1 : EEE */
+#define RK_CRYPTO_TDES_EEE		BIT(3)
+/* 0: DES, 1:TDES */
+#define RK_CRYPTO_TDES_SELECT		BIT(2)
+/* 0: Slave, 1:Fifo */
+#define RK_CRYPTO_TDES_FIFO_MODE	BIT(1)
+/* Encryption = 0 , Decryption = 1 */
+#define RK_CRYPTO_TDES_DEC		BIT(0)
+
+#define RK_CRYPTO_TDES_STS		0x0104
+#define RK_CRYPTO_TDES_DONE		BIT(0)
+
+#define RK_CRYPTO_TDES_DIN_0		0x0108
+#define RK_CRYPTO_TDES_DIN_1		0x010c
+#define RK_CRYPTO_TDES_DOUT_0		0x0110
+#define RK_CRYPTO_TDES_DOUT_1		0x0114
+#define RK_CRYPTO_TDES_IV_0		0x0118
+#define RK_CRYPTO_TDES_IV_1		0x011c
+#define RK_CRYPTO_TDES_KEY1_0		0x0120
+#define RK_CRYPTO_TDES_KEY1_1		0x0124
+#define RK_CRYPTO_TDES_KEY2_0		0x0128
+#define RK_CRYPTO_TDES_KEY2_1		0x012c
+#define RK_CRYPTO_TDES_KEY3_0		0x0130
+#define RK_CRYPTO_TDES_KEY3_1		0x0134
+
+#define CRYPTO_READ(dev, offset)		  \
+		readl_relaxed(((dev)->reg + (offset)))
+#define CRYPTO_WRITE(dev, offset, val)	  \
+		writel_relaxed((val), ((dev)->reg + (offset)))
+
+struct rk_crypto_info {
+	struct device			*dev;
+	struct clk			*aclk;
+	struct clk			*hclk;
+	struct clk			*sclk;
+	struct clk			*dmaclk;
+	struct reset_control		*rst;
+	void __iomem			*reg;
+	int				irq;
+	struct crypto_queue		queue;
+	struct tasklet_struct		crypto_tasklet;
+	struct ablkcipher_request	*ablk_req;
+	/* device lock */
+	spinlock_t			lock;
+
+	/* the public variable */
+	struct scatterlist		*sg_src;
+	struct scatterlist		*sg_dst;
+	struct scatterlist		sg_tmp;
+	struct scatterlist		*first;
+	unsigned int			left_bytes;
+	void				*addr_vir;
+	int				aligned;
+	int				align_size;
+	size_t				nents;
+	unsigned int			total;
+	unsigned int			count;
+	u32				mode;
+	dma_addr_t			addr_in;
+	dma_addr_t			addr_out;
+	int (*start)(struct rk_crypto_info *dev);
+	int (*update)(struct rk_crypto_info *dev);
+	void (*complete)(struct rk_crypto_info *dev, int err);
+	int (*enable_clk)(struct rk_crypto_info *dev);
+	void (*disable_clk)(struct rk_crypto_info *dev);
+	int (*load_data)(struct rk_crypto_info *dev,
+			 struct scatterlist *sg_src,
+			 struct scatterlist *sg_dst);
+	void (*unload_data)(struct rk_crypto_info *dev);
+};
+
+/* the private variable of cipher */
+struct rk_cipher_ctx {
+	struct rk_crypto_info		*dev;
+	unsigned int			keylen;
+};
+
+struct rk_crypto_tmp {
+	struct rk_crypto_info *dev;
+	struct crypto_alg alg;
+};
+
+extern struct rk_crypto_tmp rk_ecb_aes_alg;
+extern struct rk_crypto_tmp rk_cbc_aes_alg;
+extern struct rk_crypto_tmp rk_ecb_des_alg;
+extern struct rk_crypto_tmp rk_cbc_des_alg;
+extern struct rk_crypto_tmp rk_ecb_des3_ede_alg;
+extern struct rk_crypto_tmp rk_cbc_des3_ede_alg;
+
+#endif
diff --git a/drivers/crypto/rockchip/rk3288_crypto_ablkcipher.c b/drivers/crypto/rockchip/rk3288_crypto_ablkcipher.c
new file mode 100644
index 000000000000..d98b681f6c06
--- /dev/null
+++ b/drivers/crypto/rockchip/rk3288_crypto_ablkcipher.c
@@ -0,0 +1,505 @@
+/*
+ * Crypto acceleration support for Rockchip RK3288
+ *
+ * Copyright (c) 2015, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author: Zain Wang <zain.wang@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.
+ *
+ * Some ideas are from marvell-cesa.c and s5p-sss.c driver.
+ */
+#include "rk3288_crypto.h"
+
+#define RK_CRYPTO_DEC			BIT(0)
+
+static void rk_crypto_complete(struct rk_crypto_info *dev, int err)
+{
+	if (dev->ablk_req->base.complete)
+		dev->ablk_req->base.complete(&dev->ablk_req->base, err);
+}
+
+static int rk_handle_req(struct rk_crypto_info *dev,
+			 struct ablkcipher_request *req)
+{
+	unsigned long flags;
+	int err;
+
+	if (!IS_ALIGNED(req->nbytes, dev->align_size))
+		return -EINVAL;
+
+	dev->left_bytes = req->nbytes;
+	dev->total = req->nbytes;
+	dev->sg_src = req->src;
+	dev->first = req->src;
+	dev->nents = sg_nents(req->src);
+	dev->sg_dst = req->dst;
+	dev->aligned = 1;
+	dev->ablk_req = req;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	err = ablkcipher_enqueue_request(&dev->queue, req);
+	spin_unlock_irqrestore(&dev->lock, flags);
+	tasklet_schedule(&dev->crypto_tasklet);
+	return err;
+}
+
+static int rk_aes_setkey(struct crypto_ablkcipher *cipher,
+			 const u8 *key, unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct rk_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	if (keylen != AES_KEYSIZE_128 && keylen != AES_KEYSIZE_192 &&
+	    keylen != AES_KEYSIZE_256) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+	ctx->keylen = keylen;
+	memcpy_toio(ctx->dev->reg + RK_CRYPTO_AES_KEY_0, key, keylen);
+	return 0;
+}
+
+static int rk_tdes_setkey(struct crypto_ablkcipher *cipher,
+			  const u8 *key, unsigned int keylen)
+{
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct rk_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	u32 tmp[DES_EXPKEY_WORDS];
+
+	if (keylen != DES_KEY_SIZE && keylen != DES3_EDE_KEY_SIZE) {
+		crypto_ablkcipher_set_flags(cipher, CRYPTO_TFM_RES_BAD_KEY_LEN);
+		return -EINVAL;
+	}
+
+	if (keylen == DES_KEY_SIZE) {
+		if (!des_ekey(tmp, key) &&
+		    (tfm->crt_flags & CRYPTO_TFM_REQ_WEAK_KEY)) {
+			tfm->crt_flags |= CRYPTO_TFM_RES_WEAK_KEY;
+			return -EINVAL;
+		}
+	}
+
+	ctx->keylen = keylen;
+	memcpy_toio(ctx->dev->reg + RK_CRYPTO_TDES_KEY1_0, key, keylen);
+	return 0;
+}
+
+static int rk_aes_ecb_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_AES_ECB_MODE;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_aes_ecb_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_AES_ECB_MODE | RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_aes_cbc_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_AES_CBC_MODE;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_aes_cbc_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_AES_CBC_MODE | RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des_ecb_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = 0;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des_ecb_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des_cbc_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_CHAINMODE_CBC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des_cbc_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_CHAINMODE_CBC | RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des3_ede_ecb_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_SELECT;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des3_ede_ecb_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_SELECT | RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des3_ede_cbc_encrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_SELECT | RK_CRYPTO_TDES_CHAINMODE_CBC;
+	return rk_handle_req(dev, req);
+}
+
+static int rk_des3_ede_cbc_decrypt(struct ablkcipher_request *req)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(req);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(tfm);
+	struct rk_crypto_info *dev = ctx->dev;
+
+	dev->mode = RK_CRYPTO_TDES_SELECT | RK_CRYPTO_TDES_CHAINMODE_CBC |
+		    RK_CRYPTO_DEC;
+	return rk_handle_req(dev, req);
+}
+
+static void rk_ablk_hw_init(struct rk_crypto_info *dev)
+{
+	struct crypto_ablkcipher *cipher =
+		crypto_ablkcipher_reqtfm(dev->ablk_req);
+	struct crypto_tfm *tfm = crypto_ablkcipher_tfm(cipher);
+	struct rk_cipher_ctx *ctx = crypto_ablkcipher_ctx(cipher);
+	u32 ivsize, block, conf_reg = 0;
+
+	block = crypto_tfm_alg_blocksize(tfm);
+	ivsize = crypto_ablkcipher_ivsize(cipher);
+
+	if (block == DES_BLOCK_SIZE) {
+		dev->mode |= RK_CRYPTO_TDES_FIFO_MODE |
+			     RK_CRYPTO_TDES_BYTESWAP_KEY |
+			     RK_CRYPTO_TDES_BYTESWAP_IV;
+		CRYPTO_WRITE(dev, RK_CRYPTO_TDES_CTRL, dev->mode);
+		memcpy_toio(dev->reg + RK_CRYPTO_TDES_IV_0,
+			    dev->ablk_req->info, ivsize);
+		conf_reg = RK_CRYPTO_DESSEL;
+	} else {
+		dev->mode |= RK_CRYPTO_AES_FIFO_MODE |
+			     RK_CRYPTO_AES_KEY_CHANGE |
+			     RK_CRYPTO_AES_BYTESWAP_KEY |
+			     RK_CRYPTO_AES_BYTESWAP_IV;
+		if (ctx->keylen == AES_KEYSIZE_192)
+			dev->mode |= RK_CRYPTO_AES_192BIT_key;
+		else if (ctx->keylen == AES_KEYSIZE_256)
+			dev->mode |= RK_CRYPTO_AES_256BIT_key;
+		CRYPTO_WRITE(dev, RK_CRYPTO_AES_CTRL, dev->mode);
+		memcpy_toio(dev->reg + RK_CRYPTO_AES_IV_0,
+			    dev->ablk_req->info, ivsize);
+	}
+	conf_reg |= RK_CRYPTO_BYTESWAP_BTFIFO |
+		    RK_CRYPTO_BYTESWAP_BRFIFO;
+	CRYPTO_WRITE(dev, RK_CRYPTO_CONF, conf_reg);
+	CRYPTO_WRITE(dev, RK_CRYPTO_INTENA,
+		     RK_CRYPTO_BCDMA_ERR_ENA | RK_CRYPTO_BCDMA_DONE_ENA);
+}
+
+static void crypto_dma_start(struct rk_crypto_info *dev)
+{
+	CRYPTO_WRITE(dev, RK_CRYPTO_BRDMAS, dev->addr_in);
+	CRYPTO_WRITE(dev, RK_CRYPTO_BRDMAL, dev->count / 4);
+	CRYPTO_WRITE(dev, RK_CRYPTO_BTDMAS, dev->addr_out);
+	CRYPTO_WRITE(dev, RK_CRYPTO_CTRL, RK_CRYPTO_BLOCK_START |
+		     _SBF(RK_CRYPTO_BLOCK_START, 16));
+}
+
+static int rk_set_data_start(struct rk_crypto_info *dev)
+{
+	int err;
+
+	err = dev->load_data(dev, dev->sg_src, dev->sg_dst);
+	if (!err)
+		crypto_dma_start(dev);
+	return err;
+}
+
+static int rk_ablk_start(struct rk_crypto_info *dev)
+{
+	unsigned long flags;
+	int err;
+
+	spin_lock_irqsave(&dev->lock, flags);
+	rk_ablk_hw_init(dev);
+	err = rk_set_data_start(dev);
+	spin_unlock_irqrestore(&dev->lock, flags);
+	return err;
+}
+
+static void rk_iv_copyback(struct rk_crypto_info *dev)
+{
+	struct crypto_ablkcipher *tfm = crypto_ablkcipher_reqtfm(dev->ablk_req);
+	u32 ivsize = crypto_ablkcipher_ivsize(tfm);
+
+	if (ivsize == DES_BLOCK_SIZE)
+		memcpy_fromio(dev->ablk_req->info,
+			      dev->reg + RK_CRYPTO_TDES_IV_0, ivsize);
+	else if (ivsize == AES_BLOCK_SIZE)
+		memcpy_fromio(dev->ablk_req->info,
+			      dev->reg + RK_CRYPTO_AES_IV_0, ivsize);
+}
+
+/* return:
+ *	true	some err was occurred
+ *	fault	no err, continue
+ */
+static int rk_ablk_rx(struct rk_crypto_info *dev)
+{
+	int err = 0;
+
+	dev->unload_data(dev);
+	if (!dev->aligned) {
+		if (!sg_pcopy_from_buffer(dev->ablk_req->dst, dev->nents,
+					  dev->addr_vir, dev->count,
+					  dev->total - dev->left_bytes -
+					  dev->count)) {
+			err = -EINVAL;
+			goto out_rx;
+		}
+	}
+	if (dev->left_bytes) {
+		if (dev->aligned) {
+			if (sg_is_last(dev->sg_src)) {
+				dev_err(dev->dev, "[%s:%d] Lack of data\n",
+					__func__, __LINE__);
+				err = -ENOMEM;
+				goto out_rx;
+			}
+			dev->sg_src = sg_next(dev->sg_src);
+			dev->sg_dst = sg_next(dev->sg_dst);
+		}
+		err = rk_set_data_start(dev);
+	} else {
+		rk_iv_copyback(dev);
+		/* here show the calculation is over without any err */
+		dev->complete(dev, 0);
+	}
+out_rx:
+	return err;
+}
+
+static int rk_ablk_cra_init(struct crypto_tfm *tfm)
+{
+	struct rk_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+	struct crypto_alg *alg = tfm->__crt_alg;
+	struct rk_crypto_tmp *algt;
+
+	algt = container_of(alg, struct rk_crypto_tmp, alg);
+
+	ctx->dev = algt->dev;
+	ctx->dev->align_size = crypto_tfm_alg_alignmask(tfm) + 1;
+	ctx->dev->start = rk_ablk_start;
+	ctx->dev->update = rk_ablk_rx;
+	ctx->dev->complete = rk_crypto_complete;
+	ctx->dev->addr_vir = (char *)__get_free_page(GFP_KERNEL);
+
+	return ctx->dev->addr_vir ? ctx->dev->enable_clk(ctx->dev) : -ENOMEM;
+}
+
+static void rk_ablk_cra_exit(struct crypto_tfm *tfm)
+{
+	struct rk_cipher_ctx *ctx = crypto_tfm_ctx(tfm);
+
+	free_page((unsigned long)ctx->dev->addr_vir);
+	ctx->dev->disable_clk(ctx->dev);
+}
+
+struct rk_crypto_tmp rk_ecb_aes_alg = {
+	.alg = {
+		.cra_name		= "ecb(aes)",
+		.cra_driver_name	= "ecb-aes-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= AES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x0f,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= AES_MIN_KEY_SIZE,
+			.max_keysize	= AES_MAX_KEY_SIZE,
+			.setkey		= rk_aes_setkey,
+			.encrypt	= rk_aes_ecb_encrypt,
+			.decrypt	= rk_aes_ecb_decrypt,
+		}
+	}
+};
+
+struct rk_crypto_tmp rk_cbc_aes_alg = {
+	.alg = {
+		.cra_name		= "cbc(aes)",
+		.cra_driver_name	= "cbc-aes-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= AES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x0f,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= AES_MIN_KEY_SIZE,
+			.max_keysize	= AES_MAX_KEY_SIZE,
+			.ivsize		= AES_BLOCK_SIZE,
+			.setkey		= rk_aes_setkey,
+			.encrypt	= rk_aes_cbc_encrypt,
+			.decrypt	= rk_aes_cbc_decrypt,
+		}
+	}
+};
+
+struct rk_crypto_tmp rk_ecb_des_alg = {
+	.alg = {
+		.cra_name		= "ecb(des)",
+		.cra_driver_name	= "ecb-des-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= DES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x07,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= DES_KEY_SIZE,
+			.max_keysize	= DES_KEY_SIZE,
+			.setkey		= rk_tdes_setkey,
+			.encrypt	= rk_des_ecb_encrypt,
+			.decrypt	= rk_des_ecb_decrypt,
+		}
+	}
+};
+
+struct rk_crypto_tmp rk_cbc_des_alg = {
+	.alg = {
+		.cra_name		= "cbc(des)",
+		.cra_driver_name	= "cbc-des-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= DES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x07,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= DES_KEY_SIZE,
+			.max_keysize	= DES_KEY_SIZE,
+			.ivsize		= DES_BLOCK_SIZE,
+			.setkey		= rk_tdes_setkey,
+			.encrypt	= rk_des_cbc_encrypt,
+			.decrypt	= rk_des_cbc_decrypt,
+		}
+	}
+};
+
+struct rk_crypto_tmp rk_ecb_des3_ede_alg = {
+	.alg = {
+		.cra_name		= "ecb(des3_ede)",
+		.cra_driver_name	= "ecb-des3-ede-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= DES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x07,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= DES3_EDE_KEY_SIZE,
+			.max_keysize	= DES3_EDE_KEY_SIZE,
+			.ivsize		= DES_BLOCK_SIZE,
+			.setkey		= rk_tdes_setkey,
+			.encrypt	= rk_des3_ede_ecb_encrypt,
+			.decrypt	= rk_des3_ede_ecb_decrypt,
+		}
+	}
+};
+
+struct rk_crypto_tmp rk_cbc_des3_ede_alg = {
+	.alg = {
+		.cra_name		= "cbc(des3_ede)",
+		.cra_driver_name	= "cbc-des3-ede-rk",
+		.cra_priority		= 300,
+		.cra_flags		= CRYPTO_ALG_TYPE_ABLKCIPHER |
+					  CRYPTO_ALG_ASYNC,
+		.cra_blocksize		= DES_BLOCK_SIZE,
+		.cra_ctxsize		= sizeof(struct rk_cipher_ctx),
+		.cra_alignmask		= 0x07,
+		.cra_type		= &crypto_ablkcipher_type,
+		.cra_module		= THIS_MODULE,
+		.cra_init		= rk_ablk_cra_init,
+		.cra_exit		= rk_ablk_cra_exit,
+		.cra_u.ablkcipher	= {
+			.min_keysize	= DES3_EDE_KEY_SIZE,
+			.max_keysize	= DES3_EDE_KEY_SIZE,
+			.ivsize		= DES_BLOCK_SIZE,
+			.setkey		= rk_tdes_setkey,
+			.encrypt	= rk_des3_ede_cbc_encrypt,
+			.decrypt	= rk_des3_ede_cbc_decrypt,
+		}
+	}
+};
diff --git a/drivers/crypto/sahara.c b/drivers/crypto/sahara.c
index f68c24a98277..6c4f91c5e6b3 100644
--- a/drivers/crypto/sahara.c
+++ b/drivers/crypto/sahara.c
@@ -130,18 +130,18 @@
 #define SAHARA_REG_IDAR		0x20
 
 struct sahara_hw_desc {
-	u32		hdr;
-	u32		len1;
-	dma_addr_t	p1;
-	u32		len2;
-	dma_addr_t	p2;
-	dma_addr_t	next;
+	u32	hdr;
+	u32	len1;
+	u32	p1;
+	u32	len2;
+	u32	p2;
+	u32	next;
 };
 
 struct sahara_hw_link {
-	u32		len;
-	dma_addr_t	p;
-	dma_addr_t	next;
+	u32	len;
+	u32	p;
+	u32	next;
 };
 
 struct sahara_ctx {
@@ -228,9 +228,9 @@ struct sahara_dev {
 
 	size_t			total;
 	struct scatterlist	*in_sg;
-	unsigned int		nb_in_sg;
+	int		nb_in_sg;
 	struct scatterlist	*out_sg;
-	unsigned int		nb_out_sg;
+	int		nb_out_sg;
 
 	u32			error;
 };
@@ -416,8 +416,8 @@ static void sahara_dump_descriptors(struct sahara_dev *dev)
 		return;
 
 	for (i = 0; i < SAHARA_MAX_HW_DESC; i++) {
-		dev_dbg(dev->device, "Descriptor (%d) (0x%08x):\n",
-			i, dev->hw_phys_desc[i]);
+		dev_dbg(dev->device, "Descriptor (%d) (%pad):\n",
+			i, &dev->hw_phys_desc[i]);
 		dev_dbg(dev->device, "\thdr = 0x%08x\n", dev->hw_desc[i]->hdr);
 		dev_dbg(dev->device, "\tlen1 = %u\n", dev->hw_desc[i]->len1);
 		dev_dbg(dev->device, "\tp1 = 0x%08x\n", dev->hw_desc[i]->p1);
@@ -437,8 +437,8 @@ static void sahara_dump_links(struct sahara_dev *dev)
 		return;
 
 	for (i = 0; i < SAHARA_MAX_HW_LINK; i++) {
-		dev_dbg(dev->device, "Link (%d) (0x%08x):\n",
-			i, dev->hw_phys_link[i]);
+		dev_dbg(dev->device, "Link (%d) (%pad):\n",
+			i, &dev->hw_phys_link[i]);
 		dev_dbg(dev->device, "\tlen = %u\n", dev->hw_link[i]->len);
 		dev_dbg(dev->device, "\tp = 0x%08x\n", dev->hw_link[i]->p);
 		dev_dbg(dev->device, "\tnext = 0x%08x\n",
@@ -477,7 +477,15 @@ static int sahara_hw_descriptor_create(struct sahara_dev *dev)
 	}
 
 	dev->nb_in_sg = sg_nents_for_len(dev->in_sg, dev->total);
+	if (dev->nb_in_sg < 0) {
+		dev_err(dev->device, "Invalid numbers of src SG.\n");
+		return dev->nb_in_sg;
+	}
 	dev->nb_out_sg = sg_nents_for_len(dev->out_sg, dev->total);
+	if (dev->nb_out_sg < 0) {
+		dev_err(dev->device, "Invalid numbers of dst SG.\n");
+		return dev->nb_out_sg;
+	}
 	if ((dev->nb_in_sg + dev->nb_out_sg) > SAHARA_MAX_HW_LINK) {
 		dev_err(dev->device, "not enough hw links (%d)\n",
 			dev->nb_in_sg + dev->nb_out_sg);
@@ -793,6 +801,10 @@ static int sahara_sha_hw_links_create(struct sahara_dev *dev,
 	dev->in_sg = rctx->in_sg;
 
 	dev->nb_in_sg = sg_nents_for_len(dev->in_sg, rctx->total);
+	if (dev->nb_in_sg < 0) {
+		dev_err(dev->device, "Invalid numbers of src SG.\n");
+		return dev->nb_in_sg;
+	}
 	if ((dev->nb_in_sg) > SAHARA_MAX_HW_LINK) {
 		dev_err(dev->device, "not enough hw links (%d)\n",
 			dev->nb_in_sg + dev->nb_out_sg);
diff --git a/drivers/crypto/sunxi-ss/sun4i-ss-core.c b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
index eab6fe227fa0..107cd2a41cae 100644
--- a/drivers/crypto/sunxi-ss/sun4i-ss-core.c
+++ b/drivers/crypto/sunxi-ss/sun4i-ss-core.c
@@ -39,6 +39,7 @@ static struct sun4i_ss_alg_template ss_algs[] = {
 		.import = sun4i_hash_import_md5,
 		.halg = {
 			.digestsize = MD5_DIGEST_SIZE,
+			.statesize = sizeof(struct md5_state),
 			.base = {
 				.cra_name = "md5",
 				.cra_driver_name = "md5-sun4i-ss",
@@ -66,6 +67,7 @@ static struct sun4i_ss_alg_template ss_algs[] = {
 		.import = sun4i_hash_import_sha1,
 		.halg = {
 			.digestsize = SHA1_DIGEST_SIZE,
+			.statesize = sizeof(struct sha1_state),
 			.base = {
 				.cra_name = "sha1",
 				.cra_driver_name = "sha1-sun4i-ss",
diff --git a/drivers/crypto/talitos.c b/drivers/crypto/talitos.c
index b6f9f42e2985..a0d4a08313ae 100644
--- a/drivers/crypto/talitos.c
+++ b/drivers/crypto/talitos.c
@@ -1216,6 +1216,7 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 	struct talitos_private *priv = dev_get_drvdata(dev);
 	bool is_sec1 = has_ftr_sec1(priv);
 	int max_len = is_sec1 ? TALITOS1_MAX_DATA_LEN : TALITOS2_MAX_DATA_LEN;
+	void *err;
 
 	if (cryptlen + authsize > max_len) {
 		dev_err(dev, "length exceeds h/w max limit\n");
@@ -1228,14 +1229,29 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 	if (!dst || dst == src) {
 		src_nents = sg_nents_for_len(src,
 					     assoclen + cryptlen + authsize);
+		if (src_nents < 0) {
+			dev_err(dev, "Invalid number of src SG.\n");
+			err = ERR_PTR(-EINVAL);
+			goto error_sg;
+		}
 		src_nents = (src_nents == 1) ? 0 : src_nents;
 		dst_nents = dst ? src_nents : 0;
 	} else { /* dst && dst != src*/
 		src_nents = sg_nents_for_len(src, assoclen + cryptlen +
 						 (encrypt ? 0 : authsize));
+		if (src_nents < 0) {
+			dev_err(dev, "Invalid number of src SG.\n");
+			err = ERR_PTR(-EINVAL);
+			goto error_sg;
+		}
 		src_nents = (src_nents == 1) ? 0 : src_nents;
 		dst_nents = sg_nents_for_len(dst, assoclen + cryptlen +
 						 (encrypt ? authsize : 0));
+		if (dst_nents < 0) {
+			dev_err(dev, "Invalid number of dst SG.\n");
+			err = ERR_PTR(-EINVAL);
+			goto error_sg;
+		}
 		dst_nents = (dst_nents == 1) ? 0 : dst_nents;
 	}
 
@@ -1260,11 +1276,9 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 
 	edesc = kmalloc(alloc_len, GFP_DMA | flags);
 	if (!edesc) {
-		if (iv_dma)
-			dma_unmap_single(dev, iv_dma, ivsize, DMA_TO_DEVICE);
-
 		dev_err(dev, "could not allocate edescriptor\n");
-		return ERR_PTR(-ENOMEM);
+		err = ERR_PTR(-ENOMEM);
+		goto error_sg;
 	}
 
 	edesc->src_nents = src_nents;
@@ -1277,6 +1291,10 @@ static struct talitos_edesc *talitos_edesc_alloc(struct device *dev,
 						     DMA_BIDIRECTIONAL);
 
 	return edesc;
+error_sg:
+	if (iv_dma)
+		dma_unmap_single(dev, iv_dma, ivsize, DMA_TO_DEVICE);
+	return err;
 }
 
 static struct talitos_edesc *aead_edesc_alloc(struct aead_request *areq, u8 *iv,
@@ -1830,11 +1848,16 @@ static int ahash_process_req(struct ahash_request *areq, unsigned int nbytes)
 	unsigned int nbytes_to_hash;
 	unsigned int to_hash_later;
 	unsigned int nsg;
+	int nents;
 
 	if (!req_ctx->last && (nbytes + req_ctx->nbuf <= blocksize)) {
 		/* Buffer up to one whole block */
-		sg_copy_to_buffer(areq->src,
-				  sg_nents_for_len(areq->src, nbytes),
+		nents = sg_nents_for_len(areq->src, nbytes);
+		if (nents < 0) {
+			dev_err(ctx->dev, "Invalid number of src SG.\n");
+			return nents;
+		}
+		sg_copy_to_buffer(areq->src, nents,
 				  req_ctx->buf + req_ctx->nbuf, nbytes);
 		req_ctx->nbuf += nbytes;
 		return 0;
@@ -1867,7 +1890,11 @@ static int ahash_process_req(struct ahash_request *areq, unsigned int nbytes)
 		req_ctx->psrc = areq->src;
 
 	if (to_hash_later) {
-		int nents = sg_nents_for_len(areq->src, nbytes);
+		nents = sg_nents_for_len(areq->src, nbytes);
+		if (nents < 0) {
+			dev_err(ctx->dev, "Invalid number of src SG.\n");
+			return nents;
+		}
 		sg_pcopy_to_buffer(areq->src, nents,
 				      req_ctx->bufnext,
 				      to_hash_later,
@@ -2297,6 +2324,22 @@ static struct talitos_alg_template driver_algs[] = {
 	/* ABLKCIPHER algorithms. */
 	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
 		.alg.crypto = {
+			.cra_name = "ecb(aes)",
+			.cra_driver_name = "ecb-aes-talitos",
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC,
+			.cra_ablkcipher = {
+				.min_keysize = AES_MIN_KEY_SIZE,
+				.max_keysize = AES_MAX_KEY_SIZE,
+				.ivsize = AES_BLOCK_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_COMMON_NONSNOOP_NO_AFEU |
+				     DESC_HDR_SEL0_AESU,
+	},
+	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.alg.crypto = {
 			.cra_name = "cbc(aes)",
 			.cra_driver_name = "cbc-aes-talitos",
 			.cra_blocksize = AES_BLOCK_SIZE,
@@ -2314,6 +2357,73 @@ static struct talitos_alg_template driver_algs[] = {
 	},
 	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
 		.alg.crypto = {
+			.cra_name = "ctr(aes)",
+			.cra_driver_name = "ctr-aes-talitos",
+			.cra_blocksize = AES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC,
+			.cra_ablkcipher = {
+				.min_keysize = AES_MIN_KEY_SIZE,
+				.max_keysize = AES_MAX_KEY_SIZE,
+				.ivsize = AES_BLOCK_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_COMMON_NONSNOOP_NO_AFEU |
+				     DESC_HDR_SEL0_AESU |
+				     DESC_HDR_MODE0_AESU_CTR,
+	},
+	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.alg.crypto = {
+			.cra_name = "ecb(des)",
+			.cra_driver_name = "ecb-des-talitos",
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC,
+			.cra_ablkcipher = {
+				.min_keysize = DES_KEY_SIZE,
+				.max_keysize = DES_KEY_SIZE,
+				.ivsize = DES_BLOCK_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_COMMON_NONSNOOP_NO_AFEU |
+				     DESC_HDR_SEL0_DEU,
+	},
+	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.alg.crypto = {
+			.cra_name = "cbc(des)",
+			.cra_driver_name = "cbc-des-talitos",
+			.cra_blocksize = DES_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC,
+			.cra_ablkcipher = {
+				.min_keysize = DES_KEY_SIZE,
+				.max_keysize = DES_KEY_SIZE,
+				.ivsize = DES_BLOCK_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_COMMON_NONSNOOP_NO_AFEU |
+				     DESC_HDR_SEL0_DEU |
+				     DESC_HDR_MODE0_DEU_CBC,
+	},
+	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.alg.crypto = {
+			.cra_name = "ecb(des3_ede)",
+			.cra_driver_name = "ecb-3des-talitos",
+			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
+			.cra_flags = CRYPTO_ALG_TYPE_ABLKCIPHER |
+				     CRYPTO_ALG_ASYNC,
+			.cra_ablkcipher = {
+				.min_keysize = DES3_EDE_KEY_SIZE,
+				.max_keysize = DES3_EDE_KEY_SIZE,
+				.ivsize = DES3_EDE_BLOCK_SIZE,
+			}
+		},
+		.desc_hdr_template = DESC_HDR_TYPE_COMMON_NONSNOOP_NO_AFEU |
+				     DESC_HDR_SEL0_DEU |
+				     DESC_HDR_MODE0_DEU_3DES,
+	},
+	{	.type = CRYPTO_ALG_TYPE_ABLKCIPHER,
+		.alg.crypto = {
 			.cra_name = "cbc(des3_ede)",
 			.cra_driver_name = "cbc-3des-talitos",
 			.cra_blocksize = DES3_EDE_BLOCK_SIZE,
diff --git a/drivers/crypto/talitos.h b/drivers/crypto/talitos.h
index 0090f3211d68..8dd8f40e2771 100644
--- a/drivers/crypto/talitos.h
+++ b/drivers/crypto/talitos.h
@@ -345,6 +345,7 @@ static inline bool has_ftr_sec1(struct talitos_private *priv)
 /* primary execution unit mode (MODE0) and derivatives */
 #define	DESC_HDR_MODE0_ENCRYPT		cpu_to_be32(0x00100000)
 #define	DESC_HDR_MODE0_AESU_CBC		cpu_to_be32(0x00200000)
+#define	DESC_HDR_MODE0_AESU_CTR		cpu_to_be32(0x00600000)
 #define	DESC_HDR_MODE0_DEU_CBC		cpu_to_be32(0x00400000)
 #define	DESC_HDR_MODE0_DEU_3DES		cpu_to_be32(0x00200000)
 #define	DESC_HDR_MODE0_MDEU_CONT	cpu_to_be32(0x08000000)
diff --git a/drivers/crypto/ux500/Kconfig b/drivers/crypto/ux500/Kconfig
index 30796441b0a6..0e338bf6dfb7 100644
--- a/drivers/crypto/ux500/Kconfig
+++ b/drivers/crypto/ux500/Kconfig
@@ -18,6 +18,8 @@ config CRYPTO_DEV_UX500_HASH
         tristate "UX500 crypto driver for HASH block"
         depends on CRYPTO_DEV_UX500
         select CRYPTO_HASH
+	select CRYPTO_SHA1
+	select CRYPTO_SHA256
         help
           This selects the hash driver for the UX500_HASH hardware.
           Depends on UX500/STM DMA if running in DMA mode.
diff --git a/drivers/crypto/ux500/hash/hash_core.c b/drivers/crypto/ux500/hash/hash_core.c
index f47d112041b2..d6fdc583ce5d 100644
--- a/drivers/crypto/ux500/hash/hash_core.c
+++ b/drivers/crypto/ux500/hash/hash_core.c
@@ -41,22 +41,6 @@ static int hash_mode;
 module_param(hash_mode, int, 0);
 MODULE_PARM_DESC(hash_mode, "CPU or DMA mode. CPU = 0 (default), DMA = 1");
 
-/**
- * Pre-calculated empty message digests.
- */
-static const u8 zero_message_hash_sha1[SHA1_DIGEST_SIZE] = {
-	0xda, 0x39, 0xa3, 0xee, 0x5e, 0x6b, 0x4b, 0x0d,
-	0x32, 0x55, 0xbf, 0xef, 0x95, 0x60, 0x18, 0x90,
-	0xaf, 0xd8, 0x07, 0x09
-};
-
-static const u8 zero_message_hash_sha256[SHA256_DIGEST_SIZE] = {
-	0xe3, 0xb0, 0xc4, 0x42, 0x98, 0xfc, 0x1c, 0x14,
-	0x9a, 0xfb, 0xf4, 0xc8, 0x99, 0x6f, 0xb9, 0x24,
-	0x27, 0xae, 0x41, 0xe4, 0x64, 0x9b, 0x93, 0x4c,
-	0xa4, 0x95, 0x99, 0x1b, 0x78, 0x52, 0xb8, 0x55
-};
-
 /* HMAC-SHA1, no key */
 static const u8 zero_message_hmac_sha1[SHA1_DIGEST_SIZE] = {
 	0xfb, 0xdb, 0x1d, 0x1b, 0x18, 0xaa, 0x6c, 0x08,
@@ -242,13 +226,13 @@ static int get_empty_message_digest(
 
 	if (HASH_OPER_MODE_HASH == ctx->config.oper_mode) {
 		if (HASH_ALGO_SHA1 == ctx->config.algorithm) {
-			memcpy(zero_hash, &zero_message_hash_sha1[0],
+			memcpy(zero_hash, &sha1_zero_message_hash[0],
 			       SHA1_DIGEST_SIZE);
 			*zero_hash_size = SHA1_DIGEST_SIZE;
 			*zero_digest = true;
 		} else if (HASH_ALGO_SHA256 ==
 				ctx->config.algorithm) {
-			memcpy(zero_hash, &zero_message_hash_sha256[0],
+			memcpy(zero_hash, &sha256_zero_message_hash[0],
 			       SHA256_DIGEST_SIZE);
 			*zero_hash_size = SHA256_DIGEST_SIZE;
 			*zero_digest = true;
diff --git a/drivers/crypto/vmx/aes.c b/drivers/crypto/vmx/aes.c
index 263af709e536..022c7ab7351a 100644
--- a/drivers/crypto/vmx/aes.c
+++ b/drivers/crypto/vmx/aes.c
@@ -83,10 +83,10 @@ static int p8_aes_setkey(struct crypto_tfm *tfm, const u8 *key,
 
 	preempt_disable();
 	pagefault_disable();
-	enable_kernel_altivec();
 	enable_kernel_vsx();
 	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
 	ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+	disable_kernel_vsx();
 	pagefault_enable();
 	preempt_enable();
 
@@ -103,9 +103,9 @@ static void p8_aes_encrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
 	} else {
 		preempt_disable();
 		pagefault_disable();
-		enable_kernel_altivec();
 		enable_kernel_vsx();
 		aes_p8_encrypt(src, dst, &ctx->enc_key);
+		disable_kernel_vsx();
 		pagefault_enable();
 		preempt_enable();
 	}
@@ -120,9 +120,9 @@ static void p8_aes_decrypt(struct crypto_tfm *tfm, u8 *dst, const u8 *src)
 	} else {
 		preempt_disable();
 		pagefault_disable();
-		enable_kernel_altivec();
 		enable_kernel_vsx();
 		aes_p8_decrypt(src, dst, &ctx->dec_key);
+		disable_kernel_vsx();
 		pagefault_enable();
 		preempt_enable();
 	}
diff --git a/drivers/crypto/vmx/aes_cbc.c b/drivers/crypto/vmx/aes_cbc.c
index 0b8fe2ec5315..495577b6d31b 100644
--- a/drivers/crypto/vmx/aes_cbc.c
+++ b/drivers/crypto/vmx/aes_cbc.c
@@ -84,10 +84,10 @@ static int p8_aes_cbc_setkey(struct crypto_tfm *tfm, const u8 *key,
 
 	preempt_disable();
 	pagefault_disable();
-	enable_kernel_altivec();
 	enable_kernel_vsx();
 	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
 	ret += aes_p8_set_decrypt_key(key, keylen * 8, &ctx->dec_key);
+	disable_kernel_vsx();
 	pagefault_enable();
 	preempt_enable();
 
@@ -115,7 +115,6 @@ static int p8_aes_cbc_encrypt(struct blkcipher_desc *desc,
 	} else {
 		preempt_disable();
 		pagefault_disable();
-		enable_kernel_altivec();
 		enable_kernel_vsx();
 
 		blkcipher_walk_init(&walk, dst, src, nbytes);
@@ -129,6 +128,7 @@ static int p8_aes_cbc_encrypt(struct blkcipher_desc *desc,
 			ret = blkcipher_walk_done(desc, &walk, nbytes);
 		}
 
+		disable_kernel_vsx();
 		pagefault_enable();
 		preempt_enable();
 	}
@@ -156,7 +156,6 @@ static int p8_aes_cbc_decrypt(struct blkcipher_desc *desc,
 	} else {
 		preempt_disable();
 		pagefault_disable();
-		enable_kernel_altivec();
 		enable_kernel_vsx();
 
 		blkcipher_walk_init(&walk, dst, src, nbytes);
@@ -170,6 +169,7 @@ static int p8_aes_cbc_decrypt(struct blkcipher_desc *desc,
 			ret = blkcipher_walk_done(desc, &walk, nbytes);
 		}
 
+		disable_kernel_vsx();
 		pagefault_enable();
 		preempt_enable();
 	}
@@ -191,7 +191,7 @@ struct crypto_alg p8_aes_cbc_alg = {
 	.cra_init = p8_aes_cbc_init,
 	.cra_exit = p8_aes_cbc_exit,
 	.cra_blkcipher = {
-			  .ivsize = 0,
+			  .ivsize = AES_BLOCK_SIZE,
 			  .min_keysize = AES_MIN_KEY_SIZE,
 			  .max_keysize = AES_MAX_KEY_SIZE,
 			  .setkey = p8_aes_cbc_setkey,
diff --git a/drivers/crypto/vmx/aes_ctr.c b/drivers/crypto/vmx/aes_ctr.c
index ee1306cd8f59..0a3c1b04cf3c 100644
--- a/drivers/crypto/vmx/aes_ctr.c
+++ b/drivers/crypto/vmx/aes_ctr.c
@@ -81,9 +81,9 @@ static int p8_aes_ctr_setkey(struct crypto_tfm *tfm, const u8 *key,
 	struct p8_aes_ctr_ctx *ctx = crypto_tfm_ctx(tfm);
 
 	pagefault_disable();
-	enable_kernel_altivec();
 	enable_kernel_vsx();
 	ret = aes_p8_set_encrypt_key(key, keylen * 8, &ctx->enc_key);
+	disable_kernel_vsx();
 	pagefault_enable();
 
 	ret += crypto_blkcipher_setkey(ctx->fallback, key, keylen);
@@ -100,9 +100,9 @@ static void p8_aes_ctr_final(struct p8_aes_ctr_ctx *ctx,
 	unsigned int nbytes = walk->nbytes;
 
 	pagefault_disable();
-	enable_kernel_altivec();
 	enable_kernel_vsx();
 	aes_p8_encrypt(ctrblk, keystream, &ctx->enc_key);
+	disable_kernel_vsx();
 	pagefault_enable();
 
 	crypto_xor(keystream, src, nbytes);
@@ -133,7 +133,6 @@ static int p8_aes_ctr_crypt(struct blkcipher_desc *desc,
 		ret = blkcipher_walk_virt_block(desc, &walk, AES_BLOCK_SIZE);
 		while ((nbytes = walk.nbytes) >= AES_BLOCK_SIZE) {
 			pagefault_disable();
-			enable_kernel_altivec();
 			enable_kernel_vsx();
 			aes_p8_ctr32_encrypt_blocks(walk.src.virt.addr,
 						    walk.dst.virt.addr,
@@ -142,6 +141,7 @@ static int p8_aes_ctr_crypt(struct blkcipher_desc *desc,
 						    AES_BLOCK_SIZE,
 						    &ctx->enc_key,
 						    walk.iv);
+			disable_kernel_vsx();
 			pagefault_enable();
 
 			/* We need to update IV mostly for last bytes/round */
@@ -175,7 +175,7 @@ struct crypto_alg p8_aes_ctr_alg = {
 	.cra_init = p8_aes_ctr_init,
 	.cra_exit = p8_aes_ctr_exit,
 	.cra_blkcipher = {
-			  .ivsize = 0,
+			  .ivsize = AES_BLOCK_SIZE,
 			  .min_keysize = AES_MIN_KEY_SIZE,
 			  .max_keysize = AES_MAX_KEY_SIZE,
 			  .setkey = p8_aes_ctr_setkey,
diff --git a/drivers/crypto/vmx/ghash.c b/drivers/crypto/vmx/ghash.c
index 2183a2e77641..6c999cb01b80 100644
--- a/drivers/crypto/vmx/ghash.c
+++ b/drivers/crypto/vmx/ghash.c
@@ -118,10 +118,9 @@ static int p8_ghash_setkey(struct crypto_shash *tfm, const u8 *key,
 
 	preempt_disable();
 	pagefault_disable();
-	enable_kernel_altivec();
 	enable_kernel_vsx();
-	enable_kernel_fp();
 	gcm_init_p8(ctx->htable, (const u64 *) key);
+	disable_kernel_vsx();
 	pagefault_enable();
 	preempt_enable();
 	return crypto_shash_setkey(ctx->fallback, key, keylen);
@@ -149,11 +148,10 @@ static int p8_ghash_update(struct shash_desc *desc,
 			       GHASH_DIGEST_SIZE - dctx->bytes);
 			preempt_disable();
 			pagefault_disable();
-			enable_kernel_altivec();
 			enable_kernel_vsx();
-			enable_kernel_fp();
 			gcm_ghash_p8(dctx->shash, ctx->htable,
 				     dctx->buffer, GHASH_DIGEST_SIZE);
+			disable_kernel_vsx();
 			pagefault_enable();
 			preempt_enable();
 			src += GHASH_DIGEST_SIZE - dctx->bytes;
@@ -164,10 +162,9 @@ static int p8_ghash_update(struct shash_desc *desc,
 		if (len) {
 			preempt_disable();
 			pagefault_disable();
-			enable_kernel_altivec();
 			enable_kernel_vsx();
-			enable_kernel_fp();
 			gcm_ghash_p8(dctx->shash, ctx->htable, src, len);
+			disable_kernel_vsx();
 			pagefault_enable();
 			preempt_enable();
 			src += len;
@@ -195,11 +192,10 @@ static int p8_ghash_final(struct shash_desc *desc, u8 *out)
 				dctx->buffer[i] = 0;
 			preempt_disable();
 			pagefault_disable();
-			enable_kernel_altivec();
 			enable_kernel_vsx();
-			enable_kernel_fp();
 			gcm_ghash_p8(dctx->shash, ctx->htable,
 				     dctx->buffer, GHASH_DIGEST_SIZE);
+			disable_kernel_vsx();
 			pagefault_enable();
 			preempt_enable();
 			dctx->bytes = 0;
diff --git a/drivers/dca/dca-core.c b/drivers/dca/dca-core.c
index 819dfda88236..7afbb28d6a0f 100644
--- a/drivers/dca/dca-core.c
+++ b/drivers/dca/dca-core.c
@@ -321,7 +321,8 @@ EXPORT_SYMBOL_GPL(dca_get_tag);
  * @ops - pointer to struct of dca operation function pointers
  * @priv_size - size of extra mem to be added for provider's needs
  */
-struct dca_provider *alloc_dca_provider(struct dca_ops *ops, int priv_size)
+struct dca_provider *alloc_dca_provider(const struct dca_ops *ops,
+					int priv_size)
 {
 	struct dca_provider *dca;
 	int alloc_size;
diff --git a/drivers/devfreq/devfreq-event.c b/drivers/devfreq/devfreq-event.c
index f304a0289eda..38bf144ca147 100644
--- a/drivers/devfreq/devfreq-event.c
+++ b/drivers/devfreq/devfreq-event.c
@@ -226,17 +226,12 @@ struct devfreq_event_dev *devfreq_event_get_edev_by_phandle(struct device *dev,
 	struct device_node *node;
 	struct devfreq_event_dev *edev;
 
-	if (!dev->of_node) {
-		dev_err(dev, "device does not have a device node entry\n");
+	if (!dev->of_node)
 		return ERR_PTR(-EINVAL);
-	}
 
 	node = of_parse_phandle(dev->of_node, "devfreq-events", index);
-	if (!node) {
-		dev_err(dev, "failed to get phandle in %s node\n",
-			dev->of_node->full_name);
+	if (!node)
 		return ERR_PTR(-ENODEV);
-	}
 
 	mutex_lock(&devfreq_event_list_lock);
 	list_for_each_entry(edev, &devfreq_event_list, node) {
@@ -248,8 +243,6 @@ out:
 	mutex_unlock(&devfreq_event_list_lock);
 
 	if (!edev) {
-		dev_err(dev, "unable to get devfreq-event device : %s\n",
-			node->name);
 		of_node_put(node);
 		return ERR_PTR(-ENODEV);
 	}
@@ -277,7 +270,7 @@ int devfreq_event_get_edev_count(struct device *dev)
 
 	count = of_property_count_elems_of_size(dev->of_node, "devfreq-events",
 						sizeof(u32));
-	if (count < 0 ) {
+	if (count < 0) {
 		dev_err(dev,
 			"failed to get the count of devfreq-event in %s node\n",
 			dev->of_node->full_name);
@@ -402,7 +395,8 @@ struct devfreq_event_dev *devm_devfreq_event_add_edev(struct device *dev,
 {
 	struct devfreq_event_dev **ptr, *edev;
 
-	ptr = devres_alloc(devm_devfreq_event_release, sizeof(*ptr), GFP_KERNEL);
+	ptr = devres_alloc(devm_devfreq_event_release, sizeof(*ptr),
+				GFP_KERNEL);
 	if (!ptr)
 		return ERR_PTR(-ENOMEM);
 
diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index ca848cc6a8fd..984c5e9e7bdd 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -85,6 +85,46 @@ static int devfreq_get_freq_level(struct devfreq *devfreq, unsigned long freq)
 }
 
 /**
+ * devfreq_set_freq_table() - Initialize freq_table for the frequency
+ * @devfreq:	the devfreq instance
+ */
+static void devfreq_set_freq_table(struct devfreq *devfreq)
+{
+	struct devfreq_dev_profile *profile = devfreq->profile;
+	struct dev_pm_opp *opp;
+	unsigned long freq;
+	int i, count;
+
+	/* Initialize the freq_table from OPP table */
+	count = dev_pm_opp_get_opp_count(devfreq->dev.parent);
+	if (count <= 0)
+		return;
+
+	profile->max_state = count;
+	profile->freq_table = devm_kcalloc(devfreq->dev.parent,
+					profile->max_state,
+					sizeof(*profile->freq_table),
+					GFP_KERNEL);
+	if (!profile->freq_table) {
+		profile->max_state = 0;
+		return;
+	}
+
+	rcu_read_lock();
+	for (i = 0, freq = 0; i < profile->max_state; i++, freq++) {
+		opp = dev_pm_opp_find_freq_ceil(devfreq->dev.parent, &freq);
+		if (IS_ERR(opp)) {
+			devm_kfree(devfreq->dev.parent, profile->freq_table);
+			profile->max_state = 0;
+			rcu_read_unlock();
+			return;
+		}
+		profile->freq_table[i] = freq;
+	}
+	rcu_read_unlock();
+}
+
+/**
  * devfreq_update_status() - Update statistics of devfreq behavior
  * @devfreq:	the devfreq instance
  * @freq:	the update target frequency
@@ -478,6 +518,12 @@ struct devfreq *devfreq_add_device(struct device *dev,
 	devfreq->data = data;
 	devfreq->nb.notifier_call = devfreq_notifier_call;
 
+	if (!devfreq->profile->max_state && !devfreq->profile->freq_table) {
+		mutex_unlock(&devfreq->lock);
+		devfreq_set_freq_table(devfreq);
+		mutex_lock(&devfreq->lock);
+	}
+
 	devfreq->trans_table =	devm_kzalloc(dev, sizeof(unsigned int) *
 						devfreq->profile->max_state *
 						devfreq->profile->max_state,
@@ -921,12 +967,6 @@ unlock:
 	return ret;
 }
 
-static ssize_t min_freq_show(struct device *dev, struct device_attribute *attr,
-			     char *buf)
-{
-	return sprintf(buf, "%lu\n", to_devfreq(dev)->min_freq);
-}
-
 static ssize_t max_freq_store(struct device *dev, struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
@@ -953,13 +993,17 @@ unlock:
 	mutex_unlock(&df->lock);
 	return ret;
 }
-static DEVICE_ATTR_RW(min_freq);
 
-static ssize_t max_freq_show(struct device *dev, struct device_attribute *attr,
-			     char *buf)
-{
-	return sprintf(buf, "%lu\n", to_devfreq(dev)->max_freq);
+#define show_one(name)						\
+static ssize_t name##_show					\
+(struct device *dev, struct device_attribute *attr, char *buf)	\
+{								\
+	return sprintf(buf, "%lu\n", to_devfreq(dev)->name);	\
 }
+show_one(min_freq);
+show_one(max_freq);
+
+static DEVICE_ATTR_RW(min_freq);
 static DEVICE_ATTR_RW(max_freq);
 
 static ssize_t available_frequencies_show(struct device *d,
@@ -1005,11 +1049,13 @@ static ssize_t trans_stat_show(struct device *dev,
 	if (!devfreq->stop_polling &&
 			devfreq_update_status(devfreq, devfreq->previous_freq))
 		return 0;
+	if (max_state == 0)
+		return sprintf(buf, "Not Supported.\n");
 
-	len = sprintf(buf, "   From  :   To\n");
-	len += sprintf(buf + len, "         :");
+	len = sprintf(buf, "     From  :   To\n");
+	len += sprintf(buf + len, "           :");
 	for (i = 0; i < max_state; i++)
-		len += sprintf(buf + len, "%8u",
+		len += sprintf(buf + len, "%10lu",
 				devfreq->profile->freq_table[i]);
 
 	len += sprintf(buf + len, "   time(ms)\n");
@@ -1021,10 +1067,10 @@ static ssize_t trans_stat_show(struct device *dev,
 		} else {
 			len += sprintf(buf + len, " ");
 		}
-		len += sprintf(buf + len, "%8u:",
+		len += sprintf(buf + len, "%10lu:",
 				devfreq->profile->freq_table[i]);
 		for (j = 0; j < max_state; j++)
-			len += sprintf(buf + len, "%8u",
+			len += sprintf(buf + len, "%10u",
 				devfreq->trans_table[(i * max_state) + j]);
 		len += sprintf(buf + len, "%10u\n",
 			jiffies_to_msecs(devfreq->time_in_state[i]));
diff --git a/drivers/devfreq/tegra-devfreq.c b/drivers/devfreq/tegra-devfreq.c
index 848b93ee930f..fe9dce0245bf 100644
--- a/drivers/devfreq/tegra-devfreq.c
+++ b/drivers/devfreq/tegra-devfreq.c
@@ -500,6 +500,8 @@ static int tegra_devfreq_target(struct device *dev, unsigned long *freq,
 	clk_set_min_rate(tegra->emc_clock, rate);
 	clk_set_rate(tegra->emc_clock, 0);
 
+	*freq = rate;
+
 	return 0;
 }
 
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index e6cd1a32025a..79b1390f2016 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -431,8 +431,20 @@ config STE_DMA40
 	help
 	  Support for ST-Ericsson DMA40 controller
 
+config STM32_DMA
+	bool "STMicroelectronics STM32 DMA support"
+	depends on ARCH_STM32
+	select DMA_ENGINE
+	select DMA_OF
+	select DMA_VIRTUAL_CHANNELS
+	help
+	  Enable support for the on-chip DMA controller on STMicroelectronics
+	  STM32 MCUs.
+	  If you have a board based on such a MCU and wish to use DMA say Y or M
+	  here.
+
 config S3C24XX_DMAC
-	tristate "Samsung S3C24XX DMA support"
+	bool "Samsung S3C24XX DMA support"
 	depends on ARCH_S3C24XX
 	select DMA_ENGINE
 	select DMA_VIRTUAL_CHANNELS
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index ef9c099bd2b6..2dd0a067a0ca 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -56,6 +56,7 @@ obj-$(CONFIG_QCOM_BAM_DMA) += qcom_bam_dma.o
 obj-$(CONFIG_RENESAS_DMA) += sh/
 obj-$(CONFIG_SIRF_DMA) += sirf-dma.o
 obj-$(CONFIG_STE_DMA40) += ste_dma40.o ste_dma40_ll.o
+obj-$(CONFIG_STM32_DMA) += stm32-dma.o
 obj-$(CONFIG_S3C24XX_DMAC) += s3c24xx-dma.o
 obj-$(CONFIG_TXX9_DMAC) += txx9dmac.o
 obj-$(CONFIG_TEGRA20_APB_DMA) += tegra20-apb-dma.o
diff --git a/drivers/dma/acpi-dma.c b/drivers/dma/acpi-dma.c
index 16d0daa058a5..eed6bda01790 100644
--- a/drivers/dma/acpi-dma.c
+++ b/drivers/dma/acpi-dma.c
@@ -15,6 +15,7 @@
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/module.h>
+#include <linux/kernel.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
@@ -72,7 +73,9 @@ static int acpi_dma_parse_resource_group(const struct acpi_csrt_group *grp,
 	si = (const struct acpi_csrt_shared_info *)&grp[1];
 
 	/* Match device by MMIO and IRQ */
-	if (si->mmio_base_low != mem || si->gsi_interrupt != irq)
+	if (si->mmio_base_low != lower_32_bits(mem) ||
+	    si->mmio_base_high != upper_32_bits(mem) ||
+	    si->gsi_interrupt != irq)
 		return 0;
 
 	dev_dbg(&adev->dev, "matches with %.4s%04X (rev %u)\n",
diff --git a/drivers/dma/at_xdmac.c b/drivers/dma/at_xdmac.c
index 370c661c7d7b..8e304b1befc5 100644
--- a/drivers/dma/at_xdmac.c
+++ b/drivers/dma/at_xdmac.c
@@ -176,6 +176,7 @@
 #define AT_XDMAC_MAX_CHAN	0x20
 #define AT_XDMAC_MAX_CSIZE	16	/* 16 data */
 #define AT_XDMAC_MAX_DWIDTH	8	/* 64 bits */
+#define AT_XDMAC_RESIDUE_MAX_RETRIES	5
 
 #define AT_XDMAC_DMA_BUSWIDTHS\
 	(BIT(DMA_SLAVE_BUSWIDTH_UNDEFINED) |\
@@ -863,8 +864,12 @@ at_xdmac_interleaved_queue_desc(struct dma_chan *chan,
 	 * access. Hopefully we can access DDR through both ports (at least on
 	 * SAMA5D4x), so we can use the same interface for source and dest,
 	 * that solves the fact we don't know the direction.
+	 * ERRATA: Even if useless for memory transfers, the PERID has to not
+	 * match the one of another channel. If not, it could lead to spurious
+	 * flag status.
 	 */
-	u32			chan_cc = AT_XDMAC_CC_DIF(0)
+	u32			chan_cc = AT_XDMAC_CC_PERID(0x3f)
+					| AT_XDMAC_CC_DIF(0)
 					| AT_XDMAC_CC_SIF(0)
 					| AT_XDMAC_CC_MBSIZE_SIXTEEN
 					| AT_XDMAC_CC_TYPE_MEM_TRAN;
@@ -1041,8 +1046,12 @@ at_xdmac_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
 	 * access DDR through both ports (at least on SAMA5D4x), so we can use
 	 * the same interface for source and dest, that solves the fact we
 	 * don't know the direction.
+	 * ERRATA: Even if useless for memory transfers, the PERID has to not
+	 * match the one of another channel. If not, it could lead to spurious
+	 * flag status.
 	 */
-	u32			chan_cc = AT_XDMAC_CC_DAM_INCREMENTED_AM
+	u32			chan_cc = AT_XDMAC_CC_PERID(0x3f)
+					| AT_XDMAC_CC_DAM_INCREMENTED_AM
 					| AT_XDMAC_CC_SAM_INCREMENTED_AM
 					| AT_XDMAC_CC_DIF(0)
 					| AT_XDMAC_CC_SIF(0)
@@ -1143,8 +1152,12 @@ static struct at_xdmac_desc *at_xdmac_memset_create_desc(struct dma_chan *chan,
 	 * access. Hopefully we can access DDR through both ports (at least on
 	 * SAMA5D4x), so we can use the same interface for source and dest,
 	 * that solves the fact we don't know the direction.
+	 * ERRATA: Even if useless for memory transfers, the PERID has to not
+	 * match the one of another channel. If not, it could lead to spurious
+	 * flag status.
 	 */
-	u32			chan_cc = AT_XDMAC_CC_DAM_UBS_AM
+	u32			chan_cc = AT_XDMAC_CC_PERID(0x3f)
+					| AT_XDMAC_CC_DAM_UBS_AM
 					| AT_XDMAC_CC_SAM_INCREMENTED_AM
 					| AT_XDMAC_CC_DIF(0)
 					| AT_XDMAC_CC_SIF(0)
@@ -1383,8 +1396,8 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 	struct at_xdmac_desc	*desc, *_desc;
 	struct list_head	*descs_list;
 	enum dma_status		ret;
-	int			residue;
-	u32			cur_nda, mask, value;
+	int			residue, retry;
+	u32			cur_nda, check_nda, cur_ubc, mask, value;
 	u8			dwidth = 0;
 	unsigned long		flags;
 
@@ -1421,7 +1434,42 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 			cpu_relax();
 	}
 
+	/*
+	 * When processing the residue, we need to read two registers but we
+	 * can't do it in an atomic way. AT_XDMAC_CNDA is used to find where
+	 * we stand in the descriptor list and AT_XDMAC_CUBC is used
+	 * to know how many data are remaining for the current descriptor.
+	 * Since the dma channel is not paused to not loose data, between the
+	 * AT_XDMAC_CNDA and AT_XDMAC_CUBC read, we may have change of
+	 * descriptor.
+	 * For that reason, after reading AT_XDMAC_CUBC, we check if we are
+	 * still using the same descriptor by reading a second time
+	 * AT_XDMAC_CNDA. If AT_XDMAC_CNDA has changed, it means we have to
+	 * read again AT_XDMAC_CUBC.
+	 * Memory barriers are used to ensure the read order of the registers.
+	 * A max number of retries is set because unlikely it can never ends if
+	 * we are transferring a lot of data with small buffers.
+	 */
 	cur_nda = at_xdmac_chan_read(atchan, AT_XDMAC_CNDA) & 0xfffffffc;
+	rmb();
+	cur_ubc = at_xdmac_chan_read(atchan, AT_XDMAC_CUBC);
+	for (retry = 0; retry < AT_XDMAC_RESIDUE_MAX_RETRIES; retry++) {
+		rmb();
+		check_nda = at_xdmac_chan_read(atchan, AT_XDMAC_CNDA) & 0xfffffffc;
+
+		if (likely(cur_nda == check_nda))
+			break;
+
+		cur_nda = check_nda;
+		rmb();
+		cur_ubc = at_xdmac_chan_read(atchan, AT_XDMAC_CUBC);
+	}
+
+	if (unlikely(retry >= AT_XDMAC_RESIDUE_MAX_RETRIES)) {
+		ret = DMA_ERROR;
+		goto spin_unlock;
+	}
+
 	/*
 	 * Remove size of all microblocks already transferred and the current
 	 * one. Then add the remaining size to transfer of the current
@@ -1434,7 +1482,7 @@ at_xdmac_tx_status(struct dma_chan *chan, dma_cookie_t cookie,
 		if ((desc->lld.mbr_nda & 0xfffffffc) == cur_nda)
 			break;
 	}
-	residue += at_xdmac_chan_read(atchan, AT_XDMAC_CUBC) << dwidth;
+	residue += cur_ubc << dwidth;
 
 	dma_set_residue(txstate, residue);
 
@@ -1688,6 +1736,7 @@ static int at_xdmac_device_terminate_all(struct dma_chan *chan)
 	list_for_each_entry_safe(desc, _desc, &atchan->xfers_list, xfer_node)
 		at_xdmac_remove_xfer(atchan, desc);
 
+	clear_bit(AT_XDMAC_CHAN_IS_PAUSED, &atchan->status);
 	clear_bit(AT_XDMAC_CHAN_IS_CYCLIC, &atchan->status);
 	spin_unlock_irqrestore(&atchan->lock, flags);
 
@@ -1820,6 +1869,8 @@ static int atmel_xdmac_resume(struct device *dev)
 		atchan = to_at_xdmac_chan(chan);
 		at_xdmac_chan_write(atchan, AT_XDMAC_CC, atchan->save_cc);
 		if (at_xdmac_chan_is_cyclic(atchan)) {
+			if (at_xdmac_chan_is_paused(atchan))
+				at_xdmac_device_resume(chan);
 			at_xdmac_chan_write(atchan, AT_XDMAC_CNDA, atchan->save_cnda);
 			at_xdmac_chan_write(atchan, AT_XDMAC_CNDC, atchan->save_cndc);
 			at_xdmac_chan_write(atchan, AT_XDMAC_CIE, atchan->save_cim);
@@ -1998,8 +2049,6 @@ static int at_xdmac_remove(struct platform_device *pdev)
 	dma_async_device_unregister(&atxdmac->dma);
 	clk_disable_unprepare(atxdmac->clk);
 
-	synchronize_irq(atxdmac->irq);
-
 	free_irq(atxdmac->irq, atxdmac->dma.dev);
 
 	for (i = 0; i < atxdmac->dma.chancnt; i++) {
diff --git a/drivers/dma/dma-axi-dmac.c b/drivers/dma/dma-axi-dmac.c
index 5b2395e7e04d..c3468094393e 100644
--- a/drivers/dma/dma-axi-dmac.c
+++ b/drivers/dma/dma-axi-dmac.c
@@ -307,6 +307,13 @@ static int axi_dmac_terminate_all(struct dma_chan *c)
 	return 0;
 }
 
+static void axi_dmac_synchronize(struct dma_chan *c)
+{
+	struct axi_dmac_chan *chan = to_axi_dmac_chan(c);
+
+	vchan_synchronize(&chan->vchan);
+}
+
 static void axi_dmac_issue_pending(struct dma_chan *c)
 {
 	struct axi_dmac_chan *chan = to_axi_dmac_chan(c);
@@ -613,6 +620,7 @@ static int axi_dmac_probe(struct platform_device *pdev)
 	dma_dev->device_prep_dma_cyclic = axi_dmac_prep_dma_cyclic;
 	dma_dev->device_prep_interleaved_dma = axi_dmac_prep_interleaved;
 	dma_dev->device_terminate_all = axi_dmac_terminate_all;
+	dma_dev->device_synchronize = axi_dmac_synchronize;
 	dma_dev->dev = &pdev->dev;
 	dma_dev->chancnt = 1;
 	dma_dev->src_addr_widths = BIT(dmac->chan.src_width);
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index 3ecec1445adf..c50a247be2e0 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -43,6 +43,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/platform_device.h>
 #include <linux/dma-mapping.h>
 #include <linux/init.h>
 #include <linux/module.h>
@@ -265,8 +266,11 @@ static void dma_chan_put(struct dma_chan *chan)
 	module_put(dma_chan_to_owner(chan));
 
 	/* This channel is not in use anymore, free it */
-	if (!chan->client_count && chan->device->device_free_chan_resources)
+	if (!chan->client_count && chan->device->device_free_chan_resources) {
+		/* Make sure all operations have completed */
+		dmaengine_synchronize(chan);
 		chan->device->device_free_chan_resources(chan);
+	}
 
 	/* If the channel is used via a DMA request router, free the mapping */
 	if (chan->router && chan->router->route_free) {
@@ -493,6 +497,7 @@ int dma_get_slave_caps(struct dma_chan *chan, struct dma_slave_caps *caps)
 	caps->dst_addr_widths = device->dst_addr_widths;
 	caps->directions = device->directions;
 	caps->residue_granularity = device->residue_granularity;
+	caps->descriptor_reuse = device->descriptor_reuse;
 
 	/*
 	 * Some devices implement only pause (e.g. to get residuum) but no
@@ -511,7 +516,7 @@ static struct dma_chan *private_candidate(const dma_cap_mask_t *mask,
 {
 	struct dma_chan *chan;
 
-	if (!__dma_device_satisfies_mask(dev, mask)) {
+	if (mask && !__dma_device_satisfies_mask(dev, mask)) {
 		pr_debug("%s: wrong capabilities\n", __func__);
 		return NULL;
 	}
@@ -542,6 +547,42 @@ static struct dma_chan *private_candidate(const dma_cap_mask_t *mask,
 	return NULL;
 }
 
+static struct dma_chan *find_candidate(struct dma_device *device,
+				       const dma_cap_mask_t *mask,
+				       dma_filter_fn fn, void *fn_param)
+{
+	struct dma_chan *chan = private_candidate(mask, device, fn, fn_param);
+	int err;
+
+	if (chan) {
+		/* Found a suitable channel, try to grab, prep, and return it.
+		 * We first set DMA_PRIVATE to disable balance_ref_count as this
+		 * channel will not be published in the general-purpose
+		 * allocator
+		 */
+		dma_cap_set(DMA_PRIVATE, device->cap_mask);
+		device->privatecnt++;
+		err = dma_chan_get(chan);
+
+		if (err) {
+			if (err == -ENODEV) {
+				pr_debug("%s: %s module removed\n", __func__,
+					 dma_chan_name(chan));
+				list_del_rcu(&device->global_node);
+			} else
+				pr_debug("%s: failed to get %s: (%d)\n",
+					 __func__, dma_chan_name(chan), err);
+
+			if (--device->privatecnt == 0)
+				dma_cap_clear(DMA_PRIVATE, device->cap_mask);
+
+			chan = ERR_PTR(err);
+		}
+	}
+
+	return chan ? chan : ERR_PTR(-EPROBE_DEFER);
+}
+
 /**
  * dma_get_slave_channel - try to get specific channel exclusively
  * @chan: target channel
@@ -580,7 +621,6 @@ struct dma_chan *dma_get_any_slave_channel(struct dma_device *device)
 {
 	dma_cap_mask_t mask;
 	struct dma_chan *chan;
-	int err;
 
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
@@ -588,23 +628,11 @@ struct dma_chan *dma_get_any_slave_channel(struct dma_device *device)
 	/* lock against __dma_request_channel */
 	mutex_lock(&dma_list_mutex);
 
-	chan = private_candidate(&mask, device, NULL, NULL);
-	if (chan) {
-		dma_cap_set(DMA_PRIVATE, device->cap_mask);
-		device->privatecnt++;
-		err = dma_chan_get(chan);
-		if (err) {
-			pr_debug("%s: failed to get %s: (%d)\n",
-				__func__, dma_chan_name(chan), err);
-			chan = NULL;
-			if (--device->privatecnt == 0)
-				dma_cap_clear(DMA_PRIVATE, device->cap_mask);
-		}
-	}
+	chan = find_candidate(device, &mask, NULL, NULL);
 
 	mutex_unlock(&dma_list_mutex);
 
-	return chan;
+	return IS_ERR(chan) ? NULL : chan;
 }
 EXPORT_SYMBOL_GPL(dma_get_any_slave_channel);
 
@@ -621,35 +649,15 @@ struct dma_chan *__dma_request_channel(const dma_cap_mask_t *mask,
 {
 	struct dma_device *device, *_d;
 	struct dma_chan *chan = NULL;
-	int err;
 
 	/* Find a channel */
 	mutex_lock(&dma_list_mutex);
 	list_for_each_entry_safe(device, _d, &dma_device_list, global_node) {
-		chan = private_candidate(mask, device, fn, fn_param);
-		if (chan) {
-			/* Found a suitable channel, try to grab, prep, and
-			 * return it.  We first set DMA_PRIVATE to disable
-			 * balance_ref_count as this channel will not be
-			 * published in the general-purpose allocator
-			 */
-			dma_cap_set(DMA_PRIVATE, device->cap_mask);
-			device->privatecnt++;
-			err = dma_chan_get(chan);
+		chan = find_candidate(device, mask, fn, fn_param);
+		if (!IS_ERR(chan))
+			break;
 
-			if (err == -ENODEV) {
-				pr_debug("%s: %s module removed\n",
-					 __func__, dma_chan_name(chan));
-				list_del_rcu(&device->global_node);
-			} else if (err)
-				pr_debug("%s: failed to get %s: (%d)\n",
-					 __func__, dma_chan_name(chan), err);
-			else
-				break;
-			if (--device->privatecnt == 0)
-				dma_cap_clear(DMA_PRIVATE, device->cap_mask);
-			chan = NULL;
-		}
+		chan = NULL;
 	}
 	mutex_unlock(&dma_list_mutex);
 
@@ -662,27 +670,73 @@ struct dma_chan *__dma_request_channel(const dma_cap_mask_t *mask,
 }
 EXPORT_SYMBOL_GPL(__dma_request_channel);
 
+static const struct dma_slave_map *dma_filter_match(struct dma_device *device,
+						    const char *name,
+						    struct device *dev)
+{
+	int i;
+
+	if (!device->filter.mapcnt)
+		return NULL;
+
+	for (i = 0; i < device->filter.mapcnt; i++) {
+		const struct dma_slave_map *map = &device->filter.map[i];
+
+		if (!strcmp(map->devname, dev_name(dev)) &&
+		    !strcmp(map->slave, name))
+			return map;
+	}
+
+	return NULL;
+}
+
 /**
- * dma_request_slave_channel_reason - try to allocate an exclusive slave channel
+ * dma_request_chan - try to allocate an exclusive slave channel
  * @dev:	pointer to client device structure
  * @name:	slave channel name
  *
  * Returns pointer to appropriate DMA channel on success or an error pointer.
  */
-struct dma_chan *dma_request_slave_channel_reason(struct device *dev,
-						  const char *name)
+struct dma_chan *dma_request_chan(struct device *dev, const char *name)
 {
+	struct dma_device *d, *_d;
+	struct dma_chan *chan = NULL;
+
 	/* If device-tree is present get slave info from here */
 	if (dev->of_node)
-		return of_dma_request_slave_channel(dev->of_node, name);
+		chan = of_dma_request_slave_channel(dev->of_node, name);
 
 	/* If device was enumerated by ACPI get slave info from here */
-	if (ACPI_HANDLE(dev))
-		return acpi_dma_request_slave_chan_by_name(dev, name);
+	if (has_acpi_companion(dev) && !chan)
+		chan = acpi_dma_request_slave_chan_by_name(dev, name);
+
+	if (chan) {
+		/* Valid channel found or requester need to be deferred */
+		if (!IS_ERR(chan) || PTR_ERR(chan) == -EPROBE_DEFER)
+			return chan;
+	}
 
-	return ERR_PTR(-ENODEV);
+	/* Try to find the channel via the DMA filter map(s) */
+	mutex_lock(&dma_list_mutex);
+	list_for_each_entry_safe(d, _d, &dma_device_list, global_node) {
+		dma_cap_mask_t mask;
+		const struct dma_slave_map *map = dma_filter_match(d, name, dev);
+
+		if (!map)
+			continue;
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+
+		chan = find_candidate(d, &mask, d->filter.fn, map->param);
+		if (!IS_ERR(chan))
+			break;
+	}
+	mutex_unlock(&dma_list_mutex);
+
+	return chan ? chan : ERR_PTR(-EPROBE_DEFER);
 }
-EXPORT_SYMBOL_GPL(dma_request_slave_channel_reason);
+EXPORT_SYMBOL_GPL(dma_request_chan);
 
 /**
  * dma_request_slave_channel - try to allocate an exclusive slave channel
@@ -694,17 +748,35 @@ EXPORT_SYMBOL_GPL(dma_request_slave_channel_reason);
 struct dma_chan *dma_request_slave_channel(struct device *dev,
 					   const char *name)
 {
-	struct dma_chan *ch = dma_request_slave_channel_reason(dev, name);
+	struct dma_chan *ch = dma_request_chan(dev, name);
 	if (IS_ERR(ch))
 		return NULL;
 
-	dma_cap_set(DMA_PRIVATE, ch->device->cap_mask);
-	ch->device->privatecnt++;
-
 	return ch;
 }
 EXPORT_SYMBOL_GPL(dma_request_slave_channel);
 
+/**
+ * dma_request_chan_by_mask - allocate a channel satisfying certain capabilities
+ * @mask: capabilities that the channel must satisfy
+ *
+ * Returns pointer to appropriate DMA channel on success or an error pointer.
+ */
+struct dma_chan *dma_request_chan_by_mask(const dma_cap_mask_t *mask)
+{
+	struct dma_chan *chan;
+
+	if (!mask)
+		return ERR_PTR(-ENODEV);
+
+	chan = __dma_request_channel(mask, NULL, NULL);
+	if (!chan)
+		chan = ERR_PTR(-ENODEV);
+
+	return chan;
+}
+EXPORT_SYMBOL_GPL(dma_request_chan_by_mask);
+
 void dma_release_channel(struct dma_chan *chan)
 {
 	mutex_lock(&dma_list_mutex);
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index 7067b6ddc1db..5ad0ec1f0e29 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -536,16 +536,17 @@ EXPORT_SYMBOL(dw_dma_get_dst_addr);
 
 /* Called with dwc->lock held and all DMAC interrupts disabled */
 static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
-		u32 status_err, u32 status_xfer)
+		u32 status_block, u32 status_err, u32 status_xfer)
 {
 	unsigned long flags;
 
-	if (dwc->mask) {
+	if (status_block & dwc->mask) {
 		void (*callback)(void *param);
 		void *callback_param;
 
 		dev_vdbg(chan2dev(&dwc->chan), "new cyclic period llp 0x%08x\n",
 				channel_readl(dwc, LLP));
+		dma_writel(dw, CLEAR.BLOCK, dwc->mask);
 
 		callback = dwc->cdesc->period_callback;
 		callback_param = dwc->cdesc->period_callback_param;
@@ -577,6 +578,7 @@ static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
 		channel_writel(dwc, CTL_LO, 0);
 		channel_writel(dwc, CTL_HI, 0);
 
+		dma_writel(dw, CLEAR.BLOCK, dwc->mask);
 		dma_writel(dw, CLEAR.ERROR, dwc->mask);
 		dma_writel(dw, CLEAR.XFER, dwc->mask);
 
@@ -585,6 +587,9 @@ static void dwc_handle_cyclic(struct dw_dma *dw, struct dw_dma_chan *dwc,
 
 		spin_unlock_irqrestore(&dwc->lock, flags);
 	}
+
+	/* Re-enable interrupts */
+	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
 }
 
 /* ------------------------------------------------------------------------- */
@@ -593,10 +598,12 @@ static void dw_dma_tasklet(unsigned long data)
 {
 	struct dw_dma *dw = (struct dw_dma *)data;
 	struct dw_dma_chan *dwc;
+	u32 status_block;
 	u32 status_xfer;
 	u32 status_err;
 	int i;
 
+	status_block = dma_readl(dw, RAW.BLOCK);
 	status_xfer = dma_readl(dw, RAW.XFER);
 	status_err = dma_readl(dw, RAW.ERROR);
 
@@ -605,16 +612,15 @@ static void dw_dma_tasklet(unsigned long data)
 	for (i = 0; i < dw->dma.chancnt; i++) {
 		dwc = &dw->chan[i];
 		if (test_bit(DW_DMA_IS_CYCLIC, &dwc->flags))
-			dwc_handle_cyclic(dw, dwc, status_err, status_xfer);
+			dwc_handle_cyclic(dw, dwc, status_block, status_err,
+					status_xfer);
 		else if (status_err & (1 << i))
 			dwc_handle_error(dw, dwc);
 		else if (status_xfer & (1 << i))
 			dwc_scan_descriptors(dw, dwc);
 	}
 
-	/*
-	 * Re-enable interrupts.
-	 */
+	/* Re-enable interrupts */
 	channel_set_bit(dw, MASK.XFER, dw->all_chan_mask);
 	channel_set_bit(dw, MASK.ERROR, dw->all_chan_mask);
 }
@@ -622,12 +628,17 @@ static void dw_dma_tasklet(unsigned long data)
 static irqreturn_t dw_dma_interrupt(int irq, void *dev_id)
 {
 	struct dw_dma *dw = dev_id;
-	u32 status = dma_readl(dw, STATUS_INT);
+	u32 status;
 
+	/* Check if we have any interrupt from the DMAC which is not in use */
+	if (!dw->in_use)
+		return IRQ_NONE;
+
+	status = dma_readl(dw, STATUS_INT);
 	dev_vdbg(dw->dma.dev, "%s: status=0x%x\n", __func__, status);
 
 	/* Check if we have any interrupt from the DMAC */
-	if (!status || !dw->in_use)
+	if (!status)
 		return IRQ_NONE;
 
 	/*
@@ -635,6 +646,7 @@ static irqreturn_t dw_dma_interrupt(int irq, void *dev_id)
 	 * softirq handler.
 	 */
 	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
+	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 	channel_clear_bit(dw, MASK.ERROR, dw->all_chan_mask);
 
 	status = dma_readl(dw, STATUS_INT);
@@ -645,6 +657,7 @@ static irqreturn_t dw_dma_interrupt(int irq, void *dev_id)
 
 		/* Try to recover */
 		channel_clear_bit(dw, MASK.XFER, (1 << 8) - 1);
+		channel_clear_bit(dw, MASK.BLOCK, (1 << 8) - 1);
 		channel_clear_bit(dw, MASK.SRC_TRAN, (1 << 8) - 1);
 		channel_clear_bit(dw, MASK.DST_TRAN, (1 << 8) - 1);
 		channel_clear_bit(dw, MASK.ERROR, (1 << 8) - 1);
@@ -1111,6 +1124,7 @@ static void dw_dma_off(struct dw_dma *dw)
 	dma_writel(dw, CFG, 0);
 
 	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
+	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
 	channel_clear_bit(dw, MASK.SRC_TRAN, dw->all_chan_mask);
 	channel_clear_bit(dw, MASK.DST_TRAN, dw->all_chan_mask);
 	channel_clear_bit(dw, MASK.ERROR, dw->all_chan_mask);
@@ -1216,6 +1230,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 
 	/* Disable interrupts */
 	channel_clear_bit(dw, MASK.XFER, dwc->mask);
+	channel_clear_bit(dw, MASK.BLOCK, dwc->mask);
 	channel_clear_bit(dw, MASK.ERROR, dwc->mask);
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
@@ -1245,7 +1260,7 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 int dw_dma_cyclic_start(struct dma_chan *chan)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
-	struct dw_dma		*dw = to_dw_dma(dwc->chan.device);
+	struct dw_dma		*dw = to_dw_dma(chan->device);
 	unsigned long		flags;
 
 	if (!test_bit(DW_DMA_IS_CYCLIC, &dwc->flags)) {
@@ -1255,25 +1270,10 @@ int dw_dma_cyclic_start(struct dma_chan *chan)
 
 	spin_lock_irqsave(&dwc->lock, flags);
 
-	/* Assert channel is idle */
-	if (dma_readl(dw, CH_EN) & dwc->mask) {
-		dev_err(chan2dev(&dwc->chan),
-			"%s: BUG: Attempted to start non-idle channel\n",
-			__func__);
-		dwc_dump_chan_regs(dwc);
-		spin_unlock_irqrestore(&dwc->lock, flags);
-		return -EBUSY;
-	}
-
-	dma_writel(dw, CLEAR.ERROR, dwc->mask);
-	dma_writel(dw, CLEAR.XFER, dwc->mask);
-
-	/* Setup DMAC channel registers */
-	channel_writel(dwc, LLP, dwc->cdesc->desc[0]->txd.phys);
-	channel_writel(dwc, CTL_LO, DWC_CTLL_LLP_D_EN | DWC_CTLL_LLP_S_EN);
-	channel_writel(dwc, CTL_HI, 0);
+	/* Enable interrupts to perform cyclic transfer */
+	channel_set_bit(dw, MASK.BLOCK, dwc->mask);
 
-	channel_set_bit(dw, CH_EN, dwc->mask);
+	dwc_dostart(dwc, dwc->cdesc->desc[0]);
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
@@ -1479,6 +1479,7 @@ void dw_dma_cyclic_free(struct dma_chan *chan)
 
 	dwc_chan_disable(dw, dwc);
 
+	dma_writel(dw, CLEAR.BLOCK, dwc->mask);
 	dma_writel(dw, CLEAR.ERROR, dwc->mask);
 	dma_writel(dw, CLEAR.XFER, dwc->mask);
 
@@ -1567,9 +1568,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 	/* Force dma off, just in case */
 	dw_dma_off(dw);
 
-	/* Disable BLOCK interrupts as well */
-	channel_clear_bit(dw, MASK.BLOCK, dw->all_chan_mask);
-
 	/* Create a pool of consistent memory blocks for hardware descriptors */
 	dw->desc_pool = dmam_pool_create("dw_dmac_desc_pool", chip->dev,
 					 sizeof(struct dw_desc), 4, 0);
diff --git a/drivers/dma/dw/pci.c b/drivers/dma/dw/pci.c
index 4c30fdd092b3..358f9689a3f5 100644
--- a/drivers/dma/dw/pci.c
+++ b/drivers/dma/dw/pci.c
@@ -108,6 +108,10 @@ static const struct pci_device_id dw_pci_id_table[] = {
 
 	/* Haswell */
 	{ PCI_VDEVICE(INTEL, 0x9c60) },
+
+	/* Broadwell */
+	{ PCI_VDEVICE(INTEL, 0x9ce0) },
+
 	{ }
 };
 MODULE_DEVICE_TABLE(pci, dw_pci_id_table);
diff --git a/drivers/dma/dw/platform.c b/drivers/dma/dw/platform.c
index 68a4815750b5..26edbe3a27ac 100644
--- a/drivers/dma/dw/platform.c
+++ b/drivers/dma/dw/platform.c
@@ -103,18 +103,21 @@ dw_dma_parse_dt(struct platform_device *pdev)
 	struct device_node *np = pdev->dev.of_node;
 	struct dw_dma_platform_data *pdata;
 	u32 tmp, arr[DW_DMA_MAX_NR_MASTERS];
+	u32 nr_channels;
 
 	if (!np) {
 		dev_err(&pdev->dev, "Missing DT data\n");
 		return NULL;
 	}
 
+	if (of_property_read_u32(np, "dma-channels", &nr_channels))
+		return NULL;
+
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
 		return NULL;
 
-	if (of_property_read_u32(np, "dma-channels", &pdata->nr_channels))
-		return NULL;
+	pdata->nr_channels = nr_channels;
 
 	if (of_property_read_bool(np, "is_private"))
 		pdata->is_private = true;
@@ -155,7 +158,6 @@ static int dw_probe(struct platform_device *pdev)
 	struct dw_dma_chip *chip;
 	struct device *dev = &pdev->dev;
 	struct resource *mem;
-	const struct acpi_device_id *id;
 	struct dw_dma_platform_data *pdata;
 	int err;
 
@@ -179,11 +181,6 @@ static int dw_probe(struct platform_device *pdev)
 	pdata = dev_get_platdata(dev);
 	if (!pdata)
 		pdata = dw_dma_parse_dt(pdev);
-	if (!pdata && has_acpi_companion(dev)) {
-		id = acpi_match_device(dev->driver->acpi_match_table, dev);
-		if (id)
-			pdata = (struct dw_dma_platform_data *)id->driver_data;
-	}
 
 	chip->dev = dev;
 
@@ -239,7 +236,19 @@ static void dw_shutdown(struct platform_device *pdev)
 {
 	struct dw_dma_chip *chip = platform_get_drvdata(pdev);
 
+	/*
+	 * We have to call dw_dma_disable() to stop any ongoing transfer. On
+	 * some platforms we can't do that since DMA device is powered off.
+	 * Moreover we have no possibility to check if the platform is affected
+	 * or not. That's why we call pm_runtime_get_sync() / pm_runtime_put()
+	 * unconditionally. On the other hand we can't use
+	 * pm_runtime_suspended() because runtime PM framework is not fully
+	 * used by the driver.
+	 */
+	pm_runtime_get_sync(chip->dev);
 	dw_dma_disable(chip);
+	pm_runtime_put_sync_suspend(chip->dev);
+
 	clk_disable_unprepare(chip->clk);
 }
 
@@ -252,17 +261,8 @@ MODULE_DEVICE_TABLE(of, dw_dma_of_id_table);
 #endif
 
 #ifdef CONFIG_ACPI
-static struct dw_dma_platform_data dw_dma_acpi_pdata = {
-	.nr_channels = 8,
-	.is_private = true,
-	.chan_allocation_order = CHAN_ALLOCATION_ASCENDING,
-	.chan_priority = CHAN_PRIORITY_ASCENDING,
-	.block_size = 4095,
-	.nr_masters = 2,
-};
-
 static const struct acpi_device_id dw_dma_acpi_id_table[] = {
-	{ "INTL9C60", (kernel_ulong_t)&dw_dma_acpi_pdata },
+	{ "INTL9C60", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, dw_dma_acpi_id_table);
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 16fe773fb846..e3d7fcb69b4c 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -113,6 +113,9 @@
 #define GET_NUM_REGN(x)		((x & 0x300000) >> 20) /* bits 20-21 */
 #define CHMAP_EXIST		BIT(24)
 
+/* CCSTAT register */
+#define EDMA_CCSTAT_ACTV	BIT(4)
+
 /*
  * Max of 20 segments per channel to conserve PaRAM slots
  * Also note that MAX_NR_SG should be atleast the no.of periods
@@ -484,7 +487,7 @@ static void edma_read_slot(struct edma_cc *ecc, unsigned slot,
  */
 static int edma_alloc_slot(struct edma_cc *ecc, int slot)
 {
-	if (slot > 0) {
+	if (slot >= 0) {
 		slot = EDMA_CHAN_SLOT(slot);
 		/* Requesting entry paRAM slot for a HW triggered channel. */
 		if (ecc->chmap_exist && slot < ecc->num_channels)
@@ -1680,9 +1683,20 @@ static void edma_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&echan->vchan.lock, flags);
 }
 
+/*
+ * This limit exists to avoid a possible infinite loop when waiting for proof
+ * that a particular transfer is completed. This limit can be hit if there
+ * are large bursts to/from slow devices or the CPU is never able to catch
+ * the DMA hardware idle. On an AM335x transfering 48 bytes from the UART
+ * RX-FIFO, as many as 55 loops have been seen.
+ */
+#define EDMA_MAX_TR_WAIT_LOOPS 1000
+
 static u32 edma_residue(struct edma_desc *edesc)
 {
 	bool dst = edesc->direction == DMA_DEV_TO_MEM;
+	int loop_count = EDMA_MAX_TR_WAIT_LOOPS;
+	struct edma_chan *echan = edesc->echan;
 	struct edma_pset *pset = edesc->pset;
 	dma_addr_t done, pos;
 	int i;
@@ -1691,7 +1705,32 @@ static u32 edma_residue(struct edma_desc *edesc)
 	 * We always read the dst/src position from the first RamPar
 	 * pset. That's the one which is active now.
 	 */
-	pos = edma_get_position(edesc->echan->ecc, edesc->echan->slot[0], dst);
+	pos = edma_get_position(echan->ecc, echan->slot[0], dst);
+
+	/*
+	 * "pos" may represent a transfer request that is still being
+	 * processed by the EDMACC or EDMATC. We will busy wait until
+	 * any one of the situations occurs:
+	 *   1. the DMA hardware is idle
+	 *   2. a new transfer request is setup
+	 *   3. we hit the loop limit
+	 */
+	while (edma_read(echan->ecc, EDMA_CCSTAT) & EDMA_CCSTAT_ACTV) {
+		/* check if a new transfer request is setup */
+		if (edma_get_position(echan->ecc,
+				      echan->slot[0], dst) != pos) {
+			break;
+		}
+
+		if (!--loop_count) {
+			dev_dbg_ratelimited(echan->vchan.chan.device->dev,
+				"%s: timeout waiting for PaRAM update\n",
+				__func__);
+			break;
+		}
+
+		cpu_relax();
+	}
 
 	/*
 	 * Cyclic is simple. Just subtract pset[0].addr from pos.
@@ -2314,6 +2353,10 @@ static int edma_probe(struct platform_device *pdev)
 		edma_set_chmap(&ecc->slave_chans[i], ecc->dummy_slot);
 	}
 
+	ecc->dma_slave.filter.map = info->slave_map;
+	ecc->dma_slave.filter.mapcnt = info->slavecnt;
+	ecc->dma_slave.filter.fn = edma_filter_fn;
+
 	ret = dma_async_device_register(&ecc->dma_slave);
 	if (ret) {
 		dev_err(dev, "slave ddev registration failed (%d)\n", ret);
@@ -2421,7 +2464,13 @@ static struct platform_driver edma_driver = {
 	},
 };
 
+static int edma_tptc_probe(struct platform_device *pdev)
+{
+	return 0;
+}
+
 static struct platform_driver edma_tptc_driver = {
+	.probe		= edma_tptc_probe,
 	.driver = {
 		.name	= "edma3-tptc",
 		.of_match_table = edma_tptc_of_ids,
diff --git a/drivers/dma/fsl-edma.c b/drivers/dma/fsl-edma.c
index 915eec3cc279..be2e62b87948 100644
--- a/drivers/dma/fsl-edma.c
+++ b/drivers/dma/fsl-edma.c
@@ -116,6 +116,10 @@
 				BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) | \
 				BIT(DMA_SLAVE_BUSWIDTH_4_BYTES) | \
 				BIT(DMA_SLAVE_BUSWIDTH_8_BYTES)
+enum fsl_edma_pm_state {
+	RUNNING = 0,
+	SUSPENDED,
+};
 
 struct fsl_edma_hw_tcd {
 	__le32	saddr;
@@ -147,6 +151,9 @@ struct fsl_edma_slave_config {
 struct fsl_edma_chan {
 	struct virt_dma_chan		vchan;
 	enum dma_status			status;
+	enum fsl_edma_pm_state		pm_state;
+	bool				idle;
+	u32				slave_id;
 	struct fsl_edma_engine		*edma;
 	struct fsl_edma_desc		*edesc;
 	struct fsl_edma_slave_config	fsc;
@@ -298,6 +305,7 @@ static int fsl_edma_terminate_all(struct dma_chan *chan)
 	spin_lock_irqsave(&fsl_chan->vchan.lock, flags);
 	fsl_edma_disable_request(fsl_chan);
 	fsl_chan->edesc = NULL;
+	fsl_chan->idle = true;
 	vchan_get_all_descriptors(&fsl_chan->vchan, &head);
 	spin_unlock_irqrestore(&fsl_chan->vchan.lock, flags);
 	vchan_dma_desc_free_list(&fsl_chan->vchan, &head);
@@ -313,6 +321,7 @@ static int fsl_edma_pause(struct dma_chan *chan)
 	if (fsl_chan->edesc) {
 		fsl_edma_disable_request(fsl_chan);
 		fsl_chan->status = DMA_PAUSED;
+		fsl_chan->idle = true;
 	}
 	spin_unlock_irqrestore(&fsl_chan->vchan.lock, flags);
 	return 0;
@@ -327,6 +336,7 @@ static int fsl_edma_resume(struct dma_chan *chan)
 	if (fsl_chan->edesc) {
 		fsl_edma_enable_request(fsl_chan);
 		fsl_chan->status = DMA_IN_PROGRESS;
+		fsl_chan->idle = false;
 	}
 	spin_unlock_irqrestore(&fsl_chan->vchan.lock, flags);
 	return 0;
@@ -648,6 +658,7 @@ static void fsl_edma_xfer_desc(struct fsl_edma_chan *fsl_chan)
 	fsl_edma_set_tcd_regs(fsl_chan, fsl_chan->edesc->tcd[0].vtcd);
 	fsl_edma_enable_request(fsl_chan);
 	fsl_chan->status = DMA_IN_PROGRESS;
+	fsl_chan->idle = false;
 }
 
 static irqreturn_t fsl_edma_tx_handler(int irq, void *dev_id)
@@ -676,6 +687,7 @@ static irqreturn_t fsl_edma_tx_handler(int irq, void *dev_id)
 				vchan_cookie_complete(&fsl_chan->edesc->vdesc);
 				fsl_chan->edesc = NULL;
 				fsl_chan->status = DMA_COMPLETE;
+				fsl_chan->idle = true;
 			} else {
 				vchan_cyclic_callback(&fsl_chan->edesc->vdesc);
 			}
@@ -704,6 +716,7 @@ static irqreturn_t fsl_edma_err_handler(int irq, void *dev_id)
 			edma_writeb(fsl_edma, EDMA_CERR_CERR(ch),
 				fsl_edma->membase + EDMA_CERR);
 			fsl_edma->chans[ch].status = DMA_ERROR;
+			fsl_edma->chans[ch].idle = true;
 		}
 	}
 	return IRQ_HANDLED;
@@ -724,6 +737,12 @@ static void fsl_edma_issue_pending(struct dma_chan *chan)
 
 	spin_lock_irqsave(&fsl_chan->vchan.lock, flags);
 
+	if (unlikely(fsl_chan->pm_state != RUNNING)) {
+		spin_unlock_irqrestore(&fsl_chan->vchan.lock, flags);
+		/* cannot submit due to suspend */
+		return;
+	}
+
 	if (vchan_issue_pending(&fsl_chan->vchan) && !fsl_chan->edesc)
 		fsl_edma_xfer_desc(fsl_chan);
 
@@ -735,6 +754,7 @@ static struct dma_chan *fsl_edma_xlate(struct of_phandle_args *dma_spec,
 {
 	struct fsl_edma_engine *fsl_edma = ofdma->of_dma_data;
 	struct dma_chan *chan, *_chan;
+	struct fsl_edma_chan *fsl_chan;
 	unsigned long chans_per_mux = fsl_edma->n_chans / DMAMUX_NR;
 
 	if (dma_spec->args_count != 2)
@@ -748,8 +768,10 @@ static struct dma_chan *fsl_edma_xlate(struct of_phandle_args *dma_spec,
 			chan = dma_get_slave_channel(chan);
 			if (chan) {
 				chan->device->privatecnt++;
-				fsl_edma_chan_mux(to_fsl_edma_chan(chan),
-					dma_spec->args[1], true);
+				fsl_chan = to_fsl_edma_chan(chan);
+				fsl_chan->slave_id = dma_spec->args[1];
+				fsl_edma_chan_mux(fsl_chan, fsl_chan->slave_id,
+						true);
 				mutex_unlock(&fsl_edma->fsl_edma_mutex);
 				return chan;
 			}
@@ -888,7 +910,9 @@ static int fsl_edma_probe(struct platform_device *pdev)
 		struct fsl_edma_chan *fsl_chan = &fsl_edma->chans[i];
 
 		fsl_chan->edma = fsl_edma;
-
+		fsl_chan->pm_state = RUNNING;
+		fsl_chan->slave_id = 0;
+		fsl_chan->idle = true;
 		fsl_chan->vchan.desc_free = fsl_edma_free_desc;
 		vchan_init(&fsl_chan->vchan, &fsl_edma->dma_dev);
 
@@ -959,6 +983,60 @@ static int fsl_edma_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int fsl_edma_suspend_late(struct device *dev)
+{
+	struct fsl_edma_engine *fsl_edma = dev_get_drvdata(dev);
+	struct fsl_edma_chan *fsl_chan;
+	unsigned long flags;
+	int i;
+
+	for (i = 0; i < fsl_edma->n_chans; i++) {
+		fsl_chan = &fsl_edma->chans[i];
+		spin_lock_irqsave(&fsl_chan->vchan.lock, flags);
+		/* Make sure chan is idle or will force disable. */
+		if (unlikely(!fsl_chan->idle)) {
+			dev_warn(dev, "WARN: There is non-idle channel.");
+			fsl_edma_disable_request(fsl_chan);
+			fsl_edma_chan_mux(fsl_chan, 0, false);
+		}
+
+		fsl_chan->pm_state = SUSPENDED;
+		spin_unlock_irqrestore(&fsl_chan->vchan.lock, flags);
+	}
+
+	return 0;
+}
+
+static int fsl_edma_resume_early(struct device *dev)
+{
+	struct fsl_edma_engine *fsl_edma = dev_get_drvdata(dev);
+	struct fsl_edma_chan *fsl_chan;
+	int i;
+
+	for (i = 0; i < fsl_edma->n_chans; i++) {
+		fsl_chan = &fsl_edma->chans[i];
+		fsl_chan->pm_state = RUNNING;
+		edma_writew(fsl_edma, 0x0, fsl_edma->membase + EDMA_TCD_CSR(i));
+		if (fsl_chan->slave_id != 0)
+			fsl_edma_chan_mux(fsl_chan, fsl_chan->slave_id, true);
+	}
+
+	edma_writel(fsl_edma, EDMA_CR_ERGA | EDMA_CR_ERCA,
+			fsl_edma->membase + EDMA_CR);
+
+	return 0;
+}
+
+/*
+ * eDMA provides the service to others, so it should be suspend late
+ * and resume early. When eDMA suspend, all of the clients should stop
+ * the DMA data transmission and let the channel idle.
+ */
+static const struct dev_pm_ops fsl_edma_pm_ops = {
+	.suspend_late   = fsl_edma_suspend_late,
+	.resume_early   = fsl_edma_resume_early,
+};
+
 static const struct of_device_id fsl_edma_dt_ids[] = {
 	{ .compatible = "fsl,vf610-edma", },
 	{ /* sentinel */ }
@@ -969,6 +1047,7 @@ static struct platform_driver fsl_edma_driver = {
 	.driver		= {
 		.name	= "fsl-edma",
 		.of_match_table = fsl_edma_dt_ids,
+		.pm     = &fsl_edma_pm_ops,
 	},
 	.probe          = fsl_edma_probe,
 	.remove		= fsl_edma_remove,
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index 2209f75fdf05..aac85c30c2cf 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -522,6 +522,8 @@ static dma_cookie_t fsldma_run_tx_complete_actions(struct fsldma_chan *chan,
 			chan_dbg(chan, "LD %p callback\n", desc);
 			txd->callback(txd->callback_param);
 		}
+
+		dma_descriptor_unmap(txd);
 	}
 
 	/* Run any dependencies */
diff --git a/drivers/dma/hsu/hsu.c b/drivers/dma/hsu/hsu.c
index 823ad728aecf..eef145edb936 100644
--- a/drivers/dma/hsu/hsu.c
+++ b/drivers/dma/hsu/hsu.c
@@ -228,6 +228,8 @@ static struct dma_async_tx_descriptor *hsu_dma_prep_slave_sg(
 	for_each_sg(sgl, sg, sg_len, i) {
 		desc->sg[i].addr = sg_dma_address(sg);
 		desc->sg[i].len = sg_dma_len(sg);
+
+		desc->length += sg_dma_len(sg);
 	}
 
 	desc->nents = sg_len;
@@ -249,21 +251,10 @@ static void hsu_dma_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&hsuc->vchan.lock, flags);
 }
 
-static size_t hsu_dma_desc_size(struct hsu_dma_desc *desc)
-{
-	size_t bytes = 0;
-	unsigned int i;
-
-	for (i = desc->active; i < desc->nents; i++)
-		bytes += desc->sg[i].len;
-
-	return bytes;
-}
-
 static size_t hsu_dma_active_desc_size(struct hsu_dma_chan *hsuc)
 {
 	struct hsu_dma_desc *desc = hsuc->desc;
-	size_t bytes = hsu_dma_desc_size(desc);
+	size_t bytes = desc->length;
 	int i;
 
 	i = desc->active % HSU_DMA_CHAN_NR_DESC;
@@ -294,7 +285,7 @@ static enum dma_status hsu_dma_tx_status(struct dma_chan *chan,
 		dma_set_residue(state, bytes);
 		status = hsuc->desc->status;
 	} else if (vdesc) {
-		bytes = hsu_dma_desc_size(to_hsu_dma_desc(vdesc));
+		bytes = to_hsu_dma_desc(vdesc)->length;
 		dma_set_residue(state, bytes);
 	}
 	spin_unlock_irqrestore(&hsuc->vchan.lock, flags);
diff --git a/drivers/dma/hsu/hsu.h b/drivers/dma/hsu/hsu.h
index f06579c6d548..578a8ee8cd05 100644
--- a/drivers/dma/hsu/hsu.h
+++ b/drivers/dma/hsu/hsu.h
@@ -65,6 +65,7 @@ struct hsu_dma_desc {
 	enum dma_transfer_direction direction;
 	struct hsu_dma_sg *sg;
 	unsigned int nents;
+	size_t length;
 	unsigned int active;
 	enum dma_status status;
 };
diff --git a/drivers/dma/idma64.c b/drivers/dma/idma64.c
index 7d56b47e4fcf..3cb7b2c78197 100644
--- a/drivers/dma/idma64.c
+++ b/drivers/dma/idma64.c
@@ -178,20 +178,12 @@ static irqreturn_t idma64_irq(int irq, void *dev)
 	if (!status)
 		return IRQ_NONE;
 
-	/* Disable interrupts */
-	channel_clear_bit(idma64, MASK(XFER), idma64->all_chan_mask);
-	channel_clear_bit(idma64, MASK(ERROR), idma64->all_chan_mask);
-
 	status_xfer = dma_readl(idma64, RAW(XFER));
 	status_err = dma_readl(idma64, RAW(ERROR));
 
 	for (i = 0; i < idma64->dma.chancnt; i++)
 		idma64_chan_irq(idma64, i, status_err, status_xfer);
 
-	/* Re-enable interrupts */
-	channel_set_bit(idma64, MASK(XFER), idma64->all_chan_mask);
-	channel_set_bit(idma64, MASK(ERROR), idma64->all_chan_mask);
-
 	return IRQ_HANDLED;
 }
 
@@ -239,7 +231,7 @@ static void idma64_vdesc_free(struct virt_dma_desc *vdesc)
 	idma64_desc_free(idma64c, to_idma64_desc(vdesc));
 }
 
-static u64 idma64_hw_desc_fill(struct idma64_hw_desc *hw,
+static void idma64_hw_desc_fill(struct idma64_hw_desc *hw,
 		struct dma_slave_config *config,
 		enum dma_transfer_direction direction, u64 llp)
 {
@@ -276,26 +268,26 @@ static u64 idma64_hw_desc_fill(struct idma64_hw_desc *hw,
 		     IDMA64C_CTLL_SRC_WIDTH(src_width);
 
 	lli->llp = llp;
-	return hw->llp;
 }
 
 static void idma64_desc_fill(struct idma64_chan *idma64c,
 		struct idma64_desc *desc)
 {
 	struct dma_slave_config *config = &idma64c->config;
-	struct idma64_hw_desc *hw = &desc->hw[desc->ndesc - 1];
+	unsigned int i = desc->ndesc;
+	struct idma64_hw_desc *hw = &desc->hw[i - 1];
 	struct idma64_lli *lli = hw->lli;
 	u64 llp = 0;
-	unsigned int i = desc->ndesc;
 
 	/* Fill the hardware descriptors and link them to a list */
 	do {
 		hw = &desc->hw[--i];
-		llp = idma64_hw_desc_fill(hw, config, desc->direction, llp);
+		idma64_hw_desc_fill(hw, config, desc->direction, llp);
+		llp = hw->llp;
 		desc->length += hw->len;
 	} while (i);
 
-	/* Trigger interrupt after last block */
+	/* Trigger an interrupt after the last block is transfered */
 	lli->ctllo |= IDMA64C_CTLL_INT_EN;
 }
 
@@ -596,6 +588,8 @@ static int idma64_probe(struct idma64_chip *chip)
 
 	idma64->dma.dev = chip->dev;
 
+	dma_set_max_seg_size(idma64->dma.dev, IDMA64C_CTLH_BLOCK_TS_MASK);
+
 	ret = dma_async_device_register(&idma64->dma);
 	if (ret)
 		return ret;
diff --git a/drivers/dma/idma64.h b/drivers/dma/idma64.h
index f6aeff0af8a5..8423f13ed0da 100644
--- a/drivers/dma/idma64.h
+++ b/drivers/dma/idma64.h
@@ -54,7 +54,8 @@
 #define IDMA64C_CTLL_LLP_S_EN		(1 << 28)	/* src block chain */
 
 /* Bitfields in CTL_HI */
-#define IDMA64C_CTLH_BLOCK_TS(x)	((x) & ((1 << 17) - 1))
+#define IDMA64C_CTLH_BLOCK_TS_MASK	((1 << 17) - 1)
+#define IDMA64C_CTLH_BLOCK_TS(x)	((x) & IDMA64C_CTLH_BLOCK_TS_MASK)
 #define IDMA64C_CTLH_DONE		(1 << 17)
 
 /* Bitfields in CFG_LO */
diff --git a/drivers/dma/img-mdc-dma.c b/drivers/dma/img-mdc-dma.c
index 9ca56830cc63..a4c53be482cf 100644
--- a/drivers/dma/img-mdc-dma.c
+++ b/drivers/dma/img-mdc-dma.c
@@ -651,6 +651,48 @@ static enum dma_status mdc_tx_status(struct dma_chan *chan,
 	return ret;
 }
 
+static unsigned int mdc_get_new_events(struct mdc_chan *mchan)
+{
+	u32 val, processed, done1, done2;
+	unsigned int ret;
+
+	val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
+	processed = (val >> MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT) &
+				MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK;
+	/*
+	 * CMDS_DONE may have incremented between reading CMDS_PROCESSED
+	 * and clearing INT_ACTIVE.  Re-read CMDS_PROCESSED to ensure we
+	 * didn't miss a command completion.
+	 */
+	do {
+		val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
+
+		done1 = (val >> MDC_CMDS_PROCESSED_CMDS_DONE_SHIFT) &
+			MDC_CMDS_PROCESSED_CMDS_DONE_MASK;
+
+		val &= ~((MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK <<
+			  MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT) |
+			 MDC_CMDS_PROCESSED_INT_ACTIVE);
+
+		val |= done1 << MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT;
+
+		mdc_chan_writel(mchan, val, MDC_CMDS_PROCESSED);
+
+		val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
+
+		done2 = (val >> MDC_CMDS_PROCESSED_CMDS_DONE_SHIFT) &
+			MDC_CMDS_PROCESSED_CMDS_DONE_MASK;
+	} while (done1 != done2);
+
+	if (done1 >= processed)
+		ret = done1 - processed;
+	else
+		ret = ((MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK + 1) -
+			processed) + done1;
+
+	return ret;
+}
+
 static int mdc_terminate_all(struct dma_chan *chan)
 {
 	struct mdc_chan *mchan = to_mdc_chan(chan);
@@ -667,6 +709,8 @@ static int mdc_terminate_all(struct dma_chan *chan)
 	mchan->desc = NULL;
 	vchan_get_all_descriptors(&mchan->vc, &head);
 
+	mdc_get_new_events(mchan);
+
 	spin_unlock_irqrestore(&mchan->vc.lock, flags);
 
 	if (mdesc)
@@ -703,35 +747,17 @@ static irqreturn_t mdc_chan_irq(int irq, void *dev_id)
 {
 	struct mdc_chan *mchan = (struct mdc_chan *)dev_id;
 	struct mdc_tx_desc *mdesc;
-	u32 val, processed, done1, done2;
-	unsigned int i;
+	unsigned int i, new_events;
 
 	spin_lock(&mchan->vc.lock);
 
-	val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
-	processed = (val >> MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT) &
-		MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK;
-	/*
-	 * CMDS_DONE may have incremented between reading CMDS_PROCESSED
-	 * and clearing INT_ACTIVE.  Re-read CMDS_PROCESSED to ensure we
-	 * didn't miss a command completion.
-	 */
-	do {
-		val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
-		done1 = (val >> MDC_CMDS_PROCESSED_CMDS_DONE_SHIFT) &
-			MDC_CMDS_PROCESSED_CMDS_DONE_MASK;
-		val &= ~((MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK <<
-			  MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT) |
-			 MDC_CMDS_PROCESSED_INT_ACTIVE);
-		val |= done1 << MDC_CMDS_PROCESSED_CMDS_PROCESSED_SHIFT;
-		mdc_chan_writel(mchan, val, MDC_CMDS_PROCESSED);
-		val = mdc_chan_readl(mchan, MDC_CMDS_PROCESSED);
-		done2 = (val >> MDC_CMDS_PROCESSED_CMDS_DONE_SHIFT) &
-			MDC_CMDS_PROCESSED_CMDS_DONE_MASK;
-	} while (done1 != done2);
-
 	dev_dbg(mdma2dev(mchan->mdma), "IRQ on channel %d\n", mchan->chan_nr);
 
+	new_events = mdc_get_new_events(mchan);
+
+	if (!new_events)
+		goto out;
+
 	mdesc = mchan->desc;
 	if (!mdesc) {
 		dev_warn(mdma2dev(mchan->mdma),
@@ -740,8 +766,7 @@ static irqreturn_t mdc_chan_irq(int irq, void *dev_id)
 		goto out;
 	}
 
-	for (i = processed; i != done1;
-	     i = (i + 1) % (MDC_CMDS_PROCESSED_CMDS_PROCESSED_MASK + 1)) {
+	for (i = 0; i < new_events; i++) {
 		/*
 		 * The first interrupt in a transfer indicates that the
 		 * command list has been loaded, not that a command has
@@ -979,7 +1004,6 @@ static int mdc_dma_remove(struct platform_device *pdev)
 				 vc.chan.device_node) {
 		list_del(&mchan->vc.chan.device_node);
 
-		synchronize_irq(mchan->irq);
 		devm_free_irq(&pdev->dev, mchan->irq, mchan);
 
 		tasklet_kill(&mchan->vc.task);
diff --git a/drivers/dma/ioat/dca.c b/drivers/dma/ioat/dca.c
index 2cb7c308d5c7..0b9b6b07db9e 100644
--- a/drivers/dma/ioat/dca.c
+++ b/drivers/dma/ioat/dca.c
@@ -224,7 +224,7 @@ static u8 ioat_dca_get_tag(struct dca_provider *dca,
 	return tag;
 }
 
-static struct dca_ops ioat_dca_ops = {
+static const struct dca_ops ioat_dca_ops = {
 	.add_requester		= ioat_dca_add_requester,
 	.remove_requester	= ioat_dca_remove_requester,
 	.get_tag		= ioat_dca_get_tag,
diff --git a/drivers/dma/ioat/dma.c b/drivers/dma/ioat/dma.c
index 1d5df2ef148b..21539d5c54c3 100644
--- a/drivers/dma/ioat/dma.c
+++ b/drivers/dma/ioat/dma.c
@@ -861,32 +861,42 @@ void ioat_timer_event(unsigned long data)
 			return;
 	}
 
+	spin_lock_bh(&ioat_chan->cleanup_lock);
+
+	/* handle the no-actives case */
+	if (!ioat_ring_active(ioat_chan)) {
+		spin_lock_bh(&ioat_chan->prep_lock);
+		check_active(ioat_chan);
+		spin_unlock_bh(&ioat_chan->prep_lock);
+		spin_unlock_bh(&ioat_chan->cleanup_lock);
+		return;
+	}
+
 	/* if we haven't made progress and we have already
 	 * acknowledged a pending completion once, then be more
 	 * forceful with a restart
 	 */
-	spin_lock_bh(&ioat_chan->cleanup_lock);
 	if (ioat_cleanup_preamble(ioat_chan, &phys_complete))
 		__cleanup(ioat_chan, phys_complete);
 	else if (test_bit(IOAT_COMPLETION_ACK, &ioat_chan->state)) {
+		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));
+
 		spin_lock_bh(&ioat_chan->prep_lock);
 		ioat_restart_channel(ioat_chan);
 		spin_unlock_bh(&ioat_chan->prep_lock);
 		spin_unlock_bh(&ioat_chan->cleanup_lock);
 		return;
-	} else {
+	} else
 		set_bit(IOAT_COMPLETION_ACK, &ioat_chan->state);
-		mod_timer(&ioat_chan->timer, jiffies + COMPLETION_TIMEOUT);
-	}
-
 
-	if (ioat_ring_active(ioat_chan))
-		mod_timer(&ioat_chan->timer, jiffies + COMPLETION_TIMEOUT);
-	else {
-		spin_lock_bh(&ioat_chan->prep_lock);
-		check_active(ioat_chan);
-		spin_unlock_bh(&ioat_chan->prep_lock);
-	}
+	mod_timer(&ioat_chan->timer, jiffies + COMPLETION_TIMEOUT);
 	spin_unlock_bh(&ioat_chan->cleanup_lock);
 }
 
diff --git a/drivers/dma/ioat/dma.h b/drivers/dma/ioat/dma.h
index 8f4e607d5817..b8f48074789f 100644
--- a/drivers/dma/ioat/dma.h
+++ b/drivers/dma/ioat/dma.h
@@ -235,43 +235,11 @@ ioat_chan_by_index(struct ioatdma_device *ioat_dma, int index)
 	return ioat_dma->idx[index];
 }
 
-static inline u64 ioat_chansts_32(struct ioatdma_chan *ioat_chan)
-{
-	u8 ver = ioat_chan->ioat_dma->version;
-	u64 status;
-	u32 status_lo;
-
-	/* We need to read the low address first as this causes the
-	 * chipset to latch the upper bits for the subsequent read
-	 */
-	status_lo = readl(ioat_chan->reg_base + IOAT_CHANSTS_OFFSET_LOW(ver));
-	status = readl(ioat_chan->reg_base + IOAT_CHANSTS_OFFSET_HIGH(ver));
-	status <<= 32;
-	status |= status_lo;
-
-	return status;
-}
-
-#if BITS_PER_LONG == 64
-
 static inline u64 ioat_chansts(struct ioatdma_chan *ioat_chan)
 {
-	u8 ver = ioat_chan->ioat_dma->version;
-	u64 status;
-
-	 /* With IOAT v3.3 the status register is 64bit.  */
-	if (ver >= IOAT_VER_3_3)
-		status = readq(ioat_chan->reg_base + IOAT_CHANSTS_OFFSET(ver));
-	else
-		status = ioat_chansts_32(ioat_chan);
-
-	return status;
+	return readq(ioat_chan->reg_base + IOAT_CHANSTS_OFFSET);
 }
 
-#else
-#define ioat_chansts ioat_chansts_32
-#endif
-
 static inline u64 ioat_chansts_to_addr(u64 status)
 {
 	return status & IOAT_CHANSTS_COMPLETED_DESCRIPTOR_ADDR;
diff --git a/drivers/dma/ioat/registers.h b/drivers/dma/ioat/registers.h
index 909352f74c89..4994a3623aee 100644
--- a/drivers/dma/ioat/registers.h
+++ b/drivers/dma/ioat/registers.h
@@ -99,19 +99,9 @@
 #define IOAT_DMA_COMP_V1			0x0001	/* Compatibility with DMA version 1 */
 #define IOAT_DMA_COMP_V2			0x0002	/* Compatibility with DMA version 2 */
 
-
-#define IOAT1_CHANSTS_OFFSET		0x04	/* 64-bit Channel Status Register */
-#define IOAT2_CHANSTS_OFFSET		0x08	/* 64-bit Channel Status Register */
-#define IOAT_CHANSTS_OFFSET(ver)		((ver) < IOAT_VER_2_0 \
-						? IOAT1_CHANSTS_OFFSET : IOAT2_CHANSTS_OFFSET)
-#define IOAT1_CHANSTS_OFFSET_LOW	0x04
-#define IOAT2_CHANSTS_OFFSET_LOW	0x08
-#define IOAT_CHANSTS_OFFSET_LOW(ver)		((ver) < IOAT_VER_2_0 \
-						? IOAT1_CHANSTS_OFFSET_LOW : IOAT2_CHANSTS_OFFSET_LOW)
-#define IOAT1_CHANSTS_OFFSET_HIGH	0x08
-#define IOAT2_CHANSTS_OFFSET_HIGH	0x0C
-#define IOAT_CHANSTS_OFFSET_HIGH(ver)		((ver) < IOAT_VER_2_0 \
-						? IOAT1_CHANSTS_OFFSET_HIGH : IOAT2_CHANSTS_OFFSET_HIGH)
+/* IOAT1 define left for i7300_idle driver to not fail compiling */
+#define IOAT1_CHANSTS_OFFSET		0x04
+#define IOAT_CHANSTS_OFFSET		0x08	/* 64-bit Channel Status Register */
 #define IOAT_CHANSTS_COMPLETED_DESCRIPTOR_ADDR	(~0x3fULL)
 #define IOAT_CHANSTS_SOFT_ERR			0x10ULL
 #define IOAT_CHANSTS_UNAFFILIATED_ERR		0x8ULL
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 1c2de9a834a9..14091f878f80 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -139,46 +139,10 @@ static void mv_chan_clear_err_status(struct mv_xor_chan *chan)
 }
 
 static void mv_chan_set_mode(struct mv_xor_chan *chan,
-			     enum dma_transaction_type type)
+			     u32 op_mode)
 {
-	u32 op_mode;
 	u32 config = readl_relaxed(XOR_CONFIG(chan));
 
-	switch (type) {
-	case DMA_XOR:
-		op_mode = XOR_OPERATION_MODE_XOR;
-		break;
-	case DMA_MEMCPY:
-		op_mode = XOR_OPERATION_MODE_MEMCPY;
-		break;
-	default:
-		dev_err(mv_chan_to_devp(chan),
-			"error: unsupported operation %d\n",
-			type);
-		BUG();
-		return;
-	}
-
-	config &= ~0x7;
-	config |= op_mode;
-
-#if defined(__BIG_ENDIAN)
-	config |= XOR_DESCRIPTOR_SWAP;
-#else
-	config &= ~XOR_DESCRIPTOR_SWAP;
-#endif
-
-	writel_relaxed(config, XOR_CONFIG(chan));
-	chan->current_type = type;
-}
-
-static void mv_chan_set_mode_to_desc(struct mv_xor_chan *chan)
-{
-	u32 op_mode;
-	u32 config = readl_relaxed(XOR_CONFIG(chan));
-
-	op_mode = XOR_OPERATION_MODE_IN_DESC;
-
 	config &= ~0x7;
 	config |= op_mode;
 
@@ -1043,9 +1007,9 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 	mv_chan_unmask_interrupts(mv_chan);
 
 	if (mv_chan->op_in_desc == XOR_MODE_IN_DESC)
-		mv_chan_set_mode_to_desc(mv_chan);
+		mv_chan_set_mode(mv_chan, XOR_OPERATION_MODE_IN_DESC);
 	else
-		mv_chan_set_mode(mv_chan, DMA_XOR);
+		mv_chan_set_mode(mv_chan, XOR_OPERATION_MODE_XOR);
 
 	spin_lock_init(&mv_chan->lock);
 	INIT_LIST_HEAD(&mv_chan->chain);
@@ -1121,6 +1085,57 @@ mv_xor_conf_mbus_windows(struct mv_xor_device *xordev,
 	writel(0, base + WINDOW_OVERRIDE_CTRL(1));
 }
 
+/*
+ * Since this XOR driver is basically used only for RAID5, we don't
+ * need to care about synchronizing ->suspend with DMA activity,
+ * because the DMA engine will naturally be quiet due to the block
+ * devices being suspended.
+ */
+static int mv_xor_suspend(struct platform_device *pdev, pm_message_t state)
+{
+	struct mv_xor_device *xordev = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = 0; i < MV_XOR_MAX_CHANNELS; i++) {
+		struct mv_xor_chan *mv_chan = xordev->channels[i];
+
+		if (!mv_chan)
+			continue;
+
+		mv_chan->saved_config_reg =
+			readl_relaxed(XOR_CONFIG(mv_chan));
+		mv_chan->saved_int_mask_reg =
+			readl_relaxed(XOR_INTR_MASK(mv_chan));
+	}
+
+	return 0;
+}
+
+static int mv_xor_resume(struct platform_device *dev)
+{
+	struct mv_xor_device *xordev = platform_get_drvdata(dev);
+	const struct mbus_dram_target_info *dram;
+	int i;
+
+	for (i = 0; i < MV_XOR_MAX_CHANNELS; i++) {
+		struct mv_xor_chan *mv_chan = xordev->channels[i];
+
+		if (!mv_chan)
+			continue;
+
+		writel_relaxed(mv_chan->saved_config_reg,
+			       XOR_CONFIG(mv_chan));
+		writel_relaxed(mv_chan->saved_int_mask_reg,
+			       XOR_INTR_MASK(mv_chan));
+	}
+
+	dram = mv_mbus_dram_info();
+	if (dram)
+		mv_xor_conf_mbus_windows(xordev, dram);
+
+	return 0;
+}
+
 static const struct of_device_id mv_xor_dt_ids[] = {
 	{ .compatible = "marvell,orion-xor", .data = (void *)XOR_MODE_IN_REG },
 	{ .compatible = "marvell,armada-380-xor", .data = (void *)XOR_MODE_IN_DESC },
@@ -1282,6 +1297,8 @@ err_channel_add:
 
 static struct platform_driver mv_xor_driver = {
 	.probe		= mv_xor_probe,
+	.suspend        = mv_xor_suspend,
+	.resume         = mv_xor_resume,
 	.driver		= {
 		.name	        = MV_XOR_NAME,
 		.of_match_table = of_match_ptr(mv_xor_dt_ids),
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index b7455b42137b..c19fe30e5ae9 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -110,7 +110,6 @@ struct mv_xor_chan {
 	void __iomem		*mmr_high_base;
 	unsigned int		idx;
 	int                     irq;
-	enum dma_transaction_type	current_type;
 	struct list_head	chain;
 	struct list_head	free_slots;
 	struct list_head	allocated_slots;
@@ -126,6 +125,7 @@ struct mv_xor_chan {
 	char			dummy_src[MV_XOR_MIN_BYTE_COUNT];
 	char			dummy_dst[MV_XOR_MIN_BYTE_COUNT];
 	dma_addr_t		dummy_src_addr, dummy_dst_addr;
+	u32                     saved_config_reg, saved_int_mask_reg;
 };
 
 /**
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 1dfc71c90123..9794b073d7d7 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -28,8 +28,6 @@
 struct omap_dmadev {
 	struct dma_device ddev;
 	spinlock_t lock;
-	struct tasklet_struct task;
-	struct list_head pending;
 	void __iomem *base;
 	const struct omap_dma_reg *reg_map;
 	struct omap_system_dma_plat_info *plat;
@@ -42,7 +40,6 @@ struct omap_dmadev {
 
 struct omap_chan {
 	struct virt_dma_chan vc;
-	struct list_head node;
 	void __iomem *channel_base;
 	const struct omap_dma_reg *reg_map;
 	uint32_t ccr;
@@ -454,33 +451,6 @@ static void omap_dma_callback(int ch, u16 status, void *data)
 	spin_unlock_irqrestore(&c->vc.lock, flags);
 }
 
-/*
- * This callback schedules all pending channels.  We could be more
- * clever here by postponing allocation of the real DMA channels to
- * this point, and freeing them when our virtual channel becomes idle.
- *
- * We would then need to deal with 'all channels in-use'
- */
-static void omap_dma_sched(unsigned long data)
-{
-	struct omap_dmadev *d = (struct omap_dmadev *)data;
-	LIST_HEAD(head);
-
-	spin_lock_irq(&d->lock);
-	list_splice_tail_init(&d->pending, &head);
-	spin_unlock_irq(&d->lock);
-
-	while (!list_empty(&head)) {
-		struct omap_chan *c = list_first_entry(&head,
-			struct omap_chan, node);
-
-		spin_lock_irq(&c->vc.lock);
-		list_del_init(&c->node);
-		omap_dma_start_desc(c);
-		spin_unlock_irq(&c->vc.lock);
-	}
-}
-
 static irqreturn_t omap_dma_irq(int irq, void *devid)
 {
 	struct omap_dmadev *od = devid;
@@ -703,8 +673,14 @@ static enum dma_status omap_dma_tx_status(struct dma_chan *chan,
 	struct omap_chan *c = to_omap_dma_chan(chan);
 	struct virt_dma_desc *vd;
 	enum dma_status ret;
+	uint32_t ccr;
 	unsigned long flags;
 
+	ccr = omap_dma_chan_read(c, CCR);
+	/* The channel is no longer active, handle the completion right away */
+	if (!(ccr & CCR_ENABLE))
+		omap_dma_callback(c->dma_ch, 0, c);
+
 	ret = dma_cookie_status(chan, cookie, txstate);
 	if (ret == DMA_COMPLETE || !txstate)
 		return ret;
@@ -719,7 +695,7 @@ static enum dma_status omap_dma_tx_status(struct dma_chan *chan,
 
 		if (d->dir == DMA_MEM_TO_DEV)
 			pos = omap_dma_get_src_pos(c);
-		else if (d->dir == DMA_DEV_TO_MEM)
+		else if (d->dir == DMA_DEV_TO_MEM  || d->dir == DMA_MEM_TO_MEM)
 			pos = omap_dma_get_dst_pos(c);
 		else
 			pos = 0;
@@ -739,22 +715,8 @@ static void omap_dma_issue_pending(struct dma_chan *chan)
 	unsigned long flags;
 
 	spin_lock_irqsave(&c->vc.lock, flags);
-	if (vchan_issue_pending(&c->vc) && !c->desc) {
-		/*
-		 * c->cyclic is used only by audio and in this case the DMA need
-		 * to be started without delay.
-		 */
-		if (!c->cyclic) {
-			struct omap_dmadev *d = to_omap_dma_dev(chan->device);
-			spin_lock(&d->lock);
-			if (list_empty(&c->node))
-				list_add_tail(&c->node, &d->pending);
-			spin_unlock(&d->lock);
-			tasklet_schedule(&d->task);
-		} else {
-			omap_dma_start_desc(c);
-		}
-	}
+	if (vchan_issue_pending(&c->vc) && !c->desc)
+		omap_dma_start_desc(c);
 	spin_unlock_irqrestore(&c->vc.lock, flags);
 }
 
@@ -768,7 +730,7 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	struct scatterlist *sgent;
 	struct omap_desc *d;
 	dma_addr_t dev_addr;
-	unsigned i, j = 0, es, en, frame_bytes;
+	unsigned i, es, en, frame_bytes;
 	u32 burst;
 
 	if (dir == DMA_DEV_TO_MEM) {
@@ -845,13 +807,12 @@ static struct dma_async_tx_descriptor *omap_dma_prep_slave_sg(
 	en = burst;
 	frame_bytes = es_bytes[es] * en;
 	for_each_sg(sgl, sgent, sglen, i) {
-		d->sg[j].addr = sg_dma_address(sgent);
-		d->sg[j].en = en;
-		d->sg[j].fn = sg_dma_len(sgent) / frame_bytes;
-		j++;
+		d->sg[i].addr = sg_dma_address(sgent);
+		d->sg[i].en = en;
+		d->sg[i].fn = sg_dma_len(sgent) / frame_bytes;
 	}
 
-	d->sglen = j;
+	d->sglen = sglen;
 
 	return vchan_tx_prep(&c->vc, &d->vd, tx_flags);
 }
@@ -1018,17 +979,11 @@ static int omap_dma_slave_config(struct dma_chan *chan, struct dma_slave_config
 static int omap_dma_terminate_all(struct dma_chan *chan)
 {
 	struct omap_chan *c = to_omap_dma_chan(chan);
-	struct omap_dmadev *d = to_omap_dma_dev(c->vc.chan.device);
 	unsigned long flags;
 	LIST_HEAD(head);
 
 	spin_lock_irqsave(&c->vc.lock, flags);
 
-	/* Prevent this channel being scheduled */
-	spin_lock(&d->lock);
-	list_del_init(&c->node);
-	spin_unlock(&d->lock);
-
 	/*
 	 * Stop DMA activity: we assume the callback will not be called
 	 * after omap_dma_stop() returns (even if it does, it will see
@@ -1102,14 +1057,12 @@ static int omap_dma_chan_init(struct omap_dmadev *od)
 	c->reg_map = od->reg_map;
 	c->vc.desc_free = omap_dma_desc_free;
 	vchan_init(&c->vc, &od->ddev);
-	INIT_LIST_HEAD(&c->node);
 
 	return 0;
 }
 
 static void omap_dma_free(struct omap_dmadev *od)
 {
-	tasklet_kill(&od->task);
 	while (!list_empty(&od->ddev.channels)) {
 		struct omap_chan *c = list_first_entry(&od->ddev.channels,
 			struct omap_chan, vc.chan.device_node);
@@ -1165,12 +1118,9 @@ static int omap_dma_probe(struct platform_device *pdev)
 	od->ddev.residue_granularity = DMA_RESIDUE_GRANULARITY_BURST;
 	od->ddev.dev = &pdev->dev;
 	INIT_LIST_HEAD(&od->ddev.channels);
-	INIT_LIST_HEAD(&od->pending);
 	spin_lock_init(&od->lock);
 	spin_lock_init(&od->irq_lock);
 
-	tasklet_init(&od->task, omap_dma_sched, (unsigned long)od);
-
 	od->dma_requests = OMAP_SDMA_REQUESTS;
 	if (pdev->dev.of_node && of_property_read_u32(pdev->dev.of_node,
 						      "dma-requests",
@@ -1203,6 +1153,10 @@ static int omap_dma_probe(struct platform_device *pdev)
 			return rc;
 	}
 
+	od->ddev.filter.map = od->plat->slave_map;
+	od->ddev.filter.mapcnt = od->plat->slavecnt;
+	od->ddev.filter.fn = omap_dma_filter_fn;
+
 	rc = dma_async_device_register(&od->ddev);
 	if (rc) {
 		pr_warn("OMAP-DMA: failed to register slave DMA engine device: %d\n",
diff --git a/drivers/dma/pxa_dma.c b/drivers/dma/pxa_dma.c
index fc4156afa070..debca824bed6 100644
--- a/drivers/dma/pxa_dma.c
+++ b/drivers/dma/pxa_dma.c
@@ -583,6 +583,8 @@ static void set_updater_desc(struct pxad_desc_sw *sw_desc,
 		(PXA_DCMD_LENGTH & sizeof(u32));
 	if (flags & DMA_PREP_INTERRUPT)
 		updater->dcmd |= PXA_DCMD_ENDIRQEN;
+	if (sw_desc->cyclic)
+		sw_desc->hw_desc[sw_desc->nb_desc - 2]->ddadr = sw_desc->first;
 }
 
 static bool is_desc_completed(struct virt_dma_desc *vd)
@@ -673,6 +675,10 @@ static irqreturn_t pxad_chan_handler(int irq, void *dev_id)
 		dev_dbg(&chan->vc.chan.dev->device,
 			"%s(): checking txd %p[%x]: completed=%d\n",
 			__func__, vd, vd->tx.cookie, is_desc_completed(vd));
+		if (to_pxad_sw_desc(vd)->cyclic) {
+			vchan_cyclic_callback(vd);
+			break;
+		}
 		if (is_desc_completed(vd)) {
 			list_del(&vd->node);
 			vchan_cookie_complete(vd);
@@ -1080,7 +1086,7 @@ pxad_prep_dma_cyclic(struct dma_chan *dchan,
 		return NULL;
 
 	pxad_get_config(chan, dir, &dcmd, &dsadr, &dtadr);
-	dcmd |= PXA_DCMD_ENDIRQEN | (PXA_DCMD_LENGTH | period_len);
+	dcmd |= PXA_DCMD_ENDIRQEN | (PXA_DCMD_LENGTH & period_len);
 	dev_dbg(&chan->vc.chan.dev->device,
 		"%s(): buf_addr=0x%lx len=%zu period=%zu dir=%d flags=%lx\n",
 		__func__, (unsigned long)buf_addr, len, period_len, dir, flags);
@@ -1414,6 +1420,7 @@ static int pxad_probe(struct platform_device *op)
 	pdev->slave.dst_addr_widths = widths;
 	pdev->slave.directions = BIT(DMA_MEM_TO_DEV) | BIT(DMA_DEV_TO_MEM);
 	pdev->slave.residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
+	pdev->slave.descriptor_reuse = true;
 
 	pdev->slave.dev = &op->dev;
 	ret = pxad_init_dmadev(op, pdev, dma_channels);
diff --git a/drivers/dma/sh/Kconfig b/drivers/dma/sh/Kconfig
index 9fda65af841e..f32c430eb16c 100644
--- a/drivers/dma/sh/Kconfig
+++ b/drivers/dma/sh/Kconfig
@@ -47,12 +47,6 @@ config RCAR_DMAC
 	  This driver supports the general purpose DMA controller found in the
 	  Renesas R-Car second generation SoCs.
 
-config RCAR_HPB_DMAE
-	tristate "Renesas R-Car HPB DMAC support"
-	depends on SH_DMAE_BASE
-	help
-	  Enable support for the Renesas R-Car series DMA controllers.
-
 config RENESAS_USB_DMAC
 	tristate "Renesas USB-DMA Controller"
 	depends on ARCH_SHMOBILE || COMPILE_TEST
diff --git a/drivers/dma/sh/Makefile b/drivers/dma/sh/Makefile
index 0133e4658196..f1e2fd64f279 100644
--- a/drivers/dma/sh/Makefile
+++ b/drivers/dma/sh/Makefile
@@ -14,6 +14,5 @@ shdma-objs := $(shdma-y)
 obj-$(CONFIG_SH_DMAE) += shdma.o
 
 obj-$(CONFIG_RCAR_DMAC) += rcar-dmac.o
-obj-$(CONFIG_RCAR_HPB_DMAE) += rcar-hpbdma.o
 obj-$(CONFIG_RENESAS_USB_DMAC) += usb-dmac.o
 obj-$(CONFIG_SUDMAC) += sudmac.o
diff --git a/drivers/dma/sh/rcar-hpbdma.c b/drivers/dma/sh/rcar-hpbdma.c
deleted file mode 100644
index 749f26ecd3b3..000000000000
--- a/drivers/dma/sh/rcar-hpbdma.c
+++ /dev/null
@@ -1,669 +0,0 @@
-/*
- * Copyright (C) 2011-2013 Renesas Electronics Corporation
- * Copyright (C) 2013 Cogent Embedded, Inc.
- *
- * This file is based on the drivers/dma/sh/shdma.c
- *
- * Renesas SuperH DMA Engine support
- *
- * This is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * - DMA of SuperH does not have Hardware DMA chain mode.
- * - max DMA size is 16MB.
- *
- */
-
-#include <linux/dmaengine.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/module.h>
-#include <linux/platform_data/dma-rcar-hpbdma.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/shdma-base.h>
-#include <linux/slab.h>
-
-/* DMA channel registers */
-#define HPB_DMAE_DSAR0	0x00
-#define HPB_DMAE_DDAR0	0x04
-#define HPB_DMAE_DTCR0	0x08
-#define HPB_DMAE_DSAR1	0x0C
-#define HPB_DMAE_DDAR1	0x10
-#define HPB_DMAE_DTCR1	0x14
-#define HPB_DMAE_DSASR	0x18
-#define HPB_DMAE_DDASR	0x1C
-#define HPB_DMAE_DTCSR	0x20
-#define HPB_DMAE_DPTR	0x24
-#define HPB_DMAE_DCR	0x28
-#define HPB_DMAE_DCMDR	0x2C
-#define HPB_DMAE_DSTPR	0x30
-#define HPB_DMAE_DSTSR	0x34
-#define HPB_DMAE_DDBGR	0x38
-#define HPB_DMAE_DDBGR2	0x3C
-#define HPB_DMAE_CHAN(n)	(0x40 * (n))
-
-/* DMA command register (DCMDR) bits */
-#define HPB_DMAE_DCMDR_BDOUT	BIT(7)
-#define HPB_DMAE_DCMDR_DQSPD	BIT(6)
-#define HPB_DMAE_DCMDR_DQSPC	BIT(5)
-#define HPB_DMAE_DCMDR_DMSPD	BIT(4)
-#define HPB_DMAE_DCMDR_DMSPC	BIT(3)
-#define HPB_DMAE_DCMDR_DQEND	BIT(2)
-#define HPB_DMAE_DCMDR_DNXT	BIT(1)
-#define HPB_DMAE_DCMDR_DMEN	BIT(0)
-
-/* DMA forced stop register (DSTPR) bits */
-#define HPB_DMAE_DSTPR_DMSTP	BIT(0)
-
-/* DMA status register (DSTSR) bits */
-#define HPB_DMAE_DSTSR_DQSTS	BIT(2)
-#define HPB_DMAE_DSTSR_DMSTS	BIT(0)
-
-/* DMA common registers */
-#define HPB_DMAE_DTIMR		0x00
-#define HPB_DMAE_DINTSR0		0x0C
-#define HPB_DMAE_DINTSR1		0x10
-#define HPB_DMAE_DINTCR0		0x14
-#define HPB_DMAE_DINTCR1		0x18
-#define HPB_DMAE_DINTMR0		0x1C
-#define HPB_DMAE_DINTMR1		0x20
-#define HPB_DMAE_DACTSR0		0x24
-#define HPB_DMAE_DACTSR1		0x28
-#define HPB_DMAE_HSRSTR(n)	(0x40 + (n) * 4)
-#define HPB_DMAE_HPB_DMASPR(n)	(0x140 + (n) * 4)
-#define HPB_DMAE_HPB_DMLVLR0	0x160
-#define HPB_DMAE_HPB_DMLVLR1	0x164
-#define HPB_DMAE_HPB_DMSHPT0	0x168
-#define HPB_DMAE_HPB_DMSHPT1	0x16C
-
-#define HPB_DMA_SLAVE_NUMBER 256
-#define HPB_DMA_TCR_MAX 0x01000000	/* 16 MiB */
-
-struct hpb_dmae_chan {
-	struct shdma_chan shdma_chan;
-	int xfer_mode;			/* DMA transfer mode */
-#define XFER_SINGLE	1
-#define XFER_DOUBLE	2
-	unsigned plane_idx;		/* current DMA information set */
-	bool first_desc;		/* first/next transfer */
-	int xmit_shift;			/* log_2(bytes_per_xfer) */
-	void __iomem *base;
-	const struct hpb_dmae_slave_config *cfg;
-	char dev_id[16];		/* unique name per DMAC of channel */
-	dma_addr_t slave_addr;
-};
-
-struct hpb_dmae_device {
-	struct shdma_dev shdma_dev;
-	spinlock_t reg_lock;		/* comm_reg operation lock */
-	struct hpb_dmae_pdata *pdata;
-	void __iomem *chan_reg;
-	void __iomem *comm_reg;
-	void __iomem *reset_reg;
-	void __iomem *mode_reg;
-};
-
-struct hpb_dmae_regs {
-	u32 sar; /* SAR / source address */
-	u32 dar; /* DAR / destination address */
-	u32 tcr; /* TCR / transfer count */
-};
-
-struct hpb_desc {
-	struct shdma_desc shdma_desc;
-	struct hpb_dmae_regs hw;
-	unsigned plane_idx;
-};
-
-#define to_chan(schan) container_of(schan, struct hpb_dmae_chan, shdma_chan)
-#define to_desc(sdesc) container_of(sdesc, struct hpb_desc, shdma_desc)
-#define to_dev(sc) container_of(sc->shdma_chan.dma_chan.device, \
-				struct hpb_dmae_device, shdma_dev.dma_dev)
-
-static void ch_reg_write(struct hpb_dmae_chan *hpb_dc, u32 data, u32 reg)
-{
-	iowrite32(data, hpb_dc->base + reg);
-}
-
-static u32 ch_reg_read(struct hpb_dmae_chan *hpb_dc, u32 reg)
-{
-	return ioread32(hpb_dc->base + reg);
-}
-
-static void dcmdr_write(struct hpb_dmae_device *hpbdev, u32 data)
-{
-	iowrite32(data, hpbdev->chan_reg + HPB_DMAE_DCMDR);
-}
-
-static void hsrstr_write(struct hpb_dmae_device *hpbdev, u32 ch)
-{
-	iowrite32(0x1, hpbdev->comm_reg + HPB_DMAE_HSRSTR(ch));
-}
-
-static u32 dintsr_read(struct hpb_dmae_device *hpbdev, u32 ch)
-{
-	u32 v;
-
-	if (ch < 32)
-		v = ioread32(hpbdev->comm_reg + HPB_DMAE_DINTSR0) >> ch;
-	else
-		v = ioread32(hpbdev->comm_reg + HPB_DMAE_DINTSR1) >> (ch - 32);
-	return v & 0x1;
-}
-
-static void dintcr_write(struct hpb_dmae_device *hpbdev, u32 ch)
-{
-	if (ch < 32)
-		iowrite32((0x1 << ch), hpbdev->comm_reg + HPB_DMAE_DINTCR0);
-	else
-		iowrite32((0x1 << (ch - 32)),
-			  hpbdev->comm_reg + HPB_DMAE_DINTCR1);
-}
-
-static void asyncmdr_write(struct hpb_dmae_device *hpbdev, u32 data)
-{
-	iowrite32(data, hpbdev->mode_reg);
-}
-
-static u32 asyncmdr_read(struct hpb_dmae_device *hpbdev)
-{
-	return ioread32(hpbdev->mode_reg);
-}
-
-static void hpb_dmae_enable_int(struct hpb_dmae_device *hpbdev, u32 ch)
-{
-	u32 intreg;
-
-	spin_lock_irq(&hpbdev->reg_lock);
-	if (ch < 32) {
-		intreg = ioread32(hpbdev->comm_reg + HPB_DMAE_DINTMR0);
-		iowrite32(BIT(ch) | intreg,
-			  hpbdev->comm_reg + HPB_DMAE_DINTMR0);
-	} else {
-		intreg = ioread32(hpbdev->comm_reg + HPB_DMAE_DINTMR1);
-		iowrite32(BIT(ch - 32) | intreg,
-			  hpbdev->comm_reg + HPB_DMAE_DINTMR1);
-	}
-	spin_unlock_irq(&hpbdev->reg_lock);
-}
-
-static void hpb_dmae_async_reset(struct hpb_dmae_device *hpbdev, u32 data)
-{
-	u32 rstr;
-	int timeout = 10000;	/* 100 ms */
-
-	spin_lock(&hpbdev->reg_lock);
-	rstr = ioread32(hpbdev->reset_reg);
-	rstr |= data;
-	iowrite32(rstr, hpbdev->reset_reg);
-	do {
-		rstr = ioread32(hpbdev->reset_reg);
-		if ((rstr & data) == data)
-			break;
-		udelay(10);
-	} while (timeout--);
-
-	if (timeout < 0)
-		dev_err(hpbdev->shdma_dev.dma_dev.dev,
-			"%s timeout\n", __func__);
-
-	rstr &= ~data;
-	iowrite32(rstr, hpbdev->reset_reg);
-	spin_unlock(&hpbdev->reg_lock);
-}
-
-static void hpb_dmae_set_async_mode(struct hpb_dmae_device *hpbdev,
-				    u32 mask, u32 data)
-{
-	u32 mode;
-
-	spin_lock_irq(&hpbdev->reg_lock);
-	mode = asyncmdr_read(hpbdev);
-	mode &= ~mask;
-	mode |= data;
-	asyncmdr_write(hpbdev, mode);
-	spin_unlock_irq(&hpbdev->reg_lock);
-}
-
-static void hpb_dmae_ctl_stop(struct hpb_dmae_device *hpbdev)
-{
-	dcmdr_write(hpbdev, HPB_DMAE_DCMDR_DQSPD);
-}
-
-static void hpb_dmae_reset(struct hpb_dmae_device *hpbdev)
-{
-	u32 ch;
-
-	for (ch = 0; ch < hpbdev->pdata->num_hw_channels; ch++)
-		hsrstr_write(hpbdev, ch);
-}
-
-static unsigned int calc_xmit_shift(struct hpb_dmae_chan *hpb_chan)
-{
-	struct hpb_dmae_device *hpbdev = to_dev(hpb_chan);
-	struct hpb_dmae_pdata *pdata = hpbdev->pdata;
-	int width = ch_reg_read(hpb_chan, HPB_DMAE_DCR);
-	int i;
-
-	switch (width & (HPB_DMAE_DCR_SPDS_MASK | HPB_DMAE_DCR_DPDS_MASK)) {
-	case HPB_DMAE_DCR_SPDS_8BIT | HPB_DMAE_DCR_DPDS_8BIT:
-	default:
-		i = XMIT_SZ_8BIT;
-		break;
-	case HPB_DMAE_DCR_SPDS_16BIT | HPB_DMAE_DCR_DPDS_16BIT:
-		i = XMIT_SZ_16BIT;
-		break;
-	case HPB_DMAE_DCR_SPDS_32BIT | HPB_DMAE_DCR_DPDS_32BIT:
-		i = XMIT_SZ_32BIT;
-		break;
-	}
-	return pdata->ts_shift[i];
-}
-
-static void hpb_dmae_set_reg(struct hpb_dmae_chan *hpb_chan,
-			     struct hpb_dmae_regs *hw, unsigned plane)
-{
-	ch_reg_write(hpb_chan, hw->sar,
-		     plane ? HPB_DMAE_DSAR1 : HPB_DMAE_DSAR0);
-	ch_reg_write(hpb_chan, hw->dar,
-		     plane ? HPB_DMAE_DDAR1 : HPB_DMAE_DDAR0);
-	ch_reg_write(hpb_chan, hw->tcr >> hpb_chan->xmit_shift,
-		     plane ? HPB_DMAE_DTCR1 : HPB_DMAE_DTCR0);
-}
-
-static void hpb_dmae_start(struct hpb_dmae_chan *hpb_chan, bool next)
-{
-	ch_reg_write(hpb_chan, (next ? HPB_DMAE_DCMDR_DNXT : 0) |
-		     HPB_DMAE_DCMDR_DMEN, HPB_DMAE_DCMDR);
-}
-
-static void hpb_dmae_halt(struct shdma_chan *schan)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-
-	ch_reg_write(chan, HPB_DMAE_DCMDR_DQEND, HPB_DMAE_DCMDR);
-	ch_reg_write(chan, HPB_DMAE_DSTPR_DMSTP, HPB_DMAE_DSTPR);
-
-	chan->plane_idx = 0;
-	chan->first_desc = true;
-}
-
-static const struct hpb_dmae_slave_config *
-hpb_dmae_find_slave(struct hpb_dmae_chan *hpb_chan, int slave_id)
-{
-	struct hpb_dmae_device *hpbdev = to_dev(hpb_chan);
-	struct hpb_dmae_pdata *pdata = hpbdev->pdata;
-	int i;
-
-	if (slave_id >= HPB_DMA_SLAVE_NUMBER)
-		return NULL;
-
-	for (i = 0; i < pdata->num_slaves; i++)
-		if (pdata->slaves[i].id == slave_id)
-			return pdata->slaves + i;
-
-	return NULL;
-}
-
-static void hpb_dmae_start_xfer(struct shdma_chan *schan,
-				struct shdma_desc *sdesc)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-	struct hpb_dmae_device *hpbdev = to_dev(chan);
-	struct hpb_desc *desc = to_desc(sdesc);
-
-	if (chan->cfg->flags & HPB_DMAE_SET_ASYNC_RESET)
-		hpb_dmae_async_reset(hpbdev, chan->cfg->rstr);
-
-	desc->plane_idx = chan->plane_idx;
-	hpb_dmae_set_reg(chan, &desc->hw, chan->plane_idx);
-	hpb_dmae_start(chan, !chan->first_desc);
-
-	if (chan->xfer_mode == XFER_DOUBLE) {
-		chan->plane_idx ^= 1;
-		chan->first_desc = false;
-	}
-}
-
-static bool hpb_dmae_desc_completed(struct shdma_chan *schan,
-				    struct shdma_desc *sdesc)
-{
-	/*
-	 * This is correct since we always have at most single
-	 * outstanding DMA transfer per channel, and by the time
-	 * we get completion interrupt the transfer is completed.
-	 * This will change if we ever use alternating DMA
-	 * information sets and submit two descriptors at once.
-	 */
-	return true;
-}
-
-static bool hpb_dmae_chan_irq(struct shdma_chan *schan, int irq)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-	struct hpb_dmae_device *hpbdev = to_dev(chan);
-	int ch = chan->cfg->dma_ch;
-
-	/* Check Complete DMA Transfer */
-	if (dintsr_read(hpbdev, ch)) {
-		/* Clear Interrupt status */
-		dintcr_write(hpbdev, ch);
-		return true;
-	}
-	return false;
-}
-
-static int hpb_dmae_desc_setup(struct shdma_chan *schan,
-			       struct shdma_desc *sdesc,
-			       dma_addr_t src, dma_addr_t dst, size_t *len)
-{
-	struct hpb_desc *desc = to_desc(sdesc);
-
-	if (*len > (size_t)HPB_DMA_TCR_MAX)
-		*len = (size_t)HPB_DMA_TCR_MAX;
-
-	desc->hw.sar = src;
-	desc->hw.dar = dst;
-	desc->hw.tcr = *len;
-
-	return 0;
-}
-
-static size_t hpb_dmae_get_partial(struct shdma_chan *schan,
-				   struct shdma_desc *sdesc)
-{
-	struct hpb_desc *desc = to_desc(sdesc);
-	struct hpb_dmae_chan *chan = to_chan(schan);
-	u32 tcr = ch_reg_read(chan, desc->plane_idx ?
-			      HPB_DMAE_DTCR1 : HPB_DMAE_DTCR0);
-
-	return (desc->hw.tcr - tcr) << chan->xmit_shift;
-}
-
-static bool hpb_dmae_channel_busy(struct shdma_chan *schan)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-	u32 dstsr = ch_reg_read(chan, HPB_DMAE_DSTSR);
-
-	if (chan->xfer_mode == XFER_DOUBLE)
-		return dstsr & HPB_DMAE_DSTSR_DQSTS;
-	else
-		return dstsr & HPB_DMAE_DSTSR_DMSTS;
-}
-
-static int
-hpb_dmae_alloc_chan_resources(struct hpb_dmae_chan *hpb_chan,
-			      const struct hpb_dmae_slave_config *cfg)
-{
-	struct hpb_dmae_device *hpbdev = to_dev(hpb_chan);
-	struct hpb_dmae_pdata *pdata = hpbdev->pdata;
-	const struct hpb_dmae_channel *channel = pdata->channels;
-	int slave_id = cfg->id;
-	int i, err;
-
-	for (i = 0; i < pdata->num_channels; i++, channel++) {
-		if (channel->s_id == slave_id) {
-			struct device *dev = hpb_chan->shdma_chan.dev;
-
-			hpb_chan->base = hpbdev->chan_reg +
-				HPB_DMAE_CHAN(cfg->dma_ch);
-
-			dev_dbg(dev, "Detected Slave device\n");
-			dev_dbg(dev, " -- slave_id       : 0x%x\n", slave_id);
-			dev_dbg(dev, " -- cfg->dma_ch    : %d\n", cfg->dma_ch);
-			dev_dbg(dev, " -- channel->ch_irq: %d\n",
-				channel->ch_irq);
-			break;
-		}
-	}
-
-	err = shdma_request_irq(&hpb_chan->shdma_chan, channel->ch_irq,
-				IRQF_SHARED, hpb_chan->dev_id);
-	if (err) {
-		dev_err(hpb_chan->shdma_chan.dev,
-			"DMA channel request_irq %d failed with error %d\n",
-			channel->ch_irq, err);
-		return err;
-	}
-
-	hpb_chan->plane_idx = 0;
-	hpb_chan->first_desc = true;
-
-	if ((cfg->dcr & (HPB_DMAE_DCR_CT | HPB_DMAE_DCR_DIP)) == 0) {
-		hpb_chan->xfer_mode = XFER_SINGLE;
-	} else if ((cfg->dcr & (HPB_DMAE_DCR_CT | HPB_DMAE_DCR_DIP)) ==
-		   (HPB_DMAE_DCR_CT | HPB_DMAE_DCR_DIP)) {
-		hpb_chan->xfer_mode = XFER_DOUBLE;
-	} else {
-		dev_err(hpb_chan->shdma_chan.dev, "DCR setting error");
-		return -EINVAL;
-	}
-
-	if (cfg->flags & HPB_DMAE_SET_ASYNC_MODE)
-		hpb_dmae_set_async_mode(hpbdev, cfg->mdm, cfg->mdr);
-	ch_reg_write(hpb_chan, cfg->dcr, HPB_DMAE_DCR);
-	ch_reg_write(hpb_chan, cfg->port, HPB_DMAE_DPTR);
-	hpb_chan->xmit_shift = calc_xmit_shift(hpb_chan);
-	hpb_dmae_enable_int(hpbdev, cfg->dma_ch);
-
-	return 0;
-}
-
-static int hpb_dmae_set_slave(struct shdma_chan *schan, int slave_id,
-			      dma_addr_t slave_addr, bool try)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-	const struct hpb_dmae_slave_config *sc =
-		hpb_dmae_find_slave(chan, slave_id);
-
-	if (!sc)
-		return -ENODEV;
-	if (try)
-		return 0;
-	chan->cfg = sc;
-	chan->slave_addr = slave_addr ? : sc->addr;
-	return hpb_dmae_alloc_chan_resources(chan, sc);
-}
-
-static void hpb_dmae_setup_xfer(struct shdma_chan *schan, int slave_id)
-{
-}
-
-static dma_addr_t hpb_dmae_slave_addr(struct shdma_chan *schan)
-{
-	struct hpb_dmae_chan *chan = to_chan(schan);
-
-	return chan->slave_addr;
-}
-
-static struct shdma_desc *hpb_dmae_embedded_desc(void *buf, int i)
-{
-	return &((struct hpb_desc *)buf)[i].shdma_desc;
-}
-
-static const struct shdma_ops hpb_dmae_ops = {
-	.desc_completed = hpb_dmae_desc_completed,
-	.halt_channel = hpb_dmae_halt,
-	.channel_busy = hpb_dmae_channel_busy,
-	.slave_addr = hpb_dmae_slave_addr,
-	.desc_setup = hpb_dmae_desc_setup,
-	.set_slave = hpb_dmae_set_slave,
-	.setup_xfer = hpb_dmae_setup_xfer,
-	.start_xfer = hpb_dmae_start_xfer,
-	.embedded_desc = hpb_dmae_embedded_desc,
-	.chan_irq = hpb_dmae_chan_irq,
-	.get_partial = hpb_dmae_get_partial,
-};
-
-static int hpb_dmae_chan_probe(struct hpb_dmae_device *hpbdev, int id)
-{
-	struct shdma_dev *sdev = &hpbdev->shdma_dev;
-	struct platform_device *pdev =
-		to_platform_device(hpbdev->shdma_dev.dma_dev.dev);
-	struct hpb_dmae_chan *new_hpb_chan;
-	struct shdma_chan *schan;
-
-	/* Alloc channel */
-	new_hpb_chan = devm_kzalloc(&pdev->dev,
-				    sizeof(struct hpb_dmae_chan), GFP_KERNEL);
-	if (!new_hpb_chan) {
-		dev_err(hpbdev->shdma_dev.dma_dev.dev,
-			"No free memory for allocating DMA channels!\n");
-		return -ENOMEM;
-	}
-
-	schan = &new_hpb_chan->shdma_chan;
-	schan->max_xfer_len = HPB_DMA_TCR_MAX;
-
-	shdma_chan_probe(sdev, schan, id);
-
-	if (pdev->id >= 0)
-		snprintf(new_hpb_chan->dev_id, sizeof(new_hpb_chan->dev_id),
-			 "hpb-dmae%d.%d", pdev->id, id);
-	else
-		snprintf(new_hpb_chan->dev_id, sizeof(new_hpb_chan->dev_id),
-			 "hpb-dma.%d", id);
-
-	return 0;
-}
-
-static int hpb_dmae_probe(struct platform_device *pdev)
-{
-	const enum dma_slave_buswidth widths = DMA_SLAVE_BUSWIDTH_1_BYTE |
-		DMA_SLAVE_BUSWIDTH_2_BYTES | DMA_SLAVE_BUSWIDTH_4_BYTES;
-	struct hpb_dmae_pdata *pdata = pdev->dev.platform_data;
-	struct hpb_dmae_device *hpbdev;
-	struct dma_device *dma_dev;
-	struct resource *chan, *comm, *rest, *mode, *irq_res;
-	int err, i;
-
-	/* Get platform data */
-	if (!pdata || !pdata->num_channels)
-		return -ENODEV;
-
-	chan = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	comm = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	rest = platform_get_resource(pdev, IORESOURCE_MEM, 2);
-	mode = platform_get_resource(pdev, IORESOURCE_MEM, 3);
-
-	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!irq_res)
-		return -ENODEV;
-
-	hpbdev = devm_kzalloc(&pdev->dev, sizeof(struct hpb_dmae_device),
-			      GFP_KERNEL);
-	if (!hpbdev) {
-		dev_err(&pdev->dev, "Not enough memory\n");
-		return -ENOMEM;
-	}
-
-	hpbdev->chan_reg = devm_ioremap_resource(&pdev->dev, chan);
-	if (IS_ERR(hpbdev->chan_reg))
-		return PTR_ERR(hpbdev->chan_reg);
-
-	hpbdev->comm_reg = devm_ioremap_resource(&pdev->dev, comm);
-	if (IS_ERR(hpbdev->comm_reg))
-		return PTR_ERR(hpbdev->comm_reg);
-
-	hpbdev->reset_reg = devm_ioremap_resource(&pdev->dev, rest);
-	if (IS_ERR(hpbdev->reset_reg))
-		return PTR_ERR(hpbdev->reset_reg);
-
-	hpbdev->mode_reg = devm_ioremap_resource(&pdev->dev, mode);
-	if (IS_ERR(hpbdev->mode_reg))
-		return PTR_ERR(hpbdev->mode_reg);
-
-	dma_dev = &hpbdev->shdma_dev.dma_dev;
-
-	spin_lock_init(&hpbdev->reg_lock);
-
-	/* Platform data */
-	hpbdev->pdata = pdata;
-
-	pm_runtime_enable(&pdev->dev);
-	err = pm_runtime_get_sync(&pdev->dev);
-	if (err < 0)
-		dev_err(&pdev->dev, "%s(): GET = %d\n", __func__, err);
-
-	/* Reset DMA controller */
-	hpb_dmae_reset(hpbdev);
-
-	pm_runtime_put(&pdev->dev);
-
-	dma_cap_set(DMA_MEMCPY, dma_dev->cap_mask);
-	dma_cap_set(DMA_SLAVE, dma_dev->cap_mask);
-	dma_dev->src_addr_widths = widths;
-	dma_dev->dst_addr_widths = widths;
-	dma_dev->directions = BIT(DMA_MEM_TO_DEV) | BIT(DMA_DEV_TO_MEM);
-	dma_dev->residue_granularity = DMA_RESIDUE_GRANULARITY_DESCRIPTOR;
-
-	hpbdev->shdma_dev.ops = &hpb_dmae_ops;
-	hpbdev->shdma_dev.desc_size = sizeof(struct hpb_desc);
-	err = shdma_init(&pdev->dev, &hpbdev->shdma_dev, pdata->num_channels);
-	if (err < 0)
-		goto error;
-
-	/* Create DMA channels */
-	for (i = 0; i < pdata->num_channels; i++)
-		hpb_dmae_chan_probe(hpbdev, i);
-
-	platform_set_drvdata(pdev, hpbdev);
-	err = dma_async_device_register(dma_dev);
-	if (!err)
-		return 0;
-
-	shdma_cleanup(&hpbdev->shdma_dev);
-error:
-	pm_runtime_disable(&pdev->dev);
-	return err;
-}
-
-static void hpb_dmae_chan_remove(struct hpb_dmae_device *hpbdev)
-{
-	struct shdma_chan *schan;
-	int i;
-
-	shdma_for_each_chan(schan, &hpbdev->shdma_dev, i) {
-		BUG_ON(!schan);
-
-		shdma_chan_remove(schan);
-	}
-}
-
-static int hpb_dmae_remove(struct platform_device *pdev)
-{
-	struct hpb_dmae_device *hpbdev = platform_get_drvdata(pdev);
-
-	dma_async_device_unregister(&hpbdev->shdma_dev.dma_dev);
-
-	pm_runtime_disable(&pdev->dev);
-
-	hpb_dmae_chan_remove(hpbdev);
-
-	return 0;
-}
-
-static void hpb_dmae_shutdown(struct platform_device *pdev)
-{
-	struct hpb_dmae_device *hpbdev = platform_get_drvdata(pdev);
-	hpb_dmae_ctl_stop(hpbdev);
-}
-
-static struct platform_driver hpb_dmae_driver = {
-	.probe		= hpb_dmae_probe,
-	.remove		= hpb_dmae_remove,
-	.shutdown	= hpb_dmae_shutdown,
-	.driver = {
-		.name	= "hpb-dma-engine",
-	},
-};
-module_platform_driver(hpb_dmae_driver);
-
-MODULE_AUTHOR("Max Filippov <max.filippov@cogentembedded.com>");
-MODULE_DESCRIPTION("Renesas HPB DMA Engine driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/dma/sh/usb-dmac.c b/drivers/dma/sh/usb-dmac.c
index f1bcc2a163b3..749f1bd5d65d 100644
--- a/drivers/dma/sh/usb-dmac.c
+++ b/drivers/dma/sh/usb-dmac.c
@@ -448,7 +448,7 @@ usb_dmac_prep_slave_sg(struct dma_chan *chan, struct scatterlist *sgl,
 static int usb_dmac_chan_terminate_all(struct dma_chan *chan)
 {
 	struct usb_dmac_chan *uchan = to_usb_dmac_chan(chan);
-	struct usb_dmac_desc *desc;
+	struct usb_dmac_desc *desc, *_desc;
 	unsigned long flags;
 	LIST_HEAD(head);
 	LIST_HEAD(list);
@@ -459,7 +459,7 @@ static int usb_dmac_chan_terminate_all(struct dma_chan *chan)
 	if (uchan->desc)
 		uchan->desc = NULL;
 	list_splice_init(&uchan->desc_got, &list);
-	list_for_each_entry(desc, &list, node)
+	list_for_each_entry_safe(desc, _desc, &list, node)
 		list_move_tail(&desc->node, &uchan->desc_freed);
 	spin_unlock_irqrestore(&uchan->vc.lock, flags);
 	vchan_dma_desc_free_list(&uchan->vc, &head);
diff --git a/drivers/dma/ste_dma40.c b/drivers/dma/ste_dma40.c
index dd3e7ba273ad..6fb8307468ab 100644
--- a/drivers/dma/ste_dma40.c
+++ b/drivers/dma/ste_dma40.c
@@ -3543,8 +3543,8 @@ static int __init d40_probe(struct platform_device *pdev)
 	struct stedma40_platform_data *plat_data = dev_get_platdata(&pdev->dev);
 	struct device_node *np = pdev->dev.of_node;
 	int ret = -ENOENT;
-	struct d40_base *base = NULL;
-	struct resource *res = NULL;
+	struct d40_base *base;
+	struct resource *res;
 	int num_reserved_chans;
 	u32 val;
 
@@ -3552,17 +3552,17 @@ static int __init d40_probe(struct platform_device *pdev)
 		if (np) {
 			if (d40_of_probe(pdev, np)) {
 				ret = -ENOMEM;
-				goto failure;
+				goto report_failure;
 			}
 		} else {
 			d40_err(&pdev->dev, "No pdata or Device Tree provided\n");
-			goto failure;
+			goto report_failure;
 		}
 	}
 
 	base = d40_hw_detect_init(pdev);
 	if (!base)
-		goto failure;
+		goto report_failure;
 
 	num_reserved_chans = d40_phy_res_init(base);
 
@@ -3693,51 +3693,48 @@ static int __init d40_probe(struct platform_device *pdev)
 	return 0;
 
 failure:
-	if (base) {
-		if (base->desc_slab)
-			kmem_cache_destroy(base->desc_slab);
-		if (base->virtbase)
-			iounmap(base->virtbase);
-
-		if (base->lcla_pool.base && base->plat_data->use_esram_lcla) {
-			iounmap(base->lcla_pool.base);
-			base->lcla_pool.base = NULL;
-		}
+	kmem_cache_destroy(base->desc_slab);
+	if (base->virtbase)
+		iounmap(base->virtbase);
 
-		if (base->lcla_pool.dma_addr)
-			dma_unmap_single(base->dev, base->lcla_pool.dma_addr,
-					 SZ_1K * base->num_phy_chans,
-					 DMA_TO_DEVICE);
-
-		if (!base->lcla_pool.base_unaligned && base->lcla_pool.base)
-			free_pages((unsigned long)base->lcla_pool.base,
-				   base->lcla_pool.pages);
-
-		kfree(base->lcla_pool.base_unaligned);
-
-		if (base->phy_lcpa)
-			release_mem_region(base->phy_lcpa,
-					   base->lcpa_size);
-		if (base->phy_start)
-			release_mem_region(base->phy_start,
-					   base->phy_size);
-		if (base->clk) {
-			clk_disable_unprepare(base->clk);
-			clk_put(base->clk);
-		}
+	if (base->lcla_pool.base && base->plat_data->use_esram_lcla) {
+		iounmap(base->lcla_pool.base);
+		base->lcla_pool.base = NULL;
+	}
 
-		if (base->lcpa_regulator) {
-			regulator_disable(base->lcpa_regulator);
-			regulator_put(base->lcpa_regulator);
-		}
+	if (base->lcla_pool.dma_addr)
+		dma_unmap_single(base->dev, base->lcla_pool.dma_addr,
+				 SZ_1K * base->num_phy_chans,
+				 DMA_TO_DEVICE);
 
-		kfree(base->lcla_pool.alloc_map);
-		kfree(base->lookup_log_chans);
-		kfree(base->lookup_phy_chans);
-		kfree(base->phy_res);
-		kfree(base);
+	if (!base->lcla_pool.base_unaligned && base->lcla_pool.base)
+		free_pages((unsigned long)base->lcla_pool.base,
+			   base->lcla_pool.pages);
+
+	kfree(base->lcla_pool.base_unaligned);
+
+	if (base->phy_lcpa)
+		release_mem_region(base->phy_lcpa,
+				   base->lcpa_size);
+	if (base->phy_start)
+		release_mem_region(base->phy_start,
+				   base->phy_size);
+	if (base->clk) {
+		clk_disable_unprepare(base->clk);
+		clk_put(base->clk);
+	}
+
+	if (base->lcpa_regulator) {
+		regulator_disable(base->lcpa_regulator);
+		regulator_put(base->lcpa_regulator);
 	}
 
+	kfree(base->lcla_pool.alloc_map);
+	kfree(base->lookup_log_chans);
+	kfree(base->lookup_phy_chans);
+	kfree(base->phy_res);
+	kfree(base);
+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
new file mode 100644
index 000000000000..047476a1383d
--- /dev/null
+++ b/drivers/dma/stm32-dma.c
@@ -0,0 +1,1141 @@
+/*
+ * Driver for STM32 DMA controller
+ *
+ * Inspired by dma-jz4740.c and tegra20-apb-dma.c
+ *
+ * Copyright (C) M'boumba Cedric Madianga 2015
+ * Author: M'boumba Cedric Madianga <cedric.madianga@gmail.com>
+ *
+ * License terms:  GNU General Public License (GPL), version 2
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_dma.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include "virt-dma.h"
+
+#define STM32_DMA_LISR			0x0000 /* DMA Low Int Status Reg */
+#define STM32_DMA_HISR			0x0004 /* DMA High Int Status Reg */
+#define STM32_DMA_LIFCR			0x0008 /* DMA Low Int Flag Clear Reg */
+#define STM32_DMA_HIFCR			0x000c /* DMA High Int Flag Clear Reg */
+#define STM32_DMA_TCI			BIT(5) /* Transfer Complete Interrupt */
+#define STM32_DMA_TEI			BIT(3) /* Transfer Error Interrupt */
+#define STM32_DMA_DMEI			BIT(2) /* Direct Mode Error Interrupt */
+#define STM32_DMA_FEI			BIT(0) /* FIFO Error Interrupt */
+
+/* DMA Stream x Configuration Register */
+#define STM32_DMA_SCR(x)		(0x0010 + 0x18 * (x)) /* x = 0..7 */
+#define STM32_DMA_SCR_REQ(n)		((n & 0x7) << 25)
+#define STM32_DMA_SCR_MBURST_MASK	GENMASK(24, 23)
+#define STM32_DMA_SCR_MBURST(n)	        ((n & 0x3) << 23)
+#define STM32_DMA_SCR_PBURST_MASK	GENMASK(22, 21)
+#define STM32_DMA_SCR_PBURST(n)	        ((n & 0x3) << 21)
+#define STM32_DMA_SCR_PL_MASK		GENMASK(17, 16)
+#define STM32_DMA_SCR_PL(n)		((n & 0x3) << 16)
+#define STM32_DMA_SCR_MSIZE_MASK	GENMASK(14, 13)
+#define STM32_DMA_SCR_MSIZE(n)		((n & 0x3) << 13)
+#define STM32_DMA_SCR_PSIZE_MASK	GENMASK(12, 11)
+#define STM32_DMA_SCR_PSIZE(n)		((n & 0x3) << 11)
+#define STM32_DMA_SCR_PSIZE_GET(n)	((n & STM32_DMA_SCR_PSIZE_MASK) >> 11)
+#define STM32_DMA_SCR_DIR_MASK		GENMASK(7, 6)
+#define STM32_DMA_SCR_DIR(n)		((n & 0x3) << 6)
+#define STM32_DMA_SCR_CT		BIT(19) /* Target in double buffer */
+#define STM32_DMA_SCR_DBM		BIT(18) /* Double Buffer Mode */
+#define STM32_DMA_SCR_PINCOS		BIT(15) /* Peripheral inc offset size */
+#define STM32_DMA_SCR_MINC		BIT(10) /* Memory increment mode */
+#define STM32_DMA_SCR_PINC		BIT(9) /* Peripheral increment mode */
+#define STM32_DMA_SCR_CIRC		BIT(8) /* Circular mode */
+#define STM32_DMA_SCR_PFCTRL		BIT(5) /* Peripheral Flow Controller */
+#define STM32_DMA_SCR_TCIE		BIT(4) /* Transfer Cplete Int Enable*/
+#define STM32_DMA_SCR_TEIE		BIT(2) /* Transfer Error Int Enable */
+#define STM32_DMA_SCR_DMEIE		BIT(1) /* Direct Mode Err Int Enable */
+#define STM32_DMA_SCR_EN		BIT(0) /* Stream Enable */
+#define STM32_DMA_SCR_CFG_MASK		(STM32_DMA_SCR_PINC \
+					| STM32_DMA_SCR_MINC \
+					| STM32_DMA_SCR_PINCOS \
+					| STM32_DMA_SCR_PL_MASK)
+#define STM32_DMA_SCR_IRQ_MASK		(STM32_DMA_SCR_TCIE \
+					| STM32_DMA_SCR_TEIE \
+					| STM32_DMA_SCR_DMEIE)
+
+/* DMA Stream x number of data register */
+#define STM32_DMA_SNDTR(x)		(0x0014 + 0x18 * (x))
+
+/* DMA stream peripheral address register */
+#define STM32_DMA_SPAR(x)		(0x0018 + 0x18 * (x))
+
+/* DMA stream x memory 0 address register */
+#define STM32_DMA_SM0AR(x)		(0x001c + 0x18 * (x))
+
+/* DMA stream x memory 1 address register */
+#define STM32_DMA_SM1AR(x)		(0x0020 + 0x18 * (x))
+
+/* DMA stream x FIFO control register */
+#define STM32_DMA_SFCR(x)		(0x0024 + 0x18 * (x))
+#define STM32_DMA_SFCR_FTH_MASK		GENMASK(1, 0)
+#define STM32_DMA_SFCR_FTH(n)		(n & STM32_DMA_SFCR_FTH_MASK)
+#define STM32_DMA_SFCR_FEIE		BIT(7) /* FIFO error interrupt enable */
+#define STM32_DMA_SFCR_DMDIS		BIT(2) /* Direct mode disable */
+#define STM32_DMA_SFCR_MASK		(STM32_DMA_SFCR_FEIE \
+					| STM32_DMA_SFCR_DMDIS)
+
+/* DMA direction */
+#define STM32_DMA_DEV_TO_MEM		0x00
+#define	STM32_DMA_MEM_TO_DEV		0x01
+#define	STM32_DMA_MEM_TO_MEM		0x02
+
+/* DMA priority level */
+#define STM32_DMA_PRIORITY_LOW		0x00
+#define STM32_DMA_PRIORITY_MEDIUM	0x01
+#define STM32_DMA_PRIORITY_HIGH		0x02
+#define STM32_DMA_PRIORITY_VERY_HIGH	0x03
+
+/* DMA FIFO threshold selection */
+#define STM32_DMA_FIFO_THRESHOLD_1QUARTERFULL		0x00
+#define STM32_DMA_FIFO_THRESHOLD_HALFFULL		0x01
+#define STM32_DMA_FIFO_THRESHOLD_3QUARTERSFULL		0x02
+#define STM32_DMA_FIFO_THRESHOLD_FULL			0x03
+
+#define STM32_DMA_MAX_DATA_ITEMS	0xffff
+#define STM32_DMA_MAX_CHANNELS		0x08
+#define STM32_DMA_MAX_REQUEST_ID	0x08
+#define STM32_DMA_MAX_DATA_PARAM	0x03
+
+enum stm32_dma_width {
+	STM32_DMA_BYTE,
+	STM32_DMA_HALF_WORD,
+	STM32_DMA_WORD,
+};
+
+enum stm32_dma_burst_size {
+	STM32_DMA_BURST_SINGLE,
+	STM32_DMA_BURST_INCR4,
+	STM32_DMA_BURST_INCR8,
+	STM32_DMA_BURST_INCR16,
+};
+
+struct stm32_dma_cfg {
+	u32 channel_id;
+	u32 request_line;
+	u32 stream_config;
+	u32 threshold;
+};
+
+struct stm32_dma_chan_reg {
+	u32 dma_lisr;
+	u32 dma_hisr;
+	u32 dma_lifcr;
+	u32 dma_hifcr;
+	u32 dma_scr;
+	u32 dma_sndtr;
+	u32 dma_spar;
+	u32 dma_sm0ar;
+	u32 dma_sm1ar;
+	u32 dma_sfcr;
+};
+
+struct stm32_dma_sg_req {
+	u32 len;
+	struct stm32_dma_chan_reg chan_reg;
+};
+
+struct stm32_dma_desc {
+	struct virt_dma_desc vdesc;
+	bool cyclic;
+	u32 num_sgs;
+	struct stm32_dma_sg_req sg_req[];
+};
+
+struct stm32_dma_chan {
+	struct virt_dma_chan vchan;
+	bool config_init;
+	bool busy;
+	u32 id;
+	u32 irq;
+	struct stm32_dma_desc *desc;
+	u32 next_sg;
+	struct dma_slave_config	dma_sconfig;
+	struct stm32_dma_chan_reg chan_reg;
+};
+
+struct stm32_dma_device {
+	struct dma_device ddev;
+	void __iomem *base;
+	struct clk *clk;
+	struct reset_control *rst;
+	bool mem2mem;
+	struct stm32_dma_chan chan[STM32_DMA_MAX_CHANNELS];
+};
+
+static struct stm32_dma_device *stm32_dma_get_dev(struct stm32_dma_chan *chan)
+{
+	return container_of(chan->vchan.chan.device, struct stm32_dma_device,
+			    ddev);
+}
+
+static struct stm32_dma_chan *to_stm32_dma_chan(struct dma_chan *c)
+{
+	return container_of(c, struct stm32_dma_chan, vchan.chan);
+}
+
+static struct stm32_dma_desc *to_stm32_dma_desc(struct virt_dma_desc *vdesc)
+{
+	return container_of(vdesc, struct stm32_dma_desc, vdesc);
+}
+
+static struct device *chan2dev(struct stm32_dma_chan *chan)
+{
+	return &chan->vchan.chan.dev->device;
+}
+
+static u32 stm32_dma_read(struct stm32_dma_device *dmadev, u32 reg)
+{
+	return readl_relaxed(dmadev->base + reg);
+}
+
+static void stm32_dma_write(struct stm32_dma_device *dmadev, u32 reg, u32 val)
+{
+	writel_relaxed(val, dmadev->base + reg);
+}
+
+static struct stm32_dma_desc *stm32_dma_alloc_desc(u32 num_sgs)
+{
+	return kzalloc(sizeof(struct stm32_dma_desc) +
+		       sizeof(struct stm32_dma_sg_req) * num_sgs, GFP_NOWAIT);
+}
+
+static int stm32_dma_get_width(struct stm32_dma_chan *chan,
+			       enum dma_slave_buswidth width)
+{
+	switch (width) {
+	case DMA_SLAVE_BUSWIDTH_1_BYTE:
+		return STM32_DMA_BYTE;
+	case DMA_SLAVE_BUSWIDTH_2_BYTES:
+		return STM32_DMA_HALF_WORD;
+	case DMA_SLAVE_BUSWIDTH_4_BYTES:
+		return STM32_DMA_WORD;
+	default:
+		dev_err(chan2dev(chan), "Dma bus width not supported\n");
+		return -EINVAL;
+	}
+}
+
+static int stm32_dma_get_burst(struct stm32_dma_chan *chan, u32 maxburst)
+{
+	switch (maxburst) {
+	case 0:
+	case 1:
+		return STM32_DMA_BURST_SINGLE;
+	case 4:
+		return STM32_DMA_BURST_INCR4;
+	case 8:
+		return STM32_DMA_BURST_INCR8;
+	case 16:
+		return STM32_DMA_BURST_INCR16;
+	default:
+		dev_err(chan2dev(chan), "Dma burst size not supported\n");
+		return -EINVAL;
+	}
+}
+
+static void stm32_dma_set_fifo_config(struct stm32_dma_chan *chan,
+				      u32 src_maxburst, u32 dst_maxburst)
+{
+	chan->chan_reg.dma_sfcr &= ~STM32_DMA_SFCR_MASK;
+	chan->chan_reg.dma_scr &= ~STM32_DMA_SCR_DMEIE;
+
+	if ((!src_maxburst) && (!dst_maxburst)) {
+		/* Using direct mode */
+		chan->chan_reg.dma_scr |= STM32_DMA_SCR_DMEIE;
+	} else {
+		/* Using FIFO mode */
+		chan->chan_reg.dma_sfcr |= STM32_DMA_SFCR_MASK;
+	}
+}
+
+static int stm32_dma_slave_config(struct dma_chan *c,
+				  struct dma_slave_config *config)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+
+	memcpy(&chan->dma_sconfig, config, sizeof(*config));
+
+	chan->config_init = true;
+
+	return 0;
+}
+
+static u32 stm32_dma_irq_status(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 flags, dma_isr;
+
+	/*
+	 * Read "flags" from DMA_xISR register corresponding to the selected
+	 * DMA channel at the correct bit offset inside that register.
+	 *
+	 * If (ch % 4) is 2 or 3, left shift the mask by 16 bits.
+	 * If (ch % 4) is 1 or 3, additionally left shift the mask by 6 bits.
+	 */
+
+	if (chan->id & 4)
+		dma_isr = stm32_dma_read(dmadev, STM32_DMA_HISR);
+	else
+		dma_isr = stm32_dma_read(dmadev, STM32_DMA_LISR);
+
+	flags = dma_isr >> (((chan->id & 2) << 3) | ((chan->id & 1) * 6));
+
+	return flags;
+}
+
+static void stm32_dma_irq_clear(struct stm32_dma_chan *chan, u32 flags)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 dma_ifcr;
+
+	/*
+	 * Write "flags" to the DMA_xIFCR register corresponding to the selected
+	 * DMA channel at the correct bit offset inside that register.
+	 *
+	 * If (ch % 4) is 2 or 3, left shift the mask by 16 bits.
+	 * If (ch % 4) is 1 or 3, additionally left shift the mask by 6 bits.
+	 */
+	dma_ifcr = flags << (((chan->id & 2) << 3) | ((chan->id & 1) * 6));
+
+	if (chan->id & 4)
+		stm32_dma_write(dmadev, STM32_DMA_HIFCR, dma_ifcr);
+	else
+		stm32_dma_write(dmadev, STM32_DMA_LIFCR, dma_ifcr);
+}
+
+static int stm32_dma_disable_chan(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	unsigned long timeout = jiffies + msecs_to_jiffies(5000);
+	u32 dma_scr, id;
+
+	id = chan->id;
+	dma_scr = stm32_dma_read(dmadev, STM32_DMA_SCR(id));
+
+	if (dma_scr & STM32_DMA_SCR_EN) {
+		dma_scr &= ~STM32_DMA_SCR_EN;
+		stm32_dma_write(dmadev, STM32_DMA_SCR(id), dma_scr);
+
+		do {
+			dma_scr = stm32_dma_read(dmadev, STM32_DMA_SCR(id));
+			dma_scr &= STM32_DMA_SCR_EN;
+			if (!dma_scr)
+				break;
+
+			if (time_after_eq(jiffies, timeout)) {
+				dev_err(chan2dev(chan), "%s: timeout!\n",
+					__func__);
+				return -EBUSY;
+			}
+			cond_resched();
+		} while (1);
+	}
+
+	return 0;
+}
+
+static void stm32_dma_stop(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 dma_scr, dma_sfcr, status;
+	int ret;
+
+	/* Disable interrupts */
+	dma_scr = stm32_dma_read(dmadev, STM32_DMA_SCR(chan->id));
+	dma_scr &= ~STM32_DMA_SCR_IRQ_MASK;
+	stm32_dma_write(dmadev, STM32_DMA_SCR(chan->id), dma_scr);
+	dma_sfcr = stm32_dma_read(dmadev, STM32_DMA_SFCR(chan->id));
+	dma_sfcr &= ~STM32_DMA_SFCR_FEIE;
+	stm32_dma_write(dmadev, STM32_DMA_SFCR(chan->id), dma_sfcr);
+
+	/* Disable DMA */
+	ret = stm32_dma_disable_chan(chan);
+	if (ret < 0)
+		return;
+
+	/* Clear interrupt status if it is there */
+	status = stm32_dma_irq_status(chan);
+	if (status) {
+		dev_dbg(chan2dev(chan), "%s(): clearing interrupt: 0x%08x\n",
+			__func__, status);
+		stm32_dma_irq_clear(chan, status);
+	}
+
+	chan->busy = false;
+}
+
+static int stm32_dma_terminate_all(struct dma_chan *c)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	unsigned long flags;
+	LIST_HEAD(head);
+
+	spin_lock_irqsave(&chan->vchan.lock, flags);
+
+	if (chan->busy) {
+		stm32_dma_stop(chan);
+		chan->desc = NULL;
+	}
+
+	vchan_get_all_descriptors(&chan->vchan, &head);
+	spin_unlock_irqrestore(&chan->vchan.lock, flags);
+	vchan_dma_desc_free_list(&chan->vchan, &head);
+
+	return 0;
+}
+
+static void stm32_dma_dump_reg(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 scr = stm32_dma_read(dmadev, STM32_DMA_SCR(chan->id));
+	u32 ndtr = stm32_dma_read(dmadev, STM32_DMA_SNDTR(chan->id));
+	u32 spar = stm32_dma_read(dmadev, STM32_DMA_SPAR(chan->id));
+	u32 sm0ar = stm32_dma_read(dmadev, STM32_DMA_SM0AR(chan->id));
+	u32 sm1ar = stm32_dma_read(dmadev, STM32_DMA_SM1AR(chan->id));
+	u32 sfcr = stm32_dma_read(dmadev, STM32_DMA_SFCR(chan->id));
+
+	dev_dbg(chan2dev(chan), "SCR:   0x%08x\n", scr);
+	dev_dbg(chan2dev(chan), "NDTR:  0x%08x\n", ndtr);
+	dev_dbg(chan2dev(chan), "SPAR:  0x%08x\n", spar);
+	dev_dbg(chan2dev(chan), "SM0AR: 0x%08x\n", sm0ar);
+	dev_dbg(chan2dev(chan), "SM1AR: 0x%08x\n", sm1ar);
+	dev_dbg(chan2dev(chan), "SFCR:  0x%08x\n", sfcr);
+}
+
+static int stm32_dma_start_transfer(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	struct virt_dma_desc *vdesc;
+	struct stm32_dma_sg_req *sg_req;
+	struct stm32_dma_chan_reg *reg;
+	u32 status;
+	int ret;
+
+	ret = stm32_dma_disable_chan(chan);
+	if (ret < 0)
+		return ret;
+
+	if (!chan->desc) {
+		vdesc = vchan_next_desc(&chan->vchan);
+		if (!vdesc)
+			return -EPERM;
+
+		chan->desc = to_stm32_dma_desc(vdesc);
+		chan->next_sg = 0;
+	}
+
+	if (chan->next_sg == chan->desc->num_sgs)
+		chan->next_sg = 0;
+
+	sg_req = &chan->desc->sg_req[chan->next_sg];
+	reg = &sg_req->chan_reg;
+
+	stm32_dma_write(dmadev, STM32_DMA_SCR(chan->id), reg->dma_scr);
+	stm32_dma_write(dmadev, STM32_DMA_SPAR(chan->id), reg->dma_spar);
+	stm32_dma_write(dmadev, STM32_DMA_SM0AR(chan->id), reg->dma_sm0ar);
+	stm32_dma_write(dmadev, STM32_DMA_SFCR(chan->id), reg->dma_sfcr);
+	stm32_dma_write(dmadev, STM32_DMA_SM1AR(chan->id), reg->dma_sm1ar);
+	stm32_dma_write(dmadev, STM32_DMA_SNDTR(chan->id), reg->dma_sndtr);
+
+	chan->next_sg++;
+
+	/* Clear interrupt status if it is there */
+	status = stm32_dma_irq_status(chan);
+	if (status)
+		stm32_dma_irq_clear(chan, status);
+
+	stm32_dma_dump_reg(chan);
+
+	/* Start DMA */
+	reg->dma_scr |= STM32_DMA_SCR_EN;
+	stm32_dma_write(dmadev, STM32_DMA_SCR(chan->id), reg->dma_scr);
+
+	chan->busy = true;
+
+	return 0;
+}
+
+static void stm32_dma_configure_next_sg(struct stm32_dma_chan *chan)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	struct stm32_dma_sg_req *sg_req;
+	u32 dma_scr, dma_sm0ar, dma_sm1ar, id;
+
+	id = chan->id;
+	dma_scr = stm32_dma_read(dmadev, STM32_DMA_SCR(id));
+
+	if (dma_scr & STM32_DMA_SCR_DBM) {
+		if (chan->next_sg == chan->desc->num_sgs)
+			chan->next_sg = 0;
+
+		sg_req = &chan->desc->sg_req[chan->next_sg];
+
+		if (dma_scr & STM32_DMA_SCR_CT) {
+			dma_sm0ar = sg_req->chan_reg.dma_sm0ar;
+			stm32_dma_write(dmadev, STM32_DMA_SM0AR(id), dma_sm0ar);
+			dev_dbg(chan2dev(chan), "CT=1 <=> SM0AR: 0x%08x\n",
+				stm32_dma_read(dmadev, STM32_DMA_SM0AR(id)));
+		} else {
+			dma_sm1ar = sg_req->chan_reg.dma_sm1ar;
+			stm32_dma_write(dmadev, STM32_DMA_SM1AR(id), dma_sm1ar);
+			dev_dbg(chan2dev(chan), "CT=0 <=> SM1AR: 0x%08x\n",
+				stm32_dma_read(dmadev, STM32_DMA_SM1AR(id)));
+		}
+
+		chan->next_sg++;
+	}
+}
+
+static void stm32_dma_handle_chan_done(struct stm32_dma_chan *chan)
+{
+	if (chan->desc) {
+		if (chan->desc->cyclic) {
+			vchan_cyclic_callback(&chan->desc->vdesc);
+			stm32_dma_configure_next_sg(chan);
+		} else {
+			chan->busy = false;
+			if (chan->next_sg == chan->desc->num_sgs) {
+				list_del(&chan->desc->vdesc.node);
+				vchan_cookie_complete(&chan->desc->vdesc);
+				chan->desc = NULL;
+			}
+			stm32_dma_start_transfer(chan);
+		}
+	}
+}
+
+static irqreturn_t stm32_dma_chan_irq(int irq, void *devid)
+{
+	struct stm32_dma_chan *chan = devid;
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 status, scr, sfcr;
+
+	spin_lock(&chan->vchan.lock);
+
+	status = stm32_dma_irq_status(chan);
+	scr = stm32_dma_read(dmadev, STM32_DMA_SCR(chan->id));
+	sfcr = stm32_dma_read(dmadev, STM32_DMA_SFCR(chan->id));
+
+	if ((status & STM32_DMA_TCI) && (scr & STM32_DMA_SCR_TCIE)) {
+		stm32_dma_irq_clear(chan, STM32_DMA_TCI);
+		stm32_dma_handle_chan_done(chan);
+
+	} else {
+		stm32_dma_irq_clear(chan, status);
+		dev_err(chan2dev(chan), "DMA error: status=0x%08x\n", status);
+	}
+
+	spin_unlock(&chan->vchan.lock);
+
+	return IRQ_HANDLED;
+}
+
+static void stm32_dma_issue_pending(struct dma_chan *c)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&chan->vchan.lock, flags);
+	if (!chan->busy) {
+		if (vchan_issue_pending(&chan->vchan) && !chan->desc) {
+			ret = stm32_dma_start_transfer(chan);
+			if ((!ret) && (chan->desc->cyclic))
+				stm32_dma_configure_next_sg(chan);
+		}
+	}
+	spin_unlock_irqrestore(&chan->vchan.lock, flags);
+}
+
+static int stm32_dma_set_xfer_param(struct stm32_dma_chan *chan,
+				    enum dma_transfer_direction direction,
+				    enum dma_slave_buswidth *buswidth)
+{
+	enum dma_slave_buswidth src_addr_width, dst_addr_width;
+	int src_bus_width, dst_bus_width;
+	int src_burst_size, dst_burst_size;
+	u32 src_maxburst, dst_maxburst;
+	dma_addr_t src_addr, dst_addr;
+	u32 dma_scr = 0;
+
+	src_addr_width = chan->dma_sconfig.src_addr_width;
+	dst_addr_width = chan->dma_sconfig.dst_addr_width;
+	src_maxburst = chan->dma_sconfig.src_maxburst;
+	dst_maxburst = chan->dma_sconfig.dst_maxburst;
+	src_addr = chan->dma_sconfig.src_addr;
+	dst_addr = chan->dma_sconfig.dst_addr;
+
+	switch (direction) {
+	case DMA_MEM_TO_DEV:
+		dst_bus_width = stm32_dma_get_width(chan, dst_addr_width);
+		if (dst_bus_width < 0)
+			return dst_bus_width;
+
+		dst_burst_size = stm32_dma_get_burst(chan, dst_maxburst);
+		if (dst_burst_size < 0)
+			return dst_burst_size;
+
+		if (!src_addr_width)
+			src_addr_width = dst_addr_width;
+
+		src_bus_width = stm32_dma_get_width(chan, src_addr_width);
+		if (src_bus_width < 0)
+			return src_bus_width;
+
+		src_burst_size = stm32_dma_get_burst(chan, src_maxburst);
+		if (src_burst_size < 0)
+			return src_burst_size;
+
+		dma_scr = STM32_DMA_SCR_DIR(STM32_DMA_MEM_TO_DEV) |
+			STM32_DMA_SCR_PSIZE(dst_bus_width) |
+			STM32_DMA_SCR_MSIZE(src_bus_width) |
+			STM32_DMA_SCR_PBURST(dst_burst_size) |
+			STM32_DMA_SCR_MBURST(src_burst_size);
+
+		chan->chan_reg.dma_spar = chan->dma_sconfig.dst_addr;
+		*buswidth = dst_addr_width;
+		break;
+
+	case DMA_DEV_TO_MEM:
+		src_bus_width = stm32_dma_get_width(chan, src_addr_width);
+		if (src_bus_width < 0)
+			return src_bus_width;
+
+		src_burst_size = stm32_dma_get_burst(chan, src_maxburst);
+		if (src_burst_size < 0)
+			return src_burst_size;
+
+		if (!dst_addr_width)
+			dst_addr_width = src_addr_width;
+
+		dst_bus_width = stm32_dma_get_width(chan, dst_addr_width);
+		if (dst_bus_width < 0)
+			return dst_bus_width;
+
+		dst_burst_size = stm32_dma_get_burst(chan, dst_maxburst);
+		if (dst_burst_size < 0)
+			return dst_burst_size;
+
+		dma_scr = STM32_DMA_SCR_DIR(STM32_DMA_DEV_TO_MEM) |
+			STM32_DMA_SCR_PSIZE(src_bus_width) |
+			STM32_DMA_SCR_MSIZE(dst_bus_width) |
+			STM32_DMA_SCR_PBURST(src_burst_size) |
+			STM32_DMA_SCR_MBURST(dst_burst_size);
+
+		chan->chan_reg.dma_spar = chan->dma_sconfig.src_addr;
+		*buswidth = chan->dma_sconfig.src_addr_width;
+		break;
+
+	default:
+		dev_err(chan2dev(chan), "Dma direction is not supported\n");
+		return -EINVAL;
+	}
+
+	stm32_dma_set_fifo_config(chan, src_maxburst, dst_maxburst);
+
+	chan->chan_reg.dma_scr &= ~(STM32_DMA_SCR_DIR_MASK |
+			STM32_DMA_SCR_PSIZE_MASK | STM32_DMA_SCR_MSIZE_MASK |
+			STM32_DMA_SCR_PBURST_MASK | STM32_DMA_SCR_MBURST_MASK);
+	chan->chan_reg.dma_scr |= dma_scr;
+
+	return 0;
+}
+
+static void stm32_dma_clear_reg(struct stm32_dma_chan_reg *regs)
+{
+	memset(regs, 0, sizeof(struct stm32_dma_chan_reg));
+}
+
+static struct dma_async_tx_descriptor *stm32_dma_prep_slave_sg(
+	struct dma_chan *c, struct scatterlist *sgl,
+	u32 sg_len, enum dma_transfer_direction direction,
+	unsigned long flags, void *context)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	struct stm32_dma_desc *desc;
+	struct scatterlist *sg;
+	enum dma_slave_buswidth buswidth;
+	u32 nb_data_items;
+	int i, ret;
+
+	if (!chan->config_init) {
+		dev_err(chan2dev(chan), "dma channel is not configured\n");
+		return NULL;
+	}
+
+	if (sg_len < 1) {
+		dev_err(chan2dev(chan), "Invalid segment length %d\n", sg_len);
+		return NULL;
+	}
+
+	desc = stm32_dma_alloc_desc(sg_len);
+	if (!desc)
+		return NULL;
+
+	ret = stm32_dma_set_xfer_param(chan, direction, &buswidth);
+	if (ret < 0)
+		goto err;
+
+	/* Set peripheral flow controller */
+	if (chan->dma_sconfig.device_fc)
+		chan->chan_reg.dma_scr |= STM32_DMA_SCR_PFCTRL;
+	else
+		chan->chan_reg.dma_scr &= ~STM32_DMA_SCR_PFCTRL;
+
+	for_each_sg(sgl, sg, sg_len, i) {
+		desc->sg_req[i].len = sg_dma_len(sg);
+
+		nb_data_items = desc->sg_req[i].len / buswidth;
+		if (nb_data_items > STM32_DMA_MAX_DATA_ITEMS) {
+			dev_err(chan2dev(chan), "nb items not supported\n");
+			goto err;
+		}
+
+		stm32_dma_clear_reg(&desc->sg_req[i].chan_reg);
+		desc->sg_req[i].chan_reg.dma_scr = chan->chan_reg.dma_scr;
+		desc->sg_req[i].chan_reg.dma_sfcr = chan->chan_reg.dma_sfcr;
+		desc->sg_req[i].chan_reg.dma_spar = chan->chan_reg.dma_spar;
+		desc->sg_req[i].chan_reg.dma_sm0ar = sg_dma_address(sg);
+		desc->sg_req[i].chan_reg.dma_sm1ar = sg_dma_address(sg);
+		desc->sg_req[i].chan_reg.dma_sndtr = nb_data_items;
+	}
+
+	desc->num_sgs = sg_len;
+	desc->cyclic = false;
+
+	return vchan_tx_prep(&chan->vchan, &desc->vdesc, flags);
+
+err:
+	kfree(desc);
+	return NULL;
+}
+
+static struct dma_async_tx_descriptor *stm32_dma_prep_dma_cyclic(
+	struct dma_chan *c, dma_addr_t buf_addr, size_t buf_len,
+	size_t period_len, enum dma_transfer_direction direction,
+	unsigned long flags)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	struct stm32_dma_desc *desc;
+	enum dma_slave_buswidth buswidth;
+	u32 num_periods, nb_data_items;
+	int i, ret;
+
+	if (!buf_len || !period_len) {
+		dev_err(chan2dev(chan), "Invalid buffer/period len\n");
+		return NULL;
+	}
+
+	if (!chan->config_init) {
+		dev_err(chan2dev(chan), "dma channel is not configured\n");
+		return NULL;
+	}
+
+	if (buf_len % period_len) {
+		dev_err(chan2dev(chan), "buf_len not multiple of period_len\n");
+		return NULL;
+	}
+
+	/*
+	 * We allow to take more number of requests till DMA is
+	 * not started. The driver will loop over all requests.
+	 * Once DMA is started then new requests can be queued only after
+	 * terminating the DMA.
+	 */
+	if (chan->busy) {
+		dev_err(chan2dev(chan), "Request not allowed when dma busy\n");
+		return NULL;
+	}
+
+	ret = stm32_dma_set_xfer_param(chan, direction, &buswidth);
+	if (ret < 0)
+		return NULL;
+
+	nb_data_items = period_len / buswidth;
+	if (nb_data_items > STM32_DMA_MAX_DATA_ITEMS) {
+		dev_err(chan2dev(chan), "number of items not supported\n");
+		return NULL;
+	}
+
+	/*  Enable Circular mode or double buffer mode */
+	if (buf_len == period_len)
+		chan->chan_reg.dma_scr |= STM32_DMA_SCR_CIRC;
+	else
+		chan->chan_reg.dma_scr |= STM32_DMA_SCR_DBM;
+
+	/* Clear periph ctrl if client set it */
+	chan->chan_reg.dma_scr &= ~STM32_DMA_SCR_PFCTRL;
+
+	num_periods = buf_len / period_len;
+
+	desc = stm32_dma_alloc_desc(num_periods);
+	if (!desc)
+		return NULL;
+
+	for (i = 0; i < num_periods; i++) {
+		desc->sg_req[i].len = period_len;
+
+		stm32_dma_clear_reg(&desc->sg_req[i].chan_reg);
+		desc->sg_req[i].chan_reg.dma_scr = chan->chan_reg.dma_scr;
+		desc->sg_req[i].chan_reg.dma_sfcr = chan->chan_reg.dma_sfcr;
+		desc->sg_req[i].chan_reg.dma_spar = chan->chan_reg.dma_spar;
+		desc->sg_req[i].chan_reg.dma_sm0ar = buf_addr;
+		desc->sg_req[i].chan_reg.dma_sm1ar = buf_addr;
+		desc->sg_req[i].chan_reg.dma_sndtr = nb_data_items;
+		buf_addr += period_len;
+	}
+
+	desc->num_sgs = num_periods;
+	desc->cyclic = true;
+
+	return vchan_tx_prep(&chan->vchan, &desc->vdesc, flags);
+}
+
+static struct dma_async_tx_descriptor *stm32_dma_prep_dma_memcpy(
+	struct dma_chan *c, dma_addr_t dest,
+	dma_addr_t src, size_t len, unsigned long flags)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	u32 num_sgs;
+	struct stm32_dma_desc *desc;
+	size_t xfer_count, offset;
+	int i;
+
+	num_sgs = DIV_ROUND_UP(len, STM32_DMA_MAX_DATA_ITEMS);
+	desc = stm32_dma_alloc_desc(num_sgs);
+	if (!desc)
+		return NULL;
+
+	for (offset = 0, i = 0; offset < len; offset += xfer_count, i++) {
+		xfer_count = min_t(size_t, len - offset,
+				   STM32_DMA_MAX_DATA_ITEMS);
+
+		desc->sg_req[i].len = xfer_count;
+
+		stm32_dma_clear_reg(&desc->sg_req[i].chan_reg);
+		desc->sg_req[i].chan_reg.dma_scr =
+			STM32_DMA_SCR_DIR(STM32_DMA_MEM_TO_MEM) |
+			STM32_DMA_SCR_MINC |
+			STM32_DMA_SCR_PINC |
+			STM32_DMA_SCR_TCIE |
+			STM32_DMA_SCR_TEIE;
+		desc->sg_req[i].chan_reg.dma_sfcr = STM32_DMA_SFCR_DMDIS |
+			STM32_DMA_SFCR_FTH(STM32_DMA_FIFO_THRESHOLD_FULL) |
+			STM32_DMA_SFCR_FEIE;
+		desc->sg_req[i].chan_reg.dma_spar = src + offset;
+		desc->sg_req[i].chan_reg.dma_sm0ar = dest + offset;
+		desc->sg_req[i].chan_reg.dma_sndtr = xfer_count;
+	}
+
+	desc->num_sgs = num_sgs;
+	desc->cyclic = false;
+
+	return vchan_tx_prep(&chan->vchan, &desc->vdesc, flags);
+}
+
+static size_t stm32_dma_desc_residue(struct stm32_dma_chan *chan,
+				     struct stm32_dma_desc *desc,
+				     u32 next_sg)
+{
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	u32 dma_scr, width, residue, count;
+	int i;
+
+	residue = 0;
+
+	for (i = next_sg; i < desc->num_sgs; i++)
+		residue += desc->sg_req[i].len;
+
+	if (next_sg != 0) {
+		dma_scr = stm32_dma_read(dmadev, STM32_DMA_SCR(chan->id));
+		width = STM32_DMA_SCR_PSIZE_GET(dma_scr);
+		count = stm32_dma_read(dmadev, STM32_DMA_SNDTR(chan->id));
+
+		residue += count << width;
+	}
+
+	return residue;
+}
+
+static enum dma_status stm32_dma_tx_status(struct dma_chan *c,
+					   dma_cookie_t cookie,
+					   struct dma_tx_state *state)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	struct virt_dma_desc *vdesc;
+	enum dma_status status;
+	unsigned long flags;
+	u32 residue;
+
+	status = dma_cookie_status(c, cookie, state);
+	if ((status == DMA_COMPLETE) || (!state))
+		return status;
+
+	spin_lock_irqsave(&chan->vchan.lock, flags);
+	vdesc = vchan_find_desc(&chan->vchan, cookie);
+	if (cookie == chan->desc->vdesc.tx.cookie) {
+		residue = stm32_dma_desc_residue(chan, chan->desc,
+						 chan->next_sg);
+	} else if (vdesc) {
+		residue = stm32_dma_desc_residue(chan,
+						 to_stm32_dma_desc(vdesc), 0);
+	} else {
+		residue = 0;
+	}
+
+	dma_set_residue(state, residue);
+
+	spin_unlock_irqrestore(&chan->vchan.lock, flags);
+
+	return status;
+}
+
+static int stm32_dma_alloc_chan_resources(struct dma_chan *c)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	int ret;
+
+	chan->config_init = false;
+	ret = clk_prepare_enable(dmadev->clk);
+	if (ret < 0) {
+		dev_err(chan2dev(chan), "clk_prepare_enable failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = stm32_dma_disable_chan(chan);
+	if (ret < 0)
+		clk_disable_unprepare(dmadev->clk);
+
+	return ret;
+}
+
+static void stm32_dma_free_chan_resources(struct dma_chan *c)
+{
+	struct stm32_dma_chan *chan = to_stm32_dma_chan(c);
+	struct stm32_dma_device *dmadev = stm32_dma_get_dev(chan);
+	unsigned long flags;
+
+	dev_dbg(chan2dev(chan), "Freeing channel %d\n", chan->id);
+
+	if (chan->busy) {
+		spin_lock_irqsave(&chan->vchan.lock, flags);
+		stm32_dma_stop(chan);
+		chan->desc = NULL;
+		spin_unlock_irqrestore(&chan->vchan.lock, flags);
+	}
+
+	clk_disable_unprepare(dmadev->clk);
+
+	vchan_free_chan_resources(to_virt_chan(c));
+}
+
+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,
+			  struct stm32_dma_cfg *cfg)
+{
+	stm32_dma_clear_reg(&chan->chan_reg);
+
+	chan->chan_reg.dma_scr = cfg->stream_config & STM32_DMA_SCR_CFG_MASK;
+	chan->chan_reg.dma_scr |= STM32_DMA_SCR_REQ(cfg->request_line);
+
+	/* Enable Interrupts  */
+	chan->chan_reg.dma_scr |= STM32_DMA_SCR_TEIE | STM32_DMA_SCR_TCIE;
+
+	chan->chan_reg.dma_sfcr = cfg->threshold & STM32_DMA_SFCR_FTH_MASK;
+}
+
+static struct dma_chan *stm32_dma_of_xlate(struct of_phandle_args *dma_spec,
+					   struct of_dma *ofdma)
+{
+	struct stm32_dma_device *dmadev = ofdma->of_dma_data;
+	struct stm32_dma_cfg cfg;
+	struct stm32_dma_chan *chan;
+	struct dma_chan *c;
+
+	if (dma_spec->args_count < 3)
+		return NULL;
+
+	cfg.channel_id = dma_spec->args[0];
+	cfg.request_line = dma_spec->args[1];
+	cfg.stream_config = dma_spec->args[2];
+	cfg.threshold = 0;
+
+	if ((cfg.channel_id >= STM32_DMA_MAX_CHANNELS) || (cfg.request_line >=
+				STM32_DMA_MAX_REQUEST_ID))
+		return NULL;
+
+	if (dma_spec->args_count > 3)
+		cfg.threshold = dma_spec->args[3];
+
+	chan = &dmadev->chan[cfg.channel_id];
+
+	c = dma_get_slave_channel(&chan->vchan.chan);
+	if (c)
+		stm32_dma_set_config(chan, &cfg);
+
+	return c;
+}
+
+static const struct of_device_id stm32_dma_of_match[] = {
+	{ .compatible = "st,stm32-dma", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, stm32_dma_of_match);
+
+static int stm32_dma_probe(struct platform_device *pdev)
+{
+	struct stm32_dma_chan *chan;
+	struct stm32_dma_device *dmadev;
+	struct dma_device *dd;
+	const struct of_device_id *match;
+	struct resource *res;
+	int i, ret;
+
+	match = of_match_device(stm32_dma_of_match, &pdev->dev);
+	if (!match) {
+		dev_err(&pdev->dev, "Error: No device match found\n");
+		return -ENODEV;
+	}
+
+	dmadev = devm_kzalloc(&pdev->dev, sizeof(*dmadev), GFP_KERNEL);
+	if (!dmadev)
+		return -ENOMEM;
+
+	dd = &dmadev->ddev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dmadev->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dmadev->base))
+		return PTR_ERR(dmadev->base);
+
+	dmadev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(dmadev->clk)) {
+		dev_err(&pdev->dev, "Error: Missing controller clock\n");
+		return PTR_ERR(dmadev->clk);
+	}
+
+	dmadev->mem2mem = of_property_read_bool(pdev->dev.of_node,
+						"st,mem2mem");
+
+	dmadev->rst = devm_reset_control_get(&pdev->dev, NULL);
+	if (!IS_ERR(dmadev->rst)) {
+		reset_control_assert(dmadev->rst);
+		udelay(2);
+		reset_control_deassert(dmadev->rst);
+	}
+
+	dma_cap_set(DMA_SLAVE, dd->cap_mask);
+	dma_cap_set(DMA_PRIVATE, dd->cap_mask);
+	dma_cap_set(DMA_CYCLIC, dd->cap_mask);
+	dd->device_alloc_chan_resources = stm32_dma_alloc_chan_resources;
+	dd->device_free_chan_resources = stm32_dma_free_chan_resources;
+	dd->device_tx_status = stm32_dma_tx_status;
+	dd->device_issue_pending = stm32_dma_issue_pending;
+	dd->device_prep_slave_sg = stm32_dma_prep_slave_sg;
+	dd->device_prep_dma_cyclic = stm32_dma_prep_dma_cyclic;
+	dd->device_config = stm32_dma_slave_config;
+	dd->device_terminate_all = stm32_dma_terminate_all;
+	dd->src_addr_widths = BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) |
+		BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) |
+		BIT(DMA_SLAVE_BUSWIDTH_4_BYTES);
+	dd->dst_addr_widths = BIT(DMA_SLAVE_BUSWIDTH_1_BYTE) |
+		BIT(DMA_SLAVE_BUSWIDTH_2_BYTES) |
+		BIT(DMA_SLAVE_BUSWIDTH_4_BYTES);
+	dd->directions = BIT(DMA_DEV_TO_MEM) | BIT(DMA_MEM_TO_DEV);
+	dd->residue_granularity = DMA_RESIDUE_GRANULARITY_BURST;
+	dd->dev = &pdev->dev;
+	INIT_LIST_HEAD(&dd->channels);
+
+	if (dmadev->mem2mem) {
+		dma_cap_set(DMA_MEMCPY, dd->cap_mask);
+		dd->device_prep_dma_memcpy = stm32_dma_prep_dma_memcpy;
+		dd->directions |= BIT(DMA_MEM_TO_MEM);
+	}
+
+	for (i = 0; i < STM32_DMA_MAX_CHANNELS; i++) {
+		chan = &dmadev->chan[i];
+		chan->id = i;
+		chan->vchan.desc_free = stm32_dma_desc_free;
+		vchan_init(&chan->vchan, dd);
+	}
+
+	ret = dma_async_device_register(dd);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < STM32_DMA_MAX_CHANNELS; i++) {
+		chan = &dmadev->chan[i];
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, i);
+		if (!res) {
+			ret = -EINVAL;
+			dev_err(&pdev->dev, "No irq resource for chan %d\n", i);
+			goto err_unregister;
+		}
+		chan->irq = res->start;
+		ret = devm_request_irq(&pdev->dev, chan->irq,
+				       stm32_dma_chan_irq, 0,
+				       dev_name(chan2dev(chan)), chan);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"request_irq failed with err %d channel %d\n",
+				ret, i);
+			goto err_unregister;
+		}
+	}
+
+	ret = of_dma_controller_register(pdev->dev.of_node,
+					 stm32_dma_of_xlate, dmadev);
+	if (ret < 0) {
+		dev_err(&pdev->dev,
+			"STM32 DMA DMA OF registration failed %d\n", ret);
+		goto err_unregister;
+	}
+
+	platform_set_drvdata(pdev, dmadev);
+
+	dev_info(&pdev->dev, "STM32 DMA driver registered\n");
+
+	return 0;
+
+err_unregister:
+	dma_async_device_unregister(dd);
+
+	return ret;
+}
+
+static struct platform_driver stm32_dma_driver = {
+	.driver = {
+		.name = "stm32-dma",
+		.of_match_table = stm32_dma_of_match,
+	},
+};
+
+static int __init stm32_dma_init(void)
+{
+	return platform_driver_probe(&stm32_dma_driver, stm32_dma_probe);
+}
+subsys_initcall(stm32_dma_init);
diff --git a/drivers/dma/tegra20-apb-dma.c b/drivers/dma/tegra20-apb-dma.c
index c8f79dcaaee8..935da8192f59 100644
--- a/drivers/dma/tegra20-apb-dma.c
+++ b/drivers/dma/tegra20-apb-dma.c
@@ -296,7 +296,7 @@ static struct tegra_dma_desc *tegra_dma_desc_get(
 	spin_unlock_irqrestore(&tdc->lock, flags);
 
 	/* Allocate DMA desc */
-	dma_desc = kzalloc(sizeof(*dma_desc), GFP_ATOMIC);
+	dma_desc = kzalloc(sizeof(*dma_desc), GFP_NOWAIT);
 	if (!dma_desc) {
 		dev_err(tdc2dev(tdc), "dma_desc alloc failed\n");
 		return NULL;
@@ -336,7 +336,7 @@ static struct tegra_dma_sg_req *tegra_dma_sg_req_get(
 	}
 	spin_unlock_irqrestore(&tdc->lock, flags);
 
-	sg_req = kzalloc(sizeof(struct tegra_dma_sg_req), GFP_ATOMIC);
+	sg_req = kzalloc(sizeof(struct tegra_dma_sg_req), GFP_NOWAIT);
 	if (!sg_req)
 		dev_err(tdc2dev(tdc), "sg_req alloc failed\n");
 	return sg_req;
@@ -1186,10 +1186,12 @@ static int tegra_dma_alloc_chan_resources(struct dma_chan *dc)
 
 	dma_cookie_init(&tdc->dma_chan);
 	tdc->config_init = false;
-	ret = clk_prepare_enable(tdma->dma_clk);
+
+	ret = pm_runtime_get_sync(tdma->dev);
 	if (ret < 0)
-		dev_err(tdc2dev(tdc), "clk_prepare_enable failed: %d\n", ret);
-	return ret;
+		return ret;
+
+	return 0;
 }
 
 static void tegra_dma_free_chan_resources(struct dma_chan *dc)
@@ -1232,7 +1234,7 @@ static void tegra_dma_free_chan_resources(struct dma_chan *dc)
 		list_del(&sg_req->node);
 		kfree(sg_req);
 	}
-	clk_disable_unprepare(tdma->dma_clk);
+	pm_runtime_put(tdma->dev);
 
 	tdc->slave_id = 0;
 }
@@ -1356,20 +1358,14 @@ static int tegra_dma_probe(struct platform_device *pdev)
 	spin_lock_init(&tdma->global_lock);
 
 	pm_runtime_enable(&pdev->dev);
-	if (!pm_runtime_enabled(&pdev->dev)) {
+	if (!pm_runtime_enabled(&pdev->dev))
 		ret = tegra_dma_runtime_resume(&pdev->dev);
-		if (ret) {
-			dev_err(&pdev->dev, "dma_runtime_resume failed %d\n",
-				ret);
-			goto err_pm_disable;
-		}
-	}
+	else
+		ret = pm_runtime_get_sync(&pdev->dev);
 
-	/* Enable clock before accessing registers */
-	ret = clk_prepare_enable(tdma->dma_clk);
 	if (ret < 0) {
-		dev_err(&pdev->dev, "clk_prepare_enable failed: %d\n", ret);
-		goto err_pm_disable;
+		pm_runtime_disable(&pdev->dev);
+		return ret;
 	}
 
 	/* Reset DMA controller */
@@ -1382,7 +1378,7 @@ static int tegra_dma_probe(struct platform_device *pdev)
 	tdma_write(tdma, TEGRA_APBDMA_CONTROL, 0);
 	tdma_write(tdma, TEGRA_APBDMA_IRQ_MASK_SET, 0xFFFFFFFFul);
 
-	clk_disable_unprepare(tdma->dma_clk);
+	pm_runtime_put(&pdev->dev);
 
 	INIT_LIST_HEAD(&tdma->dma_dev.channels);
 	for (i = 0; i < cdata->nr_channels; i++) {
@@ -1400,8 +1396,7 @@ static int tegra_dma_probe(struct platform_device *pdev)
 		}
 		tdc->irq = res->start;
 		snprintf(tdc->name, sizeof(tdc->name), "apbdma.%d", i);
-		ret = devm_request_irq(&pdev->dev, tdc->irq,
-				tegra_dma_isr, 0, tdc->name, tdc);
+		ret = request_irq(tdc->irq, tegra_dma_isr, 0, tdc->name, tdc);
 		if (ret) {
 			dev_err(&pdev->dev,
 				"request_irq failed with err %d channel %d\n",
@@ -1482,10 +1477,11 @@ err_unregister_dma_dev:
 err_irq:
 	while (--i >= 0) {
 		struct tegra_dma_channel *tdc = &tdma->channels[i];
+
+		free_irq(tdc->irq, tdc);
 		tasklet_kill(&tdc->tasklet);
 	}
 
-err_pm_disable:
 	pm_runtime_disable(&pdev->dev);
 	if (!pm_runtime_status_suspended(&pdev->dev))
 		tegra_dma_runtime_suspend(&pdev->dev);
@@ -1502,6 +1498,7 @@ static int tegra_dma_remove(struct platform_device *pdev)
 
 	for (i = 0; i < tdma->chip_data->nr_channels; ++i) {
 		tdc = &tdma->channels[i];
+		free_irq(tdc->irq, tdc);
 		tasklet_kill(&tdc->tasklet);
 	}
 
@@ -1514,8 +1511,7 @@ static int tegra_dma_remove(struct platform_device *pdev)
 
 static int tegra_dma_runtime_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct tegra_dma *tdma = platform_get_drvdata(pdev);
+	struct tegra_dma *tdma = dev_get_drvdata(dev);
 
 	clk_disable_unprepare(tdma->dma_clk);
 	return 0;
@@ -1523,8 +1519,7 @@ static int tegra_dma_runtime_suspend(struct device *dev)
 
 static int tegra_dma_runtime_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct tegra_dma *tdma = platform_get_drvdata(pdev);
+	struct tegra_dma *tdma = dev_get_drvdata(dev);
 	int ret;
 
 	ret = clk_prepare_enable(tdma->dma_clk);
@@ -1543,7 +1538,7 @@ static int tegra_dma_pm_suspend(struct device *dev)
 	int ret;
 
 	/* Enable clock before accessing register */
-	ret = tegra_dma_runtime_resume(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		return ret;
 
@@ -1552,15 +1547,22 @@ static int tegra_dma_pm_suspend(struct device *dev)
 		struct tegra_dma_channel *tdc = &tdma->channels[i];
 		struct tegra_dma_channel_regs *ch_reg = &tdc->channel_reg;
 
+		/* Only save the state of DMA channels that are in use */
+		if (!tdc->config_init)
+			continue;
+
 		ch_reg->csr = tdc_read(tdc, TEGRA_APBDMA_CHAN_CSR);
 		ch_reg->ahb_ptr = tdc_read(tdc, TEGRA_APBDMA_CHAN_AHBPTR);
 		ch_reg->apb_ptr = tdc_read(tdc, TEGRA_APBDMA_CHAN_APBPTR);
 		ch_reg->ahb_seq = tdc_read(tdc, TEGRA_APBDMA_CHAN_AHBSEQ);
 		ch_reg->apb_seq = tdc_read(tdc, TEGRA_APBDMA_CHAN_APBSEQ);
+		if (tdma->chip_data->support_separate_wcount_reg)
+			ch_reg->wcount = tdc_read(tdc,
+						  TEGRA_APBDMA_CHAN_WCOUNT);
 	}
 
 	/* Disable clock */
-	tegra_dma_runtime_suspend(dev);
+	pm_runtime_put(dev);
 	return 0;
 }
 
@@ -1571,7 +1573,7 @@ static int tegra_dma_pm_resume(struct device *dev)
 	int ret;
 
 	/* Enable clock before accessing register */
-	ret = tegra_dma_runtime_resume(dev);
+	ret = pm_runtime_get_sync(dev);
 	if (ret < 0)
 		return ret;
 
@@ -1583,6 +1585,13 @@ static int tegra_dma_pm_resume(struct device *dev)
 		struct tegra_dma_channel *tdc = &tdma->channels[i];
 		struct tegra_dma_channel_regs *ch_reg = &tdc->channel_reg;
 
+		/* Only restore the state of DMA channels that are in use */
+		if (!tdc->config_init)
+			continue;
+
+		if (tdma->chip_data->support_separate_wcount_reg)
+			tdc_write(tdc, TEGRA_APBDMA_CHAN_WCOUNT,
+				  ch_reg->wcount);
 		tdc_write(tdc, TEGRA_APBDMA_CHAN_APBSEQ, ch_reg->apb_seq);
 		tdc_write(tdc, TEGRA_APBDMA_CHAN_APBPTR, ch_reg->apb_ptr);
 		tdc_write(tdc, TEGRA_APBDMA_CHAN_AHBSEQ, ch_reg->ahb_seq);
@@ -1592,16 +1601,14 @@ static int tegra_dma_pm_resume(struct device *dev)
 	}
 
 	/* Disable clock */
-	tegra_dma_runtime_suspend(dev);
+	pm_runtime_put(dev);
 	return 0;
 }
 #endif
 
 static const struct dev_pm_ops tegra_dma_dev_pm_ops = {
-#ifdef CONFIG_PM
-	.runtime_suspend = tegra_dma_runtime_suspend,
-	.runtime_resume = tegra_dma_runtime_resume,
-#endif
+	SET_RUNTIME_PM_OPS(tegra_dma_runtime_suspend, tegra_dma_runtime_resume,
+			   NULL)
 	SET_SYSTEM_SLEEP_PM_OPS(tegra_dma_pm_suspend, tegra_dma_pm_resume)
 };
 
diff --git a/drivers/dma/ti-dma-crossbar.c b/drivers/dma/ti-dma-crossbar.c
index a415edbe61b1..e107779b1a2e 100644
--- a/drivers/dma/ti-dma-crossbar.c
+++ b/drivers/dma/ti-dma-crossbar.c
@@ -12,7 +12,6 @@
 #include <linux/init.h>
 #include <linux/list.h>
 #include <linux/io.h>
-#include <linux/idr.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/of_dma.h>
@@ -198,7 +197,8 @@ struct ti_dra7_xbar_data {
 	void __iomem *iomem;
 
 	struct dma_router dmarouter;
-	struct idr map_idr;
+	struct mutex mutex;
+	unsigned long *dma_inuse;
 
 	u16 safe_val; /* Value to rest the crossbar lines */
 	u32 xbar_requests; /* number of DMA requests connected to XBAR */
@@ -225,7 +225,9 @@ static void ti_dra7_xbar_free(struct device *dev, void *route_data)
 		map->xbar_in, map->xbar_out);
 
 	ti_dra7_xbar_write(xbar->iomem, map->xbar_out, xbar->safe_val);
-	idr_remove(&xbar->map_idr, map->xbar_out);
+	mutex_lock(&xbar->mutex);
+	clear_bit(map->xbar_out, xbar->dma_inuse);
+	mutex_unlock(&xbar->mutex);
 	kfree(map);
 }
 
@@ -255,8 +257,17 @@ static void *ti_dra7_xbar_route_allocate(struct of_phandle_args *dma_spec,
 		return ERR_PTR(-ENOMEM);
 	}
 
-	map->xbar_out = idr_alloc(&xbar->map_idr, NULL, 0, xbar->dma_requests,
-				  GFP_KERNEL);
+	mutex_lock(&xbar->mutex);
+	map->xbar_out = find_first_zero_bit(xbar->dma_inuse,
+					    xbar->dma_requests);
+	mutex_unlock(&xbar->mutex);
+	if (map->xbar_out == xbar->dma_requests) {
+		dev_err(&pdev->dev, "Run out of free DMA requests\n");
+		kfree(map);
+		return ERR_PTR(-ENOMEM);
+	}
+	set_bit(map->xbar_out, xbar->dma_inuse);
+
 	map->xbar_in = (u16)dma_spec->args[0];
 
 	dma_spec->args[0] = map->xbar_out + xbar->dma_offset;
@@ -278,17 +289,29 @@ static const struct of_device_id ti_dra7_master_match[] = {
 		.compatible = "ti,edma3",
 		.data = (void *)TI_XBAR_EDMA_OFFSET,
 	},
+	{
+		.compatible = "ti,edma3-tpcc",
+		.data = (void *)TI_XBAR_EDMA_OFFSET,
+	},
 	{},
 };
 
+static inline void ti_dra7_xbar_reserve(int offset, int len, unsigned long *p)
+{
+	for (; len > 0; len--)
+		clear_bit(offset + (len - 1), p);
+}
+
 static int ti_dra7_xbar_probe(struct platform_device *pdev)
 {
 	struct device_node *node = pdev->dev.of_node;
 	const struct of_device_id *match;
 	struct device_node *dma_node;
 	struct ti_dra7_xbar_data *xbar;
+	struct property *prop;
 	struct resource *res;
 	u32 safe_val;
+	size_t sz;
 	void __iomem *iomem;
 	int i, ret;
 
@@ -299,8 +322,6 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 	if (!xbar)
 		return -ENOMEM;
 
-	idr_init(&xbar->map_idr);
-
 	dma_node = of_parse_phandle(node, "dma-masters", 0);
 	if (!dma_node) {
 		dev_err(&pdev->dev, "Can't get DMA master node\n");
@@ -322,6 +343,12 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 	}
 	of_node_put(dma_node);
 
+	xbar->dma_inuse = devm_kcalloc(&pdev->dev,
+				       BITS_TO_LONGS(xbar->dma_requests),
+				       sizeof(unsigned long), GFP_KERNEL);
+	if (!xbar->dma_inuse)
+		return -ENOMEM;
+
 	if (of_property_read_u32(node, "dma-requests", &xbar->xbar_requests)) {
 		dev_info(&pdev->dev,
 			 "Missing XBAR input information, using %u.\n",
@@ -332,6 +359,33 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 	if (!of_property_read_u32(node, "ti,dma-safe-map", &safe_val))
 		xbar->safe_val = (u16)safe_val;
 
+
+	prop = of_find_property(node, "ti,reserved-dma-request-ranges", &sz);
+	if (prop) {
+		const char pname[] = "ti,reserved-dma-request-ranges";
+		u32 (*rsv_events)[2];
+		size_t nelm = sz / sizeof(*rsv_events);
+		int i;
+
+		if (!nelm)
+			return -EINVAL;
+
+		rsv_events = kcalloc(nelm, sizeof(*rsv_events), GFP_KERNEL);
+		if (!rsv_events)
+			return -ENOMEM;
+
+		ret = of_property_read_u32_array(node, pname, (u32 *)rsv_events,
+						 nelm * 2);
+		if (ret)
+			return ret;
+
+		for (i = 0; i < nelm; i++) {
+			ti_dra7_xbar_reserve(rsv_events[i][0], rsv_events[i][1],
+					     xbar->dma_inuse);
+		}
+		kfree(rsv_events);
+	}
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	iomem = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(iomem))
@@ -343,18 +397,23 @@ static int ti_dra7_xbar_probe(struct platform_device *pdev)
 	xbar->dmarouter.route_free = ti_dra7_xbar_free;
 	xbar->dma_offset = (u32)match->data;
 
+	mutex_init(&xbar->mutex);
 	platform_set_drvdata(pdev, xbar);
 
 	/* Reset the crossbar */
-	for (i = 0; i < xbar->dma_requests; i++)
-		ti_dra7_xbar_write(xbar->iomem, i, xbar->safe_val);
+	for (i = 0; i < xbar->dma_requests; i++) {
+		if (!test_bit(i, xbar->dma_inuse))
+			ti_dra7_xbar_write(xbar->iomem, i, xbar->safe_val);
+	}
 
 	ret = of_dma_router_register(node, ti_dra7_xbar_route_allocate,
 				     &xbar->dmarouter);
 	if (ret) {
 		/* Restore the defaults for the crossbar */
-		for (i = 0; i < xbar->dma_requests; i++)
-			ti_dra7_xbar_write(xbar->iomem, i, i);
+		for (i = 0; i < xbar->dma_requests; i++) {
+			if (!test_bit(i, xbar->dma_inuse))
+				ti_dra7_xbar_write(xbar->iomem, i, i);
+		}
 	}
 
 	return ret;
diff --git a/drivers/dma/virt-dma.c b/drivers/dma/virt-dma.c
index 6f80432a3f0a..a35c211857dd 100644
--- a/drivers/dma/virt-dma.c
+++ b/drivers/dma/virt-dma.c
@@ -29,7 +29,7 @@ dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *tx)
 	spin_lock_irqsave(&vc->lock, flags);
 	cookie = dma_cookie_assign(tx);
 
-	list_add_tail(&vd->node, &vc->desc_submitted);
+	list_move_tail(&vd->node, &vc->desc_submitted);
 	spin_unlock_irqrestore(&vc->lock, flags);
 
 	dev_dbg(vc->chan.device->dev, "vchan %p: txd %p[%x]: submitted\n",
@@ -39,6 +39,33 @@ dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *tx)
 }
 EXPORT_SYMBOL_GPL(vchan_tx_submit);
 
+/**
+ * vchan_tx_desc_free - free a reusable descriptor
+ * @tx: the transfer
+ *
+ * This function frees a previously allocated reusable descriptor. The only
+ * other way is to clear the DMA_CTRL_REUSE flag and submit one last time the
+ * transfer.
+ *
+ * Returns 0 upon success
+ */
+int vchan_tx_desc_free(struct dma_async_tx_descriptor *tx)
+{
+	struct virt_dma_chan *vc = to_virt_chan(tx->chan);
+	struct virt_dma_desc *vd = to_virt_desc(tx);
+	unsigned long flags;
+
+	spin_lock_irqsave(&vc->lock, flags);
+	list_del(&vd->node);
+	spin_unlock_irqrestore(&vc->lock, flags);
+
+	dev_dbg(vc->chan.device->dev, "vchan %p: txd %p[%x]: freeing\n",
+		vc, vd, vd->tx.cookie);
+	vc->desc_free(vd);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vchan_tx_desc_free);
+
 struct virt_dma_desc *vchan_find_desc(struct virt_dma_chan *vc,
 	dma_cookie_t cookie)
 {
@@ -83,8 +110,10 @@ static void vchan_complete(unsigned long arg)
 		cb_data = vd->tx.callback_param;
 
 		list_del(&vd->node);
-
-		vc->desc_free(vd);
+		if (dmaengine_desc_test_reuse(&vd->tx))
+			list_add(&vd->node, &vc->desc_allocated);
+		else
+			vc->desc_free(vd);
 
 		if (cb)
 			cb(cb_data);
@@ -96,9 +125,13 @@ void vchan_dma_desc_free_list(struct virt_dma_chan *vc, struct list_head *head)
 	while (!list_empty(head)) {
 		struct virt_dma_desc *vd = list_first_entry(head,
 			struct virt_dma_desc, node);
-		list_del(&vd->node);
-		dev_dbg(vc->chan.device->dev, "txd %p: freeing\n", vd);
-		vc->desc_free(vd);
+		if (dmaengine_desc_test_reuse(&vd->tx)) {
+			list_move_tail(&vd->node, &vc->desc_allocated);
+		} else {
+			dev_dbg(vc->chan.device->dev, "txd %p: freeing\n", vd);
+			list_del(&vd->node);
+			vc->desc_free(vd);
+		}
 	}
 }
 EXPORT_SYMBOL_GPL(vchan_dma_desc_free_list);
@@ -108,6 +141,7 @@ void vchan_init(struct virt_dma_chan *vc, struct dma_device *dmadev)
 	dma_cookie_init(&vc->chan);
 
 	spin_lock_init(&vc->lock);
+	INIT_LIST_HEAD(&vc->desc_allocated);
 	INIT_LIST_HEAD(&vc->desc_submitted);
 	INIT_LIST_HEAD(&vc->desc_issued);
 	INIT_LIST_HEAD(&vc->desc_completed);
diff --git a/drivers/dma/virt-dma.h b/drivers/dma/virt-dma.h
index 2fa47745a41f..d9731ca5e262 100644
--- a/drivers/dma/virt-dma.h
+++ b/drivers/dma/virt-dma.h
@@ -29,6 +29,7 @@ struct virt_dma_chan {
 	spinlock_t lock;
 
 	/* protected by vc.lock */
+	struct list_head desc_allocated;
 	struct list_head desc_submitted;
 	struct list_head desc_issued;
 	struct list_head desc_completed;
@@ -55,10 +56,17 @@ static inline struct dma_async_tx_descriptor *vchan_tx_prep(struct virt_dma_chan
 	struct virt_dma_desc *vd, unsigned long tx_flags)
 {
 	extern dma_cookie_t vchan_tx_submit(struct dma_async_tx_descriptor *);
+	extern int vchan_tx_desc_free(struct dma_async_tx_descriptor *);
+	unsigned long flags;
 
 	dma_async_tx_descriptor_init(&vd->tx, &vc->chan);
 	vd->tx.flags = tx_flags;
 	vd->tx.tx_submit = vchan_tx_submit;
+	vd->tx.desc_free = vchan_tx_desc_free;
+
+	spin_lock_irqsave(&vc->lock, flags);
+	list_add_tail(&vd->node, &vc->desc_allocated);
+	spin_unlock_irqrestore(&vc->lock, flags);
 
 	return &vd->tx;
 }
@@ -134,6 +142,7 @@ static inline struct virt_dma_desc *vchan_next_desc(struct virt_dma_chan *vc)
 static inline void vchan_get_all_descriptors(struct virt_dma_chan *vc,
 	struct list_head *head)
 {
+	list_splice_tail_init(&vc->desc_allocated, head);
 	list_splice_tail_init(&vc->desc_submitted, head);
 	list_splice_tail_init(&vc->desc_issued, head);
 	list_splice_tail_init(&vc->desc_completed, head);
@@ -141,14 +150,30 @@ static inline void vchan_get_all_descriptors(struct virt_dma_chan *vc,
 
 static inline void vchan_free_chan_resources(struct virt_dma_chan *vc)
 {
+	struct virt_dma_desc *vd;
 	unsigned long flags;
 	LIST_HEAD(head);
 
 	spin_lock_irqsave(&vc->lock, flags);
 	vchan_get_all_descriptors(vc, &head);
+	list_for_each_entry(vd, &head, node)
+		dmaengine_desc_clear_reuse(&vd->tx);
 	spin_unlock_irqrestore(&vc->lock, flags);
 
 	vchan_dma_desc_free_list(vc, &head);
 }
 
+/**
+ * vchan_synchronize() - synchronize callback execution to the current context
+ * @vc: virtual channel to synchronize
+ *
+ * Makes sure that all scheduled or active callbacks have finished running. For
+ * proper operation the caller has to ensure that no new callbacks are scheduled
+ * after the invocation of this function started.
+ */
+static inline void vchan_synchronize(struct virt_dma_chan *vc)
+{
+	tasklet_kill(&vc->task);
+}
+
 #endif
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index dbf53e08bdd1..be163e20fe56 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -10,7 +10,7 @@ obj-$(CONFIG_EDAC)			:= edac_stub.o
 obj-$(CONFIG_EDAC_MM_EDAC)		+= edac_core.o
 
 edac_core-y	:= edac_mc.o edac_device.o edac_mc_sysfs.o
-edac_core-y	+= edac_module.o edac_device_sysfs.o
+edac_core-y	+= edac_module.o edac_device_sysfs.o wq.o
 
 edac_core-$(CONFIG_EDAC_DEBUG)		+= debugfs.o
 
diff --git a/drivers/edac/edac_device.c b/drivers/edac/edac_device.c
index 592af5f0cf39..a97900333e2d 100644
--- a/drivers/edac/edac_device.c
+++ b/drivers/edac/edac_device.c
@@ -390,11 +390,9 @@ static void edac_device_workq_function(struct work_struct *work_req)
 	 * between integral seconds
 	 */
 	if (edac_dev->poll_msec == 1000)
-		queue_delayed_work(edac_workqueue, &edac_dev->work,
-				round_jiffies_relative(edac_dev->delay));
+		edac_queue_work(&edac_dev->work, round_jiffies_relative(edac_dev->delay));
 	else
-		queue_delayed_work(edac_workqueue, &edac_dev->work,
-				edac_dev->delay);
+		edac_queue_work(&edac_dev->work, edac_dev->delay);
 }
 
 /*
@@ -402,8 +400,8 @@ static void edac_device_workq_function(struct work_struct *work_req)
  *	initialize a workq item for this edac_device instance
  *	passing in the new delay period in msec
  */
-void edac_device_workq_setup(struct edac_device_ctl_info *edac_dev,
-				unsigned msec)
+static void edac_device_workq_setup(struct edac_device_ctl_info *edac_dev,
+				    unsigned msec)
 {
 	edac_dbg(0, "\n");
 
@@ -422,29 +420,23 @@ void edac_device_workq_setup(struct edac_device_ctl_info *edac_dev,
 	 * to fire together on the 1 second exactly
 	 */
 	if (edac_dev->poll_msec == 1000)
-		queue_delayed_work(edac_workqueue, &edac_dev->work,
-				round_jiffies_relative(edac_dev->delay));
+		edac_queue_work(&edac_dev->work, round_jiffies_relative(edac_dev->delay));
 	else
-		queue_delayed_work(edac_workqueue, &edac_dev->work,
-				edac_dev->delay);
+		edac_queue_work(&edac_dev->work, edac_dev->delay);
 }
 
 /*
  * edac_device_workq_teardown
  *	stop the workq processing on this edac_dev
  */
-void edac_device_workq_teardown(struct edac_device_ctl_info *edac_dev)
+static void edac_device_workq_teardown(struct edac_device_ctl_info *edac_dev)
 {
-	int status;
-
 	if (!edac_dev->edac_check)
 		return;
 
-	status = cancel_delayed_work(&edac_dev->work);
-	if (status == 0) {
-		/* workq instance might be running, wait for it */
-		flush_workqueue(edac_workqueue);
-	}
+	edac_dev->op_state = OP_OFFLINE;
+
+	edac_stop_work(&edac_dev->work);
 }
 
 /*
@@ -457,16 +449,15 @@ void edac_device_workq_teardown(struct edac_device_ctl_info *edac_dev)
 void edac_device_reset_delay_period(struct edac_device_ctl_info *edac_dev,
 					unsigned long value)
 {
-	/* cancel the current workq request, without the mutex lock */
-	edac_device_workq_teardown(edac_dev);
+	unsigned long jiffs = msecs_to_jiffies(value);
 
-	/* acquire the mutex before doing the workq setup */
-	mutex_lock(&device_ctls_mutex);
+	if (value == 1000)
+		jiffs = round_jiffies_relative(value);
 
-	/* restart the workq request, with new delay value */
-	edac_device_workq_setup(edac_dev, value);
+	edac_dev->poll_msec = value;
+	edac_dev->delay	    = jiffs;
 
-	mutex_unlock(&device_ctls_mutex);
+	edac_mod_work(&edac_dev->work, jiffs);
 }
 
 /*
diff --git a/drivers/edac/edac_device_sysfs.c b/drivers/edac/edac_device_sysfs.c
index fb68a06ad683..93da1a45c716 100644
--- a/drivers/edac/edac_device_sysfs.c
+++ b/drivers/edac/edac_device_sysfs.c
@@ -237,11 +237,6 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 
 	/* get the /sys/devices/system/edac reference */
 	edac_subsys = edac_get_sysfs_subsys();
-	if (edac_subsys == NULL) {
-		edac_dbg(1, "no edac_subsys error\n");
-		err = -ENODEV;
-		goto err_out;
-	}
 
 	/* Point to the 'edac_subsys' this instance 'reports' to */
 	edac_dev->edac_subsys = edac_subsys;
@@ -256,7 +251,7 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 
 	if (!try_module_get(edac_dev->owner)) {
 		err = -ENODEV;
-		goto err_mod_get;
+		goto err_out;
 	}
 
 	/* register */
@@ -282,9 +277,6 @@ int edac_device_register_sysfs_main_kobj(struct edac_device_ctl_info *edac_dev)
 err_kobj_reg:
 	module_put(edac_dev->owner);
 
-err_mod_get:
-	edac_put_sysfs_subsys();
-
 err_out:
 	return err;
 }
@@ -306,7 +298,6 @@ void edac_device_unregister_sysfs_main_kobj(struct edac_device_ctl_info *dev)
 	 *   b) 'kfree' the memory
 	 */
 	kobject_put(&dev->kobj);
-	edac_put_sysfs_subsys();
 }
 
 /* edac_dev -> instance information */
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 77ecd6a4179a..8adfc167c2e3 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -548,8 +548,7 @@ static void edac_mc_workq_function(struct work_struct *work_req)
 	mutex_unlock(&mem_ctls_mutex);
 
 	/* Reschedule */
-	queue_delayed_work(edac_workqueue, &mci->work,
-			msecs_to_jiffies(edac_mc_get_poll_msec()));
+	edac_queue_work(&mci->work, msecs_to_jiffies(edac_mc_get_poll_msec()));
 }
 
 /*
@@ -561,8 +560,7 @@ static void edac_mc_workq_function(struct work_struct *work_req)
  *
  *		called with the mem_ctls_mutex held
  */
-static void edac_mc_workq_setup(struct mem_ctl_info *mci, unsigned msec,
-				bool init)
+static void edac_mc_workq_setup(struct mem_ctl_info *mci, unsigned msec)
 {
 	edac_dbg(0, "\n");
 
@@ -570,10 +568,9 @@ static void edac_mc_workq_setup(struct mem_ctl_info *mci, unsigned msec,
 	if (mci->op_state != OP_RUNNING_POLL)
 		return;
 
-	if (init)
-		INIT_DELAYED_WORK(&mci->work, edac_mc_workq_function);
+	INIT_DELAYED_WORK(&mci->work, edac_mc_workq_function);
 
-	mod_delayed_work(edac_workqueue, &mci->work, msecs_to_jiffies(msec));
+	edac_queue_work(&mci->work, msecs_to_jiffies(msec));
 }
 
 /*
@@ -586,18 +583,9 @@ static void edac_mc_workq_setup(struct mem_ctl_info *mci, unsigned msec,
  */
 static void edac_mc_workq_teardown(struct mem_ctl_info *mci)
 {
-	int status;
-
-	if (mci->op_state != OP_RUNNING_POLL)
-		return;
-
-	status = cancel_delayed_work(&mci->work);
-	if (status == 0) {
-		edac_dbg(0, "not canceled, flush the queue\n");
+	mci->op_state = OP_OFFLINE;
 
-		/* workq instance might be running, wait for it */
-		flush_workqueue(edac_workqueue);
-	}
+	edac_stop_work(&mci->work);
 }
 
 /*
@@ -616,9 +604,8 @@ void edac_mc_reset_delay_period(unsigned long value)
 	list_for_each(item, &mc_devices) {
 		mci = list_entry(item, struct mem_ctl_info, link);
 
-		edac_mc_workq_setup(mci, value, false);
+		edac_mod_work(&mci->work, value);
 	}
-
 	mutex_unlock(&mem_ctls_mutex);
 }
 
@@ -789,7 +776,7 @@ int edac_mc_add_mc_with_groups(struct mem_ctl_info *mci,
 		/* This instance is NOW RUNNING */
 		mci->op_state = OP_RUNNING_POLL;
 
-		edac_mc_workq_setup(mci, edac_mc_get_poll_msec(), true);
+		edac_mc_workq_setup(mci, edac_mc_get_poll_msec());
 	} else {
 		mci->op_state = OP_RUNNING_INTERRUPT;
 	}
diff --git a/drivers/edac/edac_mc_sysfs.c b/drivers/edac/edac_mc_sysfs.c
index a75acea0f674..26e65ab5932a 100644
--- a/drivers/edac/edac_mc_sysfs.c
+++ b/drivers/edac/edac_mc_sysfs.c
@@ -880,21 +880,26 @@ static struct device_type mci_attr_type = {
 int edac_create_sysfs_mci_device(struct mem_ctl_info *mci,
 				 const struct attribute_group **groups)
 {
+	char *name;
 	int i, err;
 
 	/*
 	 * The memory controller needs its own bus, in order to avoid
 	 * namespace conflicts at /sys/bus/edac.
 	 */
-	mci->bus->name = kasprintf(GFP_KERNEL, "mc%d", mci->mc_idx);
-	if (!mci->bus->name)
+	name = kasprintf(GFP_KERNEL, "mc%d", mci->mc_idx);
+	if (!name)
 		return -ENOMEM;
 
+	mci->bus->name = name;
+
 	edac_dbg(0, "creating bus %s\n", mci->bus->name);
 
 	err = bus_register(mci->bus);
-	if (err < 0)
-		goto fail_free_name;
+	if (err < 0) {
+		kfree(name);
+		return err;
+	}
 
 	/* get the /sys/devices/system/edac subsys reference */
 	mci->dev.type = &mci_attr_type;
@@ -961,8 +966,8 @@ fail_unregister_dimm:
 	device_unregister(&mci->dev);
 fail_unregister_bus:
 	bus_unregister(mci->bus);
-fail_free_name:
-	kfree(mci->bus->name);
+	kfree(name);
+
 	return err;
 }
 
@@ -993,10 +998,12 @@ void edac_remove_sysfs_mci_device(struct mem_ctl_info *mci)
 
 void edac_unregister_sysfs(struct mem_ctl_info *mci)
 {
+	const char *name = mci->bus->name;
+
 	edac_dbg(1, "Unregistering device %s\n", dev_name(&mci->dev));
 	device_unregister(&mci->dev);
 	bus_unregister(mci->bus);
-	kfree(mci->bus->name);
+	kfree(name);
 }
 
 static void mc_attr_release(struct device *dev)
@@ -1018,24 +1025,15 @@ static struct device_type mc_attr_type = {
  */
 int __init edac_mc_sysfs_init(void)
 {
-	struct bus_type *edac_subsys;
 	int err;
 
-	/* get the /sys/devices/system/edac subsys reference */
-	edac_subsys = edac_get_sysfs_subsys();
-	if (edac_subsys == NULL) {
-		edac_dbg(1, "no edac_subsys\n");
-		err = -EINVAL;
-		goto out;
-	}
-
 	mci_pdev = kzalloc(sizeof(*mci_pdev), GFP_KERNEL);
 	if (!mci_pdev) {
 		err = -ENOMEM;
-		goto out_put_sysfs;
+		goto out;
 	}
 
-	mci_pdev->bus = edac_subsys;
+	mci_pdev->bus = edac_get_sysfs_subsys();
 	mci_pdev->type = &mc_attr_type;
 	device_initialize(mci_pdev);
 	dev_set_name(mci_pdev, "mc");
@@ -1050,8 +1048,6 @@ int __init edac_mc_sysfs_init(void)
 
  out_dev_free:
 	kfree(mci_pdev);
- out_put_sysfs:
-	edac_put_sysfs_subsys();
  out:
 	return err;
 }
@@ -1059,5 +1055,4 @@ int __init edac_mc_sysfs_init(void)
 void edac_mc_sysfs_exit(void)
 {
 	device_unregister(mci_pdev);
-	edac_put_sysfs_subsys();
 }
diff --git a/drivers/edac/edac_module.c b/drivers/edac/edac_module.c
index 9cb082a19d8a..5f8543be995a 100644
--- a/drivers/edac/edac_module.c
+++ b/drivers/edac/edac_module.c
@@ -43,9 +43,6 @@ module_param_call(edac_debug_level, edac_set_debug_level, param_get_int,
 MODULE_PARM_DESC(edac_debug_level, "EDAC debug level: [0-4], default: 2");
 #endif
 
-/* scope is to module level only */
-struct workqueue_struct *edac_workqueue;
-
 /*
  * edac_op_state_to_string()
  */
@@ -66,31 +63,37 @@ char *edac_op_state_to_string(int opstate)
 }
 
 /*
- * edac_workqueue_setup
- *	initialize the edac work queue for polling operations
+ * sysfs object: /sys/devices/system/edac
+ *	need to export to other files
  */
-static int edac_workqueue_setup(void)
+static struct bus_type edac_subsys = {
+	.name = "edac",
+	.dev_name = "edac",
+};
+
+static int edac_subsys_init(void)
 {
-	edac_workqueue = create_singlethread_workqueue("edac-poller");
-	if (edac_workqueue == NULL)
-		return -ENODEV;
-	else
-		return 0;
+	int err;
+
+	/* create the /sys/devices/system/edac directory */
+	err = subsys_system_register(&edac_subsys, NULL);
+	if (err)
+		printk(KERN_ERR "Error registering toplevel EDAC sysfs dir\n");
+
+	return err;
 }
 
-/*
- * edac_workqueue_teardown
- *	teardown the edac workqueue
- */
-static void edac_workqueue_teardown(void)
+static void edac_subsys_exit(void)
 {
-	if (edac_workqueue) {
-		flush_workqueue(edac_workqueue);
-		destroy_workqueue(edac_workqueue);
-		edac_workqueue = NULL;
-	}
+	bus_unregister(&edac_subsys);
 }
 
+/* return pointer to the 'edac' node in sysfs */
+struct bus_type *edac_get_sysfs_subsys(void)
+{
+	return &edac_subsys;
+}
+EXPORT_SYMBOL_GPL(edac_get_sysfs_subsys);
 /*
  * edac_init
  *      module initialization entry point
@@ -101,6 +104,10 @@ static int __init edac_init(void)
 
 	edac_printk(KERN_INFO, EDAC_MC, EDAC_VERSION "\n");
 
+	err = edac_subsys_init();
+	if (err)
+		return err;
+
 	/*
 	 * Harvest and clear any boot/initialization PCI parity errors
 	 *
@@ -129,6 +136,8 @@ err_wq:
 	edac_mc_sysfs_exit();
 
 err_sysfs:
+	edac_subsys_exit();
+
 	return err;
 }
 
@@ -144,6 +153,7 @@ static void __exit edac_exit(void)
 	edac_workqueue_teardown();
 	edac_mc_sysfs_exit();
 	edac_debugfs_exit();
+	edac_subsys_exit();
 }
 
 /*
diff --git a/drivers/edac/edac_module.h b/drivers/edac/edac_module.h
index b95a48fc723d..cfaacb99c973 100644
--- a/drivers/edac/edac_module.h
+++ b/drivers/edac/edac_module.h
@@ -47,10 +47,12 @@ extern int edac_device_create_sysfs(struct edac_device_ctl_info *edac_dev);
 extern void edac_device_remove_sysfs(struct edac_device_ctl_info *edac_dev);
 
 /* edac core workqueue: single CPU mode */
-extern struct workqueue_struct *edac_workqueue;
-extern void edac_device_workq_setup(struct edac_device_ctl_info *edac_dev,
-				    unsigned msec);
-extern void edac_device_workq_teardown(struct edac_device_ctl_info *edac_dev);
+int edac_workqueue_setup(void);
+void edac_workqueue_teardown(void);
+bool edac_queue_work(struct delayed_work *work, unsigned long delay);
+bool edac_stop_work(struct delayed_work *work);
+bool edac_mod_work(struct delayed_work *work, unsigned long delay);
+
 extern void edac_device_reset_delay_period(struct edac_device_ctl_info
 					   *edac_dev, unsigned long value);
 extern void edac_mc_reset_delay_period(unsigned long value);
diff --git a/drivers/edac/edac_pci.c b/drivers/edac/edac_pci.c
index 2cf44b4db80c..99685388d3fb 100644
--- a/drivers/edac/edac_pci.c
+++ b/drivers/edac/edac_pci.c
@@ -178,41 +178,6 @@ static void del_edac_pci_from_global_list(struct edac_pci_ctl_info *pci)
 	INIT_LIST_HEAD(&pci->link);
 }
 
-#if 0
-/* Older code, but might use in the future */
-
-/*
- * edac_pci_find()
- * 	Search for an edac_pci_ctl_info structure whose index is 'idx'
- *
- * If found, return a pointer to the structure
- * Else return NULL.
- *
- * Caller must hold pci_ctls_mutex.
- */
-struct edac_pci_ctl_info *edac_pci_find(int idx)
-{
-	struct list_head *item;
-	struct edac_pci_ctl_info *pci;
-
-	/* Iterage over list, looking for exact match of ID */
-	list_for_each(item, &edac_pci_list) {
-		pci = list_entry(item, struct edac_pci_ctl_info, link);
-
-		if (pci->pci_idx >= idx) {
-			if (pci->pci_idx == idx)
-				return pci;
-
-			/* not on list, so terminate early */
-			break;
-		}
-	}
-
-	return NULL;
-}
-EXPORT_SYMBOL_GPL(edac_pci_find);
-#endif
-
 /*
  * edac_pci_workq_function()
  *
@@ -244,7 +209,7 @@ static void edac_pci_workq_function(struct work_struct *work_req)
 			delay = msecs_to_jiffies(msec);
 
 		/* Reschedule only if we are in POLL mode */
-		queue_delayed_work(edac_workqueue, &pci->work, delay);
+		edac_queue_work(&pci->work, delay);
 	}
 
 	mutex_unlock(&edac_pci_ctls_mutex);
@@ -264,8 +229,8 @@ static void edac_pci_workq_setup(struct edac_pci_ctl_info *pci,
 	edac_dbg(0, "\n");
 
 	INIT_DELAYED_WORK(&pci->work, edac_pci_workq_function);
-	queue_delayed_work(edac_workqueue, &pci->work,
-			msecs_to_jiffies(edac_pci_get_poll_msec()));
+
+	edac_queue_work(&pci->work, msecs_to_jiffies(edac_pci_get_poll_msec()));
 }
 
 /*
@@ -274,37 +239,12 @@ static void edac_pci_workq_setup(struct edac_pci_ctl_info *pci,
  */
 static void edac_pci_workq_teardown(struct edac_pci_ctl_info *pci)
 {
-	int status;
-
-	edac_dbg(0, "\n");
-
-	status = cancel_delayed_work(&pci->work);
-	if (status == 0)
-		flush_workqueue(edac_workqueue);
-}
-
-/*
- * edac_pci_reset_delay_period
- *
- *	called with a new period value for the workq period
- *	a) stop current workq timer
- *	b) restart workq timer with new value
- */
-void edac_pci_reset_delay_period(struct edac_pci_ctl_info *pci,
-				 unsigned long value)
-{
 	edac_dbg(0, "\n");
 
-	edac_pci_workq_teardown(pci);
-
-	/* need to lock for the setup */
-	mutex_lock(&edac_pci_ctls_mutex);
-
-	edac_pci_workq_setup(pci, value);
+	pci->op_state = OP_OFFLINE;
 
-	mutex_unlock(&edac_pci_ctls_mutex);
+	edac_stop_work(&pci->work);
 }
-EXPORT_SYMBOL_GPL(edac_pci_reset_delay_period);
 
 /*
  * edac_pci_alloc_index: Allocate a unique PCI index number
diff --git a/drivers/edac/edac_pci_sysfs.c b/drivers/edac/edac_pci_sysfs.c
index 24d877f6e577..6e3428ba400f 100644
--- a/drivers/edac/edac_pci_sysfs.c
+++ b/drivers/edac/edac_pci_sysfs.c
@@ -331,10 +331,7 @@ static struct kobj_type ktype_edac_pci_main_kobj = {
 };
 
 /**
- * edac_pci_main_kobj_setup()
- *
- *	setup the sysfs for EDAC PCI attributes
- *	assumes edac_subsys has already been initialized
+ * edac_pci_main_kobj_setup: Setup the sysfs for EDAC PCI attributes.
  */
 static int edac_pci_main_kobj_setup(void)
 {
@@ -351,11 +348,6 @@ static int edac_pci_main_kobj_setup(void)
 	 * controls and attributes
 	 */
 	edac_subsys = edac_get_sysfs_subsys();
-	if (edac_subsys == NULL) {
-		edac_dbg(1, "no edac_subsys\n");
-		err = -ENODEV;
-		goto decrement_count_fail;
-	}
 
 	/* Bump the reference count on this module to ensure the
 	 * modules isn't unloaded until we deconstruct the top
@@ -364,7 +356,7 @@ static int edac_pci_main_kobj_setup(void)
 	if (!try_module_get(THIS_MODULE)) {
 		edac_dbg(1, "try_module_get() failed\n");
 		err = -ENODEV;
-		goto mod_get_fail;
+		goto decrement_count_fail;
 	}
 
 	edac_pci_top_main_kobj = kzalloc(sizeof(struct kobject), GFP_KERNEL);
@@ -399,9 +391,6 @@ kobject_init_and_add_fail:
 kzalloc_fail:
 	module_put(THIS_MODULE);
 
-mod_get_fail:
-	edac_put_sysfs_subsys();
-
 decrement_count_fail:
 	/* if are on this error exit, nothing to tear down */
 	atomic_dec(&edac_pci_sysfs_refcount);
@@ -426,7 +415,6 @@ static void edac_pci_main_kobj_teardown(void)
 	if (atomic_dec_return(&edac_pci_sysfs_refcount) == 0) {
 		edac_dbg(0, "called kobject_put on main kobj\n");
 		kobject_put(edac_pci_top_main_kobj);
-		edac_put_sysfs_subsys();
 	}
 }
 
diff --git a/drivers/edac/edac_stub.c b/drivers/edac/edac_stub.c
index ff07aae5b7fb..952e411f01f2 100644
--- a/drivers/edac/edac_stub.c
+++ b/drivers/edac/edac_stub.c
@@ -26,8 +26,6 @@ EXPORT_SYMBOL_GPL(edac_handlers);
 int edac_err_assert = 0;
 EXPORT_SYMBOL_GPL(edac_err_assert);
 
-static atomic_t edac_subsys_valid = ATOMIC_INIT(0);
-
 int edac_report_status = EDAC_REPORTING_ENABLED;
 EXPORT_SYMBOL_GPL(edac_report_status);
 
@@ -68,42 +66,3 @@ void edac_atomic_assert_error(void)
 	edac_err_assert++;
 }
 EXPORT_SYMBOL_GPL(edac_atomic_assert_error);
-
-/*
- * sysfs object: /sys/devices/system/edac
- *	need to export to other files
- */
-struct bus_type edac_subsys = {
-	.name = "edac",
-	.dev_name = "edac",
-};
-EXPORT_SYMBOL_GPL(edac_subsys);
-
-/* return pointer to the 'edac' node in sysfs */
-struct bus_type *edac_get_sysfs_subsys(void)
-{
-	int err = 0;
-
-	if (atomic_read(&edac_subsys_valid))
-		goto out;
-
-	/* create the /sys/devices/system/edac directory */
-	err = subsys_system_register(&edac_subsys, NULL);
-	if (err) {
-		printk(KERN_ERR "Error registering toplevel EDAC sysfs dir\n");
-		return NULL;
-	}
-
-out:
-	atomic_inc(&edac_subsys_valid);
-	return &edac_subsys;
-}
-EXPORT_SYMBOL_GPL(edac_get_sysfs_subsys);
-
-void edac_put_sysfs_subsys(void)
-{
-	/* last user unregisters it */
-	if (atomic_dec_and_test(&edac_subsys_valid))
-		bus_unregister(&edac_subsys);
-}
-EXPORT_SYMBOL_GPL(edac_put_sysfs_subsys);
diff --git a/drivers/edac/i5100_edac.c b/drivers/edac/i5100_edac.c
index 40917775dca1..c655162caf08 100644
--- a/drivers/edac/i5100_edac.c
+++ b/drivers/edac/i5100_edac.c
@@ -575,9 +575,7 @@ static void i5100_check_error(struct mem_ctl_info *mci)
 
 static void i5100_refresh_scrubbing(struct work_struct *work)
 {
-	struct delayed_work *i5100_scrubbing = container_of(work,
-							    struct delayed_work,
-							    work);
+	struct delayed_work *i5100_scrubbing = to_delayed_work(work);
 	struct i5100_priv *priv = container_of(i5100_scrubbing,
 					       struct i5100_priv,
 					       i5100_scrubbing);
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index 23ef8e9f2c9a..b7139c160baf 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -20,6 +20,7 @@
 #include <linux/edac.h>
 #include <linux/smp.h>
 #include <linux/gfp.h>
+#include <linux/fsl/edac.h>
 
 #include <linux/of_platform.h>
 #include <linux/of_device.h>
@@ -238,10 +239,12 @@ static irqreturn_t mpc85xx_pci_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-int mpc85xx_pci_err_probe(struct platform_device *op)
+static int mpc85xx_pci_err_probe(struct platform_device *op)
 {
 	struct edac_pci_ctl_info *pci;
 	struct mpc85xx_pci_pdata *pdata;
+	struct mpc85xx_edac_pci_plat_data *plat_data;
+	struct device_node *of_node;
 	struct resource r;
 	int res = 0;
 
@@ -266,7 +269,15 @@ int mpc85xx_pci_err_probe(struct platform_device *op)
 	pdata->name = "mpc85xx_pci_err";
 	pdata->irq = NO_IRQ;
 
-	if (mpc85xx_pcie_find_capability(op->dev.of_node) > 0)
+	plat_data = op->dev.platform_data;
+	if (!plat_data) {
+		dev_err(&op->dev, "no platform data");
+		res = -ENXIO;
+		goto err;
+	}
+	of_node = plat_data->of_node;
+
+	if (mpc85xx_pcie_find_capability(of_node) > 0)
 		pdata->is_pcie = true;
 
 	dev_set_drvdata(&op->dev, pci);
@@ -284,7 +295,7 @@ int mpc85xx_pci_err_probe(struct platform_device *op)
 
 	pdata->edac_idx = edac_pci_idx++;
 
-	res = of_address_to_resource(op->dev.of_node, 0, &r);
+	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__);
@@ -339,7 +350,7 @@ int mpc85xx_pci_err_probe(struct platform_device *op)
 	}
 
 	if (edac_op_state == EDAC_OPSTATE_INT) {
-		pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
+		pdata->irq = irq_of_parse_and_map(of_node, 0);
 		res = devm_request_irq(&op->dev, pdata->irq,
 				       mpc85xx_pci_isr,
 				       IRQF_SHARED,
@@ -386,8 +397,22 @@ err:
 	devres_release_group(&op->dev, mpc85xx_pci_err_probe);
 	return res;
 }
-EXPORT_SYMBOL(mpc85xx_pci_err_probe);
 
+static const struct platform_device_id mpc85xx_pci_err_match[] = {
+	{
+		.name = "mpc85xx-pci-edac"
+	},
+	{}
+};
+
+static struct platform_driver mpc85xx_pci_err_driver = {
+	.probe = mpc85xx_pci_err_probe,
+	.id_table = mpc85xx_pci_err_match,
+	.driver = {
+		.name = "mpc85xx_pci_err",
+		.suppress_bind_attrs = true,
+	},
+};
 #endif				/* CONFIG_PCI */
 
 /**************************** L2 Err device ***************************/
@@ -1208,6 +1233,14 @@ static void __init mpc85xx_mc_clear_rfxe(void *data)
 }
 #endif
 
+static struct platform_driver * const drivers[] = {
+	&mpc85xx_mc_err_driver,
+	&mpc85xx_l2_err_driver,
+#ifdef CONFIG_PCI
+	&mpc85xx_pci_err_driver,
+#endif
+};
+
 static int __init mpc85xx_mc_init(void)
 {
 	int res = 0;
@@ -1226,13 +1259,9 @@ static int __init mpc85xx_mc_init(void)
 		break;
 	}
 
-	res = platform_driver_register(&mpc85xx_mc_err_driver);
-	if (res)
-		printk(KERN_WARNING EDAC_MOD_STR "MC fails to register\n");
-
-	res = platform_driver_register(&mpc85xx_l2_err_driver);
+	res = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 	if (res)
-		printk(KERN_WARNING EDAC_MOD_STR "L2 fails to register\n");
+		printk(KERN_WARNING EDAC_MOD_STR "drivers fail to register\n");
 
 #ifdef CONFIG_FSL_SOC_BOOKE
 	pvr = mfspr(SPRN_PVR);
@@ -1270,8 +1299,7 @@ static void __exit mpc85xx_mc_exit(void)
 		on_each_cpu(mpc85xx_mc_restore_hid1, NULL, 0);
 	}
 #endif
-	platform_driver_unregister(&mpc85xx_l2_err_driver);
-	platform_driver_unregister(&mpc85xx_mc_err_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_exit(mpc85xx_mc_exit);
diff --git a/drivers/edac/mv64x60_edac.c b/drivers/edac/mv64x60_edac.c
index 0574e1bbe45c..6c54127e6eae 100644
--- a/drivers/edac/mv64x60_edac.c
+++ b/drivers/edac/mv64x60_edac.c
@@ -847,6 +847,15 @@ static struct platform_driver mv64x60_mc_err_driver = {
 	}
 };
 
+static struct platform_driver * const drivers[] = {
+	&mv64x60_mc_err_driver,
+	&mv64x60_cpu_err_driver,
+	&mv64x60_sram_err_driver,
+#ifdef CONFIG_PCI
+	&mv64x60_pci_err_driver,
+#endif
+};
+
 static int __init mv64x60_edac_init(void)
 {
 	int ret = 0;
@@ -863,39 +872,13 @@ static int __init mv64x60_edac_init(void)
 		break;
 	}
 
-	ret = platform_driver_register(&mv64x60_mc_err_driver);
-	if (ret)
-		printk(KERN_WARNING EDAC_MOD_STR "MC err failed to register\n");
-
-	ret = platform_driver_register(&mv64x60_cpu_err_driver);
-	if (ret)
-		printk(KERN_WARNING EDAC_MOD_STR
-			"CPU err failed to register\n");
-
-	ret = platform_driver_register(&mv64x60_sram_err_driver);
-	if (ret)
-		printk(KERN_WARNING EDAC_MOD_STR
-			"SRAM err failed to register\n");
-
-#ifdef CONFIG_PCI
-	ret = platform_driver_register(&mv64x60_pci_err_driver);
-	if (ret)
-		printk(KERN_WARNING EDAC_MOD_STR
-			"PCI err failed to register\n");
-#endif
-
-	return ret;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_init(mv64x60_edac_init);
 
 static void __exit mv64x60_edac_exit(void)
 {
-#ifdef CONFIG_PCI
-	platform_driver_unregister(&mv64x60_pci_err_driver);
-#endif
-	platform_driver_unregister(&mv64x60_sram_err_driver);
-	platform_driver_unregister(&mv64x60_cpu_err_driver);
-	platform_driver_unregister(&mv64x60_mc_err_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(mv64x60_edac_exit);
 
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 429309c62699..f5c6b97c8958 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -65,15 +65,20 @@ static const u32 ibridge_dram_rule[] = {
 	0xd8, 0xe0, 0xe8, 0xf0, 0xf8,
 };
 
-#define SAD_LIMIT(reg)		((GET_BITFIELD(reg, 6, 25) << 26) | 0x3ffffff)
-#define DRAM_ATTR(reg)		GET_BITFIELD(reg, 2,  3)
-#define INTERLEAVE_MODE(reg)	GET_BITFIELD(reg, 1,  1)
+static const u32 knl_dram_rule[] = {
+	0x60, 0x68, 0x70, 0x78, 0x80, /* 0-4 */
+	0x88, 0x90, 0x98, 0xa0, 0xa8, /* 5-9 */
+	0xb0, 0xb8, 0xc0, 0xc8, 0xd0, /* 10-14 */
+	0xd8, 0xe0, 0xe8, 0xf0, 0xf8, /* 15-19 */
+	0x100, 0x108, 0x110, 0x118,   /* 20-23 */
+};
+
 #define DRAM_RULE_ENABLE(reg)	GET_BITFIELD(reg, 0,  0)
 #define A7MODE(reg)		GET_BITFIELD(reg, 26, 26)
 
-static char *get_dram_attr(u32 reg)
+static char *show_dram_attr(u32 attr)
 {
-	switch(DRAM_ATTR(reg)) {
+	switch (attr) {
 		case 0:
 			return "DRAM";
 		case 1:
@@ -97,6 +102,14 @@ static const u32 ibridge_interleave_list[] = {
 	0xdc, 0xe4, 0xec, 0xf4, 0xfc,
 };
 
+static const u32 knl_interleave_list[] = {
+	0x64, 0x6c, 0x74, 0x7c, 0x84, /* 0-4 */
+	0x8c, 0x94, 0x9c, 0xa4, 0xac, /* 5-9 */
+	0xb4, 0xbc, 0xc4, 0xcc, 0xd4, /* 10-14 */
+	0xdc, 0xe4, 0xec, 0xf4, 0xfc, /* 15-19 */
+	0x104, 0x10c, 0x114, 0x11c,   /* 20-23 */
+};
+
 struct interleave_pkg {
 	unsigned char start;
 	unsigned char end;
@@ -134,10 +147,13 @@ static inline int sad_pkg(const struct interleave_pkg *table, u32 reg,
 /* Devices 12 Function 7 */
 
 #define TOLM		0x80
-#define	TOHM		0x84
+#define TOHM		0x84
 #define HASWELL_TOLM	0xd0
 #define HASWELL_TOHM_0	0xd4
 #define HASWELL_TOHM_1	0xd8
+#define KNL_TOLM	0xd0
+#define KNL_TOHM_0	0xd4
+#define KNL_TOHM_1	0xd8
 
 #define GET_TOLM(reg)		((GET_BITFIELD(reg, 0,  3) << 28) | 0x3ffffff)
 #define GET_TOHM(reg)		((GET_BITFIELD(reg, 0, 20) << 25) | 0x3ffffff)
@@ -148,6 +164,8 @@ static inline int sad_pkg(const struct interleave_pkg *table, u32 reg,
 
 #define SOURCE_ID(reg)		GET_BITFIELD(reg, 9, 11)
 
+#define SOURCE_ID_KNL(reg)	GET_BITFIELD(reg, 12, 14)
+
 #define SAD_CONTROL	0xf4
 
 /* Device 14 function 0 */
@@ -170,6 +188,7 @@ static const u32 tad_dram_rule[] = {
 /* Device 15, function 0 */
 
 #define MCMTR			0x7c
+#define KNL_MCMTR		0x624
 
 #define IS_ECC_ENABLED(mcmtr)		GET_BITFIELD(mcmtr, 2, 2)
 #define IS_LOCKSTEP_ENABLED(mcmtr)	GET_BITFIELD(mcmtr, 1, 1)
@@ -186,6 +205,8 @@ static const int mtr_regs[] = {
 	0x80, 0x84, 0x88,
 };
 
+static const int knl_mtr_reg = 0xb60;
+
 #define RANK_DISABLE(mtr)		GET_BITFIELD(mtr, 16, 19)
 #define IS_DIMM_PRESENT(mtr)		GET_BITFIELD(mtr, 14, 14)
 #define RANK_CNT_BITS(mtr)		GET_BITFIELD(mtr, 12, 13)
@@ -256,6 +277,9 @@ static const u32 correrrthrsld[] = {
 
 #define NUM_CHANNELS		8	/* 2MC per socket, four chan per MC */
 #define MAX_DIMMS		3	/* Max DIMMS per channel */
+#define KNL_MAX_CHAS		38	/* KNL max num. of Cache Home Agents */
+#define KNL_MAX_CHANNELS	6	/* KNL max num. of PCI channels */
+#define KNL_MAX_EDCS		8	/* Embedded DRAM controllers */
 #define CHANNEL_UNSPECIFIED	0xf	/* Intel IA32 SDM 15-14 */
 
 enum type {
@@ -263,6 +287,7 @@ enum type {
 	IVY_BRIDGE,
 	HASWELL,
 	BROADWELL,
+	KNIGHTS_LANDING,
 };
 
 struct sbridge_pvt;
@@ -273,6 +298,10 @@ struct sbridge_info {
 	u64		(*get_tolm)(struct sbridge_pvt *pvt);
 	u64		(*get_tohm)(struct sbridge_pvt *pvt);
 	u64		(*rir_limit)(u32 reg);
+	u64		(*sad_limit)(u32 reg);
+	u32		(*interleave_mode)(u32 reg);
+	char*		(*show_interleave_mode)(u32 reg);
+	u32		(*dram_attr)(u32 reg);
 	const u32	*dram_rule;
 	const u32	*interleave_list;
 	const struct interleave_pkg *interleave_pkg;
@@ -308,6 +337,16 @@ struct sbridge_dev {
 	struct mem_ctl_info	*mci;
 };
 
+struct knl_pvt {
+	struct pci_dev          *pci_cha[KNL_MAX_CHAS];
+	struct pci_dev          *pci_channel[KNL_MAX_CHANNELS];
+	struct pci_dev          *pci_mc0;
+	struct pci_dev          *pci_mc1;
+	struct pci_dev          *pci_mc0_misc;
+	struct pci_dev          *pci_mc1_misc;
+	struct pci_dev          *pci_mc_info; /* tolm, tohm */
+};
+
 struct sbridge_pvt {
 	struct pci_dev		*pci_ta, *pci_ddrio, *pci_ras;
 	struct pci_dev		*pci_sad0, *pci_sad1;
@@ -336,6 +375,7 @@ struct sbridge_pvt {
 
 	/* Memory description */
 	u64			tolm, tohm;
+	struct knl_pvt knl;
 };
 
 #define PCI_DESCR(device_id, opt)	\
@@ -509,6 +549,50 @@ static const struct pci_id_table pci_dev_descr_haswell_table[] = {
 	{0,}			/* 0 terminated list. */
 };
 
+/* Knight's Landing Support */
+/*
+ * KNL's memory channels are swizzled between memory controllers.
+ * MC0 is mapped to CH3,5,6 and MC1 is mapped to CH0,1,2
+ */
+#define knl_channel_remap(channel) ((channel + 3) % 6)
+
+/* Memory controller, TAD tables, error injection - 2-8-0, 2-9-0 (2 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_MC       0x7840
+/* DRAM channel stuff; bank addrs, dimmmtr, etc.. 2-8-2 - 2-9-4 (6 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL  0x7843
+/* kdrwdbu TAD limits/offsets, MCMTR - 2-10-1, 2-11-1 (2 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_TA       0x7844
+/* CHA broadcast registers, dram rules - 1-29-0 (1 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0     0x782a
+/* SAD target - 1-29-1 (1 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_SAD1     0x782b
+/* Caching / Home Agent */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_CHA      0x782c
+/* Device with TOLM and TOHM, 0-5-0 (1 of these) */
+#define PCI_DEVICE_ID_INTEL_KNL_IMC_TOLHM    0x7810
+
+/*
+ * KNL differs from SB, IB, and Haswell in that it has multiple
+ * instances of the same device with the same device ID, so we handle that
+ * by creating as many copies in the table as we expect to find.
+ * (Like device ID must be grouped together.)
+ */
+
+static const struct pci_id_descr pci_dev_descr_knl[] = {
+	[0]         = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0, 0) },
+	[1]         = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_SAD1, 0) },
+	[2 ... 3]   = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_MC, 0)},
+	[4 ... 41]  = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHA, 0) },
+	[42 ... 47] = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL, 0) },
+	[48]        = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TA, 0) },
+	[49]        = { PCI_DESCR(PCI_DEVICE_ID_INTEL_KNL_IMC_TOLHM, 0) },
+};
+
+static const struct pci_id_table pci_dev_descr_knl_table[] = {
+	PCI_ID_TABLE_ENTRY(pci_dev_descr_knl),
+	{0,}
+};
+
 /*
  * Broadwell support
  *
@@ -585,6 +669,7 @@ static const struct pci_device_id sbridge_pci_tbl[] = {
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0)},
 	{0,}			/* 0 terminated list. */
 };
 
@@ -598,7 +683,7 @@ static inline int numrank(enum type type, u32 mtr)
 	int ranks = (1 << RANK_CNT_BITS(mtr));
 	int max = 4;
 
-	if (type == HASWELL || type == BROADWELL)
+	if (type == HASWELL || type == BROADWELL || type == KNIGHTS_LANDING)
 		max = 8;
 
 	if (ranks > max) {
@@ -636,10 +721,19 @@ static inline int numcol(u32 mtr)
 	return 1 << cols;
 }
 
-static struct sbridge_dev *get_sbridge_dev(u8 bus)
+static struct sbridge_dev *get_sbridge_dev(u8 bus, int multi_bus)
 {
 	struct sbridge_dev *sbridge_dev;
 
+	/*
+	 * If we have devices scattered across several busses that pertain
+	 * to the same memory controller, we'll lump them all together.
+	 */
+	if (multi_bus) {
+		return list_first_entry_or_null(&sbridge_edac_list,
+				struct sbridge_dev, list);
+	}
+
 	list_for_each_entry(sbridge_dev, &sbridge_edac_list, list) {
 		if (sbridge_dev->bus == bus)
 			return sbridge_dev;
@@ -718,6 +812,67 @@ static u64 rir_limit(u32 reg)
 	return ((u64)GET_BITFIELD(reg,  1, 10) << 29) | 0x1fffffff;
 }
 
+static u64 sad_limit(u32 reg)
+{
+	return (GET_BITFIELD(reg, 6, 25) << 26) | 0x3ffffff;
+}
+
+static u32 interleave_mode(u32 reg)
+{
+	return GET_BITFIELD(reg, 1, 1);
+}
+
+char *show_interleave_mode(u32 reg)
+{
+	return interleave_mode(reg) ? "8:6" : "[8:6]XOR[18:16]";
+}
+
+static u32 dram_attr(u32 reg)
+{
+	return GET_BITFIELD(reg, 2, 3);
+}
+
+static u64 knl_sad_limit(u32 reg)
+{
+	return (GET_BITFIELD(reg, 7, 26) << 26) | 0x3ffffff;
+}
+
+static u32 knl_interleave_mode(u32 reg)
+{
+	return GET_BITFIELD(reg, 1, 2);
+}
+
+static char *knl_show_interleave_mode(u32 reg)
+{
+	char *s;
+
+	switch (knl_interleave_mode(reg)) {
+	case 0:
+		s = "use address bits [8:6]";
+		break;
+	case 1:
+		s = "use address bits [10:8]";
+		break;
+	case 2:
+		s = "use address bits [14:12]";
+		break;
+	case 3:
+		s = "use address bits [32:30]";
+		break;
+	default:
+		WARN_ON(1);
+		break;
+	}
+
+	return s;
+}
+
+static u32 dram_attr_knl(u32 reg)
+{
+	return GET_BITFIELD(reg, 3, 4);
+}
+
+
 static enum mem_type get_memory_type(struct sbridge_pvt *pvt)
 {
 	u32 reg;
@@ -769,6 +924,12 @@ out:
 	return mtype;
 }
 
+static enum dev_type knl_get_width(struct sbridge_pvt *pvt, u32 mtr)
+{
+	/* for KNL value is fixed */
+	return DEV_X16;
+}
+
 static enum dev_type sbridge_get_width(struct sbridge_pvt *pvt, u32 mtr)
 {
 	/* there's no way to figure out */
@@ -812,6 +973,12 @@ static enum dev_type broadwell_get_width(struct sbridge_pvt *pvt, u32 mtr)
 	return __ibridge_get_width(GET_BITFIELD(mtr, 8, 9));
 }
 
+static enum mem_type knl_get_memory_type(struct sbridge_pvt *pvt)
+{
+	/* DDR4 RDIMMS and LRDIMMS are supported */
+	return MEM_RDDR4;
+}
+
 static u8 get_node_id(struct sbridge_pvt *pvt)
 {
 	u32 reg;
@@ -827,6 +994,15 @@ static u8 haswell_get_node_id(struct sbridge_pvt *pvt)
 	return GET_BITFIELD(reg, 0, 3);
 }
 
+static u8 knl_get_node_id(struct sbridge_pvt *pvt)
+{
+	u32 reg;
+
+	pci_read_config_dword(pvt->pci_sad1, SAD_CONTROL, &reg);
+	return GET_BITFIELD(reg, 0, 2);
+}
+
+
 static u64 haswell_get_tolm(struct sbridge_pvt *pvt)
 {
 	u32 reg;
@@ -848,6 +1024,26 @@ static u64 haswell_get_tohm(struct sbridge_pvt *pvt)
 	return rc | 0x1ffffff;
 }
 
+static u64 knl_get_tolm(struct sbridge_pvt *pvt)
+{
+	u32 reg;
+
+	pci_read_config_dword(pvt->knl.pci_mc_info, KNL_TOLM, &reg);
+	return (GET_BITFIELD(reg, 26, 31) << 26) | 0x3ffffff;
+}
+
+static u64 knl_get_tohm(struct sbridge_pvt *pvt)
+{
+	u64 rc;
+	u32 reg_lo, reg_hi;
+
+	pci_read_config_dword(pvt->knl.pci_mc_info, KNL_TOHM_0, &reg_lo);
+	pci_read_config_dword(pvt->knl.pci_mc_info, KNL_TOHM_1, &reg_hi);
+	rc = ((u64)reg_hi << 32) | reg_lo;
+	return rc | 0x3ffffff;
+}
+
+
 static u64 haswell_rir_limit(u32 reg)
 {
 	return (((u64)GET_BITFIELD(reg,  1, 11) + 1) << 29) - 1;
@@ -905,11 +1101,22 @@ static int check_if_ecc_is_active(const u8 bus, enum type type)
 	case BROADWELL:
 		id = PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0_TA;
 		break;
+	case KNIGHTS_LANDING:
+		/*
+		 * KNL doesn't group things by bus the same way
+		 * SB/IB/Haswell does.
+		 */
+		id = PCI_DEVICE_ID_INTEL_KNL_IMC_TA;
+		break;
 	default:
 		return -ENODEV;
 	}
 
-	pdev = get_pdev_same_bus(bus, id);
+	if (type != KNIGHTS_LANDING)
+		pdev = get_pdev_same_bus(bus, id);
+	else
+		pdev = pci_get_device(PCI_VENDOR_ID_INTEL, id, 0);
+
 	if (!pdev) {
 		sbridge_printk(KERN_ERR, "Couldn't find PCI device "
 					"%04x:%04x! on bus %02d\n",
@@ -917,7 +1124,8 @@ static int check_if_ecc_is_active(const u8 bus, enum type type)
 		return -ENODEV;
 	}
 
-	pci_read_config_dword(pdev, MCMTR, &mcmtr);
+	pci_read_config_dword(pdev,
+			type == KNIGHTS_LANDING ? KNL_MCMTR : MCMTR, &mcmtr);
 	if (!IS_ECC_ENABLED(mcmtr)) {
 		sbridge_printk(KERN_ERR, "ECC is disabled. Aborting\n");
 		return -ENODEV;
@@ -925,6 +1133,476 @@ static int check_if_ecc_is_active(const u8 bus, enum type type)
 	return 0;
 }
 
+/* Low bits of TAD limit, and some metadata. */
+static const u32 knl_tad_dram_limit_lo[] = {
+	0x400, 0x500, 0x600, 0x700,
+	0x800, 0x900, 0xa00, 0xb00,
+};
+
+/* Low bits of TAD offset. */
+static const u32 knl_tad_dram_offset_lo[] = {
+	0x404, 0x504, 0x604, 0x704,
+	0x804, 0x904, 0xa04, 0xb04,
+};
+
+/* High 16 bits of TAD limit and offset. */
+static const u32 knl_tad_dram_hi[] = {
+	0x408, 0x508, 0x608, 0x708,
+	0x808, 0x908, 0xa08, 0xb08,
+};
+
+/* Number of ways a tad entry is interleaved. */
+static const u32 knl_tad_ways[] = {
+	8, 6, 4, 3, 2, 1,
+};
+
+/*
+ * Retrieve the n'th Target Address Decode table entry
+ * from the memory controller's TAD table.
+ *
+ * @pvt:	driver private data
+ * @entry:	which entry you want to retrieve
+ * @mc:		which memory controller (0 or 1)
+ * @offset:	output tad range offset
+ * @limit:	output address of first byte above tad range
+ * @ways:	output number of interleave ways
+ *
+ * The offset value has curious semantics.  It's a sort of running total
+ * of the sizes of all the memory regions that aren't mapped in this
+ * tad table.
+ */
+static int knl_get_tad(const struct sbridge_pvt *pvt,
+		const int entry,
+		const int mc,
+		u64 *offset,
+		u64 *limit,
+		int *ways)
+{
+	u32 reg_limit_lo, reg_offset_lo, reg_hi;
+	struct pci_dev *pci_mc;
+	int way_id;
+
+	switch (mc) {
+	case 0:
+		pci_mc = pvt->knl.pci_mc0;
+		break;
+	case 1:
+		pci_mc = pvt->knl.pci_mc1;
+		break;
+	default:
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	pci_read_config_dword(pci_mc,
+			knl_tad_dram_limit_lo[entry], &reg_limit_lo);
+	pci_read_config_dword(pci_mc,
+			knl_tad_dram_offset_lo[entry], &reg_offset_lo);
+	pci_read_config_dword(pci_mc,
+			knl_tad_dram_hi[entry], &reg_hi);
+
+	/* Is this TAD entry enabled? */
+	if (!GET_BITFIELD(reg_limit_lo, 0, 0))
+		return -ENODEV;
+
+	way_id = GET_BITFIELD(reg_limit_lo, 3, 5);
+
+	if (way_id < ARRAY_SIZE(knl_tad_ways)) {
+		*ways = knl_tad_ways[way_id];
+	} else {
+		*ways = 0;
+		sbridge_printk(KERN_ERR,
+				"Unexpected value %d in mc_tad_limit_lo wayness field\n",
+				way_id);
+		return -ENODEV;
+	}
+
+	/*
+	 * The least significant 6 bits of base and limit are truncated.
+	 * For limit, we fill the missing bits with 1s.
+	 */
+	*offset = ((u64) GET_BITFIELD(reg_offset_lo, 6, 31) << 6) |
+				((u64) GET_BITFIELD(reg_hi, 0,  15) << 32);
+	*limit = ((u64) GET_BITFIELD(reg_limit_lo,  6, 31) << 6) | 63 |
+				((u64) GET_BITFIELD(reg_hi, 16, 31) << 32);
+
+	return 0;
+}
+
+/* Determine which memory controller is responsible for a given channel. */
+static int knl_channel_mc(int channel)
+{
+	WARN_ON(channel < 0 || channel >= 6);
+
+	return channel < 3 ? 1 : 0;
+}
+
+/*
+ * Get the Nth entry from EDC_ROUTE_TABLE register.
+ * (This is the per-tile mapping of logical interleave targets to
+ *  physical EDC modules.)
+ *
+ * entry 0: 0:2
+ *       1: 3:5
+ *       2: 6:8
+ *       3: 9:11
+ *       4: 12:14
+ *       5: 15:17
+ *       6: 18:20
+ *       7: 21:23
+ * reserved: 24:31
+ */
+static u32 knl_get_edc_route(int entry, u32 reg)
+{
+	WARN_ON(entry >= KNL_MAX_EDCS);
+	return GET_BITFIELD(reg, entry*3, (entry*3)+2);
+}
+
+/*
+ * Get the Nth entry from MC_ROUTE_TABLE register.
+ * (This is the per-tile mapping of logical interleave targets to
+ *  physical DRAM channels modules.)
+ *
+ * entry 0: mc 0:2   channel 18:19
+ *       1: mc 3:5   channel 20:21
+ *       2: mc 6:8   channel 22:23
+ *       3: mc 9:11  channel 24:25
+ *       4: mc 12:14 channel 26:27
+ *       5: mc 15:17 channel 28:29
+ * reserved: 30:31
+ *
+ * Though we have 3 bits to identify the MC, we should only see
+ * the values 0 or 1.
+ */
+
+static u32 knl_get_mc_route(int entry, u32 reg)
+{
+	int mc, chan;
+
+	WARN_ON(entry >= KNL_MAX_CHANNELS);
+
+	mc = GET_BITFIELD(reg, entry*3, (entry*3)+2);
+	chan = GET_BITFIELD(reg, (entry*2) + 18, (entry*2) + 18 + 1);
+
+	return knl_channel_remap(mc*3 + chan);
+}
+
+/*
+ * Render the EDC_ROUTE register in human-readable form.
+ * Output string s should be at least KNL_MAX_EDCS*2 bytes.
+ */
+static void knl_show_edc_route(u32 reg, char *s)
+{
+	int i;
+
+	for (i = 0; i < KNL_MAX_EDCS; i++) {
+		s[i*2] = knl_get_edc_route(i, reg) + '0';
+		s[i*2+1] = '-';
+	}
+
+	s[KNL_MAX_EDCS*2 - 1] = '\0';
+}
+
+/*
+ * Render the MC_ROUTE register in human-readable form.
+ * Output string s should be at least KNL_MAX_CHANNELS*2 bytes.
+ */
+static void knl_show_mc_route(u32 reg, char *s)
+{
+	int i;
+
+	for (i = 0; i < KNL_MAX_CHANNELS; i++) {
+		s[i*2] = knl_get_mc_route(i, reg) + '0';
+		s[i*2+1] = '-';
+	}
+
+	s[KNL_MAX_CHANNELS*2 - 1] = '\0';
+}
+
+#define KNL_EDC_ROUTE 0xb8
+#define KNL_MC_ROUTE 0xb4
+
+/* Is this dram rule backed by regular DRAM in flat mode? */
+#define KNL_EDRAM(reg) GET_BITFIELD(reg, 29, 29)
+
+/* Is this dram rule cached? */
+#define KNL_CACHEABLE(reg) GET_BITFIELD(reg, 28, 28)
+
+/* Is this rule backed by edc ? */
+#define KNL_EDRAM_ONLY(reg) GET_BITFIELD(reg, 29, 29)
+
+/* Is this rule backed by DRAM, cacheable in EDRAM? */
+#define KNL_CACHEABLE(reg) GET_BITFIELD(reg, 28, 28)
+
+/* Is this rule mod3? */
+#define KNL_MOD3(reg) GET_BITFIELD(reg, 27, 27)
+
+/*
+ * Figure out how big our RAM modules are.
+ *
+ * The DIMMMTR register in KNL doesn't tell us the size of the DIMMs, so we
+ * have to figure this out from the SAD rules, interleave lists, route tables,
+ * and TAD rules.
+ *
+ * SAD rules can have holes in them (e.g. the 3G-4G hole), so we have to
+ * inspect the TAD rules to figure out how large the SAD regions really are.
+ *
+ * When we know the real size of a SAD region and how many ways it's
+ * interleaved, we know the individual contribution of each channel to
+ * TAD is size/ways.
+ *
+ * Finally, we have to check whether each channel participates in each SAD
+ * region.
+ *
+ * Fortunately, KNL only supports one DIMM per channel, so once we know how
+ * much memory the channel uses, we know the DIMM is at least that large.
+ * (The BIOS might possibly choose not to map all available memory, in which
+ * case we will underreport the size of the DIMM.)
+ *
+ * In theory, we could try to determine the EDC sizes as well, but that would
+ * only work in flat mode, not in cache mode.
+ *
+ * @mc_sizes: Output sizes of channels (must have space for KNL_MAX_CHANNELS
+ *            elements)
+ */
+static int knl_get_dimm_capacity(struct sbridge_pvt *pvt, u64 *mc_sizes)
+{
+	u64 sad_base, sad_size, sad_limit = 0;
+	u64 tad_base, tad_size, tad_limit, tad_deadspace, tad_livespace;
+	int sad_rule = 0;
+	int tad_rule = 0;
+	int intrlv_ways, tad_ways;
+	u32 first_pkg, pkg;
+	int i;
+	u64 sad_actual_size[2]; /* sad size accounting for holes, per mc */
+	u32 dram_rule, interleave_reg;
+	u32 mc_route_reg[KNL_MAX_CHAS];
+	u32 edc_route_reg[KNL_MAX_CHAS];
+	int edram_only;
+	char edc_route_string[KNL_MAX_EDCS*2];
+	char mc_route_string[KNL_MAX_CHANNELS*2];
+	int cur_reg_start;
+	int mc;
+	int channel;
+	int way;
+	int participants[KNL_MAX_CHANNELS];
+	int participant_count = 0;
+
+	for (i = 0; i < KNL_MAX_CHANNELS; i++)
+		mc_sizes[i] = 0;
+
+	/* Read the EDC route table in each CHA. */
+	cur_reg_start = 0;
+	for (i = 0; i < KNL_MAX_CHAS; i++) {
+		pci_read_config_dword(pvt->knl.pci_cha[i],
+				KNL_EDC_ROUTE, &edc_route_reg[i]);
+
+		if (i > 0 && edc_route_reg[i] != edc_route_reg[i-1]) {
+			knl_show_edc_route(edc_route_reg[i-1],
+					edc_route_string);
+			if (cur_reg_start == i-1)
+				edac_dbg(0, "edc route table for CHA %d: %s\n",
+					cur_reg_start, edc_route_string);
+			else
+				edac_dbg(0, "edc route table for CHA %d-%d: %s\n",
+					cur_reg_start, i-1, edc_route_string);
+			cur_reg_start = i;
+		}
+	}
+	knl_show_edc_route(edc_route_reg[i-1], edc_route_string);
+	if (cur_reg_start == i-1)
+		edac_dbg(0, "edc route table for CHA %d: %s\n",
+			cur_reg_start, edc_route_string);
+	else
+		edac_dbg(0, "edc route table for CHA %d-%d: %s\n",
+			cur_reg_start, i-1, edc_route_string);
+
+	/* Read the MC route table in each CHA. */
+	cur_reg_start = 0;
+	for (i = 0; i < KNL_MAX_CHAS; i++) {
+		pci_read_config_dword(pvt->knl.pci_cha[i],
+			KNL_MC_ROUTE, &mc_route_reg[i]);
+
+		if (i > 0 && mc_route_reg[i] != mc_route_reg[i-1]) {
+			knl_show_mc_route(mc_route_reg[i-1], mc_route_string);
+			if (cur_reg_start == i-1)
+				edac_dbg(0, "mc route table for CHA %d: %s\n",
+					cur_reg_start, mc_route_string);
+			else
+				edac_dbg(0, "mc route table for CHA %d-%d: %s\n",
+					cur_reg_start, i-1, mc_route_string);
+			cur_reg_start = i;
+		}
+	}
+	knl_show_mc_route(mc_route_reg[i-1], mc_route_string);
+	if (cur_reg_start == i-1)
+		edac_dbg(0, "mc route table for CHA %d: %s\n",
+			cur_reg_start, mc_route_string);
+	else
+		edac_dbg(0, "mc route table for CHA %d-%d: %s\n",
+			cur_reg_start, i-1, mc_route_string);
+
+	/* Process DRAM rules */
+	for (sad_rule = 0; sad_rule < pvt->info.max_sad; sad_rule++) {
+		/* previous limit becomes the new base */
+		sad_base = sad_limit;
+
+		pci_read_config_dword(pvt->pci_sad0,
+			pvt->info.dram_rule[sad_rule], &dram_rule);
+
+		if (!DRAM_RULE_ENABLE(dram_rule))
+			break;
+
+		edram_only = KNL_EDRAM_ONLY(dram_rule);
+
+		sad_limit = pvt->info.sad_limit(dram_rule)+1;
+		sad_size = sad_limit - sad_base;
+
+		pci_read_config_dword(pvt->pci_sad0,
+			pvt->info.interleave_list[sad_rule], &interleave_reg);
+
+		/*
+		 * Find out how many ways this dram rule is interleaved.
+		 * We stop when we see the first channel again.
+		 */
+		first_pkg = sad_pkg(pvt->info.interleave_pkg,
+						interleave_reg, 0);
+		for (intrlv_ways = 1; intrlv_ways < 8; intrlv_ways++) {
+			pkg = sad_pkg(pvt->info.interleave_pkg,
+						interleave_reg, intrlv_ways);
+
+			if ((pkg & 0x8) == 0) {
+				/*
+				 * 0 bit means memory is non-local,
+				 * which KNL doesn't support
+				 */
+				edac_dbg(0, "Unexpected interleave target %d\n",
+					pkg);
+				return -1;
+			}
+
+			if (pkg == first_pkg)
+				break;
+		}
+		if (KNL_MOD3(dram_rule))
+			intrlv_ways *= 3;
+
+		edac_dbg(3, "dram rule %d (base 0x%llx, limit 0x%llx), %d way interleave%s\n",
+			sad_rule,
+			sad_base,
+			sad_limit,
+			intrlv_ways,
+			edram_only ? ", EDRAM" : "");
+
+		/*
+		 * Find out how big the SAD region really is by iterating
+		 * over TAD tables (SAD regions may contain holes).
+		 * Each memory controller might have a different TAD table, so
+		 * we have to look at both.
+		 *
+		 * Livespace is the memory that's mapped in this TAD table,
+		 * deadspace is the holes (this could be the MMIO hole, or it
+		 * could be memory that's mapped by the other TAD table but
+		 * not this one).
+		 */
+		for (mc = 0; mc < 2; mc++) {
+			sad_actual_size[mc] = 0;
+			tad_livespace = 0;
+			for (tad_rule = 0;
+					tad_rule < ARRAY_SIZE(
+						knl_tad_dram_limit_lo);
+					tad_rule++) {
+				if (knl_get_tad(pvt,
+						tad_rule,
+						mc,
+						&tad_deadspace,
+						&tad_limit,
+						&tad_ways))
+					break;
+
+				tad_size = (tad_limit+1) -
+					(tad_livespace + tad_deadspace);
+				tad_livespace += tad_size;
+				tad_base = (tad_limit+1) - tad_size;
+
+				if (tad_base < sad_base) {
+					if (tad_limit > sad_base)
+						edac_dbg(0, "TAD region overlaps lower SAD boundary -- TAD tables may be configured incorrectly.\n");
+				} else if (tad_base < sad_limit) {
+					if (tad_limit+1 > sad_limit) {
+						edac_dbg(0, "TAD region overlaps upper SAD boundary -- TAD tables may be configured incorrectly.\n");
+					} else {
+						/* TAD region is completely inside SAD region */
+						edac_dbg(3, "TAD region %d 0x%llx - 0x%llx (%lld bytes) table%d\n",
+							tad_rule, tad_base,
+							tad_limit, tad_size,
+							mc);
+						sad_actual_size[mc] += tad_size;
+					}
+				}
+				tad_base = tad_limit+1;
+			}
+		}
+
+		for (mc = 0; mc < 2; mc++) {
+			edac_dbg(3, " total TAD DRAM footprint in table%d : 0x%llx (%lld bytes)\n",
+				mc, sad_actual_size[mc], sad_actual_size[mc]);
+		}
+
+		/* Ignore EDRAM rule */
+		if (edram_only)
+			continue;
+
+		/* Figure out which channels participate in interleave. */
+		for (channel = 0; channel < KNL_MAX_CHANNELS; channel++)
+			participants[channel] = 0;
+
+		/* For each channel, does at least one CHA have
+		 * this channel mapped to the given target?
+		 */
+		for (channel = 0; channel < KNL_MAX_CHANNELS; channel++) {
+			for (way = 0; way < intrlv_ways; way++) {
+				int target;
+				int cha;
+
+				if (KNL_MOD3(dram_rule))
+					target = way;
+				else
+					target = 0x7 & sad_pkg(
+				pvt->info.interleave_pkg, interleave_reg, way);
+
+				for (cha = 0; cha < KNL_MAX_CHAS; cha++) {
+					if (knl_get_mc_route(target,
+						mc_route_reg[cha]) == channel
+						&& !participants[channel]) {
+						participant_count++;
+						participants[channel] = 1;
+						break;
+					}
+				}
+			}
+		}
+
+		if (participant_count != intrlv_ways)
+			edac_dbg(0, "participant_count (%d) != interleave_ways (%d): DIMM size may be incorrect\n",
+				participant_count, intrlv_ways);
+
+		for (channel = 0; channel < KNL_MAX_CHANNELS; channel++) {
+			mc = knl_channel_mc(channel);
+			if (participants[channel]) {
+				edac_dbg(4, "mc channel %d contributes %lld bytes via sad entry %d\n",
+					channel,
+					sad_actual_size[mc]/intrlv_ways,
+					sad_rule);
+				mc_sizes[channel] +=
+					sad_actual_size[mc]/intrlv_ways;
+			}
+		}
+	}
+
+	return 0;
+}
+
 static int get_dimm_config(struct mem_ctl_info *mci)
 {
 	struct sbridge_pvt *pvt = mci->pvt_info;
@@ -934,13 +1612,20 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 	u32 reg;
 	enum edac_type mode;
 	enum mem_type mtype;
+	int channels = pvt->info.type == KNIGHTS_LANDING ?
+		KNL_MAX_CHANNELS : NUM_CHANNELS;
+	u64 knl_mc_sizes[KNL_MAX_CHANNELS];
 
-	if (pvt->info.type == HASWELL || pvt->info.type == BROADWELL)
+	if (pvt->info.type == HASWELL || pvt->info.type == BROADWELL ||
+			pvt->info.type == KNIGHTS_LANDING)
 		pci_read_config_dword(pvt->pci_sad1, SAD_TARGET, &reg);
 	else
 		pci_read_config_dword(pvt->pci_br0, SAD_TARGET, &reg);
 
-	pvt->sbridge_dev->source_id = SOURCE_ID(reg);
+	if (pvt->info.type == KNIGHTS_LANDING)
+		pvt->sbridge_dev->source_id = SOURCE_ID_KNL(reg);
+	else
+		pvt->sbridge_dev->source_id = SOURCE_ID(reg);
 
 	pvt->sbridge_dev->node_id = pvt->info.get_node_id(pvt);
 	edac_dbg(0, "mc#%d: Node ID: %d, source ID: %d\n",
@@ -948,31 +1633,42 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 		 pvt->sbridge_dev->node_id,
 		 pvt->sbridge_dev->source_id);
 
-	pci_read_config_dword(pvt->pci_ras, RASENABLES, &reg);
-	if (IS_MIRROR_ENABLED(reg)) {
-		edac_dbg(0, "Memory mirror is enabled\n");
-		pvt->is_mirrored = true;
-	} else {
-		edac_dbg(0, "Memory mirror is disabled\n");
+	/* KNL doesn't support mirroring or lockstep,
+	 * and is always closed page
+	 */
+	if (pvt->info.type == KNIGHTS_LANDING) {
+		mode = EDAC_S4ECD4ED;
 		pvt->is_mirrored = false;
-	}
 
-	pci_read_config_dword(pvt->pci_ta, MCMTR, &pvt->info.mcmtr);
-	if (IS_LOCKSTEP_ENABLED(pvt->info.mcmtr)) {
-		edac_dbg(0, "Lockstep is enabled\n");
-		mode = EDAC_S8ECD8ED;
-		pvt->is_lockstep = true;
+		if (knl_get_dimm_capacity(pvt, knl_mc_sizes) != 0)
+			return -1;
 	} else {
-		edac_dbg(0, "Lockstep is disabled\n");
-		mode = EDAC_S4ECD4ED;
-		pvt->is_lockstep = false;
-	}
-	if (IS_CLOSE_PG(pvt->info.mcmtr)) {
-		edac_dbg(0, "address map is on closed page mode\n");
-		pvt->is_close_pg = true;
-	} else {
-		edac_dbg(0, "address map is on open page mode\n");
-		pvt->is_close_pg = false;
+		pci_read_config_dword(pvt->pci_ras, RASENABLES, &reg);
+		if (IS_MIRROR_ENABLED(reg)) {
+			edac_dbg(0, "Memory mirror is enabled\n");
+			pvt->is_mirrored = true;
+		} else {
+			edac_dbg(0, "Memory mirror is disabled\n");
+			pvt->is_mirrored = false;
+		}
+
+		pci_read_config_dword(pvt->pci_ta, MCMTR, &pvt->info.mcmtr);
+		if (IS_LOCKSTEP_ENABLED(pvt->info.mcmtr)) {
+			edac_dbg(0, "Lockstep is enabled\n");
+			mode = EDAC_S8ECD8ED;
+			pvt->is_lockstep = true;
+		} else {
+			edac_dbg(0, "Lockstep is disabled\n");
+			mode = EDAC_S4ECD4ED;
+			pvt->is_lockstep = false;
+		}
+		if (IS_CLOSE_PG(pvt->info.mcmtr)) {
+			edac_dbg(0, "address map is on closed page mode\n");
+			pvt->is_close_pg = true;
+		} else {
+			edac_dbg(0, "address map is on open page mode\n");
+			pvt->is_close_pg = false;
+		}
 	}
 
 	mtype = pvt->info.get_memory_type(pvt);
@@ -988,23 +1684,46 @@ static int get_dimm_config(struct mem_ctl_info *mci)
 	else
 		banks = 8;
 
-	for (i = 0; i < NUM_CHANNELS; i++) {
+	for (i = 0; i < channels; i++) {
 		u32 mtr;
 
-		if (!pvt->pci_tad[i])
-			continue;
-		for (j = 0; j < ARRAY_SIZE(mtr_regs); j++) {
+		int max_dimms_per_channel;
+
+		if (pvt->info.type == KNIGHTS_LANDING) {
+			max_dimms_per_channel = 1;
+			if (!pvt->knl.pci_channel[i])
+				continue;
+		} else {
+			max_dimms_per_channel = ARRAY_SIZE(mtr_regs);
+			if (!pvt->pci_tad[i])
+				continue;
+		}
+
+		for (j = 0; j < max_dimms_per_channel; j++) {
 			dimm = EDAC_DIMM_PTR(mci->layers, mci->dimms, mci->n_layers,
 				       i, j, 0);
-			pci_read_config_dword(pvt->pci_tad[i],
-					      mtr_regs[j], &mtr);
+			if (pvt->info.type == KNIGHTS_LANDING) {
+				pci_read_config_dword(pvt->knl.pci_channel[i],
+					knl_mtr_reg, &mtr);
+			} else {
+				pci_read_config_dword(pvt->pci_tad[i],
+					mtr_regs[j], &mtr);
+			}
 			edac_dbg(4, "Channel #%d  MTR%d = %x\n", i, j, mtr);
 			if (IS_DIMM_PRESENT(mtr)) {
 				pvt->channel[i].dimms++;
 
 				ranks = numrank(pvt->info.type, mtr);
-				rows = numrow(mtr);
-				cols = numcol(mtr);
+
+				if (pvt->info.type == KNIGHTS_LANDING) {
+					/* For DDR4, this is fixed. */
+					cols = 1 << 10;
+					rows = knl_mc_sizes[i] /
+						((u64) cols * ranks * banks * 8);
+				} else {
+					rows = numrow(mtr);
+					cols = numcol(mtr);
+				}
 
 				size = ((u64)rows * cols * banks * ranks) >> (20 - 3);
 				npages = MiB_TO_PAGES(size);
@@ -1069,7 +1788,7 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 		/* SAD_LIMIT Address range is 45:26 */
 		pci_read_config_dword(pvt->pci_sad0, pvt->info.dram_rule[n_sads],
 				      &reg);
-		limit = SAD_LIMIT(reg);
+		limit = pvt->info.sad_limit(reg);
 
 		if (!DRAM_RULE_ENABLE(reg))
 			continue;
@@ -1081,10 +1800,10 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 		gb = div_u64_rem(tmp_mb, 1024, &mb);
 		edac_dbg(0, "SAD#%d %s up to %u.%03u GB (0x%016Lx) Interleave: %s reg=0x%08x\n",
 			 n_sads,
-			 get_dram_attr(reg),
+			 show_dram_attr(pvt->info.dram_attr(reg)),
 			 gb, (mb*1000)/1024,
 			 ((u64)tmp_mb) << 20L,
-			 INTERLEAVE_MODE(reg) ? "8:6" : "[8:6]XOR[18:16]",
+			 pvt->info.show_interleave_mode(reg),
 			 reg);
 		prv = limit;
 
@@ -1101,6 +1820,9 @@ static void get_memory_layout(const struct mem_ctl_info *mci)
 		}
 	}
 
+	if (pvt->info.type == KNIGHTS_LANDING)
+		return;
+
 	/*
 	 * Step 3) Get TAD range
 	 */
@@ -1248,7 +1970,7 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		if (!DRAM_RULE_ENABLE(reg))
 			continue;
 
-		limit = SAD_LIMIT(reg);
+		limit = pvt->info.sad_limit(reg);
 		if (limit <= prv) {
 			sprintf(msg, "Can't discover the memory socket");
 			return -EINVAL;
@@ -1262,8 +1984,8 @@ static int get_memory_error_data(struct mem_ctl_info *mci,
 		return -EINVAL;
 	}
 	dram_rule = reg;
-	*area_type = get_dram_attr(dram_rule);
-	interleave_mode = INTERLEAVE_MODE(dram_rule);
+	*area_type = show_dram_attr(pvt->info.dram_attr(dram_rule));
+	interleave_mode = pvt->info.interleave_mode(dram_rule);
 
 	pci_read_config_dword(pvt->pci_sad0, pvt->info.interleave_list[n_sads],
 			      &reg);
@@ -1567,7 +2289,8 @@ static void sbridge_put_all_devices(void)
 static int sbridge_get_onedevice(struct pci_dev **prev,
 				 u8 *num_mc,
 				 const struct pci_id_table *table,
-				 const unsigned devno)
+				 const unsigned devno,
+				 const int multi_bus)
 {
 	struct sbridge_dev *sbridge_dev;
 	const struct pci_id_descr *dev_descr = &table->descr[devno];
@@ -1603,7 +2326,7 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
 	}
 	bus = pdev->bus->number;
 
-	sbridge_dev = get_sbridge_dev(bus);
+	sbridge_dev = get_sbridge_dev(bus, multi_bus);
 	if (!sbridge_dev) {
 		sbridge_dev = alloc_sbridge_dev(bus, table);
 		if (!sbridge_dev) {
@@ -1652,21 +2375,32 @@ static int sbridge_get_onedevice(struct pci_dev **prev,
  * @num_mc: pointer to the memory controllers count, to be incremented in case
  *	    of success.
  * @table: model specific table
+ * @allow_dups: allow for multiple devices to exist with the same device id
+ *              (as implemented, this isn't expected to work correctly in the
+ *              multi-socket case).
+ * @multi_bus: don't assume devices on different buses belong to different
+ *             memory controllers.
  *
  * returns 0 in case of success or error code
  */
-static int sbridge_get_all_devices(u8 *num_mc,
-				   const struct pci_id_table *table)
+static int sbridge_get_all_devices_full(u8 *num_mc,
+					const struct pci_id_table *table,
+					int allow_dups,
+					int multi_bus)
 {
 	int i, rc;
 	struct pci_dev *pdev = NULL;
 
 	while (table && table->descr) {
 		for (i = 0; i < table->n_devs; i++) {
-			pdev = NULL;
+			if (!allow_dups || i == 0 ||
+					table->descr[i].dev_id !=
+						table->descr[i-1].dev_id) {
+				pdev = NULL;
+			}
 			do {
 				rc = sbridge_get_onedevice(&pdev, num_mc,
-							   table, i);
+							   table, i, multi_bus);
 				if (rc < 0) {
 					if (i == 0) {
 						i = table->n_devs;
@@ -1675,7 +2409,7 @@ static int sbridge_get_all_devices(u8 *num_mc,
 					sbridge_put_all_devices();
 					return -ENODEV;
 				}
-			} while (pdev);
+			} while (pdev && !allow_dups);
 		}
 		table++;
 	}
@@ -1683,6 +2417,11 @@ static int sbridge_get_all_devices(u8 *num_mc,
 	return 0;
 }
 
+#define sbridge_get_all_devices(num_mc, table) \
+		sbridge_get_all_devices_full(num_mc, table, 0, 0)
+#define sbridge_get_all_devices_knl(num_mc, table) \
+		sbridge_get_all_devices_full(num_mc, table, 1, 1)
+
 static int sbridge_mci_bind_devs(struct mem_ctl_info *mci,
 				 struct sbridge_dev *sbridge_dev)
 {
@@ -2038,6 +2777,131 @@ enodev:
 	return -ENODEV;
 }
 
+static int knl_mci_bind_devs(struct mem_ctl_info *mci,
+			struct sbridge_dev *sbridge_dev)
+{
+	struct sbridge_pvt *pvt = mci->pvt_info;
+	struct pci_dev *pdev;
+	int dev, func;
+
+	int i;
+	int devidx;
+
+	for (i = 0; i < sbridge_dev->n_devs; i++) {
+		pdev = sbridge_dev->pdev[i];
+		if (!pdev)
+			continue;
+
+		/* Extract PCI device and function. */
+		dev = (pdev->devfn >> 3) & 0x1f;
+		func = pdev->devfn & 0x7;
+
+		switch (pdev->device) {
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_MC:
+			if (dev == 8)
+				pvt->knl.pci_mc0 = pdev;
+			else if (dev == 9)
+				pvt->knl.pci_mc1 = pdev;
+			else {
+				sbridge_printk(KERN_ERR,
+					"Memory controller in unexpected place! (dev %d, fn %d)\n",
+					dev, func);
+				continue;
+			}
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0:
+			pvt->pci_sad0 = pdev;
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_SAD1:
+			pvt->pci_sad1 = pdev;
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_CHA:
+			/* There are one of these per tile, and range from
+			 * 1.14.0 to 1.18.5.
+			 */
+			devidx = ((dev-14)*8)+func;
+
+			if (devidx < 0 || devidx >= KNL_MAX_CHAS) {
+				sbridge_printk(KERN_ERR,
+					"Caching and Home Agent in unexpected place! (dev %d, fn %d)\n",
+					dev, func);
+				continue;
+			}
+
+			WARN_ON(pvt->knl.pci_cha[devidx] != NULL);
+
+			pvt->knl.pci_cha[devidx] = pdev;
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_CHANNEL:
+			devidx = -1;
+
+			/*
+			 *  MC0 channels 0-2 are device 9 function 2-4,
+			 *  MC1 channels 3-5 are device 8 function 2-4.
+			 */
+
+			if (dev == 9)
+				devidx = func-2;
+			else if (dev == 8)
+				devidx = 3 + (func-2);
+
+			if (devidx < 0 || devidx >= KNL_MAX_CHANNELS) {
+				sbridge_printk(KERN_ERR,
+					"DRAM Channel Registers in unexpected place! (dev %d, fn %d)\n",
+					dev, func);
+				continue;
+			}
+
+			WARN_ON(pvt->knl.pci_channel[devidx] != NULL);
+			pvt->knl.pci_channel[devidx] = pdev;
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_TOLHM:
+			pvt->knl.pci_mc_info = pdev;
+			break;
+
+		case PCI_DEVICE_ID_INTEL_KNL_IMC_TA:
+			pvt->pci_ta = pdev;
+			break;
+
+		default:
+			sbridge_printk(KERN_ERR, "Unexpected device %d\n",
+				pdev->device);
+			break;
+		}
+	}
+
+	if (!pvt->knl.pci_mc0  || !pvt->knl.pci_mc1 ||
+	    !pvt->pci_sad0     || !pvt->pci_sad1    ||
+	    !pvt->pci_ta) {
+		goto enodev;
+	}
+
+	for (i = 0; i < KNL_MAX_CHANNELS; i++) {
+		if (!pvt->knl.pci_channel[i]) {
+			sbridge_printk(KERN_ERR, "Missing channel %d\n", i);
+			goto enodev;
+		}
+	}
+
+	for (i = 0; i < KNL_MAX_CHAS; i++) {
+		if (!pvt->knl.pci_cha[i]) {
+			sbridge_printk(KERN_ERR, "Missing CHA %d\n", i);
+			goto enodev;
+		}
+	}
+
+	return 0;
+
+enodev:
+	sbridge_printk(KERN_ERR, "Some needed devices are missing\n");
+	return -ENODEV;
+}
+
 /****************************************************************************
 			Error check routines
  ****************************************************************************/
@@ -2127,8 +2991,36 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 	if (!GET_BITFIELD(m->status, 58, 58))
 		return;
 
-	rc = get_memory_error_data(mci, m->addr, &socket, &ha,
-				   &channel_mask, &rank, &area_type, msg);
+	if (pvt->info.type == KNIGHTS_LANDING) {
+		if (channel == 14) {
+			edac_dbg(0, "%s%s err_code:%04x:%04x EDRAM bank %d\n",
+				overflow ? " OVERFLOW" : "",
+				(uncorrected_error && recoverable)
+				? " recoverable" : "",
+				mscod, errcode,
+				m->bank);
+		} else {
+			char A = *("A");
+
+			channel = knl_channel_remap(channel);
+			channel_mask = 1 << channel;
+			snprintf(msg, sizeof(msg),
+				"%s%s err_code:%04x:%04x channel:%d (DIMM_%c)",
+				overflow ? " OVERFLOW" : "",
+				(uncorrected_error && recoverable)
+				? " recoverable" : " ",
+				mscod, errcode, channel, A + channel);
+			edac_mc_handle_error(tp_event, mci, core_err_cnt,
+				m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
+				channel, 0, -1,
+				optype, msg);
+		}
+		return;
+	} else {
+		rc = get_memory_error_data(mci, m->addr, &socket, &ha,
+				&channel_mask, &rank, &area_type, msg);
+	}
+
 	if (rc < 0)
 		goto err_parsing;
 	new_mci = get_mci_for_node_id(socket);
@@ -2359,10 +3251,11 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 
 	/* allocate a new MC control structure */
 	layers[0].type = EDAC_MC_LAYER_CHANNEL;
-	layers[0].size = NUM_CHANNELS;
+	layers[0].size = type == KNIGHTS_LANDING ?
+		KNL_MAX_CHANNELS : NUM_CHANNELS;
 	layers[0].is_virt_csrow = false;
 	layers[1].type = EDAC_MC_LAYER_SLOT;
-	layers[1].size = MAX_DIMMS;
+	layers[1].size = type == KNIGHTS_LANDING ? 1 : MAX_DIMMS;
 	layers[1].is_virt_csrow = true;
 	mci = edac_mc_alloc(sbridge_dev->mc, ARRAY_SIZE(layers), layers,
 			    sizeof(*pvt));
@@ -2380,7 +3273,8 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 	pvt->sbridge_dev = sbridge_dev;
 	sbridge_dev->mci = mci;
 
-	mci->mtype_cap = MEM_FLAG_DDR3;
+	mci->mtype_cap = type == KNIGHTS_LANDING ?
+		MEM_FLAG_DDR4 : MEM_FLAG_DDR3;
 	mci->edac_ctl_cap = EDAC_FLAG_NONE;
 	mci->edac_cap = EDAC_FLAG_NONE;
 	mci->mod_name = "sbridge_edac.c";
@@ -2401,6 +3295,10 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.get_memory_type = get_memory_type;
 		pvt->info.get_node_id = get_node_id;
 		pvt->info.rir_limit = rir_limit;
+		pvt->info.sad_limit = sad_limit;
+		pvt->info.interleave_mode = interleave_mode;
+		pvt->info.show_interleave_mode = show_interleave_mode;
+		pvt->info.dram_attr = dram_attr;
 		pvt->info.max_sad = ARRAY_SIZE(ibridge_dram_rule);
 		pvt->info.interleave_list = ibridge_interleave_list;
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
@@ -2421,6 +3319,10 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.get_memory_type = get_memory_type;
 		pvt->info.get_node_id = get_node_id;
 		pvt->info.rir_limit = rir_limit;
+		pvt->info.sad_limit = sad_limit;
+		pvt->info.interleave_mode = interleave_mode;
+		pvt->info.show_interleave_mode = show_interleave_mode;
+		pvt->info.dram_attr = dram_attr;
 		pvt->info.max_sad = ARRAY_SIZE(sbridge_dram_rule);
 		pvt->info.interleave_list = sbridge_interleave_list;
 		pvt->info.max_interleave = ARRAY_SIZE(sbridge_interleave_list);
@@ -2441,6 +3343,10 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.get_memory_type = haswell_get_memory_type;
 		pvt->info.get_node_id = haswell_get_node_id;
 		pvt->info.rir_limit = haswell_rir_limit;
+		pvt->info.sad_limit = sad_limit;
+		pvt->info.interleave_mode = interleave_mode;
+		pvt->info.show_interleave_mode = show_interleave_mode;
+		pvt->info.dram_attr = dram_attr;
 		pvt->info.max_sad = ARRAY_SIZE(ibridge_dram_rule);
 		pvt->info.interleave_list = ibridge_interleave_list;
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
@@ -2461,6 +3367,10 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		pvt->info.get_memory_type = haswell_get_memory_type;
 		pvt->info.get_node_id = haswell_get_node_id;
 		pvt->info.rir_limit = haswell_rir_limit;
+		pvt->info.sad_limit = sad_limit;
+		pvt->info.interleave_mode = interleave_mode;
+		pvt->info.show_interleave_mode = show_interleave_mode;
+		pvt->info.dram_attr = dram_attr;
 		pvt->info.max_sad = ARRAY_SIZE(ibridge_dram_rule);
 		pvt->info.interleave_list = ibridge_interleave_list;
 		pvt->info.max_interleave = ARRAY_SIZE(ibridge_interleave_list);
@@ -2473,6 +3383,30 @@ static int sbridge_register_mci(struct sbridge_dev *sbridge_dev, enum type type)
 		if (unlikely(rc < 0))
 			goto fail0;
 		break;
+	case KNIGHTS_LANDING:
+		/* pvt->info.rankcfgr == ??? */
+		pvt->info.get_tolm = knl_get_tolm;
+		pvt->info.get_tohm = knl_get_tohm;
+		pvt->info.dram_rule = knl_dram_rule;
+		pvt->info.get_memory_type = knl_get_memory_type;
+		pvt->info.get_node_id = knl_get_node_id;
+		pvt->info.rir_limit = NULL;
+		pvt->info.sad_limit = knl_sad_limit;
+		pvt->info.interleave_mode = knl_interleave_mode;
+		pvt->info.show_interleave_mode = knl_show_interleave_mode;
+		pvt->info.dram_attr = dram_attr_knl;
+		pvt->info.max_sad = ARRAY_SIZE(knl_dram_rule);
+		pvt->info.interleave_list = knl_interleave_list;
+		pvt->info.max_interleave = ARRAY_SIZE(knl_interleave_list);
+		pvt->info.interleave_pkg = ibridge_interleave_pkg;
+		pvt->info.get_width = knl_get_width;
+		mci->ctl_name = kasprintf(GFP_KERNEL,
+			"Knights Landing Socket#%d", mci->mc_idx);
+
+		rc = knl_mci_bind_devs(mci, sbridge_dev);
+		if (unlikely(rc < 0))
+			goto fail0;
+		break;
 	}
 
 	/* Get dimm basic config and the memory layout */
@@ -2527,20 +3461,29 @@ static int sbridge_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	switch (pdev->device) {
 	case PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA:
-		rc = sbridge_get_all_devices(&num_mc, pci_dev_descr_ibridge_table);
+		rc = sbridge_get_all_devices(&num_mc,
+					pci_dev_descr_ibridge_table);
 		type = IVY_BRIDGE;
 		break;
 	case PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0:
-		rc = sbridge_get_all_devices(&num_mc, pci_dev_descr_sbridge_table);
+		rc = sbridge_get_all_devices(&num_mc,
+					pci_dev_descr_sbridge_table);
 		type = SANDY_BRIDGE;
 		break;
 	case PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0:
-		rc = sbridge_get_all_devices(&num_mc, pci_dev_descr_haswell_table);
+		rc = sbridge_get_all_devices(&num_mc,
+					pci_dev_descr_haswell_table);
 		type = HASWELL;
 		break;
 	case PCI_DEVICE_ID_INTEL_BROADWELL_IMC_HA0:
-		rc = sbridge_get_all_devices(&num_mc, pci_dev_descr_broadwell_table);
+		rc = sbridge_get_all_devices(&num_mc,
+					pci_dev_descr_broadwell_table);
 		type = BROADWELL;
+	    break;
+	case PCI_DEVICE_ID_INTEL_KNL_IMC_SAD0:
+		rc = sbridge_get_all_devices_knl(&num_mc,
+					pci_dev_descr_knl_table);
+		type = KNIGHTS_LANDING;
 		break;
 	}
 	if (unlikely(rc < 0)) {
diff --git a/drivers/edac/wq.c b/drivers/edac/wq.c
new file mode 100644
index 000000000000..1b8c07e44fd8
--- /dev/null
+++ b/drivers/edac/wq.c
@@ -0,0 +1,42 @@
+#include "edac_module.h"
+
+static struct workqueue_struct *wq;
+
+bool edac_queue_work(struct delayed_work *work, unsigned long delay)
+{
+	return queue_delayed_work(wq, work, delay);
+}
+EXPORT_SYMBOL_GPL(edac_queue_work);
+
+bool edac_mod_work(struct delayed_work *work, unsigned long delay)
+{
+	return mod_delayed_work(wq, work, delay);
+}
+EXPORT_SYMBOL_GPL(edac_mod_work);
+
+bool edac_stop_work(struct delayed_work *work)
+{
+	bool ret;
+
+	ret = cancel_delayed_work_sync(work);
+	flush_workqueue(wq);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(edac_stop_work);
+
+int edac_workqueue_setup(void)
+{
+	wq = create_singlethread_workqueue("edac-poller");
+	if (!wq)
+		return -ENODEV;
+	else
+		return 0;
+}
+
+void edac_workqueue_teardown(void)
+{
+	flush_workqueue(wq);
+	destroy_workqueue(wq);
+	wq = NULL;
+}
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 0cebbf668886..3d89e60a3e71 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -52,6 +52,15 @@ config EXTCON_MAX14577
 	  Maxim MAX14577/77836. The MAX14577/77836 MUIC is a USB port accessory
 	  detector and switch.
 
+config EXTCON_MAX3355
+	tristate "Maxim MAX3355 USB OTG EXTCON Support"
+	depends on GPIOLIB || COMPILE_TEST
+	help
+	  If you say yes here you get support for the USB OTG role detection by
+	  MAX3355. The MAX3355 chip integrates a charge pump and comparators to
+	  enable a system with an integrated USB OTG dual-role transceiver to
+	  function as an USB OTG dual-role device.
+
 config EXTCON_MAX77693
 	tristate "Maxim MAX77693 EXTCON Support"
 	depends on MFD_MAX77693 && INPUT
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index ba787d04295b..2a0e4f45d5b2 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_EXTCON_ARIZONA)	+= extcon-arizona.o
 obj-$(CONFIG_EXTCON_AXP288)	+= extcon-axp288.o
 obj-$(CONFIG_EXTCON_GPIO)	+= extcon-gpio.o
 obj-$(CONFIG_EXTCON_MAX14577)	+= extcon-max14577.o
+obj-$(CONFIG_EXTCON_MAX3355)	+= extcon-max3355.o
 obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
 obj-$(CONFIG_EXTCON_MAX77843)	+= extcon-max77843.o
 obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
diff --git a/drivers/extcon/extcon-arizona.c b/drivers/extcon/extcon-arizona.c
index e4890dd4fefd..c121d01a5cd6 100644
--- a/drivers/extcon/extcon-arizona.c
+++ b/drivers/extcon/extcon-arizona.c
@@ -1201,10 +1201,58 @@ static void arizona_micd_set_level(struct arizona *arizona, int index,
 	regmap_update_bits(arizona->regmap, reg, mask, level);
 }
 
-static int arizona_extcon_device_get_pdata(struct arizona *arizona)
+static int arizona_extcon_get_micd_configs(struct device *dev,
+					   struct arizona *arizona)
+{
+	const char * const prop = "wlf,micd-configs";
+	const int entries_per_config = 3;
+	struct arizona_micd_config *micd_configs;
+	int nconfs, ret;
+	int i, j;
+	u32 *vals;
+
+	nconfs = device_property_read_u32_array(arizona->dev, prop, NULL, 0);
+	if (nconfs <= 0)
+		return 0;
+
+	vals = kcalloc(nconfs, sizeof(u32), GFP_KERNEL);
+	if (!vals)
+		return -ENOMEM;
+
+	ret = device_property_read_u32_array(arizona->dev, prop, vals, nconfs);
+	if (ret < 0)
+		goto out;
+
+	nconfs /= entries_per_config;
+
+	micd_configs = devm_kzalloc(dev,
+				    nconfs * sizeof(struct arizona_micd_range),
+				    GFP_KERNEL);
+	if (!micd_configs) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	for (i = 0, j = 0; i < nconfs; ++i) {
+		micd_configs[i].src = vals[j++] ? ARIZONA_ACCDET_SRC : 0;
+		micd_configs[i].bias = vals[j++];
+		micd_configs[i].gpio = vals[j++];
+	}
+
+	arizona->pdata.micd_configs = micd_configs;
+	arizona->pdata.num_micd_configs = nconfs;
+
+out:
+	kfree(vals);
+	return ret;
+}
+
+static int arizona_extcon_device_get_pdata(struct device *dev,
+					   struct arizona *arizona)
 {
 	struct arizona_pdata *pdata = &arizona->pdata;
 	unsigned int val = ARIZONA_ACCDET_MODE_HPL;
+	int ret;
 
 	device_property_read_u32(arizona->dev, "wlf,hpdet-channel", &val);
 	switch (val) {
@@ -1230,12 +1278,29 @@ static int arizona_extcon_device_get_pdata(struct arizona *arizona)
 	device_property_read_u32(arizona->dev, "wlf,micd-dbtime",
 				 &pdata->micd_dbtime);
 
-	device_property_read_u32(arizona->dev, "wlf,micd-timeout",
+	device_property_read_u32(arizona->dev, "wlf,micd-timeout-ms",
 				 &pdata->micd_timeout);
 
 	pdata->micd_force_micbias = device_property_read_bool(arizona->dev,
 						"wlf,micd-force-micbias");
 
+	pdata->micd_software_compare = device_property_read_bool(arizona->dev,
+						"wlf,micd-software-compare");
+
+	pdata->jd_invert = device_property_read_bool(arizona->dev,
+						     "wlf,jd-invert");
+
+	device_property_read_u32(arizona->dev, "wlf,gpsw", &pdata->gpsw);
+
+	pdata->jd_gpio5 = device_property_read_bool(arizona->dev,
+						    "wlf,use-jd2");
+	pdata->jd_gpio5_nopull = device_property_read_bool(arizona->dev,
+						"wlf,use-jd2-nopull");
+
+	ret = arizona_extcon_get_micd_configs(dev, arizona);
+	if (ret < 0)
+		dev_err(arizona->dev, "Failed to read micd configs: %d\n", ret);
+
 	return 0;
 }
 
@@ -1257,7 +1322,7 @@ static int arizona_extcon_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	if (!dev_get_platdata(arizona->dev))
-		arizona_extcon_device_get_pdata(arizona);
+		arizona_extcon_device_get_pdata(&pdev->dev, arizona);
 
 	info->micvdd = devm_regulator_get(&pdev->dev, "MICVDD");
 	if (IS_ERR(info->micvdd)) {
diff --git a/drivers/extcon/extcon-max14577.c b/drivers/extcon/extcon-max14577.c
index 601dbd996487..b30ab97ce75f 100644
--- a/drivers/extcon/extcon-max14577.c
+++ b/drivers/extcon/extcon-max14577.c
@@ -692,7 +692,7 @@ static int max14577_muic_probe(struct platform_device *pdev)
 	/* Support irq domain for max14577 MUIC device */
 	for (i = 0; i < info->muic_irqs_num; i++) {
 		struct max14577_muic_irq *muic_irq = &info->muic_irqs[i];
-		unsigned int virq = 0;
+		int virq = 0;
 
 		virq = regmap_irq_get_virq(max14577->irq_data, muic_irq->irq);
 		if (virq <= 0)
diff --git a/drivers/extcon/extcon-max3355.c b/drivers/extcon/extcon-max3355.c
new file mode 100644
index 000000000000..c24abec5d06c
--- /dev/null
+++ b/drivers/extcon/extcon-max3355.c
@@ -0,0 +1,146 @@
+/*
+ * Maxim Integrated MAX3355 USB OTG chip extcon driver
+ *
+ * Copyright (C)  2014-2015 Cogent Embedded, Inc.
+ * Author: Sergei Shtylyov <sergei.shtylyov@cogentembedded.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.
+ */
+
+#include <linux/extcon.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+struct max3355_data {
+	struct extcon_dev *edev;
+	struct gpio_desc *id_gpiod;
+	struct gpio_desc *shdn_gpiod;
+};
+
+static const unsigned int max3355_cable[] = {
+	EXTCON_USB,
+	EXTCON_USB_HOST,
+	EXTCON_NONE,
+};
+
+static irqreturn_t max3355_id_irq(int irq, void *dev_id)
+{
+	struct max3355_data *data = dev_id;
+	int id = gpiod_get_value_cansleep(data->id_gpiod);
+
+	if (id) {
+		/*
+		 * ID = 1 means USB HOST cable detached.
+		 * 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);
+	} 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);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int max3355_probe(struct platform_device *pdev)
+{
+	struct max3355_data *data;
+	struct gpio_desc *gpiod;
+	int irq, err;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(struct max3355_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	gpiod = devm_gpiod_get(&pdev->dev, "id", GPIOD_IN);
+	if (IS_ERR(gpiod)) {
+		dev_err(&pdev->dev, "failed to get ID_OUT GPIO\n");
+		return PTR_ERR(gpiod);
+	}
+	data->id_gpiod = gpiod;
+
+	gpiod = devm_gpiod_get(&pdev->dev, "maxim,shdn", GPIOD_OUT_HIGH);
+	if (IS_ERR(gpiod)) {
+		dev_err(&pdev->dev, "failed to get SHDN# GPIO\n");
+		return PTR_ERR(gpiod);
+	}
+	data->shdn_gpiod = gpiod;
+
+	data->edev = devm_extcon_dev_allocate(&pdev->dev, max3355_cable);
+	if (IS_ERR(data->edev)) {
+		dev_err(&pdev->dev, "failed to allocate extcon device\n");
+		return PTR_ERR(data->edev);
+	}
+
+	err = devm_extcon_dev_register(&pdev->dev, data->edev);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to register extcon device\n");
+		return err;
+	}
+
+	irq = gpiod_to_irq(data->id_gpiod);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to translate ID_OUT GPIO to IRQ\n");
+		return irq;
+	}
+
+	err = devm_request_threaded_irq(&pdev->dev, irq, NULL, max3355_id_irq,
+					IRQF_ONESHOT | IRQF_NO_SUSPEND |
+					IRQF_TRIGGER_RISING |
+					IRQF_TRIGGER_FALLING,
+					pdev->name, data);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to request ID_OUT IRQ\n");
+		return err;
+	}
+
+	platform_set_drvdata(pdev, data);
+
+	/* Perform initial detection */
+	max3355_id_irq(irq, data);
+
+	return 0;
+}
+
+static int max3355_remove(struct platform_device *pdev)
+{
+	struct max3355_data *data = platform_get_drvdata(pdev);
+
+	gpiod_set_value_cansleep(data->shdn_gpiod, 0);
+
+	return 0;
+}
+
+static const struct of_device_id max3355_match_table[] = {
+	{ .compatible = "maxim,max3355", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max3355_match_table);
+
+static struct platform_driver max3355_driver = {
+	.probe		= max3355_probe,
+	.remove		= max3355_remove,
+	.driver		= {
+		.name	= "extcon-max3355",
+		.of_match_table = max3355_match_table,
+	},
+};
+
+module_platform_driver(max3355_driver);
+
+MODULE_AUTHOR("Sergei Shtylyov <sergei.shtylyov@cogentembedded.com>");
+MODULE_DESCRIPTION("Maxim MAX3355 extcon driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index 44c499e1beee..fdf8f5d4d4e9 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -1127,11 +1127,11 @@ static int max77693_muic_probe(struct platform_device *pdev)
 	/* Support irq domain for MAX77693 MUIC device */
 	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++) {
 		struct max77693_muic_irq *muic_irq = &muic_irqs[i];
-		unsigned int virq = 0;
+		int virq;
 
 		virq = regmap_irq_get_virq(max77693->irq_data_muic,
 					muic_irq->irq);
-		if (!virq)
+		if (virq <= 0)
 			return -EINVAL;
 		muic_irq->virq = virq;
 
diff --git a/drivers/extcon/extcon-max77843.c b/drivers/extcon/extcon-max77843.c
index 9f9ea334399c..74dfb7f4f277 100644
--- a/drivers/extcon/extcon-max77843.c
+++ b/drivers/extcon/extcon-max77843.c
@@ -811,7 +811,7 @@ static int max77843_muic_probe(struct platform_device *pdev)
 
 	for (i = 0; i < ARRAY_SIZE(max77843_muic_irqs); i++) {
 		struct max77843_muic_irq *muic_irq = &max77843_muic_irqs[i];
-		unsigned int virq = 0;
+		int virq = 0;
 
 		virq = regmap_irq_get_virq(max77843->irq_data_muic,
 				muic_irq->irq);
diff --git a/drivers/extcon/extcon-rt8973a.c b/drivers/extcon/extcon-rt8973a.c
index 36bf1d63791c..e1bb82809bef 100644
--- a/drivers/extcon/extcon-rt8973a.c
+++ b/drivers/extcon/extcon-rt8973a.c
@@ -603,7 +603,7 @@ static int rt8973a_muic_i2c_probe(struct i2c_client *i2c,
 
 		ret = devm_request_threaded_irq(info->dev, virq, NULL,
 						rt8973a_muic_irq_handler,
-						IRQF_NO_SUSPEND,
+						IRQF_NO_SUSPEND | IRQF_ONESHOT,
 						muic_irq->name, info);
 		if (ret) {
 			dev_err(info->dev,
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index cf478fe6b335..49a3a1185bb6 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -173,6 +173,9 @@ config QCOM_SCM_64
 	def_bool y
 	depends on QCOM_SCM && ARM64
 
+config HAVE_ARM_SMCCC
+	bool
+
 source "drivers/firmware/broadcom/Kconfig"
 source "drivers/firmware/google/Kconfig"
 source "drivers/firmware/efi/Kconfig"
diff --git a/drivers/firmware/broadcom/bcm47xx_nvram.c b/drivers/firmware/broadcom/bcm47xx_nvram.c
index e41594510b97..0c2f0a61b0ea 100644
--- a/drivers/firmware/broadcom/bcm47xx_nvram.c
+++ b/drivers/firmware/broadcom/bcm47xx_nvram.c
@@ -56,9 +56,7 @@ static u32 find_nvram_size(void __iomem *end)
 static int nvram_find_and_copy(void __iomem *iobase, u32 lim)
 {
 	struct nvram_header __iomem *header;
-	int i;
 	u32 off;
-	u32 *src, *dst;
 	u32 size;
 
 	if (nvram_len) {
@@ -95,10 +93,7 @@ static int nvram_find_and_copy(void __iomem *iobase, u32 lim)
 	return -ENXIO;
 
 found:
-	src = (u32 *)header;
-	dst = (u32 *)nvram_buf;
-	for (i = 0; i < sizeof(struct nvram_header); i += 4)
-		*dst++ = __raw_readl(src++);
+	__ioread32_copy(nvram_buf, header, sizeof(*header) / 4);
 	header = (struct nvram_header *)nvram_buf;
 	nvram_len = header->len;
 	if (nvram_len > size) {
@@ -111,8 +106,8 @@ found:
 		nvram_len = NVRAM_SPACE - 1;
 	}
 	/* proceed reading data after header */
-	for (; i < nvram_len; i += 4)
-		*dst++ = readl(src++);
+	__ioread32_copy(nvram_buf + sizeof(*header), header + 1,
+			DIV_ROUND_UP(nvram_len, 4));
 	nvram_buf[NVRAM_SPACE - 1] = '\0';
 
 	return 0;
diff --git a/drivers/firmware/dmi_scan.c b/drivers/firmware/dmi_scan.c
index 0e08e665f715..88bebe1968b7 100644
--- a/drivers/firmware/dmi_scan.c
+++ b/drivers/firmware/dmi_scan.c
@@ -321,39 +321,58 @@ static void __init dmi_save_ipmi_device(const struct dmi_header *dm)
 	list_add_tail(&dev->list, &dmi_devices);
 }
 
-static void __init dmi_save_dev_onboard(int instance, int segment, int bus,
-					int devfn, const char *name)
+static void __init dmi_save_dev_pciaddr(int instance, int segment, int bus,
+					int devfn, const char *name, int type)
 {
-	struct dmi_dev_onboard *onboard_dev;
+	struct dmi_dev_onboard *dev;
 
-	onboard_dev = dmi_alloc(sizeof(*onboard_dev) + strlen(name) + 1);
-	if (!onboard_dev)
+	/* Ignore invalid values */
+	if (type == DMI_DEV_TYPE_DEV_SLOT &&
+	    segment == 0xFFFF && bus == 0xFF && devfn == 0xFF)
 		return;
 
-	onboard_dev->instance = instance;
-	onboard_dev->segment = segment;
-	onboard_dev->bus = bus;
-	onboard_dev->devfn = devfn;
+	dev = dmi_alloc(sizeof(*dev) + strlen(name) + 1);
+	if (!dev)
+		return;
 
-	strcpy((char *)&onboard_dev[1], name);
-	onboard_dev->dev.type = DMI_DEV_TYPE_DEV_ONBOARD;
-	onboard_dev->dev.name = (char *)&onboard_dev[1];
-	onboard_dev->dev.device_data = onboard_dev;
+	dev->instance = instance;
+	dev->segment = segment;
+	dev->bus = bus;
+	dev->devfn = devfn;
 
-	list_add(&onboard_dev->dev.list, &dmi_devices);
+	strcpy((char *)&dev[1], name);
+	dev->dev.type = type;
+	dev->dev.name = (char *)&dev[1];
+	dev->dev.device_data = dev;
+
+	list_add(&dev->dev.list, &dmi_devices);
 }
 
 static void __init dmi_save_extended_devices(const struct dmi_header *dm)
 {
-	const u8 *d = (u8 *) dm + 5;
+	const char *name;
+	const u8 *d = (u8 *)dm;
 
 	/* Skip disabled device */
-	if ((*d & 0x80) == 0)
+	if ((d[0x5] & 0x80) == 0)
 		return;
 
-	dmi_save_dev_onboard(*(d+1), *(u16 *)(d+2), *(d+4), *(d+5),
-			     dmi_string_nosave(dm, *(d-1)));
-	dmi_save_one_device(*d & 0x7f, dmi_string_nosave(dm, *(d - 1)));
+	name = dmi_string_nosave(dm, d[0x4]);
+	dmi_save_dev_pciaddr(d[0x6], *(u16 *)(d + 0x7), d[0x9], d[0xA], name,
+			     DMI_DEV_TYPE_DEV_ONBOARD);
+	dmi_save_one_device(d[0x5] & 0x7f, name);
+}
+
+static void __init dmi_save_system_slot(const struct dmi_header *dm)
+{
+	const u8 *d = (u8 *)dm;
+
+	/* Need SMBIOS 2.6+ structure */
+	if (dm->length < 0x11)
+		return;
+	dmi_save_dev_pciaddr(*(u16 *)(d + 0x9), *(u16 *)(d + 0xD), d[0xF],
+			     d[0x10], dmi_string_nosave(dm, d[0x4]),
+			     DMI_DEV_TYPE_DEV_SLOT);
 }
 
 static void __init count_mem_devices(const struct dmi_header *dm, void *v)
@@ -426,6 +445,9 @@ static void __init dmi_decode(const struct dmi_header *dm, void *dummy)
 		dmi_save_ident(dm, DMI_CHASSIS_SERIAL, 7);
 		dmi_save_ident(dm, DMI_CHASSIS_ASSET_TAG, 8);
 		break;
+	case 9:		/* System Slots */
+		dmi_save_system_slot(dm);
+		break;
 	case 10:	/* Onboard Devices Information */
 		dmi_save_devices(dm);
 		break;
@@ -869,7 +891,7 @@ EXPORT_SYMBOL(dmi_name_in_vendors);
  *	@from: previous device found in search, or %NULL for new search.
  *
  *	Iterates through the list of known onboard devices. If a device is
- *	found with a matching @vendor and @device, a pointer to its device
+ *	found with a matching @type and @name, a pointer to its device
  *	structure is returned.  Otherwise, %NULL is returned.
  *	A new search is initiated by passing %NULL as the @from argument.
  *	If @from is not %NULL, searches continue from next device.
diff --git a/drivers/firmware/efi/Makefile b/drivers/firmware/efi/Makefile
index ec379a4164cc..62e654f255f4 100644
--- a/drivers/firmware/efi/Makefile
+++ b/drivers/firmware/efi/Makefile
@@ -18,3 +18,7 @@ obj-$(CONFIG_EFI_RUNTIME_MAP)		+= runtime-map.o
 obj-$(CONFIG_EFI_RUNTIME_WRAPPERS)	+= runtime-wrappers.o
 obj-$(CONFIG_EFI_STUB)			+= libstub/
 obj-$(CONFIG_EFI_FAKE_MEMMAP)		+= fake_mem.o
+
+arm-obj-$(CONFIG_EFI)			:= arm-init.o arm-runtime.o
+obj-$(CONFIG_ARM)			+= $(arm-obj-y)
+obj-$(CONFIG_ARM64)			+= $(arm-obj-y)
diff --git a/drivers/firmware/efi/arm-init.c b/drivers/firmware/efi/arm-init.c
new file mode 100644
index 000000000000..9e15d571b53c
--- /dev/null
+++ b/drivers/firmware/efi/arm-init.c
@@ -0,0 +1,209 @@
+/*
+ * Extensible Firmware Interface
+ *
+ * Based on Extensible Firmware Interface Specification version 2.4
+ *
+ * Copyright (C) 2013 - 2015 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/efi.h>
+#include <linux/init.h>
+#include <linux/memblock.h>
+#include <linux/mm_types.h>
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+
+#include <asm/efi.h>
+
+struct efi_memory_map memmap;
+
+u64 efi_system_table;
+
+static int __init is_normal_ram(efi_memory_desc_t *md)
+{
+	if (md->attribute & EFI_MEMORY_WB)
+		return 1;
+	return 0;
+}
+
+/*
+ * Translate a EFI virtual address into a physical address: this is necessary,
+ * as some data members of the EFI system table are virtually remapped after
+ * SetVirtualAddressMap() has been called.
+ */
+static phys_addr_t efi_to_phys(unsigned long addr)
+{
+	efi_memory_desc_t *md;
+
+	for_each_efi_memory_desc(&memmap, md) {
+		if (!(md->attribute & EFI_MEMORY_RUNTIME))
+			continue;
+		if (md->virt_addr == 0)
+			/* no virtual mapping has been installed by the stub */
+			break;
+		if (md->virt_addr <= addr &&
+		    (addr - md->virt_addr) < (md->num_pages << EFI_PAGE_SHIFT))
+			return md->phys_addr + addr - md->virt_addr;
+	}
+	return addr;
+}
+
+static int __init uefi_init(void)
+{
+	efi_char16_t *c16;
+	void *config_tables;
+	size_t table_size;
+	char vendor[100] = "unknown";
+	int i, retval;
+
+	efi.systab = early_memremap(efi_system_table,
+				    sizeof(efi_system_table_t));
+	if (efi.systab == NULL) {
+		pr_warn("Unable to map EFI system table.\n");
+		return -ENOMEM;
+	}
+
+	set_bit(EFI_BOOT, &efi.flags);
+	if (IS_ENABLED(CONFIG_64BIT))
+		set_bit(EFI_64BIT, &efi.flags);
+
+	/*
+	 * Verify the EFI Table
+	 */
+	if (efi.systab->hdr.signature != EFI_SYSTEM_TABLE_SIGNATURE) {
+		pr_err("System table signature incorrect\n");
+		retval = -EINVAL;
+		goto out;
+	}
+	if ((efi.systab->hdr.revision >> 16) < 2)
+		pr_warn("Warning: EFI system table version %d.%02d, expected 2.00 or greater\n",
+			efi.systab->hdr.revision >> 16,
+			efi.systab->hdr.revision & 0xffff);
+
+	/* Show what we know for posterity */
+	c16 = early_memremap(efi_to_phys(efi.systab->fw_vendor),
+			     sizeof(vendor) * sizeof(efi_char16_t));
+	if (c16) {
+		for (i = 0; i < (int) sizeof(vendor) - 1 && *c16; ++i)
+			vendor[i] = c16[i];
+		vendor[i] = '\0';
+		early_memunmap(c16, sizeof(vendor) * sizeof(efi_char16_t));
+	}
+
+	pr_info("EFI v%u.%.02u by %s\n",
+		efi.systab->hdr.revision >> 16,
+		efi.systab->hdr.revision & 0xffff, vendor);
+
+	table_size = sizeof(efi_config_table_64_t) * efi.systab->nr_tables;
+	config_tables = early_memremap(efi_to_phys(efi.systab->tables),
+				       table_size);
+	if (config_tables == NULL) {
+		pr_warn("Unable to map EFI config table array.\n");
+		retval = -ENOMEM;
+		goto out;
+	}
+	retval = efi_config_parse_tables(config_tables, efi.systab->nr_tables,
+					 sizeof(efi_config_table_t), NULL);
+
+	early_memunmap(config_tables, table_size);
+out:
+	early_memunmap(efi.systab,  sizeof(efi_system_table_t));
+	return retval;
+}
+
+/*
+ * Return true for RAM regions we want to permanently reserve.
+ */
+static __init int is_reserve_region(efi_memory_desc_t *md)
+{
+	switch (md->type) {
+	case EFI_LOADER_CODE:
+	case EFI_LOADER_DATA:
+	case EFI_BOOT_SERVICES_CODE:
+	case EFI_BOOT_SERVICES_DATA:
+	case EFI_CONVENTIONAL_MEMORY:
+	case EFI_PERSISTENT_MEMORY:
+		return 0;
+	default:
+		break;
+	}
+	return is_normal_ram(md);
+}
+
+static __init void reserve_regions(void)
+{
+	efi_memory_desc_t *md;
+	u64 paddr, npages, size;
+
+	if (efi_enabled(EFI_DBG))
+		pr_info("Processing EFI memory map:\n");
+
+	for_each_efi_memory_desc(&memmap, md) {
+		paddr = md->phys_addr;
+		npages = md->num_pages;
+
+		if (efi_enabled(EFI_DBG)) {
+			char buf[64];
+
+			pr_info("  0x%012llx-0x%012llx %s",
+				paddr, paddr + (npages << EFI_PAGE_SHIFT) - 1,
+				efi_md_typeattr_format(buf, sizeof(buf), md));
+		}
+
+		memrange_efi_to_native(&paddr, &npages);
+		size = npages << PAGE_SHIFT;
+
+		if (is_normal_ram(md))
+			early_init_dt_add_memory_arch(paddr, size);
+
+		if (is_reserve_region(md)) {
+			memblock_mark_nomap(paddr, size);
+			if (efi_enabled(EFI_DBG))
+				pr_cont("*");
+		}
+
+		if (efi_enabled(EFI_DBG))
+			pr_cont("\n");
+	}
+
+	set_bit(EFI_MEMMAP, &efi.flags);
+}
+
+void __init efi_init(void)
+{
+	struct efi_fdt_params params;
+
+	/* Grab UEFI information placed in FDT by stub */
+	if (!efi_get_fdt_params(&params))
+		return;
+
+	efi_system_table = params.system_table;
+
+	memmap.phys_map = params.mmap;
+	memmap.map = early_memremap(params.mmap, params.mmap_size);
+	if (memmap.map == NULL) {
+		/*
+		* If we are booting via UEFI, the UEFI memory map is the only
+		* description of memory we have, so there is little point in
+		* proceeding if we cannot access it.
+		*/
+		panic("Unable to map EFI memory map.\n");
+	}
+	memmap.map_end = memmap.map + params.mmap_size;
+	memmap.desc_size = params.desc_size;
+	memmap.desc_version = params.desc_ver;
+
+	if (uefi_init() < 0)
+		return;
+
+	reserve_regions();
+	early_memunmap(memmap.map, params.mmap_size);
+	memblock_mark_nomap(params.mmap & PAGE_MASK,
+			    PAGE_ALIGN(params.mmap_size +
+				       (params.mmap & ~PAGE_MASK)));
+}
diff --git a/drivers/firmware/efi/arm-runtime.c b/drivers/firmware/efi/arm-runtime.c
new file mode 100644
index 000000000000..6ae21e41a429
--- /dev/null
+++ b/drivers/firmware/efi/arm-runtime.c
@@ -0,0 +1,135 @@
+/*
+ * Extensible Firmware Interface
+ *
+ * Based on Extensible Firmware Interface Specification version 2.4
+ *
+ * Copyright (C) 2013, 2014 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/efi.h>
+#include <linux/io.h>
+#include <linux/memblock.h>
+#include <linux/mm_types.h>
+#include <linux/preempt.h>
+#include <linux/rbtree.h>
+#include <linux/rwsem.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+
+#include <asm/cacheflush.h>
+#include <asm/efi.h>
+#include <asm/mmu.h>
+#include <asm/pgalloc.h>
+#include <asm/pgtable.h>
+
+extern u64 efi_system_table;
+
+static struct mm_struct efi_mm = {
+	.mm_rb			= RB_ROOT,
+	.mm_users		= ATOMIC_INIT(2),
+	.mm_count		= ATOMIC_INIT(1),
+	.mmap_sem		= __RWSEM_INITIALIZER(efi_mm.mmap_sem),
+	.page_table_lock	= __SPIN_LOCK_UNLOCKED(efi_mm.page_table_lock),
+	.mmlist			= LIST_HEAD_INIT(efi_mm.mmlist),
+};
+
+static bool __init efi_virtmap_init(void)
+{
+	efi_memory_desc_t *md;
+
+	efi_mm.pgd = pgd_alloc(&efi_mm);
+	init_new_context(NULL, &efi_mm);
+
+	for_each_efi_memory_desc(&memmap, md) {
+		phys_addr_t phys = md->phys_addr;
+		int ret;
+
+		if (!(md->attribute & EFI_MEMORY_RUNTIME))
+			continue;
+		if (md->virt_addr == 0)
+			return false;
+
+		ret = efi_create_mapping(&efi_mm, md);
+		if  (!ret) {
+			pr_info("  EFI remap %pa => %p\n",
+				&phys, (void *)(unsigned long)md->virt_addr);
+		} else {
+			pr_warn("  EFI remap %pa: failed to create mapping (%d)\n",
+				&phys, ret);
+			return false;
+		}
+	}
+	return true;
+}
+
+/*
+ * Enable the UEFI Runtime Services if all prerequisites are in place, i.e.,
+ * non-early mapping of the UEFI system table and virtual mappings for all
+ * EFI_MEMORY_RUNTIME regions.
+ */
+static int __init arm_enable_runtime_services(void)
+{
+	u64 mapsize;
+
+	if (!efi_enabled(EFI_BOOT)) {
+		pr_info("EFI services will not be available.\n");
+		return 0;
+	}
+
+	if (efi_runtime_disabled()) {
+		pr_info("EFI runtime services will be disabled.\n");
+		return 0;
+	}
+
+	pr_info("Remapping and enabling EFI services.\n");
+
+	mapsize = memmap.map_end - memmap.map;
+	memmap.map = (__force void *)ioremap_cache(memmap.phys_map,
+						   mapsize);
+	if (!memmap.map) {
+		pr_err("Failed to remap EFI memory map\n");
+		return -ENOMEM;
+	}
+	memmap.map_end = memmap.map + mapsize;
+	efi.memmap = &memmap;
+
+	efi.systab = (__force void *)ioremap_cache(efi_system_table,
+						   sizeof(efi_system_table_t));
+	if (!efi.systab) {
+		pr_err("Failed to remap EFI System Table\n");
+		return -ENOMEM;
+	}
+	set_bit(EFI_SYSTEM_TABLES, &efi.flags);
+
+	if (!efi_virtmap_init()) {
+		pr_err("No UEFI virtual mapping was installed -- runtime services will not be available\n");
+		return -ENOMEM;
+	}
+
+	/* Set up runtime services function pointers */
+	efi_native_runtime_setup();
+	set_bit(EFI_RUNTIME_SERVICES, &efi.flags);
+
+	efi.runtime_version = efi.systab->hdr.revision;
+
+	return 0;
+}
+early_initcall(arm_enable_runtime_services);
+
+void efi_virtmap_load(void)
+{
+	preempt_disable();
+	efi_set_pgd(&efi_mm);
+}
+
+void efi_virtmap_unload(void)
+{
+	efi_set_pgd(current->active_mm);
+	preempt_enable();
+}
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index 027ca212179f..2cd37dad67a6 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -25,6 +25,8 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 
+#include <asm/early_ioremap.h>
+
 struct efi __read_mostly efi = {
 	.mps			= EFI_INVALID_TABLE_ADDR,
 	.acpi			= EFI_INVALID_TABLE_ADDR,
diff --git a/drivers/firmware/efi/efivars.c b/drivers/firmware/efi/efivars.c
index 756eca8c4cf8..10e6774ab2a2 100644
--- a/drivers/firmware/efi/efivars.c
+++ b/drivers/firmware/efi/efivars.c
@@ -221,7 +221,7 @@ sanity_check(struct efi_variable *var, efi_char16_t *name, efi_guid_t vendor,
 	}
 
 	if ((attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    efivar_validate(name, data, size) == false) {
+	    efivar_validate(vendor, name, data, size) == false) {
 		printk(KERN_ERR "efivars: Malformed variable content\n");
 		return -EINVAL;
 	}
@@ -447,7 +447,8 @@ static ssize_t efivar_create(struct file *filp, struct kobject *kobj,
 	}
 
 	if ((attributes & ~EFI_VARIABLE_MASK) != 0 ||
-	    efivar_validate(name, data, size) == false) {
+	    efivar_validate(new_var->VendorGuid, name, data,
+			    size) == false) {
 		printk(KERN_ERR "efivars: Malformed variable content\n");
 		return -EINVAL;
 	}
@@ -540,38 +541,30 @@ static ssize_t efivar_delete(struct file *filp, struct kobject *kobj,
 static int
 efivar_create_sysfs_entry(struct efivar_entry *new_var)
 {
-	int i, short_name_size;
+	int short_name_size;
 	char *short_name;
-	unsigned long variable_name_size;
-	efi_char16_t *variable_name;
+	unsigned long utf8_name_size;
+	efi_char16_t *variable_name = new_var->var.VariableName;
 	int ret;
 
-	variable_name = new_var->var.VariableName;
-	variable_name_size = ucs2_strlen(variable_name) * sizeof(efi_char16_t);
-
 	/*
-	 * Length of the variable bytes in ASCII, plus the '-' separator,
+	 * Length of the variable bytes in UTF8, plus the '-' separator,
 	 * plus the GUID, plus trailing NUL
 	 */
-	short_name_size = variable_name_size / sizeof(efi_char16_t)
-				+ 1 + EFI_VARIABLE_GUID_LEN + 1;
-
-	short_name = kzalloc(short_name_size, GFP_KERNEL);
+	utf8_name_size = ucs2_utf8size(variable_name);
+	short_name_size = utf8_name_size + 1 + EFI_VARIABLE_GUID_LEN + 1;
 
+	short_name = kmalloc(short_name_size, GFP_KERNEL);
 	if (!short_name)
 		return -ENOMEM;
 
-	/* Convert Unicode to normal chars (assume top bits are 0),
-	   ala UTF-8 */
-	for (i=0; i < (int)(variable_name_size / sizeof(efi_char16_t)); i++) {
-		short_name[i] = variable_name[i] & 0xFF;
-	}
+	ucs2_as_utf8(short_name, variable_name, short_name_size);
+
 	/* This is ugly, but necessary to separate one vendor's
 	   private variables from another's.         */
-
-	*(short_name + strlen(short_name)) = '-';
+	short_name[utf8_name_size] = '-';
 	efi_guid_to_str(&new_var->var.VendorGuid,
-			 short_name + strlen(short_name));
+			 short_name + utf8_name_size + 1);
 
 	new_var->kobj.kset = efivars_kset;
 
diff --git a/drivers/firmware/efi/libstub/Makefile b/drivers/firmware/efi/libstub/Makefile
index 3c0467d3688c..aaf9c0bab42e 100644
--- a/drivers/firmware/efi/libstub/Makefile
+++ b/drivers/firmware/efi/libstub/Makefile
@@ -8,7 +8,7 @@ cflags-$(CONFIG_X86_32)		:= -march=i386
 cflags-$(CONFIG_X86_64)		:= -mcmodel=small
 cflags-$(CONFIG_X86)		+= -m$(BITS) -D__KERNEL__ $(LINUX_INCLUDE) -O2 \
 				   -fPIC -fno-strict-aliasing -mno-red-zone \
-				   -mno-mmx -mno-sse -DDISABLE_BRANCH_PROFILING
+				   -mno-mmx -mno-sse
 
 cflags-$(CONFIG_ARM64)		:= $(subst -pg,,$(KBUILD_CFLAGS))
 cflags-$(CONFIG_ARM)		:= $(subst -pg,,$(KBUILD_CFLAGS)) \
@@ -16,12 +16,13 @@ cflags-$(CONFIG_ARM)		:= $(subst -pg,,$(KBUILD_CFLAGS)) \
 
 cflags-$(CONFIG_EFI_ARMSTUB)	+= -I$(srctree)/scripts/dtc/libfdt
 
-KBUILD_CFLAGS			:= $(cflags-y) \
+KBUILD_CFLAGS			:= $(cflags-y) -DDISABLE_BRANCH_PROFILING \
 				   $(call cc-option,-ffreestanding) \
 				   $(call cc-option,-fno-stack-protector)
 
 GCOV_PROFILE			:= n
 KASAN_SANITIZE			:= n
+UBSAN_SANITIZE			:= n
 
 lib-y				:= efi-stub-helper.o
 
@@ -34,6 +35,7 @@ $(obj)/lib-%.o: $(srctree)/lib/%.c FORCE
 lib-$(CONFIG_EFI_ARMSTUB)	+= arm-stub.o fdt.o string.o \
 				   $(patsubst %.c,lib-%.o,$(arm-deps))
 
+lib-$(CONFIG_ARM)		+= arm32-stub.o
 lib-$(CONFIG_ARM64)		+= arm64-stub.o
 CFLAGS_arm64-stub.o 		:= -DTEXT_OFFSET=$(TEXT_OFFSET)
 
@@ -67,3 +69,11 @@ quiet_cmd_stubcopy = STUBCPY $@
 		     $(OBJDUMP) -r $@ | grep $(STUBCOPY_RELOC-y)	\
 		     && (echo >&2 "$@: absolute symbol references not allowed in the EFI stub"; \
 			 rm -f $@; /bin/false); else /bin/false; fi
+
+#
+# ARM discards the .data section because it disallows r/w data in the
+# 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_RELOC-$(CONFIG_ARM)	:= R_ARM_ABS
diff --git a/drivers/firmware/efi/libstub/arm-stub.c b/drivers/firmware/efi/libstub/arm-stub.c
index 950c87f5d279..3397902e4040 100644
--- a/drivers/firmware/efi/libstub/arm-stub.c
+++ b/drivers/firmware/efi/libstub/arm-stub.c
@@ -303,8 +303,10 @@ fail:
  * The value chosen is the largest non-zero power of 2 suitable for this purpose
  * both on 32-bit and 64-bit ARM CPUs, to maximize the likelihood that it can
  * be mapped efficiently.
+ * Since 32-bit ARM could potentially execute with a 1G/3G user/kernel split,
+ * map everything below 1 GB.
  */
-#define EFI_RT_VIRTUAL_BASE	0x40000000
+#define EFI_RT_VIRTUAL_BASE	SZ_512M
 
 static int cmp_mem_desc(const void *l, const void *r)
 {
diff --git a/drivers/firmware/efi/libstub/arm32-stub.c b/drivers/firmware/efi/libstub/arm32-stub.c
new file mode 100644
index 000000000000..495ebd657e38
--- /dev/null
+++ b/drivers/firmware/efi/libstub/arm32-stub.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2013 Linaro Ltd;  <roy.franz@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.
+ *
+ */
+#include <linux/efi.h>
+#include <asm/efi.h>
+
+efi_status_t handle_kernel_image(efi_system_table_t *sys_table,
+				 unsigned long *image_addr,
+				 unsigned long *image_size,
+				 unsigned long *reserve_addr,
+				 unsigned long *reserve_size,
+				 unsigned long dram_base,
+				 efi_loaded_image_t *image)
+{
+	unsigned long nr_pages;
+	efi_status_t status;
+	/* Use alloc_addr to tranlsate between types */
+	efi_physical_addr_t alloc_addr;
+
+	/*
+	 * Verify that the DRAM base address is compatible with the ARM
+	 * boot protocol, which determines the base of DRAM by masking
+	 * off the low 27 bits of the address at which the zImage is
+	 * loaded. These assumptions are made by the decompressor,
+	 * before any memory map is available.
+	 */
+	dram_base = round_up(dram_base, SZ_128M);
+
+	/*
+	 * Reserve memory for the uncompressed kernel image. This is
+	 * all that prevents any future allocations from conflicting
+	 * with the kernel. Since we can't tell from the compressed
+	 * image how much DRAM the kernel actually uses (due to BSS
+	 * size uncertainty) we allocate the maximum possible size.
+	 * Do this very early, as prints can cause memory allocations
+	 * that may conflict with this.
+	 */
+	alloc_addr = dram_base;
+	*reserve_size = MAX_UNCOMP_KERNEL_SIZE;
+	nr_pages = round_up(*reserve_size, EFI_PAGE_SIZE) / EFI_PAGE_SIZE;
+	status = sys_table->boottime->allocate_pages(EFI_ALLOCATE_ADDRESS,
+						     EFI_LOADER_DATA,
+						     nr_pages, &alloc_addr);
+	if (status != EFI_SUCCESS) {
+		*reserve_size = 0;
+		pr_efi_err(sys_table, "Unable to allocate memory for uncompressed kernel.\n");
+		return status;
+	}
+	*reserve_addr = alloc_addr;
+
+	/*
+	 * Relocate the zImage, so that it appears in the lowest 128 MB
+	 * memory window.
+	 */
+	*image_size = image->image_size;
+	status = efi_relocate_kernel(sys_table, image_addr, *image_size,
+				     *image_size,
+				     dram_base + MAX_UNCOMP_KERNEL_SIZE, 0);
+	if (status != EFI_SUCCESS) {
+		pr_efi_err(sys_table, "Failed to relocate kernel.\n");
+		efi_free(sys_table, *reserve_size, *reserve_addr);
+		*reserve_size = 0;
+		return status;
+	}
+
+	/*
+	 * Check to see if we were able to allocate memory low enough
+	 * in memory. The kernel determines the base of DRAM from the
+	 * address at which the zImage is loaded.
+	 */
+	if (*image_addr + *image_size > dram_base + ZIMAGE_OFFSET_LIMIT) {
+		pr_efi_err(sys_table, "Failed to relocate kernel, no low memory available.\n");
+		efi_free(sys_table, *reserve_size, *reserve_addr);
+		*reserve_size = 0;
+		efi_free(sys_table, *image_size, *image_addr);
+		*image_size = 0;
+		return EFI_LOAD_ERROR;
+	}
+	return EFI_SUCCESS;
+}
diff --git a/drivers/firmware/efi/libstub/fdt.c b/drivers/firmware/efi/libstub/fdt.c
index b62e2f5dcab3..cf7b7d46302a 100644
--- a/drivers/firmware/efi/libstub/fdt.c
+++ b/drivers/firmware/efi/libstub/fdt.c
@@ -253,7 +253,7 @@ efi_status_t allocate_new_fdt_and_exit_boot(efi_system_table_t *sys_table,
 			sys_table->boottime->free_pool(memory_map);
 			new_fdt_size += EFI_PAGE_SIZE;
 		} else {
-			pr_efi_err(sys_table, "Unable to constuct new device tree.\n");
+			pr_efi_err(sys_table, "Unable to construct new device tree.\n");
 			goto fail_free_mmap;
 		}
 	}
diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
index 70a0fb10517f..7f2ea21c730d 100644
--- a/drivers/firmware/efi/vars.c
+++ b/drivers/firmware/efi/vars.c
@@ -165,67 +165,133 @@ validate_ascii_string(efi_char16_t *var_name, int match, u8 *buffer,
 }
 
 struct variable_validate {
+	efi_guid_t vendor;
 	char *name;
 	bool (*validate)(efi_char16_t *var_name, int match, u8 *data,
 			 unsigned long len);
 };
 
+/*
+ * This is the list of variables we need to validate, as well as the
+ * whitelist for what we think is safe not to default to immutable.
+ *
+ * If it has a validate() method that's not NULL, it'll go into the
+ * validation routine.  If not, it is assumed valid, but still used for
+ * whitelisting.
+ *
+ * Note that it's sorted by {vendor,name}, but globbed names must come after
+ * any other name with the same prefix.
+ */
 static const struct variable_validate variable_validate[] = {
-	{ "BootNext", validate_uint16 },
-	{ "BootOrder", validate_boot_order },
-	{ "DriverOrder", validate_boot_order },
-	{ "Boot*", validate_load_option },
-	{ "Driver*", validate_load_option },
-	{ "ConIn", validate_device_path },
-	{ "ConInDev", validate_device_path },
-	{ "ConOut", validate_device_path },
-	{ "ConOutDev", validate_device_path },
-	{ "ErrOut", validate_device_path },
-	{ "ErrOutDev", validate_device_path },
-	{ "Timeout", validate_uint16 },
-	{ "Lang", validate_ascii_string },
-	{ "PlatformLang", validate_ascii_string },
-	{ "", NULL },
+	{ EFI_GLOBAL_VARIABLE_GUID, "BootNext", validate_uint16 },
+	{ EFI_GLOBAL_VARIABLE_GUID, "BootOrder", validate_boot_order },
+	{ EFI_GLOBAL_VARIABLE_GUID, "Boot*", validate_load_option },
+	{ EFI_GLOBAL_VARIABLE_GUID, "DriverOrder", validate_boot_order },
+	{ EFI_GLOBAL_VARIABLE_GUID, "Driver*", validate_load_option },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ConIn", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ConInDev", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ConOut", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ConOutDev", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ErrOut", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "ErrOutDev", validate_device_path },
+	{ EFI_GLOBAL_VARIABLE_GUID, "Lang", validate_ascii_string },
+	{ EFI_GLOBAL_VARIABLE_GUID, "OsIndications", NULL },
+	{ EFI_GLOBAL_VARIABLE_GUID, "PlatformLang", validate_ascii_string },
+	{ EFI_GLOBAL_VARIABLE_GUID, "Timeout", validate_uint16 },
+	{ LINUX_EFI_CRASH_GUID, "*", NULL },
+	{ NULL_GUID, "", NULL },
 };
 
+static bool
+variable_matches(const char *var_name, size_t len, const char *match_name,
+		 int *match)
+{
+	for (*match = 0; ; (*match)++) {
+		char c = match_name[*match];
+		char u = var_name[*match];
+
+		/* Wildcard in the matching name means we've matched */
+		if (c == '*')
+			return true;
+
+		/* Case sensitive match */
+		if (!c && *match == len)
+			return true;
+
+		if (c != u)
+			return false;
+
+		if (!c)
+			return true;
+	}
+	return true;
+}
+
 bool
-efivar_validate(efi_char16_t *var_name, u8 *data, unsigned long len)
+efivar_validate(efi_guid_t vendor, efi_char16_t *var_name, u8 *data,
+		unsigned long data_size)
 {
 	int i;
-	u16 *unicode_name = var_name;
+	unsigned long utf8_size;
+	u8 *utf8_name;
 
-	for (i = 0; variable_validate[i].validate != NULL; i++) {
-		const char *name = variable_validate[i].name;
-		int match;
+	utf8_size = ucs2_utf8size(var_name);
+	utf8_name = kmalloc(utf8_size + 1, GFP_KERNEL);
+	if (!utf8_name)
+		return false;
 
-		for (match = 0; ; match++) {
-			char c = name[match];
-			u16 u = unicode_name[match];
+	ucs2_as_utf8(utf8_name, var_name, utf8_size);
+	utf8_name[utf8_size] = '\0';
 
-			/* All special variables are plain ascii */
-			if (u > 127)
-				return true;
+	for (i = 0; variable_validate[i].name[0] != '\0'; i++) {
+		const char *name = variable_validate[i].name;
+		int match = 0;
 
-			/* Wildcard in the matching name means we've matched */
-			if (c == '*')
-				return variable_validate[i].validate(var_name,
-							     match, data, len);
+		if (efi_guidcmp(vendor, variable_validate[i].vendor))
+			continue;
 
-			/* Case sensitive match */
-			if (c != u)
+		if (variable_matches(utf8_name, utf8_size+1, name, &match)) {
+			if (variable_validate[i].validate == NULL)
 				break;
-
-			/* Reached the end of the string while matching */
-			if (!c)
-				return variable_validate[i].validate(var_name,
-							     match, data, len);
+			kfree(utf8_name);
+			return variable_validate[i].validate(var_name, match,
+							     data, data_size);
 		}
 	}
-
+	kfree(utf8_name);
 	return true;
 }
 EXPORT_SYMBOL_GPL(efivar_validate);
 
+bool
+efivar_variable_is_removable(efi_guid_t vendor, const char *var_name,
+			     size_t len)
+{
+	int i;
+	bool found = false;
+	int match = 0;
+
+	/*
+	 * Check if our variable is in the validated variables list
+	 */
+	for (i = 0; variable_validate[i].name[0] != '\0'; i++) {
+		if (efi_guidcmp(variable_validate[i].vendor, vendor))
+			continue;
+
+		if (variable_matches(var_name, len,
+				     variable_validate[i].name, &match)) {
+			found = true;
+			break;
+		}
+	}
+
+	/*
+	 * If it's in our list, it is removable.
+	 */
+	return found;
+}
+EXPORT_SYMBOL_GPL(efivar_variable_is_removable);
+
 static efi_status_t
 check_var_size(u32 attributes, unsigned long size)
 {
@@ -852,7 +918,7 @@ int efivar_entry_set_get_size(struct efivar_entry *entry, u32 attributes,
 
 	*set = false;
 
-	if (efivar_validate(name, data, *size) == false)
+	if (efivar_validate(*vendor, name, data, *size) == false)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index d24f35d74b27..f25cd79c8a79 100644
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -13,6 +13,7 @@
 
 #define pr_fmt(fmt) "psci: " fmt
 
+#include <linux/arm-smccc.h>
 #include <linux/errno.h>
 #include <linux/linkage.h>
 #include <linux/of.h>
@@ -58,8 +59,6 @@ struct psci_operations psci_ops;
 
 typedef unsigned long (psci_fn)(unsigned long, unsigned long,
 				unsigned long, unsigned long);
-asmlinkage psci_fn __invoke_psci_fn_hvc;
-asmlinkage psci_fn __invoke_psci_fn_smc;
 static psci_fn *invoke_psci_fn;
 
 enum psci_function {
@@ -107,6 +106,26 @@ bool psci_power_state_is_valid(u32 state)
 	return !(state & ~valid_mask);
 }
 
+static unsigned long __invoke_psci_fn_hvc(unsigned long function_id,
+			unsigned long arg0, unsigned long arg1,
+			unsigned long arg2)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_hvc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+	return res.a0;
+}
+
+static unsigned long __invoke_psci_fn_smc(unsigned long function_id,
+			unsigned long arg0, unsigned long arg1,
+			unsigned long arg2)
+{
+	struct arm_smccc_res res;
+
+	arm_smccc_smc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+	return res.a0;
+}
+
 static int psci_to_linux_errno(int errno)
 {
 	switch (errno) {
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index b18bea08ff25..c88dd24a4b1f 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -134,8 +134,8 @@ config GPIO_BCM_KONA
 
 config GPIO_BRCMSTB
 	tristate "BRCMSTB GPIO support"
-	default y if ARCH_BRCMSTB
-	depends on OF_GPIO && (ARCH_BRCMSTB || COMPILE_TEST)
+	default y if (ARCH_BRCMSTB || BMIPS_GENERIC)
+	depends on OF_GPIO && (ARCH_BRCMSTB || BMIPS_GENERIC || COMPILE_TEST)
 	select GPIO_GENERIC
 	select GPIOLIB_IRQCHIP
 	help
@@ -344,13 +344,6 @@ config GPIO_RCAR
 	help
 	  Say yes here to support GPIO on Renesas R-Car SoCs.
 
-config GPIO_SAMSUNG
-	bool
-	depends on PLAT_SAMSUNG
-	help
-	  Legacy GPIO support. Use only for platforms without support for
-	  pinctrl.
-
 config GPIO_SPEAR_SPICS
 	bool "ST SPEAr13xx SPI Chip Select as GPIO support"
 	depends on PLAT_SPEAR
@@ -496,8 +489,21 @@ menu "Port-mapped I/O GPIO drivers"
 
 config GPIO_104_IDIO_16
 	tristate "ACCES 104-IDIO-16 GPIO support"
+	select GPIOLIB_IRQCHIP
 	help
-	  Enables GPIO support for the ACCES 104-IDIO-16 family.
+	  Enables GPIO support for the ACCES 104-IDIO-16 family. The base port
+	  address for the device may be set via the idio_16_base module
+	  parameter. The interrupt line number for the device may be set via the
+	  idio_16_irq module parameter.
+
+config GPIO_104_IDI_48
+	tristate "ACCES 104-IDI-48 GPIO support"
+	select GPIOLIB_IRQCHIP
+	help
+	  Enables GPIO support for the ACCES 104-IDI-48 family. The base port
+	  address for the device may be configured via the idi_48_base module
+	  parameter. The interrupt line number for the device may be configured
+	  via the idi_48_irq module parameter.
 
 config GPIO_F7188X
 	tristate "F71869, F71869A, F71882FG and F71889F GPIO support"
@@ -907,7 +913,6 @@ menu "PCI GPIO expanders"
 
 config GPIO_AMD8111
 	tristate "AMD 8111 GPIO driver"
-	depends on PCI
 	help
 	  The AMD 8111 south bridge contains 32 GPIO pins which can be used.
 
@@ -919,7 +924,7 @@ config GPIO_AMD8111
 
 config GPIO_BT8XX
 	tristate "BT8XX GPIO abuser"
-	depends on PCI && VIDEO_BT848=n
+	depends on VIDEO_BT848=n
 	help
 	  The BT8xx frame grabber chip has 24 GPIO pins that can be abused
 	  as a cheap PCI GPIO card.
@@ -935,14 +940,13 @@ config GPIO_BT8XX
 
 config GPIO_INTEL_MID
 	bool "Intel Mid GPIO support"
-	depends on PCI && X86
+	depends on X86
 	select GPIOLIB_IRQCHIP
 	help
 	  Say Y here to support Intel Mid GPIO.
 
 config GPIO_ML_IOH
 	tristate "OKI SEMICONDUCTOR ML7213 IOH GPIO support"
-	depends on PCI
 	select GENERIC_IRQ_CHIP
 	help
 	  ML7213 is companion chip for Intel Atom E6xx series.
@@ -952,7 +956,7 @@ config GPIO_ML_IOH
 
 config GPIO_PCH
 	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7223/ML7831) GPIO"
-	depends on PCI && (X86_32 || COMPILE_TEST)
+	depends on X86_32 || MIPS || COMPILE_TEST
 	select GENERIC_IRQ_CHIP
 	help
 	  This driver is for PCH(Platform controller Hub) GPIO of Intel Topcliff
@@ -968,7 +972,6 @@ config GPIO_PCH
 
 config GPIO_RDC321X
 	tristate "RDC R-321x GPIO support"
-	depends on PCI
 	select MFD_CORE
 	select MFD_RDC321X
 	help
@@ -977,7 +980,7 @@ config GPIO_RDC321X
 
 config GPIO_SODAVILLE
 	bool "Intel Sodaville GPIO support"
-	depends on X86 && PCI && OF
+	depends on X86 && OF
 	select GPIO_GENERIC
 	select GENERIC_IRQ_CHIP
 	help
@@ -1028,7 +1031,7 @@ menu "USB GPIO expanders"
 
 config GPIO_VIPERBOARD
 	tristate "Viperboard GPIO a & b support"
-	depends on MFD_VIPERBOARD && USB
+	depends on MFD_VIPERBOARD
 	help
 	  Say yes here to access the GPIO signals of Nano River
 	  Technologies Viperboard. There are two GPIO chips on the
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 986dbd838cea..ece7d7cbdc80 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_GPIO_ACPI)		+= gpiolib-acpi.o
 obj-$(CONFIG_GPIO_GENERIC)	+= gpio-generic.o
 
 obj-$(CONFIG_GPIO_104_IDIO_16)	+= gpio-104-idio-16.o
+obj-$(CONFIG_GPIO_104_IDI_48)	+= gpio-104-idi-48.o
 obj-$(CONFIG_GPIO_74X164)	+= gpio-74x164.o
 obj-$(CONFIG_GPIO_74XX_MMIO)	+= gpio-74xx-mmio.o
 obj-$(CONFIG_GPIO_ADNP)		+= gpio-adnp.o
@@ -79,7 +80,6 @@ obj-$(CONFIG_GPIO_PXA)		+= gpio-pxa.o
 obj-$(CONFIG_GPIO_RC5T583)	+= gpio-rc5t583.o
 obj-$(CONFIG_GPIO_RDC321X)	+= gpio-rdc321x.o
 obj-$(CONFIG_GPIO_RCAR)		+= gpio-rcar.o
-obj-$(CONFIG_GPIO_SAMSUNG)	+= gpio-samsung.o
 obj-$(CONFIG_ARCH_SA1100)	+= gpio-sa1100.o
 obj-$(CONFIG_GPIO_SCH)		+= gpio-sch.o
 obj-$(CONFIG_GPIO_SCH311X)	+= gpio-sch311x.o
diff --git a/drivers/gpio/gpio-104-idi-48.c b/drivers/gpio/gpio-104-idi-48.c
new file mode 100644
index 000000000000..52eed328ce99
--- /dev/null
+++ b/drivers/gpio/gpio-104-idi-48.c
@@ -0,0 +1,343 @@
+/*
+ * GPIO driver for the ACCES 104-IDI-48 family
+ * Copyright (C) 2015 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.
+ */
+#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/interrupt.h>
+#include <linux/irqdesc.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+static unsigned idi_48_base;
+module_param(idi_48_base, uint, 0);
+MODULE_PARM_DESC(idi_48_base, "ACCES 104-IDI-48 base address");
+static unsigned idi_48_irq;
+module_param(idi_48_irq, uint, 0);
+MODULE_PARM_DESC(idi_48_irq, "ACCES 104-IDI-48 interrupt line number");
+
+/**
+ * struct idi_48_gpio - GPIO device private data structure
+ * @chip:	instance of the gpio_chip
+ * @lock:	synchronization lock to prevent I/O race conditions
+ * @ack_lock:	synchronization lock to prevent IRQ handler race conditions
+ * @irq_mask:	input bits affected by interrupts
+ * @base:	base port address of the GPIO device
+ * @extent:	extent of port address region of the GPIO device
+ * @irq:	Interrupt line number
+ * @cos_enb:	Change-Of-State IRQ enable boundaries mask
+ */
+struct idi_48_gpio {
+	struct gpio_chip chip;
+	spinlock_t lock;
+	spinlock_t ack_lock;
+	unsigned char irq_mask[6];
+	unsigned base;
+	unsigned extent;
+	unsigned irq;
+	unsigned char cos_enb;
+};
+
+static int idi_48_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
+{
+	return 1;
+}
+
+static int idi_48_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
+{
+	return 0;
+}
+
+static int idi_48_gpio_get(struct gpio_chip *chip, unsigned offset)
+{
+	struct idi_48_gpio *const idi48gpio = gpiochip_get_data(chip);
+	unsigned i;
+	const unsigned register_offset[6] = { 0, 1, 2, 4, 5, 6 };
+	unsigned base_offset;
+	unsigned mask;
+
+	for (i = 0; i < 48; i += 8)
+		if (offset < i + 8) {
+			base_offset = register_offset[i / 8];
+			mask = BIT(offset - i);
+
+			return !!(inb(idi48gpio->base + base_offset) & mask);
+		}
+
+	/* The following line should never execute since offset < 48 */
+	return 0;
+}
+
+static void idi_48_irq_ack(struct irq_data *data)
+{
+}
+
+static void idi_48_irq_mask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct idi_48_gpio *const idi48gpio = gpiochip_get_data(chip);
+	const unsigned offset = irqd_to_hwirq(data);
+	unsigned i;
+	unsigned mask;
+	unsigned boundary;
+	unsigned long flags;
+
+	for (i = 0; i < 48; i += 8)
+		if (offset < i + 8) {
+			mask = BIT(offset - i);
+			boundary = i / 8;
+
+			idi48gpio->irq_mask[boundary] &= ~mask;
+
+			if (!idi48gpio->irq_mask[boundary]) {
+				idi48gpio->cos_enb &= ~BIT(boundary);
+
+				spin_lock_irqsave(&idi48gpio->lock, flags);
+
+				outb(idi48gpio->cos_enb, idi48gpio->base + 7);
+
+				spin_unlock_irqrestore(&idi48gpio->lock, flags);
+			}
+
+			return;
+		}
+}
+
+static void idi_48_irq_unmask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct idi_48_gpio *const idi48gpio = gpiochip_get_data(chip);
+	const unsigned offset = irqd_to_hwirq(data);
+	unsigned i;
+	unsigned mask;
+	unsigned boundary;
+	unsigned prev_irq_mask;
+	unsigned long flags;
+
+	for (i = 0; i < 48; i += 8)
+		if (offset < i + 8) {
+			mask = BIT(offset - i);
+			boundary = i / 8;
+			prev_irq_mask = idi48gpio->irq_mask[boundary];
+
+			idi48gpio->irq_mask[boundary] |= mask;
+
+			if (!prev_irq_mask) {
+				idi48gpio->cos_enb |= BIT(boundary);
+
+				spin_lock_irqsave(&idi48gpio->lock, flags);
+
+				outb(idi48gpio->cos_enb, idi48gpio->base + 7);
+
+				spin_unlock_irqrestore(&idi48gpio->lock, flags);
+			}
+
+			return;
+		}
+}
+
+static int idi_48_irq_set_type(struct irq_data *data, unsigned flow_type)
+{
+	/* The only valid irq types are none and both-edges */
+	if (flow_type != IRQ_TYPE_NONE &&
+		(flow_type & IRQ_TYPE_EDGE_BOTH) != IRQ_TYPE_EDGE_BOTH)
+		return -EINVAL;
+
+	return 0;
+}
+
+static struct irq_chip idi_48_irqchip = {
+	.name = "104-idi-48",
+	.irq_ack = idi_48_irq_ack,
+	.irq_mask = idi_48_irq_mask,
+	.irq_unmask = idi_48_irq_unmask,
+	.irq_set_type = idi_48_irq_set_type
+};
+
+static irqreturn_t idi_48_irq_handler(int irq, void *dev_id)
+{
+	struct idi_48_gpio *const idi48gpio = dev_id;
+	unsigned long cos_status;
+	unsigned long boundary;
+	unsigned long irq_mask;
+	unsigned long bit_num;
+	unsigned long gpio;
+	struct gpio_chip *const chip = &idi48gpio->chip;
+
+	spin_lock(&idi48gpio->ack_lock);
+
+	spin_lock(&idi48gpio->lock);
+
+	cos_status = inb(idi48gpio->base + 7);
+
+	spin_unlock(&idi48gpio->lock);
+
+	/* IRQ Status (bit 6) is active low (0 = IRQ generated by device) */
+	if (cos_status & BIT(6)) {
+		spin_unlock(&idi48gpio->ack_lock);
+		return IRQ_NONE;
+	}
+
+	/* Bit 0-5 indicate which Change-Of-State boundary triggered the IRQ */
+	cos_status &= 0x3F;
+
+	for_each_set_bit(boundary, &cos_status, 6) {
+		irq_mask = idi48gpio->irq_mask[boundary];
+
+		for_each_set_bit(bit_num, &irq_mask, 8) {
+			gpio = bit_num + boundary * 8;
+
+			generic_handle_irq(irq_find_mapping(chip->irqdomain,
+				gpio));
+		}
+	}
+
+	spin_unlock(&idi48gpio->ack_lock);
+
+	return IRQ_HANDLED;
+}
+
+static int __init idi_48_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct idi_48_gpio *idi48gpio;
+	const unsigned base = idi_48_base;
+	const unsigned extent = 8;
+	const char *const name = dev_name(dev);
+	int err;
+	const unsigned irq = idi_48_irq;
+
+	idi48gpio = devm_kzalloc(dev, sizeof(*idi48gpio), GFP_KERNEL);
+	if (!idi48gpio)
+		return -ENOMEM;
+
+	if (!request_region(base, extent, name)) {
+		dev_err(dev, "Unable to lock %s port addresses (0x%X-0x%X)\n",
+			name, base, base + extent);
+		err = -EBUSY;
+		goto err_lock_io_port;
+	}
+
+	idi48gpio->chip.label = name;
+	idi48gpio->chip.parent = dev;
+	idi48gpio->chip.owner = THIS_MODULE;
+	idi48gpio->chip.base = -1;
+	idi48gpio->chip.ngpio = 48;
+	idi48gpio->chip.get_direction = idi_48_gpio_get_direction;
+	idi48gpio->chip.direction_input = idi_48_gpio_direction_input;
+	idi48gpio->chip.get = idi_48_gpio_get;
+	idi48gpio->base = base;
+	idi48gpio->extent = extent;
+	idi48gpio->irq = irq;
+
+	spin_lock_init(&idi48gpio->lock);
+
+	dev_set_drvdata(dev, idi48gpio);
+
+	err = gpiochip_add_data(&idi48gpio->chip, idi48gpio);
+	if (err) {
+		dev_err(dev, "GPIO registering failed (%d)\n", err);
+		goto err_gpio_register;
+	}
+
+	/* Disable IRQ by default */
+	outb(0, base + 7);
+	inb(base + 7);
+
+	err = gpiochip_irqchip_add(&idi48gpio->chip, &idi_48_irqchip, 0,
+		handle_edge_irq, IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(dev, "Could not add irqchip (%d)\n", err);
+		goto err_gpiochip_irqchip_add;
+	}
+
+	err = request_irq(irq, idi_48_irq_handler, 0, name, idi48gpio);
+	if (err) {
+		dev_err(dev, "IRQ handler registering failed (%d)\n", err);
+		goto err_request_irq;
+	}
+
+	return 0;
+
+err_request_irq:
+err_gpiochip_irqchip_add:
+	gpiochip_remove(&idi48gpio->chip);
+err_gpio_register:
+	release_region(base, extent);
+err_lock_io_port:
+	return err;
+}
+
+static int idi_48_remove(struct platform_device *pdev)
+{
+	struct idi_48_gpio *const idi48gpio = platform_get_drvdata(pdev);
+
+	free_irq(idi48gpio->irq, idi48gpio);
+	gpiochip_remove(&idi48gpio->chip);
+	release_region(idi48gpio->base, idi48gpio->extent);
+
+	return 0;
+}
+
+static struct platform_device *idi_48_device;
+
+static struct platform_driver idi_48_driver = {
+	.driver = {
+		.name = "104-idi-48"
+	},
+	.remove = idi_48_remove
+};
+
+static void __exit idi_48_exit(void)
+{
+	platform_device_unregister(idi_48_device);
+	platform_driver_unregister(&idi_48_driver);
+}
+
+static int __init idi_48_init(void)
+{
+	int err;
+
+	idi_48_device = platform_device_alloc(idi_48_driver.driver.name, -1);
+	if (!idi_48_device)
+		return -ENOMEM;
+
+	err = platform_device_add(idi_48_device);
+	if (err)
+		goto err_platform_device;
+
+	err = platform_driver_probe(&idi_48_driver, idi_48_probe);
+	if (err)
+		goto err_platform_driver;
+
+	return 0;
+
+err_platform_driver:
+	platform_device_del(idi_48_device);
+err_platform_device:
+	platform_device_put(idi_48_device);
+	return err;
+}
+
+module_init(idi_48_init);
+module_exit(idi_48_exit);
+
+MODULE_AUTHOR("William Breathitt Gray <vilhelm.gray@gmail.com>");
+MODULE_DESCRIPTION("ACCES 104-IDI-48 GPIO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-104-idio-16.c b/drivers/gpio/gpio-104-idio-16.c
index 5400d7d4d8fd..4d69b50b2d84 100644
--- a/drivers/gpio/gpio-104-idio-16.c
+++ b/drivers/gpio/gpio-104-idio-16.c
@@ -11,11 +11,14 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  * General Public License for more details.
  */
+#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/interrupt.h>
+#include <linux/irqdesc.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
@@ -25,20 +28,27 @@
 static unsigned idio_16_base;
 module_param(idio_16_base, uint, 0);
 MODULE_PARM_DESC(idio_16_base, "ACCES 104-IDIO-16 base address");
+static unsigned idio_16_irq;
+module_param(idio_16_irq, uint, 0);
+MODULE_PARM_DESC(idio_16_irq, "ACCES 104-IDIO-16 interrupt line number");
 
 /**
  * struct idio_16_gpio - GPIO device private data structure
  * @chip:	instance of the gpio_chip
- * @lock:	synchronization lock to prevent gpio_set race conditions
+ * @lock:	synchronization lock to prevent I/O race conditions
+ * @irq_mask:	I/O bits affected by interrupts
  * @base:	base port address of the GPIO device
  * @extent:	extent of port address region of the GPIO device
+ * @irq:	Interrupt line number
  * @out_state:	output bits state
  */
 struct idio_16_gpio {
 	struct gpio_chip chip;
 	spinlock_t lock;
+	unsigned long irq_mask;
 	unsigned base;
 	unsigned extent;
+	unsigned irq;
 	unsigned out_state;
 };
 
@@ -62,29 +72,24 @@ static int idio_16_gpio_direction_output(struct gpio_chip *chip,
 	return 0;
 }
 
-static struct idio_16_gpio *to_idio16gpio(struct gpio_chip *gc)
-{
-	return container_of(gc, struct idio_16_gpio, chip);
-}
-
 static int idio_16_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct idio_16_gpio *const idio16gpio = to_idio16gpio(chip);
-	const unsigned BIT_MASK = 1U << (offset-16);
+	struct idio_16_gpio *const idio16gpio = gpiochip_get_data(chip);
+	const unsigned mask = BIT(offset-16);
 
 	if (offset < 16)
 		return -EINVAL;
 
 	if (offset < 24)
-		return !!(inb(idio16gpio->base + 1) & BIT_MASK);
+		return !!(inb(idio16gpio->base + 1) & mask);
 
-	return !!(inb(idio16gpio->base + 5) & (BIT_MASK>>8));
+	return !!(inb(idio16gpio->base + 5) & (mask>>8));
 }
 
 static void idio_16_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct idio_16_gpio *const idio16gpio = to_idio16gpio(chip);
-	const unsigned BIT_MASK = 1U << offset;
+	struct idio_16_gpio *const idio16gpio = gpiochip_get_data(chip);
+	const unsigned mask = BIT(offset);
 	unsigned long flags;
 
 	if (offset > 15)
@@ -93,9 +98,9 @@ static void idio_16_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	spin_lock_irqsave(&idio16gpio->lock, flags);
 
 	if (value)
-		idio16gpio->out_state |= BIT_MASK;
+		idio16gpio->out_state |= mask;
 	else
-		idio16gpio->out_state &= ~BIT_MASK;
+		idio16gpio->out_state &= ~mask;
 
 	if (offset > 7)
 		outb(idio16gpio->out_state >> 8, idio16gpio->base + 4);
@@ -105,29 +110,106 @@ static void idio_16_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	spin_unlock_irqrestore(&idio16gpio->lock, flags);
 }
 
+static void idio_16_irq_ack(struct irq_data *data)
+{
+}
+
+static void idio_16_irq_mask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct idio_16_gpio *const idio16gpio = gpiochip_get_data(chip);
+	const unsigned long mask = BIT(irqd_to_hwirq(data));
+	unsigned long flags;
+
+	idio16gpio->irq_mask &= ~mask;
+
+	if (!idio16gpio->irq_mask) {
+		spin_lock_irqsave(&idio16gpio->lock, flags);
+
+		outb(0, idio16gpio->base + 2);
+
+		spin_unlock_irqrestore(&idio16gpio->lock, flags);
+	}
+}
+
+static void idio_16_irq_unmask(struct irq_data *data)
+{
+	struct gpio_chip *chip = irq_data_get_irq_chip_data(data);
+	struct idio_16_gpio *const idio16gpio = gpiochip_get_data(chip);
+	const unsigned long mask = BIT(irqd_to_hwirq(data));
+	const unsigned long prev_irq_mask = idio16gpio->irq_mask;
+	unsigned long flags;
+
+	idio16gpio->irq_mask |= mask;
+
+	if (!prev_irq_mask) {
+		spin_lock_irqsave(&idio16gpio->lock, flags);
+
+		inb(idio16gpio->base + 2);
+
+		spin_unlock_irqrestore(&idio16gpio->lock, flags);
+	}
+}
+
+static int idio_16_irq_set_type(struct irq_data *data, unsigned flow_type)
+{
+	/* The only valid irq types are none and both-edges */
+	if (flow_type != IRQ_TYPE_NONE &&
+		(flow_type & IRQ_TYPE_EDGE_BOTH) != IRQ_TYPE_EDGE_BOTH)
+		return -EINVAL;
+
+	return 0;
+}
+
+static struct irq_chip idio_16_irqchip = {
+	.name = "104-idio-16",
+	.irq_ack = idio_16_irq_ack,
+	.irq_mask = idio_16_irq_mask,
+	.irq_unmask = idio_16_irq_unmask,
+	.irq_set_type = idio_16_irq_set_type
+};
+
+static irqreturn_t idio_16_irq_handler(int irq, void *dev_id)
+{
+	struct idio_16_gpio *const idio16gpio = dev_id;
+	struct gpio_chip *const chip = &idio16gpio->chip;
+	int gpio;
+
+	for_each_set_bit(gpio, &idio16gpio->irq_mask, chip->ngpio)
+		generic_handle_irq(irq_find_mapping(chip->irqdomain, gpio));
+
+	spin_lock(&idio16gpio->lock);
+
+	outb(0, idio16gpio->base + 1);
+
+	spin_unlock(&idio16gpio->lock);
+
+	return IRQ_HANDLED;
+}
+
 static int __init idio_16_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct idio_16_gpio *idio16gpio;
+	const unsigned base = idio_16_base;
+	const unsigned extent = 8;
+	const char *const name = dev_name(dev);
 	int err;
-
-	const unsigned BASE = idio_16_base;
-	const unsigned EXTENT = 8;
-	const char *const NAME = dev_name(dev);
+	const unsigned irq = idio_16_irq;
 
 	idio16gpio = devm_kzalloc(dev, sizeof(*idio16gpio), GFP_KERNEL);
 	if (!idio16gpio)
 		return -ENOMEM;
 
-	if (!request_region(BASE, EXTENT, NAME)) {
+	if (!request_region(base, extent, name)) {
 		dev_err(dev, "Unable to lock %s port addresses (0x%X-0x%X)\n",
-			NAME, BASE, BASE + EXTENT);
+			name, base, base + extent);
 		err = -EBUSY;
 		goto err_lock_io_port;
 	}
 
-	idio16gpio->chip.label = NAME;
-	idio16gpio->chip.dev = dev;
+	idio16gpio->chip.label = name;
+	idio16gpio->chip.parent = dev;
 	idio16gpio->chip.owner = THIS_MODULE;
 	idio16gpio->chip.base = -1;
 	idio16gpio->chip.ngpio = 32;
@@ -136,24 +218,45 @@ static int __init idio_16_probe(struct platform_device *pdev)
 	idio16gpio->chip.direction_output = idio_16_gpio_direction_output;
 	idio16gpio->chip.get = idio_16_gpio_get;
 	idio16gpio->chip.set = idio_16_gpio_set;
-	idio16gpio->base = BASE;
-	idio16gpio->extent = EXTENT;
+	idio16gpio->base = base;
+	idio16gpio->extent = extent;
+	idio16gpio->irq = irq;
 	idio16gpio->out_state = 0xFFFF;
 
 	spin_lock_init(&idio16gpio->lock);
 
 	dev_set_drvdata(dev, idio16gpio);
 
-	err = gpiochip_add(&idio16gpio->chip);
+	err = gpiochip_add_data(&idio16gpio->chip, idio16gpio);
 	if (err) {
 		dev_err(dev, "GPIO registering failed (%d)\n", err);
 		goto err_gpio_register;
 	}
 
+	/* Disable IRQ by default */
+	outb(0, base + 2);
+	outb(0, base + 1);
+
+	err = gpiochip_irqchip_add(&idio16gpio->chip, &idio_16_irqchip, 0,
+		handle_edge_irq, IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(dev, "Could not add irqchip (%d)\n", err);
+		goto err_gpiochip_irqchip_add;
+	}
+
+	err = request_irq(irq, idio_16_irq_handler, 0, name, idio16gpio);
+	if (err) {
+		dev_err(dev, "IRQ handler registering failed (%d)\n", err);
+		goto err_request_irq;
+	}
+
 	return 0;
 
+err_request_irq:
+err_gpiochip_irqchip_add:
+	gpiochip_remove(&idio16gpio->chip);
 err_gpio_register:
-	release_region(BASE, EXTENT);
+	release_region(base, extent);
 err_lock_io_port:
 	return err;
 }
@@ -162,6 +265,7 @@ static int idio_16_remove(struct platform_device *pdev)
 {
 	struct idio_16_gpio *const idio16gpio = platform_get_drvdata(pdev);
 
+	free_irq(idio16gpio->irq, idio16gpio);
 	gpiochip_remove(&idio16gpio->chip);
 	release_region(idio16gpio->base, idio16gpio->extent);
 
diff --git a/drivers/gpio/gpio-74x164.c b/drivers/gpio/gpio-74x164.c
index 60172f835d15..c81224ff2dca 100644
--- a/drivers/gpio/gpio-74x164.c
+++ b/drivers/gpio/gpio-74x164.c
@@ -20,56 +20,34 @@
 #define GEN_74X164_NUMBER_GPIOS	8
 
 struct gen_74x164_chip {
-	u8			*buffer;
 	struct gpio_chip	gpio_chip;
 	struct mutex		lock;
 	u32			registers;
-};
-
-static struct gen_74x164_chip *gpio_to_74x164_chip(struct gpio_chip *gc)
-{
-	return container_of(gc, struct gen_74x164_chip, gpio_chip);
-}
-
-static int __gen_74x164_write_config(struct gen_74x164_chip *chip)
-{
-	struct spi_device *spi = to_spi_device(chip->gpio_chip.dev);
-	struct spi_message message;
-	struct spi_transfer *msg_buf;
-	int i, ret = 0;
-
-	msg_buf = kzalloc(chip->registers * sizeof(struct spi_transfer),
-			GFP_KERNEL);
-	if (!msg_buf)
-		return -ENOMEM;
-
-	spi_message_init(&message);
-
 	/*
 	 * Since the registers are chained, every byte sent will make
 	 * the previous byte shift to the next register in the
-	 * chain. Thus, the first byte send will end up in the last
+	 * chain. Thus, the first byte sent will end up in the last
 	 * register at the end of the transfer. So, to have a logical
-	 * numbering, send the bytes in reverse order so that the last
-	 * byte of the buffer will end up in the last register.
+	 * numbering, store the bytes in reverse order.
 	 */
-	for (i = chip->registers - 1; i >= 0; i--) {
-		msg_buf[i].tx_buf = chip->buffer + i;
-		msg_buf[i].len = sizeof(u8);
-		spi_message_add_tail(msg_buf + i, &message);
-	}
-
-	ret = spi_sync(spi, &message);
+	u8			buffer[0];
+};
 
-	kfree(msg_buf);
+static int __gen_74x164_write_config(struct gen_74x164_chip *chip)
+{
+	struct spi_transfer xfer = {
+		.tx_buf = chip->buffer,
+		.len = chip->registers,
+	};
 
-	return ret;
+	return spi_sync_transfer(to_spi_device(chip->gpio_chip.parent),
+				 &xfer, 1);
 }
 
 static int gen_74x164_get_value(struct gpio_chip *gc, unsigned offset)
 {
-	struct gen_74x164_chip *chip = gpio_to_74x164_chip(gc);
-	u8 bank = offset / 8;
+	struct gen_74x164_chip *chip = gpiochip_get_data(gc);
+	u8 bank = chip->registers - 1 - offset / 8;
 	u8 pin = offset % 8;
 	int ret;
 
@@ -83,8 +61,8 @@ static int gen_74x164_get_value(struct gpio_chip *gc, unsigned offset)
 static void gen_74x164_set_value(struct gpio_chip *gc,
 		unsigned offset, int val)
 {
-	struct gen_74x164_chip *chip = gpio_to_74x164_chip(gc);
-	u8 bank = offset / 8;
+	struct gen_74x164_chip *chip = gpiochip_get_data(gc);
+	u8 bank = chip->registers - 1 - offset / 8;
 	u8 pin = offset % 8;
 
 	mutex_lock(&chip->lock);
@@ -107,6 +85,7 @@ static int gen_74x164_direction_output(struct gpio_chip *gc,
 static int gen_74x164_probe(struct spi_device *spi)
 {
 	struct gen_74x164_chip *chip;
+	u32 nregs;
 	int ret;
 
 	/*
@@ -118,7 +97,14 @@ static int gen_74x164_probe(struct spi_device *spi)
 	if (ret < 0)
 		return ret;
 
-	chip = devm_kzalloc(&spi->dev, sizeof(*chip), GFP_KERNEL);
+	if (of_property_read_u32(spi->dev.of_node, "registers-number",
+				 &nregs)) {
+		dev_err(&spi->dev,
+			"Missing registers-number property in the DT.\n");
+		return -EINVAL;
+	}
+
+	chip = devm_kzalloc(&spi->dev, sizeof(*chip) + nregs, GFP_KERNEL);
 	if (!chip)
 		return -ENOMEM;
 
@@ -130,20 +116,11 @@ static int gen_74x164_probe(struct spi_device *spi)
 	chip->gpio_chip.set = gen_74x164_set_value;
 	chip->gpio_chip.base = -1;
 
-	if (of_property_read_u32(spi->dev.of_node, "registers-number",
-				 &chip->registers)) {
-		dev_err(&spi->dev,
-			"Missing registers-number property in the DT.\n");
-		return -EINVAL;
-	}
-
+	chip->registers = nregs;
 	chip->gpio_chip.ngpio = GEN_74X164_NUMBER_GPIOS * chip->registers;
-	chip->buffer = devm_kzalloc(&spi->dev, chip->registers, GFP_KERNEL);
-	if (!chip->buffer)
-		return -ENOMEM;
 
 	chip->gpio_chip.can_sleep = true;
-	chip->gpio_chip.dev = &spi->dev;
+	chip->gpio_chip.parent = &spi->dev;
 	chip->gpio_chip.owner = THIS_MODULE;
 
 	mutex_init(&chip->lock);
@@ -154,7 +131,7 @@ static int gen_74x164_probe(struct spi_device *spi)
 		goto exit_destroy;
 	}
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = gpiochip_add_data(&chip->gpio_chip, chip);
 	if (!ret)
 		return 0;
 
diff --git a/drivers/gpio/gpio-74xx-mmio.c b/drivers/gpio/gpio-74xx-mmio.c
index 6b186829087c..372b0e01adc6 100644
--- a/drivers/gpio/gpio-74xx-mmio.c
+++ b/drivers/gpio/gpio-74xx-mmio.c
@@ -10,10 +10,9 @@
  */
 
 #include <linux/err.h>
-#include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
 
 #define MMIO_74XX_DIR_IN	(0 << 8)
@@ -21,7 +20,7 @@
 #define MMIO_74XX_BIT_CNT(x)	((x) & 0xff)
 
 struct mmio_74xx_gpio_priv {
-	struct bgpio_chip	bgc;
+	struct gpio_chip	gc;
 	unsigned		flags;
 };
 
@@ -78,30 +77,23 @@ static const struct of_device_id mmio_74xx_gpio_ids[] = {
 };
 MODULE_DEVICE_TABLE(of, mmio_74xx_gpio_ids);
 
-static inline struct mmio_74xx_gpio_priv *to_74xx_gpio(struct gpio_chip *gc)
-{
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return container_of(bgc, struct mmio_74xx_gpio_priv, bgc);
-}
-
 static int mmio_74xx_get_direction(struct gpio_chip *gc, unsigned offset)
 {
-	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+	struct mmio_74xx_gpio_priv *priv = gpiochip_get_data(gc);
 
-	return (priv->flags & MMIO_74XX_DIR_OUT) ? GPIOF_DIR_OUT : GPIOF_DIR_IN;
+	return !(priv->flags & MMIO_74XX_DIR_OUT);
 }
 
 static int mmio_74xx_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+	struct mmio_74xx_gpio_priv *priv = gpiochip_get_data(gc);
 
 	return (priv->flags & MMIO_74XX_DIR_OUT) ? -ENOTSUPP : 0;
 }
 
 static int mmio_74xx_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct mmio_74xx_gpio_priv *priv = to_74xx_gpio(gc);
+	struct mmio_74xx_gpio_priv *priv = gpiochip_get_data(gc);
 
 	if (priv->flags & MMIO_74XX_DIR_OUT) {
 		gc->set(gc, gpio, val);
@@ -134,28 +126,29 @@ static int mmio_74xx_gpio_probe(struct platform_device *pdev)
 
 	priv->flags = (uintptr_t) of_id->data;
 
-	err = bgpio_init(&priv->bgc, &pdev->dev,
+	err = bgpio_init(&priv->gc, &pdev->dev,
 			 DIV_ROUND_UP(MMIO_74XX_BIT_CNT(priv->flags), 8),
 			 dat, NULL, NULL, NULL, NULL, 0);
 	if (err)
 		return err;
 
-	priv->bgc.gc.direction_input = mmio_74xx_dir_in;
-	priv->bgc.gc.direction_output = mmio_74xx_dir_out;
-	priv->bgc.gc.get_direction = mmio_74xx_get_direction;
-	priv->bgc.gc.ngpio = MMIO_74XX_BIT_CNT(priv->flags);
-	priv->bgc.gc.owner = THIS_MODULE;
+	priv->gc.direction_input = mmio_74xx_dir_in;
+	priv->gc.direction_output = mmio_74xx_dir_out;
+	priv->gc.get_direction = mmio_74xx_get_direction;
+	priv->gc.ngpio = MMIO_74XX_BIT_CNT(priv->flags);
+	priv->gc.owner = THIS_MODULE;
 
 	platform_set_drvdata(pdev, priv);
 
-	return gpiochip_add(&priv->bgc.gc);
+	return gpiochip_add_data(&priv->gc, priv);
 }
 
 static int mmio_74xx_gpio_remove(struct platform_device *pdev)
 {
 	struct mmio_74xx_gpio_priv *priv = platform_get_drvdata(pdev);
 
-	return bgpio_remove(&priv->bgc);
+	gpiochip_remove(&priv->gc);
+	return 0;
 }
 
 static struct platform_driver mmio_74xx_gpio_driver = {
diff --git a/drivers/gpio/gpio-adnp.c b/drivers/gpio/gpio-adnp.c
index d3d0a90fe542..fb5b47b69f14 100644
--- a/drivers/gpio/gpio-adnp.c
+++ b/drivers/gpio/gpio-adnp.c
@@ -36,18 +36,13 @@ struct adnp {
 	u8 *irq_low;
 };
 
-static inline struct adnp *to_adnp(struct gpio_chip *chip)
-{
-	return container_of(chip, struct adnp, gpio);
-}
-
 static int adnp_read(struct adnp *adnp, unsigned offset, uint8_t *value)
 {
 	int err;
 
 	err = i2c_smbus_read_byte_data(adnp->client, offset);
 	if (err < 0) {
-		dev_err(adnp->gpio.dev, "%s failed: %d\n",
+		dev_err(adnp->gpio.parent, "%s failed: %d\n",
 			"i2c_smbus_read_byte_data()", err);
 		return err;
 	}
@@ -62,7 +57,7 @@ static int adnp_write(struct adnp *adnp, unsigned offset, uint8_t value)
 
 	err = i2c_smbus_write_byte_data(adnp->client, offset, value);
 	if (err < 0) {
-		dev_err(adnp->gpio.dev, "%s failed: %d\n",
+		dev_err(adnp->gpio.parent, "%s failed: %d\n",
 			"i2c_smbus_write_byte_data()", err);
 		return err;
 	}
@@ -72,7 +67,7 @@ static int adnp_write(struct adnp *adnp, unsigned offset, uint8_t value)
 
 static int adnp_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct adnp *adnp = to_adnp(chip);
+	struct adnp *adnp = gpiochip_get_data(chip);
 	unsigned int reg = offset >> adnp->reg_shift;
 	unsigned int pos = offset & 7;
 	u8 value;
@@ -106,7 +101,7 @@ static void __adnp_gpio_set(struct adnp *adnp, unsigned offset, int value)
 
 static void adnp_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct adnp *adnp = to_adnp(chip);
+	struct adnp *adnp = gpiochip_get_data(chip);
 
 	mutex_lock(&adnp->i2c_lock);
 	__adnp_gpio_set(adnp, offset, value);
@@ -115,7 +110,7 @@ static void adnp_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int adnp_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct adnp *adnp = to_adnp(chip);
+	struct adnp *adnp = gpiochip_get_data(chip);
 	unsigned int reg = offset >> adnp->reg_shift;
 	unsigned int pos = offset & 7;
 	u8 value;
@@ -150,7 +145,7 @@ out:
 static int adnp_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				      int value)
 {
-	struct adnp *adnp = to_adnp(chip);
+	struct adnp *adnp = gpiochip_get_data(chip);
 	unsigned int reg = offset >> adnp->reg_shift;
 	unsigned int pos = offset & 7;
 	int err;
@@ -187,7 +182,7 @@ out:
 
 static void adnp_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct adnp *adnp = to_adnp(chip);
+	struct adnp *adnp = gpiochip_get_data(chip);
 	unsigned int num_regs = 1 << adnp->reg_shift, i, j;
 	int err;
 
@@ -266,11 +261,11 @@ static int adnp_gpio_setup(struct adnp *adnp, unsigned int num_gpios)
 	chip->base = -1;
 	chip->ngpio = num_gpios;
 	chip->label = adnp->client->name;
-	chip->dev = &adnp->client->dev;
-	chip->of_node = chip->dev->of_node;
+	chip->parent = &adnp->client->dev;
+	chip->of_node = chip->parent->of_node;
 	chip->owner = THIS_MODULE;
 
-	err = gpiochip_add(chip);
+	err = gpiochip_add_data(chip, adnp);
 	if (err)
 		return err;
 
@@ -340,7 +335,7 @@ static irqreturn_t adnp_irq(int irq, void *data)
 static void adnp_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct adnp *adnp = to_adnp(gc);
+	struct adnp *adnp = gpiochip_get_data(gc);
 	unsigned int reg = d->hwirq >> adnp->reg_shift;
 	unsigned int pos = d->hwirq & 7;
 
@@ -350,7 +345,7 @@ static void adnp_irq_mask(struct irq_data *d)
 static void adnp_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct adnp *adnp = to_adnp(gc);
+	struct adnp *adnp = gpiochip_get_data(gc);
 	unsigned int reg = d->hwirq >> adnp->reg_shift;
 	unsigned int pos = d->hwirq & 7;
 
@@ -360,7 +355,7 @@ static void adnp_irq_unmask(struct irq_data *d)
 static int adnp_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct adnp *adnp = to_adnp(gc);
+	struct adnp *adnp = gpiochip_get_data(gc);
 	unsigned int reg = d->hwirq >> adnp->reg_shift;
 	unsigned int pos = d->hwirq & 7;
 
@@ -390,7 +385,7 @@ static int adnp_irq_set_type(struct irq_data *d, unsigned int type)
 static void adnp_irq_bus_lock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct adnp *adnp = to_adnp(gc);
+	struct adnp *adnp = gpiochip_get_data(gc);
 
 	mutex_lock(&adnp->irq_lock);
 }
@@ -398,7 +393,7 @@ static void adnp_irq_bus_lock(struct irq_data *d)
 static void adnp_irq_bus_unlock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct adnp *adnp = to_adnp(gc);
+	struct adnp *adnp = gpiochip_get_data(gc);
 	unsigned int num_regs = 1 << adnp->reg_shift, i;
 
 	mutex_lock(&adnp->i2c_lock);
@@ -435,7 +430,8 @@ static int adnp_irq_setup(struct adnp *adnp)
 	 * is chosen to match the register layout of the hardware in that
 	 * each segment contains the corresponding bits for all interrupts.
 	 */
-	adnp->irq_enable = devm_kzalloc(chip->dev, num_regs * 6, GFP_KERNEL);
+	adnp->irq_enable = devm_kzalloc(chip->parent, num_regs * 6,
+					GFP_KERNEL);
 	if (!adnp->irq_enable)
 		return -ENOMEM;
 
@@ -462,12 +458,12 @@ static int adnp_irq_setup(struct adnp *adnp)
 		adnp->irq_enable[i] = 0x00;
 	}
 
-	err = devm_request_threaded_irq(chip->dev, adnp->client->irq,
+	err = devm_request_threaded_irq(chip->parent, adnp->client->irq,
 					NULL, adnp_irq,
 					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-					dev_name(chip->dev), adnp);
+					dev_name(chip->parent), adnp);
 	if (err != 0) {
-		dev_err(chip->dev, "can't request IRQ#%d: %d\n",
+		dev_err(chip->parent, "can't request IRQ#%d: %d\n",
 			adnp->client->irq, err);
 		return err;
 	}
@@ -478,7 +474,7 @@ static int adnp_irq_setup(struct adnp *adnp)
 				   handle_simple_irq,
 				   IRQ_TYPE_NONE);
 	if (err) {
-		dev_err(chip->dev,
+		dev_err(chip->parent,
 			"could not connect irqchip to gpiochip\n");
 		return err;
 	}
@@ -547,7 +543,6 @@ MODULE_DEVICE_TABLE(of, adnp_of_match);
 static struct i2c_driver adnp_i2c_driver = {
 	.driver = {
 		.name = "gpio-adnp",
-		.owner = THIS_MODULE,
 		.of_match_table = adnp_of_match,
 	},
 	.probe = adnp_i2c_probe,
diff --git a/drivers/gpio/gpio-adp5520.c b/drivers/gpio/gpio-adp5520.c
index caff711ca5a9..4fa7ff1fec9a 100644
--- a/drivers/gpio/gpio-adp5520.c
+++ b/drivers/gpio/gpio-adp5520.c
@@ -27,7 +27,7 @@ static int adp5520_gpio_get_value(struct gpio_chip *chip, unsigned off)
 	struct adp5520_gpio *dev;
 	uint8_t reg_val;
 
-	dev = container_of(chip, struct adp5520_gpio, gpio_chip);
+	dev = gpiochip_get_data(chip);
 
 	/*
 	 * There are dedicated registers for GPIO IN/OUT.
@@ -46,7 +46,7 @@ static void adp5520_gpio_set_value(struct gpio_chip *chip,
 		unsigned off, int val)
 {
 	struct adp5520_gpio *dev;
-	dev = container_of(chip, struct adp5520_gpio, gpio_chip);
+	dev = gpiochip_get_data(chip);
 
 	if (val)
 		adp5520_set_bits(dev->master, ADP5520_GPIO_OUT, dev->lut[off]);
@@ -57,7 +57,7 @@ static void adp5520_gpio_set_value(struct gpio_chip *chip,
 static int adp5520_gpio_direction_input(struct gpio_chip *chip, unsigned off)
 {
 	struct adp5520_gpio *dev;
-	dev = container_of(chip, struct adp5520_gpio, gpio_chip);
+	dev = gpiochip_get_data(chip);
 
 	clear_bit(off, &dev->output);
 
@@ -70,7 +70,7 @@ static int adp5520_gpio_direction_output(struct gpio_chip *chip,
 {
 	struct adp5520_gpio *dev;
 	int ret = 0;
-	dev = container_of(chip, struct adp5520_gpio, gpio_chip);
+	dev = gpiochip_get_data(chip);
 
 	set_bit(off, &dev->output);
 
@@ -153,7 +153,7 @@ static int adp5520_gpio_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	ret = gpiochip_add(&dev->gpio_chip);
+	ret = gpiochip_add_data(&dev->gpio_chip, dev);
 	if (ret)
 		goto err;
 
diff --git a/drivers/gpio/gpio-adp5588.c b/drivers/gpio/gpio-adp5588.c
index 984186ee58a0..19a0eba1e942 100644
--- a/drivers/gpio/gpio-adp5588.c
+++ b/drivers/gpio/gpio-adp5588.c
@@ -65,8 +65,7 @@ static int adp5588_gpio_write(struct i2c_client *client, u8 reg, u8 val)
 
 static int adp5588_gpio_get_value(struct gpio_chip *chip, unsigned off)
 {
-	struct adp5588_gpio *dev =
-	    container_of(chip, struct adp5588_gpio, gpio_chip);
+	struct adp5588_gpio *dev = gpiochip_get_data(chip);
 	unsigned bank = ADP5588_BANK(off);
 	unsigned bit = ADP5588_BIT(off);
 	int val;
@@ -87,8 +86,7 @@ static void adp5588_gpio_set_value(struct gpio_chip *chip,
 				   unsigned off, int val)
 {
 	unsigned bank, bit;
-	struct adp5588_gpio *dev =
-	    container_of(chip, struct adp5588_gpio, gpio_chip);
+	struct adp5588_gpio *dev = gpiochip_get_data(chip);
 
 	bank = ADP5588_BANK(off);
 	bit = ADP5588_BIT(off);
@@ -108,8 +106,7 @@ static int adp5588_gpio_direction_input(struct gpio_chip *chip, unsigned off)
 {
 	int ret;
 	unsigned bank;
-	struct adp5588_gpio *dev =
-	    container_of(chip, struct adp5588_gpio, gpio_chip);
+	struct adp5588_gpio *dev = gpiochip_get_data(chip);
 
 	bank = ADP5588_BANK(off);
 
@@ -126,8 +123,7 @@ static int adp5588_gpio_direction_output(struct gpio_chip *chip,
 {
 	int ret;
 	unsigned bank, bit;
-	struct adp5588_gpio *dev =
-	    container_of(chip, struct adp5588_gpio, gpio_chip);
+	struct adp5588_gpio *dev = gpiochip_get_data(chip);
 
 	bank = ADP5588_BANK(off);
 	bit = ADP5588_BIT(off);
@@ -152,8 +148,8 @@ static int adp5588_gpio_direction_output(struct gpio_chip *chip,
 #ifdef CONFIG_GPIO_ADP5588_IRQ
 static int adp5588_gpio_to_irq(struct gpio_chip *chip, unsigned off)
 {
-	struct adp5588_gpio *dev =
-		container_of(chip, struct adp5588_gpio, gpio_chip);
+	struct adp5588_gpio *dev = gpiochip_get_data(chip);
+
 	return dev->irq_base + off;
 }
 
@@ -418,7 +414,7 @@ static int adp5588_gpio_probe(struct i2c_client *client,
 		}
 	}
 
-	ret = gpiochip_add(&dev->gpio_chip);
+	ret = gpiochip_add_data(&dev->gpio_chip, dev);
 	if (ret)
 		goto err_irq;
 
diff --git a/drivers/gpio/gpio-altera.c b/drivers/gpio/gpio-altera.c
index 3e6661bab54a..3f87a03abc22 100644
--- a/drivers/gpio/gpio-altera.c
+++ b/drivers/gpio/gpio-altera.c
@@ -42,11 +42,6 @@ struct altera_gpio_chip {
 	int mapped_irq;
 };
 
-static struct altera_gpio_chip *to_altera(struct gpio_chip *gc)
-{
-	return container_of(gc, struct altera_gpio_chip, mmchip.gc);
-}
-
 static void altera_gpio_irq_unmask(struct irq_data *d)
 {
 	struct altera_gpio_chip *altera_gc;
@@ -54,7 +49,7 @@ static void altera_gpio_irq_unmask(struct irq_data *d)
 	unsigned long flags;
 	u32 intmask;
 
-	altera_gc = to_altera(irq_data_get_irq_chip_data(d));
+	altera_gc = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	mm_gc = &altera_gc->mmchip;
 
 	spin_lock_irqsave(&altera_gc->gpio_lock, flags);
@@ -72,7 +67,7 @@ static void altera_gpio_irq_mask(struct irq_data *d)
 	unsigned long flags;
 	u32 intmask;
 
-	altera_gc = to_altera(irq_data_get_irq_chip_data(d));
+	altera_gc = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	mm_gc = &altera_gc->mmchip;
 
 	spin_lock_irqsave(&altera_gc->gpio_lock, flags);
@@ -92,7 +87,7 @@ static int altera_gpio_irq_set_type(struct irq_data *d,
 {
 	struct altera_gpio_chip *altera_gc;
 
-	altera_gc = to_altera(irq_data_get_irq_chip_data(d));
+	altera_gc = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 
 	if (type == IRQ_TYPE_NONE)
 		return 0;
@@ -145,7 +140,7 @@ static void altera_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 	unsigned int data_reg;
 
 	mm_gc = to_of_mm_gpio_chip(gc);
-	chip = container_of(mm_gc, struct altera_gpio_chip, mmchip);
+	chip = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&chip->gpio_lock, flags);
 	data_reg = readl(mm_gc->regs + ALTERA_GPIO_DATA);
@@ -165,7 +160,7 @@ static int altera_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 	unsigned int gpio_ddr;
 
 	mm_gc = to_of_mm_gpio_chip(gc);
-	chip = container_of(mm_gc, struct altera_gpio_chip, mmchip);
+	chip = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&chip->gpio_lock, flags);
 	/* Set pin as input, assumes software controlled IP */
@@ -186,7 +181,7 @@ static int altera_gpio_direction_output(struct gpio_chip *gc,
 	unsigned int data_reg, gpio_ddr;
 
 	mm_gc = to_of_mm_gpio_chip(gc);
-	chip = container_of(mm_gc, struct altera_gpio_chip, mmchip);
+	chip = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&chip->gpio_lock, flags);
 	/* Sets the GPIO value */
@@ -215,7 +210,7 @@ static void altera_gpio_irq_edge_handler(struct irq_desc *desc)
 	unsigned long status;
 	int i;
 
-	altera_gc = to_altera(irq_desc_get_handler_data(desc));
+	altera_gc = gpiochip_get_data(irq_desc_get_handler_data(desc));
 	chip = irq_desc_get_chip(desc);
 	mm_gc = &altera_gc->mmchip;
 	irqdomain = altera_gc->mmchip.gc.irqdomain;
@@ -244,7 +239,7 @@ static void altera_gpio_irq_leveL_high_handler(struct irq_desc *desc)
 	unsigned long status;
 	int i;
 
-	altera_gc = to_altera(irq_desc_get_handler_data(desc));
+	altera_gc = gpiochip_get_data(irq_desc_get_handler_data(desc));
 	chip = irq_desc_get_chip(desc);
 	mm_gc = &altera_gc->mmchip;
 	irqdomain = altera_gc->mmchip.gc.irqdomain;
@@ -290,9 +285,9 @@ static int altera_gpio_probe(struct platform_device *pdev)
 	altera_gc->mmchip.gc.get		= altera_gpio_get;
 	altera_gc->mmchip.gc.set		= altera_gpio_set;
 	altera_gc->mmchip.gc.owner		= THIS_MODULE;
-	altera_gc->mmchip.gc.dev		= &pdev->dev;
+	altera_gc->mmchip.gc.parent		= &pdev->dev;
 
-	ret = of_mm_gpiochip_add(node, &altera_gc->mmchip);
+	ret = of_mm_gpiochip_add_data(node, &altera_gc->mmchip, altera_gc);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed adding memory mapped gpiochip\n");
 		return ret;
@@ -317,8 +312,8 @@ static int altera_gpio_probe(struct platform_device *pdev)
 		handle_simple_irq, IRQ_TYPE_NONE);
 
 	if (ret) {
-		dev_info(&pdev->dev, "could not add irqchip\n");
-		return ret;
+		dev_err(&pdev->dev, "could not add irqchip\n");
+		goto teardown;
 	}
 
 	gpiochip_set_chained_irqchip(&altera_gc->mmchip.gc,
@@ -331,6 +326,7 @@ static int altera_gpio_probe(struct platform_device *pdev)
 skip_irq:
 	return 0;
 teardown:
+	of_mm_gpiochip_remove(&altera_gc->mmchip);
 	pr_err("%s: registration failed with status %d\n",
 		node->full_name, ret);
 
diff --git a/drivers/gpio/gpio-amd8111.c b/drivers/gpio/gpio-amd8111.c
index d00d81928fe8..c7040fffc5b4 100644
--- a/drivers/gpio/gpio-amd8111.c
+++ b/drivers/gpio/gpio-amd8111.c
@@ -75,11 +75,9 @@ struct amd_gpio {
 	u8			orig[32];
 };
 
-#define to_agp(chip)	container_of(chip, struct amd_gpio, chip)
-
 static int amd_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 
 	agp->orig[offset] = ioread8(agp->pm + AMD_REG_GPIO(offset)) &
 		(AMD_GPIO_DEBOUNCE | AMD_GPIO_MODE_MASK | AMD_GPIO_X_MASK);
@@ -91,7 +89,7 @@ static int amd_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void amd_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 
 	dev_dbg(&agp->pdev->dev, "Freed gpio %d, data %x\n", offset, agp->orig[offset]);
 
@@ -100,7 +98,7 @@ static void amd_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static void amd_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 	u8 temp;
 	unsigned long flags;
 
@@ -115,7 +113,7 @@ static void amd_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int amd_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 	u8 temp;
 
 	temp = ioread8(agp->pm + AMD_REG_GPIO(offset));
@@ -127,7 +125,7 @@ static int amd_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static int amd_gpio_dirout(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 	u8 temp;
 	unsigned long flags;
 
@@ -144,7 +142,7 @@ static int amd_gpio_dirout(struct gpio_chip *chip, unsigned offset, int value)
 
 static int amd_gpio_dirin(struct gpio_chip *chip, unsigned offset)
 {
-	struct amd_gpio *agp = to_agp(chip);
+	struct amd_gpio *agp = gpiochip_get_data(chip);
 	u8 temp;
 	unsigned long flags;
 
@@ -220,12 +218,12 @@ found:
 		goto out;
 	}
 	gp.pdev = pdev;
-	gp.chip.dev = &pdev->dev;
+	gp.chip.parent = &pdev->dev;
 
 	spin_lock_init(&gp.lock);
 
 	printk(KERN_INFO "AMD-8111 GPIO detected\n");
-	err = gpiochip_add(&gp.chip);
+	err = gpiochip_add_data(&gp.chip, &gp);
 	if (err) {
 		printk(KERN_ERR "GPIO registering failed (%d)\n",
 		       err);
diff --git a/drivers/gpio/gpio-amdpt.c b/drivers/gpio/gpio-amdpt.c
index cbbb966d4fc0..c2484046e8e9 100644
--- a/drivers/gpio/gpio-amdpt.c
+++ b/drivers/gpio/gpio-amdpt.c
@@ -31,22 +31,20 @@ struct pt_gpio_chip {
 	spinlock_t               lock;
 };
 
-#define to_pt_gpio(c)	container_of(c, struct pt_gpio_chip, gc)
-
 static int pt_gpio_request(struct gpio_chip *gc, unsigned offset)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 using_pins;
 
-	dev_dbg(gc->dev, "pt_gpio_request offset=%x\n", offset);
+	dev_dbg(gc->parent, "pt_gpio_request offset=%x\n", offset);
 
 	spin_lock_irqsave(&pt_gpio->lock, flags);
 
 	using_pins = readl(pt_gpio->reg_base + PT_SYNC_REG);
 	if (using_pins & BIT(offset)) {
-		dev_warn(gc->dev, "PT GPIO pin %x reconfigured\n",
-			offset);
+		dev_warn(gc->parent, "PT GPIO pin %x reconfigured\n",
+			 offset);
 		spin_unlock_irqrestore(&pt_gpio->lock, flags);
 		return -EINVAL;
 	}
@@ -60,7 +58,7 @@ static int pt_gpio_request(struct gpio_chip *gc, unsigned offset)
 
 static void pt_gpio_free(struct gpio_chip *gc, unsigned offset)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 using_pins;
 
@@ -72,16 +70,16 @@ static void pt_gpio_free(struct gpio_chip *gc, unsigned offset)
 
 	spin_unlock_irqrestore(&pt_gpio->lock, flags);
 
-	dev_dbg(gc->dev, "pt_gpio_free offset=%x\n", offset);
+	dev_dbg(gc->parent, "pt_gpio_free offset=%x\n", offset);
 }
 
 static void pt_gpio_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 data;
 
-	dev_dbg(gc->dev, "pt_gpio_set_value offset=%x, value=%x\n",
+	dev_dbg(gc->parent, "pt_gpio_set_value offset=%x, value=%x\n",
 		offset, value);
 
 	spin_lock_irqsave(&pt_gpio->lock, flags);
@@ -97,7 +95,7 @@ static void pt_gpio_set_value(struct gpio_chip *gc, unsigned offset, int value)
 
 static int pt_gpio_get_value(struct gpio_chip *gc, unsigned offset)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 data;
 
@@ -116,7 +114,7 @@ static int pt_gpio_get_value(struct gpio_chip *gc, unsigned offset)
 	data >>= offset;
 	data &= 1;
 
-	dev_dbg(gc->dev, "pt_gpio_get_value offset=%x, value=%x\n",
+	dev_dbg(gc->parent, "pt_gpio_get_value offset=%x, value=%x\n",
 		offset, data);
 
 	return data;
@@ -124,11 +122,11 @@ static int pt_gpio_get_value(struct gpio_chip *gc, unsigned offset)
 
 static int pt_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 data;
 
-	dev_dbg(gc->dev, "pt_gpio_dirction_input offset=%x\n", offset);
+	dev_dbg(gc->parent, "pt_gpio_dirction_input offset=%x\n", offset);
 
 	spin_lock_irqsave(&pt_gpio->lock, flags);
 
@@ -144,11 +142,11 @@ static int pt_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 static int pt_gpio_direction_output(struct gpio_chip *gc,
 					unsigned offset, int value)
 {
-	struct pt_gpio_chip *pt_gpio = to_pt_gpio(gc);
+	struct pt_gpio_chip *pt_gpio = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 data;
 
-	dev_dbg(gc->dev, "pt_gpio_direction_output offset=%x, value=%x\n",
+	dev_dbg(gc->parent, "pt_gpio_direction_output offset=%x, value=%x\n",
 		offset, value);
 
 	spin_lock_irqsave(&pt_gpio->lock, flags);
@@ -202,7 +200,7 @@ static int pt_gpio_probe(struct platform_device *pdev)
 
 	pt_gpio->gc.label            = pdev->name;
 	pt_gpio->gc.owner            = THIS_MODULE;
-	pt_gpio->gc.dev              = dev;
+	pt_gpio->gc.parent              = dev;
 	pt_gpio->gc.request          = pt_gpio_request;
 	pt_gpio->gc.free             = pt_gpio_free;
 	pt_gpio->gc.direction_input  = pt_gpio_direction_input;
@@ -214,7 +212,7 @@ static int pt_gpio_probe(struct platform_device *pdev)
 #if defined(CONFIG_OF_GPIO)
 	pt_gpio->gc.of_node          = pdev->dev.of_node;
 #endif
-	ret = gpiochip_add(&pt_gpio->gc);
+	ret = gpiochip_add_data(&pt_gpio->gc, pt_gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register GPIO lib\n");
 		return ret;
diff --git a/drivers/gpio/gpio-arizona.c b/drivers/gpio/gpio-arizona.c
index ca002739616a..e910c1f41d93 100644
--- a/drivers/gpio/gpio-arizona.c
+++ b/drivers/gpio/gpio-arizona.c
@@ -28,14 +28,9 @@ struct arizona_gpio {
 	struct gpio_chip gpio_chip;
 };
 
-static inline struct arizona_gpio *to_arizona_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct arizona_gpio, gpio_chip);
-}
-
 static int arizona_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona_gpio *arizona_gpio = gpiochip_get_data(chip);
 	struct arizona *arizona = arizona_gpio->arizona;
 
 	return regmap_update_bits(arizona->regmap, ARIZONA_GPIO1_CTRL + offset,
@@ -44,7 +39,7 @@ static int arizona_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int arizona_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona_gpio *arizona_gpio = gpiochip_get_data(chip);
 	struct arizona *arizona = arizona_gpio->arizona;
 	unsigned int val;
 	int ret;
@@ -62,7 +57,7 @@ static int arizona_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int arizona_gpio_direction_out(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
-	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona_gpio *arizona_gpio = gpiochip_get_data(chip);
 	struct arizona *arizona = arizona_gpio->arizona;
 
 	if (value)
@@ -74,7 +69,7 @@ static int arizona_gpio_direction_out(struct gpio_chip *chip,
 
 static void arizona_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct arizona_gpio *arizona_gpio = to_arizona_gpio(chip);
+	struct arizona_gpio *arizona_gpio = gpiochip_get_data(chip);
 	struct arizona *arizona = arizona_gpio->arizona;
 
 	if (value)
@@ -108,7 +103,7 @@ static int arizona_gpio_probe(struct platform_device *pdev)
 
 	arizona_gpio->arizona = arizona;
 	arizona_gpio->gpio_chip = template_chip;
-	arizona_gpio->gpio_chip.dev = &pdev->dev;
+	arizona_gpio->gpio_chip.parent = &pdev->dev;
 #ifdef CONFIG_OF_GPIO
 	arizona_gpio->gpio_chip.of_node = arizona->dev->of_node;
 #endif
@@ -122,6 +117,10 @@ static int arizona_gpio_probe(struct platform_device *pdev)
 	case WM1814:
 		arizona_gpio->gpio_chip.ngpio = 5;
 		break;
+	case WM1831:
+	case CS47L24:
+		arizona_gpio->gpio_chip.ngpio = 2;
+		break;
 	default:
 		dev_err(&pdev->dev, "Unknown chip variant %d\n",
 			arizona->type);
@@ -133,7 +132,7 @@ static int arizona_gpio_probe(struct platform_device *pdev)
 	else
 		arizona_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&arizona_gpio->gpio_chip);
+	ret = gpiochip_add_data(&arizona_gpio->gpio_chip, arizona_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n",
 			ret);
diff --git a/drivers/gpio/gpio-ath79.c b/drivers/gpio/gpio-ath79.c
index 5eaea8b812cf..d13dd133a907 100644
--- a/drivers/gpio/gpio-ath79.c
+++ b/drivers/gpio/gpio-ath79.c
@@ -24,12 +24,10 @@ struct ath79_gpio_ctrl {
 	spinlock_t lock;
 };
 
-#define to_ath79_gpio_ctrl(c) container_of(c, struct ath79_gpio_ctrl, chip)
-
 static void ath79_gpio_set_value(struct gpio_chip *chip,
 				unsigned gpio, int value)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 
 	if (value)
 		__raw_writel(BIT(gpio), ctrl->base + AR71XX_GPIO_REG_SET);
@@ -39,7 +37,7 @@ static void ath79_gpio_set_value(struct gpio_chip *chip,
 
 static int ath79_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 
 	return (__raw_readl(ctrl->base + AR71XX_GPIO_REG_IN) >> gpio) & 1;
 }
@@ -47,7 +45,7 @@ static int ath79_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
 static int ath79_gpio_direction_input(struct gpio_chip *chip,
 				       unsigned offset)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctrl->lock, flags);
@@ -64,7 +62,7 @@ static int ath79_gpio_direction_input(struct gpio_chip *chip,
 static int ath79_gpio_direction_output(struct gpio_chip *chip,
 					unsigned offset, int value)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctrl->lock, flags);
@@ -85,7 +83,7 @@ static int ath79_gpio_direction_output(struct gpio_chip *chip,
 
 static int ar934x_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctrl->lock, flags);
@@ -102,7 +100,7 @@ static int ar934x_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int ar934x_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 					int value)
 {
-	struct ath79_gpio_ctrl *ctrl = to_ath79_gpio_ctrl(chip);
+	struct ath79_gpio_ctrl *ctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&ctrl->lock, flags);
@@ -138,7 +136,7 @@ static const struct of_device_id ath79_gpio_of_match[] = {
 
 static int ath79_gpio_probe(struct platform_device *pdev)
 {
-	struct ath79_gpio_platform_data *pdata = pdev->dev.platform_data;
+	struct ath79_gpio_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct device_node *np = pdev->dev.of_node;
 	struct ath79_gpio_ctrl *ctrl;
 	struct resource *res;
@@ -177,14 +175,14 @@ static int ath79_gpio_probe(struct platform_device *pdev)
 
 	spin_lock_init(&ctrl->lock);
 	memcpy(&ctrl->chip, &ath79_gpio_chip, sizeof(ctrl->chip));
-	ctrl->chip.dev = &pdev->dev;
+	ctrl->chip.parent = &pdev->dev;
 	ctrl->chip.ngpio = ath79_gpio_count;
 	if (oe_inverted) {
 		ctrl->chip.direction_input = ar934x_gpio_direction_input;
 		ctrl->chip.direction_output = ar934x_gpio_direction_output;
 	}
 
-	err = gpiochip_add(&ctrl->chip);
+	err = gpiochip_add_data(&ctrl->chip, ctrl);
 	if (err) {
 		dev_err(&pdev->dev,
 			"cannot add AR71xx GPIO chip, error=%d", err);
diff --git a/drivers/gpio/gpio-bcm-kona.c b/drivers/gpio/gpio-bcm-kona.c
index 33a1f9779b86..b6c5abe85daf 100644
--- a/drivers/gpio/gpio-bcm-kona.c
+++ b/drivers/gpio/gpio-bcm-kona.c
@@ -78,11 +78,6 @@ struct bcm_kona_gpio_bank {
 	struct bcm_kona_gpio *kona_gpio;
 };
 
-static inline struct bcm_kona_gpio *to_kona_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct bcm_kona_gpio, gpio_chip);
-}
-
 static inline void bcm_kona_gpio_write_lock_regs(void __iomem *reg_base,
 						int bank_id, u32 lockcode)
 {
@@ -124,7 +119,7 @@ static void bcm_kona_gpio_unlock_gpio(struct bcm_kona_gpio *kona_gpio,
 
 static int bcm_kona_gpio_get_dir(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcm_kona_gpio *kona_gpio = to_kona_gpio(chip);
+	struct bcm_kona_gpio *kona_gpio = gpiochip_get_data(chip);
 	void __iomem *reg_base = kona_gpio->reg_base;
 	u32 val;
 
@@ -141,7 +136,7 @@ static void bcm_kona_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 	u32 val, reg_offset;
 	unsigned long flags;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
@@ -168,7 +163,7 @@ static int bcm_kona_gpio_get(struct gpio_chip *chip, unsigned gpio)
 	u32 val, reg_offset;
 	unsigned long flags;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
@@ -188,7 +183,7 @@ static int bcm_kona_gpio_get(struct gpio_chip *chip, unsigned gpio)
 
 static int bcm_kona_gpio_request(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcm_kona_gpio *kona_gpio = to_kona_gpio(chip);
+	struct bcm_kona_gpio *kona_gpio = gpiochip_get_data(chip);
 
 	bcm_kona_gpio_unlock_gpio(kona_gpio, gpio);
 	return 0;
@@ -196,7 +191,7 @@ static int bcm_kona_gpio_request(struct gpio_chip *chip, unsigned gpio)
 
 static void bcm_kona_gpio_free(struct gpio_chip *chip, unsigned gpio)
 {
-	struct bcm_kona_gpio *kona_gpio = to_kona_gpio(chip);
+	struct bcm_kona_gpio *kona_gpio = gpiochip_get_data(chip);
 
 	bcm_kona_gpio_lock_gpio(kona_gpio, gpio);
 }
@@ -208,7 +203,7 @@ static int bcm_kona_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 	u32 val;
 	unsigned long flags;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
@@ -232,7 +227,7 @@ static int bcm_kona_gpio_direction_output(struct gpio_chip *chip,
 	u32 val, reg_offset;
 	unsigned long flags;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	reg_base = kona_gpio->reg_base;
 	spin_lock_irqsave(&kona_gpio->lock, flags);
 
@@ -255,7 +250,7 @@ static int bcm_kona_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
 {
 	struct bcm_kona_gpio *kona_gpio;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	if (gpio >= kona_gpio->gpio_chip.ngpio)
 		return -ENXIO;
 	return irq_create_mapping(kona_gpio->irq_domain, gpio);
@@ -269,11 +264,11 @@ static int bcm_kona_gpio_set_debounce(struct gpio_chip *chip, unsigned gpio,
 	u32 val, res;
 	unsigned long flags;
 
-	kona_gpio = to_kona_gpio(chip);
+	kona_gpio = gpiochip_get_data(chip);
 	reg_base = kona_gpio->reg_base;
 	/* debounce must be 1-128ms (or 0) */
 	if ((debounce > 0 && debounce < 1000) || debounce > 128000) {
-		dev_err(chip->dev, "Debounce value %u not in range\n",
+		dev_err(chip->parent, "Debounce value %u not in range\n",
 			debounce);
 		return -EINVAL;
 	}
@@ -416,7 +411,7 @@ static int bcm_kona_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	case IRQ_TYPE_LEVEL_LOW:
 		/* BCM GPIO doesn't support level triggering */
 	default:
-		dev_err(kona_gpio->gpio_chip.dev,
+		dev_err(kona_gpio->gpio_chip.parent,
 			"Invalid BCM GPIO irq type 0x%x\n", type);
 		return -EINVAL;
 	}
@@ -477,7 +472,7 @@ static int bcm_kona_gpio_irq_reqres(struct irq_data *d)
 	struct bcm_kona_gpio *kona_gpio = irq_data_get_irq_chip_data(d);
 
 	if (gpiochip_lock_as_irq(&kona_gpio->gpio_chip, d->hwirq)) {
-		dev_err(kona_gpio->gpio_chip.dev,
+		dev_err(kona_gpio->gpio_chip.parent,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			d->hwirq);
 		return -EINVAL;
@@ -635,7 +630,7 @@ static int bcm_kona_gpio_probe(struct platform_device *pdev)
 
 	bcm_kona_gpio_reset(kona_gpio);
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, kona_gpio);
 	if (ret < 0) {
 		dev_err(dev, "Couldn't add GPIO chip -- %d\n", ret);
 		goto err_irq_domain;
diff --git a/drivers/gpio/gpio-brcmstb.c b/drivers/gpio/gpio-brcmstb.c
index 4c64627c6bb5..d7644251e869 100644
--- a/drivers/gpio/gpio-brcmstb.c
+++ b/drivers/gpio/gpio-brcmstb.c
@@ -16,7 +16,6 @@
 #include <linux/of_device.h>
 #include <linux/of_irq.h>
 #include <linux/module.h>
-#include <linux/basic_mmio_gpio.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/interrupt.h>
@@ -35,7 +34,7 @@
 struct brcmstb_gpio_bank {
 	struct list_head node;
 	int id;
-	struct bgpio_chip bgc;
+	struct gpio_chip gc;
 	struct brcmstb_gpio_priv *parent_priv;
 	u32 width;
 	struct irq_chip irq_chip;
@@ -57,37 +56,30 @@ struct brcmstb_gpio_priv {
 /* assumes MAX_GPIO_PER_BANK is a multiple of 2 */
 #define GPIO_BIT(gpio)          ((gpio) & (MAX_GPIO_PER_BANK - 1))
 
-static inline struct brcmstb_gpio_bank *
-brcmstb_gpio_gc_to_bank(struct gpio_chip *gc)
-{
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	return container_of(bgc, struct brcmstb_gpio_bank, bgc);
-}
-
 static inline struct brcmstb_gpio_priv *
 brcmstb_gpio_gc_to_priv(struct gpio_chip *gc)
 {
-	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	struct brcmstb_gpio_bank *bank = gpiochip_get_data(gc);
 	return bank->parent_priv;
 }
 
 static void brcmstb_gpio_set_imask(struct brcmstb_gpio_bank *bank,
 		unsigned int offset, bool enable)
 {
-	struct bgpio_chip *bgc = &bank->bgc;
+	struct gpio_chip *gc = &bank->gc;
 	struct brcmstb_gpio_priv *priv = bank->parent_priv;
-	u32 mask = bgc->pin2mask(bgc, offset);
+	u32 mask = gc->pin2mask(gc, offset);
 	u32 imask;
 	unsigned long flags;
 
-	spin_lock_irqsave(&bgc->lock, flags);
-	imask = bgc->read_reg(priv->reg_base + GIO_MASK(bank->id));
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
+	imask = gc->read_reg(priv->reg_base + GIO_MASK(bank->id));
 	if (enable)
 		imask |= mask;
 	else
 		imask &= ~mask;
-	bgc->write_reg(priv->reg_base + GIO_MASK(bank->id), imask);
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	gc->write_reg(priv->reg_base + GIO_MASK(bank->id), imask);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
 /* -------------------- IRQ chip functions -------------------- */
@@ -95,7 +87,7 @@ static void brcmstb_gpio_set_imask(struct brcmstb_gpio_bank *bank,
 static void brcmstb_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	struct brcmstb_gpio_bank *bank = gpiochip_get_data(gc);
 
 	brcmstb_gpio_set_imask(bank, d->hwirq, false);
 }
@@ -103,7 +95,7 @@ static void brcmstb_gpio_irq_mask(struct irq_data *d)
 static void brcmstb_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	struct brcmstb_gpio_bank *bank = gpiochip_get_data(gc);
 
 	brcmstb_gpio_set_imask(bank, d->hwirq, true);
 }
@@ -111,7 +103,7 @@ static void brcmstb_gpio_irq_unmask(struct irq_data *d)
 static int brcmstb_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	struct brcmstb_gpio_bank *bank = gpiochip_get_data(gc);
 	struct brcmstb_gpio_priv *priv = bank->parent_priv;
 	u32 mask = BIT(d->hwirq);
 	u32 edge_insensitive, iedge_insensitive;
@@ -149,23 +141,23 @@ static int brcmstb_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 		return -EINVAL;
 	}
 
-	spin_lock_irqsave(&bank->bgc.lock, flags);
+	spin_lock_irqsave(&bank->gc.bgpio_lock, flags);
 
-	iedge_config = bank->bgc.read_reg(priv->reg_base +
+	iedge_config = bank->gc.read_reg(priv->reg_base +
 			GIO_EC(bank->id)) & ~mask;
-	iedge_insensitive = bank->bgc.read_reg(priv->reg_base +
+	iedge_insensitive = bank->gc.read_reg(priv->reg_base +
 			GIO_EI(bank->id)) & ~mask;
-	ilevel = bank->bgc.read_reg(priv->reg_base +
+	ilevel = bank->gc.read_reg(priv->reg_base +
 			GIO_LEVEL(bank->id)) & ~mask;
 
-	bank->bgc.write_reg(priv->reg_base + GIO_EC(bank->id),
+	bank->gc.write_reg(priv->reg_base + GIO_EC(bank->id),
 			iedge_config | edge_config);
-	bank->bgc.write_reg(priv->reg_base + GIO_EI(bank->id),
+	bank->gc.write_reg(priv->reg_base + GIO_EI(bank->id),
 			iedge_insensitive | edge_insensitive);
-	bank->bgc.write_reg(priv->reg_base + GIO_LEVEL(bank->id),
+	bank->gc.write_reg(priv->reg_base + GIO_LEVEL(bank->id),
 			ilevel | level);
 
-	spin_unlock_irqrestore(&bank->bgc.lock, flags);
+	spin_unlock_irqrestore(&bank->gc.bgpio_lock, flags);
 	return 0;
 }
 
@@ -210,29 +202,29 @@ static irqreturn_t brcmstb_gpio_wake_irq_handler(int irq, void *data)
 static void brcmstb_gpio_irq_bank_handler(struct brcmstb_gpio_bank *bank)
 {
 	struct brcmstb_gpio_priv *priv = bank->parent_priv;
-	struct irq_domain *irq_domain = bank->bgc.gc.irqdomain;
+	struct irq_domain *irq_domain = bank->gc.irqdomain;
 	void __iomem *reg_base = priv->reg_base;
 	unsigned long status;
 	unsigned long flags;
 
-	spin_lock_irqsave(&bank->bgc.lock, flags);
-	while ((status = bank->bgc.read_reg(reg_base + GIO_STAT(bank->id)) &
-			 bank->bgc.read_reg(reg_base + GIO_MASK(bank->id)))) {
+	spin_lock_irqsave(&bank->gc.bgpio_lock, flags);
+	while ((status = bank->gc.read_reg(reg_base + GIO_STAT(bank->id)) &
+			 bank->gc.read_reg(reg_base + GIO_MASK(bank->id)))) {
 		int bit;
 
 		for_each_set_bit(bit, &status, 32) {
-			u32 stat = bank->bgc.read_reg(reg_base +
+			u32 stat = bank->gc.read_reg(reg_base +
 						      GIO_STAT(bank->id));
 			if (bit >= bank->width)
 				dev_warn(&priv->pdev->dev,
 					 "IRQ for invalid GPIO (bank=%d, offset=%d)\n",
 					 bank->id, bit);
-			bank->bgc.write_reg(reg_base + GIO_STAT(bank->id),
+			bank->gc.write_reg(reg_base + GIO_STAT(bank->id),
 					    stat | BIT(bit));
 			generic_handle_irq(irq_find_mapping(irq_domain, bit));
 		}
 	}
-	spin_unlock_irqrestore(&bank->bgc.lock, flags);
+	spin_unlock_irqrestore(&bank->gc.bgpio_lock, flags);
 }
 
 /* Each UPG GIO block has one IRQ for all banks */
@@ -303,9 +295,7 @@ static int brcmstb_gpio_remove(struct platform_device *pdev)
 	 */
 	list_for_each(pos, &priv->bank_list) {
 		bank = list_entry(pos, struct brcmstb_gpio_bank, node);
-		ret = bgpio_remove(&bank->bgc);
-		if (ret)
-			dev_err(&pdev->dev, "gpiochip_remove fail in cleanup\n");
+		gpiochip_remove(&bank->gc);
 	}
 	if (priv->reboot_notifier.notifier_call) {
 		ret = unregister_reboot_notifier(&priv->reboot_notifier);
@@ -320,7 +310,7 @@ static int brcmstb_gpio_of_xlate(struct gpio_chip *gc,
 		const struct of_phandle_args *gpiospec, u32 *flags)
 {
 	struct brcmstb_gpio_priv *priv = brcmstb_gpio_gc_to_priv(gc);
-	struct brcmstb_gpio_bank *bank = brcmstb_gpio_gc_to_bank(gc);
+	struct brcmstb_gpio_bank *bank = gpiochip_get_data(gc);
 	int offset;
 
 	if (gc->of_gpio_n_cells != 2) {
@@ -398,9 +388,9 @@ static int brcmstb_gpio_irq_setup(struct platform_device *pdev,
 	if (priv->can_wake)
 		bank->irq_chip.irq_set_wake = brcmstb_gpio_irq_set_wake;
 
-	gpiochip_irqchip_add(&bank->bgc.gc, &bank->irq_chip, 0,
+	gpiochip_irqchip_add(&bank->gc, &bank->irq_chip, 0,
 			handle_simple_irq, IRQ_TYPE_NONE);
-	gpiochip_set_chained_irqchip(&bank->bgc.gc, &bank->irq_chip,
+	gpiochip_set_chained_irqchip(&bank->gc, &bank->irq_chip,
 			priv->parent_irq, brcmstb_gpio_irq_handler);
 
 	return 0;
@@ -419,6 +409,7 @@ static int brcmstb_gpio_probe(struct platform_device *pdev)
 	int num_banks = 0;
 	int err;
 	static int gpio_base;
+	unsigned long flags = 0;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -448,10 +439,21 @@ static int brcmstb_gpio_probe(struct platform_device *pdev)
 	if (brcmstb_gpio_sanity_check_banks(dev, np, res))
 		return -EINVAL;
 
+	/*
+	 * MIPS endianness is configured by boot strap, which also reverses all
+	 * bus endianness (i.e., big-endian CPU + big endian bus ==> native
+	 * endian I/O).
+	 *
+	 * Other architectures (e.g., ARM) either do not support big endian, or
+	 * else leave I/O in little endian mode.
+	 */
+#if defined(CONFIG_MIPS) && defined(__BIG_ENDIAN)
+	flags = BGPIOF_BIG_ENDIAN_BYTE_ORDER;
+#endif
+
 	of_property_for_each_u32(np, "brcm,gpio-bank-widths", prop, p,
 			bank_width) {
 		struct brcmstb_gpio_bank *bank;
-		struct bgpio_chip *bgc;
 		struct gpio_chip *gc;
 
 		bank = devm_kzalloc(dev, sizeof(*bank), GFP_KERNEL);
@@ -473,17 +475,16 @@ static int brcmstb_gpio_probe(struct platform_device *pdev)
 		 * Regs are 4 bytes wide, have data reg, no set/clear regs,
 		 * and direction bits have 0 = output and 1 = input
 		 */
-		bgc = &bank->bgc;
-		err = bgpio_init(bgc, dev, 4,
+		gc = &bank->gc;
+		err = bgpio_init(gc, dev, 4,
 				reg_base + GIO_DATA(bank->id),
 				NULL, NULL, NULL,
-				reg_base + GIO_IODIR(bank->id), 0);
+				reg_base + GIO_IODIR(bank->id), flags);
 		if (err) {
 			dev_err(dev, "bgpio_init() failed\n");
 			goto fail;
 		}
 
-		gc = &bgc->gc;
 		gc->of_node = np;
 		gc->owner = THIS_MODULE;
 		gc->label = np->full_name;
@@ -497,9 +498,9 @@ static int brcmstb_gpio_probe(struct platform_device *pdev)
 		 * Mask all interrupts by default, since wakeup interrupts may
 		 * be retained from S5 cold boot
 		 */
-		bank->bgc.write_reg(reg_base + GIO_MASK(bank->id), 0);
+		gc->write_reg(reg_base + GIO_MASK(bank->id), 0);
 
-		err = gpiochip_add(gc);
+		err = gpiochip_add_data(gc, bank);
 		if (err) {
 			dev_err(dev, "Could not add gpiochip for bank %d\n",
 					bank->id);
diff --git a/drivers/gpio/gpio-bt8xx.c b/drivers/gpio/gpio-bt8xx.c
index 7e4c43c18960..acefb25e8eca 100644
--- a/drivers/gpio/gpio-bt8xx.c
+++ b/drivers/gpio/gpio-bt8xx.c
@@ -80,7 +80,7 @@ MODULE_PARM_DESC(gpiobase, "The GPIO number base. -1 means dynamic, which is the
 
 static int bt8xxgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 {
-	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	struct bt8xxgpio *bg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u32 outen, data;
 
@@ -101,7 +101,7 @@ static int bt8xxgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 
 static int bt8xxgpio_gpio_get(struct gpio_chip *gpio, unsigned nr)
 {
-	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	struct bt8xxgpio *bg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u32 val;
 
@@ -115,7 +115,7 @@ static int bt8xxgpio_gpio_get(struct gpio_chip *gpio, unsigned nr)
 static int bt8xxgpio_gpio_direction_output(struct gpio_chip *gpio,
 					unsigned nr, int val)
 {
-	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	struct bt8xxgpio *bg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u32 outen, data;
 
@@ -140,7 +140,7 @@ static int bt8xxgpio_gpio_direction_output(struct gpio_chip *gpio,
 static void bt8xxgpio_gpio_set(struct gpio_chip *gpio,
 			    unsigned nr, int val)
 {
-	struct bt8xxgpio *bg = container_of(gpio, struct bt8xxgpio, gpio);
+	struct bt8xxgpio *bg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u32 data;
 
@@ -217,7 +217,7 @@ static int bt8xxgpio_probe(struct pci_dev *dev,
 	bgwrite(0, BT848_GPIO_OUT_EN);
 
 	bt8xxgpio_gpio_setup(bg);
-	err = gpiochip_add(&bg->gpio);
+	err = gpiochip_add_data(&bg->gpio, bg);
 	if (err) {
 		printk(KERN_ERR "bt8xxgpio: Failed to register GPIOs\n");
 		goto err_disable;
diff --git a/drivers/gpio/gpio-clps711x.c b/drivers/gpio/gpio-clps711x.c
index b6908f1ff1ab..c84f9551f108 100644
--- a/drivers/gpio/gpio-clps711x.c
+++ b/drivers/gpio/gpio-clps711x.c
@@ -10,24 +10,23 @@
  */
 
 #include <linux/err.h>
-#include <linux/gpio.h>
 #include <linux/module.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
 
 static int clps711x_gpio_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
 	void __iomem *dat, *dir;
-	struct bgpio_chip *bgc;
+	struct gpio_chip *gc;
 	struct resource *res;
 	int err, id = np ? of_alias_get_id(np, "gpio") : pdev->id;
 
 	if ((id < 0) || (id > 4))
 		return -ENODEV;
 
-	bgc = devm_kzalloc(&pdev->dev, sizeof(*bgc), GFP_KERNEL);
-	if (!bgc)
+	gc = devm_kzalloc(&pdev->dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -43,11 +42,11 @@ static int clps711x_gpio_probe(struct platform_device *pdev)
 	switch (id) {
 	case 3:
 		/* PORTD is inverted logic for direction register */
-		err = bgpio_init(bgc, &pdev->dev, 1, dat, NULL, NULL,
+		err = bgpio_init(gc, &pdev->dev, 1, dat, NULL, NULL,
 				 NULL, dir, 0);
 		break;
 	default:
-		err = bgpio_init(bgc, &pdev->dev, 1, dat, NULL, NULL,
+		err = bgpio_init(gc, &pdev->dev, 1, dat, NULL, NULL,
 				 dir, NULL, 0);
 		break;
 	}
@@ -58,24 +57,25 @@ static int clps711x_gpio_probe(struct platform_device *pdev)
 	switch (id) {
 	case 4:
 		/* PORTE is 3 lines only */
-		bgc->gc.ngpio = 3;
+		gc->ngpio = 3;
 		break;
 	default:
 		break;
 	}
 
-	bgc->gc.base = id * 8;
-	bgc->gc.owner = THIS_MODULE;
-	platform_set_drvdata(pdev, bgc);
+	gc->base = id * 8;
+	gc->owner = THIS_MODULE;
+	platform_set_drvdata(pdev, gc);
 
-	return gpiochip_add(&bgc->gc);
+	return gpiochip_add_data(gc, NULL);
 }
 
 static int clps711x_gpio_remove(struct platform_device *pdev)
 {
-	struct bgpio_chip *bgc = platform_get_drvdata(pdev);
+	struct gpio_chip *gc = platform_get_drvdata(pdev);
 
-	return bgpio_remove(bgc);
+	gpiochip_remove(gc);
+	return 0;
 }
 
 static const struct of_device_id __maybe_unused clps711x_gpio_ids[] = {
diff --git a/drivers/gpio/gpio-crystalcove.c b/drivers/gpio/gpio-crystalcove.c
index fddd204dc9b6..7865ef0d3352 100644
--- a/drivers/gpio/gpio-crystalcove.c
+++ b/drivers/gpio/gpio-crystalcove.c
@@ -86,11 +86,6 @@ struct crystalcove_gpio {
 	bool set_irq_mask;
 };
 
-static inline struct crystalcove_gpio *to_cg(struct gpio_chip *gc)
-{
-	return container_of(gc, struct crystalcove_gpio, chip);
-}
-
 static inline int to_reg(int gpio, enum ctrl_register reg_type)
 {
 	int reg;
@@ -134,7 +129,7 @@ static void crystalcove_update_irq_ctrl(struct crystalcove_gpio *cg, int gpio)
 
 static int crystalcove_gpio_dir_in(struct gpio_chip *chip, unsigned gpio)
 {
-	struct crystalcove_gpio *cg = to_cg(chip);
+	struct crystalcove_gpio *cg = gpiochip_get_data(chip);
 
 	if (gpio > CRYSTALCOVE_VGPIO_NUM)
 		return 0;
@@ -146,7 +141,7 @@ static int crystalcove_gpio_dir_in(struct gpio_chip *chip, unsigned gpio)
 static int crystalcove_gpio_dir_out(struct gpio_chip *chip, unsigned gpio,
 				    int value)
 {
-	struct crystalcove_gpio *cg = to_cg(chip);
+	struct crystalcove_gpio *cg = gpiochip_get_data(chip);
 
 	if (gpio > CRYSTALCOVE_VGPIO_NUM)
 		return 0;
@@ -157,7 +152,7 @@ static int crystalcove_gpio_dir_out(struct gpio_chip *chip, unsigned gpio,
 
 static int crystalcove_gpio_get(struct gpio_chip *chip, unsigned gpio)
 {
-	struct crystalcove_gpio *cg = to_cg(chip);
+	struct crystalcove_gpio *cg = gpiochip_get_data(chip);
 	int ret;
 	unsigned int val;
 
@@ -174,7 +169,7 @@ static int crystalcove_gpio_get(struct gpio_chip *chip, unsigned gpio)
 static void crystalcove_gpio_set(struct gpio_chip *chip,
 				 unsigned gpio, int value)
 {
-	struct crystalcove_gpio *cg = to_cg(chip);
+	struct crystalcove_gpio *cg = gpiochip_get_data(chip);
 
 	if (gpio > CRYSTALCOVE_VGPIO_NUM)
 		return;
@@ -187,7 +182,8 @@ static void crystalcove_gpio_set(struct gpio_chip *chip,
 
 static int crystalcove_irq_type(struct irq_data *data, unsigned type)
 {
-	struct crystalcove_gpio *cg = to_cg(irq_data_get_irq_chip_data(data));
+	struct crystalcove_gpio *cg =
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 
 	switch (type) {
 	case IRQ_TYPE_NONE:
@@ -213,14 +209,16 @@ static int crystalcove_irq_type(struct irq_data *data, unsigned type)
 
 static void crystalcove_bus_lock(struct irq_data *data)
 {
-	struct crystalcove_gpio *cg = to_cg(irq_data_get_irq_chip_data(data));
+	struct crystalcove_gpio *cg =
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 
 	mutex_lock(&cg->buslock);
 }
 
 static void crystalcove_bus_sync_unlock(struct irq_data *data)
 {
-	struct crystalcove_gpio *cg = to_cg(irq_data_get_irq_chip_data(data));
+	struct crystalcove_gpio *cg =
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 	int gpio = data->hwirq;
 
 	if (cg->update & UPDATE_IRQ_TYPE)
@@ -234,7 +232,8 @@ static void crystalcove_bus_sync_unlock(struct irq_data *data)
 
 static void crystalcove_irq_unmask(struct irq_data *data)
 {
-	struct crystalcove_gpio *cg = to_cg(irq_data_get_irq_chip_data(data));
+	struct crystalcove_gpio *cg =
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 
 	cg->set_irq_mask = false;
 	cg->update |= UPDATE_IRQ_MASK;
@@ -242,7 +241,8 @@ static void crystalcove_irq_unmask(struct irq_data *data)
 
 static void crystalcove_irq_mask(struct irq_data *data)
 {
-	struct crystalcove_gpio *cg = to_cg(irq_data_get_irq_chip_data(data));
+	struct crystalcove_gpio *cg =
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 
 	cg->set_irq_mask = true;
 	cg->update |= UPDATE_IRQ_MASK;
@@ -288,7 +288,7 @@ static irqreturn_t crystalcove_gpio_irq_handler(int irq, void *data)
 static void crystalcove_gpio_dbg_show(struct seq_file *s,
 				      struct gpio_chip *chip)
 {
-	struct crystalcove_gpio *cg = to_cg(chip);
+	struct crystalcove_gpio *cg = gpiochip_get_data(chip);
 	int gpio, offset;
 	unsigned int ctlo, ctli, mirqs0, mirqsx, irq;
 
@@ -341,11 +341,11 @@ static int crystalcove_gpio_probe(struct platform_device *pdev)
 	cg->chip.base = -1;
 	cg->chip.ngpio = CRYSTALCOVE_VGPIO_NUM;
 	cg->chip.can_sleep = true;
-	cg->chip.dev = dev;
+	cg->chip.parent = dev;
 	cg->chip.dbg_show = crystalcove_gpio_dbg_show;
 	cg->regmap = pmic->regmap;
 
-	retval = gpiochip_add(&cg->chip);
+	retval = gpiochip_add_data(&cg->chip, cg);
 	if (retval) {
 		dev_warn(&pdev->dev, "add gpio chip error: %d\n", retval);
 		return retval;
diff --git a/drivers/gpio/gpio-cs5535.c b/drivers/gpio/gpio-cs5535.c
index 7b0b198a563d..eccb712e09fb 100644
--- a/drivers/gpio/gpio-cs5535.c
+++ b/drivers/gpio/gpio-cs5535.c
@@ -42,6 +42,10 @@ static ulong mask = GPIO_DEFAULT_MASK;
 module_param_named(mask, mask, ulong, 0444);
 MODULE_PARM_DESC(mask, "GPIO channel mask.");
 
+/*
+ * FIXME: convert this singleton driver to use the state container
+ * design pattern, see Documentation/driver-model/design-patterns.txt
+ */
 static struct cs5535_gpio_chip {
 	struct gpio_chip chip;
 	resource_size_t base;
@@ -201,8 +205,7 @@ EXPORT_SYMBOL_GPL(cs5535_gpio_setup_event);
 
 static int chip_gpio_request(struct gpio_chip *c, unsigned offset)
 {
-	struct cs5535_gpio_chip *chip =
-		container_of(c, struct cs5535_gpio_chip, chip);
+	struct cs5535_gpio_chip *chip = gpiochip_get_data(c);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -242,8 +245,7 @@ static void chip_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 
 static int chip_direction_input(struct gpio_chip *c, unsigned offset)
 {
-	struct cs5535_gpio_chip *chip =
-		container_of(c, struct cs5535_gpio_chip, chip);
+	struct cs5535_gpio_chip *chip = gpiochip_get_data(c);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -256,8 +258,7 @@ static int chip_direction_input(struct gpio_chip *c, unsigned offset)
 
 static int chip_direction_output(struct gpio_chip *c, unsigned offset, int val)
 {
-	struct cs5535_gpio_chip *chip =
-		container_of(c, struct cs5535_gpio_chip, chip);
+	struct cs5535_gpio_chip *chip = gpiochip_get_data(c);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -347,7 +348,7 @@ static int cs5535_gpio_probe(struct platform_device *pdev)
 				mask_orig, mask);
 
 	/* finally, register with the generic GPIO API */
-	err = gpiochip_add(&cs5535_gpio_chip.chip);
+	err = gpiochip_add_data(&cs5535_gpio_chip.chip, &cs5535_gpio_chip);
 	if (err)
 		goto done;
 
diff --git a/drivers/gpio/gpio-da9052.c b/drivers/gpio/gpio-da9052.c
index 2e9578ec0ca1..f9b3247ad14b 100644
--- a/drivers/gpio/gpio-da9052.c
+++ b/drivers/gpio/gpio-da9052.c
@@ -51,11 +51,6 @@ struct da9052_gpio {
 	struct gpio_chip gp;
 };
 
-static inline struct da9052_gpio *to_da9052_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct da9052_gpio, gp);
-}
-
 static unsigned char da9052_gpio_port_odd(unsigned offset)
 {
 	return offset % 2;
@@ -63,7 +58,7 @@ static unsigned char da9052_gpio_port_odd(unsigned offset)
 
 static int da9052_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct da9052_gpio *gpio = to_da9052_gpio(gc);
+	struct da9052_gpio *gpio = gpiochip_get_data(gc);
 	int da9052_port_direction = 0;
 	int ret;
 
@@ -89,15 +84,12 @@ static int da9052_gpio_get(struct gpio_chip *gc, unsigned offset)
 					      DA9052_STATUS_D_REG);
 		if (ret < 0)
 			return ret;
-		if (ret & (1 << DA9052_GPIO_SHIFT_COUNT(offset)))
-			return 1;
-		else
-			return 0;
+		return !!(ret & (1 << DA9052_GPIO_SHIFT_COUNT(offset)));
 	case DA9052_OUTPUT_PUSHPULL:
 		if (da9052_gpio_port_odd(offset))
-			return ret & DA9052_GPIO_ODD_PORT_MODE;
+			return !!(ret & DA9052_GPIO_ODD_PORT_MODE);
 		else
-			return ret & DA9052_GPIO_EVEN_PORT_MODE;
+			return !!(ret & DA9052_GPIO_EVEN_PORT_MODE);
 	default:
 		return -EINVAL;
 	}
@@ -105,7 +97,7 @@ static int da9052_gpio_get(struct gpio_chip *gc, unsigned offset)
 
 static void da9052_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct da9052_gpio *gpio = to_da9052_gpio(gc);
+	struct da9052_gpio *gpio = gpiochip_get_data(gc);
 	int ret;
 
 	if (da9052_gpio_port_odd(offset)) {
@@ -131,7 +123,7 @@ static void da9052_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 
 static int da9052_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct da9052_gpio *gpio = to_da9052_gpio(gc);
+	struct da9052_gpio *gpio = gpiochip_get_data(gc);
 	unsigned char register_value;
 	int ret;
 
@@ -157,7 +149,7 @@ static int da9052_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 static int da9052_gpio_direction_output(struct gpio_chip *gc,
 					unsigned offset, int value)
 {
-	struct da9052_gpio *gpio = to_da9052_gpio(gc);
+	struct da9052_gpio *gpio = gpiochip_get_data(gc);
 	unsigned char register_value;
 	int ret;
 
@@ -182,7 +174,7 @@ static int da9052_gpio_direction_output(struct gpio_chip *gc,
 
 static int da9052_gpio_to_irq(struct gpio_chip *gc, u32 offset)
 {
-	struct da9052_gpio *gpio = to_da9052_gpio(gc);
+	struct da9052_gpio *gpio = gpiochip_get_data(gc);
 	struct da9052 *da9052 = gpio->da9052;
 
 	int irq;
@@ -222,7 +214,7 @@ static int da9052_gpio_probe(struct platform_device *pdev)
 	if (pdata && pdata->gpio_base)
 		gpio->gp.base = pdata->gpio_base;
 
-	ret = gpiochip_add(&gpio->gp);
+	ret = gpiochip_add_data(&gpio->gp, gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-da9055.c b/drivers/gpio/gpio-da9055.c
index 7227e6ed3cb9..18210fb2cb13 100644
--- a/drivers/gpio/gpio-da9055.c
+++ b/drivers/gpio/gpio-da9055.c
@@ -35,14 +35,9 @@ struct da9055_gpio {
 	struct gpio_chip gp;
 };
 
-static inline struct da9055_gpio *to_da9055_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct da9055_gpio, gp);
-}
-
 static int da9055_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct da9055_gpio *gpio = to_da9055_gpio(gc);
+	struct da9055_gpio *gpio = gpiochip_get_data(gc);
 	int gpio_direction = 0;
 	int ret;
 
@@ -71,7 +66,7 @@ static int da9055_gpio_get(struct gpio_chip *gc, unsigned offset)
 
 static void da9055_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct da9055_gpio *gpio = to_da9055_gpio(gc);
+	struct da9055_gpio *gpio = gpiochip_get_data(gc);
 
 	da9055_reg_update(gpio->da9055,
 			DA9055_REG_GPIO_MODE0_2,
@@ -81,7 +76,7 @@ static void da9055_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 
 static int da9055_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct da9055_gpio *gpio = to_da9055_gpio(gc);
+	struct da9055_gpio *gpio = gpiochip_get_data(gc);
 	unsigned char reg_byte;
 
 	reg_byte = (DA9055_ACT_LOW | DA9055_GPI)
@@ -97,7 +92,7 @@ static int da9055_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 static int da9055_gpio_direction_output(struct gpio_chip *gc,
 					unsigned offset, int value)
 {
-	struct da9055_gpio *gpio = to_da9055_gpio(gc);
+	struct da9055_gpio *gpio = gpiochip_get_data(gc);
 	unsigned char reg_byte;
 	int ret;
 
@@ -119,7 +114,7 @@ static int da9055_gpio_direction_output(struct gpio_chip *gc,
 
 static int da9055_gpio_to_irq(struct gpio_chip *gc, u32 offset)
 {
-	struct da9055_gpio *gpio = to_da9055_gpio(gc);
+	struct da9055_gpio *gpio = gpiochip_get_data(gc);
 	struct da9055 *da9055 = gpio->da9055;
 
 	return regmap_irq_get_virq(da9055->irq_data,
@@ -156,7 +151,7 @@ static int da9055_gpio_probe(struct platform_device *pdev)
 	if (pdata && pdata->gpio_base)
 		gpio->gp.base = pdata->gpio_base;
 
-	ret = gpiochip_add(&gpio->gp);
+	ret = gpiochip_add_data(&gpio->gp, gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		goto err_mem;
diff --git a/drivers/gpio/gpio-davinci.c b/drivers/gpio/gpio-davinci.c
index 5e715388803d..cd007a67b302 100644
--- a/drivers/gpio/gpio-davinci.c
+++ b/drivers/gpio/gpio-davinci.c
@@ -41,9 +41,6 @@ typedef struct irq_chip *(*gpio_get_irq_chip_cb_t)(unsigned int irq);
 
 #define BINTEN	0x8 /* GPIO Interrupt Per-Bank Enable Register */
 
-#define chip2controller(chip)	\
-	container_of(chip, struct davinci_gpio_controller, chip)
-
 static void __iomem *gpio_base;
 
 static struct davinci_gpio_regs __iomem *gpio2regs(unsigned gpio)
@@ -82,7 +79,7 @@ static int davinci_gpio_irq_setup(struct platform_device *pdev);
 static inline int __davinci_direction(struct gpio_chip *chip,
 			unsigned offset, bool out, int value)
 {
-	struct davinci_gpio_controller *d = chip2controller(chip);
+	struct davinci_gpio_controller *d = gpiochip_get_data(chip);
 	struct davinci_gpio_regs __iomem *g = d->regs;
 	unsigned long flags;
 	u32 temp;
@@ -122,10 +119,10 @@ davinci_direction_out(struct gpio_chip *chip, unsigned offset, int value)
  */
 static int davinci_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct davinci_gpio_controller *d = chip2controller(chip);
+	struct davinci_gpio_controller *d = gpiochip_get_data(chip);
 	struct davinci_gpio_regs __iomem *g = d->regs;
 
-	return (1 << offset) & readl_relaxed(&g->in_data);
+	return !!((1 << offset) & readl_relaxed(&g->in_data));
 }
 
 /*
@@ -134,7 +131,7 @@ static int davinci_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void
 davinci_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct davinci_gpio_controller *d = chip2controller(chip);
+	struct davinci_gpio_controller *d = gpiochip_get_data(chip);
 	struct davinci_gpio_regs __iomem *g = d->regs;
 
 	writel_relaxed((1 << offset), value ? &g->set_data : &g->clr_data);
@@ -149,7 +146,7 @@ davinci_gpio_get_pdata(struct platform_device *pdev)
 	u32 val;
 
 	if (!IS_ENABLED(CONFIG_OF) || !pdev->dev.of_node)
-		return pdev->dev.platform_data;
+		return dev_get_platdata(&pdev->dev);
 
 	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
@@ -179,8 +176,8 @@ static int davinci_gpio_of_xlate(struct gpio_chip *gc,
 			     const struct of_phandle_args *gpiospec,
 			     u32 *flags)
 {
-	struct davinci_gpio_controller *chips = dev_get_drvdata(gc->dev);
-	struct davinci_gpio_platform_data *pdata = dev_get_platdata(gc->dev);
+	struct davinci_gpio_controller *chips = dev_get_drvdata(gc->parent);
+	struct davinci_gpio_platform_data *pdata = dev_get_platdata(gc->parent);
 
 	if (gpiospec->args[0] > pdata->ngpio)
 		return -EINVAL;
@@ -198,7 +195,7 @@ static int davinci_gpio_of_xlate(struct gpio_chip *gc,
 static int davinci_gpio_probe(struct platform_device *pdev)
 {
 	int i, base;
-	unsigned ngpio;
+	unsigned ngpio, nbank;
 	struct davinci_gpio_controller *chips;
 	struct davinci_gpio_platform_data *pdata;
 	struct davinci_gpio_regs __iomem *regs;
@@ -227,8 +224,9 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 	if (WARN_ON(ARCH_NR_GPIOS < ngpio))
 		ngpio = ARCH_NR_GPIOS;
 
+	nbank = DIV_ROUND_UP(ngpio, 32);
 	chips = devm_kzalloc(dev,
-			     ngpio * sizeof(struct davinci_gpio_controller),
+			     nbank * sizeof(struct davinci_gpio_controller),
 			     GFP_KERNEL);
 	if (!chips)
 		return -ENOMEM;
@@ -254,7 +252,7 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 #ifdef CONFIG_OF_GPIO
 		chips[i].chip.of_gpio_n_cells = 2;
 		chips[i].chip.of_xlate = davinci_gpio_of_xlate;
-		chips[i].chip.dev = dev;
+		chips[i].chip.parent = dev;
 		chips[i].chip.of_node = dev->of_node;
 #endif
 		spin_lock_init(&chips[i].lock);
@@ -265,7 +263,7 @@ static int davinci_gpio_probe(struct platform_device *pdev)
 		chips[i].clr_data = &regs->clr_data;
 		chips[i].in_data = &regs->in_data;
 
-		gpiochip_add(&chips[i].chip);
+		gpiochip_add_data(&chips[i].chip, &chips[i]);
 	}
 
 	platform_set_drvdata(pdev, chips);
@@ -368,7 +366,7 @@ static void gpio_irq_handler(struct irq_desc *desc)
 
 static int gpio_to_irq_banked(struct gpio_chip *chip, unsigned offset)
 {
-	struct davinci_gpio_controller *d = chip2controller(chip);
+	struct davinci_gpio_controller *d = gpiochip_get_data(chip);
 
 	if (d->irq_domain)
 		return irq_create_mapping(d->irq_domain, d->chip.base + offset);
@@ -378,7 +376,7 @@ static int gpio_to_irq_banked(struct gpio_chip *chip, unsigned offset)
 
 static int gpio_to_irq_unbanked(struct gpio_chip *chip, unsigned offset)
 {
-	struct davinci_gpio_controller *d = chip2controller(chip);
+	struct davinci_gpio_controller *d = gpiochip_get_data(chip);
 
 	/*
 	 * NOTE:  we assume for now that only irqs in the first gpio_chip
@@ -514,7 +512,7 @@ static int davinci_gpio_irq_setup(struct platform_device *pdev)
 			return irq;
 		}
 
-		irq_domain = irq_domain_add_legacy(NULL, ngpio, irq, 0,
+		irq_domain = irq_domain_add_legacy(dev->of_node, ngpio, irq, 0,
 							&davinci_gpio_irq_ops,
 							chips);
 		if (!irq_domain) {
diff --git a/drivers/gpio/gpio-dln2.c b/drivers/gpio/gpio-dln2.c
index 6685712c15cf..e11a7d126e74 100644
--- a/drivers/gpio/gpio-dln2.c
+++ b/drivers/gpio/gpio-dln2.c
@@ -153,7 +153,7 @@ static int dln2_gpio_pin_set_out_val(struct dln2_gpio *dln2,
 
 static int dln2_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 	struct dln2_gpio_pin req = {
 		.pin = cpu_to_le16(offset),
 	};
@@ -194,14 +194,14 @@ out_disable:
 
 static void dln2_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 
 	dln2_gpio_pin_cmd(dln2, DLN2_GPIO_PIN_DISABLE, offset);
 }
 
 static int dln2_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 
 	if (test_bit(offset, dln2->output_enabled))
 		return GPIOF_DIR_OUT;
@@ -211,7 +211,7 @@ static int dln2_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 
 static int dln2_gpio_get(struct gpio_chip *chip, unsigned int offset)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 	int dir;
 
 	dir = dln2_gpio_get_direction(chip, offset);
@@ -226,7 +226,7 @@ static int dln2_gpio_get(struct gpio_chip *chip, unsigned int offset)
 
 static void dln2_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 
 	dln2_gpio_pin_set_out_val(dln2, offset, value);
 }
@@ -234,7 +234,7 @@ static void dln2_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int dln2_gpio_set_direction(struct gpio_chip *chip, unsigned offset,
 				   unsigned dir)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 	struct dln2_gpio_pin_val req = {
 		.pin = cpu_to_le16(offset),
 		.value = dir,
@@ -262,7 +262,7 @@ static int dln2_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int dln2_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				      int value)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 	int ret;
 
 	ret = dln2_gpio_pin_set_out_val(dln2, offset, value);
@@ -275,7 +275,7 @@ static int dln2_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 static int dln2_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
 				  unsigned debounce)
 {
-	struct dln2_gpio *dln2 = container_of(chip, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(chip);
 	__le32 duration = cpu_to_le32(debounce);
 
 	return dln2_transfer_tx(dln2->pdev, DLN2_GPIO_SET_DEBOUNCE,
@@ -302,7 +302,7 @@ static int dln2_gpio_set_event_cfg(struct dln2_gpio *dln2, unsigned pin,
 static void dln2_irq_unmask(struct irq_data *irqd)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
-	struct dln2_gpio *dln2 = container_of(gc, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(gc);
 	int pin = irqd_to_hwirq(irqd);
 
 	set_bit(pin, dln2->unmasked_irqs);
@@ -311,7 +311,7 @@ static void dln2_irq_unmask(struct irq_data *irqd)
 static void dln2_irq_mask(struct irq_data *irqd)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
-	struct dln2_gpio *dln2 = container_of(gc, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(gc);
 	int pin = irqd_to_hwirq(irqd);
 
 	clear_bit(pin, dln2->unmasked_irqs);
@@ -320,7 +320,7 @@ static void dln2_irq_mask(struct irq_data *irqd)
 static int dln2_irq_set_type(struct irq_data *irqd, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
-	struct dln2_gpio *dln2 = container_of(gc, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(gc);
 	int pin = irqd_to_hwirq(irqd);
 
 	switch (type) {
@@ -349,7 +349,7 @@ static int dln2_irq_set_type(struct irq_data *irqd, unsigned type)
 static void dln2_irq_bus_lock(struct irq_data *irqd)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
-	struct dln2_gpio *dln2 = container_of(gc, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(gc);
 
 	mutex_lock(&dln2->irq_lock);
 }
@@ -357,7 +357,7 @@ static void dln2_irq_bus_lock(struct irq_data *irqd)
 static void dln2_irq_bus_unlock(struct irq_data *irqd)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(irqd);
-	struct dln2_gpio *dln2 = container_of(gc, struct dln2_gpio, gpio);
+	struct dln2_gpio *dln2 = gpiochip_get_data(gc);
 	int pin = irqd_to_hwirq(irqd);
 	int enabled, unmasked;
 	unsigned type;
@@ -377,7 +377,7 @@ static void dln2_irq_bus_unlock(struct irq_data *irqd)
 
 		ret = dln2_gpio_set_event_cfg(dln2, pin, type, 0);
 		if (ret)
-			dev_err(dln2->gpio.dev, "failed to set event\n");
+			dev_err(dln2->gpio.parent, "failed to set event\n");
 	}
 
 	mutex_unlock(&dln2->irq_lock);
@@ -406,19 +406,19 @@ static void dln2_gpio_event(struct platform_device *pdev, u16 echo,
 	struct dln2_gpio *dln2 = platform_get_drvdata(pdev);
 
 	if (len < sizeof(*event)) {
-		dev_err(dln2->gpio.dev, "short event message\n");
+		dev_err(dln2->gpio.parent, "short event message\n");
 		return;
 	}
 
 	pin = le16_to_cpu(event->pin);
 	if (pin >= dln2->gpio.ngpio) {
-		dev_err(dln2->gpio.dev, "out of bounds pin %d\n", pin);
+		dev_err(dln2->gpio.parent, "out of bounds pin %d\n", pin);
 		return;
 	}
 
 	irq = irq_find_mapping(dln2->gpio.irqdomain, pin);
 	if (!irq) {
-		dev_err(dln2->gpio.dev, "pin %d not mapped to IRQ\n", pin);
+		dev_err(dln2->gpio.parent, "pin %d not mapped to IRQ\n", pin);
 		return;
 	}
 
@@ -462,7 +462,7 @@ static int dln2_gpio_probe(struct platform_device *pdev)
 	dln2->pdev = pdev;
 
 	dln2->gpio.label = "dln2";
-	dln2->gpio.dev = dev;
+	dln2->gpio.parent = dev;
 	dln2->gpio.owner = THIS_MODULE;
 	dln2->gpio.base = -1;
 	dln2->gpio.ngpio = pins;
@@ -479,7 +479,7 @@ static int dln2_gpio_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dln2);
 
-	ret = gpiochip_add(&dln2->gpio);
+	ret = gpiochip_add_data(&dln2->gpio, dln2);
 	if (ret < 0) {
 		dev_err(dev, "failed to add gpio chip: %d\n", ret);
 		goto out;
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index fcd5b0acfc72..597de1ef497b 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -7,7 +7,9 @@
  *
  * All enquiries to support@picochip.com
  */
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
+/* FIXME: for gpio_get_value(), replace this with direct register read */
+#include <linux/gpio.h>
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
@@ -66,7 +68,7 @@ struct dwapb_context {
 #endif
 
 struct dwapb_gpio_port {
-	struct bgpio_chip	bgc;
+	struct gpio_chip	gc;
 	bool			is_registered;
 	struct dwapb_gpio	*gpio;
 #ifdef CONFIG_PM_SLEEP
@@ -83,33 +85,26 @@ struct dwapb_gpio {
 	struct irq_domain	*domain;
 };
 
-static inline struct dwapb_gpio_port *
-to_dwapb_gpio_port(struct bgpio_chip *bgc)
-{
-	return container_of(bgc, struct dwapb_gpio_port, bgc);
-}
-
 static inline u32 dwapb_read(struct dwapb_gpio *gpio, unsigned int offset)
 {
-	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	struct gpio_chip *gc	= &gpio->ports[0].gc;
 	void __iomem *reg_base	= gpio->regs;
 
-	return bgc->read_reg(reg_base + offset);
+	return gc->read_reg(reg_base + offset);
 }
 
 static inline void dwapb_write(struct dwapb_gpio *gpio, unsigned int offset,
 			       u32 val)
 {
-	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	struct gpio_chip *gc	= &gpio->ports[0].gc;
 	void __iomem *reg_base	= gpio->regs;
 
-	bgc->write_reg(reg_base + offset, val);
+	gc->write_reg(reg_base + offset, val);
 }
 
 static int dwapb_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct dwapb_gpio_port *port = to_dwapb_gpio_port(bgc);
+	struct dwapb_gpio_port *port = gpiochip_get_data(gc);
 	struct dwapb_gpio *gpio = port->gpio;
 
 	return irq_find_mapping(gpio->domain, offset);
@@ -119,7 +114,7 @@ static void dwapb_toggle_trigger(struct dwapb_gpio *gpio, unsigned int offs)
 {
 	u32 v = dwapb_read(gpio, GPIO_INT_POLARITY);
 
-	if (gpio_get_value(gpio->ports[0].bgc.gc.base + offs))
+	if (gpio_get_value(gpio->ports[0].gc.base + offs))
 		v &= ~BIT(offs);
 	else
 		v |= BIT(offs);
@@ -162,39 +157,39 @@ static void dwapb_irq_enable(struct irq_data *d)
 {
 	struct irq_chip_generic *igc = irq_data_get_irq_chip_data(d);
 	struct dwapb_gpio *gpio = igc->private;
-	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
+	struct gpio_chip *gc = &gpio->ports[0].gc;
 	unsigned long flags;
 	u32 val;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 	val = dwapb_read(gpio, GPIO_INTEN);
 	val |= BIT(d->hwirq);
 	dwapb_write(gpio, GPIO_INTEN, val);
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
 static void dwapb_irq_disable(struct irq_data *d)
 {
 	struct irq_chip_generic *igc = irq_data_get_irq_chip_data(d);
 	struct dwapb_gpio *gpio = igc->private;
-	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
+	struct gpio_chip *gc = &gpio->ports[0].gc;
 	unsigned long flags;
 	u32 val;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 	val = dwapb_read(gpio, GPIO_INTEN);
 	val &= ~BIT(d->hwirq);
 	dwapb_write(gpio, GPIO_INTEN, val);
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
 static int dwapb_irq_reqres(struct irq_data *d)
 {
 	struct irq_chip_generic *igc = irq_data_get_irq_chip_data(d);
 	struct dwapb_gpio *gpio = igc->private;
-	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
+	struct gpio_chip *gc = &gpio->ports[0].gc;
 
-	if (gpiochip_lock_as_irq(&bgc->gc, irqd_to_hwirq(d))) {
+	if (gpiochip_lock_as_irq(gc, irqd_to_hwirq(d))) {
 		dev_err(gpio->dev, "unable to lock HW IRQ %lu for IRQ\n",
 			irqd_to_hwirq(d));
 		return -EINVAL;
@@ -206,16 +201,16 @@ static void dwapb_irq_relres(struct irq_data *d)
 {
 	struct irq_chip_generic *igc = irq_data_get_irq_chip_data(d);
 	struct dwapb_gpio *gpio = igc->private;
-	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
+	struct gpio_chip *gc = &gpio->ports[0].gc;
 
-	gpiochip_unlock_as_irq(&bgc->gc, irqd_to_hwirq(d));
+	gpiochip_unlock_as_irq(gc, irqd_to_hwirq(d));
 }
 
 static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 {
 	struct irq_chip_generic *igc = irq_data_get_irq_chip_data(d);
 	struct dwapb_gpio *gpio = igc->private;
-	struct bgpio_chip *bgc = &gpio->ports[0].bgc;
+	struct gpio_chip *gc = &gpio->ports[0].gc;
 	int bit = d->hwirq;
 	unsigned long level, polarity, flags;
 
@@ -223,7 +218,7 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 		     IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW))
 		return -EINVAL;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 	level = dwapb_read(gpio, GPIO_INTTYPE_LEVEL);
 	polarity = dwapb_read(gpio, GPIO_INT_POLARITY);
 
@@ -254,7 +249,7 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 
 	dwapb_write(gpio, GPIO_INTTYPE_LEVEL, level);
 	dwapb_write(gpio, GPIO_INT_POLARITY, polarity);
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
@@ -262,13 +257,12 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 static int dwapb_gpio_set_debounce(struct gpio_chip *gc,
 				   unsigned offset, unsigned debounce)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct dwapb_gpio_port *port = to_dwapb_gpio_port(bgc);
+	struct dwapb_gpio_port *port = gpiochip_get_data(gc);
 	struct dwapb_gpio *gpio = port->gpio;
 	unsigned long flags, val_deb;
-	unsigned long mask = bgc->pin2mask(bgc, offset);
+	unsigned long mask = gc->pin2mask(gc, offset);
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
 	val_deb = dwapb_read(gpio, GPIO_PORTA_DEBOUNCE);
 	if (debounce)
@@ -276,7 +270,7 @@ static int dwapb_gpio_set_debounce(struct gpio_chip *gc,
 	else
 		dwapb_write(gpio, GPIO_PORTA_DEBOUNCE, val_deb & ~mask);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
@@ -295,7 +289,7 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 				 struct dwapb_gpio_port *port,
 				 struct dwapb_port_property *pp)
 {
-	struct gpio_chip *gc = &port->bgc.gc;
+	struct gpio_chip *gc = &port->gc;
 	struct device_node *node = pp->node;
 	struct irq_chip_generic	*irq_gc = NULL;
 	unsigned int hwirq, ngpio = gc->ngpio;
@@ -369,13 +363,13 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	for (hwirq = 0 ; hwirq < ngpio ; hwirq++)
 		irq_create_mapping(gpio->domain, hwirq);
 
-	port->bgc.gc.to_irq = dwapb_gpio_to_irq;
+	port->gc.to_irq = dwapb_gpio_to_irq;
 }
 
 static void dwapb_irq_teardown(struct dwapb_gpio *gpio)
 {
 	struct dwapb_gpio_port *port = &gpio->ports[0];
-	struct gpio_chip *gc = &port->bgc.gc;
+	struct gpio_chip *gc = &port->gc;
 	unsigned int ngpio = gc->ngpio;
 	irq_hw_number_t hwirq;
 
@@ -412,7 +406,7 @@ static int dwapb_gpio_add_port(struct dwapb_gpio *gpio,
 	dirout = gpio->regs + GPIO_SWPORTA_DDR +
 		(pp->idx * GPIO_SWPORT_DDR_SIZE);
 
-	err = bgpio_init(&port->bgc, gpio->dev, 4, dat, set, NULL, dirout,
+	err = bgpio_init(&port->gc, gpio->dev, 4, dat, set, NULL, dirout,
 			 NULL, false);
 	if (err) {
 		dev_err(gpio->dev, "failed to init gpio chip for %s\n",
@@ -421,19 +415,19 @@ static int dwapb_gpio_add_port(struct dwapb_gpio *gpio,
 	}
 
 #ifdef CONFIG_OF_GPIO
-	port->bgc.gc.of_node = pp->node;
+	port->gc.of_node = pp->node;
 #endif
-	port->bgc.gc.ngpio = pp->ngpio;
-	port->bgc.gc.base = pp->gpio_base;
+	port->gc.ngpio = pp->ngpio;
+	port->gc.base = pp->gpio_base;
 
 	/* Only port A support debounce */
 	if (pp->idx == 0)
-		port->bgc.gc.set_debounce = dwapb_gpio_set_debounce;
+		port->gc.set_debounce = dwapb_gpio_set_debounce;
 
 	if (pp->irq)
 		dwapb_configure_irqs(gpio, port, pp);
 
-	err = gpiochip_add(&port->bgc.gc);
+	err = gpiochip_add_data(&port->gc, port);
 	if (err)
 		dev_err(gpio->dev, "failed to register gpiochip for %s\n",
 			pp->name);
@@ -449,7 +443,7 @@ static void dwapb_gpio_unregister(struct dwapb_gpio *gpio)
 
 	for (m = 0; m < gpio->nr_ports; ++m)
 		if (gpio->ports[m].is_registered)
-			gpiochip_remove(&gpio->ports[m].bgc.gc);
+			gpiochip_remove(&gpio->ports[m].gc);
 }
 
 static struct dwapb_platform_data *
@@ -591,11 +585,11 @@ static int dwapb_gpio_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dwapb_gpio *gpio = platform_get_drvdata(pdev);
-	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	struct gpio_chip *gc	= &gpio->ports[0].gc;
 	unsigned long flags;
 	int i;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 	for (i = 0; i < gpio->nr_ports; i++) {
 		unsigned int offset;
 		unsigned int idx = gpio->ports[i].idx;
@@ -624,7 +618,7 @@ static int dwapb_gpio_suspend(struct device *dev)
 			dwapb_write(gpio, GPIO_INTMASK, 0xffffffff);
 		}
 	}
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
@@ -633,11 +627,11 @@ static int dwapb_gpio_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dwapb_gpio *gpio = platform_get_drvdata(pdev);
-	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	struct gpio_chip *gc	= &gpio->ports[0].gc;
 	unsigned long flags;
 	int i;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 	for (i = 0; i < gpio->nr_ports; i++) {
 		unsigned int offset;
 		unsigned int idx = gpio->ports[i].idx;
@@ -666,7 +660,7 @@ static int dwapb_gpio_resume(struct device *dev)
 			dwapb_write(gpio, GPIO_PORTA_EOI, 0xffffffff);
 		}
 	}
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
diff --git a/drivers/gpio/gpio-em.c b/drivers/gpio/gpio-em.c
index 6bca1e125e12..8d32ccc980d9 100644
--- a/drivers/gpio/gpio-em.c
+++ b/drivers/gpio/gpio-em.c
@@ -103,7 +103,7 @@ static int em_gio_irq_reqres(struct irq_data *d)
 	struct em_gio_priv *p = irq_data_get_irq_chip_data(d);
 
 	if (gpiochip_lock_as_irq(&p->gpio_chip, irqd_to_hwirq(d))) {
-		dev_err(p->gpio_chip.dev,
+		dev_err(p->gpio_chip.parent,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			irqd_to_hwirq(d));
 		return -EINVAL;
@@ -192,7 +192,7 @@ static irqreturn_t em_gio_irq_handler(int irq, void *dev_id)
 
 static inline struct em_gio_priv *gpio_to_priv(struct gpio_chip *chip)
 {
-	return container_of(chip, struct em_gio_priv, gpio_chip);
+	return gpiochip_get_data(chip);
 }
 
 static int em_gio_direction_input(struct gpio_chip *chip, unsigned offset)
@@ -203,7 +203,7 @@ static int em_gio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int em_gio_get(struct gpio_chip *chip, unsigned offset)
 {
-	return (int)(em_gio_read(gpio_to_priv(chip), GIO_I) & BIT(offset));
+	return !!(em_gio_read(gpio_to_priv(chip), GIO_I) & BIT(offset));
 }
 
 static void __em_gio_set(struct gpio_chip *chip, unsigned int reg,
@@ -332,7 +332,7 @@ static int em_gio_probe(struct platform_device *pdev)
 	gpio_chip->request = em_gio_request;
 	gpio_chip->free = em_gio_free;
 	gpio_chip->label = name;
-	gpio_chip->dev = &pdev->dev;
+	gpio_chip->parent = &pdev->dev;
 	gpio_chip->owner = THIS_MODULE;
 	gpio_chip->base = -1;
 	gpio_chip->ngpio = ngpios;
@@ -368,7 +368,7 @@ static int em_gio_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
-	ret = gpiochip_add(gpio_chip);
+	ret = gpiochip_add_data(gpio_chip, p);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add GPIO controller\n");
 		goto err1;
diff --git a/drivers/gpio/gpio-ep93xx.c b/drivers/gpio/gpio-ep93xx.c
index 3e3947b35c83..ad279078fed7 100644
--- a/drivers/gpio/gpio-ep93xx.c
+++ b/drivers/gpio/gpio-ep93xx.c
@@ -16,10 +16,11 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
-#include <linux/gpio.h>
 #include <linux/irq.h>
 #include <linux/slab.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
+/* FIXME: this is here for gpio_to_irq() - get rid of this! */
+#include <linux/gpio.h>
 
 #include <mach/hardware.h>
 #include <mach/gpio-ep93xx.h>
@@ -28,7 +29,7 @@
 
 struct ep93xx_gpio {
 	void __iomem		*mmio_base;
-	struct bgpio_chip	bgc[8];
+	struct gpio_chip	gc[8];
 };
 
 /*************************************************************************
@@ -319,26 +320,26 @@ static int ep93xx_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 	return 64 + gpio;
 }
 
-static int ep93xx_gpio_add_bank(struct bgpio_chip *bgc, struct device *dev,
+static int ep93xx_gpio_add_bank(struct gpio_chip *gc, struct device *dev,
 	void __iomem *mmio_base, struct ep93xx_gpio_bank *bank)
 {
 	void __iomem *data = mmio_base + bank->data;
 	void __iomem *dir =  mmio_base + bank->dir;
 	int err;
 
-	err = bgpio_init(bgc, dev, 1, data, NULL, NULL, dir, NULL, 0);
+	err = bgpio_init(gc, dev, 1, data, NULL, NULL, dir, NULL, 0);
 	if (err)
 		return err;
 
-	bgc->gc.label = bank->label;
-	bgc->gc.base = bank->base;
+	gc->label = bank->label;
+	gc->base = bank->base;
 
 	if (bank->has_debounce) {
-		bgc->gc.set_debounce = ep93xx_gpio_set_debounce;
-		bgc->gc.to_irq = ep93xx_gpio_to_irq;
+		gc->set_debounce = ep93xx_gpio_set_debounce;
+		gc->to_irq = ep93xx_gpio_to_irq;
 	}
 
-	return gpiochip_add(&bgc->gc);
+	return gpiochip_add_data(gc, NULL);
 }
 
 static int ep93xx_gpio_probe(struct platform_device *pdev)
@@ -358,10 +359,10 @@ static int ep93xx_gpio_probe(struct platform_device *pdev)
 		return PTR_ERR(ep93xx_gpio->mmio_base);
 
 	for (i = 0; i < ARRAY_SIZE(ep93xx_gpio_banks); i++) {
-		struct bgpio_chip *bgc = &ep93xx_gpio->bgc[i];
+		struct gpio_chip *gc = &ep93xx_gpio->gc[i];
 		struct ep93xx_gpio_bank *bank = &ep93xx_gpio_banks[i];
 
-		if (ep93xx_gpio_add_bank(bgc, &pdev->dev,
+		if (ep93xx_gpio_add_bank(gc, &pdev->dev,
 					 ep93xx_gpio->mmio_base, bank))
 			dev_warn(&pdev->dev, "Unable to add gpio bank %s\n",
 				bank->label);
diff --git a/drivers/gpio/gpio-etraxfs.c b/drivers/gpio/gpio-etraxfs.c
index 5c15dd12172d..00b022c9acb3 100644
--- a/drivers/gpio/gpio-etraxfs.c
+++ b/drivers/gpio/gpio-etraxfs.c
@@ -1,12 +1,10 @@
 #include <linux/kernel.h>
 #include <linux/init.h>
-#include <linux/gpio.h>
 #include <linux/gpio/driver.h>
 #include <linux/of_gpio.h>
 #include <linux/io.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
-#include <linux/basic_mmio_gpio.h>
 
 #define ETRAX_FS_rw_pa_dout	0
 #define ETRAX_FS_r_pa_din	4
@@ -67,7 +65,7 @@ struct etraxfs_gpio_block {
 };
 
 struct etraxfs_gpio_chip {
-	struct bgpio_chip bgc;
+	struct gpio_chip gc;
 	struct etraxfs_gpio_block *block;
 };
 
@@ -176,11 +174,6 @@ static const struct etraxfs_gpio_info etraxfs_gpio_artpec3 = {
 	.rw_intr_pins	= ARTPEC3_rw_intr_pins,
 };
 
-static struct etraxfs_gpio_chip *to_etraxfs(struct gpio_chip *gc)
-{
-	return container_of(gc, struct etraxfs_gpio_chip, bgc.gc);
-}
-
 static unsigned int etraxfs_gpio_chip_to_port(struct gpio_chip *gc)
 {
 	return gc->label[0] - 'A';
@@ -220,13 +213,13 @@ static unsigned int etraxfs_gpio_to_group_irq(unsigned int gpio)
 static unsigned int etraxfs_gpio_to_group_pin(struct etraxfs_gpio_chip *chip,
 					      unsigned int gpio)
 {
-	return 4 * etraxfs_gpio_chip_to_port(&chip->bgc.gc) + gpio / 8;
+	return 4 * etraxfs_gpio_chip_to_port(&chip->gc) + gpio / 8;
 }
 
 static void etraxfs_gpio_irq_ack(struct irq_data *d)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 
@@ -236,7 +229,7 @@ static void etraxfs_gpio_irq_ack(struct irq_data *d)
 static void etraxfs_gpio_irq_mask(struct irq_data *d)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 
@@ -249,7 +242,7 @@ static void etraxfs_gpio_irq_mask(struct irq_data *d)
 static void etraxfs_gpio_irq_unmask(struct irq_data *d)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 
@@ -262,7 +255,7 @@ static void etraxfs_gpio_irq_unmask(struct irq_data *d)
 static int etraxfs_gpio_irq_set_type(struct irq_data *d, u32 type)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 	u32 cfg;
@@ -299,7 +292,7 @@ static int etraxfs_gpio_irq_set_type(struct irq_data *d, u32 type)
 static int etraxfs_gpio_irq_request_resources(struct irq_data *d)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 	int ret = -EBUSY;
@@ -308,7 +301,7 @@ static int etraxfs_gpio_irq_request_resources(struct irq_data *d)
 	if (block->group[grpirq])
 		goto out;
 
-	ret = gpiochip_lock_as_irq(&chip->bgc.gc, d->hwirq);
+	ret = gpiochip_lock_as_irq(&chip->gc, d->hwirq);
 	if (ret)
 		goto out;
 
@@ -330,13 +323,13 @@ out:
 static void etraxfs_gpio_irq_release_resources(struct irq_data *d)
 {
 	struct etraxfs_gpio_chip *chip =
-		to_etraxfs(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	struct etraxfs_gpio_block *block = chip->block;
 	unsigned int grpirq = etraxfs_gpio_to_group_irq(d->hwirq);
 
 	spin_lock(&block->lock);
 	block->group[grpirq] = 0;
-	gpiochip_unlock_as_irq(&chip->bgc.gc, d->hwirq);
+	gpiochip_unlock_as_irq(&chip->gc, d->hwirq);
 	spin_unlock(&block->lock);
 }
 
@@ -419,7 +412,7 @@ static int etraxfs_gpio_probe(struct platform_device *pdev)
 
 	for (i = 0; i < info->num_ports; i++) {
 		struct etraxfs_gpio_chip *chip = &chips[i];
-		struct bgpio_chip *bgc = &chip->bgc;
+		struct gpio_chip *gc = &chip->gc;
 		const struct etraxfs_gpio_port *port = &info->ports[i];
 		unsigned long flags = BGPIOF_READ_OUTPUT_REG_SET;
 		void __iomem *dat = regs + port->din;
@@ -433,7 +426,7 @@ static int etraxfs_gpio_probe(struct platform_device *pdev)
 			flags = BGPIOF_NO_OUTPUT;
 		}
 
-		ret = bgpio_init(bgc, dev, 4,
+		ret = bgpio_init(gc, dev, 4,
 				 dat, set, NULL, dirout, NULL,
 				 flags);
 		if (ret) {
@@ -442,28 +435,28 @@ static int etraxfs_gpio_probe(struct platform_device *pdev)
 			continue;
 		}
 
-		bgc->gc.ngpio = port->ngpio;
-		bgc->gc.label = port->label;
+		gc->ngpio = port->ngpio;
+		gc->label = port->label;
 
-		bgc->gc.of_node = dev->of_node;
-		bgc->gc.of_gpio_n_cells = 3;
-		bgc->gc.of_xlate = etraxfs_gpio_of_xlate;
+		gc->of_node = dev->of_node;
+		gc->of_gpio_n_cells = 3;
+		gc->of_xlate = etraxfs_gpio_of_xlate;
 
-		ret = gpiochip_add(&bgc->gc);
+		ret = gpiochip_add_data(gc, chip);
 		if (ret) {
 			dev_err(dev, "Unable to register port %s\n",
-				bgc->gc.label);
+				gc->label);
 			continue;
 		}
 
 		if (i > 0 && !allportsirq)
 			continue;
 
-		ret = gpiochip_irqchip_add(&bgc->gc, &etraxfs_gpio_irq_chip, 0,
+		ret = gpiochip_irqchip_add(gc, &etraxfs_gpio_irq_chip, 0,
 					   handle_level_irq, IRQ_TYPE_NONE);
 		if (ret) {
 			dev_err(dev, "Unable to add irqchip to port %s\n",
-				bgc->gc.label);
+				gc->label);
 		}
 	}
 
diff --git a/drivers/gpio/gpio-f7188x.c b/drivers/gpio/gpio-f7188x.c
index 5e3c4fa67d82..d62fd6bbaf82 100644
--- a/drivers/gpio/gpio-f7188x.c
+++ b/drivers/gpio/gpio-f7188x.c
@@ -193,8 +193,7 @@ static struct f7188x_gpio_bank f71889_gpio_bank[] = {
 static int f7188x_gpio_direction_in(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;
 
@@ -215,8 +214,7 @@ static int f7188x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 static int f7188x_gpio_get(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, data;
 
@@ -241,8 +239,7 @@ static int f7188x_gpio_direction_out(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
 	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, data_out;
 
@@ -270,8 +267,7 @@ static int f7188x_gpio_direction_out(struct gpio_chip *chip,
 static void f7188x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
 	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 data_out;
 
@@ -298,7 +294,7 @@ static int f7188x_gpio_probe(struct platform_device *pdev)
 {
 	int err;
 	int i;
-	struct f7188x_sio *sio = pdev->dev.platform_data;
+	struct f7188x_sio *sio = dev_get_platdata(&pdev->dev);
 	struct f7188x_gpio_data *data;
 
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
@@ -333,10 +329,10 @@ static int f7188x_gpio_probe(struct platform_device *pdev)
 	for (i = 0; i < data->nr_bank; i++) {
 		struct f7188x_gpio_bank *bank = &data->bank[i];
 
-		bank->chip.dev = &pdev->dev;
+		bank->chip.parent = &pdev->dev;
 		bank->data = data;
 
-		err = gpiochip_add(&bank->chip);
+		err = gpiochip_add_data(&bank->chip, bank);
 		if (err) {
 			dev_err(&pdev->dev,
 				"Failed to register gpiochip %d: %d\n",
diff --git a/drivers/gpio/gpio-ge.c b/drivers/gpio/gpio-ge.c
index f9ac3f351753..cbbec838a9d1 100644
--- a/drivers/gpio/gpio-ge.c
+++ b/drivers/gpio/gpio-ge.c
@@ -24,7 +24,7 @@
 #include <linux/of_gpio.h>
 #include <linux/of_address.h>
 #include <linux/module.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 
 #define GEF_GPIO_DIRECT		0x00
 #define GEF_GPIO_IN		0x04
@@ -55,19 +55,19 @@ static int __init gef_gpio_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
 		of_match_device(gef_gpio_ids, &pdev->dev);
-	struct bgpio_chip *bgc;
+	struct gpio_chip *gc;
 	void __iomem *regs;
 	int ret;
 
-	bgc = devm_kzalloc(&pdev->dev, sizeof(*bgc), GFP_KERNEL);
-	if (!bgc)
+	gc = devm_kzalloc(&pdev->dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
 		return -ENOMEM;
 
 	regs = of_iomap(pdev->dev.of_node, 0);
 	if (!regs)
 		return -ENOMEM;
 
-	ret = bgpio_init(bgc, &pdev->dev, 4, regs + GEF_GPIO_IN,
+	ret = bgpio_init(gc, &pdev->dev, 4, regs + GEF_GPIO_IN,
 			 regs + GEF_GPIO_OUT, NULL, NULL,
 			 regs + GEF_GPIO_DIRECT, BGPIOF_BIG_ENDIAN_BYTE_ORDER);
 	if (ret) {
@@ -76,20 +76,20 @@ static int __init gef_gpio_probe(struct platform_device *pdev)
 	}
 
 	/* Setup pointers to chip functions */
-	bgc->gc.label = devm_kstrdup(&pdev->dev, pdev->dev.of_node->full_name,
+	gc->label = devm_kstrdup(&pdev->dev, pdev->dev.of_node->full_name,
 				     GFP_KERNEL);
-	if (!bgc->gc.label) {
+	if (!gc->label) {
 		ret = -ENOMEM;
 		goto err0;
 	}
 
-	bgc->gc.base = -1;
-	bgc->gc.ngpio = (u16)(uintptr_t)of_id->data;
-	bgc->gc.of_gpio_n_cells = 2;
-	bgc->gc.of_node = pdev->dev.of_node;
+	gc->base = -1;
+	gc->ngpio = (u16)(uintptr_t)of_id->data;
+	gc->of_gpio_n_cells = 2;
+	gc->of_node = pdev->dev.of_node;
 
 	/* This function adds a memory mapped GPIO chip */
-	ret = gpiochip_add(&bgc->gc);
+	ret = gpiochip_add_data(gc, NULL);
 	if (ret)
 		goto err0;
 
diff --git a/drivers/gpio/gpio-generic.c b/drivers/gpio/gpio-generic.c
index 88ae70ddb127..2a4f2333a50b 100644
--- a/drivers/gpio/gpio-generic.c
+++ b/drivers/gpio/gpio-generic.c
@@ -56,11 +56,11 @@ o        `                     ~~~~\___/~~~~    ` controller in FPGA is ,.`
 #include <linux/log2.h>
 #include <linux/ioport.h>
 #include <linux/io.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/slab.h>
+#include <linux/bitops.h>
 #include <linux/platform_device.h>
 #include <linux/mod_devicetable.h>
-#include <linux/basic_mmio_gpio.h>
 
 static void bgpio_write8(void __iomem *reg, unsigned long data)
 {
@@ -124,33 +124,30 @@ static unsigned long bgpio_read32be(void __iomem *reg)
 	return ioread32be(reg);
 }
 
-static unsigned long bgpio_pin2mask(struct bgpio_chip *bgc, unsigned int pin)
+static unsigned long bgpio_pin2mask(struct gpio_chip *gc, unsigned int pin)
 {
-	return 1 << pin;
+	return BIT(pin);
 }
 
-static unsigned long bgpio_pin2mask_be(struct bgpio_chip *bgc,
+static unsigned long bgpio_pin2mask_be(struct gpio_chip *gc,
 				       unsigned int pin)
 {
-	return 1 << (bgc->bits - 1 - pin);
+	return BIT(gc->bgpio_bits - 1 - pin);
 }
 
 static int bgpio_get_set(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	unsigned long pinmask = bgc->pin2mask(bgc, gpio);
+	unsigned long pinmask = gc->pin2mask(gc, gpio);
 
-	if (bgc->dir & pinmask)
-		return !!(bgc->read_reg(bgc->reg_set) & pinmask);
+	if (gc->bgpio_dir & pinmask)
+		return !!(gc->read_reg(gc->reg_set) & pinmask);
 	else
-		return !!(bgc->read_reg(bgc->reg_dat) & pinmask);
+		return !!(gc->read_reg(gc->reg_dat) & pinmask);
 }
 
 static int bgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return !!(bgc->read_reg(bgc->reg_dat) & bgc->pin2mask(bgc, gpio));
+	return !!(gc->read_reg(gc->reg_dat) & gc->pin2mask(gc, gpio));
 }
 
 static void bgpio_set_none(struct gpio_chip *gc, unsigned int gpio, int val)
@@ -159,53 +156,50 @@ static void bgpio_set_none(struct gpio_chip *gc, unsigned int gpio, int val)
 
 static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	unsigned long mask = bgc->pin2mask(bgc, gpio);
+	unsigned long mask = gc->pin2mask(gc, gpio);
 	unsigned long flags;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
 	if (val)
-		bgc->data |= mask;
+		gc->bgpio_data |= mask;
 	else
-		bgc->data &= ~mask;
+		gc->bgpio_data &= ~mask;
 
-	bgc->write_reg(bgc->reg_dat, bgc->data);
+	gc->write_reg(gc->reg_dat, gc->bgpio_data);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
 static void bgpio_set_with_clear(struct gpio_chip *gc, unsigned int gpio,
 				 int val)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	unsigned long mask = bgc->pin2mask(bgc, gpio);
+	unsigned long mask = gc->pin2mask(gc, gpio);
 
 	if (val)
-		bgc->write_reg(bgc->reg_set, mask);
+		gc->write_reg(gc->reg_set, mask);
 	else
-		bgc->write_reg(bgc->reg_clr, mask);
+		gc->write_reg(gc->reg_clr, mask);
 }
 
 static void bgpio_set_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	unsigned long mask = bgc->pin2mask(bgc, gpio);
+	unsigned long mask = gc->pin2mask(gc, gpio);
 	unsigned long flags;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
 	if (val)
-		bgc->data |= mask;
+		gc->bgpio_data |= mask;
 	else
-		bgc->data &= ~mask;
+		gc->bgpio_data &= ~mask;
 
-	bgc->write_reg(bgc->reg_set, bgc->data);
+	gc->write_reg(gc->reg_set, gc->bgpio_data);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
-static void bgpio_multiple_get_masks(struct bgpio_chip *bgc,
+static void bgpio_multiple_get_masks(struct gpio_chip *gc,
 				     unsigned long *mask, unsigned long *bits,
 				     unsigned long *set_mask,
 				     unsigned long *clear_mask)
@@ -215,19 +209,19 @@ static void bgpio_multiple_get_masks(struct bgpio_chip *bgc,
 	*set_mask = 0;
 	*clear_mask = 0;
 
-	for (i = 0; i < bgc->bits; i++) {
+	for (i = 0; i < gc->bgpio_bits; i++) {
 		if (*mask == 0)
 			break;
 		if (__test_and_clear_bit(i, mask)) {
 			if (test_bit(i, bits))
-				*set_mask |= bgc->pin2mask(bgc, i);
+				*set_mask |= gc->pin2mask(gc, i);
 			else
-				*clear_mask |= bgc->pin2mask(bgc, i);
+				*clear_mask |= gc->pin2mask(gc, i);
 		}
 	}
 }
 
-static void bgpio_set_multiple_single_reg(struct bgpio_chip *bgc,
+static void bgpio_set_multiple_single_reg(struct gpio_chip *gc,
 					  unsigned long *mask,
 					  unsigned long *bits,
 					  void __iomem *reg)
@@ -235,47 +229,42 @@ static void bgpio_set_multiple_single_reg(struct bgpio_chip *bgc,
 	unsigned long flags;
 	unsigned long set_mask, clear_mask;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-	bgpio_multiple_get_masks(bgc, mask, bits, &set_mask, &clear_mask);
+	bgpio_multiple_get_masks(gc, mask, bits, &set_mask, &clear_mask);
 
-	bgc->data |= set_mask;
-	bgc->data &= ~clear_mask;
+	gc->bgpio_data |= set_mask;
+	gc->bgpio_data &= ~clear_mask;
 
-	bgc->write_reg(reg, bgc->data);
+	gc->write_reg(reg, gc->bgpio_data);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
 static void bgpio_set_multiple(struct gpio_chip *gc, unsigned long *mask,
 			       unsigned long *bits)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	bgpio_set_multiple_single_reg(bgc, mask, bits, bgc->reg_dat);
+	bgpio_set_multiple_single_reg(gc, mask, bits, gc->reg_dat);
 }
 
 static void bgpio_set_multiple_set(struct gpio_chip *gc, unsigned long *mask,
 				   unsigned long *bits)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	bgpio_set_multiple_single_reg(bgc, mask, bits, bgc->reg_set);
+	bgpio_set_multiple_single_reg(gc, mask, bits, gc->reg_set);
 }
 
 static void bgpio_set_multiple_with_clear(struct gpio_chip *gc,
 					  unsigned long *mask,
 					  unsigned long *bits)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
 	unsigned long set_mask, clear_mask;
 
-	bgpio_multiple_get_masks(bgc, mask, bits, &set_mask, &clear_mask);
+	bgpio_multiple_get_masks(gc, mask, bits, &set_mask, &clear_mask);
 
 	if (set_mask)
-		bgc->write_reg(bgc->reg_set, set_mask);
+		gc->write_reg(gc->reg_set, set_mask);
 	if (clear_mask)
-		bgc->write_reg(bgc->reg_clr, clear_mask);
+		gc->write_reg(gc->reg_clr, clear_mask);
 }
 
 static int bgpio_simple_dir_in(struct gpio_chip *gc, unsigned int gpio)
@@ -299,111 +288,103 @@ static int bgpio_simple_dir_out(struct gpio_chip *gc, unsigned int gpio,
 
 static int bgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
 	unsigned long flags;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-	bgc->dir &= ~bgc->pin2mask(bgc, gpio);
-	bgc->write_reg(bgc->reg_dir, bgc->dir);
+	gc->bgpio_dir &= ~gc->pin2mask(gc, gpio);
+	gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
 
 static int bgpio_get_dir(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return (bgc->read_reg(bgc->reg_dir) & bgc->pin2mask(bgc, gpio)) ?
-	       GPIOF_DIR_OUT : GPIOF_DIR_IN;
+	/* Return 0 if output, 1 of input */
+	return !(gc->read_reg(gc->reg_dir) & gc->pin2mask(gc, gpio));
 }
 
 static int bgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
 	unsigned long flags;
 
 	gc->set(gc, gpio, val);
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-	bgc->dir |= bgc->pin2mask(bgc, gpio);
-	bgc->write_reg(bgc->reg_dir, bgc->dir);
+	gc->bgpio_dir |= gc->pin2mask(gc, gpio);
+	gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
 
 static int bgpio_dir_in_inv(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
 	unsigned long flags;
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-	bgc->dir |= bgc->pin2mask(bgc, gpio);
-	bgc->write_reg(bgc->reg_dir, bgc->dir);
+	gc->bgpio_dir |= gc->pin2mask(gc, gpio);
+	gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
 
 static int bgpio_dir_out_inv(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
 	unsigned long flags;
 
 	gc->set(gc, gpio, val);
 
-	spin_lock_irqsave(&bgc->lock, flags);
+	spin_lock_irqsave(&gc->bgpio_lock, flags);
 
-	bgc->dir &= ~bgc->pin2mask(bgc, gpio);
-	bgc->write_reg(bgc->reg_dir, bgc->dir);
+	gc->bgpio_dir &= ~gc->pin2mask(gc, gpio);
+	gc->write_reg(gc->reg_dir, gc->bgpio_dir);
 
-	spin_unlock_irqrestore(&bgc->lock, flags);
+	spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 
 	return 0;
 }
 
 static int bgpio_get_dir_inv(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return (bgc->read_reg(bgc->reg_dir) & bgc->pin2mask(bgc, gpio)) ?
-	       GPIOF_DIR_IN : GPIOF_DIR_OUT;
+	/* Return 0 if output, 1 if input */
+	return !!(gc->read_reg(gc->reg_dir) & gc->pin2mask(gc, gpio));
 }
 
 static int bgpio_setup_accessors(struct device *dev,
-				 struct bgpio_chip *bgc,
+				 struct gpio_chip *gc,
 				 bool bit_be,
 				 bool byte_be)
 {
 
-	switch (bgc->bits) {
+	switch (gc->bgpio_bits) {
 	case 8:
-		bgc->read_reg	= bgpio_read8;
-		bgc->write_reg	= bgpio_write8;
+		gc->read_reg	= bgpio_read8;
+		gc->write_reg	= bgpio_write8;
 		break;
 	case 16:
 		if (byte_be) {
-			bgc->read_reg	= bgpio_read16be;
-			bgc->write_reg	= bgpio_write16be;
+			gc->read_reg	= bgpio_read16be;
+			gc->write_reg	= bgpio_write16be;
 		} else {
-			bgc->read_reg	= bgpio_read16;
-			bgc->write_reg	= bgpio_write16;
+			gc->read_reg	= bgpio_read16;
+			gc->write_reg	= bgpio_write16;
 		}
 		break;
 	case 32:
 		if (byte_be) {
-			bgc->read_reg	= bgpio_read32be;
-			bgc->write_reg	= bgpio_write32be;
+			gc->read_reg	= bgpio_read32be;
+			gc->write_reg	= bgpio_write32be;
 		} else {
-			bgc->read_reg	= bgpio_read32;
-			bgc->write_reg	= bgpio_write32;
+			gc->read_reg	= bgpio_read32;
+			gc->write_reg	= bgpio_write32;
 		}
 		break;
 #if BITS_PER_LONG >= 64
@@ -413,17 +394,17 @@ static int bgpio_setup_accessors(struct device *dev,
 				"64 bit big endian byte order unsupported\n");
 			return -EINVAL;
 		} else {
-			bgc->read_reg	= bgpio_read64;
-			bgc->write_reg	= bgpio_write64;
+			gc->read_reg	= bgpio_read64;
+			gc->write_reg	= bgpio_write64;
 		}
 		break;
 #endif /* BITS_PER_LONG >= 64 */
 	default:
-		dev_err(dev, "unsupported data width %u bits\n", bgc->bits);
+		dev_err(dev, "unsupported data width %u bits\n", gc->bgpio_bits);
 		return -EINVAL;
 	}
 
-	bgc->pin2mask = bit_be ? bgpio_pin2mask_be : bgpio_pin2mask;
+	gc->pin2mask = bit_be ? bgpio_pin2mask_be : bgpio_pin2mask;
 
 	return 0;
 }
@@ -450,44 +431,44 @@ static int bgpio_setup_accessors(struct device *dev,
  *	- an input direction register (named "dirin") where a 1 bit indicates
  *	the GPIO is an input.
  */
-static int bgpio_setup_io(struct bgpio_chip *bgc,
+static int bgpio_setup_io(struct gpio_chip *gc,
 			  void __iomem *dat,
 			  void __iomem *set,
 			  void __iomem *clr,
 			  unsigned long flags)
 {
 
-	bgc->reg_dat = dat;
-	if (!bgc->reg_dat)
+	gc->reg_dat = dat;
+	if (!gc->reg_dat)
 		return -EINVAL;
 
 	if (set && clr) {
-		bgc->reg_set = set;
-		bgc->reg_clr = clr;
-		bgc->gc.set = bgpio_set_with_clear;
-		bgc->gc.set_multiple = bgpio_set_multiple_with_clear;
+		gc->reg_set = set;
+		gc->reg_clr = clr;
+		gc->set = bgpio_set_with_clear;
+		gc->set_multiple = bgpio_set_multiple_with_clear;
 	} else if (set && !clr) {
-		bgc->reg_set = set;
-		bgc->gc.set = bgpio_set_set;
-		bgc->gc.set_multiple = bgpio_set_multiple_set;
+		gc->reg_set = set;
+		gc->set = bgpio_set_set;
+		gc->set_multiple = bgpio_set_multiple_set;
 	} else if (flags & BGPIOF_NO_OUTPUT) {
-		bgc->gc.set = bgpio_set_none;
-		bgc->gc.set_multiple = NULL;
+		gc->set = bgpio_set_none;
+		gc->set_multiple = NULL;
 	} else {
-		bgc->gc.set = bgpio_set;
-		bgc->gc.set_multiple = bgpio_set_multiple;
+		gc->set = bgpio_set;
+		gc->set_multiple = bgpio_set_multiple;
 	}
 
 	if (!(flags & BGPIOF_UNREADABLE_REG_SET) &&
 	    (flags & BGPIOF_READ_OUTPUT_REG_SET))
-		bgc->gc.get = bgpio_get_set;
+		gc->get = bgpio_get_set;
 	else
-		bgc->gc.get = bgpio_get;
+		gc->get = bgpio_get;
 
 	return 0;
 }
 
-static int bgpio_setup_direction(struct bgpio_chip *bgc,
+static int bgpio_setup_direction(struct gpio_chip *gc,
 				 void __iomem *dirout,
 				 void __iomem *dirin,
 				 unsigned long flags)
@@ -495,21 +476,21 @@ static int bgpio_setup_direction(struct bgpio_chip *bgc,
 	if (dirout && dirin) {
 		return -EINVAL;
 	} else if (dirout) {
-		bgc->reg_dir = dirout;
-		bgc->gc.direction_output = bgpio_dir_out;
-		bgc->gc.direction_input = bgpio_dir_in;
-		bgc->gc.get_direction = bgpio_get_dir;
+		gc->reg_dir = dirout;
+		gc->direction_output = bgpio_dir_out;
+		gc->direction_input = bgpio_dir_in;
+		gc->get_direction = bgpio_get_dir;
 	} else if (dirin) {
-		bgc->reg_dir = dirin;
-		bgc->gc.direction_output = bgpio_dir_out_inv;
-		bgc->gc.direction_input = bgpio_dir_in_inv;
-		bgc->gc.get_direction = bgpio_get_dir_inv;
+		gc->reg_dir = dirin;
+		gc->direction_output = bgpio_dir_out_inv;
+		gc->direction_input = bgpio_dir_in_inv;
+		gc->get_direction = bgpio_get_dir_inv;
 	} else {
 		if (flags & BGPIOF_NO_OUTPUT)
-			bgc->gc.direction_output = bgpio_dir_out_err;
+			gc->direction_output = bgpio_dir_out_err;
 		else
-			bgc->gc.direction_output = bgpio_simple_dir_out;
-		bgc->gc.direction_input = bgpio_simple_dir_in;
+			gc->direction_output = bgpio_simple_dir_out;
+		gc->direction_input = bgpio_simple_dir_in;
 	}
 
 	return 0;
@@ -523,14 +504,7 @@ static int bgpio_request(struct gpio_chip *chip, unsigned gpio_pin)
 	return -EINVAL;
 }
 
-int bgpio_remove(struct bgpio_chip *bgc)
-{
-	gpiochip_remove(&bgc->gc);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(bgpio_remove);
-
-int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
+int bgpio_init(struct gpio_chip *gc, struct device *dev,
 	       unsigned long sz, void __iomem *dat, void __iomem *set,
 	       void __iomem *clr, void __iomem *dirout, void __iomem *dirin,
 	       unsigned long flags)
@@ -540,36 +514,36 @@ int bgpio_init(struct bgpio_chip *bgc, struct device *dev,
 	if (!is_power_of_2(sz))
 		return -EINVAL;
 
-	bgc->bits = sz * 8;
-	if (bgc->bits > BITS_PER_LONG)
+	gc->bgpio_bits = sz * 8;
+	if (gc->bgpio_bits > BITS_PER_LONG)
 		return -EINVAL;
 
-	spin_lock_init(&bgc->lock);
-	bgc->gc.dev = dev;
-	bgc->gc.label = dev_name(dev);
-	bgc->gc.base = -1;
-	bgc->gc.ngpio = bgc->bits;
-	bgc->gc.request = bgpio_request;
+	spin_lock_init(&gc->bgpio_lock);
+	gc->parent = dev;
+	gc->label = dev_name(dev);
+	gc->base = -1;
+	gc->ngpio = gc->bgpio_bits;
+	gc->request = bgpio_request;
 
-	ret = bgpio_setup_io(bgc, dat, set, clr, flags);
+	ret = bgpio_setup_io(gc, dat, set, clr, flags);
 	if (ret)
 		return ret;
 
-	ret = bgpio_setup_accessors(dev, bgc, flags & BGPIOF_BIG_ENDIAN,
+	ret = bgpio_setup_accessors(dev, gc, flags & BGPIOF_BIG_ENDIAN,
 				    flags & BGPIOF_BIG_ENDIAN_BYTE_ORDER);
 	if (ret)
 		return ret;
 
-	ret = bgpio_setup_direction(bgc, dirout, dirin, flags);
+	ret = bgpio_setup_direction(gc, dirout, dirin, flags);
 	if (ret)
 		return ret;
 
-	bgc->data = bgc->read_reg(bgc->reg_dat);
-	if (bgc->gc.set == bgpio_set_set &&
+	gc->bgpio_data = gc->read_reg(gc->reg_dat);
+	if (gc->set == bgpio_set_set &&
 			!(flags & BGPIOF_UNREADABLE_REG_SET))
-		bgc->data = bgc->read_reg(bgc->reg_set);
-	if (bgc->reg_dir && !(flags & BGPIOF_UNREADABLE_REG_DIR))
-		bgc->dir = bgc->read_reg(bgc->reg_dir);
+		gc->bgpio_data = gc->read_reg(gc->reg_set);
+	if (gc->reg_dir && !(flags & BGPIOF_UNREADABLE_REG_DIR))
+		gc->bgpio_dir = gc->read_reg(gc->reg_dir);
 
 	return ret;
 }
@@ -607,7 +581,7 @@ static int bgpio_pdev_probe(struct platform_device *pdev)
 	unsigned long sz;
 	unsigned long flags = pdev->id_entry->driver_data;
 	int err;
-	struct bgpio_chip *bgc;
+	struct gpio_chip *gc;
 	struct bgpio_pdata *pdata = dev_get_platdata(dev);
 
 	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dat");
@@ -636,32 +610,33 @@ static int bgpio_pdev_probe(struct platform_device *pdev)
 	if (IS_ERR(dirin))
 		return PTR_ERR(dirin);
 
-	bgc = devm_kzalloc(&pdev->dev, sizeof(*bgc), GFP_KERNEL);
-	if (!bgc)
+	gc = devm_kzalloc(&pdev->dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
 		return -ENOMEM;
 
-	err = bgpio_init(bgc, dev, sz, dat, set, clr, dirout, dirin, flags);
+	err = bgpio_init(gc, dev, sz, dat, set, clr, dirout, dirin, flags);
 	if (err)
 		return err;
 
 	if (pdata) {
 		if (pdata->label)
-			bgc->gc.label = pdata->label;
-		bgc->gc.base = pdata->base;
+			gc->label = pdata->label;
+		gc->base = pdata->base;
 		if (pdata->ngpio > 0)
-			bgc->gc.ngpio = pdata->ngpio;
+			gc->ngpio = pdata->ngpio;
 	}
 
-	platform_set_drvdata(pdev, bgc);
+	platform_set_drvdata(pdev, gc);
 
-	return gpiochip_add(&bgc->gc);
+	return gpiochip_add_data(gc, NULL);
 }
 
 static int bgpio_pdev_remove(struct platform_device *pdev)
 {
-	struct bgpio_chip *bgc = platform_get_drvdata(pdev);
+	struct gpio_chip *gc = platform_get_drvdata(pdev);
 
-	return bgpio_remove(bgc);
+	gpiochip_remove(gc);
+	return 0;
 }
 
 static const struct platform_device_id bgpio_id_table[] = {
diff --git a/drivers/gpio/gpio-grgpio.c b/drivers/gpio/gpio-grgpio.c
index 801423fe8143..7847dd34f86f 100644
--- a/drivers/gpio/gpio-grgpio.c
+++ b/drivers/gpio/gpio-grgpio.c
@@ -31,7 +31,7 @@
 #include <linux/gpio.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
@@ -63,7 +63,7 @@ struct grgpio_lirq {
 };
 
 struct grgpio_priv {
-	struct bgpio_chip bgc;
+	struct gpio_chip gc;
 	void __iomem *regs;
 	struct device *dev;
 
@@ -92,29 +92,22 @@ struct grgpio_priv {
 	struct grgpio_lirq lirqs[GRGPIO_MAX_NGPIO];
 };
 
-static inline struct grgpio_priv *grgpio_gc_to_priv(struct gpio_chip *gc)
-{
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return container_of(bgc, struct grgpio_priv, bgc);
-}
-
 static void grgpio_set_imask(struct grgpio_priv *priv, unsigned int offset,
 			     int val)
 {
-	struct bgpio_chip *bgc = &priv->bgc;
-	unsigned long mask = bgc->pin2mask(bgc, offset);
+	struct gpio_chip *gc = &priv->gc;
+	unsigned long mask = gc->pin2mask(gc, offset);
 
 	if (val)
 		priv->imask |= mask;
 	else
 		priv->imask &= ~mask;
-	bgc->write_reg(priv->regs + GRGPIO_IMASK, priv->imask);
+	gc->write_reg(priv->regs + GRGPIO_IMASK, priv->imask);
 }
 
 static int grgpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct grgpio_priv *priv = grgpio_gc_to_priv(gc);
+	struct grgpio_priv *priv = gpiochip_get_data(gc);
 
 	if (offset >= gc->ngpio)
 		return -ENXIO;
@@ -158,15 +151,15 @@ static int grgpio_irq_set_type(struct irq_data *d, unsigned int type)
 		return -EINVAL;
 	}
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
-	ipol = priv->bgc.read_reg(priv->regs + GRGPIO_IPOL) & ~mask;
-	iedge = priv->bgc.read_reg(priv->regs + GRGPIO_IEDGE) & ~mask;
+	ipol = priv->gc.read_reg(priv->regs + GRGPIO_IPOL) & ~mask;
+	iedge = priv->gc.read_reg(priv->regs + GRGPIO_IEDGE) & ~mask;
 
-	priv->bgc.write_reg(priv->regs + GRGPIO_IPOL, ipol | pol);
-	priv->bgc.write_reg(priv->regs + GRGPIO_IEDGE, iedge | edge);
+	priv->gc.write_reg(priv->regs + GRGPIO_IPOL, ipol | pol);
+	priv->gc.write_reg(priv->regs + GRGPIO_IEDGE, iedge | edge);
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 
 	return 0;
 }
@@ -177,11 +170,11 @@ static void grgpio_irq_mask(struct irq_data *d)
 	int offset = d->hwirq;
 	unsigned long flags;
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	grgpio_set_imask(priv, offset, 0);
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 }
 
 static void grgpio_irq_unmask(struct irq_data *d)
@@ -190,11 +183,11 @@ static void grgpio_irq_unmask(struct irq_data *d)
 	int offset = d->hwirq;
 	unsigned long flags;
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	grgpio_set_imask(priv, offset, 1);
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 }
 
 static struct irq_chip grgpio_irq_chip = {
@@ -207,12 +200,12 @@ static struct irq_chip grgpio_irq_chip = {
 static irqreturn_t grgpio_irq_handler(int irq, void *dev)
 {
 	struct grgpio_priv *priv = dev;
-	int ngpio = priv->bgc.gc.ngpio;
+	int ngpio = priv->gc.ngpio;
 	unsigned long flags;
 	int i;
 	int match = 0;
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	/*
 	 * For each gpio line, call its interrupt handler if it its underlying
@@ -228,7 +221,7 @@ static irqreturn_t grgpio_irq_handler(int irq, void *dev)
 		}
 	}
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 
 	if (!match)
 		dev_warn(priv->dev, "No gpio line matched irq %d\n", irq);
@@ -260,7 +253,7 @@ static int grgpio_irq_map(struct irq_domain *d, unsigned int irq,
 	dev_dbg(priv->dev, "Mapping irq %d for gpio line %d\n",
 		irq, offset);
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	/* Request underlying irq if not already requested */
 	lirq->irq = irq;
@@ -273,14 +266,14 @@ static int grgpio_irq_map(struct irq_domain *d, unsigned int irq,
 				"Could not request underlying irq %d\n",
 				uirq->uirq);
 
-			spin_unlock_irqrestore(&priv->bgc.lock, flags);
+			spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 
 			return ret;
 		}
 	}
 	uirq->refcnt++;
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 
 	/* Setup irq  */
 	irq_set_chip_data(irq, priv);
@@ -298,13 +291,13 @@ static void grgpio_irq_unmap(struct irq_domain *d, unsigned int irq)
 	struct grgpio_lirq *lirq;
 	struct grgpio_uirq *uirq;
 	unsigned long flags;
-	int ngpio = priv->bgc.gc.ngpio;
+	int ngpio = priv->gc.ngpio;
 	int i;
 
 	irq_set_chip_and_handler(irq, NULL, NULL);
 	irq_set_chip_data(irq, NULL);
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	/* Free underlying irq if last user unmapped */
 	index = -1;
@@ -326,7 +319,7 @@ static void grgpio_irq_unmap(struct irq_domain *d, unsigned int irq)
 			free_irq(uirq->uirq, priv);
 	}
 
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 }
 
 static const struct irq_domain_ops grgpio_irq_domain_ops = {
@@ -341,7 +334,6 @@ static int grgpio_probe(struct platform_device *ofdev)
 	struct device_node *np = ofdev->dev.of_node;
 	void  __iomem *regs;
 	struct gpio_chip *gc;
-	struct bgpio_chip *bgc;
 	struct grgpio_priv *priv;
 	struct resource *res;
 	int err;
@@ -359,8 +351,8 @@ static int grgpio_probe(struct platform_device *ofdev)
 	if (IS_ERR(regs))
 		return PTR_ERR(regs);
 
-	bgc = &priv->bgc;
-	err = bgpio_init(bgc, &ofdev->dev, 4, regs + GRGPIO_DATA,
+	gc = &priv->gc;
+	err = bgpio_init(gc, &ofdev->dev, 4, regs + GRGPIO_DATA,
 			 regs + GRGPIO_OUTPUT, NULL, regs + GRGPIO_DIR, NULL,
 			 BGPIOF_BIG_ENDIAN_BYTE_ORDER);
 	if (err) {
@@ -369,10 +361,9 @@ static int grgpio_probe(struct platform_device *ofdev)
 	}
 
 	priv->regs = regs;
-	priv->imask = bgc->read_reg(regs + GRGPIO_IMASK);
+	priv->imask = gc->read_reg(regs + GRGPIO_IMASK);
 	priv->dev = &ofdev->dev;
 
-	gc = &bgc->gc;
 	gc->of_node = np;
 	gc->owner = THIS_MODULE;
 	gc->to_irq = grgpio_to_irq;
@@ -435,7 +426,7 @@ static int grgpio_probe(struct platform_device *ofdev)
 
 	platform_set_drvdata(ofdev, priv);
 
-	err = gpiochip_add(gc);
+	err = gpiochip_add_data(gc, priv);
 	if (err) {
 		dev_err(&ofdev->dev, "Could not add gpiochip\n");
 		if (priv->domain)
@@ -456,7 +447,7 @@ static int grgpio_remove(struct platform_device *ofdev)
 	int i;
 	int ret = 0;
 
-	spin_lock_irqsave(&priv->bgc.lock, flags);
+	spin_lock_irqsave(&priv->gc.bgpio_lock, flags);
 
 	if (priv->domain) {
 		for (i = 0; i < GRGPIO_MAX_NGPIO; i++) {
@@ -467,13 +458,13 @@ static int grgpio_remove(struct platform_device *ofdev)
 		}
 	}
 
-	gpiochip_remove(&priv->bgc.gc);
+	gpiochip_remove(&priv->gc);
 
 	if (priv->domain)
 		irq_domain_remove(priv->domain);
 
 out:
-	spin_unlock_irqrestore(&priv->bgc.lock, flags);
+	spin_unlock_irqrestore(&priv->gc.bgpio_lock, flags);
 
 	return ret;
 }
diff --git a/drivers/gpio/gpio-ich.c b/drivers/gpio/gpio-ich.c
index 4ba7ed502131..a4893386abbf 100644
--- a/drivers/gpio/gpio-ich.c
+++ b/drivers/gpio/gpio-ich.c
@@ -282,7 +282,7 @@ static void ichx_gpiolib_setup(struct gpio_chip *chip)
 {
 	chip->owner = THIS_MODULE;
 	chip->label = DRV_NAME;
-	chip->dev = &ichx_priv.dev->dev;
+	chip->parent = &ichx_priv.dev->dev;
 
 	/* Allow chip-specific overrides of request()/get() */
 	chip->request = ichx_priv.desc->request ?
@@ -499,7 +499,7 @@ static int ichx_gpio_probe(struct platform_device *pdev)
 
 init:
 	ichx_gpiolib_setup(&ichx_priv.chip);
-	err = gpiochip_add(&ichx_priv.chip);
+	err = gpiochip_add_data(&ichx_priv.chip, NULL);
 	if (err) {
 		pr_err("Failed to register GPIOs\n");
 		goto add_err;
diff --git a/drivers/gpio/gpio-intel-mid.c b/drivers/gpio/gpio-intel-mid.c
index 70097472b02c..cdaba13cb8e8 100644
--- a/drivers/gpio/gpio-intel-mid.c
+++ b/drivers/gpio/gpio-intel-mid.c
@@ -78,15 +78,10 @@ struct intel_mid_gpio {
 	struct pci_dev			*pdev;
 };
 
-static inline struct intel_mid_gpio *to_intel_gpio_priv(struct gpio_chip *gc)
-{
-	return container_of(gc, struct intel_mid_gpio, chip);
-}
-
 static void __iomem *gpio_reg(struct gpio_chip *chip, unsigned offset,
 			      enum GPIO_REG reg_type)
 {
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(chip);
+	struct intel_mid_gpio *priv = gpiochip_get_data(chip);
 	unsigned nreg = chip->ngpio / 32;
 	u8 reg = offset / 32;
 
@@ -96,7 +91,7 @@ static void __iomem *gpio_reg(struct gpio_chip *chip, unsigned offset,
 static void __iomem *gpio_reg_2bit(struct gpio_chip *chip, unsigned offset,
 				   enum GPIO_REG reg_type)
 {
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(chip);
+	struct intel_mid_gpio *priv = gpiochip_get_data(chip);
 	unsigned nreg = chip->ngpio / 32;
 	u8 reg = offset / 16;
 
@@ -120,7 +115,7 @@ static int intel_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	void __iomem *gplr = gpio_reg(chip, offset, GPLR);
 
-	return readl(gplr) & BIT(offset % 32);
+	return !!(readl(gplr) & BIT(offset % 32));
 }
 
 static void intel_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -138,7 +133,7 @@ static void intel_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int intel_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(chip);
+	struct intel_mid_gpio *priv = gpiochip_get_data(chip);
 	void __iomem *gpdr = gpio_reg(chip, offset, GPDR);
 	u32 value;
 	unsigned long flags;
@@ -161,7 +156,7 @@ static int intel_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int intel_gpio_direction_output(struct gpio_chip *chip,
 			unsigned offset, int value)
 {
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(chip);
+	struct intel_mid_gpio *priv = gpiochip_get_data(chip);
 	void __iomem *gpdr = gpio_reg(chip, offset, GPDR);
 	unsigned long flags;
 
@@ -185,7 +180,7 @@ static int intel_gpio_direction_output(struct gpio_chip *chip,
 static int intel_mid_irq_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(gc);
+	struct intel_mid_gpio *priv = gpiochip_get_data(gc);
 	u32 gpio = irqd_to_hwirq(d);
 	unsigned long flags;
 	u32 value;
@@ -304,7 +299,7 @@ MODULE_DEVICE_TABLE(pci, intel_gpio_ids);
 static void intel_mid_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct intel_mid_gpio *priv = to_intel_gpio_priv(gc);
+	struct intel_mid_gpio *priv = gpiochip_get_data(gc);
 	struct irq_data *data = irq_desc_get_irq_data(desc);
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
 	u32 base, gpio, mask;
@@ -392,7 +387,7 @@ static int intel_gpio_probe(struct pci_dev *pdev,
 
 	priv->reg_base = pcim_iomap_table(pdev)[0];
 	priv->chip.label = dev_name(&pdev->dev);
-	priv->chip.dev = &pdev->dev;
+	priv->chip.parent = &pdev->dev;
 	priv->chip.request = intel_gpio_request;
 	priv->chip.direction_input = intel_gpio_direction_input;
 	priv->chip.direction_output = intel_gpio_direction_output;
@@ -406,7 +401,7 @@ static int intel_gpio_probe(struct pci_dev *pdev,
 	spin_lock_init(&priv->lock);
 
 	pci_set_drvdata(pdev, priv);
-	retval = gpiochip_add(&priv->chip);
+	retval = gpiochip_add_data(&priv->chip, priv);
 	if (retval) {
 		dev_err(&pdev->dev, "gpiochip_add error %d\n", retval);
 		return retval;
diff --git a/drivers/gpio/gpio-iop.c b/drivers/gpio/gpio-iop.c
index 2ed0237a8baf..fb65e5850e0c 100644
--- a/drivers/gpio/gpio-iop.c
+++ b/drivers/gpio/gpio-iop.c
@@ -114,7 +114,7 @@ static int iop3xx_gpio_probe(struct platform_device *pdev)
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
-	return gpiochip_add(&iop3xx_chip);
+	return gpiochip_add_data(&iop3xx_chip, NULL);
 }
 
 static struct platform_driver iop3xx_gpio_driver = {
diff --git a/drivers/gpio/gpio-it87.c b/drivers/gpio/gpio-it87.c
index 21f6f7c0eb34..b219c82414bf 100644
--- a/drivers/gpio/gpio-it87.c
+++ b/drivers/gpio/gpio-it87.c
@@ -77,11 +77,6 @@ static struct it87_gpio it87_gpio_chip = {
 	.lock = __SPIN_LOCK_UNLOCKED(it87_gpio_chip.lock),
 };
 
-static inline struct it87_gpio *to_it87_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct it87_gpio, chip);
-}
-
 /* Superio chip access functions; copied from wdt_it87 */
 
 static inline int superio_enter(void)
@@ -165,7 +160,7 @@ static int it87_gpio_request(struct gpio_chip *chip, unsigned gpio_num)
 {
 	u8 mask, group;
 	int rc = 0;
-	struct it87_gpio *it87_gpio = to_it87_gpio(chip);
+	struct it87_gpio *it87_gpio = gpiochip_get_data(chip);
 
 	mask = 1 << (gpio_num % 8);
 	group = (gpio_num / 8);
@@ -198,7 +193,7 @@ static int it87_gpio_get(struct gpio_chip *chip, unsigned gpio_num)
 {
 	u16 reg;
 	u8 mask;
-	struct it87_gpio *it87_gpio = to_it87_gpio(chip);
+	struct it87_gpio *it87_gpio = gpiochip_get_data(chip);
 
 	mask = 1 << (gpio_num % 8);
 	reg = (gpio_num / 8) + it87_gpio->io_base;
@@ -210,7 +205,7 @@ static int it87_gpio_direction_in(struct gpio_chip *chip, unsigned gpio_num)
 {
 	u8 mask, group;
 	int rc = 0;
-	struct it87_gpio *it87_gpio = to_it87_gpio(chip);
+	struct it87_gpio *it87_gpio = gpiochip_get_data(chip);
 
 	mask = 1 << (gpio_num % 8);
 	group = (gpio_num / 8);
@@ -236,7 +231,7 @@ static void it87_gpio_set(struct gpio_chip *chip,
 {
 	u8 mask, curr_vals;
 	u16 reg;
-	struct it87_gpio *it87_gpio = to_it87_gpio(chip);
+	struct it87_gpio *it87_gpio = gpiochip_get_data(chip);
 
 	mask = 1 << (gpio_num % 8);
 	reg = (gpio_num / 8) + it87_gpio->io_base;
@@ -253,7 +248,7 @@ static int it87_gpio_direction_out(struct gpio_chip *chip,
 {
 	u8 mask, group;
 	int rc = 0;
-	struct it87_gpio *it87_gpio = to_it87_gpio(chip);
+	struct it87_gpio *it87_gpio = gpiochip_get_data(chip);
 
 	mask = 1 << (gpio_num % 8);
 	group = (gpio_num / 8);
@@ -380,7 +375,7 @@ static int __init it87_gpio_init(void)
 
 	it87_gpio->chip.names = (const char *const*)labels_table;
 
-	rc = gpiochip_add(&it87_gpio->chip);
+	rc = gpiochip_add_data(&it87_gpio->chip, it87_gpio);
 	if (rc)
 		goto labels_free;
 
diff --git a/drivers/gpio/gpio-janz-ttl.c b/drivers/gpio/gpio-janz-ttl.c
index 3a1664335f5e..482aa0353868 100644
--- a/drivers/gpio/gpio-janz-ttl.c
+++ b/drivers/gpio/gpio-janz-ttl.c
@@ -59,7 +59,7 @@ struct ttl_module {
 
 static int ttl_get_value(struct gpio_chip *gpio, unsigned offset)
 {
-	struct ttl_module *mod = dev_get_drvdata(gpio->dev);
+	struct ttl_module *mod = dev_get_drvdata(gpio->parent);
 	u8 *shadow;
 	int ret;
 
@@ -76,12 +76,12 @@ static int ttl_get_value(struct gpio_chip *gpio, unsigned offset)
 	spin_lock(&mod->lock);
 	ret = *shadow & (1 << offset);
 	spin_unlock(&mod->lock);
-	return ret;
+	return !!ret;
 }
 
 static void ttl_set_value(struct gpio_chip *gpio, unsigned offset, int value)
 {
-	struct ttl_module *mod = dev_get_drvdata(gpio->dev);
+	struct ttl_module *mod = dev_get_drvdata(gpio->parent);
 	void __iomem *port;
 	u8 *shadow;
 
@@ -172,7 +172,7 @@ static int ttl_probe(struct platform_device *pdev)
 
 	/* Initialize the GPIO data structures */
 	gpio = &mod->gpio;
-	gpio->dev = &pdev->dev;
+	gpio->parent = &pdev->dev;
 	gpio->label = pdev->name;
 	gpio->get = ttl_get_value;
 	gpio->set = ttl_set_value;
@@ -182,7 +182,7 @@ static int ttl_probe(struct platform_device *pdev)
 	gpio->base = -1;
 	gpio->ngpio = 20;
 
-	ret = gpiochip_add(gpio);
+	ret = gpiochip_add_data(gpio, NULL);
 	if (ret) {
 		dev_err(dev, "unable to add GPIO chip\n");
 		return ret;
diff --git a/drivers/gpio/gpio-kempld.c b/drivers/gpio/gpio-kempld.c
index 83f281dda1e0..01117747b965 100644
--- a/drivers/gpio/gpio-kempld.c
+++ b/drivers/gpio/gpio-kempld.c
@@ -65,17 +65,15 @@ static int kempld_gpio_get_bit(struct kempld_device_data *pld, u8 reg, u8 bit)
 
 static int kempld_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct kempld_gpio_data *gpio
-		= container_of(chip, struct kempld_gpio_data, chip);
+	struct kempld_gpio_data *gpio = gpiochip_get_data(chip);
 	struct kempld_device_data *pld = gpio->pld;
 
-	return kempld_gpio_get_bit(pld, KEMPLD_GPIO_LVL_NUM(offset), offset);
+	return !!kempld_gpio_get_bit(pld, KEMPLD_GPIO_LVL_NUM(offset), offset);
 }
 
 static void kempld_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct kempld_gpio_data *gpio
-		= container_of(chip, struct kempld_gpio_data, chip);
+	struct kempld_gpio_data *gpio = gpiochip_get_data(chip);
 	struct kempld_device_data *pld = gpio->pld;
 
 	kempld_get_mutex(pld);
@@ -85,8 +83,7 @@ static void kempld_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int kempld_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct kempld_gpio_data *gpio
-		= container_of(chip, struct kempld_gpio_data, chip);
+	struct kempld_gpio_data *gpio = gpiochip_get_data(chip);
 	struct kempld_device_data *pld = gpio->pld;
 
 	kempld_get_mutex(pld);
@@ -99,8 +96,7 @@ static int kempld_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int kempld_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 					int value)
 {
-	struct kempld_gpio_data *gpio
-		= container_of(chip, struct kempld_gpio_data, chip);
+	struct kempld_gpio_data *gpio = gpiochip_get_data(chip);
 	struct kempld_device_data *pld = gpio->pld;
 
 	kempld_get_mutex(pld);
@@ -113,8 +109,7 @@ static int kempld_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static int kempld_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct kempld_gpio_data *gpio
-		= container_of(chip, struct kempld_gpio_data, chip);
+	struct kempld_gpio_data *gpio = gpiochip_get_data(chip);
 	struct kempld_device_data *pld = gpio->pld;
 
 	return !kempld_gpio_get_bit(pld, KEMPLD_GPIO_DIR_NUM(offset), offset);
@@ -166,7 +161,7 @@ static int kempld_gpio_probe(struct platform_device *pdev)
 	chip = &gpio->chip;
 	chip->label = "gpio-kempld";
 	chip->owner = THIS_MODULE;
-	chip->dev = dev;
+	chip->parent = dev;
 	chip->can_sleep = true;
 	if (pdata && pdata->gpio_base)
 		chip->base = pdata->gpio_base;
@@ -183,7 +178,7 @@ static int kempld_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, gpio);
 	if (ret) {
 		dev_err(dev, "Could not register GPIO chip\n");
 		return ret;
diff --git a/drivers/gpio/gpio-ks8695.c b/drivers/gpio/gpio-ks8695.c
index cc09b237e88c..9f86ed9c753b 100644
--- a/drivers/gpio/gpio-ks8695.c
+++ b/drivers/gpio/gpio-ks8695.c
@@ -234,7 +234,7 @@ static struct gpio_chip ks8695_gpio_chip = {
 /* Register the GPIOs */
 void ks8695_register_gpios(void)
 {
-	if (gpiochip_add(&ks8695_gpio_chip))
+	if (gpiochip_add_data(&ks8695_gpio_chip, NULL))
 		printk(KERN_ERR "Unable to register core GPIOs\n");
 }
 
diff --git a/drivers/gpio/gpio-loongson.c b/drivers/gpio/gpio-loongson.c
index ccc65a1aea88..92c4fe7b2677 100644
--- a/drivers/gpio/gpio-loongson.c
+++ b/drivers/gpio/gpio-loongson.c
@@ -110,6 +110,6 @@ static struct gpio_chip loongson_chip = {
 
 static int __init loongson_gpio_setup(void)
 {
-	return gpiochip_add(&loongson_chip);
+	return gpiochip_add_data(&loongson_chip, NULL);
 }
 postcore_initcall(loongson_gpio_setup);
diff --git a/drivers/gpio/gpio-lp3943.c b/drivers/gpio/gpio-lp3943.c
index cfc5b12b43ad..1c8e2ae26938 100644
--- a/drivers/gpio/gpio-lp3943.c
+++ b/drivers/gpio/gpio-lp3943.c
@@ -45,14 +45,9 @@ struct lp3943_gpio {
 	u16 input_mask;		/* 1 = GPIO is input direction, 0 = output */
 };
 
-static inline struct lp3943_gpio *to_lp3943_gpio(struct gpio_chip *_chip)
-{
-	return container_of(_chip, struct lp3943_gpio, chip);
-}
-
 static int lp3943_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 	struct lp3943 *lp3943 = lp3943_gpio->lp3943;
 
 	/* Return an error if the pin is already assigned */
@@ -64,7 +59,7 @@ static int lp3943_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void lp3943_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 	struct lp3943 *lp3943 = lp3943_gpio->lp3943;
 
 	clear_bit(offset, &lp3943->pin_used);
@@ -82,7 +77,7 @@ static int lp3943_gpio_set_mode(struct lp3943_gpio *lp3943_gpio, u8 offset,
 
 static int lp3943_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 
 	lp3943_gpio->input_mask |= BIT(offset);
 
@@ -138,7 +133,7 @@ static int lp3943_get_gpio_out_status(struct lp3943_gpio *lp3943_gpio,
 
 static int lp3943_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 
 	/*
 	 * Limitation:
@@ -157,7 +152,7 @@ static int lp3943_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void lp3943_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 	u8 data;
 
 	if (value)
@@ -171,7 +166,7 @@ static void lp3943_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int lp3943_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 					int value)
 {
-	struct lp3943_gpio *lp3943_gpio = to_lp3943_gpio(chip);
+	struct lp3943_gpio *lp3943_gpio = gpiochip_get_data(chip);
 
 	lp3943_gpio_set(chip, offset, value);
 	lp3943_gpio->input_mask &= ~BIT(offset);
@@ -205,11 +200,11 @@ static int lp3943_gpio_probe(struct platform_device *pdev)
 
 	lp3943_gpio->lp3943 = lp3943;
 	lp3943_gpio->chip = lp3943_gpio_chip;
-	lp3943_gpio->chip.dev = &pdev->dev;
+	lp3943_gpio->chip.parent = &pdev->dev;
 
 	platform_set_drvdata(pdev, lp3943_gpio);
 
-	return gpiochip_add(&lp3943_gpio->chip);
+	return gpiochip_add_data(&lp3943_gpio->chip, lp3943_gpio);
 }
 
 static int lp3943_gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-lpc18xx.c b/drivers/gpio/gpio-lpc18xx.c
index e39dcb0af8ae..98832c9f614a 100644
--- a/drivers/gpio/gpio-lpc18xx.c
+++ b/drivers/gpio/gpio-lpc18xx.c
@@ -31,27 +31,22 @@ struct lpc18xx_gpio_chip {
 	spinlock_t lock;
 };
 
-static inline struct lpc18xx_gpio_chip *to_lpc18xx_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct lpc18xx_gpio_chip, gpio);
-}
-
 static void lpc18xx_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	struct lpc18xx_gpio_chip *gc = gpiochip_get_data(chip);
 	writeb(value ? 1 : 0, gc->base + offset);
 }
 
 static int lpc18xx_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	struct lpc18xx_gpio_chip *gc = gpiochip_get_data(chip);
 	return !!readb(gc->base + offset);
 }
 
 static int lpc18xx_gpio_direction(struct gpio_chip *chip, unsigned offset,
 				  bool out)
 {
-	struct lpc18xx_gpio_chip *gc = to_lpc18xx_gpio(chip);
+	struct lpc18xx_gpio_chip *gc = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 port, pin, dir;
 
@@ -127,9 +122,9 @@ static int lpc18xx_gpio_probe(struct platform_device *pdev)
 
 	spin_lock_init(&gc->lock);
 
-	gc->gpio.dev = &pdev->dev;
+	gc->gpio.parent = &pdev->dev;
 
-	ret = gpiochip_add(&gc->gpio);
+	ret = gpiochip_add_data(&gc->gpio, gc);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to add gpio chip\n");
 		clk_disable_unprepare(gc->clk);
diff --git a/drivers/gpio/gpio-lpc32xx.c b/drivers/gpio/gpio-lpc32xx.c
index 47e2dde63734..4cecf4ce96c1 100644
--- a/drivers/gpio/gpio-lpc32xx.c
+++ b/drivers/gpio/gpio-lpc32xx.c
@@ -165,12 +165,6 @@ struct lpc32xx_gpio_chip {
 	struct gpio_regs	*gpio_grp;
 };
 
-static inline struct lpc32xx_gpio_chip *to_lpc32xx_gpio(
-	struct gpio_chip *gpc)
-{
-	return container_of(gpc, struct lpc32xx_gpio_chip, chip);
-}
-
 static void __set_gpio_dir_p012(struct lpc32xx_gpio_chip *group,
 	unsigned pin, int input)
 {
@@ -261,7 +255,7 @@ static int __get_gpo_state_p3(struct lpc32xx_gpio_chip *group,
 static int lpc32xx_gpio_dir_input_p012(struct gpio_chip *chip,
 	unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_dir_p012(group, pin, 1);
 
@@ -271,7 +265,7 @@ static int lpc32xx_gpio_dir_input_p012(struct gpio_chip *chip,
 static int lpc32xx_gpio_dir_input_p3(struct gpio_chip *chip,
 	unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_dir_p3(group, pin, 1);
 
@@ -286,29 +280,29 @@ static int lpc32xx_gpio_dir_in_always(struct gpio_chip *chip,
 
 static int lpc32xx_gpio_get_value_p012(struct gpio_chip *chip, unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
-	return __get_gpio_state_p012(group, pin);
+	return !!__get_gpio_state_p012(group, pin);
 }
 
 static int lpc32xx_gpio_get_value_p3(struct gpio_chip *chip, unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
-	return __get_gpio_state_p3(group, pin);
+	return !!__get_gpio_state_p3(group, pin);
 }
 
 static int lpc32xx_gpi_get_value(struct gpio_chip *chip, unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
-	return __get_gpi_state_p3(group, pin);
+	return !!__get_gpi_state_p3(group, pin);
 }
 
 static int lpc32xx_gpio_dir_output_p012(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_level_p012(group, pin, value);
 	__set_gpio_dir_p012(group, pin, 0);
@@ -319,7 +313,7 @@ static int lpc32xx_gpio_dir_output_p012(struct gpio_chip *chip, unsigned pin,
 static int lpc32xx_gpio_dir_output_p3(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_level_p3(group, pin, value);
 	__set_gpio_dir_p3(group, pin, 0);
@@ -330,7 +324,7 @@ static int lpc32xx_gpio_dir_output_p3(struct gpio_chip *chip, unsigned pin,
 static int lpc32xx_gpio_dir_out_always(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpo_level_p3(group, pin, value);
 	return 0;
@@ -339,7 +333,7 @@ static int lpc32xx_gpio_dir_out_always(struct gpio_chip *chip, unsigned pin,
 static void lpc32xx_gpio_set_value_p012(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_level_p012(group, pin, value);
 }
@@ -347,7 +341,7 @@ static void lpc32xx_gpio_set_value_p012(struct gpio_chip *chip, unsigned pin,
 static void lpc32xx_gpio_set_value_p3(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpio_level_p3(group, pin, value);
 }
@@ -355,16 +349,16 @@ static void lpc32xx_gpio_set_value_p3(struct gpio_chip *chip, unsigned pin,
 static void lpc32xx_gpo_set_value(struct gpio_chip *chip, unsigned pin,
 	int value)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
 	__set_gpo_level_p3(group, pin, value);
 }
 
 static int lpc32xx_gpo_get_value(struct gpio_chip *chip, unsigned pin)
 {
-	struct lpc32xx_gpio_chip *group = to_lpc32xx_gpio(chip);
+	struct lpc32xx_gpio_chip *group = gpiochip_get_data(chip);
 
-	return __get_gpo_state_p3(group, pin);
+	return !!__get_gpo_state_p3(group, pin);
 }
 
 static int lpc32xx_gpio_request(struct gpio_chip *chip, unsigned pin)
@@ -553,7 +547,8 @@ static int lpc32xx_gpio_probe(struct platform_device *pdev)
 			lpc32xx_gpiochip[i].chip.of_gpio_n_cells = 3;
 			lpc32xx_gpiochip[i].chip.of_node = pdev->dev.of_node;
 		}
-		gpiochip_add(&lpc32xx_gpiochip[i].chip);
+		gpiochip_add_data(&lpc32xx_gpiochip[i].chip,
+				  &lpc32xx_gpiochip[i]);
 	}
 
 	return 0;
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index 127c37b380ae..13107772be4f 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -97,7 +97,7 @@ struct lp_gpio {
 static unsigned long lp_gpio_reg(struct gpio_chip *chip, unsigned offset,
 				 int reg)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	int reg_offset;
 
 	if (reg == LP_CONFIG1 || reg == LP_CONFIG2)
@@ -112,7 +112,7 @@ static unsigned long lp_gpio_reg(struct gpio_chip *chip, unsigned offset,
 
 static int lp_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	unsigned long reg = lp_gpio_reg(chip, offset, LP_CONFIG1);
 	unsigned long conf2 = lp_gpio_reg(chip, offset, LP_CONFIG2);
 	unsigned long acpi_use = lp_gpio_reg(chip, offset, LP_ACPI_OWNED);
@@ -137,7 +137,7 @@ static int lp_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void lp_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	unsigned long conf2 = lp_gpio_reg(chip, offset, LP_CONFIG2);
 
 	/* disable input sensing */
@@ -149,7 +149,7 @@ static void lp_gpio_free(struct gpio_chip *chip, unsigned offset)
 static int lp_irq_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct lp_gpio *lg = container_of(gc, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(gc);
 	u32 hwirq = irqd_to_hwirq(d);
 	unsigned long flags;
 	u32 value;
@@ -191,7 +191,7 @@ static int lp_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void lp_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	unsigned long reg = lp_gpio_reg(chip, offset, LP_CONFIG1);
 	unsigned long flags;
 
@@ -207,7 +207,7 @@ static void lp_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int lp_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	unsigned long reg = lp_gpio_reg(chip, offset, LP_CONFIG1);
 	unsigned long flags;
 
@@ -221,7 +221,7 @@ static int lp_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int lp_gpio_direction_output(struct gpio_chip *chip,
 				      unsigned offset, int value)
 {
-	struct lp_gpio *lg = container_of(chip, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(chip);
 	unsigned long reg = lp_gpio_reg(chip, offset, LP_CONFIG1);
 	unsigned long flags;
 
@@ -238,7 +238,7 @@ static void lp_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct irq_data *data = irq_desc_get_irq_data(desc);
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct lp_gpio *lg = container_of(gc, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(gc);
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
 	u32 base, pin, mask;
 	unsigned long reg, ena, pending;
@@ -273,7 +273,7 @@ static void lp_irq_mask(struct irq_data *d)
 static void lp_irq_enable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct lp_gpio *lg = container_of(gc, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(gc);
 	u32 hwirq = irqd_to_hwirq(d);
 	unsigned long reg = lp_gpio_reg(&lg->chip, hwirq, LP_INT_ENABLE);
 	unsigned long flags;
@@ -286,7 +286,7 @@ static void lp_irq_enable(struct irq_data *d)
 static void lp_irq_disable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct lp_gpio *lg = container_of(gc, struct lp_gpio, chip);
+	struct lp_gpio *lg = gpiochip_get_data(gc);
 	u32 hwirq = irqd_to_hwirq(d);
 	unsigned long reg = lp_gpio_reg(&lg->chip, hwirq, LP_INT_ENABLE);
 	unsigned long flags;
@@ -368,9 +368,9 @@ static int lp_gpio_probe(struct platform_device *pdev)
 	gc->base = -1;
 	gc->ngpio = LP_NUM_GPIO;
 	gc->can_sleep = false;
-	gc->dev = dev;
+	gc->parent = dev;
 
-	ret = gpiochip_add(gc);
+	ret = gpiochip_add_data(gc, lg);
 	if (ret) {
 		dev_err(dev, "failed adding lp-gpio chip\n");
 		return ret;
diff --git a/drivers/gpio/gpio-max7300.c b/drivers/gpio/gpio-max7300.c
index 0cc2c279ab5c..1ae9ba851c9a 100644
--- a/drivers/gpio/gpio-max7300.c
+++ b/drivers/gpio/gpio-max7300.c
@@ -65,7 +65,6 @@ MODULE_DEVICE_TABLE(i2c, max7300_id);
 static struct i2c_driver max7300_driver = {
 	.driver = {
 		.name = "max7300",
-		.owner = THIS_MODULE,
 	},
 	.probe = max7300_probe,
 	.remove = max7300_remove,
diff --git a/drivers/gpio/gpio-max730x.c b/drivers/gpio/gpio-max730x.c
index 0f57d2d248ec..08807368f007 100644
--- a/drivers/gpio/gpio-max730x.c
+++ b/drivers/gpio/gpio-max730x.c
@@ -50,7 +50,7 @@
 
 static int max7301_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct max7301 *ts = container_of(chip, struct max7301, chip);
+	struct max7301 *ts = gpiochip_get_data(chip);
 	u8 *config;
 	u8 offset_bits, pin_config;
 	int ret;
@@ -92,7 +92,7 @@ static int __max7301_set(struct max7301 *ts, unsigned offset, int value)
 static int max7301_direction_output(struct gpio_chip *chip, unsigned offset,
 				    int value)
 {
-	struct max7301 *ts = container_of(chip, struct max7301, chip);
+	struct max7301 *ts = gpiochip_get_data(chip);
 	u8 *config;
 	u8 offset_bits;
 	int ret;
@@ -120,7 +120,7 @@ static int max7301_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static int max7301_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct max7301 *ts = container_of(chip, struct max7301, chip);
+	struct max7301 *ts = gpiochip_get_data(chip);
 	int config, level = -EINVAL;
 
 	/* First 4 pins are unused in the controller */
@@ -148,7 +148,7 @@ static int max7301_get(struct gpio_chip *chip, unsigned offset)
 
 static void max7301_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct max7301 *ts = container_of(chip, struct max7301, chip);
+	struct max7301 *ts = gpiochip_get_data(chip);
 
 	/* First 4 pins are unused in the controller */
 	offset += 4;
@@ -189,7 +189,7 @@ int __max730x_probe(struct max7301 *ts)
 
 	ts->chip.ngpio = PIN_NUMBER;
 	ts->chip.can_sleep = true;
-	ts->chip.dev = dev;
+	ts->chip.parent = dev;
 	ts->chip.owner = THIS_MODULE;
 
 	/*
@@ -213,7 +213,7 @@ int __max730x_probe(struct max7301 *ts)
 		}
 	}
 
-	ret = gpiochip_add(&ts->chip);
+	ret = gpiochip_add_data(&ts->chip, ts);
 	if (ret)
 		goto exit_destroy;
 
diff --git a/drivers/gpio/gpio-max732x.c b/drivers/gpio/gpio-max732x.c
index 8c5252c6c327..a9aaf9d822b4 100644
--- a/drivers/gpio/gpio-max732x.c
+++ b/drivers/gpio/gpio-max732x.c
@@ -158,11 +158,6 @@ struct max732x_chip {
 #endif
 };
 
-static inline struct max732x_chip *to_max732x(struct gpio_chip *gc)
-{
-	return container_of(gc, struct max732x_chip, gpio_chip);
-}
-
 static int max732x_writeb(struct max732x_chip *chip, int group_a, uint8_t val)
 {
 	struct i2c_client *client;
@@ -201,21 +196,21 @@ static inline int is_group_a(struct max732x_chip *chip, unsigned off)
 
 static int max732x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	uint8_t reg_val;
 	int ret;
 
 	ret = max732x_readb(chip, is_group_a(chip, off), &reg_val);
 	if (ret < 0)
-		return 0;
+		return ret;
 
-	return reg_val & (1u << (off & 0x7));
+	return !!(reg_val & (1u << (off & 0x7)));
 }
 
 static void max732x_gpio_set_mask(struct gpio_chip *gc, unsigned off, int mask,
 				  int val)
 {
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	uint8_t reg_out;
 	int ret;
 
@@ -259,7 +254,7 @@ static void max732x_gpio_set_multiple(struct gpio_chip *gc,
 
 static int max732x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	unsigned int mask = 1u << off;
 
 	if ((mask & chip->dir_input) == 0) {
@@ -281,7 +276,7 @@ static int max732x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 static int max732x_gpio_direction_output(struct gpio_chip *gc,
 		unsigned off, int val)
 {
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	unsigned int mask = 1u << off;
 
 	if ((mask & chip->dir_output) == 0) {
@@ -356,7 +351,7 @@ static void max732x_irq_update_mask(struct max732x_chip *chip)
 static void max732x_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 
 	chip->irq_mask_cur &= ~(1 << d->hwirq);
 }
@@ -364,7 +359,7 @@ static void max732x_irq_mask(struct irq_data *d)
 static void max732x_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 
 	chip->irq_mask_cur |= 1 << d->hwirq;
 }
@@ -372,7 +367,7 @@ static void max732x_irq_unmask(struct irq_data *d)
 static void max732x_irq_bus_lock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 
 	mutex_lock(&chip->irq_lock);
 	chip->irq_mask_cur = chip->irq_mask;
@@ -381,7 +376,7 @@ static void max732x_irq_bus_lock(struct irq_data *d)
 static void max732x_irq_bus_sync_unlock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	uint16_t new_irqs;
 	uint16_t level;
 
@@ -400,7 +395,7 @@ static void max732x_irq_bus_sync_unlock(struct irq_data *d)
 static int max732x_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct max732x_chip *chip = to_max732x(gc);
+	struct max732x_chip *chip = gpiochip_get_data(gc);
 	uint16_t off = d->hwirq;
 	uint16_t mask = 1 << off;
 
@@ -603,7 +598,7 @@ static int max732x_setup_gpio(struct max732x_chip *chip,
 	gc->base = gpio_start;
 	gc->ngpio = port;
 	gc->label = chip->client->name;
-	gc->dev = &chip->client->dev;
+	gc->parent = &chip->client->dev;
 	gc->owner = THIS_MODULE;
 
 	return port;
@@ -649,7 +644,7 @@ static int max732x_probe(struct i2c_client *client,
 	chip->client = client;
 
 	nr_port = max732x_setup_gpio(chip, id, pdata->gpio_base);
-	chip->gpio_chip.dev = &client->dev;
+	chip->gpio_chip.parent = &client->dev;
 
 	addr_a = (client->addr & 0x0f) | 0x60;
 	addr_b = (client->addr & 0x0f) | 0x50;
@@ -694,7 +689,7 @@ static int max732x_probe(struct i2c_client *client,
 			goto out_failed;
 	}
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = gpiochip_add_data(&chip->gpio_chip, chip);
 	if (ret)
 		goto out_failed;
 
@@ -749,7 +744,6 @@ static int max732x_remove(struct i2c_client *client)
 static struct i2c_driver max732x_driver = {
 	.driver = {
 		.name		= "max732x",
-		.owner		= THIS_MODULE,
 		.of_match_table	= of_match_ptr(max732x_of_table),
 	},
 	.probe		= max732x_probe,
diff --git a/drivers/gpio/gpio-mb86s7x.c b/drivers/gpio/gpio-mb86s7x.c
index ee93c0ab0a59..7fffc1d6c055 100644
--- a/drivers/gpio/gpio-mb86s7x.c
+++ b/drivers/gpio/gpio-mb86s7x.c
@@ -44,14 +44,9 @@ struct mb86s70_gpio_chip {
 	spinlock_t lock;
 };
 
-static inline struct mb86s70_gpio_chip *chip_to_mb86s70(struct gpio_chip *gc)
-{
-	return container_of(gc, struct mb86s70_gpio_chip, gc);
-}
-
 static int mb86s70_gpio_request(struct gpio_chip *gc, unsigned gpio)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 val;
 
@@ -73,7 +68,7 @@ static int mb86s70_gpio_request(struct gpio_chip *gc, unsigned gpio)
 
 static void mb86s70_gpio_free(struct gpio_chip *gc, unsigned gpio)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 	unsigned long flags;
 	u32 val;
 
@@ -88,7 +83,7 @@ static void mb86s70_gpio_free(struct gpio_chip *gc, unsigned gpio)
 
 static int mb86s70_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 	unsigned long flags;
 	unsigned char val;
 
@@ -106,7 +101,7 @@ static int mb86s70_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
 static int mb86s70_gpio_direction_output(struct gpio_chip *gc,
 					 unsigned gpio, int value)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 	unsigned long flags;
 	unsigned char val;
 
@@ -130,14 +125,14 @@ static int mb86s70_gpio_direction_output(struct gpio_chip *gc,
 
 static int mb86s70_gpio_get(struct gpio_chip *gc, unsigned gpio)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 
 	return !!(readl(gchip->base + PDR(gpio)) & OFFSET(gpio));
 }
 
 static void mb86s70_gpio_set(struct gpio_chip *gc, unsigned gpio, int value)
 {
-	struct mb86s70_gpio_chip *gchip = chip_to_mb86s70(gc);
+	struct mb86s70_gpio_chip *gchip = gpiochip_get_data(gc);
 	unsigned long flags;
 	unsigned char val;
 
@@ -187,12 +182,12 @@ static int mb86s70_gpio_probe(struct platform_device *pdev)
 	gchip->gc.label = dev_name(&pdev->dev);
 	gchip->gc.ngpio = 32;
 	gchip->gc.owner = THIS_MODULE;
-	gchip->gc.dev = &pdev->dev;
+	gchip->gc.parent = &pdev->dev;
 	gchip->gc.base = -1;
 
 	platform_set_drvdata(pdev, gchip);
 
-	ret = gpiochip_add(&gchip->gc);
+	ret = gpiochip_add_data(&gchip->gc, gchip);
 	if (ret) {
 		dev_err(&pdev->dev, "couldn't register gpio driver\n");
 		clk_disable_unprepare(gchip->clk);
diff --git a/drivers/gpio/gpio-mc33880.c b/drivers/gpio/gpio-mc33880.c
index 2853731db5bc..0f0df7956264 100644
--- a/drivers/gpio/gpio-mc33880.c
+++ b/drivers/gpio/gpio-mc33880.c
@@ -71,7 +71,7 @@ static int __mc33880_set(struct mc33880 *mc, unsigned offset, int value)
 
 static void mc33880_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct mc33880 *mc = container_of(chip, struct mc33880, chip);
+	struct mc33880 *mc = gpiochip_get_data(chip);
 
 	mutex_lock(&mc->lock);
 
@@ -116,7 +116,7 @@ static int mc33880_probe(struct spi_device *spi)
 	mc->chip.base = pdata->base;
 	mc->chip.ngpio = PIN_NUMBER;
 	mc->chip.can_sleep = true;
-	mc->chip.dev = &spi->dev;
+	mc->chip.parent = &spi->dev;
 	mc->chip.owner = THIS_MODULE;
 
 	mc->port_config = 0x00;
@@ -135,7 +135,7 @@ static int mc33880_probe(struct spi_device *spi)
 		goto exit_destroy;
 	}
 
-	ret = gpiochip_add(&mc->chip);
+	ret = gpiochip_add_data(&mc->chip, mc);
 	if (ret)
 		goto exit_destroy;
 
diff --git a/drivers/gpio/gpio-mc9s08dz60.c b/drivers/gpio/gpio-mc9s08dz60.c
index d62b4f8182bf..ba22fb92a6e7 100644
--- a/drivers/gpio/gpio-mc9s08dz60.c
+++ b/drivers/gpio/gpio-mc9s08dz60.c
@@ -29,12 +29,6 @@ struct mc9s08dz60 {
 	struct gpio_chip chip;
 };
 
-static inline struct mc9s08dz60 *to_mc9s08dz60(struct gpio_chip *gc)
-{
-	return container_of(gc, struct mc9s08dz60, chip);
-}
-
-
 static void mc9s_gpio_to_reg_and_bit(int offset, u8 *reg, u8 *bit)
 {
 	*reg = 0x20 + offset / GPIO_NUM_PER_GROUP;
@@ -45,7 +39,7 @@ static int mc9s08dz60_get_value(struct gpio_chip *gc, unsigned offset)
 {
 	u8 reg, bit;
 	s32 value;
-	struct mc9s08dz60 *mc9s = to_mc9s08dz60(gc);
+	struct mc9s08dz60 *mc9s = gpiochip_get_data(gc);
 
 	mc9s_gpio_to_reg_and_bit(offset, &reg, &bit);
 	value = i2c_smbus_read_byte_data(mc9s->client, reg);
@@ -75,7 +69,7 @@ static int mc9s08dz60_set(struct mc9s08dz60 *mc9s, unsigned offset, int val)
 
 static void mc9s08dz60_set_value(struct gpio_chip *gc, unsigned offset, int val)
 {
-	struct mc9s08dz60 *mc9s = to_mc9s08dz60(gc);
+	struct mc9s08dz60 *mc9s = gpiochip_get_data(gc);
 
 	mc9s08dz60_set(mc9s, offset, val);
 }
@@ -83,7 +77,7 @@ static void mc9s08dz60_set_value(struct gpio_chip *gc, unsigned offset, int val)
 static int mc9s08dz60_direction_output(struct gpio_chip *gc,
 				       unsigned offset, int val)
 {
-	struct mc9s08dz60 *mc9s = to_mc9s08dz60(gc);
+	struct mc9s08dz60 *mc9s = gpiochip_get_data(gc);
 
 	return mc9s08dz60_set(mc9s, offset, val);
 }
@@ -99,7 +93,7 @@ static int mc9s08dz60_probe(struct i2c_client *client,
 
 	mc9s->chip.label = client->name;
 	mc9s->chip.base = -1;
-	mc9s->chip.dev = &client->dev;
+	mc9s->chip.parent = &client->dev;
 	mc9s->chip.owner = THIS_MODULE;
 	mc9s->chip.ngpio = GPIO_NUM;
 	mc9s->chip.can_sleep = true;
@@ -109,7 +103,7 @@ static int mc9s08dz60_probe(struct i2c_client *client,
 	mc9s->client = client;
 	i2c_set_clientdata(client, mc9s);
 
-	return gpiochip_add(&mc9s->chip);
+	return gpiochip_add_data(&mc9s->chip, mc9s);
 }
 
 static int mc9s08dz60_remove(struct i2c_client *client)
@@ -131,7 +125,6 @@ MODULE_DEVICE_TABLE(i2c, mc9s08dz60_id);
 
 static struct i2c_driver mc9s08dz60_i2c_driver = {
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "mc9s08dz60",
 	},
 	.probe = mc9s08dz60_probe,
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 4a41694919da..c767879e4dd9 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -269,7 +269,7 @@ static const struct mcp23s08_ops mcp23s17_ops = {
 
 static int mcp23s08_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct mcp23s08	*mcp = container_of(chip, struct mcp23s08, chip);
+	struct mcp23s08	*mcp = gpiochip_get_data(chip);
 	int status;
 
 	mutex_lock(&mcp->lock);
@@ -281,7 +281,7 @@ static int mcp23s08_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int mcp23s08_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct mcp23s08	*mcp = container_of(chip, struct mcp23s08, chip);
+	struct mcp23s08	*mcp = gpiochip_get_data(chip);
 	int status;
 
 	mutex_lock(&mcp->lock);
@@ -312,7 +312,7 @@ static int __mcp23s08_set(struct mcp23s08 *mcp, unsigned mask, int value)
 
 static void mcp23s08_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct mcp23s08	*mcp = container_of(chip, struct mcp23s08, chip);
+	struct mcp23s08	*mcp = gpiochip_get_data(chip);
 	unsigned mask = 1 << offset;
 
 	mutex_lock(&mcp->lock);
@@ -323,7 +323,7 @@ static void mcp23s08_set(struct gpio_chip *chip, unsigned offset, int value)
 static int
 mcp23s08_direction_output(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct mcp23s08	*mcp = container_of(chip, struct mcp23s08, chip);
+	struct mcp23s08	*mcp = gpiochip_get_data(chip);
 	unsigned mask = 1 << offset;
 	int status;
 
@@ -377,7 +377,7 @@ static irqreturn_t mcp23s08_irq(int irq, void *data)
 
 static int mcp23s08_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct mcp23s08 *mcp = container_of(chip, struct mcp23s08, chip);
+	struct mcp23s08 *mcp = gpiochip_get_data(chip);
 
 	return irq_find_mapping(mcp->irq_domain, offset);
 }
@@ -446,7 +446,7 @@ static int mcp23s08_irq_reqres(struct irq_data *data)
 	struct mcp23s08 *mcp = irq_data_get_irq_chip_data(data);
 
 	if (gpiochip_lock_as_irq(&mcp->chip, data->hwirq)) {
-		dev_err(mcp->chip.dev,
+		dev_err(mcp->chip.parent,
 			"unable to lock HW IRQ %lu for IRQ usage\n",
 			data->hwirq);
 		return -EINVAL;
@@ -481,7 +481,8 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 
 	mutex_init(&mcp->irq_lock);
 
-	mcp->irq_domain = irq_domain_add_linear(chip->dev->of_node, chip->ngpio,
+	mcp->irq_domain = irq_domain_add_linear(chip->parent->of_node,
+						chip->ngpio,
 						&irq_domain_simple_ops, mcp);
 	if (!mcp->irq_domain)
 		return -ENODEV;
@@ -491,10 +492,11 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 	else
 		irqflags |= IRQF_TRIGGER_LOW;
 
-	err = devm_request_threaded_irq(chip->dev, mcp->irq, NULL, mcp23s08_irq,
-					irqflags, dev_name(chip->dev), mcp);
+	err = devm_request_threaded_irq(chip->parent, mcp->irq, NULL,
+					mcp23s08_irq,
+					irqflags, dev_name(chip->parent), mcp);
 	if (err != 0) {
-		dev_err(chip->dev, "unable to request IRQ#%d: %d\n",
+		dev_err(chip->parent, "unable to request IRQ#%d: %d\n",
 			mcp->irq, err);
 		return err;
 	}
@@ -542,7 +544,7 @@ static void mcp23s08_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 	int		t;
 	unsigned	mask;
 
-	mcp = container_of(chip, struct mcp23s08, chip);
+	mcp = gpiochip_get_data(chip);
 
 	/* NOTE: we only handle one bank for now ... */
 	bank = '0' + ((mcp->addr >> 1) & 0x7);
@@ -638,7 +640,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 
 	mcp->chip.base = pdata->base;
 	mcp->chip.can_sleep = true;
-	mcp->chip.dev = dev;
+	mcp->chip.parent = dev;
 	mcp->chip.owner = THIS_MODULE;
 
 	/* verify MCP_IOCON.SEQOP = 0, so sequential reads work,
@@ -652,7 +654,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	mcp->irq_controller = pdata->irq_controller;
 	if (mcp->irq && mcp->irq_controller) {
 		mcp->irq_active_high =
-			of_property_read_bool(mcp->chip.dev->of_node,
+			of_property_read_bool(mcp->chip.parent->of_node,
 					      "microchip,irq-active-high");
 
 		if (type == MCP_TYPE_017)
@@ -702,7 +704,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 			goto fail;
 	}
 
-	status = gpiochip_add(&mcp->chip);
+	status = gpiochip_add_data(&mcp->chip, mcp);
 	if (status < 0)
 		goto fail;
 
diff --git a/drivers/gpio/gpio-ml-ioh.c b/drivers/gpio/gpio-ml-ioh.c
index 5536108aa9db..796a5a4bc4f5 100644
--- a/drivers/gpio/gpio-ml-ioh.c
+++ b/drivers/gpio/gpio-ml-ioh.c
@@ -106,7 +106,7 @@ static const int num_ports[] = {6, 12, 16, 16, 15, 16, 16, 12};
 static void ioh_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 {
 	u32 reg_val;
-	struct ioh_gpio *chip =	container_of(gpio, struct ioh_gpio, gpio);
+	struct ioh_gpio *chip =	gpiochip_get_data(gpio);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->spinlock, flags);
@@ -122,15 +122,15 @@ static void ioh_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 
 static int ioh_gpio_get(struct gpio_chip *gpio, unsigned nr)
 {
-	struct ioh_gpio *chip =	container_of(gpio, struct ioh_gpio, gpio);
+	struct ioh_gpio *chip =	gpiochip_get_data(gpio);
 
-	return ioread32(&chip->reg->regs[chip->ch].pi) & (1 << nr);
+	return !!(ioread32(&chip->reg->regs[chip->ch].pi) & (1 << nr));
 }
 
 static int ioh_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 				     int val)
 {
-	struct ioh_gpio *chip =	container_of(gpio, struct ioh_gpio, gpio);
+	struct ioh_gpio *chip =	gpiochip_get_data(gpio);
 	u32 pm;
 	u32 reg_val;
 	unsigned long flags;
@@ -155,7 +155,7 @@ static int ioh_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 
 static int ioh_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 {
-	struct ioh_gpio *chip =	container_of(gpio, struct ioh_gpio, gpio);
+	struct ioh_gpio *chip =	gpiochip_get_data(gpio);
 	u32 pm;
 	unsigned long flags;
 
@@ -225,7 +225,7 @@ static void ioh_gpio_restore_reg_conf(struct ioh_gpio *chip)
 
 static int ioh_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
 {
-	struct ioh_gpio *chip = container_of(gpio, struct ioh_gpio, gpio);
+	struct ioh_gpio *chip = gpiochip_get_data(gpio);
 	return chip->irq_base + offset;
 }
 
@@ -450,7 +450,7 @@ static int ioh_gpio_probe(struct pci_dev *pdev,
 		chip->ch = i;
 		spin_lock_init(&chip->spinlock);
 		ioh_gpio_setup(chip, num_ports[i]);
-		ret = gpiochip_add(&chip->gpio);
+		ret = gpiochip_add_data(&chip->gpio, chip);
 		if (ret) {
 			dev_err(&pdev->dev, "IOH gpio: Failed to register GPIO\n");
 			goto err_gpiochip_add;
diff --git a/drivers/gpio/gpio-mm-lantiq.c b/drivers/gpio/gpio-mm-lantiq.c
index f67ef2283d64..54e5d8257d34 100644
--- a/drivers/gpio/gpio-mm-lantiq.c
+++ b/drivers/gpio/gpio-mm-lantiq.c
@@ -61,9 +61,7 @@ static void ltq_mm_apply(struct ltq_mm *chip)
  */
 static void ltq_mm_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct ltq_mm *chip =
-		container_of(mm_gc, struct ltq_mm, mmchip);
+	struct ltq_mm *chip = gpiochip_get_data(gc);
 
 	if (value)
 		chip->shadow |= (1 << offset);
@@ -122,7 +120,7 @@ static int ltq_mm_probe(struct platform_device *pdev)
 	if (!of_property_read_u32(pdev->dev.of_node, "lantiq,shadow", &shadow))
 		chip->shadow = shadow;
 
-	return of_mm_gpiochip_add(pdev->dev.of_node, &chip->mmchip);
+	return of_mm_gpiochip_add_data(pdev->dev.of_node, &chip->mmchip, chip);
 }
 
 static int ltq_mm_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-moxart.c b/drivers/gpio/gpio-moxart.c
index d3355a6dc9b1..ca604538ebf7 100644
--- a/drivers/gpio/gpio-moxart.c
+++ b/drivers/gpio/gpio-moxart.c
@@ -14,7 +14,6 @@
 #include <linux/init.h>
 #include <linux/irq.h>
 #include <linux/io.h>
-#include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
@@ -23,7 +22,7 @@
 #include <linux/delay.h>
 #include <linux/timer.h>
 #include <linux/bitops.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 
 #define GPIO_DATA_OUT		0x00
 #define GPIO_DATA_IN		0x04
@@ -33,12 +32,12 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct resource *res;
-	struct bgpio_chip *bgc;
+	struct gpio_chip *gc;
 	void __iomem *base;
 	int ret;
 
-	bgc = devm_kzalloc(dev, sizeof(*bgc), GFP_KERNEL);
-	if (!bgc)
+	gc = devm_kzalloc(dev, sizeof(*gc), GFP_KERNEL);
+	if (!gc)
 		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -46,7 +45,7 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
-	ret = bgpio_init(bgc, dev, 4, base + GPIO_DATA_IN,
+	ret = bgpio_init(gc, dev, 4, base + GPIO_DATA_IN,
 			 base + GPIO_DATA_OUT, NULL,
 			 base + GPIO_PIN_DIRECTION, NULL,
 			 BGPIOF_READ_OUTPUT_REG_SET);
@@ -55,16 +54,16 @@ static int moxart_gpio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	bgc->gc.label = "moxart-gpio";
-	bgc->gc.request = gpiochip_generic_request;
-	bgc->gc.free = gpiochip_generic_free;
-	bgc->data = bgc->read_reg(bgc->reg_set);
-	bgc->gc.base = 0;
-	bgc->gc.ngpio = 32;
-	bgc->gc.dev = dev;
-	bgc->gc.owner = THIS_MODULE;
+	gc->label = "moxart-gpio";
+	gc->request = gpiochip_generic_request;
+	gc->free = gpiochip_generic_free;
+	gc->bgpio_data = gc->read_reg(gc->reg_set);
+	gc->base = 0;
+	gc->ngpio = 32;
+	gc->parent = dev;
+	gc->owner = THIS_MODULE;
 
-	ret = gpiochip_add(&bgc->gc);
+	ret = gpiochip_add_data(gc, NULL);
 	if (ret) {
 		dev_err(dev, "%s: gpiochip_add failed\n",
 			dev->of_node->full_name);
diff --git a/drivers/gpio/gpio-mpc5200.c b/drivers/gpio/gpio-mpc5200.c
index 4c542153e923..0e5a6709f27d 100644
--- a/drivers/gpio/gpio-mpc5200.c
+++ b/drivers/gpio/gpio-mpc5200.c
@@ -71,8 +71,7 @@ static inline void
 __mpc52xx_wkup_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	struct mpc52xx_gpio_wkup __iomem *regs = mm_gc->regs;
 
 	if (val)
@@ -100,8 +99,7 @@ mpc52xx_wkup_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 static int mpc52xx_wkup_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	struct mpc52xx_gpio_wkup __iomem *regs = mm_gc->regs;
 	unsigned long flags;
 
@@ -125,8 +123,7 @@ mpc52xx_wkup_gpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
 	struct mpc52xx_gpio_wkup __iomem *regs = mm_gc->regs;
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&gpio_lock, flags);
@@ -169,7 +166,7 @@ static int mpc52xx_wkup_gpiochip_probe(struct platform_device *ofdev)
 	gc->get              = mpc52xx_wkup_gpio_get;
 	gc->set              = mpc52xx_wkup_gpio_set;
 
-	ret = of_mm_gpiochip_add(ofdev->dev.of_node, &chip->mmchip);
+	ret = of_mm_gpiochip_add_data(ofdev->dev.of_node, &chip->mmchip, chip);
 	if (ret)
 		return ret;
 
@@ -236,8 +233,7 @@ static inline void
 __mpc52xx_simple_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	struct mpc52xx_gpio __iomem *regs = mm_gc->regs;
 
 	if (val)
@@ -264,8 +260,7 @@ mpc52xx_simple_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 static int mpc52xx_simple_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	struct mpc52xx_gpio __iomem *regs = mm_gc->regs;
 	unsigned long flags;
 
@@ -288,8 +283,7 @@ static int
 mpc52xx_simple_gpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct mpc52xx_gpiochip *chip = container_of(mm_gc,
-			struct mpc52xx_gpiochip, mmchip);
+	struct mpc52xx_gpiochip *chip = gpiochip_get_data(gc);
 	struct mpc52xx_gpio __iomem *regs = mm_gc->regs;
 	unsigned long flags;
 
@@ -334,7 +328,7 @@ static int mpc52xx_simple_gpiochip_probe(struct platform_device *ofdev)
 	gc->get              = mpc52xx_simple_gpio_get;
 	gc->set              = mpc52xx_simple_gpio_set;
 
-	ret = of_mm_gpiochip_add(ofdev->dev.of_node, &chip->mmchip);
+	ret = of_mm_gpiochip_add_data(ofdev->dev.of_node, &chip->mmchip, chip);
 	if (ret)
 		return ret;
 
@@ -360,15 +354,14 @@ static struct platform_driver mpc52xx_simple_gpiochip_driver = {
 	.remove = mpc52xx_gpiochip_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&mpc52xx_wkup_gpiochip_driver,
+	&mpc52xx_simple_gpiochip_driver,
+};
+
 static int __init mpc52xx_gpio_init(void)
 {
-	if (platform_driver_register(&mpc52xx_wkup_gpiochip_driver))
-		printk(KERN_ERR "Unable to register wakeup GPIO driver\n");
-
-	if (platform_driver_register(&mpc52xx_simple_gpiochip_driver))
-		printk(KERN_ERR "Unable to register simple GPIO driver\n");
-
-	return 0;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 /* Make sure we get initialised before anyone else tries to use us */
@@ -376,9 +369,7 @@ subsys_initcall(mpc52xx_gpio_init);
 
 static void __exit mpc52xx_gpio_exit(void)
 {
-	platform_driver_unregister(&mpc52xx_wkup_gpiochip_driver);
-
-	platform_driver_unregister(&mpc52xx_simple_gpiochip_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(mpc52xx_gpio_exit);
 
diff --git a/drivers/gpio/gpio-mpc8xxx.c b/drivers/gpio/gpio-mpc8xxx.c
index 48ef368347ab..9d40787e66c0 100644
--- a/drivers/gpio/gpio-mpc8xxx.c
+++ b/drivers/gpio/gpio-mpc8xxx.c
@@ -49,15 +49,10 @@ static inline u32 mpc8xxx_gpio2mask(unsigned int gpio)
 	return 1u << (MPC8XXX_GPIO_PINS - 1 - gpio);
 }
 
-static inline struct mpc8xxx_gpio_chip *
-to_mpc8xxx_gpio_chip(struct of_mm_gpio_chip *mm)
-{
-	return container_of(mm, struct mpc8xxx_gpio_chip, mm_gc);
-}
-
 static void mpc8xxx_gpio_save_regs(struct of_mm_gpio_chip *mm)
 {
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc =
+		container_of(mm, struct mpc8xxx_gpio_chip, mm_gc);
 
 	mpc8xxx_gc->data = in_be32(mm->regs + GPIO_DAT);
 }
@@ -71,7 +66,7 @@ static int mpc8572_gpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
 	u32 val;
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 	u32 out_mask, out_shadow;
 
 	out_mask = in_be32(mm->regs + GPIO_DIR);
@@ -79,7 +74,7 @@ static int mpc8572_gpio_get(struct gpio_chip *gc, unsigned int gpio)
 	val = in_be32(mm->regs + GPIO_DAT) & ~out_mask;
 	out_shadow = mpc8xxx_gc->data & out_mask;
 
-	return (val | out_shadow) & mpc8xxx_gpio2mask(gpio);
+	return !!((val | out_shadow) & mpc8xxx_gpio2mask(gpio));
 }
 
 static int mpc8xxx_gpio_get(struct gpio_chip *gc, unsigned int gpio)
@@ -92,7 +87,7 @@ static int mpc8xxx_gpio_get(struct gpio_chip *gc, unsigned int gpio)
 static void mpc8xxx_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	raw_spin_lock_irqsave(&mpc8xxx_gc->lock, flags);
@@ -111,7 +106,7 @@ static void mpc8xxx_gpio_set_multiple(struct gpio_chip *gc,
 				      unsigned long *mask, unsigned long *bits)
 {
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 	unsigned long flags;
 	int i;
 
@@ -136,7 +131,7 @@ static void mpc8xxx_gpio_set_multiple(struct gpio_chip *gc,
 static int mpc8xxx_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	raw_spin_lock_irqsave(&mpc8xxx_gc->lock, flags);
@@ -151,7 +146,7 @@ static int mpc8xxx_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 static int mpc8xxx_gpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	mpc8xxx_gpio_set(gc, gpio, val);
@@ -185,8 +180,7 @@ static int mpc5125_gpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val
 
 static int mpc8xxx_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct of_mm_gpio_chip *mm = to_of_mm_gpio_chip(gc);
-	struct mpc8xxx_gpio_chip *mpc8xxx_gc = to_mpc8xxx_gpio_chip(mm);
+	struct mpc8xxx_gpio_chip *mpc8xxx_gc = gpiochip_get_data(gc);
 
 	if (mpc8xxx_gc->irq && offset < MPC8XXX_GPIO_PINS)
 		return irq_create_mapping(mpc8xxx_gc->irq, offset);
@@ -417,7 +411,7 @@ static int mpc8xxx_probe(struct platform_device *pdev)
 	gc->set_multiple = mpc8xxx_gpio_set_multiple;
 	gc->to_irq = mpc8xxx_gpio_to_irq;
 
-	ret = of_mm_gpiochip_add(np, mm_gc);
+	ret = of_mm_gpiochip_add_data(np, mm_gc, mpc8xxx_gc);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-msic.c b/drivers/gpio/gpio-msic.c
index 22523aae8abe..d75649787e6c 100644
--- a/drivers/gpio/gpio-msic.c
+++ b/drivers/gpio/gpio-msic.c
@@ -143,7 +143,7 @@ static int msic_gpio_get(struct gpio_chip *chip, unsigned offset)
 	if (ret < 0)
 		return ret;
 
-	return r & MSIC_GPIO_DIN_MASK;
+	return !!(r & MSIC_GPIO_DIN_MASK);
 }
 
 static void msic_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -179,7 +179,7 @@ static int msic_irq_type(struct irq_data *data, unsigned type)
 
 static int msic_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct msic_gpio *mg = container_of(chip, struct msic_gpio, chip);
+	struct msic_gpio *mg = gpiochip_get_data(chip);
 	return mg->irq_base + offset;
 }
 
@@ -293,11 +293,11 @@ static int platform_msic_gpio_probe(struct platform_device *pdev)
 	mg->chip.base = pdata->gpio_base;
 	mg->chip.ngpio = MSIC_NUM_GPIO;
 	mg->chip.can_sleep = true;
-	mg->chip.dev = dev;
+	mg->chip.parent = dev;
 
 	mutex_init(&mg->buslock);
 
-	retval = gpiochip_add(&mg->chip);
+	retval = gpiochip_add_data(&mg->chip, mg);
 	if (retval) {
 		dev_err(dev, "Adding MSIC gpio chip failed\n");
 		goto err;
diff --git a/drivers/gpio/gpio-mvebu.c b/drivers/gpio/gpio-mvebu.c
index d428b97876c5..a5eacc1dff09 100644
--- a/drivers/gpio/gpio-mvebu.c
+++ b/drivers/gpio/gpio-mvebu.c
@@ -187,8 +187,7 @@ static void __iomem *mvebu_gpioreg_level_mask(struct mvebu_gpio_chip *mvchip)
 
 static void mvebu_gpio_set(struct gpio_chip *chip, unsigned pin, int value)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 u;
 
@@ -204,8 +203,7 @@ static void mvebu_gpio_set(struct gpio_chip *chip, unsigned pin, int value)
 
 static int mvebu_gpio_get(struct gpio_chip *chip, unsigned pin)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	u32 u;
 
 	if (readl_relaxed(mvebu_gpioreg_io_conf(mvchip)) & (1 << pin)) {
@@ -220,8 +218,7 @@ static int mvebu_gpio_get(struct gpio_chip *chip, unsigned pin)
 
 static void mvebu_gpio_blink(struct gpio_chip *chip, unsigned pin, int value)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 u;
 
@@ -237,8 +234,7 @@ static void mvebu_gpio_blink(struct gpio_chip *chip, unsigned pin, int value)
 
 static int mvebu_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	unsigned long flags;
 	int ret;
 	u32 u;
@@ -261,8 +257,7 @@ static int mvebu_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 static int mvebu_gpio_direction_output(struct gpio_chip *chip, unsigned pin,
 				       int value)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	unsigned long flags;
 	int ret;
 	u32 u;
@@ -287,8 +282,7 @@ static int mvebu_gpio_direction_output(struct gpio_chip *chip, unsigned pin,
 
 static int mvebu_gpio_to_irq(struct gpio_chip *chip, unsigned pin)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	return irq_create_mapping(mvchip->domain, pin);
 }
 
@@ -494,8 +488,7 @@ static void mvebu_gpio_irq_handler(struct irq_desc *desc)
 
 static void mvebu_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct mvebu_gpio_chip *mvchip =
-		container_of(chip, struct mvebu_gpio_chip, chip);
+	struct mvebu_gpio_chip *mvchip = gpiochip_get_data(chip);
 	u32 out, io_conf, blink, in_pol, data_in, cause, edg_msk, lvl_msk;
 	int i;
 
@@ -698,7 +691,7 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 
 	mvchip->soc_variant = soc_variant;
 	mvchip->chip.label = dev_name(&pdev->dev);
-	mvchip->chip.dev = &pdev->dev;
+	mvchip->chip.parent = &pdev->dev;
 	mvchip->chip.request = gpiochip_generic_request;
 	mvchip->chip.free = gpiochip_generic_free;
 	mvchip->chip.direction_input = mvebu_gpio_direction_input;
@@ -763,7 +756,7 @@ static int mvebu_gpio_probe(struct platform_device *pdev)
 		BUG();
 	}
 
-	gpiochip_add(&mvchip->chip);
+	gpiochip_add_data(&mvchip->chip, mvchip);
 
 	/* Some gpio controllers do not provide irq support */
 	if (!of_irq_count(np))
diff --git a/drivers/gpio/gpio-mxc.c b/drivers/gpio/gpio-mxc.c
index 6ea8df6c7397..7fd21cb53c81 100644
--- a/drivers/gpio/gpio-mxc.c
+++ b/drivers/gpio/gpio-mxc.c
@@ -26,10 +26,11 @@
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
-#include <linux/gpio.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
+/* FIXME: for gpio_get_value() replace this with direct register read */
+#include <linux/gpio.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/module.h>
@@ -64,7 +65,7 @@ struct mxc_gpio_port {
 	int irq;
 	int irq_high;
 	struct irq_domain *domain;
-	struct bgpio_chip bgc;
+	struct gpio_chip gc;
 	u32 both_edges;
 };
 
@@ -172,7 +173,7 @@ static int gpio_set_irq_type(struct irq_data *d, u32 type)
 	struct mxc_gpio_port *port = gc->private;
 	u32 bit, val;
 	u32 gpio_idx = d->hwirq;
-	u32 gpio = port->bgc.gc.base + gpio_idx;
+	u32 gpio = port->gc.base + gpio_idx;
 	int edge;
 	void __iomem *reg = port->base;
 
@@ -398,9 +399,7 @@ static void mxc_gpio_get_hw(struct platform_device *pdev)
 
 static int mxc_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct mxc_gpio_port *port =
-		container_of(bgc, struct mxc_gpio_port, bgc);
+	struct mxc_gpio_port *port = gpiochip_get_data(gc);
 
 	return irq_find_mapping(port->domain, offset);
 }
@@ -451,7 +450,7 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 							 port);
 	}
 
-	err = bgpio_init(&port->bgc, &pdev->dev, 4,
+	err = bgpio_init(&port->gc, &pdev->dev, 4,
 			 port->base + GPIO_PSR,
 			 port->base + GPIO_DR, NULL,
 			 port->base + GPIO_GDIR, NULL,
@@ -459,13 +458,13 @@ static int mxc_gpio_probe(struct platform_device *pdev)
 	if (err)
 		goto out_bgio;
 
-	port->bgc.gc.to_irq = mxc_gpio_to_irq;
-	port->bgc.gc.base = (pdev->id < 0) ? of_alias_get_id(np, "gpio") * 32 :
+	port->gc.to_irq = mxc_gpio_to_irq;
+	port->gc.base = (pdev->id < 0) ? of_alias_get_id(np, "gpio") * 32 :
 					     pdev->id * 32;
 
-	err = gpiochip_add(&port->bgc.gc);
+	err = gpiochip_add_data(&port->gc, port);
 	if (err)
-		goto out_bgpio_remove;
+		goto out_bgio;
 
 	irq_base = irq_alloc_descs(-1, 0, 32, numa_node_id());
 	if (irq_base < 0) {
@@ -494,9 +493,7 @@ out_irqdomain_remove:
 out_irqdesc_free:
 	irq_free_descs(irq_base, 32);
 out_gpiochip_remove:
-	gpiochip_remove(&port->bgc.gc);
-out_bgpio_remove:
-	bgpio_remove(&port->bgc);
+	gpiochip_remove(&port->gc);
 out_bgio:
 	dev_info(&pdev->dev, "%s failed with errno %d\n", __func__, err);
 	return err;
diff --git a/drivers/gpio/gpio-mxs.c b/drivers/gpio/gpio-mxs.c
index a4288f428819..b9daa0bf32a4 100644
--- a/drivers/gpio/gpio-mxs.c
+++ b/drivers/gpio/gpio-mxs.c
@@ -26,13 +26,14 @@
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
-#include <linux/gpio.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
+/* FIXME: for gpio_get_value(), replace this by direct register read */
+#include <linux/gpio.h>
 #include <linux/module.h>
 
 #define MXS_SET		0x4
@@ -64,7 +65,7 @@ struct mxs_gpio_port {
 	int id;
 	int irq;
 	struct irq_domain *domain;
-	struct bgpio_chip bgc;
+	struct gpio_chip gc;
 	enum mxs_gpio_id devid;
 	u32 both_edges;
 };
@@ -93,7 +94,7 @@ static int mxs_gpio_set_irq_type(struct irq_data *d, unsigned int type)
 	port->both_edges &= ~pin_mask;
 	switch (type) {
 	case IRQ_TYPE_EDGE_BOTH:
-		val = gpio_get_value(port->bgc.gc.base + d->hwirq);
+		val = gpio_get_value(port->gc.base + d->hwirq);
 		if (val)
 			edge = GPIO_INT_FALL_EDGE;
 		else
@@ -225,18 +226,14 @@ static int __init mxs_gpio_init_gc(struct mxs_gpio_port *port, int irq_base)
 
 static int mxs_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct mxs_gpio_port *port =
-		container_of(bgc, struct mxs_gpio_port, bgc);
+	struct mxs_gpio_port *port = gpiochip_get_data(gc);
 
 	return irq_find_mapping(port->domain, offset);
 }
 
 static int mxs_gpio_get_direction(struct gpio_chip *gc, unsigned offset)
 {
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct mxs_gpio_port *port =
-		container_of(bgc, struct mxs_gpio_port, bgc);
+	struct mxs_gpio_port *port = gpiochip_get_data(gc);
 	u32 mask = 1 << offset;
 	u32 dir;
 
@@ -330,26 +327,24 @@ static int mxs_gpio_probe(struct platform_device *pdev)
 	irq_set_chained_handler_and_data(port->irq, mxs_gpio_irq_handler,
 					 port);
 
-	err = bgpio_init(&port->bgc, &pdev->dev, 4,
+	err = bgpio_init(&port->gc, &pdev->dev, 4,
 			 port->base + PINCTRL_DIN(port),
 			 port->base + PINCTRL_DOUT(port) + MXS_SET,
 			 port->base + PINCTRL_DOUT(port) + MXS_CLR,
 			 port->base + PINCTRL_DOE(port), NULL, 0);
 	if (err)
-		goto out_irqdesc_free;
+		goto out_irqdomain_remove;
 
-	port->bgc.gc.to_irq = mxs_gpio_to_irq;
-	port->bgc.gc.get_direction = mxs_gpio_get_direction;
-	port->bgc.gc.base = port->id * 32;
+	port->gc.to_irq = mxs_gpio_to_irq;
+	port->gc.get_direction = mxs_gpio_get_direction;
+	port->gc.base = port->id * 32;
 
-	err = gpiochip_add(&port->bgc.gc);
+	err = gpiochip_add_data(&port->gc, port);
 	if (err)
-		goto out_bgpio_remove;
+		goto out_irqdomain_remove;
 
 	return 0;
 
-out_bgpio_remove:
-	bgpio_remove(&port->bgc);
 out_irqdomain_remove:
 	irq_domain_remove(port->domain);
 out_irqdesc_free:
diff --git a/drivers/gpio/gpio-octeon.c b/drivers/gpio/gpio-octeon.c
index 62ae251d4490..7665ebcd0c1d 100644
--- a/drivers/gpio/gpio-octeon.c
+++ b/drivers/gpio/gpio-octeon.c
@@ -41,7 +41,7 @@ struct octeon_gpio {
 
 static int octeon_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct octeon_gpio *gpio = container_of(chip, struct octeon_gpio, chip);
+	struct octeon_gpio *gpio = gpiochip_get_data(chip);
 
 	cvmx_write_csr(gpio->register_base + bit_cfg_reg(offset), 0);
 	return 0;
@@ -49,7 +49,7 @@ static int octeon_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 
 static void octeon_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct octeon_gpio *gpio = container_of(chip, struct octeon_gpio, chip);
+	struct octeon_gpio *gpio = gpiochip_get_data(chip);
 	u64 mask = 1ull << offset;
 	u64 reg = gpio->register_base + (value ? TX_SET : TX_CLEAR);
 	cvmx_write_csr(reg, mask);
@@ -58,7 +58,7 @@ static void octeon_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int octeon_gpio_dir_out(struct gpio_chip *chip, unsigned offset,
 			       int value)
 {
-	struct octeon_gpio *gpio = container_of(chip, struct octeon_gpio, chip);
+	struct octeon_gpio *gpio = gpiochip_get_data(chip);
 	union cvmx_gpio_bit_cfgx cfgx;
 
 	octeon_gpio_set(chip, offset, value);
@@ -72,7 +72,7 @@ static int octeon_gpio_dir_out(struct gpio_chip *chip, unsigned offset,
 
 static int octeon_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct octeon_gpio *gpio = container_of(chip, struct octeon_gpio, chip);
+	struct octeon_gpio *gpio = gpiochip_get_data(chip);
 	u64 read_bits = cvmx_read_csr(gpio->register_base + RX_DAT);
 
 	return ((1ull << offset) & read_bits) != 0;
@@ -108,7 +108,7 @@ static int octeon_gpio_probe(struct platform_device *pdev)
 
 	pdev->dev.platform_data = chip;
 	chip->label = "octeon-gpio";
-	chip->dev = &pdev->dev;
+	chip->parent = &pdev->dev;
 	chip->owner = THIS_MODULE;
 	chip->base = 0;
 	chip->can_sleep = false;
@@ -117,7 +117,7 @@ static int octeon_gpio_probe(struct platform_device *pdev)
 	chip->get = octeon_gpio_get;
 	chip->direction_output = octeon_gpio_dir_out;
 	chip->set = octeon_gpio_set;
-	err = gpiochip_add(chip);
+	err = gpiochip_add_data(chip, gpio);
 	if (err)
 		goto out;
 
@@ -128,7 +128,7 @@ out:
 
 static int octeon_gpio_remove(struct platform_device *pdev)
 {
-	struct gpio_chip *chip = pdev->dev.platform_data;
+	struct gpio_chip *chip = dev_get_platdata(&pdev->dev);
 	gpiochip_remove(chip);
 	return 0;
 }
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index f7fbb46d5d79..189f672bebc1 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -93,7 +93,7 @@ static void omap_gpio_unmask_irq(struct irq_data *d);
 static inline struct gpio_bank *omap_irq_data_get_bank(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
-	return container_of(chip, struct gpio_bank, chip);
+	return gpiochip_get_data(chip);
 }
 
 static void omap_set_gpio_direction(struct gpio_bank *bank, int gpio,
@@ -661,7 +661,7 @@ static int omap_gpio_wake_enable(struct irq_data *d, unsigned int enable)
 
 static int omap_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_bank *bank = container_of(chip, struct gpio_bank, chip);
+	struct gpio_bank *bank = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	/*
@@ -681,7 +681,7 @@ static int omap_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void omap_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_bank *bank = container_of(chip, struct gpio_bank, chip);
+	struct gpio_bank *bank = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	raw_spin_lock_irqsave(&bank->lock, flags);
@@ -954,7 +954,7 @@ static int omap_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 	void __iomem *reg;
 	int dir;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 	reg = bank->base + bank->regs->direction;
 	raw_spin_lock_irqsave(&bank->lock, flags);
 	dir = !!(readl_relaxed(reg) & BIT(offset));
@@ -967,7 +967,7 @@ static int omap_gpio_input(struct gpio_chip *chip, unsigned offset)
 	struct gpio_bank *bank;
 	unsigned long flags;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 	raw_spin_lock_irqsave(&bank->lock, flags);
 	omap_set_gpio_direction(bank, offset, 1);
 	raw_spin_unlock_irqrestore(&bank->lock, flags);
@@ -978,7 +978,7 @@ static int omap_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct gpio_bank *bank;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 
 	if (omap_gpio_is_input(bank, offset))
 		return omap_get_gpio_datain(bank, offset);
@@ -991,7 +991,7 @@ static int omap_gpio_output(struct gpio_chip *chip, unsigned offset, int value)
 	struct gpio_bank *bank;
 	unsigned long flags;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 	raw_spin_lock_irqsave(&bank->lock, flags);
 	bank->set_dataout(bank, offset, value);
 	omap_set_gpio_direction(bank, offset, 0);
@@ -1005,7 +1005,7 @@ static int omap_gpio_debounce(struct gpio_chip *chip, unsigned offset,
 	struct gpio_bank *bank;
 	unsigned long flags;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 
 	raw_spin_lock_irqsave(&bank->lock, flags);
 	omap2_set_gpio_debounce(bank, offset, debounce);
@@ -1019,7 +1019,7 @@ static void omap_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 	struct gpio_bank *bank;
 	unsigned long flags;
 
-	bank = container_of(chip, struct gpio_bank, chip);
+	bank = gpiochip_get_data(chip);
 	raw_spin_lock_irqsave(&bank->lock, flags);
 	bank->set_dataout(bank, offset, value);
 	raw_spin_unlock_irqrestore(&bank->lock, flags);
@@ -1090,7 +1090,7 @@ static int omap_gpio_chip_init(struct gpio_bank *bank, struct irq_chip *irqc)
 	if (bank->is_mpuio) {
 		bank->chip.label = "mpuio";
 		if (bank->regs->wkup_en)
-			bank->chip.dev = &omap_mpuio_device.dev;
+			bank->chip.parent = &omap_mpuio_device.dev;
 		bank->chip.base = OMAP_MPUIO(0);
 	} else {
 		bank->chip.label = "gpio";
@@ -1098,7 +1098,7 @@ static int omap_gpio_chip_init(struct gpio_bank *bank, struct irq_chip *irqc)
 	}
 	bank->chip.ngpio = bank->width;
 
-	ret = gpiochip_add(&bank->chip);
+	ret = gpiochip_add_data(&bank->chip, bank);
 	if (ret) {
 		dev_err(bank->dev, "Could not register gpio chip %d\n", ret);
 		return ret;
@@ -1197,7 +1197,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	}
 
 	bank->dev = dev;
-	bank->chip.dev = dev;
+	bank->chip.parent = dev;
 	bank->chip.owner = THIS_MODULE;
 	bank->dbck_flag = pdata->dbck_flag;
 	bank->stride = pdata->bank_stride;
diff --git a/drivers/gpio/gpio-palmas.c b/drivers/gpio/gpio-palmas.c
index 52b447c071cb..fdfb3b1e0def 100644
--- a/drivers/gpio/gpio-palmas.c
+++ b/drivers/gpio/gpio-palmas.c
@@ -35,14 +35,9 @@ struct palmas_device_data {
 	int ngpio;
 };
 
-static inline struct palmas_gpio *to_palmas_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct palmas_gpio, gpio_chip);
-}
-
 static int palmas_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct palmas_gpio *pg = to_palmas_gpio(gc);
+	struct palmas_gpio *pg = gpiochip_get_data(gc);
 	struct palmas *palmas = pg->palmas;
 	unsigned int val;
 	int ret;
@@ -54,7 +49,7 @@ static int palmas_gpio_get(struct gpio_chip *gc, unsigned offset)
 
 	ret = palmas_read(palmas, PALMAS_GPIO_BASE, reg, &val);
 	if (ret < 0) {
-		dev_err(gc->dev, "Reg 0x%02x read failed, %d\n", reg, ret);
+		dev_err(gc->parent, "Reg 0x%02x read failed, %d\n", reg, ret);
 		return ret;
 	}
 
@@ -65,7 +60,7 @@ static int palmas_gpio_get(struct gpio_chip *gc, unsigned offset)
 
 	ret = palmas_read(palmas, PALMAS_GPIO_BASE, reg, &val);
 	if (ret < 0) {
-		dev_err(gc->dev, "Reg 0x%02x read failed, %d\n", reg, ret);
+		dev_err(gc->parent, "Reg 0x%02x read failed, %d\n", reg, ret);
 		return ret;
 	}
 	return !!(val & BIT(offset));
@@ -74,7 +69,7 @@ static int palmas_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void palmas_gpio_set(struct gpio_chip *gc, unsigned offset,
 			int value)
 {
-	struct palmas_gpio *pg = to_palmas_gpio(gc);
+	struct palmas_gpio *pg = gpiochip_get_data(gc);
 	struct palmas *palmas = pg->palmas;
 	int ret;
 	unsigned int reg;
@@ -90,13 +85,13 @@ static void palmas_gpio_set(struct gpio_chip *gc, unsigned offset,
 
 	ret = palmas_write(palmas, PALMAS_GPIO_BASE, reg, BIT(offset));
 	if (ret < 0)
-		dev_err(gc->dev, "Reg 0x%02x write failed, %d\n", reg, ret);
+		dev_err(gc->parent, "Reg 0x%02x write failed, %d\n", reg, ret);
 }
 
 static int palmas_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct palmas_gpio *pg = to_palmas_gpio(gc);
+	struct palmas_gpio *pg = gpiochip_get_data(gc);
 	struct palmas *palmas = pg->palmas;
 	int ret;
 	unsigned int reg;
@@ -111,13 +106,14 @@ static int palmas_gpio_output(struct gpio_chip *gc, unsigned offset,
 	ret = palmas_update_bits(palmas, PALMAS_GPIO_BASE, reg,
 				BIT(offset), BIT(offset));
 	if (ret < 0)
-		dev_err(gc->dev, "Reg 0x%02x update failed, %d\n", reg, ret);
+		dev_err(gc->parent, "Reg 0x%02x update failed, %d\n", reg,
+			ret);
 	return ret;
 }
 
 static int palmas_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct palmas_gpio *pg = to_palmas_gpio(gc);
+	struct palmas_gpio *pg = gpiochip_get_data(gc);
 	struct palmas *palmas = pg->palmas;
 	int ret;
 	unsigned int reg;
@@ -128,13 +124,14 @@ static int palmas_gpio_input(struct gpio_chip *gc, unsigned offset)
 
 	ret = palmas_update_bits(palmas, PALMAS_GPIO_BASE, reg, BIT(offset), 0);
 	if (ret < 0)
-		dev_err(gc->dev, "Reg 0x%02x update failed, %d\n", reg, ret);
+		dev_err(gc->parent, "Reg 0x%02x update failed, %d\n", reg,
+			ret);
 	return ret;
 }
 
 static int palmas_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct palmas_gpio *pg = to_palmas_gpio(gc);
+	struct palmas_gpio *pg = gpiochip_get_data(gc);
 	struct palmas *palmas = pg->palmas;
 
 	return palmas_irq_get_virq(palmas, PALMAS_GPIO_0_IRQ + offset);
@@ -188,7 +185,7 @@ static int palmas_gpio_probe(struct platform_device *pdev)
 	palmas_gpio->gpio_chip.to_irq = palmas_gpio_to_irq;
 	palmas_gpio->gpio_chip.set	= palmas_gpio_set;
 	palmas_gpio->gpio_chip.get	= palmas_gpio_get;
-	palmas_gpio->gpio_chip.dev = &pdev->dev;
+	palmas_gpio->gpio_chip.parent = &pdev->dev;
 #ifdef CONFIG_OF_GPIO
 	palmas_gpio->gpio_chip.of_node = pdev->dev.of_node;
 #endif
@@ -198,7 +195,7 @@ static int palmas_gpio_probe(struct platform_device *pdev)
 	else
 		palmas_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&palmas_gpio->gpio_chip);
+	ret = gpiochip_add_data(&palmas_gpio->gpio_chip, palmas_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index 2d4892cc70fb..23196c5fc17c 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -18,9 +18,7 @@
 #include <linux/i2c.h>
 #include <linux/platform_data/pca953x.h>
 #include <linux/slab.h>
-#ifdef CONFIG_OF_GPIO
 #include <linux/of_platform.h>
-#endif
 #include <linux/acpi.h>
 
 #define PCA953X_INPUT		0
@@ -109,11 +107,6 @@ struct pca953x_chip {
 	unsigned long driver_data;
 };
 
-static inline struct pca953x_chip *to_pca(struct gpio_chip *gc)
-{
-	return container_of(gc, struct pca953x_chip, gpio_chip);
-}
-
 static int pca953x_read_single(struct pca953x_chip *chip, int reg, u32 *val,
 				int off)
 {
@@ -216,7 +209,7 @@ static int pca953x_read_regs(struct pca953x_chip *chip, int reg, u8 *val)
 
 static int pca953x_gpio_direction_input(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
@@ -245,7 +238,7 @@ exit:
 static int pca953x_gpio_direction_output(struct gpio_chip *gc,
 		unsigned off, int val)
 {
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
@@ -295,7 +288,7 @@ exit:
 
 static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 {
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u32 reg_val;
 	int ret, offset = 0;
 
@@ -323,7 +316,7 @@ static int pca953x_gpio_get_value(struct gpio_chip *gc, unsigned off)
 
 static void pca953x_gpio_set_value(struct gpio_chip *gc, unsigned off, int val)
 {
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 reg_val;
 	int ret, offset = 0;
 
@@ -352,6 +345,43 @@ exit:
 	mutex_unlock(&chip->i2c_lock);
 }
 
+
+static void pca953x_gpio_set_multiple(struct gpio_chip *gc,
+		unsigned long *mask, unsigned long *bits)
+{
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
+	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;
+	}
+
+	memcpy(reg_val, chip->reg_output, NBANK(chip));
+	mutex_lock(&chip->i2c_lock);
+	for(bank=0; bank<NBANK(chip); bank++) {
+		unsigned bankmask = mask[bank/4] >> ((bank % 4) * 8);
+		if(bankmask) {
+			unsigned bankval  = bits[bank/4] >> ((bank % 4) * 8);
+			reg_val[bank] = (reg_val[bank] & ~bankmask) | bankval;
+		}
+	}
+	ret = i2c_smbus_write_i2c_block_data(chip->client, offset << bank_shift, NBANK(chip), reg_val);
+	if (ret)
+		goto exit;
+
+	memcpy(chip->reg_output, reg_val, NBANK(chip));
+exit:
+	mutex_unlock(&chip->i2c_lock);
+}
+
 static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 {
 	struct gpio_chip *gc;
@@ -362,12 +392,13 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 	gc->direction_output = pca953x_gpio_direction_output;
 	gc->get = pca953x_gpio_get_value;
 	gc->set = pca953x_gpio_set_value;
+	gc->set_multiple = pca953x_gpio_set_multiple;
 	gc->can_sleep = true;
 
 	gc->base = chip->gpio_start;
 	gc->ngpio = gpios;
 	gc->label = chip->client->name;
-	gc->dev = &chip->client->dev;
+	gc->parent = &chip->client->dev;
 	gc->owner = THIS_MODULE;
 	gc->names = chip->names;
 }
@@ -376,7 +407,7 @@ static void pca953x_setup_gpio(struct pca953x_chip *chip, int gpios)
 static void pca953x_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] &= ~(1 << (d->hwirq % BANK_SZ));
 }
@@ -384,7 +415,7 @@ static void pca953x_irq_mask(struct irq_data *d)
 static void pca953x_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 
 	chip->irq_mask[d->hwirq / BANK_SZ] |= 1 << (d->hwirq % BANK_SZ);
 }
@@ -392,7 +423,7 @@ static void pca953x_irq_unmask(struct irq_data *d)
 static void pca953x_irq_bus_lock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 
 	mutex_lock(&chip->irq_lock);
 }
@@ -400,7 +431,7 @@ static void pca953x_irq_bus_lock(struct irq_data *d)
 static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	u8 new_irqs;
 	int level, i;
 
@@ -423,7 +454,7 @@ static void pca953x_irq_bus_sync_unlock(struct irq_data *d)
 static int pca953x_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pca953x_chip *chip = to_pca(gc);
+	struct pca953x_chip *chip = gpiochip_get_data(gc);
 	int bank_nb = d->hwirq / BANK_SZ;
 	u8 mask = 1 << (d->hwirq % BANK_SZ);
 
@@ -660,6 +691,8 @@ out:
 	return ret;
 }
 
+static const struct of_device_id pca953x_dt_ids[];
+
 static int pca953x_probe(struct i2c_client *client,
 				   const struct i2c_device_id *id)
 {
@@ -691,12 +724,18 @@ static int pca953x_probe(struct i2c_client *client,
 		chip->driver_data = id->driver_data;
 	} else {
 		const struct acpi_device_id *id;
+		const struct of_device_id *match;
 
-		id = acpi_match_device(pca953x_acpi_ids, &client->dev);
-		if (!id)
-			return -ENODEV;
+		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;
 
-		chip->driver_data = id->driver_data;
+			chip->driver_data = id->driver_data;
+		}
 	}
 
 	chip->chip_type = PCA_CHIP_TYPE(chip->driver_data);
@@ -715,7 +754,7 @@ static int pca953x_probe(struct i2c_client *client,
 	if (ret)
 		return ret;
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = gpiochip_add_data(&chip->gpio_chip, chip);
 	if (ret)
 		return ret;
 
@@ -755,33 +794,39 @@ static int pca953x_remove(struct i2c_client *client)
 	return 0;
 }
 
+/* convenience to stop overlong match-table lines */
+#define OF_953X(__nrgpio, __int) (void *)(__nrgpio | PCA953X_TYPE | __int)
+#define OF_957X(__nrgpio, __int) (void *)(__nrgpio | PCA957X_TYPE | __int)
+
 static const struct of_device_id pca953x_dt_ids[] = {
-	{ .compatible = "nxp,pca9505", },
-	{ .compatible = "nxp,pca9534", },
-	{ .compatible = "nxp,pca9535", },
-	{ .compatible = "nxp,pca9536", },
-	{ .compatible = "nxp,pca9537", },
-	{ .compatible = "nxp,pca9538", },
-	{ .compatible = "nxp,pca9539", },
-	{ .compatible = "nxp,pca9554", },
-	{ .compatible = "nxp,pca9555", },
-	{ .compatible = "nxp,pca9556", },
-	{ .compatible = "nxp,pca9557", },
-	{ .compatible = "nxp,pca9574", },
-	{ .compatible = "nxp,pca9575", },
-	{ .compatible = "nxp,pca9698", },
-
-	{ .compatible = "maxim,max7310", },
-	{ .compatible = "maxim,max7312", },
-	{ .compatible = "maxim,max7313", },
-	{ .compatible = "maxim,max7315", },
-
-	{ .compatible = "ti,pca6107", },
-	{ .compatible = "ti,tca6408", },
-	{ .compatible = "ti,tca6416", },
-	{ .compatible = "ti,tca6424", },
-
-	{ .compatible = "exar,xra1202", },
+	{ .compatible = "nxp,pca9505", .data = OF_953X(40, PCA_INT), },
+	{ .compatible = "nxp,pca9534", .data = OF_953X( 8, PCA_INT), },
+	{ .compatible = "nxp,pca9535", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "nxp,pca9536", .data = OF_953X( 4, 0), },
+	{ .compatible = "nxp,pca9537", .data = OF_953X( 4, PCA_INT), },
+	{ .compatible = "nxp,pca9538", .data = OF_953X( 8, PCA_INT), },
+	{ .compatible = "nxp,pca9539", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "nxp,pca9554", .data = OF_953X( 8, PCA_INT), },
+	{ .compatible = "nxp,pca9555", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "nxp,pca9556", .data = OF_953X( 8, 0), },
+	{ .compatible = "nxp,pca9557", .data = OF_953X( 8, 0), },
+	{ .compatible = "nxp,pca9574", .data = OF_957X( 8, PCA_INT), },
+	{ .compatible = "nxp,pca9575", .data = OF_957X(16, PCA_INT), },
+	{ .compatible = "nxp,pca9698", .data = OF_953X(40, 0), },
+
+	{ .compatible = "maxim,max7310", .data = OF_953X( 8, 0), },
+	{ .compatible = "maxim,max7312", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "maxim,max7313", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "maxim,max7315", .data = OF_953X( 8, PCA_INT), },
+
+	{ .compatible = "ti,pca6107", .data = OF_953X( 8, PCA_INT), },
+	{ .compatible = "ti,tca6408", .data = OF_953X( 8, PCA_INT), },
+	{ .compatible = "ti,tca6416", .data = OF_953X(16, PCA_INT), },
+	{ .compatible = "ti,tca6424", .data = OF_953X(24, PCA_INT), },
+
+	{ .compatible = "onsemi,pca9654", .data = OF_953X( 8, PCA_INT), },
+
+	{ .compatible = "exar,xra1202", .data = OF_953X( 8, 0), },
 	{ }
 };
 
diff --git a/drivers/gpio/gpio-pcf857x.c b/drivers/gpio/gpio-pcf857x.c
index 1d4d9bc8b69d..709cd3fc2a70 100644
--- a/drivers/gpio/gpio-pcf857x.c
+++ b/drivers/gpio/gpio-pcf857x.c
@@ -137,7 +137,7 @@ static int i2c_read_le16(struct i2c_client *client)
 
 static int pcf857x_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
+	struct pcf857x	*gpio = gpiochip_get_data(chip);
 	int		status;
 
 	mutex_lock(&gpio->lock);
@@ -150,16 +150,16 @@ static int pcf857x_input(struct gpio_chip *chip, unsigned offset)
 
 static int pcf857x_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
+	struct pcf857x	*gpio = gpiochip_get_data(chip);
 	int		value;
 
 	value = gpio->read(gpio->client);
-	return (value < 0) ? 0 : (value & (1 << offset));
+	return (value < 0) ? value : !!(value & (1 << offset));
 }
 
 static int pcf857x_output(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct pcf857x	*gpio = container_of(chip, struct pcf857x, chip);
+	struct pcf857x	*gpio = gpiochip_get_data(chip);
 	unsigned	bit = 1 << offset;
 	int		status;
 
@@ -293,7 +293,7 @@ static int pcf857x_probe(struct i2c_client *client,
 
 	gpio->chip.base			= pdata ? pdata->gpio_base : -1;
 	gpio->chip.can_sleep		= true;
-	gpio->chip.dev			= &client->dev;
+	gpio->chip.parent		= &client->dev;
 	gpio->chip.owner		= THIS_MODULE;
 	gpio->chip.get			= pcf857x_get;
 	gpio->chip.set			= pcf857x_set;
@@ -372,7 +372,7 @@ static int pcf857x_probe(struct i2c_client *client,
 	gpio->out = ~n_latch;
 	gpio->status = gpio->out;
 
-	status = gpiochip_add(&gpio->chip);
+	status = gpiochip_add_data(&gpio->chip, gpio);
 	if (status < 0)
 		goto fail;
 
@@ -447,7 +447,6 @@ static int pcf857x_remove(struct i2c_client *client)
 static struct i2c_driver pcf857x_driver = {
 	.driver = {
 		.name	= "pcf857x",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(pcf857x_of_table),
 	},
 	.probe	= pcf857x_probe,
diff --git a/drivers/gpio/gpio-pch.c b/drivers/gpio/gpio-pch.c
index 34ed176df15a..7c7135da5d4a 100644
--- a/drivers/gpio/gpio-pch.c
+++ b/drivers/gpio/gpio-pch.c
@@ -109,7 +109,7 @@ struct pch_gpio {
 static void pch_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 {
 	u32 reg_val;
-	struct pch_gpio *chip =	container_of(gpio, struct pch_gpio, gpio);
+	struct pch_gpio *chip =	gpiochip_get_data(gpio);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->spinlock, flags);
@@ -125,15 +125,15 @@ static void pch_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 
 static int pch_gpio_get(struct gpio_chip *gpio, unsigned nr)
 {
-	struct pch_gpio *chip =	container_of(gpio, struct pch_gpio, gpio);
+	struct pch_gpio *chip =	gpiochip_get_data(gpio);
 
-	return ioread32(&chip->reg->pi) & (1 << nr);
+	return (ioread32(&chip->reg->pi) >> nr) & 1;
 }
 
 static int pch_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 				     int val)
 {
-	struct pch_gpio *chip =	container_of(gpio, struct pch_gpio, gpio);
+	struct pch_gpio *chip =	gpiochip_get_data(gpio);
 	u32 pm;
 	u32 reg_val;
 	unsigned long flags;
@@ -158,7 +158,7 @@ static int pch_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 
 static int pch_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 {
-	struct pch_gpio *chip =	container_of(gpio, struct pch_gpio, gpio);
+	struct pch_gpio *chip =	gpiochip_get_data(gpio);
 	u32 pm;
 	unsigned long flags;
 
@@ -211,7 +211,7 @@ static void pch_gpio_restore_reg_conf(struct pch_gpio *chip)
 
 static int pch_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
 {
-	struct pch_gpio *chip = container_of(gpio, struct pch_gpio, gpio);
+	struct pch_gpio *chip = gpiochip_get_data(gpio);
 	return chip->irq_base + offset;
 }
 
@@ -220,7 +220,7 @@ static void pch_gpio_setup(struct pch_gpio *chip)
 	struct gpio_chip *gpio = &chip->gpio;
 
 	gpio->label = dev_name(chip->dev);
-	gpio->dev = chip->dev;
+	gpio->parent = chip->dev;
 	gpio->owner = THIS_MODULE;
 	gpio->direction_input = pch_gpio_direction_input;
 	gpio->get = pch_gpio_get;
@@ -394,7 +394,10 @@ static int pch_gpio_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, chip);
 	spin_lock_init(&chip->spinlock);
 	pch_gpio_setup(chip);
-	ret = gpiochip_add(&chip->gpio);
+#ifdef CONFIG_OF_GPIO
+	chip->gpio.of_node = pdev->dev.of_node;
+#endif
+	ret = gpiochip_add_data(&chip->gpio, chip);
 	if (ret) {
 		dev_err(&pdev->dev, "PCH gpio: Failed to register GPIO\n");
 		goto err_gpiochip_add;
diff --git a/drivers/gpio/gpio-pl061.c b/drivers/gpio/gpio-pl061.c
index 4d4b37676702..5cb38212bbc0 100644
--- a/drivers/gpio/gpio-pl061.c
+++ b/drivers/gpio/gpio-pl061.c
@@ -14,6 +14,7 @@
 #include <linux/module.h>
 #include <linux/io.h>
 #include <linux/ioport.h>
+#include <linux/interrupt.h>
 #include <linux/irq.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/bitops.h>
@@ -60,7 +61,7 @@ struct pl061_gpio {
 
 static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 	unsigned char gpiodir;
 
@@ -79,7 +80,7 @@ static int pl061_direction_input(struct gpio_chip *gc, unsigned offset)
 static int pl061_direction_output(struct gpio_chip *gc, unsigned offset,
 		int value)
 {
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 	unsigned char gpiodir;
 
@@ -104,14 +105,14 @@ static int pl061_direction_output(struct gpio_chip *gc, unsigned offset,
 
 static int pl061_get_value(struct gpio_chip *gc, unsigned offset)
 {
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 
 	return !!readb(chip->base + (BIT(offset + 2)));
 }
 
 static void pl061_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 
 	writeb(!!value << offset, chip->base + (BIT(offset + 2)));
 }
@@ -119,7 +120,7 @@ static void pl061_set_value(struct gpio_chip *gc, unsigned offset, int value)
 static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	int offset = irqd_to_hwirq(d);
 	unsigned long flags;
 	u8 gpiois, gpioibe, gpioiev;
@@ -131,7 +132,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 	if ((trigger & (IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW)) &&
 	    (trigger & (IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING)))
 	{
-		dev_err(gc->dev,
+		dev_err(gc->parent,
 			"trying to configure line %d for both level and edge "
 			"detection, choose one!\n",
 			offset);
@@ -158,7 +159,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 		else
 			gpioiev &= ~bit;
 		irq_set_handler_locked(d, handle_level_irq);
-		dev_dbg(gc->dev, "line %d: IRQ on %s level\n",
+		dev_dbg(gc->parent, "line %d: IRQ on %s level\n",
 			offset,
 			polarity ? "HIGH" : "LOW");
 	} else if ((trigger & IRQ_TYPE_EDGE_BOTH) == IRQ_TYPE_EDGE_BOTH) {
@@ -167,7 +168,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 		/* Select both edges, setting this makes GPIOEV be ignored */
 		gpioibe |= bit;
 		irq_set_handler_locked(d, handle_edge_irq);
-		dev_dbg(gc->dev, "line %d: IRQ on both edges\n", offset);
+		dev_dbg(gc->parent, "line %d: IRQ on both edges\n", offset);
 	} else if ((trigger & IRQ_TYPE_EDGE_RISING) ||
 		   (trigger & IRQ_TYPE_EDGE_FALLING)) {
 		bool rising = trigger & IRQ_TYPE_EDGE_RISING;
@@ -182,7 +183,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 		else
 			gpioiev &= ~bit;
 		irq_set_handler_locked(d, handle_edge_irq);
-		dev_dbg(gc->dev, "line %d: IRQ on %s edge\n",
+		dev_dbg(gc->parent, "line %d: IRQ on %s edge\n",
 			offset,
 			rising ? "RISING" : "FALLING");
 	} else {
@@ -191,7 +192,7 @@ static int pl061_irq_type(struct irq_data *d, unsigned trigger)
 		gpioibe &= ~bit;
 		gpioiev &= ~bit;
 		irq_set_handler_locked(d, handle_bad_irq);
-		dev_warn(gc->dev, "no trigger selected for line %d\n",
+		dev_warn(gc->parent, "no trigger selected for line %d\n",
 			 offset);
 	}
 
@@ -209,7 +210,7 @@ static void pl061_irq_handler(struct irq_desc *desc)
 	unsigned long pending;
 	int offset;
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	struct irq_chip *irqchip = irq_desc_get_chip(desc);
 
 	chained_irq_enter(irqchip, desc);
@@ -227,7 +228,7 @@ static void pl061_irq_handler(struct irq_desc *desc)
 static void pl061_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
@@ -240,7 +241,7 @@ static void pl061_irq_mask(struct irq_data *d)
 static void pl061_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 	u8 gpioie;
 
@@ -261,7 +262,7 @@ static void pl061_irq_unmask(struct irq_data *d)
 static void pl061_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct pl061_gpio *chip = container_of(gc, struct pl061_gpio, gc);
+	struct pl061_gpio *chip = gpiochip_get_data(gc);
 	u8 mask = BIT(irqd_to_hwirq(d) % PL061_GPIO_NR);
 
 	spin_lock(&chip->lock);
@@ -269,12 +270,20 @@ static void pl061_irq_ack(struct irq_data *d)
 	spin_unlock(&chip->lock);
 }
 
+static int pl061_irq_set_wake(struct irq_data *d, unsigned int state)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+
+	return irq_set_irq_wake(gc->irq_parent, state);
+}
+
 static struct irq_chip pl061_irqchip = {
 	.name		= "pl061",
 	.irq_ack	= pl061_irq_ack,
 	.irq_mask	= pl061_irq_mask,
 	.irq_unmask	= pl061_irq_unmask,
 	.irq_set_type	= pl061_irq_type,
+	.irq_set_wake	= pl061_irq_set_wake,
 };
 
 static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
@@ -316,10 +325,10 @@ static int pl061_probe(struct amba_device *adev, const struct amba_id *id)
 	chip->gc.set = pl061_set_value;
 	chip->gc.ngpio = PL061_GPIO_NR;
 	chip->gc.label = dev_name(dev);
-	chip->gc.dev = dev;
+	chip->gc.parent = dev;
 	chip->gc.owner = THIS_MODULE;
 
-	ret = gpiochip_add(&chip->gc);
+	ret = gpiochip_add_data(&chip->gc, chip);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-pxa.c b/drivers/gpio/gpio-pxa.c
index df2ce550f309..b2b7b78664b8 100644
--- a/drivers/gpio/gpio-pxa.c
+++ b/drivers/gpio/gpio-pxa.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/syscore_ops.h>
 #include <linux/slab.h>
@@ -64,20 +65,11 @@
 int pxa_last_gpio;
 static int irq_base;
 
-#ifdef CONFIG_OF
-static struct irq_domain *domain;
-static struct device_node *pxa_gpio_of_node;
-#endif
-
-struct pxa_gpio_chip {
-	struct gpio_chip chip;
+struct pxa_gpio_bank {
 	void __iomem	*regbase;
-	char label[10];
-
 	unsigned long	irq_mask;
 	unsigned long	irq_edge_rise;
 	unsigned long	irq_edge_fall;
-	int (*set_wake)(unsigned int gpio, unsigned int on);
 
 #ifdef CONFIG_PM
 	unsigned long	saved_gplr;
@@ -87,6 +79,17 @@ struct pxa_gpio_chip {
 #endif
 };
 
+struct pxa_gpio_chip {
+	struct device *dev;
+	struct gpio_chip chip;
+	struct pxa_gpio_bank *banks;
+	struct irq_domain *irqdomain;
+
+	int irq0;
+	int irq1;
+	int (*set_wake)(unsigned int gpio, unsigned int on);
+};
+
 enum pxa_gpio_type {
 	PXA25X_GPIO = 0,
 	PXA26X_GPIO,
@@ -104,9 +107,8 @@ struct pxa_gpio_id {
 };
 
 static DEFINE_SPINLOCK(gpio_lock);
-static struct pxa_gpio_chip *pxa_gpio_chips;
+static struct pxa_gpio_chip *pxa_gpio_chip;
 static enum pxa_gpio_type gpio_type;
-static void __iomem *gpio_reg_base;
 
 static struct pxa_gpio_id pxa25x_id = {
 	.type		= PXA25X_GPIO,
@@ -148,17 +150,28 @@ static struct pxa_gpio_id pxa1928_id = {
 	.gpio_nums	= 224,
 };
 
-#define for_each_gpio_chip(i, c)			\
-	for (i = 0, c = &pxa_gpio_chips[0]; i <= pxa_last_gpio; i += 32, c++)
+#define for_each_gpio_bank(i, b, pc)					\
+	for (i = 0, b = pc->banks; i <= pxa_last_gpio; i += 32, b++)
 
-static inline void __iomem *gpio_chip_base(struct gpio_chip *c)
+static inline struct pxa_gpio_chip *chip_to_pxachip(struct gpio_chip *c)
 {
-	return container_of(c, struct pxa_gpio_chip, chip)->regbase;
+	struct pxa_gpio_chip *pxa_chip = gpiochip_get_data(c);
+
+	return pxa_chip;
 }
 
-static inline struct pxa_gpio_chip *gpio_to_pxachip(unsigned gpio)
+static inline void __iomem *gpio_bank_base(struct gpio_chip *c, int gpio)
 {
-	return &pxa_gpio_chips[gpio_to_bank(gpio)];
+	struct pxa_gpio_chip *p = gpiochip_get_data(c);
+	struct pxa_gpio_bank *bank = p->banks + (gpio / 32);
+
+	return bank->regbase;
+}
+
+static inline struct pxa_gpio_bank *gpio_to_pxabank(struct gpio_chip *c,
+						    unsigned gpio)
+{
+	return chip_to_pxachip(c)->banks + gpio / 32;
 }
 
 static inline int gpio_is_pxa_type(int type)
@@ -187,15 +200,13 @@ static inline int __gpio_is_inverted(int gpio)
  * is attributed as "occupied" here (I know this terminology isn't
  * accurate, you are welcome to propose a better one :-)
  */
-static inline int __gpio_is_occupied(unsigned gpio)
+static inline int __gpio_is_occupied(struct pxa_gpio_chip *pchip, unsigned gpio)
 {
-	struct pxa_gpio_chip *pxachip;
 	void __iomem *base;
 	unsigned long gafr = 0, gpdr = 0;
 	int ret, af = 0, dir = 0;
 
-	pxachip = gpio_to_pxachip(gpio);
-	base = gpio_chip_base(&pxachip->chip);
+	base = gpio_bank_base(&pchip->chip, gpio);
 	gpdr = readl_relaxed(base + GPDR_OFFSET);
 
 	switch (gpio_type) {
@@ -218,21 +229,35 @@ static inline int __gpio_is_occupied(unsigned gpio)
 	return ret;
 }
 
-static int pxa_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
+int pxa_irq_to_gpio(int irq)
 {
-	return chip->base + offset + irq_base;
+	struct pxa_gpio_chip *pchip = pxa_gpio_chip;
+	int irq_gpio0;
+
+	irq_gpio0 = irq_find_mapping(pchip->irqdomain, 0);
+	if (irq_gpio0 > 0)
+		return irq - irq_gpio0;
+
+	return irq_gpio0;
 }
 
-int pxa_irq_to_gpio(int irq)
+static int pxa_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	return irq - irq_base;
+	struct pxa_gpio_chip *pchip = chip_to_pxachip(chip);
+
+	return irq_find_mapping(pchip->irqdomain, offset);
 }
 
 static int pxa_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	void __iomem *base = gpio_chip_base(chip);
-	uint32_t value, mask = 1 << offset;
+	void __iomem *base = gpio_bank_base(chip, offset);
+	uint32_t value, mask = GPIO_bit(offset);
 	unsigned long flags;
+	int ret;
+
+	ret = pinctrl_gpio_direction_input(chip->base + offset);
+	if (!ret)
+		return 0;
 
 	spin_lock_irqsave(&gpio_lock, flags);
 
@@ -250,12 +275,17 @@ static int pxa_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int pxa_gpio_direction_output(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
-	void __iomem *base = gpio_chip_base(chip);
-	uint32_t tmp, mask = 1 << offset;
+	void __iomem *base = gpio_bank_base(chip, offset);
+	uint32_t tmp, mask = GPIO_bit(offset);
 	unsigned long flags;
+	int ret;
 
 	writel_relaxed(mask, base + (value ? GPSR_OFFSET : GPCR_OFFSET));
 
+	ret = pinctrl_gpio_direction_output(chip->base + offset);
+	if (!ret)
+		return 0;
+
 	spin_lock_irqsave(&gpio_lock, flags);
 
 	tmp = readl_relaxed(base + GPDR_OFFSET);
@@ -271,14 +301,18 @@ static int pxa_gpio_direction_output(struct gpio_chip *chip,
 
 static int pxa_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	u32 gplr = readl_relaxed(gpio_chip_base(chip) + GPLR_OFFSET);
-	return !!(gplr & (1 << offset));
+	void __iomem *base = gpio_bank_base(chip, offset);
+	u32 gplr = readl_relaxed(base + GPLR_OFFSET);
+
+	return !!(gplr & GPIO_bit(offset));
 }
 
 static void pxa_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	writel_relaxed(1 << offset, gpio_chip_base(chip) +
-				(value ? GPSR_OFFSET : GPCR_OFFSET));
+	void __iomem *base = gpio_bank_base(chip, offset);
+
+	writel_relaxed(GPIO_bit(offset),
+		       base + (value ? GPSR_OFFSET : GPCR_OFFSET));
 }
 
 #ifdef CONFIG_OF_GPIO
@@ -289,61 +323,61 @@ static int pxa_gpio_of_xlate(struct gpio_chip *gc,
 	if (gpiospec->args[0] > pxa_last_gpio)
 		return -EINVAL;
 
-	if (gc != &pxa_gpio_chips[gpiospec->args[0] / 32].chip)
-		return -EINVAL;
-
 	if (flags)
 		*flags = gpiospec->args[1];
 
-	return gpiospec->args[0] % 32;
+	return gpiospec->args[0];
 }
 #endif
 
-static int pxa_init_gpio_chip(int gpio_end,
-					int (*set_wake)(unsigned int, unsigned int))
+static int pxa_gpio_request(struct gpio_chip *chip, unsigned int offset)
 {
-	int i, gpio, nbanks = gpio_to_bank(gpio_end) + 1;
-	struct pxa_gpio_chip *chips;
-
-	chips = kzalloc(nbanks * sizeof(struct pxa_gpio_chip), GFP_KERNEL);
-	if (chips == NULL) {
-		pr_err("%s: failed to allocate GPIO chips\n", __func__);
-		return -ENOMEM;
-	}
+	return pinctrl_request_gpio(chip->base + offset);
+}
 
-	for (i = 0, gpio = 0; i < nbanks; i++, gpio += 32) {
-		struct gpio_chip *c = &chips[i].chip;
+static void pxa_gpio_free(struct gpio_chip *chip, unsigned int offset)
+{
+	pinctrl_free_gpio(chip->base + offset);
+}
 
-		sprintf(chips[i].label, "gpio-%d", i);
-		chips[i].regbase = gpio_reg_base + BANK_OFF(i);
-		chips[i].set_wake = set_wake;
+static int pxa_init_gpio_chip(struct pxa_gpio_chip *pchip, int ngpio,
+			      struct device_node *np, void __iomem *regbase)
+{
+	int i, gpio, nbanks = DIV_ROUND_UP(ngpio, 32);
+	struct pxa_gpio_bank *bank;
 
-		c->base  = gpio;
-		c->label = chips[i].label;
+	pchip->banks = devm_kcalloc(pchip->dev, nbanks, sizeof(*pchip->banks),
+				    GFP_KERNEL);
+	if (!pchip->banks)
+		return -ENOMEM;
 
-		c->direction_input  = pxa_gpio_direction_input;
-		c->direction_output = pxa_gpio_direction_output;
-		c->get = pxa_gpio_get;
-		c->set = pxa_gpio_set;
-		c->to_irq = pxa_gpio_to_irq;
+	pchip->chip.label = "gpio-pxa";
+	pchip->chip.direction_input  = pxa_gpio_direction_input;
+	pchip->chip.direction_output = pxa_gpio_direction_output;
+	pchip->chip.get = pxa_gpio_get;
+	pchip->chip.set = pxa_gpio_set;
+	pchip->chip.to_irq = pxa_gpio_to_irq;
+	pchip->chip.ngpio = ngpio;
+	pchip->chip.request = pxa_gpio_request;
+	pchip->chip.free = pxa_gpio_free;
 #ifdef CONFIG_OF_GPIO
-		c->of_node = pxa_gpio_of_node;
-		c->of_xlate = pxa_gpio_of_xlate;
-		c->of_gpio_n_cells = 2;
+	pchip->chip.of_node = np;
+	pchip->chip.of_xlate = pxa_gpio_of_xlate;
+	pchip->chip.of_gpio_n_cells = 2;
 #endif
 
-		/* number of GPIOs on last bank may be less than 32 */
-		c->ngpio = (gpio + 31 > gpio_end) ? (gpio_end - gpio + 1) : 32;
-		gpiochip_add(c);
+	for (i = 0, gpio = 0; i < nbanks; i++, gpio += 32) {
+		bank = pchip->banks + i;
+		bank->regbase = regbase + BANK_OFF(i);
 	}
-	pxa_gpio_chips = chips;
-	return 0;
+
+	return gpiochip_add_data(&pchip->chip, pchip);
 }
 
 /* Update only those GRERx and GFERx edge detection register bits if those
  * bits are set in c->irq_mask
  */
-static inline void update_edge_detect(struct pxa_gpio_chip *c)
+static inline void update_edge_detect(struct pxa_gpio_bank *c)
 {
 	uint32_t grer, gfer;
 
@@ -357,12 +391,11 @@ static inline void update_edge_detect(struct pxa_gpio_chip *c)
 
 static int pxa_gpio_irq_type(struct irq_data *d, unsigned int type)
 {
-	struct pxa_gpio_chip *c;
-	int gpio = pxa_irq_to_gpio(d->irq);
+	struct pxa_gpio_chip *pchip = irq_data_get_irq_chip_data(d);
+	unsigned int gpio = irqd_to_hwirq(d);
+	struct pxa_gpio_bank *c = gpio_to_pxabank(&pchip->chip, gpio);
 	unsigned long gpdr, mask = GPIO_bit(gpio);
 
-	c = gpio_to_pxachip(gpio);
-
 	if (type == IRQ_TYPE_PROBE) {
 		/* Don't mess with enabled GPIOs using preconfigured edges or
 		 * GPIOs set to alternate function or to output during probe
@@ -370,7 +403,7 @@ static int pxa_gpio_irq_type(struct irq_data *d, unsigned int type)
 		if ((c->irq_edge_rise | c->irq_edge_fall) & GPIO_bit(gpio))
 			return 0;
 
-		if (__gpio_is_occupied(gpio))
+		if (__gpio_is_occupied(pchip, gpio))
 			return 0;
 
 		type = IRQ_TYPE_EDGE_RISING | IRQ_TYPE_EDGE_FALLING;
@@ -401,20 +434,16 @@ static int pxa_gpio_irq_type(struct irq_data *d, unsigned int type)
 	return 0;
 }
 
-static void pxa_gpio_demux_handler(struct irq_desc *desc)
+static irqreturn_t pxa_gpio_demux_handler(int in_irq, void *d)
 {
-	struct pxa_gpio_chip *c;
-	int loop, gpio, gpio_base, n;
+	int loop, gpio, n, handled = 0;
 	unsigned long gedr;
-	struct irq_chip *chip = irq_desc_get_chip(desc);
-
-	chained_irq_enter(chip, desc);
+	struct pxa_gpio_chip *pchip = d;
+	struct pxa_gpio_bank *c;
 
 	do {
 		loop = 0;
-		for_each_gpio_chip(gpio, c) {
-			gpio_base = c->chip.base;
-
+		for_each_gpio_bank(gpio, c, pchip) {
 			gedr = readl_relaxed(c->regbase + GEDR_OFFSET);
 			gedr = gedr & c->irq_mask;
 			writel_relaxed(gedr, c->regbase + GEDR_OFFSET);
@@ -422,51 +451,71 @@ static void pxa_gpio_demux_handler(struct irq_desc *desc)
 			for_each_set_bit(n, &gedr, BITS_PER_LONG) {
 				loop = 1;
 
-				generic_handle_irq(gpio_to_irq(gpio_base + n));
+				generic_handle_irq(gpio_to_irq(gpio + n));
 			}
 		}
+		handled += loop;
 	} while (loop);
 
-	chained_irq_exit(chip, desc);
+	return handled ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static irqreturn_t pxa_gpio_direct_handler(int in_irq, void *d)
+{
+	struct pxa_gpio_chip *pchip = d;
+
+	if (in_irq == pchip->irq0) {
+		generic_handle_irq(gpio_to_irq(0));
+	} else if (in_irq == pchip->irq1) {
+		generic_handle_irq(gpio_to_irq(1));
+	} else {
+		pr_err("%s() unknown irq %d\n", __func__, in_irq);
+		return IRQ_NONE;
+	}
+	return IRQ_HANDLED;
 }
 
 static void pxa_ack_muxed_gpio(struct irq_data *d)
 {
-	int gpio = pxa_irq_to_gpio(d->irq);
-	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
+	struct pxa_gpio_chip *pchip = irq_data_get_irq_chip_data(d);
+	unsigned int gpio = irqd_to_hwirq(d);
+	void __iomem *base = gpio_bank_base(&pchip->chip, gpio);
 
-	writel_relaxed(GPIO_bit(gpio), c->regbase + GEDR_OFFSET);
+	writel_relaxed(GPIO_bit(gpio), base + GEDR_OFFSET);
 }
 
 static void pxa_mask_muxed_gpio(struct irq_data *d)
 {
-	int gpio = pxa_irq_to_gpio(d->irq);
-	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
+	struct pxa_gpio_chip *pchip = irq_data_get_irq_chip_data(d);
+	unsigned int gpio = irqd_to_hwirq(d);
+	struct pxa_gpio_bank *b = gpio_to_pxabank(&pchip->chip, gpio);
+	void __iomem *base = gpio_bank_base(&pchip->chip, gpio);
 	uint32_t grer, gfer;
 
-	c->irq_mask &= ~GPIO_bit(gpio);
+	b->irq_mask &= ~GPIO_bit(gpio);
 
-	grer = readl_relaxed(c->regbase + GRER_OFFSET) & ~GPIO_bit(gpio);
-	gfer = readl_relaxed(c->regbase + GFER_OFFSET) & ~GPIO_bit(gpio);
-	writel_relaxed(grer, c->regbase + GRER_OFFSET);
-	writel_relaxed(gfer, c->regbase + GFER_OFFSET);
+	grer = readl_relaxed(base + GRER_OFFSET) & ~GPIO_bit(gpio);
+	gfer = readl_relaxed(base + GFER_OFFSET) & ~GPIO_bit(gpio);
+	writel_relaxed(grer, base + GRER_OFFSET);
+	writel_relaxed(gfer, base + GFER_OFFSET);
 }
 
 static int pxa_gpio_set_wake(struct irq_data *d, unsigned int on)
 {
-	int gpio = pxa_irq_to_gpio(d->irq);
-	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
+	struct pxa_gpio_chip *pchip = irq_data_get_irq_chip_data(d);
+	unsigned int gpio = irqd_to_hwirq(d);
 
-	if (c->set_wake)
-		return c->set_wake(gpio, on);
+	if (pchip->set_wake)
+		return pchip->set_wake(gpio, on);
 	else
 		return 0;
 }
 
 static void pxa_unmask_muxed_gpio(struct irq_data *d)
 {
-	int gpio = pxa_irq_to_gpio(d->irq);
-	struct pxa_gpio_chip *c = gpio_to_pxachip(gpio);
+	struct pxa_gpio_chip *pchip = irq_data_get_irq_chip_data(d);
+	unsigned int gpio = irqd_to_hwirq(d);
+	struct pxa_gpio_bank *c = gpio_to_pxabank(&pchip->chip, gpio);
 
 	c->irq_mask |= GPIO_bit(gpio);
 	update_edge_detect(c);
@@ -506,24 +555,12 @@ static int pxa_gpio_nums(struct platform_device *pdev)
 	return count;
 }
 
-#ifdef CONFIG_OF
-static const struct of_device_id pxa_gpio_dt_ids[] = {
-	{ .compatible = "intel,pxa25x-gpio",	.data = &pxa25x_id, },
-	{ .compatible = "intel,pxa26x-gpio",	.data = &pxa26x_id, },
-	{ .compatible = "intel,pxa27x-gpio",	.data = &pxa27x_id, },
-	{ .compatible = "intel,pxa3xx-gpio",	.data = &pxa3xx_id, },
-	{ .compatible = "marvell,pxa93x-gpio",	.data = &pxa93x_id, },
-	{ .compatible = "marvell,mmp-gpio",	.data = &mmp_id, },
-	{ .compatible = "marvell,mmp2-gpio",	.data = &mmp2_id, },
-	{ .compatible = "marvell,pxa1928-gpio",	.data = &pxa1928_id, },
-	{}
-};
-
 static int pxa_irq_domain_map(struct irq_domain *d, unsigned int irq,
 			      irq_hw_number_t hw)
 {
 	irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
 				 handle_edge_irq);
+	irq_set_chip_data(irq, d->host_data);
 	irq_set_noprobe(irq);
 	return 0;
 }
@@ -533,10 +570,23 @@ const struct irq_domain_ops pxa_irq_domain_ops = {
 	.xlate	= irq_domain_xlate_twocell,
 };
 
-static int pxa_gpio_probe_dt(struct platform_device *pdev)
+#ifdef CONFIG_OF
+static const struct of_device_id pxa_gpio_dt_ids[] = {
+	{ .compatible = "intel,pxa25x-gpio",	.data = &pxa25x_id, },
+	{ .compatible = "intel,pxa26x-gpio",	.data = &pxa26x_id, },
+	{ .compatible = "intel,pxa27x-gpio",	.data = &pxa27x_id, },
+	{ .compatible = "intel,pxa3xx-gpio",	.data = &pxa3xx_id, },
+	{ .compatible = "marvell,pxa93x-gpio",	.data = &pxa93x_id, },
+	{ .compatible = "marvell,mmp-gpio",	.data = &mmp_id, },
+	{ .compatible = "marvell,mmp2-gpio",	.data = &mmp2_id, },
+	{ .compatible = "marvell,pxa1928-gpio",	.data = &pxa1928_id, },
+	{}
+};
+
+static int pxa_gpio_probe_dt(struct platform_device *pdev,
+			     struct pxa_gpio_chip *pchip)
 {
-	int ret = 0, nr_gpios;
-	struct device_node *np = pdev->dev.of_node;
+	int nr_gpios;
 	const struct of_device_id *of_id =
 				of_match_device(pxa_gpio_dt_ids, &pdev->dev);
 	const struct pxa_gpio_id *gpio_id;
@@ -554,57 +604,64 @@ static int pxa_gpio_probe_dt(struct platform_device *pdev)
 	irq_base = irq_alloc_descs(-1, 0, nr_gpios, 0);
 	if (irq_base < 0) {
 		dev_err(&pdev->dev, "Failed to allocate IRQ numbers\n");
-		ret = irq_base;
-		goto err;
+		return irq_base;
 	}
-	domain = irq_domain_add_legacy(np, nr_gpios, irq_base, 0,
-				       &pxa_irq_domain_ops, NULL);
-	pxa_gpio_of_node = np;
-	return 0;
-err:
-	iounmap(gpio_reg_base);
-	return ret;
+	return irq_base;
 }
 #else
-#define pxa_gpio_probe_dt(pdev)		(-1)
+#define pxa_gpio_probe_dt(pdev, pchip)		(-1)
 #endif
 
 static int pxa_gpio_probe(struct platform_device *pdev)
 {
-	struct pxa_gpio_chip *c;
+	struct pxa_gpio_chip *pchip;
+	struct pxa_gpio_bank *c;
 	struct resource *res;
 	struct clk *clk;
 	struct pxa_gpio_platform_data *info;
-	int gpio, irq, ret, use_of = 0;
+	void __iomem *gpio_reg_base;
+	int gpio, ret;
 	int irq0 = 0, irq1 = 0, irq_mux, gpio_offset = 0;
 
+	pchip = devm_kzalloc(&pdev->dev, sizeof(*pchip), GFP_KERNEL);
+	if (!pchip)
+		return -ENOMEM;
+	pchip->dev = &pdev->dev;
+
 	info = dev_get_platdata(&pdev->dev);
 	if (info) {
 		irq_base = info->irq_base;
 		if (irq_base <= 0)
 			return -EINVAL;
 		pxa_last_gpio = pxa_gpio_nums(pdev);
+		pchip->set_wake = info->gpio_set_wake;
 	} else {
-		irq_base = 0;
-		use_of = 1;
-		ret = pxa_gpio_probe_dt(pdev);
-		if (ret < 0)
+		irq_base = pxa_gpio_probe_dt(pdev, pchip);
+		if (irq_base < 0)
 			return -EINVAL;
 	}
 
 	if (!pxa_last_gpio)
 		return -EINVAL;
 
+	pchip->irqdomain = irq_domain_add_legacy(pdev->dev.of_node,
+						 pxa_last_gpio + 1, irq_base,
+						 0, &pxa_irq_domain_ops, pchip);
+	if (!pchip->irqdomain)
+		return -ENOMEM;
+
 	irq0 = platform_get_irq_byname(pdev, "gpio0");
 	irq1 = platform_get_irq_byname(pdev, "gpio1");
 	irq_mux = platform_get_irq_byname(pdev, "gpio_mux");
 	if ((irq0 > 0 && irq1 <= 0) || (irq0 <= 0 && irq1 > 0)
 		|| (irq_mux <= 0))
 		return -EINVAL;
+
+	pchip->irq0 = irq0;
+	pchip->irq1 = irq1;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res)
-		return -EINVAL;
-	gpio_reg_base = ioremap(res->start, resource_size(res));
+	gpio_reg_base = devm_ioremap(&pdev->dev, res->start,
+				     resource_size(res));
 	if (!gpio_reg_base)
 		return -EINVAL;
 
@@ -615,21 +672,24 @@ static int pxa_gpio_probe(struct platform_device *pdev)
 	if (IS_ERR(clk)) {
 		dev_err(&pdev->dev, "Error %ld to get gpio clock\n",
 			PTR_ERR(clk));
-		iounmap(gpio_reg_base);
 		return PTR_ERR(clk);
 	}
 	ret = clk_prepare_enable(clk);
 	if (ret) {
 		clk_put(clk);
-		iounmap(gpio_reg_base);
 		return ret;
 	}
 
 	/* Initialize GPIO chips */
-	pxa_init_gpio_chip(pxa_last_gpio, info ? info->gpio_set_wake : NULL);
+	ret = pxa_init_gpio_chip(pchip, pxa_last_gpio + 1, pdev->dev.of_node,
+				 gpio_reg_base);
+	if (ret) {
+		clk_put(clk);
+		return ret;
+	}
 
 	/* clear all GPIO edge detects */
-	for_each_gpio_chip(gpio, c) {
+	for_each_gpio_bank(gpio, c, pchip) {
 		writel_relaxed(0, c->regbase + GFER_OFFSET);
 		writel_relaxed(0, c->regbase + GRER_OFFSET);
 		writel_relaxed(~0, c->regbase + GEDR_OFFSET);
@@ -638,34 +698,31 @@ static int pxa_gpio_probe(struct platform_device *pdev)
 			writel_relaxed(~0, c->regbase + ED_MASK_OFFSET);
 	}
 
-	if (!use_of) {
-		if (irq0 > 0) {
-			irq = gpio_to_irq(0);
-			irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
-						 handle_edge_irq);
-			irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
-		}
-		if (irq1 > 0) {
-			irq = gpio_to_irq(1);
-			irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
-						 handle_edge_irq);
-			irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
-		}
-
-		for (irq  = gpio_to_irq(gpio_offset);
-			irq <= gpio_to_irq(pxa_last_gpio); irq++) {
-			irq_set_chip_and_handler(irq, &pxa_muxed_gpio_chip,
-						 handle_edge_irq);
-			irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
-		}
+	if (irq0 > 0) {
+		ret = devm_request_irq(&pdev->dev,
+				       irq0, pxa_gpio_direct_handler, 0,
+				       "gpio-0", pchip);
+		if (ret)
+			dev_err(&pdev->dev, "request of gpio0 irq failed: %d\n",
+				ret);
 	}
+	if (irq1 > 0) {
+		ret = devm_request_irq(&pdev->dev,
+				       irq1, pxa_gpio_direct_handler, 0,
+				       "gpio-1", pchip);
+		if (ret)
+			dev_err(&pdev->dev, "request of gpio1 irq failed: %d\n",
+				ret);
+	}
+	ret = devm_request_irq(&pdev->dev,
+			       irq_mux, pxa_gpio_demux_handler, 0,
+				       "gpio-mux", pchip);
+	if (ret)
+		dev_err(&pdev->dev, "request of gpio-mux irq failed: %d\n",
+				ret);
 
-	if (irq0 > 0)
-		irq_set_chained_handler(irq0, pxa_gpio_demux_handler);
-	if (irq1 > 0)
-		irq_set_chained_handler(irq1, pxa_gpio_demux_handler);
+	pxa_gpio_chip = pchip;
 
-	irq_set_chained_handler(irq_mux, pxa_gpio_demux_handler);
 	return 0;
 }
 
@@ -690,19 +747,32 @@ static struct platform_driver pxa_gpio_driver = {
 	.id_table	= gpio_id_table,
 };
 
-static int __init pxa_gpio_init(void)
+static int __init pxa_gpio_legacy_init(void)
 {
+	if (of_have_populated_dt())
+		return 0;
+
 	return platform_driver_register(&pxa_gpio_driver);
 }
-postcore_initcall(pxa_gpio_init);
+postcore_initcall(pxa_gpio_legacy_init);
+
+static int __init pxa_gpio_dt_init(void)
+{
+	if (of_have_populated_dt())
+		return platform_driver_register(&pxa_gpio_driver);
+
+	return 0;
+}
+device_initcall(pxa_gpio_dt_init);
 
 #ifdef CONFIG_PM
 static int pxa_gpio_suspend(void)
 {
-	struct pxa_gpio_chip *c;
+	struct pxa_gpio_chip *pchip = pxa_gpio_chip;
+	struct pxa_gpio_bank *c;
 	int gpio;
 
-	for_each_gpio_chip(gpio, c) {
+	for_each_gpio_bank(gpio, c, pchip) {
 		c->saved_gplr = readl_relaxed(c->regbase + GPLR_OFFSET);
 		c->saved_gpdr = readl_relaxed(c->regbase + GPDR_OFFSET);
 		c->saved_grer = readl_relaxed(c->regbase + GRER_OFFSET);
@@ -716,10 +786,11 @@ static int pxa_gpio_suspend(void)
 
 static void pxa_gpio_resume(void)
 {
-	struct pxa_gpio_chip *c;
+	struct pxa_gpio_chip *pchip = pxa_gpio_chip;
+	struct pxa_gpio_bank *c;
 	int gpio;
 
-	for_each_gpio_chip(gpio, c) {
+	for_each_gpio_bank(gpio, c, pchip) {
 		/* restore level with set/clear */
 		writel_relaxed(c->saved_gplr, c->regbase + GPSR_OFFSET);
 		writel_relaxed(~c->saved_gplr, c->regbase + GPCR_OFFSET);
diff --git a/drivers/gpio/gpio-rc5t583.c b/drivers/gpio/gpio-rc5t583.c
index 6eabf239676b..1e2d210b3369 100644
--- a/drivers/gpio/gpio-rc5t583.c
+++ b/drivers/gpio/gpio-rc5t583.c
@@ -34,14 +34,9 @@ struct rc5t583_gpio {
 	struct rc5t583 *rc5t583;
 };
 
-static inline struct rc5t583_gpio *to_rc5t583_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct rc5t583_gpio, gpio_chip);
-}
-
 static int rc5t583_gpio_get(struct gpio_chip *gc, unsigned int offset)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 	struct device *parent = rc5t583_gpio->rc5t583->dev;
 	uint8_t val = 0;
 	int ret;
@@ -55,7 +50,7 @@ static int rc5t583_gpio_get(struct gpio_chip *gc, unsigned int offset)
 
 static void rc5t583_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 	struct device *parent = rc5t583_gpio->rc5t583->dev;
 	if (val)
 		rc5t583_set_bits(parent, RC5T583_GPIO_IOOUT, BIT(offset));
@@ -65,7 +60,7 @@ static void rc5t583_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 
 static int rc5t583_gpio_dir_input(struct gpio_chip *gc, unsigned int offset)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 	struct device *parent = rc5t583_gpio->rc5t583->dev;
 	int ret;
 
@@ -80,7 +75,7 @@ static int rc5t583_gpio_dir_input(struct gpio_chip *gc, unsigned int offset)
 static int rc5t583_gpio_dir_output(struct gpio_chip *gc, unsigned offset,
 			int value)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 	struct device *parent = rc5t583_gpio->rc5t583->dev;
 	int ret;
 
@@ -95,7 +90,7 @@ static int rc5t583_gpio_dir_output(struct gpio_chip *gc, unsigned offset,
 
 static int rc5t583_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 
 	if (offset < RC5T583_MAX_GPIO)
 		return rc5t583_gpio->rc5t583->irq_base +
@@ -105,7 +100,7 @@ static int rc5t583_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 
 static void rc5t583_gpio_free(struct gpio_chip *gc, unsigned offset)
 {
-	struct rc5t583_gpio *rc5t583_gpio = to_rc5t583_gpio(gc);
+	struct rc5t583_gpio *rc5t583_gpio = gpiochip_get_data(gc);
 	struct device *parent = rc5t583_gpio->rc5t583->dev;
 
 	rc5t583_set_bits(parent, RC5T583_GPIO_PGSEL, BIT(offset));
@@ -132,7 +127,7 @@ static int rc5t583_gpio_probe(struct platform_device *pdev)
 	rc5t583_gpio->gpio_chip.to_irq = rc5t583_gpio_to_irq,
 	rc5t583_gpio->gpio_chip.ngpio = RC5T583_MAX_GPIO,
 	rc5t583_gpio->gpio_chip.can_sleep = true,
-	rc5t583_gpio->gpio_chip.dev = &pdev->dev;
+	rc5t583_gpio->gpio_chip.parent = &pdev->dev;
 	rc5t583_gpio->gpio_chip.base = -1;
 	rc5t583_gpio->rc5t583 = rc5t583;
 
@@ -141,7 +136,7 @@ static int rc5t583_gpio_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, rc5t583_gpio);
 
-	return gpiochip_add(&rc5t583_gpio->gpio_chip);
+	return gpiochip_add_data(&rc5t583_gpio->gpio_chip, rc5t583_gpio);
 }
 
 static int rc5t583_gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-rcar.c b/drivers/gpio/gpio-rcar.c
index 2a8122444614..d9ab0cd1d205 100644
--- a/drivers/gpio/gpio-rcar.c
+++ b/drivers/gpio/gpio-rcar.c
@@ -25,7 +25,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/pinctrl/consumer.h>
-#include <linux/platform_data/gpio-rcar.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/spinlock.h>
@@ -34,12 +33,13 @@
 struct gpio_rcar_priv {
 	void __iomem *base;
 	spinlock_t lock;
-	struct gpio_rcar_config config;
 	struct platform_device *pdev;
 	struct gpio_chip gpio_chip;
 	struct irq_chip irq_chip;
-	unsigned int irq_parent;
 	struct clk *clk;
+	unsigned int irq_parent;
+	bool has_both_edge_trigger;
+	bool needs_clk;
 };
 
 #define IOINTSEL 0x00	/* General IO/Interrupt Switching Register */
@@ -84,8 +84,7 @@ static void gpio_rcar_modify_bit(struct gpio_rcar_priv *p, int offs,
 static void gpio_rcar_irq_disable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct gpio_rcar_priv *p = container_of(gc, struct gpio_rcar_priv,
-						gpio_chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
 
 	gpio_rcar_write(p, INTMSK, ~BIT(irqd_to_hwirq(d)));
 }
@@ -93,8 +92,7 @@ static void gpio_rcar_irq_disable(struct irq_data *d)
 static void gpio_rcar_irq_enable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct gpio_rcar_priv *p = container_of(gc, struct gpio_rcar_priv,
-						gpio_chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
 
 	gpio_rcar_write(p, MSKCLR, BIT(irqd_to_hwirq(d)));
 }
@@ -121,7 +119,7 @@ static void gpio_rcar_config_interrupt_input_mode(struct gpio_rcar_priv *p,
 	gpio_rcar_modify_bit(p, EDGLEVEL, hwirq, !level_trigger);
 
 	/* Select one edge or both edges in BOTHEDGE */
-	if (p->config.has_both_edge_trigger)
+	if (p->has_both_edge_trigger)
 		gpio_rcar_modify_bit(p, BOTHEDGE, hwirq, both);
 
 	/* Select "Interrupt Input Mode" in IOINTSEL */
@@ -137,8 +135,7 @@ static void gpio_rcar_config_interrupt_input_mode(struct gpio_rcar_priv *p,
 static int gpio_rcar_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct gpio_rcar_priv *p = container_of(gc, struct gpio_rcar_priv,
-						gpio_chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
 	unsigned int hwirq = irqd_to_hwirq(d);
 
 	dev_dbg(&p->pdev->dev, "sense irq = %d, type = %d\n", hwirq, type);
@@ -161,7 +158,7 @@ static int gpio_rcar_irq_set_type(struct irq_data *d, unsigned int type)
 						      false);
 		break;
 	case IRQ_TYPE_EDGE_BOTH:
-		if (!p->config.has_both_edge_trigger)
+		if (!p->has_both_edge_trigger)
 			return -EINVAL;
 		gpio_rcar_config_interrupt_input_mode(p, hwirq, true, false,
 						      true);
@@ -175,8 +172,7 @@ static int gpio_rcar_irq_set_type(struct irq_data *d, unsigned int type)
 static int gpio_rcar_irq_set_wake(struct irq_data *d, unsigned int on)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct gpio_rcar_priv *p = container_of(gc, struct gpio_rcar_priv,
-						gpio_chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
 	int error;
 
 	if (p->irq_parent) {
@@ -200,6 +196,44 @@ static int gpio_rcar_irq_set_wake(struct irq_data *d, unsigned int on)
 	return 0;
 }
 
+static void gpio_rcar_irq_bus_lock(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
+
+	pm_runtime_get_sync(&p->pdev->dev);
+}
+
+static void gpio_rcar_irq_bus_sync_unlock(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
+
+	pm_runtime_put(&p->pdev->dev);
+}
+
+
+static int gpio_rcar_irq_request_resources(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
+	int error;
+
+	error = pm_runtime_get_sync(&p->pdev->dev);
+	if (error < 0)
+		return error;
+
+	return 0;
+}
+
+static void gpio_rcar_irq_release_resources(struct irq_data *d)
+{
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct gpio_rcar_priv *p = gpiochip_get_data(gc);
+
+	pm_runtime_put(&p->pdev->dev);
+}
+
 static irqreturn_t gpio_rcar_irq_handler(int irq, void *dev_id)
 {
 	struct gpio_rcar_priv *p = dev_id;
@@ -218,16 +252,11 @@ static irqreturn_t gpio_rcar_irq_handler(int irq, void *dev_id)
 	return irqs_handled ? IRQ_HANDLED : IRQ_NONE;
 }
 
-static inline struct gpio_rcar_priv *gpio_to_priv(struct gpio_chip *chip)
-{
-	return container_of(chip, struct gpio_rcar_priv, gpio_chip);
-}
-
 static void gpio_rcar_config_general_input_output_mode(struct gpio_chip *chip,
 						       unsigned int gpio,
 						       bool output)
 {
-	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	/* follow steps in the GPIO documentation for
@@ -251,7 +280,7 @@ static void gpio_rcar_config_general_input_output_mode(struct gpio_chip *chip,
 
 static int gpio_rcar_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(chip);
 	int error;
 
 	error = pm_runtime_get_sync(&p->pdev->dev);
@@ -267,7 +296,7 @@ static int gpio_rcar_request(struct gpio_chip *chip, unsigned offset)
 
 static void gpio_rcar_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(chip);
 
 	pinctrl_free_gpio(chip->base + offset);
 
@@ -291,15 +320,15 @@ static int gpio_rcar_get(struct gpio_chip *chip, unsigned offset)
 
 	/* testing on r8a7790 shows that INDT does not show correct pin state
 	 * when configured as output, so use OUTDT in case of output pins */
-	if (gpio_rcar_read(gpio_to_priv(chip), INOUTSEL) & bit)
-		return !!(gpio_rcar_read(gpio_to_priv(chip), OUTDT) & bit);
+	if (gpio_rcar_read(gpiochip_get_data(chip), INOUTSEL) & bit)
+		return !!(gpio_rcar_read(gpiochip_get_data(chip), OUTDT) & bit);
 	else
-		return !!(gpio_rcar_read(gpio_to_priv(chip), INDT) & bit);
+		return !!(gpio_rcar_read(gpiochip_get_data(chip), INDT) & bit);
 }
 
 static void gpio_rcar_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct gpio_rcar_priv *p = gpio_to_priv(chip);
+	struct gpio_rcar_priv *p = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&p->lock, flags);
@@ -318,14 +347,17 @@ static int gpio_rcar_direction_output(struct gpio_chip *chip, unsigned offset,
 
 struct gpio_rcar_info {
 	bool has_both_edge_trigger;
+	bool needs_clk;
 };
 
 static const struct gpio_rcar_info gpio_rcar_info_gen1 = {
 	.has_both_edge_trigger = false,
+	.needs_clk = false,
 };
 
 static const struct gpio_rcar_info gpio_rcar_info_gen2 = {
 	.has_both_edge_trigger = true,
+	.needs_clk = true,
 };
 
 static const struct of_device_id gpio_rcar_of_table[] = {
@@ -355,39 +387,30 @@ static const struct of_device_id gpio_rcar_of_table[] = {
 
 MODULE_DEVICE_TABLE(of, gpio_rcar_of_table);
 
-static int gpio_rcar_parse_pdata(struct gpio_rcar_priv *p)
+static int gpio_rcar_parse_dt(struct gpio_rcar_priv *p, unsigned int *npins)
 {
-	struct gpio_rcar_config *pdata = dev_get_platdata(&p->pdev->dev);
 	struct device_node *np = p->pdev->dev.of_node;
+	const struct of_device_id *match;
+	const struct gpio_rcar_info *info;
 	struct of_phandle_args args;
 	int ret;
 
-	if (pdata) {
-		p->config = *pdata;
-	} else if (IS_ENABLED(CONFIG_OF) && np) {
-		const struct of_device_id *match;
-		const struct gpio_rcar_info *info;
-
-		match = of_match_node(gpio_rcar_of_table, np);
-		if (!match)
-			return -EINVAL;
+	match = of_match_node(gpio_rcar_of_table, np);
+	if (!match)
+		return -EINVAL;
 
-		info = match->data;
+	info = match->data;
 
-		ret = of_parse_phandle_with_fixed_args(np, "gpio-ranges", 3, 0,
-						       &args);
-		p->config.number_of_pins = ret == 0 ? args.args[2]
-					 : RCAR_MAX_GPIO_PER_BANK;
-		p->config.gpio_base = -1;
-		p->config.has_both_edge_trigger = info->has_both_edge_trigger;
-	}
+	ret = of_parse_phandle_with_fixed_args(np, "gpio-ranges", 3, 0, &args);
+	*npins = ret == 0 ? args.args[2] : RCAR_MAX_GPIO_PER_BANK;
+	p->has_both_edge_trigger = info->has_both_edge_trigger;
+	p->needs_clk = info->needs_clk;
 
-	if (p->config.number_of_pins == 0 ||
-	    p->config.number_of_pins > RCAR_MAX_GPIO_PER_BANK) {
+	if (*npins == 0 || *npins > RCAR_MAX_GPIO_PER_BANK) {
 		dev_warn(&p->pdev->dev,
-			 "Invalid number of gpio lines %u, using %u\n",
-			 p->config.number_of_pins, RCAR_MAX_GPIO_PER_BANK);
-		p->config.number_of_pins = RCAR_MAX_GPIO_PER_BANK;
+			 "Invalid number of gpio lines %u, using %u\n", *npins,
+			 RCAR_MAX_GPIO_PER_BANK);
+		*npins = RCAR_MAX_GPIO_PER_BANK;
 	}
 
 	return 0;
@@ -401,6 +424,7 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 	struct irq_chip *irq_chip;
 	struct device *dev = &pdev->dev;
 	const char *name = dev_name(dev);
+	unsigned int npins;
 	int ret;
 
 	p = devm_kzalloc(dev, sizeof(*p), GFP_KERNEL);
@@ -410,8 +434,8 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 	p->pdev = pdev;
 	spin_lock_init(&p->lock);
 
-	/* Get device configuration from DT node or platform data. */
-	ret = gpio_rcar_parse_pdata(p);
+	/* Get device configuration from DT node */
+	ret = gpio_rcar_parse_dt(p, &npins);
 	if (ret < 0)
 		return ret;
 
@@ -419,7 +443,11 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 
 	p->clk = devm_clk_get(dev, NULL);
 	if (IS_ERR(p->clk)) {
-		dev_warn(dev, "unable to get clock\n");
+		if (p->needs_clk) {
+			dev_err(dev, "unable to get clock\n");
+			ret = PTR_ERR(p->clk);
+			goto err0;
+		}
 		p->clk = NULL;
 	}
 
@@ -449,10 +477,10 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 	gpio_chip->direction_output = gpio_rcar_direction_output;
 	gpio_chip->set = gpio_rcar_set;
 	gpio_chip->label = name;
-	gpio_chip->dev = dev;
+	gpio_chip->parent = dev;
 	gpio_chip->owner = THIS_MODULE;
-	gpio_chip->base = p->config.gpio_base;
-	gpio_chip->ngpio = p->config.number_of_pins;
+	gpio_chip->base = -1;
+	gpio_chip->ngpio = npins;
 
 	irq_chip = &p->irq_chip;
 	irq_chip->name = name;
@@ -460,16 +488,20 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 	irq_chip->irq_unmask = gpio_rcar_irq_enable;
 	irq_chip->irq_set_type = gpio_rcar_irq_set_type;
 	irq_chip->irq_set_wake = gpio_rcar_irq_set_wake;
+	irq_chip->irq_bus_lock = gpio_rcar_irq_bus_lock;
+	irq_chip->irq_bus_sync_unlock = gpio_rcar_irq_bus_sync_unlock;
+	irq_chip->irq_request_resources = gpio_rcar_irq_request_resources;
+	irq_chip->irq_release_resources = gpio_rcar_irq_release_resources;
 	irq_chip->flags	= IRQCHIP_SET_TYPE_MASKED | IRQCHIP_MASK_ON_SUSPEND;
 
-	ret = gpiochip_add(gpio_chip);
+	ret = gpiochip_add_data(gpio_chip, p);
 	if (ret) {
 		dev_err(dev, "failed to add GPIO controller\n");
 		goto err0;
 	}
 
-	ret = gpiochip_irqchip_add(gpio_chip, irq_chip, p->config.irq_base,
-				   handle_level_irq, IRQ_TYPE_NONE);
+	ret = gpiochip_irqchip_add(gpio_chip, irq_chip, 0, handle_level_irq,
+				   IRQ_TYPE_NONE);
 	if (ret) {
 		dev_err(dev, "cannot add irqchip\n");
 		goto err1;
@@ -483,22 +515,7 @@ static int gpio_rcar_probe(struct platform_device *pdev)
 		goto err1;
 	}
 
-	dev_info(dev, "driving %d GPIOs\n", p->config.number_of_pins);
-
-	/* warn in case of mismatch if irq base is specified */
-	if (p->config.irq_base) {
-		ret = irq_find_mapping(gpio_chip->irqdomain, 0);
-		if (p->config.irq_base != ret)
-			dev_warn(dev, "irq base mismatch (%u/%u)\n",
-				 p->config.irq_base, ret);
-	}
-
-	if (p->config.pctl_name) {
-		ret = gpiochip_add_pin_range(gpio_chip, p->config.pctl_name, 0,
-					     gpio_chip->base, gpio_chip->ngpio);
-		if (ret < 0)
-			dev_warn(dev, "failed to add pin range\n");
-	}
+	dev_info(dev, "driving %d GPIOs\n", npins);
 
 	return 0;
 
diff --git a/drivers/gpio/gpio-rdc321x.c b/drivers/gpio/gpio-rdc321x.c
index d729bc8a554d..96ddee3f464a 100644
--- a/drivers/gpio/gpio-rdc321x.c
+++ b/drivers/gpio/gpio-rdc321x.c
@@ -47,7 +47,7 @@ static int rdc_gpio_get_value(struct gpio_chip *chip, unsigned gpio)
 	u32 value = 0;
 	int reg;
 
-	gpch = container_of(chip, struct rdc321x_gpio, chip);
+	gpch = gpiochip_get_data(chip);
 	reg = gpio < 32 ? gpch->reg1_data_base : gpch->reg2_data_base;
 
 	spin_lock(&gpch->lock);
@@ -65,7 +65,7 @@ static void rdc_gpio_set_value_impl(struct gpio_chip *chip,
 	struct rdc321x_gpio *gpch;
 	int reg = (gpio < 32) ? 0 : 1;
 
-	gpch = container_of(chip, struct rdc321x_gpio, chip);
+	gpch = gpiochip_get_data(chip);
 
 	if (value)
 		gpch->data_reg[reg] |= 1 << (gpio & 0x1f);
@@ -83,7 +83,7 @@ static void rdc_gpio_set_value(struct gpio_chip *chip,
 {
 	struct rdc321x_gpio *gpch;
 
-	gpch = container_of(chip, struct rdc321x_gpio, chip);
+	gpch = gpiochip_get_data(chip);
 	spin_lock(&gpch->lock);
 	rdc_gpio_set_value_impl(chip, gpio, value);
 	spin_unlock(&gpch->lock);
@@ -96,7 +96,7 @@ static int rdc_gpio_config(struct gpio_chip *chip,
 	int err;
 	u32 reg;
 
-	gpch = container_of(chip, struct rdc321x_gpio, chip);
+	gpch = gpiochip_get_data(chip);
 
 	spin_lock(&gpch->lock);
 	err = pci_read_config_dword(gpch->sb_pdev, gpio < 32 ?
@@ -194,7 +194,7 @@ static int rdc321x_gpio_probe(struct platform_device *pdev)
 
 	dev_info(&pdev->dev, "registering %d GPIOs\n",
 					rdc321x_gpio_dev->chip.ngpio);
-	return gpiochip_add(&rdc321x_gpio_dev->chip);
+	return gpiochip_add_data(&rdc321x_gpio_dev->chip, rdc321x_gpio_dev);
 }
 
 static int rdc321x_gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-sa1100.c b/drivers/gpio/gpio-sa1100.c
index 990fa9023e22..0c99e8fb9af3 100644
--- a/drivers/gpio/gpio-sa1100.c
+++ b/drivers/gpio/gpio-sa1100.c
@@ -17,7 +17,7 @@
 
 static int sa1100_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	return GPLR & GPIO_GPIO(offset);
+	return !!(GPLR & GPIO_GPIO(offset));
 }
 
 static void sa1100_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
@@ -238,7 +238,7 @@ void __init sa1100_init_gpio(void)
 	GRER = 0;
 	GEDR = -1;
 
-	gpiochip_add(&sa1100_gpio_chip);
+	gpiochip_add_data(&sa1100_gpio_chip, NULL);
 
 	sa1100_gpio_irqdomain = irq_domain_add_simple(NULL,
 			28, IRQ_GPIO0,
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
deleted file mode 100644
index 7c288ba4dc87..000000000000
--- a/drivers/gpio/gpio-samsung.c
+++ /dev/null
@@ -1,1328 +0,0 @@
-/*
- * Copyright (c) 2009-2011 Samsung Electronics Co., Ltd.
- *		http://www.samsung.com/
- *
- * Copyright 2008 Openmoko, Inc.
- * Copyright 2008 Simtec Electronics
- *      Ben Dooks <ben@simtec.co.uk>
- *      http://armlinux.simtec.co.uk/
- *
- * SAMSUNG - GPIOlib support
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/kernel.h>
-#include <linux/irq.h>
-#include <linux/io.h>
-#include <linux/gpio.h>
-#include <linux/init.h>
-#include <linux/spinlock.h>
-#include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/device.h>
-#include <linux/ioport.h>
-#include <linux/of.h>
-#include <linux/slab.h>
-#include <linux/of_address.h>
-
-#include <asm/irq.h>
-
-#include <mach/map.h>
-#include <mach/regs-gpio.h>
-#include <mach/gpio-samsung.h>
-
-#include <plat/cpu.h>
-#include <plat/gpio-core.h>
-#include <plat/gpio-cfg.h>
-#include <plat/gpio-cfg-helpers.h>
-#include <plat/pm.h>
-
-int samsung_gpio_setpull_updown(struct samsung_gpio_chip *chip,
-				unsigned int off, samsung_gpio_pull_t pull)
-{
-	void __iomem *reg = chip->base + 0x08;
-	int shift = off * 2;
-	u32 pup;
-
-	pup = __raw_readl(reg);
-	pup &= ~(3 << shift);
-	pup |= pull << shift;
-	__raw_writel(pup, reg);
-
-	return 0;
-}
-
-samsung_gpio_pull_t samsung_gpio_getpull_updown(struct samsung_gpio_chip *chip,
-						unsigned int off)
-{
-	void __iomem *reg = chip->base + 0x08;
-	int shift = off * 2;
-	u32 pup = __raw_readl(reg);
-
-	pup >>= shift;
-	pup &= 0x3;
-
-	return (__force samsung_gpio_pull_t)pup;
-}
-
-int s3c2443_gpio_setpull(struct samsung_gpio_chip *chip,
-			 unsigned int off, samsung_gpio_pull_t pull)
-{
-	switch (pull) {
-	case S3C_GPIO_PULL_NONE:
-		pull = 0x01;
-		break;
-	case S3C_GPIO_PULL_UP:
-		pull = 0x00;
-		break;
-	case S3C_GPIO_PULL_DOWN:
-		pull = 0x02;
-		break;
-	}
-	return samsung_gpio_setpull_updown(chip, off, pull);
-}
-
-samsung_gpio_pull_t s3c2443_gpio_getpull(struct samsung_gpio_chip *chip,
-					 unsigned int off)
-{
-	samsung_gpio_pull_t pull;
-
-	pull = samsung_gpio_getpull_updown(chip, off);
-
-	switch (pull) {
-	case 0x00:
-		pull = S3C_GPIO_PULL_UP;
-		break;
-	case 0x01:
-	case 0x03:
-		pull = S3C_GPIO_PULL_NONE;
-		break;
-	case 0x02:
-		pull = S3C_GPIO_PULL_DOWN;
-		break;
-	}
-
-	return pull;
-}
-
-static int s3c24xx_gpio_setpull_1(struct samsung_gpio_chip *chip,
-				  unsigned int off, samsung_gpio_pull_t pull,
-				  samsung_gpio_pull_t updown)
-{
-	void __iomem *reg = chip->base + 0x08;
-	u32 pup = __raw_readl(reg);
-
-	if (pull == updown)
-		pup &= ~(1 << off);
-	else if (pull == S3C_GPIO_PULL_NONE)
-		pup |= (1 << off);
-	else
-		return -EINVAL;
-
-	__raw_writel(pup, reg);
-	return 0;
-}
-
-static samsung_gpio_pull_t s3c24xx_gpio_getpull_1(struct samsung_gpio_chip *chip,
-						  unsigned int off,
-						  samsung_gpio_pull_t updown)
-{
-	void __iomem *reg = chip->base + 0x08;
-	u32 pup = __raw_readl(reg);
-
-	pup &= (1 << off);
-	return pup ? S3C_GPIO_PULL_NONE : updown;
-}
-
-samsung_gpio_pull_t s3c24xx_gpio_getpull_1up(struct samsung_gpio_chip *chip,
-					     unsigned int off)
-{
-	return s3c24xx_gpio_getpull_1(chip, off, S3C_GPIO_PULL_UP);
-}
-
-int s3c24xx_gpio_setpull_1up(struct samsung_gpio_chip *chip,
-			     unsigned int off, samsung_gpio_pull_t pull)
-{
-	return s3c24xx_gpio_setpull_1(chip, off, pull, S3C_GPIO_PULL_UP);
-}
-
-samsung_gpio_pull_t s3c24xx_gpio_getpull_1down(struct samsung_gpio_chip *chip,
-					       unsigned int off)
-{
-	return s3c24xx_gpio_getpull_1(chip, off, S3C_GPIO_PULL_DOWN);
-}
-
-int s3c24xx_gpio_setpull_1down(struct samsung_gpio_chip *chip,
-			       unsigned int off, samsung_gpio_pull_t pull)
-{
-	return s3c24xx_gpio_setpull_1(chip, off, pull, S3C_GPIO_PULL_DOWN);
-}
-
-/*
- * samsung_gpio_setcfg_2bit - Samsung 2bit style GPIO configuration.
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- * @cfg: The configuration value to set.
- *
- * This helper deal with the GPIO cases where the control register
- * has two bits of configuration per gpio, which have the following
- * functions:
- *	00 = input
- *	01 = output
- *	1x = special function
- */
-
-static int samsung_gpio_setcfg_2bit(struct samsung_gpio_chip *chip,
-				    unsigned int off, unsigned int cfg)
-{
-	void __iomem *reg = chip->base;
-	unsigned int shift = off * 2;
-	u32 con;
-
-	if (samsung_gpio_is_cfg_special(cfg)) {
-		cfg &= 0xf;
-		if (cfg > 3)
-			return -EINVAL;
-
-		cfg <<= shift;
-	}
-
-	con = __raw_readl(reg);
-	con &= ~(0x3 << shift);
-	con |= cfg;
-	__raw_writel(con, reg);
-
-	return 0;
-}
-
-/*
- * samsung_gpio_getcfg_2bit - Samsung 2bit style GPIO configuration read.
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- *
- * The reverse of samsung_gpio_setcfg_2bit(). Will return a value which
- * could be directly passed back to samsung_gpio_setcfg_2bit(), from the
- * S3C_GPIO_SPECIAL() macro.
- */
-
-static unsigned int samsung_gpio_getcfg_2bit(struct samsung_gpio_chip *chip,
-					     unsigned int off)
-{
-	u32 con;
-
-	con = __raw_readl(chip->base);
-	con >>= off * 2;
-	con &= 3;
-
-	/* this conversion works for IN and OUT as well as special mode */
-	return S3C_GPIO_SPECIAL(con);
-}
-
-/*
- * samsung_gpio_setcfg_4bit - Samsung 4bit single register GPIO config.
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- * @cfg: The configuration value to set.
- *
- * This helper deal with the GPIO cases where the control register has 4 bits
- * of control per GPIO, generally in the form of:
- *	0000 = Input
- *	0001 = Output
- *	others = Special functions (dependent on bank)
- *
- * Note, since the code to deal with the case where there are two control
- * registers instead of one, we do not have a separate set of functions for
- * each case.
- */
-
-static int samsung_gpio_setcfg_4bit(struct samsung_gpio_chip *chip,
-				    unsigned int off, unsigned int cfg)
-{
-	void __iomem *reg = chip->base;
-	unsigned int shift = (off & 7) * 4;
-	u32 con;
-
-	if (off < 8 && chip->chip.ngpio > 8)
-		reg -= 4;
-
-	if (samsung_gpio_is_cfg_special(cfg)) {
-		cfg &= 0xf;
-		cfg <<= shift;
-	}
-
-	con = __raw_readl(reg);
-	con &= ~(0xf << shift);
-	con |= cfg;
-	__raw_writel(con, reg);
-
-	return 0;
-}
-
-/*
- * samsung_gpio_getcfg_4bit - Samsung 4bit single register GPIO config read.
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- *
- * The reverse of samsung_gpio_setcfg_4bit(), turning a gpio configuration
- * register setting into a value the software can use, such as could be passed
- * to samsung_gpio_setcfg_4bit().
- *
- * @sa samsung_gpio_getcfg_2bit
- */
-
-static unsigned samsung_gpio_getcfg_4bit(struct samsung_gpio_chip *chip,
-					 unsigned int off)
-{
-	void __iomem *reg = chip->base;
-	unsigned int shift = (off & 7) * 4;
-	u32 con;
-
-	if (off < 8 && chip->chip.ngpio > 8)
-		reg -= 4;
-
-	con = __raw_readl(reg);
-	con >>= shift;
-	con &= 0xf;
-
-	/* this conversion works for IN and OUT as well as special mode */
-	return S3C_GPIO_SPECIAL(con);
-}
-
-#ifdef CONFIG_PLAT_S3C24XX
-/*
- * s3c24xx_gpio_setcfg_abank - S3C24XX style GPIO configuration (Bank A)
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- * @cfg: The configuration value to set.
- *
- * This helper deal with the GPIO cases where the control register
- * has one bit of configuration for the gpio, where setting the bit
- * means the pin is in special function mode and unset means output.
- */
-
-static int s3c24xx_gpio_setcfg_abank(struct samsung_gpio_chip *chip,
-				     unsigned int off, unsigned int cfg)
-{
-	void __iomem *reg = chip->base;
-	unsigned int shift = off;
-	u32 con;
-
-	if (samsung_gpio_is_cfg_special(cfg)) {
-		cfg &= 0xf;
-
-		/* Map output to 0, and SFN2 to 1 */
-		cfg -= 1;
-		if (cfg > 1)
-			return -EINVAL;
-
-		cfg <<= shift;
-	}
-
-	con = __raw_readl(reg);
-	con &= ~(0x1 << shift);
-	con |= cfg;
-	__raw_writel(con, reg);
-
-	return 0;
-}
-
-/*
- * s3c24xx_gpio_getcfg_abank - S3C24XX style GPIO configuration read (Bank A)
- * @chip: The gpio chip that is being configured.
- * @off: The offset for the GPIO being configured.
- *
- * The reverse of s3c24xx_gpio_setcfg_abank() turning an GPIO into a usable
- * GPIO configuration value.
- *
- * @sa samsung_gpio_getcfg_2bit
- * @sa samsung_gpio_getcfg_4bit
- */
-
-static unsigned s3c24xx_gpio_getcfg_abank(struct samsung_gpio_chip *chip,
-					  unsigned int off)
-{
-	u32 con;
-
-	con = __raw_readl(chip->base);
-	con >>= off;
-	con &= 1;
-	con++;
-
-	return S3C_GPIO_SFN(con);
-}
-#endif
-
-static void __init samsung_gpiolib_set_cfg(struct samsung_gpio_cfg *chipcfg,
-					   int nr_chips)
-{
-	for (; nr_chips > 0; nr_chips--, chipcfg++) {
-		if (!chipcfg->set_config)
-			chipcfg->set_config = samsung_gpio_setcfg_4bit;
-		if (!chipcfg->get_config)
-			chipcfg->get_config = samsung_gpio_getcfg_4bit;
-		if (!chipcfg->set_pull)
-			chipcfg->set_pull = samsung_gpio_setpull_updown;
-		if (!chipcfg->get_pull)
-			chipcfg->get_pull = samsung_gpio_getpull_updown;
-	}
-}
-
-struct samsung_gpio_cfg s3c24xx_gpiocfg_default = {
-	.set_config	= samsung_gpio_setcfg_2bit,
-	.get_config	= samsung_gpio_getcfg_2bit,
-};
-
-#ifdef CONFIG_PLAT_S3C24XX
-static struct samsung_gpio_cfg s3c24xx_gpiocfg_banka = {
-	.set_config	= s3c24xx_gpio_setcfg_abank,
-	.get_config	= s3c24xx_gpio_getcfg_abank,
-};
-#endif
-
-static struct samsung_gpio_cfg samsung_gpio_cfgs[] = {
-	[0] = {
-		.cfg_eint	= 0x0,
-	},
-	[1] = {
-		.cfg_eint	= 0x3,
-	},
-	[2] = {
-		.cfg_eint	= 0x7,
-	},
-	[3] = {
-		.cfg_eint	= 0xF,
-	},
-	[4] = {
-		.cfg_eint	= 0x0,
-		.set_config	= samsung_gpio_setcfg_2bit,
-		.get_config	= samsung_gpio_getcfg_2bit,
-	},
-	[5] = {
-		.cfg_eint	= 0x2,
-		.set_config	= samsung_gpio_setcfg_2bit,
-		.get_config	= samsung_gpio_getcfg_2bit,
-	},
-	[6] = {
-		.cfg_eint	= 0x3,
-		.set_config	= samsung_gpio_setcfg_2bit,
-		.get_config	= samsung_gpio_getcfg_2bit,
-	},
-	[7] = {
-		.set_config	= samsung_gpio_setcfg_2bit,
-		.get_config	= samsung_gpio_getcfg_2bit,
-	},
-};
-
-/*
- * Default routines for controlling GPIO, based on the original S3C24XX
- * GPIO functions which deal with the case where each gpio bank of the
- * chip is as following:
- *
- * base + 0x00: Control register, 2 bits per gpio
- *	        gpio n: 2 bits starting at (2*n)
- *		00 = input, 01 = output, others mean special-function
- * base + 0x04: Data register, 1 bit per gpio
- *		bit n: data bit n
-*/
-
-static int samsung_gpiolib_2bit_input(struct gpio_chip *chip, unsigned offset)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long flags;
-	unsigned long con;
-
-	samsung_gpio_lock(ourchip, flags);
-
-	con = __raw_readl(base + 0x00);
-	con &= ~(3 << (offset * 2));
-
-	__raw_writel(con, base + 0x00);
-
-	samsung_gpio_unlock(ourchip, flags);
-	return 0;
-}
-
-static int samsung_gpiolib_2bit_output(struct gpio_chip *chip,
-				       unsigned offset, int value)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long flags;
-	unsigned long dat;
-	unsigned long con;
-
-	samsung_gpio_lock(ourchip, flags);
-
-	dat = __raw_readl(base + 0x04);
-	dat &= ~(1 << offset);
-	if (value)
-		dat |= 1 << offset;
-	__raw_writel(dat, base + 0x04);
-
-	con = __raw_readl(base + 0x00);
-	con &= ~(3 << (offset * 2));
-	con |= 1 << (offset * 2);
-
-	__raw_writel(con, base + 0x00);
-	__raw_writel(dat, base + 0x04);
-
-	samsung_gpio_unlock(ourchip, flags);
-	return 0;
-}
-
-/*
- * The samsung_gpiolib_4bit routines are to control the gpio banks where
- * the gpio configuration register (GPxCON) has 4 bits per GPIO, as the
- * following example:
- *
- * base + 0x00: Control register, 4 bits per gpio
- *		gpio n: 4 bits starting at (4*n)
- *		0000 = input, 0001 = output, others mean special-function
- * base + 0x04: Data register, 1 bit per gpio
- *		bit n: data bit n
- *
- * Note, since the data register is one bit per gpio and is at base + 0x4
- * we can use samsung_gpiolib_get and samsung_gpiolib_set to change the
- * state of the output.
- */
-
-static int samsung_gpiolib_4bit_input(struct gpio_chip *chip,
-				      unsigned int offset)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long con;
-
-	con = __raw_readl(base + GPIOCON_OFF);
-	if (ourchip->bitmap_gpio_int & BIT(offset))
-		con |= 0xf << con_4bit_shift(offset);
-	else
-		con &= ~(0xf << con_4bit_shift(offset));
-	__raw_writel(con, base + GPIOCON_OFF);
-
-	pr_debug("%s: %p: CON now %08lx\n", __func__, base, con);
-
-	return 0;
-}
-
-static int samsung_gpiolib_4bit_output(struct gpio_chip *chip,
-				       unsigned int offset, int value)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long con;
-	unsigned long dat;
-
-	con = __raw_readl(base + GPIOCON_OFF);
-	con &= ~(0xf << con_4bit_shift(offset));
-	con |= 0x1 << con_4bit_shift(offset);
-
-	dat = __raw_readl(base + GPIODAT_OFF);
-
-	if (value)
-		dat |= 1 << offset;
-	else
-		dat &= ~(1 << offset);
-
-	__raw_writel(dat, base + GPIODAT_OFF);
-	__raw_writel(con, base + GPIOCON_OFF);
-	__raw_writel(dat, base + GPIODAT_OFF);
-
-	pr_debug("%s: %p: CON %08lx, DAT %08lx\n", __func__, base, con, dat);
-
-	return 0;
-}
-
-/*
- * The next set of routines are for the case where the GPIO configuration
- * registers are 4 bits per GPIO but there is more than one register (the
- * bank has more than 8 GPIOs.
- *
- * This case is the similar to the 4 bit case, but the registers are as
- * follows:
- *
- * base + 0x00: Control register, 4 bits per gpio (lower 8 GPIOs)
- *		gpio n: 4 bits starting at (4*n)
- *		0000 = input, 0001 = output, others mean special-function
- * base + 0x04: Control register, 4 bits per gpio (up to 8 additions GPIOs)
- *		gpio n: 4 bits starting at (4*n)
- *		0000 = input, 0001 = output, others mean special-function
- * base + 0x08: Data register, 1 bit per gpio
- *		bit n: data bit n
- *
- * To allow us to use the samsung_gpiolib_get and samsung_gpiolib_set
- * routines we store the 'base + 0x4' address so that these routines see
- * the data register at ourchip->base + 0x04.
- */
-
-static int samsung_gpiolib_4bit2_input(struct gpio_chip *chip,
-				       unsigned int offset)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	void __iomem *regcon = base;
-	unsigned long con;
-
-	if (offset > 7)
-		offset -= 8;
-	else
-		regcon -= 4;
-
-	con = __raw_readl(regcon);
-	con &= ~(0xf << con_4bit_shift(offset));
-	__raw_writel(con, regcon);
-
-	pr_debug("%s: %p: CON %08lx\n", __func__, base, con);
-
-	return 0;
-}
-
-static int samsung_gpiolib_4bit2_output(struct gpio_chip *chip,
-					unsigned int offset, int value)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	void __iomem *regcon = base;
-	unsigned long con;
-	unsigned long dat;
-	unsigned con_offset = offset;
-
-	if (con_offset > 7)
-		con_offset -= 8;
-	else
-		regcon -= 4;
-
-	con = __raw_readl(regcon);
-	con &= ~(0xf << con_4bit_shift(con_offset));
-	con |= 0x1 << con_4bit_shift(con_offset);
-
-	dat = __raw_readl(base + GPIODAT_OFF);
-
-	if (value)
-		dat |= 1 << offset;
-	else
-		dat &= ~(1 << offset);
-
-	__raw_writel(dat, base + GPIODAT_OFF);
-	__raw_writel(con, regcon);
-	__raw_writel(dat, base + GPIODAT_OFF);
-
-	pr_debug("%s: %p: CON %08lx, DAT %08lx\n", __func__, base, con, dat);
-
-	return 0;
-}
-
-#ifdef CONFIG_PLAT_S3C24XX
-/* The next set of routines are for the case of s3c24xx bank a */
-
-static int s3c24xx_gpiolib_banka_input(struct gpio_chip *chip, unsigned offset)
-{
-	return -EINVAL;
-}
-
-static int s3c24xx_gpiolib_banka_output(struct gpio_chip *chip,
-					unsigned offset, int value)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long flags;
-	unsigned long dat;
-	unsigned long con;
-
-	local_irq_save(flags);
-
-	con = __raw_readl(base + 0x00);
-	dat = __raw_readl(base + 0x04);
-
-	dat &= ~(1 << offset);
-	if (value)
-		dat |= 1 << offset;
-
-	__raw_writel(dat, base + 0x04);
-
-	con &= ~(1 << offset);
-
-	__raw_writel(con, base + 0x00);
-	__raw_writel(dat, base + 0x04);
-
-	local_irq_restore(flags);
-	return 0;
-}
-#endif
-
-static void samsung_gpiolib_set(struct gpio_chip *chip,
-				unsigned offset, int value)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	void __iomem *base = ourchip->base;
-	unsigned long flags;
-	unsigned long dat;
-
-	samsung_gpio_lock(ourchip, flags);
-
-	dat = __raw_readl(base + 0x04);
-	dat &= ~(1 << offset);
-	if (value)
-		dat |= 1 << offset;
-	__raw_writel(dat, base + 0x04);
-
-	samsung_gpio_unlock(ourchip, flags);
-}
-
-static int samsung_gpiolib_get(struct gpio_chip *chip, unsigned offset)
-{
-	struct samsung_gpio_chip *ourchip = to_samsung_gpio(chip);
-	unsigned long val;
-
-	val = __raw_readl(ourchip->base + 0x04);
-	val >>= offset;
-	val &= 1;
-
-	return val;
-}
-
-/*
- * CONFIG_S3C_GPIO_TRACK enables the tracking of the s3c specific gpios
- * for use with the configuration calls, and other parts of the s3c gpiolib
- * support code.
- *
- * Not all s3c support code will need this, as some configurations of cpu
- * may only support one or two different configuration options and have an
- * easy gpio to samsung_gpio_chip mapping function. If this is the case, then
- * the machine support file should provide its own samsung_gpiolib_getchip()
- * and any other necessary functions.
- */
-
-#ifdef CONFIG_S3C_GPIO_TRACK
-struct samsung_gpio_chip *s3c_gpios[S3C_GPIO_END];
-
-static __init void s3c_gpiolib_track(struct samsung_gpio_chip *chip)
-{
-	unsigned int gpn;
-	int i;
-
-	gpn = chip->chip.base;
-	for (i = 0; i < chip->chip.ngpio; i++, gpn++) {
-		BUG_ON(gpn >= ARRAY_SIZE(s3c_gpios));
-		s3c_gpios[gpn] = chip;
-	}
-}
-#endif /* CONFIG_S3C_GPIO_TRACK */
-
-/*
- * samsung_gpiolib_add() - add the Samsung gpio_chip.
- * @chip: The chip to register
- *
- * This is a wrapper to gpiochip_add() that takes our specific gpio chip
- * information and makes the necessary alterations for the platform and
- * notes the information for use with the configuration systems and any
- * other parts of the system.
- */
-
-static void __init samsung_gpiolib_add(struct samsung_gpio_chip *chip)
-{
-	struct gpio_chip *gc = &chip->chip;
-	int ret;
-
-	BUG_ON(!chip->base);
-	BUG_ON(!gc->label);
-	BUG_ON(!gc->ngpio);
-
-	spin_lock_init(&chip->lock);
-
-	if (!gc->direction_input)
-		gc->direction_input = samsung_gpiolib_2bit_input;
-	if (!gc->direction_output)
-		gc->direction_output = samsung_gpiolib_2bit_output;
-	if (!gc->set)
-		gc->set = samsung_gpiolib_set;
-	if (!gc->get)
-		gc->get = samsung_gpiolib_get;
-
-#ifdef CONFIG_PM
-	if (chip->pm != NULL) {
-		if (!chip->pm->save || !chip->pm->resume)
-			pr_err("gpio: %s has missing PM functions\n",
-			       gc->label);
-	} else
-		pr_err("gpio: %s has no PM function\n", gc->label);
-#endif
-
-	/* gpiochip_add() prints own failure message on error. */
-	ret = gpiochip_add(gc);
-	if (ret >= 0)
-		s3c_gpiolib_track(chip);
-}
-
-static void __init s3c24xx_gpiolib_add_chips(struct samsung_gpio_chip *chip,
-					     int nr_chips, void __iomem *base)
-{
-	int i;
-	struct gpio_chip *gc = &chip->chip;
-
-	for (i = 0 ; i < nr_chips; i++, chip++) {
-		/* skip banks not present on SoC */
-		if (chip->chip.base >= S3C_GPIO_END)
-			continue;
-
-		if (!chip->config)
-			chip->config = &s3c24xx_gpiocfg_default;
-		if (!chip->pm)
-			chip->pm = __gpio_pm(&samsung_gpio_pm_2bit);
-		if ((base != NULL) && (chip->base == NULL))
-			chip->base = base + ((i) * 0x10);
-
-		if (!gc->direction_input)
-			gc->direction_input = samsung_gpiolib_2bit_input;
-		if (!gc->direction_output)
-			gc->direction_output = samsung_gpiolib_2bit_output;
-
-		samsung_gpiolib_add(chip);
-	}
-}
-
-static void __init samsung_gpiolib_add_2bit_chips(struct samsung_gpio_chip *chip,
-						  int nr_chips, void __iomem *base,
-						  unsigned int offset)
-{
-	int i;
-
-	for (i = 0 ; i < nr_chips; i++, chip++) {
-		chip->chip.direction_input = samsung_gpiolib_2bit_input;
-		chip->chip.direction_output = samsung_gpiolib_2bit_output;
-
-		if (!chip->config)
-			chip->config = &samsung_gpio_cfgs[7];
-		if (!chip->pm)
-			chip->pm = __gpio_pm(&samsung_gpio_pm_2bit);
-		if ((base != NULL) && (chip->base == NULL))
-			chip->base = base + ((i) * offset);
-
-		samsung_gpiolib_add(chip);
-	}
-}
-
-/*
- * samsung_gpiolib_add_4bit_chips - 4bit single register GPIO config.
- * @chip: The gpio chip that is being configured.
- * @nr_chips: The no of chips (gpio ports) for the GPIO being configured.
- *
- * This helper deal with the GPIO cases where the control register has 4 bits
- * of control per GPIO, generally in the form of:
- * 0000 = Input
- * 0001 = Output
- * others = Special functions (dependent on bank)
- *
- * Note, since the code to deal with the case where there are two control
- * registers instead of one, we do not have a separate set of function
- * (samsung_gpiolib_add_4bit2_chips)for each case.
- */
-
-static void __init samsung_gpiolib_add_4bit_chips(struct samsung_gpio_chip *chip,
-						  int nr_chips, void __iomem *base)
-{
-	int i;
-
-	for (i = 0 ; i < nr_chips; i++, chip++) {
-		chip->chip.direction_input = samsung_gpiolib_4bit_input;
-		chip->chip.direction_output = samsung_gpiolib_4bit_output;
-
-		if (!chip->config)
-			chip->config = &samsung_gpio_cfgs[2];
-		if (!chip->pm)
-			chip->pm = __gpio_pm(&samsung_gpio_pm_4bit);
-		if ((base != NULL) && (chip->base == NULL))
-			chip->base = base + ((i) * 0x20);
-
-		chip->bitmap_gpio_int = 0;
-
-		samsung_gpiolib_add(chip);
-	}
-}
-
-static void __init samsung_gpiolib_add_4bit2_chips(struct samsung_gpio_chip *chip,
-						   int nr_chips)
-{
-	for (; nr_chips > 0; nr_chips--, chip++) {
-		chip->chip.direction_input = samsung_gpiolib_4bit2_input;
-		chip->chip.direction_output = samsung_gpiolib_4bit2_output;
-
-		if (!chip->config)
-			chip->config = &samsung_gpio_cfgs[2];
-		if (!chip->pm)
-			chip->pm = __gpio_pm(&samsung_gpio_pm_4bit);
-
-		samsung_gpiolib_add(chip);
-	}
-}
-
-int samsung_gpiolib_to_irq(struct gpio_chip *chip, unsigned int offset)
-{
-	struct samsung_gpio_chip *samsung_chip = container_of(chip, struct samsung_gpio_chip, chip);
-
-	return samsung_chip->irq_base + offset;
-}
-
-#ifdef CONFIG_PLAT_S3C24XX
-static int s3c24xx_gpiolib_fbank_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	if (offset < 4) {
-		if (soc_is_s3c2412())
-			return IRQ_EINT0_2412 + offset;
-		else
-			return IRQ_EINT0 + offset;
-	}
-
-	if (offset < 8)
-		return IRQ_EINT4 + offset - 4;
-
-	return -EINVAL;
-}
-#endif
-
-#ifdef CONFIG_ARCH_S3C64XX
-static int s3c64xx_gpiolib_mbank_to_irq(struct gpio_chip *chip, unsigned pin)
-{
-	return pin < 5 ? IRQ_EINT(23) + pin : -ENXIO;
-}
-
-static int s3c64xx_gpiolib_lbank_to_irq(struct gpio_chip *chip, unsigned pin)
-{
-	return pin >= 8 ? IRQ_EINT(16) + pin - 8 : -ENXIO;
-}
-#endif
-
-struct samsung_gpio_chip s3c24xx_gpios[] = {
-#ifdef CONFIG_PLAT_S3C24XX
-	{
-		.config	= &s3c24xx_gpiocfg_banka,
-		.chip	= {
-			.base			= S3C2410_GPA(0),
-			.owner			= THIS_MODULE,
-			.label			= "GPIOA",
-			.ngpio			= 27,
-			.direction_input	= s3c24xx_gpiolib_banka_input,
-			.direction_output	= s3c24xx_gpiolib_banka_output,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPB(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOB",
-			.ngpio	= 11,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPC(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOC",
-			.ngpio	= 16,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPD(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOD",
-			.ngpio	= 16,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPE(0),
-			.label	= "GPIOE",
-			.owner	= THIS_MODULE,
-			.ngpio	= 16,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPF(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOF",
-			.ngpio	= 8,
-			.to_irq	= s3c24xx_gpiolib_fbank_to_irq,
-		},
-	}, {
-		.irq_base = IRQ_EINT8,
-		.chip	= {
-			.base	= S3C2410_GPG(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOG",
-			.ngpio	= 16,
-			.to_irq	= samsung_gpiolib_to_irq,
-		},
-	}, {
-		.chip	= {
-			.base	= S3C2410_GPH(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOH",
-			.ngpio	= 15,
-		},
-	},
-		/* GPIOS for the S3C2443 and later devices. */
-	{
-		.base	= S3C2440_GPJCON,
-		.chip	= {
-			.base	= S3C2410_GPJ(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOJ",
-			.ngpio	= 16,
-		},
-	}, {
-		.base	= S3C2443_GPKCON,
-		.chip	= {
-			.base	= S3C2410_GPK(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOK",
-			.ngpio	= 16,
-		},
-	}, {
-		.base	= S3C2443_GPLCON,
-		.chip	= {
-			.base	= S3C2410_GPL(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOL",
-			.ngpio	= 15,
-		},
-	}, {
-		.base	= S3C2443_GPMCON,
-		.chip	= {
-			.base	= S3C2410_GPM(0),
-			.owner	= THIS_MODULE,
-			.label	= "GPIOM",
-			.ngpio	= 2,
-		},
-	},
-#endif
-};
-
-/*
- * GPIO bank summary:
- *
- * Bank	GPIOs	Style	SlpCon	ExtInt Group
- * A	8	4Bit	Yes	1
- * B	7	4Bit	Yes	1
- * C	8	4Bit	Yes	2
- * D	5	4Bit	Yes	3
- * E	5	4Bit	Yes	None
- * F	16	2Bit	Yes	4 [1]
- * G	7	4Bit	Yes	5
- * H	10	4Bit[2]	Yes	6
- * I	16	2Bit	Yes	None
- * J	12	2Bit	Yes	None
- * K	16	4Bit[2]	No	None
- * L	15	4Bit[2] No	None
- * M	6	4Bit	No	IRQ_EINT
- * N	16	2Bit	No	IRQ_EINT
- * O	16	2Bit	Yes	7
- * P	15	2Bit	Yes	8
- * Q	9	2Bit	Yes	9
- *
- * [1] BANKF pins 14,15 do not form part of the external interrupt sources
- * [2] BANK has two control registers, GPxCON0 and GPxCON1
- */
-
-static struct samsung_gpio_chip s3c64xx_gpios_4bit[] = {
-#ifdef CONFIG_ARCH_S3C64XX
-	{
-		.chip	= {
-			.base	= S3C64XX_GPA(0),
-			.ngpio	= S3C64XX_GPIO_A_NR,
-			.label	= "GPA",
-		},
-	}, {
-		.chip	= {
-			.base	= S3C64XX_GPB(0),
-			.ngpio	= S3C64XX_GPIO_B_NR,
-			.label	= "GPB",
-		},
-	}, {
-		.chip	= {
-			.base	= S3C64XX_GPC(0),
-			.ngpio	= S3C64XX_GPIO_C_NR,
-			.label	= "GPC",
-		},
-	}, {
-		.chip	= {
-			.base	= S3C64XX_GPD(0),
-			.ngpio	= S3C64XX_GPIO_D_NR,
-			.label	= "GPD",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[0],
-		.chip	= {
-			.base	= S3C64XX_GPE(0),
-			.ngpio	= S3C64XX_GPIO_E_NR,
-			.label	= "GPE",
-		},
-	}, {
-		.base	= S3C64XX_GPG_BASE,
-		.chip	= {
-			.base	= S3C64XX_GPG(0),
-			.ngpio	= S3C64XX_GPIO_G_NR,
-			.label	= "GPG",
-		},
-	}, {
-		.base	= S3C64XX_GPM_BASE,
-		.config	= &samsung_gpio_cfgs[1],
-		.chip	= {
-			.base	= S3C64XX_GPM(0),
-			.ngpio	= S3C64XX_GPIO_M_NR,
-			.label	= "GPM",
-			.to_irq = s3c64xx_gpiolib_mbank_to_irq,
-		},
-	},
-#endif
-};
-
-static struct samsung_gpio_chip s3c64xx_gpios_4bit2[] = {
-#ifdef CONFIG_ARCH_S3C64XX
-	{
-		.base	= S3C64XX_GPH_BASE + 0x4,
-		.chip	= {
-			.base	= S3C64XX_GPH(0),
-			.ngpio	= S3C64XX_GPIO_H_NR,
-			.label	= "GPH",
-		},
-	}, {
-		.base	= S3C64XX_GPK_BASE + 0x4,
-		.config	= &samsung_gpio_cfgs[0],
-		.chip	= {
-			.base	= S3C64XX_GPK(0),
-			.ngpio	= S3C64XX_GPIO_K_NR,
-			.label	= "GPK",
-		},
-	}, {
-		.base	= S3C64XX_GPL_BASE + 0x4,
-		.config	= &samsung_gpio_cfgs[1],
-		.chip	= {
-			.base	= S3C64XX_GPL(0),
-			.ngpio	= S3C64XX_GPIO_L_NR,
-			.label	= "GPL",
-			.to_irq = s3c64xx_gpiolib_lbank_to_irq,
-		},
-	},
-#endif
-};
-
-static struct samsung_gpio_chip s3c64xx_gpios_2bit[] = {
-#ifdef CONFIG_ARCH_S3C64XX
-	{
-		.base	= S3C64XX_GPF_BASE,
-		.config	= &samsung_gpio_cfgs[6],
-		.chip	= {
-			.base	= S3C64XX_GPF(0),
-			.ngpio	= S3C64XX_GPIO_F_NR,
-			.label	= "GPF",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[7],
-		.chip	= {
-			.base	= S3C64XX_GPI(0),
-			.ngpio	= S3C64XX_GPIO_I_NR,
-			.label	= "GPI",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[7],
-		.chip	= {
-			.base	= S3C64XX_GPJ(0),
-			.ngpio	= S3C64XX_GPIO_J_NR,
-			.label	= "GPJ",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[6],
-		.chip	= {
-			.base	= S3C64XX_GPO(0),
-			.ngpio	= S3C64XX_GPIO_O_NR,
-			.label	= "GPO",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[6],
-		.chip	= {
-			.base	= S3C64XX_GPP(0),
-			.ngpio	= S3C64XX_GPIO_P_NR,
-			.label	= "GPP",
-		},
-	}, {
-		.config	= &samsung_gpio_cfgs[6],
-		.chip	= {
-			.base	= S3C64XX_GPQ(0),
-			.ngpio	= S3C64XX_GPIO_Q_NR,
-			.label	= "GPQ",
-		},
-	}, {
-		.base	= S3C64XX_GPN_BASE,
-		.irq_base = IRQ_EINT(0),
-		.config	= &samsung_gpio_cfgs[5],
-		.chip	= {
-			.base	= S3C64XX_GPN(0),
-			.ngpio	= S3C64XX_GPIO_N_NR,
-			.label	= "GPN",
-			.to_irq = samsung_gpiolib_to_irq,
-		},
-	},
-#endif
-};
-
-/* TODO: cleanup soc_is_* */
-static __init int samsung_gpiolib_init(void)
-{
-	/*
-	 * Currently there are two drivers that can provide GPIO support for
-	 * Samsung SoCs. For device tree enabled platforms, the new
-	 * pinctrl-samsung driver is used, providing both GPIO and pin control
-	 * interfaces. For legacy (non-DT) platforms this driver is used.
-	 */
-	if (of_have_populated_dt())
-		return -ENODEV;
-
-	samsung_gpiolib_set_cfg(samsung_gpio_cfgs, ARRAY_SIZE(samsung_gpio_cfgs));
-
-	if (soc_is_s3c24xx()) {
-		s3c24xx_gpiolib_add_chips(s3c24xx_gpios,
-				ARRAY_SIZE(s3c24xx_gpios), S3C24XX_VA_GPIO);
-	} else if (soc_is_s3c64xx()) {
-		samsung_gpiolib_add_2bit_chips(s3c64xx_gpios_2bit,
-				ARRAY_SIZE(s3c64xx_gpios_2bit),
-				S3C64XX_VA_GPIO + 0xE0, 0x20);
-		samsung_gpiolib_add_4bit_chips(s3c64xx_gpios_4bit,
-				ARRAY_SIZE(s3c64xx_gpios_4bit),
-				S3C64XX_VA_GPIO);
-		samsung_gpiolib_add_4bit2_chips(s3c64xx_gpios_4bit2,
-				ARRAY_SIZE(s3c64xx_gpios_4bit2));
-	} else {
-		WARN(1, "Unknown SoC in gpio-samsung, no GPIOs added\n");
-		return -ENODEV;
-	}
-
-	return 0;
-}
-core_initcall(samsung_gpiolib_init);
-
-int s3c_gpio_cfgpin(unsigned int pin, unsigned int config)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned long flags;
-	int offset;
-	int ret;
-
-	if (!chip)
-		return -EINVAL;
-
-	offset = pin - chip->chip.base;
-
-	samsung_gpio_lock(chip, flags);
-	ret = samsung_gpio_do_setcfg(chip, offset, config);
-	samsung_gpio_unlock(chip, flags);
-
-	return ret;
-}
-EXPORT_SYMBOL(s3c_gpio_cfgpin);
-
-int s3c_gpio_cfgpin_range(unsigned int start, unsigned int nr,
-			  unsigned int cfg)
-{
-	int ret;
-
-	for (; nr > 0; nr--, start++) {
-		ret = s3c_gpio_cfgpin(start, cfg);
-		if (ret != 0)
-			return ret;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(s3c_gpio_cfgpin_range);
-
-int s3c_gpio_cfgall_range(unsigned int start, unsigned int nr,
-			  unsigned int cfg, samsung_gpio_pull_t pull)
-{
-	int ret;
-
-	for (; nr > 0; nr--, start++) {
-		s3c_gpio_setpull(start, pull);
-		ret = s3c_gpio_cfgpin(start, cfg);
-		if (ret != 0)
-			return ret;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(s3c_gpio_cfgall_range);
-
-unsigned s3c_gpio_getcfg(unsigned int pin)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned long flags;
-	unsigned ret = 0;
-	int offset;
-
-	if (chip) {
-		offset = pin - chip->chip.base;
-
-		samsung_gpio_lock(chip, flags);
-		ret = samsung_gpio_do_getcfg(chip, offset);
-		samsung_gpio_unlock(chip, flags);
-	}
-
-	return ret;
-}
-EXPORT_SYMBOL(s3c_gpio_getcfg);
-
-int s3c_gpio_setpull(unsigned int pin, samsung_gpio_pull_t pull)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned long flags;
-	int offset, ret;
-
-	if (!chip)
-		return -EINVAL;
-
-	offset = pin - chip->chip.base;
-
-	samsung_gpio_lock(chip, flags);
-	ret = samsung_gpio_do_setpull(chip, offset, pull);
-	samsung_gpio_unlock(chip, flags);
-
-	return ret;
-}
-EXPORT_SYMBOL(s3c_gpio_setpull);
-
-samsung_gpio_pull_t s3c_gpio_getpull(unsigned int pin)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned long flags;
-	int offset;
-	u32 pup = 0;
-
-	if (chip) {
-		offset = pin - chip->chip.base;
-
-		samsung_gpio_lock(chip, flags);
-		pup = samsung_gpio_do_getpull(chip, offset);
-		samsung_gpio_unlock(chip, flags);
-	}
-
-	return (__force samsung_gpio_pull_t)pup;
-}
-EXPORT_SYMBOL(s3c_gpio_getpull);
-
-#ifdef CONFIG_PLAT_S3C24XX
-unsigned int s3c2410_modify_misccr(unsigned int clear, unsigned int change)
-{
-	unsigned long flags;
-	unsigned long misccr;
-
-	local_irq_save(flags);
-	misccr = __raw_readl(S3C24XX_MISCCR);
-	misccr &= ~clear;
-	misccr ^= change;
-	__raw_writel(misccr, S3C24XX_MISCCR);
-	local_irq_restore(flags);
-
-	return misccr;
-}
-EXPORT_SYMBOL(s3c2410_modify_misccr);
-#endif
diff --git a/drivers/gpio/gpio-sch.c b/drivers/gpio/gpio-sch.c
index b72906f5b999..5314ee4b947d 100644
--- a/drivers/gpio/gpio-sch.c
+++ b/drivers/gpio/gpio-sch.c
@@ -41,8 +41,6 @@ struct sch_gpio {
 	unsigned short resume_base;
 };
 
-#define to_sch_gpio(gc)	container_of(gc, struct sch_gpio, chip)
-
 static unsigned sch_gpio_offset(struct sch_gpio *sch, unsigned gpio,
 				unsigned reg)
 {
@@ -65,7 +63,7 @@ static unsigned sch_gpio_bit(struct sch_gpio *sch, unsigned gpio)
 
 static int sch_gpio_reg_get(struct gpio_chip *gc, unsigned gpio, unsigned reg)
 {
-	struct sch_gpio *sch = to_sch_gpio(gc);
+	struct sch_gpio *sch = gpiochip_get_data(gc);
 	unsigned short offset, bit;
 	u8 reg_val;
 
@@ -80,7 +78,7 @@ static int sch_gpio_reg_get(struct gpio_chip *gc, unsigned gpio, unsigned reg)
 static void sch_gpio_reg_set(struct gpio_chip *gc, unsigned gpio, unsigned reg,
 			     int val)
 {
-	struct sch_gpio *sch = to_sch_gpio(gc);
+	struct sch_gpio *sch = gpiochip_get_data(gc);
 	unsigned short offset, bit;
 	u8 reg_val;
 
@@ -97,7 +95,7 @@ static void sch_gpio_reg_set(struct gpio_chip *gc, unsigned gpio, unsigned reg,
 
 static int sch_gpio_direction_in(struct gpio_chip *gc, unsigned gpio_num)
 {
-	struct sch_gpio *sch = to_sch_gpio(gc);
+	struct sch_gpio *sch = gpiochip_get_data(gc);
 
 	spin_lock(&sch->lock);
 	sch_gpio_reg_set(gc, gpio_num, GIO, 1);
@@ -112,7 +110,7 @@ static int sch_gpio_get(struct gpio_chip *gc, unsigned gpio_num)
 
 static void sch_gpio_set(struct gpio_chip *gc, unsigned gpio_num, int val)
 {
-	struct sch_gpio *sch = to_sch_gpio(gc);
+	struct sch_gpio *sch = gpiochip_get_data(gc);
 
 	spin_lock(&sch->lock);
 	sch_gpio_reg_set(gc, gpio_num, GLV, val);
@@ -122,7 +120,7 @@ static void sch_gpio_set(struct gpio_chip *gc, unsigned gpio_num, int val)
 static int sch_gpio_direction_out(struct gpio_chip *gc, unsigned gpio_num,
 				  int val)
 {
-	struct sch_gpio *sch = to_sch_gpio(gc);
+	struct sch_gpio *sch = gpiochip_get_data(gc);
 
 	spin_lock(&sch->lock);
 	sch_gpio_reg_set(gc, gpio_num, GIO, 0);
@@ -171,7 +169,7 @@ static int sch_gpio_probe(struct platform_device *pdev)
 	sch->iobase = res->start;
 	sch->chip = sch_gpio_chip;
 	sch->chip.label = dev_name(&pdev->dev);
-	sch->chip.dev = &pdev->dev;
+	sch->chip.parent = &pdev->dev;
 
 	switch (pdev->id) {
 	case PCI_DEVICE_ID_INTEL_SCH_LPC:
@@ -217,7 +215,7 @@ static int sch_gpio_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, sch);
 
-	return gpiochip_add(&sch->chip);
+	return gpiochip_add_data(&sch->chip, sch);
 }
 
 static int sch_gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-sch311x.c b/drivers/gpio/gpio-sch311x.c
index 0cb11413e814..1cbd77a04e7b 100644
--- a/drivers/gpio/gpio-sch311x.c
+++ b/drivers/gpio/gpio-sch311x.c
@@ -93,13 +93,6 @@ static struct sch311x_gpio_block_def sch311x_gpio_blocks[] = {
 	},
 };
 
-static inline struct sch311x_gpio_block *
-to_sch311x_gpio_block(struct gpio_chip *chip)
-{
-	return container_of(chip, struct sch311x_gpio_block, chip);
-}
-
-
 /*
  *	Super-IO functions
  */
@@ -142,14 +135,14 @@ static inline void sch311x_sio_outb(int sio_config_port, int reg, int val)
 
 static int sch311x_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 
 	if (block->config_regs[offset] == 0) /* GPIO is not available */
 		return -ENODEV;
 
 	if (!request_region(block->runtime_reg + block->config_regs[offset],
 			    1, DRV_NAME)) {
-		dev_err(chip->dev, "Failed to request region 0x%04x.\n",
+		dev_err(chip->parent, "Failed to request region 0x%04x.\n",
 			block->runtime_reg + block->config_regs[offset]);
 		return -EBUSY;
 	}
@@ -158,7 +151,7 @@ static int sch311x_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void sch311x_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 
 	if (block->config_regs[offset] == 0) /* GPIO is not available */
 		return;
@@ -168,7 +161,7 @@ static void sch311x_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static int sch311x_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 	unsigned char data;
 
 	spin_lock(&block->lock);
@@ -192,7 +185,7 @@ static void __sch311x_gpio_set(struct sch311x_gpio_block *block,
 static void sch311x_gpio_set(struct gpio_chip *chip, unsigned offset,
 			     int value)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 
 	spin_lock(&block->lock);
 	 __sch311x_gpio_set(block, offset, value);
@@ -201,7 +194,7 @@ static void sch311x_gpio_set(struct gpio_chip *chip, unsigned offset,
 
 static int sch311x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 
 	spin_lock(&block->lock);
 	outb(SCH311X_GPIO_CONF_IN, block->runtime_reg +
@@ -214,7 +207,7 @@ static int sch311x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 static int sch311x_gpio_direction_out(struct gpio_chip *chip, unsigned offset,
 				      int value)
 {
-	struct sch311x_gpio_block *block = to_sch311x_gpio_block(chip);
+	struct sch311x_gpio_block *block = gpiochip_get_data(chip);
 
 	spin_lock(&block->lock);
 
@@ -229,7 +222,7 @@ static int sch311x_gpio_direction_out(struct gpio_chip *chip, unsigned offset,
 
 static int sch311x_gpio_probe(struct platform_device *pdev)
 {
-	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
+	struct sch311x_pdev_data *pdata = dev_get_platdata(&pdev->dev);
 	struct sch311x_gpio_priv *priv;
 	struct sch311x_gpio_block *block;
 	int err, i;
@@ -261,13 +254,13 @@ static int sch311x_gpio_probe(struct platform_device *pdev)
 		block->chip.get = sch311x_gpio_get;
 		block->chip.set = sch311x_gpio_set;
 		block->chip.ngpio = 8;
-		block->chip.dev = &pdev->dev;
+		block->chip.parent = &pdev->dev;
 		block->chip.base = sch311x_gpio_blocks[i].base;
 		block->config_regs = sch311x_gpio_blocks[i].config_regs;
 		block->data_reg = sch311x_gpio_blocks[i].data_reg;
 		block->runtime_reg = pdata->runtime_reg;
 
-		err = gpiochip_add(&block->chip);
+		err = gpiochip_add_data(&block->chip, block);
 		if (err < 0) {
 			dev_err(&pdev->dev,
 				"Could not register gpiochip, %d\n", err);
@@ -289,7 +282,7 @@ exit_err:
 
 static int sch311x_gpio_remove(struct platform_device *pdev)
 {
-	struct sch311x_pdev_data *pdata = pdev->dev.platform_data;
+	struct sch311x_pdev_data *pdata = dev_get_platdata(&pdev->dev);
 	struct sch311x_gpio_priv *priv = platform_get_drvdata(pdev);
 	int i;
 
diff --git a/drivers/gpio/gpio-sodaville.c b/drivers/gpio/gpio-sodaville.c
index 34b02b42ab9e..e3cb6772f6ec 100644
--- a/drivers/gpio/gpio-sodaville.c
+++ b/drivers/gpio/gpio-sodaville.c
@@ -10,7 +10,6 @@
  */
 
 #include <linux/errno.h>
-#include <linux/gpio.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -20,7 +19,7 @@
 #include <linux/pci.h>
 #include <linux/platform_device.h>
 #include <linux/of_irq.h>
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 
 #define DRV_NAME		"sdv_gpio"
 #define SDV_NUM_PUB_GPIOS	12
@@ -43,7 +42,7 @@ struct sdv_gpio_chip_data {
 	void __iomem *gpio_pub_base;
 	struct irq_domain *id;
 	struct irq_chip_generic *gc;
-	struct bgpio_chip bgpio;
+	struct gpio_chip chip;
 };
 
 static int sdv_gpio_pub_set_type(struct irq_data *d, unsigned int type)
@@ -226,14 +225,14 @@ static int sdv_gpio_probe(struct pci_dev *pdev,
 		writel(mux_val, sd->gpio_pub_base + GPMUXCTL);
 	}
 
-	ret = bgpio_init(&sd->bgpio, &pdev->dev, 4,
+	ret = bgpio_init(&sd->chip, &pdev->dev, 4,
 			sd->gpio_pub_base + GPINR, sd->gpio_pub_base + GPOUTR,
 			NULL, sd->gpio_pub_base + GPOER, NULL, 0);
 	if (ret)
 		goto unmap;
-	sd->bgpio.gc.ngpio = SDV_NUM_PUB_GPIOS;
+	sd->chip.ngpio = SDV_NUM_PUB_GPIOS;
 
-	ret = gpiochip_add(&sd->bgpio.gc);
+	ret = gpiochip_add_data(&sd->chip, sd);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "gpiochip_add() failed.\n");
 		goto unmap;
@@ -265,7 +264,7 @@ static void sdv_gpio_remove(struct pci_dev *pdev)
 	free_irq(pdev->irq, sd);
 	irq_free_descs(sd->irq_base, SDV_NUM_PUB_GPIOS);
 
-	gpiochip_remove(&sd->bgpio.gc);
+	gpiochip_remove(&sd->chip);
 	pci_release_region(pdev, GPIO_BAR);
 	iounmap(sd->gpio_pub_base);
 	pci_disable_device(pdev);
diff --git a/drivers/gpio/gpio-spear-spics.c b/drivers/gpio/gpio-spear-spics.c
index 69ffca5b073b..50fb09080a6b 100644
--- a/drivers/gpio/gpio-spear-spics.c
+++ b/drivers/gpio/gpio-spear-spics.c
@@ -62,8 +62,7 @@ static int spics_get_value(struct gpio_chip *chip, unsigned offset)
 
 static void spics_set_value(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct spear_spics *spics = container_of(chip, struct spear_spics,
-			chip);
+	struct spear_spics *spics = gpiochip_get_data(chip);
 	u32 tmp;
 
 	/* select chip select from register */
@@ -94,8 +93,7 @@ static int spics_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static int spics_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct spear_spics *spics = container_of(chip, struct spear_spics,
-			chip);
+	struct spear_spics *spics = gpiochip_get_data(chip);
 	u32 tmp;
 
 	if (!spics->use_count++) {
@@ -110,8 +108,7 @@ static int spics_request(struct gpio_chip *chip, unsigned offset)
 
 static void spics_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct spear_spics *spics = container_of(chip, struct spear_spics,
-			chip);
+	struct spear_spics *spics = gpiochip_get_data(chip);
 	u32 tmp;
 
 	if (!--spics->use_count) {
@@ -164,11 +161,11 @@ static int spics_gpio_probe(struct platform_device *pdev)
 	spics->chip.get = spics_get_value;
 	spics->chip.set = spics_set_value;
 	spics->chip.label = dev_name(&pdev->dev);
-	spics->chip.dev = &pdev->dev;
+	spics->chip.parent = &pdev->dev;
 	spics->chip.owner = THIS_MODULE;
 	spics->last_off = -1;
 
-	ret = gpiochip_add(&spics->chip);
+	ret = gpiochip_add_data(&spics->chip, spics);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpio chip\n");
 		return ret;
diff --git a/drivers/gpio/gpio-sta2x11.c b/drivers/gpio/gpio-sta2x11.c
index 55e47828ddfc..83af1cb36333 100644
--- a/drivers/gpio/gpio-sta2x11.c
+++ b/drivers/gpio/gpio-sta2x11.c
@@ -74,7 +74,7 @@ static inline u32 __bit(int nr)
 
 static void gsta_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 {
-	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_gpio *chip = gpiochip_get_data(gpio);
 	struct gsta_regs __iomem *regs = __regs(chip, nr);
 	u32 bit = __bit(nr);
 
@@ -86,17 +86,17 @@ static void gsta_gpio_set(struct gpio_chip *gpio, unsigned nr, int val)
 
 static int gsta_gpio_get(struct gpio_chip *gpio, unsigned nr)
 {
-	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_gpio *chip = gpiochip_get_data(gpio);
 	struct gsta_regs __iomem *regs = __regs(chip, nr);
 	u32 bit = __bit(nr);
 
-	return readl(&regs->dat) & bit;
+	return !!(readl(&regs->dat) & bit);
 }
 
 static int gsta_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 				      int val)
 {
-	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_gpio *chip = gpiochip_get_data(gpio);
 	struct gsta_regs __iomem *regs = __regs(chip, nr);
 	u32 bit = __bit(nr);
 
@@ -111,7 +111,7 @@ static int gsta_gpio_direction_output(struct gpio_chip *gpio, unsigned nr,
 
 static int gsta_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 {
-	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_gpio *chip = gpiochip_get_data(gpio);
 	struct gsta_regs __iomem *regs = __regs(chip, nr);
 	u32 bit = __bit(nr);
 
@@ -121,7 +121,7 @@ static int gsta_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 
 static int gsta_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
 {
-	struct gsta_gpio *chip = container_of(gpio, struct gsta_gpio, gpio);
+	struct gsta_gpio *chip = gpiochip_get_data(gpio);
 	return chip->irq_base + offset;
 }
 
@@ -409,7 +409,7 @@ static int gsta_probe(struct platform_device *dev)
 		goto err_free_descs;
 	}
 
-	err = gpiochip_add(&chip->gpio);
+	err = gpiochip_add_data(&chip->gpio, chip);
 	if (err < 0) {
 		dev_err(&dev->dev, "sta2x11 gpio: Can't register (%i)\n",
 			-err);
diff --git a/drivers/gpio/gpio-stmpe.c b/drivers/gpio/gpio-stmpe.c
index dabfb99dddef..5197edf1acfd 100644
--- a/drivers/gpio/gpio-stmpe.c
+++ b/drivers/gpio/gpio-stmpe.c
@@ -36,14 +36,9 @@ struct stmpe_gpio {
 	u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
 };
 
-static inline struct stmpe_gpio *to_stmpe_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct stmpe_gpio, chip);
-}
-
 static int stmpe_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(chip);
+	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);
@@ -58,7 +53,7 @@ static int stmpe_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void stmpe_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(chip);
+	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);
@@ -77,7 +72,7 @@ static void stmpe_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 static int stmpe_gpio_direction_output(struct gpio_chip *chip,
 					 unsigned offset, int val)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(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);
@@ -90,7 +85,7 @@ static int stmpe_gpio_direction_output(struct gpio_chip *chip,
 static int stmpe_gpio_direction_input(struct gpio_chip *chip,
 					unsigned offset)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(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);
@@ -100,7 +95,7 @@ static int stmpe_gpio_direction_input(struct gpio_chip *chip,
 
 static int stmpe_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(chip);
+	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(chip);
 	struct stmpe *stmpe = stmpe_gpio->stmpe;
 
 	if (stmpe_gpio->norequest_mask & (1 << offset))
@@ -123,7 +118,7 @@ static struct gpio_chip template_chip = {
 static int stmpe_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -151,7 +146,7 @@ static int stmpe_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 static void stmpe_gpio_irq_lock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 
 	mutex_lock(&stmpe_gpio->irq_lock);
 }
@@ -159,7 +154,7 @@ static void stmpe_gpio_irq_lock(struct irq_data *d)
 static void stmpe_gpio_irq_sync_unlock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	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[] = {
@@ -193,7 +188,7 @@ static void stmpe_gpio_irq_sync_unlock(struct irq_data *d)
 static void stmpe_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -204,7 +199,7 @@ static void stmpe_gpio_irq_mask(struct irq_data *d)
 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 = to_stmpe_gpio(gc);
+	struct stmpe_gpio *stmpe_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -216,7 +211,7 @@ static void stmpe_dbg_show_one(struct seq_file *s,
 			       struct gpio_chip *gc,
 			       unsigned offset, unsigned gpio)
 {
-	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	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);
@@ -356,7 +351,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 	stmpe_gpio->stmpe = stmpe;
 	stmpe_gpio->chip = template_chip;
 	stmpe_gpio->chip.ngpio = stmpe->num_gpios;
-	stmpe_gpio->chip.dev = &pdev->dev;
+	stmpe_gpio->chip.parent = &pdev->dev;
 	stmpe_gpio->chip.of_node = np;
 	stmpe_gpio->chip.base = -1;
 
@@ -375,7 +370,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_free;
 
-	ret = gpiochip_add(&stmpe_gpio->chip);
+	ret = gpiochip_add_data(&stmpe_gpio->chip, stmpe_gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
 		goto out_disable;
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index 81bdbe7ba2a4..d11dd48570b2 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -100,8 +100,7 @@ struct xway_stp {
  */
 static void xway_stp_set(struct gpio_chip *gc, unsigned gpio, int val)
 {
-	struct xway_stp *chip =
-		container_of(gc, struct xway_stp, gc);
+	struct xway_stp *chip = gpiochip_get_data(gc);
 
 	if (val)
 		chip->shadow |= BIT(gpio);
@@ -135,11 +134,10 @@ static int xway_stp_dir_out(struct gpio_chip *gc, unsigned gpio, int val)
  */
 static int xway_stp_request(struct gpio_chip *gc, unsigned gpio)
 {
-	struct xway_stp *chip =
-		container_of(gc, struct xway_stp, gc);
+	struct xway_stp *chip = gpiochip_get_data(gc);
 
 	if ((gpio < 8) && (chip->reserved & BIT(gpio))) {
-		dev_err(gc->dev, "GPIO %d is driven by hardware\n", gpio);
+		dev_err(gc->parent, "GPIO %d is driven by hardware\n", gpio);
 		return -ENODEV;
 	}
 
@@ -214,7 +212,7 @@ static int xway_stp_probe(struct platform_device *pdev)
 	if (IS_ERR(chip->virt))
 		return PTR_ERR(chip->virt);
 
-	chip->gc.dev = &pdev->dev;
+	chip->gc.parent = &pdev->dev;
 	chip->gc.label = "stp-xway";
 	chip->gc.direction_output = xway_stp_dir_out;
 	chip->gc.set = xway_stp_set;
@@ -260,7 +258,7 @@ static int xway_stp_probe(struct platform_device *pdev)
 
 	ret = xway_stp_hw_init(chip);
 	if (!ret)
-		ret = gpiochip_add(&chip->gc);
+		ret = gpiochip_add_data(&chip->gc, chip);
 
 	if (!ret)
 		dev_info(&pdev->dev, "Init done\n");
diff --git a/drivers/gpio/gpio-sx150x.c b/drivers/gpio/gpio-sx150x.c
index 76f920173a2f..e6cff1cabd0c 100644
--- a/drivers/gpio/gpio-sx150x.c
+++ b/drivers/gpio/gpio-sx150x.c
@@ -32,8 +32,19 @@
 #define NO_UPDATE_PENDING	-1
 
 /* The chip models of sx150x */
-#define SX150X_456 0
-#define SX150X_789 1
+#define SX150X_123 0
+#define SX150X_456 1
+#define SX150X_789 2
+
+struct sx150x_123_pri {
+	u8 reg_pld_mode;
+	u8 reg_pld_table0;
+	u8 reg_pld_table1;
+	u8 reg_pld_table2;
+	u8 reg_pld_table3;
+	u8 reg_pld_table4;
+	u8 reg_advance;
+};
 
 struct sx150x_456_pri {
 	u8 reg_pld_mode;
@@ -65,6 +76,7 @@ struct sx150x_device_data {
 	u8 reg_sense;
 	u8 ngpios;
 	union {
+		struct sx150x_123_pri x123;
 		struct sx150x_456_pri x456;
 		struct sx150x_789_pri x789;
 	} pri;
@@ -142,12 +154,33 @@ static const struct sx150x_device_data sx150x_devices[] = {
 		},
 		.ngpios	= 16
 	},
+	[3] = { /* sx1502q */
+		.model = SX150X_123,
+		.reg_pullup	= 0x02,
+		.reg_pulldn	= 0x03,
+		.reg_dir	= 0x01,
+		.reg_data	= 0x00,
+		.reg_irq_mask	= 0x05,
+		.reg_irq_src	= 0x08,
+		.reg_sense	= 0x07,
+		.pri.x123 = {
+			.reg_pld_mode	= 0x10,
+			.reg_pld_table0	= 0x11,
+			.reg_pld_table1	= 0x12,
+			.reg_pld_table2	= 0x13,
+			.reg_pld_table3	= 0x14,
+			.reg_pld_table4	= 0x15,
+			.reg_advance	= 0xad,
+		},
+		.ngpios	= 8,
+	},
 };
 
 static const struct i2c_device_id sx150x_id[] = {
 	{"sx1508q", 0},
 	{"sx1509q", 1},
 	{"sx1506q", 2},
+	{"sx1502q", 3},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, sx150x_id);
@@ -156,15 +189,11 @@ static const struct of_device_id sx150x_of_match[] = {
 	{ .compatible = "semtech,sx1508q" },
 	{ .compatible = "semtech,sx1509q" },
 	{ .compatible = "semtech,sx1506q" },
+	{ .compatible = "semtech,sx1502q" },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sx150x_of_match);
 
-struct sx150x_chip *to_sx150x(struct gpio_chip *gc)
-{
-	return container_of(gc, struct sx150x_chip, gpio_chip);
-}
-
 static s32 sx150x_i2c_write(struct i2c_client *client, u8 reg, u8 val)
 {
 	s32 err = i2c_smbus_write_byte_data(client, reg, val);
@@ -301,7 +330,7 @@ static int sx150x_io_output(struct sx150x_chip *chip, unsigned offset, int val)
 
 static int sx150x_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct sx150x_chip *chip = to_sx150x(gc);
+	struct sx150x_chip *chip = gpiochip_get_data(gc);
 	int status = -EINVAL;
 
 	if (!offset_is_oscio(chip, offset)) {
@@ -310,12 +339,12 @@ static int sx150x_gpio_get(struct gpio_chip *gc, unsigned offset)
 		mutex_unlock(&chip->lock);
 	}
 
-	return status;
+	return (status < 0) ? status : !!status;
 }
 
 static void sx150x_gpio_set(struct gpio_chip *gc, unsigned offset, int val)
 {
-	struct sx150x_chip *chip = to_sx150x(gc);
+	struct sx150x_chip *chip = gpiochip_get_data(gc);
 
 	mutex_lock(&chip->lock);
 	if (offset_is_oscio(chip, offset))
@@ -327,7 +356,7 @@ static void sx150x_gpio_set(struct gpio_chip *gc, unsigned offset, int val)
 
 static int sx150x_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct sx150x_chip *chip = to_sx150x(gc);
+	struct sx150x_chip *chip = gpiochip_get_data(gc);
 	int status = -EINVAL;
 
 	if (!offset_is_oscio(chip, offset)) {
@@ -342,7 +371,7 @@ static int sx150x_gpio_direction_output(struct gpio_chip *gc,
 					unsigned offset,
 					int val)
 {
-	struct sx150x_chip *chip = to_sx150x(gc);
+	struct sx150x_chip *chip = gpiochip_get_data(gc);
 	int status = 0;
 
 	if (!offset_is_oscio(chip, offset)) {
@@ -355,7 +384,7 @@ static int sx150x_gpio_direction_output(struct gpio_chip *gc,
 
 static void sx150x_irq_mask(struct irq_data *d)
 {
-	struct sx150x_chip *chip = to_sx150x(irq_data_get_irq_chip_data(d));
+	struct sx150x_chip *chip = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	unsigned n = d->hwirq;
 
 	chip->irq_masked |= (1 << n);
@@ -364,7 +393,7 @@ static void sx150x_irq_mask(struct irq_data *d)
 
 static void sx150x_irq_unmask(struct irq_data *d)
 {
-	struct sx150x_chip *chip = to_sx150x(irq_data_get_irq_chip_data(d));
+	struct sx150x_chip *chip = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	unsigned n = d->hwirq;
 
 	chip->irq_masked &= ~(1 << n);
@@ -373,7 +402,7 @@ static void sx150x_irq_unmask(struct irq_data *d)
 
 static int sx150x_irq_set_type(struct irq_data *d, unsigned int flow_type)
 {
-	struct sx150x_chip *chip = to_sx150x(irq_data_get_irq_chip_data(d));
+	struct sx150x_chip *chip = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	unsigned n, val = 0;
 
 	if (flow_type & (IRQ_TYPE_LEVEL_HIGH | IRQ_TYPE_LEVEL_LOW))
@@ -428,14 +457,14 @@ static irqreturn_t sx150x_irq_thread_fn(int irq, void *dev_id)
 
 static void sx150x_irq_bus_lock(struct irq_data *d)
 {
-	struct sx150x_chip *chip = to_sx150x(irq_data_get_irq_chip_data(d));
+	struct sx150x_chip *chip = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 
 	mutex_lock(&chip->lock);
 }
 
 static void sx150x_irq_bus_sync_unlock(struct irq_data *d)
 {
-	struct sx150x_chip *chip = to_sx150x(irq_data_get_irq_chip_data(d));
+	struct sx150x_chip *chip = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	unsigned n;
 
 	if (chip->irq_update == NO_UPDATE_PENDING)
@@ -473,7 +502,7 @@ static void sx150x_init_chip(struct sx150x_chip *chip,
 
 	chip->client                     = client;
 	chip->dev_cfg                    = &sx150x_devices[driver_data];
-	chip->gpio_chip.dev              = &client->dev;
+	chip->gpio_chip.parent              = &client->dev;
 	chip->gpio_chip.label            = client->name;
 	chip->gpio_chip.direction_input  = sx150x_gpio_direction_input;
 	chip->gpio_chip.direction_output = sx150x_gpio_direction_output;
@@ -545,10 +574,14 @@ static int sx150x_init_hw(struct sx150x_chip *chip,
 		err = sx150x_i2c_write(chip->client,
 				chip->dev_cfg->pri.x789.reg_misc,
 				0x01);
-	else
+	else if (chip->dev_cfg->model == SX150X_456)
 		err = sx150x_i2c_write(chip->client,
 				chip->dev_cfg->pri.x456.reg_advance,
 				0x04);
+	else
+		err = sx150x_i2c_write(chip->client,
+				chip->dev_cfg->pri.x123.reg_advance,
+				0x00);
 	if (err < 0)
 		return err;
 
@@ -574,13 +607,20 @@ static int sx150x_init_hw(struct sx150x_chip *chip,
 				pdata->io_polarity);
 		if (err < 0)
 			return err;
-	} else {
+	} else if (chip->dev_cfg->model == SX150X_456) {
 		/* Set all pins to work in normal mode */
 		err = sx150x_init_io(chip,
 				chip->dev_cfg->pri.x456.reg_pld_mode,
 				0);
 		if (err < 0)
 			return err;
+	} else {
+		/* Set all pins to work in normal mode */
+		err = sx150x_init_io(chip,
+				chip->dev_cfg->pri.x123.reg_pld_mode,
+				0);
+		if (err < 0)
+			return err;
 	}
 
 
@@ -647,7 +687,7 @@ static int sx150x_probe(struct i2c_client *client,
 	if (rc < 0)
 		return rc;
 
-	rc = gpiochip_add(&chip->gpio_chip);
+	rc = gpiochip_add_data(&chip->gpio_chip, chip);
 	if (rc)
 		return rc;
 
@@ -680,7 +720,6 @@ static int sx150x_remove(struct i2c_client *client)
 static struct i2c_driver sx150x_driver = {
 	.driver = {
 		.name = "sx150x",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(sx150x_of_match),
 	},
 	.probe    = sx150x_probe,
diff --git a/drivers/gpio/gpio-syscon.c b/drivers/gpio/gpio-syscon.c
index 7b25fdf64802..e5c5b6205886 100644
--- a/drivers/gpio/gpio-syscon.c
+++ b/drivers/gpio/gpio-syscon.c
@@ -59,14 +59,9 @@ struct syscon_gpio_priv {
 	u32				dir_reg_offset;
 };
 
-static inline struct syscon_gpio_priv *to_syscon_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct syscon_gpio_priv, chip);
-}
-
 static int syscon_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	struct syscon_gpio_priv *priv = gpiochip_get_data(chip);
 	unsigned int val, offs;
 	int ret;
 
@@ -82,7 +77,7 @@ static int syscon_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void syscon_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	struct syscon_gpio_priv *priv = gpiochip_get_data(chip);
 	unsigned int offs;
 
 	offs = priv->dreg_offset + priv->data->dat_bit_offset + offset;
@@ -95,7 +90,7 @@ static void syscon_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 
 static int syscon_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	struct syscon_gpio_priv *priv = gpiochip_get_data(chip);
 
 	if (priv->data->flags & GPIO_SYSCON_FEAT_DIR) {
 		unsigned int offs;
@@ -113,7 +108,7 @@ static int syscon_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 
 static int syscon_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	struct syscon_gpio_priv *priv = gpiochip_get_data(chip);
 
 	if (priv->data->flags & GPIO_SYSCON_FEAT_DIR) {
 		unsigned int offs;
@@ -144,7 +139,7 @@ static const struct syscon_gpio_data clps711x_mctrl_gpio = {
 
 static void keystone_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	struct syscon_gpio_priv *priv = gpiochip_get_data(chip);
 	unsigned int offs;
 	int ret;
 
@@ -159,7 +154,7 @@ static void keystone_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 			BIT(offs % SYSCON_REG_BITS) | KEYSTONE_LOCK_BIT,
 			BIT(offs % SYSCON_REG_BITS) | KEYSTONE_LOCK_BIT);
 	if (ret < 0)
-		dev_err(chip->dev, "gpio write failed ret(%d)\n", ret);
+		dev_err(chip->parent, "gpio write failed ret(%d)\n", ret);
 }
 
 static const struct syscon_gpio_data keystone_dsp_gpio = {
@@ -228,7 +223,7 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 		priv->dir_reg_offset <<= 3;
 	}
 
-	priv->chip.dev = dev;
+	priv->chip.parent = dev;
 	priv->chip.owner = THIS_MODULE;
 	priv->chip.label = dev_name(dev);
 	priv->chip.base = -1;
@@ -243,7 +238,7 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, priv);
 
-	return gpiochip_add(&priv->chip);
+	return gpiochip_add_data(&priv->chip, priv);
 }
 
 static int syscon_gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-tb10x.c b/drivers/gpio/gpio-tb10x.c
index 4356e6c20fc5..5eaec20ddbc7 100644
--- a/drivers/gpio/gpio-tb10x.c
+++ b/drivers/gpio/gpio-tb10x.c
@@ -87,14 +87,9 @@ static inline void tb10x_set_bits(struct tb10x_gpio *gpio, unsigned int offs,
 	spin_unlock_irqrestore(&gpio->spinlock, flags);
 }
 
-static inline struct tb10x_gpio *to_tb10x_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct tb10x_gpio, gc);
-}
-
 static int tb10x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct tb10x_gpio *tb10x_gpio = to_tb10x_gpio(chip);
+	struct tb10x_gpio *tb10x_gpio = gpiochip_get_data(chip);
 	int mask = BIT(offset);
 	int val = TB10X_GPIO_DIR_IN << offset;
 
@@ -105,7 +100,7 @@ static int tb10x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int tb10x_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct tb10x_gpio *tb10x_gpio = to_tb10x_gpio(chip);
+	struct tb10x_gpio *tb10x_gpio = gpiochip_get_data(chip);
 	int val;
 
 	val = tb10x_reg_read(tb10x_gpio, OFFSET_TO_REG_DATA);
@@ -118,7 +113,7 @@ static int tb10x_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void tb10x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct tb10x_gpio *tb10x_gpio = to_tb10x_gpio(chip);
+	struct tb10x_gpio *tb10x_gpio = gpiochip_get_data(chip);
 	int mask = BIT(offset);
 	int val = value << offset;
 
@@ -128,7 +123,7 @@ static void tb10x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int tb10x_gpio_direction_out(struct gpio_chip *chip,
 					unsigned offset, int value)
 {
-	struct tb10x_gpio *tb10x_gpio = to_tb10x_gpio(chip);
+	struct tb10x_gpio *tb10x_gpio = gpiochip_get_data(chip);
 	int mask = BIT(offset);
 	int val = TB10X_GPIO_DIR_OUT << offset;
 
@@ -140,7 +135,7 @@ static int tb10x_gpio_direction_out(struct gpio_chip *chip,
 
 static int tb10x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct tb10x_gpio *tb10x_gpio = to_tb10x_gpio(chip);
+	struct tb10x_gpio *tb10x_gpio = gpiochip_get_data(chip);
 
 	return irq_create_mapping(tb10x_gpio->domain, offset);
 }
@@ -197,7 +192,7 @@ static int tb10x_gpio_probe(struct platform_device *pdev)
 		return PTR_ERR(tb10x_gpio->base);
 
 	tb10x_gpio->gc.label		= of_node_full_name(dn);
-	tb10x_gpio->gc.dev		= &pdev->dev;
+	tb10x_gpio->gc.parent		= &pdev->dev;
 	tb10x_gpio->gc.owner		= THIS_MODULE;
 	tb10x_gpio->gc.direction_input	= tb10x_gpio_direction_in;
 	tb10x_gpio->gc.get		= tb10x_gpio_get;
@@ -210,7 +205,7 @@ static int tb10x_gpio_probe(struct platform_device *pdev)
 	tb10x_gpio->gc.can_sleep	= false;
 
 
-	ret = gpiochip_add(&tb10x_gpio->gc);
+	ret = gpiochip_add_data(&tb10x_gpio->gc, tb10x_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not add gpiochip.\n");
 		goto fail_gpiochip_registration;
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index d1d585ddb9ab..05a27ec55add 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -34,14 +34,9 @@ struct tc3589x_gpio {
 	u8 oldregs[CACHE_NR_REGS][CACHE_NR_BANKS];
 };
 
-static inline struct tc3589x_gpio *to_tc3589x_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct tc3589x_gpio, chip);
-}
-
 static int tc3589x_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(chip);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	u8 reg = TC3589x_GPIODATA0 + (offset / 8) * 2;
 	u8 mask = 1 << (offset % 8);
@@ -51,12 +46,12 @@ static int tc3589x_gpio_get(struct gpio_chip *chip, unsigned offset)
 	if (ret < 0)
 		return ret;
 
-	return ret & mask;
+	return !!(ret & mask);
 }
 
 static void tc3589x_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
+	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;
@@ -68,7 +63,7 @@ static void tc3589x_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 static int tc3589x_gpio_direction_output(struct gpio_chip *chip,
 					 unsigned offset, int val)
 {
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
+	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;
@@ -81,7 +76,7 @@ static int tc3589x_gpio_direction_output(struct gpio_chip *chip,
 static int tc3589x_gpio_direction_input(struct gpio_chip *chip,
 					unsigned offset)
 {
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(chip);
+	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;
@@ -102,7 +97,7 @@ static struct gpio_chip template_chip = {
 static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(gc);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -130,7 +125,7 @@ static int tc3589x_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 static void tc3589x_gpio_irq_lock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(gc);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(gc);
 
 	mutex_lock(&tc3589x_gpio->irq_lock);
 }
@@ -138,7 +133,7 @@ static void tc3589x_gpio_irq_lock(struct irq_data *d)
 static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(gc);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(gc);
 	struct tc3589x *tc3589x = tc3589x_gpio->tc3589x;
 	static const u8 regmap[] = {
 		[REG_IBE]	= TC3589x_GPIOIBE0,
@@ -167,7 +162,7 @@ static void tc3589x_gpio_irq_sync_unlock(struct irq_data *d)
 static void tc3589x_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(gc);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -178,7 +173,7 @@ static void tc3589x_gpio_irq_mask(struct irq_data *d)
 static void tc3589x_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct tc3589x_gpio *tc3589x_gpio = to_tc3589x_gpio(gc);
+	struct tc3589x_gpio *tc3589x_gpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
@@ -258,7 +253,7 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 
 	tc3589x_gpio->chip = template_chip;
 	tc3589x_gpio->chip.ngpio = tc3589x->num_gpio;
-	tc3589x_gpio->chip.dev = &pdev->dev;
+	tc3589x_gpio->chip.parent = &pdev->dev;
 	tc3589x_gpio->chip.base = -1;
 	tc3589x_gpio->chip.of_node = np;
 
@@ -277,7 +272,7 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	ret = gpiochip_add(&tc3589x_gpio->chip);
+	ret = gpiochip_add_data(&tc3589x_gpio->chip, tc3589x_gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-tegra.c b/drivers/gpio/gpio-tegra.c
index 896bf29776b0..9a1a7e2ef388 100644
--- a/drivers/gpio/gpio-tegra.c
+++ b/drivers/gpio/gpio-tegra.c
@@ -545,7 +545,7 @@ static int tegra_gpio_probe(struct platform_device *pdev)
 
 	tegra_gpio_chip.of_node = pdev->dev.of_node;
 
-	ret = gpiochip_add(&tegra_gpio_chip);
+	ret = gpiochip_add_data(&tegra_gpio_chip, NULL);
 	if (ret < 0) {
 		irq_domain_remove(irq_domain);
 		return ret;
diff --git a/drivers/gpio/gpio-timberdale.c b/drivers/gpio/gpio-timberdale.c
index 30653e6319e9..a6de10c5275b 100644
--- a/drivers/gpio/gpio-timberdale.c
+++ b/drivers/gpio/gpio-timberdale.c
@@ -53,7 +53,7 @@ struct timbgpio {
 static int timbgpio_update_bit(struct gpio_chip *gpio, unsigned index,
 	unsigned offset, bool enabled)
 {
-	struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio);
+	struct timbgpio *tgpio = gpiochip_get_data(gpio);
 	u32 reg;
 
 	spin_lock(&tgpio->lock);
@@ -77,7 +77,7 @@ static int timbgpio_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 
 static int timbgpio_gpio_get(struct gpio_chip *gpio, unsigned nr)
 {
-	struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio);
+	struct timbgpio *tgpio = gpiochip_get_data(gpio);
 	u32 value;
 
 	value = ioread32(tgpio->membase + TGPIOVAL);
@@ -98,7 +98,7 @@ static void timbgpio_gpio_set(struct gpio_chip *gpio,
 
 static int timbgpio_to_irq(struct gpio_chip *gpio, unsigned offset)
 {
-	struct timbgpio *tgpio = container_of(gpio, struct timbgpio, gpio);
+	struct timbgpio *tgpio = gpiochip_get_data(gpio);
 
 	if (tgpio->irq_base <= 0)
 		return -EINVAL;
@@ -268,7 +268,7 @@ static int timbgpio_probe(struct platform_device *pdev)
 
 	gc->label = dev_name(&pdev->dev);
 	gc->owner = THIS_MODULE;
-	gc->dev = &pdev->dev;
+	gc->parent = &pdev->dev;
 	gc->direction_input = timbgpio_gpio_direction_input;
 	gc->get = timbgpio_gpio_get;
 	gc->direction_output = timbgpio_gpio_direction_output;
@@ -279,7 +279,7 @@ static int timbgpio_probe(struct platform_device *pdev)
 	gc->ngpio = pdata->nr_pins;
 	gc->can_sleep = false;
 
-	err = gpiochip_add(gc);
+	err = gpiochip_add_data(gc, tgpio);
 	if (err)
 		return err;
 
diff --git a/drivers/gpio/gpio-tps6586x.c b/drivers/gpio/gpio-tps6586x.c
index 9c9238e838a9..87de5486a29e 100644
--- a/drivers/gpio/gpio-tps6586x.c
+++ b/drivers/gpio/gpio-tps6586x.c
@@ -38,14 +38,9 @@ struct tps6586x_gpio {
 	struct device *parent;
 };
 
-static inline struct tps6586x_gpio *to_tps6586x_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct tps6586x_gpio, gpio_chip);
-}
-
 static int tps6586x_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	struct tps6586x_gpio *tps6586x_gpio = gpiochip_get_data(gc);
 	uint8_t val;
 	int ret;
 
@@ -59,7 +54,7 @@ static int tps6586x_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps6586x_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	struct tps6586x_gpio *tps6586x_gpio = gpiochip_get_data(gc);
 
 	tps6586x_update(tps6586x_gpio->parent, TPS6586X_GPIOSET2,
 			value << offset, 1 << offset);
@@ -68,7 +63,7 @@ static void tps6586x_gpio_set(struct gpio_chip *gc, unsigned offset,
 static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	struct tps6586x_gpio *tps6586x_gpio = gpiochip_get_data(gc);
 	uint8_t val, mask;
 
 	tps6586x_gpio_set(gc, offset, value);
@@ -82,7 +77,7 @@ static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
 
 static int tps6586x_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps6586x_gpio *tps6586x_gpio = to_tps6586x_gpio(gc);
+	struct tps6586x_gpio *tps6586x_gpio = gpiochip_get_data(gc);
 
 	return tps6586x_irq_get_virq(tps6586x_gpio->parent,
 				TPS6586X_INT_PLDO_0 + offset);
@@ -104,7 +99,7 @@ static int tps6586x_gpio_probe(struct platform_device *pdev)
 
 	tps6586x_gpio->gpio_chip.owner = THIS_MODULE;
 	tps6586x_gpio->gpio_chip.label = pdev->name;
-	tps6586x_gpio->gpio_chip.dev = &pdev->dev;
+	tps6586x_gpio->gpio_chip.parent = &pdev->dev;
 	tps6586x_gpio->gpio_chip.ngpio = 4;
 	tps6586x_gpio->gpio_chip.can_sleep = true;
 
@@ -122,7 +117,7 @@ static int tps6586x_gpio_probe(struct platform_device *pdev)
 	else
 		tps6586x_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&tps6586x_gpio->gpio_chip);
+	ret = gpiochip_add_data(&tps6586x_gpio->gpio_chip, tps6586x_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-tps65910.c b/drivers/gpio/gpio-tps65910.c
index 88f1f5ff4e96..e81eee7627a3 100644
--- a/drivers/gpio/gpio-tps65910.c
+++ b/drivers/gpio/gpio-tps65910.c
@@ -27,14 +27,9 @@ struct tps65910_gpio {
 	struct tps65910 *tps65910;
 };
 
-static inline struct tps65910_gpio *to_tps65910_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct tps65910_gpio, gpio_chip);
-}
-
 static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910_gpio *tps65910_gpio = gpiochip_get_data(gc);
 	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 	unsigned int val;
 
@@ -49,7 +44,7 @@ static int tps65910_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps65910_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910_gpio *tps65910_gpio = gpiochip_get_data(gc);
 	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	if (value)
@@ -63,7 +58,7 @@ static void tps65910_gpio_set(struct gpio_chip *gc, unsigned offset,
 static int tps65910_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910_gpio *tps65910_gpio = gpiochip_get_data(gc);
 	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	/* Set the initial value */
@@ -75,7 +70,7 @@ static int tps65910_gpio_output(struct gpio_chip *gc, unsigned offset,
 
 static int tps65910_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65910_gpio *tps65910_gpio = to_tps65910_gpio(gc);
+	struct tps65910_gpio *tps65910_gpio = gpiochip_get_data(gc);
 	struct tps65910 *tps65910 = tps65910_gpio->tps65910;
 
 	return tps65910_reg_clear_bits(tps65910, TPS65910_GPIO0 + offset,
@@ -146,7 +141,7 @@ static int tps65910_gpio_probe(struct platform_device *pdev)
 	tps65910_gpio->gpio_chip.direction_output = tps65910_gpio_output;
 	tps65910_gpio->gpio_chip.set	= tps65910_gpio_set;
 	tps65910_gpio->gpio_chip.get	= tps65910_gpio_get;
-	tps65910_gpio->gpio_chip.dev = &pdev->dev;
+	tps65910_gpio->gpio_chip.parent = &pdev->dev;
 #ifdef CONFIG_OF_GPIO
 	tps65910_gpio->gpio_chip.of_node = tps65910->dev->of_node;
 #endif
@@ -175,7 +170,7 @@ static int tps65910_gpio_probe(struct platform_device *pdev)
 	}
 
 skip_init:
-	ret = gpiochip_add(&tps65910_gpio->gpio_chip);
+	ret = gpiochip_add_data(&tps65910_gpio->gpio_chip, tps65910_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-tps65912.c b/drivers/gpio/gpio-tps65912.c
index 9cdbc0c9cb2d..4f2029c7da3a 100644
--- a/drivers/gpio/gpio-tps65912.c
+++ b/drivers/gpio/gpio-tps65912.c
@@ -26,11 +26,9 @@ struct tps65912_gpio_data {
 	struct gpio_chip gpio_chip;
 };
 
-#define to_tgd(gc) container_of(gc, struct tps65912_gpio_data, gpio_chip)
-
 static int tps65912_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912_gpio_data *tps65912_gpio = gpiochip_get_data(gc);
 	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 	int val;
 
@@ -45,7 +43,7 @@ static int tps65912_gpio_get(struct gpio_chip *gc, unsigned offset)
 static void tps65912_gpio_set(struct gpio_chip *gc, unsigned offset,
 			      int value)
 {
-	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912_gpio_data *tps65912_gpio = gpiochip_get_data(gc);
 	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	if (value)
@@ -59,7 +57,7 @@ static void tps65912_gpio_set(struct gpio_chip *gc, unsigned offset,
 static int tps65912_gpio_output(struct gpio_chip *gc, unsigned offset,
 				int value)
 {
-	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912_gpio_data *tps65912_gpio = gpiochip_get_data(gc);
 	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	/* Set the initial value */
@@ -71,7 +69,7 @@ static int tps65912_gpio_output(struct gpio_chip *gc, unsigned offset,
 
 static int tps65912_gpio_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct tps65912_gpio_data *tps65912_gpio = to_tgd(gc);
+	struct tps65912_gpio_data *tps65912_gpio = gpiochip_get_data(gc);
 	struct tps65912 *tps65912 = tps65912_gpio->tps65912;
 
 	return tps65912_clear_bits(tps65912, TPS65912_GPIO1 + offset,
@@ -104,11 +102,11 @@ static int tps65912_gpio_probe(struct platform_device *pdev)
 
 	tps65912_gpio->tps65912 = tps65912;
 	tps65912_gpio->gpio_chip = template_chip;
-	tps65912_gpio->gpio_chip.dev = &pdev->dev;
+	tps65912_gpio->gpio_chip.parent = &pdev->dev;
 	if (pdata && pdata->gpio_base)
 		tps65912_gpio->gpio_chip.base = pdata->gpio_base;
 
-	ret = gpiochip_add(&tps65912_gpio->gpio_chip);
+	ret = gpiochip_add_data(&tps65912_gpio->gpio_chip, tps65912_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-ts5500.c b/drivers/gpio/gpio-ts5500.c
index b29a102d136b..5f945083f9d8 100644
--- a/drivers/gpio/gpio-ts5500.c
+++ b/drivers/gpio/gpio-ts5500.c
@@ -185,11 +185,6 @@ static const struct ts5500_dio ts5500_lcd[] = {
 	TS5500_DIO_IN_IRQ(0x73, 7, 1),
 };
 
-static inline struct ts5500_priv *ts5500_gc_to_priv(struct gpio_chip *chip)
-{
-	return container_of(chip, struct ts5500_priv, gpio_chip);
-}
-
 static inline void ts5500_set_mask(u8 mask, u8 addr)
 {
 	u8 val = inb(addr);
@@ -206,7 +201,7 @@ static inline void ts5500_clear_mask(u8 mask, u8 addr)
 
 static int ts5500_gpio_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct ts5500_priv *priv = ts5500_gc_to_priv(chip);
+	struct ts5500_priv *priv = gpiochip_get_data(chip);
 	const struct ts5500_dio line = priv->pinout[offset];
 	unsigned long flags;
 
@@ -225,7 +220,7 @@ static int ts5500_gpio_input(struct gpio_chip *chip, unsigned offset)
 
 static int ts5500_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct ts5500_priv *priv = ts5500_gc_to_priv(chip);
+	struct ts5500_priv *priv = gpiochip_get_data(chip);
 	const struct ts5500_dio line = priv->pinout[offset];
 
 	return !!(inb(line.value_addr) & line.value_mask);
@@ -233,7 +228,7 @@ static int ts5500_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static int ts5500_gpio_output(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct ts5500_priv *priv = ts5500_gc_to_priv(chip);
+	struct ts5500_priv *priv = gpiochip_get_data(chip);
 	const struct ts5500_dio line = priv->pinout[offset];
 	unsigned long flags;
 
@@ -255,7 +250,7 @@ static int ts5500_gpio_output(struct gpio_chip *chip, unsigned offset, int val)
 
 static void ts5500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct ts5500_priv *priv = ts5500_gc_to_priv(chip);
+	struct ts5500_priv *priv = gpiochip_get_data(chip);
 	const struct ts5500_dio line = priv->pinout[offset];
 	unsigned long flags;
 
@@ -269,7 +264,7 @@ static void ts5500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 
 static int ts5500_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct ts5500_priv *priv = ts5500_gc_to_priv(chip);
+	struct ts5500_priv *priv = gpiochip_get_data(chip);
 	const struct ts5500_dio *block = priv->pinout;
 	const struct ts5500_dio line = block[offset];
 
@@ -315,7 +310,8 @@ static void ts5500_disable_irq(struct ts5500_priv *priv)
 	else if (priv->hwirq == 1)
 		ts5500_clear_mask(BIT(6), 0x7d); /* LCD_RS on IRQ1 */
 	else
-		dev_err(priv->gpio_chip.dev, "invalid hwirq %d\n", priv->hwirq);
+		dev_err(priv->gpio_chip.parent, "invalid hwirq %d\n",
+			priv->hwirq);
 	spin_unlock_irqrestore(&priv->lock, flags);
 }
 
@@ -346,7 +342,7 @@ static int ts5500_dio_probe(struct platform_device *pdev)
 
 	priv->gpio_chip.owner = THIS_MODULE;
 	priv->gpio_chip.label = name;
-	priv->gpio_chip.dev = dev;
+	priv->gpio_chip.parent = dev;
 	priv->gpio_chip.direction_input = ts5500_gpio_input;
 	priv->gpio_chip.direction_output = ts5500_gpio_output;
 	priv->gpio_chip.get = ts5500_gpio_get;
@@ -413,7 +409,7 @@ static int ts5500_dio_probe(struct platform_device *pdev)
 		break;
 	}
 
-	ret = gpiochip_add(&priv->gpio_chip);
+	ret = gpiochip_add_data(&priv->gpio_chip, priv);
 	if (ret) {
 		dev_err(dev, "failed to register the gpio chip\n");
 		return ret;
diff --git a/drivers/gpio/gpio-twl4030.c b/drivers/gpio/gpio-twl4030.c
index 9e1dbb9877c1..4b807b0e0c8e 100644
--- a/drivers/gpio/gpio-twl4030.c
+++ b/drivers/gpio/gpio-twl4030.c
@@ -76,11 +76,6 @@ struct gpio_twl4030_priv {
 
 /*----------------------------------------------------------------------*/
 
-static inline struct gpio_twl4030_priv *to_gpio_twl4030(struct gpio_chip *chip)
-{
-	return container_of(chip, struct gpio_twl4030_priv, gpio_chip);
-}
-
 /*
  * To configure TWL4030 GPIO module registers
  */
@@ -205,7 +200,7 @@ static int twl4030_get_gpio_datain(int gpio)
 
 static int twl_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 	int status = 0;
 
 	mutex_lock(&priv->mutex);
@@ -256,7 +251,7 @@ static int twl_request(struct gpio_chip *chip, unsigned offset)
 		/* optionally have the first two GPIOs switch vMMC1
 		 * and vMMC2 power supplies based on card presence.
 		 */
-		pdata = dev_get_platdata(chip->dev);
+		pdata = dev_get_platdata(chip->parent);
 		if (pdata)
 			value |= pdata->mmc_cd & 0x03;
 
@@ -273,7 +268,7 @@ done:
 
 static void twl_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 
 	mutex_lock(&priv->mutex);
 	if (offset >= TWL4030_GPIO_MAX) {
@@ -293,7 +288,7 @@ out:
 
 static int twl_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 	int ret;
 
 	mutex_lock(&priv->mutex);
@@ -312,7 +307,7 @@ static int twl_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int twl_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 	int ret;
 	int status = 0;
 
@@ -327,7 +322,7 @@ static int twl_get(struct gpio_chip *chip, unsigned offset)
 	else
 		status = twl4030_get_gpio_datain(offset);
 
-	ret = (status <= 0) ? 0 : 1;
+	ret = (status < 0) ? status : !!status;
 out:
 	mutex_unlock(&priv->mutex);
 	return ret;
@@ -335,7 +330,7 @@ out:
 
 static void twl_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 
 	mutex_lock(&priv->mutex);
 	if (offset < TWL4030_GPIO_MAX)
@@ -353,7 +348,7 @@ static void twl_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int twl_direction_out(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 	int ret = 0;
 
 	mutex_lock(&priv->mutex);
@@ -379,7 +374,7 @@ static int twl_direction_out(struct gpio_chip *chip, unsigned offset, int value)
 
 static int twl_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_twl4030_priv *priv = to_gpio_twl4030(chip);
+	struct gpio_twl4030_priv *priv = gpiochip_get_data(chip);
 
 	return (priv->irq_base && (offset < TWL4030_GPIO_MAX))
 		? (priv->irq_base + offset)
@@ -509,7 +504,7 @@ no_irqs:
 	priv->gpio_chip = template_chip;
 	priv->gpio_chip.base = -1;
 	priv->gpio_chip.ngpio = TWL4030_GPIO_MAX;
-	priv->gpio_chip.dev = &pdev->dev;
+	priv->gpio_chip.parent = &pdev->dev;
 
 	mutex_init(&priv->mutex);
 
@@ -544,7 +539,7 @@ no_irqs:
 	if (pdata->use_leds)
 		priv->gpio_chip.ngpio += 2;
 
-	ret = gpiochip_add(&priv->gpio_chip);
+	ret = gpiochip_add_data(&priv->gpio_chip, priv);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "could not register gpiochip, %d\n", ret);
 		priv->gpio_chip.ngpio = 0;
diff --git a/drivers/gpio/gpio-twl6040.c b/drivers/gpio/gpio-twl6040.c
index c946e7eef3ee..8e9e9853f3bd 100644
--- a/drivers/gpio/gpio-twl6040.c
+++ b/drivers/gpio/gpio-twl6040.c
@@ -36,7 +36,7 @@ static struct gpio_chip twl6040gpo_chip;
 
 static int twl6040gpo_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct twl6040 *twl6040 = dev_get_drvdata(chip->dev->parent);
+	struct twl6040 *twl6040 = dev_get_drvdata(chip->parent->parent);
 	int ret = 0;
 
 	ret = twl6040_reg_read(twl6040, TWL6040_REG_GPOCTL);
@@ -55,7 +55,7 @@ static int twl6040gpo_direction_out(struct gpio_chip *chip, unsigned offset,
 
 static void twl6040gpo_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct twl6040 *twl6040 = dev_get_drvdata(chip->dev->parent);
+	struct twl6040 *twl6040 = dev_get_drvdata(chip->parent->parent);
 	int ret;
 	u8 gpoctl;
 
@@ -95,12 +95,12 @@ static int gpo_twl6040_probe(struct platform_device *pdev)
 	else
 		twl6040gpo_chip.ngpio = 1; /* twl6041 have 1 GPO */
 
-	twl6040gpo_chip.dev = &pdev->dev;
+	twl6040gpo_chip.parent = &pdev->dev;
 #ifdef CONFIG_OF_GPIO
 	twl6040gpo_chip.of_node = twl6040_core_dev->of_node;
 #endif
 
-	ret = gpiochip_add(&twl6040gpo_chip);
+	ret = gpiochip_add_data(&twl6040gpo_chip, NULL);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "could not register gpiochip, %d\n", ret);
 		twl6040gpo_chip.ngpio = 0;
diff --git a/drivers/gpio/gpio-tz1090-pdc.c b/drivers/gpio/gpio-tz1090-pdc.c
index 3623d009d808..5b7781741ee9 100644
--- a/drivers/gpio/gpio-tz1090-pdc.c
+++ b/drivers/gpio/gpio-tz1090-pdc.c
@@ -49,7 +49,6 @@ struct tz1090_pdc_gpio {
 	void __iomem *reg;
 	int irq[GPIO_PDC_NIRQ];
 };
-#define to_pdc(c)	container_of(c, struct tz1090_pdc_gpio, chip)
 
 /* Register accesses into the PDC MMIO area */
 
@@ -70,7 +69,7 @@ static inline unsigned int pdc_read(struct tz1090_pdc_gpio *priv,
 static int tz1090_pdc_gpio_direction_input(struct gpio_chip *chip,
 					   unsigned int offset)
 {
-	struct tz1090_pdc_gpio *priv = to_pdc(chip);
+	struct tz1090_pdc_gpio *priv = gpiochip_get_data(chip);
 	u32 value;
 	int lstat;
 
@@ -87,7 +86,7 @@ static int tz1090_pdc_gpio_direction_output(struct gpio_chip *chip,
 					    unsigned int offset,
 					    int output_value)
 {
-	struct tz1090_pdc_gpio *priv = to_pdc(chip);
+	struct tz1090_pdc_gpio *priv = gpiochip_get_data(chip);
 	u32 value;
 	int lstat;
 
@@ -112,14 +111,14 @@ static int tz1090_pdc_gpio_direction_output(struct gpio_chip *chip,
 
 static int tz1090_pdc_gpio_get(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_pdc_gpio *priv = to_pdc(chip);
-	return pdc_read(priv, REG_SOC_GPIO_STATUS) & BIT(offset);
+	struct tz1090_pdc_gpio *priv = gpiochip_get_data(chip);
+	return !!(pdc_read(priv, REG_SOC_GPIO_STATUS) & BIT(offset));
 }
 
 static void tz1090_pdc_gpio_set(struct gpio_chip *chip, unsigned int offset,
 				int output_value)
 {
-	struct tz1090_pdc_gpio *priv = to_pdc(chip);
+	struct tz1090_pdc_gpio *priv = gpiochip_get_data(chip);
 	u32 value;
 	int lstat;
 
@@ -139,7 +138,7 @@ static void tz1090_pdc_gpio_set(struct gpio_chip *chip, unsigned int offset,
 
 static int tz1090_pdc_gpio_to_irq(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_pdc_gpio *priv = to_pdc(chip);
+	struct tz1090_pdc_gpio *priv = gpiochip_get_data(chip);
 	unsigned int syswake = offset - GPIO_PDC_IRQ_FIRST;
 	int irq;
 
@@ -188,7 +187,7 @@ static int tz1090_pdc_gpio_probe(struct platform_device *pdev)
 
 	/* Set up GPIO chip */
 	priv->chip.label		= "tz1090-pdc-gpio";
-	priv->chip.dev			= &pdev->dev;
+	priv->chip.parent		= &pdev->dev;
 	priv->chip.direction_input	= tz1090_pdc_gpio_direction_input;
 	priv->chip.direction_output	= tz1090_pdc_gpio_direction_output;
 	priv->chip.get			= tz1090_pdc_gpio_get;
@@ -207,7 +206,7 @@ static int tz1090_pdc_gpio_probe(struct platform_device *pdev)
 		priv->irq[i] = irq_of_parse_and_map(np, i);
 
 	/* Add the GPIO bank */
-	gpiochip_add(&priv->chip);
+	gpiochip_add_data(&priv->chip, priv);
 
 	return 0;
 }
diff --git a/drivers/gpio/gpio-tz1090.c b/drivers/gpio/gpio-tz1090.c
index 87bb1b1eee8d..ca958e0f6909 100644
--- a/drivers/gpio/gpio-tz1090.c
+++ b/drivers/gpio/gpio-tz1090.c
@@ -62,7 +62,6 @@ struct tz1090_gpio_bank {
 	int irq;
 	char label[16];
 };
-#define to_bank(c)	container_of(c, struct tz1090_gpio_bank, chip)
 
 /**
  * struct tz1090_gpio - Overall GPIO device private data
@@ -187,7 +186,7 @@ static inline int tz1090_gpio_read_bit(struct tz1090_gpio_bank *bank,
 static int tz1090_gpio_direction_input(struct gpio_chip *chip,
 				       unsigned int offset)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 	tz1090_gpio_set_bit(bank, REG_GPIO_DIR, offset);
 
 	return 0;
@@ -196,7 +195,7 @@ static int tz1090_gpio_direction_input(struct gpio_chip *chip,
 static int tz1090_gpio_direction_output(struct gpio_chip *chip,
 					unsigned int offset, int output_value)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 	int lstat;
 
 	__global_lock2(lstat);
@@ -212,9 +211,9 @@ static int tz1090_gpio_direction_output(struct gpio_chip *chip,
  */
 static int tz1090_gpio_get(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 
-	return tz1090_gpio_read_bit(bank, REG_GPIO_DIN, offset);
+	return !!tz1090_gpio_read_bit(bank, REG_GPIO_DIN, offset);
 }
 
 /*
@@ -223,14 +222,14 @@ static int tz1090_gpio_get(struct gpio_chip *chip, unsigned int offset)
 static void tz1090_gpio_set(struct gpio_chip *chip, unsigned int offset,
 			    int output_value)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 
 	tz1090_gpio_mod_bit(bank, REG_GPIO_DOUT, offset, output_value);
 }
 
 static int tz1090_gpio_request(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 	int ret;
 
 	ret = pinctrl_request_gpio(chip->base + offset);
@@ -245,7 +244,7 @@ static int tz1090_gpio_request(struct gpio_chip *chip, unsigned int offset)
 
 static void tz1090_gpio_free(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 
 	pinctrl_free_gpio(chip->base + offset);
 
@@ -254,7 +253,7 @@ static void tz1090_gpio_free(struct gpio_chip *chip, unsigned int offset)
 
 static int tz1090_gpio_to_irq(struct gpio_chip *chip, unsigned int offset)
 {
-	struct tz1090_gpio_bank *bank = to_bank(chip);
+	struct tz1090_gpio_bank *bank = gpiochip_get_data(chip);
 
 	if (!bank->domain)
 		return -EINVAL;
@@ -425,7 +424,7 @@ static int tz1090_gpio_bank_probe(struct tz1090_gpio_bank_info *info)
 	snprintf(bank->label, sizeof(bank->label), "tz1090-gpio-%u",
 		 info->index);
 	bank->chip.label		= bank->label;
-	bank->chip.dev			= dev;
+	bank->chip.parent		= dev;
 	bank->chip.direction_input	= tz1090_gpio_direction_input;
 	bank->chip.direction_output	= tz1090_gpio_direction_output;
 	bank->chip.get			= tz1090_gpio_get;
@@ -440,7 +439,7 @@ static int tz1090_gpio_bank_probe(struct tz1090_gpio_bank_info *info)
 	bank->chip.ngpio		= 30;
 
 	/* Add the GPIO bank */
-	gpiochip_add(&bank->chip);
+	gpiochip_add_data(&bank->chip, bank);
 
 	/* Get the GPIO bank IRQ if provided */
 	bank->irq = irq_of_parse_and_map(np, 0);
diff --git a/drivers/gpio/gpio-ucb1400.c b/drivers/gpio/gpio-ucb1400.c
index d502825159b9..2c5cd46bfa6e 100644
--- a/drivers/gpio/gpio-ucb1400.c
+++ b/drivers/gpio/gpio-ucb1400.c
@@ -15,7 +15,7 @@
 static int ucb1400_gpio_dir_in(struct gpio_chip *gc, unsigned off)
 {
 	struct ucb1400_gpio *gpio;
-	gpio = container_of(gc, struct ucb1400_gpio, gc);
+	gpio = gpiochip_get_data(gc);
 	ucb1400_gpio_set_direction(gpio->ac97, off, 0);
 	return 0;
 }
@@ -23,7 +23,7 @@ static int ucb1400_gpio_dir_in(struct gpio_chip *gc, unsigned off)
 static int ucb1400_gpio_dir_out(struct gpio_chip *gc, unsigned off, int val)
 {
 	struct ucb1400_gpio *gpio;
-	gpio = container_of(gc, struct ucb1400_gpio, gc);
+	gpio = gpiochip_get_data(gc);
 	ucb1400_gpio_set_direction(gpio->ac97, off, 1);
 	ucb1400_gpio_set_value(gpio->ac97, off, val);
 	return 0;
@@ -32,14 +32,15 @@ static int ucb1400_gpio_dir_out(struct gpio_chip *gc, unsigned off, int val)
 static int ucb1400_gpio_get(struct gpio_chip *gc, unsigned off)
 {
 	struct ucb1400_gpio *gpio;
-	gpio = container_of(gc, struct ucb1400_gpio, gc);
-	return ucb1400_gpio_get_value(gpio->ac97, off);
+
+	gpio = gpiochip_get_data(gc);
+	return !!ucb1400_gpio_get_value(gpio->ac97, off);
 }
 
 static void ucb1400_gpio_set(struct gpio_chip *gc, unsigned off, int val)
 {
 	struct ucb1400_gpio *gpio;
-	gpio = container_of(gc, struct ucb1400_gpio, gc);
+	gpio = gpiochip_get_data(gc);
 	ucb1400_gpio_set_value(gpio->ac97, off, val);
 }
 
@@ -66,7 +67,7 @@ static int ucb1400_gpio_probe(struct platform_device *dev)
 	ucb->gc.set = ucb1400_gpio_set;
 	ucb->gc.can_sleep = true;
 
-	err = gpiochip_add(&ucb->gc);
+	err = gpiochip_add_data(&ucb->gc, ucb);
 	if (err)
 		goto err;
 
diff --git a/drivers/gpio/gpio-vf610.c b/drivers/gpio/gpio-vf610.c
index 87b950cec6ec..6284bdbe1e0c 100644
--- a/drivers/gpio/gpio-vf610.c
+++ b/drivers/gpio/gpio-vf610.c
@@ -62,11 +62,6 @@ struct vf610_gpio_port {
 
 static struct irq_chip vf610_gpio_irq_chip;
 
-static struct vf610_gpio_port *to_vf610_gp(struct gpio_chip *gc)
-{
-	return container_of(gc, struct vf610_gpio_port, gc);
-}
-
 static const struct of_device_id vf610_gpio_dt_ids[] = {
 	{ .compatible = "fsl,vf610-gpio" },
 	{ /* sentinel */ }
@@ -84,14 +79,14 @@ static inline u32 vf610_gpio_readl(void __iomem *reg)
 
 static int vf610_gpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
-	struct vf610_gpio_port *port = to_vf610_gp(gc);
+	struct vf610_gpio_port *port = gpiochip_get_data(gc);
 
 	return !!(vf610_gpio_readl(port->gpio_base + GPIO_PDIR) & BIT(gpio));
 }
 
 static void vf610_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
-	struct vf610_gpio_port *port = to_vf610_gp(gc);
+	struct vf610_gpio_port *port = gpiochip_get_data(gc);
 	unsigned long mask = BIT(gpio);
 
 	if (val)
@@ -116,7 +111,7 @@ static int vf610_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 static void vf610_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_desc_get_handler_data(desc));
+		gpiochip_get_data(irq_desc_get_handler_data(desc));
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	int pin;
 	unsigned long irq_isfr;
@@ -137,7 +132,7 @@ static void vf610_gpio_irq_handler(struct irq_desc *desc)
 static void vf610_gpio_irq_ack(struct irq_data *d)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	int gpio = d->hwirq;
 
 	vf610_gpio_writel(BIT(gpio), port->base + PORT_ISFR);
@@ -146,7 +141,7 @@ static void vf610_gpio_irq_ack(struct irq_data *d)
 static int vf610_gpio_irq_set_type(struct irq_data *d, u32 type)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	u8 irqc;
 
 	switch (type) {
@@ -182,7 +177,7 @@ static int vf610_gpio_irq_set_type(struct irq_data *d, u32 type)
 static void vf610_gpio_irq_mask(struct irq_data *d)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	void __iomem *pcr_base = port->base + PORT_PCR(d->hwirq);
 
 	vf610_gpio_writel(0, pcr_base);
@@ -191,7 +186,7 @@ static void vf610_gpio_irq_mask(struct irq_data *d)
 static void vf610_gpio_irq_unmask(struct irq_data *d)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	void __iomem *pcr_base = port->base + PORT_PCR(d->hwirq);
 
 	vf610_gpio_writel(port->irqc[d->hwirq] << PORT_PCR_IRQC_OFFSET,
@@ -201,7 +196,7 @@ static void vf610_gpio_irq_unmask(struct irq_data *d)
 static int vf610_gpio_irq_set_wake(struct irq_data *d, u32 enable)
 {
 	struct vf610_gpio_port *port =
-		to_vf610_gp(irq_data_get_irq_chip_data(d));
+		gpiochip_get_data(irq_data_get_irq_chip_data(d));
 
 	if (enable)
 		enable_irq_wake(port->irq);
@@ -249,7 +244,7 @@ static int vf610_gpio_probe(struct platform_device *pdev)
 
 	gc = &port->gc;
 	gc->of_node = np;
-	gc->dev = dev;
+	gc->parent = dev;
 	gc->label = "vf610-gpio";
 	gc->ngpio = VF610_GPIO_PER_PORT;
 	gc->base = of_alias_get_id(np, "gpio") * VF610_GPIO_PER_PORT;
@@ -261,7 +256,7 @@ static int vf610_gpio_probe(struct platform_device *pdev)
 	gc->direction_output = vf610_gpio_direction_output;
 	gc->set = vf610_gpio_set;
 
-	ret = gpiochip_add(gc);
+	ret = gpiochip_add_data(gc, port);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/gpio/gpio-viperboard.c b/drivers/gpio/gpio-viperboard.c
index e2a11f27807f..1170b035cb92 100644
--- a/drivers/gpio/gpio-viperboard.c
+++ b/drivers/gpio/gpio-viperboard.c
@@ -88,14 +88,13 @@ static int vprbrd_gpioa_get(struct gpio_chip *chip,
 		unsigned offset)
 {
 	int ret, answer, error = 0;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpioa);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpioa_msg *gamsg = (struct vprbrd_gpioa_msg *)vb->buf;
 
 	/* if io is set to output, just return the saved value */
 	if (gpio->gpioa_out & (1 << offset))
-		return gpio->gpioa_val & (1 << offset);
+		return !!(gpio->gpioa_val & (1 << offset));
 
 	mutex_lock(&vb->lock);
 
@@ -139,8 +138,7 @@ static void vprbrd_gpioa_set(struct gpio_chip *chip,
 		unsigned offset, int value)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpioa);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpioa_msg *gamsg = (struct vprbrd_gpioa_msg *)vb->buf;
 
@@ -173,7 +171,7 @@ static void vprbrd_gpioa_set(struct gpio_chip *chip,
 		mutex_unlock(&vb->lock);
 
 		if (ret != sizeof(struct vprbrd_gpioa_msg))
-			dev_err(chip->dev, "usb error setting pin value\n");
+			dev_err(chip->parent, "usb error setting pin value\n");
 	}
 }
 
@@ -181,8 +179,7 @@ static int vprbrd_gpioa_direction_input(struct gpio_chip *chip,
 			unsigned offset)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpioa);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpioa_msg *gamsg = (struct vprbrd_gpioa_msg *)vb->buf;
 
@@ -219,8 +216,7 @@ static int vprbrd_gpioa_direction_output(struct gpio_chip *chip,
 			unsigned offset, int value)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpioa);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpioa_msg *gamsg = (struct vprbrd_gpioa_msg *)vb->buf;
 
@@ -287,8 +283,7 @@ static int vprbrd_gpiob_get(struct gpio_chip *chip,
 {
 	int ret;
 	u16 val;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpiob);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpiob_msg *gbmsg = (struct vprbrd_gpiob_msg *)vb->buf;
 
@@ -319,8 +314,7 @@ static void vprbrd_gpiob_set(struct gpio_chip *chip,
 		unsigned offset, int value)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpiob);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 	struct vprbrd_gpiob_msg *gbmsg = (struct vprbrd_gpiob_msg *)vb->buf;
 
@@ -345,7 +339,7 @@ static void vprbrd_gpiob_set(struct gpio_chip *chip,
 		mutex_unlock(&vb->lock);
 
 		if (ret != sizeof(struct vprbrd_gpiob_msg))
-			dev_err(chip->dev, "usb error setting pin value\n");
+			dev_err(chip->parent, "usb error setting pin value\n");
 	}
 }
 
@@ -353,8 +347,7 @@ static int vprbrd_gpiob_direction_input(struct gpio_chip *chip,
 			unsigned offset)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpiob);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 
 	gpio->gpiob_out &= ~(1 << offset);
@@ -366,7 +359,7 @@ static int vprbrd_gpiob_direction_input(struct gpio_chip *chip,
 	mutex_unlock(&vb->lock);
 
 	if (ret)
-		dev_err(chip->dev, "usb error setting pin to input\n");
+		dev_err(chip->parent, "usb error setting pin to input\n");
 
 	return ret;
 }
@@ -375,8 +368,7 @@ static int vprbrd_gpiob_direction_output(struct gpio_chip *chip,
 			unsigned offset, int value)
 {
 	int ret;
-	struct vprbrd_gpio *gpio =
-			container_of(chip, struct vprbrd_gpio, gpiob);
+	struct vprbrd_gpio *gpio = gpiochip_get_data(chip);
 	struct vprbrd *vb = gpio->vb;
 
 	gpio->gpiob_out |= (1 << offset);
@@ -385,7 +377,7 @@ static int vprbrd_gpiob_direction_output(struct gpio_chip *chip,
 
 	ret = vprbrd_gpiob_setdir(vb, offset, 1);
 	if (ret)
-		dev_err(chip->dev, "usb error setting pin to output\n");
+		dev_err(chip->parent, "usb error setting pin to output\n");
 
 	mutex_unlock(&vb->lock);
 
@@ -409,7 +401,7 @@ static int vprbrd_gpio_probe(struct platform_device *pdev)
 	vb_gpio->vb = vb;
 	/* registering gpio a */
 	vb_gpio->gpioa.label = "viperboard gpio a";
-	vb_gpio->gpioa.dev = &pdev->dev;
+	vb_gpio->gpioa.parent = &pdev->dev;
 	vb_gpio->gpioa.owner = THIS_MODULE;
 	vb_gpio->gpioa.base = -1;
 	vb_gpio->gpioa.ngpio = 16;
@@ -418,15 +410,15 @@ static int vprbrd_gpio_probe(struct platform_device *pdev)
 	vb_gpio->gpioa.get = vprbrd_gpioa_get;
 	vb_gpio->gpioa.direction_input = vprbrd_gpioa_direction_input;
 	vb_gpio->gpioa.direction_output = vprbrd_gpioa_direction_output;
-	ret = gpiochip_add(&vb_gpio->gpioa);
+	ret = gpiochip_add_data(&vb_gpio->gpioa, vb_gpio);
 	if (ret < 0) {
-		dev_err(vb_gpio->gpioa.dev, "could not add gpio a");
+		dev_err(vb_gpio->gpioa.parent, "could not add gpio a");
 		goto err_gpioa;
 	}
 
 	/* registering gpio b */
 	vb_gpio->gpiob.label = "viperboard gpio b";
-	vb_gpio->gpiob.dev = &pdev->dev;
+	vb_gpio->gpiob.parent = &pdev->dev;
 	vb_gpio->gpiob.owner = THIS_MODULE;
 	vb_gpio->gpiob.base = -1;
 	vb_gpio->gpiob.ngpio = 16;
@@ -435,9 +427,9 @@ static int vprbrd_gpio_probe(struct platform_device *pdev)
 	vb_gpio->gpiob.get = vprbrd_gpiob_get;
 	vb_gpio->gpiob.direction_input = vprbrd_gpiob_direction_input;
 	vb_gpio->gpiob.direction_output = vprbrd_gpiob_direction_output;
-	ret = gpiochip_add(&vb_gpio->gpiob);
+	ret = gpiochip_add_data(&vb_gpio->gpiob, vb_gpio);
 	if (ret < 0) {
-		dev_err(vb_gpio->gpiob.dev, "could not add gpio b");
+		dev_err(vb_gpio->gpiob.parent, "could not add gpio b");
 		goto err_gpiob;
 	}
 
diff --git a/drivers/gpio/gpio-vr41xx.c b/drivers/gpio/gpio-vr41xx.c
index c1caa459c02d..ac8deb01f6f6 100644
--- a/drivers/gpio/gpio-vr41xx.c
+++ b/drivers/gpio/gpio-vr41xx.c
@@ -139,7 +139,7 @@ static void unmask_giuint_low(struct irq_data *d)
 static unsigned int startup_giuint(struct irq_data *data)
 {
 	if (gpiochip_lock_as_irq(&vr41xx_gpio_chip, data->hwirq))
-		dev_err(vr41xx_gpio_chip.dev,
+		dev_err(vr41xx_gpio_chip.parent,
 			"unable to lock HW IRQ %lu for IRQ\n",
 			data->hwirq);
 	/* Satisfy the .enable semantics by unmasking the line */
@@ -542,9 +542,9 @@ static int giu_probe(struct platform_device *pdev)
 	if (!giu_base)
 		return -ENOMEM;
 
-	vr41xx_gpio_chip.dev = &pdev->dev;
+	vr41xx_gpio_chip.parent = &pdev->dev;
 
-	ret = gpiochip_add(&vr41xx_gpio_chip);
+	ret = gpiochip_add_data(&vr41xx_gpio_chip, NULL);
 	if (!ret) {
 		iounmap(giu_base);
 		return -ENODEV;
diff --git a/drivers/gpio/gpio-vx855.c b/drivers/gpio/gpio-vx855.c
index 57b470d5b39e..764999cc0794 100644
--- a/drivers/gpio/gpio-vx855.c
+++ b/drivers/gpio/gpio-vx855.c
@@ -96,7 +96,7 @@ static inline u_int32_t gpio_o_bit(int i)
 static int vx855gpio_direction_input(struct gpio_chip *gpio,
 				     unsigned int nr)
 {
-	struct vx855_gpio *vg = container_of(gpio, struct vx855_gpio, gpio);
+	struct vx855_gpio *vg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u_int32_t reg_out;
 
@@ -120,7 +120,7 @@ static int vx855gpio_direction_input(struct gpio_chip *gpio,
 
 static int vx855gpio_get(struct gpio_chip *gpio, unsigned int nr)
 {
-	struct vx855_gpio *vg = container_of(gpio, struct vx855_gpio, gpio);
+	struct vx855_gpio *vg = gpiochip_get_data(gpio);
 	u_int32_t reg_in;
 	int ret = 0;
 
@@ -146,7 +146,7 @@ static int vx855gpio_get(struct gpio_chip *gpio, unsigned int nr)
 static void vx855gpio_set(struct gpio_chip *gpio, unsigned int nr,
 			  int val)
 {
-	struct vx855_gpio *vg = container_of(gpio, struct vx855_gpio, gpio);
+	struct vx855_gpio *vg = gpiochip_get_data(gpio);
 	unsigned long flags;
 	u_int32_t reg_out;
 
@@ -259,7 +259,7 @@ static int vx855gpio_probe(struct platform_device *pdev)
 
 	vx855gpio_gpio_setup(vg);
 
-	return gpiochip_add(&vg->gpio);
+	return gpiochip_add_data(&vg->gpio, vg);
 }
 
 static int vx855gpio_remove(struct platform_device *pdev)
diff --git a/drivers/gpio/gpio-wm831x.c b/drivers/gpio/gpio-wm831x.c
index 58ce75c188b7..98390070fb64 100644
--- a/drivers/gpio/gpio-wm831x.c
+++ b/drivers/gpio/gpio-wm831x.c
@@ -30,14 +30,9 @@ struct wm831x_gpio {
 	struct gpio_chip gpio_chip;
 };
 
-static inline struct wm831x_gpio *to_wm831x_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct wm831x_gpio, gpio_chip);
-}
-
 static int wm831x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 	int val = WM831X_GPN_DIR;
 
@@ -51,7 +46,7 @@ static int wm831x_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int wm831x_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 	int ret;
 
@@ -67,7 +62,7 @@ static int wm831x_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void wm831x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 
 	wm831x_set_bits(wm831x, WM831X_GPIO_LEVEL, 1 << offset,
@@ -77,7 +72,7 @@ static void wm831x_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int wm831x_gpio_direction_out(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 	int val = 0;
 	int ret;
@@ -99,7 +94,7 @@ static int wm831x_gpio_direction_out(struct gpio_chip *chip,
 
 static int wm831x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 
 	return irq_create_mapping(wm831x->irq_domain,
@@ -109,7 +104,7 @@ static int wm831x_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 static int wm831x_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
 				    unsigned debounce)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 	int reg = WM831X_GPIO1_CONTROL + offset;
 	int ret, fn;
@@ -140,7 +135,7 @@ static int wm831x_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
 #ifdef CONFIG_DEBUG_FS
 static void wm831x_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct wm831x_gpio *wm831x_gpio = to_wm831x_gpio(chip);
+	struct wm831x_gpio *wm831x_gpio = gpiochip_get_data(chip);
 	struct wm831x *wm831x = wm831x_gpio->wm831x;
 	int i, tristated;
 
@@ -258,13 +253,13 @@ static int wm831x_gpio_probe(struct platform_device *pdev)
 	wm831x_gpio->wm831x = wm831x;
 	wm831x_gpio->gpio_chip = template_chip;
 	wm831x_gpio->gpio_chip.ngpio = wm831x->num_gpio;
-	wm831x_gpio->gpio_chip.dev = &pdev->dev;
+	wm831x_gpio->gpio_chip.parent = &pdev->dev;
 	if (pdata && pdata->gpio_base)
 		wm831x_gpio->gpio_chip.base = pdata->gpio_base;
 	else
 		wm831x_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&wm831x_gpio->gpio_chip);
+	ret = gpiochip_add_data(&wm831x_gpio->gpio_chip, wm831x_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-wm8350.c b/drivers/gpio/gpio-wm8350.c
index 060b89303bb6..0a306b4baa73 100644
--- a/drivers/gpio/gpio-wm8350.c
+++ b/drivers/gpio/gpio-wm8350.c
@@ -28,14 +28,9 @@ struct wm8350_gpio_data {
 	struct gpio_chip gpio_chip;
 };
 
-static inline struct wm8350_gpio_data *to_wm8350_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct wm8350_gpio_data, gpio_chip);
-}
-
 static int wm8350_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8350_gpio_data *wm8350_gpio = to_wm8350_gpio(chip);
+	struct wm8350_gpio_data *wm8350_gpio = gpiochip_get_data(chip);
 	struct wm8350 *wm8350 = wm8350_gpio->wm8350;
 
 	return wm8350_set_bits(wm8350, WM8350_GPIO_CONFIGURATION_I_O,
@@ -44,7 +39,7 @@ static int wm8350_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int wm8350_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8350_gpio_data *wm8350_gpio = to_wm8350_gpio(chip);
+	struct wm8350_gpio_data *wm8350_gpio = gpiochip_get_data(chip);
 	struct wm8350 *wm8350 = wm8350_gpio->wm8350;
 	int ret;
 
@@ -60,7 +55,7 @@ static int wm8350_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void wm8350_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct wm8350_gpio_data *wm8350_gpio = to_wm8350_gpio(chip);
+	struct wm8350_gpio_data *wm8350_gpio = gpiochip_get_data(chip);
 	struct wm8350 *wm8350 = wm8350_gpio->wm8350;
 
 	if (value)
@@ -72,7 +67,7 @@ static void wm8350_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 static int wm8350_gpio_direction_out(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
-	struct wm8350_gpio_data *wm8350_gpio = to_wm8350_gpio(chip);
+	struct wm8350_gpio_data *wm8350_gpio = gpiochip_get_data(chip);
 	struct wm8350 *wm8350 = wm8350_gpio->wm8350;
 	int ret;
 
@@ -89,7 +84,7 @@ static int wm8350_gpio_direction_out(struct gpio_chip *chip,
 
 static int wm8350_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8350_gpio_data *wm8350_gpio = to_wm8350_gpio(chip);
+	struct wm8350_gpio_data *wm8350_gpio = gpiochip_get_data(chip);
 	struct wm8350 *wm8350 = wm8350_gpio->wm8350;
 
 	if (!wm8350->irq_base)
@@ -124,13 +119,13 @@ static int wm8350_gpio_probe(struct platform_device *pdev)
 	wm8350_gpio->wm8350 = wm8350;
 	wm8350_gpio->gpio_chip = template_chip;
 	wm8350_gpio->gpio_chip.ngpio = 13;
-	wm8350_gpio->gpio_chip.dev = &pdev->dev;
+	wm8350_gpio->gpio_chip.parent = &pdev->dev;
 	if (pdata && pdata->gpio_base)
 		wm8350_gpio->gpio_chip.base = pdata->gpio_base;
 	else
 		wm8350_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&wm8350_gpio->gpio_chip);
+	ret = gpiochip_add_data(&wm8350_gpio->gpio_chip, wm8350_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n", ret);
 		return ret;
diff --git a/drivers/gpio/gpio-wm8994.c b/drivers/gpio/gpio-wm8994.c
index 6f5e42db4b9e..3ae4c1597494 100644
--- a/drivers/gpio/gpio-wm8994.c
+++ b/drivers/gpio/gpio-wm8994.c
@@ -31,14 +31,9 @@ struct wm8994_gpio {
 	struct gpio_chip gpio_chip;
 };
 
-static inline struct wm8994_gpio *to_wm8994_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct wm8994_gpio, gpio_chip);
-}
-
 static int wm8994_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
 	switch (wm8994->type) {
@@ -61,7 +56,7 @@ static int wm8994_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static int wm8994_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
 	return wm8994_set_bits(wm8994, WM8994_GPIO_1 + offset,
@@ -70,7 +65,7 @@ static int wm8994_gpio_direction_in(struct gpio_chip *chip, unsigned offset)
 
 static int wm8994_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 	int ret;
 
@@ -87,7 +82,7 @@ static int wm8994_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int wm8994_gpio_direction_out(struct gpio_chip *chip,
 				     unsigned offset, int value)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
 	if (value)
@@ -99,7 +94,7 @@ static int wm8994_gpio_direction_out(struct gpio_chip *chip,
 
 static void wm8994_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
 	if (value)
@@ -110,7 +105,7 @@ static void wm8994_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int wm8994_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 
 	return regmap_irq_get_virq(wm8994->irq_data, offset);
@@ -174,7 +169,7 @@ static const char *wm8994_gpio_fn(u16 fn)
 
 static void wm8994_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct wm8994_gpio *wm8994_gpio = to_wm8994_gpio(chip);
+	struct wm8994_gpio *wm8994_gpio = gpiochip_get_data(chip);
 	struct wm8994 *wm8994 = wm8994_gpio->wm8994;
 	int i;
 
@@ -260,13 +255,13 @@ static int wm8994_gpio_probe(struct platform_device *pdev)
 	wm8994_gpio->wm8994 = wm8994;
 	wm8994_gpio->gpio_chip = template_chip;
 	wm8994_gpio->gpio_chip.ngpio = WM8994_GPIO_MAX;
-	wm8994_gpio->gpio_chip.dev = &pdev->dev;
+	wm8994_gpio->gpio_chip.parent = &pdev->dev;
 	if (pdata && pdata->gpio_base)
 		wm8994_gpio->gpio_chip.base = pdata->gpio_base;
 	else
 		wm8994_gpio->gpio_chip.base = -1;
 
-	ret = gpiochip_add(&wm8994_gpio->gpio_chip);
+	ret = gpiochip_add_data(&wm8994_gpio->gpio_chip, wm8994_gpio);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Could not register gpiochip, %d\n",
 			ret);
diff --git a/drivers/gpio/gpio-xgene-sb.c b/drivers/gpio/gpio-xgene-sb.c
index d57068b9083e..282004deb5d4 100644
--- a/drivers/gpio/gpio-xgene-sb.c
+++ b/drivers/gpio/gpio-xgene-sb.c
@@ -23,10 +23,8 @@
 #include <linux/io.h>
 #include <linux/platform_device.h>
 #include <linux/of_gpio.h>
-#include <linux/gpio.h>
 #include <linux/gpio/driver.h>
 #include <linux/acpi.h>
-#include <linux/basic_mmio_gpio.h>
 
 #include "gpiolib.h"
 
@@ -43,38 +41,31 @@
 
 /**
  * struct xgene_gpio_sb - GPIO-Standby private data structure.
- * @bgc:			memory-mapped GPIO controllers.
+ * @gc:				memory-mapped GPIO controllers.
  * @irq:			Mapping GPIO pins and interrupt number
  * nirq:			Number of GPIO pins that supports interrupt
  */
 struct xgene_gpio_sb {
-	struct bgpio_chip	bgc;
+	struct gpio_chip	gc;
 	u32 *irq;
 	u32 nirq;
 };
 
-static inline struct xgene_gpio_sb *to_xgene_gpio_sb(struct gpio_chip *gc)
-{
-	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-
-	return container_of(bgc, struct xgene_gpio_sb, bgc);
-}
-
-static void xgene_gpio_set_bit(struct bgpio_chip *bgc, void __iomem *reg, u32 gpio, int val)
+static void xgene_gpio_set_bit(struct gpio_chip *gc, void __iomem *reg, u32 gpio, int val)
 {
 	u32 data;
 
-	data = bgc->read_reg(reg);
+	data = gc->read_reg(reg);
 	if (val)
 		data |= GPIO_MASK(gpio);
 	else
 		data &= ~GPIO_MASK(gpio);
-	bgc->write_reg(reg, data);
+	gc->write_reg(reg, data);
 }
 
 static int apm_gpio_sb_to_irq(struct gpio_chip *gc, u32 gpio)
 {
-	struct xgene_gpio_sb *priv = to_xgene_gpio_sb(gc);
+	struct xgene_gpio_sb *priv = gpiochip_get_data(gc);
 
 	if (priv->irq[gpio])
 		return priv->irq[gpio];
@@ -99,15 +90,15 @@ static int xgene_gpio_sb_probe(struct platform_device *pdev)
 	if (IS_ERR(regs))
 		return PTR_ERR(regs);
 
-	ret = bgpio_init(&priv->bgc, &pdev->dev, 4,
+	ret = bgpio_init(&priv->gc, &pdev->dev, 4,
 			regs + MPA_GPIO_IN_ADDR,
 			regs + MPA_GPIO_OUT_ADDR, NULL,
 			regs + MPA_GPIO_OE_ADDR, NULL, 0);
         if (ret)
                 return ret;
 
-	priv->bgc.gc.to_irq = apm_gpio_sb_to_irq;
-	priv->bgc.gc.ngpio = XGENE_MAX_GPIO_DS;
+	priv->gc.to_irq = apm_gpio_sb_to_irq;
+	priv->gc.ngpio = XGENE_MAX_GPIO_DS;
 
 	priv->nirq = XGENE_MAX_GPIO_DS_IRQ;
 
@@ -118,14 +109,14 @@ static int xgene_gpio_sb_probe(struct platform_device *pdev)
 
 	for (i = 0; i < priv->nirq; i++) {
 		priv->irq[default_lines[i]] = platform_get_irq(pdev, i);
-		xgene_gpio_set_bit(&priv->bgc, regs + MPA_GPIO_SEL_LO,
+		xgene_gpio_set_bit(&priv->gc, regs + MPA_GPIO_SEL_LO,
                                    default_lines[i] * 2, 1);
-		xgene_gpio_set_bit(&priv->bgc, regs + MPA_GPIO_INT_LVL, i, 1);
+		xgene_gpio_set_bit(&priv->gc, regs + MPA_GPIO_INT_LVL, i, 1);
 	}
 
 	platform_set_drvdata(pdev, priv);
 
-	ret = gpiochip_add(&priv->bgc.gc);
+	ret = gpiochip_add_data(&priv->gc, priv);
 	if (ret)
 		dev_err(&pdev->dev, "failed to register X-Gene GPIO Standby driver\n");
 	else
@@ -133,7 +124,7 @@ static int xgene_gpio_sb_probe(struct platform_device *pdev)
 
 	if (priv->nirq > 0) {
 		/* Register interrupt handlers for gpio signaled acpi events */
-		acpi_gpiochip_request_interrupts(&priv->bgc.gc);
+		acpi_gpiochip_request_interrupts(&priv->gc);
 	}
 
 	return ret;
@@ -144,10 +135,11 @@ static int xgene_gpio_sb_remove(struct platform_device *pdev)
 	struct xgene_gpio_sb *priv = platform_get_drvdata(pdev);
 
 	if (priv->nirq > 0) {
-		acpi_gpiochip_free_interrupts(&priv->bgc.gc);
+		acpi_gpiochip_free_interrupts(&priv->gc);
 	}
 
-	return bgpio_remove(&priv->bgc);
+	gpiochip_remove(&priv->gc);
+	return 0;
 }
 
 static const struct of_device_id xgene_gpio_sb_of_match[] = {
diff --git a/drivers/gpio/gpio-xgene.c b/drivers/gpio/gpio-xgene.c
index 18a8182d4fec..592e9cdf9c53 100644
--- a/drivers/gpio/gpio-xgene.c
+++ b/drivers/gpio/gpio-xgene.c
@@ -47,14 +47,9 @@ struct xgene_gpio {
 #endif
 };
 
-static inline struct xgene_gpio *to_xgene_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct xgene_gpio, chip);
-}
-
 static int xgene_gpio_get(struct gpio_chip *gc, unsigned int offset)
 {
-	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	struct xgene_gpio *chip = gpiochip_get_data(gc);
 	unsigned long bank_offset;
 	u32 bit_offset;
 
@@ -65,7 +60,7 @@ static int xgene_gpio_get(struct gpio_chip *gc, unsigned int offset)
 
 static void __xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 {
-	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	struct xgene_gpio *chip = gpiochip_get_data(gc);
 	unsigned long bank_offset;
 	u32 setval, bit_offset;
 
@@ -82,7 +77,7 @@ static void __xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 
 static void xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 {
-	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	struct xgene_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -92,7 +87,7 @@ static void xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
 
 static int xgene_gpio_dir_in(struct gpio_chip *gc, unsigned int offset)
 {
-	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	struct xgene_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags, bank_offset;
 	u32 dirval, bit_offset;
 
@@ -113,7 +108,7 @@ static int xgene_gpio_dir_in(struct gpio_chip *gc, unsigned int offset)
 static int xgene_gpio_dir_out(struct gpio_chip *gc,
 					unsigned int offset, int val)
 {
-	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	struct xgene_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags, bank_offset;
 	u32 dirval, bit_offset;
 
@@ -188,7 +183,7 @@ static int xgene_gpio_probe(struct platform_device *pdev)
 	gpio->chip.ngpio = XGENE_MAX_GPIOS;
 
 	spin_lock_init(&gpio->lock);
-	gpio->chip.dev = &pdev->dev;
+	gpio->chip.parent = &pdev->dev;
 	gpio->chip.direction_input = xgene_gpio_dir_in;
 	gpio->chip.direction_output = xgene_gpio_dir_out;
 	gpio->chip.get = xgene_gpio_get;
@@ -198,7 +193,7 @@ static int xgene_gpio_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, gpio);
 
-	err = gpiochip_add(&gpio->chip);
+	err = gpiochip_add_data(&gpio->chip, gpio);
 	if (err) {
 		dev_err(&pdev->dev,
 			"failed to register gpiochip.\n");
diff --git a/drivers/gpio/gpio-xilinx.c b/drivers/gpio/gpio-xilinx.c
index d5284dfe01fe..d0fbb7f99523 100644
--- a/drivers/gpio/gpio-xilinx.c
+++ b/drivers/gpio/gpio-xilinx.c
@@ -92,8 +92,7 @@ static inline int xgpio_offset(struct xgpio_instance *chip, int gpio)
 static int xgpio_get(struct gpio_chip *gc, unsigned int gpio)
 {
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct xgpio_instance *chip =
-	    container_of(mm_gc, struct xgpio_instance, mmchip);
+	struct xgpio_instance *chip = gpiochip_get_data(gc);
 	u32 val;
 
 	val = xgpio_readreg(mm_gc->regs + XGPIO_DATA_OFFSET +
@@ -115,8 +114,7 @@ static void xgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	unsigned long flags;
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct xgpio_instance *chip =
-	    container_of(mm_gc, struct xgpio_instance, mmchip);
+	struct xgpio_instance *chip = gpiochip_get_data(gc);
 	int index =  xgpio_index(chip, gpio);
 	int offset =  xgpio_offset(chip, gpio);
 
@@ -147,8 +145,7 @@ static int xgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	unsigned long flags;
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct xgpio_instance *chip =
-	    container_of(mm_gc, struct xgpio_instance, mmchip);
+	struct xgpio_instance *chip = gpiochip_get_data(gc);
 	int index =  xgpio_index(chip, gpio);
 	int offset =  xgpio_offset(chip, gpio);
 
@@ -180,8 +177,7 @@ static int xgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 {
 	unsigned long flags;
 	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
-	struct xgpio_instance *chip =
-	    container_of(mm_gc, struct xgpio_instance, mmchip);
+	struct xgpio_instance *chip = gpiochip_get_data(gc);
 	int index =  xgpio_index(chip, gpio);
 	int offset =  xgpio_offset(chip, gpio);
 
@@ -212,7 +208,7 @@ static int xgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 static void xgpio_save_regs(struct of_mm_gpio_chip *mm_gc)
 {
 	struct xgpio_instance *chip =
-	    container_of(mm_gc, struct xgpio_instance, mmchip);
+		container_of(mm_gc, struct xgpio_instance, mmchip);
 
 	xgpio_writereg(mm_gc->regs + XGPIO_DATA_OFFSET,	chip->gpio_state[0]);
 	xgpio_writereg(mm_gc->regs + XGPIO_TRI_OFFSET, chip->gpio_dir[0]);
@@ -305,7 +301,7 @@ static int xgpio_probe(struct platform_device *pdev)
 	}
 
 	chip->mmchip.gc.ngpio = chip->gpio_width[0] + chip->gpio_width[1];
-	chip->mmchip.gc.dev = &pdev->dev;
+	chip->mmchip.gc.parent = &pdev->dev;
 	chip->mmchip.gc.direction_input = xgpio_dir_in;
 	chip->mmchip.gc.direction_output = xgpio_dir_out;
 	chip->mmchip.gc.get = xgpio_get;
@@ -314,7 +310,7 @@ static int xgpio_probe(struct platform_device *pdev)
 	chip->mmchip.save_regs = xgpio_save_regs;
 
 	/* Call the OF gpio helper to setup and register the GPIO device */
-	status = of_mm_gpiochip_add(np, &chip->mmchip);
+	status = of_mm_gpiochip_add_data(np, &chip->mmchip, chip);
 	if (status) {
 		pr_err("%s: error in probe function with status %d\n",
 		       np->full_name, status);
diff --git a/drivers/gpio/gpio-xlp.c b/drivers/gpio/gpio-xlp.c
index bc06a2cd2c1d..aa5813d2deb1 100644
--- a/drivers/gpio/gpio-xlp.c
+++ b/drivers/gpio/gpio-xlp.c
@@ -100,11 +100,6 @@ struct xlp_gpio_priv {
 	spinlock_t lock;
 };
 
-static struct xlp_gpio_priv *gpio_chip_to_xlp_priv(struct gpio_chip *gc)
-{
-	return container_of(gc, struct xlp_gpio_priv, chip);
-}
-
 static int xlp_gpio_get_reg(void __iomem *addr, unsigned gpio)
 {
 	u32 pos, regset;
@@ -133,7 +128,7 @@ static void xlp_gpio_set_reg(void __iomem *addr, unsigned gpio, int state)
 static void xlp_gpio_irq_disable(struct irq_data *d)
 {
 	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -145,7 +140,7 @@ static void xlp_gpio_irq_disable(struct irq_data *d)
 static void xlp_gpio_irq_mask_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -158,7 +153,7 @@ static void xlp_gpio_irq_mask_ack(struct irq_data *d)
 static void xlp_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -170,7 +165,7 @@ static void xlp_gpio_irq_unmask(struct irq_data *d)
 static int xlp_gpio_set_irq_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc  = irq_data_get_irq_chip_data(d);
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 	int pol, irq_type;
 
 	switch (type) {
@@ -235,7 +230,7 @@ static void xlp_gpio_generic_handler(struct irq_desc *desc)
 
 static int xlp_gpio_dir_output(struct gpio_chip *gc, unsigned gpio, int state)
 {
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 
 	BUG_ON(gpio >= gc->ngpio);
 	xlp_gpio_set_reg(priv->gpio_out_en, gpio, 0x1);
@@ -245,7 +240,7 @@ static int xlp_gpio_dir_output(struct gpio_chip *gc, unsigned gpio, int state)
 
 static int xlp_gpio_dir_input(struct gpio_chip *gc, unsigned gpio)
 {
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 
 	BUG_ON(gpio >= gc->ngpio);
 	xlp_gpio_set_reg(priv->gpio_out_en, gpio, 0x0);
@@ -255,7 +250,7 @@ static int xlp_gpio_dir_input(struct gpio_chip *gc, unsigned gpio)
 
 static int xlp_gpio_get(struct gpio_chip *gc, unsigned gpio)
 {
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 
 	BUG_ON(gpio >= gc->ngpio);
 	return xlp_gpio_get_reg(priv->gpio_paddrv, gpio);
@@ -263,7 +258,7 @@ static int xlp_gpio_get(struct gpio_chip *gc, unsigned gpio)
 
 static void xlp_gpio_set(struct gpio_chip *gc, unsigned gpio, int state)
 {
-	struct xlp_gpio_priv *priv = gpio_chip_to_xlp_priv(gc);
+	struct xlp_gpio_priv *priv = gpiochip_get_data(gc);
 
 	BUG_ON(gpio >= gc->ngpio);
 	xlp_gpio_set_reg(priv->gpio_paddrv, gpio, state);
@@ -373,7 +368,7 @@ static int xlp_gpio_probe(struct platform_device *pdev)
 	gc->owner = THIS_MODULE;
 	gc->label = dev_name(&pdev->dev);
 	gc->base = 0;
-	gc->dev = &pdev->dev;
+	gc->parent = &pdev->dev;
 	gc->ngpio = ngpio;
 	gc->of_node = pdev->dev.of_node;
 	gc->direction_output = xlp_gpio_dir_output;
@@ -388,7 +383,7 @@ static int xlp_gpio_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
-	err = gpiochip_add(gc);
+	err = gpiochip_add_data(gc, priv);
 	if (err < 0)
 		goto out_free_desc;
 
diff --git a/drivers/gpio/gpio-xtensa.c b/drivers/gpio/gpio-xtensa.c
index 93ec95df67a3..f16c0427952e 100644
--- a/drivers/gpio/gpio-xtensa.c
+++ b/drivers/gpio/gpio-xtensa.c
@@ -148,10 +148,10 @@ static int xtensa_gpio_probe(struct platform_device *pdev)
 {
 	int ret;
 
-	ret = gpiochip_add(&impwire_chip);
+	ret = gpiochip_add_data(&impwire_chip, NULL);
 	if (ret)
 		return ret;
-	return gpiochip_add(&expstate_chip);
+	return gpiochip_add_data(&expstate_chip, NULL);
 }
 
 static struct platform_driver xtensa_gpio_driver = {
diff --git a/drivers/gpio/gpio-zevio.c b/drivers/gpio/gpio-zevio.c
index 6f02d7c4cc57..cda6d922be98 100644
--- a/drivers/gpio/gpio-zevio.c
+++ b/drivers/gpio/gpio-zevio.c
@@ -52,9 +52,6 @@
 #define ZEVIO_GPIO_INPUT			0x18
 #define ZEVIO_GPIO_INT_STICKY		0x20
 
-#define to_zevio_gpio(chip) container_of(to_of_mm_gpio_chip(chip), \
-				struct zevio_gpio, chip)
-
 /* Bit number of GPIO in its section */
 #define ZEVIO_GPIO_BIT(gpio) (gpio&7)
 
@@ -80,7 +77,7 @@ static inline void zevio_gpio_port_set(struct zevio_gpio *c, unsigned pin,
 /* Functions for struct gpio_chip */
 static int zevio_gpio_get(struct gpio_chip *chip, unsigned pin)
 {
-	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	struct zevio_gpio *controller = gpiochip_get_data(chip);
 	u32 val, dir;
 
 	spin_lock(&controller->lock);
@@ -96,7 +93,7 @@ static int zevio_gpio_get(struct gpio_chip *chip, unsigned pin)
 
 static void zevio_gpio_set(struct gpio_chip *chip, unsigned pin, int value)
 {
-	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	struct zevio_gpio *controller = gpiochip_get_data(chip);
 	u32 val;
 
 	spin_lock(&controller->lock);
@@ -112,7 +109,7 @@ static void zevio_gpio_set(struct gpio_chip *chip, unsigned pin, int value)
 
 static int zevio_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 {
-	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	struct zevio_gpio *controller = gpiochip_get_data(chip);
 	u32 val;
 
 	spin_lock(&controller->lock);
@@ -129,7 +126,7 @@ static int zevio_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 static int zevio_gpio_direction_output(struct gpio_chip *chip,
 				       unsigned pin, int value)
 {
-	struct zevio_gpio *controller = to_zevio_gpio(chip);
+	struct zevio_gpio *controller = gpiochip_get_data(chip);
 	u32 val;
 
 	spin_lock(&controller->lock);
@@ -185,9 +182,11 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 
 	/* Copy our reference */
 	controller->chip.gc = zevio_gpio_chip;
-	controller->chip.gc.dev = &pdev->dev;
+	controller->chip.gc.parent = &pdev->dev;
 
-	status = of_mm_gpiochip_add(pdev->dev.of_node, &(controller->chip));
+	status = of_mm_gpiochip_add_data(pdev->dev.of_node,
+					 &(controller->chip),
+					 controller);
 	if (status) {
 		dev_err(&pdev->dev, "failed to add gpiochip: %d\n", status);
 		return status;
@@ -199,7 +198,7 @@ static int zevio_gpio_probe(struct platform_device *pdev)
 	for (i = 0; i < controller->chip.gc.ngpio; i += 8)
 		zevio_gpio_port_set(controller, i, ZEVIO_GPIO_INT_MASK, 0xFF);
 
-	dev_dbg(controller->chip.gc.dev, "ZEVIO GPIO controller set up!\n");
+	dev_dbg(controller->chip.gc.parent, "ZEVIO GPIO controller set up!\n");
 
 	return 0;
 }
diff --git a/drivers/gpio/gpio-zx.c b/drivers/gpio/gpio-zx.c
index 1dcf7a66dd36..47c79fa65670 100644
--- a/drivers/gpio/gpio-zx.c
+++ b/drivers/gpio/gpio-zx.c
@@ -43,14 +43,9 @@ struct zx_gpio {
 	struct gpio_chip	gc;
 };
 
-static inline struct zx_gpio *to_zx(struct gpio_chip *gc)
-{
-	return container_of(gc, struct zx_gpio, gc);
-}
-
 static int zx_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 	u16 gpiodir;
 
@@ -69,7 +64,7 @@ static int zx_direction_input(struct gpio_chip *gc, unsigned offset)
 static int zx_direction_output(struct gpio_chip *gc, unsigned offset,
 		int value)
 {
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	unsigned long flags;
 	u16 gpiodir;
 
@@ -92,14 +87,14 @@ static int zx_direction_output(struct gpio_chip *gc, unsigned offset,
 
 static int zx_get_value(struct gpio_chip *gc, unsigned offset)
 {
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 
 	return !!(readw_relaxed(chip->base + ZX_GPIO_DI) & BIT(offset));
 }
 
 static void zx_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 
 	if (value)
 		writew_relaxed(BIT(offset), chip->base + ZX_GPIO_DO1);
@@ -110,7 +105,7 @@ static void zx_set_value(struct gpio_chip *gc, unsigned offset, int value)
 static int zx_irq_type(struct irq_data *d, unsigned trigger)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	int offset = irqd_to_hwirq(d);
 	unsigned long flags;
 	u16 gpiois, gpioi_epos, gpioi_eneg, gpioiev;
@@ -162,7 +157,7 @@ static void zx_irq_handler(struct irq_desc *desc)
 	unsigned long pending;
 	int offset;
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	struct irq_chip *irqchip = irq_desc_get_chip(desc);
 
 	chained_irq_enter(irqchip, desc);
@@ -181,7 +176,7 @@ static void zx_irq_handler(struct irq_desc *desc)
 static void zx_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	u16 mask = BIT(irqd_to_hwirq(d) % ZX_GPIO_NR);
 	u16 gpioie;
 
@@ -196,7 +191,7 @@ static void zx_irq_mask(struct irq_data *d)
 static void zx_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct zx_gpio *chip = to_zx(gc);
+	struct zx_gpio *chip = gpiochip_get_data(gc);
 	u16 mask = BIT(irqd_to_hwirq(d) % ZX_GPIO_NR);
 	u16 gpioie;
 
@@ -245,10 +240,10 @@ static int zx_gpio_probe(struct platform_device *pdev)
 	chip->gc.base = ZX_GPIO_NR * id;
 	chip->gc.ngpio = ZX_GPIO_NR;
 	chip->gc.label = dev_name(dev);
-	chip->gc.dev = dev;
+	chip->gc.parent = dev;
 	chip->gc.owner = THIS_MODULE;
 
-	ret = gpiochip_add(&chip->gc);
+	ret = gpiochip_add_data(&chip->gc, chip);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-zynq.c b/drivers/gpio/gpio-zynq.c
index 8abeacac5885..66d3d247d76d 100644
--- a/drivers/gpio/gpio-zynq.c
+++ b/drivers/gpio/gpio-zynq.c
@@ -131,11 +131,6 @@ struct zynq_platform_data {
 static struct irq_chip zynq_gpio_level_irqchip;
 static struct irq_chip zynq_gpio_edge_irqchip;
 
-static struct zynq_gpio *to_zynq_gpio(struct gpio_chip *gc)
-{
-	return container_of(gc, struct zynq_gpio, chip);
-}
-
 /**
  * zynq_gpio_get_bank_pin - Get the bank number and pin number within that bank
  * for a given pin in the GPIO device
@@ -183,7 +178,7 @@ static int zynq_gpio_get_value(struct gpio_chip *chip, unsigned int pin)
 {
 	u32 data;
 	unsigned int bank_num, bank_pin_num;
-	struct zynq_gpio *gpio = to_zynq_gpio(chip);
+	struct zynq_gpio *gpio = gpiochip_get_data(chip);
 
 	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
@@ -207,7 +202,7 @@ static void zynq_gpio_set_value(struct gpio_chip *chip, unsigned int pin,
 				int state)
 {
 	unsigned int reg_offset, bank_num, bank_pin_num;
-	struct zynq_gpio *gpio = to_zynq_gpio(chip);
+	struct zynq_gpio *gpio = gpiochip_get_data(chip);
 
 	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
@@ -244,7 +239,7 @@ static int zynq_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
 {
 	u32 reg;
 	unsigned int bank_num, bank_pin_num;
-	struct zynq_gpio *gpio = to_zynq_gpio(chip);
+	struct zynq_gpio *gpio = gpiochip_get_data(chip);
 
 	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
@@ -277,7 +272,7 @@ static int zynq_gpio_dir_out(struct gpio_chip *chip, unsigned int pin,
 {
 	u32 reg;
 	unsigned int bank_num, bank_pin_num;
-	struct zynq_gpio *gpio = to_zynq_gpio(chip);
+	struct zynq_gpio *gpio = gpiochip_get_data(chip);
 
 	zynq_gpio_get_bank_pin(pin, &bank_num, &bank_pin_num, gpio);
 
@@ -308,7 +303,7 @@ static void zynq_gpio_irq_mask(struct irq_data *irq_data)
 {
 	unsigned int device_pin_num, bank_num, bank_pin_num;
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_data_get_irq_chip_data(irq_data));
+		gpiochip_get_data(irq_data_get_irq_chip_data(irq_data));
 
 	device_pin_num = irq_data->hwirq;
 	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
@@ -329,7 +324,7 @@ static void zynq_gpio_irq_unmask(struct irq_data *irq_data)
 {
 	unsigned int device_pin_num, bank_num, bank_pin_num;
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_data_get_irq_chip_data(irq_data));
+		gpiochip_get_data(irq_data_get_irq_chip_data(irq_data));
 
 	device_pin_num = irq_data->hwirq;
 	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
@@ -349,7 +344,7 @@ static void zynq_gpio_irq_ack(struct irq_data *irq_data)
 {
 	unsigned int device_pin_num, bank_num, bank_pin_num;
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_data_get_irq_chip_data(irq_data));
+		gpiochip_get_data(irq_data_get_irq_chip_data(irq_data));
 
 	device_pin_num = irq_data->hwirq;
 	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
@@ -400,7 +395,7 @@ static int zynq_gpio_set_irq_type(struct irq_data *irq_data, unsigned int type)
 	u32 int_type, int_pol, int_any;
 	unsigned int device_pin_num, bank_num, bank_pin_num;
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_data_get_irq_chip_data(irq_data));
+		gpiochip_get_data(irq_data_get_irq_chip_data(irq_data));
 
 	device_pin_num = irq_data->hwirq;
 	zynq_gpio_get_bank_pin(device_pin_num, &bank_num, &bank_pin_num, gpio);
@@ -464,7 +459,7 @@ static int zynq_gpio_set_irq_type(struct irq_data *irq_data, unsigned int type)
 static int zynq_gpio_set_wake(struct irq_data *data, unsigned int on)
 {
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_data_get_irq_chip_data(data));
+		gpiochip_get_data(irq_data_get_irq_chip_data(data));
 
 	irq_set_irq_wake(gpio->irq, on);
 
@@ -530,7 +525,7 @@ static void zynq_gpio_irqhandler(struct irq_desc *desc)
 	u32 int_sts, int_enb;
 	unsigned int bank_num;
 	struct zynq_gpio *gpio =
-		to_zynq_gpio(irq_desc_get_handler_data(desc));
+		gpiochip_get_data(irq_desc_get_handler_data(desc));
 	struct irq_chip *irqchip = irq_desc_get_chip(desc);
 
 	chained_irq_enter(irqchip, desc);
@@ -592,7 +587,7 @@ static int zynq_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
 	int ret;
 
-	ret = pm_runtime_get_sync(chip->dev);
+	ret = pm_runtime_get_sync(chip->parent);
 
 	/*
 	 * If the device is already active pm_runtime_get() will return 1 on
@@ -603,7 +598,7 @@ static int zynq_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void zynq_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	pm_runtime_put(chip->dev);
+	pm_runtime_put(chip->parent);
 }
 
 static const struct dev_pm_ops zynq_gpio_dev_pm_ops = {
@@ -698,7 +693,7 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	chip = &gpio->chip;
 	chip->label = gpio->p_data->label;
 	chip->owner = THIS_MODULE;
-	chip->dev = &pdev->dev;
+	chip->parent = &pdev->dev;
 	chip->get = zynq_gpio_get_value;
 	chip->set = zynq_gpio_set_value;
 	chip->request = zynq_gpio_request;
@@ -708,23 +703,23 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	chip->base = -1;
 	chip->ngpio = gpio->p_data->ngpio;
 
-	/* Enable GPIO clock */
+	/* Retrieve GPIO clock */
 	gpio->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(gpio->clk)) {
 		dev_err(&pdev->dev, "input clock not found.\n");
 		return PTR_ERR(gpio->clk);
 	}
-	ret = clk_prepare_enable(gpio->clk);
-	if (ret) {
-		dev_err(&pdev->dev, "Unable to enable clock.\n");
+
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
 		return ret;
-	}
 
 	/* report a bug if gpio chip registration fails */
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, gpio);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to add gpio chip\n");
-		goto err_disable_clk;
+		goto err_pm_put;
 	}
 
 	/* disable interrupts for all banks */
@@ -742,15 +737,14 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	gpiochip_set_chained_irqchip(chip, &zynq_gpio_edge_irqchip, gpio->irq,
 				     zynq_gpio_irqhandler);
 
-	pm_runtime_set_active(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
+	pm_runtime_put(&pdev->dev);
 
 	return 0;
 
 err_rm_gpiochip:
 	gpiochip_remove(chip);
-err_disable_clk:
-	clk_disable_unprepare(gpio->clk);
+err_pm_put:
+	pm_runtime_put(&pdev->dev);
 
 	return ret;
 }
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index 7ab7bc3c9300..540cbc88c7a2 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -51,10 +51,10 @@ struct acpi_gpio_chip {
 
 static int acpi_gpiochip_find(struct gpio_chip *gc, void *data)
 {
-	if (!gc->dev)
+	if (!gc->parent)
 		return false;
 
-	return ACPI_HANDLE(gc->dev) == data;
+	return ACPI_HANDLE(gc->parent) == data;
 }
 
 #ifdef CONFIG_PINCTRL
@@ -184,7 +184,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 	if (agpio->connection_type != ACPI_RESOURCE_GPIO_TYPE_INT)
 		return AE_OK;
 
-	handle = ACPI_HANDLE(chip->dev);
+	handle = ACPI_HANDLE(chip->parent);
 	pin = agpio->pin_table[0];
 
 	if (pin <= 255) {
@@ -208,7 +208,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 
 	desc = gpiochip_request_own_desc(chip, pin, "ACPI:Event");
 	if (IS_ERR(desc)) {
-		dev_err(chip->dev, "Failed to request GPIO\n");
+		dev_err(chip->parent, "Failed to request GPIO\n");
 		return AE_ERROR;
 	}
 
@@ -216,13 +216,13 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 
 	ret = gpiochip_lock_as_irq(chip, pin);
 	if (ret) {
-		dev_err(chip->dev, "Failed to lock GPIO as interrupt\n");
+		dev_err(chip->parent, "Failed to lock GPIO as interrupt\n");
 		goto fail_free_desc;
 	}
 
 	irq = gpiod_to_irq(desc);
 	if (irq < 0) {
-		dev_err(chip->dev, "Failed to translate GPIO to IRQ\n");
+		dev_err(chip->parent, "Failed to translate GPIO to IRQ\n");
 		goto fail_unlock_irq;
 	}
 
@@ -259,7 +259,8 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 	ret = request_threaded_irq(event->irq, NULL, handler, irqflags,
 				   "ACPI:Event", event);
 	if (ret) {
-		dev_err(chip->dev, "Failed to setup interrupt handler for %d\n",
+		dev_err(chip->parent,
+			"Failed to setup interrupt handler for %d\n",
 			event->irq);
 		goto fail_free_event;
 	}
@@ -293,10 +294,10 @@ void acpi_gpiochip_request_interrupts(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
-	if (!chip->dev || !chip->to_irq)
+	if (!chip->parent || !chip->to_irq)
 		return;
 
-	handle = ACPI_HANDLE(chip->dev);
+	handle = ACPI_HANDLE(chip->parent);
 	if (!handle)
 		return;
 
@@ -323,10 +324,10 @@ void acpi_gpiochip_free_interrupts(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
-	if (!chip->dev || !chip->to_irq)
+	if (!chip->parent || !chip->to_irq)
 		return;
 
-	handle = ACPI_HANDLE(chip->dev);
+	handle = ACPI_HANDLE(chip->parent);
 	if (!handle)
 		return;
 
@@ -417,10 +418,15 @@ static int acpi_find_gpio(struct acpi_resource *ares, void *data)
 		 * ActiveLow is only specified for GpioInt resource. If
 		 * GpioIo is used then the only way to set the flag is
 		 * to use _DSD "gpios" property.
+		 * Note: we expect here:
+		 * - ACPI_ACTIVE_LOW == GPIO_ACTIVE_LOW
+		 * - ACPI_ACTIVE_HIGH == GPIO_ACTIVE_HIGH
 		 */
-		if (lookup->info.gpioint)
-			lookup->info.active_low =
-				agpio->polarity == ACPI_ACTIVE_LOW;
+		if (lookup->info.gpioint) {
+			lookup->info.polarity = agpio->polarity;
+			lookup->info.triggering = agpio->triggering;
+		}
+
 	}
 
 	return 1;
@@ -447,7 +453,7 @@ static int acpi_gpio_resource_lookup(struct acpi_gpio_lookup *lookup,
 	if (info) {
 		*info = lookup->info;
 		if (lookup->active_low)
-			info->active_low = lookup->active_low;
+			info->polarity = lookup->active_low;
 	}
 	return 0;
 }
@@ -595,6 +601,7 @@ struct gpio_desc *acpi_node_get_gpiod(struct fwnode_handle *fwnode,
 int acpi_dev_gpio_irq_get(struct acpi_device *adev, int index)
 {
 	int idx, i;
+	unsigned int irq_flags;
 
 	for (i = 0, idx = 0; idx <= index; i++) {
 		struct acpi_gpio_info info;
@@ -603,8 +610,23 @@ int acpi_dev_gpio_irq_get(struct acpi_device *adev, int index)
 		desc = acpi_get_gpiod_by_index(adev, NULL, i, &info);
 		if (IS_ERR(desc))
 			break;
-		if (info.gpioint && idx++ == index)
-			return gpiod_to_irq(desc);
+		if (info.gpioint && idx++ == index) {
+			int irq = gpiod_to_irq(desc);
+
+			if (irq < 0)
+				return irq;
+
+			irq_flags = acpi_dev_get_irq_type(info.triggering,
+							  info.polarity);
+
+			/* Set type if specified and different than the current one */
+			if (irq_flags != IRQ_TYPE_NONE &&
+			    irq_flags != irq_get_trigger_type(irq))
+				irq_set_irq_type(irq, irq_flags);
+
+			return irq;
+		}
+
 	}
 	return -ENOENT;
 }
@@ -748,7 +770,7 @@ out:
 static void acpi_gpiochip_request_regions(struct acpi_gpio_chip *achip)
 {
 	struct gpio_chip *chip = achip->chip;
-	acpi_handle handle = ACPI_HANDLE(chip->dev);
+	acpi_handle handle = ACPI_HANDLE(chip->parent);
 	acpi_status status;
 
 	INIT_LIST_HEAD(&achip->conns);
@@ -757,20 +779,22 @@ static void acpi_gpiochip_request_regions(struct acpi_gpio_chip *achip)
 						    acpi_gpio_adr_space_handler,
 						    NULL, achip);
 	if (ACPI_FAILURE(status))
-		dev_err(chip->dev, "Failed to install GPIO OpRegion handler\n");
+		dev_err(chip->parent,
+		        "Failed to install GPIO OpRegion handler\n");
 }
 
 static void acpi_gpiochip_free_regions(struct acpi_gpio_chip *achip)
 {
 	struct gpio_chip *chip = achip->chip;
-	acpi_handle handle = ACPI_HANDLE(chip->dev);
+	acpi_handle handle = ACPI_HANDLE(chip->parent);
 	struct acpi_gpio_connection *conn, *tmp;
 	acpi_status status;
 
 	status = acpi_remove_address_space_handler(handle, ACPI_ADR_SPACE_GPIO,
 						   acpi_gpio_adr_space_handler);
 	if (ACPI_FAILURE(status)) {
-		dev_err(chip->dev, "Failed to remove GPIO OpRegion handler\n");
+		dev_err(chip->parent,
+			"Failed to remove GPIO OpRegion handler\n");
 		return;
 	}
 
@@ -787,16 +811,16 @@ void acpi_gpiochip_add(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
-	if (!chip || !chip->dev)
+	if (!chip || !chip->parent)
 		return;
 
-	handle = ACPI_HANDLE(chip->dev);
+	handle = ACPI_HANDLE(chip->parent);
 	if (!handle)
 		return;
 
 	acpi_gpio = kzalloc(sizeof(*acpi_gpio), GFP_KERNEL);
 	if (!acpi_gpio) {
-		dev_err(chip->dev,
+		dev_err(chip->parent,
 			"Failed to allocate memory for ACPI GPIO chip\n");
 		return;
 	}
@@ -806,7 +830,7 @@ void acpi_gpiochip_add(struct gpio_chip *chip)
 
 	status = acpi_attach_data(handle, acpi_gpio_chip_dh, acpi_gpio);
 	if (ACPI_FAILURE(status)) {
-		dev_err(chip->dev, "Failed to attach ACPI GPIO chip\n");
+		dev_err(chip->parent, "Failed to attach ACPI GPIO chip\n");
 		kfree(acpi_gpio);
 		return;
 	}
@@ -820,16 +844,16 @@ void acpi_gpiochip_remove(struct gpio_chip *chip)
 	acpi_handle handle;
 	acpi_status status;
 
-	if (!chip || !chip->dev)
+	if (!chip || !chip->parent)
 		return;
 
-	handle = ACPI_HANDLE(chip->dev);
+	handle = ACPI_HANDLE(chip->parent);
 	if (!handle)
 		return;
 
 	status = acpi_get_data(handle, acpi_gpio_chip_dh, (void **)&acpi_gpio);
 	if (ACPI_FAILURE(status)) {
-		dev_warn(chip->dev, "Failed to retrieve ACPI GPIO chip\n");
+		dev_warn(chip->parent, "Failed to retrieve ACPI GPIO chip\n");
 		return;
 	}
 
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 5fe34a9df3e6..42a4bb7cf49a 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -262,9 +262,10 @@ int of_gpio_simple_xlate(struct gpio_chip *gc,
 EXPORT_SYMBOL(of_gpio_simple_xlate);
 
 /**
- * of_mm_gpiochip_add - Add memory mapped GPIO chip (bank)
+ * of_mm_gpiochip_add_data - Add memory mapped GPIO chip (bank)
  * @np:		device node of the GPIO chip
  * @mm_gc:	pointer to the of_mm_gpio_chip allocated structure
+ * @data:	driver data to store in the struct gpio_chip
  *
  * To use this function you should allocate and fill mm_gc with:
  *
@@ -280,8 +281,9 @@ EXPORT_SYMBOL(of_gpio_simple_xlate);
  * do all necessary work for you. Then you'll able to use .regs
  * to manage GPIOs from the callbacks.
  */
-int of_mm_gpiochip_add(struct device_node *np,
-		       struct of_mm_gpio_chip *mm_gc)
+int of_mm_gpiochip_add_data(struct device_node *np,
+			    struct of_mm_gpio_chip *mm_gc,
+			    void *data)
 {
 	int ret = -ENOMEM;
 	struct gpio_chip *gc = &mm_gc->gc;
@@ -301,7 +303,7 @@ int of_mm_gpiochip_add(struct device_node *np,
 
 	mm_gc->gc.of_node = np;
 
-	ret = gpiochip_add(gc);
+	ret = gpiochip_add_data(gc, data);
 	if (ret)
 		goto err2;
 
@@ -315,7 +317,7 @@ err0:
 	       np->full_name, ret);
 	return ret;
 }
-EXPORT_SYMBOL(of_mm_gpiochip_add);
+EXPORT_SYMBOL(of_mm_gpiochip_add_data);
 
 /**
  * of_mm_gpiochip_remove - Remove memory mapped GPIO chip (bank)
@@ -423,8 +425,8 @@ int of_gpiochip_add(struct gpio_chip *chip)
 {
 	int status;
 
-	if ((!chip->of_node) && (chip->dev))
-		chip->of_node = chip->dev->of_node;
+	if ((!chip->of_node) && (chip->parent))
+		chip->of_node = chip->parent->of_node;
 
 	if (!chip->of_node)
 		return 0;
diff --git a/drivers/gpio/gpiolib-sysfs.c b/drivers/gpio/gpiolib-sysfs.c
index b57ed8e55ab5..405dfcaadc4c 100644
--- a/drivers/gpio/gpiolib-sysfs.c
+++ b/drivers/gpio/gpiolib-sysfs.c
@@ -605,7 +605,7 @@ int gpiod_export(struct gpio_desc *desc, bool direction_may_change)
 	if (chip->names && chip->names[offset])
 		ioname = chip->names[offset];
 
-	dev = device_create_with_groups(&gpio_class, chip->dev,
+	dev = device_create_with_groups(&gpio_class, chip->parent,
 					MKDEV(0, 0), data, gpio_groups,
 					ioname ? ioname : "gpio%u",
 					desc_to_gpio(desc));
@@ -730,7 +730,8 @@ int gpiochip_sysfs_register(struct gpio_chip *chip)
 		return 0;
 
 	/* use chip->base for the ID; it's already known to be unique */
-	dev = device_create_with_groups(&gpio_class, chip->dev, MKDEV(0, 0),
+	dev = device_create_with_groups(&gpio_class, chip->parent,
+					MKDEV(0, 0),
 					chip, gpiochip_groups,
 					"gpiochip%d", chip->base);
 	if (IS_ERR(dev))
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index a3ad3bab2942..5c1ba879f889 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -182,39 +182,62 @@ EXPORT_SYMBOL_GPL(gpiod_get_direction);
 
 /*
  * Add a new chip to the global chips list, keeping the list of chips sorted
- * by base order.
+ * by range(means [base, base + ngpio - 1]) order.
  *
  * Return -EBUSY if the new chip overlaps with some other chip's integer
  * space.
  */
 static int gpiochip_add_to_list(struct gpio_chip *chip)
 {
-	struct list_head *pos;
-	struct gpio_chip *_chip;
-	int err = 0;
+	struct gpio_chip *iterator;
+	struct gpio_chip *previous = NULL;
 
-	/* find where to insert our chip */
-	list_for_each(pos, &gpio_chips) {
-		_chip = list_entry(pos, struct gpio_chip, list);
-		/* shall we insert before _chip? */
-		if (_chip->base >= chip->base + chip->ngpio)
-			break;
+	if (list_empty(&gpio_chips)) {
+		list_add_tail(&chip->list, &gpio_chips);
+		return 0;
 	}
 
-	/* are we stepping on the chip right before? */
-	if (pos != &gpio_chips && pos->prev != &gpio_chips) {
-		_chip = list_entry(pos->prev, struct gpio_chip, list);
-		if (_chip->base + _chip->ngpio > chip->base) {
-			dev_err(chip->dev,
-			       "GPIO integer space overlap, cannot add chip\n");
-			err = -EBUSY;
+	list_for_each_entry(iterator, &gpio_chips, list) {
+		if (iterator->base >= chip->base + chip->ngpio) {
+			/*
+			 * Iterator is the first GPIO chip so there is no
+			 * previous one
+			 */
+			if (!previous) {
+				goto found;
+			} else {
+				/*
+				 * We found a valid range(means
+				 * [base, base + ngpio - 1]) between previous
+				 * and iterator chip.
+				 */
+				if (previous->base + previous->ngpio
+						<= chip->base)
+					goto found;
+			}
 		}
+		previous = iterator;
+	}
+
+	/*
+	 * We are beyond the last chip in the list and iterator now
+	 * points to the head.
+	 * Let iterator point to the last chip in the list.
+	 */
+
+	iterator = list_last_entry(&gpio_chips, struct gpio_chip, list);
+	if (iterator->base + iterator->ngpio <= chip->base) {
+		list_add(&chip->list, &iterator->list);
+		return 0;
 	}
 
-	if (!err)
-		list_add_tail(&chip->list, pos);
+	dev_err(chip->parent,
+	       "GPIO integer space overlap, cannot add chip\n");
+	return -EBUSY;
 
-	return err;
+found:
+	list_add_tail(&chip->list, &iterator->list);
+	return 0;
 }
 
 /**
@@ -252,7 +275,7 @@ static struct gpio_desc *gpio_name_to_desc(const char * const name)
  * Takes the names from gc->names and checks if they are all unique. If they
  * are, they are assigned to their gpio descriptors.
  *
- * Returns -EEXIST if one of the names is already used for a different GPIO.
+ * Warning if one of the names is already used for a different GPIO.
  */
 static int gpiochip_set_desc_names(struct gpio_chip *gc)
 {
@@ -267,7 +290,7 @@ static int gpiochip_set_desc_names(struct gpio_chip *gc)
 
 		gpio = gpio_name_to_desc(gc->names[i]);
 		if (gpio)
-			dev_warn(gc->dev, "Detected name collision for "
+			dev_warn(gc->parent, "Detected name collision for "
 				 "GPIO name '%s'\n",
 				 gc->names[i]);
 	}
@@ -280,7 +303,7 @@ static int gpiochip_set_desc_names(struct gpio_chip *gc)
 }
 
 /**
- * gpiochip_add() - register a gpio_chip
+ * gpiochip_add_data() - register a gpio_chip
  * @chip: the chip to register, with chip->base initialized
  * Context: potentially before irqs will work
  *
@@ -288,15 +311,15 @@ static int gpiochip_set_desc_names(struct gpio_chip *gc)
  * because the chip->base is invalid or already associated with a
  * different chip.  Otherwise it returns zero as a success code.
  *
- * When gpiochip_add() is called very early during boot, so that GPIOs
- * can be freely used, the chip->dev device must be registered before
+ * When gpiochip_add_data() is called very early during boot, so that GPIOs
+ * can be freely used, the chip->parent device must be registered before
  * the gpio framework's arch_initcall().  Otherwise sysfs initialization
  * for GPIOs will fail rudely.
  *
  * If chip->base is negative, this requests dynamic assignment of
  * a range of valid GPIOs.
  */
-int gpiochip_add(struct gpio_chip *chip)
+int gpiochip_add_data(struct gpio_chip *chip, void *data)
 {
 	unsigned long	flags;
 	int		status = 0;
@@ -308,6 +331,13 @@ int gpiochip_add(struct gpio_chip *chip)
 	if (!descs)
 		return -ENOMEM;
 
+	chip->data = data;
+
+	if (chip->ngpio == 0) {
+		chip_err(chip, "tried to insert a GPIO chip with zero lines\n");
+		return -EINVAL;
+	}
+
 	spin_lock_irqsave(&gpio_lock, flags);
 
 	if (base < 0) {
@@ -348,8 +378,8 @@ int gpiochip_add(struct gpio_chip *chip)
 	INIT_LIST_HEAD(&chip->pin_ranges);
 #endif
 
-	if (!chip->owner && chip->dev && chip->dev->driver)
-		chip->owner = chip->dev->driver->owner;
+	if (!chip->owner && chip->parent && chip->parent->driver)
+		chip->owner = chip->parent->driver->owner;
 
 	status = gpiochip_set_desc_names(chip);
 	if (status)
@@ -389,7 +419,7 @@ err_free_descs:
 		chip->label ? : "generic");
 	return status;
 }
-EXPORT_SYMBOL_GPL(gpiochip_add);
+EXPORT_SYMBOL_GPL(gpiochip_add_data);
 
 /**
  * gpiochip_remove() - unregister a gpio_chip
@@ -424,7 +454,8 @@ void gpiochip_remove(struct gpio_chip *chip)
 	spin_unlock_irqrestore(&gpio_lock, flags);
 
 	if (requested)
-		dev_crit(chip->dev, "REMOVING GPIOCHIP WITH GPIOS STILL REQUESTED\n");
+		dev_crit(chip->parent,
+			 "REMOVING GPIOCHIP WITH GPIOS STILL REQUESTED\n");
 
 	kfree(chip->desc);
 	chip->desc = NULL;
@@ -659,7 +690,7 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
  * gpiochip, providing an irq domain to translate the local IRQs to
  * global irqs in the gpiolib core, and making sure that the gpiochip
  * is passed as chip data to all related functions. Driver callbacks
- * need to use container_of() to get their local state containers back
+ * need to use gpiochip_get_data() to get their local state containers back
  * from the gpiochip passed as chip data. An irqdomain will be stored
  * in the gpiochip that shall be used by the driver to handle IRQ number
  * translation. The gpiochip will need to be initialized and registered
@@ -683,15 +714,16 @@ int _gpiochip_irqchip_add(struct gpio_chip *gpiochip,
 	if (!gpiochip || !irqchip)
 		return -EINVAL;
 
-	if (!gpiochip->dev) {
+	if (!gpiochip->parent) {
 		pr_err("missing gpiochip .dev parent pointer\n");
 		return -EINVAL;
 	}
-	of_node = gpiochip->dev->of_node;
+	of_node = gpiochip->parent->of_node;
 #ifdef CONFIG_OF_GPIO
 	/*
 	 * If the gpiochip has an assigned OF node this takes precedence
-	 * FIXME: get rid of this and use gpiochip->dev->of_node everywhere
+	 * FIXME: get rid of this and use gpiochip->parent->of_node
+	 * everywhere
 	 */
 	if (gpiochip->of_node)
 		of_node = gpiochip->of_node;
@@ -1279,13 +1311,7 @@ static int _gpiod_get_raw_value(const struct gpio_desc *desc)
 	chip = desc->chip;
 	offset = gpio_chip_hwgpio(desc);
 	value = chip->get ? chip->get(chip, offset) : -EIO;
-	/*
-	 * FIXME: fix all drivers to clamp to [0,1] or return negative,
-	 * then change this to:
-	 * value = value < 0 ? value : !!value;
-	 * so we can properly propagate error codes.
-	 */
-	value = !!value;
+	value = value < 0 ? value : !!value;
 	trace_gpio_value(desc_to_gpio(desc), 1, value);
 	return value;
 }
@@ -1882,7 +1908,7 @@ static struct gpio_desc *acpi_find_gpio(struct device *dev, const char *con_id,
 			return desc;
 	}
 
-	if (info.active_low)
+	if (info.polarity == GPIO_ACTIVE_LOW)
 		*flags |= GPIO_ACTIVE_LOW;
 
 	return desc;
@@ -2220,7 +2246,7 @@ struct gpio_desc *fwnode_get_named_gpiod(struct fwnode_handle *fwnode,
 
 		desc = acpi_node_get_gpiod(fwnode, propname, 0, &info);
 		if (!IS_ERR(desc))
-			active_low = info.active_low;
+			active_low = info.polarity == GPIO_ACTIVE_LOW;
 	}
 
 	if (IS_ERR(desc))
@@ -2512,7 +2538,7 @@ static int gpiolib_seq_show(struct seq_file *s, void *v)
 
 	seq_printf(s, "%sGPIOs %d-%d", (char *)s->private,
 			chip->base, chip->base + chip->ngpio - 1);
-	dev = chip->dev;
+	dev = chip->parent;
 	if (dev)
 		seq_printf(s, ", %s/%s", dev->bus ? dev->bus->name : "no-bus",
 			dev_name(dev));
diff --git a/drivers/gpio/gpiolib.h b/drivers/gpio/gpiolib.h
index 2b87cbd68478..99ed3b00ffe9 100644
--- a/drivers/gpio/gpiolib.h
+++ b/drivers/gpio/gpiolib.h
@@ -16,7 +16,7 @@
 #include <linux/device.h>
 
 enum of_gpio_flags;
-
+enum gpiod_flags;
 struct acpi_device;
 
 /**
@@ -26,7 +26,8 @@ struct acpi_device;
  */
 struct acpi_gpio_info {
 	bool gpioint;
-	bool active_low;
+	int polarity;
+	int triggering;
 };
 
 /* gpio suffixes used for ACPI and device tree lookup */
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index c4bf9a1cf4a6..8ae7ab68cb97 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -82,13 +82,13 @@ config DRM_TTM
 
 config DRM_GEM_CMA_HELPER
 	bool
-	depends on DRM && HAVE_DMA_ATTRS
+	depends on DRM
 	help
 	  Choose this if you need the GEM CMA helper functions
 
 config DRM_KMS_CMA_HELPER
 	bool
-	depends on DRM && HAVE_DMA_ATTRS
+	depends on DRM
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_FB_HELPER
 	select FB_SYS_FILLRECT
@@ -160,6 +160,7 @@ config DRM_AMDGPU
 	  If M is selected, the module will be called amdgpu.
 
 source "drivers/gpu/drm/amd/amdgpu/Kconfig"
+source "drivers/gpu/drm/amd/powerplay/Kconfig"
 
 source "drivers/gpu/drm/nouveau/Kconfig"
 
@@ -266,3 +267,5 @@ source "drivers/gpu/drm/amd/amdkfd/Kconfig"
 source "drivers/gpu/drm/imx/Kconfig"
 
 source "drivers/gpu/drm/vc4/Kconfig"
+
+source "drivers/gpu/drm/etnaviv/Kconfig"
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 1e9ff4c3e3db..61766dec6a8d 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -21,8 +21,6 @@ drm-$(CONFIG_DRM_PANEL) += drm_panel.o
 drm-$(CONFIG_OF) += drm_of.o
 drm-$(CONFIG_AGP) += drm_agpsupport.o
 
-drm-y += $(drm-m)
-
 drm_kms_helper-y := drm_crtc_helper.o drm_dp_helper.o drm_probe_helper.o \
 		drm_plane_helper.o drm_dp_mst_topology.o drm_atomic_helper.o
 drm_kms_helper-$(CONFIG_DRM_LOAD_EDID_FIRMWARE) += drm_edid_load.o
@@ -62,7 +60,7 @@ obj-$(CONFIG_DRM_ARMADA) += armada/
 obj-$(CONFIG_DRM_ATMEL_HLCDC)	+= atmel-hlcdc/
 obj-$(CONFIG_DRM_RCAR_DU) += rcar-du/
 obj-$(CONFIG_DRM_SHMOBILE) +=shmobile/
-obj-$(CONFIG_DRM_OMAP)	+= omapdrm/
+obj-y			+= omapdrm/
 obj-y			+= tilcdc/
 obj-$(CONFIG_DRM_QXL) += qxl/
 obj-$(CONFIG_DRM_BOCHS) += bochs/
@@ -75,3 +73,4 @@ obj-y			+= i2c/
 obj-y			+= panel/
 obj-y			+= bridge/
 obj-$(CONFIG_DRM_FSL_DCU) += fsl-dcu/
+obj-$(CONFIG_DRM_ETNAVIV) += etnaviv/
diff --git a/drivers/gpu/drm/amd/amdgpu/Makefile b/drivers/gpu/drm/amd/amdgpu/Makefile
index 04c270757030..20c9539abc36 100644
--- a/drivers/gpu/drm/amd/amdgpu/Makefile
+++ b/drivers/gpu/drm/amd/amdgpu/Makefile
@@ -2,10 +2,13 @@
 # Makefile for the drm device driver.  This driver provides support for the
 # Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
 
-ccflags-y := -Iinclude/drm -Idrivers/gpu/drm/amd/include/asic_reg \
-	-Idrivers/gpu/drm/amd/include \
-	-Idrivers/gpu/drm/amd/amdgpu \
-	-Idrivers/gpu/drm/amd/scheduler
+FULL_AMD_PATH=$(src)/..
+
+ccflags-y := -Iinclude/drm -I$(FULL_AMD_PATH)/include/asic_reg \
+	-I$(FULL_AMD_PATH)/include \
+	-I$(FULL_AMD_PATH)/amdgpu \
+	-I$(FULL_AMD_PATH)/scheduler \
+	-I$(FULL_AMD_PATH)/powerplay/inc
 
 amdgpu-y := amdgpu_drv.o
 
@@ -22,7 +25,7 @@ amdgpu-y += amdgpu_device.o amdgpu_kms.o \
 	amdgpu_ucode.o amdgpu_bo_list.o amdgpu_ctx.o amdgpu_sync.o
 
 # add asic specific block
-amdgpu-$(CONFIG_DRM_AMDGPU_CIK)+= cik.o gmc_v7_0.o cik_ih.o kv_smc.o kv_dpm.o \
+amdgpu-$(CONFIG_DRM_AMDGPU_CIK)+= cik.o cik_ih.o kv_smc.o kv_dpm.o \
 	ci_smc.o ci_dpm.o dce_v8_0.o gfx_v7_0.o cik_sdma.o uvd_v4_2.o vce_v2_0.o \
 	amdgpu_amdkfd_gfx_v7.o
 
@@ -31,6 +34,7 @@ amdgpu-y += \
 
 # add GMC block
 amdgpu-y += \
+	gmc_v7_0.o \
 	gmc_v8_0.o
 
 # add IH block
@@ -44,6 +48,7 @@ amdgpu-y += \
 # add SMC block
 amdgpu-y += \
 	amdgpu_dpm.o \
+	amdgpu_powerplay.o \
 	cz_smc.o cz_dpm.o \
 	tonga_smc.o tonga_dpm.o \
 	fiji_smc.o fiji_dpm.o \
@@ -94,6 +99,14 @@ 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 048cfe073dae..5e7770f9a415 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu.h
@@ -52,6 +52,7 @@
 #include "amdgpu_irq.h"
 #include "amdgpu_ucode.h"
 #include "amdgpu_gds.h"
+#include "amd_powerplay.h"
 
 #include "gpu_scheduler.h"
 
@@ -85,6 +86,9 @@ extern int amdgpu_enable_scheduler;
 extern int amdgpu_sched_jobs;
 extern int amdgpu_sched_hw_submission;
 extern int amdgpu_enable_semaphores;
+extern int amdgpu_powerplay;
+extern unsigned amdgpu_pcie_gen_cap;
+extern unsigned amdgpu_pcie_lane_cap;
 
 #define AMDGPU_WAIT_IDLE_TIMEOUT_IN_MS	        3000
 #define AMDGPU_MAX_USEC_TIMEOUT			100000	/* 100 ms */
@@ -130,47 +134,6 @@ extern int amdgpu_enable_semaphores;
 #define AMDGPU_RESET_VCE			(1 << 13)
 #define AMDGPU_RESET_VCE1			(1 << 14)
 
-/* CG block flags */
-#define AMDGPU_CG_BLOCK_GFX			(1 << 0)
-#define AMDGPU_CG_BLOCK_MC			(1 << 1)
-#define AMDGPU_CG_BLOCK_SDMA			(1 << 2)
-#define AMDGPU_CG_BLOCK_UVD			(1 << 3)
-#define AMDGPU_CG_BLOCK_VCE			(1 << 4)
-#define AMDGPU_CG_BLOCK_HDP			(1 << 5)
-#define AMDGPU_CG_BLOCK_BIF			(1 << 6)
-
-/* CG flags */
-#define AMDGPU_CG_SUPPORT_GFX_MGCG		(1 << 0)
-#define AMDGPU_CG_SUPPORT_GFX_MGLS		(1 << 1)
-#define AMDGPU_CG_SUPPORT_GFX_CGCG		(1 << 2)
-#define AMDGPU_CG_SUPPORT_GFX_CGLS		(1 << 3)
-#define AMDGPU_CG_SUPPORT_GFX_CGTS		(1 << 4)
-#define AMDGPU_CG_SUPPORT_GFX_CGTS_LS		(1 << 5)
-#define AMDGPU_CG_SUPPORT_GFX_CP_LS		(1 << 6)
-#define AMDGPU_CG_SUPPORT_GFX_RLC_LS		(1 << 7)
-#define AMDGPU_CG_SUPPORT_MC_LS			(1 << 8)
-#define AMDGPU_CG_SUPPORT_MC_MGCG		(1 << 9)
-#define AMDGPU_CG_SUPPORT_SDMA_LS		(1 << 10)
-#define AMDGPU_CG_SUPPORT_SDMA_MGCG		(1 << 11)
-#define AMDGPU_CG_SUPPORT_BIF_LS		(1 << 12)
-#define AMDGPU_CG_SUPPORT_UVD_MGCG		(1 << 13)
-#define AMDGPU_CG_SUPPORT_VCE_MGCG		(1 << 14)
-#define AMDGPU_CG_SUPPORT_HDP_LS		(1 << 15)
-#define AMDGPU_CG_SUPPORT_HDP_MGCG		(1 << 16)
-
-/* PG flags */
-#define AMDGPU_PG_SUPPORT_GFX_PG		(1 << 0)
-#define AMDGPU_PG_SUPPORT_GFX_SMG		(1 << 1)
-#define AMDGPU_PG_SUPPORT_GFX_DMG		(1 << 2)
-#define AMDGPU_PG_SUPPORT_UVD			(1 << 3)
-#define AMDGPU_PG_SUPPORT_VCE			(1 << 4)
-#define AMDGPU_PG_SUPPORT_CP			(1 << 5)
-#define AMDGPU_PG_SUPPORT_GDS			(1 << 6)
-#define AMDGPU_PG_SUPPORT_RLC_SMU_HS		(1 << 7)
-#define AMDGPU_PG_SUPPORT_SDMA			(1 << 8)
-#define AMDGPU_PG_SUPPORT_ACP			(1 << 9)
-#define AMDGPU_PG_SUPPORT_SAMU			(1 << 10)
-
 /* GFX current status */
 #define AMDGPU_GFX_NORMAL_MODE			0x00000000L
 #define AMDGPU_GFX_SAFE_MODE			0x00000001L
@@ -604,8 +567,6 @@ struct amdgpu_sa_manager {
 	uint32_t		align;
 };
 
-struct amdgpu_sa_bo;
-
 /* sub-allocation buffer */
 struct amdgpu_sa_bo {
 	struct list_head		olist;
@@ -918,8 +879,8 @@ struct amdgpu_ring {
 #define AMDGPU_VM_FAULT_STOP_ALWAYS	2
 
 struct amdgpu_vm_pt {
-	struct amdgpu_bo	*bo;
-	uint64_t		addr;
+	struct amdgpu_bo_list_entry	entry;
+	uint64_t			addr;
 };
 
 struct amdgpu_vm_id {
@@ -981,9 +942,12 @@ struct amdgpu_vm_manager {
 void amdgpu_vm_manager_fini(struct amdgpu_device *adev);
 int amdgpu_vm_init(struct amdgpu_device *adev, struct amdgpu_vm *vm);
 void amdgpu_vm_fini(struct amdgpu_device *adev, struct amdgpu_vm *vm);
-struct amdgpu_bo_list_entry *amdgpu_vm_get_bos(struct amdgpu_device *adev,
-					       struct amdgpu_vm *vm,
-					       struct list_head *head);
+void amdgpu_vm_get_pd_bo(struct amdgpu_vm *vm,
+			 struct list_head *validated,
+			 struct amdgpu_bo_list_entry *entry);
+void amdgpu_vm_get_pt_bos(struct amdgpu_vm *vm, struct list_head *duplicates);
+void amdgpu_vm_move_pt_bos_in_lru(struct amdgpu_device *adev,
+				  struct amdgpu_vm *vm);
 int amdgpu_vm_grab_id(struct amdgpu_vm *vm, struct amdgpu_ring *ring,
 		      struct amdgpu_sync *sync);
 void amdgpu_vm_flush(struct amdgpu_ring *ring,
@@ -1024,11 +988,9 @@ int amdgpu_vm_free_job(struct amdgpu_job *job);
  * context related structures
  */
 
-#define AMDGPU_CTX_MAX_CS_PENDING	16
-
 struct amdgpu_ctx_ring {
 	uint64_t		sequence;
-	struct fence		*fences[AMDGPU_CTX_MAX_CS_PENDING];
+	struct fence		**fences;
 	struct amd_sched_entity	entity;
 };
 
@@ -1037,6 +999,7 @@ struct amdgpu_ctx {
 	struct amdgpu_device    *adev;
 	unsigned		reset_counter;
 	spinlock_t		ring_lock;
+	struct fence            **fences;
 	struct amdgpu_ctx_ring	rings[AMDGPU_MAX_RINGS];
 };
 
@@ -1047,7 +1010,7 @@ struct amdgpu_ctx_mgr {
 	struct idr		ctx_handles;
 };
 
-int amdgpu_ctx_init(struct amdgpu_device *adev, bool kernel,
+int amdgpu_ctx_init(struct amdgpu_device *adev, enum amd_sched_priority pri,
 		    struct amdgpu_ctx *ctx);
 void amdgpu_ctx_fini(struct amdgpu_ctx *ctx);
 
@@ -1254,7 +1217,7 @@ struct amdgpu_cs_parser {
 	unsigned		nchunks;
 	struct amdgpu_cs_chunk	*chunks;
 	/* relocations */
-	struct amdgpu_bo_list_entry	*vm_bos;
+	struct amdgpu_bo_list_entry	vm_pd;
 	struct list_head	validated;
 	struct fence		*fence;
 
@@ -1301,31 +1264,7 @@ struct amdgpu_wb {
 int amdgpu_wb_get(struct amdgpu_device *adev, u32 *wb);
 void amdgpu_wb_free(struct amdgpu_device *adev, u32 wb);
 
-/**
- * struct amdgpu_pm - power management datas
- * It keeps track of various data needed to take powermanagement decision.
- */
 
-enum amdgpu_pm_state_type {
-	/* not used for dpm */
-	POWER_STATE_TYPE_DEFAULT,
-	POWER_STATE_TYPE_POWERSAVE,
-	/* user selectable states */
-	POWER_STATE_TYPE_BATTERY,
-	POWER_STATE_TYPE_BALANCED,
-	POWER_STATE_TYPE_PERFORMANCE,
-	/* internal states */
-	POWER_STATE_TYPE_INTERNAL_UVD,
-	POWER_STATE_TYPE_INTERNAL_UVD_SD,
-	POWER_STATE_TYPE_INTERNAL_UVD_HD,
-	POWER_STATE_TYPE_INTERNAL_UVD_HD2,
-	POWER_STATE_TYPE_INTERNAL_UVD_MVC,
-	POWER_STATE_TYPE_INTERNAL_BOOT,
-	POWER_STATE_TYPE_INTERNAL_THERMAL,
-	POWER_STATE_TYPE_INTERNAL_ACPI,
-	POWER_STATE_TYPE_INTERNAL_ULV,
-	POWER_STATE_TYPE_INTERNAL_3DPERF,
-};
 
 enum amdgpu_int_thermal_type {
 	THERMAL_TYPE_NONE,
@@ -1607,8 +1546,8 @@ struct amdgpu_dpm {
 	/* vce requirements */
 	struct amdgpu_vce_state vce_states[AMDGPU_MAX_VCE_LEVELS];
 	enum amdgpu_vce_level vce_level;
-	enum amdgpu_pm_state_type state;
-	enum amdgpu_pm_state_type user_state;
+	enum amd_pm_state_type state;
+	enum amd_pm_state_type user_state;
 	u32                     platform_caps;
 	u32                     voltage_response_time;
 	u32                     backbias_response_time;
@@ -1661,8 +1600,13 @@ struct amdgpu_pm {
 	const struct firmware	*fw;	/* SMC firmware */
 	uint32_t                fw_version;
 	const struct amdgpu_dpm_funcs *funcs;
+	uint32_t                pcie_gen_mask;
+	uint32_t                pcie_mlw_mask;
+	struct amd_pp_display_configuration pm_display_cfg;/* set by DAL */
 };
 
+void amdgpu_get_pcie_info(struct amdgpu_device *adev);
+
 /*
  * UVD
  */
@@ -1830,6 +1774,8 @@ struct amdgpu_cu_info {
  */
 struct amdgpu_asic_funcs {
 	bool (*read_disabled_bios)(struct amdgpu_device *adev);
+	bool (*read_bios_from_rom)(struct amdgpu_device *adev,
+				   u8 *bios, u32 length_bytes);
 	int (*read_register)(struct amdgpu_device *adev, u32 se_num,
 			     u32 sh_num, u32 reg_offset, u32 *value);
 	void (*set_vga_state)(struct amdgpu_device *adev, bool state);
@@ -2060,6 +2006,10 @@ struct amdgpu_device {
 	/* interrupts */
 	struct amdgpu_irq		irq;
 
+	/* powerplay */
+	struct amd_powerplay		powerplay;
+	bool				pp_enabled;
+
 	/* dpm */
 	struct amdgpu_pm		pm;
 	u32				cg_flags;
@@ -2236,6 +2186,7 @@ amdgpu_get_sdma_instance(struct amdgpu_ring *ring)
 #define amdgpu_asic_set_vce_clocks(adev, ev, ec) (adev)->asic_funcs->set_vce_clocks((adev), (ev), (ec))
 #define amdgpu_asic_get_gpu_clock_counter(adev) (adev)->asic_funcs->get_gpu_clock_counter((adev))
 #define amdgpu_asic_read_disabled_bios(adev) (adev)->asic_funcs->read_disabled_bios((adev))
+#define amdgpu_asic_read_bios_from_rom(adev, b, l) (adev)->asic_funcs->read_bios_from_rom((adev), (b), (l))
 #define amdgpu_asic_read_register(adev, se, sh, offset, v)((adev)->asic_funcs->read_register((adev), (se), (sh), (offset), (v)))
 #define amdgpu_asic_get_cu_info(adev, info) (adev)->asic_funcs->get_cu_info((adev), (info))
 #define amdgpu_gart_flush_gpu_tlb(adev, vmid) (adev)->gart.gart_funcs->flush_gpu_tlb((adev), (vmid))
@@ -2277,24 +2228,78 @@ amdgpu_get_sdma_instance(struct amdgpu_ring *ring)
 #define amdgpu_display_resume_mc_access(adev, s) (adev)->mode_info.funcs->resume_mc_access((adev), (s))
 #define amdgpu_emit_copy_buffer(adev, ib, s, d, b) (adev)->mman.buffer_funcs->emit_copy_buffer((ib),  (s), (d), (b))
 #define amdgpu_emit_fill_buffer(adev, ib, s, d, b) (adev)->mman.buffer_funcs->emit_fill_buffer((ib), (s), (d), (b))
-#define amdgpu_dpm_get_temperature(adev) (adev)->pm.funcs->get_temperature((adev))
 #define amdgpu_dpm_pre_set_power_state(adev) (adev)->pm.funcs->pre_set_power_state((adev))
 #define amdgpu_dpm_set_power_state(adev) (adev)->pm.funcs->set_power_state((adev))
 #define amdgpu_dpm_post_set_power_state(adev) (adev)->pm.funcs->post_set_power_state((adev))
 #define amdgpu_dpm_display_configuration_changed(adev) (adev)->pm.funcs->display_configuration_changed((adev))
-#define amdgpu_dpm_get_sclk(adev, l) (adev)->pm.funcs->get_sclk((adev), (l))
-#define amdgpu_dpm_get_mclk(adev, l) (adev)->pm.funcs->get_mclk((adev), (l))
 #define amdgpu_dpm_print_power_state(adev, ps) (adev)->pm.funcs->print_power_state((adev), (ps))
-#define amdgpu_dpm_debugfs_print_current_performance_level(adev, m) (adev)->pm.funcs->debugfs_print_current_performance_level((adev), (m))
-#define amdgpu_dpm_force_performance_level(adev, l) (adev)->pm.funcs->force_performance_level((adev), (l))
 #define amdgpu_dpm_vblank_too_short(adev) (adev)->pm.funcs->vblank_too_short((adev))
-#define amdgpu_dpm_powergate_uvd(adev, g) (adev)->pm.funcs->powergate_uvd((adev), (g))
-#define amdgpu_dpm_powergate_vce(adev, g) (adev)->pm.funcs->powergate_vce((adev), (g))
 #define amdgpu_dpm_enable_bapm(adev, e) (adev)->pm.funcs->enable_bapm((adev), (e))
-#define amdgpu_dpm_set_fan_control_mode(adev, m) (adev)->pm.funcs->set_fan_control_mode((adev), (m))
-#define amdgpu_dpm_get_fan_control_mode(adev) (adev)->pm.funcs->get_fan_control_mode((adev))
-#define amdgpu_dpm_set_fan_speed_percent(adev, s) (adev)->pm.funcs->set_fan_speed_percent((adev), (s))
-#define amdgpu_dpm_get_fan_speed_percent(adev, s) (adev)->pm.funcs->get_fan_speed_percent((adev), (s))
+
+#define amdgpu_dpm_get_temperature(adev) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->get_temperature((adev)->powerplay.pp_handle) : \
+	      (adev)->pm.funcs->get_temperature((adev)))
+
+#define amdgpu_dpm_set_fan_control_mode(adev, m) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->set_fan_control_mode((adev)->powerplay.pp_handle, (m)) : \
+	      (adev)->pm.funcs->set_fan_control_mode((adev), (m)))
+
+#define amdgpu_dpm_get_fan_control_mode(adev) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->get_fan_control_mode((adev)->powerplay.pp_handle) : \
+	      (adev)->pm.funcs->get_fan_control_mode((adev)))
+
+#define amdgpu_dpm_set_fan_speed_percent(adev, s) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->set_fan_speed_percent((adev)->powerplay.pp_handle, (s)) : \
+	      (adev)->pm.funcs->set_fan_speed_percent((adev), (s)))
+
+#define amdgpu_dpm_get_fan_speed_percent(adev, s) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->get_fan_speed_percent((adev)->powerplay.pp_handle, (s)) : \
+	      (adev)->pm.funcs->get_fan_speed_percent((adev), (s)))
+
+#define amdgpu_dpm_get_sclk(adev, l) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->get_sclk((adev)->powerplay.pp_handle, (l)) : \
+		(adev)->pm.funcs->get_sclk((adev), (l)))
+
+#define amdgpu_dpm_get_mclk(adev, l)  \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->get_mclk((adev)->powerplay.pp_handle, (l)) : \
+	      (adev)->pm.funcs->get_mclk((adev), (l)))
+
+
+#define amdgpu_dpm_force_performance_level(adev, l) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->force_performance_level((adev)->powerplay.pp_handle, (l)) : \
+	      (adev)->pm.funcs->force_performance_level((adev), (l)))
+
+#define amdgpu_dpm_powergate_uvd(adev, g) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->powergate_uvd((adev)->powerplay.pp_handle, (g)) : \
+	      (adev)->pm.funcs->powergate_uvd((adev), (g)))
+
+#define amdgpu_dpm_powergate_vce(adev, g) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->powergate_vce((adev)->powerplay.pp_handle, (g)) : \
+	      (adev)->pm.funcs->powergate_vce((adev), (g)))
+
+#define amdgpu_dpm_debugfs_print_current_performance_level(adev, m) \
+	((adev)->pp_enabled ?						\
+	      (adev)->powerplay.pp_funcs->print_current_performance_level((adev)->powerplay.pp_handle, (m)) : \
+	      (adev)->pm.funcs->debugfs_print_current_performance_level((adev), (m)))
+
+#define amdgpu_dpm_get_current_power_state(adev) \
+	(adev)->powerplay.pp_funcs->get_current_power_state((adev)->powerplay.pp_handle)
+
+#define amdgpu_dpm_get_performance_level(adev) \
+	(adev)->powerplay.pp_funcs->get_performance_level((adev)->powerplay.pp_handle)
+
+#define amdgpu_dpm_dispatch_task(adev, event_id, input, output)		\
+	(adev)->powerplay.pp_funcs->dispatch_tasks((adev)->powerplay.pp_handle, (event_id), (input), (output))
 
 #define amdgpu_gds_switch(adev, r, v, d, w, a) (adev)->gds.funcs->patch_gds_switch((r), (v), (d), (w), (a))
 
@@ -2314,6 +2319,8 @@ bool amdgpu_ttm_bo_is_amdgpu_bo(struct ttm_buffer_object *bo);
 int amdgpu_ttm_tt_set_userptr(struct ttm_tt *ttm, uint64_t addr,
 				     uint32_t flags);
 bool amdgpu_ttm_tt_has_userptr(struct ttm_tt *ttm);
+bool amdgpu_ttm_tt_affect_userptr(struct ttm_tt *ttm, unsigned long start,
+				  unsigned long end);
 bool amdgpu_ttm_tt_is_readonly(struct ttm_tt *ttm);
 uint32_t amdgpu_ttm_tt_pte_flags(struct amdgpu_device *adev, struct ttm_tt *ttm,
 				 struct ttm_mem_reg *mem);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.c
index a142d5ae148d..5cd7b736a9de 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.c
@@ -29,66 +29,10 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include "amdgpu.h"
-#include "amdgpu_acpi.h"
+#include "amd_acpi.h"
 #include "atom.h"
 
-#define ACPI_AC_CLASS           "ac_adapter"
-
 extern void amdgpu_pm_acpi_event_handler(struct amdgpu_device *adev);
-
-struct atif_verify_interface {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u16 version;		/* version */
-	u32 notification_mask;	/* supported notifications mask */
-	u32 function_bits;	/* supported functions bit vector */
-} __packed;
-
-struct atif_system_params {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u32 valid_mask;		/* valid flags mask */
-	u32 flags;		/* flags */
-	u8 command_code;	/* notify command code */
-} __packed;
-
-struct atif_sbios_requests {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u32 pending;		/* pending sbios requests */
-	u8 panel_exp_mode;	/* panel expansion mode */
-	u8 thermal_gfx;		/* thermal state: target gfx controller */
-	u8 thermal_state;	/* thermal state: state id (0: exit state, non-0: state) */
-	u8 forced_power_gfx;	/* forced power state: target gfx controller */
-	u8 forced_power_state;	/* forced power state: state id */
-	u8 system_power_src;	/* system power source */
-	u8 backlight_level;	/* panel backlight level (0-255) */
-} __packed;
-
-#define ATIF_NOTIFY_MASK	0x3
-#define ATIF_NOTIFY_NONE	0
-#define ATIF_NOTIFY_81		1
-#define ATIF_NOTIFY_N		2
-
-struct atcs_verify_interface {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u16 version;		/* version */
-	u32 function_bits;	/* supported functions bit vector */
-} __packed;
-
-#define ATCS_VALID_FLAGS_MASK	0x3
-
-struct atcs_pref_req_input {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u16 client_id;		/* client id (bit 2-0: func num, 7-3: dev num, 15-8: bus num) */
-	u16 valid_flags_mask;	/* valid flags mask */
-	u16 flags;		/* flags */
-	u8 req_type;		/* request type */
-	u8 perf_req;		/* performance request */
-} __packed;
-
-struct atcs_pref_req_output {
-	u16 size;		/* structure size in bytes (includes size field) */
-	u8 ret_val;		/* return value */
-} __packed;
-
 /* Call the ATIF method
  */
 /**
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v7.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v7.c
index 0e1376317683..362bedc9e507 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v7.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v7.c
@@ -154,7 +154,7 @@ static const struct kfd2kgd_calls kfd2kgd = {
 	.get_fw_version = get_fw_version
 };
 
-struct kfd2kgd_calls *amdgpu_amdkfd_gfx_7_get_functions()
+struct kfd2kgd_calls *amdgpu_amdkfd_gfx_7_get_functions(void)
 {
 	return (struct kfd2kgd_calls *)&kfd2kgd;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v8.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v8.c
index 79fa5c7de856..04b744d64b57 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v8.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_amdkfd_gfx_v8.c
@@ -115,7 +115,7 @@ static const struct kfd2kgd_calls kfd2kgd = {
 	.get_fw_version = get_fw_version
 };
 
-struct kfd2kgd_calls *amdgpu_amdkfd_gfx_8_0_get_functions()
+struct kfd2kgd_calls *amdgpu_amdkfd_gfx_8_0_get_functions(void)
 {
 	return (struct kfd2kgd_calls *)&kfd2kgd;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_atpx_handler.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_atpx_handler.c
index 5a8fbadbd27b..3c895863fcf5 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_atpx_handler.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_atpx_handler.c
@@ -11,7 +11,7 @@
 #include <linux/acpi.h>
 #include <linux/pci.h>
 
-#include "amdgpu_acpi.h"
+#include "amd_acpi.h"
 
 struct amdgpu_atpx_functions {
 	bool px_params;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_bios.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_bios.c
index c44c0c6afd1b..80add22375ee 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_bios.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_bios.c
@@ -35,6 +35,13 @@
  * BIOS.
  */
 
+#define AMD_VBIOS_SIGNATURE " 761295520"
+#define AMD_VBIOS_SIGNATURE_OFFSET 0x30
+#define AMD_VBIOS_SIGNATURE_SIZE sizeof(AMD_VBIOS_SIGNATURE)
+#define AMD_VBIOS_SIGNATURE_END (AMD_VBIOS_SIGNATURE_OFFSET + AMD_VBIOS_SIGNATURE_SIZE)
+#define AMD_IS_VALID_VBIOS(p) ((p)[0] == 0x55 && (p)[1] == 0xAA)
+#define AMD_VBIOS_LENGTH(p) ((p)[2] << 9)
+
 /* If you boot an IGP board with a discrete card as the primary,
  * the IGP rom is not accessible via the rom bar as the IGP rom is
  * part of the system bios.  On boot, the system bios puts a
@@ -58,7 +65,7 @@ static bool igp_read_bios_from_vram(struct amdgpu_device *adev)
 		return false;
 	}
 
-	if (size == 0 || bios[0] != 0x55 || bios[1] != 0xaa) {
+	if (size == 0 || !AMD_IS_VALID_VBIOS(bios)) {
 		iounmap(bios);
 		return false;
 	}
@@ -74,7 +81,7 @@ static bool igp_read_bios_from_vram(struct amdgpu_device *adev)
 
 bool amdgpu_read_bios(struct amdgpu_device *adev)
 {
-	uint8_t __iomem *bios, val1, val2;
+	uint8_t __iomem *bios, val[2];
 	size_t size;
 
 	adev->bios = NULL;
@@ -84,10 +91,10 @@ bool amdgpu_read_bios(struct amdgpu_device *adev)
 		return false;
 	}
 
-	val1 = readb(&bios[0]);
-	val2 = readb(&bios[1]);
+	val[0] = readb(&bios[0]);
+	val[1] = readb(&bios[1]);
 
-	if (size == 0 || val1 != 0x55 || val2 != 0xaa) {
+	if (size == 0 || !AMD_IS_VALID_VBIOS(val)) {
 		pci_unmap_rom(adev->pdev, bios);
 		return false;
 	}
@@ -101,6 +108,38 @@ bool amdgpu_read_bios(struct amdgpu_device *adev)
 	return true;
 }
 
+static bool amdgpu_read_bios_from_rom(struct amdgpu_device *adev)
+{
+	u8 header[AMD_VBIOS_SIGNATURE_END+1] = {0};
+	int len;
+
+	if (!adev->asic_funcs->read_bios_from_rom)
+		return false;
+
+	/* validate VBIOS signature */
+	if (amdgpu_asic_read_bios_from_rom(adev, &header[0], sizeof(header)) == false)
+		return false;
+	header[AMD_VBIOS_SIGNATURE_END] = 0;
+
+	if ((!AMD_IS_VALID_VBIOS(header)) ||
+	    0 != memcmp((char *)&header[AMD_VBIOS_SIGNATURE_OFFSET],
+			AMD_VBIOS_SIGNATURE,
+			strlen(AMD_VBIOS_SIGNATURE)))
+		return false;
+
+	/* valid vbios, go on */
+	len = AMD_VBIOS_LENGTH(header);
+	len = ALIGN(len, 4);
+	adev->bios = kmalloc(len, GFP_KERNEL);
+	if (!adev->bios) {
+		DRM_ERROR("no memory to allocate for BIOS\n");
+		return false;
+	}
+
+	/* read complete BIOS */
+	return amdgpu_asic_read_bios_from_rom(adev, adev->bios, len);
+}
+
 static bool amdgpu_read_platform_bios(struct amdgpu_device *adev)
 {
 	uint8_t __iomem *bios;
@@ -113,7 +152,7 @@ static bool amdgpu_read_platform_bios(struct amdgpu_device *adev)
 		return false;
 	}
 
-	if (size == 0 || bios[0] != 0x55 || bios[1] != 0xaa) {
+	if (size == 0 || !AMD_IS_VALID_VBIOS(bios)) {
 		return false;
 	}
 	adev->bios = kmemdup(bios, size, GFP_KERNEL);
@@ -230,7 +269,7 @@ static bool amdgpu_atrm_get_bios(struct amdgpu_device *adev)
 			break;
 	}
 
-	if (i == 0 || adev->bios[0] != 0x55 || adev->bios[1] != 0xaa) {
+	if (i == 0 || !AMD_IS_VALID_VBIOS(adev->bios)) {
 		kfree(adev->bios);
 		return false;
 	}
@@ -320,6 +359,9 @@ bool amdgpu_get_bios(struct amdgpu_device *adev)
 	if (r == false)
 		r = amdgpu_read_bios(adev);
 	if (r == false) {
+		r = amdgpu_read_bios_from_rom(adev);
+	}
+	if (r == false) {
 		r = amdgpu_read_disabled_bios(adev);
 	}
 	if (r == false) {
@@ -330,7 +372,7 @@ bool amdgpu_get_bios(struct amdgpu_device *adev)
 		adev->bios = NULL;
 		return false;
 	}
-	if (adev->bios[0] != 0x55 || adev->bios[1] != 0xaa) {
+	if (!AMD_IS_VALID_VBIOS(adev->bios)) {
 		printk("BIOS signature incorrect %x %x\n", adev->bios[0], adev->bios[1]);
 		goto free_bios;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_cgs.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_cgs.c
index 8e995148f56e..7a4b101e10c6 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_cgs.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_cgs.c
@@ -24,6 +24,7 @@
 #include <linux/list.h>
 #include <linux/slab.h>
 #include <linux/pci.h>
+#include <linux/acpi.h>
 #include <drm/drmP.h>
 #include <linux/firmware.h>
 #include <drm/amdgpu_drm.h>
@@ -32,7 +33,6 @@
 #include "atom.h"
 #include "amdgpu_ucode.h"
 
-
 struct amdgpu_cgs_device {
 	struct cgs_device base;
 	struct amdgpu_device *adev;
@@ -398,6 +398,41 @@ static void amdgpu_cgs_write_pci_config_dword(void *cgs_device, unsigned addr,
 	WARN(ret, "pci_write_config_dword error");
 }
 
+
+static int amdgpu_cgs_get_pci_resource(void *cgs_device,
+				       enum cgs_resource_type resource_type,
+				       uint64_t size,
+				       uint64_t offset,
+				       uint64_t *resource_base)
+{
+	CGS_FUNC_ADEV;
+
+	if (resource_base == NULL)
+		return -EINVAL;
+
+	switch (resource_type) {
+	case CGS_RESOURCE_TYPE_MMIO:
+		if (adev->rmmio_size == 0)
+			return -ENOENT;
+		if ((offset + size) > adev->rmmio_size)
+			return -EINVAL;
+		*resource_base = adev->rmmio_base;
+		return 0;
+	case CGS_RESOURCE_TYPE_DOORBELL:
+		if (adev->doorbell.size == 0)
+			return -ENOENT;
+		if ((offset + size) > adev->doorbell.size)
+			return -EINVAL;
+		*resource_base = adev->doorbell.base;
+		return 0;
+	case CGS_RESOURCE_TYPE_FB:
+	case CGS_RESOURCE_TYPE_IO:
+	case CGS_RESOURCE_TYPE_ROM:
+	default:
+		return -EINVAL;
+	}
+}
+
 static const void *amdgpu_cgs_atom_get_data_table(void *cgs_device,
 						  unsigned table, uint16_t *size,
 						  uint8_t *frev, uint8_t *crev)
@@ -703,6 +738,9 @@ static int amdgpu_cgs_get_firmware_info(void *cgs_device,
 		case CHIP_TONGA:
 			strcpy(fw_name, "amdgpu/tonga_smc.bin");
 			break;
+		case CHIP_FIJI:
+			strcpy(fw_name, "amdgpu/fiji_smc.bin");
+			break;
 		default:
 			DRM_ERROR("SMC firmware not supported\n");
 			return -EINVAL;
@@ -736,6 +774,294 @@ static int amdgpu_cgs_get_firmware_info(void *cgs_device,
 	return 0;
 }
 
+static int amdgpu_cgs_query_system_info(void *cgs_device,
+				struct cgs_system_info *sys_info)
+{
+	CGS_FUNC_ADEV;
+
+	if (NULL == sys_info)
+		return -ENODEV;
+
+	if (sizeof(struct cgs_system_info) != sys_info->size)
+		return -ENODEV;
+
+	switch (sys_info->info_id) {
+	case CGS_SYSTEM_INFO_ADAPTER_BDF_ID:
+		sys_info->value = adev->pdev->devfn | (adev->pdev->bus->number << 8);
+		break;
+	case CGS_SYSTEM_INFO_PCIE_GEN_INFO:
+		sys_info->value = adev->pm.pcie_gen_mask;
+		break;
+	case CGS_SYSTEM_INFO_PCIE_MLW:
+		sys_info->value = adev->pm.pcie_mlw_mask;
+		break;
+	case CGS_SYSTEM_INFO_CG_FLAGS:
+		sys_info->value = adev->cg_flags;
+		break;
+	case CGS_SYSTEM_INFO_PG_FLAGS:
+		sys_info->value = adev->pg_flags;
+		break;
+	default:
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int amdgpu_cgs_get_active_displays_info(void *cgs_device,
+					  struct cgs_display_info *info)
+{
+	CGS_FUNC_ADEV;
+	struct amdgpu_crtc *amdgpu_crtc;
+	struct drm_device *ddev = adev->ddev;
+	struct drm_crtc *crtc;
+	uint32_t line_time_us, vblank_lines;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	if (adev->mode_info.num_crtc && adev->mode_info.mode_config_initialized) {
+		list_for_each_entry(crtc,
+				&ddev->mode_config.crtc_list, head) {
+			amdgpu_crtc = to_amdgpu_crtc(crtc);
+			if (crtc->enabled) {
+				info->active_display_mask |= (1 << amdgpu_crtc->crtc_id);
+				info->display_count++;
+			}
+			if (info->mode_info != NULL &&
+				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 -
+							amdgpu_crtc->hw_mode.crtc_vdisplay +
+							(amdgpu_crtc->v_border * 2);
+				info->mode_info->vblank_time_us = vblank_lines * line_time_us;
+				info->mode_info->refresh_rate = drm_mode_vrefresh(&amdgpu_crtc->hw_mode);
+				info->mode_info->ref_clock = adev->clock.spll.reference_freq;
+				info->mode_info++;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/** \brief evaluate acpi namespace object, handle or pathname must be valid
+ *  \param cgs_device
+ *  \param info input/output arguments for the control method
+ *  \return status
+ */
+
+#if defined(CONFIG_ACPI)
+static int amdgpu_cgs_acpi_eval_object(void *cgs_device,
+				    struct cgs_acpi_method_info *info)
+{
+	CGS_FUNC_ADEV;
+	acpi_handle handle;
+	struct acpi_object_list input;
+	struct acpi_buffer output = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *params = NULL;
+	union acpi_object *obj = NULL;
+	uint8_t name[5] = {'\0'};
+	struct cgs_acpi_method_argument *argument = NULL;
+	uint32_t i, count;
+	acpi_status status;
+	int result;
+	uint32_t func_no = 0xFFFFFFFF;
+
+	handle = ACPI_HANDLE(&adev->pdev->dev);
+	if (!handle)
+		return -ENODEV;
+
+	memset(&input, 0, sizeof(struct acpi_object_list));
+
+	/* validate input info */
+	if (info->size != sizeof(struct cgs_acpi_method_info))
+		return -EINVAL;
+
+	input.count = info->input_count;
+	if (info->input_count > 0) {
+		if (info->pinput_argument == NULL)
+			return -EINVAL;
+		argument = info->pinput_argument;
+		func_no = argument->value;
+		for (i = 0; i < info->input_count; i++) {
+			if (((argument->type == ACPI_TYPE_STRING) ||
+			     (argument->type == ACPI_TYPE_BUFFER)) &&
+			    (argument->pointer == NULL))
+				return -EINVAL;
+			argument++;
+		}
+	}
+
+	if (info->output_count > 0) {
+		if (info->poutput_argument == NULL)
+			return -EINVAL;
+		argument = info->poutput_argument;
+		for (i = 0; i < info->output_count; i++) {
+			if (((argument->type == ACPI_TYPE_STRING) ||
+				(argument->type == ACPI_TYPE_BUFFER))
+				&& (argument->pointer == NULL))
+				return -EINVAL;
+			argument++;
+		}
+	}
+
+	/* The path name passed to acpi_evaluate_object should be null terminated */
+	if ((info->field & CGS_ACPI_FIELD_METHOD_NAME) != 0) {
+		strncpy(name, (char *)&(info->name), sizeof(uint32_t));
+		name[4] = '\0';
+	}
+
+	/* parse input parameters */
+	if (input.count > 0) {
+		input.pointer = params =
+				kzalloc(sizeof(union acpi_object) * input.count, GFP_KERNEL);
+		if (params == NULL)
+			return -EINVAL;
+
+		argument = info->pinput_argument;
+
+		for (i = 0; i < input.count; i++) {
+			params->type = argument->type;
+			switch (params->type) {
+			case ACPI_TYPE_INTEGER:
+				params->integer.value = argument->value;
+				break;
+			case ACPI_TYPE_STRING:
+				params->string.length = argument->method_length;
+				params->string.pointer = argument->pointer;
+				break;
+			case ACPI_TYPE_BUFFER:
+				params->buffer.length = argument->method_length;
+				params->buffer.pointer = argument->pointer;
+				break;
+			default:
+				break;
+			}
+			params++;
+			argument++;
+		}
+	}
+
+	/* parse output info */
+	count = info->output_count;
+	argument = info->poutput_argument;
+
+	/* evaluate the acpi method */
+	status = acpi_evaluate_object(handle, name, &input, &output);
+
+	if (ACPI_FAILURE(status)) {
+		result = -EIO;
+		goto error;
+	}
+
+	/* return the output info */
+	obj = output.pointer;
+
+	if (count > 1) {
+		if ((obj->type != ACPI_TYPE_PACKAGE) ||
+			(obj->package.count != count)) {
+			result = -EIO;
+			goto error;
+		}
+		params = obj->package.elements;
+	} else
+		params = obj;
+
+	if (params == NULL) {
+		result = -EIO;
+		goto error;
+	}
+
+	for (i = 0; i < count; i++) {
+		if (argument->type != params->type) {
+			result = -EIO;
+			goto error;
+		}
+		switch (params->type) {
+		case ACPI_TYPE_INTEGER:
+			argument->value = params->integer.value;
+			break;
+		case ACPI_TYPE_STRING:
+			if ((params->string.length != argument->data_length) ||
+				(params->string.pointer == NULL)) {
+				result = -EIO;
+				goto error;
+			}
+			strncpy(argument->pointer,
+				params->string.pointer,
+				params->string.length);
+			break;
+		case ACPI_TYPE_BUFFER:
+			if (params->buffer.pointer == NULL) {
+				result = -EIO;
+				goto error;
+			}
+			memcpy(argument->pointer,
+				params->buffer.pointer,
+				argument->data_length);
+			break;
+		default:
+			break;
+		}
+		argument++;
+		params++;
+	}
+
+error:
+	if (obj != NULL)
+		kfree(obj);
+	kfree((void *)input.pointer);
+	return result;
+}
+#else
+static int amdgpu_cgs_acpi_eval_object(void *cgs_device,
+				struct cgs_acpi_method_info *info)
+{
+	return -EIO;
+}
+#endif
+
+int amdgpu_cgs_call_acpi_method(void *cgs_device,
+					uint32_t acpi_method,
+					uint32_t acpi_function,
+					void *pinput, void *poutput,
+					uint32_t output_count,
+					uint32_t input_size,
+					uint32_t output_size)
+{
+	struct cgs_acpi_method_argument acpi_input[2] = { {0}, {0} };
+	struct cgs_acpi_method_argument acpi_output = {0};
+	struct cgs_acpi_method_info info = {0};
+
+	acpi_input[0].type = CGS_ACPI_TYPE_INTEGER;
+	acpi_input[0].method_length = sizeof(uint32_t);
+	acpi_input[0].data_length = sizeof(uint32_t);
+	acpi_input[0].value = acpi_function;
+
+	acpi_input[1].type = CGS_ACPI_TYPE_BUFFER;
+	acpi_input[1].method_length = CGS_ACPI_MAX_BUFFER_SIZE;
+	acpi_input[1].data_length = input_size;
+	acpi_input[1].pointer = pinput;
+
+	acpi_output.type = CGS_ACPI_TYPE_BUFFER;
+	acpi_output.method_length = CGS_ACPI_MAX_BUFFER_SIZE;
+	acpi_output.data_length = output_size;
+	acpi_output.pointer = poutput;
+
+	info.size = sizeof(struct cgs_acpi_method_info);
+	info.field = CGS_ACPI_FIELD_METHOD_NAME | CGS_ACPI_FIELD_INPUT_ARGUMENT_COUNT;
+	info.input_count = 2;
+	info.name = acpi_method;
+	info.pinput_argument = acpi_input;
+	info.output_count = output_count;
+	info.poutput_argument = &acpi_output;
+
+	return amdgpu_cgs_acpi_eval_object(cgs_device, &info);
+}
+
 static const struct cgs_ops amdgpu_cgs_ops = {
 	amdgpu_cgs_gpu_mem_info,
 	amdgpu_cgs_gmap_kmem,
@@ -756,6 +1082,7 @@ static const struct cgs_ops amdgpu_cgs_ops = {
 	amdgpu_cgs_write_pci_config_byte,
 	amdgpu_cgs_write_pci_config_word,
 	amdgpu_cgs_write_pci_config_dword,
+	amdgpu_cgs_get_pci_resource,
 	amdgpu_cgs_atom_get_data_table,
 	amdgpu_cgs_atom_get_cmd_table_revs,
 	amdgpu_cgs_atom_exec_cmd_table,
@@ -768,7 +1095,10 @@ static const struct cgs_ops amdgpu_cgs_ops = {
 	amdgpu_cgs_set_camera_voltages,
 	amdgpu_cgs_get_firmware_info,
 	amdgpu_cgs_set_powergating_state,
-	amdgpu_cgs_set_clockgating_state
+	amdgpu_cgs_set_clockgating_state,
+	amdgpu_cgs_get_active_displays_info,
+	amdgpu_cgs_call_acpi_method,
+	amdgpu_cgs_query_system_info,
 };
 
 static const struct cgs_os_ops amdgpu_cgs_os_ops = {
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
index 89c3dd62ba21..119cdc2c43e7 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_connectors.c
@@ -77,7 +77,7 @@ void amdgpu_connector_hotplug(struct drm_connector *connector)
 			} else if (amdgpu_atombios_dp_needs_link_train(amdgpu_connector)) {
 				/* Don't try to start link training before we
 				 * have the dpcd */
-				if (!amdgpu_atombios_dp_get_dpcd(amdgpu_connector))
+				if (amdgpu_atombios_dp_get_dpcd(amdgpu_connector))
 					return;
 
 				/* set it to OFF so that drm_helper_connector_dpms()
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
index 25a3e2485cc2..b882e8175615 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_cs.c
@@ -406,8 +406,8 @@ static int amdgpu_cs_parser_relocs(struct amdgpu_cs_parser *p)
 		amdgpu_cs_buckets_get_list(&buckets, &p->validated);
 	}
 
-	p->vm_bos = amdgpu_vm_get_bos(p->adev, &fpriv->vm,
-				      &p->validated);
+	INIT_LIST_HEAD(&duplicates);
+	amdgpu_vm_get_pd_bo(&fpriv->vm, &p->validated, &p->vm_pd);
 
 	if (p->uf.bo)
 		list_add(&p->uf_entry.tv.head, &p->validated);
@@ -415,20 +415,23 @@ static int amdgpu_cs_parser_relocs(struct amdgpu_cs_parser *p)
 	if (need_mmap_lock)
 		down_read(&current->mm->mmap_sem);
 
-	INIT_LIST_HEAD(&duplicates);
 	r = ttm_eu_reserve_buffers(&p->ticket, &p->validated, true, &duplicates);
 	if (unlikely(r != 0))
 		goto error_reserve;
 
-	r = amdgpu_cs_list_validate(p->adev, &fpriv->vm, &p->validated);
+	amdgpu_vm_get_pt_bos(&fpriv->vm, &duplicates);
+
+	r = amdgpu_cs_list_validate(p->adev, &fpriv->vm, &duplicates);
 	if (r)
 		goto error_validate;
 
-	r = amdgpu_cs_list_validate(p->adev, &fpriv->vm, &duplicates);
+	r = amdgpu_cs_list_validate(p->adev, &fpriv->vm, &p->validated);
 
 error_validate:
-	if (r)
+	if (r) {
+		amdgpu_vm_move_pt_bos_in_lru(p->adev, &fpriv->vm);
 		ttm_eu_backoff_reservation(&p->ticket, &p->validated);
+	}
 
 error_reserve:
 	if (need_mmap_lock)
@@ -472,9 +475,12 @@ static int cmp_size_smaller_first(void *priv, struct list_head *a,
  **/
 static void amdgpu_cs_parser_fini(struct amdgpu_cs_parser *parser, int error, bool backoff)
 {
+	struct amdgpu_fpriv *fpriv = parser->filp->driver_priv;
 	unsigned i;
 
 	if (!error) {
+		amdgpu_vm_move_pt_bos_in_lru(parser->adev, &fpriv->vm);
+
 		/* Sort the buffer list from the smallest to largest buffer,
 		 * which affects the order of buffers in the LRU list.
 		 * This assures that the smallest buffers are added first
@@ -501,7 +507,6 @@ static void amdgpu_cs_parser_fini(struct amdgpu_cs_parser *parser, int error, bo
 	if (parser->bo_list)
 		amdgpu_bo_list_put(parser->bo_list);
 
-	drm_free_large(parser->vm_bos);
 	for (i = 0; i < parser->nchunks; i++)
 		drm_free_large(parser->chunks[i].kdata);
 	kfree(parser->chunks);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
index fec65f01c031..17d1fb12128a 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ctx.c
@@ -25,7 +25,7 @@
 #include <drm/drmP.h>
 #include "amdgpu.h"
 
-int amdgpu_ctx_init(struct amdgpu_device *adev, bool kernel,
+int amdgpu_ctx_init(struct amdgpu_device *adev, enum amd_sched_priority pri,
 		    struct amdgpu_ctx *ctx)
 {
 	unsigned i, j;
@@ -35,17 +35,25 @@ int amdgpu_ctx_init(struct amdgpu_device *adev, bool kernel,
 	ctx->adev = adev;
 	kref_init(&ctx->refcount);
 	spin_lock_init(&ctx->ring_lock);
-	for (i = 0; i < AMDGPU_MAX_RINGS; ++i)
-		ctx->rings[i].sequence = 1;
+	ctx->fences = kzalloc(sizeof(struct fence *) * amdgpu_sched_jobs *
+			 AMDGPU_MAX_RINGS, GFP_KERNEL);
+	if (!ctx->fences)
+		return -ENOMEM;
 
+	for (i = 0; i < AMDGPU_MAX_RINGS; ++i) {
+		ctx->rings[i].sequence = 1;
+		ctx->rings[i].fences = (void *)ctx->fences + sizeof(struct fence *) *
+			amdgpu_sched_jobs * i;
+	}
 	if (amdgpu_enable_scheduler) {
 		/* create context entity for each ring */
 		for (i = 0; i < adev->num_rings; i++) {
 			struct amd_sched_rq *rq;
-			if (kernel)
-				rq = &adev->rings[i]->sched.kernel_rq;
-			else
-				rq = &adev->rings[i]->sched.sched_rq;
+			if (pri >= AMD_SCHED_MAX_PRIORITY) {
+				kfree(ctx->fences);
+				return -EINVAL;
+			}
+			rq = &adev->rings[i]->sched.sched_rq[pri];
 			r = amd_sched_entity_init(&adev->rings[i]->sched,
 						  &ctx->rings[i].entity,
 						  rq, amdgpu_sched_jobs);
@@ -57,7 +65,7 @@ int amdgpu_ctx_init(struct amdgpu_device *adev, bool kernel,
 			for (j = 0; j < i; j++)
 				amd_sched_entity_fini(&adev->rings[j]->sched,
 						      &ctx->rings[j].entity);
-			kfree(ctx);
+			kfree(ctx->fences);
 			return r;
 		}
 	}
@@ -73,8 +81,9 @@ void amdgpu_ctx_fini(struct amdgpu_ctx *ctx)
 		return;
 
 	for (i = 0; i < AMDGPU_MAX_RINGS; ++i)
-		for (j = 0; j < AMDGPU_CTX_MAX_CS_PENDING; ++j)
+		for (j = 0; j < amdgpu_sched_jobs; ++j)
 			fence_put(ctx->rings[i].fences[j]);
+	kfree(ctx->fences);
 
 	if (amdgpu_enable_scheduler) {
 		for (i = 0; i < adev->num_rings; i++)
@@ -103,9 +112,13 @@ static int amdgpu_ctx_alloc(struct amdgpu_device *adev,
 		return r;
 	}
 	*id = (uint32_t)r;
-	r = amdgpu_ctx_init(adev, false, ctx);
+	r = amdgpu_ctx_init(adev, AMD_SCHED_PRIORITY_NORMAL, ctx);
+	if (r) {
+		idr_remove(&mgr->ctx_handles, *id);
+		*id = 0;
+		kfree(ctx);
+	}
 	mutex_unlock(&mgr->lock);
-
 	return r;
 }
 
@@ -239,7 +252,7 @@ uint64_t amdgpu_ctx_add_fence(struct amdgpu_ctx *ctx, struct amdgpu_ring *ring,
 	unsigned idx = 0;
 	struct fence *other = NULL;
 
-	idx = seq % AMDGPU_CTX_MAX_CS_PENDING;
+	idx = seq & (amdgpu_sched_jobs - 1);
 	other = cring->fences[idx];
 	if (other) {
 		signed long r;
@@ -274,12 +287,12 @@ struct fence *amdgpu_ctx_get_fence(struct amdgpu_ctx *ctx,
 	}
 
 
-	if (seq + AMDGPU_CTX_MAX_CS_PENDING < cring->sequence) {
+	if (seq + amdgpu_sched_jobs < cring->sequence) {
 		spin_unlock(&ctx->ring_lock);
 		return NULL;
 	}
 
-	fence = fence_get(cring->fences[seq % AMDGPU_CTX_MAX_CS_PENDING]);
+	fence = fence_get(cring->fences[seq & (amdgpu_sched_jobs - 1)]);
 	spin_unlock(&ctx->ring_lock);
 
 	return fence;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
index d5b421330145..51bfc114584e 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_device.c
@@ -38,6 +38,7 @@
 #include "amdgpu_i2c.h"
 #include "atom.h"
 #include "amdgpu_atombios.h"
+#include "amd_pcie.h"
 #ifdef CONFIG_DRM_AMDGPU_CIK
 #include "cik.h"
 #endif
@@ -949,6 +950,15 @@ static bool amdgpu_check_pot_argument(int arg)
  */
 static void amdgpu_check_arguments(struct amdgpu_device *adev)
 {
+	if (amdgpu_sched_jobs < 4) {
+		dev_warn(adev->dev, "sched jobs (%d) must be at least 4\n",
+			 amdgpu_sched_jobs);
+		amdgpu_sched_jobs = 4;
+	} else if (!amdgpu_check_pot_argument(amdgpu_sched_jobs)){
+		dev_warn(adev->dev, "sched jobs (%d) must be a power of 2\n",
+			 amdgpu_sched_jobs);
+		amdgpu_sched_jobs = roundup_pow_of_two(amdgpu_sched_jobs);
+	}
 	/* vramlimit must be a power of two */
 	if (!amdgpu_check_pot_argument(amdgpu_vram_limit)) {
 		dev_warn(adev->dev, "vram limit (%d) must be a power of 2\n",
@@ -1214,12 +1224,14 @@ static int amdgpu_early_init(struct amdgpu_device *adev)
 		} else {
 			if (adev->ip_blocks[i].funcs->early_init) {
 				r = adev->ip_blocks[i].funcs->early_init((void *)adev);
-				if (r == -ENOENT)
+				if (r == -ENOENT) {
 					adev->ip_block_status[i].valid = false;
-				else if (r)
+				} else if (r) {
+					DRM_ERROR("early_init %d failed %d\n", i, r);
 					return r;
-				else
+				} else {
 					adev->ip_block_status[i].valid = true;
+				}
 			} else {
 				adev->ip_block_status[i].valid = true;
 			}
@@ -1237,20 +1249,28 @@ static int amdgpu_init(struct amdgpu_device *adev)
 		if (!adev->ip_block_status[i].valid)
 			continue;
 		r = adev->ip_blocks[i].funcs->sw_init((void *)adev);
-		if (r)
+		if (r) {
+			DRM_ERROR("sw_init %d failed %d\n", i, r);
 			return r;
+		}
 		adev->ip_block_status[i].sw = true;
 		/* need to do gmc hw init early so we can allocate gpu mem */
 		if (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_GMC) {
 			r = amdgpu_vram_scratch_init(adev);
-			if (r)
+			if (r) {
+				DRM_ERROR("amdgpu_vram_scratch_init failed %d\n", r);
 				return r;
+			}
 			r = adev->ip_blocks[i].funcs->hw_init((void *)adev);
-			if (r)
+			if (r) {
+				DRM_ERROR("hw_init %d failed %d\n", i, r);
 				return r;
+			}
 			r = amdgpu_wb_init(adev);
-			if (r)
+			if (r) {
+				DRM_ERROR("amdgpu_wb_init failed %d\n", r);
 				return r;
+			}
 			adev->ip_block_status[i].hw = true;
 		}
 	}
@@ -1262,8 +1282,10 @@ static int amdgpu_init(struct amdgpu_device *adev)
 		if (adev->ip_blocks[i].type == AMD_IP_BLOCK_TYPE_GMC)
 			continue;
 		r = adev->ip_blocks[i].funcs->hw_init((void *)adev);
-		if (r)
+		if (r) {
+			DRM_ERROR("hw_init %d failed %d\n", i, r);
 			return r;
+		}
 		adev->ip_block_status[i].hw = true;
 	}
 
@@ -1280,12 +1302,16 @@ static int amdgpu_late_init(struct amdgpu_device *adev)
 		/* enable clockgating to save power */
 		r = adev->ip_blocks[i].funcs->set_clockgating_state((void *)adev,
 								    AMD_CG_STATE_GATE);
-		if (r)
+		if (r) {
+			DRM_ERROR("set_clockgating_state(gate) %d failed %d\n", i, r);
 			return r;
+		}
 		if (adev->ip_blocks[i].funcs->late_init) {
 			r = adev->ip_blocks[i].funcs->late_init((void *)adev);
-			if (r)
+			if (r) {
+				DRM_ERROR("late_init %d failed %d\n", i, r);
 				return r;
+			}
 		}
 	}
 
@@ -1306,10 +1332,15 @@ static int amdgpu_fini(struct amdgpu_device *adev)
 		/* 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)
+		if (r) {
+			DRM_ERROR("set_clockgating_state(ungate) %d failed %d\n", i, r);
 			return r;
+		}
 		r = adev->ip_blocks[i].funcs->hw_fini((void *)adev);
 		/* XXX handle errors */
+		if (r) {
+			DRM_DEBUG("hw_fini %d failed %d\n", i, r);
+		}
 		adev->ip_block_status[i].hw = false;
 	}
 
@@ -1318,6 +1349,9 @@ static int amdgpu_fini(struct amdgpu_device *adev)
 			continue;
 		r = adev->ip_blocks[i].funcs->sw_fini((void *)adev);
 		/* XXX handle errors */
+		if (r) {
+			DRM_DEBUG("sw_fini %d failed %d\n", i, r);
+		}
 		adev->ip_block_status[i].sw = false;
 		adev->ip_block_status[i].valid = false;
 	}
@@ -1335,9 +1369,15 @@ static int amdgpu_suspend(struct amdgpu_device *adev)
 		/* ungate blocks so that suspend can properly shut them down */
 		r = adev->ip_blocks[i].funcs->set_clockgating_state((void *)adev,
 								    AMD_CG_STATE_UNGATE);
+		if (r) {
+			DRM_ERROR("set_clockgating_state(ungate) %d failed %d\n", i, r);
+		}
 		/* XXX handle errors */
 		r = adev->ip_blocks[i].funcs->suspend(adev);
 		/* XXX handle errors */
+		if (r) {
+			DRM_ERROR("suspend %d failed %d\n", i, r);
+		}
 	}
 
 	return 0;
@@ -1351,8 +1391,10 @@ static int amdgpu_resume(struct amdgpu_device *adev)
 		if (!adev->ip_block_status[i].valid)
 			continue;
 		r = adev->ip_blocks[i].funcs->resume(adev);
-		if (r)
+		if (r) {
+			DRM_ERROR("resume %d failed %d\n", i, r);
 			return r;
+		}
 	}
 
 	return 0;
@@ -1484,8 +1526,10 @@ int amdgpu_device_init(struct amdgpu_device *adev,
 		return -EINVAL;
 	}
 	r = amdgpu_atombios_init(adev);
-	if (r)
+	if (r) {
+		dev_err(adev->dev, "amdgpu_atombios_init failed\n");
 		return r;
+	}
 
 	/* Post card if necessary */
 	if (!amdgpu_card_posted(adev)) {
@@ -1499,21 +1543,26 @@ int amdgpu_device_init(struct amdgpu_device *adev,
 
 	/* Initialize clocks */
 	r = amdgpu_atombios_get_clock_info(adev);
-	if (r)
+	if (r) {
+		dev_err(adev->dev, "amdgpu_atombios_get_clock_info failed\n");
 		return r;
+	}
 	/* init i2c buses */
 	amdgpu_atombios_i2c_init(adev);
 
 	/* Fence driver */
 	r = amdgpu_fence_driver_init(adev);
-	if (r)
+	if (r) {
+		dev_err(adev->dev, "amdgpu_fence_driver_init failed\n");
 		return r;
+	}
 
 	/* init the mode config */
 	drm_mode_config_init(adev->ddev);
 
 	r = amdgpu_init(adev);
 	if (r) {
+		dev_err(adev->dev, "amdgpu_init failed\n");
 		amdgpu_fini(adev);
 		return r;
 	}
@@ -1528,7 +1577,7 @@ int amdgpu_device_init(struct amdgpu_device *adev,
 		return r;
 	}
 
-	r = amdgpu_ctx_init(adev, true, &adev->kernel_ctx);
+	r = amdgpu_ctx_init(adev, AMD_SCHED_PRIORITY_KERNEL, &adev->kernel_ctx);
 	if (r) {
 		dev_err(adev->dev, "failed to create kernel context (%d).\n", r);
 		return r;
@@ -1570,8 +1619,10 @@ int amdgpu_device_init(struct amdgpu_device *adev,
 	 * explicit gating rather than handling it automatically.
 	 */
 	r = amdgpu_late_init(adev);
-	if (r)
+	if (r) {
+		dev_err(adev->dev, "amdgpu_late_init failed\n");
 		return r;
+	}
 
 	return 0;
 }
@@ -1744,15 +1795,20 @@ int amdgpu_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 	}
 
 	/* post card */
-	amdgpu_atom_asic_init(adev->mode_info.atom_context);
+	if (!amdgpu_card_posted(adev))
+		amdgpu_atom_asic_init(adev->mode_info.atom_context);
 
 	r = amdgpu_resume(adev);
+	if (r)
+		DRM_ERROR("amdgpu_resume failed (%d).\n", r);
 
 	amdgpu_fence_driver_resume(adev);
 
-	r = amdgpu_ib_ring_tests(adev);
-	if (r)
-		DRM_ERROR("ib ring test failed (%d).\n", r);
+	if (resume) {
+		r = amdgpu_ib_ring_tests(adev);
+		if (r)
+			DRM_ERROR("ib ring test failed (%d).\n", r);
+	}
 
 	r = amdgpu_late_init(adev);
 	if (r)
@@ -1788,6 +1844,7 @@ int amdgpu_resume_kms(struct drm_device *dev, bool resume, bool fbcon)
 	}
 
 	drm_kms_helper_poll_enable(dev);
+	drm_helper_hpd_irq_event(dev);
 
 	if (fbcon) {
 		amdgpu_fbdev_set_suspend(adev, 0);
@@ -1881,6 +1938,100 @@ retry:
 	return r;
 }
 
+#define AMDGPU_DEFAULT_PCIE_GEN_MASK 0x30007  /* gen: chipset 1/2, asic 1/2/3 */
+#define AMDGPU_DEFAULT_PCIE_MLW_MASK 0x2f0000 /* 1/2/4/8/16 lanes */
+
+void amdgpu_get_pcie_info(struct amdgpu_device *adev)
+{
+	u32 mask;
+	int ret;
+
+	if (amdgpu_pcie_gen_cap)
+		adev->pm.pcie_gen_mask = amdgpu_pcie_gen_cap;
+
+	if (amdgpu_pcie_lane_cap)
+		adev->pm.pcie_mlw_mask = amdgpu_pcie_lane_cap;
+
+	/* covers APUs as well */
+	if (pci_is_root_bus(adev->pdev->bus)) {
+		if (adev->pm.pcie_gen_mask == 0)
+			adev->pm.pcie_gen_mask = AMDGPU_DEFAULT_PCIE_GEN_MASK;
+		if (adev->pm.pcie_mlw_mask == 0)
+			adev->pm.pcie_mlw_mask = AMDGPU_DEFAULT_PCIE_MLW_MASK;
+		return;
+	}
+
+	if (adev->pm.pcie_gen_mask == 0) {
+		ret = drm_pcie_get_speed_cap_mask(adev->ddev, &mask);
+		if (!ret) {
+			adev->pm.pcie_gen_mask = (CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN1 |
+						  CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN2 |
+						  CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN3);
+
+			if (mask & DRM_PCIE_SPEED_25)
+				adev->pm.pcie_gen_mask |= CAIL_PCIE_LINK_SPEED_SUPPORT_GEN1;
+			if (mask & DRM_PCIE_SPEED_50)
+				adev->pm.pcie_gen_mask |= CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2;
+			if (mask & DRM_PCIE_SPEED_80)
+				adev->pm.pcie_gen_mask |= CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3;
+		} else {
+			adev->pm.pcie_gen_mask = AMDGPU_DEFAULT_PCIE_GEN_MASK;
+		}
+	}
+	if (adev->pm.pcie_mlw_mask == 0) {
+		ret = drm_pcie_get_max_link_width(adev->ddev, &mask);
+		if (!ret) {
+			switch (mask) {
+			case 32:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X32 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X16 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X12 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X8 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X4 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 16:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X16 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X12 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X8 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X4 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 12:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X12 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X8 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X4 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 8:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X8 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X4 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 4:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X4 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 2:
+				adev->pm.pcie_mlw_mask = (CAIL_PCIE_LINK_WIDTH_SUPPORT_X2 |
+							  CAIL_PCIE_LINK_WIDTH_SUPPORT_X1);
+				break;
+			case 1:
+				adev->pm.pcie_mlw_mask = CAIL_PCIE_LINK_WIDTH_SUPPORT_X1;
+				break;
+			default:
+				break;
+			}
+		} else {
+			adev->pm.pcie_mlw_mask = AMDGPU_DEFAULT_PCIE_MLW_MASK;
+		}
+	}
+}
 
 /*
  * Debugfs
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_display.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_display.c
index 5580d3420c3a..1846d65b7285 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_display.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_display.c
@@ -72,8 +72,8 @@ static void amdgpu_flip_work_func(struct work_struct *__work)
 
 	struct drm_crtc *crtc = &amdgpuCrtc->base;
 	unsigned long flags;
-	unsigned i;
-	int vpos, hpos, stat, min_udelay;
+	unsigned i, repcnt = 4;
+	int vpos, hpos, stat, min_udelay = 0;
 	struct drm_vblank_crtc *vblank = &crtc->dev->vblank[work->crtc_id];
 
 	amdgpu_flip_wait_fence(adev, &work->excl);
@@ -96,7 +96,7 @@ static void amdgpu_flip_work_func(struct work_struct *__work)
 	 * In practice this won't execute very often unless on very fast
 	 * machines because the time window for this to happen is very small.
 	 */
-	for (;;) {
+	while (amdgpuCrtc->enabled && --repcnt) {
 		/* GET_DISTANCE_TO_VBLANKSTART returns distance to real vblank
 		 * start in hpos, and to the "fudged earlier" vblank start in
 		 * vpos.
@@ -112,12 +112,24 @@ static void amdgpu_flip_work_func(struct work_struct *__work)
 			break;
 
 		/* Sleep at least until estimated real start of hw vblank */
-		spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
 		min_udelay = (-hpos + 1) * max(vblank->linedur_ns / 1000, 5);
+		if (min_udelay > vblank->framedur_ns / 2000) {
+			/* Don't wait ridiculously long - something is wrong */
+			repcnt = 0;
+			break;
+		}
+		spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
 		usleep_range(min_udelay, 2 * min_udelay);
 		spin_lock_irqsave(&crtc->dev->event_lock, flags);
 	};
 
+	if (!repcnt)
+		DRM_DEBUG_DRIVER("Delay problem on crtc %d: min_udelay %d, "
+				 "framedur %d, linedur %d, stat %d, vpos %d, "
+				 "hpos %d\n", work->crtc_id, min_udelay,
+				 vblank->framedur_ns / 1000,
+				 vblank->linedur_ns / 1000, stat, vpos, hpos);
+
 	/* do the flip (mmio) */
 	adev->mode_info.funcs->page_flip(adev, work->crtc_id, work->base);
 	/* set the flip status */
@@ -518,7 +530,7 @@ static const struct drm_framebuffer_funcs amdgpu_fb_funcs = {
 int
 amdgpu_framebuffer_init(struct drm_device *dev,
 			struct amdgpu_framebuffer *rfb,
-			struct drm_mode_fb_cmd2 *mode_cmd,
+			const struct drm_mode_fb_cmd2 *mode_cmd,
 			struct drm_gem_object *obj)
 {
 	int ret;
@@ -535,7 +547,7 @@ amdgpu_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 amdgpu_user_framebuffer_create(struct drm_device *dev,
 			       struct drm_file *file_priv,
-			       struct drm_mode_fb_cmd2 *mode_cmd)
+			       const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct amdgpu_framebuffer *amdgpu_fb;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
index 0508c5cd103a..9ef1db87cf26 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_drv.c
@@ -79,9 +79,12 @@ int amdgpu_vm_fault_stop = 0;
 int amdgpu_vm_debug = 0;
 int amdgpu_exp_hw_support = 0;
 int amdgpu_enable_scheduler = 1;
-int amdgpu_sched_jobs = 16;
+int amdgpu_sched_jobs = 32;
 int amdgpu_sched_hw_submission = 2;
 int amdgpu_enable_semaphores = 0;
+int amdgpu_powerplay = -1;
+unsigned amdgpu_pcie_gen_cap = 0;
+unsigned amdgpu_pcie_lane_cap = 0;
 
 MODULE_PARM_DESC(vramlimit, "Restrict VRAM for testing, in megabytes");
 module_param_named(vramlimit, amdgpu_vram_limit, int, 0600);
@@ -155,7 +158,7 @@ module_param_named(exp_hw_support, amdgpu_exp_hw_support, int, 0444);
 MODULE_PARM_DESC(enable_scheduler, "enable SW GPU scheduler (1 = enable (default), 0 = disable)");
 module_param_named(enable_scheduler, amdgpu_enable_scheduler, int, 0444);
 
-MODULE_PARM_DESC(sched_jobs, "the max number of jobs supported in the sw queue (default 16)");
+MODULE_PARM_DESC(sched_jobs, "the max number of jobs supported in the sw queue (default 32)");
 module_param_named(sched_jobs, amdgpu_sched_jobs, int, 0444);
 
 MODULE_PARM_DESC(sched_hw_submission, "the max number of HW submissions (default 2)");
@@ -164,6 +167,17 @@ module_param_named(sched_hw_submission, amdgpu_sched_hw_submission, int, 0444);
 MODULE_PARM_DESC(enable_semaphores, "Enable semaphores (1 = enable, 0 = disable (default))");
 module_param_named(enable_semaphores, amdgpu_enable_semaphores, int, 0644);
 
+#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);
+#endif
+
+MODULE_PARM_DESC(pcie_gen_cap, "PCIE Gen Caps (0: autodetect (default))");
+module_param_named(pcie_gen_cap, amdgpu_pcie_gen_cap, uint, 0444);
+
+MODULE_PARM_DESC(pcie_lane_cap, "PCIE Lane Caps (0: autodetect (default))");
+module_param_named(pcie_lane_cap, amdgpu_pcie_lane_cap, uint, 0444);
+
 static struct pci_device_id pciidlist[] = {
 #ifdef CONFIG_DRM_AMDGPU_CIK
 	/* Kaveri */
@@ -250,11 +264,11 @@ static struct pci_device_id pciidlist[] = {
 	{0x1002, 0x985F, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_MULLINS|AMD_IS_MOBILITY|AMD_IS_APU},
 #endif
 	/* topaz */
-	{0x1002, 0x6900, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ|AMD_EXP_HW_SUPPORT},
-	{0x1002, 0x6901, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ|AMD_EXP_HW_SUPPORT},
-	{0x1002, 0x6902, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ|AMD_EXP_HW_SUPPORT},
-	{0x1002, 0x6903, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ|AMD_EXP_HW_SUPPORT},
-	{0x1002, 0x6907, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ|AMD_EXP_HW_SUPPORT},
+	{0x1002, 0x6900, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ},
+	{0x1002, 0x6901, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ},
+	{0x1002, 0x6902, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ},
+	{0x1002, 0x6903, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ},
+	{0x1002, 0x6907, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TOPAZ},
 	/* tonga */
 	{0x1002, 0x6920, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TONGA},
 	{0x1002, 0x6921, PCI_ANY_ID, PCI_ANY_ID, 0, 0, CHIP_TONGA},
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
index 093a8c618931..919146780a15 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_fb.c
@@ -45,7 +45,6 @@
 struct amdgpu_fbdev {
 	struct drm_fb_helper helper;
 	struct amdgpu_framebuffer rfb;
-	struct list_head fbdev_list;
 	struct amdgpu_device *adev;
 };
 
@@ -264,7 +263,7 @@ out_unref:
 
 	}
 	if (fb && ret) {
-		drm_gem_object_unreference(gobj);
+		drm_gem_object_unreference_unlocked(gobj);
 		drm_framebuffer_unregister_private(fb);
 		drm_framebuffer_cleanup(fb);
 		kfree(fb);
@@ -334,6 +333,10 @@ int amdgpu_fbdev_init(struct amdgpu_device *adev)
 	if (!adev->mode_info.mode_config_initialized)
 		return 0;
 
+	/* don't init fbdev if there are no connectors */
+	if (list_empty(&adev->ddev->mode_config.connector_list))
+		return 0;
+
 	/* select 8 bpp console on low vram cards */
 	if (adev->mc.real_vram_size <= (32*1024*1024))
 		bpp_sel = 8;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
index 9c253c535d26..d20c2a8929cb 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_gem.c
@@ -448,7 +448,7 @@ static void amdgpu_gem_va_update_vm(struct amdgpu_device *adev,
 				    struct amdgpu_bo_va *bo_va, uint32_t operation)
 {
 	struct ttm_validate_buffer tv, *entry;
-	struct amdgpu_bo_list_entry *vm_bos;
+	struct amdgpu_bo_list_entry vm_pd;
 	struct ww_acquire_ctx ticket;
 	struct list_head list, duplicates;
 	unsigned domain;
@@ -461,15 +461,14 @@ static void amdgpu_gem_va_update_vm(struct amdgpu_device *adev,
 	tv.shared = true;
 	list_add(&tv.head, &list);
 
-	vm_bos = amdgpu_vm_get_bos(adev, bo_va->vm, &list);
-	if (!vm_bos)
-		return;
+	amdgpu_vm_get_pd_bo(bo_va->vm, &list, &vm_pd);
 
 	/* Provide duplicates to avoid -EALREADY */
 	r = ttm_eu_reserve_buffers(&ticket, &list, true, &duplicates);
 	if (r)
-		goto error_free;
+		goto error_print;
 
+	amdgpu_vm_get_pt_bos(bo_va->vm, &duplicates);
 	list_for_each_entry(entry, &list, head) {
 		domain = amdgpu_mem_type_to_domain(entry->bo->mem.mem_type);
 		/* if anything is swapped out don't swap it in here,
@@ -499,9 +498,7 @@ static void amdgpu_gem_va_update_vm(struct amdgpu_device *adev,
 error_unreserve:
 	ttm_eu_backoff_reservation(&ticket, &list);
 
-error_free:
-	drm_free_large(vm_bos);
-
+error_print:
 	if (r && r != -ERESTARTSYS)
 		DRM_ERROR("Couldn't update BO_VA (%d)\n", r);
 }
@@ -599,7 +596,8 @@ int amdgpu_gem_va_ioctl(struct drm_device *dev, void *data,
 		break;
 	}
 	ttm_eu_backoff_reservation(&ticket, &list);
-	if (!r && !(args->flags & AMDGPU_VM_DELAY_UPDATE))
+	if (!r && !(args->flags & AMDGPU_VM_DELAY_UPDATE) &&
+	    !amdgpu_vm_debug)
 		amdgpu_gem_va_update_vm(adev, bo_va, args->operation);
 
 	drm_gem_object_unreference_unlocked(gobj);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.c
index 7c42ff670080..f594cfaa97e5 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.c
@@ -25,6 +25,7 @@
  *          Alex Deucher
  *          Jerome Glisse
  */
+#include <linux/irq.h>
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/amdgpu_drm.h>
@@ -312,6 +313,7 @@ int amdgpu_irq_add_id(struct amdgpu_device *adev, unsigned src_id,
 	}
 
 	adev->irq.sources[src_id] = source;
+
 	return 0;
 }
 
@@ -335,15 +337,19 @@ void amdgpu_irq_dispatch(struct amdgpu_device *adev,
 		return;
 	}
 
-	src = adev->irq.sources[src_id];
-	if (!src) {
-		DRM_DEBUG("Unhandled interrupt src_id: %d\n", src_id);
-		return;
-	}
+	if (adev->irq.virq[src_id]) {
+		generic_handle_irq(irq_find_mapping(adev->irq.domain, src_id));
+	} else {
+		src = adev->irq.sources[src_id];
+		if (!src) {
+			DRM_DEBUG("Unhandled interrupt src_id: %d\n", src_id);
+			return;
+		}
 
-	r = src->funcs->process(adev, src, entry);
-	if (r)
-		DRM_ERROR("error processing interrupt (%d)\n", r);
+		r = src->funcs->process(adev, src, entry);
+		if (r)
+			DRM_ERROR("error processing interrupt (%d)\n", r);
+	}
 }
 
 /**
@@ -461,3 +467,90 @@ bool amdgpu_irq_enabled(struct amdgpu_device *adev, struct amdgpu_irq_src *src,
 
 	return !!atomic_read(&src->enabled_types[type]);
 }
+
+/* gen irq */
+static void amdgpu_irq_mask(struct irq_data *irqd)
+{
+	/* XXX */
+}
+
+static void amdgpu_irq_unmask(struct irq_data *irqd)
+{
+	/* XXX */
+}
+
+static struct irq_chip amdgpu_irq_chip = {
+	.name = "amdgpu-ih",
+	.irq_mask = amdgpu_irq_mask,
+	.irq_unmask = amdgpu_irq_unmask,
+};
+
+static int amdgpu_irqdomain_map(struct irq_domain *d,
+				unsigned int irq, irq_hw_number_t hwirq)
+{
+	if (hwirq >= AMDGPU_MAX_IRQ_SRC_ID)
+		return -EPERM;
+
+	irq_set_chip_and_handler(irq,
+				 &amdgpu_irq_chip, handle_simple_irq);
+	return 0;
+}
+
+static struct irq_domain_ops amdgpu_hw_irqdomain_ops = {
+	.map = amdgpu_irqdomain_map,
+};
+
+/**
+ * amdgpu_irq_add_domain - create a linear irq domain
+ *
+ * @adev: amdgpu device pointer
+ *
+ * Create an irq domain for GPU interrupt sources
+ * that may be driven by another driver (e.g., ACP).
+ */
+int amdgpu_irq_add_domain(struct amdgpu_device *adev)
+{
+	adev->irq.domain = irq_domain_add_linear(NULL, AMDGPU_MAX_IRQ_SRC_ID,
+						 &amdgpu_hw_irqdomain_ops, adev);
+	if (!adev->irq.domain) {
+		DRM_ERROR("GPU irq add domain failed\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/**
+ * amdgpu_irq_remove_domain - remove the irq domain
+ *
+ * @adev: amdgpu device pointer
+ *
+ * Remove the irq domain for GPU interrupt sources
+ * that may be driven by another driver (e.g., ACP).
+ */
+void amdgpu_irq_remove_domain(struct amdgpu_device *adev)
+{
+	if (adev->irq.domain) {
+		irq_domain_remove(adev->irq.domain);
+		adev->irq.domain = NULL;
+	}
+}
+
+/**
+ * amdgpu_irq_create_mapping - create a mapping between a domain irq and a
+ *                             Linux irq
+ *
+ * @adev: amdgpu device pointer
+ * @src_id: IH source id
+ *
+ * Create a mapping between a domain irq (GPU IH src id) and a Linux irq
+ * Use this for components that generate a GPU interrupt, but are driven
+ * by a different driver (e.g., ACP).
+ * Returns the Linux irq.
+ */
+unsigned amdgpu_irq_create_mapping(struct amdgpu_device *adev, unsigned src_id)
+{
+	adev->irq.virq[src_id] = irq_create_mapping(adev->irq.domain, src_id);
+
+	return adev->irq.virq[src_id];
+}
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.h
index 17b01aef4278..e124b59f39c1 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_irq.h
@@ -24,6 +24,7 @@
 #ifndef __AMDGPU_IRQ_H__
 #define __AMDGPU_IRQ_H__
 
+#include <linux/irqdomain.h>
 #include "amdgpu_ih.h"
 
 #define AMDGPU_MAX_IRQ_SRC_ID	0x100
@@ -65,6 +66,10 @@ struct amdgpu_irq {
 	/* interrupt ring */
 	struct amdgpu_ih_ring		ih;
 	const struct amdgpu_ih_funcs	*ih_funcs;
+
+	/* gen irq stuff */
+	struct irq_domain		*domain; /* GPU irq controller domain */
+	unsigned			virq[AMDGPU_MAX_IRQ_SRC_ID];
 };
 
 void amdgpu_irq_preinstall(struct drm_device *dev);
@@ -90,4 +95,8 @@ int amdgpu_irq_put(struct amdgpu_device *adev, struct amdgpu_irq_src *src,
 bool amdgpu_irq_enabled(struct amdgpu_device *adev, struct amdgpu_irq_src *src,
 			unsigned type);
 
+int amdgpu_irq_add_domain(struct amdgpu_device *adev);
+void amdgpu_irq_remove_domain(struct amdgpu_device *adev);
+unsigned amdgpu_irq_create_mapping(struct amdgpu_device *adev, unsigned src_id);
+
 #endif
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_mn.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_mn.c
index b1969f2b2038..d4e2780c0796 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_mn.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_mn.c
@@ -142,7 +142,8 @@ static void amdgpu_mn_invalidate_range_start(struct mmu_notifier *mn,
 
 		list_for_each_entry(bo, &node->bos, mn_list) {
 
-			if (!bo->tbo.ttm || bo->tbo.ttm->state != tt_bound)
+			if (!amdgpu_ttm_tt_affect_userptr(bo->tbo.ttm, start,
+							  end))
 				continue;
 
 			r = amdgpu_bo_reserve(bo, true);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_mode.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_mode.h
index 064ebb347074..fdc1be8550da 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_mode.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_mode.h
@@ -35,6 +35,7 @@
 #include <drm/drm_dp_helper.h>
 #include <drm/drm_fixed.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
 #include <drm/drm_plane_helper.h>
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
@@ -556,7 +557,7 @@ int amdgpu_get_crtc_scanoutpos(struct drm_device *dev, unsigned int pipe,
 
 int amdgpu_framebuffer_init(struct drm_device *dev,
 			     struct amdgpu_framebuffer *rfb,
-			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     const struct drm_mode_fb_cmd2 *mode_cmd,
 			     struct drm_gem_object *obj);
 
 int amdgpufb_remove(struct drm_device *dev, struct drm_framebuffer *fb);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_object.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_object.c
index c3ce103b6a33..b8fbbd7699e4 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_object.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_object.c
@@ -33,6 +33,7 @@
 #include <linux/slab.h>
 #include <drm/drmP.h>
 #include <drm/amdgpu_drm.h>
+#include <drm/drm_cache.h>
 #include "amdgpu.h"
 #include "amdgpu_trace.h"
 
@@ -261,6 +262,13 @@ int amdgpu_bo_create_restricted(struct amdgpu_device *adev,
 				       AMDGPU_GEM_DOMAIN_OA);
 
 	bo->flags = flags;
+
+	/* For architectures that don't support WC memory,
+	 * mask out the WC flag from the BO
+	 */
+	if (!drm_arch_can_wc_memory())
+		bo->flags &= ~AMDGPU_GEM_CREATE_CPU_GTT_USWC;
+
 	amdgpu_fill_placement_to_bo(bo, placement);
 	/* Kernel allocation are uninterruptible */
 	r = ttm_bo_init(&adev->mman.bdev, &bo->tbo, size, type,
@@ -399,7 +407,8 @@ int amdgpu_bo_pin_restricted(struct amdgpu_bo *bo, u32 domain,
 		}
 		if (fpfn > bo->placements[i].fpfn)
 			bo->placements[i].fpfn = fpfn;
-		if (lpfn && lpfn < bo->placements[i].lpfn)
+		if (!bo->placements[i].lpfn ||
+		    (lpfn && lpfn < bo->placements[i].lpfn))
 			bo->placements[i].lpfn = lpfn;
 		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_object.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_object.h
index ea756e77b023..5107fb291bdb 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_object.h
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_object.h
@@ -96,6 +96,7 @@ static inline void amdgpu_bo_unreserve(struct amdgpu_bo *bo)
  */
 static inline u64 amdgpu_bo_gpu_offset(struct amdgpu_bo *bo)
 {
+	WARN_ON_ONCE(bo->tbo.mem.mem_type == TTM_PL_SYSTEM);
 	return bo->tbo.offset;
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_pm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_pm.c
index 22a8c7d3a3ab..95a4a25d8df9 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_pm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_pm.c
@@ -30,10 +30,16 @@
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
 
+#include "amd_powerplay.h"
+
 static int amdgpu_debugfs_pm_init(struct amdgpu_device *adev);
 
 void amdgpu_pm_acpi_event_handler(struct amdgpu_device *adev)
 {
+	if (adev->pp_enabled)
+		/* TODO */
+		return;
+
 	if (adev->pm.dpm_enabled) {
 		mutex_lock(&adev->pm.mutex);
 		if (power_supply_is_system_supplied() > 0)
@@ -52,7 +58,12 @@ static ssize_t amdgpu_get_dpm_state(struct device *dev,
 {
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct amdgpu_device *adev = ddev->dev_private;
-	enum amdgpu_pm_state_type pm = adev->pm.dpm.user_state;
+	enum amd_pm_state_type pm;
+
+	if (adev->pp_enabled) {
+		pm = amdgpu_dpm_get_current_power_state(adev);
+	} else
+		pm = adev->pm.dpm.user_state;
 
 	return snprintf(buf, PAGE_SIZE, "%s\n",
 			(pm == POWER_STATE_TYPE_BATTERY) ? "battery" :
@@ -66,40 +77,61 @@ static ssize_t amdgpu_set_dpm_state(struct device *dev,
 {
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct amdgpu_device *adev = ddev->dev_private;
+	enum amd_pm_state_type  state;
 
-	mutex_lock(&adev->pm.mutex);
 	if (strncmp("battery", buf, strlen("battery")) == 0)
-		adev->pm.dpm.user_state = POWER_STATE_TYPE_BATTERY;
+		state = POWER_STATE_TYPE_BATTERY;
 	else if (strncmp("balanced", buf, strlen("balanced")) == 0)
-		adev->pm.dpm.user_state = POWER_STATE_TYPE_BALANCED;
+		state = POWER_STATE_TYPE_BALANCED;
 	else if (strncmp("performance", buf, strlen("performance")) == 0)
-		adev->pm.dpm.user_state = POWER_STATE_TYPE_PERFORMANCE;
+		state = POWER_STATE_TYPE_PERFORMANCE;
 	else {
-		mutex_unlock(&adev->pm.mutex);
 		count = -EINVAL;
 		goto fail;
 	}
-	mutex_unlock(&adev->pm.mutex);
 
-	/* Can't set dpm state when the card is off */
-	if (!(adev->flags & AMD_IS_PX) ||
-	    (ddev->switch_power_state == DRM_SWITCH_POWER_ON))
-		amdgpu_pm_compute_clocks(adev);
+	if (adev->pp_enabled) {
+		amdgpu_dpm_dispatch_task(adev, AMD_PP_EVENT_ENABLE_USER_STATE, &state, NULL);
+	} else {
+		mutex_lock(&adev->pm.mutex);
+		adev->pm.dpm.user_state = state;
+		mutex_unlock(&adev->pm.mutex);
+
+		/* Can't set dpm state when the card is off */
+		if (!(adev->flags & AMD_IS_PX) ||
+		    (ddev->switch_power_state == DRM_SWITCH_POWER_ON))
+			amdgpu_pm_compute_clocks(adev);
+	}
 fail:
 	return count;
 }
 
 static ssize_t amdgpu_get_dpm_forced_performance_level(struct device *dev,
-						       struct device_attribute *attr,
-						       char *buf)
+						struct device_attribute *attr,
+								char *buf)
 {
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct amdgpu_device *adev = ddev->dev_private;
-	enum amdgpu_dpm_forced_level level = adev->pm.dpm.forced_level;
 
-	return snprintf(buf, PAGE_SIZE, "%s\n",
-			(level == AMDGPU_DPM_FORCED_LEVEL_AUTO) ? "auto" :
-			(level == AMDGPU_DPM_FORCED_LEVEL_LOW) ? "low" : "high");
+	if  ((adev->flags & AMD_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return snprintf(buf, PAGE_SIZE, "off\n");
+
+	if (adev->pp_enabled) {
+		enum amd_dpm_forced_level level;
+
+		level = amdgpu_dpm_get_performance_level(adev);
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+				(level == AMD_DPM_FORCED_LEVEL_AUTO) ? "auto" :
+				(level == AMD_DPM_FORCED_LEVEL_LOW) ? "low" : "high");
+	} else {
+		enum amdgpu_dpm_forced_level level;
+
+		level = adev->pm.dpm.forced_level;
+		return snprintf(buf, PAGE_SIZE, "%s\n",
+				(level == AMDGPU_DPM_FORCED_LEVEL_AUTO) ? "auto" :
+				(level == AMDGPU_DPM_FORCED_LEVEL_LOW) ? "low" : "high");
+	}
 }
 
 static ssize_t amdgpu_set_dpm_forced_performance_level(struct device *dev,
@@ -112,7 +144,11 @@ static ssize_t amdgpu_set_dpm_forced_performance_level(struct device *dev,
 	enum amdgpu_dpm_forced_level level;
 	int ret = 0;
 
-	mutex_lock(&adev->pm.mutex);
+	/* Can't force performance level when the card is off */
+	if  ((adev->flags & AMD_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
 	if (strncmp("low", buf, strlen("low")) == 0) {
 		level = AMDGPU_DPM_FORCED_LEVEL_LOW;
 	} else if (strncmp("high", buf, strlen("high")) == 0) {
@@ -123,18 +159,24 @@ static ssize_t amdgpu_set_dpm_forced_performance_level(struct device *dev,
 		count = -EINVAL;
 		goto fail;
 	}
-	if (adev->pm.funcs->force_performance_level) {
+
+	if (adev->pp_enabled)
+		amdgpu_dpm_force_performance_level(adev, level);
+	else {
+		mutex_lock(&adev->pm.mutex);
 		if (adev->pm.dpm.thermal_active) {
 			count = -EINVAL;
+			mutex_unlock(&adev->pm.mutex);
 			goto fail;
 		}
 		ret = amdgpu_dpm_force_performance_level(adev, level);
 		if (ret)
 			count = -EINVAL;
+		else
+			adev->pm.dpm.forced_level = level;
+		mutex_unlock(&adev->pm.mutex);
 	}
 fail:
-	mutex_unlock(&adev->pm.mutex);
-
 	return count;
 }
 
@@ -148,12 +190,18 @@ static ssize_t amdgpu_hwmon_show_temp(struct device *dev,
 				      char *buf)
 {
 	struct amdgpu_device *adev = dev_get_drvdata(dev);
+	struct drm_device *ddev = adev->ddev;
 	int temp;
 
-	if (adev->pm.funcs->get_temperature)
-		temp = amdgpu_dpm_get_temperature(adev);
-	else
+	/* Can't get temperature when the card is off */
+	if  ((adev->flags & AMD_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
+		return -EINVAL;
+
+	if (!adev->pp_enabled && !adev->pm.funcs->get_temperature)
 		temp = 0;
+	else
+		temp = amdgpu_dpm_get_temperature(adev);
 
 	return snprintf(buf, PAGE_SIZE, "%d\n", temp);
 }
@@ -181,8 +229,10 @@ static ssize_t amdgpu_hwmon_get_pwm1_enable(struct device *dev,
 	struct amdgpu_device *adev = dev_get_drvdata(dev);
 	u32 pwm_mode = 0;
 
-	if (adev->pm.funcs->get_fan_control_mode)
-		pwm_mode = amdgpu_dpm_get_fan_control_mode(adev);
+	if (!adev->pp_enabled && !adev->pm.funcs->get_fan_control_mode)
+		return -EINVAL;
+
+	pwm_mode = amdgpu_dpm_get_fan_control_mode(adev);
 
 	/* never 0 (full-speed), fuse or smc-controlled always */
 	return sprintf(buf, "%i\n", pwm_mode == FDO_PWM_MODE_STATIC ? 1 : 2);
@@ -197,7 +247,7 @@ static ssize_t amdgpu_hwmon_set_pwm1_enable(struct device *dev,
 	int err;
 	int value;
 
-	if(!adev->pm.funcs->set_fan_control_mode)
+	if (!adev->pp_enabled && !adev->pm.funcs->set_fan_control_mode)
 		return -EINVAL;
 
 	err = kstrtoint(buf, 10, &value);
@@ -290,11 +340,11 @@ static struct attribute *hwmon_attributes[] = {
 static umode_t hwmon_attributes_visible(struct kobject *kobj,
 					struct attribute *attr, int index)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct amdgpu_device *adev = dev_get_drvdata(dev);
 	umode_t effective_mode = attr->mode;
 
-	/* Skip attributes if DPM is not enabled */
+	/* Skip limit attributes if DPM is not enabled */
 	if (!adev->pm.dpm_enabled &&
 	    (attr == &sensor_dev_attr_temp1_crit.dev_attr.attr ||
 	     attr == &sensor_dev_attr_temp1_crit_hyst.dev_attr.attr ||
@@ -304,6 +354,9 @@ static umode_t hwmon_attributes_visible(struct kobject *kobj,
 	     attr == &sensor_dev_attr_pwm1_min.dev_attr.attr))
 		return 0;
 
+	if (adev->pp_enabled)
+		return effective_mode;
+
 	/* Skip fan attributes if fan is not present */
 	if (adev->pm.no_fan &&
 	    (attr == &sensor_dev_attr_pwm1.dev_attr.attr ||
@@ -351,7 +404,7 @@ void amdgpu_dpm_thermal_work_handler(struct work_struct *work)
 		container_of(work, struct amdgpu_device,
 			     pm.dpm.thermal.work);
 	/* switch to the thermal state */
-	enum amdgpu_pm_state_type dpm_state = POWER_STATE_TYPE_INTERNAL_THERMAL;
+	enum amd_pm_state_type dpm_state = POWER_STATE_TYPE_INTERNAL_THERMAL;
 
 	if (!adev->pm.dpm_enabled)
 		return;
@@ -379,7 +432,7 @@ void amdgpu_dpm_thermal_work_handler(struct work_struct *work)
 }
 
 static struct amdgpu_ps *amdgpu_dpm_pick_power_state(struct amdgpu_device *adev,
-						     enum amdgpu_pm_state_type dpm_state)
+						     enum amd_pm_state_type dpm_state)
 {
 	int i;
 	struct amdgpu_ps *ps;
@@ -516,7 +569,7 @@ static void amdgpu_dpm_change_power_state_locked(struct amdgpu_device *adev)
 {
 	int i;
 	struct amdgpu_ps *ps;
-	enum amdgpu_pm_state_type dpm_state;
+	enum amd_pm_state_type dpm_state;
 	int ret;
 
 	/* if dpm init failed */
@@ -595,11 +648,6 @@ force:
 
 	/* update display watermarks based on new power state */
 	amdgpu_display_bandwidth_update(adev);
-	/* update displays */
-	amdgpu_dpm_display_configuration_changed(adev);
-
-	adev->pm.dpm.current_active_crtcs = adev->pm.dpm.new_active_crtcs;
-	adev->pm.dpm.current_active_crtc_count = adev->pm.dpm.new_active_crtc_count;
 
 	/* wait for the rings to drain */
 	for (i = 0; i < AMDGPU_MAX_RINGS; i++) {
@@ -616,6 +664,12 @@ force:
 
 	amdgpu_dpm_post_set_power_state(adev);
 
+	/* update displays */
+	amdgpu_dpm_display_configuration_changed(adev);
+
+	adev->pm.dpm.current_active_crtcs = adev->pm.dpm.new_active_crtcs;
+	adev->pm.dpm.current_active_crtc_count = adev->pm.dpm.new_active_crtc_count;
+
 	if (adev->pm.funcs->force_performance_level) {
 		if (adev->pm.dpm.thermal_active) {
 			enum amdgpu_dpm_forced_level level = adev->pm.dpm.forced_level;
@@ -635,49 +689,54 @@ done:
 
 void amdgpu_dpm_enable_uvd(struct amdgpu_device *adev, bool enable)
 {
-	if (adev->pm.funcs->powergate_uvd) {
-		mutex_lock(&adev->pm.mutex);
-		/* enable/disable UVD */
+	if (adev->pp_enabled)
 		amdgpu_dpm_powergate_uvd(adev, !enable);
-		mutex_unlock(&adev->pm.mutex);
-	} else {
-		if (enable) {
+	else {
+		if (adev->pm.funcs->powergate_uvd) {
 			mutex_lock(&adev->pm.mutex);
-			adev->pm.dpm.uvd_active = true;
-			adev->pm.dpm.state = POWER_STATE_TYPE_INTERNAL_UVD;
+			/* enable/disable UVD */
+			amdgpu_dpm_powergate_uvd(adev, !enable);
 			mutex_unlock(&adev->pm.mutex);
 		} else {
-			mutex_lock(&adev->pm.mutex);
-			adev->pm.dpm.uvd_active = false;
-			mutex_unlock(&adev->pm.mutex);
+			if (enable) {
+				mutex_lock(&adev->pm.mutex);
+				adev->pm.dpm.uvd_active = true;
+				adev->pm.dpm.state = POWER_STATE_TYPE_INTERNAL_UVD;
+				mutex_unlock(&adev->pm.mutex);
+			} else {
+				mutex_lock(&adev->pm.mutex);
+				adev->pm.dpm.uvd_active = false;
+				mutex_unlock(&adev->pm.mutex);
+			}
+			amdgpu_pm_compute_clocks(adev);
 		}
 
-		amdgpu_pm_compute_clocks(adev);
 	}
 }
 
 void amdgpu_dpm_enable_vce(struct amdgpu_device *adev, bool enable)
 {
-	if (adev->pm.funcs->powergate_vce) {
-		mutex_lock(&adev->pm.mutex);
-		/* enable/disable VCE */
+	if (adev->pp_enabled)
 		amdgpu_dpm_powergate_vce(adev, !enable);
-
-		mutex_unlock(&adev->pm.mutex);
-	} else {
-		if (enable) {
+	else {
+		if (adev->pm.funcs->powergate_vce) {
 			mutex_lock(&adev->pm.mutex);
-			adev->pm.dpm.vce_active = true;
-			/* XXX select vce level based on ring/task */
-			adev->pm.dpm.vce_level = AMDGPU_VCE_LEVEL_AC_ALL;
+			amdgpu_dpm_powergate_vce(adev, !enable);
 			mutex_unlock(&adev->pm.mutex);
 		} else {
-			mutex_lock(&adev->pm.mutex);
-			adev->pm.dpm.vce_active = false;
-			mutex_unlock(&adev->pm.mutex);
+			if (enable) {
+				mutex_lock(&adev->pm.mutex);
+				adev->pm.dpm.vce_active = true;
+				/* XXX select vce level based on ring/task */
+				adev->pm.dpm.vce_level = AMDGPU_VCE_LEVEL_AC_ALL;
+				mutex_unlock(&adev->pm.mutex);
+			} else {
+				mutex_lock(&adev->pm.mutex);
+				adev->pm.dpm.vce_active = false;
+				mutex_unlock(&adev->pm.mutex);
+			}
+			amdgpu_pm_compute_clocks(adev);
 		}
-
-		amdgpu_pm_compute_clocks(adev);
 	}
 }
 
@@ -685,10 +744,13 @@ void amdgpu_pm_print_power_states(struct amdgpu_device *adev)
 {
 	int i;
 
-	for (i = 0; i < adev->pm.dpm.num_ps; i++) {
-		printk("== power state %d ==\n", i);
+	if (adev->pp_enabled)
+		/* TO DO */
+		return;
+
+	for (i = 0; i < adev->pm.dpm.num_ps; i++)
 		amdgpu_dpm_print_power_state(adev, &adev->pm.dpm.ps[i]);
-	}
+
 }
 
 int amdgpu_pm_sysfs_init(struct amdgpu_device *adev)
@@ -698,8 +760,11 @@ int amdgpu_pm_sysfs_init(struct amdgpu_device *adev)
 	if (adev->pm.sysfs_initialized)
 		return 0;
 
-	if (adev->pm.funcs->get_temperature == NULL)
-		return 0;
+	if (!adev->pp_enabled) {
+		if (adev->pm.funcs->get_temperature == NULL)
+			return 0;
+	}
+
 	adev->pm.int_hwmon_dev = hwmon_device_register_with_groups(adev->dev,
 								   DRIVER_NAME, adev,
 								   hwmon_groups);
@@ -748,32 +813,43 @@ void amdgpu_pm_compute_clocks(struct amdgpu_device *adev)
 	if (!adev->pm.dpm_enabled)
 		return;
 
-	mutex_lock(&adev->pm.mutex);
+	if (adev->pp_enabled) {
+		int i = 0;
 
-	/* update active crtc counts */
-	adev->pm.dpm.new_active_crtcs = 0;
-	adev->pm.dpm.new_active_crtc_count = 0;
-	if (adev->mode_info.num_crtc && adev->mode_info.mode_config_initialized) {
-		list_for_each_entry(crtc,
-				    &ddev->mode_config.crtc_list, head) {
-			amdgpu_crtc = to_amdgpu_crtc(crtc);
-			if (crtc->enabled) {
-				adev->pm.dpm.new_active_crtcs |= (1 << amdgpu_crtc->crtc_id);
-				adev->pm.dpm.new_active_crtc_count++;
+		amdgpu_display_bandwidth_update(adev);
+		mutex_lock(&adev->ring_lock);
+			for (i = 0; i < AMDGPU_MAX_RINGS; i++) {
+				struct amdgpu_ring *ring = adev->rings[i];
+				if (ring && ring->ready)
+					amdgpu_fence_wait_empty(ring);
 			}
-		}
-	}
+		mutex_unlock(&adev->ring_lock);
 
-	/* update battery/ac status */
-	if (power_supply_is_system_supplied() > 0)
-		adev->pm.dpm.ac_power = true;
-	else
-		adev->pm.dpm.ac_power = false;
-
-	amdgpu_dpm_change_power_state_locked(adev);
+		amdgpu_dpm_dispatch_task(adev, AMD_PP_EVENT_DISPLAY_CONFIG_CHANGE, NULL, NULL);
+	} else {
+		mutex_lock(&adev->pm.mutex);
+		adev->pm.dpm.new_active_crtcs = 0;
+		adev->pm.dpm.new_active_crtc_count = 0;
+		if (adev->mode_info.num_crtc && adev->mode_info.mode_config_initialized) {
+			list_for_each_entry(crtc,
+					    &ddev->mode_config.crtc_list, head) {
+				amdgpu_crtc = to_amdgpu_crtc(crtc);
+				if (crtc->enabled) {
+					adev->pm.dpm.new_active_crtcs |= (1 << amdgpu_crtc->crtc_id);
+					adev->pm.dpm.new_active_crtc_count++;
+				}
+			}
+		}
+		/* update battery/ac status */
+		if (power_supply_is_system_supplied() > 0)
+			adev->pm.dpm.ac_power = true;
+		else
+			adev->pm.dpm.ac_power = false;
 
-	mutex_unlock(&adev->pm.mutex);
+		amdgpu_dpm_change_power_state_locked(adev);
 
+		mutex_unlock(&adev->pm.mutex);
+	}
 }
 
 /*
@@ -786,8 +862,18 @@ static int amdgpu_debugfs_pm_info(struct seq_file *m, void *data)
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
 	struct drm_device *dev = node->minor->dev;
 	struct amdgpu_device *adev = dev->dev_private;
+	struct drm_device *ddev = adev->ddev;
 
-	if (adev->pm.dpm_enabled) {
+	if (!adev->pm.dpm_enabled) {
+		seq_printf(m, "dpm not enabled\n");
+		return 0;
+	}
+	if  ((adev->flags & AMD_IS_PX) &&
+	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON)) {
+		seq_printf(m, "PX asic powered off\n");
+	} else if (adev->pp_enabled) {
+		amdgpu_dpm_debugfs_print_current_performance_level(adev, m);
+	} else {
 		mutex_lock(&adev->pm.mutex);
 		if (adev->pm.funcs->debugfs_print_current_performance_level)
 			amdgpu_dpm_debugfs_print_current_performance_level(adev, m);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c
new file mode 100644
index 000000000000..3cb6d6c413c7
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.c
@@ -0,0 +1,330 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: AMD
+ *
+ */
+#include "atom.h"
+#include "amdgpu.h"
+#include "amd_shared.h"
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include "amdgpu_pm.h"
+#include <drm/amdgpu_drm.h>
+#include "amdgpu_powerplay.h"
+#include "cik_dpm.h"
+#include "vi_dpm.h"
+
+static int amdgpu_powerplay_init(struct amdgpu_device *adev)
+{
+	int ret = 0;
+	struct amd_powerplay *amd_pp;
+
+	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);
+
+		if (pp_init == NULL)
+			return -ENOMEM;
+
+		pp_init->chip_family = adev->family;
+		pp_init->chip_id = adev->asic_type;
+		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;
+
+		switch (adev->asic_type) {
+#ifdef CONFIG_DRM_AMDGPU_CIK
+		case CHIP_BONAIRE:
+		case CHIP_HAWAII:
+			amd_pp->ip_funcs = &ci_dpm_ip_funcs;
+			break;
+		case CHIP_KABINI:
+		case CHIP_MULLINS:
+		case CHIP_KAVERI:
+			amd_pp->ip_funcs = &kv_dpm_ip_funcs;
+			break;
+#endif
+		case CHIP_TOPAZ:
+			amd_pp->ip_funcs = &iceland_dpm_ip_funcs;
+			break;
+		case CHIP_TONGA:
+			amd_pp->ip_funcs = &tonga_dpm_ip_funcs;
+			break;
+		case CHIP_FIJI:
+			amd_pp->ip_funcs = &fiji_dpm_ip_funcs;
+			break;
+		case CHIP_CARRIZO:
+		case CHIP_STONEY:
+			amd_pp->ip_funcs = &cz_dpm_ip_funcs;
+			break;
+		default:
+			ret = -EINVAL;
+			break;
+		}
+	}
+	return ret;
+}
+
+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_TONGA:
+	case CHIP_FIJI:
+		adev->pp_enabled = (amdgpu_powerplay == 0) ? false : true;
+		break;
+	case CHIP_CARRIZO:
+	case CHIP_STONEY:
+		adev->pp_enabled = (amdgpu_powerplay > 0) ? true : false;
+		break;
+	/* These chips don't have powerplay implemenations */
+	case CHIP_BONAIRE:
+	case CHIP_HAWAII:
+	case CHIP_KABINI:
+	case CHIP_MULLINS:
+	case CHIP_KAVERI:
+	case CHIP_TOPAZ:
+	default:
+		adev->pp_enabled = false;
+		break;
+	}
+#else
+	adev->pp_enabled = false;
+#endif
+
+	ret = amdgpu_powerplay_init(adev);
+	if (ret)
+		return ret;
+
+	if (adev->powerplay.ip_funcs->early_init)
+		ret = adev->powerplay.ip_funcs->early_init(
+					adev->powerplay.pp_handle);
+	return ret;
+}
+
+
+static int amdgpu_pp_late_init(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->late_init)
+		ret = adev->powerplay.ip_funcs->late_init(
+					adev->powerplay.pp_handle);
+
+#ifdef CONFIG_DRM_AMD_POWERPLAY
+	if (adev->pp_enabled) {
+		amdgpu_pm_sysfs_init(adev);
+		amdgpu_dpm_dispatch_task(adev, AMD_PP_EVENT_COMPLETE_INIT, NULL, NULL);
+	}
+#endif
+	return ret;
+}
+
+static int amdgpu_pp_sw_init(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->sw_init)
+		ret = adev->powerplay.ip_funcs->sw_init(
+					adev->powerplay.pp_handle);
+
+#ifdef CONFIG_DRM_AMD_POWERPLAY
+	if (adev->pp_enabled) {
+		if (amdgpu_dpm == 0)
+			adev->pm.dpm_enabled = false;
+		else
+			adev->pm.dpm_enabled = true;
+	}
+#endif
+
+	return ret;
+}
+
+static int amdgpu_pp_sw_fini(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->sw_fini)
+		ret = adev->powerplay.ip_funcs->sw_fini(
+					adev->powerplay.pp_handle);
+	if (ret)
+		return ret;
+
+#ifdef CONFIG_DRM_AMD_POWERPLAY
+	if (adev->pp_enabled) {
+		amdgpu_pm_sysfs_fini(adev);
+		amd_powerplay_fini(adev->powerplay.pp_handle);
+	}
+#endif
+
+	return ret;
+}
+
+static int amdgpu_pp_hw_init(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->pp_enabled && adev->firmware.smu_load)
+		amdgpu_ucode_init_bo(adev);
+
+	if (adev->powerplay.ip_funcs->hw_init)
+		ret = adev->powerplay.ip_funcs->hw_init(
+					adev->powerplay.pp_handle);
+
+	return ret;
+}
+
+static int amdgpu_pp_hw_fini(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->hw_fini)
+		ret = adev->powerplay.ip_funcs->hw_fini(
+					adev->powerplay.pp_handle);
+
+	if (adev->pp_enabled && adev->firmware.smu_load)
+		amdgpu_ucode_fini_bo(adev);
+
+	return ret;
+}
+
+static int amdgpu_pp_suspend(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->suspend)
+		ret = adev->powerplay.ip_funcs->suspend(
+					 adev->powerplay.pp_handle);
+	return ret;
+}
+
+static int amdgpu_pp_resume(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->resume)
+		ret = adev->powerplay.ip_funcs->resume(
+					adev->powerplay.pp_handle);
+	return ret;
+}
+
+static int amdgpu_pp_set_clockgating_state(void *handle,
+					enum amd_clockgating_state state)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->set_clockgating_state)
+		ret = adev->powerplay.ip_funcs->set_clockgating_state(
+				adev->powerplay.pp_handle, state);
+	return ret;
+}
+
+static int amdgpu_pp_set_powergating_state(void *handle,
+					enum amd_powergating_state state)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->set_powergating_state)
+		ret = adev->powerplay.ip_funcs->set_powergating_state(
+				 adev->powerplay.pp_handle, state);
+	return ret;
+}
+
+
+static bool amdgpu_pp_is_idle(void *handle)
+{
+	bool ret = true;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->is_idle)
+		ret = adev->powerplay.ip_funcs->is_idle(
+					adev->powerplay.pp_handle);
+	return ret;
+}
+
+static int amdgpu_pp_wait_for_idle(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->wait_for_idle)
+		ret = adev->powerplay.ip_funcs->wait_for_idle(
+					adev->powerplay.pp_handle);
+	return ret;
+}
+
+static int amdgpu_pp_soft_reset(void *handle)
+{
+	int ret = 0;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->soft_reset)
+		ret = adev->powerplay.ip_funcs->soft_reset(
+					adev->powerplay.pp_handle);
+	return ret;
+}
+
+static void amdgpu_pp_print_status(void *handle)
+{
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (adev->powerplay.ip_funcs->print_status)
+		adev->powerplay.ip_funcs->print_status(
+					adev->powerplay.pp_handle);
+}
+
+const struct amd_ip_funcs amdgpu_pp_ip_funcs = {
+	.early_init = amdgpu_pp_early_init,
+	.late_init = amdgpu_pp_late_init,
+	.sw_init = amdgpu_pp_sw_init,
+	.sw_fini = amdgpu_pp_sw_fini,
+	.hw_init = amdgpu_pp_hw_init,
+	.hw_fini = amdgpu_pp_hw_fini,
+	.suspend = amdgpu_pp_suspend,
+	.resume = amdgpu_pp_resume,
+	.is_idle = amdgpu_pp_is_idle,
+	.wait_for_idle = amdgpu_pp_wait_for_idle,
+	.soft_reset = amdgpu_pp_soft_reset,
+	.print_status = amdgpu_pp_print_status,
+	.set_clockgating_state = amdgpu_pp_set_clockgating_state,
+	.set_powergating_state = amdgpu_pp_set_powergating_state,
+};
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.h b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.h
new file mode 100644
index 000000000000..da5cf47cfd99
--- /dev/null
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_powerplay.h
@@ -0,0 +1,33 @@
+/*
+ *  Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: AMD
+ *
+ */
+
+#ifndef __AMDGPU_POPWERPLAY_H__
+#define __AMDGPU_POPWERPLAY_H__
+
+#include "amd_shared.h"
+
+extern const struct amd_ip_funcs amdgpu_pp_ip_funcs;
+
+#endif /* __AMDSOC_DM_H__ */
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
index 78e9b0f14661..d1f234dd2126 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ring.c
@@ -487,7 +487,7 @@ static int amdgpu_debugfs_ring_info(struct seq_file *m, void *data)
 	seq_printf(m, "rptr: 0x%08x [%5d]\n",
 		   rptr, rptr);
 
-	rptr_next = ~0;
+	rptr_next = le32_to_cpu(*ring->next_rptr_cpu_addr);
 
 	seq_printf(m, "driver's copy of the wptr: 0x%08x [%5d]\n",
 		   ring->wptr, ring->wptr);
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_sa.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_sa.c
index 8b88edb0434b..ca72a2e487b9 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_sa.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_sa.c
@@ -354,12 +354,15 @@ int amdgpu_sa_bo_new(struct amdgpu_sa_manager *sa_manager,
 
 		for (i = 0, count = 0; i < AMDGPU_MAX_RINGS; ++i)
 			if (fences[i])
-				fences[count++] = fences[i];
+				fences[count++] = fence_get(fences[i]);
 
 		if (count) {
 			spin_unlock(&sa_manager->wq.lock);
 			t = fence_wait_any_timeout(fences, count, false,
 						   MAX_SCHEDULE_TIMEOUT);
+			for (i = 0; i < count; ++i)
+				fence_put(fences[i]);
+
 			r = (t > 0) ? 0 : t;
 			spin_lock(&sa_manager->wq.lock);
 		} else {
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_sync.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_sync.c
index dd005c336c97..181ce39ef5e5 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_sync.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_sync.c
@@ -293,7 +293,8 @@ int amdgpu_sync_rings(struct amdgpu_sync *sync,
 		fence = to_amdgpu_fence(sync->sync_to[i]);
 
 		/* check if we really need to sync */
-		if (!amdgpu_fence_need_sync(fence, ring))
+		if (!amdgpu_enable_scheduler &&
+		    !amdgpu_fence_need_sync(fence, ring))
 			continue;
 
 		/* prevent GPU deadlocks */
@@ -303,7 +304,7 @@ int amdgpu_sync_rings(struct amdgpu_sync *sync,
 		}
 
 		if (amdgpu_enable_scheduler || !amdgpu_enable_semaphores) {
-			r = fence_wait(&fence->base, true);
+			r = fence_wait(sync->sync_to[i], true);
 			if (r)
 				return r;
 			continue;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
index 8a1752ff3d8e..1cbb16e15307 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_ttm.c
@@ -712,7 +712,7 @@ static int amdgpu_ttm_tt_populate(struct ttm_tt *ttm)
 						       0, PAGE_SIZE,
 						       PCI_DMA_BIDIRECTIONAL);
 		if (pci_dma_mapping_error(adev->pdev, gtt->ttm.dma_address[i])) {
-			while (--i) {
+			while (i--) {
 				pci_unmap_page(adev->pdev, gtt->ttm.dma_address[i],
 					       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 				gtt->ttm.dma_address[i] = 0;
@@ -783,6 +783,25 @@ bool amdgpu_ttm_tt_has_userptr(struct ttm_tt *ttm)
 	return !!gtt->userptr;
 }
 
+bool amdgpu_ttm_tt_affect_userptr(struct ttm_tt *ttm, unsigned long start,
+				  unsigned long end)
+{
+	struct amdgpu_ttm_tt *gtt = (void *)ttm;
+	unsigned long size;
+
+	if (gtt == NULL)
+		return false;
+
+	if (gtt->ttm.ttm.state != tt_bound || !gtt->userptr)
+		return false;
+
+	size = (unsigned long)gtt->ttm.ttm.num_pages * PAGE_SIZE;
+	if (gtt->userptr > end || gtt->userptr + size <= start)
+		return false;
+
+	return true;
+}
+
 bool amdgpu_ttm_tt_is_readonly(struct ttm_tt *ttm)
 {
 	struct amdgpu_ttm_tt *gtt = (void *)ttm;
@@ -808,7 +827,7 @@ uint32_t amdgpu_ttm_tt_pte_flags(struct amdgpu_device *adev, struct ttm_tt *ttm,
 			flags |= AMDGPU_PTE_SNOOPED;
 	}
 
-	if (adev->asic_type >= CHIP_TOPAZ)
+	if (adev->asic_type >= CHIP_TONGA)
 		flags |= AMDGPU_PTE_EXECUTABLE;
 
 	flags |= AMDGPU_PTE_READABLE;
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
index b53d273eb7a1..9599f7559b3d 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
+++ b/drivers/gpu/drm/amd/amdgpu/amdgpu_vm.c
@@ -75,50 +75,77 @@ static unsigned amdgpu_vm_directory_size(struct amdgpu_device *adev)
 }
 
 /**
- * amdgpu_vm_get_bos - add the vm BOs to a validation list
+ * amdgpu_vm_get_pd_bo - add the VM PD to a validation list
  *
  * @vm: vm providing the BOs
- * @head: head of validation list
+ * @validated: head of validation list
+ * @entry: entry to add
  *
  * Add the page directory to the list of BOs to
- * validate for command submission (cayman+).
+ * validate for command submission.
  */
-struct amdgpu_bo_list_entry *amdgpu_vm_get_bos(struct amdgpu_device *adev,
-					  struct amdgpu_vm *vm,
-					  struct list_head *head)
+void amdgpu_vm_get_pd_bo(struct amdgpu_vm *vm,
+			 struct list_head *validated,
+			 struct amdgpu_bo_list_entry *entry)
 {
-	struct amdgpu_bo_list_entry *list;
-	unsigned i, idx;
+	entry->robj = vm->page_directory;
+	entry->prefered_domains = AMDGPU_GEM_DOMAIN_VRAM;
+	entry->allowed_domains = AMDGPU_GEM_DOMAIN_VRAM;
+	entry->priority = 0;
+	entry->tv.bo = &vm->page_directory->tbo;
+	entry->tv.shared = true;
+	list_add(&entry->tv.head, validated);
+}
 
-	list = drm_malloc_ab(vm->max_pde_used + 2,
-			     sizeof(struct amdgpu_bo_list_entry));
-	if (!list) {
-		return NULL;
-	}
+/**
+ * amdgpu_vm_get_bos - add the vm BOs to a duplicates list
+ *
+ * @vm: vm providing the BOs
+ * @duplicates: head of duplicates list
+ *
+ * Add the page directory to the BO duplicates list
+ * for command submission.
+ */
+void amdgpu_vm_get_pt_bos(struct amdgpu_vm *vm, struct list_head *duplicates)
+{
+	unsigned i;
 
 	/* add the vm page table to the list */
-	list[0].robj = vm->page_directory;
-	list[0].prefered_domains = AMDGPU_GEM_DOMAIN_VRAM;
-	list[0].allowed_domains = AMDGPU_GEM_DOMAIN_VRAM;
-	list[0].priority = 0;
-	list[0].tv.bo = &vm->page_directory->tbo;
-	list[0].tv.shared = true;
-	list_add(&list[0].tv.head, head);
-
-	for (i = 0, idx = 1; i <= vm->max_pde_used; i++) {
-		if (!vm->page_tables[i].bo)
+	for (i = 0; i <= vm->max_pde_used; ++i) {
+		struct amdgpu_bo_list_entry *entry = &vm->page_tables[i].entry;
+
+		if (!entry->robj)
 			continue;
 
-		list[idx].robj = vm->page_tables[i].bo;
-		list[idx].prefered_domains = AMDGPU_GEM_DOMAIN_VRAM;
-		list[idx].allowed_domains = AMDGPU_GEM_DOMAIN_VRAM;
-		list[idx].priority = 0;
-		list[idx].tv.bo = &list[idx].robj->tbo;
-		list[idx].tv.shared = true;
-		list_add(&list[idx++].tv.head, head);
+		list_add(&entry->tv.head, duplicates);
 	}
 
-	return list;
+}
+
+/**
+ * amdgpu_vm_move_pt_bos_in_lru - move the PT BOs to the LRU tail
+ *
+ * @adev: amdgpu device instance
+ * @vm: vm providing the BOs
+ *
+ * Move the PT BOs to the tail of the LRU.
+ */
+void amdgpu_vm_move_pt_bos_in_lru(struct amdgpu_device *adev,
+				  struct amdgpu_vm *vm)
+{
+	struct ttm_bo_global *glob = adev->mman.bdev.glob;
+	unsigned i;
+
+	spin_lock(&glob->lru_lock);
+	for (i = 0; i <= vm->max_pde_used; ++i) {
+		struct amdgpu_bo_list_entry *entry = &vm->page_tables[i].entry;
+
+		if (!entry->robj)
+			continue;
+
+		ttm_bo_move_to_lru_tail(&entry->robj->tbo);
+	}
+	spin_unlock(&glob->lru_lock);
 }
 
 /**
@@ -461,7 +488,7 @@ int amdgpu_vm_update_page_directory(struct amdgpu_device *adev,
 
 	/* walk over the address space and update the page directory */
 	for (pt_idx = 0; pt_idx <= vm->max_pde_used; ++pt_idx) {
-		struct amdgpu_bo *bo = vm->page_tables[pt_idx].bo;
+		struct amdgpu_bo *bo = vm->page_tables[pt_idx].entry.robj;
 		uint64_t pde, pt;
 
 		if (bo == NULL)
@@ -638,7 +665,7 @@ static int amdgpu_vm_update_ptes(struct amdgpu_device *adev,
 	/* walk over the address space and update the page tables */
 	for (addr = start; addr < end; ) {
 		uint64_t pt_idx = addr >> amdgpu_vm_block_size;
-		struct amdgpu_bo *pt = vm->page_tables[pt_idx].bo;
+		struct amdgpu_bo *pt = vm->page_tables[pt_idx].entry.robj;
 		unsigned nptes;
 		uint64_t pte;
 		int r;
@@ -1010,13 +1037,13 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 		return -EINVAL;
 
 	/* make sure object fit at this offset */
-	eaddr = saddr + size;
+	eaddr = saddr + size - 1;
 	if ((saddr >= eaddr) || (offset + size > amdgpu_bo_size(bo_va->bo)))
 		return -EINVAL;
 
 	last_pfn = eaddr / AMDGPU_GPU_PAGE_SIZE;
-	if (last_pfn > adev->vm_manager.max_pfn) {
-		dev_err(adev->dev, "va above limit (0x%08X > 0x%08X)\n",
+	if (last_pfn >= adev->vm_manager.max_pfn) {
+		dev_err(adev->dev, "va above limit (0x%08X >= 0x%08X)\n",
 			last_pfn, adev->vm_manager.max_pfn);
 		return -EINVAL;
 	}
@@ -1025,7 +1052,7 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 	eaddr /= AMDGPU_GPU_PAGE_SIZE;
 
 	spin_lock(&vm->it_lock);
-	it = interval_tree_iter_first(&vm->va, saddr, eaddr - 1);
+	it = interval_tree_iter_first(&vm->va, saddr, eaddr);
 	spin_unlock(&vm->it_lock);
 	if (it) {
 		struct amdgpu_bo_va_mapping *tmp;
@@ -1046,7 +1073,7 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 
 	INIT_LIST_HEAD(&mapping->list);
 	mapping->it.start = saddr;
-	mapping->it.last = eaddr - 1;
+	mapping->it.last = eaddr;
 	mapping->offset = offset;
 	mapping->flags = flags;
 
@@ -1070,9 +1097,11 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 	/* walk over the address space and allocate the page tables */
 	for (pt_idx = saddr; pt_idx <= eaddr; ++pt_idx) {
 		struct reservation_object *resv = vm->page_directory->tbo.resv;
+		struct amdgpu_bo_list_entry *entry;
 		struct amdgpu_bo *pt;
 
-		if (vm->page_tables[pt_idx].bo)
+		entry = &vm->page_tables[pt_idx].entry;
+		if (entry->robj)
 			continue;
 
 		r = amdgpu_bo_create(adev, AMDGPU_VM_PTE_COUNT * 8,
@@ -1094,8 +1123,13 @@ int amdgpu_vm_bo_map(struct amdgpu_device *adev,
 			goto error_free;
 		}
 
+		entry->robj = pt;
+		entry->prefered_domains = AMDGPU_GEM_DOMAIN_VRAM;
+		entry->allowed_domains = AMDGPU_GEM_DOMAIN_VRAM;
+		entry->priority = 0;
+		entry->tv.bo = &entry->robj->tbo;
+		entry->tv.shared = true;
 		vm->page_tables[pt_idx].addr = 0;
-		vm->page_tables[pt_idx].bo = pt;
 	}
 
 	return 0;
@@ -1248,7 +1282,7 @@ int amdgpu_vm_init(struct amdgpu_device *adev, struct amdgpu_vm *vm)
 {
 	const unsigned align = min(AMDGPU_VM_PTB_ALIGN_SIZE,
 		AMDGPU_VM_PTE_COUNT * 8);
-	unsigned pd_size, pd_entries, pts_size;
+	unsigned pd_size, pd_entries;
 	int i, r;
 
 	for (i = 0; i < AMDGPU_MAX_RINGS; ++i) {
@@ -1266,8 +1300,7 @@ int amdgpu_vm_init(struct amdgpu_device *adev, struct amdgpu_vm *vm)
 	pd_entries = amdgpu_vm_num_pdes(adev);
 
 	/* allocate page table array */
-	pts_size = pd_entries * sizeof(struct amdgpu_vm_pt);
-	vm->page_tables = kzalloc(pts_size, GFP_KERNEL);
+	vm->page_tables = drm_calloc_large(pd_entries, sizeof(struct amdgpu_vm_pt));
 	if (vm->page_tables == NULL) {
 		DRM_ERROR("Cannot allocate memory for page table array\n");
 		return -ENOMEM;
@@ -1326,8 +1359,8 @@ void amdgpu_vm_fini(struct amdgpu_device *adev, struct amdgpu_vm *vm)
 	}
 
 	for (i = 0; i < amdgpu_vm_num_pdes(adev); i++)
-		amdgpu_bo_unref(&vm->page_tables[i].bo);
-	kfree(vm->page_tables);
+		amdgpu_bo_unref(&vm->page_tables[i].entry.robj);
+	drm_free_large(vm->page_tables);
 
 	amdgpu_bo_unref(&vm->page_directory);
 	fence_put(vm->page_directory_fence);
diff --git a/drivers/gpu/drm/amd/amdgpu/atombios_dp.c b/drivers/gpu/drm/amd/amdgpu/atombios_dp.c
index 92b6acadfc52..bf731e9f643e 100644
--- a/drivers/gpu/drm/amd/amdgpu/atombios_dp.c
+++ b/drivers/gpu/drm/amd/amdgpu/atombios_dp.c
@@ -243,7 +243,7 @@ static void amdgpu_atombios_dp_get_adjust_train(const u8 link_status[DP_LINK_STA
 
 /* convert bits per color to bits per pixel */
 /* get bpc from the EDID */
-static int amdgpu_atombios_dp_convert_bpc_to_bpp(int bpc)
+static unsigned amdgpu_atombios_dp_convert_bpc_to_bpp(int bpc)
 {
 	if (bpc == 0)
 		return 24;
@@ -251,64 +251,44 @@ static int amdgpu_atombios_dp_convert_bpc_to_bpp(int bpc)
 		return bpc * 3;
 }
 
-/* get the max pix clock supported by the link rate and lane num */
-static int amdgpu_atombios_dp_get_max_dp_pix_clock(int link_rate,
-					    int lane_num,
-					    int bpp)
-{
-	return (link_rate * lane_num * 8) / bpp;
-}
-
 /***** amdgpu specific DP functions *****/
 
-/* First get the min lane# when low rate is used according to pixel clock
- * (prefer low rate), second check max lane# supported by DP panel,
- * if the max lane# < low rate lane# then use max lane# instead.
- */
-static int amdgpu_atombios_dp_get_dp_lane_number(struct drm_connector *connector,
+static int amdgpu_atombios_dp_get_dp_link_config(struct drm_connector *connector,
 						 const u8 dpcd[DP_DPCD_SIZE],
-						 int pix_clock)
+						 unsigned pix_clock,
+						 unsigned *dp_lanes, unsigned *dp_rate)
 {
-	int bpp = amdgpu_atombios_dp_convert_bpc_to_bpp(amdgpu_connector_get_monitor_bpc(connector));
-	int max_link_rate = drm_dp_max_link_rate(dpcd);
-	int max_lane_num = drm_dp_max_lane_count(dpcd);
-	int lane_num;
-	int max_dp_pix_clock;
-
-	for (lane_num = 1; lane_num < max_lane_num; lane_num <<= 1) {
-		max_dp_pix_clock = amdgpu_atombios_dp_get_max_dp_pix_clock(max_link_rate, lane_num, bpp);
-		if (pix_clock <= max_dp_pix_clock)
-			break;
-	}
-
-	return lane_num;
-}
-
-static int amdgpu_atombios_dp_get_dp_link_clock(struct drm_connector *connector,
-						const u8 dpcd[DP_DPCD_SIZE],
-						int pix_clock)
-{
-	int bpp = amdgpu_atombios_dp_convert_bpc_to_bpp(amdgpu_connector_get_monitor_bpc(connector));
-	int lane_num, max_pix_clock;
+	unsigned bpp =
+		amdgpu_atombios_dp_convert_bpc_to_bpp(amdgpu_connector_get_monitor_bpc(connector));
+	static const unsigned link_rates[3] = { 162000, 270000, 540000 };
+	unsigned max_link_rate = drm_dp_max_link_rate(dpcd);
+	unsigned max_lane_num = drm_dp_max_lane_count(dpcd);
+	unsigned lane_num, i, max_pix_clock;
 
 	if (amdgpu_connector_encoder_get_dp_bridge_encoder_id(connector) ==
-	    ENCODER_OBJECT_ID_NUTMEG)
-		return 270000;
-
-	lane_num = amdgpu_atombios_dp_get_dp_lane_number(connector, dpcd, pix_clock);
-	max_pix_clock = amdgpu_atombios_dp_get_max_dp_pix_clock(162000, lane_num, bpp);
-	if (pix_clock <= max_pix_clock)
-		return 162000;
-	max_pix_clock = amdgpu_atombios_dp_get_max_dp_pix_clock(270000, lane_num, bpp);
-	if (pix_clock <= max_pix_clock)
-		return 270000;
-	if (amdgpu_connector_is_dp12_capable(connector)) {
-		max_pix_clock = amdgpu_atombios_dp_get_max_dp_pix_clock(540000, lane_num, bpp);
-		if (pix_clock <= max_pix_clock)
-			return 540000;
+	    ENCODER_OBJECT_ID_NUTMEG) {
+		for (lane_num = 1; lane_num <= max_lane_num; lane_num <<= 1) {
+			max_pix_clock = (lane_num * 270000 * 8) / bpp;
+			if (max_pix_clock >= pix_clock) {
+				*dp_lanes = lane_num;
+				*dp_rate = 270000;
+				return 0;
+			}
+		}
+	} else {
+		for (lane_num = 1; lane_num <= max_lane_num; lane_num <<= 1) {
+			for (i = 0; i < ARRAY_SIZE(link_rates) && link_rates[i] <= max_link_rate; i++) {
+				max_pix_clock = (lane_num * link_rates[i] * 8) / bpp;
+				if (max_pix_clock >= pix_clock) {
+					*dp_lanes = lane_num;
+					*dp_rate = link_rates[i];
+					return 0;
+				}
+			}
+		}
 	}
 
-	return drm_dp_max_link_rate(dpcd);
+	return -EINVAL;
 }
 
 static u8 amdgpu_atombios_dp_encoder_service(struct amdgpu_device *adev,
@@ -422,6 +402,7 @@ void amdgpu_atombios_dp_set_link_config(struct drm_connector *connector,
 {
 	struct amdgpu_connector *amdgpu_connector = to_amdgpu_connector(connector);
 	struct amdgpu_connector_atom_dig *dig_connector;
+	int ret;
 
 	if (!amdgpu_connector->con_priv)
 		return;
@@ -429,10 +410,14 @@ void amdgpu_atombios_dp_set_link_config(struct drm_connector *connector,
 
 	if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
 	    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP)) {
-		dig_connector->dp_clock =
-			amdgpu_atombios_dp_get_dp_link_clock(connector, dig_connector->dpcd, mode->clock);
-		dig_connector->dp_lane_count =
-			amdgpu_atombios_dp_get_dp_lane_number(connector, dig_connector->dpcd, mode->clock);
+		ret = amdgpu_atombios_dp_get_dp_link_config(connector, dig_connector->dpcd,
+							    mode->clock,
+							    &dig_connector->dp_lane_count,
+							    &dig_connector->dp_clock);
+		if (ret) {
+			dig_connector->dp_clock = 0;
+			dig_connector->dp_lane_count = 0;
+		}
 	}
 }
 
@@ -441,14 +426,17 @@ int amdgpu_atombios_dp_mode_valid_helper(struct drm_connector *connector,
 {
 	struct amdgpu_connector *amdgpu_connector = to_amdgpu_connector(connector);
 	struct amdgpu_connector_atom_dig *dig_connector;
-	int dp_clock;
+	unsigned dp_lanes, dp_clock;
+	int ret;
 
 	if (!amdgpu_connector->con_priv)
 		return MODE_CLOCK_HIGH;
 	dig_connector = amdgpu_connector->con_priv;
 
-	dp_clock =
-		amdgpu_atombios_dp_get_dp_link_clock(connector, dig_connector->dpcd, mode->clock);
+	ret = amdgpu_atombios_dp_get_dp_link_config(connector, dig_connector->dpcd,
+						    mode->clock, &dp_lanes, &dp_clock);
+	if (ret)
+		return MODE_CLOCK_HIGH;
 
 	if ((dp_clock == 540000) &&
 	    (!amdgpu_connector_is_dp12_capable(connector)))
diff --git a/drivers/gpu/drm/amd/amdgpu/ci_dpm.c b/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
index 57a2e347f04d..474ca02b0949 100644
--- a/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/ci_dpm.c
@@ -31,6 +31,7 @@
 #include "ci_dpm.h"
 #include "gfx_v7_0.h"
 #include "atom.h"
+#include "amd_pcie.h"
 #include <linux/seq_file.h>
 
 #include "smu/smu_7_0_1_d.h"
@@ -1395,7 +1396,6 @@ static void ci_thermal_stop_thermal_controller(struct amdgpu_device *adev)
 		ci_fan_ctrl_set_default_mode(adev);
 }
 
-#if 0
 static int ci_read_smc_soft_register(struct amdgpu_device *adev,
 				     u16 reg_offset, u32 *value)
 {
@@ -1405,7 +1405,6 @@ static int ci_read_smc_soft_register(struct amdgpu_device *adev,
 				      pi->soft_regs_start + reg_offset,
 				      value, pi->sram_end);
 }
-#endif
 
 static int ci_write_smc_soft_register(struct amdgpu_device *adev,
 				      u16 reg_offset, u32 value)
@@ -5837,18 +5836,16 @@ static int ci_dpm_init(struct amdgpu_device *adev)
 	u8 frev, crev;
 	struct ci_power_info *pi;
 	int ret;
-	u32 mask;
 
 	pi = kzalloc(sizeof(struct ci_power_info), GFP_KERNEL);
 	if (pi == NULL)
 		return -ENOMEM;
 	adev->pm.dpm.priv = pi;
 
-	ret = drm_pcie_get_speed_cap_mask(adev->ddev, &mask);
-	if (ret)
-		pi->sys_pcie_mask = 0;
-	else
-		pi->sys_pcie_mask = mask;
+	pi->sys_pcie_mask =
+		(adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_MASK) >>
+		CAIL_PCIE_LINK_SPEED_SUPPORT_SHIFT;
+
 	pi->force_pcie_gen = AMDGPU_PCIE_GEN_INVALID;
 
 	pi->pcie_gen_performance.max = AMDGPU_PCIE_GEN1;
@@ -6084,11 +6081,23 @@ ci_dpm_debugfs_print_current_performance_level(struct amdgpu_device *adev,
 	struct amdgpu_ps *rps = &pi->current_rps;
 	u32 sclk = ci_get_average_sclk_freq(adev);
 	u32 mclk = ci_get_average_mclk_freq(adev);
+	u32 activity_percent = 50;
+	int ret;
+
+	ret = ci_read_smc_soft_register(adev, offsetof(SMU7_SoftRegisters, AverageGraphicsA),
+					&activity_percent);
+
+	if (ret == 0) {
+		activity_percent += 0x80;
+		activity_percent >>= 8;
+		activity_percent = activity_percent > 100 ? 100 : activity_percent;
+	}
 
 	seq_printf(m, "uvd %sabled\n", pi->uvd_enabled ? "en" : "dis");
 	seq_printf(m, "vce %sabled\n", rps->vce_active ? "en" : "dis");
 	seq_printf(m, "power level avg    sclk: %u mclk: %u\n",
 		   sclk, mclk);
+	seq_printf(m, "GPU load: %u %%\n", activity_percent);
 }
 
 static void ci_dpm_print_power_state(struct amdgpu_device *adev,
diff --git a/drivers/gpu/drm/amd/amdgpu/cik.c b/drivers/gpu/drm/amd/amdgpu/cik.c
index 484710cfdf82..155965ed14a3 100644
--- a/drivers/gpu/drm/amd/amdgpu/cik.c
+++ b/drivers/gpu/drm/amd/amdgpu/cik.c
@@ -32,6 +32,7 @@
 #include "amdgpu_vce.h"
 #include "cikd.h"
 #include "atom.h"
+#include "amd_pcie.h"
 
 #include "cik.h"
 #include "gmc_v7_0.h"
@@ -65,6 +66,7 @@
 #include "oss/oss_2_0_sh_mask.h"
 
 #include "amdgpu_amdkfd.h"
+#include "amdgpu_powerplay.h"
 
 /*
  * Indirect registers accessor
@@ -929,6 +931,37 @@ static bool cik_read_disabled_bios(struct amdgpu_device *adev)
 	return r;
 }
 
+static bool cik_read_bios_from_rom(struct amdgpu_device *adev,
+				   u8 *bios, u32 length_bytes)
+{
+	u32 *dw_ptr;
+	unsigned long flags;
+	u32 i, length_dw;
+
+	if (bios == NULL)
+		return false;
+	if (length_bytes == 0)
+		return false;
+	/* APU vbios image is part of sbios image */
+	if (adev->flags & AMD_IS_APU)
+		return false;
+
+	dw_ptr = (u32 *)bios;
+	length_dw = ALIGN(length_bytes, 4) / 4;
+	/* take the smc lock since we are using the smc index */
+	spin_lock_irqsave(&adev->smc_idx_lock, flags);
+	/* set rom index to 0 */
+	WREG32(mmSMC_IND_INDEX_0, ixROM_INDEX);
+	WREG32(mmSMC_IND_DATA_0, 0);
+	/* set index to data for continous read */
+	WREG32(mmSMC_IND_INDEX_0, ixROM_DATA);
+	for (i = 0; i < length_dw; i++)
+		dw_ptr[i] = RREG32(mmSMC_IND_DATA_0);
+	spin_unlock_irqrestore(&adev->smc_idx_lock, flags);
+
+	return true;
+}
+
 static struct amdgpu_allowed_register_entry cik_allowed_read_registers[] = {
 	{mmGRBM_STATUS, false},
 	{mmGB_ADDR_CONFIG, false},
@@ -1563,8 +1596,8 @@ static void cik_pcie_gen3_enable(struct amdgpu_device *adev)
 {
 	struct pci_dev *root = adev->pdev->bus->self;
 	int bridge_pos, gpu_pos;
-	u32 speed_cntl, mask, current_data_rate;
-	int ret, i;
+	u32 speed_cntl, current_data_rate;
+	int i;
 	u16 tmp16;
 
 	if (pci_is_root_bus(adev->pdev->bus))
@@ -1576,23 +1609,20 @@ static void cik_pcie_gen3_enable(struct amdgpu_device *adev)
 	if (adev->flags & AMD_IS_APU)
 		return;
 
-	ret = drm_pcie_get_speed_cap_mask(adev->ddev, &mask);
-	if (ret != 0)
-		return;
-
-	if (!(mask & (DRM_PCIE_SPEED_50 | DRM_PCIE_SPEED_80)))
+	if (!(adev->pm.pcie_gen_mask & (CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2 |
+					CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)))
 		return;
 
 	speed_cntl = RREG32_PCIE(ixPCIE_LC_SPEED_CNTL);
 	current_data_rate = (speed_cntl & PCIE_LC_SPEED_CNTL__LC_CURRENT_DATA_RATE_MASK) >>
 		PCIE_LC_SPEED_CNTL__LC_CURRENT_DATA_RATE__SHIFT;
-	if (mask & DRM_PCIE_SPEED_80) {
+	if (adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3) {
 		if (current_data_rate == 2) {
 			DRM_INFO("PCIE gen 3 link speeds already enabled\n");
 			return;
 		}
 		DRM_INFO("enabling PCIE gen 3 link speeds, disable with amdgpu.pcie_gen2=0\n");
-	} else if (mask & DRM_PCIE_SPEED_50) {
+	} else if (adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2) {
 		if (current_data_rate == 1) {
 			DRM_INFO("PCIE gen 2 link speeds already enabled\n");
 			return;
@@ -1608,7 +1638,7 @@ static void cik_pcie_gen3_enable(struct amdgpu_device *adev)
 	if (!gpu_pos)
 		return;
 
-	if (mask & DRM_PCIE_SPEED_80) {
+	if (adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3) {
 		/* re-try equalization if gen3 is not already enabled */
 		if (current_data_rate != 2) {
 			u16 bridge_cfg, gpu_cfg;
@@ -1703,9 +1733,9 @@ static void cik_pcie_gen3_enable(struct amdgpu_device *adev)
 
 	pci_read_config_word(adev->pdev, gpu_pos + PCI_EXP_LNKCTL2, &tmp16);
 	tmp16 &= ~0xf;
-	if (mask & DRM_PCIE_SPEED_80)
+	if (adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)
 		tmp16 |= 3; /* gen3 */
-	else if (mask & DRM_PCIE_SPEED_50)
+	else if (adev->pm.pcie_gen_mask & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2)
 		tmp16 |= 2; /* gen2 */
 	else
 		tmp16 |= 1; /* gen1 */
@@ -1732,6 +1762,9 @@ static void cik_program_aspm(struct amdgpu_device *adev)
 	if (amdgpu_aspm == 0)
 		return;
 
+	if (pci_is_root_bus(adev->pdev->bus))
+		return;
+
 	/* XXX double check APUs */
 	if (adev->flags & AMD_IS_APU)
 		return;
@@ -1922,7 +1955,7 @@ static const struct amdgpu_ip_block_version bonaire_ip_blocks[] =
 		.major = 7,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &ci_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -1990,7 +2023,7 @@ static const struct amdgpu_ip_block_version hawaii_ip_blocks[] =
 		.major = 7,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &ci_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -2058,7 +2091,7 @@ static const struct amdgpu_ip_block_version kabini_ip_blocks[] =
 		.major = 7,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &kv_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -2126,7 +2159,7 @@ static const struct amdgpu_ip_block_version mullins_ip_blocks[] =
 		.major = 7,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &kv_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -2194,7 +2227,7 @@ static const struct amdgpu_ip_block_version kaveri_ip_blocks[] =
 		.major = 7,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &kv_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -2267,6 +2300,7 @@ int cik_set_ip_blocks(struct amdgpu_device *adev)
 static const struct amdgpu_asic_funcs cik_asic_funcs =
 {
 	.read_disabled_bios = &cik_read_disabled_bios,
+	.read_bios_from_rom = &cik_read_bios_from_rom,
 	.read_register = &cik_read_register,
 	.reset = &cik_asic_reset,
 	.set_vga_state = &cik_vga_set_state,
@@ -2301,72 +2335,72 @@ static int cik_common_early_init(void *handle)
 	switch (adev->asic_type) {
 	case CHIP_BONAIRE:
 		adev->cg_flags =
-			AMDGPU_CG_SUPPORT_GFX_MGCG |
-			AMDGPU_CG_SUPPORT_GFX_MGLS |
-			/*AMDGPU_CG_SUPPORT_GFX_CGCG |*/
-			AMDGPU_CG_SUPPORT_GFX_CGLS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS_LS |
-			AMDGPU_CG_SUPPORT_GFX_CP_LS |
-			AMDGPU_CG_SUPPORT_MC_LS |
-			AMDGPU_CG_SUPPORT_MC_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_LS |
-			AMDGPU_CG_SUPPORT_BIF_LS |
-			AMDGPU_CG_SUPPORT_VCE_MGCG |
-			AMDGPU_CG_SUPPORT_UVD_MGCG |
-			AMDGPU_CG_SUPPORT_HDP_LS |
-			AMDGPU_CG_SUPPORT_HDP_MGCG;
+			AMD_CG_SUPPORT_GFX_MGCG |
+			AMD_CG_SUPPORT_GFX_MGLS |
+			/*AMD_CG_SUPPORT_GFX_CGCG |*/
+			AMD_CG_SUPPORT_GFX_CGLS |
+			AMD_CG_SUPPORT_GFX_CGTS |
+			AMD_CG_SUPPORT_GFX_CGTS_LS |
+			AMD_CG_SUPPORT_GFX_CP_LS |
+			AMD_CG_SUPPORT_MC_LS |
+			AMD_CG_SUPPORT_MC_MGCG |
+			AMD_CG_SUPPORT_SDMA_MGCG |
+			AMD_CG_SUPPORT_SDMA_LS |
+			AMD_CG_SUPPORT_BIF_LS |
+			AMD_CG_SUPPORT_VCE_MGCG |
+			AMD_CG_SUPPORT_UVD_MGCG |
+			AMD_CG_SUPPORT_HDP_LS |
+			AMD_CG_SUPPORT_HDP_MGCG;
 		adev->pg_flags = 0;
 		adev->external_rev_id = adev->rev_id + 0x14;
 		break;
 	case CHIP_HAWAII:
 		adev->cg_flags =
-			AMDGPU_CG_SUPPORT_GFX_MGCG |
-			AMDGPU_CG_SUPPORT_GFX_MGLS |
-			/*AMDGPU_CG_SUPPORT_GFX_CGCG |*/
-			AMDGPU_CG_SUPPORT_GFX_CGLS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS |
-			AMDGPU_CG_SUPPORT_GFX_CP_LS |
-			AMDGPU_CG_SUPPORT_MC_LS |
-			AMDGPU_CG_SUPPORT_MC_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_LS |
-			AMDGPU_CG_SUPPORT_BIF_LS |
-			AMDGPU_CG_SUPPORT_VCE_MGCG |
-			AMDGPU_CG_SUPPORT_UVD_MGCG |
-			AMDGPU_CG_SUPPORT_HDP_LS |
-			AMDGPU_CG_SUPPORT_HDP_MGCG;
+			AMD_CG_SUPPORT_GFX_MGCG |
+			AMD_CG_SUPPORT_GFX_MGLS |
+			/*AMD_CG_SUPPORT_GFX_CGCG |*/
+			AMD_CG_SUPPORT_GFX_CGLS |
+			AMD_CG_SUPPORT_GFX_CGTS |
+			AMD_CG_SUPPORT_GFX_CP_LS |
+			AMD_CG_SUPPORT_MC_LS |
+			AMD_CG_SUPPORT_MC_MGCG |
+			AMD_CG_SUPPORT_SDMA_MGCG |
+			AMD_CG_SUPPORT_SDMA_LS |
+			AMD_CG_SUPPORT_BIF_LS |
+			AMD_CG_SUPPORT_VCE_MGCG |
+			AMD_CG_SUPPORT_UVD_MGCG |
+			AMD_CG_SUPPORT_HDP_LS |
+			AMD_CG_SUPPORT_HDP_MGCG;
 		adev->pg_flags = 0;
 		adev->external_rev_id = 0x28;
 		break;
 	case CHIP_KAVERI:
 		adev->cg_flags =
-			AMDGPU_CG_SUPPORT_GFX_MGCG |
-			AMDGPU_CG_SUPPORT_GFX_MGLS |
-			/*AMDGPU_CG_SUPPORT_GFX_CGCG |*/
-			AMDGPU_CG_SUPPORT_GFX_CGLS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS_LS |
-			AMDGPU_CG_SUPPORT_GFX_CP_LS |
-			AMDGPU_CG_SUPPORT_SDMA_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_LS |
-			AMDGPU_CG_SUPPORT_BIF_LS |
-			AMDGPU_CG_SUPPORT_VCE_MGCG |
-			AMDGPU_CG_SUPPORT_UVD_MGCG |
-			AMDGPU_CG_SUPPORT_HDP_LS |
-			AMDGPU_CG_SUPPORT_HDP_MGCG;
+			AMD_CG_SUPPORT_GFX_MGCG |
+			AMD_CG_SUPPORT_GFX_MGLS |
+			/*AMD_CG_SUPPORT_GFX_CGCG |*/
+			AMD_CG_SUPPORT_GFX_CGLS |
+			AMD_CG_SUPPORT_GFX_CGTS |
+			AMD_CG_SUPPORT_GFX_CGTS_LS |
+			AMD_CG_SUPPORT_GFX_CP_LS |
+			AMD_CG_SUPPORT_SDMA_MGCG |
+			AMD_CG_SUPPORT_SDMA_LS |
+			AMD_CG_SUPPORT_BIF_LS |
+			AMD_CG_SUPPORT_VCE_MGCG |
+			AMD_CG_SUPPORT_UVD_MGCG |
+			AMD_CG_SUPPORT_HDP_LS |
+			AMD_CG_SUPPORT_HDP_MGCG;
 		adev->pg_flags =
-			/*AMDGPU_PG_SUPPORT_GFX_PG |
-			  AMDGPU_PG_SUPPORT_GFX_SMG |
-			  AMDGPU_PG_SUPPORT_GFX_DMG |*/
-			AMDGPU_PG_SUPPORT_UVD |
-			/*AMDGPU_PG_SUPPORT_VCE |
-			  AMDGPU_PG_SUPPORT_CP |
-			  AMDGPU_PG_SUPPORT_GDS |
-			  AMDGPU_PG_SUPPORT_RLC_SMU_HS |
-			  AMDGPU_PG_SUPPORT_ACP |
-			  AMDGPU_PG_SUPPORT_SAMU |*/
+			/*AMD_PG_SUPPORT_GFX_PG |
+			  AMD_PG_SUPPORT_GFX_SMG |
+			  AMD_PG_SUPPORT_GFX_DMG |*/
+			AMD_PG_SUPPORT_UVD |
+			/*AMD_PG_SUPPORT_VCE |
+			  AMD_PG_SUPPORT_CP |
+			  AMD_PG_SUPPORT_GDS |
+			  AMD_PG_SUPPORT_RLC_SMU_HS |
+			  AMD_PG_SUPPORT_ACP |
+			  AMD_PG_SUPPORT_SAMU |*/
 			0;
 		if (adev->pdev->device == 0x1312 ||
 			adev->pdev->device == 0x1316 ||
@@ -2378,29 +2412,29 @@ static int cik_common_early_init(void *handle)
 	case CHIP_KABINI:
 	case CHIP_MULLINS:
 		adev->cg_flags =
-			AMDGPU_CG_SUPPORT_GFX_MGCG |
-			AMDGPU_CG_SUPPORT_GFX_MGLS |
-			/*AMDGPU_CG_SUPPORT_GFX_CGCG |*/
-			AMDGPU_CG_SUPPORT_GFX_CGLS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS |
-			AMDGPU_CG_SUPPORT_GFX_CGTS_LS |
-			AMDGPU_CG_SUPPORT_GFX_CP_LS |
-			AMDGPU_CG_SUPPORT_SDMA_MGCG |
-			AMDGPU_CG_SUPPORT_SDMA_LS |
-			AMDGPU_CG_SUPPORT_BIF_LS |
-			AMDGPU_CG_SUPPORT_VCE_MGCG |
-			AMDGPU_CG_SUPPORT_UVD_MGCG |
-			AMDGPU_CG_SUPPORT_HDP_LS |
-			AMDGPU_CG_SUPPORT_HDP_MGCG;
+			AMD_CG_SUPPORT_GFX_MGCG |
+			AMD_CG_SUPPORT_GFX_MGLS |
+			/*AMD_CG_SUPPORT_GFX_CGCG |*/
+			AMD_CG_SUPPORT_GFX_CGLS |
+			AMD_CG_SUPPORT_GFX_CGTS |
+			AMD_CG_SUPPORT_GFX_CGTS_LS |
+			AMD_CG_SUPPORT_GFX_CP_LS |
+			AMD_CG_SUPPORT_SDMA_MGCG |
+			AMD_CG_SUPPORT_SDMA_LS |
+			AMD_CG_SUPPORT_BIF_LS |
+			AMD_CG_SUPPORT_VCE_MGCG |
+			AMD_CG_SUPPORT_UVD_MGCG |
+			AMD_CG_SUPPORT_HDP_LS |
+			AMD_CG_SUPPORT_HDP_MGCG;
 		adev->pg_flags =
-			/*AMDGPU_PG_SUPPORT_GFX_PG |
-			  AMDGPU_PG_SUPPORT_GFX_SMG | */
-			AMDGPU_PG_SUPPORT_UVD |
-			/*AMDGPU_PG_SUPPORT_VCE |
-			  AMDGPU_PG_SUPPORT_CP |
-			  AMDGPU_PG_SUPPORT_GDS |
-			  AMDGPU_PG_SUPPORT_RLC_SMU_HS |
-			  AMDGPU_PG_SUPPORT_SAMU |*/
+			/*AMD_PG_SUPPORT_GFX_PG |
+			  AMD_PG_SUPPORT_GFX_SMG | */
+			AMD_PG_SUPPORT_UVD |
+			/*AMD_PG_SUPPORT_VCE |
+			  AMD_PG_SUPPORT_CP |
+			  AMD_PG_SUPPORT_GDS |
+			  AMD_PG_SUPPORT_RLC_SMU_HS |
+			  AMD_PG_SUPPORT_SAMU |*/
 			0;
 		if (adev->asic_type == CHIP_KABINI) {
 			if (adev->rev_id == 0)
@@ -2417,6 +2451,8 @@ static int cik_common_early_init(void *handle)
 		return -EINVAL;
 	}
 
+	amdgpu_get_pcie_info(adev);
+
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/cik_ih.c b/drivers/gpu/drm/amd/amdgpu/cik_ih.c
index 8993c50cb89f..30c9b3beeef9 100644
--- a/drivers/gpu/drm/amd/amdgpu/cik_ih.c
+++ b/drivers/gpu/drm/amd/amdgpu/cik_ih.c
@@ -274,6 +274,11 @@ static void cik_ih_set_rptr(struct amdgpu_device *adev)
 static int cik_ih_early_init(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int ret;
+
+	ret = amdgpu_irq_add_domain(adev);
+	if (ret)
+		return ret;
 
 	cik_ih_set_interrupt_funcs(adev);
 
@@ -300,6 +305,7 @@ static int cik_ih_sw_fini(void *handle)
 
 	amdgpu_irq_fini(adev);
 	amdgpu_ih_ring_fini(adev);
+	amdgpu_irq_remove_domain(adev);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/cik_sdma.c b/drivers/gpu/drm/amd/amdgpu/cik_sdma.c
index 5f712ceddf08..c55ecf0ea845 100644
--- a/drivers/gpu/drm/amd/amdgpu/cik_sdma.c
+++ b/drivers/gpu/drm/amd/amdgpu/cik_sdma.c
@@ -885,7 +885,7 @@ static void cik_enable_sdma_mgcg(struct amdgpu_device *adev,
 {
 	u32 orig, data;
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_SDMA_MGCG)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_SDMA_MGCG)) {
 		WREG32(mmSDMA0_CLK_CTRL + SDMA0_REGISTER_OFFSET, 0x00000100);
 		WREG32(mmSDMA0_CLK_CTRL + SDMA1_REGISTER_OFFSET, 0x00000100);
 	} else {
@@ -906,7 +906,7 @@ static void cik_enable_sdma_mgls(struct amdgpu_device *adev,
 {
 	u32 orig, data;
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_SDMA_LS)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_SDMA_LS)) {
 		orig = data = RREG32(mmSDMA0_POWER_CNTL + SDMA0_REGISTER_OFFSET);
 		data |= 0x100;
 		if (orig != data)
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
index 8035d4d6a4f5..e7ef2261ff4a 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/cz_dpm.c
@@ -445,13 +445,13 @@ static int cz_dpm_init(struct amdgpu_device *adev)
 	pi->gfx_pg_threshold = 500;
 	pi->caps_fps = true;
 	/* uvd */
-	pi->caps_uvd_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_UVD) ? true : false;
+	pi->caps_uvd_pg = (adev->pg_flags & AMD_PG_SUPPORT_UVD) ? true : false;
 	pi->caps_uvd_dpm = true;
 	/* vce */
-	pi->caps_vce_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_VCE) ? true : false;
+	pi->caps_vce_pg = (adev->pg_flags & AMD_PG_SUPPORT_VCE) ? true : false;
 	pi->caps_vce_dpm = true;
 	/* acp */
-	pi->caps_acp_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_ACP) ? true : false;
+	pi->caps_acp_pg = (adev->pg_flags & AMD_PG_SUPPORT_ACP) ? true : false;
 	pi->caps_acp_dpm = true;
 
 	pi->caps_stable_power_state = false;
@@ -1078,6 +1078,37 @@ static uint32_t cz_get_eclk_level(struct amdgpu_device *adev,
 	return i;
 }
 
+static uint32_t cz_get_uvd_level(struct amdgpu_device *adev,
+				 uint32_t clock, uint16_t msg)
+{
+	int i = 0;
+	struct amdgpu_uvd_clock_voltage_dependency_table *table =
+		&adev->pm.dpm.dyn_state.uvd_clock_voltage_dependency_table;
+
+	switch (msg) {
+	case PPSMC_MSG_SetUvdSoftMin:
+	case PPSMC_MSG_SetUvdHardMin:
+		for (i = 0; i < table->count; i++)
+			if (clock <= table->entries[i].vclk)
+				break;
+		if (i == table->count)
+			i = table->count - 1;
+		break;
+	case PPSMC_MSG_SetUvdSoftMax:
+	case PPSMC_MSG_SetUvdHardMax:
+		for (i = table->count - 1; i >= 0; i--)
+			if (clock >= table->entries[i].vclk)
+				break;
+		if (i < 0)
+			i = 0;
+		break;
+	default:
+		break;
+	}
+
+	return i;
+}
+
 static int cz_program_bootup_state(struct amdgpu_device *adev)
 {
 	struct cz_power_info *pi = cz_get_pi(adev);
@@ -1739,6 +1770,200 @@ static int cz_dpm_unforce_dpm_levels(struct amdgpu_device *adev)
 	return 0;
 }
 
+static int cz_dpm_uvd_force_highest(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	int ret = 0;
+
+	if (pi->uvd_dpm.soft_min_clk != pi->uvd_dpm.soft_max_clk) {
+		pi->uvd_dpm.soft_min_clk =
+			pi->uvd_dpm.soft_max_clk;
+		ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetUvdSoftMin,
+				cz_get_uvd_level(adev,
+					pi->uvd_dpm.soft_min_clk,
+					PPSMC_MSG_SetUvdSoftMin));
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int cz_dpm_uvd_force_lowest(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	int ret = 0;
+
+	if (pi->uvd_dpm.soft_max_clk != pi->uvd_dpm.soft_min_clk) {
+		pi->uvd_dpm.soft_max_clk = pi->uvd_dpm.soft_min_clk;
+		ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetUvdSoftMax,
+				cz_get_uvd_level(adev,
+					pi->uvd_dpm.soft_max_clk,
+					PPSMC_MSG_SetUvdSoftMax));
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static uint32_t cz_dpm_get_max_uvd_level(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+
+	if (!pi->max_uvd_level) {
+		cz_send_msg_to_smc(adev, PPSMC_MSG_GetMaxUvdLevel);
+		pi->max_uvd_level = cz_get_argument(adev) + 1;
+	}
+
+	if (pi->max_uvd_level > CZ_MAX_HARDWARE_POWERLEVELS) {
+		DRM_ERROR("Invalid max uvd level!\n");
+		return -EINVAL;
+	}
+
+	return pi->max_uvd_level;
+}
+
+static int cz_dpm_unforce_uvd_dpm_levels(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	struct amdgpu_uvd_clock_voltage_dependency_table *dep_table =
+		&adev->pm.dpm.dyn_state.uvd_clock_voltage_dependency_table;
+	uint32_t level = 0;
+	int ret = 0;
+
+	pi->uvd_dpm.soft_min_clk = dep_table->entries[0].vclk;
+	level = cz_dpm_get_max_uvd_level(adev) - 1;
+	if (level < dep_table->count)
+		pi->uvd_dpm.soft_max_clk = dep_table->entries[level].vclk;
+	else
+		pi->uvd_dpm.soft_max_clk =
+			dep_table->entries[dep_table->count - 1].vclk;
+
+	/* get min/max sclk soft value
+	 * notify SMU to execute */
+	ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetUvdSoftMin,
+				cz_get_uvd_level(adev,
+					pi->uvd_dpm.soft_min_clk,
+					PPSMC_MSG_SetUvdSoftMin));
+	if (ret)
+		return ret;
+
+	ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetUvdSoftMax,
+				cz_get_uvd_level(adev,
+					pi->uvd_dpm.soft_max_clk,
+					PPSMC_MSG_SetUvdSoftMax));
+	if (ret)
+		return ret;
+
+	DRM_DEBUG("DPM uvd unforce state min=%d, max=%d.\n",
+		  pi->uvd_dpm.soft_min_clk,
+		  pi->uvd_dpm.soft_max_clk);
+
+	return 0;
+}
+
+static int cz_dpm_vce_force_highest(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	int ret = 0;
+
+	if (pi->vce_dpm.soft_min_clk != pi->vce_dpm.soft_max_clk) {
+		pi->vce_dpm.soft_min_clk =
+			pi->vce_dpm.soft_max_clk;
+		ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetEclkSoftMin,
+				cz_get_eclk_level(adev,
+					pi->vce_dpm.soft_min_clk,
+					PPSMC_MSG_SetEclkSoftMin));
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static int cz_dpm_vce_force_lowest(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	int ret = 0;
+
+	if (pi->vce_dpm.soft_max_clk != pi->vce_dpm.soft_min_clk) {
+		pi->vce_dpm.soft_max_clk = pi->vce_dpm.soft_min_clk;
+		ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetEclkSoftMax,
+				cz_get_uvd_level(adev,
+					pi->vce_dpm.soft_max_clk,
+					PPSMC_MSG_SetEclkSoftMax));
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
+
+static uint32_t cz_dpm_get_max_vce_level(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+
+	if (!pi->max_vce_level) {
+		cz_send_msg_to_smc(adev, PPSMC_MSG_GetMaxEclkLevel);
+		pi->max_vce_level = cz_get_argument(adev) + 1;
+	}
+
+	if (pi->max_vce_level > CZ_MAX_HARDWARE_POWERLEVELS) {
+		DRM_ERROR("Invalid max vce level!\n");
+		return -EINVAL;
+	}
+
+	return pi->max_vce_level;
+}
+
+static int cz_dpm_unforce_vce_dpm_levels(struct amdgpu_device *adev)
+{
+	struct cz_power_info *pi = cz_get_pi(adev);
+	struct amdgpu_vce_clock_voltage_dependency_table *dep_table =
+		&adev->pm.dpm.dyn_state.vce_clock_voltage_dependency_table;
+	uint32_t level = 0;
+	int ret = 0;
+
+	pi->vce_dpm.soft_min_clk = dep_table->entries[0].ecclk;
+	level = cz_dpm_get_max_vce_level(adev) - 1;
+	if (level < dep_table->count)
+		pi->vce_dpm.soft_max_clk = dep_table->entries[level].ecclk;
+	else
+		pi->vce_dpm.soft_max_clk =
+			dep_table->entries[dep_table->count - 1].ecclk;
+
+	/* get min/max sclk soft value
+	 * notify SMU to execute */
+	ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetEclkSoftMin,
+				cz_get_eclk_level(adev,
+					pi->vce_dpm.soft_min_clk,
+					PPSMC_MSG_SetEclkSoftMin));
+	if (ret)
+		return ret;
+
+	ret = cz_send_msg_to_smc_with_parameter(adev,
+				PPSMC_MSG_SetEclkSoftMax,
+				cz_get_eclk_level(adev,
+					pi->vce_dpm.soft_max_clk,
+					PPSMC_MSG_SetEclkSoftMax));
+	if (ret)
+		return ret;
+
+	DRM_DEBUG("DPM vce unforce state min=%d, max=%d.\n",
+		  pi->vce_dpm.soft_min_clk,
+		  pi->vce_dpm.soft_max_clk);
+
+	return 0;
+}
+
 static int cz_dpm_force_dpm_level(struct amdgpu_device *adev,
 				  enum amdgpu_dpm_forced_level level)
 {
@@ -1746,25 +1971,70 @@ static int cz_dpm_force_dpm_level(struct amdgpu_device *adev,
 
 	switch (level) {
 	case AMDGPU_DPM_FORCED_LEVEL_HIGH:
+		/* sclk */
 		ret = cz_dpm_unforce_dpm_levels(adev);
 		if (ret)
 			return ret;
 		ret = cz_dpm_force_highest(adev);
 		if (ret)
 			return ret;
+
+		/* uvd */
+		ret = cz_dpm_unforce_uvd_dpm_levels(adev);
+		if (ret)
+			return ret;
+		ret = cz_dpm_uvd_force_highest(adev);
+		if (ret)
+			return ret;
+
+		/* vce */
+		ret = cz_dpm_unforce_vce_dpm_levels(adev);
+		if (ret)
+			return ret;
+		ret = cz_dpm_vce_force_highest(adev);
+		if (ret)
+			return ret;
 		break;
 	case AMDGPU_DPM_FORCED_LEVEL_LOW:
+		/* sclk */
 		ret = cz_dpm_unforce_dpm_levels(adev);
 		if (ret)
 			return ret;
 		ret = cz_dpm_force_lowest(adev);
 		if (ret)
 			return ret;
+
+		/* uvd */
+		ret = cz_dpm_unforce_uvd_dpm_levels(adev);
+		if (ret)
+			return ret;
+		ret = cz_dpm_uvd_force_lowest(adev);
+		if (ret)
+			return ret;
+
+		/* vce */
+		ret = cz_dpm_unforce_vce_dpm_levels(adev);
+		if (ret)
+			return ret;
+		ret = cz_dpm_vce_force_lowest(adev);
+		if (ret)
+			return ret;
 		break;
 	case AMDGPU_DPM_FORCED_LEVEL_AUTO:
+		/* sclk */
 		ret = cz_dpm_unforce_dpm_levels(adev);
 		if (ret)
 			return ret;
+
+		/* uvd */
+		ret = cz_dpm_unforce_uvd_dpm_levels(adev);
+		if (ret)
+			return ret;
+
+		/* vce */
+		ret = cz_dpm_unforce_vce_dpm_levels(adev);
+		if (ret)
+			return ret;
 		break;
 	default:
 		break;
@@ -1905,7 +2175,8 @@ static int cz_update_vce_dpm(struct amdgpu_device *adev)
 		pi->vce_dpm.hard_min_clk = table->entries[table->count-1].ecclk;
 
 	} else { /* non-stable p-state cases. without vce.Arbiter.EcclkHardMin */
-		pi->vce_dpm.hard_min_clk = table->entries[0].ecclk;
+		/* leave it as set by user */
+		/*pi->vce_dpm.hard_min_clk = table->entries[0].ecclk;*/
 	}
 
 	cz_send_msg_to_smc_with_parameter(adev,
@@ -1931,8 +2202,7 @@ static void cz_dpm_powergate_vce(struct amdgpu_device *adev, bool gate)
 							    AMD_PG_STATE_GATE);
 
 				cz_enable_vce_dpm(adev, false);
-				/* TODO: to figure out why vce can't be poweroff. */
-				/* cz_send_msg_to_smc(adev, PPSMC_MSG_VCEPowerOFF); */
+				cz_send_msg_to_smc(adev, PPSMC_MSG_VCEPowerOFF);
 				pi->vce_power_gated = true;
 			} else {
 				cz_send_msg_to_smc(adev, PPSMC_MSG_VCEPowerON);
@@ -1955,10 +2225,8 @@ static void cz_dpm_powergate_vce(struct amdgpu_device *adev, bool gate)
 		}
 	} else { /*pi->caps_vce_pg*/
 		cz_update_vce_dpm(adev);
-		cz_enable_vce_dpm(adev, true);
+		cz_enable_vce_dpm(adev, !gate);
 	}
-
-	return;
 }
 
 const struct amd_ip_funcs cz_dpm_ip_funcs = {
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_dpm.h b/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
index 99e1afc89629..5df8c1faab51 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
+++ b/drivers/gpu/drm/amd/amdgpu/cz_dpm.h
@@ -183,6 +183,8 @@ struct cz_power_info {
 	uint32_t voltage_drop_threshold;
 	uint32_t gfx_pg_threshold;
 	uint32_t max_sclk_level;
+	uint32_t max_uvd_level;
+	uint32_t max_vce_level;
 	/* flags */
 	bool didt_enabled;
 	bool video_start;
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_ih.c b/drivers/gpu/drm/amd/amdgpu/cz_ih.c
index bc751bfbcae2..c79638f8e732 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_ih.c
+++ b/drivers/gpu/drm/amd/amdgpu/cz_ih.c
@@ -253,8 +253,14 @@ static void cz_ih_set_rptr(struct amdgpu_device *adev)
 static int cz_ih_early_init(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int ret;
+
+	ret = amdgpu_irq_add_domain(adev);
+	if (ret)
+		return ret;
 
 	cz_ih_set_interrupt_funcs(adev);
+
 	return 0;
 }
 
@@ -278,6 +284,7 @@ static int cz_ih_sw_fini(void *handle)
 
 	amdgpu_irq_fini(adev);
 	amdgpu_ih_ring_fini(adev);
+	amdgpu_irq_remove_domain(adev);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
index 4dcc8fba5792..093599aba64b 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v10_0.c
@@ -3729,7 +3729,7 @@ static void dce_v10_0_encoder_add(struct amdgpu_device *adev,
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC1:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC2:
 		drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-				 DRM_MODE_ENCODER_DAC);
+				 DRM_MODE_ENCODER_DAC, NULL);
 		drm_encoder_helper_add(encoder, &dce_v10_0_dac_helper_funcs);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
@@ -3740,15 +3740,15 @@ static void dce_v10_0_encoder_add(struct amdgpu_device *adev,
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
 			amdgpu_encoder->rmx_type = RMX_FULL;
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_lcd_info(amdgpu_encoder);
 		} else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT)) {
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		} else {
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		}
 		drm_encoder_helper_add(encoder, &dce_v10_0_dig_helper_funcs);
@@ -3766,13 +3766,13 @@ static void dce_v10_0_encoder_add(struct amdgpu_device *adev,
 		amdgpu_encoder->is_ext_encoder = true;
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 		else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 		else
 			drm_encoder_init(dev, encoder, &dce_v10_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &dce_v10_0_ext_helper_funcs);
 		break;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
index 8f1e51128b33..8e67249d4367 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v11_0.c
@@ -211,9 +211,9 @@ static bool dce_v11_0_is_counter_moving(struct amdgpu_device *adev, int crtc)
  */
 static void dce_v11_0_vblank_wait(struct amdgpu_device *adev, int crtc)
 {
-	unsigned i = 0;
+	unsigned i = 100;
 
-	if (crtc >= adev->mode_info.num_crtc)
+	if (crtc < 0 || crtc >= adev->mode_info.num_crtc)
 		return;
 
 	if (!(RREG32(mmCRTC_CONTROL + crtc_offsets[crtc]) & CRTC_CONTROL__CRTC_MASTER_EN_MASK))
@@ -223,14 +223,16 @@ static void dce_v11_0_vblank_wait(struct amdgpu_device *adev, int crtc)
 	 * wait for another frame.
 	 */
 	while (dce_v11_0_is_in_vblank(adev, crtc)) {
-		if (i++ % 100 == 0) {
+		if (i++ == 100) {
+			i = 0;
 			if (!dce_v11_0_is_counter_moving(adev, crtc))
 				break;
 		}
 	}
 
 	while (!dce_v11_0_is_in_vblank(adev, crtc)) {
-		if (i++ % 100 == 0) {
+		if (i++ == 100) {
+			i = 0;
 			if (!dce_v11_0_is_counter_moving(adev, crtc))
 				break;
 		}
@@ -239,7 +241,7 @@ static void dce_v11_0_vblank_wait(struct amdgpu_device *adev, int crtc)
 
 static u32 dce_v11_0_vblank_get_counter(struct amdgpu_device *adev, int crtc)
 {
-	if (crtc >= adev->mode_info.num_crtc)
+	if (crtc < 0 || crtc >= adev->mode_info.num_crtc)
 		return 0;
 	else
 		return RREG32(mmCRTC_STATUS_FRAME_COUNT + crtc_offsets[crtc]);
@@ -3384,7 +3386,7 @@ static void dce_v11_0_crtc_vblank_int_ack(struct amdgpu_device *adev,
 {
 	u32 tmp;
 
-	if (crtc >= adev->mode_info.num_crtc) {
+	if (crtc < 0 || crtc >= adev->mode_info.num_crtc) {
 		DRM_DEBUG("invalid crtc %d\n", crtc);
 		return;
 	}
@@ -3399,7 +3401,7 @@ static void dce_v11_0_crtc_vline_int_ack(struct amdgpu_device *adev,
 {
 	u32 tmp;
 
-	if (crtc >= adev->mode_info.num_crtc) {
+	if (crtc < 0 || crtc >= adev->mode_info.num_crtc) {
 		DRM_DEBUG("invalid crtc %d\n", crtc);
 		return;
 	}
@@ -3722,7 +3724,7 @@ static void dce_v11_0_encoder_add(struct amdgpu_device *adev,
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC1:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC2:
 		drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-				 DRM_MODE_ENCODER_DAC);
+				 DRM_MODE_ENCODER_DAC, NULL);
 		drm_encoder_helper_add(encoder, &dce_v11_0_dac_helper_funcs);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
@@ -3733,15 +3735,15 @@ static void dce_v11_0_encoder_add(struct amdgpu_device *adev,
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
 			amdgpu_encoder->rmx_type = RMX_FULL;
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_lcd_info(amdgpu_encoder);
 		} else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT)) {
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		} else {
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		}
 		drm_encoder_helper_add(encoder, &dce_v11_0_dig_helper_funcs);
@@ -3759,13 +3761,13 @@ static void dce_v11_0_encoder_add(struct amdgpu_device *adev,
 		amdgpu_encoder->is_ext_encoder = true;
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 		else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 		else
 			drm_encoder_init(dev, encoder, &dce_v11_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &dce_v11_0_ext_helper_funcs);
 		break;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
index 42d954dc436d..d0e128c24813 100644
--- a/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/dce_v8_0.c
@@ -3659,7 +3659,7 @@ static void dce_v8_0_encoder_add(struct amdgpu_device *adev,
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC1:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC2:
 		drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-				 DRM_MODE_ENCODER_DAC);
+				 DRM_MODE_ENCODER_DAC, NULL);
 		drm_encoder_helper_add(encoder, &dce_v8_0_dac_helper_funcs);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
@@ -3670,15 +3670,15 @@ static void dce_v8_0_encoder_add(struct amdgpu_device *adev,
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
 			amdgpu_encoder->rmx_type = RMX_FULL;
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_lcd_info(amdgpu_encoder);
 		} else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT)) {
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		} else {
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 			amdgpu_encoder->enc_priv = amdgpu_atombios_encoder_get_dig_info(amdgpu_encoder);
 		}
 		drm_encoder_helper_add(encoder, &dce_v8_0_dig_helper_funcs);
@@ -3696,13 +3696,13 @@ static void dce_v8_0_encoder_add(struct amdgpu_device *adev,
 		amdgpu_encoder->is_ext_encoder = true;
 		if (amdgpu_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_LVDS);
+					 DRM_MODE_ENCODER_LVDS, NULL);
 		else if (amdgpu_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT))
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_DAC);
+					 DRM_MODE_ENCODER_DAC, NULL);
 		else
 			drm_encoder_init(dev, encoder, &dce_v8_0_encoder_funcs,
-					 DRM_MODE_ENCODER_TMDS);
+					 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &dce_v8_0_ext_helper_funcs);
 		break;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/fiji_dpm.c b/drivers/gpu/drm/amd/amdgpu/fiji_dpm.c
index 8f9845d9a986..4b0e45a27129 100644
--- a/drivers/gpu/drm/amd/amdgpu/fiji_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/fiji_dpm.c
@@ -24,7 +24,7 @@
 #include <linux/firmware.h>
 #include "drmP.h"
 #include "amdgpu.h"
-#include "fiji_smumgr.h"
+#include "fiji_smum.h"
 
 MODULE_FIRMWARE("amdgpu/fiji_smc.bin");
 
diff --git a/drivers/gpu/drm/amd/amdgpu/fiji_ppsmc.h b/drivers/gpu/drm/amd/amdgpu/fiji_ppsmc.h
deleted file mode 100644
index 3c4824082990..000000000000
--- a/drivers/gpu/drm/amd/amdgpu/fiji_ppsmc.h
+++ /dev/null
@@ -1,182 +0,0 @@
-/*
- * Copyright 2014 Advanced Micro Devices, Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- */
-
-#ifndef FIJI_PP_SMC_H
-#define FIJI_PP_SMC_H
-
-#pragma pack(push, 1)
-
-#define PPSMC_SWSTATE_FLAG_DC                           0x01
-#define PPSMC_SWSTATE_FLAG_UVD                          0x02
-#define PPSMC_SWSTATE_FLAG_VCE                          0x04
-
-#define PPSMC_THERMAL_PROTECT_TYPE_INTERNAL             0x00
-#define PPSMC_THERMAL_PROTECT_TYPE_EXTERNAL             0x01
-#define PPSMC_THERMAL_PROTECT_TYPE_NONE                 0xff
-
-#define PPSMC_SYSTEMFLAG_GPIO_DC                        0x01
-#define PPSMC_SYSTEMFLAG_STEPVDDC                       0x02
-#define PPSMC_SYSTEMFLAG_GDDR5                          0x04
-
-#define PPSMC_SYSTEMFLAG_DISABLE_BABYSTEP               0x08
-
-#define PPSMC_SYSTEMFLAG_REGULATOR_HOT                  0x10
-#define PPSMC_SYSTEMFLAG_REGULATOR_HOT_ANALOG           0x20
-
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_MASK              0x07
-#define PPSMC_EXTRAFLAGS_AC2DC_DONT_WAIT_FOR_VBLANK     0x08
-
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTODPMLOWSTATE   0x00
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTOINITIALSTATE  0x01
-
-#define PPSMC_DPM2FLAGS_TDPCLMP                         0x01
-#define PPSMC_DPM2FLAGS_PWRSHFT                         0x02
-#define PPSMC_DPM2FLAGS_OCP                             0x04
-
-#define PPSMC_DISPLAY_WATERMARK_LOW                     0
-#define PPSMC_DISPLAY_WATERMARK_HIGH                    1
-
-#define PPSMC_STATEFLAG_AUTO_PULSE_SKIP    		0x01
-#define PPSMC_STATEFLAG_POWERBOOST         		0x02
-#define PPSMC_STATEFLAG_PSKIP_ON_TDP_FAULT 		0x04
-#define PPSMC_STATEFLAG_POWERSHIFT         		0x08
-#define PPSMC_STATEFLAG_SLOW_READ_MARGIN   		0x10
-#define PPSMC_STATEFLAG_DEEPSLEEP_THROTTLE 		0x20
-#define PPSMC_STATEFLAG_DEEPSLEEP_BYPASS   		0x40
-
-#define FDO_MODE_HARDWARE 0
-#define FDO_MODE_PIECE_WISE_LINEAR 1
-
-enum FAN_CONTROL {
-	FAN_CONTROL_FUZZY,
-	FAN_CONTROL_TABLE
-};
-
-//Gemini Modes
-#define PPSMC_GeminiModeNone   0  //Single GPU board
-#define PPSMC_GeminiModeMaster 1  //Master GPU on a Gemini board
-#define PPSMC_GeminiModeSlave  2  //Slave GPU on a Gemini board
-
-#define PPSMC_Result_OK             			((uint16_t)0x01)
-#define PPSMC_Result_NoMore         			((uint16_t)0x02)
-#define PPSMC_Result_NotNow         			((uint16_t)0x03)
-#define PPSMC_Result_Failed         			((uint16_t)0xFF)
-#define PPSMC_Result_UnknownCmd     			((uint16_t)0xFE)
-#define PPSMC_Result_UnknownVT      			((uint16_t)0xFD)
-
-typedef uint16_t PPSMC_Result;
-
-#define PPSMC_isERROR(x) ((uint16_t)0x80 & (x))
-
-#define PPSMC_MSG_Halt                      		((uint16_t)0x10)
-#define PPSMC_MSG_Resume                    		((uint16_t)0x11)
-#define PPSMC_MSG_EnableDPMLevel            		((uint16_t)0x12)
-#define PPSMC_MSG_ZeroLevelsDisabled        		((uint16_t)0x13)
-#define PPSMC_MSG_OneLevelsDisabled         		((uint16_t)0x14)
-#define PPSMC_MSG_TwoLevelsDisabled         		((uint16_t)0x15)
-#define PPSMC_MSG_EnableThermalInterrupt    		((uint16_t)0x16)
-#define PPSMC_MSG_RunningOnAC               		((uint16_t)0x17)
-#define PPSMC_MSG_LevelUp                   		((uint16_t)0x18)
-#define PPSMC_MSG_LevelDown                 		((uint16_t)0x19)
-#define PPSMC_MSG_ResetDPMCounters          		((uint16_t)0x1a)
-#define PPSMC_MSG_SwitchToSwState           		((uint16_t)0x20)
-#define PPSMC_MSG_SwitchToSwStateLast       		((uint16_t)0x3f)
-#define PPSMC_MSG_SwitchToInitialState      		((uint16_t)0x40)
-#define PPSMC_MSG_NoForcedLevel             		((uint16_t)0x41)
-#define PPSMC_MSG_ForceHigh                 		((uint16_t)0x42)
-#define PPSMC_MSG_ForceMediumOrHigh         		((uint16_t)0x43)
-#define PPSMC_MSG_SwitchToMinimumPower      		((uint16_t)0x51)
-#define PPSMC_MSG_ResumeFromMinimumPower    		((uint16_t)0x52)
-#define PPSMC_MSG_EnableCac                 		((uint16_t)0x53)
-#define PPSMC_MSG_DisableCac                		((uint16_t)0x54)
-#define PPSMC_DPMStateHistoryStart          		((uint16_t)0x55)
-#define PPSMC_DPMStateHistoryStop           		((uint16_t)0x56)
-#define PPSMC_CACHistoryStart               		((uint16_t)0x57)
-#define PPSMC_CACHistoryStop                		((uint16_t)0x58)
-#define PPSMC_TDPClampingActive             		((uint16_t)0x59)
-#define PPSMC_TDPClampingInactive           		((uint16_t)0x5A)
-#define PPSMC_StartFanControl               		((uint16_t)0x5B)
-#define PPSMC_StopFanControl                		((uint16_t)0x5C)
-#define PPSMC_NoDisplay                     		((uint16_t)0x5D)
-#define PPSMC_HasDisplay                    		((uint16_t)0x5E)
-#define PPSMC_MSG_UVDPowerOFF               		((uint16_t)0x60)
-#define PPSMC_MSG_UVDPowerON                		((uint16_t)0x61)
-#define PPSMC_MSG_EnableULV                 		((uint16_t)0x62)
-#define PPSMC_MSG_DisableULV                		((uint16_t)0x63)
-#define PPSMC_MSG_EnterULV                  		((uint16_t)0x64)
-#define PPSMC_MSG_ExitULV                   		((uint16_t)0x65)
-#define PPSMC_PowerShiftActive              		((uint16_t)0x6A)
-#define PPSMC_PowerShiftInactive            		((uint16_t)0x6B)
-#define PPSMC_OCPActive                     		((uint16_t)0x6C)
-#define PPSMC_OCPInactive                   		((uint16_t)0x6D)
-#define PPSMC_CACLongTermAvgEnable          		((uint16_t)0x6E)
-#define PPSMC_CACLongTermAvgDisable         		((uint16_t)0x6F)
-#define PPSMC_MSG_InferredStateSweep_Start  		((uint16_t)0x70)
-#define PPSMC_MSG_InferredStateSweep_Stop   		((uint16_t)0x71)
-#define PPSMC_MSG_SwitchToLowestInfState    		((uint16_t)0x72)
-#define PPSMC_MSG_SwitchToNonInfState       		((uint16_t)0x73)
-#define PPSMC_MSG_AllStateSweep_Start       		((uint16_t)0x74)
-#define PPSMC_MSG_AllStateSweep_Stop        		((uint16_t)0x75)
-#define PPSMC_MSG_SwitchNextLowerInfState   		((uint16_t)0x76)
-#define PPSMC_MSG_SwitchNextHigherInfState  		((uint16_t)0x77)
-#define PPSMC_MSG_MclkRetrainingTest        		((uint16_t)0x78)
-#define PPSMC_MSG_ForceTDPClamping          		((uint16_t)0x79)
-#define PPSMC_MSG_CollectCAC_PowerCorreln   		((uint16_t)0x7A)
-#define PPSMC_MSG_CollectCAC_WeightCalib    		((uint16_t)0x7B)
-#define PPSMC_MSG_CollectCAC_SQonly         		((uint16_t)0x7C)
-#define PPSMC_MSG_CollectCAC_TemperaturePwr 		((uint16_t)0x7D)
-#define PPSMC_MSG_ExtremitiesTest_Start     		((uint16_t)0x7E)
-#define PPSMC_MSG_ExtremitiesTest_Stop      		((uint16_t)0x7F)
-#define PPSMC_FlushDataCache                		((uint16_t)0x80)
-#define PPSMC_FlushInstrCache               		((uint16_t)0x81)
-#define PPSMC_MSG_SetEnabledLevels          		((uint16_t)0x82)
-#define PPSMC_MSG_SetForcedLevels           		((uint16_t)0x83)
-#define PPSMC_MSG_ResetToDefaults           		((uint16_t)0x84)
-#define PPSMC_MSG_SetForcedLevelsAndJump    		((uint16_t)0x85)
-#define PPSMC_MSG_SetCACHistoryMode         		((uint16_t)0x86)
-#define PPSMC_MSG_EnableDTE                 		((uint16_t)0x87)
-#define PPSMC_MSG_DisableDTE                		((uint16_t)0x88)
-#define PPSMC_MSG_SmcSpaceSetAddress        		((uint16_t)0x89)
-#define PPSMC_MSG_SmcSpaceWriteDWordInc     		((uint16_t)0x8A)
-#define PPSMC_MSG_SmcSpaceWriteWordInc      		((uint16_t)0x8B)
-#define PPSMC_MSG_SmcSpaceWriteByteInc      		((uint16_t)0x8C)
-
-#define PPSMC_MSG_BREAK                     		((uint16_t)0xF8)
-
-#define PPSMC_MSG_Test                      		((uint16_t)0x100)
-#define PPSMC_MSG_DRV_DRAM_ADDR_HI            		((uint16_t)0x250)
-#define PPSMC_MSG_DRV_DRAM_ADDR_LO            		((uint16_t)0x251)
-#define PPSMC_MSG_SMU_DRAM_ADDR_HI            		((uint16_t)0x252)
-#define PPSMC_MSG_SMU_DRAM_ADDR_LO            		((uint16_t)0x253)
-#define PPSMC_MSG_LoadUcodes                  		((uint16_t)0x254)
-
-typedef uint16_t PPSMC_Msg;
-
-#define PPSMC_EVENT_STATUS_THERMAL          		0x00000001
-#define PPSMC_EVENT_STATUS_REGULATORHOT     		0x00000002
-#define PPSMC_EVENT_STATUS_DC               		0x00000004
-#define PPSMC_EVENT_STATUS_GPIO17           		0x00000008
-
-#pragma pack(pop)
-
-#endif
diff --git a/drivers/gpu/drm/amd/amdgpu/fiji_smc.c b/drivers/gpu/drm/amd/amdgpu/fiji_smc.c
index bda1249eb871..e35340afd3db 100644
--- a/drivers/gpu/drm/amd/amdgpu/fiji_smc.c
+++ b/drivers/gpu/drm/amd/amdgpu/fiji_smc.c
@@ -25,7 +25,7 @@
 #include "drmP.h"
 #include "amdgpu.h"
 #include "fiji_ppsmc.h"
-#include "fiji_smumgr.h"
+#include "fiji_smum.h"
 #include "smu_ucode_xfer_vi.h"
 #include "amdgpu_ucode.h"
 
diff --git a/drivers/gpu/drm/amd/amdgpu/fiji_smumgr.h b/drivers/gpu/drm/amd/amdgpu/fiji_smum.h
index 1cef03deeac3..1cef03deeac3 100644
--- a/drivers/gpu/drm/amd/amdgpu/fiji_smumgr.h
+++ b/drivers/gpu/drm/amd/amdgpu/fiji_smum.h
diff --git a/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c b/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
index 72793f93e2fc..06602df707f8 100644
--- a/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gfx_v7_0.c
@@ -3628,6 +3628,19 @@ static void gfx_v7_0_ring_emit_vm_flush(struct amdgpu_ring *ring,
 					unsigned vm_id, uint64_t pd_addr)
 {
 	int usepfp = (ring->type == AMDGPU_RING_TYPE_GFX);
+	uint32_t seq = ring->fence_drv.sync_seq;
+	uint64_t addr = ring->fence_drv.gpu_addr;
+
+	amdgpu_ring_write(ring, PACKET3(PACKET3_WAIT_REG_MEM, 5));
+	amdgpu_ring_write(ring, (WAIT_REG_MEM_MEM_SPACE(1) | /* memory */
+				 WAIT_REG_MEM_FUNCTION(3) | /* equal */
+				 WAIT_REG_MEM_ENGINE(usepfp)));   /* pfp or me */
+	amdgpu_ring_write(ring, addr & 0xfffffffc);
+	amdgpu_ring_write(ring, upper_32_bits(addr) & 0xffffffff);
+	amdgpu_ring_write(ring, seq);
+	amdgpu_ring_write(ring, 0xffffffff);
+	amdgpu_ring_write(ring, 4); /* poll interval */
+
 	if (usepfp) {
 		/* synce CE with ME to prevent CE fetch CEIB before context switch done */
 		amdgpu_ring_write(ring, PACKET3(PACKET3_SWITCH_BUFFER, 0));
@@ -4109,7 +4122,7 @@ static void gfx_v7_0_enable_cgcg(struct amdgpu_device *adev, bool enable)
 
 	orig = data = RREG32(mmRLC_CGCG_CGLS_CTRL);
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_CGCG)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_GFX_CGCG)) {
 		gfx_v7_0_enable_gui_idle_interrupt(adev, true);
 
 		tmp = gfx_v7_0_halt_rlc(adev);
@@ -4147,9 +4160,9 @@ static void gfx_v7_0_enable_mgcg(struct amdgpu_device *adev, bool enable)
 {
 	u32 data, orig, tmp = 0;
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_MGCG)) {
-		if (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_MGLS) {
-			if (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_CP_LS) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_GFX_MGCG)) {
+		if (adev->cg_flags & AMD_CG_SUPPORT_GFX_MGLS) {
+			if (adev->cg_flags & AMD_CG_SUPPORT_GFX_CP_LS) {
 				orig = data = RREG32(mmCP_MEM_SLP_CNTL);
 				data |= CP_MEM_SLP_CNTL__CP_MEM_LS_EN_MASK;
 				if (orig != data)
@@ -4176,14 +4189,14 @@ static void gfx_v7_0_enable_mgcg(struct amdgpu_device *adev, bool enable)
 
 		gfx_v7_0_update_rlc(adev, tmp);
 
-		if (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_CGTS) {
+		if (adev->cg_flags & AMD_CG_SUPPORT_GFX_CGTS) {
 			orig = data = RREG32(mmCGTS_SM_CTRL_REG);
 			data &= ~CGTS_SM_CTRL_REG__SM_MODE_MASK;
 			data |= (0x2 << CGTS_SM_CTRL_REG__SM_MODE__SHIFT);
 			data |= CGTS_SM_CTRL_REG__SM_MODE_ENABLE_MASK;
 			data &= ~CGTS_SM_CTRL_REG__OVERRIDE_MASK;
-			if ((adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_MGLS) &&
-			    (adev->cg_flags & AMDGPU_CG_SUPPORT_GFX_CGTS_LS))
+			if ((adev->cg_flags & AMD_CG_SUPPORT_GFX_MGLS) &&
+			    (adev->cg_flags & AMD_CG_SUPPORT_GFX_CGTS_LS))
 				data &= ~CGTS_SM_CTRL_REG__LS_OVERRIDE_MASK;
 			data &= ~CGTS_SM_CTRL_REG__ON_MONITOR_ADD_MASK;
 			data |= CGTS_SM_CTRL_REG__ON_MONITOR_ADD_EN_MASK;
@@ -4249,7 +4262,7 @@ static void gfx_v7_0_enable_sclk_slowdown_on_pu(struct amdgpu_device *adev,
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_RLC_SMU_HS))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_RLC_SMU_HS))
 		data |= RLC_PG_CNTL__SMU_CLK_SLOWDOWN_ON_PU_ENABLE_MASK;
 	else
 		data &= ~RLC_PG_CNTL__SMU_CLK_SLOWDOWN_ON_PU_ENABLE_MASK;
@@ -4263,7 +4276,7 @@ static void gfx_v7_0_enable_sclk_slowdown_on_pd(struct amdgpu_device *adev,
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_RLC_SMU_HS))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_RLC_SMU_HS))
 		data |= RLC_PG_CNTL__SMU_CLK_SLOWDOWN_ON_PD_ENABLE_MASK;
 	else
 		data &= ~RLC_PG_CNTL__SMU_CLK_SLOWDOWN_ON_PD_ENABLE_MASK;
@@ -4276,7 +4289,7 @@ static void gfx_v7_0_enable_cp_pg(struct amdgpu_device *adev, bool enable)
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_CP))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_CP))
 		data &= ~0x8000;
 	else
 		data |= 0x8000;
@@ -4289,7 +4302,7 @@ static void gfx_v7_0_enable_gds_pg(struct amdgpu_device *adev, bool enable)
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_GDS))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_GDS))
 		data &= ~0x2000;
 	else
 		data |= 0x2000;
@@ -4370,7 +4383,7 @@ static void gfx_v7_0_enable_gfx_cgpg(struct amdgpu_device *adev,
 {
 	u32 data, orig;
 
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_PG)) {
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_GFX_PG)) {
 		orig = data = RREG32(mmRLC_PG_CNTL);
 		data |= RLC_PG_CNTL__GFX_POWER_GATING_ENABLE_MASK;
 		if (orig != data)
@@ -4442,7 +4455,7 @@ static void gfx_v7_0_enable_gfx_static_mgpg(struct amdgpu_device *adev,
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_SMG))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_GFX_SMG))
 		data |= RLC_PG_CNTL__STATIC_PER_CU_PG_ENABLE_MASK;
 	else
 		data &= ~RLC_PG_CNTL__STATIC_PER_CU_PG_ENABLE_MASK;
@@ -4456,7 +4469,7 @@ static void gfx_v7_0_enable_gfx_dynamic_mgpg(struct amdgpu_device *adev,
 	u32 data, orig;
 
 	orig = data = RREG32(mmRLC_PG_CNTL);
-	if (enable && (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_DMG))
+	if (enable && (adev->pg_flags & AMD_PG_SUPPORT_GFX_DMG))
 		data |= RLC_PG_CNTL__DYN_PER_CU_PG_ENABLE_MASK;
 	else
 		data &= ~RLC_PG_CNTL__DYN_PER_CU_PG_ENABLE_MASK;
@@ -4623,15 +4636,15 @@ static void gfx_v7_0_get_csb_buffer(struct amdgpu_device *adev,
 
 static void gfx_v7_0_init_pg(struct amdgpu_device *adev)
 {
-	if (adev->pg_flags & (AMDGPU_PG_SUPPORT_GFX_PG |
-			      AMDGPU_PG_SUPPORT_GFX_SMG |
-			      AMDGPU_PG_SUPPORT_GFX_DMG |
-			      AMDGPU_PG_SUPPORT_CP |
-			      AMDGPU_PG_SUPPORT_GDS |
-			      AMDGPU_PG_SUPPORT_RLC_SMU_HS)) {
+	if (adev->pg_flags & (AMD_PG_SUPPORT_GFX_PG |
+			      AMD_PG_SUPPORT_GFX_SMG |
+			      AMD_PG_SUPPORT_GFX_DMG |
+			      AMD_PG_SUPPORT_CP |
+			      AMD_PG_SUPPORT_GDS |
+			      AMD_PG_SUPPORT_RLC_SMU_HS)) {
 		gfx_v7_0_enable_sclk_slowdown_on_pu(adev, true);
 		gfx_v7_0_enable_sclk_slowdown_on_pd(adev, true);
-		if (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_PG) {
+		if (adev->pg_flags & AMD_PG_SUPPORT_GFX_PG) {
 			gfx_v7_0_init_gfx_cgpg(adev);
 			gfx_v7_0_enable_cp_pg(adev, true);
 			gfx_v7_0_enable_gds_pg(adev, true);
@@ -4643,14 +4656,14 @@ static void gfx_v7_0_init_pg(struct amdgpu_device *adev)
 
 static void gfx_v7_0_fini_pg(struct amdgpu_device *adev)
 {
-	if (adev->pg_flags & (AMDGPU_PG_SUPPORT_GFX_PG |
-			      AMDGPU_PG_SUPPORT_GFX_SMG |
-			      AMDGPU_PG_SUPPORT_GFX_DMG |
-			      AMDGPU_PG_SUPPORT_CP |
-			      AMDGPU_PG_SUPPORT_GDS |
-			      AMDGPU_PG_SUPPORT_RLC_SMU_HS)) {
+	if (adev->pg_flags & (AMD_PG_SUPPORT_GFX_PG |
+			      AMD_PG_SUPPORT_GFX_SMG |
+			      AMD_PG_SUPPORT_GFX_DMG |
+			      AMD_PG_SUPPORT_CP |
+			      AMD_PG_SUPPORT_GDS |
+			      AMD_PG_SUPPORT_RLC_SMU_HS)) {
 		gfx_v7_0_update_gfx_pg(adev, false);
-		if (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_PG) {
+		if (adev->pg_flags & AMD_PG_SUPPORT_GFX_PG) {
 			gfx_v7_0_enable_cp_pg(adev, false);
 			gfx_v7_0_enable_gds_pg(adev, false);
 		}
@@ -4738,6 +4751,22 @@ static int gfx_v7_0_early_init(void *handle)
 	return 0;
 }
 
+static int gfx_v7_0_late_init(void *handle)
+{
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int r;
+
+	r = amdgpu_irq_get(adev, &adev->gfx.priv_reg_irq, 0);
+	if (r)
+		return r;
+
+	r = amdgpu_irq_get(adev, &adev->gfx.priv_inst_irq, 0);
+	if (r)
+		return r;
+
+	return 0;
+}
+
 static int gfx_v7_0_sw_init(void *handle)
 {
 	struct amdgpu_ring *ring;
@@ -4890,6 +4919,8 @@ static int gfx_v7_0_hw_fini(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	amdgpu_irq_put(adev, &adev->gfx.priv_reg_irq, 0);
+	amdgpu_irq_put(adev, &adev->gfx.priv_inst_irq, 0);
 	gfx_v7_0_cp_enable(adev, false);
 	gfx_v7_0_rlc_stop(adev);
 	gfx_v7_0_fini_pg(adev);
@@ -5509,14 +5540,14 @@ static int gfx_v7_0_set_powergating_state(void *handle,
 	if (state == AMD_PG_STATE_GATE)
 		gate = true;
 
-	if (adev->pg_flags & (AMDGPU_PG_SUPPORT_GFX_PG |
-			      AMDGPU_PG_SUPPORT_GFX_SMG |
-			      AMDGPU_PG_SUPPORT_GFX_DMG |
-			      AMDGPU_PG_SUPPORT_CP |
-			      AMDGPU_PG_SUPPORT_GDS |
-			      AMDGPU_PG_SUPPORT_RLC_SMU_HS)) {
+	if (adev->pg_flags & (AMD_PG_SUPPORT_GFX_PG |
+			      AMD_PG_SUPPORT_GFX_SMG |
+			      AMD_PG_SUPPORT_GFX_DMG |
+			      AMD_PG_SUPPORT_CP |
+			      AMD_PG_SUPPORT_GDS |
+			      AMD_PG_SUPPORT_RLC_SMU_HS)) {
 		gfx_v7_0_update_gfx_pg(adev, gate);
-		if (adev->pg_flags & AMDGPU_PG_SUPPORT_GFX_PG) {
+		if (adev->pg_flags & AMD_PG_SUPPORT_GFX_PG) {
 			gfx_v7_0_enable_cp_pg(adev, gate);
 			gfx_v7_0_enable_gds_pg(adev, gate);
 		}
@@ -5527,7 +5558,7 @@ static int gfx_v7_0_set_powergating_state(void *handle,
 
 const struct amd_ip_funcs gfx_v7_0_ip_funcs = {
 	.early_init = gfx_v7_0_early_init,
-	.late_init = NULL,
+	.late_init = gfx_v7_0_late_init,
 	.sw_init = gfx_v7_0_sw_init,
 	.sw_fini = gfx_v7_0_sw_fini,
 	.hw_init = gfx_v7_0_hw_init,
diff --git a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
index e1dcab98e249..7086ac17abee 100644
--- a/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gfx_v8_0.c
@@ -66,6 +66,27 @@
 #define MACRO_TILE_ASPECT(x)				((x) << GB_MACROTILE_MODE0__MACRO_TILE_ASPECT__SHIFT)
 #define NUM_BANKS(x)					((x) << GB_MACROTILE_MODE0__NUM_BANKS__SHIFT)
 
+#define RLC_CGTT_MGCG_OVERRIDE__CPF_MASK            0x00000001L
+#define RLC_CGTT_MGCG_OVERRIDE__RLC_MASK            0x00000002L
+#define RLC_CGTT_MGCG_OVERRIDE__MGCG_MASK           0x00000004L
+#define RLC_CGTT_MGCG_OVERRIDE__CGCG_MASK           0x00000008L
+#define RLC_CGTT_MGCG_OVERRIDE__CGLS_MASK           0x00000010L
+#define RLC_CGTT_MGCG_OVERRIDE__GRBM_MASK           0x00000020L
+
+/* BPM SERDES CMD */
+#define SET_BPM_SERDES_CMD    1
+#define CLE_BPM_SERDES_CMD    0
+
+/* BPM Register Address*/
+enum {
+	BPM_REG_CGLS_EN = 0,        /* Enable/Disable CGLS */
+	BPM_REG_CGLS_ON,            /* ON/OFF CGLS: shall be controlled by RLC FW */
+	BPM_REG_CGCG_OVERRIDE,      /* Set/Clear CGCG Override */
+	BPM_REG_MGCG_OVERRIDE,      /* Set/Clear MGCG Override */
+	BPM_REG_FGCG_OVERRIDE,      /* Set/Clear FGCG Override */
+	BPM_REG_FGCG_MAX
+};
+
 MODULE_FIRMWARE("amdgpu/carrizo_ce.bin");
 MODULE_FIRMWARE("amdgpu/carrizo_pfp.bin");
 MODULE_FIRMWARE("amdgpu/carrizo_me.bin");
@@ -90,7 +111,6 @@ MODULE_FIRMWARE("amdgpu/topaz_ce.bin");
 MODULE_FIRMWARE("amdgpu/topaz_pfp.bin");
 MODULE_FIRMWARE("amdgpu/topaz_me.bin");
 MODULE_FIRMWARE("amdgpu/topaz_mec.bin");
-MODULE_FIRMWARE("amdgpu/topaz_mec2.bin");
 MODULE_FIRMWARE("amdgpu/topaz_rlc.bin");
 
 MODULE_FIRMWARE("amdgpu/fiji_ce.bin");
@@ -807,7 +827,8 @@ static int gfx_v8_0_init_microcode(struct amdgpu_device *adev)
 	adev->gfx.mec_fw_version = le32_to_cpu(cp_hdr->header.ucode_version);
 	adev->gfx.mec_feature_version = le32_to_cpu(cp_hdr->ucode_feature_version);
 
-	if (adev->asic_type != CHIP_STONEY) {
+	if ((adev->asic_type != CHIP_STONEY) &&
+	    (adev->asic_type != CHIP_TOPAZ)) {
 		snprintf(fw_name, sizeof(fw_name), "amdgpu/%s_mec2.bin", chip_name);
 		err = request_firmware(&adev->gfx.mec2_fw, fw_name, adev->dev);
 		if (!err) {
@@ -964,6 +985,322 @@ static int gfx_v8_0_mec_init(struct amdgpu_device *adev)
 	return 0;
 }
 
+static const u32 vgpr_init_compute_shader[] =
+{
+	0x7e000209, 0x7e020208,
+	0x7e040207, 0x7e060206,
+	0x7e080205, 0x7e0a0204,
+	0x7e0c0203, 0x7e0e0202,
+	0x7e100201, 0x7e120200,
+	0x7e140209, 0x7e160208,
+	0x7e180207, 0x7e1a0206,
+	0x7e1c0205, 0x7e1e0204,
+	0x7e200203, 0x7e220202,
+	0x7e240201, 0x7e260200,
+	0x7e280209, 0x7e2a0208,
+	0x7e2c0207, 0x7e2e0206,
+	0x7e300205, 0x7e320204,
+	0x7e340203, 0x7e360202,
+	0x7e380201, 0x7e3a0200,
+	0x7e3c0209, 0x7e3e0208,
+	0x7e400207, 0x7e420206,
+	0x7e440205, 0x7e460204,
+	0x7e480203, 0x7e4a0202,
+	0x7e4c0201, 0x7e4e0200,
+	0x7e500209, 0x7e520208,
+	0x7e540207, 0x7e560206,
+	0x7e580205, 0x7e5a0204,
+	0x7e5c0203, 0x7e5e0202,
+	0x7e600201, 0x7e620200,
+	0x7e640209, 0x7e660208,
+	0x7e680207, 0x7e6a0206,
+	0x7e6c0205, 0x7e6e0204,
+	0x7e700203, 0x7e720202,
+	0x7e740201, 0x7e760200,
+	0x7e780209, 0x7e7a0208,
+	0x7e7c0207, 0x7e7e0206,
+	0xbf8a0000, 0xbf810000,
+};
+
+static const u32 sgpr_init_compute_shader[] =
+{
+	0xbe8a0100, 0xbe8c0102,
+	0xbe8e0104, 0xbe900106,
+	0xbe920108, 0xbe940100,
+	0xbe960102, 0xbe980104,
+	0xbe9a0106, 0xbe9c0108,
+	0xbe9e0100, 0xbea00102,
+	0xbea20104, 0xbea40106,
+	0xbea60108, 0xbea80100,
+	0xbeaa0102, 0xbeac0104,
+	0xbeae0106, 0xbeb00108,
+	0xbeb20100, 0xbeb40102,
+	0xbeb60104, 0xbeb80106,
+	0xbeba0108, 0xbebc0100,
+	0xbebe0102, 0xbec00104,
+	0xbec20106, 0xbec40108,
+	0xbec60100, 0xbec80102,
+	0xbee60004, 0xbee70005,
+	0xbeea0006, 0xbeeb0007,
+	0xbee80008, 0xbee90009,
+	0xbefc0000, 0xbf8a0000,
+	0xbf810000, 0x00000000,
+};
+
+static const u32 vgpr_init_regs[] =
+{
+	mmCOMPUTE_STATIC_THREAD_MGMT_SE0, 0xffffffff,
+	mmCOMPUTE_RESOURCE_LIMITS, 0,
+	mmCOMPUTE_NUM_THREAD_X, 256*4,
+	mmCOMPUTE_NUM_THREAD_Y, 1,
+	mmCOMPUTE_NUM_THREAD_Z, 1,
+	mmCOMPUTE_PGM_RSRC2, 20,
+	mmCOMPUTE_USER_DATA_0, 0xedcedc00,
+	mmCOMPUTE_USER_DATA_1, 0xedcedc01,
+	mmCOMPUTE_USER_DATA_2, 0xedcedc02,
+	mmCOMPUTE_USER_DATA_3, 0xedcedc03,
+	mmCOMPUTE_USER_DATA_4, 0xedcedc04,
+	mmCOMPUTE_USER_DATA_5, 0xedcedc05,
+	mmCOMPUTE_USER_DATA_6, 0xedcedc06,
+	mmCOMPUTE_USER_DATA_7, 0xedcedc07,
+	mmCOMPUTE_USER_DATA_8, 0xedcedc08,
+	mmCOMPUTE_USER_DATA_9, 0xedcedc09,
+};
+
+static const u32 sgpr1_init_regs[] =
+{
+	mmCOMPUTE_STATIC_THREAD_MGMT_SE0, 0x0f,
+	mmCOMPUTE_RESOURCE_LIMITS, 0x1000000,
+	mmCOMPUTE_NUM_THREAD_X, 256*5,
+	mmCOMPUTE_NUM_THREAD_Y, 1,
+	mmCOMPUTE_NUM_THREAD_Z, 1,
+	mmCOMPUTE_PGM_RSRC2, 20,
+	mmCOMPUTE_USER_DATA_0, 0xedcedc00,
+	mmCOMPUTE_USER_DATA_1, 0xedcedc01,
+	mmCOMPUTE_USER_DATA_2, 0xedcedc02,
+	mmCOMPUTE_USER_DATA_3, 0xedcedc03,
+	mmCOMPUTE_USER_DATA_4, 0xedcedc04,
+	mmCOMPUTE_USER_DATA_5, 0xedcedc05,
+	mmCOMPUTE_USER_DATA_6, 0xedcedc06,
+	mmCOMPUTE_USER_DATA_7, 0xedcedc07,
+	mmCOMPUTE_USER_DATA_8, 0xedcedc08,
+	mmCOMPUTE_USER_DATA_9, 0xedcedc09,
+};
+
+static const u32 sgpr2_init_regs[] =
+{
+	mmCOMPUTE_STATIC_THREAD_MGMT_SE0, 0xf0,
+	mmCOMPUTE_RESOURCE_LIMITS, 0x1000000,
+	mmCOMPUTE_NUM_THREAD_X, 256*5,
+	mmCOMPUTE_NUM_THREAD_Y, 1,
+	mmCOMPUTE_NUM_THREAD_Z, 1,
+	mmCOMPUTE_PGM_RSRC2, 20,
+	mmCOMPUTE_USER_DATA_0, 0xedcedc00,
+	mmCOMPUTE_USER_DATA_1, 0xedcedc01,
+	mmCOMPUTE_USER_DATA_2, 0xedcedc02,
+	mmCOMPUTE_USER_DATA_3, 0xedcedc03,
+	mmCOMPUTE_USER_DATA_4, 0xedcedc04,
+	mmCOMPUTE_USER_DATA_5, 0xedcedc05,
+	mmCOMPUTE_USER_DATA_6, 0xedcedc06,
+	mmCOMPUTE_USER_DATA_7, 0xedcedc07,
+	mmCOMPUTE_USER_DATA_8, 0xedcedc08,
+	mmCOMPUTE_USER_DATA_9, 0xedcedc09,
+};
+
+static const u32 sec_ded_counter_registers[] =
+{
+	mmCPC_EDC_ATC_CNT,
+	mmCPC_EDC_SCRATCH_CNT,
+	mmCPC_EDC_UCODE_CNT,
+	mmCPF_EDC_ATC_CNT,
+	mmCPF_EDC_ROQ_CNT,
+	mmCPF_EDC_TAG_CNT,
+	mmCPG_EDC_ATC_CNT,
+	mmCPG_EDC_DMA_CNT,
+	mmCPG_EDC_TAG_CNT,
+	mmDC_EDC_CSINVOC_CNT,
+	mmDC_EDC_RESTORE_CNT,
+	mmDC_EDC_STATE_CNT,
+	mmGDS_EDC_CNT,
+	mmGDS_EDC_GRBM_CNT,
+	mmGDS_EDC_OA_DED,
+	mmSPI_EDC_CNT,
+	mmSQC_ATC_EDC_GATCL1_CNT,
+	mmSQC_EDC_CNT,
+	mmSQ_EDC_DED_CNT,
+	mmSQ_EDC_INFO,
+	mmSQ_EDC_SEC_CNT,
+	mmTCC_EDC_CNT,
+	mmTCP_ATC_EDC_GATCL1_CNT,
+	mmTCP_EDC_CNT,
+	mmTD_EDC_CNT
+};
+
+static int gfx_v8_0_do_edc_gpr_workarounds(struct amdgpu_device *adev)
+{
+	struct amdgpu_ring *ring = &adev->gfx.compute_ring[0];
+	struct amdgpu_ib ib;
+	struct fence *f = NULL;
+	int r, i;
+	u32 tmp;
+	unsigned total_size, vgpr_offset, sgpr_offset;
+	u64 gpu_addr;
+
+	/* only supported on CZ */
+	if (adev->asic_type != CHIP_CARRIZO)
+		return 0;
+
+	/* bail if the compute ring is not ready */
+	if (!ring->ready)
+		return 0;
+
+	tmp = RREG32(mmGB_EDC_MODE);
+	WREG32(mmGB_EDC_MODE, 0);
+
+	total_size =
+		(((ARRAY_SIZE(vgpr_init_regs) / 2) * 3) + 4 + 5 + 2) * 4;
+	total_size +=
+		(((ARRAY_SIZE(sgpr1_init_regs) / 2) * 3) + 4 + 5 + 2) * 4;
+	total_size +=
+		(((ARRAY_SIZE(sgpr2_init_regs) / 2) * 3) + 4 + 5 + 2) * 4;
+	total_size = ALIGN(total_size, 256);
+	vgpr_offset = total_size;
+	total_size += ALIGN(sizeof(vgpr_init_compute_shader), 256);
+	sgpr_offset = total_size;
+	total_size += sizeof(sgpr_init_compute_shader);
+
+	/* allocate an indirect buffer to put the commands in */
+	memset(&ib, 0, sizeof(ib));
+	r = amdgpu_ib_get(ring, NULL, total_size, &ib);
+	if (r) {
+		DRM_ERROR("amdgpu: failed to get ib (%d).\n", r);
+		return r;
+	}
+
+	/* load the compute shaders */
+	for (i = 0; i < ARRAY_SIZE(vgpr_init_compute_shader); i++)
+		ib.ptr[i + (vgpr_offset / 4)] = vgpr_init_compute_shader[i];
+
+	for (i = 0; i < ARRAY_SIZE(sgpr_init_compute_shader); i++)
+		ib.ptr[i + (sgpr_offset / 4)] = sgpr_init_compute_shader[i];
+
+	/* init the ib length to 0 */
+	ib.length_dw = 0;
+
+	/* VGPR */
+	/* write the register state for the compute dispatch */
+	for (i = 0; i < ARRAY_SIZE(vgpr_init_regs); i += 2) {
+		ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 1);
+		ib.ptr[ib.length_dw++] = vgpr_init_regs[i] - PACKET3_SET_SH_REG_START;
+		ib.ptr[ib.length_dw++] = vgpr_init_regs[i + 1];
+	}
+	/* write the shader start address: mmCOMPUTE_PGM_LO, mmCOMPUTE_PGM_HI */
+	gpu_addr = (ib.gpu_addr + (u64)vgpr_offset) >> 8;
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 2);
+	ib.ptr[ib.length_dw++] = mmCOMPUTE_PGM_LO - PACKET3_SET_SH_REG_START;
+	ib.ptr[ib.length_dw++] = lower_32_bits(gpu_addr);
+	ib.ptr[ib.length_dw++] = upper_32_bits(gpu_addr);
+
+	/* write dispatch packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_DISPATCH_DIRECT, 3);
+	ib.ptr[ib.length_dw++] = 8; /* x */
+	ib.ptr[ib.length_dw++] = 1; /* y */
+	ib.ptr[ib.length_dw++] = 1; /* z */
+	ib.ptr[ib.length_dw++] =
+		REG_SET_FIELD(0, COMPUTE_DISPATCH_INITIATOR, COMPUTE_SHADER_EN, 1);
+
+	/* write CS partial flush packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_EVENT_WRITE, 0);
+	ib.ptr[ib.length_dw++] = EVENT_TYPE(7) | EVENT_INDEX(4);
+
+	/* SGPR1 */
+	/* write the register state for the compute dispatch */
+	for (i = 0; i < ARRAY_SIZE(sgpr1_init_regs); i += 2) {
+		ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 1);
+		ib.ptr[ib.length_dw++] = sgpr1_init_regs[i] - PACKET3_SET_SH_REG_START;
+		ib.ptr[ib.length_dw++] = sgpr1_init_regs[i + 1];
+	}
+	/* write the shader start address: mmCOMPUTE_PGM_LO, mmCOMPUTE_PGM_HI */
+	gpu_addr = (ib.gpu_addr + (u64)sgpr_offset) >> 8;
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 2);
+	ib.ptr[ib.length_dw++] = mmCOMPUTE_PGM_LO - PACKET3_SET_SH_REG_START;
+	ib.ptr[ib.length_dw++] = lower_32_bits(gpu_addr);
+	ib.ptr[ib.length_dw++] = upper_32_bits(gpu_addr);
+
+	/* write dispatch packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_DISPATCH_DIRECT, 3);
+	ib.ptr[ib.length_dw++] = 8; /* x */
+	ib.ptr[ib.length_dw++] = 1; /* y */
+	ib.ptr[ib.length_dw++] = 1; /* z */
+	ib.ptr[ib.length_dw++] =
+		REG_SET_FIELD(0, COMPUTE_DISPATCH_INITIATOR, COMPUTE_SHADER_EN, 1);
+
+	/* write CS partial flush packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_EVENT_WRITE, 0);
+	ib.ptr[ib.length_dw++] = EVENT_TYPE(7) | EVENT_INDEX(4);
+
+	/* SGPR2 */
+	/* write the register state for the compute dispatch */
+	for (i = 0; i < ARRAY_SIZE(sgpr2_init_regs); i += 2) {
+		ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 1);
+		ib.ptr[ib.length_dw++] = sgpr2_init_regs[i] - PACKET3_SET_SH_REG_START;
+		ib.ptr[ib.length_dw++] = sgpr2_init_regs[i + 1];
+	}
+	/* write the shader start address: mmCOMPUTE_PGM_LO, mmCOMPUTE_PGM_HI */
+	gpu_addr = (ib.gpu_addr + (u64)sgpr_offset) >> 8;
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_SET_SH_REG, 2);
+	ib.ptr[ib.length_dw++] = mmCOMPUTE_PGM_LO - PACKET3_SET_SH_REG_START;
+	ib.ptr[ib.length_dw++] = lower_32_bits(gpu_addr);
+	ib.ptr[ib.length_dw++] = upper_32_bits(gpu_addr);
+
+	/* write dispatch packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_DISPATCH_DIRECT, 3);
+	ib.ptr[ib.length_dw++] = 8; /* x */
+	ib.ptr[ib.length_dw++] = 1; /* y */
+	ib.ptr[ib.length_dw++] = 1; /* z */
+	ib.ptr[ib.length_dw++] =
+		REG_SET_FIELD(0, COMPUTE_DISPATCH_INITIATOR, COMPUTE_SHADER_EN, 1);
+
+	/* write CS partial flush packet */
+	ib.ptr[ib.length_dw++] = PACKET3(PACKET3_EVENT_WRITE, 0);
+	ib.ptr[ib.length_dw++] = EVENT_TYPE(7) | EVENT_INDEX(4);
+
+	/* shedule the ib on the ring */
+	r = amdgpu_sched_ib_submit_kernel_helper(adev, ring, &ib, 1, NULL,
+						 AMDGPU_FENCE_OWNER_UNDEFINED,
+						 &f);
+	if (r) {
+		DRM_ERROR("amdgpu: ib submit failed (%d).\n", r);
+		goto fail;
+	}
+
+	/* wait for the GPU to finish processing the IB */
+	r = fence_wait(f, false);
+	if (r) {
+		DRM_ERROR("amdgpu: fence wait failed (%d).\n", r);
+		goto fail;
+	}
+
+	tmp = REG_SET_FIELD(tmp, GB_EDC_MODE, DED_MODE, 2);
+	tmp = REG_SET_FIELD(tmp, GB_EDC_MODE, PROP_FED, 1);
+	WREG32(mmGB_EDC_MODE, tmp);
+
+	tmp = RREG32(mmCC_GC_EDC_CONFIG);
+	tmp = REG_SET_FIELD(tmp, CC_GC_EDC_CONFIG, DIS_EDC, 0) | 1;
+	WREG32(mmCC_GC_EDC_CONFIG, tmp);
+
+
+	/* read back registers to clear the counters */
+	for (i = 0; i < ARRAY_SIZE(sec_ded_counter_registers); i++)
+		RREG32(sec_ded_counter_registers[i]);
+
+fail:
+	fence_put(f);
+	amdgpu_ib_free(adev, &ib);
+
+	return r;
+}
+
 static void gfx_v8_0_gpu_early_init(struct amdgpu_device *adev)
 {
 	u32 gb_addr_config;
@@ -1323,1418 +1660,923 @@ static int gfx_v8_0_sw_fini(void *handle)
 
 static void gfx_v8_0_tiling_mode_table_init(struct amdgpu_device *adev)
 {
-	const u32 num_tile_mode_states = 32;
-	const u32 num_secondary_tile_mode_states = 16;
-	u32 reg_offset, gb_tile_moden, split_equal_to_row_size;
+	uint32_t *modearray, *mod2array;
+	const u32 num_tile_mode_states = ARRAY_SIZE(adev->gfx.config.tile_mode_array);
+	const u32 num_secondary_tile_mode_states = ARRAY_SIZE(adev->gfx.config.macrotile_mode_array);
+	u32 reg_offset;
 
-	switch (adev->gfx.config.mem_row_size_in_kb) {
-	case 1:
-		split_equal_to_row_size = ADDR_SURF_TILE_SPLIT_1KB;
-		break;
-	case 2:
-	default:
-		split_equal_to_row_size = ADDR_SURF_TILE_SPLIT_2KB;
-		break;
-	case 4:
-		split_equal_to_row_size = ADDR_SURF_TILE_SPLIT_4KB;
-		break;
-	}
+	modearray = adev->gfx.config.tile_mode_array;
+	mod2array = adev->gfx.config.macrotile_mode_array;
+
+	for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+		modearray[reg_offset] = 0;
+
+	for (reg_offset = 0; reg_offset <  num_secondary_tile_mode_states; reg_offset++)
+		mod2array[reg_offset] = 0;
 
 	switch (adev->asic_type) {
 	case CHIP_TOPAZ:
-		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 1:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 2:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 3:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 4:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 5:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 6:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 8:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
-						PIPE_CONFIG(ADDR_SURF_P2));
-				break;
-			case 9:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 10:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 11:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 13:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 14:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 15:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 16:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 18:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 19:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 20:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 21:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 22:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 24:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 25:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 26:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 27:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 28:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 29:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 7:
-			case 12:
-			case 17:
-			case 23:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.tile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_TILE_MODE0 + reg_offset, gb_tile_moden);
-		}
-		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 1:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 2:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 3:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 4:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 5:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 6:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 8:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 9:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 10:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 11:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 12:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 13:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 14:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 7:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.macrotile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_MACROTILE_MODE0 + reg_offset, gb_tile_moden);
-		}
+		modearray[0] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[1] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[2] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[3] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[4] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[5] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[6] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[8] = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
+				PIPE_CONFIG(ADDR_SURF_P2));
+		modearray[9] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[10] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[11] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[13] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[14] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[15] = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[16] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[18] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[19] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[20] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[21] = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[22] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[24] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[25] = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[26] = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[27] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[28] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[29] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+
+		mod2array[0] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[1] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[2] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[3] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[4] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[5] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[6] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[8] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[9] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[10] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[11] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[12] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[13] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[14] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+
+		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+			if (reg_offset != 7 && reg_offset != 12 && reg_offset != 17 &&
+			    reg_offset != 23)
+				WREG32(mmGB_TILE_MODE0 + reg_offset, modearray[reg_offset]);
+
+		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++)
+			if (reg_offset != 7)
+				WREG32(mmGB_MACROTILE_MODE0 + reg_offset, mod2array[reg_offset]);
+
+		break;
 	case CHIP_FIJI:
-		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 1:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 2:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 3:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 4:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 5:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 6:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 7:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 8:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16));
-				break;
-			case 9:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 10:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 11:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 12:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 13:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 14:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 15:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 16:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 17:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 18:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 19:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 20:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 21:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 22:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 23:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 24:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 25:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 26:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 27:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 28:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 29:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 30:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			default:
-				gb_tile_moden = 0;
-				break;
-			}
-			adev->gfx.config.tile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_TILE_MODE0 + reg_offset, gb_tile_moden);
-		}
-		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 1:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 2:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 3:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 4:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 5:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 6:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 8:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 9:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 10:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 11:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 12:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 13:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 14:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_4_BANK));
-				break;
-			case 7:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			}
-			adev->gfx.config.macrotile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_MACROTILE_MODE0 + reg_offset, gb_tile_moden);
-		}
+		modearray[0] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[1] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[2] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[3] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[4] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[5] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[6] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[7] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[8] = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16));
+		modearray[9] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[10] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[11] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[12] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[13] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[14] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[15] = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[16] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[17] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[18] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[19] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[20] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[21] = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[22] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[23] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[24] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[25] = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[26] = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[27] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[28] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[29] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P16_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[30] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+
+		mod2array[0] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[1] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[2] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[3] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[4] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[5] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[6] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[8] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[9] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[10] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[11] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[12] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[13] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[14] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_4_BANK));
+
+		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+			WREG32(mmGB_TILE_MODE0 + reg_offset, modearray[reg_offset]);
+
+		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++)
+			if (reg_offset != 7)
+				WREG32(mmGB_MACROTILE_MODE0 + reg_offset, mod2array[reg_offset]);
+
 		break;
 	case CHIP_TONGA:
-		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 1:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 2:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 3:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 4:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 5:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 6:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 7:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 8:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16));
-				break;
-			case 9:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 10:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 11:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 12:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 13:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 14:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 15:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 16:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 17:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 18:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 19:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 20:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 21:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 22:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 23:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 24:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 25:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 26:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 27:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 28:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 29:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 30:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P4_16x16) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.tile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_TILE_MODE0 + reg_offset, gb_tile_moden);
-		}
-		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 1:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 2:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 3:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 4:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 5:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 6:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 8:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 9:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 10:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 11:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 12:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 13:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_4_BANK));
-				break;
-			case 14:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
-						NUM_BANKS(ADDR_SURF_4_BANK));
-				break;
-			case 7:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.macrotile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_MACROTILE_MODE0 + reg_offset, gb_tile_moden);
-		}
+		modearray[0] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[1] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[2] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[3] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[4] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[5] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[6] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[7] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[8] = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16));
+		modearray[9] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[10] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[11] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[12] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[13] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[14] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[15] = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[16] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[17] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[18] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[19] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[20] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[21] = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[22] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[23] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[24] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[25] = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[26] = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[27] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[28] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[29] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P8_32x32_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[30] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P4_16x16) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+
+		mod2array[0] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[1] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[2] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[3] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[4] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[5] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[6] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[8] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[9] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[10] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[11] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[12] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[13] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_4_BANK));
+		mod2array[14] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_1) |
+				 NUM_BANKS(ADDR_SURF_4_BANK));
+
+		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+			WREG32(mmGB_TILE_MODE0 + reg_offset, modearray[reg_offset]);
+
+		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++)
+			if (reg_offset != 7)
+				WREG32(mmGB_MACROTILE_MODE0 + reg_offset, mod2array[reg_offset]);
+
 		break;
 	case CHIP_STONEY:
-		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 1:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 2:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 3:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 4:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 5:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 6:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 8:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
-						PIPE_CONFIG(ADDR_SURF_P2));
-				break;
-			case 9:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 10:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 11:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 13:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 14:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 15:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 16:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 18:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 19:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 20:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 21:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 22:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 24:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 25:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 26:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 27:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 28:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 29:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 7:
-			case 12:
-			case 17:
-			case 23:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.tile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_TILE_MODE0 + reg_offset, gb_tile_moden);
-		}
-		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 1:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 2:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 3:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 4:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 5:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 6:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 8:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 9:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 10:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 11:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 12:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 13:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 14:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 7:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.macrotile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_MACROTILE_MODE0 + reg_offset, gb_tile_moden);
-		}
+		modearray[0] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[1] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[2] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[3] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[4] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[5] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[6] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[8] = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
+				PIPE_CONFIG(ADDR_SURF_P2));
+		modearray[9] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[10] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[11] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[13] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[14] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[15] = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[16] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[18] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[19] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[20] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[21] = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[22] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[24] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[25] = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[26] = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[27] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[28] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[29] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+
+		mod2array[0] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[1] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[2] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[3] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[4] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[5] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[6] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[8] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[9] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[10] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[11] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[12] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[13] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[14] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+
+		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+			if (reg_offset != 7 && reg_offset != 12 && reg_offset != 17 &&
+			    reg_offset != 23)
+				WREG32(mmGB_TILE_MODE0 + reg_offset, modearray[reg_offset]);
+
+		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++)
+			if (reg_offset != 7)
+				WREG32(mmGB_MACROTILE_MODE0 + reg_offset, mod2array[reg_offset]);
+
 		break;
-	case CHIP_CARRIZO:
 	default:
-		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 1:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 2:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 3:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 4:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 5:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 6:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
-				break;
-			case 8:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
-						PIPE_CONFIG(ADDR_SURF_P2));
-				break;
-			case 9:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 10:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 11:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 13:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 14:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 15:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 16:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 18:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 19:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 20:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 21:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 22:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 24:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 25:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 26:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
-				break;
-			case 27:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 28:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
-				break;
-			case 29:
-				gb_tile_moden = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
-						PIPE_CONFIG(ADDR_SURF_P2) |
-						MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
-						SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
-				break;
-			case 7:
-			case 12:
-			case 17:
-			case 23:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.tile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_TILE_MODE0 + reg_offset, gb_tile_moden);
-		}
-		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++) {
-			switch (reg_offset) {
-			case 0:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 1:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 2:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 3:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 4:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 5:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 6:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 8:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 9:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 10:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 11:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 12:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 13:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
-						NUM_BANKS(ADDR_SURF_16_BANK));
-				break;
-			case 14:
-				gb_tile_moden = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
-						BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
-						MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
-						NUM_BANKS(ADDR_SURF_8_BANK));
-				break;
-			case 7:
-				/* unused idx */
-				continue;
-			default:
-				gb_tile_moden = 0;
-				break;
-			};
-			adev->gfx.config.macrotile_mode_array[reg_offset] = gb_tile_moden;
-			WREG32(mmGB_MACROTILE_MODE0 + reg_offset, gb_tile_moden);
-		}
+		dev_warn(adev->dev,
+			 "Unknown chip type (%d) in function gfx_v8_0_tiling_mode_table_init() falling through to CHIP_CARRIZO\n",
+			 adev->asic_type);
+
+	case CHIP_CARRIZO:
+		modearray[0] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_64B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[1] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_128B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[2] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_256B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[3] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_512B) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[4] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[5] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[6] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				TILE_SPLIT(ADDR_SURF_TILE_SPLIT_2KB) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DEPTH_MICRO_TILING));
+		modearray[8] = (ARRAY_MODE(ARRAY_LINEAR_ALIGNED) |
+				PIPE_CONFIG(ADDR_SURF_P2));
+		modearray[9] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				PIPE_CONFIG(ADDR_SURF_P2) |
+				MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[10] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[11] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_DISPLAY_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[13] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[14] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[15] = (ARRAY_MODE(ARRAY_3D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[16] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+		modearray[18] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[19] = (ARRAY_MODE(ARRAY_1D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[20] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[21] = (ARRAY_MODE(ARRAY_3D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[22] = (ARRAY_MODE(ARRAY_PRT_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[24] = (ARRAY_MODE(ARRAY_2D_TILED_THICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THIN_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[25] = (ARRAY_MODE(ARRAY_2D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[26] = (ARRAY_MODE(ARRAY_3D_TILED_XTHICK) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_THICK_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_1));
+		modearray[27] = (ARRAY_MODE(ARRAY_1D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[28] = (ARRAY_MODE(ARRAY_2D_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_2));
+		modearray[29] = (ARRAY_MODE(ARRAY_PRT_TILED_THIN1) |
+				 PIPE_CONFIG(ADDR_SURF_P2) |
+				 MICRO_TILE_MODE_NEW(ADDR_SURF_ROTATED_MICRO_TILING) |
+				 SAMPLE_SPLIT(ADDR_SURF_SAMPLE_SPLIT_8));
+
+		mod2array[0] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[1] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[2] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[3] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[4] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[5] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[6] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				NUM_BANKS(ADDR_SURF_8_BANK));
+		mod2array[8] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_8) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[9] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_4) |
+				BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[10] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_4) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[11] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_2) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[12] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_2) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[13] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_4) |
+				 NUM_BANKS(ADDR_SURF_16_BANK));
+		mod2array[14] = (BANK_WIDTH(ADDR_SURF_BANK_WIDTH_1) |
+				 BANK_HEIGHT(ADDR_SURF_BANK_HEIGHT_1) |
+				 MACRO_TILE_ASPECT(ADDR_SURF_MACRO_ASPECT_2) |
+				 NUM_BANKS(ADDR_SURF_8_BANK));
+
+		for (reg_offset = 0; reg_offset < num_tile_mode_states; reg_offset++)
+			if (reg_offset != 7 && reg_offset != 12 && reg_offset != 17 &&
+			    reg_offset != 23)
+				WREG32(mmGB_TILE_MODE0 + reg_offset, modearray[reg_offset]);
+
+		for (reg_offset = 0; reg_offset < num_secondary_tile_mode_states; reg_offset++)
+			if (reg_offset != 7)
+				WREG32(mmGB_MACROTILE_MODE0 + reg_offset, mod2array[reg_offset]);
+
+		break;
 	}
 }
 
 static u32 gfx_v8_0_create_bitmask(u32 bit_width)
 {
-	u32 i, mask = 0;
-
-	for (i = 0; i < bit_width; i++) {
-		mask <<= 1;
-		mask |= 1;
-	}
-	return mask;
+	return (u32)((1ULL << bit_width) - 1);
 }
 
 void gfx_v8_0_select_se_sh(struct amdgpu_device *adev, u32 se_num, u32 sh_num)
@@ -2809,7 +2651,7 @@ static void gfx_v8_0_setup_rb(struct amdgpu_device *adev,
 	mutex_lock(&adev->grbm_idx_mutex);
 	for (i = 0; i < se_num; i++) {
 		gfx_v8_0_select_se_sh(adev, i, 0xffffffff);
-		data = 0;
+		data = RREG32(mmPA_SC_RASTER_CONFIG);
 		for (j = 0; j < sh_per_se; j++) {
 			switch (enabled_rbs & 3) {
 			case 0:
@@ -2997,17 +2839,11 @@ static void gfx_v8_0_enable_gui_idle_interrupt(struct amdgpu_device *adev,
 {
 	u32 tmp = RREG32(mmCP_INT_CNTL_RING0);
 
-	if (enable) {
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_BUSY_INT_ENABLE, 1);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_EMPTY_INT_ENABLE, 1);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CMP_BUSY_INT_ENABLE, 1);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, GFX_IDLE_INT_ENABLE, 1);
-	} else {
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_BUSY_INT_ENABLE, 0);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_EMPTY_INT_ENABLE, 0);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CMP_BUSY_INT_ENABLE, 0);
-		tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, GFX_IDLE_INT_ENABLE, 0);
-	}
+	tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_BUSY_INT_ENABLE, enable ? 1 : 0);
+	tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CNTX_EMPTY_INT_ENABLE, enable ? 1 : 0);
+	tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, CMP_BUSY_INT_ENABLE, enable ? 1 : 0);
+	tmp = REG_SET_FIELD(tmp, CP_INT_CNTL_RING0, GFX_IDLE_INT_ENABLE, enable ? 1 : 0);
+
 	WREG32(mmCP_INT_CNTL_RING0, tmp);
 }
 
@@ -3087,16 +2923,18 @@ static int gfx_v8_0_rlc_resume(struct amdgpu_device *adev)
 
 	gfx_v8_0_rlc_reset(adev);
 
-	if (!adev->firmware.smu_load) {
-		/* legacy rlc firmware loading */
-		r = gfx_v8_0_rlc_load_microcode(adev);
-		if (r)
-			return r;
-	} else {
-		r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-						AMDGPU_UCODE_ID_RLC_G);
-		if (r)
-			return -EINVAL;
+	if (!adev->pp_enabled) {
+		if (!adev->firmware.smu_load) {
+			/* legacy rlc firmware loading */
+			r = gfx_v8_0_rlc_load_microcode(adev);
+			if (r)
+				return r;
+		} else {
+			r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+							AMDGPU_UCODE_ID_RLC_G);
+			if (r)
+				return -EINVAL;
+		}
 	}
 
 	gfx_v8_0_rlc_start(adev);
@@ -3941,6 +3779,11 @@ static int gfx_v8_0_cp_compute_resume(struct amdgpu_device *adev)
 		tmp = REG_SET_FIELD(tmp, CP_HQD_PERSISTENT_STATE, PRELOAD_SIZE, 0x53);
 		WREG32(mmCP_HQD_PERSISTENT_STATE, tmp);
 		mqd->cp_hqd_persistent_state = tmp;
+		if (adev->asic_type == CHIP_STONEY) {
+			tmp = RREG32(mmCP_ME1_PIPE3_INT_CNTL);
+			tmp = REG_SET_FIELD(tmp, CP_ME1_PIPE3_INT_CNTL, GENERIC2_INT_ENABLE, 1);
+			WREG32(mmCP_ME1_PIPE3_INT_CNTL, tmp);
+		}
 
 		/* activate the queue */
 		mqd->cp_hqd_active = 1;
@@ -3982,35 +3825,43 @@ static int gfx_v8_0_cp_resume(struct amdgpu_device *adev)
 	if (!(adev->flags & AMD_IS_APU))
 		gfx_v8_0_enable_gui_idle_interrupt(adev, false);
 
-	if (!adev->firmware.smu_load) {
-		/* legacy firmware loading */
-		r = gfx_v8_0_cp_gfx_load_microcode(adev);
-		if (r)
-			return r;
-
-		r = gfx_v8_0_cp_compute_load_microcode(adev);
-		if (r)
-			return r;
-	} else {
-		r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-						AMDGPU_UCODE_ID_CP_CE);
-		if (r)
-			return -EINVAL;
-
-		r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-						AMDGPU_UCODE_ID_CP_PFP);
-		if (r)
-			return -EINVAL;
-
-		r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-						AMDGPU_UCODE_ID_CP_ME);
-		if (r)
-			return -EINVAL;
+	if (!adev->pp_enabled) {
+		if (!adev->firmware.smu_load) {
+			/* legacy firmware loading */
+			r = gfx_v8_0_cp_gfx_load_microcode(adev);
+			if (r)
+				return r;
 
-		r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-						AMDGPU_UCODE_ID_CP_MEC1);
-		if (r)
-			return -EINVAL;
+			r = gfx_v8_0_cp_compute_load_microcode(adev);
+			if (r)
+				return r;
+		} else {
+			r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+							AMDGPU_UCODE_ID_CP_CE);
+			if (r)
+				return -EINVAL;
+
+			r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+							AMDGPU_UCODE_ID_CP_PFP);
+			if (r)
+				return -EINVAL;
+
+			r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+							AMDGPU_UCODE_ID_CP_ME);
+			if (r)
+				return -EINVAL;
+
+			if (adev->asic_type == CHIP_TOPAZ) {
+				r = gfx_v8_0_cp_compute_load_microcode(adev);
+				if (r)
+					return r;
+			} else {
+				r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+										 AMDGPU_UCODE_ID_CP_MEC1);
+				if (r)
+					return -EINVAL;
+			}
+		}
 	}
 
 	r = gfx_v8_0_cp_gfx_resume(adev);
@@ -4056,6 +3907,8 @@ static int gfx_v8_0_hw_fini(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	amdgpu_irq_put(adev, &adev->gfx.priv_reg_irq, 0);
+	amdgpu_irq_put(adev, &adev->gfx.priv_inst_irq, 0);
 	gfx_v8_0_cp_enable(adev, false);
 	gfx_v8_0_rlc_stop(adev);
 	gfx_v8_0_cp_compute_fini(adev);
@@ -4341,7 +4194,18 @@ static int gfx_v8_0_soft_reset(void *handle)
 		gfx_v8_0_cp_gfx_enable(adev, false);
 
 		/* Disable MEC parsing/prefetching */
-		/* XXX todo */
+		gfx_v8_0_cp_compute_enable(adev, false);
+
+		if (grbm_soft_reset || srbm_soft_reset) {
+			tmp = RREG32(mmGMCON_DEBUG);
+			tmp = REG_SET_FIELD(tmp,
+					    GMCON_DEBUG, GFX_STALL, 1);
+			tmp = REG_SET_FIELD(tmp,
+					    GMCON_DEBUG, GFX_CLEAR, 1);
+			WREG32(mmGMCON_DEBUG, tmp);
+
+			udelay(50);
+		}
 
 		if (grbm_soft_reset) {
 			tmp = RREG32(mmGRBM_SOFT_RESET);
@@ -4370,6 +4234,16 @@ static int gfx_v8_0_soft_reset(void *handle)
 			WREG32(mmSRBM_SOFT_RESET, tmp);
 			tmp = RREG32(mmSRBM_SOFT_RESET);
 		}
+
+		if (grbm_soft_reset || srbm_soft_reset) {
+			tmp = RREG32(mmGMCON_DEBUG);
+			tmp = REG_SET_FIELD(tmp,
+					    GMCON_DEBUG, GFX_STALL, 0);
+			tmp = REG_SET_FIELD(tmp,
+					    GMCON_DEBUG, GFX_CLEAR, 0);
+			WREG32(mmGMCON_DEBUG, tmp);
+		}
+
 		/* Wait a little for things to settle down */
 		udelay(50);
 		gfx_v8_0_print_status((void *)adev);
@@ -4458,15 +4332,269 @@ static int gfx_v8_0_early_init(void *handle)
 	return 0;
 }
 
+static int gfx_v8_0_late_init(void *handle)
+{
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int r;
+
+	r = amdgpu_irq_get(adev, &adev->gfx.priv_reg_irq, 0);
+	if (r)
+		return r;
+
+	r = amdgpu_irq_get(adev, &adev->gfx.priv_inst_irq, 0);
+	if (r)
+		return r;
+
+	/* requires IBs so do in late init after IB pool is initialized */
+	r = gfx_v8_0_do_edc_gpr_workarounds(adev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
 static int gfx_v8_0_set_powergating_state(void *handle,
 					  enum amd_powergating_state state)
 {
 	return 0;
 }
 
+static void fiji_send_serdes_cmd(struct amdgpu_device *adev,
+		uint32_t reg_addr, uint32_t cmd)
+{
+	uint32_t data;
+
+	gfx_v8_0_select_se_sh(adev, 0xffffffff, 0xffffffff);
+
+	WREG32(mmRLC_SERDES_WR_CU_MASTER_MASK, 0xffffffff);
+	WREG32(mmRLC_SERDES_WR_NONCU_MASTER_MASK, 0xffffffff);
+
+	data = RREG32(mmRLC_SERDES_WR_CTRL);
+	data &= ~(RLC_SERDES_WR_CTRL__WRITE_COMMAND_MASK |
+			RLC_SERDES_WR_CTRL__READ_COMMAND_MASK |
+			RLC_SERDES_WR_CTRL__P1_SELECT_MASK |
+			RLC_SERDES_WR_CTRL__P2_SELECT_MASK |
+			RLC_SERDES_WR_CTRL__RDDATA_RESET_MASK |
+			RLC_SERDES_WR_CTRL__POWER_DOWN_MASK |
+			RLC_SERDES_WR_CTRL__POWER_UP_MASK |
+			RLC_SERDES_WR_CTRL__SHORT_FORMAT_MASK |
+			RLC_SERDES_WR_CTRL__BPM_DATA_MASK |
+			RLC_SERDES_WR_CTRL__REG_ADDR_MASK |
+			RLC_SERDES_WR_CTRL__SRBM_OVERRIDE_MASK);
+	data |= (RLC_SERDES_WR_CTRL__RSVD_BPM_ADDR_MASK |
+			(cmd << RLC_SERDES_WR_CTRL__BPM_DATA__SHIFT) |
+			(reg_addr << RLC_SERDES_WR_CTRL__REG_ADDR__SHIFT) |
+			(0xff << RLC_SERDES_WR_CTRL__BPM_ADDR__SHIFT));
+
+	WREG32(mmRLC_SERDES_WR_CTRL, data);
+}
+
+static void fiji_update_medium_grain_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	/* It is disabled by HW by default */
+	if (enable) {
+		/* 1 - RLC memory Light sleep */
+		temp = data = RREG32(mmRLC_MEM_SLP_CNTL);
+		data |= RLC_MEM_SLP_CNTL__RLC_MEM_LS_EN_MASK;
+		if (temp != data)
+			WREG32(mmRLC_MEM_SLP_CNTL, data);
+
+		/* 2 - CP memory Light sleep */
+		temp = data = RREG32(mmCP_MEM_SLP_CNTL);
+		data |= CP_MEM_SLP_CNTL__CP_MEM_LS_EN_MASK;
+		if (temp != data)
+			WREG32(mmCP_MEM_SLP_CNTL, data);
+
+		/* 3 - RLC_CGTT_MGCG_OVERRIDE */
+		temp = data = RREG32(mmRLC_CGTT_MGCG_OVERRIDE);
+		data &= ~(RLC_CGTT_MGCG_OVERRIDE__CPF_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__RLC_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__MGCG_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__GRBM_MASK);
+
+		if (temp != data)
+			WREG32(mmRLC_CGTT_MGCG_OVERRIDE, data);
+
+		/* 4 - wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* 5 - clear mgcg override */
+		fiji_send_serdes_cmd(adev, BPM_REG_MGCG_OVERRIDE, CLE_BPM_SERDES_CMD);
+
+		/* 6 - Enable CGTS(Tree Shade) MGCG /MGLS */
+		temp = data = RREG32(mmCGTS_SM_CTRL_REG);
+		data &= ~(CGTS_SM_CTRL_REG__SM_MODE_MASK);
+		data |= (0x2 << CGTS_SM_CTRL_REG__SM_MODE__SHIFT);
+		data |= CGTS_SM_CTRL_REG__SM_MODE_ENABLE_MASK;
+		data &= ~CGTS_SM_CTRL_REG__OVERRIDE_MASK;
+		data &= ~CGTS_SM_CTRL_REG__LS_OVERRIDE_MASK;
+		data |= CGTS_SM_CTRL_REG__ON_MONITOR_ADD_EN_MASK;
+		data |= (0x96 << CGTS_SM_CTRL_REG__ON_MONITOR_ADD__SHIFT);
+		if (temp != data)
+			WREG32(mmCGTS_SM_CTRL_REG, data);
+		udelay(50);
+
+		/* 7 - wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+	} else {
+		/* 1 - MGCG_OVERRIDE[0] for CP and MGCG_OVERRIDE[1] for RLC */
+		temp = data = RREG32(mmRLC_CGTT_MGCG_OVERRIDE);
+		data |= (RLC_CGTT_MGCG_OVERRIDE__CPF_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__RLC_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__MGCG_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__GRBM_MASK);
+		if (temp != data)
+			WREG32(mmRLC_CGTT_MGCG_OVERRIDE, data);
+
+		/* 2 - disable MGLS in RLC */
+		data = RREG32(mmRLC_MEM_SLP_CNTL);
+		if (data & RLC_MEM_SLP_CNTL__RLC_MEM_LS_EN_MASK) {
+			data &= ~RLC_MEM_SLP_CNTL__RLC_MEM_LS_EN_MASK;
+			WREG32(mmRLC_MEM_SLP_CNTL, data);
+		}
+
+		/* 3 - disable MGLS in CP */
+		data = RREG32(mmCP_MEM_SLP_CNTL);
+		if (data & CP_MEM_SLP_CNTL__CP_MEM_LS_EN_MASK) {
+			data &= ~CP_MEM_SLP_CNTL__CP_MEM_LS_EN_MASK;
+			WREG32(mmCP_MEM_SLP_CNTL, data);
+		}
+
+		/* 4 - Disable CGTS(Tree Shade) MGCG and MGLS */
+		temp = data = RREG32(mmCGTS_SM_CTRL_REG);
+		data |= (CGTS_SM_CTRL_REG__OVERRIDE_MASK |
+				CGTS_SM_CTRL_REG__LS_OVERRIDE_MASK);
+		if (temp != data)
+			WREG32(mmCGTS_SM_CTRL_REG, data);
+
+		/* 5 - wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* 6 - set mgcg override */
+		fiji_send_serdes_cmd(adev, BPM_REG_MGCG_OVERRIDE, SET_BPM_SERDES_CMD);
+
+		udelay(50);
+
+		/* 7- wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+	}
+}
+
+static void fiji_update_coarse_grain_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, temp1, data, data1;
+
+	temp = data = RREG32(mmRLC_CGCG_CGLS_CTRL);
+
+	if (enable) {
+		/* 1 enable cntx_empty_int_enable/cntx_busy_int_enable/
+		 * Cmp_busy/GFX_Idle interrupts
+		 */
+		gfx_v8_0_enable_gui_idle_interrupt(adev, true);
+
+		temp1 = data1 =	RREG32(mmRLC_CGTT_MGCG_OVERRIDE);
+		data1 &= ~RLC_CGTT_MGCG_OVERRIDE__CGCG_MASK;
+		if (temp1 != data1)
+			WREG32(mmRLC_CGTT_MGCG_OVERRIDE, data1);
+
+		/* 2 wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* 3 - clear cgcg override */
+		fiji_send_serdes_cmd(adev, BPM_REG_CGCG_OVERRIDE, CLE_BPM_SERDES_CMD);
+
+		/* wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* 4 - write cmd to set CGLS */
+		fiji_send_serdes_cmd(adev, BPM_REG_CGLS_EN, SET_BPM_SERDES_CMD);
+
+		/* 5 - enable cgcg */
+		data |= RLC_CGCG_CGLS_CTRL__CGCG_EN_MASK;
+
+		/* enable cgls*/
+		data |= RLC_CGCG_CGLS_CTRL__CGLS_EN_MASK;
+
+		temp1 = data1 =	RREG32(mmRLC_CGTT_MGCG_OVERRIDE);
+		data1 &= ~RLC_CGTT_MGCG_OVERRIDE__CGLS_MASK;
+
+		if (temp1 != data1)
+			WREG32(mmRLC_CGTT_MGCG_OVERRIDE, data1);
+
+		if (temp != data)
+			WREG32(mmRLC_CGCG_CGLS_CTRL, data);
+	} else {
+		/* disable cntx_empty_int_enable & GFX Idle interrupt */
+		gfx_v8_0_enable_gui_idle_interrupt(adev, false);
+
+		/* TEST CGCG */
+		temp1 = data1 =	RREG32(mmRLC_CGTT_MGCG_OVERRIDE);
+		data1 |= (RLC_CGTT_MGCG_OVERRIDE__CGCG_MASK |
+				RLC_CGTT_MGCG_OVERRIDE__CGLS_MASK);
+		if (temp1 != data1)
+			WREG32(mmRLC_CGTT_MGCG_OVERRIDE, data1);
+
+		/* read gfx register to wake up cgcg */
+		RREG32(mmCB_CGTT_SCLK_CTRL);
+		RREG32(mmCB_CGTT_SCLK_CTRL);
+		RREG32(mmCB_CGTT_SCLK_CTRL);
+		RREG32(mmCB_CGTT_SCLK_CTRL);
+
+		/* wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* write cmd to Set CGCG Overrride */
+		fiji_send_serdes_cmd(adev, BPM_REG_CGCG_OVERRIDE, SET_BPM_SERDES_CMD);
+
+		/* wait for RLC_SERDES_CU_MASTER & RLC_SERDES_NONCU_MASTER idle */
+		gfx_v8_0_wait_for_rlc_serdes(adev);
+
+		/* write cmd to Clear CGLS */
+		fiji_send_serdes_cmd(adev, BPM_REG_CGLS_EN, CLE_BPM_SERDES_CMD);
+
+		/* disable cgcg, cgls should be disabled too. */
+		data &= ~(RLC_CGCG_CGLS_CTRL__CGCG_EN_MASK |
+				RLC_CGCG_CGLS_CTRL__CGLS_EN_MASK);
+		if (temp != data)
+			WREG32(mmRLC_CGCG_CGLS_CTRL, data);
+	}
+}
+static int fiji_update_gfx_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	if (enable) {
+		/* CGCG/CGLS should be enabled after MGCG/MGLS/TS(CG/LS)
+		 * ===  MGCG + MGLS + TS(CG/LS) ===
+		 */
+		fiji_update_medium_grain_clock_gating(adev, enable);
+		fiji_update_coarse_grain_clock_gating(adev, enable);
+	} else {
+		/* CGCG/CGLS should be disabled before MGCG/MGLS/TS(CG/LS)
+		 * ===  CGCG + CGLS ===
+		 */
+		fiji_update_coarse_grain_clock_gating(adev, enable);
+		fiji_update_medium_grain_clock_gating(adev, enable);
+	}
+	return 0;
+}
+
 static int gfx_v8_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	switch (adev->asic_type) {
+	case CHIP_FIJI:
+		fiji_update_gfx_clock_gating(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		break;
+	default:
+		break;
+	}
 	return 0;
 }
 
@@ -4627,7 +4755,7 @@ static void gfx_v8_0_ring_emit_fence_gfx(struct amdgpu_ring *ring, u64 addr,
 				 EVENT_TYPE(CACHE_FLUSH_AND_INV_TS_EVENT) |
 				 EVENT_INDEX(5)));
 	amdgpu_ring_write(ring, addr & 0xfffffffc);
-	amdgpu_ring_write(ring, (upper_32_bits(addr) & 0xffff) | 
+	amdgpu_ring_write(ring, (upper_32_bits(addr) & 0xffff) |
 			  DATA_SEL(write64bit ? 2 : 1) | INT_SEL(int_sel ? 2 : 0));
 	amdgpu_ring_write(ring, lower_32_bits(seq));
 	amdgpu_ring_write(ring, upper_32_bits(seq));
@@ -4681,7 +4809,8 @@ static void gfx_v8_0_ring_emit_vm_flush(struct amdgpu_ring *ring,
 
 	amdgpu_ring_write(ring, PACKET3(PACKET3_WAIT_REG_MEM, 5));
 	amdgpu_ring_write(ring, (WAIT_REG_MEM_MEM_SPACE(1) | /* memory */
-		 WAIT_REG_MEM_FUNCTION(3))); /* equal */
+				 WAIT_REG_MEM_FUNCTION(3) | /* equal */
+				 WAIT_REG_MEM_ENGINE(usepfp))); /* pfp or me */
 	amdgpu_ring_write(ring, addr & 0xfffffffc);
 	amdgpu_ring_write(ring, upper_32_bits(addr) & 0xffffffff);
 	amdgpu_ring_write(ring, seq);
@@ -4867,7 +4996,7 @@ static int gfx_v8_0_set_priv_reg_fault_state(struct amdgpu_device *adev,
 	case AMDGPU_IRQ_STATE_ENABLE:
 		cp_int_cntl = RREG32(mmCP_INT_CNTL_RING0);
 		cp_int_cntl = REG_SET_FIELD(cp_int_cntl, CP_INT_CNTL_RING0,
-					    PRIV_REG_INT_ENABLE, 0);
+					    PRIV_REG_INT_ENABLE, 1);
 		WREG32(mmCP_INT_CNTL_RING0, cp_int_cntl);
 		break;
 	default:
@@ -4995,7 +5124,7 @@ static int gfx_v8_0_priv_inst_irq(struct amdgpu_device *adev,
 
 const struct amd_ip_funcs gfx_v8_0_ip_funcs = {
 	.early_init = gfx_v8_0_early_init,
-	.late_init = NULL,
+	.late_init = gfx_v8_0_late_init,
 	.sw_init = gfx_v8_0_sw_init,
 	.sw_fini = gfx_v8_0_sw_fini,
 	.hw_init = gfx_v8_0_hw_init,
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
index ed8abb58a785..b8060795b27b 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v7_0.c
@@ -42,9 +42,39 @@ static void gmc_v7_0_set_irq_funcs(struct amdgpu_device *adev);
 
 MODULE_FIRMWARE("radeon/bonaire_mc.bin");
 MODULE_FIRMWARE("radeon/hawaii_mc.bin");
+MODULE_FIRMWARE("amdgpu/topaz_mc.bin");
+
+static const u32 golden_settings_iceland_a11[] =
+{
+	mmVM_PRT_APERTURE0_LOW_ADDR, 0x0fffffff, 0x0fffffff,
+	mmVM_PRT_APERTURE1_LOW_ADDR, 0x0fffffff, 0x0fffffff,
+	mmVM_PRT_APERTURE2_LOW_ADDR, 0x0fffffff, 0x0fffffff,
+	mmVM_PRT_APERTURE3_LOW_ADDR, 0x0fffffff, 0x0fffffff
+};
+
+static const u32 iceland_mgcg_cgcg_init[] =
+{
+	mmMC_MEM_POWER_LS, 0xffffffff, 0x00000104
+};
+
+static void gmc_v7_0_init_golden_registers(struct amdgpu_device *adev)
+{
+	switch (adev->asic_type) {
+	case CHIP_TOPAZ:
+		amdgpu_program_register_sequence(adev,
+						 iceland_mgcg_cgcg_init,
+						 (const u32)ARRAY_SIZE(iceland_mgcg_cgcg_init));
+		amdgpu_program_register_sequence(adev,
+						 golden_settings_iceland_a11,
+						 (const u32)ARRAY_SIZE(golden_settings_iceland_a11));
+		break;
+	default:
+		break;
+	}
+}
 
 /**
- * gmc8_mc_wait_for_idle - wait for MC idle callback.
+ * gmc7_mc_wait_for_idle - wait for MC idle callback.
  *
  * @adev: amdgpu_device pointer
  *
@@ -132,13 +162,20 @@ static int gmc_v7_0_init_microcode(struct amdgpu_device *adev)
 	case CHIP_HAWAII:
 		chip_name = "hawaii";
 		break;
+	case CHIP_TOPAZ:
+		chip_name = "topaz";
+		break;
 	case CHIP_KAVERI:
 	case CHIP_KABINI:
 		return 0;
 	default: BUG();
 	}
 
-	snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+	if (adev->asic_type == CHIP_TOPAZ)
+		snprintf(fw_name, sizeof(fw_name), "amdgpu/%s_mc.bin", chip_name);
+	else
+		snprintf(fw_name, sizeof(fw_name), "radeon/%s_mc.bin", chip_name);
+
 	err = request_firmware(&adev->mc.fw, fw_name, adev->dev);
 	if (err)
 		goto out;
@@ -370,6 +407,10 @@ static int gmc_v7_0_mc_init(struct amdgpu_device *adev)
 	adev->mc.real_vram_size = RREG32(mmCONFIG_MEMSIZE) * 1024ULL * 1024ULL;
 	adev->mc.visible_vram_size = adev->mc.aper_size;
 
+	/* In case the PCI BAR is larger than the actual amount of vram */
+	if (adev->mc.visible_vram_size > adev->mc.real_vram_size)
+		adev->mc.visible_vram_size = adev->mc.real_vram_size;
+
 	/* unless the user had overridden it, set the gart
 	 * size equal to the 1024 or vram, whichever is larger.
 	 */
@@ -751,7 +792,7 @@ static void gmc_v7_0_enable_mc_ls(struct amdgpu_device *adev,
 
 	for (i = 0; i < ARRAY_SIZE(mc_cg_registers); i++) {
 		orig = data = RREG32(mc_cg_registers[i]);
-		if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_MC_LS))
+		if (enable && (adev->cg_flags & AMD_CG_SUPPORT_MC_LS))
 			data |= mc_cg_ls_en[i];
 		else
 			data &= ~mc_cg_ls_en[i];
@@ -768,7 +809,7 @@ static void gmc_v7_0_enable_mc_mgcg(struct amdgpu_device *adev,
 
 	for (i = 0; i < ARRAY_SIZE(mc_cg_registers); i++) {
 		orig = data = RREG32(mc_cg_registers[i]);
-		if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_MC_MGCG))
+		if (enable && (adev->cg_flags & AMD_CG_SUPPORT_MC_MGCG))
 			data |= mc_cg_en[i];
 		else
 			data &= ~mc_cg_en[i];
@@ -784,7 +825,7 @@ static void gmc_v7_0_enable_bif_mgls(struct amdgpu_device *adev,
 
 	orig = data = RREG32_PCIE(ixPCIE_CNTL2);
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_BIF_LS)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_BIF_LS)) {
 		data = REG_SET_FIELD(data, PCIE_CNTL2, SLV_MEM_LS_EN, 1);
 		data = REG_SET_FIELD(data, PCIE_CNTL2, MST_MEM_LS_EN, 1);
 		data = REG_SET_FIELD(data, PCIE_CNTL2, REPLAY_MEM_LS_EN, 1);
@@ -807,7 +848,7 @@ static void gmc_v7_0_enable_hdp_mgcg(struct amdgpu_device *adev,
 
 	orig = data = RREG32(mmHDP_HOST_PATH_CNTL);
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_HDP_MGCG))
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_HDP_MGCG))
 		data = REG_SET_FIELD(data, HDP_HOST_PATH_CNTL, CLOCK_GATING_DIS, 0);
 	else
 		data = REG_SET_FIELD(data, HDP_HOST_PATH_CNTL, CLOCK_GATING_DIS, 1);
@@ -823,7 +864,7 @@ static void gmc_v7_0_enable_hdp_ls(struct amdgpu_device *adev,
 
 	orig = data = RREG32(mmHDP_MEM_POWER_LS);
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_HDP_LS))
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_HDP_LS))
 		data = REG_SET_FIELD(data, HDP_MEM_POWER_LS, LS_ENABLE, 1);
 	else
 		data = REG_SET_FIELD(data, HDP_MEM_POWER_LS, LS_ENABLE, 0);
@@ -980,6 +1021,8 @@ static int gmc_v7_0_hw_init(void *handle)
 	int r;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	gmc_v7_0_init_golden_registers(adev);
+
 	gmc_v7_0_mc_program(adev);
 
 	if (!(adev->flags & AMD_IS_APU)) {
@@ -1012,7 +1055,6 @@ static int gmc_v7_0_suspend(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
 	if (adev->vm_manager.enabled) {
-		amdgpu_vm_manager_fini(adev);
 		gmc_v7_0_vm_fini(adev);
 		adev->vm_manager.enabled = false;
 	}
diff --git a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
index d39028440814..3efd45546241 100644
--- a/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/gmc_v8_0.c
@@ -42,9 +42,7 @@
 static void gmc_v8_0_set_gart_funcs(struct amdgpu_device *adev);
 static void gmc_v8_0_set_irq_funcs(struct amdgpu_device *adev);
 
-MODULE_FIRMWARE("amdgpu/topaz_mc.bin");
 MODULE_FIRMWARE("amdgpu/tonga_mc.bin");
-MODULE_FIRMWARE("amdgpu/fiji_mc.bin");
 
 static const u32 golden_settings_tonga_a11[] =
 {
@@ -75,19 +73,6 @@ static const u32 fiji_mgcg_cgcg_init[] =
 	mmMC_MEM_POWER_LS, 0xffffffff, 0x00000104
 };
 
-static const u32 golden_settings_iceland_a11[] =
-{
-	mmVM_PRT_APERTURE0_LOW_ADDR, 0x0fffffff, 0x0fffffff,
-	mmVM_PRT_APERTURE1_LOW_ADDR, 0x0fffffff, 0x0fffffff,
-	mmVM_PRT_APERTURE2_LOW_ADDR, 0x0fffffff, 0x0fffffff,
-	mmVM_PRT_APERTURE3_LOW_ADDR, 0x0fffffff, 0x0fffffff
-};
-
-static const u32 iceland_mgcg_cgcg_init[] =
-{
-	mmMC_MEM_POWER_LS, 0xffffffff, 0x00000104
-};
-
 static const u32 cz_mgcg_cgcg_init[] =
 {
 	mmMC_MEM_POWER_LS, 0xffffffff, 0x00000104
@@ -102,14 +87,6 @@ static const u32 stoney_mgcg_cgcg_init[] =
 static void gmc_v8_0_init_golden_registers(struct amdgpu_device *adev)
 {
 	switch (adev->asic_type) {
-	case CHIP_TOPAZ:
-		amdgpu_program_register_sequence(adev,
-						 iceland_mgcg_cgcg_init,
-						 (const u32)ARRAY_SIZE(iceland_mgcg_cgcg_init));
-		amdgpu_program_register_sequence(adev,
-						 golden_settings_iceland_a11,
-						 (const u32)ARRAY_SIZE(golden_settings_iceland_a11));
-		break;
 	case CHIP_FIJI:
 		amdgpu_program_register_sequence(adev,
 						 fiji_mgcg_cgcg_init,
@@ -229,15 +206,10 @@ static int gmc_v8_0_init_microcode(struct amdgpu_device *adev)
 	DRM_DEBUG("\n");
 
 	switch (adev->asic_type) {
-	case CHIP_TOPAZ:
-		chip_name = "topaz";
-		break;
 	case CHIP_TONGA:
 		chip_name = "tonga";
 		break;
 	case CHIP_FIJI:
-		chip_name = "fiji";
-		break;
 	case CHIP_CARRIZO:
 	case CHIP_STONEY:
 		return 0;
@@ -476,6 +448,10 @@ static int gmc_v8_0_mc_init(struct amdgpu_device *adev)
 	adev->mc.real_vram_size = RREG32(mmCONFIG_MEMSIZE) * 1024ULL * 1024ULL;
 	adev->mc.visible_vram_size = adev->mc.aper_size;
 
+	/* In case the PCI BAR is larger than the actual amount of vram */
+	if (adev->mc.visible_vram_size > adev->mc.real_vram_size)
+		adev->mc.visible_vram_size = adev->mc.real_vram_size;
+
 	/* unless the user had overridden it, set the gart
 	 * size equal to the 1024 or vram, whichever is larger.
 	 */
@@ -1003,7 +979,7 @@ static int gmc_v8_0_hw_init(void *handle)
 
 	gmc_v8_0_mc_program(adev);
 
-	if (!(adev->flags & AMD_IS_APU)) {
+	if (adev->asic_type == CHIP_TONGA) {
 		r = gmc_v8_0_mc_load_microcode(adev);
 		if (r) {
 			DRM_ERROR("Failed to load MC firmware!\n");
@@ -1033,7 +1009,6 @@ static int gmc_v8_0_suspend(void *handle)
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
 	if (adev->vm_manager.enabled) {
-		amdgpu_vm_manager_fini(adev);
 		gmc_v8_0_vm_fini(adev);
 		adev->vm_manager.enabled = false;
 	}
@@ -1324,9 +1299,181 @@ static int gmc_v8_0_process_interrupt(struct amdgpu_device *adev,
 	return 0;
 }
 
+static void fiji_update_mc_medium_grain_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t data;
+
+	if (enable) {
+		data = RREG32(mmMC_HUB_MISC_HUB_CG);
+		data |= MC_HUB_MISC_HUB_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_HUB_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_SIP_CG);
+		data |= MC_HUB_MISC_SIP_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_SIP_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_VM_CG);
+		data |= MC_HUB_MISC_VM_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_VM_CG, data);
+
+		data = RREG32(mmMC_XPB_CLK_GAT);
+		data |= MC_XPB_CLK_GAT__ENABLE_MASK;
+		WREG32(mmMC_XPB_CLK_GAT, data);
+
+		data = RREG32(mmATC_MISC_CG);
+		data |= ATC_MISC_CG__ENABLE_MASK;
+		WREG32(mmATC_MISC_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_WR_CG);
+		data |= MC_CITF_MISC_WR_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_WR_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_RD_CG);
+		data |= MC_CITF_MISC_RD_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_RD_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_VM_CG);
+		data |= MC_CITF_MISC_VM_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_VM_CG, data);
+
+		data = RREG32(mmVM_L2_CG);
+		data |= VM_L2_CG__ENABLE_MASK;
+		WREG32(mmVM_L2_CG, data);
+	} else {
+		data = RREG32(mmMC_HUB_MISC_HUB_CG);
+		data &= ~MC_HUB_MISC_HUB_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_HUB_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_SIP_CG);
+		data &= ~MC_HUB_MISC_SIP_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_SIP_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_VM_CG);
+		data &= ~MC_HUB_MISC_VM_CG__ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_VM_CG, data);
+
+		data = RREG32(mmMC_XPB_CLK_GAT);
+		data &= ~MC_XPB_CLK_GAT__ENABLE_MASK;
+		WREG32(mmMC_XPB_CLK_GAT, data);
+
+		data = RREG32(mmATC_MISC_CG);
+		data &= ~ATC_MISC_CG__ENABLE_MASK;
+		WREG32(mmATC_MISC_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_WR_CG);
+		data &= ~MC_CITF_MISC_WR_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_WR_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_RD_CG);
+		data &= ~MC_CITF_MISC_RD_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_RD_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_VM_CG);
+		data &= ~MC_CITF_MISC_VM_CG__ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_VM_CG, data);
+
+		data = RREG32(mmVM_L2_CG);
+		data &= ~VM_L2_CG__ENABLE_MASK;
+		WREG32(mmVM_L2_CG, data);
+	}
+}
+
+static void fiji_update_mc_light_sleep(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t data;
+
+	if (enable) {
+		data = RREG32(mmMC_HUB_MISC_HUB_CG);
+		data |= MC_HUB_MISC_HUB_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_HUB_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_SIP_CG);
+		data |= MC_HUB_MISC_SIP_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_SIP_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_VM_CG);
+		data |= MC_HUB_MISC_VM_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_VM_CG, data);
+
+		data = RREG32(mmMC_XPB_CLK_GAT);
+		data |= MC_XPB_CLK_GAT__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_XPB_CLK_GAT, data);
+
+		data = RREG32(mmATC_MISC_CG);
+		data |= ATC_MISC_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmATC_MISC_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_WR_CG);
+		data |= MC_CITF_MISC_WR_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_WR_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_RD_CG);
+		data |= MC_CITF_MISC_RD_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_RD_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_VM_CG);
+		data |= MC_CITF_MISC_VM_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_VM_CG, data);
+
+		data = RREG32(mmVM_L2_CG);
+		data |= VM_L2_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmVM_L2_CG, data);
+	} else {
+		data = RREG32(mmMC_HUB_MISC_HUB_CG);
+		data &= ~MC_HUB_MISC_HUB_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_HUB_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_SIP_CG);
+		data &= ~MC_HUB_MISC_SIP_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_SIP_CG, data);
+
+		data = RREG32(mmMC_HUB_MISC_VM_CG);
+		data &= ~MC_HUB_MISC_VM_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_HUB_MISC_VM_CG, data);
+
+		data = RREG32(mmMC_XPB_CLK_GAT);
+		data &= ~MC_XPB_CLK_GAT__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_XPB_CLK_GAT, data);
+
+		data = RREG32(mmATC_MISC_CG);
+		data &= ~ATC_MISC_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmATC_MISC_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_WR_CG);
+		data &= ~MC_CITF_MISC_WR_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_WR_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_RD_CG);
+		data &= ~MC_CITF_MISC_RD_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_RD_CG, data);
+
+		data = RREG32(mmMC_CITF_MISC_VM_CG);
+		data &= ~MC_CITF_MISC_VM_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmMC_CITF_MISC_VM_CG, data);
+
+		data = RREG32(mmVM_L2_CG);
+		data &= ~VM_L2_CG__MEM_LS_ENABLE_MASK;
+		WREG32(mmVM_L2_CG, data);
+	}
+}
+
 static int gmc_v8_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	switch (adev->asic_type) {
+	case CHIP_FIJI:
+		fiji_update_mc_medium_grain_clock_gating(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		fiji_update_mc_light_sleep(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		break;
+	default:
+		break;
+	}
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/iceland_ih.c b/drivers/gpu/drm/amd/amdgpu/iceland_ih.c
index 779532d350ff..679e7394a495 100644
--- a/drivers/gpu/drm/amd/amdgpu/iceland_ih.c
+++ b/drivers/gpu/drm/amd/amdgpu/iceland_ih.c
@@ -253,8 +253,14 @@ static void iceland_ih_set_rptr(struct amdgpu_device *adev)
 static int iceland_ih_early_init(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int ret;
+
+	ret = amdgpu_irq_add_domain(adev);
+	if (ret)
+		return ret;
 
 	iceland_ih_set_interrupt_funcs(adev);
+
 	return 0;
 }
 
@@ -278,6 +284,7 @@ static int iceland_ih_sw_fini(void *handle)
 
 	amdgpu_irq_fini(adev);
 	amdgpu_ih_ring_fini(adev);
+	amdgpu_irq_remove_domain(adev);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/iceland_smc.c b/drivers/gpu/drm/amd/amdgpu/iceland_smc.c
index 966d4b2ed9da..090486c18249 100644
--- a/drivers/gpu/drm/amd/amdgpu/iceland_smc.c
+++ b/drivers/gpu/drm/amd/amdgpu/iceland_smc.c
@@ -432,7 +432,7 @@ static uint32_t iceland_smu_get_mask_for_fw_type(uint32_t fw_type)
 		case AMDGPU_UCODE_ID_CP_ME:
 			return UCODE_ID_CP_ME_MASK;
 		case AMDGPU_UCODE_ID_CP_MEC1:
-			return UCODE_ID_CP_MEC_MASK | UCODE_ID_CP_MEC_JT1_MASK | UCODE_ID_CP_MEC_JT2_MASK;
+			return UCODE_ID_CP_MEC_MASK | UCODE_ID_CP_MEC_JT1_MASK;
 		case AMDGPU_UCODE_ID_CP_MEC2:
 			return UCODE_ID_CP_MEC_MASK;
 		case AMDGPU_UCODE_ID_RLC_G:
@@ -522,12 +522,6 @@ static int iceland_smu_request_load_fw(struct amdgpu_device *adev)
 		return -EINVAL;
 	}
 
-	if (iceland_smu_populate_single_firmware_entry(adev, UCODE_ID_CP_MEC_JT2,
-			&toc->entry[toc->num_entries++])) {
-		DRM_ERROR("Failed to get firmware entry for MEC_JT2\n");
-		return -EINVAL;
-	}
-
 	if (iceland_smu_populate_single_firmware_entry(adev, UCODE_ID_SDMA0,
 			&toc->entry[toc->num_entries++])) {
 		DRM_ERROR("Failed to get firmware entry for SDMA0\n");
@@ -550,8 +544,8 @@ static int iceland_smu_request_load_fw(struct amdgpu_device *adev)
 			UCODE_ID_CP_ME_MASK |
 			UCODE_ID_CP_PFP_MASK |
 			UCODE_ID_CP_MEC_MASK |
-			UCODE_ID_CP_MEC_JT1_MASK |
-			UCODE_ID_CP_MEC_JT2_MASK;
+			UCODE_ID_CP_MEC_JT1_MASK;
+
 
 	if (iceland_send_msg_to_smc_with_parameter_without_waiting(adev, PPSMC_MSG_LoadUcodes, fw_to_load)) {
 		DRM_ERROR("Fail to request SMU load ucode\n");
diff --git a/drivers/gpu/drm/amd/amdgpu/kv_dpm.c b/drivers/gpu/drm/amd/amdgpu/kv_dpm.c
index 7e9154c7f1db..654d76723bc3 100644
--- a/drivers/gpu/drm/amd/amdgpu/kv_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/kv_dpm.c
@@ -2859,11 +2859,11 @@ static int kv_dpm_init(struct amdgpu_device *adev)
 	pi->voltage_drop_t = 0;
 	pi->caps_sclk_throttle_low_notification = false;
 	pi->caps_fps = false; /* true? */
-	pi->caps_uvd_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_UVD) ? true : false;
+	pi->caps_uvd_pg = (adev->pg_flags & AMD_PG_SUPPORT_UVD) ? true : false;
 	pi->caps_uvd_dpm = true;
-	pi->caps_vce_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_VCE) ? true : false;
-	pi->caps_samu_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_SAMU) ? true : false;
-	pi->caps_acp_pg = (adev->pg_flags & AMDGPU_PG_SUPPORT_ACP) ? true : false;
+	pi->caps_vce_pg = (adev->pg_flags & AMD_PG_SUPPORT_VCE) ? true : false;
+	pi->caps_samu_pg = (adev->pg_flags & AMD_PG_SUPPORT_SAMU) ? true : false;
+	pi->caps_acp_pg = (adev->pg_flags & AMD_PG_SUPPORT_ACP) ? true : false;
 	pi->caps_stable_p_state = false;
 
 	ret = kv_parse_sys_info_table(adev);
diff --git a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
index 7253132f04b8..ad54c46751b0 100644
--- a/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/sdma_v3_0.c
@@ -727,18 +727,20 @@ static int sdma_v3_0_start(struct amdgpu_device *adev)
 {
 	int r, i;
 
-	if (!adev->firmware.smu_load) {
-		r = sdma_v3_0_load_microcode(adev);
-		if (r)
-			return r;
-	} else {
-		for (i = 0; i < adev->sdma.num_instances; i++) {
-			r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
-									 (i == 0) ?
-									 AMDGPU_UCODE_ID_SDMA0 :
-									 AMDGPU_UCODE_ID_SDMA1);
+	if (!adev->pp_enabled) {
+		if (!adev->firmware.smu_load) {
+			r = sdma_v3_0_load_microcode(adev);
 			if (r)
-				return -EINVAL;
+				return r;
+		} else {
+			for (i = 0; i < adev->sdma.num_instances; i++) {
+				r = adev->smu.smumgr_funcs->check_fw_load_finish(adev,
+										 (i == 0) ?
+										 AMDGPU_UCODE_ID_SDMA0 :
+										 AMDGPU_UCODE_ID_SDMA1);
+				if (r)
+					return -EINVAL;
+			}
 		}
 	}
 
@@ -1427,9 +1429,114 @@ static int sdma_v3_0_process_illegal_inst_irq(struct amdgpu_device *adev,
 	return 0;
 }
 
+static void fiji_update_sdma_medium_grain_clock_gating(
+		struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	if (enable) {
+		temp = data = RREG32(mmSDMA0_CLK_CTRL);
+		data &= ~(SDMA0_CLK_CTRL__SOFT_OVERRIDE7_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE6_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE5_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE4_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE3_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE2_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE1_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE0_MASK);
+		if (data != temp)
+			WREG32(mmSDMA0_CLK_CTRL, data);
+
+		temp = data = RREG32(mmSDMA1_CLK_CTRL);
+		data &= ~(SDMA1_CLK_CTRL__SOFT_OVERRIDE7_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE6_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE5_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE4_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE3_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE2_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE1_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE0_MASK);
+
+		if (data != temp)
+			WREG32(mmSDMA1_CLK_CTRL, data);
+	} else {
+		temp = data = RREG32(mmSDMA0_CLK_CTRL);
+		data |= SDMA0_CLK_CTRL__SOFT_OVERRIDE7_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE6_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE5_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE4_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE3_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE2_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE1_MASK |
+				SDMA0_CLK_CTRL__SOFT_OVERRIDE0_MASK;
+
+		if (data != temp)
+			WREG32(mmSDMA0_CLK_CTRL, data);
+
+		temp = data = RREG32(mmSDMA1_CLK_CTRL);
+		data |= SDMA1_CLK_CTRL__SOFT_OVERRIDE7_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE6_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE5_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE4_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE3_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE2_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE1_MASK |
+				SDMA1_CLK_CTRL__SOFT_OVERRIDE0_MASK;
+
+		if (data != temp)
+			WREG32(mmSDMA1_CLK_CTRL, data);
+	}
+}
+
+static void fiji_update_sdma_medium_grain_light_sleep(
+		struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	if (enable) {
+		temp = data = RREG32(mmSDMA0_POWER_CNTL);
+		data |= SDMA0_POWER_CNTL__MEM_POWER_OVERRIDE_MASK;
+
+		if (temp != data)
+			WREG32(mmSDMA0_POWER_CNTL, data);
+
+		temp = data = RREG32(mmSDMA1_POWER_CNTL);
+		data |= SDMA1_POWER_CNTL__MEM_POWER_OVERRIDE_MASK;
+
+		if (temp != data)
+			WREG32(mmSDMA1_POWER_CNTL, data);
+	} else {
+		temp = data = RREG32(mmSDMA0_POWER_CNTL);
+		data &= ~SDMA0_POWER_CNTL__MEM_POWER_OVERRIDE_MASK;
+
+		if (temp != data)
+			WREG32(mmSDMA0_POWER_CNTL, data);
+
+		temp = data = RREG32(mmSDMA1_POWER_CNTL);
+		data &= ~SDMA1_POWER_CNTL__MEM_POWER_OVERRIDE_MASK;
+
+		if (temp != data)
+			WREG32(mmSDMA1_POWER_CNTL, data);
+	}
+}
+
 static int sdma_v3_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	switch (adev->asic_type) {
+	case CHIP_FIJI:
+		fiji_update_sdma_medium_grain_clock_gating(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		fiji_update_sdma_medium_grain_light_sleep(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		break;
+	default:
+		break;
+	}
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_dpm.c b/drivers/gpu/drm/amd/amdgpu/tonga_dpm.c
index 204903897b4f..0497784b3652 100644
--- a/drivers/gpu/drm/amd/amdgpu/tonga_dpm.c
+++ b/drivers/gpu/drm/amd/amdgpu/tonga_dpm.c
@@ -24,7 +24,7 @@
 #include <linux/firmware.h>
 #include "drmP.h"
 #include "amdgpu.h"
-#include "tonga_smumgr.h"
+#include "tonga_smum.h"
 
 MODULE_FIRMWARE("amdgpu/tonga_smc.bin");
 
@@ -122,25 +122,12 @@ static int tonga_dpm_hw_fini(void *handle)
 
 static int tonga_dpm_suspend(void *handle)
 {
-	return 0;
+	return tonga_dpm_hw_fini(handle);
 }
 
 static int tonga_dpm_resume(void *handle)
 {
-	int ret;
-	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
-
-	mutex_lock(&adev->pm.mutex);
-
-	ret = tonga_smu_start(adev);
-	if (ret) {
-		DRM_ERROR("SMU start failed\n");
-		goto fail;
-	}
-
-fail:
-	mutex_unlock(&adev->pm.mutex);
-	return ret;
+	return tonga_dpm_hw_init(handle);
 }
 
 static int tonga_dpm_set_clockgating_state(void *handle,
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_ih.c b/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
index 743c372837aa..b6f7d7bff929 100644
--- a/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
+++ b/drivers/gpu/drm/amd/amdgpu/tonga_ih.c
@@ -273,8 +273,14 @@ static void tonga_ih_set_rptr(struct amdgpu_device *adev)
 static int tonga_ih_early_init(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	int ret;
+
+	ret = amdgpu_irq_add_domain(adev);
+	if (ret)
+		return ret;
 
 	tonga_ih_set_interrupt_funcs(adev);
+
 	return 0;
 }
 
@@ -301,6 +307,7 @@ static int tonga_ih_sw_fini(void *handle)
 
 	amdgpu_irq_fini(adev);
 	amdgpu_ih_ring_fini(adev);
+	amdgpu_irq_add_domain(adev);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_ppsmc.h b/drivers/gpu/drm/amd/amdgpu/tonga_ppsmc.h
deleted file mode 100644
index 811781f69482..000000000000
--- a/drivers/gpu/drm/amd/amdgpu/tonga_ppsmc.h
+++ /dev/null
@@ -1,198 +0,0 @@
-/*
- * Copyright 2014 Advanced Micro Devices, Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- */
-
-#ifndef TONGA_PP_SMC_H
-#define TONGA_PP_SMC_H
-
-#pragma pack(push, 1)
-
-#define PPSMC_SWSTATE_FLAG_DC                           0x01
-#define PPSMC_SWSTATE_FLAG_UVD                          0x02
-#define PPSMC_SWSTATE_FLAG_VCE                          0x04
-#define PPSMC_SWSTATE_FLAG_PCIE_X1                      0x08
-
-#define PPSMC_THERMAL_PROTECT_TYPE_INTERNAL             0x00
-#define PPSMC_THERMAL_PROTECT_TYPE_EXTERNAL             0x01
-#define PPSMC_THERMAL_PROTECT_TYPE_NONE                 0xff 
-
-#define PPSMC_SYSTEMFLAG_GPIO_DC                        0x01 
-#define PPSMC_SYSTEMFLAG_STEPVDDC                       0x02 
-#define PPSMC_SYSTEMFLAG_GDDR5                          0x04 
-
-#define PPSMC_SYSTEMFLAG_DISABLE_BABYSTEP               0x08 
-
-#define PPSMC_SYSTEMFLAG_REGULATOR_HOT                  0x10 
-#define PPSMC_SYSTEMFLAG_REGULATOR_HOT_ANALOG           0x20 
-#define PPSMC_SYSTEMFLAG_12CHANNEL                      0x40
-
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_MASK              0x07
-#define PPSMC_EXTRAFLAGS_AC2DC_DONT_WAIT_FOR_VBLANK     0x08
-
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTODPMLOWSTATE   0x00
-#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTOINITIALSTATE  0x01
-
-#define PPSMC_EXTRAFLAGS_AC2DC_GPIO5_POLARITY_HIGH      0x10
-#define PPSMC_EXTRAFLAGS_DRIVER_TO_GPIO17               0x20
-#define PPSMC_EXTRAFLAGS_PCC_TO_GPIO17                  0x40
-
-#define PPSMC_DPM2FLAGS_TDPCLMP                         0x01 
-#define PPSMC_DPM2FLAGS_PWRSHFT                         0x02 
-#define PPSMC_DPM2FLAGS_OCP                             0x04 
-
-#define PPSMC_DISPLAY_WATERMARK_LOW                     0
-#define PPSMC_DISPLAY_WATERMARK_HIGH                    1
-
-#define PPSMC_STATEFLAG_AUTO_PULSE_SKIP    		0x01
-#define PPSMC_STATEFLAG_POWERBOOST         		0x02
-#define PPSMC_STATEFLAG_PSKIP_ON_TDP_FAULT 		0x04
-#define PPSMC_STATEFLAG_POWERSHIFT         		0x08
-#define PPSMC_STATEFLAG_SLOW_READ_MARGIN   		0x10
-#define PPSMC_STATEFLAG_DEEPSLEEP_THROTTLE 		0x20
-#define PPSMC_STATEFLAG_DEEPSLEEP_BYPASS   		0x40
-
-#define FDO_MODE_HARDWARE 0
-#define FDO_MODE_PIECE_WISE_LINEAR 1
-
-enum FAN_CONTROL {
-	FAN_CONTROL_FUZZY,
-	FAN_CONTROL_TABLE
-};
-
-#define PPSMC_Result_OK             			((uint16_t)0x01)
-#define PPSMC_Result_NoMore         			((uint16_t)0x02)
-#define PPSMC_Result_NotNow         			((uint16_t)0x03)
-#define PPSMC_Result_Failed         			((uint16_t)0xFF) 
-#define PPSMC_Result_UnknownCmd     			((uint16_t)0xFE) 
-#define PPSMC_Result_UnknownVT      			((uint16_t)0xFD) 
-
-typedef uint16_t PPSMC_Result;
-
-#define PPSMC_isERROR(x) ((uint16_t)0x80 & (x))
-
-#define PPSMC_MSG_Halt                      		((uint16_t)0x10)
-#define PPSMC_MSG_Resume                    		((uint16_t)0x11)
-#define PPSMC_MSG_EnableDPMLevel            		((uint16_t)0x12)
-#define PPSMC_MSG_ZeroLevelsDisabled        		((uint16_t)0x13)
-#define PPSMC_MSG_OneLevelsDisabled         		((uint16_t)0x14)
-#define PPSMC_MSG_TwoLevelsDisabled         		((uint16_t)0x15)
-#define PPSMC_MSG_EnableThermalInterrupt    		((uint16_t)0x16)
-#define PPSMC_MSG_RunningOnAC               		((uint16_t)0x17)
-#define PPSMC_MSG_LevelUp                   		((uint16_t)0x18)
-#define PPSMC_MSG_LevelDown                 		((uint16_t)0x19)
-#define PPSMC_MSG_ResetDPMCounters          		((uint16_t)0x1a)
-#define PPSMC_MSG_SwitchToSwState           		((uint16_t)0x20)
-#define PPSMC_MSG_SwitchToSwStateLast       		((uint16_t)0x3f)
-#define PPSMC_MSG_SwitchToInitialState      		((uint16_t)0x40)
-#define PPSMC_MSG_NoForcedLevel             		((uint16_t)0x41)
-#define PPSMC_MSG_ForceHigh                 		((uint16_t)0x42)
-#define PPSMC_MSG_ForceMediumOrHigh         		((uint16_t)0x43)
-#define PPSMC_MSG_SwitchToMinimumPower      		((uint16_t)0x51)
-#define PPSMC_MSG_ResumeFromMinimumPower    		((uint16_t)0x52)
-#define PPSMC_MSG_EnableCac                 		((uint16_t)0x53)
-#define PPSMC_MSG_DisableCac                		((uint16_t)0x54)
-#define PPSMC_DPMStateHistoryStart          		((uint16_t)0x55)
-#define PPSMC_DPMStateHistoryStop           		((uint16_t)0x56)
-#define PPSMC_CACHistoryStart               		((uint16_t)0x57)
-#define PPSMC_CACHistoryStop                		((uint16_t)0x58)
-#define PPSMC_TDPClampingActive             		((uint16_t)0x59)
-#define PPSMC_TDPClampingInactive           		((uint16_t)0x5A)
-#define PPSMC_StartFanControl               		((uint16_t)0x5B)
-#define PPSMC_StopFanControl                		((uint16_t)0x5C)
-#define PPSMC_NoDisplay                     		((uint16_t)0x5D)
-#define PPSMC_HasDisplay                    		((uint16_t)0x5E)
-#define PPSMC_MSG_UVDPowerOFF               		((uint16_t)0x60)
-#define PPSMC_MSG_UVDPowerON                		((uint16_t)0x61)
-#define PPSMC_MSG_EnableULV                 		((uint16_t)0x62)
-#define PPSMC_MSG_DisableULV                		((uint16_t)0x63)
-#define PPSMC_MSG_EnterULV                  		((uint16_t)0x64)
-#define PPSMC_MSG_ExitULV                   		((uint16_t)0x65)
-#define PPSMC_PowerShiftActive              		((uint16_t)0x6A)
-#define PPSMC_PowerShiftInactive            		((uint16_t)0x6B)
-#define PPSMC_OCPActive                     		((uint16_t)0x6C)
-#define PPSMC_OCPInactive                   		((uint16_t)0x6D)
-#define PPSMC_CACLongTermAvgEnable          		((uint16_t)0x6E)
-#define PPSMC_CACLongTermAvgDisable         		((uint16_t)0x6F)
-#define PPSMC_MSG_InferredStateSweep_Start  		((uint16_t)0x70)
-#define PPSMC_MSG_InferredStateSweep_Stop   		((uint16_t)0x71)
-#define PPSMC_MSG_SwitchToLowestInfState    		((uint16_t)0x72)
-#define PPSMC_MSG_SwitchToNonInfState       		((uint16_t)0x73)
-#define PPSMC_MSG_AllStateSweep_Start       		((uint16_t)0x74)
-#define PPSMC_MSG_AllStateSweep_Stop        		((uint16_t)0x75)
-#define PPSMC_MSG_SwitchNextLowerInfState   		((uint16_t)0x76)
-#define PPSMC_MSG_SwitchNextHigherInfState  		((uint16_t)0x77)
-#define PPSMC_MSG_MclkRetrainingTest        		((uint16_t)0x78)
-#define PPSMC_MSG_ForceTDPClamping          		((uint16_t)0x79)
-#define PPSMC_MSG_CollectCAC_PowerCorreln   		((uint16_t)0x7A)
-#define PPSMC_MSG_CollectCAC_WeightCalib    		((uint16_t)0x7B)
-#define PPSMC_MSG_CollectCAC_SQonly         		((uint16_t)0x7C)
-#define PPSMC_MSG_CollectCAC_TemperaturePwr 		((uint16_t)0x7D)
-#define PPSMC_MSG_ExtremitiesTest_Start     		((uint16_t)0x7E)
-#define PPSMC_MSG_ExtremitiesTest_Stop      		((uint16_t)0x7F)
-#define PPSMC_FlushDataCache                		((uint16_t)0x80)
-#define PPSMC_FlushInstrCache               		((uint16_t)0x81)
-#define PPSMC_MSG_SetEnabledLevels          		((uint16_t)0x82)
-#define PPSMC_MSG_SetForcedLevels           		((uint16_t)0x83)
-#define PPSMC_MSG_ResetToDefaults           		((uint16_t)0x84)
-#define PPSMC_MSG_SetForcedLevelsAndJump    		((uint16_t)0x85)
-#define PPSMC_MSG_SetCACHistoryMode         		((uint16_t)0x86)
-#define PPSMC_MSG_EnableDTE                 		((uint16_t)0x87)
-#define PPSMC_MSG_DisableDTE                		((uint16_t)0x88)
-#define PPSMC_MSG_SmcSpaceSetAddress        		((uint16_t)0x89)
-#define PPSMC_MSG_SmcSpaceWriteDWordInc     		((uint16_t)0x8A)
-#define PPSMC_MSG_SmcSpaceWriteWordInc      		((uint16_t)0x8B)
-#define PPSMC_MSG_SmcSpaceWriteByteInc      		((uint16_t)0x8C)
-#define PPSMC_MSG_ChangeNearTDPLimit        		((uint16_t)0x90)
-#define PPSMC_MSG_ChangeSafePowerLimit      		((uint16_t)0x91)
-#define PPSMC_MSG_DPMStateSweepStart        		((uint16_t)0x92)
-#define PPSMC_MSG_DPMStateSweepStop         		((uint16_t)0x93)
-#define PPSMC_MSG_OVRDDisableSCLKDS         		((uint16_t)0x94)
-#define PPSMC_MSG_CancelDisableOVRDSCLKDS   		((uint16_t)0x95)
-#define PPSMC_MSG_ThrottleOVRDSCLKDS        		((uint16_t)0x96)
-#define PPSMC_MSG_CancelThrottleOVRDSCLKDS  		((uint16_t)0x97)
-#define PPSMC_MSG_GPIO17                    		((uint16_t)0x98)
-#define PPSMC_MSG_API_SetSvi2Volt_Vddc      		((uint16_t)0x99)
-#define PPSMC_MSG_API_SetSvi2Volt_Vddci     		((uint16_t)0x9A)
-#define PPSMC_MSG_API_SetSvi2Volt_Mvdd      		((uint16_t)0x9B)
-#define PPSMC_MSG_API_GetSvi2Volt_Vddc      		((uint16_t)0x9C)
-#define PPSMC_MSG_API_GetSvi2Volt_Vddci     		((uint16_t)0x9D)
-#define PPSMC_MSG_API_GetSvi2Volt_Mvdd      		((uint16_t)0x9E)
-
-#define PPSMC_MSG_BREAK                     		((uint16_t)0xF8)
-
-#define PPSMC_MSG_Test                      		((uint16_t)0x100)
-#define PPSMC_MSG_DRV_DRAM_ADDR_HI            		((uint16_t)0x250)
-#define PPSMC_MSG_DRV_DRAM_ADDR_LO            		((uint16_t)0x251)
-#define PPSMC_MSG_SMU_DRAM_ADDR_HI            		((uint16_t)0x252)
-#define PPSMC_MSG_SMU_DRAM_ADDR_LO            		((uint16_t)0x253)
-#define PPSMC_MSG_LoadUcodes                  		((uint16_t)0x254)
-
-typedef uint16_t PPSMC_Msg;
-
-#define PPSMC_EVENT_STATUS_THERMAL          		0x00000001
-#define PPSMC_EVENT_STATUS_REGULATORHOT     		0x00000002
-#define PPSMC_EVENT_STATUS_DC               		0x00000004
-#define PPSMC_EVENT_STATUS_GPIO17           		0x00000008
-
-#pragma pack(pop)
-
-#endif
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_smc.c b/drivers/gpu/drm/amd/amdgpu/tonga_smc.c
index 5421309c1862..361c49a82323 100644
--- a/drivers/gpu/drm/amd/amdgpu/tonga_smc.c
+++ b/drivers/gpu/drm/amd/amdgpu/tonga_smc.c
@@ -25,7 +25,7 @@
 #include "drmP.h"
 #include "amdgpu.h"
 #include "tonga_ppsmc.h"
-#include "tonga_smumgr.h"
+#include "tonga_smum.h"
 #include "smu_ucode_xfer_vi.h"
 #include "amdgpu_ucode.h"
 
diff --git a/drivers/gpu/drm/amd/amdgpu/tonga_smumgr.h b/drivers/gpu/drm/amd/amdgpu/tonga_smum.h
index c031ff99fe3e..c031ff99fe3e 100644
--- a/drivers/gpu/drm/amd/amdgpu/tonga_smumgr.h
+++ b/drivers/gpu/drm/amd/amdgpu/tonga_smum.h
diff --git a/drivers/gpu/drm/amd/amdgpu/uvd_v4_2.c b/drivers/gpu/drm/amd/amdgpu/uvd_v4_2.c
index 5e9f73af83a8..fbd3767671bb 100644
--- a/drivers/gpu/drm/amd/amdgpu/uvd_v4_2.c
+++ b/drivers/gpu/drm/amd/amdgpu/uvd_v4_2.c
@@ -611,7 +611,7 @@ static void uvd_v4_2_enable_mgcg(struct amdgpu_device *adev,
 {
 	u32 orig, data;
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_UVD_MGCG)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_UVD_MGCG)) {
 		data = RREG32_UVD_CTX(ixUVD_CGC_MEM_CTRL);
 		data = 0xfff;
 		WREG32_UVD_CTX(ixUVD_CGC_MEM_CTRL, data);
@@ -830,6 +830,9 @@ static int uvd_v4_2_set_clockgating_state(void *handle,
 	bool gate = false;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	if (!(adev->cg_flags & AMD_CG_SUPPORT_UVD_MGCG))
+		return 0;
+
 	if (state == AMD_CG_STATE_GATE)
 		gate = true;
 
@@ -848,7 +851,10 @@ static int uvd_v4_2_set_powergating_state(void *handle,
 	 * revisit this when there is a cleaner line between
 	 * the smc and the hw blocks
 	 */
-	 struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (!(adev->pg_flags & AMD_PG_SUPPORT_UVD))
+		return 0;
 
 	if (state == AMD_PG_STATE_GATE) {
 		uvd_v4_2_stop(adev);
diff --git a/drivers/gpu/drm/amd/amdgpu/uvd_v5_0.c b/drivers/gpu/drm/amd/amdgpu/uvd_v5_0.c
index 38864f562981..57f1c5bf3bf1 100644
--- a/drivers/gpu/drm/amd/amdgpu/uvd_v5_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/uvd_v5_0.c
@@ -774,6 +774,11 @@ static int uvd_v5_0_process_interrupt(struct amdgpu_device *adev,
 static int uvd_v5_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	if (!(adev->cg_flags & AMD_CG_SUPPORT_UVD_MGCG))
+		return 0;
+
 	return 0;
 }
 
@@ -789,6 +794,9 @@ static int uvd_v5_0_set_powergating_state(void *handle,
 	 */
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	if (!(adev->pg_flags & AMD_PG_SUPPORT_UVD))
+		return 0;
+
 	if (state == AMD_PG_STATE_GATE) {
 		uvd_v5_0_stop(adev);
 		return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c b/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
index 121915bbc3b6..0b365b7651ff 100644
--- a/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/uvd_v6_0.c
@@ -279,6 +279,234 @@ static void uvd_v6_0_mc_resume(struct amdgpu_device *adev)
 	WREG32(mmUVD_VCPU_CACHE_SIZE2, size);
 }
 
+static void cz_set_uvd_clock_gating_branches(struct amdgpu_device *adev,
+		bool enable)
+{
+	u32 data, data1;
+
+	data = RREG32(mmUVD_CGC_GATE);
+	data1 = RREG32(mmUVD_SUVD_CGC_GATE);
+	if (enable) {
+		data |= UVD_CGC_GATE__SYS_MASK |
+				UVD_CGC_GATE__UDEC_MASK |
+				UVD_CGC_GATE__MPEG2_MASK |
+				UVD_CGC_GATE__RBC_MASK |
+				UVD_CGC_GATE__LMI_MC_MASK |
+				UVD_CGC_GATE__IDCT_MASK |
+				UVD_CGC_GATE__MPRD_MASK |
+				UVD_CGC_GATE__MPC_MASK |
+				UVD_CGC_GATE__LBSI_MASK |
+				UVD_CGC_GATE__LRBBM_MASK |
+				UVD_CGC_GATE__UDEC_RE_MASK |
+				UVD_CGC_GATE__UDEC_CM_MASK |
+				UVD_CGC_GATE__UDEC_IT_MASK |
+				UVD_CGC_GATE__UDEC_DB_MASK |
+				UVD_CGC_GATE__UDEC_MP_MASK |
+				UVD_CGC_GATE__WCB_MASK |
+				UVD_CGC_GATE__VCPU_MASK |
+				UVD_CGC_GATE__SCPU_MASK;
+		data1 |= UVD_SUVD_CGC_GATE__SRE_MASK |
+				UVD_SUVD_CGC_GATE__SIT_MASK |
+				UVD_SUVD_CGC_GATE__SMP_MASK |
+				UVD_SUVD_CGC_GATE__SCM_MASK |
+				UVD_SUVD_CGC_GATE__SDB_MASK |
+				UVD_SUVD_CGC_GATE__SRE_H264_MASK |
+				UVD_SUVD_CGC_GATE__SRE_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SIT_H264_MASK |
+				UVD_SUVD_CGC_GATE__SIT_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SCM_H264_MASK |
+				UVD_SUVD_CGC_GATE__SCM_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SDB_H264_MASK |
+				UVD_SUVD_CGC_GATE__SDB_HEVC_MASK;
+	} else {
+		data &= ~(UVD_CGC_GATE__SYS_MASK |
+				UVD_CGC_GATE__UDEC_MASK |
+				UVD_CGC_GATE__MPEG2_MASK |
+				UVD_CGC_GATE__RBC_MASK |
+				UVD_CGC_GATE__LMI_MC_MASK |
+				UVD_CGC_GATE__LMI_UMC_MASK |
+				UVD_CGC_GATE__IDCT_MASK |
+				UVD_CGC_GATE__MPRD_MASK |
+				UVD_CGC_GATE__MPC_MASK |
+				UVD_CGC_GATE__LBSI_MASK |
+				UVD_CGC_GATE__LRBBM_MASK |
+				UVD_CGC_GATE__UDEC_RE_MASK |
+				UVD_CGC_GATE__UDEC_CM_MASK |
+				UVD_CGC_GATE__UDEC_IT_MASK |
+				UVD_CGC_GATE__UDEC_DB_MASK |
+				UVD_CGC_GATE__UDEC_MP_MASK |
+				UVD_CGC_GATE__WCB_MASK |
+				UVD_CGC_GATE__VCPU_MASK |
+				UVD_CGC_GATE__SCPU_MASK);
+		data1 &= ~(UVD_SUVD_CGC_GATE__SRE_MASK |
+				UVD_SUVD_CGC_GATE__SIT_MASK |
+				UVD_SUVD_CGC_GATE__SMP_MASK |
+				UVD_SUVD_CGC_GATE__SCM_MASK |
+				UVD_SUVD_CGC_GATE__SDB_MASK |
+				UVD_SUVD_CGC_GATE__SRE_H264_MASK |
+				UVD_SUVD_CGC_GATE__SRE_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SIT_H264_MASK |
+				UVD_SUVD_CGC_GATE__SIT_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SCM_H264_MASK |
+				UVD_SUVD_CGC_GATE__SCM_HEVC_MASK |
+				UVD_SUVD_CGC_GATE__SDB_H264_MASK |
+				UVD_SUVD_CGC_GATE__SDB_HEVC_MASK);
+	}
+	WREG32(mmUVD_CGC_GATE, data);
+	WREG32(mmUVD_SUVD_CGC_GATE, data1);
+}
+
+static void tonga_set_uvd_clock_gating_branches(struct amdgpu_device *adev,
+		bool enable)
+{
+	u32 data, data1;
+
+	data = RREG32(mmUVD_CGC_GATE);
+	data1 = RREG32(mmUVD_SUVD_CGC_GATE);
+	if (enable) {
+		data |= UVD_CGC_GATE__SYS_MASK |
+				UVD_CGC_GATE__UDEC_MASK |
+				UVD_CGC_GATE__MPEG2_MASK |
+				UVD_CGC_GATE__RBC_MASK |
+				UVD_CGC_GATE__LMI_MC_MASK |
+				UVD_CGC_GATE__IDCT_MASK |
+				UVD_CGC_GATE__MPRD_MASK |
+				UVD_CGC_GATE__MPC_MASK |
+				UVD_CGC_GATE__LBSI_MASK |
+				UVD_CGC_GATE__LRBBM_MASK |
+				UVD_CGC_GATE__UDEC_RE_MASK |
+				UVD_CGC_GATE__UDEC_CM_MASK |
+				UVD_CGC_GATE__UDEC_IT_MASK |
+				UVD_CGC_GATE__UDEC_DB_MASK |
+				UVD_CGC_GATE__UDEC_MP_MASK |
+				UVD_CGC_GATE__WCB_MASK |
+				UVD_CGC_GATE__VCPU_MASK |
+				UVD_CGC_GATE__SCPU_MASK;
+		data1 |= UVD_SUVD_CGC_GATE__SRE_MASK |
+				UVD_SUVD_CGC_GATE__SIT_MASK |
+				UVD_SUVD_CGC_GATE__SMP_MASK |
+				UVD_SUVD_CGC_GATE__SCM_MASK |
+				UVD_SUVD_CGC_GATE__SDB_MASK;
+	} else {
+		data &= ~(UVD_CGC_GATE__SYS_MASK |
+				UVD_CGC_GATE__UDEC_MASK |
+				UVD_CGC_GATE__MPEG2_MASK |
+				UVD_CGC_GATE__RBC_MASK |
+				UVD_CGC_GATE__LMI_MC_MASK |
+				UVD_CGC_GATE__LMI_UMC_MASK |
+				UVD_CGC_GATE__IDCT_MASK |
+				UVD_CGC_GATE__MPRD_MASK |
+				UVD_CGC_GATE__MPC_MASK |
+				UVD_CGC_GATE__LBSI_MASK |
+				UVD_CGC_GATE__LRBBM_MASK |
+				UVD_CGC_GATE__UDEC_RE_MASK |
+				UVD_CGC_GATE__UDEC_CM_MASK |
+				UVD_CGC_GATE__UDEC_IT_MASK |
+				UVD_CGC_GATE__UDEC_DB_MASK |
+				UVD_CGC_GATE__UDEC_MP_MASK |
+				UVD_CGC_GATE__WCB_MASK |
+				UVD_CGC_GATE__VCPU_MASK |
+				UVD_CGC_GATE__SCPU_MASK);
+		data1 &= ~(UVD_SUVD_CGC_GATE__SRE_MASK |
+				UVD_SUVD_CGC_GATE__SIT_MASK |
+				UVD_SUVD_CGC_GATE__SMP_MASK |
+				UVD_SUVD_CGC_GATE__SCM_MASK |
+				UVD_SUVD_CGC_GATE__SDB_MASK);
+	}
+	WREG32(mmUVD_CGC_GATE, data);
+	WREG32(mmUVD_SUVD_CGC_GATE, data1);
+}
+
+static void uvd_v6_0_set_uvd_dynamic_clock_mode(struct amdgpu_device *adev,
+		bool swmode)
+{
+	u32 data, data1 = 0, data2;
+
+	/* Always un-gate UVD REGS bit */
+	data = RREG32(mmUVD_CGC_GATE);
+	data &= ~(UVD_CGC_GATE__REGS_MASK);
+	WREG32(mmUVD_CGC_GATE, data);
+
+	data = RREG32(mmUVD_CGC_CTRL);
+	data &= ~(UVD_CGC_CTRL__CLK_OFF_DELAY_MASK |
+			UVD_CGC_CTRL__CLK_GATE_DLY_TIMER_MASK);
+	data |= UVD_CGC_CTRL__DYN_CLOCK_MODE_MASK |
+			1 << REG_FIELD_SHIFT(UVD_CGC_CTRL, CLK_GATE_DLY_TIMER) |
+			4 << REG_FIELD_SHIFT(UVD_CGC_CTRL, CLK_OFF_DELAY);
+
+	data2 = RREG32(mmUVD_SUVD_CGC_CTRL);
+	if (swmode) {
+		data &= ~(UVD_CGC_CTRL__UDEC_RE_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_CM_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_IT_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_DB_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_MP_MODE_MASK |
+				UVD_CGC_CTRL__SYS_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_MODE_MASK |
+				UVD_CGC_CTRL__MPEG2_MODE_MASK |
+				UVD_CGC_CTRL__REGS_MODE_MASK |
+				UVD_CGC_CTRL__RBC_MODE_MASK |
+				UVD_CGC_CTRL__LMI_MC_MODE_MASK |
+				UVD_CGC_CTRL__LMI_UMC_MODE_MASK |
+				UVD_CGC_CTRL__IDCT_MODE_MASK |
+				UVD_CGC_CTRL__MPRD_MODE_MASK |
+				UVD_CGC_CTRL__MPC_MODE_MASK |
+				UVD_CGC_CTRL__LBSI_MODE_MASK |
+				UVD_CGC_CTRL__LRBBM_MODE_MASK |
+				UVD_CGC_CTRL__WCB_MODE_MASK |
+				UVD_CGC_CTRL__VCPU_MODE_MASK |
+				UVD_CGC_CTRL__JPEG_MODE_MASK |
+				UVD_CGC_CTRL__SCPU_MODE_MASK);
+		data1 |= UVD_CGC_CTRL2__DYN_OCLK_RAMP_EN_MASK |
+				UVD_CGC_CTRL2__DYN_RCLK_RAMP_EN_MASK;
+		data1 &= ~UVD_CGC_CTRL2__GATER_DIV_ID_MASK;
+		data1 |= 7 << REG_FIELD_SHIFT(UVD_CGC_CTRL2, GATER_DIV_ID);
+		data2 &= ~(UVD_SUVD_CGC_CTRL__SRE_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SIT_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SMP_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SCM_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SDB_MODE_MASK);
+	} else {
+		data |= UVD_CGC_CTRL__UDEC_RE_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_CM_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_IT_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_DB_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_MP_MODE_MASK |
+				UVD_CGC_CTRL__SYS_MODE_MASK |
+				UVD_CGC_CTRL__UDEC_MODE_MASK |
+				UVD_CGC_CTRL__MPEG2_MODE_MASK |
+				UVD_CGC_CTRL__REGS_MODE_MASK |
+				UVD_CGC_CTRL__RBC_MODE_MASK |
+				UVD_CGC_CTRL__LMI_MC_MODE_MASK |
+				UVD_CGC_CTRL__LMI_UMC_MODE_MASK |
+				UVD_CGC_CTRL__IDCT_MODE_MASK |
+				UVD_CGC_CTRL__MPRD_MODE_MASK |
+				UVD_CGC_CTRL__MPC_MODE_MASK |
+				UVD_CGC_CTRL__LBSI_MODE_MASK |
+				UVD_CGC_CTRL__LRBBM_MODE_MASK |
+				UVD_CGC_CTRL__WCB_MODE_MASK |
+				UVD_CGC_CTRL__VCPU_MODE_MASK |
+				UVD_CGC_CTRL__SCPU_MODE_MASK;
+		data2 |= UVD_SUVD_CGC_CTRL__SRE_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SIT_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SMP_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SCM_MODE_MASK |
+				UVD_SUVD_CGC_CTRL__SDB_MODE_MASK;
+	}
+	WREG32(mmUVD_CGC_CTRL, data);
+	WREG32(mmUVD_SUVD_CGC_CTRL, data2);
+
+	data = RREG32_UVD_CTX(ixUVD_CGC_CTRL2);
+	data &= ~(REG_FIELD_MASK(UVD_CGC_CTRL2, DYN_OCLK_RAMP_EN) |
+			REG_FIELD_MASK(UVD_CGC_CTRL2, DYN_RCLK_RAMP_EN) |
+			REG_FIELD_MASK(UVD_CGC_CTRL2, GATER_DIV_ID));
+	data1 &= (REG_FIELD_MASK(UVD_CGC_CTRL2, DYN_OCLK_RAMP_EN) |
+			REG_FIELD_MASK(UVD_CGC_CTRL2, DYN_RCLK_RAMP_EN) |
+			REG_FIELD_MASK(UVD_CGC_CTRL2, GATER_DIV_ID));
+	data |= data1;
+	WREG32_UVD_CTX(ixUVD_CGC_CTRL2, data);
+}
+
 /**
  * uvd_v6_0_start - start UVD block
  *
@@ -303,8 +531,19 @@ static int uvd_v6_0_start(struct amdgpu_device *adev)
 
 	uvd_v6_0_mc_resume(adev);
 
-	/* disable clock gating */
-	WREG32(mmUVD_CGC_GATE, 0);
+	/* Set dynamic clock gating in S/W control mode */
+	if (adev->cg_flags & AMD_CG_SUPPORT_UVD_MGCG) {
+		if (adev->flags & AMD_IS_APU)
+			cz_set_uvd_clock_gating_branches(adev, false);
+		else
+			tonga_set_uvd_clock_gating_branches(adev, false);
+		uvd_v6_0_set_uvd_dynamic_clock_mode(adev, true);
+	} else {
+		/* disable clock gating */
+		uint32_t data = RREG32(mmUVD_CGC_CTRL);
+		data &= ~UVD_CGC_CTRL__DYN_CLOCK_MODE_MASK;
+		WREG32(mmUVD_CGC_CTRL, data);
+	}
 
 	/* disable interupt */
 	WREG32_P(mmUVD_MASTINT_EN, 0, ~(1 << 1));
@@ -758,6 +997,24 @@ static int uvd_v6_0_process_interrupt(struct amdgpu_device *adev,
 static int uvd_v6_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	bool enable = (state == AMD_CG_STATE_GATE) ? true : false;
+
+	if (!(adev->cg_flags & AMD_CG_SUPPORT_UVD_MGCG))
+		return 0;
+
+	if (enable) {
+		if (adev->flags & AMD_IS_APU)
+			cz_set_uvd_clock_gating_branches(adev, enable);
+		else
+			tonga_set_uvd_clock_gating_branches(adev, enable);
+		uvd_v6_0_set_uvd_dynamic_clock_mode(adev, true);
+	} else {
+		uint32_t data = RREG32(mmUVD_CGC_CTRL);
+		data &= ~UVD_CGC_CTRL__DYN_CLOCK_MODE_MASK;
+		WREG32(mmUVD_CGC_CTRL, data);
+	}
+
 	return 0;
 }
 
@@ -773,6 +1030,9 @@ static int uvd_v6_0_set_powergating_state(void *handle,
 	 */
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	if (!(adev->pg_flags & AMD_PG_SUPPORT_UVD))
+		return 0;
+
 	if (state == AMD_PG_STATE_GATE) {
 		uvd_v6_0_stop(adev);
 		return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/vce_v2_0.c b/drivers/gpu/drm/amd/amdgpu/vce_v2_0.c
index 52ac7a8f1e58..a822edacfa95 100644
--- a/drivers/gpu/drm/amd/amdgpu/vce_v2_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/vce_v2_0.c
@@ -373,7 +373,7 @@ static void vce_v2_0_enable_mgcg(struct amdgpu_device *adev, bool enable)
 {
 	bool sw_cg = false;
 
-	if (enable && (adev->cg_flags & AMDGPU_CG_SUPPORT_VCE_MGCG)) {
+	if (enable && (adev->cg_flags & AMD_CG_SUPPORT_VCE_MGCG)) {
 		if (sw_cg)
 			vce_v2_0_set_sw_cg(adev, true);
 		else
@@ -608,6 +608,9 @@ static int vce_v2_0_set_powergating_state(void *handle,
 	 */
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	if (!(adev->pg_flags & AMD_PG_SUPPORT_VCE))
+		return 0;
+
 	if (state == AMD_PG_STATE_GATE)
 		/* XXX do we need a vce_v2_0_stop()? */
 		return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c b/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
index 370c6c9d81c2..d662fa9f9091 100644
--- a/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
+++ b/drivers/gpu/drm/amd/amdgpu/vce_v3_0.c
@@ -103,6 +103,108 @@ static void vce_v3_0_ring_set_wptr(struct amdgpu_ring *ring)
 		WREG32(mmVCE_RB_WPTR2, ring->wptr);
 }
 
+static void vce_v3_0_override_vce_clock_gating(struct amdgpu_device *adev, bool override)
+{
+	u32 tmp, data;
+
+	tmp = data = RREG32(mmVCE_RB_ARB_CTRL);
+	if (override)
+		data |= VCE_RB_ARB_CTRL__VCE_CGTT_OVERRIDE_MASK;
+	else
+		data &= ~VCE_RB_ARB_CTRL__VCE_CGTT_OVERRIDE_MASK;
+
+	if (tmp != data)
+		WREG32(mmVCE_RB_ARB_CTRL, data);
+}
+
+static void vce_v3_0_set_vce_sw_clock_gating(struct amdgpu_device *adev,
+					     bool gated)
+{
+	u32 tmp, data;
+	/* Set Override to disable Clock Gating */
+	vce_v3_0_override_vce_clock_gating(adev, true);
+
+	if (!gated) {
+		/* Force CLOCK ON for VCE_CLOCK_GATING_B,
+		 * {*_FORCE_ON, *_FORCE_OFF} = {1, 0}
+		 * VREG can be FORCE ON or set to Dynamic, but can't be OFF
+		 */
+		tmp = data = RREG32(mmVCE_CLOCK_GATING_B);
+		data |= 0x1ff;
+		data &= ~0xef0000;
+		if (tmp != data)
+			WREG32(mmVCE_CLOCK_GATING_B, data);
+
+		/* Force CLOCK ON for VCE_UENC_CLOCK_GATING,
+		 * {*_FORCE_ON, *_FORCE_OFF} = {1, 0}
+		 */
+		tmp = data = RREG32(mmVCE_UENC_CLOCK_GATING);
+		data |= 0x3ff000;
+		data &= ~0xffc00000;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_CLOCK_GATING, data);
+
+		/* set VCE_UENC_CLOCK_GATING_2 */
+		tmp = data = RREG32(mmVCE_UENC_CLOCK_GATING_2);
+		data |= 0x2;
+		data &= ~0x2;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_CLOCK_GATING_2, data);
+
+		/* Force CLOCK ON for VCE_UENC_REG_CLOCK_GATING */
+		tmp = data = RREG32(mmVCE_UENC_REG_CLOCK_GATING);
+		data |= 0x37f;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_REG_CLOCK_GATING, data);
+
+		/* Force VCE_UENC_DMA_DCLK_CTRL Clock ON */
+		tmp = data = RREG32(mmVCE_UENC_DMA_DCLK_CTRL);
+		data |= VCE_UENC_DMA_DCLK_CTRL__WRDMCLK_FORCEON_MASK |
+				VCE_UENC_DMA_DCLK_CTRL__RDDMCLK_FORCEON_MASK |
+				VCE_UENC_DMA_DCLK_CTRL__REGCLK_FORCEON_MASK  |
+				0x8;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_DMA_DCLK_CTRL, data);
+	} else {
+		/* Force CLOCK OFF for VCE_CLOCK_GATING_B,
+		 * {*, *_FORCE_OFF} = {*, 1}
+		 * set VREG to Dynamic, as it can't be OFF
+		 */
+		tmp = data = RREG32(mmVCE_CLOCK_GATING_B);
+		data &= ~0x80010;
+		data |= 0xe70008;
+		if (tmp != data)
+			WREG32(mmVCE_CLOCK_GATING_B, data);
+		/* Force CLOCK OFF for VCE_UENC_CLOCK_GATING,
+		 * Force ClOCK OFF takes precedent over Force CLOCK ON setting.
+		 * {*_FORCE_ON, *_FORCE_OFF} = {*, 1}
+		 */
+		tmp = data = RREG32(mmVCE_UENC_CLOCK_GATING);
+		data |= 0xffc00000;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_CLOCK_GATING, data);
+		/* Set VCE_UENC_CLOCK_GATING_2 */
+		tmp = data = RREG32(mmVCE_UENC_CLOCK_GATING_2);
+		data |= 0x10000;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_CLOCK_GATING_2, data);
+		/* Set VCE_UENC_REG_CLOCK_GATING to dynamic */
+		tmp = data = RREG32(mmVCE_UENC_REG_CLOCK_GATING);
+		data &= ~0xffc00000;
+		if (tmp != data)
+			WREG32(mmVCE_UENC_REG_CLOCK_GATING, data);
+		/* Set VCE_UENC_DMA_DCLK_CTRL CG always in dynamic mode */
+		tmp = data = RREG32(mmVCE_UENC_DMA_DCLK_CTRL);
+		data &= ~(VCE_UENC_DMA_DCLK_CTRL__WRDMCLK_FORCEON_MASK |
+				VCE_UENC_DMA_DCLK_CTRL__RDDMCLK_FORCEON_MASK |
+				VCE_UENC_DMA_DCLK_CTRL__REGCLK_FORCEON_MASK  |
+				0x8);
+		if (tmp != data)
+			WREG32(mmVCE_UENC_DMA_DCLK_CTRL, data);
+	}
+	vce_v3_0_override_vce_clock_gating(adev, false);
+}
+
 /**
  * vce_v3_0_start - start VCE block
  *
@@ -121,7 +223,7 @@ static int vce_v3_0_start(struct amdgpu_device *adev)
 		if (adev->vce.harvest_config & (1 << idx))
 			continue;
 
-		if(idx == 0)
+		if (idx == 0)
 			WREG32_P(mmGRBM_GFX_INDEX, 0,
 				~GRBM_GFX_INDEX__VCE_INSTANCE_MASK);
 		else
@@ -174,6 +276,10 @@ static int vce_v3_0_start(struct amdgpu_device *adev)
 		/* clear BUSY flag */
 		WREG32_P(mmVCE_STATUS, 0, ~1);
 
+		/* Set Clock-Gating off */
+		if (adev->cg_flags & AMD_CG_SUPPORT_VCE_MGCG)
+			vce_v3_0_set_vce_sw_clock_gating(adev, false);
+
 		if (r) {
 			DRM_ERROR("VCE not responding, giving up!!!\n");
 			mutex_unlock(&adev->grbm_idx_mutex);
@@ -208,14 +314,11 @@ static int vce_v3_0_start(struct amdgpu_device *adev)
 static unsigned vce_v3_0_get_harvest_config(struct amdgpu_device *adev)
 {
 	u32 tmp;
-	unsigned ret;
 
 	/* Fiji, Stoney are single pipe */
 	if ((adev->asic_type == CHIP_FIJI) ||
-	    (adev->asic_type == CHIP_STONEY)){
-		ret = AMDGPU_VCE_HARVEST_VCE1;
-		return ret;
-	}
+	    (adev->asic_type == CHIP_STONEY))
+		return AMDGPU_VCE_HARVEST_VCE1;
 
 	/* Tonga and CZ are dual or single pipe */
 	if (adev->flags & AMD_IS_APU)
@@ -229,19 +332,14 @@ static unsigned vce_v3_0_get_harvest_config(struct amdgpu_device *adev)
 
 	switch (tmp) {
 	case 1:
-		ret = AMDGPU_VCE_HARVEST_VCE0;
-		break;
+		return AMDGPU_VCE_HARVEST_VCE0;
 	case 2:
-		ret = AMDGPU_VCE_HARVEST_VCE1;
-		break;
+		return AMDGPU_VCE_HARVEST_VCE1;
 	case 3:
-		ret = AMDGPU_VCE_HARVEST_VCE0 | AMDGPU_VCE_HARVEST_VCE1;
-		break;
+		return AMDGPU_VCE_HARVEST_VCE0 | AMDGPU_VCE_HARVEST_VCE1;
 	default:
-		ret = 0;
+		return 0;
 	}
-
-	return ret;
 }
 
 static int vce_v3_0_early_init(void *handle)
@@ -316,28 +414,22 @@ static int vce_v3_0_sw_fini(void *handle)
 
 static int vce_v3_0_hw_init(void *handle)
 {
-	struct amdgpu_ring *ring;
-	int r;
+	int r, i;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
 	r = vce_v3_0_start(adev);
 	if (r)
 		return r;
 
-	ring = &adev->vce.ring[0];
-	ring->ready = true;
-	r = amdgpu_ring_test_ring(ring);
-	if (r) {
-		ring->ready = false;
-		return r;
-	}
+	adev->vce.ring[0].ready = false;
+	adev->vce.ring[1].ready = false;
 
-	ring = &adev->vce.ring[1];
-	ring->ready = true;
-	r = amdgpu_ring_test_ring(ring);
-	if (r) {
-		ring->ready = false;
-		return r;
+	for (i = 0; i < 2; i++) {
+		r = amdgpu_ring_test_ring(&adev->vce.ring[i]);
+		if (r)
+			return r;
+		else
+			adev->vce.ring[i].ready = true;
 	}
 
 	DRM_INFO("VCE initialized successfully.\n");
@@ -437,17 +529,9 @@ static bool vce_v3_0_is_idle(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 mask = 0;
-	int idx;
 
-	for (idx = 0; idx < 2; ++idx) {
-		if (adev->vce.harvest_config & (1 << idx))
-			continue;
-
-		if (idx == 0)
-			mask |= SRBM_STATUS2__VCE0_BUSY_MASK;
-		else
-			mask |= SRBM_STATUS2__VCE1_BUSY_MASK;
-	}
+	mask |= (adev->vce.harvest_config & AMDGPU_VCE_HARVEST_VCE0) ? 0 : SRBM_STATUS2__VCE0_BUSY_MASK;
+	mask |= (adev->vce.harvest_config & AMDGPU_VCE_HARVEST_VCE1) ? 0 : SRBM_STATUS2__VCE1_BUSY_MASK;
 
 	return !(RREG32(mmSRBM_STATUS2) & mask);
 }
@@ -456,23 +540,11 @@ static int vce_v3_0_wait_for_idle(void *handle)
 {
 	unsigned i;
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
-	u32 mask = 0;
-	int idx;
 
-	for (idx = 0; idx < 2; ++idx) {
-		if (adev->vce.harvest_config & (1 << idx))
-			continue;
-
-		if (idx == 0)
-			mask |= SRBM_STATUS2__VCE0_BUSY_MASK;
-		else
-			mask |= SRBM_STATUS2__VCE1_BUSY_MASK;
-	}
-
-	for (i = 0; i < adev->usec_timeout; i++) {
-		if (!(RREG32(mmSRBM_STATUS2) & mask))
+	for (i = 0; i < adev->usec_timeout; i++)
+		if (vce_v3_0_is_idle(handle))
 			return 0;
-	}
+
 	return -ETIMEDOUT;
 }
 
@@ -480,17 +552,10 @@ static int vce_v3_0_soft_reset(void *handle)
 {
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 	u32 mask = 0;
-	int idx;
 
-	for (idx = 0; idx < 2; ++idx) {
-		if (adev->vce.harvest_config & (1 << idx))
-			continue;
+	mask |= (adev->vce.harvest_config & AMDGPU_VCE_HARVEST_VCE0) ? 0 : SRBM_SOFT_RESET__SOFT_RESET_VCE0_MASK;
+	mask |= (adev->vce.harvest_config & AMDGPU_VCE_HARVEST_VCE1) ? 0 : SRBM_SOFT_RESET__SOFT_RESET_VCE1_MASK;
 
-		if (idx == 0)
-			mask |= SRBM_SOFT_RESET__SOFT_RESET_VCE0_MASK;
-		else
-			mask |= SRBM_SOFT_RESET__SOFT_RESET_VCE1_MASK;
-	}
 	WREG32_P(mmSRBM_SOFT_RESET, mask,
 		 ~(SRBM_SOFT_RESET__SOFT_RESET_VCE0_MASK |
 		   SRBM_SOFT_RESET__SOFT_RESET_VCE1_MASK));
@@ -592,10 +657,8 @@ static int vce_v3_0_process_interrupt(struct amdgpu_device *adev,
 
 	switch (entry->src_data) {
 	case 0:
-		amdgpu_fence_process(&adev->vce.ring[0]);
-		break;
 	case 1:
-		amdgpu_fence_process(&adev->vce.ring[1]);
+		amdgpu_fence_process(&adev->vce.ring[entry->src_data]);
 		break;
 	default:
 		DRM_ERROR("Unhandled interrupt: %d %d\n",
@@ -609,6 +672,47 @@ static int vce_v3_0_process_interrupt(struct amdgpu_device *adev,
 static int vce_v3_0_set_clockgating_state(void *handle,
 					  enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+	bool enable = (state == AMD_CG_STATE_GATE) ? true : false;
+	int i;
+
+	if (!(adev->cg_flags & AMD_CG_SUPPORT_VCE_MGCG))
+		return 0;
+
+	mutex_lock(&adev->grbm_idx_mutex);
+	for (i = 0; i < 2; i++) {
+		/* Program VCE Instance 0 or 1 if not harvested */
+		if (adev->vce.harvest_config & (1 << i))
+			continue;
+
+		if (i == 0)
+			WREG32_P(mmGRBM_GFX_INDEX, 0,
+					~GRBM_GFX_INDEX__VCE_INSTANCE_MASK);
+		else
+			WREG32_P(mmGRBM_GFX_INDEX,
+					GRBM_GFX_INDEX__VCE_INSTANCE_MASK,
+					~GRBM_GFX_INDEX__VCE_INSTANCE_MASK);
+
+		if (enable) {
+			/* initialize VCE_CLOCK_GATING_A: Clock ON/OFF delay */
+			uint32_t data = RREG32(mmVCE_CLOCK_GATING_A);
+			data &= ~(0xf | 0xff0);
+			data |= ((0x0 << 0) | (0x04 << 4));
+			WREG32(mmVCE_CLOCK_GATING_A, data);
+
+			/* initialize VCE_UENC_CLOCK_GATING: Clock ON/OFF delay */
+			data = RREG32(mmVCE_UENC_CLOCK_GATING);
+			data &= ~(0xf | 0xff0);
+			data |= ((0x0 << 0) | (0x04 << 4));
+			WREG32(mmVCE_UENC_CLOCK_GATING, data);
+		}
+
+		vce_v3_0_set_vce_sw_clock_gating(adev, enable);
+	}
+
+	WREG32_P(mmGRBM_GFX_INDEX, 0, ~GRBM_GFX_INDEX__VCE_INSTANCE_MASK);
+	mutex_unlock(&adev->grbm_idx_mutex);
+
 	return 0;
 }
 
@@ -624,6 +728,9 @@ static int vce_v3_0_set_powergating_state(void *handle,
 	 */
 	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
 
+	if (!(adev->pg_flags & AMD_PG_SUPPORT_VCE))
+		return 0;
+
 	if (state == AMD_PG_STATE_GATE)
 		/* XXX do we need a vce_v3_0_stop()? */
 		return 0;
diff --git a/drivers/gpu/drm/amd/amdgpu/vi.c b/drivers/gpu/drm/amd/amdgpu/vi.c
index 2adc1c855e85..0d14d108a6c4 100644
--- a/drivers/gpu/drm/amd/amdgpu/vi.c
+++ b/drivers/gpu/drm/amd/amdgpu/vi.c
@@ -31,6 +31,7 @@
 #include "amdgpu_vce.h"
 #include "amdgpu_ucode.h"
 #include "atom.h"
+#include "amd_pcie.h"
 
 #include "gmc/gmc_8_1_d.h"
 #include "gmc/gmc_8_1_sh_mask.h"
@@ -60,6 +61,7 @@
 #include "vi.h"
 #include "vi_dpm.h"
 #include "gmc_v8_0.h"
+#include "gmc_v7_0.h"
 #include "gfx_v8_0.h"
 #include "sdma_v2_4.h"
 #include "sdma_v3_0.h"
@@ -71,6 +73,7 @@
 #include "uvd_v5_0.h"
 #include "uvd_v6_0.h"
 #include "vce_v3_0.h"
+#include "amdgpu_powerplay.h"
 
 /*
  * Indirect registers accessor
@@ -376,6 +379,38 @@ static bool vi_read_disabled_bios(struct amdgpu_device *adev)
 	WREG32_SMC(ixROM_CNTL, rom_cntl);
 	return r;
 }
+
+static bool vi_read_bios_from_rom(struct amdgpu_device *adev,
+				  u8 *bios, u32 length_bytes)
+{
+	u32 *dw_ptr;
+	unsigned long flags;
+	u32 i, length_dw;
+
+	if (bios == NULL)
+		return false;
+	if (length_bytes == 0)
+		return false;
+	/* APU vbios image is part of sbios image */
+	if (adev->flags & AMD_IS_APU)
+		return false;
+
+	dw_ptr = (u32 *)bios;
+	length_dw = ALIGN(length_bytes, 4) / 4;
+	/* take the smc lock since we are using the smc index */
+	spin_lock_irqsave(&adev->smc_idx_lock, flags);
+	/* set rom index to 0 */
+	WREG32(mmSMC_IND_INDEX_0, ixROM_INDEX);
+	WREG32(mmSMC_IND_DATA_0, 0);
+	/* set index to data for continous read */
+	WREG32(mmSMC_IND_INDEX_0, ixROM_DATA);
+	for (i = 0; i < length_dw; i++)
+		dw_ptr[i] = RREG32(mmSMC_IND_DATA_0);
+	spin_unlock_irqrestore(&adev->smc_idx_lock, flags);
+
+	return true;
+}
+
 static struct amdgpu_allowed_register_entry tonga_allowed_read_registers[] = {
 	{mmGB_MACROTILE_MODE7, true},
 };
@@ -1019,9 +1054,6 @@ static int vi_set_vce_clocks(struct amdgpu_device *adev, u32 evclk, u32 ecclk)
 
 static void vi_pcie_gen3_enable(struct amdgpu_device *adev)
 {
-	u32 mask;
-	int ret;
-
 	if (pci_is_root_bus(adev->pdev->bus))
 		return;
 
@@ -1031,11 +1063,8 @@ static void vi_pcie_gen3_enable(struct amdgpu_device *adev)
 	if (adev->flags & AMD_IS_APU)
 		return;
 
-	ret = drm_pcie_get_speed_cap_mask(adev->ddev, &mask);
-	if (ret != 0)
-		return;
-
-	if (!(mask & (DRM_PCIE_SPEED_50 | DRM_PCIE_SPEED_80)))
+	if (!(adev->pm.pcie_gen_mask & (CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2 |
+					CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)))
 		return;
 
 	/* todo */
@@ -1081,10 +1110,10 @@ static const struct amdgpu_ip_block_version topaz_ip_blocks[] =
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_GMC,
-		.major = 8,
-		.minor = 0,
+		.major = 7,
+		.minor = 4,
 		.rev = 0,
-		.funcs = &gmc_v8_0_ip_funcs,
+		.funcs = &gmc_v7_0_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_IH,
@@ -1098,7 +1127,7 @@ static const struct amdgpu_ip_block_version topaz_ip_blocks[] =
 		.major = 7,
 		.minor = 1,
 		.rev = 0,
-		.funcs = &iceland_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_GFX,
@@ -1145,7 +1174,7 @@ static const struct amdgpu_ip_block_version tonga_ip_blocks[] =
 		.major = 7,
 		.minor = 1,
 		.rev = 0,
-		.funcs = &tonga_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -1213,7 +1242,7 @@ static const struct amdgpu_ip_block_version fiji_ip_blocks[] =
 		.major = 7,
 		.minor = 1,
 		.rev = 0,
-		.funcs = &fiji_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs,
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -1281,7 +1310,7 @@ static const struct amdgpu_ip_block_version cz_ip_blocks[] =
 		.major = 8,
 		.minor = 0,
 		.rev = 0,
-		.funcs = &cz_dpm_ip_funcs,
+		.funcs = &amdgpu_pp_ip_funcs
 	},
 	{
 		.type = AMD_IP_BLOCK_TYPE_DCE,
@@ -1354,20 +1383,18 @@ int vi_set_ip_blocks(struct amdgpu_device *adev)
 
 static uint32_t vi_get_rev_id(struct amdgpu_device *adev)
 {
-	if (adev->asic_type == CHIP_TOPAZ)
-		return (RREG32(mmPCIE_EFUSE4) & PCIE_EFUSE4__STRAP_BIF_ATI_REV_ID_MASK)
-			>> PCIE_EFUSE4__STRAP_BIF_ATI_REV_ID__SHIFT;
-	else if (adev->flags & AMD_IS_APU)
+	if (adev->flags & AMD_IS_APU)
 		return (RREG32_SMC(ATI_REV_ID_FUSE_MACRO__ADDRESS) & ATI_REV_ID_FUSE_MACRO__MASK)
 			>> ATI_REV_ID_FUSE_MACRO__SHIFT;
 	else
-		return (RREG32(mmCC_DRM_ID_STRAPS) & CC_DRM_ID_STRAPS__ATI_REV_ID_MASK)
-			>> CC_DRM_ID_STRAPS__ATI_REV_ID__SHIFT;
+		return (RREG32(mmPCIE_EFUSE4) & PCIE_EFUSE4__STRAP_BIF_ATI_REV_ID_MASK)
+			>> PCIE_EFUSE4__STRAP_BIF_ATI_REV_ID__SHIFT;
 }
 
 static const struct amdgpu_asic_funcs vi_asic_funcs =
 {
 	.read_disabled_bios = &vi_read_disabled_bios,
+	.read_bios_from_rom = &vi_read_bios_from_rom,
 	.read_register = &vi_read_register,
 	.reset = &vi_asic_reset,
 	.set_vga_state = &vi_vga_set_state,
@@ -1430,8 +1457,7 @@ static int vi_common_early_init(void *handle)
 	case CHIP_STONEY:
 		adev->has_uvd = true;
 		adev->cg_flags = 0;
-		/* Disable UVD pg */
-		adev->pg_flags = /* AMDGPU_PG_SUPPORT_UVD | */AMDGPU_PG_SUPPORT_VCE;
+		adev->pg_flags = 0;
 		adev->external_rev_id = adev->rev_id + 0x1;
 		break;
 	default:
@@ -1442,6 +1468,8 @@ static int vi_common_early_init(void *handle)
 	if (amdgpu_smc_load_fw && smc_enabled)
 		adev->firmware.smu_load = true;
 
+	amdgpu_get_pcie_info(adev);
+
 	return 0;
 }
 
@@ -1515,9 +1543,95 @@ static int vi_common_soft_reset(void *handle)
 	return 0;
 }
 
+static void fiji_update_bif_medium_grain_light_sleep(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	temp = data = RREG32_PCIE(ixPCIE_CNTL2);
+
+	if (enable)
+		data |= PCIE_CNTL2__SLV_MEM_LS_EN_MASK |
+				PCIE_CNTL2__MST_MEM_LS_EN_MASK |
+				PCIE_CNTL2__REPLAY_MEM_LS_EN_MASK;
+	else
+		data &= ~(PCIE_CNTL2__SLV_MEM_LS_EN_MASK |
+				PCIE_CNTL2__MST_MEM_LS_EN_MASK |
+				PCIE_CNTL2__REPLAY_MEM_LS_EN_MASK);
+
+	if (temp != data)
+		WREG32_PCIE(ixPCIE_CNTL2, data);
+}
+
+static void fiji_update_hdp_medium_grain_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	temp = data = RREG32(mmHDP_HOST_PATH_CNTL);
+
+	if (enable)
+		data &= ~HDP_HOST_PATH_CNTL__CLOCK_GATING_DIS_MASK;
+	else
+		data |= HDP_HOST_PATH_CNTL__CLOCK_GATING_DIS_MASK;
+
+	if (temp != data)
+		WREG32(mmHDP_HOST_PATH_CNTL, data);
+}
+
+static void fiji_update_hdp_light_sleep(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	temp = data = RREG32(mmHDP_MEM_POWER_LS);
+
+	if (enable)
+		data |= HDP_MEM_POWER_LS__LS_ENABLE_MASK;
+	else
+		data &= ~HDP_MEM_POWER_LS__LS_ENABLE_MASK;
+
+	if (temp != data)
+		WREG32(mmHDP_MEM_POWER_LS, data);
+}
+
+static void fiji_update_rom_medium_grain_clock_gating(struct amdgpu_device *adev,
+		bool enable)
+{
+	uint32_t temp, data;
+
+	temp = data = RREG32_SMC(ixCGTT_ROM_CLK_CTRL0);
+
+	if (enable)
+		data &= ~(CGTT_ROM_CLK_CTRL0__SOFT_OVERRIDE0_MASK |
+				CGTT_ROM_CLK_CTRL0__SOFT_OVERRIDE1_MASK);
+	else
+		data |= CGTT_ROM_CLK_CTRL0__SOFT_OVERRIDE0_MASK |
+				CGTT_ROM_CLK_CTRL0__SOFT_OVERRIDE1_MASK;
+
+	if (temp != data)
+		WREG32_SMC(ixCGTT_ROM_CLK_CTRL0, data);
+}
+
 static int vi_common_set_clockgating_state(void *handle,
 					    enum amd_clockgating_state state)
 {
+	struct amdgpu_device *adev = (struct amdgpu_device *)handle;
+
+	switch (adev->asic_type) {
+	case CHIP_FIJI:
+		fiji_update_bif_medium_grain_light_sleep(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		fiji_update_hdp_medium_grain_clock_gating(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		fiji_update_hdp_light_sleep(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		fiji_update_rom_medium_grain_clock_gating(adev,
+				state == AMD_CG_STATE_GATE ? true : false);
+		break;
+	default:
+		break;
+	}
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c b/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c
index c6a1b4cc6458..d2b49c026cf6 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_chardev.c
@@ -558,20 +558,10 @@ static int kfd_ioctl_dbg_address_watch(struct file *filep,
 		return -EINVAL;
 
 	/* this is the actual buffer to work with */
-
-	args_buff = kmalloc(args->buf_size_in_bytes -
-					sizeof(*args), GFP_KERNEL);
-	if (args_buff == NULL)
-		return -ENOMEM;
-
-	status = copy_from_user(args_buff, cmd_from_user,
+	args_buff = memdup_user(cmd_from_user,
 				args->buf_size_in_bytes - sizeof(*args));
-
-	if (status != 0) {
-		pr_debug("Failed to copy address watch user data\n");
-		kfree(args_buff);
-		return -EINVAL;
-	}
+	if (IS_ERR(args_buff))
+		return PTR_ERR(args_buff);
 
 	aw_info.process = p;
 
@@ -677,22 +667,12 @@ static int kfd_ioctl_dbg_wave_control(struct file *filep,
 	if (cmd_from_user == NULL)
 		return -EINVAL;
 
-	/* this is the actual buffer to work with */
-
-	args_buff = kmalloc(args->buf_size_in_bytes - sizeof(*args),
-			GFP_KERNEL);
-
-	if (args_buff == NULL)
-		return -ENOMEM;
+	/* copy the entire buffer from user */
 
-	/* Now copy the entire buffer from user */
-	status = copy_from_user(args_buff, cmd_from_user,
+	args_buff = memdup_user(cmd_from_user,
 				args->buf_size_in_bytes - sizeof(*args));
-	if (status != 0) {
-		pr_debug("Failed to copy wave control user data\n");
-		kfree(args_buff);
-		return -EINVAL;
-	}
+	if (IS_ERR(args_buff))
+		return PTR_ERR(args_buff);
 
 	/* move ptr to the start of the "pay-load" area */
 	wac_info.process = p;
diff --git a/drivers/gpu/drm/amd/amdkfd/kfd_process.c b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
index 9be007081b72..a902ae037398 100644
--- a/drivers/gpu/drm/amd/amdkfd/kfd_process.c
+++ b/drivers/gpu/drm/amd/amdkfd/kfd_process.c
@@ -194,7 +194,7 @@ static void kfd_process_wq_release(struct work_struct *work)
 
 	kfree(p);
 
-	kfree((void *)work);
+	kfree(work);
 }
 
 static void kfd_process_destroy_delayed(struct rcu_head *rcu)
diff --git a/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.h b/drivers/gpu/drm/amd/include/amd_acpi.h
index 01a29c3d7011..496360eb3fba 100644
--- a/drivers/gpu/drm/amd/amdgpu/amdgpu_acpi.h
+++ b/drivers/gpu/drm/amd/include/amd_acpi.h
@@ -21,14 +21,63 @@
  *
  */
 
-#ifndef AMDGPU_ACPI_H
-#define AMDGPU_ACPI_H
+#ifndef AMD_ACPI_H
+#define AMD_ACPI_H
 
-struct amdgpu_device;
-struct acpi_bus_event;
+#define ACPI_AC_CLASS           "ac_adapter"
 
-int amdgpu_atif_handler(struct amdgpu_device *adev,
-		struct acpi_bus_event *event);
+struct atif_verify_interface {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u16 version;		/* version */
+	u32 notification_mask;	/* supported notifications mask */
+	u32 function_bits;	/* supported functions bit vector */
+} __packed;
+
+struct atif_system_params {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u32 valid_mask;		/* valid flags mask */
+	u32 flags;		/* flags */
+	u8 command_code;	/* notify command code */
+} __packed;
+
+struct atif_sbios_requests {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u32 pending;		/* pending sbios requests */
+	u8 panel_exp_mode;	/* panel expansion mode */
+	u8 thermal_gfx;		/* thermal state: target gfx controller */
+	u8 thermal_state;	/* thermal state: state id (0: exit state, non-0: state) */
+	u8 forced_power_gfx;	/* forced power state: target gfx controller */
+	u8 forced_power_state;	/* forced power state: state id */
+	u8 system_power_src;	/* system power source */
+	u8 backlight_level;	/* panel backlight level (0-255) */
+} __packed;
+
+#define ATIF_NOTIFY_MASK	0x3
+#define ATIF_NOTIFY_NONE	0
+#define ATIF_NOTIFY_81		1
+#define ATIF_NOTIFY_N		2
+
+struct atcs_verify_interface {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u16 version;		/* version */
+	u32 function_bits;	/* supported functions bit vector */
+} __packed;
+
+#define ATCS_VALID_FLAGS_MASK	0x3
+
+struct atcs_pref_req_input {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u16 client_id;		/* client id (bit 2-0: func num, 7-3: dev num, 15-8: bus num) */
+	u16 valid_flags_mask;	/* valid flags mask */
+	u16 flags;		/* flags */
+	u8 req_type;		/* request type */
+	u8 perf_req;		/* performance request */
+} __packed;
+
+struct atcs_pref_req_output {
+	u16 size;		/* structure size in bytes (includes size field) */
+	u8 ret_val;		/* return value */
+} __packed;
 
 /* AMD hw uses four ACPI control methods:
  * 1. ATIF
diff --git a/drivers/gpu/drm/amd/include/amd_pcie.h b/drivers/gpu/drm/amd/include/amd_pcie.h
new file mode 100644
index 000000000000..7c2a916c1e63
--- /dev/null
+++ b/drivers/gpu/drm/amd/include/amd_pcie.h
@@ -0,0 +1,50 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __AMD_PCIE_H__
+#define __AMD_PCIE_H__
+
+/* Following flags shows PCIe link speed supported in driver which are decided by chipset and ASIC */
+#define CAIL_PCIE_LINK_SPEED_SUPPORT_GEN1        0x00010000
+#define CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2        0x00020000
+#define CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3        0x00040000
+#define CAIL_PCIE_LINK_SPEED_SUPPORT_MASK        0xFFFF0000
+#define CAIL_PCIE_LINK_SPEED_SUPPORT_SHIFT       16
+
+/* Following flags shows PCIe link speed supported by ASIC H/W.*/
+#define CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN1   0x00000001
+#define CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN2   0x00000002
+#define CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN3   0x00000004
+#define CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_MASK   0x0000FFFF
+#define CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_SHIFT  0
+
+/* Following flags shows PCIe lane width switch supported in driver which are decided by chipset and ASIC */
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X1          0x00010000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X2          0x00020000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X4          0x00040000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X8          0x00080000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X12         0x00100000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X16         0x00200000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_X32         0x00400000
+#define CAIL_PCIE_LINK_WIDTH_SUPPORT_SHIFT       16
+
+#endif
diff --git a/drivers/gpu/drm/amd/include/amd_pcie_helpers.h b/drivers/gpu/drm/amd/include/amd_pcie_helpers.h
new file mode 100644
index 000000000000..5725bf85eacc
--- /dev/null
+++ b/drivers/gpu/drm/amd/include/amd_pcie_helpers.h
@@ -0,0 +1,141 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __AMD_PCIE_HELPERS_H__
+#define __AMD_PCIE_HELPERS_H__
+
+#include "amd_pcie.h"
+
+static inline bool is_pcie_gen3_supported(uint32_t pcie_link_speed_cap)
+{
+	if (pcie_link_speed_cap & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)
+		return true;
+
+	return false;
+}
+
+static inline bool is_pcie_gen2_supported(uint32_t pcie_link_speed_cap)
+{
+	if (pcie_link_speed_cap & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN2)
+		return true;
+
+	return false;
+}
+
+/* Get the new PCIE speed given the ASIC PCIE Cap and the NewState's requested PCIE speed*/
+static inline uint16_t get_pcie_gen_support(uint32_t pcie_link_speed_cap,
+					    uint16_t ns_pcie_gen)
+{
+	uint32_t asic_pcie_link_speed_cap = (pcie_link_speed_cap &
+		CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_MASK);
+	uint32_t sys_pcie_link_speed_cap  = (pcie_link_speed_cap &
+		CAIL_PCIE_LINK_SPEED_SUPPORT_MASK);
+
+	switch (asic_pcie_link_speed_cap) {
+	case CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN1:
+		return PP_PCIEGen1;
+
+	case CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN2:
+		return PP_PCIEGen2;
+
+	case CAIL_ASIC_PCIE_LINK_SPEED_SUPPORT_GEN3:
+		return PP_PCIEGen3;
+
+	default:
+		if (is_pcie_gen3_supported(sys_pcie_link_speed_cap) &&
+			(ns_pcie_gen == PP_PCIEGen3)) {
+			return PP_PCIEGen3;
+		} else if (is_pcie_gen2_supported(sys_pcie_link_speed_cap) &&
+			((ns_pcie_gen == PP_PCIEGen3) || (ns_pcie_gen == PP_PCIEGen2))) {
+			return PP_PCIEGen2;
+		}
+	}
+
+	return PP_PCIEGen1;
+}
+
+static inline uint16_t get_pcie_lane_support(uint32_t pcie_lane_width_cap,
+					     uint16_t ns_pcie_lanes)
+{
+	int i, j;
+	uint16_t new_pcie_lanes = ns_pcie_lanes;
+	uint16_t pcie_lanes[7] = {1, 2, 4, 8, 12, 16, 32};
+
+	switch (pcie_lane_width_cap) {
+	case 0:
+		printk(KERN_ERR "No valid PCIE lane width reported");
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X1:
+		new_pcie_lanes = 1;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X2:
+		new_pcie_lanes = 2;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X4:
+		new_pcie_lanes = 4;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X8:
+		new_pcie_lanes = 8;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X12:
+		new_pcie_lanes = 12;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X16:
+		new_pcie_lanes = 16;
+		break;
+	case CAIL_PCIE_LINK_WIDTH_SUPPORT_X32:
+		new_pcie_lanes = 32;
+		break;
+	default:
+		for (i = 0; i < 7; i++) {
+			if (ns_pcie_lanes == pcie_lanes[i]) {
+				if (pcie_lane_width_cap & (0x10000 << i)) {
+					break;
+				} else {
+					for (j = i - 1; j >= 0; j--) {
+						if (pcie_lane_width_cap & (0x10000 << j)) {
+							new_pcie_lanes = pcie_lanes[j];
+							break;
+						}
+					}
+
+					if (j < 0) {
+						for (j = i + 1; j < 7; j++) {
+							if (pcie_lane_width_cap & (0x10000 << j)) {
+								new_pcie_lanes = pcie_lanes[j];
+								break;
+							}
+						}
+						if (j > 7)
+							printk(KERN_ERR "Cannot find a valid PCIE lane width!");
+					}
+				}
+				break;
+			}
+		}
+		break;
+	}
+
+	return new_pcie_lanes;
+}
+
+#endif
diff --git a/drivers/gpu/drm/amd/include/amd_shared.h b/drivers/gpu/drm/amd/include/amd_shared.h
index fe28fb353fab..dbf7e6413cab 100644
--- a/drivers/gpu/drm/amd/include/amd_shared.h
+++ b/drivers/gpu/drm/amd/include/amd_shared.h
@@ -85,6 +85,59 @@ enum amd_powergating_state {
 	AMD_PG_STATE_UNGATE,
 };
 
+/* CG flags */
+#define AMD_CG_SUPPORT_GFX_MGCG			(1 << 0)
+#define AMD_CG_SUPPORT_GFX_MGLS			(1 << 1)
+#define AMD_CG_SUPPORT_GFX_CGCG			(1 << 2)
+#define AMD_CG_SUPPORT_GFX_CGLS			(1 << 3)
+#define AMD_CG_SUPPORT_GFX_CGTS			(1 << 4)
+#define AMD_CG_SUPPORT_GFX_CGTS_LS		(1 << 5)
+#define AMD_CG_SUPPORT_GFX_CP_LS		(1 << 6)
+#define AMD_CG_SUPPORT_GFX_RLC_LS		(1 << 7)
+#define AMD_CG_SUPPORT_MC_LS			(1 << 8)
+#define AMD_CG_SUPPORT_MC_MGCG			(1 << 9)
+#define AMD_CG_SUPPORT_SDMA_LS			(1 << 10)
+#define AMD_CG_SUPPORT_SDMA_MGCG		(1 << 11)
+#define AMD_CG_SUPPORT_BIF_LS			(1 << 12)
+#define AMD_CG_SUPPORT_UVD_MGCG			(1 << 13)
+#define AMD_CG_SUPPORT_VCE_MGCG			(1 << 14)
+#define AMD_CG_SUPPORT_HDP_LS			(1 << 15)
+#define AMD_CG_SUPPORT_HDP_MGCG			(1 << 16)
+
+/* PG flags */
+#define AMD_PG_SUPPORT_GFX_PG			(1 << 0)
+#define AMD_PG_SUPPORT_GFX_SMG			(1 << 1)
+#define AMD_PG_SUPPORT_GFX_DMG			(1 << 2)
+#define AMD_PG_SUPPORT_UVD			(1 << 3)
+#define AMD_PG_SUPPORT_VCE			(1 << 4)
+#define AMD_PG_SUPPORT_CP			(1 << 5)
+#define AMD_PG_SUPPORT_GDS			(1 << 6)
+#define AMD_PG_SUPPORT_RLC_SMU_HS		(1 << 7)
+#define AMD_PG_SUPPORT_SDMA			(1 << 8)
+#define AMD_PG_SUPPORT_ACP			(1 << 9)
+#define AMD_PG_SUPPORT_SAMU			(1 << 10)
+
+enum amd_pm_state_type {
+	/* not used for dpm */
+	POWER_STATE_TYPE_DEFAULT,
+	POWER_STATE_TYPE_POWERSAVE,
+	/* user selectable states */
+	POWER_STATE_TYPE_BATTERY,
+	POWER_STATE_TYPE_BALANCED,
+	POWER_STATE_TYPE_PERFORMANCE,
+	/* internal states */
+	POWER_STATE_TYPE_INTERNAL_UVD,
+	POWER_STATE_TYPE_INTERNAL_UVD_SD,
+	POWER_STATE_TYPE_INTERNAL_UVD_HD,
+	POWER_STATE_TYPE_INTERNAL_UVD_HD2,
+	POWER_STATE_TYPE_INTERNAL_UVD_MVC,
+	POWER_STATE_TYPE_INTERNAL_BOOT,
+	POWER_STATE_TYPE_INTERNAL_THERMAL,
+	POWER_STATE_TYPE_INTERNAL_ACPI,
+	POWER_STATE_TYPE_INTERNAL_ULV,
+	POWER_STATE_TYPE_INTERNAL_3DPERF,
+};
+
 struct amd_ip_funcs {
 	/* sets up early driver state (pre sw_init), does not configure hw - Optional */
 	int (*early_init)(void *handle);
diff --git a/drivers/gpu/drm/amd/include/asic_reg/bif/bif_5_0_d.h b/drivers/gpu/drm/amd/include/asic_reg/bif/bif_5_0_d.h
index 92b6ba0047af..293329719bba 100644
--- a/drivers/gpu/drm/amd/include/asic_reg/bif/bif_5_0_d.h
+++ b/drivers/gpu/drm/amd/include/asic_reg/bif/bif_5_0_d.h
@@ -596,6 +596,7 @@
 #define mmSWRST_EP_CONTROL_0                                                    0x14ac
 #define mmCPM_CONTROL                                                           0x14b8
 #define mmGSKT_CONTROL                                                          0x14bf
+#define ixSWRST_COMMAND_1                                                       0x1400103
 #define ixLM_CONTROL                                                            0x1400120
 #define ixLM_PCIETXMUX0                                                         0x1400121
 #define ixLM_PCIETXMUX1                                                         0x1400122
diff --git a/drivers/gpu/drm/amd/include/asic_reg/gca/gfx_8_0_d.h b/drivers/gpu/drm/amd/include/asic_reg/gca/gfx_8_0_d.h
index daf763ba1a8f..a9b6923192ee 100644
--- a/drivers/gpu/drm/amd/include/asic_reg/gca/gfx_8_0_d.h
+++ b/drivers/gpu/drm/amd/include/asic_reg/gca/gfx_8_0_d.h
@@ -2807,5 +2807,18 @@
 #define ixDIDT_DBR_WEIGHT0_3                                                    0x90
 #define ixDIDT_DBR_WEIGHT4_7                                                    0x91
 #define ixDIDT_DBR_WEIGHT8_11                                                   0x92
+#define mmTD_EDC_CNT                                                            0x252e
+#define mmCPF_EDC_TAG_CNT                                                       0x3188
+#define mmCPF_EDC_ROQ_CNT                                                       0x3189
+#define mmCPF_EDC_ATC_CNT                                                       0x318a
+#define mmCPG_EDC_TAG_CNT                                                       0x318b
+#define mmCPG_EDC_ATC_CNT                                                       0x318c
+#define mmCPG_EDC_DMA_CNT                                                       0x318d
+#define mmCPC_EDC_SCRATCH_CNT                                                   0x318e
+#define mmCPC_EDC_UCODE_CNT                                                     0x318f
+#define mmCPC_EDC_ATC_CNT                                                       0x3190
+#define mmDC_EDC_STATE_CNT                                                      0x3191
+#define mmDC_EDC_CSINVOC_CNT                                                    0x3192
+#define mmDC_EDC_RESTORE_CNT                                                    0x3193
 
 #endif /* GFX_8_0_D_H */
diff --git a/drivers/gpu/drm/amd/include/atombios.h b/drivers/gpu/drm/amd/include/atombios.h
index 552622675ace..eaf451e26643 100644
--- a/drivers/gpu/drm/amd/include/atombios.h
+++ b/drivers/gpu/drm/amd/include/atombios.h
@@ -550,6 +550,13 @@ typedef struct _COMPUTE_MEMORY_CLOCK_PARAM_PARAMETERS_V2_1
 //MPLL_CNTL_FLAG_BYPASS_AD_PLL has a wrong name, should be BYPASS_DQ_PLL
 #define MPLL_CNTL_FLAG_BYPASS_AD_PLL            0x04
 
+// use for ComputeMemoryClockParamTable
+typedef struct _COMPUTE_MEMORY_CLOCK_PARAM_PARAMETERS_V2_2
+{
+  COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V4 ulClock;
+  ULONG ulReserved;
+}COMPUTE_MEMORY_CLOCK_PARAM_PARAMETERS_V2_2;
+
 typedef struct _DYNAMICE_MEMORY_SETTINGS_PARAMETER
 {
   ATOM_COMPUTE_CLOCK_FREQ ulClock;
@@ -4988,6 +4995,78 @@ typedef struct  _ATOM_ASIC_PROFILING_INFO_V3_3
   ULONG  ulSDCMargine;
 }ATOM_ASIC_PROFILING_INFO_V3_3;
 
+// for Fiji speed EVV algorithm
+typedef struct  _ATOM_ASIC_PROFILING_INFO_V3_4
+{
+  ATOM_COMMON_TABLE_HEADER         asHeader;
+  ULONG  ulEvvLkgFactor;
+  ULONG  ulBoardCoreTemp;
+  ULONG  ulMaxVddc;
+  ULONG  ulMinVddc;
+  ULONG  ulLoadLineSlop;
+  ULONG  ulLeakageTemp;
+  ULONG  ulLeakageVoltage;
+  EFUSE_LINEAR_FUNC_PARAM sCACm;
+  EFUSE_LINEAR_FUNC_PARAM sCACb;
+  EFUSE_LOGISTIC_FUNC_PARAM sKt_b;
+  EFUSE_LOGISTIC_FUNC_PARAM sKv_m;
+  EFUSE_LOGISTIC_FUNC_PARAM sKv_b;
+  USHORT usLkgEuseIndex;
+  UCHAR  ucLkgEfuseBitLSB;
+  UCHAR  ucLkgEfuseLength;
+  ULONG  ulLkgEncodeLn_MaxDivMin;
+  ULONG  ulLkgEncodeMax;
+  ULONG  ulLkgEncodeMin;
+  ULONG  ulEfuseLogisticAlpha;
+  USHORT usPowerDpm0;
+  USHORT usPowerDpm1;
+  USHORT usPowerDpm2;
+  USHORT usPowerDpm3;
+  USHORT usPowerDpm4;
+  USHORT usPowerDpm5;
+  USHORT usPowerDpm6;
+  USHORT usPowerDpm7;
+  ULONG  ulTdpDerateDPM0;
+  ULONG  ulTdpDerateDPM1;
+  ULONG  ulTdpDerateDPM2;
+  ULONG  ulTdpDerateDPM3;
+  ULONG  ulTdpDerateDPM4;
+  ULONG  ulTdpDerateDPM5;
+  ULONG  ulTdpDerateDPM6;
+  ULONG  ulTdpDerateDPM7;
+  EFUSE_LINEAR_FUNC_PARAM sRoFuse;
+  ULONG  ulEvvDefaultVddc;
+  ULONG  ulEvvNoCalcVddc;
+  USHORT usParamNegFlag;
+  USHORT usSpeed_Model;
+  ULONG  ulSM_A0;
+  ULONG  ulSM_A1;
+  ULONG  ulSM_A2;
+  ULONG  ulSM_A3;
+  ULONG  ulSM_A4;
+  ULONG  ulSM_A5;
+  ULONG  ulSM_A6;
+  ULONG  ulSM_A7;
+  UCHAR  ucSM_A0_sign;
+  UCHAR  ucSM_A1_sign;
+  UCHAR  ucSM_A2_sign;
+  UCHAR  ucSM_A3_sign;
+  UCHAR  ucSM_A4_sign;
+  UCHAR  ucSM_A5_sign;
+  UCHAR  ucSM_A6_sign;
+  UCHAR  ucSM_A7_sign;
+  ULONG ulMargin_RO_a;
+  ULONG ulMargin_RO_b;
+  ULONG ulMargin_RO_c;
+  ULONG ulMargin_fixed;
+  ULONG ulMargin_Fmax_mean;
+  ULONG ulMargin_plat_mean;
+  ULONG ulMargin_Fmax_sigma;
+  ULONG ulMargin_plat_sigma;
+  ULONG ulMargin_DC_sigma;
+  ULONG ulReserved[8];            // Reserved for future ASIC
+}ATOM_ASIC_PROFILING_INFO_V3_4;
+
 typedef struct _ATOM_POWER_SOURCE_OBJECT
 {
    UCHAR  ucPwrSrcId;                                   // Power source
diff --git a/drivers/gpu/drm/amd/include/cgs_common.h b/drivers/gpu/drm/amd/include/cgs_common.h
index 992dcd8a5c6a..aec38fc3834f 100644
--- a/drivers/gpu/drm/amd/include/cgs_common.h
+++ b/drivers/gpu/drm/amd/include/cgs_common.h
@@ -105,6 +105,36 @@ enum cgs_ucode_id {
 	CGS_UCODE_ID_MAXIMUM,
 };
 
+enum cgs_system_info_id {
+	CGS_SYSTEM_INFO_ADAPTER_BDF_ID = 1,
+	CGS_SYSTEM_INFO_PCIE_GEN_INFO,
+	CGS_SYSTEM_INFO_PCIE_MLW,
+	CGS_SYSTEM_INFO_CG_FLAGS,
+	CGS_SYSTEM_INFO_PG_FLAGS,
+	CGS_SYSTEM_INFO_ID_MAXIMUM,
+};
+
+struct cgs_system_info {
+	uint64_t       size;
+	uint64_t       info_id;
+	union {
+		void           *ptr;
+		uint64_t        value;
+	};
+	uint64_t               padding[13];
+};
+
+/*
+ * enum cgs_resource_type - GPU resource type
+ */
+enum cgs_resource_type {
+	CGS_RESOURCE_TYPE_MMIO = 0,
+	CGS_RESOURCE_TYPE_FB,
+	CGS_RESOURCE_TYPE_IO,
+	CGS_RESOURCE_TYPE_DOORBELL,
+	CGS_RESOURCE_TYPE_ROM,
+};
+
 /**
  * struct cgs_clock_limits - Clock limits
  *
@@ -127,8 +157,53 @@ struct cgs_firmware_info {
 	void			*kptr;
 };
 
+struct cgs_mode_info {
+	uint32_t		refresh_rate;
+	uint32_t		ref_clock;
+	uint32_t		vblank_time_us;
+};
+
+struct cgs_display_info {
+	uint32_t		display_count;
+	uint32_t		active_display_mask;
+	struct cgs_mode_info *mode_info;
+};
+
 typedef unsigned long cgs_handle_t;
 
+#define CGS_ACPI_METHOD_ATCS          0x53435441
+#define CGS_ACPI_METHOD_ATIF          0x46495441
+#define CGS_ACPI_METHOD_ATPX          0x58505441
+#define CGS_ACPI_FIELD_METHOD_NAME                      0x00000001
+#define CGS_ACPI_FIELD_INPUT_ARGUMENT_COUNT             0x00000002
+#define CGS_ACPI_MAX_BUFFER_SIZE     256
+#define CGS_ACPI_TYPE_ANY                      0x00
+#define CGS_ACPI_TYPE_INTEGER               0x01
+#define CGS_ACPI_TYPE_STRING                0x02
+#define CGS_ACPI_TYPE_BUFFER                0x03
+#define CGS_ACPI_TYPE_PACKAGE               0x04
+
+struct cgs_acpi_method_argument {
+	uint32_t type;
+	uint32_t method_length;
+	uint32_t data_length;
+	union{
+		uint32_t value;
+		void *pointer;
+	};
+};
+
+struct cgs_acpi_method_info {
+	uint32_t size;
+	uint32_t field;
+	uint32_t input_count;
+	uint32_t name;
+	struct cgs_acpi_method_argument *pinput_argument;
+	uint32_t output_count;
+	struct cgs_acpi_method_argument *poutput_argument;
+	uint32_t padding[9];
+};
+
 /**
  * cgs_gpu_mem_info() - Return information about memory heaps
  * @cgs_device: opaque device handle
@@ -355,6 +430,23 @@ typedef void (*cgs_write_pci_config_word_t)(void *cgs_device, unsigned addr,
 typedef void (*cgs_write_pci_config_dword_t)(void *cgs_device, unsigned addr,
 					     uint32_t value);
 
+
+/**
+ * cgs_get_pci_resource() - provide access to a device resource (PCI BAR)
+ * @cgs_device:	opaque device handle
+ * @resource_type:	Type of Resource (MMIO, IO, ROM, FB, DOORBELL)
+ * @size:	size of the region
+ * @offset:	offset from the start of the region
+ * @resource_base:	base address (not including offset) returned
+ *
+ * Return: 0 on success, -errno otherwise
+ */
+typedef int (*cgs_get_pci_resource_t)(void *cgs_device,
+				      enum cgs_resource_type resource_type,
+				      uint64_t size,
+				      uint64_t offset,
+				      uint64_t *resource_base);
+
 /**
  * cgs_atom_get_data_table() - Get a pointer to an ATOM BIOS data table
  * @cgs_device:	opaque device handle
@@ -493,6 +585,21 @@ typedef int(*cgs_set_clockgating_state)(void *cgs_device,
 				  enum amd_ip_block_type block_type,
 				  enum amd_clockgating_state state);
 
+typedef int(*cgs_get_active_displays_info)(
+					void *cgs_device,
+					struct cgs_display_info *info);
+
+typedef int (*cgs_call_acpi_method)(void *cgs_device,
+					uint32_t acpi_method,
+					uint32_t acpi_function,
+					void *pinput, void *poutput,
+					uint32_t output_count,
+					uint32_t input_size,
+					uint32_t output_size);
+
+typedef int (*cgs_query_system_info)(void *cgs_device,
+				struct cgs_system_info *sys_info);
+
 struct cgs_ops {
 	/* memory management calls (similar to KFD interface) */
 	cgs_gpu_mem_info_t gpu_mem_info;
@@ -516,6 +623,8 @@ struct cgs_ops {
 	cgs_write_pci_config_byte_t write_pci_config_byte;
 	cgs_write_pci_config_word_t write_pci_config_word;
 	cgs_write_pci_config_dword_t write_pci_config_dword;
+	/* PCI resources */
+	cgs_get_pci_resource_t get_pci_resource;
 	/* ATOM BIOS */
 	cgs_atom_get_data_table_t atom_get_data_table;
 	cgs_atom_get_cmd_table_revs_t atom_get_cmd_table_revs;
@@ -533,7 +642,12 @@ struct cgs_ops {
 	/* cg pg interface*/
 	cgs_set_powergating_state set_powergating_state;
 	cgs_set_clockgating_state set_clockgating_state;
-	/* ACPI (TODO) */
+	/* display manager */
+	cgs_get_active_displays_info get_active_displays_info;
+	/* ACPI */
+	cgs_call_acpi_method call_acpi_method;
+	/* get system info */
+	cgs_query_system_info query_system_info;
 };
 
 struct cgs_os_ops; /* To be define in OS-specific CGS header */
@@ -620,5 +734,15 @@ struct cgs_device
 	CGS_CALL(set_powergating_state, dev, block_type, state)
 #define cgs_set_clockgating_state(dev, block_type, state)	\
 	CGS_CALL(set_clockgating_state, dev, block_type, state)
+#define cgs_get_active_displays_info(dev, info)	\
+	CGS_CALL(get_active_displays_info, dev, info)
+#define cgs_call_acpi_method(dev, acpi_method, acpi_function, pintput, poutput, output_count, input_size, output_size)	\
+	CGS_CALL(call_acpi_method, dev, acpi_method, acpi_function, pintput, poutput, output_count, input_size, output_size)
+#define cgs_query_system_info(dev, sys_info)	\
+	CGS_CALL(query_system_info, dev, sys_info)
+#define cgs_get_pci_resource(cgs_device, resource_type, size, offset, \
+	resource_base) \
+	CGS_CALL(get_pci_resource, cgs_device, resource_type, size, offset, \
+	resource_base)
 
 #endif /* _CGS_COMMON_H */
diff --git a/drivers/gpu/drm/amd/powerplay/Kconfig b/drivers/gpu/drm/amd/powerplay/Kconfig
new file mode 100644
index 000000000000..af380335b425
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/Kconfig
@@ -0,0 +1,6 @@
+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/Makefile b/drivers/gpu/drm/amd/powerplay/Makefile
new file mode 100644
index 000000000000..e195bf59da86
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/Makefile
@@ -0,0 +1,22 @@
+
+subdir-ccflags-y += -Iinclude/drm  \
+		-Idrivers/gpu/drm/amd/powerplay/inc/  \
+		-Idrivers/gpu/drm/amd/include/asic_reg  \
+		-Idrivers/gpu/drm/amd/include  \
+		-Idrivers/gpu/drm/amd/powerplay/smumgr\
+		-Idrivers/gpu/drm/amd/powerplay/hwmgr \
+		-Idrivers/gpu/drm/amd/powerplay/eventmgr
+
+AMD_PP_PATH = ../powerplay
+
+PP_LIBS = smumgr hwmgr eventmgr
+
+AMD_POWERPLAY = $(addsuffix /Makefile,$(addprefix drivers/gpu/drm/amd/powerplay/,$(PP_LIBS)))
+
+include $(AMD_POWERPLAY)
+
+POWER_MGR = amd_powerplay.o
+
+AMD_PP_POWER = $(addprefix $(AMD_PP_PATH)/,$(POWER_MGR))
+
+AMD_POWERPLAY_FILES += $(AMD_PP_POWER)
diff --git a/drivers/gpu/drm/amd/powerplay/amd_powerplay.c b/drivers/gpu/drm/amd/powerplay/amd_powerplay.c
new file mode 100644
index 000000000000..589599f66fcc
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/amd_powerplay.c
@@ -0,0 +1,668 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/gfp.h>
+#include <linux/slab.h>
+#include "amd_shared.h"
+#include "amd_powerplay.h"
+#include "pp_instance.h"
+#include "power_state.h"
+#include "eventmanager.h"
+
+#define PP_CHECK(handle)						\
+	do {								\
+		if ((handle) == NULL || (handle)->pp_valid != PP_VALID)	\
+			return -EINVAL;					\
+	} while (0)
+
+static int pp_early_init(void *handle)
+{
+	return 0;
+}
+
+static int pp_sw_init(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_hwmgr  *hwmgr;
+	int ret = 0;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	hwmgr = pp_handle->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->pptable_func == NULL ||
+	    hwmgr->hwmgr_func == NULL ||
+	    hwmgr->pptable_func->pptable_init == NULL ||
+	    hwmgr->hwmgr_func->backend_init == NULL)
+		return -EINVAL;
+
+	ret = hwmgr->pptable_func->pptable_init(hwmgr);
+
+	if (ret == 0)
+		ret = hwmgr->hwmgr_func->backend_init(hwmgr);
+
+	if (ret)
+		printk("amdgpu: powerplay initialization failed\n");
+	else
+		printk("amdgpu: powerplay initialized\n");
+
+	return ret;
+}
+
+static int pp_sw_fini(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_hwmgr  *hwmgr;
+	int ret = 0;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	hwmgr = pp_handle->hwmgr;
+
+	if (hwmgr != NULL || hwmgr->hwmgr_func != NULL ||
+	    hwmgr->hwmgr_func->backend_fini != NULL)
+		ret = hwmgr->hwmgr_func->backend_fini(hwmgr);
+
+	return ret;
+}
+
+static int pp_hw_init(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_smumgr *smumgr;
+	struct pp_eventmgr *eventmgr;
+	int ret = 0;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	smumgr = pp_handle->smu_mgr;
+
+	if (smumgr == NULL || smumgr->smumgr_funcs == NULL ||
+		smumgr->smumgr_funcs->smu_init == NULL ||
+		smumgr->smumgr_funcs->start_smu == NULL)
+		return -EINVAL;
+
+	ret = smumgr->smumgr_funcs->smu_init(smumgr);
+	if (ret) {
+		printk(KERN_ERR "[ powerplay ] smc initialization failed\n");
+		return ret;
+	}
+
+	ret = smumgr->smumgr_funcs->start_smu(smumgr);
+	if (ret) {
+		printk(KERN_ERR "[ powerplay ] smc start failed\n");
+		smumgr->smumgr_funcs->smu_fini(smumgr);
+		return ret;
+	}
+
+	hw_init_power_state_table(pp_handle->hwmgr);
+	eventmgr = pp_handle->eventmgr;
+
+	if (eventmgr == NULL || eventmgr->pp_eventmgr_init == NULL)
+		return -EINVAL;
+
+	ret = eventmgr->pp_eventmgr_init(eventmgr);
+	return 0;
+}
+
+static int pp_hw_fini(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_smumgr *smumgr;
+	struct pp_eventmgr *eventmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	eventmgr = pp_handle->eventmgr;
+
+	if (eventmgr != NULL || eventmgr->pp_eventmgr_fini != NULL)
+		eventmgr->pp_eventmgr_fini(eventmgr);
+
+	smumgr = pp_handle->smu_mgr;
+
+	if (smumgr != NULL || smumgr->smumgr_funcs != NULL ||
+		smumgr->smumgr_funcs->smu_fini != NULL)
+		smumgr->smumgr_funcs->smu_fini(smumgr);
+
+	return 0;
+}
+
+static bool pp_is_idle(void *handle)
+{
+	return 0;
+}
+
+static int pp_wait_for_idle(void *handle)
+{
+	return 0;
+}
+
+static int pp_sw_reset(void *handle)
+{
+	return 0;
+}
+
+static void pp_print_status(void *handle)
+{
+
+}
+
+static int pp_set_clockgating_state(void *handle,
+				    enum amd_clockgating_state state)
+{
+	return 0;
+}
+
+static int pp_set_powergating_state(void *handle,
+				    enum amd_powergating_state state)
+{
+	return 0;
+}
+
+static int pp_suspend(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_eventmgr *eventmgr;
+	struct pem_event_data event_data = { {0} };
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	eventmgr = pp_handle->eventmgr;
+	pem_handle_event(eventmgr, AMD_PP_EVENT_SUSPEND, &event_data);
+	return 0;
+}
+
+static int pp_resume(void *handle)
+{
+	struct pp_instance *pp_handle;
+	struct pp_eventmgr *eventmgr;
+	struct pem_event_data event_data = { {0} };
+	struct pp_smumgr *smumgr;
+	int ret;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+	smumgr = pp_handle->smu_mgr;
+
+	if (smumgr == NULL || smumgr->smumgr_funcs == NULL ||
+		smumgr->smumgr_funcs->start_smu == NULL)
+		return -EINVAL;
+
+	ret = smumgr->smumgr_funcs->start_smu(smumgr);
+	if (ret) {
+		printk(KERN_ERR "[ powerplay ] smc start failed\n");
+		smumgr->smumgr_funcs->smu_fini(smumgr);
+		return ret;
+	}
+
+	eventmgr = pp_handle->eventmgr;
+	pem_handle_event(eventmgr, AMD_PP_EVENT_RESUME, &event_data);
+
+	return 0;
+}
+
+const struct amd_ip_funcs pp_ip_funcs = {
+	.early_init = pp_early_init,
+	.late_init = NULL,
+	.sw_init = pp_sw_init,
+	.sw_fini = pp_sw_fini,
+	.hw_init = pp_hw_init,
+	.hw_fini = pp_hw_fini,
+	.suspend = pp_suspend,
+	.resume = pp_resume,
+	.is_idle = pp_is_idle,
+	.wait_for_idle = pp_wait_for_idle,
+	.soft_reset = pp_sw_reset,
+	.print_status = pp_print_status,
+	.set_clockgating_state = pp_set_clockgating_state,
+	.set_powergating_state = pp_set_powergating_state,
+};
+
+static int pp_dpm_load_fw(void *handle)
+{
+	return 0;
+}
+
+static int pp_dpm_fw_loading_complete(void *handle)
+{
+	return 0;
+}
+
+static int pp_dpm_force_performance_level(void *handle,
+					enum amd_dpm_forced_level level)
+{
+	struct pp_instance *pp_handle;
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	pp_handle = (struct pp_instance *)handle;
+
+	hwmgr = pp_handle->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	    hwmgr->hwmgr_func->force_dpm_level == NULL)
+		return -EINVAL;
+
+	hwmgr->hwmgr_func->force_dpm_level(hwmgr, level);
+
+	return 0;
+}
+
+static enum amd_dpm_forced_level pp_dpm_get_performance_level(
+								void *handle)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	return (((struct pp_instance *)handle)->hwmgr->dpm_level);
+}
+
+static int pp_dpm_get_sclk(void *handle, bool low)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	    hwmgr->hwmgr_func->get_sclk == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_sclk(hwmgr, low);
+}
+
+static int pp_dpm_get_mclk(void *handle, bool low)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	    hwmgr->hwmgr_func->get_mclk == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_mclk(hwmgr, low);
+}
+
+static int pp_dpm_powergate_vce(void *handle, bool gate)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	    hwmgr->hwmgr_func->powergate_vce == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->powergate_vce(hwmgr, gate);
+}
+
+static int pp_dpm_powergate_uvd(void *handle, bool gate)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	    hwmgr->hwmgr_func->powergate_uvd == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->powergate_uvd(hwmgr, gate);
+}
+
+static enum PP_StateUILabel power_state_convert(enum amd_pm_state_type  state)
+{
+	switch (state) {
+	case POWER_STATE_TYPE_BATTERY:
+		return PP_StateUILabel_Battery;
+	case POWER_STATE_TYPE_BALANCED:
+		return PP_StateUILabel_Balanced;
+	case POWER_STATE_TYPE_PERFORMANCE:
+		return PP_StateUILabel_Performance;
+	default:
+		return PP_StateUILabel_None;
+	}
+}
+
+int pp_dpm_dispatch_tasks(void *handle, enum amd_pp_event event_id, void *input, void *output)
+{
+	int ret = 0;
+	struct pp_instance *pp_handle;
+	struct pem_event_data data = { {0} };
+
+	pp_handle = (struct pp_instance *)handle;
+
+	if (pp_handle == NULL)
+		return -EINVAL;
+
+	switch (event_id) {
+	case AMD_PP_EVENT_DISPLAY_CONFIG_CHANGE:
+		ret = pem_handle_event(pp_handle->eventmgr, event_id, &data);
+		break;
+	case AMD_PP_EVENT_ENABLE_USER_STATE:
+	{
+		enum amd_pm_state_type  ps;
+
+		if (input == NULL)
+			return -EINVAL;
+		ps = *(unsigned long *)input;
+
+		data.requested_ui_label = power_state_convert(ps);
+		ret = pem_handle_event(pp_handle->eventmgr, event_id, &data);
+		break;
+	}
+	case AMD_PP_EVENT_COMPLETE_INIT:
+		ret = pem_handle_event(pp_handle->eventmgr, event_id, &data);
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+enum amd_pm_state_type pp_dpm_get_current_power_state(void *handle)
+{
+	struct pp_hwmgr *hwmgr;
+	struct pp_power_state *state;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->current_ps == NULL)
+		return -EINVAL;
+
+	state = hwmgr->current_ps;
+
+	switch (state->classification.ui_label) {
+	case PP_StateUILabel_Battery:
+		return POWER_STATE_TYPE_BATTERY;
+	case PP_StateUILabel_Balanced:
+		return POWER_STATE_TYPE_BALANCED;
+	case PP_StateUILabel_Performance:
+		return POWER_STATE_TYPE_PERFORMANCE;
+	default:
+		return POWER_STATE_TYPE_DEFAULT;
+	}
+}
+
+static void
+pp_debugfs_print_current_performance_level(void *handle,
+					       struct seq_file *m)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->print_current_perforce_level == NULL)
+		return;
+
+	hwmgr->hwmgr_func->print_current_perforce_level(hwmgr, m);
+}
+
+static int pp_dpm_set_fan_control_mode(void *handle, uint32_t mode)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->set_fan_control_mode == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->set_fan_control_mode(hwmgr, mode);
+}
+
+static int pp_dpm_get_fan_control_mode(void *handle)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->get_fan_control_mode == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_fan_control_mode(hwmgr);
+}
+
+static int pp_dpm_set_fan_speed_percent(void *handle, uint32_t percent)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->set_fan_speed_percent == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->set_fan_speed_percent(hwmgr, percent);
+}
+
+static int pp_dpm_get_fan_speed_percent(void *handle, uint32_t *speed)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->get_fan_speed_percent == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_fan_speed_percent(hwmgr, speed);
+}
+
+static int pp_dpm_get_temperature(void *handle)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	if (hwmgr == NULL || hwmgr->hwmgr_func == NULL ||
+	  hwmgr->hwmgr_func->get_temperature == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_temperature(hwmgr);
+}
+
+const struct amd_powerplay_funcs pp_dpm_funcs = {
+	.get_temperature = pp_dpm_get_temperature,
+	.load_firmware = pp_dpm_load_fw,
+	.wait_for_fw_loading_complete = pp_dpm_fw_loading_complete,
+	.force_performance_level = pp_dpm_force_performance_level,
+	.get_performance_level = pp_dpm_get_performance_level,
+	.get_current_power_state = pp_dpm_get_current_power_state,
+	.get_sclk = pp_dpm_get_sclk,
+	.get_mclk = pp_dpm_get_mclk,
+	.powergate_vce = pp_dpm_powergate_vce,
+	.powergate_uvd = pp_dpm_powergate_uvd,
+	.dispatch_tasks = pp_dpm_dispatch_tasks,
+	.print_current_performance_level = pp_debugfs_print_current_performance_level,
+	.set_fan_control_mode = pp_dpm_set_fan_control_mode,
+	.get_fan_control_mode = pp_dpm_get_fan_control_mode,
+	.set_fan_speed_percent = pp_dpm_set_fan_speed_percent,
+	.get_fan_speed_percent = pp_dpm_get_fan_speed_percent,
+};
+
+static int amd_pp_instance_init(struct amd_pp_init *pp_init,
+				struct amd_powerplay *amd_pp)
+{
+	int ret;
+	struct pp_instance *handle;
+
+	handle = kzalloc(sizeof(struct pp_instance), GFP_KERNEL);
+	if (handle == NULL)
+		return -ENOMEM;
+
+	handle->pp_valid = PP_VALID;
+
+	ret = smum_init(pp_init, handle);
+	if (ret)
+		goto fail_smum;
+
+	ret = hwmgr_init(pp_init, handle);
+	if (ret)
+		goto fail_hwmgr;
+
+	ret = eventmgr_init(handle);
+	if (ret)
+		goto fail_eventmgr;
+
+	amd_pp->pp_handle = handle;
+	return 0;
+
+fail_eventmgr:
+	hwmgr_fini(handle->hwmgr);
+fail_hwmgr:
+	smum_fini(handle->smu_mgr);
+fail_smum:
+	kfree(handle);
+	return ret;
+}
+
+static int amd_pp_instance_fini(void *handle)
+{
+	struct pp_instance *instance = (struct pp_instance *)handle;
+
+	if (instance == NULL)
+		return -EINVAL;
+
+	eventmgr_fini(instance->eventmgr);
+
+	hwmgr_fini(instance->hwmgr);
+
+	smum_fini(instance->smu_mgr);
+
+	kfree(handle);
+	return 0;
+}
+
+int amd_powerplay_init(struct amd_pp_init *pp_init,
+		       struct amd_powerplay *amd_pp)
+{
+	int ret;
+
+	if (pp_init == NULL || amd_pp == NULL)
+		return -EINVAL;
+
+	ret = amd_pp_instance_init(pp_init, amd_pp);
+
+	if (ret)
+		return ret;
+
+	amd_pp->ip_funcs = &pp_ip_funcs;
+	amd_pp->pp_funcs = &pp_dpm_funcs;
+
+	return 0;
+}
+
+int amd_powerplay_fini(void *handle)
+{
+	amd_pp_instance_fini(handle);
+
+	return 0;
+}
+
+/* export this function to DAL */
+
+int amd_powerplay_display_configuration_change(void *handle, const void *input)
+{
+	struct pp_hwmgr  *hwmgr;
+	const struct amd_pp_display_configuration *display_config = input;
+
+	PP_CHECK((struct pp_instance *)handle);
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	phm_store_dal_configuration_data(hwmgr, display_config);
+
+	return 0;
+}
+
+int amd_powerplay_get_display_power_level(void *handle,
+		struct amd_pp_dal_clock_info *output)
+{
+	struct pp_hwmgr  *hwmgr;
+
+	PP_CHECK((struct pp_instance *)handle);
+
+	if (output == NULL)
+		return -EINVAL;
+
+	hwmgr = ((struct pp_instance *)handle)->hwmgr;
+
+	return phm_get_dal_power_level(hwmgr, output);
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/Makefile b/drivers/gpu/drm/amd/powerplay/eventmgr/Makefile
new file mode 100644
index 000000000000..7509e3850087
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/Makefile
@@ -0,0 +1,11 @@
+#
+# Makefile for the 'event manager' sub-component of powerplay.
+# It provides the event management services for the driver.
+
+EVENT_MGR = eventmgr.o eventinit.o eventmanagement.o  \
+		eventactionchains.o eventsubchains.o eventtasks.o psm.o
+
+AMD_PP_EVENT = $(addprefix $(AMD_PP_PATH)/eventmgr/,$(EVENT_MGR))
+
+AMD_POWERPLAY_FILES += $(AMD_PP_EVENT)
+
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c
new file mode 100644
index 000000000000..6b52c78cb404
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.c
@@ -0,0 +1,290 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "eventmgr.h"
+#include "eventactionchains.h"
+#include "eventsubchains.h"
+
+static const pem_event_action *initialize_event[] = {
+	block_adjust_power_state_tasks,
+	power_budget_tasks,
+	system_config_tasks,
+	setup_asic_tasks,
+	enable_dynamic_state_management_tasks,
+	enable_clock_power_gatings_tasks,
+	get_2d_performance_state_tasks,
+	set_performance_state_tasks,
+	initialize_thermal_controller_tasks,
+	conditionally_force_3d_performance_state_tasks,
+	process_vbios_eventinfo_tasks,
+	broadcast_power_policy_tasks,
+	NULL
+};
+
+const struct action_chain initialize_action_chain = {
+	"Initialize",
+	initialize_event
+};
+
+static const pem_event_action *uninitialize_event[] = {
+	ungate_all_display_phys_tasks,
+	uninitialize_display_phy_access_tasks,
+	disable_gfx_voltage_island_power_gating_tasks,
+	disable_gfx_clock_gating_tasks,
+	set_boot_state_tasks,
+	adjust_power_state_tasks,
+	disable_dynamic_state_management_tasks,
+	disable_clock_power_gatings_tasks,
+	cleanup_asic_tasks,
+	prepare_for_pnp_stop_tasks,
+	NULL
+};
+
+const struct action_chain uninitialize_action_chain = {
+	"Uninitialize",
+	uninitialize_event
+};
+
+static const pem_event_action *power_source_change_event_pp_enabled[] = {
+	set_power_source_tasks,
+	set_power_saving_state_tasks,
+	adjust_power_state_tasks,
+	enable_disable_fps_tasks,
+	set_nbmcu_state_tasks,
+	broadcast_power_policy_tasks,
+	NULL
+};
+
+const struct action_chain power_source_change_action_chain_pp_enabled = {
+	"Power source change - PowerPlay enabled",
+	power_source_change_event_pp_enabled
+};
+
+static const pem_event_action *power_source_change_event_pp_disabled[] = {
+	set_power_source_tasks,
+	set_nbmcu_state_tasks,
+	NULL
+};
+
+const struct action_chain power_source_changes_action_chain_pp_disabled = {
+	"Power source change - PowerPlay disabled",
+	power_source_change_event_pp_disabled
+};
+
+static const pem_event_action *power_source_change_event_hardware_dc[] = {
+	set_power_source_tasks,
+	set_power_saving_state_tasks,
+	adjust_power_state_tasks,
+	enable_disable_fps_tasks,
+	reset_hardware_dc_notification_tasks,
+	set_nbmcu_state_tasks,
+	broadcast_power_policy_tasks,
+	NULL
+};
+
+const struct action_chain power_source_change_action_chain_hardware_dc = {
+	"Power source change - with Hardware DC switching",
+	power_source_change_event_hardware_dc
+};
+
+static const pem_event_action *suspend_event[] = {
+	reset_display_phy_access_tasks,
+	unregister_interrupt_tasks,
+	disable_gfx_voltage_island_power_gating_tasks,
+	disable_gfx_clock_gating_tasks,
+	notify_smu_suspend_tasks,
+	disable_smc_firmware_ctf_tasks,
+	set_boot_state_tasks,
+	adjust_power_state_tasks,
+	disable_fps_tasks,
+	vari_bright_suspend_tasks,
+	reset_fan_speed_to_default_tasks,
+	power_down_asic_tasks,
+	disable_stutter_mode_tasks,
+	set_connected_standby_tasks,
+	block_hw_access_tasks,
+	NULL
+};
+
+const struct action_chain suspend_action_chain = {
+	"Suspend",
+	suspend_event
+};
+
+static const pem_event_action *resume_event[] = {
+	unblock_hw_access_tasks,
+	resume_connected_standby_tasks,
+	notify_smu_resume_tasks,
+	reset_display_configCounter_tasks,
+	update_dal_configuration_tasks,
+	vari_bright_resume_tasks,
+	block_adjust_power_state_tasks,
+	setup_asic_tasks,
+	enable_stutter_mode_tasks, /*must do this in boot state and before SMC is started */
+	enable_dynamic_state_management_tasks,
+	enable_clock_power_gatings_tasks,
+	enable_disable_bapm_tasks,
+	initialize_thermal_controller_tasks,
+	reset_boot_state_tasks,
+	adjust_power_state_tasks,
+	enable_disable_fps_tasks,
+	notify_hw_power_source_tasks,
+	process_vbios_event_info_tasks,
+	enable_gfx_clock_gating_tasks,
+	enable_gfx_voltage_island_power_gating_tasks,
+	reset_clock_gating_tasks,
+	notify_smu_vpu_recovery_end_tasks,
+	disable_vpu_cap_tasks,
+	execute_escape_sequence_tasks,
+	NULL
+};
+
+
+const struct action_chain resume_action_chain = {
+	"resume",
+	resume_event
+};
+
+static const pem_event_action *complete_init_event[] = {
+	unblock_adjust_power_state_tasks,
+	adjust_power_state_tasks,
+	enable_gfx_clock_gating_tasks,
+	enable_gfx_voltage_island_power_gating_tasks,
+	notify_power_state_change_tasks,
+	NULL
+};
+
+const struct action_chain complete_init_action_chain = {
+	"complete init",
+	complete_init_event
+};
+
+static const pem_event_action *enable_gfx_clock_gating_event[] = {
+	enable_gfx_clock_gating_tasks,
+	NULL
+};
+
+const struct action_chain enable_gfx_clock_gating_action_chain = {
+	"enable gfx clock gate",
+	enable_gfx_clock_gating_event
+};
+
+static const pem_event_action *disable_gfx_clock_gating_event[] = {
+	disable_gfx_clock_gating_tasks,
+	NULL
+};
+
+const struct action_chain disable_gfx_clock_gating_action_chain = {
+	"disable gfx clock gate",
+	disable_gfx_clock_gating_event
+};
+
+static const pem_event_action *enable_cgpg_event[] = {
+	enable_cgpg_tasks,
+	NULL
+};
+
+const struct action_chain enable_cgpg_action_chain = {
+	"eable cg pg",
+	enable_cgpg_event
+};
+
+static const pem_event_action *disable_cgpg_event[] = {
+	disable_cgpg_tasks,
+	NULL
+};
+
+const struct action_chain disable_cgpg_action_chain = {
+	"disable cg pg",
+	disable_cgpg_event
+};
+
+
+/* Enable user _2d performance and activate */
+
+static const pem_event_action *enable_user_state_event[] = {
+	create_new_user_performance_state_tasks,
+	adjust_power_state_tasks,
+	NULL
+};
+
+const struct action_chain enable_user_state_action_chain = {
+	"Enable user state",
+	enable_user_state_event
+};
+
+static const pem_event_action *enable_user_2d_performance_event[] = {
+	enable_user_2d_performance_tasks,
+	add_user_2d_performance_state_tasks,
+	set_performance_state_tasks,
+	adjust_power_state_tasks,
+	delete_user_2d_performance_state_tasks,
+	NULL
+};
+
+const struct action_chain enable_user_2d_performance_action_chain = {
+	"enable_user_2d_performance_event_activate",
+	enable_user_2d_performance_event
+};
+
+
+static const pem_event_action *disable_user_2d_performance_event[] = {
+	disable_user_2d_performance_tasks,
+	delete_user_2d_performance_state_tasks,
+	NULL
+};
+
+const struct action_chain disable_user_2d_performance_action_chain = {
+	"disable_user_2d_performance_event",
+	disable_user_2d_performance_event
+};
+
+
+static const pem_event_action *display_config_change_event[] = {
+	/* countDisplayConfigurationChangeEventTasks, */
+	unblock_adjust_power_state_tasks,
+	set_cpu_power_state,
+	notify_hw_power_source_tasks,
+	/* updateDALConfigurationTasks,
+	variBrightDisplayConfigurationChangeTasks, */
+	adjust_power_state_tasks,
+	/*enableDisableFPSTasks,
+	setNBMCUStateTasks,
+	notifyPCIEDeviceReadyTasks,*/
+	NULL
+};
+
+const struct action_chain display_config_change_action_chain = {
+	"Display configuration change",
+	display_config_change_event
+};
+
+static const pem_event_action *readjust_power_state_event[] = {
+	adjust_power_state_tasks,
+	NULL
+};
+
+const struct action_chain readjust_power_state_action_chain = {
+	"re-adjust power state",
+	readjust_power_state_event
+};
+
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.h b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.h
new file mode 100644
index 000000000000..f181e53cdcda
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventactionchains.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _EVENT_ACTION_CHAINS_H_
+#define _EVENT_ACTION_CHAINS_H_
+#include "eventmgr.h"
+
+extern const struct action_chain initialize_action_chain;
+
+extern const struct action_chain uninitialize_action_chain;
+
+extern const struct action_chain power_source_change_action_chain_pp_enabled;
+
+extern const struct action_chain power_source_changes_action_chain_pp_disabled;
+
+extern const struct action_chain power_source_change_action_chain_hardware_dc;
+
+extern const struct action_chain suspend_action_chain;
+
+extern const struct action_chain resume_action_chain;
+
+extern const struct action_chain complete_init_action_chain;
+
+extern const struct action_chain enable_gfx_clock_gating_action_chain;
+
+extern const struct action_chain disable_gfx_clock_gating_action_chain;
+
+extern const struct action_chain enable_cgpg_action_chain;
+
+extern const struct action_chain disable_cgpg_action_chain;
+
+extern const struct action_chain enable_user_2d_performance_action_chain;
+
+extern const struct action_chain disable_user_2d_performance_action_chain;
+
+extern const struct action_chain enable_user_state_action_chain;
+
+extern const struct action_chain readjust_power_state_action_chain;
+
+extern const struct action_chain display_config_change_action_chain;
+
+#endif /*_EVENT_ACTION_CHAINS_H_*/
+
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.c
new file mode 100644
index 000000000000..d5ec8ccbe97d
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.c
@@ -0,0 +1,195 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "eventmgr.h"
+#include "eventinit.h"
+#include "ppinterrupt.h"
+#include "hardwaremanager.h"
+
+void pem_init_feature_info(struct pp_eventmgr *eventmgr)
+{
+
+	/* PowerPlay info */
+	eventmgr->ui_state_info[PP_PowerSource_AC].default_ui_lable =
+					    PP_StateUILabel_Performance;
+
+	eventmgr->ui_state_info[PP_PowerSource_AC].current_ui_label =
+					    PP_StateUILabel_Performance;
+
+	eventmgr->ui_state_info[PP_PowerSource_DC].default_ui_lable =
+						  PP_StateUILabel_Battery;
+
+	eventmgr->ui_state_info[PP_PowerSource_DC].current_ui_label =
+						  PP_StateUILabel_Battery;
+
+	if (phm_cap_enabled(eventmgr->platform_descriptor->platformCaps, PHM_PlatformCaps_PowerPlaySupport)) {
+		eventmgr->features[PP_Feature_PowerPlay].supported = true;
+		eventmgr->features[PP_Feature_PowerPlay].version = PEM_CURRENT_POWERPLAY_FEATURE_VERSION;
+		eventmgr->features[PP_Feature_PowerPlay].enabled_default = true;
+		eventmgr->features[PP_Feature_PowerPlay].enabled = true;
+	} else {
+		eventmgr->features[PP_Feature_PowerPlay].supported = false;
+		eventmgr->features[PP_Feature_PowerPlay].enabled = false;
+		eventmgr->features[PP_Feature_PowerPlay].enabled_default = false;
+	}
+
+	eventmgr->features[PP_Feature_Force3DClock].supported = true;
+	eventmgr->features[PP_Feature_Force3DClock].enabled = false;
+	eventmgr->features[PP_Feature_Force3DClock].enabled_default = false;
+	eventmgr->features[PP_Feature_Force3DClock].version = 1;
+
+	/* over drive*/
+	eventmgr->features[PP_Feature_User2DPerformance].version = 4;
+	eventmgr->features[PP_Feature_User3DPerformance].version = 4;
+	eventmgr->features[PP_Feature_OverdriveTest].version = 4;
+
+	eventmgr->features[PP_Feature_OverDrive].version = 4;
+	eventmgr->features[PP_Feature_OverDrive].enabled = false;
+	eventmgr->features[PP_Feature_OverDrive].enabled_default = false;
+
+	eventmgr->features[PP_Feature_User2DPerformance].supported = false;
+	eventmgr->features[PP_Feature_User2DPerformance].enabled = false;
+	eventmgr->features[PP_Feature_User2DPerformance].enabled_default = false;
+
+	eventmgr->features[PP_Feature_User3DPerformance].supported = false;
+	eventmgr->features[PP_Feature_User3DPerformance].enabled = false;
+	eventmgr->features[PP_Feature_User3DPerformance].enabled_default = false;
+
+	eventmgr->features[PP_Feature_OverdriveTest].supported = false;
+	eventmgr->features[PP_Feature_OverdriveTest].enabled = false;
+	eventmgr->features[PP_Feature_OverdriveTest].enabled_default = false;
+
+	eventmgr->features[PP_Feature_OverDrive].supported = false;
+
+	eventmgr->features[PP_Feature_PowerBudgetWaiver].enabled_default = false;
+	eventmgr->features[PP_Feature_PowerBudgetWaiver].version = 1;
+	eventmgr->features[PP_Feature_PowerBudgetWaiver].supported = false;
+	eventmgr->features[PP_Feature_PowerBudgetWaiver].enabled = false;
+
+	/* Multi UVD States support */
+	eventmgr->features[PP_Feature_MultiUVDState].supported = false;
+	eventmgr->features[PP_Feature_MultiUVDState].enabled = false;
+	eventmgr->features[PP_Feature_MultiUVDState].enabled_default = false;
+
+	/* Dynamic UVD States support */
+	eventmgr->features[PP_Feature_DynamicUVDState].supported = false;
+	eventmgr->features[PP_Feature_DynamicUVDState].enabled = false;
+	eventmgr->features[PP_Feature_DynamicUVDState].enabled_default = false;
+
+	/* VCE DPM support */
+	eventmgr->features[PP_Feature_VCEDPM].supported = false;
+	eventmgr->features[PP_Feature_VCEDPM].enabled = false;
+	eventmgr->features[PP_Feature_VCEDPM].enabled_default = false;
+
+	/* ACP PowerGating support */
+	eventmgr->features[PP_Feature_ACP_POWERGATING].supported = false;
+	eventmgr->features[PP_Feature_ACP_POWERGATING].enabled = false;
+	eventmgr->features[PP_Feature_ACP_POWERGATING].enabled_default = false;
+
+	/* PPM support */
+	eventmgr->features[PP_Feature_PPM].version = 1;
+	eventmgr->features[PP_Feature_PPM].supported = false;
+	eventmgr->features[PP_Feature_PPM].enabled = false;
+
+	/* FFC support (enables fan and temp settings, Gemini needs temp settings) */
+	if (phm_cap_enabled(eventmgr->platform_descriptor->platformCaps, PHM_PlatformCaps_ODFuzzyFanControlSupport) ||
+	    phm_cap_enabled(eventmgr->platform_descriptor->platformCaps, PHM_PlatformCaps_GeminiRegulatorFanControlSupport)) {
+		eventmgr->features[PP_Feature_FFC].version = 1;
+		eventmgr->features[PP_Feature_FFC].supported = true;
+		eventmgr->features[PP_Feature_FFC].enabled = true;
+		eventmgr->features[PP_Feature_FFC].enabled_default = true;
+	} else {
+		eventmgr->features[PP_Feature_FFC].supported = false;
+		eventmgr->features[PP_Feature_FFC].enabled = false;
+		eventmgr->features[PP_Feature_FFC].enabled_default = false;
+	}
+
+	eventmgr->features[PP_Feature_VariBright].supported = false;
+	eventmgr->features[PP_Feature_VariBright].enabled = false;
+	eventmgr->features[PP_Feature_VariBright].enabled_default = false;
+
+	eventmgr->features[PP_Feature_BACO].supported = false;
+	eventmgr->features[PP_Feature_BACO].supported = false;
+	eventmgr->features[PP_Feature_BACO].enabled_default = false;
+
+	/* PowerDown feature support */
+	eventmgr->features[PP_Feature_PowerDown].supported = false;
+	eventmgr->features[PP_Feature_PowerDown].enabled = false;
+	eventmgr->features[PP_Feature_PowerDown].enabled_default = false;
+
+	eventmgr->features[PP_Feature_FPS].version = 1;
+	eventmgr->features[PP_Feature_FPS].supported = false;
+	eventmgr->features[PP_Feature_FPS].enabled_default = false;
+	eventmgr->features[PP_Feature_FPS].enabled = false;
+
+	eventmgr->features[PP_Feature_ViPG].version = 1;
+	eventmgr->features[PP_Feature_ViPG].supported = false;
+	eventmgr->features[PP_Feature_ViPG].enabled_default = false;
+	eventmgr->features[PP_Feature_ViPG].enabled = false;
+}
+
+static int thermal_interrupt_callback(void *private_data,
+				      unsigned src_id, const uint32_t *iv_entry)
+{
+	/* TO DO hanle PEM_Event_ThermalNotification (struct pp_eventmgr *)private_data*/
+	printk("current thermal is out of range \n");
+	return 0;
+}
+
+int pem_register_interrupts(struct pp_eventmgr *eventmgr)
+{
+	int result = 0;
+	struct pp_interrupt_registration_info info;
+
+	info.call_back = thermal_interrupt_callback;
+	info.context = eventmgr;
+
+	result = phm_register_thermal_interrupt(eventmgr->hwmgr, &info);
+
+	/* TODO:
+	 * 2. Register CTF event interrupt
+	 * 3. Register for vbios events interrupt
+	 * 4. Register External Throttle Interrupt
+	 * 5. Register Smc To Host Interrupt
+	 * */
+	return result;
+}
+
+
+int pem_unregister_interrupts(struct pp_eventmgr *eventmgr)
+{
+	return 0;
+}
+
+
+void pem_uninit_featureInfo(struct pp_eventmgr *eventmgr)
+{
+	eventmgr->features[PP_Feature_MultiUVDState].supported = false;
+	eventmgr->features[PP_Feature_VariBright].supported = false;
+	eventmgr->features[PP_Feature_PowerBudgetWaiver].supported = false;
+	eventmgr->features[PP_Feature_OverDrive].supported = false;
+	eventmgr->features[PP_Feature_OverdriveTest].supported = false;
+	eventmgr->features[PP_Feature_User3DPerformance].supported = false;
+	eventmgr->features[PP_Feature_User2DPerformance].supported = false;
+	eventmgr->features[PP_Feature_PowerPlay].supported = false;
+	eventmgr->features[PP_Feature_Force3DClock].supported = false;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.h b/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.h
new file mode 100644
index 000000000000..9ef96aab3f24
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventinit.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _EVENTINIT_H_
+#define _EVENTINIT_H_
+
+#define PEM_CURRENT_POWERPLAY_FEATURE_VERSION 4
+
+void pem_init_feature_info(struct pp_eventmgr *eventmgr);
+void pem_uninit_featureInfo(struct pp_eventmgr *eventmgr);
+int pem_register_interrupts(struct pp_eventmgr *eventmgr);
+int pem_unregister_interrupts(struct pp_eventmgr *eventmgr);
+
+#endif /* _EVENTINIT_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.c
new file mode 100644
index 000000000000..1e2ad5603080
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "eventmanagement.h"
+#include "eventmgr.h"
+#include "eventactionchains.h"
+
+int pem_init_event_action_chains(struct pp_eventmgr *eventmgr)
+{
+	int i;
+
+	for (i = 0; i < AMD_PP_EVENT_MAX; i++)
+		eventmgr->event_chain[i] = NULL;
+
+	eventmgr->event_chain[AMD_PP_EVENT_SUSPEND] = pem_get_suspend_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_INITIALIZE] = pem_get_initialize_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_UNINITIALIZE] = pem_get_uninitialize_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_POWER_SOURCE_CHANGE] = pem_get_power_source_change_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_HIBERNATE] = pem_get_hibernate_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_RESUME] = pem_get_resume_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_THERMAL_NOTIFICATION] = pem_get_thermal_notification_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_VBIOS_NOTIFICATION] = pem_get_vbios_notification_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENTER_THERMAL_STATE] = pem_get_enter_thermal_state_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_EXIT_THERMAL_STATE] = pem_get_exit_thermal_state_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENABLE_POWER_PLAY] = pem_get_enable_powerplay_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_DISABLE_POWER_PLAY] = pem_get_disable_powerplay_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENABLE_OVER_DRIVE_TEST] = pem_get_enable_overdrive_test_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_DISABLE_OVER_DRIVE_TEST] = pem_get_disable_overdrive_test_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENABLE_GFX_CLOCK_GATING] = pem_get_enable_gfx_clock_gating_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_DISABLE_GFX_CLOCK_GATING] = pem_get_disable_gfx_clock_gating_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENABLE_CGPG] = pem_get_enable_cgpg_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_DISABLE_CGPG] = pem_get_disable_cgpg_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_COMPLETE_INIT] = pem_get_complete_init_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_SCREEN_ON] = pem_get_screen_on_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_SCREEN_OFF] = pem_get_screen_off_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_PRE_SUSPEND] = pem_get_pre_suspend_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_PRE_RESUME] = pem_get_pre_resume_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_ENABLE_USER_STATE] = pem_enable_user_state_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_READJUST_POWER_STATE] = pem_readjust_power_state_action_chain(eventmgr);
+	eventmgr->event_chain[AMD_PP_EVENT_DISPLAY_CONFIG_CHANGE] = pem_display_config_change_action_chain(eventmgr);
+	return 0;
+}
+
+int pem_excute_event_chain(struct pp_eventmgr *eventmgr, const struct action_chain *event_chain, struct pem_event_data *event_data)
+{
+	const pem_event_action **paction_chain;
+	const pem_event_action *psub_chain;
+	int tmp_result = 0;
+	int result = 0;
+
+	if (eventmgr == NULL || event_chain == NULL || event_data == NULL)
+		return -EINVAL;
+
+	for (paction_chain = event_chain->action_chain; NULL != *paction_chain; paction_chain++) {
+		if (0 != result)
+			return result;
+
+		for (psub_chain = *paction_chain; NULL != *psub_chain; psub_chain++) {
+			tmp_result = (*psub_chain)(eventmgr, event_data);
+			if (0 == result)
+				result = tmp_result;
+		}
+	}
+
+	return result;
+}
+
+const struct action_chain *pem_get_suspend_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &suspend_action_chain;
+}
+
+const struct action_chain *pem_get_initialize_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &initialize_action_chain;
+}
+
+const struct action_chain *pem_get_uninitialize_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &uninitialize_action_chain;
+}
+
+const struct action_chain *pem_get_power_source_change_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &power_source_change_action_chain_pp_enabled;  /* other case base on feature info*/
+}
+
+const struct action_chain *pem_get_resume_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &resume_action_chain;
+}
+
+const struct action_chain *pem_get_hibernate_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_thermal_notification_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_vbios_notification_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_enter_thermal_state_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_exit_thermal_state_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_enable_powerplay_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_disable_powerplay_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_enable_overdrive_test_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_disable_overdrive_test_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_enable_gfx_clock_gating_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &enable_gfx_clock_gating_action_chain;
+}
+
+const struct action_chain *pem_get_disable_gfx_clock_gating_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &disable_gfx_clock_gating_action_chain;
+}
+
+const struct action_chain *pem_get_enable_cgpg_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &enable_cgpg_action_chain;
+}
+
+const struct action_chain *pem_get_disable_cgpg_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &disable_cgpg_action_chain;
+}
+
+const struct action_chain *pem_get_complete_init_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &complete_init_action_chain;
+}
+
+const struct action_chain *pem_get_screen_on_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_screen_off_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_pre_suspend_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_get_pre_resume_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return NULL;
+}
+
+const struct action_chain *pem_enable_user_state_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &enable_user_state_action_chain;
+}
+
+const struct action_chain *pem_readjust_power_state_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &readjust_power_state_action_chain;
+}
+
+const struct action_chain *pem_display_config_change_action_chain(struct pp_eventmgr *eventmgr)
+{
+	return &display_config_change_action_chain;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.h b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.h
new file mode 100644
index 000000000000..383d4b295aa9
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmanagement.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _EVENT_MANAGEMENT_H_
+#define _EVENT_MANAGEMENT_H_
+
+#include "eventmgr.h"
+
+int pem_init_event_action_chains(struct pp_eventmgr *eventmgr);
+int pem_excute_event_chain(struct pp_eventmgr *eventmgr, const struct action_chain *event_chain, struct pem_event_data *event_data);
+const struct action_chain *pem_get_suspend_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_initialize_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_uninitialize_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_power_source_change_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_resume_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_hibernate_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_thermal_notification_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_vbios_notification_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_enter_thermal_state_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_exit_thermal_state_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_enable_powerplay_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_disable_powerplay_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_enable_overdrive_test_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_disable_overdrive_test_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_enable_gfx_clock_gating_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_disable_gfx_clock_gating_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_enable_cgpg_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_disable_cgpg_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_complete_init_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_screen_on_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_screen_off_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_pre_suspend_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_get_pre_resume_action_chain(struct pp_eventmgr *eventmgr);
+
+extern const struct action_chain *pem_enable_user_state_action_chain(struct pp_eventmgr *eventmgr);
+extern const struct action_chain *pem_readjust_power_state_action_chain(struct pp_eventmgr *eventmgr);
+const struct action_chain *pem_display_config_change_action_chain(struct pp_eventmgr *eventmgr);
+
+
+#endif /* _EVENT_MANAGEMENT_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventmgr.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmgr.c
new file mode 100644
index 000000000000..46410e3c7349
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventmgr.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include "eventmgr.h"
+#include "hwmgr.h"
+#include "eventinit.h"
+#include "eventmanagement.h"
+
+static int pem_init(struct pp_eventmgr *eventmgr)
+{
+	int result = 0;
+	struct pem_event_data event_data = { {0} };
+
+	/* Initialize PowerPlay feature info */
+	pem_init_feature_info(eventmgr);
+
+	/* Initialize event action chains */
+	pem_init_event_action_chains(eventmgr);
+
+	/* Call initialization event */
+	result = pem_handle_event(eventmgr, AMD_PP_EVENT_INITIALIZE, &event_data);
+
+	if (0 != result)
+		return result;
+
+	/* Register interrupt callback functions */
+	result = pem_register_interrupts(eventmgr);
+	return 0;
+}
+
+static void pem_fini(struct pp_eventmgr *eventmgr)
+{
+	struct pem_event_data event_data = { {0} };
+
+	pem_uninit_featureInfo(eventmgr);
+	pem_unregister_interrupts(eventmgr);
+
+	pem_handle_event(eventmgr, AMD_PP_EVENT_UNINITIALIZE, &event_data);
+
+	if (eventmgr != NULL)
+		kfree(eventmgr);
+}
+
+int eventmgr_init(struct pp_instance *handle)
+{
+	int result = 0;
+	struct pp_eventmgr *eventmgr;
+
+	if (handle == NULL)
+		return -EINVAL;
+
+	eventmgr = kzalloc(sizeof(struct pp_eventmgr), GFP_KERNEL);
+	if (eventmgr == NULL)
+		return -ENOMEM;
+
+	eventmgr->hwmgr = handle->hwmgr;
+	handle->eventmgr = eventmgr;
+
+	eventmgr->platform_descriptor = &(eventmgr->hwmgr->platform_descriptor);
+	eventmgr->pp_eventmgr_init = pem_init;
+	eventmgr->pp_eventmgr_fini = pem_fini;
+
+	return result;
+}
+
+int eventmgr_fini(struct pp_eventmgr *eventmgr)
+{
+	kfree(eventmgr);
+	return 0;
+}
+
+static int pem_handle_event_unlocked(struct pp_eventmgr *eventmgr, enum amd_pp_event event, struct pem_event_data *data)
+{
+	if (eventmgr == NULL || event >= AMD_PP_EVENT_MAX || data == NULL)
+		return -EINVAL;
+
+	return pem_excute_event_chain(eventmgr, eventmgr->event_chain[event], data);
+}
+
+int pem_handle_event(struct pp_eventmgr *eventmgr, enum amd_pp_event event, struct pem_event_data *event_data)
+{
+	int r = 0;
+
+	r = pem_handle_event_unlocked(eventmgr, event, event_data);
+
+	return r;
+}
+
+bool pem_is_hw_access_blocked(struct pp_eventmgr *eventmgr)
+{
+	return (eventmgr->block_adjust_power_state || phm_is_hw_access_blocked(eventmgr->hwmgr));
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.c
new file mode 100644
index 000000000000..9ef2d90e2886
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.c
@@ -0,0 +1,410 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "eventmgr.h"
+#include "eventsubchains.h"
+#include "eventtasks.h"
+#include "hardwaremanager.h"
+
+const pem_event_action reset_display_phy_access_tasks[] = {
+	pem_task_reset_display_phys_access,
+	NULL
+};
+
+const pem_event_action broadcast_power_policy_tasks[] = {
+	/* PEM_Task_BroadcastPowerPolicyChange, */
+	NULL
+};
+
+const pem_event_action unregister_interrupt_tasks[] = {
+	pem_task_unregister_interrupts,
+	NULL
+};
+
+/* Disable GFX Voltage Islands Power Gating */
+const pem_event_action disable_gfx_voltage_island_powergating_tasks[] = {
+	pem_task_disable_voltage_island_power_gating,
+	NULL
+};
+
+const pem_event_action disable_gfx_clockgating_tasks[] = {
+	pem_task_disable_gfx_clock_gating,
+	NULL
+};
+
+const pem_event_action block_adjust_power_state_tasks[] = {
+	pem_task_block_adjust_power_state,
+	NULL
+};
+
+
+const pem_event_action unblock_adjust_power_state_tasks[] = {
+	pem_task_unblock_adjust_power_state,
+	NULL
+};
+
+const pem_event_action set_performance_state_tasks[] = {
+	pem_task_set_performance_state,
+	NULL
+};
+
+const pem_event_action get_2d_performance_state_tasks[] = {
+	pem_task_get_2D_performance_state_id,
+	NULL
+};
+
+const pem_event_action conditionally_force3D_performance_state_tasks[] = {
+	pem_task_conditionally_force_3d_performance_state,
+	NULL
+};
+
+const pem_event_action process_vbios_eventinfo_tasks[] = {
+	/* PEM_Task_ProcessVbiosEventInfo,*/
+	NULL
+};
+
+const pem_event_action enable_dynamic_state_management_tasks[] = {
+	/* PEM_Task_ResetBAPMPolicyChangedFlag,*/
+	pem_task_get_boot_state_id,
+	pem_task_enable_dynamic_state_management,
+	pem_task_register_interrupts,
+	NULL
+};
+
+const pem_event_action enable_clock_power_gatings_tasks[] = {
+	pem_task_enable_clock_power_gatings_tasks,
+	pem_task_powerdown_uvd_tasks,
+	pem_task_powerdown_vce_tasks,
+	NULL
+};
+
+const pem_event_action setup_asic_tasks[] = {
+	pem_task_setup_asic,
+	NULL
+};
+
+const pem_event_action power_budget_tasks[] = {
+	/* TODO
+	 * PEM_Task_PowerBudgetWaiverAvailable,
+	 * PEM_Task_PowerBudgetWarningMessage,
+	 * PEM_Task_PruneStatesBasedOnPowerBudget,
+	*/
+	NULL
+};
+
+const pem_event_action system_config_tasks[] = {
+	/* PEM_Task_PruneStatesBasedOnSystemConfig,*/
+	NULL
+};
+
+
+const pem_event_action conditionally_force_3d_performance_state_tasks[] = {
+	pem_task_conditionally_force_3d_performance_state,
+	NULL
+};
+
+const pem_event_action ungate_all_display_phys_tasks[] = {
+	/* PEM_Task_GetDisplayPhyAccessInfo */
+	NULL
+};
+
+const pem_event_action uninitialize_display_phy_access_tasks[] = {
+	/* PEM_Task_UninitializeDisplayPhysAccess, */
+	NULL
+};
+
+const pem_event_action disable_gfx_voltage_island_power_gating_tasks[] = {
+	/* PEM_Task_DisableVoltageIslandPowerGating, */
+	NULL
+};
+
+const pem_event_action disable_gfx_clock_gating_tasks[] = {
+	pem_task_disable_gfx_clock_gating,
+	NULL
+};
+
+const pem_event_action set_boot_state_tasks[] = {
+	pem_task_get_boot_state_id,
+	pem_task_set_boot_state,
+	NULL
+};
+
+const pem_event_action adjust_power_state_tasks[] = {
+	pem_task_notify_hw_mgr_display_configuration_change,
+	pem_task_adjust_power_state,
+	pem_task_notify_smc_display_config_after_power_state_adjustment,
+	pem_task_update_allowed_performance_levels,
+	/* to do pem_task_Enable_disable_bapm, */
+	NULL
+};
+
+const pem_event_action disable_dynamic_state_management_tasks[] = {
+	pem_task_unregister_interrupts,
+	pem_task_get_boot_state_id,
+	pem_task_disable_dynamic_state_management,
+	NULL
+};
+
+const pem_event_action disable_clock_power_gatings_tasks[] = {
+	pem_task_disable_clock_power_gatings_tasks,
+	NULL
+};
+
+const pem_event_action cleanup_asic_tasks[] = {
+	/* PEM_Task_DisableFPS,*/
+	pem_task_cleanup_asic,
+	NULL
+};
+
+const pem_event_action prepare_for_pnp_stop_tasks[] = {
+	/* PEM_Task_PrepareForPnpStop,*/
+	NULL
+};
+
+const pem_event_action set_power_source_tasks[] = {
+	pem_task_set_power_source,
+	pem_task_notify_hw_of_power_source,
+	NULL
+};
+
+const pem_event_action set_power_saving_state_tasks[] = {
+	pem_task_reset_power_saving_state,
+	pem_task_get_power_saving_state,
+	pem_task_set_power_saving_state,
+	/* PEM_Task_ResetODDCState,
+	 * PEM_Task_GetODDCState,
+	 * PEM_Task_SetODDCState,*/
+	NULL
+};
+
+const pem_event_action enable_disable_fps_tasks[] = {
+	/* PEM_Task_EnableDisableFPS,*/
+	NULL
+};
+
+const pem_event_action set_nbmcu_state_tasks[] = {
+	/* PEM_Task_NBMCUStateChange,*/
+	NULL
+};
+
+const pem_event_action reset_hardware_dc_notification_tasks[] = {
+	/* PEM_Task_ResetHardwareDCNotification,*/
+	NULL
+};
+
+
+const pem_event_action notify_smu_suspend_tasks[] = {
+	/* PEM_Task_NotifySMUSuspend,*/
+	NULL
+};
+
+const pem_event_action disable_smc_firmware_ctf_tasks[] = {
+	/* PEM_Task_DisableSMCFirmwareCTF,*/
+	NULL
+};
+
+const pem_event_action disable_fps_tasks[] = {
+	/* PEM_Task_DisableFPS,*/
+	NULL
+};
+
+const pem_event_action vari_bright_suspend_tasks[] = {
+	/* PEM_Task_VariBright_Suspend,*/
+	NULL
+};
+
+const pem_event_action reset_fan_speed_to_default_tasks[] = {
+	/* PEM_Task_ResetFanSpeedToDefault,*/
+	NULL
+};
+
+const pem_event_action power_down_asic_tasks[] = {
+	/* PEM_Task_DisableFPS,*/
+	pem_task_power_down_asic,
+	NULL
+};
+
+const pem_event_action disable_stutter_mode_tasks[] = {
+	/* PEM_Task_DisableStutterMode,*/
+	NULL
+};
+
+const pem_event_action set_connected_standby_tasks[] = {
+	/* PEM_Task_SetConnectedStandby,*/
+	NULL
+};
+
+const pem_event_action block_hw_access_tasks[] = {
+	pem_task_block_hw_access,
+	NULL
+};
+
+const pem_event_action unblock_hw_access_tasks[] = {
+	pem_task_un_block_hw_access,
+	NULL
+};
+
+const pem_event_action resume_connected_standby_tasks[] = {
+	/* PEM_Task_ResumeConnectedStandby,*/
+	NULL
+};
+
+const pem_event_action notify_smu_resume_tasks[] = {
+	/* PEM_Task_NotifySMUResume,*/
+	NULL
+};
+
+const pem_event_action reset_display_configCounter_tasks[] = {
+	pem_task_reset_display_phys_access,
+	NULL
+};
+
+const pem_event_action update_dal_configuration_tasks[] = {
+	/* PEM_Task_CheckVBlankTime,*/
+	NULL
+};
+
+const pem_event_action vari_bright_resume_tasks[] = {
+	/* PEM_Task_VariBright_Resume,*/
+	NULL
+};
+
+const pem_event_action notify_hw_power_source_tasks[] = {
+	pem_task_notify_hw_of_power_source,
+	NULL
+};
+
+const pem_event_action process_vbios_event_info_tasks[] = {
+	/* PEM_Task_ProcessVbiosEventInfo,*/
+	NULL
+};
+
+const pem_event_action enable_gfx_clock_gating_tasks[] = {
+	pem_task_enable_gfx_clock_gating,
+	NULL
+};
+
+const pem_event_action enable_gfx_voltage_island_power_gating_tasks[] = {
+	pem_task_enable_voltage_island_power_gating,
+	NULL
+};
+
+const pem_event_action reset_clock_gating_tasks[] = {
+	/* PEM_Task_ResetClockGating*/
+	NULL
+};
+
+const pem_event_action notify_smu_vpu_recovery_end_tasks[] = {
+	/* PEM_Task_NotifySmuVPURecoveryEnd,*/
+	NULL
+};
+
+const pem_event_action disable_vpu_cap_tasks[] = {
+	/* PEM_Task_DisableVPUCap,*/
+	NULL
+};
+
+const pem_event_action execute_escape_sequence_tasks[] = {
+	/* PEM_Task_ExecuteEscapesequence,*/
+	NULL
+};
+
+const pem_event_action notify_power_state_change_tasks[] = {
+	pem_task_notify_power_state_change,
+	NULL
+};
+
+const pem_event_action enable_cgpg_tasks[] = {
+	pem_task_enable_cgpg,
+	NULL
+};
+
+const pem_event_action disable_cgpg_tasks[] = {
+	pem_task_disable_cgpg,
+	NULL
+};
+
+const pem_event_action enable_user_2d_performance_tasks[] = {
+	/* PEM_Task_SetUser2DPerformanceFlag,*/
+	/* PEM_Task_UpdateUser2DPerformanceEnableEvents,*/
+	NULL
+};
+
+const pem_event_action add_user_2d_performance_state_tasks[] = {
+	/* PEM_Task_Get2DPerformanceTemplate,*/
+	/* PEM_Task_AllocateNewPowerStateMemory,*/
+	/* PEM_Task_CopyNewPowerStateInfo,*/
+	/* PEM_Task_UpdateNewPowerStateClocks,*/
+	/* PEM_Task_UpdateNewPowerStateUser2DPerformanceFlag,*/
+	/* PEM_Task_AddPowerState,*/
+	/* PEM_Task_ReleaseNewPowerStateMemory,*/
+	NULL
+};
+
+const pem_event_action delete_user_2d_performance_state_tasks[] = {
+	/* PEM_Task_GetCurrentUser2DPerformanceStateID,*/
+	/* PEM_Task_DeletePowerState,*/
+	/* PEM_Task_SetCurrentUser2DPerformanceStateID,*/
+	NULL
+};
+
+const pem_event_action disable_user_2d_performance_tasks[] = {
+	/* PEM_Task_ResetUser2DPerformanceFlag,*/
+	/* PEM_Task_UpdateUser2DPerformanceDisableEvents,*/
+	NULL
+};
+
+const pem_event_action enable_stutter_mode_tasks[] = {
+	pem_task_enable_stutter_mode,
+	NULL
+};
+
+const pem_event_action enable_disable_bapm_tasks[] = {
+	/*PEM_Task_EnableDisableBAPM,*/
+	NULL
+};
+
+const pem_event_action reset_boot_state_tasks[] = {
+	pem_task_reset_boot_state,
+	NULL
+};
+
+const pem_event_action create_new_user_performance_state_tasks[] = {
+	pem_task_create_user_performance_state,
+	NULL
+};
+
+const pem_event_action initialize_thermal_controller_tasks[] = {
+	pem_task_initialize_thermal_controller,
+	NULL
+};
+
+const pem_event_action uninitialize_thermal_controller_tasks[] = {
+	pem_task_uninitialize_thermal_controller,
+	NULL
+};
+
+const pem_event_action set_cpu_power_state[] = {
+	pem_task_set_cpu_power_state,
+	NULL
+};
\ No newline at end of file
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.h b/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.h
new file mode 100644
index 000000000000..7714cb927428
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventsubchains.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _EVENT_SUB_CHAINS_H_
+#define _EVENT_SUB_CHAINS_H_
+
+#include "eventmgr.h"
+
+extern const pem_event_action reset_display_phy_access_tasks[];
+extern const pem_event_action broadcast_power_policy_tasks[];
+extern const pem_event_action unregister_interrupt_tasks[];
+extern const pem_event_action disable_GFX_voltage_island_powergating_tasks[];
+extern const pem_event_action disable_GFX_clockgating_tasks[];
+extern const pem_event_action block_adjust_power_state_tasks[];
+extern const pem_event_action unblock_adjust_power_state_tasks[];
+extern const pem_event_action set_performance_state_tasks[];
+extern const pem_event_action get_2D_performance_state_tasks[];
+extern const pem_event_action conditionally_force3D_performance_state_tasks[];
+extern const pem_event_action process_vbios_eventinfo_tasks[];
+extern const pem_event_action enable_dynamic_state_management_tasks[];
+extern const pem_event_action enable_clock_power_gatings_tasks[];
+extern const pem_event_action conditionally_force3D_performance_state_tasks[];
+extern const pem_event_action setup_asic_tasks[];
+extern const pem_event_action power_budget_tasks[];
+extern const pem_event_action system_config_tasks[];
+extern const pem_event_action get_2d_performance_state_tasks[];
+extern const pem_event_action conditionally_force_3d_performance_state_tasks[];
+extern const pem_event_action ungate_all_display_phys_tasks[];
+extern const pem_event_action uninitialize_display_phy_access_tasks[];
+extern const pem_event_action disable_gfx_voltage_island_power_gating_tasks[];
+extern const pem_event_action disable_gfx_clock_gating_tasks[];
+extern const pem_event_action set_boot_state_tasks[];
+extern const pem_event_action adjust_power_state_tasks[];
+extern const pem_event_action disable_dynamic_state_management_tasks[];
+extern const pem_event_action disable_clock_power_gatings_tasks[];
+extern const pem_event_action cleanup_asic_tasks[];
+extern const pem_event_action prepare_for_pnp_stop_tasks[];
+extern const pem_event_action set_power_source_tasks[];
+extern const pem_event_action set_power_saving_state_tasks[];
+extern const pem_event_action enable_disable_fps_tasks[];
+extern const pem_event_action set_nbmcu_state_tasks[];
+extern const pem_event_action reset_hardware_dc_notification_tasks[];
+extern const pem_event_action notify_smu_suspend_tasks[];
+extern const pem_event_action disable_smc_firmware_ctf_tasks[];
+extern const pem_event_action disable_fps_tasks[];
+extern const pem_event_action vari_bright_suspend_tasks[];
+extern const pem_event_action reset_fan_speed_to_default_tasks[];
+extern const pem_event_action power_down_asic_tasks[];
+extern const pem_event_action disable_stutter_mode_tasks[];
+extern const pem_event_action set_connected_standby_tasks[];
+extern const pem_event_action block_hw_access_tasks[];
+extern const pem_event_action unblock_hw_access_tasks[];
+extern const pem_event_action resume_connected_standby_tasks[];
+extern const pem_event_action notify_smu_resume_tasks[];
+extern const pem_event_action reset_display_configCounter_tasks[];
+extern const pem_event_action update_dal_configuration_tasks[];
+extern const pem_event_action vari_bright_resume_tasks[];
+extern const pem_event_action notify_hw_power_source_tasks[];
+extern const pem_event_action process_vbios_event_info_tasks[];
+extern const pem_event_action enable_gfx_clock_gating_tasks[];
+extern const pem_event_action enable_gfx_voltage_island_power_gating_tasks[];
+extern const pem_event_action reset_clock_gating_tasks[];
+extern const pem_event_action notify_smu_vpu_recovery_end_tasks[];
+extern const pem_event_action disable_vpu_cap_tasks[];
+extern const pem_event_action execute_escape_sequence_tasks[];
+extern const pem_event_action notify_power_state_change_tasks[];
+extern const pem_event_action enable_cgpg_tasks[];
+extern const pem_event_action disable_cgpg_tasks[];
+extern const pem_event_action enable_user_2d_performance_tasks[];
+extern const pem_event_action add_user_2d_performance_state_tasks[];
+extern const pem_event_action delete_user_2d_performance_state_tasks[];
+extern const pem_event_action disable_user_2d_performance_tasks[];
+extern const pem_event_action enable_stutter_mode_tasks[];
+extern const pem_event_action enable_disable_bapm_tasks[];
+extern const pem_event_action reset_boot_state_tasks[];
+extern const pem_event_action create_new_user_performance_state_tasks[];
+extern const pem_event_action initialize_thermal_controller_tasks[];
+extern const pem_event_action uninitialize_thermal_controller_tasks[];
+extern const pem_event_action set_cpu_power_state[];
+#endif /* _EVENT_SUB_CHAINS_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.c b/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.c
new file mode 100644
index 000000000000..5cd123472db4
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.c
@@ -0,0 +1,438 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "eventmgr.h"
+#include "eventinit.h"
+#include "eventmanagement.h"
+#include "eventmanager.h"
+#include "hardwaremanager.h"
+#include "eventtasks.h"
+#include "power_state.h"
+#include "hwmgr.h"
+#include "amd_powerplay.h"
+#include "psm.h"
+
+#define TEMP_RANGE_MIN (90 * 1000)
+#define TEMP_RANGE_MAX (120 * 1000)
+
+int pem_task_update_allowed_performance_levels(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+
+	if (pem_is_hw_access_blocked(eventmgr))
+		return 0;
+
+	phm_force_dpm_levels(eventmgr->hwmgr, AMD_DPM_FORCED_LEVEL_AUTO);
+
+	return 0;
+}
+
+/* eventtasks_generic.c */
+int pem_task_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	struct pp_hwmgr *hwmgr;
+
+	if (pem_is_hw_access_blocked(eventmgr))
+		return 0;
+
+	hwmgr = eventmgr->hwmgr;
+	if (event_data->pnew_power_state != NULL)
+		hwmgr->request_ps = event_data->pnew_power_state;
+
+	if (phm_cap_enabled(eventmgr->platform_descriptor->platformCaps, PHM_PlatformCaps_DynamicPatchPowerState))
+		psm_adjust_power_state_dynamic(eventmgr, event_data->skip_state_adjust_rules);
+	else
+		psm_adjust_power_state_static(eventmgr, event_data->skip_state_adjust_rules);
+
+	return 0;
+}
+
+int pem_task_power_down_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_power_down_asic(eventmgr->hwmgr);
+}
+
+int pem_task_set_boot_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	if (pem_is_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID))
+		return psm_set_states(eventmgr, &(event_data->requested_state_id));
+
+	return 0;
+}
+
+int pem_task_reset_boot_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_update_new_power_state_clocks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_system_shutdown(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_register_interrupts(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_unregister_interrupts(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return pem_unregister_interrupts(eventmgr);
+}
+
+int pem_task_get_boot_state_id(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	int result;
+
+	result = psm_get_state_by_classification(eventmgr,
+		PP_StateClassificationFlag_Boot,
+		&(event_data->requested_state_id)
+	);
+
+	if (0 == result)
+		pem_set_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID);
+	else
+		pem_unset_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID);
+
+	return result;
+}
+
+int pem_task_enable_dynamic_state_management(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_enable_dynamic_state_management(eventmgr->hwmgr);
+}
+
+int pem_task_disable_dynamic_state_management(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_enable_clock_power_gatings_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_enable_clock_power_gatings(eventmgr->hwmgr);
+}
+
+int pem_task_powerdown_uvd_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_powerdown_uvd(eventmgr->hwmgr);
+}
+
+int pem_task_powerdown_vce_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	phm_powergate_uvd(eventmgr->hwmgr, true);
+	phm_powergate_vce(eventmgr->hwmgr, true);
+	return 0;
+}
+
+int pem_task_disable_clock_power_gatings_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_start_asic_block_usage(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_stop_asic_block_usage(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_setup_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_setup_asic(eventmgr->hwmgr);
+}
+
+int pem_task_cleanup_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_store_dal_configuration(struct pp_eventmgr *eventmgr, const struct amd_display_configuration *display_config)
+{
+	/* TODO */
+	return 0;
+	/*phm_store_dal_configuration_data(eventmgr->hwmgr, display_config) */
+}
+
+int pem_task_notify_hw_mgr_display_configuration_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	if (pem_is_hw_access_blocked(eventmgr))
+		return 0;
+
+	return phm_display_configuration_changed(eventmgr->hwmgr);
+}
+
+int pem_task_notify_hw_mgr_pre_display_configuration_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return 0;
+}
+
+int pem_task_notify_smc_display_config_after_power_state_adjustment(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	if (pem_is_hw_access_blocked(eventmgr))
+		return 0;
+
+	return phm_notify_smc_display_config_after_ps_adjustment(eventmgr->hwmgr);
+}
+
+int pem_task_block_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	eventmgr->block_adjust_power_state = true;
+	/* to do PHM_ResetIPSCounter(pEventMgr->pHwMgr);*/
+	return 0;
+}
+
+int pem_task_unblock_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	eventmgr->block_adjust_power_state = false;
+	return 0;
+}
+
+int pem_task_notify_power_state_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_block_hw_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_un_block_hw_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_reset_display_phys_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_set_cpu_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_set_cpu_power_state(eventmgr->hwmgr);
+}
+
+/*powersaving*/
+
+int pem_task_set_power_source(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_notify_hw_of_power_source(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_get_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_reset_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_set_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_set_screen_state_on(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_set_screen_state_off(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_enable_voltage_island_power_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_disable_voltage_island_power_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_enable_cgpg(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_disable_cgpg(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_enable_clock_power_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+
+int pem_task_enable_gfx_clock_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_disable_gfx_clock_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+
+/* performance */
+int pem_task_set_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	if (pem_is_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID))
+		return psm_set_states(eventmgr, &(event_data->requested_state_id));
+
+	return 0;
+}
+
+int pem_task_conditionally_force_3d_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_enable_stutter_mode(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	/* TODO */
+	return 0;
+}
+
+int pem_task_get_2D_performance_state_id(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	int result;
+
+	if (eventmgr->features[PP_Feature_PowerPlay].supported &&
+		!(eventmgr->features[PP_Feature_PowerPlay].enabled))
+			result = psm_get_state_by_classification(eventmgr,
+					PP_StateClassificationFlag_Boot,
+					&(event_data->requested_state_id));
+	else if (eventmgr->features[PP_Feature_User2DPerformance].enabled)
+			result = psm_get_state_by_classification(eventmgr,
+				   PP_StateClassificationFlag_User2DPerformance,
+					&(event_data->requested_state_id));
+	else
+		result = psm_get_ui_state(eventmgr, PP_StateUILabel_Performance,
+					&(event_data->requested_state_id));
+
+	if (0 == result)
+		pem_set_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID);
+	else
+		pem_unset_event_data_valid(event_data->valid_fields, PEM_EventDataValid_RequestedStateID);
+
+	return result;
+}
+
+int pem_task_create_user_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	struct pp_power_state *state;
+	int table_entries;
+	struct pp_hwmgr *hwmgr = eventmgr->hwmgr;
+	int i;
+
+	table_entries = hwmgr->num_ps;
+	state = hwmgr->ps;
+
+restart_search:
+	for (i = 0; i < table_entries; i++) {
+		if (state->classification.ui_label & event_data->requested_ui_label) {
+			event_data->pnew_power_state = state;
+			return 0;
+		}
+		state = (struct pp_power_state *)((unsigned long)state + hwmgr->ps_size);
+	}
+
+	switch (event_data->requested_ui_label) {
+	case PP_StateUILabel_Battery:
+	case PP_StateUILabel_Balanced:
+		event_data->requested_ui_label = PP_StateUILabel_Performance;
+		goto restart_search;
+	default:
+		break;
+	}
+	return -1;
+}
+
+int pem_task_initialize_thermal_controller(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	struct PP_TemperatureRange range;
+
+	range.max = TEMP_RANGE_MAX;
+	range.min = TEMP_RANGE_MIN;
+
+	if (eventmgr == NULL || eventmgr->platform_descriptor == NULL)
+		return -EINVAL;
+
+	if (phm_cap_enabled(eventmgr->platform_descriptor->platformCaps, PHM_PlatformCaps_ThermalController))
+		return phm_start_thermal_controller(eventmgr->hwmgr, &range);
+
+	return 0;
+}
+
+int pem_task_uninitialize_thermal_controller(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data)
+{
+	return phm_stop_thermal_controller(eventmgr->hwmgr);
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.h b/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.h
new file mode 100644
index 000000000000..6c6297e3b598
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/eventtasks.h
@@ -0,0 +1,88 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _EVENT_TASKS_H_
+#define _EVENT_TASKS_H_
+#include "eventmgr.h"
+
+struct amd_display_configuration;
+
+/* eventtasks_generic.c */
+int pem_task_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_power_down_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_get_boot_state_id(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_set_boot_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_reset_boot_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_update_new_power_state_clocks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_system_shutdown(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_register_interrupts(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_unregister_interrupts(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_dynamic_state_management(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_disable_dynamic_state_management(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_clock_power_gatings_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_powerdown_uvd_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_powerdown_vce_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_disable_clock_power_gatings_tasks(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_start_asic_block_usage(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_stop_asic_block_usage(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_setup_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_cleanup_asic(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_store_dal_configuration (struct pp_eventmgr *eventmgr, const struct amd_display_configuration *display_config);
+int pem_task_notify_hw_mgr_display_configuration_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_notify_hw_mgr_pre_display_configuration_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_block_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_unblock_adjust_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_notify_power_state_change(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_block_hw_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_un_block_hw_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_reset_display_phys_access(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_set_cpu_power_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_notify_smc_display_config_after_power_state_adjustment(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+/*powersaving*/
+
+int pem_task_set_power_source(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_notify_hw_of_power_source(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_get_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_reset_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_set_power_saving_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_set_screen_state_on(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_set_screen_state_off(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_voltage_island_power_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_disable_voltage_island_power_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_cgpg(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_disable_cgpg(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_gfx_clock_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_disable_gfx_clock_gating(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_enable_stutter_mode(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+
+/* performance */
+int pem_task_set_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_conditionally_force_3d_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_get_2D_performance_state_id(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_create_user_performance_state(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_update_allowed_performance_levels(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+/*thermal */
+int pem_task_initialize_thermal_controller(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+int pem_task_uninitialize_thermal_controller(struct pp_eventmgr *eventmgr, struct pem_event_data *event_data);
+
+#endif /* _EVENT_TASKS_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/psm.c b/drivers/gpu/drm/amd/powerplay/eventmgr/psm.c
new file mode 100644
index 000000000000..a46225c0fc01
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/psm.c
@@ -0,0 +1,117 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "psm.h"
+
+int psm_get_ui_state(struct pp_eventmgr *eventmgr, enum PP_StateUILabel ui_label, unsigned long *state_id)
+{
+	struct pp_power_state *state;
+	int table_entries;
+	struct pp_hwmgr *hwmgr = eventmgr->hwmgr;
+	int i;
+
+	table_entries = hwmgr->num_ps;
+	state = hwmgr->ps;
+
+	for (i = 0; i < table_entries; i++) {
+		if (state->classification.ui_label & ui_label) {
+			*state_id = state->id;
+			return 0;
+		}
+		state = (struct pp_power_state *)((unsigned long)state + hwmgr->ps_size);
+	}
+	return -1;
+}
+
+int psm_get_state_by_classification(struct pp_eventmgr *eventmgr, enum PP_StateClassificationFlag flag, unsigned long *state_id)
+{
+	struct pp_power_state *state;
+	int table_entries;
+	struct pp_hwmgr *hwmgr = eventmgr->hwmgr;
+	int i;
+
+	table_entries = hwmgr->num_ps;
+	state = hwmgr->ps;
+
+	for (i = 0; i < table_entries; i++) {
+		if (state->classification.flags & flag) {
+			*state_id = state->id;
+			return 0;
+		}
+		state = (struct pp_power_state *)((unsigned long)state + hwmgr->ps_size);
+	}
+	return -1;
+}
+
+int psm_set_states(struct pp_eventmgr *eventmgr, unsigned long *state_id)
+{
+	struct pp_power_state *state;
+	int table_entries;
+	struct pp_hwmgr *hwmgr = eventmgr->hwmgr;
+	int i;
+
+	table_entries = hwmgr->num_ps;
+	state = hwmgr->ps;
+
+	for (i = 0; i < table_entries; i++) {
+		if (state->id == *state_id) {
+			hwmgr->request_ps = state;
+			return 0;
+		}
+		state = (struct pp_power_state *)((unsigned long)state + hwmgr->ps_size);
+	}
+	return -1;
+}
+
+int psm_adjust_power_state_dynamic(struct pp_eventmgr *eventmgr, bool skip)
+{
+
+	struct pp_power_state *pcurrent;
+	struct pp_power_state *requested;
+	struct pp_hwmgr *hwmgr;
+	bool equal;
+
+	if (skip)
+		return 0;
+
+	hwmgr = eventmgr->hwmgr;
+	pcurrent = hwmgr->current_ps;
+	requested = hwmgr->request_ps;
+
+	if (requested == NULL)
+		return 0;
+
+	if (pcurrent == NULL || (0 != phm_check_states_equal(hwmgr, &pcurrent->hardware, &requested->hardware, &equal)))
+		equal = false;
+
+	if (!equal || phm_check_smc_update_required_for_display_configuration(hwmgr)) {
+		phm_apply_state_adjust_rules(hwmgr, requested, pcurrent);
+		phm_set_power_state(hwmgr, &pcurrent->hardware, &requested->hardware);
+		hwmgr->current_ps = requested;
+	}
+	return 0;
+}
+
+int psm_adjust_power_state_static(struct pp_eventmgr *eventmgr, bool skip)
+{
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/eventmgr/psm.h b/drivers/gpu/drm/amd/powerplay/eventmgr/psm.h
new file mode 100644
index 000000000000..fbdff3e02aa3
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/eventmgr/psm.h
@@ -0,0 +1,38 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "eventmgr.h"
+#include "eventinit.h"
+#include "eventmanagement.h"
+#include "eventmanager.h"
+#include "power_state.h"
+#include "hardwaremanager.h"
+
+int psm_get_ui_state(struct pp_eventmgr *eventmgr, enum PP_StateUILabel ui_label, unsigned long *state_id);
+
+int psm_get_state_by_classification(struct pp_eventmgr *eventmgr, enum PP_StateClassificationFlag flag, unsigned long *state_id);
+
+int psm_set_states(struct pp_eventmgr *eventmgr, unsigned long *state_id);
+
+int psm_adjust_power_state_dynamic(struct pp_eventmgr *eventmgr, bool skip);
+
+int psm_adjust_power_state_static(struct pp_eventmgr *eventmgr, bool skip);
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/Makefile b/drivers/gpu/drm/amd/powerplay/hwmgr/Makefile
new file mode 100644
index 000000000000..b664e34dbcc0
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/Makefile
@@ -0,0 +1,15 @@
+#
+# Makefile for the 'hw manager' sub-component of powerplay.
+# It provides the hardware management services for the driver.
+
+HARDWARE_MGR = hwmgr.o processpptables.o functiontables.o \
+	       hardwaremanager.o pp_acpi.o cz_hwmgr.o \
+               cz_clockpowergating.o \
+	       tonga_processpptables.o ppatomctrl.o \
+               tonga_hwmgr.o pppcielanes.o  tonga_thermal.o\
+               fiji_powertune.o fiji_hwmgr.o tonga_clockpowergating.o \
+               fiji_clockpowergating.o fiji_thermal.o
+
+AMD_PP_HWMGR = $(addprefix $(AMD_PP_PATH)/hwmgr/,$(HARDWARE_MGR))
+
+AMD_POWERPLAY_FILES += $(AMD_PP_HWMGR)
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.c b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.c
new file mode 100644
index 000000000000..ff08ce41bde9
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.c
@@ -0,0 +1,252 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "hwmgr.h"
+#include "cz_clockpowergating.h"
+#include "cz_ppsmc.h"
+
+/* PhyID -> Status Mapping in DDI_PHY_GEN_STATUS
+    0    GFX0L (3:0),                  (27:24),
+    1    GFX0H (7:4),                  (31:28),
+    2    GFX1L (3:0),                  (19:16),
+    3    GFX1H (7:4),                  (23:20),
+    4    DDIL   (3:0),                   (11: 8),
+    5    DDIH  (7:4),                   (15:12),
+    6    DDI2L (3:0),                   ( 3: 0),
+    7    DDI2H (7:4),                   ( 7: 4),
+*/
+#define DDI_PHY_GEN_STATUS_VAL(phyID)   (1 << ((3 - ((phyID & 0x07)/2))*8 + (phyID & 0x01)*4))
+#define IS_PHY_ID_USED_BY_PLL(PhyID)    (((0xF3 & (1 << PhyID)) & 0xFF) ? true : false)
+
+
+int cz_phm_set_asic_block_gating(struct pp_hwmgr *hwmgr, enum PHM_AsicBlock block, enum PHM_ClockGateSetting gating)
+{
+	int ret = 0;
+
+	switch (block) {
+	case PHM_AsicBlock_UVD_MVC:
+	case PHM_AsicBlock_UVD:
+	case PHM_AsicBlock_UVD_HD:
+	case PHM_AsicBlock_UVD_SD:
+		if (gating == PHM_ClockGateSetting_StaticOff)
+			ret = cz_dpm_powerdown_uvd(hwmgr);
+		else
+			ret = cz_dpm_powerup_uvd(hwmgr);
+		break;
+	case PHM_AsicBlock_GFX:
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+
+bool cz_phm_is_safe_for_asic_block(struct pp_hwmgr *hwmgr, const struct pp_hw_power_state *state, enum PHM_AsicBlock block)
+{
+	return true;
+}
+
+
+int cz_phm_enable_disable_gfx_power_gating(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return 0;
+}
+
+int cz_phm_smu_power_up_down_pcie(struct pp_hwmgr *hwmgr, uint32_t target, bool up, uint32_t args)
+{
+	/* TODO */
+	return 0;
+}
+
+int cz_phm_initialize_display_phy_access(struct pp_hwmgr *hwmgr, bool initialize, bool accesshw)
+{
+	/* TODO */
+	return 0;
+}
+
+int cz_phm_get_display_phy_access_info(struct pp_hwmgr *hwmgr)
+{
+	/* TODO */
+	return 0;
+}
+
+int cz_phm_gate_unused_display_phys(struct pp_hwmgr *hwmgr)
+{
+	/* TODO */
+	return 0;
+}
+
+int cz_phm_ungate_all_display_phys(struct pp_hwmgr *hwmgr)
+{
+	/* TODO */
+	return 0;
+}
+
+static int cz_tf_uvd_power_gating_initialize(struct pp_hwmgr *hwmgr, void *pInput, void *pOutput, void *pStorage, int Result)
+{
+	return 0;
+}
+
+static int cz_tf_vce_power_gating_initialize(struct pp_hwmgr *hwmgr, void *pInput, void *pOutput, void *pStorage, int Result)
+{
+	return 0;
+}
+
+int cz_enable_disable_uvd_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	uint32_t dpm_features = 0;
+
+	if (enable &&
+		phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				  PHM_PlatformCaps_UVDDPM)) {
+		cz_hwmgr->dpm_flags |= DPMFlags_UVD_Enabled;
+		dpm_features |= UVD_DPM_MASK;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			    PPSMC_MSG_EnableAllSmuFeatures, dpm_features);
+	} else {
+		dpm_features |= UVD_DPM_MASK;
+		cz_hwmgr->dpm_flags &= ~DPMFlags_UVD_Enabled;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			   PPSMC_MSG_DisableAllSmuFeatures, dpm_features);
+	}
+	return 0;
+}
+
+int cz_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	uint32_t dpm_features = 0;
+
+	if (enable && phm_cap_enabled(
+				hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_VCEDPM)) {
+		cz_hwmgr->dpm_flags |= DPMFlags_VCE_Enabled;
+		dpm_features |= VCE_DPM_MASK;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			    PPSMC_MSG_EnableAllSmuFeatures, dpm_features);
+	} else {
+		dpm_features |= VCE_DPM_MASK;
+		cz_hwmgr->dpm_flags &= ~DPMFlags_VCE_Enabled;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			   PPSMC_MSG_DisableAllSmuFeatures, dpm_features);
+	}
+
+	return 0;
+}
+
+
+int cz_dpm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (cz_hwmgr->uvd_power_gated == bgate)
+		return 0;
+
+	cz_hwmgr->uvd_power_gated = bgate;
+
+	if (bgate) {
+		cgs_set_clockgating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_CG_STATE_UNGATE);
+		cgs_set_powergating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_PG_STATE_GATE);
+		cz_dpm_update_uvd_dpm(hwmgr, true);
+		cz_dpm_powerdown_uvd(hwmgr);
+	} else {
+		cz_dpm_powerup_uvd(hwmgr);
+		cgs_set_clockgating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_PG_STATE_GATE);
+		cgs_set_powergating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_CG_STATE_UNGATE);
+		cz_dpm_update_uvd_dpm(hwmgr, false);
+	}
+
+	return 0;
+}
+
+int cz_dpm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_VCEPowerGating)) {
+		if (cz_hwmgr->vce_power_gated != bgate) {
+			if (bgate) {
+				cgs_set_clockgating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_CG_STATE_UNGATE);
+				cgs_set_powergating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_PG_STATE_GATE);
+				cz_enable_disable_vce_dpm(hwmgr, false);
+			/* TODO: to figure out why vce can't be poweroff*/
+				cz_hwmgr->vce_power_gated = true;
+			} else {
+				cz_dpm_powerup_vce(hwmgr);
+				cz_hwmgr->vce_power_gated = false;
+				cgs_set_clockgating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_PG_STATE_GATE);
+				cgs_set_powergating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_CG_STATE_UNGATE);
+				cz_dpm_update_vce_dpm(hwmgr);
+				cz_enable_disable_vce_dpm(hwmgr, true);
+				return 0;
+			}
+		}
+	} else {
+		cz_dpm_update_vce_dpm(hwmgr);
+		cz_enable_disable_vce_dpm(hwmgr, !bgate);
+		return 0;
+	}
+
+	if (!cz_hwmgr->vce_power_gated)
+		cz_dpm_update_vce_dpm(hwmgr);
+
+	return 0;
+}
+
+
+static struct phm_master_table_item cz_enable_clock_power_gatings_list[] = {
+	/*we don't need an exit table here, because there is only D3 cold on Kv*/
+	{ phm_cf_want_uvd_power_gating, cz_tf_uvd_power_gating_initialize },
+	{ phm_cf_want_vce_power_gating, cz_tf_vce_power_gating_initialize },
+	/* to do { NULL, cz_tf_xdma_power_gating_enable }, */
+	{ NULL, NULL }
+};
+
+struct phm_master_table_header cz_phm_enable_clock_power_gatings_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_enable_clock_power_gatings_list
+};
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.h b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.h
new file mode 100644
index 000000000000..bbbc0571320e
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_clockpowergating.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _CZ_CLOCK_POWER_GATING_H_
+#define _CZ_CLOCK_POWER_GATING_H_
+
+#include "cz_hwmgr.h"
+#include "pp_asicblocks.h"
+
+extern int cz_phm_set_asic_block_gating(struct pp_hwmgr *hwmgr, enum PHM_AsicBlock block, enum PHM_ClockGateSetting gating);
+extern struct phm_master_table_header cz_phm_enable_clock_power_gatings_master;
+extern struct phm_master_table_header cz_phm_disable_clock_power_gatings_master;
+extern int cz_dpm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate);
+extern int cz_dpm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate);
+extern int cz_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable);
+extern int cz_enable_disable_uvd_dpm(struct pp_hwmgr *hwmgr, bool enable);
+#endif /* _CZ_CLOCK_POWER_GATING_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c
new file mode 100644
index 000000000000..cf01177ca3b5
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.c
@@ -0,0 +1,1755 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include "atom-types.h"
+#include "atombios.h"
+#include "processpptables.h"
+#include "pp_debug.h"
+#include "cgs_common.h"
+#include "smu/smu_8_0_d.h"
+#include "smu8_fusion.h"
+#include "smu/smu_8_0_sh_mask.h"
+#include "smumgr.h"
+#include "hwmgr.h"
+#include "hardwaremanager.h"
+#include "cz_ppsmc.h"
+#include "cz_hwmgr.h"
+#include "power_state.h"
+#include "cz_clockpowergating.h"
+#include "pp_debug.h"
+
+#define ixSMUSVI_NB_CURRENTVID 0xD8230044
+#define CURRENT_NB_VID_MASK 0xff000000
+#define CURRENT_NB_VID__SHIFT 24
+#define ixSMUSVI_GFX_CURRENTVID  0xD8230048
+#define CURRENT_GFX_VID_MASK 0xff000000
+#define CURRENT_GFX_VID__SHIFT 24
+
+static const unsigned long PhwCz_Magic = (unsigned long) PHM_Cz_Magic;
+
+static struct cz_power_state *cast_PhwCzPowerState(struct pp_hw_power_state *hw_ps)
+{
+	if (PhwCz_Magic != hw_ps->magic)
+		return NULL;
+
+	return (struct cz_power_state *)hw_ps;
+}
+
+static const struct cz_power_state *cast_const_PhwCzPowerState(
+				const struct pp_hw_power_state *hw_ps)
+{
+	if (PhwCz_Magic != hw_ps->magic)
+		return NULL;
+
+	return (struct cz_power_state *)hw_ps;
+}
+
+uint32_t cz_get_eclk_level(struct pp_hwmgr *hwmgr,
+					uint32_t clock, uint32_t msg)
+{
+	int i = 0;
+	struct phm_vce_clock_voltage_dependency_table *ptable =
+		hwmgr->dyn_state.vce_clock_voltage_dependency_table;
+
+	switch (msg) {
+	case PPSMC_MSG_SetEclkSoftMin:
+	case PPSMC_MSG_SetEclkHardMin:
+		for (i = 0; i < (int)ptable->count; i++) {
+			if (clock <= ptable->entries[i].ecclk)
+				break;
+		}
+		break;
+
+	case PPSMC_MSG_SetEclkSoftMax:
+	case PPSMC_MSG_SetEclkHardMax:
+		for (i = ptable->count - 1; i >= 0; i--) {
+			if (clock >= ptable->entries[i].ecclk)
+				break;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return i;
+}
+
+static uint32_t cz_get_sclk_level(struct pp_hwmgr *hwmgr,
+				uint32_t clock, uint32_t msg)
+{
+	int i = 0;
+	struct phm_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.vddc_dependency_on_sclk;
+
+	switch (msg) {
+	case PPSMC_MSG_SetSclkSoftMin:
+	case PPSMC_MSG_SetSclkHardMin:
+		for (i = 0; i < (int)table->count; i++) {
+			if (clock <= table->entries[i].clk)
+				break;
+		}
+		break;
+
+	case PPSMC_MSG_SetSclkSoftMax:
+	case PPSMC_MSG_SetSclkHardMax:
+		for (i = table->count - 1; i >= 0; i--) {
+			if (clock >= table->entries[i].clk)
+				break;
+		}
+		break;
+
+	default:
+		break;
+	}
+	return i;
+}
+
+static uint32_t cz_get_uvd_level(struct pp_hwmgr *hwmgr,
+					uint32_t clock, uint32_t msg)
+{
+	int i = 0;
+	struct phm_uvd_clock_voltage_dependency_table *ptable =
+		hwmgr->dyn_state.uvd_clock_voltage_dependency_table;
+
+	switch (msg) {
+	case PPSMC_MSG_SetUvdSoftMin:
+	case PPSMC_MSG_SetUvdHardMin:
+		for (i = 0; i < (int)ptable->count; i++) {
+			if (clock <= ptable->entries[i].vclk)
+				break;
+		}
+		break;
+
+	case PPSMC_MSG_SetUvdSoftMax:
+	case PPSMC_MSG_SetUvdHardMax:
+		for (i = ptable->count - 1; i >= 0; i--) {
+			if (clock >= ptable->entries[i].vclk)
+				break;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return i;
+}
+
+static uint32_t cz_get_max_sclk_level(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (cz_hwmgr->max_sclk_level == 0) {
+		smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_GetMaxSclkLevel);
+		cz_hwmgr->max_sclk_level = smum_get_argument(hwmgr->smumgr) + 1;
+	}
+
+	return cz_hwmgr->max_sclk_level;
+}
+
+static int cz_initialize_dpm_defaults(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	uint32_t i;
+	struct cgs_system_info sys_info = {0};
+	int result;
+
+	cz_hwmgr->gfx_ramp_step = 256*25/100;
+
+	cz_hwmgr->gfx_ramp_delay = 1; /* by default, we delay 1us */
+
+	for (i = 0; i < CZ_MAX_HARDWARE_POWERLEVELS; i++)
+		cz_hwmgr->activity_target[i] = CZ_AT_DFLT;
+
+	cz_hwmgr->mgcg_cgtt_local0 = 0x00000000;
+	cz_hwmgr->mgcg_cgtt_local1 = 0x00000000;
+
+	cz_hwmgr->clock_slow_down_freq = 25000;
+
+	cz_hwmgr->skip_clock_slow_down = 1;
+
+	cz_hwmgr->enable_nb_ps_policy = 1; /* disable until UNB is ready, Enabled */
+
+	cz_hwmgr->voltage_drop_in_dce_power_gating = 0; /* disable until fully verified */
+
+	cz_hwmgr->voting_rights_clients = 0x00C00033;
+
+	cz_hwmgr->static_screen_threshold = 8;
+
+	cz_hwmgr->ddi_power_gating_disabled = 0;
+
+	cz_hwmgr->bapm_enabled = 1;
+
+	cz_hwmgr->voltage_drop_threshold = 0;
+
+	cz_hwmgr->gfx_power_gating_threshold = 500;
+
+	cz_hwmgr->vce_slow_sclk_threshold = 20000;
+
+	cz_hwmgr->dce_slow_sclk_threshold = 30000;
+
+	cz_hwmgr->disable_driver_thermal_policy = 1;
+
+	cz_hwmgr->disable_nb_ps3_in_battery = 0;
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+							PHM_PlatformCaps_ABM);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				    PHM_PlatformCaps_NonABMSupportInPPLib);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+					   PHM_PlatformCaps_SclkDeepSleep);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_DynamicM3Arbiter);
+
+	cz_hwmgr->override_dynamic_mgpg = 1;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				  PHM_PlatformCaps_DynamicPatchPowerState);
+
+	cz_hwmgr->thermal_auto_throttling_treshold = 0;
+
+	cz_hwmgr->tdr_clock = 0;
+
+	cz_hwmgr->disable_gfx_power_gating_in_uvd = 0;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_DynamicUVDState);
+
+	cz_hwmgr->cc6_settings.cpu_cc6_disable = false;
+	cz_hwmgr->cc6_settings.cpu_pstate_disable = false;
+	cz_hwmgr->cc6_settings.nb_pstate_switch_disable = false;
+	cz_hwmgr->cc6_settings.cpu_pstate_separation_time = 0;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				   PHM_PlatformCaps_DisableVoltageIsland);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		      PHM_PlatformCaps_UVDPowerGating);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		      PHM_PlatformCaps_VCEPowerGating);
+	sys_info.size = sizeof(struct cgs_system_info);
+	sys_info.info_id = CGS_SYSTEM_INFO_PG_FLAGS;
+	result = cgs_query_system_info(hwmgr->device, &sys_info);
+	if (!result) {
+		if (sys_info.value & AMD_PG_SUPPORT_UVD)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				      PHM_PlatformCaps_UVDPowerGating);
+		if (sys_info.value & AMD_PG_SUPPORT_VCE)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				      PHM_PlatformCaps_VCEPowerGating);
+	}
+
+	return 0;
+}
+
+static uint32_t cz_convert_8Bit_index_to_voltage(
+			struct pp_hwmgr *hwmgr, uint16_t voltage)
+{
+	return 6200 - (voltage * 25);
+}
+
+static int cz_construct_max_power_limits_table(struct pp_hwmgr *hwmgr,
+			struct phm_clock_and_voltage_limits *table)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)hwmgr->backend;
+	struct cz_sys_info *sys_info = &cz_hwmgr->sys_info;
+	struct phm_clock_voltage_dependency_table *dep_table =
+				hwmgr->dyn_state.vddc_dependency_on_sclk;
+
+	if (dep_table->count > 0) {
+		table->sclk = dep_table->entries[dep_table->count-1].clk;
+		table->vddc = cz_convert_8Bit_index_to_voltage(hwmgr,
+		   (uint16_t)dep_table->entries[dep_table->count-1].v);
+	}
+	table->mclk = sys_info->nbp_memory_clock[0];
+	return 0;
+}
+
+static int cz_init_dynamic_state_adjustment_rule_settings(
+			struct pp_hwmgr *hwmgr,
+			ATOM_CLK_VOLT_CAPABILITY *disp_voltage_table)
+{
+	uint32_t table_size =
+		sizeof(struct phm_clock_voltage_dependency_table) +
+		(7 * sizeof(struct phm_clock_voltage_dependency_record));
+
+	struct phm_clock_voltage_dependency_table *table_clk_vlt =
+					kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table_clk_vlt) {
+		printk(KERN_ERR "[ powerplay ] Can not allocate memory!\n");
+		return -ENOMEM;
+	}
+
+	table_clk_vlt->count = 8;
+	table_clk_vlt->entries[0].clk = PP_DAL_POWERLEVEL_0;
+	table_clk_vlt->entries[0].v = 0;
+	table_clk_vlt->entries[1].clk = PP_DAL_POWERLEVEL_1;
+	table_clk_vlt->entries[1].v = 1;
+	table_clk_vlt->entries[2].clk = PP_DAL_POWERLEVEL_2;
+	table_clk_vlt->entries[2].v = 2;
+	table_clk_vlt->entries[3].clk = PP_DAL_POWERLEVEL_3;
+	table_clk_vlt->entries[3].v = 3;
+	table_clk_vlt->entries[4].clk = PP_DAL_POWERLEVEL_4;
+	table_clk_vlt->entries[4].v = 4;
+	table_clk_vlt->entries[5].clk = PP_DAL_POWERLEVEL_5;
+	table_clk_vlt->entries[5].v = 5;
+	table_clk_vlt->entries[6].clk = PP_DAL_POWERLEVEL_6;
+	table_clk_vlt->entries[6].v = 6;
+	table_clk_vlt->entries[7].clk = PP_DAL_POWERLEVEL_7;
+	table_clk_vlt->entries[7].v = 7;
+	hwmgr->dyn_state.vddc_dep_on_dal_pwrl = table_clk_vlt;
+
+	return 0;
+}
+
+static int cz_get_system_info_data(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)hwmgr->backend;
+	ATOM_INTEGRATED_SYSTEM_INFO_V1_9 *info = NULL;
+	uint32_t i;
+	int result = 0;
+	uint8_t frev, crev;
+	uint16_t size;
+
+	info = (ATOM_INTEGRATED_SYSTEM_INFO_V1_9 *) cgs_atom_get_data_table(
+			hwmgr->device,
+			GetIndexIntoMasterTable(DATA, IntegratedSystemInfo),
+			&size, &frev, &crev);
+
+	if (crev != 9) {
+		printk(KERN_ERR "[ powerplay ] Unsupported IGP table: %d %d\n", frev, crev);
+		return -EINVAL;
+	}
+
+	if (info == NULL) {
+		printk(KERN_ERR "[ powerplay ] Could not retrieve the Integrated System Info Table!\n");
+		return -EINVAL;
+	}
+
+	cz_hwmgr->sys_info.bootup_uma_clock =
+				   le32_to_cpu(info->ulBootUpUMAClock);
+
+	cz_hwmgr->sys_info.bootup_engine_clock =
+				le32_to_cpu(info->ulBootUpEngineClock);
+
+	cz_hwmgr->sys_info.dentist_vco_freq =
+				   le32_to_cpu(info->ulDentistVCOFreq);
+
+	cz_hwmgr->sys_info.system_config =
+				     le32_to_cpu(info->ulSystemConfig);
+
+	cz_hwmgr->sys_info.bootup_nb_voltage_index =
+				  le16_to_cpu(info->usBootUpNBVoltage);
+
+	cz_hwmgr->sys_info.htc_hyst_lmt =
+			(info->ucHtcHystLmt == 0) ? 5 : info->ucHtcHystLmt;
+
+	cz_hwmgr->sys_info.htc_tmp_lmt =
+			(info->ucHtcTmpLmt == 0) ? 203 : info->ucHtcTmpLmt;
+
+	if (cz_hwmgr->sys_info.htc_tmp_lmt <=
+			cz_hwmgr->sys_info.htc_hyst_lmt) {
+		printk(KERN_ERR "[ powerplay ] The htcTmpLmt should be larger than htcHystLmt.\n");
+		return -EINVAL;
+	}
+
+	cz_hwmgr->sys_info.nb_dpm_enable =
+				cz_hwmgr->enable_nb_ps_policy &&
+				(le32_to_cpu(info->ulSystemConfig) >> 3 & 0x1);
+
+	for (i = 0; i < CZ_NUM_NBPSTATES; i++) {
+		if (i < CZ_NUM_NBPMEMORYCLOCK) {
+			cz_hwmgr->sys_info.nbp_memory_clock[i] =
+			  le32_to_cpu(info->ulNbpStateMemclkFreq[i]);
+		}
+		cz_hwmgr->sys_info.nbp_n_clock[i] =
+			    le32_to_cpu(info->ulNbpStateNClkFreq[i]);
+	}
+
+	for (i = 0; i < MAX_DISPLAY_CLOCK_LEVEL; i++) {
+		cz_hwmgr->sys_info.display_clock[i] =
+					le32_to_cpu(info->sDispClkVoltageMapping[i].ulMaximumSupportedCLK);
+	}
+
+	/* Here use 4 levels, make sure not exceed */
+	for (i = 0; i < CZ_NUM_NBPSTATES; i++) {
+		cz_hwmgr->sys_info.nbp_voltage_index[i] =
+			     le16_to_cpu(info->usNBPStateVoltage[i]);
+	}
+
+	if (!cz_hwmgr->sys_info.nb_dpm_enable) {
+		for (i = 1; i < CZ_NUM_NBPSTATES; i++) {
+			if (i < CZ_NUM_NBPMEMORYCLOCK) {
+				cz_hwmgr->sys_info.nbp_memory_clock[i] =
+				    cz_hwmgr->sys_info.nbp_memory_clock[0];
+			}
+			cz_hwmgr->sys_info.nbp_n_clock[i] =
+				    cz_hwmgr->sys_info.nbp_n_clock[0];
+			cz_hwmgr->sys_info.nbp_voltage_index[i] =
+				    cz_hwmgr->sys_info.nbp_voltage_index[0];
+		}
+	}
+
+	if (le32_to_cpu(info->ulGPUCapInfo) &
+		SYS_INFO_GPUCAPS__ENABEL_DFS_BYPASS) {
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				    PHM_PlatformCaps_EnableDFSBypass);
+	}
+
+	cz_hwmgr->sys_info.uma_channel_number = info->ucUMAChannelNumber;
+
+	cz_construct_max_power_limits_table (hwmgr,
+				    &hwmgr->dyn_state.max_clock_voltage_on_ac);
+
+	cz_init_dynamic_state_adjustment_rule_settings(hwmgr,
+				    &info->sDISPCLK_Voltage[0]);
+
+	return result;
+}
+
+static int cz_construct_boot_state(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	cz_hwmgr->boot_power_level.engineClock =
+				cz_hwmgr->sys_info.bootup_engine_clock;
+
+	cz_hwmgr->boot_power_level.vddcIndex =
+			(uint8_t)cz_hwmgr->sys_info.bootup_nb_voltage_index;
+
+	cz_hwmgr->boot_power_level.dsDividerIndex = 0;
+
+	cz_hwmgr->boot_power_level.ssDividerIndex = 0;
+
+	cz_hwmgr->boot_power_level.allowGnbSlow = 1;
+
+	cz_hwmgr->boot_power_level.forceNBPstate = 0;
+
+	cz_hwmgr->boot_power_level.hysteresis_up = 0;
+
+	cz_hwmgr->boot_power_level.numSIMDToPowerDown = 0;
+
+	cz_hwmgr->boot_power_level.display_wm = 0;
+
+	cz_hwmgr->boot_power_level.vce_wm = 0;
+
+	return 0;
+}
+
+static int cz_tf_reset_active_process_mask(struct pp_hwmgr *hwmgr, void *input,
+					void *output, void *storage, int result)
+{
+	return 0;
+}
+
+static int cz_tf_upload_pptable_to_smu(struct pp_hwmgr *hwmgr, void *input,
+				       void *output, void *storage, int result)
+{
+	struct SMU8_Fusion_ClkTable *clock_table;
+	int ret;
+	uint32_t i;
+	void *table = NULL;
+	pp_atomctrl_clock_dividers_kong dividers;
+
+	struct phm_clock_voltage_dependency_table *vddc_table =
+		hwmgr->dyn_state.vddc_dependency_on_sclk;
+	struct phm_clock_voltage_dependency_table *vdd_gfx_table =
+		hwmgr->dyn_state.vdd_gfx_dependency_on_sclk;
+	struct phm_acp_clock_voltage_dependency_table *acp_table =
+		hwmgr->dyn_state.acp_clock_voltage_dependency_table;
+	struct phm_uvd_clock_voltage_dependency_table *uvd_table =
+		hwmgr->dyn_state.uvd_clock_voltage_dependency_table;
+	struct phm_vce_clock_voltage_dependency_table *vce_table =
+		hwmgr->dyn_state.vce_clock_voltage_dependency_table;
+
+	if (!hwmgr->need_pp_table_upload)
+		return 0;
+
+	ret = smum_download_powerplay_table(hwmgr->smumgr, &table);
+
+	PP_ASSERT_WITH_CODE((0 == ret && NULL != table),
+			    "Fail to get clock table from SMU!", return -EINVAL;);
+
+	clock_table = (struct SMU8_Fusion_ClkTable *)table;
+
+	/* patch clock table */
+	PP_ASSERT_WITH_CODE((vddc_table->count <= CZ_MAX_HARDWARE_POWERLEVELS),
+			    "Dependency table entry exceeds max limit!", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((vdd_gfx_table->count <= CZ_MAX_HARDWARE_POWERLEVELS),
+			    "Dependency table entry exceeds max limit!", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((acp_table->count <= CZ_MAX_HARDWARE_POWERLEVELS),
+			    "Dependency table entry exceeds max limit!", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((uvd_table->count <= CZ_MAX_HARDWARE_POWERLEVELS),
+			    "Dependency table entry exceeds max limit!", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((vce_table->count <= CZ_MAX_HARDWARE_POWERLEVELS),
+			    "Dependency table entry exceeds max limit!", return -EINVAL;);
+
+	for (i = 0; i < CZ_MAX_HARDWARE_POWERLEVELS; i++) {
+
+		/* vddc_sclk */
+		clock_table->SclkBreakdownTable.ClkLevel[i].GnbVid =
+			(i < vddc_table->count) ? (uint8_t)vddc_table->entries[i].v : 0;
+		clock_table->SclkBreakdownTable.ClkLevel[i].Frequency =
+			(i < vddc_table->count) ? vddc_table->entries[i].clk : 0;
+
+		atomctrl_get_engine_pll_dividers_kong(hwmgr,
+						      clock_table->SclkBreakdownTable.ClkLevel[i].Frequency,
+						      &dividers);
+
+		clock_table->SclkBreakdownTable.ClkLevel[i].DfsDid =
+			(uint8_t)dividers.pll_post_divider;
+
+		/* vddgfx_sclk */
+		clock_table->SclkBreakdownTable.ClkLevel[i].GfxVid =
+			(i < vdd_gfx_table->count) ? (uint8_t)vdd_gfx_table->entries[i].v : 0;
+
+		/* acp breakdown */
+		clock_table->AclkBreakdownTable.ClkLevel[i].GfxVid =
+			(i < acp_table->count) ? (uint8_t)acp_table->entries[i].v : 0;
+		clock_table->AclkBreakdownTable.ClkLevel[i].Frequency =
+			(i < acp_table->count) ? acp_table->entries[i].acpclk : 0;
+
+		atomctrl_get_engine_pll_dividers_kong(hwmgr,
+						      clock_table->AclkBreakdownTable.ClkLevel[i].Frequency,
+						      &dividers);
+
+		clock_table->AclkBreakdownTable.ClkLevel[i].DfsDid =
+			(uint8_t)dividers.pll_post_divider;
+
+
+		/* uvd breakdown */
+		clock_table->VclkBreakdownTable.ClkLevel[i].GfxVid =
+			(i < uvd_table->count) ? (uint8_t)uvd_table->entries[i].v : 0;
+		clock_table->VclkBreakdownTable.ClkLevel[i].Frequency =
+			(i < uvd_table->count) ? uvd_table->entries[i].vclk : 0;
+
+		atomctrl_get_engine_pll_dividers_kong(hwmgr,
+						      clock_table->VclkBreakdownTable.ClkLevel[i].Frequency,
+						      &dividers);
+
+		clock_table->VclkBreakdownTable.ClkLevel[i].DfsDid =
+			(uint8_t)dividers.pll_post_divider;
+
+		clock_table->DclkBreakdownTable.ClkLevel[i].GfxVid =
+			(i < uvd_table->count) ? (uint8_t)uvd_table->entries[i].v : 0;
+		clock_table->DclkBreakdownTable.ClkLevel[i].Frequency =
+			(i < uvd_table->count) ? uvd_table->entries[i].dclk : 0;
+
+		atomctrl_get_engine_pll_dividers_kong(hwmgr,
+						      clock_table->DclkBreakdownTable.ClkLevel[i].Frequency,
+						      &dividers);
+
+		clock_table->DclkBreakdownTable.ClkLevel[i].DfsDid =
+			(uint8_t)dividers.pll_post_divider;
+
+		/* vce breakdown */
+		clock_table->EclkBreakdownTable.ClkLevel[i].GfxVid =
+			(i < vce_table->count) ? (uint8_t)vce_table->entries[i].v : 0;
+		clock_table->EclkBreakdownTable.ClkLevel[i].Frequency =
+			(i < vce_table->count) ? vce_table->entries[i].ecclk : 0;
+
+
+		atomctrl_get_engine_pll_dividers_kong(hwmgr,
+						      clock_table->EclkBreakdownTable.ClkLevel[i].Frequency,
+						      &dividers);
+
+		clock_table->EclkBreakdownTable.ClkLevel[i].DfsDid =
+			(uint8_t)dividers.pll_post_divider;
+
+	}
+	ret = smum_upload_powerplay_table(hwmgr->smumgr);
+
+	return ret;
+}
+
+static int cz_tf_init_sclk_limit(struct pp_hwmgr *hwmgr, void *input,
+				 void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_clock_voltage_dependency_table *table =
+					hwmgr->dyn_state.vddc_dependency_on_sclk;
+	unsigned long clock = 0, level;
+
+	if (NULL == table || table->count <= 0)
+		return -EINVAL;
+
+	cz_hwmgr->sclk_dpm.soft_min_clk = table->entries[0].clk;
+	cz_hwmgr->sclk_dpm.hard_min_clk = table->entries[0].clk;
+
+	level = cz_get_max_sclk_level(hwmgr) - 1;
+
+	if (level < table->count)
+		clock = table->entries[level].clk;
+	else
+		clock = table->entries[table->count - 1].clk;
+
+	cz_hwmgr->sclk_dpm.soft_max_clk = clock;
+	cz_hwmgr->sclk_dpm.hard_max_clk = clock;
+
+	return 0;
+}
+
+static int cz_tf_init_uvd_limit(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_uvd_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.uvd_clock_voltage_dependency_table;
+	unsigned long clock = 0, level;
+
+	if (NULL == table || table->count <= 0)
+		return -EINVAL;
+
+	cz_hwmgr->uvd_dpm.soft_min_clk = 0;
+	cz_hwmgr->uvd_dpm.hard_min_clk = 0;
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_GetMaxUvdLevel);
+	level = smum_get_argument(hwmgr->smumgr);
+
+	if (level < table->count)
+		clock = table->entries[level].vclk;
+	else
+		clock = table->entries[table->count - 1].vclk;
+
+	cz_hwmgr->uvd_dpm.soft_max_clk = clock;
+	cz_hwmgr->uvd_dpm.hard_max_clk = clock;
+
+	return 0;
+}
+
+static int cz_tf_init_vce_limit(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_vce_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.vce_clock_voltage_dependency_table;
+	unsigned long clock = 0, level;
+
+	if (NULL == table || table->count <= 0)
+		return -EINVAL;
+
+	cz_hwmgr->vce_dpm.soft_min_clk = 0;
+	cz_hwmgr->vce_dpm.hard_min_clk = 0;
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_GetMaxEclkLevel);
+	level = smum_get_argument(hwmgr->smumgr);
+
+	if (level < table->count)
+		clock = table->entries[level].ecclk;
+	else
+		clock = table->entries[table->count - 1].ecclk;
+
+	cz_hwmgr->vce_dpm.soft_max_clk = clock;
+	cz_hwmgr->vce_dpm.hard_max_clk = clock;
+
+	return 0;
+}
+
+static int cz_tf_init_acp_limit(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_acp_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.acp_clock_voltage_dependency_table;
+	unsigned long clock = 0, level;
+
+	if (NULL == table || table->count <= 0)
+		return -EINVAL;
+
+	cz_hwmgr->acp_dpm.soft_min_clk = 0;
+	cz_hwmgr->acp_dpm.hard_min_clk = 0;
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_GetMaxAclkLevel);
+	level = smum_get_argument(hwmgr->smumgr);
+
+	if (level < table->count)
+		clock = table->entries[level].acpclk;
+	else
+		clock = table->entries[table->count - 1].acpclk;
+
+	cz_hwmgr->acp_dpm.soft_max_clk = clock;
+	cz_hwmgr->acp_dpm.hard_max_clk = clock;
+	return 0;
+}
+
+static int cz_tf_init_power_gate_state(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	cz_hwmgr->uvd_power_gated = false;
+	cz_hwmgr->vce_power_gated = false;
+	cz_hwmgr->samu_power_gated = false;
+	cz_hwmgr->acp_power_gated = false;
+	cz_hwmgr->pgacpinit = true;
+
+	return 0;
+}
+
+static int cz_tf_init_sclk_threshold(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	cz_hwmgr->low_sclk_interrupt_threshold = 0;
+
+	return 0;
+}
+static int cz_tf_update_sclk_limit(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_clock_voltage_dependency_table *table =
+					hwmgr->dyn_state.vddc_dependency_on_sclk;
+
+	unsigned long clock = 0;
+	unsigned long level;
+	unsigned long stable_pstate_sclk;
+	struct PP_Clocks clocks;
+	unsigned long percentage;
+
+	cz_hwmgr->sclk_dpm.soft_min_clk = table->entries[0].clk;
+	level = cz_get_max_sclk_level(hwmgr) - 1;
+
+	if (level < table->count)
+		cz_hwmgr->sclk_dpm.soft_max_clk  = table->entries[level].clk;
+	else
+		cz_hwmgr->sclk_dpm.soft_max_clk  = table->entries[table->count - 1].clk;
+
+	/*PECI_GetMinClockSettings(pHwMgr->pPECI, &clocks);*/
+	clock = clocks.engineClock;
+
+	if (cz_hwmgr->sclk_dpm.hard_min_clk != clock) {
+		cz_hwmgr->sclk_dpm.hard_min_clk = clock;
+
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SetSclkHardMin,
+						 cz_get_sclk_level(hwmgr,
+					cz_hwmgr->sclk_dpm.hard_min_clk,
+					     PPSMC_MSG_SetSclkHardMin));
+	}
+
+	clock = cz_hwmgr->sclk_dpm.soft_min_clk;
+
+	/* update minimum clocks for Stable P-State feature */
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				     PHM_PlatformCaps_StablePState)) {
+		percentage = 75;
+		/*Sclk - calculate sclk value based on percentage and find FLOOR sclk from VddcDependencyOnSCLK table  */
+		stable_pstate_sclk = (hwmgr->dyn_state.max_clock_voltage_on_ac.mclk *
+					percentage) / 100;
+
+		if (clock < stable_pstate_sclk)
+			clock = stable_pstate_sclk;
+	} else {
+		if (clock < hwmgr->gfx_arbiter.sclk)
+			clock = hwmgr->gfx_arbiter.sclk;
+	}
+
+	if (cz_hwmgr->sclk_dpm.soft_min_clk != clock) {
+		cz_hwmgr->sclk_dpm.soft_min_clk = clock;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SetSclkSoftMin,
+						cz_get_sclk_level(hwmgr,
+					cz_hwmgr->sclk_dpm.soft_min_clk,
+					     PPSMC_MSG_SetSclkSoftMin));
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				    PHM_PlatformCaps_StablePState) &&
+			 cz_hwmgr->sclk_dpm.soft_max_clk != clock) {
+		cz_hwmgr->sclk_dpm.soft_max_clk = clock;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SetSclkSoftMax,
+						cz_get_sclk_level(hwmgr,
+					cz_hwmgr->sclk_dpm.soft_max_clk,
+					PPSMC_MSG_SetSclkSoftMax));
+	}
+
+	return 0;
+}
+
+static int cz_tf_set_deep_sleep_sclk_threshold(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_SclkDeepSleep)) {
+		uint32_t clks = hwmgr->display_config.min_core_set_clock_in_sr;
+		if (clks == 0)
+			clks = CZ_MIN_DEEP_SLEEP_SCLK;
+
+		PP_DBG_LOG("Setting Deep Sleep Clock: %d\n", clks);
+
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetMinDeepSleepSclk,
+				clks);
+	}
+
+	return 0;
+}
+
+static int cz_tf_set_watermark_threshold(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr =
+				  (struct cz_hwmgr *)(hwmgr->backend);
+
+	smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_SetWatermarkFrequency,
+				      cz_hwmgr->sclk_dpm.soft_max_clk);
+
+	return 0;
+}
+
+static int cz_tf_set_enabled_levels(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	return 0;
+}
+
+
+static int cz_tf_enable_nb_dpm(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	int ret = 0;
+
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	unsigned long dpm_features = 0;
+
+	if (!cz_hwmgr->is_nb_dpm_enabled) {
+		PP_DBG_LOG("enabling ALL SMU features.\n");
+		dpm_features |= NB_DPM_MASK;
+		ret = smum_send_msg_to_smc_with_parameter(
+							     hwmgr->smumgr,
+					 PPSMC_MSG_EnableAllSmuFeatures,
+							     dpm_features);
+		if (ret == 0)
+			cz_hwmgr->is_nb_dpm_enabled = true;
+	}
+
+	return ret;
+}
+
+static int cz_nbdpm_pstate_enable_disable(struct pp_hwmgr *hwmgr, bool enable, bool lock)
+{
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (hw_data->is_nb_dpm_enabled) {
+		if (enable) {
+			PP_DBG_LOG("enable Low Memory PState.\n");
+
+			return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_EnableLowMemoryPstate,
+						(lock ? 1 : 0));
+		} else {
+			PP_DBG_LOG("disable Low Memory PState.\n");
+
+			return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_DisableLowMemoryPstate,
+						(lock ? 1 : 0));
+		}
+	}
+
+	return 0;
+}
+
+static int cz_tf_update_low_mem_pstate(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	bool disable_switch;
+	bool enable_low_mem_state;
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+	const struct phm_set_power_state_input *states = (struct phm_set_power_state_input *)input;
+	const struct cz_power_state *pnew_state = cast_const_PhwCzPowerState(states->pnew_state);
+
+	if (hw_data->sys_info.nb_dpm_enable) {
+		disable_switch = hw_data->cc6_settings.nb_pstate_switch_disable ? true : false;
+		enable_low_mem_state = hw_data->cc6_settings.nb_pstate_switch_disable ? false : true;
+
+		if (pnew_state->action == FORCE_HIGH)
+			cz_nbdpm_pstate_enable_disable(hwmgr, false, disable_switch);
+		else if(pnew_state->action == CANCEL_FORCE_HIGH)
+			cz_nbdpm_pstate_enable_disable(hwmgr, false, disable_switch);
+		else 
+			cz_nbdpm_pstate_enable_disable(hwmgr, enable_low_mem_state, disable_switch);
+	}
+	return 0;
+}
+
+static struct phm_master_table_item cz_set_power_state_list[] = {
+	{NULL, cz_tf_update_sclk_limit},
+	{NULL, cz_tf_set_deep_sleep_sclk_threshold},
+	{NULL, cz_tf_set_watermark_threshold},
+	{NULL, cz_tf_set_enabled_levels},
+	{NULL, cz_tf_enable_nb_dpm},
+	{NULL, cz_tf_update_low_mem_pstate},
+	{NULL, NULL}
+};
+
+static struct phm_master_table_header cz_set_power_state_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_set_power_state_list
+};
+
+static struct phm_master_table_item cz_setup_asic_list[] = {
+	{NULL, cz_tf_reset_active_process_mask},
+	{NULL, cz_tf_upload_pptable_to_smu},
+	{NULL, cz_tf_init_sclk_limit},
+	{NULL, cz_tf_init_uvd_limit},
+	{NULL, cz_tf_init_vce_limit},
+	{NULL, cz_tf_init_acp_limit},
+	{NULL, cz_tf_init_power_gate_state},
+	{NULL, cz_tf_init_sclk_threshold},
+	{NULL, NULL}
+};
+
+static struct phm_master_table_header cz_setup_asic_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_setup_asic_list
+};
+
+static int cz_tf_power_up_display_clock_sys_pll(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+	hw_data->disp_clk_bypass_pending = false;
+	hw_data->disp_clk_bypass = false;
+
+	return 0;
+}
+
+static int cz_tf_clear_nb_dpm_flag(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+	hw_data->is_nb_dpm_enabled = false;
+
+	return 0;
+}
+
+static int cz_tf_reset_cc6_data(struct pp_hwmgr *hwmgr,
+					void *input, void *output,
+					void *storage, int result)
+{
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+
+	hw_data->cc6_settings.cc6_setting_changed = false;
+	hw_data->cc6_settings.cpu_pstate_separation_time = 0;
+	hw_data->cc6_settings.cpu_cc6_disable = false;
+	hw_data->cc6_settings.cpu_pstate_disable = false;
+
+	return 0;
+}
+
+static struct phm_master_table_item cz_power_down_asic_list[] = {
+	{NULL, cz_tf_power_up_display_clock_sys_pll},
+	{NULL, cz_tf_clear_nb_dpm_flag},
+	{NULL, cz_tf_reset_cc6_data},
+	{NULL, NULL}
+};
+
+static struct phm_master_table_header cz_power_down_asic_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_power_down_asic_list
+};
+
+static int cz_tf_program_voting_clients(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	PHMCZ_WRITE_SMC_REGISTER(hwmgr->device, CG_FREQ_TRAN_VOTING_0,
+				PPCZ_VOTINGRIGHTSCLIENTS_DFLT0);
+	return 0;
+}
+
+static int cz_tf_start_dpm(struct pp_hwmgr *hwmgr, void *input, void *output,
+			   void *storage, int result)
+{
+	int res = 0xff;
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	unsigned long dpm_features = 0;
+
+	cz_hwmgr->dpm_flags |= DPMFlags_SCLK_Enabled;
+	dpm_features |= SCLK_DPM_MASK;
+
+	res = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_EnableAllSmuFeatures,
+				dpm_features);
+
+	return res;
+}
+
+static int cz_tf_program_bootup_state(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	cz_hwmgr->sclk_dpm.soft_min_clk = cz_hwmgr->sys_info.bootup_engine_clock;
+	cz_hwmgr->sclk_dpm.soft_max_clk = cz_hwmgr->sys_info.bootup_engine_clock;
+
+	smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetSclkSoftMin,
+				cz_get_sclk_level(hwmgr,
+				cz_hwmgr->sclk_dpm.soft_min_clk,
+				PPSMC_MSG_SetSclkSoftMin));
+
+	smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetSclkSoftMax,
+				cz_get_sclk_level(hwmgr,
+				cz_hwmgr->sclk_dpm.soft_max_clk,
+				PPSMC_MSG_SetSclkSoftMax));
+
+	return 0;
+}
+
+int cz_tf_reset_acp_boot_level(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	cz_hwmgr->acp_boot_level = 0xff;
+	return 0;
+}
+
+static bool cz_dpm_check_smu_features(struct pp_hwmgr *hwmgr,
+				unsigned long check_feature)
+{
+	int result;
+	unsigned long features;
+
+	result = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_GetFeatureStatus, 0);
+	if (result == 0) {
+		features = smum_get_argument(hwmgr->smumgr);
+		if (features & check_feature)
+			return true;
+	}
+
+	return result;
+}
+
+static int cz_tf_check_for_dpm_disabled(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	if (cz_dpm_check_smu_features(hwmgr, SMU_EnabledFeatureScoreboard_SclkDpmOn))
+		return PP_Result_TableImmediateExit;
+	return 0;
+}
+
+static int cz_tf_enable_didt(struct pp_hwmgr *hwmgr, void *input,
+				void *output, void *storage, int result)
+{
+	/* TO DO */
+	return 0;
+}
+
+static int cz_tf_check_for_dpm_enabled(struct pp_hwmgr *hwmgr,
+						void *input, void *output,
+						void *storage, int result)
+{
+	if (!cz_dpm_check_smu_features(hwmgr,
+			     SMU_EnabledFeatureScoreboard_SclkDpmOn))
+		return PP_Result_TableImmediateExit;
+	return 0;
+}
+
+static struct phm_master_table_item cz_disable_dpm_list[] = {
+	{ NULL, cz_tf_check_for_dpm_enabled},
+	{NULL, NULL},
+};
+
+
+static struct phm_master_table_header cz_disable_dpm_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_disable_dpm_list
+};
+
+static struct phm_master_table_item cz_enable_dpm_list[] = {
+	{ NULL, cz_tf_check_for_dpm_disabled },
+	{ NULL, cz_tf_program_voting_clients },
+	{ NULL, cz_tf_start_dpm},
+	{ NULL, cz_tf_program_bootup_state},
+	{ NULL, cz_tf_enable_didt },
+	{ NULL, cz_tf_reset_acp_boot_level },
+	{NULL, NULL},
+};
+
+static struct phm_master_table_header cz_enable_dpm_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	cz_enable_dpm_list
+};
+
+static int cz_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
+				struct pp_power_state  *prequest_ps,
+			const struct pp_power_state *pcurrent_ps)
+{
+	struct cz_power_state *cz_ps =
+				cast_PhwCzPowerState(&prequest_ps->hardware);
+
+	const struct cz_power_state *cz_current_ps =
+				cast_const_PhwCzPowerState(&pcurrent_ps->hardware);
+
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct PP_Clocks clocks;
+	bool force_high;
+	unsigned long  num_of_active_displays = 4;
+
+	cz_ps->evclk = hwmgr->vce_arbiter.evclk;
+	cz_ps->ecclk = hwmgr->vce_arbiter.ecclk;
+
+	cz_ps->need_dfs_bypass = true;
+
+	cz_hwmgr->video_start = (hwmgr->uvd_arbiter.vclk != 0 || hwmgr->uvd_arbiter.dclk != 0 ||
+				hwmgr->vce_arbiter.evclk != 0 || hwmgr->vce_arbiter.ecclk != 0);
+
+	cz_hwmgr->battery_state = (PP_StateUILabel_Battery == prequest_ps->classification.ui_label);
+
+	/* to do PECI_GetMinClockSettings(pHwMgr->pPECI, &clocks); */
+	/* PECI_GetNumberOfActiveDisplays(pHwMgr->pPECI, &numOfActiveDisplays); */
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_StablePState))
+		clocks.memoryClock = hwmgr->dyn_state.max_clock_voltage_on_ac.mclk;
+	else
+		clocks.memoryClock = 0;
+
+	if (clocks.memoryClock < hwmgr->gfx_arbiter.mclk)
+		clocks.memoryClock = hwmgr->gfx_arbiter.mclk;
+
+	force_high = (clocks.memoryClock > cz_hwmgr->sys_info.nbp_memory_clock[CZ_NUM_NBPMEMORYCLOCK - 1])
+			|| (num_of_active_displays >= 3);
+
+	cz_ps->action = cz_current_ps->action;
+
+	if ((force_high == false) && (cz_ps->action == FORCE_HIGH))
+		cz_ps->action = CANCEL_FORCE_HIGH;
+	else if ((force_high == true) && (cz_ps->action != FORCE_HIGH))
+		cz_ps->action = FORCE_HIGH;
+	else
+		cz_ps->action = DO_NOTHING;
+
+	return 0;
+}
+
+static int cz_hwmgr_backend_init(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+
+	result = cz_initialize_dpm_defaults(hwmgr);
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] cz_initialize_dpm_defaults failed\n");
+		return result;
+	}
+
+	result = cz_get_system_info_data(hwmgr);
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] cz_get_system_info_data failed\n");
+		return result;
+	}
+
+	cz_construct_boot_state(hwmgr);
+
+	result = phm_construct_table(hwmgr, &cz_setup_asic_master,
+				&(hwmgr->setup_asic));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to construct setup ASIC\n");
+		return result;
+	}
+
+	result = phm_construct_table(hwmgr, &cz_power_down_asic_master,
+				&(hwmgr->power_down_asic));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to construct power down ASIC\n");
+		return result;
+	}
+
+	result = phm_construct_table(hwmgr, &cz_disable_dpm_master,
+				&(hwmgr->disable_dynamic_state_management));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to disable_dynamic_state\n");
+		return result;
+	}
+	result = phm_construct_table(hwmgr, &cz_enable_dpm_master,
+				&(hwmgr->enable_dynamic_state_management));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to enable_dynamic_state\n");
+		return result;
+	}
+	result = phm_construct_table(hwmgr, &cz_set_power_state_master,
+				&(hwmgr->set_power_state));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to construct set_power_state\n");
+		return result;
+	}
+
+	result = phm_construct_table(hwmgr, &cz_phm_enable_clock_power_gatings_master, &(hwmgr->enable_clock_power_gatings));
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] Fail to construct enable_clock_power_gatings\n");
+		return result;
+	}
+	return result;
+}
+
+static int cz_hwmgr_backend_fini(struct pp_hwmgr *hwmgr)
+{
+	if (hwmgr != NULL || hwmgr->backend != NULL) {
+		kfree(hwmgr->backend);
+		kfree(hwmgr);
+	}
+	return 0;
+}
+
+int cz_phm_force_dpm_highest(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (cz_hwmgr->sclk_dpm.soft_min_clk !=
+				cz_hwmgr->sclk_dpm.soft_max_clk)
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SetSclkSoftMin,
+						cz_get_sclk_level(hwmgr,
+						cz_hwmgr->sclk_dpm.soft_max_clk,
+						PPSMC_MSG_SetSclkSoftMin));
+	return 0;
+}
+
+int cz_phm_unforce_dpm_levels(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.vddc_dependency_on_sclk;
+	unsigned long clock = 0, level;
+
+	if (NULL == table || table->count <= 0)
+		return -EINVAL;
+
+	cz_hwmgr->sclk_dpm.soft_min_clk = table->entries[0].clk;
+	cz_hwmgr->sclk_dpm.hard_min_clk = table->entries[0].clk;
+
+	level = cz_get_max_sclk_level(hwmgr) - 1;
+
+	if (level < table->count)
+		clock = table->entries[level].clk;
+	else
+		clock = table->entries[table->count - 1].clk;
+
+	cz_hwmgr->sclk_dpm.soft_max_clk = clock;
+	cz_hwmgr->sclk_dpm.hard_max_clk = clock;
+
+	smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetSclkSoftMin,
+				cz_get_sclk_level(hwmgr,
+				cz_hwmgr->sclk_dpm.soft_min_clk,
+				PPSMC_MSG_SetSclkSoftMin));
+
+	smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetSclkSoftMax,
+				cz_get_sclk_level(hwmgr,
+				cz_hwmgr->sclk_dpm.soft_max_clk,
+				PPSMC_MSG_SetSclkSoftMax));
+
+	return 0;
+}
+
+int cz_phm_force_dpm_lowest(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (cz_hwmgr->sclk_dpm.soft_min_clk !=
+				cz_hwmgr->sclk_dpm.soft_max_clk) {
+		cz_hwmgr->sclk_dpm.soft_max_clk =
+			cz_hwmgr->sclk_dpm.soft_min_clk;
+
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetSclkSoftMax,
+				cz_get_sclk_level(hwmgr,
+				cz_hwmgr->sclk_dpm.soft_max_clk,
+				PPSMC_MSG_SetSclkSoftMax));
+	}
+
+	return 0;
+}
+
+static int cz_dpm_force_dpm_level(struct pp_hwmgr *hwmgr,
+				enum amd_dpm_forced_level level)
+{
+	int ret = 0;
+
+	switch (level) {
+	case AMD_DPM_FORCED_LEVEL_HIGH:
+		ret = cz_phm_force_dpm_highest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_LOW:
+		ret = cz_phm_force_dpm_lowest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_AUTO:
+		ret = cz_phm_unforce_dpm_levels(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	default:
+		break;
+	}
+
+	hwmgr->dpm_level = level;
+
+	return ret;
+}
+
+int cz_dpm_powerdown_uvd(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_UVDPowerGating))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						     PPSMC_MSG_UVDPowerOFF);
+	return 0;
+}
+
+int cz_dpm_powerup_uvd(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_UVDPowerGating)) {
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				  PHM_PlatformCaps_UVDDynamicPowerGating)) {
+			return smum_send_msg_to_smc_with_parameter(
+								hwmgr->smumgr,
+						   PPSMC_MSG_UVDPowerON, 1);
+		} else {
+			return smum_send_msg_to_smc_with_parameter(
+								hwmgr->smumgr,
+						   PPSMC_MSG_UVDPowerON, 0);
+		}
+	}
+
+	return 0;
+}
+
+int cz_dpm_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_uvd_clock_voltage_dependency_table *ptable =
+		hwmgr->dyn_state.uvd_clock_voltage_dependency_table;
+
+	if (!bgate) {
+		/* Stable Pstate is enabled and we need to set the UVD DPM to highest level */
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_StablePState)) {
+			cz_hwmgr->uvd_dpm.hard_min_clk =
+				   ptable->entries[ptable->count - 1].vclk;
+
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						     PPSMC_MSG_SetUvdHardMin,
+						      cz_get_uvd_level(hwmgr,
+					     cz_hwmgr->uvd_dpm.hard_min_clk,
+						   PPSMC_MSG_SetUvdHardMin));
+
+			cz_enable_disable_uvd_dpm(hwmgr, true);
+		} else
+			cz_enable_disable_uvd_dpm(hwmgr, true);
+	} else
+		cz_enable_disable_uvd_dpm(hwmgr, false);
+
+	return 0;
+}
+
+int  cz_dpm_update_vce_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct phm_vce_clock_voltage_dependency_table *ptable =
+		hwmgr->dyn_state.vce_clock_voltage_dependency_table;
+
+	/* Stable Pstate is enabled and we need to set the VCE DPM to highest level */
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_StablePState)) {
+		cz_hwmgr->vce_dpm.hard_min_clk =
+				  ptable->entries[ptable->count - 1].ecclk;
+
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_SetEclkHardMin,
+					cz_get_eclk_level(hwmgr,
+					     cz_hwmgr->vce_dpm.hard_min_clk,
+						PPSMC_MSG_SetEclkHardMin));
+	} else {
+		/*EPR# 419220 -HW limitation to to */
+		cz_hwmgr->vce_dpm.hard_min_clk = hwmgr->vce_arbiter.ecclk;
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					    PPSMC_MSG_SetEclkHardMin,
+					    cz_get_eclk_level(hwmgr,
+				     cz_hwmgr->vce_dpm.hard_min_clk,
+					  PPSMC_MSG_SetEclkHardMin));
+
+	}
+	return 0;
+}
+
+int cz_dpm_powerdown_vce(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_VCEPowerGating))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						     PPSMC_MSG_VCEPowerOFF);
+	return 0;
+}
+
+int cz_dpm_powerup_vce(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					 PHM_PlatformCaps_VCEPowerGating))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						     PPSMC_MSG_VCEPowerON);
+	return 0;
+}
+
+static int cz_dpm_get_mclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	return cz_hwmgr->sys_info.bootup_uma_clock;
+}
+
+static int cz_dpm_get_sclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct pp_power_state  *ps;
+	struct cz_power_state  *cz_ps;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	ps = hwmgr->request_ps;
+
+	if (ps == NULL)
+		return -EINVAL;
+
+	cz_ps = cast_PhwCzPowerState(&ps->hardware);
+
+	if (low)
+		return cz_ps->levels[0].engineClock;
+	else
+		return cz_ps->levels[cz_ps->level-1].engineClock;
+}
+
+static int cz_dpm_patch_boot_state(struct pp_hwmgr *hwmgr,
+					struct pp_hw_power_state *hw_ps)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+	struct cz_power_state *cz_ps = cast_PhwCzPowerState(hw_ps);
+
+	cz_ps->level = 1;
+	cz_ps->nbps_flags = 0;
+	cz_ps->bapm_flags = 0;
+	cz_ps->levels[0] = cz_hwmgr->boot_power_level;
+
+	return 0;
+}
+
+static int cz_dpm_get_pp_table_entry_callback(
+						     struct pp_hwmgr *hwmgr,
+					   struct pp_hw_power_state *hw_ps,
+							  unsigned int index,
+						     const void *clock_info)
+{
+	struct cz_power_state *cz_ps = cast_PhwCzPowerState(hw_ps);
+
+	const ATOM_PPLIB_CZ_CLOCK_INFO *cz_clock_info = clock_info;
+
+	struct phm_clock_voltage_dependency_table *table =
+				    hwmgr->dyn_state.vddc_dependency_on_sclk;
+	uint8_t clock_info_index = cz_clock_info->index;
+
+	if (clock_info_index > (uint8_t)(hwmgr->platform_descriptor.hardwareActivityPerformanceLevels - 1))
+		clock_info_index = (uint8_t)(hwmgr->platform_descriptor.hardwareActivityPerformanceLevels - 1);
+
+	cz_ps->levels[index].engineClock = table->entries[clock_info_index].clk;
+	cz_ps->levels[index].vddcIndex = (uint8_t)table->entries[clock_info_index].v;
+
+	cz_ps->level = index + 1;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_SclkDeepSleep)) {
+		cz_ps->levels[index].dsDividerIndex = 5;
+		cz_ps->levels[index].ssDividerIndex = 5;
+	}
+
+	return 0;
+}
+
+static int cz_dpm_get_num_of_pp_table_entries(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	unsigned long ret = 0;
+
+	result = pp_tables_get_num_of_entries(hwmgr, &ret);
+
+	return result ? 0 : ret;
+}
+
+static int cz_dpm_get_pp_table_entry(struct pp_hwmgr *hwmgr,
+		    unsigned long entry, struct pp_power_state *ps)
+{
+	int result;
+	struct cz_power_state *cz_ps;
+
+	ps->hardware.magic = PhwCz_Magic;
+
+	cz_ps = cast_PhwCzPowerState(&(ps->hardware));
+
+	result = pp_tables_get_entry(hwmgr, entry, ps,
+			cz_dpm_get_pp_table_entry_callback);
+
+	cz_ps->uvd_clocks.vclk = ps->uvd_clocks.VCLK;
+	cz_ps->uvd_clocks.dclk = ps->uvd_clocks.DCLK;
+
+	return result;
+}
+
+int cz_get_power_state_size(struct pp_hwmgr *hwmgr)
+{
+	return sizeof(struct cz_power_state);
+}
+
+static void
+cz_print_current_perforce_level(struct pp_hwmgr *hwmgr, struct seq_file *m)
+{
+	struct cz_hwmgr *cz_hwmgr = (struct cz_hwmgr *)(hwmgr->backend);
+
+	struct phm_clock_voltage_dependency_table *table =
+				hwmgr->dyn_state.vddc_dependency_on_sclk;
+
+	struct phm_vce_clock_voltage_dependency_table *vce_table =
+		hwmgr->dyn_state.vce_clock_voltage_dependency_table;
+
+	struct phm_uvd_clock_voltage_dependency_table *uvd_table =
+		hwmgr->dyn_state.uvd_clock_voltage_dependency_table;
+
+	uint32_t sclk_index = PHM_GET_FIELD(cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixTARGET_AND_CURRENT_PROFILE_INDEX),
+					TARGET_AND_CURRENT_PROFILE_INDEX, CURR_SCLK_INDEX);
+	uint32_t uvd_index = PHM_GET_FIELD(cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixTARGET_AND_CURRENT_PROFILE_INDEX_2),
+					TARGET_AND_CURRENT_PROFILE_INDEX_2, CURR_UVD_INDEX);
+	uint32_t vce_index = PHM_GET_FIELD(cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixTARGET_AND_CURRENT_PROFILE_INDEX_2),
+					TARGET_AND_CURRENT_PROFILE_INDEX_2, CURR_VCE_INDEX);
+
+	uint32_t sclk, vclk, dclk, ecclk, tmp, activity_percent;
+	uint16_t vddnb, vddgfx;
+	int result;
+
+	if (sclk_index >= NUM_SCLK_LEVELS) {
+		seq_printf(m, "\n invalid sclk dpm profile %d\n", sclk_index);
+	} else {
+		sclk = table->entries[sclk_index].clk;
+		seq_printf(m, "\n index: %u sclk: %u MHz\n", sclk_index, sclk/100);
+	}
+
+	tmp = (cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixSMUSVI_NB_CURRENTVID) &
+		CURRENT_NB_VID_MASK) >> CURRENT_NB_VID__SHIFT;
+	vddnb = cz_convert_8Bit_index_to_voltage(hwmgr, tmp);
+	tmp = (cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixSMUSVI_GFX_CURRENTVID) &
+		CURRENT_GFX_VID_MASK) >> CURRENT_GFX_VID__SHIFT;
+	vddgfx = cz_convert_8Bit_index_to_voltage(hwmgr, (u16)tmp);
+	seq_printf(m, "\n vddnb: %u vddgfx: %u\n", vddnb, vddgfx);
+
+	seq_printf(m, "\n uvd    %sabled\n", cz_hwmgr->uvd_power_gated ? "dis" : "en");
+	if (!cz_hwmgr->uvd_power_gated) {
+		if (uvd_index >= CZ_MAX_HARDWARE_POWERLEVELS) {
+			seq_printf(m, "\n invalid uvd dpm level %d\n", uvd_index);
+		} else {
+			vclk = uvd_table->entries[uvd_index].vclk;
+			dclk = uvd_table->entries[uvd_index].dclk;
+			seq_printf(m, "\n index: %u uvd vclk: %u MHz dclk: %u MHz\n", uvd_index, vclk/100, dclk/100);
+		}
+	}
+
+	seq_printf(m, "\n vce    %sabled\n", cz_hwmgr->vce_power_gated ? "dis" : "en");
+	if (!cz_hwmgr->vce_power_gated) {
+		if (vce_index >= CZ_MAX_HARDWARE_POWERLEVELS) {
+			seq_printf(m, "\n invalid vce dpm level %d\n", vce_index);
+		} else {
+			ecclk = vce_table->entries[vce_index].ecclk;
+			seq_printf(m, "\n index: %u vce ecclk: %u MHz\n", vce_index, ecclk/100);
+		}
+	}
+
+	result = smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_GetAverageGraphicsActivity);
+	if (0 == result) {
+		activity_percent = cgs_read_register(hwmgr->device, mmSMU_MP1_SRBM2P_ARG_0);
+		activity_percent = activity_percent > 100 ? 100 : activity_percent;
+	} else {
+		activity_percent = 50;
+	}
+
+	seq_printf(m, "\n [GPU load]: %u %%\n\n", activity_percent);
+}
+
+static void cz_hw_print_display_cfg(
+	const struct cc6_settings *cc6_settings)
+{
+	PP_DBG_LOG("New Display Configuration:\n");
+
+	PP_DBG_LOG("   cpu_cc6_disable: %d\n",
+			cc6_settings->cpu_cc6_disable);
+	PP_DBG_LOG("   cpu_pstate_disable: %d\n",
+			cc6_settings->cpu_pstate_disable);
+	PP_DBG_LOG("   nb_pstate_switch_disable: %d\n",
+			cc6_settings->nb_pstate_switch_disable);
+	PP_DBG_LOG("   cpu_pstate_separation_time: %d\n\n",
+			cc6_settings->cpu_pstate_separation_time);
+}
+
+ static int cz_set_cpu_power_state(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+	uint32_t data = 0;
+
+	if (hw_data->cc6_settings.cc6_setting_changed == true) {
+
+		hw_data->cc6_settings.cc6_setting_changed = false;
+
+		cz_hw_print_display_cfg(&hw_data->cc6_settings);
+
+		data |= (hw_data->cc6_settings.cpu_pstate_separation_time
+			& PWRMGT_SEPARATION_TIME_MASK)
+			<< PWRMGT_SEPARATION_TIME_SHIFT;
+
+		data|= (hw_data->cc6_settings.cpu_cc6_disable ? 0x1 : 0x0)
+			<< PWRMGT_DISABLE_CPU_CSTATES_SHIFT;
+
+		data|= (hw_data->cc6_settings.cpu_pstate_disable ? 0x1 : 0x0)
+			<< PWRMGT_DISABLE_CPU_PSTATES_SHIFT;
+
+		PP_DBG_LOG("SetDisplaySizePowerParams data: 0x%X\n",
+			data);
+
+		smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SetDisplaySizePowerParams,
+						data);
+	}
+
+	return 0;
+}
+
+
+ static int cz_store_cc6_data(struct pp_hwmgr *hwmgr, uint32_t separation_time,
+			bool cc6_disable, bool pstate_disable, bool pstate_switch_disable)
+ {
+	struct cz_hwmgr *hw_data = (struct cz_hwmgr *)(hwmgr->backend);
+
+	if (separation_time !=
+		hw_data->cc6_settings.cpu_pstate_separation_time
+		|| cc6_disable !=
+		hw_data->cc6_settings.cpu_cc6_disable
+		|| pstate_disable !=
+		hw_data->cc6_settings.cpu_pstate_disable
+		|| pstate_switch_disable !=
+		hw_data->cc6_settings.nb_pstate_switch_disable) {
+
+		hw_data->cc6_settings.cc6_setting_changed = true;
+
+		hw_data->cc6_settings.cpu_pstate_separation_time =
+			separation_time;
+		hw_data->cc6_settings.cpu_cc6_disable =
+			cc6_disable;
+		hw_data->cc6_settings.cpu_pstate_disable =
+			pstate_disable;
+		hw_data->cc6_settings.nb_pstate_switch_disable =
+			pstate_switch_disable;
+
+	}
+
+	return 0;
+}
+
+ static int cz_get_dal_power_level(struct pp_hwmgr *hwmgr,
+		struct amd_pp_dal_clock_info*info)
+{
+	uint32_t i;
+	const struct phm_clock_voltage_dependency_table * table =
+			hwmgr->dyn_state.vddc_dep_on_dal_pwrl;
+	const struct phm_clock_and_voltage_limits* limits =
+			&hwmgr->dyn_state.max_clock_voltage_on_ac;
+
+	info->engine_max_clock = limits->sclk;
+	info->memory_max_clock = limits->mclk;
+
+	for (i = table->count - 1; i > 0; i--) {
+
+		if (limits->vddc >= table->entries[i].v) {
+			info->level = table->entries[i].clk;
+			return 0;
+		}
+	}
+	return -EINVAL;
+}
+
+static const struct pp_hwmgr_func cz_hwmgr_funcs = {
+	.backend_init = cz_hwmgr_backend_init,
+	.backend_fini = cz_hwmgr_backend_fini,
+	.asic_setup = NULL,
+	.apply_state_adjust_rules = cz_apply_state_adjust_rules,
+	.force_dpm_level = cz_dpm_force_dpm_level,
+	.get_power_state_size = cz_get_power_state_size,
+	.powerdown_uvd = cz_dpm_powerdown_uvd,
+	.powergate_uvd = cz_dpm_powergate_uvd,
+	.powergate_vce = cz_dpm_powergate_vce,
+	.get_mclk = cz_dpm_get_mclk,
+	.get_sclk = cz_dpm_get_sclk,
+	.patch_boot_state = cz_dpm_patch_boot_state,
+	.get_pp_table_entry = cz_dpm_get_pp_table_entry,
+	.get_num_of_pp_table_entries = cz_dpm_get_num_of_pp_table_entries,
+	.print_current_perforce_level = cz_print_current_perforce_level,
+	.set_cpu_power_state = cz_set_cpu_power_state,
+	.store_cc6_data = cz_store_cc6_data,
+	.get_dal_power_level= cz_get_dal_power_level,
+};
+
+int cz_hwmgr_init(struct pp_hwmgr *hwmgr)
+{
+	struct cz_hwmgr *cz_hwmgr;
+	int ret = 0;
+
+	cz_hwmgr = kzalloc(sizeof(struct cz_hwmgr), GFP_KERNEL);
+	if (cz_hwmgr == NULL)
+		return -ENOMEM;
+
+	hwmgr->backend = cz_hwmgr;
+	hwmgr->hwmgr_func = &cz_hwmgr_funcs;
+	hwmgr->pptable_func = &pptable_funcs;
+	return ret;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.h b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.h
new file mode 100644
index 000000000000..c477f1cf3f23
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/cz_hwmgr.h
@@ -0,0 +1,326 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _CZ_HWMGR_H_
+#define _CZ_HWMGR_H_
+
+#include "cgs_common.h"
+#include "ppatomctrl.h"
+
+#define CZ_NUM_NBPSTATES               4
+#define CZ_NUM_NBPMEMORYCLOCK          2
+#define MAX_DISPLAY_CLOCK_LEVEL        8
+#define CZ_AT_DFLT                     30
+#define CZ_MAX_HARDWARE_POWERLEVELS    8
+#define PPCZ_VOTINGRIGHTSCLIENTS_DFLT0   0x3FFFC102
+#define CZ_MIN_DEEP_SLEEP_SCLK         800
+
+/* Carrizo device IDs */
+#define DEVICE_ID_CZ_9870             0x9870
+#define DEVICE_ID_CZ_9874             0x9874
+#define DEVICE_ID_CZ_9875             0x9875
+#define DEVICE_ID_CZ_9876             0x9876
+#define DEVICE_ID_CZ_9877             0x9877
+
+#define PHMCZ_WRITE_SMC_REGISTER(device, reg, value)                            \
+		cgs_write_ind_register(device, CGS_IND_REG__SMC, ix##reg, value)
+
+struct cz_dpm_entry {
+	uint32_t soft_min_clk;
+	uint32_t hard_min_clk;
+	uint32_t soft_max_clk;
+	uint32_t hard_max_clk;
+};
+
+struct cz_sys_info {
+	uint32_t bootup_uma_clock;
+	uint32_t bootup_engine_clock;
+	uint32_t dentist_vco_freq;
+	uint32_t nb_dpm_enable;
+	uint32_t nbp_memory_clock[CZ_NUM_NBPMEMORYCLOCK];
+	uint32_t nbp_n_clock[CZ_NUM_NBPSTATES];
+	uint16_t nbp_voltage_index[CZ_NUM_NBPSTATES];
+	uint32_t display_clock[MAX_DISPLAY_CLOCK_LEVEL];
+	uint16_t bootup_nb_voltage_index;
+	uint8_t htc_tmp_lmt;
+	uint8_t htc_hyst_lmt;
+	uint32_t system_config;
+	uint32_t uma_channel_number;
+};
+
+#define MAX_DISPLAYPHY_IDS			0x8
+#define DISPLAYPHY_LANEMASK			0xF
+#define UNKNOWN_TRANSMITTER_PHY_ID		(-1)
+
+#define DISPLAYPHY_PHYID_SHIFT			24
+#define DISPLAYPHY_LANESELECT_SHIFT		16
+
+#define DISPLAYPHY_RX_SELECT			0x1
+#define DISPLAYPHY_TX_SELECT			0x2
+#define DISPLAYPHY_CORE_SELECT			0x4
+
+#define DDI_POWERGATING_ARG(phyID, lanemask, rx, tx, core) \
+		(((uint32_t)(phyID))<<DISPLAYPHY_PHYID_SHIFT | \
+		((uint32_t)(lanemask))<<DISPLAYPHY_LANESELECT_SHIFT | \
+		((rx) ? DISPLAYPHY_RX_SELECT : 0) | \
+		((tx) ? DISPLAYPHY_TX_SELECT : 0) | \
+		((core) ? DISPLAYPHY_CORE_SELECT : 0))
+
+struct cz_display_phy_info_entry {
+	uint8_t phy_present;
+	uint8_t active_lane_mapping;
+	uint8_t display_config_type;
+	uint8_t active_number_of_lanes;
+};
+
+#define CZ_MAX_DISPLAYPHY_IDS			10
+
+struct cz_display_phy_info {
+	bool display_phy_access_initialized;
+	struct cz_display_phy_info_entry entries[CZ_MAX_DISPLAYPHY_IDS];
+};
+
+struct cz_power_level {
+	uint32_t engineClock;
+	uint8_t vddcIndex;
+	uint8_t dsDividerIndex;
+	uint8_t ssDividerIndex;
+	uint8_t allowGnbSlow;
+	uint8_t forceNBPstate;
+	uint8_t display_wm;
+	uint8_t vce_wm;
+	uint8_t numSIMDToPowerDown;
+	uint8_t hysteresis_up;
+	uint8_t rsv[3];
+};
+
+struct cz_uvd_clocks {
+	uint32_t vclk;
+	uint32_t dclk;
+	uint32_t vclk_low_divider;
+	uint32_t vclk_high_divider;
+	uint32_t dclk_low_divider;
+	uint32_t dclk_high_divider;
+};
+
+enum cz_pstate_previous_action {
+	DO_NOTHING = 1,
+	FORCE_HIGH,
+	CANCEL_FORCE_HIGH
+};
+
+struct pp_disable_nb_ps_flags {
+	union {
+		struct {
+			uint32_t entry : 1;
+			uint32_t display : 1;
+			uint32_t driver: 1;
+			uint32_t vce : 1;
+			uint32_t uvd : 1;
+			uint32_t acp : 1;
+			uint32_t reserved: 26;
+		} bits;
+		uint32_t u32All;
+	};
+};
+
+struct cz_power_state {
+	unsigned int magic;
+	uint32_t level;
+	struct cz_uvd_clocks uvd_clocks;
+	uint32_t evclk;
+	uint32_t ecclk;
+	uint32_t samclk;
+	uint32_t acpclk;
+	bool need_dfs_bypass;
+	uint32_t nbps_flags;
+	uint32_t bapm_flags;
+	uint8_t dpm_0_pg_nb_ps_low;
+	uint8_t dpm_0_pg_nb_ps_high;
+	uint8_t dpm_x_nb_ps_low;
+	uint8_t dpm_x_nb_ps_high;
+	enum cz_pstate_previous_action action;
+	struct cz_power_level levels[CZ_MAX_HARDWARE_POWERLEVELS];
+	struct pp_disable_nb_ps_flags disable_nb_ps_flag;
+};
+
+#define DPMFlags_SCLK_Enabled			0x00000001
+#define DPMFlags_UVD_Enabled			0x00000002
+#define DPMFlags_VCE_Enabled			0x00000004
+#define DPMFlags_ACP_Enabled			0x00000008
+#define DPMFlags_ForceHighestValid		0x40000000
+#define DPMFlags_Debug				0x80000000
+
+#define SMU_EnabledFeatureScoreboard_AcpDpmOn   0x00000001 /* bit 0 */
+#define SMU_EnabledFeatureScoreboard_SclkDpmOn    0x00200000
+#define SMU_EnabledFeatureScoreboard_UvdDpmOn   0x00800000 /* bit 23 */
+#define SMU_EnabledFeatureScoreboard_VceDpmOn   0x01000000 /* bit 24 */
+
+struct cc6_settings {
+	bool cc6_setting_changed;
+	bool nb_pstate_switch_disable;/* controls NB PState switch */
+	bool cpu_cc6_disable; /* controls CPU CState switch ( on or off) */
+	bool cpu_pstate_disable;
+	uint32_t cpu_pstate_separation_time;
+};
+
+struct cz_hwmgr {
+	uint32_t activity_target[CZ_MAX_HARDWARE_POWERLEVELS];
+	uint32_t dpm_interval;
+
+	uint32_t voltage_drop_threshold;
+
+	uint32_t voting_rights_clients;
+
+	uint32_t disable_driver_thermal_policy;
+
+	uint32_t static_screen_threshold;
+
+	uint32_t gfx_power_gating_threshold;
+
+	uint32_t activity_hysteresis;
+	uint32_t bootup_sclk_divider;
+	uint32_t gfx_ramp_step;
+	uint32_t gfx_ramp_delay; /* in micro-seconds */
+
+	uint32_t thermal_auto_throttling_treshold;
+
+	struct cz_sys_info sys_info;
+
+	struct cz_power_level boot_power_level;
+	struct cz_power_state *cz_current_ps;
+	struct cz_power_state *cz_requested_ps;
+
+	uint32_t mgcg_cgtt_local0;
+	uint32_t mgcg_cgtt_local1;
+
+	uint32_t tdr_clock; /* in 10khz unit */
+
+	uint32_t ddi_power_gating_disabled;
+	uint32_t disable_gfx_power_gating_in_uvd;
+	uint32_t disable_nb_ps3_in_battery;
+
+	uint32_t lock_nb_ps_in_uvd_play_back;
+
+	struct cz_display_phy_info display_phy_info;
+	uint32_t vce_slow_sclk_threshold; /* default 200mhz */
+	uint32_t dce_slow_sclk_threshold; /* default 300mhz */
+	uint32_t min_sclk_did;  /* minimum sclk divider */
+
+	bool disp_clk_bypass;
+	bool disp_clk_bypass_pending;
+	uint32_t bapm_enabled;
+	uint32_t clock_slow_down_freq;
+	uint32_t skip_clock_slow_down;
+	uint32_t enable_nb_ps_policy;
+	uint32_t voltage_drop_in_dce_power_gating;
+	uint32_t uvd_dpm_interval;
+	uint32_t override_dynamic_mgpg;
+	uint32_t lclk_deep_enabled;
+
+	uint32_t uvd_performance;
+
+	bool video_start;
+	bool battery_state;
+	uint32_t lowest_valid;
+	uint32_t highest_valid;
+	uint32_t high_voltage_threshold;
+	uint32_t is_nb_dpm_enabled;
+	struct cc6_settings cc6_settings;
+	uint32_t is_voltage_island_enabled;
+
+	bool pgacpinit;
+
+	uint8_t disp_config;
+
+	/* PowerTune */
+	uint32_t power_containment_features;
+	bool cac_enabled;
+	bool disable_uvd_power_tune_feature;
+	bool enable_ba_pm_feature;
+	bool enable_tdc_limit_feature;
+
+	uint32_t sram_end;
+	uint32_t dpm_table_start;
+	uint32_t soft_regs_start;
+
+	uint8_t uvd_level_count;
+	uint8_t vce_level_count;
+
+	uint8_t acp_level_count;
+	uint8_t samu_level_count;
+	uint32_t fps_high_threshold;
+	uint32_t fps_low_threshold;
+
+	uint32_t dpm_flags;
+	struct cz_dpm_entry sclk_dpm;
+	struct cz_dpm_entry uvd_dpm;
+	struct cz_dpm_entry vce_dpm;
+	struct cz_dpm_entry acp_dpm;
+
+	uint8_t uvd_boot_level;
+	uint8_t vce_boot_level;
+	uint8_t acp_boot_level;
+	uint8_t samu_boot_level;
+	uint8_t uvd_interval;
+	uint8_t vce_interval;
+	uint8_t acp_interval;
+	uint8_t samu_interval;
+
+	uint8_t graphics_interval;
+	uint8_t graphics_therm_throttle_enable;
+	uint8_t graphics_voltage_change_enable;
+
+	uint8_t graphics_clk_slow_enable;
+	uint8_t graphics_clk_slow_divider;
+
+	uint32_t display_cac;
+	uint32_t low_sclk_interrupt_threshold;
+
+	uint32_t dram_log_addr_h;
+	uint32_t dram_log_addr_l;
+	uint32_t dram_log_phy_addr_h;
+	uint32_t dram_log_phy_addr_l;
+	uint32_t dram_log_buff_size;
+
+	bool uvd_power_gated;
+	bool vce_power_gated;
+	bool samu_power_gated;
+	bool acp_power_gated;
+	bool acp_power_up_no_dsp;
+	uint32_t active_process_mask;
+
+	uint32_t max_sclk_level;
+	uint32_t num_of_clk_entries;
+};
+
+struct pp_hwmgr;
+
+int cz_hwmgr_init(struct pp_hwmgr *hwmgr);
+int cz_dpm_powerdown_uvd(struct pp_hwmgr *hwmgr);
+int cz_dpm_powerup_uvd(struct pp_hwmgr *hwmgr);
+int cz_dpm_powerdown_vce(struct pp_hwmgr *hwmgr);
+int cz_dpm_powerup_vce(struct pp_hwmgr *hwmgr);
+int cz_dpm_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate);
+int  cz_dpm_update_vce_dpm(struct pp_hwmgr *hwmgr);
+#endif /* _CZ_HWMGR_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.c b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.c
new file mode 100644
index 000000000000..e68edf06ed73
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.c
@@ -0,0 +1,114 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "hwmgr.h"
+#include "fiji_clockpowergating.h"
+#include "fiji_ppsmc.h"
+#include "fiji_hwmgr.h"
+
+int fiji_phm_disable_clock_power_gating(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	data->uvd_power_gated = false;
+	data->vce_power_gated = false;
+	data->samu_power_gated = false;
+	data->acp_power_gated = false;
+
+	return 0;
+}
+
+int fiji_phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (data->uvd_power_gated == bgate)
+		return 0;
+
+	data->uvd_power_gated = bgate;
+
+	if (bgate)
+		fiji_update_uvd_dpm(hwmgr, true);
+	else
+		fiji_update_uvd_dpm(hwmgr, false);
+
+	return 0;
+}
+
+int fiji_phm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_set_power_state_input states;
+	const struct pp_power_state  *pcurrent;
+	struct pp_power_state  *requested;
+
+	if (data->vce_power_gated == bgate)
+		return 0;
+
+	data->vce_power_gated = bgate;
+
+	pcurrent = hwmgr->current_ps;
+	requested = hwmgr->request_ps;
+
+	states.pcurrent_state = &(pcurrent->hardware);
+	states.pnew_state = &(requested->hardware);
+
+	fiji_update_vce_dpm(hwmgr, &states);
+	fiji_enable_disable_vce_dpm(hwmgr, !bgate);
+
+	return 0;
+}
+
+int fiji_phm_powergate_samu(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (data->samu_power_gated == bgate)
+		return 0;
+
+	data->samu_power_gated = bgate;
+
+	if (bgate)
+		fiji_update_samu_dpm(hwmgr, true);
+	else
+		fiji_update_samu_dpm(hwmgr, false);
+
+	return 0;
+}
+
+int fiji_phm_powergate_acp(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (data->acp_power_gated == bgate)
+		return 0;
+
+	data->acp_power_gated = bgate;
+
+	if (bgate)
+		fiji_update_acp_dpm(hwmgr, true);
+	else
+		fiji_update_acp_dpm(hwmgr, false);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.h b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.h
new file mode 100644
index 000000000000..33af5f511ab8
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_clockpowergating.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _FIJI_CLOCK_POWER_GATING_H_
+#define _FIJI_CLOCK_POWER_GATING_H_
+
+#include "fiji_hwmgr.h"
+#include "pp_asicblocks.h"
+
+extern int fiji_phm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate);
+extern int fiji_phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate);
+extern int fiji_phm_powergate_samu(struct pp_hwmgr *hwmgr, bool bgate);
+extern int fiji_phm_powergate_acp(struct pp_hwmgr *hwmgr, bool bgate);
+extern int fiji_phm_disable_clock_power_gating(struct pp_hwmgr *hwmgr);
+#endif /* _TONGA_CLOCK_POWER_GATING_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_dyn_defaults.h b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_dyn_defaults.h
new file mode 100644
index 000000000000..32d43e8fecb2
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_dyn_defaults.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef FIJI_DYN_DEFAULTS_H
+#define FIJI_DYN_DEFAULTS_H
+
+/** \file
+* Volcanic Islands Dynamic default parameters.
+*/
+
+enum FIJIdpm_TrendDetection
+{
+    FIJIAdpm_TrendDetection_AUTO,
+    FIJIAdpm_TrendDetection_UP,
+    FIJIAdpm_TrendDetection_DOWN
+};
+typedef enum FIJIdpm_TrendDetection FIJIdpm_TrendDetection;
+
+/* We need to fill in the default values!!!!!!!!!!!!!!!!!!!!!!! */
+
+/* Bit vector representing same fields as hardware register. */
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT0    0x3FFFC102  /* CP_Gfx_busy ????
+                                                         * HDP_busy
+                                                         * IH_busy
+                                                         * UVD_busy
+                                                         * VCE_busy
+                                                         * ACP_busy
+                                                         * SAMU_busy
+                                                         * SDMA enabled */
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT1    0x000400  /* FE_Gfx_busy  - Intended for primary usage.   Rest are for flexibility. ????
+                                                       * SH_Gfx_busy
+                                                       * RB_Gfx_busy
+                                                       * VCE_busy */
+
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT2    0xC00080  /* SH_Gfx_busy - Intended for primary usage.   Rest are for flexibility.
+                                                       * FE_Gfx_busy
+                                                       * RB_Gfx_busy
+                                                       * ACP_busy */
+
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT3    0xC00200  /* RB_Gfx_busy - Intended for primary usage.   Rest are for flexibility.
+                                                       * FE_Gfx_busy
+                                                       * SH_Gfx_busy
+                                                       * UVD_busy */
+
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT4    0xC01680  /* UVD_busy
+                                                       * VCE_busy
+                                                       * ACP_busy
+                                                       * SAMU_busy */
+
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT5    0xC00033  /* GFX, HDP */
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT6    0xC00033  /* GFX, HDP */
+#define PPFIJI_VOTINGRIGHTSCLIENTS_DFLT7    0x3FFFC000  /* GFX, HDP */
+
+
+/* thermal protection counter (units). */
+#define PPFIJI_THERMALPROTECTCOUNTER_DFLT            0x200 /* ~19us */
+
+/* static screen threshold unit */
+#define PPFIJI_STATICSCREENTHRESHOLDUNIT_DFLT    0
+
+/* static screen threshold */
+#define PPFIJI_STATICSCREENTHRESHOLD_DFLT        0x00C8
+
+/* gfx idle clock stop threshold */
+#define PPFIJI_GFXIDLECLOCKSTOPTHRESHOLD_DFLT        0x200 /* ~19us with static screen threshold unit of 0 */
+
+/* Fixed reference divider to use when building baby stepping tables. */
+#define PPFIJI_REFERENCEDIVIDER_DFLT                  4
+
+/* ULV voltage change delay time
+ * Used to be delay_vreg in N.I. split for S.I.
+ * Using N.I. delay_vreg value as default
+ * ReferenceClock = 2700
+ * VoltageResponseTime = 1000
+ * VDDCDelayTime = (VoltageResponseTime * ReferenceClock) / 1600 = 1687
+ */
+#define PPFIJI_ULVVOLTAGECHANGEDELAY_DFLT             1687
+
+#define PPFIJI_CGULVPARAMETER_DFLT			0x00040035
+#define PPFIJI_CGULVCONTROL_DFLT			0x00007450
+#define PPFIJI_TARGETACTIVITY_DFLT			30 /* 30%*/
+#define PPFIJI_MCLK_TARGETACTIVITY_DFLT		10 /* 10% */
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.c
new file mode 100644
index 000000000000..28031a7eddba
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.c
@@ -0,0 +1,5127 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include "linux/delay.h"
+
+#include "hwmgr.h"
+#include "fiji_smumgr.h"
+#include "atombios.h"
+#include "hardwaremanager.h"
+#include "ppatomctrl.h"
+#include "atombios.h"
+#include "cgs_common.h"
+#include "fiji_dyn_defaults.h"
+#include "fiji_powertune.h"
+#include "smu73.h"
+#include "smu/smu_7_1_3_d.h"
+#include "smu/smu_7_1_3_sh_mask.h"
+#include "gmc/gmc_8_1_d.h"
+#include "gmc/gmc_8_1_sh_mask.h"
+#include "bif/bif_5_0_d.h"
+#include "bif/bif_5_0_sh_mask.h"
+#include "dce/dce_10_0_d.h"
+#include "dce/dce_10_0_sh_mask.h"
+#include "pppcielanes.h"
+#include "fiji_hwmgr.h"
+#include "tonga_processpptables.h"
+#include "tonga_pptable.h"
+#include "pp_debug.h"
+#include "pp_acpi.h"
+#include "amd_pcie_helpers.h"
+#include "cgs_linux.h"
+#include "ppinterrupt.h"
+
+#include "fiji_clockpowergating.h"
+#include "fiji_thermal.h"
+
+#define VOLTAGE_SCALE	4
+#define SMC_RAM_END		0x40000
+#define VDDC_VDDCI_DELTA	300
+
+#define MC_SEQ_MISC0_GDDR5_SHIFT 28
+#define MC_SEQ_MISC0_GDDR5_MASK  0xf0000000
+#define MC_SEQ_MISC0_GDDR5_VALUE 5
+
+#define MC_CG_ARB_FREQ_F0           0x0a /* boot-up default */
+#define MC_CG_ARB_FREQ_F1           0x0b
+#define MC_CG_ARB_FREQ_F2           0x0c
+#define MC_CG_ARB_FREQ_F3           0x0d
+
+/* From smc_reg.h */
+#define SMC_CG_IND_START            0xc0030000
+#define SMC_CG_IND_END              0xc0040000  /* First byte after SMC_CG_IND */
+
+#define VOLTAGE_SCALE               4
+#define VOLTAGE_VID_OFFSET_SCALE1   625
+#define VOLTAGE_VID_OFFSET_SCALE2   100
+
+#define VDDC_VDDCI_DELTA            300
+
+#define ixSWRST_COMMAND_1           0x1400103
+#define MC_SEQ_CNTL__CAC_EN_MASK    0x40000000
+
+/** Values for the CG_THERMAL_CTRL::DPM_EVENT_SRC field. */
+enum DPM_EVENT_SRC {
+    DPM_EVENT_SRC_ANALOG = 0,               /* Internal analog trip point */
+    DPM_EVENT_SRC_EXTERNAL = 1,             /* External (GPIO 17) signal */
+    DPM_EVENT_SRC_DIGITAL = 2,              /* Internal digital trip point (DIG_THERM_DPM) */
+    DPM_EVENT_SRC_ANALOG_OR_EXTERNAL = 3,   /* Internal analog or external */
+    DPM_EVENT_SRC_DIGITAL_OR_EXTERNAL = 4   /* Internal digital or external */
+};
+
+
+/* [2.5%,~2.5%] Clock stretched is multiple of 2.5% vs
+ * not and [Fmin, Fmax, LDO_REFSEL, USE_FOR_LOW_FREQ]
+ */
+uint16_t fiji_clock_stretcher_lookup_table[2][4] = { {600, 1050, 3, 0},
+                                                {600, 1050, 6, 1} };
+
+/* [FF, SS] type, [] 4 voltage ranges, and
+ * [Floor Freq, Boundary Freq, VID min , VID max]
+ */
+uint32_t fiji_clock_stretcher_ddt_table[2][4][4] =
+{ { {265, 529, 120, 128}, {325, 650, 96, 119}, {430, 860, 32, 95}, {0, 0, 0, 31} },
+  { {275, 550, 104, 112}, {319, 638, 96, 103}, {360, 720, 64, 95}, {384, 768, 32, 63} } };
+
+/* [Use_For_Low_freq] value, [0%, 5%, 10%, 7.14%, 14.28%, 20%]
+ * (coming from PWR_CKS_CNTL.stretch_amount reg spec)
+ */
+uint8_t fiji_clock_stretch_amount_conversion[2][6] = { {0, 1, 3, 2, 4, 5},
+                                                  {0, 2, 4, 5, 6, 5} };
+
+const unsigned long PhwFiji_Magic = (unsigned long)(PHM_VIslands_Magic);
+
+struct fiji_power_state *cast_phw_fiji_power_state(
+				  struct pp_hw_power_state *hw_ps)
+{
+	PP_ASSERT_WITH_CODE((PhwFiji_Magic == hw_ps->magic),
+				"Invalid Powerstate Type!",
+				 return NULL;);
+
+	return (struct fiji_power_state *)hw_ps;
+}
+
+const struct fiji_power_state *cast_const_phw_fiji_power_state(
+				 const struct pp_hw_power_state *hw_ps)
+{
+	PP_ASSERT_WITH_CODE((PhwFiji_Magic == hw_ps->magic),
+				"Invalid Powerstate Type!",
+				 return NULL;);
+
+	return (const struct fiji_power_state *)hw_ps;
+}
+
+static bool fiji_is_dpm_running(struct pp_hwmgr *hwmgr)
+{
+	return (1 == PHM_READ_INDIRECT_FIELD(hwmgr->device,
+			CGS_IND_REG__SMC, FEATURE_STATUS, VOLTAGE_CONTROLLER_ON))
+			? true : false;
+}
+
+static void fiji_init_dpm_defaults(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_ulv_parm *ulv = &data->ulv;
+
+	ulv->cg_ulv_parameter = PPFIJI_CGULVPARAMETER_DFLT;
+	data->voting_rights_clients0 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT0;
+	data->voting_rights_clients1 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT1;
+	data->voting_rights_clients2 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT2;
+	data->voting_rights_clients3 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT3;
+	data->voting_rights_clients4 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT4;
+	data->voting_rights_clients5 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT5;
+	data->voting_rights_clients6 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT6;
+	data->voting_rights_clients7 = PPFIJI_VOTINGRIGHTSCLIENTS_DFLT7;
+
+	data->static_screen_threshold_unit =
+			PPFIJI_STATICSCREENTHRESHOLDUNIT_DFLT;
+	data->static_screen_threshold =
+			PPFIJI_STATICSCREENTHRESHOLD_DFLT;
+
+	/* Unset ABM cap as it moved to DAL.
+	 * Add PHM_PlatformCaps_NonABMSupportInPPLib
+	 * for re-direct ABM related request to DAL
+	 */
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ABM);
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_NonABMSupportInPPLib);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DynamicACTiming);
+
+	fiji_initialize_power_tune_defaults(hwmgr);
+
+	data->mclk_stutter_mode_threshold = 60000;
+	data->pcie_gen_performance.max = PP_PCIEGen1;
+	data->pcie_gen_performance.min = PP_PCIEGen3;
+	data->pcie_gen_power_saving.max = PP_PCIEGen1;
+	data->pcie_gen_power_saving.min = PP_PCIEGen3;
+	data->pcie_lane_performance.max = 0;
+	data->pcie_lane_performance.min = 16;
+	data->pcie_lane_power_saving.max = 0;
+	data->pcie_lane_power_saving.min = 16;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DynamicUVDState);
+}
+
+static int fiji_get_sclk_for_voltage_evv(struct pp_hwmgr *hwmgr,
+	phm_ppt_v1_voltage_lookup_table *lookup_table,
+	uint16_t virtual_voltage_id, int32_t *sclk)
+{
+	uint8_t entryId;
+	uint8_t voltageId;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	PP_ASSERT_WITH_CODE(lookup_table->count != 0, "Lookup table is empty", return -EINVAL);
+
+	/* search for leakage voltage ID 0xff01 ~ 0xff08 and sckl */
+	for (entryId = 0; entryId < table_info->vdd_dep_on_sclk->count; entryId++) {
+		voltageId = table_info->vdd_dep_on_sclk->entries[entryId].vddInd;
+		if (lookup_table->entries[voltageId].us_vdd == virtual_voltage_id)
+			break;
+	}
+
+	PP_ASSERT_WITH_CODE(entryId < table_info->vdd_dep_on_sclk->count,
+			"Can't find requested voltage id in vdd_dep_on_sclk table!",
+			return -EINVAL;
+			);
+
+	*sclk = table_info->vdd_dep_on_sclk->entries[entryId].clk;
+
+	return 0;
+}
+
+/**
+* Get Leakage VDDC based on leakage ID.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+*/
+static int fiji_get_evv_voltages(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint16_t    vv_id;
+	uint16_t    vddc = 0;
+	uint16_t    evv_default = 1150;
+	uint16_t    i, j;
+	uint32_t  sclk = 0;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)hwmgr->pptable;
+	struct phm_ppt_v1_clock_voltage_dependency_table *sclk_table =
+			table_info->vdd_dep_on_sclk;
+	int result;
+
+	for (i = 0; i < FIJI_MAX_LEAKAGE_COUNT; i++) {
+		vv_id = ATOM_VIRTUAL_VOLTAGE_ID0 + i;
+		if (!fiji_get_sclk_for_voltage_evv(hwmgr,
+				table_info->vddc_lookup_table, vv_id, &sclk)) {
+			if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_ClockStretcher)) {
+				for (j = 1; j < sclk_table->count; j++) {
+					if (sclk_table->entries[j].clk == sclk &&
+							sclk_table->entries[j].cks_enable == 0) {
+						sclk += 5000;
+						break;
+					}
+				}
+			}
+
+			if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_EnableDriverEVV))
+				result = atomctrl_calculate_voltage_evv_on_sclk(hwmgr,
+						VOLTAGE_TYPE_VDDC, sclk, vv_id, &vddc, i, true);
+			else
+				result = -EINVAL;
+
+			if (result)
+				result = atomctrl_get_voltage_evv_on_sclk(hwmgr,
+						VOLTAGE_TYPE_VDDC, sclk,vv_id, &vddc);
+
+			/* need to make sure vddc is less than 2v or else, it could burn the ASIC. */
+			PP_ASSERT_WITH_CODE((vddc < 2000),
+					"Invalid VDDC value, greater than 2v!", result = -EINVAL;);
+
+			if (result)
+				/* 1.15V is the default safe value for Fiji */
+				vddc = evv_default;
+
+			/* the voltage should not be zero nor equal to leakage ID */
+			if (vddc != 0 && vddc != vv_id) {
+				data->vddc_leakage.actual_voltage
+				[data->vddc_leakage.count] = vddc;
+				data->vddc_leakage.leakage_id
+				[data->vddc_leakage.count] = vv_id;
+				data->vddc_leakage.count++;
+			}
+		}
+	}
+	return 0;
+}
+
+/**
+ * Change virtual leakage voltage to actual value.
+ *
+ * @param     hwmgr  the address of the powerplay hardware manager.
+ * @param     pointer to changing voltage
+ * @param     pointer to leakage table
+ */
+static void fiji_patch_with_vdd_leakage(struct pp_hwmgr *hwmgr,
+		uint16_t *voltage, struct fiji_leakage_voltage *leakage_table)
+{
+	uint32_t index;
+
+	/* search for leakage voltage ID 0xff01 ~ 0xff08 */
+	for (index = 0; index < leakage_table->count; index++) {
+		/* if this voltage matches a leakage voltage ID */
+		/* patch with actual leakage voltage */
+		if (leakage_table->leakage_id[index] == *voltage) {
+			*voltage = leakage_table->actual_voltage[index];
+			break;
+		}
+	}
+
+	if (*voltage > ATOM_VIRTUAL_VOLTAGE_ID0)
+		printk(KERN_ERR "Voltage value looks like a Leakage ID but it's not patched \n");
+}
+
+/**
+* Patch voltage lookup table by EVV leakages.
+*
+* @param     hwmgr  the address of the powerplay hardware manager.
+* @param     pointer to voltage lookup table
+* @param     pointer to leakage table
+* @return     always 0
+*/
+static int fiji_patch_lookup_table_with_leakage(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_voltage_lookup_table *lookup_table,
+		struct fiji_leakage_voltage *leakage_table)
+{
+	uint32_t i;
+
+	for (i = 0; i < lookup_table->count; i++)
+		fiji_patch_with_vdd_leakage(hwmgr,
+				&lookup_table->entries[i].us_vdd, leakage_table);
+
+	return 0;
+}
+
+static int fiji_patch_clock_voltage_limits_with_vddc_leakage(
+		struct pp_hwmgr *hwmgr, struct fiji_leakage_voltage *leakage_table,
+		uint16_t *vddc)
+{
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	fiji_patch_with_vdd_leakage(hwmgr, (uint16_t *)vddc, leakage_table);
+	hwmgr->dyn_state.max_clock_voltage_on_dc.vddc =
+			table_info->max_clock_voltage_on_dc.vddc;
+	return 0;
+}
+
+static int fiji_patch_voltage_dependency_tables_with_lookup_table(
+		struct pp_hwmgr *hwmgr)
+{
+	uint8_t entryId;
+	uint8_t voltageId;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	struct phm_ppt_v1_clock_voltage_dependency_table *sclk_table =
+			table_info->vdd_dep_on_sclk;
+	struct phm_ppt_v1_clock_voltage_dependency_table *mclk_table =
+			table_info->vdd_dep_on_mclk;
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table =
+			table_info->mm_dep_table;
+
+	for (entryId = 0; entryId < sclk_table->count; ++entryId) {
+		voltageId = sclk_table->entries[entryId].vddInd;
+		sclk_table->entries[entryId].vddc =
+				table_info->vddc_lookup_table->entries[voltageId].us_vdd;
+	}
+
+	for (entryId = 0; entryId < mclk_table->count; ++entryId) {
+		voltageId = mclk_table->entries[entryId].vddInd;
+		mclk_table->entries[entryId].vddc =
+			table_info->vddc_lookup_table->entries[voltageId].us_vdd;
+	}
+
+	for (entryId = 0; entryId < mm_table->count; ++entryId) {
+		voltageId = mm_table->entries[entryId].vddcInd;
+		mm_table->entries[entryId].vddc =
+			table_info->vddc_lookup_table->entries[voltageId].us_vdd;
+	}
+
+	return 0;
+
+}
+
+static int fiji_calc_voltage_dependency_tables(struct pp_hwmgr *hwmgr)
+{
+	/* Need to determine if we need calculated voltage. */
+	return 0;
+}
+
+static int fiji_calc_mm_voltage_dependency_table(struct pp_hwmgr *hwmgr)
+{
+	/* Need to determine if we need calculated voltage from mm table. */
+	return 0;
+}
+
+static int fiji_sort_lookup_table(struct pp_hwmgr *hwmgr,
+		struct phm_ppt_v1_voltage_lookup_table *lookup_table)
+{
+	uint32_t table_size, i, j;
+	struct phm_ppt_v1_voltage_lookup_record tmp_voltage_lookup_record;
+	table_size = lookup_table->count;
+
+	PP_ASSERT_WITH_CODE(0 != lookup_table->count,
+		"Lookup table is empty", return -EINVAL);
+
+	/* Sorting voltages */
+	for (i = 0; i < table_size - 1; i++) {
+		for (j = i + 1; j > 0; j--) {
+			if (lookup_table->entries[j].us_vdd <
+					lookup_table->entries[j - 1].us_vdd) {
+				tmp_voltage_lookup_record = lookup_table->entries[j - 1];
+				lookup_table->entries[j - 1] = lookup_table->entries[j];
+				lookup_table->entries[j] = tmp_voltage_lookup_record;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_complete_dependency_tables(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+	int tmp_result;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	tmp_result = fiji_patch_lookup_table_with_leakage(hwmgr,
+			table_info->vddc_lookup_table, &(data->vddc_leakage));
+	if (tmp_result)
+		result = tmp_result;
+
+	tmp_result = fiji_patch_clock_voltage_limits_with_vddc_leakage(hwmgr,
+			&(data->vddc_leakage), &table_info->max_clock_voltage_on_dc.vddc);
+	if (tmp_result)
+		result = tmp_result;
+
+	tmp_result = fiji_patch_voltage_dependency_tables_with_lookup_table(hwmgr);
+	if (tmp_result)
+		result = tmp_result;
+
+	tmp_result = fiji_calc_voltage_dependency_tables(hwmgr);
+	if (tmp_result)
+		result = tmp_result;
+
+	tmp_result = fiji_calc_mm_voltage_dependency_table(hwmgr);
+	if (tmp_result)
+		result = tmp_result;
+
+	tmp_result = fiji_sort_lookup_table(hwmgr, table_info->vddc_lookup_table);
+	if(tmp_result)
+		result = tmp_result;
+
+	return result;
+}
+
+static int fiji_set_private_data_based_on_pptable(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	struct phm_ppt_v1_clock_voltage_dependency_table *allowed_sclk_vdd_table =
+			table_info->vdd_dep_on_sclk;
+	struct phm_ppt_v1_clock_voltage_dependency_table *allowed_mclk_vdd_table =
+			table_info->vdd_dep_on_mclk;
+
+	PP_ASSERT_WITH_CODE(allowed_sclk_vdd_table != NULL,
+		"VDD dependency on SCLK table is missing. 	\
+		This table is mandatory", return -EINVAL);
+	PP_ASSERT_WITH_CODE(allowed_sclk_vdd_table->count >= 1,
+		"VDD dependency on SCLK table has to have is missing. 	\
+		This table is mandatory", return -EINVAL);
+
+	PP_ASSERT_WITH_CODE(allowed_mclk_vdd_table != NULL,
+		"VDD dependency on MCLK table is missing. 	\
+		This table is mandatory", return -EINVAL);
+	PP_ASSERT_WITH_CODE(allowed_mclk_vdd_table->count >= 1,
+		"VDD dependency on MCLK table has to have is missing.	 \
+		This table is mandatory", return -EINVAL);
+
+	data->min_vddc_in_pptable = (uint16_t)allowed_sclk_vdd_table->entries[0].vddc;
+	data->max_vddc_in_pptable =	(uint16_t)allowed_sclk_vdd_table->
+			entries[allowed_sclk_vdd_table->count - 1].vddc;
+
+	table_info->max_clock_voltage_on_ac.sclk =
+		allowed_sclk_vdd_table->entries[allowed_sclk_vdd_table->count - 1].clk;
+	table_info->max_clock_voltage_on_ac.mclk =
+		allowed_mclk_vdd_table->entries[allowed_mclk_vdd_table->count - 1].clk;
+	table_info->max_clock_voltage_on_ac.vddc =
+		allowed_sclk_vdd_table->entries[allowed_sclk_vdd_table->count - 1].vddc;
+	table_info->max_clock_voltage_on_ac.vddci =
+		allowed_mclk_vdd_table->entries[allowed_mclk_vdd_table->count - 1].vddci;
+
+	hwmgr->dyn_state.max_clock_voltage_on_ac.sclk =
+		table_info->max_clock_voltage_on_ac.sclk;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.mclk =
+		table_info->max_clock_voltage_on_ac.mclk;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.vddc =
+		table_info->max_clock_voltage_on_ac.vddc;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.vddci =
+		table_info->max_clock_voltage_on_ac.vddci;
+
+	return 0;
+}
+
+static uint16_t fiji_get_current_pcie_speed(struct pp_hwmgr *hwmgr)
+{
+	uint32_t speedCntl = 0;
+
+	/* mmPCIE_PORT_INDEX rename as mmPCIE_INDEX */
+	speedCntl = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__PCIE,
+			ixPCIE_LC_SPEED_CNTL);
+	return((uint16_t)PHM_GET_FIELD(speedCntl,
+			PCIE_LC_SPEED_CNTL, LC_CURRENT_DATA_RATE));
+}
+
+static int fiji_get_current_pcie_lane_number(struct pp_hwmgr *hwmgr)
+{
+	uint32_t link_width;
+
+	/* mmPCIE_PORT_INDEX rename as mmPCIE_INDEX */
+	link_width = PHM_READ_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__PCIE,
+			PCIE_LC_LINK_WIDTH_CNTL, LC_LINK_WIDTH_RD);
+
+	PP_ASSERT_WITH_CODE((7 >= link_width),
+			"Invalid PCIe lane width!", return 0);
+
+	return decode_pcie_lane_width(link_width);
+}
+
+/** Patch the Boot State to match VBIOS boot clocks and voltage.
+*
+* @param hwmgr Pointer to the hardware manager.
+* @param pPowerState The address of the PowerState instance being created.
+*
+*/
+static int fiji_patch_boot_state(struct pp_hwmgr *hwmgr,
+		struct pp_hw_power_state *hw_ps)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_power_state *ps = (struct fiji_power_state *)hw_ps;
+	ATOM_FIRMWARE_INFO_V2_2 *fw_info;
+	uint16_t size;
+	uint8_t frev, crev;
+	int index = GetIndexIntoMasterTable(DATA, FirmwareInfo);
+
+	/* First retrieve the Boot clocks and VDDC from the firmware info table.
+	 * We assume here that fw_info is unchanged if this call fails.
+	 */
+	fw_info = (ATOM_FIRMWARE_INFO_V2_2 *)cgs_atom_get_data_table(
+			hwmgr->device, index,
+			&size, &frev, &crev);
+	if (!fw_info)
+		/* During a test, there is no firmware info table. */
+		return 0;
+
+	/* Patch the state. */
+	data->vbios_boot_state.sclk_bootup_value =
+			le32_to_cpu(fw_info->ulDefaultEngineClock);
+	data->vbios_boot_state.mclk_bootup_value =
+			le32_to_cpu(fw_info->ulDefaultMemoryClock);
+	data->vbios_boot_state.mvdd_bootup_value =
+			le16_to_cpu(fw_info->usBootUpMVDDCVoltage);
+	data->vbios_boot_state.vddc_bootup_value =
+			le16_to_cpu(fw_info->usBootUpVDDCVoltage);
+	data->vbios_boot_state.vddci_bootup_value =
+			le16_to_cpu(fw_info->usBootUpVDDCIVoltage);
+	data->vbios_boot_state.pcie_gen_bootup_value =
+			fiji_get_current_pcie_speed(hwmgr);
+	data->vbios_boot_state.pcie_lane_bootup_value =
+			(uint16_t)fiji_get_current_pcie_lane_number(hwmgr);
+
+	/* set boot power state */
+	ps->performance_levels[0].memory_clock = data->vbios_boot_state.mclk_bootup_value;
+	ps->performance_levels[0].engine_clock = data->vbios_boot_state.sclk_bootup_value;
+	ps->performance_levels[0].pcie_gen = data->vbios_boot_state.pcie_gen_bootup_value;
+	ps->performance_levels[0].pcie_lane = data->vbios_boot_state.pcie_lane_bootup_value;
+
+	return 0;
+}
+
+static int fiji_hwmgr_backend_init(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t i;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	bool stay_in_boot;
+	int result;
+
+	data->dll_default_on = false;
+	data->sram_end = SMC_RAM_END;
+
+	for (i = 0; i < SMU73_MAX_LEVELS_GRAPHICS; i++)
+		data->activity_target[i] = FIJI_AT_DFLT;
+
+	data->vddc_vddci_delta = VDDC_VDDCI_DELTA;
+
+	data->mclk_activity_target = PPFIJI_MCLK_TARGETACTIVITY_DFLT;
+	data->mclk_dpm0_activity_target = 0xa;
+
+	data->sclk_dpm_key_disabled = 0;
+	data->mclk_dpm_key_disabled = 0;
+	data->pcie_dpm_key_disabled = 0;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_UnTabledHardwareInterface);
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_TablelessHardwareInterface);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SclkDeepSleep);
+
+	data->gpio_debug = 0;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DynamicPatchPowerState);
+
+	/* need to set voltage control types before EVV patching */
+	data->voltage_control = FIJI_VOLTAGE_CONTROL_NONE;
+	data->vddci_control = FIJI_VOLTAGE_CONTROL_NONE;
+	data->mvdd_control = FIJI_VOLTAGE_CONTROL_NONE;
+
+	if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+			VOLTAGE_TYPE_VDDC, VOLTAGE_OBJ_SVID2))
+		data->voltage_control = FIJI_VOLTAGE_CONTROL_BY_SVID2;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EnableMVDDControl))
+		if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+				VOLTAGE_TYPE_MVDDC, VOLTAGE_OBJ_GPIO_LUT))
+			data->mvdd_control = FIJI_VOLTAGE_CONTROL_BY_GPIO;
+
+	if (data->mvdd_control == FIJI_VOLTAGE_CONTROL_NONE)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EnableMVDDControl);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ControlVDDCI)) {
+		if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+				VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_GPIO_LUT))
+			data->vddci_control = FIJI_VOLTAGE_CONTROL_BY_GPIO;
+		else if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+				VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_SVID2))
+			data->vddci_control = FIJI_VOLTAGE_CONTROL_BY_SVID2;
+	}
+
+	if (data->vddci_control == FIJI_VOLTAGE_CONTROL_NONE)
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ControlVDDCI);
+
+	if (table_info && table_info->cac_dtp_table->usClockStretchAmount)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ClockStretcher);
+
+	fiji_init_dpm_defaults(hwmgr);
+
+	/* Get leakage voltage based on leakage ID. */
+	fiji_get_evv_voltages(hwmgr);
+
+	/* Patch our voltage dependency table with actual leakage voltage
+	 * We need to perform leakage translation before it's used by other functions
+	 */
+	fiji_complete_dependency_tables(hwmgr);
+
+	/* Parse pptable data read from VBIOS */
+	fiji_set_private_data_based_on_pptable(hwmgr);
+
+	/* ULV Support */
+	data->ulv.ulv_supported = true; /* ULV feature is enabled by default */
+
+	/* Initalize Dynamic State Adjustment Rule Settings */
+	result = tonga_initializa_dynamic_state_adjustment_rule_settings(hwmgr);
+
+	if (!result) {
+		data->uvd_enabled = false;
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_EnableSMU7ThermalManagement);
+		data->vddc_phase_shed_control = false;
+	}
+
+	stay_in_boot = phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_StayInBootState);
+
+	if (0 == result) {
+		struct cgs_system_info sys_info = {0};
+
+		data->is_tlu_enabled = 0;
+		hwmgr->platform_descriptor.hardwareActivityPerformanceLevels =
+				FIJI_MAX_HARDWARE_POWERLEVELS;
+		hwmgr->platform_descriptor.hardwarePerformanceLevels = 2;
+		hwmgr->platform_descriptor.minimumClocksReductionPercentage  = 50;
+
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_FanSpeedInTableIsRPM);
+
+		if (table_info->cac_dtp_table->usDefaultTargetOperatingTemp &&
+				hwmgr->thermal_controller.
+				advanceFanControlParameters.ucFanControlMode) {
+			hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanPWM =
+					hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanPWM;
+			hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanRPM =
+					hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanRPM;
+			hwmgr->dyn_state.cac_dtp_table->usOperatingTempMinLimit =
+					table_info->cac_dtp_table->usOperatingTempMinLimit;
+			hwmgr->dyn_state.cac_dtp_table->usOperatingTempMaxLimit =
+					table_info->cac_dtp_table->usOperatingTempMaxLimit;
+			hwmgr->dyn_state.cac_dtp_table->usDefaultTargetOperatingTemp =
+					table_info->cac_dtp_table->usDefaultTargetOperatingTemp;
+			hwmgr->dyn_state.cac_dtp_table->usOperatingTempStep =
+					table_info->cac_dtp_table->usOperatingTempStep;
+			hwmgr->dyn_state.cac_dtp_table->usTargetOperatingTemp =
+					table_info->cac_dtp_table->usTargetOperatingTemp;
+
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_ODFuzzyFanControlSupport);
+		}
+
+		sys_info.size = sizeof(struct cgs_system_info);
+		sys_info.info_id = CGS_SYSTEM_INFO_PCIE_GEN_INFO;
+		result = cgs_query_system_info(hwmgr->device, &sys_info);
+		if (result)
+			data->pcie_gen_cap = 0x30007;
+		else
+			data->pcie_gen_cap = (uint32_t)sys_info.value;
+		if (data->pcie_gen_cap & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)
+			data->pcie_spc_cap = 20;
+		sys_info.size = sizeof(struct cgs_system_info);
+		sys_info.info_id = CGS_SYSTEM_INFO_PCIE_MLW;
+		result = cgs_query_system_info(hwmgr->device, &sys_info);
+		if (result)
+			data->pcie_lane_cap = 0x2f0000;
+		else
+			data->pcie_lane_cap = (uint32_t)sys_info.value;
+	} else {
+		/* Ignore return value in here, we are cleaning up a mess. */
+		tonga_hwmgr_backend_fini(hwmgr);
+	}
+
+	return 0;
+}
+
+/**
+ * Read clock related registers.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+static int fiji_read_clock_registers(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	data->clock_registers.vCG_SPLL_FUNC_CNTL =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_FUNC_CNTL);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_2 =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_FUNC_CNTL_2);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_3 =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_FUNC_CNTL_3);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_4 =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_FUNC_CNTL_4);
+	data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_SPREAD_SPECTRUM);
+	data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2 =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_SPLL_SPREAD_SPECTRUM_2);
+
+	return 0;
+}
+
+/**
+ * Find out if memory is GDDR5.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+static int fiji_get_memory_type(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t temp;
+
+	temp = cgs_read_register(hwmgr->device, mmMC_SEQ_MISC0);
+
+	data->is_memory_gddr5 = (MC_SEQ_MISC0_GDDR5_VALUE ==
+			((temp & MC_SEQ_MISC0_GDDR5_MASK) >>
+			 MC_SEQ_MISC0_GDDR5_SHIFT));
+
+	return 0;
+}
+
+/**
+ * Enables Dynamic Power Management by SMC
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+static int fiji_enable_acpi_power_management(struct pp_hwmgr *hwmgr)
+{
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			GENERAL_PWRMGT, STATIC_PM_EN, 1);
+
+	return 0;
+}
+
+/**
+ * Initialize PowerGating States for different engines
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+static int fiji_init_power_gate_state(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	data->uvd_power_gated = false;
+	data->vce_power_gated = false;
+	data->samu_power_gated = false;
+	data->acp_power_gated = false;
+	data->pg_acp_init = true;
+
+	return 0;
+}
+
+static int fiji_init_sclk_threshold(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	data->low_sclk_interrupt_threshold = 0;
+
+	return 0;
+}
+
+static int fiji_setup_asic_task(struct pp_hwmgr *hwmgr)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = fiji_read_clock_registers(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to read clock registers!", result = tmp_result);
+
+	tmp_result = fiji_get_memory_type(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to get memory type!", result = tmp_result);
+
+	tmp_result = fiji_enable_acpi_power_management(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable ACPI power management!", result = tmp_result);
+
+	tmp_result = fiji_init_power_gate_state(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to init power gate state!", result = tmp_result);
+
+	tmp_result = tonga_get_mc_microcode_version(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to get MC microcode version!", result = tmp_result);
+
+	tmp_result = fiji_init_sclk_threshold(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to init sclk threshold!", result = tmp_result);
+
+	return result;
+}
+
+/**
+* Checks if we want to support voltage control
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+*/
+static bool fiji_voltage_control(const struct pp_hwmgr *hwmgr)
+{
+	const struct fiji_hwmgr *data =
+			(const struct fiji_hwmgr *)(hwmgr->backend);
+
+	return (FIJI_VOLTAGE_CONTROL_NONE != data->voltage_control);
+}
+
+/**
+* Enable voltage control
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+*/
+static int fiji_enable_voltage_control(struct pp_hwmgr *hwmgr)
+{
+	/* enable voltage control */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			GENERAL_PWRMGT, VOLT_PWRMGT_EN, 1);
+
+	return 0;
+}
+
+/**
+* Remove repeated voltage values and create table with unique values.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    vol_table  the pointer to changing voltage table
+* @return    0 in success
+*/
+
+static int fiji_trim_voltage_table(struct pp_hwmgr *hwmgr,
+		struct pp_atomctrl_voltage_table *vol_table)
+{
+	uint32_t i, j;
+	uint16_t vvalue;
+	bool found = false;
+	struct pp_atomctrl_voltage_table *table;
+
+	PP_ASSERT_WITH_CODE((NULL != vol_table),
+			"Voltage Table empty.", return -EINVAL);
+	table = kzalloc(sizeof(struct pp_atomctrl_voltage_table),
+			GFP_KERNEL);
+
+	if (NULL == table)
+		return -ENOMEM;
+
+	table->mask_low = vol_table->mask_low;
+	table->phase_delay = vol_table->phase_delay;
+
+	for (i = 0; i < vol_table->count; i++) {
+		vvalue = vol_table->entries[i].value;
+		found = false;
+
+		for (j = 0; j < table->count; j++) {
+			if (vvalue == table->entries[j].value) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			table->entries[table->count].value = vvalue;
+			table->entries[table->count].smio_low =
+					vol_table->entries[i].smio_low;
+			table->count++;
+		}
+	}
+
+	memcpy(vol_table, table, sizeof(struct pp_atomctrl_voltage_table));
+	kfree(table);
+
+	return 0;
+}
+
+static int fiji_get_svi2_mvdd_voltage_table(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_clock_voltage_dependency_table *dep_table)
+{
+	uint32_t i;
+	int result;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct pp_atomctrl_voltage_table *vol_table = &(data->mvdd_voltage_table);
+
+	PP_ASSERT_WITH_CODE((0 != dep_table->count),
+			"Voltage Dependency Table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+	vol_table->count = dep_table->count;
+
+	for (i = 0; i < dep_table->count; i++) {
+		vol_table->entries[i].value = dep_table->entries[i].mvdd;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	result = fiji_trim_voltage_table(hwmgr, vol_table);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to trim MVDD table.", return result);
+
+	return 0;
+}
+
+static int fiji_get_svi2_vddci_voltage_table(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_clock_voltage_dependency_table *dep_table)
+{
+	uint32_t i;
+	int result;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct pp_atomctrl_voltage_table *vol_table = &(data->vddci_voltage_table);
+
+	PP_ASSERT_WITH_CODE((0 != dep_table->count),
+			"Voltage Dependency Table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+	vol_table->count = dep_table->count;
+
+	for (i = 0; i < dep_table->count; i++) {
+		vol_table->entries[i].value = dep_table->entries[i].vddci;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	result = fiji_trim_voltage_table(hwmgr, vol_table);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to trim VDDCI table.", return result);
+
+	return 0;
+}
+
+static int fiji_get_svi2_vdd_voltage_table(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_voltage_lookup_table *lookup_table)
+{
+	int i = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct pp_atomctrl_voltage_table *vol_table = &(data->vddc_voltage_table);
+
+	PP_ASSERT_WITH_CODE((0 != lookup_table->count),
+			"Voltage Lookup Table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+
+	vol_table->count = lookup_table->count;
+
+	for (i = 0; i < vol_table->count; i++) {
+		vol_table->entries[i].value = lookup_table->entries[i].us_vdd;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	return 0;
+}
+
+/* ---- Voltage Tables ----
+ * If the voltage table would be bigger than
+ * what will fit into the state table on
+ * the SMC keep only the higher entries.
+ */
+static void fiji_trim_voltage_table_to_fit_state_table(struct pp_hwmgr *hwmgr,
+		uint32_t max_vol_steps, struct pp_atomctrl_voltage_table *vol_table)
+{
+	unsigned int i, diff;
+
+	if (vol_table->count <= max_vol_steps)
+		return;
+
+	diff = vol_table->count - max_vol_steps;
+
+	for (i = 0; i < max_vol_steps; i++)
+		vol_table->entries[i] = vol_table->entries[i + diff];
+
+	vol_table->count = max_vol_steps;
+
+	return;
+}
+
+/**
+* Create Voltage Tables.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+*/
+static int fiji_construct_voltage_tables(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)hwmgr->pptable;
+	int result;
+
+	if (FIJI_VOLTAGE_CONTROL_BY_GPIO == data->mvdd_control) {
+		result = atomctrl_get_voltage_table_v3(hwmgr,
+				VOLTAGE_TYPE_MVDDC,	VOLTAGE_OBJ_GPIO_LUT,
+				&(data->mvdd_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to retrieve MVDD table.",
+				return result);
+	} else if (FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->mvdd_control) {
+		result = fiji_get_svi2_mvdd_voltage_table(hwmgr,
+				table_info->vdd_dep_on_mclk);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to retrieve SVI2 MVDD table from dependancy table.",
+				return result;);
+	}
+
+	if (FIJI_VOLTAGE_CONTROL_BY_GPIO == data->vddci_control) {
+		result = atomctrl_get_voltage_table_v3(hwmgr,
+				VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_GPIO_LUT,
+				&(data->vddci_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to retrieve VDDCI table.",
+				return result);
+	} else if (FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->vddci_control) {
+		result = fiji_get_svi2_vddci_voltage_table(hwmgr,
+				table_info->vdd_dep_on_mclk);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to retrieve SVI2 VDDCI table from dependancy table.",
+				return result);
+	}
+
+	if(FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+		result = fiji_get_svi2_vdd_voltage_table(hwmgr,
+				table_info->vddc_lookup_table);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to retrieve SVI2 VDDC table from lookup table.",
+				return result);
+	}
+
+	PP_ASSERT_WITH_CODE(
+			(data->vddc_voltage_table.count <= (SMU73_MAX_LEVELS_VDDC)),
+			"Too many voltage values for VDDC. Trimming to fit state table.",
+			fiji_trim_voltage_table_to_fit_state_table(hwmgr,
+					SMU73_MAX_LEVELS_VDDC, &(data->vddc_voltage_table)));
+
+	PP_ASSERT_WITH_CODE(
+			(data->vddci_voltage_table.count <= (SMU73_MAX_LEVELS_VDDCI)),
+			"Too many voltage values for VDDCI. Trimming to fit state table.",
+			fiji_trim_voltage_table_to_fit_state_table(hwmgr,
+					SMU73_MAX_LEVELS_VDDCI, &(data->vddci_voltage_table)));
+
+	PP_ASSERT_WITH_CODE(
+			(data->mvdd_voltage_table.count <= (SMU73_MAX_LEVELS_MVDD)),
+			"Too many voltage values for MVDD. Trimming to fit state table.",
+			fiji_trim_voltage_table_to_fit_state_table(hwmgr,
+					SMU73_MAX_LEVELS_MVDD, &(data->mvdd_voltage_table)));
+
+	return 0;
+}
+
+static int fiji_initialize_mc_reg_table(struct pp_hwmgr *hwmgr)
+{
+	/* Program additional LP registers
+	 * that are no longer programmed by VBIOS
+	 */
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RAS_TIMING_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_RAS_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_CAS_TIMING_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_CAS_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_MISC_TIMING2_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_MISC_TIMING2));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_WR_CTL_D1_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_WR_CTL_D1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RD_CTL_D0_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_RD_CTL_D0));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RD_CTL_D1_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_RD_CTL_D1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_TIMING_LP,
+			cgs_read_register(hwmgr->device, mmMC_SEQ_PMG_TIMING));
+
+	return 0;
+}
+
+/**
+* Programs static screed detection parameters
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+*/
+static int fiji_program_static_screen_threshold_parameters(
+		struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/* Set static screen threshold unit */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_STATIC_SCREEN_PARAMETER, STATIC_SCREEN_THRESHOLD_UNIT,
+			data->static_screen_threshold_unit);
+	/* Set static screen threshold */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_STATIC_SCREEN_PARAMETER, STATIC_SCREEN_THRESHOLD,
+			data->static_screen_threshold);
+
+	return 0;
+}
+
+/**
+* Setup display gap for glitch free memory clock switching.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always  0
+*/
+static int fiji_enable_display_gap(struct pp_hwmgr *hwmgr)
+{
+	uint32_t displayGap =
+			cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixCG_DISPLAY_GAP_CNTL);
+
+	displayGap = PHM_SET_FIELD(displayGap, CG_DISPLAY_GAP_CNTL,
+			DISP_GAP, DISPLAY_GAP_IGNORE);
+
+	displayGap = PHM_SET_FIELD(displayGap, CG_DISPLAY_GAP_CNTL,
+			DISP_GAP_MCHG, DISPLAY_GAP_VBLANK);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_DISPLAY_GAP_CNTL, displayGap);
+
+	return 0;
+}
+
+/**
+* Programs activity state transition voting clients
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always  0
+*/
+static int fiji_program_voting_clients(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/* Clear reset for voting clients before enabling DPM */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			SCLK_PWRMGT_CNTL, RESET_SCLK_CNT, 0);
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			SCLK_PWRMGT_CNTL, RESET_BUSY_CNT, 0);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_0, data->voting_rights_clients0);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_1, data->voting_rights_clients1);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_2, data->voting_rights_clients2);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_3, data->voting_rights_clients3);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_4, data->voting_rights_clients4);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_5, data->voting_rights_clients5);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_6, data->voting_rights_clients6);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_FREQ_TRAN_VOTING_7, data->voting_rights_clients7);
+
+	return 0;
+}
+
+/**
+* Get the location of various tables inside the FW image.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always  0
+*/
+static int fiji_process_firmware_header(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_smumgr *smu_data = (struct fiji_smumgr *)(hwmgr->smumgr->backend);
+	uint32_t tmp;
+	int result;
+	bool error = false;
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, DpmTable),
+			&tmp, data->sram_end);
+
+	if (0 == result)
+		data->dpm_table_start = tmp;
+
+	error |= (0 != result);
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, SoftRegisters),
+			&tmp, data->sram_end);
+
+	if (!result) {
+		data->soft_regs_start = tmp;
+		smu_data->soft_regs_start = tmp;
+	}
+
+	error |= (0 != result);
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, mcRegisterTable),
+			&tmp, data->sram_end);
+
+	if (!result)
+		data->mc_reg_table_start = tmp;
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, FanTable),
+			&tmp, data->sram_end);
+
+	if (!result)
+		data->fan_table_start = tmp;
+
+	error |= (0 != result);
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, mcArbDramTimingTable),
+			&tmp, data->sram_end);
+
+	if (!result)
+		data->arb_table_start = tmp;
+
+	error |= (0 != result);
+
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, Version),
+			&tmp, data->sram_end);
+
+	if (!result)
+		hwmgr->microcode_version_info.SMC = tmp;
+
+	error |= (0 != result);
+
+	return error ? -1 : 0;
+}
+
+/* Copy one arb setting to another and then switch the active set.
+ * arb_src and arb_dest is one of the MC_CG_ARB_FREQ_Fx constants.
+ */
+static int fiji_copy_and_switch_arb_sets(struct pp_hwmgr *hwmgr,
+		uint32_t arb_src, uint32_t arb_dest)
+{
+	uint32_t mc_arb_dram_timing;
+	uint32_t mc_arb_dram_timing2;
+	uint32_t burst_time;
+	uint32_t mc_cg_config;
+
+	switch (arb_src) {
+	case MC_CG_ARB_FREQ_F0:
+		mc_arb_dram_timing  = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING);
+		mc_arb_dram_timing2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2);
+		burst_time = PHM_READ_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE0);
+		break;
+	case MC_CG_ARB_FREQ_F1:
+		mc_arb_dram_timing  = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING_1);
+		mc_arb_dram_timing2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2_1);
+		burst_time = PHM_READ_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE1);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (arb_dest) {
+	case MC_CG_ARB_FREQ_F0:
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING, mc_arb_dram_timing);
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2, mc_arb_dram_timing2);
+		PHM_WRITE_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE0, burst_time);
+		break;
+	case MC_CG_ARB_FREQ_F1:
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING_1, mc_arb_dram_timing);
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2_1, mc_arb_dram_timing2);
+		PHM_WRITE_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE1, burst_time);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mc_cg_config = cgs_read_register(hwmgr->device, mmMC_CG_CONFIG);
+	mc_cg_config |= 0x0000000F;
+	cgs_write_register(hwmgr->device, mmMC_CG_CONFIG, mc_cg_config);
+	PHM_WRITE_FIELD(hwmgr->device, MC_ARB_CG, CG_ARB_REQ, arb_dest);
+
+	return 0;
+}
+
+/**
+* Initial switch from ARB F0->F1
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+* This function is to be called from the SetPowerState table.
+*/
+static int fiji_initial_switch_from_arbf0_to_f1(struct pp_hwmgr *hwmgr)
+{
+	return fiji_copy_and_switch_arb_sets(hwmgr,
+			MC_CG_ARB_FREQ_F0, MC_CG_ARB_FREQ_F1);
+}
+
+static int fiji_reset_single_dpm_table(struct pp_hwmgr *hwmgr,
+		struct fiji_single_dpm_table *dpm_table, uint32_t count)
+{
+	int i;
+	PP_ASSERT_WITH_CODE(count <= MAX_REGULAR_DPM_NUMBER,
+			"Fatal error, can not set up single DPM table entries "
+			"to exceed max number!",);
+
+	dpm_table->count = count;
+	for (i = 0; i < MAX_REGULAR_DPM_NUMBER; i++)
+		dpm_table->dpm_levels[i].enabled = false;
+
+	return 0;
+}
+
+static void fiji_setup_pcie_table_entry(
+	struct fiji_single_dpm_table *dpm_table,
+	uint32_t index, uint32_t pcie_gen,
+	uint32_t pcie_lanes)
+{
+	dpm_table->dpm_levels[index].value = pcie_gen;
+	dpm_table->dpm_levels[index].param1 = pcie_lanes;
+	dpm_table->dpm_levels[index].enabled = 1;
+}
+
+static int fiji_setup_default_pcie_table(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_pcie_table *pcie_table = table_info->pcie_table;
+	uint32_t i, max_entry;
+
+	PP_ASSERT_WITH_CODE((data->use_pcie_performance_levels ||
+			data->use_pcie_power_saving_levels), "No pcie performance levels!",
+			return -EINVAL);
+
+	if (data->use_pcie_performance_levels &&
+			!data->use_pcie_power_saving_levels) {
+		data->pcie_gen_power_saving = data->pcie_gen_performance;
+		data->pcie_lane_power_saving = data->pcie_lane_performance;
+	} else if (!data->use_pcie_performance_levels &&
+			data->use_pcie_power_saving_levels) {
+		data->pcie_gen_performance = data->pcie_gen_power_saving;
+		data->pcie_lane_performance = data->pcie_lane_power_saving;
+	}
+
+	fiji_reset_single_dpm_table(hwmgr,
+			&data->dpm_table.pcie_speed_table, SMU73_MAX_LEVELS_LINK);
+
+	if (pcie_table != NULL) {
+		/* max_entry is used to make sure we reserve one PCIE level
+		 * for boot level (fix for A+A PSPP issue).
+		 * If PCIE table from PPTable have ULV entry + 8 entries,
+		 * then ignore the last entry.*/
+		max_entry = (SMU73_MAX_LEVELS_LINK < pcie_table->count) ?
+				SMU73_MAX_LEVELS_LINK : pcie_table->count;
+		for (i = 1; i < max_entry; i++) {
+			fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, i - 1,
+					get_pcie_gen_support(data->pcie_gen_cap,
+							pcie_table->entries[i].gen_speed),
+					get_pcie_lane_support(data->pcie_lane_cap,
+							pcie_table->entries[i].lane_width));
+		}
+		data->dpm_table.pcie_speed_table.count = max_entry - 1;
+	} else {
+		/* Hardcode Pcie Table */
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 0,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Min_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 1,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Min_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 2,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Max_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 3,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Max_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 4,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Max_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+		fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 5,
+				get_pcie_gen_support(data->pcie_gen_cap,
+						PP_Max_PCIEGen),
+				get_pcie_lane_support(data->pcie_lane_cap,
+						PP_Max_PCIELane));
+
+		data->dpm_table.pcie_speed_table.count = 6;
+	}
+	/* Populate last level for boot PCIE level, but do not increment count. */
+	fiji_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table,
+			data->dpm_table.pcie_speed_table.count,
+			get_pcie_gen_support(data->pcie_gen_cap,
+					PP_Min_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap,
+					PP_Max_PCIELane));
+
+	return 0;
+}
+
+/*
+ * This function is to initalize all DPM state tables
+ * for SMU7 based on the dependency table.
+ * Dynamic state patching function will then trim these
+ * state tables to the allowed range based
+ * on the power policy or external client requests,
+ * such as UVD request, etc.
+ */
+static int fiji_setup_default_dpm_tables(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint32_t i;
+
+	struct phm_ppt_v1_clock_voltage_dependency_table *dep_sclk_table =
+			table_info->vdd_dep_on_sclk;
+	struct phm_ppt_v1_clock_voltage_dependency_table *dep_mclk_table =
+			table_info->vdd_dep_on_mclk;
+
+	PP_ASSERT_WITH_CODE(dep_sclk_table != NULL,
+			"SCLK dependency table is missing. This table is mandatory",
+			return -EINVAL);
+	PP_ASSERT_WITH_CODE(dep_sclk_table->count >= 1,
+			"SCLK dependency table has to have is missing. "
+			"This table is mandatory",
+			return -EINVAL);
+
+	PP_ASSERT_WITH_CODE(dep_mclk_table != NULL,
+			"MCLK dependency table is missing. This table is mandatory",
+			return -EINVAL);
+	PP_ASSERT_WITH_CODE(dep_mclk_table->count >= 1,
+			"MCLK dependency table has to have is missing. "
+			"This table is mandatory",
+			return -EINVAL);
+
+	/* clear the state table to reset everything to default */
+	fiji_reset_single_dpm_table(hwmgr,
+			&data->dpm_table.sclk_table, SMU73_MAX_LEVELS_GRAPHICS);
+	fiji_reset_single_dpm_table(hwmgr,
+			&data->dpm_table.mclk_table, SMU73_MAX_LEVELS_MEMORY);
+
+	/* Initialize Sclk DPM table based on allow Sclk values */
+	data->dpm_table.sclk_table.count = 0;
+	for (i = 0; i < dep_sclk_table->count; i++) {
+		if (i == 0 || data->dpm_table.sclk_table.dpm_levels
+				[data->dpm_table.sclk_table.count - 1].value !=
+						dep_sclk_table->entries[i].clk) {
+			data->dpm_table.sclk_table.dpm_levels
+			[data->dpm_table.sclk_table.count].value =
+					dep_sclk_table->entries[i].clk;
+			data->dpm_table.sclk_table.dpm_levels
+			[data->dpm_table.sclk_table.count].enabled =
+					(i == 0) ? true : false;
+			data->dpm_table.sclk_table.count++;
+		}
+	}
+
+	/* Initialize Mclk DPM table based on allow Mclk values */
+	data->dpm_table.mclk_table.count = 0;
+	for (i=0; i<dep_mclk_table->count; i++) {
+		if ( i==0 || data->dpm_table.mclk_table.dpm_levels
+				[data->dpm_table.mclk_table.count - 1].value !=
+						dep_mclk_table->entries[i].clk) {
+			data->dpm_table.mclk_table.dpm_levels
+			[data->dpm_table.mclk_table.count].value =
+					dep_mclk_table->entries[i].clk;
+			data->dpm_table.mclk_table.dpm_levels
+			[data->dpm_table.mclk_table.count].enabled =
+					(i == 0) ? true : false;
+			data->dpm_table.mclk_table.count++;
+		}
+	}
+
+	/* setup PCIE gen speed levels */
+	fiji_setup_default_pcie_table(hwmgr);
+
+	/* save a copy of the default DPM table */
+	memcpy(&(data->golden_dpm_table), &(data->dpm_table),
+			sizeof(struct fiji_dpm_table));
+
+	return 0;
+}
+
+/**
+ * @brief PhwFiji_GetVoltageOrder
+ *  Returns index of requested voltage record in lookup(table)
+ * @param lookup_table - lookup list to search in
+ * @param voltage - voltage to look for
+ * @return 0 on success
+ */
+uint8_t fiji_get_voltage_index(
+		struct phm_ppt_v1_voltage_lookup_table *lookup_table, uint16_t voltage)
+{
+	uint8_t count = (uint8_t) (lookup_table->count);
+	uint8_t i;
+
+	PP_ASSERT_WITH_CODE((NULL != lookup_table),
+			"Lookup Table empty.", return 0);
+	PP_ASSERT_WITH_CODE((0 != count),
+			"Lookup Table empty.", return 0);
+
+	for (i = 0; i < lookup_table->count; i++) {
+		/* find first voltage equal or bigger than requested */
+		if (lookup_table->entries[i].us_vdd >= voltage)
+			return i;
+	}
+	/* voltage is bigger than max voltage in the table */
+	return i - 1;
+}
+
+/**
+* Preparation of vddc and vddgfx CAC tables for SMC.
+*
+* @param    hwmgr  the address of the hardware manager
+* @param    table  the SMC DPM table structure to be populated
+* @return   always 0
+*/
+static int fiji_populate_cac_table(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	uint32_t count;
+	uint8_t index;
+	int result = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_voltage_lookup_table *lookup_table =
+			table_info->vddc_lookup_table;
+	/* tables is already swapped, so in order to use the value from it,
+	 * we need to swap it back.
+	 * We are populating vddc CAC data to BapmVddc table
+	 * in split and merged mode
+	 */
+	for( count = 0; count<lookup_table->count; count++) {
+		index = fiji_get_voltage_index(lookup_table,
+				data->vddc_voltage_table.entries[count].value);
+		table->BapmVddcVidLoSidd[count] = (uint8_t) ((6200 -
+				(lookup_table->entries[index].us_cac_low *
+						VOLTAGE_SCALE)) / 25);
+		table->BapmVddcVidHiSidd[count] = (uint8_t) ((6200 -
+				(lookup_table->entries[index].us_cac_high *
+						VOLTAGE_SCALE)) / 25);
+	}
+
+	return result;
+}
+
+/**
+* Preparation of voltage tables for SMC.
+*
+* @param    hwmgr   the address of the hardware manager
+* @param    table   the SMC DPM table structure to be populated
+* @return   always  0
+*/
+
+int fiji_populate_smc_voltage_tables(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	int result;
+
+	result = fiji_populate_cac_table(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate CAC voltage tables to SMC",
+			return -EINVAL);
+
+	return 0;
+}
+
+static int fiji_populate_ulv_level(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_Ulv *state)
+{
+	int result = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	state->CcPwrDynRm = 0;
+	state->CcPwrDynRm1 = 0;
+
+	state->VddcOffset = (uint16_t) table_info->us_ulv_voltage_offset;
+	state->VddcOffsetVid = (uint8_t)( table_info->us_ulv_voltage_offset *
+			VOLTAGE_VID_OFFSET_SCALE2 / VOLTAGE_VID_OFFSET_SCALE1 );
+
+	state->VddcPhase = (data->vddc_phase_shed_control) ? 0 : 1;
+
+	if (!result) {
+		CONVERT_FROM_HOST_TO_SMC_UL(state->CcPwrDynRm);
+		CONVERT_FROM_HOST_TO_SMC_UL(state->CcPwrDynRm1);
+		CONVERT_FROM_HOST_TO_SMC_US(state->VddcOffset);
+	}
+	return result;
+}
+
+static int fiji_populate_ulv_state(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	return fiji_populate_ulv_level(hwmgr, &table->Ulv);
+}
+
+static int32_t fiji_get_dpm_level_enable_mask_value(
+		struct fiji_single_dpm_table* dpm_table)
+{
+	int32_t i;
+	int32_t mask = 0;
+
+	for (i = dpm_table->count; i > 0; i--) {
+		mask = mask << 1;
+		if (dpm_table->dpm_levels[i - 1].enabled)
+			mask |= 0x1;
+		else
+			mask &= 0xFFFFFFFE;
+	}
+	return mask;
+}
+
+static int fiji_populate_smc_link_level(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_dpm_table *dpm_table = &data->dpm_table;
+	int i;
+
+	/* Index (dpm_table->pcie_speed_table.count)
+	 * is reserved for PCIE boot level. */
+	for (i = 0; i <= dpm_table->pcie_speed_table.count; i++) {
+		table->LinkLevel[i].PcieGenSpeed  =
+				(uint8_t)dpm_table->pcie_speed_table.dpm_levels[i].value;
+		table->LinkLevel[i].PcieLaneCount = (uint8_t)encode_pcie_lane_width(
+				dpm_table->pcie_speed_table.dpm_levels[i].param1);
+		table->LinkLevel[i].EnabledForActivity = 1;
+		table->LinkLevel[i].SPC = (uint8_t)(data->pcie_spc_cap & 0xff);
+		table->LinkLevel[i].DownThreshold = PP_HOST_TO_SMC_UL(5);
+		table->LinkLevel[i].UpThreshold = PP_HOST_TO_SMC_UL(30);
+	}
+
+	data->smc_state_table.LinkLevelCount =
+			(uint8_t)dpm_table->pcie_speed_table.count;
+	data->dpm_level_enable_mask.pcie_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&dpm_table->pcie_speed_table);
+
+	return 0;
+}
+
+/**
+* Calculates the SCLK dividers using the provided engine clock
+*
+* @param    hwmgr  the address of the hardware manager
+* @param    clock  the engine clock to use to populate the structure
+* @param    sclk   the SMC SCLK structure to be populated
+*/
+static int fiji_calculate_sclk_params(struct pp_hwmgr *hwmgr,
+		uint32_t clock, struct SMU73_Discrete_GraphicsLevel *sclk)
+{
+	const struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	uint32_t spll_func_cntl            = data->clock_registers.vCG_SPLL_FUNC_CNTL;
+	uint32_t spll_func_cntl_3          = data->clock_registers.vCG_SPLL_FUNC_CNTL_3;
+	uint32_t spll_func_cntl_4          = data->clock_registers.vCG_SPLL_FUNC_CNTL_4;
+	uint32_t cg_spll_spread_spectrum   = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM;
+	uint32_t cg_spll_spread_spectrum_2 = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2;
+	uint32_t ref_clock;
+	uint32_t ref_divider;
+	uint32_t fbdiv;
+	int result;
+
+	/* get the engine clock dividers for this clock value */
+	result = atomctrl_get_engine_pll_dividers_vi(hwmgr, clock,  &dividers);
+
+	PP_ASSERT_WITH_CODE(result == 0,
+			"Error retrieving Engine Clock dividers from VBIOS.",
+			return result);
+
+	/* To get FBDIV we need to multiply this by 16384 and divide it by Fref. */
+	ref_clock = atomctrl_get_reference_clock(hwmgr);
+	ref_divider = 1 + dividers.uc_pll_ref_div;
+
+	/* low 14 bits is fraction and high 12 bits is divider */
+	fbdiv = dividers.ul_fb_div.ul_fb_divider & 0x3FFFFFF;
+
+	/* SPLL_FUNC_CNTL setup */
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl, CG_SPLL_FUNC_CNTL,
+			SPLL_REF_DIV, dividers.uc_pll_ref_div);
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl, CG_SPLL_FUNC_CNTL,
+			SPLL_PDIV_A,  dividers.uc_pll_post_div);
+
+	/* SPLL_FUNC_CNTL_3 setup*/
+	spll_func_cntl_3 = PHM_SET_FIELD(spll_func_cntl_3, CG_SPLL_FUNC_CNTL_3,
+			SPLL_FB_DIV, fbdiv);
+
+	/* set to use fractional accumulation*/
+	spll_func_cntl_3 = PHM_SET_FIELD(spll_func_cntl_3, CG_SPLL_FUNC_CNTL_3,
+			SPLL_DITHEN, 1);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_EngineSpreadSpectrumSupport)) {
+		struct pp_atomctrl_internal_ss_info ssInfo;
+
+		uint32_t vco_freq = clock * dividers.uc_pll_post_div;
+		if (!atomctrl_get_engine_clock_spread_spectrum(hwmgr,
+				vco_freq, &ssInfo)) {
+			/*
+			 * ss_info.speed_spectrum_percentage -- in unit of 0.01%
+			 * ss_info.speed_spectrum_rate -- in unit of khz
+			 *
+			 * clks = reference_clock * 10 / (REFDIV + 1) / speed_spectrum_rate / 2
+			 */
+			uint32_t clk_s = ref_clock * 5 /
+					(ref_divider * ssInfo.speed_spectrum_rate);
+			/* clkv = 2 * D * fbdiv / NS */
+			uint32_t clk_v = 4 * ssInfo.speed_spectrum_percentage *
+					fbdiv / (clk_s * 10000);
+
+			cg_spll_spread_spectrum = PHM_SET_FIELD(cg_spll_spread_spectrum,
+					CG_SPLL_SPREAD_SPECTRUM, CLKS, clk_s);
+			cg_spll_spread_spectrum = PHM_SET_FIELD(cg_spll_spread_spectrum,
+					CG_SPLL_SPREAD_SPECTRUM, SSEN, 1);
+			cg_spll_spread_spectrum_2 = PHM_SET_FIELD(cg_spll_spread_spectrum_2,
+					CG_SPLL_SPREAD_SPECTRUM_2, CLKV, clk_v);
+		}
+	}
+
+	sclk->SclkFrequency        = clock;
+	sclk->CgSpllFuncCntl3      = spll_func_cntl_3;
+	sclk->CgSpllFuncCntl4      = spll_func_cntl_4;
+	sclk->SpllSpreadSpectrum   = cg_spll_spread_spectrum;
+	sclk->SpllSpreadSpectrum2  = cg_spll_spread_spectrum_2;
+	sclk->SclkDid              = (uint8_t)dividers.pll_post_divider;
+
+	return 0;
+}
+
+static uint16_t fiji_find_closest_vddci(struct pp_hwmgr *hwmgr, uint16_t vddci)
+{
+	uint32_t  i;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct pp_atomctrl_voltage_table *vddci_table =
+			&(data->vddci_voltage_table);
+
+	for (i = 0; i < vddci_table->count; i++) {
+		if (vddci_table->entries[i].value >= vddci)
+			return vddci_table->entries[i].value;
+	}
+
+	PP_ASSERT_WITH_CODE(false,
+			"VDDCI is larger than max VDDCI in VDDCI Voltage Table!",
+			return vddci_table->entries[i].value);
+}
+
+static int fiji_get_dependency_volt_by_clk(struct pp_hwmgr *hwmgr,
+		struct phm_ppt_v1_clock_voltage_dependency_table* dep_table,
+		uint32_t clock, SMU_VoltageLevel *voltage, uint32_t *mvdd)
+{
+	uint32_t i;
+	uint16_t vddci;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	*voltage = *mvdd = 0;
+
+	/* clock - voltage dependency table is empty table */
+	if (dep_table->count == 0)
+		return -EINVAL;
+
+	for (i = 0; i < dep_table->count; i++) {
+		/* find first sclk bigger than request */
+		if (dep_table->entries[i].clk >= clock) {
+			*voltage |= (dep_table->entries[i].vddc *
+					VOLTAGE_SCALE) << VDDC_SHIFT;
+			if (FIJI_VOLTAGE_CONTROL_NONE == data->vddci_control)
+				*voltage |= (data->vbios_boot_state.vddci_bootup_value *
+						VOLTAGE_SCALE) << VDDCI_SHIFT;
+			else if (dep_table->entries[i].vddci)
+				*voltage |= (dep_table->entries[i].vddci *
+						VOLTAGE_SCALE) << VDDCI_SHIFT;
+			else {
+				vddci = fiji_find_closest_vddci(hwmgr,
+						(dep_table->entries[i].vddc -
+								(uint16_t)data->vddc_vddci_delta));
+				*voltage |= (vddci * VOLTAGE_SCALE) <<	VDDCI_SHIFT;
+			}
+
+			if (FIJI_VOLTAGE_CONTROL_NONE == data->mvdd_control)
+				*mvdd = data->vbios_boot_state.mvdd_bootup_value *
+					VOLTAGE_SCALE;
+			else if (dep_table->entries[i].mvdd)
+				*mvdd = (uint32_t) dep_table->entries[i].mvdd *
+					VOLTAGE_SCALE;
+
+			*voltage |= 1 << PHASES_SHIFT;
+			return 0;
+		}
+	}
+
+	/* sclk is bigger than max sclk in the dependence table */
+	*voltage |= (dep_table->entries[i - 1].vddc * VOLTAGE_SCALE) << VDDC_SHIFT;
+
+	if (FIJI_VOLTAGE_CONTROL_NONE == data->vddci_control)
+		*voltage |= (data->vbios_boot_state.vddci_bootup_value *
+				VOLTAGE_SCALE) << VDDCI_SHIFT;
+	else if (dep_table->entries[i-1].vddci) {
+		vddci = fiji_find_closest_vddci(hwmgr,
+				(dep_table->entries[i].vddc -
+						(uint16_t)data->vddc_vddci_delta));
+		*voltage |= (vddci * VOLTAGE_SCALE) << VDDCI_SHIFT;
+	}
+
+	if (FIJI_VOLTAGE_CONTROL_NONE == data->mvdd_control)
+		*mvdd = data->vbios_boot_state.mvdd_bootup_value * VOLTAGE_SCALE;
+	else if (dep_table->entries[i].mvdd)
+		*mvdd = (uint32_t) dep_table->entries[i - 1].mvdd * VOLTAGE_SCALE;
+
+	return 0;
+}
+/**
+* Populates single SMC SCLK structure using the provided engine clock
+*
+* @param    hwmgr      the address of the hardware manager
+* @param    clock the engine clock to use to populate the structure
+* @param    sclk        the SMC SCLK structure to be populated
+*/
+
+static int fiji_populate_single_graphic_level(struct pp_hwmgr *hwmgr,
+		uint32_t clock, uint16_t sclk_al_threshold,
+		struct SMU73_Discrete_GraphicsLevel *level)
+{
+	int result;
+	/* PP_Clocks minClocks; */
+	uint32_t threshold, mvdd;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	result = fiji_calculate_sclk_params(hwmgr, clock, level);
+
+	/* populate graphics levels */
+	result = fiji_get_dependency_volt_by_clk(hwmgr,
+			table_info->vdd_dep_on_sclk, clock,
+			&level->MinVoltage, &mvdd);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"can not find VDDC voltage value for "
+			"VDDC engine clock dependency table",
+			return result);
+
+	level->SclkFrequency = clock;
+	level->ActivityLevel = sclk_al_threshold;
+	level->CcPwrDynRm = 0;
+	level->CcPwrDynRm1 = 0;
+	level->EnabledForActivity = 0;
+	level->EnabledForThrottle = 1;
+	level->UpHyst = 10;
+	level->DownHyst = 0;
+	level->VoltageDownHyst = 0;
+	level->PowerThrottle = 0;
+
+	threshold = clock * data->fast_watermark_threshold / 100;
+
+	/*
+	* TODO: get minimum clocks from dal configaration
+	* PECI_GetMinClockSettings(hwmgr->pPECI, &minClocks);
+	*/
+	/* data->DisplayTiming.minClockInSR = minClocks.engineClockInSR; */
+
+	/* get level->DeepSleepDivId
+	if (phm_cap_enabled(hwmgr->platformDescriptor.platformCaps, PHM_PlatformCaps_SclkDeepSleep))
+	{
+	level->DeepSleepDivId = PhwFiji_GetSleepDividerIdFromClock(hwmgr, clock, minClocks.engineClockInSR);
+	} */
+
+	/* Default to slow, highest DPM level will be
+	 * set to PPSMC_DISPLAY_WATERMARK_LOW later.
+	 */
+	level->DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+
+	CONVERT_FROM_HOST_TO_SMC_UL(level->MinVoltage);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->SclkFrequency);
+	CONVERT_FROM_HOST_TO_SMC_US(level->ActivityLevel);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->CgSpllFuncCntl3);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->CgSpllFuncCntl4);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->SpllSpreadSpectrum);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->SpllSpreadSpectrum2);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->CcPwrDynRm);
+	CONVERT_FROM_HOST_TO_SMC_UL(level->CcPwrDynRm1);
+
+	return 0;
+}
+/**
+* Populates all SMC SCLK levels' structure based on the trimmed allowed dpm engine clock states
+*
+* @param    hwmgr      the address of the hardware manager
+*/
+static int fiji_populate_all_graphic_levels(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_dpm_table *dpm_table = &data->dpm_table;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_pcie_table *pcie_table = table_info->pcie_table;
+	uint8_t pcie_entry_cnt = (uint8_t) data->dpm_table.pcie_speed_table.count;
+	int result = 0;
+	uint32_t array = data->dpm_table_start +
+			offsetof(SMU73_Discrete_DpmTable, GraphicsLevel);
+	uint32_t array_size = sizeof(struct SMU73_Discrete_GraphicsLevel) *
+			SMU73_MAX_LEVELS_GRAPHICS;
+	struct SMU73_Discrete_GraphicsLevel *levels =
+			data->smc_state_table.GraphicsLevel;
+	uint32_t i, max_entry;
+	uint8_t hightest_pcie_level_enabled = 0,
+			lowest_pcie_level_enabled = 0,
+			mid_pcie_level_enabled = 0,
+			count = 0;
+
+	for (i = 0; i < dpm_table->sclk_table.count; i++) {
+		result = fiji_populate_single_graphic_level(hwmgr,
+				dpm_table->sclk_table.dpm_levels[i].value,
+				(uint16_t)data->activity_target[i],
+				&levels[i]);
+		if (result)
+			return result;
+
+		/* Making sure only DPM level 0-1 have Deep Sleep Div ID populated. */
+		if (i > 1)
+			levels[i].DeepSleepDivId = 0;
+	}
+
+	/* Only enable level 0 for now.*/
+	levels[0].EnabledForActivity = 1;
+
+	/* set highest level watermark to high */
+	levels[dpm_table->sclk_table.count - 1].DisplayWatermark =
+			PPSMC_DISPLAY_WATERMARK_HIGH;
+
+	data->smc_state_table.GraphicsDpmLevelCount =
+			(uint8_t)dpm_table->sclk_table.count;
+	data->dpm_level_enable_mask.sclk_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&dpm_table->sclk_table);
+
+	if (pcie_table != NULL) {
+		PP_ASSERT_WITH_CODE((1 <= pcie_entry_cnt),
+				"There must be 1 or more PCIE levels defined in PPTable.",
+				return -EINVAL);
+		max_entry = pcie_entry_cnt - 1;
+		for (i = 0; i < dpm_table->sclk_table.count; i++)
+			levels[i].pcieDpmLevel =
+					(uint8_t) ((i < max_entry)? i : max_entry);
+	} else {
+		while (data->dpm_level_enable_mask.pcie_dpm_enable_mask &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+						(1 << (hightest_pcie_level_enabled + 1))) != 0 ))
+			hightest_pcie_level_enabled++;
+
+		while (data->dpm_level_enable_mask.pcie_dpm_enable_mask &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+						(1 << lowest_pcie_level_enabled)) == 0 ))
+			lowest_pcie_level_enabled++;
+
+		while ((count < hightest_pcie_level_enabled) &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+						(1 << (lowest_pcie_level_enabled + 1 + count))) == 0 ))
+			count++;
+
+		mid_pcie_level_enabled = (lowest_pcie_level_enabled + 1+ count) <
+				hightest_pcie_level_enabled?
+						(lowest_pcie_level_enabled + 1 + count) :
+						hightest_pcie_level_enabled;
+
+		/* set pcieDpmLevel to hightest_pcie_level_enabled */
+		for(i = 2; i < dpm_table->sclk_table.count; i++)
+			levels[i].pcieDpmLevel = hightest_pcie_level_enabled;
+
+		/* set pcieDpmLevel to lowest_pcie_level_enabled */
+		levels[0].pcieDpmLevel = lowest_pcie_level_enabled;
+
+		/* set pcieDpmLevel to mid_pcie_level_enabled */
+		levels[1].pcieDpmLevel = mid_pcie_level_enabled;
+	}
+	/* level count will send to smc once at init smc table and never change */
+	result = fiji_copy_bytes_to_smc(hwmgr->smumgr, array, (uint8_t *)levels,
+			(uint32_t)array_size, data->sram_end);
+
+	return result;
+}
+
+/**
+ * MCLK Frequency Ratio
+ * SEQ_CG_RESP  Bit[31:24] - 0x0
+ * Bit[27:24] \96 DDR3 Frequency ratio
+ * 0x0 <= 100MHz,       450 < 0x8 <= 500MHz
+ * 100 < 0x1 <= 150MHz,       500 < 0x9 <= 550MHz
+ * 150 < 0x2 <= 200MHz,       550 < 0xA <= 600MHz
+ * 200 < 0x3 <= 250MHz,       600 < 0xB <= 650MHz
+ * 250 < 0x4 <= 300MHz,       650 < 0xC <= 700MHz
+ * 300 < 0x5 <= 350MHz,       700 < 0xD <= 750MHz
+ * 350 < 0x6 <= 400MHz,       750 < 0xE <= 800MHz
+ * 400 < 0x7 <= 450MHz,       800 < 0xF
+ */
+static uint8_t fiji_get_mclk_frequency_ratio(uint32_t mem_clock)
+{
+	if (mem_clock <= 10000) return 0x0;
+	if (mem_clock <= 15000) return 0x1;
+	if (mem_clock <= 20000) return 0x2;
+	if (mem_clock <= 25000) return 0x3;
+	if (mem_clock <= 30000) return 0x4;
+	if (mem_clock <= 35000) return 0x5;
+	if (mem_clock <= 40000) return 0x6;
+	if (mem_clock <= 45000) return 0x7;
+	if (mem_clock <= 50000) return 0x8;
+	if (mem_clock <= 55000) return 0x9;
+	if (mem_clock <= 60000) return 0xa;
+	if (mem_clock <= 65000) return 0xb;
+	if (mem_clock <= 70000) return 0xc;
+	if (mem_clock <= 75000) return 0xd;
+	if (mem_clock <= 80000) return 0xe;
+	/* mem_clock > 800MHz */
+	return 0xf;
+}
+
+/**
+* Populates the SMC MCLK structure using the provided memory clock
+*
+* @param    hwmgr   the address of the hardware manager
+* @param    clock   the memory clock to use to populate the structure
+* @param    sclk    the SMC SCLK structure to be populated
+*/
+static int fiji_calculate_mclk_params(struct pp_hwmgr *hwmgr,
+    uint32_t clock, struct SMU73_Discrete_MemoryLevel *mclk)
+{
+	struct pp_atomctrl_memory_clock_param mem_param;
+	int result;
+
+	result = atomctrl_get_memory_pll_dividers_vi(hwmgr, clock, &mem_param);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to get Memory PLL Dividers.",);
+
+	/* Save the result data to outpupt memory level structure */
+	mclk->MclkFrequency   = clock;
+	mclk->MclkDivider     = (uint8_t)mem_param.mpll_post_divider;
+	mclk->FreqRange       = fiji_get_mclk_frequency_ratio(clock);
+
+	return result;
+}
+
+static int fiji_populate_single_memory_level(struct pp_hwmgr *hwmgr,
+		uint32_t clock, struct SMU73_Discrete_MemoryLevel *mem_level)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	int result = 0;
+
+	if (table_info->vdd_dep_on_mclk) {
+		result = fiji_get_dependency_volt_by_clk(hwmgr,
+				table_info->vdd_dep_on_mclk, clock,
+				&mem_level->MinVoltage, &mem_level->MinMvdd);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find MinVddc voltage value from memory "
+				"VDDC voltage dependency table", return result);
+	}
+
+	mem_level->EnabledForThrottle = 1;
+	mem_level->EnabledForActivity = 0;
+	mem_level->UpHyst = 0;
+	mem_level->DownHyst = 100;
+	mem_level->VoltageDownHyst = 0;
+	mem_level->ActivityLevel = (uint16_t)data->mclk_activity_target;
+	mem_level->StutterEnable = false;
+
+	mem_level->DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+
+	/* enable stutter mode if all the follow condition applied
+	 * PECI_GetNumberOfActiveDisplays(hwmgr->pPECI,
+	 * &(data->DisplayTiming.numExistingDisplays));
+	 */
+	data->display_timing.num_existing_displays = 1;
+
+	if ((data->mclk_stutter_mode_threshold) &&
+		(clock <= data->mclk_stutter_mode_threshold) &&
+		(!data->is_uvd_enabled) &&
+		(PHM_READ_FIELD(hwmgr->device, DPG_PIPE_STUTTER_CONTROL,
+				STUTTER_ENABLE) & 0x1))
+		mem_level->StutterEnable = true;
+
+	result = fiji_calculate_mclk_params(hwmgr, clock, mem_level);
+	if (!result) {
+		CONVERT_FROM_HOST_TO_SMC_UL(mem_level->MinMvdd);
+		CONVERT_FROM_HOST_TO_SMC_UL(mem_level->MclkFrequency);
+		CONVERT_FROM_HOST_TO_SMC_US(mem_level->ActivityLevel);
+		CONVERT_FROM_HOST_TO_SMC_UL(mem_level->MinVoltage);
+	}
+	return result;
+}
+
+/**
+* Populates all SMC MCLK levels' structure based on the trimmed allowed dpm memory clock states
+*
+* @param    hwmgr      the address of the hardware manager
+*/
+static int fiji_populate_all_memory_levels(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_dpm_table *dpm_table = &data->dpm_table;
+	int result;
+	/* populate MCLK dpm table to SMU7 */
+	uint32_t array = data->dpm_table_start +
+			offsetof(SMU73_Discrete_DpmTable, MemoryLevel);
+	uint32_t array_size = sizeof(SMU73_Discrete_MemoryLevel) *
+			SMU73_MAX_LEVELS_MEMORY;
+	struct SMU73_Discrete_MemoryLevel *levels =
+			data->smc_state_table.MemoryLevel;
+	uint32_t i;
+
+	for (i = 0; i < dpm_table->mclk_table.count; i++) {
+		PP_ASSERT_WITH_CODE((0 != dpm_table->mclk_table.dpm_levels[i].value),
+				"can not populate memory level as memory clock is zero",
+				return -EINVAL);
+		result = fiji_populate_single_memory_level(hwmgr,
+				dpm_table->mclk_table.dpm_levels[i].value,
+				&levels[i]);
+		if (result)
+			return result;
+	}
+
+	/* Only enable level 0 for now. */
+	levels[0].EnabledForActivity = 1;
+
+	/* in order to prevent MC activity from stutter mode to push DPM up.
+	 * the UVD change complements this by putting the MCLK in
+	 * a higher state by default such that we are not effected by
+	 * up threshold or and MCLK DPM latency.
+	 */
+	levels[0].ActivityLevel = (uint16_t)data->mclk_dpm0_activity_target;
+	CONVERT_FROM_HOST_TO_SMC_US(levels[0].ActivityLevel);
+
+	data->smc_state_table.MemoryDpmLevelCount =
+			(uint8_t)dpm_table->mclk_table.count;
+	data->dpm_level_enable_mask.mclk_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&dpm_table->mclk_table);
+	/* set highest level watermark to high */
+	levels[dpm_table->mclk_table.count - 1].DisplayWatermark =
+			PPSMC_DISPLAY_WATERMARK_HIGH;
+
+	/* level count will send to smc once at init smc table and never change */
+	result = fiji_copy_bytes_to_smc(hwmgr->smumgr, array, (uint8_t *)levels,
+			(uint32_t)array_size, data->sram_end);
+
+	return result;
+}
+
+/**
+* Populates the SMC MVDD structure using the provided memory clock.
+*
+* @param    hwmgr      the address of the hardware manager
+* @param    mclk        the MCLK value to be used in the decision if MVDD should be high or low.
+* @param    voltage     the SMC VOLTAGE structure to be populated
+*/
+int fiji_populate_mvdd_value(struct pp_hwmgr *hwmgr,
+		uint32_t mclk, SMIO_Pattern *smio_pat)
+{
+	const struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint32_t i = 0;
+
+	if (FIJI_VOLTAGE_CONTROL_NONE != data->mvdd_control) {
+		/* find mvdd value which clock is more than request */
+		for (i = 0; i < table_info->vdd_dep_on_mclk->count; i++) {
+			if (mclk <= table_info->vdd_dep_on_mclk->entries[i].clk) {
+				smio_pat->Voltage = data->mvdd_voltage_table.entries[i].value;
+				break;
+			}
+		}
+		PP_ASSERT_WITH_CODE(i < table_info->vdd_dep_on_mclk->count,
+				"MVDD Voltage is outside the supported range.",
+				return -EINVAL);
+	} else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int fiji_populate_smc_acpi_level(struct pp_hwmgr *hwmgr,
+		SMU73_Discrete_DpmTable *table)
+{
+	int result = 0;
+	const struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	SMIO_Pattern vol_level;
+	uint32_t mvdd;
+	uint16_t us_mvdd;
+	uint32_t spll_func_cntl    = data->clock_registers.vCG_SPLL_FUNC_CNTL;
+	uint32_t spll_func_cntl_2  = data->clock_registers.vCG_SPLL_FUNC_CNTL_2;
+
+	table->ACPILevel.Flags &= ~PPSMC_SWSTATE_FLAG_DC;
+
+	if (!data->sclk_dpm_key_disabled) {
+		/* Get MinVoltage and Frequency from DPM0,
+		 * already converted to SMC_UL */
+		table->ACPILevel.SclkFrequency =
+				data->dpm_table.sclk_table.dpm_levels[0].value;
+		result = fiji_get_dependency_volt_by_clk(hwmgr,
+				table_info->vdd_dep_on_sclk,
+				table->ACPILevel.SclkFrequency,
+				&table->ACPILevel.MinVoltage, &mvdd);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Cannot find ACPI VDDC voltage value "
+				"in Clock Dependency Table",);
+	} else {
+		table->ACPILevel.SclkFrequency =
+				data->vbios_boot_state.sclk_bootup_value;
+		table->ACPILevel.MinVoltage =
+				data->vbios_boot_state.vddc_bootup_value * VOLTAGE_SCALE;
+	}
+
+	/* get the engine clock dividers for this clock value */
+	result = atomctrl_get_engine_pll_dividers_vi(hwmgr,
+			table->ACPILevel.SclkFrequency,  &dividers);
+	PP_ASSERT_WITH_CODE(result == 0,
+			"Error retrieving Engine Clock dividers from VBIOS.",
+			return result);
+
+	table->ACPILevel.SclkDid = (uint8_t)dividers.pll_post_divider;
+	table->ACPILevel.DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+	table->ACPILevel.DeepSleepDivId = 0;
+
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl, CG_SPLL_FUNC_CNTL,
+			SPLL_PWRON, 0);
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl, CG_SPLL_FUNC_CNTL,
+			SPLL_RESET, 1);
+	spll_func_cntl_2 = PHM_SET_FIELD(spll_func_cntl_2, CG_SPLL_FUNC_CNTL_2,
+			SCLK_MUX_SEL, 4);
+
+	table->ACPILevel.CgSpllFuncCntl = spll_func_cntl;
+	table->ACPILevel.CgSpllFuncCntl2 = spll_func_cntl_2;
+	table->ACPILevel.CgSpllFuncCntl3 = data->clock_registers.vCG_SPLL_FUNC_CNTL_3;
+	table->ACPILevel.CgSpllFuncCntl4 = data->clock_registers.vCG_SPLL_FUNC_CNTL_4;
+	table->ACPILevel.SpllSpreadSpectrum = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM;
+	table->ACPILevel.SpllSpreadSpectrum2 = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2;
+	table->ACPILevel.CcPwrDynRm = 0;
+	table->ACPILevel.CcPwrDynRm1 = 0;
+
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.Flags);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SclkFrequency);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.MinVoltage);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl3);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl4);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SpllSpreadSpectrum);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SpllSpreadSpectrum2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CcPwrDynRm);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CcPwrDynRm1);
+
+	if (!data->mclk_dpm_key_disabled) {
+		/* Get MinVoltage and Frequency from DPM0, already converted to SMC_UL */
+		table->MemoryACPILevel.MclkFrequency =
+				data->dpm_table.mclk_table.dpm_levels[0].value;
+		result = fiji_get_dependency_volt_by_clk(hwmgr,
+				table_info->vdd_dep_on_mclk,
+				table->MemoryACPILevel.MclkFrequency,
+				&table->MemoryACPILevel.MinVoltage, &mvdd);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Cannot find ACPI VDDCI voltage value "
+				"in Clock Dependency Table",);
+	} else {
+		table->MemoryACPILevel.MclkFrequency =
+				data->vbios_boot_state.mclk_bootup_value;
+		table->MemoryACPILevel.MinVoltage =
+				data->vbios_boot_state.vddci_bootup_value * VOLTAGE_SCALE;
+	}
+
+	us_mvdd = 0;
+	if ((FIJI_VOLTAGE_CONTROL_NONE == data->mvdd_control) ||
+			(data->mclk_dpm_key_disabled))
+		us_mvdd = data->vbios_boot_state.mvdd_bootup_value;
+	else {
+		if (!fiji_populate_mvdd_value(hwmgr,
+				data->dpm_table.mclk_table.dpm_levels[0].value,
+				&vol_level))
+			us_mvdd = vol_level.Voltage;
+	}
+
+	table->MemoryACPILevel.MinMvdd =
+			PP_HOST_TO_SMC_UL(us_mvdd * VOLTAGE_SCALE);
+
+	table->MemoryACPILevel.EnabledForThrottle = 0;
+	table->MemoryACPILevel.EnabledForActivity = 0;
+	table->MemoryACPILevel.UpHyst = 0;
+	table->MemoryACPILevel.DownHyst = 100;
+	table->MemoryACPILevel.VoltageDownHyst = 0;
+	table->MemoryACPILevel.ActivityLevel =
+			PP_HOST_TO_SMC_US((uint16_t)data->mclk_activity_target);
+
+	table->MemoryACPILevel.StutterEnable = false;
+	CONVERT_FROM_HOST_TO_SMC_UL(table->MemoryACPILevel.MclkFrequency);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->MemoryACPILevel.MinVoltage);
+
+	return result;
+}
+
+static int fiji_populate_smc_vce_level(struct pp_hwmgr *hwmgr,
+		SMU73_Discrete_DpmTable *table)
+{
+	int result = -EINVAL;
+	uint8_t count;
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table =
+			table_info->mm_dep_table;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	table->VceLevelCount = (uint8_t)(mm_table->count);
+	table->VceBootLevel = 0;
+
+	for(count = 0; count < table->VceLevelCount; count++) {
+		table->VceLevel[count].Frequency = mm_table->entries[count].eclk;
+		table->VceLevel[count].MinVoltage |=
+				(mm_table->entries[count].vddc * VOLTAGE_SCALE) << VDDC_SHIFT;
+		table->VceLevel[count].MinVoltage |=
+				((mm_table->entries[count].vddc - data->vddc_vddci_delta) *
+						VOLTAGE_SCALE) << VDDCI_SHIFT;
+		table->VceLevel[count].MinVoltage |= 1 << PHASES_SHIFT;
+
+		/*retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+				table->VceLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for VCE engine clock",
+				return result);
+
+		table->VceLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->VceLevel[count].Frequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->VceLevel[count].MinVoltage);
+	}
+	return result;
+}
+
+static int fiji_populate_smc_acp_level(struct pp_hwmgr *hwmgr,
+		SMU73_Discrete_DpmTable *table)
+{
+	int result = -EINVAL;
+	uint8_t count;
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table =
+			table_info->mm_dep_table;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	table->AcpLevelCount = (uint8_t)(mm_table->count);
+	table->AcpBootLevel = 0;
+
+	for (count = 0; count < table->AcpLevelCount; count++) {
+		table->AcpLevel[count].Frequency = mm_table->entries[count].aclk;
+		table->AcpLevel[count].MinVoltage |= (mm_table->entries[count].vddc *
+				VOLTAGE_SCALE) << VDDC_SHIFT;
+		table->AcpLevel[count].MinVoltage |= ((mm_table->entries[count].vddc -
+				data->vddc_vddci_delta) * VOLTAGE_SCALE) << VDDCI_SHIFT;
+		table->AcpLevel[count].MinVoltage |= 1 << PHASES_SHIFT;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+				table->AcpLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for engine clock", return result);
+
+		table->AcpLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->AcpLevel[count].Frequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->AcpLevel[count].MinVoltage);
+	}
+	return result;
+}
+
+static int fiji_populate_smc_samu_level(struct pp_hwmgr *hwmgr,
+		SMU73_Discrete_DpmTable *table)
+{
+	int result = -EINVAL;
+	uint8_t count;
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table =
+			table_info->mm_dep_table;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	table->SamuBootLevel = 0;
+	table->SamuLevelCount = (uint8_t)(mm_table->count);
+
+	for (count = 0; count < table->SamuLevelCount; count++) {
+		/* not sure whether we need evclk or not */
+		table->SamuLevel[count].Frequency = mm_table->entries[count].samclock;
+		table->SamuLevel[count].MinVoltage |= (mm_table->entries[count].vddc *
+				VOLTAGE_SCALE) << VDDC_SHIFT;
+		table->SamuLevel[count].MinVoltage |= ((mm_table->entries[count].vddc -
+				data->vddc_vddci_delta) * VOLTAGE_SCALE) << VDDCI_SHIFT;
+		table->SamuLevel[count].MinVoltage |= 1 << PHASES_SHIFT;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+				table->SamuLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for samu clock", return result);
+
+		table->SamuLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->SamuLevel[count].Frequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->SamuLevel[count].MinVoltage);
+	}
+	return result;
+}
+
+static int fiji_populate_memory_timing_parameters(struct pp_hwmgr *hwmgr,
+		int32_t eng_clock, int32_t mem_clock,
+		struct SMU73_Discrete_MCArbDramTimingTableEntry *arb_regs)
+{
+	uint32_t dram_timing;
+	uint32_t dram_timing2;
+	uint32_t burstTime;
+	ULONG state, trrds, trrdl;
+	int result;
+
+	result = atomctrl_set_engine_dram_timings_rv770(hwmgr,
+			eng_clock, mem_clock);
+	PP_ASSERT_WITH_CODE(result == 0,
+			"Error calling VBIOS to set DRAM_TIMING.", return result);
+
+	dram_timing = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING);
+	dram_timing2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2);
+	burstTime = cgs_read_register(hwmgr->device, mmMC_ARB_BURST_TIME);
+
+	state = PHM_GET_FIELD(burstTime, MC_ARB_BURST_TIME, STATE0);
+	trrds = PHM_GET_FIELD(burstTime, MC_ARB_BURST_TIME, TRRDS0);
+	trrdl = PHM_GET_FIELD(burstTime, MC_ARB_BURST_TIME, TRRDL0);
+
+	arb_regs->McArbDramTiming  = PP_HOST_TO_SMC_UL(dram_timing);
+	arb_regs->McArbDramTiming2 = PP_HOST_TO_SMC_UL(dram_timing2);
+	arb_regs->McArbBurstTime   = (uint8_t)burstTime;
+	arb_regs->TRRDS            = (uint8_t)trrds;
+	arb_regs->TRRDL            = (uint8_t)trrdl;
+
+	return 0;
+}
+
+static int fiji_program_memory_timing_parameters(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct SMU73_Discrete_MCArbDramTimingTable arb_regs;
+	uint32_t i, j;
+	int result = 0;
+
+	for (i = 0; i < data->dpm_table.sclk_table.count; i++) {
+		for (j = 0; j < data->dpm_table.mclk_table.count; j++) {
+			result = fiji_populate_memory_timing_parameters(hwmgr,
+					data->dpm_table.sclk_table.dpm_levels[i].value,
+					data->dpm_table.mclk_table.dpm_levels[j].value,
+					&arb_regs.entries[i][j]);
+			if (result)
+				break;
+		}
+	}
+
+	if (!result)
+		result = fiji_copy_bytes_to_smc(
+				hwmgr->smumgr,
+				data->arb_table_start,
+				(uint8_t *)&arb_regs,
+				sizeof(SMU73_Discrete_MCArbDramTimingTable),
+				data->sram_end);
+	return result;
+}
+
+static int fiji_populate_smc_uvd_level(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	int result = -EINVAL;
+	uint8_t count;
+	struct pp_atomctrl_clock_dividers_vi dividers;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table =
+			table_info->mm_dep_table;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	table->UvdLevelCount = (uint8_t)(mm_table->count);
+	table->UvdBootLevel = 0;
+
+	for (count = 0; count < table->UvdLevelCount; count++) {
+		table->UvdLevel[count].VclkFrequency = mm_table->entries[count].vclk;
+		table->UvdLevel[count].DclkFrequency = mm_table->entries[count].dclk;
+		table->UvdLevel[count].MinVoltage |= (mm_table->entries[count].vddc *
+				VOLTAGE_SCALE) << VDDC_SHIFT;
+		table->UvdLevel[count].MinVoltage |= ((mm_table->entries[count].vddc -
+				data->vddc_vddci_delta) * VOLTAGE_SCALE) << VDDCI_SHIFT;
+		table->UvdLevel[count].MinVoltage |= 1 << PHASES_SHIFT;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+				table->UvdLevel[count].VclkFrequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for Vclk clock", return result);
+
+		table->UvdLevel[count].VclkDivider = (uint8_t)dividers.pll_post_divider;
+
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+				table->UvdLevel[count].DclkFrequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for Dclk clock", return result);
+
+		table->UvdLevel[count].DclkDivider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->UvdLevel[count].VclkFrequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->UvdLevel[count].DclkFrequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->UvdLevel[count].MinVoltage);
+
+	}
+	return result;
+}
+
+static int fiji_find_boot_level(struct fiji_single_dpm_table *table,
+		uint32_t value, uint32_t *boot_level)
+{
+	int result = -EINVAL;
+	uint32_t i;
+
+	for (i = 0; i < table->count; i++) {
+		if (value == table->dpm_levels[i].value) {
+			*boot_level = i;
+			result = 0;
+		}
+	}
+	return result;
+}
+
+static int fiji_populate_smc_boot_level(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	int result = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	table->GraphicsBootLevel = 0;
+	table->MemoryBootLevel = 0;
+
+	/* find boot level from dpm table */
+	result = fiji_find_boot_level(&(data->dpm_table.sclk_table),
+			data->vbios_boot_state.sclk_bootup_value,
+			(uint32_t *)&(table->GraphicsBootLevel));
+
+	result = fiji_find_boot_level(&(data->dpm_table.mclk_table),
+			data->vbios_boot_state.mclk_bootup_value,
+			(uint32_t *)&(table->MemoryBootLevel));
+
+	table->BootVddc  = data->vbios_boot_state.vddc_bootup_value *
+			VOLTAGE_SCALE;
+	table->BootVddci = data->vbios_boot_state.vddci_bootup_value *
+			VOLTAGE_SCALE;
+	table->BootMVdd  = data->vbios_boot_state.mvdd_bootup_value *
+			VOLTAGE_SCALE;
+
+	CONVERT_FROM_HOST_TO_SMC_US(table->BootVddc);
+	CONVERT_FROM_HOST_TO_SMC_US(table->BootVddci);
+	CONVERT_FROM_HOST_TO_SMC_US(table->BootMVdd);
+
+	return 0;
+}
+
+static int fiji_populate_smc_initailial_state(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint8_t count, level;
+
+	count = (uint8_t)(table_info->vdd_dep_on_sclk->count);
+	for (level = 0; level < count; level++) {
+		if(table_info->vdd_dep_on_sclk->entries[level].clk >=
+				data->vbios_boot_state.sclk_bootup_value) {
+			data->smc_state_table.GraphicsBootLevel = level;
+			break;
+		}
+	}
+
+	count = (uint8_t)(table_info->vdd_dep_on_mclk->count);
+	for (level = 0; level < count; level++) {
+		if(table_info->vdd_dep_on_mclk->entries[level].clk >=
+				data->vbios_boot_state.mclk_bootup_value) {
+			data->smc_state_table.MemoryBootLevel = level;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_populate_clock_stretcher_data_table(struct pp_hwmgr *hwmgr)
+{
+	uint32_t ro, efuse, efuse2, clock_freq, volt_without_cks,
+			volt_with_cks, value;
+	uint16_t clock_freq_u16;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint8_t type, i, j, cks_setting, stretch_amount, stretch_amount2,
+			volt_offset = 0;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_clock_voltage_dependency_table *sclk_table =
+			table_info->vdd_dep_on_sclk;
+
+	stretch_amount = (uint8_t)table_info->cac_dtp_table->usClockStretchAmount;
+
+	/* Read SMU_Eefuse to read and calculate RO and determine
+	 * if the part is SS or FF. if RO >= 1660MHz, part is FF.
+	 */
+	efuse = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixSMU_EFUSE_0 + (146 * 4));
+	efuse2 = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixSMU_EFUSE_0 + (148 * 4));
+	efuse &= 0xFF000000;
+	efuse = efuse >> 24;
+	efuse2 &= 0xF;
+
+	if (efuse2 == 1)
+		ro = (2300 - 1350) * efuse / 255 + 1350;
+	else
+		ro = (2500 - 1000) * efuse / 255 + 1000;
+
+	if (ro >= 1660)
+		type = 0;
+	else
+		type = 1;
+
+	/* Populate Stretch amount */
+	data->smc_state_table.ClockStretcherAmount = stretch_amount;
+
+	/* Populate Sclk_CKS_masterEn0_7 and Sclk_voltageOffset */
+	for (i = 0; i < sclk_table->count; i++) {
+		data->smc_state_table.Sclk_CKS_masterEn0_7 |=
+				sclk_table->entries[i].cks_enable << i;
+		volt_without_cks = (uint32_t)((14041 *
+			(sclk_table->entries[i].clk/100) / 10000 + 3571 + 75 - ro) * 1000 /
+			(4026 - (13924 * (sclk_table->entries[i].clk/100) / 10000)));
+		volt_with_cks = (uint32_t)((13946 *
+			(sclk_table->entries[i].clk/100) / 10000 + 3320 + 45 - ro) * 1000 /
+			(3664 - (11454 * (sclk_table->entries[i].clk/100) / 10000)));
+		if (volt_without_cks >= volt_with_cks)
+			volt_offset = (uint8_t)(((volt_without_cks - volt_with_cks +
+					sclk_table->entries[i].cks_voffset) * 100 / 625) + 1);
+		data->smc_state_table.Sclk_voltageOffset[i] = volt_offset;
+	}
+
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, PWR_CKS_ENABLE,
+			STRETCH_ENABLE, 0x0);
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, PWR_CKS_ENABLE,
+			masterReset, 0x1);
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, PWR_CKS_ENABLE,
+			staticEnable, 0x1);
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, PWR_CKS_ENABLE,
+			masterReset, 0x0);
+
+	/* Populate CKS Lookup Table */
+	if (stretch_amount == 1 || stretch_amount == 2 || stretch_amount == 5)
+		stretch_amount2 = 0;
+	else if (stretch_amount == 3 || stretch_amount == 4)
+		stretch_amount2 = 1;
+	else {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ClockStretcher);
+		PP_ASSERT_WITH_CODE(false,
+				"Stretch Amount in PPTable not supported\n",
+				return -EINVAL);
+	}
+
+	value = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixPWR_CKS_CNTL);
+	value &= 0xFFC2FF87;
+	data->smc_state_table.CKS_LOOKUPTable.CKS_LOOKUPTableEntry[0].minFreq =
+			fiji_clock_stretcher_lookup_table[stretch_amount2][0];
+	data->smc_state_table.CKS_LOOKUPTable.CKS_LOOKUPTableEntry[0].maxFreq =
+			fiji_clock_stretcher_lookup_table[stretch_amount2][1];
+	clock_freq_u16 = (uint16_t)(PP_SMC_TO_HOST_UL(data->smc_state_table.
+			GraphicsLevel[data->smc_state_table.GraphicsDpmLevelCount - 1].
+			SclkFrequency) / 100);
+	if (fiji_clock_stretcher_lookup_table[stretch_amount2][0] <
+			clock_freq_u16 &&
+	    fiji_clock_stretcher_lookup_table[stretch_amount2][1] >
+			clock_freq_u16) {
+		/* Program PWR_CKS_CNTL. CKS_USE_FOR_LOW_FREQ */
+		value |= (fiji_clock_stretcher_lookup_table[stretch_amount2][3]) << 16;
+		/* Program PWR_CKS_CNTL. CKS_LDO_REFSEL */
+		value |= (fiji_clock_stretcher_lookup_table[stretch_amount2][2]) << 18;
+		/* Program PWR_CKS_CNTL. CKS_STRETCH_AMOUNT */
+		value |= (fiji_clock_stretch_amount_conversion
+				[fiji_clock_stretcher_lookup_table[stretch_amount2][3]]
+				 [stretch_amount]) << 3;
+	}
+	CONVERT_FROM_HOST_TO_SMC_US(data->smc_state_table.CKS_LOOKUPTable.
+			CKS_LOOKUPTableEntry[0].minFreq);
+	CONVERT_FROM_HOST_TO_SMC_US(data->smc_state_table.CKS_LOOKUPTable.
+			CKS_LOOKUPTableEntry[0].maxFreq);
+	data->smc_state_table.CKS_LOOKUPTable.CKS_LOOKUPTableEntry[0].setting =
+			fiji_clock_stretcher_lookup_table[stretch_amount2][2] & 0x7F;
+	data->smc_state_table.CKS_LOOKUPTable.CKS_LOOKUPTableEntry[0].setting |=
+			(fiji_clock_stretcher_lookup_table[stretch_amount2][3]) << 7;
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixPWR_CKS_CNTL, value);
+
+	/* Populate DDT Lookup Table */
+	for (i = 0; i < 4; i++) {
+		/* Assign the minimum and maximum VID stored
+		 * in the last row of Clock Stretcher Voltage Table.
+		 */
+		data->smc_state_table.ClockStretcherDataTable.
+		ClockStretcherDataTableEntry[i].minVID =
+				(uint8_t) fiji_clock_stretcher_ddt_table[type][i][2];
+		data->smc_state_table.ClockStretcherDataTable.
+		ClockStretcherDataTableEntry[i].maxVID =
+				(uint8_t) fiji_clock_stretcher_ddt_table[type][i][3];
+		/* Loop through each SCLK and check the frequency
+		 * to see if it lies within the frequency for clock stretcher.
+		 */
+		for (j = 0; j < data->smc_state_table.GraphicsDpmLevelCount; j++) {
+			cks_setting = 0;
+			clock_freq = PP_SMC_TO_HOST_UL(
+					data->smc_state_table.GraphicsLevel[j].SclkFrequency);
+			/* Check the allowed frequency against the sclk level[j].
+			 *  Sclk's endianness has already been converted,
+			 *  and it's in 10Khz unit,
+			 *  as opposed to Data table, which is in Mhz unit.
+			 */
+			if (clock_freq >=
+					(fiji_clock_stretcher_ddt_table[type][i][0]) * 100) {
+				cks_setting |= 0x2;
+				if (clock_freq <
+						(fiji_clock_stretcher_ddt_table[type][i][1]) * 100)
+					cks_setting |= 0x1;
+			}
+			data->smc_state_table.ClockStretcherDataTable.
+			ClockStretcherDataTableEntry[i].setting |= cks_setting << (j * 2);
+		}
+		CONVERT_FROM_HOST_TO_SMC_US(data->smc_state_table.
+				ClockStretcherDataTable.
+				ClockStretcherDataTableEntry[i].setting);
+	}
+
+	value = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixPWR_CKS_CNTL);
+	value &= 0xFFFFFFFE;
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixPWR_CKS_CNTL, value);
+
+	return 0;
+}
+
+/**
+* Populates the SMC VRConfig field in DPM table.
+*
+* @param    hwmgr   the address of the hardware manager
+* @param    table   the SMC DPM table structure to be populated
+* @return   always 0
+*/
+static int fiji_populate_vr_config(struct pp_hwmgr *hwmgr,
+		struct SMU73_Discrete_DpmTable *table)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint16_t config;
+
+	config = VR_MERGED_WITH_VDDC;
+	table->VRConfig |= (config << VRCONF_VDDGFX_SHIFT);
+
+	/* Set Vddc Voltage Controller */
+	if(FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+		config = VR_SVI2_PLANE_1;
+		table->VRConfig |= config;
+	} else {
+		PP_ASSERT_WITH_CODE(false,
+				"VDDC should be on SVI2 control in merged mode!",);
+	}
+	/* Set Vddci Voltage Controller */
+	if(FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->vddci_control) {
+		config = VR_SVI2_PLANE_2;  /* only in merged mode */
+		table->VRConfig |= (config << VRCONF_VDDCI_SHIFT);
+	} else if (FIJI_VOLTAGE_CONTROL_BY_GPIO == data->vddci_control) {
+		config = VR_SMIO_PATTERN_1;
+		table->VRConfig |= (config << VRCONF_VDDCI_SHIFT);
+	} else {
+		config = VR_STATIC_VOLTAGE;
+		table->VRConfig |= (config << VRCONF_VDDCI_SHIFT);
+	}
+	/* Set Mvdd Voltage Controller */
+	if(FIJI_VOLTAGE_CONTROL_BY_SVID2 == data->mvdd_control) {
+		config = VR_SVI2_PLANE_2;
+		table->VRConfig |= (config << VRCONF_MVDD_SHIFT);
+	} else if(FIJI_VOLTAGE_CONTROL_BY_GPIO == data->mvdd_control) {
+		config = VR_SMIO_PATTERN_2;
+		table->VRConfig |= (config << VRCONF_MVDD_SHIFT);
+	} else {
+		config = VR_STATIC_VOLTAGE;
+		table->VRConfig |= (config << VRCONF_MVDD_SHIFT);
+	}
+
+	return 0;
+}
+
+/**
+* Initializes the SMC table and uploads it
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput  the pointer to input data (PowerState)
+* @return   always 0
+*/
+static int fiji_init_smc_table(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct SMU73_Discrete_DpmTable *table = &(data->smc_state_table);
+	const struct fiji_ulv_parm *ulv = &(data->ulv);
+	uint8_t i;
+	struct pp_atomctrl_gpio_pin_assignment gpio_pin;
+
+	result = fiji_setup_default_dpm_tables(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to setup default DPM tables!", return result);
+
+	if(FIJI_VOLTAGE_CONTROL_NONE != data->voltage_control)
+		fiji_populate_smc_voltage_tables(hwmgr, table);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_AutomaticDCTransition))
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_GPIO_DC;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_StepVddc))
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_STEPVDDC;
+
+	if (data->is_memory_gddr5)
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_GDDR5;
+
+	if (ulv->ulv_supported && table_info->us_ulv_voltage_offset) {
+		result = fiji_populate_ulv_state(hwmgr, table);
+		PP_ASSERT_WITH_CODE(0 == result,
+				"Failed to initialize ULV state!", return result);
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixCG_ULV_PARAMETER, ulv->cg_ulv_parameter);
+	}
+
+	result = fiji_populate_smc_link_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize Link Level!", return result);
+
+	result = fiji_populate_all_graphic_levels(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize Graphics Level!", return result);
+
+	result = fiji_populate_all_memory_levels(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize Memory Level!", return result);
+
+	result = fiji_populate_smc_acpi_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize ACPI Level!", return result);
+
+	result = fiji_populate_smc_vce_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize VCE Level!", return result);
+
+	result = fiji_populate_smc_acp_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize ACP Level!", return result);
+
+	result = fiji_populate_smc_samu_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize SAMU Level!", return result);
+
+	/* Since only the initial state is completely set up at this point
+	 * (the other states are just copies of the boot state) we only
+	 * need to populate the  ARB settings for the initial state.
+	 */
+	result = fiji_program_memory_timing_parameters(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to Write ARB settings for the initial state.", return result);
+
+	result = fiji_populate_smc_uvd_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize UVD Level!", return result);
+
+	result = fiji_populate_smc_boot_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize Boot Level!", return result);
+
+	result = fiji_populate_smc_initailial_state(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize Boot State!", return result);
+
+	result = fiji_populate_bapm_parameters_in_dpm_table(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to populate BAPM Parameters!", return result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ClockStretcher)) {
+		result = fiji_populate_clock_stretcher_data_table(hwmgr);
+		PP_ASSERT_WITH_CODE(0 == result,
+				"Failed to populate Clock Stretcher Data Table!",
+				return result);
+	}
+
+	table->GraphicsVoltageChangeEnable  = 1;
+	table->GraphicsThermThrottleEnable  = 1;
+	table->GraphicsInterval = 1;
+	table->VoltageInterval  = 1;
+	table->ThermalInterval  = 1;
+	table->TemperatureLimitHigh =
+			table_info->cac_dtp_table->usTargetOperatingTemp *
+			FIJI_Q88_FORMAT_CONVERSION_UNIT;
+	table->TemperatureLimitLow  =
+			(table_info->cac_dtp_table->usTargetOperatingTemp - 1) *
+			FIJI_Q88_FORMAT_CONVERSION_UNIT;
+	table->MemoryVoltageChangeEnable = 1;
+	table->MemoryInterval = 1;
+	table->VoltageResponseTime = 0;
+	table->PhaseResponseTime = 0;
+	table->MemoryThermThrottleEnable = 1;
+	table->PCIeBootLinkLevel = 0;      /* 0:Gen1 1:Gen2 2:Gen3*/
+	table->PCIeGenInterval = 1;
+
+	result = fiji_populate_vr_config(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to populate VRConfig setting!", return result);
+
+	table->ThermGpio = 17;
+	table->SclkStepSize = 0x4000;
+
+	if (atomctrl_get_pp_assign_pin(hwmgr, VDDC_VRHOT_GPIO_PINID, &gpio_pin)) {
+		table->VRHotGpio = gpio_pin.uc_gpio_pin_bit_shift;
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_RegulatorHot);
+	} else {
+		table->VRHotGpio = FIJI_UNUSED_GPIO_PIN;
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_RegulatorHot);
+	}
+
+	if (atomctrl_get_pp_assign_pin(hwmgr, PP_AC_DC_SWITCH_GPIO_PINID,
+			&gpio_pin)) {
+		table->AcDcGpio = gpio_pin.uc_gpio_pin_bit_shift;
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_AutomaticDCTransition);
+	} else {
+		table->AcDcGpio = FIJI_UNUSED_GPIO_PIN;
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_AutomaticDCTransition);
+	}
+
+	/* Thermal Output GPIO */
+	if (atomctrl_get_pp_assign_pin(hwmgr, THERMAL_INT_OUTPUT_GPIO_PINID,
+			&gpio_pin)) {
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ThermalOutGPIO);
+
+		table->ThermOutGpio = gpio_pin.uc_gpio_pin_bit_shift;
+
+		/* For porlarity read GPIOPAD_A with assigned Gpio pin
+		 * since VBIOS will program this register to set 'inactive state',
+		 * driver can then determine 'active state' from this and
+		 * program SMU with correct polarity
+		 */
+		table->ThermOutPolarity = (0 == (cgs_read_register(hwmgr->device, mmGPIOPAD_A) &
+				(1 << gpio_pin.uc_gpio_pin_bit_shift))) ? 1:0;
+		table->ThermOutMode = SMU7_THERM_OUT_MODE_THERM_ONLY;
+
+		/* if required, combine VRHot/PCC with thermal out GPIO */
+		if(phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_RegulatorHot) &&
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_CombinePCCWithThermalSignal))
+			table->ThermOutMode = SMU7_THERM_OUT_MODE_THERM_VRHOT;
+	} else {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ThermalOutGPIO);
+		table->ThermOutGpio = 17;
+		table->ThermOutPolarity = 1;
+		table->ThermOutMode = SMU7_THERM_OUT_MODE_DISABLE;
+	}
+
+	for (i = 0; i < SMU73_MAX_ENTRIES_SMIO; i++)
+		table->Smio[i] = PP_HOST_TO_SMC_UL(table->Smio[i]);
+
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SystemFlags);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->VRConfig);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SmioMask1);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SmioMask2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SclkStepSize);
+	CONVERT_FROM_HOST_TO_SMC_US(table->TemperatureLimitHigh);
+	CONVERT_FROM_HOST_TO_SMC_US(table->TemperatureLimitLow);
+	CONVERT_FROM_HOST_TO_SMC_US(table->VoltageResponseTime);
+	CONVERT_FROM_HOST_TO_SMC_US(table->PhaseResponseTime);
+
+	/* Upload all dpm data to SMC memory.(dpm level, dpm level count etc) */
+	result = fiji_copy_bytes_to_smc(hwmgr->smumgr,
+			data->dpm_table_start +
+			offsetof(SMU73_Discrete_DpmTable, SystemFlags),
+			(uint8_t *)&(table->SystemFlags),
+			sizeof(SMU73_Discrete_DpmTable) - 3 * sizeof(SMU73_PIDController),
+			data->sram_end);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to upload dpm data to SMC memory!", return result);
+
+	return 0;
+}
+
+/**
+* Initialize the ARB DRAM timing table's index field.
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always 0
+*/
+static int fiji_init_arb_table_index(struct pp_hwmgr *hwmgr)
+{
+	const struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t tmp;
+	int result;
+
+	/* This is a read-modify-write on the first byte of the ARB table.
+	 * The first byte in the SMU73_Discrete_MCArbDramTimingTable structure
+	 * is the field 'current'.
+	 * This solution is ugly, but we never write the whole table only
+	 * individual fields in it.
+	 * In reality this field should not be in that structure
+	 * but in a soft register.
+	 */
+	result = fiji_read_smc_sram_dword(hwmgr->smumgr,
+			data->arb_table_start, &tmp, data->sram_end);
+
+	if (result)
+		return result;
+
+	tmp &= 0x00FFFFFF;
+	tmp |= ((uint32_t)MC_CG_ARB_FREQ_F1) << 24;
+
+	return fiji_write_smc_sram_dword(hwmgr->smumgr,
+			data->arb_table_start,  tmp, data->sram_end);
+}
+
+static int fiji_enable_vrhot_gpio_interrupt(struct pp_hwmgr *hwmgr)
+{
+	if(phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_RegulatorHot))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_EnableVRHotGPIOInterrupt);
+
+	return 0;
+}
+
+static int fiji_enable_sclk_control(struct pp_hwmgr *hwmgr)
+{
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, SCLK_PWRMGT_CNTL,
+			SCLK_PWRMGT_OFF, 0);
+	return 0;
+}
+
+static int fiji_enable_ulv(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_ulv_parm *ulv = &(data->ulv);
+
+	if (ulv->ulv_supported)
+		return smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_EnableULV);
+
+	return 0;
+}
+
+static int fiji_enable_deep_sleep_master_switch(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SclkDeepSleep)) {
+		if (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_MASTER_DeepSleep_ON))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to enable Master Deep Sleep switch failed!",
+					return -1);
+	} else {
+		if (smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_MASTER_DeepSleep_OFF)) {
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to disable Master Deep Sleep switch failed!",
+					return -1);
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_enable_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t   val, val0, val2;
+	uint32_t   i, cpl_cntl, cpl_threshold, mc_threshold;
+
+	/* enable SCLK dpm */
+	if(!data->sclk_dpm_key_disabled)
+		PP_ASSERT_WITH_CODE(
+		(0 == smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_DPM_Enable)),
+		"Failed to enable SCLK DPM during DPM Start Function!",
+		return -1);
+
+	/* enable MCLK dpm */
+	if(0 == data->mclk_dpm_key_disabled) {
+		cpl_threshold = 0;
+		mc_threshold = 0;
+
+		/* Read per MCD tile (0 - 7) */
+		for (i = 0; i < 8; i++) {
+			PHM_WRITE_FIELD(hwmgr->device, MC_CONFIG_MCD, MC_RD_ENABLE, i);
+			val = cgs_read_register(hwmgr->device, mmMC_SEQ_RESERVE_0_S) & 0xf0000000;
+			if (0xf0000000 != val) {
+				/* count number of MCQ that has channel(s) enabled */
+				cpl_threshold++;
+				/* only harvest 3 or full 4 supported */
+				mc_threshold = val ? 3 : 4;
+			}
+		}
+		PP_ASSERT_WITH_CODE(0 != cpl_threshold,
+				"Number of MCQ is zero!", return -EINVAL;);
+
+		mc_threshold = ((mc_threshold & LCAC_MC0_CNTL__MC0_THRESHOLD_MASK) <<
+				LCAC_MC0_CNTL__MC0_THRESHOLD__SHIFT) |
+						LCAC_MC0_CNTL__MC0_ENABLE_MASK;
+		cpl_cntl = ((cpl_threshold & LCAC_CPL_CNTL__CPL_THRESHOLD_MASK) <<
+				LCAC_CPL_CNTL__CPL_THRESHOLD__SHIFT) |
+						LCAC_CPL_CNTL__CPL_ENABLE_MASK;
+		cpl_cntl = (cpl_cntl | (8 << LCAC_CPL_CNTL__CPL_BLOCK_ID__SHIFT));
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_MC0_CNTL, mc_threshold);
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_MC1_CNTL, mc_threshold);
+		if (8 == cpl_threshold) {
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC2_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC3_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC4_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC5_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC6_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC7_CNTL, mc_threshold);
+		}
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_CPL_CNTL, cpl_cntl);
+
+		udelay(5);
+
+		mc_threshold = mc_threshold |
+				(1 << LCAC_MC0_CNTL__MC0_SIGNAL_ID__SHIFT);
+		cpl_cntl = cpl_cntl | (1 << LCAC_CPL_CNTL__CPL_SIGNAL_ID__SHIFT);
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_MC0_CNTL, mc_threshold);
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_MC1_CNTL, mc_threshold);
+		if (8 == cpl_threshold) {
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC2_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC3_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC4_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC5_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC6_CNTL, mc_threshold);
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+					ixLCAC_MC7_CNTL, mc_threshold);
+		}
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+				ixLCAC_CPL_CNTL, cpl_cntl);
+
+		/* Program CAC_EN per MCD (0-7) Tile */
+		val0 = val = cgs_read_register(hwmgr->device, mmMC_CONFIG_MCD);
+		val &= ~(MC_CONFIG_MCD__MCD0_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD1_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD2_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD3_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD4_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD5_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD6_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MCD7_WR_ENABLE_MASK |
+				MC_CONFIG_MCD__MC_RD_ENABLE_MASK);
+
+		for (i = 0; i < 8; i++) {
+			/* Enable MCD i Tile read & write */
+			val2  = (val | (i << MC_CONFIG_MCD__MC_RD_ENABLE__SHIFT) |
+					(1 << i));
+			cgs_write_register(hwmgr->device, mmMC_CONFIG_MCD, val2);
+			/* Enbale CAC_ON MCD i Tile */
+			val2 = cgs_read_register(hwmgr->device, mmMC_SEQ_CNTL);
+			val2 |= MC_SEQ_CNTL__CAC_EN_MASK;
+			cgs_write_register(hwmgr->device, mmMC_SEQ_CNTL, val2);
+		}
+		/* Set MC_CONFIG_MCD back to its default setting val0 */
+		cgs_write_register(hwmgr->device, mmMC_CONFIG_MCD, val0);
+
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						PPSMC_MSG_MCLKDPM_Enable)),
+				"Failed to enable MCLK DPM during DPM Start Function!",
+				return -1);
+	}
+	return 0;
+}
+
+static int fiji_start_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/*enable general power management */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT,
+			GLOBAL_PWRMGT_EN, 1);
+	/* enable sclk deep sleep */
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, SCLK_PWRMGT_CNTL,
+			DYNAMIC_PM_EN, 1);
+	/* prepare for PCIE DPM */
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			data->soft_regs_start + offsetof(SMU73_SoftRegisters,
+					VoltageChangeTimeout), 0x1000);
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__PCIE,
+			SWRST_COMMAND_1, RESETLC, 0x0);
+
+	PP_ASSERT_WITH_CODE(
+			(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					PPSMC_MSG_Voltage_Cntl_Enable)),
+			"Failed to enable voltage DPM during DPM Start Function!",
+			return -1);
+
+	if (fiji_enable_sclk_mclk_dpm(hwmgr)) {
+		printk(KERN_ERR "Failed to enable Sclk DPM and Mclk DPM!");
+		return -1;
+	}
+
+	/* enable PCIE dpm */
+	if(!data->pcie_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						PPSMC_MSG_PCIeDPM_Enable)),
+				"Failed to enable pcie DPM during DPM Start Function!",
+				return -1);
+	}
+
+	return 0;
+}
+
+static void fiji_set_dpm_event_sources(struct pp_hwmgr *hwmgr,
+		uint32_t sources)
+{
+	bool protection;
+	enum DPM_EVENT_SRC src;
+
+	switch (sources) {
+	default:
+		printk(KERN_ERR "Unknown throttling event sources.");
+		/* fall through */
+	case 0:
+		protection = false;
+		/* src is unused */
+		break;
+	case (1 << PHM_AutoThrottleSource_Thermal):
+		protection = true;
+		src = DPM_EVENT_SRC_DIGITAL;
+		break;
+	case (1 << PHM_AutoThrottleSource_External):
+		protection = true;
+		src = DPM_EVENT_SRC_EXTERNAL;
+		break;
+	case (1 << PHM_AutoThrottleSource_External) |
+			(1 << PHM_AutoThrottleSource_Thermal):
+		protection = true;
+		src = DPM_EVENT_SRC_DIGITAL_OR_EXTERNAL;
+		break;
+	}
+	/* Order matters - don't enable thermal protection for the wrong source. */
+	if (protection) {
+		PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_CTRL,
+				DPM_EVENT_SRC, src);
+		PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT,
+				THERMAL_PROTECTION_DIS,
+				phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+						PHM_PlatformCaps_ThermalController));
+	} else
+		PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT,
+				THERMAL_PROTECTION_DIS, 1);
+}
+
+static int fiji_enable_auto_throttle_source(struct pp_hwmgr *hwmgr,
+		PHM_AutoThrottleSource source)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (!(data->active_auto_throttle_sources & (1 << source))) {
+		data->active_auto_throttle_sources |= 1 << source;
+		fiji_set_dpm_event_sources(hwmgr, data->active_auto_throttle_sources);
+	}
+	return 0;
+}
+
+static int fiji_enable_thermal_auto_throttle(struct pp_hwmgr *hwmgr)
+{
+	return fiji_enable_auto_throttle_source(hwmgr, PHM_AutoThrottleSource_Thermal);
+}
+
+static int fiji_enable_dpm_tasks(struct pp_hwmgr *hwmgr)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = (!fiji_is_dpm_running(hwmgr))? 0 : -1;
+	PP_ASSERT_WITH_CODE(result == 0,
+			"DPM is already running right now, no need to enable DPM!",
+			return 0);
+
+	if (fiji_voltage_control(hwmgr)) {
+		tmp_result = fiji_enable_voltage_control(hwmgr);
+		PP_ASSERT_WITH_CODE(tmp_result == 0,
+				"Failed to enable voltage control!",
+				result = tmp_result);
+	}
+
+	if (fiji_voltage_control(hwmgr)) {
+		tmp_result = fiji_construct_voltage_tables(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == tmp_result),
+				"Failed to contruct voltage tables!",
+				result = tmp_result);
+	}
+
+	tmp_result = fiji_initialize_mc_reg_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to initialize MC reg table!", result = tmp_result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EngineSpreadSpectrumSupport))
+		PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				GENERAL_PWRMGT, DYN_SPREAD_SPECTRUM_EN, 1);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ThermalController))
+		PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				GENERAL_PWRMGT, THERMAL_PROTECTION_DIS, 0);
+
+	tmp_result = fiji_program_static_screen_threshold_parameters(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to program static screen threshold parameters!",
+			result = tmp_result);
+
+	tmp_result = fiji_enable_display_gap(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable display gap!", result = tmp_result);
+
+	tmp_result = fiji_program_voting_clients(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to program voting clients!", result = tmp_result);
+
+	tmp_result = fiji_process_firmware_header(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to process firmware header!", result = tmp_result);
+
+	tmp_result = fiji_initial_switch_from_arbf0_to_f1(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to initialize switch from ArbF0 to F1!",
+			result = tmp_result);
+
+	tmp_result = fiji_init_smc_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to initialize SMC table!", result = tmp_result);
+
+	tmp_result = fiji_init_arb_table_index(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to initialize ARB table index!", result = tmp_result);
+
+	tmp_result = fiji_populate_pm_fuses(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to populate PM fuses!", result = tmp_result);
+
+	tmp_result = fiji_enable_vrhot_gpio_interrupt(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable VR hot GPIO interrupt!", result = tmp_result);
+
+	tmp_result = tonga_notify_smc_display_change(hwmgr, false);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to notify no display!", result = tmp_result);
+
+	tmp_result = fiji_enable_sclk_control(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable SCLK control!", result = tmp_result);
+
+	tmp_result = fiji_enable_ulv(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable ULV!", result = tmp_result);
+
+	tmp_result = fiji_enable_deep_sleep_master_switch(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable deep sleep master switch!", result = tmp_result);
+
+	tmp_result = fiji_start_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to start DPM!", result = tmp_result);
+
+	tmp_result = fiji_enable_smc_cac(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable SMC CAC!", result = tmp_result);
+
+	tmp_result = fiji_enable_power_containment(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable power containment!", result = tmp_result);
+
+	tmp_result = fiji_power_control_set_level(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to power control set level!", result = tmp_result);
+
+	tmp_result = fiji_enable_thermal_auto_throttle(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable thermal auto throttle!", result = tmp_result);
+
+	return result;
+}
+
+static int fiji_force_dpm_highest(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t level, tmp;
+
+	if (!data->sclk_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.sclk_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++;
+			if (level)
+				smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_SCLKDPM_SetEnabledMask,
+						(1 << level));
+		}
+	}
+
+	if (!data->mclk_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.mclk_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++;
+			if (level)
+				smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_MCLKDPM_SetEnabledMask,
+						(1 << level));
+		}
+	}
+
+	if (!data->pcie_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.pcie_dpm_enable_mask) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.pcie_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++;
+			if (level)
+				smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_PCIeDPM_ForceLevel,
+						(1 << level));
+		}
+	}
+	return 0;
+}
+
+static void fiji_apply_dal_min_voltage_request(struct pp_hwmgr *hwmgr)
+{
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)hwmgr->pptable;
+	struct phm_clock_voltage_dependency_table *table =
+				table_info->vddc_dep_on_dal_pwrl;
+	struct phm_ppt_v1_clock_voltage_dependency_table *vddc_table;
+	enum PP_DAL_POWERLEVEL dal_power_level = hwmgr->dal_power_level;
+	uint32_t req_vddc = 0, req_volt, i;
+
+	if (!table && !(dal_power_level >= PP_DAL_POWERLEVEL_ULTRALOW &&
+			dal_power_level <= PP_DAL_POWERLEVEL_PERFORMANCE))
+		return;
+
+	for (i= 0; i < table->count; i++) {
+		if (dal_power_level == table->entries[i].clk) {
+			req_vddc = table->entries[i].v;
+			break;
+		}
+	}
+
+	vddc_table = table_info->vdd_dep_on_sclk;
+	for (i= 0; i < vddc_table->count; i++) {
+		if (req_vddc <= vddc_table->entries[i].vddc) {
+			req_volt = (((uint32_t)vddc_table->entries[i].vddc) * VOLTAGE_SCALE)
+					<< VDDC_SHIFT;
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_VddC_Request, req_volt);
+			return;
+		}
+	}
+	printk(KERN_ERR "DAL requested level can not"
+			" found a available voltage in VDDC DPM Table \n");
+}
+
+static int fiji_upload_dpmlevel_enable_mask(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	fiji_apply_dal_min_voltage_request(hwmgr);
+
+	if (!data->sclk_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.sclk_dpm_enable_mask)
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_SCLKDPM_SetEnabledMask,
+					data->dpm_level_enable_mask.sclk_dpm_enable_mask);
+	}
+	return 0;
+}
+
+static int fiji_unforce_dpm_levels(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (!fiji_is_dpm_running(hwmgr))
+		return -EINVAL;
+
+	if (!data->pcie_dpm_key_disabled) {
+		smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_PCIeDPM_UnForceLevel);
+	}
+
+	return fiji_upload_dpmlevel_enable_mask(hwmgr);
+}
+
+static uint32_t fiji_get_lowest_enabled_level(
+		struct pp_hwmgr *hwmgr, uint32_t mask)
+{
+	uint32_t level = 0;
+
+	while(0 == (mask & (1 << level)))
+		level++;
+
+	return level;
+}
+
+static int fiji_force_dpm_lowest(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data =
+			(struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t level;
+
+	if (!data->sclk_dpm_key_disabled)
+		if (data->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+			level = fiji_get_lowest_enabled_level(hwmgr,
+							      data->dpm_level_enable_mask.sclk_dpm_enable_mask);
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+							    PPSMC_MSG_SCLKDPM_SetEnabledMask,
+							    (1 << level));
+
+	}
+
+	if (!data->mclk_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask) {
+			level = fiji_get_lowest_enabled_level(hwmgr,
+							      data->dpm_level_enable_mask.mclk_dpm_enable_mask);
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+							    PPSMC_MSG_MCLKDPM_SetEnabledMask,
+							    (1 << level));
+		}
+	}
+
+	if (!data->pcie_dpm_key_disabled) {
+		if (data->dpm_level_enable_mask.pcie_dpm_enable_mask) {
+			level = fiji_get_lowest_enabled_level(hwmgr,
+							      data->dpm_level_enable_mask.pcie_dpm_enable_mask);
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+							    PPSMC_MSG_PCIeDPM_ForceLevel,
+							    (1 << level));
+		}
+	}
+
+	return 0;
+
+}
+static int fiji_dpm_force_dpm_level(struct pp_hwmgr *hwmgr,
+				enum amd_dpm_forced_level level)
+{
+	int ret = 0;
+
+	switch (level) {
+	case AMD_DPM_FORCED_LEVEL_HIGH:
+		ret = fiji_force_dpm_highest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_LOW:
+		ret = fiji_force_dpm_lowest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_AUTO:
+		ret = fiji_unforce_dpm_levels(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	default:
+		break;
+	}
+
+	hwmgr->dpm_level = level;
+
+	return ret;
+}
+
+static int fiji_get_power_state_size(struct pp_hwmgr *hwmgr)
+{
+	return sizeof(struct fiji_power_state);
+}
+
+static int fiji_get_pp_table_entry_callback_func(struct pp_hwmgr *hwmgr,
+		void *state, struct pp_power_state *power_state,
+		void *pp_table, uint32_t classification_flag)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_power_state  *fiji_power_state =
+			(struct fiji_power_state *)(&(power_state->hardware));
+	struct fiji_performance_level *performance_level;
+	ATOM_Tonga_State *state_entry = (ATOM_Tonga_State *)state;
+	ATOM_Tonga_POWERPLAYTABLE *powerplay_table =
+			(ATOM_Tonga_POWERPLAYTABLE *)pp_table;
+	ATOM_Tonga_SCLK_Dependency_Table *sclk_dep_table =
+			(ATOM_Tonga_SCLK_Dependency_Table *)
+			(((unsigned long)powerplay_table) +
+				le16_to_cpu(powerplay_table->usSclkDependencyTableOffset));
+	ATOM_Tonga_MCLK_Dependency_Table *mclk_dep_table =
+			(ATOM_Tonga_MCLK_Dependency_Table *)
+			(((unsigned long)powerplay_table) +
+				le16_to_cpu(powerplay_table->usMclkDependencyTableOffset));
+
+	/* The following fields are not initialized here: id orderedList allStatesList */
+	power_state->classification.ui_label =
+			(le16_to_cpu(state_entry->usClassification) &
+			ATOM_PPLIB_CLASSIFICATION_UI_MASK) >>
+			ATOM_PPLIB_CLASSIFICATION_UI_SHIFT;
+	power_state->classification.flags = classification_flag;
+	/* NOTE: There is a classification2 flag in BIOS that is not being used right now */
+
+	power_state->classification.temporary_state = false;
+	power_state->classification.to_be_deleted = false;
+
+	power_state->validation.disallowOnDC =
+			(0 != (le32_to_cpu(state_entry->ulCapsAndSettings) &
+					ATOM_Tonga_DISALLOW_ON_DC));
+
+	power_state->pcie.lanes = 0;
+
+	power_state->display.disableFrameModulation = false;
+	power_state->display.limitRefreshrate = false;
+	power_state->display.enableVariBright =
+			(0 != (le32_to_cpu(state_entry->ulCapsAndSettings) &
+					ATOM_Tonga_ENABLE_VARIBRIGHT));
+
+	power_state->validation.supportedPowerLevels = 0;
+	power_state->uvd_clocks.VCLK = 0;
+	power_state->uvd_clocks.DCLK = 0;
+	power_state->temperatures.min = 0;
+	power_state->temperatures.max = 0;
+
+	performance_level = &(fiji_power_state->performance_levels
+			[fiji_power_state->performance_level_count++]);
+
+	PP_ASSERT_WITH_CODE(
+			(fiji_power_state->performance_level_count < SMU73_MAX_LEVELS_GRAPHICS),
+			"Performance levels exceeds SMC limit!",
+			return -1);
+
+	PP_ASSERT_WITH_CODE(
+			(fiji_power_state->performance_level_count <=
+					hwmgr->platform_descriptor.hardwareActivityPerformanceLevels),
+			"Performance levels exceeds Driver limit!",
+			return -1);
+
+	/* Performance levels are arranged from low to high. */
+	performance_level->memory_clock = mclk_dep_table->entries
+			[state_entry->ucMemoryClockIndexLow].ulMclk;
+	performance_level->engine_clock = sclk_dep_table->entries
+			[state_entry->ucEngineClockIndexLow].ulSclk;
+	performance_level->pcie_gen = get_pcie_gen_support(data->pcie_gen_cap,
+			state_entry->ucPCIEGenLow);
+	performance_level->pcie_lane = get_pcie_lane_support(data->pcie_lane_cap,
+			state_entry->ucPCIELaneHigh);
+
+	performance_level = &(fiji_power_state->performance_levels
+			[fiji_power_state->performance_level_count++]);
+	performance_level->memory_clock = mclk_dep_table->entries
+			[state_entry->ucMemoryClockIndexHigh].ulMclk;
+	performance_level->engine_clock = sclk_dep_table->entries
+			[state_entry->ucEngineClockIndexHigh].ulSclk;
+	performance_level->pcie_gen = get_pcie_gen_support(data->pcie_gen_cap,
+			state_entry->ucPCIEGenHigh);
+	performance_level->pcie_lane = get_pcie_lane_support(data->pcie_lane_cap,
+			state_entry->ucPCIELaneHigh);
+
+	return 0;
+}
+
+static int fiji_get_pp_table_entry(struct pp_hwmgr *hwmgr,
+		unsigned long entry_index, struct pp_power_state *state)
+{
+	int result;
+	struct fiji_power_state *ps;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_clock_voltage_dependency_table *dep_mclk_table =
+			table_info->vdd_dep_on_mclk;
+
+	state->hardware.magic = PHM_VIslands_Magic;
+
+	ps = (struct fiji_power_state *)(&state->hardware);
+
+	result = tonga_get_powerplay_table_entry(hwmgr, entry_index, state,
+			fiji_get_pp_table_entry_callback_func);
+
+	/* This is the earliest time we have all the dependency table and the VBIOS boot state
+	 * as PP_Tables_GetPowerPlayTableEntry retrieves the VBIOS boot state
+	 * if there is only one VDDCI/MCLK level, check if it's the same as VBIOS boot state
+	 */
+	if (dep_mclk_table != NULL && dep_mclk_table->count == 1) {
+		if (dep_mclk_table->entries[0].clk !=
+				data->vbios_boot_state.mclk_bootup_value)
+			printk(KERN_ERR "Single MCLK entry VDDCI/MCLK dependency table "
+					"does not match VBIOS boot MCLK level");
+		if (dep_mclk_table->entries[0].vddci !=
+				data->vbios_boot_state.vddci_bootup_value)
+			printk(KERN_ERR "Single VDDCI entry VDDCI/MCLK dependency table "
+					"does not match VBIOS boot VDDCI level");
+	}
+
+	/* set DC compatible flag if this state supports DC */
+	if (!state->validation.disallowOnDC)
+		ps->dc_compatible = true;
+
+	if (state->classification.flags & PP_StateClassificationFlag_ACPI)
+		data->acpi_pcie_gen = ps->performance_levels[0].pcie_gen;
+
+	ps->uvd_clks.vclk = state->uvd_clocks.VCLK;
+	ps->uvd_clks.dclk = state->uvd_clocks.DCLK;
+
+	if (!result) {
+		uint32_t i;
+
+		switch (state->classification.ui_label) {
+		case PP_StateUILabel_Performance:
+			data->use_pcie_performance_levels = true;
+
+			for (i = 0; i < ps->performance_level_count; i++) {
+				if (data->pcie_gen_performance.max <
+						ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_performance.max =
+							ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_gen_performance.min >
+						ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_performance.min =
+							ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_lane_performance.max <
+						ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_performance.max =
+							ps->performance_levels[i].pcie_lane;
+
+				if (data->pcie_lane_performance.min >
+						ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_performance.min =
+							ps->performance_levels[i].pcie_lane;
+			}
+			break;
+		case PP_StateUILabel_Battery:
+			data->use_pcie_power_saving_levels = true;
+
+			for (i = 0; i < ps->performance_level_count; i++) {
+				if (data->pcie_gen_power_saving.max <
+						ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_power_saving.max =
+							ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_gen_power_saving.min >
+						ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_power_saving.min =
+							ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_lane_power_saving.max <
+						ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_power_saving.max =
+							ps->performance_levels[i].pcie_lane;
+
+				if (data->pcie_lane_power_saving.min >
+						ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_power_saving.min =
+							ps->performance_levels[i].pcie_lane;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+static int fiji_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
+				struct pp_power_state  *request_ps,
+			const struct pp_power_state *current_ps)
+{
+	struct fiji_power_state *fiji_ps =
+				cast_phw_fiji_power_state(&request_ps->hardware);
+	uint32_t sclk;
+	uint32_t mclk;
+	struct PP_Clocks minimum_clocks = {0};
+	bool disable_mclk_switching;
+	bool disable_mclk_switching_for_frame_lock;
+	struct cgs_display_info info = {0};
+	const struct phm_clock_and_voltage_limits *max_limits;
+	uint32_t i;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	int32_t count;
+	int32_t stable_pstate_sclk = 0, stable_pstate_mclk = 0;
+
+	data->battery_state = (PP_StateUILabel_Battery ==
+			request_ps->classification.ui_label);
+
+	PP_ASSERT_WITH_CODE(fiji_ps->performance_level_count == 2,
+				 "VI should always have 2 performance levels",);
+
+	max_limits = (PP_PowerSource_AC == hwmgr->power_source) ?
+			&(hwmgr->dyn_state.max_clock_voltage_on_ac) :
+			&(hwmgr->dyn_state.max_clock_voltage_on_dc);
+
+	/* Cap clock DPM tables at DC MAX if it is in DC. */
+	if (PP_PowerSource_DC == hwmgr->power_source) {
+		for (i = 0; i < fiji_ps->performance_level_count; i++) {
+			if (fiji_ps->performance_levels[i].memory_clock > max_limits->mclk)
+				fiji_ps->performance_levels[i].memory_clock = max_limits->mclk;
+			if (fiji_ps->performance_levels[i].engine_clock > max_limits->sclk)
+				fiji_ps->performance_levels[i].engine_clock = max_limits->sclk;
+		}
+	}
+
+	fiji_ps->vce_clks.evclk = hwmgr->vce_arbiter.evclk;
+	fiji_ps->vce_clks.ecclk = hwmgr->vce_arbiter.ecclk;
+
+	fiji_ps->acp_clk = hwmgr->acp_arbiter.acpclk;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	/*TO DO result = PHM_CheckVBlankTime(hwmgr, &vblankTooShort);*/
+
+	/* TO DO GetMinClockSettings(hwmgr->pPECI, &minimum_clocks); */
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_StablePState)) {
+		max_limits = &(hwmgr->dyn_state.max_clock_voltage_on_ac);
+		stable_pstate_sclk = (max_limits->sclk * 75) / 100;
+
+		for (count = table_info->vdd_dep_on_sclk->count - 1;
+				count >= 0; count--) {
+			if (stable_pstate_sclk >=
+					table_info->vdd_dep_on_sclk->entries[count].clk) {
+				stable_pstate_sclk =
+						table_info->vdd_dep_on_sclk->entries[count].clk;
+				break;
+			}
+		}
+
+		if (count < 0)
+			stable_pstate_sclk = table_info->vdd_dep_on_sclk->entries[0].clk;
+
+		stable_pstate_mclk = max_limits->mclk;
+
+		minimum_clocks.engineClock = stable_pstate_sclk;
+		minimum_clocks.memoryClock = stable_pstate_mclk;
+	}
+
+	if (minimum_clocks.engineClock < hwmgr->gfx_arbiter.sclk)
+		minimum_clocks.engineClock = hwmgr->gfx_arbiter.sclk;
+
+	if (minimum_clocks.memoryClock < hwmgr->gfx_arbiter.mclk)
+		minimum_clocks.memoryClock = hwmgr->gfx_arbiter.mclk;
+
+	fiji_ps->sclk_threshold = hwmgr->gfx_arbiter.sclk_threshold;
+
+	if (0 != hwmgr->gfx_arbiter.sclk_over_drive) {
+		PP_ASSERT_WITH_CODE((hwmgr->gfx_arbiter.sclk_over_drive <=
+				hwmgr->platform_descriptor.overdriveLimit.engineClock),
+				"Overdrive sclk exceeds limit",
+				hwmgr->gfx_arbiter.sclk_over_drive =
+						hwmgr->platform_descriptor.overdriveLimit.engineClock);
+
+		if (hwmgr->gfx_arbiter.sclk_over_drive >= hwmgr->gfx_arbiter.sclk)
+			fiji_ps->performance_levels[1].engine_clock =
+					hwmgr->gfx_arbiter.sclk_over_drive;
+	}
+
+	if (0 != hwmgr->gfx_arbiter.mclk_over_drive) {
+		PP_ASSERT_WITH_CODE((hwmgr->gfx_arbiter.mclk_over_drive <=
+				hwmgr->platform_descriptor.overdriveLimit.memoryClock),
+				"Overdrive mclk exceeds limit",
+				hwmgr->gfx_arbiter.mclk_over_drive =
+						hwmgr->platform_descriptor.overdriveLimit.memoryClock);
+
+		if (hwmgr->gfx_arbiter.mclk_over_drive >= hwmgr->gfx_arbiter.mclk)
+			fiji_ps->performance_levels[1].memory_clock =
+					hwmgr->gfx_arbiter.mclk_over_drive;
+	}
+
+	disable_mclk_switching_for_frame_lock = phm_cap_enabled(
+				    hwmgr->platform_descriptor.platformCaps,
+				    PHM_PlatformCaps_DisableMclkSwitchingForFrameLock);
+
+	disable_mclk_switching = (1 < info.display_count) ||
+				    disable_mclk_switching_for_frame_lock;
+
+	sclk = fiji_ps->performance_levels[0].engine_clock;
+	mclk = fiji_ps->performance_levels[0].memory_clock;
+
+	if (disable_mclk_switching)
+		mclk = fiji_ps->performance_levels
+		[fiji_ps->performance_level_count - 1].memory_clock;
+
+	if (sclk < minimum_clocks.engineClock)
+		sclk = (minimum_clocks.engineClock > max_limits->sclk) ?
+				max_limits->sclk : minimum_clocks.engineClock;
+
+	if (mclk < minimum_clocks.memoryClock)
+		mclk = (minimum_clocks.memoryClock > max_limits->mclk) ?
+				max_limits->mclk : minimum_clocks.memoryClock;
+
+	fiji_ps->performance_levels[0].engine_clock = sclk;
+	fiji_ps->performance_levels[0].memory_clock = mclk;
+
+	fiji_ps->performance_levels[1].engine_clock =
+		(fiji_ps->performance_levels[1].engine_clock >=
+				fiji_ps->performance_levels[0].engine_clock) ?
+						fiji_ps->performance_levels[1].engine_clock :
+						fiji_ps->performance_levels[0].engine_clock;
+
+	if (disable_mclk_switching) {
+		if (mclk < fiji_ps->performance_levels[1].memory_clock)
+			mclk = fiji_ps->performance_levels[1].memory_clock;
+
+		fiji_ps->performance_levels[0].memory_clock = mclk;
+		fiji_ps->performance_levels[1].memory_clock = mclk;
+	} else {
+		if (fiji_ps->performance_levels[1].memory_clock <
+				fiji_ps->performance_levels[0].memory_clock)
+			fiji_ps->performance_levels[1].memory_clock =
+					fiji_ps->performance_levels[0].memory_clock;
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_StablePState)) {
+		for (i = 0; i < fiji_ps->performance_level_count; i++) {
+			fiji_ps->performance_levels[i].engine_clock = stable_pstate_sclk;
+			fiji_ps->performance_levels[i].memory_clock = stable_pstate_mclk;
+			fiji_ps->performance_levels[i].pcie_gen = data->pcie_gen_performance.max;
+			fiji_ps->performance_levels[i].pcie_lane = data->pcie_gen_performance.max;
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_find_dpm_states_clocks_in_dpm_table(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	const struct fiji_power_state *fiji_ps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_single_dpm_table *sclk_table = &(data->dpm_table.sclk_table);
+	uint32_t sclk = fiji_ps->performance_levels
+			[fiji_ps->performance_level_count - 1].engine_clock;
+	struct fiji_single_dpm_table *mclk_table = &(data->dpm_table.mclk_table);
+	uint32_t mclk = fiji_ps->performance_levels
+			[fiji_ps->performance_level_count - 1].memory_clock;
+	struct PP_Clocks min_clocks = {0};
+	uint32_t i;
+	struct cgs_display_info info = {0};
+
+	data->need_update_smu7_dpm_table = 0;
+
+	for (i = 0; i < sclk_table->count; i++) {
+		if (sclk == sclk_table->dpm_levels[i].value)
+			break;
+	}
+
+	if (i >= sclk_table->count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_OD_UPDATE_SCLK;
+	else {
+	/* TODO: Check SCLK in DAL's minimum clocks
+	 * in case DeepSleep divider update is required.
+	 */
+		if(data->display_timing.min_clock_in_sr != min_clocks.engineClockInSR)
+			data->need_update_smu7_dpm_table |= DPMTABLE_UPDATE_SCLK;
+	}
+
+	for (i = 0; i < mclk_table->count; i++) {
+		if (mclk == mclk_table->dpm_levels[i].value)
+			break;
+	}
+
+	if (i >= mclk_table->count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_OD_UPDATE_MCLK;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	if (data->display_timing.num_existing_displays != info.display_count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_UPDATE_MCLK;
+
+	return 0;
+}
+
+static uint16_t fiji_get_maximum_link_speed(struct pp_hwmgr *hwmgr,
+		const struct fiji_power_state *fiji_ps)
+{
+	uint32_t i;
+	uint32_t sclk, max_sclk = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_dpm_table *dpm_table = &data->dpm_table;
+
+	for (i = 0; i < fiji_ps->performance_level_count; i++) {
+		sclk = fiji_ps->performance_levels[i].engine_clock;
+		if (max_sclk < sclk)
+			max_sclk = sclk;
+	}
+
+	for (i = 0; i < dpm_table->sclk_table.count; i++) {
+		if (dpm_table->sclk_table.dpm_levels[i].value == max_sclk)
+			return (uint16_t) ((i >= dpm_table->pcie_speed_table.count) ?
+					dpm_table->pcie_speed_table.dpm_levels
+					[dpm_table->pcie_speed_table.count - 1].value :
+					dpm_table->pcie_speed_table.dpm_levels[i].value);
+	}
+
+	return 0;
+}
+
+static int fiji_request_link_speed_change_before_state_change(
+		struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	const struct fiji_power_state *fiji_nps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+	const struct fiji_power_state *fiji_cps =
+			cast_const_phw_fiji_power_state(states->pcurrent_state);
+
+	uint16_t target_link_speed = fiji_get_maximum_link_speed(hwmgr, fiji_nps);
+	uint16_t current_link_speed;
+
+	if (data->force_pcie_gen == PP_PCIEGenInvalid)
+		current_link_speed = fiji_get_maximum_link_speed(hwmgr, fiji_cps);
+	else
+		current_link_speed = data->force_pcie_gen;
+
+	data->force_pcie_gen = PP_PCIEGenInvalid;
+	data->pspp_notify_required = false;
+	if (target_link_speed > current_link_speed) {
+		switch(target_link_speed) {
+		case PP_PCIEGen3:
+			if (0 == acpi_pcie_perf_request(hwmgr->device, PCIE_PERF_REQ_GEN3, false))
+				break;
+			data->force_pcie_gen = PP_PCIEGen2;
+			if (current_link_speed == PP_PCIEGen2)
+				break;
+		case PP_PCIEGen2:
+			if (0 == acpi_pcie_perf_request(hwmgr->device, PCIE_PERF_REQ_GEN2, false))
+				break;
+		default:
+			data->force_pcie_gen = fiji_get_current_pcie_speed(hwmgr);
+			break;
+		}
+	} else {
+		if (target_link_speed < current_link_speed)
+			data->pspp_notify_required = true;
+	}
+
+	return 0;
+}
+
+static int fiji_freeze_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if ((0 == data->sclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+			(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK))) {
+		PP_ASSERT_WITH_CODE(true == fiji_is_dpm_running(hwmgr),
+				"Trying to freeze SCLK DPM when DPM is disabled",);
+		PP_ASSERT_WITH_CODE(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_SCLKDPM_FreezeLevel),
+				"Failed to freeze SCLK DPM during FreezeSclkMclkDPM Function!",
+				return -1);
+	}
+
+	if ((0 == data->mclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+		 DPMTABLE_OD_UPDATE_MCLK)) {
+		PP_ASSERT_WITH_CODE(true == fiji_is_dpm_running(hwmgr),
+				"Trying to freeze MCLK DPM when DPM is disabled",);
+		PP_ASSERT_WITH_CODE(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_MCLKDPM_FreezeLevel),
+				"Failed to freeze MCLK DPM during FreezeSclkMclkDPM Function!",
+				return -1);
+	}
+
+	return 0;
+}
+
+static int fiji_populate_and_upload_sclk_mclk_dpm_levels(
+		struct pp_hwmgr *hwmgr, const void *input)
+{
+	int result = 0;
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	const struct fiji_power_state *fiji_ps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t sclk = fiji_ps->performance_levels
+			[fiji_ps->performance_level_count - 1].engine_clock;
+	uint32_t mclk = fiji_ps->performance_levels
+			[fiji_ps->performance_level_count - 1].memory_clock;
+	struct fiji_dpm_table *dpm_table = &data->dpm_table;
+
+	struct fiji_dpm_table *golden_dpm_table = &data->golden_dpm_table;
+	uint32_t dpm_count, clock_percent;
+	uint32_t i;
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if (data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_SCLK) {
+		dpm_table->sclk_table.dpm_levels
+		[dpm_table->sclk_table.count - 1].value = sclk;
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_OD6PlusinACSupport) ||
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_OD6PlusinDCSupport)) {
+		/* Need to do calculation based on the golden DPM table
+		 * as the Heatmap GPU Clock axis is also based on the default values
+		 */
+			PP_ASSERT_WITH_CODE(
+				(golden_dpm_table->sclk_table.dpm_levels
+						[golden_dpm_table->sclk_table.count - 1].value != 0),
+				"Divide by 0!",
+				return -1);
+			dpm_count = dpm_table->sclk_table.count < 2 ?
+					0 : dpm_table->sclk_table.count - 2;
+			for (i = dpm_count; i > 1; i--) {
+				if (sclk > golden_dpm_table->sclk_table.dpm_levels
+						[golden_dpm_table->sclk_table.count-1].value) {
+					clock_percent =
+						((sclk - golden_dpm_table->sclk_table.dpm_levels
+							[golden_dpm_table->sclk_table.count-1].value) * 100) /
+						golden_dpm_table->sclk_table.dpm_levels
+							[golden_dpm_table->sclk_table.count-1].value;
+
+					dpm_table->sclk_table.dpm_levels[i].value =
+							golden_dpm_table->sclk_table.dpm_levels[i].value +
+							(golden_dpm_table->sclk_table.dpm_levels[i].value *
+								clock_percent)/100;
+
+				} else if (golden_dpm_table->sclk_table.dpm_levels
+						[dpm_table->sclk_table.count-1].value > sclk) {
+					clock_percent =
+						((golden_dpm_table->sclk_table.dpm_levels
+						[golden_dpm_table->sclk_table.count - 1].value - sclk) *
+								100) /
+						golden_dpm_table->sclk_table.dpm_levels
+							[golden_dpm_table->sclk_table.count-1].value;
+
+					dpm_table->sclk_table.dpm_levels[i].value =
+							golden_dpm_table->sclk_table.dpm_levels[i].value -
+							(golden_dpm_table->sclk_table.dpm_levels[i].value *
+									clock_percent) / 100;
+				} else
+					dpm_table->sclk_table.dpm_levels[i].value =
+							golden_dpm_table->sclk_table.dpm_levels[i].value;
+			}
+		}
+	}
+
+	if (data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_MCLK) {
+		dpm_table->mclk_table.dpm_levels
+			[dpm_table->mclk_table.count - 1].value = mclk;
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_OD6PlusinACSupport) ||
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_OD6PlusinDCSupport)) {
+
+			PP_ASSERT_WITH_CODE(
+					(golden_dpm_table->mclk_table.dpm_levels
+						[golden_dpm_table->mclk_table.count-1].value != 0),
+					"Divide by 0!",
+					return -1);
+			dpm_count = dpm_table->mclk_table.count < 2 ?
+					0 : dpm_table->mclk_table.count - 2;
+			for (i = dpm_count; i > 1; i--) {
+				if (mclk > golden_dpm_table->mclk_table.dpm_levels
+						[golden_dpm_table->mclk_table.count-1].value) {
+					clock_percent = ((mclk -
+							golden_dpm_table->mclk_table.dpm_levels
+							[golden_dpm_table->mclk_table.count-1].value) * 100) /
+							golden_dpm_table->mclk_table.dpm_levels
+							[golden_dpm_table->mclk_table.count-1].value;
+
+					dpm_table->mclk_table.dpm_levels[i].value =
+							golden_dpm_table->mclk_table.dpm_levels[i].value +
+							(golden_dpm_table->mclk_table.dpm_levels[i].value *
+									clock_percent) / 100;
+
+				} else if (golden_dpm_table->mclk_table.dpm_levels
+						[dpm_table->mclk_table.count-1].value > mclk) {
+					clock_percent = ((golden_dpm_table->mclk_table.dpm_levels
+							[golden_dpm_table->mclk_table.count-1].value - mclk) * 100) /
+									golden_dpm_table->mclk_table.dpm_levels
+									[golden_dpm_table->mclk_table.count-1].value;
+
+					dpm_table->mclk_table.dpm_levels[i].value =
+							golden_dpm_table->mclk_table.dpm_levels[i].value -
+							(golden_dpm_table->mclk_table.dpm_levels[i].value *
+									clock_percent) / 100;
+				} else
+					dpm_table->mclk_table.dpm_levels[i].value =
+							golden_dpm_table->mclk_table.dpm_levels[i].value;
+			}
+		}
+	}
+
+	if (data->need_update_smu7_dpm_table &
+			(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK)) {
+		result = fiji_populate_all_memory_levels(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to populate SCLK during PopulateNewDPMClocksStates Function!",
+				return result);
+	}
+
+	if (data->need_update_smu7_dpm_table &
+			(DPMTABLE_OD_UPDATE_MCLK + DPMTABLE_UPDATE_MCLK)) {
+		/*populate MCLK dpm table to SMU7 */
+		result = fiji_populate_all_memory_levels(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to populate MCLK during PopulateNewDPMClocksStates Function!",
+				return result);
+	}
+
+	return result;
+}
+
+static int fiji_trim_single_dpm_states(struct pp_hwmgr *hwmgr,
+			  struct fiji_single_dpm_table * dpm_table,
+			     uint32_t low_limit, uint32_t high_limit)
+{
+	uint32_t i;
+
+	for (i = 0; i < dpm_table->count; i++) {
+		if ((dpm_table->dpm_levels[i].value < low_limit) ||
+		    (dpm_table->dpm_levels[i].value > high_limit))
+			dpm_table->dpm_levels[i].enabled = false;
+		else
+			dpm_table->dpm_levels[i].enabled = true;
+	}
+	return 0;
+}
+
+static int fiji_trim_dpm_states(struct pp_hwmgr *hwmgr,
+		const struct fiji_power_state *fiji_ps)
+{
+	int result = 0;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t high_limit_count;
+
+	PP_ASSERT_WITH_CODE((fiji_ps->performance_level_count >= 1),
+			"power state did not have any performance level",
+			return -1);
+
+	high_limit_count = (1 == fiji_ps->performance_level_count) ? 0 : 1;
+
+	fiji_trim_single_dpm_states(hwmgr,
+			&(data->dpm_table.sclk_table),
+			fiji_ps->performance_levels[0].engine_clock,
+			fiji_ps->performance_levels[high_limit_count].engine_clock);
+
+	fiji_trim_single_dpm_states(hwmgr,
+			&(data->dpm_table.mclk_table),
+			fiji_ps->performance_levels[0].memory_clock,
+			fiji_ps->performance_levels[high_limit_count].memory_clock);
+
+	return result;
+}
+
+static int fiji_generate_dpm_level_enable_mask(
+		struct pp_hwmgr *hwmgr, const void *input)
+{
+	int result;
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	const struct fiji_power_state *fiji_ps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+
+	result = fiji_trim_dpm_states(hwmgr, fiji_ps);
+	if (result)
+		return result;
+
+	data->dpm_level_enable_mask.sclk_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&data->dpm_table.sclk_table);
+	data->dpm_level_enable_mask.mclk_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&data->dpm_table.mclk_table);
+	data->last_mclk_dpm_enable_mask =
+			data->dpm_level_enable_mask.mclk_dpm_enable_mask;
+
+	if (data->uvd_enabled) {
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask & 1)
+			data->dpm_level_enable_mask.mclk_dpm_enable_mask &= 0xFFFFFFFE;
+	}
+
+	data->dpm_level_enable_mask.pcie_dpm_enable_mask =
+			fiji_get_dpm_level_enable_mask_value(&data->dpm_table.pcie_speed_table);
+
+	return 0;
+}
+
+int fiji_enable_disable_uvd_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable ?
+				  (PPSMC_Msg)PPSMC_MSG_UVDDPM_Enable :
+				  (PPSMC_Msg)PPSMC_MSG_UVDDPM_Disable);
+}
+
+int fiji_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable?
+			PPSMC_MSG_VCEDPM_Enable :
+			PPSMC_MSG_VCEDPM_Disable);
+}
+
+int fiji_enable_disable_samu_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable?
+			PPSMC_MSG_SAMUDPM_Enable :
+			PPSMC_MSG_SAMUDPM_Disable);
+}
+
+int fiji_enable_disable_acp_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable?
+			PPSMC_MSG_ACPDPM_Enable :
+			PPSMC_MSG_ACPDPM_Disable);
+}
+
+int fiji_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (!bgate) {
+		data->smc_state_table.UvdBootLevel = 0;
+		if (table_info->mm_dep_table->count > 0)
+			data->smc_state_table.UvdBootLevel =
+					(uint8_t) (table_info->mm_dep_table->count - 1);
+		mm_boot_level_offset = data->dpm_table_start +
+				offsetof(SMU73_Discrete_DpmTable, UvdBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0x00FFFFFF;
+		mm_boot_level_value |= data->smc_state_table.UvdBootLevel << 24;
+		cgs_write_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (!phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_UVDDPM) ||
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_StablePState))
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_UVDDPM_SetEnabledMask,
+					(uint32_t)(1 << data->smc_state_table.UvdBootLevel));
+	}
+
+	return fiji_enable_disable_uvd_dpm(hwmgr, !bgate);
+}
+
+int fiji_update_vce_dpm(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	const struct fiji_power_state *fiji_nps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+	const struct fiji_power_state *fiji_cps =
+			cast_const_phw_fiji_power_state(states->pcurrent_state);
+
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (fiji_nps->vce_clks.evclk >0 &&
+	(fiji_cps == NULL || fiji_cps->vce_clks.evclk == 0)) {
+		data->smc_state_table.VceBootLevel =
+				(uint8_t) (table_info->mm_dep_table->count - 1);
+
+		mm_boot_level_offset = data->dpm_table_start +
+				offsetof(SMU73_Discrete_DpmTable, VceBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0xFF00FFFF;
+		mm_boot_level_value |= data->smc_state_table.VceBootLevel << 16;
+		cgs_write_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_StablePState)) {
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_VCEDPM_SetEnabledMask,
+					(uint32_t)1 << data->smc_state_table.VceBootLevel);
+
+			fiji_enable_disable_vce_dpm(hwmgr, true);
+		} else if (fiji_nps->vce_clks.evclk == 0 &&
+				fiji_cps != NULL &&
+				fiji_cps->vce_clks.evclk > 0)
+			fiji_enable_disable_vce_dpm(hwmgr, false);
+	}
+
+	return 0;
+}
+
+int fiji_update_samu_dpm(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (!bgate) {
+		data->smc_state_table.SamuBootLevel =
+				(uint8_t) (table_info->mm_dep_table->count - 1);
+		mm_boot_level_offset = data->dpm_table_start +
+				offsetof(SMU73_Discrete_DpmTable, SamuBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0xFFFFFF00;
+		mm_boot_level_value |= data->smc_state_table.SamuBootLevel << 0;
+		cgs_write_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_StablePState))
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_SAMUDPM_SetEnabledMask,
+					(uint32_t)(1 << data->smc_state_table.SamuBootLevel));
+	}
+
+	return fiji_enable_disable_samu_dpm(hwmgr, !bgate);
+}
+
+int fiji_update_acp_dpm(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (!bgate) {
+		data->smc_state_table.AcpBootLevel =
+				(uint8_t) (table_info->mm_dep_table->count - 1);
+		mm_boot_level_offset = data->dpm_table_start +
+				offsetof(SMU73_Discrete_DpmTable, AcpBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0xFFFF00FF;
+		mm_boot_level_value |= data->smc_state_table.AcpBootLevel << 8;
+		cgs_write_ind_register(hwmgr->device,
+				CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_StablePState))
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_ACPDPM_SetEnabledMask,
+						(uint32_t)(1 << data->smc_state_table.AcpBootLevel));
+	}
+
+	return fiji_enable_disable_acp_dpm(hwmgr, !bgate);
+}
+
+static int fiji_update_sclk_threshold(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	int result = 0;
+	uint32_t low_sclk_interrupt_threshold = 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SclkThrottleLowNotification)
+		&& (hwmgr->gfx_arbiter.sclk_threshold !=
+				data->low_sclk_interrupt_threshold)) {
+		data->low_sclk_interrupt_threshold =
+				hwmgr->gfx_arbiter.sclk_threshold;
+		low_sclk_interrupt_threshold =
+				data->low_sclk_interrupt_threshold;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(low_sclk_interrupt_threshold);
+
+		result = fiji_copy_bytes_to_smc(
+				hwmgr->smumgr,
+				data->dpm_table_start +
+				offsetof(SMU73_Discrete_DpmTable,
+					LowSclkInterruptThreshold),
+				(uint8_t *)&low_sclk_interrupt_threshold,
+				sizeof(uint32_t),
+				data->sram_end);
+	}
+
+	return result;
+}
+
+static int fiji_program_mem_timing_parameters(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (data->need_update_smu7_dpm_table &
+		(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_OD_UPDATE_MCLK))
+		return fiji_program_memory_timing_parameters(hwmgr);
+
+	return 0;
+}
+
+static int fiji_unfreeze_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if ((0 == data->sclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+		(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK))) {
+
+		PP_ASSERT_WITH_CODE(true == fiji_is_dpm_running(hwmgr),
+				"Trying to Unfreeze SCLK DPM when DPM is disabled",);
+		PP_ASSERT_WITH_CODE(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_SCLKDPM_UnfreezeLevel),
+			"Failed to unfreeze SCLK DPM during UnFreezeSclkMclkDPM Function!",
+			return -1);
+	}
+
+	if ((0 == data->mclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_MCLK)) {
+
+		PP_ASSERT_WITH_CODE(true == fiji_is_dpm_running(hwmgr),
+				"Trying to Unfreeze MCLK DPM when DPM is disabled",);
+		PP_ASSERT_WITH_CODE(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+				PPSMC_MSG_SCLKDPM_UnfreezeLevel),
+		    "Failed to unfreeze MCLK DPM during UnFreezeSclkMclkDPM Function!",
+		    return -1);
+	}
+
+	data->need_update_smu7_dpm_table = 0;
+
+	return 0;
+}
+
+/* Look up the voltaged based on DAL's requested level.
+ * and then send the requested VDDC voltage to SMC
+ */
+static void fiji_apply_dal_minimum_voltage_request(struct pp_hwmgr *hwmgr)
+{
+	return;
+}
+
+int fiji_upload_dpm_level_enable_mask(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/* Apply minimum voltage based on DAL's request level */
+	fiji_apply_dal_minimum_voltage_request(hwmgr);
+
+	if (0 == data->sclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this,
+		 *  we should skip this message.
+		 */
+		if (!fiji_is_dpm_running(hwmgr))
+			printk(KERN_ERR "[ powerplay ] "
+					"Trying to set Enable Mask when DPM is disabled \n");
+
+		if (data->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+			result = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_SCLKDPM_SetEnabledMask,
+					data->dpm_level_enable_mask.sclk_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == result),
+				"Set Sclk Dpm enable Mask failed", return -1);
+		}
+	}
+
+	if (0 == data->mclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this,
+		 *  we should skip this message.
+		 */
+		if (!fiji_is_dpm_running(hwmgr))
+			printk(KERN_ERR "[ powerplay ]"
+					" Trying to set Enable Mask when DPM is disabled \n");
+
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask) {
+			result = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_MCLKDPM_SetEnabledMask,
+					data->dpm_level_enable_mask.mclk_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == result),
+				"Set Mclk Dpm enable Mask failed", return -1);
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_notify_link_speed_change_after_state_change(
+		struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states =
+			(const struct phm_set_power_state_input *)input;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	const struct fiji_power_state *fiji_ps =
+			cast_const_phw_fiji_power_state(states->pnew_state);
+	uint16_t target_link_speed = fiji_get_maximum_link_speed(hwmgr, fiji_ps);
+	uint8_t  request;
+
+	if (data->pspp_notify_required) {
+		if (target_link_speed == PP_PCIEGen3)
+			request = PCIE_PERF_REQ_GEN3;
+		else if (target_link_speed == PP_PCIEGen2)
+			request = PCIE_PERF_REQ_GEN2;
+		else
+			request = PCIE_PERF_REQ_GEN1;
+
+		if(request == PCIE_PERF_REQ_GEN1 &&
+				fiji_get_current_pcie_speed(hwmgr) > 0)
+			return 0;
+
+		if (acpi_pcie_perf_request(hwmgr->device, request, false)) {
+			if (PP_PCIEGen2 == target_link_speed)
+				printk("PSPP request to switch to Gen2 from Gen3 Failed!");
+			else
+				printk("PSPP request to switch to Gen1 from Gen2 Failed!");
+		}
+	}
+
+	return 0;
+}
+
+static int fiji_set_power_state_tasks(struct pp_hwmgr *hwmgr,
+		const void *input)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = fiji_find_dpm_states_clocks_in_dpm_table(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to find DPM states clocks in DPM table!",
+			result = tmp_result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PCIEPerformanceRequest)) {
+		tmp_result =
+			fiji_request_link_speed_change_before_state_change(hwmgr, input);
+		PP_ASSERT_WITH_CODE((0 == tmp_result),
+				"Failed to request link speed change before state change!",
+				result = tmp_result);
+	}
+
+	tmp_result = fiji_freeze_sclk_mclk_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to freeze SCLK MCLK DPM!", result = tmp_result);
+
+	tmp_result = fiji_populate_and_upload_sclk_mclk_dpm_levels(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to populate and upload SCLK MCLK DPM levels!",
+			result = tmp_result);
+
+	tmp_result = fiji_generate_dpm_level_enable_mask(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to generate DPM level enabled mask!",
+			result = tmp_result);
+
+	tmp_result = fiji_update_vce_dpm(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to update VCE DPM!",
+			result = tmp_result);
+
+	tmp_result = fiji_update_sclk_threshold(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to update SCLK threshold!",
+			result = tmp_result);
+
+	tmp_result = fiji_program_mem_timing_parameters(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to program memory timing parameters!",
+			result = tmp_result);
+
+	tmp_result = fiji_unfreeze_sclk_mclk_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to unfreeze SCLK MCLK DPM!",
+			result = tmp_result);
+
+	tmp_result = fiji_upload_dpm_level_enable_mask(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to upload DPM level enabled mask!",
+			result = tmp_result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PCIEPerformanceRequest)) {
+		tmp_result =
+			fiji_notify_link_speed_change_after_state_change(hwmgr, input);
+		PP_ASSERT_WITH_CODE((0 == tmp_result),
+				"Failed to notify link speed change after state change!",
+				result = tmp_result);
+	}
+
+	return result;
+}
+
+static int fiji_dpm_get_sclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct pp_power_state  *ps;
+	struct fiji_power_state  *fiji_ps;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	ps = hwmgr->request_ps;
+
+	if (ps == NULL)
+		return -EINVAL;
+
+	fiji_ps = cast_phw_fiji_power_state(&ps->hardware);
+
+	if (low)
+		return fiji_ps->performance_levels[0].engine_clock;
+	else
+		return fiji_ps->performance_levels
+				[fiji_ps->performance_level_count-1].engine_clock;
+}
+
+static int fiji_dpm_get_mclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct pp_power_state  *ps;
+	struct fiji_power_state  *fiji_ps;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	ps = hwmgr->request_ps;
+
+	if (ps == NULL)
+		return -EINVAL;
+
+	fiji_ps = cast_phw_fiji_power_state(&ps->hardware);
+
+	if (low)
+		return fiji_ps->performance_levels[0].memory_clock;
+	else
+		return fiji_ps->performance_levels
+				[fiji_ps->performance_level_count-1].memory_clock;
+}
+
+static void fiji_print_current_perforce_level(
+		struct pp_hwmgr *hwmgr, struct seq_file *m)
+{
+	uint32_t sclk, mclk, activity_percent = 0;
+	uint32_t offset;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_API_GetSclkFrequency);
+
+	sclk = cgs_read_register(hwmgr->device, mmSMC_MSG_ARG_0);
+
+	smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_API_GetMclkFrequency);
+
+	mclk = cgs_read_register(hwmgr->device, mmSMC_MSG_ARG_0);
+	seq_printf(m, "\n [  mclk  ]: %u MHz\n\n [  sclk  ]: %u MHz\n",
+			mclk / 100, sclk / 100);
+
+	offset = data->soft_regs_start + offsetof(SMU73_SoftRegisters, AverageGraphicsActivity);
+	activity_percent = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, offset);
+	activity_percent += 0x80;
+	activity_percent >>= 8;
+
+	seq_printf(m, "\n [GPU load]: %u%%\n\n", activity_percent > 100 ? 100 : activity_percent);
+}
+
+static int fiji_program_display_gap(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t num_active_displays = 0;
+	uint32_t display_gap = cgs_read_ind_register(hwmgr->device,
+			CGS_IND_REG__SMC, ixCG_DISPLAY_GAP_CNTL);
+	uint32_t display_gap2;
+	uint32_t pre_vbi_time_in_us;
+	uint32_t frame_time_in_us;
+	uint32_t ref_clock;
+	uint32_t refresh_rate = 0;
+	struct cgs_display_info info = {0};
+	struct cgs_mode_info mode_info;
+
+	info.mode_info = &mode_info;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+	num_active_displays = info.display_count;
+
+	display_gap = PHM_SET_FIELD(display_gap, CG_DISPLAY_GAP_CNTL,
+			DISP_GAP, (num_active_displays > 0)?
+			DISPLAY_GAP_VBLANK_OR_WM : DISPLAY_GAP_IGNORE);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_DISPLAY_GAP_CNTL, display_gap);
+
+	ref_clock = mode_info.ref_clock;
+	refresh_rate = mode_info.refresh_rate;
+
+	if (refresh_rate == 0)
+		refresh_rate = 60;
+
+	frame_time_in_us = 1000000 / refresh_rate;
+
+	pre_vbi_time_in_us = frame_time_in_us - 200 - mode_info.vblank_time_us;
+	display_gap2 = pre_vbi_time_in_us * (ref_clock / 100);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_DISPLAY_GAP_CNTL2, display_gap2);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			data->soft_regs_start +
+			offsetof(SMU73_SoftRegisters, PreVBlankGap), 0x64);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			data->soft_regs_start +
+			offsetof(SMU73_SoftRegisters, VBlankTimeout),
+			(frame_time_in_us - pre_vbi_time_in_us));
+
+	if (num_active_displays == 1)
+		tonga_notify_smc_display_change(hwmgr, true);
+
+	return 0;
+}
+
+int fiji_display_configuration_changed_task(struct pp_hwmgr *hwmgr)
+{
+	return fiji_program_display_gap(hwmgr);
+}
+
+static int fiji_set_max_fan_pwm_output(struct pp_hwmgr *hwmgr,
+		uint16_t us_max_fan_pwm)
+{
+	hwmgr->thermal_controller.
+	advanceFanControlParameters.usMaxFanPWM = us_max_fan_pwm;
+
+	if (phm_is_hw_access_blocked(hwmgr))
+		return 0;
+
+	return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			PPSMC_MSG_SetFanPwmMax, us_max_fan_pwm);
+}
+
+static int fiji_set_max_fan_rpm_output(struct pp_hwmgr *hwmgr,
+		uint16_t us_max_fan_rpm)
+{
+	hwmgr->thermal_controller.
+	advanceFanControlParameters.usMaxFanRPM = us_max_fan_rpm;
+
+	if (phm_is_hw_access_blocked(hwmgr))
+		return 0;
+
+	return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+			PPSMC_MSG_SetFanRpmMax, us_max_fan_rpm);
+}
+
+int fiji_dpm_set_interrupt_state(void *private_data,
+					 unsigned src_id, unsigned type,
+					 int enabled)
+{
+	uint32_t cg_thermal_int;
+	struct pp_hwmgr *hwmgr = ((struct pp_eventmgr *)private_data)->hwmgr;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	switch (type) {
+	case AMD_THERMAL_IRQ_LOW_TO_HIGH:
+		if (enabled) {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int |= CG_THERMAL_INT_CTRL__THERM_INTH_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		} else {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int &= ~CG_THERMAL_INT_CTRL__THERM_INTH_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		}
+		break;
+
+	case AMD_THERMAL_IRQ_HIGH_TO_LOW:
+		if (enabled) {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int |= CG_THERMAL_INT_CTRL__THERM_INTL_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		} else {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int &= ~CG_THERMAL_INT_CTRL__THERM_INTL_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device,
+					CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+int fiji_register_internal_thermal_interrupt(struct pp_hwmgr *hwmgr,
+					const void *thermal_interrupt_info)
+{
+	int result;
+	const struct pp_interrupt_registration_info *info =
+			(const struct pp_interrupt_registration_info *)
+			thermal_interrupt_info;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	result = cgs_add_irq_source(hwmgr->device, 230, AMD_THERMAL_IRQ_LAST,
+				fiji_dpm_set_interrupt_state,
+				info->call_back, info->context);
+
+	if (result)
+		return -EINVAL;
+
+	result = cgs_add_irq_source(hwmgr->device, 231, AMD_THERMAL_IRQ_LAST,
+				fiji_dpm_set_interrupt_state,
+				info->call_back, info->context);
+
+	if (result)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int fiji_set_fan_control_mode(struct pp_hwmgr *hwmgr, uint32_t mode)
+{
+	if (mode) {
+		/* stop auto-manage */
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_MicrocodeFanControl))
+			fiji_fan_ctrl_stop_smc_fan_control(hwmgr);
+		fiji_fan_ctrl_set_static_mode(hwmgr, mode);
+	} else
+		/* restart auto-manage */
+		fiji_fan_ctrl_reset_fan_speed_to_default(hwmgr);
+
+	return 0;
+}
+
+static int fiji_get_fan_control_mode(struct pp_hwmgr *hwmgr)
+{
+	if (hwmgr->fan_ctrl_is_in_default_mode)
+		return hwmgr->fan_ctrl_default_mode;
+	else
+		return PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_FDO_CTRL2, FDO_PWM_MODE);
+}
+
+static const struct pp_hwmgr_func fiji_hwmgr_funcs = {
+	.backend_init = &fiji_hwmgr_backend_init,
+	.backend_fini = &tonga_hwmgr_backend_fini,
+	.asic_setup = &fiji_setup_asic_task,
+	.dynamic_state_management_enable = &fiji_enable_dpm_tasks,
+	.force_dpm_level = &fiji_dpm_force_dpm_level,
+	.get_num_of_pp_table_entries = &tonga_get_number_of_powerplay_table_entries,
+	.get_power_state_size = &fiji_get_power_state_size,
+	.get_pp_table_entry = &fiji_get_pp_table_entry,
+	.patch_boot_state = &fiji_patch_boot_state,
+	.apply_state_adjust_rules = &fiji_apply_state_adjust_rules,
+	.power_state_set = &fiji_set_power_state_tasks,
+	.get_sclk = &fiji_dpm_get_sclk,
+	.get_mclk = &fiji_dpm_get_mclk,
+	.print_current_perforce_level = &fiji_print_current_perforce_level,
+	.powergate_uvd = &fiji_phm_powergate_uvd,
+	.powergate_vce = &fiji_phm_powergate_vce,
+	.disable_clock_power_gating = &fiji_phm_disable_clock_power_gating,
+	.notify_smc_display_config_after_ps_adjustment =
+			&tonga_notify_smc_display_config_after_ps_adjustment,
+	.display_config_changed = &fiji_display_configuration_changed_task,
+	.set_max_fan_pwm_output = fiji_set_max_fan_pwm_output,
+	.set_max_fan_rpm_output = fiji_set_max_fan_rpm_output,
+	.get_temperature = fiji_thermal_get_temperature,
+	.stop_thermal_controller = fiji_thermal_stop_thermal_controller,
+	.get_fan_speed_info = fiji_fan_ctrl_get_fan_speed_info,
+	.get_fan_speed_percent = fiji_fan_ctrl_get_fan_speed_percent,
+	.set_fan_speed_percent = fiji_fan_ctrl_set_fan_speed_percent,
+	.reset_fan_speed_to_default = fiji_fan_ctrl_reset_fan_speed_to_default,
+	.get_fan_speed_rpm = fiji_fan_ctrl_get_fan_speed_rpm,
+	.set_fan_speed_rpm = fiji_fan_ctrl_set_fan_speed_rpm,
+	.uninitialize_thermal_controller = fiji_thermal_ctrl_uninitialize_thermal_controller,
+	.register_internal_thermal_interrupt = fiji_register_internal_thermal_interrupt,
+	.set_fan_control_mode = fiji_set_fan_control_mode,
+	.get_fan_control_mode = fiji_get_fan_control_mode,
+};
+
+int fiji_hwmgr_init(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr  *data;
+	int ret = 0;
+
+	data = kzalloc(sizeof(struct fiji_hwmgr), GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+
+	hwmgr->backend = data;
+	hwmgr->hwmgr_func = &fiji_hwmgr_funcs;
+	hwmgr->pptable_func = &tonga_pptable_funcs;
+	pp_fiji_thermal_initialize(hwmgr);
+	return ret;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.h b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.h
new file mode 100644
index 000000000000..22e273b1c1c5
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_hwmgr.h
@@ -0,0 +1,361 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _FIJI_HWMGR_H_
+#define _FIJI_HWMGR_H_
+
+#include "hwmgr.h"
+#include "smu73.h"
+#include "smu73_discrete.h"
+#include "ppatomctrl.h"
+#include "fiji_ppsmc.h"
+
+#define FIJI_MAX_HARDWARE_POWERLEVELS	2
+#define FIJI_AT_DFLT	30
+
+#define FIJI_VOLTAGE_CONTROL_NONE                   0x0
+#define FIJI_VOLTAGE_CONTROL_BY_GPIO                0x1
+#define FIJI_VOLTAGE_CONTROL_BY_SVID2               0x2
+#define FIJI_VOLTAGE_CONTROL_MERGED                 0x3
+
+#define DPMTABLE_OD_UPDATE_SCLK     0x00000001
+#define DPMTABLE_OD_UPDATE_MCLK     0x00000002
+#define DPMTABLE_UPDATE_SCLK        0x00000004
+#define DPMTABLE_UPDATE_MCLK        0x00000008
+
+struct fiji_performance_level {
+	uint32_t  memory_clock;
+	uint32_t  engine_clock;
+	uint16_t  pcie_gen;
+	uint16_t  pcie_lane;
+};
+
+struct fiji_uvd_clocks {
+	uint32_t  vclk;
+	uint32_t  dclk;
+};
+
+struct fiji_vce_clocks {
+	uint32_t  evclk;
+	uint32_t  ecclk;
+};
+
+struct fiji_power_state {
+    uint32_t                  magic;
+    struct fiji_uvd_clocks    uvd_clks;
+    struct fiji_vce_clocks    vce_clks;
+    uint32_t                  sam_clk;
+    uint32_t                  acp_clk;
+    uint16_t                  performance_level_count;
+    bool                      dc_compatible;
+    uint32_t                  sclk_threshold;
+    struct fiji_performance_level  performance_levels[FIJI_MAX_HARDWARE_POWERLEVELS];
+};
+
+struct fiji_dpm_level {
+	bool	enabled;
+    uint32_t	value;
+    uint32_t	param1;
+};
+
+#define FIJI_MAX_DEEPSLEEP_DIVIDER_ID 5
+#define MAX_REGULAR_DPM_NUMBER 8
+#define FIJI_MINIMUM_ENGINE_CLOCK 2500
+
+struct fiji_single_dpm_table {
+	uint32_t		count;
+	struct fiji_dpm_level	dpm_levels[MAX_REGULAR_DPM_NUMBER];
+};
+
+struct fiji_dpm_table {
+	struct fiji_single_dpm_table  sclk_table;
+	struct fiji_single_dpm_table  mclk_table;
+	struct fiji_single_dpm_table  pcie_speed_table;
+	struct fiji_single_dpm_table  vddc_table;
+	struct fiji_single_dpm_table  vddci_table;
+	struct fiji_single_dpm_table  mvdd_table;
+};
+
+struct fiji_clock_registers {
+	uint32_t  vCG_SPLL_FUNC_CNTL;
+	uint32_t  vCG_SPLL_FUNC_CNTL_2;
+	uint32_t  vCG_SPLL_FUNC_CNTL_3;
+	uint32_t  vCG_SPLL_FUNC_CNTL_4;
+	uint32_t  vCG_SPLL_SPREAD_SPECTRUM;
+	uint32_t  vCG_SPLL_SPREAD_SPECTRUM_2;
+	uint32_t  vDLL_CNTL;
+	uint32_t  vMCLK_PWRMGT_CNTL;
+	uint32_t  vMPLL_AD_FUNC_CNTL;
+	uint32_t  vMPLL_DQ_FUNC_CNTL;
+	uint32_t  vMPLL_FUNC_CNTL;
+	uint32_t  vMPLL_FUNC_CNTL_1;
+	uint32_t  vMPLL_FUNC_CNTL_2;
+	uint32_t  vMPLL_SS1;
+	uint32_t  vMPLL_SS2;
+};
+
+struct fiji_voltage_smio_registers {
+	uint32_t vS0_VID_LOWER_SMIO_CNTL;
+};
+
+#define FIJI_MAX_LEAKAGE_COUNT  8
+struct fiji_leakage_voltage {
+	uint16_t  count;
+	uint16_t  leakage_id[FIJI_MAX_LEAKAGE_COUNT];
+	uint16_t  actual_voltage[FIJI_MAX_LEAKAGE_COUNT];
+};
+
+struct fiji_vbios_boot_state {
+	uint16_t    mvdd_bootup_value;
+	uint16_t    vddc_bootup_value;
+	uint16_t    vddci_bootup_value;
+	uint32_t    sclk_bootup_value;
+	uint32_t    mclk_bootup_value;
+	uint16_t    pcie_gen_bootup_value;
+	uint16_t    pcie_lane_bootup_value;
+};
+
+struct fiji_bacos {
+	uint32_t                       best_match;
+	uint32_t                       baco_flags;
+	struct fiji_performance_level  performance_level;
+};
+
+/* Ultra Low Voltage parameter structure */
+struct fiji_ulv_parm {
+	bool                           ulv_supported;
+	uint32_t                       cg_ulv_parameter;
+	uint32_t                       ulv_volt_change_delay;
+	struct fiji_performance_level  ulv_power_level;
+};
+
+struct fiji_display_timing {
+	uint32_t  min_clock_in_sr;
+	uint32_t  num_existing_displays;
+};
+
+struct fiji_dpmlevel_enable_mask {
+	uint32_t  uvd_dpm_enable_mask;
+	uint32_t  vce_dpm_enable_mask;
+	uint32_t  acp_dpm_enable_mask;
+	uint32_t  samu_dpm_enable_mask;
+	uint32_t  sclk_dpm_enable_mask;
+	uint32_t  mclk_dpm_enable_mask;
+	uint32_t  pcie_dpm_enable_mask;
+};
+
+struct fiji_pcie_perf_range {
+	uint16_t  max;
+	uint16_t  min;
+};
+
+struct fiji_hwmgr {
+	struct fiji_dpm_table			dpm_table;
+	struct fiji_dpm_table			golden_dpm_table;
+
+	uint32_t						voting_rights_clients0;
+	uint32_t						voting_rights_clients1;
+	uint32_t						voting_rights_clients2;
+	uint32_t						voting_rights_clients3;
+	uint32_t						voting_rights_clients4;
+	uint32_t						voting_rights_clients5;
+	uint32_t						voting_rights_clients6;
+	uint32_t						voting_rights_clients7;
+	uint32_t						static_screen_threshold_unit;
+	uint32_t						static_screen_threshold;
+	uint32_t						voltage_control;
+	uint32_t						vddc_vddci_delta;
+
+	uint32_t						active_auto_throttle_sources;
+
+	struct fiji_clock_registers            clock_registers;
+	struct fiji_voltage_smio_registers      voltage_smio_registers;
+
+	bool                           is_memory_gddr5;
+	uint16_t                       acpi_vddc;
+	bool                           pspp_notify_required;
+	uint16_t                       force_pcie_gen;
+	uint16_t                       acpi_pcie_gen;
+	uint32_t                       pcie_gen_cap;
+	uint32_t                       pcie_lane_cap;
+	uint32_t                       pcie_spc_cap;
+	struct fiji_leakage_voltage          vddc_leakage;
+	struct fiji_leakage_voltage          Vddci_leakage;
+
+	uint32_t                             mvdd_control;
+	uint32_t                             vddc_mask_low;
+	uint32_t                             mvdd_mask_low;
+	uint16_t                            max_vddc_in_pptable;
+	uint16_t                            min_vddc_in_pptable;
+	uint16_t                            max_vddci_in_pptable;
+	uint16_t                            min_vddci_in_pptable;
+	uint32_t                             mclk_strobe_mode_threshold;
+	uint32_t                             mclk_stutter_mode_threshold;
+	uint32_t                             mclk_edc_enable_threshold;
+	uint32_t                             mclk_edcwr_enable_threshold;
+	bool                                is_uvd_enabled;
+	struct fiji_vbios_boot_state        vbios_boot_state;
+
+	bool                           battery_state;
+	bool                           is_tlu_enabled;
+
+	/* ---- SMC SRAM Address of firmware header tables ---- */
+	uint32_t                             sram_end;
+	uint32_t                             dpm_table_start;
+	uint32_t                             soft_regs_start;
+	uint32_t                             mc_reg_table_start;
+	uint32_t                             fan_table_start;
+	uint32_t                             arb_table_start;
+	struct SMU73_Discrete_DpmTable       smc_state_table;
+	struct SMU73_Discrete_Ulv            ulv_setting;
+
+	/* ---- Stuff originally coming from Evergreen ---- */
+	uint32_t                             vddci_control;
+	struct pp_atomctrl_voltage_table     vddc_voltage_table;
+	struct pp_atomctrl_voltage_table     vddci_voltage_table;
+	struct pp_atomctrl_voltage_table     mvdd_voltage_table;
+
+	uint32_t                             mgcg_cgtt_local2;
+	uint32_t                             mgcg_cgtt_local3;
+	uint32_t                             gpio_debug;
+	uint32_t                             mc_micro_code_feature;
+	uint32_t                             highest_mclk;
+	uint16_t                             acpi_vddci;
+	uint8_t                              mvdd_high_index;
+	uint8_t                              mvdd_low_index;
+	bool                                 dll_default_on;
+	bool                                 performance_request_registered;
+
+	/* ---- Low Power Features ---- */
+	struct fiji_bacos                    bacos;
+	struct fiji_ulv_parm                 ulv;
+
+	/* ---- CAC Stuff ---- */
+	uint32_t                       cac_table_start;
+	bool                           cac_configuration_required;
+	bool                           driver_calculate_cac_leakage;
+	bool                           cac_enabled;
+
+	/* ---- DPM2 Parameters ---- */
+	uint32_t                       power_containment_features;
+	bool                           enable_dte_feature;
+	bool                           enable_tdc_limit_feature;
+	bool                           enable_pkg_pwr_tracking_feature;
+	bool                           disable_uvd_power_tune_feature;
+	struct fiji_pt_defaults       *power_tune_defaults;
+	struct SMU73_Discrete_PmFuses  power_tune_table;
+	uint32_t                       dte_tj_offset;
+	uint32_t                       fast_watermark_threshold;
+
+	/* ---- Phase Shedding ---- */
+	bool                           vddc_phase_shed_control;
+
+	/* ---- DI/DT ---- */
+	struct fiji_display_timing        display_timing;
+
+	/* ---- Thermal Temperature Setting ---- */
+	struct fiji_dpmlevel_enable_mask     dpm_level_enable_mask;
+	uint32_t                             need_update_smu7_dpm_table;
+	uint32_t                             sclk_dpm_key_disabled;
+	uint32_t                             mclk_dpm_key_disabled;
+	uint32_t                             pcie_dpm_key_disabled;
+	uint32_t                             min_engine_clocks;
+	struct fiji_pcie_perf_range          pcie_gen_performance;
+	struct fiji_pcie_perf_range          pcie_lane_performance;
+	struct fiji_pcie_perf_range          pcie_gen_power_saving;
+	struct fiji_pcie_perf_range          pcie_lane_power_saving;
+	bool                                 use_pcie_performance_levels;
+	bool                                 use_pcie_power_saving_levels;
+	uint32_t                             activity_target[SMU73_MAX_LEVELS_GRAPHICS];
+	uint32_t                             mclk_activity_target;
+	uint32_t                             mclk_dpm0_activity_target;
+	uint32_t                             low_sclk_interrupt_threshold;
+	uint32_t                             last_mclk_dpm_enable_mask;
+	bool                                 uvd_enabled;
+
+	/* ---- Power Gating States ---- */
+	bool                           uvd_power_gated;
+	bool                           vce_power_gated;
+	bool                           samu_power_gated;
+	bool                           acp_power_gated;
+	bool                           pg_acp_init;
+	bool                           frtc_enabled;
+	bool                           frtc_status_changed;
+};
+
+/* To convert to Q8.8 format for firmware */
+#define FIJI_Q88_FORMAT_CONVERSION_UNIT             256
+
+enum Fiji_I2CLineID {
+    Fiji_I2CLineID_DDC1 = 0x90,
+    Fiji_I2CLineID_DDC2 = 0x91,
+    Fiji_I2CLineID_DDC3 = 0x92,
+    Fiji_I2CLineID_DDC4 = 0x93,
+    Fiji_I2CLineID_DDC5 = 0x94,
+    Fiji_I2CLineID_DDC6 = 0x95,
+    Fiji_I2CLineID_SCLSDA = 0x96,
+    Fiji_I2CLineID_DDCVGA = 0x97
+};
+
+#define Fiji_I2C_DDC1DATA          0
+#define Fiji_I2C_DDC1CLK           1
+#define Fiji_I2C_DDC2DATA          2
+#define Fiji_I2C_DDC2CLK           3
+#define Fiji_I2C_DDC3DATA          4
+#define Fiji_I2C_DDC3CLK           5
+#define Fiji_I2C_SDA               40
+#define Fiji_I2C_SCL               41
+#define Fiji_I2C_DDC4DATA          65
+#define Fiji_I2C_DDC4CLK           66
+#define Fiji_I2C_DDC5DATA          0x48
+#define Fiji_I2C_DDC5CLK           0x49
+#define Fiji_I2C_DDC6DATA          0x4a
+#define Fiji_I2C_DDC6CLK           0x4b
+#define Fiji_I2C_DDCVGADATA        0x4c
+#define Fiji_I2C_DDCVGACLK         0x4d
+
+#define FIJI_UNUSED_GPIO_PIN       0x7F
+
+extern int tonga_initializa_dynamic_state_adjustment_rule_settings(struct pp_hwmgr *hwmgr);
+extern int tonga_hwmgr_backend_fini(struct pp_hwmgr *hwmgr);
+extern int tonga_get_mc_microcode_version (struct pp_hwmgr *hwmgr);
+extern int tonga_notify_smc_display_config_after_ps_adjustment(struct pp_hwmgr *hwmgr);
+extern int tonga_notify_smc_display_change(struct pp_hwmgr *hwmgr, bool has_display);
+int fiji_update_vce_dpm(struct pp_hwmgr *hwmgr, const void *input);
+int fiji_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate);
+int fiji_update_samu_dpm(struct pp_hwmgr *hwmgr, bool bgate);
+int fiji_update_acp_dpm(struct pp_hwmgr *hwmgr, bool bgate);
+int fiji_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable);
+
+#define PP_HOST_TO_SMC_UL(X) cpu_to_be32(X)
+#define PP_SMC_TO_HOST_UL(X) be32_to_cpu(X)
+
+#define PP_HOST_TO_SMC_US(X) cpu_to_be16(X)
+#define PP_SMC_TO_HOST_US(X) be16_to_cpu(X)
+
+#define CONVERT_FROM_HOST_TO_SMC_UL(X) ((X) = PP_HOST_TO_SMC_UL(X))
+#define CONVERT_FROM_SMC_TO_HOST_UL(X) ((X) = PP_SMC_TO_HOST_UL(X))
+
+#define CONVERT_FROM_HOST_TO_SMC_US(X) ((X) = PP_HOST_TO_SMC_US(X))
+
+#endif /* _FIJI_HWMGR_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.c b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.c
new file mode 100644
index 000000000000..6efcb2bac45f
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.c
@@ -0,0 +1,553 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "hwmgr.h"
+#include "smumgr.h"
+#include "fiji_hwmgr.h"
+#include "fiji_powertune.h"
+#include "fiji_smumgr.h"
+#include "smu73_discrete.h"
+#include "pp_debug.h"
+
+#define VOLTAGE_SCALE  4
+#define POWERTUNE_DEFAULT_SET_MAX    1
+
+struct fiji_pt_defaults fiji_power_tune_data_set_array[POWERTUNE_DEFAULT_SET_MAX] = {
+		/*sviLoadLIneEn,  SviLoadLineVddC, TDC_VDDC_ThrottleReleaseLimitPerc */
+		{1,               0xF,             0xFD,
+		/* TDC_MAWt, TdcWaterfallCtl, DTEAmbientTempBase */
+		0x19,        5,               45}
+};
+
+void fiji_initialize_power_tune_defaults(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *fiji_hwmgr = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct  phm_ppt_v1_information *table_info =
+			(struct  phm_ppt_v1_information *)(hwmgr->pptable);
+	uint32_t tmp = 0;
+
+	if(table_info &&
+			table_info->cac_dtp_table->usPowerTuneDataSetID <= POWERTUNE_DEFAULT_SET_MAX &&
+			table_info->cac_dtp_table->usPowerTuneDataSetID)
+		fiji_hwmgr->power_tune_defaults =
+				&fiji_power_tune_data_set_array
+				[table_info->cac_dtp_table->usPowerTuneDataSetID - 1];
+	else
+		fiji_hwmgr->power_tune_defaults = &fiji_power_tune_data_set_array[0];
+
+	/* Assume disabled */
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PowerContainment);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_CAC);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SQRamping);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DBRamping);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_TDRamping);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_TCPRamping);
+
+	fiji_hwmgr->dte_tj_offset = tmp;
+
+	if (!tmp) {
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_PowerContainment);
+
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_CAC);
+
+		fiji_hwmgr->fast_watermark_threshold = 100;
+
+		tmp = 1;
+		fiji_hwmgr->enable_dte_feature = tmp ? false : true;
+		fiji_hwmgr->enable_tdc_limit_feature = tmp ? true : false;
+		fiji_hwmgr->enable_pkg_pwr_tracking_feature = tmp ? true : false;
+	}
+}
+
+/* PPGen has the gain setting generated in x * 100 unit
+ * This function is to convert the unit to x * 4096(0x1000) unit.
+ *  This is the unit expected by SMC firmware
+ */
+static uint16_t scale_fan_gain_settings(uint16_t raw_setting)
+{
+	uint32_t tmp;
+	tmp = raw_setting * 4096 / 100;
+	return (uint16_t)tmp;
+}
+
+static void get_scl_sda_value(uint8_t line, uint8_t *scl, uint8_t* sda)
+{
+	switch (line) {
+	case Fiji_I2CLineID_DDC1 :
+		*scl = Fiji_I2C_DDC1CLK;
+		*sda = Fiji_I2C_DDC1DATA;
+		break;
+	case Fiji_I2CLineID_DDC2 :
+		*scl = Fiji_I2C_DDC2CLK;
+		*sda = Fiji_I2C_DDC2DATA;
+		break;
+	case Fiji_I2CLineID_DDC3 :
+		*scl = Fiji_I2C_DDC3CLK;
+		*sda = Fiji_I2C_DDC3DATA;
+		break;
+	case Fiji_I2CLineID_DDC4 :
+		*scl = Fiji_I2C_DDC4CLK;
+		*sda = Fiji_I2C_DDC4DATA;
+		break;
+	case Fiji_I2CLineID_DDC5 :
+		*scl = Fiji_I2C_DDC5CLK;
+		*sda = Fiji_I2C_DDC5DATA;
+		break;
+	case Fiji_I2CLineID_DDC6 :
+		*scl = Fiji_I2C_DDC6CLK;
+		*sda = Fiji_I2C_DDC6DATA;
+		break;
+	case Fiji_I2CLineID_SCLSDA :
+		*scl = Fiji_I2C_SCL;
+		*sda = Fiji_I2C_SDA;
+		break;
+	case Fiji_I2CLineID_DDCVGA :
+		*scl = Fiji_I2C_DDCVGACLK;
+		*sda = Fiji_I2C_DDCVGADATA;
+		break;
+	default:
+		*scl = 0;
+		*sda = 0;
+		break;
+	}
+}
+
+int fiji_populate_bapm_parameters_in_dpm_table(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct fiji_pt_defaults *defaults = data->power_tune_defaults;
+	SMU73_Discrete_DpmTable  *dpm_table = &(data->smc_state_table);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_cac_tdp_table *cac_dtp_table = table_info->cac_dtp_table;
+	struct pp_advance_fan_control_parameters *fan_table=
+			&hwmgr->thermal_controller.advanceFanControlParameters;
+	uint8_t uc_scl, uc_sda;
+
+	/* TDP number of fraction bits are changed from 8 to 7 for Fiji
+	 * as requested by SMC team
+	 */
+	dpm_table->DefaultTdp = PP_HOST_TO_SMC_US(
+			(uint16_t)(cac_dtp_table->usTDP * 128));
+	dpm_table->TargetTdp = PP_HOST_TO_SMC_US(
+			(uint16_t)(cac_dtp_table->usTDP * 128));
+
+	PP_ASSERT_WITH_CODE(cac_dtp_table->usTargetOperatingTemp <= 255,
+			"Target Operating Temp is out of Range!",);
+
+	dpm_table->GpuTjMax = (uint8_t)(cac_dtp_table->usTargetOperatingTemp);
+	dpm_table->GpuTjHyst = 8;
+
+	dpm_table->DTEAmbientTempBase = defaults->DTEAmbientTempBase;
+
+	/* The following are for new Fiji Multi-input fan/thermal control */
+	dpm_table->TemperatureLimitEdge = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTargetOperatingTemp * 256);
+	dpm_table->TemperatureLimitHotspot = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitHotspot * 256);
+	dpm_table->TemperatureLimitLiquid1 = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitLiquid1 * 256);
+	dpm_table->TemperatureLimitLiquid2 = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitLiquid2 * 256);
+	dpm_table->TemperatureLimitVrVddc = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitVrVddc * 256);
+	dpm_table->TemperatureLimitVrMvdd = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitVrMvdd * 256);
+	dpm_table->TemperatureLimitPlx = PP_HOST_TO_SMC_US(
+			cac_dtp_table->usTemperatureLimitPlx * 256);
+
+	dpm_table->FanGainEdge = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainEdge));
+	dpm_table->FanGainHotspot = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainHotspot));
+	dpm_table->FanGainLiquid = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainLiquid));
+	dpm_table->FanGainVrVddc = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainVrVddc));
+	dpm_table->FanGainVrMvdd = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainVrMvdd));
+	dpm_table->FanGainPlx = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainPlx));
+	dpm_table->FanGainHbm = PP_HOST_TO_SMC_US(
+			scale_fan_gain_settings(fan_table->usFanGainHbm));
+
+	dpm_table->Liquid1_I2C_address = cac_dtp_table->ucLiquid1_I2C_address;
+	dpm_table->Liquid2_I2C_address = cac_dtp_table->ucLiquid2_I2C_address;
+	dpm_table->Vr_I2C_address = cac_dtp_table->ucVr_I2C_address;
+	dpm_table->Plx_I2C_address = cac_dtp_table->ucPlx_I2C_address;
+
+	get_scl_sda_value(cac_dtp_table->ucLiquid_I2C_Line, &uc_scl, &uc_sda);
+	dpm_table->Liquid_I2C_LineSCL = uc_scl;
+	dpm_table->Liquid_I2C_LineSDA = uc_sda;
+
+	get_scl_sda_value(cac_dtp_table->ucVr_I2C_Line, &uc_scl, &uc_sda);
+	dpm_table->Vr_I2C_LineSCL = uc_scl;
+	dpm_table->Vr_I2C_LineSDA = uc_sda;
+
+	get_scl_sda_value(cac_dtp_table->ucPlx_I2C_Line, &uc_scl, &uc_sda);
+	dpm_table->Plx_I2C_LineSCL = uc_scl;
+	dpm_table->Plx_I2C_LineSDA = uc_sda;
+
+	return 0;
+}
+
+static int fiji_populate_svi_load_line(struct pp_hwmgr *hwmgr)
+{
+    struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+    struct fiji_pt_defaults *defaults = data->power_tune_defaults;
+
+    data->power_tune_table.SviLoadLineEn = defaults->SviLoadLineEn;
+    data->power_tune_table.SviLoadLineVddC = defaults->SviLoadLineVddC;
+    data->power_tune_table.SviLoadLineTrimVddC = 3;
+    data->power_tune_table.SviLoadLineOffsetVddC = 0;
+
+    return 0;
+}
+
+static int fiji_populate_tdc_limit(struct pp_hwmgr *hwmgr)
+{
+	uint16_t tdc_limit;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct  fiji_pt_defaults *defaults = data->power_tune_defaults;
+
+	/* TDC number of fraction bits are changed from 8 to 7
+	 * for Fiji as requested by SMC team
+	 */
+	tdc_limit = (uint16_t)(table_info->cac_dtp_table->usTDC * 128);
+	data->power_tune_table.TDC_VDDC_PkgLimit =
+			CONVERT_FROM_HOST_TO_SMC_US(tdc_limit);
+	data->power_tune_table.TDC_VDDC_ThrottleReleaseLimitPerc =
+			defaults->TDC_VDDC_ThrottleReleaseLimitPerc;
+	data->power_tune_table.TDC_MAWt = defaults->TDC_MAWt;
+
+	return 0;
+}
+
+static int fiji_populate_dw8(struct pp_hwmgr *hwmgr, uint32_t fuse_table_offset)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct  fiji_pt_defaults *defaults = data->power_tune_defaults;
+	uint32_t temp;
+
+	if (fiji_read_smc_sram_dword(hwmgr->smumgr,
+			fuse_table_offset +
+			offsetof(SMU73_Discrete_PmFuses, TdcWaterfallCtl),
+			(uint32_t *)&temp, data->sram_end))
+		PP_ASSERT_WITH_CODE(false,
+				"Attempt to read PmFuses.DW6 (SviLoadLineEn) from SMC Failed!",
+				return -EINVAL);
+	else {
+		data->power_tune_table.TdcWaterfallCtl = defaults->TdcWaterfallCtl;
+		data->power_tune_table.LPMLTemperatureMin =
+				(uint8_t)((temp >> 16) & 0xff);
+		data->power_tune_table.LPMLTemperatureMax =
+				(uint8_t)((temp >> 8) & 0xff);
+		data->power_tune_table.Reserved = (uint8_t)(temp & 0xff);
+	}
+	return 0;
+}
+
+static int fiji_populate_temperature_scaler(struct pp_hwmgr *hwmgr)
+{
+	int i;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/* Currently not used. Set all to zero. */
+	for (i = 0; i < 16; i++)
+		data->power_tune_table.LPMLTemperatureScaler[i] = 0;
+
+	return 0;
+}
+
+static int fiji_populate_fuzzy_fan(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if( (hwmgr->thermal_controller.advanceFanControlParameters.
+			usFanOutputSensitivity & (1 << 15)) ||
+			0 == hwmgr->thermal_controller.advanceFanControlParameters.
+			usFanOutputSensitivity )
+		hwmgr->thermal_controller.advanceFanControlParameters.
+		usFanOutputSensitivity = hwmgr->thermal_controller.
+			advanceFanControlParameters.usDefaultFanOutputSensitivity;
+
+	data->power_tune_table.FuzzyFan_PwmSetDelta =
+			PP_HOST_TO_SMC_US(hwmgr->thermal_controller.
+					advanceFanControlParameters.usFanOutputSensitivity);
+	return 0;
+}
+
+static int fiji_populate_gnb_lpml(struct pp_hwmgr *hwmgr)
+{
+	int i;
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	/* Currently not used. Set all to zero. */
+	for (i = 0; i < 16; i++)
+		data->power_tune_table.GnbLPML[i] = 0;
+
+	return 0;
+}
+
+static int fiji_min_max_vgnb_lpml_id_from_bapm_vddc(struct pp_hwmgr *hwmgr)
+{
+ /*   int  i, min, max;
+    struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+    uint8_t * pHiVID = data->power_tune_table.BapmVddCVidHiSidd;
+    uint8_t * pLoVID = data->power_tune_table.BapmVddCVidLoSidd;
+
+    min = max = pHiVID[0];
+    for (i = 0; i < 8; i++) {
+        if (0 != pHiVID[i]) {
+            if (min > pHiVID[i])
+                min = pHiVID[i];
+            if (max < pHiVID[i])
+                max = pHiVID[i];
+        }
+
+        if (0 != pLoVID[i]) {
+            if (min > pLoVID[i])
+                min = pLoVID[i];
+            if (max < pLoVID[i])
+                max = pLoVID[i];
+        }
+    }
+
+    PP_ASSERT_WITH_CODE((0 != min) && (0 != max), "BapmVddcVidSidd table does not exist!", return int_Failed);
+    data->power_tune_table.GnbLPMLMaxVid = (uint8_t)max;
+    data->power_tune_table.GnbLPMLMinVid = (uint8_t)min;
+*/
+    return 0;
+}
+
+static int fiji_populate_bapm_vddc_base_leakage_sidd(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint16_t HiSidd = data->power_tune_table.BapmVddCBaseLeakageHiSidd;
+	uint16_t LoSidd = data->power_tune_table.BapmVddCBaseLeakageLoSidd;
+	struct phm_cac_tdp_table *cac_table = table_info->cac_dtp_table;
+
+	HiSidd = (uint16_t)(cac_table->usHighCACLeakage / 100 * 256);
+	LoSidd = (uint16_t)(cac_table->usLowCACLeakage / 100 * 256);
+
+	data->power_tune_table.BapmVddCBaseLeakageHiSidd =
+			CONVERT_FROM_HOST_TO_SMC_US(HiSidd);
+	data->power_tune_table.BapmVddCBaseLeakageLoSidd =
+			CONVERT_FROM_HOST_TO_SMC_US(LoSidd);
+
+	return 0;
+}
+
+int fiji_populate_pm_fuses(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	uint32_t pm_fuse_table_offset;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PowerContainment)) {
+		if (fiji_read_smc_sram_dword(hwmgr->smumgr,
+				SMU7_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU73_Firmware_Header, PmFuseTable),
+				&pm_fuse_table_offset, data->sram_end))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to get pm_fuse_table_offset Failed!",
+					return -EINVAL);
+
+		/* DW6 */
+		if (fiji_populate_svi_load_line(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate SviLoadLine Failed!",
+					return -EINVAL);
+		/* DW7 */
+		if (fiji_populate_tdc_limit(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate TDCLimit Failed!", return -EINVAL);
+		/* DW8 */
+		if (fiji_populate_dw8(hwmgr, pm_fuse_table_offset))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate TdcWaterfallCtl, "
+					"LPMLTemperature Min and Max Failed!",
+					return -EINVAL);
+
+		/* DW9-DW12 */
+		if (0 != fiji_populate_temperature_scaler(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate LPMLTemperatureScaler Failed!",
+					return -EINVAL);
+
+		/* DW13-DW14 */
+		if(fiji_populate_fuzzy_fan(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate Fuzzy Fan Control parameters Failed!",
+					return -EINVAL);
+
+		/* DW15-DW18 */
+		if (fiji_populate_gnb_lpml(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate GnbLPML Failed!",
+					return -EINVAL);
+
+		/* DW19 */
+		if (fiji_min_max_vgnb_lpml_id_from_bapm_vddc(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate GnbLPML Min and Max Vid Failed!",
+					return -EINVAL);
+
+		/* DW20 */
+		if (fiji_populate_bapm_vddc_base_leakage_sidd(hwmgr))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to populate BapmVddCBaseLeakage Hi and Lo "
+					"Sidd Failed!", return -EINVAL);
+
+		if (fiji_copy_bytes_to_smc(hwmgr->smumgr, pm_fuse_table_offset,
+				(uint8_t *)&data->power_tune_table,
+				sizeof(struct SMU73_Discrete_PmFuses), data->sram_end))
+			PP_ASSERT_WITH_CODE(false,
+					"Attempt to download PmFuseTable Failed!",
+					return -EINVAL);
+	}
+	return 0;
+}
+
+int fiji_enable_smc_cac(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	int result = 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_CAC)) {
+		int smc_result;
+		smc_result = smum_send_msg_to_smc(hwmgr->smumgr,
+				(uint16_t)(PPSMC_MSG_EnableCac));
+		PP_ASSERT_WITH_CODE((0 == smc_result),
+				"Failed to enable CAC in SMC.", result = -1);
+
+        data->cac_enabled = (0 == smc_result) ? true : false;
+	}
+	return result;
+}
+
+int fiji_set_power_limit(struct pp_hwmgr *hwmgr, uint32_t n)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+
+	if(data->power_containment_features &
+			POWERCONTAINMENT_FEATURE_PkgPwrLimit)
+		return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_PkgPwrSetLimit, n);
+	return 0;
+}
+
+static int fiji_set_overdriver_target_tdp(struct pp_hwmgr *pHwMgr, uint32_t target_tdp)
+{
+	return smum_send_msg_to_smc_with_parameter(pHwMgr->smumgr,
+			PPSMC_MSG_OverDriveSetTargetTdp, target_tdp);
+}
+
+int fiji_enable_power_containment(struct pp_hwmgr *hwmgr)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	int smc_result;
+	int result = 0;
+
+	data->power_containment_features = 0;
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PowerContainment)) {
+		if (data->enable_dte_feature) {
+			smc_result = smum_send_msg_to_smc(hwmgr->smumgr,
+					(uint16_t)(PPSMC_MSG_EnableDTE));
+			PP_ASSERT_WITH_CODE((0 == smc_result),
+					"Failed to enable DTE in SMC.", result = -1;);
+			if (0 == smc_result)
+				data->power_containment_features |= POWERCONTAINMENT_FEATURE_DTE;
+		}
+
+		if (data->enable_tdc_limit_feature) {
+			smc_result = smum_send_msg_to_smc(hwmgr->smumgr,
+					(uint16_t)(PPSMC_MSG_TDCLimitEnable));
+			PP_ASSERT_WITH_CODE((0 == smc_result),
+					"Failed to enable TDCLimit in SMC.", result = -1;);
+			if (0 == smc_result)
+				data->power_containment_features |=
+						POWERCONTAINMENT_FEATURE_TDCLimit;
+		}
+
+		if (data->enable_pkg_pwr_tracking_feature) {
+			smc_result = smum_send_msg_to_smc(hwmgr->smumgr,
+					(uint16_t)(PPSMC_MSG_PkgPwrLimitEnable));
+			PP_ASSERT_WITH_CODE((0 == smc_result),
+					"Failed to enable PkgPwrTracking in SMC.", result = -1;);
+			if (0 == smc_result) {
+				struct phm_cac_tdp_table *cac_table =
+						table_info->cac_dtp_table;
+				uint32_t default_limit =
+					(uint32_t)(cac_table->usMaximumPowerDeliveryLimit * 256);
+
+				data->power_containment_features |=
+						POWERCONTAINMENT_FEATURE_PkgPwrLimit;
+
+				if (fiji_set_power_limit(hwmgr, default_limit))
+					printk(KERN_ERR "Failed to set Default Power Limit in SMC!");
+			}
+		}
+	}
+	return result;
+}
+
+int fiji_power_control_set_level(struct pp_hwmgr *hwmgr)
+{
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_cac_tdp_table *cac_table = table_info->cac_dtp_table;
+	int adjust_percent, target_tdp;
+	int result = 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PowerContainment)) {
+		/* adjustment percentage has already been validated */
+		adjust_percent = hwmgr->platform_descriptor.TDPAdjustmentPolarity ?
+				hwmgr->platform_descriptor.TDPAdjustment :
+				(-1 * hwmgr->platform_descriptor.TDPAdjustment);
+		/* SMC requested that target_tdp to be 7 bit fraction in DPM table
+		 * but message to be 8 bit fraction for messages
+		 */
+		target_tdp = ((100 + adjust_percent) * (int)(cac_table->usTDP * 256)) / 100;
+		result = fiji_set_overdriver_target_tdp(hwmgr, (uint32_t)target_tdp);
+	}
+
+	return result;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.h b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.h
new file mode 100644
index 000000000000..55e58200f33a
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_powertune.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef FIJI_POWERTUNE_H
+#define FIJI_POWERTUNE_H
+
+enum fiji_pt_config_reg_type {
+	FIJI_CONFIGREG_MMR = 0,
+	FIJI_CONFIGREG_SMC_IND,
+	FIJI_CONFIGREG_DIDT_IND,
+	FIJI_CONFIGREG_CACHE,
+	FIJI_CONFIGREG_MAX
+};
+
+/* PowerContainment Features */
+#define POWERCONTAINMENT_FEATURE_DTE             0x00000001
+#define POWERCONTAINMENT_FEATURE_TDCLimit        0x00000002
+#define POWERCONTAINMENT_FEATURE_PkgPwrLimit     0x00000004
+
+struct fiji_pt_config_reg {
+	uint32_t                           offset;
+	uint32_t                           mask;
+	uint32_t                           shift;
+	uint32_t                           value;
+	enum fiji_pt_config_reg_type       type;
+};
+
+struct fiji_pt_defaults
+{
+    uint8_t   SviLoadLineEn;
+    uint8_t   SviLoadLineVddC;
+    uint8_t   TDC_VDDC_ThrottleReleaseLimitPerc;
+    uint8_t   TDC_MAWt;
+    uint8_t   TdcWaterfallCtl;
+    uint8_t   DTEAmbientTempBase;
+};
+
+void fiji_initialize_power_tune_defaults(struct pp_hwmgr *hwmgr);
+int fiji_populate_bapm_parameters_in_dpm_table(struct pp_hwmgr *hwmgr);
+int fiji_populate_pm_fuses(struct pp_hwmgr *hwmgr);
+int fiji_enable_smc_cac(struct pp_hwmgr *hwmgr);
+int fiji_enable_power_containment(struct pp_hwmgr *hwmgr);
+int fiji_set_power_limit(struct pp_hwmgr *hwmgr, uint32_t n);
+int fiji_power_control_set_level(struct pp_hwmgr *hwmgr);
+
+#endif  /* FIJI_POWERTUNE_H */
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.c b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.c
new file mode 100644
index 000000000000..e76a7de9aa32
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.c
@@ -0,0 +1,687 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <asm/div64.h>
+#include "fiji_thermal.h"
+#include "fiji_hwmgr.h"
+#include "fiji_smumgr.h"
+#include "fiji_ppsmc.h"
+#include "smu/smu_7_1_3_d.h"
+#include "smu/smu_7_1_3_sh_mask.h"
+
+int fiji_fan_ctrl_get_fan_speed_info(struct pp_hwmgr *hwmgr,
+		struct phm_fan_speed_info *fan_speed_info)
+{
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	fan_speed_info->supports_percent_read = true;
+	fan_speed_info->supports_percent_write = true;
+	fan_speed_info->min_percent = 0;
+	fan_speed_info->max_percent = 100;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_FanSpeedInTableIsRPM) &&
+		hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution) {
+		fan_speed_info->supports_rpm_read = true;
+		fan_speed_info->supports_rpm_write = true;
+		fan_speed_info->min_rpm = hwmgr->thermal_controller.fanInfo.ulMinRPM;
+		fan_speed_info->max_rpm = hwmgr->thermal_controller.fanInfo.ulMaxRPM;
+	} else {
+		fan_speed_info->min_rpm = 0;
+		fan_speed_info->max_rpm = 0;
+	}
+
+	return 0;
+}
+
+int fiji_fan_ctrl_get_fan_speed_percent(struct pp_hwmgr *hwmgr,
+		uint32_t *speed)
+{
+	uint32_t duty100;
+	uint32_t duty;
+	uint64_t tmp64;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL1, FMAX_DUTY100);
+	duty = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_STATUS, FDO_PWM_DUTY);
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+
+	tmp64 = (uint64_t)duty * 100;
+	do_div(tmp64, duty100);
+	*speed = (uint32_t)tmp64;
+
+	if (*speed > 100)
+		*speed = 100;
+
+	return 0;
+}
+
+int fiji_fan_ctrl_get_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t *speed)
+{
+	uint32_t tach_period;
+	uint32_t crystal_clock_freq;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan ||
+			(hwmgr->thermal_controller.fanInfo.
+				ucTachometerPulsesPerRevolution == 0))
+		return 0;
+
+	tach_period = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_TACH_STATUS, TACH_PERIOD);
+
+	if (tach_period == 0)
+		return -EINVAL;
+
+	crystal_clock_freq = tonga_get_xclk(hwmgr);
+
+	*speed = 60 * crystal_clock_freq * 10000/ tach_period;
+
+	return 0;
+}
+
+/**
+* Set Fan Speed Control to static mode, so that the user can decide what speed to use.
+* @param    hwmgr  the address of the powerplay hardware manager.
+*           mode    the fan control mode, 0 default, 1 by percent, 5, by RPM
+* @exception Should always succeed.
+*/
+int fiji_fan_ctrl_set_static_mode(struct pp_hwmgr *hwmgr, uint32_t mode)
+{
+
+	if (hwmgr->fan_ctrl_is_in_default_mode) {
+		hwmgr->fan_ctrl_default_mode =
+				PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device,	CGS_IND_REG__SMC,
+						CG_FDO_CTRL2, FDO_PWM_MODE);
+		hwmgr->tmin =
+				PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+						CG_FDO_CTRL2, TMIN);
+		hwmgr->fan_ctrl_is_in_default_mode = false;
+	}
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL2, TMIN, 0);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL2, FDO_PWM_MODE, mode);
+
+	return 0;
+}
+
+/**
+* Reset Fan Speed Control to default mode.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @exception Should always succeed.
+*/
+int fiji_fan_ctrl_set_default_mode(struct pp_hwmgr *hwmgr)
+{
+	if (!hwmgr->fan_ctrl_is_in_default_mode) {
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_FDO_CTRL2, FDO_PWM_MODE, hwmgr->fan_ctrl_default_mode);
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_FDO_CTRL2, TMIN, hwmgr->tmin);
+		hwmgr->fan_ctrl_is_in_default_mode = true;
+	}
+
+	return 0;
+}
+
+int fiji_fan_ctrl_start_smc_fan_control(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ODFuzzyFanControlSupport)) {
+		cgs_write_register(hwmgr->device, mmSMC_MSG_ARG_0, FAN_CONTROL_FUZZY);
+		result = smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StartFanControl);
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_FanSpeedInTableIsRPM))
+			hwmgr->hwmgr_func->set_max_fan_rpm_output(hwmgr,
+					hwmgr->thermal_controller.
+					advanceFanControlParameters.usMaxFanRPM);
+		else
+			hwmgr->hwmgr_func->set_max_fan_pwm_output(hwmgr,
+					hwmgr->thermal_controller.
+					advanceFanControlParameters.usMaxFanPWM);
+
+	} else {
+		cgs_write_register(hwmgr->device, mmSMC_MSG_ARG_0, FAN_CONTROL_TABLE);
+		result = smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StartFanControl);
+	}
+
+	if (!result && hwmgr->thermal_controller.
+			advanceFanControlParameters.ucTargetTemperature)
+		result = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetFanTemperatureTarget,
+				hwmgr->thermal_controller.
+				advanceFanControlParameters.ucTargetTemperature);
+
+	return result;
+}
+
+
+int fiji_fan_ctrl_stop_smc_fan_control(struct pp_hwmgr *hwmgr)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StopFanControl);
+}
+
+/**
+* Set Fan Speed in percent.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    speed is the percentage value (0% - 100%) to be set.
+* @exception Fails is the 100% setting appears to be 0.
+*/
+int fiji_fan_ctrl_set_fan_speed_percent(struct pp_hwmgr *hwmgr,
+		uint32_t speed)
+{
+	uint32_t duty100;
+	uint32_t duty;
+	uint64_t tmp64;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	if (speed > 100)
+		speed = 100;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_MicrocodeFanControl))
+		fiji_fan_ctrl_stop_smc_fan_control(hwmgr);
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL1, FMAX_DUTY100);
+
+	if (duty100 == 0)
+		return -EINVAL;
+
+	tmp64 = (uint64_t)speed * 100;
+	do_div(tmp64, duty100);
+	duty = (uint32_t)tmp64;
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL0, FDO_STATIC_DUTY, duty);
+
+	return fiji_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+}
+
+/**
+* Reset Fan Speed to default.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @exception Always succeeds.
+*/
+int fiji_fan_ctrl_reset_fan_speed_to_default(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_MicrocodeFanControl)) {
+		result = fiji_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+		if (!result)
+			result = fiji_fan_ctrl_start_smc_fan_control(hwmgr);
+	} else
+		result = fiji_fan_ctrl_set_default_mode(hwmgr);
+
+	return result;
+}
+
+/**
+* Set Fan Speed in RPM.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    speed is the percentage value (min - max) to be set.
+* @exception Fails is the speed not lie between min and max.
+*/
+int fiji_fan_ctrl_set_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t speed)
+{
+	uint32_t tach_period;
+	uint32_t crystal_clock_freq;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan ||
+			(hwmgr->thermal_controller.fanInfo.
+			ucTachometerPulsesPerRevolution == 0) ||
+			(speed < hwmgr->thermal_controller.fanInfo.ulMinRPM) ||
+			(speed > hwmgr->thermal_controller.fanInfo.ulMaxRPM))
+		return 0;
+
+	crystal_clock_freq = tonga_get_xclk(hwmgr);
+
+	tach_period = 60 * crystal_clock_freq * 10000 / (8 * speed);
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_TACH_STATUS, TACH_PERIOD, tach_period);
+
+	return fiji_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+}
+
+/**
+* Reads the remote temperature from the SIslands thermal controller.
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+int fiji_thermal_get_temperature(struct pp_hwmgr *hwmgr)
+{
+	int temp;
+
+	temp = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_MULT_THERMAL_STATUS, CTF_TEMP);
+
+	/* Bit 9 means the reading is lower than the lowest usable value. */
+	if (temp & 0x200)
+		temp = FIJI_THERMAL_MAXIMUM_TEMP_READING;
+	else
+		temp = temp & 0x1ff;
+
+	temp *= PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+
+	return temp;
+}
+
+/**
+* Set the requested temperature range for high and low alert signals
+*
+* @param    hwmgr The address of the hardware manager.
+* @param    range Temperature range to be programmed for high and low alert signals
+* @exception PP_Result_BadInput if the input data is not valid.
+*/
+static int fiji_thermal_set_temperature_range(struct pp_hwmgr *hwmgr,
+		uint32_t low_temp, uint32_t high_temp)
+{
+	uint32_t low = FIJI_THERMAL_MINIMUM_ALERT_TEMP *
+			PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+	uint32_t high = FIJI_THERMAL_MAXIMUM_ALERT_TEMP *
+			PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+
+	if (low < low_temp)
+		low = low_temp;
+	if (high > high_temp)
+		high = high_temp;
+
+	if (low > high)
+		return -EINVAL;
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, DIG_THERM_INTH,
+			(high / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, DIG_THERM_INTL,
+			(low / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_CTRL, DIG_THERM_DPM,
+			(high / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+
+	return 0;
+}
+
+/**
+* Programs thermal controller one-time setting registers
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+static int fiji_thermal_initialize(struct pp_hwmgr *hwmgr)
+{
+	if (hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution)
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_TACH_CTRL, EDGE_PER_REV,
+				hwmgr->thermal_controller.fanInfo.
+				ucTachometerPulsesPerRevolution - 1);
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL2, TACH_PWM_RESP_RATE, 0x28);
+
+	return 0;
+}
+
+/**
+* Enable thermal alerts on the RV770 thermal controller.
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+static int fiji_thermal_enable_alert(struct pp_hwmgr *hwmgr)
+{
+	uint32_t alert;
+
+	alert = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, THERM_INT_MASK);
+	alert &= ~(FIJI_THERMAL_HIGH_ALERT_MASK | FIJI_THERMAL_LOW_ALERT_MASK);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, THERM_INT_MASK, alert);
+
+	/* send message to SMU to enable internal thermal interrupts */
+	return smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_Thermal_Cntl_Enable);
+}
+
+/**
+* Disable thermal alerts on the RV770 thermal controller.
+* @param    hwmgr The address of the hardware manager.
+*/
+static int fiji_thermal_disable_alert(struct pp_hwmgr *hwmgr)
+{
+	uint32_t alert;
+
+	alert = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, THERM_INT_MASK);
+	alert |= (FIJI_THERMAL_HIGH_ALERT_MASK | FIJI_THERMAL_LOW_ALERT_MASK);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_THERMAL_INT, THERM_INT_MASK, alert);
+
+	/* send message to SMU to disable internal thermal interrupts */
+	return smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_Thermal_Cntl_Disable);
+}
+
+/**
+* Uninitialize the thermal controller.
+* Currently just disables alerts.
+* @param    hwmgr The address of the hardware manager.
+*/
+int fiji_thermal_stop_thermal_controller(struct pp_hwmgr *hwmgr)
+{
+	int result = fiji_thermal_disable_alert(hwmgr);
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		fiji_fan_ctrl_set_default_mode(hwmgr);
+
+	return result;
+}
+
+/**
+* Set up the fan table to control the fan using the SMC.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_fiji_thermal_setup_fan_table(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+	struct fiji_hwmgr *data = (struct fiji_hwmgr *)(hwmgr->backend);
+	SMU73_Discrete_FanTable fan_table = { FDO_MODE_HARDWARE };
+	uint32_t duty100;
+	uint32_t t_diff1, t_diff2, pwm_diff1, pwm_diff2;
+	uint16_t fdo_min, slope1, slope2;
+	uint32_t reference_clock;
+	int res;
+	uint64_t tmp64;
+
+	if (data->fan_table_start == 0) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_MicrocodeFanControl);
+		return 0;
+	}
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+			CG_FDO_CTRL1, FMAX_DUTY100);
+
+	if (duty100 == 0) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_MicrocodeFanControl);
+		return 0;
+	}
+
+	tmp64 = hwmgr->thermal_controller.advanceFanControlParameters.
+			usPWMMin * duty100;
+	do_div(tmp64, 10000);
+	fdo_min = (uint16_t)tmp64;
+
+	t_diff1 = hwmgr->thermal_controller.advanceFanControlParameters.usTMed -
+			hwmgr->thermal_controller.advanceFanControlParameters.usTMin;
+	t_diff2 = hwmgr->thermal_controller.advanceFanControlParameters.usTHigh -
+			hwmgr->thermal_controller.advanceFanControlParameters.usTMed;
+
+	pwm_diff1 = hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed -
+			hwmgr->thermal_controller.advanceFanControlParameters.usPWMMin;
+	pwm_diff2 = hwmgr->thermal_controller.advanceFanControlParameters.usPWMHigh -
+			hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed;
+
+	slope1 = (uint16_t)((50 + ((16 * duty100 * pwm_diff1) / t_diff1)) / 100);
+	slope2 = (uint16_t)((50 + ((16 * duty100 * pwm_diff2) / t_diff2)) / 100);
+
+	fan_table.TempMin = cpu_to_be16((50 + hwmgr->
+			thermal_controller.advanceFanControlParameters.usTMin) / 100);
+	fan_table.TempMed = cpu_to_be16((50 + hwmgr->
+			thermal_controller.advanceFanControlParameters.usTMed) / 100);
+	fan_table.TempMax = cpu_to_be16((50 + hwmgr->
+			thermal_controller.advanceFanControlParameters.usTMax) / 100);
+
+	fan_table.Slope1 = cpu_to_be16(slope1);
+	fan_table.Slope2 = cpu_to_be16(slope2);
+
+	fan_table.FdoMin = cpu_to_be16(fdo_min);
+
+	fan_table.HystDown = cpu_to_be16(hwmgr->
+			thermal_controller.advanceFanControlParameters.ucTHyst);
+
+	fan_table.HystUp = cpu_to_be16(1);
+
+	fan_table.HystSlope = cpu_to_be16(1);
+
+	fan_table.TempRespLim = cpu_to_be16(5);
+
+	reference_clock = tonga_get_xclk(hwmgr);
+
+	fan_table.RefreshPeriod = cpu_to_be32((hwmgr->
+			thermal_controller.advanceFanControlParameters.ulCycleDelay *
+			reference_clock) / 1600);
+
+	fan_table.FdoMax = cpu_to_be16((uint16_t)duty100);
+
+	fan_table.TempSrc = (uint8_t)PHM_READ_VFPF_INDIRECT_FIELD(
+			hwmgr->device, CGS_IND_REG__SMC,
+			CG_MULT_THERMAL_CTRL, TEMP_SEL);
+
+	res = fiji_copy_bytes_to_smc(hwmgr->smumgr, data->fan_table_start,
+			(uint8_t *)&fan_table, (uint32_t)sizeof(fan_table),
+			data->sram_end);
+
+	if (!res && hwmgr->thermal_controller.
+			advanceFanControlParameters.ucMinimumPWMLimit)
+		res = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetFanMinPwm,
+				hwmgr->thermal_controller.
+				advanceFanControlParameters.ucMinimumPWMLimit);
+
+	if (!res && hwmgr->thermal_controller.
+			advanceFanControlParameters.ulMinFanSCLKAcousticLimit)
+		res = smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+				PPSMC_MSG_SetFanSclkTarget,
+				hwmgr->thermal_controller.
+				advanceFanControlParameters.ulMinFanSCLKAcousticLimit);
+
+	if (res)
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_MicrocodeFanControl);
+
+	return 0;
+}
+
+/**
+* Start the fan control on the SMC.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_fiji_thermal_start_smc_fan_control(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+/* If the fantable setup has failed we could have disabled
+ * PHM_PlatformCaps_MicrocodeFanControl even after
+ * this function was included in the table.
+ * Make sure that we still think controlling the fan is OK.
+*/
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_MicrocodeFanControl)) {
+		fiji_fan_ctrl_start_smc_fan_control(hwmgr);
+		fiji_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+	}
+
+	return 0;
+}
+
+/**
+* Set temperature range for high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_fiji_thermal_set_temperature_range(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+	struct PP_TemperatureRange *range = (struct PP_TemperatureRange *)input;
+
+	if (range == NULL)
+		return -EINVAL;
+
+	return fiji_thermal_set_temperature_range(hwmgr, range->min, range->max);
+}
+
+/**
+* Programs one-time setting registers
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from initialize thermal controller routine
+*/
+int tf_fiji_thermal_initialize(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+    return fiji_thermal_initialize(hwmgr);
+}
+
+/**
+* Enable high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from enable alert routine
+*/
+int tf_fiji_thermal_enable_alert(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+	return fiji_thermal_enable_alert(hwmgr);
+}
+
+/**
+* Disable high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from disable alert routine
+*/
+static int tf_fiji_thermal_disable_alert(struct pp_hwmgr *hwmgr,
+		void *input, void *output, void *storage, int result)
+{
+	return fiji_thermal_disable_alert(hwmgr);
+}
+
+static struct phm_master_table_item
+fiji_thermal_start_thermal_controller_master_list[] = {
+	{NULL, tf_fiji_thermal_initialize},
+	{NULL, tf_fiji_thermal_set_temperature_range},
+	{NULL, tf_fiji_thermal_enable_alert},
+/* We should restrict performance levels to low before we halt the SMC.
+ * On the other hand we are still in boot state when we do this
+ * so it would be pointless.
+ * If this assumption changes we have to revisit this table.
+ */
+	{NULL, tf_fiji_thermal_setup_fan_table},
+	{NULL, tf_fiji_thermal_start_smc_fan_control},
+	{NULL, NULL}
+};
+
+static struct phm_master_table_header
+fiji_thermal_start_thermal_controller_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	fiji_thermal_start_thermal_controller_master_list
+};
+
+static struct phm_master_table_item
+fiji_thermal_set_temperature_range_master_list[] = {
+	{NULL, tf_fiji_thermal_disable_alert},
+	{NULL, tf_fiji_thermal_set_temperature_range},
+	{NULL, tf_fiji_thermal_enable_alert},
+	{NULL, NULL}
+};
+
+struct phm_master_table_header
+fiji_thermal_set_temperature_range_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	fiji_thermal_set_temperature_range_master_list
+};
+
+int fiji_thermal_ctrl_uninitialize_thermal_controller(struct pp_hwmgr *hwmgr)
+{
+	if (!hwmgr->thermal_controller.fanInfo.bNoFan)
+		fiji_fan_ctrl_set_default_mode(hwmgr);
+	return 0;
+}
+
+/**
+* Initializes the thermal controller related functions in the Hardware Manager structure.
+* @param    hwmgr The address of the hardware manager.
+* @exception Any error code from the low-level communication.
+*/
+int pp_fiji_thermal_initialize(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	result = phm_construct_table(hwmgr,
+			&fiji_thermal_set_temperature_range_master,
+			&(hwmgr->set_temperature_range));
+
+	if (!result) {
+		result = phm_construct_table(hwmgr,
+				&fiji_thermal_start_thermal_controller_master,
+				&(hwmgr->start_thermal_controller));
+		if (result)
+			phm_destroy_table(hwmgr, &(hwmgr->set_temperature_range));
+	}
+
+	if (!result)
+		hwmgr->fan_ctrl_is_in_default_mode = true;
+	return result;
+}
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.h b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.h
new file mode 100644
index 000000000000..8621493b8574
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/fiji_thermal.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef FIJI_THERMAL_H
+#define FIJI_THERMAL_H
+
+#include "hwmgr.h"
+
+#define FIJI_THERMAL_HIGH_ALERT_MASK         0x1
+#define FIJI_THERMAL_LOW_ALERT_MASK          0x2
+
+#define FIJI_THERMAL_MINIMUM_TEMP_READING    -256
+#define FIJI_THERMAL_MAXIMUM_TEMP_READING    255
+
+#define FIJI_THERMAL_MINIMUM_ALERT_TEMP      0
+#define FIJI_THERMAL_MAXIMUM_ALERT_TEMP      255
+
+#define FDO_PWM_MODE_STATIC  1
+#define FDO_PWM_MODE_STATIC_RPM 5
+
+
+extern int tf_fiji_thermal_initialize(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+extern int tf_fiji_thermal_set_temperature_range(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+extern int tf_fiji_thermal_enable_alert(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+
+extern int fiji_thermal_get_temperature(struct pp_hwmgr *hwmgr);
+extern int fiji_thermal_stop_thermal_controller(struct pp_hwmgr *hwmgr);
+extern int fiji_fan_ctrl_get_fan_speed_info(struct pp_hwmgr *hwmgr, struct phm_fan_speed_info *fan_speed_info);
+extern int fiji_fan_ctrl_get_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t *speed);
+extern int fiji_fan_ctrl_set_default_mode(struct pp_hwmgr *hwmgr);
+extern int fiji_fan_ctrl_set_static_mode(struct pp_hwmgr *hwmgr, uint32_t mode);
+extern int fiji_fan_ctrl_set_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t speed);
+extern int fiji_fan_ctrl_reset_fan_speed_to_default(struct pp_hwmgr *hwmgr);
+extern int pp_fiji_thermal_initialize(struct pp_hwmgr *hwmgr);
+extern int fiji_thermal_ctrl_uninitialize_thermal_controller(struct pp_hwmgr *hwmgr);
+extern int fiji_fan_ctrl_set_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t speed);
+extern int fiji_fan_ctrl_get_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t *speed);
+extern int fiji_fan_ctrl_stop_smc_fan_control(struct pp_hwmgr *hwmgr);
+extern uint32_t tonga_get_xclk(struct pp_hwmgr *hwmgr);
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/functiontables.c b/drivers/gpu/drm/amd/powerplay/hwmgr/functiontables.c
new file mode 100644
index 000000000000..9deadabbc81c
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/functiontables.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include "hwmgr.h"
+
+static int phm_run_table(struct pp_hwmgr *hwmgr,
+			 struct phm_runtime_table_header *rt_table,
+			 void *input,
+			 void *output,
+			 void *temp_storage)
+{
+	int result = 0;
+	phm_table_function *function;
+
+	for (function = rt_table->function_list; NULL != *function; function++) {
+		int tmp = (*function)(hwmgr, input, output, temp_storage, result);
+
+		if (tmp == PP_Result_TableImmediateExit)
+			break;
+		if (tmp) {
+			if (0 == result)
+				result = tmp;
+			if (rt_table->exit_error)
+				break;
+		}
+	}
+
+	return result;
+}
+
+int phm_dispatch_table(struct pp_hwmgr *hwmgr,
+		       struct phm_runtime_table_header *rt_table,
+		       void *input, void *output)
+{
+	int result = 0;
+	void *temp_storage = NULL;
+
+	if (hwmgr == NULL || rt_table == NULL || rt_table->function_list == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Parameter!\n");
+		return 0; /*temp return ture because some function not implement on some asic */
+	}
+
+	if (0 != rt_table->storage_size) {
+		temp_storage = kzalloc(rt_table->storage_size, GFP_KERNEL);
+		if (temp_storage == NULL) {
+			printk(KERN_ERR "[ powerplay ] Could not allocate table temporary storage\n");
+			return -ENOMEM;
+		}
+	}
+
+	result = phm_run_table(hwmgr, rt_table, input, output, temp_storage);
+
+	if (NULL != temp_storage)
+		kfree(temp_storage);
+
+	return result;
+}
+
+int phm_construct_table(struct pp_hwmgr *hwmgr,
+			struct phm_master_table_header *master_table,
+			struct phm_runtime_table_header *rt_table)
+{
+	uint32_t function_count = 0;
+	const struct phm_master_table_item *table_item;
+	uint32_t size;
+	phm_table_function *run_time_list;
+	phm_table_function *rtf;
+
+	if (hwmgr == NULL || master_table == NULL || rt_table == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Parameter!\n");
+		return -EINVAL;
+	}
+
+	for (table_item = master_table->master_list;
+		NULL != table_item->tableFunction; table_item++) {
+		if ((NULL == table_item->isFunctionNeededInRuntimeTable) ||
+		    (table_item->isFunctionNeededInRuntimeTable(hwmgr)))
+			function_count++;
+	}
+
+	size = (function_count + 1) * sizeof(phm_table_function);
+	run_time_list = kzalloc(size, GFP_KERNEL);
+
+	if (NULL == run_time_list)
+		return -ENOMEM;
+
+	rtf = run_time_list;
+	for (table_item = master_table->master_list;
+		NULL != table_item->tableFunction; table_item++) {
+		if ((rtf - run_time_list) > function_count) {
+			printk(KERN_ERR "[ powerplay ] Check function results have changed\n");
+			kfree(run_time_list);
+			return -EINVAL;
+		}
+
+		if ((NULL == table_item->isFunctionNeededInRuntimeTable) ||
+		     (table_item->isFunctionNeededInRuntimeTable(hwmgr))) {
+			*(rtf++) = table_item->tableFunction;
+		}
+	}
+
+	if ((rtf - run_time_list) > function_count) {
+		printk(KERN_ERR "[ powerplay ] Check function results have changed\n");
+		kfree(run_time_list);
+		return -EINVAL;
+	}
+
+	*rtf = NULL;
+	rt_table->function_list = run_time_list;
+	rt_table->exit_error = (0 != (master_table->flags & PHM_MasterTableFlag_ExitOnError));
+	rt_table->storage_size = master_table->storage_size;
+	return 0;
+}
+
+int phm_destroy_table(struct pp_hwmgr *hwmgr,
+		      struct phm_runtime_table_header *rt_table)
+{
+	if (hwmgr == NULL || rt_table == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Parameter\n");
+		return -EINVAL;
+	}
+
+	if (NULL == rt_table->function_list)
+		return 0;
+
+	kfree(rt_table->function_list);
+
+	rt_table->function_list = NULL;
+	rt_table->storage_size = 0;
+	rt_table->exit_error = false;
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/hardwaremanager.c b/drivers/gpu/drm/amd/powerplay/hwmgr/hardwaremanager.c
new file mode 100644
index 000000000000..0f2d5e4bc241
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/hardwaremanager.c
@@ -0,0 +1,334 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/errno.h>
+#include "hwmgr.h"
+#include "hardwaremanager.h"
+#include "power_state.h"
+#include "pp_acpi.h"
+#include "amd_acpi.h"
+#include "amd_powerplay.h"
+
+#define PHM_FUNC_CHECK(hw) \
+	do {							\
+		if ((hw) == NULL || (hw)->hwmgr_func == NULL)	\
+			return -EINVAL;				\
+	} while (0)
+
+void phm_init_dynamic_caps(struct pp_hwmgr *hwmgr)
+{
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableVoltageTransition);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableEngineTransition);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableMemoryTransition);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableMGClockGating);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableMGCGTSSM);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableLSClockGating);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_Force3DClockSupport);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableLightSleep);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableMCLS);
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisablePowerGating);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableDPM);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_DisableSMUUVDHandshake);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_ThermalAutoThrottling);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_PCIEPerformanceRequest);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_NoOD5Support);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_UserMaxClockForMultiDisplays);
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_VpuRecoveryInProgress);
+
+	if (acpi_atcs_functions_supported(hwmgr->device, ATCS_FUNCTION_PCIE_PERFORMANCE_REQUEST) &&
+		acpi_atcs_functions_supported(hwmgr->device, ATCS_FUNCTION_PCIE_DEVICE_READY_NOTIFICATION))
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_PCIEPerformanceRequest);
+}
+
+bool phm_is_hw_access_blocked(struct pp_hwmgr *hwmgr)
+{
+	return hwmgr->block_hw_access;
+}
+
+int phm_block_hw_access(struct pp_hwmgr *hwmgr, bool block)
+{
+	hwmgr->block_hw_access = block;
+	return 0;
+}
+
+int phm_setup_asic(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->asic_setup)
+			return hwmgr->hwmgr_func->asic_setup(hwmgr);
+	} else {
+		return phm_dispatch_table(hwmgr, &(hwmgr->setup_asic),
+					  NULL, NULL);
+	}
+
+	return 0;
+}
+
+int phm_power_down_asic(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->power_off_asic)
+			return hwmgr->hwmgr_func->power_off_asic(hwmgr);
+	} else {
+		return phm_dispatch_table(hwmgr, &(hwmgr->power_down_asic),
+					  NULL, NULL);
+	}
+
+	return 0;
+}
+
+int phm_set_power_state(struct pp_hwmgr *hwmgr,
+		    const struct pp_hw_power_state *pcurrent_state,
+		    const struct pp_hw_power_state *pnew_power_state)
+{
+	struct phm_set_power_state_input states;
+
+	PHM_FUNC_CHECK(hwmgr);
+
+	states.pcurrent_state = pcurrent_state;
+	states.pnew_state = pnew_power_state;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->power_state_set)
+			return hwmgr->hwmgr_func->power_state_set(hwmgr, &states);
+	} else {
+		return phm_dispatch_table(hwmgr, &(hwmgr->set_power_state), &states, NULL);
+	}
+
+	return 0;
+}
+
+int phm_enable_dynamic_state_management(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->dynamic_state_management_enable)
+			return hwmgr->hwmgr_func->dynamic_state_management_enable(hwmgr);
+	} else {
+		return phm_dispatch_table(hwmgr,
+				&(hwmgr->enable_dynamic_state_management),
+				NULL, NULL);
+	}
+	return 0;
+}
+
+int phm_force_dpm_levels(struct pp_hwmgr *hwmgr, enum amd_dpm_forced_level level)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->force_dpm_level != NULL)
+		return hwmgr->hwmgr_func->force_dpm_level(hwmgr, level);
+
+	return 0;
+}
+
+int phm_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
+				   struct pp_power_state *adjusted_ps,
+			     const struct pp_power_state *current_ps)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->apply_state_adjust_rules != NULL)
+		return hwmgr->hwmgr_func->apply_state_adjust_rules(
+									hwmgr,
+								 adjusted_ps,
+								 current_ps);
+	return 0;
+}
+
+int phm_powerdown_uvd(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->powerdown_uvd != NULL)
+		return hwmgr->hwmgr_func->powerdown_uvd(hwmgr);
+	return 0;
+}
+
+int phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool gate)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->powergate_uvd != NULL)
+		return hwmgr->hwmgr_func->powergate_uvd(hwmgr, gate);
+	return 0;
+}
+
+int phm_powergate_vce(struct pp_hwmgr *hwmgr, bool gate)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->powergate_vce != NULL)
+		return hwmgr->hwmgr_func->powergate_vce(hwmgr, gate);
+	return 0;
+}
+
+int phm_enable_clock_power_gatings(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->enable_clock_power_gating)
+			return hwmgr->hwmgr_func->enable_clock_power_gating(hwmgr);
+	} else {
+		return phm_dispatch_table(hwmgr, &(hwmgr->enable_clock_power_gatings), NULL, NULL);
+	}
+	return 0;
+}
+
+int phm_display_configuration_changed(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				 PHM_PlatformCaps_TablelessHardwareInterface)) {
+		if (NULL != hwmgr->hwmgr_func->display_config_changed)
+			hwmgr->hwmgr_func->display_config_changed(hwmgr);
+	} else
+		return phm_dispatch_table(hwmgr, &hwmgr->display_configuration_changed, NULL, NULL);
+	return 0;
+}
+
+int phm_notify_smc_display_config_after_ps_adjustment(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				 PHM_PlatformCaps_TablelessHardwareInterface))
+		if (NULL != hwmgr->hwmgr_func->notify_smc_display_config_after_ps_adjustment)
+			hwmgr->hwmgr_func->notify_smc_display_config_after_ps_adjustment(hwmgr);
+
+	return 0;
+}
+
+int phm_stop_thermal_controller(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->stop_thermal_controller == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->stop_thermal_controller(hwmgr);
+}
+
+int phm_register_thermal_interrupt(struct pp_hwmgr *hwmgr, const void *info)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->register_internal_thermal_interrupt == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->register_internal_thermal_interrupt(hwmgr, info);
+}
+
+/**
+* Initializes the thermal controller subsystem.
+*
+* @param    pHwMgr  the address of the powerplay hardware manager.
+* @param    pTemperatureRange the address of the structure holding the temperature range.
+* @exception PP_Result_Failed if any of the paramters is NULL, otherwise the return value from the dispatcher.
+*/
+int phm_start_thermal_controller(struct pp_hwmgr *hwmgr, struct PP_TemperatureRange *temperature_range)
+{
+	return phm_dispatch_table(hwmgr, &(hwmgr->start_thermal_controller), temperature_range, NULL);
+}
+
+
+bool phm_check_smc_update_required_for_display_configuration(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->check_smc_update_required_for_display_configuration == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->check_smc_update_required_for_display_configuration(hwmgr);
+}
+
+
+int phm_check_states_equal(struct pp_hwmgr *hwmgr,
+				 const struct pp_hw_power_state *pstate1,
+				 const struct pp_hw_power_state *pstate2,
+				 bool *equal)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->check_states_equal == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->check_states_equal(hwmgr, pstate1, pstate2, equal);
+}
+
+int phm_store_dal_configuration_data(struct pp_hwmgr *hwmgr,
+		    const struct amd_pp_display_configuration *display_config)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->store_cc6_data == NULL)
+		return -EINVAL;
+
+	hwmgr->display_config = *display_config;
+	/* to do pass other display configuration in furture */
+
+	if (hwmgr->hwmgr_func->store_cc6_data)
+		hwmgr->hwmgr_func->store_cc6_data(hwmgr,
+				display_config->cpu_pstate_separation_time,
+				display_config->cpu_cc6_disable,
+				display_config->cpu_pstate_disable,
+				display_config->nb_pstate_switch_disable);
+
+	return 0;
+}
+
+int phm_get_dal_power_level(struct pp_hwmgr *hwmgr,
+		struct amd_pp_dal_clock_info *info)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (info == NULL || hwmgr->hwmgr_func->get_dal_power_level == NULL)
+		return -EINVAL;
+
+	return hwmgr->hwmgr_func->get_dal_power_level(hwmgr, info);
+}
+
+int phm_set_cpu_power_state(struct pp_hwmgr *hwmgr)
+{
+	PHM_FUNC_CHECK(hwmgr);
+
+	if (hwmgr->hwmgr_func->set_cpu_power_state != NULL)
+		return hwmgr->hwmgr_func->set_cpu_power_state(hwmgr);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr.c
new file mode 100644
index 000000000000..5fb98aa2e719
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr.c
@@ -0,0 +1,563 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include "linux/delay.h"
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include "cgs_common.h"
+#include "power_state.h"
+#include "hwmgr.h"
+#include "pppcielanes.h"
+#include "pp_debug.h"
+#include "ppatomctrl.h"
+
+extern int cz_hwmgr_init(struct pp_hwmgr *hwmgr);
+extern int tonga_hwmgr_init(struct pp_hwmgr *hwmgr);
+extern int fiji_hwmgr_init(struct pp_hwmgr *hwmgr);
+
+int hwmgr_init(struct amd_pp_init *pp_init, struct pp_instance *handle)
+{
+	struct pp_hwmgr *hwmgr;
+
+	if ((handle == NULL) || (pp_init == NULL))
+		return -EINVAL;
+
+	hwmgr = kzalloc(sizeof(struct pp_hwmgr), GFP_KERNEL);
+	if (hwmgr == NULL)
+		return -ENOMEM;
+
+	handle->hwmgr = hwmgr;
+	hwmgr->smumgr = handle->smu_mgr;
+	hwmgr->device = pp_init->device;
+	hwmgr->chip_family = pp_init->chip_family;
+	hwmgr->chip_id = pp_init->chip_id;
+	hwmgr->hw_revision = pp_init->rev_id;
+	hwmgr->usec_timeout = AMD_MAX_USEC_TIMEOUT;
+	hwmgr->power_source = PP_PowerSource_AC;
+
+	switch (hwmgr->chip_family) {
+	case AMD_FAMILY_CZ:
+		cz_hwmgr_init(hwmgr);
+		break;
+	case AMD_FAMILY_VI:
+		switch (hwmgr->chip_id) {
+		case CHIP_TONGA:
+			tonga_hwmgr_init(hwmgr);
+			break;
+		case CHIP_FIJI:
+			fiji_hwmgr_init(hwmgr);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	phm_init_dynamic_caps(hwmgr);
+
+	return 0;
+}
+
+int hwmgr_fini(struct pp_hwmgr *hwmgr)
+{
+	if (hwmgr == NULL || hwmgr->ps == NULL)
+		return -EINVAL;
+
+	kfree(hwmgr->ps);
+	kfree(hwmgr);
+	return 0;
+}
+
+int hw_init_power_state_table(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	unsigned int i;
+	unsigned int table_entries;
+	struct pp_power_state *state;
+	int size;
+
+	if (hwmgr->hwmgr_func->get_num_of_pp_table_entries == NULL)
+		return -EINVAL;
+
+	if (hwmgr->hwmgr_func->get_power_state_size == NULL)
+		return -EINVAL;
+
+	hwmgr->num_ps = table_entries = hwmgr->hwmgr_func->get_num_of_pp_table_entries(hwmgr);
+
+	hwmgr->ps_size = size = hwmgr->hwmgr_func->get_power_state_size(hwmgr) +
+					  sizeof(struct pp_power_state);
+
+	hwmgr->ps = kzalloc(size * table_entries, GFP_KERNEL);
+
+	if (hwmgr->ps == NULL)
+		return -ENOMEM;
+
+	state = hwmgr->ps;
+
+	for (i = 0; i < table_entries; i++) {
+		result = hwmgr->hwmgr_func->get_pp_table_entry(hwmgr, i, state);
+
+		if (state->classification.flags & PP_StateClassificationFlag_Boot) {
+			hwmgr->boot_ps = state;
+			hwmgr->current_ps = hwmgr->request_ps = state;
+		}
+
+		state->id = i + 1; /* assigned unique num for every power state id */
+
+		if (state->classification.flags & PP_StateClassificationFlag_Uvd)
+			hwmgr->uvd_ps = state;
+		state = (struct pp_power_state *)((unsigned long)state + size);
+	}
+
+	return 0;
+}
+
+
+/**
+ * Returns once the part of the register indicated by the mask has
+ * reached the given value.
+ */
+int phm_wait_on_register(struct pp_hwmgr *hwmgr, uint32_t index,
+			 uint32_t value, uint32_t mask)
+{
+	uint32_t i;
+	uint32_t cur_value;
+
+	if (hwmgr == NULL || hwmgr->device == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Hardware Manager!");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < hwmgr->usec_timeout; i++) {
+		cur_value = cgs_read_register(hwmgr->device, index);
+		if ((cur_value & mask) == (value & mask))
+			break;
+		udelay(1);
+	}
+
+	/* timeout means wrong logic*/
+	if (i == hwmgr->usec_timeout)
+		return -1;
+	return 0;
+}
+
+int phm_wait_for_register_unequal(struct pp_hwmgr *hwmgr,
+				uint32_t index, uint32_t value, uint32_t mask)
+{
+	uint32_t i;
+	uint32_t cur_value;
+
+	if (hwmgr == NULL || hwmgr->device == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Hardware Manager!");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < hwmgr->usec_timeout; i++) {
+		cur_value = cgs_read_register(hwmgr->device, index);
+		if ((cur_value & mask) != (value & mask))
+			break;
+		udelay(1);
+	}
+
+	/* timeout means wrong logic*/
+	if (i == hwmgr->usec_timeout)
+		return -1;
+	return 0;
+}
+
+
+/**
+ * Returns once the part of the register indicated by the mask has
+ * reached the given value.The indirect space is described by giving
+ * the memory-mapped index of the indirect index register.
+ */
+void phm_wait_on_indirect_register(struct pp_hwmgr *hwmgr,
+				uint32_t indirect_port,
+				uint32_t index,
+				uint32_t value,
+				uint32_t mask)
+{
+	if (hwmgr == NULL || hwmgr->device == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Hardware Manager!");
+		return;
+	}
+
+	cgs_write_register(hwmgr->device, indirect_port, index);
+	phm_wait_on_register(hwmgr, indirect_port + 1, mask, value);
+}
+
+void phm_wait_for_indirect_register_unequal(struct pp_hwmgr *hwmgr,
+					uint32_t indirect_port,
+					uint32_t index,
+					uint32_t value,
+					uint32_t mask)
+{
+	if (hwmgr == NULL || hwmgr->device == NULL) {
+		printk(KERN_ERR "[ powerplay ] Invalid Hardware Manager!");
+		return;
+	}
+
+	cgs_write_register(hwmgr->device, indirect_port, index);
+	phm_wait_for_register_unequal(hwmgr, indirect_port + 1,
+				      value, mask);
+}
+
+bool phm_cf_want_uvd_power_gating(struct pp_hwmgr *hwmgr)
+{
+	return phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_UVDPowerGating);
+}
+
+bool phm_cf_want_vce_power_gating(struct pp_hwmgr *hwmgr)
+{
+	return phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_VCEPowerGating);
+}
+
+
+int phm_trim_voltage_table(struct pp_atomctrl_voltage_table *vol_table)
+{
+	uint32_t i, j;
+	uint16_t vvalue;
+	bool found = false;
+	struct pp_atomctrl_voltage_table *table;
+
+	PP_ASSERT_WITH_CODE((NULL != vol_table),
+			"Voltage Table empty.", return -EINVAL);
+
+	table = kzalloc(sizeof(struct pp_atomctrl_voltage_table),
+			GFP_KERNEL);
+
+	if (NULL == table)
+		return -EINVAL;
+
+	table->mask_low = vol_table->mask_low;
+	table->phase_delay = vol_table->phase_delay;
+
+	for (i = 0; i < vol_table->count; i++) {
+		vvalue = vol_table->entries[i].value;
+		found = false;
+
+		for (j = 0; j < table->count; j++) {
+			if (vvalue == table->entries[j].value) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			table->entries[table->count].value = vvalue;
+			table->entries[table->count].smio_low =
+					vol_table->entries[i].smio_low;
+			table->count++;
+		}
+	}
+
+	memcpy(vol_table, table, sizeof(struct pp_atomctrl_voltage_table));
+	kfree(table);
+
+	return 0;
+}
+
+int phm_get_svi2_mvdd_voltage_table(struct pp_atomctrl_voltage_table *vol_table,
+		phm_ppt_v1_clock_voltage_dependency_table *dep_table)
+{
+	uint32_t i;
+	int result;
+
+	PP_ASSERT_WITH_CODE((0 != dep_table->count),
+			"Voltage Dependency Table empty.", return -EINVAL);
+
+	PP_ASSERT_WITH_CODE((NULL != vol_table),
+			"vol_table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+	vol_table->count = dep_table->count;
+
+	for (i = 0; i < dep_table->count; i++) {
+		vol_table->entries[i].value = dep_table->entries[i].mvdd;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	result = phm_trim_voltage_table(vol_table);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to trim MVDD table.", return result);
+
+	return 0;
+}
+
+int phm_get_svi2_vddci_voltage_table(struct pp_atomctrl_voltage_table *vol_table,
+		phm_ppt_v1_clock_voltage_dependency_table *dep_table)
+{
+	uint32_t i;
+	int result;
+
+	PP_ASSERT_WITH_CODE((0 != dep_table->count),
+			"Voltage Dependency Table empty.", return -EINVAL);
+
+	PP_ASSERT_WITH_CODE((NULL != vol_table),
+			"vol_table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+	vol_table->count = dep_table->count;
+
+	for (i = 0; i < dep_table->count; i++) {
+		vol_table->entries[i].value = dep_table->entries[i].vddci;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	result = phm_trim_voltage_table(vol_table);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to trim VDDCI table.", return result);
+
+	return 0;
+}
+
+int phm_get_svi2_vdd_voltage_table(struct pp_atomctrl_voltage_table *vol_table,
+		phm_ppt_v1_voltage_lookup_table *lookup_table)
+{
+	int i = 0;
+
+	PP_ASSERT_WITH_CODE((0 != lookup_table->count),
+			"Voltage Lookup Table empty.", return -EINVAL);
+
+	PP_ASSERT_WITH_CODE((NULL != vol_table),
+			"vol_table empty.", return -EINVAL);
+
+	vol_table->mask_low = 0;
+	vol_table->phase_delay = 0;
+
+	vol_table->count = lookup_table->count;
+
+	for (i = 0; i < vol_table->count; i++) {
+		vol_table->entries[i].value = lookup_table->entries[i].us_vdd;
+		vol_table->entries[i].smio_low = 0;
+	}
+
+	return 0;
+}
+
+void phm_trim_voltage_table_to_fit_state_table(uint32_t max_vol_steps,
+				struct pp_atomctrl_voltage_table *vol_table)
+{
+	unsigned int i, diff;
+
+	if (vol_table->count <= max_vol_steps)
+		return;
+
+	diff = vol_table->count - max_vol_steps;
+
+	for (i = 0; i < max_vol_steps; i++)
+		vol_table->entries[i] = vol_table->entries[i + diff];
+
+	vol_table->count = max_vol_steps;
+
+	return;
+}
+
+int phm_reset_single_dpm_table(void *table,
+				uint32_t count, int max)
+{
+	int i;
+
+	struct vi_dpm_table *dpm_table = (struct vi_dpm_table *)table;
+
+	PP_ASSERT_WITH_CODE(count <= max,
+			"Fatal error, can not set up single DPM table entries to exceed max number!",
+			   );
+
+	dpm_table->count = count;
+	for (i = 0; i < max; i++)
+		dpm_table->dpm_level[i].enabled = false;
+
+	return 0;
+}
+
+void phm_setup_pcie_table_entry(
+	void *table,
+	uint32_t index, uint32_t pcie_gen,
+	uint32_t pcie_lanes)
+{
+	struct vi_dpm_table *dpm_table = (struct vi_dpm_table *)table;
+	dpm_table->dpm_level[index].value = pcie_gen;
+	dpm_table->dpm_level[index].param1 = pcie_lanes;
+	dpm_table->dpm_level[index].enabled = 1;
+}
+
+int32_t phm_get_dpm_level_enable_mask_value(void *table)
+{
+	int32_t i;
+	int32_t mask = 0;
+	struct vi_dpm_table *dpm_table = (struct vi_dpm_table *)table;
+
+	for (i = dpm_table->count; i > 0; i--) {
+		mask = mask << 1;
+		if (dpm_table->dpm_level[i - 1].enabled)
+			mask |= 0x1;
+		else
+			mask &= 0xFFFFFFFE;
+	}
+
+	return mask;
+}
+
+uint8_t phm_get_voltage_index(
+		struct phm_ppt_v1_voltage_lookup_table *lookup_table, uint16_t voltage)
+{
+	uint8_t count = (uint8_t) (lookup_table->count);
+	uint8_t i;
+
+	PP_ASSERT_WITH_CODE((NULL != lookup_table),
+			"Lookup Table empty.", return 0);
+	PP_ASSERT_WITH_CODE((0 != count),
+			"Lookup Table empty.", return 0);
+
+	for (i = 0; i < lookup_table->count; i++) {
+		/* find first voltage equal or bigger than requested */
+		if (lookup_table->entries[i].us_vdd >= voltage)
+			return i;
+	}
+	/* voltage is bigger than max voltage in the table */
+	return i - 1;
+}
+
+uint16_t phm_find_closest_vddci(struct pp_atomctrl_voltage_table *vddci_table, uint16_t vddci)
+{
+	uint32_t  i;
+
+	for (i = 0; i < vddci_table->count; i++) {
+		if (vddci_table->entries[i].value >= vddci)
+			return vddci_table->entries[i].value;
+	}
+
+	PP_ASSERT_WITH_CODE(false,
+			"VDDCI is larger than max VDDCI in VDDCI Voltage Table!",
+			return vddci_table->entries[i].value);
+}
+
+int phm_find_boot_level(void *table,
+		uint32_t value, uint32_t *boot_level)
+{
+	int result = -EINVAL;
+	uint32_t i;
+	struct vi_dpm_table *dpm_table = (struct vi_dpm_table *)table;
+
+	for (i = 0; i < dpm_table->count; i++) {
+		if (value == dpm_table->dpm_level[i].value) {
+			*boot_level = i;
+			result = 0;
+		}
+	}
+
+	return result;
+}
+
+int phm_get_sclk_for_voltage_evv(struct pp_hwmgr *hwmgr,
+	phm_ppt_v1_voltage_lookup_table *lookup_table,
+	uint16_t virtual_voltage_id, int32_t *sclk)
+{
+	uint8_t entryId;
+	uint8_t voltageId;
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	PP_ASSERT_WITH_CODE(lookup_table->count != 0, "Lookup table is empty", return -EINVAL);
+
+	/* search for leakage voltage ID 0xff01 ~ 0xff08 and sckl */
+	for (entryId = 0; entryId < table_info->vdd_dep_on_sclk->count; entryId++) {
+		voltageId = table_info->vdd_dep_on_sclk->entries[entryId].vddInd;
+		if (lookup_table->entries[voltageId].us_vdd == virtual_voltage_id)
+			break;
+	}
+
+	PP_ASSERT_WITH_CODE(entryId < table_info->vdd_dep_on_sclk->count,
+			"Can't find requested voltage id in vdd_dep_on_sclk table!",
+			return -EINVAL;
+			);
+
+	*sclk = table_info->vdd_dep_on_sclk->entries[entryId].clk;
+
+	return 0;
+}
+
+/**
+ * Initialize Dynamic State Adjustment Rule Settings
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ */
+int phm_initializa_dynamic_state_adjustment_rule_settings(struct pp_hwmgr *hwmgr)
+{
+	uint32_t table_size;
+	struct phm_clock_voltage_dependency_table *table_clk_vlt;
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	/* initialize vddc_dep_on_dal_pwrl table */
+	table_size = sizeof(uint32_t) + 4 * sizeof(struct phm_clock_voltage_dependency_record);
+	table_clk_vlt = (struct phm_clock_voltage_dependency_table *)kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table_clk_vlt) {
+		printk(KERN_ERR "[ powerplay ] Can not allocate space for vddc_dep_on_dal_pwrl! \n");
+		return -ENOMEM;
+	} else {
+		table_clk_vlt->count = 4;
+		table_clk_vlt->entries[0].clk = PP_DAL_POWERLEVEL_ULTRALOW;
+		table_clk_vlt->entries[0].v = 0;
+		table_clk_vlt->entries[1].clk = PP_DAL_POWERLEVEL_LOW;
+		table_clk_vlt->entries[1].v = 720;
+		table_clk_vlt->entries[2].clk = PP_DAL_POWERLEVEL_NOMINAL;
+		table_clk_vlt->entries[2].v = 810;
+		table_clk_vlt->entries[3].clk = PP_DAL_POWERLEVEL_PERFORMANCE;
+		table_clk_vlt->entries[3].v = 900;
+		pptable_info->vddc_dep_on_dal_pwrl = table_clk_vlt;
+		hwmgr->dyn_state.vddc_dep_on_dal_pwrl = table_clk_vlt;
+	}
+
+	return 0;
+}
+
+int phm_hwmgr_backend_fini(struct pp_hwmgr *hwmgr)
+{
+	if (NULL != hwmgr->dyn_state.vddc_dep_on_dal_pwrl) {
+		kfree(hwmgr->dyn_state.vddc_dep_on_dal_pwrl);
+		hwmgr->dyn_state.vddc_dep_on_dal_pwrl = NULL;
+	}
+
+	if (NULL != hwmgr->backend) {
+		kfree(hwmgr->backend);
+		hwmgr->backend = NULL;
+	}
+
+	return 0;
+}
+
+uint32_t phm_get_lowest_enabled_level(struct pp_hwmgr *hwmgr, uint32_t mask)
+{
+	uint32_t level = 0;
+
+	while (0 == (mask & (1 << level)))
+		level++;
+
+	return level;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr_ppt.h b/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr_ppt.h
new file mode 100644
index 000000000000..c9e6c2d80ea6
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/hwmgr_ppt.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef PP_HWMGR_PPT_H
+#define PP_HWMGR_PPT_H
+
+#include "hardwaremanager.h"
+#include "smumgr.h"
+#include "atom-types.h"
+
+struct phm_ppt_v1_clock_voltage_dependency_record {
+	uint32_t clk;
+	uint8_t vddInd;
+	uint16_t vdd_offset;
+	uint16_t vddc;
+	uint16_t vddgfx;
+	uint16_t vddci;
+	uint16_t mvdd;
+	uint8_t phases;
+	uint8_t cks_enable;
+	uint8_t cks_voffset;
+};
+
+typedef struct phm_ppt_v1_clock_voltage_dependency_record phm_ppt_v1_clock_voltage_dependency_record;
+
+struct phm_ppt_v1_clock_voltage_dependency_table {
+	uint32_t count;                                            /* Number of entries. */
+	phm_ppt_v1_clock_voltage_dependency_record entries[1];     /* Dynamically allocate count entries. */
+};
+
+typedef struct phm_ppt_v1_clock_voltage_dependency_table phm_ppt_v1_clock_voltage_dependency_table;
+
+
+/* Multimedia Clock Voltage Dependency records and table */
+struct phm_ppt_v1_mm_clock_voltage_dependency_record {
+	uint32_t  dclk;                                              /* UVD D-clock */
+	uint32_t  vclk;                                              /* UVD V-clock */
+	uint32_t  eclk;                                              /* VCE clock */
+	uint32_t  aclk;                                              /* ACP clock */
+	uint32_t  samclock;                                          /* SAMU clock */
+	uint8_t	vddcInd;
+	uint16_t vddgfx_offset;
+	uint16_t vddc;
+	uint16_t vddgfx;
+	uint8_t phases;
+};
+typedef struct phm_ppt_v1_mm_clock_voltage_dependency_record phm_ppt_v1_mm_clock_voltage_dependency_record;
+
+struct phm_ppt_v1_mm_clock_voltage_dependency_table {
+	uint32_t count;													/* Number of entries. */
+	phm_ppt_v1_mm_clock_voltage_dependency_record entries[1];		/* Dynamically allocate count entries. */
+};
+typedef struct phm_ppt_v1_mm_clock_voltage_dependency_table phm_ppt_v1_mm_clock_voltage_dependency_table;
+
+struct phm_ppt_v1_voltage_lookup_record {
+	uint16_t us_calculated;
+	uint16_t us_vdd;												/* Base voltage */
+	uint16_t us_cac_low;
+	uint16_t us_cac_mid;
+	uint16_t us_cac_high;
+};
+typedef struct phm_ppt_v1_voltage_lookup_record phm_ppt_v1_voltage_lookup_record;
+
+struct phm_ppt_v1_voltage_lookup_table {
+	uint32_t count;
+	phm_ppt_v1_voltage_lookup_record entries[1];    /* Dynamically allocate count entries. */
+};
+typedef struct phm_ppt_v1_voltage_lookup_table phm_ppt_v1_voltage_lookup_table;
+
+/* PCIE records and Table */
+
+struct phm_ppt_v1_pcie_record {
+	uint8_t gen_speed;
+	uint8_t lane_width;
+};
+typedef struct phm_ppt_v1_pcie_record phm_ppt_v1_pcie_record;
+
+struct phm_ppt_v1_pcie_table {
+	uint32_t count;                                            /* Number of entries. */
+	phm_ppt_v1_pcie_record entries[1];                         /* Dynamically allocate count entries. */
+};
+typedef struct phm_ppt_v1_pcie_table phm_ppt_v1_pcie_table;
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/pp_acpi.c b/drivers/gpu/drm/amd/powerplay/hwmgr/pp_acpi.c
new file mode 100644
index 000000000000..7b2d5000292d
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/pp_acpi.c
@@ -0,0 +1,76 @@
+#include <linux/errno.h>
+#include "linux/delay.h"
+#include "hwmgr.h"
+#include "amd_acpi.h"
+
+bool acpi_atcs_functions_supported(void *device, uint32_t index)
+{
+	int32_t result;
+	struct atcs_verify_interface output_buf = {0};
+
+	int32_t temp_buffer = 1;
+
+	result = cgs_call_acpi_method(device, CGS_ACPI_METHOD_ATCS,
+						ATCS_FUNCTION_VERIFY_INTERFACE,
+						&temp_buffer,
+						&output_buf,
+						1,
+						sizeof(temp_buffer),
+						sizeof(output_buf));
+
+	return result == 0 ? (output_buf.function_bits & (1 << (index - 1))) != 0 : false;
+}
+
+int acpi_pcie_perf_request(void *device, uint8_t perf_req, bool advertise)
+{
+	struct atcs_pref_req_input atcs_input;
+	struct atcs_pref_req_output atcs_output;
+	u32 retry = 3;
+	int result;
+	struct cgs_system_info info = {0};
+
+	if (!acpi_atcs_functions_supported(device, ATCS_FUNCTION_PCIE_PERFORMANCE_REQUEST))
+		return -EINVAL;
+
+	info.size = sizeof(struct cgs_system_info);
+	info.info_id = CGS_SYSTEM_INFO_ADAPTER_BDF_ID;
+	result = cgs_query_system_info(device, &info);
+	if (result != 0)
+		return -EINVAL;
+	atcs_input.client_id = (uint16_t)info.value;
+	atcs_input.size = sizeof(struct atcs_pref_req_input);
+	atcs_input.valid_flags_mask = ATCS_VALID_FLAGS_MASK;
+	atcs_input.flags = ATCS_WAIT_FOR_COMPLETION;
+	if (advertise)
+		atcs_input.flags |= ATCS_ADVERTISE_CAPS;
+	atcs_input.req_type = ATCS_PCIE_LINK_SPEED;
+	atcs_input.perf_req = perf_req;
+
+	atcs_output.size = sizeof(struct atcs_pref_req_input);
+
+	while (retry--) {
+		result = cgs_call_acpi_method(device,
+						CGS_ACPI_METHOD_ATCS,
+						ATCS_FUNCTION_PCIE_PERFORMANCE_REQUEST,
+						&atcs_input,
+						&atcs_output,
+						0,
+						sizeof(atcs_input),
+						sizeof(atcs_output));
+		if (result != 0)
+			return -EIO;
+
+		switch (atcs_output.ret_val) {
+		case ATCS_REQUEST_REFUSED:
+		default:
+			return -EINVAL;
+		case ATCS_REQUEST_COMPLETE:
+			return 0;
+		case ATCS_REQUEST_IN_PROGRESS:
+			udelay(10);
+			break;
+		}
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.c b/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.c
new file mode 100644
index 000000000000..2a83a4af2904
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.c
@@ -0,0 +1,1207 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+
+#include "ppatomctrl.h"
+#include "atombios.h"
+#include "cgs_common.h"
+#include "pp_debug.h"
+#include "ppevvmath.h"
+
+#define MEM_ID_MASK           0xff000000
+#define MEM_ID_SHIFT          24
+#define CLOCK_RANGE_MASK      0x00ffffff
+#define CLOCK_RANGE_SHIFT     0
+#define LOW_NIBBLE_MASK       0xf
+#define DATA_EQU_PREV         0
+#define DATA_FROM_TABLE       4
+
+union voltage_object_info {
+	struct _ATOM_VOLTAGE_OBJECT_INFO v1;
+	struct _ATOM_VOLTAGE_OBJECT_INFO_V2 v2;
+	struct _ATOM_VOLTAGE_OBJECT_INFO_V3_1 v3;
+};
+
+static int atomctrl_retrieve_ac_timing(
+		uint8_t index,
+		ATOM_INIT_REG_BLOCK *reg_block,
+		pp_atomctrl_mc_reg_table *table)
+{
+	uint32_t i, j;
+	uint8_t tmem_id;
+	ATOM_MEMORY_SETTING_DATA_BLOCK *reg_data = (ATOM_MEMORY_SETTING_DATA_BLOCK *)
+		((uint8_t *)reg_block + (2 * sizeof(uint16_t)) + le16_to_cpu(reg_block->usRegIndexTblSize));
+
+	uint8_t num_ranges = 0;
+
+	while (*(uint32_t *)reg_data != END_OF_REG_DATA_BLOCK &&
+			num_ranges < VBIOS_MAX_AC_TIMING_ENTRIES) {
+		tmem_id = (uint8_t)((*(uint32_t *)reg_data & MEM_ID_MASK) >> MEM_ID_SHIFT);
+
+		if (index == tmem_id) {
+			table->mc_reg_table_entry[num_ranges].mclk_max =
+				(uint32_t)((*(uint32_t *)reg_data & CLOCK_RANGE_MASK) >>
+						CLOCK_RANGE_SHIFT);
+
+			for (i = 0, j = 1; i < table->last; i++) {
+				if ((table->mc_reg_address[i].uc_pre_reg_data &
+							LOW_NIBBLE_MASK) == DATA_FROM_TABLE) {
+					table->mc_reg_table_entry[num_ranges].mc_data[i] =
+						(uint32_t)*((uint32_t *)reg_data + j);
+					j++;
+				} else if ((table->mc_reg_address[i].uc_pre_reg_data &
+							LOW_NIBBLE_MASK) == DATA_EQU_PREV) {
+					table->mc_reg_table_entry[num_ranges].mc_data[i] =
+						table->mc_reg_table_entry[num_ranges].mc_data[i-1];
+				}
+			}
+			num_ranges++;
+		}
+
+		reg_data = (ATOM_MEMORY_SETTING_DATA_BLOCK *)
+			((uint8_t *)reg_data + le16_to_cpu(reg_block->usRegDataBlkSize)) ;
+	}
+
+	PP_ASSERT_WITH_CODE((*(uint32_t *)reg_data == END_OF_REG_DATA_BLOCK),
+			"Invalid VramInfo table.", return -1);
+	table->num_entries = num_ranges;
+
+	return 0;
+}
+
+/**
+ * Get memory clock AC timing registers index from VBIOS table
+ * VBIOS set end of memory clock AC timing registers by ucPreRegDataLength bit6 = 1
+ * @param    reg_block the address ATOM_INIT_REG_BLOCK
+ * @param    table the address of MCRegTable
+ * @return   0
+ */
+static int atomctrl_set_mc_reg_address_table(
+		ATOM_INIT_REG_BLOCK *reg_block,
+		pp_atomctrl_mc_reg_table *table)
+{
+	uint8_t i = 0;
+	uint8_t num_entries = (uint8_t)((le16_to_cpu(reg_block->usRegIndexTblSize))
+			/ sizeof(ATOM_INIT_REG_INDEX_FORMAT));
+	ATOM_INIT_REG_INDEX_FORMAT *format = &reg_block->asRegIndexBuf[0];
+
+	num_entries--;        /* subtract 1 data end mark entry */
+
+	PP_ASSERT_WITH_CODE((num_entries <= VBIOS_MC_REGISTER_ARRAY_SIZE),
+			"Invalid VramInfo table.", return -1);
+
+	/* ucPreRegDataLength bit6 = 1 is the end of memory clock AC timing registers */
+	while ((!(format->ucPreRegDataLength & ACCESS_PLACEHOLDER)) &&
+			(i < num_entries)) {
+		table->mc_reg_address[i].s1 =
+			(uint16_t)(le16_to_cpu(format->usRegIndex));
+		table->mc_reg_address[i].uc_pre_reg_data =
+			format->ucPreRegDataLength;
+
+		i++;
+		format = (ATOM_INIT_REG_INDEX_FORMAT *)
+			((uint8_t *)format + sizeof(ATOM_INIT_REG_INDEX_FORMAT));
+	}
+
+	table->last = i;
+	return 0;
+}
+
+
+int atomctrl_initialize_mc_reg_table(
+		struct pp_hwmgr *hwmgr,
+		uint8_t module_index,
+		pp_atomctrl_mc_reg_table *table)
+{
+	ATOM_VRAM_INFO_HEADER_V2_1 *vram_info;
+	ATOM_INIT_REG_BLOCK *reg_block;
+	int result = 0;
+	u8 frev, crev;
+	u16 size;
+
+	vram_info = (ATOM_VRAM_INFO_HEADER_V2_1 *)
+		cgs_atom_get_data_table(hwmgr->device,
+				GetIndexIntoMasterTable(DATA, VRAM_Info), &size, &frev, &crev);
+
+	if (module_index >= vram_info->ucNumOfVRAMModule) {
+		printk(KERN_ERR "[ powerplay ] Invalid VramInfo table.");
+		result = -1;
+	} else if (vram_info->sHeader.ucTableFormatRevision < 2) {
+		printk(KERN_ERR "[ powerplay ] Invalid VramInfo table.");
+		result = -1;
+	}
+
+	if (0 == result) {
+		reg_block = (ATOM_INIT_REG_BLOCK *)
+			((uint8_t *)vram_info + le16_to_cpu(vram_info->usMemClkPatchTblOffset));
+		result = atomctrl_set_mc_reg_address_table(reg_block, table);
+	}
+
+	if (0 == result) {
+		result = atomctrl_retrieve_ac_timing(module_index,
+					reg_block, table);
+	}
+
+	return result;
+}
+
+/**
+ * Set DRAM timings based on engine clock and memory clock.
+ */
+int atomctrl_set_engine_dram_timings_rv770(
+		struct pp_hwmgr *hwmgr,
+		uint32_t engine_clock,
+		uint32_t memory_clock)
+{
+	SET_ENGINE_CLOCK_PS_ALLOCATION engine_clock_parameters;
+
+	/* They are both in 10KHz Units. */
+	engine_clock_parameters.ulTargetEngineClock =
+		(uint32_t) engine_clock & SET_CLOCK_FREQ_MASK;
+	engine_clock_parameters.ulTargetEngineClock |=
+		(COMPUTE_ENGINE_PLL_PARAM << 24);
+
+	/* in 10 khz units.*/
+	engine_clock_parameters.sReserved.ulClock =
+		(uint32_t) memory_clock & SET_CLOCK_FREQ_MASK;
+	return cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, DynamicMemorySettings),
+			&engine_clock_parameters);
+}
+
+/**
+ * Private Function to get the PowerPlay Table Address.
+ * WARNING: The tabled returned by this function is in
+ * dynamically allocated memory.
+ * The caller has to release if by calling kfree.
+ */
+static ATOM_VOLTAGE_OBJECT_INFO *get_voltage_info_table(void *device)
+{
+	int index = GetIndexIntoMasterTable(DATA, VoltageObjectInfo);
+	u8 frev, crev;
+	u16 size;
+	union voltage_object_info *voltage_info;
+
+	voltage_info = (union voltage_object_info *)
+		cgs_atom_get_data_table(device, index,
+			&size, &frev, &crev);
+
+	if (voltage_info != NULL)
+		return (ATOM_VOLTAGE_OBJECT_INFO *) &(voltage_info->v3);
+	else
+		return NULL;
+}
+
+static const ATOM_VOLTAGE_OBJECT_V3 *atomctrl_lookup_voltage_type_v3(
+		const ATOM_VOLTAGE_OBJECT_INFO_V3_1 * voltage_object_info_table,
+		uint8_t voltage_type, uint8_t voltage_mode)
+{
+	unsigned int size = le16_to_cpu(voltage_object_info_table->sHeader.usStructureSize);
+	unsigned int offset = offsetof(ATOM_VOLTAGE_OBJECT_INFO_V3_1, asVoltageObj[0]);
+	uint8_t *start = (uint8_t *)voltage_object_info_table;
+
+	while (offset < size) {
+		const ATOM_VOLTAGE_OBJECT_V3 *voltage_object =
+			(const ATOM_VOLTAGE_OBJECT_V3 *)(start + offset);
+
+		if (voltage_type == voltage_object->asGpioVoltageObj.sHeader.ucVoltageType &&
+			voltage_mode == voltage_object->asGpioVoltageObj.sHeader.ucVoltageMode)
+			return voltage_object;
+
+		offset += le16_to_cpu(voltage_object->asGpioVoltageObj.sHeader.usSize);
+	}
+
+	return NULL;
+}
+
+/** atomctrl_get_memory_pll_dividers_si().
+ *
+ * @param hwmgr                 input parameter: pointer to HwMgr
+ * @param clock_value             input parameter: memory clock
+ * @param dividers                 output parameter: memory PLL dividers
+ * @param strobe_mode            input parameter: 1 for strobe mode,  0 for performance mode
+ */
+int atomctrl_get_memory_pll_dividers_si(
+		struct pp_hwmgr *hwmgr,
+		uint32_t clock_value,
+		pp_atomctrl_memory_clock_param *mpll_param,
+		bool strobe_mode)
+{
+	COMPUTE_MEMORY_CLOCK_PARAM_PARAMETERS_V2_1 mpll_parameters;
+	int result;
+
+	mpll_parameters.ulClock = (uint32_t) clock_value;
+	mpll_parameters.ucInputFlag = (uint8_t)((strobe_mode) ? 1 : 0);
+
+	result = cgs_atom_exec_cmd_table
+		(hwmgr->device,
+		 GetIndexIntoMasterTable(COMMAND, ComputeMemoryClockParam),
+		 &mpll_parameters);
+
+	if (0 == result) {
+		mpll_param->mpll_fb_divider.clk_frac =
+			mpll_parameters.ulFbDiv.usFbDivFrac;
+		mpll_param->mpll_fb_divider.cl_kf =
+			mpll_parameters.ulFbDiv.usFbDiv;
+		mpll_param->mpll_post_divider =
+			(uint32_t)mpll_parameters.ucPostDiv;
+		mpll_param->vco_mode =
+			(uint32_t)(mpll_parameters.ucPllCntlFlag &
+					MPLL_CNTL_FLAG_VCO_MODE_MASK);
+		mpll_param->yclk_sel =
+			(uint32_t)((mpll_parameters.ucPllCntlFlag &
+						MPLL_CNTL_FLAG_BYPASS_DQ_PLL) ? 1 : 0);
+		mpll_param->qdr =
+			(uint32_t)((mpll_parameters.ucPllCntlFlag &
+						MPLL_CNTL_FLAG_QDR_ENABLE) ? 1 : 0);
+		mpll_param->half_rate =
+			(uint32_t)((mpll_parameters.ucPllCntlFlag &
+						MPLL_CNTL_FLAG_AD_HALF_RATE) ? 1 : 0);
+		mpll_param->dll_speed =
+			(uint32_t)(mpll_parameters.ucDllSpeed);
+		mpll_param->bw_ctrl =
+			(uint32_t)(mpll_parameters.ucBWCntl);
+	}
+
+	return result;
+}
+
+/** atomctrl_get_memory_pll_dividers_vi().
+ *
+ * @param hwmgr                 input parameter: pointer to HwMgr
+ * @param clock_value             input parameter: memory clock
+ * @param dividers               output parameter: memory PLL dividers
+ */
+int atomctrl_get_memory_pll_dividers_vi(struct pp_hwmgr *hwmgr,
+		uint32_t clock_value, pp_atomctrl_memory_clock_param *mpll_param)
+{
+	COMPUTE_MEMORY_CLOCK_PARAM_PARAMETERS_V2_2 mpll_parameters;
+	int result;
+
+	mpll_parameters.ulClock.ulClock = (uint32_t)clock_value;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ComputeMemoryClockParam),
+			&mpll_parameters);
+
+	if (!result)
+		mpll_param->mpll_post_divider =
+				(uint32_t)mpll_parameters.ulClock.ucPostDiv;
+
+	return result;
+}
+
+int atomctrl_get_engine_pll_dividers_kong(struct pp_hwmgr *hwmgr,
+					  uint32_t clock_value,
+					  pp_atomctrl_clock_dividers_kong *dividers)
+{
+	COMPUTE_MEMORY_ENGINE_PLL_PARAMETERS_V4 pll_parameters;
+	int result;
+
+	pll_parameters.ulClock = clock_value;
+
+	result = cgs_atom_exec_cmd_table
+		(hwmgr->device,
+		 GetIndexIntoMasterTable(COMMAND, ComputeMemoryEnginePLL),
+		 &pll_parameters);
+
+	if (0 == result) {
+		dividers->pll_post_divider = pll_parameters.ucPostDiv;
+		dividers->real_clock = pll_parameters.ulClock;
+	}
+
+	return result;
+}
+
+int atomctrl_get_engine_pll_dividers_vi(
+		struct pp_hwmgr *hwmgr,
+		uint32_t clock_value,
+		pp_atomctrl_clock_dividers_vi *dividers)
+{
+	COMPUTE_GPU_CLOCK_OUTPUT_PARAMETERS_V1_6 pll_patameters;
+	int result;
+
+	pll_patameters.ulClock.ulClock = clock_value;
+	pll_patameters.ulClock.ucPostDiv = COMPUTE_GPUCLK_INPUT_FLAG_SCLK;
+
+	result = cgs_atom_exec_cmd_table
+		(hwmgr->device,
+		 GetIndexIntoMasterTable(COMMAND, ComputeMemoryEnginePLL),
+		 &pll_patameters);
+
+	if (0 == result) {
+		dividers->pll_post_divider =
+			pll_patameters.ulClock.ucPostDiv;
+		dividers->real_clock =
+			pll_patameters.ulClock.ulClock;
+
+		dividers->ul_fb_div.ul_fb_div_frac =
+			pll_patameters.ulFbDiv.usFbDivFrac;
+		dividers->ul_fb_div.ul_fb_div =
+			pll_patameters.ulFbDiv.usFbDiv;
+
+		dividers->uc_pll_ref_div =
+			pll_patameters.ucPllRefDiv;
+		dividers->uc_pll_post_div =
+			pll_patameters.ucPllPostDiv;
+		dividers->uc_pll_cntl_flag =
+			pll_patameters.ucPllCntlFlag;
+	}
+
+	return result;
+}
+
+int atomctrl_get_dfs_pll_dividers_vi(
+		struct pp_hwmgr *hwmgr,
+		uint32_t clock_value,
+		pp_atomctrl_clock_dividers_vi *dividers)
+{
+	COMPUTE_GPU_CLOCK_OUTPUT_PARAMETERS_V1_6 pll_patameters;
+	int result;
+
+	pll_patameters.ulClock.ulClock = clock_value;
+	pll_patameters.ulClock.ucPostDiv =
+		COMPUTE_GPUCLK_INPUT_FLAG_DEFAULT_GPUCLK;
+
+	result = cgs_atom_exec_cmd_table
+		(hwmgr->device,
+		 GetIndexIntoMasterTable(COMMAND, ComputeMemoryEnginePLL),
+		 &pll_patameters);
+
+	if (0 == result) {
+		dividers->pll_post_divider =
+			pll_patameters.ulClock.ucPostDiv;
+		dividers->real_clock =
+			pll_patameters.ulClock.ulClock;
+
+		dividers->ul_fb_div.ul_fb_div_frac =
+			pll_patameters.ulFbDiv.usFbDivFrac;
+		dividers->ul_fb_div.ul_fb_div =
+			pll_patameters.ulFbDiv.usFbDiv;
+
+		dividers->uc_pll_ref_div =
+			pll_patameters.ucPllRefDiv;
+		dividers->uc_pll_post_div =
+			pll_patameters.ucPllPostDiv;
+		dividers->uc_pll_cntl_flag =
+			pll_patameters.ucPllCntlFlag;
+	}
+
+	return result;
+}
+
+/**
+ * Get the reference clock in 10KHz
+ */
+uint32_t atomctrl_get_reference_clock(struct pp_hwmgr *hwmgr)
+{
+	ATOM_FIRMWARE_INFO *fw_info;
+	u8 frev, crev;
+	u16 size;
+	uint32_t clock;
+
+	fw_info = (ATOM_FIRMWARE_INFO *)
+		cgs_atom_get_data_table(hwmgr->device,
+			GetIndexIntoMasterTable(DATA, FirmwareInfo),
+			&size, &frev, &crev);
+
+	if (fw_info == NULL)
+		clock = 2700;
+	else
+		clock = (uint32_t)(le16_to_cpu(fw_info->usReferenceClock));
+
+	return clock;
+}
+
+/**
+ * Returns true if the given voltage type is controlled by GPIO pins.
+ * voltage_type is one of SET_VOLTAGE_TYPE_ASIC_VDDC,
+ * SET_VOLTAGE_TYPE_ASIC_MVDDC, SET_VOLTAGE_TYPE_ASIC_MVDDQ.
+ * voltage_mode is one of ATOM_SET_VOLTAGE, ATOM_SET_VOLTAGE_PHASE
+ */
+bool atomctrl_is_voltage_controled_by_gpio_v3(
+		struct pp_hwmgr *hwmgr,
+		uint8_t voltage_type,
+		uint8_t voltage_mode)
+{
+	ATOM_VOLTAGE_OBJECT_INFO_V3_1 *voltage_info =
+		(ATOM_VOLTAGE_OBJECT_INFO_V3_1 *)get_voltage_info_table(hwmgr->device);
+	bool ret;
+
+	PP_ASSERT_WITH_CODE((NULL != voltage_info),
+			"Could not find Voltage Table in BIOS.", return false;);
+
+	ret = (NULL != atomctrl_lookup_voltage_type_v3
+			(voltage_info, voltage_type, voltage_mode)) ? true : false;
+
+	return ret;
+}
+
+int atomctrl_get_voltage_table_v3(
+		struct pp_hwmgr *hwmgr,
+		uint8_t voltage_type,
+		uint8_t voltage_mode,
+		pp_atomctrl_voltage_table *voltage_table)
+{
+	ATOM_VOLTAGE_OBJECT_INFO_V3_1 *voltage_info =
+		(ATOM_VOLTAGE_OBJECT_INFO_V3_1 *)get_voltage_info_table(hwmgr->device);
+	const ATOM_VOLTAGE_OBJECT_V3 *voltage_object;
+	unsigned int i;
+
+	PP_ASSERT_WITH_CODE((NULL != voltage_info),
+			"Could not find Voltage Table in BIOS.", return -1;);
+
+	voltage_object = atomctrl_lookup_voltage_type_v3
+		(voltage_info, voltage_type, voltage_mode);
+
+	if (voltage_object == NULL)
+		return -1;
+
+	PP_ASSERT_WITH_CODE(
+			(voltage_object->asGpioVoltageObj.ucGpioEntryNum <=
+			PP_ATOMCTRL_MAX_VOLTAGE_ENTRIES),
+			"Too many voltage entries!",
+			return -1;
+			);
+
+	for (i = 0; i < voltage_object->asGpioVoltageObj.ucGpioEntryNum; i++) {
+		voltage_table->entries[i].value =
+			voltage_object->asGpioVoltageObj.asVolGpioLut[i].usVoltageValue;
+		voltage_table->entries[i].smio_low =
+			voltage_object->asGpioVoltageObj.asVolGpioLut[i].ulVoltageId;
+	}
+
+	voltage_table->mask_low    =
+		voltage_object->asGpioVoltageObj.ulGpioMaskVal;
+	voltage_table->count      =
+		voltage_object->asGpioVoltageObj.ucGpioEntryNum;
+	voltage_table->phase_delay =
+		voltage_object->asGpioVoltageObj.ucPhaseDelay;
+
+	return 0;
+}
+
+static bool atomctrl_lookup_gpio_pin(
+		ATOM_GPIO_PIN_LUT * gpio_lookup_table,
+		const uint32_t pinId,
+		pp_atomctrl_gpio_pin_assignment *gpio_pin_assignment)
+{
+	unsigned int size = le16_to_cpu(gpio_lookup_table->sHeader.usStructureSize);
+	unsigned int offset = offsetof(ATOM_GPIO_PIN_LUT, asGPIO_Pin[0]);
+	uint8_t *start = (uint8_t *)gpio_lookup_table;
+
+	while (offset < size) {
+		const ATOM_GPIO_PIN_ASSIGNMENT *pin_assignment =
+			(const ATOM_GPIO_PIN_ASSIGNMENT *)(start + offset);
+
+		if (pinId == pin_assignment->ucGPIO_ID) {
+			gpio_pin_assignment->uc_gpio_pin_bit_shift =
+				pin_assignment->ucGpioPinBitShift;
+			gpio_pin_assignment->us_gpio_pin_aindex =
+				le16_to_cpu(pin_assignment->usGpioPin_AIndex);
+			return false;
+		}
+
+		offset += offsetof(ATOM_GPIO_PIN_ASSIGNMENT, ucGPIO_ID) + 1;
+	}
+
+	return true;
+}
+
+/**
+ * Private Function to get the PowerPlay Table Address.
+ * WARNING: The tabled returned by this function is in
+ * dynamically allocated memory.
+ * The caller has to release if by calling kfree.
+ */
+static ATOM_GPIO_PIN_LUT *get_gpio_lookup_table(void *device)
+{
+	u8 frev, crev;
+	u16 size;
+	void *table_address;
+
+	table_address = (ATOM_GPIO_PIN_LUT *)
+		cgs_atom_get_data_table(device,
+				GetIndexIntoMasterTable(DATA, GPIO_Pin_LUT),
+				&size, &frev, &crev);
+
+	PP_ASSERT_WITH_CODE((NULL != table_address),
+			"Error retrieving BIOS Table Address!", return NULL;);
+
+	return (ATOM_GPIO_PIN_LUT *)table_address;
+}
+
+/**
+ * Returns 1 if the given pin id find in lookup table.
+ */
+bool atomctrl_get_pp_assign_pin(
+		struct pp_hwmgr *hwmgr,
+		const uint32_t pinId,
+		pp_atomctrl_gpio_pin_assignment *gpio_pin_assignment)
+{
+	bool bRet = 0;
+	ATOM_GPIO_PIN_LUT *gpio_lookup_table =
+		get_gpio_lookup_table(hwmgr->device);
+
+	PP_ASSERT_WITH_CODE((NULL != gpio_lookup_table),
+			"Could not find GPIO lookup Table in BIOS.", return -1);
+
+	bRet = atomctrl_lookup_gpio_pin(gpio_lookup_table, pinId,
+		gpio_pin_assignment);
+
+	return bRet;
+}
+
+int atomctrl_calculate_voltage_evv_on_sclk(
+		struct pp_hwmgr *hwmgr,
+		uint8_t voltage_type,
+		uint32_t sclk,
+		uint16_t virtual_voltage_Id,
+		uint16_t *voltage,
+		uint16_t dpm_level,
+		bool debug)
+{
+	ATOM_ASIC_PROFILING_INFO_V3_4 *getASICProfilingInfo;
+
+	EFUSE_LINEAR_FUNC_PARAM sRO_fuse;
+	EFUSE_LINEAR_FUNC_PARAM sCACm_fuse;
+	EFUSE_LINEAR_FUNC_PARAM sCACb_fuse;
+	EFUSE_LOGISTIC_FUNC_PARAM sKt_Beta_fuse;
+	EFUSE_LOGISTIC_FUNC_PARAM sKv_m_fuse;
+	EFUSE_LOGISTIC_FUNC_PARAM sKv_b_fuse;
+	EFUSE_INPUT_PARAMETER sInput_FuseValues;
+	READ_EFUSE_VALUE_PARAMETER sOutput_FuseValues;
+
+	uint32_t ul_RO_fused, ul_CACb_fused, ul_CACm_fused, ul_Kt_Beta_fused, ul_Kv_m_fused, ul_Kv_b_fused;
+	fInt fSM_A0, fSM_A1, fSM_A2, fSM_A3, fSM_A4, fSM_A5, fSM_A6, fSM_A7;
+	fInt fMargin_RO_a, fMargin_RO_b, fMargin_RO_c, fMargin_fixed, fMargin_FMAX_mean, fMargin_Plat_mean, fMargin_FMAX_sigma, fMargin_Plat_sigma, fMargin_DC_sigma;
+	fInt fLkg_FT, repeat;
+	fInt fMicro_FMAX, fMicro_CR, fSigma_FMAX, fSigma_CR, fSigma_DC, fDC_SCLK, fSquared_Sigma_DC, fSquared_Sigma_CR, fSquared_Sigma_FMAX;
+	fInt fRLL_LoadLine, fPowerDPMx, fDerateTDP, fVDDC_base, fA_Term, fC_Term, fB_Term, fRO_DC_margin;
+	fInt fRO_fused, fCACm_fused, fCACb_fused, fKv_m_fused, fKv_b_fused, fKt_Beta_fused, fFT_Lkg_V0NORM;
+	fInt fSclk_margin, fSclk, fEVV_V;
+	fInt fV_min, fV_max, fT_prod, fLKG_Factor, fT_FT, fV_FT, fV_x, fTDP_Power, fTDP_Power_right, fTDP_Power_left, fTDP_Current, fV_NL;
+	uint32_t ul_FT_Lkg_V0NORM;
+	fInt fLn_MaxDivMin, fMin, fAverage, fRange;
+	fInt fRoots[2];
+	fInt fStepSize = GetScaledFraction(625, 100000);
+
+	int result;
+
+	getASICProfilingInfo = (ATOM_ASIC_PROFILING_INFO_V3_4 *)
+			cgs_atom_get_data_table(hwmgr->device,
+					GetIndexIntoMasterTable(DATA, ASIC_ProfilingInfo),
+					NULL, NULL, NULL);
+
+	if (!getASICProfilingInfo)
+		return -1;
+
+	if(getASICProfilingInfo->asHeader.ucTableFormatRevision < 3 ||
+			(getASICProfilingInfo->asHeader.ucTableFormatRevision == 3 &&
+			getASICProfilingInfo->asHeader.ucTableContentRevision < 4))
+		return -1;
+
+	/*-----------------------------------------------------------
+	 *GETTING MULTI-STEP PARAMETERS RELATED TO CURRENT DPM LEVEL
+	 *-----------------------------------------------------------
+	 */
+	fRLL_LoadLine = Divide(getASICProfilingInfo->ulLoadLineSlop, 1000);
+
+	switch (dpm_level) {
+	case 1:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm1);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM1, 1000);
+		break;
+	case 2:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm2);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM2, 1000);
+		break;
+	case 3:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm3);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM3, 1000);
+		break;
+	case 4:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm4);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM4, 1000);
+		break;
+	case 5:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm5);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM5, 1000);
+		break;
+	case 6:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm6);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM6, 1000);
+		break;
+	case 7:
+		fPowerDPMx = Convert_ULONG_ToFraction(getASICProfilingInfo->usPowerDpm7);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM7, 1000);
+		break;
+	default:
+		printk(KERN_ERR "DPM Level not supported\n");
+		fPowerDPMx = Convert_ULONG_ToFraction(1);
+		fDerateTDP = GetScaledFraction(getASICProfilingInfo->ulTdpDerateDPM0, 1000);
+	}
+
+	/*-------------------------
+	 * DECODING FUSE VALUES
+	 * ------------------------
+	 */
+	/*Decode RO_Fused*/
+	sRO_fuse = getASICProfilingInfo->sRoFuse;
+
+	sInput_FuseValues.usEfuseIndex = sRO_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sRO_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sRO_fuse.ucEfuseLength;
+
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	/* Finally, the actual fuse value */
+	ul_RO_fused = sOutput_FuseValues.ulEfuseValue;
+	fMin = GetScaledFraction(sRO_fuse.ulEfuseMin, 1);
+	fRange = GetScaledFraction(sRO_fuse.ulEfuseEncodeRange, 1);
+	fRO_fused = fDecodeLinearFuse(ul_RO_fused, fMin, fRange, sRO_fuse.ucEfuseLength);
+
+	sCACm_fuse = getASICProfilingInfo->sCACm;
+
+	sInput_FuseValues.usEfuseIndex = sCACm_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sCACm_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sCACm_fuse.ucEfuseLength;
+
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	ul_CACm_fused = sOutput_FuseValues.ulEfuseValue;
+	fMin = GetScaledFraction(sCACm_fuse.ulEfuseMin, 1000);
+	fRange = GetScaledFraction(sCACm_fuse.ulEfuseEncodeRange, 1000);
+
+	fCACm_fused = fDecodeLinearFuse(ul_CACm_fused, fMin, fRange, sCACm_fuse.ucEfuseLength);
+
+	sCACb_fuse = getASICProfilingInfo->sCACb;
+
+	sInput_FuseValues.usEfuseIndex = sCACb_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sCACb_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sCACb_fuse.ucEfuseLength;
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	ul_CACb_fused = sOutput_FuseValues.ulEfuseValue;
+	fMin = GetScaledFraction(sCACb_fuse.ulEfuseMin, 1000);
+	fRange = GetScaledFraction(sCACb_fuse.ulEfuseEncodeRange, 1000);
+
+	fCACb_fused = fDecodeLinearFuse(ul_CACb_fused, fMin, fRange, sCACb_fuse.ucEfuseLength);
+
+	sKt_Beta_fuse = getASICProfilingInfo->sKt_b;
+
+	sInput_FuseValues.usEfuseIndex = sKt_Beta_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sKt_Beta_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sKt_Beta_fuse.ucEfuseLength;
+
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	ul_Kt_Beta_fused = sOutput_FuseValues.ulEfuseValue;
+	fAverage = GetScaledFraction(sKt_Beta_fuse.ulEfuseEncodeAverage, 1000);
+	fRange = GetScaledFraction(sKt_Beta_fuse.ulEfuseEncodeRange, 1000);
+
+	fKt_Beta_fused = fDecodeLogisticFuse(ul_Kt_Beta_fused,
+			fAverage, fRange, sKt_Beta_fuse.ucEfuseLength);
+
+	sKv_m_fuse = getASICProfilingInfo->sKv_m;
+
+	sInput_FuseValues.usEfuseIndex = sKv_m_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sKv_m_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sKv_m_fuse.ucEfuseLength;
+
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+	if (result)
+		return result;
+
+	ul_Kv_m_fused = sOutput_FuseValues.ulEfuseValue;
+	fAverage = GetScaledFraction(sKv_m_fuse.ulEfuseEncodeAverage, 1000);
+	fRange = GetScaledFraction((sKv_m_fuse.ulEfuseEncodeRange & 0x7fffffff), 1000);
+	fRange = fMultiply(fRange, ConvertToFraction(-1));
+
+	fKv_m_fused = fDecodeLogisticFuse(ul_Kv_m_fused,
+			fAverage, fRange, sKv_m_fuse.ucEfuseLength);
+
+	sKv_b_fuse = getASICProfilingInfo->sKv_b;
+
+	sInput_FuseValues.usEfuseIndex = sKv_b_fuse.usEfuseIndex;
+	sInput_FuseValues.ucBitShift = sKv_b_fuse.ucEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = sKv_b_fuse.ucEfuseLength;
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	ul_Kv_b_fused = sOutput_FuseValues.ulEfuseValue;
+	fAverage = GetScaledFraction(sKv_b_fuse.ulEfuseEncodeAverage, 1000);
+	fRange = GetScaledFraction(sKv_b_fuse.ulEfuseEncodeRange, 1000);
+
+	fKv_b_fused = fDecodeLogisticFuse(ul_Kv_b_fused,
+			fAverage, fRange, sKv_b_fuse.ucEfuseLength);
+
+	/* Decoding the Leakage - No special struct container */
+	/*
+	 * usLkgEuseIndex=56
+	 * ucLkgEfuseBitLSB=6
+	 * ucLkgEfuseLength=10
+	 * ulLkgEncodeLn_MaxDivMin=69077
+	 * ulLkgEncodeMax=1000000
+	 * ulLkgEncodeMin=1000
+	 * ulEfuseLogisticAlpha=13
+	 */
+
+	sInput_FuseValues.usEfuseIndex = getASICProfilingInfo->usLkgEuseIndex;
+	sInput_FuseValues.ucBitShift = getASICProfilingInfo->ucLkgEfuseBitLSB;
+	sInput_FuseValues.ucBitLength = getASICProfilingInfo->ucLkgEfuseLength;
+
+	sOutput_FuseValues.sEfuse = sInput_FuseValues;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&sOutput_FuseValues);
+
+	if (result)
+		return result;
+
+	ul_FT_Lkg_V0NORM = sOutput_FuseValues.ulEfuseValue;
+	fLn_MaxDivMin = GetScaledFraction(getASICProfilingInfo->ulLkgEncodeLn_MaxDivMin, 10000);
+	fMin = GetScaledFraction(getASICProfilingInfo->ulLkgEncodeMin, 10000);
+
+	fFT_Lkg_V0NORM = fDecodeLeakageID(ul_FT_Lkg_V0NORM,
+			fLn_MaxDivMin, fMin, getASICProfilingInfo->ucLkgEfuseLength);
+	fLkg_FT = fFT_Lkg_V0NORM;
+
+	/*-------------------------------------------
+	 * PART 2 - Grabbing all required values
+	 *-------------------------------------------
+	 */
+	fSM_A0 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A0, 1000000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A0_sign)));
+	fSM_A1 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A1, 1000000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A1_sign)));
+	fSM_A2 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A2, 100000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A2_sign)));
+	fSM_A3 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A3, 1000000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A3_sign)));
+	fSM_A4 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A4, 1000000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A4_sign)));
+	fSM_A5 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A5, 1000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A5_sign)));
+	fSM_A6 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A6, 1000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A6_sign)));
+	fSM_A7 = fMultiply(GetScaledFraction(getASICProfilingInfo->ulSM_A7, 1000),
+			ConvertToFraction(uPow(-1, getASICProfilingInfo->ucSM_A7_sign)));
+
+	fMargin_RO_a = ConvertToFraction(getASICProfilingInfo->ulMargin_RO_a);
+	fMargin_RO_b = ConvertToFraction(getASICProfilingInfo->ulMargin_RO_b);
+	fMargin_RO_c = ConvertToFraction(getASICProfilingInfo->ulMargin_RO_c);
+
+	fMargin_fixed = ConvertToFraction(getASICProfilingInfo->ulMargin_fixed);
+
+	fMargin_FMAX_mean = GetScaledFraction(
+			getASICProfilingInfo->ulMargin_Fmax_mean, 10000);
+	fMargin_Plat_mean = GetScaledFraction(
+			getASICProfilingInfo->ulMargin_plat_mean, 10000);
+	fMargin_FMAX_sigma = GetScaledFraction(
+			getASICProfilingInfo->ulMargin_Fmax_sigma, 10000);
+	fMargin_Plat_sigma = GetScaledFraction(
+			getASICProfilingInfo->ulMargin_plat_sigma, 10000);
+
+	fMargin_DC_sigma = GetScaledFraction(
+			getASICProfilingInfo->ulMargin_DC_sigma, 100);
+	fMargin_DC_sigma = fDivide(fMargin_DC_sigma, ConvertToFraction(1000));
+
+	fCACm_fused = fDivide(fCACm_fused, ConvertToFraction(100));
+	fCACb_fused = fDivide(fCACb_fused, ConvertToFraction(100));
+	fKt_Beta_fused = fDivide(fKt_Beta_fused, ConvertToFraction(100));
+	fKv_m_fused =  fNegate(fDivide(fKv_m_fused, ConvertToFraction(100)));
+	fKv_b_fused = fDivide(fKv_b_fused, ConvertToFraction(10));
+
+	fSclk = GetScaledFraction(sclk, 100);
+
+	fV_max = fDivide(GetScaledFraction(
+			getASICProfilingInfo->ulMaxVddc, 1000), ConvertToFraction(4));
+	fT_prod = GetScaledFraction(getASICProfilingInfo->ulBoardCoreTemp, 10);
+	fLKG_Factor = GetScaledFraction(getASICProfilingInfo->ulEvvLkgFactor, 100);
+	fT_FT = GetScaledFraction(getASICProfilingInfo->ulLeakageTemp, 10);
+	fV_FT = fDivide(GetScaledFraction(
+			getASICProfilingInfo->ulLeakageVoltage, 1000), ConvertToFraction(4));
+	fV_min = fDivide(GetScaledFraction(
+			getASICProfilingInfo->ulMinVddc, 1000), ConvertToFraction(4));
+
+	/*-----------------------
+	 * PART 3
+	 *-----------------------
+	 */
+
+	fA_Term = fAdd(fMargin_RO_a, fAdd(fMultiply(fSM_A4,fSclk), fSM_A5));
+	fB_Term = fAdd(fAdd(fMultiply(fSM_A2, fSclk), fSM_A6), fMargin_RO_b);
+	fC_Term = fAdd(fMargin_RO_c,
+			fAdd(fMultiply(fSM_A0,fLkg_FT),
+			fAdd(fMultiply(fSM_A1, fMultiply(fLkg_FT,fSclk)),
+			fAdd(fMultiply(fSM_A3, fSclk),
+			fSubtract(fSM_A7,fRO_fused)))));
+
+	fVDDC_base = fSubtract(fRO_fused,
+			fSubtract(fMargin_RO_c,
+					fSubtract(fSM_A3, fMultiply(fSM_A1, fSclk))));
+	fVDDC_base = fDivide(fVDDC_base, fAdd(fMultiply(fSM_A0,fSclk), fSM_A2));
+
+	repeat = fSubtract(fVDDC_base,
+			fDivide(fMargin_DC_sigma, ConvertToFraction(1000)));
+
+	fRO_DC_margin = fAdd(fMultiply(fMargin_RO_a,
+			fGetSquare(repeat)),
+			fAdd(fMultiply(fMargin_RO_b, repeat),
+			fMargin_RO_c));
+
+	fDC_SCLK = fSubtract(fRO_fused,
+			fSubtract(fRO_DC_margin,
+			fSubtract(fSM_A3,
+			fMultiply(fSM_A2, repeat))));
+	fDC_SCLK = fDivide(fDC_SCLK, fAdd(fMultiply(fSM_A0,repeat), fSM_A1));
+
+	fSigma_DC = fSubtract(fSclk, fDC_SCLK);
+
+	fMicro_FMAX = fMultiply(fSclk, fMargin_FMAX_mean);
+	fMicro_CR = fMultiply(fSclk, fMargin_Plat_mean);
+	fSigma_FMAX = fMultiply(fSclk, fMargin_FMAX_sigma);
+	fSigma_CR = fMultiply(fSclk, fMargin_Plat_sigma);
+
+	fSquared_Sigma_DC = fGetSquare(fSigma_DC);
+	fSquared_Sigma_CR = fGetSquare(fSigma_CR);
+	fSquared_Sigma_FMAX = fGetSquare(fSigma_FMAX);
+
+	fSclk_margin = fAdd(fMicro_FMAX,
+			fAdd(fMicro_CR,
+			fAdd(fMargin_fixed,
+			fSqrt(fAdd(fSquared_Sigma_FMAX,
+			fAdd(fSquared_Sigma_DC, fSquared_Sigma_CR))))));
+	/*
+	 fA_Term = fSM_A4 * (fSclk + fSclk_margin) + fSM_A5;
+	 fB_Term = fSM_A2 * (fSclk + fSclk_margin) + fSM_A6;
+	 fC_Term = fRO_DC_margin + fSM_A0 * fLkg_FT + fSM_A1 * fLkg_FT * (fSclk + fSclk_margin) + fSM_A3 * (fSclk + fSclk_margin) + fSM_A7 - fRO_fused;
+	 */
+
+	fA_Term = fAdd(fMultiply(fSM_A4, fAdd(fSclk, fSclk_margin)), fSM_A5);
+	fB_Term = fAdd(fMultiply(fSM_A2, fAdd(fSclk, fSclk_margin)), fSM_A6);
+	fC_Term = fAdd(fRO_DC_margin,
+			fAdd(fMultiply(fSM_A0, fLkg_FT),
+			fAdd(fMultiply(fMultiply(fSM_A1, fLkg_FT),
+			fAdd(fSclk, fSclk_margin)),
+			fAdd(fMultiply(fSM_A3,
+			fAdd(fSclk, fSclk_margin)),
+			fSubtract(fSM_A7, fRO_fused)))));
+
+	SolveQuadracticEqn(fA_Term, fB_Term, fC_Term, fRoots);
+
+	if (GreaterThan(fRoots[0], fRoots[1]))
+		fEVV_V = fRoots[1];
+	else
+		fEVV_V = fRoots[0];
+
+	if (GreaterThan(fV_min, fEVV_V))
+		fEVV_V = fV_min;
+	else if (GreaterThan(fEVV_V, fV_max))
+		fEVV_V = fSubtract(fV_max, fStepSize);
+
+	fEVV_V = fRoundUpByStepSize(fEVV_V, fStepSize, 0);
+
+	/*-----------------
+	 * PART 4
+	 *-----------------
+	 */
+
+	fV_x = fV_min;
+
+	while (GreaterThan(fAdd(fV_max, fStepSize), fV_x)) {
+		fTDP_Power_left = fMultiply(fMultiply(fMultiply(fAdd(
+				fMultiply(fCACm_fused, fV_x), fCACb_fused), fSclk),
+				fGetSquare(fV_x)), fDerateTDP);
+
+		fTDP_Power_right = fMultiply(fFT_Lkg_V0NORM, fMultiply(fLKG_Factor,
+				fMultiply(fExponential(fMultiply(fAdd(fMultiply(fKv_m_fused,
+				fT_prod), fKv_b_fused), fV_x)), fV_x)));
+		fTDP_Power_right = fMultiply(fTDP_Power_right, fExponential(fMultiply(
+				fKt_Beta_fused, fT_prod)));
+		fTDP_Power_right = fDivide(fTDP_Power_right, fExponential(fMultiply(
+				fAdd(fMultiply(fKv_m_fused, fT_prod), fKv_b_fused), fV_FT)));
+		fTDP_Power_right = fDivide(fTDP_Power_right, fExponential(fMultiply(
+				fKt_Beta_fused, fT_FT)));
+
+		fTDP_Power = fAdd(fTDP_Power_left, fTDP_Power_right);
+
+		fTDP_Current = fDivide(fTDP_Power, fV_x);
+
+		fV_NL = fAdd(fV_x, fDivide(fMultiply(fTDP_Current, fRLL_LoadLine),
+				ConvertToFraction(10)));
+
+		fV_NL = fRoundUpByStepSize(fV_NL, fStepSize, 0);
+
+		if (GreaterThan(fV_max, fV_NL) &&
+			(GreaterThan(fV_NL,fEVV_V) ||
+			Equal(fV_NL, fEVV_V))) {
+			fV_NL = fMultiply(fV_NL, ConvertToFraction(1000));
+
+			*voltage = (uint16_t)fV_NL.partial.real;
+			break;
+		} else
+			fV_x = fAdd(fV_x, fStepSize);
+	}
+
+	return result;
+}
+
+/** atomctrl_get_voltage_evv_on_sclk gets voltage via call to ATOM COMMAND table.
+ * @param hwmgr               	input: pointer to hwManager
+ * @param voltage_type            input: type of EVV voltage VDDC or VDDGFX
+ * @param sclk                        input: in 10Khz unit. DPM state SCLK frequency
+ *                                   		which is define in PPTable SCLK/VDDC dependence
+ *				table associated with this virtual_voltage_Id
+ * @param virtual_voltage_Id      input: voltage id which match per voltage DPM state: 0xff01, 0xff02.. 0xff08
+ * @param voltage		       output: real voltage level in unit of mv
+ */
+int atomctrl_get_voltage_evv_on_sclk(
+		struct pp_hwmgr *hwmgr,
+		uint8_t voltage_type,
+		uint32_t sclk, uint16_t virtual_voltage_Id,
+		uint16_t *voltage)
+{
+	int result;
+	GET_VOLTAGE_INFO_INPUT_PARAMETER_V1_2 get_voltage_info_param_space;
+
+	get_voltage_info_param_space.ucVoltageType   =
+		voltage_type;
+	get_voltage_info_param_space.ucVoltageMode   =
+		ATOM_GET_VOLTAGE_EVV_VOLTAGE;
+	get_voltage_info_param_space.usVoltageLevel  =
+		virtual_voltage_Id;
+	get_voltage_info_param_space.ulSCLKFreq      =
+		sclk;
+
+	result = cgs_atom_exec_cmd_table(hwmgr->device,
+			GetIndexIntoMasterTable(COMMAND, GetVoltageInfo),
+			&get_voltage_info_param_space);
+
+	if (0 != result)
+		return result;
+
+	*voltage = ((GET_EVV_VOLTAGE_INFO_OUTPUT_PARAMETER_V1_2 *)
+			(&get_voltage_info_param_space))->usVoltageLevel;
+
+	return result;
+}
+
+/**
+ * Get the mpll reference clock in 10KHz
+ */
+uint32_t atomctrl_get_mpll_reference_clock(struct pp_hwmgr *hwmgr)
+{
+	ATOM_COMMON_TABLE_HEADER *fw_info;
+	uint32_t clock;
+	u8 frev, crev;
+	u16 size;
+
+	fw_info = (ATOM_COMMON_TABLE_HEADER *)
+		cgs_atom_get_data_table(hwmgr->device,
+				GetIndexIntoMasterTable(DATA, FirmwareInfo),
+				&size, &frev, &crev);
+
+	if (fw_info == NULL)
+		clock = 2700;
+	else {
+		if ((fw_info->ucTableFormatRevision == 2) &&
+			(le16_to_cpu(fw_info->usStructureSize) >= sizeof(ATOM_FIRMWARE_INFO_V2_1))) {
+			ATOM_FIRMWARE_INFO_V2_1 *fwInfo_2_1 =
+				(ATOM_FIRMWARE_INFO_V2_1 *)fw_info;
+			clock = (uint32_t)(le16_to_cpu(fwInfo_2_1->usMemoryReferenceClock));
+		} else {
+			ATOM_FIRMWARE_INFO *fwInfo_0_0 =
+				(ATOM_FIRMWARE_INFO *)fw_info;
+			clock = (uint32_t)(le16_to_cpu(fwInfo_0_0->usReferenceClock));
+		}
+	}
+
+	return clock;
+}
+
+/**
+ * Get the asic internal spread spectrum table
+ */
+static ATOM_ASIC_INTERNAL_SS_INFO *asic_internal_ss_get_ss_table(void *device)
+{
+	ATOM_ASIC_INTERNAL_SS_INFO *table = NULL;
+	u8 frev, crev;
+	u16 size;
+
+	table = (ATOM_ASIC_INTERNAL_SS_INFO *)
+		cgs_atom_get_data_table(device,
+			GetIndexIntoMasterTable(DATA, ASIC_InternalSS_Info),
+			&size, &frev, &crev);
+
+	return table;
+}
+
+/**
+ * Get the asic internal spread spectrum assignment
+ */
+static int asic_internal_ss_get_ss_asignment(struct pp_hwmgr *hwmgr,
+		const uint8_t clockSource,
+		const uint32_t clockSpeed,
+		pp_atomctrl_internal_ss_info *ssEntry)
+{
+	ATOM_ASIC_INTERNAL_SS_INFO *table;
+	ATOM_ASIC_SS_ASSIGNMENT *ssInfo;
+	int entry_found = 0;
+
+	memset(ssEntry, 0x00, sizeof(pp_atomctrl_internal_ss_info));
+
+	table = asic_internal_ss_get_ss_table(hwmgr->device);
+
+	if (NULL == table)
+		return -1;
+
+	ssInfo = &table->asSpreadSpectrum[0];
+
+	while (((uint8_t *)ssInfo - (uint8_t *)table) <
+		le16_to_cpu(table->sHeader.usStructureSize)) {
+		if ((clockSource == ssInfo->ucClockIndication) &&
+			((uint32_t)clockSpeed <= le32_to_cpu(ssInfo->ulTargetClockRange))) {
+			entry_found = 1;
+			break;
+		}
+
+		ssInfo = (ATOM_ASIC_SS_ASSIGNMENT *)((uint8_t *)ssInfo +
+				sizeof(ATOM_ASIC_SS_ASSIGNMENT));
+	}
+
+	if (entry_found) {
+		ssEntry->speed_spectrum_percentage =
+			ssInfo->usSpreadSpectrumPercentage;
+		ssEntry->speed_spectrum_rate = ssInfo->usSpreadRateInKhz;
+
+		if (((GET_DATA_TABLE_MAJOR_REVISION(table) == 2) &&
+			(GET_DATA_TABLE_MINOR_REVISION(table) >= 2)) ||
+			(GET_DATA_TABLE_MAJOR_REVISION(table) == 3)) {
+			ssEntry->speed_spectrum_rate /= 100;
+		}
+
+		switch (ssInfo->ucSpreadSpectrumMode) {
+		case 0:
+			ssEntry->speed_spectrum_mode =
+				pp_atomctrl_spread_spectrum_mode_down;
+			break;
+		case 1:
+			ssEntry->speed_spectrum_mode =
+				pp_atomctrl_spread_spectrum_mode_center;
+			break;
+		default:
+			ssEntry->speed_spectrum_mode =
+				pp_atomctrl_spread_spectrum_mode_down;
+			break;
+		}
+	}
+
+	return entry_found ? 0 : 1;
+}
+
+/**
+ * Get the memory clock spread spectrum info
+ */
+int atomctrl_get_memory_clock_spread_spectrum(
+		struct pp_hwmgr *hwmgr,
+		const uint32_t memory_clock,
+		pp_atomctrl_internal_ss_info *ssInfo)
+{
+	return asic_internal_ss_get_ss_asignment(hwmgr,
+			ASIC_INTERNAL_MEMORY_SS, memory_clock, ssInfo);
+}
+/**
+ * Get the engine clock spread spectrum info
+ */
+int atomctrl_get_engine_clock_spread_spectrum(
+		struct pp_hwmgr *hwmgr,
+		const uint32_t engine_clock,
+		pp_atomctrl_internal_ss_info *ssInfo)
+{
+	return asic_internal_ss_get_ss_asignment(hwmgr,
+			ASIC_INTERNAL_ENGINE_SS, engine_clock, ssInfo);
+}
+
+int atomctrl_read_efuse(void *device, uint16_t start_index,
+		uint16_t end_index, uint32_t mask, uint32_t *efuse)
+{
+	int result;
+	READ_EFUSE_VALUE_PARAMETER efuse_param;
+
+	efuse_param.sEfuse.usEfuseIndex = (start_index / 32) * 4;
+	efuse_param.sEfuse.ucBitShift = (uint8_t)
+			(start_index - ((start_index / 32) * 32));
+	efuse_param.sEfuse.ucBitLength  = (uint8_t)
+			((end_index - start_index) + 1);
+
+	result = cgs_atom_exec_cmd_table(device,
+			GetIndexIntoMasterTable(COMMAND, ReadEfuseValue),
+			&efuse_param);
+	if (!result)
+		*efuse = efuse_param.ulEfuseValue & mask;
+
+	return result;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.h b/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.h
new file mode 100644
index 000000000000..627420b80a5f
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/ppatomctrl.h
@@ -0,0 +1,246 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef PP_ATOMVOLTAGECTRL_H
+#define PP_ATOMVOLTAGECTRL_H
+
+#include "hwmgr.h"
+
+#define MEM_TYPE_GDDR5  0x50
+#define MEM_TYPE_GDDR4  0x40
+#define MEM_TYPE_GDDR3  0x30
+#define MEM_TYPE_DDR2   0x20
+#define MEM_TYPE_GDDR1  0x10
+#define MEM_TYPE_DDR3   0xb0
+#define MEM_TYPE_MASK   0xF0
+
+
+/* As returned from PowerConnectorDetectionTable. */
+#define PP_ATOM_POWER_BUDGET_DISABLE_OVERDRIVE  0x80
+#define PP_ATOM_POWER_BUDGET_SHOW_WARNING       0x40
+#define PP_ATOM_POWER_BUDGET_SHOW_WAIVER        0x20
+#define PP_ATOM_POWER_POWER_BUDGET_BEHAVIOUR    0x0F
+
+/* New functions for Evergreen and beyond. */
+#define PP_ATOMCTRL_MAX_VOLTAGE_ENTRIES 32
+
+struct pp_atomctrl_clock_dividers {
+	uint32_t pll_post_divider;
+	uint32_t pll_feedback_divider;
+	uint32_t pll_ref_divider;
+	bool  enable_post_divider;
+};
+
+typedef struct pp_atomctrl_clock_dividers pp_atomctrl_clock_dividers;
+
+union pp_atomctrl_tcipll_fb_divider {
+	struct {
+		uint32_t ul_fb_div_frac : 14;
+		uint32_t ul_fb_div : 12;
+		uint32_t un_used : 6;
+	};
+	uint32_t ul_fb_divider;
+};
+
+typedef union pp_atomctrl_tcipll_fb_divider pp_atomctrl_tcipll_fb_divider;
+
+struct pp_atomctrl_clock_dividers_rv730 {
+	uint32_t pll_post_divider;
+	pp_atomctrl_tcipll_fb_divider mpll_feedback_divider;
+	uint32_t pll_ref_divider;
+	bool  enable_post_divider;
+	bool  enable_dithen;
+	uint32_t vco_mode;
+};
+typedef struct pp_atomctrl_clock_dividers_rv730 pp_atomctrl_clock_dividers_rv730;
+
+
+struct pp_atomctrl_clock_dividers_kong {
+	uint32_t    pll_post_divider;
+	uint32_t    real_clock;
+};
+typedef struct pp_atomctrl_clock_dividers_kong pp_atomctrl_clock_dividers_kong;
+
+struct pp_atomctrl_clock_dividers_ci {
+	uint32_t    pll_post_divider;               /* post divider value */
+	uint32_t    real_clock;
+	pp_atomctrl_tcipll_fb_divider   ul_fb_div;         /* Output Parameter: PLL FB divider */
+	uint8_t   uc_pll_ref_div;                      /* Output Parameter: PLL ref divider */
+	uint8_t   uc_pll_post_div;                      /* Output Parameter: PLL post divider */
+	uint8_t   uc_pll_cntl_flag;                    /*Output Flags: control flag */
+};
+typedef struct pp_atomctrl_clock_dividers_ci pp_atomctrl_clock_dividers_ci;
+
+struct pp_atomctrl_clock_dividers_vi {
+	uint32_t    pll_post_divider;               /* post divider value */
+	uint32_t    real_clock;
+	pp_atomctrl_tcipll_fb_divider   ul_fb_div;         /*Output Parameter: PLL FB divider */
+	uint8_t   uc_pll_ref_div;                      /*Output Parameter: PLL ref divider */
+	uint8_t   uc_pll_post_div;                     /*Output Parameter: PLL post divider */
+	uint8_t   uc_pll_cntl_flag;                    /*Output Flags: control flag */
+};
+typedef struct pp_atomctrl_clock_dividers_vi pp_atomctrl_clock_dividers_vi;
+
+union pp_atomctrl_s_mpll_fb_divider {
+	struct {
+		uint32_t cl_kf : 12;
+		uint32_t clk_frac : 12;
+		uint32_t un_used : 8;
+	};
+	uint32_t ul_fb_divider;
+};
+typedef union pp_atomctrl_s_mpll_fb_divider pp_atomctrl_s_mpll_fb_divider;
+
+enum pp_atomctrl_spread_spectrum_mode {
+	pp_atomctrl_spread_spectrum_mode_down = 0,
+	pp_atomctrl_spread_spectrum_mode_center
+};
+typedef enum pp_atomctrl_spread_spectrum_mode pp_atomctrl_spread_spectrum_mode;
+
+struct pp_atomctrl_memory_clock_param {
+	pp_atomctrl_s_mpll_fb_divider mpll_fb_divider;
+	uint32_t mpll_post_divider;
+	uint32_t bw_ctrl;
+	uint32_t dll_speed;
+	uint32_t vco_mode;
+	uint32_t yclk_sel;
+	uint32_t qdr;
+	uint32_t half_rate;
+};
+typedef struct pp_atomctrl_memory_clock_param pp_atomctrl_memory_clock_param;
+
+struct pp_atomctrl_internal_ss_info {
+	uint32_t speed_spectrum_percentage;                      /* in 1/100 percentage */
+	uint32_t speed_spectrum_rate;                            /* in KHz */
+	pp_atomctrl_spread_spectrum_mode speed_spectrum_mode;
+};
+typedef struct pp_atomctrl_internal_ss_info pp_atomctrl_internal_ss_info;
+
+#ifndef NUMBER_OF_M3ARB_PARAMS
+#define NUMBER_OF_M3ARB_PARAMS 3
+#endif
+
+#ifndef NUMBER_OF_M3ARB_PARAM_SETS
+#define NUMBER_OF_M3ARB_PARAM_SETS 10
+#endif
+
+struct pp_atomctrl_kong_system_info {
+	uint32_t			ul_bootup_uma_clock;          /* in 10kHz unit */
+	uint16_t			us_max_nb_voltage;            /* high NB voltage, calculated using current VDDNB (D24F2xDC) and VDDNB offset fuse; */
+	uint16_t			us_min_nb_voltage;            /* low NB voltage, calculated using current VDDNB (D24F2xDC) and VDDNB offset fuse; */
+	uint16_t			us_bootup_nb_voltage;         /* boot up NB voltage */
+	uint8_t			uc_htc_tmp_lmt;               /* bit [22:16] of D24F3x64 Hardware Thermal Control (HTC) Register, may not be needed, TBD */
+	uint8_t			uc_tj_offset;                /* bit [28:22] of D24F3xE4 Thermtrip Status Register,may not be needed, TBD */
+	/* 0: default 1: uvd 2: fs-3d */
+	uint32_t          ul_csr_m3_srb_cntl[NUMBER_OF_M3ARB_PARAM_SETS][NUMBER_OF_M3ARB_PARAMS];/* arrays with values for CSR M3 arbiter for default */
+};
+typedef struct pp_atomctrl_kong_system_info pp_atomctrl_kong_system_info;
+
+struct pp_atomctrl_memory_info {
+	uint8_t memory_vendor;
+	uint8_t memory_type;
+};
+typedef struct pp_atomctrl_memory_info pp_atomctrl_memory_info;
+
+#define MAX_AC_TIMING_ENTRIES 16
+
+struct pp_atomctrl_memory_clock_range_table {
+	uint8_t   num_entries;
+	uint8_t   rsv[3];
+
+	uint32_t mclk[MAX_AC_TIMING_ENTRIES];
+};
+typedef struct pp_atomctrl_memory_clock_range_table pp_atomctrl_memory_clock_range_table;
+
+struct pp_atomctrl_voltage_table_entry {
+	uint16_t value;
+	uint32_t smio_low;
+};
+
+typedef struct pp_atomctrl_voltage_table_entry pp_atomctrl_voltage_table_entry;
+
+struct pp_atomctrl_voltage_table {
+	uint32_t count;
+	uint32_t mask_low;
+	uint32_t phase_delay;   /* Used for ATOM_GPIO_VOLTAGE_OBJECT_V3 and later */
+	pp_atomctrl_voltage_table_entry entries[PP_ATOMCTRL_MAX_VOLTAGE_ENTRIES];
+};
+
+typedef struct pp_atomctrl_voltage_table pp_atomctrl_voltage_table;
+
+#define VBIOS_MC_REGISTER_ARRAY_SIZE           32
+#define VBIOS_MAX_AC_TIMING_ENTRIES            20
+
+struct pp_atomctrl_mc_reg_entry {
+	uint32_t           mclk_max;
+	uint32_t mc_data[VBIOS_MC_REGISTER_ARRAY_SIZE];
+};
+typedef struct pp_atomctrl_mc_reg_entry pp_atomctrl_mc_reg_entry;
+
+struct pp_atomctrl_mc_register_address {
+	uint16_t s1;
+	uint8_t  uc_pre_reg_data;
+};
+
+typedef struct pp_atomctrl_mc_register_address pp_atomctrl_mc_register_address;
+
+struct pp_atomctrl_mc_reg_table {
+	uint8_t                         last;                    /* number of registers */
+	uint8_t                         num_entries;             /* number of AC timing entries */
+	pp_atomctrl_mc_reg_entry        mc_reg_table_entry[VBIOS_MAX_AC_TIMING_ENTRIES];
+	pp_atomctrl_mc_register_address mc_reg_address[VBIOS_MC_REGISTER_ARRAY_SIZE];
+};
+typedef struct pp_atomctrl_mc_reg_table pp_atomctrl_mc_reg_table;
+
+struct pp_atomctrl_gpio_pin_assignment {
+	uint16_t                   us_gpio_pin_aindex;
+	uint8_t                    uc_gpio_pin_bit_shift;
+};
+typedef struct pp_atomctrl_gpio_pin_assignment pp_atomctrl_gpio_pin_assignment;
+
+extern bool atomctrl_get_pp_assign_pin(struct pp_hwmgr *hwmgr, const uint32_t pinId, pp_atomctrl_gpio_pin_assignment *gpio_pin_assignment);
+extern int atomctrl_get_voltage_evv_on_sclk(struct pp_hwmgr *hwmgr, uint8_t voltage_type, uint32_t sclk, uint16_t virtual_voltage_Id, uint16_t *voltage);
+extern uint32_t atomctrl_get_mpll_reference_clock(struct pp_hwmgr *hwmgr);
+extern int atomctrl_get_memory_clock_spread_spectrum(struct pp_hwmgr *hwmgr, const uint32_t memory_clock, pp_atomctrl_internal_ss_info *ssInfo);
+extern int atomctrl_get_engine_clock_spread_spectrum(struct pp_hwmgr *hwmgr, const uint32_t engine_clock, pp_atomctrl_internal_ss_info *ssInfo);
+extern int atomctrl_initialize_mc_reg_table(struct pp_hwmgr *hwmgr, uint8_t module_index, pp_atomctrl_mc_reg_table *table);
+extern int atomctrl_set_engine_dram_timings_rv770(struct pp_hwmgr *hwmgr, uint32_t engine_clock, uint32_t memory_clock);
+extern uint32_t atomctrl_get_reference_clock(struct pp_hwmgr *hwmgr);
+extern int atomctrl_get_memory_pll_dividers_si(struct pp_hwmgr *hwmgr, uint32_t clock_value, pp_atomctrl_memory_clock_param *mpll_param, bool strobe_mode);
+extern int atomctrl_get_engine_pll_dividers_vi(struct pp_hwmgr *hwmgr, uint32_t clock_value, pp_atomctrl_clock_dividers_vi *dividers);
+extern int atomctrl_get_dfs_pll_dividers_vi(struct pp_hwmgr *hwmgr, uint32_t clock_value, pp_atomctrl_clock_dividers_vi *dividers);
+extern bool atomctrl_is_voltage_controled_by_gpio_v3(struct pp_hwmgr *hwmgr, uint8_t voltage_type, uint8_t voltage_mode);
+extern int atomctrl_get_voltage_table_v3(struct pp_hwmgr *hwmgr, uint8_t voltage_type, uint8_t voltage_mode, pp_atomctrl_voltage_table *voltage_table);
+extern int atomctrl_get_memory_pll_dividers_vi(struct pp_hwmgr *hwmgr,
+		uint32_t clock_value, pp_atomctrl_memory_clock_param *mpll_param);
+extern int atomctrl_get_engine_pll_dividers_kong(struct pp_hwmgr *hwmgr,
+						 uint32_t clock_value,
+						 pp_atomctrl_clock_dividers_kong *dividers);
+extern int atomctrl_read_efuse(void *device, uint16_t start_index,
+		uint16_t end_index, uint32_t mask, uint32_t *efuse);
+extern int atomctrl_calculate_voltage_evv_on_sclk(struct pp_hwmgr *hwmgr, uint8_t voltage_type,
+		uint32_t sclk, uint16_t virtual_voltage_Id, uint16_t *voltage, uint16_t dpm_level, bool debug);
+
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/ppevvmath.h b/drivers/gpu/drm/amd/powerplay/hwmgr/ppevvmath.h
new file mode 100644
index 000000000000..b7429a527828
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/ppevvmath.h
@@ -0,0 +1,612 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <asm/div64.h>
+
+#define SHIFT_AMOUNT 16 /* We multiply all original integers with 2^SHIFT_AMOUNT to get the fInt representation */
+
+#define PRECISION 5 /* Change this value to change the number of decimal places in the final output - 5 is a good default */
+
+#define SHIFTED_2 (2 << SHIFT_AMOUNT)
+#define MAX (1 << (SHIFT_AMOUNT - 1)) - 1 /* 32767 - Might change in the future */
+
+/* -------------------------------------------------------------------------------
+ * NEW TYPE - fINT
+ * -------------------------------------------------------------------------------
+ * A variable of type fInt can be accessed in 3 ways using the dot (.) operator
+ * fInt A;
+ * A.full => The full number as it is. Generally not easy to read
+ * A.partial.real => Only the integer portion
+ * A.partial.decimal => Only the fractional portion
+ */
+typedef union _fInt {
+    int full;
+    struct _partial {
+        unsigned int decimal: SHIFT_AMOUNT; /*Needs to always be unsigned*/
+        int real: 32 - SHIFT_AMOUNT;
+    } partial;
+} fInt;
+
+/* -------------------------------------------------------------------------------
+ * Function Declarations
+ *  -------------------------------------------------------------------------------
+ */
+fInt ConvertToFraction(int);                       /* Use this to convert an INT to a FINT */
+fInt Convert_ULONG_ToFraction(uint32_t);              /* Use this to convert an uint32_t to a FINT */
+fInt GetScaledFraction(int, int);                  /* Use this to convert an INT to a FINT after scaling it by a factor */
+int ConvertBackToInteger(fInt);                    /* Convert a FINT back to an INT that is scaled by 1000 (i.e. last 3 digits are the decimal digits) */
+
+fInt fNegate(fInt);                                /* Returns -1 * input fInt value */
+fInt fAdd (fInt, fInt);                            /* Returns the sum of two fInt numbers */
+fInt fSubtract (fInt A, fInt B);                   /* Returns A-B - Sometimes easier than Adding negative numbers */
+fInt fMultiply (fInt, fInt);                       /* Returns the product of two fInt numbers */
+fInt fDivide (fInt A, fInt B);                     /* Returns A/B */
+fInt fGetSquare(fInt);                             /* Returns the square of a fInt number */
+fInt fSqrt(fInt);                                  /* Returns the Square Root of a fInt number */
+
+int uAbs(int);                                     /* Returns the Absolute value of the Int */
+fInt fAbs(fInt);                                   /* Returns the Absolute value of the fInt */
+int uPow(int base, int exponent);                  /* Returns base^exponent an INT */
+
+void SolveQuadracticEqn(fInt, fInt, fInt, fInt[]); /* Returns the 2 roots via the array */
+bool Equal(fInt, fInt);                         /* Returns true if two fInts are equal to each other */
+bool GreaterThan(fInt A, fInt B);               /* Returns true if A > B */
+
+fInt fExponential(fInt exponent);                  /* Can be used to calculate e^exponent */
+fInt fNaturalLog(fInt value);                      /* Can be used to calculate ln(value) */
+
+/* Fuse decoding functions
+ * -------------------------------------------------------------------------------------
+ */
+fInt fDecodeLinearFuse(uint32_t fuse_value, fInt f_min, fInt f_range, uint32_t bitlength);
+fInt fDecodeLogisticFuse(uint32_t fuse_value, fInt f_average, fInt f_range, uint32_t bitlength);
+fInt fDecodeLeakageID (uint32_t leakageID_fuse, fInt ln_max_div_min, fInt f_min, uint32_t bitlength);
+
+/* Internal Support Functions - Use these ONLY for testing or adding to internal functions
+ * -------------------------------------------------------------------------------------
+ * Some of the following functions take two INTs as their input - This is unsafe for a variety of reasons.
+ */
+fInt Add (int, int);                               /* Add two INTs and return Sum as FINT */
+fInt Multiply (int, int);                          /* Multiply two INTs and return Product as FINT */
+fInt Divide (int, int);                            /* You get the idea... */
+fInt fNegate(fInt);
+
+int uGetScaledDecimal (fInt);                      /* Internal function */
+int GetReal (fInt A);                              /* Internal function */
+
+/* Future Additions and Incomplete Functions
+ * -------------------------------------------------------------------------------------
+ */
+int GetRoundedValue(fInt);                         /* Incomplete function - Useful only when Precision is lacking */
+                                                   /* Let us say we have 2.126 but can only handle 2 decimal points. We could */
+                                                   /* either chop of 6 and keep 2.12 or use this function to get 2.13, which is more accurate */
+
+/* -------------------------------------------------------------------------------------
+ * TROUBLESHOOTING INFORMATION
+ * -------------------------------------------------------------------------------------
+ * 1) ConvertToFraction - InputOutOfRangeException: Only accepts numbers smaller than MAX (default: 32767)
+ * 2) fAdd - OutputOutOfRangeException: Output bigger than MAX (default: 32767)
+ * 3) fMultiply - OutputOutOfRangeException:
+ * 4) fGetSquare - OutputOutOfRangeException:
+ * 5) fDivide - DivideByZeroException
+ * 6) fSqrt - NegativeSquareRootException: Input cannot be a negative number
+ */
+
+/* -------------------------------------------------------------------------------------
+ * START OF CODE
+ * -------------------------------------------------------------------------------------
+ */
+fInt fExponential(fInt exponent)        /*Can be used to calculate e^exponent*/
+{
+	uint32_t i;
+	bool bNegated = false;
+
+	fInt fPositiveOne = ConvertToFraction(1);
+	fInt fZERO = ConvertToFraction(0);
+
+	fInt lower_bound = Divide(78, 10000);
+	fInt solution = fPositiveOne; /*Starting off with baseline of 1 */
+	fInt error_term;
+
+	uint32_t k_array[11] = {55452, 27726, 13863, 6931, 4055, 2231, 1178, 606, 308, 155, 78};
+	uint32_t expk_array[11] = {2560000, 160000, 40000, 20000, 15000, 12500, 11250, 10625, 10313, 10156, 10078};
+
+	if (GreaterThan(fZERO, exponent)) {
+		exponent = fNegate(exponent);
+		bNegated = true;
+	}
+
+	while (GreaterThan(exponent, lower_bound)) {
+		for (i = 0; i < 11; i++) {
+			if (GreaterThan(exponent, GetScaledFraction(k_array[i], 10000))) {
+				exponent = fSubtract(exponent, GetScaledFraction(k_array[i], 10000));
+				solution = fMultiply(solution, GetScaledFraction(expk_array[i], 10000));
+			}
+		}
+	}
+
+	error_term = fAdd(fPositiveOne, exponent);
+
+	solution = fMultiply(solution, error_term);
+
+	if (bNegated)
+		solution = fDivide(fPositiveOne, solution);
+
+	return solution;
+}
+
+fInt fNaturalLog(fInt value)
+{
+	uint32_t i;
+	fInt upper_bound = Divide(8, 1000);
+	fInt fNegativeOne = ConvertToFraction(-1);
+	fInt solution = ConvertToFraction(0); /*Starting off with baseline of 0 */
+	fInt error_term;
+
+	uint32_t k_array[10] = {160000, 40000, 20000, 15000, 12500, 11250, 10625, 10313, 10156, 10078};
+	uint32_t logk_array[10] = {27726, 13863, 6931, 4055, 2231, 1178, 606, 308, 155, 78};
+
+	while (GreaterThan(fAdd(value, fNegativeOne), upper_bound)) {
+		for (i = 0; i < 10; i++) {
+			if (GreaterThan(value, GetScaledFraction(k_array[i], 10000))) {
+				value = fDivide(value, GetScaledFraction(k_array[i], 10000));
+				solution = fAdd(solution, GetScaledFraction(logk_array[i], 10000));
+			}
+		}
+	}
+
+	error_term = fAdd(fNegativeOne, value);
+
+	return (fAdd(solution, error_term));
+}
+
+fInt fDecodeLinearFuse(uint32_t fuse_value, fInt f_min, fInt f_range, uint32_t bitlength)
+{
+	fInt f_fuse_value = Convert_ULONG_ToFraction(fuse_value);
+	fInt f_bit_max_value = Convert_ULONG_ToFraction((uPow(2, bitlength)) - 1);
+
+	fInt f_decoded_value;
+
+	f_decoded_value = fDivide(f_fuse_value, f_bit_max_value);
+	f_decoded_value = fMultiply(f_decoded_value, f_range);
+	f_decoded_value = fAdd(f_decoded_value, f_min);
+
+	return f_decoded_value;
+}
+
+
+fInt fDecodeLogisticFuse(uint32_t fuse_value, fInt f_average, fInt f_range, uint32_t bitlength)
+{
+	fInt f_fuse_value = Convert_ULONG_ToFraction(fuse_value);
+	fInt f_bit_max_value = Convert_ULONG_ToFraction((uPow(2, bitlength)) - 1);
+
+	fInt f_CONSTANT_NEG13 = ConvertToFraction(-13);
+	fInt f_CONSTANT1 = ConvertToFraction(1);
+
+	fInt f_decoded_value;
+
+	f_decoded_value = fSubtract(fDivide(f_bit_max_value, f_fuse_value), f_CONSTANT1);
+	f_decoded_value = fNaturalLog(f_decoded_value);
+	f_decoded_value = fMultiply(f_decoded_value, fDivide(f_range, f_CONSTANT_NEG13));
+	f_decoded_value = fAdd(f_decoded_value, f_average);
+
+	return f_decoded_value;
+}
+
+fInt fDecodeLeakageID (uint32_t leakageID_fuse, fInt ln_max_div_min, fInt f_min, uint32_t bitlength)
+{
+	fInt fLeakage;
+	fInt f_bit_max_value = Convert_ULONG_ToFraction((uPow(2, bitlength)) - 1);
+
+	fLeakage = fMultiply(ln_max_div_min, Convert_ULONG_ToFraction(leakageID_fuse));
+	fLeakage = fDivide(fLeakage, f_bit_max_value);
+	fLeakage = fExponential(fLeakage);
+	fLeakage = fMultiply(fLeakage, f_min);
+
+	return fLeakage;
+}
+
+fInt ConvertToFraction(int X) /*Add all range checking here. Is it possible to make fInt a private declaration? */
+{
+	fInt temp;
+
+	if (X <= MAX)
+		temp.full = (X << SHIFT_AMOUNT);
+	else
+		temp.full = 0;
+
+	return temp;
+}
+
+fInt fNegate(fInt X)
+{
+	fInt CONSTANT_NEGONE = ConvertToFraction(-1);
+	return (fMultiply(X, CONSTANT_NEGONE));
+}
+
+fInt Convert_ULONG_ToFraction(uint32_t X)
+{
+	fInt temp;
+
+	if (X <= MAX)
+		temp.full = (X << SHIFT_AMOUNT);
+	else
+		temp.full = 0;
+
+	return temp;
+}
+
+fInt GetScaledFraction(int X, int factor)
+{
+	int times_shifted, factor_shifted;
+	bool bNEGATED;
+	fInt fValue;
+
+	times_shifted = 0;
+	factor_shifted = 0;
+	bNEGATED = false;
+
+	if (X < 0) {
+		X = -1*X;
+		bNEGATED = true;
+	}
+
+	if (factor < 0) {
+		factor = -1*factor;
+		bNEGATED = !bNEGATED; /*If bNEGATED = true due to X < 0, this will cover the case of negative cancelling negative */
+	}
+
+	if ((X > MAX) || factor > MAX) {
+		if ((X/factor) <= MAX) {
+			while (X > MAX) {
+				X = X >> 1;
+				times_shifted++;
+			}
+
+			while (factor > MAX) {
+				factor = factor >> 1;
+				factor_shifted++;
+			}
+		} else {
+			fValue.full = 0;
+			return fValue;
+		}
+	}
+
+	if (factor == 1)
+	return (ConvertToFraction(X));
+
+	fValue = fDivide(ConvertToFraction(X * uPow(-1, bNEGATED)), ConvertToFraction(factor));
+
+	fValue.full = fValue.full << times_shifted;
+	fValue.full = fValue.full >> factor_shifted;
+
+	return fValue;
+}
+
+/* Addition using two fInts */
+fInt fAdd (fInt X, fInt Y)
+{
+	fInt Sum;
+
+	Sum.full = X.full + Y.full;
+
+	return Sum;
+}
+
+/* Addition using two fInts */
+fInt fSubtract (fInt X, fInt Y)
+{
+	fInt Difference;
+
+	Difference.full = X.full - Y.full;
+
+	return Difference;
+}
+
+bool Equal(fInt A, fInt B)
+{
+	if (A.full == B.full)
+		return true;
+	else
+		return false;
+}
+
+bool GreaterThan(fInt A, fInt B)
+{
+	if (A.full > B.full)
+		return true;
+	else
+		return false;
+}
+
+fInt fMultiply (fInt X, fInt Y) /* Uses 64-bit integers (int64_t) */
+{
+	fInt Product;
+	int64_t tempProduct;
+	bool X_LessThanOne, Y_LessThanOne;
+
+	X_LessThanOne = (X.partial.real == 0 && X.partial.decimal != 0 && X.full >= 0);
+	Y_LessThanOne = (Y.partial.real == 0 && Y.partial.decimal != 0 && Y.full >= 0);
+
+	/*The following is for a very specific common case: Non-zero number with ONLY fractional portion*/
+	/* TEMPORARILY DISABLED - CAN BE USED TO IMPROVE PRECISION
+
+	if (X_LessThanOne && Y_LessThanOne) {
+		Product.full = X.full * Y.full;
+		return Product
+	}*/
+
+	tempProduct = ((int64_t)X.full) * ((int64_t)Y.full); /*Q(16,16)*Q(16,16) = Q(32, 32) - Might become a negative number! */
+	tempProduct = tempProduct >> 16; /*Remove lagging 16 bits - Will lose some precision from decimal; */
+	Product.full = (int)tempProduct; /*The int64_t will lose the leading 16 bits that were part of the integer portion */
+
+	return Product;
+}
+
+fInt fDivide (fInt X, fInt Y)
+{
+	fInt fZERO, fQuotient;
+	int64_t longlongX, longlongY;
+
+	fZERO = ConvertToFraction(0);
+
+	if (Equal(Y, fZERO))
+	return fZERO;
+
+	longlongX = (int64_t)X.full;
+	longlongY = (int64_t)Y.full;
+
+	longlongX = longlongX << 16; /*Q(16,16) -> Q(32,32) */
+
+	div64_s64(longlongX, longlongY); /*Q(32,32) divided by Q(16,16) = Q(16,16) Back to original format */
+
+	fQuotient.full = (int)longlongX;
+	return fQuotient;
+}
+
+int ConvertBackToInteger (fInt A) /*THIS is the function that will be used to check with the Golden settings table*/
+{
+	fInt fullNumber, scaledDecimal, scaledReal;
+
+	scaledReal.full = GetReal(A) * uPow(10, PRECISION-1); /* DOUBLE CHECK THISSSS!!! */
+
+	scaledDecimal.full = uGetScaledDecimal(A);
+
+	fullNumber = fAdd(scaledDecimal,scaledReal);
+
+	return fullNumber.full;
+}
+
+fInt fGetSquare(fInt A)
+{
+	return fMultiply(A,A);
+}
+
+/* x_new = x_old - (x_old^2 - C) / (2 * x_old) */
+fInt fSqrt(fInt num)
+{
+	fInt F_divide_Fprime, Fprime;
+	fInt test;
+	fInt twoShifted;
+	int seed, counter, error;
+	fInt x_new, x_old, C, y;
+
+	fInt fZERO = ConvertToFraction(0);
+
+	/* (0 > num) is the same as (num < 0), i.e., num is negative */
+
+	if (GreaterThan(fZERO, num) || Equal(fZERO, num))
+		return fZERO;
+
+	C = num;
+
+	if (num.partial.real > 3000)
+		seed = 60;
+	else if (num.partial.real > 1000)
+		seed = 30;
+	else if (num.partial.real > 100)
+		seed = 10;
+	else
+		seed = 2;
+
+	counter = 0;
+
+	if (Equal(num, fZERO)) /*Square Root of Zero is zero */
+		return fZERO;
+
+	twoShifted = ConvertToFraction(2);
+	x_new = ConvertToFraction(seed);
+
+	do {
+		counter++;
+
+		x_old.full = x_new.full;
+
+		test = fGetSquare(x_old); /*1.75*1.75 is reverting back to 1 when shifted down */
+		y = fSubtract(test, C); /*y = f(x) = x^2 - C; */
+
+		Fprime = fMultiply(twoShifted, x_old);
+		F_divide_Fprime = fDivide(y, Fprime);
+
+		x_new = fSubtract(x_old, F_divide_Fprime);
+
+		error = ConvertBackToInteger(x_new) - ConvertBackToInteger(x_old);
+
+		if (counter > 20) /*20 is already way too many iterations. If we dont have an answer by then, we never will*/
+			return x_new;
+
+	} while (uAbs(error) > 0);
+
+	return (x_new);
+}
+
+void SolveQuadracticEqn(fInt A, fInt B, fInt C, fInt Roots[])
+{
+	fInt *pRoots = &Roots[0];
+	fInt temp, root_first, root_second;
+	fInt f_CONSTANT10, f_CONSTANT100;
+
+	f_CONSTANT100 = ConvertToFraction(100);
+	f_CONSTANT10 = ConvertToFraction(10);
+
+	while(GreaterThan(A, f_CONSTANT100) || GreaterThan(B, f_CONSTANT100) || GreaterThan(C, f_CONSTANT100)) {
+		A = fDivide(A, f_CONSTANT10);
+		B = fDivide(B, f_CONSTANT10);
+		C = fDivide(C, f_CONSTANT10);
+	}
+
+	temp = fMultiply(ConvertToFraction(4), A); /* root = 4*A */
+	temp = fMultiply(temp, C); /* root = 4*A*C */
+	temp = fSubtract(fGetSquare(B), temp); /* root = b^2 - 4AC */
+	temp = fSqrt(temp); /*root = Sqrt (b^2 - 4AC); */
+
+	root_first = fSubtract(fNegate(B), temp); /* b - Sqrt(b^2 - 4AC) */
+	root_second = fAdd(fNegate(B), temp); /* b + Sqrt(b^2 - 4AC) */
+
+	root_first = fDivide(root_first, ConvertToFraction(2)); /* [b +- Sqrt(b^2 - 4AC)]/[2] */
+	root_first = fDivide(root_first, A); /*[b +- Sqrt(b^2 - 4AC)]/[2*A] */
+
+	root_second = fDivide(root_second, ConvertToFraction(2)); /* [b +- Sqrt(b^2 - 4AC)]/[2] */
+	root_second = fDivide(root_second, A); /*[b +- Sqrt(b^2 - 4AC)]/[2*A] */
+
+	*(pRoots + 0) = root_first;
+	*(pRoots + 1) = root_second;
+}
+
+/* -----------------------------------------------------------------------------
+ * SUPPORT FUNCTIONS
+ * -----------------------------------------------------------------------------
+ */
+
+/* Addition using two normal ints - Temporary - Use only for testing purposes?. */
+fInt Add (int X, int Y)
+{
+	fInt A, B, Sum;
+
+	A.full = (X << SHIFT_AMOUNT);
+	B.full = (Y << SHIFT_AMOUNT);
+
+	Sum.full = A.full + B.full;
+
+	return Sum;
+}
+
+/* Conversion Functions */
+int GetReal (fInt A)
+{
+	return (A.full >> SHIFT_AMOUNT);
+}
+
+/* Temporarily Disabled */
+int GetRoundedValue(fInt A) /*For now, round the 3rd decimal place */
+{
+	/* ROUNDING TEMPORARLY DISABLED
+	int temp = A.full;
+	int decimal_cutoff, decimal_mask = 0x000001FF;
+	decimal_cutoff = temp & decimal_mask;
+	if (decimal_cutoff > 0x147) {
+		temp += 673;
+	}*/
+
+	return ConvertBackToInteger(A)/10000; /*Temporary - in case this was used somewhere else */
+}
+
+fInt Multiply (int X, int Y)
+{
+	fInt A, B, Product;
+
+	A.full = X << SHIFT_AMOUNT;
+	B.full = Y << SHIFT_AMOUNT;
+
+	Product = fMultiply(A, B);
+
+	return Product;
+}
+
+fInt Divide (int X, int Y)
+{
+	fInt A, B, Quotient;
+
+	A.full = X << SHIFT_AMOUNT;
+	B.full = Y << SHIFT_AMOUNT;
+
+	Quotient = fDivide(A, B);
+
+	return Quotient;
+}
+
+int uGetScaledDecimal (fInt A) /*Converts the fractional portion to whole integers - Costly function */
+{
+	int dec[PRECISION];
+	int i, scaledDecimal = 0, tmp = A.partial.decimal;
+
+	for (i = 0; i < PRECISION; i++) {
+		dec[i] = tmp / (1 << SHIFT_AMOUNT);
+		tmp = tmp - ((1 << SHIFT_AMOUNT)*dec[i]);
+		tmp *= 10;
+		scaledDecimal = scaledDecimal + dec[i]*uPow(10, PRECISION - 1 -i);
+	}
+
+	return scaledDecimal;
+}
+
+int uPow(int base, int power)
+{
+	if (power == 0)
+		return 1;
+	else
+		return (base)*uPow(base, power - 1);
+}
+
+fInt fAbs(fInt A)
+{
+	if (A.partial.real < 0)
+		return (fMultiply(A, ConvertToFraction(-1)));
+	else
+		return A;
+}
+
+int uAbs(int X)
+{
+	if (X < 0)
+		return (X * -1);
+	else
+		return X;
+}
+
+fInt fRoundUpByStepSize(fInt A, fInt fStepSize, bool error_term)
+{
+	fInt solution;
+
+	solution = fDivide(A, fStepSize);
+	solution.partial.decimal = 0; /*All fractional digits changes to 0 */
+
+	if (error_term)
+		solution.partial.real += 1; /*Error term of 1 added */
+
+	solution = fMultiply(solution, fStepSize);
+	solution = fAdd(solution, fStepSize);
+
+	return solution;
+}
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.c b/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.c
new file mode 100644
index 000000000000..186496a34cbe
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.c
@@ -0,0 +1,64 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include <linux/types.h>
+#include "atom-types.h"
+#include "atombios.h"
+#include "pppcielanes.h"
+
+/** \file
+ * Functions related to PCIe lane changes.
+ */
+
+/* For converting from number of lanes to lane bits.  */
+static const unsigned char pp_r600_encode_lanes[] = {
+	0,          /*  0 Not Supported  */
+	1,          /*  1 Lane  */
+	2,          /*  2 Lanes  */
+	0,          /*  3 Not Supported  */
+	3,          /*  4 Lanes  */
+	0,          /*  5 Not Supported  */
+	0,          /*  6 Not Supported  */
+	0,          /*  7 Not Supported  */
+	4,          /*  8 Lanes  */
+	0,          /*  9 Not Supported  */
+	0,          /* 10 Not Supported  */
+	0,          /* 11 Not Supported  */
+	5,          /* 12 Lanes (Not actually supported)  */
+	0,          /* 13 Not Supported  */
+	0,          /* 14 Not Supported  */
+	0,          /* 15 Not Supported  */
+	6           /* 16 Lanes  */
+};
+
+static const unsigned char pp_r600_decoded_lanes[8] = { 16, 1, 2, 4, 8, 12, 16, };
+
+uint8_t encode_pcie_lane_width(uint32_t num_lanes)
+{
+	return pp_r600_encode_lanes[num_lanes];
+}
+
+uint8_t decode_pcie_lane_width(uint32_t num_lanes)
+{
+	return pp_r600_decoded_lanes[num_lanes];
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.h b/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.h
new file mode 100644
index 000000000000..70b163b35570
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/pppcielanes.h
@@ -0,0 +1,31 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef PP_PCIELANES_H
+#define PP_PCIELANES_H
+
+extern uint8_t encode_pcie_lane_width(uint32_t num_lanes);
+extern uint8_t decode_pcie_lane_width(uint32_t num_lanes);
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.c b/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.c
new file mode 100644
index 000000000000..2f1a14fe05b1
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.c
@@ -0,0 +1,1688 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "processpptables.h"
+#include <atom-types.h>
+#include <atombios.h>
+#include "pp_debug.h"
+#include "pptable.h"
+#include "power_state.h"
+#include "hwmgr.h"
+#include "hardwaremanager.h"
+
+
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V2 12
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V3 14
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V4 16
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V5 18
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V6 20
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V7 22
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V8 24
+#define SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V9 26
+
+#define NUM_BITS_CLOCK_INFO_ARRAY_INDEX 6
+
+static uint16_t get_vce_table_offset(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t vce_table_offset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	   sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+			(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *extended_header =
+						(const ATOM_PPLIB_EXTENDEDHEADER *)
+						(((unsigned long)powerplay_table3) +
+						le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(extended_header->usSize) >=
+			   SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V2)
+				vce_table_offset = le16_to_cpu(extended_header->usVCETableOffset);
+		}
+	}
+
+	return vce_table_offset;
+}
+
+static uint16_t get_vce_clock_info_array_offset(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_table_offset(hwmgr,
+						powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset + 1;
+
+	return 0;
+}
+
+static uint16_t get_vce_clock_info_array_size(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_clock_info_array_offset(hwmgr,
+							powerplay_table);
+	uint16_t table_size = 0;
+
+	if (table_offset > 0) {
+		const VCEClockInfoArray *p = (const VCEClockInfoArray *)
+			(((unsigned long) powerplay_table) + table_offset);
+		table_size = sizeof(uint8_t) + p->ucNumEntries * sizeof(VCEClockInfo);
+	}
+
+	return table_size;
+}
+
+static uint16_t get_vce_clock_voltage_limit_table_offset(struct pp_hwmgr *hwmgr,
+				const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_clock_info_array_offset(hwmgr,
+							powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset + get_vce_clock_info_array_size(hwmgr,
+							powerplay_table);
+
+	return 0;
+}
+
+static uint16_t get_vce_clock_voltage_limit_table_size(struct pp_hwmgr *hwmgr,
+							const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_clock_voltage_limit_table_offset(hwmgr, powerplay_table);
+	uint16_t table_size = 0;
+
+	if (table_offset > 0) {
+		const ATOM_PPLIB_VCE_Clock_Voltage_Limit_Table *ptable =
+			(const ATOM_PPLIB_VCE_Clock_Voltage_Limit_Table *)(((unsigned long) powerplay_table) + table_offset);
+
+		table_size = sizeof(uint8_t) + ptable->numEntries * sizeof(ATOM_PPLIB_VCE_Clock_Voltage_Limit_Record);
+	}
+	return table_size;
+}
+
+static uint16_t get_vce_state_table_offset(struct pp_hwmgr *hwmgr, const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_clock_voltage_limit_table_offset(hwmgr, powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset + get_vce_clock_voltage_limit_table_size(hwmgr, powerplay_table);
+
+	return 0;
+}
+
+static const ATOM_PPLIB_VCE_State_Table *get_vce_state_table(
+						struct pp_hwmgr *hwmgr,
+						const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_vce_state_table_offset(hwmgr, powerplay_table);
+
+	if (table_offset > 0)
+		return (const ATOM_PPLIB_VCE_State_Table *)(((unsigned long) powerplay_table) + table_offset);
+
+	return NULL;
+}
+
+static uint16_t get_uvd_table_offset(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t uvd_table_offset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+			(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *extended_header =
+					(const ATOM_PPLIB_EXTENDEDHEADER *)
+					(((unsigned long)powerplay_table3) +
+				le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(extended_header->usSize) >=
+			    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V3)
+				uvd_table_offset = le16_to_cpu(extended_header->usUVDTableOffset);
+		}
+	}
+	return uvd_table_offset;
+}
+
+static uint16_t get_uvd_clock_info_array_offset(struct pp_hwmgr *hwmgr,
+			 const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_uvd_table_offset(hwmgr,
+						    powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset + 1;
+	return 0;
+}
+
+static uint16_t get_uvd_clock_info_array_size(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_uvd_clock_info_array_offset(hwmgr,
+						    powerplay_table);
+	uint16_t table_size = 0;
+
+	if (table_offset > 0) {
+		const UVDClockInfoArray *p = (const UVDClockInfoArray *)
+					(((unsigned long) powerplay_table)
+					+ table_offset);
+		table_size = sizeof(UCHAR) +
+			     p->ucNumEntries * sizeof(UVDClockInfo);
+	}
+
+	return table_size;
+}
+
+static uint16_t get_uvd_clock_voltage_limit_table_offset(
+			struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_uvd_clock_info_array_offset(hwmgr,
+						     powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset +
+			get_uvd_clock_info_array_size(hwmgr, powerplay_table);
+
+	return 0;
+}
+
+static uint16_t get_samu_table_offset(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t samu_table_offset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+			(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *extended_header =
+				(const ATOM_PPLIB_EXTENDEDHEADER *)
+				(((unsigned long)powerplay_table3) +
+				le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(extended_header->usSize) >=
+			    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V4)
+				samu_table_offset = le16_to_cpu(extended_header->usSAMUTableOffset);
+		}
+	}
+
+	return samu_table_offset;
+}
+
+static uint16_t get_samu_clock_voltage_limit_table_offset(
+			struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t table_offset = get_samu_table_offset(hwmgr,
+					    powerplay_table);
+
+	if (table_offset > 0)
+		return table_offset + 1;
+
+	return 0;
+}
+
+static uint16_t get_acp_table_offset(struct pp_hwmgr *hwmgr,
+				const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t acp_table_offset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+			(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *pExtendedHeader =
+				(const ATOM_PPLIB_EXTENDEDHEADER *)
+				(((unsigned long)powerplay_table3) +
+				le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(pExtendedHeader->usSize) >=
+			    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V6)
+				acp_table_offset = le16_to_cpu(pExtendedHeader->usACPTableOffset);
+		}
+	}
+
+	return acp_table_offset;
+}
+
+static uint16_t get_acp_clock_voltage_limit_table_offset(
+				struct pp_hwmgr *hwmgr,
+				const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t tableOffset = get_acp_table_offset(hwmgr, powerplay_table);
+
+	if (tableOffset > 0)
+		return tableOffset + 1;
+
+	return 0;
+}
+
+static uint16_t get_cacp_tdp_table_offset(
+				struct pp_hwmgr *hwmgr,
+				const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t cacTdpTableOffset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+				(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *pExtendedHeader =
+					(const ATOM_PPLIB_EXTENDEDHEADER *)
+					(((unsigned long)powerplay_table3) +
+				le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(pExtendedHeader->usSize) >=
+			    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V7)
+				cacTdpTableOffset = le16_to_cpu(pExtendedHeader->usPowerTuneTableOffset);
+		}
+	}
+
+	return cacTdpTableOffset;
+}
+
+static int get_cac_tdp_table(struct pp_hwmgr *hwmgr,
+				struct phm_cac_tdp_table **ptable,
+				const ATOM_PowerTune_Table *table,
+				uint16_t us_maximum_power_delivery_limit)
+{
+	unsigned long table_size;
+	struct phm_cac_tdp_table *tdp_table;
+
+	table_size = sizeof(unsigned long) + sizeof(struct phm_cac_tdp_table);
+
+	tdp_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == tdp_table)
+		return -ENOMEM;
+
+	tdp_table->usTDP = le16_to_cpu(table->usTDP);
+	tdp_table->usConfigurableTDP = le16_to_cpu(table->usConfigurableTDP);
+	tdp_table->usTDC = le16_to_cpu(table->usTDC);
+	tdp_table->usBatteryPowerLimit = le16_to_cpu(table->usBatteryPowerLimit);
+	tdp_table->usSmallPowerLimit = le16_to_cpu(table->usSmallPowerLimit);
+	tdp_table->usLowCACLeakage = le16_to_cpu(table->usLowCACLeakage);
+	tdp_table->usHighCACLeakage = le16_to_cpu(table->usHighCACLeakage);
+	tdp_table->usMaximumPowerDeliveryLimit = us_maximum_power_delivery_limit;
+
+	*ptable = tdp_table;
+
+	return 0;
+}
+
+static uint16_t get_sclk_vdd_gfx_table_offset(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t sclk_vdd_gfx_table_offset = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3)) {
+		const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3 =
+				(const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+		if (powerplay_table3->usExtendendedHeaderOffset > 0) {
+			const ATOM_PPLIB_EXTENDEDHEADER  *pExtendedHeader =
+				(const ATOM_PPLIB_EXTENDEDHEADER *)
+				(((unsigned long)powerplay_table3) +
+				le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+			if (le16_to_cpu(pExtendedHeader->usSize) >=
+			    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V8)
+				sclk_vdd_gfx_table_offset =
+					le16_to_cpu(pExtendedHeader->usSclkVddgfxTableOffset);
+		}
+	}
+
+	return sclk_vdd_gfx_table_offset;
+}
+
+static uint16_t get_sclk_vdd_gfx_clock_voltage_dependency_table_offset(
+			struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	uint16_t tableOffset = get_sclk_vdd_gfx_table_offset(hwmgr, powerplay_table);
+
+	if (tableOffset > 0)
+		return tableOffset;
+
+	return 0;
+}
+
+
+static int get_clock_voltage_dependency_table(struct pp_hwmgr *hwmgr,
+		struct phm_clock_voltage_dependency_table **ptable,
+		const ATOM_PPLIB_Clock_Voltage_Dependency_Table *table)
+{
+
+	unsigned long table_size, i;
+	struct phm_clock_voltage_dependency_table *dep_table;
+
+	table_size = sizeof(unsigned long) +
+		sizeof(struct phm_clock_voltage_dependency_table)
+		* table->ucNumEntries;
+
+	dep_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == dep_table)
+		return -ENOMEM;
+
+	dep_table->count = (unsigned long)table->ucNumEntries;
+
+	for (i = 0; i < dep_table->count; i++) {
+		dep_table->entries[i].clk =
+			((unsigned long)table->entries[i].ucClockHigh << 16) |
+			le16_to_cpu(table->entries[i].usClockLow);
+			dep_table->entries[i].v =
+				(unsigned long)le16_to_cpu(table->entries[i].usVoltage);
+	}
+
+	*ptable = dep_table;
+
+	return 0;
+}
+
+static int get_valid_clk(struct pp_hwmgr *hwmgr,
+			struct phm_clock_array **ptable,
+			const struct phm_clock_voltage_dependency_table *table)
+{
+	unsigned long table_size, i;
+	struct phm_clock_array *clock_table;
+
+	table_size = sizeof(unsigned long) + sizeof(unsigned long) * table->count;
+	clock_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == clock_table)
+		return -ENOMEM;
+
+	clock_table->count = (unsigned long)table->count;
+
+	for (i = 0; i < clock_table->count; i++)
+		clock_table->values[i] = (unsigned long)table->entries[i].clk;
+
+	*ptable = clock_table;
+
+	return 0;
+}
+
+static int get_clock_voltage_limit(struct pp_hwmgr *hwmgr,
+			struct phm_clock_and_voltage_limits *limits,
+			const ATOM_PPLIB_Clock_Voltage_Limit_Table *table)
+{
+	limits->sclk = ((unsigned long)table->entries[0].ucSclkHigh << 16) |
+			le16_to_cpu(table->entries[0].usSclkLow);
+	limits->mclk = ((unsigned long)table->entries[0].ucMclkHigh << 16) |
+			le16_to_cpu(table->entries[0].usMclkLow);
+	limits->vddc = (unsigned long)le16_to_cpu(table->entries[0].usVddc);
+	limits->vddci = (unsigned long)le16_to_cpu(table->entries[0].usVddci);
+
+	return 0;
+}
+
+
+static void set_hw_cap(struct pp_hwmgr *hwmgr, bool enable,
+		       enum phm_platform_caps cap)
+{
+	if (enable)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps, cap);
+	else
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps, cap);
+}
+
+static int set_platform_caps(struct pp_hwmgr *hwmgr,
+			unsigned long powerplay_caps)
+{
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_POWERPLAY),
+		PHM_PlatformCaps_PowerPlaySupport
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_SBIOSPOWERSOURCE),
+		PHM_PlatformCaps_BiosPowerSourceControl
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_ASPM_L0s),
+		PHM_PlatformCaps_EnableASPML0s
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_ASPM_L1),
+		PHM_PlatformCaps_EnableASPML1
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_BACKBIAS),
+		PHM_PlatformCaps_EnableBackbias
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_HARDWAREDC),
+		PHM_PlatformCaps_AutomaticDCTransition
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_GEMINIPRIMARY),
+		PHM_PlatformCaps_GeminiPrimary
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_STEPVDDC),
+		PHM_PlatformCaps_StepVddc
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_VOLTAGECONTROL),
+		PHM_PlatformCaps_EnableVoltageControl
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_SIDEPORTCONTROL),
+		PHM_PlatformCaps_EnableSideportControl
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_TURNOFFPLL_ASPML1),
+		PHM_PlatformCaps_TurnOffPll_ASPML1
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_HTLINKCONTROL),
+		PHM_PlatformCaps_EnableHTLinkControl
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_MVDDCONTROL),
+		PHM_PlatformCaps_EnableMVDDControl
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_VDDCI_CONTROL),
+		PHM_PlatformCaps_ControlVDDCI
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_REGULATOR_HOT),
+		PHM_PlatformCaps_RegulatorHot
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_GOTO_BOOT_ON_ALERT),
+		PHM_PlatformCaps_BootStateOnAlert
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_DONT_WAIT_FOR_VBLANK_ON_ALERT),
+		PHM_PlatformCaps_DontWaitForVBlankOnAlert
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_BACO),
+		PHM_PlatformCaps_BACO
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_NEW_CAC_VOLTAGE),
+		PHM_PlatformCaps_NewCACVoltage
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_REVERT_GPIO5_POLARITY),
+		PHM_PlatformCaps_RevertGPIO5Polarity
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_OUTPUT_THERMAL2GPIO17),
+		PHM_PlatformCaps_Thermal2GPIO17
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_VRHOT_GPIO_CONFIGURABLE),
+		PHM_PlatformCaps_VRHotGPIOConfigurable
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_TEMP_INVERSION),
+		PHM_PlatformCaps_TempInversion
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_EVV),
+		PHM_PlatformCaps_EVV
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_COMBINE_PCC_WITH_THERMAL_SIGNAL),
+		PHM_PlatformCaps_CombinePCCWithThermalSignal
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_LOAD_POST_PRODUCTION_FIRMWARE),
+		PHM_PlatformCaps_LoadPostProductionFirmware
+	);
+
+	set_hw_cap(
+		hwmgr,
+		0 != (powerplay_caps & ATOM_PP_PLATFORM_CAP_DISABLE_USING_ACTUAL_TEMPERATURE_FOR_POWER_CALC),
+		PHM_PlatformCaps_DisableUsingActualTemperatureForPowerCalc
+	);
+
+	return 0;
+}
+
+static PP_StateClassificationFlags make_classification_flags(
+						   struct pp_hwmgr *hwmgr,
+						    USHORT classification,
+						   USHORT classification2)
+{
+	PP_StateClassificationFlags result = 0;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_BOOT)
+		result |= PP_StateClassificationFlag_Boot;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_THERMAL)
+		result |= PP_StateClassificationFlag_Thermal;
+
+	if (classification &
+			ATOM_PPLIB_CLASSIFICATION_LIMITEDPOWERSOURCE)
+		result |= PP_StateClassificationFlag_LimitedPowerSource;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_REST)
+		result |= PP_StateClassificationFlag_Rest;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_FORCED)
+		result |= PP_StateClassificationFlag_Forced;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_3DPERFORMANCE)
+		result |= PP_StateClassificationFlag_3DPerformance;
+
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_OVERDRIVETEMPLATE)
+		result |= PP_StateClassificationFlag_ACOverdriveTemplate;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_UVDSTATE)
+		result |= PP_StateClassificationFlag_Uvd;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_HDSTATE)
+		result |= PP_StateClassificationFlag_UvdHD;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_SDSTATE)
+		result |= PP_StateClassificationFlag_UvdSD;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_HD2STATE)
+		result |= PP_StateClassificationFlag_HD2;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_ACPI)
+		result |= PP_StateClassificationFlag_ACPI;
+
+	if (classification2 & ATOM_PPLIB_CLASSIFICATION2_LIMITEDPOWERSOURCE_2)
+		result |= PP_StateClassificationFlag_LimitedPowerSource_2;
+
+
+	if (classification2 & ATOM_PPLIB_CLASSIFICATION2_ULV)
+		result |= PP_StateClassificationFlag_ULV;
+
+	if (classification2 & ATOM_PPLIB_CLASSIFICATION2_MVC)
+		result |= PP_StateClassificationFlag_UvdMVC;
+
+	return result;
+}
+
+static int init_non_clock_fields(struct pp_hwmgr *hwmgr,
+						struct pp_power_state *ps,
+							    uint8_t version,
+			 const ATOM_PPLIB_NONCLOCK_INFO *pnon_clock_info) {
+	unsigned long rrr_index;
+	unsigned long tmp;
+
+	ps->classification.ui_label = (le16_to_cpu(pnon_clock_info->usClassification) &
+					ATOM_PPLIB_CLASSIFICATION_UI_MASK) >> ATOM_PPLIB_CLASSIFICATION_UI_SHIFT;
+	ps->classification.flags = make_classification_flags(hwmgr,
+				le16_to_cpu(pnon_clock_info->usClassification),
+				le16_to_cpu(pnon_clock_info->usClassification2));
+
+	ps->classification.temporary_state = false;
+	ps->classification.to_be_deleted = false;
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_SINGLE_DISPLAY_ONLY;
+
+	ps->validation.singleDisplayOnly = (0 != tmp);
+
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_DISALLOW_ON_DC;
+
+	ps->validation.disallowOnDC = (0 != tmp);
+
+	ps->pcie.lanes = ((le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+				ATOM_PPLIB_PCIE_LINK_WIDTH_MASK) >>
+				ATOM_PPLIB_PCIE_LINK_WIDTH_SHIFT) + 1;
+
+	ps->pcie.lanes = 0;
+
+	ps->display.disableFrameModulation = false;
+
+	rrr_index = (le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+			ATOM_PPLIB_LIMITED_REFRESHRATE_VALUE_MASK) >>
+			ATOM_PPLIB_LIMITED_REFRESHRATE_VALUE_SHIFT;
+
+	if (rrr_index != ATOM_PPLIB_LIMITED_REFRESHRATE_UNLIMITED) {
+		static const uint8_t look_up[(ATOM_PPLIB_LIMITED_REFRESHRATE_VALUE_MASK >> ATOM_PPLIB_LIMITED_REFRESHRATE_VALUE_SHIFT) + 1] = \
+								{ 0, 50, 0 };
+
+		ps->display.refreshrateSource = PP_RefreshrateSource_Explicit;
+		ps->display.explicitRefreshrate = look_up[rrr_index];
+		ps->display.limitRefreshrate = true;
+
+		if (ps->display.explicitRefreshrate == 0)
+			ps->display.limitRefreshrate = false;
+	} else
+		ps->display.limitRefreshrate = false;
+
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_ENABLE_VARIBRIGHT;
+
+	ps->display.enableVariBright = (0 != tmp);
+
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_SWSTATE_MEMORY_DLL_OFF;
+
+	ps->memory.dllOff = (0 != tmp);
+
+	ps->memory.m3arb = (le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+			    ATOM_PPLIB_M3ARB_MASK) >> ATOM_PPLIB_M3ARB_SHIFT;
+
+	ps->temperatures.min = PP_TEMPERATURE_UNITS_PER_CENTIGRADES *
+				     pnon_clock_info->ucMinTemperature;
+
+	ps->temperatures.max = PP_TEMPERATURE_UNITS_PER_CENTIGRADES *
+				     pnon_clock_info->ucMaxTemperature;
+
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_SOFTWARE_DISABLE_LOADBALANCING;
+
+	ps->software.disableLoadBalancing = tmp;
+
+	tmp = le32_to_cpu(pnon_clock_info->ulCapsAndSettings) &
+		ATOM_PPLIB_SOFTWARE_ENABLE_SLEEP_FOR_TIMESTAMPS;
+
+	ps->software.enableSleepForTimestamps = (0 != tmp);
+
+	ps->validation.supportedPowerLevels = pnon_clock_info->ucRequiredPower;
+
+	if (ATOM_PPLIB_NONCLOCKINFO_VER1 < version) {
+		ps->uvd_clocks.VCLK = pnon_clock_info->ulVCLK;
+		ps->uvd_clocks.DCLK = pnon_clock_info->ulDCLK;
+	} else {
+		ps->uvd_clocks.VCLK = 0;
+		ps->uvd_clocks.DCLK = 0;
+	}
+
+	return 0;
+}
+
+static ULONG size_of_entry_v2(ULONG num_dpm_levels)
+{
+	return (sizeof(UCHAR) + sizeof(UCHAR) +
+			(num_dpm_levels * sizeof(UCHAR)));
+}
+
+static const ATOM_PPLIB_STATE_V2 *get_state_entry_v2(
+					const StateArray * pstate_arrays,
+							 ULONG entry_index)
+{
+	ULONG i;
+	const ATOM_PPLIB_STATE_V2 *pstate;
+
+	pstate = pstate_arrays->states;
+	if (entry_index <= pstate_arrays->ucNumEntries) {
+		for (i = 0; i < entry_index; i++)
+			pstate = (ATOM_PPLIB_STATE_V2 *)(
+						  (unsigned long)pstate +
+			     size_of_entry_v2(pstate->ucNumDPMLevels));
+	}
+	return pstate;
+}
+
+
+static const ATOM_PPLIB_POWERPLAYTABLE *get_powerplay_table(
+				     struct pp_hwmgr *hwmgr)
+{
+	const void *table_addr = NULL;
+	uint8_t frev, crev;
+	uint16_t size;
+
+	table_addr = cgs_atom_get_data_table(hwmgr->device,
+			GetIndexIntoMasterTable(DATA, PowerPlayInfo),
+			&size, &frev, &crev);
+
+	hwmgr->soft_pp_table = table_addr;
+
+	return (const ATOM_PPLIB_POWERPLAYTABLE *)table_addr;
+}
+
+
+int pp_tables_get_num_of_entries(struct pp_hwmgr *hwmgr,
+				     unsigned long *num_of_entries)
+{
+	const StateArray *pstate_arrays;
+	const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table = get_powerplay_table(hwmgr);
+
+	if (powerplay_table == NULL)
+		return -1;
+
+	if (powerplay_table->sHeader.ucTableFormatRevision >= 6) {
+		pstate_arrays = (StateArray *)(((unsigned long)powerplay_table) +
+					le16_to_cpu(powerplay_table->usStateArrayOffset));
+
+		*num_of_entries = (unsigned long)(pstate_arrays->ucNumEntries);
+	} else
+		*num_of_entries = (unsigned long)(powerplay_table->ucNumStates);
+
+	return 0;
+}
+
+int pp_tables_get_entry(struct pp_hwmgr *hwmgr,
+				unsigned long entry_index,
+				struct pp_power_state *ps,
+			 pp_tables_hw_clock_info_callback func)
+{
+	int i;
+	const StateArray *pstate_arrays;
+	const ATOM_PPLIB_STATE_V2 *pstate_entry_v2;
+	const ATOM_PPLIB_NONCLOCK_INFO *pnon_clock_info;
+	const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table = get_powerplay_table(hwmgr);
+	int result = 0;
+	int res = 0;
+
+	const ClockInfoArray *pclock_arrays;
+
+	const NonClockInfoArray *pnon_clock_arrays;
+
+	const ATOM_PPLIB_STATE *pstate_entry;
+
+	if (powerplay_table == NULL)
+		return -1;
+
+	ps->classification.bios_index = entry_index;
+
+	if (powerplay_table->sHeader.ucTableFormatRevision >= 6) {
+		pstate_arrays = (StateArray *)(((unsigned long)powerplay_table) +
+					le16_to_cpu(powerplay_table->usStateArrayOffset));
+
+		if (entry_index > pstate_arrays->ucNumEntries)
+			return -1;
+
+		pstate_entry_v2 = get_state_entry_v2(pstate_arrays, entry_index);
+		pclock_arrays = (ClockInfoArray *)(((unsigned long)powerplay_table) +
+					le16_to_cpu(powerplay_table->usClockInfoArrayOffset));
+
+		pnon_clock_arrays = (NonClockInfoArray *)(((unsigned long)powerplay_table) +
+						le16_to_cpu(powerplay_table->usNonClockInfoArrayOffset));
+
+		pnon_clock_info = (ATOM_PPLIB_NONCLOCK_INFO *)((unsigned long)(pnon_clock_arrays->nonClockInfo) +
+					(pstate_entry_v2->nonClockInfoIndex * pnon_clock_arrays->ucEntrySize));
+
+		result = init_non_clock_fields(hwmgr, ps, pnon_clock_arrays->ucEntrySize, pnon_clock_info);
+
+		for (i = 0; i < pstate_entry_v2->ucNumDPMLevels; i++) {
+			const void *pclock_info = (const void *)(
+							(unsigned long)(pclock_arrays->clockInfo) +
+							(pstate_entry_v2->clockInfoIndex[i] * pclock_arrays->ucEntrySize));
+			res = func(hwmgr, &ps->hardware, i, pclock_info);
+			if ((0 == result) && (0 != res))
+				result = res;
+		}
+	} else {
+		if (entry_index > powerplay_table->ucNumStates)
+			return -1;
+
+		pstate_entry = (ATOM_PPLIB_STATE *)((unsigned long)powerplay_table + powerplay_table->usStateArrayOffset +
+				entry_index * powerplay_table->ucStateEntrySize);
+
+		pnon_clock_info = (ATOM_PPLIB_NONCLOCK_INFO *)((unsigned long)powerplay_table +
+						le16_to_cpu(powerplay_table->usNonClockInfoArrayOffset) +
+						pstate_entry->ucNonClockStateIndex *
+						powerplay_table->ucNonClockSize);
+
+		result = init_non_clock_fields(hwmgr, ps,
+							powerplay_table->ucNonClockSize,
+							pnon_clock_info);
+
+		for (i = 0; i < powerplay_table->ucStateEntrySize-1; i++) {
+			const void *pclock_info = (const void *)((unsigned long)powerplay_table +
+						le16_to_cpu(powerplay_table->usClockInfoArrayOffset) +
+						pstate_entry->ucClockStateIndices[i] *
+						powerplay_table->ucClockInfoSize);
+
+			int res = func(hwmgr, &ps->hardware, i, pclock_info);
+
+			if ((0 == result) && (0 != res))
+					result = res;
+		}
+	}
+
+	if ((0 == result) &&
+		(0 != (ps->classification.flags & PP_StateClassificationFlag_Boot)))
+		result = hwmgr->hwmgr_func->patch_boot_state(hwmgr, &(ps->hardware));
+
+	return result;
+}
+
+
+
+static int init_powerplay_tables(
+			struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table
+)
+{
+	return 0;
+}
+
+
+static int init_thermal_controller(
+			struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	return 0;
+}
+
+static int init_overdrive_limits_V1_4(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table,
+			const ATOM_FIRMWARE_INFO_V1_4 *fw_info)
+{
+	hwmgr->platform_descriptor.overdriveLimit.engineClock =
+				le32_to_cpu(fw_info->ulASICMaxEngineClock);
+
+	hwmgr->platform_descriptor.overdriveLimit.memoryClock =
+				le32_to_cpu(fw_info->ulASICMaxMemoryClock);
+
+	hwmgr->platform_descriptor.maxOverdriveVDDC =
+		le32_to_cpu(fw_info->ul3DAccelerationEngineClock) & 0x7FF;
+
+	hwmgr->platform_descriptor.minOverdriveVDDC =
+			   le16_to_cpu(fw_info->usBootUpVDDCVoltage);
+
+	hwmgr->platform_descriptor.maxOverdriveVDDC =
+			   le16_to_cpu(fw_info->usBootUpVDDCVoltage);
+
+	hwmgr->platform_descriptor.overdriveVDDCStep = 0;
+	return 0;
+}
+
+static int init_overdrive_limits_V2_1(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table,
+			const ATOM_FIRMWARE_INFO_V2_1 *fw_info)
+{
+	const ATOM_PPLIB_POWERPLAYTABLE3 *powerplay_table3;
+	const ATOM_PPLIB_EXTENDEDHEADER *header;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) <
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE3))
+		return 0;
+
+	powerplay_table3 = (const ATOM_PPLIB_POWERPLAYTABLE3 *)powerplay_table;
+
+	if (0 == powerplay_table3->usExtendendedHeaderOffset)
+		return 0;
+
+	header = (ATOM_PPLIB_EXTENDEDHEADER *)(((unsigned long) powerplay_table) +
+			le16_to_cpu(powerplay_table3->usExtendendedHeaderOffset));
+
+	hwmgr->platform_descriptor.overdriveLimit.engineClock = le32_to_cpu(header->ulMaxEngineClock);
+	hwmgr->platform_descriptor.overdriveLimit.memoryClock = le32_to_cpu(header->ulMaxMemoryClock);
+
+
+	hwmgr->platform_descriptor.minOverdriveVDDC = 0;
+	hwmgr->platform_descriptor.maxOverdriveVDDC = 0;
+	hwmgr->platform_descriptor.overdriveVDDCStep = 0;
+
+	return 0;
+}
+
+static int init_overdrive_limits(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	int result;
+	uint8_t frev, crev;
+	uint16_t size;
+
+	const ATOM_COMMON_TABLE_HEADER *fw_info = NULL;
+
+	hwmgr->platform_descriptor.overdriveLimit.engineClock = 0;
+	hwmgr->platform_descriptor.overdriveLimit.memoryClock = 0;
+	hwmgr->platform_descriptor.minOverdriveVDDC = 0;
+	hwmgr->platform_descriptor.maxOverdriveVDDC = 0;
+
+	/* We assume here that fw_info is unchanged if this call fails.*/
+	fw_info = cgs_atom_get_data_table(hwmgr->device,
+			 GetIndexIntoMasterTable(DATA, FirmwareInfo),
+			 &size, &frev, &crev);
+
+	if ((fw_info->ucTableFormatRevision == 1)
+		&& (fw_info->usStructureSize >= sizeof(ATOM_FIRMWARE_INFO_V1_4)))
+		result = init_overdrive_limits_V1_4(hwmgr,
+				powerplay_table,
+				(const ATOM_FIRMWARE_INFO_V1_4 *)fw_info);
+
+	else if ((fw_info->ucTableFormatRevision == 2)
+		&& (fw_info->usStructureSize >= sizeof(ATOM_FIRMWARE_INFO_V2_1)))
+		result = init_overdrive_limits_V2_1(hwmgr,
+				powerplay_table,
+				(const ATOM_FIRMWARE_INFO_V2_1 *)fw_info);
+
+	if (hwmgr->platform_descriptor.overdriveLimit.engineClock > 0
+		&& hwmgr->platform_descriptor.overdriveLimit.memoryClock > 0
+		&& !phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_OverdriveDisabledByPowerBudget))
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_ACOverdriveSupport);
+
+	return result;
+}
+
+static int get_uvd_clock_voltage_limit_table(struct pp_hwmgr *hwmgr,
+		struct phm_uvd_clock_voltage_dependency_table **ptable,
+		const ATOM_PPLIB_UVD_Clock_Voltage_Limit_Table *table,
+		const UVDClockInfoArray *array)
+{
+	unsigned long table_size, i;
+	struct phm_uvd_clock_voltage_dependency_table *uvd_table;
+
+	table_size = sizeof(unsigned long) +
+		 sizeof(struct phm_uvd_clock_voltage_dependency_table) *
+		 table->numEntries;
+
+	uvd_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == uvd_table)
+		return -ENOMEM;
+
+	uvd_table->count = table->numEntries;
+
+	for (i = 0; i < table->numEntries; i++) {
+		const UVDClockInfo *entry =
+			&array->entries[table->entries[i].ucUVDClockInfoIndex];
+		uvd_table->entries[i].v = (unsigned long)le16_to_cpu(table->entries[i].usVoltage);
+		uvd_table->entries[i].vclk = ((unsigned long)entry->ucVClkHigh << 16)
+					 | le16_to_cpu(entry->usVClkLow);
+		uvd_table->entries[i].dclk = ((unsigned long)entry->ucDClkHigh << 16)
+					 | le16_to_cpu(entry->usDClkLow);
+	}
+
+	*ptable = uvd_table;
+
+	return 0;
+}
+
+static int get_vce_clock_voltage_limit_table(struct pp_hwmgr *hwmgr,
+		struct phm_vce_clock_voltage_dependency_table **ptable,
+		const ATOM_PPLIB_VCE_Clock_Voltage_Limit_Table *table,
+		const VCEClockInfoArray    *array)
+{
+	unsigned long table_size, i;
+	struct phm_vce_clock_voltage_dependency_table *vce_table = NULL;
+
+	table_size = sizeof(unsigned long) +
+			sizeof(struct phm_vce_clock_voltage_dependency_table)
+			* table->numEntries;
+
+	vce_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == vce_table)
+		return -ENOMEM;
+
+	vce_table->count = table->numEntries;
+	for (i = 0; i < table->numEntries; i++) {
+		const VCEClockInfo *entry = &array->entries[table->entries[i].ucVCEClockInfoIndex];
+
+		vce_table->entries[i].v = (unsigned long)le16_to_cpu(table->entries[i].usVoltage);
+		vce_table->entries[i].evclk = ((unsigned long)entry->ucEVClkHigh << 16)
+					| le16_to_cpu(entry->usEVClkLow);
+		vce_table->entries[i].ecclk = ((unsigned long)entry->ucECClkHigh << 16)
+					| le16_to_cpu(entry->usECClkLow);
+	}
+
+	*ptable = vce_table;
+
+	return 0;
+}
+
+static int get_samu_clock_voltage_limit_table(struct pp_hwmgr *hwmgr,
+		 struct phm_samu_clock_voltage_dependency_table **ptable,
+		 const ATOM_PPLIB_SAMClk_Voltage_Limit_Table *table)
+{
+	unsigned long table_size, i;
+	struct phm_samu_clock_voltage_dependency_table *samu_table;
+
+	table_size = sizeof(unsigned long) +
+		sizeof(struct phm_samu_clock_voltage_dependency_table) *
+		table->numEntries;
+
+	samu_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == samu_table)
+		return -ENOMEM;
+
+	samu_table->count = table->numEntries;
+
+	for (i = 0; i < table->numEntries; i++) {
+		samu_table->entries[i].v = (unsigned long)le16_to_cpu(table->entries[i].usVoltage);
+		samu_table->entries[i].samclk = ((unsigned long)table->entries[i].ucSAMClockHigh << 16)
+					 | le16_to_cpu(table->entries[i].usSAMClockLow);
+	}
+
+	*ptable = samu_table;
+
+	return 0;
+}
+
+static int get_acp_clock_voltage_limit_table(struct pp_hwmgr *hwmgr,
+		struct phm_acp_clock_voltage_dependency_table **ptable,
+		const ATOM_PPLIB_ACPClk_Voltage_Limit_Table *table)
+{
+	unsigned table_size, i;
+	struct phm_acp_clock_voltage_dependency_table *acp_table;
+
+	table_size = sizeof(unsigned long) +
+		sizeof(struct phm_acp_clock_voltage_dependency_table) *
+		table->numEntries;
+
+	acp_table = kzalloc(table_size, GFP_KERNEL);
+	if (NULL == acp_table)
+		return -ENOMEM;
+
+	acp_table->count = (unsigned long)table->numEntries;
+
+	for (i = 0; i < table->numEntries; i++) {
+		acp_table->entries[i].v = (unsigned long)le16_to_cpu(table->entries[i].usVoltage);
+		acp_table->entries[i].acpclk = ((unsigned long)table->entries[i].ucACPClockHigh << 16)
+					 | le16_to_cpu(table->entries[i].usACPClockLow);
+	}
+
+	*ptable = acp_table;
+
+	return 0;
+}
+
+static int init_clock_voltage_dependency(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	ATOM_PPLIB_Clock_Voltage_Dependency_Table *table;
+	ATOM_PPLIB_Clock_Voltage_Limit_Table *limit_table;
+	int result = 0;
+
+	uint16_t vce_clock_info_array_offset;
+	uint16_t uvd_clock_info_array_offset;
+	uint16_t table_offset;
+
+	hwmgr->dyn_state.vddc_dependency_on_sclk = NULL;
+	hwmgr->dyn_state.vddci_dependency_on_mclk = NULL;
+	hwmgr->dyn_state.vddc_dependency_on_mclk = NULL;
+	hwmgr->dyn_state.vddc_dep_on_dal_pwrl = NULL;
+	hwmgr->dyn_state.mvdd_dependency_on_mclk = NULL;
+	hwmgr->dyn_state.vce_clock_voltage_dependency_table = NULL;
+	hwmgr->dyn_state.uvd_clock_voltage_dependency_table = NULL;
+	hwmgr->dyn_state.samu_clock_voltage_dependency_table = NULL;
+	hwmgr->dyn_state.acp_clock_voltage_dependency_table = NULL;
+	hwmgr->dyn_state.ppm_parameter_table = NULL;
+	hwmgr->dyn_state.vdd_gfx_dependency_on_sclk = NULL;
+
+	vce_clock_info_array_offset = get_vce_clock_info_array_offset(
+						hwmgr, powerplay_table);
+	table_offset = get_vce_clock_voltage_limit_table_offset(hwmgr,
+						powerplay_table);
+	if (vce_clock_info_array_offset > 0 && table_offset > 0) {
+		const VCEClockInfoArray *array = (const VCEClockInfoArray *)
+				(((unsigned long) powerplay_table) +
+				vce_clock_info_array_offset);
+		const ATOM_PPLIB_VCE_Clock_Voltage_Limit_Table *table =
+				(const ATOM_PPLIB_VCE_Clock_Voltage_Limit_Table *)
+				(((unsigned long) powerplay_table) + table_offset);
+		result = get_vce_clock_voltage_limit_table(hwmgr,
+				&hwmgr->dyn_state.vce_clock_voltage_dependency_table,
+				table, array);
+	}
+
+	uvd_clock_info_array_offset = get_uvd_clock_info_array_offset(hwmgr, powerplay_table);
+	table_offset = get_uvd_clock_voltage_limit_table_offset(hwmgr, powerplay_table);
+
+	if (uvd_clock_info_array_offset > 0 && table_offset > 0) {
+		const UVDClockInfoArray *array = (const UVDClockInfoArray *)
+				(((unsigned long) powerplay_table) +
+				uvd_clock_info_array_offset);
+		const ATOM_PPLIB_UVD_Clock_Voltage_Limit_Table *ptable =
+				(const ATOM_PPLIB_UVD_Clock_Voltage_Limit_Table *)
+				(((unsigned long) powerplay_table) + table_offset);
+		result = get_uvd_clock_voltage_limit_table(hwmgr,
+				&hwmgr->dyn_state.uvd_clock_voltage_dependency_table, ptable, array);
+	}
+
+	table_offset = get_samu_clock_voltage_limit_table_offset(hwmgr,
+							    powerplay_table);
+
+	if (table_offset > 0) {
+		const ATOM_PPLIB_SAMClk_Voltage_Limit_Table *ptable =
+				(const ATOM_PPLIB_SAMClk_Voltage_Limit_Table *)
+				(((unsigned long) powerplay_table) + table_offset);
+		result = get_samu_clock_voltage_limit_table(hwmgr,
+				&hwmgr->dyn_state.samu_clock_voltage_dependency_table, ptable);
+	}
+
+	table_offset = get_acp_clock_voltage_limit_table_offset(hwmgr,
+							     powerplay_table);
+
+	if (table_offset > 0) {
+		const ATOM_PPLIB_ACPClk_Voltage_Limit_Table *ptable =
+				(const ATOM_PPLIB_ACPClk_Voltage_Limit_Table *)
+				(((unsigned long) powerplay_table) + table_offset);
+		result = get_acp_clock_voltage_limit_table(hwmgr,
+				&hwmgr->dyn_state.acp_clock_voltage_dependency_table, ptable);
+	}
+
+	table_offset = get_cacp_tdp_table_offset(hwmgr, powerplay_table);
+	if (table_offset > 0) {
+		UCHAR rev_id = *(UCHAR *)(((unsigned long)powerplay_table) + table_offset);
+
+		if (rev_id > 0) {
+			const ATOM_PPLIB_POWERTUNE_Table_V1 *tune_table =
+				(const ATOM_PPLIB_POWERTUNE_Table_V1 *)
+				(((unsigned long) powerplay_table) + table_offset);
+			result = get_cac_tdp_table(hwmgr, &hwmgr->dyn_state.cac_dtp_table,
+				&tune_table->power_tune_table,
+				le16_to_cpu(tune_table->usMaximumPowerDeliveryLimit));
+			hwmgr->dyn_state.cac_dtp_table->usDefaultTargetOperatingTemp =
+				le16_to_cpu(tune_table->usTjMax);
+		} else {
+			const ATOM_PPLIB_POWERTUNE_Table *tune_table =
+				(const ATOM_PPLIB_POWERTUNE_Table *)
+				(((unsigned long) powerplay_table) + table_offset);
+			result = get_cac_tdp_table(hwmgr,
+				&hwmgr->dyn_state.cac_dtp_table,
+				&tune_table->power_tune_table, 255);
+		}
+	}
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+		sizeof(ATOM_PPLIB_POWERPLAYTABLE4)) {
+		const ATOM_PPLIB_POWERPLAYTABLE4 *powerplay_table4 =
+				(const ATOM_PPLIB_POWERPLAYTABLE4 *)powerplay_table;
+		if (0 != powerplay_table4->usVddcDependencyOnSCLKOffset) {
+			table = (ATOM_PPLIB_Clock_Voltage_Dependency_Table *)
+				(((unsigned long) powerplay_table4) +
+				powerplay_table4->usVddcDependencyOnSCLKOffset);
+			result = get_clock_voltage_dependency_table(hwmgr,
+				&hwmgr->dyn_state.vddc_dependency_on_sclk, table);
+		}
+
+		if (result == 0 && (0 != powerplay_table4->usVddciDependencyOnMCLKOffset)) {
+			table = (ATOM_PPLIB_Clock_Voltage_Dependency_Table *)
+				(((unsigned long) powerplay_table4) +
+				powerplay_table4->usVddciDependencyOnMCLKOffset);
+			result = get_clock_voltage_dependency_table(hwmgr,
+				&hwmgr->dyn_state.vddci_dependency_on_mclk, table);
+		}
+
+		if (result == 0 && (0 != powerplay_table4->usVddcDependencyOnMCLKOffset)) {
+			table = (ATOM_PPLIB_Clock_Voltage_Dependency_Table *)
+				(((unsigned long) powerplay_table4) +
+				powerplay_table4->usVddcDependencyOnMCLKOffset);
+			result = get_clock_voltage_dependency_table(hwmgr,
+				&hwmgr->dyn_state.vddc_dependency_on_mclk, table);
+		}
+
+		if (result == 0 && (0 != powerplay_table4->usMaxClockVoltageOnDCOffset)) {
+			limit_table = (ATOM_PPLIB_Clock_Voltage_Limit_Table *)
+				(((unsigned long) powerplay_table4) +
+				powerplay_table4->usMaxClockVoltageOnDCOffset);
+			result = get_clock_voltage_limit(hwmgr,
+				&hwmgr->dyn_state.max_clock_voltage_on_dc, limit_table);
+		}
+
+		if (result == 0 && (NULL != hwmgr->dyn_state.vddc_dependency_on_mclk) &&
+			(0 != hwmgr->dyn_state.vddc_dependency_on_mclk->count))
+			result = get_valid_clk(hwmgr, &hwmgr->dyn_state.valid_mclk_values,
+					hwmgr->dyn_state.vddc_dependency_on_mclk);
+
+		if(result == 0 && (NULL != hwmgr->dyn_state.vddc_dependency_on_sclk) &&
+			(0 != hwmgr->dyn_state.vddc_dependency_on_sclk->count))
+			result = get_valid_clk(hwmgr,
+				&hwmgr->dyn_state.valid_sclk_values,
+				hwmgr->dyn_state.vddc_dependency_on_sclk);
+
+		if (result == 0 && (0 != powerplay_table4->usMvddDependencyOnMCLKOffset)) {
+			table = (ATOM_PPLIB_Clock_Voltage_Dependency_Table *)
+				(((unsigned long) powerplay_table4) +
+				powerplay_table4->usMvddDependencyOnMCLKOffset);
+			result = get_clock_voltage_dependency_table(hwmgr,
+				&hwmgr->dyn_state.mvdd_dependency_on_mclk, table);
+		}
+	}
+
+	table_offset = get_sclk_vdd_gfx_clock_voltage_dependency_table_offset(hwmgr,
+								powerplay_table);
+
+	if (table_offset > 0) {
+		table = (ATOM_PPLIB_Clock_Voltage_Dependency_Table *)
+			(((unsigned long) powerplay_table) + table_offset);
+		result = get_clock_voltage_dependency_table(hwmgr,
+			&hwmgr->dyn_state.vdd_gfx_dependency_on_sclk, table);
+	}
+
+	return result;
+}
+
+static int get_cac_leakage_table(struct pp_hwmgr *hwmgr,
+				 struct phm_cac_leakage_table **ptable,
+				const ATOM_PPLIB_CAC_Leakage_Table *table)
+{
+	struct phm_cac_leakage_table  *cac_leakage_table;
+	unsigned long            table_size, i;
+
+	if (hwmgr == NULL || table == NULL || ptable == NULL)
+		return -EINVAL;
+
+	table_size = sizeof(ULONG) +
+		(sizeof(struct phm_cac_leakage_table) * table->ucNumEntries);
+
+	cac_leakage_table = kzalloc(table_size, GFP_KERNEL);
+
+	if (cac_leakage_table == NULL)
+		return -ENOMEM;
+
+	cac_leakage_table->count = (ULONG)table->ucNumEntries;
+
+	for (i = 0; i < cac_leakage_table->count; i++) {
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_EVV)) {
+			cac_leakage_table->entries[i].Vddc1 = le16_to_cpu(table->entries[i].usVddc1);
+			cac_leakage_table->entries[i].Vddc2 = le16_to_cpu(table->entries[i].usVddc2);
+			cac_leakage_table->entries[i].Vddc3 = le16_to_cpu(table->entries[i].usVddc3);
+		} else {
+			cac_leakage_table->entries[i].Vddc    = le16_to_cpu(table->entries[i].usVddc);
+			cac_leakage_table->entries[i].Leakage = le32_to_cpu(table->entries[i].ulLeakageValue);
+		}
+	}
+
+	*ptable = cac_leakage_table;
+
+	return 0;
+}
+
+static int get_platform_power_management_table(struct pp_hwmgr *hwmgr,
+			ATOM_PPLIB_PPM_Table *atom_ppm_table)
+{
+	struct phm_ppm_table *ptr = kzalloc(sizeof(struct phm_ppm_table), GFP_KERNEL);
+
+	if (NULL == ptr)
+		return -ENOMEM;
+
+	ptr->ppm_design            = atom_ppm_table->ucPpmDesign;
+	ptr->cpu_core_number        = le16_to_cpu(atom_ppm_table->usCpuCoreNumber);
+	ptr->platform_tdp          = le32_to_cpu(atom_ppm_table->ulPlatformTDP);
+	ptr->small_ac_platform_tdp   = le32_to_cpu(atom_ppm_table->ulSmallACPlatformTDP);
+	ptr->platform_tdc          = le32_to_cpu(atom_ppm_table->ulPlatformTDC);
+	ptr->small_ac_platform_tdc   = le32_to_cpu(atom_ppm_table->ulSmallACPlatformTDC);
+	ptr->apu_tdp               = le32_to_cpu(atom_ppm_table->ulApuTDP);
+	ptr->dgpu_tdp              = le32_to_cpu(atom_ppm_table->ulDGpuTDP);
+	ptr->dgpu_ulv_power         = le32_to_cpu(atom_ppm_table->ulDGpuUlvPower);
+	ptr->tj_max                = le32_to_cpu(atom_ppm_table->ulTjmax);
+	hwmgr->dyn_state.ppm_parameter_table = ptr;
+
+	return 0;
+}
+
+static int init_dpm2_parameters(struct pp_hwmgr *hwmgr,
+			const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	int result = 0;
+
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE5)) {
+		const  ATOM_PPLIB_POWERPLAYTABLE5 *ptable5 =
+				(const ATOM_PPLIB_POWERPLAYTABLE5 *)powerplay_table;
+		const  ATOM_PPLIB_POWERPLAYTABLE4 *ptable4 =
+				(const ATOM_PPLIB_POWERPLAYTABLE4 *)
+				(&ptable5->basicTable4);
+		const  ATOM_PPLIB_POWERPLAYTABLE3 *ptable3 =
+				(const ATOM_PPLIB_POWERPLAYTABLE3 *)
+				(&ptable4->basicTable3);
+		const  ATOM_PPLIB_EXTENDEDHEADER  *extended_header;
+		uint16_t table_offset;
+		ATOM_PPLIB_PPM_Table *atom_ppm_table;
+
+		hwmgr->platform_descriptor.TDPLimit     = le32_to_cpu(ptable5->ulTDPLimit);
+		hwmgr->platform_descriptor.nearTDPLimit = le32_to_cpu(ptable5->ulNearTDPLimit);
+
+		hwmgr->platform_descriptor.TDPODLimit   = le16_to_cpu(ptable5->usTDPODLimit);
+		hwmgr->platform_descriptor.TDPAdjustment = 0;
+
+		hwmgr->platform_descriptor.VidAdjustment = 0;
+		hwmgr->platform_descriptor.VidAdjustmentPolarity = 0;
+		hwmgr->platform_descriptor.VidMinLimit     = 0;
+		hwmgr->platform_descriptor.VidMaxLimit     = 1500000;
+		hwmgr->platform_descriptor.VidStep         = 6250;
+
+		hwmgr->platform_descriptor.nearTDPLimitAdjusted = le32_to_cpu(ptable5->ulNearTDPLimit);
+
+		if (hwmgr->platform_descriptor.TDPODLimit != 0)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_PowerControl);
+
+		hwmgr->platform_descriptor.SQRampingThreshold = le32_to_cpu(ptable5->ulSQRampingThreshold);
+
+		hwmgr->platform_descriptor.CACLeakage = le32_to_cpu(ptable5->ulCACLeakage);
+
+		hwmgr->dyn_state.cac_leakage_table = NULL;
+
+		if (0 != ptable5->usCACLeakageTableOffset) {
+			const ATOM_PPLIB_CAC_Leakage_Table *pCAC_leakage_table =
+				(ATOM_PPLIB_CAC_Leakage_Table *)(((unsigned long)ptable5) +
+				le16_to_cpu(ptable5->usCACLeakageTableOffset));
+			result = get_cac_leakage_table(hwmgr,
+				&hwmgr->dyn_state.cac_leakage_table, pCAC_leakage_table);
+		}
+
+		hwmgr->platform_descriptor.LoadLineSlope = le16_to_cpu(ptable5->usLoadLineSlope);
+
+		hwmgr->dyn_state.ppm_parameter_table = NULL;
+
+		if (0 != ptable3->usExtendendedHeaderOffset) {
+			extended_header = (const ATOM_PPLIB_EXTENDEDHEADER *)
+					(((unsigned long)powerplay_table) +
+					le16_to_cpu(ptable3->usExtendendedHeaderOffset));
+			if ((extended_header->usPPMTableOffset > 0) &&
+				le16_to_cpu(extended_header->usSize) >=
+				    SIZE_OF_ATOM_PPLIB_EXTENDEDHEADER_V5) {
+				table_offset = le16_to_cpu(extended_header->usPPMTableOffset);
+				atom_ppm_table = (ATOM_PPLIB_PPM_Table *)
+					(((unsigned long)powerplay_table) + table_offset);
+				if (0 == get_platform_power_management_table(hwmgr, atom_ppm_table))
+					phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+						PHM_PlatformCaps_EnablePlatformPowerManagement);
+			}
+		}
+	}
+	return result;
+}
+
+static int init_phase_shedding_table(struct pp_hwmgr *hwmgr,
+		const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table)
+{
+	if (le16_to_cpu(powerplay_table->usTableSize) >=
+	    sizeof(ATOM_PPLIB_POWERPLAYTABLE4)) {
+		const ATOM_PPLIB_POWERPLAYTABLE4 *powerplay_table4 =
+				(const ATOM_PPLIB_POWERPLAYTABLE4 *)powerplay_table;
+
+		if (0 != powerplay_table4->usVddcPhaseShedLimitsTableOffset) {
+			const ATOM_PPLIB_PhaseSheddingLimits_Table *ptable =
+				(ATOM_PPLIB_PhaseSheddingLimits_Table *)
+				(((unsigned long)powerplay_table4) +
+				le16_to_cpu(powerplay_table4->usVddcPhaseShedLimitsTableOffset));
+			struct phm_phase_shedding_limits_table *table;
+			unsigned long size, i;
+
+
+			size = sizeof(unsigned long) +
+				(sizeof(struct phm_phase_shedding_limits_table) *
+				ptable->ucNumEntries);
+
+			table = kzalloc(size, GFP_KERNEL);
+
+			if (table == NULL)
+				return -ENOMEM;
+
+			table->count = (unsigned long)ptable->ucNumEntries;
+
+			for (i = 0; i < table->count; i++) {
+				table->entries[i].Voltage = (unsigned long)le16_to_cpu(ptable->entries[i].usVoltage);
+				table->entries[i].Sclk    = ((unsigned long)ptable->entries[i].ucSclkHigh << 16)
+							| le16_to_cpu(ptable->entries[i].usSclkLow);
+				table->entries[i].Mclk    = ((unsigned long)ptable->entries[i].ucMclkHigh << 16)
+							| le16_to_cpu(ptable->entries[i].usMclkLow);
+			}
+			hwmgr->dyn_state.vddc_phase_shed_limits_table = table;
+		}
+	}
+
+	return 0;
+}
+
+int get_number_of_vce_state_table_entries(
+						  struct pp_hwmgr *hwmgr)
+{
+	const ATOM_PPLIB_POWERPLAYTABLE *table =
+					     get_powerplay_table(hwmgr);
+	const ATOM_PPLIB_VCE_State_Table *vce_table =
+				    get_vce_state_table(hwmgr, table);
+
+	if (vce_table > 0)
+		return vce_table->numEntries;
+
+	return 0;
+}
+
+int get_vce_state_table_entry(struct pp_hwmgr *hwmgr,
+							unsigned long i,
+							struct PP_VCEState *vce_state,
+							void **clock_info,
+							unsigned long *flag)
+{
+	const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table = get_powerplay_table(hwmgr);
+
+	const ATOM_PPLIB_VCE_State_Table *vce_state_table = get_vce_state_table(hwmgr, powerplay_table);
+
+	unsigned short vce_clock_info_array_offset = get_vce_clock_info_array_offset(hwmgr, powerplay_table);
+
+	const VCEClockInfoArray *vce_clock_info_array = (const VCEClockInfoArray *)(((unsigned long) powerplay_table) + vce_clock_info_array_offset);
+
+	const ClockInfoArray *clock_arrays = (ClockInfoArray *)(((unsigned long)powerplay_table) + powerplay_table->usClockInfoArrayOffset);
+
+	const ATOM_PPLIB_VCE_State_Record *record = &vce_state_table->entries[i];
+
+	const VCEClockInfo *vce_clock_info = &vce_clock_info_array->entries[record->ucVCEClockInfoIndex];
+
+	unsigned long clockInfoIndex = record->ucClockInfoIndex & 0x3F;
+
+	*flag = (record->ucClockInfoIndex >> NUM_BITS_CLOCK_INFO_ARRAY_INDEX);
+
+	vce_state->evclk = ((uint32_t)vce_clock_info->ucEVClkHigh << 16) | vce_clock_info->usEVClkLow;
+	vce_state->ecclk = ((uint32_t)vce_clock_info->ucECClkHigh << 16) | vce_clock_info->usECClkLow;
+
+	*clock_info = (void *)((unsigned long)(clock_arrays->clockInfo) + (clockInfoIndex * clock_arrays->ucEntrySize));
+
+	return 0;
+}
+
+
+static int pp_tables_initialize(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	const ATOM_PPLIB_POWERPLAYTABLE *powerplay_table;
+
+	hwmgr->need_pp_table_upload = true;
+
+	powerplay_table = get_powerplay_table(hwmgr);
+
+	result = init_powerplay_tables(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_powerplay_tables failed", return result);
+
+	result = set_platform_caps(hwmgr,
+				le32_to_cpu(powerplay_table->ulPlatformCaps));
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "set_platform_caps failed", return result);
+
+	result = init_thermal_controller(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_thermal_controller failed", return result);
+
+	result = init_overdrive_limits(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_overdrive_limits failed", return result);
+
+	result = init_clock_voltage_dependency(hwmgr,
+					       powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_clock_voltage_dependency failed", return result);
+
+	result = init_dpm2_parameters(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_dpm2_parameters failed", return result);
+
+	result = init_phase_shedding_table(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_phase_shedding_table failed", return result);
+
+	return result;
+}
+
+static int pp_tables_uninitialize(struct pp_hwmgr *hwmgr)
+{
+	if (NULL != hwmgr->soft_pp_table) {
+		kfree(hwmgr->soft_pp_table);
+		hwmgr->soft_pp_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vddc_dependency_on_sclk) {
+		kfree(hwmgr->dyn_state.vddc_dependency_on_sclk);
+		hwmgr->dyn_state.vddc_dependency_on_sclk = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vddci_dependency_on_mclk) {
+		kfree(hwmgr->dyn_state.vddci_dependency_on_mclk);
+		hwmgr->dyn_state.vddci_dependency_on_mclk = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vddc_dependency_on_mclk) {
+		kfree(hwmgr->dyn_state.vddc_dependency_on_mclk);
+		hwmgr->dyn_state.vddc_dependency_on_mclk = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.mvdd_dependency_on_mclk) {
+		kfree(hwmgr->dyn_state.mvdd_dependency_on_mclk);
+		hwmgr->dyn_state.mvdd_dependency_on_mclk = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.valid_mclk_values) {
+		kfree(hwmgr->dyn_state.valid_mclk_values);
+		hwmgr->dyn_state.valid_mclk_values = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.valid_sclk_values) {
+		kfree(hwmgr->dyn_state.valid_sclk_values);
+		hwmgr->dyn_state.valid_sclk_values = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.cac_leakage_table) {
+		kfree(hwmgr->dyn_state.cac_leakage_table);
+		hwmgr->dyn_state.cac_leakage_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vddc_phase_shed_limits_table) {
+		kfree(hwmgr->dyn_state.vddc_phase_shed_limits_table);
+		hwmgr->dyn_state.vddc_phase_shed_limits_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vce_clock_voltage_dependency_table) {
+		kfree(hwmgr->dyn_state.vce_clock_voltage_dependency_table);
+		hwmgr->dyn_state.vce_clock_voltage_dependency_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.uvd_clock_voltage_dependency_table) {
+		kfree(hwmgr->dyn_state.uvd_clock_voltage_dependency_table);
+		hwmgr->dyn_state.uvd_clock_voltage_dependency_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.samu_clock_voltage_dependency_table) {
+		kfree(hwmgr->dyn_state.samu_clock_voltage_dependency_table);
+		hwmgr->dyn_state.samu_clock_voltage_dependency_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.acp_clock_voltage_dependency_table) {
+		kfree(hwmgr->dyn_state.acp_clock_voltage_dependency_table);
+		hwmgr->dyn_state.acp_clock_voltage_dependency_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.cac_dtp_table) {
+		kfree(hwmgr->dyn_state.cac_dtp_table);
+		hwmgr->dyn_state.cac_dtp_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.ppm_parameter_table) {
+		kfree(hwmgr->dyn_state.ppm_parameter_table);
+		hwmgr->dyn_state.ppm_parameter_table = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vdd_gfx_dependency_on_sclk) {
+		kfree(hwmgr->dyn_state.vdd_gfx_dependency_on_sclk);
+		hwmgr->dyn_state.vdd_gfx_dependency_on_sclk = NULL;
+	}
+
+	if (NULL != hwmgr->dyn_state.vq_budgeting_table) {
+		kfree(hwmgr->dyn_state.vq_budgeting_table);
+		hwmgr->dyn_state.vq_budgeting_table = NULL;
+	}
+
+	return 0;
+}
+
+const struct pp_table_func pptable_funcs = {
+	.pptable_init = pp_tables_initialize,
+	.pptable_fini = pp_tables_uninitialize,
+	.pptable_get_number_of_vce_state_table_entries =
+				get_number_of_vce_state_table_entries,
+	.pptable_get_vce_state_table_entry =
+						get_vce_state_table_entry,
+};
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.h b/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.h
new file mode 100644
index 000000000000..30434802417e
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/processpptables.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ * Interface Functions related to the BIOS PowerPlay Tables.
+ *
+ */
+
+#ifndef PROCESSPPTABLES_H
+#define PROCESSPPTABLES_H
+
+struct pp_hwmgr;
+struct pp_power_state;
+struct pp_hw_power_state;
+
+extern const struct pp_table_func pptable_funcs;
+
+typedef int (*pp_tables_hw_clock_info_callback)(struct pp_hwmgr *hwmgr,
+					struct pp_hw_power_state *hw_ps,
+							unsigned int index,
+						 const void *clock_info);
+
+int pp_tables_get_num_of_entries(struct pp_hwmgr *hwmgr,
+				     unsigned long *num_of_entries);
+
+int pp_tables_get_entry(struct pp_hwmgr *hwmgr,
+						unsigned long entry_index,
+						struct pp_power_state *ps,
+				pp_tables_hw_clock_info_callback func);
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.c b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.c
new file mode 100644
index 000000000000..e58d038a997b
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.c
@@ -0,0 +1,350 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "hwmgr.h"
+#include "tonga_clockpowergating.h"
+#include "tonga_ppsmc.h"
+#include "tonga_hwmgr.h"
+
+int tonga_phm_powerdown_uvd(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cf_want_uvd_power_gating(hwmgr))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						     PPSMC_MSG_UVDPowerOFF);
+	return 0;
+}
+
+int tonga_phm_powerup_uvd(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cf_want_uvd_power_gating(hwmgr)) {
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				  PHM_PlatformCaps_UVDDynamicPowerGating)) {
+			return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+								PPSMC_MSG_UVDPowerON, 1);
+		} else {
+			return smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+								PPSMC_MSG_UVDPowerON, 0);
+		}
+	}
+
+	return 0;
+}
+
+int tonga_phm_powerdown_vce(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cf_want_vce_power_gating(hwmgr))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						  PPSMC_MSG_VCEPowerOFF);
+	return 0;
+}
+
+int tonga_phm_powerup_vce(struct pp_hwmgr *hwmgr)
+{
+	if (phm_cf_want_vce_power_gating(hwmgr))
+		return smum_send_msg_to_smc(hwmgr->smumgr,
+						  PPSMC_MSG_VCEPowerON);
+	return 0;
+}
+
+int tonga_phm_set_asic_block_gating(struct pp_hwmgr *hwmgr, enum PHM_AsicBlock block, enum PHM_ClockGateSetting gating)
+{
+	int ret = 0;
+
+	switch (block) {
+	case PHM_AsicBlock_UVD_MVC:
+	case PHM_AsicBlock_UVD:
+	case PHM_AsicBlock_UVD_HD:
+	case PHM_AsicBlock_UVD_SD:
+		if (gating == PHM_ClockGateSetting_StaticOff)
+			ret = tonga_phm_powerdown_uvd(hwmgr);
+		else
+			ret = tonga_phm_powerup_uvd(hwmgr);
+		break;
+	case PHM_AsicBlock_GFX:
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+int tonga_phm_disable_clock_power_gating(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	data->uvd_power_gated = false;
+	data->vce_power_gated = false;
+
+	tonga_phm_powerup_uvd(hwmgr);
+	tonga_phm_powerup_vce(hwmgr);
+
+	return 0;
+}
+
+int tonga_phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	if (data->uvd_power_gated == bgate)
+		return 0;
+
+	data->uvd_power_gated = bgate;
+
+	if (bgate) {
+		cgs_set_clockgating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_CG_STATE_UNGATE);
+		cgs_set_powergating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_PG_STATE_GATE);
+		tonga_update_uvd_dpm(hwmgr, true);
+		tonga_phm_powerdown_uvd(hwmgr);
+	} else {
+		tonga_phm_powerup_uvd(hwmgr);
+		cgs_set_powergating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_PG_STATE_UNGATE);
+		cgs_set_clockgating_state(hwmgr->device,
+						AMD_IP_BLOCK_TYPE_UVD,
+						AMD_PG_STATE_GATE);
+
+		tonga_update_uvd_dpm(hwmgr, false);
+	}
+
+	return 0;
+}
+
+int tonga_phm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct phm_set_power_state_input states;
+	const struct pp_power_state  *pcurrent;
+	struct pp_power_state  *requested;
+
+	pcurrent = hwmgr->current_ps;
+	requested = hwmgr->request_ps;
+
+	states.pcurrent_state = &(pcurrent->hardware);
+	states.pnew_state = &(requested->hardware);
+
+	if (phm_cf_want_vce_power_gating(hwmgr)) {
+		if (data->vce_power_gated != bgate) {
+			if (bgate) {
+				cgs_set_clockgating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_CG_STATE_UNGATE);
+				cgs_set_powergating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_PG_STATE_GATE);
+				tonga_enable_disable_vce_dpm(hwmgr, false);
+				data->vce_power_gated = true;
+			} else {
+				tonga_phm_powerup_vce(hwmgr);
+				data->vce_power_gated = false;
+				cgs_set_powergating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_PG_STATE_UNGATE);
+				cgs_set_clockgating_state(
+							hwmgr->device,
+							AMD_IP_BLOCK_TYPE_VCE,
+							AMD_PG_STATE_GATE);
+
+				tonga_update_vce_dpm(hwmgr, &states);
+				tonga_enable_disable_vce_dpm(hwmgr, true);
+				return 0;
+			}
+		}
+	} else {
+		tonga_update_vce_dpm(hwmgr, &states);
+		tonga_enable_disable_vce_dpm(hwmgr, true);
+		return 0;
+	}
+
+	if (!data->vce_power_gated)
+		tonga_update_vce_dpm(hwmgr, &states);
+
+	return 0;
+}
+
+int tonga_phm_update_clock_gatings(struct pp_hwmgr *hwmgr,
+					const uint32_t *msg_id)
+{
+	PPSMC_Msg msg;
+	uint32_t value;
+
+	switch ((*msg_id & PP_GROUP_MASK) >> PP_GROUP_SHIFT) {
+	case PP_GROUP_GFX:
+		switch ((*msg_id & PP_BLOCK_MASK) >> PP_BLOCK_SHIFT) {
+		case PP_BLOCK_GFX_CG:
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+				  ? PPSMC_MSG_EnableClockGatingFeature
+				  : PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_GFX_CGCG_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			if (PP_STATE_SUPPORT_LS & *msg_id) {
+				msg = (*msg_id & PP_STATE_MASK) & PP_STATE_LS
+					? PPSMC_MSG_EnableClockGatingFeature
+					: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_GFX_CGLS_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		case PP_BLOCK_GFX_MG:
+			/* For GFX MGCG, there are three different ones;
+			 * CPF, RLC, and all others.  CPF MGCG will not be used for Tonga.
+			 * For GFX MGLS, Tonga will not support it.
+			 * */
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = (CG_RLC_MGCG_MASK | CG_GFX_OTHERS_MGCG_MASK);
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		default:
+			return -1;
+		}
+		break;
+
+	case PP_GROUP_SYS:
+		switch ((*msg_id & PP_BLOCK_MASK) >> PP_BLOCK_SHIFT) {
+		case PP_BLOCK_SYS_BIF:
+			if (PP_STATE_SUPPORT_LS & *msg_id) {
+				msg = (*msg_id & PP_STATE_MASK) & PP_STATE_LS
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_BIF_MGLS_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		case PP_BLOCK_SYS_MC:
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_MC_MGCG_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+
+			if (PP_STATE_SUPPORT_LS & *msg_id) {
+				msg = (*msg_id & PP_STATE_MASK) & PP_STATE_LS
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_MC_MGLS_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+
+			}
+			break;
+
+		case PP_BLOCK_SYS_HDP:
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+					? PPSMC_MSG_EnableClockGatingFeature
+					: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_HDP_MGCG_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+
+			if (PP_STATE_SUPPORT_LS & *msg_id) {
+				msg = (*msg_id & PP_STATE_MASK) & PP_STATE_LS
+					? PPSMC_MSG_EnableClockGatingFeature
+					: PPSMC_MSG_DisableClockGatingFeature;
+
+				value = CG_SYS_HDP_MGLS_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		case PP_BLOCK_SYS_SDMA:
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_SDMA_MGCG_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+
+			if (PP_STATE_SUPPORT_LS & *msg_id) {
+				msg = (*msg_id & PP_STATE_MASK) & PP_STATE_LS
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+
+				value = CG_SYS_SDMA_MGLS_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		case PP_BLOCK_SYS_ROM:
+			if (PP_STATE_SUPPORT_CG & *msg_id) {
+				msg = ((*msg_id & PP_STATE_MASK) & PP_STATE_CG)
+				? PPSMC_MSG_EnableClockGatingFeature
+				: PPSMC_MSG_DisableClockGatingFeature;
+				value = CG_SYS_ROM_MASK;
+
+				if (0 != smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, msg, value))
+					return -1;
+			}
+			break;
+
+		default:
+			return -1;
+
+		}
+		break;
+
+	default:
+		return -1;
+
+	}
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.h
new file mode 100644
index 000000000000..8bc38cb17b7f
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_clockpowergating.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _TONGA_CLOCK_POWER_GATING_H_
+#define _TONGA_CLOCK_POWER_GATING_H_
+
+#include "tonga_hwmgr.h"
+#include "pp_asicblocks.h"
+
+extern int tonga_phm_set_asic_block_gating(struct pp_hwmgr *hwmgr, enum PHM_AsicBlock block, enum PHM_ClockGateSetting gating);
+extern int tonga_phm_powergate_vce(struct pp_hwmgr *hwmgr, bool bgate);
+extern int tonga_phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool bgate);
+extern int tonga_phm_powerdown_uvd(struct pp_hwmgr *hwmgr);
+extern int tonga_phm_disable_clock_power_gating(struct pp_hwmgr *hwmgr);
+extern int tonga_phm_update_clock_gatings(struct pp_hwmgr *hwmgr, const uint32_t *msg_id);
+#endif /* _TONGA_CLOCK_POWER_GATING_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_dyn_defaults.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_dyn_defaults.h
new file mode 100644
index 000000000000..080d69d77f04
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_dyn_defaults.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef TONGA_DYN_DEFAULTS_H
+#define TONGA_DYN_DEFAULTS_H
+
+
+/** \file
+ * Volcanic Islands Dynamic default parameters.
+ */
+
+enum TONGAdpm_TrendDetection {
+	TONGAdpm_TrendDetection_AUTO,
+	TONGAdpm_TrendDetection_UP,
+	TONGAdpm_TrendDetection_DOWN
+};
+typedef enum TONGAdpm_TrendDetection TONGAdpm_TrendDetection;
+
+/* Bit vector representing same fields as hardware register. */
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT0              0x3FFFC102  /* CP_Gfx_busy */
+/* HDP_busy */
+/* IH_busy */
+/* DRM_busy */
+/* DRMDMA_busy */
+/* UVD_busy */
+/* VCE_busy */
+/* ACP_busy */
+/* SAMU_busy */
+/* AVP_busy  */
+/* SDMA enabled */
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT1              0x000400  /* FE_Gfx_busy  - Intended for primary usage.   Rest are for flexibility. */
+/* SH_Gfx_busy */
+/* RB_Gfx_busy */
+/* VCE_busy */
+
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT2              0xC00080  /* SH_Gfx_busy - Intended for primary usage.   Rest are for flexibility. */
+/* FE_Gfx_busy */
+/* RB_Gfx_busy */
+/* ACP_busy */
+
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT3              0xC00200  /* RB_Gfx_busy - Intended for primary usage.   Rest are for flexibility. */
+/* FE_Gfx_busy */
+/* SH_Gfx_busy */
+/* UVD_busy */
+
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT4              0xC01680  /* UVD_busy */
+/* VCE_busy */
+/* ACP_busy */
+/* SAMU_busy */
+
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT5              0xC00033  /* GFX, HDP, DRMDMA */
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT6              0xC00033  /* GFX, HDP, DRMDMA */
+#define PPTONGA_VOTINGRIGHTSCLIENTS_DFLT7              0x3FFFC000  /* GFX, HDP, DRMDMA */
+
+
+/* thermal protection counter (units).*/
+#define PPTONGA_THERMALPROTECTCOUNTER_DFLT            0x200 /* ~19us */
+
+/* static screen threshold unit */
+#define PPTONGA_STATICSCREENTHRESHOLDUNIT_DFLT        0
+
+/* static screen threshold */
+#define PPTONGA_STATICSCREENTHRESHOLD_DFLT            0x00C8
+
+/* gfx idle clock stop threshold */
+#define PPTONGA_GFXIDLECLOCKSTOPTHRESHOLD_DFLT        0x200 /* ~19us with static screen threshold unit of 0 */
+
+/* Fixed reference divider to use when building baby stepping tables. */
+#define PPTONGA_REFERENCEDIVIDER_DFLT                  4
+
+/*
+ * ULV voltage change delay time
+ * Used to be delay_vreg in N.I. split for S.I.
+ * Using N.I. delay_vreg value as default
+ * ReferenceClock = 2700
+ * VoltageResponseTime = 1000
+ * VDDCDelayTime = (VoltageResponseTime * ReferenceClock) / 1600 = 1687
+ */
+
+#define PPTONGA_ULVVOLTAGECHANGEDELAY_DFLT             1687
+
+#define PPTONGA_CGULVPARAMETER_DFLT                    0x00040035
+#define PPTONGA_CGULVCONTROL_DFLT                      0x00007450
+#define PPTONGA_TARGETACTIVITY_DFLT                     30  /*30%  */
+#define PPTONGA_MCLK_TARGETACTIVITY_DFLT                10  /*10%  */
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.c b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.c
new file mode 100644
index 000000000000..980d3bf8ea76
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.c
@@ -0,0 +1,6090 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+#include "linux/delay.h"
+#include "pp_acpi.h"
+#include "hwmgr.h"
+#include <atombios.h>
+#include "tonga_hwmgr.h"
+#include "pptable.h"
+#include "processpptables.h"
+#include "tonga_processpptables.h"
+#include "tonga_pptable.h"
+#include "pp_debug.h"
+#include "tonga_ppsmc.h"
+#include "cgs_common.h"
+#include "pppcielanes.h"
+#include "tonga_dyn_defaults.h"
+#include "smumgr.h"
+#include "tonga_smumgr.h"
+#include "tonga_clockpowergating.h"
+#include "tonga_thermal.h"
+
+#include "smu/smu_7_1_2_d.h"
+#include "smu/smu_7_1_2_sh_mask.h"
+
+#include "gmc/gmc_8_1_d.h"
+#include "gmc/gmc_8_1_sh_mask.h"
+
+#include "bif/bif_5_0_d.h"
+#include "bif/bif_5_0_sh_mask.h"
+
+#include "cgs_linux.h"
+#include "eventmgr.h"
+#include "amd_pcie_helpers.h"
+
+#define MC_CG_ARB_FREQ_F0           0x0a
+#define MC_CG_ARB_FREQ_F1           0x0b
+#define MC_CG_ARB_FREQ_F2           0x0c
+#define MC_CG_ARB_FREQ_F3           0x0d
+
+#define MC_CG_SEQ_DRAMCONF_S0       0x05
+#define MC_CG_SEQ_DRAMCONF_S1       0x06
+#define MC_CG_SEQ_YCLK_SUSPEND      0x04
+#define MC_CG_SEQ_YCLK_RESUME       0x0a
+
+#define PCIE_BUS_CLK                10000
+#define TCLK                        (PCIE_BUS_CLK / 10)
+
+#define SMC_RAM_END 0x40000
+#define SMC_CG_IND_START            0xc0030000
+#define SMC_CG_IND_END              0xc0040000  /* First byte after SMC_CG_IND*/
+
+#define VOLTAGE_SCALE               4
+#define VOLTAGE_VID_OFFSET_SCALE1   625
+#define VOLTAGE_VID_OFFSET_SCALE2   100
+
+#define VDDC_VDDCI_DELTA            200
+#define VDDC_VDDGFX_DELTA           300
+
+#define MC_SEQ_MISC0_GDDR5_SHIFT 28
+#define MC_SEQ_MISC0_GDDR5_MASK  0xf0000000
+#define MC_SEQ_MISC0_GDDR5_VALUE 5
+
+typedef uint32_t PECI_RegistryValue;
+
+/* [2.5%,~2.5%] Clock stretched is multiple of 2.5% vs not and [Fmin, Fmax, LDO_REFSEL, USE_FOR_LOW_FREQ] */
+uint16_t PP_ClockStretcherLookupTable[2][4] = {
+	{600, 1050, 3, 0},
+	{600, 1050, 6, 1} };
+
+/* [FF, SS] type, [] 4 voltage ranges, and [Floor Freq, Boundary Freq, VID min , VID max] */
+uint32_t PP_ClockStretcherDDTTable[2][4][4] = {
+	{ {265, 529, 120, 128}, {325, 650, 96, 119}, {430, 860, 32, 95}, {0, 0, 0, 31} },
+	{ {275, 550, 104, 112}, {319, 638, 96, 103}, {360, 720, 64, 95}, {384, 768, 32, 63} } };
+
+/* [Use_For_Low_freq] value, [0%, 5%, 10%, 7.14%, 14.28%, 20%] (coming from PWR_CKS_CNTL.stretch_amount reg spec) */
+uint8_t PP_ClockStretchAmountConversion[2][6] = {
+	{0, 1, 3, 2, 4, 5},
+	{0, 2, 4, 5, 6, 5} };
+
+/* Values for the CG_THERMAL_CTRL::DPM_EVENT_SRC field. */
+enum DPM_EVENT_SRC {
+	DPM_EVENT_SRC_ANALOG = 0,               /* Internal analog trip point */
+	DPM_EVENT_SRC_EXTERNAL = 1,             /* External (GPIO 17) signal */
+	DPM_EVENT_SRC_DIGITAL = 2,              /* Internal digital trip point (DIG_THERM_DPM) */
+	DPM_EVENT_SRC_ANALOG_OR_EXTERNAL = 3,   /* Internal analog or external */
+	DPM_EVENT_SRC_DIGITAL_OR_EXTERNAL = 4   /* Internal digital or external */
+};
+typedef enum DPM_EVENT_SRC DPM_EVENT_SRC;
+
+const unsigned long PhwTonga_Magic = (unsigned long)(PHM_VIslands_Magic);
+
+struct tonga_power_state *cast_phw_tonga_power_state(
+				  struct pp_hw_power_state *hw_ps)
+{
+	if (hw_ps == NULL)
+		return NULL;
+
+	PP_ASSERT_WITH_CODE((PhwTonga_Magic == hw_ps->magic),
+				"Invalid Powerstate Type!",
+				 return NULL);
+
+	return (struct tonga_power_state *)hw_ps;
+}
+
+const struct tonga_power_state *cast_const_phw_tonga_power_state(
+				 const struct pp_hw_power_state *hw_ps)
+{
+	if (hw_ps == NULL)
+		return NULL;
+
+	PP_ASSERT_WITH_CODE((PhwTonga_Magic == hw_ps->magic),
+				"Invalid Powerstate Type!",
+				 return NULL);
+
+	return (const struct tonga_power_state *)hw_ps;
+}
+
+int tonga_add_voltage(struct pp_hwmgr *hwmgr,
+	phm_ppt_v1_voltage_lookup_table *look_up_table,
+	phm_ppt_v1_voltage_lookup_record *record)
+{
+	uint32_t i;
+	PP_ASSERT_WITH_CODE((NULL != look_up_table),
+		"Lookup Table empty.", return -1;);
+	PP_ASSERT_WITH_CODE((0 != look_up_table->count),
+		"Lookup Table empty.", return -1;);
+	PP_ASSERT_WITH_CODE((SMU72_MAX_LEVELS_VDDGFX >= look_up_table->count),
+		"Lookup Table is full.", return -1;);
+
+	/* This is to avoid entering duplicate calculated records. */
+	for (i = 0; i < look_up_table->count; i++) {
+		if (look_up_table->entries[i].us_vdd == record->us_vdd) {
+			if (look_up_table->entries[i].us_calculated == 1)
+				return 0;
+			else
+				break;
+		}
+	}
+
+	look_up_table->entries[i].us_calculated = 1;
+	look_up_table->entries[i].us_vdd = record->us_vdd;
+	look_up_table->entries[i].us_cac_low = record->us_cac_low;
+	look_up_table->entries[i].us_cac_mid = record->us_cac_mid;
+	look_up_table->entries[i].us_cac_high = record->us_cac_high;
+	/* Only increment the count when we're appending, not replacing duplicate entry. */
+	if (i == look_up_table->count)
+		look_up_table->count++;
+
+	return 0;
+}
+
+int tonga_notify_smc_display_change(struct pp_hwmgr *hwmgr, bool has_display)
+{
+	PPSMC_Msg msg = has_display? (PPSMC_Msg)PPSMC_HasDisplay : (PPSMC_Msg)PPSMC_NoDisplay;
+
+	return (smum_send_msg_to_smc(hwmgr->smumgr, msg) == 0) ?  0 : -1;
+}
+
+uint8_t tonga_get_voltage_id(pp_atomctrl_voltage_table *voltage_table,
+		uint32_t voltage)
+{
+	uint8_t count = (uint8_t) (voltage_table->count);
+	uint8_t i = 0;
+
+	PP_ASSERT_WITH_CODE((NULL != voltage_table),
+		"Voltage Table empty.", return 0;);
+	PP_ASSERT_WITH_CODE((0 != count),
+		"Voltage Table empty.", return 0;);
+
+	for (i = 0; i < count; i++) {
+		/* find first voltage bigger than requested */
+		if (voltage_table->entries[i].value >= voltage)
+			return i;
+	}
+
+	/* voltage is bigger than max voltage in the table */
+	return i - 1;
+}
+
+/**
+ * @brief PhwTonga_GetVoltageOrder
+ *  Returns index of requested voltage record in lookup(table)
+ * @param hwmgr - pointer to hardware manager
+ * @param lookupTable - lookup list to search in
+ * @param voltage - voltage to look for
+ * @return 0 on success
+ */
+uint8_t tonga_get_voltage_index(phm_ppt_v1_voltage_lookup_table *look_up_table,
+		uint16_t voltage)
+{
+	uint8_t count = (uint8_t) (look_up_table->count);
+	uint8_t i;
+
+	PP_ASSERT_WITH_CODE((NULL != look_up_table), "Lookup Table empty.", return 0;);
+	PP_ASSERT_WITH_CODE((0 != count), "Lookup Table empty.", return 0;);
+
+	for (i = 0; i < count; i++) {
+		/* find first voltage equal or bigger than requested */
+		if (look_up_table->entries[i].us_vdd >= voltage)
+			return i;
+	}
+
+	/* voltage is bigger than max voltage in the table */
+	return i-1;
+}
+
+bool tonga_is_dpm_running(struct pp_hwmgr *hwmgr)
+{
+	/*
+	 * We return the status of Voltage Control instead of checking SCLK/MCLK DPM
+	 * because we may have test scenarios that need us intentionly disable SCLK/MCLK DPM,
+	 * whereas voltage control is a fundemental change that will not be disabled
+	 */
+
+	return (0 == PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+					FEATURE_STATUS, VOLTAGE_CONTROLLER_ON) ? 1 : 0);
+}
+
+/**
+ * Re-generate the DPM level mask value
+ * @param    hwmgr      the address of the hardware manager
+ */
+static uint32_t tonga_get_dpm_level_enable_mask_value(
+			struct tonga_single_dpm_table * dpm_table)
+{
+	uint32_t i;
+	uint32_t mask_value = 0;
+
+	for (i = dpm_table->count; i > 0; i--) {
+		mask_value = mask_value << 1;
+
+		if (dpm_table->dpm_levels[i-1].enabled)
+			mask_value |= 0x1;
+		else
+			mask_value &= 0xFFFFFFFE;
+	}
+	return mask_value;
+}
+
+/**
+ * Retrieve DPM default values from registry (if available)
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ */
+void tonga_initialize_dpm_defaults(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	phw_tonga_ulv_parm *ulv = &(data->ulv);
+	uint32_t tmp;
+
+	ulv->ch_ulv_parameter = PPTONGA_CGULVPARAMETER_DFLT;
+	data->voting_rights_clients0 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT0;
+	data->voting_rights_clients1 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT1;
+	data->voting_rights_clients2 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT2;
+	data->voting_rights_clients3 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT3;
+	data->voting_rights_clients4 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT4;
+	data->voting_rights_clients5 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT5;
+	data->voting_rights_clients6 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT6;
+	data->voting_rights_clients7 = PPTONGA_VOTINGRIGHTSCLIENTS_DFLT7;
+
+	data->static_screen_threshold_unit = PPTONGA_STATICSCREENTHRESHOLDUNIT_DFLT;
+	data->static_screen_threshold = PPTONGA_STATICSCREENTHRESHOLD_DFLT;
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_ABM);
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_NonABMSupportInPPLib);
+
+	tmp = 0;
+	if (tmp == 0)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DynamicACTiming);
+
+	tmp = 0;
+	if (0 != tmp)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DisableMemoryTransition);
+
+	data->mclk_strobe_mode_threshold = 40000;
+	data->mclk_stutter_mode_threshold = 30000;
+	data->mclk_edc_enable_threshold = 40000;
+	data->mclk_edc_wr_enable_threshold = 40000;
+
+	tmp = 0;
+	if (tmp != 0)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_DisableMCLS);
+
+	data->pcie_gen_performance.max = PP_PCIEGen1;
+	data->pcie_gen_performance.min = PP_PCIEGen3;
+	data->pcie_gen_power_saving.max = PP_PCIEGen1;
+	data->pcie_gen_power_saving.min = PP_PCIEGen3;
+
+	data->pcie_lane_performance.max = 0;
+	data->pcie_lane_performance.min = 16;
+	data->pcie_lane_power_saving.max = 0;
+	data->pcie_lane_power_saving.min = 16;
+
+	tmp = 0;
+
+	if (tmp)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SclkThrottleLowNotification);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_DynamicUVDState);
+
+}
+
+int tonga_update_sclk_threshold(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	int result = 0;
+	uint32_t low_sclk_interrupt_threshold = 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_SclkThrottleLowNotification)
+		&& (hwmgr->gfx_arbiter.sclk_threshold != data->low_sclk_interrupt_threshold)) {
+		data->low_sclk_interrupt_threshold = hwmgr->gfx_arbiter.sclk_threshold;
+		low_sclk_interrupt_threshold = data->low_sclk_interrupt_threshold;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(low_sclk_interrupt_threshold);
+
+		result = tonga_copy_bytes_to_smc(
+				hwmgr->smumgr,
+				data->dpm_table_start + offsetof(SMU72_Discrete_DpmTable,
+				LowSclkInterruptThreshold),
+				(uint8_t *)&low_sclk_interrupt_threshold,
+				sizeof(uint32_t),
+				data->sram_end
+				);
+	}
+
+	return result;
+}
+
+/**
+ * Find SCLK value that is associated with specified virtual_voltage_Id.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @param    virtual_voltage_Id  voltageId to look for.
+ * @param    sclk output value .
+ * @return   always 0 if success and 2 if association not found
+ */
+static int tonga_get_sclk_for_voltage_evv(struct pp_hwmgr *hwmgr,
+	phm_ppt_v1_voltage_lookup_table *lookup_table,
+	uint16_t virtual_voltage_id, uint32_t *sclk)
+{
+	uint8_t entryId;
+	uint8_t voltageId;
+	struct phm_ppt_v1_information *pptable_info =
+					(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	PP_ASSERT_WITH_CODE(lookup_table->count != 0, "Lookup table is empty", return -1);
+
+	/* search for leakage voltage ID 0xff01 ~ 0xff08 and sckl */
+	for (entryId = 0; entryId < pptable_info->vdd_dep_on_sclk->count; entryId++) {
+		voltageId = pptable_info->vdd_dep_on_sclk->entries[entryId].vddInd;
+		if (lookup_table->entries[voltageId].us_vdd == virtual_voltage_id)
+			break;
+	}
+
+	PP_ASSERT_WITH_CODE(entryId < pptable_info->vdd_dep_on_sclk->count,
+			"Can't find requested voltage id in vdd_dep_on_sclk table!",
+			return -1;
+			);
+
+	*sclk = pptable_info->vdd_dep_on_sclk->entries[entryId].clk;
+
+	return 0;
+}
+
+/**
+ * Get Leakage VDDC based on leakage ID.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   2 if vddgfx returned is greater than 2V or if BIOS
+ */
+int tonga_get_evv_voltage(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_clock_voltage_dependency_table *sclk_table = pptable_info->vdd_dep_on_sclk;
+	uint16_t    virtual_voltage_id;
+	uint16_t    vddc = 0;
+	uint16_t    vddgfx = 0;
+	uint16_t    i, j;
+	uint32_t  sclk = 0;
+
+	/* retrieve voltage for leakage ID (0xff01 + i) */
+	for (i = 0; i < TONGA_MAX_LEAKAGE_COUNT; i++) {
+		virtual_voltage_id = ATOM_VIRTUAL_VOLTAGE_ID0 + i;
+
+		/* in split mode we should have only vddgfx EVV leakages */
+		if (data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) {
+			if (0 == tonga_get_sclk_for_voltage_evv(hwmgr,
+						pptable_info->vddgfx_lookup_table, virtual_voltage_id, &sclk)) {
+				if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+							PHM_PlatformCaps_ClockStretcher)) {
+					for (j = 1; j < sclk_table->count; j++) {
+						if (sclk_table->entries[j].clk == sclk &&
+								sclk_table->entries[j].cks_enable == 0) {
+							sclk += 5000;
+							break;
+						}
+					}
+				}
+				PP_ASSERT_WITH_CODE(0 == atomctrl_get_voltage_evv_on_sclk
+						(hwmgr, VOLTAGE_TYPE_VDDGFX, sclk,
+						 virtual_voltage_id, &vddgfx),
+						"Error retrieving EVV voltage value!", continue);
+
+				/* need to make sure vddgfx is less than 2v or else, it could burn the ASIC. */
+				PP_ASSERT_WITH_CODE((vddgfx < 2000 && vddgfx != 0), "Invalid VDDGFX value!", return -1);
+
+				/* the voltage should not be zero nor equal to leakage ID */
+				if (vddgfx != 0 && vddgfx != virtual_voltage_id) {
+					data->vddcgfx_leakage.actual_voltage[data->vddcgfx_leakage.count] = vddgfx;
+					data->vddcgfx_leakage.leakage_id[data->vddcgfx_leakage.count] = virtual_voltage_id;
+					data->vddcgfx_leakage.count++;
+				}
+			}
+		} else {
+			/*  in merged mode we have only vddc EVV leakages */
+			if (0 == tonga_get_sclk_for_voltage_evv(hwmgr,
+						pptable_info->vddc_lookup_table,
+						virtual_voltage_id, &sclk)) {
+				PP_ASSERT_WITH_CODE(0 == atomctrl_get_voltage_evv_on_sclk
+						(hwmgr, VOLTAGE_TYPE_VDDC, sclk,
+						 virtual_voltage_id, &vddc),
+						"Error retrieving EVV voltage value!", continue);
+
+				/* need to make sure vddc is less than 2v or else, it could burn the ASIC. */
+				if (vddc > 2000)
+					printk(KERN_ERR "[ powerplay ] Invalid VDDC value! \n");
+
+				/* the voltage should not be zero nor equal to leakage ID */
+				if (vddc != 0 && vddc != virtual_voltage_id) {
+					data->vddc_leakage.actual_voltage[data->vddc_leakage.count] = vddc;
+					data->vddc_leakage.leakage_id[data->vddc_leakage.count] = virtual_voltage_id;
+					data->vddc_leakage.count++;
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+int tonga_enable_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* enable SCLK dpm */
+	if (0 == data->sclk_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						   PPSMC_MSG_DPM_Enable)),
+				"Failed to enable SCLK DPM during DPM Start Function!",
+				return -1);
+	}
+
+	/* enable MCLK dpm */
+	if (0 == data->mclk_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					     PPSMC_MSG_MCLKDPM_Enable)),
+				"Failed to enable MCLK DPM during DPM Start Function!",
+				return -1);
+
+		PHM_WRITE_FIELD(hwmgr->device, MC_SEQ_CNTL_3, CAC_EN, 0x1);
+
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_MC0_CNTL, 0x05);/* CH0,1 read */
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_MC1_CNTL, 0x05);/* CH2,3 read */
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_CPL_CNTL, 0x100005);/*Read */
+
+		udelay(10);
+
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_MC0_CNTL, 0x400005);/* CH0,1 write */
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_MC1_CNTL, 0x400005);/* CH2,3 write */
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixLCAC_CPL_CNTL, 0x500005);/* write */
+
+	}
+
+	return 0;
+}
+
+int tonga_start_dpm(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* enable general power management */
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT, GLOBAL_PWRMGT_EN, 1);
+	/* enable sclk deep sleep */
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, SCLK_PWRMGT_CNTL, DYNAMIC_PM_EN, 1);
+
+	/* prepare for PCIE DPM */
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, data->soft_regs_start +
+			offsetof(SMU72_SoftRegisters, VoltageChangeTimeout), 0x1000);
+
+	PHM_WRITE_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__PCIE, SWRST_COMMAND_1, RESETLC, 0x0);
+
+	PP_ASSERT_WITH_CODE(
+			(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					PPSMC_MSG_Voltage_Cntl_Enable)),
+			"Failed to enable voltage DPM during DPM Start Function!",
+			return -1);
+
+	if (0 != tonga_enable_sclk_mclk_dpm(hwmgr)) {
+		PP_ASSERT_WITH_CODE(0, "Failed to enable Sclk DPM and Mclk DPM!", return -1);
+	}
+
+	/* enable PCIE dpm */
+	if (0 == data->pcie_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						PPSMC_MSG_PCIeDPM_Enable)),
+				"Failed to enable pcie DPM during DPM Start Function!",
+				return -1
+				);
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_Falcon_QuickTransition)) {
+				   smum_send_msg_to_smc(hwmgr->smumgr,
+				    PPSMC_MSG_EnableACDCGPIOInterrupt);
+	}
+
+	return 0;
+}
+
+int tonga_disable_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* disable SCLK dpm */
+	if (0 == data->sclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+		PP_ASSERT_WITH_CODE(
+				(0 == tonga_is_dpm_running(hwmgr)),
+				"Trying to Disable SCLK DPM when DPM is disabled",
+				return -1
+				);
+
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						  PPSMC_MSG_DPM_Disable)),
+				"Failed to disable SCLK DPM during DPM stop Function!",
+				return -1);
+	}
+
+	/* disable MCLK dpm */
+	if (0 == data->mclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this, we should skip this message. */
+		PP_ASSERT_WITH_CODE(
+				(0 == tonga_is_dpm_running(hwmgr)),
+				"Trying to Disable MCLK DPM when DPM is disabled",
+				return -1
+				);
+
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					    PPSMC_MSG_MCLKDPM_Disable)),
+				"Failed to Disable MCLK DPM during DPM stop Function!",
+				return -1);
+	}
+
+	return 0;
+}
+
+int tonga_stop_dpm(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT, GLOBAL_PWRMGT_EN, 0);
+	/* disable sclk deep sleep*/
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, SCLK_PWRMGT_CNTL, DYNAMIC_PM_EN, 0);
+
+	/* disable PCIE dpm */
+	if (0 == data->pcie_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+		PP_ASSERT_WITH_CODE(
+				(0 == tonga_is_dpm_running(hwmgr)),
+				"Trying to Disable PCIE DPM when DPM is disabled",
+				return -1
+				);
+		PP_ASSERT_WITH_CODE(
+				(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+						PPSMC_MSG_PCIeDPM_Disable)),
+				"Failed to disable pcie DPM during DPM stop Function!",
+				return -1);
+	}
+
+	if (0 != tonga_disable_sclk_mclk_dpm(hwmgr))
+		PP_ASSERT_WITH_CODE(0, "Failed to disable Sclk DPM and Mclk DPM!", return -1);
+
+	/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+	PP_ASSERT_WITH_CODE(
+			(0 == tonga_is_dpm_running(hwmgr)),
+			"Trying to Disable Voltage CNTL when DPM is disabled",
+			return -1
+			);
+
+	PP_ASSERT_WITH_CODE(
+			(0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					PPSMC_MSG_Voltage_Cntl_Disable)),
+			"Failed to disable voltage DPM during DPM stop Function!",
+			return -1);
+
+	return 0;
+}
+
+int tonga_enable_sclk_control(struct pp_hwmgr *hwmgr)
+{
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, SCLK_PWRMGT_CNTL, SCLK_PWRMGT_OFF, 0);
+
+	return 0;
+}
+
+/**
+ * Send a message to the SMC and return a parameter
+ *
+ * @param    hwmgr:  the address of the powerplay hardware manager.
+ * @param    msg: the message to send.
+ * @param    parameter: pointer to the received parameter
+ * @return   The response that came from the SMC.
+ */
+PPSMC_Result tonga_send_msg_to_smc_return_parameter(
+		struct pp_hwmgr *hwmgr,
+		PPSMC_Msg msg,
+		uint32_t *parameter)
+{
+	int result;
+
+	result = smum_send_msg_to_smc(hwmgr->smumgr, msg);
+
+	if ((0 == result) && parameter) {
+		*parameter = cgs_read_register(hwmgr->device, mmSMC_MSG_ARG_0);
+	}
+
+	return result;
+}
+
+/**
+ * force DPM power State
+ *
+ * @param    hwmgr:  the address of the powerplay hardware manager.
+ * @param    n     :  DPM level
+ * @return   The response that came from the SMC.
+ */
+int tonga_dpm_force_state(struct pp_hwmgr *hwmgr, uint32_t n)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t level_mask = 1 << n;
+
+	/* Checking if DPM is running.  If we discover hang because of this, we should skip this message. */
+	PP_ASSERT_WITH_CODE(0 == tonga_is_dpm_running(hwmgr),
+			"Trying to force SCLK when DPM is disabled", return -1;);
+	if (0 == data->sclk_dpm_key_disabled)
+		return (0 == smum_send_msg_to_smc_with_parameter(
+							     hwmgr->smumgr,
+		     (PPSMC_Msg)(PPSMC_MSG_SCLKDPM_SetEnabledMask),
+						    level_mask) ? 0 : 1);
+
+	return 0;
+}
+
+/**
+ * force DPM power State
+ *
+ * @param    hwmgr:  the address of the powerplay hardware manager.
+ * @param    n     :  DPM level
+ * @return   The response that came from the SMC.
+ */
+int tonga_dpm_force_state_mclk(struct pp_hwmgr *hwmgr, uint32_t n)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t level_mask = 1 << n;
+
+	/* Checking if DPM is running.  If we discover hang because of this, we should skip this message. */
+	PP_ASSERT_WITH_CODE(0 == tonga_is_dpm_running(hwmgr),
+			"Trying to Force MCLK when DPM is disabled", return -1;);
+	if (0 == data->mclk_dpm_key_disabled)
+		return (0 == smum_send_msg_to_smc_with_parameter(
+								hwmgr->smumgr,
+								(PPSMC_Msg)(PPSMC_MSG_MCLKDPM_SetEnabledMask),
+								level_mask) ? 0 : 1);
+
+	return 0;
+}
+
+/**
+ * force DPM power State
+ *
+ * @param    hwmgr:  the address of the powerplay hardware manager.
+ * @param    n     :  DPM level
+ * @return   The response that came from the SMC.
+ */
+int tonga_dpm_force_state_pcie(struct pp_hwmgr *hwmgr, uint32_t n)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+	PP_ASSERT_WITH_CODE(0 == tonga_is_dpm_running(hwmgr),
+			"Trying to Force PCIE level when DPM is disabled", return -1;);
+	if (0 == data->pcie_dpm_key_disabled)
+		return (0 == smum_send_msg_to_smc_with_parameter(
+							     hwmgr->smumgr,
+			   (PPSMC_Msg)(PPSMC_MSG_PCIeDPM_ForceLevel),
+								n) ? 0 : 1);
+
+	return 0;
+}
+
+/**
+ * Set the initial state by calling SMC to switch to this state directly
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_set_boot_state(struct pp_hwmgr *hwmgr)
+{
+	/*
+	* SMC only stores one state that SW will ask to switch too,
+	* so we switch the the just uploaded one
+	*/
+	return (0 == tonga_disable_sclk_mclk_dpm(hwmgr)) ? 0 : 1;
+}
+
+/**
+ * Get the location of various tables inside the FW image.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_process_firmware_header(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_smumgr *tonga_smu = (struct tonga_smumgr *)(hwmgr->smumgr->backend);
+
+	uint32_t tmp;
+	int result;
+	bool error = 0;
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, DpmTable),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		data->dpm_table_start = tmp;
+	}
+
+	error |= (0 != result);
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, SoftRegisters),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		data->soft_regs_start = tmp;
+		tonga_smu->ulSoftRegsStart = tmp;
+	}
+
+	error |= (0 != result);
+
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, mcRegisterTable),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		data->mc_reg_table_start = tmp;
+	}
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, FanTable),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		data->fan_table_start = tmp;
+	}
+
+	error |= (0 != result);
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, mcArbDramTimingTable),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		data->arb_table_start = tmp;
+	}
+
+	error |= (0 != result);
+
+
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				SMU72_FIRMWARE_HEADER_LOCATION +
+				offsetof(SMU72_Firmware_Header, Version),
+				&tmp, data->sram_end);
+
+	if (0 == result) {
+		hwmgr->microcode_version_info.SMC = tmp;
+	}
+
+	error |= (0 != result);
+
+	return error ? 1 : 0;
+}
+
+/**
+ * Read clock related registers.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_read_clock_registers(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	data->clock_registers.vCG_SPLL_FUNC_CNTL         =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_FUNC_CNTL);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_2       =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_FUNC_CNTL_2);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_3       =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_FUNC_CNTL_3);
+	data->clock_registers.vCG_SPLL_FUNC_CNTL_4       =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_FUNC_CNTL_4);
+	data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM   =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_SPREAD_SPECTRUM);
+	data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2 =
+		cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_SPLL_SPREAD_SPECTRUM_2);
+	data->clock_registers.vDLL_CNTL                  =
+		cgs_read_register(hwmgr->device, mmDLL_CNTL);
+	data->clock_registers.vMCLK_PWRMGT_CNTL          =
+		cgs_read_register(hwmgr->device, mmMCLK_PWRMGT_CNTL);
+	data->clock_registers.vMPLL_AD_FUNC_CNTL         =
+		cgs_read_register(hwmgr->device, mmMPLL_AD_FUNC_CNTL);
+	data->clock_registers.vMPLL_DQ_FUNC_CNTL         =
+		cgs_read_register(hwmgr->device, mmMPLL_DQ_FUNC_CNTL);
+	data->clock_registers.vMPLL_FUNC_CNTL            =
+		cgs_read_register(hwmgr->device, mmMPLL_FUNC_CNTL);
+	data->clock_registers.vMPLL_FUNC_CNTL_1          =
+		cgs_read_register(hwmgr->device, mmMPLL_FUNC_CNTL_1);
+	data->clock_registers.vMPLL_FUNC_CNTL_2          =
+		cgs_read_register(hwmgr->device, mmMPLL_FUNC_CNTL_2);
+	data->clock_registers.vMPLL_SS1                  =
+		cgs_read_register(hwmgr->device, mmMPLL_SS1);
+	data->clock_registers.vMPLL_SS2                  =
+		cgs_read_register(hwmgr->device, mmMPLL_SS2);
+
+	return 0;
+}
+
+/**
+ * Find out if memory is GDDR5.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_get_memory_type(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t temp;
+
+	temp = cgs_read_register(hwmgr->device, mmMC_SEQ_MISC0);
+
+	data->is_memory_GDDR5 = (MC_SEQ_MISC0_GDDR5_VALUE ==
+			((temp & MC_SEQ_MISC0_GDDR5_MASK) >>
+			 MC_SEQ_MISC0_GDDR5_SHIFT));
+
+	return 0;
+}
+
+/**
+ * Enables Dynamic Power Management by SMC
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_enable_acpi_power_management(struct pp_hwmgr *hwmgr)
+{
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT, STATIC_PM_EN, 1);
+
+	return 0;
+}
+
+/**
+ * Initialize PowerGating States for different engines
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_init_power_gate_state(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	data->uvd_power_gated = 0;
+	data->vce_power_gated = 0;
+	data->samu_power_gated = 0;
+	data->acp_power_gated = 0;
+	data->pg_acp_init = 1;
+
+	return 0;
+}
+
+/**
+ * Checks if DPM is enabled
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_check_for_dpm_running(struct pp_hwmgr *hwmgr)
+{
+	/*
+	 * We return the status of Voltage Control instead of checking SCLK/MCLK DPM
+	 * because we may have test scenarios that need us intentionly disable SCLK/MCLK DPM,
+	 * whereas voltage control is a fundemental change that will not be disabled
+	 */
+	return (0 == tonga_is_dpm_running(hwmgr) ? 0 : 1);
+}
+
+/**
+ * Checks if DPM is stopped
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_check_for_dpm_stopped(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	if (0 != tonga_is_dpm_running(hwmgr)) {
+		/* If HW Virtualization is enabled, dpm_table_start will not have a valid value */
+		if (!data->dpm_table_start) {
+			return 1;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Remove repeated voltage values and create table with unique values.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @param    voltage_table  the pointer to changing voltage table
+ * @return    1 in success
+ */
+
+static int tonga_trim_voltage_table(struct pp_hwmgr *hwmgr,
+			pp_atomctrl_voltage_table *voltage_table)
+{
+	uint32_t table_size, i, j;
+	uint16_t vvalue;
+	bool bVoltageFound = 0;
+	pp_atomctrl_voltage_table *table;
+
+	PP_ASSERT_WITH_CODE((NULL != voltage_table), "Voltage Table empty.", return -1;);
+	table_size = sizeof(pp_atomctrl_voltage_table);
+	table = kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table)
+		return -ENOMEM;
+
+	memset(table, 0x00, table_size);
+	table->mask_low = voltage_table->mask_low;
+	table->phase_delay = voltage_table->phase_delay;
+
+	for (i = 0; i < voltage_table->count; i++) {
+		vvalue = voltage_table->entries[i].value;
+		bVoltageFound = 0;
+
+		for (j = 0; j < table->count; j++) {
+			if (vvalue == table->entries[j].value) {
+				bVoltageFound = 1;
+				break;
+			}
+		}
+
+		if (!bVoltageFound) {
+			table->entries[table->count].value = vvalue;
+			table->entries[table->count].smio_low =
+				voltage_table->entries[i].smio_low;
+			table->count++;
+		}
+	}
+
+	memcpy(table, voltage_table, sizeof(pp_atomctrl_voltage_table));
+
+	kfree(table);
+
+	return 0;
+}
+
+static int tonga_get_svi2_vdd_ci_voltage_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_clock_voltage_dependency_table *voltage_dependency_table)
+{
+	uint32_t i;
+	int result;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	pp_atomctrl_voltage_table *vddci_voltage_table = &(data->vddci_voltage_table);
+
+	PP_ASSERT_WITH_CODE((0 != voltage_dependency_table->count),
+			"Voltage Dependency Table empty.", return -1;);
+
+	vddci_voltage_table->mask_low = 0;
+	vddci_voltage_table->phase_delay = 0;
+	vddci_voltage_table->count = voltage_dependency_table->count;
+
+	for (i = 0; i < voltage_dependency_table->count; i++) {
+		vddci_voltage_table->entries[i].value =
+			voltage_dependency_table->entries[i].vddci;
+		vddci_voltage_table->entries[i].smio_low = 0;
+	}
+
+	result = tonga_trim_voltage_table(hwmgr, vddci_voltage_table);
+	PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to trim VDDCI table.", return result;);
+
+	return 0;
+}
+
+
+
+static int tonga_get_svi2_vdd_voltage_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_voltage_lookup_table *look_up_table,
+		pp_atomctrl_voltage_table *voltage_table)
+{
+	uint8_t i = 0;
+
+	PP_ASSERT_WITH_CODE((0 != look_up_table->count),
+			"Voltage Lookup Table empty.", return -1;);
+
+	voltage_table->mask_low = 0;
+	voltage_table->phase_delay = 0;
+
+	voltage_table->count = look_up_table->count;
+
+	for (i = 0; i < voltage_table->count; i++) {
+		voltage_table->entries[i].value = look_up_table->entries[i].us_vdd;
+		voltage_table->entries[i].smio_low = 0;
+	}
+
+	return 0;
+}
+
+/*
+ * -------------------------------------------------------- Voltage Tables --------------------------------------------------------------------------
+ * If the voltage table would be bigger than what will fit into the state table on the SMC keep only the higher entries.
+ */
+
+static void tonga_trim_voltage_table_to_fit_state_table(
+		struct pp_hwmgr *hwmgr,
+		uint32_t max_voltage_steps,
+		pp_atomctrl_voltage_table *voltage_table)
+{
+	unsigned int i, diff;
+
+	if (voltage_table->count <= max_voltage_steps) {
+		return;
+	}
+
+	diff = voltage_table->count - max_voltage_steps;
+
+	for (i = 0; i < max_voltage_steps; i++) {
+		voltage_table->entries[i] = voltage_table->entries[i + diff];
+	}
+
+	voltage_table->count = max_voltage_steps;
+
+	return;
+}
+
+/**
+ * Create Voltage Tables.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_construct_voltage_tables(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	int result;
+
+	/* MVDD has only GPIO voltage control */
+	if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->mvdd_control) {
+		result = atomctrl_get_voltage_table_v3(hwmgr,
+					VOLTAGE_TYPE_MVDDC, VOLTAGE_OBJ_GPIO_LUT, &(data->mvdd_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to retrieve MVDD table.", return result;);
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->vdd_ci_control) {
+		/* GPIO voltage */
+		result = atomctrl_get_voltage_table_v3(hwmgr,
+					VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_GPIO_LUT, &(data->vddci_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to retrieve VDDCI table.", return result;);
+	} else if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_ci_control) {
+		/* SVI2 voltage */
+		result = tonga_get_svi2_vdd_ci_voltage_table(hwmgr,
+					pptable_info->vdd_dep_on_mclk);
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to retrieve SVI2 VDDCI table from dependancy table.", return result;);
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_gfx_control) {
+		/* VDDGFX has only SVI2 voltage control */
+		result = tonga_get_svi2_vdd_voltage_table(hwmgr,
+					pptable_info->vddgfx_lookup_table, &(data->vddgfx_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to retrieve SVI2 VDDGFX table from lookup table.", return result;);
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+		/* VDDC has only SVI2 voltage control */
+		result = tonga_get_svi2_vdd_voltage_table(hwmgr,
+					pptable_info->vddc_lookup_table, &(data->vddc_voltage_table));
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to retrieve SVI2 VDDC table from lookup table.", return result;);
+	}
+
+	PP_ASSERT_WITH_CODE(
+			(data->vddc_voltage_table.count <= (SMU72_MAX_LEVELS_VDDC)),
+			"Too many voltage values for VDDC. Trimming to fit state table.",
+			tonga_trim_voltage_table_to_fit_state_table(hwmgr,
+			SMU72_MAX_LEVELS_VDDC, &(data->vddc_voltage_table));
+			);
+
+	PP_ASSERT_WITH_CODE(
+			(data->vddgfx_voltage_table.count <= (SMU72_MAX_LEVELS_VDDGFX)),
+			"Too many voltage values for VDDGFX. Trimming to fit state table.",
+			tonga_trim_voltage_table_to_fit_state_table(hwmgr,
+			SMU72_MAX_LEVELS_VDDGFX, &(data->vddgfx_voltage_table));
+			);
+
+	PP_ASSERT_WITH_CODE(
+			(data->vddci_voltage_table.count <= (SMU72_MAX_LEVELS_VDDCI)),
+			"Too many voltage values for VDDCI. Trimming to fit state table.",
+			tonga_trim_voltage_table_to_fit_state_table(hwmgr,
+			SMU72_MAX_LEVELS_VDDCI, &(data->vddci_voltage_table));
+			);
+
+	PP_ASSERT_WITH_CODE(
+			(data->mvdd_voltage_table.count <= (SMU72_MAX_LEVELS_MVDD)),
+			"Too many voltage values for MVDD. Trimming to fit state table.",
+			tonga_trim_voltage_table_to_fit_state_table(hwmgr,
+			SMU72_MAX_LEVELS_MVDD, &(data->mvdd_voltage_table));
+			);
+
+	return 0;
+}
+
+/**
+ * Vddc table preparation for SMC.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    table     the SMC DPM table structure to be populated
+ * @return   always 0
+ */
+static int tonga_populate_smc_vddc_table(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	unsigned int count;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+		table->VddcLevelCount = data->vddc_voltage_table.count;
+		for (count = 0; count < table->VddcLevelCount; count++) {
+			table->VddcTable[count] =
+				PP_HOST_TO_SMC_US(data->vddc_voltage_table.entries[count].value * VOLTAGE_SCALE);
+		}
+		CONVERT_FROM_HOST_TO_SMC_UL(table->VddcLevelCount);
+	}
+	return 0;
+}
+
+/**
+ * VddGfx table preparation for SMC.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    table     the SMC DPM table structure to be populated
+ * @return   always 0
+ */
+static int tonga_populate_smc_vdd_gfx_table(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	unsigned int count;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_gfx_control) {
+		table->VddGfxLevelCount = data->vddgfx_voltage_table.count;
+		for (count = 0; count < data->vddgfx_voltage_table.count; count++) {
+			table->VddGfxTable[count] =
+				PP_HOST_TO_SMC_US(data->vddgfx_voltage_table.entries[count].value * VOLTAGE_SCALE);
+		}
+		CONVERT_FROM_HOST_TO_SMC_UL(table->VddGfxLevelCount);
+	}
+	return 0;
+}
+
+/**
+ * Vddci table preparation for SMC.
+ *
+ * @param    *hwmgr The address of the hardware manager.
+ * @param    *table The SMC DPM table structure to be populated.
+ * @return   0
+ */
+static int tonga_populate_smc_vdd_ci_table(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t count;
+
+	table->VddciLevelCount = data->vddci_voltage_table.count;
+	for (count = 0; count < table->VddciLevelCount; count++) {
+		if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_ci_control) {
+			table->VddciTable[count] =
+				PP_HOST_TO_SMC_US(data->vddci_voltage_table.entries[count].value * VOLTAGE_SCALE);
+		} else if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->vdd_ci_control) {
+			table->SmioTable1.Pattern[count].Voltage =
+				PP_HOST_TO_SMC_US(data->vddci_voltage_table.entries[count].value * VOLTAGE_SCALE);
+			/* Index into DpmTable.Smio. Drive bits from Smio entry to get this voltage level. */
+			table->SmioTable1.Pattern[count].Smio =
+				(uint8_t) count;
+			table->Smio[count] |=
+				data->vddci_voltage_table.entries[count].smio_low;
+			table->VddciTable[count] =
+				PP_HOST_TO_SMC_US(data->vddci_voltage_table.entries[count].value * VOLTAGE_SCALE);
+		}
+	}
+
+	table->SmioMask1 = data->vddci_voltage_table.mask_low;
+	CONVERT_FROM_HOST_TO_SMC_UL(table->VddciLevelCount);
+
+	return 0;
+}
+
+/**
+ * Mvdd table preparation for SMC.
+ *
+ * @param    *hwmgr The address of the hardware manager.
+ * @param    *table The SMC DPM table structure to be populated.
+ * @return   0
+ */
+static int tonga_populate_smc_mvdd_table(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t count;
+
+	if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->mvdd_control) {
+		table->MvddLevelCount = data->mvdd_voltage_table.count;
+		for (count = 0; count < table->MvddLevelCount; count++) {
+			table->SmioTable2.Pattern[count].Voltage =
+				PP_HOST_TO_SMC_US(data->mvdd_voltage_table.entries[count].value * VOLTAGE_SCALE);
+			/* Index into DpmTable.Smio. Drive bits from Smio entry to get this voltage level.*/
+			table->SmioTable2.Pattern[count].Smio =
+				(uint8_t) count;
+			table->Smio[count] |=
+				data->mvdd_voltage_table.entries[count].smio_low;
+		}
+		table->SmioMask2 = data->vddci_voltage_table.mask_low;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->MvddLevelCount);
+	}
+
+	return 0;
+}
+
+/**
+ * Convert a voltage value in mv unit to VID number required by SMU firmware
+ */
+static uint8_t convert_to_vid(uint16_t vddc)
+{
+	return (uint8_t) ((6200 - (vddc * VOLTAGE_SCALE)) / 25);
+}
+
+
+/**
+ * Preparation of vddc and vddgfx CAC tables for SMC.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    table     the SMC DPM table structure to be populated
+ * @return   always 0
+ */
+static int tonga_populate_cac_tables(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	uint32_t count;
+	uint8_t index;
+	int result = 0;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct phm_ppt_v1_voltage_lookup_table *vddgfx_lookup_table = pptable_info->vddgfx_lookup_table;
+	struct phm_ppt_v1_voltage_lookup_table *vddc_lookup_table = pptable_info->vddc_lookup_table;
+
+	/* pTables is already swapped, so in order to use the value from it, we need to swap it back. */
+	uint32_t vddcLevelCount = PP_SMC_TO_HOST_UL(table->VddcLevelCount);
+	uint32_t vddgfxLevelCount = PP_SMC_TO_HOST_UL(table->VddGfxLevelCount);
+
+	for (count = 0; count < vddcLevelCount; count++) {
+		/* We are populating vddc CAC data to BapmVddc table in split and merged mode */
+		index = tonga_get_voltage_index(vddc_lookup_table,
+			data->vddc_voltage_table.entries[count].value);
+		table->BapmVddcVidLoSidd[count] =
+			convert_to_vid(vddc_lookup_table->entries[index].us_cac_low);
+		table->BapmVddcVidHiSidd[count] =
+			convert_to_vid(vddc_lookup_table->entries[index].us_cac_mid);
+		table->BapmVddcVidHiSidd2[count] =
+			convert_to_vid(vddc_lookup_table->entries[index].us_cac_high);
+	}
+
+	if ((data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2)) {
+		/* We are populating vddgfx CAC data to BapmVddgfx table in split mode */
+		for (count = 0; count < vddgfxLevelCount; count++) {
+			index = tonga_get_voltage_index(vddgfx_lookup_table,
+				data->vddgfx_voltage_table.entries[count].value);
+			table->BapmVddGfxVidLoSidd[count] =
+				convert_to_vid(vddgfx_lookup_table->entries[index].us_cac_low);
+			table->BapmVddGfxVidHiSidd[count] =
+				convert_to_vid(vddgfx_lookup_table->entries[index].us_cac_mid);
+			table->BapmVddGfxVidHiSidd2[count] =
+				convert_to_vid(vddgfx_lookup_table->entries[index].us_cac_high);
+		}
+	} else {
+		for (count = 0; count < vddcLevelCount; count++) {
+			index = tonga_get_voltage_index(vddc_lookup_table,
+				data->vddc_voltage_table.entries[count].value);
+			table->BapmVddGfxVidLoSidd[count] =
+				convert_to_vid(vddc_lookup_table->entries[index].us_cac_low);
+			table->BapmVddGfxVidHiSidd[count] =
+				convert_to_vid(vddc_lookup_table->entries[index].us_cac_mid);
+			table->BapmVddGfxVidHiSidd2[count] =
+				convert_to_vid(vddc_lookup_table->entries[index].us_cac_high);
+		}
+	}
+
+	return result;
+}
+
+
+/**
+ * Preparation of voltage tables for SMC.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    table     the SMC DPM table structure to be populated
+ * @return   always 0
+ */
+
+int tonga_populate_smc_voltage_tables(struct pp_hwmgr *hwmgr,
+	SMU72_Discrete_DpmTable *table)
+{
+	int result;
+
+	result = tonga_populate_smc_vddc_table(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate VDDC voltage table to SMC", return -1);
+
+	result = tonga_populate_smc_vdd_ci_table(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate VDDCI voltage table to SMC", return -1);
+
+	result = tonga_populate_smc_vdd_gfx_table(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate VDDGFX voltage table to SMC", return -1);
+
+	result = tonga_populate_smc_mvdd_table(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate MVDD voltage table to SMC", return -1);
+
+	result = tonga_populate_cac_tables(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"can not populate CAC voltage tables to SMC", return -1);
+
+	return 0;
+}
+
+/**
+ * Populates the SMC VRConfig field in DPM table.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    table     the SMC DPM table structure to be populated
+ * @return   always 0
+ */
+static int tonga_populate_vr_config(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint16_t config;
+
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_gfx_control) {
+		/*  Splitted mode */
+		config = VR_SVI2_PLANE_1;
+		table->VRConfig |= (config<<VRCONF_VDDGFX_SHIFT);
+
+		if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+			config = VR_SVI2_PLANE_2;
+			table->VRConfig |= config;
+		} else {
+			printk(KERN_ERR "[ powerplay ] VDDC and VDDGFX should be both on SVI2 control in splitted mode! \n");
+		}
+	} else {
+		/* Merged mode  */
+		config = VR_MERGED_WITH_VDDC;
+		table->VRConfig |= (config<<VRCONF_VDDGFX_SHIFT);
+
+		/* Set Vddc Voltage Controller  */
+		if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->voltage_control) {
+			config = VR_SVI2_PLANE_1;
+			table->VRConfig |= config;
+		} else {
+			printk(KERN_ERR "[ powerplay ] VDDC should be on SVI2 control in merged mode! \n");
+		}
+	}
+
+	/* Set Vddci Voltage Controller  */
+	if (TONGA_VOLTAGE_CONTROL_BY_SVID2 == data->vdd_ci_control) {
+		config = VR_SVI2_PLANE_2;  /* only in merged mode */
+		table->VRConfig |= (config<<VRCONF_VDDCI_SHIFT);
+	} else if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->vdd_ci_control) {
+		config = VR_SMIO_PATTERN_1;
+		table->VRConfig |= (config<<VRCONF_VDDCI_SHIFT);
+	}
+
+	/* Set Mvdd Voltage Controller */
+	if (TONGA_VOLTAGE_CONTROL_BY_GPIO == data->mvdd_control) {
+		config = VR_SMIO_PATTERN_2;
+		table->VRConfig |= (config<<VRCONF_MVDD_SHIFT);
+	}
+
+	return 0;
+}
+
+static int tonga_get_dependecy_volt_by_clk(struct pp_hwmgr *hwmgr,
+	phm_ppt_v1_clock_voltage_dependency_table *allowed_clock_voltage_table,
+	uint32_t clock, SMU_VoltageLevel *voltage, uint32_t *mvdd)
+{
+	uint32_t i = 0;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	/* clock - voltage dependency table is empty table */
+	if (allowed_clock_voltage_table->count == 0)
+		return -1;
+
+	for (i = 0; i < allowed_clock_voltage_table->count; i++) {
+		/* find first sclk bigger than request */
+		if (allowed_clock_voltage_table->entries[i].clk >= clock) {
+			voltage->VddGfx = tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+								allowed_clock_voltage_table->entries[i].vddgfx);
+
+			voltage->Vddc = tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+								allowed_clock_voltage_table->entries[i].vddc);
+
+			if (allowed_clock_voltage_table->entries[i].vddci) {
+				voltage->Vddci = tonga_get_voltage_id(&data->vddci_voltage_table,
+									allowed_clock_voltage_table->entries[i].vddci);
+			} else {
+				voltage->Vddci = tonga_get_voltage_id(&data->vddci_voltage_table,
+									allowed_clock_voltage_table->entries[i].vddc - data->vddc_vddci_delta);
+			}
+
+			if (allowed_clock_voltage_table->entries[i].mvdd) {
+				*mvdd = (uint32_t) allowed_clock_voltage_table->entries[i].mvdd;
+			}
+
+			voltage->Phases = 1;
+			return 0;
+		}
+	}
+
+	/* sclk is bigger than max sclk in the dependence table */
+	voltage->VddGfx = tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+		allowed_clock_voltage_table->entries[i-1].vddgfx);
+	voltage->Vddc = tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+		allowed_clock_voltage_table->entries[i-1].vddc);
+
+	if (allowed_clock_voltage_table->entries[i-1].vddci) {
+		voltage->Vddci = tonga_get_voltage_id(&data->vddci_voltage_table,
+			allowed_clock_voltage_table->entries[i-1].vddci);
+	}
+	if (allowed_clock_voltage_table->entries[i-1].mvdd) {
+		*mvdd = (uint32_t) allowed_clock_voltage_table->entries[i-1].mvdd;
+	}
+
+	return 0;
+}
+
+/**
+ * Call SMC to reset S0/S1 to S1 and Reset SMIO to initial value
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_reset_to_default(struct pp_hwmgr *hwmgr)
+{
+	return (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_ResetToDefaults) == 0) ? 0 : 1;
+}
+
+int tonga_populate_memory_timing_parameters(
+		struct pp_hwmgr *hwmgr,
+		uint32_t engine_clock,
+		uint32_t memory_clock,
+		struct SMU72_Discrete_MCArbDramTimingTableEntry *arb_regs
+		)
+{
+	uint32_t dramTiming;
+	uint32_t dramTiming2;
+	uint32_t burstTime;
+	int result;
+
+	result = atomctrl_set_engine_dram_timings_rv770(hwmgr,
+				engine_clock, memory_clock);
+
+	PP_ASSERT_WITH_CODE(result == 0,
+		"Error calling VBIOS to set DRAM_TIMING.", return result);
+
+	dramTiming  = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING);
+	dramTiming2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2);
+	burstTime = PHM_READ_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE0);
+
+	arb_regs->McArbDramTiming  = PP_HOST_TO_SMC_UL(dramTiming);
+	arb_regs->McArbDramTiming2 = PP_HOST_TO_SMC_UL(dramTiming2);
+	arb_regs->McArbBurstTime = (uint8_t)burstTime;
+
+	return 0;
+}
+
+/**
+ * Setup parameters for the MC ARB.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ * This function is to be called from the SetPowerState table.
+ */
+int tonga_program_memory_timing_parameters(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	int result = 0;
+	SMU72_Discrete_MCArbDramTimingTable  arb_regs;
+	uint32_t i, j;
+
+	memset(&arb_regs, 0x00, sizeof(SMU72_Discrete_MCArbDramTimingTable));
+
+	for (i = 0; i < data->dpm_table.sclk_table.count; i++) {
+		for (j = 0; j < data->dpm_table.mclk_table.count; j++) {
+			result = tonga_populate_memory_timing_parameters
+				(hwmgr, data->dpm_table.sclk_table.dpm_levels[i].value,
+				 data->dpm_table.mclk_table.dpm_levels[j].value,
+				 &arb_regs.entries[i][j]);
+
+			if (0 != result) {
+				break;
+			}
+		}
+	}
+
+	if (0 == result) {
+		result = tonga_copy_bytes_to_smc(
+				hwmgr->smumgr,
+				data->arb_table_start,
+				(uint8_t *)&arb_regs,
+				sizeof(SMU72_Discrete_MCArbDramTimingTable),
+				data->sram_end
+				);
+	}
+
+	return result;
+}
+
+static int tonga_populate_smc_link_level(struct pp_hwmgr *hwmgr, SMU72_Discrete_DpmTable *table)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_dpm_table *dpm_table = &data->dpm_table;
+	uint32_t i;
+
+	/* Index (dpm_table->pcie_speed_table.count) is reserved for PCIE boot level. */
+	for (i = 0; i <= dpm_table->pcie_speed_table.count; i++) {
+		table->LinkLevel[i].PcieGenSpeed  =
+			(uint8_t)dpm_table->pcie_speed_table.dpm_levels[i].value;
+		table->LinkLevel[i].PcieLaneCount =
+			(uint8_t)encode_pcie_lane_width(dpm_table->pcie_speed_table.dpm_levels[i].param1);
+		table->LinkLevel[i].EnabledForActivity =
+			1;
+		table->LinkLevel[i].SPC =
+			(uint8_t)(data->pcie_spc_cap & 0xff);
+		table->LinkLevel[i].DownThreshold =
+			PP_HOST_TO_SMC_UL(5);
+		table->LinkLevel[i].UpThreshold =
+			PP_HOST_TO_SMC_UL(30);
+	}
+
+	data->smc_state_table.LinkLevelCount =
+		(uint8_t)dpm_table->pcie_speed_table.count;
+	data->dpm_level_enable_mask.pcie_dpm_enable_mask =
+		tonga_get_dpm_level_enable_mask_value(&dpm_table->pcie_speed_table);
+
+	return 0;
+}
+
+static int tonga_populate_smc_uvd_level(struct pp_hwmgr *hwmgr,
+					SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+
+	uint8_t count;
+	pp_atomctrl_clock_dividers_vi dividers;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	table->UvdLevelCount = (uint8_t) (mm_table->count);
+	table->UvdBootLevel = 0;
+
+	for (count = 0; count < table->UvdLevelCount; count++) {
+		table->UvdLevel[count].VclkFrequency = mm_table->entries[count].vclk;
+		table->UvdLevel[count].DclkFrequency = mm_table->entries[count].dclk;
+		table->UvdLevel[count].MinVoltage.Vddc =
+			tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+						mm_table->entries[count].vddc);
+		table->UvdLevel[count].MinVoltage.VddGfx =
+			(data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) ?
+			tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+						mm_table->entries[count].vddgfx) : 0;
+		table->UvdLevel[count].MinVoltage.Vddci =
+			tonga_get_voltage_id(&data->vddci_voltage_table,
+					     mm_table->entries[count].vddc - data->vddc_vddci_delta);
+		table->UvdLevel[count].MinVoltage.Phases = 1;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+							  table->UvdLevel[count].VclkFrequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				    "can not find divide id for Vclk clock", return result);
+
+		table->UvdLevel[count].VclkDivider = (uint8_t)dividers.pll_post_divider;
+
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+							  table->UvdLevel[count].DclkFrequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				    "can not find divide id for Dclk clock", return result);
+
+		table->UvdLevel[count].DclkDivider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->UvdLevel[count].VclkFrequency);
+		CONVERT_FROM_HOST_TO_SMC_UL(table->UvdLevel[count].DclkFrequency);
+		//CONVERT_FROM_HOST_TO_SMC_UL((uint32_t)table->UvdLevel[count].MinVoltage);
+	}
+
+	return result;
+
+}
+
+static int tonga_populate_smc_vce_level(struct pp_hwmgr *hwmgr,
+		SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+
+	uint8_t count;
+	pp_atomctrl_clock_dividers_vi dividers;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	table->VceLevelCount = (uint8_t) (mm_table->count);
+	table->VceBootLevel = 0;
+
+	for (count = 0; count < table->VceLevelCount; count++) {
+		table->VceLevel[count].Frequency =
+			mm_table->entries[count].eclk;
+		table->VceLevel[count].MinVoltage.Vddc =
+			tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+				mm_table->entries[count].vddc);
+		table->VceLevel[count].MinVoltage.VddGfx =
+			(data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) ?
+			tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+				mm_table->entries[count].vddgfx) : 0;
+		table->VceLevel[count].MinVoltage.Vddci =
+			tonga_get_voltage_id(&data->vddci_voltage_table,
+				mm_table->entries[count].vddc - data->vddc_vddci_delta);
+		table->VceLevel[count].MinVoltage.Phases = 1;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+					table->VceLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"can not find divide id for VCE engine clock", return result);
+
+		table->VceLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->VceLevel[count].Frequency);
+	}
+
+	return result;
+}
+
+static int tonga_populate_smc_acp_level(struct pp_hwmgr *hwmgr,
+		SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+	uint8_t count;
+	pp_atomctrl_clock_dividers_vi dividers;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	table->AcpLevelCount = (uint8_t) (mm_table->count);
+	table->AcpBootLevel = 0;
+
+	for (count = 0; count < table->AcpLevelCount; count++) {
+		table->AcpLevel[count].Frequency =
+			pptable_info->mm_dep_table->entries[count].aclk;
+		table->AcpLevel[count].MinVoltage.Vddc =
+			tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+			mm_table->entries[count].vddc);
+		table->AcpLevel[count].MinVoltage.VddGfx =
+			(data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) ?
+			tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+				mm_table->entries[count].vddgfx) : 0;
+		table->AcpLevel[count].MinVoltage.Vddci =
+			tonga_get_voltage_id(&data->vddci_voltage_table,
+				mm_table->entries[count].vddc - data->vddc_vddci_delta);
+		table->AcpLevel[count].MinVoltage.Phases = 1;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+			table->AcpLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+			"can not find divide id for engine clock", return result);
+
+		table->AcpLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->AcpLevel[count].Frequency);
+	}
+
+	return result;
+}
+
+static int tonga_populate_smc_samu_level(struct pp_hwmgr *hwmgr,
+	SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+	uint8_t count;
+	pp_atomctrl_clock_dividers_vi dividers;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	table->SamuBootLevel = 0;
+	table->SamuLevelCount = (uint8_t) (mm_table->count);
+
+	for (count = 0; count < table->SamuLevelCount; count++) {
+		/* not sure whether we need evclk or not */
+		table->SamuLevel[count].Frequency =
+			pptable_info->mm_dep_table->entries[count].samclock;
+		table->SamuLevel[count].MinVoltage.Vddc =
+			tonga_get_voltage_index(pptable_info->vddc_lookup_table,
+				mm_table->entries[count].vddc);
+		table->SamuLevel[count].MinVoltage.VddGfx =
+			(data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) ?
+			tonga_get_voltage_index(pptable_info->vddgfx_lookup_table,
+				mm_table->entries[count].vddgfx) : 0;
+		table->SamuLevel[count].MinVoltage.Vddci =
+			tonga_get_voltage_id(&data->vddci_voltage_table,
+				mm_table->entries[count].vddc - data->vddc_vddci_delta);
+		table->SamuLevel[count].MinVoltage.Phases = 1;
+
+		/* retrieve divider value for VBIOS */
+		result = atomctrl_get_dfs_pll_dividers_vi(hwmgr,
+					table->SamuLevel[count].Frequency, &dividers);
+		PP_ASSERT_WITH_CODE((0 == result),
+			"can not find divide id for samu clock", return result);
+
+		table->SamuLevel[count].Divider = (uint8_t)dividers.pll_post_divider;
+
+		CONVERT_FROM_HOST_TO_SMC_UL(table->SamuLevel[count].Frequency);
+	}
+
+	return result;
+}
+
+/**
+ * Populates the SMC MCLK structure using the provided memory clock
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    memory_clock the memory clock to use to populate the structure
+ * @param    sclk        the SMC SCLK structure to be populated
+ */
+static int tonga_calculate_mclk_params(
+		struct pp_hwmgr *hwmgr,
+		uint32_t memory_clock,
+		SMU72_Discrete_MemoryLevel *mclk,
+		bool strobe_mode,
+		bool dllStateOn
+		)
+{
+	const tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	uint32_t  dll_cntl = data->clock_registers.vDLL_CNTL;
+	uint32_t  mclk_pwrmgt_cntl = data->clock_registers.vMCLK_PWRMGT_CNTL;
+	uint32_t  mpll_ad_func_cntl = data->clock_registers.vMPLL_AD_FUNC_CNTL;
+	uint32_t  mpll_dq_func_cntl = data->clock_registers.vMPLL_DQ_FUNC_CNTL;
+	uint32_t  mpll_func_cntl = data->clock_registers.vMPLL_FUNC_CNTL;
+	uint32_t  mpll_func_cntl_1 = data->clock_registers.vMPLL_FUNC_CNTL_1;
+	uint32_t  mpll_func_cntl_2 = data->clock_registers.vMPLL_FUNC_CNTL_2;
+	uint32_t  mpll_ss1 = data->clock_registers.vMPLL_SS1;
+	uint32_t  mpll_ss2 = data->clock_registers.vMPLL_SS2;
+
+	pp_atomctrl_memory_clock_param mpll_param;
+	int result;
+
+	result = atomctrl_get_memory_pll_dividers_si(hwmgr,
+				memory_clock, &mpll_param, strobe_mode);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Error retrieving Memory Clock Parameters from VBIOS.", return result);
+
+	/* MPLL_FUNC_CNTL setup*/
+	mpll_func_cntl = PHM_SET_FIELD(mpll_func_cntl, MPLL_FUNC_CNTL, BWCTRL, mpll_param.bw_ctrl);
+
+	/* MPLL_FUNC_CNTL_1 setup*/
+	mpll_func_cntl_1  = PHM_SET_FIELD(mpll_func_cntl_1,
+							MPLL_FUNC_CNTL_1, CLKF, mpll_param.mpll_fb_divider.cl_kf);
+	mpll_func_cntl_1  = PHM_SET_FIELD(mpll_func_cntl_1,
+							MPLL_FUNC_CNTL_1, CLKFRAC, mpll_param.mpll_fb_divider.clk_frac);
+	mpll_func_cntl_1  = PHM_SET_FIELD(mpll_func_cntl_1,
+							MPLL_FUNC_CNTL_1, VCO_MODE, mpll_param.vco_mode);
+
+	/* MPLL_AD_FUNC_CNTL setup*/
+	mpll_ad_func_cntl = PHM_SET_FIELD(mpll_ad_func_cntl,
+							MPLL_AD_FUNC_CNTL, YCLK_POST_DIV, mpll_param.mpll_post_divider);
+
+	if (data->is_memory_GDDR5) {
+		/* MPLL_DQ_FUNC_CNTL setup*/
+		mpll_dq_func_cntl  = PHM_SET_FIELD(mpll_dq_func_cntl,
+								MPLL_DQ_FUNC_CNTL, YCLK_SEL, mpll_param.yclk_sel);
+		mpll_dq_func_cntl  = PHM_SET_FIELD(mpll_dq_func_cntl,
+								MPLL_DQ_FUNC_CNTL, YCLK_POST_DIV, mpll_param.mpll_post_divider);
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_MemorySpreadSpectrumSupport)) {
+		/*
+		 ************************************
+		 Fref = Reference Frequency
+		 NF = Feedback divider ratio
+		 NR = Reference divider ratio
+		 Fnom = Nominal VCO output frequency = Fref * NF / NR
+		 Fs = Spreading Rate
+		 D = Percentage down-spread / 2
+		 Fint = Reference input frequency to PFD = Fref / NR
+		 NS = Spreading rate divider ratio = int(Fint / (2 * Fs))
+		 CLKS = NS - 1 = ISS_STEP_NUM[11:0]
+		 NV = D * Fs / Fnom * 4 * ((Fnom/Fref * NR) ^ 2)
+		 CLKV = 65536 * NV = ISS_STEP_SIZE[25:0]
+		 *************************************
+		 */
+		pp_atomctrl_internal_ss_info ss_info;
+		uint32_t freq_nom;
+		uint32_t tmp;
+		uint32_t reference_clock = atomctrl_get_mpll_reference_clock(hwmgr);
+
+		/* for GDDR5 for all modes and DDR3 */
+		if (1 == mpll_param.qdr)
+			freq_nom = memory_clock * 4 * (1 << mpll_param.mpll_post_divider);
+		else
+			freq_nom = memory_clock * 2 * (1 << mpll_param.mpll_post_divider);
+
+		/* tmp = (freq_nom / reference_clock * reference_divider) ^ 2  Note: S.I. reference_divider = 1*/
+		tmp = (freq_nom / reference_clock);
+		tmp = tmp * tmp;
+
+		if (0 == atomctrl_get_memory_clock_spread_spectrum(hwmgr, freq_nom, &ss_info)) {
+			/* ss_info.speed_spectrum_percentage -- in unit of 0.01% */
+			/* ss.Info.speed_spectrum_rate -- in unit of khz */
+			/* CLKS = reference_clock / (2 * speed_spectrum_rate * reference_divider) * 10 */
+			/*     = reference_clock * 5 / speed_spectrum_rate */
+			uint32_t clks = reference_clock * 5 / ss_info.speed_spectrum_rate;
+
+			/* CLKV = 65536 * speed_spectrum_percentage / 2 * spreadSpecrumRate / freq_nom * 4 / 100000 * ((freq_nom / reference_clock) ^ 2) */
+			/*     = 131 * speed_spectrum_percentage * speed_spectrum_rate / 100 * ((freq_nom / reference_clock) ^ 2) / freq_nom */
+			uint32_t clkv =
+				(uint32_t)((((131 * ss_info.speed_spectrum_percentage *
+							ss_info.speed_spectrum_rate) / 100) * tmp) / freq_nom);
+
+			mpll_ss1 = PHM_SET_FIELD(mpll_ss1, MPLL_SS1, CLKV, clkv);
+			mpll_ss2 = PHM_SET_FIELD(mpll_ss2, MPLL_SS2, CLKS, clks);
+		}
+	}
+
+	/* MCLK_PWRMGT_CNTL setup */
+	mclk_pwrmgt_cntl = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, DLL_SPEED, mpll_param.dll_speed);
+	mclk_pwrmgt_cntl = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK0_PDNB, dllStateOn);
+	mclk_pwrmgt_cntl = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK1_PDNB, dllStateOn);
+
+
+	/* Save the result data to outpupt memory level structure */
+	mclk->MclkFrequency   = memory_clock;
+	mclk->MpllFuncCntl    = mpll_func_cntl;
+	mclk->MpllFuncCntl_1  = mpll_func_cntl_1;
+	mclk->MpllFuncCntl_2  = mpll_func_cntl_2;
+	mclk->MpllAdFuncCntl  = mpll_ad_func_cntl;
+	mclk->MpllDqFuncCntl  = mpll_dq_func_cntl;
+	mclk->MclkPwrmgtCntl  = mclk_pwrmgt_cntl;
+	mclk->DllCntl         = dll_cntl;
+	mclk->MpllSs1         = mpll_ss1;
+	mclk->MpllSs2         = mpll_ss2;
+
+	return 0;
+}
+
+static uint8_t tonga_get_mclk_frequency_ratio(uint32_t memory_clock,
+		bool strobe_mode)
+{
+	uint8_t mc_para_index;
+
+	if (strobe_mode) {
+		if (memory_clock < 12500) {
+			mc_para_index = 0x00;
+		} else if (memory_clock > 47500) {
+			mc_para_index = 0x0f;
+		} else {
+			mc_para_index = (uint8_t)((memory_clock - 10000) / 2500);
+		}
+	} else {
+		if (memory_clock < 65000) {
+			mc_para_index = 0x00;
+		} else if (memory_clock > 135000) {
+			mc_para_index = 0x0f;
+		} else {
+			mc_para_index = (uint8_t)((memory_clock - 60000) / 5000);
+		}
+	}
+
+	return mc_para_index;
+}
+
+static uint8_t tonga_get_ddr3_mclk_frequency_ratio(uint32_t memory_clock)
+{
+	uint8_t mc_para_index;
+
+	if (memory_clock < 10000) {
+		mc_para_index = 0;
+	} else if (memory_clock >= 80000) {
+		mc_para_index = 0x0f;
+	} else {
+		mc_para_index = (uint8_t)((memory_clock - 10000) / 5000 + 1);
+	}
+
+	return mc_para_index;
+}
+
+static int tonga_populate_single_memory_level(
+		struct pp_hwmgr *hwmgr,
+		uint32_t memory_clock,
+		SMU72_Discrete_MemoryLevel *memory_level
+		)
+{
+	uint32_t minMvdd = 0;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	int result = 0;
+	bool dllStateOn;
+	struct cgs_display_info info = {0};
+
+
+	if (NULL != pptable_info->vdd_dep_on_mclk) {
+		result = tonga_get_dependecy_volt_by_clk(hwmgr,
+			pptable_info->vdd_dep_on_mclk, memory_clock, &memory_level->MinVoltage, &minMvdd);
+		PP_ASSERT_WITH_CODE((0 == result),
+			"can not find MinVddc voltage value from memory VDDC voltage dependency table", return result);
+	}
+
+	if (data->mvdd_control == TONGA_VOLTAGE_CONTROL_NONE) {
+		memory_level->MinMvdd = data->vbios_boot_state.mvdd_bootup_value;
+	} else {
+		memory_level->MinMvdd = minMvdd;
+	}
+	memory_level->EnabledForThrottle = 1;
+	memory_level->EnabledForActivity = 0;
+	memory_level->UpHyst = 0;
+	memory_level->DownHyst = 100;
+	memory_level->VoltageDownHyst = 0;
+
+	/* Indicates maximum activity level for this performance level.*/
+	memory_level->ActivityLevel = (uint16_t)data->mclk_activity_target;
+	memory_level->StutterEnable = 0;
+	memory_level->StrobeEnable = 0;
+	memory_level->EdcReadEnable = 0;
+	memory_level->EdcWriteEnable = 0;
+	memory_level->RttEnable = 0;
+
+	/* default set to low watermark. Highest level will be set to high later.*/
+	memory_level->DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+	data->display_timing.num_existing_displays = info.display_count;
+
+	if ((data->mclk_stutter_mode_threshold != 0) &&
+			(memory_clock <= data->mclk_stutter_mode_threshold) &&
+			(data->is_uvd_enabled == 0)
+#if defined(LINUX)
+			&& (PHM_READ_FIELD(hwmgr->device, DPG_PIPE_STUTTER_CONTROL, STUTTER_ENABLE) & 0x1)
+			&& (data->display_timing.num_existing_displays <= 2)
+			&& (data->display_timing.num_existing_displays != 0)
+#endif
+	)
+		memory_level->StutterEnable = 1;
+
+	/* decide strobe mode*/
+	memory_level->StrobeEnable = (data->mclk_strobe_mode_threshold != 0) &&
+		(memory_clock <= data->mclk_strobe_mode_threshold);
+
+	/* decide EDC mode and memory clock ratio*/
+	if (data->is_memory_GDDR5) {
+		memory_level->StrobeRatio = tonga_get_mclk_frequency_ratio(memory_clock,
+					memory_level->StrobeEnable);
+
+		if ((data->mclk_edc_enable_threshold != 0) &&
+				(memory_clock > data->mclk_edc_enable_threshold)) {
+			memory_level->EdcReadEnable = 1;
+		}
+
+		if ((data->mclk_edc_wr_enable_threshold != 0) &&
+				(memory_clock > data->mclk_edc_wr_enable_threshold)) {
+			memory_level->EdcWriteEnable = 1;
+		}
+
+		if (memory_level->StrobeEnable) {
+			if (tonga_get_mclk_frequency_ratio(memory_clock, 1) >=
+					((cgs_read_register(hwmgr->device, mmMC_SEQ_MISC7) >> 16) & 0xf)) {
+				dllStateOn = ((cgs_read_register(hwmgr->device, mmMC_SEQ_MISC5) >> 1) & 0x1) ? 1 : 0;
+			} else {
+				dllStateOn = ((cgs_read_register(hwmgr->device, mmMC_SEQ_MISC6) >> 1) & 0x1) ? 1 : 0;
+			}
+
+		} else {
+			dllStateOn = data->dll_defaule_on;
+		}
+	} else {
+		memory_level->StrobeRatio =
+			tonga_get_ddr3_mclk_frequency_ratio(memory_clock);
+		dllStateOn = ((cgs_read_register(hwmgr->device, mmMC_SEQ_MISC5) >> 1) & 0x1) ? 1 : 0;
+	}
+
+	result = tonga_calculate_mclk_params(hwmgr,
+		memory_clock, memory_level, memory_level->StrobeEnable, dllStateOn);
+
+	if (0 == result) {
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MinMvdd);
+		/* MCLK frequency in units of 10KHz*/
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MclkFrequency);
+		/* Indicates maximum activity level for this performance level.*/
+		CONVERT_FROM_HOST_TO_SMC_US(memory_level->ActivityLevel);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllFuncCntl);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllFuncCntl_1);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllFuncCntl_2);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllAdFuncCntl);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllDqFuncCntl);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MclkPwrmgtCntl);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->DllCntl);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllSs1);
+		CONVERT_FROM_HOST_TO_SMC_UL(memory_level->MpllSs2);
+	}
+
+	return result;
+}
+
+/**
+ * Populates the SMC MVDD structure using the provided memory clock.
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    mclk        the MCLK value to be used in the decision if MVDD should be high or low.
+ * @param    voltage     the SMC VOLTAGE structure to be populated
+ */
+int tonga_populate_mvdd_value(struct pp_hwmgr *hwmgr, uint32_t mclk, SMIO_Pattern *smio_pattern)
+{
+	const tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint32_t i = 0;
+
+	if (TONGA_VOLTAGE_CONTROL_NONE != data->mvdd_control) {
+		/* find mvdd value which clock is more than request */
+		for (i = 0; i < pptable_info->vdd_dep_on_mclk->count; i++) {
+			if (mclk <= pptable_info->vdd_dep_on_mclk->entries[i].clk) {
+				/* Always round to higher voltage. */
+				smio_pattern->Voltage = data->mvdd_voltage_table.entries[i].value;
+				break;
+			}
+		}
+
+		PP_ASSERT_WITH_CODE(i < pptable_info->vdd_dep_on_mclk->count,
+			"MVDD Voltage is outside the supported range.", return -1);
+
+	} else {
+		return -1;
+	}
+
+	return 0;
+}
+
+
+static int tonga_populate_smv_acpi_level(struct pp_hwmgr *hwmgr,
+	SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+	const tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	pp_atomctrl_clock_dividers_vi dividers;
+	SMIO_Pattern voltage_level;
+	uint32_t spll_func_cntl    = data->clock_registers.vCG_SPLL_FUNC_CNTL;
+	uint32_t spll_func_cntl_2  = data->clock_registers.vCG_SPLL_FUNC_CNTL_2;
+	uint32_t dll_cntl          = data->clock_registers.vDLL_CNTL;
+	uint32_t mclk_pwrmgt_cntl  = data->clock_registers.vMCLK_PWRMGT_CNTL;
+
+	/* The ACPI state should not do DPM on DC (or ever).*/
+	table->ACPILevel.Flags &= ~PPSMC_SWSTATE_FLAG_DC;
+
+	table->ACPILevel.MinVoltage = data->smc_state_table.GraphicsLevel[0].MinVoltage;
+
+	/* assign zero for now*/
+	table->ACPILevel.SclkFrequency = atomctrl_get_reference_clock(hwmgr);
+
+	/* get the engine clock dividers for this clock value*/
+	result = atomctrl_get_engine_pll_dividers_vi(hwmgr,
+		table->ACPILevel.SclkFrequency,  &dividers);
+
+	PP_ASSERT_WITH_CODE(result == 0,
+		"Error retrieving Engine Clock dividers from VBIOS.", return result);
+
+	/* divider ID for required SCLK*/
+	table->ACPILevel.SclkDid = (uint8_t)dividers.pll_post_divider;
+	table->ACPILevel.DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+	table->ACPILevel.DeepSleepDivId = 0;
+
+	spll_func_cntl      = PHM_SET_FIELD(spll_func_cntl,
+							CG_SPLL_FUNC_CNTL,   SPLL_PWRON,     0);
+	spll_func_cntl      = PHM_SET_FIELD(spll_func_cntl,
+							CG_SPLL_FUNC_CNTL,   SPLL_RESET,     1);
+	spll_func_cntl_2    = PHM_SET_FIELD(spll_func_cntl_2,
+							CG_SPLL_FUNC_CNTL_2, SCLK_MUX_SEL,   4);
+
+	table->ACPILevel.CgSpllFuncCntl = spll_func_cntl;
+	table->ACPILevel.CgSpllFuncCntl2 = spll_func_cntl_2;
+	table->ACPILevel.CgSpllFuncCntl3 = data->clock_registers.vCG_SPLL_FUNC_CNTL_3;
+	table->ACPILevel.CgSpllFuncCntl4 = data->clock_registers.vCG_SPLL_FUNC_CNTL_4;
+	table->ACPILevel.SpllSpreadSpectrum = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM;
+	table->ACPILevel.SpllSpreadSpectrum2 = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2;
+	table->ACPILevel.CcPwrDynRm = 0;
+	table->ACPILevel.CcPwrDynRm1 = 0;
+
+
+	/* For various features to be enabled/disabled while this level is active.*/
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.Flags);
+	/* SCLK frequency in units of 10KHz*/
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SclkFrequency);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl3);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CgSpllFuncCntl4);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SpllSpreadSpectrum);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.SpllSpreadSpectrum2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CcPwrDynRm);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->ACPILevel.CcPwrDynRm1);
+
+	/* table->MemoryACPILevel.MinVddcPhases = table->ACPILevel.MinVddcPhases;*/
+	table->MemoryACPILevel.MinVoltage = data->smc_state_table.MemoryLevel[0].MinVoltage;
+
+	/*  CONVERT_FROM_HOST_TO_SMC_UL(table->MemoryACPILevel.MinVoltage);*/
+
+	if (0 == tonga_populate_mvdd_value(hwmgr, 0, &voltage_level))
+		table->MemoryACPILevel.MinMvdd =
+			PP_HOST_TO_SMC_UL(voltage_level.Voltage * VOLTAGE_SCALE);
+	else
+		table->MemoryACPILevel.MinMvdd = 0;
+
+	/* Force reset on DLL*/
+	mclk_pwrmgt_cntl    = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK0_RESET, 0x1);
+	mclk_pwrmgt_cntl    = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK1_RESET, 0x1);
+
+	/* Disable DLL in ACPIState*/
+	mclk_pwrmgt_cntl    = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK0_PDNB, 0);
+	mclk_pwrmgt_cntl    = PHM_SET_FIELD(mclk_pwrmgt_cntl,
+		MCLK_PWRMGT_CNTL, MRDCK1_PDNB, 0);
+
+	/* Enable DLL bypass signal*/
+	dll_cntl            = PHM_SET_FIELD(dll_cntl,
+		DLL_CNTL, MRDCK0_BYPASS, 0);
+	dll_cntl            = PHM_SET_FIELD(dll_cntl,
+		DLL_CNTL, MRDCK1_BYPASS, 0);
+
+	table->MemoryACPILevel.DllCntl            =
+		PP_HOST_TO_SMC_UL(dll_cntl);
+	table->MemoryACPILevel.MclkPwrmgtCntl     =
+		PP_HOST_TO_SMC_UL(mclk_pwrmgt_cntl);
+	table->MemoryACPILevel.MpllAdFuncCntl     =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_AD_FUNC_CNTL);
+	table->MemoryACPILevel.MpllDqFuncCntl     =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_DQ_FUNC_CNTL);
+	table->MemoryACPILevel.MpllFuncCntl       =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_FUNC_CNTL);
+	table->MemoryACPILevel.MpllFuncCntl_1     =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_FUNC_CNTL_1);
+	table->MemoryACPILevel.MpllFuncCntl_2     =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_FUNC_CNTL_2);
+	table->MemoryACPILevel.MpllSs1            =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_SS1);
+	table->MemoryACPILevel.MpllSs2            =
+		PP_HOST_TO_SMC_UL(data->clock_registers.vMPLL_SS2);
+
+	table->MemoryACPILevel.EnabledForThrottle = 0;
+	table->MemoryACPILevel.EnabledForActivity = 0;
+	table->MemoryACPILevel.UpHyst = 0;
+	table->MemoryACPILevel.DownHyst = 100;
+	table->MemoryACPILevel.VoltageDownHyst = 0;
+	/* Indicates maximum activity level for this performance level.*/
+	table->MemoryACPILevel.ActivityLevel = PP_HOST_TO_SMC_US((uint16_t)data->mclk_activity_target);
+
+	table->MemoryACPILevel.StutterEnable = 0;
+	table->MemoryACPILevel.StrobeEnable = 0;
+	table->MemoryACPILevel.EdcReadEnable = 0;
+	table->MemoryACPILevel.EdcWriteEnable = 0;
+	table->MemoryACPILevel.RttEnable = 0;
+
+	return result;
+}
+
+static int tonga_find_boot_level(struct tonga_single_dpm_table *table, uint32_t value, uint32_t *boot_level)
+{
+	int result = 0;
+	uint32_t i;
+
+	for (i = 0; i < table->count; i++) {
+		if (value == table->dpm_levels[i].value) {
+			*boot_level = i;
+			result = 0;
+		}
+	}
+	return result;
+}
+
+static int tonga_populate_smc_boot_level(struct pp_hwmgr *hwmgr,
+			SMU72_Discrete_DpmTable *table)
+{
+	int result = 0;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	table->GraphicsBootLevel  = 0;        /* 0 == DPM[0] (low), etc. */
+	table->MemoryBootLevel    = 0;        /* 0 == DPM[0] (low), etc. */
+
+	/* find boot level from dpm table*/
+	result = tonga_find_boot_level(&(data->dpm_table.sclk_table),
+	data->vbios_boot_state.sclk_bootup_value,
+	(uint32_t *)&(data->smc_state_table.GraphicsBootLevel));
+
+	if (0 != result) {
+		data->smc_state_table.GraphicsBootLevel = 0;
+		printk(KERN_ERR "[ powerplay ] VBIOS did not find boot engine clock value \
+			in dependency table. Using Graphics DPM level 0!");
+		result = 0;
+	}
+
+	result = tonga_find_boot_level(&(data->dpm_table.mclk_table),
+		data->vbios_boot_state.mclk_bootup_value,
+		(uint32_t *)&(data->smc_state_table.MemoryBootLevel));
+
+	if (0 != result) {
+		data->smc_state_table.MemoryBootLevel = 0;
+		printk(KERN_ERR "[ powerplay ] VBIOS did not find boot engine clock value \
+			in dependency table. Using Memory DPM level 0!");
+		result = 0;
+	}
+
+	table->BootVoltage.Vddc =
+		tonga_get_voltage_id(&(data->vddc_voltage_table),
+			data->vbios_boot_state.vddc_bootup_value);
+	table->BootVoltage.VddGfx =
+		tonga_get_voltage_id(&(data->vddgfx_voltage_table),
+			data->vbios_boot_state.vddgfx_bootup_value);
+	table->BootVoltage.Vddci =
+		tonga_get_voltage_id(&(data->vddci_voltage_table),
+			data->vbios_boot_state.vddci_bootup_value);
+	table->BootMVdd = data->vbios_boot_state.mvdd_bootup_value;
+
+	CONVERT_FROM_HOST_TO_SMC_US(table->BootMVdd);
+
+	return result;
+}
+
+
+/**
+ * Calculates the SCLK dividers using the provided engine clock
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    engine_clock the engine clock to use to populate the structure
+ * @param    sclk        the SMC SCLK structure to be populated
+ */
+int tonga_calculate_sclk_params(struct pp_hwmgr *hwmgr,
+		uint32_t engine_clock, SMU72_Discrete_GraphicsLevel *sclk)
+{
+	const tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	pp_atomctrl_clock_dividers_vi dividers;
+	uint32_t spll_func_cntl            = data->clock_registers.vCG_SPLL_FUNC_CNTL;
+	uint32_t spll_func_cntl_3          = data->clock_registers.vCG_SPLL_FUNC_CNTL_3;
+	uint32_t spll_func_cntl_4          = data->clock_registers.vCG_SPLL_FUNC_CNTL_4;
+	uint32_t cg_spll_spread_spectrum   = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM;
+	uint32_t cg_spll_spread_spectrum_2 = data->clock_registers.vCG_SPLL_SPREAD_SPECTRUM_2;
+	uint32_t    reference_clock;
+	uint32_t reference_divider;
+	uint32_t fbdiv;
+	int result;
+
+	/* get the engine clock dividers for this clock value*/
+	result = atomctrl_get_engine_pll_dividers_vi(hwmgr, engine_clock,  &dividers);
+
+	PP_ASSERT_WITH_CODE(result == 0,
+		"Error retrieving Engine Clock dividers from VBIOS.", return result);
+
+	/* To get FBDIV we need to multiply this by 16384 and divide it by Fref.*/
+	reference_clock = atomctrl_get_reference_clock(hwmgr);
+
+	reference_divider = 1 + dividers.uc_pll_ref_div;
+
+	/* low 14 bits is fraction and high 12 bits is divider*/
+	fbdiv = dividers.ul_fb_div.ul_fb_divider & 0x3FFFFFF;
+
+	/* SPLL_FUNC_CNTL setup*/
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl,
+		CG_SPLL_FUNC_CNTL, SPLL_REF_DIV, dividers.uc_pll_ref_div);
+	spll_func_cntl = PHM_SET_FIELD(spll_func_cntl,
+		CG_SPLL_FUNC_CNTL, SPLL_PDIV_A,  dividers.uc_pll_post_div);
+
+	/* SPLL_FUNC_CNTL_3 setup*/
+	spll_func_cntl_3 = PHM_SET_FIELD(spll_func_cntl_3,
+		CG_SPLL_FUNC_CNTL_3, SPLL_FB_DIV, fbdiv);
+
+	/* set to use fractional accumulation*/
+	spll_func_cntl_3 = PHM_SET_FIELD(spll_func_cntl_3,
+		CG_SPLL_FUNC_CNTL_3, SPLL_DITHEN, 1);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EngineSpreadSpectrumSupport)) {
+		pp_atomctrl_internal_ss_info ss_info;
+
+		uint32_t vcoFreq = engine_clock * dividers.uc_pll_post_div;
+		if (0 == atomctrl_get_engine_clock_spread_spectrum(hwmgr, vcoFreq, &ss_info)) {
+			/*
+			* ss_info.speed_spectrum_percentage -- in unit of 0.01%
+			* ss_info.speed_spectrum_rate -- in unit of khz
+			*/
+			/* clks = reference_clock * 10 / (REFDIV + 1) / speed_spectrum_rate / 2 */
+			uint32_t clkS = reference_clock * 5 / (reference_divider * ss_info.speed_spectrum_rate);
+
+			/* clkv = 2 * D * fbdiv / NS */
+			uint32_t clkV = 4 * ss_info.speed_spectrum_percentage * fbdiv / (clkS * 10000);
+
+			cg_spll_spread_spectrum =
+				PHM_SET_FIELD(cg_spll_spread_spectrum, CG_SPLL_SPREAD_SPECTRUM, CLKS, clkS);
+			cg_spll_spread_spectrum =
+				PHM_SET_FIELD(cg_spll_spread_spectrum, CG_SPLL_SPREAD_SPECTRUM, SSEN, 1);
+			cg_spll_spread_spectrum_2 =
+				PHM_SET_FIELD(cg_spll_spread_spectrum_2, CG_SPLL_SPREAD_SPECTRUM_2, CLKV, clkV);
+		}
+	}
+
+	sclk->SclkFrequency        = engine_clock;
+	sclk->CgSpllFuncCntl3      = spll_func_cntl_3;
+	sclk->CgSpllFuncCntl4      = spll_func_cntl_4;
+	sclk->SpllSpreadSpectrum   = cg_spll_spread_spectrum;
+	sclk->SpllSpreadSpectrum2  = cg_spll_spread_spectrum_2;
+	sclk->SclkDid              = (uint8_t)dividers.pll_post_divider;
+
+	return 0;
+}
+
+/**
+ * Populates single SMC SCLK structure using the provided engine clock
+ *
+ * @param    hwmgr      the address of the hardware manager
+ * @param    engine_clock the engine clock to use to populate the structure
+ * @param    sclk        the SMC SCLK structure to be populated
+ */
+static int tonga_populate_single_graphic_level(struct pp_hwmgr *hwmgr, uint32_t engine_clock, uint16_t sclk_activity_level_threshold, SMU72_Discrete_GraphicsLevel *graphic_level)
+{
+	int result;
+	uint32_t threshold;
+	uint32_t mvdd;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	result = tonga_calculate_sclk_params(hwmgr, engine_clock, graphic_level);
+
+
+	/* populate graphics levels*/
+	result = tonga_get_dependecy_volt_by_clk(hwmgr,
+		pptable_info->vdd_dep_on_sclk, engine_clock,
+		&graphic_level->MinVoltage, &mvdd);
+	PP_ASSERT_WITH_CODE((0 == result),
+		"can not find VDDC voltage value for VDDC	\
+		engine clock dependency table", return result);
+
+	/* SCLK frequency in units of 10KHz*/
+	graphic_level->SclkFrequency = engine_clock;
+
+	/* Indicates maximum activity level for this performance level. 50% for now*/
+	graphic_level->ActivityLevel = sclk_activity_level_threshold;
+
+	graphic_level->CcPwrDynRm = 0;
+	graphic_level->CcPwrDynRm1 = 0;
+	/* this level can be used if activity is high enough.*/
+	graphic_level->EnabledForActivity = 0;
+	/* this level can be used for throttling.*/
+	graphic_level->EnabledForThrottle = 1;
+	graphic_level->UpHyst = 0;
+	graphic_level->DownHyst = 0;
+	graphic_level->VoltageDownHyst = 0;
+	graphic_level->PowerThrottle = 0;
+
+	threshold = engine_clock * data->fast_watemark_threshold / 100;
+/*
+	*get the DAL clock. do it in funture.
+	PECI_GetMinClockSettings(hwmgr->peci, &minClocks);
+	data->display_timing.min_clock_insr = minClocks.engineClockInSR;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_SclkDeepSleep))
+	{
+		graphic_level->DeepSleepDivId = PhwTonga_GetSleepDividerIdFromClock(hwmgr, engine_clock, minClocks.engineClockInSR);
+	}
+*/
+
+	/* Default to slow, highest DPM level will be set to PPSMC_DISPLAY_WATERMARK_LOW later.*/
+	graphic_level->DisplayWatermark = PPSMC_DISPLAY_WATERMARK_LOW;
+
+	if (0 == result) {
+		/* CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->MinVoltage);*/
+		/* CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->MinVddcPhases);*/
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->SclkFrequency);
+		CONVERT_FROM_HOST_TO_SMC_US(graphic_level->ActivityLevel);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->CgSpllFuncCntl3);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->CgSpllFuncCntl4);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->SpllSpreadSpectrum);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->SpllSpreadSpectrum2);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->CcPwrDynRm);
+		CONVERT_FROM_HOST_TO_SMC_UL(graphic_level->CcPwrDynRm1);
+	}
+
+	return result;
+}
+
+/**
+ * Populates all SMC SCLK levels' structure based on the trimmed allowed dpm engine clock states
+ *
+ * @param    hwmgr      the address of the hardware manager
+ */
+static int tonga_populate_all_graphic_levels(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	struct tonga_dpm_table *dpm_table = &data->dpm_table;
+	phm_ppt_v1_pcie_table *pcie_table = pptable_info->pcie_table;
+	uint8_t pcie_entry_count = (uint8_t) data->dpm_table.pcie_speed_table.count;
+	int result = 0;
+	uint32_t level_array_adress = data->dpm_table_start +
+		offsetof(SMU72_Discrete_DpmTable, GraphicsLevel);
+	uint32_t level_array_size = sizeof(SMU72_Discrete_GraphicsLevel) *
+		SMU72_MAX_LEVELS_GRAPHICS;   /* 64 -> long; 32 -> int*/
+	SMU72_Discrete_GraphicsLevel *levels = data->smc_state_table.GraphicsLevel;
+	uint32_t i, maxEntry;
+	uint8_t highest_pcie_level_enabled = 0, lowest_pcie_level_enabled = 0, mid_pcie_level_enabled = 0, count = 0;
+	PECI_RegistryValue reg_value;
+	memset(levels, 0x00, level_array_size);
+
+	for (i = 0; i < dpm_table->sclk_table.count; i++) {
+		result = tonga_populate_single_graphic_level(hwmgr,
+					dpm_table->sclk_table.dpm_levels[i].value,
+					(uint16_t)data->activity_target[i],
+					&(data->smc_state_table.GraphicsLevel[i]));
+
+		if (0 != result)
+			return result;
+
+		/* Making sure only DPM level 0-1 have Deep Sleep Div ID populated. */
+		if (i > 1)
+			data->smc_state_table.GraphicsLevel[i].DeepSleepDivId = 0;
+
+		if (0 == i) {
+			reg_value = 0;
+			if (reg_value != 0)
+				data->smc_state_table.GraphicsLevel[0].UpHyst = (uint8_t)reg_value;
+		}
+
+		if (1 == i) {
+			reg_value = 0;
+			if (reg_value != 0)
+				data->smc_state_table.GraphicsLevel[1].UpHyst = (uint8_t)reg_value;
+		}
+	}
+
+	/* Only enable level 0 for now. */
+	data->smc_state_table.GraphicsLevel[0].EnabledForActivity = 1;
+
+	/* set highest level watermark to high */
+	if (dpm_table->sclk_table.count > 1)
+		data->smc_state_table.GraphicsLevel[dpm_table->sclk_table.count-1].DisplayWatermark =
+			PPSMC_DISPLAY_WATERMARK_HIGH;
+
+	data->smc_state_table.GraphicsDpmLevelCount =
+		(uint8_t)dpm_table->sclk_table.count;
+	data->dpm_level_enable_mask.sclk_dpm_enable_mask =
+		tonga_get_dpm_level_enable_mask_value(&dpm_table->sclk_table);
+
+	if (pcie_table != NULL) {
+		PP_ASSERT_WITH_CODE((pcie_entry_count >= 1),
+			"There must be 1 or more PCIE levels defined in PPTable.", return -1);
+		maxEntry = pcie_entry_count - 1; /* for indexing, we need to decrement by 1.*/
+		for (i = 0; i < dpm_table->sclk_table.count; i++) {
+			data->smc_state_table.GraphicsLevel[i].pcieDpmLevel =
+				(uint8_t) ((i < maxEntry) ? i : maxEntry);
+		}
+	} else {
+		if (0 == data->dpm_level_enable_mask.pcie_dpm_enable_mask)
+			printk(KERN_ERR "[ powerplay ] Pcie Dpm Enablemask is 0!");
+
+		while (data->dpm_level_enable_mask.pcie_dpm_enable_mask &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+					(1<<(highest_pcie_level_enabled+1))) != 0)) {
+			highest_pcie_level_enabled++;
+		}
+
+		while (data->dpm_level_enable_mask.pcie_dpm_enable_mask &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+					(1<<lowest_pcie_level_enabled)) == 0)) {
+			lowest_pcie_level_enabled++;
+		}
+
+		while ((count < highest_pcie_level_enabled) &&
+				((data->dpm_level_enable_mask.pcie_dpm_enable_mask &
+					(1<<(lowest_pcie_level_enabled+1+count))) == 0)) {
+			count++;
+		}
+		mid_pcie_level_enabled = (lowest_pcie_level_enabled+1+count) < highest_pcie_level_enabled ?
+			(lowest_pcie_level_enabled+1+count) : highest_pcie_level_enabled;
+
+
+		/* set pcieDpmLevel to highest_pcie_level_enabled*/
+		for (i = 2; i < dpm_table->sclk_table.count; i++) {
+			data->smc_state_table.GraphicsLevel[i].pcieDpmLevel = highest_pcie_level_enabled;
+		}
+
+		/* set pcieDpmLevel to lowest_pcie_level_enabled*/
+		data->smc_state_table.GraphicsLevel[0].pcieDpmLevel = lowest_pcie_level_enabled;
+
+		/* set pcieDpmLevel to mid_pcie_level_enabled*/
+		data->smc_state_table.GraphicsLevel[1].pcieDpmLevel = mid_pcie_level_enabled;
+	}
+	/* level count will send to smc once at init smc table and never change*/
+	result = tonga_copy_bytes_to_smc(hwmgr->smumgr, level_array_adress, (uint8_t *)levels, (uint32_t)level_array_size, data->sram_end);
+
+	if (0 != result)
+		return result;
+
+	return 0;
+}
+
+/**
+ * Populates all SMC MCLK levels' structure based on the trimmed allowed dpm memory clock states
+ *
+ * @param    hwmgr      the address of the hardware manager
+ */
+
+static int tonga_populate_all_memory_levels(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_dpm_table *dpm_table = &data->dpm_table;
+	int result;
+	/* populate MCLK dpm table to SMU7 */
+	uint32_t level_array_adress = data->dpm_table_start + offsetof(SMU72_Discrete_DpmTable, MemoryLevel);
+	uint32_t level_array_size = sizeof(SMU72_Discrete_MemoryLevel) * SMU72_MAX_LEVELS_MEMORY;
+	SMU72_Discrete_MemoryLevel *levels = data->smc_state_table.MemoryLevel;
+	uint32_t i;
+
+	memset(levels, 0x00, level_array_size);
+
+	for (i = 0; i < dpm_table->mclk_table.count; i++) {
+		PP_ASSERT_WITH_CODE((0 != dpm_table->mclk_table.dpm_levels[i].value),
+			"can not populate memory level as memory clock is zero", return -1);
+		result = tonga_populate_single_memory_level(hwmgr, dpm_table->mclk_table.dpm_levels[i].value,
+			&(data->smc_state_table.MemoryLevel[i]));
+		if (0 != result) {
+			return result;
+		}
+	}
+
+	/* Only enable level 0 for now.*/
+	data->smc_state_table.MemoryLevel[0].EnabledForActivity = 1;
+
+	/*
+	* in order to prevent MC activity from stutter mode to push DPM up.
+	* the UVD change complements this by putting the MCLK in a higher state
+	* by default such that we are not effected by up threshold or and MCLK DPM latency.
+	*/
+	data->smc_state_table.MemoryLevel[0].ActivityLevel = 0x1F;
+	CONVERT_FROM_HOST_TO_SMC_US(data->smc_state_table.MemoryLevel[0].ActivityLevel);
+
+	data->smc_state_table.MemoryDpmLevelCount = (uint8_t)dpm_table->mclk_table.count;
+	data->dpm_level_enable_mask.mclk_dpm_enable_mask = tonga_get_dpm_level_enable_mask_value(&dpm_table->mclk_table);
+	/* set highest level watermark to high*/
+	data->smc_state_table.MemoryLevel[dpm_table->mclk_table.count-1].DisplayWatermark = PPSMC_DISPLAY_WATERMARK_HIGH;
+
+	/* level count will send to smc once at init smc table and never change*/
+	result = tonga_copy_bytes_to_smc(hwmgr->smumgr,
+		level_array_adress, (uint8_t *)levels, (uint32_t)level_array_size, data->sram_end);
+
+	if (0 != result) {
+		return result;
+	}
+
+	return 0;
+}
+
+struct TONGA_DLL_SPEED_SETTING {
+	uint16_t            Min;                          /* Minimum Data Rate*/
+	uint16_t            Max;                          /* Maximum Data Rate*/
+	uint32_t 			dll_speed;                     /* The desired DLL_SPEED setting*/
+};
+
+static int tonga_populate_clock_stretcher_data_table(struct pp_hwmgr *hwmgr)
+{
+	return 0;
+}
+
+/* ---------------------------------------- ULV related functions ----------------------------------------------------*/
+
+
+static int tonga_reset_single_dpm_table(
+	struct pp_hwmgr *hwmgr,
+	struct tonga_single_dpm_table *dpm_table,
+	uint32_t count)
+{
+	uint32_t i;
+	if (!(count <= MAX_REGULAR_DPM_NUMBER))
+		printk(KERN_ERR "[ powerplay ] Fatal error, can not set up single DPM \
+			table entries to exceed max number! \n");
+
+	dpm_table->count = count;
+	for (i = 0; i < MAX_REGULAR_DPM_NUMBER; i++) {
+		dpm_table->dpm_levels[i].enabled = 0;
+	}
+
+	return 0;
+}
+
+static void tonga_setup_pcie_table_entry(
+	struct tonga_single_dpm_table *dpm_table,
+	uint32_t index, uint32_t pcie_gen,
+	uint32_t pcie_lanes)
+{
+	dpm_table->dpm_levels[index].value = pcie_gen;
+	dpm_table->dpm_levels[index].param1 = pcie_lanes;
+	dpm_table->dpm_levels[index].enabled = 1;
+}
+
+static int tonga_setup_default_pcie_tables(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_pcie_table *pcie_table = pptable_info->pcie_table;
+	uint32_t i, maxEntry;
+
+	if (data->use_pcie_performance_levels && !data->use_pcie_power_saving_levels) {
+		data->pcie_gen_power_saving = data->pcie_gen_performance;
+		data->pcie_lane_power_saving = data->pcie_lane_performance;
+	} else if (!data->use_pcie_performance_levels && data->use_pcie_power_saving_levels) {
+		data->pcie_gen_performance = data->pcie_gen_power_saving;
+		data->pcie_lane_performance = data->pcie_lane_power_saving;
+	}
+
+	tonga_reset_single_dpm_table(hwmgr, &data->dpm_table.pcie_speed_table, SMU72_MAX_LEVELS_LINK);
+
+	if (pcie_table != NULL) {
+		/*
+		* maxEntry is used to make sure we reserve one PCIE level for boot level (fix for A+A PSPP issue).
+		* If PCIE table from PPTable have ULV entry + 8 entries, then ignore the last entry.
+		*/
+		maxEntry = (SMU72_MAX_LEVELS_LINK < pcie_table->count) ?
+						SMU72_MAX_LEVELS_LINK : pcie_table->count;
+		for (i = 1; i < maxEntry; i++) {
+			tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, i-1,
+				get_pcie_gen_support(data->pcie_gen_cap, pcie_table->entries[i].gen_speed),
+				get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		}
+		data->dpm_table.pcie_speed_table.count = maxEntry - 1;
+	} else {
+		/* Hardcode Pcie Table */
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 0,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Min_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 1,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Min_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 2,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Max_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 3,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Max_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 4,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Max_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table, 5,
+			get_pcie_gen_support(data->pcie_gen_cap, PP_Max_PCIEGen),
+			get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+		data->dpm_table.pcie_speed_table.count = 6;
+	}
+	/* Populate last level for boot PCIE level, but do not increment count. */
+	tonga_setup_pcie_table_entry(&data->dpm_table.pcie_speed_table,
+		data->dpm_table.pcie_speed_table.count,
+		get_pcie_gen_support(data->pcie_gen_cap, PP_Min_PCIEGen),
+		get_pcie_lane_support(data->pcie_lane_cap, PP_Max_PCIELane));
+
+	return 0;
+
+}
+
+/*
+ * This function is to initalize all DPM state tables for SMU7 based on the dependency table.
+ * Dynamic state patching function will then trim these state tables to the allowed range based
+ * on the power policy or external client requests, such as UVD request, etc.
+ */
+static int tonga_setup_default_dpm_tables(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint32_t i;
+
+	phm_ppt_v1_clock_voltage_dependency_table *allowed_vdd_sclk_table =
+		pptable_info->vdd_dep_on_sclk;
+	phm_ppt_v1_clock_voltage_dependency_table *allowed_vdd_mclk_table =
+		pptable_info->vdd_dep_on_mclk;
+
+	PP_ASSERT_WITH_CODE(allowed_vdd_sclk_table != NULL,
+		"SCLK dependency table is missing. This table is mandatory", return -1);
+	PP_ASSERT_WITH_CODE(allowed_vdd_sclk_table->count >= 1,
+		"SCLK dependency table has to have is missing. This table is mandatory", return -1);
+
+	PP_ASSERT_WITH_CODE(allowed_vdd_mclk_table != NULL,
+		"MCLK dependency table is missing. This table is mandatory", return -1);
+	PP_ASSERT_WITH_CODE(allowed_vdd_mclk_table->count >= 1,
+		"VMCLK dependency table has to have is missing. This table is mandatory", return -1);
+
+	/* clear the state table to reset everything to default */
+	memset(&(data->dpm_table), 0x00, sizeof(data->dpm_table));
+	tonga_reset_single_dpm_table(hwmgr, &data->dpm_table.sclk_table, SMU72_MAX_LEVELS_GRAPHICS);
+	tonga_reset_single_dpm_table(hwmgr, &data->dpm_table.mclk_table, SMU72_MAX_LEVELS_MEMORY);
+	/* tonga_reset_single_dpm_table(hwmgr, &tonga_hwmgr->dpm_table.VddcTable, SMU72_MAX_LEVELS_VDDC); */
+	/* tonga_reset_single_dpm_table(hwmgr, &tonga_hwmgr->dpm_table.vdd_gfx_table, SMU72_MAX_LEVELS_VDDGFX);*/
+	/* tonga_reset_single_dpm_table(hwmgr, &tonga_hwmgr->dpm_table.vdd_ci_table, SMU72_MAX_LEVELS_VDDCI);*/
+	/* tonga_reset_single_dpm_table(hwmgr, &tonga_hwmgr->dpm_table.mvdd_table, SMU72_MAX_LEVELS_MVDD);*/
+
+	PP_ASSERT_WITH_CODE(allowed_vdd_sclk_table != NULL,
+		"SCLK dependency table is missing. This table is mandatory", return -1);
+	/* Initialize Sclk DPM table based on allow Sclk values*/
+	data->dpm_table.sclk_table.count = 0;
+
+	for (i = 0; i < allowed_vdd_sclk_table->count; i++) {
+		if (i == 0 || data->dpm_table.sclk_table.dpm_levels[data->dpm_table.sclk_table.count-1].value !=
+				allowed_vdd_sclk_table->entries[i].clk) {
+			data->dpm_table.sclk_table.dpm_levels[data->dpm_table.sclk_table.count].value =
+				allowed_vdd_sclk_table->entries[i].clk;
+			data->dpm_table.sclk_table.dpm_levels[data->dpm_table.sclk_table.count].enabled = 1; /*(i==0) ? 1 : 0; to do */
+			data->dpm_table.sclk_table.count++;
+		}
+	}
+
+	PP_ASSERT_WITH_CODE(allowed_vdd_mclk_table != NULL,
+		"MCLK dependency table is missing. This table is mandatory", return -1);
+	/* Initialize Mclk DPM table based on allow Mclk values */
+	data->dpm_table.mclk_table.count = 0;
+	for (i = 0; i < allowed_vdd_mclk_table->count; i++) {
+		if (i == 0 || data->dpm_table.mclk_table.dpm_levels[data->dpm_table.mclk_table.count-1].value !=
+			allowed_vdd_mclk_table->entries[i].clk) {
+			data->dpm_table.mclk_table.dpm_levels[data->dpm_table.mclk_table.count].value =
+				allowed_vdd_mclk_table->entries[i].clk;
+			data->dpm_table.mclk_table.dpm_levels[data->dpm_table.mclk_table.count].enabled = 1; /*(i==0) ? 1 : 0; */
+			data->dpm_table.mclk_table.count++;
+		}
+	}
+
+	/* Initialize Vddc DPM table based on allow Vddc values.  And populate corresponding std values. */
+	for (i = 0; i < allowed_vdd_sclk_table->count; i++) {
+		data->dpm_table.vddc_table.dpm_levels[i].value = allowed_vdd_mclk_table->entries[i].vddc;
+		/* tonga_hwmgr->dpm_table.VddcTable.dpm_levels[i].param1 = stdVoltageTable->entries[i].Leakage; */
+		/* param1 is for corresponding std voltage */
+		data->dpm_table.vddc_table.dpm_levels[i].enabled = 1;
+	}
+	data->dpm_table.vddc_table.count = allowed_vdd_sclk_table->count;
+
+	if (NULL != allowed_vdd_mclk_table) {
+		/* Initialize Vddci DPM table based on allow Mclk values */
+		for (i = 0; i < allowed_vdd_mclk_table->count; i++) {
+			data->dpm_table.vdd_ci_table.dpm_levels[i].value = allowed_vdd_mclk_table->entries[i].vddci;
+			data->dpm_table.vdd_ci_table.dpm_levels[i].enabled = 1;
+			data->dpm_table.mvdd_table.dpm_levels[i].value = allowed_vdd_mclk_table->entries[i].mvdd;
+			data->dpm_table.mvdd_table.dpm_levels[i].enabled = 1;
+		}
+		data->dpm_table.vdd_ci_table.count = allowed_vdd_mclk_table->count;
+		data->dpm_table.mvdd_table.count = allowed_vdd_mclk_table->count;
+	}
+
+	/* setup PCIE gen speed levels*/
+	tonga_setup_default_pcie_tables(hwmgr);
+
+	/* save a copy of the default DPM table*/
+	memcpy(&(data->golden_dpm_table), &(data->dpm_table), sizeof(struct tonga_dpm_table));
+
+	return 0;
+}
+
+int tonga_populate_smc_initial_state(struct pp_hwmgr *hwmgr,
+		const struct tonga_power_state *bootState)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	uint8_t count, level;
+
+	count = (uint8_t) (pptable_info->vdd_dep_on_sclk->count);
+	for (level = 0; level < count; level++) {
+		if (pptable_info->vdd_dep_on_sclk->entries[level].clk >=
+			bootState->performance_levels[0].engine_clock) {
+			data->smc_state_table.GraphicsBootLevel = level;
+			break;
+		}
+	}
+
+	count = (uint8_t) (pptable_info->vdd_dep_on_mclk->count);
+	for (level = 0; level < count; level++) {
+		if (pptable_info->vdd_dep_on_mclk->entries[level].clk >=
+			bootState->performance_levels[0].memory_clock) {
+			data->smc_state_table.MemoryBootLevel = level;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Initializes the SMC table and uploads it
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @param    pInput  the pointer to input data (PowerState)
+ * @return   always 0
+ */
+int tonga_init_smc_table(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	SMU72_Discrete_DpmTable  *table = &(data->smc_state_table);
+	const phw_tonga_ulv_parm *ulv = &(data->ulv);
+	uint8_t i;
+	PECI_RegistryValue reg_value;
+	pp_atomctrl_gpio_pin_assignment gpio_pin_assignment;
+
+	result = tonga_setup_default_dpm_tables(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to setup default DPM tables!", return result;);
+	memset(&(data->smc_state_table), 0x00, sizeof(data->smc_state_table));
+	if (TONGA_VOLTAGE_CONTROL_NONE != data->voltage_control) {
+		tonga_populate_smc_voltage_tables(hwmgr, table);
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_AutomaticDCTransition)) {
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_GPIO_DC;
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_StepVddc)) {
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_STEPVDDC;
+	}
+
+	if (data->is_memory_GDDR5) {
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_GDDR5;
+	}
+
+	i = PHM_READ_FIELD(hwmgr->device, CC_MC_MAX_CHANNEL, NOOFCHAN);
+
+	if (i == 1 || i == 0) {
+		table->SystemFlags |= PPSMC_SYSTEMFLAG_12CHANNEL;
+	}
+
+	if (ulv->ulv_supported && pptable_info->us_ulv_voltage_offset) {
+		PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to initialize ULV state!", return result;);
+
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCG_ULV_PARAMETER, ulv->ch_ulv_parameter);
+	}
+
+	result = tonga_populate_smc_link_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize Link Level!", return result;);
+
+	result = tonga_populate_all_graphic_levels(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize Graphics Level!", return result;);
+
+	result = tonga_populate_all_memory_levels(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize Memory Level!", return result;);
+
+	result = tonga_populate_smv_acpi_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize ACPI Level!", return result;);
+
+	result = tonga_populate_smc_vce_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize VCE Level!", return result;);
+
+	result = tonga_populate_smc_acp_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize ACP Level!", return result;);
+
+	result = tonga_populate_smc_samu_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize SAMU Level!", return result;);
+
+	/* Since only the initial state is completely set up at this point (the other states are just copies of the boot state) we only */
+	/* need to populate the  ARB settings for the initial state. */
+	result = tonga_program_memory_timing_parameters(hwmgr);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to Write ARB settings for the initial state.", return result;);
+
+	result = tonga_populate_smc_uvd_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize UVD Level!", return result;);
+
+	result = tonga_populate_smc_boot_level(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize Boot Level!", return result;);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ClockStretcher)) {
+		result = tonga_populate_clock_stretcher_data_table(hwmgr);
+		PP_ASSERT_WITH_CODE(0 == result,
+			"Failed to populate Clock Stretcher Data Table!", return result;);
+	}
+	table->GraphicsVoltageChangeEnable  = 1;
+	table->GraphicsThermThrottleEnable  = 1;
+	table->GraphicsInterval = 1;
+	table->VoltageInterval  = 1;
+	table->ThermalInterval  = 1;
+	table->TemperatureLimitHigh =
+		pptable_info->cac_dtp_table->usTargetOperatingTemp *
+		TONGA_Q88_FORMAT_CONVERSION_UNIT;
+	table->TemperatureLimitLow =
+		(pptable_info->cac_dtp_table->usTargetOperatingTemp - 1) *
+		TONGA_Q88_FORMAT_CONVERSION_UNIT;
+	table->MemoryVoltageChangeEnable  = 1;
+	table->MemoryInterval  = 1;
+	table->VoltageResponseTime  = 0;
+	table->PhaseResponseTime  = 0;
+	table->MemoryThermThrottleEnable  = 1;
+
+	/*
+	* Cail reads current link status and reports it as cap (we cannot change this due to some previous issues we had)
+	* SMC drops the link status to lowest level after enabling DPM by PowerPlay. After pnp or toggling CF, driver gets reloaded again
+	* but this time Cail reads current link status which was set to low by SMC and reports it as cap to powerplay
+	* To avoid it, we set PCIeBootLinkLevel to highest dpm level
+	*/
+	PP_ASSERT_WITH_CODE((1 <= data->dpm_table.pcie_speed_table.count),
+			"There must be 1 or more PCIE levels defined in PPTable.",
+			return -1);
+
+	table->PCIeBootLinkLevel = (uint8_t) (data->dpm_table.pcie_speed_table.count);
+
+	table->PCIeGenInterval  = 1;
+
+	result = tonga_populate_vr_config(hwmgr, table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to populate VRConfig setting!", return result);
+
+	table->ThermGpio  = 17;
+	table->SclkStepSize = 0x4000;
+
+	reg_value = 0;
+	if ((0 == reg_value) &&
+		(0 == atomctrl_get_pp_assign_pin(hwmgr,
+			VDDC_VRHOT_GPIO_PINID, &gpio_pin_assignment))) {
+		table->VRHotGpio = gpio_pin_assignment.uc_gpio_pin_bit_shift;
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_RegulatorHot);
+	} else {
+		table->VRHotGpio = TONGA_UNUSED_GPIO_PIN;
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_RegulatorHot);
+	}
+
+	/* ACDC Switch GPIO */
+	reg_value = 0;
+	if ((0 == reg_value) &&
+		(0 == atomctrl_get_pp_assign_pin(hwmgr,
+			PP_AC_DC_SWITCH_GPIO_PINID, &gpio_pin_assignment))) {
+		table->AcDcGpio = gpio_pin_assignment.uc_gpio_pin_bit_shift;
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_AutomaticDCTransition);
+	} else {
+		table->AcDcGpio = TONGA_UNUSED_GPIO_PIN;
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_AutomaticDCTransition);
+	}
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_Falcon_QuickTransition);
+
+	reg_value = 0;
+	if (1 == reg_value) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_AutomaticDCTransition);
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_Falcon_QuickTransition);
+	}
+
+	reg_value = 0;
+	if ((0 == reg_value) &&
+		(0 == atomctrl_get_pp_assign_pin(hwmgr,
+			THERMAL_INT_OUTPUT_GPIO_PINID, &gpio_pin_assignment))) {
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ThermalOutGPIO);
+
+		table->ThermOutGpio = gpio_pin_assignment.uc_gpio_pin_bit_shift;
+
+		table->ThermOutPolarity =
+			(0 == (cgs_read_register(hwmgr->device, mmGPIOPAD_A) &
+			(1 << gpio_pin_assignment.uc_gpio_pin_bit_shift))) ? 1:0;
+
+		table->ThermOutMode = SMU7_THERM_OUT_MODE_THERM_ONLY;
+
+		/* if required, combine VRHot/PCC with thermal out GPIO*/
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_RegulatorHot) &&
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_CombinePCCWithThermalSignal)){
+			table->ThermOutMode = SMU7_THERM_OUT_MODE_THERM_VRHOT;
+		}
+	} else {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ThermalOutGPIO);
+
+		table->ThermOutGpio = 17;
+		table->ThermOutPolarity = 1;
+		table->ThermOutMode = SMU7_THERM_OUT_MODE_DISABLE;
+	}
+
+	for (i = 0; i < SMU72_MAX_ENTRIES_SMIO; i++) {
+		table->Smio[i] = PP_HOST_TO_SMC_UL(table->Smio[i]);
+	}
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SystemFlags);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->VRConfig);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SmioMask1);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SmioMask2);
+	CONVERT_FROM_HOST_TO_SMC_UL(table->SclkStepSize);
+	CONVERT_FROM_HOST_TO_SMC_US(table->TemperatureLimitHigh);
+	CONVERT_FROM_HOST_TO_SMC_US(table->TemperatureLimitLow);
+	CONVERT_FROM_HOST_TO_SMC_US(table->VoltageResponseTime);
+	CONVERT_FROM_HOST_TO_SMC_US(table->PhaseResponseTime);
+
+	/* Upload all dpm data to SMC memory.(dpm level, dpm level count etc) */
+	result = tonga_copy_bytes_to_smc(hwmgr->smumgr, data->dpm_table_start +
+										offsetof(SMU72_Discrete_DpmTable, SystemFlags),
+										(uint8_t *)&(table->SystemFlags),
+										sizeof(SMU72_Discrete_DpmTable)-3 * sizeof(SMU72_PIDController),
+										data->sram_end);
+
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to upload dpm data to SMC memory!", return result;);
+
+	return result;
+}
+
+/* Look up the voltaged based on DAL's requested level. and then send the requested VDDC voltage to SMC*/
+static void tonga_apply_dal_minimum_voltage_request(struct pp_hwmgr *hwmgr)
+{
+	return;
+}
+
+int tonga_upload_dpm_level_enable_mask(struct pp_hwmgr *hwmgr)
+{
+	PPSMC_Result result;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* Apply minimum voltage based on DAL's request level */
+	tonga_apply_dal_minimum_voltage_request(hwmgr);
+
+	if (0 == data->sclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+		if (0 != tonga_is_dpm_running(hwmgr))
+			printk(KERN_ERR "[ powerplay ] Trying to set Enable Mask when DPM is disabled \n");
+
+		if (0 != data->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+			result = smum_send_msg_to_smc_with_parameter(
+								hwmgr->smumgr,
+				(PPSMC_Msg)PPSMC_MSG_SCLKDPM_SetEnabledMask,
+				data->dpm_level_enable_mask.sclk_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == result),
+				"Set Sclk Dpm enable Mask failed", return -1);
+		}
+	}
+
+	if (0 == data->mclk_dpm_key_disabled) {
+		/* Checking if DPM is running.  If we discover hang because of this, we should skip this message.*/
+		if (0 != tonga_is_dpm_running(hwmgr))
+			printk(KERN_ERR "[ powerplay ] Trying to set Enable Mask when DPM is disabled \n");
+
+		if (0 != data->dpm_level_enable_mask.mclk_dpm_enable_mask) {
+			result = smum_send_msg_to_smc_with_parameter(
+								hwmgr->smumgr,
+				(PPSMC_Msg)PPSMC_MSG_MCLKDPM_SetEnabledMask,
+				data->dpm_level_enable_mask.mclk_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == result),
+				"Set Mclk Dpm enable Mask failed", return -1);
+		}
+	}
+
+	return 0;
+}
+
+
+int tonga_force_dpm_highest(struct pp_hwmgr *hwmgr)
+{
+	uint32_t level, tmp;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->pcie_dpm_key_disabled) {
+		/* PCIE */
+		if (data->dpm_level_enable_mask.pcie_dpm_enable_mask != 0) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.pcie_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++ ;
+
+			if (0 != level) {
+				PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state_pcie(hwmgr, level)),
+					"force highest pcie dpm state failed!", return -1);
+			}
+		}
+	}
+
+	if (0 == data->sclk_dpm_key_disabled) {
+		/* SCLK */
+		if (data->dpm_level_enable_mask.sclk_dpm_enable_mask != 0) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.sclk_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++ ;
+
+			if (0 != level) {
+				PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state(hwmgr, level)),
+					"force highest sclk dpm state failed!", return -1);
+				if (PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device,
+					CGS_IND_REG__SMC, TARGET_AND_CURRENT_PROFILE_INDEX, CURR_SCLK_INDEX) != level)
+					printk(KERN_ERR "[ powerplay ] Target_and_current_Profile_Index. \
+						Curr_Sclk_Index does not match the level \n");
+
+			}
+		}
+	}
+
+	if (0 == data->mclk_dpm_key_disabled) {
+		/* MCLK */
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask != 0) {
+			level = 0;
+			tmp = data->dpm_level_enable_mask.mclk_dpm_enable_mask;
+			while (tmp >>= 1)
+				level++ ;
+
+			if (0 != level) {
+				PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state_mclk(hwmgr, level)),
+					"force highest mclk dpm state failed!", return -1);
+				if (PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+					TARGET_AND_CURRENT_PROFILE_INDEX, CURR_MCLK_INDEX) != level)
+					printk(KERN_ERR "[ powerplay ] Target_and_current_Profile_Index. \
+						Curr_Mclk_Index does not match the level \n");
+			}
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Find the MC microcode version and store it in the HwMgr struct
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_get_mc_microcode_version (struct pp_hwmgr *hwmgr)
+{
+	cgs_write_register(hwmgr->device, mmMC_SEQ_IO_DEBUG_INDEX, 0x9F);
+
+	hwmgr->microcode_version_info.MC = cgs_read_register(hwmgr->device, mmMC_SEQ_IO_DEBUG_DATA);
+
+	return 0;
+}
+
+/**
+ * Initialize Dynamic State Adjustment Rule Settings
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ */
+int tonga_initializa_dynamic_state_adjustment_rule_settings(struct pp_hwmgr *hwmgr)
+{
+	uint32_t table_size;
+	struct phm_clock_voltage_dependency_table *table_clk_vlt;
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	hwmgr->dyn_state.mclk_sclk_ratio = 4;
+	hwmgr->dyn_state.sclk_mclk_delta = 15000;      /* 150 MHz */
+	hwmgr->dyn_state.vddc_vddci_delta = 200;       /* 200mV */
+
+	/* initialize vddc_dep_on_dal_pwrl table */
+	table_size = sizeof(uint32_t) + 4 * sizeof(struct phm_clock_voltage_dependency_record);
+	table_clk_vlt = (struct phm_clock_voltage_dependency_table *)kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table_clk_vlt) {
+		printk(KERN_ERR "[ powerplay ] Can not allocate space for vddc_dep_on_dal_pwrl! \n");
+		return -ENOMEM;
+	} else {
+		table_clk_vlt->count = 4;
+		table_clk_vlt->entries[0].clk = PP_DAL_POWERLEVEL_ULTRALOW;
+		table_clk_vlt->entries[0].v = 0;
+		table_clk_vlt->entries[1].clk = PP_DAL_POWERLEVEL_LOW;
+		table_clk_vlt->entries[1].v = 720;
+		table_clk_vlt->entries[2].clk = PP_DAL_POWERLEVEL_NOMINAL;
+		table_clk_vlt->entries[2].v = 810;
+		table_clk_vlt->entries[3].clk = PP_DAL_POWERLEVEL_PERFORMANCE;
+		table_clk_vlt->entries[3].v = 900;
+		pptable_info->vddc_dep_on_dal_pwrl = table_clk_vlt;
+		hwmgr->dyn_state.vddc_dep_on_dal_pwrl = table_clk_vlt;
+	}
+
+	return 0;
+}
+
+static int tonga_set_private_var_based_on_pptale(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	phm_ppt_v1_clock_voltage_dependency_table *allowed_sclk_vdd_table =
+		pptable_info->vdd_dep_on_sclk;
+	phm_ppt_v1_clock_voltage_dependency_table *allowed_mclk_vdd_table =
+		pptable_info->vdd_dep_on_mclk;
+
+	PP_ASSERT_WITH_CODE(allowed_sclk_vdd_table != NULL,
+		"VDD dependency on SCLK table is missing. 	\
+		This table is mandatory", return -1);
+	PP_ASSERT_WITH_CODE(allowed_sclk_vdd_table->count >= 1,
+		"VDD dependency on SCLK table has to have is missing. 	\
+		This table is mandatory", return -1);
+
+	PP_ASSERT_WITH_CODE(allowed_mclk_vdd_table != NULL,
+		"VDD dependency on MCLK table is missing. 	\
+		This table is mandatory", return -1);
+	PP_ASSERT_WITH_CODE(allowed_mclk_vdd_table->count >= 1,
+		"VDD dependency on MCLK table has to have is missing.	 \
+		This table is mandatory", return -1);
+
+	data->min_vddc_in_pp_table = (uint16_t)allowed_sclk_vdd_table->entries[0].vddc;
+	data->max_vddc_in_pp_table = (uint16_t)allowed_sclk_vdd_table->entries[allowed_sclk_vdd_table->count - 1].vddc;
+
+	pptable_info->max_clock_voltage_on_ac.sclk =
+		allowed_sclk_vdd_table->entries[allowed_sclk_vdd_table->count - 1].clk;
+	pptable_info->max_clock_voltage_on_ac.mclk =
+		allowed_mclk_vdd_table->entries[allowed_mclk_vdd_table->count - 1].clk;
+	pptable_info->max_clock_voltage_on_ac.vddc =
+		allowed_sclk_vdd_table->entries[allowed_sclk_vdd_table->count - 1].vddc;
+	pptable_info->max_clock_voltage_on_ac.vddci =
+		allowed_mclk_vdd_table->entries[allowed_mclk_vdd_table->count - 1].vddci;
+
+	hwmgr->dyn_state.max_clock_voltage_on_ac.sclk =
+		pptable_info->max_clock_voltage_on_ac.sclk;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.mclk =
+		pptable_info->max_clock_voltage_on_ac.mclk;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.vddc =
+		pptable_info->max_clock_voltage_on_ac.vddc;
+	hwmgr->dyn_state.max_clock_voltage_on_ac.vddci =
+		pptable_info->max_clock_voltage_on_ac.vddci;
+
+	return 0;
+}
+
+int tonga_unforce_dpm_levels(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	int result = 1;
+
+	PP_ASSERT_WITH_CODE (0 == tonga_is_dpm_running(hwmgr),
+		"Trying to Unforce DPM when DPM is disabled. Returning without sending SMC message.",
+							return result);
+
+	if (0 == data->pcie_dpm_key_disabled) {
+		PP_ASSERT_WITH_CODE((0 == smum_send_msg_to_smc(
+							     hwmgr->smumgr,
+					PPSMC_MSG_PCIeDPM_UnForceLevel)),
+					   "unforce pcie level failed!",
+								return -1);
+	}
+
+	result = tonga_upload_dpm_level_enable_mask(hwmgr);
+
+	return result;
+}
+
+static uint32_t tonga_get_lowest_enable_level(
+				struct pp_hwmgr *hwmgr, uint32_t level_mask)
+{
+	uint32_t level = 0;
+
+	while (0 == (level_mask & (1 << level)))
+		level++;
+
+	return level;
+}
+
+static int tonga_force_dpm_lowest(struct pp_hwmgr *hwmgr)
+{
+	uint32_t level;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->pcie_dpm_key_disabled) {
+		/* PCIE */
+		if (data->dpm_level_enable_mask.pcie_dpm_enable_mask != 0) {
+			level = tonga_get_lowest_enable_level(hwmgr,
+							      data->dpm_level_enable_mask.pcie_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state_pcie(hwmgr, level)),
+					    "force lowest pcie dpm state failed!", return -1);
+		}
+	}
+
+	if (0 == data->sclk_dpm_key_disabled) {
+		/* SCLK */
+		if (0 != data->dpm_level_enable_mask.sclk_dpm_enable_mask) {
+			level = tonga_get_lowest_enable_level(hwmgr,
+							      data->dpm_level_enable_mask.sclk_dpm_enable_mask);
+
+			PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state(hwmgr, level)),
+					    "force sclk dpm state failed!", return -1);
+
+			if (PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device,
+							 CGS_IND_REG__SMC, TARGET_AND_CURRENT_PROFILE_INDEX, CURR_SCLK_INDEX) != level)
+				printk(KERN_ERR "[ powerplay ] Target_and_current_Profile_Index.	\
+				Curr_Sclk_Index does not match the level \n");
+		}
+	}
+
+	if (0 == data->mclk_dpm_key_disabled) {
+		/* MCLK */
+		if (data->dpm_level_enable_mask.mclk_dpm_enable_mask != 0) {
+			level = tonga_get_lowest_enable_level(hwmgr,
+							      data->dpm_level_enable_mask.mclk_dpm_enable_mask);
+			PP_ASSERT_WITH_CODE((0 == tonga_dpm_force_state_mclk(hwmgr, level)),
+					    "force lowest mclk dpm state failed!", return -1);
+			if (PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+							 TARGET_AND_CURRENT_PROFILE_INDEX, CURR_MCLK_INDEX) != level)
+				printk(KERN_ERR "[ powerplay ] Target_and_current_Profile_Index. \
+						Curr_Mclk_Index does not match the level \n");
+		}
+	}
+
+	return 0;
+}
+
+static int tonga_patch_voltage_dependency_tables_with_lookup_table(struct pp_hwmgr *hwmgr)
+{
+	uint8_t entryId;
+	uint8_t voltageId;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	phm_ppt_v1_clock_voltage_dependency_table *sclk_table = pptable_info->vdd_dep_on_sclk;
+	phm_ppt_v1_clock_voltage_dependency_table *mclk_table = pptable_info->vdd_dep_on_mclk;
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	if (data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) {
+		for (entryId = 0; entryId < sclk_table->count; ++entryId) {
+			voltageId = sclk_table->entries[entryId].vddInd;
+			sclk_table->entries[entryId].vddgfx =
+				pptable_info->vddgfx_lookup_table->entries[voltageId].us_vdd;
+		}
+	} else {
+		for (entryId = 0; entryId < sclk_table->count; ++entryId) {
+			voltageId = sclk_table->entries[entryId].vddInd;
+			sclk_table->entries[entryId].vddc =
+				pptable_info->vddc_lookup_table->entries[voltageId].us_vdd;
+		}
+	}
+
+	for (entryId = 0; entryId < mclk_table->count; ++entryId) {
+		voltageId = mclk_table->entries[entryId].vddInd;
+		mclk_table->entries[entryId].vddc =
+			pptable_info->vddc_lookup_table->entries[voltageId].us_vdd;
+	}
+
+	for (entryId = 0; entryId < mm_table->count; ++entryId) {
+		voltageId = mm_table->entries[entryId].vddcInd;
+		mm_table->entries[entryId].vddc =
+			pptable_info->vddc_lookup_table->entries[voltageId].us_vdd;
+	}
+
+	return 0;
+
+}
+
+static int tonga_calc_voltage_dependency_tables(struct pp_hwmgr *hwmgr)
+{
+	uint8_t entryId;
+	phm_ppt_v1_voltage_lookup_record v_record;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	phm_ppt_v1_clock_voltage_dependency_table *sclk_table = pptable_info->vdd_dep_on_sclk;
+	phm_ppt_v1_clock_voltage_dependency_table *mclk_table = pptable_info->vdd_dep_on_mclk;
+
+	if (data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) {
+		for (entryId = 0; entryId < sclk_table->count; ++entryId) {
+			if (sclk_table->entries[entryId].vdd_offset & (1 << 15))
+				v_record.us_vdd = sclk_table->entries[entryId].vddgfx +
+					sclk_table->entries[entryId].vdd_offset - 0xFFFF;
+			else
+				v_record.us_vdd = sclk_table->entries[entryId].vddgfx +
+					sclk_table->entries[entryId].vdd_offset;
+
+			sclk_table->entries[entryId].vddc =
+				v_record.us_cac_low = v_record.us_cac_mid =
+				v_record.us_cac_high = v_record.us_vdd;
+
+			tonga_add_voltage(hwmgr, pptable_info->vddc_lookup_table, &v_record);
+		}
+
+		for (entryId = 0; entryId < mclk_table->count; ++entryId) {
+			if (mclk_table->entries[entryId].vdd_offset & (1 << 15))
+				v_record.us_vdd = mclk_table->entries[entryId].vddc +
+					mclk_table->entries[entryId].vdd_offset - 0xFFFF;
+			else
+				v_record.us_vdd = mclk_table->entries[entryId].vddc +
+					mclk_table->entries[entryId].vdd_offset;
+
+			mclk_table->entries[entryId].vddgfx = v_record.us_cac_low =
+				v_record.us_cac_mid = v_record.us_cac_high = v_record.us_vdd;
+			tonga_add_voltage(hwmgr, pptable_info->vddgfx_lookup_table, &v_record);
+		}
+	}
+
+	return 0;
+
+}
+
+static int tonga_calc_mm_voltage_dependency_table(struct pp_hwmgr *hwmgr)
+{
+	uint32_t entryId;
+	phm_ppt_v1_voltage_lookup_record v_record;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table = pptable_info->mm_dep_table;
+
+	if (data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) {
+		for (entryId = 0; entryId < mm_table->count; entryId++) {
+			if (mm_table->entries[entryId].vddgfx_offset & (1 << 15))
+				v_record.us_vdd = mm_table->entries[entryId].vddc +
+					mm_table->entries[entryId].vddgfx_offset - 0xFFFF;
+			else
+				v_record.us_vdd = mm_table->entries[entryId].vddc +
+					mm_table->entries[entryId].vddgfx_offset;
+
+			/* Add the calculated VDDGFX to the VDDGFX lookup table */
+			mm_table->entries[entryId].vddgfx = v_record.us_cac_low =
+				v_record.us_cac_mid = v_record.us_cac_high = v_record.us_vdd;
+			tonga_add_voltage(hwmgr, pptable_info->vddgfx_lookup_table, &v_record);
+		}
+	}
+	return 0;
+}
+
+
+/**
+ * Change virtual leakage voltage to actual value.
+ *
+ * @param     hwmgr  the address of the powerplay hardware manager.
+ * @param     pointer to changing voltage
+ * @param     pointer to leakage table
+ */
+static void tonga_patch_with_vdd_leakage(struct pp_hwmgr *hwmgr,
+		uint16_t *voltage, phw_tonga_leakage_voltage *pLeakageTable)
+{
+	uint32_t leakage_index;
+
+	/* search for leakage voltage ID 0xff01 ~ 0xff08 */
+	for (leakage_index = 0; leakage_index < pLeakageTable->count; leakage_index++) {
+		/* if this voltage matches a leakage voltage ID */
+		/* patch with actual leakage voltage */
+		if (pLeakageTable->leakage_id[leakage_index] == *voltage) {
+			*voltage = pLeakageTable->actual_voltage[leakage_index];
+			break;
+		}
+	}
+
+	if (*voltage > ATOM_VIRTUAL_VOLTAGE_ID0)
+		printk(KERN_ERR "[ powerplay ] Voltage value looks like a Leakage ID but it's not patched \n");
+}
+
+/**
+ * Patch voltage lookup table by EVV leakages.
+ *
+ * @param     hwmgr  the address of the powerplay hardware manager.
+ * @param     pointer to voltage lookup table
+ * @param     pointer to leakage table
+ * @return     always 0
+ */
+static int tonga_patch_lookup_table_with_leakage(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_voltage_lookup_table *lookup_table,
+		phw_tonga_leakage_voltage *pLeakageTable)
+{
+	uint32_t i;
+
+	for (i = 0; i < lookup_table->count; i++) {
+		tonga_patch_with_vdd_leakage(hwmgr,
+			&lookup_table->entries[i].us_vdd, pLeakageTable);
+	}
+
+	return 0;
+}
+
+static int tonga_patch_clock_voltage_lomits_with_vddc_leakage(struct pp_hwmgr *hwmgr,
+		phw_tonga_leakage_voltage *pLeakageTable, uint16_t *Vddc)
+{
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	tonga_patch_with_vdd_leakage(hwmgr, (uint16_t *)Vddc, pLeakageTable);
+	hwmgr->dyn_state.max_clock_voltage_on_dc.vddc =
+		pptable_info->max_clock_voltage_on_dc.vddc;
+
+	return 0;
+}
+
+static int tonga_patch_clock_voltage_limits_with_vddgfx_leakage(
+		struct pp_hwmgr *hwmgr, phw_tonga_leakage_voltage *pLeakageTable,
+		uint16_t *Vddgfx)
+{
+	tonga_patch_with_vdd_leakage(hwmgr, (uint16_t *)Vddgfx, pLeakageTable);
+	return 0;
+}
+
+int tonga_sort_lookup_table(struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_voltage_lookup_table *lookup_table)
+{
+	uint32_t table_size, i, j;
+	phm_ppt_v1_voltage_lookup_record tmp_voltage_lookup_record;
+	table_size = lookup_table->count;
+
+	PP_ASSERT_WITH_CODE(0 != lookup_table->count,
+		"Lookup table is empty", return -1);
+
+	/* Sorting voltages */
+	for (i = 0; i < table_size - 1; i++) {
+		for (j = i + 1; j > 0; j--) {
+			if (lookup_table->entries[j].us_vdd < lookup_table->entries[j-1].us_vdd) {
+				tmp_voltage_lookup_record = lookup_table->entries[j-1];
+				lookup_table->entries[j-1] = lookup_table->entries[j];
+				lookup_table->entries[j] = tmp_voltage_lookup_record;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int tonga_complete_dependency_tables(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+	int tmp_result;
+	tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (data->vdd_gfx_control == TONGA_VOLTAGE_CONTROL_BY_SVID2) {
+		tmp_result = tonga_patch_lookup_table_with_leakage(hwmgr,
+			pptable_info->vddgfx_lookup_table, &(data->vddcgfx_leakage));
+		if (tmp_result != 0)
+			result = tmp_result;
+
+		tmp_result = tonga_patch_clock_voltage_limits_with_vddgfx_leakage(hwmgr,
+			&(data->vddcgfx_leakage), &pptable_info->max_clock_voltage_on_dc.vddgfx);
+		if (tmp_result != 0)
+			result = tmp_result;
+	} else {
+		tmp_result = tonga_patch_lookup_table_with_leakage(hwmgr,
+			pptable_info->vddc_lookup_table, &(data->vddc_leakage));
+		if (tmp_result != 0)
+			result = tmp_result;
+
+		tmp_result = tonga_patch_clock_voltage_lomits_with_vddc_leakage(hwmgr,
+			&(data->vddc_leakage), &pptable_info->max_clock_voltage_on_dc.vddc);
+		if (tmp_result != 0)
+			result = tmp_result;
+	}
+
+	tmp_result = tonga_patch_voltage_dependency_tables_with_lookup_table(hwmgr);
+	if (tmp_result != 0)
+		result = tmp_result;
+
+	tmp_result = tonga_calc_voltage_dependency_tables(hwmgr);
+	if (tmp_result != 0)
+		result = tmp_result;
+
+	tmp_result = tonga_calc_mm_voltage_dependency_table(hwmgr);
+	if (tmp_result != 0)
+		result = tmp_result;
+
+	tmp_result = tonga_sort_lookup_table(hwmgr, pptable_info->vddgfx_lookup_table);
+	if (tmp_result != 0)
+		result = tmp_result;
+
+	tmp_result = tonga_sort_lookup_table(hwmgr, pptable_info->vddc_lookup_table);
+	if (tmp_result != 0)
+		result = tmp_result;
+
+	return result;
+}
+
+int tonga_init_sclk_threshold(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	data->low_sclk_interrupt_threshold = 0;
+
+	return 0;
+}
+
+int tonga_setup_asic_task(struct pp_hwmgr *hwmgr)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = tonga_read_clock_registers(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to read clock registers!", result = tmp_result);
+
+	tmp_result = tonga_get_memory_type(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to get memory type!", result = tmp_result);
+
+	tmp_result = tonga_enable_acpi_power_management(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to enable ACPI power management!", result = tmp_result);
+
+	tmp_result = tonga_init_power_gate_state(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to init power gate state!", result = tmp_result);
+
+	tmp_result = tonga_get_mc_microcode_version(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to get MC microcode version!", result = tmp_result);
+
+	tmp_result = tonga_init_sclk_threshold(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to init sclk threshold!", result = tmp_result);
+
+	return result;
+}
+
+/**
+ * Enable voltage control
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_enable_voltage_control(struct pp_hwmgr *hwmgr)
+{
+	/* enable voltage control */
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, GENERAL_PWRMGT, VOLT_PWRMGT_EN, 1);
+
+	return 0;
+}
+
+/**
+ * Checks if we want to support voltage control
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ */
+bool cf_tonga_voltage_control(const struct pp_hwmgr *hwmgr)
+{
+	const struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	return(TONGA_VOLTAGE_CONTROL_NONE != data->voltage_control);
+}
+
+/*---------------------------MC----------------------------*/
+
+uint8_t tonga_get_memory_modile_index(struct pp_hwmgr *hwmgr)
+{
+	return (uint8_t) (0xFF & (cgs_read_register(hwmgr->device, mmBIOS_SCRATCH_4) >> 16));
+}
+
+bool tonga_check_s0_mc_reg_index(uint16_t inReg, uint16_t *outReg)
+{
+	bool result = 1;
+
+	switch (inReg) {
+	case  mmMC_SEQ_RAS_TIMING:
+		*outReg = mmMC_SEQ_RAS_TIMING_LP;
+		break;
+
+	case  mmMC_SEQ_DLL_STBY:
+		*outReg = mmMC_SEQ_DLL_STBY_LP;
+		break;
+
+	case  mmMC_SEQ_G5PDX_CMD0:
+		*outReg = mmMC_SEQ_G5PDX_CMD0_LP;
+		break;
+
+	case  mmMC_SEQ_G5PDX_CMD1:
+		*outReg = mmMC_SEQ_G5PDX_CMD1_LP;
+		break;
+
+	case  mmMC_SEQ_G5PDX_CTRL:
+		*outReg = mmMC_SEQ_G5PDX_CTRL_LP;
+		break;
+
+	case mmMC_SEQ_CAS_TIMING:
+		*outReg = mmMC_SEQ_CAS_TIMING_LP;
+		break;
+
+	case mmMC_SEQ_MISC_TIMING:
+		*outReg = mmMC_SEQ_MISC_TIMING_LP;
+		break;
+
+	case mmMC_SEQ_MISC_TIMING2:
+		*outReg = mmMC_SEQ_MISC_TIMING2_LP;
+		break;
+
+	case mmMC_SEQ_PMG_DVS_CMD:
+		*outReg = mmMC_SEQ_PMG_DVS_CMD_LP;
+		break;
+
+	case mmMC_SEQ_PMG_DVS_CTL:
+		*outReg = mmMC_SEQ_PMG_DVS_CTL_LP;
+		break;
+
+	case mmMC_SEQ_RD_CTL_D0:
+		*outReg = mmMC_SEQ_RD_CTL_D0_LP;
+		break;
+
+	case mmMC_SEQ_RD_CTL_D1:
+		*outReg = mmMC_SEQ_RD_CTL_D1_LP;
+		break;
+
+	case mmMC_SEQ_WR_CTL_D0:
+		*outReg = mmMC_SEQ_WR_CTL_D0_LP;
+		break;
+
+	case mmMC_SEQ_WR_CTL_D1:
+		*outReg = mmMC_SEQ_WR_CTL_D1_LP;
+		break;
+
+	case mmMC_PMG_CMD_EMRS:
+		*outReg = mmMC_SEQ_PMG_CMD_EMRS_LP;
+		break;
+
+	case mmMC_PMG_CMD_MRS:
+		*outReg = mmMC_SEQ_PMG_CMD_MRS_LP;
+		break;
+
+	case mmMC_PMG_CMD_MRS1:
+		*outReg = mmMC_SEQ_PMG_CMD_MRS1_LP;
+		break;
+
+	case mmMC_SEQ_PMG_TIMING:
+		*outReg = mmMC_SEQ_PMG_TIMING_LP;
+		break;
+
+	case mmMC_PMG_CMD_MRS2:
+		*outReg = mmMC_SEQ_PMG_CMD_MRS2_LP;
+		break;
+
+	case mmMC_SEQ_WR_CTL_2:
+		*outReg = mmMC_SEQ_WR_CTL_2_LP;
+		break;
+
+	default:
+		result = 0;
+		break;
+	}
+
+	return result;
+}
+
+int tonga_set_s0_mc_reg_index(phw_tonga_mc_reg_table *table)
+{
+	uint32_t i;
+	uint16_t address;
+
+	for (i = 0; i < table->last; i++) {
+		table->mc_reg_address[i].s0 =
+			tonga_check_s0_mc_reg_index(table->mc_reg_address[i].s1, &address)
+			? address : table->mc_reg_address[i].s1;
+	}
+	return 0;
+}
+
+int tonga_copy_vbios_smc_reg_table(const pp_atomctrl_mc_reg_table *table, phw_tonga_mc_reg_table *ni_table)
+{
+	uint8_t i, j;
+
+	PP_ASSERT_WITH_CODE((table->last <= SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+		"Invalid VramInfo table.", return -1);
+	PP_ASSERT_WITH_CODE((table->num_entries <= MAX_AC_TIMING_ENTRIES),
+		"Invalid VramInfo table.", return -1);
+
+	for (i = 0; i < table->last; i++) {
+		ni_table->mc_reg_address[i].s1 = table->mc_reg_address[i].s1;
+	}
+	ni_table->last = table->last;
+
+	for (i = 0; i < table->num_entries; i++) {
+		ni_table->mc_reg_table_entry[i].mclk_max =
+			table->mc_reg_table_entry[i].mclk_max;
+		for (j = 0; j < table->last; j++) {
+			ni_table->mc_reg_table_entry[i].mc_data[j] =
+				table->mc_reg_table_entry[i].mc_data[j];
+		}
+	}
+
+	ni_table->num_entries = table->num_entries;
+
+	return 0;
+}
+
+/**
+ * VBIOS omits some information to reduce size, we need to recover them here.
+ * 1.   when we see mmMC_SEQ_MISC1, bit[31:16] EMRS1, need to be write to  mmMC_PMG_CMD_EMRS /_LP[15:0].
+ *      Bit[15:0] MRS, need to be update mmMC_PMG_CMD_MRS/_LP[15:0]
+ * 2.   when we see mmMC_SEQ_RESERVE_M, bit[15:0] EMRS2, need to be write to mmMC_PMG_CMD_MRS1/_LP[15:0].
+ * 3.   need to set these data for each clock range
+ *
+ * @param    hwmgr the address of the powerplay hardware manager.
+ * @param    table the address of MCRegTable
+ * @return   always 0
+ */
+int tonga_set_mc_special_registers(struct pp_hwmgr *hwmgr, phw_tonga_mc_reg_table *table)
+{
+	uint8_t i, j, k;
+	uint32_t temp_reg;
+	const tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	for (i = 0, j = table->last; i < table->last; i++) {
+		PP_ASSERT_WITH_CODE((j < SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+			"Invalid VramInfo table.", return -1);
+		switch (table->mc_reg_address[i].s1) {
+		/*
+		* mmMC_SEQ_MISC1, bit[31:16] EMRS1, need to be write to  mmMC_PMG_CMD_EMRS /_LP[15:0].
+		* Bit[15:0] MRS, need to be update mmMC_PMG_CMD_MRS/_LP[15:0]
+		*/
+		case mmMC_SEQ_MISC1:
+			temp_reg = cgs_read_register(hwmgr->device, mmMC_PMG_CMD_EMRS);
+			table->mc_reg_address[j].s1 = mmMC_PMG_CMD_EMRS;
+			table->mc_reg_address[j].s0 = mmMC_SEQ_PMG_CMD_EMRS_LP;
+			for (k = 0; k < table->num_entries; k++) {
+				table->mc_reg_table_entry[k].mc_data[j] =
+					((temp_reg & 0xffff0000)) |
+					((table->mc_reg_table_entry[k].mc_data[i] & 0xffff0000) >> 16);
+			}
+			j++;
+			PP_ASSERT_WITH_CODE((j < SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+				"Invalid VramInfo table.", return -1);
+
+			temp_reg = cgs_read_register(hwmgr->device, mmMC_PMG_CMD_MRS);
+			table->mc_reg_address[j].s1 = mmMC_PMG_CMD_MRS;
+			table->mc_reg_address[j].s0 = mmMC_SEQ_PMG_CMD_MRS_LP;
+			for (k = 0; k < table->num_entries; k++) {
+				table->mc_reg_table_entry[k].mc_data[j] =
+					(temp_reg & 0xffff0000) |
+					(table->mc_reg_table_entry[k].mc_data[i] & 0x0000ffff);
+
+				if (!data->is_memory_GDDR5) {
+					table->mc_reg_table_entry[k].mc_data[j] |= 0x100;
+				}
+			}
+			j++;
+			PP_ASSERT_WITH_CODE((j <= SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+				"Invalid VramInfo table.", return -1);
+
+			if (!data->is_memory_GDDR5) {
+				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++) {
+					table->mc_reg_table_entry[k].mc_data[j] =
+						(table->mc_reg_table_entry[k].mc_data[i] & 0xffff0000) >> 16;
+				}
+				j++;
+				PP_ASSERT_WITH_CODE((j <= SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+					"Invalid VramInfo table.", return -1);
+			}
+
+			break;
+
+		case mmMC_SEQ_RESERVE_M:
+			temp_reg = cgs_read_register(hwmgr->device, mmMC_PMG_CMD_MRS1);
+			table->mc_reg_address[j].s1 = mmMC_PMG_CMD_MRS1;
+			table->mc_reg_address[j].s0 = mmMC_SEQ_PMG_CMD_MRS1_LP;
+			for (k = 0; k < table->num_entries; k++) {
+				table->mc_reg_table_entry[k].mc_data[j] =
+					(temp_reg & 0xffff0000) |
+					(table->mc_reg_table_entry[k].mc_data[i] & 0x0000ffff);
+			}
+			j++;
+			PP_ASSERT_WITH_CODE((j <= SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE),
+				"Invalid VramInfo table.", return -1);
+			break;
+
+		default:
+			break;
+		}
+
+	}
+
+	table->last = j;
+
+	return 0;
+}
+
+int tonga_set_valid_flag(phw_tonga_mc_reg_table *table)
+{
+	uint8_t i, j;
+	for (i = 0; i < table->last; i++) {
+		for (j = 1; j < table->num_entries; j++) {
+			if (table->mc_reg_table_entry[j-1].mc_data[i] !=
+				table->mc_reg_table_entry[j].mc_data[i]) {
+				table->validflag |= (1<<i);
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int tonga_initialize_mc_reg_table(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+	pp_atomctrl_mc_reg_table *table;
+	phw_tonga_mc_reg_table *ni_table = &data->tonga_mc_reg_table;
+	uint8_t module_index = tonga_get_memory_modile_index(hwmgr);
+
+	table = kzalloc(sizeof(pp_atomctrl_mc_reg_table), GFP_KERNEL);
+
+	if (NULL == table)
+		return -ENOMEM;
+
+	/* Program additional LP registers that are no longer programmed by VBIOS */
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RAS_TIMING_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_RAS_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_CAS_TIMING_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_CAS_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_DLL_STBY_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_DLL_STBY));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_G5PDX_CMD0_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_G5PDX_CMD0));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_G5PDX_CMD1_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_G5PDX_CMD1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_G5PDX_CTRL_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_G5PDX_CTRL));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_DVS_CMD_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_PMG_DVS_CMD));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_DVS_CTL_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_PMG_DVS_CTL));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_MISC_TIMING_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_MISC_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_MISC_TIMING2_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_MISC_TIMING2));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_CMD_EMRS_LP, cgs_read_register(hwmgr->device, mmMC_PMG_CMD_EMRS));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_CMD_MRS_LP, cgs_read_register(hwmgr->device, mmMC_PMG_CMD_MRS));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_CMD_MRS1_LP, cgs_read_register(hwmgr->device, mmMC_PMG_CMD_MRS1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_WR_CTL_D0_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_WR_CTL_D0));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_WR_CTL_D1_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_WR_CTL_D1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RD_CTL_D0_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_RD_CTL_D0));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_RD_CTL_D1_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_RD_CTL_D1));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_TIMING_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_PMG_TIMING));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_PMG_CMD_MRS2_LP, cgs_read_register(hwmgr->device, mmMC_PMG_CMD_MRS2));
+	cgs_write_register(hwmgr->device, mmMC_SEQ_WR_CTL_2_LP, cgs_read_register(hwmgr->device, mmMC_SEQ_WR_CTL_2));
+
+	memset(table, 0x00, sizeof(pp_atomctrl_mc_reg_table));
+
+	result = atomctrl_initialize_mc_reg_table(hwmgr, module_index, table);
+
+	if (0 == result)
+		result = tonga_copy_vbios_smc_reg_table(table, ni_table);
+
+	if (0 == result) {
+		tonga_set_s0_mc_reg_index(ni_table);
+		result = tonga_set_mc_special_registers(hwmgr, ni_table);
+	}
+
+	if (0 == result)
+		tonga_set_valid_flag(ni_table);
+
+	kfree(table);
+	return result;
+}
+
+/*
+* Copy one arb setting to another and then switch the active set.
+* arbFreqSrc and arbFreqDest is one of the MC_CG_ARB_FREQ_Fx constants.
+*/
+int tonga_copy_and_switch_arb_sets(struct pp_hwmgr *hwmgr,
+		uint32_t arbFreqSrc, uint32_t arbFreqDest)
+{
+	uint32_t mc_arb_dram_timing;
+	uint32_t mc_arb_dram_timing2;
+	uint32_t burst_time;
+	uint32_t mc_cg_config;
+
+	switch (arbFreqSrc) {
+	case MC_CG_ARB_FREQ_F0:
+		mc_arb_dram_timing  = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING);
+		mc_arb_dram_timing2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2);
+		burst_time = PHM_READ_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE0);
+		break;
+
+	case MC_CG_ARB_FREQ_F1:
+		mc_arb_dram_timing  = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING_1);
+		mc_arb_dram_timing2 = cgs_read_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2_1);
+		burst_time = PHM_READ_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE1);
+		break;
+
+	default:
+		return -1;
+	}
+
+	switch (arbFreqDest) {
+	case MC_CG_ARB_FREQ_F0:
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING, mc_arb_dram_timing);
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2, mc_arb_dram_timing2);
+		PHM_WRITE_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE0, burst_time);
+		break;
+
+	case MC_CG_ARB_FREQ_F1:
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING_1, mc_arb_dram_timing);
+		cgs_write_register(hwmgr->device, mmMC_ARB_DRAM_TIMING2_1, mc_arb_dram_timing2);
+		PHM_WRITE_FIELD(hwmgr->device, MC_ARB_BURST_TIME, STATE1, burst_time);
+		break;
+
+	default:
+		return -1;
+	}
+
+	mc_cg_config = cgs_read_register(hwmgr->device, mmMC_CG_CONFIG);
+	mc_cg_config |= 0x0000000F;
+	cgs_write_register(hwmgr->device, mmMC_CG_CONFIG, mc_cg_config);
+	PHM_WRITE_FIELD(hwmgr->device, MC_ARB_CG, CG_ARB_REQ, arbFreqDest);
+
+	return 0;
+}
+
+/**
+ * Initial switch from ARB F0->F1
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ * This function is to be called from the SetPowerState table.
+ */
+int tonga_initial_switch_from_arb_f0_to_f1(struct pp_hwmgr *hwmgr)
+{
+	return tonga_copy_and_switch_arb_sets(hwmgr, MC_CG_ARB_FREQ_F0, MC_CG_ARB_FREQ_F1);
+}
+
+/**
+ * Initialize the ARB DRAM timing table's index field.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_init_arb_table_index(struct pp_hwmgr *hwmgr)
+{
+	const tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t tmp;
+	int result;
+
+	/*
+	* This is a read-modify-write on the first byte of the ARB table.
+	* The first byte in the SMU72_Discrete_MCArbDramTimingTable structure is the field 'current'.
+	* This solution is ugly, but we never write the whole table only individual fields in it.
+	* In reality this field should not be in that structure but in a soft register.
+	*/
+	result = tonga_read_smc_sram_dword(hwmgr->smumgr,
+				data->arb_table_start, &tmp, data->sram_end);
+
+	if (0 != result)
+		return result;
+
+	tmp &= 0x00FFFFFF;
+	tmp |= ((uint32_t)MC_CG_ARB_FREQ_F1) << 24;
+
+	return tonga_write_smc_sram_dword(hwmgr->smumgr,
+			data->arb_table_start,  tmp, data->sram_end);
+}
+
+int tonga_populate_mc_reg_address(struct pp_hwmgr *hwmgr, SMU72_Discrete_MCRegisters *mc_reg_table)
+{
+	const struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	uint32_t i, j;
+
+	for (i = 0, j = 0; j < data->tonga_mc_reg_table.last; j++) {
+		if (data->tonga_mc_reg_table.validflag & 1<<j) {
+			PP_ASSERT_WITH_CODE(i < SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE,
+				"Index of mc_reg_table->address[] array out of boundary", return -1);
+			mc_reg_table->address[i].s0 =
+				PP_HOST_TO_SMC_US(data->tonga_mc_reg_table.mc_reg_address[j].s0);
+			mc_reg_table->address[i].s1 =
+				PP_HOST_TO_SMC_US(data->tonga_mc_reg_table.mc_reg_address[j].s1);
+			i++;
+		}
+	}
+
+	mc_reg_table->last = (uint8_t)i;
+
+	return 0;
+}
+
+/*convert register values from driver to SMC format */
+void tonga_convert_mc_registers(
+	const phw_tonga_mc_reg_entry * pEntry,
+	SMU72_Discrete_MCRegisterSet *pData,
+	uint32_t numEntries, uint32_t validflag)
+{
+	uint32_t i, j;
+
+	for (i = 0, j = 0; j < numEntries; j++) {
+		if (validflag & 1<<j) {
+			pData->value[i] = PP_HOST_TO_SMC_UL(pEntry->mc_data[j]);
+			i++;
+		}
+	}
+}
+
+/* find the entry in the memory range table, then populate the value to SMC's tonga_mc_reg_table */
+int tonga_convert_mc_reg_table_entry_to_smc(
+		struct pp_hwmgr *hwmgr,
+		const uint32_t memory_clock,
+		SMU72_Discrete_MCRegisterSet *mc_reg_table_data
+		)
+{
+	const tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t i = 0;
+
+	for (i = 0; i < data->tonga_mc_reg_table.num_entries; i++) {
+		if (memory_clock <=
+			data->tonga_mc_reg_table.mc_reg_table_entry[i].mclk_max) {
+			break;
+		}
+	}
+
+	if ((i == data->tonga_mc_reg_table.num_entries) && (i > 0))
+		--i;
+
+	tonga_convert_mc_registers(&data->tonga_mc_reg_table.mc_reg_table_entry[i],
+		mc_reg_table_data, data->tonga_mc_reg_table.last, data->tonga_mc_reg_table.validflag);
+
+	return 0;
+}
+
+int tonga_convert_mc_reg_table_to_smc(struct pp_hwmgr *hwmgr,
+		SMU72_Discrete_MCRegisters *mc_reg_table)
+{
+	int result = 0;
+	tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	int res;
+	uint32_t i;
+
+	for (i = 0; i < data->dpm_table.mclk_table.count; i++) {
+		res = tonga_convert_mc_reg_table_entry_to_smc(
+				hwmgr,
+				data->dpm_table.mclk_table.dpm_levels[i].value,
+				&mc_reg_table->data[i]
+				);
+
+		if (0 != res)
+			result = res;
+	}
+
+	return result;
+}
+
+int tonga_populate_initial_mc_reg_table(struct pp_hwmgr *hwmgr)
+{
+	int result;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	memset(&data->mc_reg_table, 0x00, sizeof(SMU72_Discrete_MCRegisters));
+	result = tonga_populate_mc_reg_address(hwmgr, &(data->mc_reg_table));
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize MCRegTable for the MC register addresses!", return result;);
+
+	result = tonga_convert_mc_reg_table_to_smc(hwmgr, &data->mc_reg_table);
+	PP_ASSERT_WITH_CODE(0 == result,
+		"Failed to initialize MCRegTable for driver state!", return result;);
+
+	return tonga_copy_bytes_to_smc(hwmgr->smumgr, data->mc_reg_table_start,
+			(uint8_t *)&data->mc_reg_table, sizeof(SMU72_Discrete_MCRegisters), data->sram_end);
+}
+
+/**
+ * Programs static screed detection parameters
+ *
+ * @param   hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_program_static_screen_threshold_parameters(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* Set static screen threshold unit*/
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device,
+		CGS_IND_REG__SMC, CG_STATIC_SCREEN_PARAMETER, STATIC_SCREEN_THRESHOLD_UNIT,
+		data->static_screen_threshold_unit);
+	/* Set static screen threshold*/
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device,
+		CGS_IND_REG__SMC, CG_STATIC_SCREEN_PARAMETER, STATIC_SCREEN_THRESHOLD,
+		data->static_screen_threshold);
+
+	return 0;
+}
+
+/**
+ * Setup display gap for glitch free memory clock switching.
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_enable_display_gap(struct pp_hwmgr *hwmgr)
+{
+	uint32_t display_gap = cgs_read_ind_register(hwmgr->device,
+							CGS_IND_REG__SMC, ixCG_DISPLAY_GAP_CNTL);
+
+	display_gap = PHM_SET_FIELD(display_gap,
+					CG_DISPLAY_GAP_CNTL, DISP_GAP, DISPLAY_GAP_IGNORE);
+
+	display_gap = PHM_SET_FIELD(display_gap,
+					CG_DISPLAY_GAP_CNTL, DISP_GAP_MCHG, DISPLAY_GAP_VBLANK);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_DISPLAY_GAP_CNTL, display_gap);
+
+	return 0;
+}
+
+/**
+ * Programs activity state transition voting clients
+ *
+ * @param    hwmgr  the address of the powerplay hardware manager.
+ * @return   always 0
+ */
+int tonga_program_voting_clients(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr *data = (tonga_hwmgr *)(hwmgr->backend);
+
+	/* Clear reset for voting clients before enabling DPM */
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+		SCLK_PWRMGT_CNTL, RESET_SCLK_CNT, 0);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+		SCLK_PWRMGT_CNTL, RESET_BUSY_CNT, 0);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_0, data->voting_rights_clients0);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_1, data->voting_rights_clients1);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_2, data->voting_rights_clients2);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_3, data->voting_rights_clients3);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_4, data->voting_rights_clients4);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_5, data->voting_rights_clients5);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_6, data->voting_rights_clients6);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+		ixCG_FREQ_TRAN_VOTING_7, data->voting_rights_clients7);
+
+	return 0;
+}
+
+
+int tonga_enable_dpm_tasks(struct pp_hwmgr *hwmgr)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = tonga_check_for_dpm_stopped(hwmgr);
+
+	if (cf_tonga_voltage_control(hwmgr)) {
+		tmp_result = tonga_enable_voltage_control(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to enable voltage control!", result = tmp_result);
+
+		tmp_result = tonga_construct_voltage_tables(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == tmp_result),
+			"Failed to contruct voltage tables!", result = tmp_result);
+	}
+
+	tmp_result = tonga_initialize_mc_reg_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to initialize MC reg table!", result = tmp_result);
+
+	tmp_result = tonga_program_static_screen_threshold_parameters(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to program static screen threshold parameters!", result = tmp_result);
+
+	tmp_result = tonga_enable_display_gap(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to enable display gap!", result = tmp_result);
+
+	tmp_result = tonga_program_voting_clients(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to program voting clients!", result = tmp_result);
+
+	tmp_result = tonga_process_firmware_header(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to process firmware header!", result = tmp_result);
+
+	tmp_result = tonga_initial_switch_from_arb_f0_to_f1(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to initialize switch from ArbF0 to F1!", result = tmp_result);
+
+	tmp_result = tonga_init_smc_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to initialize SMC table!", result = tmp_result);
+
+	tmp_result = tonga_init_arb_table_index(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to initialize ARB table index!", result = tmp_result);
+
+	tmp_result = tonga_populate_initial_mc_reg_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to populate initialize MC Reg table!", result = tmp_result);
+
+	tmp_result = tonga_notify_smc_display_change(hwmgr, false);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to notify no display!", result = tmp_result);
+
+	/* enable SCLK control */
+	tmp_result = tonga_enable_sclk_control(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to enable SCLK control!", result = tmp_result);
+
+	/* enable DPM */
+	tmp_result = tonga_start_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to start DPM!", result = tmp_result);
+
+	return result;
+}
+
+int tonga_disable_dpm_tasks(struct pp_hwmgr *hwmgr)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = tonga_check_for_dpm_running(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"SMC is still running!", return 0);
+
+	tmp_result = tonga_stop_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to stop DPM!", result = tmp_result);
+
+	tmp_result = tonga_reset_to_default(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result),
+		"Failed to reset to default!", result = tmp_result);
+
+	return result;
+}
+
+int tonga_reset_asic_tasks(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	result = tonga_set_boot_state(hwmgr);
+	if (0 != result)
+		printk(KERN_ERR "[ powerplay ] Failed to reset asic via set boot state! \n");
+
+	return result;
+}
+
+int tonga_hwmgr_backend_fini(struct pp_hwmgr *hwmgr)
+{
+	if (NULL != hwmgr->dyn_state.vddc_dep_on_dal_pwrl) {
+		kfree(hwmgr->dyn_state.vddc_dep_on_dal_pwrl);
+		hwmgr->dyn_state.vddc_dep_on_dal_pwrl = NULL;
+	}
+
+	if (NULL != hwmgr->backend) {
+		kfree(hwmgr->backend);
+		hwmgr->backend = NULL;
+	}
+
+	return 0;
+}
+
+/**
+ * Initializes the Volcanic Islands Hardware Manager
+ *
+ * @param   hwmgr the address of the powerplay hardware manager.
+ * @return   1 if success; otherwise appropriate error code.
+ */
+int tonga_hwmgr_backend_init(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+	SMU72_Discrete_DpmTable  *table = NULL;
+	tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	pp_atomctrl_gpio_pin_assignment gpio_pin_assignment;
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	phw_tonga_ulv_parm *ulv;
+	struct cgs_system_info sys_info = {0};
+
+	PP_ASSERT_WITH_CODE((NULL != hwmgr),
+		"Invalid Parameter!", return -1;);
+
+	data->dll_defaule_on = 0;
+	data->sram_end = SMC_RAM_END;
+
+	data->activity_target[0] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[1] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[2] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[3] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[4] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[5] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[6] = PPTONGA_TARGETACTIVITY_DFLT;
+	data->activity_target[7] = PPTONGA_TARGETACTIVITY_DFLT;
+
+	data->vddc_vddci_delta = VDDC_VDDCI_DELTA;
+	data->vddc_vddgfx_delta = VDDC_VDDGFX_DELTA;
+	data->mclk_activity_target = PPTONGA_MCLK_TARGETACTIVITY_DFLT;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_DisableVoltageIsland);
+
+	data->sclk_dpm_key_disabled = 0;
+	data->mclk_dpm_key_disabled = 0;
+	data->pcie_dpm_key_disabled = 0;
+	data->pcc_monitor_enabled = 0;
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_UnTabledHardwareInterface);
+
+	data->gpio_debug = 0;
+	data->engine_clock_data = 0;
+	data->memory_clock_data = 0;
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_DynamicPatchPowerState);
+
+	/* need to set voltage control types before EVV patching*/
+	data->voltage_control = TONGA_VOLTAGE_CONTROL_NONE;
+	data->vdd_ci_control = TONGA_VOLTAGE_CONTROL_NONE;
+	data->vdd_gfx_control = TONGA_VOLTAGE_CONTROL_NONE;
+	data->mvdd_control = TONGA_VOLTAGE_CONTROL_NONE;
+
+	if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+				VOLTAGE_TYPE_VDDC, VOLTAGE_OBJ_SVID2)) {
+		data->voltage_control = TONGA_VOLTAGE_CONTROL_BY_SVID2;
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ControlVDDGFX)) {
+		if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+			VOLTAGE_TYPE_VDDGFX, VOLTAGE_OBJ_SVID2)) {
+			data->vdd_gfx_control = TONGA_VOLTAGE_CONTROL_BY_SVID2;
+		}
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_NONE == data->vdd_gfx_control) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ControlVDDGFX);
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EnableMVDDControl)) {
+		if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+					VOLTAGE_TYPE_MVDDC, VOLTAGE_OBJ_GPIO_LUT)) {
+			data->mvdd_control = TONGA_VOLTAGE_CONTROL_BY_GPIO;
+		}
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_NONE == data->mvdd_control) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_EnableMVDDControl);
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ControlVDDCI)) {
+		if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+					VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_GPIO_LUT))
+			data->vdd_ci_control = TONGA_VOLTAGE_CONTROL_BY_GPIO;
+		else if (atomctrl_is_voltage_controled_by_gpio_v3(hwmgr,
+						VOLTAGE_TYPE_VDDCI, VOLTAGE_OBJ_SVID2))
+			data->vdd_ci_control = TONGA_VOLTAGE_CONTROL_BY_SVID2;
+	}
+
+	if (TONGA_VOLTAGE_CONTROL_NONE == data->vdd_ci_control)
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_ControlVDDCI);
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_TablelessHardwareInterface);
+
+	if (pptable_info->cac_dtp_table->usClockStretchAmount != 0)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ClockStretcher);
+
+	/* Initializes DPM default values*/
+	tonga_initialize_dpm_defaults(hwmgr);
+
+	/* Get leakage voltage based on leakage ID.*/
+	PP_ASSERT_WITH_CODE((0 == tonga_get_evv_voltage(hwmgr)),
+		"Get EVV Voltage Failed.  Abort Driver loading!", return -1);
+
+	tonga_complete_dependency_tables(hwmgr);
+
+	/* Parse pptable data read from VBIOS*/
+	tonga_set_private_var_based_on_pptale(hwmgr);
+
+	/* ULV Support*/
+	ulv = &(data->ulv);
+	ulv->ulv_supported = 0;
+
+	/* Initalize Dynamic State Adjustment Rule Settings*/
+	result = tonga_initializa_dynamic_state_adjustment_rule_settings(hwmgr);
+	if (result)
+		printk(KERN_ERR "[ powerplay ] tonga_initializa_dynamic_state_adjustment_rule_settings failed!\n");
+	data->uvd_enabled = 0;
+
+	table = &(data->smc_state_table);
+
+	/*
+	* if ucGPIO_ID=VDDC_PCC_GPIO_PINID in GPIO_LUTable,
+	* Peak Current Control feature is enabled and we should program PCC HW register
+	*/
+	if (0 == atomctrl_get_pp_assign_pin(hwmgr, VDDC_PCC_GPIO_PINID, &gpio_pin_assignment)) {
+		uint32_t temp_reg = cgs_read_ind_register(hwmgr->device,
+										CGS_IND_REG__SMC, ixCNB_PWRMGT_CNTL);
+
+		switch (gpio_pin_assignment.uc_gpio_pin_bit_shift) {
+		case 0:
+			temp_reg = PHM_SET_FIELD(temp_reg,
+				CNB_PWRMGT_CNTL, GNB_SLOW_MODE, 0x1);
+			break;
+		case 1:
+			temp_reg = PHM_SET_FIELD(temp_reg,
+				CNB_PWRMGT_CNTL, GNB_SLOW_MODE, 0x2);
+			break;
+		case 2:
+			temp_reg = PHM_SET_FIELD(temp_reg,
+				CNB_PWRMGT_CNTL, GNB_SLOW, 0x1);
+			break;
+		case 3:
+			temp_reg = PHM_SET_FIELD(temp_reg,
+				CNB_PWRMGT_CNTL, FORCE_NB_PS1, 0x1);
+			break;
+		case 4:
+			temp_reg = PHM_SET_FIELD(temp_reg,
+				CNB_PWRMGT_CNTL, DPM_ENABLED, 0x1);
+			break;
+		default:
+			printk(KERN_ERR "[ powerplay ] Failed to setup PCC HW register!	\
+				Wrong GPIO assigned for VDDC_PCC_GPIO_PINID! \n");
+			break;
+		}
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC,
+			ixCNB_PWRMGT_CNTL, temp_reg);
+	}
+
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_EnableSMU7ThermalManagement);
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_SMU7);
+
+	data->vddc_phase_shed_control = 0;
+
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		      PHM_PlatformCaps_UVDPowerGating);
+	phm_cap_unset(hwmgr->platform_descriptor.platformCaps,
+		      PHM_PlatformCaps_VCEPowerGating);
+	sys_info.size = sizeof(struct cgs_system_info);
+	sys_info.info_id = CGS_SYSTEM_INFO_PG_FLAGS;
+	result = cgs_query_system_info(hwmgr->device, &sys_info);
+	if (!result) {
+		if (sys_info.value & AMD_PG_SUPPORT_UVD)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				      PHM_PlatformCaps_UVDPowerGating);
+		if (sys_info.value & AMD_PG_SUPPORT_VCE)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+				      PHM_PlatformCaps_VCEPowerGating);
+	}
+
+	if (0 == result) {
+		data->is_tlu_enabled = 0;
+		hwmgr->platform_descriptor.hardwareActivityPerformanceLevels =
+			TONGA_MAX_HARDWARE_POWERLEVELS;
+		hwmgr->platform_descriptor.hardwarePerformanceLevels = 2;
+		hwmgr->platform_descriptor.minimumClocksReductionPercentage  = 50;
+
+		sys_info.size = sizeof(struct cgs_system_info);
+		sys_info.info_id = CGS_SYSTEM_INFO_PCIE_GEN_INFO;
+		result = cgs_query_system_info(hwmgr->device, &sys_info);
+		if (result)
+			data->pcie_gen_cap = 0x30007;
+		else
+			data->pcie_gen_cap = (uint32_t)sys_info.value;
+		if (data->pcie_gen_cap & CAIL_PCIE_LINK_SPEED_SUPPORT_GEN3)
+			data->pcie_spc_cap = 20;
+		sys_info.size = sizeof(struct cgs_system_info);
+		sys_info.info_id = CGS_SYSTEM_INFO_PCIE_MLW;
+		result = cgs_query_system_info(hwmgr->device, &sys_info);
+		if (result)
+			data->pcie_lane_cap = 0x2f0000;
+		else
+			data->pcie_lane_cap = (uint32_t)sys_info.value;
+	} else {
+		/* Ignore return value in here, we are cleaning up a mess. */
+		tonga_hwmgr_backend_fini(hwmgr);
+	}
+
+	return result;
+}
+
+static int tonga_force_dpm_level(struct pp_hwmgr *hwmgr,
+		enum amd_dpm_forced_level level)
+{
+	int ret = 0;
+
+	switch (level) {
+	case AMD_DPM_FORCED_LEVEL_HIGH:
+		ret = tonga_force_dpm_highest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_LOW:
+		ret = tonga_force_dpm_lowest(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	case AMD_DPM_FORCED_LEVEL_AUTO:
+		ret = tonga_unforce_dpm_levels(hwmgr);
+		if (ret)
+			return ret;
+		break;
+	default:
+		break;
+	}
+
+	hwmgr->dpm_level = level;
+	return ret;
+}
+
+static int tonga_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
+				struct pp_power_state  *prequest_ps,
+			const struct pp_power_state *pcurrent_ps)
+{
+	struct tonga_power_state *tonga_ps =
+				cast_phw_tonga_power_state(&prequest_ps->hardware);
+
+	uint32_t sclk;
+	uint32_t mclk;
+	struct PP_Clocks minimum_clocks = {0};
+	bool disable_mclk_switching;
+	bool disable_mclk_switching_for_frame_lock;
+	struct cgs_display_info info = {0};
+	const struct phm_clock_and_voltage_limits *max_limits;
+	uint32_t i;
+	tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	int32_t count;
+	int32_t stable_pstate_sclk = 0, stable_pstate_mclk = 0;
+
+	data->battery_state = (PP_StateUILabel_Battery == prequest_ps->classification.ui_label);
+
+	PP_ASSERT_WITH_CODE(tonga_ps->performance_level_count == 2,
+				 "VI should always have 2 performance levels",
+				 );
+
+	max_limits = (PP_PowerSource_AC == hwmgr->power_source) ?
+			&(hwmgr->dyn_state.max_clock_voltage_on_ac) :
+			&(hwmgr->dyn_state.max_clock_voltage_on_dc);
+
+	if (PP_PowerSource_DC == hwmgr->power_source) {
+		for (i = 0; i < tonga_ps->performance_level_count; i++) {
+			if (tonga_ps->performance_levels[i].memory_clock > max_limits->mclk)
+				tonga_ps->performance_levels[i].memory_clock = max_limits->mclk;
+			if (tonga_ps->performance_levels[i].engine_clock > max_limits->sclk)
+				tonga_ps->performance_levels[i].engine_clock = max_limits->sclk;
+		}
+	}
+
+	tonga_ps->vce_clocks.EVCLK = hwmgr->vce_arbiter.evclk;
+	tonga_ps->vce_clocks.ECCLK = hwmgr->vce_arbiter.ecclk;
+
+	tonga_ps->acp_clk = hwmgr->acp_arbiter.acpclk;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	/*TO DO result = PHM_CheckVBlankTime(hwmgr, &vblankTooShort);*/
+
+	/* TO DO GetMinClockSettings(hwmgr->pPECI, &minimum_clocks); */
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_StablePState)) {
+
+		max_limits = &(hwmgr->dyn_state.max_clock_voltage_on_ac);
+		stable_pstate_sclk = (max_limits->sclk * 75) / 100;
+
+		for (count = pptable_info->vdd_dep_on_sclk->count-1; count >= 0; count--) {
+			if (stable_pstate_sclk >= pptable_info->vdd_dep_on_sclk->entries[count].clk) {
+				stable_pstate_sclk = pptable_info->vdd_dep_on_sclk->entries[count].clk;
+				break;
+			}
+		}
+
+		if (count < 0)
+			stable_pstate_sclk = pptable_info->vdd_dep_on_sclk->entries[0].clk;
+
+		stable_pstate_mclk = max_limits->mclk;
+
+		minimum_clocks.engineClock = stable_pstate_sclk;
+		minimum_clocks.memoryClock = stable_pstate_mclk;
+	}
+
+	if (minimum_clocks.engineClock < hwmgr->gfx_arbiter.sclk)
+		minimum_clocks.engineClock = hwmgr->gfx_arbiter.sclk;
+
+	if (minimum_clocks.memoryClock < hwmgr->gfx_arbiter.mclk)
+		minimum_clocks.memoryClock = hwmgr->gfx_arbiter.mclk;
+
+	tonga_ps->sclk_threshold = hwmgr->gfx_arbiter.sclk_threshold;
+
+	if (0 != hwmgr->gfx_arbiter.sclk_over_drive) {
+		PP_ASSERT_WITH_CODE((hwmgr->gfx_arbiter.sclk_over_drive <= hwmgr->platform_descriptor.overdriveLimit.engineClock),
+					"Overdrive sclk exceeds limit",
+					hwmgr->gfx_arbiter.sclk_over_drive = hwmgr->platform_descriptor.overdriveLimit.engineClock);
+
+		if (hwmgr->gfx_arbiter.sclk_over_drive >= hwmgr->gfx_arbiter.sclk)
+			tonga_ps->performance_levels[1].engine_clock = hwmgr->gfx_arbiter.sclk_over_drive;
+	}
+
+	if (0 != hwmgr->gfx_arbiter.mclk_over_drive) {
+		PP_ASSERT_WITH_CODE((hwmgr->gfx_arbiter.mclk_over_drive <= hwmgr->platform_descriptor.overdriveLimit.memoryClock),
+			"Overdrive mclk exceeds limit",
+			hwmgr->gfx_arbiter.mclk_over_drive = hwmgr->platform_descriptor.overdriveLimit.memoryClock);
+
+		if (hwmgr->gfx_arbiter.mclk_over_drive >= hwmgr->gfx_arbiter.mclk)
+			tonga_ps->performance_levels[1].memory_clock = hwmgr->gfx_arbiter.mclk_over_drive;
+	}
+
+	disable_mclk_switching_for_frame_lock = phm_cap_enabled(
+				    hwmgr->platform_descriptor.platformCaps,
+				    PHM_PlatformCaps_DisableMclkSwitchingForFrameLock);
+
+	disable_mclk_switching = (1 < info.display_count) ||
+				    disable_mclk_switching_for_frame_lock;
+
+	sclk  = tonga_ps->performance_levels[0].engine_clock;
+	mclk  = tonga_ps->performance_levels[0].memory_clock;
+
+	if (disable_mclk_switching)
+		mclk  = tonga_ps->performance_levels[tonga_ps->performance_level_count - 1].memory_clock;
+
+	if (sclk < minimum_clocks.engineClock)
+		sclk = (minimum_clocks.engineClock > max_limits->sclk) ? max_limits->sclk : minimum_clocks.engineClock;
+
+	if (mclk < minimum_clocks.memoryClock)
+		mclk = (minimum_clocks.memoryClock > max_limits->mclk) ? max_limits->mclk : minimum_clocks.memoryClock;
+
+	tonga_ps->performance_levels[0].engine_clock = sclk;
+	tonga_ps->performance_levels[0].memory_clock = mclk;
+
+	tonga_ps->performance_levels[1].engine_clock =
+		(tonga_ps->performance_levels[1].engine_clock >= tonga_ps->performance_levels[0].engine_clock) ?
+			      tonga_ps->performance_levels[1].engine_clock :
+			      tonga_ps->performance_levels[0].engine_clock;
+
+	if (disable_mclk_switching) {
+		if (mclk < tonga_ps->performance_levels[1].memory_clock)
+			mclk = tonga_ps->performance_levels[1].memory_clock;
+
+		tonga_ps->performance_levels[0].memory_clock = mclk;
+		tonga_ps->performance_levels[1].memory_clock = mclk;
+	} else {
+		if (tonga_ps->performance_levels[1].memory_clock < tonga_ps->performance_levels[0].memory_clock)
+			tonga_ps->performance_levels[1].memory_clock = tonga_ps->performance_levels[0].memory_clock;
+	}
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_StablePState)) {
+		for (i=0; i < tonga_ps->performance_level_count; i++) {
+			tonga_ps->performance_levels[i].engine_clock = stable_pstate_sclk;
+			tonga_ps->performance_levels[i].memory_clock = stable_pstate_mclk;
+			tonga_ps->performance_levels[i].pcie_gen = data->pcie_gen_performance.max;
+			tonga_ps->performance_levels[i].pcie_lane = data->pcie_gen_performance.max;
+		}
+	}
+
+	return 0;
+}
+
+int tonga_get_power_state_size(struct pp_hwmgr *hwmgr)
+{
+	return sizeof(struct tonga_power_state);
+}
+
+static int tonga_dpm_get_mclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct pp_power_state  *ps;
+	struct tonga_power_state  *tonga_ps;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	ps = hwmgr->request_ps;
+
+	if (ps == NULL)
+		return -EINVAL;
+
+	tonga_ps = cast_phw_tonga_power_state(&ps->hardware);
+
+	if (low)
+		return tonga_ps->performance_levels[0].memory_clock;
+	else
+		return tonga_ps->performance_levels[tonga_ps->performance_level_count-1].memory_clock;
+}
+
+static int tonga_dpm_get_sclk(struct pp_hwmgr *hwmgr, bool low)
+{
+	struct pp_power_state  *ps;
+	struct tonga_power_state  *tonga_ps;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	ps = hwmgr->request_ps;
+
+	if (ps == NULL)
+		return -EINVAL;
+
+	tonga_ps = cast_phw_tonga_power_state(&ps->hardware);
+
+	if (low)
+		return tonga_ps->performance_levels[0].engine_clock;
+	else
+		return tonga_ps->performance_levels[tonga_ps->performance_level_count-1].engine_clock;
+}
+
+static uint16_t tonga_get_current_pcie_speed(
+						   struct pp_hwmgr *hwmgr)
+{
+	uint32_t speed_cntl = 0;
+
+	speed_cntl = cgs_read_ind_register(hwmgr->device,
+						   CGS_IND_REG__PCIE,
+						   ixPCIE_LC_SPEED_CNTL);
+	return((uint16_t)PHM_GET_FIELD(speed_cntl,
+			PCIE_LC_SPEED_CNTL, LC_CURRENT_DATA_RATE));
+}
+
+static int tonga_get_current_pcie_lane_number(
+						   struct pp_hwmgr *hwmgr)
+{
+	uint32_t link_width;
+
+	link_width = PHM_READ_INDIRECT_FIELD(hwmgr->device,
+							CGS_IND_REG__PCIE,
+						  PCIE_LC_LINK_WIDTH_CNTL,
+							LC_LINK_WIDTH_RD);
+
+	PP_ASSERT_WITH_CODE((7 >= link_width),
+			"Invalid PCIe lane width!", return 0);
+
+	return decode_pcie_lane_width(link_width);
+}
+
+static int tonga_dpm_patch_boot_state(struct pp_hwmgr *hwmgr,
+					struct pp_hw_power_state *hw_ps)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_power_state *ps = (struct tonga_power_state *)hw_ps;
+	ATOM_FIRMWARE_INFO_V2_2 *fw_info;
+	uint16_t size;
+	uint8_t frev, crev;
+	int index = GetIndexIntoMasterTable(DATA, FirmwareInfo);
+
+	/* First retrieve the Boot clocks and VDDC from the firmware info table.
+	 * We assume here that fw_info is unchanged if this call fails.
+	 */
+	fw_info = (ATOM_FIRMWARE_INFO_V2_2 *)cgs_atom_get_data_table(
+			hwmgr->device, index,
+			&size, &frev, &crev);
+	if (!fw_info)
+		/* During a test, there is no firmware info table. */
+		return 0;
+
+	/* Patch the state. */
+	data->vbios_boot_state.sclk_bootup_value  = le32_to_cpu(fw_info->ulDefaultEngineClock);
+	data->vbios_boot_state.mclk_bootup_value  = le32_to_cpu(fw_info->ulDefaultMemoryClock);
+	data->vbios_boot_state.mvdd_bootup_value  = le16_to_cpu(fw_info->usBootUpMVDDCVoltage);
+	data->vbios_boot_state.vddc_bootup_value  = le16_to_cpu(fw_info->usBootUpVDDCVoltage);
+	data->vbios_boot_state.vddci_bootup_value = le16_to_cpu(fw_info->usBootUpVDDCIVoltage);
+	data->vbios_boot_state.pcie_gen_bootup_value = tonga_get_current_pcie_speed(hwmgr);
+	data->vbios_boot_state.pcie_lane_bootup_value =
+			(uint16_t)tonga_get_current_pcie_lane_number(hwmgr);
+
+	/* set boot power state */
+	ps->performance_levels[0].memory_clock = data->vbios_boot_state.mclk_bootup_value;
+	ps->performance_levels[0].engine_clock = data->vbios_boot_state.sclk_bootup_value;
+	ps->performance_levels[0].pcie_gen = data->vbios_boot_state.pcie_gen_bootup_value;
+	ps->performance_levels[0].pcie_lane = data->vbios_boot_state.pcie_lane_bootup_value;
+
+	return 0;
+}
+
+static int tonga_get_pp_table_entry_callback_func(struct pp_hwmgr *hwmgr,
+		void *state, struct pp_power_state *power_state,
+		void *pp_table, uint32_t classification_flag)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	struct tonga_power_state  *tonga_ps =
+			(struct tonga_power_state *)(&(power_state->hardware));
+
+	struct tonga_performance_level *performance_level;
+
+	ATOM_Tonga_State *state_entry = (ATOM_Tonga_State *)state;
+
+	ATOM_Tonga_POWERPLAYTABLE *powerplay_table =
+			(ATOM_Tonga_POWERPLAYTABLE *)pp_table;
+
+	ATOM_Tonga_SCLK_Dependency_Table *sclk_dep_table =
+			(ATOM_Tonga_SCLK_Dependency_Table *)
+			(((unsigned long)powerplay_table) +
+			le16_to_cpu(powerplay_table->usSclkDependencyTableOffset));
+
+	ATOM_Tonga_MCLK_Dependency_Table *mclk_dep_table =
+			(ATOM_Tonga_MCLK_Dependency_Table *)
+			(((unsigned long)powerplay_table) +
+			le16_to_cpu(powerplay_table->usMclkDependencyTableOffset));
+
+	/* The following fields are not initialized here: id orderedList allStatesList */
+	power_state->classification.ui_label =
+			(le16_to_cpu(state_entry->usClassification) &
+			ATOM_PPLIB_CLASSIFICATION_UI_MASK) >>
+			ATOM_PPLIB_CLASSIFICATION_UI_SHIFT;
+	power_state->classification.flags = classification_flag;
+	/* NOTE: There is a classification2 flag in BIOS that is not being used right now */
+
+	power_state->classification.temporary_state = false;
+	power_state->classification.to_be_deleted = false;
+
+	power_state->validation.disallowOnDC =
+			(0 != (le32_to_cpu(state_entry->ulCapsAndSettings) & ATOM_Tonga_DISALLOW_ON_DC));
+
+	power_state->pcie.lanes = 0;
+
+	power_state->display.disableFrameModulation = false;
+	power_state->display.limitRefreshrate = false;
+	power_state->display.enableVariBright =
+			(0 != (le32_to_cpu(state_entry->ulCapsAndSettings) & ATOM_Tonga_ENABLE_VARIBRIGHT));
+
+	power_state->validation.supportedPowerLevels = 0;
+	power_state->uvd_clocks.VCLK = 0;
+	power_state->uvd_clocks.DCLK = 0;
+	power_state->temperatures.min = 0;
+	power_state->temperatures.max = 0;
+
+	performance_level = &(tonga_ps->performance_levels
+			[tonga_ps->performance_level_count++]);
+
+	PP_ASSERT_WITH_CODE(
+			(tonga_ps->performance_level_count < SMU72_MAX_LEVELS_GRAPHICS),
+			"Performance levels exceeds SMC limit!",
+			return -1);
+
+	PP_ASSERT_WITH_CODE(
+			(tonga_ps->performance_level_count <=
+					hwmgr->platform_descriptor.hardwareActivityPerformanceLevels),
+			"Performance levels exceeds Driver limit!",
+			return -1);
+
+	/* Performance levels are arranged from low to high. */
+	performance_level->memory_clock =
+				le32_to_cpu(mclk_dep_table->entries[state_entry->ucMemoryClockIndexLow].ulMclk);
+
+	performance_level->engine_clock =
+				le32_to_cpu(sclk_dep_table->entries[state_entry->ucEngineClockIndexLow].ulSclk);
+
+	performance_level->pcie_gen = get_pcie_gen_support(
+							data->pcie_gen_cap,
+					     state_entry->ucPCIEGenLow);
+
+	performance_level->pcie_lane = get_pcie_lane_support(
+						    data->pcie_lane_cap,
+					   state_entry->ucPCIELaneHigh);
+
+	performance_level =
+			&(tonga_ps->performance_levels[tonga_ps->performance_level_count++]);
+
+	performance_level->memory_clock =
+				le32_to_cpu(mclk_dep_table->entries[state_entry->ucMemoryClockIndexHigh].ulMclk);
+
+	performance_level->engine_clock =
+				le32_to_cpu(sclk_dep_table->entries[state_entry->ucEngineClockIndexHigh].ulSclk);
+
+	performance_level->pcie_gen = get_pcie_gen_support(
+							data->pcie_gen_cap,
+					    state_entry->ucPCIEGenHigh);
+
+	performance_level->pcie_lane = get_pcie_lane_support(
+						    data->pcie_lane_cap,
+					   state_entry->ucPCIELaneHigh);
+
+	return 0;
+}
+
+static int tonga_get_pp_table_entry(struct pp_hwmgr *hwmgr,
+		    unsigned long entry_index, struct pp_power_state *ps)
+{
+	int result;
+	struct tonga_power_state *tonga_ps;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	struct phm_ppt_v1_information *table_info =
+			(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	struct phm_ppt_v1_clock_voltage_dependency_table *dep_mclk_table =
+					   table_info->vdd_dep_on_mclk;
+
+	ps->hardware.magic = PhwTonga_Magic;
+
+	tonga_ps = cast_phw_tonga_power_state(&(ps->hardware));
+
+	result = tonga_get_powerplay_table_entry(hwmgr, entry_index, ps,
+			tonga_get_pp_table_entry_callback_func);
+
+	/* This is the earliest time we have all the dependency table and the VBIOS boot state
+	 * as PP_Tables_GetPowerPlayTableEntry retrieves the VBIOS boot state
+	 * if there is only one VDDCI/MCLK level, check if it's the same as VBIOS boot state
+	 */
+	if (dep_mclk_table != NULL && dep_mclk_table->count == 1) {
+		if (dep_mclk_table->entries[0].clk !=
+				data->vbios_boot_state.mclk_bootup_value)
+			printk(KERN_ERR "Single MCLK entry VDDCI/MCLK dependency table "
+					"does not match VBIOS boot MCLK level");
+		if (dep_mclk_table->entries[0].vddci !=
+				data->vbios_boot_state.vddci_bootup_value)
+			printk(KERN_ERR "Single VDDCI entry VDDCI/MCLK dependency table "
+					"does not match VBIOS boot VDDCI level");
+	}
+
+	/* set DC compatible flag if this state supports DC */
+	if (!ps->validation.disallowOnDC)
+		tonga_ps->dc_compatible = true;
+
+	if (ps->classification.flags & PP_StateClassificationFlag_ACPI)
+		data->acpi_pcie_gen = tonga_ps->performance_levels[0].pcie_gen;
+	else if (ps->classification.flags & PP_StateClassificationFlag_Boot) {
+		if (data->bacos.best_match == 0xffff) {
+			/* For V.I. use boot state as base BACO state */
+			data->bacos.best_match = PP_StateClassificationFlag_Boot;
+			data->bacos.performance_level = tonga_ps->performance_levels[0];
+		}
+	}
+
+	tonga_ps->uvd_clocks.VCLK = ps->uvd_clocks.VCLK;
+	tonga_ps->uvd_clocks.DCLK = ps->uvd_clocks.DCLK;
+
+	if (!result) {
+		uint32_t i;
+
+		switch (ps->classification.ui_label) {
+		case PP_StateUILabel_Performance:
+			data->use_pcie_performance_levels = true;
+
+			for (i = 0; i < tonga_ps->performance_level_count; i++) {
+				if (data->pcie_gen_performance.max <
+						tonga_ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_performance.max =
+							tonga_ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_gen_performance.min >
+						tonga_ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_performance.min =
+							tonga_ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_lane_performance.max <
+						tonga_ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_performance.max =
+							tonga_ps->performance_levels[i].pcie_lane;
+
+				if (data->pcie_lane_performance.min >
+						tonga_ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_performance.min =
+							tonga_ps->performance_levels[i].pcie_lane;
+			}
+			break;
+		case PP_StateUILabel_Battery:
+			data->use_pcie_power_saving_levels = true;
+
+			for (i = 0; i < tonga_ps->performance_level_count; i++) {
+				if (data->pcie_gen_power_saving.max <
+						tonga_ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_power_saving.max =
+							tonga_ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_gen_power_saving.min >
+						tonga_ps->performance_levels[i].pcie_gen)
+					data->pcie_gen_power_saving.min =
+							tonga_ps->performance_levels[i].pcie_gen;
+
+				if (data->pcie_lane_power_saving.max <
+						tonga_ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_power_saving.max =
+							tonga_ps->performance_levels[i].pcie_lane;
+
+				if (data->pcie_lane_power_saving.min >
+						tonga_ps->performance_levels[i].pcie_lane)
+					data->pcie_lane_power_saving.min =
+							tonga_ps->performance_levels[i].pcie_lane;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+	return 0;
+}
+
+static void
+tonga_print_current_perforce_level(struct pp_hwmgr *hwmgr, struct seq_file *m)
+{
+	uint32_t sclk, mclk, activity_percent;
+	uint32_t offset;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	smum_send_msg_to_smc(hwmgr->smumgr, (PPSMC_Msg)(PPSMC_MSG_API_GetSclkFrequency));
+
+	sclk = cgs_read_register(hwmgr->device, mmSMC_MSG_ARG_0);
+
+	smum_send_msg_to_smc(hwmgr->smumgr, (PPSMC_Msg)(PPSMC_MSG_API_GetMclkFrequency));
+
+	mclk = cgs_read_register(hwmgr->device, mmSMC_MSG_ARG_0);
+	seq_printf(m, "\n [  mclk  ]: %u MHz\n\n [  sclk  ]: %u MHz\n", mclk/100, sclk/100);
+
+
+	offset = data->soft_regs_start + offsetof(SMU72_SoftRegisters, AverageGraphicsActivity);
+	activity_percent = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, offset);
+	activity_percent += 0x80;
+	activity_percent >>= 8;
+
+	seq_printf(m, "\n [GPU load]: %u%%\n\n", activity_percent > 100 ? 100 : activity_percent);
+
+}
+
+static int tonga_find_dpm_states_clocks_in_dpm_table(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	const struct tonga_power_state *tonga_ps = cast_const_phw_tonga_power_state(states->pnew_state);
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_single_dpm_table *psclk_table = &(data->dpm_table.sclk_table);
+	uint32_t sclk = tonga_ps->performance_levels[tonga_ps->performance_level_count-1].engine_clock;
+	struct tonga_single_dpm_table *pmclk_table = &(data->dpm_table.mclk_table);
+	uint32_t mclk = tonga_ps->performance_levels[tonga_ps->performance_level_count-1].memory_clock;
+	struct PP_Clocks min_clocks = {0};
+	uint32_t i;
+	struct cgs_display_info info = {0};
+
+	data->need_update_smu7_dpm_table = 0;
+
+	for (i = 0; i < psclk_table->count; i++) {
+		if (sclk == psclk_table->dpm_levels[i].value)
+			break;
+	}
+
+	if (i >= psclk_table->count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_OD_UPDATE_SCLK;
+	else {
+	/* TODO: Check SCLK in DAL's minimum clocks in case DeepSleep divider update is required.*/
+		if(data->display_timing.min_clock_insr != min_clocks.engineClockInSR)
+			data->need_update_smu7_dpm_table |= DPMTABLE_UPDATE_SCLK;
+	}
+
+	for (i=0; i < pmclk_table->count; i++) {
+		if (mclk == pmclk_table->dpm_levels[i].value)
+			break;
+	}
+
+	if (i >= pmclk_table->count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_OD_UPDATE_MCLK;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	if (data->display_timing.num_existing_displays != info.display_count)
+		data->need_update_smu7_dpm_table |= DPMTABLE_UPDATE_MCLK;
+
+	return 0;
+}
+
+static uint16_t tonga_get_maximum_link_speed(struct pp_hwmgr *hwmgr, const struct tonga_power_state *hw_ps)
+{
+	uint32_t i;
+	uint32_t sclk, max_sclk = 0;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	struct tonga_dpm_table *pdpm_table = &data->dpm_table;
+
+	for (i = 0; i < hw_ps->performance_level_count; i++) {
+		sclk = hw_ps->performance_levels[i].engine_clock;
+		if (max_sclk < sclk)
+			max_sclk = sclk;
+	}
+
+	for (i = 0; i < pdpm_table->sclk_table.count; i++) {
+		if (pdpm_table->sclk_table.dpm_levels[i].value == max_sclk)
+			return (uint16_t) ((i >= pdpm_table->pcie_speed_table.count) ?
+					pdpm_table->pcie_speed_table.dpm_levels[pdpm_table->pcie_speed_table.count-1].value :
+					pdpm_table->pcie_speed_table.dpm_levels[i].value);
+	}
+
+	return 0;
+}
+
+static int tonga_request_link_speed_change_before_state_change(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	const struct tonga_power_state *tonga_nps = cast_const_phw_tonga_power_state(states->pnew_state);
+	const struct tonga_power_state *tonga_cps = cast_const_phw_tonga_power_state(states->pcurrent_state);
+
+	uint16_t target_link_speed = tonga_get_maximum_link_speed(hwmgr, tonga_nps);
+	uint16_t current_link_speed;
+
+	if (data->force_pcie_gen == PP_PCIEGenInvalid)
+		current_link_speed = tonga_get_maximum_link_speed(hwmgr, tonga_cps);
+	else
+		current_link_speed = data->force_pcie_gen;
+
+	data->force_pcie_gen = PP_PCIEGenInvalid;
+	data->pspp_notify_required = false;
+	if (target_link_speed > current_link_speed) {
+		switch(target_link_speed) {
+		case PP_PCIEGen3:
+			if (0 == acpi_pcie_perf_request(hwmgr->device, PCIE_PERF_REQ_GEN3, false))
+				break;
+			data->force_pcie_gen = PP_PCIEGen2;
+			if (current_link_speed == PP_PCIEGen2)
+				break;
+		case PP_PCIEGen2:
+			if (0 == acpi_pcie_perf_request(hwmgr->device, PCIE_PERF_REQ_GEN2, false))
+				break;
+		default:
+			data->force_pcie_gen = tonga_get_current_pcie_speed(hwmgr);
+			break;
+		}
+	} else {
+		if (target_link_speed < current_link_speed)
+			data->pspp_notify_required = true;
+	}
+
+	return 0;
+}
+
+static int tonga_freeze_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if ((0 == data->sclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+		(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK))) {
+		PP_ASSERT_WITH_CODE(
+			true == tonga_is_dpm_running(hwmgr),
+			"Trying to freeze SCLK DPM when DPM is disabled",
+			);
+		PP_ASSERT_WITH_CODE(
+			0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					  PPSMC_MSG_SCLKDPM_FreezeLevel),
+			"Failed to freeze SCLK DPM during FreezeSclkMclkDPM Function!",
+			return -1);
+	}
+
+	if ((0 == data->mclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+		 DPMTABLE_OD_UPDATE_MCLK)) {
+		PP_ASSERT_WITH_CODE(true == tonga_is_dpm_running(hwmgr),
+			"Trying to freeze MCLK DPM when DPM is disabled",
+			);
+		PP_ASSERT_WITH_CODE(
+			0 == smum_send_msg_to_smc(hwmgr->smumgr,
+							PPSMC_MSG_MCLKDPM_FreezeLevel),
+			"Failed to freeze MCLK DPM during FreezeSclkMclkDPM Function!",
+			return -1);
+	}
+
+	return 0;
+}
+
+static int tonga_populate_and_upload_sclk_mclk_dpm_levels(struct pp_hwmgr *hwmgr, const void *input)
+{
+	int result = 0;
+
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	const struct tonga_power_state *tonga_ps = cast_const_phw_tonga_power_state(states->pnew_state);
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t sclk = tonga_ps->performance_levels[tonga_ps->performance_level_count-1].engine_clock;
+	uint32_t mclk = tonga_ps->performance_levels[tonga_ps->performance_level_count-1].memory_clock;
+	struct tonga_dpm_table *pdpm_table = &data->dpm_table;
+
+	struct tonga_dpm_table *pgolden_dpm_table = &data->golden_dpm_table;
+	uint32_t dpm_count, clock_percent;
+	uint32_t i;
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if (data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_SCLK) {
+		pdpm_table->sclk_table.dpm_levels[pdpm_table->sclk_table.count-1].value = sclk;
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_OD6PlusinACSupport) ||
+		    phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_OD6PlusinDCSupport)) {
+		/* Need to do calculation based on the golden DPM table
+		 * as the Heatmap GPU Clock axis is also based on the default values
+		 */
+			PP_ASSERT_WITH_CODE(
+				(pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value != 0),
+				"Divide by 0!",
+				return -1);
+			dpm_count = pdpm_table->sclk_table.count < 2 ? 0 : pdpm_table->sclk_table.count-2;
+			for (i = dpm_count; i > 1; i--) {
+				if (sclk > pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value) {
+					clock_percent = ((sclk - pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value)*100) /
+							pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value;
+
+					pdpm_table->sclk_table.dpm_levels[i].value =
+							pgolden_dpm_table->sclk_table.dpm_levels[i].value +
+							(pgolden_dpm_table->sclk_table.dpm_levels[i].value * clock_percent)/100;
+
+				} else if (pgolden_dpm_table->sclk_table.dpm_levels[pdpm_table->sclk_table.count-1].value > sclk) {
+					clock_percent = ((pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value - sclk)*100) /
+								pgolden_dpm_table->sclk_table.dpm_levels[pgolden_dpm_table->sclk_table.count-1].value;
+
+					pdpm_table->sclk_table.dpm_levels[i].value =
+							pgolden_dpm_table->sclk_table.dpm_levels[i].value -
+							(pgolden_dpm_table->sclk_table.dpm_levels[i].value * clock_percent)/100;
+				} else
+					pdpm_table->sclk_table.dpm_levels[i].value =
+							pgolden_dpm_table->sclk_table.dpm_levels[i].value;
+			}
+		}
+	}
+
+	if (data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_MCLK) {
+		pdpm_table->mclk_table.dpm_levels[pdpm_table->mclk_table.count-1].value = mclk;
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_OD6PlusinACSupport) ||
+			phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_OD6PlusinDCSupport)) {
+
+			PP_ASSERT_WITH_CODE(
+					(pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value != 0),
+					"Divide by 0!",
+					return -1);
+			dpm_count = pdpm_table->mclk_table.count < 2? 0 : pdpm_table->mclk_table.count-2;
+			for (i = dpm_count; i > 1; i--) {
+				if (mclk > pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value) {
+						clock_percent = ((mclk - pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value)*100) /
+								    pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value;
+
+						pdpm_table->mclk_table.dpm_levels[i].value =
+										pgolden_dpm_table->mclk_table.dpm_levels[i].value +
+										(pgolden_dpm_table->mclk_table.dpm_levels[i].value * clock_percent)/100;
+
+				} else if (pgolden_dpm_table->mclk_table.dpm_levels[pdpm_table->mclk_table.count-1].value > mclk) {
+						clock_percent = ((pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value - mclk)*100) /
+								    pgolden_dpm_table->mclk_table.dpm_levels[pgolden_dpm_table->mclk_table.count-1].value;
+
+						pdpm_table->mclk_table.dpm_levels[i].value =
+									pgolden_dpm_table->mclk_table.dpm_levels[i].value -
+									(pgolden_dpm_table->mclk_table.dpm_levels[i].value * clock_percent)/100;
+				} else
+					pdpm_table->mclk_table.dpm_levels[i].value = pgolden_dpm_table->mclk_table.dpm_levels[i].value;
+			}
+		}
+	}
+
+	if (data->need_update_smu7_dpm_table & (DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK)) {
+		result = tonga_populate_all_memory_levels(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == result),
+			"Failed to populate SCLK during PopulateNewDPMClocksStates Function!",
+			return result);
+	}
+
+	if (data->need_update_smu7_dpm_table & (DPMTABLE_OD_UPDATE_MCLK + DPMTABLE_UPDATE_MCLK)) {
+		/*populate MCLK dpm table to SMU7 */
+		result = tonga_populate_all_memory_levels(hwmgr);
+		PP_ASSERT_WITH_CODE((0 == result),
+				"Failed to populate MCLK during PopulateNewDPMClocksStates Function!",
+				return result);
+	}
+
+	return result;
+}
+
+static	int tonga_trim_single_dpm_states(struct pp_hwmgr *hwmgr,
+			  struct tonga_single_dpm_table * pdpm_table,
+			     uint32_t low_limit, uint32_t high_limit)
+{
+	uint32_t i;
+
+	for (i = 0; i < pdpm_table->count; i++) {
+		if ((pdpm_table->dpm_levels[i].value < low_limit) ||
+		    (pdpm_table->dpm_levels[i].value > high_limit))
+			pdpm_table->dpm_levels[i].enabled = false;
+		else
+			pdpm_table->dpm_levels[i].enabled = true;
+	}
+	return 0;
+}
+
+static int tonga_trim_dpm_states(struct pp_hwmgr *hwmgr, const struct tonga_power_state *hw_state)
+{
+	int result = 0;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t high_limit_count;
+
+	PP_ASSERT_WITH_CODE((hw_state->performance_level_count >= 1),
+				"power state did not have any performance level",
+				 return -1);
+
+	high_limit_count = (1 == hw_state->performance_level_count) ? 0: 1;
+
+	tonga_trim_single_dpm_states(hwmgr,
+					&(data->dpm_table.sclk_table),
+					hw_state->performance_levels[0].engine_clock,
+					hw_state->performance_levels[high_limit_count].engine_clock);
+
+	tonga_trim_single_dpm_states(hwmgr,
+						&(data->dpm_table.mclk_table),
+						hw_state->performance_levels[0].memory_clock,
+						hw_state->performance_levels[high_limit_count].memory_clock);
+
+	return result;
+}
+
+static int tonga_generate_dpm_level_enable_mask(struct pp_hwmgr *hwmgr, const void *input)
+{
+	int result;
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	const struct tonga_power_state *tonga_ps = cast_const_phw_tonga_power_state(states->pnew_state);
+
+	result = tonga_trim_dpm_states(hwmgr, tonga_ps);
+	if (0 != result)
+		return result;
+
+	data->dpm_level_enable_mask.sclk_dpm_enable_mask = tonga_get_dpm_level_enable_mask_value(&data->dpm_table.sclk_table);
+	data->dpm_level_enable_mask.mclk_dpm_enable_mask = tonga_get_dpm_level_enable_mask_value(&data->dpm_table.mclk_table);
+	data->last_mclk_dpm_enable_mask = data->dpm_level_enable_mask.mclk_dpm_enable_mask;
+	if (data->uvd_enabled)
+		data->dpm_level_enable_mask.mclk_dpm_enable_mask &= 0xFFFFFFFE;
+
+	data->dpm_level_enable_mask.pcie_dpm_enable_mask = tonga_get_dpm_level_enable_mask_value(&data->dpm_table.pcie_speed_table);
+
+	return 0;
+}
+
+int tonga_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable ?
+				  (PPSMC_Msg)PPSMC_MSG_VCEDPM_Enable :
+				  (PPSMC_Msg)PPSMC_MSG_VCEDPM_Disable);
+}
+
+int tonga_enable_disable_uvd_dpm(struct pp_hwmgr *hwmgr, bool enable)
+{
+	return smum_send_msg_to_smc(hwmgr->smumgr, enable ?
+				  (PPSMC_Msg)PPSMC_MSG_UVDDPM_Enable :
+				  (PPSMC_Msg)PPSMC_MSG_UVDDPM_Disable);
+}
+
+int tonga_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *ptable_information = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (!bgate) {
+		data->smc_state_table.UvdBootLevel = (uint8_t) (ptable_information->mm_dep_table->count - 1);
+		mm_boot_level_offset = data->dpm_table_start + offsetof(SMU72_Discrete_DpmTable, UvdBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0x00FFFFFF;
+		mm_boot_level_value |= data->smc_state_table.UvdBootLevel << 24;
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (!phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_UVDDPM) ||
+		    phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_StablePState))
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+						PPSMC_MSG_UVDDPM_SetEnabledMask,
+						(uint32_t)(1 << data->smc_state_table.UvdBootLevel));
+	}
+
+	return tonga_enable_disable_uvd_dpm(hwmgr, !bgate);
+}
+
+int tonga_update_vce_dpm(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	const struct tonga_power_state *tonga_nps = cast_const_phw_tonga_power_state(states->pnew_state);
+	const struct tonga_power_state *tonga_cps = cast_const_phw_tonga_power_state(states->pcurrent_state);
+
+	uint32_t mm_boot_level_offset, mm_boot_level_value;
+	struct phm_ppt_v1_information *pptable_info = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (tonga_nps->vce_clocks.EVCLK > 0 && (tonga_cps == NULL || tonga_cps->vce_clocks.EVCLK == 0)) {
+		data->smc_state_table.VceBootLevel = (uint8_t) (pptable_info->mm_dep_table->count - 1);
+
+		mm_boot_level_offset = data->dpm_table_start + offsetof(SMU72_Discrete_DpmTable, VceBootLevel);
+		mm_boot_level_offset /= 4;
+		mm_boot_level_offset *= 4;
+		mm_boot_level_value = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, mm_boot_level_offset);
+		mm_boot_level_value &= 0xFF00FFFF;
+		mm_boot_level_value |= data->smc_state_table.VceBootLevel << 16;
+		cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, mm_boot_level_offset, mm_boot_level_value);
+
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_StablePState))
+			smum_send_msg_to_smc_with_parameter(hwmgr->smumgr,
+					PPSMC_MSG_VCEDPM_SetEnabledMask,
+				(uint32_t)(1 << data->smc_state_table.VceBootLevel));
+
+		tonga_enable_disable_vce_dpm(hwmgr, true);
+	} else if (tonga_nps->vce_clocks.EVCLK == 0 && tonga_cps != NULL && tonga_cps->vce_clocks.EVCLK > 0)
+		tonga_enable_disable_vce_dpm(hwmgr, false);
+
+	return 0;
+}
+
+static int tonga_update_and_upload_mc_reg_table(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	uint32_t address;
+	int32_t result;
+
+	if (0 == (data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_MCLK))
+		return 0;
+
+
+	memset(&data->mc_reg_table, 0, sizeof(SMU72_Discrete_MCRegisters));
+
+	result = tonga_convert_mc_reg_table_to_smc(hwmgr, &(data->mc_reg_table));
+
+	if(result != 0)
+		return result;
+
+
+	address = data->mc_reg_table_start + (uint32_t)offsetof(SMU72_Discrete_MCRegisters, data[0]);
+
+	return  tonga_copy_bytes_to_smc(hwmgr->smumgr, address,
+				 (uint8_t *)&data->mc_reg_table.data[0],
+				sizeof(SMU72_Discrete_MCRegisterSet) * data->dpm_table.mclk_table.count,
+				data->sram_end);
+}
+
+static int tonga_program_memory_timing_parameters_conditionally(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	if (data->need_update_smu7_dpm_table &
+		(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_OD_UPDATE_MCLK))
+		return tonga_program_memory_timing_parameters(hwmgr);
+
+	return 0;
+}
+
+static int tonga_unfreeze_sclk_mclk_dpm(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+
+	if (0 == data->need_update_smu7_dpm_table)
+		return 0;
+
+	if ((0 == data->sclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table &
+		(DPMTABLE_OD_UPDATE_SCLK + DPMTABLE_UPDATE_SCLK))) {
+
+		PP_ASSERT_WITH_CODE(true == tonga_is_dpm_running(hwmgr),
+			"Trying to Unfreeze SCLK DPM when DPM is disabled",
+			);
+		PP_ASSERT_WITH_CODE(
+			 0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					 PPSMC_MSG_SCLKDPM_UnfreezeLevel),
+			"Failed to unfreeze SCLK DPM during UnFreezeSclkMclkDPM Function!",
+			return -1);
+	}
+
+	if ((0 == data->mclk_dpm_key_disabled) &&
+		(data->need_update_smu7_dpm_table & DPMTABLE_OD_UPDATE_MCLK)) {
+
+		PP_ASSERT_WITH_CODE(
+				true == tonga_is_dpm_running(hwmgr),
+				"Trying to Unfreeze MCLK DPM when DPM is disabled",
+				);
+		PP_ASSERT_WITH_CODE(
+			 0 == smum_send_msg_to_smc(hwmgr->smumgr,
+					 PPSMC_MSG_SCLKDPM_UnfreezeLevel),
+		    "Failed to unfreeze MCLK DPM during UnFreezeSclkMclkDPM Function!",
+		    return -1);
+	}
+
+	data->need_update_smu7_dpm_table = 0;
+
+	return 0;
+}
+
+static int tonga_notify_link_speed_change_after_state_change(struct pp_hwmgr *hwmgr, const void *input)
+{
+	const struct phm_set_power_state_input *states = (const struct phm_set_power_state_input *)input;
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	const struct tonga_power_state *tonga_ps = cast_const_phw_tonga_power_state(states->pnew_state);
+	uint16_t target_link_speed = tonga_get_maximum_link_speed(hwmgr, tonga_ps);
+	uint8_t  request;
+
+	if (data->pspp_notify_required  ||
+	    data->pcie_performance_request) {
+		if (target_link_speed == PP_PCIEGen3)
+			request = PCIE_PERF_REQ_GEN3;
+		else if (target_link_speed == PP_PCIEGen2)
+			request = PCIE_PERF_REQ_GEN2;
+		else
+			request = PCIE_PERF_REQ_GEN1;
+
+		if(request == PCIE_PERF_REQ_GEN1 && tonga_get_current_pcie_speed(hwmgr) > 0) {
+			data->pcie_performance_request = false;
+			return 0;
+		}
+
+		if (0 != acpi_pcie_perf_request(hwmgr->device, request, false)) {
+			if (PP_PCIEGen2 == target_link_speed)
+				printk("PSPP request to switch to Gen2 from Gen3 Failed!");
+			else
+				printk("PSPP request to switch to Gen1 from Gen2 Failed!");
+		}
+	}
+
+	data->pcie_performance_request = false;
+	return 0;
+}
+
+static int tonga_set_power_state_tasks(struct pp_hwmgr *hwmgr, const void *input)
+{
+	int tmp_result, result = 0;
+
+	tmp_result = tonga_find_dpm_states_clocks_in_dpm_table(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to find DPM states clocks in DPM table!", result = tmp_result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_PCIEPerformanceRequest)) {
+		tmp_result = tonga_request_link_speed_change_before_state_change(hwmgr, input);
+		PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to request link speed change before state change!", result = tmp_result);
+	}
+
+	tmp_result = tonga_freeze_sclk_mclk_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to freeze SCLK MCLK DPM!", result = tmp_result);
+
+	tmp_result = tonga_populate_and_upload_sclk_mclk_dpm_levels(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to populate and upload SCLK MCLK DPM levels!", result = tmp_result);
+
+	tmp_result = tonga_generate_dpm_level_enable_mask(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to generate DPM level enabled mask!", result = tmp_result);
+
+	tmp_result = tonga_update_vce_dpm(hwmgr, input);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to update VCE DPM!", result = tmp_result);
+
+	tmp_result = tonga_update_sclk_threshold(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to update SCLK threshold!", result = tmp_result);
+
+	tmp_result = tonga_update_and_upload_mc_reg_table(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to upload MC reg table!", result = tmp_result);
+
+	tmp_result = tonga_program_memory_timing_parameters_conditionally(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to program memory timing parameters!", result = tmp_result);
+
+	tmp_result = tonga_unfreeze_sclk_mclk_dpm(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to unfreeze SCLK MCLK DPM!", result = tmp_result);
+
+	tmp_result = tonga_upload_dpm_level_enable_mask(hwmgr);
+	PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to upload DPM level enabled mask!", result = tmp_result);
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_PCIEPerformanceRequest)) {
+		tmp_result = tonga_notify_link_speed_change_after_state_change(hwmgr, input);
+		PP_ASSERT_WITH_CODE((0 == tmp_result), "Failed to notify link speed change after state change!", result = tmp_result);
+	}
+
+	return result;
+}
+
+/**
+*  Set maximum target operating fan output PWM
+*
+* @param    pHwMgr:  the address of the powerplay hardware manager.
+* @param    usMaxFanPwm:  max operating fan PWM in percents
+* @return   The response that came from the SMC.
+*/
+static int tonga_set_max_fan_pwm_output(struct pp_hwmgr *hwmgr, uint16_t us_max_fan_pwm)
+{
+	hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanPWM = us_max_fan_pwm;
+
+	if (phm_is_hw_access_blocked(hwmgr))
+		return 0;
+
+	return (0 == smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_SetFanPwmMax, us_max_fan_pwm) ? 0 : -1);
+}
+
+int tonga_notify_smc_display_config_after_ps_adjustment(struct pp_hwmgr *hwmgr)
+{
+	uint32_t num_active_displays = 0;
+	struct cgs_display_info info = {0};
+	info.mode_info = NULL;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	num_active_displays = info.display_count;
+
+	if (num_active_displays > 1)  /* to do && (pHwMgr->pPECI->displayConfiguration.bMultiMonitorInSync != TRUE)) */
+		tonga_notify_smc_display_change(hwmgr, false);
+	else
+		tonga_notify_smc_display_change(hwmgr, true);
+
+	return 0;
+}
+
+/**
+* Programs the display gap
+*
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @return   always OK
+*/
+int tonga_program_display_gap(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	uint32_t num_active_displays = 0;
+	uint32_t display_gap = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_DISPLAY_GAP_CNTL);
+	uint32_t display_gap2;
+	uint32_t pre_vbi_time_in_us;
+	uint32_t frame_time_in_us;
+	uint32_t ref_clock;
+	uint32_t refresh_rate = 0;
+	struct cgs_display_info info = {0};
+	struct cgs_mode_info mode_info;
+
+	info.mode_info = &mode_info;
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+	num_active_displays = info.display_count;
+
+	display_gap = PHM_SET_FIELD(display_gap, CG_DISPLAY_GAP_CNTL, DISP_GAP, (num_active_displays > 0)? DISPLAY_GAP_VBLANK_OR_WM : DISPLAY_GAP_IGNORE);
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_DISPLAY_GAP_CNTL, display_gap);
+
+	ref_clock = mode_info.ref_clock;
+	refresh_rate = mode_info.refresh_rate;
+
+	if(0 == refresh_rate)
+		refresh_rate = 60;
+
+	frame_time_in_us = 1000000 / refresh_rate;
+
+	pre_vbi_time_in_us = frame_time_in_us - 200 - mode_info.vblank_time_us;
+	display_gap2 = pre_vbi_time_in_us * (ref_clock / 100);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_DISPLAY_GAP_CNTL2, display_gap2);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, data->soft_regs_start + offsetof(SMU72_SoftRegisters, PreVBlankGap), 0x64);
+
+	cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, data->soft_regs_start + offsetof(SMU72_SoftRegisters, VBlankTimeout), (frame_time_in_us - pre_vbi_time_in_us));
+
+	if (num_active_displays == 1)
+		tonga_notify_smc_display_change(hwmgr, true);
+
+	return 0;
+}
+
+int tonga_display_configuration_changed_task(struct pp_hwmgr *hwmgr)
+{
+
+	tonga_program_display_gap(hwmgr);
+
+	/* to do PhwTonga_CacUpdateDisplayConfiguration(pHwMgr); */
+	return 0;
+}
+
+/**
+*  Set maximum target operating fan output RPM
+*
+* @param    pHwMgr:  the address of the powerplay hardware manager.
+* @param    usMaxFanRpm:  max operating fan RPM value.
+* @return   The response that came from the SMC.
+*/
+static int tonga_set_max_fan_rpm_output(struct pp_hwmgr *hwmgr, uint16_t us_max_fan_pwm)
+{
+	hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanRPM = us_max_fan_pwm;
+
+	if (phm_is_hw_access_blocked(hwmgr))
+		return 0;
+
+	return (0 == smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_SetFanRpmMax, us_max_fan_pwm) ? 0 : -1);
+}
+
+uint32_t tonga_get_xclk(struct pp_hwmgr *hwmgr)
+{
+	uint32_t reference_clock;
+	uint32_t tc;
+	uint32_t divide;
+
+	ATOM_FIRMWARE_INFO *fw_info;
+	uint16_t size;
+	uint8_t frev, crev;
+	int index = GetIndexIntoMasterTable(DATA, FirmwareInfo);
+
+	tc = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_CLKPIN_CNTL_2, MUX_TCLK_TO_XCLK);
+
+	if (tc)
+		return TCLK;
+
+	fw_info = (ATOM_FIRMWARE_INFO *)cgs_atom_get_data_table(hwmgr->device, index,
+						  &size, &frev, &crev);
+
+	if (!fw_info)
+		return 0;
+
+	reference_clock = le16_to_cpu(fw_info->usMinPixelClockPLL_Output);
+
+	divide = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_CLKPIN_CNTL, XTALIN_DIVIDE);
+
+	if (0 != divide)
+		return reference_clock / 4;
+
+	return reference_clock;
+}
+
+int tonga_dpm_set_interrupt_state(void *private_data,
+					 unsigned src_id, unsigned type,
+					 int enabled)
+{
+	uint32_t cg_thermal_int;
+	struct pp_hwmgr *hwmgr = ((struct pp_eventmgr *)private_data)->hwmgr;
+
+	if (hwmgr == NULL)
+		return -EINVAL;
+
+	switch (type) {
+	case AMD_THERMAL_IRQ_LOW_TO_HIGH:
+		if (enabled) {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int |= CG_THERMAL_INT_CTRL__THERM_INTH_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		} else {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int &= ~CG_THERMAL_INT_CTRL__THERM_INTH_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		}
+		break;
+
+	case AMD_THERMAL_IRQ_HIGH_TO_LOW:
+		if (enabled) {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int |= CG_THERMAL_INT_CTRL__THERM_INTL_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		} else {
+			cg_thermal_int = cgs_read_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT);
+			cg_thermal_int &= ~CG_THERMAL_INT_CTRL__THERM_INTL_MASK_MASK;
+			cgs_write_ind_register(hwmgr->device, CGS_IND_REG__SMC, ixCG_THERMAL_INT, cg_thermal_int);
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+int tonga_register_internal_thermal_interrupt(struct pp_hwmgr *hwmgr,
+					const void *thermal_interrupt_info)
+{
+	int result;
+	const struct pp_interrupt_registration_info *info =
+			(const struct pp_interrupt_registration_info *)thermal_interrupt_info;
+
+	if (info == NULL)
+		return -EINVAL;
+
+	result = cgs_add_irq_source(hwmgr->device, 230, AMD_THERMAL_IRQ_LAST,
+				tonga_dpm_set_interrupt_state,
+				info->call_back, info->context);
+
+	if (result)
+		return -EINVAL;
+
+	result = cgs_add_irq_source(hwmgr->device, 231, AMD_THERMAL_IRQ_LAST,
+				tonga_dpm_set_interrupt_state,
+				info->call_back, info->context);
+
+	if (result)
+		return -EINVAL;
+
+	return 0;
+}
+
+bool tonga_check_smc_update_required_for_display_configuration(struct pp_hwmgr *hwmgr)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	bool is_update_required = false;
+	struct cgs_display_info info = {0,0,NULL};
+
+	cgs_get_active_displays_info(hwmgr->device, &info);
+
+	if (data->display_timing.num_existing_displays != info.display_count)
+		is_update_required = true;
+/* TO DO NEED TO GET DEEP SLEEP CLOCK FROM DAL
+	if (phm_cap_enabled(hwmgr->hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_SclkDeepSleep)) {
+		cgs_get_min_clock_settings(hwmgr->device, &min_clocks);
+		if(min_clocks.engineClockInSR != data->display_timing.minClockInSR)
+			is_update_required = true;
+*/
+	return is_update_required;
+}
+
+static inline bool tonga_are_power_levels_equal(const struct tonga_performance_level *pl1,
+							   const struct tonga_performance_level *pl2)
+{
+	return ((pl1->memory_clock == pl2->memory_clock) &&
+		  (pl1->engine_clock == pl2->engine_clock) &&
+		  (pl1->pcie_gen == pl2->pcie_gen) &&
+		  (pl1->pcie_lane == pl2->pcie_lane));
+}
+
+int tonga_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 tonga_power_state *psa = cast_const_phw_tonga_power_state(pstate1);
+	const struct tonga_power_state *psb = cast_const_phw_tonga_power_state(pstate2);
+	int i;
+
+	if (equal == NULL || psa == NULL || psb == NULL)
+		return -EINVAL;
+
+	/* 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;
+		return 0;
+	}
+
+	for (i = 0; i < psa->performance_level_count; i++) {
+		if (!tonga_are_power_levels_equal(&(psa->performance_levels[i]), &(psb->performance_levels[i]))) {
+			/* If we have found even one performance level pair that is different the states are different. */
+			*equal = false;
+			return 0;
+		}
+	}
+
+	/* If all performance levels are the same try to use the UVD clocks to break the tie.*/
+	*equal = ((psa->uvd_clocks.VCLK == psb->uvd_clocks.VCLK) && (psa->uvd_clocks.DCLK == psb->uvd_clocks.DCLK));
+	*equal &= ((psa->vce_clocks.EVCLK == psb->vce_clocks.EVCLK) && (psa->vce_clocks.ECCLK == psb->vce_clocks.ECCLK));
+	*equal &= (psa->sclk_threshold == psb->sclk_threshold);
+	*equal &= (psa->acp_clk == psb->acp_clk);
+
+	return 0;
+}
+
+static int tonga_set_fan_control_mode(struct pp_hwmgr *hwmgr, uint32_t mode)
+{
+	if (mode) {
+		/* stop auto-manage */
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps,
+				PHM_PlatformCaps_MicrocodeFanControl))
+			tonga_fan_ctrl_stop_smc_fan_control(hwmgr);
+		tonga_fan_ctrl_set_static_mode(hwmgr, mode);
+	} else
+		/* restart auto-manage */
+		tonga_fan_ctrl_reset_fan_speed_to_default(hwmgr);
+
+	return 0;
+}
+
+static int tonga_get_fan_control_mode(struct pp_hwmgr *hwmgr)
+{
+	if (hwmgr->fan_ctrl_is_in_default_mode)
+		return hwmgr->fan_ctrl_default_mode;
+	else
+		return PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+				CG_FDO_CTRL2, FDO_PWM_MODE);
+}
+
+static const struct pp_hwmgr_func tonga_hwmgr_funcs = {
+	.backend_init = &tonga_hwmgr_backend_init,
+	.backend_fini = &tonga_hwmgr_backend_fini,
+	.asic_setup = &tonga_setup_asic_task,
+	.dynamic_state_management_enable = &tonga_enable_dpm_tasks,
+	.apply_state_adjust_rules = tonga_apply_state_adjust_rules,
+	.force_dpm_level = &tonga_force_dpm_level,
+	.power_state_set = tonga_set_power_state_tasks,
+	.get_power_state_size = tonga_get_power_state_size,
+	.get_mclk = tonga_dpm_get_mclk,
+	.get_sclk = tonga_dpm_get_sclk,
+	.patch_boot_state = tonga_dpm_patch_boot_state,
+	.get_pp_table_entry = tonga_get_pp_table_entry,
+	.get_num_of_pp_table_entries = tonga_get_number_of_powerplay_table_entries,
+	.print_current_perforce_level = tonga_print_current_perforce_level,
+	.powerdown_uvd = tonga_phm_powerdown_uvd,
+	.powergate_uvd = tonga_phm_powergate_uvd,
+	.powergate_vce = tonga_phm_powergate_vce,
+	.disable_clock_power_gating = tonga_phm_disable_clock_power_gating,
+	.notify_smc_display_config_after_ps_adjustment = tonga_notify_smc_display_config_after_ps_adjustment,
+	.display_config_changed = tonga_display_configuration_changed_task,
+	.set_max_fan_pwm_output = tonga_set_max_fan_pwm_output,
+	.set_max_fan_rpm_output = tonga_set_max_fan_rpm_output,
+	.get_temperature = tonga_thermal_get_temperature,
+	.stop_thermal_controller = tonga_thermal_stop_thermal_controller,
+	.get_fan_speed_info = tonga_fan_ctrl_get_fan_speed_info,
+	.get_fan_speed_percent = tonga_fan_ctrl_get_fan_speed_percent,
+	.set_fan_speed_percent = tonga_fan_ctrl_set_fan_speed_percent,
+	.reset_fan_speed_to_default = tonga_fan_ctrl_reset_fan_speed_to_default,
+	.get_fan_speed_rpm = tonga_fan_ctrl_get_fan_speed_rpm,
+	.set_fan_speed_rpm = tonga_fan_ctrl_set_fan_speed_rpm,
+	.uninitialize_thermal_controller = tonga_thermal_ctrl_uninitialize_thermal_controller,
+	.register_internal_thermal_interrupt = tonga_register_internal_thermal_interrupt,
+	.check_smc_update_required_for_display_configuration = tonga_check_smc_update_required_for_display_configuration,
+	.check_states_equal = tonga_check_states_equal,
+	.set_fan_control_mode = tonga_set_fan_control_mode,
+	.get_fan_control_mode = tonga_get_fan_control_mode,
+};
+
+int tonga_hwmgr_init(struct pp_hwmgr *hwmgr)
+{
+	tonga_hwmgr  *data;
+
+	data = kzalloc (sizeof(tonga_hwmgr), GFP_KERNEL);
+	if (data == NULL)
+		return -ENOMEM;
+	memset(data, 0x00, sizeof(tonga_hwmgr));
+
+	hwmgr->backend = data;
+	hwmgr->hwmgr_func = &tonga_hwmgr_funcs;
+	hwmgr->pptable_func = &tonga_pptable_funcs;
+	pp_tonga_thermal_initialize(hwmgr);
+	return 0;
+}
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.h
new file mode 100644
index 000000000000..49168d262ccc
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_hwmgr.h
@@ -0,0 +1,408 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef TONGA_HWMGR_H
+#define TONGA_HWMGR_H
+
+#include "hwmgr.h"
+#include "smu72_discrete.h"
+#include "ppatomctrl.h"
+#include "ppinterrupt.h"
+#include "tonga_powertune.h"
+
+#define TONGA_MAX_HARDWARE_POWERLEVELS 2
+#define TONGA_DYNCLK_NUMBER_OF_TREND_COEFFICIENTS 15
+
+struct tonga_performance_level {
+	uint32_t	memory_clock;
+	uint32_t	engine_clock;
+	uint16_t    pcie_gen;
+	uint16_t    pcie_lane;
+};
+
+struct _phw_tonga_bacos {
+	uint32_t                          best_match;
+	uint32_t                          baco_flags;
+	struct tonga_performance_level		  performance_level;
+};
+typedef struct _phw_tonga_bacos phw_tonga_bacos;
+
+struct _phw_tonga_uvd_clocks {
+	uint32_t   VCLK;
+	uint32_t   DCLK;
+};
+
+typedef struct _phw_tonga_uvd_clocks phw_tonga_uvd_clocks;
+
+struct _phw_tonga_vce_clocks {
+	uint32_t   EVCLK;
+	uint32_t   ECCLK;
+};
+
+typedef struct _phw_tonga_vce_clocks phw_tonga_vce_clocks;
+
+struct tonga_power_state {
+	uint32_t                    magic;
+	phw_tonga_uvd_clocks        uvd_clocks;
+	phw_tonga_vce_clocks        vce_clocks;
+	uint32_t                    sam_clk;
+	uint32_t                    acp_clk;
+	uint16_t                    performance_level_count;
+	bool                        dc_compatible;
+	uint32_t                    sclk_threshold;
+	struct tonga_performance_level performance_levels[TONGA_MAX_HARDWARE_POWERLEVELS];
+};
+
+struct _phw_tonga_dpm_level {
+	bool  		enabled;
+	uint32_t    value;
+	uint32_t    param1;
+};
+typedef struct _phw_tonga_dpm_level phw_tonga_dpm_level;
+
+#define TONGA_MAX_DEEPSLEEP_DIVIDER_ID 5
+#define MAX_REGULAR_DPM_NUMBER 8
+#define TONGA_MINIMUM_ENGINE_CLOCK 2500
+
+struct tonga_single_dpm_table {
+	uint32_t count;
+	phw_tonga_dpm_level dpm_levels[MAX_REGULAR_DPM_NUMBER];
+};
+
+struct tonga_dpm_table {
+	struct tonga_single_dpm_table  sclk_table;
+	struct tonga_single_dpm_table  mclk_table;
+	struct tonga_single_dpm_table  pcie_speed_table;
+	struct tonga_single_dpm_table  vddc_table;
+	struct tonga_single_dpm_table  vdd_gfx_table;
+	struct tonga_single_dpm_table  vdd_ci_table;
+	struct tonga_single_dpm_table  mvdd_table;
+};
+typedef struct _phw_tonga_dpm_table phw_tonga_dpm_table;
+
+
+struct _phw_tonga_clock_regisiters {
+	uint32_t  vCG_SPLL_FUNC_CNTL;
+	uint32_t  vCG_SPLL_FUNC_CNTL_2;
+	uint32_t  vCG_SPLL_FUNC_CNTL_3;
+	uint32_t  vCG_SPLL_FUNC_CNTL_4;
+	uint32_t  vCG_SPLL_SPREAD_SPECTRUM;
+	uint32_t  vCG_SPLL_SPREAD_SPECTRUM_2;
+	uint32_t  vDLL_CNTL;
+	uint32_t  vMCLK_PWRMGT_CNTL;
+	uint32_t  vMPLL_AD_FUNC_CNTL;
+	uint32_t  vMPLL_DQ_FUNC_CNTL;
+	uint32_t  vMPLL_FUNC_CNTL;
+	uint32_t  vMPLL_FUNC_CNTL_1;
+	uint32_t  vMPLL_FUNC_CNTL_2;
+	uint32_t  vMPLL_SS1;
+	uint32_t  vMPLL_SS2;
+};
+typedef struct _phw_tonga_clock_regisiters phw_tonga_clock_registers;
+
+struct _phw_tonga_voltage_smio_registers {
+	uint32_t vs0_vid_lower_smio_cntl;
+};
+typedef struct _phw_tonga_voltage_smio_registers phw_tonga_voltage_smio_registers;
+
+
+struct _phw_tonga_mc_reg_entry {
+	uint32_t mclk_max;
+	uint32_t mc_data[SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE];
+};
+typedef struct _phw_tonga_mc_reg_entry phw_tonga_mc_reg_entry;
+
+struct _phw_tonga_mc_reg_table {
+	uint8_t   last;               /* number of registers*/
+	uint8_t   num_entries;        /* number of entries in mc_reg_table_entry used*/
+	uint16_t  validflag;          /* indicate the corresponding register is valid or not. 1: valid, 0: invalid. bit0->address[0], bit1->address[1], etc.*/
+	phw_tonga_mc_reg_entry    mc_reg_table_entry[MAX_AC_TIMING_ENTRIES];
+	SMU72_Discrete_MCRegisterAddress mc_reg_address[SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE];
+};
+typedef struct _phw_tonga_mc_reg_table phw_tonga_mc_reg_table;
+
+#define DISABLE_MC_LOADMICROCODE   1
+#define DISABLE_MC_CFGPROGRAMMING  2
+
+/*Ultra Low Voltage parameter structure */
+struct _phw_tonga_ulv_parm{
+	bool	ulv_supported;
+	uint32_t   ch_ulv_parameter;
+	uint32_t   ulv_volt_change_delay;
+	struct tonga_performance_level   ulv_power_level;
+};
+typedef struct _phw_tonga_ulv_parm phw_tonga_ulv_parm;
+
+#define TONGA_MAX_LEAKAGE_COUNT  8
+
+struct _phw_tonga_leakage_voltage {
+	uint16_t  count;
+	uint16_t  leakage_id[TONGA_MAX_LEAKAGE_COUNT];
+	uint16_t  actual_voltage[TONGA_MAX_LEAKAGE_COUNT];
+};
+typedef struct _phw_tonga_leakage_voltage phw_tonga_leakage_voltage;
+
+struct _phw_tonga_display_timing {
+	uint32_t min_clock_insr;
+	uint32_t num_existing_displays;
+};
+typedef struct _phw_tonga_display_timing phw_tonga_display_timing;
+
+struct _phw_tonga_dpmlevel_enable_mask {
+	uint32_t uvd_dpm_enable_mask;
+	uint32_t vce_dpm_enable_mask;
+	uint32_t acp_dpm_enable_mask;
+	uint32_t samu_dpm_enable_mask;
+	uint32_t sclk_dpm_enable_mask;
+	uint32_t mclk_dpm_enable_mask;
+	uint32_t pcie_dpm_enable_mask;
+};
+typedef struct _phw_tonga_dpmlevel_enable_mask phw_tonga_dpmlevel_enable_mask;
+
+struct _phw_tonga_pcie_perf_range {
+	uint16_t max;
+	uint16_t min;
+};
+typedef struct _phw_tonga_pcie_perf_range phw_tonga_pcie_perf_range;
+
+struct _phw_tonga_vbios_boot_state {
+	uint16_t					mvdd_bootup_value;
+	uint16_t					vddc_bootup_value;
+	uint16_t					vddci_bootup_value;
+	uint16_t					vddgfx_bootup_value;
+	uint32_t					sclk_bootup_value;
+	uint32_t					mclk_bootup_value;
+	uint16_t					pcie_gen_bootup_value;
+	uint16_t					pcie_lane_bootup_value;
+};
+typedef struct _phw_tonga_vbios_boot_state phw_tonga_vbios_boot_state;
+
+#define DPMTABLE_OD_UPDATE_SCLK     0x00000001
+#define DPMTABLE_OD_UPDATE_MCLK     0x00000002
+#define DPMTABLE_UPDATE_SCLK        0x00000004
+#define DPMTABLE_UPDATE_MCLK        0x00000008
+
+/* We need to review which fields are needed. */
+/* This is mostly a copy of the RV7xx/Evergreen structure which is close, but not identical to the N.Islands one. */
+struct tonga_hwmgr {
+	struct tonga_dpm_table               dpm_table;
+	struct tonga_dpm_table               golden_dpm_table;
+
+	uint32_t                           voting_rights_clients0;
+	uint32_t                           voting_rights_clients1;
+	uint32_t                           voting_rights_clients2;
+	uint32_t                           voting_rights_clients3;
+	uint32_t                           voting_rights_clients4;
+	uint32_t                           voting_rights_clients5;
+	uint32_t                           voting_rights_clients6;
+	uint32_t                           voting_rights_clients7;
+	uint32_t                           static_screen_threshold_unit;
+	uint32_t                           static_screen_threshold;
+	uint32_t                           voltage_control;
+	uint32_t                           vdd_gfx_control;
+
+	uint32_t                           vddc_vddci_delta;
+	uint32_t                           vddc_vddgfx_delta;
+
+	struct pp_interrupt_registration_info    internal_high_thermal_interrupt_info;
+	struct pp_interrupt_registration_info    internal_low_thermal_interrupt_info;
+	struct pp_interrupt_registration_info    smc_to_host_interrupt_info;
+	uint32_t                          active_auto_throttle_sources;
+
+	struct pp_interrupt_registration_info    external_throttle_interrupt;
+	irq_handler_func_t             external_throttle_callback;
+	void                             *external_throttle_context;
+
+	struct pp_interrupt_registration_info    ctf_interrupt_info;
+	irq_handler_func_t             ctf_callback;
+	void                             *ctf_context;
+
+	phw_tonga_clock_registers      	  clock_registers;
+	phw_tonga_voltage_smio_registers  voltage_smio_registers;
+
+	bool                         	is_memory_GDDR5;
+	uint16_t                          acpi_vddc;
+	bool                         	pspp_notify_required;        /* Flag to indicate if PSPP notification to SBIOS is required */
+	uint16_t                          force_pcie_gen;            /* The forced PCI-E speed if not 0xffff */
+	uint16_t                          acpi_pcie_gen;             /* The PCI-E speed at ACPI time */
+	uint32_t                           pcie_gen_cap;             /* The PCI-E speed capabilities bitmap from CAIL */
+	uint32_t                           pcie_lane_cap;            /* The PCI-E lane capabilities bitmap from CAIL */
+	uint32_t                           pcie_spc_cap;             /* Symbol Per Clock Capabilities from registry */
+	phw_tonga_leakage_voltage       	vddc_leakage;            /* The Leakage VDDC supported (based on leakage ID).*/
+	phw_tonga_leakage_voltage       	vddcgfx_leakage;         /* The Leakage VDDC supported (based on leakage ID). */
+	phw_tonga_leakage_voltage       	vddci_leakage;           /* The Leakage VDDCI supported (based on leakage ID). */
+
+	uint32_t                           mvdd_control;
+	uint32_t                           vddc_mask_low;
+	uint32_t                           mvdd_mask_low;
+	uint16_t                          max_vddc_in_pp_table;        /* the maximum VDDC value in the powerplay table*/
+	uint16_t                          min_vddc_in_pp_table;
+	uint16_t                          max_vddci_in_pp_table;       /* the maximum VDDCI value in the powerplay table */
+	uint16_t                          min_vddci_in_pp_table;
+	uint32_t                           mclk_strobe_mode_threshold;
+	uint32_t                           mclk_stutter_mode_threshold;
+	uint32_t                           mclk_edc_enable_threshold;
+	uint32_t                           mclk_edc_wr_enable_threshold;
+	bool                         	is_uvd_enabled;
+	bool                         	is_xdma_enabled;
+	phw_tonga_vbios_boot_state      vbios_boot_state;
+
+	bool                         battery_state;
+	bool                         is_tlu_enabled;
+	bool                         pcie_performance_request;
+
+	/* -------------- SMC SRAM Address of firmware header tables ----------------*/
+	uint32_t                           sram_end;                 /* The first address after the SMC SRAM. */
+	uint32_t                           dpm_table_start;          /* The start of the dpm table in the SMC SRAM. */
+	uint32_t                           soft_regs_start;          /* The start of the soft registers in the SMC SRAM. */
+	uint32_t                           mc_reg_table_start;       /* The start of the mc register table in the SMC SRAM. */
+	uint32_t                           fan_table_start;          /* The start of the fan table in the SMC SRAM. */
+	uint32_t                           arb_table_start;          /* The start of the ARB setting table in the SMC SRAM. */
+	SMU72_Discrete_DpmTable         smc_state_table;             /* The carbon copy of the SMC state table. */
+	SMU72_Discrete_MCRegisters      mc_reg_table;
+	SMU72_Discrete_Ulv              ulv_setting;                 /* The carbon copy of ULV setting. */
+	/* -------------- Stuff originally coming from Evergreen --------------------*/
+	phw_tonga_mc_reg_table			tonga_mc_reg_table;
+	uint32_t                         vdd_ci_control;
+	pp_atomctrl_voltage_table        vddc_voltage_table;
+	pp_atomctrl_voltage_table        vddci_voltage_table;
+	pp_atomctrl_voltage_table        vddgfx_voltage_table;
+	pp_atomctrl_voltage_table        mvdd_voltage_table;
+
+	uint32_t                           mgcg_cgtt_local2;
+	uint32_t                           mgcg_cgtt_local3;
+	uint32_t                           gpio_debug;
+	uint32_t							mc_micro_code_feature;
+	uint32_t							highest_mclk;
+	uint16_t                          acpi_vdd_ci;
+	uint8_t                           mvdd_high_index;
+	uint8_t                           mvdd_low_index;
+	bool                         dll_defaule_on;
+	bool                         performance_request_registered;
+
+	/* ----------------- Low Power Features ---------------------*/
+	phw_tonga_bacos					bacos;
+	phw_tonga_ulv_parm              ulv;
+	/* ----------------- CAC Stuff ---------------------*/
+	uint32_t					cac_table_start;
+	bool                         cac_configuration_required;    /* TRUE if PP_CACConfigurationRequired == 1 */
+	bool                         driver_calculate_cac_leakage;  /* TRUE if PP_DriverCalculateCACLeakage == 1 */
+	bool                         cac_enabled;
+	/* ----------------- DPM2 Parameters ---------------------*/
+	uint32_t					power_containment_features;
+	bool                         enable_bapm_feature;
+	bool                         enable_tdc_limit_feature;
+	bool                         enable_pkg_pwr_tracking_feature;
+	bool                         disable_uvd_power_tune_feature;
+	phw_tonga_pt_defaults           *power_tune_defaults;
+	SMU72_Discrete_PmFuses           power_tune_table;
+	uint32_t                           ul_dte_tj_offset;             /* Fudge factor in DPM table to correct HW DTE errors */
+	uint32_t                           fast_watemark_threshold;      /* use fast watermark if clock is equal or above this. In percentage of the target high sclk. */
+
+	/* ----------------- Phase Shedding ---------------------*/
+	bool                         vddc_phase_shed_control;
+	/* --------------------- DI/DT --------------------------*/
+	phw_tonga_display_timing       display_timing;
+	/* --------- ReadRegistry data for memory and engine clock margins ---- */
+	uint32_t                           engine_clock_data;
+	uint32_t                           memory_clock_data;
+	/* -------- Thermal Temperature Setting --------------*/
+	phw_tonga_dpmlevel_enable_mask     dpm_level_enable_mask;
+	uint32_t                           need_update_smu7_dpm_table;
+	uint32_t                           sclk_dpm_key_disabled;
+	uint32_t                           mclk_dpm_key_disabled;
+	uint32_t                           pcie_dpm_key_disabled;
+	uint32_t                           min_engine_clocks; /* used to store the previous dal min sclock */
+	phw_tonga_pcie_perf_range       pcie_gen_performance;
+	phw_tonga_pcie_perf_range       pcie_lane_performance;
+	phw_tonga_pcie_perf_range       pcie_gen_power_saving;
+	phw_tonga_pcie_perf_range       pcie_lane_power_saving;
+	bool                            use_pcie_performance_levels;
+	bool                            use_pcie_power_saving_levels;
+	uint32_t                           activity_target[SMU72_MAX_LEVELS_GRAPHICS]; /* percentage value from 0-100, default 50 */
+	uint32_t                           mclk_activity_target;
+	uint32_t                           low_sclk_interrupt_threshold;
+	uint32_t                           last_mclk_dpm_enable_mask;
+	bool								uvd_enabled;
+	uint32_t                           pcc_monitor_enabled;
+
+	/* --------- Power Gating States ------------*/
+	bool                           uvd_power_gated;  /* 1: gated, 0:not gated */
+	bool                           vce_power_gated;  /* 1: gated, 0:not gated */
+	bool                           samu_power_gated; /* 1: gated, 0:not gated */
+	bool                           acp_power_gated;  /* 1: gated, 0:not gated */
+	bool                           pg_acp_init;
+
+};
+
+typedef struct tonga_hwmgr tonga_hwmgr;
+
+#define TONGA_DPM2_NEAR_TDP_DEC          10
+#define TONGA_DPM2_ABOVE_SAFE_INC        5
+#define TONGA_DPM2_BELOW_SAFE_INC        20
+
+#define TONGA_DPM2_LTA_WINDOW_SIZE       7  /* Log2 of the LTA window size (l2numWin_TDP). Eg. If LTA windows size is 128, then this value should be Log2(128) = 7. */
+
+#define TONGA_DPM2_LTS_TRUNCATE          0
+
+#define TONGA_DPM2_TDP_SAFE_LIMIT_PERCENT            80  /* Maximum 100 */
+
+#define TONGA_DPM2_MAXPS_PERCENT_H                   90  /* Maximum 0xFF */
+#define TONGA_DPM2_MAXPS_PERCENT_M                   90  /* Maximum 0xFF */
+
+#define TONGA_DPM2_PWREFFICIENCYRATIO_MARGIN         50
+
+#define TONGA_DPM2_SQ_RAMP_MAX_POWER                 0x3FFF
+#define TONGA_DPM2_SQ_RAMP_MIN_POWER                 0x12
+#define TONGA_DPM2_SQ_RAMP_MAX_POWER_DELTA           0x15
+#define TONGA_DPM2_SQ_RAMP_SHORT_TERM_INTERVAL_SIZE  0x1E
+#define TONGA_DPM2_SQ_RAMP_LONG_TERM_INTERVAL_RATIO  0xF
+
+#define TONGA_VOLTAGE_CONTROL_NONE                   0x0
+#define TONGA_VOLTAGE_CONTROL_BY_GPIO                0x1
+#define TONGA_VOLTAGE_CONTROL_BY_SVID2               0x2
+#define TONGA_VOLTAGE_CONTROL_MERGED                 0x3
+
+#define TONGA_Q88_FORMAT_CONVERSION_UNIT             256 /*To convert to Q8.8 format for firmware */
+
+#define TONGA_UNUSED_GPIO_PIN                        0x7F
+
+#define PP_HOST_TO_SMC_UL(X) cpu_to_be32(X)
+#define PP_SMC_TO_HOST_UL(X) be32_to_cpu(X)
+
+#define PP_HOST_TO_SMC_US(X) cpu_to_be16(X)
+#define PP_SMC_TO_HOST_US(X) be16_to_cpu(X)
+
+#define CONVERT_FROM_HOST_TO_SMC_UL(X) ((X) = PP_HOST_TO_SMC_UL(X))
+#define CONVERT_FROM_SMC_TO_HOST_UL(X) ((X) = PP_SMC_TO_HOST_UL(X))
+
+#define CONVERT_FROM_HOST_TO_SMC_US(X) ((X) = PP_HOST_TO_SMC_US(X))
+
+int tonga_hwmgr_init(struct pp_hwmgr *hwmgr);
+int tonga_update_vce_dpm(struct pp_hwmgr *hwmgr, const void *input);
+int tonga_update_uvd_dpm(struct pp_hwmgr *hwmgr, bool bgate);
+int tonga_enable_disable_uvd_dpm(struct pp_hwmgr *hwmgr, bool enable);
+int tonga_enable_disable_vce_dpm(struct pp_hwmgr *hwmgr, bool enable);
+uint32_t tonga_get_xclk(struct pp_hwmgr *hwmgr);
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_powertune.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_powertune.h
new file mode 100644
index 000000000000..8e6670b3cb67
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_powertune.h
@@ -0,0 +1,66 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef TONGA_POWERTUNE_H
+#define TONGA_POWERTUNE_H
+
+enum _phw_tonga_ptc_config_reg_type {
+	TONGA_CONFIGREG_MMR = 0,
+	TONGA_CONFIGREG_SMC_IND,
+	TONGA_CONFIGREG_DIDT_IND,
+	TONGA_CONFIGREG_CACHE,
+
+	TONGA_CONFIGREG_MAX
+};
+typedef enum _phw_tonga_ptc_config_reg_type phw_tonga_ptc_config_reg_type;
+
+/* PowerContainment Features */
+#define POWERCONTAINMENT_FEATURE_BAPM            0x00000001
+#define POWERCONTAINMENT_FEATURE_TDCLimit        0x00000002
+#define POWERCONTAINMENT_FEATURE_PkgPwrLimit     0x00000004
+
+struct _phw_tonga_pt_config_reg {
+	uint32_t                           Offset;
+	uint32_t                           Mask;
+	uint32_t                           Shift;
+	uint32_t                           Value;
+	phw_tonga_ptc_config_reg_type     Type;
+};
+typedef struct _phw_tonga_pt_config_reg phw_tonga_pt_config_reg;
+
+struct _phw_tonga_pt_defaults {
+	uint8_t   svi_load_line_en;
+	uint8_t   svi_load_line_vddC;
+	uint8_t   tdc_vddc_throttle_release_limit_perc;
+	uint8_t   tdc_mawt;
+	uint8_t   tdc_waterfall_ctl;
+	uint8_t   dte_ambient_temp_base;
+	uint32_t  display_cac;
+	uint32_t  bamp_temp_gradient;
+	uint16_t  bapmti_r[SMU72_DTE_ITERATIONS * SMU72_DTE_SOURCES * SMU72_DTE_SINKS];
+	uint16_t  bapmti_rc[SMU72_DTE_ITERATIONS * SMU72_DTE_SOURCES * SMU72_DTE_SINKS];
+};
+typedef struct _phw_tonga_pt_defaults phw_tonga_pt_defaults;
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_pptable.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_pptable.h
new file mode 100644
index 000000000000..9a4456e6521b
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_pptable.h
@@ -0,0 +1,406 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef TONGA_PPTABLE_H
+#define TONGA_PPTABLE_H
+
+/** \file
+ * This is a PowerPlay table header file
+ */
+#pragma pack(push, 1)
+
+#include "hwmgr.h"
+
+#define ATOM_TONGA_PP_FANPARAMETERS_TACHOMETER_PULSES_PER_REVOLUTION_MASK 0x0f
+#define ATOM_TONGA_PP_FANPARAMETERS_NOFAN                                 0x80    /* No fan is connected to this controller. */
+
+#define ATOM_TONGA_PP_THERMALCONTROLLER_NONE      0
+#define ATOM_TONGA_PP_THERMALCONTROLLER_LM96163   17
+#define ATOM_TONGA_PP_THERMALCONTROLLER_TONGA     21
+#define ATOM_TONGA_PP_THERMALCONTROLLER_FIJI      22
+
+/*
+ * Thermal controller 'combo type' to use an external controller for Fan control and an internal controller for thermal.
+ * We probably should reserve the bit 0x80 for this use.
+ * To keep the number of these types low we should also use the same code for all ASICs (i.e. do not distinguish RV6xx and RV7xx Internal here).
+ * The driver can pick the correct internal controller based on the ASIC.
+ */
+
+#define ATOM_TONGA_PP_THERMALCONTROLLER_ADT7473_WITH_INTERNAL   0x89    /* ADT7473 Fan Control + Internal Thermal Controller */
+#define ATOM_TONGA_PP_THERMALCONTROLLER_EMC2103_WITH_INTERNAL   0x8D    /* EMC2103 Fan Control + Internal Thermal Controller */
+
+/*/* ATOM_TONGA_POWERPLAYTABLE::ulPlatformCaps */
+#define ATOM_TONGA_PP_PLATFORM_CAP_VDDGFX_CONTROL              0x1            /* This cap indicates whether vddgfx will be a separated power rail. */
+#define ATOM_TONGA_PP_PLATFORM_CAP_POWERPLAY                   0x2            /* This cap indicates whether this is a mobile part and CCC need to show Powerplay page. */
+#define ATOM_TONGA_PP_PLATFORM_CAP_SBIOSPOWERSOURCE            0x4            /* This cap indicates whether power source notificaiton is done by SBIOS directly. */
+#define ATOM_TONGA_PP_PLATFORM_CAP_DISABLE_VOLTAGE_ISLAND      0x8            /* Enable the option to overwrite voltage island feature to be disabled, regardless of VddGfx power rail support. */
+#define ____RETIRE16____                                0x10
+#define ATOM_TONGA_PP_PLATFORM_CAP_HARDWAREDC                 0x20            /* This cap indicates whether power source notificaiton is done by GPIO directly. */
+#define ____RETIRE64____                                0x40
+#define ____RETIRE128____                               0x80
+#define ____RETIRE256____                              0x100
+#define ____RETIRE512____                              0x200
+#define ____RETIRE1024____                             0x400
+#define ____RETIRE2048____                             0x800
+#define ATOM_TONGA_PP_PLATFORM_CAP_MVDD_CONTROL             0x1000            /* This cap indicates dynamic MVDD is required. Uncheck to disable it. */
+#define ____RETIRE2000____                            0x2000
+#define ____RETIRE4000____                            0x4000
+#define ATOM_TONGA_PP_PLATFORM_CAP_VDDCI_CONTROL            0x8000            /* This cap indicates dynamic VDDCI is required. Uncheck to disable it. */
+#define ____RETIRE10000____                          0x10000
+#define ATOM_TONGA_PP_PLATFORM_CAP_BACO                    0x20000            /* Enable to indicate the driver supports BACO state. */
+
+#define ATOM_TONGA_PP_PLATFORM_CAP_OUTPUT_THERMAL2GPIO17         0x100000     /* Enable to indicate the driver supports thermal2GPIO17. */
+#define ATOM_TONGA_PP_PLATFORM_COMBINE_PCC_WITH_THERMAL_SIGNAL  0x1000000     /* Enable to indicate if thermal and PCC are sharing the same GPIO */
+#define ATOM_TONGA_PLATFORM_LOAD_POST_PRODUCTION_FIRMWARE       0x2000000
+
+/* ATOM_PPLIB_NONCLOCK_INFO::usClassification */
+#define ATOM_PPLIB_CLASSIFICATION_UI_MASK               0x0007
+#define ATOM_PPLIB_CLASSIFICATION_UI_SHIFT              0
+#define ATOM_PPLIB_CLASSIFICATION_UI_NONE               0
+#define ATOM_PPLIB_CLASSIFICATION_UI_BATTERY            1
+#define ATOM_PPLIB_CLASSIFICATION_UI_BALANCED           3
+#define ATOM_PPLIB_CLASSIFICATION_UI_PERFORMANCE        5
+/* 2, 4, 6, 7 are reserved */
+
+#define ATOM_PPLIB_CLASSIFICATION_BOOT                  0x0008
+#define ATOM_PPLIB_CLASSIFICATION_THERMAL               0x0010
+#define ATOM_PPLIB_CLASSIFICATION_LIMITEDPOWERSOURCE    0x0020
+#define ATOM_PPLIB_CLASSIFICATION_REST                  0x0040
+#define ATOM_PPLIB_CLASSIFICATION_FORCED                0x0080
+#define ATOM_PPLIB_CLASSIFICATION_ACPI                  0x1000
+
+/* ATOM_PPLIB_NONCLOCK_INFO::usClassification2 */
+#define ATOM_PPLIB_CLASSIFICATION2_LIMITEDPOWERSOURCE_2 0x0001
+
+#define ATOM_Tonga_DISALLOW_ON_DC                       0x00004000
+#define ATOM_Tonga_ENABLE_VARIBRIGHT                    0x00008000
+
+#define ATOM_Tonga_TABLE_REVISION_TONGA                 7
+
+typedef struct _ATOM_Tonga_POWERPLAYTABLE {
+	ATOM_COMMON_TABLE_HEADER sHeader;
+
+	UCHAR  ucTableRevision;
+	USHORT usTableSize;						/*the size of header structure */
+
+	ULONG	ulGoldenPPID;
+	ULONG	ulGoldenRevision;
+	USHORT	usFormatID;
+
+	USHORT	usVoltageTime;					 /*in microseconds */
+	ULONG	ulPlatformCaps;					  /*See ATOM_Tonga_CAPS_* */
+
+	ULONG	ulMaxODEngineClock; 			   /*For Overdrive.  */
+	ULONG	ulMaxODMemoryClock; 			   /*For Overdrive. */
+
+	USHORT	usPowerControlLimit;
+	USHORT	usUlvVoltageOffset;				  /*in mv units */
+
+	USHORT	usStateArrayOffset;				  /*points to ATOM_Tonga_State_Array */
+	USHORT	usFanTableOffset;				  /*points to ATOM_Tonga_Fan_Table */
+	USHORT	usThermalControllerOffset;		   /*points to ATOM_Tonga_Thermal_Controller */
+	USHORT	usReserv;						   /*CustomThermalPolicy removed for Tonga. Keep this filed as reserved. */
+
+	USHORT	usMclkDependencyTableOffset;	   /*points to ATOM_Tonga_MCLK_Dependency_Table */
+	USHORT	usSclkDependencyTableOffset;	   /*points to ATOM_Tonga_SCLK_Dependency_Table */
+	USHORT	usVddcLookupTableOffset;		   /*points to ATOM_Tonga_Voltage_Lookup_Table */
+	USHORT	usVddgfxLookupTableOffset; 		/*points to ATOM_Tonga_Voltage_Lookup_Table */
+
+	USHORT	usMMDependencyTableOffset;		  /*points to ATOM_Tonga_MM_Dependency_Table */
+
+	USHORT	usVCEStateTableOffset;			   /*points to ATOM_Tonga_VCE_State_Table; */
+
+	USHORT	usPPMTableOffset;				  /*points to ATOM_Tonga_PPM_Table */
+	USHORT	usPowerTuneTableOffset;			  /*points to ATOM_PowerTune_Table */
+
+	USHORT	usHardLimitTableOffset; 		   /*points to ATOM_Tonga_Hard_Limit_Table */
+
+	USHORT	usPCIETableOffset;				  /*points to ATOM_Tonga_PCIE_Table */
+
+	USHORT	usGPIOTableOffset;				  /*points to ATOM_Tonga_GPIO_Table */
+
+	USHORT	usReserved[6];					   /*TODO: modify reserved size to fit structure aligning */
+} ATOM_Tonga_POWERPLAYTABLE;
+
+typedef struct _ATOM_Tonga_State {
+	UCHAR  ucEngineClockIndexHigh;
+	UCHAR  ucEngineClockIndexLow;
+
+	UCHAR  ucMemoryClockIndexHigh;
+	UCHAR  ucMemoryClockIndexLow;
+
+	UCHAR  ucPCIEGenLow;
+	UCHAR  ucPCIEGenHigh;
+
+	UCHAR  ucPCIELaneLow;
+	UCHAR  ucPCIELaneHigh;
+
+	USHORT usClassification;
+	ULONG ulCapsAndSettings;
+	USHORT usClassification2;
+	UCHAR  ucUnused[4];
+} ATOM_Tonga_State;
+
+typedef struct _ATOM_Tonga_State_Array {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries;		/* Number of entries. */
+	ATOM_Tonga_State states[1];	/* Dynamically allocate entries. */
+} ATOM_Tonga_State_Array;
+
+typedef struct _ATOM_Tonga_MCLK_Dependency_Record {
+	UCHAR  ucVddcInd;	/* Vddc voltage */
+	USHORT usVddci;
+	USHORT usVddgfxOffset;	/* Offset relative to Vddc voltage */
+	USHORT usMvdd;
+	ULONG ulMclk;
+	USHORT usReserved;
+} ATOM_Tonga_MCLK_Dependency_Record;
+
+typedef struct _ATOM_Tonga_MCLK_Dependency_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries; 										/* Number of entries. */
+	ATOM_Tonga_MCLK_Dependency_Record entries[1];				/* Dynamically allocate entries. */
+} ATOM_Tonga_MCLK_Dependency_Table;
+
+typedef struct _ATOM_Tonga_SCLK_Dependency_Record {
+	UCHAR  ucVddInd;											/* Base voltage */
+	USHORT usVddcOffset;										/* Offset relative to base voltage */
+	ULONG ulSclk;
+	USHORT usEdcCurrent;
+	UCHAR  ucReliabilityTemperature;
+	UCHAR  ucCKSVOffsetandDisable;							  /* Bits 0~6: Voltage offset for CKS, Bit 7: Disable/enable for the SCLK level. */
+} ATOM_Tonga_SCLK_Dependency_Record;
+
+typedef struct _ATOM_Tonga_SCLK_Dependency_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries; 										/* Number of entries. */
+	ATOM_Tonga_SCLK_Dependency_Record entries[1];				 /* Dynamically allocate entries. */
+} ATOM_Tonga_SCLK_Dependency_Table;
+
+typedef struct _ATOM_Tonga_PCIE_Record {
+	UCHAR ucPCIEGenSpeed;
+	UCHAR usPCIELaneWidth;
+	UCHAR ucReserved[2];
+} ATOM_Tonga_PCIE_Record;
+
+typedef struct _ATOM_Tonga_PCIE_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries; 										/* Number of entries. */
+	ATOM_Tonga_PCIE_Record entries[1];							/* Dynamically allocate entries. */
+} ATOM_Tonga_PCIE_Table;
+
+typedef struct _ATOM_Tonga_MM_Dependency_Record {
+	UCHAR   ucVddcInd;											 /* VDDC voltage */
+	USHORT  usVddgfxOffset;									  /* Offset relative to VDDC voltage */
+	ULONG  ulDClk;												/* UVD D-clock */
+	ULONG  ulVClk;												/* UVD V-clock */
+	ULONG  ulEClk;												/* VCE clock */
+	ULONG  ulAClk;												/* ACP clock */
+	ULONG  ulSAMUClk;											/* SAMU clock */
+} ATOM_Tonga_MM_Dependency_Record;
+
+typedef struct _ATOM_Tonga_MM_Dependency_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries; 										/* Number of entries. */
+	ATOM_Tonga_MM_Dependency_Record entries[1]; 			   /* Dynamically allocate entries. */
+} ATOM_Tonga_MM_Dependency_Table;
+
+typedef struct _ATOM_Tonga_Voltage_Lookup_Record {
+	USHORT usVdd;											   /* Base voltage */
+	USHORT usCACLow;
+	USHORT usCACMid;
+	USHORT usCACHigh;
+} ATOM_Tonga_Voltage_Lookup_Record;
+
+typedef struct _ATOM_Tonga_Voltage_Lookup_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries; 										/* Number of entries. */
+	ATOM_Tonga_Voltage_Lookup_Record entries[1];				/* Dynamically allocate entries. */
+} ATOM_Tonga_Voltage_Lookup_Table;
+
+typedef struct _ATOM_Tonga_Fan_Table {
+	UCHAR   ucRevId;						 /* Change this if the table format changes or version changes so that the other fields are not the same. */
+	UCHAR   ucTHyst;						 /* Temperature hysteresis. Integer. */
+	USHORT  usTMin; 						 /* The temperature, in 0.01 centigrades, below which we just run at a minimal PWM. */
+	USHORT  usTMed; 						 /* The middle temperature where we change slopes. */
+	USHORT  usTHigh;						 /* The high point above TMed for adjusting the second slope. */
+	USHORT  usPWMMin;						 /* The minimum PWM value in percent (0.01% increments). */
+	USHORT  usPWMMed;						 /* The PWM value (in percent) at TMed. */
+	USHORT  usPWMHigh;						 /* The PWM value at THigh. */
+	USHORT  usTMax; 						 /* The max temperature */
+	UCHAR   ucFanControlMode;				  /* Legacy or Fuzzy Fan mode */
+	USHORT  usFanPWMMax;					  /* Maximum allowed fan power in percent */
+	USHORT  usFanOutputSensitivity;		  /* Sensitivity of fan reaction to temepature changes */
+	USHORT  usFanRPMMax;					  /* The default value in RPM */
+	ULONG  ulMinFanSCLKAcousticLimit;	   /* Minimum Fan Controller SCLK Frequency Acoustic Limit. */
+	UCHAR   ucTargetTemperature;			 /* Advanced fan controller target temperature. */
+	UCHAR   ucMinimumPWMLimit; 			  /* The minimum PWM that the advanced fan controller can set.	This should be set to the highest PWM that will run the fan at its lowest RPM. */
+	USHORT  usReserved;
+} ATOM_Tonga_Fan_Table;
+
+typedef struct _ATOM_Fiji_Fan_Table {
+	UCHAR   ucRevId;						 /* Change this if the table format changes or version changes so that the other fields are not the same. */
+	UCHAR   ucTHyst;						 /* Temperature hysteresis. Integer. */
+	USHORT  usTMin; 						 /* The temperature, in 0.01 centigrades, below which we just run at a minimal PWM. */
+	USHORT  usTMed; 						 /* The middle temperature where we change slopes. */
+	USHORT  usTHigh;						 /* The high point above TMed for adjusting the second slope. */
+	USHORT  usPWMMin;						 /* The minimum PWM value in percent (0.01% increments). */
+	USHORT  usPWMMed;						 /* The PWM value (in percent) at TMed. */
+	USHORT  usPWMHigh;						 /* The PWM value at THigh. */
+	USHORT  usTMax; 						 /* The max temperature */
+	UCHAR   ucFanControlMode;				  /* Legacy or Fuzzy Fan mode */
+	USHORT  usFanPWMMax;					  /* Maximum allowed fan power in percent */
+	USHORT  usFanOutputSensitivity;		  /* Sensitivity of fan reaction to temepature changes */
+	USHORT  usFanRPMMax;					  /* The default value in RPM */
+	ULONG  ulMinFanSCLKAcousticLimit;		/* Minimum Fan Controller SCLK Frequency Acoustic Limit. */
+	UCHAR   ucTargetTemperature;			 /* Advanced fan controller target temperature. */
+	UCHAR   ucMinimumPWMLimit; 			  /* The minimum PWM that the advanced fan controller can set.	This should be set to the highest PWM that will run the fan at its lowest RPM. */
+	USHORT  usFanGainEdge;
+	USHORT  usFanGainHotspot;
+	USHORT  usFanGainLiquid;
+	USHORT  usFanGainVrVddc;
+	USHORT  usFanGainVrMvdd;
+	USHORT  usFanGainPlx;
+	USHORT  usFanGainHbm;
+	USHORT  usReserved;
+} ATOM_Fiji_Fan_Table;
+
+typedef struct _ATOM_Tonga_Thermal_Controller {
+	UCHAR ucRevId;
+	UCHAR ucType;		   /* one of ATOM_TONGA_PP_THERMALCONTROLLER_* */
+	UCHAR ucI2cLine;		/* as interpreted by DAL I2C */
+	UCHAR ucI2cAddress;
+	UCHAR ucFanParameters;	/* Fan Control Parameters. */
+	UCHAR ucFanMinRPM; 	 /* Fan Minimum RPM (hundreds) -- for display purposes only. */
+	UCHAR ucFanMaxRPM; 	 /* Fan Maximum RPM (hundreds) -- for display purposes only. */
+	UCHAR ucReserved;
+	UCHAR ucFlags;		   /* to be defined */
+} ATOM_Tonga_Thermal_Controller;
+
+typedef struct _ATOM_Tonga_VCE_State_Record {
+	UCHAR  ucVCEClockIndex;	/*index into usVCEDependencyTableOffset of 'ATOM_Tonga_MM_Dependency_Table' type */
+	UCHAR  ucFlag;		/* 2 bits indicates memory p-states */
+	UCHAR  ucSCLKIndex;		/*index into ATOM_Tonga_SCLK_Dependency_Table */
+	UCHAR  ucMCLKIndex;		/*index into ATOM_Tonga_MCLK_Dependency_Table */
+} ATOM_Tonga_VCE_State_Record;
+
+typedef struct _ATOM_Tonga_VCE_State_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries;
+	ATOM_Tonga_VCE_State_Record entries[1];
+} ATOM_Tonga_VCE_State_Table;
+
+typedef struct _ATOM_Tonga_PowerTune_Table {
+	UCHAR  ucRevId;
+	USHORT usTDP;
+	USHORT usConfigurableTDP;
+	USHORT usTDC;
+	USHORT usBatteryPowerLimit;
+	USHORT usSmallPowerLimit;
+	USHORT usLowCACLeakage;
+	USHORT usHighCACLeakage;
+	USHORT usMaximumPowerDeliveryLimit;
+	USHORT usTjMax;
+	USHORT usPowerTuneDataSetID;
+	USHORT usEDCLimit;
+	USHORT usSoftwareShutdownTemp;
+	USHORT usClockStretchAmount;
+	USHORT usReserve[2];
+} ATOM_Tonga_PowerTune_Table;
+
+typedef struct _ATOM_Fiji_PowerTune_Table {
+	UCHAR  ucRevId;
+	USHORT usTDP;
+	USHORT usConfigurableTDP;
+	USHORT usTDC;
+	USHORT usBatteryPowerLimit;
+	USHORT usSmallPowerLimit;
+	USHORT usLowCACLeakage;
+	USHORT usHighCACLeakage;
+	USHORT usMaximumPowerDeliveryLimit;
+	USHORT usTjMax;  /* For Fiji, this is also usTemperatureLimitEdge; */
+	USHORT usPowerTuneDataSetID;
+	USHORT usEDCLimit;
+	USHORT usSoftwareShutdownTemp;
+	USHORT usClockStretchAmount;
+	USHORT usTemperatureLimitHotspot;  /*The following are added for Fiji */
+	USHORT usTemperatureLimitLiquid1;
+	USHORT usTemperatureLimitLiquid2;
+	USHORT usTemperatureLimitVrVddc;
+	USHORT usTemperatureLimitVrMvdd;
+	USHORT usTemperatureLimitPlx;
+	UCHAR  ucLiquid1_I2C_address;  /*Liquid */
+	UCHAR  ucLiquid2_I2C_address;
+	UCHAR  ucLiquid_I2C_Line;
+	UCHAR  ucVr_I2C_address;	/*VR */
+	UCHAR  ucVr_I2C_Line;
+	UCHAR  ucPlx_I2C_address;  /*PLX */
+	UCHAR  ucPlx_I2C_Line;
+	USHORT usReserved;
+} ATOM_Fiji_PowerTune_Table;
+
+#define ATOM_PPM_A_A    1
+#define ATOM_PPM_A_I    2
+typedef struct _ATOM_Tonga_PPM_Table {
+	UCHAR   ucRevId;
+	UCHAR   ucPpmDesign;		  /*A+I or A+A */
+	USHORT  usCpuCoreNumber;
+	ULONG  ulPlatformTDP;
+	ULONG  ulSmallACPlatformTDP;
+	ULONG  ulPlatformTDC;
+	ULONG  ulSmallACPlatformTDC;
+	ULONG  ulApuTDP;
+	ULONG  ulDGpuTDP;
+	ULONG  ulDGpuUlvPower;
+	ULONG  ulTjmax;
+} ATOM_Tonga_PPM_Table;
+
+typedef struct _ATOM_Tonga_Hard_Limit_Record {
+	ULONG  ulSCLKLimit;
+	ULONG  ulMCLKLimit;
+	USHORT  usVddcLimit;
+	USHORT  usVddciLimit;
+	USHORT  usVddgfxLimit;
+} ATOM_Tonga_Hard_Limit_Record;
+
+typedef struct _ATOM_Tonga_Hard_Limit_Table {
+	UCHAR ucRevId;
+	UCHAR ucNumEntries;
+	ATOM_Tonga_Hard_Limit_Record entries[1];
+} ATOM_Tonga_Hard_Limit_Table;
+
+typedef struct _ATOM_Tonga_GPIO_Table {
+	UCHAR  ucRevId;
+	UCHAR  ucVRHotTriggeredSclkDpmIndex;		/* If VRHot signal is triggered SCLK will be limited to this DPM level */
+	UCHAR  ucReserve[5];
+} ATOM_Tonga_GPIO_Table;
+
+typedef struct _PPTable_Generic_SubTable_Header {
+	UCHAR  ucRevId;
+} PPTable_Generic_SubTable_Header;
+
+
+#pragma pack(pop)
+
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.c b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.c
new file mode 100644
index 000000000000..34f4bef3691f
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.c
@@ -0,0 +1,1142 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/fb.h>
+
+#include "tonga_processpptables.h"
+#include "ppatomctrl.h"
+#include "atombios.h"
+#include "pp_debug.h"
+#include "hwmgr.h"
+#include "cgs_common.h"
+#include "tonga_pptable.h"
+
+/**
+ * Private Function used during initialization.
+ * @param hwmgr Pointer to the hardware manager.
+ * @param setIt A flag indication if the capability should be set (TRUE) or reset (FALSE).
+ * @param cap Which capability to set/reset.
+ */
+static void set_hw_cap(struct pp_hwmgr *hwmgr, bool setIt, enum phm_platform_caps cap)
+{
+	if (setIt)
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps, cap);
+	else
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps, cap);
+}
+
+
+/**
+ * Private Function used during initialization.
+ * @param hwmgr Pointer to the hardware manager.
+ * @param powerplay_caps the bit array (from BIOS) of capability bits.
+ * @exception the current implementation always returns 1.
+ */
+static int set_platform_caps(struct pp_hwmgr *hwmgr, uint32_t powerplay_caps)
+{
+	PP_ASSERT_WITH_CODE((~powerplay_caps & ____RETIRE16____),
+		"ATOM_PP_PLATFORM_CAP_ASPM_L1 is not supported!", continue);
+	PP_ASSERT_WITH_CODE((~powerplay_caps & ____RETIRE64____),
+		"ATOM_PP_PLATFORM_CAP_GEMINIPRIMARY is not supported!", continue);
+	PP_ASSERT_WITH_CODE((~powerplay_caps & ____RETIRE512____),
+		"ATOM_PP_PLATFORM_CAP_SIDEPORTCONTROL is not supported!", continue);
+	PP_ASSERT_WITH_CODE((~powerplay_caps & ____RETIRE1024____),
+		"ATOM_PP_PLATFORM_CAP_TURNOFFPLL_ASPML1 is not supported!", continue);
+	PP_ASSERT_WITH_CODE((~powerplay_caps & ____RETIRE2048____),
+		"ATOM_PP_PLATFORM_CAP_HTLINKCONTROL is not supported!", continue);
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_POWERPLAY),
+			PHM_PlatformCaps_PowerPlaySupport
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_SBIOSPOWERSOURCE),
+			PHM_PlatformCaps_BiosPowerSourceControl
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_HARDWAREDC),
+			PHM_PlatformCaps_AutomaticDCTransition
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_MVDD_CONTROL),
+			PHM_PlatformCaps_EnableMVDDControl
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_VDDCI_CONTROL),
+			PHM_PlatformCaps_ControlVDDCI
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_VDDGFX_CONTROL),
+			PHM_PlatformCaps_ControlVDDGFX
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_BACO),
+			PHM_PlatformCaps_BACO
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_CAP_DISABLE_VOLTAGE_ISLAND),
+			PHM_PlatformCaps_DisableVoltageIsland
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PP_PLATFORM_COMBINE_PCC_WITH_THERMAL_SIGNAL),
+			PHM_PlatformCaps_CombinePCCWithThermalSignal
+		  );
+
+	set_hw_cap(
+			hwmgr,
+			0 != (powerplay_caps & ATOM_TONGA_PLATFORM_LOAD_POST_PRODUCTION_FIRMWARE),
+			PHM_PlatformCaps_LoadPostProductionFirmware
+		  );
+
+	return 0;
+}
+
+/**
+ * Private Function to get the PowerPlay Table Address.
+ */
+const void *get_powerplay_table(struct pp_hwmgr *hwmgr)
+{
+	int index = GetIndexIntoMasterTable(DATA, PowerPlayInfo);
+
+	u16 size;
+	u8 frev, crev;
+	void *table_address;
+
+	table_address = (ATOM_Tonga_POWERPLAYTABLE *)
+		cgs_atom_get_data_table(hwmgr->device, index, &size, &frev, &crev);
+
+	hwmgr->soft_pp_table = table_address;	/*Cache the result in RAM.*/
+
+	return table_address;
+}
+
+static int get_vddc_lookup_table(
+		struct pp_hwmgr	*hwmgr,
+		phm_ppt_v1_voltage_lookup_table	**lookup_table,
+		const ATOM_Tonga_Voltage_Lookup_Table	*vddc_lookup_pp_tables,
+		uint32_t	max_levels
+		)
+{
+	uint32_t table_size, i;
+	phm_ppt_v1_voltage_lookup_table *table;
+
+	PP_ASSERT_WITH_CODE((0 != vddc_lookup_pp_tables->ucNumEntries),
+		"Invalid CAC Leakage PowerPlay Table!", return 1);
+
+	table_size = sizeof(uint32_t) +
+		sizeof(phm_ppt_v1_voltage_lookup_record) * max_levels;
+
+	table = (phm_ppt_v1_voltage_lookup_table *)
+		kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table)
+		return -ENOMEM;
+
+	memset(table, 0x00, table_size);
+
+	table->count = vddc_lookup_pp_tables->ucNumEntries;
+
+	for (i = 0; i < vddc_lookup_pp_tables->ucNumEntries; i++) {
+		table->entries[i].us_calculated = 0;
+		table->entries[i].us_vdd =
+			vddc_lookup_pp_tables->entries[i].usVdd;
+		table->entries[i].us_cac_low =
+			vddc_lookup_pp_tables->entries[i].usCACLow;
+		table->entries[i].us_cac_mid =
+			vddc_lookup_pp_tables->entries[i].usCACMid;
+		table->entries[i].us_cac_high =
+			vddc_lookup_pp_tables->entries[i].usCACHigh;
+	}
+
+	*lookup_table = table;
+
+	return 0;
+}
+
+/**
+ * Private Function used during initialization.
+ * Initialize Platform Power Management Parameter table
+ * @param hwmgr Pointer to the hardware manager.
+ * @param atom_ppm_table Pointer to PPM table in VBIOS
+ */
+static int get_platform_power_management_table(
+		struct pp_hwmgr *hwmgr,
+		ATOM_Tonga_PPM_Table *atom_ppm_table)
+{
+	struct phm_ppm_table *ptr = kzalloc(sizeof(ATOM_Tonga_PPM_Table), GFP_KERNEL);
+	struct phm_ppt_v1_information *pp_table_information =
+		(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (NULL == ptr)
+		return -ENOMEM;
+
+	ptr->ppm_design
+		= atom_ppm_table->ucPpmDesign;
+	ptr->cpu_core_number
+		= atom_ppm_table->usCpuCoreNumber;
+	ptr->platform_tdp
+		= atom_ppm_table->ulPlatformTDP;
+	ptr->small_ac_platform_tdp
+		= atom_ppm_table->ulSmallACPlatformTDP;
+	ptr->platform_tdc
+		= atom_ppm_table->ulPlatformTDC;
+	ptr->small_ac_platform_tdc
+		= atom_ppm_table->ulSmallACPlatformTDC;
+	ptr->apu_tdp
+		= atom_ppm_table->ulApuTDP;
+	ptr->dgpu_tdp
+		= atom_ppm_table->ulDGpuTDP;
+	ptr->dgpu_ulv_power
+		= atom_ppm_table->ulDGpuUlvPower;
+	ptr->tj_max
+		= atom_ppm_table->ulTjmax;
+
+	pp_table_information->ppm_parameter_table = ptr;
+
+	return 0;
+}
+
+/**
+ * Private Function used during initialization.
+ * Initialize TDP limits for DPM2
+ * @param hwmgr Pointer to the hardware manager.
+ * @param powerplay_table Pointer to the PowerPlay Table.
+ */
+static int init_dpm_2_parameters(
+		struct pp_hwmgr *hwmgr,
+		const ATOM_Tonga_POWERPLAYTABLE *powerplay_table
+		)
+{
+	int result = 0;
+	struct phm_ppt_v1_information *pp_table_information = (struct phm_ppt_v1_information *)(hwmgr->pptable);
+	ATOM_Tonga_PPM_Table *atom_ppm_table;
+	uint32_t disable_ppm = 0;
+	uint32_t disable_power_control = 0;
+
+	pp_table_information->us_ulv_voltage_offset =
+		le16_to_cpu(powerplay_table->usUlvVoltageOffset);
+
+	pp_table_information->ppm_parameter_table = NULL;
+	pp_table_information->vddc_lookup_table = NULL;
+	pp_table_information->vddgfx_lookup_table = NULL;
+	/* TDP limits */
+	hwmgr->platform_descriptor.TDPODLimit =
+		le16_to_cpu(powerplay_table->usPowerControlLimit);
+	hwmgr->platform_descriptor.TDPAdjustment = 0;
+	hwmgr->platform_descriptor.VidAdjustment = 0;
+	hwmgr->platform_descriptor.VidAdjustmentPolarity = 0;
+	hwmgr->platform_descriptor.VidMinLimit = 0;
+	hwmgr->platform_descriptor.VidMaxLimit = 1500000;
+	hwmgr->platform_descriptor.VidStep = 6250;
+
+	disable_power_control = 0;
+	if (0 == disable_power_control) {
+		/* enable TDP overdrive (PowerControl) feature as well if supported */
+		if (hwmgr->platform_descriptor.TDPODLimit != 0)
+			phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_PowerControl);
+	}
+
+	if (0 != powerplay_table->usVddcLookupTableOffset) {
+		const ATOM_Tonga_Voltage_Lookup_Table *pVddcCACTable =
+			(ATOM_Tonga_Voltage_Lookup_Table *)(((unsigned long)powerplay_table) +
+			le16_to_cpu(powerplay_table->usVddcLookupTableOffset));
+
+		result = get_vddc_lookup_table(hwmgr,
+			&pp_table_information->vddc_lookup_table, pVddcCACTable, 16);
+	}
+
+	if (0 != powerplay_table->usVddgfxLookupTableOffset) {
+		const ATOM_Tonga_Voltage_Lookup_Table *pVddgfxCACTable =
+			(ATOM_Tonga_Voltage_Lookup_Table *)(((unsigned long)powerplay_table) +
+			le16_to_cpu(powerplay_table->usVddgfxLookupTableOffset));
+
+		result = get_vddc_lookup_table(hwmgr,
+			&pp_table_information->vddgfx_lookup_table, pVddgfxCACTable, 16);
+	}
+
+	disable_ppm = 0;
+	if (0 == disable_ppm) {
+		atom_ppm_table = (ATOM_Tonga_PPM_Table *)
+			(((unsigned long)powerplay_table) + le16_to_cpu(powerplay_table->usPPMTableOffset));
+
+		if (0 != powerplay_table->usPPMTableOffset) {
+			if (1 == get_platform_power_management_table(hwmgr, atom_ppm_table)) {
+				phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+					PHM_PlatformCaps_EnablePlatformPowerManagement);
+			}
+		}
+	}
+
+	return result;
+}
+
+static int get_valid_clk(
+		struct pp_hwmgr *hwmgr,
+		struct phm_clock_array **clk_table,
+		const phm_ppt_v1_clock_voltage_dependency_table  * clk_volt_pp_table
+		)
+{
+	uint32_t table_size, i;
+	struct phm_clock_array *table;
+
+	PP_ASSERT_WITH_CODE((0 != clk_volt_pp_table->count),
+		"Invalid PowerPlay Table!", return -1);
+
+	table_size = sizeof(uint32_t) +
+		sizeof(uint32_t) * clk_volt_pp_table->count;
+
+	table = (struct phm_clock_array *)kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == table)
+		return -ENOMEM;
+
+	memset(table, 0x00, table_size);
+
+	table->count = (uint32_t)clk_volt_pp_table->count;
+
+	for (i = 0; i < table->count; i++)
+		table->values[i] = (uint32_t)clk_volt_pp_table->entries[i].clk;
+
+	*clk_table = table;
+
+	return 0;
+}
+
+static int get_hard_limits(
+		struct pp_hwmgr *hwmgr,
+		struct phm_clock_and_voltage_limits *limits,
+		const ATOM_Tonga_Hard_Limit_Table * limitable
+		)
+{
+	PP_ASSERT_WITH_CODE((0 != limitable->ucNumEntries), "Invalid PowerPlay Table!", return -1);
+
+	/* currently we always take entries[0] parameters */
+	limits->sclk = (uint32_t)limitable->entries[0].ulSCLKLimit;
+	limits->mclk = (uint32_t)limitable->entries[0].ulMCLKLimit;
+	limits->vddc = (uint16_t)limitable->entries[0].usVddcLimit;
+	limits->vddci = (uint16_t)limitable->entries[0].usVddciLimit;
+	limits->vddgfx = (uint16_t)limitable->entries[0].usVddgfxLimit;
+
+	return 0;
+}
+
+static int get_mclk_voltage_dependency_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_clock_voltage_dependency_table **pp_tonga_mclk_dep_table,
+		const ATOM_Tonga_MCLK_Dependency_Table * mclk_dep_table
+		)
+{
+	uint32_t table_size, i;
+	phm_ppt_v1_clock_voltage_dependency_table *mclk_table;
+
+	PP_ASSERT_WITH_CODE((0 != mclk_dep_table->ucNumEntries),
+		"Invalid PowerPlay Table!", return -1);
+
+	table_size = sizeof(uint32_t) + sizeof(phm_ppt_v1_clock_voltage_dependency_record)
+		* mclk_dep_table->ucNumEntries;
+
+	mclk_table = (phm_ppt_v1_clock_voltage_dependency_table *)
+		kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == mclk_table)
+		return -ENOMEM;
+
+	memset(mclk_table, 0x00, table_size);
+
+	mclk_table->count = (uint32_t)mclk_dep_table->ucNumEntries;
+
+	for (i = 0; i < mclk_dep_table->ucNumEntries; i++) {
+		mclk_table->entries[i].vddInd =
+			mclk_dep_table->entries[i].ucVddcInd;
+		mclk_table->entries[i].vdd_offset =
+			mclk_dep_table->entries[i].usVddgfxOffset;
+		mclk_table->entries[i].vddci =
+			mclk_dep_table->entries[i].usVddci;
+		mclk_table->entries[i].mvdd =
+			mclk_dep_table->entries[i].usMvdd;
+		mclk_table->entries[i].clk =
+			mclk_dep_table->entries[i].ulMclk;
+	}
+
+	*pp_tonga_mclk_dep_table = mclk_table;
+
+	return 0;
+}
+
+static int get_sclk_voltage_dependency_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_clock_voltage_dependency_table **pp_tonga_sclk_dep_table,
+		const ATOM_Tonga_SCLK_Dependency_Table * sclk_dep_table
+		)
+{
+	uint32_t table_size, i;
+	phm_ppt_v1_clock_voltage_dependency_table *sclk_table;
+
+	PP_ASSERT_WITH_CODE((0 != sclk_dep_table->ucNumEntries),
+		"Invalid PowerPlay Table!", return -1);
+
+	table_size = sizeof(uint32_t) + sizeof(phm_ppt_v1_clock_voltage_dependency_record)
+		* sclk_dep_table->ucNumEntries;
+
+	sclk_table = (phm_ppt_v1_clock_voltage_dependency_table *)
+		kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == sclk_table)
+		return -ENOMEM;
+
+	memset(sclk_table, 0x00, table_size);
+
+	sclk_table->count = (uint32_t)sclk_dep_table->ucNumEntries;
+
+	for (i = 0; i < sclk_dep_table->ucNumEntries; i++) {
+		sclk_table->entries[i].vddInd =
+			sclk_dep_table->entries[i].ucVddInd;
+		sclk_table->entries[i].vdd_offset =
+			sclk_dep_table->entries[i].usVddcOffset;
+		sclk_table->entries[i].clk =
+			sclk_dep_table->entries[i].ulSclk;
+		sclk_table->entries[i].cks_enable =
+			(((sclk_dep_table->entries[i].ucCKSVOffsetandDisable & 0x80) >> 7) == 0) ? 1 : 0;
+		sclk_table->entries[i].cks_voffset =
+			(sclk_dep_table->entries[i].ucCKSVOffsetandDisable & 0x7F);
+	}
+
+	*pp_tonga_sclk_dep_table = sclk_table;
+
+	return 0;
+}
+
+static int get_pcie_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_pcie_table **pp_tonga_pcie_table,
+		const ATOM_Tonga_PCIE_Table * atom_pcie_table
+		)
+{
+	uint32_t table_size, i, pcie_count;
+	phm_ppt_v1_pcie_table *pcie_table;
+	struct phm_ppt_v1_information *pp_table_information =
+		(struct phm_ppt_v1_information *)(hwmgr->pptable);
+	PP_ASSERT_WITH_CODE((0 != atom_pcie_table->ucNumEntries),
+		"Invalid PowerPlay Table!", return -1);
+
+	table_size = sizeof(uint32_t) +
+		sizeof(phm_ppt_v1_pcie_record) * atom_pcie_table->ucNumEntries;
+
+	pcie_table = (phm_ppt_v1_pcie_table *)kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == pcie_table)
+		return -ENOMEM;
+
+	memset(pcie_table, 0x00, table_size);
+
+	/*
+	* Make sure the number of pcie entries are less than or equal to sclk dpm levels.
+	* Since first PCIE entry is for ULV, #pcie has to be <= SclkLevel + 1.
+	*/
+	pcie_count = (pp_table_information->vdd_dep_on_sclk->count) + 1;
+	if ((uint32_t)atom_pcie_table->ucNumEntries <= pcie_count)
+		pcie_count = (uint32_t)atom_pcie_table->ucNumEntries;
+	else
+		printk(KERN_ERR "[ powerplay ] Number of Pcie Entries exceed the number of SCLK Dpm Levels! \
+		Disregarding the excess entries... \n");
+
+	pcie_table->count = pcie_count;
+
+	for (i = 0; i < pcie_count; i++) {
+		pcie_table->entries[i].gen_speed =
+			atom_pcie_table->entries[i].ucPCIEGenSpeed;
+		pcie_table->entries[i].lane_width =
+			atom_pcie_table->entries[i].usPCIELaneWidth;
+	}
+
+	*pp_tonga_pcie_table = pcie_table;
+
+	return 0;
+}
+
+static int get_cac_tdp_table(
+		struct pp_hwmgr *hwmgr,
+		struct phm_cac_tdp_table **cac_tdp_table,
+		const PPTable_Generic_SubTable_Header * table
+		)
+{
+	uint32_t table_size;
+	struct phm_cac_tdp_table *tdp_table;
+
+	table_size = sizeof(uint32_t) + sizeof(struct phm_cac_tdp_table);
+	tdp_table = kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == tdp_table)
+		return -ENOMEM;
+
+	memset(tdp_table, 0x00, table_size);
+
+	hwmgr->dyn_state.cac_dtp_table = kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == hwmgr->dyn_state.cac_dtp_table)
+		return -ENOMEM;
+
+	memset(hwmgr->dyn_state.cac_dtp_table, 0x00, table_size);
+
+	if (table->ucRevId < 3) {
+		const ATOM_Tonga_PowerTune_Table *tonga_table =
+			(ATOM_Tonga_PowerTune_Table *)table;
+		tdp_table->usTDP = tonga_table->usTDP;
+		tdp_table->usConfigurableTDP =
+			tonga_table->usConfigurableTDP;
+		tdp_table->usTDC = tonga_table->usTDC;
+		tdp_table->usBatteryPowerLimit =
+			tonga_table->usBatteryPowerLimit;
+		tdp_table->usSmallPowerLimit =
+			tonga_table->usSmallPowerLimit;
+		tdp_table->usLowCACLeakage =
+			tonga_table->usLowCACLeakage;
+		tdp_table->usHighCACLeakage =
+			tonga_table->usHighCACLeakage;
+		tdp_table->usMaximumPowerDeliveryLimit =
+			tonga_table->usMaximumPowerDeliveryLimit;
+		tdp_table->usDefaultTargetOperatingTemp =
+			tonga_table->usTjMax;
+		tdp_table->usTargetOperatingTemp =
+			tonga_table->usTjMax; /*Set the initial temp to the same as default */
+		tdp_table->usPowerTuneDataSetID =
+			tonga_table->usPowerTuneDataSetID;
+		tdp_table->usSoftwareShutdownTemp =
+			tonga_table->usSoftwareShutdownTemp;
+		tdp_table->usClockStretchAmount =
+			tonga_table->usClockStretchAmount;
+	} else {   /* Fiji and newer */
+		const ATOM_Fiji_PowerTune_Table *fijitable =
+			(ATOM_Fiji_PowerTune_Table *)table;
+		tdp_table->usTDP = fijitable->usTDP;
+		tdp_table->usConfigurableTDP = fijitable->usConfigurableTDP;
+		tdp_table->usTDC = fijitable->usTDC;
+		tdp_table->usBatteryPowerLimit = fijitable->usBatteryPowerLimit;
+		tdp_table->usSmallPowerLimit = fijitable->usSmallPowerLimit;
+		tdp_table->usLowCACLeakage = fijitable->usLowCACLeakage;
+		tdp_table->usHighCACLeakage = fijitable->usHighCACLeakage;
+		tdp_table->usMaximumPowerDeliveryLimit =
+			fijitable->usMaximumPowerDeliveryLimit;
+		tdp_table->usDefaultTargetOperatingTemp =
+			fijitable->usTjMax;
+		tdp_table->usTargetOperatingTemp =
+			fijitable->usTjMax; /*Set the initial temp to the same as default */
+		tdp_table->usPowerTuneDataSetID =
+			fijitable->usPowerTuneDataSetID;
+		tdp_table->usSoftwareShutdownTemp =
+			fijitable->usSoftwareShutdownTemp;
+		tdp_table->usClockStretchAmount =
+			fijitable->usClockStretchAmount;
+		tdp_table->usTemperatureLimitHotspot =
+			fijitable->usTemperatureLimitHotspot;
+		tdp_table->usTemperatureLimitLiquid1 =
+			fijitable->usTemperatureLimitLiquid1;
+		tdp_table->usTemperatureLimitLiquid2 =
+			fijitable->usTemperatureLimitLiquid2;
+		tdp_table->usTemperatureLimitVrVddc =
+			fijitable->usTemperatureLimitVrVddc;
+		tdp_table->usTemperatureLimitVrMvdd =
+			fijitable->usTemperatureLimitVrMvdd;
+		tdp_table->usTemperatureLimitPlx =
+			fijitable->usTemperatureLimitPlx;
+		tdp_table->ucLiquid1_I2C_address =
+			fijitable->ucLiquid1_I2C_address;
+		tdp_table->ucLiquid2_I2C_address =
+			fijitable->ucLiquid2_I2C_address;
+		tdp_table->ucLiquid_I2C_Line =
+			fijitable->ucLiquid_I2C_Line;
+		tdp_table->ucVr_I2C_address = fijitable->ucVr_I2C_address;
+		tdp_table->ucVr_I2C_Line = fijitable->ucVr_I2C_Line;
+		tdp_table->ucPlx_I2C_address = fijitable->ucPlx_I2C_address;
+		tdp_table->ucPlx_I2C_Line = fijitable->ucPlx_I2C_Line;
+	}
+
+	*cac_tdp_table = tdp_table;
+
+	return 0;
+}
+
+static int get_mm_clock_voltage_table(
+		struct pp_hwmgr *hwmgr,
+		phm_ppt_v1_mm_clock_voltage_dependency_table **tonga_mm_table,
+		const ATOM_Tonga_MM_Dependency_Table * mm_dependency_table
+		)
+{
+	uint32_t table_size, i;
+	const ATOM_Tonga_MM_Dependency_Record *mm_dependency_record;
+	phm_ppt_v1_mm_clock_voltage_dependency_table *mm_table;
+
+	PP_ASSERT_WITH_CODE((0 != mm_dependency_table->ucNumEntries),
+		"Invalid PowerPlay Table!", return -1);
+	table_size = sizeof(uint32_t) +
+		sizeof(phm_ppt_v1_mm_clock_voltage_dependency_record)
+		* mm_dependency_table->ucNumEntries;
+	mm_table = (phm_ppt_v1_mm_clock_voltage_dependency_table *)
+		kzalloc(table_size, GFP_KERNEL);
+
+	if (NULL == mm_table)
+		return -ENOMEM;
+
+	memset(mm_table, 0x00, table_size);
+
+	mm_table->count = mm_dependency_table->ucNumEntries;
+
+	for (i = 0; i < mm_dependency_table->ucNumEntries; i++) {
+		mm_dependency_record = &mm_dependency_table->entries[i];
+		mm_table->entries[i].vddcInd = mm_dependency_record->ucVddcInd;
+		mm_table->entries[i].vddgfx_offset = mm_dependency_record->usVddgfxOffset;
+		mm_table->entries[i].aclk = mm_dependency_record->ulAClk;
+		mm_table->entries[i].samclock = mm_dependency_record->ulSAMUClk;
+		mm_table->entries[i].eclk = mm_dependency_record->ulEClk;
+		mm_table->entries[i].vclk = mm_dependency_record->ulVClk;
+		mm_table->entries[i].dclk = mm_dependency_record->ulDClk;
+	}
+
+	*tonga_mm_table = mm_table;
+
+	return 0;
+}
+
+/**
+ * Private Function used during initialization.
+ * Initialize clock voltage dependency
+ * @param hwmgr Pointer to the hardware manager.
+ * @param powerplay_table Pointer to the PowerPlay Table.
+ */
+static int init_clock_voltage_dependency(
+		struct pp_hwmgr *hwmgr,
+		const ATOM_Tonga_POWERPLAYTABLE *powerplay_table
+		)
+{
+	int result = 0;
+	struct phm_ppt_v1_information *pp_table_information =
+		(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	const ATOM_Tonga_MM_Dependency_Table *mm_dependency_table =
+		(const ATOM_Tonga_MM_Dependency_Table *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usMMDependencyTableOffset));
+	const PPTable_Generic_SubTable_Header *pPowerTuneTable =
+		(const PPTable_Generic_SubTable_Header *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usPowerTuneTableOffset));
+	const ATOM_Tonga_MCLK_Dependency_Table *mclk_dep_table =
+		(const ATOM_Tonga_MCLK_Dependency_Table *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usMclkDependencyTableOffset));
+	const ATOM_Tonga_SCLK_Dependency_Table *sclk_dep_table =
+		(const ATOM_Tonga_SCLK_Dependency_Table *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usSclkDependencyTableOffset));
+	const ATOM_Tonga_Hard_Limit_Table *pHardLimits =
+		(const ATOM_Tonga_Hard_Limit_Table *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usHardLimitTableOffset));
+	const ATOM_Tonga_PCIE_Table *pcie_table =
+		(const ATOM_Tonga_PCIE_Table *)(((unsigned long) powerplay_table) +
+		le16_to_cpu(powerplay_table->usPCIETableOffset));
+
+	pp_table_information->vdd_dep_on_sclk = NULL;
+	pp_table_information->vdd_dep_on_mclk = NULL;
+	pp_table_information->mm_dep_table = NULL;
+	pp_table_information->pcie_table = NULL;
+
+	if (powerplay_table->usMMDependencyTableOffset != 0)
+		result = get_mm_clock_voltage_table(hwmgr,
+		&pp_table_information->mm_dep_table, mm_dependency_table);
+
+	if (result == 0 && powerplay_table->usPowerTuneTableOffset != 0)
+		result = get_cac_tdp_table(hwmgr,
+		&pp_table_information->cac_dtp_table, pPowerTuneTable);
+
+	if (result == 0 && powerplay_table->usSclkDependencyTableOffset != 0)
+		result = get_sclk_voltage_dependency_table(hwmgr,
+		&pp_table_information->vdd_dep_on_sclk, sclk_dep_table);
+
+	if (result == 0 && powerplay_table->usMclkDependencyTableOffset != 0)
+		result = get_mclk_voltage_dependency_table(hwmgr,
+		&pp_table_information->vdd_dep_on_mclk, mclk_dep_table);
+
+	if (result == 0 && powerplay_table->usPCIETableOffset != 0)
+		result = get_pcie_table(hwmgr,
+		&pp_table_information->pcie_table, pcie_table);
+
+	if (result == 0 && powerplay_table->usHardLimitTableOffset != 0)
+		result = get_hard_limits(hwmgr,
+		&pp_table_information->max_clock_voltage_on_dc, pHardLimits);
+
+	hwmgr->dyn_state.max_clock_voltage_on_dc.sclk =
+		pp_table_information->max_clock_voltage_on_dc.sclk;
+	hwmgr->dyn_state.max_clock_voltage_on_dc.mclk =
+		pp_table_information->max_clock_voltage_on_dc.mclk;
+	hwmgr->dyn_state.max_clock_voltage_on_dc.vddc =
+		pp_table_information->max_clock_voltage_on_dc.vddc;
+	hwmgr->dyn_state.max_clock_voltage_on_dc.vddci =
+		pp_table_information->max_clock_voltage_on_dc.vddci;
+
+	if (result == 0 && (NULL != pp_table_information->vdd_dep_on_mclk)
+		&& (0 != pp_table_information->vdd_dep_on_mclk->count))
+		result = get_valid_clk(hwmgr, &pp_table_information->valid_mclk_values,
+		pp_table_information->vdd_dep_on_mclk);
+
+	if (result == 0 && (NULL != pp_table_information->vdd_dep_on_sclk)
+		&& (0 != pp_table_information->vdd_dep_on_sclk->count))
+		result = get_valid_clk(hwmgr, &pp_table_information->valid_sclk_values,
+		pp_table_information->vdd_dep_on_sclk);
+
+	return result;
+}
+
+/** Retrieves the (signed) Overdrive limits from VBIOS.
+ * The max engine clock, memory clock and max temperature come from the firmware info table.
+ *
+ * The information is placed into the platform descriptor.
+ *
+ * @param hwmgr source of the VBIOS table and owner of the platform descriptor to be updated.
+ * @param powerplay_table the address of the PowerPlay table.
+ *
+ * @return 1 as long as the firmware info table was present and of a supported version.
+ */
+static int init_over_drive_limits(
+		struct pp_hwmgr *hwmgr,
+		const ATOM_Tonga_POWERPLAYTABLE *powerplay_table)
+{
+	hwmgr->platform_descriptor.overdriveLimit.engineClock =
+		le16_to_cpu(powerplay_table->ulMaxODEngineClock);
+	hwmgr->platform_descriptor.overdriveLimit.memoryClock =
+		le16_to_cpu(powerplay_table->ulMaxODMemoryClock);
+
+	hwmgr->platform_descriptor.minOverdriveVDDC = 0;
+	hwmgr->platform_descriptor.maxOverdriveVDDC = 0;
+	hwmgr->platform_descriptor.overdriveVDDCStep = 0;
+
+	if (hwmgr->platform_descriptor.overdriveLimit.engineClock > 0 \
+		&& hwmgr->platform_descriptor.overdriveLimit.memoryClock > 0) {
+		phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+			PHM_PlatformCaps_ACOverdriveSupport);
+	}
+
+	return 0;
+}
+
+/**
+ * Private Function used during initialization.
+ * Inspect the PowerPlay table for obvious signs of corruption.
+ * @param hwmgr Pointer to the hardware manager.
+ * @param powerplay_table Pointer to the PowerPlay Table.
+ * @exception This implementation always returns 1.
+ */
+static int init_thermal_controller(
+		struct pp_hwmgr *hwmgr,
+		const ATOM_Tonga_POWERPLAYTABLE *powerplay_table
+		)
+{
+	const PPTable_Generic_SubTable_Header *fan_table;
+	ATOM_Tonga_Thermal_Controller *thermal_controller;
+
+	thermal_controller = (ATOM_Tonga_Thermal_Controller *)
+		(((unsigned long)powerplay_table) +
+		le16_to_cpu(powerplay_table->usThermalControllerOffset));
+	PP_ASSERT_WITH_CODE((0 != powerplay_table->usThermalControllerOffset),
+		"Thermal controller table not set!", return -1);
+
+	hwmgr->thermal_controller.ucType = thermal_controller->ucType;
+	hwmgr->thermal_controller.ucI2cLine = thermal_controller->ucI2cLine;
+	hwmgr->thermal_controller.ucI2cAddress = thermal_controller->ucI2cAddress;
+
+	hwmgr->thermal_controller.fanInfo.bNoFan =
+		(0 != (thermal_controller->ucFanParameters & ATOM_TONGA_PP_FANPARAMETERS_NOFAN));
+
+	hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution =
+		thermal_controller->ucFanParameters &
+		ATOM_TONGA_PP_FANPARAMETERS_TACHOMETER_PULSES_PER_REVOLUTION_MASK;
+
+	hwmgr->thermal_controller.fanInfo.ulMinRPM
+		= thermal_controller->ucFanMinRPM * 100UL;
+	hwmgr->thermal_controller.fanInfo.ulMaxRPM
+		= thermal_controller->ucFanMaxRPM * 100UL;
+
+	set_hw_cap(
+			hwmgr,
+			ATOM_TONGA_PP_THERMALCONTROLLER_NONE != hwmgr->thermal_controller.ucType,
+			PHM_PlatformCaps_ThermalController
+		  );
+
+	if (0 == powerplay_table->usFanTableOffset)
+		return 0;
+
+	fan_table = (const PPTable_Generic_SubTable_Header *)
+		(((unsigned long)powerplay_table) +
+		le16_to_cpu(powerplay_table->usFanTableOffset));
+
+	PP_ASSERT_WITH_CODE((0 != powerplay_table->usFanTableOffset),
+		"Fan table not set!", return -1);
+	PP_ASSERT_WITH_CODE((0 < fan_table->ucRevId),
+		"Unsupported fan table format!", return -1);
+
+	hwmgr->thermal_controller.advanceFanControlParameters.ulCycleDelay
+		= 100000;
+	phm_cap_set(hwmgr->platform_descriptor.platformCaps,
+		PHM_PlatformCaps_MicrocodeFanControl);
+
+	if (fan_table->ucRevId < 8) {
+		const ATOM_Tonga_Fan_Table *tonga_fan_table =
+			(ATOM_Tonga_Fan_Table *)fan_table;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucTHyst
+			= tonga_fan_table->ucTHyst;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMin
+			= tonga_fan_table->usTMin;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMed
+			= tonga_fan_table->usTMed;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTHigh
+			= tonga_fan_table->usTHigh;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMMin
+			= tonga_fan_table->usPWMMin;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed
+			= tonga_fan_table->usPWMMed;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMHigh
+			= tonga_fan_table->usPWMHigh;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMax
+			= 10900;                  /* hard coded */
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMax
+			= tonga_fan_table->usTMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucFanControlMode
+			= tonga_fan_table->ucFanControlMode;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanPWM
+			= tonga_fan_table->usFanPWMMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultFanOutputSensitivity
+			= 4836;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanOutputSensitivity
+			= tonga_fan_table->usFanOutputSensitivity;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanRPM
+			= tonga_fan_table->usFanRPMMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.ulMinFanSCLKAcousticLimit
+			= (tonga_fan_table->ulMinFanSCLKAcousticLimit / 100); /* PPTable stores it in 10Khz unit for 2 decimal places.  SMC wants MHz. */
+		hwmgr->thermal_controller.advanceFanControlParameters.ucTargetTemperature
+			= tonga_fan_table->ucTargetTemperature;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucMinimumPWMLimit
+			= tonga_fan_table->ucMinimumPWMLimit;
+	} else {
+		const ATOM_Fiji_Fan_Table *fiji_fan_table =
+			(ATOM_Fiji_Fan_Table *)fan_table;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucTHyst
+			= fiji_fan_table->ucTHyst;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMin
+			= fiji_fan_table->usTMin;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMed
+			= fiji_fan_table->usTMed;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTHigh
+			= fiji_fan_table->usTHigh;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMMin
+			= fiji_fan_table->usPWMMin;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed
+			= fiji_fan_table->usPWMMed;
+		hwmgr->thermal_controller.advanceFanControlParameters.usPWMHigh
+			= fiji_fan_table->usPWMHigh;
+		hwmgr->thermal_controller.advanceFanControlParameters.usTMax
+			= fiji_fan_table->usTMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucFanControlMode
+			= fiji_fan_table->ucFanControlMode;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanPWM
+			= fiji_fan_table->usFanPWMMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultFanOutputSensitivity
+			= 4836;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanOutputSensitivity
+			= fiji_fan_table->usFanOutputSensitivity;
+		hwmgr->thermal_controller.advanceFanControlParameters.usDefaultMaxFanRPM
+			= fiji_fan_table->usFanRPMMax;
+		hwmgr->thermal_controller.advanceFanControlParameters.ulMinFanSCLKAcousticLimit
+			= (fiji_fan_table->ulMinFanSCLKAcousticLimit / 100); /* PPTable stores it in 10Khz unit for 2 decimal places.  SMC wants MHz. */
+		hwmgr->thermal_controller.advanceFanControlParameters.ucTargetTemperature
+			= fiji_fan_table->ucTargetTemperature;
+		hwmgr->thermal_controller.advanceFanControlParameters.ucMinimumPWMLimit
+			= fiji_fan_table->ucMinimumPWMLimit;
+
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainEdge
+			= fiji_fan_table->usFanGainEdge;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainHotspot
+			= fiji_fan_table->usFanGainHotspot;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainLiquid
+			= fiji_fan_table->usFanGainLiquid;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainVrVddc
+			= fiji_fan_table->usFanGainVrVddc;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainVrMvdd
+			= fiji_fan_table->usFanGainVrMvdd;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainPlx
+			= fiji_fan_table->usFanGainPlx;
+		hwmgr->thermal_controller.advanceFanControlParameters.usFanGainHbm
+			= fiji_fan_table->usFanGainHbm;
+	}
+
+	return 0;
+}
+
+/**
+ * Private Function used during initialization.
+ * Inspect the PowerPlay table for obvious signs of corruption.
+ * @param hwmgr Pointer to the hardware manager.
+ * @param powerplay_table Pointer to the PowerPlay Table.
+ * @exception 2 if the powerplay table is incorrect.
+ */
+static int check_powerplay_tables(
+		struct pp_hwmgr *hwmgr,
+		const ATOM_Tonga_POWERPLAYTABLE *powerplay_table
+		)
+{
+	const ATOM_Tonga_State_Array *state_arrays;
+
+	state_arrays = (ATOM_Tonga_State_Array *)(((unsigned long)powerplay_table) +
+		le16_to_cpu(powerplay_table->usStateArrayOffset));
+
+	PP_ASSERT_WITH_CODE((ATOM_Tonga_TABLE_REVISION_TONGA <=
+		powerplay_table->sHeader.ucTableFormatRevision),
+		"Unsupported PPTable format!", return -1);
+	PP_ASSERT_WITH_CODE((0 != powerplay_table->usStateArrayOffset),
+		"State table is not set!", return -1);
+	PP_ASSERT_WITH_CODE((0 < powerplay_table->sHeader.usStructureSize),
+		"Invalid PowerPlay Table!", return -1);
+	PP_ASSERT_WITH_CODE((0 < state_arrays->ucNumEntries),
+		"Invalid PowerPlay Table!", return -1);
+
+	return 0;
+}
+
+int tonga_pp_tables_initialize(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+	const ATOM_Tonga_POWERPLAYTABLE *powerplay_table;
+
+	hwmgr->pptable = kzalloc(sizeof(struct phm_ppt_v1_information), GFP_KERNEL);
+
+	PP_ASSERT_WITH_CODE((NULL != hwmgr->pptable),
+			    "Failed to allocate hwmgr->pptable!", return -ENOMEM);
+
+	memset(hwmgr->pptable, 0x00, sizeof(struct phm_ppt_v1_information));
+
+	powerplay_table = get_powerplay_table(hwmgr);
+
+	PP_ASSERT_WITH_CODE((NULL != powerplay_table),
+		"Missing PowerPlay Table!", return -1);
+
+	result = check_powerplay_tables(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "check_powerplay_tables failed", return result);
+
+	result = set_platform_caps(hwmgr,
+				   le32_to_cpu(powerplay_table->ulPlatformCaps));
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "set_platform_caps failed", return result);
+
+	result = init_thermal_controller(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_thermal_controller failed", return result);
+
+	result = init_over_drive_limits(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_over_drive_limits failed", return result);
+
+	result = init_clock_voltage_dependency(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_clock_voltage_dependency failed", return result);
+
+	result = init_dpm_2_parameters(hwmgr, powerplay_table);
+
+	PP_ASSERT_WITH_CODE((result == 0),
+			    "init_dpm_2_parameters failed", return result);
+
+	return result;
+}
+
+int tonga_pp_tables_uninitialize(struct pp_hwmgr *hwmgr)
+{
+	int result = 0;
+	struct phm_ppt_v1_information *pp_table_information =
+		(struct phm_ppt_v1_information *)(hwmgr->pptable);
+
+	if (NULL != hwmgr->soft_pp_table) {
+		kfree(hwmgr->soft_pp_table);
+		hwmgr->soft_pp_table = NULL;
+	}
+
+	if (NULL != pp_table_information->vdd_dep_on_sclk)
+		pp_table_information->vdd_dep_on_sclk = NULL;
+
+	if (NULL != pp_table_information->vdd_dep_on_mclk)
+		pp_table_information->vdd_dep_on_mclk = NULL;
+
+	if (NULL != pp_table_information->valid_mclk_values)
+		pp_table_information->valid_mclk_values = NULL;
+
+	if (NULL != pp_table_information->valid_sclk_values)
+		pp_table_information->valid_sclk_values = NULL;
+
+	if (NULL != pp_table_information->vddc_lookup_table)
+		pp_table_information->vddc_lookup_table = NULL;
+
+	if (NULL != pp_table_information->vddgfx_lookup_table)
+		pp_table_information->vddgfx_lookup_table = NULL;
+
+	if (NULL != pp_table_information->mm_dep_table)
+		pp_table_information->mm_dep_table = NULL;
+
+	if (NULL != pp_table_information->cac_dtp_table)
+		pp_table_information->cac_dtp_table = NULL;
+
+	if (NULL != hwmgr->dyn_state.cac_dtp_table)
+		hwmgr->dyn_state.cac_dtp_table = NULL;
+
+	if (NULL != pp_table_information->ppm_parameter_table)
+		pp_table_information->ppm_parameter_table = NULL;
+
+	if (NULL != pp_table_information->pcie_table)
+		pp_table_information->pcie_table = NULL;
+
+	if (NULL != hwmgr->pptable) {
+		kfree(hwmgr->pptable);
+		hwmgr->pptable = NULL;
+	}
+
+	return result;
+}
+
+const struct pp_table_func tonga_pptable_funcs = {
+	.pptable_init = tonga_pp_tables_initialize,
+	.pptable_fini = tonga_pp_tables_uninitialize,
+};
+
+int tonga_get_number_of_powerplay_table_entries(struct pp_hwmgr *hwmgr)
+{
+	const ATOM_Tonga_State_Array * state_arrays;
+	const ATOM_Tonga_POWERPLAYTABLE *pp_table = get_powerplay_table(hwmgr);
+
+	PP_ASSERT_WITH_CODE((NULL != pp_table),
+			"Missing PowerPlay Table!", return -1);
+	PP_ASSERT_WITH_CODE((pp_table->sHeader.ucTableFormatRevision >=
+			ATOM_Tonga_TABLE_REVISION_TONGA),
+			"Incorrect PowerPlay table revision!", return -1);
+
+	state_arrays = (ATOM_Tonga_State_Array *)(((unsigned long)pp_table) +
+			le16_to_cpu(pp_table->usStateArrayOffset));
+
+	return (uint32_t)(state_arrays->ucNumEntries);
+}
+
+/**
+* Private function to convert flags stored in the BIOS to software flags in PowerPlay.
+*/
+static uint32_t make_classification_flags(struct pp_hwmgr *hwmgr,
+		uint16_t classification, uint16_t classification2)
+{
+	uint32_t result = 0;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_BOOT)
+		result |= PP_StateClassificationFlag_Boot;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_THERMAL)
+		result |= PP_StateClassificationFlag_Thermal;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_LIMITEDPOWERSOURCE)
+		result |= PP_StateClassificationFlag_LimitedPowerSource;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_REST)
+		result |= PP_StateClassificationFlag_Rest;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_FORCED)
+		result |= PP_StateClassificationFlag_Forced;
+
+	if (classification & ATOM_PPLIB_CLASSIFICATION_ACPI)
+		result |= PP_StateClassificationFlag_ACPI;
+
+	if (classification2 & ATOM_PPLIB_CLASSIFICATION2_LIMITEDPOWERSOURCE_2)
+		result |= PP_StateClassificationFlag_LimitedPowerSource_2;
+
+	return result;
+}
+
+/**
+* Create a Power State out of an entry in the PowerPlay table.
+* This function is called by the hardware back-end.
+* @param hwmgr Pointer to the hardware manager.
+* @param entry_index The index of the entry to be extracted from the table.
+* @param power_state The address of the PowerState instance being created.
+* @return -1 if the entry cannot be retrieved.
+*/
+int tonga_get_powerplay_table_entry(struct pp_hwmgr *hwmgr,
+		uint32_t entry_index, struct pp_power_state *power_state,
+		int (*call_back_func)(struct pp_hwmgr *, void *,
+				struct pp_power_state *, void *, uint32_t))
+{
+	int result = 0;
+	const ATOM_Tonga_State_Array * state_arrays;
+	const ATOM_Tonga_State *state_entry;
+	const ATOM_Tonga_POWERPLAYTABLE *pp_table = get_powerplay_table(hwmgr);
+
+	PP_ASSERT_WITH_CODE((NULL != pp_table), "Missing PowerPlay Table!", return -1;);
+	power_state->classification.bios_index = entry_index;
+
+	if (pp_table->sHeader.ucTableFormatRevision >=
+			ATOM_Tonga_TABLE_REVISION_TONGA) {
+		state_arrays = (ATOM_Tonga_State_Array *)(((unsigned long)pp_table) +
+				le16_to_cpu(pp_table->usStateArrayOffset));
+
+		PP_ASSERT_WITH_CODE((0 < pp_table->usStateArrayOffset),
+				"Invalid PowerPlay Table State Array Offset.", return -1);
+		PP_ASSERT_WITH_CODE((0 < state_arrays->ucNumEntries),
+				"Invalid PowerPlay Table State Array.", return -1);
+		PP_ASSERT_WITH_CODE((entry_index <= state_arrays->ucNumEntries),
+				"Invalid PowerPlay Table State Array Entry.", return -1);
+
+		state_entry = &(state_arrays->states[entry_index]);
+
+		result = call_back_func(hwmgr, (void *)state_entry, power_state,
+				(void *)pp_table,
+				make_classification_flags(hwmgr,
+					le16_to_cpu(state_entry->usClassification),
+					le16_to_cpu(state_entry->usClassification2)));
+	}
+
+	if (!result && (power_state->classification.flags &
+			PP_StateClassificationFlag_Boot))
+		result = hwmgr->hwmgr_func->patch_boot_state(hwmgr, &(power_state->hardware));
+
+	return result;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.h
new file mode 100644
index 000000000000..d24b8887f466
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_processpptables.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef TONGA_PROCESSPPTABLES_H
+#define TONGA_PROCESSPPTABLES_H
+
+#include "hwmgr.h"
+
+extern const struct pp_table_func tonga_pptable_funcs;
+extern int tonga_get_number_of_powerplay_table_entries(struct pp_hwmgr *hwmgr);
+extern int tonga_get_powerplay_table_entry(struct pp_hwmgr *hwmgr, uint32_t entry_index,
+		struct pp_power_state *power_state, int (*call_back_func)(struct pp_hwmgr *, void *,
+				struct pp_power_state *, void *, uint32_t));
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.c b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.c
new file mode 100644
index 000000000000..a188174747c9
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.c
@@ -0,0 +1,590 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <asm/div64.h>
+#include "tonga_thermal.h"
+#include "tonga_hwmgr.h"
+#include "tonga_smumgr.h"
+#include "tonga_ppsmc.h"
+#include "smu/smu_7_1_2_d.h"
+#include "smu/smu_7_1_2_sh_mask.h"
+
+/**
+* Get Fan Speed Control Parameters.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pSpeed is the address of the structure where the result is to be placed.
+* @exception Always succeeds except if we cannot zero out the output structure.
+*/
+int tonga_fan_ctrl_get_fan_speed_info(struct pp_hwmgr *hwmgr, struct phm_fan_speed_info *fan_speed_info)
+{
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	fan_speed_info->supports_percent_read = true;
+	fan_speed_info->supports_percent_write = true;
+	fan_speed_info->min_percent = 0;
+	fan_speed_info->max_percent = 100;
+
+	if (0 != hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution) {
+		fan_speed_info->supports_rpm_read = true;
+		fan_speed_info->supports_rpm_write = true;
+		fan_speed_info->min_rpm = hwmgr->thermal_controller.fanInfo.ulMinRPM;
+		fan_speed_info->max_rpm = hwmgr->thermal_controller.fanInfo.ulMaxRPM;
+	} else {
+		fan_speed_info->min_rpm = 0;
+		fan_speed_info->max_rpm = 0;
+	}
+
+	return 0;
+}
+
+/**
+* Get Fan Speed in percent.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pSpeed is the address of the structure where the result is to be placed.
+* @exception Fails is the 100% setting appears to be 0.
+*/
+int tonga_fan_ctrl_get_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t *speed)
+{
+	uint32_t duty100;
+	uint32_t duty;
+	uint64_t tmp64;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL1, FMAX_DUTY100);
+	duty = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_STATUS, FDO_PWM_DUTY);
+
+	if (0 == duty100)
+		return -EINVAL;
+
+
+	tmp64 = (uint64_t)duty * 100;
+	do_div(tmp64, duty100);
+	*speed = (uint32_t)tmp64;
+
+	if (*speed > 100)
+		*speed = 100;
+
+	return 0;
+}
+
+/**
+* Get Fan Speed in RPM.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    speed is the address of the structure where the result is to be placed.
+* @exception Returns not supported if no fan is found or if pulses per revolution are not set
+*/
+int tonga_fan_ctrl_get_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t *speed)
+{
+	return 0;
+}
+
+/**
+* Set Fan Speed Control to static mode, so that the user can decide what speed to use.
+* @param    hwmgr  the address of the powerplay hardware manager.
+*           mode    the fan control mode, 0 default, 1 by percent, 5, by RPM
+* @exception Should always succeed.
+*/
+int tonga_fan_ctrl_set_static_mode(struct pp_hwmgr *hwmgr, uint32_t mode)
+{
+
+	if (hwmgr->fan_ctrl_is_in_default_mode) {
+		hwmgr->fan_ctrl_default_mode = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, FDO_PWM_MODE);
+		hwmgr->tmin = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, TMIN);
+		hwmgr->fan_ctrl_is_in_default_mode = false;
+	}
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, TMIN, 0);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, FDO_PWM_MODE, mode);
+
+	return 0;
+}
+
+/**
+* Reset Fan Speed Control to default mode.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @exception Should always succeed.
+*/
+int tonga_fan_ctrl_set_default_mode(struct pp_hwmgr *hwmgr)
+{
+	if (!hwmgr->fan_ctrl_is_in_default_mode) {
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, FDO_PWM_MODE, hwmgr->fan_ctrl_default_mode);
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, TMIN, hwmgr->tmin);
+		hwmgr->fan_ctrl_is_in_default_mode = true;
+	}
+
+	return 0;
+}
+
+int tonga_fan_ctrl_start_smc_fan_control(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_ODFuzzyFanControlSupport)) {
+		cgs_write_register(hwmgr->device, mmSMC_MSG_ARG_0, FAN_CONTROL_FUZZY);
+		result = (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StartFanControl) == 0) ?  0 : -EINVAL;
+/*
+		if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_FanSpeedInTableIsRPM))
+			hwmgr->set_max_fan_rpm_output(hwmgr, hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanRPM);
+		else
+			hwmgr->set_max_fan_pwm_output(hwmgr, hwmgr->thermal_controller.advanceFanControlParameters.usMaxFanPWM);
+*/
+	} else {
+		cgs_write_register(hwmgr->device, mmSMC_MSG_ARG_0, FAN_CONTROL_TABLE);
+		result = (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StartFanControl) == 0) ?  0 : -EINVAL;
+	}
+/* TO DO FOR SOME DEVICE ID 0X692b, send this msg return invalid command.
+	if (result == 0 && hwmgr->thermal_controller.advanceFanControlParameters.ucTargetTemperature != 0)
+		result = (0 == smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_SetFanTemperatureTarget, \
+								hwmgr->thermal_controller.advanceFanControlParameters.ucTargetTemperature) ? 0 : -EINVAL);
+*/
+	return result;
+}
+
+
+int tonga_fan_ctrl_stop_smc_fan_control(struct pp_hwmgr *hwmgr)
+{
+	return (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_StopFanControl) == 0) ?  0 : -EINVAL;
+}
+
+/**
+* Set Fan Speed in percent.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    speed is the percentage value (0% - 100%) to be set.
+* @exception Fails is the 100% setting appears to be 0.
+*/
+int tonga_fan_ctrl_set_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t speed)
+{
+	uint32_t duty100;
+	uint32_t duty;
+	uint64_t tmp64;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return -EINVAL;
+
+	if (speed > 100)
+		speed = 100;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl))
+		tonga_fan_ctrl_stop_smc_fan_control(hwmgr);
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL1, FMAX_DUTY100);
+
+	if (0 == duty100)
+		return -EINVAL;
+
+	tmp64 = (uint64_t)speed * 100;
+	do_div(tmp64, duty100);
+	duty = (uint32_t)tmp64;
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL0, FDO_STATIC_DUTY, duty);
+
+	return tonga_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+}
+
+/**
+* Reset Fan Speed to default.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @exception Always succeeds.
+*/
+int tonga_fan_ctrl_reset_fan_speed_to_default(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		return 0;
+
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl)) {
+		result = tonga_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+		if (0 == result)
+			result = tonga_fan_ctrl_start_smc_fan_control(hwmgr);
+	} else
+		result = tonga_fan_ctrl_set_default_mode(hwmgr);
+
+	return result;
+}
+
+/**
+* Set Fan Speed in RPM.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    speed is the percentage value (min - max) to be set.
+* @exception Fails is the speed not lie between min and max.
+*/
+int tonga_fan_ctrl_set_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t speed)
+{
+	return 0;
+}
+
+/**
+* Reads the remote temperature from the SIslands thermal controller.
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+int tonga_thermal_get_temperature(struct pp_hwmgr *hwmgr)
+{
+	int temp;
+
+	temp = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_MULT_THERMAL_STATUS, CTF_TEMP);
+
+/* Bit 9 means the reading is lower than the lowest usable value. */
+	if (0 != (0x200 & temp))
+		temp = TONGA_THERMAL_MAXIMUM_TEMP_READING;
+	else
+		temp = (temp & 0x1ff);
+
+	temp = temp * PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+
+	return temp;
+}
+
+/**
+* Set the requested temperature range for high and low alert signals
+*
+* @param    hwmgr The address of the hardware manager.
+* @param    range Temperature range to be programmed for high and low alert signals
+* @exception PP_Result_BadInput if the input data is not valid.
+*/
+static int tonga_thermal_set_temperature_range(struct pp_hwmgr *hwmgr, uint32_t low_temp, uint32_t high_temp)
+{
+	uint32_t low = TONGA_THERMAL_MINIMUM_ALERT_TEMP * PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+	uint32_t high = TONGA_THERMAL_MAXIMUM_ALERT_TEMP * PP_TEMPERATURE_UNITS_PER_CENTIGRADES;
+
+	if (low < low_temp)
+		low = low_temp;
+	if (high > high_temp)
+		high = high_temp;
+
+	if (low > high)
+		return -EINVAL;
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, DIG_THERM_INTH, (high / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, DIG_THERM_INTL, (low / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_CTRL, DIG_THERM_DPM, (high / PP_TEMPERATURE_UNITS_PER_CENTIGRADES));
+
+	return 0;
+}
+
+/**
+* Programs thermal controller one-time setting registers
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+static int tonga_thermal_initialize(struct pp_hwmgr *hwmgr)
+{
+	if (0 != hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution)
+		PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC,
+						CG_TACH_CTRL, EDGE_PER_REV,
+						hwmgr->thermal_controller.fanInfo.ucTachometerPulsesPerRevolution - 1);
+
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL2, TACH_PWM_RESP_RATE, 0x28);
+
+	return 0;
+}
+
+/**
+* Enable thermal alerts on the RV770 thermal controller.
+*
+* @param    hwmgr The address of the hardware manager.
+*/
+static int tonga_thermal_enable_alert(struct pp_hwmgr *hwmgr)
+{
+	uint32_t alert;
+
+	alert = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, THERM_INT_MASK);
+	alert &= ~(TONGA_THERMAL_HIGH_ALERT_MASK | TONGA_THERMAL_LOW_ALERT_MASK);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, THERM_INT_MASK, alert);
+
+	/* send message to SMU to enable internal thermal interrupts */
+	return (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_Thermal_Cntl_Enable) == 0) ? 0 : -1;
+}
+
+/**
+* Disable thermal alerts on the RV770 thermal controller.
+* @param    hwmgr The address of the hardware manager.
+*/
+static int tonga_thermal_disable_alert(struct pp_hwmgr *hwmgr)
+{
+	uint32_t alert;
+
+	alert = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, THERM_INT_MASK);
+	alert |= (TONGA_THERMAL_HIGH_ALERT_MASK | TONGA_THERMAL_LOW_ALERT_MASK);
+	PHM_WRITE_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_THERMAL_INT, THERM_INT_MASK, alert);
+
+	/* send message to SMU to disable internal thermal interrupts */
+	return (smum_send_msg_to_smc(hwmgr->smumgr, PPSMC_MSG_Thermal_Cntl_Disable) == 0) ? 0 : -1;
+}
+
+/**
+* Uninitialize the thermal controller.
+* Currently just disables alerts.
+* @param    hwmgr The address of the hardware manager.
+*/
+int tonga_thermal_stop_thermal_controller(struct pp_hwmgr *hwmgr)
+{
+	int result = tonga_thermal_disable_alert(hwmgr);
+
+	if (hwmgr->thermal_controller.fanInfo.bNoFan)
+		tonga_fan_ctrl_set_default_mode(hwmgr);
+
+	return result;
+}
+
+/**
+* Set up the fan table to control the fan using the SMC.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_tonga_thermal_setup_fan_table(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+	struct tonga_hwmgr *data = (struct tonga_hwmgr *)(hwmgr->backend);
+	SMU72_Discrete_FanTable fan_table = { FDO_MODE_HARDWARE };
+	uint32_t duty100;
+	uint32_t t_diff1, t_diff2, pwm_diff1, pwm_diff2;
+	uint16_t fdo_min, slope1, slope2;
+	uint32_t reference_clock;
+	int res;
+	uint64_t tmp64;
+
+	if (!phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl))
+		return 0;
+
+	if (0 == data->fan_table_start) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl);
+		return 0;
+	}
+
+	duty100 = PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_FDO_CTRL1, FMAX_DUTY100);
+
+	if (0 == duty100) {
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl);
+		return 0;
+	}
+
+	tmp64 = hwmgr->thermal_controller.advanceFanControlParameters.usPWMMin * duty100;
+	do_div(tmp64, 10000);
+	fdo_min = (uint16_t)tmp64;
+
+	t_diff1 = hwmgr->thermal_controller.advanceFanControlParameters.usTMed - hwmgr->thermal_controller.advanceFanControlParameters.usTMin;
+	t_diff2 = hwmgr->thermal_controller.advanceFanControlParameters.usTHigh - hwmgr->thermal_controller.advanceFanControlParameters.usTMed;
+
+	pwm_diff1 = hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed - hwmgr->thermal_controller.advanceFanControlParameters.usPWMMin;
+	pwm_diff2 = hwmgr->thermal_controller.advanceFanControlParameters.usPWMHigh - hwmgr->thermal_controller.advanceFanControlParameters.usPWMMed;
+
+	slope1 = (uint16_t)((50 + ((16 * duty100 * pwm_diff1) / t_diff1)) / 100);
+	slope2 = (uint16_t)((50 + ((16 * duty100 * pwm_diff2) / t_diff2)) / 100);
+
+	fan_table.TempMin = cpu_to_be16((50 + hwmgr->thermal_controller.advanceFanControlParameters.usTMin) / 100);
+	fan_table.TempMed = cpu_to_be16((50 + hwmgr->thermal_controller.advanceFanControlParameters.usTMed) / 100);
+	fan_table.TempMax = cpu_to_be16((50 + hwmgr->thermal_controller.advanceFanControlParameters.usTMax) / 100);
+
+	fan_table.Slope1 = cpu_to_be16(slope1);
+	fan_table.Slope2 = cpu_to_be16(slope2);
+
+	fan_table.FdoMin = cpu_to_be16(fdo_min);
+
+	fan_table.HystDown = cpu_to_be16(hwmgr->thermal_controller.advanceFanControlParameters.ucTHyst);
+
+	fan_table.HystUp = cpu_to_be16(1);
+
+	fan_table.HystSlope = cpu_to_be16(1);
+
+	fan_table.TempRespLim = cpu_to_be16(5);
+
+	reference_clock = tonga_get_xclk(hwmgr);
+
+	fan_table.RefreshPeriod = cpu_to_be32((hwmgr->thermal_controller.advanceFanControlParameters.ulCycleDelay * reference_clock) / 1600);
+
+	fan_table.FdoMax = cpu_to_be16((uint16_t)duty100);
+
+	fan_table.TempSrc = (uint8_t)PHM_READ_VFPF_INDIRECT_FIELD(hwmgr->device, CGS_IND_REG__SMC, CG_MULT_THERMAL_CTRL, TEMP_SEL);
+
+	fan_table.FanControl_GL_Flag = 1;
+
+	res = tonga_copy_bytes_to_smc(hwmgr->smumgr, data->fan_table_start, (uint8_t *)&fan_table, (uint32_t)sizeof(fan_table), data->sram_end);
+/* TO DO FOR SOME DEVICE ID 0X692b, send this msg return invalid command.
+	if (res == 0 && hwmgr->thermal_controller.advanceFanControlParameters.ucMinimumPWMLimit != 0)
+		res = (0 == smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_SetFanMinPwm, \
+						hwmgr->thermal_controller.advanceFanControlParameters.ucMinimumPWMLimit) ? 0 : -1);
+
+	if (res == 0 && hwmgr->thermal_controller.advanceFanControlParameters.ulMinFanSCLKAcousticLimit != 0)
+		res = (0 == smum_send_msg_to_smc_with_parameter(hwmgr->smumgr, PPSMC_MSG_SetFanSclkTarget, \
+					hwmgr->thermal_controller.advanceFanControlParameters.ulMinFanSCLKAcousticLimit) ? 0 : -1);
+
+	if (0 != res)
+		phm_cap_unset(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl);
+*/
+	return 0;
+}
+
+/**
+* Start the fan control on the SMC.
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_tonga_thermal_start_smc_fan_control(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+/* If the fantable setup has failed we could have disabled PHM_PlatformCaps_MicrocodeFanControl even after this function was included in the table.
+ * Make sure that we still think controlling the fan is OK.
+*/
+	if (phm_cap_enabled(hwmgr->platform_descriptor.platformCaps, PHM_PlatformCaps_MicrocodeFanControl)) {
+		tonga_fan_ctrl_start_smc_fan_control(hwmgr);
+		tonga_fan_ctrl_set_static_mode(hwmgr, FDO_PWM_MODE_STATIC);
+	}
+
+	return 0;
+}
+
+/**
+* Set temperature range for high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from set temperature range routine
+*/
+int tf_tonga_thermal_set_temperature_range(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+	struct PP_TemperatureRange *range = (struct PP_TemperatureRange *)input;
+
+	if (range == NULL)
+		return -EINVAL;
+
+	return tonga_thermal_set_temperature_range(hwmgr, range->min, range->max);
+}
+
+/**
+* Programs one-time setting registers
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from initialize thermal controller routine
+*/
+int tf_tonga_thermal_initialize(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+    return tonga_thermal_initialize(hwmgr);
+}
+
+/**
+* Enable high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from enable alert routine
+*/
+int tf_tonga_thermal_enable_alert(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+	return tonga_thermal_enable_alert(hwmgr);
+}
+
+/**
+* Disable high and low alerts
+* @param    hwmgr  the address of the powerplay hardware manager.
+* @param    pInput the pointer to input data
+* @param    pOutput the pointer to output data
+* @param    pStorage the pointer to temporary storage
+* @param    Result the last failure code
+* @return   result from disable alert routine
+*/
+static int tf_tonga_thermal_disable_alert(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result)
+{
+	return tonga_thermal_disable_alert(hwmgr);
+}
+
+static struct phm_master_table_item tonga_thermal_start_thermal_controller_master_list[] = {
+	{ NULL, tf_tonga_thermal_initialize },
+	{ NULL, tf_tonga_thermal_set_temperature_range },
+	{ NULL, tf_tonga_thermal_enable_alert },
+/* We should restrict performance levels to low before we halt the SMC.
+ * On the other hand we are still in boot state when we do this so it would be pointless.
+ * If this assumption changes we have to revisit this table.
+ */
+	{ NULL, tf_tonga_thermal_setup_fan_table},
+	{ NULL, tf_tonga_thermal_start_smc_fan_control},
+	{ NULL, NULL }
+};
+
+static struct phm_master_table_header tonga_thermal_start_thermal_controller_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	tonga_thermal_start_thermal_controller_master_list
+};
+
+static struct phm_master_table_item tonga_thermal_set_temperature_range_master_list[] = {
+	{ NULL, tf_tonga_thermal_disable_alert},
+	{ NULL, tf_tonga_thermal_set_temperature_range},
+	{ NULL, tf_tonga_thermal_enable_alert},
+	{ NULL, NULL }
+};
+
+struct phm_master_table_header tonga_thermal_set_temperature_range_master = {
+	0,
+	PHM_MasterTableFlag_None,
+	tonga_thermal_set_temperature_range_master_list
+};
+
+int tonga_thermal_ctrl_uninitialize_thermal_controller(struct pp_hwmgr *hwmgr)
+{
+	if (!hwmgr->thermal_controller.fanInfo.bNoFan)
+		tonga_fan_ctrl_set_default_mode(hwmgr);
+	return 0;
+}
+
+/**
+* Initializes the thermal controller related functions in the Hardware Manager structure.
+* @param    hwmgr The address of the hardware manager.
+* @exception Any error code from the low-level communication.
+*/
+int pp_tonga_thermal_initialize(struct pp_hwmgr *hwmgr)
+{
+	int result;
+
+	result = phm_construct_table(hwmgr, &tonga_thermal_set_temperature_range_master, &(hwmgr->set_temperature_range));
+
+	if (0 == result) {
+		result = phm_construct_table(hwmgr,
+						&tonga_thermal_start_thermal_controller_master,
+						&(hwmgr->start_thermal_controller));
+		if (0 != result)
+			phm_destroy_table(hwmgr, &(hwmgr->set_temperature_range));
+	}
+
+	if (0 == result)
+		hwmgr->fan_ctrl_is_in_default_mode = true;
+	return result;
+}
+
diff --git a/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.h b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.h
new file mode 100644
index 000000000000..aa335f267e25
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/hwmgr/tonga_thermal.h
@@ -0,0 +1,61 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef TONGA_THERMAL_H
+#define TONGA_THERMAL_H
+
+#include "hwmgr.h"
+
+#define TONGA_THERMAL_HIGH_ALERT_MASK         0x1
+#define TONGA_THERMAL_LOW_ALERT_MASK          0x2
+
+#define TONGA_THERMAL_MINIMUM_TEMP_READING    -256
+#define TONGA_THERMAL_MAXIMUM_TEMP_READING    255
+
+#define TONGA_THERMAL_MINIMUM_ALERT_TEMP      0
+#define TONGA_THERMAL_MAXIMUM_ALERT_TEMP      255
+
+#define FDO_PWM_MODE_STATIC  1
+#define FDO_PWM_MODE_STATIC_RPM 5
+
+
+extern int tf_tonga_thermal_initialize(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+extern int tf_tonga_thermal_set_temperature_range(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+extern int tf_tonga_thermal_enable_alert(struct pp_hwmgr *hwmgr, void *input, void *output, void *storage, int result);
+
+extern int tonga_thermal_get_temperature(struct pp_hwmgr *hwmgr);
+extern int tonga_thermal_stop_thermal_controller(struct pp_hwmgr *hwmgr);
+extern int tonga_fan_ctrl_get_fan_speed_info(struct pp_hwmgr *hwmgr, struct phm_fan_speed_info *fan_speed_info);
+extern int tonga_fan_ctrl_get_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t *speed);
+extern int tonga_fan_ctrl_set_default_mode(struct pp_hwmgr *hwmgr);
+extern int tonga_fan_ctrl_set_static_mode(struct pp_hwmgr *hwmgr, uint32_t mode);
+extern int tonga_fan_ctrl_set_fan_speed_percent(struct pp_hwmgr *hwmgr, uint32_t speed);
+extern int tonga_fan_ctrl_reset_fan_speed_to_default(struct pp_hwmgr *hwmgr);
+extern int pp_tonga_thermal_initialize(struct pp_hwmgr *hwmgr);
+extern int tonga_thermal_ctrl_uninitialize_thermal_controller(struct pp_hwmgr *hwmgr);
+extern int tonga_fan_ctrl_set_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t speed);
+extern int tonga_fan_ctrl_get_fan_speed_rpm(struct pp_hwmgr *hwmgr, uint32_t *speed);
+extern int tonga_fan_ctrl_stop_smc_fan_control(struct pp_hwmgr *hwmgr);
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/inc/amd_powerplay.h b/drivers/gpu/drm/amd/powerplay/inc/amd_powerplay.h
new file mode 100644
index 000000000000..e61a3e67852e
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/amd_powerplay.h
@@ -0,0 +1,299 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _AMD_POWERPLAY_H_
+#define _AMD_POWERPLAY_H_
+
+#include <linux/seq_file.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include "amd_shared.h"
+#include "cgs_common.h"
+
+enum amd_pp_event {
+	AMD_PP_EVENT_INITIALIZE = 0,
+	AMD_PP_EVENT_UNINITIALIZE,
+	AMD_PP_EVENT_POWER_SOURCE_CHANGE,
+	AMD_PP_EVENT_SUSPEND,
+	AMD_PP_EVENT_RESUME,
+	AMD_PP_EVENT_ENTER_REST_STATE,
+	AMD_PP_EVENT_EXIT_REST_STATE,
+	AMD_PP_EVENT_DISPLAY_CONFIG_CHANGE,
+	AMD_PP_EVENT_THERMAL_NOTIFICATION,
+	AMD_PP_EVENT_VBIOS_NOTIFICATION,
+	AMD_PP_EVENT_ENTER_THERMAL_STATE,
+	AMD_PP_EVENT_EXIT_THERMAL_STATE,
+	AMD_PP_EVENT_ENTER_FORCED_STATE,
+	AMD_PP_EVENT_EXIT_FORCED_STATE,
+	AMD_PP_EVENT_ENTER_EXCLUSIVE_MODE,
+	AMD_PP_EVENT_EXIT_EXCLUSIVE_MODE,
+	AMD_PP_EVENT_ENTER_SCREEN_SAVER,
+	AMD_PP_EVENT_EXIT_SCREEN_SAVER,
+	AMD_PP_EVENT_VPU_RECOVERY_BEGIN,
+	AMD_PP_EVENT_VPU_RECOVERY_END,
+	AMD_PP_EVENT_ENABLE_POWER_PLAY,
+	AMD_PP_EVENT_DISABLE_POWER_PLAY,
+	AMD_PP_EVENT_CHANGE_POWER_SOURCE_UI_LABEL,
+	AMD_PP_EVENT_ENABLE_USER2D_PERFORMANCE,
+	AMD_PP_EVENT_DISABLE_USER2D_PERFORMANCE,
+	AMD_PP_EVENT_ENABLE_USER3D_PERFORMANCE,
+	AMD_PP_EVENT_DISABLE_USER3D_PERFORMANCE,
+	AMD_PP_EVENT_ENABLE_OVER_DRIVE_TEST,
+	AMD_PP_EVENT_DISABLE_OVER_DRIVE_TEST,
+	AMD_PP_EVENT_ENABLE_REDUCED_REFRESH_RATE,
+	AMD_PP_EVENT_DISABLE_REDUCED_REFRESH_RATE,
+	AMD_PP_EVENT_ENABLE_GFX_CLOCK_GATING,
+	AMD_PP_EVENT_DISABLE_GFX_CLOCK_GATING,
+	AMD_PP_EVENT_ENABLE_CGPG,
+	AMD_PP_EVENT_DISABLE_CGPG,
+	AMD_PP_EVENT_ENTER_TEXT_MODE,
+	AMD_PP_EVENT_EXIT_TEXT_MODE,
+	AMD_PP_EVENT_VIDEO_START,
+	AMD_PP_EVENT_VIDEO_STOP,
+	AMD_PP_EVENT_ENABLE_USER_STATE,
+	AMD_PP_EVENT_DISABLE_USER_STATE,
+	AMD_PP_EVENT_READJUST_POWER_STATE,
+	AMD_PP_EVENT_START_INACTIVITY,
+	AMD_PP_EVENT_STOP_INACTIVITY,
+	AMD_PP_EVENT_LINKED_ADAPTERS_READY,
+	AMD_PP_EVENT_ADAPTER_SAFE_TO_DISABLE,
+	AMD_PP_EVENT_COMPLETE_INIT,
+	AMD_PP_EVENT_CRITICAL_THERMAL_FAULT,
+	AMD_PP_EVENT_BACKLIGHT_CHANGED,
+	AMD_PP_EVENT_ENABLE_VARI_BRIGHT,
+	AMD_PP_EVENT_DISABLE_VARI_BRIGHT,
+	AMD_PP_EVENT_ENABLE_VARI_BRIGHT_ON_POWER_XPRESS,
+	AMD_PP_EVENT_DISABLE_VARI_BRIGHT_ON_POWER_XPRESS,
+	AMD_PP_EVENT_SET_VARI_BRIGHT_LEVEL,
+	AMD_PP_EVENT_VARI_BRIGHT_MONITOR_MEASUREMENT,
+	AMD_PP_EVENT_SCREEN_ON,
+	AMD_PP_EVENT_SCREEN_OFF,
+	AMD_PP_EVENT_PRE_DISPLAY_CONFIG_CHANGE,
+	AMD_PP_EVENT_ENTER_ULP_STATE,
+	AMD_PP_EVENT_EXIT_ULP_STATE,
+	AMD_PP_EVENT_REGISTER_IP_STATE,
+	AMD_PP_EVENT_UNREGISTER_IP_STATE,
+	AMD_PP_EVENT_ENTER_MGPU_MODE,
+	AMD_PP_EVENT_EXIT_MGPU_MODE,
+	AMD_PP_EVENT_ENTER_MULTI_GPU_MODE,
+	AMD_PP_EVENT_PRE_SUSPEND,
+	AMD_PP_EVENT_PRE_RESUME,
+	AMD_PP_EVENT_ENTER_BACOS,
+	AMD_PP_EVENT_EXIT_BACOS,
+	AMD_PP_EVENT_RESUME_BACO,
+	AMD_PP_EVENT_RESET_BACO,
+	AMD_PP_EVENT_PRE_DISPLAY_PHY_ACCESS,
+	AMD_PP_EVENT_POST_DISPLAY_PHY_CCESS,
+	AMD_PP_EVENT_START_COMPUTE_APPLICATION,
+	AMD_PP_EVENT_STOP_COMPUTE_APPLICATION,
+	AMD_PP_EVENT_REDUCE_POWER_LIMIT,
+	AMD_PP_EVENT_ENTER_FRAME_LOCK,
+	AMD_PP_EVENT_EXIT_FRAME_LOOCK,
+	AMD_PP_EVENT_LONG_IDLE_REQUEST_BACO,
+	AMD_PP_EVENT_LONG_IDLE_ENTER_BACO,
+	AMD_PP_EVENT_LONG_IDLE_EXIT_BACO,
+	AMD_PP_EVENT_HIBERNATE,
+	AMD_PP_EVENT_CONNECTED_STANDBY,
+	AMD_PP_EVENT_ENTER_SELF_REFRESH,
+	AMD_PP_EVENT_EXIT_SELF_REFRESH,
+	AMD_PP_EVENT_START_AVFS_BTC,
+	AMD_PP_EVENT_MAX
+};
+
+enum amd_dpm_forced_level {
+	AMD_DPM_FORCED_LEVEL_AUTO = 0,
+	AMD_DPM_FORCED_LEVEL_LOW = 1,
+	AMD_DPM_FORCED_LEVEL_HIGH = 2,
+};
+
+struct amd_pp_init {
+	struct cgs_device *device;
+	uint32_t chip_family;
+	uint32_t chip_id;
+	uint32_t rev_id;
+};
+enum amd_pp_display_config_type{
+	AMD_PP_DisplayConfigType_None = 0,
+	AMD_PP_DisplayConfigType_DP54 ,
+	AMD_PP_DisplayConfigType_DP432 ,
+	AMD_PP_DisplayConfigType_DP324 ,
+	AMD_PP_DisplayConfigType_DP27,
+	AMD_PP_DisplayConfigType_DP243,
+	AMD_PP_DisplayConfigType_DP216,
+	AMD_PP_DisplayConfigType_DP162,
+	AMD_PP_DisplayConfigType_HDMI6G ,
+	AMD_PP_DisplayConfigType_HDMI297 ,
+	AMD_PP_DisplayConfigType_HDMI162,
+	AMD_PP_DisplayConfigType_LVDS,
+	AMD_PP_DisplayConfigType_DVI,
+	AMD_PP_DisplayConfigType_WIRELESS,
+	AMD_PP_DisplayConfigType_VGA
+};
+
+struct single_display_configuration
+{
+	uint32_t controller_index;
+	uint32_t controller_id;
+	uint32_t signal_type;
+	uint32_t display_state;
+	/* phy id for the primary internal transmitter */
+	uint8_t primary_transmitter_phyi_d;
+	/* bitmap with the active lanes */
+	uint8_t primary_transmitter_active_lanemap;
+	/* phy id for the secondary internal transmitter (for dual-link dvi) */
+	uint8_t secondary_transmitter_phy_id;
+	/* bitmap with the active lanes */
+	uint8_t secondary_transmitter_active_lanemap;
+	/* misc phy settings for SMU. */
+	uint32_t config_flags;
+	uint32_t display_type;
+	uint32_t view_resolution_cx;
+	uint32_t view_resolution_cy;
+	enum amd_pp_display_config_type displayconfigtype;
+	uint32_t vertical_refresh; /* for active display */
+};
+
+#define MAX_NUM_DISPLAY 32
+
+struct amd_pp_display_configuration {
+	bool nb_pstate_switch_disable;/* controls NB PState switch */
+	bool cpu_cc6_disable; /* controls CPU CState switch ( on or off) */
+	bool cpu_pstate_disable;
+	uint32_t cpu_pstate_separation_time;
+
+	uint32_t num_display;  /* total number of display*/
+	uint32_t num_path_including_non_display;
+	uint32_t crossfire_display_index;
+	uint32_t min_mem_set_clock;
+	uint32_t min_core_set_clock;
+	/* unit 10KHz x bit*/
+	uint32_t min_bus_bandwidth;
+	/* minimum required stutter sclk, in 10khz uint32_t ulMinCoreSetClk;*/
+	uint32_t min_core_set_clock_in_sr;
+
+	struct single_display_configuration displays[MAX_NUM_DISPLAY];
+
+	uint32_t vrefresh; /* for active display*/
+
+	uint32_t min_vblank_time; /* for active display*/
+	bool multi_monitor_in_sync;
+	/* Controller Index of primary display - used in MCLK SMC switching hang
+	 * SW Workaround*/
+	uint32_t crtc_index;
+	/* htotal*1000/pixelclk - used in MCLK SMC switching hang SW Workaround*/
+	uint32_t line_time_in_us;
+	bool invalid_vblank_time;
+
+	uint32_t display_clk;
+	/*
+	 * for given display configuration if multimonitormnsync == false then
+	 * Memory clock DPMS with this latency or below is allowed, DPMS with
+	 * higher latency not allowed.
+	 */
+	uint32_t dce_tolerable_mclk_in_active_latency;
+};
+
+struct amd_pp_dal_clock_info {
+	uint32_t	engine_max_clock;
+	uint32_t	memory_max_clock;
+	uint32_t	level;
+};
+
+enum {
+	PP_GROUP_UNKNOWN = 0,
+	PP_GROUP_GFX = 1,
+	PP_GROUP_SYS,
+	PP_GROUP_MAX
+};
+
+#define PP_GROUP_MASK        0xF0000000
+#define PP_GROUP_SHIFT       28
+
+#define PP_BLOCK_MASK        0x0FFFFF00
+#define PP_BLOCK_SHIFT       8
+
+#define PP_BLOCK_GFX_CG         0x01
+#define PP_BLOCK_GFX_MG         0x02
+#define PP_BLOCK_SYS_BIF        0x01
+#define PP_BLOCK_SYS_MC         0x02
+#define PP_BLOCK_SYS_ROM        0x04
+#define PP_BLOCK_SYS_DRM        0x08
+#define PP_BLOCK_SYS_HDP        0x10
+#define PP_BLOCK_SYS_SDMA       0x20
+
+#define PP_STATE_MASK           0x0000000F
+#define PP_STATE_SHIFT          0
+#define PP_STATE_SUPPORT_MASK   0x000000F0
+#define PP_STATE_SUPPORT_SHIFT  0
+
+#define PP_STATE_CG             0x01
+#define PP_STATE_LS             0x02
+#define PP_STATE_DS             0x04
+#define PP_STATE_SD             0x08
+#define PP_STATE_SUPPORT_CG     0x10
+#define PP_STATE_SUPPORT_LS     0x20
+#define PP_STATE_SUPPORT_DS     0x40
+#define PP_STATE_SUPPORT_SD     0x80
+
+#define PP_CG_MSG_ID(group, block, support, state) (group << PP_GROUP_SHIFT |\
+								block << PP_BLOCK_SHIFT |\
+								support << PP_STATE_SUPPORT_SHIFT |\
+								state << PP_STATE_SHIFT)
+
+struct amd_powerplay_funcs {
+	int (*get_temperature)(void *handle);
+	int (*load_firmware)(void *handle);
+	int (*wait_for_fw_loading_complete)(void *handle);
+	int (*force_performance_level)(void *handle, enum amd_dpm_forced_level level);
+	enum amd_dpm_forced_level (*get_performance_level)(void *handle);
+	enum amd_pm_state_type (*get_current_power_state)(void *handle);
+	int (*get_sclk)(void *handle, bool low);
+	int (*get_mclk)(void *handle, bool low);
+	int (*powergate_vce)(void *handle, bool gate);
+	int (*powergate_uvd)(void *handle, bool gate);
+	int (*dispatch_tasks)(void *handle, enum amd_pp_event event_id,
+				   void *input, void *output);
+	void (*print_current_performance_level)(void *handle,
+						      struct seq_file *m);
+	int (*set_fan_control_mode)(void *handle, uint32_t mode);
+	int (*get_fan_control_mode)(void *handle);
+	int (*set_fan_speed_percent)(void *handle, uint32_t percent);
+	int (*get_fan_speed_percent)(void *handle, uint32_t *speed);
+};
+
+struct amd_powerplay {
+	void *pp_handle;
+	const struct amd_ip_funcs *ip_funcs;
+	const struct amd_powerplay_funcs *pp_funcs;
+};
+
+int amd_powerplay_init(struct amd_pp_init *pp_init,
+		       struct amd_powerplay *amd_pp);
+int amd_powerplay_fini(void *handle);
+
+int amd_powerplay_display_configuration_change(void *handle, const void *input);
+
+int amd_powerplay_get_display_power_level(void *handle,
+		struct amd_pp_dal_clock_info *output);
+
+
+#endif /* _AMD_POWERPLAY_H_ */
diff --git a/drivers/gpu/drm/amd/amdgpu/cz_ppsmc.h b/drivers/gpu/drm/amd/powerplay/inc/cz_ppsmc.h
index 273616ab43db..9b698780aed8 100644
--- a/drivers/gpu/drm/amd/amdgpu/cz_ppsmc.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/cz_ppsmc.h
@@ -164,6 +164,7 @@ enum DPM_ARRAY {
 #define PPSMC_MSG_SetLoggerAddressHigh        ((uint16_t) 0x26C)
 #define PPSMC_MSG_SetLoggerAddressLow         ((uint16_t) 0x26D)
 #define PPSMC_MSG_SetWatermarkFrequency       ((uint16_t) 0x26E)
+#define PPSMC_MSG_SetDisplaySizePowerParams   ((uint16_t) 0x26F)
 
 /* REMOVE LATER*/
 #define PPSMC_MSG_DPM_ForceState              ((uint16_t) 0x104)
diff --git a/drivers/gpu/drm/amd/powerplay/inc/eventmanager.h b/drivers/gpu/drm/amd/powerplay/inc/eventmanager.h
new file mode 100644
index 000000000000..b9d84de8a44d
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/eventmanager.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _EVENT_MANAGER_H_
+#define _EVENT_MANAGER_H_
+
+#include "power_state.h"
+#include "pp_power_source.h"
+#include "hardwaremanager.h"
+#include "pp_asicblocks.h"
+
+struct pp_eventmgr;
+enum amd_pp_event;
+
+enum PEM_EventDataValid {
+	PEM_EventDataValid_RequestedStateID = 0,
+	PEM_EventDataValid_RequestedUILabel,
+	PEM_EventDataValid_NewPowerState,
+	PEM_EventDataValid_RequestedPowerSource,
+	PEM_EventDataValid_RequestedClocks,
+	PEM_EventDataValid_CurrentTemperature,
+	PEM_EventDataValid_AsicBlocks,
+	PEM_EventDataValid_ODParameters,
+	PEM_EventDataValid_PXAdapterPrefs,
+	PEM_EventDataValid_PXUserPrefs,
+	PEM_EventDataValid_PXSwitchReason,
+	PEM_EventDataValid_PXSwitchPhase,
+	PEM_EventDataValid_HdVideo,
+	PEM_EventDataValid_BacklightLevel,
+	PEM_EventDatavalid_VariBrightParams,
+	PEM_EventDataValid_VariBrightLevel,
+	PEM_EventDataValid_VariBrightImmediateChange,
+	PEM_EventDataValid_PercentWhite,
+	PEM_EventDataValid_SdVideo,
+	PEM_EventDataValid_HTLinkChangeReason,
+	PEM_EventDataValid_HWBlocks,
+	PEM_EventDataValid_RequestedThermalState,
+	PEM_EventDataValid_MvcVideo,
+	PEM_EventDataValid_Max
+};
+
+typedef enum PEM_EventDataValid PEM_EventDataValid;
+
+/* Number of bits in ULONG variable */
+#define PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD (sizeof(unsigned long)*8)
+
+/* Number of ULONG entries used by event data valid bits */
+#define PEM_MAX_NUM_EVENTDATAVALID_ULONG_ENTRIES                                 \
+		((PEM_EventDataValid_Max + PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD - 1) /  \
+		PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD)
+
+static inline void pem_set_event_data_valid(unsigned long *fields, PEM_EventDataValid valid_field)
+{
+	fields[valid_field / PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD] |=
+		(1UL << (valid_field % PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD));
+}
+
+static inline void pem_unset_event_data_valid(unsigned long *fields, PEM_EventDataValid valid_field)
+{
+	fields[valid_field / PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD] &=
+		~(1UL << (valid_field % PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD));
+}
+
+static inline unsigned long pem_is_event_data_valid(const unsigned long *fields, PEM_EventDataValid valid_field)
+{
+	return fields[valid_field / PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD] &
+		(1UL << (valid_field % PEM_MAX_NUM_EVENTDATAVALID_BITS_PER_FIELD));
+}
+
+struct pem_event_data {
+	unsigned long	valid_fields[100];
+	unsigned long   requested_state_id;
+	enum PP_StateUILabel requested_ui_label;
+	struct pp_power_state  *pnew_power_state;
+	enum pp_power_source  requested_power_source;
+	struct PP_Clocks       requested_clocks;
+	bool         skip_state_adjust_rules;
+	struct phm_asic_blocks  asic_blocks;
+	/* to doPP_ThermalState requestedThermalState;
+	enum ThermalStateRequestSrc requestThermalStateSrc;
+	PP_Temperature  currentTemperature;*/
+
+};
+
+int pem_handle_event(struct pp_eventmgr *eventmgr, enum amd_pp_event event,
+		     struct pem_event_data *event_data);
+
+bool pem_is_hw_access_blocked(struct pp_eventmgr *eventmgr);
+
+#endif /* _EVENT_MANAGER_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/inc/eventmgr.h b/drivers/gpu/drm/amd/powerplay/inc/eventmgr.h
new file mode 100644
index 000000000000..10437dcfd365
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/eventmgr.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _EVENTMGR_H_
+#define _EVENTMGR_H_
+
+#include <linux/mutex.h>
+#include "pp_instance.h"
+#include "hardwaremanager.h"
+#include "eventmanager.h"
+#include "pp_feature.h"
+#include "pp_power_source.h"
+#include "power_state.h"
+
+typedef int (*pem_event_action)(struct pp_eventmgr *eventmgr,
+				struct pem_event_data *event_data);
+
+struct action_chain {
+	const char *description;  /* action chain description for debugging purpose */
+	const pem_event_action **action_chain; /* pointer to chain of event actions */
+};
+
+struct pem_power_source_ui_state_info {
+	enum PP_StateUILabel current_ui_label;
+	enum PP_StateUILabel default_ui_lable;
+	unsigned long    configurable_ui_mapping;
+};
+
+struct pp_clock_range {
+	uint32_t min_sclk_khz;
+	uint32_t max_sclk_khz;
+
+	uint32_t min_mclk_khz;
+	uint32_t max_mclk_khz;
+
+	uint32_t min_vclk_khz;
+	uint32_t max_vclk_khz;
+
+	uint32_t min_dclk_khz;
+	uint32_t max_dclk_khz;
+
+	uint32_t min_aclk_khz;
+	uint32_t max_aclk_khz;
+
+	uint32_t min_eclk_khz;
+	uint32_t max_eclk_khz;
+};
+
+enum pp_state {
+	UNINITIALIZED,
+	INACTIVE,
+	ACTIVE
+};
+
+enum pp_ring_index {
+	PP_RING_TYPE_GFX_INDEX = 0,
+	PP_RING_TYPE_DMA_INDEX,
+	PP_RING_TYPE_DMA1_INDEX,
+	PP_RING_TYPE_UVD_INDEX,
+	PP_RING_TYPE_VCE0_INDEX,
+	PP_RING_TYPE_VCE1_INDEX,
+	PP_RING_TYPE_CP1_INDEX,
+	PP_RING_TYPE_CP2_INDEX,
+	PP_NUM_RINGS,
+};
+
+struct pp_request {
+	uint32_t flags;
+	uint32_t sclk;
+	uint32_t sclk_throttle;
+	uint32_t mclk;
+	uint32_t vclk;
+	uint32_t dclk;
+	uint32_t eclk;
+	uint32_t aclk;
+	uint32_t iclk;
+	uint32_t vp8clk;
+	uint32_t rsv[32];
+};
+
+struct pp_eventmgr {
+	struct pp_hwmgr	*hwmgr;
+	struct pp_smumgr *smumgr;
+
+	struct pp_feature_info features[PP_Feature_Max];
+	const struct action_chain *event_chain[AMD_PP_EVENT_MAX];
+	struct phm_platform_descriptor   *platform_descriptor;
+	struct pp_clock_range clock_range;
+	enum pp_power_source  current_power_source;
+	struct pem_power_source_ui_state_info  ui_state_info[PP_PowerSource_Max];
+	enum pp_state states[PP_NUM_RINGS];
+	struct pp_request hi_req;
+	struct list_head context_list;
+	struct mutex lock;
+	bool  block_adjust_power_state;
+	bool enable_cg;
+	bool enable_gfx_cgpg;
+	int (*pp_eventmgr_init)(struct pp_eventmgr *eventmgr);
+	void (*pp_eventmgr_fini)(struct pp_eventmgr *eventmgr);
+};
+
+int eventmgr_init(struct pp_instance *handle);
+int eventmgr_fini(struct pp_eventmgr *eventmgr);
+
+#endif /* _EVENTMGR_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/inc/fiji_ppsmc.h b/drivers/gpu/drm/amd/powerplay/inc/fiji_ppsmc.h
new file mode 100644
index 000000000000..7ae494569a60
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/fiji_ppsmc.h
@@ -0,0 +1,412 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+
+#ifndef _FIJI_PP_SMC_H_
+#define _FIJI_PP_SMC_H_
+
+#pragma pack(push, 1)
+
+#define PPSMC_SWSTATE_FLAG_DC                           0x01
+#define PPSMC_SWSTATE_FLAG_UVD                          0x02
+#define PPSMC_SWSTATE_FLAG_VCE                          0x04
+
+#define PPSMC_THERMAL_PROTECT_TYPE_INTERNAL             0x00
+#define PPSMC_THERMAL_PROTECT_TYPE_EXTERNAL             0x01
+#define PPSMC_THERMAL_PROTECT_TYPE_NONE                 0xff
+
+#define PPSMC_SYSTEMFLAG_GPIO_DC                        0x01
+#define PPSMC_SYSTEMFLAG_STEPVDDC                       0x02
+#define PPSMC_SYSTEMFLAG_GDDR5                          0x04
+
+#define PPSMC_SYSTEMFLAG_DISABLE_BABYSTEP               0x08
+
+#define PPSMC_SYSTEMFLAG_REGULATOR_HOT                  0x10
+#define PPSMC_SYSTEMFLAG_REGULATOR_HOT_ANALOG           0x20
+
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_MASK              0x07
+#define PPSMC_EXTRAFLAGS_AC2DC_DONT_WAIT_FOR_VBLANK     0x08
+
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTODPMLOWSTATE   0x00
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTOINITIALSTATE  0x01
+
+/* Defines for DPM 2.0 */
+#define PPSMC_DPM2FLAGS_TDPCLMP                         0x01
+#define PPSMC_DPM2FLAGS_PWRSHFT                         0x02
+#define PPSMC_DPM2FLAGS_OCP                             0x04
+
+/* Defines for display watermark level */
+#define PPSMC_DISPLAY_WATERMARK_LOW                     0
+#define PPSMC_DISPLAY_WATERMARK_HIGH                    1
+
+/* In the HW performance level's state flags: */
+#define PPSMC_STATEFLAG_AUTO_PULSE_SKIP    0x01
+#define PPSMC_STATEFLAG_POWERBOOST         0x02
+#define PPSMC_STATEFLAG_PSKIP_ON_TDP_FAULT 0x04
+#define PPSMC_STATEFLAG_POWERSHIFT         0x08
+#define PPSMC_STATEFLAG_SLOW_READ_MARGIN   0x10
+#define PPSMC_STATEFLAG_DEEPSLEEP_THROTTLE 0x20
+#define PPSMC_STATEFLAG_DEEPSLEEP_BYPASS   0x40
+
+/* Fan control algorithm: */
+#define FDO_MODE_HARDWARE 0
+#define FDO_MODE_PIECE_WISE_LINEAR 1
+
+enum FAN_CONTROL {
+  FAN_CONTROL_FUZZY,
+  FAN_CONTROL_TABLE
+};
+
+/* Gemini Modes*/
+#define PPSMC_GeminiModeNone   0  /*Single GPU board*/
+#define PPSMC_GeminiModeMaster 1  /*Master GPU on a Gemini board*/
+#define PPSMC_GeminiModeSlave  2  /*Slave GPU on a Gemini board*/
+
+
+/* Return codes for driver to SMC communication. */
+#define PPSMC_Result_OK             ((uint16_t)0x01)
+#define PPSMC_Result_NoMore         ((uint16_t)0x02)
+
+#define PPSMC_Result_NotNow         ((uint16_t)0x03)
+
+#define PPSMC_Result_Failed         ((uint16_t)0xFF)
+#define PPSMC_Result_UnknownCmd     ((uint16_t)0xFE)
+#define PPSMC_Result_UnknownVT      ((uint16_t)0xFD)
+
+#define PPSMC_isERROR(x) ((uint16_t)0x80 & (x))
+
+
+#define PPSMC_MSG_Halt                      ((uint16_t)0x10)
+#define PPSMC_MSG_Resume                    ((uint16_t)0x11)
+#define PPSMC_MSG_EnableDPMLevel            ((uint16_t)0x12)
+#define PPSMC_MSG_ZeroLevelsDisabled        ((uint16_t)0x13)
+#define PPSMC_MSG_OneLevelsDisabled         ((uint16_t)0x14)
+#define PPSMC_MSG_TwoLevelsDisabled         ((uint16_t)0x15)
+#define PPSMC_MSG_EnableThermalInterrupt    ((uint16_t)0x16)
+#define PPSMC_MSG_RunningOnAC               ((uint16_t)0x17)
+#define PPSMC_MSG_LevelUp                   ((uint16_t)0x18)
+#define PPSMC_MSG_LevelDown                 ((uint16_t)0x19)
+#define PPSMC_MSG_ResetDPMCounters          ((uint16_t)0x1a)
+#define PPSMC_MSG_SwitchToSwState           ((uint16_t)0x20)
+
+#define PPSMC_MSG_SwitchToSwStateLast       ((uint16_t)0x3f)
+#define PPSMC_MSG_SwitchToInitialState      ((uint16_t)0x40)
+#define PPSMC_MSG_NoForcedLevel             ((uint16_t)0x41)
+#define PPSMC_MSG_ForceHigh                 ((uint16_t)0x42)
+#define PPSMC_MSG_ForceMediumOrHigh         ((uint16_t)0x43)
+
+#define PPSMC_MSG_SwitchToMinimumPower      ((uint16_t)0x51)
+#define PPSMC_MSG_ResumeFromMinimumPower    ((uint16_t)0x52)
+#define PPSMC_MSG_EnableCac                 ((uint16_t)0x53)
+#define PPSMC_MSG_DisableCac                ((uint16_t)0x54)
+#define PPSMC_DPMStateHistoryStart          ((uint16_t)0x55)
+#define PPSMC_DPMStateHistoryStop           ((uint16_t)0x56)
+#define PPSMC_CACHistoryStart               ((uint16_t)0x57)
+#define PPSMC_CACHistoryStop                ((uint16_t)0x58)
+#define PPSMC_TDPClampingActive             ((uint16_t)0x59)
+#define PPSMC_TDPClampingInactive           ((uint16_t)0x5A)
+#define PPSMC_StartFanControl               ((uint16_t)0x5B)
+#define PPSMC_StopFanControl                ((uint16_t)0x5C)
+#define PPSMC_NoDisplay                     ((uint16_t)0x5D)
+#define PPSMC_HasDisplay                    ((uint16_t)0x5E)
+#define PPSMC_MSG_UVDPowerOFF               ((uint16_t)0x60)
+#define PPSMC_MSG_UVDPowerON                ((uint16_t)0x61)
+#define PPSMC_MSG_EnableULV                 ((uint16_t)0x62)
+#define PPSMC_MSG_DisableULV                ((uint16_t)0x63)
+#define PPSMC_MSG_EnterULV                  ((uint16_t)0x64)
+#define PPSMC_MSG_ExitULV                   ((uint16_t)0x65)
+#define PPSMC_PowerShiftActive              ((uint16_t)0x6A)
+#define PPSMC_PowerShiftInactive            ((uint16_t)0x6B)
+#define PPSMC_OCPActive                     ((uint16_t)0x6C)
+#define PPSMC_OCPInactive                   ((uint16_t)0x6D)
+#define PPSMC_CACLongTermAvgEnable          ((uint16_t)0x6E)
+#define PPSMC_CACLongTermAvgDisable         ((uint16_t)0x6F)
+#define PPSMC_MSG_InferredStateSweep_Start  ((uint16_t)0x70)
+#define PPSMC_MSG_InferredStateSweep_Stop   ((uint16_t)0x71)
+#define PPSMC_MSG_SwitchToLowestInfState    ((uint16_t)0x72)
+#define PPSMC_MSG_SwitchToNonInfState       ((uint16_t)0x73)
+#define PPSMC_MSG_AllStateSweep_Start       ((uint16_t)0x74)
+#define PPSMC_MSG_AllStateSweep_Stop        ((uint16_t)0x75)
+#define PPSMC_MSG_SwitchNextLowerInfState   ((uint16_t)0x76)
+#define PPSMC_MSG_SwitchNextHigherInfState  ((uint16_t)0x77)
+#define PPSMC_MSG_MclkRetrainingTest        ((uint16_t)0x78)
+#define PPSMC_MSG_ForceTDPClamping          ((uint16_t)0x79)
+#define PPSMC_MSG_CollectCAC_PowerCorreln   ((uint16_t)0x7A)
+#define PPSMC_MSG_CollectCAC_WeightCalib    ((uint16_t)0x7B)
+#define PPSMC_MSG_CollectCAC_SQonly         ((uint16_t)0x7C)
+#define PPSMC_MSG_CollectCAC_TemperaturePwr ((uint16_t)0x7D)
+
+#define PPSMC_MSG_ExtremitiesTest_Start     ((uint16_t)0x7E)
+#define PPSMC_MSG_ExtremitiesTest_Stop      ((uint16_t)0x7F)
+#define PPSMC_FlushDataCache                ((uint16_t)0x80)
+#define PPSMC_FlushInstrCache               ((uint16_t)0x81)
+
+#define PPSMC_MSG_SetEnabledLevels          ((uint16_t)0x82)
+#define PPSMC_MSG_SetForcedLevels           ((uint16_t)0x83)
+
+#define PPSMC_MSG_ResetToDefaults           ((uint16_t)0x84)
+
+#define PPSMC_MSG_SetForcedLevelsAndJump      ((uint16_t)0x85)
+#define PPSMC_MSG_SetCACHistoryMode           ((uint16_t)0x86)
+#define PPSMC_MSG_EnableDTE                   ((uint16_t)0x87)
+#define PPSMC_MSG_DisableDTE                  ((uint16_t)0x88)
+
+#define PPSMC_MSG_SmcSpaceSetAddress          ((uint16_t)0x89)
+
+#define PPSMC_MSG_BREAK                       ((uint16_t)0xF8)
+
+/* Trinity Specific Messages*/
+#define PPSMC_MSG_Test                        ((uint16_t) 0x100)
+#define PPSMC_MSG_DPM_Voltage_Pwrmgt          ((uint16_t) 0x101)
+#define PPSMC_MSG_DPM_Config                  ((uint16_t) 0x102)
+#define PPSMC_MSG_PM_Controller_Start         ((uint16_t) 0x103)
+#define PPSMC_MSG_DPM_ForceState              ((uint16_t) 0x104)
+#define PPSMC_MSG_PG_PowerDownSIMD            ((uint16_t) 0x105)
+#define PPSMC_MSG_PG_PowerUpSIMD              ((uint16_t) 0x106)
+#define PPSMC_MSG_PM_Controller_Stop          ((uint16_t) 0x107)
+#define PPSMC_MSG_PG_SIMD_Config              ((uint16_t) 0x108)
+#define PPSMC_MSG_Voltage_Cntl_Enable         ((uint16_t) 0x109)
+#define PPSMC_MSG_Thermal_Cntl_Enable         ((uint16_t) 0x10a)
+#define PPSMC_MSG_Reset_Service               ((uint16_t) 0x10b)
+#define PPSMC_MSG_VCEPowerOFF                 ((uint16_t) 0x10e)
+#define PPSMC_MSG_VCEPowerON                  ((uint16_t) 0x10f)
+#define PPSMC_MSG_DPM_Disable_VCE_HS          ((uint16_t) 0x110)
+#define PPSMC_MSG_DPM_Enable_VCE_HS           ((uint16_t) 0x111)
+#define PPSMC_MSG_DPM_N_LevelsDisabled        ((uint16_t) 0x112)
+#define PPSMC_MSG_DCEPowerOFF                 ((uint16_t) 0x113)
+#define PPSMC_MSG_DCEPowerON                  ((uint16_t) 0x114)
+#define PPSMC_MSG_PCIE_DDIPowerDown           ((uint16_t) 0x117)
+#define PPSMC_MSG_PCIE_DDIPowerUp             ((uint16_t) 0x118)
+#define PPSMC_MSG_PCIE_CascadePLLPowerDown    ((uint16_t) 0x119)
+#define PPSMC_MSG_PCIE_CascadePLLPowerUp      ((uint16_t) 0x11a)
+#define PPSMC_MSG_SYSPLLPowerOff              ((uint16_t) 0x11b)
+#define PPSMC_MSG_SYSPLLPowerOn               ((uint16_t) 0x11c)
+#define PPSMC_MSG_DCE_RemoveVoltageAdjustment ((uint16_t) 0x11d)
+#define PPSMC_MSG_DCE_AllowVoltageAdjustment  ((uint16_t) 0x11e)
+#define PPSMC_MSG_DISPLAYPHYStatusNotify      ((uint16_t) 0x11f)
+#define PPSMC_MSG_EnableBAPM                  ((uint16_t) 0x120)
+#define PPSMC_MSG_DisableBAPM                 ((uint16_t) 0x121)
+#define PPSMC_MSG_Spmi_Enable                 ((uint16_t) 0x122)
+#define PPSMC_MSG_Spmi_Timer                  ((uint16_t) 0x123)
+#define PPSMC_MSG_LCLK_DPM_Config             ((uint16_t) 0x124)
+#define PPSMC_MSG_VddNB_Request               ((uint16_t) 0x125)
+#define PPSMC_MSG_PCIE_DDIPhyPowerDown        ((uint32_t) 0x126)
+#define PPSMC_MSG_PCIE_DDIPhyPowerUp          ((uint32_t) 0x127)
+#define PPSMC_MSG_MCLKDPM_Config              ((uint16_t) 0x128)
+
+#define PPSMC_MSG_UVDDPM_Config               ((uint16_t) 0x129)
+#define PPSMC_MSG_VCEDPM_Config               ((uint16_t) 0x12A)
+#define PPSMC_MSG_ACPDPM_Config               ((uint16_t) 0x12B)
+#define PPSMC_MSG_SAMUDPM_Config              ((uint16_t) 0x12C)
+#define PPSMC_MSG_UVDDPM_SetEnabledMask       ((uint16_t) 0x12D)
+#define PPSMC_MSG_VCEDPM_SetEnabledMask       ((uint16_t) 0x12E)
+#define PPSMC_MSG_ACPDPM_SetEnabledMask       ((uint16_t) 0x12F)
+#define PPSMC_MSG_SAMUDPM_SetEnabledMask      ((uint16_t) 0x130)
+#define PPSMC_MSG_MCLKDPM_ForceState          ((uint16_t) 0x131)
+#define PPSMC_MSG_MCLKDPM_NoForcedLevel       ((uint16_t) 0x132)
+#define PPSMC_MSG_Thermal_Cntl_Disable        ((uint16_t) 0x133)
+#define PPSMC_MSG_SetTDPLimit                 ((uint16_t) 0x134)
+#define PPSMC_MSG_Voltage_Cntl_Disable        ((uint16_t) 0x135)
+#define PPSMC_MSG_PCIeDPM_Enable              ((uint16_t) 0x136)
+#define PPSMC_MSG_ACPPowerOFF                 ((uint16_t) 0x137)
+#define PPSMC_MSG_ACPPowerON                  ((uint16_t) 0x138)
+#define PPSMC_MSG_SAMPowerOFF                 ((uint16_t) 0x139)
+#define PPSMC_MSG_SAMPowerON                  ((uint16_t) 0x13a)
+#define PPSMC_MSG_SDMAPowerOFF                ((uint16_t) 0x13b)
+#define PPSMC_MSG_SDMAPowerON                 ((uint16_t) 0x13c)
+#define PPSMC_MSG_PCIeDPM_Disable             ((uint16_t) 0x13d)
+#define PPSMC_MSG_IOMMUPowerOFF               ((uint16_t) 0x13e)
+#define PPSMC_MSG_IOMMUPowerON                ((uint16_t) 0x13f)
+#define PPSMC_MSG_NBDPM_Enable                ((uint16_t) 0x140)
+#define PPSMC_MSG_NBDPM_Disable               ((uint16_t) 0x141)
+#define PPSMC_MSG_NBDPM_ForceNominal          ((uint16_t) 0x142)
+#define PPSMC_MSG_NBDPM_ForcePerformance      ((uint16_t) 0x143)
+#define PPSMC_MSG_NBDPM_UnForce               ((uint16_t) 0x144)
+#define PPSMC_MSG_SCLKDPM_SetEnabledMask      ((uint16_t) 0x145)
+#define PPSMC_MSG_MCLKDPM_SetEnabledMask      ((uint16_t) 0x146)
+#define PPSMC_MSG_PCIeDPM_ForceLevel          ((uint16_t) 0x147)
+#define PPSMC_MSG_PCIeDPM_UnForceLevel        ((uint16_t) 0x148)
+#define PPSMC_MSG_EnableACDCGPIOInterrupt     ((uint16_t) 0x149)
+#define PPSMC_MSG_EnableVRHotGPIOInterrupt    ((uint16_t) 0x14a)
+#define PPSMC_MSG_SwitchToAC                  ((uint16_t) 0x14b)
+
+#define PPSMC_MSG_XDMAPowerOFF                ((uint16_t) 0x14c)
+#define PPSMC_MSG_XDMAPowerON                 ((uint16_t) 0x14d)
+
+#define PPSMC_MSG_DPM_Enable                  ((uint16_t) 0x14e)
+#define PPSMC_MSG_DPM_Disable                 ((uint16_t) 0x14f)
+#define PPSMC_MSG_MCLKDPM_Enable              ((uint16_t) 0x150)
+#define PPSMC_MSG_MCLKDPM_Disable             ((uint16_t) 0x151)
+#define PPSMC_MSG_LCLKDPM_Enable              ((uint16_t) 0x152)
+#define PPSMC_MSG_LCLKDPM_Disable             ((uint16_t) 0x153)
+#define PPSMC_MSG_UVDDPM_Enable               ((uint16_t) 0x154)
+#define PPSMC_MSG_UVDDPM_Disable              ((uint16_t) 0x155)
+#define PPSMC_MSG_SAMUDPM_Enable              ((uint16_t) 0x156)
+#define PPSMC_MSG_SAMUDPM_Disable             ((uint16_t) 0x157)
+#define PPSMC_MSG_ACPDPM_Enable               ((uint16_t) 0x158)
+#define PPSMC_MSG_ACPDPM_Disable              ((uint16_t) 0x159)
+#define PPSMC_MSG_VCEDPM_Enable               ((uint16_t) 0x15a)
+#define PPSMC_MSG_VCEDPM_Disable              ((uint16_t) 0x15b)
+#define PPSMC_MSG_LCLKDPM_SetEnabledMask      ((uint16_t) 0x15c)
+#define PPSMC_MSG_DPM_FPS_Mode                ((uint16_t) 0x15d)
+#define PPSMC_MSG_DPM_Activity_Mode           ((uint16_t) 0x15e)
+#define PPSMC_MSG_VddC_Request                ((uint16_t) 0x15f)
+#define PPSMC_MSG_MCLKDPM_GetEnabledMask      ((uint16_t) 0x160)
+#define PPSMC_MSG_LCLKDPM_GetEnabledMask      ((uint16_t) 0x161)
+#define PPSMC_MSG_SCLKDPM_GetEnabledMask      ((uint16_t) 0x162)
+#define PPSMC_MSG_UVDDPM_GetEnabledMask       ((uint16_t) 0x163)
+#define PPSMC_MSG_SAMUDPM_GetEnabledMask      ((uint16_t) 0x164)
+#define PPSMC_MSG_ACPDPM_GetEnabledMask       ((uint16_t) 0x165)
+#define PPSMC_MSG_VCEDPM_GetEnabledMask       ((uint16_t) 0x166)
+#define PPSMC_MSG_PCIeDPM_SetEnabledMask      ((uint16_t) 0x167)
+#define PPSMC_MSG_PCIeDPM_GetEnabledMask      ((uint16_t) 0x168)
+#define PPSMC_MSG_TDCLimitEnable              ((uint16_t) 0x169)
+#define PPSMC_MSG_TDCLimitDisable             ((uint16_t) 0x16a)
+#define PPSMC_MSG_DPM_AutoRotate_Mode         ((uint16_t) 0x16b)
+#define PPSMC_MSG_DISPCLK_FROM_FCH            ((uint16_t) 0x16c)
+#define PPSMC_MSG_DISPCLK_FROM_DFS            ((uint16_t) 0x16d)
+#define PPSMC_MSG_DPREFCLK_FROM_FCH           ((uint16_t) 0x16e)
+#define PPSMC_MSG_DPREFCLK_FROM_DFS           ((uint16_t) 0x16f)
+#define PPSMC_MSG_PmStatusLogStart            ((uint16_t) 0x170)
+#define PPSMC_MSG_PmStatusLogSample           ((uint16_t) 0x171)
+#define PPSMC_MSG_SCLK_AutoDPM_ON             ((uint16_t) 0x172)
+#define PPSMC_MSG_MCLK_AutoDPM_ON             ((uint16_t) 0x173)
+#define PPSMC_MSG_LCLK_AutoDPM_ON             ((uint16_t) 0x174)
+#define PPSMC_MSG_UVD_AutoDPM_ON              ((uint16_t) 0x175)
+#define PPSMC_MSG_SAMU_AutoDPM_ON             ((uint16_t) 0x176)
+#define PPSMC_MSG_ACP_AutoDPM_ON              ((uint16_t) 0x177)
+#define PPSMC_MSG_VCE_AutoDPM_ON              ((uint16_t) 0x178)
+#define PPSMC_MSG_PCIe_AutoDPM_ON             ((uint16_t) 0x179)
+#define PPSMC_MSG_MASTER_AutoDPM_ON           ((uint16_t) 0x17a)
+#define PPSMC_MSG_MASTER_AutoDPM_OFF          ((uint16_t) 0x17b)
+#define PPSMC_MSG_DYNAMICDISPPHYPOWER         ((uint16_t) 0x17c)
+#define PPSMC_MSG_CAC_COLLECTION_ON           ((uint16_t) 0x17d)
+#define PPSMC_MSG_CAC_COLLECTION_OFF          ((uint16_t) 0x17e)
+#define PPSMC_MSG_CAC_CORRELATION_ON          ((uint16_t) 0x17f)
+#define PPSMC_MSG_CAC_CORRELATION_OFF         ((uint16_t) 0x180)
+#define PPSMC_MSG_PM_STATUS_TO_DRAM_ON        ((uint16_t) 0x181)
+#define PPSMC_MSG_PM_STATUS_TO_DRAM_OFF       ((uint16_t) 0x182)
+#define PPSMC_MSG_ALLOW_LOWSCLK_INTERRUPT     ((uint16_t) 0x184)
+#define PPSMC_MSG_PkgPwrLimitEnable           ((uint16_t) 0x185)
+#define PPSMC_MSG_PkgPwrLimitDisable          ((uint16_t) 0x186)
+#define PPSMC_MSG_PkgPwrSetLimit              ((uint16_t) 0x187)
+#define PPSMC_MSG_OverDriveSetTargetTdp       ((uint16_t) 0x188)
+#define PPSMC_MSG_SCLKDPM_FreezeLevel         ((uint16_t) 0x189)
+#define PPSMC_MSG_SCLKDPM_UnfreezeLevel       ((uint16_t) 0x18A)
+#define PPSMC_MSG_MCLKDPM_FreezeLevel         ((uint16_t) 0x18B)
+#define PPSMC_MSG_MCLKDPM_UnfreezeLevel       ((uint16_t) 0x18C)
+#define PPSMC_MSG_START_DRAM_LOGGING          ((uint16_t) 0x18D)
+#define PPSMC_MSG_STOP_DRAM_LOGGING           ((uint16_t) 0x18E)
+#define PPSMC_MSG_MASTER_DeepSleep_ON         ((uint16_t) 0x18F)
+#define PPSMC_MSG_MASTER_DeepSleep_OFF        ((uint16_t) 0x190)
+#define PPSMC_MSG_Remove_DC_Clamp             ((uint16_t) 0x191)
+#define PPSMC_MSG_DisableACDCGPIOInterrupt    ((uint16_t) 0x192)
+#define PPSMC_MSG_OverrideVoltageControl_SetVddc       ((uint16_t) 0x193)
+#define PPSMC_MSG_OverrideVoltageControl_SetVddci      ((uint16_t) 0x194)
+#define PPSMC_MSG_SetVidOffset_1              ((uint16_t) 0x195)
+#define PPSMC_MSG_SetVidOffset_2              ((uint16_t) 0x207)
+#define PPSMC_MSG_GetVidOffset_1              ((uint16_t) 0x196)
+#define PPSMC_MSG_GetVidOffset_2              ((uint16_t) 0x208)
+#define PPSMC_MSG_THERMAL_OVERDRIVE_Enable    ((uint16_t) 0x197)
+#define PPSMC_MSG_THERMAL_OVERDRIVE_Disable   ((uint16_t) 0x198)
+#define PPSMC_MSG_SetTjMax                    ((uint16_t) 0x199)
+#define PPSMC_MSG_SetFanPwmMax                ((uint16_t) 0x19A)
+#define PPSMC_MSG_WaitForMclkSwitchFinish     ((uint16_t) 0x19B)
+#define PPSMC_MSG_ENABLE_THERMAL_DPM          ((uint16_t) 0x19C)
+#define PPSMC_MSG_DISABLE_THERMAL_DPM         ((uint16_t) 0x19D)
+
+#define PPSMC_MSG_API_GetSclkFrequency        ((uint16_t) 0x200)
+#define PPSMC_MSG_API_GetMclkFrequency        ((uint16_t) 0x201)
+#define PPSMC_MSG_API_GetSclkBusy             ((uint16_t) 0x202)
+#define PPSMC_MSG_API_GetMclkBusy             ((uint16_t) 0x203)
+#define PPSMC_MSG_API_GetAsicPower            ((uint16_t) 0x204)
+#define PPSMC_MSG_SetFanRpmMax                ((uint16_t) 0x205)
+#define PPSMC_MSG_SetFanSclkTarget            ((uint16_t) 0x206)
+#define PPSMC_MSG_SetFanMinPwm                ((uint16_t) 0x209)
+#define PPSMC_MSG_SetFanTemperatureTarget     ((uint16_t) 0x20A)
+
+#define PPSMC_MSG_BACO_StartMonitor           ((uint16_t) 0x240)
+#define PPSMC_MSG_BACO_Cancel                 ((uint16_t) 0x241)
+#define PPSMC_MSG_EnableVddGfx                ((uint16_t) 0x242)
+#define PPSMC_MSG_DisableVddGfx               ((uint16_t) 0x243)
+#define PPSMC_MSG_UcodeAddressLow             ((uint16_t) 0x244)
+#define PPSMC_MSG_UcodeAddressHigh            ((uint16_t) 0x245)
+#define PPSMC_MSG_UcodeLoadStatus             ((uint16_t) 0x246)
+
+#define PPSMC_MSG_DRV_DRAM_ADDR_HI            ((uint16_t) 0x250)
+#define PPSMC_MSG_DRV_DRAM_ADDR_LO            ((uint16_t) 0x251)
+#define PPSMC_MSG_SMU_DRAM_ADDR_HI            ((uint16_t) 0x252)
+#define PPSMC_MSG_SMU_DRAM_ADDR_LO            ((uint16_t) 0x253)
+#define PPSMC_MSG_LoadUcodes                  ((uint16_t) 0x254)
+#define PPSMC_MSG_PowerStateNotify            ((uint16_t) 0x255)
+#define PPSMC_MSG_COND_EXEC_DRAM_ADDR_HI      ((uint16_t) 0x256)
+#define PPSMC_MSG_COND_EXEC_DRAM_ADDR_LO      ((uint16_t) 0x257)
+#define PPSMC_MSG_VBIOS_DRAM_ADDR_HI          ((uint16_t) 0x258)
+#define PPSMC_MSG_VBIOS_DRAM_ADDR_LO          ((uint16_t) 0x259)
+#define PPSMC_MSG_LoadVBios                   ((uint16_t) 0x25A)
+#define PPSMC_MSG_GetUcodeVersion             ((uint16_t) 0x25B)
+#define DMCUSMC_MSG_PSREntry                  ((uint16_t) 0x25C)
+#define DMCUSMC_MSG_PSRExit                   ((uint16_t) 0x25D)
+#define PPSMC_MSG_EnableClockGatingFeature    ((uint16_t) 0x260)
+#define PPSMC_MSG_DisableClockGatingFeature   ((uint16_t) 0x261)
+#define PPSMC_MSG_IsDeviceRunning             ((uint16_t) 0x262)
+#define PPSMC_MSG_LoadMetaData                ((uint16_t) 0x263)
+#define PPSMC_MSG_TMON_AutoCaliberate_Enable  ((uint16_t) 0x264)
+#define PPSMC_MSG_TMON_AutoCaliberate_Disable ((uint16_t) 0x265)
+#define PPSMC_MSG_GetTelemetry1Slope          ((uint16_t) 0x266)
+#define PPSMC_MSG_GetTelemetry1Offset         ((uint16_t) 0x267)
+#define PPSMC_MSG_GetTelemetry2Slope          ((uint16_t) 0x268)
+#define PPSMC_MSG_GetTelemetry2Offset         ((uint16_t) 0x269)
+#define PPSMC_MSG_EnableAvfs                  ((uint16_t) 0x26A)
+#define PPSMC_MSG_DisableAvfs                 ((uint16_t) 0x26B)
+#define PPSMC_MSG_PerformBtc                  ((uint16_t) 0x26C)
+#define PPSMC_MSG_GetHbmCode                  ((uint16_t) 0x26D)
+#define PPSMC_MSG_GetVrVddcTemperature        ((uint16_t) 0x26E)
+#define PPSMC_MSG_GetVrMvddTemperature        ((uint16_t) 0x26F)
+#define PPSMC_MSG_GetLiquidTemperature        ((uint16_t) 0x270)
+#define PPSMC_MSG_GetPlxTemperature           ((uint16_t) 0x271)
+#define PPSMC_MSG_RequestI2CControl           ((uint16_t) 0x272)
+#define PPSMC_MSG_ReleaseI2CControl           ((uint16_t) 0x273)
+#define PPSMC_MSG_LedConfig                   ((uint16_t) 0x274)
+#define PPSMC_MSG_SetHbmFanCode               ((uint16_t) 0x275)
+#define PPSMC_MSG_SetHbmThrottleCode          ((uint16_t) 0x276)
+
+#define PPSMC_MSG_GetEnabledPsm               ((uint16_t) 0x400)
+#define PPSMC_MSG_AgmStartPsm                 ((uint16_t) 0x401)
+#define PPSMC_MSG_AgmReadPsm                  ((uint16_t) 0x402)
+#define PPSMC_MSG_AgmResetPsm                 ((uint16_t) 0x403)
+#define PPSMC_MSG_ReadVftCell                 ((uint16_t) 0x404)
+
+/* AVFS Only - Remove Later */
+#define PPSMC_MSG_VftTableIsValid             ((uint16_t) 0x666)
+
+/* If the SMC firmware has an event status soft register this is what the individual bits mean.*/
+#define PPSMC_EVENT_STATUS_THERMAL          0x00000001
+#define PPSMC_EVENT_STATUS_REGULATORHOT     0x00000002
+#define PPSMC_EVENT_STATUS_DC               0x00000004
+
+typedef uint16_t PPSMC_Msg;
+
+#pragma pack(pop)
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/fiji_pwrvirus.h b/drivers/gpu/drm/amd/powerplay/inc/fiji_pwrvirus.h
new file mode 100644
index 000000000000..0262ad35502a
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/fiji_pwrvirus.h
@@ -0,0 +1,10299 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _FIJI_PWRVIRUS_H_
+#define _FIJI_PWRVIRUS_H_
+
+#define mmCP_HYP_MEC1_UCODE_ADDR	0xf81a
+#define mmCP_HYP_MEC1_UCODE_DATA	0xf81b
+#define mmCP_HYP_MEC2_UCODE_ADDR	0xf81c
+#define mmCP_HYP_MEC2_UCODE_DATA	0xf81d
+
+enum PWR_Command
+{
+   PwrCmdNull = 0,
+   PwrCmdWrite,
+   PwrCmdEnd,
+   PwrCmdMax
+};
+typedef enum PWR_Command PWR_Command;
+
+struct PWR_Command_Table
+{
+   PWR_Command        command;
+   ULONG              data;
+   ULONG              reg;
+};
+typedef struct PWR_Command_Table PWR_Command_Table;
+
+#define PWR_VIRUS_TABLE_SIZE  10243
+static PWR_Command_Table PwrVirusTable[PWR_VIRUS_TABLE_SIZE] =
+{
+    { PwrCmdWrite, 0x100100b6, mmPCIE_INDEX                               },
+    { PwrCmdWrite, 0x00000000, mmPCIE_DATA                                },
+    { PwrCmdWrite, 0x100100b6, mmPCIE_INDEX                               },
+    { PwrCmdWrite, 0x0300078c, mmPCIE_DATA                                },
+    { PwrCmdWrite, 0x00000000, mmBIF_CLK_CTRL                             },
+    { PwrCmdWrite, 0x00000001, mmBIF_CLK_CTRL                             },
+    { PwrCmdWrite, 0x00000000, mmBIF_CLK_CTRL                             },
+    { PwrCmdWrite, 0x00000003, mmBIF_FB_EN                                },
+    { PwrCmdWrite, 0x00000000, mmBIF_FB_EN                                },
+    { PwrCmdWrite, 0x00000001, mmBIF_DOORBELL_APER_EN                     },
+    { PwrCmdWrite, 0x00000000, mmBIF_DOORBELL_APER_EN                     },
+    { PwrCmdWrite, 0x014000c0, mmPCIE_INDEX                               },
+    { PwrCmdWrite, 0x00000000, mmPCIE_DATA                                },
+    { PwrCmdWrite, 0x014000c0, mmPCIE_INDEX                               },
+    { PwrCmdWrite, 0x22000000, mmPCIE_DATA                                },
+    { PwrCmdWrite, 0x014000c0, mmPCIE_INDEX                               },
+    { PwrCmdWrite, 0x00000000, mmPCIE_DATA                                },
+    /*
+    { PwrCmdWrite, 0x009f0090, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x00000000, mmMC_CITF_CNTL                             },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x009f0090, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x009f0090, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_FB_OFFSET                          },*/
+    { PwrCmdWrite, 0x00000000, mmRLC_CSIB_ADDR_LO                         },
+    { PwrCmdWrite, 0x00000000, mmRLC_CSIB_ADDR_HI                         },
+    { PwrCmdWrite, 0x00000000, mmRLC_CSIB_LENGTH                          },
+    /*
+    { PwrCmdWrite, 0x00000000, mmMC_VM_MX_L1_TLB_CNTL                     },
+    { PwrCmdWrite, 0x00000001, mmMC_VM_SYSTEM_APERTURE_LOW_ADDR           },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_SYSTEM_APERTURE_HIGH_ADDR          },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_FB_LOCATION                        },
+    { PwrCmdWrite, 0x009f0090, mmMC_VM_FB_LOCATION                        },*/
+    { PwrCmdWrite, 0x00000000, mmVM_CONTEXT0_CNTL                         },
+    { PwrCmdWrite, 0x00000000, mmVM_CONTEXT1_CNTL                         },
+    /*
+    { PwrCmdWrite, 0x00000000, mmMC_VM_AGP_BASE                           },
+    { PwrCmdWrite, 0x00000002, mmMC_VM_AGP_BOT                            },
+    { PwrCmdWrite, 0x00000000, mmMC_VM_AGP_TOP                            },*/
+    { PwrCmdWrite, 0x04000000, mmATC_VM_APERTURE0_LOW_ADDR                },
+    { PwrCmdWrite, 0x0400ff20, mmATC_VM_APERTURE0_HIGH_ADDR               },
+    { PwrCmdWrite, 0x00000002, mmATC_VM_APERTURE0_CNTL                    },
+    { PwrCmdWrite, 0x0000ffff, mmATC_VM_APERTURE0_CNTL2                   },
+    { PwrCmdWrite, 0x00000001, mmATC_VM_APERTURE1_LOW_ADDR                },
+    { PwrCmdWrite, 0x00000000, mmATC_VM_APERTURE1_HIGH_ADDR               },
+    { PwrCmdWrite, 0x00000000, mmATC_VM_APERTURE1_CNTL                    },
+    { PwrCmdWrite, 0x00000000, mmATC_VM_APERTURE1_CNTL2                   },
+    //{ PwrCmdWrite, 0x00000000, mmMC_ARB_RAMCFG                            },
+    { PwrCmdWrite, 0x12011003, mmGB_ADDR_CONFIG                           },
+    { PwrCmdWrite, 0x00800010, mmGB_TILE_MODE0                            },
+    { PwrCmdWrite, 0x00800810, mmGB_TILE_MODE1                            },
+    { PwrCmdWrite, 0x00801010, mmGB_TILE_MODE2                            },
+    { PwrCmdWrite, 0x00801810, mmGB_TILE_MODE3                            },
+    { PwrCmdWrite, 0x00802810, mmGB_TILE_MODE4                            },
+    { PwrCmdWrite, 0x00802808, mmGB_TILE_MODE5                            },
+    { PwrCmdWrite, 0x00802814, mmGB_TILE_MODE6                            },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE7                            },
+    { PwrCmdWrite, 0x00000004, mmGB_TILE_MODE8                            },
+    { PwrCmdWrite, 0x02000008, mmGB_TILE_MODE9                            },
+    { PwrCmdWrite, 0x02000010, mmGB_TILE_MODE10                           },
+    { PwrCmdWrite, 0x06000014, mmGB_TILE_MODE11                           },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE12                           },
+    { PwrCmdWrite, 0x02400008, mmGB_TILE_MODE13                           },
+    { PwrCmdWrite, 0x02400010, mmGB_TILE_MODE14                           },
+    { PwrCmdWrite, 0x02400030, mmGB_TILE_MODE15                           },
+    { PwrCmdWrite, 0x06400014, mmGB_TILE_MODE16                           },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE17                           },
+    { PwrCmdWrite, 0x0040000c, mmGB_TILE_MODE18                           },
+    { PwrCmdWrite, 0x0100000c, mmGB_TILE_MODE19                           },
+    { PwrCmdWrite, 0x0100001c, mmGB_TILE_MODE20                           },
+    { PwrCmdWrite, 0x01000034, mmGB_TILE_MODE21                           },
+    { PwrCmdWrite, 0x01000024, mmGB_TILE_MODE22                           },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE23                           },
+    { PwrCmdWrite, 0x0040001c, mmGB_TILE_MODE24                           },
+    { PwrCmdWrite, 0x01000020, mmGB_TILE_MODE25                           },
+    { PwrCmdWrite, 0x01000038, mmGB_TILE_MODE26                           },
+    { PwrCmdWrite, 0x02c00008, mmGB_TILE_MODE27                           },
+    { PwrCmdWrite, 0x02c00010, mmGB_TILE_MODE28                           },
+    { PwrCmdWrite, 0x06c00014, mmGB_TILE_MODE29                           },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE30                           },
+    { PwrCmdWrite, 0x00000000, mmGB_TILE_MODE31                           },
+    { PwrCmdWrite, 0x000000a8, mmGB_MACROTILE_MODE0                       },
+    { PwrCmdWrite, 0x000000a4, mmGB_MACROTILE_MODE1                       },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE2                       },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE3                       },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE4                       },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE5                       },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE6                       },
+    { PwrCmdWrite, 0x00000000, mmGB_MACROTILE_MODE7                       },
+    { PwrCmdWrite, 0x000000ee, mmGB_MACROTILE_MODE8                       },
+    { PwrCmdWrite, 0x000000ea, mmGB_MACROTILE_MODE9                       },
+    { PwrCmdWrite, 0x000000e9, mmGB_MACROTILE_MODE10                      },
+    { PwrCmdWrite, 0x000000e5, mmGB_MACROTILE_MODE11                      },
+    { PwrCmdWrite, 0x000000e4, mmGB_MACROTILE_MODE12                      },
+    { PwrCmdWrite, 0x000000e0, mmGB_MACROTILE_MODE13                      },
+    { PwrCmdWrite, 0x00000090, mmGB_MACROTILE_MODE14                      },
+    { PwrCmdWrite, 0x00000000, mmGB_MACROTILE_MODE15                      },
+    { PwrCmdWrite, 0x00900000, mmHDP_NONSURFACE_BASE                      },
+    { PwrCmdWrite, 0x00008000, mmHDP_NONSURFACE_INFO                      },
+    { PwrCmdWrite, 0x3fffffff, mmHDP_NONSURFACE_SIZE                      },
+    { PwrCmdWrite, 0x00000003, mmBIF_FB_EN                                },
+    //{ PwrCmdWrite, 0x00000000, mmMC_VM_FB_OFFSET                          },
+    { PwrCmdWrite, 0x00000000, mmSRBM_CNTL                                },
+    { PwrCmdWrite, 0x00020000, mmSRBM_CNTL                                },
+    { PwrCmdWrite, 0x80000000, mmATC_VMID0_PASID_MAPPING                  },
+    { PwrCmdWrite, 0x00000000, mmATC_VMID_PASID_MAPPING_UPDATE_STATUS     },
+    { PwrCmdWrite, 0x00000000, mmRLC_CNTL                                 },
+    { PwrCmdWrite, 0x00000000, mmRLC_CNTL                                 },
+    { PwrCmdWrite, 0x00000000, mmRLC_CNTL                                 },
+    { PwrCmdWrite, 0xe0000000, mmGRBM_GFX_INDEX                           },
+    { PwrCmdWrite, 0x00000000, mmCGTS_TCC_DISABLE                         },
+    { PwrCmdWrite, 0x00000000, mmTCP_ADDR_CONFIG                          },
+    { PwrCmdWrite, 0x000000ff, mmTCP_ADDR_CONFIG                          },
+    { PwrCmdWrite, 0x76543210, mmTCP_CHAN_STEER_LO                        },
+    { PwrCmdWrite, 0xfedcba98, mmTCP_CHAN_STEER_HI                        },
+    { PwrCmdWrite, 0x00000000, mmDB_DEBUG2                                },
+    { PwrCmdWrite, 0x00000000, mmDB_DEBUG                                 },
+    { PwrCmdWrite, 0x00002b16, mmCP_QUEUE_THRESHOLDS                      },
+    { PwrCmdWrite, 0x00006030, mmCP_MEQ_THRESHOLDS                        },
+    { PwrCmdWrite, 0x01000104, mmSPI_CONFIG_CNTL_1                        },
+    { PwrCmdWrite, 0x98184020, mmPA_SC_FIFO_SIZE                          },
+    { PwrCmdWrite, 0x00000001, mmVGT_NUM_INSTANCES                        },
+    { PwrCmdWrite, 0x00000000, mmCP_PERFMON_CNTL                          },
+    { PwrCmdWrite, 0x01180000, mmSQ_CONFIG                                },
+    { PwrCmdWrite, 0x00000000, mmVGT_CACHE_INVALIDATION                   },
+    { PwrCmdWrite, 0x00000000, mmSQ_THREAD_TRACE_BASE                     },
+    { PwrCmdWrite, 0x0000df80, mmSQ_THREAD_TRACE_MASK                     },
+    { PwrCmdWrite, 0x02249249, mmSQ_THREAD_TRACE_MODE                     },
+    { PwrCmdWrite, 0x00000000, mmPA_SC_LINE_STIPPLE_STATE                 },
+    { PwrCmdWrite, 0x00000000, mmCB_PERFCOUNTER0_SELECT1                  },
+    { PwrCmdWrite, 0x06000100, mmCGTT_VGT_CLK_CTRL                        },
+    { PwrCmdWrite, 0x00000007, mmPA_CL_ENHANCE                            },
+    { PwrCmdWrite, 0x00000001, mmPA_SC_ENHANCE                            },
+    { PwrCmdWrite, 0x00ffffff, mmPA_SC_FORCE_EOV_MAX_CNTS                 },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000010, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000020, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000030, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000040, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000050, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000060, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000070, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000080, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000090, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000a0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000b0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000c0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000d0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000e0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x000000f0, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmRLC_PG_CNTL                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS2                             },
+    { PwrCmdWrite, 0x15000000, mmCP_ME_CNTL                               },
+    { PwrCmdWrite, 0x50000000, mmCP_MEC_CNTL                              },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x0000000e, mmSH_MEM_APE1_BASE                         },
+    { PwrCmdWrite, 0x0000020d, mmSH_MEM_APE1_LIMIT                        },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000320, mmSH_MEM_CONFIG                            },
+    { PwrCmdWrite, 0x00000000, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_RB_VMID                               },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmRLC_CNTL                                 },
+    { PwrCmdWrite, 0x00000000, mmRLC_CNTL                                 },
+    { PwrCmdWrite, 0x00000000, mmRLC_SRM_CNTL                             },
+    { PwrCmdWrite, 0x00000002, mmRLC_SRM_CNTL                             },
+    { PwrCmdWrite, 0x00000000, mmCP_ME_CNTL                               },
+    { PwrCmdWrite, 0x15000000, mmCP_ME_CNTL                               },
+    { PwrCmdWrite, 0x00000000, mmCP_MEC_CNTL                              },
+    { PwrCmdWrite, 0x50000000, mmCP_MEC_CNTL                              },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x0840800a, mmCP_RB0_CNTL                              },
+    { PwrCmdWrite, 0xf30fff0f, mmTCC_CTRL                                 },
+    { PwrCmdWrite, 0x00000002, mmTCC_EXE_DISABLE                          },
+    { PwrCmdWrite, 0x000000ff, mmTCP_ADDR_CONFIG                          },
+    { PwrCmdWrite, 0x540ff000, mmCP_CPC_IC_BASE_LO                        },
+    { PwrCmdWrite, 0x000000b4, mmCP_CPC_IC_BASE_HI                        },
+    { PwrCmdWrite, 0x00010000, mmCP_HYP_MEC1_UCODE_ADDR                   },
+    { PwrCmdWrite, 0x00041b75, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000710e8, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000910dd, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000a1081, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000b016f, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000c0e3c, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000d10ec, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000e0188, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00101b5d, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00150a6c, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00170c5e, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x001d0c8c, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x001e0cfe, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00221408, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00370d7b, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00390dcb, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x003c142f, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x003f0b27, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00400e63, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00500f62, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00460fa7, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00490fa7, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005811d4, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00680ad6, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00760b00, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00780b0c, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00790af7, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x007d1aba, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x007e1abe, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00591260, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005a12fb, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00861ac7, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x008c1b01, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x008d1b34, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a014b9, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a1152e, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a216fb, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a41890, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a31906, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a50b14, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00621387, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005c0b27, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00160a75, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC1_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00010000, mmCP_HYP_MEC2_UCODE_ADDR                   },
+    { PwrCmdWrite, 0x00041b75, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000710e8, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000910dd, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000a1081, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000b016f, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000c0e3c, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000d10ec, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000e0188, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00101b5d, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00150a6c, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00170c5e, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x001d0c8c, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x001e0cfe, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00221408, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00370d7b, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00390dcb, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x003c142f, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x003f0b27, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00400e63, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00500f62, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00460fa7, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00490fa7, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005811d4, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00680ad6, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00760b00, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00780b0c, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00790af7, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x007d1aba, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x007e1abe, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00591260, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005a12fb, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00861ac7, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x008c1b01, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x008d1b34, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a014b9, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a1152e, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a216fb, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a41890, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a31906, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00a50b14, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00621387, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x005c0b27, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x00160a75, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x000f016a, mmCP_HYP_MEC2_UCODE_DATA                   },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x540fe800, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0x7e000200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e020201, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e040204, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e060205, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a080500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a0a0303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbf810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54106f00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000400b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00004000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00804fac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x540fef00, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0xc0031502, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00001e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x540ff000, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000145, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdcc10000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd010000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd410000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080061, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24ccffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3cd08000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1cd0ffcf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d018001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x050c0019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x84c00000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000067, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000006a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000006d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000008f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000099, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800000a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800000af, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x388c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08880002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98800003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000002d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28080001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d808001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc180000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc080000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc0700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d10ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d10c017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0d000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd0130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14cc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c00036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000005d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14d00011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c01b10, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00e0080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000013b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00e0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000013b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x280c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00052, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28180039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x280c0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00052, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28180039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x280c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00052, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28180039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000069, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28080001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca88004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc00006f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000013b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28180080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d10c017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd0130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000013b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd4c0380, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdcc0388, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55dc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdcc038c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce0c0390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce0c0394, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce4c0398, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce4c039c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce8c03a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56a80020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce8c03a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcecc03a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcecc03ac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf0c03b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf0c03b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4c03b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57740020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4c03bc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8c03c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57b80020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8c03c4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfcc03c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57fc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfcc03cc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05dc002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc12009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d200a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc012009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25e01c00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25e40300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25e800c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25ec003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e25c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de5c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xddc10000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02ee000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31100006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc1c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4df0388, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d7038c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d5dc01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4e30390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d70394, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d62001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4e70398, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d7039c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d66401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4eb03a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d6a801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ef03a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703ac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d6ec01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4f303b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d73001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4f703b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703bc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d77401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4fb03c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703c4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d7b801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ff03c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d703cc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d7fc01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc080000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4d70380, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1c88001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0e0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc01e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3cd00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0085, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc006a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc01e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3cd00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc180000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc080000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1c88001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc180000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc080000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400051, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04180018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aac0027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80002f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04080002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08880001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000367, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9880fff3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04080010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08880001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80c0309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80c0319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9880fffc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00e0100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d4001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x155c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e80180, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202c003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000bfc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900091a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05280196, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d4fe04, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800001b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000032b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000350, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000352, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000035f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000701, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000047c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000019f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d98001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0044, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9400036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40005b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40005d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840006d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11540015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a4003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1998003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af0007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1264001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15dc000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d65400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a38003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd5c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7df1c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800045, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411326a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc415326b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293279, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000056, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00059, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c8000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40005a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29988000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000073, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25140fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001b6d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153279, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd00005f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000075, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26f00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15100010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d190004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af07fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001427, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4412e01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0434001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4412e40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04343000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf413267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd1c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0160, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc810001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b4c0057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b700213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b740199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f4f400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55180020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2198003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x248dfffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc12e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af4007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33740003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26d80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ae8003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26680001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253348, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413348, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253348, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x958000d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000315, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04303000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26680001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b342010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1714000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25540800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x459801b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d77400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x199c01e2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e4002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e5c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e540002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc80c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54d00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000282, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc80c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54d00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000282, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000282, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc80c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1334e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01334f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd413350, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813351, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd881334d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3271, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153274, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50cc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cdcc011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05900008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd00006a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0006b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3272, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d594002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54d00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc12e23, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd012e24, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc12e25, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15540002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b340057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b280213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980198, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e40020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20cc003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113274, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01e0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2d540002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x078c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07d40000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001239, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04f80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x057c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd5c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840007c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400069, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c018a6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4412e22, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800007c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c018a2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd4c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680fffc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800002e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800002e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000069, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9540188f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc013cfff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x38d00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdcc30000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c01882, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000304, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x49980198, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e40020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x459801a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04302000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000329, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc812e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04302000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16ec001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1998003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce00000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a18003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4093249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1888003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000671, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419324c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1598001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14d80011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24dc00ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31e00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31dc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c00036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95801827, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14dc0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800006d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51dc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a0000ad, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04080000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af4003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740004d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca88005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24880001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f4b4009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313274, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d33400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a4004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1eecffdd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800003c3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aa80030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a8004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3272, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19e80042, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e8e800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de9c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3271, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50cc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ce8c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd30011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11e80007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd300001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240059, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1660001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e320009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0328000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e72400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0430000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02ac000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d310002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa87600, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280222, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4280058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x22ec003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8380018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57b00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04343108, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13740008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2374007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32a80003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ec0057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e40213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc0199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cecc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ce4c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800003e7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980104, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x49980104, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800003f2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000448, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813279, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf41326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x254c0700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a641fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0726, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a640200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1237b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2264003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8813260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4280034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001427, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c01755, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde830000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0174c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bb80040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100044, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19180024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x551c003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000043d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c8000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840006c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28200000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000043f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x282000f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x195c00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2555fff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0360001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5e124dc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e624001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80fff9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02ee000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2555fff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980158, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x49980158, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980170, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16200010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x195400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1154000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e80488, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0006c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18f807f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e40077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ec0199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6e400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000048e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000494, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004de, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000685, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000686, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800006ac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ccc001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1264000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d79400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e7a400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52a8001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d69401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202c007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aec0028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004cc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419324e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26e8003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aec003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12f4000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d324d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d75401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d290004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f8f4001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f52800f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50e00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004d1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0dc002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x6665fc00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1c011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a644000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f534002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x6665fc00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e76401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004d7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aec003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3257, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12f4000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d75401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1c011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a644000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202c003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e804e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004e7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800004f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000505, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x277401ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf41325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000671, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640fff4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17e00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84131db, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b301ff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26edf000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8413260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05a80507, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000050c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000528, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000057d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800005c2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800005f3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000671, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bd400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c004d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d150005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00063b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2511fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000624, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1be00fe4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000066, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400068, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000671, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bd400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c004d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d150005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400067, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00063b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2511fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000624, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bd400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ed6c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113271, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193272, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d51401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113274, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253276, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400061, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2730000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7db1800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00062, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000063, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400065, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc820001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b700057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b680213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b740199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46ec0188, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17e00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26e01000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c131fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x191807e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x192007ec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1334a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x69dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de20014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x561c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013344, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13345, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425334d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419334e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d334f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213350, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253351, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b680057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b700213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b740199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46ec01b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800068, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2010007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1910003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd00001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd00001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2010003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x191807e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9540000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2511fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1334a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013344, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013345, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180050, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0052, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280042, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813273, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13275, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000068, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400067, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07d40000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00124f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x057c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b680057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b700213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b740199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc820001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46ec0190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2154003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bd800e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd80005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420004d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1e000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28340001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f598004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1be800e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801327a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800005f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000075, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424004c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a4004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x277401ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf41325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xda000068, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113277, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9540002d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1334a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425334d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419334e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d334f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213350, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253351, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b680057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b700213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b740199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46ec01b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1334a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1be000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0360001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc63124dc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e724001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80fff9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02ee000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fc14001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x194c1c03, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc0003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c002d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000697, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x194c00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c004c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27301fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce00005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cf0c00d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0007e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b301ff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25100007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31100005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900008e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000075e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202c007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a9feff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1374000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1774000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d30b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce813265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00ac006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00e0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28880700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0006de, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14cc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30d4000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41530b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19980028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800006c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15600008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8380023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fa38011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1a0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x282c2002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e280008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd3800025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202400d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca48001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d8003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x194c00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c004c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27301fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce00005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cf0c00d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000712, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x194c1c03, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc0003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c002d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e80714, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000071c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000720, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000747, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000071d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800007c4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000732, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000745, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000744, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000072e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0007e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a64008c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b301fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000075e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0fff1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0007e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000723, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41f02f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000743, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8813247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0ffde, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000072e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0007e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15600008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84131db, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b301ff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8413260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc8000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x195800e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd80005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418004c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dd7fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1a001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46200200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04283247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af80057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af40213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6f400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2000025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc6990000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x329c325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x329c3269, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x329c3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01defff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d8009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000078a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fff2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03e7ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3f0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1f30001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03e4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001219, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d30b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bf0003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000b80, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x203c003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf0130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46200008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2000025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31dc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ec0057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e40213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc0199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cecc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ce4c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000448, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31980002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19580066, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15600008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0120001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11980003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da18001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d24db, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580137b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00ee000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113269, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19080070, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x190c00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2518000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05a80809, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000080e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000080f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000898, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000946, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800009e1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04a80811, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000815, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000834, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000085e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000085e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04341001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3045, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c091, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31300021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84002f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293059, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56a8001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000241, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000084a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02f0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4252087, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5668001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000084a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04341001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431ecaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02e0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31300021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84002f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293059, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56a8001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00021d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd410000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84802e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001a41, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43b02f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec80278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56f00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001608, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8813247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80802e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000085e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x950001fa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02e0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aec0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a40006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de6000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10e40008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e2e000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d10ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2110003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d10ff9e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0245301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801325f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0121fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29108eff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e524009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0127ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e524009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0131fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e524009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013279, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000866, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000866, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0100010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd2400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0180003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd1c002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000866, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04a8089a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000089e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800008fa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000945, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000945, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31300022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04183000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d91801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x459801e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2738000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b342010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x172c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef7400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc79d3300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc7a13301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8393300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0260001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce793301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x964012a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c028009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800008d2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x242c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02620c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0260400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6e400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae8001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2f0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800008d2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdf93300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce393301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04182000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000903, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31240022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ec30011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32f80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x67180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bfc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd981325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000915, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c1325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0fff6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f818001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001606, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d838001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16240014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a2801f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e2a000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2264003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00075e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x66d80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1330000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13f40014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33e80010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680ffec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04a80948, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000094c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000099b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800009e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800009e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04183000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d91801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x459801e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2738000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b342010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x172c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef7400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc79d3300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc7a13301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8393300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0260001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce793301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x964011fe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c028009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000978, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x242c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0260010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0260800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6e400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae8001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2f0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000978, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdf93300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce393301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04182000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dda801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e838011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84802e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001802, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x469c0390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04183000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b342010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x172c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef7400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4280011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04182000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0014df, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31280014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce8802ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800062, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31280034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04a809e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800009ec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a45, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a59, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a59, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d91801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a70250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e72401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b342010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x172c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b30c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef7400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x66740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04383000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b38007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33b40003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x4598001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740002f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4002eb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4002ec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4002ed, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4002ee, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04382000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd84802e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001715, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04382000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffbc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04341001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431ecaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a55, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x233c0032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf0130b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49302ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8413247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5198001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193269, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2598000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80002f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53b8001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7db9801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000a5e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c01106, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e02, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e03, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c010fd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ce4c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc80c0072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x58e801fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc01e2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e4002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e5c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e540002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9540000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x44cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55900020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x44cc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd812e01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd012e02, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd412e03, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2264003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1e64001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14d00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ab1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a0010ac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd880003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010de, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc010ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d403f7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0cc009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41b0367, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d958004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d85800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc1e0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d001fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05280adc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000af1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000adf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ae7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000ace, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd8d2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d803f7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc010ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0cc009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11940014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29544001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29544003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000af4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd44d2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd44dc000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000ace, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd8d2c00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000b0a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd44d2c00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28148004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4593240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0105e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2198003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x199c0034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef3400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14e80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a8000af, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c01043, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a01fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3620005c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2464003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc6290ce7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16ac001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ac003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ee6c00d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00fff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000367, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640102e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x199c0037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a00035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0005d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16f8001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9780000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc035f0ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e764009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19b401f8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13740008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e76400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x199c0034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ae4003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000b7c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aec003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a4003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12ec001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1374000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02e4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1774000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc01e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13fc0018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dbd800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d98ff15, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x592c00fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd80000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12e00016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x592c007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12e00015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1264001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1620000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e32000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12e4001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5924007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a4003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013257, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd413258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00fdb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9780f5ca, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001219, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001b6d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d324e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431324d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07740003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x269c003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e4004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f67000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f674002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53740002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab42010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab8c006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a8000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b740000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf40001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000bec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000b47, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b34060b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec00ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a8004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc415325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18580037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x262001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d54001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14f00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd280200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd680208, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcda80210, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc6930200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc6970208, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc69b0210, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd900003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd940003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9400040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010de, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14fc0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24f800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd88130b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d83c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4093249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1888003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000671, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419324c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1598001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14d80011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24e000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x321c0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580ffee, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c30, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9480000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800f29, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800f23, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c00036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800f1a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9600f502, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0f500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000f05, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1f30001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16e4001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640f4f4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33740002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40f4f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aec003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12ec001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1374000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02e4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1774000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00ac005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00e0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc8000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28884900, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ff3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400ee1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c40a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c40c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c40d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d0007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15580010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x255400ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c411, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c40f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c40e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c410, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e80033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ec0034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c414, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c415, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c413, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c412, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc0032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c030011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c038011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431c417, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435c416, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c419, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc418, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf413261, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013262, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13263, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813264, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc0030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d77000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000cd6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51b80020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f97801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000cd6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ca7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc0031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435c40b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4280032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012c2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f8cc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13f4000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bf0060b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000cf4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0677, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13fc0017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc032800b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb7800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ffbc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d42011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d001e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd4c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800e6c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d59401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x596001fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ce0c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x505c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50600020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8240010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e800c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x122c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000d1f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8240010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x566c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413261, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13262, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b740008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x566c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce413261, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec13262, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012c2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f8cc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13f4000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bf0060b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000d57, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0677, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13fc0017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0328009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb7800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ffbc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04143000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd413267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e51001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d2d0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19640057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19580213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19600199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da6400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04142000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd413267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4153267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d001e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d40030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d80034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05280d83, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c424001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000d8a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000d95, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000db1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000d95, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000dbc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11540010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e010001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00187c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d75400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4610000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580f3d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x526c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e80058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e2ec01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc82c0072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5ae0073a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea2800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580f3c6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc3a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980fff5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02a0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01c405, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd441c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580f3b1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c409, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11540010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4610000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580f3a5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00da7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50500020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8280072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5aac007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12d80017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56a00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e82400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e58c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19d4003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28182002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00104f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc011000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c908009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d614011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1a0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb0800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e280008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20880188, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20240090, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca48001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c018001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd901a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1624001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841325f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00038, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ac0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ac0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13f4000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b301ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2330003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00038, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0001a2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc80003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24b00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1330000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ac0024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b304000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a800e5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da9800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1910003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2220003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e2a000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00038, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d40030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d001e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18fc0034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24e8000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80e71, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000edd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e91, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e91, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ea1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000eaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e7c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e7f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e7f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e87, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000e8f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51dc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9e001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a200008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213262, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253261, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a200008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213264, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253263, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc820001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e82005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da1801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1800072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8180072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x59a001fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea2800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce80001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd180001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15980002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421c401, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425c401, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ee6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2580, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac260c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0828, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2440, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0093, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac31dc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac31e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ede, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac7c06, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db07c00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acc337, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0c330, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acc335, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0c336, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac9002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db09001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac9012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db09011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acec70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0ec6f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ebc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5a10000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5a50000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05280eea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ef1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000efe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f11, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f2e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000efe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f1f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce190000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce190000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0f26f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e80058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7daec01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc82c0072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5af8073a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eba800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0f25c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02a0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15980002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c405, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56240020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0f24e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c409, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40f247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce190000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce190000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0f240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439c040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2580, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac260c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0828, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2440, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac2390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac0093, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac31dc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31ac31e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ef2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac7c06, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db07c00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acc337, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0c330, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acc335, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0c336, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39acec70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db0ec6f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac9002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db09002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39ac9012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3db09012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ef1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b740008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c1325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c034001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c038001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e0007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32240003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f88, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e52401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8280072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ac0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26f0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af000fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1334000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24e02000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f63400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e00074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32240003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000f9d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51e40020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5a401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8280072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ac0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26f0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1af000fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13380016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e00039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fa3800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb7800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e0007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1220001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fa3800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e00074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fa3800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1c084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d001e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31140005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31140006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00104f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05280fb7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28140002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fbe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fbe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fc2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fbe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fd1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ff2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ff2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e80039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52a8003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d59401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d69401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140004b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d958004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d150005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x159c0011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31a00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31a40001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e25800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0fff5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fff4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000fef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d100010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01326f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc011000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33b40003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0340008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000ffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c908009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d614011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1a0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb0800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x282c2002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x208801a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e280008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20240030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca48001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28340000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x507c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d7d401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x557c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28342002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000102f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c018001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1cccfe08, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00b33, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da2400f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da28002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1ac002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d2ac002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3ef40010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40f11d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde410000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdcc10000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd010000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd410000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdd810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xddc10000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde010000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c024001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100086, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5510003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001075, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15800f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15c002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d520002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cde0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e20001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001071, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c00036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00b01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc200000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc1c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc180000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc0c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc240000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc40003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4080029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc80003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a800e5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da9800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a400e5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12500009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x248c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x200c006d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x200c0228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410002b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18881fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d4072c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc00d1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd4c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3094000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x38d80000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x311c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30940007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1620001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010c4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x259c007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a00030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010cb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x199c0fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010cb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000aac, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434002e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2020002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07a810d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000bfc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000104c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x200c007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28240007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xde430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc80003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24b00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1330000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a800e5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da9800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d3249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b304000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x192400fd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d59401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06681110, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ac0024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19180070, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19100078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18f40058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5978073a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001117, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001118, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001122, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000112d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001130, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001133, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000117b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24ec0f00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32ec0600, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000117b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24ec0f00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32ec0600, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000117b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001122, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc81c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55e00020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001122, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02a0200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e8e8009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x22a8003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x22a80074, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2774001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13740014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eb6800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25ecffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55700020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15f40010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13740002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x275c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c018001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15dc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39e00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc1c01e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e40008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc2001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e40008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e62000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da58001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001165, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc2001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1a0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e0d000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95000007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e02401e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06640008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05d80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc2401e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da58001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05e00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da2000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9600ffe6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00116b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce00001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81c078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x22640435, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0528117e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x312c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001185, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001182, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001182, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a0400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1198001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d81c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19a000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de2c00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26200010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc415326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc420007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011a3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d654001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c020001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011b6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253279, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc415326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2730003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3b380006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3f38000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0430000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb10004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e57000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e578002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d67c002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0be40001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d3a4002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x202c002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26200010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e640010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce81325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434002e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07a811cf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00feb8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x954009a7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000bfc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1c07c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c07d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c08c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c07e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18f0012f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18f40612, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc00c1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cf7400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x39600004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0140004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11600001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18fc003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9740001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x166c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011ee, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a6c003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800011e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ac007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab00030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aac0fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001205, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x166c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11600001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ffbc00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a2800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c07c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c08c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03ae000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a0800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c07c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c08c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03ae000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a4000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81c07c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c08c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17fc001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30d00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000052, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640090f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1514001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19180038, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d324e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431324d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab0c006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000127f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d3258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313257, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab0c012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e624004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f67800f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53740002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ab42010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a8000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b740000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f6b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf40001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1514001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0012e1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x964008d7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9800036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300677, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012aa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b34060b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f37000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec00ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a8002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef6c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7edec00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4140032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1858003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0cc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0006c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d407f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2598003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d190004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d5d4001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d52000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d514002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d958001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd5c002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc1325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1ccc001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14f00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd980003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9800040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd9c00040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800010de, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33f80003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800051, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc80003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24b00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1330000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18a800e5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1d980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7da9800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b74003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b304000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431326c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b4c00f8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50700020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04e81324, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18ac0024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50600020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30e40004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d71401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x596401fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b74008d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e76400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a640000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000132c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000133b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001344, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42530b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a68003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2024003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11980014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d19000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd0130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce4130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de6800f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce40001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc428000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8240011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de6800f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffe0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00104f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28182002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340035, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140023, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d614011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4100026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05980008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1a0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb0800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3e280008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cb4800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20240030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ca48001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b4c00f8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28340000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x507c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30e40004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d7d401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x557c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28342002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c018001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf81a2a4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c007eb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50500020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d0d001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8100072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x591c01fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45140210, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x595801fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11980009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1624001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400069, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a307fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x23304076, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc00e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0015, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x4514020c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a2001e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a204001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a64003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1264001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15dc000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dcdc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5dc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001427, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340022, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4412e01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0434001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf430000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4412e40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c030, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41c031, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x248dfffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc12e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc812e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45140248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013257, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0434000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdb000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd140001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9980ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8200011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013259, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0337fff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f220009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d01c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f01c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c01c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c000d61, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50500020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001427, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd0c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd0c00072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8240072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd240001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19682011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5a6c01fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12ec0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eeac00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aec0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4180011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf830000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfa0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4380007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d40038, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400029, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9540073d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18c80066, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30880001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00187c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd910000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x4220000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24e80007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24ec0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5310000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001465, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1000072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc82c0072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2c0001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18f02011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5aec01fc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12ec0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aec0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a8146a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1f0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001478, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001478, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000147a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000147a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000147a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f1b400e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000147a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f334002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000147b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e024001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000144a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fbfc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x251001ef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94800007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00187c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42c0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd910000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40d325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800012c2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13f4000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bf0060b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800014a9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d325a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0677, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb81ff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0328007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb7800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13fc0017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ffbc00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03a0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45dc0390, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04183000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b380057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b340213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c424001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c428001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c42c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c430001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c434001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04182000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd813267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a0800fd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x109c000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce080228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9880000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce480250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce880258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0ec75, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc80230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce480250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce880258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52a80020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x66580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc80260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec80288, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf080290, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec80298, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf0802a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4802a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27580001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc802b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80802b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x178c000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b8003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cf8c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8802c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc802c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8802d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf8802d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bc800ea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25b8ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd2800c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5230309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e3a400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001539, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd08034b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd880353, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00163f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b0353, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd14005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000154f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd080238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd08034b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2598ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d200008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc80230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd900309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8100319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340801, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2198003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd910ce7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4190ce6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d918005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d918004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd810ce6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdd1054f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000156e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x090c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdcd050e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x040c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x110c0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc4001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41230a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41230b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41230c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc41230d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc480329, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc48032a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc4802e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f02e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d8003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09940001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x44100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x69100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000157f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970290, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b0288, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d59401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b02a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49f0298, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x041c0040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dcdc002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d924019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d26400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00163f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001579, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d010021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d914019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd480298, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd8802a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10d40010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12180016, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc51f0309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d95800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d62000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdd00309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce113320, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f02e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b02b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18dc01e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9400e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c0001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00163f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015aa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a302b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12240004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e5e400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ab02a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04100000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce4c0319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d9d8002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea14005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015bc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04240001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e624004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d25000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2620000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fff4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd0d3330, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce0802b8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd8802b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ab02e0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aa807f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f02d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49702d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b02c8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49f02c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96800028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d4e000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9600000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d964002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d694001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cde4002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de94001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd64002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d694001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00163f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800015cd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930238, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d698002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd4802d8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x129c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc50f0319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11a0000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1e000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1198000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd953300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e0e000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a8000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce953301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce100319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b70280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73800a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x536c0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9780eb68, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001608, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001609, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30b40000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b400011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b70258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53780020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb3801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7faf8019, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x67b40001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x57b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bb0260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fab8001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf880260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x66f40001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4353247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7f4009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fff7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x269c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a00018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a00060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x269c0018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a40060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11dc0006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29dc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de5c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b70228, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc80230, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f514005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2510000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001644, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd080240, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f130005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001688, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00120d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001219, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001232, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340801, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f130004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01051e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42d051f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ed2c005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96c0fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01051f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000055, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5170309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x195c07f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x196007f6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04340001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x6b740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001665, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a702a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ab0298, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f634014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e76401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56680020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8113320, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce480298, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce8802a0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc5170319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b702b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x255c000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f5f4001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8113330, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf4802b0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11340001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x195c07e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x196007ee, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8353300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1e4001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8353301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce4802d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8100309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8100319, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc48f0250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd4c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x64d80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580005c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc24001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd2000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc435324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7df5c00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800016f1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc033ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2f3000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a7003e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13b80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a7000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07b80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a700064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800016df, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb30002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4392083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffca, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2030007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800016f2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ff9c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001608, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bc800ea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd80802e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18fc0064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00042, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51980020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dd9801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x45980400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b380057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b340213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f7b400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f73400a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14f4001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bf02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x192807fa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bf0258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a70250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53fc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e7e401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x667c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0aec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eebc00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x43300007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7db30011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd3000025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc03ec005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfca200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x192807fa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01f007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1d0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2110007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x203c003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0017f5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18fc01e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00185b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8413247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40ffd5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bf02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0ea24, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14d4001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d52400e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49f0258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a30250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51dc0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400017, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d534002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dae4005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32e0001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec80270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000174f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b740001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00178a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40fff3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001608, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ab0268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7daa4005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32a0001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001765, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01f007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d1d0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2110007d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8013256, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c0017f2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd013254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4113248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b3034b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f13000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001855, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32a4001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8413247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd080260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce880268, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ffc0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ec28001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32e0001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253255, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431324f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e72400c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9680fff7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aa4003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aa400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32680003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a800046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4293260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1aa400e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800017e2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc027ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2e6400ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a4009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4240009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19e403e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26680003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12a80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea68001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19e400e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea68001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea68001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19e40064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x32640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a40005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06640003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce412082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a640003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800017d0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16a40005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce412082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea64002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4292083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ea68005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a80ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc429325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26a400ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40ffca, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2024007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800017e3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4a70280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4ab0278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52640020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7eae8014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e6a401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56680020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce480278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce880280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x042c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec80270, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800017fe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bf02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800017fe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43b02eb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42302ec, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fa3801a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x47b8020c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x15e00008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1220000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2a206032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x513c001e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e3e001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4bf02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000180f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b3c0077, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1330000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd200000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4200007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd3800002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc30001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc1e0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04380032, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf80000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001427, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc413248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3269, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33fc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdfc30000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bfc0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdfc30000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd441326a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x173c0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300303, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3f0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ff3c004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001842, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdfc30000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4413249, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c43c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x23fc003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1326d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdf830000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd441326e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1fb8ffc6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xddc30000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf813265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001852, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c00142b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13252, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013253, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001878, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49f02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c00018, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c420001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13252, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013253, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c3000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c0012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001878, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41f02ed, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42302ee, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc13252, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013253, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e2a0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28340001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x313c0bcc, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x393c051f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d3c050e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x393c0560, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d3c054f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x393c1538, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d3c1537, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b740800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bc800ea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e8007c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c42c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a8189a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000189e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800018c5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800018f2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c414001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0007e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x50580020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d59401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc8140072, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09240002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c418001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4340004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc42130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a24002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2020002c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc418000d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1198001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14cc0004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7cd8c00a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc130b7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce0130b5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd1400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x5978073a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bb80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf800024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd800026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9600e8a8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9640e8a5, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800018a9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc55b0309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d5c0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2598ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dad800c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0ffd2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fff9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x442c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x65180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7df9c00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c13260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd901325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940fff1, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x66d80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x56ec0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26240007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940fff7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000189e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc023007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19e4003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7de1c009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dee000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96000007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c13260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd901325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc421325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x261c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000189e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940fff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000189e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28cc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43d3265, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bc800ea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18e00064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06281911, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14f4001d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24cc0003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x86800000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001915, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x800019af, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001a2b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8000016a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc48032b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc480333, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc48033b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc480343, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98800011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46640400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04203000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b3c0057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b200213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e3e000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e32000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04180000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f438001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00068, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213254, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a1c003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00065, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc01f007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1e0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97800062, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0bb80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x43bc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fcbc001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc7df032b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1fc00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0101, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c0102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001994, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc033ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2f3000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f003e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13b80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07b80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f00064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001982, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb30002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4392083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffcb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2030007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001995, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc1325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98800009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x41bc0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x53fc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e7fc011, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd3c00025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0012, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9bc0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x653c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dbd8001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ff8f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2bfc0008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x043c2000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcfc13267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c410001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc55b0309, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x3d5c0010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2598ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x05540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d91800c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580fff8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09780001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x65180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9580005d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200101, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400058, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dc24001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41d3248, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25dc000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7df9c00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95c00053, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e41c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a70003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a7000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33240003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a400046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1a7000e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001a21, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc033ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2f3000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f270009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x266400ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f003e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27240003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12640004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e724001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e724001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e724001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06640002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f00064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16700005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001a0f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x16700005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e730002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4252083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e724005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x26640001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a40ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x267000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffca, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2030007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001a22, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ff9f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001a31, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8080280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213246, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4253245, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52200020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e26401a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x46640400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04203000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4213267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b180057, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b200213, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1b300199, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e1a000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e32000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce000024, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4970258, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4930250, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x51540020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4af0280, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4b30278, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x52ec0020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140020, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04280000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x65180001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800060, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x8c001628, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4193247, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x25980001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200101, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x30f00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95800056, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb0003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b800046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4393260, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bb000e4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001aa2, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc033ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2f3000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f3b0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf01325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b800ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4300009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9700fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f003e6, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27380003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13b80004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f000e8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07b80002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x19f00064, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33300002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0b300003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001a90, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x17b00005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf012082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01203f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x13300005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb30002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4392083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7fb38005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b80ffdf, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c00034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc00013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc431325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27300010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc439325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27b000ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b00ffca, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2030007b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf00325b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001aa3, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce01325d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04300001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7f2b0014, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ef2c01a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd2400025, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x4664001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000026, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400027, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x06a80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55100001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ff9c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc49b02e9, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99800008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc430000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2b300008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf000013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04302000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcf013267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc4313267, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x244c00ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc4c0200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc44f0200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc410000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d158010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x059cc000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccdd0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0037, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000049, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c003a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500e69a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d0003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d40021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd840004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c003c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x14cc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c00028, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000033, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc438000b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0009, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x27fc0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43dc07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1bfc0078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7ffbc00c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x97c0fffd, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x99000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0120840, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x282c0040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001ae8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0121841, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x282c001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c07c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c08c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c079, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c07e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcec0001b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a200001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9a00ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x166c001f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04200004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9ac0fffb, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc434000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9b40ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc425c07f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8000034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940e66b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800004a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0036, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9900fffe, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18cc0021, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc00047, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc000046, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0039, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c003d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c40c001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24d003ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d47fea, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x18d87ff4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd00004c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd40004e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd80004d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c405, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc02a0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2aa80001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c406, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc414000e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x29540008, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x295c0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8c1325e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcdc0001a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11980002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x4110000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0160800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7d15000a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0164010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c080, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c081, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c082, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc01c083, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c084, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x98c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400048, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c003b, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x94c0ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000c16, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c40a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd901c40d, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c410, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c40e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd801c40f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc40c0040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x09540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9940ffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04140096, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8400013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1c400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc411c401, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9500fffa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424003e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04d00001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x11100002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd01c40c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0180034, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd81c411, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd841c414, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0a540001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcd41c412, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x2468000f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc419c416, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x41980003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc41c003f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7dda0001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x12200002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x10cc0002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xccc1c40c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd901c411, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce41c412, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd8800013, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xce292e40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e01, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e02, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e03, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc412e00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000aa7, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc43c0007, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc120000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x31144000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x95400005, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xdc030000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd800002a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xcc3c000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b70, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x33f80003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd4400078, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x9780e601, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x188cfff0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x04e40002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001190, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400006, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x90000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc424005e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x96400003, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7c408001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x88000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80001b74, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000168, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110501, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120206, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130703, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92100400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92110105, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92120602, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x92130307, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbf810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x54106500, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0x7e000200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e020204, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc00a0505, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbf8c007f, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb8900904, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb8911a04, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb8920304, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb8930b44, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x921c0d0c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x921c1c13, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x921d0c12, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x811c1d1c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x811c111c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x921cff1c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000400, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x921dff10, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000100, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x81181d1c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e040218, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0701000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050102, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xe0501000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80050302, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbf810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x54106900, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0x7e080200, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x7e100204, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbefc00ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00010000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x24200087, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x262200ff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000001f0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x20222282, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x28182111, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000040c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd81a0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000080c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xd86c0000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x1100000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xbf810000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x80000004, mmCP_DFY_CNTL                              },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_ADDR_HI                           },
+    { PwrCmdWrite, 0x54116f00, mmCP_DFY_ADDR_LO                           },
+    { PwrCmdWrite, 0xc0310800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4540fe8, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000041, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000000c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07808000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54116f00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00005301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4540fef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee20, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0310800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb454105e, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000c0, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07808000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54117300, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00005301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4540fef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee20, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0310800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4541065, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000500, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000001c, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07808000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54117700, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00005301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4540fef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee20, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xc0310800, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000040, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4541069, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000444, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x0000008a, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x07808000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xffffffff, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000002, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xaaaaaaaa, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x55555555, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee40, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000010, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000001, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000004, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x54117b00, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00005301, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0xb4540fef, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x540fee20, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x000000b4, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x08000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_DFY_DATA_0                            },
+    { PwrCmdWrite, 0x00000000, mmCP_MEC_CNTL                              },
+    { PwrCmdWrite, 0x00000000, mmCP_MEC_CNTL                              },
+    { PwrCmdWrite, 0x00000004, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x54116f00, mmCP_MQD_BASE_ADDR                         },
+    { PwrCmdWrite, 0x000000b4, mmCP_MQD_BASE_ADDR_HI                      },
+    { PwrCmdWrite, 0xb4540fef, mmCP_HQD_PQ_BASE                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_BASE_HI                        },
+    { PwrCmdWrite, 0x540fee20, mmCP_HQD_PQ_WPTR_POLL_ADDR                 },
+    { PwrCmdWrite, 0x000000b4, mmCP_HQD_PQ_WPTR_POLL_ADDR_HI              },
+    { PwrCmdWrite, 0x00005301, mmCP_HQD_PERSISTENT_STATE                  },
+    { PwrCmdWrite, 0x00010000, mmCP_HQD_VMID                              },
+    { PwrCmdWrite, 0xc8318509, mmCP_HQD_PQ_CONTROL                        },
+    { PwrCmdWrite, 0x00000005, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x54117300, mmCP_MQD_BASE_ADDR                         },
+    { PwrCmdWrite, 0x000000b4, mmCP_MQD_BASE_ADDR_HI                      },
+    { PwrCmdWrite, 0xb4540fef, mmCP_HQD_PQ_BASE                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_BASE_HI                        },
+    { PwrCmdWrite, 0x540fee20, mmCP_HQD_PQ_WPTR_POLL_ADDR                 },
+    { PwrCmdWrite, 0x000000b4, mmCP_HQD_PQ_WPTR_POLL_ADDR_HI              },
+    { PwrCmdWrite, 0x00005301, mmCP_HQD_PERSISTENT_STATE                  },
+    { PwrCmdWrite, 0x00010000, mmCP_HQD_VMID                              },
+    { PwrCmdWrite, 0xc8318509, mmCP_HQD_PQ_CONTROL                        },
+    { PwrCmdWrite, 0x00000006, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x54117700, mmCP_MQD_BASE_ADDR                         },
+    { PwrCmdWrite, 0x000000b4, mmCP_MQD_BASE_ADDR_HI                      },
+    { PwrCmdWrite, 0xb4540fef, mmCP_HQD_PQ_BASE                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_BASE_HI                        },
+    { PwrCmdWrite, 0x540fee20, mmCP_HQD_PQ_WPTR_POLL_ADDR                 },
+    { PwrCmdWrite, 0x000000b4, mmCP_HQD_PQ_WPTR_POLL_ADDR_HI              },
+    { PwrCmdWrite, 0x00005301, mmCP_HQD_PERSISTENT_STATE                  },
+    { PwrCmdWrite, 0x00010000, mmCP_HQD_VMID                              },
+    { PwrCmdWrite, 0xc8318509, mmCP_HQD_PQ_CONTROL                        },
+    { PwrCmdWrite, 0x00000007, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x54117b00, mmCP_MQD_BASE_ADDR                         },
+    { PwrCmdWrite, 0x000000b4, mmCP_MQD_BASE_ADDR_HI                      },
+    { PwrCmdWrite, 0xb4540fef, mmCP_HQD_PQ_BASE                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_BASE_HI                        },
+    { PwrCmdWrite, 0x540fee20, mmCP_HQD_PQ_WPTR_POLL_ADDR                 },
+    { PwrCmdWrite, 0x000000b4, mmCP_HQD_PQ_WPTR_POLL_ADDR_HI              },
+    { PwrCmdWrite, 0x00005301, mmCP_HQD_PERSISTENT_STATE                  },
+    { PwrCmdWrite, 0x00010000, mmCP_HQD_VMID                              },
+    { PwrCmdWrite, 0xc8318509, mmCP_HQD_PQ_CONTROL                        },
+    { PwrCmdWrite, 0x00000004, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000104, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000204, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000304, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000404, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000504, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000604, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000704, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000005, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000105, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000205, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000305, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000405, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000505, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000605, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000705, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000006, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000106, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000206, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000306, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000406, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000506, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000606, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000706, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000007, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000107, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000207, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000307, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000407, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000507, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000607, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000707, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000008, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000108, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000208, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000308, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000408, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000508, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000608, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000708, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000009, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000109, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000209, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000309, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000409, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000509, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000609, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000709, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_RPTR                           },
+    { PwrCmdWrite, 0x00000000, mmCP_HQD_PQ_WPTR                           },
+    { PwrCmdWrite, 0x00000001, mmCP_HQD_ACTIVE                            },
+    { PwrCmdWrite, 0x00000004, mmSRBM_GFX_CNTL                            },
+    { PwrCmdWrite, 0x01010101, mmCP_PQ_WPTR_POLL_CNTL1                    },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdWrite, 0x00000000, mmGRBM_STATUS                              },
+    { PwrCmdEnd,   0x00000000, 0x00000000                                 },
+};
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/hardwaremanager.h b/drivers/gpu/drm/amd/powerplay/inc/hardwaremanager.h
new file mode 100644
index 000000000000..91795efe1336
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/hardwaremanager.h
@@ -0,0 +1,385 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _HARDWARE_MANAGER_H_
+#define _HARDWARE_MANAGER_H_
+
+
+
+struct pp_hwmgr;
+struct pp_hw_power_state;
+struct pp_power_state;
+enum amd_dpm_forced_level;
+struct PP_TemperatureRange;
+
+struct phm_fan_speed_info {
+	uint32_t min_percent;
+	uint32_t max_percent;
+	uint32_t min_rpm;
+	uint32_t max_rpm;
+	bool supports_percent_read;
+	bool supports_percent_write;
+	bool supports_rpm_read;
+	bool supports_rpm_write;
+};
+
+/* Automatic Power State Throttling */
+enum PHM_AutoThrottleSource
+{
+    PHM_AutoThrottleSource_Thermal,
+    PHM_AutoThrottleSource_External
+};
+
+typedef enum PHM_AutoThrottleSource PHM_AutoThrottleSource;
+
+enum phm_platform_caps {
+	PHM_PlatformCaps_AtomBiosPpV1 = 0,
+	PHM_PlatformCaps_PowerPlaySupport,
+	PHM_PlatformCaps_ACOverdriveSupport,
+	PHM_PlatformCaps_BacklightSupport,
+	PHM_PlatformCaps_ThermalController,
+	PHM_PlatformCaps_BiosPowerSourceControl,
+	PHM_PlatformCaps_DisableVoltageTransition,
+	PHM_PlatformCaps_DisableEngineTransition,
+	PHM_PlatformCaps_DisableMemoryTransition,
+	PHM_PlatformCaps_DynamicPowerManagement,
+	PHM_PlatformCaps_EnableASPML0s,
+	PHM_PlatformCaps_EnableASPML1,
+	PHM_PlatformCaps_OD5inACSupport,
+	PHM_PlatformCaps_OD5inDCSupport,
+	PHM_PlatformCaps_SoftStateOD5,
+	PHM_PlatformCaps_NoOD5Support,
+	PHM_PlatformCaps_ContinuousHardwarePerformanceRange,
+	PHM_PlatformCaps_ActivityReporting,
+	PHM_PlatformCaps_EnableBackbias,
+	PHM_PlatformCaps_OverdriveDisabledByPowerBudget,
+	PHM_PlatformCaps_ShowPowerBudgetWarning,
+	PHM_PlatformCaps_PowerBudgetWaiverAvailable,
+	PHM_PlatformCaps_GFXClockGatingSupport,
+	PHM_PlatformCaps_MMClockGatingSupport,
+	PHM_PlatformCaps_AutomaticDCTransition,
+	PHM_PlatformCaps_GeminiPrimary,
+	PHM_PlatformCaps_MemorySpreadSpectrumSupport,
+	PHM_PlatformCaps_EngineSpreadSpectrumSupport,
+	PHM_PlatformCaps_StepVddc,
+	PHM_PlatformCaps_DynamicPCIEGen2Support,
+	PHM_PlatformCaps_SMC,
+	PHM_PlatformCaps_FaultyInternalThermalReading,          /* Internal thermal controller reports faulty temperature value when DAC2 is active */
+	PHM_PlatformCaps_EnableVoltageControl,                  /* indicates voltage can be controlled */
+	PHM_PlatformCaps_EnableSideportControl,                 /* indicates Sideport can be controlled */
+	PHM_PlatformCaps_VideoPlaybackEEUNotification,          /* indicates EEU notification of video start/stop is required */
+	PHM_PlatformCaps_TurnOffPll_ASPML1,                     /* PCIE Turn Off PLL in ASPM L1 */
+	PHM_PlatformCaps_EnableHTLinkControl,                   /* indicates HT Link can be controlled by ACPI or CLMC overrided/automated mode. */
+	PHM_PlatformCaps_PerformanceStateOnly,                  /* indicates only performance power state to be used on current system. */
+	PHM_PlatformCaps_ExclusiveModeAlwaysHigh,               /* In Exclusive (3D) mode always stay in High state. */
+	PHM_PlatformCaps_DisableMGClockGating,                  /* to disable Medium Grain Clock Gating or not */
+	PHM_PlatformCaps_DisableMGCGTSSM,                       /* TO disable Medium Grain Clock Gating Shader Complex control */
+	PHM_PlatformCaps_UVDAlwaysHigh,                         /* In UVD mode always stay in High state */
+	PHM_PlatformCaps_DisablePowerGating,                    /* to disable power gating */
+	PHM_PlatformCaps_CustomThermalPolicy,                   /* indicates only performance power state to be used on current system. */
+	PHM_PlatformCaps_StayInBootState,                       /* Stay in Boot State, do not do clock/voltage or PCIe Lane and Gen switching (RV7xx and up). */
+	PHM_PlatformCaps_SMCAllowSeparateSWThermalState,        /* SMC use separate SW thermal state, instead of the default SMC thermal policy. */
+	PHM_PlatformCaps_MultiUVDStateSupport,                  /* Powerplay state table supports multi UVD states. */
+	PHM_PlatformCaps_EnableSCLKDeepSleepForUVD,             /* With HW ECOs, we don't need to disable SCLK Deep Sleep for UVD state. */
+	PHM_PlatformCaps_EnableMCUHTLinkControl,                /* Enable HT link control by MCU */
+	PHM_PlatformCaps_ABM,                                   /* ABM support.*/
+	PHM_PlatformCaps_KongThermalPolicy,                     /* A thermal policy specific for Kong */
+	PHM_PlatformCaps_SwitchVDDNB,                           /* if the users want to switch VDDNB */
+	PHM_PlatformCaps_ULPS,                                  /* support ULPS mode either through ACPI state or ULPS state */
+	PHM_PlatformCaps_NativeULPS,                            /* hardware capable of ULPS state (other than through the ACPI state) */
+	PHM_PlatformCaps_EnableMVDDControl,                     /* indicates that memory voltage can be controlled */
+	PHM_PlatformCaps_ControlVDDCI,                          /* Control VDDCI separately from VDDC. */
+	PHM_PlatformCaps_DisableDCODT,                          /* indicates if DC ODT apply or not */
+	PHM_PlatformCaps_DynamicACTiming,                       /* if the SMC dynamically re-programs MC SEQ register values */
+	PHM_PlatformCaps_EnableThermalIntByGPIO,                /* enable throttle control through GPIO */
+	PHM_PlatformCaps_BootStateOnAlert,                      /* Go to boot state on alerts, e.g. on an AC->DC transition. */
+	PHM_PlatformCaps_DontWaitForVBlankOnAlert,              /* Do NOT wait for VBLANK during an alert (e.g. AC->DC transition). */
+	PHM_PlatformCaps_Force3DClockSupport,                   /* indicates if the platform supports force 3D clock. */
+	PHM_PlatformCaps_MicrocodeFanControl,                   /* Fan is controlled by the SMC microcode. */
+	PHM_PlatformCaps_AdjustUVDPriorityForSP,
+	PHM_PlatformCaps_DisableLightSleep,                     /* Light sleep for evergreen family. */
+	PHM_PlatformCaps_DisableMCLS,                           /* MC Light sleep */
+	PHM_PlatformCaps_RegulatorHot,                          /* Enable throttling on 'regulator hot' events. */
+	PHM_PlatformCaps_BACO,                                  /* Support Bus Alive Chip Off mode */
+	PHM_PlatformCaps_DisableDPM,                            /* Disable DPM, supported from Llano */
+	PHM_PlatformCaps_DynamicM3Arbiter,                      /* support dynamically change m3 arbitor parameters */
+	PHM_PlatformCaps_SclkDeepSleep,                         /* support sclk deep sleep */
+	PHM_PlatformCaps_DynamicPatchPowerState,                /* this ASIC supports to patch power state dynamically */
+	PHM_PlatformCaps_ThermalAutoThrottling,                 /* enabling auto thermal throttling, */
+	PHM_PlatformCaps_SumoThermalPolicy,                     /* A thermal policy specific for Sumo */
+	PHM_PlatformCaps_PCIEPerformanceRequest,                /* support to change RC voltage */
+	PHM_PlatformCaps_BLControlledByGPU,                     /* support varibright */
+	PHM_PlatformCaps_PowerContainment,                      /* support DPM2 power containment (AKA TDP clamping) */
+	PHM_PlatformCaps_SQRamping,                             /* support DPM2 SQ power throttle */
+	PHM_PlatformCaps_CAC,                                   /* support Capacitance * Activity power estimation */
+	PHM_PlatformCaps_NIChipsets,                            /* Northern Island and beyond chipsets */
+	PHM_PlatformCaps_TrinityChipsets,                       /* Trinity chipset */
+	PHM_PlatformCaps_EvergreenChipsets,                     /* Evergreen family chipset */
+	PHM_PlatformCaps_PowerControl,                          /* Cayman and beyond chipsets */
+	PHM_PlatformCaps_DisableLSClockGating,                  /* to disable Light Sleep control for HDP memories */
+	PHM_PlatformCaps_BoostState,                            /* this ASIC supports boost state */
+	PHM_PlatformCaps_UserMaxClockForMultiDisplays,          /* indicates if max memory clock is used for all status when multiple displays are connected */
+	PHM_PlatformCaps_RegWriteDelay,                         /* indicates if back to back reg write delay is required */
+	PHM_PlatformCaps_NonABMSupportInPPLib,                  /* ABM is not supported in PPLIB, (moved from PPLIB to DAL) */
+	PHM_PlatformCaps_GFXDynamicMGPowerGating,               /* Enable Dynamic MG PowerGating on Trinity */
+	PHM_PlatformCaps_DisableSMUUVDHandshake,                /* Disable SMU UVD Handshake */
+	PHM_PlatformCaps_DTE,                                   /* Support Digital Temperature Estimation */
+	PHM_PlatformCaps_W5100Specifc_SmuSkipMsgDTE,            /* This is for the feature requested by David B., and Tonny W.*/
+	PHM_PlatformCaps_UVDPowerGating,                        /* enable UVD power gating, supported from Llano */
+	PHM_PlatformCaps_UVDDynamicPowerGating,                 /* enable UVD Dynamic power gating, supported from UVD5 */
+	PHM_PlatformCaps_VCEPowerGating,                        /* Enable VCE power gating, supported for TN and later ASICs */
+	PHM_PlatformCaps_SamuPowerGating,                       /* Enable SAMU power gating, supported for KV and later ASICs */
+	PHM_PlatformCaps_UVDDPM,                                /* UVD clock DPM */
+	PHM_PlatformCaps_VCEDPM,                                /* VCE clock DPM */
+	PHM_PlatformCaps_SamuDPM,                               /* SAMU clock DPM */
+	PHM_PlatformCaps_AcpDPM,                                /* ACP clock DPM */
+	PHM_PlatformCaps_SclkDeepSleepAboveLow,                 /* Enable SCLK Deep Sleep on all DPM states */
+	PHM_PlatformCaps_DynamicUVDState,                       /* Dynamic UVD State */
+	PHM_PlatformCaps_WantSAMClkWithDummyBackEnd,            /* Set SAM Clk With Dummy Back End */
+	PHM_PlatformCaps_WantUVDClkWithDummyBackEnd,            /* Set UVD Clk With Dummy Back End */
+	PHM_PlatformCaps_WantVCEClkWithDummyBackEnd,            /* Set VCE Clk With Dummy Back End */
+	PHM_PlatformCaps_WantACPClkWithDummyBackEnd,            /* Set SAM Clk With Dummy Back End */
+	PHM_PlatformCaps_OD6inACSupport,                        /* indicates that the ASIC/back end supports OD6 */
+	PHM_PlatformCaps_OD6inDCSupport,                        /* indicates that the ASIC/back end supports OD6 in DC */
+	PHM_PlatformCaps_EnablePlatformPowerManagement,         /* indicates that Platform Power Management feature is supported */
+	PHM_PlatformCaps_SurpriseRemoval,                       /* indicates that surprise removal feature is requested */
+	PHM_PlatformCaps_NewCACVoltage,                         /* indicates new CAC voltage table support */
+	PHM_PlatformCaps_DBRamping,                             /* for dI/dT feature */
+	PHM_PlatformCaps_TDRamping,                             /* for dI/dT feature */
+	PHM_PlatformCaps_TCPRamping,                            /* for dI/dT feature */
+	PHM_PlatformCaps_EnableSMU7ThermalManagement,           /* SMC will manage thermal events */
+	PHM_PlatformCaps_FPS,                                   /* FPS support */
+	PHM_PlatformCaps_ACP,                                   /* ACP support */
+	PHM_PlatformCaps_SclkThrottleLowNotification,           /* SCLK Throttle Low Notification */
+	PHM_PlatformCaps_XDMAEnabled,                           /* XDMA engine is enabled */
+	PHM_PlatformCaps_UseDummyBackEnd,                       /* use dummy back end */
+	PHM_PlatformCaps_EnableDFSBypass,                       /* Enable DFS bypass */
+	PHM_PlatformCaps_VddNBDirectRequest,
+	PHM_PlatformCaps_PauseMMSessions,
+	PHM_PlatformCaps_UnTabledHardwareInterface,             /* Tableless/direct call hardware interface for CI and newer ASICs */
+	PHM_PlatformCaps_SMU7,                                  /* indicates that vpuRecoveryBegin without SMU shutdown */
+	PHM_PlatformCaps_RevertGPIO5Polarity,                   /* indicates revert GPIO5 plarity table support */
+	PHM_PlatformCaps_Thermal2GPIO17,                        /* indicates thermal2GPIO17 table support */
+	PHM_PlatformCaps_ThermalOutGPIO,                        /* indicates ThermalOutGPIO support, pin number is assigned by VBIOS */
+	PHM_PlatformCaps_DisableMclkSwitchingForFrameLock,      /* Disable memory clock switch during Framelock */
+	PHM_PlatformCaps_VRHotGPIOConfigurable,                 /* indicates VR_HOT GPIO configurable */
+	PHM_PlatformCaps_TempInversion,                         /* enable Temp Inversion feature */
+	PHM_PlatformCaps_IOIC3,
+	PHM_PlatformCaps_ConnectedStandby,
+	PHM_PlatformCaps_EVV,
+	PHM_PlatformCaps_EnableLongIdleBACOSupport,
+	PHM_PlatformCaps_CombinePCCWithThermalSignal,
+	PHM_PlatformCaps_DisableUsingActualTemperatureForPowerCalc,
+	PHM_PlatformCaps_StablePState,
+	PHM_PlatformCaps_OD6PlusinACSupport,
+	PHM_PlatformCaps_OD6PlusinDCSupport,
+	PHM_PlatformCaps_ODThermalLimitUnlock,
+	PHM_PlatformCaps_ReducePowerLimit,
+	PHM_PlatformCaps_ODFuzzyFanControlSupport,
+	PHM_PlatformCaps_GeminiRegulatorFanControlSupport,
+	PHM_PlatformCaps_ControlVDDGFX,
+	PHM_PlatformCaps_BBBSupported,
+	PHM_PlatformCaps_DisableVoltageIsland,
+	PHM_PlatformCaps_FanSpeedInTableIsRPM,
+	PHM_PlatformCaps_GFXClockGatingManagedInCAIL,
+	PHM_PlatformCaps_IcelandULPSSWWorkAround,
+	PHM_PlatformCaps_FPSEnhancement,
+	PHM_PlatformCaps_LoadPostProductionFirmware,
+	PHM_PlatformCaps_VpuRecoveryInProgress,
+	PHM_PlatformCaps_Falcon_QuickTransition,
+	PHM_PlatformCaps_AVFS,
+	PHM_PlatformCaps_ClockStretcher,
+	PHM_PlatformCaps_TablelessHardwareInterface,
+	PHM_PlatformCaps_EnableDriverEVV,
+	PHM_PlatformCaps_Max
+};
+
+#define PHM_MAX_NUM_CAPS_BITS_PER_FIELD (sizeof(uint32_t)*8)
+
+/* Number of uint32_t entries used by CAPS table */
+#define PHM_MAX_NUM_CAPS_ULONG_ENTRIES \
+	((PHM_PlatformCaps_Max + ((PHM_MAX_NUM_CAPS_BITS_PER_FIELD) - 1)) / (PHM_MAX_NUM_CAPS_BITS_PER_FIELD))
+
+struct pp_hw_descriptor {
+	uint32_t hw_caps[PHM_MAX_NUM_CAPS_ULONG_ENTRIES];
+};
+
+enum PHM_PerformanceLevelDesignation {
+	PHM_PerformanceLevelDesignation_Activity,
+	PHM_PerformanceLevelDesignation_PowerContainment
+};
+
+typedef enum PHM_PerformanceLevelDesignation PHM_PerformanceLevelDesignation;
+
+struct PHM_PerformanceLevel {
+    uint32_t    coreClock;
+    uint32_t    memory_clock;
+    uint32_t  vddc;
+    uint32_t  vddci;
+    uint32_t    nonLocalMemoryFreq;
+    uint32_t nonLocalMemoryWidth;
+};
+
+typedef struct PHM_PerformanceLevel PHM_PerformanceLevel;
+
+/* Function for setting a platform cap */
+static inline void phm_cap_set(uint32_t *caps,
+			enum phm_platform_caps c)
+{
+	caps[c / PHM_MAX_NUM_CAPS_BITS_PER_FIELD] |= (1UL <<
+			     (c & (PHM_MAX_NUM_CAPS_BITS_PER_FIELD - 1)));
+}
+
+static inline void phm_cap_unset(uint32_t *caps,
+			enum phm_platform_caps c)
+{
+	caps[c / PHM_MAX_NUM_CAPS_BITS_PER_FIELD] &= ~(1UL << (c & (PHM_MAX_NUM_CAPS_BITS_PER_FIELD - 1)));
+}
+
+static inline bool phm_cap_enabled(const uint32_t *caps, enum phm_platform_caps c)
+{
+	return (0 != (caps[c / PHM_MAX_NUM_CAPS_BITS_PER_FIELD] &
+		  (1UL << (c & (PHM_MAX_NUM_CAPS_BITS_PER_FIELD - 1)))));
+}
+
+#define PP_PCIEGenInvalid  0xffff
+enum PP_PCIEGen {
+    PP_PCIEGen1 = 0,                /* PCIE 1.0 - Transfer rate of 2.5 GT/s */
+    PP_PCIEGen2,                    /*PCIE 2.0 - Transfer rate of 5.0 GT/s */
+    PP_PCIEGen3                     /*PCIE 3.0 - Transfer rate of 8.0 GT/s */
+};
+
+typedef enum PP_PCIEGen PP_PCIEGen;
+
+#define PP_Min_PCIEGen     PP_PCIEGen1
+#define PP_Max_PCIEGen     PP_PCIEGen3
+#define PP_Min_PCIELane    1
+#define PP_Max_PCIELane    32
+
+enum phm_clock_Type {
+	PHM_DispClock = 1,
+	PHM_SClock,
+	PHM_MemClock
+};
+
+#define MAX_NUM_CLOCKS 16
+
+struct PP_Clocks {
+	uint32_t engineClock;
+	uint32_t memoryClock;
+	uint32_t BusBandwidth;
+	uint32_t engineClockInSR;
+};
+
+struct phm_platform_descriptor {
+	uint32_t platformCaps[PHM_MAX_NUM_CAPS_ULONG_ENTRIES];
+	uint32_t vbiosInterruptId;
+	struct PP_Clocks overdriveLimit;
+	struct PP_Clocks clockStep;
+	uint32_t hardwareActivityPerformanceLevels;
+	uint32_t minimumClocksReductionPercentage;
+	uint32_t minOverdriveVDDC;
+	uint32_t maxOverdriveVDDC;
+	uint32_t overdriveVDDCStep;
+	uint32_t hardwarePerformanceLevels;
+	uint16_t powerBudget;
+	uint32_t TDPLimit;
+	uint32_t nearTDPLimit;
+	uint32_t nearTDPLimitAdjusted;
+	uint32_t SQRampingThreshold;
+	uint32_t CACLeakage;
+	uint16_t TDPODLimit;
+	uint32_t TDPAdjustment;
+	bool TDPAdjustmentPolarity;
+	uint16_t LoadLineSlope;
+	uint32_t  VidMinLimit;
+	uint32_t  VidMaxLimit;
+	uint32_t  VidStep;
+	uint32_t  VidAdjustment;
+	bool VidAdjustmentPolarity;
+};
+
+struct phm_clocks {
+	uint32_t num_of_entries;
+	uint32_t clock[MAX_NUM_CLOCKS];
+};
+
+enum PP_DAL_POWERLEVEL {
+	PP_DAL_POWERLEVEL_INVALID = 0,
+	PP_DAL_POWERLEVEL_ULTRALOW,
+	PP_DAL_POWERLEVEL_LOW,
+	PP_DAL_POWERLEVEL_NOMINAL,
+	PP_DAL_POWERLEVEL_PERFORMANCE,
+
+	PP_DAL_POWERLEVEL_0 = PP_DAL_POWERLEVEL_ULTRALOW,
+	PP_DAL_POWERLEVEL_1 = PP_DAL_POWERLEVEL_LOW,
+	PP_DAL_POWERLEVEL_2 = PP_DAL_POWERLEVEL_NOMINAL,
+	PP_DAL_POWERLEVEL_3 = PP_DAL_POWERLEVEL_PERFORMANCE,
+	PP_DAL_POWERLEVEL_4 = PP_DAL_POWERLEVEL_3+1,
+	PP_DAL_POWERLEVEL_5 = PP_DAL_POWERLEVEL_4+1,
+	PP_DAL_POWERLEVEL_6 = PP_DAL_POWERLEVEL_5+1,
+	PP_DAL_POWERLEVEL_7 = PP_DAL_POWERLEVEL_6+1,
+};
+
+
+extern int phm_enable_clock_power_gatings(struct pp_hwmgr *hwmgr);
+extern int phm_powergate_uvd(struct pp_hwmgr *hwmgr, bool gate);
+extern int phm_powergate_vce(struct pp_hwmgr *hwmgr, bool gate);
+extern int phm_powerdown_uvd(struct pp_hwmgr *hwmgr);
+extern int phm_setup_asic(struct pp_hwmgr *hwmgr);
+extern int phm_enable_dynamic_state_management(struct pp_hwmgr *hwmgr);
+extern void phm_init_dynamic_caps(struct pp_hwmgr *hwmgr);
+extern bool phm_is_hw_access_blocked(struct pp_hwmgr *hwmgr);
+extern int phm_block_hw_access(struct pp_hwmgr *hwmgr, bool block);
+extern int phm_set_power_state(struct pp_hwmgr *hwmgr,
+		    const struct pp_hw_power_state *pcurrent_state,
+		 const struct pp_hw_power_state *pnew_power_state);
+
+extern int phm_apply_state_adjust_rules(struct pp_hwmgr *hwmgr,
+				   struct pp_power_state *adjusted_ps,
+			     const struct pp_power_state *current_ps);
+
+extern int phm_force_dpm_levels(struct pp_hwmgr *hwmgr, enum amd_dpm_forced_level level);
+extern int phm_display_configuration_changed(struct pp_hwmgr *hwmgr);
+extern int phm_notify_smc_display_config_after_ps_adjustment(struct pp_hwmgr *hwmgr);
+extern int phm_register_thermal_interrupt(struct pp_hwmgr *hwmgr, const void *info);
+extern int phm_start_thermal_controller(struct pp_hwmgr *hwmgr, struct PP_TemperatureRange *temperature_range);
+extern int phm_stop_thermal_controller(struct pp_hwmgr *hwmgr);
+extern bool phm_check_smc_update_required_for_display_configuration(struct pp_hwmgr *hwmgr);
+
+extern int phm_check_states_equal(struct pp_hwmgr *hwmgr,
+				 const struct pp_hw_power_state *pstate1,
+				 const struct pp_hw_power_state *pstate2,
+				 bool *equal);
+
+extern int phm_store_dal_configuration_data(struct pp_hwmgr *hwmgr,
+		const struct amd_pp_display_configuration *display_config);
+
+extern int phm_get_dal_power_level(struct pp_hwmgr *hwmgr,
+		struct amd_pp_dal_clock_info*info);
+
+extern int phm_set_cpu_power_state(struct pp_hwmgr *hwmgr);
+
+extern int phm_power_down_asic(struct pp_hwmgr *hwmgr);
+
+#endif /* _HARDWARE_MANAGER_H_ */
+
diff --git a/drivers/gpu/drm/amd/powerplay/inc/hwmgr.h b/drivers/gpu/drm/amd/powerplay/inc/hwmgr.h
new file mode 100644
index 000000000000..aeaa3dbba525
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/hwmgr.h
@@ -0,0 +1,801 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _HWMGR_H_
+#define _HWMGR_H_
+
+#include <linux/seq_file.h>
+#include "amd_powerplay.h"
+#include "pp_instance.h"
+#include "hardwaremanager.h"
+#include "pp_power_source.h"
+#include "hwmgr_ppt.h"
+#include "ppatomctrl.h"
+#include "hwmgr_ppt.h"
+
+struct pp_instance;
+struct pp_hwmgr;
+struct pp_hw_power_state;
+struct pp_power_state;
+struct PP_VCEState;
+struct phm_fan_speed_info;
+struct pp_atomctrl_voltage_table;
+
+
+enum DISPLAY_GAP {
+	DISPLAY_GAP_VBLANK_OR_WM = 0,   /* Wait for vblank or MCHG watermark. */
+	DISPLAY_GAP_VBLANK       = 1,   /* Wait for vblank. */
+	DISPLAY_GAP_WATERMARK    = 2,   /* Wait for MCHG watermark. (Note that HW may deassert WM in VBI depending on DC_STUTTER_CNTL.) */
+	DISPLAY_GAP_IGNORE       = 3    /* Do not wait. */
+};
+typedef enum DISPLAY_GAP DISPLAY_GAP;
+
+
+struct vi_dpm_level {
+	bool enabled;
+	uint32_t value;
+	uint32_t param1;
+};
+
+struct vi_dpm_table {
+	uint32_t count;
+	struct vi_dpm_level dpm_level[1];
+};
+
+enum PP_Result {
+	PP_Result_TableImmediateExit = 0x13,
+};
+
+#define PCIE_PERF_REQ_REMOVE_REGISTRY   0
+#define PCIE_PERF_REQ_FORCE_LOWPOWER    1
+#define PCIE_PERF_REQ_GEN1         2
+#define PCIE_PERF_REQ_GEN2         3
+#define PCIE_PERF_REQ_GEN3         4
+
+enum PHM_BackEnd_Magic {
+	PHM_Dummy_Magic       = 0xAA5555AA,
+	PHM_RV770_Magic       = 0xDCBAABCD,
+	PHM_Kong_Magic        = 0x239478DF,
+	PHM_NIslands_Magic    = 0x736C494E,
+	PHM_Sumo_Magic        = 0x8339FA11,
+	PHM_SIslands_Magic    = 0x369431AC,
+	PHM_Trinity_Magic     = 0x96751873,
+	PHM_CIslands_Magic    = 0x38AC78B0,
+	PHM_Kv_Magic          = 0xDCBBABC0,
+	PHM_VIslands_Magic    = 0x20130307,
+	PHM_Cz_Magic          = 0x67DCBA25
+};
+
+
+#define PHM_PCIE_POWERGATING_TARGET_GFX            0
+#define PHM_PCIE_POWERGATING_TARGET_DDI            1
+#define PHM_PCIE_POWERGATING_TARGET_PLLCASCADE     2
+#define PHM_PCIE_POWERGATING_TARGET_PHY            3
+
+typedef int (*phm_table_function)(struct pp_hwmgr *hwmgr, void *input,
+				  void *output, void *storage, int result);
+
+typedef bool (*phm_check_function)(struct pp_hwmgr *hwmgr);
+
+struct phm_set_power_state_input {
+	const struct pp_hw_power_state *pcurrent_state;
+	const struct pp_hw_power_state *pnew_state;
+};
+
+struct phm_acp_arbiter {
+	uint32_t acpclk;
+};
+
+struct phm_uvd_arbiter {
+	uint32_t vclk;
+	uint32_t dclk;
+	uint32_t vclk_ceiling;
+	uint32_t dclk_ceiling;
+};
+
+struct phm_vce_arbiter {
+	uint32_t   evclk;
+	uint32_t   ecclk;
+};
+
+struct phm_gfx_arbiter {
+	uint32_t sclk;
+	uint32_t mclk;
+	uint32_t sclk_over_drive;
+	uint32_t mclk_over_drive;
+	uint32_t sclk_threshold;
+	uint32_t num_cus;
+};
+
+/* Entries in the master tables */
+struct phm_master_table_item {
+	phm_check_function isFunctionNeededInRuntimeTable;
+	phm_table_function tableFunction;
+};
+
+enum phm_master_table_flag {
+	PHM_MasterTableFlag_None         = 0,
+	PHM_MasterTableFlag_ExitOnError  = 1,
+};
+
+/* The header of the master tables */
+struct phm_master_table_header {
+	uint32_t storage_size;
+	uint32_t flags;
+	struct phm_master_table_item *master_list;
+};
+
+struct phm_runtime_table_header {
+	uint32_t storage_size;
+	bool exit_error;
+	phm_table_function *function_list;
+};
+
+struct phm_clock_array {
+	uint32_t count;
+	uint32_t values[1];
+};
+
+struct phm_clock_voltage_dependency_record {
+	uint32_t clk;
+	uint32_t v;
+};
+
+struct phm_vceclock_voltage_dependency_record {
+	uint32_t ecclk;
+	uint32_t evclk;
+	uint32_t v;
+};
+
+struct phm_uvdclock_voltage_dependency_record {
+	uint32_t vclk;
+	uint32_t dclk;
+	uint32_t v;
+};
+
+struct phm_samuclock_voltage_dependency_record {
+	uint32_t samclk;
+	uint32_t v;
+};
+
+struct phm_acpclock_voltage_dependency_record {
+	uint32_t acpclk;
+	uint32_t v;
+};
+
+struct phm_clock_voltage_dependency_table {
+	uint32_t count;										/* Number of entries. */
+	struct phm_clock_voltage_dependency_record entries[1];		/* Dynamically allocate count entries. */
+};
+
+struct phm_phase_shedding_limits_record {
+	uint32_t  Voltage;
+	uint32_t    Sclk;
+	uint32_t    Mclk;
+};
+
+
+extern int phm_dispatch_table(struct pp_hwmgr *hwmgr,
+			      struct phm_runtime_table_header *rt_table,
+			      void *input, void *output);
+
+extern int phm_construct_table(struct pp_hwmgr *hwmgr,
+			       struct phm_master_table_header *master_table,
+			       struct phm_runtime_table_header *rt_table);
+
+extern int phm_destroy_table(struct pp_hwmgr *hwmgr,
+			     struct phm_runtime_table_header *rt_table);
+
+
+struct phm_uvd_clock_voltage_dependency_record {
+	uint32_t vclk;
+	uint32_t dclk;
+	uint32_t v;
+};
+
+struct phm_uvd_clock_voltage_dependency_table {
+	uint8_t count;
+	struct phm_uvd_clock_voltage_dependency_record entries[1];
+};
+
+struct phm_acp_clock_voltage_dependency_record {
+	uint32_t acpclk;
+	uint32_t v;
+};
+
+struct phm_acp_clock_voltage_dependency_table {
+	uint32_t count;
+	struct phm_acp_clock_voltage_dependency_record entries[1];
+};
+
+struct phm_vce_clock_voltage_dependency_record {
+	uint32_t ecclk;
+	uint32_t evclk;
+	uint32_t v;
+};
+
+struct phm_phase_shedding_limits_table {
+	uint32_t                           count;
+	struct phm_phase_shedding_limits_record  entries[1];
+};
+
+struct phm_vceclock_voltage_dependency_table {
+	uint8_t count;                                    /* Number of entries. */
+	struct phm_vceclock_voltage_dependency_record entries[1]; /* Dynamically allocate count entries. */
+};
+
+struct phm_uvdclock_voltage_dependency_table {
+	uint8_t count;                                    /* Number of entries. */
+	struct phm_uvdclock_voltage_dependency_record entries[1]; /* Dynamically allocate count entries. */
+};
+
+struct phm_samuclock_voltage_dependency_table {
+	uint8_t count;                                    /* Number of entries. */
+	struct phm_samuclock_voltage_dependency_record entries[1]; /* Dynamically allocate count entries. */
+};
+
+struct phm_acpclock_voltage_dependency_table {
+	uint32_t count;                                    /* Number of entries. */
+	struct phm_acpclock_voltage_dependency_record entries[1]; /* Dynamically allocate count entries. */
+};
+
+struct phm_vce_clock_voltage_dependency_table {
+	uint8_t count;
+	struct phm_vce_clock_voltage_dependency_record entries[1];
+};
+
+struct pp_hwmgr_func {
+	int (*backend_init)(struct pp_hwmgr *hw_mgr);
+	int (*backend_fini)(struct pp_hwmgr *hw_mgr);
+	int (*asic_setup)(struct pp_hwmgr *hw_mgr);
+	int (*get_power_state_size)(struct pp_hwmgr *hw_mgr);
+
+	int (*apply_state_adjust_rules)(struct pp_hwmgr *hwmgr,
+				struct pp_power_state  *prequest_ps,
+			const struct pp_power_state *pcurrent_ps);
+
+	int (*force_dpm_level)(struct pp_hwmgr *hw_mgr,
+					enum amd_dpm_forced_level level);
+
+	int (*dynamic_state_management_enable)(
+						struct pp_hwmgr *hw_mgr);
+
+	int (*patch_boot_state)(struct pp_hwmgr *hwmgr,
+				     struct pp_hw_power_state *hw_ps);
+
+	int (*get_pp_table_entry)(struct pp_hwmgr *hwmgr,
+			    unsigned long, struct pp_power_state *);
+	int (*get_num_of_pp_table_entries)(struct pp_hwmgr *hwmgr);
+	int (*powerdown_uvd)(struct pp_hwmgr *hwmgr);
+	int (*powergate_vce)(struct pp_hwmgr *hwmgr, bool bgate);
+	int (*powergate_uvd)(struct pp_hwmgr *hwmgr, bool bgate);
+	int (*get_mclk)(struct pp_hwmgr *hwmgr, bool low);
+	int (*get_sclk)(struct pp_hwmgr *hwmgr, bool low);
+	int (*power_state_set)(struct pp_hwmgr *hwmgr,
+						const void *state);
+	void (*print_current_perforce_level)(struct pp_hwmgr *hwmgr,
+							struct seq_file *m);
+	int (*enable_clock_power_gating)(struct pp_hwmgr *hwmgr);
+	int (*notify_smc_display_config_after_ps_adjustment)(struct pp_hwmgr *hwmgr);
+	int (*display_config_changed)(struct pp_hwmgr *hwmgr);
+	int (*disable_clock_power_gating)(struct pp_hwmgr *hwmgr);
+	int (*update_clock_gatings)(struct pp_hwmgr *hwmgr,
+						const uint32_t *msg_id);
+	int (*set_max_fan_rpm_output)(struct pp_hwmgr *hwmgr, uint16_t us_max_fan_pwm);
+	int (*set_max_fan_pwm_output)(struct pp_hwmgr *hwmgr, uint16_t us_max_fan_pwm);
+	int (*get_temperature)(struct pp_hwmgr *hwmgr);
+	int (*stop_thermal_controller)(struct pp_hwmgr *hwmgr);
+	int (*get_fan_speed_info)(struct pp_hwmgr *hwmgr, struct phm_fan_speed_info *fan_speed_info);
+	int (*set_fan_control_mode)(struct pp_hwmgr *hwmgr, uint32_t mode);
+	int (*get_fan_control_mode)(struct pp_hwmgr *hwmgr);
+	int (*set_fan_speed_percent)(struct pp_hwmgr *hwmgr, uint32_t percent);
+	int (*get_fan_speed_percent)(struct pp_hwmgr *hwmgr, uint32_t *speed);
+	int (*set_fan_speed_rpm)(struct pp_hwmgr *hwmgr, uint32_t percent);
+	int (*get_fan_speed_rpm)(struct pp_hwmgr *hwmgr, uint32_t *speed);
+	int (*reset_fan_speed_to_default)(struct pp_hwmgr *hwmgr);
+	int (*uninitialize_thermal_controller)(struct pp_hwmgr *hwmgr);
+	int (*register_internal_thermal_interrupt)(struct pp_hwmgr *hwmgr,
+					const void *thermal_interrupt_info);
+	bool (*check_smc_update_required_for_display_configuration)(struct pp_hwmgr *hwmgr);
+	int (*check_states_equal)(struct pp_hwmgr *hwmgr,
+					const struct pp_hw_power_state *pstate1,
+					const struct pp_hw_power_state *pstate2,
+					bool *equal);
+	int (*set_cpu_power_state)(struct pp_hwmgr *hwmgr);
+	int (*store_cc6_data)(struct pp_hwmgr *hwmgr, uint32_t separation_time,
+				bool cc6_disable, bool pstate_disable,
+				bool pstate_switch_disable);
+	int (*get_dal_power_level)(struct pp_hwmgr *hwmgr,
+				   struct amd_pp_dal_clock_info *info);
+	int (*power_off_asic)(struct pp_hwmgr *hwmgr);
+};
+
+struct pp_table_func {
+	int (*pptable_init)(struct pp_hwmgr *hw_mgr);
+	int (*pptable_fini)(struct pp_hwmgr *hw_mgr);
+	int (*pptable_get_number_of_vce_state_table_entries)(struct pp_hwmgr *hw_mgr);
+	int (*pptable_get_vce_state_table_entry)(
+						struct pp_hwmgr *hwmgr,
+						unsigned long i,
+						struct PP_VCEState *vce_state,
+						void **clock_info,
+						unsigned long *flag);
+};
+
+union phm_cac_leakage_record {
+	struct {
+		uint16_t Vddc;          /* in CI, we use it for StdVoltageHiSidd */
+		uint32_t Leakage;       /* in CI, we use it for StdVoltageLoSidd */
+	};
+	struct {
+		uint16_t Vddc1;
+		uint16_t Vddc2;
+		uint16_t Vddc3;
+	};
+};
+
+struct phm_cac_leakage_table {
+	uint32_t count;
+	union phm_cac_leakage_record entries[1];
+};
+
+struct phm_samu_clock_voltage_dependency_record {
+	uint32_t samclk;
+	uint32_t v;
+};
+
+
+struct phm_samu_clock_voltage_dependency_table {
+	uint8_t count;
+	struct phm_samu_clock_voltage_dependency_record entries[1];
+};
+
+struct phm_cac_tdp_table {
+	uint16_t usTDP;
+	uint16_t usConfigurableTDP;
+	uint16_t usTDC;
+	uint16_t usBatteryPowerLimit;
+	uint16_t usSmallPowerLimit;
+	uint16_t usLowCACLeakage;
+	uint16_t usHighCACLeakage;
+	uint16_t usMaximumPowerDeliveryLimit;
+	uint16_t usOperatingTempMinLimit;
+	uint16_t usOperatingTempMaxLimit;
+	uint16_t usOperatingTempStep;
+	uint16_t usOperatingTempHyst;
+	uint16_t usDefaultTargetOperatingTemp;
+	uint16_t usTargetOperatingTemp;
+	uint16_t usPowerTuneDataSetID;
+	uint16_t usSoftwareShutdownTemp;
+	uint16_t usClockStretchAmount;
+	uint16_t usTemperatureLimitHotspot;
+	uint16_t usTemperatureLimitLiquid1;
+	uint16_t usTemperatureLimitLiquid2;
+	uint16_t usTemperatureLimitVrVddc;
+	uint16_t usTemperatureLimitVrMvdd;
+	uint16_t usTemperatureLimitPlx;
+	uint8_t  ucLiquid1_I2C_address;
+	uint8_t  ucLiquid2_I2C_address;
+	uint8_t  ucLiquid_I2C_Line;
+	uint8_t  ucVr_I2C_address;
+	uint8_t  ucVr_I2C_Line;
+	uint8_t  ucPlx_I2C_address;
+	uint8_t  ucPlx_I2C_Line;
+};
+
+struct phm_ppm_table {
+	uint8_t   ppm_design;
+	uint16_t  cpu_core_number;
+	uint32_t  platform_tdp;
+	uint32_t  small_ac_platform_tdp;
+	uint32_t  platform_tdc;
+	uint32_t  small_ac_platform_tdc;
+	uint32_t  apu_tdp;
+	uint32_t  dgpu_tdp;
+	uint32_t  dgpu_ulv_power;
+	uint32_t  tj_max;
+};
+
+struct phm_vq_budgeting_record {
+	uint32_t ulCUs;
+	uint32_t ulSustainableSOCPowerLimitLow;
+	uint32_t ulSustainableSOCPowerLimitHigh;
+	uint32_t ulMinSclkLow;
+	uint32_t ulMinSclkHigh;
+	uint8_t  ucDispConfig;
+	uint32_t ulDClk;
+	uint32_t ulEClk;
+	uint32_t ulSustainableSclk;
+	uint32_t ulSustainableCUs;
+};
+
+struct phm_vq_budgeting_table {
+	uint8_t numEntries;
+	struct phm_vq_budgeting_record entries[1];
+};
+
+struct phm_clock_and_voltage_limits {
+	uint32_t sclk;
+	uint32_t mclk;
+	uint16_t vddc;
+	uint16_t vddci;
+	uint16_t vddgfx;
+};
+
+/* Structure to hold PPTable information */
+
+struct phm_ppt_v1_information {
+	struct phm_ppt_v1_clock_voltage_dependency_table *vdd_dep_on_sclk;
+	struct phm_ppt_v1_clock_voltage_dependency_table *vdd_dep_on_mclk;
+	struct phm_clock_array *valid_sclk_values;
+	struct phm_clock_array *valid_mclk_values;
+	struct phm_clock_and_voltage_limits max_clock_voltage_on_dc;
+	struct phm_clock_and_voltage_limits max_clock_voltage_on_ac;
+	struct phm_clock_voltage_dependency_table *vddc_dep_on_dal_pwrl;
+	struct phm_ppm_table *ppm_parameter_table;
+	struct phm_cac_tdp_table *cac_dtp_table;
+	struct phm_ppt_v1_mm_clock_voltage_dependency_table *mm_dep_table;
+	struct phm_ppt_v1_voltage_lookup_table *vddc_lookup_table;
+	struct phm_ppt_v1_voltage_lookup_table *vddgfx_lookup_table;
+	struct phm_ppt_v1_pcie_table *pcie_table;
+	uint16_t us_ulv_voltage_offset;
+};
+
+struct phm_dynamic_state_info {
+	struct phm_clock_voltage_dependency_table *vddc_dependency_on_sclk;
+	struct phm_clock_voltage_dependency_table *vddci_dependency_on_mclk;
+	struct phm_clock_voltage_dependency_table *vddc_dependency_on_mclk;
+	struct phm_clock_voltage_dependency_table *mvdd_dependency_on_mclk;
+	struct phm_clock_voltage_dependency_table *vddc_dep_on_dal_pwrl;
+	struct phm_clock_array                    *valid_sclk_values;
+	struct phm_clock_array                    *valid_mclk_values;
+	struct phm_clock_and_voltage_limits       max_clock_voltage_on_dc;
+	struct phm_clock_and_voltage_limits       max_clock_voltage_on_ac;
+	uint32_t                                  mclk_sclk_ratio;
+	uint32_t                                  sclk_mclk_delta;
+	uint32_t                                  vddc_vddci_delta;
+	uint32_t                                  min_vddc_for_pcie_gen2;
+	struct phm_cac_leakage_table              *cac_leakage_table;
+	struct phm_phase_shedding_limits_table	  *vddc_phase_shed_limits_table;
+
+	struct phm_vce_clock_voltage_dependency_table
+					    *vce_clock_voltage_dependency_table;
+	struct phm_uvd_clock_voltage_dependency_table
+					    *uvd_clock_voltage_dependency_table;
+	struct phm_acp_clock_voltage_dependency_table
+					    *acp_clock_voltage_dependency_table;
+	struct phm_samu_clock_voltage_dependency_table
+					   *samu_clock_voltage_dependency_table;
+
+	struct phm_ppm_table                          *ppm_parameter_table;
+	struct phm_cac_tdp_table                      *cac_dtp_table;
+	struct phm_clock_voltage_dependency_table	  *vdd_gfx_dependency_on_sclk;
+	struct phm_vq_budgeting_table		  		  *vq_budgeting_table;
+};
+
+struct pp_fan_info {
+	bool bNoFan;
+	uint8_t   ucTachometerPulsesPerRevolution;
+	uint32_t   ulMinRPM;
+	uint32_t   ulMaxRPM;
+};
+
+struct pp_advance_fan_control_parameters {
+	uint16_t  usTMin;                          /* The temperature, in 0.01 centigrades, below which we just run at a minimal PWM. */
+	uint16_t  usTMed;                          /* The middle temperature where we change slopes. */
+	uint16_t  usTHigh;                         /* The high temperature for setting the second slope. */
+	uint16_t  usPWMMin;                        /* The minimum PWM value in percent (0.01% increments). */
+	uint16_t  usPWMMed;                        /* The PWM value (in percent) at TMed. */
+	uint16_t  usPWMHigh;                       /* The PWM value at THigh. */
+	uint8_t   ucTHyst;                         /* Temperature hysteresis. Integer. */
+	uint32_t   ulCycleDelay;                   /* The time between two invocations of the fan control routine in microseconds. */
+	uint16_t  usTMax;                          /* The max temperature */
+	uint8_t   ucFanControlMode;
+	uint16_t  usFanPWMMinLimit;
+	uint16_t  usFanPWMMaxLimit;
+	uint16_t  usFanPWMStep;
+	uint16_t  usDefaultMaxFanPWM;
+	uint16_t  usFanOutputSensitivity;
+	uint16_t  usDefaultFanOutputSensitivity;
+	uint16_t  usMaxFanPWM;                     /* The max Fan PWM value for Fuzzy Fan Control feature */
+	uint16_t  usFanRPMMinLimit;                /* Minimum limit range in percentage, need to calculate based on minRPM/MaxRpm */
+	uint16_t  usFanRPMMaxLimit;                /* Maximum limit range in percentage, usually set to 100% by default */
+	uint16_t  usFanRPMStep;                    /* Step increments/decerements, in percent */
+	uint16_t  usDefaultMaxFanRPM;              /* The max Fan RPM value for Fuzzy Fan Control feature, default from PPTable */
+	uint16_t  usMaxFanRPM;                     /* The max Fan RPM value for Fuzzy Fan Control feature, user defined */
+	uint16_t  usFanCurrentLow;                 /* Low current */
+	uint16_t  usFanCurrentHigh;                /* High current */
+	uint16_t  usFanRPMLow;                     /* Low RPM */
+	uint16_t  usFanRPMHigh;                    /* High RPM */
+	uint32_t   ulMinFanSCLKAcousticLimit;      /* Minimum Fan Controller SCLK Frequency Acoustic Limit. */
+	uint8_t   ucTargetTemperature;             /* Advanced fan controller target temperature. */
+	uint8_t   ucMinimumPWMLimit;               /* The minimum PWM that the advanced fan controller can set.  This should be set to the highest PWM that will run the fan at its lowest RPM. */
+	uint16_t  usFanGainEdge;                   /* The following is added for Fiji */
+	uint16_t  usFanGainHotspot;
+	uint16_t  usFanGainLiquid;
+	uint16_t  usFanGainVrVddc;
+	uint16_t  usFanGainVrMvdd;
+	uint16_t  usFanGainPlx;
+	uint16_t  usFanGainHbm;
+};
+
+struct pp_thermal_controller_info {
+	uint8_t ucType;
+	uint8_t ucI2cLine;
+	uint8_t ucI2cAddress;
+	struct pp_fan_info fanInfo;
+	struct pp_advance_fan_control_parameters advanceFanControlParameters;
+};
+
+struct phm_microcode_version_info {
+	uint32_t SMC;
+	uint32_t DMCU;
+	uint32_t MC;
+	uint32_t NB;
+};
+
+/**
+ * The main hardware manager structure.
+ */
+struct pp_hwmgr {
+	uint32_t chip_family;
+	uint32_t chip_id;
+	uint32_t hw_revision;
+	uint32_t sub_sys_id;
+	uint32_t sub_vendor_id;
+
+	void *device;
+	struct pp_smumgr *smumgr;
+	const void *soft_pp_table;
+	bool need_pp_table_upload;
+	enum amd_dpm_forced_level dpm_level;
+	bool block_hw_access;
+	struct phm_gfx_arbiter gfx_arbiter;
+	struct phm_acp_arbiter acp_arbiter;
+	struct phm_uvd_arbiter uvd_arbiter;
+	struct phm_vce_arbiter vce_arbiter;
+	uint32_t usec_timeout;
+	void *pptable;
+	struct phm_platform_descriptor platform_descriptor;
+	void *backend;
+	enum PP_DAL_POWERLEVEL dal_power_level;
+	struct phm_dynamic_state_info dyn_state;
+	struct phm_runtime_table_header setup_asic;
+	struct phm_runtime_table_header power_down_asic;
+	struct phm_runtime_table_header disable_dynamic_state_management;
+	struct phm_runtime_table_header enable_dynamic_state_management;
+	struct phm_runtime_table_header set_power_state;
+	struct phm_runtime_table_header enable_clock_power_gatings;
+	struct phm_runtime_table_header display_configuration_changed;
+	struct phm_runtime_table_header start_thermal_controller;
+	struct phm_runtime_table_header set_temperature_range;
+	const struct pp_hwmgr_func *hwmgr_func;
+	const struct pp_table_func *pptable_func;
+	struct pp_power_state    *ps;
+	enum pp_power_source  power_source;
+	uint32_t num_ps;
+	struct pp_thermal_controller_info thermal_controller;
+	bool fan_ctrl_is_in_default_mode;
+	uint32_t fan_ctrl_default_mode;
+	uint32_t tmin;
+	struct phm_microcode_version_info microcode_version_info;
+	uint32_t ps_size;
+	struct pp_power_state    *current_ps;
+	struct pp_power_state    *request_ps;
+	struct pp_power_state    *boot_ps;
+	struct pp_power_state    *uvd_ps;
+	struct amd_pp_display_configuration display_config;
+};
+
+
+extern int hwmgr_init(struct amd_pp_init *pp_init,
+		      struct pp_instance *handle);
+
+extern int hwmgr_fini(struct pp_hwmgr *hwmgr);
+
+extern int hw_init_power_state_table(struct pp_hwmgr *hwmgr);
+
+extern int phm_wait_on_register(struct pp_hwmgr *hwmgr, uint32_t index,
+				uint32_t value, uint32_t mask);
+
+extern int phm_wait_for_register_unequal(struct pp_hwmgr *hwmgr,
+				uint32_t index, uint32_t value, uint32_t mask);
+
+extern uint32_t phm_read_indirect_register(struct pp_hwmgr *hwmgr,
+		uint32_t indirect_port, uint32_t index);
+
+extern void phm_write_indirect_register(struct pp_hwmgr *hwmgr,
+		uint32_t indirect_port,
+		uint32_t index,
+		uint32_t value);
+
+extern void phm_wait_on_indirect_register(struct pp_hwmgr *hwmgr,
+				uint32_t indirect_port,
+				uint32_t index,
+				uint32_t value,
+				uint32_t mask);
+
+extern void phm_wait_for_indirect_register_unequal(
+				struct pp_hwmgr *hwmgr,
+				uint32_t indirect_port,
+				uint32_t index,
+				uint32_t value,
+				uint32_t mask);
+
+extern bool phm_cf_want_uvd_power_gating(struct pp_hwmgr *hwmgr);
+extern bool phm_cf_want_vce_power_gating(struct pp_hwmgr *hwmgr);
+extern bool phm_cf_want_microcode_fan_ctrl(struct pp_hwmgr *hwmgr);
+
+extern int phm_trim_voltage_table(struct pp_atomctrl_voltage_table *vol_table);
+extern int phm_get_svi2_mvdd_voltage_table(struct pp_atomctrl_voltage_table *vol_table, phm_ppt_v1_clock_voltage_dependency_table *dep_table);
+extern int phm_get_svi2_vddci_voltage_table(struct pp_atomctrl_voltage_table *vol_table, phm_ppt_v1_clock_voltage_dependency_table *dep_table);
+extern int phm_get_svi2_vdd_voltage_table(struct pp_atomctrl_voltage_table *vol_table, phm_ppt_v1_voltage_lookup_table *lookup_table);
+extern void phm_trim_voltage_table_to_fit_state_table(uint32_t max_vol_steps, struct pp_atomctrl_voltage_table *vol_table);
+extern int phm_reset_single_dpm_table(void *table, uint32_t count, int max);
+extern void phm_setup_pcie_table_entry(void *table, uint32_t index, uint32_t pcie_gen, uint32_t pcie_lanes);
+extern int32_t phm_get_dpm_level_enable_mask_value(void *table);
+extern uint8_t phm_get_voltage_index(struct phm_ppt_v1_voltage_lookup_table *lookup_table, uint16_t voltage);
+extern uint16_t phm_find_closest_vddci(struct pp_atomctrl_voltage_table *vddci_table, uint16_t vddci);
+extern int phm_find_boot_level(void *table, uint32_t value, uint32_t *boot_level);
+extern int phm_get_sclk_for_voltage_evv(struct pp_hwmgr *hwmgr, phm_ppt_v1_voltage_lookup_table *lookup_table,
+								uint16_t virtual_voltage_id, int32_t *sclk);
+extern int phm_initializa_dynamic_state_adjustment_rule_settings(struct pp_hwmgr *hwmgr);
+extern int phm_hwmgr_backend_fini(struct pp_hwmgr *hwmgr);
+extern uint32_t phm_get_lowest_enabled_level(struct pp_hwmgr *hwmgr, uint32_t mask);
+
+
+#define PHM_ENTIRE_REGISTER_MASK 0xFFFFFFFFU
+
+#define PHM_FIELD_SHIFT(reg, field) reg##__##field##__SHIFT
+#define PHM_FIELD_MASK(reg, field) reg##__##field##_MASK
+
+#define PHM_SET_FIELD(origval, reg, field, fieldval)	\
+	(((origval) & ~PHM_FIELD_MASK(reg, field)) |	\
+	 (PHM_FIELD_MASK(reg, field) & ((fieldval) << PHM_FIELD_SHIFT(reg, field))))
+
+#define PHM_GET_FIELD(value, reg, field)	\
+	(((value) & PHM_FIELD_MASK(reg, field)) >>	\
+	 PHM_FIELD_SHIFT(reg, field))
+
+
+#define PHM_WAIT_REGISTER_GIVEN_INDEX(hwmgr, index, value, mask)	\
+	phm_wait_on_register(hwmgr, index, value, mask)
+
+#define PHM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, index, value, mask)	\
+	phm_wait_for_register_unequal(hwmgr, index, value, mask)
+
+#define PHM_WAIT_INDIRECT_REGISTER_GIVEN_INDEX(hwmgr, port, index, value, mask)	\
+	phm_wait_on_indirect_register(hwmgr, mm##port##_INDEX, index, value, mask)
+
+#define PHM_WAIT_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, port, index, value, mask)	\
+	phm_wait_for_indirect_register_unequal(hwmgr, mm##port##_INDEX, index, value, mask)
+
+#define PHM_WAIT_VFPF_INDIRECT_REGISTER_GIVEN_INDEX(hwmgr, port, index, value, mask)	\
+	phm_wait_on_indirect_register(hwmgr, mm##port##_INDEX_0, index, value, mask)
+
+#define PHM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, port, index, value, mask)	\
+	phm_wait_for_indirect_register_unequal(hwmgr, mm##port##_INDEX_0, index, value, mask)
+
+/* Operations on named registers. */
+
+#define PHM_WAIT_REGISTER(hwmgr, reg, value, mask)	\
+	PHM_WAIT_REGISTER_GIVEN_INDEX(hwmgr, mm##reg, value, mask)
+
+#define PHM_WAIT_REGISTER_UNEQUAL(hwmgr, reg, value, mask)	\
+	PHM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, mm##reg, value, mask)
+
+#define PHM_WAIT_INDIRECT_REGISTER(hwmgr, port, reg, value, mask)	\
+	PHM_WAIT_INDIRECT_REGISTER_GIVEN_INDEX(hwmgr, port, ix##reg, value, mask)
+
+#define PHM_WAIT_INDIRECT_REGISTER_UNEQUAL(hwmgr, port, reg, value, mask)	\
+	PHM_WAIT_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, port, ix##reg, value, mask)
+
+#define PHM_WAIT_VFPF_INDIRECT_REGISTER(hwmgr, port, reg, value, mask)	\
+	PHM_WAIT_VFPF_INDIRECT_REGISTER_GIVEN_INDEX(hwmgr, port, ix##reg, value, mask)
+
+#define PHM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL(hwmgr, port, reg, value, mask)	\
+	PHM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, port, ix##reg, value, mask)
+
+/* Operations on named fields. */
+
+#define PHM_READ_FIELD(device, reg, field)	\
+	PHM_GET_FIELD(cgs_read_register(device, mm##reg), reg, field)
+
+#define PHM_READ_INDIRECT_FIELD(device, port, reg, field)	\
+	PHM_GET_FIELD(cgs_read_ind_register(device, port, ix##reg),	\
+			reg, field)
+
+#define PHM_READ_VFPF_INDIRECT_FIELD(device, port, reg, field)	\
+	PHM_GET_FIELD(cgs_read_ind_register(device, port, ix##reg),	\
+			reg, field)
+
+#define PHM_WRITE_FIELD(device, reg, field, fieldval)	\
+	cgs_write_register(device, mm##reg, PHM_SET_FIELD(	\
+				cgs_read_register(device, mm##reg), reg, field, fieldval))
+
+#define PHM_WRITE_INDIRECT_FIELD(device, port, reg, field, fieldval)	\
+	cgs_write_ind_register(device, port, ix##reg,	\
+			PHM_SET_FIELD(cgs_read_ind_register(device, port, ix##reg),	\
+				reg, field, fieldval))
+
+#define PHM_WRITE_VFPF_INDIRECT_FIELD(device, port, reg, field, fieldval)	\
+	cgs_write_ind_register(device, port, ix##reg,	\
+			PHM_SET_FIELD(cgs_read_ind_register(device, port, ix##reg),	\
+				reg, field, fieldval))
+
+#define PHM_WAIT_FIELD(hwmgr, reg, field, fieldval)	\
+	PHM_WAIT_REGISTER(hwmgr, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_INDIRECT_FIELD(hwmgr, port, reg, field, fieldval)	\
+	PHM_WAIT_INDIRECT_REGISTER(hwmgr, port, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_VFPF_INDIRECT_FIELD(hwmgr, port, reg, field, fieldval)	\
+	PHM_WAIT_VFPF_INDIRECT_REGISTER(hwmgr, port, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_FIELD_UNEQUAL(hwmgr, reg, field, fieldval)	\
+	PHM_WAIT_REGISTER_UNEQUAL(hwmgr, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_INDIRECT_FIELD_UNEQUAL(hwmgr, port, reg, field, fieldval)	\
+	PHM_WAIT_INDIRECT_REGISTER_UNEQUAL(hwmgr, port, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(hwmgr, port, reg, field, fieldval)	\
+	PHM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL(hwmgr, port, reg, (fieldval)	\
+			<< PHM_FIELD_SHIFT(reg, field), PHM_FIELD_MASK(reg, field))
+
+/* Operations on arrays of registers & fields. */
+
+#define PHM_READ_ARRAY_REGISTER(device, reg, offset)	\
+	cgs_read_register(device, mm##reg + (offset))
+
+#define PHM_WRITE_ARRAY_REGISTER(device, reg, offset, value)	\
+	cgs_write_register(device, mm##reg + (offset), value)
+
+#define PHM_WAIT_ARRAY_REGISTER(hwmgr, reg, offset, value, mask)	\
+	PHM_WAIT_REGISTER_GIVEN_INDEX(hwmgr, mm##reg + (offset), value, mask)
+
+#define PHM_WAIT_ARRAY_REGISTER_UNEQUAL(hwmgr, reg, offset, value, mask)	\
+	PHM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, mm##reg + (offset), value, mask)
+
+#define PHM_READ_ARRAY_FIELD(hwmgr, reg, offset, field) \
+	PHM_GET_FIELD(PHM_READ_ARRAY_REGISTER(hwmgr->device, reg, offset), reg, field)
+
+#define PHM_WRITE_ARRAY_FIELD(hwmgr, reg, offset, field, fieldvalue)	\
+	PHM_WRITE_ARRAY_REGISTER(hwmgr->device, reg, offset,	\
+			PHM_SET_FIELD(PHM_READ_ARRAY_REGISTER(hwmgr->device, reg, offset),	\
+				reg, field, fieldvalue))
+
+#define PHM_WAIT_ARRAY_FIELD(hwmgr, reg, offset, field, fieldvalue)	\
+	PHM_WAIT_REGISTER_GIVEN_INDEX(hwmgr, mm##reg + (offset),	\
+			(fieldvalue) << PHM_FIELD_SHIFT(reg, field),	\
+			PHM_FIELD_MASK(reg, field))
+
+#define PHM_WAIT_ARRAY_FIELD_UNEQUAL(hwmgr, reg, offset, field, fieldvalue)	\
+	PHM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(hwmgr, mm##reg + (offset),	\
+			(fieldvalue) << PHM_FIELD_SHIFT(reg, field),	\
+			PHM_FIELD_MASK(reg, field))
+
+#endif /* _HWMGR_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/inc/power_state.h b/drivers/gpu/drm/amd/powerplay/inc/power_state.h
new file mode 100644
index 000000000000..a3f0ce4d5835
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/power_state.h
@@ -0,0 +1,200 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef PP_POWERSTATE_H
+#define PP_POWERSTATE_H
+
+struct pp_hw_power_state {
+	unsigned int magic;
+};
+
+struct pp_power_state;
+
+
+#define PP_INVALID_POWER_STATE_ID (0)
+
+
+/*
+ * An item of a list containing Power States.
+ */
+
+struct PP_StateLinkedList {
+	struct pp_power_state *next;
+	struct pp_power_state *prev;
+};
+
+
+enum PP_StateUILabel {
+	PP_StateUILabel_None,
+	PP_StateUILabel_Battery,
+	PP_StateUILabel_MiddleLow,
+	PP_StateUILabel_Balanced,
+	PP_StateUILabel_MiddleHigh,
+	PP_StateUILabel_Performance,
+	PP_StateUILabel_BACO
+};
+
+enum PP_StateClassificationFlag {
+	PP_StateClassificationFlag_Boot                = 0x0001,
+	PP_StateClassificationFlag_Thermal             = 0x0002,
+	PP_StateClassificationFlag_LimitedPowerSource  = 0x0004,
+	PP_StateClassificationFlag_Rest                = 0x0008,
+	PP_StateClassificationFlag_Forced              = 0x0010,
+	PP_StateClassificationFlag_User3DPerformance   = 0x0020,
+	PP_StateClassificationFlag_User2DPerformance   = 0x0040,
+	PP_StateClassificationFlag_3DPerformance       = 0x0080,
+	PP_StateClassificationFlag_ACOverdriveTemplate   = 0x0100,
+	PP_StateClassificationFlag_Uvd                 = 0x0200,
+	PP_StateClassificationFlag_3DPerformanceLow    = 0x0400,
+	PP_StateClassificationFlag_ACPI                = 0x0800,
+	PP_StateClassificationFlag_HD2                 = 0x1000,
+	PP_StateClassificationFlag_UvdHD               = 0x2000,
+	PP_StateClassificationFlag_UvdSD               = 0x4000,
+	PP_StateClassificationFlag_UserDCPerformance    = 0x8000,
+	PP_StateClassificationFlag_DCOverdriveTemplate   = 0x10000,
+	PP_StateClassificationFlag_BACO                  = 0x20000,
+	PP_StateClassificationFlag_LimitedPowerSource_2  = 0x40000,
+	PP_StateClassificationFlag_ULV                   = 0x80000,
+	PP_StateClassificationFlag_UvdMVC               = 0x100000,
+};
+
+typedef unsigned int PP_StateClassificationFlags;
+
+struct PP_StateClassificationBlock {
+	enum PP_StateUILabel         ui_label;
+	enum PP_StateClassificationFlag  flags;
+	int                          bios_index;
+	bool                      temporary_state;
+	bool                      to_be_deleted;
+};
+
+struct PP_StatePcieBlock {
+	unsigned int lanes;
+};
+
+enum PP_RefreshrateSource {
+	PP_RefreshrateSource_EDID,
+	PP_RefreshrateSource_Explicit
+};
+
+struct PP_StateDisplayBlock {
+	bool              disableFrameModulation;
+	bool              limitRefreshrate;
+	enum PP_RefreshrateSource refreshrateSource;
+	int                  explicitRefreshrate;
+	int                  edidRefreshrateIndex;
+	bool              enableVariBright;
+};
+
+struct PP_StateMemroyBlock {
+	bool              dllOff;
+	uint8_t                 m3arb;
+	uint8_t                 unused[3];
+};
+
+struct PP_StateSoftwareAlgorithmBlock {
+	bool disableLoadBalancing;
+	bool enableSleepForTimestamps;
+};
+
+#define PP_TEMPERATURE_UNITS_PER_CENTIGRADES 1000
+
+/**
+ * Type to hold a temperature range.
+ */
+struct PP_TemperatureRange {
+	uint32_t min;
+	uint32_t max;
+};
+
+struct PP_StateValidationBlock {
+	bool singleDisplayOnly;
+	bool disallowOnDC;
+	uint8_t supportedPowerLevels;
+};
+
+struct PP_UVD_CLOCKS {
+	uint32_t VCLK;
+	uint32_t DCLK;
+};
+
+/**
+* Structure to hold a PowerPlay Power State.
+*/
+struct pp_power_state {
+	uint32_t                            id;
+	struct PP_StateLinkedList                  orderedList;
+	struct PP_StateLinkedList                  allStatesList;
+
+	struct PP_StateClassificationBlock         classification;
+	struct PP_StateValidationBlock             validation;
+	struct PP_StatePcieBlock                   pcie;
+	struct PP_StateDisplayBlock                display;
+	struct PP_StateMemroyBlock                 memory;
+	struct PP_TemperatureRange                 temperatures;
+	struct PP_StateSoftwareAlgorithmBlock      software;
+	struct PP_UVD_CLOCKS                       uvd_clocks;
+	struct pp_hw_power_state  hardware;
+};
+
+
+/*Structure to hold a VCE state entry*/
+struct PP_VCEState {
+	uint32_t evclk;
+	uint32_t ecclk;
+	uint32_t sclk;
+	uint32_t mclk;
+};
+
+enum PP_MMProfilingState {
+	PP_MMProfilingState_NA = 0,
+	PP_MMProfilingState_Started,
+	PP_MMProfilingState_Stopped
+};
+
+struct PP_Clock_Engine_Request {
+	unsigned long clientType;
+	unsigned long ctxid;
+	uint64_t  context_handle;
+	unsigned long sclk;
+	unsigned long sclkHardMin;
+	unsigned long mclk;
+	unsigned long iclk;
+	unsigned long evclk;
+	unsigned long ecclk;
+	unsigned long ecclkHardMin;
+	unsigned long vclk;
+	unsigned long dclk;
+	unsigned long samclk;
+	unsigned long acpclk;
+	unsigned long sclkOverdrive;
+	unsigned long mclkOverdrive;
+	unsigned long sclk_threshold;
+	unsigned long flag;
+	unsigned long vclk_ceiling;
+	unsigned long dclk_ceiling;
+	unsigned long num_cus;
+	unsigned long pmflag;
+	enum PP_MMProfilingState MMProfilingState;
+};
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_acpi.h b/drivers/gpu/drm/amd/powerplay/inc/pp_acpi.h
new file mode 100644
index 000000000000..3bd5e69b9045
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_acpi.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+extern bool acpi_atcs_functions_supported(void *device,
+							uint32_t index);
+extern int acpi_pcie_perf_request(void *device,
+						uint8_t perf_req,
+						bool advertise);
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_asicblocks.h b/drivers/gpu/drm/amd/powerplay/inc/pp_asicblocks.h
new file mode 100644
index 000000000000..0c1593e53654
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_asicblocks.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef PP_ASICBLOCKS_H
+#define PP_ASICBLOCKS_H
+
+
+enum PHM_AsicBlock {
+	PHM_AsicBlock_GFX,
+	PHM_AsicBlock_UVD_MVC,
+	PHM_AsicBlock_UVD,
+	PHM_AsicBlock_UVD_HD,
+	PHM_AsicBlock_UVD_SD,
+	PHM_AsicBlock_Count
+};
+
+enum PHM_ClockGateSetting {
+	PHM_ClockGateSetting_StaticOn,
+	PHM_ClockGateSetting_StaticOff,
+	PHM_ClockGateSetting_Dynamic
+};
+
+struct phm_asic_blocks {
+	bool gfx : 1;
+	bool uvd : 1;
+};
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_debug.h b/drivers/gpu/drm/amd/powerplay/inc/pp_debug.h
new file mode 100644
index 000000000000..d7d83b7c7f95
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_debug.h
@@ -0,0 +1,47 @@
+
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef PP_DEBUG_H
+#define PP_DEBUG_H
+
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#define PP_ASSERT_WITH_CODE(cond, msg, code)	\
+	do {					\
+		if (!(cond)) {			\
+			printk("%s\n", msg);	\
+			code;			\
+		}				\
+	} while (0)
+
+
+#define PP_DBG_LOG(fmt, ...) \
+	do { \
+		if(0)printk(KERN_INFO "[ pp_dbg ] " fmt, ##__VA_ARGS__); \
+	} while (0)
+
+
+#endif /* PP_DEBUG_H */
+
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_feature.h b/drivers/gpu/drm/amd/powerplay/inc/pp_feature.h
new file mode 100644
index 000000000000..0faf6a25c18b
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_feature.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _PP_FEATURE_H_
+#define _PP_FEATURE_H_
+
+/**
+ * PowerPlay feature ids.
+ */
+enum pp_feature {
+	PP_Feature_PowerPlay = 0,
+	PP_Feature_User2DPerformance,
+	PP_Feature_User3DPerformance,
+	PP_Feature_VariBright,
+	PP_Feature_VariBrightOnPowerXpress,
+	PP_Feature_ReducedRefreshRate,
+	PP_Feature_GFXClockGating,
+	PP_Feature_OverdriveTest,
+	PP_Feature_OverDrive,
+	PP_Feature_PowerBudgetWaiver,
+	PP_Feature_PowerControl,
+	PP_Feature_PowerControl_2,
+	PP_Feature_MultiUVDState,
+	PP_Feature_Force3DClock,
+	PP_Feature_BACO,
+	PP_Feature_PowerDown,
+	PP_Feature_DynamicUVDState,
+	PP_Feature_VCEDPM,
+	PP_Feature_PPM,
+	PP_Feature_ACP_POWERGATING,
+	PP_Feature_FFC,
+	PP_Feature_FPS,
+	PP_Feature_ViPG,
+	PP_Feature_Max
+};
+
+/**
+ * Struct for PowerPlay feature info.
+ */
+struct pp_feature_info {
+	bool supported;               /* feature supported by PowerPlay */
+	bool enabled;                 /* feature enabled in PowerPlay */
+	bool enabled_default;        /* default enable status of the feature */
+	uint32_t version;             /* feature version */
+};
+
+#endif /* _PP_FEATURE_H_ */
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_instance.h b/drivers/gpu/drm/amd/powerplay/inc/pp_instance.h
new file mode 100644
index 000000000000..4d8ed1f33de4
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_instance.h
@@ -0,0 +1,39 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _PP_INSTANCE_H_
+#define _PP_INSTANCE_H_
+
+#include "smumgr.h"
+#include "hwmgr.h"
+#include "eventmgr.h"
+
+#define PP_VALID  0x1F1F1F1F
+
+struct pp_instance {
+	uint32_t pp_valid;
+	struct pp_smumgr *smu_mgr;
+	struct pp_hwmgr *hwmgr;
+	struct pp_eventmgr *eventmgr;
+};
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/pp_power_source.h b/drivers/gpu/drm/amd/powerplay/inc/pp_power_source.h
new file mode 100644
index 000000000000..b43315cc5d58
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/pp_power_source.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef PP_POWERSOURCE_H
+#define PP_POWERSOURCE_H
+
+enum pp_power_source {
+	PP_PowerSource_AC = 0,
+	PP_PowerSource_DC,
+	PP_PowerSource_LimitedPower,
+	PP_PowerSource_LimitedPower_2,
+	PP_PowerSource_Max
+};
+
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/ppinterrupt.h b/drivers/gpu/drm/amd/powerplay/inc/ppinterrupt.h
new file mode 100644
index 000000000000..c067e0925b6b
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/ppinterrupt.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _PP_INTERRUPT_H_
+#define _PP_INTERRUPT_H_
+
+enum amd_thermal_irq {
+	AMD_THERMAL_IRQ_LOW_TO_HIGH = 0,
+	AMD_THERMAL_IRQ_HIGH_TO_LOW,
+
+	AMD_THERMAL_IRQ_LAST
+};
+
+/* The type of the interrupt callback functions in PowerPlay */
+typedef int (*irq_handler_func_t)(void *private_data,
+				unsigned src_id, const uint32_t *iv_entry);
+
+/* Event Manager action chain list information */
+struct pp_interrupt_registration_info {
+	irq_handler_func_t call_back; /* Pointer to callback function */
+	void *context;                   /* Pointer to callback function context */
+	uint32_t src_id;               /* Registered interrupt id */
+	const uint32_t *iv_entry;
+};
+
+#endif /* _PP_INTERRUPT_H_ */
diff --git a/drivers/gpu/drm/amd/amdgpu/smu7.h b/drivers/gpu/drm/amd/powerplay/inc/smu7.h
index 75a380a15292..75a380a15292 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu7.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu7.h
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smu72.h b/drivers/gpu/drm/amd/powerplay/inc/smu72.h
new file mode 100644
index 000000000000..b73d6b59ac32
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu72.h
@@ -0,0 +1,664 @@
+#ifndef SMU72_H
+#define SMU72_H
+
+#if !defined(SMC_MICROCODE)
+#pragma pack(push, 1)
+#endif
+
+#define SMU__NUM_SCLK_DPM_STATE  8
+#define SMU__NUM_MCLK_DPM_LEVELS 4
+#define SMU__NUM_LCLK_DPM_LEVELS 8
+#define SMU__NUM_PCIE_DPM_LEVELS 8
+
+enum SID_OPTION {
+	SID_OPTION_HI,
+	SID_OPTION_LO,
+	SID_OPTION_COUNT
+};
+
+enum Poly3rdOrderCoeff {
+	LEAKAGE_TEMPERATURE_SCALAR,
+	LEAKAGE_VOLTAGE_SCALAR,
+	DYNAMIC_VOLTAGE_SCALAR,
+	POLY_3RD_ORDER_COUNT
+};
+
+struct SMU7_Poly3rdOrder_Data {
+	int32_t a;
+	int32_t b;
+	int32_t c;
+	int32_t d;
+	uint8_t a_shift;
+	uint8_t b_shift;
+	uint8_t c_shift;
+	uint8_t x_shift;
+};
+
+typedef struct SMU7_Poly3rdOrder_Data SMU7_Poly3rdOrder_Data;
+
+struct Power_Calculator_Data {
+	uint16_t NoLoadVoltage;
+	uint16_t LoadVoltage;
+	uint16_t Resistance;
+	uint16_t Temperature;
+	uint16_t BaseLeakage;
+	uint16_t LkgTempScalar;
+	uint16_t LkgVoltScalar;
+	uint16_t LkgAreaScalar;
+	uint16_t LkgPower;
+	uint16_t DynVoltScalar;
+	uint32_t Cac;
+	uint32_t DynPower;
+	uint32_t TotalCurrent;
+	uint32_t TotalPower;
+};
+
+typedef struct Power_Calculator_Data PowerCalculatorData_t;
+
+struct Gc_Cac_Weight_Data {
+	uint8_t index;
+	uint32_t value;
+};
+
+typedef struct Gc_Cac_Weight_Data GcCacWeight_Data;
+
+
+typedef struct {
+	uint32_t high;
+	uint32_t low;
+} data_64_t;
+
+typedef struct {
+	data_64_t high;
+	data_64_t low;
+} data_128_t;
+
+#define SMU7_CONTEXT_ID_SMC        1
+#define SMU7_CONTEXT_ID_VBIOS      2
+
+#define SMU72_MAX_LEVELS_VDDC            16
+#define SMU72_MAX_LEVELS_VDDGFX          16
+#define SMU72_MAX_LEVELS_VDDCI           8
+#define SMU72_MAX_LEVELS_MVDD            4
+
+#define SMU_MAX_SMIO_LEVELS              4
+
+#define SMU72_MAX_LEVELS_GRAPHICS        SMU__NUM_SCLK_DPM_STATE   /* SCLK + SQ DPM + ULV */
+#define SMU72_MAX_LEVELS_MEMORY          SMU__NUM_MCLK_DPM_LEVELS   /* MCLK Levels DPM */
+#define SMU72_MAX_LEVELS_GIO             SMU__NUM_LCLK_DPM_LEVELS  /* LCLK Levels */
+#define SMU72_MAX_LEVELS_LINK            SMU__NUM_PCIE_DPM_LEVELS  /* PCIe speed and number of lanes. */
+#define SMU72_MAX_LEVELS_UVD             8   /* VCLK/DCLK levels for UVD. */
+#define SMU72_MAX_LEVELS_VCE             8   /* ECLK levels for VCE. */
+#define SMU72_MAX_LEVELS_ACP             8   /* ACLK levels for ACP. */
+#define SMU72_MAX_LEVELS_SAMU            8   /* SAMCLK levels for SAMU. */
+#define SMU72_MAX_ENTRIES_SMIO           32  /* Number of entries in SMIO table. */
+
+#define DPM_NO_LIMIT 0
+#define DPM_NO_UP 1
+#define DPM_GO_DOWN 2
+#define DPM_GO_UP 3
+
+#define SMU7_FIRST_DPM_GRAPHICS_LEVEL    0
+#define SMU7_FIRST_DPM_MEMORY_LEVEL      0
+
+#define GPIO_CLAMP_MODE_VRHOT      1
+#define GPIO_CLAMP_MODE_THERM      2
+#define GPIO_CLAMP_MODE_DC         4
+
+#define SCRATCH_B_TARG_PCIE_INDEX_SHIFT 0
+#define SCRATCH_B_TARG_PCIE_INDEX_MASK  (0x7<<SCRATCH_B_TARG_PCIE_INDEX_SHIFT)
+#define SCRATCH_B_CURR_PCIE_INDEX_SHIFT 3
+#define SCRATCH_B_CURR_PCIE_INDEX_MASK  (0x7<<SCRATCH_B_CURR_PCIE_INDEX_SHIFT)
+#define SCRATCH_B_TARG_UVD_INDEX_SHIFT  6
+#define SCRATCH_B_TARG_UVD_INDEX_MASK   (0x7<<SCRATCH_B_TARG_UVD_INDEX_SHIFT)
+#define SCRATCH_B_CURR_UVD_INDEX_SHIFT  9
+#define SCRATCH_B_CURR_UVD_INDEX_MASK   (0x7<<SCRATCH_B_CURR_UVD_INDEX_SHIFT)
+#define SCRATCH_B_TARG_VCE_INDEX_SHIFT  12
+#define SCRATCH_B_TARG_VCE_INDEX_MASK   (0x7<<SCRATCH_B_TARG_VCE_INDEX_SHIFT)
+#define SCRATCH_B_CURR_VCE_INDEX_SHIFT  15
+#define SCRATCH_B_CURR_VCE_INDEX_MASK   (0x7<<SCRATCH_B_CURR_VCE_INDEX_SHIFT)
+#define SCRATCH_B_TARG_ACP_INDEX_SHIFT  18
+#define SCRATCH_B_TARG_ACP_INDEX_MASK   (0x7<<SCRATCH_B_TARG_ACP_INDEX_SHIFT)
+#define SCRATCH_B_CURR_ACP_INDEX_SHIFT  21
+#define SCRATCH_B_CURR_ACP_INDEX_MASK   (0x7<<SCRATCH_B_CURR_ACP_INDEX_SHIFT)
+#define SCRATCH_B_TARG_SAMU_INDEX_SHIFT 24
+#define SCRATCH_B_TARG_SAMU_INDEX_MASK  (0x7<<SCRATCH_B_TARG_SAMU_INDEX_SHIFT)
+#define SCRATCH_B_CURR_SAMU_INDEX_SHIFT 27
+#define SCRATCH_B_CURR_SAMU_INDEX_MASK  (0x7<<SCRATCH_B_CURR_SAMU_INDEX_SHIFT)
+
+/* Virtualization Defines */
+#define CG_XDMA_MASK  0x1
+#define CG_XDMA_SHIFT 0
+#define CG_UVD_MASK   0x2
+#define CG_UVD_SHIFT  1
+#define CG_VCE_MASK   0x4
+#define CG_VCE_SHIFT  2
+#define CG_SAMU_MASK  0x8
+#define CG_SAMU_SHIFT 3
+#define CG_GFX_MASK   0x10
+#define CG_GFX_SHIFT  4
+#define CG_SDMA_MASK  0x20
+#define CG_SDMA_SHIFT 5
+#define CG_HDP_MASK   0x40
+#define CG_HDP_SHIFT  6
+#define CG_MC_MASK    0x80
+#define CG_MC_SHIFT   7
+#define CG_DRM_MASK   0x100
+#define CG_DRM_SHIFT  8
+#define CG_ROM_MASK   0x200
+#define CG_ROM_SHIFT  9
+#define CG_BIF_MASK   0x400
+#define CG_BIF_SHIFT  10
+
+#define SMU72_DTE_ITERATIONS 5
+#define SMU72_DTE_SOURCES 3
+#define SMU72_DTE_SINKS 1
+#define SMU72_NUM_CPU_TES 0
+#define SMU72_NUM_GPU_TES 1
+#define SMU72_NUM_NON_TES 2
+#define SMU72_DTE_FAN_SCALAR_MIN 0x100
+#define SMU72_DTE_FAN_SCALAR_MAX 0x166
+#define SMU72_DTE_FAN_TEMP_MAX 93
+#define SMU72_DTE_FAN_TEMP_MIN 83
+
+#if defined SMU__FUSION_ONLY
+#define SMU7_DTE_ITERATIONS 5
+#define SMU7_DTE_SOURCES 5
+#define SMU7_DTE_SINKS 3
+#define SMU7_NUM_CPU_TES 2
+#define SMU7_NUM_GPU_TES 1
+#define SMU7_NUM_NON_TES 2
+#endif
+
+struct SMU7_HystController_Data {
+	uint8_t waterfall_up;
+	uint8_t waterfall_down;
+	uint8_t waterfall_limit;
+	uint8_t spare;
+	uint16_t release_cnt;
+	uint16_t release_limit;
+};
+
+typedef struct SMU7_HystController_Data SMU7_HystController_Data;
+
+struct SMU72_PIDController {
+	uint32_t Ki;
+	int32_t LFWindupUpperLim;
+	int32_t LFWindupLowerLim;
+	uint32_t StatePrecision;
+	uint32_t LfPrecision;
+	uint32_t LfOffset;
+	uint32_t MaxState;
+	uint32_t MaxLfFraction;
+	uint32_t StateShift;
+};
+
+typedef struct SMU72_PIDController SMU72_PIDController;
+
+struct SMU7_LocalDpmScoreboard {
+	uint32_t PercentageBusy;
+
+	int32_t  PIDError;
+	int32_t  PIDIntegral;
+	int32_t  PIDOutput;
+
+	uint32_t SigmaDeltaAccum;
+	uint32_t SigmaDeltaOutput;
+	uint32_t SigmaDeltaLevel;
+
+	uint32_t UtilizationSetpoint;
+
+	uint8_t  TdpClampMode;
+	uint8_t  TdcClampMode;
+	uint8_t  ThermClampMode;
+	uint8_t  VoltageBusy;
+
+	int8_t   CurrLevel;
+	int8_t   TargLevel;
+	uint8_t  LevelChangeInProgress;
+	uint8_t  UpHyst;
+
+	uint8_t  DownHyst;
+	uint8_t  VoltageDownHyst;
+	uint8_t  DpmEnable;
+	uint8_t  DpmRunning;
+
+	uint8_t  DpmForce;
+	uint8_t  DpmForceLevel;
+	uint8_t  DisplayWatermark;
+	uint8_t  McArbIndex;
+
+	uint32_t MinimumPerfSclk;
+
+	uint8_t  AcpiReq;
+	uint8_t  AcpiAck;
+	uint8_t  GfxClkSlow;
+	uint8_t  GpioClampMode; /* bit0 = VRHOT: bit1 = THERM: bit2 = DC */
+
+	uint8_t  FpsFilterWeight;
+	uint8_t  EnabledLevelsChange;
+	uint8_t  DteClampMode;
+	uint8_t  FpsClampMode;
+
+	uint16_t LevelResidencyCounters[SMU72_MAX_LEVELS_GRAPHICS];
+	uint16_t LevelSwitchCounters[SMU72_MAX_LEVELS_GRAPHICS];
+
+	void     (*TargetStateCalculator)(uint8_t);
+	void     (*SavedTargetStateCalculator)(uint8_t);
+
+	uint16_t AutoDpmInterval;
+	uint16_t AutoDpmRange;
+
+	uint8_t  FpsEnabled;
+	uint8_t  MaxPerfLevel;
+	uint8_t  AllowLowClkInterruptToHost;
+	uint8_t  FpsRunning;
+
+	uint32_t MaxAllowedFrequency;
+
+	uint32_t FilteredSclkFrequency;
+	uint32_t LastSclkFrequency;
+	uint32_t FilteredSclkFrequencyCnt;
+};
+
+typedef struct SMU7_LocalDpmScoreboard SMU7_LocalDpmScoreboard;
+
+#define SMU7_MAX_VOLTAGE_CLIENTS 12
+
+typedef uint8_t (*VoltageChangeHandler_t)(uint16_t, uint8_t);
+
+struct SMU_VoltageLevel {
+	uint8_t Vddc;
+	uint8_t Vddci;
+	uint8_t VddGfx;
+	uint8_t Phases;
+};
+
+typedef struct SMU_VoltageLevel SMU_VoltageLevel;
+
+struct SMU7_VoltageScoreboard {
+	SMU_VoltageLevel CurrentVoltage;
+	SMU_VoltageLevel TargetVoltage;
+	uint16_t MaxVid;
+	uint8_t  HighestVidOffset;
+	uint8_t  CurrentVidOffset;
+
+	uint8_t  ControllerBusy;
+	uint8_t  CurrentVid;
+	uint8_t  CurrentVddciVid;
+	uint8_t  VddGfxShutdown; /* 0 = normal mode, 1 = shut down */
+
+	SMU_VoltageLevel RequestedVoltage[SMU7_MAX_VOLTAGE_CLIENTS];
+	uint8_t  EnabledRequest[SMU7_MAX_VOLTAGE_CLIENTS];
+
+	uint8_t  TargetIndex;
+	uint8_t  Delay;
+	uint8_t  ControllerEnable;
+	uint8_t  ControllerRunning;
+	uint16_t CurrentStdVoltageHiSidd;
+	uint16_t CurrentStdVoltageLoSidd;
+	uint8_t  OverrideVoltage;
+	uint8_t  VddcUseUlvOffset;
+	uint8_t  VddGfxUseUlvOffset;
+	uint8_t  padding;
+
+	VoltageChangeHandler_t ChangeVddc;
+	VoltageChangeHandler_t ChangeVddGfx;
+	VoltageChangeHandler_t ChangeVddci;
+	VoltageChangeHandler_t ChangePhase;
+	VoltageChangeHandler_t ChangeMvdd;
+
+	VoltageChangeHandler_t functionLinks[6];
+
+	uint8_t *VddcFollower1;
+	uint8_t *VddcFollower2;
+	int16_t  Driver_OD_RequestedVidOffset1;
+	int16_t  Driver_OD_RequestedVidOffset2;
+
+};
+
+typedef struct SMU7_VoltageScoreboard SMU7_VoltageScoreboard;
+
+#define SMU7_MAX_PCIE_LINK_SPEEDS 3 /* 0:Gen1 1:Gen2 2:Gen3 */
+
+struct SMU7_PCIeLinkSpeedScoreboard {
+	uint8_t     DpmEnable;
+	uint8_t     DpmRunning;
+	uint8_t     DpmForce;
+	uint8_t     DpmForceLevel;
+
+	uint8_t     CurrentLinkSpeed;
+	uint8_t     EnabledLevelsChange;
+	uint16_t    AutoDpmInterval;
+
+	uint16_t    AutoDpmRange;
+	uint16_t    AutoDpmCount;
+
+	uint8_t     DpmMode;
+	uint8_t     AcpiReq;
+	uint8_t     AcpiAck;
+	uint8_t     CurrentLinkLevel;
+
+};
+
+typedef struct SMU7_PCIeLinkSpeedScoreboard SMU7_PCIeLinkSpeedScoreboard;
+
+/* -------------------------------------------------------- CAC table ------------------------------------------------------ */
+#define SMU7_LKGE_LUT_NUM_OF_TEMP_ENTRIES 16
+#define SMU7_LKGE_LUT_NUM_OF_VOLT_ENTRIES 16
+#define SMU7_SCALE_I  7
+#define SMU7_SCALE_R 12
+
+struct SMU7_PowerScoreboard {
+	PowerCalculatorData_t VddGfxPowerData[SID_OPTION_COUNT];
+	PowerCalculatorData_t VddcPowerData[SID_OPTION_COUNT];
+
+	uint32_t TotalGpuPower;
+	uint32_t TdcCurrent;
+
+	uint16_t   VddciTotalPower;
+	uint16_t   sparesasfsdfd;
+	uint16_t   Vddr1Power;
+	uint16_t   RocPower;
+
+	uint16_t   CalcMeasPowerBlend;
+	uint8_t    SidOptionPower;
+	uint8_t    SidOptionCurrent;
+
+	uint32_t   WinTime;
+
+	uint16_t Telemetry_1_slope;
+	uint16_t Telemetry_2_slope;
+	int32_t Telemetry_1_offset;
+	int32_t Telemetry_2_offset;
+
+	uint32_t VddcCurrentTelemetry;
+	uint32_t VddGfxCurrentTelemetry;
+	uint32_t VddcPowerTelemetry;
+	uint32_t VddGfxPowerTelemetry;
+	uint32_t VddciPowerTelemetry;
+
+	uint32_t VddcPower;
+	uint32_t VddGfxPower;
+	uint32_t VddciPower;
+
+	uint32_t TelemetryCurrent[2];
+	uint32_t TelemetryVoltage[2];
+	uint32_t TelemetryPower[2];
+};
+
+typedef struct SMU7_PowerScoreboard SMU7_PowerScoreboard;
+
+struct SMU7_ThermalScoreboard {
+	int16_t  GpuLimit;
+	int16_t  GpuHyst;
+	uint16_t CurrGnbTemp;
+	uint16_t FilteredGnbTemp;
+
+	uint8_t  ControllerEnable;
+	uint8_t  ControllerRunning;
+	uint8_t  AutoTmonCalInterval;
+	uint8_t  AutoTmonCalEnable;
+
+	uint8_t  ThermalDpmEnabled;
+	uint8_t  SclkEnabledMask;
+	uint8_t  spare[2];
+	int32_t  temperature_gradient;
+
+	SMU7_HystController_Data HystControllerData;
+	int32_t  WeightedSensorTemperature;
+	uint16_t TemperatureLimit[SMU72_MAX_LEVELS_GRAPHICS];
+	uint32_t Alpha;
+};
+
+typedef struct SMU7_ThermalScoreboard SMU7_ThermalScoreboard;
+
+/* For FeatureEnables: */
+#define SMU7_SCLK_DPM_CONFIG_MASK                        0x01
+#define SMU7_VOLTAGE_CONTROLLER_CONFIG_MASK              0x02
+#define SMU7_THERMAL_CONTROLLER_CONFIG_MASK              0x04
+#define SMU7_MCLK_DPM_CONFIG_MASK                        0x08
+#define SMU7_UVD_DPM_CONFIG_MASK                         0x10
+#define SMU7_VCE_DPM_CONFIG_MASK                         0x20
+#define SMU7_ACP_DPM_CONFIG_MASK                         0x40
+#define SMU7_SAMU_DPM_CONFIG_MASK                        0x80
+#define SMU7_PCIEGEN_DPM_CONFIG_MASK                    0x100
+
+#define SMU7_ACP_MCLK_HANDSHAKE_DISABLE                  0x00000001
+#define SMU7_ACP_SCLK_HANDSHAKE_DISABLE                  0x00000002
+#define SMU7_UVD_MCLK_HANDSHAKE_DISABLE                  0x00000100
+#define SMU7_UVD_SCLK_HANDSHAKE_DISABLE                  0x00000200
+#define SMU7_VCE_MCLK_HANDSHAKE_DISABLE                  0x00010000
+#define SMU7_VCE_SCLK_HANDSHAKE_DISABLE                  0x00020000
+
+/* All 'soft registers' should be uint32_t. */
+struct SMU72_SoftRegisters {
+	uint32_t        RefClockFrequency;
+	uint32_t        PmTimerPeriod;
+	uint32_t        FeatureEnables;
+
+	uint32_t        PreVBlankGap;
+	uint32_t        VBlankTimeout;
+	uint32_t        TrainTimeGap;
+
+	uint32_t        MvddSwitchTime;
+	uint32_t        LongestAcpiTrainTime;
+	uint32_t        AcpiDelay;
+	uint32_t        G5TrainTime;
+	uint32_t        DelayMpllPwron;
+	uint32_t        VoltageChangeTimeout;
+
+	uint32_t        HandshakeDisables;
+
+	uint8_t         DisplayPhy1Config;
+	uint8_t         DisplayPhy2Config;
+	uint8_t         DisplayPhy3Config;
+	uint8_t         DisplayPhy4Config;
+
+	uint8_t         DisplayPhy5Config;
+	uint8_t         DisplayPhy6Config;
+	uint8_t         DisplayPhy7Config;
+	uint8_t         DisplayPhy8Config;
+
+	uint32_t        AverageGraphicsActivity;
+	uint32_t        AverageMemoryActivity;
+	uint32_t        AverageGioActivity;
+
+	uint8_t         SClkDpmEnabledLevels;
+	uint8_t         MClkDpmEnabledLevels;
+	uint8_t         LClkDpmEnabledLevels;
+	uint8_t         PCIeDpmEnabledLevels;
+
+	uint8_t         UVDDpmEnabledLevels;
+	uint8_t         SAMUDpmEnabledLevels;
+	uint8_t         ACPDpmEnabledLevels;
+	uint8_t         VCEDpmEnabledLevels;
+
+	uint32_t        DRAM_LOG_ADDR_H;
+	uint32_t        DRAM_LOG_ADDR_L;
+	uint32_t        DRAM_LOG_PHY_ADDR_H;
+	uint32_t        DRAM_LOG_PHY_ADDR_L;
+	uint32_t        DRAM_LOG_BUFF_SIZE;
+	uint32_t        UlvEnterCount;
+	uint32_t        UlvTime;
+	uint32_t        UcodeLoadStatus;
+	uint32_t        Reserved[2];
+
+};
+
+typedef struct SMU72_SoftRegisters SMU72_SoftRegisters;
+
+struct SMU72_Firmware_Header {
+	uint32_t Digest[5];
+	uint32_t Version;
+	uint32_t HeaderSize;
+	uint32_t Flags;
+	uint32_t EntryPoint;
+	uint32_t CodeSize;
+	uint32_t ImageSize;
+
+	uint32_t Rtos;
+	uint32_t SoftRegisters;
+	uint32_t DpmTable;
+	uint32_t FanTable;
+	uint32_t CacConfigTable;
+	uint32_t CacStatusTable;
+	uint32_t mcRegisterTable;
+	uint32_t mcArbDramTimingTable;
+	uint32_t PmFuseTable;
+	uint32_t Globals;
+	uint32_t ClockStretcherTable;
+	uint32_t Reserved[41];
+	uint32_t Signature;
+};
+
+typedef struct SMU72_Firmware_Header SMU72_Firmware_Header;
+
+#define SMU72_FIRMWARE_HEADER_LOCATION 0x20000
+
+enum  DisplayConfig {
+	PowerDown = 1,
+	DP54x4,
+	DP54x2,
+	DP54x1,
+	DP27x4,
+	DP27x2,
+	DP27x1,
+	HDMI297,
+	HDMI162,
+	LVDS,
+	DP324x4,
+	DP324x2,
+	DP324x1
+};
+
+#define MC_BLOCK_COUNT 1
+#define CPL_BLOCK_COUNT 5
+#define SE_BLOCK_COUNT 15
+#define GC_BLOCK_COUNT 24
+
+struct SMU7_Local_Cac {
+	uint8_t BlockId;
+	uint8_t SignalId;
+	uint8_t Threshold;
+	uint8_t Padding;
+};
+
+typedef struct SMU7_Local_Cac SMU7_Local_Cac;
+
+struct SMU7_Local_Cac_Table {
+	SMU7_Local_Cac CplLocalCac[CPL_BLOCK_COUNT];
+	SMU7_Local_Cac McLocalCac[MC_BLOCK_COUNT];
+	SMU7_Local_Cac SeLocalCac[SE_BLOCK_COUNT];
+	SMU7_Local_Cac GcLocalCac[GC_BLOCK_COUNT];
+};
+
+typedef struct SMU7_Local_Cac_Table SMU7_Local_Cac_Table;
+
+#if !defined(SMC_MICROCODE)
+#pragma pack(pop)
+#endif
+
+/* Description of Clock Gating bitmask for Tonga: */
+/* System Clock Gating */
+#define CG_SYS_BITMASK_FIRST_BIT      0  /* First bit of Sys CG bitmask */
+#define CG_SYS_BITMASK_LAST_BIT       9  /* Last bit of Sys CG bitmask */
+#define CG_SYS_BIF_MGLS_SHIFT         0
+#define CG_SYS_ROM_SHIFT              1
+#define CG_SYS_MC_MGCG_SHIFT          2
+#define CG_SYS_MC_MGLS_SHIFT          3
+#define CG_SYS_SDMA_MGCG_SHIFT        4
+#define CG_SYS_SDMA_MGLS_SHIFT        5
+#define CG_SYS_DRM_MGCG_SHIFT         6
+#define CG_SYS_HDP_MGCG_SHIFT         7
+#define CG_SYS_HDP_MGLS_SHIFT         8
+#define CG_SYS_DRM_MGLS_SHIFT         9
+
+#define CG_SYS_BIF_MGLS_MASK          0x1
+#define CG_SYS_ROM_MASK               0x2
+#define CG_SYS_MC_MGCG_MASK           0x4
+#define CG_SYS_MC_MGLS_MASK           0x8
+#define CG_SYS_SDMA_MGCG_MASK         0x10
+#define CG_SYS_SDMA_MGLS_MASK         0x20
+#define CG_SYS_DRM_MGCG_MASK          0x40
+#define CG_SYS_HDP_MGCG_MASK          0x80
+#define CG_SYS_HDP_MGLS_MASK          0x100
+#define CG_SYS_DRM_MGLS_MASK          0x200
+
+/* Graphics Clock Gating */
+#define CG_GFX_BITMASK_FIRST_BIT      16 /* First bit of Gfx CG bitmask */
+#define CG_GFX_BITMASK_LAST_BIT       20 /* Last bit of Gfx CG bitmask */
+#define CG_GFX_CGCG_SHIFT             16
+#define CG_GFX_CGLS_SHIFT             17
+#define CG_CPF_MGCG_SHIFT             18
+#define CG_RLC_MGCG_SHIFT             19
+#define CG_GFX_OTHERS_MGCG_SHIFT      20
+
+#define CG_GFX_CGCG_MASK              0x00010000
+#define CG_GFX_CGLS_MASK              0x00020000
+#define CG_CPF_MGCG_MASK              0x00040000
+#define CG_RLC_MGCG_MASK              0x00080000
+#define CG_GFX_OTHERS_MGCG_MASK       0x00100000
+
+/* Voltage Regulator Configuration */
+/* VR Config info is contained in dpmTable.VRConfig */
+
+#define VRCONF_VDDC_MASK         0x000000FF
+#define VRCONF_VDDC_SHIFT        0
+#define VRCONF_VDDGFX_MASK       0x0000FF00
+#define VRCONF_VDDGFX_SHIFT      8
+#define VRCONF_VDDCI_MASK        0x00FF0000
+#define VRCONF_VDDCI_SHIFT       16
+#define VRCONF_MVDD_MASK         0xFF000000
+#define VRCONF_MVDD_SHIFT        24
+
+#define VR_MERGED_WITH_VDDC      0
+#define VR_SVI2_PLANE_1          1
+#define VR_SVI2_PLANE_2          2
+#define VR_SMIO_PATTERN_1        3
+#define VR_SMIO_PATTERN_2        4
+#define VR_STATIC_VOLTAGE        5
+
+/* Clock Stretcher Configuration */
+
+#define CLOCK_STRETCHER_MAX_ENTRIES 0x4
+#define CKS_LOOKUPTable_MAX_ENTRIES 0x4
+
+/* The 'settings' field is subdivided in the following way: */
+#define CLOCK_STRETCHER_SETTING_DDT_MASK             0x01
+#define CLOCK_STRETCHER_SETTING_DDT_SHIFT            0x0
+#define CLOCK_STRETCHER_SETTING_STRETCH_AMOUNT_MASK  0x1E
+#define CLOCK_STRETCHER_SETTING_STRETCH_AMOUNT_SHIFT 0x1
+#define CLOCK_STRETCHER_SETTING_ENABLE_MASK          0x80
+#define CLOCK_STRETCHER_SETTING_ENABLE_SHIFT         0x7
+
+struct SMU_ClockStretcherDataTableEntry {
+	uint8_t minVID;
+	uint8_t maxVID;
+
+	uint16_t setting;
+};
+typedef struct SMU_ClockStretcherDataTableEntry SMU_ClockStretcherDataTableEntry;
+
+struct SMU_ClockStretcherDataTable {
+	SMU_ClockStretcherDataTableEntry ClockStretcherDataTableEntry[CLOCK_STRETCHER_MAX_ENTRIES];
+};
+typedef struct SMU_ClockStretcherDataTable SMU_ClockStretcherDataTable;
+
+struct SMU_CKS_LOOKUPTableEntry {
+	uint16_t minFreq;
+	uint16_t maxFreq;
+
+	uint8_t setting;
+	uint8_t padding[3];
+};
+typedef struct SMU_CKS_LOOKUPTableEntry SMU_CKS_LOOKUPTableEntry;
+
+struct SMU_CKS_LOOKUPTable {
+	SMU_CKS_LOOKUPTableEntry CKS_LOOKUPTableEntry[CKS_LOOKUPTable_MAX_ENTRIES];
+};
+typedef struct SMU_CKS_LOOKUPTable SMU_CKS_LOOKUPTable;
+
+#endif
+
+
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smu72_discrete.h b/drivers/gpu/drm/amd/powerplay/inc/smu72_discrete.h
new file mode 100644
index 000000000000..98f76e925e65
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu72_discrete.h
@@ -0,0 +1,760 @@
+#ifndef SMU72_DISCRETE_H
+#define SMU72_DISCRETE_H
+
+#include "smu72.h"
+
+#if !defined(SMC_MICROCODE)
+#pragma pack(push, 1)
+#endif
+
+struct SMIO_Pattern {
+	uint16_t Voltage;
+	uint8_t  Smio;
+	uint8_t  padding;
+};
+
+typedef struct SMIO_Pattern SMIO_Pattern;
+
+struct SMIO_Table {
+	SMIO_Pattern Pattern[SMU_MAX_SMIO_LEVELS];
+};
+
+typedef struct SMIO_Table SMIO_Table;
+
+struct SMU72_Discrete_GraphicsLevel {
+	SMU_VoltageLevel MinVoltage;
+
+	uint32_t    SclkFrequency;
+
+	uint8_t     pcieDpmLevel;
+	uint8_t     DeepSleepDivId;
+	uint16_t    ActivityLevel;
+
+	uint32_t    CgSpllFuncCntl3;
+	uint32_t    CgSpllFuncCntl4;
+	uint32_t    SpllSpreadSpectrum;
+	uint32_t    SpllSpreadSpectrum2;
+	uint32_t    CcPwrDynRm;
+	uint32_t    CcPwrDynRm1;
+	uint8_t     SclkDid;
+	uint8_t     DisplayWatermark;
+	uint8_t     EnabledForActivity;
+	uint8_t     EnabledForThrottle;
+	uint8_t     UpHyst;
+	uint8_t     DownHyst;
+	uint8_t     VoltageDownHyst;
+	uint8_t     PowerThrottle;
+};
+
+typedef struct SMU72_Discrete_GraphicsLevel SMU72_Discrete_GraphicsLevel;
+
+struct SMU72_Discrete_ACPILevel {
+	uint32_t    Flags;
+	SMU_VoltageLevel MinVoltage;
+	uint32_t    SclkFrequency;
+	uint8_t     SclkDid;
+	uint8_t     DisplayWatermark;
+	uint8_t     DeepSleepDivId;
+	uint8_t     padding;
+	uint32_t    CgSpllFuncCntl;
+	uint32_t    CgSpllFuncCntl2;
+	uint32_t    CgSpllFuncCntl3;
+	uint32_t    CgSpllFuncCntl4;
+	uint32_t    SpllSpreadSpectrum;
+	uint32_t    SpllSpreadSpectrum2;
+	uint32_t    CcPwrDynRm;
+	uint32_t    CcPwrDynRm1;
+};
+
+typedef struct SMU72_Discrete_ACPILevel SMU72_Discrete_ACPILevel;
+
+struct SMU72_Discrete_Ulv {
+	uint32_t    CcPwrDynRm;
+	uint32_t    CcPwrDynRm1;
+	uint16_t    VddcOffset;
+	uint8_t     VddcOffsetVid;
+	uint8_t     VddcPhase;
+	uint32_t    Reserved;
+};
+
+typedef struct SMU72_Discrete_Ulv SMU72_Discrete_Ulv;
+
+struct SMU72_Discrete_MemoryLevel {
+	SMU_VoltageLevel MinVoltage;
+	uint32_t    MinMvdd;
+
+	uint32_t    MclkFrequency;
+
+	uint8_t     EdcReadEnable;
+	uint8_t     EdcWriteEnable;
+	uint8_t     RttEnable;
+	uint8_t     StutterEnable;
+
+	uint8_t     StrobeEnable;
+	uint8_t     StrobeRatio;
+	uint8_t     EnabledForThrottle;
+	uint8_t     EnabledForActivity;
+
+	uint8_t     UpHyst;
+	uint8_t     DownHyst;
+	uint8_t     VoltageDownHyst;
+	uint8_t     padding;
+
+	uint16_t    ActivityLevel;
+	uint8_t     DisplayWatermark;
+	uint8_t     padding1;
+
+	uint32_t    MpllFuncCntl;
+	uint32_t    MpllFuncCntl_1;
+	uint32_t    MpllFuncCntl_2;
+	uint32_t    MpllAdFuncCntl;
+	uint32_t    MpllDqFuncCntl;
+	uint32_t    MclkPwrmgtCntl;
+	uint32_t    DllCntl;
+	uint32_t    MpllSs1;
+	uint32_t    MpllSs2;
+};
+
+typedef struct SMU72_Discrete_MemoryLevel SMU72_Discrete_MemoryLevel;
+
+struct SMU72_Discrete_LinkLevel {
+	uint8_t     PcieGenSpeed;           /*< 0:PciE-gen1 1:PciE-gen2 2:PciE-gen3 */
+	uint8_t     PcieLaneCount;          /*< 1=x1, 2=x2, 3=x4, 4=x8, 5=x12, 6=x16 */
+	uint8_t     EnabledForActivity;
+	uint8_t     SPC;
+	uint32_t    DownThreshold;
+	uint32_t    UpThreshold;
+	uint32_t    Reserved;
+};
+
+typedef struct SMU72_Discrete_LinkLevel SMU72_Discrete_LinkLevel;
+
+/* MC ARB DRAM Timing registers. */
+struct SMU72_Discrete_MCArbDramTimingTableEntry {
+	uint32_t McArbDramTiming;
+	uint32_t McArbDramTiming2;
+	uint8_t  McArbBurstTime;
+	uint8_t  padding[3];
+};
+
+typedef struct SMU72_Discrete_MCArbDramTimingTableEntry SMU72_Discrete_MCArbDramTimingTableEntry;
+
+struct SMU72_Discrete_MCArbDramTimingTable {
+	SMU72_Discrete_MCArbDramTimingTableEntry entries[SMU__NUM_SCLK_DPM_STATE][SMU__NUM_MCLK_DPM_LEVELS];
+};
+
+typedef struct SMU72_Discrete_MCArbDramTimingTable SMU72_Discrete_MCArbDramTimingTable;
+
+/* UVD VCLK/DCLK state (level) definition. */
+struct SMU72_Discrete_UvdLevel {
+	uint32_t VclkFrequency;
+	uint32_t DclkFrequency;
+	SMU_VoltageLevel MinVoltage;
+	uint8_t  VclkDivider;
+	uint8_t  DclkDivider;
+	uint8_t  padding[2];
+};
+
+typedef struct SMU72_Discrete_UvdLevel SMU72_Discrete_UvdLevel;
+
+/* Clocks for other external blocks (VCE, ACP, SAMU). */
+struct SMU72_Discrete_ExtClkLevel {
+	uint32_t Frequency;
+	SMU_VoltageLevel MinVoltage;
+	uint8_t  Divider;
+	uint8_t  padding[3];
+};
+
+typedef struct SMU72_Discrete_ExtClkLevel SMU72_Discrete_ExtClkLevel;
+
+struct SMU72_Discrete_StateInfo {
+	uint32_t SclkFrequency;
+	uint32_t MclkFrequency;
+	uint32_t VclkFrequency;
+	uint32_t DclkFrequency;
+	uint32_t SamclkFrequency;
+	uint32_t AclkFrequency;
+	uint32_t EclkFrequency;
+	uint16_t MvddVoltage;
+	uint16_t padding16;
+	uint8_t  DisplayWatermark;
+	uint8_t  McArbIndex;
+	uint8_t  McRegIndex;
+	uint8_t  SeqIndex;
+	uint8_t  SclkDid;
+	int8_t   SclkIndex;
+	int8_t   MclkIndex;
+	uint8_t  PCIeGen;
+
+};
+
+typedef struct SMU72_Discrete_StateInfo SMU72_Discrete_StateInfo;
+
+struct SMU72_Discrete_DpmTable {
+	/* Multi-DPM controller settings */
+	SMU72_PIDController                  GraphicsPIDController;
+	SMU72_PIDController                  MemoryPIDController;
+	SMU72_PIDController                  LinkPIDController;
+
+	uint32_t                            SystemFlags;
+
+	/* SMIO masks for voltage and phase controls */
+	uint32_t                            VRConfig;
+	uint32_t                            SmioMask1;
+	uint32_t                            SmioMask2;
+	SMIO_Table                          SmioTable1;
+	SMIO_Table                          SmioTable2;
+
+	uint32_t                            VddcLevelCount;
+	uint32_t                            VddciLevelCount;
+	uint32_t                            VddGfxLevelCount;
+	uint32_t                            MvddLevelCount;
+
+	uint16_t                            VddcTable[SMU72_MAX_LEVELS_VDDC];
+	uint16_t                            VddGfxTable[SMU72_MAX_LEVELS_VDDGFX];
+	uint16_t                            VddciTable[SMU72_MAX_LEVELS_VDDCI];
+
+	uint8_t                             BapmVddGfxVidHiSidd[SMU72_MAX_LEVELS_VDDGFX];
+	uint8_t                             BapmVddGfxVidLoSidd[SMU72_MAX_LEVELS_VDDGFX];
+	uint8_t                             BapmVddGfxVidHiSidd2[SMU72_MAX_LEVELS_VDDGFX];
+
+	uint8_t                             BapmVddcVidHiSidd[SMU72_MAX_LEVELS_VDDC];
+	uint8_t                             BapmVddcVidLoSidd[SMU72_MAX_LEVELS_VDDC];
+	uint8_t                             BapmVddcVidHiSidd2[SMU72_MAX_LEVELS_VDDC];
+
+	uint8_t                             GraphicsDpmLevelCount;
+	uint8_t                             MemoryDpmLevelCount;
+	uint8_t                             LinkLevelCount;
+	uint8_t                             MasterDeepSleepControl;
+
+	uint8_t                             UvdLevelCount;
+	uint8_t                             VceLevelCount;
+	uint8_t                             AcpLevelCount;
+	uint8_t                             SamuLevelCount;
+
+	uint8_t                             ThermOutGpio;
+	uint8_t                             ThermOutPolarity;
+	uint8_t                             ThermOutMode;
+	uint8_t                             DPMFreezeAndForced;
+	uint32_t                            Reserved[4];
+
+	/* State table entries for each DPM state */
+	SMU72_Discrete_GraphicsLevel        GraphicsLevel[SMU72_MAX_LEVELS_GRAPHICS];
+	SMU72_Discrete_MemoryLevel          MemoryACPILevel;
+	SMU72_Discrete_MemoryLevel          MemoryLevel[SMU72_MAX_LEVELS_MEMORY];
+	SMU72_Discrete_LinkLevel            LinkLevel[SMU72_MAX_LEVELS_LINK];
+	SMU72_Discrete_ACPILevel            ACPILevel;
+	SMU72_Discrete_UvdLevel             UvdLevel[SMU72_MAX_LEVELS_UVD];
+	SMU72_Discrete_ExtClkLevel          VceLevel[SMU72_MAX_LEVELS_VCE];
+	SMU72_Discrete_ExtClkLevel          AcpLevel[SMU72_MAX_LEVELS_ACP];
+	SMU72_Discrete_ExtClkLevel          SamuLevel[SMU72_MAX_LEVELS_SAMU];
+	SMU72_Discrete_Ulv                  Ulv;
+
+	uint32_t                            SclkStepSize;
+	uint32_t                            Smio[SMU72_MAX_ENTRIES_SMIO];
+
+	uint8_t                             UvdBootLevel;
+	uint8_t                             VceBootLevel;
+	uint8_t                             AcpBootLevel;
+	uint8_t                             SamuBootLevel;
+
+	uint8_t                             GraphicsBootLevel;
+	uint8_t                             GraphicsVoltageChangeEnable;
+	uint8_t                             GraphicsThermThrottleEnable;
+	uint8_t                             GraphicsInterval;
+
+	uint8_t                             VoltageInterval;
+	uint8_t                             ThermalInterval;
+	uint16_t                            TemperatureLimitHigh;
+
+	uint16_t                            TemperatureLimitLow;
+	uint8_t                             MemoryBootLevel;
+	uint8_t                             MemoryVoltageChangeEnable;
+
+	uint16_t                            BootMVdd;
+	uint8_t                             MemoryInterval;
+	uint8_t                             MemoryThermThrottleEnable;
+
+	uint16_t                            VoltageResponseTime;
+	uint16_t                            PhaseResponseTime;
+
+	uint8_t                             PCIeBootLinkLevel;
+	uint8_t                             PCIeGenInterval;
+	uint8_t                             DTEInterval;
+	uint8_t                             DTEMode;
+
+	uint8_t                             SVI2Enable;
+	uint8_t                             VRHotGpio;
+	uint8_t                             AcDcGpio;
+	uint8_t                             ThermGpio;
+
+	uint16_t                            PPM_PkgPwrLimit;
+	uint16_t                            PPM_TemperatureLimit;
+
+	uint16_t                            DefaultTdp;
+	uint16_t                            TargetTdp;
+
+	uint16_t                            FpsHighThreshold;
+	uint16_t                            FpsLowThreshold;
+
+	uint16_t                            BAPMTI_R[SMU72_DTE_ITERATIONS][SMU72_DTE_SOURCES][SMU72_DTE_SINKS];
+	uint16_t                            BAPMTI_RC[SMU72_DTE_ITERATIONS][SMU72_DTE_SOURCES][SMU72_DTE_SINKS];
+
+	uint8_t                             DTEAmbientTempBase;
+	uint8_t                             DTETjOffset;
+	uint8_t                             GpuTjMax;
+	uint8_t                             GpuTjHyst;
+
+	SMU_VoltageLevel                    BootVoltage;
+
+	uint32_t                            BAPM_TEMP_GRADIENT;
+
+	uint32_t                            LowSclkInterruptThreshold;
+	uint32_t                            VddGfxReChkWait;
+
+	uint8_t                             ClockStretcherAmount;
+
+	uint8_t                             Sclk_CKS_masterEn0_7;
+	uint8_t                             Sclk_CKS_masterEn8_15;
+	uint8_t                             padding[1];
+
+	uint8_t                             Sclk_voltageOffset[8];
+
+	SMU_ClockStretcherDataTable         ClockStretcherDataTable;
+	SMU_CKS_LOOKUPTable                 CKS_LOOKUPTable;
+};
+
+typedef struct SMU72_Discrete_DpmTable SMU72_Discrete_DpmTable;
+
+/* --------------------------------------------------- AC Timing Parameters ------------------------------------------------ */
+#define SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE 16
+#define SMU72_DISCRETE_MC_REGISTER_ARRAY_SET_COUNT SMU72_MAX_LEVELS_MEMORY /* DPM */
+
+struct SMU72_Discrete_MCRegisterAddress {
+	uint16_t s0;
+	uint16_t s1;
+};
+
+typedef struct SMU72_Discrete_MCRegisterAddress SMU72_Discrete_MCRegisterAddress;
+
+struct SMU72_Discrete_MCRegisterSet {
+	uint32_t value[SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE];
+};
+
+typedef struct SMU72_Discrete_MCRegisterSet SMU72_Discrete_MCRegisterSet;
+
+struct SMU72_Discrete_MCRegisters {
+	uint8_t                             last;
+	uint8_t                             reserved[3];
+	SMU72_Discrete_MCRegisterAddress     address[SMU72_DISCRETE_MC_REGISTER_ARRAY_SIZE];
+	SMU72_Discrete_MCRegisterSet         data[SMU72_DISCRETE_MC_REGISTER_ARRAY_SET_COUNT];
+};
+
+typedef struct SMU72_Discrete_MCRegisters SMU72_Discrete_MCRegisters;
+
+
+/* --------------------------------------------------- Fan Table ----------------------------------------------------------- */
+
+struct SMU72_Discrete_FanTable {
+	uint16_t FdoMode;
+	int16_t  TempMin;
+	int16_t  TempMed;
+	int16_t  TempMax;
+	int16_t  Slope1;
+	int16_t  Slope2;
+	int16_t  FdoMin;
+	int16_t  HystUp;
+	int16_t  HystDown;
+	int16_t  HystSlope;
+	int16_t  TempRespLim;
+	int16_t  TempCurr;
+	int16_t  SlopeCurr;
+	int16_t  PwmCurr;
+	uint32_t RefreshPeriod;
+	int16_t  FdoMax;
+	uint8_t  TempSrc;
+	int8_t   FanControl_GL_Flag;
+};
+
+typedef struct SMU72_Discrete_FanTable SMU72_Discrete_FanTable;
+
+#define SMU7_DISCRETE_GPIO_SCLK_DEBUG             4
+#define SMU7_DISCRETE_GPIO_SCLK_DEBUG_BIT         (0x1 << SMU7_DISCRETE_GPIO_SCLK_DEBUG)
+
+struct SMU7_MclkDpmScoreboard {
+
+	uint32_t PercentageBusy;
+
+	int32_t  PIDError;
+	int32_t  PIDIntegral;
+	int32_t  PIDOutput;
+
+	uint32_t SigmaDeltaAccum;
+	uint32_t SigmaDeltaOutput;
+	uint32_t SigmaDeltaLevel;
+
+	uint32_t UtilizationSetpoint;
+
+	uint8_t  TdpClampMode;
+	uint8_t  TdcClampMode;
+	uint8_t  ThermClampMode;
+	uint8_t  VoltageBusy;
+
+	int8_t   CurrLevel;
+	int8_t   TargLevel;
+	uint8_t  LevelChangeInProgress;
+	uint8_t  UpHyst;
+
+	uint8_t  DownHyst;
+	uint8_t  VoltageDownHyst;
+	uint8_t  DpmEnable;
+	uint8_t  DpmRunning;
+
+	uint8_t  DpmForce;
+	uint8_t  DpmForceLevel;
+	uint8_t  DisplayWatermark;
+	uint8_t  McArbIndex;
+
+	uint32_t MinimumPerfMclk;
+
+	uint8_t  AcpiReq;
+	uint8_t  AcpiAck;
+	uint8_t  MclkSwitchInProgress;
+	uint8_t  MclkSwitchCritical;
+
+	uint8_t  IgnoreVBlank;
+	uint8_t  TargetMclkIndex;
+	uint8_t  TargetMvddIndex;
+	uint8_t  MclkSwitchResult;
+
+	uint16_t VbiFailureCount;
+	uint8_t  VbiWaitCounter;
+	uint8_t  EnabledLevelsChange;
+
+	uint16_t LevelResidencyCountersN[SMU72_MAX_LEVELS_MEMORY];
+	uint16_t LevelSwitchCounters[SMU72_MAX_LEVELS_MEMORY];
+
+	void     (*TargetStateCalculator)(uint8_t);
+	void     (*SavedTargetStateCalculator)(uint8_t);
+
+	uint16_t AutoDpmInterval;
+	uint16_t AutoDpmRange;
+
+	uint16_t VbiTimeoutCount;
+	uint16_t MclkSwitchingTime;
+
+	uint8_t  fastSwitch;
+	uint8_t  Save_PIC_VDDGFX_EXIT;
+	uint8_t  Save_PIC_VDDGFX_ENTER;
+	uint8_t  padding;
+
+};
+
+typedef struct SMU7_MclkDpmScoreboard SMU7_MclkDpmScoreboard;
+
+struct SMU7_UlvScoreboard {
+	uint8_t     EnterUlv;
+	uint8_t     ExitUlv;
+	uint8_t     UlvActive;
+	uint8_t     WaitingForUlv;
+	uint8_t     UlvEnable;
+	uint8_t     UlvRunning;
+	uint8_t     UlvMasterEnable;
+	uint8_t     padding;
+	uint32_t    UlvAbortedCount;
+	uint32_t    UlvTimeStamp;
+};
+
+typedef struct SMU7_UlvScoreboard SMU7_UlvScoreboard;
+
+struct VddgfxSavedRegisters {
+	uint32_t GPU_DBG[3];
+	uint32_t MEC_BaseAddress_Hi;
+	uint32_t MEC_BaseAddress_Lo;
+	uint32_t THM_TMON0_CTRL2__RDIR_PRESENT;
+	uint32_t THM_TMON1_CTRL2__RDIR_PRESENT;
+	uint32_t CP_INT_CNTL;
+};
+
+typedef struct VddgfxSavedRegisters VddgfxSavedRegisters;
+
+struct SMU7_VddGfxScoreboard {
+	uint8_t     VddGfxEnable;
+	uint8_t     VddGfxActive;
+	uint8_t     VPUResetOccured;
+	uint8_t     padding;
+
+	uint32_t    VddGfxEnteredCount;
+	uint32_t    VddGfxAbortedCount;
+
+	uint32_t    VddGfxVid;
+
+	VddgfxSavedRegisters SavedRegisters;
+};
+
+typedef struct SMU7_VddGfxScoreboard SMU7_VddGfxScoreboard;
+
+struct SMU7_TdcLimitScoreboard {
+	uint8_t  Enable;
+	uint8_t  Running;
+	uint16_t Alpha;
+	uint32_t FilteredIddc;
+	uint32_t IddcLimit;
+	uint32_t IddcHyst;
+	SMU7_HystController_Data HystControllerData;
+};
+
+typedef struct SMU7_TdcLimitScoreboard SMU7_TdcLimitScoreboard;
+
+struct SMU7_PkgPwrLimitScoreboard {
+	uint8_t  Enable;
+	uint8_t  Running;
+	uint16_t Alpha;
+	uint32_t FilteredPkgPwr;
+	uint32_t Limit;
+	uint32_t Hyst;
+	uint32_t LimitFromDriver;
+	SMU7_HystController_Data HystControllerData;
+};
+
+typedef struct SMU7_PkgPwrLimitScoreboard SMU7_PkgPwrLimitScoreboard;
+
+struct SMU7_BapmScoreboard {
+	uint32_t source_powers[SMU72_DTE_SOURCES];
+	uint32_t source_powers_last[SMU72_DTE_SOURCES];
+	int32_t entity_temperatures[SMU72_NUM_GPU_TES];
+	int32_t initial_entity_temperatures[SMU72_NUM_GPU_TES];
+	int32_t Limit;
+	int32_t Hyst;
+	int32_t therm_influence_coeff_table[SMU72_DTE_ITERATIONS * SMU72_DTE_SOURCES * SMU72_DTE_SINKS * 2];
+	int32_t therm_node_table[SMU72_DTE_ITERATIONS * SMU72_DTE_SOURCES * SMU72_DTE_SINKS];
+	uint16_t ConfigTDPPowerScalar;
+	uint16_t FanSpeedPowerScalar;
+	uint16_t OverDrivePowerScalar;
+	uint16_t OverDriveLimitScalar;
+	uint16_t FinalPowerScalar;
+	uint8_t VariantID;
+	uint8_t spare997;
+
+	SMU7_HystController_Data HystControllerData;
+
+	int32_t temperature_gradient_slope;
+	int32_t temperature_gradient;
+	uint32_t measured_temperature;
+};
+
+
+typedef struct SMU7_BapmScoreboard SMU7_BapmScoreboard;
+
+struct SMU7_AcpiScoreboard {
+	uint32_t SavedInterruptMask[2];
+	uint8_t LastACPIRequest;
+	uint8_t CgBifResp;
+	uint8_t RequestType;
+	uint8_t Padding;
+	SMU72_Discrete_ACPILevel D0Level;
+};
+
+typedef struct SMU7_AcpiScoreboard SMU7_AcpiScoreboard;
+
+struct SMU72_Discrete_PmFuses {
+	/* dw1  */
+	uint8_t SviLoadLineEn;
+	uint8_t SviLoadLineVddC;
+	uint8_t SviLoadLineTrimVddC;
+	uint8_t SviLoadLineOffsetVddC;
+
+	/* dw2 */
+	uint16_t TDC_VDDC_PkgLimit;
+	uint8_t TDC_VDDC_ThrottleReleaseLimitPerc;
+	uint8_t TDC_MAWt;
+
+	/* dw3 */
+	uint8_t TdcWaterfallCtl;
+	uint8_t LPMLTemperatureMin;
+	uint8_t LPMLTemperatureMax;
+	uint8_t Reserved;
+
+	/* dw4-dw7  */
+	uint8_t LPMLTemperatureScaler[16];
+
+	/* dw8-dw9  */
+	int16_t FuzzyFan_ErrorSetDelta;
+	int16_t FuzzyFan_ErrorRateSetDelta;
+	int16_t FuzzyFan_PwmSetDelta;
+	uint16_t Reserved6;
+
+	/* dw10-dw14  */
+	uint8_t GnbLPML[16];
+
+	/* dw15 */
+	uint8_t GnbLPMLMaxVid;
+	uint8_t GnbLPMLMinVid;
+	uint8_t Reserved1[2];
+
+	/* dw16 */
+	uint16_t BapmVddCBaseLeakageHiSidd;
+	uint16_t BapmVddCBaseLeakageLoSidd;
+};
+
+typedef struct SMU72_Discrete_PmFuses SMU72_Discrete_PmFuses;
+
+struct SMU7_Discrete_Log_Header_Table {
+	uint32_t    version;
+	uint32_t    asic_id;
+	uint16_t    flags;
+	uint16_t    entry_size;
+	uint32_t    total_size;
+	uint32_t    num_of_entries;
+	uint8_t     type;
+	uint8_t     mode;
+	uint8_t     filler_0[2];
+	uint32_t    filler_1[2];
+};
+
+typedef struct SMU7_Discrete_Log_Header_Table SMU7_Discrete_Log_Header_Table;
+
+struct SMU7_Discrete_Log_Cntl {
+	uint8_t             Enabled;
+	uint8_t             Type;
+	uint8_t             padding[2];
+	uint32_t            BufferSize;
+	uint32_t            SamplesLogged;
+	uint32_t            SampleSize;
+	uint32_t            AddrL;
+	uint32_t            AddrH;
+};
+
+typedef struct SMU7_Discrete_Log_Cntl SMU7_Discrete_Log_Cntl;
+
+#define CAC_ACC_NW_NUM_OF_SIGNALS 87
+
+struct SMU7_Discrete_Cac_Collection_Table {
+	uint32_t temperature;
+	uint32_t cac_acc_nw[CAC_ACC_NW_NUM_OF_SIGNALS];
+};
+
+typedef struct SMU7_Discrete_Cac_Collection_Table SMU7_Discrete_Cac_Collection_Table;
+
+struct SMU7_Discrete_Cac_Verification_Table {
+	uint32_t VddcTotalPower;
+	uint32_t VddcLeakagePower;
+	uint32_t VddcConstantPower;
+	uint32_t VddcGfxDynamicPower;
+	uint32_t VddcUvdDynamicPower;
+	uint32_t VddcVceDynamicPower;
+	uint32_t VddcAcpDynamicPower;
+	uint32_t VddcPcieDynamicPower;
+	uint32_t VddcDceDynamicPower;
+	uint32_t VddcCurrent;
+	uint32_t VddcVoltage;
+	uint32_t VddciTotalPower;
+	uint32_t VddciLeakagePower;
+	uint32_t VddciConstantPower;
+	uint32_t VddciDynamicPower;
+	uint32_t Vddr1TotalPower;
+	uint32_t Vddr1LeakagePower;
+	uint32_t Vddr1ConstantPower;
+	uint32_t Vddr1DynamicPower;
+	uint32_t spare[4];
+	uint32_t temperature;
+};
+
+typedef struct SMU7_Discrete_Cac_Verification_Table SMU7_Discrete_Cac_Verification_Table;
+
+struct SMU7_Discrete_Pm_Status_Table {
+	/* Thermal entities */
+	int32_t T_meas_max;
+	int32_t T_meas_acc;
+	int32_t T_calc_max;
+	int32_t T_calc_acc;
+	uint32_t P_scalar_acc;
+	uint32_t P_calc_max;
+	uint32_t P_calc_acc;
+
+	/*Voltage domains */
+	uint32_t I_calc_max;
+	uint32_t I_calc_acc;
+	uint32_t I_calc_acc_vddci;
+	uint32_t V_calc_noload_acc;
+	uint32_t V_calc_load_acc;
+	uint32_t V_calc_noload_acc_vddci;
+	uint32_t P_meas_acc;
+	uint32_t V_meas_noload_acc;
+	uint32_t V_meas_load_acc;
+	uint32_t I_meas_acc;
+	uint32_t P_meas_acc_vddci;
+	uint32_t V_meas_noload_acc_vddci;
+	uint32_t V_meas_load_acc_vddci;
+	uint32_t I_meas_acc_vddci;
+
+	/*Frequency */
+	uint16_t Sclk_dpm_residency[8];
+	uint16_t Uvd_dpm_residency[8];
+	uint16_t Vce_dpm_residency[8];
+	uint16_t Mclk_dpm_residency[4];
+
+	/*Chip */
+	uint32_t P_vddci_acc;
+	uint32_t P_vddr1_acc;
+	uint32_t P_nte1_acc;
+	uint32_t PkgPwr_max;
+	uint32_t PkgPwr_acc;
+	uint32_t MclkSwitchingTime_max;
+	uint32_t MclkSwitchingTime_acc;
+	uint32_t FanPwm_acc;
+	uint32_t FanRpm_acc;
+
+	uint32_t AccCnt;
+};
+
+typedef struct SMU7_Discrete_Pm_Status_Table SMU7_Discrete_Pm_Status_Table;
+
+/*FIXME THESE NEED TO BE UPDATED */
+#define SMU7_SCLK_CAC 0x561
+#define SMU7_MCLK_CAC 0xF9
+#define SMU7_VCLK_CAC 0x2DE
+#define SMU7_DCLK_CAC 0x2DE
+#define SMU7_ECLK_CAC 0x25E
+#define SMU7_ACLK_CAC 0x25E
+#define SMU7_SAMCLK_CAC 0x25E
+#define SMU7_DISPCLK_CAC 0x100
+#define SMU7_CAC_CONSTANT 0x2EE3430
+#define SMU7_CAC_CONSTANT_SHIFT 18
+
+#define SMU7_VDDCI_MCLK_CONST        1765
+#define SMU7_VDDCI_MCLK_CONST_SHIFT  16
+#define SMU7_VDDCI_VDDCI_CONST       50958
+#define SMU7_VDDCI_VDDCI_CONST_SHIFT 14
+#define SMU7_VDDCI_CONST             11781
+
+#define SMU7_12C_VDDCI_MCLK_CONST        1623
+#define SMU7_12C_VDDCI_MCLK_CONST_SHIFT  15
+#define SMU7_12C_VDDCI_VDDCI_CONST       40088
+#define SMU7_12C_VDDCI_VDDCI_CONST_SHIFT 13
+#define SMU7_12C_VDDCI_CONST             20856
+
+#define SMU7_VDDCI_STROBE_PWR        1331
+
+#define SMU7_VDDR1_CONST            693
+#define SMU7_VDDR1_CAC_WEIGHT       20
+#define SMU7_VDDR1_CAC_WEIGHT_SHIFT 19
+#define SMU7_VDDR1_STROBE_PWR       512
+
+#define SMU7_AREA_COEFF_UVD 0xA78
+#define SMU7_AREA_COEFF_VCE 0x190A
+#define SMU7_AREA_COEFF_ACP 0x22D1
+#define SMU7_AREA_COEFF_SAMU 0x534
+
+/*ThermOutMode values */
+#define SMU7_THERM_OUT_MODE_DISABLE       0x0
+#define SMU7_THERM_OUT_MODE_THERM_ONLY    0x1
+#define SMU7_THERM_OUT_MODE_THERM_VRHOT   0x2
+
+#if !defined(SMC_MICROCODE)
+#pragma pack(pop)
+#endif
+
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smu73.h b/drivers/gpu/drm/amd/powerplay/inc/smu73.h
new file mode 100644
index 000000000000..c6b12a4c00db
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu73.h
@@ -0,0 +1,720 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _SMU73_H_
+#define _SMU73_H_
+
+#pragma pack(push, 1)
+enum SID_OPTION {
+  SID_OPTION_HI,
+  SID_OPTION_LO,
+  SID_OPTION_COUNT
+};
+
+enum Poly3rdOrderCoeff {
+    LEAKAGE_TEMPERATURE_SCALAR,
+    LEAKAGE_VOLTAGE_SCALAR,
+    DYNAMIC_VOLTAGE_SCALAR,
+    POLY_3RD_ORDER_COUNT
+};
+
+struct SMU7_Poly3rdOrder_Data
+{
+    int32_t a;
+    int32_t b;
+    int32_t c;
+    int32_t d;
+    uint8_t a_shift;
+    uint8_t b_shift;
+    uint8_t c_shift;
+    uint8_t x_shift;
+};
+
+typedef struct SMU7_Poly3rdOrder_Data SMU7_Poly3rdOrder_Data;
+
+struct Power_Calculator_Data
+{
+  uint16_t NoLoadVoltage;
+  uint16_t LoadVoltage;
+  uint16_t Resistance;
+  uint16_t Temperature;
+  uint16_t BaseLeakage;
+  uint16_t LkgTempScalar;
+  uint16_t LkgVoltScalar;
+  uint16_t LkgAreaScalar;
+  uint16_t LkgPower;
+  uint16_t DynVoltScalar;
+  uint32_t Cac;
+  uint32_t DynPower;
+  uint32_t TotalCurrent;
+  uint32_t TotalPower;
+};
+
+typedef struct Power_Calculator_Data PowerCalculatorData_t;
+
+struct Gc_Cac_Weight_Data
+{
+  uint8_t index;
+  uint32_t value;
+};
+
+typedef struct Gc_Cac_Weight_Data GcCacWeight_Data;
+
+
+typedef struct {
+  uint32_t high;
+  uint32_t low;
+} data_64_t;
+
+typedef struct {
+  data_64_t high;
+  data_64_t low;
+} data_128_t;
+
+#define SMU__NUM_SCLK_DPM_STATE  8
+#define SMU__NUM_MCLK_DPM_LEVELS 4
+#define SMU__NUM_LCLK_DPM_LEVELS 8
+#define SMU__NUM_PCIE_DPM_LEVELS 8
+
+#define SMU7_CONTEXT_ID_SMC        1
+#define SMU7_CONTEXT_ID_VBIOS      2
+
+#define SMU73_MAX_LEVELS_VDDC            16
+#define SMU73_MAX_LEVELS_VDDGFX          16
+#define SMU73_MAX_LEVELS_VDDCI           8
+#define SMU73_MAX_LEVELS_MVDD            4
+
+#define SMU_MAX_SMIO_LEVELS              4
+
+#define SMU73_MAX_LEVELS_GRAPHICS        SMU__NUM_SCLK_DPM_STATE   // SCLK + SQ DPM + ULV
+#define SMU73_MAX_LEVELS_MEMORY          SMU__NUM_MCLK_DPM_LEVELS   // MCLK Levels DPM
+#define SMU73_MAX_LEVELS_GIO             SMU__NUM_LCLK_DPM_LEVELS  // LCLK Levels
+#define SMU73_MAX_LEVELS_LINK            SMU__NUM_PCIE_DPM_LEVELS  // PCIe speed and number of lanes.
+#define SMU73_MAX_LEVELS_UVD             8   // VCLK/DCLK levels for UVD.
+#define SMU73_MAX_LEVELS_VCE             8   // ECLK levels for VCE.
+#define SMU73_MAX_LEVELS_ACP             8   // ACLK levels for ACP.
+#define SMU73_MAX_LEVELS_SAMU            8   // SAMCLK levels for SAMU.
+#define SMU73_MAX_ENTRIES_SMIO           32  // Number of entries in SMIO table.
+
+#define DPM_NO_LIMIT 0
+#define DPM_NO_UP 1
+#define DPM_GO_DOWN 2
+#define DPM_GO_UP 3
+
+#define SMU7_FIRST_DPM_GRAPHICS_LEVEL    0
+#define SMU7_FIRST_DPM_MEMORY_LEVEL      0
+
+#define GPIO_CLAMP_MODE_VRHOT      1
+#define GPIO_CLAMP_MODE_THERM      2
+#define GPIO_CLAMP_MODE_DC         4
+
+#define SCRATCH_B_TARG_PCIE_INDEX_SHIFT 0
+#define SCRATCH_B_TARG_PCIE_INDEX_MASK  (0x7<<SCRATCH_B_TARG_PCIE_INDEX_SHIFT)
+#define SCRATCH_B_CURR_PCIE_INDEX_SHIFT 3
+#define SCRATCH_B_CURR_PCIE_INDEX_MASK  (0x7<<SCRATCH_B_CURR_PCIE_INDEX_SHIFT)
+#define SCRATCH_B_TARG_UVD_INDEX_SHIFT  6
+#define SCRATCH_B_TARG_UVD_INDEX_MASK   (0x7<<SCRATCH_B_TARG_UVD_INDEX_SHIFT)
+#define SCRATCH_B_CURR_UVD_INDEX_SHIFT  9
+#define SCRATCH_B_CURR_UVD_INDEX_MASK   (0x7<<SCRATCH_B_CURR_UVD_INDEX_SHIFT)
+#define SCRATCH_B_TARG_VCE_INDEX_SHIFT  12
+#define SCRATCH_B_TARG_VCE_INDEX_MASK   (0x7<<SCRATCH_B_TARG_VCE_INDEX_SHIFT)
+#define SCRATCH_B_CURR_VCE_INDEX_SHIFT  15
+#define SCRATCH_B_CURR_VCE_INDEX_MASK   (0x7<<SCRATCH_B_CURR_VCE_INDEX_SHIFT)
+#define SCRATCH_B_TARG_ACP_INDEX_SHIFT  18
+#define SCRATCH_B_TARG_ACP_INDEX_MASK   (0x7<<SCRATCH_B_TARG_ACP_INDEX_SHIFT)
+#define SCRATCH_B_CURR_ACP_INDEX_SHIFT  21
+#define SCRATCH_B_CURR_ACP_INDEX_MASK   (0x7<<SCRATCH_B_CURR_ACP_INDEX_SHIFT)
+#define SCRATCH_B_TARG_SAMU_INDEX_SHIFT 24
+#define SCRATCH_B_TARG_SAMU_INDEX_MASK  (0x7<<SCRATCH_B_TARG_SAMU_INDEX_SHIFT)
+#define SCRATCH_B_CURR_SAMU_INDEX_SHIFT 27
+#define SCRATCH_B_CURR_SAMU_INDEX_MASK  (0x7<<SCRATCH_B_CURR_SAMU_INDEX_SHIFT)
+
+// Virtualization Defines
+#define CG_XDMA_MASK  0x1
+#define CG_XDMA_SHIFT 0
+#define CG_UVD_MASK   0x2
+#define CG_UVD_SHIFT  1
+#define CG_VCE_MASK   0x4
+#define CG_VCE_SHIFT  2
+#define CG_SAMU_MASK  0x8
+#define CG_SAMU_SHIFT 3
+#define CG_GFX_MASK   0x10
+#define CG_GFX_SHIFT  4
+#define CG_SDMA_MASK  0x20
+#define CG_SDMA_SHIFT 5
+#define CG_HDP_MASK   0x40
+#define CG_HDP_SHIFT  6
+#define CG_MC_MASK    0x80
+#define CG_MC_SHIFT   7
+#define CG_DRM_MASK   0x100
+#define CG_DRM_SHIFT  8
+#define CG_ROM_MASK   0x200
+#define CG_ROM_SHIFT  9
+#define CG_BIF_MASK   0x400
+#define CG_BIF_SHIFT  10
+
+#define SMU73_DTE_ITERATIONS 5
+#define SMU73_DTE_SOURCES 3
+#define SMU73_DTE_SINKS 1
+#define SMU73_NUM_CPU_TES 0
+#define SMU73_NUM_GPU_TES 1
+#define SMU73_NUM_NON_TES 2
+#define SMU73_DTE_FAN_SCALAR_MIN 0x100
+#define SMU73_DTE_FAN_SCALAR_MAX 0x166
+#define SMU73_DTE_FAN_TEMP_MAX 93
+#define SMU73_DTE_FAN_TEMP_MIN 83
+
+#define SMU73_THERMAL_INPUT_LOOP_COUNT 6
+#define SMU73_THERMAL_CLAMP_MODE_COUNT 8
+
+
+struct SMU7_HystController_Data
+{
+    uint16_t waterfall_up;
+    uint16_t waterfall_down;
+    uint16_t waterfall_limit;
+    uint16_t release_cnt;
+    uint16_t release_limit;
+    uint16_t spare;
+};
+
+typedef struct SMU7_HystController_Data SMU7_HystController_Data;
+
+struct SMU73_PIDController
+{
+    uint32_t Ki;
+    int32_t LFWindupUpperLim;
+    int32_t LFWindupLowerLim;
+    uint32_t StatePrecision;
+
+    uint32_t LfPrecision;
+    uint32_t LfOffset;
+    uint32_t MaxState;
+    uint32_t MaxLfFraction;
+    uint32_t StateShift;
+};
+
+typedef struct SMU73_PIDController SMU73_PIDController;
+
+struct SMU7_LocalDpmScoreboard
+{
+    uint32_t PercentageBusy;
+
+    int32_t  PIDError;
+    int32_t  PIDIntegral;
+    int32_t  PIDOutput;
+
+    uint32_t SigmaDeltaAccum;
+    uint32_t SigmaDeltaOutput;
+    uint32_t SigmaDeltaLevel;
+
+    uint32_t UtilizationSetpoint;
+
+    uint8_t  TdpClampMode;
+    uint8_t  TdcClampMode;
+    uint8_t  ThermClampMode;
+    uint8_t  VoltageBusy;
+
+    int8_t   CurrLevel;
+    int8_t   TargLevel;
+    uint8_t  LevelChangeInProgress;
+    uint8_t  UpHyst;
+
+    uint8_t  DownHyst;
+    uint8_t  VoltageDownHyst;
+    uint8_t  DpmEnable;
+    uint8_t  DpmRunning;
+
+    uint8_t  DpmForce;
+    uint8_t  DpmForceLevel;
+    uint8_t  DisplayWatermark;
+    uint8_t  McArbIndex;
+
+    uint32_t MinimumPerfSclk;
+
+    uint8_t  AcpiReq;
+    uint8_t  AcpiAck;
+    uint8_t  GfxClkSlow;
+    uint8_t  GpioClampMode;
+
+    uint8_t  spare2;
+    uint8_t  EnabledLevelsChange;
+    uint8_t  DteClampMode;
+    uint8_t  FpsClampMode;
+
+    uint16_t LevelResidencyCounters [SMU73_MAX_LEVELS_GRAPHICS];
+    uint16_t LevelSwitchCounters [SMU73_MAX_LEVELS_GRAPHICS];
+
+    void     (*TargetStateCalculator)(uint8_t);
+    void     (*SavedTargetStateCalculator)(uint8_t);
+
+    uint16_t AutoDpmInterval;
+    uint16_t AutoDpmRange;
+
+    uint8_t  FpsEnabled;
+    uint8_t  MaxPerfLevel;
+    uint8_t  AllowLowClkInterruptToHost;
+    uint8_t  FpsRunning;
+
+    uint32_t MaxAllowedFrequency;
+
+    uint32_t FilteredSclkFrequency;
+    uint32_t LastSclkFrequency;
+    uint32_t FilteredSclkFrequencyCnt;
+
+    uint8_t  LedEnable;
+    uint8_t  LedPin0;
+    uint8_t  LedPin1;
+    uint8_t  LedPin2;
+    uint32_t LedAndMask;
+
+    uint16_t FpsAlpha;
+    uint16_t DeltaTime;
+    uint32_t CurrentFps;
+    uint32_t FilteredFps;
+    uint32_t FrameCount;
+    uint32_t FrameCountLast;
+    uint16_t FpsTargetScalar;
+    uint16_t FpsWaterfallLimitScalar;
+    uint16_t FpsAlphaScalar;
+    uint16_t spare8;
+    SMU7_HystController_Data HystControllerData;
+};
+
+typedef struct SMU7_LocalDpmScoreboard SMU7_LocalDpmScoreboard;
+
+#define SMU7_MAX_VOLTAGE_CLIENTS 12
+
+typedef uint8_t (*VoltageChangeHandler_t)(uint16_t, uint8_t);
+
+#define VDDC_MASK    0x00007FFF
+#define VDDC_SHIFT   0
+#define VDDCI_MASK   0x3FFF8000
+#define VDDCI_SHIFT  15
+#define PHASES_MASK  0xC0000000
+#define PHASES_SHIFT 30
+
+typedef uint32_t SMU_VoltageLevel;
+
+struct SMU7_VoltageScoreboard
+{
+    SMU_VoltageLevel TargetVoltage;
+    uint16_t MaxVid;
+    uint8_t  HighestVidOffset;
+    uint8_t  CurrentVidOffset;
+
+    uint16_t CurrentVddc;
+    uint16_t CurrentVddci;
+
+
+    uint8_t  ControllerBusy;
+    uint8_t  CurrentVid;
+    uint8_t  CurrentVddciVid;
+    uint8_t  padding;
+
+    SMU_VoltageLevel RequestedVoltage[SMU7_MAX_VOLTAGE_CLIENTS];
+    SMU_VoltageLevel TargetVoltageState;
+    uint8_t  EnabledRequest[SMU7_MAX_VOLTAGE_CLIENTS];
+
+    uint8_t  padding2;
+    uint8_t  padding3;
+    uint8_t  ControllerEnable;
+    uint8_t  ControllerRunning;
+    uint16_t CurrentStdVoltageHiSidd;
+    uint16_t CurrentStdVoltageLoSidd;
+    uint8_t  OverrideVoltage;
+    uint8_t  padding4;
+    uint8_t  padding5;
+    uint8_t  CurrentPhases;
+
+    VoltageChangeHandler_t ChangeVddc;
+
+    VoltageChangeHandler_t ChangeVddci;
+    VoltageChangeHandler_t ChangePhase;
+    VoltageChangeHandler_t ChangeMvdd;
+
+    VoltageChangeHandler_t functionLinks[6];
+
+    uint16_t * VddcFollower1;
+
+    int16_t  Driver_OD_RequestedVidOffset1;
+    int16_t  Driver_OD_RequestedVidOffset2;
+
+};
+
+typedef struct SMU7_VoltageScoreboard SMU7_VoltageScoreboard;
+
+// -------------------------------------------------------------------------------------------------------------------------
+#define SMU7_MAX_PCIE_LINK_SPEEDS 3 /* 0:Gen1 1:Gen2 2:Gen3 */
+
+struct SMU7_PCIeLinkSpeedScoreboard
+{
+    uint8_t     DpmEnable;
+    uint8_t     DpmRunning;
+    uint8_t     DpmForce;
+    uint8_t     DpmForceLevel;
+
+    uint8_t     CurrentLinkSpeed;
+    uint8_t     EnabledLevelsChange;
+    uint16_t    AutoDpmInterval;
+
+    uint16_t    AutoDpmRange;
+    uint16_t    AutoDpmCount;
+
+    uint8_t     DpmMode;
+    uint8_t     AcpiReq;
+    uint8_t     AcpiAck;
+    uint8_t     CurrentLinkLevel;
+
+};
+
+typedef struct SMU7_PCIeLinkSpeedScoreboard SMU7_PCIeLinkSpeedScoreboard;
+
+// -------------------------------------------------------- CAC table ------------------------------------------------------
+#define SMU7_LKGE_LUT_NUM_OF_TEMP_ENTRIES 16
+#define SMU7_LKGE_LUT_NUM_OF_VOLT_ENTRIES 16
+
+#define SMU7_SCALE_I  7
+#define SMU7_SCALE_R 12
+
+struct SMU7_PowerScoreboard
+{
+    uint32_t GpuPower;
+
+    uint32_t VddcPower;
+    uint32_t VddcVoltage;
+    uint32_t VddcCurrent;
+
+    uint32_t MvddPower;
+    uint32_t MvddVoltage;
+    uint32_t MvddCurrent;
+
+    uint32_t RocPower;
+
+    uint16_t Telemetry_1_slope;
+    uint16_t Telemetry_2_slope;
+    int32_t  Telemetry_1_offset;
+    int32_t  Telemetry_2_offset;
+};
+typedef struct SMU7_PowerScoreboard SMU7_PowerScoreboard;
+
+// For FeatureEnables:
+#define SMU7_SCLK_DPM_CONFIG_MASK                        0x01
+#define SMU7_VOLTAGE_CONTROLLER_CONFIG_MASK              0x02
+#define SMU7_THERMAL_CONTROLLER_CONFIG_MASK              0x04
+#define SMU7_MCLK_DPM_CONFIG_MASK                        0x08
+#define SMU7_UVD_DPM_CONFIG_MASK                         0x10
+#define SMU7_VCE_DPM_CONFIG_MASK                         0x20
+#define SMU7_ACP_DPM_CONFIG_MASK                         0x40
+#define SMU7_SAMU_DPM_CONFIG_MASK                        0x80
+#define SMU7_PCIEGEN_DPM_CONFIG_MASK                    0x100
+
+#define SMU7_ACP_MCLK_HANDSHAKE_DISABLE                  0x00000001
+#define SMU7_ACP_SCLK_HANDSHAKE_DISABLE                  0x00000002
+#define SMU7_UVD_MCLK_HANDSHAKE_DISABLE                  0x00000100
+#define SMU7_UVD_SCLK_HANDSHAKE_DISABLE                  0x00000200
+#define SMU7_VCE_MCLK_HANDSHAKE_DISABLE                  0x00010000
+#define SMU7_VCE_SCLK_HANDSHAKE_DISABLE                  0x00020000
+
+// All 'soft registers' should be uint32_t.
+struct SMU73_SoftRegisters
+{
+    uint32_t        RefClockFrequency;
+    uint32_t        PmTimerPeriod;
+    uint32_t        FeatureEnables;
+
+    uint32_t        PreVBlankGap;
+    uint32_t        VBlankTimeout;
+    uint32_t        TrainTimeGap;
+
+    uint32_t        MvddSwitchTime;
+    uint32_t        LongestAcpiTrainTime;
+    uint32_t        AcpiDelay;
+    uint32_t        G5TrainTime;
+    uint32_t        DelayMpllPwron;
+    uint32_t        VoltageChangeTimeout;
+
+    uint32_t        HandshakeDisables;
+
+    uint8_t         DisplayPhy1Config;
+    uint8_t         DisplayPhy2Config;
+    uint8_t         DisplayPhy3Config;
+    uint8_t         DisplayPhy4Config;
+
+    uint8_t         DisplayPhy5Config;
+    uint8_t         DisplayPhy6Config;
+    uint8_t         DisplayPhy7Config;
+    uint8_t         DisplayPhy8Config;
+
+    uint32_t        AverageGraphicsActivity;
+    uint32_t        AverageMemoryActivity;
+    uint32_t        AverageGioActivity;
+
+    uint8_t         SClkDpmEnabledLevels;
+    uint8_t         MClkDpmEnabledLevels;
+    uint8_t         LClkDpmEnabledLevels;
+    uint8_t         PCIeDpmEnabledLevels;
+
+    uint8_t         UVDDpmEnabledLevels;
+    uint8_t         SAMUDpmEnabledLevels;
+    uint8_t         ACPDpmEnabledLevels;
+    uint8_t         VCEDpmEnabledLevels;
+
+    uint32_t        DRAM_LOG_ADDR_H;
+    uint32_t        DRAM_LOG_ADDR_L;
+    uint32_t        DRAM_LOG_PHY_ADDR_H;
+    uint32_t        DRAM_LOG_PHY_ADDR_L;
+    uint32_t        DRAM_LOG_BUFF_SIZE;
+    uint32_t        UlvEnterCount;
+    uint32_t        UlvTime;
+    uint32_t        UcodeLoadStatus;
+    uint32_t        Reserved[2];
+
+};
+
+typedef struct SMU73_SoftRegisters SMU73_SoftRegisters;
+
+struct SMU73_Firmware_Header
+{
+    uint32_t Digest[5];
+    uint32_t Version;
+    uint32_t HeaderSize;
+    uint32_t Flags;
+    uint32_t EntryPoint;
+    uint32_t CodeSize;
+    uint32_t ImageSize;
+
+    uint32_t Rtos;
+    uint32_t SoftRegisters;
+    uint32_t DpmTable;
+    uint32_t FanTable;
+    uint32_t CacConfigTable;
+    uint32_t CacStatusTable;
+
+
+    uint32_t mcRegisterTable;
+
+
+    uint32_t mcArbDramTimingTable;
+
+
+
+
+    uint32_t PmFuseTable;
+    uint32_t Globals;
+    uint32_t ClockStretcherTable;
+    uint32_t Reserved[41];
+    uint32_t Signature;
+};
+
+typedef struct SMU73_Firmware_Header SMU73_Firmware_Header;
+
+#define SMU7_FIRMWARE_HEADER_LOCATION 0x20000
+
+enum  DisplayConfig {
+    PowerDown = 1,
+    DP54x4,
+    DP54x2,
+    DP54x1,
+    DP27x4,
+    DP27x2,
+    DP27x1,
+    HDMI297,
+    HDMI162,
+    LVDS,
+    DP324x4,
+    DP324x2,
+    DP324x1
+};
+
+
+#define MC_BLOCK_COUNT 1
+#define CPL_BLOCK_COUNT 5
+#define SE_BLOCK_COUNT 15
+#define GC_BLOCK_COUNT 24
+
+struct SMU7_Local_Cac {
+  uint8_t BlockId;
+  uint8_t SignalId;
+  uint8_t Threshold;
+  uint8_t Padding;
+};
+
+typedef struct SMU7_Local_Cac SMU7_Local_Cac;
+
+struct SMU7_Local_Cac_Table {
+
+  SMU7_Local_Cac CplLocalCac[CPL_BLOCK_COUNT];
+  SMU7_Local_Cac McLocalCac[MC_BLOCK_COUNT];
+  SMU7_Local_Cac SeLocalCac[SE_BLOCK_COUNT];
+  SMU7_Local_Cac GcLocalCac[GC_BLOCK_COUNT];
+};
+
+typedef struct SMU7_Local_Cac_Table SMU7_Local_Cac_Table;
+
+#if !defined(SMC_MICROCODE)
+#pragma pack(pop)
+#endif
+
+// Description of Clock Gating bitmask for Tonga:
+// System Clock Gating
+#define CG_SYS_BITMASK_FIRST_BIT      0  // First bit of Sys CG bitmask
+#define CG_SYS_BITMASK_LAST_BIT       9  // Last bit of Sys CG bitmask
+#define CG_SYS_BIF_MGLS_SHIFT         0
+#define CG_SYS_ROM_SHIFT              1
+#define CG_SYS_MC_MGCG_SHIFT          2
+#define CG_SYS_MC_MGLS_SHIFT          3
+#define CG_SYS_SDMA_MGCG_SHIFT        4
+#define CG_SYS_SDMA_MGLS_SHIFT        5
+#define CG_SYS_DRM_MGCG_SHIFT         6
+#define CG_SYS_HDP_MGCG_SHIFT         7
+#define CG_SYS_HDP_MGLS_SHIFT         8
+#define CG_SYS_DRM_MGLS_SHIFT         9
+
+#define CG_SYS_BIF_MGLS_MASK          0x1
+#define CG_SYS_ROM_MASK               0x2
+#define CG_SYS_MC_MGCG_MASK           0x4
+#define CG_SYS_MC_MGLS_MASK           0x8
+#define CG_SYS_SDMA_MGCG_MASK         0x10
+#define CG_SYS_SDMA_MGLS_MASK         0x20
+#define CG_SYS_DRM_MGCG_MASK          0x40
+#define CG_SYS_HDP_MGCG_MASK          0x80
+#define CG_SYS_HDP_MGLS_MASK          0x100
+#define CG_SYS_DRM_MGLS_MASK          0x200
+
+// Graphics Clock Gating
+#define CG_GFX_BITMASK_FIRST_BIT      16 // First bit of Gfx CG bitmask
+#define CG_GFX_BITMASK_LAST_BIT       20 // Last bit of Gfx CG bitmask
+#define CG_GFX_CGCG_SHIFT             16
+#define CG_GFX_CGLS_SHIFT             17
+#define CG_CPF_MGCG_SHIFT             18
+#define CG_RLC_MGCG_SHIFT             19
+#define CG_GFX_OTHERS_MGCG_SHIFT      20
+
+#define CG_GFX_CGCG_MASK              0x00010000
+#define CG_GFX_CGLS_MASK              0x00020000
+#define CG_CPF_MGCG_MASK              0x00040000
+#define CG_RLC_MGCG_MASK              0x00080000
+#define CG_GFX_OTHERS_MGCG_MASK       0x00100000
+
+
+
+// Voltage Regulator Configuration
+// VR Config info is contained in dpmTable.VRConfig
+
+#define VRCONF_VDDC_MASK         0x000000FF
+#define VRCONF_VDDC_SHIFT        0
+#define VRCONF_VDDGFX_MASK       0x0000FF00
+#define VRCONF_VDDGFX_SHIFT      8
+#define VRCONF_VDDCI_MASK        0x00FF0000
+#define VRCONF_VDDCI_SHIFT       16
+#define VRCONF_MVDD_MASK         0xFF000000
+#define VRCONF_MVDD_SHIFT        24
+
+#define VR_MERGED_WITH_VDDC      0
+#define VR_SVI2_PLANE_1          1
+#define VR_SVI2_PLANE_2          2
+#define VR_SMIO_PATTERN_1        3
+#define VR_SMIO_PATTERN_2        4
+#define VR_STATIC_VOLTAGE        5
+
+// Clock Stretcher Configuration
+
+#define CLOCK_STRETCHER_MAX_ENTRIES 0x4
+#define CKS_LOOKUPTable_MAX_ENTRIES 0x4
+
+// The 'settings' field is subdivided in the following way:
+#define CLOCK_STRETCHER_SETTING_DDT_MASK             0x01
+#define CLOCK_STRETCHER_SETTING_DDT_SHIFT            0x0
+#define CLOCK_STRETCHER_SETTING_STRETCH_AMOUNT_MASK  0x1E
+#define CLOCK_STRETCHER_SETTING_STRETCH_AMOUNT_SHIFT 0x1
+#define CLOCK_STRETCHER_SETTING_ENABLE_MASK          0x80
+#define CLOCK_STRETCHER_SETTING_ENABLE_SHIFT         0x7
+
+struct SMU_ClockStretcherDataTableEntry {
+  uint8_t minVID;
+  uint8_t maxVID;
+
+
+  uint16_t setting;
+};
+typedef struct SMU_ClockStretcherDataTableEntry SMU_ClockStretcherDataTableEntry;
+
+struct SMU_ClockStretcherDataTable {
+  SMU_ClockStretcherDataTableEntry ClockStretcherDataTableEntry[CLOCK_STRETCHER_MAX_ENTRIES];
+};
+typedef struct SMU_ClockStretcherDataTable SMU_ClockStretcherDataTable;
+
+struct SMU_CKS_LOOKUPTableEntry {
+  uint16_t minFreq;
+  uint16_t maxFreq;
+
+  uint8_t setting;
+  uint8_t padding[3];
+};
+typedef struct SMU_CKS_LOOKUPTableEntry SMU_CKS_LOOKUPTableEntry;
+
+struct SMU_CKS_LOOKUPTable {
+  SMU_CKS_LOOKUPTableEntry CKS_LOOKUPTableEntry[CKS_LOOKUPTable_MAX_ENTRIES];
+};
+typedef struct SMU_CKS_LOOKUPTable SMU_CKS_LOOKUPTable;
+
+struct AgmAvfsData_t {
+  uint16_t avgPsmCount[28];
+  uint16_t minPsmCount[28];
+};
+typedef struct AgmAvfsData_t AgmAvfsData_t;
+
+// AVFS DEFINES
+
+enum VFT_COLUMNS {
+  SCLK0,
+  SCLK1,
+  SCLK2,
+  SCLK3,
+  SCLK4,
+  SCLK5,
+  SCLK6,
+  SCLK7,
+
+  NUM_VFT_COLUMNS
+};
+
+#define TEMP_RANGE_MAXSTEPS 12
+struct VFT_CELL_t {
+  uint16_t Voltage;
+};
+
+typedef struct VFT_CELL_t VFT_CELL_t;
+
+struct VFT_TABLE_t {
+  VFT_CELL_t    Cell[TEMP_RANGE_MAXSTEPS][NUM_VFT_COLUMNS];
+  uint16_t      AvfsGbv [NUM_VFT_COLUMNS];
+  uint16_t      BtcGbv  [NUM_VFT_COLUMNS];
+  uint16_t      Temperature [TEMP_RANGE_MAXSTEPS];
+
+  uint8_t       NumTemperatureSteps;
+  uint8_t       padding[3];
+};
+typedef struct VFT_TABLE_t VFT_TABLE_t;
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smu73_discrete.h b/drivers/gpu/drm/amd/powerplay/inc/smu73_discrete.h
new file mode 100644
index 000000000000..5916be08a7fe
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu73_discrete.h
@@ -0,0 +1,799 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _SMU73_DISCRETE_H_
+#define _SMU73_DISCRETE_H_
+
+#include "smu73.h"
+
+#pragma pack(push, 1)
+
+struct SMIO_Pattern
+{
+  uint16_t Voltage;
+  uint8_t  Smio;
+  uint8_t  padding;
+};
+
+typedef struct SMIO_Pattern SMIO_Pattern;
+
+struct SMIO_Table
+{
+  SMIO_Pattern Pattern[SMU_MAX_SMIO_LEVELS];
+};
+
+typedef struct SMIO_Table SMIO_Table;
+
+struct SMU73_Discrete_GraphicsLevel {
+	uint32_t    MinVoltage;
+
+	uint32_t    SclkFrequency;
+
+	uint8_t     pcieDpmLevel;
+	uint8_t     DeepSleepDivId;
+	uint16_t    ActivityLevel;
+	uint32_t    CgSpllFuncCntl3;
+	uint32_t    CgSpllFuncCntl4;
+	uint32_t    SpllSpreadSpectrum;
+	uint32_t    SpllSpreadSpectrum2;
+	uint32_t    CcPwrDynRm;
+	uint32_t    CcPwrDynRm1;
+	uint8_t     SclkDid;
+	uint8_t     DisplayWatermark;
+	uint8_t     EnabledForActivity;
+	uint8_t     EnabledForThrottle;
+	uint8_t     UpHyst;
+	uint8_t     DownHyst;
+	uint8_t     VoltageDownHyst;
+	uint8_t     PowerThrottle;
+};
+
+typedef struct SMU73_Discrete_GraphicsLevel SMU73_Discrete_GraphicsLevel;
+
+struct SMU73_Discrete_ACPILevel {
+    uint32_t    Flags;
+    uint32_t MinVoltage;
+    uint32_t    SclkFrequency;
+    uint8_t     SclkDid;
+    uint8_t     DisplayWatermark;
+    uint8_t     DeepSleepDivId;
+    uint8_t     padding;
+    uint32_t    CgSpllFuncCntl;
+    uint32_t    CgSpllFuncCntl2;
+    uint32_t    CgSpllFuncCntl3;
+    uint32_t    CgSpllFuncCntl4;
+    uint32_t    SpllSpreadSpectrum;
+    uint32_t    SpllSpreadSpectrum2;
+    uint32_t    CcPwrDynRm;
+    uint32_t    CcPwrDynRm1;
+};
+
+typedef struct SMU73_Discrete_ACPILevel SMU73_Discrete_ACPILevel;
+
+struct SMU73_Discrete_Ulv {
+	uint32_t    CcPwrDynRm;
+	uint32_t    CcPwrDynRm1;
+	uint16_t    VddcOffset;
+	uint8_t     VddcOffsetVid;
+	uint8_t     VddcPhase;
+	uint32_t    Reserved;
+};
+
+typedef struct SMU73_Discrete_Ulv SMU73_Discrete_Ulv;
+
+struct SMU73_Discrete_MemoryLevel
+{
+    uint32_t MinVoltage;
+    uint32_t    MinMvdd;
+
+    uint32_t    MclkFrequency;
+
+    uint8_t     StutterEnable;
+    uint8_t     FreqRange;
+    uint8_t     EnabledForThrottle;
+    uint8_t     EnabledForActivity;
+
+    uint8_t     UpHyst;
+    uint8_t     DownHyst;
+    uint8_t     VoltageDownHyst;
+    uint8_t     padding;
+
+    uint16_t    ActivityLevel;
+    uint8_t     DisplayWatermark;
+    uint8_t     MclkDivider;
+};
+
+typedef struct SMU73_Discrete_MemoryLevel SMU73_Discrete_MemoryLevel;
+
+struct SMU73_Discrete_LinkLevel
+{
+    uint8_t     PcieGenSpeed;           ///< 0:PciE-gen1 1:PciE-gen2 2:PciE-gen3
+    uint8_t     PcieLaneCount;          ///< 1=x1, 2=x2, 3=x4, 4=x8, 5=x12, 6=x16 
+    uint8_t     EnabledForActivity;
+    uint8_t     SPC;
+    uint32_t    DownThreshold;
+    uint32_t    UpThreshold;
+    uint32_t    Reserved;
+};
+
+typedef struct SMU73_Discrete_LinkLevel SMU73_Discrete_LinkLevel;
+
+
+// MC ARB DRAM Timing registers.
+struct SMU73_Discrete_MCArbDramTimingTableEntry
+{
+    uint32_t McArbDramTiming;
+    uint32_t McArbDramTiming2;
+    uint8_t  McArbBurstTime;
+    uint8_t  TRRDS;
+    uint8_t  TRRDL;
+    uint8_t  padding;
+};
+
+typedef struct SMU73_Discrete_MCArbDramTimingTableEntry SMU73_Discrete_MCArbDramTimingTableEntry;
+
+struct SMU73_Discrete_MCArbDramTimingTable
+{
+    SMU73_Discrete_MCArbDramTimingTableEntry entries[SMU__NUM_SCLK_DPM_STATE][SMU__NUM_MCLK_DPM_LEVELS];
+};
+
+typedef struct SMU73_Discrete_MCArbDramTimingTable SMU73_Discrete_MCArbDramTimingTable;
+
+// UVD VCLK/DCLK state (level) definition.
+struct SMU73_Discrete_UvdLevel
+{
+    uint32_t VclkFrequency;
+    uint32_t DclkFrequency;
+    uint32_t MinVoltage;
+    uint8_t  VclkDivider;
+    uint8_t  DclkDivider;
+    uint8_t  padding[2];
+};
+
+typedef struct SMU73_Discrete_UvdLevel SMU73_Discrete_UvdLevel;
+
+// Clocks for other external blocks (VCE, ACP, SAMU).
+struct SMU73_Discrete_ExtClkLevel
+{
+    uint32_t Frequency;
+    uint32_t MinVoltage;
+    uint8_t  Divider;
+    uint8_t  padding[3];
+};
+
+typedef struct SMU73_Discrete_ExtClkLevel SMU73_Discrete_ExtClkLevel;
+
+struct SMU73_Discrete_StateInfo
+{
+    uint32_t SclkFrequency;
+    uint32_t MclkFrequency;
+    uint32_t VclkFrequency;
+    uint32_t DclkFrequency;
+    uint32_t SamclkFrequency;
+    uint32_t AclkFrequency;
+    uint32_t EclkFrequency;
+    uint16_t MvddVoltage;
+    uint16_t padding16;
+    uint8_t  DisplayWatermark;
+    uint8_t  McArbIndex;
+    uint8_t  McRegIndex;
+    uint8_t  SeqIndex;
+    uint8_t  SclkDid;
+    int8_t   SclkIndex;
+    int8_t   MclkIndex;
+    uint8_t  PCIeGen;
+
+};
+
+typedef struct SMU73_Discrete_StateInfo SMU73_Discrete_StateInfo;
+
+struct SMU73_Discrete_DpmTable
+{
+    // Multi-DPM controller settings
+    SMU73_PIDController                  GraphicsPIDController;
+    SMU73_PIDController                  MemoryPIDController;
+    SMU73_PIDController                  LinkPIDController;
+
+    uint32_t                            SystemFlags;
+
+    // SMIO masks for voltage and phase controls
+    uint32_t                            VRConfig;
+    uint32_t                            SmioMask1;
+    uint32_t                            SmioMask2;
+    SMIO_Table                          SmioTable1;
+    SMIO_Table                          SmioTable2;
+
+    uint32_t                            MvddLevelCount;
+
+
+    uint8_t                             BapmVddcVidHiSidd        [SMU73_MAX_LEVELS_VDDC];
+    uint8_t                             BapmVddcVidLoSidd        [SMU73_MAX_LEVELS_VDDC];
+    uint8_t                             BapmVddcVidHiSidd2       [SMU73_MAX_LEVELS_VDDC];
+
+    uint8_t                             GraphicsDpmLevelCount;
+    uint8_t                             MemoryDpmLevelCount;
+    uint8_t                             LinkLevelCount;
+    uint8_t                             MasterDeepSleepControl;
+
+    uint8_t                             UvdLevelCount;
+    uint8_t                             VceLevelCount;
+    uint8_t                             AcpLevelCount;
+    uint8_t                             SamuLevelCount;
+
+    uint8_t                             ThermOutGpio;
+    uint8_t                             ThermOutPolarity;
+    uint8_t                             ThermOutMode;
+    uint8_t                             BootPhases;
+    uint32_t                            Reserved[4];
+
+    // State table entries for each DPM state
+    SMU73_Discrete_GraphicsLevel        GraphicsLevel           [SMU73_MAX_LEVELS_GRAPHICS];
+    SMU73_Discrete_MemoryLevel          MemoryACPILevel;
+    SMU73_Discrete_MemoryLevel          MemoryLevel             [SMU73_MAX_LEVELS_MEMORY];
+    SMU73_Discrete_LinkLevel            LinkLevel               [SMU73_MAX_LEVELS_LINK];
+    SMU73_Discrete_ACPILevel            ACPILevel;
+    SMU73_Discrete_UvdLevel             UvdLevel                [SMU73_MAX_LEVELS_UVD];
+    SMU73_Discrete_ExtClkLevel          VceLevel                [SMU73_MAX_LEVELS_VCE];
+    SMU73_Discrete_ExtClkLevel          AcpLevel                [SMU73_MAX_LEVELS_ACP];
+    SMU73_Discrete_ExtClkLevel          SamuLevel               [SMU73_MAX_LEVELS_SAMU];
+    SMU73_Discrete_Ulv                  Ulv;
+
+    uint32_t                            SclkStepSize;
+    uint32_t                            Smio                    [SMU73_MAX_ENTRIES_SMIO];
+
+    uint8_t                             UvdBootLevel;
+    uint8_t                             VceBootLevel;
+    uint8_t                             AcpBootLevel;
+    uint8_t                             SamuBootLevel;
+
+    uint8_t                             GraphicsBootLevel;
+    uint8_t                             GraphicsVoltageChangeEnable;
+    uint8_t                             GraphicsThermThrottleEnable;
+    uint8_t                             GraphicsInterval;
+
+    uint8_t                             VoltageInterval;
+    uint8_t                             ThermalInterval;
+    uint16_t                            TemperatureLimitHigh;
+
+    uint16_t                            TemperatureLimitLow;
+    uint8_t                             MemoryBootLevel;
+    uint8_t                             MemoryVoltageChangeEnable;
+
+    uint16_t                            BootMVdd;
+    uint8_t                             MemoryInterval;
+    uint8_t                             MemoryThermThrottleEnable;
+
+    uint16_t                            VoltageResponseTime;
+    uint16_t                            PhaseResponseTime;
+
+    uint8_t                             PCIeBootLinkLevel;
+    uint8_t                             PCIeGenInterval;
+    uint8_t                             DTEInterval;
+    uint8_t                             DTEMode;
+
+    uint8_t                             SVI2Enable;
+    uint8_t                             VRHotGpio;
+    uint8_t                             AcDcGpio;
+    uint8_t                             ThermGpio;
+
+    uint16_t                            PPM_PkgPwrLimit;
+    uint16_t                            PPM_TemperatureLimit;
+
+    uint16_t                            DefaultTdp;
+    uint16_t                            TargetTdp;
+
+    uint16_t                            FpsHighThreshold;
+    uint16_t                            FpsLowThreshold;
+
+    uint16_t                            TemperatureLimitEdge;
+    uint16_t                            TemperatureLimitHotspot;
+    uint16_t                            TemperatureLimitLiquid1;
+    uint16_t                            TemperatureLimitLiquid2;
+    uint16_t                            TemperatureLimitVrVddc;
+    uint16_t                            TemperatureLimitVrMvdd;
+    uint16_t                            TemperatureLimitPlx;
+
+    uint16_t                            FanGainEdge;
+    uint16_t                            FanGainHotspot;
+    uint16_t                            FanGainLiquid;
+    uint16_t                            FanGainVrVddc;
+    uint16_t                            FanGainVrMvdd;
+    uint16_t                            FanGainPlx;
+    uint16_t                            FanGainHbm;
+
+    uint8_t                             Liquid1_I2C_address;
+    uint8_t                             Liquid2_I2C_address;
+    uint8_t                             Vr_I2C_address;
+    uint8_t                             Plx_I2C_address;
+
+    uint8_t                             GeminiMode;
+    uint8_t                             spare17[3];
+    uint32_t                            GeminiApertureHigh;
+    uint32_t                            GeminiApertureLow;
+
+    uint8_t                             Liquid_I2C_LineSCL;
+    uint8_t                             Liquid_I2C_LineSDA;
+    uint8_t                             Vr_I2C_LineSCL;
+    uint8_t                             Vr_I2C_LineSDA;
+    uint8_t                             Plx_I2C_LineSCL;
+    uint8_t                             Plx_I2C_LineSDA;
+
+    uint8_t                             spare1253[2];
+    uint32_t                            spare123[2];
+
+    uint8_t                             DTEAmbientTempBase;
+    uint8_t                             DTETjOffset;
+    uint8_t                             GpuTjMax;
+    uint8_t                             GpuTjHyst;
+
+    uint16_t                            BootVddc;
+    uint16_t                            BootVddci;
+
+    uint32_t                            BAPM_TEMP_GRADIENT;
+
+    uint32_t                            LowSclkInterruptThreshold;
+    uint32_t                            VddGfxReChkWait;
+
+    uint8_t                             ClockStretcherAmount;
+    uint8_t                             Sclk_CKS_masterEn0_7;
+    uint8_t                             Sclk_CKS_masterEn8_15;
+    uint8_t                             DPMFreezeAndForced;
+
+    uint8_t                             Sclk_voltageOffset[8];
+
+    SMU_ClockStretcherDataTable         ClockStretcherDataTable;
+    SMU_CKS_LOOKUPTable                 CKS_LOOKUPTable;
+};
+
+typedef struct SMU73_Discrete_DpmTable SMU73_Discrete_DpmTable;
+
+
+// --------------------------------------------------- Fan Table -----------------------------------------------------------
+struct SMU73_Discrete_FanTable
+{
+    uint16_t FdoMode;
+    int16_t  TempMin;
+    int16_t  TempMed;
+    int16_t  TempMax;
+    int16_t  Slope1;
+    int16_t  Slope2;
+    int16_t  FdoMin;
+    int16_t  HystUp;
+    int16_t  HystDown;
+    int16_t  HystSlope;
+    int16_t  TempRespLim;
+    int16_t  TempCurr;
+    int16_t  SlopeCurr;
+    int16_t  PwmCurr;
+    uint32_t RefreshPeriod;
+    int16_t  FdoMax;
+    uint8_t  TempSrc;
+    int8_t   Padding;
+};
+
+typedef struct SMU73_Discrete_FanTable SMU73_Discrete_FanTable;
+
+#define SMU7_DISCRETE_GPIO_SCLK_DEBUG             4
+#define SMU7_DISCRETE_GPIO_SCLK_DEBUG_BIT         (0x1 << SMU7_DISCRETE_GPIO_SCLK_DEBUG)
+
+
+
+struct SMU7_MclkDpmScoreboard
+{
+
+    uint32_t PercentageBusy;
+
+    int32_t  PIDError;
+    int32_t  PIDIntegral;
+    int32_t  PIDOutput;
+
+    uint32_t SigmaDeltaAccum;
+    uint32_t SigmaDeltaOutput;
+    uint32_t SigmaDeltaLevel;
+
+    uint32_t UtilizationSetpoint;
+
+    uint8_t  TdpClampMode;
+    uint8_t  TdcClampMode;
+    uint8_t  ThermClampMode;
+    uint8_t  VoltageBusy;
+
+    int8_t   CurrLevel;
+    int8_t   TargLevel;
+    uint8_t  LevelChangeInProgress;
+    uint8_t  UpHyst;
+
+    uint8_t  DownHyst;
+    uint8_t  VoltageDownHyst;
+    uint8_t  DpmEnable;
+    uint8_t  DpmRunning;
+
+    uint8_t  DpmForce;
+    uint8_t  DpmForceLevel;
+    uint8_t  DisplayWatermark;
+    uint8_t  McArbIndex;
+
+    uint32_t MinimumPerfMclk;
+
+    uint8_t  AcpiReq;
+    uint8_t  AcpiAck;
+    uint8_t  MclkSwitchInProgress;
+    uint8_t  MclkSwitchCritical;
+
+    uint8_t  IgnoreVBlank;
+    uint8_t  TargetMclkIndex;
+    uint8_t  TargetMvddIndex;
+    uint8_t  MclkSwitchResult;
+
+    uint16_t VbiFailureCount;
+    uint8_t  VbiWaitCounter;
+    uint8_t  EnabledLevelsChange;
+
+    uint16_t LevelResidencyCounters [SMU73_MAX_LEVELS_MEMORY];
+    uint16_t LevelSwitchCounters [SMU73_MAX_LEVELS_MEMORY];
+
+    void     (*TargetStateCalculator)(uint8_t);
+    void     (*SavedTargetStateCalculator)(uint8_t);
+
+    uint16_t AutoDpmInterval;
+    uint16_t AutoDpmRange;
+
+    uint16_t VbiTimeoutCount;
+    uint16_t MclkSwitchingTime;
+
+    uint8_t  fastSwitch;
+    uint8_t  Save_PIC_VDDGFX_EXIT;
+    uint8_t  Save_PIC_VDDGFX_ENTER;
+    uint8_t  padding;
+
+};
+
+typedef struct SMU7_MclkDpmScoreboard SMU7_MclkDpmScoreboard;
+
+struct SMU7_UlvScoreboard
+{
+    uint8_t     EnterUlv;
+    uint8_t     ExitUlv;
+    uint8_t     UlvActive;
+    uint8_t     WaitingForUlv;
+    uint8_t     UlvEnable;
+    uint8_t     UlvRunning;
+    uint8_t     UlvMasterEnable;
+    uint8_t     padding;
+    uint32_t    UlvAbortedCount;
+    uint32_t    UlvTimeStamp;
+};
+
+typedef struct SMU7_UlvScoreboard SMU7_UlvScoreboard;
+
+struct VddgfxSavedRegisters
+{
+  uint32_t GPU_DBG[3];
+  uint32_t MEC_BaseAddress_Hi;
+  uint32_t MEC_BaseAddress_Lo;
+  uint32_t THM_TMON0_CTRL2__RDIR_PRESENT;
+  uint32_t THM_TMON1_CTRL2__RDIR_PRESENT;
+  uint32_t CP_INT_CNTL;
+};
+
+typedef struct VddgfxSavedRegisters VddgfxSavedRegisters;
+
+struct SMU7_VddGfxScoreboard
+{
+    uint8_t     VddGfxEnable;
+    uint8_t     VddGfxActive;
+    uint8_t     VPUResetOccured;
+    uint8_t     padding;
+
+    uint32_t    VddGfxEnteredCount;
+    uint32_t    VddGfxAbortedCount;
+
+    uint32_t    VddGfxVid;
+
+    VddgfxSavedRegisters SavedRegisters;
+};
+
+typedef struct SMU7_VddGfxScoreboard SMU7_VddGfxScoreboard;
+
+struct SMU7_TdcLimitScoreboard {
+  uint8_t  Enable;
+  uint8_t  Running;
+  uint16_t Alpha;
+  uint32_t FilteredIddc;
+  uint32_t IddcLimit;
+  uint32_t IddcHyst;
+  SMU7_HystController_Data HystControllerData;
+};
+
+typedef struct SMU7_TdcLimitScoreboard SMU7_TdcLimitScoreboard;
+
+struct SMU7_PkgPwrLimitScoreboard {
+  uint8_t  Enable;
+  uint8_t  Running;
+  uint16_t Alpha;
+  uint32_t FilteredPkgPwr;
+  uint32_t Limit;
+  uint32_t Hyst;
+  uint32_t LimitFromDriver;
+  SMU7_HystController_Data HystControllerData;
+};
+
+typedef struct SMU7_PkgPwrLimitScoreboard SMU7_PkgPwrLimitScoreboard;
+
+struct SMU7_BapmScoreboard {
+  uint32_t source_powers[SMU73_DTE_SOURCES];
+  uint32_t source_powers_last[SMU73_DTE_SOURCES];
+  int32_t entity_temperatures[SMU73_NUM_GPU_TES];
+  int32_t initial_entity_temperatures[SMU73_NUM_GPU_TES];
+  int32_t Limit;
+  int32_t Hyst;
+  int32_t therm_influence_coeff_table[SMU73_DTE_ITERATIONS * SMU73_DTE_SOURCES * SMU73_DTE_SINKS * 2];
+  int32_t therm_node_table[SMU73_DTE_ITERATIONS * SMU73_DTE_SOURCES * SMU73_DTE_SINKS];
+  uint16_t ConfigTDPPowerScalar;
+  uint16_t FanSpeedPowerScalar;
+  uint16_t OverDrivePowerScalar;
+  uint16_t OverDriveLimitScalar;
+  uint16_t FinalPowerScalar;
+  uint8_t VariantID;
+  uint8_t spare997;
+
+  SMU7_HystController_Data HystControllerData;
+
+  int32_t temperature_gradient_slope;
+  int32_t temperature_gradient;
+  uint32_t measured_temperature;
+};
+
+
+typedef struct SMU7_BapmScoreboard SMU7_BapmScoreboard;
+
+struct SMU7_AcpiScoreboard {
+  uint32_t SavedInterruptMask[2];
+  uint8_t LastACPIRequest;
+  uint8_t CgBifResp;
+  uint8_t RequestType;
+  uint8_t Padding;
+  SMU73_Discrete_ACPILevel D0Level;
+};
+
+typedef struct SMU7_AcpiScoreboard SMU7_AcpiScoreboard;
+
+struct SMU_QuadraticCoeffs {
+  int32_t m1;
+  uint32_t b;
+
+  int16_t m2;
+  uint8_t m1_shift;
+  uint8_t m2_shift;
+};
+
+typedef struct SMU_QuadraticCoeffs SMU_QuadraticCoeffs;
+
+struct SMU73_Discrete_PmFuses {
+  /* dw0-dw1 */
+  uint8_t BapmVddCVidHiSidd[8];
+
+  /* dw2-dw3 */
+  uint8_t BapmVddCVidLoSidd[8];
+
+  /* dw4-dw5 */
+  uint8_t VddCVid[8];
+
+  /* dw1*/
+  uint8_t SviLoadLineEn;
+  uint8_t SviLoadLineVddC;
+  uint8_t SviLoadLineTrimVddC;
+  uint8_t SviLoadLineOffsetVddC;
+
+  /* dw2 */
+  uint16_t TDC_VDDC_PkgLimit;
+  uint8_t TDC_VDDC_ThrottleReleaseLimitPerc;
+  uint8_t TDC_MAWt;
+
+  /* dw3 */
+  uint8_t TdcWaterfallCtl;
+  uint8_t LPMLTemperatureMin;
+  uint8_t LPMLTemperatureMax;
+  uint8_t Reserved;
+
+  /* dw4-dw7 */
+  uint8_t LPMLTemperatureScaler[16];
+
+  /* dw8-dw9 */
+  int16_t FuzzyFan_ErrorSetDelta;
+  int16_t FuzzyFan_ErrorRateSetDelta;
+  int16_t FuzzyFan_PwmSetDelta;
+  uint16_t Reserved6;
+
+  /* dw10-dw14 */
+  uint8_t GnbLPML[16];
+
+  /* dw15 */
+  uint8_t GnbLPMLMaxVid;
+  uint8_t GnbLPMLMinVid;
+  uint8_t Reserved1[2];
+
+  /* dw16 */
+  uint16_t BapmVddCBaseLeakageHiSidd;
+  uint16_t BapmVddCBaseLeakageLoSidd;
+
+  /* AVFS */
+  uint16_t  VFT_Temp[3];
+  uint16_t  padding;
+
+  SMU_QuadraticCoeffs VFT_ATE[3];
+
+  SMU_QuadraticCoeffs AVFS_GB;
+  SMU_QuadraticCoeffs ATE_ACBTC_GB;
+
+  SMU_QuadraticCoeffs P2V;
+
+  uint32_t PsmCharzFreq;
+
+  uint16_t InversionVoltage;
+  uint16_t PsmCharzTemp;
+
+  uint32_t EnabledAvfsModules;
+};
+
+typedef struct SMU73_Discrete_PmFuses SMU73_Discrete_PmFuses;
+
+struct SMU7_Discrete_Log_Header_Table {
+  uint32_t    version;
+  uint32_t    asic_id;
+  uint16_t    flags;
+  uint16_t    entry_size;
+  uint32_t    total_size;
+  uint32_t    num_of_entries;
+  uint8_t     type;
+  uint8_t     mode;
+  uint8_t     filler_0[2];
+  uint32_t    filler_1[2];
+};
+
+typedef struct SMU7_Discrete_Log_Header_Table SMU7_Discrete_Log_Header_Table;
+
+struct SMU7_Discrete_Log_Cntl {
+    uint8_t             Enabled;
+    uint8_t             Type;
+    uint8_t             padding[2];
+    uint32_t            BufferSize;
+    uint32_t            SamplesLogged;
+    uint32_t            SampleSize;
+    uint32_t            AddrL;
+    uint32_t            AddrH;
+};
+
+typedef struct SMU7_Discrete_Log_Cntl SMU7_Discrete_Log_Cntl;
+
+#define CAC_ACC_NW_NUM_OF_SIGNALS 87
+
+struct SMU7_Discrete_Cac_Collection_Table {
+  uint32_t temperature;
+  uint32_t cac_acc_nw[CAC_ACC_NW_NUM_OF_SIGNALS];
+};
+
+typedef struct SMU7_Discrete_Cac_Collection_Table SMU7_Discrete_Cac_Collection_Table;
+
+struct SMU7_Discrete_Cac_Verification_Table {
+  uint32_t VddcTotalPower;
+  uint32_t VddcLeakagePower;
+  uint32_t VddcConstantPower;
+  uint32_t VddcGfxDynamicPower;
+  uint32_t VddcUvdDynamicPower;
+  uint32_t VddcVceDynamicPower;
+  uint32_t VddcAcpDynamicPower;
+  uint32_t VddcPcieDynamicPower;
+  uint32_t VddcDceDynamicPower;
+  uint32_t VddcCurrent;
+  uint32_t VddcVoltage;
+  uint32_t VddciTotalPower;
+  uint32_t VddciLeakagePower;
+  uint32_t VddciConstantPower;
+  uint32_t VddciDynamicPower;
+  uint32_t Vddr1TotalPower;
+  uint32_t Vddr1LeakagePower;
+  uint32_t Vddr1ConstantPower;
+  uint32_t Vddr1DynamicPower;
+  uint32_t spare[4];
+  uint32_t temperature;
+};
+
+typedef struct SMU7_Discrete_Cac_Verification_Table SMU7_Discrete_Cac_Verification_Table;
+
+struct SMU7_Discrete_Pm_Status_Table {
+  //Thermal entities
+  int32_t  T_meas_max[SMU73_THERMAL_INPUT_LOOP_COUNT];
+  int32_t  T_meas_acc[SMU73_THERMAL_INPUT_LOOP_COUNT];
+  int32_t  T_meas_acc_cnt[SMU73_THERMAL_INPUT_LOOP_COUNT];
+  uint32_t T_hbm_acc;
+
+  //Voltage domains
+  uint32_t I_calc_max;
+  uint32_t I_calc_acc;
+  uint32_t P_meas_acc;
+  uint32_t V_meas_load_acc;
+  uint32_t I_meas_acc;
+  uint32_t P_meas_acc_vddci;
+  uint32_t V_meas_load_acc_vddci;
+  uint32_t I_meas_acc_vddci;
+
+  //Frequency
+  uint16_t Sclk_dpm_residency[8];
+  uint16_t Uvd_dpm_residency[8];
+  uint16_t Vce_dpm_residency[8];
+
+  //Chip
+  uint32_t P_roc_acc;
+  uint32_t PkgPwr_max;
+  uint32_t PkgPwr_acc;
+  uint32_t MclkSwitchingTime_max;
+  uint32_t MclkSwitchingTime_acc;
+  uint32_t FanPwm_acc;
+  uint32_t FanRpm_acc;
+  uint32_t Gfx_busy_acc;
+  uint32_t Mc_busy_acc;
+  uint32_t Fps_acc;
+
+  uint32_t AccCnt;
+};
+
+typedef struct SMU7_Discrete_Pm_Status_Table SMU7_Discrete_Pm_Status_Table;
+
+//FIXME THESE NEED TO BE UPDATED
+#define SMU7_SCLK_CAC 0x561
+#define SMU7_MCLK_CAC 0xF9
+#define SMU7_VCLK_CAC 0x2DE
+#define SMU7_DCLK_CAC 0x2DE
+#define SMU7_ECLK_CAC 0x25E
+#define SMU7_ACLK_CAC 0x25E
+#define SMU7_SAMCLK_CAC 0x25E
+#define SMU7_DISPCLK_CAC 0x100
+#define SMU7_CAC_CONSTANT 0x2EE3430
+#define SMU7_CAC_CONSTANT_SHIFT 18
+
+#define SMU7_VDDCI_MCLK_CONST        1765
+#define SMU7_VDDCI_MCLK_CONST_SHIFT  16
+#define SMU7_VDDCI_VDDCI_CONST       50958
+#define SMU7_VDDCI_VDDCI_CONST_SHIFT 14
+#define SMU7_VDDCI_CONST             11781
+#define SMU7_VDDCI_STROBE_PWR        1331
+
+#define SMU7_VDDR1_CONST            693
+#define SMU7_VDDR1_CAC_WEIGHT       20
+#define SMU7_VDDR1_CAC_WEIGHT_SHIFT 19
+#define SMU7_VDDR1_STROBE_PWR       512
+
+#define SMU7_AREA_COEFF_UVD 0xA78
+#define SMU7_AREA_COEFF_VCE 0x190A
+#define SMU7_AREA_COEFF_ACP 0x22D1
+#define SMU7_AREA_COEFF_SAMU 0x534
+
+//ThermOutMode values
+#define SMU7_THERM_OUT_MODE_DISABLE       0x0
+#define SMU7_THERM_OUT_MODE_THERM_ONLY    0x1
+#define SMU7_THERM_OUT_MODE_THERM_VRHOT   0x2
+
+#pragma pack(pop)
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/amdgpu/smu7_discrete.h b/drivers/gpu/drm/amd/powerplay/inc/smu7_discrete.h
index 0b0b404ff091..0b0b404ff091 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu7_discrete.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu7_discrete.h
diff --git a/drivers/gpu/drm/amd/amdgpu/smu7_fusion.h b/drivers/gpu/drm/amd/powerplay/inc/smu7_fusion.h
index 78ada9ffd508..78ada9ffd508 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu7_fusion.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu7_fusion.h
diff --git a/drivers/gpu/drm/amd/amdgpu/smu8.h b/drivers/gpu/drm/amd/powerplay/inc/smu8.h
index d758d07b6a31..d758d07b6a31 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu8.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu8.h
diff --git a/drivers/gpu/drm/amd/amdgpu/smu8_fusion.h b/drivers/gpu/drm/amd/powerplay/inc/smu8_fusion.h
index 5c9cc3c0bbfa..0c37c94e9414 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu8_fusion.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu8_fusion.h
@@ -48,6 +48,14 @@ struct SMU8_Port80MonitorTable {
 	uint8_t  EnableDramShadow;
 };
 
+/*  Display specific power management parameters */
+#define PWRMGT_SEPARATION_TIME_SHIFT            0
+#define PWRMGT_SEPARATION_TIME_MASK             0xFFFF
+#define PWRMGT_DISABLE_CPU_CSTATES_SHIFT        16
+#define PWRMGT_DISABLE_CPU_CSTATES_MASK         0x1
+#define PWRMGT_DISABLE_CPU_PSTATES_SHIFT        24
+#define PWRMGT_DISABLE_CPU_PSTATES_MASK         0x1
+
 /* Clock Table Definitions */
 #define NUM_SCLK_LEVELS     8
 #define NUM_LCLK_LEVELS     8
diff --git a/drivers/gpu/drm/amd/amdgpu/smu_ucode_xfer_cz.h b/drivers/gpu/drm/amd/powerplay/inc/smu_ucode_xfer_cz.h
index f8ba071f39c8..f8ba071f39c8 100644
--- a/drivers/gpu/drm/amd/amdgpu/smu_ucode_xfer_cz.h
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu_ucode_xfer_cz.h
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smu_ucode_xfer_vi.h b/drivers/gpu/drm/amd/powerplay/inc/smu_ucode_xfer_vi.h
new file mode 100644
index 000000000000..c24a81eebc7c
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smu_ucode_xfer_vi.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2014 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef SMU_UCODE_XFER_VI_H
+#define SMU_UCODE_XFER_VI_H
+
+#define SMU_DRAMData_TOC_VERSION  1
+#define MAX_IH_REGISTER_COUNT     65535
+#define SMU_DIGEST_SIZE_BYTES     20
+#define SMU_FB_SIZE_BYTES         1048576
+#define SMU_MAX_ENTRIES           12
+
+#define UCODE_ID_SMU              0
+#define UCODE_ID_SDMA0            1
+#define UCODE_ID_SDMA1            2
+#define UCODE_ID_CP_CE            3
+#define UCODE_ID_CP_PFP           4
+#define UCODE_ID_CP_ME            5
+#define UCODE_ID_CP_MEC           6
+#define UCODE_ID_CP_MEC_JT1       7
+#define UCODE_ID_CP_MEC_JT2       8
+#define UCODE_ID_GMCON_RENG       9
+#define UCODE_ID_RLC_G            10
+#define UCODE_ID_IH_REG_RESTORE   11
+#define UCODE_ID_VBIOS            12
+#define UCODE_ID_MISC_METADATA    13
+#define UCODE_ID_RLC_SCRATCH      32
+#define UCODE_ID_RLC_SRM_ARAM     33
+#define UCODE_ID_RLC_SRM_DRAM     34
+#define UCODE_ID_MEC_STORAGE      35
+#define UCODE_ID_VBIOS_PARAMETERS 36
+#define UCODE_META_DATA           0xFF
+
+#define UCODE_ID_SMU_MASK             0x00000001
+#define UCODE_ID_SDMA0_MASK           0x00000002
+#define UCODE_ID_SDMA1_MASK           0x00000004
+#define UCODE_ID_CP_CE_MASK           0x00000008
+#define UCODE_ID_CP_PFP_MASK          0x00000010
+#define UCODE_ID_CP_ME_MASK           0x00000020
+#define UCODE_ID_CP_MEC_MASK          0x00000040
+#define UCODE_ID_CP_MEC_JT1_MASK      0x00000080
+#define UCODE_ID_CP_MEC_JT2_MASK      0x00000100
+#define UCODE_ID_GMCON_RENG_MASK      0x00000200
+#define UCODE_ID_RLC_G_MASK           0x00000400
+#define UCODE_ID_IH_REG_RESTORE_MASK  0x00000800
+#define UCODE_ID_VBIOS_MASK           0x00001000
+
+#define UCODE_FLAG_UNHALT_MASK   0x1
+
+struct SMU_Entry {
+#ifndef __BIG_ENDIAN
+	uint16_t id;
+	uint16_t version;
+	uint32_t image_addr_high;
+	uint32_t image_addr_low;
+	uint32_t meta_data_addr_high;
+	uint32_t meta_data_addr_low;
+	uint32_t data_size_byte;
+	uint16_t flags;
+	uint16_t num_register_entries;
+#else
+	uint16_t version;
+	uint16_t id;
+	uint32_t image_addr_high;
+	uint32_t image_addr_low;
+	uint32_t meta_data_addr_high;
+	uint32_t meta_data_addr_low;
+	uint32_t data_size_byte;
+	uint16_t num_register_entries;
+	uint16_t flags;
+#endif
+};
+
+struct SMU_DRAMData_TOC {
+	uint32_t structure_version;
+	uint32_t num_entries;
+	struct SMU_Entry entry[SMU_MAX_ENTRIES];
+};
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/smumgr.h b/drivers/gpu/drm/amd/powerplay/inc/smumgr.h
new file mode 100644
index 000000000000..504f035d1843
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/smumgr.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _SMUMGR_H_
+#define _SMUMGR_H_
+#include <linux/types.h>
+#include "pp_instance.h"
+#include "amd_powerplay.h"
+
+struct pp_smumgr;
+struct pp_instance;
+
+#define smu_lower_32_bits(n) ((uint32_t)(n))
+#define smu_upper_32_bits(n) ((uint32_t)(((n)>>16)>>16))
+
+struct pp_smumgr_func {
+	int (*smu_init)(struct pp_smumgr *smumgr);
+	int (*smu_fini)(struct pp_smumgr *smumgr);
+	int (*start_smu)(struct pp_smumgr *smumgr);
+	int (*check_fw_load_finish)(struct pp_smumgr *smumgr,
+				    uint32_t firmware);
+	int (*request_smu_load_fw)(struct pp_smumgr *smumgr);
+	int (*request_smu_load_specific_fw)(struct pp_smumgr *smumgr,
+					    uint32_t firmware);
+	int (*get_argument)(struct pp_smumgr *smumgr);
+	int (*send_msg_to_smc)(struct pp_smumgr *smumgr, uint16_t msg);
+	int (*send_msg_to_smc_with_parameter)(struct pp_smumgr *smumgr,
+					  uint16_t msg, uint32_t parameter);
+	int (*download_pptable_settings)(struct pp_smumgr *smumgr,
+					 void **table);
+	int (*upload_pptable_settings)(struct pp_smumgr *smumgr);
+};
+
+struct pp_smumgr {
+	uint32_t chip_family;
+	uint32_t chip_id;
+	uint32_t hw_revision;
+	void *device;
+	void *backend;
+	uint32_t usec_timeout;
+	bool reload_fw;
+	const struct pp_smumgr_func *smumgr_funcs;
+};
+
+
+extern int smum_init(struct amd_pp_init *pp_init,
+		     struct pp_instance *handle);
+
+extern int smum_fini(struct pp_smumgr *smumgr);
+
+extern int smum_get_argument(struct pp_smumgr *smumgr);
+
+extern int smum_download_powerplay_table(struct pp_smumgr *smumgr, void **table);
+
+extern int smum_upload_powerplay_table(struct pp_smumgr *smumgr);
+
+extern int smum_send_msg_to_smc(struct pp_smumgr *smumgr, uint16_t msg);
+
+extern int smum_send_msg_to_smc_with_parameter(struct pp_smumgr *smumgr,
+					uint16_t msg, uint32_t parameter);
+
+extern int smum_wait_on_register(struct pp_smumgr *smumgr,
+				uint32_t index, uint32_t value, uint32_t mask);
+
+extern int smum_wait_for_register_unequal(struct pp_smumgr *smumgr,
+				uint32_t index, uint32_t value, uint32_t mask);
+
+extern int smum_wait_on_indirect_register(struct pp_smumgr *smumgr,
+				uint32_t indirect_port, uint32_t index,
+				uint32_t value, uint32_t mask);
+
+
+extern void smum_wait_for_indirect_register_unequal(
+				struct pp_smumgr *smumgr,
+				uint32_t indirect_port, uint32_t index,
+				uint32_t value, uint32_t mask);
+
+extern int smu_allocate_memory(void *device, uint32_t size,
+			 enum cgs_gpu_mem_type type,
+			 uint32_t byte_align, uint64_t *mc_addr,
+			 void **kptr, void *handle);
+
+extern int smu_free_memory(void *device, void *handle);
+
+#define SMUM_FIELD_SHIFT(reg, field) reg##__##field##__SHIFT
+
+#define SMUM_FIELD_MASK(reg, field) reg##__##field##_MASK
+
+#define SMUM_WAIT_INDIRECT_REGISTER_GIVEN_INDEX(smumgr,			\
+					port, index, value, mask)	\
+	smum_wait_on_indirect_register(smumgr,				\
+				mm##port##_INDEX, index, value, mask)
+
+
+#define SMUM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(smumgr,         \
+							index, value, mask) \
+		smum_wait_for_register_unequal(smumgr,            \
+					index, value, mask)
+
+#define SMUM_WAIT_REGISTER_UNEQUAL(smumgr, reg, value, mask)		\
+	SMUM_WAIT_REGISTER_UNEQUAL_GIVEN_INDEX(smumgr,			\
+				mm##reg, value, mask)
+
+#define SMUM_WAIT_FIELD_UNEQUAL(smumgr, reg, field, fieldval)		\
+	SMUM_WAIT_REGISTER_UNEQUAL(smumgr, reg,				\
+		(fieldval) << SMUM_FIELD_SHIFT(reg, field),		\
+		SMUM_FIELD_MASK(reg, field))
+
+#define SMUM_GET_FIELD(value, reg, field)				\
+		(((value) & SMUM_FIELD_MASK(reg, field))		\
+		>> SMUM_FIELD_SHIFT(reg, field))
+
+#define SMUM_READ_FIELD(device, reg, field)                           \
+		SMUM_GET_FIELD(cgs_read_register(device, mm##reg), reg, field)
+
+#define SMUM_SET_FIELD(value, reg, field, field_val)                  \
+		(((value) & ~SMUM_FIELD_MASK(reg, field)) |                    \
+		(SMUM_FIELD_MASK(reg, field) & ((field_val) <<                 \
+			SMUM_FIELD_SHIFT(reg, field))))
+
+#define SMUM_WAIT_VFPF_INDIRECT_REGISTER_GIVEN_INDEX(smumgr,		\
+				port, index, value, mask)		\
+	smum_wait_on_indirect_register(smumgr,				\
+		mm##port##_INDEX_0, index, value, mask)
+
+#define SMUM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(smumgr,	\
+				port, index, value, mask)		\
+	smum_wait_for_indirect_register_unequal(smumgr,			\
+		mm##port##_INDEX_0, index, value, mask)
+
+
+#define SMUM_WAIT_VFPF_INDIRECT_REGISTER(smumgr, port, reg, value, mask) \
+	SMUM_WAIT_VFPF_INDIRECT_REGISTER_GIVEN_INDEX(smumgr, port, ix##reg, value, mask)
+
+#define SMUM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL(smumgr, port, reg, value, mask)     \
+		SMUM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL_GIVEN_INDEX(smumgr, port, ix##reg, value, mask)
+
+
+/*Operations on named fields.*/
+
+#define SMUM_READ_VFPF_INDIRECT_FIELD(device, port, reg, field) \
+		SMUM_GET_FIELD(cgs_read_ind_register(device, port, ix##reg), \
+			reg, field)
+
+#define SMUM_WRITE_FIELD(device, reg, field, fieldval)            \
+		cgs_write_register(device, mm##reg, \
+		SMUM_SET_FIELD(cgs_read_register(device, mm##reg), reg, field, fieldval))
+
+#define SMUM_WRITE_VFPF_INDIRECT_FIELD(device, port, reg, field, fieldval)    \
+		cgs_write_ind_register(device, port, ix##reg, \
+			SMUM_SET_FIELD(cgs_read_ind_register(device, port, ix##reg), \
+			reg, field, fieldval))
+
+#define SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, port, reg, field, fieldval) \
+	SMUM_WAIT_VFPF_INDIRECT_REGISTER(smumgr, port, reg,		\
+		(fieldval) << SMUM_FIELD_SHIFT(reg, field),		\
+		SMUM_FIELD_MASK(reg, field))
+
+#define SMUM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(smumgr, port, reg, field, fieldval) \
+	SMUM_WAIT_VFPF_INDIRECT_REGISTER_UNEQUAL(smumgr, port, reg,	\
+		(fieldval) << SMUM_FIELD_SHIFT(reg, field),		\
+		SMUM_FIELD_MASK(reg, field))
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/inc/tonga_ppsmc.h b/drivers/gpu/drm/amd/powerplay/inc/tonga_ppsmc.h
new file mode 100644
index 000000000000..63631296d751
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/inc/tonga_ppsmc.h
@@ -0,0 +1,420 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef TONGA_PP_SMC_H
+#define TONGA_PP_SMC_H
+
+#pragma pack(push, 1)
+
+#define PPSMC_SWSTATE_FLAG_DC				0x01
+#define PPSMC_SWSTATE_FLAG_UVD				0x02
+#define PPSMC_SWSTATE_FLAG_VCE				0x04
+#define PPSMC_SWSTATE_FLAG_PCIE_X1			0x08
+
+#define PPSMC_THERMAL_PROTECT_TYPE_INTERNAL             0x00
+#define PPSMC_THERMAL_PROTECT_TYPE_EXTERNAL             0x01
+#define PPSMC_THERMAL_PROTECT_TYPE_NONE                 0xff
+
+#define PPSMC_SYSTEMFLAG_GPIO_DC                        0x01
+#define PPSMC_SYSTEMFLAG_STEPVDDC                       0x02
+#define PPSMC_SYSTEMFLAG_GDDR5                          0x04
+
+#define PPSMC_SYSTEMFLAG_DISABLE_BABYSTEP               0x08
+
+#define PPSMC_SYSTEMFLAG_REGULATOR_HOT                  0x10
+#define PPSMC_SYSTEMFLAG_REGULATOR_HOT_ANALOG           0x20
+#define PPSMC_SYSTEMFLAG_12CHANNEL                      0x40
+
+
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_MASK              0x07
+#define PPSMC_EXTRAFLAGS_AC2DC_DONT_WAIT_FOR_VBLANK     0x08
+
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTODPMLOWSTATE   0x00
+#define PPSMC_EXTRAFLAGS_AC2DC_ACTION_GOTOINITIALSTATE  0x01
+
+#define PPSMC_EXTRAFLAGS_AC2DC_GPIO5_POLARITY_HIGH      0x10
+#define PPSMC_EXTRAFLAGS_DRIVER_TO_GPIO17               0x20
+#define PPSMC_EXTRAFLAGS_PCC_TO_GPIO17                  0x40
+
+/* Defines for DPM 2.0 */
+#define PPSMC_DPM2FLAGS_TDPCLMP                         0x01
+#define PPSMC_DPM2FLAGS_PWRSHFT                         0x02
+#define PPSMC_DPM2FLAGS_OCP                             0x04
+
+/* Defines for display watermark level */
+
+#define PPSMC_DISPLAY_WATERMARK_LOW                     0
+#define PPSMC_DISPLAY_WATERMARK_HIGH                    1
+
+/* In the HW performance level's state flags:*/
+#define PPSMC_STATEFLAG_AUTO_PULSE_SKIP    0x01
+#define PPSMC_STATEFLAG_POWERBOOST         0x02
+#define PPSMC_STATEFLAG_PSKIP_ON_TDP_FAULT 0x04
+#define PPSMC_STATEFLAG_POWERSHIFT         0x08
+#define PPSMC_STATEFLAG_SLOW_READ_MARGIN   0x10
+#define PPSMC_STATEFLAG_DEEPSLEEP_THROTTLE 0x20
+#define PPSMC_STATEFLAG_DEEPSLEEP_BYPASS   0x40
+
+/* Fan control algorithm:*/
+#define FDO_MODE_HARDWARE 0
+#define FDO_MODE_PIECE_WISE_LINEAR 1
+
+enum FAN_CONTROL {
+	FAN_CONTROL_FUZZY,
+	FAN_CONTROL_TABLE
+};
+
+/* Return codes for driver to SMC communication.*/
+
+#define PPSMC_Result_OK             ((uint16_t)0x01)
+#define PPSMC_Result_NoMore         ((uint16_t)0x02)
+#define PPSMC_Result_NotNow         ((uint16_t)0x03)
+
+#define PPSMC_Result_Failed         ((uint16_t)0xFF)
+#define PPSMC_Result_UnknownCmd     ((uint16_t)0xFE)
+#define PPSMC_Result_UnknownVT      ((uint16_t)0xFD)
+
+typedef uint16_t PPSMC_Result;
+
+#define PPSMC_isERROR(x) ((uint16_t)0x80 & (x))
+
+
+#define PPSMC_MSG_Halt                      ((uint16_t)0x10)
+#define PPSMC_MSG_Resume                    ((uint16_t)0x11)
+#define PPSMC_MSG_EnableDPMLevel            ((uint16_t)0x12)
+#define PPSMC_MSG_ZeroLevelsDisabled        ((uint16_t)0x13)
+#define PPSMC_MSG_OneLevelsDisabled         ((uint16_t)0x14)
+#define PPSMC_MSG_TwoLevelsDisabled         ((uint16_t)0x15)
+#define PPSMC_MSG_EnableThermalInterrupt    ((uint16_t)0x16)
+#define PPSMC_MSG_RunningOnAC               ((uint16_t)0x17)
+#define PPSMC_MSG_LevelUp                   ((uint16_t)0x18)
+#define PPSMC_MSG_LevelDown                 ((uint16_t)0x19)
+#define PPSMC_MSG_ResetDPMCounters          ((uint16_t)0x1a)
+#define PPSMC_MSG_SwitchToSwState           ((uint16_t)0x20)
+
+#define PPSMC_MSG_SwitchToSwStateLast       ((uint16_t)0x3f)
+#define PPSMC_MSG_SwitchToInitialState      ((uint16_t)0x40)
+#define PPSMC_MSG_NoForcedLevel             ((uint16_t)0x41)
+#define PPSMC_MSG_ForceHigh                 ((uint16_t)0x42)
+#define PPSMC_MSG_ForceMediumOrHigh         ((uint16_t)0x43)
+
+#define PPSMC_MSG_SwitchToMinimumPower      ((uint16_t)0x51)
+#define PPSMC_MSG_ResumeFromMinimumPower    ((uint16_t)0x52)
+#define PPSMC_MSG_EnableCac                 ((uint16_t)0x53)
+#define PPSMC_MSG_DisableCac                ((uint16_t)0x54)
+#define PPSMC_DPMStateHistoryStart          ((uint16_t)0x55)
+#define PPSMC_DPMStateHistoryStop           ((uint16_t)0x56)
+#define PPSMC_CACHistoryStart               ((uint16_t)0x57)
+#define PPSMC_CACHistoryStop                ((uint16_t)0x58)
+#define PPSMC_TDPClampingActive             ((uint16_t)0x59)
+#define PPSMC_TDPClampingInactive           ((uint16_t)0x5A)
+#define PPSMC_StartFanControl               ((uint16_t)0x5B)
+#define PPSMC_StopFanControl                ((uint16_t)0x5C)
+#define PPSMC_NoDisplay                     ((uint16_t)0x5D)
+#define PPSMC_HasDisplay                    ((uint16_t)0x5E)
+#define PPSMC_MSG_UVDPowerOFF               ((uint16_t)0x60)
+#define PPSMC_MSG_UVDPowerON                ((uint16_t)0x61)
+#define PPSMC_MSG_EnableULV                 ((uint16_t)0x62)
+#define PPSMC_MSG_DisableULV                ((uint16_t)0x63)
+#define PPSMC_MSG_EnterULV                  ((uint16_t)0x64)
+#define PPSMC_MSG_ExitULV                   ((uint16_t)0x65)
+#define PPSMC_PowerShiftActive              ((uint16_t)0x6A)
+#define PPSMC_PowerShiftInactive            ((uint16_t)0x6B)
+#define PPSMC_OCPActive                     ((uint16_t)0x6C)
+#define PPSMC_OCPInactive                   ((uint16_t)0x6D)
+#define PPSMC_CACLongTermAvgEnable          ((uint16_t)0x6E)
+#define PPSMC_CACLongTermAvgDisable         ((uint16_t)0x6F)
+#define PPSMC_MSG_InferredStateSweep_Start  ((uint16_t)0x70)
+#define PPSMC_MSG_InferredStateSweep_Stop   ((uint16_t)0x71)
+#define PPSMC_MSG_SwitchToLowestInfState    ((uint16_t)0x72)
+#define PPSMC_MSG_SwitchToNonInfState       ((uint16_t)0x73)
+#define PPSMC_MSG_AllStateSweep_Start       ((uint16_t)0x74)
+#define PPSMC_MSG_AllStateSweep_Stop        ((uint16_t)0x75)
+#define PPSMC_MSG_SwitchNextLowerInfState   ((uint16_t)0x76)
+#define PPSMC_MSG_SwitchNextHigherInfState  ((uint16_t)0x77)
+#define PPSMC_MSG_MclkRetrainingTest        ((uint16_t)0x78)
+#define PPSMC_MSG_ForceTDPClamping          ((uint16_t)0x79)
+#define PPSMC_MSG_CollectCAC_PowerCorreln   ((uint16_t)0x7A)
+#define PPSMC_MSG_CollectCAC_WeightCalib    ((uint16_t)0x7B)
+#define PPSMC_MSG_CollectCAC_SQonly         ((uint16_t)0x7C)
+#define PPSMC_MSG_CollectCAC_TemperaturePwr ((uint16_t)0x7D)
+
+#define PPSMC_MSG_ExtremitiesTest_Start     ((uint16_t)0x7E)
+#define PPSMC_MSG_ExtremitiesTest_Stop      ((uint16_t)0x7F)
+#define PPSMC_FlushDataCache                ((uint16_t)0x80)
+#define PPSMC_FlushInstrCache               ((uint16_t)0x81)
+
+#define PPSMC_MSG_SetEnabledLevels          ((uint16_t)0x82)
+#define PPSMC_MSG_SetForcedLevels           ((uint16_t)0x83)
+
+#define PPSMC_MSG_ResetToDefaults           ((uint16_t)0x84)
+
+#define PPSMC_MSG_SetForcedLevelsAndJump    ((uint16_t)0x85)
+#define PPSMC_MSG_SetCACHistoryMode         ((uint16_t)0x86)
+#define PPSMC_MSG_EnableDTE                 ((uint16_t)0x87)
+#define PPSMC_MSG_DisableDTE                ((uint16_t)0x88)
+
+#define PPSMC_MSG_SmcSpaceSetAddress        ((uint16_t)0x89)
+#define PPSMC_MSG_ChangeNearTDPLimit        ((uint16_t)0x90)
+#define PPSMC_MSG_ChangeSafePowerLimit      ((uint16_t)0x91)
+
+#define PPSMC_MSG_DPMStateSweepStart        ((uint16_t)0x92)
+#define PPSMC_MSG_DPMStateSweepStop         ((uint16_t)0x93)
+
+#define PPSMC_MSG_OVRDDisableSCLKDS         ((uint16_t)0x94)
+#define PPSMC_MSG_CancelDisableOVRDSCLKDS   ((uint16_t)0x95)
+#define PPSMC_MSG_ThrottleOVRDSCLKDS        ((uint16_t)0x96)
+#define PPSMC_MSG_CancelThrottleOVRDSCLKDS  ((uint16_t)0x97)
+#define PPSMC_MSG_GPIO17					((uint16_t)0x98)
+
+#define PPSMC_MSG_API_SetSvi2Volt_Vddc      ((uint16_t)0x99)
+#define PPSMC_MSG_API_SetSvi2Volt_Vddci     ((uint16_t)0x9A)
+#define PPSMC_MSG_API_SetSvi2Volt_Mvdd      ((uint16_t)0x9B)
+#define PPSMC_MSG_API_GetSvi2Volt_Vddc      ((uint16_t)0x9C)
+#define PPSMC_MSG_API_GetSvi2Volt_Vddci     ((uint16_t)0x9D)
+#define PPSMC_MSG_API_GetSvi2Volt_Mvdd      ((uint16_t)0x9E)
+
+#define PPSMC_MSG_BREAK                     ((uint16_t)0xF8)
+
+/* Trinity Specific Messages*/
+#define PPSMC_MSG_Test                      ((uint16_t) 0x100)
+#define PPSMC_MSG_DPM_Voltage_Pwrmgt        ((uint16_t) 0x101)
+#define PPSMC_MSG_DPM_Config                ((uint16_t) 0x102)
+#define PPSMC_MSG_PM_Controller_Start       ((uint16_t) 0x103)
+#define PPSMC_MSG_DPM_ForceState            ((uint16_t) 0x104)
+#define PPSMC_MSG_PG_PowerDownSIMD          ((uint16_t) 0x105)
+#define PPSMC_MSG_PG_PowerUpSIMD            ((uint16_t) 0x106)
+#define PPSMC_MSG_PM_Controller_Stop        ((uint16_t) 0x107)
+#define PPSMC_MSG_PG_SIMD_Config            ((uint16_t) 0x108)
+#define PPSMC_MSG_Voltage_Cntl_Enable       ((uint16_t) 0x109)
+#define PPSMC_MSG_Thermal_Cntl_Enable       ((uint16_t) 0x10a)
+#define PPSMC_MSG_Reset_Service             ((uint16_t) 0x10b)
+#define PPSMC_MSG_VCEPowerOFF               ((uint16_t) 0x10e)
+#define PPSMC_MSG_VCEPowerON                ((uint16_t) 0x10f)
+#define PPSMC_MSG_DPM_Disable_VCE_HS        ((uint16_t) 0x110)
+#define PPSMC_MSG_DPM_Enable_VCE_HS         ((uint16_t) 0x111)
+#define PPSMC_MSG_DPM_N_LevelsDisabled      ((uint16_t) 0x112)
+#define PPSMC_MSG_DCEPowerOFF               ((uint16_t) 0x113)
+#define PPSMC_MSG_DCEPowerON                ((uint16_t) 0x114)
+#define PPSMC_MSG_PCIE_DDIPowerDown         ((uint16_t) 0x117)
+#define PPSMC_MSG_PCIE_DDIPowerUp           ((uint16_t) 0x118)
+#define PPSMC_MSG_PCIE_CascadePLLPowerDown  ((uint16_t) 0x119)
+#define PPSMC_MSG_PCIE_CascadePLLPowerUp    ((uint16_t) 0x11a)
+#define PPSMC_MSG_SYSPLLPowerOff            ((uint16_t) 0x11b)
+#define PPSMC_MSG_SYSPLLPowerOn             ((uint16_t) 0x11c)
+#define PPSMC_MSG_DCE_RemoveVoltageAdjustment   ((uint16_t) 0x11d)
+#define PPSMC_MSG_DCE_AllowVoltageAdjustment    ((uint16_t) 0x11e)
+#define PPSMC_MSG_DISPLAYPHYStatusNotify    ((uint16_t) 0x11f)
+#define PPSMC_MSG_EnableBAPM                ((uint16_t) 0x120)
+#define PPSMC_MSG_DisableBAPM               ((uint16_t) 0x121)
+#define PPSMC_MSG_PCIE_PHYPowerDown         ((uint16_t) 0x122)
+#define PPSMC_MSG_PCIE_PHYPowerUp           ((uint16_t) 0x123)
+#define PPSMC_MSG_UVD_DPM_Config            ((uint16_t) 0x124)
+#define PPSMC_MSG_Spmi_Enable               ((uint16_t) 0x122)
+#define PPSMC_MSG_Spmi_Timer                ((uint16_t) 0x123)
+#define PPSMC_MSG_LCLK_DPM_Config           ((uint16_t) 0x124)
+#define PPSMC_MSG_NBDPM_Config             ((uint16_t) 0x125)
+#define PPSMC_MSG_PCIE_DDIPhyPowerDown           ((uint16_t) 0x126)
+#define PPSMC_MSG_PCIE_DDIPhyPowerUp             ((uint16_t) 0x127)
+#define PPSMC_MSG_MCLKDPM_Config                ((uint16_t) 0x128)
+
+#define PPSMC_MSG_UVDDPM_Config               ((uint16_t) 0x129)
+#define PPSMC_MSG_VCEDPM_Config               ((uint16_t) 0x12A)
+#define PPSMC_MSG_ACPDPM_Config               ((uint16_t) 0x12B)
+#define PPSMC_MSG_SAMUDPM_Config              ((uint16_t) 0x12C)
+#define PPSMC_MSG_UVDDPM_SetEnabledMask       ((uint16_t) 0x12D)
+#define PPSMC_MSG_VCEDPM_SetEnabledMask       ((uint16_t) 0x12E)
+#define PPSMC_MSG_ACPDPM_SetEnabledMask       ((uint16_t) 0x12F)
+#define PPSMC_MSG_SAMUDPM_SetEnabledMask      ((uint16_t) 0x130)
+#define PPSMC_MSG_MCLKDPM_ForceState          ((uint16_t) 0x131)
+#define PPSMC_MSG_MCLKDPM_NoForcedLevel       ((uint16_t) 0x132)
+#define PPSMC_MSG_Thermal_Cntl_Disable        ((uint16_t) 0x133)
+#define PPSMC_MSG_SetTDPLimit                 ((uint16_t) 0x134)
+#define PPSMC_MSG_Voltage_Cntl_Disable        ((uint16_t) 0x135)
+#define PPSMC_MSG_PCIeDPM_Enable              ((uint16_t) 0x136)
+#define PPSMC_MSG_ACPPowerOFF                 ((uint16_t) 0x137)
+#define PPSMC_MSG_ACPPowerON                  ((uint16_t) 0x138)
+#define PPSMC_MSG_SAMPowerOFF                 ((uint16_t) 0x139)
+#define PPSMC_MSG_SAMPowerON                  ((uint16_t) 0x13a)
+#define PPSMC_MSG_SDMAPowerOFF                ((uint16_t) 0x13b)
+#define PPSMC_MSG_SDMAPowerON                 ((uint16_t) 0x13c)
+#define PPSMC_MSG_PCIeDPM_Disable             ((uint16_t) 0x13d)
+#define PPSMC_MSG_IOMMUPowerOFF               ((uint16_t) 0x13e)
+#define PPSMC_MSG_IOMMUPowerON                ((uint16_t) 0x13f)
+#define PPSMC_MSG_NBDPM_Enable                ((uint16_t) 0x140)
+#define PPSMC_MSG_NBDPM_Disable               ((uint16_t) 0x141)
+#define PPSMC_MSG_NBDPM_ForceNominal          ((uint16_t) 0x142)
+#define PPSMC_MSG_NBDPM_ForcePerformance      ((uint16_t) 0x143)
+#define PPSMC_MSG_NBDPM_UnForce               ((uint16_t) 0x144)
+#define PPSMC_MSG_SCLKDPM_SetEnabledMask      ((uint16_t) 0x145)
+#define PPSMC_MSG_MCLKDPM_SetEnabledMask      ((uint16_t) 0x146)
+#define PPSMC_MSG_PCIeDPM_ForceLevel          ((uint16_t) 0x147)
+#define PPSMC_MSG_PCIeDPM_UnForceLevel        ((uint16_t) 0x148)
+#define PPSMC_MSG_EnableACDCGPIOInterrupt     ((uint16_t) 0x149)
+#define PPSMC_MSG_EnableVRHotGPIOInterrupt    ((uint16_t) 0x14a)
+#define PPSMC_MSG_SwitchToAC                  ((uint16_t) 0x14b)
+
+#define PPSMC_MSG_XDMAPowerOFF                ((uint16_t) 0x14c)
+#define PPSMC_MSG_XDMAPowerON                 ((uint16_t) 0x14d)
+
+#define PPSMC_MSG_DPM_Enable                  ((uint16_t)0x14e)
+#define PPSMC_MSG_DPM_Disable                 ((uint16_t)0x14f)
+#define PPSMC_MSG_MCLKDPM_Enable              ((uint16_t)0x150)
+#define PPSMC_MSG_MCLKDPM_Disable             ((uint16_t)0x151)
+#define PPSMC_MSG_LCLKDPM_Enable              ((uint16_t)0x152)
+#define PPSMC_MSG_LCLKDPM_Disable             ((uint16_t)0x153)
+#define PPSMC_MSG_UVDDPM_Enable               ((uint16_t)0x154)
+#define PPSMC_MSG_UVDDPM_Disable              ((uint16_t)0x155)
+#define PPSMC_MSG_SAMUDPM_Enable              ((uint16_t)0x156)
+#define PPSMC_MSG_SAMUDPM_Disable             ((uint16_t)0x157)
+#define PPSMC_MSG_ACPDPM_Enable               ((uint16_t)0x158)
+#define PPSMC_MSG_ACPDPM_Disable              ((uint16_t)0x159)
+#define PPSMC_MSG_VCEDPM_Enable               ((uint16_t)0x15a)
+#define PPSMC_MSG_VCEDPM_Disable              ((uint16_t)0x15b)
+#define PPSMC_MSG_LCLKDPM_SetEnabledMask      ((uint16_t)0x15c)
+
+#define PPSMC_MSG_DPM_FPS_Mode                ((uint16_t) 0x15d)
+#define PPSMC_MSG_DPM_Activity_Mode           ((uint16_t) 0x15e)
+#define PPSMC_MSG_VddC_Request                ((uint16_t) 0x15f)
+#define PPSMC_MSG_MCLKDPM_GetEnabledMask      ((uint16_t) 0x160)
+#define PPSMC_MSG_LCLKDPM_GetEnabledMask      ((uint16_t) 0x161)
+#define PPSMC_MSG_SCLKDPM_GetEnabledMask      ((uint16_t) 0x162)
+#define PPSMC_MSG_UVDDPM_GetEnabledMask       ((uint16_t) 0x163)
+#define PPSMC_MSG_SAMUDPM_GetEnabledMask      ((uint16_t) 0x164)
+#define PPSMC_MSG_ACPDPM_GetEnabledMask       ((uint16_t) 0x165)
+#define PPSMC_MSG_VCEDPM_GetEnabledMask       ((uint16_t) 0x166)
+#define PPSMC_MSG_PCIeDPM_SetEnabledMask      ((uint16_t) 0x167)
+#define PPSMC_MSG_PCIeDPM_GetEnabledMask      ((uint16_t) 0x168)
+#define PPSMC_MSG_TDCLimitEnable              ((uint16_t) 0x169)
+#define PPSMC_MSG_TDCLimitDisable             ((uint16_t) 0x16a)
+#define PPSMC_MSG_DPM_AutoRotate_Mode         ((uint16_t) 0x16b)
+#define PPSMC_MSG_DISPCLK_FROM_FCH            ((uint16_t)0x16c)
+#define PPSMC_MSG_DISPCLK_FROM_DFS            ((uint16_t)0x16d)
+#define PPSMC_MSG_DPREFCLK_FROM_FCH           ((uint16_t)0x16e)
+#define PPSMC_MSG_DPREFCLK_FROM_DFS           ((uint16_t)0x16f)
+#define PPSMC_MSG_PmStatusLogStart            ((uint16_t)0x170)
+#define PPSMC_MSG_PmStatusLogSample           ((uint16_t)0x171)
+#define PPSMC_MSG_SCLK_AutoDPM_ON             ((uint16_t) 0x172)
+#define PPSMC_MSG_MCLK_AutoDPM_ON             ((uint16_t) 0x173)
+#define PPSMC_MSG_LCLK_AutoDPM_ON             ((uint16_t) 0x174)
+#define PPSMC_MSG_UVD_AutoDPM_ON              ((uint16_t) 0x175)
+#define PPSMC_MSG_SAMU_AutoDPM_ON             ((uint16_t) 0x176)
+#define PPSMC_MSG_ACP_AutoDPM_ON              ((uint16_t) 0x177)
+#define PPSMC_MSG_VCE_AutoDPM_ON              ((uint16_t) 0x178)
+#define PPSMC_MSG_PCIe_AutoDPM_ON             ((uint16_t) 0x179)
+#define PPSMC_MSG_MASTER_AutoDPM_ON           ((uint16_t) 0x17a)
+#define PPSMC_MSG_MASTER_AutoDPM_OFF          ((uint16_t) 0x17b)
+#define PPSMC_MSG_DYNAMICDISPPHYPOWER         ((uint16_t) 0x17c)
+#define PPSMC_MSG_CAC_COLLECTION_ON           ((uint16_t) 0x17d)
+#define PPSMC_MSG_CAC_COLLECTION_OFF          ((uint16_t) 0x17e)
+#define PPSMC_MSG_CAC_CORRELATION_ON          ((uint16_t) 0x17f)
+#define PPSMC_MSG_CAC_CORRELATION_OFF         ((uint16_t) 0x180)
+#define PPSMC_MSG_PM_STATUS_TO_DRAM_ON        ((uint16_t) 0x181)
+#define PPSMC_MSG_PM_STATUS_TO_DRAM_OFF       ((uint16_t) 0x182)
+#define PPSMC_MSG_UVD_HANDSHAKE_OFF           ((uint16_t) 0x183)
+#define PPSMC_MSG_ALLOW_LOWSCLK_INTERRUPT     ((uint16_t) 0x184)
+#define PPSMC_MSG_PkgPwrLimitEnable           ((uint16_t) 0x185)
+#define PPSMC_MSG_PkgPwrLimitDisable          ((uint16_t) 0x186)
+#define PPSMC_MSG_PkgPwrSetLimit              ((uint16_t) 0x187)
+#define PPSMC_MSG_OverDriveSetTargetTdp       ((uint16_t) 0x188)
+#define PPSMC_MSG_SCLKDPM_FreezeLevel         ((uint16_t) 0x189)
+#define PPSMC_MSG_SCLKDPM_UnfreezeLevel       ((uint16_t) 0x18A)
+#define PPSMC_MSG_MCLKDPM_FreezeLevel         ((uint16_t) 0x18B)
+#define PPSMC_MSG_MCLKDPM_UnfreezeLevel       ((uint16_t) 0x18C)
+#define PPSMC_MSG_START_DRAM_LOGGING          ((uint16_t) 0x18D)
+#define PPSMC_MSG_STOP_DRAM_LOGGING           ((uint16_t) 0x18E)
+#define PPSMC_MSG_MASTER_DeepSleep_ON         ((uint16_t) 0x18F)
+#define PPSMC_MSG_MASTER_DeepSleep_OFF        ((uint16_t) 0x190)
+#define PPSMC_MSG_Remove_DC_Clamp             ((uint16_t) 0x191)
+#define PPSMC_MSG_DisableACDCGPIOInterrupt    ((uint16_t) 0x192)
+#define PPSMC_MSG_OverrideVoltageControl_SetVddc       ((uint16_t) 0x193)
+#define PPSMC_MSG_OverrideVoltageControl_SetVddci      ((uint16_t) 0x194)
+#define PPSMC_MSG_SetVidOffset_1              ((uint16_t) 0x195)
+#define PPSMC_MSG_SetVidOffset_2              ((uint16_t) 0x207)
+#define PPSMC_MSG_GetVidOffset_1              ((uint16_t) 0x196)
+#define PPSMC_MSG_GetVidOffset_2              ((uint16_t) 0x208)
+#define PPSMC_MSG_THERMAL_OVERDRIVE_Enable    ((uint16_t) 0x197)
+#define PPSMC_MSG_THERMAL_OVERDRIVE_Disable	  ((uint16_t) 0x198)
+#define PPSMC_MSG_SetTjMax                    ((uint16_t) 0x199)
+#define PPSMC_MSG_SetFanPwmMax                ((uint16_t) 0x19A)
+
+#define PPSMC_MSG_WaitForMclkSwitchFinish	  ((uint16_t) 0x19B)
+#define PPSMC_MSG_ENABLE_THERMAL_DPM          ((uint16_t) 0x19C)
+#define PPSMC_MSG_DISABLE_THERMAL_DPM         ((uint16_t) 0x19D)
+#define PPSMC_MSG_Enable_PCC                  ((uint16_t) 0x19E)
+#define PPSMC_MSG_Disable_PCC                 ((uint16_t) 0x19F)
+
+#define PPSMC_MSG_API_GetSclkFrequency        ((uint16_t) 0x200)
+#define PPSMC_MSG_API_GetMclkFrequency        ((uint16_t) 0x201)
+#define PPSMC_MSG_API_GetSclkBusy             ((uint16_t) 0x202)
+#define PPSMC_MSG_API_GetMclkBusy             ((uint16_t) 0x203)
+#define PPSMC_MSG_API_GetAsicPower            ((uint16_t) 0x204)
+#define PPSMC_MSG_SetFanRpmMax                ((uint16_t) 0x205)
+#define PPSMC_MSG_SetFanSclkTarget            ((uint16_t) 0x206)
+#define PPSMC_MSG_SetFanMinPwm                ((uint16_t) 0x209)
+#define PPSMC_MSG_SetFanTemperatureTarget     ((uint16_t) 0x20A)
+
+#define PPSMC_MSG_BACO_StartMonitor           ((uint16_t) 0x240)
+#define PPSMC_MSG_BACO_Cancel                 ((uint16_t) 0x241)
+#define PPSMC_MSG_EnableVddGfx                ((uint16_t) 0x242)
+#define PPSMC_MSG_DisableVddGfx               ((uint16_t) 0x243)
+#define PPSMC_MSG_UcodeAddressLow             ((uint16_t) 0x244)
+#define PPSMC_MSG_UcodeAddressHigh            ((uint16_t) 0x245)
+#define PPSMC_MSG_UcodeLoadStatus             ((uint16_t) 0x246)
+
+#define PPSMC_MSG_DRV_DRAM_ADDR_HI			  ((uint16_t) 0x250)
+#define PPSMC_MSG_DRV_DRAM_ADDR_LO            ((uint16_t) 0x251)
+#define PPSMC_MSG_SMU_DRAM_ADDR_HI            ((uint16_t) 0x252)
+#define PPSMC_MSG_SMU_DRAM_ADDR_LO            ((uint16_t) 0x253)
+#define PPSMC_MSG_LoadUcodes                  ((uint16_t) 0x254)
+#define PPSMC_MSG_PowerStateNotify            ((uint16_t) 0x255)
+#define PPSMC_MSG_COND_EXEC_DRAM_ADDR_HI      ((uint16_t) 0x256)
+#define PPSMC_MSG_COND_EXEC_DRAM_ADDR_LO      ((uint16_t) 0x257)
+#define PPSMC_MSG_VBIOS_DRAM_ADDR_HI          ((uint16_t) 0x258)
+#define PPSMC_MSG_VBIOS_DRAM_ADDR_LO          ((uint16_t) 0x259)
+#define PPSMC_MSG_LoadVBios                   ((uint16_t) 0x25A)
+#define PPSMC_MSG_GetUcodeVersion             ((uint16_t) 0x25B)
+#define DMCUSMC_MSG_PSREntry                  ((uint16_t) 0x25C)
+#define DMCUSMC_MSG_PSRExit                   ((uint16_t) 0x25D)
+#define PPSMC_MSG_EnableClockGatingFeature    ((uint16_t) 0x260)
+#define PPSMC_MSG_DisableClockGatingFeature   ((uint16_t) 0x261)
+#define PPSMC_MSG_IsDeviceRunning             ((uint16_t) 0x262)
+#define PPSMC_MSG_LoadMetaData                ((uint16_t) 0x263)
+#define PPSMC_MSG_TMON_AutoCaliberate_Enable  ((uint16_t) 0x264)
+#define PPSMC_MSG_TMON_AutoCaliberate_Disable ((uint16_t) 0x265)
+#define PPSMC_MSG_GetTelemetry1Slope          ((uint16_t) 0x266)
+#define PPSMC_MSG_GetTelemetry1Offset         ((uint16_t) 0x267)
+#define PPSMC_MSG_GetTelemetry2Slope          ((uint16_t) 0x268)
+#define PPSMC_MSG_GetTelemetry2Offset         ((uint16_t) 0x269)
+
+typedef uint16_t PPSMC_Msg;
+
+/* If the SMC firmware has an event status soft register this is what the individual bits mean.*/
+#define PPSMC_EVENT_STATUS_THERMAL          0x00000001
+#define PPSMC_EVENT_STATUS_REGULATORHOT     0x00000002
+#define PPSMC_EVENT_STATUS_DC               0x00000004
+#define PPSMC_EVENT_STATUS_GPIO17           0x00000008
+
+
+#pragma pack(pop)
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/Makefile b/drivers/gpu/drm/amd/powerplay/smumgr/Makefile
new file mode 100644
index 000000000000..6c4ef135cf01
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/Makefile
@@ -0,0 +1,9 @@
+#
+# Makefile for the 'smu manager' sub-component of powerplay.
+# It provides the smu management services for the driver.
+
+SMU_MGR = smumgr.o cz_smumgr.o tonga_smumgr.o fiji_smumgr.o
+
+AMD_PP_SMUMGR = $(addprefix $(AMD_PP_PATH)/smumgr/,$(SMU_MGR))
+
+AMD_POWERPLAY_FILES += $(AMD_PP_SMUMGR)
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.c b/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.c
new file mode 100644
index 000000000000..ec222c665602
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.c
@@ -0,0 +1,881 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/gfp.h>
+#include "linux/delay.h"
+#include "cgs_common.h"
+#include "smu/smu_8_0_d.h"
+#include "smu/smu_8_0_sh_mask.h"
+#include "smu8.h"
+#include "smu8_fusion.h"
+#include "cz_smumgr.h"
+#include "cz_ppsmc.h"
+#include "smu_ucode_xfer_cz.h"
+#include "gca/gfx_8_0_d.h"
+#include "gca/gfx_8_0_sh_mask.h"
+#include "smumgr.h"
+
+#define SIZE_ALIGN_32(x)    (((x) + 31) / 32 * 32)
+
+static enum cz_scratch_entry firmware_list[] = {
+	CZ_SCRATCH_ENTRY_UCODE_ID_SDMA0,
+	CZ_SCRATCH_ENTRY_UCODE_ID_SDMA1,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_CE,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_PFP,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_ME,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT2,
+	CZ_SCRATCH_ENTRY_UCODE_ID_RLC_G,
+};
+
+static int cz_smum_get_argument(struct pp_smumgr *smumgr)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	return cgs_read_register(smumgr->device,
+					mmSMU_MP1_SRBM2P_ARG_0);
+}
+
+static int cz_send_msg_to_smc_async(struct pp_smumgr *smumgr,
+								uint16_t msg)
+{
+	int result = 0;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	result = SMUM_WAIT_FIELD_UNEQUAL(smumgr,
+					SMU_MP1_SRBM2P_RESP_0, CONTENT, 0);
+	if (result != 0) {
+		printk(KERN_ERR "[ powerplay ] cz_send_msg_to_smc_async failed\n");
+		return result;
+	}
+
+	cgs_write_register(smumgr->device, mmSMU_MP1_SRBM2P_RESP_0, 0);
+	cgs_write_register(smumgr->device, mmSMU_MP1_SRBM2P_MSG_0, msg);
+
+	return 0;
+}
+
+/* Send a message to the SMC, and wait for its response.*/
+static int cz_send_msg_to_smc(struct pp_smumgr *smumgr, uint16_t msg)
+{
+	int result = 0;
+
+	result = cz_send_msg_to_smc_async(smumgr, msg);
+	if (result != 0)
+		return result;
+
+	result = SMUM_WAIT_FIELD_UNEQUAL(smumgr,
+					SMU_MP1_SRBM2P_RESP_0, CONTENT, 0);
+
+	if (result != 0)
+		return result;
+
+	return 0;
+}
+
+static int cz_set_smc_sram_address(struct pp_smumgr *smumgr,
+				     uint32_t smc_address, uint32_t limit)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	if (0 != (3 & smc_address)) {
+		printk(KERN_ERR "[ powerplay ] SMC address must be 4 byte aligned\n");
+		return -1;
+	}
+
+	if (limit <= (smc_address + 3)) {
+		printk(KERN_ERR "[ powerplay ] SMC address beyond the SMC RAM area\n");
+		return -1;
+	}
+
+	cgs_write_register(smumgr->device, mmMP0PUB_IND_INDEX_0,
+				SMN_MP1_SRAM_START_ADDR + smc_address);
+
+	return 0;
+}
+
+static int cz_write_smc_sram_dword(struct pp_smumgr *smumgr,
+		uint32_t smc_address, uint32_t value, uint32_t limit)
+{
+	int result;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	result = cz_set_smc_sram_address(smumgr, smc_address, limit);
+	cgs_write_register(smumgr->device, mmMP0PUB_IND_DATA_0, value);
+
+	return 0;
+}
+
+static int cz_send_msg_to_smc_with_parameter(struct pp_smumgr *smumgr,
+					  uint16_t msg, uint32_t parameter)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	cgs_write_register(smumgr->device, mmSMU_MP1_SRBM2P_ARG_0, parameter);
+
+	return cz_send_msg_to_smc(smumgr, msg);
+}
+
+static int cz_request_smu_load_fw(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)(smumgr->backend);
+	int result = 0;
+	uint32_t smc_address;
+
+	if (!smumgr->reload_fw) {
+		printk(KERN_INFO "[ powerplay ] skip reloading...\n");
+		return 0;
+	}
+
+	smc_address = SMU8_FIRMWARE_HEADER_LOCATION +
+		offsetof(struct SMU8_Firmware_Header, UcodeLoadStatus);
+
+	cz_write_smc_sram_dword(smumgr, smc_address, 0, smc_address+4);
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+					PPSMC_MSG_DriverDramAddrHi,
+					cz_smu->toc_buffer.mc_addr_high);
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+					PPSMC_MSG_DriverDramAddrLo,
+					cz_smu->toc_buffer.mc_addr_low);
+
+	cz_send_msg_to_smc(smumgr, PPSMC_MSG_InitJobs);
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+					PPSMC_MSG_ExecuteJob,
+					cz_smu->toc_entry_aram);
+	cz_send_msg_to_smc_with_parameter(smumgr, PPSMC_MSG_ExecuteJob,
+				cz_smu->toc_entry_power_profiling_index);
+
+	result = cz_send_msg_to_smc_with_parameter(smumgr,
+					PPSMC_MSG_ExecuteJob,
+					cz_smu->toc_entry_initialize_index);
+
+	return result;
+}
+
+static int cz_check_fw_load_finish(struct pp_smumgr *smumgr,
+				   uint32_t firmware)
+{
+	int i;
+	uint32_t index = SMN_MP1_SRAM_START_ADDR +
+			 SMU8_FIRMWARE_HEADER_LOCATION +
+			 offsetof(struct SMU8_Firmware_Header, UcodeLoadStatus);
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	return cgs_read_register(smumgr->device,
+					mmSMU_MP1_SRBM2P_ARG_0);
+
+	cgs_write_register(smumgr->device, mmMP0PUB_IND_INDEX, index);
+
+	for (i = 0; i < smumgr->usec_timeout; i++) {
+		if (firmware ==
+			(cgs_read_register(smumgr->device, mmMP0PUB_IND_DATA) & firmware))
+			break;
+		udelay(1);
+	}
+
+	if (i >= smumgr->usec_timeout) {
+		printk(KERN_ERR "[ powerplay ] SMU check loaded firmware failed.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int cz_load_mec_firmware(struct pp_smumgr *smumgr)
+{
+	uint32_t reg_data;
+	uint32_t tmp;
+	int ret = 0;
+	struct cgs_firmware_info info = {0};
+	struct cz_smumgr *cz_smu;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	cz_smu = (struct cz_smumgr *)smumgr->backend;
+	ret = cgs_get_firmware_info(smumgr->device,
+						CGS_UCODE_ID_CP_MEC, &info);
+
+	if (ret)
+		return -EINVAL;
+
+	/* Disable MEC parsing/prefetching */
+	tmp = cgs_read_register(smumgr->device,
+					mmCP_MEC_CNTL);
+	tmp = SMUM_SET_FIELD(tmp, CP_MEC_CNTL, MEC_ME1_HALT, 1);
+	tmp = SMUM_SET_FIELD(tmp, CP_MEC_CNTL, MEC_ME2_HALT, 1);
+	cgs_write_register(smumgr->device, mmCP_MEC_CNTL, tmp);
+
+	tmp = cgs_read_register(smumgr->device,
+					mmCP_CPC_IC_BASE_CNTL);
+
+	tmp = SMUM_SET_FIELD(tmp, CP_CPC_IC_BASE_CNTL, VMID, 0);
+	tmp = SMUM_SET_FIELD(tmp, CP_CPC_IC_BASE_CNTL, ATC, 0);
+	tmp = SMUM_SET_FIELD(tmp, CP_CPC_IC_BASE_CNTL, CACHE_POLICY, 0);
+	tmp = SMUM_SET_FIELD(tmp, CP_CPC_IC_BASE_CNTL, MTYPE, 1);
+	cgs_write_register(smumgr->device, mmCP_CPC_IC_BASE_CNTL, tmp);
+
+	reg_data = smu_lower_32_bits(info.mc_addr) &
+			SMUM_FIELD_MASK(CP_CPC_IC_BASE_LO, IC_BASE_LO);
+	cgs_write_register(smumgr->device, mmCP_CPC_IC_BASE_LO, reg_data);
+
+	reg_data = smu_upper_32_bits(info.mc_addr) &
+			SMUM_FIELD_MASK(CP_CPC_IC_BASE_HI, IC_BASE_HI);
+	cgs_write_register(smumgr->device, mmCP_CPC_IC_BASE_HI, reg_data);
+
+	return 0;
+}
+
+static int cz_start_smu(struct pp_smumgr *smumgr)
+{
+	int ret = 0;
+	uint32_t fw_to_check = UCODE_ID_RLC_G_MASK |
+				UCODE_ID_SDMA0_MASK |
+				UCODE_ID_SDMA1_MASK |
+				UCODE_ID_CP_CE_MASK |
+				UCODE_ID_CP_ME_MASK |
+				UCODE_ID_CP_PFP_MASK |
+				UCODE_ID_CP_MEC_JT1_MASK |
+				UCODE_ID_CP_MEC_JT2_MASK;
+
+	if (smumgr->chip_id == CHIP_STONEY)
+		fw_to_check &= ~(UCODE_ID_SDMA1_MASK | UCODE_ID_CP_MEC_JT2_MASK);
+
+	cz_request_smu_load_fw(smumgr);
+	cz_check_fw_load_finish(smumgr, fw_to_check);
+
+	ret = cz_load_mec_firmware(smumgr);
+	if (ret)
+		printk(KERN_ERR "[ powerplay ] Mec Firmware load failed\n");
+
+	return ret;
+}
+
+static uint8_t cz_translate_firmware_enum_to_arg(struct pp_smumgr *smumgr,
+			enum cz_scratch_entry firmware_enum)
+{
+	uint8_t ret = 0;
+
+	switch (firmware_enum) {
+	case CZ_SCRATCH_ENTRY_UCODE_ID_SDMA0:
+		ret = UCODE_ID_SDMA0;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_SDMA1:
+		if (smumgr->chip_id == CHIP_STONEY)
+			ret = UCODE_ID_SDMA0;
+		else
+			ret = UCODE_ID_SDMA1;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_CP_CE:
+		ret = UCODE_ID_CP_CE;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_CP_PFP:
+		ret = UCODE_ID_CP_PFP;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_CP_ME:
+		ret = UCODE_ID_CP_ME;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1:
+		ret = UCODE_ID_CP_MEC_JT1;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT2:
+		if (smumgr->chip_id == CHIP_STONEY)
+			ret = UCODE_ID_CP_MEC_JT1;
+		else
+			ret = UCODE_ID_CP_MEC_JT2;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_GMCON_RENG:
+		ret = UCODE_ID_GMCON_RENG;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_RLC_G:
+		ret = UCODE_ID_RLC_G;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SCRATCH:
+		ret = UCODE_ID_RLC_SCRATCH;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_ARAM:
+		ret = UCODE_ID_RLC_SRM_ARAM;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_DRAM:
+		ret = UCODE_ID_RLC_SRM_DRAM;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_DMCU_ERAM:
+		ret = UCODE_ID_DMCU_ERAM;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_DMCU_IRAM:
+		ret = UCODE_ID_DMCU_IRAM;
+		break;
+	case CZ_SCRATCH_ENTRY_UCODE_ID_POWER_PROFILING:
+		ret = TASK_ARG_INIT_MM_PWR_LOG;
+		break;
+	case CZ_SCRATCH_ENTRY_DATA_ID_SDMA_HALT:
+	case CZ_SCRATCH_ENTRY_DATA_ID_SYS_CLOCKGATING:
+	case CZ_SCRATCH_ENTRY_DATA_ID_SDMA_RING_REGS:
+	case CZ_SCRATCH_ENTRY_DATA_ID_NONGFX_REINIT:
+	case CZ_SCRATCH_ENTRY_DATA_ID_SDMA_START:
+	case CZ_SCRATCH_ENTRY_DATA_ID_IH_REGISTERS:
+		ret = TASK_ARG_REG_MMIO;
+		break;
+	case CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE:
+		ret = TASK_ARG_INIT_CLK_TABLE;
+		break;
+	}
+
+	return ret;
+}
+
+static enum cgs_ucode_id cz_convert_fw_type_to_cgs(uint32_t fw_type)
+{
+	enum cgs_ucode_id result = CGS_UCODE_ID_MAXIMUM;
+
+	switch (fw_type) {
+	case UCODE_ID_SDMA0:
+		result = CGS_UCODE_ID_SDMA0;
+		break;
+	case UCODE_ID_SDMA1:
+		result = CGS_UCODE_ID_SDMA1;
+		break;
+	case UCODE_ID_CP_CE:
+		result = CGS_UCODE_ID_CP_CE;
+		break;
+	case UCODE_ID_CP_PFP:
+		result = CGS_UCODE_ID_CP_PFP;
+		break;
+	case UCODE_ID_CP_ME:
+		result = CGS_UCODE_ID_CP_ME;
+		break;
+	case UCODE_ID_CP_MEC_JT1:
+		result = CGS_UCODE_ID_CP_MEC_JT1;
+		break;
+	case UCODE_ID_CP_MEC_JT2:
+		result = CGS_UCODE_ID_CP_MEC_JT2;
+		break;
+	case UCODE_ID_RLC_G:
+		result = CGS_UCODE_ID_RLC_G;
+		break;
+	default:
+		break;
+	}
+
+	return result;
+}
+
+static int cz_smu_populate_single_scratch_task(
+			struct pp_smumgr *smumgr,
+			enum cz_scratch_entry fw_enum,
+			uint8_t type, bool is_last)
+{
+	uint8_t i;
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	struct TOC *toc = (struct TOC *)cz_smu->toc_buffer.kaddr;
+	struct SMU_Task *task = &toc->tasks[cz_smu->toc_entry_used_count++];
+
+	task->type = type;
+	task->arg = cz_translate_firmware_enum_to_arg(smumgr, fw_enum);
+	task->next = is_last ? END_OF_TASK_LIST : cz_smu->toc_entry_used_count;
+
+	for (i = 0; i < cz_smu->scratch_buffer_length; i++)
+		if (cz_smu->scratch_buffer[i].firmware_ID == fw_enum)
+			break;
+
+	if (i >= cz_smu->scratch_buffer_length) {
+		printk(KERN_ERR "[ powerplay ] Invalid Firmware Type\n");
+		return -EINVAL;
+	}
+
+	task->addr.low = cz_smu->scratch_buffer[i].mc_addr_low;
+	task->addr.high = cz_smu->scratch_buffer[i].mc_addr_high;
+	task->size_bytes = cz_smu->scratch_buffer[i].data_size;
+
+	if (CZ_SCRATCH_ENTRY_DATA_ID_IH_REGISTERS == fw_enum) {
+		struct cz_ih_meta_data *pIHReg_restore =
+		     (struct cz_ih_meta_data *)cz_smu->scratch_buffer[i].kaddr;
+		pIHReg_restore->command =
+			METADATA_CMD_MODE0 | METADATA_PERFORM_ON_LOAD;
+	}
+
+	return 0;
+}
+
+static int cz_smu_populate_single_ucode_load_task(
+					struct pp_smumgr *smumgr,
+					enum cz_scratch_entry fw_enum,
+					bool is_last)
+{
+	uint8_t i;
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	struct TOC *toc = (struct TOC *)cz_smu->toc_buffer.kaddr;
+	struct SMU_Task *task = &toc->tasks[cz_smu->toc_entry_used_count++];
+
+	task->type = TASK_TYPE_UCODE_LOAD;
+	task->arg = cz_translate_firmware_enum_to_arg(smumgr, fw_enum);
+	task->next = is_last ? END_OF_TASK_LIST : cz_smu->toc_entry_used_count;
+
+	for (i = 0; i < cz_smu->driver_buffer_length; i++)
+		if (cz_smu->driver_buffer[i].firmware_ID == fw_enum)
+			break;
+
+	if (i >= cz_smu->driver_buffer_length) {
+		printk(KERN_ERR "[ powerplay ] Invalid Firmware Type\n");
+		return -EINVAL;
+	}
+
+	task->addr.low = cz_smu->driver_buffer[i].mc_addr_low;
+	task->addr.high = cz_smu->driver_buffer[i].mc_addr_high;
+	task->size_bytes = cz_smu->driver_buffer[i].data_size;
+
+	return 0;
+}
+
+static int cz_smu_construct_toc_for_rlc_aram_save(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+
+	cz_smu->toc_entry_aram = cz_smu->toc_entry_used_count;
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_ARAM,
+				TASK_TYPE_UCODE_SAVE, true);
+
+	return 0;
+}
+
+static int cz_smu_initialize_toc_empty_job_list(struct pp_smumgr *smumgr)
+{
+	int i;
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	struct TOC *toc = (struct TOC *)cz_smu->toc_buffer.kaddr;
+
+	for (i = 0; i < NUM_JOBLIST_ENTRIES; i++)
+		toc->JobList[i] = (uint8_t)IGNORE_JOB;
+
+	return 0;
+}
+
+static int cz_smu_construct_toc_for_vddgfx_enter(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	struct TOC *toc = (struct TOC *)cz_smu->toc_buffer.kaddr;
+
+	toc->JobList[JOB_GFX_SAVE] = (uint8_t)cz_smu->toc_entry_used_count;
+	cz_smu_populate_single_scratch_task(smumgr,
+				    CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SCRATCH,
+				    TASK_TYPE_UCODE_SAVE, false);
+
+	cz_smu_populate_single_scratch_task(smumgr,
+				    CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_DRAM,
+				    TASK_TYPE_UCODE_SAVE, true);
+
+	return 0;
+}
+
+
+static int cz_smu_construct_toc_for_vddgfx_exit(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	struct TOC *toc = (struct TOC *)cz_smu->toc_buffer.kaddr;
+
+	toc->JobList[JOB_GFX_RESTORE] = (uint8_t)cz_smu->toc_entry_used_count;
+
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_CE, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_PFP, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_ME, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1, false);
+
+	if (smumgr->chip_id == CHIP_STONEY)
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1, false);
+	else
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT2, false);
+
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_G, false);
+
+	/* populate scratch */
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SCRATCH,
+				TASK_TYPE_UCODE_LOAD, false);
+
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_ARAM,
+				TASK_TYPE_UCODE_LOAD, false);
+
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_DRAM,
+				TASK_TYPE_UCODE_LOAD, true);
+
+	return 0;
+}
+
+static int cz_smu_construct_toc_for_power_profiling(
+						 struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+
+	cz_smu->toc_entry_power_profiling_index = cz_smu->toc_entry_used_count;
+
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_POWER_PROFILING,
+				TASK_TYPE_INITIALIZE, true);
+	return 0;
+}
+
+static int cz_smu_construct_toc_for_bootup(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+
+	cz_smu->toc_entry_initialize_index = cz_smu->toc_entry_used_count;
+
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_SDMA0, false);
+	if (smumgr->chip_id == CHIP_STONEY)
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_SDMA0, false);
+	else
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_SDMA1, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_CE, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_PFP, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_ME, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1, false);
+	if (smumgr->chip_id == CHIP_STONEY)
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1, false);
+	else
+		cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT2, false);
+	cz_smu_populate_single_ucode_load_task(smumgr,
+				CZ_SCRATCH_ENTRY_UCODE_ID_RLC_G, true);
+
+	return 0;
+}
+
+static int cz_smu_construct_toc_for_clock_table(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+
+	cz_smu->toc_entry_clock_table = cz_smu->toc_entry_used_count;
+
+	cz_smu_populate_single_scratch_task(smumgr,
+				CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE,
+				TASK_TYPE_INITIALIZE, true);
+
+	return 0;
+}
+
+static int cz_smu_construct_toc(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+
+	cz_smu->toc_entry_used_count = 0;
+
+	cz_smu_initialize_toc_empty_job_list(smumgr);
+
+	cz_smu_construct_toc_for_rlc_aram_save(smumgr);
+
+	cz_smu_construct_toc_for_vddgfx_enter(smumgr);
+
+	cz_smu_construct_toc_for_vddgfx_exit(smumgr);
+
+	cz_smu_construct_toc_for_power_profiling(smumgr);
+
+	cz_smu_construct_toc_for_bootup(smumgr);
+
+	cz_smu_construct_toc_for_clock_table(smumgr);
+
+	return 0;
+}
+
+static int cz_smu_populate_firmware_entries(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	uint32_t firmware_type;
+	uint32_t i;
+	int ret;
+	enum cgs_ucode_id ucode_id;
+	struct cgs_firmware_info info = {0};
+
+	cz_smu->driver_buffer_length = 0;
+
+	for (i = 0; i < sizeof(firmware_list)/sizeof(*firmware_list); i++) {
+
+		firmware_type = cz_translate_firmware_enum_to_arg(smumgr,
+					firmware_list[i]);
+
+		ucode_id = cz_convert_fw_type_to_cgs(firmware_type);
+
+		ret = cgs_get_firmware_info(smumgr->device,
+							ucode_id, &info);
+
+		if (ret == 0) {
+			cz_smu->driver_buffer[i].mc_addr_high =
+					smu_upper_32_bits(info.mc_addr);
+
+			cz_smu->driver_buffer[i].mc_addr_low =
+					smu_lower_32_bits(info.mc_addr);
+
+			cz_smu->driver_buffer[i].data_size = info.image_size;
+
+			cz_smu->driver_buffer[i].firmware_ID = firmware_list[i];
+			cz_smu->driver_buffer_length++;
+		}
+	}
+
+	return 0;
+}
+
+static int cz_smu_populate_single_scratch_entry(
+				struct pp_smumgr *smumgr,
+				enum cz_scratch_entry scratch_type,
+				uint32_t ulsize_byte,
+				struct cz_buffer_entry *entry)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	long long mc_addr =
+			((long long)(cz_smu->smu_buffer.mc_addr_high) << 32)
+			| cz_smu->smu_buffer.mc_addr_low;
+
+	uint32_t ulsize_aligned = SIZE_ALIGN_32(ulsize_byte);
+
+	mc_addr += cz_smu->smu_buffer_used_bytes;
+
+	entry->data_size = ulsize_byte;
+	entry->kaddr = (char *) cz_smu->smu_buffer.kaddr +
+				cz_smu->smu_buffer_used_bytes;
+	entry->mc_addr_low = smu_lower_32_bits(mc_addr);
+	entry->mc_addr_high = smu_upper_32_bits(mc_addr);
+	entry->firmware_ID = scratch_type;
+
+	cz_smu->smu_buffer_used_bytes += ulsize_aligned;
+
+	return 0;
+}
+
+static int cz_download_pptable_settings(struct pp_smumgr *smumgr, void **table)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	unsigned long i;
+
+	for (i = 0; i < cz_smu->scratch_buffer_length; i++) {
+		if (cz_smu->scratch_buffer[i].firmware_ID
+			== CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE)
+			break;
+	}
+
+	*table = (struct SMU8_Fusion_ClkTable *)cz_smu->scratch_buffer[i].kaddr;
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+				PPSMC_MSG_SetClkTableAddrHi,
+				cz_smu->scratch_buffer[i].mc_addr_high);
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+				PPSMC_MSG_SetClkTableAddrLo,
+				cz_smu->scratch_buffer[i].mc_addr_low);
+
+	cz_send_msg_to_smc_with_parameter(smumgr, PPSMC_MSG_ExecuteJob,
+				cz_smu->toc_entry_clock_table);
+
+	cz_send_msg_to_smc(smumgr, PPSMC_MSG_ClkTableXferToDram);
+
+	return 0;
+}
+
+static int cz_upload_pptable_settings(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	unsigned long i;
+
+	for (i = 0; i < cz_smu->scratch_buffer_length; i++) {
+		if (cz_smu->scratch_buffer[i].firmware_ID
+				== CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE)
+			break;
+	}
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+				PPSMC_MSG_SetClkTableAddrHi,
+				cz_smu->scratch_buffer[i].mc_addr_high);
+
+	cz_send_msg_to_smc_with_parameter(smumgr,
+				PPSMC_MSG_SetClkTableAddrLo,
+				cz_smu->scratch_buffer[i].mc_addr_low);
+
+	cz_send_msg_to_smc_with_parameter(smumgr, PPSMC_MSG_ExecuteJob,
+				cz_smu->toc_entry_clock_table);
+
+	cz_send_msg_to_smc(smumgr, PPSMC_MSG_ClkTableXferToSmu);
+
+	return 0;
+}
+
+static int cz_smu_init(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu = (struct cz_smumgr *)smumgr->backend;
+	uint64_t mc_addr = 0;
+	int ret = 0;
+
+	cz_smu->toc_buffer.data_size = 4096;
+	cz_smu->smu_buffer.data_size =
+		ALIGN(UCODE_ID_RLC_SCRATCH_SIZE_BYTE, 32) +
+		ALIGN(UCODE_ID_RLC_SRM_ARAM_SIZE_BYTE, 32) +
+		ALIGN(UCODE_ID_RLC_SRM_DRAM_SIZE_BYTE, 32) +
+		ALIGN(sizeof(struct SMU8_MultimediaPowerLogData), 32) +
+		ALIGN(sizeof(struct SMU8_Fusion_ClkTable), 32);
+
+	ret = smu_allocate_memory(smumgr->device,
+				cz_smu->toc_buffer.data_size,
+				CGS_GPU_MEM_TYPE__GART_CACHEABLE,
+				PAGE_SIZE,
+				&mc_addr,
+				&cz_smu->toc_buffer.kaddr,
+				&cz_smu->toc_buffer.handle);
+	if (ret != 0)
+		return -1;
+
+	cz_smu->toc_buffer.mc_addr_high = smu_upper_32_bits(mc_addr);
+	cz_smu->toc_buffer.mc_addr_low = smu_lower_32_bits(mc_addr);
+
+	ret = smu_allocate_memory(smumgr->device,
+				cz_smu->smu_buffer.data_size,
+				CGS_GPU_MEM_TYPE__GART_CACHEABLE,
+				PAGE_SIZE,
+				&mc_addr,
+				&cz_smu->smu_buffer.kaddr,
+				&cz_smu->smu_buffer.handle);
+	if (ret != 0)
+		return -1;
+
+	cz_smu->smu_buffer.mc_addr_high = smu_upper_32_bits(mc_addr);
+	cz_smu->smu_buffer.mc_addr_low = smu_lower_32_bits(mc_addr);
+
+	cz_smu_populate_firmware_entries(smumgr);
+	if (0 != cz_smu_populate_single_scratch_entry(smumgr,
+		CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SCRATCH,
+		UCODE_ID_RLC_SCRATCH_SIZE_BYTE,
+		&cz_smu->scratch_buffer[cz_smu->scratch_buffer_length++])) {
+		printk(KERN_ERR "[ powerplay ] Error when Populate Firmware Entry.\n");
+		return -1;
+	}
+
+	if (0 != cz_smu_populate_single_scratch_entry(smumgr,
+		CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_ARAM,
+		UCODE_ID_RLC_SRM_ARAM_SIZE_BYTE,
+		&cz_smu->scratch_buffer[cz_smu->scratch_buffer_length++])) {
+		printk(KERN_ERR "[ powerplay ] Error when Populate Firmware Entry.\n");
+		return -1;
+	}
+	if (0 != cz_smu_populate_single_scratch_entry(smumgr,
+		CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_DRAM,
+		UCODE_ID_RLC_SRM_DRAM_SIZE_BYTE,
+		&cz_smu->scratch_buffer[cz_smu->scratch_buffer_length++])) {
+		printk(KERN_ERR "[ powerplay ] Error when Populate Firmware Entry.\n");
+		return -1;
+	}
+
+	if (0 != cz_smu_populate_single_scratch_entry(smumgr,
+		CZ_SCRATCH_ENTRY_UCODE_ID_POWER_PROFILING,
+		sizeof(struct SMU8_MultimediaPowerLogData),
+		&cz_smu->scratch_buffer[cz_smu->scratch_buffer_length++])) {
+		printk(KERN_ERR "[ powerplay ] Error when Populate Firmware Entry.\n");
+		return -1;
+	}
+
+	if (0 != cz_smu_populate_single_scratch_entry(smumgr,
+		CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE,
+		sizeof(struct SMU8_Fusion_ClkTable),
+		&cz_smu->scratch_buffer[cz_smu->scratch_buffer_length++])) {
+		printk(KERN_ERR "[ powerplay ] Error when Populate Firmware Entry.\n");
+		return -1;
+	}
+	cz_smu_construct_toc(smumgr);
+
+	return 0;
+}
+
+static int cz_smu_fini(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	cz_smu = (struct cz_smumgr *)smumgr->backend;
+	if (cz_smu) {
+		cgs_free_gpu_mem(smumgr->device,
+				cz_smu->toc_buffer.handle);
+		cgs_free_gpu_mem(smumgr->device,
+				cz_smu->smu_buffer.handle);
+		kfree(cz_smu);
+		kfree(smumgr);
+	}
+
+	return 0;
+}
+
+static const struct pp_smumgr_func cz_smu_funcs = {
+	.smu_init = cz_smu_init,
+	.smu_fini = cz_smu_fini,
+	.start_smu = cz_start_smu,
+	.check_fw_load_finish = cz_check_fw_load_finish,
+	.request_smu_load_fw = NULL,
+	.request_smu_load_specific_fw = NULL,
+	.get_argument = cz_smum_get_argument,
+	.send_msg_to_smc = cz_send_msg_to_smc,
+	.send_msg_to_smc_with_parameter = cz_send_msg_to_smc_with_parameter,
+	.download_pptable_settings = cz_download_pptable_settings,
+	.upload_pptable_settings = cz_upload_pptable_settings,
+};
+
+int cz_smum_init(struct pp_smumgr *smumgr)
+{
+	struct cz_smumgr *cz_smu;
+
+	cz_smu = kzalloc(sizeof(struct cz_smumgr), GFP_KERNEL);
+	if (cz_smu == NULL)
+		return -ENOMEM;
+
+	smumgr->backend = cz_smu;
+	smumgr->smumgr_funcs = &cz_smu_funcs;
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.h b/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.h
new file mode 100644
index 000000000000..883818039248
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/cz_smumgr.h
@@ -0,0 +1,102 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _CZ_SMUMGR_H_
+#define _CZ_SMUMGR_H_
+
+
+#define MAX_NUM_FIRMWARE                        8
+#define MAX_NUM_SCRATCH                         11
+#define CZ_SCRATCH_SIZE_NONGFX_CLOCKGATING      1024
+#define CZ_SCRATCH_SIZE_NONGFX_GOLDENSETTING    2048
+#define CZ_SCRATCH_SIZE_SDMA_METADATA           1024
+#define CZ_SCRATCH_SIZE_IH                      ((2*256+1)*4)
+
+enum cz_scratch_entry {
+	CZ_SCRATCH_ENTRY_UCODE_ID_SDMA0 = 0,
+	CZ_SCRATCH_ENTRY_UCODE_ID_SDMA1,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_CE,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_PFP,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_ME,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT1,
+	CZ_SCRATCH_ENTRY_UCODE_ID_CP_MEC_JT2,
+	CZ_SCRATCH_ENTRY_UCODE_ID_GMCON_RENG,
+	CZ_SCRATCH_ENTRY_UCODE_ID_RLC_G,
+	CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SCRATCH,
+	CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_ARAM,
+	CZ_SCRATCH_ENTRY_UCODE_ID_RLC_SRM_DRAM,
+	CZ_SCRATCH_ENTRY_UCODE_ID_DMCU_ERAM,
+	CZ_SCRATCH_ENTRY_UCODE_ID_DMCU_IRAM,
+	CZ_SCRATCH_ENTRY_UCODE_ID_POWER_PROFILING,
+	CZ_SCRATCH_ENTRY_DATA_ID_SDMA_HALT,
+	CZ_SCRATCH_ENTRY_DATA_ID_SYS_CLOCKGATING,
+	CZ_SCRATCH_ENTRY_DATA_ID_SDMA_RING_REGS,
+	CZ_SCRATCH_ENTRY_DATA_ID_NONGFX_REINIT,
+	CZ_SCRATCH_ENTRY_DATA_ID_SDMA_START,
+	CZ_SCRATCH_ENTRY_DATA_ID_IH_REGISTERS,
+	CZ_SCRATCH_ENTRY_SMU8_FUSION_CLKTABLE
+};
+
+struct cz_buffer_entry {
+	uint32_t data_size;
+	uint32_t mc_addr_low;
+	uint32_t mc_addr_high;
+	void *kaddr;
+	enum cz_scratch_entry firmware_ID;
+	unsigned long handle; /* as bo handle used when release bo */
+};
+
+struct cz_register_index_data_pair {
+	uint32_t offset;
+	uint32_t value;
+};
+
+struct cz_ih_meta_data {
+	uint32_t command;
+	struct cz_register_index_data_pair register_index_value_pair[1];
+};
+
+struct cz_smumgr {
+	uint8_t driver_buffer_length;
+	uint8_t scratch_buffer_length;
+	uint16_t toc_entry_used_count;
+	uint16_t toc_entry_initialize_index;
+	uint16_t toc_entry_power_profiling_index;
+	uint16_t toc_entry_aram;
+	uint16_t toc_entry_ih_register_restore_task_index;
+	uint16_t toc_entry_clock_table;
+	uint16_t ih_register_restore_task_size;
+	uint16_t smu_buffer_used_bytes;
+
+	struct cz_buffer_entry toc_buffer;
+	struct cz_buffer_entry smu_buffer;
+	struct cz_buffer_entry firmware_buffer;
+	struct cz_buffer_entry driver_buffer[MAX_NUM_FIRMWARE];
+	struct cz_buffer_entry meta_data_buffer[MAX_NUM_FIRMWARE];
+	struct cz_buffer_entry scratch_buffer[MAX_NUM_SCRATCH];
+};
+
+struct pp_smumgr;
+
+extern int cz_smum_init(struct pp_smumgr *smumgr);
+
+#endif
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.c b/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.c
new file mode 100644
index 000000000000..cdbb9f89bf36
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.c
@@ -0,0 +1,1042 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#include "smumgr.h"
+#include "smu73.h"
+#include "smu_ucode_xfer_vi.h"
+#include "fiji_smumgr.h"
+#include "fiji_ppsmc.h"
+#include "smu73_discrete.h"
+#include "ppatomctrl.h"
+#include "smu/smu_7_1_3_d.h"
+#include "smu/smu_7_1_3_sh_mask.h"
+#include "gmc/gmc_8_1_d.h"
+#include "gmc/gmc_8_1_sh_mask.h"
+#include "oss/oss_3_0_d.h"
+#include "gca/gfx_8_0_d.h"
+#include "bif/bif_5_0_d.h"
+#include "bif/bif_5_0_sh_mask.h"
+#include "pp_debug.h"
+#include "fiji_pwrvirus.h"
+
+#define AVFS_EN_MSB                                        1568
+#define AVFS_EN_LSB                                        1568
+
+#define FIJI_SMC_SIZE 0x20000
+
+struct SMU73_Discrete_GraphicsLevel avfs_graphics_level[8] = {
+		/*  Min        Sclk       pcie     DeepSleep Activity  CgSpll      CgSpll    spllSpread  SpllSpread   CcPwr  CcPwr  Sclk   Display     Enabled     Enabled                       Voltage    Power */
+		/* Voltage,  Frequency,  DpmLevel,  DivId,    Level,  FuncCntl3,  FuncCntl4,  Spectrum,   Spectrum2,  DynRm, DynRm1  Did, Watermark, ForActivity, ForThrottle, UpHyst, DownHyst, DownHyst, Throttle */
+		{ 0x3c0fd047, 0x30750000,   0x00,     0x03,   0x1e00, 0x00200410, 0x87020000, 0x21680000, 0x0c000000,   0,      0,   0x16,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0xa00fd047, 0x409c0000,   0x01,     0x04,   0x1e00, 0x00800510, 0x87020000, 0x21680000, 0x11000000,   0,      0,   0x16,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0x0410d047, 0x50c30000,   0x01,     0x00,   0x1e00, 0x00600410, 0x87020000, 0x21680000, 0x0d000000,   0,      0,   0x0e,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0x6810d047, 0x60ea0000,   0x01,     0x00,   0x1e00, 0x00800410, 0x87020000, 0x21680000, 0x0e000000,   0,      0,   0x0c,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0xcc10d047, 0xe8fd0000,   0x01,     0x00,   0x1e00, 0x00e00410, 0x87020000, 0x21680000, 0x0f000000,   0,      0,   0x0c,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0x3011d047, 0x70110100,   0x01,     0x00,   0x1e00, 0x00400510, 0x87020000, 0x21680000, 0x10000000,   0,      0,   0x0c,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0x9411d047, 0xf8240100,   0x01,     0x00,   0x1e00, 0x00a00510, 0x87020000, 0x21680000, 0x11000000,   0,      0,   0x0c,   0x00,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 },
+		{ 0xf811d047, 0x80380100,   0x01,     0x00,   0x1e00, 0x00000610, 0x87020000, 0x21680000, 0x12000000,   0,      0,   0x0c,   0x01,       0x01,        0x01,      0x00,   0x00,      0x00,     0x00 }
+};
+
+static enum cgs_ucode_id fiji_convert_fw_type_to_cgs(uint32_t fw_type)
+{
+	enum cgs_ucode_id result = CGS_UCODE_ID_MAXIMUM;
+
+	switch (fw_type) {
+	case UCODE_ID_SMU:
+		result = CGS_UCODE_ID_SMU;
+		break;
+	case UCODE_ID_SDMA0:
+		result = CGS_UCODE_ID_SDMA0;
+		break;
+	case UCODE_ID_SDMA1:
+		result = CGS_UCODE_ID_SDMA1;
+		break;
+	case UCODE_ID_CP_CE:
+		result = CGS_UCODE_ID_CP_CE;
+		break;
+	case UCODE_ID_CP_PFP:
+		result = CGS_UCODE_ID_CP_PFP;
+		break;
+	case UCODE_ID_CP_ME:
+		result = CGS_UCODE_ID_CP_ME;
+		break;
+	case UCODE_ID_CP_MEC:
+		result = CGS_UCODE_ID_CP_MEC;
+		break;
+	case UCODE_ID_CP_MEC_JT1:
+		result = CGS_UCODE_ID_CP_MEC_JT1;
+		break;
+	case UCODE_ID_CP_MEC_JT2:
+		result = CGS_UCODE_ID_CP_MEC_JT2;
+		break;
+	case UCODE_ID_RLC_G:
+		result = CGS_UCODE_ID_RLC_G;
+		break;
+	default:
+		break;
+	}
+
+	return result;
+}
+/**
+* Set the address for reading/writing the SMC SRAM space.
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    smc_addr the address in the SMC RAM to access.
+*/
+static int fiji_set_smc_sram_address(struct pp_smumgr *smumgr,
+		uint32_t smc_addr, uint32_t limit)
+{
+	PP_ASSERT_WITH_CODE((0 == (3 & smc_addr)),
+			"SMC address must be 4 byte aligned.", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((limit > (smc_addr + 3)),
+			"SMC address is beyond the SMC RAM area.", return -EINVAL;);
+
+	cgs_write_register(smumgr->device, mmSMC_IND_INDEX_0, smc_addr);
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_0, 0);
+
+	return 0;
+}
+
+/**
+* Copy bytes from an array into the SMC RAM space.
+*
+* @param    smumgr  the address of the powerplay SMU manager.
+* @param    smcStartAddress the start address in the SMC RAM to copy bytes to.
+* @param    src the byte array to copy the bytes from.
+* @param    byteCount the number of bytes to copy.
+*/
+int fiji_copy_bytes_to_smc(struct pp_smumgr *smumgr,
+		uint32_t smcStartAddress, const uint8_t *src,
+		uint32_t byteCount, uint32_t limit)
+{
+	int result;
+	uint32_t data, originalData;
+	uint32_t addr, extraShift;
+
+	PP_ASSERT_WITH_CODE((0 == (3 & smcStartAddress)),
+			"SMC address must be 4 byte aligned.", return -EINVAL;);
+	PP_ASSERT_WITH_CODE((limit > (smcStartAddress + byteCount)),
+			"SMC address is beyond the SMC RAM area.", return -EINVAL;);
+
+	addr = smcStartAddress;
+
+	while (byteCount >= 4) {
+		/* Bytes are written into the SMC addres space with the MSB first. */
+		data = src[0] * 0x1000000 + src[1] * 0x10000 + src[2] * 0x100 + src[3];
+
+		result = fiji_set_smc_sram_address(smumgr, addr, limit);
+		if (result)
+			return result;
+
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data);
+
+		src += 4;
+		byteCount -= 4;
+		addr += 4;
+	}
+
+	if (byteCount) {
+		/* Now write the odd bytes left.
+		 * Do a read modify write cycle.
+		 */
+		data = 0;
+
+		result = fiji_set_smc_sram_address(smumgr, addr, limit);
+		if (result)
+			return result;
+
+		originalData = cgs_read_register(smumgr->device, mmSMC_IND_DATA_0);
+		extraShift = 8 * (4 - byteCount);
+
+		while (byteCount > 0) {
+			/* Bytes are written into the SMC addres
+			 * space with the MSB first.
+			 */
+			data = (0x100 * data) + *src++;
+			byteCount--;
+		}
+		data <<= extraShift;
+		data |= (originalData & ~((~0UL) << extraShift));
+
+		result = fiji_set_smc_sram_address(smumgr, addr, limit);
+		if (!result)
+			return result;
+
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data);
+	}
+	return 0;
+}
+
+int fiji_program_jump_on_start(struct pp_smumgr *smumgr)
+{
+	static unsigned char data[] = { 0xE0, 0x00, 0x80, 0x40 };
+
+	fiji_copy_bytes_to_smc(smumgr, 0x0, data, 4, sizeof(data) + 1);
+
+	return 0;
+}
+
+/**
+* Return if the SMC is currently running.
+*
+* @param    smumgr  the address of the powerplay hardware manager.
+*/
+bool fiji_is_smc_ram_running(struct pp_smumgr *smumgr)
+{
+	return ((0 == SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device,
+			CGS_IND_REG__SMC,
+			SMC_SYSCON_CLOCK_CNTL_0, ck_disable))
+			&& (0x20100 <= cgs_read_ind_register(smumgr->device,
+					CGS_IND_REG__SMC, ixSMC_PC_C)));
+}
+
+/**
+* Send a message to the SMC, and wait for its response.
+*
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    msg the message to send.
+* @return   The response that came from the SMC.
+*/
+int fiji_send_msg_to_smc(struct pp_smumgr *smumgr, uint16_t msg)
+{
+	if (!fiji_is_smc_ram_running(smumgr))
+		return -1;
+
+	if (1 != SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP)) {
+		printk(KERN_ERR "Failed to send Previous Message.");
+		SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	}
+
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, msg);
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	return 0;
+}
+
+/**
+ * Send a message to the SMC with parameter
+ * @param    smumgr:  the address of the powerplay hardware manager.
+ * @param    msg: the message to send.
+ * @param    parameter: the parameter to send
+ * @return   The response that came from the SMC.
+ */
+int fiji_send_msg_to_smc_with_parameter(struct pp_smumgr *smumgr,
+		uint16_t msg, uint32_t parameter)
+{
+	if (!fiji_is_smc_ram_running(smumgr))
+		return -1;
+
+	if (1 != SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP)) {
+		printk(KERN_ERR "Failed to send Previous Message.");
+		SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	}
+
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, parameter);
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, msg);
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	return 0;
+}
+
+
+/**
+* Send a message to the SMC with parameter, do not wait for response
+*
+* @param    smumgr:  the address of the powerplay hardware manager.
+* @param    msg: the message to send.
+* @param    parameter: the parameter to send
+* @return   The response that came from the SMC.
+*/
+int fiji_send_msg_to_smc_with_parameter_without_waiting(
+		struct pp_smumgr *smumgr, uint16_t msg, uint32_t parameter)
+{
+	if (1 != SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP)) {
+		printk(KERN_ERR "Failed to send Previous Message.");
+		SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	}
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, parameter);
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, msg);
+
+	return 0;
+}
+
+/**
+* Uploads the SMU firmware from .hex file
+*
+* @param    smumgr  the address of the powerplay SMU manager.
+* @return   0 or -1.
+*/
+
+static int fiji_upload_smu_firmware_image(struct pp_smumgr *smumgr)
+{
+	const uint8_t *src;
+	uint32_t byte_count;
+	uint32_t *data;
+	struct cgs_firmware_info info = {0};
+
+	cgs_get_firmware_info(smumgr->device,
+			fiji_convert_fw_type_to_cgs(UCODE_ID_SMU), &info);
+
+	if (info.image_size & 3) {
+		printk(KERN_ERR "SMC ucode is not 4 bytes aligned\n");
+		return -EINVAL;
+	}
+
+	if (info.image_size > FIJI_SMC_SIZE) {
+		printk(KERN_ERR "SMC address is beyond the SMC RAM area\n");
+		return -EINVAL;
+	}
+
+	cgs_write_register(smumgr->device, mmSMC_IND_INDEX_0, 0x20000);
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_0, 1);
+
+	byte_count = info.image_size;
+	src = (const uint8_t *)info.kptr;
+
+	data = (uint32_t *)src;
+	for (; byte_count >= 4; data++, byte_count -= 4)
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data[0]);
+
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_0, 0);
+	return 0;
+}
+
+/**
+* Read a 32bit value from the SMC SRAM space.
+* ALL PARAMETERS ARE IN HOST BYTE ORDER.
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    smc_addr the address in the SMC RAM to access.
+* @param    value and output parameter for the data read from the SMC SRAM.
+*/
+int fiji_read_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smc_addr,
+		uint32_t *value, uint32_t limit)
+{
+	int	result = fiji_set_smc_sram_address(smumgr, smc_addr, limit);
+
+	if (result)
+		return result;
+
+	*value = cgs_read_register(smumgr->device, mmSMC_IND_DATA_0);
+	return 0;
+}
+
+/**
+* Write a 32bit value to the SMC SRAM space.
+* ALL PARAMETERS ARE IN HOST BYTE ORDER.
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    smc_addr the address in the SMC RAM to access.
+* @param    value to write to the SMC SRAM.
+*/
+int fiji_write_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smc_addr,
+		uint32_t value, uint32_t limit)
+{
+	int result;
+
+	result = fiji_set_smc_sram_address(smumgr, smc_addr, limit);
+
+	if (result)
+		return result;
+
+	cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, value);
+	return 0;
+}
+
+static uint32_t fiji_get_mask_for_firmware_type(uint32_t fw_type)
+{
+	uint32_t result = 0;
+
+	switch (fw_type) {
+	case UCODE_ID_SDMA0:
+		result = UCODE_ID_SDMA0_MASK;
+		break;
+	case UCODE_ID_SDMA1:
+		result = UCODE_ID_SDMA1_MASK;
+		break;
+	case UCODE_ID_CP_CE:
+		result = UCODE_ID_CP_CE_MASK;
+		break;
+	case UCODE_ID_CP_PFP:
+		result = UCODE_ID_CP_PFP_MASK;
+		break;
+	case UCODE_ID_CP_ME:
+		result = UCODE_ID_CP_ME_MASK;
+		break;
+	case UCODE_ID_CP_MEC_JT1:
+		result = UCODE_ID_CP_MEC_MASK | UCODE_ID_CP_MEC_JT1_MASK;
+		break;
+	case UCODE_ID_CP_MEC_JT2:
+		result = UCODE_ID_CP_MEC_MASK | UCODE_ID_CP_MEC_JT2_MASK;
+		break;
+	case UCODE_ID_RLC_G:
+		result = UCODE_ID_RLC_G_MASK;
+		break;
+	default:
+		printk(KERN_ERR "UCode type is out of range!");
+		result = 0;
+	}
+
+	return result;
+}
+
+/* Populate one firmware image to the data structure */
+static int fiji_populate_single_firmware_entry(struct pp_smumgr *smumgr,
+		uint32_t fw_type, struct SMU_Entry *entry)
+{
+	int result;
+	struct cgs_firmware_info info = {0};
+
+	result = cgs_get_firmware_info(
+			smumgr->device,
+			fiji_convert_fw_type_to_cgs(fw_type),
+			&info);
+
+	if (!result) {
+		entry->version = 0;
+		entry->id = (uint16_t)fw_type;
+		entry->image_addr_high = smu_upper_32_bits(info.mc_addr);
+		entry->image_addr_low = smu_lower_32_bits(info.mc_addr);
+		entry->meta_data_addr_high = 0;
+		entry->meta_data_addr_low = 0;
+		entry->data_size_byte = info.image_size;
+		entry->num_register_entries = 0;
+
+		if (fw_type == UCODE_ID_RLC_G)
+			entry->flags = 1;
+		else
+			entry->flags = 0;
+	}
+
+	return result;
+}
+
+static int fiji_request_smu_load_fw(struct pp_smumgr *smumgr)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+	uint32_t fw_to_load;
+	struct SMU_DRAMData_TOC *toc;
+
+	if (priv->soft_regs_start)
+		cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+				priv->soft_regs_start +
+				offsetof(SMU73_SoftRegisters, UcodeLoadStatus),
+				0x0);
+
+	toc = (struct SMU_DRAMData_TOC *)priv->header;
+	toc->num_entries = 0;
+	toc->structure_version = 1;
+
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_RLC_G, &toc->entry[toc->num_entries++]),
+			"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_CE, &toc->entry[toc->num_entries++]),
+			"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_PFP, &toc->entry[toc->num_entries++]),
+			"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_ME, &toc->entry[toc->num_entries++]),
+			"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_MEC, &toc->entry[toc->num_entries++]),
+			"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_MEC_JT1, &toc->entry[toc->num_entries++]),
+					"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_CP_MEC_JT2, &toc->entry[toc->num_entries++]),
+					"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_SDMA0, &toc->entry[toc->num_entries++]),
+					"Failed to Get Firmware Entry.\n" , return -1 );
+	PP_ASSERT_WITH_CODE(
+			0 == fiji_populate_single_firmware_entry(smumgr,
+					UCODE_ID_SDMA1, &toc->entry[toc->num_entries++]),
+					"Failed to Get Firmware Entry.\n" , return -1 );
+
+	fiji_send_msg_to_smc_with_parameter(smumgr, PPSMC_MSG_DRV_DRAM_ADDR_HI,
+			priv->header_buffer.mc_addr_high);
+	fiji_send_msg_to_smc_with_parameter(smumgr,PPSMC_MSG_DRV_DRAM_ADDR_LO,
+			priv->header_buffer.mc_addr_low);
+
+	fw_to_load = UCODE_ID_RLC_G_MASK
+			+ UCODE_ID_SDMA0_MASK
+			+ UCODE_ID_SDMA1_MASK
+			+ UCODE_ID_CP_CE_MASK
+			+ UCODE_ID_CP_ME_MASK
+			+ UCODE_ID_CP_PFP_MASK
+			+ UCODE_ID_CP_MEC_MASK
+			+ UCODE_ID_CP_MEC_JT1_MASK
+			+ UCODE_ID_CP_MEC_JT2_MASK;
+
+	if (fiji_send_msg_to_smc_with_parameter(smumgr,
+			PPSMC_MSG_LoadUcodes, fw_to_load))
+		printk(KERN_ERR "Fail to Request SMU Load uCode");
+
+	return 0;
+}
+
+
+/* Check if the FW has been loaded, SMU will not return
+ * if loading has not finished.
+ */
+static int fiji_check_fw_load_finish(struct pp_smumgr *smumgr,
+		uint32_t fw_type)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+	uint32_t mask = fiji_get_mask_for_firmware_type(fw_type);
+
+	/* Check SOFT_REGISTERS_TABLE_28.UcodeLoadStatus */
+	if (smum_wait_on_indirect_register(smumgr, mmSMC_IND_INDEX,
+			priv->soft_regs_start +
+			offsetof(SMU73_SoftRegisters, UcodeLoadStatus),
+			mask, mask)) {
+		printk(KERN_ERR "check firmware loading failed\n");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+
+static int fiji_reload_firmware(struct pp_smumgr *smumgr)
+{
+	return smumgr->smumgr_funcs->start_smu(smumgr);
+}
+
+static bool fiji_is_hw_virtualization_enabled(struct pp_smumgr *smumgr)
+{
+	uint32_t value;
+
+	value = cgs_read_register(smumgr->device, mmBIF_IOV_FUNC_IDENTIFIER);
+	if (value & BIF_IOV_FUNC_IDENTIFIER__IOV_ENABLE_MASK) {
+		/* driver reads on SR-IOV enabled PF: 0x80000000
+		 * driver reads on SR-IOV enabled VF: 0x80000001
+		 * driver reads on SR-IOV disabled:   0x00000000
+		 */
+		return true;
+	}
+	return false;
+}
+
+static int fiji_request_smu_specific_fw_load(struct pp_smumgr *smumgr, uint32_t fw_type)
+{
+	if (fiji_is_hw_virtualization_enabled(smumgr)) {
+		uint32_t masks = fiji_get_mask_for_firmware_type(fw_type);
+		if (fiji_send_msg_to_smc_with_parameter_without_waiting(smumgr,
+				PPSMC_MSG_LoadUcodes, masks))
+			printk(KERN_ERR "Fail to Request SMU Load uCode");
+	}
+	/* For non-virtualization cases,
+	 * SMU loads all FWs at once in fiji_request_smu_load_fw.
+	 */
+	return 0;
+}
+
+static int fiji_start_smu_in_protection_mode(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+
+	/* Wait for smc boot up */
+	/* SMUM_WAIT_INDIRECT_FIELD_UNEQUAL(smumgr, SMC_IND,
+		RCU_UC_EVENTS, boot_seq_done, 0); */
+
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_RESET_CNTL, rst_reg, 1);
+
+	result = fiji_upload_smu_firmware_image(smumgr);
+	if (result)
+		return result;
+
+	/* Clear status */
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+			ixSMU_STATUS, 0);
+
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_CLOCK_CNTL_0, ck_disable, 0);
+
+	/* De-assert reset */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_RESET_CNTL, rst_reg, 0);
+
+	/* Wait for ROM firmware to initialize interrupt hendler */
+	/*SMUM_WAIT_VFPF_INDIRECT_REGISTER(smumgr, SMC_IND,
+			SMC_INTR_CNTL_MASK_0, 0x10040, 0xFFFFFFFF); */
+
+	/* Set SMU Auto Start */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMU_INPUT_DATA, AUTO_START, 1);
+
+	/* Clear firmware interrupt enable flag */
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+			ixFIRMWARE_FLAGS, 0);
+
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND, RCU_UC_EVENTS,
+			INTERRUPTS_ENABLED, 1);
+
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, 0x20000);
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, PPSMC_MSG_Test);
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	/* Wait for done bit to be set */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(smumgr, SMC_IND,
+			SMU_STATUS, SMU_DONE, 0);
+
+	/* Check pass/failed indicator */
+	if (1 != SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMU_STATUS, SMU_PASS)) {
+		PP_ASSERT_WITH_CODE(false,
+				"SMU Firmware start failed!", return -1);
+	}
+
+	/* Wait for firmware to initialize */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND,
+			FIRMWARE_FLAGS, INTERRUPTS_ENABLED, 1);
+
+	return result;
+}
+
+static int fiji_start_smu_in_non_protection_mode(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+
+	/* wait for smc boot up */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(smumgr, SMC_IND,
+			RCU_UC_EVENTS, boot_seq_done, 0);
+
+	/* Clear firmware interrupt enable flag */
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+			ixFIRMWARE_FLAGS, 0);
+
+	/* Assert reset */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_RESET_CNTL, rst_reg, 1);
+
+	result = fiji_upload_smu_firmware_image(smumgr);
+	if (result)
+		return result;
+
+	/* Set smc instruct start point at 0x0 */
+	fiji_program_jump_on_start(smumgr);
+
+	/* Enable clock */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_CLOCK_CNTL_0, ck_disable, 0);
+
+	/* De-assert reset */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+			SMC_SYSCON_RESET_CNTL, rst_reg, 0);
+
+	/* Wait for firmware to initialize */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND,
+			FIRMWARE_FLAGS, INTERRUPTS_ENABLED, 1);
+
+	return result;
+}
+
+int fiji_setup_pwr_virus(struct pp_smumgr *smumgr)
+{
+	int i, result = -1;
+	uint32_t reg, data;
+	PWR_Command_Table *virus = PwrVirusTable;
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	priv->avfs.AvfsBtcStatus = AVFS_LOAD_VIRUS;
+	for (i = 0; (i < PWR_VIRUS_TABLE_SIZE); i++) {
+		switch (virus->command) {
+		case PwrCmdWrite:
+			reg  = virus->reg;
+			data = virus->data;
+			cgs_write_register(smumgr->device, reg, data);
+			break;
+		case PwrCmdEnd:
+			priv->avfs.AvfsBtcStatus = AVFS_BTC_VIRUS_LOADED;
+			result = 0;
+			break;
+		default:
+			printk(KERN_ERR "Table Exit with Invalid Command!");
+			priv->avfs.AvfsBtcStatus = AVFS_BTC_VIRUS_FAIL;
+			result = -1;
+			break;
+		}
+		virus++;
+	}
+	return result;
+}
+
+static int fiji_start_avfs_btc(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	priv->avfs.AvfsBtcStatus = AVFS_BTC_STARTED;
+	if (priv->avfs.AvfsBtcParam) {
+		if (!fiji_send_msg_to_smc_with_parameter(smumgr,
+				PPSMC_MSG_PerformBtc, priv->avfs.AvfsBtcParam)) {
+			if (!fiji_send_msg_to_smc(smumgr, PPSMC_MSG_EnableAvfs)) {
+				priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_UNSAVED;
+				result = 0;
+			} else {
+				printk(KERN_ERR "[AVFS][fiji_start_avfs_btc] Attempt"
+						" to Enable AVFS Failed!");
+				fiji_send_msg_to_smc(smumgr, PPSMC_MSG_DisableAvfs);
+				result = -1;
+			}
+		} else {
+			printk(KERN_ERR "[AVFS][fiji_start_avfs_btc] "
+					"PerformBTC SMU msg failed");
+			result = -1;
+		}
+	}
+	/* Soft-Reset to reset the engine before loading uCode */
+	 /* halt */
+	cgs_write_register(smumgr->device, mmCP_MEC_CNTL, 0x50000000);
+	/* reset everything */
+	cgs_write_register(smumgr->device, mmGRBM_SOFT_RESET, 0xffffffff);
+	/* clear reset */
+	cgs_write_register(smumgr->device, mmGRBM_SOFT_RESET, 0);
+
+	return result;
+}
+
+int fiji_setup_pm_fuse_for_avfs(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+	uint32_t table_start;
+	uint32_t charz_freq_addr, inversion_voltage_addr, charz_freq;
+	uint16_t inversion_voltage;
+
+	charz_freq = 0x30750000; /* In 10KHz units 0x00007530 Actual value */
+	inversion_voltage = 0x1A04; /* mV Q14.2 0x41A Actual value */
+
+	PP_ASSERT_WITH_CODE(0 == fiji_read_smc_sram_dword(smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION + offsetof(SMU73_Firmware_Header,
+					PmFuseTable), &table_start, 0x40000),
+			"[AVFS][Fiji_SetupGfxLvlStruct] SMU could not communicate "
+			"starting address of PmFuse structure",
+			return -1;);
+
+	charz_freq_addr = table_start +
+			offsetof(struct SMU73_Discrete_PmFuses, PsmCharzFreq);
+	inversion_voltage_addr = table_start +
+			offsetof(struct SMU73_Discrete_PmFuses, InversionVoltage);
+
+	result = fiji_copy_bytes_to_smc(smumgr, charz_freq_addr,
+			(uint8_t *)(&charz_freq), sizeof(charz_freq), 0x40000);
+	PP_ASSERT_WITH_CODE(0 == result,
+			"[AVFS][fiji_setup_pm_fuse_for_avfs] charz_freq could not "
+			"be populated.", return -1;);
+
+	result = fiji_copy_bytes_to_smc(smumgr, inversion_voltage_addr,
+			(uint8_t *)(&inversion_voltage), sizeof(inversion_voltage), 0x40000);
+	PP_ASSERT_WITH_CODE(0 == result, "[AVFS][fiji_setup_pm_fuse_for_avfs] "
+			"charz_freq could not be populated.", return -1;);
+
+	return result;
+}
+
+int fiji_setup_graphics_level_structure(struct pp_smumgr *smumgr)
+{
+	int32_t vr_config;
+	uint32_t table_start;
+	uint32_t level_addr, vr_config_addr;
+	uint32_t level_size = sizeof(avfs_graphics_level);
+
+	PP_ASSERT_WITH_CODE(0 == fiji_read_smc_sram_dword(smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, DpmTable),
+			&table_start, 0x40000),
+			"[AVFS][Fiji_SetupGfxLvlStruct] SMU could not "
+			"communicate starting address of DPM table",
+			return -1;);
+
+	/* Default value for vr_config =
+	 * VR_MERGED_WITH_VDDC + VR_STATIC_VOLTAGE(VDDCI) */
+	vr_config = 0x01000500;   /* Real value:0x50001 */
+
+	vr_config_addr = table_start +
+			offsetof(SMU73_Discrete_DpmTable, VRConfig);
+
+	PP_ASSERT_WITH_CODE(0 == fiji_copy_bytes_to_smc(smumgr, vr_config_addr,
+			(uint8_t *)&vr_config, sizeof(int32_t), 0x40000),
+			"[AVFS][Fiji_SetupGfxLvlStruct] Problems copying "
+			"vr_config value over to SMC",
+			return -1;);
+
+	level_addr = table_start + offsetof(SMU73_Discrete_DpmTable, GraphicsLevel);
+
+	PP_ASSERT_WITH_CODE(0 == fiji_copy_bytes_to_smc(smumgr, level_addr,
+			(uint8_t *)(&avfs_graphics_level), level_size, 0x40000),
+			"[AVFS][Fiji_SetupGfxLvlStruct] Copying of DPM table failed!",
+			return -1;);
+
+	return 0;
+}
+
+/* Work in Progress */
+int fiji_restore_vft_table(struct pp_smumgr *smumgr)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	if (AVFS_BTC_COMPLETED_SAVED == priv->avfs.AvfsBtcStatus) {
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_RESTORED;
+		return 0;
+	} else
+		return -EINVAL;
+}
+
+/* Work in Progress */
+int fiji_save_vft_table(struct pp_smumgr *smumgr)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	if (AVFS_BTC_COMPLETED_SAVED == priv->avfs.AvfsBtcStatus) {
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_RESTORED;
+		return 0;
+	} else
+		return -EINVAL;
+}
+
+int fiji_avfs_event_mgr(struct pp_smumgr *smumgr, bool smu_started)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	switch (priv->avfs.AvfsBtcStatus) {
+	case AVFS_BTC_COMPLETED_SAVED: /*S3 State - Pre SMU Start */
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_RESTOREVFT_FAILED;
+		PP_ASSERT_WITH_CODE(0 == fiji_restore_vft_table(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Could not Copy Graphics "
+				"Level table over to SMU",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_RESTORED;
+		break;
+	case AVFS_BTC_COMPLETED_RESTORED: /*S3 State - Post SMU Start*/
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_SMUMSG_ERROR;
+		PP_ASSERT_WITH_CODE(0 == fiji_send_msg_to_smc(smumgr,
+				PPSMC_MSG_VftTableIsValid),
+				"[AVFS][fiji_avfs_event_mgr] SMU did not respond "
+				"correctly to VftTableIsValid Msg",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_SMUMSG_ERROR;
+		PP_ASSERT_WITH_CODE(0 == fiji_send_msg_to_smc(smumgr,
+				PPSMC_MSG_EnableAvfs),
+				"[AVFS][fiji_avfs_event_mgr] SMU did not respond "
+				"correctly to EnableAvfs Message Msg",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_SAVED;
+		break;
+	case AVFS_BTC_BOOT: /*Cold Boot State - Post SMU Start*/
+		if (!smu_started)
+			break;
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_FAILED;
+		PP_ASSERT_WITH_CODE(0 == fiji_setup_pm_fuse_for_avfs(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Failure at "
+				"fiji_setup_pm_fuse_for_avfs",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_DPMTABLESETUP_FAILED;
+		PP_ASSERT_WITH_CODE(0 == fiji_setup_graphics_level_structure(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Could not Copy Graphics Level"
+				" table over to SMU",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_VIRUS_FAIL;
+		PP_ASSERT_WITH_CODE(0 == fiji_setup_pwr_virus(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Could not setup "
+				"Pwr Virus for AVFS ",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_FAILED;
+		PP_ASSERT_WITH_CODE(0 == fiji_start_avfs_btc(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Failure at "
+				"fiji_start_avfs_btc. AVFS Disabled",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_SAVEVFT_FAILED;
+		PP_ASSERT_WITH_CODE(0 == fiji_save_vft_table(smumgr),
+				"[AVFS][fiji_avfs_event_mgr] Could not save VFT Table",
+				return -1;);
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_COMPLETED_SAVED;
+		break;
+	case AVFS_BTC_DISABLED: /* Do nothing */
+		break;
+	case AVFS_BTC_NOTSUPPORTED: /* Do nothing */
+		break;
+	default:
+		printk(KERN_ERR "[AVFS] Something is broken. See log!");
+		break;
+	}
+	return 0;
+}
+
+static int fiji_start_smu(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+
+	/* Only start SMC if SMC RAM is not running */
+	if (!fiji_is_smc_ram_running(smumgr)) {
+		fiji_avfs_event_mgr(smumgr, false);
+
+		/* Check if SMU is running in protected mode */
+		if (0 == SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device,
+				CGS_IND_REG__SMC,
+				SMU_FIRMWARE, SMU_MODE)) {
+			result = fiji_start_smu_in_non_protection_mode(smumgr);
+			if (result)
+				return result;
+		} else {
+			result = fiji_start_smu_in_protection_mode(smumgr);
+			if (result)
+				return result;
+		}
+		fiji_avfs_event_mgr(smumgr, true);
+	}
+
+	/* To initialize all clock gating before RLC loaded and running.*/
+	cgs_set_clockgating_state(smumgr->device,
+			AMD_IP_BLOCK_TYPE_GFX, AMD_CG_STATE_GATE);
+	cgs_set_clockgating_state(smumgr->device,
+			AMD_IP_BLOCK_TYPE_GMC, AMD_CG_STATE_GATE);
+	cgs_set_clockgating_state(smumgr->device,
+			AMD_IP_BLOCK_TYPE_SDMA, AMD_CG_STATE_GATE);
+	cgs_set_clockgating_state(smumgr->device,
+			AMD_IP_BLOCK_TYPE_COMMON, AMD_CG_STATE_GATE);
+
+	/* Setup SoftRegsStart here for register lookup in case
+	 * DummyBackEnd is used and ProcessFirmwareHeader is not executed
+	 */
+	fiji_read_smc_sram_dword(smumgr,
+			SMU7_FIRMWARE_HEADER_LOCATION +
+			offsetof(SMU73_Firmware_Header, SoftRegisters),
+			&(priv->soft_regs_start), 0x40000);
+
+	result = fiji_request_smu_load_fw(smumgr);
+
+	return result;
+}
+
+static bool fiji_is_hw_avfs_present(struct pp_smumgr *smumgr)
+{
+
+	uint32_t efuse = 0;
+	uint32_t mask = (1 << ((AVFS_EN_MSB - AVFS_EN_LSB) + 1)) - 1;
+
+	if (!atomctrl_read_efuse(smumgr->device, AVFS_EN_LSB, AVFS_EN_MSB,
+			mask, &efuse)) {
+		if (efuse)
+			return true;
+	}
+	return false;
+}
+
+/**
+* Write a 32bit value to the SMC SRAM space.
+* ALL PARAMETERS ARE IN HOST BYTE ORDER.
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    smc_addr the address in the SMC RAM to access.
+* @param    value to write to the SMC SRAM.
+*/
+static int fiji_smu_init(struct pp_smumgr *smumgr)
+{
+	struct fiji_smumgr *priv = (struct fiji_smumgr *)(smumgr->backend);
+	uint64_t mc_addr;
+
+	priv->header_buffer.data_size =
+			((sizeof(struct SMU_DRAMData_TOC) / 4096) + 1) * 4096;
+	smu_allocate_memory(smumgr->device,
+			priv->header_buffer.data_size,
+			CGS_GPU_MEM_TYPE__VISIBLE_CONTIG_FB,
+			PAGE_SIZE,
+			&mc_addr,
+			&priv->header_buffer.kaddr,
+			&priv->header_buffer.handle);
+
+	priv->header = priv->header_buffer.kaddr;
+	priv->header_buffer.mc_addr_high = smu_upper_32_bits(mc_addr);
+	priv->header_buffer.mc_addr_low = smu_lower_32_bits(mc_addr);
+
+	PP_ASSERT_WITH_CODE((NULL != priv->header),
+			"Out of memory.",
+			kfree(smumgr->backend);
+			cgs_free_gpu_mem(smumgr->device,
+			(cgs_handle_t)priv->header_buffer.handle);
+			return -1);
+
+	priv->avfs.AvfsBtcStatus = AVFS_BTC_BOOT;
+	if (fiji_is_hw_avfs_present(smumgr))
+		/* AVFS Parameter
+		 * 0 - BTC DC disabled, BTC AC disabled
+		 * 1 - BTC DC enabled,  BTC AC disabled
+		 * 2 - BTC DC disabled, BTC AC enabled
+		 * 3 - BTC DC enabled,  BTC AC enabled
+		 * Default is 0 - BTC DC disabled, BTC AC disabled
+		 */
+		priv->avfs.AvfsBtcParam = 0;
+	else
+		priv->avfs.AvfsBtcStatus = AVFS_BTC_NOTSUPPORTED;
+
+	priv->acpi_optimization = 1;
+
+	return 0;
+}
+
+static int fiji_smu_fini(struct pp_smumgr *smumgr)
+{
+	if (smumgr->backend) {
+		kfree(smumgr->backend);
+		smumgr->backend = NULL;
+	}
+	return 0;
+}
+
+static const struct pp_smumgr_func fiji_smu_funcs = {
+	.smu_init = &fiji_smu_init,
+	.smu_fini = &fiji_smu_fini,
+	.start_smu = &fiji_start_smu,
+	.check_fw_load_finish = &fiji_check_fw_load_finish,
+	.request_smu_load_fw = &fiji_reload_firmware,
+	.request_smu_load_specific_fw = &fiji_request_smu_specific_fw_load,
+	.send_msg_to_smc = &fiji_send_msg_to_smc,
+	.send_msg_to_smc_with_parameter = &fiji_send_msg_to_smc_with_parameter,
+	.download_pptable_settings = NULL,
+	.upload_pptable_settings = NULL,
+};
+
+int fiji_smum_init(struct pp_smumgr *smumgr)
+{
+	struct fiji_smumgr *fiji_smu = NULL;
+
+	fiji_smu = kzalloc(sizeof(struct fiji_smumgr), GFP_KERNEL);
+
+	if (fiji_smu == NULL)
+		return -ENOMEM;
+
+	smumgr->backend = fiji_smu;
+	smumgr->smumgr_funcs = &fiji_smu_funcs;
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.h b/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.h
new file mode 100644
index 000000000000..8cd22d9c9140
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/fiji_smumgr.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#ifndef _FIJI_SMUMANAGER_H_
+#define _FIJI_SMUMANAGER_H_
+
+enum AVFS_BTC_STATUS {
+	AVFS_BTC_BOOT = 0,
+	AVFS_BTC_BOOT_STARTEDSMU,
+	AVFS_LOAD_VIRUS,
+	AVFS_BTC_VIRUS_LOADED,
+	AVFS_BTC_VIRUS_FAIL,
+	AVFS_BTC_STARTED,
+	AVFS_BTC_FAILED,
+	AVFS_BTC_RESTOREVFT_FAILED,
+	AVFS_BTC_SAVEVFT_FAILED,
+	AVFS_BTC_DPMTABLESETUP_FAILED,
+	AVFS_BTC_COMPLETED_UNSAVED,
+	AVFS_BTC_COMPLETED_SAVED,
+	AVFS_BTC_COMPLETED_RESTORED,
+	AVFS_BTC_DISABLED,
+	AVFS_BTC_NOTSUPPORTED,
+	AVFS_BTC_SMUMSG_ERROR
+};
+
+struct fiji_smu_avfs {
+	enum AVFS_BTC_STATUS AvfsBtcStatus;
+	uint32_t           AvfsBtcParam;
+};
+
+struct fiji_buffer_entry {
+	uint32_t data_size;
+	uint32_t mc_addr_low;
+	uint32_t mc_addr_high;
+	void *kaddr;
+	unsigned long  handle;
+};
+
+struct fiji_smumgr {
+	uint8_t        *header;
+	uint8_t        *mec_image;
+	uint32_t        soft_regs_start;
+	struct fiji_smu_avfs avfs;
+	uint32_t        acpi_optimization;
+
+	struct fiji_buffer_entry header_buffer;
+};
+
+int fiji_smum_init(struct pp_smumgr *smumgr);
+int fiji_read_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smcAddress,
+		uint32_t *value, uint32_t limit);
+int fiji_write_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smc_addr,
+		uint32_t value, uint32_t limit);
+int fiji_copy_bytes_to_smc(struct pp_smumgr *smumgr, uint32_t smcStartAddress,
+		const uint8_t *src,	uint32_t byteCount, uint32_t limit);
+
+#endif
+
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/smumgr.c b/drivers/gpu/drm/amd/powerplay/smumgr/smumgr.c
new file mode 100644
index 000000000000..063ae71c9830
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/smumgr.c
@@ -0,0 +1,263 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include "pp_instance.h"
+#include "smumgr.h"
+#include "cgs_common.h"
+#include "linux/delay.h"
+#include "cz_smumgr.h"
+#include "tonga_smumgr.h"
+#include "fiji_smumgr.h"
+
+int smum_init(struct amd_pp_init *pp_init, struct pp_instance *handle)
+{
+	struct pp_smumgr *smumgr;
+
+	if ((handle == NULL) || (pp_init == NULL))
+		return -EINVAL;
+
+	smumgr = kzalloc(sizeof(struct pp_smumgr), GFP_KERNEL);
+	if (smumgr == NULL)
+		return -ENOMEM;
+
+	smumgr->device = pp_init->device;
+	smumgr->chip_family = pp_init->chip_family;
+	smumgr->chip_id = pp_init->chip_id;
+	smumgr->hw_revision = pp_init->rev_id;
+	smumgr->usec_timeout = AMD_MAX_USEC_TIMEOUT;
+	smumgr->reload_fw = 1;
+	handle->smu_mgr = smumgr;
+
+	switch (smumgr->chip_family) {
+	case AMD_FAMILY_CZ:
+		cz_smum_init(smumgr);
+		break;
+	case AMD_FAMILY_VI:
+		switch (smumgr->chip_id) {
+		case CHIP_TONGA:
+			tonga_smum_init(smumgr);
+			break;
+		case CHIP_FIJI:
+			fiji_smum_init(smumgr);
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	default:
+		kfree(smumgr);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int smum_fini(struct pp_smumgr *smumgr)
+{
+	kfree(smumgr);
+	return 0;
+}
+
+int smum_get_argument(struct pp_smumgr *smumgr)
+{
+	if (NULL != smumgr->smumgr_funcs->get_argument)
+		return smumgr->smumgr_funcs->get_argument(smumgr);
+
+	return 0;
+}
+
+int smum_download_powerplay_table(struct pp_smumgr *smumgr,
+								void **table)
+{
+	if (NULL != smumgr->smumgr_funcs->download_pptable_settings)
+		return smumgr->smumgr_funcs->download_pptable_settings(smumgr,
+									table);
+
+	return 0;
+}
+
+int smum_upload_powerplay_table(struct pp_smumgr *smumgr)
+{
+	if (NULL != smumgr->smumgr_funcs->upload_pptable_settings)
+		return smumgr->smumgr_funcs->upload_pptable_settings(smumgr);
+
+	return 0;
+}
+
+int smum_send_msg_to_smc(struct pp_smumgr *smumgr, uint16_t msg)
+{
+	if (smumgr == NULL || smumgr->smumgr_funcs->send_msg_to_smc == NULL)
+		return -EINVAL;
+
+	return smumgr->smumgr_funcs->send_msg_to_smc(smumgr, msg);
+}
+
+int smum_send_msg_to_smc_with_parameter(struct pp_smumgr *smumgr,
+					uint16_t msg, uint32_t parameter)
+{
+	if (smumgr == NULL ||
+		smumgr->smumgr_funcs->send_msg_to_smc_with_parameter == NULL)
+		return -EINVAL;
+	return smumgr->smumgr_funcs->send_msg_to_smc_with_parameter(
+						smumgr, msg, parameter);
+}
+
+/*
+ * Returns once the part of the register indicated by the mask has
+ * reached the given value.
+ */
+int smum_wait_on_register(struct pp_smumgr *smumgr,
+				uint32_t index,
+				uint32_t value, uint32_t mask)
+{
+	uint32_t i;
+	uint32_t cur_value;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < smumgr->usec_timeout; i++) {
+		cur_value = cgs_read_register(smumgr->device, index);
+		if ((cur_value & mask) == (value & mask))
+			break;
+		udelay(1);
+	}
+
+	/* timeout means wrong logic*/
+	if (i == smumgr->usec_timeout)
+		return -1;
+
+	return 0;
+}
+
+int smum_wait_for_register_unequal(struct pp_smumgr *smumgr,
+					uint32_t index,
+					uint32_t value, uint32_t mask)
+{
+	uint32_t i;
+	uint32_t cur_value;
+
+	if (smumgr == NULL)
+		return -EINVAL;
+
+	for (i = 0; i < smumgr->usec_timeout; i++) {
+		cur_value = cgs_read_register(smumgr->device,
+									index);
+		if ((cur_value & mask) != (value & mask))
+			break;
+		udelay(1);
+	}
+
+	/* timeout means wrong logic */
+	if (i == smumgr->usec_timeout)
+		return -1;
+
+	return 0;
+}
+
+
+/*
+ * Returns once the part of the register indicated by the mask
+ * has reached the given value.The indirect space is described by
+ * giving the memory-mapped index of the indirect index register.
+ */
+int smum_wait_on_indirect_register(struct pp_smumgr *smumgr,
+					uint32_t indirect_port,
+					uint32_t index,
+					uint32_t value,
+					uint32_t mask)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	cgs_write_register(smumgr->device, indirect_port, index);
+	return smum_wait_on_register(smumgr, indirect_port + 1,
+						mask, value);
+}
+
+void smum_wait_for_indirect_register_unequal(
+						struct pp_smumgr *smumgr,
+						uint32_t indirect_port,
+						uint32_t index,
+						uint32_t value,
+						uint32_t mask)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return;
+	cgs_write_register(smumgr->device, indirect_port, index);
+	smum_wait_for_register_unequal(smumgr, indirect_port + 1,
+						value, mask);
+}
+
+int smu_allocate_memory(void *device, uint32_t size,
+			 enum cgs_gpu_mem_type type,
+			 uint32_t byte_align, uint64_t *mc_addr,
+			 void **kptr, void *handle)
+{
+	int ret = 0;
+	cgs_handle_t cgs_handle;
+
+	if (device == NULL || handle == NULL ||
+	    mc_addr == NULL || kptr == NULL)
+		return -EINVAL;
+
+	ret = cgs_alloc_gpu_mem(device, type, size, byte_align,
+				0, 0, (cgs_handle_t *)handle);
+	if (ret)
+		return -ENOMEM;
+
+	cgs_handle = *(cgs_handle_t *)handle;
+
+	ret = cgs_gmap_gpu_mem(device, cgs_handle, mc_addr);
+	if (ret)
+		goto error_gmap;
+
+	ret = cgs_kmap_gpu_mem(device, cgs_handle, kptr);
+	if (ret)
+		goto error_kmap;
+
+	return 0;
+
+error_kmap:
+	cgs_gunmap_gpu_mem(device, cgs_handle);
+
+error_gmap:
+	cgs_free_gpu_mem(device, cgs_handle);
+	return ret;
+}
+
+int smu_free_memory(void *device, void *handle)
+{
+	cgs_handle_t cgs_handle = (cgs_handle_t)handle;
+
+	if (device == NULL || handle == NULL)
+		return -EINVAL;
+
+	cgs_kunmap_gpu_mem(device, cgs_handle);
+	cgs_gunmap_gpu_mem(device, cgs_handle);
+	cgs_free_gpu_mem(device, cgs_handle);
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.c b/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.c
new file mode 100644
index 000000000000..ebdb43a8daef
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.c
@@ -0,0 +1,819 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/gfp.h>
+
+#include "smumgr.h"
+#include "tonga_smumgr.h"
+#include "pp_debug.h"
+#include "smu_ucode_xfer_vi.h"
+#include "tonga_ppsmc.h"
+#include "smu/smu_7_1_2_d.h"
+#include "smu/smu_7_1_2_sh_mask.h"
+#include "cgs_common.h"
+
+#define TONGA_SMC_SIZE			0x20000
+#define BUFFER_SIZE			80000
+#define MAX_STRING_SIZE			15
+#define BUFFER_SIZETWO              131072 /*128 *1024*/
+
+/**
+* Set the address for reading/writing the SMC SRAM space.
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    smcAddress the address in the SMC RAM to access.
+*/
+static int tonga_set_smc_sram_address(struct pp_smumgr *smumgr,
+				uint32_t smcAddress, uint32_t limit)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+	PP_ASSERT_WITH_CODE((0 == (3 & smcAddress)),
+		"SMC address must be 4 byte aligned.",
+		return -1;);
+
+	PP_ASSERT_WITH_CODE((limit > (smcAddress + 3)),
+		"SMC address is beyond the SMC RAM area.",
+		return -1;);
+
+	cgs_write_register(smumgr->device, mmSMC_IND_INDEX_0, smcAddress);
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_11, 0);
+
+	return 0;
+}
+
+/**
+* Copy bytes from an array into the SMC RAM space.
+*
+* @param    smumgr  the address of the powerplay SMU manager.
+* @param    smcStartAddress the start address in the SMC RAM to copy bytes to.
+* @param    src the byte array to copy the bytes from.
+* @param    byteCount the number of bytes to copy.
+*/
+int tonga_copy_bytes_to_smc(struct pp_smumgr *smumgr,
+		uint32_t smcStartAddress, const uint8_t *src,
+		uint32_t byteCount, uint32_t limit)
+{
+	uint32_t addr;
+	uint32_t data, orig_data;
+	int result = 0;
+	uint32_t extra_shift;
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+	PP_ASSERT_WITH_CODE((0 == (3 & smcStartAddress)),
+		"SMC address must be 4 byte aligned.",
+		return 0;);
+
+	PP_ASSERT_WITH_CODE((limit > (smcStartAddress + byteCount)),
+		"SMC address is beyond the SMC RAM area.",
+		return 0;);
+
+	addr = smcStartAddress;
+
+	while (byteCount >= 4) {
+		/*
+		 * Bytes are written into the
+		 * SMC address space with the MSB first
+		 */
+		data = (src[0] << 24) + (src[1] << 16) + (src[2] << 8) + src[3];
+
+		result = tonga_set_smc_sram_address(smumgr, addr, limit);
+
+		if (result)
+			goto out;
+
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data);
+
+		src += 4;
+		byteCount -= 4;
+		addr += 4;
+	}
+
+	if (0 != byteCount) {
+		/* Now write odd bytes left, do a read modify write cycle */
+		data = 0;
+
+		result = tonga_set_smc_sram_address(smumgr, addr, limit);
+		if (result)
+			goto out;
+
+		orig_data = cgs_read_register(smumgr->device,
+							mmSMC_IND_DATA_0);
+		extra_shift = 8 * (4 - byteCount);
+
+		while (byteCount > 0) {
+			data = (data << 8) + *src++;
+			byteCount--;
+		}
+
+		data <<= extra_shift;
+		data |= (orig_data & ~((~0UL) << extra_shift));
+
+		result = tonga_set_smc_sram_address(smumgr, addr, limit);
+		if (result)
+			goto out;
+
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data);
+	}
+
+out:
+	return result;
+}
+
+
+int tonga_program_jump_on_start(struct pp_smumgr *smumgr)
+{
+	static unsigned char pData[] = { 0xE0, 0x00, 0x80, 0x40 };
+
+	tonga_copy_bytes_to_smc(smumgr, 0x0, pData, 4, sizeof(pData)+1);
+
+	return 0;
+}
+
+/**
+* Return if the SMC is currently running.
+*
+* @param    smumgr  the address of the powerplay hardware manager.
+*/
+static int tonga_is_smc_ram_running(struct pp_smumgr *smumgr)
+{
+	return ((0 == SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+					SMC_SYSCON_CLOCK_CNTL_0, ck_disable))
+			&& (0x20100 <= cgs_read_ind_register(smumgr->device,
+					CGS_IND_REG__SMC, ixSMC_PC_C)));
+}
+
+static int tonga_send_msg_to_smc_offset(struct pp_smumgr *smumgr)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, 0x20000);
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, PPSMC_MSG_Test);
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	return 0;
+}
+
+/**
+* Send a message to the SMC, and wait for its response.
+*
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    msg the message to send.
+* @return   The response that came from the SMC.
+*/
+static int tonga_send_msg_to_smc(struct pp_smumgr *smumgr, uint16_t msg)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	if (!tonga_is_smc_ram_running(smumgr))
+		return -1;
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	PP_ASSERT_WITH_CODE(
+		1 == SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP),
+		"Failed to send Previous Message.",
+		);
+
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, msg);
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	PP_ASSERT_WITH_CODE(
+		1 == SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP),
+		"Failed to send Message.",
+		);
+
+	return 0;
+}
+
+/*
+* Send a message to the SMC, and do not wait for its response.
+*
+* @param    smumgr  the address of the powerplay hardware manager.
+* @param    msg the message to send.
+* @return   The response that came from the SMC.
+*/
+static int tonga_send_msg_to_smc_without_waiting
+				(struct pp_smumgr *smumgr, uint16_t msg)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	PP_ASSERT_WITH_CODE(
+		1 == SMUM_READ_FIELD(smumgr->device, SMC_RESP_0, SMC_RESP),
+		"Failed to send Previous Message.",
+		);
+	cgs_write_register(smumgr->device, mmSMC_MESSAGE_0, msg);
+
+	return 0;
+}
+
+/*
+* Send a message to the SMC with parameter
+*
+* @param    smumgr:  the address of the powerplay hardware manager.
+* @param    msg: the message to send.
+* @param    parameter: the parameter to send
+* @return   The response that came from the SMC.
+*/
+static int tonga_send_msg_to_smc_with_parameter(struct pp_smumgr *smumgr,
+				uint16_t msg, uint32_t parameter)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	if (!tonga_is_smc_ram_running(smumgr))
+		return PPSMC_Result_Failed;
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, parameter);
+
+	return tonga_send_msg_to_smc(smumgr, msg);
+}
+
+/*
+* Send a message to the SMC with parameter, do not wait for response
+*
+* @param    smumgr:  the address of the powerplay hardware manager.
+* @param    msg: the message to send.
+* @param    parameter: the parameter to send
+* @return   The response that came from the SMC.
+*/
+static int tonga_send_msg_to_smc_with_parameter_without_waiting(
+			struct pp_smumgr *smumgr,
+			uint16_t msg, uint32_t parameter)
+{
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	SMUM_WAIT_FIELD_UNEQUAL(smumgr, SMC_RESP_0, SMC_RESP, 0);
+
+	cgs_write_register(smumgr->device, mmSMC_MSG_ARG_0, parameter);
+
+	return tonga_send_msg_to_smc_without_waiting(smumgr, msg);
+}
+
+/*
+ * Read a 32bit value from the SMC SRAM space.
+ * ALL PARAMETERS ARE IN HOST BYTE ORDER.
+ * @param    smumgr  the address of the powerplay hardware manager.
+ * @param    smcAddress the address in the SMC RAM to access.
+ * @param    value and output parameter for the data read from the SMC SRAM.
+ */
+int tonga_read_smc_sram_dword(struct pp_smumgr *smumgr,
+					uint32_t smcAddress, uint32_t *value,
+					uint32_t limit)
+{
+	int result;
+
+	result = tonga_set_smc_sram_address(smumgr, smcAddress, limit);
+
+	if (0 != result)
+		return result;
+
+	*value = cgs_read_register(smumgr->device, mmSMC_IND_DATA_0);
+
+	return 0;
+}
+
+/*
+ * Write a 32bit value to the SMC SRAM space.
+ * ALL PARAMETERS ARE IN HOST BYTE ORDER.
+ * @param    smumgr  the address of the powerplay hardware manager.
+ * @param    smcAddress the address in the SMC RAM to access.
+ * @param    value to write to the SMC SRAM.
+ */
+int tonga_write_smc_sram_dword(struct pp_smumgr *smumgr,
+					uint32_t smcAddress, uint32_t value,
+					uint32_t limit)
+{
+	int result;
+
+	result = tonga_set_smc_sram_address(smumgr, smcAddress, limit);
+
+	if (0 != result)
+		return result;
+
+	cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, value);
+
+	return 0;
+}
+
+static int tonga_smu_fini(struct pp_smumgr *smumgr)
+{
+	if (smumgr->backend != NULL) {
+		kfree(smumgr->backend);
+		smumgr->backend = NULL;
+	}
+	return 0;
+}
+
+static enum cgs_ucode_id tonga_convert_fw_type_to_cgs(uint32_t fw_type)
+{
+	enum cgs_ucode_id result = CGS_UCODE_ID_MAXIMUM;
+
+	switch (fw_type) {
+	case UCODE_ID_SMU:
+		result = CGS_UCODE_ID_SMU;
+		break;
+	case UCODE_ID_SDMA0:
+		result = CGS_UCODE_ID_SDMA0;
+		break;
+	case UCODE_ID_SDMA1:
+		result = CGS_UCODE_ID_SDMA1;
+		break;
+	case UCODE_ID_CP_CE:
+		result = CGS_UCODE_ID_CP_CE;
+		break;
+	case UCODE_ID_CP_PFP:
+		result = CGS_UCODE_ID_CP_PFP;
+		break;
+	case UCODE_ID_CP_ME:
+		result = CGS_UCODE_ID_CP_ME;
+		break;
+	case UCODE_ID_CP_MEC:
+		result = CGS_UCODE_ID_CP_MEC;
+		break;
+	case UCODE_ID_CP_MEC_JT1:
+		result = CGS_UCODE_ID_CP_MEC_JT1;
+		break;
+	case UCODE_ID_CP_MEC_JT2:
+		result = CGS_UCODE_ID_CP_MEC_JT2;
+		break;
+	case UCODE_ID_RLC_G:
+		result = CGS_UCODE_ID_RLC_G;
+		break;
+	default:
+		break;
+	}
+
+	return result;
+}
+
+/**
+ * Convert the PPIRI firmware type to SMU type mask.
+ * For MEC, we need to check all MEC related type
+*/
+static uint16_t tonga_get_mask_for_firmware_type(uint16_t firmwareType)
+{
+	uint16_t result = 0;
+
+	switch (firmwareType) {
+	case UCODE_ID_SDMA0:
+		result = UCODE_ID_SDMA0_MASK;
+		break;
+	case UCODE_ID_SDMA1:
+		result = UCODE_ID_SDMA1_MASK;
+		break;
+	case UCODE_ID_CP_CE:
+		result = UCODE_ID_CP_CE_MASK;
+		break;
+	case UCODE_ID_CP_PFP:
+		result = UCODE_ID_CP_PFP_MASK;
+		break;
+	case UCODE_ID_CP_ME:
+		result = UCODE_ID_CP_ME_MASK;
+		break;
+	case UCODE_ID_CP_MEC:
+	case UCODE_ID_CP_MEC_JT1:
+	case UCODE_ID_CP_MEC_JT2:
+		result = UCODE_ID_CP_MEC_MASK;
+		break;
+	case UCODE_ID_RLC_G:
+		result = UCODE_ID_RLC_G_MASK;
+		break;
+	default:
+		break;
+	}
+
+	return result;
+}
+
+/**
+ * Check if the FW has been loaded,
+ * SMU will not return if loading has not finished.
+*/
+static int tonga_check_fw_load_finish(struct pp_smumgr *smumgr, uint32_t fwType)
+{
+	uint16_t fwMask = tonga_get_mask_for_firmware_type(fwType);
+
+	if (0 != SMUM_WAIT_VFPF_INDIRECT_REGISTER(smumgr, SMC_IND,
+				SOFT_REGISTERS_TABLE_28, fwMask, fwMask)) {
+		printk(KERN_ERR "[ powerplay ] check firmware loading failed\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Populate one firmware image to the data structure */
+static int tonga_populate_single_firmware_entry(struct pp_smumgr *smumgr,
+				uint16_t firmware_type,
+				struct SMU_Entry *pentry)
+{
+	int result;
+	struct cgs_firmware_info info = {0};
+
+	result = cgs_get_firmware_info(
+				smumgr->device,
+				tonga_convert_fw_type_to_cgs(firmware_type),
+				&info);
+
+	if (result == 0) {
+		pentry->version = 0;
+		pentry->id = (uint16_t)firmware_type;
+		pentry->image_addr_high = smu_upper_32_bits(info.mc_addr);
+		pentry->image_addr_low = smu_lower_32_bits(info.mc_addr);
+		pentry->meta_data_addr_high = 0;
+		pentry->meta_data_addr_low = 0;
+		pentry->data_size_byte = info.image_size;
+		pentry->num_register_entries = 0;
+
+		if (firmware_type == UCODE_ID_RLC_G)
+			pentry->flags = 1;
+		else
+			pentry->flags = 0;
+	} else {
+		return result;
+	}
+
+	return result;
+}
+
+static int tonga_request_smu_reload_fw(struct pp_smumgr *smumgr)
+{
+	struct tonga_smumgr *tonga_smu =
+		(struct tonga_smumgr *)(smumgr->backend);
+	uint16_t fw_to_load;
+	int result = 0;
+	struct SMU_DRAMData_TOC *toc;
+	/**
+	 * First time this gets called during SmuMgr init,
+	 * we haven't processed SMU header file yet,
+	 * so Soft Register Start offset is unknown.
+	 * However, for this case, UcodeLoadStatus is already 0,
+	 * so we can skip this if the Soft Registers Start offset is 0.
+	 */
+	cgs_write_ind_register(smumgr->device,
+		CGS_IND_REG__SMC, ixSOFT_REGISTERS_TABLE_28, 0);
+
+	tonga_send_msg_to_smc_with_parameter(smumgr,
+		PPSMC_MSG_SMU_DRAM_ADDR_HI,
+		tonga_smu->smu_buffer.mc_addr_high);
+	tonga_send_msg_to_smc_with_parameter(smumgr,
+		PPSMC_MSG_SMU_DRAM_ADDR_LO,
+		tonga_smu->smu_buffer.mc_addr_low);
+
+	toc = (struct SMU_DRAMData_TOC *)tonga_smu->pHeader;
+	toc->num_entries = 0;
+	toc->structure_version = 1;
+
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry(smumgr,
+		UCODE_ID_RLC_G,
+		&toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n",
+		return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry(smumgr,
+		UCODE_ID_CP_CE,
+		&toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n",
+		return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_CP_PFP, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_CP_ME, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_CP_MEC, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_CP_MEC_JT1, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_CP_MEC_JT2, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_SDMA0, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_populate_single_firmware_entry
+		(smumgr, UCODE_ID_SDMA1, &toc->entry[toc->num_entries++]),
+		"Failed to Get Firmware Entry.\n", return -1);
+
+	tonga_send_msg_to_smc_with_parameter(smumgr,
+		PPSMC_MSG_DRV_DRAM_ADDR_HI,
+		tonga_smu->header_buffer.mc_addr_high);
+	tonga_send_msg_to_smc_with_parameter(smumgr,
+		PPSMC_MSG_DRV_DRAM_ADDR_LO,
+		tonga_smu->header_buffer.mc_addr_low);
+
+	fw_to_load = UCODE_ID_RLC_G_MASK
+			+ UCODE_ID_SDMA0_MASK
+			+ UCODE_ID_SDMA1_MASK
+			+ UCODE_ID_CP_CE_MASK
+			+ UCODE_ID_CP_ME_MASK
+			+ UCODE_ID_CP_PFP_MASK
+			+ UCODE_ID_CP_MEC_MASK;
+
+	PP_ASSERT_WITH_CODE(
+		0 == tonga_send_msg_to_smc_with_parameter_without_waiting(
+		smumgr, PPSMC_MSG_LoadUcodes, fw_to_load),
+		"Fail to Request SMU Load uCode", return 0);
+
+	return result;
+}
+
+static int tonga_request_smu_load_specific_fw(struct pp_smumgr *smumgr,
+				uint32_t firmwareType)
+{
+	return 0;
+}
+
+/**
+ * Upload the SMC firmware to the SMC microcontroller.
+ *
+ * @param    smumgr  the address of the powerplay hardware manager.
+ * @param    pFirmware the data structure containing the various sections of the firmware.
+ */
+static int tonga_smu_upload_firmware_image(struct pp_smumgr *smumgr)
+{
+	const uint8_t *src;
+	uint32_t byte_count;
+	uint32_t *data;
+	struct cgs_firmware_info info = {0};
+
+	if (smumgr == NULL || smumgr->device == NULL)
+		return -EINVAL;
+
+	cgs_get_firmware_info(smumgr->device,
+		tonga_convert_fw_type_to_cgs(UCODE_ID_SMU), &info);
+
+	if (info.image_size & 3) {
+		printk(KERN_ERR "[ powerplay ] SMC ucode is not 4 bytes aligned\n");
+		return -EINVAL;
+	}
+
+	if (info.image_size > TONGA_SMC_SIZE) {
+		printk(KERN_ERR "[ powerplay ] SMC address is beyond the SMC RAM area\n");
+		return -EINVAL;
+	}
+
+	cgs_write_register(smumgr->device, mmSMC_IND_INDEX_0, 0x20000);
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_0, 1);
+
+	byte_count = info.image_size;
+	src = (const uint8_t *)info.kptr;
+
+	data = (uint32_t *)src;
+	for (; byte_count >= 4; data++, byte_count -= 4)
+		cgs_write_register(smumgr->device, mmSMC_IND_DATA_0, data[0]);
+
+	SMUM_WRITE_FIELD(smumgr->device, SMC_IND_ACCESS_CNTL, AUTO_INCREMENT_IND_0, 0);
+
+	return 0;
+}
+
+static int tonga_start_in_protection_mode(struct pp_smumgr *smumgr)
+{
+	int result;
+
+	/* Assert reset */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_RESET_CNTL, rst_reg, 1);
+
+	result = tonga_smu_upload_firmware_image(smumgr);
+	if (result)
+		return result;
+
+	/* Clear status */
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+		ixSMU_STATUS, 0);
+
+	/* Enable clock */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_CLOCK_CNTL_0, ck_disable, 0);
+
+	/* De-assert reset */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_RESET_CNTL, rst_reg, 0);
+
+	/* Set SMU Auto Start */
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMU_INPUT_DATA, AUTO_START, 1);
+
+	/* Clear firmware interrupt enable flag */
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+		ixFIRMWARE_FLAGS, 0);
+
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND,
+		RCU_UC_EVENTS, INTERRUPTS_ENABLED, 1);
+
+	/**
+	 * Call Test SMU message with 0x20000 offset to trigger SMU start
+	 */
+	tonga_send_msg_to_smc_offset(smumgr);
+
+	/* Wait for done bit to be set */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(smumgr, SMC_IND,
+		SMU_STATUS, SMU_DONE, 0);
+
+	/* Check pass/failed indicator */
+	if (1 != SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device,
+				CGS_IND_REG__SMC, SMU_STATUS, SMU_PASS)) {
+		printk(KERN_ERR "[ powerplay ] SMU Firmware start failed\n");
+		return -EINVAL;
+	}
+
+	/* Wait for firmware to initialize */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND,
+		FIRMWARE_FLAGS, INTERRUPTS_ENABLED, 1);
+
+	return 0;
+}
+
+
+static int tonga_start_in_non_protection_mode(struct pp_smumgr *smumgr)
+{
+	int result = 0;
+
+	/* wait for smc boot up */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD_UNEQUAL(smumgr, SMC_IND,
+		RCU_UC_EVENTS, boot_seq_done, 0);
+
+	/*Clear firmware interrupt enable flag*/
+	cgs_write_ind_register(smumgr->device, CGS_IND_REG__SMC,
+		ixFIRMWARE_FLAGS, 0);
+
+
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_RESET_CNTL, rst_reg, 1);
+
+	result = tonga_smu_upload_firmware_image(smumgr);
+
+	if (result != 0)
+		return result;
+
+	/* Set smc instruct start point at 0x0 */
+	tonga_program_jump_on_start(smumgr);
+
+
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_CLOCK_CNTL_0, ck_disable, 0);
+
+	/*De-assert reset*/
+	SMUM_WRITE_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+		SMC_SYSCON_RESET_CNTL, rst_reg, 0);
+
+	/* Wait for firmware to initialize */
+	SMUM_WAIT_VFPF_INDIRECT_FIELD(smumgr, SMC_IND,
+		FIRMWARE_FLAGS, INTERRUPTS_ENABLED, 1);
+
+	return result;
+}
+
+static int tonga_start_smu(struct pp_smumgr *smumgr)
+{
+	int result;
+
+	/* Only start SMC if SMC RAM is not running */
+	if (!tonga_is_smc_ram_running(smumgr)) {
+		/*Check if SMU is running in protected mode*/
+		if (0 == SMUM_READ_VFPF_INDIRECT_FIELD(smumgr->device, CGS_IND_REG__SMC,
+					SMU_FIRMWARE, SMU_MODE)) {
+			result = tonga_start_in_non_protection_mode(smumgr);
+			if (result)
+				return result;
+		} else {
+			result = tonga_start_in_protection_mode(smumgr);
+			if (result)
+				return result;
+		}
+	}
+
+	result = tonga_request_smu_reload_fw(smumgr);
+
+	return result;
+}
+
+/**
+ * Write a 32bit value to the SMC SRAM space.
+ * ALL PARAMETERS ARE IN HOST BYTE ORDER.
+ * @param    smumgr  the address of the powerplay hardware manager.
+ * @param    smcAddress the address in the SMC RAM to access.
+ * @param    value to write to the SMC SRAM.
+ */
+static int tonga_smu_init(struct pp_smumgr *smumgr)
+{
+	struct tonga_smumgr *tonga_smu;
+	uint8_t *internal_buf;
+	uint64_t mc_addr = 0;
+	/* Allocate memory for backend private data */
+	tonga_smu = (struct tonga_smumgr *)(smumgr->backend);
+	tonga_smu->header_buffer.data_size =
+		((sizeof(struct SMU_DRAMData_TOC) / 4096) + 1) * 4096;
+	tonga_smu->smu_buffer.data_size = 200*4096;
+
+	smu_allocate_memory(smumgr->device,
+		tonga_smu->header_buffer.data_size,
+		CGS_GPU_MEM_TYPE__VISIBLE_CONTIG_FB,
+		PAGE_SIZE,
+		&mc_addr,
+		&tonga_smu->header_buffer.kaddr,
+		&tonga_smu->header_buffer.handle);
+
+	tonga_smu->pHeader = tonga_smu->header_buffer.kaddr;
+	tonga_smu->header_buffer.mc_addr_high = smu_upper_32_bits(mc_addr);
+	tonga_smu->header_buffer.mc_addr_low = smu_lower_32_bits(mc_addr);
+
+	PP_ASSERT_WITH_CODE((NULL != tonga_smu->pHeader),
+		"Out of memory.",
+		kfree(smumgr->backend);
+		cgs_free_gpu_mem(smumgr->device,
+		(cgs_handle_t)tonga_smu->header_buffer.handle);
+		return -1);
+
+	smu_allocate_memory(smumgr->device,
+		tonga_smu->smu_buffer.data_size,
+		CGS_GPU_MEM_TYPE__VISIBLE_CONTIG_FB,
+		PAGE_SIZE,
+		&mc_addr,
+		&tonga_smu->smu_buffer.kaddr,
+		&tonga_smu->smu_buffer.handle);
+
+	internal_buf = tonga_smu->smu_buffer.kaddr;
+	tonga_smu->smu_buffer.mc_addr_high = smu_upper_32_bits(mc_addr);
+	tonga_smu->smu_buffer.mc_addr_low = smu_lower_32_bits(mc_addr);
+
+	PP_ASSERT_WITH_CODE((NULL != internal_buf),
+		"Out of memory.",
+		kfree(smumgr->backend);
+		cgs_free_gpu_mem(smumgr->device,
+		(cgs_handle_t)tonga_smu->smu_buffer.handle);
+		return -1;);
+
+	return 0;
+}
+
+static const struct pp_smumgr_func tonga_smu_funcs = {
+	.smu_init = &tonga_smu_init,
+	.smu_fini = &tonga_smu_fini,
+	.start_smu = &tonga_start_smu,
+	.check_fw_load_finish = &tonga_check_fw_load_finish,
+	.request_smu_load_fw = &tonga_request_smu_reload_fw,
+	.request_smu_load_specific_fw = &tonga_request_smu_load_specific_fw,
+	.send_msg_to_smc = &tonga_send_msg_to_smc,
+	.send_msg_to_smc_with_parameter = &tonga_send_msg_to_smc_with_parameter,
+	.download_pptable_settings = NULL,
+	.upload_pptable_settings = NULL,
+};
+
+int tonga_smum_init(struct pp_smumgr *smumgr)
+{
+	struct tonga_smumgr *tonga_smu = NULL;
+
+	tonga_smu = kzalloc(sizeof(struct tonga_smumgr), GFP_KERNEL);
+
+	if (tonga_smu == NULL)
+		return -ENOMEM;
+
+	smumgr->backend = tonga_smu;
+	smumgr->smumgr_funcs = &tonga_smu_funcs;
+
+	return 0;
+}
diff --git a/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.h b/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.h
new file mode 100644
index 000000000000..33c788d7f05c
--- /dev/null
+++ b/drivers/gpu/drm/amd/powerplay/smumgr/tonga_smumgr.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright 2015 Advanced Micro Devices, Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ */
+
+#ifndef _TONGA_SMUMGR_H_
+#define _TONGA_SMUMGR_H_
+
+struct tonga_buffer_entry {
+	uint32_t data_size;
+	uint32_t mc_addr_low;
+	uint32_t mc_addr_high;
+	void *kaddr;
+	unsigned long  handle;
+};
+
+struct tonga_smumgr {
+	uint8_t *pHeader;
+	uint8_t *pMecImage;
+	uint32_t ulSoftRegsStart;
+
+	struct tonga_buffer_entry header_buffer;
+	struct tonga_buffer_entry smu_buffer;
+};
+
+extern int tonga_smum_init(struct pp_smumgr *smumgr);
+extern int tonga_copy_bytes_to_smc(struct pp_smumgr *smumgr,
+		uint32_t smcStartAddress, const uint8_t *src,
+		uint32_t byteCount, uint32_t limit);
+extern int tonga_read_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smcAddress,
+		uint32_t *value, uint32_t limit);
+extern int tonga_write_smc_sram_dword(struct pp_smumgr *smumgr, uint32_t smcAddress,
+		uint32_t value, uint32_t limit);
+
+#endif
diff --git a/drivers/gpu/drm/amd/scheduler/gpu_scheduler.c b/drivers/gpu/drm/amd/scheduler/gpu_scheduler.c
index 3a4820e863ec..8b2becd1aa07 100644
--- a/drivers/gpu/drm/amd/scheduler/gpu_scheduler.c
+++ b/drivers/gpu/drm/amd/scheduler/gpu_scheduler.c
@@ -47,6 +47,8 @@ static void amd_sched_rq_init(struct amd_sched_rq *rq)
 static void amd_sched_rq_add_entity(struct amd_sched_rq *rq,
 				    struct amd_sched_entity *entity)
 {
+	if (!list_empty(&entity->list))
+		return;
 	spin_lock(&rq->lock);
 	list_add_tail(&entity->list, &rq->entities);
 	spin_unlock(&rq->lock);
@@ -55,6 +57,8 @@ static void amd_sched_rq_add_entity(struct amd_sched_rq *rq,
 static void amd_sched_rq_remove_entity(struct amd_sched_rq *rq,
 				       struct amd_sched_entity *entity)
 {
+	if (list_empty(&entity->list))
+		return;
 	spin_lock(&rq->lock);
 	list_del_init(&entity->list);
 	if (rq->current_entity == entity)
@@ -138,9 +142,6 @@ int amd_sched_entity_init(struct amd_gpu_scheduler *sched,
 	atomic_set(&entity->fence_seq, 0);
 	entity->fence_context = fence_context_alloc(1);
 
-	/* Add the entity to the run queue */
-	amd_sched_rq_add_entity(rq, entity);
-
 	return 0;
 }
 
@@ -302,9 +303,11 @@ static bool amd_sched_entity_in(struct amd_sched_job *sched_job)
 	spin_unlock(&entity->queue_lock);
 
 	/* first job wakes up scheduler */
-	if (first)
+	if (first) {
+		/* Add the entity to the run queue */
+		amd_sched_rq_add_entity(entity->rq, entity);
 		amd_sched_wakeup(sched);
-
+	}
 	return added;
 }
 
@@ -349,14 +352,17 @@ static struct amd_sched_entity *
 amd_sched_select_entity(struct amd_gpu_scheduler *sched)
 {
 	struct amd_sched_entity *entity;
+	int i;
 
 	if (!amd_sched_ready(sched))
 		return NULL;
 
 	/* Kernel run queue has higher priority than normal run queue*/
-	entity = amd_sched_rq_select_entity(&sched->kernel_rq);
-	if (entity == NULL)
-		entity = amd_sched_rq_select_entity(&sched->sched_rq);
+	for (i = 0; i < AMD_SCHED_MAX_PRIORITY; i++) {
+		entity = amd_sched_rq_select_entity(&sched->sched_rq[i]);
+		if (entity)
+			break;
+	}
 
 	return entity;
 }
@@ -478,12 +484,13 @@ int amd_sched_init(struct amd_gpu_scheduler *sched,
 		   struct amd_sched_backend_ops *ops,
 		   unsigned hw_submission, long timeout, const char *name)
 {
+	int i;
 	sched->ops = ops;
 	sched->hw_submission_limit = hw_submission;
 	sched->name = name;
 	sched->timeout = timeout;
-	amd_sched_rq_init(&sched->sched_rq);
-	amd_sched_rq_init(&sched->kernel_rq);
+	for (i = 0; i < AMD_SCHED_MAX_PRIORITY; i++)
+		amd_sched_rq_init(&sched->sched_rq[i]);
 
 	init_waitqueue_head(&sched->wake_up_worker);
 	init_waitqueue_head(&sched->job_scheduled);
diff --git a/drivers/gpu/drm/amd/scheduler/gpu_scheduler.h b/drivers/gpu/drm/amd/scheduler/gpu_scheduler.h
index a0f0ae53aacd..9403145d7bee 100644
--- a/drivers/gpu/drm/amd/scheduler/gpu_scheduler.h
+++ b/drivers/gpu/drm/amd/scheduler/gpu_scheduler.h
@@ -104,6 +104,12 @@ struct amd_sched_backend_ops {
 	struct fence *(*run_job)(struct amd_sched_job *sched_job);
 };
 
+enum amd_sched_priority {
+	AMD_SCHED_PRIORITY_KERNEL = 0,
+	AMD_SCHED_PRIORITY_NORMAL,
+	AMD_SCHED_MAX_PRIORITY
+};
+
 /**
  * One scheduler is implemented for each hardware ring
 */
@@ -112,8 +118,7 @@ struct amd_gpu_scheduler {
 	uint32_t			hw_submission_limit;
 	long				timeout;
 	const char			*name;
-	struct amd_sched_rq		sched_rq;
-	struct amd_sched_rq		kernel_rq;
+	struct amd_sched_rq		sched_rq[AMD_SCHED_MAX_PRIORITY];
 	wait_queue_head_t		wake_up_worker;
 	wait_queue_head_t		job_scheduled;
 	atomic_t			hw_rq_count;
diff --git a/drivers/gpu/drm/armada/armada_crtc.c b/drivers/gpu/drm/armada/armada_crtc.c
index cebcab560626..0293eb74d777 100644
--- a/drivers/gpu/drm/armada/armada_crtc.c
+++ b/drivers/gpu/drm/armada/armada_crtc.c
@@ -928,11 +928,10 @@ static int armada_drm_crtc_cursor_set(struct drm_crtc *crtc,
 		}
 	}
 
-	mutex_lock(&dev->struct_mutex);
 	if (dcrtc->cursor_obj) {
 		dcrtc->cursor_obj->update = NULL;
 		dcrtc->cursor_obj->update_data = NULL;
-		drm_gem_object_unreference(&dcrtc->cursor_obj->obj);
+		drm_gem_object_unreference_unlocked(&dcrtc->cursor_obj->obj);
 	}
 	dcrtc->cursor_obj = obj;
 	dcrtc->cursor_w = w;
@@ -942,14 +941,12 @@ static int armada_drm_crtc_cursor_set(struct drm_crtc *crtc,
 		obj->update_data = dcrtc;
 		obj->update = cursor_update;
 	}
-	mutex_unlock(&dev->struct_mutex);
 
 	return ret;
 }
 
 static int armada_drm_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 {
-	struct drm_device *dev = crtc->dev;
 	struct armada_crtc *dcrtc = drm_to_armada_crtc(crtc);
 	int ret;
 
@@ -957,11 +954,9 @@ static int armada_drm_crtc_cursor_move(struct drm_crtc *crtc, int x, int y)
 	if (!dcrtc->variant->has_spu_adv_reg)
 		return -EFAULT;
 
-	mutex_lock(&dev->struct_mutex);
 	dcrtc->cursor_x = x;
 	dcrtc->cursor_y = y;
 	ret = armada_drm_crtc_cursor_update(dcrtc, false);
-	mutex_unlock(&dev->struct_mutex);
 
 	return ret;
 }
@@ -972,7 +967,7 @@ static void armada_drm_crtc_destroy(struct drm_crtc *crtc)
 	struct armada_private *priv = crtc->dev->dev_private;
 
 	if (dcrtc->cursor_obj)
-		drm_gem_object_unreference(&dcrtc->cursor_obj->obj);
+		drm_gem_object_unreference_unlocked(&dcrtc->cursor_obj->obj);
 
 	priv->dcrtc[dcrtc->num] = NULL;
 	drm_crtc_cleanup(&dcrtc->crtc);
@@ -1074,7 +1069,7 @@ armada_drm_crtc_set_property(struct drm_crtc *crtc,
 	return 0;
 }
 
-static struct drm_crtc_funcs armada_crtc_funcs = {
+static const struct drm_crtc_funcs armada_crtc_funcs = {
 	.cursor_set	= armada_drm_crtc_cursor_set,
 	.cursor_move	= armada_drm_crtc_cursor_move,
 	.destroy	= armada_drm_crtc_destroy,
@@ -1216,14 +1211,14 @@ static int armada_drm_crtc_create(struct drm_device *drm, struct device *dev,
 				       &armada_primary_plane_funcs,
 				       armada_primary_formats,
 				       ARRAY_SIZE(armada_primary_formats),
-				       DRM_PLANE_TYPE_PRIMARY);
+				       DRM_PLANE_TYPE_PRIMARY, NULL);
 	if (ret) {
 		kfree(primary);
 		return ret;
 	}
 
 	ret = drm_crtc_init_with_planes(drm, &dcrtc->crtc, &primary->base, NULL,
-					&armada_crtc_funcs);
+					&armada_crtc_funcs, NULL);
 	if (ret)
 		goto err_crtc_init;
 
diff --git a/drivers/gpu/drm/armada/armada_debugfs.c b/drivers/gpu/drm/armada/armada_debugfs.c
index 471e45627f1e..d4f7ab0a30d4 100644
--- a/drivers/gpu/drm/armada/armada_debugfs.c
+++ b/drivers/gpu/drm/armada/armada_debugfs.c
@@ -21,9 +21,9 @@ static int armada_debugfs_gem_linear_show(struct seq_file *m, void *data)
 	struct armada_private *priv = dev->dev_private;
 	int ret;
 
-	mutex_lock(&dev->struct_mutex);
+	mutex_lock(&priv->linear_lock);
 	ret = drm_mm_dump_table(m, &priv->linear);
-	mutex_unlock(&dev->struct_mutex);
+	mutex_unlock(&priv->linear_lock);
 
 	return ret;
 }
diff --git a/drivers/gpu/drm/armada/armada_drm.h b/drivers/gpu/drm/armada/armada_drm.h
index 4df6f2af2b21..3b2bb6128d40 100644
--- a/drivers/gpu/drm/armada/armada_drm.h
+++ b/drivers/gpu/drm/armada/armada_drm.h
@@ -57,7 +57,8 @@ struct armada_private {
 	DECLARE_KFIFO(fb_unref, struct drm_framebuffer *, 8);
 	struct drm_fb_helper	*fbdev;
 	struct armada_crtc	*dcrtc[2];
-	struct drm_mm		linear;
+	struct drm_mm		linear; /* protected by linear_lock */
+	struct mutex		linear_lock;
 	struct drm_property	*csc_yuv_prop;
 	struct drm_property	*csc_rgb_prop;
 	struct drm_property	*colorkey_prop;
diff --git a/drivers/gpu/drm/armada/armada_drv.c b/drivers/gpu/drm/armada/armada_drv.c
index 77ab93d60125..3bd7e1cde99e 100644
--- a/drivers/gpu/drm/armada/armada_drv.c
+++ b/drivers/gpu/drm/armada/armada_drv.c
@@ -102,6 +102,7 @@ static int armada_drm_load(struct drm_device *dev, unsigned long flags)
 	dev->mode_config.preferred_depth = 24;
 	dev->mode_config.funcs = &armada_drm_mode_config_funcs;
 	drm_mm_init(&priv->linear, mem->start, resource_size(mem));
+	mutex_init(&priv->linear_lock);
 
 	ret = component_bind_all(dev->dev, dev);
 	if (ret)
diff --git a/drivers/gpu/drm/armada/armada_fb.c b/drivers/gpu/drm/armada/armada_fb.c
index 1c90969def3e..5fa4bf20b232 100644
--- a/drivers/gpu/drm/armada/armada_fb.c
+++ b/drivers/gpu/drm/armada/armada_fb.c
@@ -35,7 +35,7 @@ static const struct drm_framebuffer_funcs armada_fb_funcs = {
 };
 
 struct armada_framebuffer *armada_framebuffer_create(struct drm_device *dev,
-	struct drm_mode_fb_cmd2 *mode, struct armada_gem_object *obj)
+	const struct drm_mode_fb_cmd2 *mode, struct armada_gem_object *obj)
 {
 	struct armada_framebuffer *dfb;
 	uint8_t format, config;
@@ -101,7 +101,7 @@ struct armada_framebuffer *armada_framebuffer_create(struct drm_device *dev,
 }
 
 static struct drm_framebuffer *armada_fb_create(struct drm_device *dev,
-	struct drm_file *dfile, struct drm_mode_fb_cmd2 *mode)
+	struct drm_file *dfile, const struct drm_mode_fb_cmd2 *mode)
 {
 	struct armada_gem_object *obj;
 	struct armada_framebuffer *dfb;
diff --git a/drivers/gpu/drm/armada/armada_fb.h b/drivers/gpu/drm/armada/armada_fb.h
index ce3f12ebfc53..48073c4f54d8 100644
--- a/drivers/gpu/drm/armada/armada_fb.h
+++ b/drivers/gpu/drm/armada/armada_fb.h
@@ -19,6 +19,6 @@ struct armada_framebuffer {
 #define drm_fb_obj(fb) drm_fb_to_armada_fb(fb)->obj
 
 struct armada_framebuffer *armada_framebuffer_create(struct drm_device *,
-	struct drm_mode_fb_cmd2 *, struct armada_gem_object *);
+	const struct drm_mode_fb_cmd2 *, struct armada_gem_object *);
 
 #endif
diff --git a/drivers/gpu/drm/armada/armada_gem.c b/drivers/gpu/drm/armada/armada_gem.c
index 60a688ef81c7..6e731db31aa4 100644
--- a/drivers/gpu/drm/armada/armada_gem.c
+++ b/drivers/gpu/drm/armada/armada_gem.c
@@ -46,22 +46,26 @@ static size_t roundup_gem_size(size_t size)
 	return roundup(size, PAGE_SIZE);
 }
 
-/* dev->struct_mutex is held here */
 void armada_gem_free_object(struct drm_gem_object *obj)
 {
 	struct armada_gem_object *dobj = drm_to_armada_gem(obj);
+	struct armada_private *priv = obj->dev->dev_private;
 
 	DRM_DEBUG_DRIVER("release obj %p\n", dobj);
 
 	drm_gem_free_mmap_offset(&dobj->obj);
 
+	might_lock(&priv->linear_lock);
+
 	if (dobj->page) {
 		/* page backed memory */
 		unsigned int order = get_order(dobj->obj.size);
 		__free_pages(dobj->page, order);
 	} else if (dobj->linear) {
 		/* linear backed memory */
+		mutex_lock(&priv->linear_lock);
 		drm_mm_remove_node(dobj->linear);
+		mutex_unlock(&priv->linear_lock);
 		kfree(dobj->linear);
 		if (dobj->addr)
 			iounmap(dobj->addr);
@@ -144,10 +148,10 @@ armada_gem_linear_back(struct drm_device *dev, struct armada_gem_object *obj)
 		if (!node)
 			return -ENOSPC;
 
-		mutex_lock(&dev->struct_mutex);
+		mutex_lock(&priv->linear_lock);
 		ret = drm_mm_insert_node(&priv->linear, node, size, align,
 					 DRM_MM_SEARCH_DEFAULT);
-		mutex_unlock(&dev->struct_mutex);
+		mutex_unlock(&priv->linear_lock);
 		if (ret) {
 			kfree(node);
 			return ret;
@@ -158,9 +162,9 @@ armada_gem_linear_back(struct drm_device *dev, struct armada_gem_object *obj)
 		/* Ensure that the memory we're returning is cleared. */
 		ptr = ioremap_wc(obj->linear->start, size);
 		if (!ptr) {
-			mutex_lock(&dev->struct_mutex);
+			mutex_lock(&priv->linear_lock);
 			drm_mm_remove_node(obj->linear);
-			mutex_unlock(&dev->struct_mutex);
+			mutex_unlock(&priv->linear_lock);
 			kfree(obj->linear);
 			obj->linear = NULL;
 			return -ENOMEM;
@@ -274,18 +278,16 @@ int armada_gem_dumb_map_offset(struct drm_file *file, struct drm_device *dev,
 	struct armada_gem_object *obj;
 	int ret = 0;
 
-	mutex_lock(&dev->struct_mutex);
 	obj = armada_gem_object_lookup(dev, file, handle);
 	if (!obj) {
 		DRM_ERROR("failed to lookup gem object\n");
-		ret = -EINVAL;
-		goto err_unlock;
+		return -EINVAL;
 	}
 
 	/* Don't allow imported objects to be mapped */
 	if (obj->obj.import_attach) {
 		ret = -EINVAL;
-		goto err_unlock;
+		goto err_unref;
 	}
 
 	ret = drm_gem_create_mmap_offset(&obj->obj);
@@ -294,9 +296,8 @@ int armada_gem_dumb_map_offset(struct drm_file *file, struct drm_device *dev,
 		DRM_DEBUG_DRIVER("handle %#x offset %llx\n", handle, *offset);
 	}
 
-	drm_gem_object_unreference(&obj->obj);
- err_unlock:
-	mutex_unlock(&dev->struct_mutex);
+ err_unref:
+	drm_gem_object_unreference_unlocked(&obj->obj);
 
 	return ret;
 }
@@ -352,13 +353,13 @@ int armada_gem_mmap_ioctl(struct drm_device *dev, void *data,
 		return -ENOENT;
 
 	if (!dobj->obj.filp) {
-		drm_gem_object_unreference(&dobj->obj);
+		drm_gem_object_unreference_unlocked(&dobj->obj);
 		return -EINVAL;
 	}
 
 	addr = vm_mmap(dobj->obj.filp, 0, args->size, PROT_READ | PROT_WRITE,
 		       MAP_SHARED, args->offset);
-	drm_gem_object_unreference(&dobj->obj);
+	drm_gem_object_unreference_unlocked(&dobj->obj);
 	if (IS_ERR_VALUE(addr))
 		return addr;
 
diff --git a/drivers/gpu/drm/armada/armada_overlay.c b/drivers/gpu/drm/armada/armada_overlay.c
index 5c22b380f8f3..148e8a42b2c6 100644
--- a/drivers/gpu/drm/armada/armada_overlay.c
+++ b/drivers/gpu/drm/armada/armada_overlay.c
@@ -460,7 +460,7 @@ int armada_overlay_plane_create(struct drm_device *dev, unsigned long crtcs)
 				       &armada_ovl_plane_funcs,
 				       armada_ovl_formats,
 				       ARRAY_SIZE(armada_ovl_formats),
-				       DRM_PLANE_TYPE_OVERLAY);
+				       DRM_PLANE_TYPE_OVERLAY, NULL);
 	if (ret) {
 		kfree(dplane);
 		return ret;
diff --git a/drivers/gpu/drm/ast/ast_drv.h b/drivers/gpu/drm/ast/ast_drv.h
index 05f6522c0457..eb5715994ac2 100644
--- a/drivers/gpu/drm/ast/ast_drv.h
+++ b/drivers/gpu/drm/ast/ast_drv.h
@@ -256,7 +256,6 @@ struct ast_framebuffer {
 struct ast_fbdev {
 	struct drm_fb_helper helper;
 	struct ast_framebuffer afb;
-	struct list_head fbdev_list;
 	void *sysram;
 	int size;
 	struct ttm_bo_kmap_obj mapping;
@@ -309,7 +308,7 @@ extern void ast_mode_fini(struct drm_device *dev);
 
 int ast_framebuffer_init(struct drm_device *dev,
 			 struct ast_framebuffer *ast_fb,
-			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 const struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct drm_gem_object *obj);
 
 int ast_fbdev_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
index a37e7ea4a00c..5320f8c57884 100644
--- a/drivers/gpu/drm/ast/ast_fb.c
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -163,7 +163,7 @@ static struct fb_ops astfb_ops = {
 };
 
 static int astfb_create_object(struct ast_fbdev *afbdev,
-			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       const struct drm_mode_fb_cmd2 *mode_cmd,
 			       struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = afbdev->helper.dev;
diff --git a/drivers/gpu/drm/ast/ast_main.c b/drivers/gpu/drm/ast/ast_main.c
index 541a610667ad..b1480acbb3c3 100644
--- a/drivers/gpu/drm/ast/ast_main.c
+++ b/drivers/gpu/drm/ast/ast_main.c
@@ -227,7 +227,7 @@ static int ast_get_dram_info(struct drm_device *dev)
 	} while (ast_read32(ast, 0x10000) != 0x01);
 	data = ast_read32(ast, 0x10004);
 
-	if (data & 0x400)
+	if (data & 0x40)
 		ast->dram_bus_width = 16;
 	else
 		ast->dram_bus_width = 32;
@@ -309,7 +309,7 @@ static const struct drm_framebuffer_funcs ast_fb_funcs = {
 
 int ast_framebuffer_init(struct drm_device *dev,
 			 struct ast_framebuffer *ast_fb,
-			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 const struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct drm_gem_object *obj)
 {
 	int ret;
@@ -327,7 +327,7 @@ int ast_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 ast_user_framebuffer_create(struct drm_device *dev,
 	       struct drm_file *filp,
-	       struct drm_mode_fb_cmd2 *mode_cmd)
+	       const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct ast_framebuffer *ast_fb;
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
index 69d19f3304a5..0123458cbd83 100644
--- a/drivers/gpu/drm/ast/ast_mode.c
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -751,7 +751,7 @@ static int ast_encoder_init(struct drm_device *dev)
 		return -ENOMEM;
 
 	drm_encoder_init(dev, &ast_encoder->base, &ast_enc_funcs,
-			 DRM_MODE_ENCODER_DAC);
+			 DRM_MODE_ENCODER_DAC, NULL);
 	drm_encoder_helper_add(&ast_encoder->base, &ast_enc_helper_funcs);
 
 	ast_encoder->base.possible_crtcs = 1;
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
index 9f6e234e7029..468a14f266a7 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_crtc.c
@@ -344,7 +344,7 @@ int atmel_hlcdc_crtc_create(struct drm_device *dev)
 	ret = drm_crtc_init_with_planes(dev, &crtc->base,
 				&planes->primary->base,
 				planes->cursor ? &planes->cursor->base : NULL,
-				&atmel_hlcdc_crtc_funcs);
+				&atmel_hlcdc_crtc_funcs, NULL);
 	if (ret < 0)
 		goto fail;
 
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
index 244df0a440b7..a45b32ba029e 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_dc.c
@@ -333,6 +333,10 @@ static const struct of_device_id atmel_hlcdc_of_match[] = {
 		.data = &atmel_hlcdc_dc_at91sam9x5,
 	},
 	{
+		.compatible = "atmel,sama5d2-hlcdc",
+		.data = &atmel_hlcdc_dc_sama5d4,
+	},
+	{
 		.compatible = "atmel,sama5d3-hlcdc",
 		.data = &atmel_hlcdc_dc_sama5d3,
 	},
@@ -342,6 +346,7 @@ static const struct of_device_id atmel_hlcdc_of_match[] = {
 	},
 	{ /* sentinel */ },
 };
+MODULE_DEVICE_TABLE(of, atmel_hlcdc_of_match);
 
 int atmel_hlcdc_dc_mode_valid(struct atmel_hlcdc_dc *dc,
 			      struct drm_display_mode *mode)
@@ -402,7 +407,7 @@ static irqreturn_t atmel_hlcdc_dc_irq_handler(int irq, void *data)
 }
 
 static struct drm_framebuffer *atmel_hlcdc_fb_create(struct drm_device *dev,
-		struct drm_file *file_priv, struct drm_mode_fb_cmd2 *mode_cmd)
+		struct drm_file *file_priv, const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	return drm_fb_cma_create(dev, file_priv, mode_cmd);
 }
@@ -733,10 +738,6 @@ static int atmel_hlcdc_dc_drm_probe(struct platform_device *pdev)
 	if (!ddev)
 		return -ENOMEM;
 
-	ret = drm_dev_set_unique(ddev, dev_name(ddev->dev));
-	if (ret)
-		goto err_unref;
-
 	ret = atmel_hlcdc_dc_load(ddev);
 	if (ret)
 		goto err_unref;
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
index 067e4c144bd6..0f7ec016e7a9 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_output.c
@@ -146,7 +146,7 @@ atmel_hlcdc_rgb_encoder_mode_set(struct drm_encoder *encoder,
 			   cfg);
 }
 
-static struct drm_encoder_helper_funcs atmel_hlcdc_panel_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs atmel_hlcdc_panel_encoder_helper_funcs = {
 	.mode_fixup = atmel_hlcdc_panel_encoder_mode_fixup,
 	.mode_set = atmel_hlcdc_rgb_encoder_mode_set,
 	.disable = atmel_hlcdc_panel_encoder_disable,
@@ -192,7 +192,7 @@ atmel_hlcdc_rgb_best_encoder(struct drm_connector *connector)
 	return &rgb->encoder;
 }
 
-static struct drm_connector_helper_funcs atmel_hlcdc_panel_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs atmel_hlcdc_panel_connector_helper_funcs = {
 	.get_modes = atmel_hlcdc_panel_get_modes,
 	.mode_valid = atmel_hlcdc_rgb_mode_valid,
 	.best_encoder = atmel_hlcdc_rgb_best_encoder,
@@ -256,7 +256,7 @@ static int atmel_hlcdc_create_panel_output(struct drm_device *dev,
 			       &atmel_hlcdc_panel_encoder_helper_funcs);
 	ret = drm_encoder_init(dev, &panel->base.encoder,
 			       &atmel_hlcdc_panel_encoder_funcs,
-			       DRM_MODE_ENCODER_LVDS);
+			       DRM_MODE_ENCODER_LVDS, NULL);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_plane.c b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_plane.c
index d0299aed517e..1ffe9c329c46 100644
--- a/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_plane.c
+++ b/drivers/gpu/drm/atmel-hlcdc/atmel_hlcdc_plane.c
@@ -941,7 +941,7 @@ atmel_hlcdc_plane_create(struct drm_device *dev,
 	ret = drm_universal_plane_init(dev, &plane->base, 0,
 				       &layer_plane_funcs,
 				       desc->formats->formats,
-				       desc->formats->nformats, type);
+				       desc->formats->nformats, type, NULL);
 	if (ret)
 		return ERR_PTR(ret);
 
diff --git a/drivers/gpu/drm/bochs/bochs.h b/drivers/gpu/drm/bochs/bochs.h
index 71f2687fc3cc..19b5adaebe24 100644
--- a/drivers/gpu/drm/bochs/bochs.h
+++ b/drivers/gpu/drm/bochs/bochs.h
@@ -149,7 +149,7 @@ int bochs_dumb_mmap_offset(struct drm_file *file, struct drm_device *dev,
 
 int bochs_framebuffer_init(struct drm_device *dev,
 			   struct bochs_framebuffer *gfb,
-			   struct drm_mode_fb_cmd2 *mode_cmd,
+			   const struct drm_mode_fb_cmd2 *mode_cmd,
 			   struct drm_gem_object *obj);
 int bochs_bo_pin(struct bochs_bo *bo, u32 pl_flag, u64 *gpu_addr);
 int bochs_bo_unpin(struct bochs_bo *bo);
diff --git a/drivers/gpu/drm/bochs/bochs_fbdev.c b/drivers/gpu/drm/bochs/bochs_fbdev.c
index 09a0637aab3e..7520bf81fc25 100644
--- a/drivers/gpu/drm/bochs/bochs_fbdev.c
+++ b/drivers/gpu/drm/bochs/bochs_fbdev.c
@@ -34,7 +34,7 @@ static struct fb_ops bochsfb_ops = {
 };
 
 static int bochsfb_create_object(struct bochs_device *bochs,
-				 struct drm_mode_fb_cmd2 *mode_cmd,
+				 const struct drm_mode_fb_cmd2 *mode_cmd,
 				 struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = bochs->dev;
diff --git a/drivers/gpu/drm/bochs/bochs_kms.c b/drivers/gpu/drm/bochs/bochs_kms.c
index 26bcd03a8cb6..2849f1b95eec 100644
--- a/drivers/gpu/drm/bochs/bochs_kms.c
+++ b/drivers/gpu/drm/bochs/bochs_kms.c
@@ -119,7 +119,7 @@ static int bochs_crtc_page_flip(struct drm_crtc *crtc,
 	bochs_crtc_mode_set_base(crtc, 0, 0, old_fb);
 	if (event) {
 		spin_lock_irqsave(&bochs->dev->event_lock, irqflags);
-		drm_send_vblank_event(bochs->dev, -1, event);
+		drm_crtc_send_vblank_event(crtc, event);
 		spin_unlock_irqrestore(&bochs->dev->event_lock, irqflags);
 	}
 	return 0;
@@ -196,7 +196,7 @@ static void bochs_encoder_init(struct drm_device *dev)
 
 	encoder->possible_crtcs = 0x1;
 	drm_encoder_init(dev, encoder, &bochs_encoder_encoder_funcs,
-			 DRM_MODE_ENCODER_DAC);
+			 DRM_MODE_ENCODER_DAC, NULL);
 	drm_encoder_helper_add(encoder, &bochs_encoder_helper_funcs);
 }
 
@@ -245,13 +245,13 @@ static enum drm_connector_status bochs_connector_detect(struct drm_connector
 	return connector_status_connected;
 }
 
-struct drm_connector_helper_funcs bochs_connector_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs bochs_connector_connector_helper_funcs = {
 	.get_modes = bochs_connector_get_modes,
 	.mode_valid = bochs_connector_mode_valid,
 	.best_encoder = bochs_connector_best_encoder,
 };
 
-struct drm_connector_funcs bochs_connector_connector_funcs = {
+static const struct drm_connector_funcs bochs_connector_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = bochs_connector_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -283,7 +283,7 @@ int bochs_kms_init(struct bochs_device *bochs)
 	bochs->dev->mode_config.preferred_depth = 24;
 	bochs->dev->mode_config.prefer_shadow = 0;
 
-	bochs->dev->mode_config.funcs = (void *)&bochs_mode_funcs;
+	bochs->dev->mode_config.funcs = &bochs_mode_funcs;
 
 	bochs_crtc_init(bochs->dev);
 	bochs_encoder_init(bochs->dev);
diff --git a/drivers/gpu/drm/bochs/bochs_mm.c b/drivers/gpu/drm/bochs/bochs_mm.c
index f69e6bf9bb0e..d812ad014da5 100644
--- a/drivers/gpu/drm/bochs/bochs_mm.c
+++ b/drivers/gpu/drm/bochs/bochs_mm.c
@@ -484,7 +484,7 @@ static const struct drm_framebuffer_funcs bochs_fb_funcs = {
 
 int bochs_framebuffer_init(struct drm_device *dev,
 			   struct bochs_framebuffer *gfb,
-			   struct drm_mode_fb_cmd2 *mode_cmd,
+			   const struct drm_mode_fb_cmd2 *mode_cmd,
 			   struct drm_gem_object *obj)
 {
 	int ret;
@@ -502,7 +502,7 @@ int bochs_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 bochs_user_framebuffer_create(struct drm_device *dev,
 			      struct drm_file *filp,
-			      struct drm_mode_fb_cmd2 *mode_cmd)
+			      const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct bochs_framebuffer *bochs_fb;
diff --git a/drivers/gpu/drm/bridge/Kconfig b/drivers/gpu/drm/bridge/Kconfig
index 6dddd392aa42..27e2022de89d 100644
--- a/drivers/gpu/drm/bridge/Kconfig
+++ b/drivers/gpu/drm/bridge/Kconfig
@@ -22,7 +22,6 @@ config DRM_DW_HDMI_AHB_AUDIO
 	  Designware HDMI block.  This is used in conjunction with
 	  the i.MX6 HDMI driver.
 
-
 config DRM_NXP_PTN3460
 	tristate "NXP PTN3460 DP/LVDS bridge"
 	depends on OF
diff --git a/drivers/gpu/drm/bridge/Makefile b/drivers/gpu/drm/bridge/Makefile
index d4e28beec30e..f13c33d67c03 100644
--- a/drivers/gpu/drm/bridge/Makefile
+++ b/drivers/gpu/drm/bridge/Makefile
@@ -1,6 +1,6 @@
 ccflags-y := -Iinclude/drm
 
-obj-$(CONFIG_DRM_DW_HDMI) += dw_hdmi.o
-obj-$(CONFIG_DRM_DW_HDMI_AHB_AUDIO) += dw_hdmi-ahb-audio.o
+obj-$(CONFIG_DRM_DW_HDMI) += dw-hdmi.o
+obj-$(CONFIG_DRM_DW_HDMI_AHB_AUDIO) += dw-hdmi-ahb-audio.o
 obj-$(CONFIG_DRM_NXP_PTN3460) += nxp-ptn3460.o
 obj-$(CONFIG_DRM_PARADE_PS8622) += parade-ps8622.o
diff --git a/drivers/gpu/drm/bridge/dw_hdmi-ahb-audio.c b/drivers/gpu/drm/bridge/dw-hdmi-ahb-audio.c
index 59f630f1c61a..122bb015f4a9 100644
--- a/drivers/gpu/drm/bridge/dw_hdmi-ahb-audio.c
+++ b/drivers/gpu/drm/bridge/dw-hdmi-ahb-audio.c
@@ -21,7 +21,7 @@
 #include <sound/pcm_drm_eld.h>
 #include <sound/pcm_iec958.h>
 
-#include "dw_hdmi-audio.h"
+#include "dw-hdmi-audio.h"
 
 #define DRIVER_NAME "dw-hdmi-ahb-audio"
 
diff --git a/drivers/gpu/drm/bridge/dw_hdmi-audio.h b/drivers/gpu/drm/bridge/dw-hdmi-audio.h
index 91f631beecc7..91f631beecc7 100644
--- a/drivers/gpu/drm/bridge/dw_hdmi-audio.h
+++ b/drivers/gpu/drm/bridge/dw-hdmi-audio.h
diff --git a/drivers/gpu/drm/bridge/dw_hdmi.c b/drivers/gpu/drm/bridge/dw-hdmi.c
index 56de9f1c95fc..b0aac4733020 100644
--- a/drivers/gpu/drm/bridge/dw_hdmi.c
+++ b/drivers/gpu/drm/bridge/dw-hdmi.c
@@ -22,13 +22,14 @@
 
 #include <drm/drm_of.h>
 #include <drm/drmP.h>
+#include <drm/drm_atomic_helper.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_edid.h>
 #include <drm/drm_encoder_slave.h>
 #include <drm/bridge/dw_hdmi.h>
 
-#include "dw_hdmi.h"
-#include "dw_hdmi-audio.h"
+#include "dw-hdmi.h"
+#include "dw-hdmi-audio.h"
 
 #define HDMI_EDID_LEN		512
 
@@ -1514,7 +1515,7 @@ static void dw_hdmi_connector_force(struct drm_connector *connector)
 	mutex_unlock(&hdmi->mutex);
 }
 
-static struct drm_connector_funcs dw_hdmi_connector_funcs = {
+static const struct drm_connector_funcs dw_hdmi_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = dw_hdmi_connector_detect,
@@ -1522,13 +1523,24 @@ static struct drm_connector_funcs dw_hdmi_connector_funcs = {
 	.force = dw_hdmi_connector_force,
 };
 
-static struct drm_connector_helper_funcs dw_hdmi_connector_helper_funcs = {
+static const struct drm_connector_funcs dw_hdmi_atomic_connector_funcs = {
+	.dpms = drm_atomic_helper_connector_dpms,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.detect = dw_hdmi_connector_detect,
+	.destroy = dw_hdmi_connector_destroy,
+	.force = dw_hdmi_connector_force,
+	.reset = drm_atomic_helper_connector_reset,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
+};
+
+static const struct drm_connector_helper_funcs dw_hdmi_connector_helper_funcs = {
 	.get_modes = dw_hdmi_connector_get_modes,
 	.mode_valid = dw_hdmi_connector_mode_valid,
 	.best_encoder = dw_hdmi_connector_best_encoder,
 };
 
-static struct drm_bridge_funcs dw_hdmi_bridge_funcs = {
+static const struct drm_bridge_funcs dw_hdmi_bridge_funcs = {
 	.enable = dw_hdmi_bridge_enable,
 	.disable = dw_hdmi_bridge_disable,
 	.pre_enable = dw_hdmi_bridge_nop,
@@ -1645,10 +1657,15 @@ static int dw_hdmi_register(struct drm_device *drm, struct dw_hdmi *hdmi)
 
 	drm_connector_helper_add(&hdmi->connector,
 				 &dw_hdmi_connector_helper_funcs);
-	drm_connector_init(drm, &hdmi->connector, &dw_hdmi_connector_funcs,
-			   DRM_MODE_CONNECTOR_HDMIA);
 
-	hdmi->connector.encoder = encoder;
+	if (drm_core_check_feature(drm, DRIVER_ATOMIC))
+		drm_connector_init(drm, &hdmi->connector,
+				   &dw_hdmi_atomic_connector_funcs,
+				   DRM_MODE_CONNECTOR_HDMIA);
+	else
+		drm_connector_init(drm, &hdmi->connector,
+				   &dw_hdmi_connector_funcs,
+				   DRM_MODE_CONNECTOR_HDMIA);
 
 	drm_mode_connector_attach_encoder(&hdmi->connector, encoder);
 
diff --git a/drivers/gpu/drm/bridge/dw_hdmi.h b/drivers/gpu/drm/bridge/dw-hdmi.h
index fc9a560429d6..fc9a560429d6 100644
--- a/drivers/gpu/drm/bridge/dw_hdmi.h
+++ b/drivers/gpu/drm/bridge/dw-hdmi.h
diff --git a/drivers/gpu/drm/bridge/nxp-ptn3460.c b/drivers/gpu/drm/bridge/nxp-ptn3460.c
index 0ffa3a6a206a..7ecd59f70b8e 100644
--- a/drivers/gpu/drm/bridge/nxp-ptn3460.c
+++ b/drivers/gpu/drm/bridge/nxp-ptn3460.c
@@ -242,7 +242,7 @@ static struct drm_encoder *ptn3460_best_encoder(struct drm_connector *connector)
 	return ptn_bridge->bridge.encoder;
 }
 
-static struct drm_connector_helper_funcs ptn3460_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs ptn3460_connector_helper_funcs = {
 	.get_modes = ptn3460_get_modes,
 	.best_encoder = ptn3460_best_encoder,
 };
@@ -258,7 +258,7 @@ static void ptn3460_connector_destroy(struct drm_connector *connector)
 	drm_connector_cleanup(connector);
 }
 
-static struct drm_connector_funcs ptn3460_connector_funcs = {
+static const struct drm_connector_funcs ptn3460_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = ptn3460_detect,
@@ -299,7 +299,7 @@ static int ptn3460_bridge_attach(struct drm_bridge *bridge)
 	return ret;
 }
 
-static struct drm_bridge_funcs ptn3460_bridge_funcs = {
+static const struct drm_bridge_funcs ptn3460_bridge_funcs = {
 	.pre_enable = ptn3460_pre_enable,
 	.enable = ptn3460_enable,
 	.disable = ptn3460_disable,
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
index 705061537a27..b774d637a00f 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.h
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -153,7 +153,6 @@ struct cirrus_device {
 struct cirrus_fbdev {
 	struct drm_fb_helper helper;
 	struct cirrus_framebuffer gfb;
-	struct list_head fbdev_list;
 	void *sysram;
 	int size;
 	int x1, y1, x2, y2; /* dirty rect */
@@ -207,7 +206,7 @@ int cirrus_dumb_create(struct drm_file *file,
 
 int cirrus_framebuffer_init(struct drm_device *dev,
 			   struct cirrus_framebuffer *gfb,
-			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    const struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct drm_gem_object *obj);
 
 bool cirrus_check_framebuffer(struct cirrus_device *cdev, int width, int height,
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
index 589103bcc06c..3b5be7272357 100644
--- a/drivers/gpu/drm/cirrus/cirrus_fbdev.c
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -135,7 +135,7 @@ static struct fb_ops cirrusfb_ops = {
 };
 
 static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
-			       struct drm_mode_fb_cmd2 *mode_cmd,
+			       const struct drm_mode_fb_cmd2 *mode_cmd,
 			       struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = afbdev->helper.dev;
diff --git a/drivers/gpu/drm/cirrus/cirrus_main.c b/drivers/gpu/drm/cirrus/cirrus_main.c
index 055fd86ba717..0907715e90fd 100644
--- a/drivers/gpu/drm/cirrus/cirrus_main.c
+++ b/drivers/gpu/drm/cirrus/cirrus_main.c
@@ -29,7 +29,7 @@ static const struct drm_framebuffer_funcs cirrus_fb_funcs = {
 
 int cirrus_framebuffer_init(struct drm_device *dev,
 			    struct cirrus_framebuffer *gfb,
-			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    const struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct drm_gem_object *obj)
 {
 	int ret;
@@ -47,7 +47,7 @@ int cirrus_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 cirrus_user_framebuffer_create(struct drm_device *dev,
 			       struct drm_file *filp,
-			       struct drm_mode_fb_cmd2 *mode_cmd)
+			       const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct cirrus_device *cdev = dev->dev_private;
 	struct drm_gem_object *obj;
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
index 61385f2298bf..4a02854a6963 100644
--- a/drivers/gpu/drm/cirrus/cirrus_mode.c
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -489,7 +489,7 @@ static struct drm_encoder *cirrus_encoder_init(struct drm_device *dev)
 	encoder->possible_crtcs = 0x1;
 
 	drm_encoder_init(dev, encoder, &cirrus_encoder_encoder_funcs,
-			 DRM_MODE_ENCODER_DAC);
+			 DRM_MODE_ENCODER_DAC, NULL);
 	drm_encoder_helper_add(encoder, &cirrus_encoder_helper_funcs);
 
 	return encoder;
@@ -533,12 +533,12 @@ static void cirrus_connector_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
-struct drm_connector_helper_funcs cirrus_vga_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs cirrus_vga_connector_helper_funcs = {
 	.get_modes = cirrus_vga_get_modes,
 	.best_encoder = cirrus_connector_best_encoder,
 };
 
-struct drm_connector_funcs cirrus_vga_connector_funcs = {
+static const struct drm_connector_funcs cirrus_vga_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = cirrus_vga_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
diff --git a/drivers/gpu/drm/drm_atomic.c b/drivers/gpu/drm/drm_atomic.c
index aeee083c7f95..9a7b44616b55 100644
--- a/drivers/gpu/drm/drm_atomic.c
+++ b/drivers/gpu/drm/drm_atomic.c
@@ -65,8 +65,6 @@ drm_atomic_state_init(struct drm_device *dev, struct drm_atomic_state *state)
 	 */
 	state->allow_modeset = true;
 
-	state->num_connector = ACCESS_ONCE(dev->mode_config.num_connector);
-
 	state->crtcs = kcalloc(dev->mode_config.num_crtc,
 			       sizeof(*state->crtcs), GFP_KERNEL);
 	if (!state->crtcs)
@@ -83,16 +81,6 @@ drm_atomic_state_init(struct drm_device *dev, struct drm_atomic_state *state)
 				      sizeof(*state->plane_states), GFP_KERNEL);
 	if (!state->plane_states)
 		goto fail;
-	state->connectors = kcalloc(state->num_connector,
-				    sizeof(*state->connectors),
-				    GFP_KERNEL);
-	if (!state->connectors)
-		goto fail;
-	state->connector_states = kcalloc(state->num_connector,
-					  sizeof(*state->connector_states),
-					  GFP_KERNEL);
-	if (!state->connector_states)
-		goto fail;
 
 	state->dev = dev;
 
@@ -288,8 +276,8 @@ drm_atomic_get_crtc_state(struct drm_atomic_state *state,
 	state->crtcs[index] = crtc;
 	crtc_state->state = state;
 
-	DRM_DEBUG_ATOMIC("Added [CRTC:%d] %p state to %p\n",
-			 crtc->base.id, crtc_state, state);
+	DRM_DEBUG_ATOMIC("Added [CRTC:%d:%s] %p state to %p\n",
+			 crtc->base.id, crtc->name, crtc_state, state);
 
 	return crtc_state;
 }
@@ -316,8 +304,7 @@ int drm_atomic_set_mode_for_crtc(struct drm_crtc_state *state,
 	if (mode && memcmp(&state->mode, mode, sizeof(*mode)) == 0)
 		return 0;
 
-	if (state->mode_blob)
-		drm_property_unreference_blob(state->mode_blob);
+	drm_property_unreference_blob(state->mode_blob);
 	state->mode_blob = NULL;
 
 	if (mode) {
@@ -363,8 +350,7 @@ int drm_atomic_set_mode_prop_for_crtc(struct drm_crtc_state *state,
 	if (blob == state->mode_blob)
 		return 0;
 
-	if (state->mode_blob)
-		drm_property_unreference_blob(state->mode_blob);
+	drm_property_unreference_blob(state->mode_blob);
 	state->mode_blob = NULL;
 
 	if (blob) {
@@ -419,8 +405,7 @@ int drm_atomic_crtc_set_property(struct drm_crtc *crtc,
 		struct drm_property_blob *mode =
 			drm_property_lookup_blob(dev, val);
 		ret = drm_atomic_set_mode_prop_for_crtc(state, mode);
-		if (mode)
-			drm_property_unreference_blob(mode);
+		drm_property_unreference_blob(mode);
 		return ret;
 	}
 	else if (crtc->funcs->atomic_set_property)
@@ -432,11 +417,20 @@ int drm_atomic_crtc_set_property(struct drm_crtc *crtc,
 }
 EXPORT_SYMBOL(drm_atomic_crtc_set_property);
 
-/*
+/**
+ * drm_atomic_crtc_get_property - get property value from CRTC state
+ * @crtc: the drm CRTC to set a property on
+ * @state: the state object to get the property value from
+ * @property: the property to set
+ * @val: return location for the property value
+ *
  * This function handles generic/core properties and calls out to
  * driver's ->atomic_get_property() for driver properties.  To ensure
  * consistent behavior you must call this function rather than the
  * driver hook directly.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
  */
 static int
 drm_atomic_crtc_get_property(struct drm_crtc *crtc,
@@ -480,8 +474,8 @@ static int drm_atomic_crtc_check(struct drm_crtc *crtc,
 	 */
 
 	if (state->active && !state->enable) {
-		DRM_DEBUG_ATOMIC("[CRTC:%d] active without enabled\n",
-				 crtc->base.id);
+		DRM_DEBUG_ATOMIC("[CRTC:%d:%s] active without enabled\n",
+				 crtc->base.id, crtc->name);
 		return -EINVAL;
 	}
 
@@ -490,14 +484,30 @@ static int drm_atomic_crtc_check(struct drm_crtc *crtc,
 	 * be able to trigger. */
 	if (drm_core_check_feature(crtc->dev, DRIVER_ATOMIC) &&
 	    WARN_ON(state->enable && !state->mode_blob)) {
-		DRM_DEBUG_ATOMIC("[CRTC:%d] enabled without mode blob\n",
-				 crtc->base.id);
+		DRM_DEBUG_ATOMIC("[CRTC:%d:%s] enabled without mode blob\n",
+				 crtc->base.id, crtc->name);
 		return -EINVAL;
 	}
 
 	if (drm_core_check_feature(crtc->dev, DRIVER_ATOMIC) &&
 	    WARN_ON(!state->enable && state->mode_blob)) {
-		DRM_DEBUG_ATOMIC("[CRTC:%d] disabled with mode blob\n",
+		DRM_DEBUG_ATOMIC("[CRTC:%d:%s] disabled with mode blob\n",
+				 crtc->base.id, crtc->name);
+		return -EINVAL;
+	}
+
+	/*
+	 * Reject event generation for when a CRTC is off and stays off.
+	 * It wouldn't be hard to implement this, but userspace has a track
+	 * record of happily burning through 100% cpu (or worse, crash) when the
+	 * display pipe is suspended. To avoid all that fun just reject updates
+	 * that ask for events since likely that indicates a bug in the
+	 * compositor's drawing loop. This is consistent with the vblank IOCTL
+	 * and legacy page_flip IOCTL which also reject service on a disabled
+	 * pipe.
+	 */
+	if (state->event && !state->active && !crtc->state->active) {
+		DRM_DEBUG_ATOMIC("[CRTC:%d] requesting event but off\n",
 				 crtc->base.id);
 		return -EINVAL;
 	}
@@ -543,8 +553,8 @@ drm_atomic_get_plane_state(struct drm_atomic_state *state,
 	state->planes[index] = plane;
 	plane_state->state = state;
 
-	DRM_DEBUG_ATOMIC("Added [PLANE:%d] %p state to %p\n",
-			 plane->base.id, plane_state, state);
+	DRM_DEBUG_ATOMIC("Added [PLANE:%d:%s] %p state to %p\n",
+			 plane->base.id, plane->name, plane_state, state);
 
 	if (plane_state->crtc) {
 		struct drm_crtc_state *crtc_state;
@@ -619,11 +629,20 @@ int drm_atomic_plane_set_property(struct drm_plane *plane,
 }
 EXPORT_SYMBOL(drm_atomic_plane_set_property);
 
-/*
+/**
+ * drm_atomic_plane_get_property - get property value from plane state
+ * @plane: the drm plane to set a property on
+ * @state: the state object to get the property value from
+ * @property: the property to set
+ * @val: return location for the property value
+ *
  * This function handles generic/core properties and calls out to
  * driver's ->atomic_get_property() for driver properties.  To ensure
  * consistent behavior you must call this function rather than the
  * driver hook directly.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
  */
 static int
 drm_atomic_plane_get_property(struct drm_plane *plane,
@@ -755,8 +774,8 @@ static int drm_atomic_plane_check(struct drm_plane *plane,
 	}
 
 	if (plane_switching_crtc(state->state, plane, state)) {
-		DRM_DEBUG_ATOMIC("[PLANE:%d] switching CRTC directly\n",
-				 plane->base.id);
+		DRM_DEBUG_ATOMIC("[PLANE:%d:%s] switching CRTC directly\n",
+				 plane->base.id, plane->name);
 		return -EINVAL;
 	}
 
@@ -792,19 +811,27 @@ drm_atomic_get_connector_state(struct drm_atomic_state *state,
 
 	index = drm_connector_index(connector);
 
-	/*
-	 * Construction of atomic state updates can race with a connector
-	 * hot-add which might overflow. In this case flip the table and just
-	 * restart the entire ioctl - no one is fast enough to livelock a cpu
-	 * with physical hotplug events anyway.
-	 *
-	 * Note that we only grab the indexes once we have the right lock to
-	 * prevent hotplug/unplugging of connectors. So removal is no problem,
-	 * at most the array is a bit too large.
-	 */
 	if (index >= state->num_connector) {
-		DRM_DEBUG_ATOMIC("Hot-added connector would overflow state array, restarting\n");
-		return ERR_PTR(-EAGAIN);
+		struct drm_connector **c;
+		struct drm_connector_state **cs;
+		int alloc = max(index + 1, config->num_connector);
+
+		c = krealloc(state->connectors, alloc * sizeof(*state->connectors), GFP_KERNEL);
+		if (!c)
+			return ERR_PTR(-ENOMEM);
+
+		state->connectors = c;
+		memset(&state->connectors[state->num_connector], 0,
+		       sizeof(*state->connectors) * (alloc - state->num_connector));
+
+		cs = krealloc(state->connector_states, alloc * sizeof(*state->connector_states), GFP_KERNEL);
+		if (!cs)
+			return ERR_PTR(-ENOMEM);
+
+		state->connector_states = cs;
+		memset(&state->connector_states[state->num_connector], 0,
+		       sizeof(*state->connector_states) * (alloc - state->num_connector));
+		state->num_connector = alloc;
 	}
 
 	if (state->connector_states[index])
@@ -875,11 +902,20 @@ int drm_atomic_connector_set_property(struct drm_connector *connector,
 }
 EXPORT_SYMBOL(drm_atomic_connector_set_property);
 
-/*
+/**
+ * drm_atomic_connector_get_property - get property value from connector state
+ * @connector: the drm connector to set a property on
+ * @state: the state object to get the property value from
+ * @property: the property to set
+ * @val: return location for the property value
+ *
  * This function handles generic/core properties and calls out to
  * driver's ->atomic_get_property() for driver properties.  To ensure
  * consistent behavior you must call this function rather than the
  * driver hook directly.
+ *
+ * RETURNS:
+ * Zero on success, error code on failure
  */
 static int
 drm_atomic_connector_get_property(struct drm_connector *connector,
@@ -980,8 +1016,8 @@ drm_atomic_set_crtc_for_plane(struct drm_plane_state *plane_state,
 	}
 
 	if (crtc)
-		DRM_DEBUG_ATOMIC("Link plane state %p to [CRTC:%d]\n",
-				 plane_state, crtc->base.id);
+		DRM_DEBUG_ATOMIC("Link plane state %p to [CRTC:%d:%s]\n",
+				 plane_state, crtc->base.id, crtc->name);
 	else
 		DRM_DEBUG_ATOMIC("Link plane state %p to [NOCRTC]\n",
 				 plane_state);
@@ -1039,17 +1075,28 @@ drm_atomic_set_crtc_for_connector(struct drm_connector_state *conn_state,
 {
 	struct drm_crtc_state *crtc_state;
 
+	if (conn_state->crtc && conn_state->crtc != crtc) {
+		crtc_state = drm_atomic_get_existing_crtc_state(conn_state->state,
+								conn_state->crtc);
+
+		crtc_state->connector_mask &=
+			~(1 << drm_connector_index(conn_state->connector));
+	}
+
 	if (crtc) {
 		crtc_state = drm_atomic_get_crtc_state(conn_state->state, crtc);
 		if (IS_ERR(crtc_state))
 			return PTR_ERR(crtc_state);
+
+		crtc_state->connector_mask |=
+			1 << drm_connector_index(conn_state->connector);
 	}
 
 	conn_state->crtc = crtc;
 
 	if (crtc)
-		DRM_DEBUG_ATOMIC("Link connector state %p to [CRTC:%d]\n",
-				 conn_state, crtc->base.id);
+		DRM_DEBUG_ATOMIC("Link connector state %p to [CRTC:%d:%s]\n",
+				 conn_state, crtc->base.id, crtc->name);
 	else
 		DRM_DEBUG_ATOMIC("Link connector state %p to [NOCRTC]\n",
 				 conn_state);
@@ -1088,8 +1135,8 @@ drm_atomic_add_affected_connectors(struct drm_atomic_state *state,
 	if (ret)
 		return ret;
 
-	DRM_DEBUG_ATOMIC("Adding all current connectors for [CRTC:%d] to %p\n",
-			 crtc->base.id, state);
+	DRM_DEBUG_ATOMIC("Adding all current connectors for [CRTC:%d:%s] to %p\n",
+			 crtc->base.id, crtc->name, state);
 
 	/*
 	 * Changed connectors are already in @state, so only need to look at the
@@ -1148,35 +1195,6 @@ drm_atomic_add_affected_planes(struct drm_atomic_state *state,
 EXPORT_SYMBOL(drm_atomic_add_affected_planes);
 
 /**
- * drm_atomic_connectors_for_crtc - count number of connected outputs
- * @state: atomic state
- * @crtc: DRM crtc
- *
- * This function counts all connectors which will be connected to @crtc
- * according to @state. Useful to recompute the enable state for @crtc.
- */
-int
-drm_atomic_connectors_for_crtc(struct drm_atomic_state *state,
-			       struct drm_crtc *crtc)
-{
-	struct drm_connector *connector;
-	struct drm_connector_state *conn_state;
-
-	int i, num_connected_connectors = 0;
-
-	for_each_connector_in_state(state, connector, conn_state, i) {
-		if (conn_state->crtc == crtc)
-			num_connected_connectors++;
-	}
-
-	DRM_DEBUG_ATOMIC("State %p has %i connectors for [CRTC:%d]\n",
-			 state, num_connected_connectors, crtc->base.id);
-
-	return num_connected_connectors;
-}
-EXPORT_SYMBOL(drm_atomic_connectors_for_crtc);
-
-/**
  * drm_atomic_legacy_backoff - locking backoff for legacy ioctls
  * @state: atomic state
  *
@@ -1191,12 +1209,7 @@ void drm_atomic_legacy_backoff(struct drm_atomic_state *state)
 retry:
 	drm_modeset_backoff(state->acquire_ctx);
 
-	ret = drm_modeset_lock(&state->dev->mode_config.connection_mutex,
-			       state->acquire_ctx);
-	if (ret)
-		goto retry;
-	ret = drm_modeset_lock_all_crtcs(state->dev,
-					 state->acquire_ctx);
+	ret = drm_modeset_lock_all_ctx(state->dev, state->acquire_ctx);
 	if (ret)
 		goto retry;
 }
@@ -1228,8 +1241,8 @@ int drm_atomic_check_only(struct drm_atomic_state *state)
 	for_each_plane_in_state(state, plane, plane_state, i) {
 		ret = drm_atomic_plane_check(plane, plane_state);
 		if (ret) {
-			DRM_DEBUG_ATOMIC("[PLANE:%d] atomic core check failed\n",
-					 plane->base.id);
+			DRM_DEBUG_ATOMIC("[PLANE:%d:%s] atomic core check failed\n",
+					 plane->base.id, plane->name);
 			return ret;
 		}
 	}
@@ -1237,8 +1250,8 @@ int drm_atomic_check_only(struct drm_atomic_state *state)
 	for_each_crtc_in_state(state, crtc, crtc_state, i) {
 		ret = drm_atomic_crtc_check(crtc, crtc_state);
 		if (ret) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] atomic core check failed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] atomic core check failed\n",
+					 crtc->base.id, crtc->name);
 			return ret;
 		}
 	}
@@ -1249,8 +1262,8 @@ int drm_atomic_check_only(struct drm_atomic_state *state)
 	if (!state->allow_modeset) {
 		for_each_crtc_in_state(state, crtc, crtc_state, i) {
 			if (drm_atomic_crtc_needs_modeset(crtc_state)) {
-				DRM_DEBUG_ATOMIC("[CRTC:%d] requires full modeset\n",
-						 crtc->base.id);
+				DRM_DEBUG_ATOMIC("[CRTC:%d:%s] requires full modeset\n",
+						 crtc->base.id, crtc->name);
 				return -EINVAL;
 			}
 		}
@@ -1433,7 +1446,7 @@ static int atomic_set_prop(struct drm_atomic_state *state,
 }
 
 /**
- * drm_atomic_update_old_fb -- Unset old_fb pointers and set plane->fb pointers.
+ * drm_atomic_clean_old_fb -- Unset old_fb pointers and set plane->fb pointers.
  *
  * @dev: drm device to check.
  * @plane_mask: plane mask for planes that were updated.
diff --git a/drivers/gpu/drm/drm_atomic_helper.c b/drivers/gpu/drm/drm_atomic_helper.c
index e5aec45bf985..4f2d3e161593 100644
--- a/drivers/gpu/drm/drm_atomic_helper.c
+++ b/drivers/gpu/drm/drm_atomic_helper.c
@@ -52,6 +52,12 @@
  * drm_atomic_helper_disable_plane(), drm_atomic_helper_disable_plane() and the
  * various functions to implement set_property callbacks. New drivers must not
  * implement these functions themselves but must use the provided helpers.
+ *
+ * The atomic helper uses the same function table structures as all other
+ * modesetting helpers. See the documentation for struct &drm_crtc_helper_funcs,
+ * struct &drm_encoder_helper_funcs and struct &drm_connector_helper_funcs. It
+ * also shares the struct &drm_plane_helper_funcs function table with the plane
+ * helpers.
  */
 static void
 drm_atomic_helper_plane_changed(struct drm_atomic_state *state,
@@ -80,6 +86,26 @@ drm_atomic_helper_plane_changed(struct drm_atomic_state *state,
 	}
 }
 
+static bool
+check_pending_encoder_assignment(struct drm_atomic_state *state,
+				 struct drm_encoder *new_encoder)
+{
+	struct drm_connector *connector;
+	struct drm_connector_state *conn_state;
+	int i;
+
+	for_each_connector_in_state(state, connector, conn_state, i) {
+		if (conn_state->best_encoder != new_encoder)
+			continue;
+
+		/* encoder already assigned and we're trying to re-steal it! */
+		if (connector->state->best_encoder != conn_state->best_encoder)
+			return false;
+	}
+
+	return true;
+}
+
 static struct drm_crtc *
 get_current_crtc_for_encoder(struct drm_device *dev,
 			     struct drm_encoder *encoder)
@@ -116,9 +142,9 @@ steal_encoder(struct drm_atomic_state *state,
 	 */
 	WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
 
-	DRM_DEBUG_ATOMIC("[ENCODER:%d:%s] in use on [CRTC:%d], stealing it\n",
+	DRM_DEBUG_ATOMIC("[ENCODER:%d:%s] in use on [CRTC:%d:%s], stealing it\n",
 			 encoder->base.id, encoder->name,
-			 encoder_crtc->base.id);
+			 encoder_crtc->base.id, encoder_crtc->name);
 
 	crtc_state = drm_atomic_get_crtc_state(state, encoder_crtc);
 	if (IS_ERR(crtc_state))
@@ -219,16 +245,24 @@ update_connector_routing(struct drm_atomic_state *state, int conn_idx)
 	}
 
 	if (new_encoder == connector_state->best_encoder) {
-		DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] keeps [ENCODER:%d:%s], now on [CRTC:%d]\n",
+		DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] keeps [ENCODER:%d:%s], now on [CRTC:%d:%s]\n",
 				 connector->base.id,
 				 connector->name,
 				 new_encoder->base.id,
 				 new_encoder->name,
-				 connector_state->crtc->base.id);
+				 connector_state->crtc->base.id,
+				 connector_state->crtc->name);
 
 		return 0;
 	}
 
+	if (!check_pending_encoder_assignment(state, new_encoder)) {
+		DRM_DEBUG_ATOMIC("Encoder for [CONNECTOR:%d:%s] already assigned\n",
+				 connector->base.id,
+				 connector->name);
+		return -EINVAL;
+	}
+
 	encoder_crtc = get_current_crtc_for_encoder(state->dev,
 						    new_encoder);
 
@@ -251,12 +285,13 @@ update_connector_routing(struct drm_atomic_state *state, int conn_idx)
 	crtc_state = state->crtc_states[idx];
 	crtc_state->connectors_changed = true;
 
-	DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] using [ENCODER:%d:%s] on [CRTC:%d]\n",
+	DRM_DEBUG_ATOMIC("[CONNECTOR:%d:%s] using [ENCODER:%d:%s] on [CRTC:%d:%s]\n",
 			 connector->base.id,
 			 connector->name,
 			 new_encoder->base.id,
 			 new_encoder->name,
-			 connector_state->crtc->base.id);
+			 connector_state->crtc->base.id,
+			 connector_state->crtc->name);
 
 	return 0;
 }
@@ -340,8 +375,8 @@ mode_fixup(struct drm_atomic_state *state)
 		ret = funcs->mode_fixup(crtc, &crtc_state->mode,
 					&crtc_state->adjusted_mode);
 		if (!ret) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] fixup failed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] fixup failed\n",
+					 crtc->base.id, crtc->name);
 			return -EINVAL;
 		}
 	}
@@ -388,14 +423,14 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 
 	for_each_crtc_in_state(state, crtc, crtc_state, i) {
 		if (!drm_mode_equal(&crtc->state->mode, &crtc_state->mode)) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] mode changed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] mode changed\n",
+					 crtc->base.id, crtc->name);
 			crtc_state->mode_changed = true;
 		}
 
 		if (crtc->state->enable != crtc_state->enable) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] enable changed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] enable changed\n",
+					 crtc->base.id, crtc->name);
 
 			/*
 			 * For clarity this assignment is done here, but
@@ -428,7 +463,8 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 	 * crtc only changed its mode but has the same set of connectors.
 	 */
 	for_each_crtc_in_state(state, crtc, crtc_state, i) {
-		int num_connectors;
+		bool has_connectors =
+			!!crtc_state->connector_mask;
 
 		/*
 		 * We must set ->active_changed after walking connectors for
@@ -436,18 +472,18 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 		 * a full modeset because update_connector_routing force that.
 		 */
 		if (crtc->state->active != crtc_state->active) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] active changed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] active changed\n",
+					 crtc->base.id, crtc->name);
 			crtc_state->active_changed = true;
 		}
 
 		if (!drm_atomic_crtc_needs_modeset(crtc_state))
 			continue;
 
-		DRM_DEBUG_ATOMIC("[CRTC:%d] needs all connectors, enable: %c, active: %c\n",
-				 crtc->base.id,
+		DRM_DEBUG_ATOMIC("[CRTC:%d:%s] needs all connectors, enable: %c, active: %c\n",
+				 crtc->base.id, crtc->name,
 				 crtc_state->enable ? 'y' : 'n',
-			      crtc_state->active ? 'y' : 'n');
+				 crtc_state->active ? 'y' : 'n');
 
 		ret = drm_atomic_add_affected_connectors(state, crtc);
 		if (ret != 0)
@@ -457,12 +493,9 @@ drm_atomic_helper_check_modeset(struct drm_device *dev,
 		if (ret != 0)
 			return ret;
 
-		num_connectors = drm_atomic_connectors_for_crtc(state,
-								crtc);
-
-		if (crtc_state->enable != !!num_connectors) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] enabled/connectors mismatch\n",
-					 crtc->base.id);
+		if (crtc_state->enable != has_connectors) {
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] enabled/connectors mismatch\n",
+					 crtc->base.id, crtc->name);
 
 			return -EINVAL;
 		}
@@ -509,8 +542,8 @@ drm_atomic_helper_check_planes(struct drm_device *dev,
 
 		ret = funcs->atomic_check(plane, plane_state);
 		if (ret) {
-			DRM_DEBUG_ATOMIC("[PLANE:%d] atomic driver check failed\n",
-					 plane->base.id);
+			DRM_DEBUG_ATOMIC("[PLANE:%d:%s] atomic driver check failed\n",
+					 plane->base.id, plane->name);
 			return ret;
 		}
 	}
@@ -525,8 +558,8 @@ drm_atomic_helper_check_planes(struct drm_device *dev,
 
 		ret = funcs->atomic_check(crtc, state->crtc_states[i]);
 		if (ret) {
-			DRM_DEBUG_ATOMIC("[CRTC:%d] atomic driver check failed\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("[CRTC:%d:%s] atomic driver check failed\n",
+					 crtc->base.id, crtc->name);
 			return ret;
 		}
 	}
@@ -639,8 +672,8 @@ disable_outputs(struct drm_device *dev, struct drm_atomic_state *old_state)
 
 		funcs = crtc->helper_private;
 
-		DRM_DEBUG_ATOMIC("disabling [CRTC:%d]\n",
-				 crtc->base.id);
+		DRM_DEBUG_ATOMIC("disabling [CRTC:%d:%s]\n",
+				 crtc->base.id, crtc->name);
 
 
 		/* Right function depends upon target state. */
@@ -751,8 +784,8 @@ crtc_set_mode(struct drm_device *dev, struct drm_atomic_state *old_state)
 		funcs = crtc->helper_private;
 
 		if (crtc->state->enable && funcs->mode_set_nofb) {
-			DRM_DEBUG_ATOMIC("modeset on [CRTC:%d]\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("modeset on [CRTC:%d:%s]\n",
+					 crtc->base.id, crtc->name);
 
 			funcs->mode_set_nofb(crtc);
 		}
@@ -851,8 +884,8 @@ void drm_atomic_helper_commit_modeset_enables(struct drm_device *dev,
 		funcs = crtc->helper_private;
 
 		if (crtc->state->enable) {
-			DRM_DEBUG_ATOMIC("enabling [CRTC:%d]\n",
-					 crtc->base.id);
+			DRM_DEBUG_ATOMIC("enabling [CRTC:%d:%s]\n",
+					 crtc->base.id, crtc->name);
 
 			if (funcs->enable)
 				funcs->enable(crtc);
@@ -913,9 +946,23 @@ static void wait_for_fences(struct drm_device *dev,
 	}
 }
 
-static bool framebuffer_changed(struct drm_device *dev,
-				struct drm_atomic_state *old_state,
-				struct drm_crtc *crtc)
+/**
+ * drm_atomic_helper_framebuffer_changed - check if framebuffer has changed
+ * @dev: DRM device
+ * @old_state: atomic state object with old state structures
+ * @crtc: DRM crtc
+ *
+ * Checks whether the framebuffer used for this CRTC changes as a result of
+ * the atomic update.  This is useful for drivers which cannot use
+ * drm_atomic_helper_wait_for_vblanks() and need to reimplement its
+ * functionality.
+ *
+ * Returns:
+ * true if the framebuffer changed.
+ */
+bool drm_atomic_helper_framebuffer_changed(struct drm_device *dev,
+					   struct drm_atomic_state *old_state,
+					   struct drm_crtc *crtc)
 {
 	struct drm_plane *plane;
 	struct drm_plane_state *old_plane_state;
@@ -932,6 +979,7 @@ static bool framebuffer_changed(struct drm_device *dev,
 
 	return false;
 }
+EXPORT_SYMBOL(drm_atomic_helper_framebuffer_changed);
 
 /**
  * drm_atomic_helper_wait_for_vblanks - wait for vblank on crtcs
@@ -966,7 +1014,8 @@ drm_atomic_helper_wait_for_vblanks(struct drm_device *dev,
 		if (old_state->legacy_cursor_update)
 			continue;
 
-		if (!framebuffer_changed(dev, old_state, crtc))
+		if (!drm_atomic_helper_framebuffer_changed(dev,
+				old_state, crtc))
 			continue;
 
 		ret = drm_crtc_vblank_get(crtc);
@@ -1342,6 +1391,49 @@ drm_atomic_helper_commit_planes_on_crtc(struct drm_crtc_state *old_crtc_state)
 EXPORT_SYMBOL(drm_atomic_helper_commit_planes_on_crtc);
 
 /**
+ * drm_atomic_helper_disable_planes_on_crtc - helper to disable CRTC's planes
+ * @crtc: CRTC
+ * @atomic: if set, synchronize with CRTC's atomic_begin/flush hooks
+ *
+ * Disables all planes associated with the given CRTC. This can be
+ * used for instance in the CRTC helper disable callback to disable
+ * all planes before shutting down the display pipeline.
+ *
+ * If the atomic-parameter is set the function calls the CRTC's
+ * atomic_begin hook before and atomic_flush hook after disabling the
+ * planes.
+ *
+ * It is a bug to call this function without having implemented the
+ * ->atomic_disable() plane hook.
+ */
+void drm_atomic_helper_disable_planes_on_crtc(struct drm_crtc *crtc,
+					      bool atomic)
+{
+	const struct drm_crtc_helper_funcs *crtc_funcs =
+		crtc->helper_private;
+	struct drm_plane *plane;
+
+	if (atomic && crtc_funcs && crtc_funcs->atomic_begin)
+		crtc_funcs->atomic_begin(crtc, NULL);
+
+	drm_for_each_plane(plane, crtc->dev) {
+		const struct drm_plane_helper_funcs *plane_funcs =
+			plane->helper_private;
+
+		if (plane->state->crtc != crtc || !plane_funcs)
+			continue;
+
+		WARN_ON(!plane_funcs->atomic_disable);
+		if (plane_funcs->atomic_disable)
+			plane_funcs->atomic_disable(plane, NULL);
+	}
+
+	if (atomic && crtc_funcs && crtc_funcs->atomic_flush)
+		crtc_funcs->atomic_flush(crtc, NULL);
+}
+EXPORT_SYMBOL(drm_atomic_helper_disable_planes_on_crtc);
+
+/**
  * drm_atomic_helper_cleanup_planes - cleanup plane resources after commit
  * @dev: DRM device
  * @old_state: atomic state object with old state structures
@@ -1401,7 +1493,7 @@ void drm_atomic_helper_swap_state(struct drm_device *dev,
 {
 	int i;
 
-	for (i = 0; i < dev->mode_config.num_connector; i++) {
+	for (i = 0; i < state->num_connector; i++) {
 		struct drm_connector *connector = state->connectors[i];
 
 		if (!connector)
@@ -1485,12 +1577,12 @@ retry:
 	drm_atomic_set_fb_for_plane(plane_state, fb);
 	plane_state->crtc_x = crtc_x;
 	plane_state->crtc_y = crtc_y;
-	plane_state->crtc_h = crtc_h;
 	plane_state->crtc_w = crtc_w;
+	plane_state->crtc_h = crtc_h;
 	plane_state->src_x = src_x;
 	plane_state->src_y = src_y;
-	plane_state->src_h = src_h;
 	plane_state->src_w = src_w;
+	plane_state->src_h = src_h;
 
 	if (plane == crtc->cursor)
 		state->legacy_cursor_update = true;
@@ -1609,12 +1701,12 @@ int __drm_atomic_helper_disable_plane(struct drm_plane *plane,
 	drm_atomic_set_fb_for_plane(plane_state, NULL);
 	plane_state->crtc_x = 0;
 	plane_state->crtc_y = 0;
-	plane_state->crtc_h = 0;
 	plane_state->crtc_w = 0;
+	plane_state->crtc_h = 0;
 	plane_state->src_x = 0;
 	plane_state->src_y = 0;
-	plane_state->src_h = 0;
 	plane_state->src_w = 0;
+	plane_state->src_h = 0;
 
 	return 0;
 }
@@ -1676,7 +1768,7 @@ static int update_output_state(struct drm_atomic_state *state,
 		if (crtc == set->crtc)
 			continue;
 
-		if (!drm_atomic_connectors_for_crtc(state, crtc)) {
+		if (!crtc_state->connector_mask) {
 			ret = drm_atomic_set_mode_prop_for_crtc(crtc_state,
 								NULL);
 			if (ret < 0)
@@ -1797,16 +1889,16 @@ int __drm_atomic_helper_set_config(struct drm_mode_set *set,
 	drm_atomic_set_fb_for_plane(primary_state, set->fb);
 	primary_state->crtc_x = 0;
 	primary_state->crtc_y = 0;
-	primary_state->crtc_h = vdisplay;
 	primary_state->crtc_w = hdisplay;
+	primary_state->crtc_h = vdisplay;
 	primary_state->src_x = set->x << 16;
 	primary_state->src_y = set->y << 16;
 	if (primary_state->rotation & (BIT(DRM_ROTATE_90) | BIT(DRM_ROTATE_270))) {
-		primary_state->src_h = hdisplay << 16;
 		primary_state->src_w = vdisplay << 16;
+		primary_state->src_h = hdisplay << 16;
 	} else {
-		primary_state->src_h = vdisplay << 16;
 		primary_state->src_w = hdisplay << 16;
+		primary_state->src_h = vdisplay << 16;
 	}
 
 commit:
@@ -1818,6 +1910,161 @@ commit:
 }
 
 /**
+ * drm_atomic_helper_disable_all - disable all currently active outputs
+ * @dev: DRM device
+ * @ctx: lock acquisition context
+ *
+ * Loops through all connectors, finding those that aren't turned off and then
+ * turns them off by setting their DPMS mode to OFF and deactivating the CRTC
+ * that they are connected to.
+ *
+ * This is used for example in suspend/resume to disable all currently active
+ * functions when suspending.
+ *
+ * Note that if callers haven't already acquired all modeset locks this might
+ * return -EDEADLK, which must be handled by calling drm_modeset_backoff().
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
+ *
+ * See also:
+ * drm_atomic_helper_suspend(), drm_atomic_helper_resume()
+ */
+int drm_atomic_helper_disable_all(struct drm_device *dev,
+				  struct drm_modeset_acquire_ctx *ctx)
+{
+	struct drm_atomic_state *state;
+	struct drm_connector *conn;
+	int err;
+
+	state = drm_atomic_state_alloc(dev);
+	if (!state)
+		return -ENOMEM;
+
+	state->acquire_ctx = ctx;
+
+	drm_for_each_connector(conn, dev) {
+		struct drm_crtc *crtc = conn->state->crtc;
+		struct drm_crtc_state *crtc_state;
+
+		if (!crtc || conn->dpms != DRM_MODE_DPMS_ON)
+			continue;
+
+		crtc_state = drm_atomic_get_crtc_state(state, crtc);
+		if (IS_ERR(crtc_state)) {
+			err = PTR_ERR(crtc_state);
+			goto free;
+		}
+
+		crtc_state->active = false;
+	}
+
+	err = drm_atomic_commit(state);
+
+free:
+	if (err < 0)
+		drm_atomic_state_free(state);
+
+	return err;
+}
+EXPORT_SYMBOL(drm_atomic_helper_disable_all);
+
+/**
+ * drm_atomic_helper_suspend - subsystem-level suspend helper
+ * @dev: DRM device
+ *
+ * Duplicates the current atomic state, disables all active outputs and then
+ * returns a pointer to the original atomic state to the caller. Drivers can
+ * pass this pointer to the drm_atomic_helper_resume() helper upon resume to
+ * restore the output configuration that was active at the time the system
+ * entered suspend.
+ *
+ * Note that it is potentially unsafe to use this. The atomic state object
+ * returned by this function is assumed to be persistent. Drivers must ensure
+ * that this holds true. Before calling this function, drivers must make sure
+ * to suspend fbdev emulation so that nothing can be using the device.
+ *
+ * Returns:
+ * A pointer to a copy of the state before suspend on success or an ERR_PTR()-
+ * encoded error code on failure. Drivers should store the returned atomic
+ * state object and pass it to the drm_atomic_helper_resume() helper upon
+ * resume.
+ *
+ * See also:
+ * drm_atomic_helper_duplicate_state(), drm_atomic_helper_disable_all(),
+ * drm_atomic_helper_resume()
+ */
+struct drm_atomic_state *drm_atomic_helper_suspend(struct drm_device *dev)
+{
+	struct drm_modeset_acquire_ctx ctx;
+	struct drm_atomic_state *state;
+	int err;
+
+	drm_modeset_acquire_init(&ctx, 0);
+
+retry:
+	err = drm_modeset_lock_all_ctx(dev, &ctx);
+	if (err < 0) {
+		state = ERR_PTR(err);
+		goto unlock;
+	}
+
+	state = drm_atomic_helper_duplicate_state(dev, &ctx);
+	if (IS_ERR(state))
+		goto unlock;
+
+	err = drm_atomic_helper_disable_all(dev, &ctx);
+	if (err < 0) {
+		drm_atomic_state_free(state);
+		state = ERR_PTR(err);
+		goto unlock;
+	}
+
+unlock:
+	if (PTR_ERR(state) == -EDEADLK) {
+		drm_modeset_backoff(&ctx);
+		goto retry;
+	}
+
+	drm_modeset_drop_locks(&ctx);
+	drm_modeset_acquire_fini(&ctx);
+	return state;
+}
+EXPORT_SYMBOL(drm_atomic_helper_suspend);
+
+/**
+ * drm_atomic_helper_resume - subsystem-level resume helper
+ * @dev: DRM device
+ * @state: atomic state to resume to
+ *
+ * Calls drm_mode_config_reset() to synchronize hardware and software states,
+ * grabs all modeset locks and commits the atomic state object. This can be
+ * used in conjunction with the drm_atomic_helper_suspend() helper to
+ * implement suspend/resume for drivers that support atomic mode-setting.
+ *
+ * Returns:
+ * 0 on success or a negative error code on failure.
+ *
+ * See also:
+ * drm_atomic_helper_suspend()
+ */
+int drm_atomic_helper_resume(struct drm_device *dev,
+			     struct drm_atomic_state *state)
+{
+	struct drm_mode_config *config = &dev->mode_config;
+	int err;
+
+	drm_mode_config_reset(dev);
+	drm_modeset_lock_all(dev);
+	state->acquire_ctx = config->acquire_ctx;
+	err = drm_atomic_commit(state);
+	drm_modeset_unlock_all(dev);
+
+	return err;
+}
+EXPORT_SYMBOL(drm_atomic_helper_resume);
+
+/**
  * drm_atomic_helper_crtc_set_property - helper for crtc properties
  * @crtc: DRM crtc
  * @property: DRM property
@@ -2051,6 +2298,15 @@ retry:
 		goto fail;
 	drm_atomic_set_fb_for_plane(plane_state, fb);
 
+	/* Make sure we don't accidentally do a full modeset. */
+	state->allow_modeset = false;
+	if (!crtc_state->active) {
+		DRM_DEBUG_ATOMIC("[CRTC:%d] disabled, rejecting legacy flip\n",
+				 crtc->base.id);
+		ret = -EINVAL;
+		goto fail;
+	}
+
 	ret = drm_atomic_async_commit(state);
 	if (ret != 0)
 		goto fail;
@@ -2173,6 +2429,12 @@ EXPORT_SYMBOL(drm_atomic_helper_connector_dpms);
  * The simpler solution is to just reset the software state to everything off,
  * which is easiest to do by calling drm_mode_config_reset(). To facilitate this
  * the atomic helpers provide default reset implementations for all hooks.
+ *
+ * On the upside the precise state tracking of atomic simplifies system suspend
+ * and resume a lot. For drivers using drm_mode_config_reset() a complete recipe
+ * is implemented in drm_atomic_helper_suspend() and drm_atomic_helper_resume().
+ * For other drivers the building blocks are split out, see the documentation
+ * for these functions.
  */
 
 /**
@@ -2184,7 +2446,7 @@ EXPORT_SYMBOL(drm_atomic_helper_connector_dpms);
  */
 void drm_atomic_helper_crtc_reset(struct drm_crtc *crtc)
 {
-	if (crtc->state && crtc->state->mode_blob)
+	if (crtc->state)
 		drm_property_unreference_blob(crtc->state->mode_blob);
 	kfree(crtc->state);
 	crtc->state = kzalloc(sizeof(*crtc->state), GFP_KERNEL);
@@ -2252,8 +2514,7 @@ EXPORT_SYMBOL(drm_atomic_helper_crtc_duplicate_state);
 void __drm_atomic_helper_crtc_destroy_state(struct drm_crtc *crtc,
 					    struct drm_crtc_state *state)
 {
-	if (state->mode_blob)
-		drm_property_unreference_blob(state->mode_blob);
+	drm_property_unreference_blob(state->mode_blob);
 }
 EXPORT_SYMBOL(__drm_atomic_helper_crtc_destroy_state);
 
@@ -2368,6 +2629,28 @@ void drm_atomic_helper_plane_destroy_state(struct drm_plane *plane,
 EXPORT_SYMBOL(drm_atomic_helper_plane_destroy_state);
 
 /**
+ * __drm_atomic_helper_connector_reset - reset state on connector
+ * @connector: drm connector
+ * @conn_state: connector state to assign
+ *
+ * Initializes the newly allocated @conn_state and assigns it to
+ * #connector ->state, usually required when initializing the drivers
+ * or when called from the ->reset hook.
+ *
+ * This is useful for drivers that subclass the connector state.
+ */
+void
+__drm_atomic_helper_connector_reset(struct drm_connector *connector,
+				    struct drm_connector_state *conn_state)
+{
+	if (conn_state)
+		conn_state->connector = connector;
+
+	connector->state = conn_state;
+}
+EXPORT_SYMBOL(__drm_atomic_helper_connector_reset);
+
+/**
  * drm_atomic_helper_connector_reset - default ->reset hook for connectors
  * @connector: drm connector
  *
@@ -2377,11 +2660,11 @@ EXPORT_SYMBOL(drm_atomic_helper_plane_destroy_state);
  */
 void drm_atomic_helper_connector_reset(struct drm_connector *connector)
 {
-	kfree(connector->state);
-	connector->state = kzalloc(sizeof(*connector->state), GFP_KERNEL);
+	struct drm_connector_state *conn_state =
+		kzalloc(sizeof(*conn_state), GFP_KERNEL);
 
-	if (connector->state)
-		connector->state->connector = connector;
+	kfree(connector->state);
+	__drm_atomic_helper_connector_reset(connector, conn_state);
 }
 EXPORT_SYMBOL(drm_atomic_helper_connector_reset);
 
@@ -2430,7 +2713,9 @@ EXPORT_SYMBOL(drm_atomic_helper_connector_duplicate_state);
  * @ctx: lock acquisition context
  *
  * Makes a copy of the current atomic state by looping over all objects and
- * duplicating their respective states.
+ * duplicating their respective states. This is used for example by suspend/
+ * resume support code to save the state prior to suspend such that it can
+ * be restored upon resume.
  *
  * Note that this treats atomic state as persistent between save and restore.
  * Drivers must make sure that this is possible and won't result in confusion
@@ -2442,6 +2727,9 @@ EXPORT_SYMBOL(drm_atomic_helper_connector_duplicate_state);
  * Returns:
  * A pointer to the copy of the atomic state object on success or an
  * ERR_PTR()-encoded error code on failure.
+ *
+ * See also:
+ * drm_atomic_helper_suspend(), drm_atomic_helper_resume()
  */
 struct drm_atomic_state *
 drm_atomic_helper_duplicate_state(struct drm_device *dev,
diff --git a/drivers/gpu/drm/drm_bridge.c b/drivers/gpu/drm/drm_bridge.c
index 6b8f7211e543..bd93453afa61 100644
--- a/drivers/gpu/drm/drm_bridge.c
+++ b/drivers/gpu/drm/drm_bridge.c
@@ -31,14 +31,14 @@
 /**
  * DOC: overview
  *
- * drm_bridge represents a device that hangs on to an encoder. These are handy
- * when a regular drm_encoder entity isn't enough to represent the entire
+ * struct &drm_bridge represents a device that hangs on to an encoder. These are
+ * handy when a regular &drm_encoder entity isn't enough to represent the entire
  * encoder chain.
  *
- * A bridge is always associated to a single drm_encoder at a time, but can be
+ * A bridge is always attached to a single &drm_encoder at a time, but can be
  * either connected to it directly, or through an intermediate bridge:
  *
- * encoder ---> bridge B ---> bridge A
+ *     encoder ---> bridge B ---> bridge A
  *
  * Here, the output of the encoder feeds to bridge B, and that furthers feeds to
  * bridge A.
@@ -46,11 +46,16 @@
  * The driver using the bridge is responsible to make the associations between
  * the encoder and bridges. Once these links are made, the bridges will
  * participate along with encoder functions to perform mode_set/enable/disable
- * through the ops provided in drm_bridge_funcs.
+ * through the ops provided in &drm_bridge_funcs.
  *
  * drm_bridge, like drm_panel, aren't drm_mode_object entities like planes,
- * crtcs, encoders or connectors. They just provide additional hooks to get the
- * desired output at the end of the encoder chain.
+ * CRTCs, encoders or connectors and hence are not visible to userspace. They
+ * just provide additional hooks to get the desired output at the end of the
+ * encoder chain.
+ *
+ * Bridges can also be chained up using the next pointer in struct &drm_bridge.
+ *
+ * Both legacy CRTC helpers and the new atomic modeset helpers support bridges.
  */
 
 static DEFINE_MUTEX(bridge_lock);
@@ -122,34 +127,12 @@ EXPORT_SYMBOL(drm_bridge_attach);
 /**
  * DOC: bridge callbacks
  *
- * The drm_bridge_funcs ops are populated by the bridge driver. The drm
- * internals(atomic and crtc helpers) use the helpers defined in drm_bridge.c
- * These helpers call a specific drm_bridge_funcs op for all the bridges
+ * The &drm_bridge_funcs ops are populated by the bridge driver. The DRM
+ * internals (atomic and CRTC helpers) use the helpers defined in drm_bridge.c
+ * These helpers call a specific &drm_bridge_funcs op for all the bridges
  * during encoder configuration.
  *
- * When creating a bridge driver, one can implement drm_bridge_funcs op with
- * the help of these rough rules:
- *
- * pre_enable: this contains things needed to be done for the bridge before
- * its clock and timings are enabled by its source. For a bridge, its source
- * is generally the encoder or bridge just before it in the encoder chain.
- *
- * enable: this contains things needed to be done for the bridge once its
- * source is enabled. In other words, enable is called once the source is
- * ready with clock and timing needed by the bridge.
- *
- * disable: this contains things needed to be done for the bridge assuming
- * that its source is still enabled, i.e. clock and timings are still on.
- *
- * post_disable: this contains things needed to be done for the bridge once
- * its source is disabled, i.e. once clocks and timings are off.
- *
- * mode_fixup: this should fixup the given mode for the bridge. It is called
- * after the encoder's mode fixup. mode_fixup can also reject a mode completely
- * if it's unsuitable for the hardware.
- *
- * mode_set: this sets up the mode for the bridge. It assumes that its source
- * (an encoder or a bridge) has set the mode too.
+ * For detailed specification of the bridge callbacks see &drm_bridge_funcs.
  */
 
 /**
@@ -159,7 +142,7 @@ EXPORT_SYMBOL(drm_bridge_attach);
  * @mode: desired mode to be set for the bridge
  * @adjusted_mode: updated mode that works for this bridge
  *
- * Calls 'mode_fixup' drm_bridge_funcs op for all the bridges in the
+ * Calls ->mode_fixup() &drm_bridge_funcs op for all the bridges in the
  * encoder chain, starting from the first bridge to the last.
  *
  * Note: the bridge passed should be the one closest to the encoder
@@ -186,11 +169,11 @@ bool drm_bridge_mode_fixup(struct drm_bridge *bridge,
 EXPORT_SYMBOL(drm_bridge_mode_fixup);
 
 /**
- * drm_bridge_disable - calls 'disable' drm_bridge_funcs op for all
+ * drm_bridge_disable - calls ->disable() &drm_bridge_funcs op for all
  *			bridges in the encoder chain.
  * @bridge: bridge control structure
  *
- * Calls 'disable' drm_bridge_funcs op for all the bridges in the encoder
+ * Calls ->disable() &drm_bridge_funcs op for all the bridges in the encoder
  * chain, starting from the last bridge to the first. These are called before
  * calling the encoder's prepare op.
  *
@@ -208,11 +191,11 @@ void drm_bridge_disable(struct drm_bridge *bridge)
 EXPORT_SYMBOL(drm_bridge_disable);
 
 /**
- * drm_bridge_post_disable - calls 'post_disable' drm_bridge_funcs op for
+ * drm_bridge_post_disable - calls ->post_disable() &drm_bridge_funcs op for
  *			     all bridges in the encoder chain.
  * @bridge: bridge control structure
  *
- * Calls 'post_disable' drm_bridge_funcs op for all the bridges in the
+ * Calls ->post_disable() &drm_bridge_funcs op for all the bridges in the
  * encoder chain, starting from the first bridge to the last. These are called
  * after completing the encoder's prepare op.
  *
@@ -236,7 +219,7 @@ EXPORT_SYMBOL(drm_bridge_post_disable);
  * @mode: desired mode to be set for the bridge
  * @adjusted_mode: updated mode that works for this bridge
  *
- * Calls 'mode_set' drm_bridge_funcs op for all the bridges in the
+ * Calls ->mode_set() &drm_bridge_funcs op for all the bridges in the
  * encoder chain, starting from the first bridge to the last.
  *
  * Note: the bridge passed should be the one closest to the encoder
@@ -256,11 +239,11 @@ void drm_bridge_mode_set(struct drm_bridge *bridge,
 EXPORT_SYMBOL(drm_bridge_mode_set);
 
 /**
- * drm_bridge_pre_enable - calls 'pre_enable' drm_bridge_funcs op for all
+ * drm_bridge_pre_enable - calls ->pre_enable() &drm_bridge_funcs op for all
  *			   bridges in the encoder chain.
  * @bridge: bridge control structure
  *
- * Calls 'pre_enable' drm_bridge_funcs op for all the bridges in the encoder
+ * Calls ->pre_enable() &drm_bridge_funcs op for all the bridges in the encoder
  * chain, starting from the last bridge to the first. These are called
  * before calling the encoder's commit op.
  *
@@ -278,11 +261,11 @@ void drm_bridge_pre_enable(struct drm_bridge *bridge)
 EXPORT_SYMBOL(drm_bridge_pre_enable);
 
 /**
- * drm_bridge_enable - calls 'enable' drm_bridge_funcs op for all bridges
+ * drm_bridge_enable - calls ->enable() &drm_bridge_funcs op for all bridges
  *		       in the encoder chain.
  * @bridge: bridge control structure
  *
- * Calls 'enable' drm_bridge_funcs op for all the bridges in the encoder
+ * Calls ->enable() &drm_bridge_funcs op for all the bridges in the encoder
  * chain, starting from the first bridge to the last. These are called
  * after completing the encoder's commit op.
  *
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index 24c5434abd1c..f6191215b2cb 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -45,7 +45,7 @@
 
 static struct drm_framebuffer *
 internal_framebuffer_create(struct drm_device *dev,
-			    struct drm_mode_fb_cmd2 *r,
+			    const struct drm_mode_fb_cmd2 *r,
 			    struct drm_file *file_priv);
 
 /* Avoid boilerplate.  I'm tired of typing. */
@@ -649,6 +649,18 @@ EXPORT_SYMBOL(drm_framebuffer_remove);
 
 DEFINE_WW_CLASS(crtc_ww_class);
 
+static unsigned int drm_num_crtcs(struct drm_device *dev)
+{
+	unsigned int num = 0;
+	struct drm_crtc *tmp;
+
+	drm_for_each_crtc(tmp, dev) {
+		num++;
+	}
+
+	return num;
+}
+
 /**
  * drm_crtc_init_with_planes - Initialise a new CRTC object with
  *    specified primary and cursor planes.
@@ -657,6 +669,7 @@ DEFINE_WW_CLASS(crtc_ww_class);
  * @primary: Primary plane for CRTC
  * @cursor: Cursor plane for CRTC
  * @funcs: callbacks for the new CRTC
+ * @name: printf style format string for the CRTC name, or NULL for default name
  *
  * Inits a new object created as base part of a driver crtc object.
  *
@@ -666,7 +679,8 @@ DEFINE_WW_CLASS(crtc_ww_class);
 int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 			      struct drm_plane *primary,
 			      struct drm_plane *cursor,
-			      const struct drm_crtc_funcs *funcs)
+			      const struct drm_crtc_funcs *funcs,
+			      const char *name, ...)
 {
 	struct drm_mode_config *config = &dev->mode_config;
 	int ret;
@@ -682,6 +696,21 @@ int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 	if (ret)
 		return ret;
 
+	if (name) {
+		va_list ap;
+
+		va_start(ap, name);
+		crtc->name = kvasprintf(GFP_KERNEL, name, ap);
+		va_end(ap);
+	} else {
+		crtc->name = kasprintf(GFP_KERNEL, "crtc-%d",
+				       drm_num_crtcs(dev));
+	}
+	if (!crtc->name) {
+		drm_mode_object_put(dev, &crtc->base);
+		return -ENOMEM;
+	}
+
 	crtc->base.properties = &crtc->properties;
 
 	list_add_tail(&crtc->head, &config->crtc_list);
@@ -728,6 +757,8 @@ void drm_crtc_cleanup(struct drm_crtc *crtc)
 	if (crtc->state && crtc->funcs->atomic_destroy_state)
 		crtc->funcs->atomic_destroy_state(crtc, crtc->state);
 
+	kfree(crtc->name);
+
 	memset(crtc, 0, sizeof(*crtc));
 }
 EXPORT_SYMBOL(drm_crtc_cleanup);
@@ -887,12 +918,19 @@ int drm_connector_init(struct drm_device *dev,
 	connector->base.properties = &connector->properties;
 	connector->dev = dev;
 	connector->funcs = funcs;
+
+	connector->connector_id = ida_simple_get(&config->connector_ida, 0, 0, GFP_KERNEL);
+	if (connector->connector_id < 0) {
+		ret = connector->connector_id;
+		goto out_put;
+	}
+
 	connector->connector_type = connector_type;
 	connector->connector_type_id =
 		ida_simple_get(connector_ida, 1, 0, GFP_KERNEL);
 	if (connector->connector_type_id < 0) {
 		ret = connector->connector_type_id;
-		goto out_put;
+		goto out_put_id;
 	}
 	connector->name =
 		kasprintf(GFP_KERNEL, "%s-%d",
@@ -900,7 +938,7 @@ int drm_connector_init(struct drm_device *dev,
 			  connector->connector_type_id);
 	if (!connector->name) {
 		ret = -ENOMEM;
-		goto out_put;
+		goto out_put_type_id;
 	}
 
 	INIT_LIST_HEAD(&connector->probed_modes);
@@ -928,7 +966,12 @@ int drm_connector_init(struct drm_device *dev,
 	}
 
 	connector->debugfs_entry = NULL;
-
+out_put_type_id:
+	if (ret)
+		ida_remove(connector_ida, connector->connector_type_id);
+out_put_id:
+	if (ret)
+		ida_remove(&config->connector_ida, connector->connector_id);
 out_put:
 	if (ret)
 		drm_mode_object_put(dev, &connector->base);
@@ -965,6 +1008,9 @@ void drm_connector_cleanup(struct drm_connector *connector)
 	ida_remove(&drm_connector_enum_list[connector->connector_type].ida,
 		   connector->connector_type_id);
 
+	ida_remove(&dev->mode_config.connector_ida,
+		   connector->connector_id);
+
 	kfree(connector->display_info.bus_formats);
 	drm_mode_object_put(dev, &connector->base);
 	kfree(connector->name);
@@ -982,32 +1028,6 @@ void drm_connector_cleanup(struct drm_connector *connector)
 EXPORT_SYMBOL(drm_connector_cleanup);
 
 /**
- * drm_connector_index - find the index of a registered connector
- * @connector: connector to find index for
- *
- * Given a registered connector, return the index of that connector within a DRM
- * device's list of connectors.
- */
-unsigned int drm_connector_index(struct drm_connector *connector)
-{
-	unsigned int index = 0;
-	struct drm_connector *tmp;
-	struct drm_mode_config *config = &connector->dev->mode_config;
-
-	WARN_ON(!drm_modeset_is_locked(&config->connection_mutex));
-
-	drm_for_each_connector(tmp, connector->dev) {
-		if (tmp == connector)
-			return index;
-
-		index++;
-	}
-
-	BUG();
-}
-EXPORT_SYMBOL(drm_connector_index);
-
-/**
  * drm_connector_register - register a connector
  * @connector: the connector to register
  *
@@ -1075,6 +1095,7 @@ EXPORT_SYMBOL(drm_connector_unplug_all);
  * @encoder: the encoder to init
  * @funcs: callbacks for this encoder
  * @encoder_type: user visible type of the encoder
+ * @name: printf style format string for the encoder name, or NULL for default name
  *
  * Initialises a preallocated encoder. Encoder should be
  * subclassed as part of driver encoder objects.
@@ -1085,7 +1106,7 @@ EXPORT_SYMBOL(drm_connector_unplug_all);
 int drm_encoder_init(struct drm_device *dev,
 		      struct drm_encoder *encoder,
 		      const struct drm_encoder_funcs *funcs,
-		      int encoder_type)
+		      int encoder_type, const char *name, ...)
 {
 	int ret;
 
@@ -1098,9 +1119,17 @@ int drm_encoder_init(struct drm_device *dev,
 	encoder->dev = dev;
 	encoder->encoder_type = encoder_type;
 	encoder->funcs = funcs;
-	encoder->name = kasprintf(GFP_KERNEL, "%s-%d",
-				  drm_encoder_enum_list[encoder_type].name,
-				  encoder->base.id);
+	if (name) {
+		va_list ap;
+
+		va_start(ap, name);
+		encoder->name = kvasprintf(GFP_KERNEL, name, ap);
+		va_end(ap);
+	} else {
+		encoder->name = kasprintf(GFP_KERNEL, "%s-%d",
+					  drm_encoder_enum_list[encoder_type].name,
+					  encoder->base.id);
+	}
 	if (!encoder->name) {
 		ret = -ENOMEM;
 		goto out_put;
@@ -1141,6 +1170,18 @@ void drm_encoder_cleanup(struct drm_encoder *encoder)
 }
 EXPORT_SYMBOL(drm_encoder_cleanup);
 
+static unsigned int drm_num_planes(struct drm_device *dev)
+{
+	unsigned int num = 0;
+	struct drm_plane *tmp;
+
+	drm_for_each_plane(tmp, dev) {
+		num++;
+	}
+
+	return num;
+}
+
 /**
  * drm_universal_plane_init - Initialize a new universal plane object
  * @dev: DRM device
@@ -1150,6 +1191,7 @@ EXPORT_SYMBOL(drm_encoder_cleanup);
  * @formats: array of supported formats (%DRM_FORMAT_*)
  * @format_count: number of elements in @formats
  * @type: type of plane (overlay, primary, cursor)
+ * @name: printf style format string for the plane name, or NULL for default name
  *
  * Initializes a plane object of type @type.
  *
@@ -1160,7 +1202,8 @@ int drm_universal_plane_init(struct drm_device *dev, struct drm_plane *plane,
 			     unsigned long possible_crtcs,
 			     const struct drm_plane_funcs *funcs,
 			     const uint32_t *formats, unsigned int format_count,
-			     enum drm_plane_type type)
+			     enum drm_plane_type type,
+			     const char *name, ...)
 {
 	struct drm_mode_config *config = &dev->mode_config;
 	int ret;
@@ -1182,6 +1225,22 @@ int drm_universal_plane_init(struct drm_device *dev, struct drm_plane *plane,
 		return -ENOMEM;
 	}
 
+	if (name) {
+		va_list ap;
+
+		va_start(ap, name);
+		plane->name = kvasprintf(GFP_KERNEL, name, ap);
+		va_end(ap);
+	} else {
+		plane->name = kasprintf(GFP_KERNEL, "plane-%d",
+					drm_num_planes(dev));
+	}
+	if (!plane->name) {
+		kfree(plane->format_types);
+		drm_mode_object_put(dev, &plane->base);
+		return -ENOMEM;
+	}
+
 	memcpy(plane->format_types, formats, format_count * sizeof(uint32_t));
 	plane->format_count = format_count;
 	plane->possible_crtcs = possible_crtcs;
@@ -1240,7 +1299,7 @@ int drm_plane_init(struct drm_device *dev, struct drm_plane *plane,
 
 	type = is_primary ? DRM_PLANE_TYPE_PRIMARY : DRM_PLANE_TYPE_OVERLAY;
 	return drm_universal_plane_init(dev, plane, possible_crtcs, funcs,
-					formats, format_count, type);
+					formats, format_count, type, NULL);
 }
 EXPORT_SYMBOL(drm_plane_init);
 
@@ -1272,6 +1331,8 @@ void drm_plane_cleanup(struct drm_plane *plane)
 	if (plane->state && plane->funcs->atomic_destroy_state)
 		plane->funcs->atomic_destroy_state(plane, plane->state);
 
+	kfree(plane->name);
+
 	memset(plane, 0, sizeof(*plane));
 }
 EXPORT_SYMBOL(drm_plane_cleanup);
@@ -1801,7 +1862,8 @@ int drm_mode_getresources(struct drm_device *dev, void *data,
 		copied = 0;
 		crtc_id = (uint32_t __user *)(unsigned long)card_res->crtc_id_ptr;
 		drm_for_each_crtc(crtc, dev) {
-			DRM_DEBUG_KMS("[CRTC:%d]\n", crtc->base.id);
+			DRM_DEBUG_KMS("[CRTC:%d:%s]\n",
+				      crtc->base.id, crtc->name);
 			if (put_user(crtc->base.id, crtc_id + copied)) {
 				ret = -EFAULT;
 				goto out;
@@ -2646,7 +2708,7 @@ int drm_mode_setcrtc(struct drm_device *dev, void *data,
 		ret = -ENOENT;
 		goto out;
 	}
-	DRM_DEBUG_KMS("[CRTC:%d]\n", crtc->base.id);
+	DRM_DEBUG_KMS("[CRTC:%d:%s]\n", crtc->base.id, crtc->name);
 
 	if (crtc_req->mode_valid) {
 		/* If we have a mode we need a framebuffer. */
@@ -3235,7 +3297,7 @@ static int framebuffer_check(const struct drm_mode_fb_cmd2 *r)
 
 static struct drm_framebuffer *
 internal_framebuffer_create(struct drm_device *dev,
-			    struct drm_mode_fb_cmd2 *r,
+			    const struct drm_mode_fb_cmd2 *r,
 			    struct drm_file *file_priv)
 {
 	struct drm_mode_config *config = &dev->mode_config;
@@ -4785,9 +4847,7 @@ static int drm_mode_connector_set_obj_prop(struct drm_mode_object *obj,
 
 	/* Do DPMS ourselves */
 	if (property == connector->dev->mode_config.dpms_property) {
-		ret = 0;
-		if (connector->funcs->dpms)
-			ret = (*connector->funcs->dpms)(connector, (int)value);
+		ret = (*connector->funcs->dpms)(connector, (int)value);
 	} else if (connector->funcs->set_property)
 		ret = connector->funcs->set_property(connector, property, value);
 
@@ -4983,6 +5043,20 @@ int drm_mode_connector_attach_encoder(struct drm_connector *connector,
 {
 	int i;
 
+	/*
+	 * In the past, drivers have attempted to model the static association
+	 * of connector to encoder in simple connector/encoder devices using a
+	 * direct assignment of connector->encoder = encoder. This connection
+	 * is a logical one and the responsibility of the core, so drivers are
+	 * expected not to mess with this.
+	 *
+	 * Note that the error return should've been enough here, but a large
+	 * majority of drivers ignores the return value, so add in a big WARN
+	 * to get people's attention.
+	 */
+	if (WARN_ON(connector->encoder))
+		return -EINVAL;
+
 	for (i = 0; i < DRM_CONNECTOR_MAX_ENCODER; i++) {
 		if (connector->encoder_ids[i] == 0) {
 			connector->encoder_ids[i] = encoder->base.id;
@@ -5704,6 +5778,7 @@ void drm_mode_config_init(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev->mode_config.plane_list);
 	idr_init(&dev->mode_config.crtc_idr);
 	idr_init(&dev->mode_config.tile_idr);
+	ida_init(&dev->mode_config.connector_ida);
 
 	drm_modeset_lock_all(dev);
 	drm_mode_create_standard_properties(dev);
@@ -5784,6 +5859,7 @@ void drm_mode_config_cleanup(struct drm_device *dev)
 		crtc->funcs->destroy(crtc);
 	}
 
+	ida_destroy(&dev->mode_config.connector_ida);
 	idr_destroy(&dev->mode_config.tile_idr);
 	idr_destroy(&dev->mode_config.crtc_idr);
 	drm_modeset_lock_fini(&dev->mode_config.connection_mutex);
diff --git a/drivers/gpu/drm/drm_crtc_helper.c b/drivers/gpu/drm/drm_crtc_helper.c
index ef534758a02c..a02a7f9a6a9d 100644
--- a/drivers/gpu/drm/drm_crtc_helper.c
+++ b/drivers/gpu/drm/drm_crtc_helper.c
@@ -51,6 +51,11 @@
  * the same callbacks which drivers can use to e.g. restore the modeset
  * configuration on resume with drm_helper_resume_force_mode().
  *
+ * Note that this helper library doesn't track the current power state of CRTCs
+ * and encoders. It can call callbacks like ->dpms() even though the hardware is
+ * already in the desired state. This deficiency has been fixed in the atomic
+ * helpers.
+ *
  * The driver callbacks are mostly compatible with the atomic modeset helpers,
  * except for the handling of the primary plane: Atomic helpers require that the
  * primary plane is implemented as a real standalone plane and not directly tied
@@ -62,6 +67,11 @@
  * converting to the plane helpers). New drivers must not use these functions
  * but need to implement the atomic interface instead, potentially using the
  * atomic helpers for that.
+ *
+ * These legacy modeset helpers use the same function table structures as
+ * all other modesetting helpers. See the documentation for struct
+ * &drm_crtc_helper_funcs, struct &drm_encoder_helper_funcs and struct
+ * &drm_connector_helper_funcs.
  */
 MODULE_AUTHOR("David Airlie, Jesse Barnes");
 MODULE_DESCRIPTION("DRM KMS helper");
@@ -206,8 +216,8 @@ static void __drm_helper_disable_unused_functions(struct drm_device *dev)
  * @dev: DRM device
  *
  * This function walks through the entire mode setting configuration of @dev. It
- * will remove any crtc links of unused encoders and encoder links of
- * disconnected connectors. Then it will disable all unused encoders and crtcs
+ * will remove any CRTC links of unused encoders and encoder links of
+ * disconnected connectors. Then it will disable all unused encoders and CRTCs
  * either by calling their disable callback if available or by calling their
  * dpms callback with DRM_MODE_DPMS_OFF.
  */
@@ -329,7 +339,7 @@ bool drm_crtc_helper_set_mode(struct drm_crtc *crtc,
 		DRM_DEBUG_KMS("CRTC fixup failed\n");
 		goto done;
 	}
-	DRM_DEBUG_KMS("[CRTC:%d]\n", crtc->base.id);
+	DRM_DEBUG_KMS("[CRTC:%d:%s]\n", crtc->base.id, crtc->name);
 
 	crtc->hwmode = *adjusted_mode;
 
@@ -445,11 +455,36 @@ drm_crtc_helper_disable(struct drm_crtc *crtc)
  * drm_crtc_helper_set_config - set a new config from userspace
  * @set: mode set configuration
  *
- * Setup a new configuration, provided by the upper layers (either an ioctl call
- * from userspace or internally e.g. from the fbdev support code) in @set, and
- * enable it. This is the main helper functions for drivers that implement
- * kernel mode setting with the crtc helper functions and the assorted
- * ->prepare(), ->modeset() and ->commit() helper callbacks.
+ * The drm_crtc_helper_set_config() helper function implements the set_config
+ * callback of struct &drm_crtc_funcs for drivers using the legacy CRTC helpers.
+ *
+ * It first tries to locate the best encoder for each connector by calling the
+ * connector ->best_encoder() (struct &drm_connector_helper_funcs) helper
+ * operation.
+ *
+ * After locating the appropriate encoders, the helper function will call the
+ * mode_fixup encoder and CRTC helper operations to adjust the requested mode,
+ * or reject it completely in which case an error will be returned to the
+ * application. If the new configuration after mode adjustment is identical to
+ * the current configuration the helper function will return without performing
+ * any other operation.
+ *
+ * If the adjusted mode is identical to the current mode but changes to the
+ * frame buffer need to be applied, the drm_crtc_helper_set_config() function
+ * will call the CRTC ->mode_set_base() (struct &drm_crtc_helper_funcs) helper
+ * operation.
+ *
+ * If the adjusted mode differs from the current mode, or if the
+ * ->mode_set_base() helper operation is not provided, the helper function
+ * performs a full mode set sequence by calling the ->prepare(), ->mode_set()
+ * and ->commit() CRTC and encoder helper operations, in that order.
+ * Alternatively it can also use the dpms and disable helper operations. For
+ * details see struct &drm_crtc_helper_funcs and struct
+ * &drm_encoder_helper_funcs.
+ *
+ * This function is deprecated.  New drivers must implement atomic modeset
+ * support, for which this function is unsuitable. Instead drivers should use
+ * drm_atomic_helper_set_config().
  *
  * Returns:
  * Returns 0 on success, negative errno numbers on failure.
@@ -484,11 +519,13 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 		set->fb = NULL;
 
 	if (set->fb) {
-		DRM_DEBUG_KMS("[CRTC:%d] [FB:%d] #connectors=%d (x y) (%i %i)\n",
-				set->crtc->base.id, set->fb->base.id,
-				(int)set->num_connectors, set->x, set->y);
+		DRM_DEBUG_KMS("[CRTC:%d:%s] [FB:%d] #connectors=%d (x y) (%i %i)\n",
+			      set->crtc->base.id, set->crtc->name,
+			      set->fb->base.id,
+			      (int)set->num_connectors, set->x, set->y);
 	} else {
-		DRM_DEBUG_KMS("[CRTC:%d] [NOFB]\n", set->crtc->base.id);
+		DRM_DEBUG_KMS("[CRTC:%d:%s] [NOFB]\n",
+			      set->crtc->base.id, set->crtc->name);
 		drm_crtc_helper_disable(set->crtc);
 		return 0;
 	}
@@ -628,12 +665,12 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 			connector->encoder->crtc = new_crtc;
 		}
 		if (new_crtc) {
-			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] to [CRTC:%d]\n",
-				connector->base.id, connector->name,
-				new_crtc->base.id);
+			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] to [CRTC:%d:%s]\n",
+				      connector->base.id, connector->name,
+				      new_crtc->base.id, new_crtc->name);
 		} else {
 			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] to [NOCRTC]\n",
-				connector->base.id, connector->name);
+				      connector->base.id, connector->name);
 		}
 	}
 
@@ -650,8 +687,8 @@ int drm_crtc_helper_set_config(struct drm_mode_set *set)
 			if (!drm_crtc_helper_set_mode(set->crtc, set->mode,
 						      set->x, set->y,
 						      save_set.fb)) {
-				DRM_ERROR("failed to set mode on [CRTC:%d]\n",
-					  set->crtc->base.id);
+				DRM_ERROR("failed to set mode on [CRTC:%d:%s]\n",
+					  set->crtc->base.id, set->crtc->name);
 				set->crtc->primary->fb = save_set.fb;
 				ret = -EINVAL;
 				goto fail;
@@ -758,10 +795,18 @@ static int drm_helper_choose_crtc_dpms(struct drm_crtc *crtc)
  * @connector: affected connector
  * @mode: DPMS mode
  *
- * This is the main helper function provided by the crtc helper framework for
+ * The drm_helper_connector_dpms() helper function implements the ->dpms()
+ * callback of struct &drm_connector_funcs for drivers using the legacy CRTC helpers.
+ *
+ * This is the main helper function provided by the CRTC helper framework for
  * implementing the DPMS connector attribute. It computes the new desired DPMS
- * state for all encoders and crtcs in the output mesh and calls the ->dpms()
- * callback provided by the driver appropriately.
+ * state for all encoders and CRTCs in the output mesh and calls the ->dpms()
+ * callbacks provided by the driver in struct &drm_crtc_helper_funcs and struct
+ * &drm_encoder_helper_funcs appropriately.
+ *
+ * This function is deprecated.  New drivers must implement atomic modeset
+ * support, for which this function is unsuitable. Instead drivers should use
+ * drm_atomic_helper_connector_dpms().
  *
  * Returns:
  * Always returns 0.
@@ -818,7 +863,7 @@ EXPORT_SYMBOL(drm_helper_connector_dpms);
  * metadata fields.
  */
 void drm_helper_mode_fill_fb_struct(struct drm_framebuffer *fb,
-				    struct drm_mode_fb_cmd2 *mode_cmd)
+				    const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	int i;
 
@@ -855,6 +900,12 @@ EXPORT_SYMBOL(drm_helper_mode_fill_fb_struct);
  * due to slight differences in allocating shared resources when the
  * configuration is restored in a different order than when userspace set it up)
  * need to use their own restore logic.
+ *
+ * This function is deprecated. New drivers should implement atomic mode-
+ * setting and use the atomic suspend/resume helpers.
+ *
+ * See also:
+ * drm_atomic_helper_suspend(), drm_atomic_helper_resume()
  */
 void drm_helper_resume_force_mode(struct drm_device *dev)
 {
@@ -913,9 +964,9 @@ EXPORT_SYMBOL(drm_helper_resume_force_mode);
  * @old_fb: previous framebuffer
  *
  * This function implements a callback useable as the ->mode_set callback
- * required by the crtc helpers. Besides the atomic plane helper functions for
+ * required by the CRTC helpers. Besides the atomic plane helper functions for
  * the primary plane the driver must also provide the ->mode_set_nofb callback
- * to set up the crtc.
+ * to set up the CRTC.
  *
  * This is a transitional helper useful for converting drivers to the atomic
  * interfaces.
@@ -979,7 +1030,7 @@ EXPORT_SYMBOL(drm_helper_crtc_mode_set);
  * @old_fb: previous framebuffer
  *
  * This function implements a callback useable as the ->mode_set_base used
- * required by the crtc helpers. The driver must provide the atomic plane helper
+ * required by the CRTC helpers. The driver must provide the atomic plane helper
  * functions for the primary plane.
  *
  * This is a transitional helper useful for converting drivers to the atomic
diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index 809959d56d78..27fbd79d0daf 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -666,7 +666,9 @@ static int build_enum_path_resources(struct drm_dp_sideband_msg_tx *msg, int por
 }
 
 static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_num,
-				  u8 vcpi, uint16_t pbn)
+				  u8 vcpi, uint16_t pbn,
+				  u8 number_sdp_streams,
+				  u8 *sdp_stream_sink)
 {
 	struct drm_dp_sideband_msg_req_body req;
 	memset(&req, 0, sizeof(req));
@@ -674,6 +676,9 @@ static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_n
 	req.u.allocate_payload.port_number = port_num;
 	req.u.allocate_payload.vcpi = vcpi;
 	req.u.allocate_payload.pbn = pbn;
+	req.u.allocate_payload.number_sdp_streams = number_sdp_streams;
+	memcpy(req.u.allocate_payload.sdp_stream_sink, sdp_stream_sink,
+		   number_sdp_streams);
 	drm_dp_encode_sideband_req(&req, msg);
 	msg->path_msg = true;
 	return 0;
@@ -798,6 +803,18 @@ static struct drm_dp_mst_branch *drm_dp_add_mst_branch_device(u8 lct, u8 *rad)
 	return mstb;
 }
 
+static void drm_dp_free_mst_port(struct kref *kref);
+
+static void drm_dp_free_mst_branch_device(struct kref *kref)
+{
+	struct drm_dp_mst_branch *mstb = container_of(kref, struct drm_dp_mst_branch, kref);
+	if (mstb->port_parent) {
+		if (list_empty(&mstb->port_parent->next))
+			kref_put(&mstb->port_parent->kref, drm_dp_free_mst_port);
+	}
+	kfree(mstb);
+}
+
 static void drm_dp_destroy_mst_branch_device(struct kref *kref)
 {
 	struct drm_dp_mst_branch *mstb = container_of(kref, struct drm_dp_mst_branch, kref);
@@ -805,6 +822,15 @@ static void drm_dp_destroy_mst_branch_device(struct kref *kref)
 	bool wake_tx = false;
 
 	/*
+	 * init kref again to be used by ports to remove mst branch when it is
+	 * not needed anymore
+	 */
+	kref_init(kref);
+
+	if (mstb->port_parent && list_empty(&mstb->port_parent->next))
+		kref_get(&mstb->port_parent->kref);
+
+	/*
 	 * destroy all ports - don't need lock
 	 * as there are no more references to the mst branch
 	 * device at this point.
@@ -830,7 +856,8 @@ static void drm_dp_destroy_mst_branch_device(struct kref *kref)
 
 	if (wake_tx)
 		wake_up(&mstb->mgr->tx_waitq);
-	kfree(mstb);
+
+	kref_put(kref, drm_dp_free_mst_branch_device);
 }
 
 static void drm_dp_put_mst_branch_device(struct drm_dp_mst_branch *mstb)
@@ -878,6 +905,7 @@ static void drm_dp_destroy_port(struct kref *kref)
 			 * from an EDID retrieval */
 
 			mutex_lock(&mgr->destroy_connector_lock);
+			kref_get(&port->parent->kref);
 			list_add(&port->next, &mgr->destroy_connector_list);
 			mutex_unlock(&mgr->destroy_connector_lock);
 			schedule_work(&mgr->destroy_connector_work);
@@ -973,17 +1001,17 @@ static struct drm_dp_mst_port *drm_dp_get_port(struct drm_dp_mst_branch *mstb, u
 static u8 drm_dp_calculate_rad(struct drm_dp_mst_port *port,
 				 u8 *rad)
 {
-	int lct = port->parent->lct;
+	int parent_lct = port->parent->lct;
 	int shift = 4;
-	int idx = lct / 2;
-	if (lct > 1) {
-		memcpy(rad, port->parent->rad, idx);
-		shift = (lct % 2) ? 4 : 0;
+	int idx = (parent_lct - 1) / 2;
+	if (parent_lct > 1) {
+		memcpy(rad, port->parent->rad, idx + 1);
+		shift = (parent_lct % 2) ? 4 : 0;
 	} else
 		rad[0] = 0;
 
 	rad[idx] |= port->port_num << shift;
-	return lct + 1;
+	return parent_lct + 1;
 }
 
 /*
@@ -1013,18 +1041,27 @@ static bool drm_dp_port_setup_pdt(struct drm_dp_mst_port *port)
 	return send_link;
 }
 
-static void drm_dp_check_port_guid(struct drm_dp_mst_branch *mstb,
-				   struct drm_dp_mst_port *port)
+static void drm_dp_check_mstb_guid(struct drm_dp_mst_branch *mstb, u8 *guid)
 {
 	int ret;
-	if (port->dpcd_rev >= 0x12) {
-		port->guid_valid = drm_dp_validate_guid(mstb->mgr, port->guid);
-		if (!port->guid_valid) {
-			ret = drm_dp_send_dpcd_write(mstb->mgr,
-						     port,
-						     DP_GUID,
-						     16, port->guid);
-			port->guid_valid = true;
+
+	memcpy(mstb->guid, guid, 16);
+
+	if (!drm_dp_validate_guid(mstb->mgr, mstb->guid)) {
+		if (mstb->port_parent) {
+			ret = drm_dp_send_dpcd_write(
+					mstb->mgr,
+					mstb->port_parent,
+					DP_GUID,
+					16,
+					mstb->guid);
+		} else {
+
+			ret = drm_dp_dpcd_write(
+					mstb->mgr->aux,
+					DP_GUID,
+					mstb->guid,
+					16);
 		}
 	}
 }
@@ -1039,7 +1076,7 @@ static void build_mst_prop_path(const struct drm_dp_mst_branch *mstb,
 	snprintf(proppath, proppath_size, "mst:%d", mstb->mgr->conn_base_id);
 	for (i = 0; i < (mstb->lct - 1); i++) {
 		int shift = (i % 2) ? 0 : 4;
-		int port_num = mstb->rad[i / 2] >> shift;
+		int port_num = (mstb->rad[i / 2] >> shift) & 0xf;
 		snprintf(temp, sizeof(temp), "-%d", port_num);
 		strlcat(proppath, temp, proppath_size);
 	}
@@ -1081,7 +1118,6 @@ static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
 	port->dpcd_rev = port_msg->dpcd_revision;
 	port->num_sdp_streams = port_msg->num_sdp_streams;
 	port->num_sdp_stream_sinks = port_msg->num_sdp_stream_sinks;
-	memcpy(port->guid, port_msg->peer_guid, 16);
 
 	/* manage mstb port lists with mgr lock - take a reference
 	   for this list */
@@ -1094,11 +1130,9 @@ static void drm_dp_add_port(struct drm_dp_mst_branch *mstb,
 
 	if (old_ddps != port->ddps) {
 		if (port->ddps) {
-			drm_dp_check_port_guid(mstb, port);
 			if (!port->input)
 				drm_dp_send_enum_path_resources(mstb->mgr, mstb, port);
 		} else {
-			port->guid_valid = false;
 			port->available_pbn = 0;
 			}
 	}
@@ -1157,10 +1191,8 @@ static void drm_dp_update_port(struct drm_dp_mst_branch *mstb,
 
 	if (old_ddps != port->ddps) {
 		if (port->ddps) {
-			drm_dp_check_port_guid(mstb, port);
 			dowork = true;
 		} else {
-			port->guid_valid = false;
 			port->available_pbn = 0;
 		}
 	}
@@ -1190,7 +1222,7 @@ static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device(struct drm_dp_mst_
 
 	for (i = 0; i < lct - 1; i++) {
 		int shift = (i % 2) ? 0 : 4;
-		int port_num = rad[i / 2] >> shift;
+		int port_num = (rad[i / 2] >> shift) & 0xf;
 
 		list_for_each_entry(port, &mstb->ports, next) {
 			if (port->port_num == port_num) {
@@ -1210,6 +1242,48 @@ out:
 	return mstb;
 }
 
+static struct drm_dp_mst_branch *get_mst_branch_device_by_guid_helper(
+	struct drm_dp_mst_branch *mstb,
+	uint8_t *guid)
+{
+	struct drm_dp_mst_branch *found_mstb;
+	struct drm_dp_mst_port *port;
+
+	if (memcmp(mstb->guid, guid, 16) == 0)
+		return mstb;
+
+
+	list_for_each_entry(port, &mstb->ports, next) {
+		if (!port->mstb)
+			continue;
+
+		found_mstb = get_mst_branch_device_by_guid_helper(port->mstb, guid);
+
+		if (found_mstb)
+			return found_mstb;
+	}
+
+	return NULL;
+}
+
+static struct drm_dp_mst_branch *drm_dp_get_mst_branch_device_by_guid(
+	struct drm_dp_mst_topology_mgr *mgr,
+	uint8_t *guid)
+{
+	struct drm_dp_mst_branch *mstb;
+
+	/* find the port by iterating down */
+	mutex_lock(&mgr->lock);
+
+	mstb = get_mst_branch_device_by_guid_helper(mgr->mst_primary, guid);
+
+	if (mstb)
+		kref_get(&mstb->kref);
+
+	mutex_unlock(&mgr->lock);
+	return mstb;
+}
+
 static void drm_dp_check_and_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
 					       struct drm_dp_mst_branch *mstb)
 {
@@ -1320,6 +1394,7 @@ static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
 				  struct drm_dp_sideband_msg_tx *txmsg)
 {
 	struct drm_dp_mst_branch *mstb = txmsg->dst;
+	u8 req_type;
 
 	/* both msg slots are full */
 	if (txmsg->seqno == -1) {
@@ -1336,7 +1411,13 @@ static int set_hdr_from_dst_qlock(struct drm_dp_sideband_msg_hdr *hdr,
 			txmsg->seqno = 1;
 		mstb->tx_slots[txmsg->seqno] = txmsg;
 	}
-	hdr->broadcast = 0;
+
+	req_type = txmsg->msg[0] & 0x7f;
+	if (req_type == DP_CONNECTION_STATUS_NOTIFY ||
+		req_type == DP_RESOURCE_STATUS_NOTIFY)
+		hdr->broadcast = 1;
+	else
+		hdr->broadcast = 0;
 	hdr->path_msg = txmsg->path_msg;
 	hdr->lct = mstb->lct;
 	hdr->lcr = mstb->lct - 1;
@@ -1438,26 +1519,18 @@ static void process_single_down_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
 }
 
 /* called holding qlock */
-static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr)
+static void process_single_up_tx_qlock(struct drm_dp_mst_topology_mgr *mgr,
+				       struct drm_dp_sideband_msg_tx *txmsg)
 {
-	struct drm_dp_sideband_msg_tx *txmsg;
 	int ret;
 
 	/* construct a chunk from the first msg in the tx_msg queue */
-	if (list_empty(&mgr->tx_msg_upq)) {
-		mgr->tx_up_in_progress = false;
-		return;
-	}
-
-	txmsg = list_first_entry(&mgr->tx_msg_upq, struct drm_dp_sideband_msg_tx, next);
 	ret = process_single_tx_qlock(mgr, txmsg, true);
-	if (ret == 1) {
-		/* up txmsgs aren't put in slots - so free after we send it */
-		list_del(&txmsg->next);
-		kfree(txmsg);
-	} else if (ret)
+
+	if (ret != 1)
 		DRM_DEBUG_KMS("failed to send msg in q %d\n", ret);
-	mgr->tx_up_in_progress = true;
+
+	txmsg->dst->tx_slots[txmsg->seqno] = NULL;
 }
 
 static void drm_dp_queue_down_tx(struct drm_dp_mst_topology_mgr *mgr,
@@ -1507,6 +1580,9 @@ static void drm_dp_send_link_address(struct drm_dp_mst_topology_mgr *mgr,
 				       txmsg->reply.u.link_addr.ports[i].num_sdp_streams,
 				       txmsg->reply.u.link_addr.ports[i].num_sdp_stream_sinks);
 			}
+
+			drm_dp_check_mstb_guid(mstb, txmsg->reply.u.link_addr.guid);
+
 			for (i = 0; i < txmsg->reply.u.link_addr.nports; i++) {
 				drm_dp_add_port(mstb, mgr->dev, &txmsg->reply.u.link_addr.ports[i]);
 			}
@@ -1554,6 +1630,37 @@ static int drm_dp_send_enum_path_resources(struct drm_dp_mst_topology_mgr *mgr,
 	return 0;
 }
 
+static struct drm_dp_mst_port *drm_dp_get_last_connected_port_to_mstb(struct drm_dp_mst_branch *mstb)
+{
+	if (!mstb->port_parent)
+		return NULL;
+
+	if (mstb->port_parent->mstb != mstb)
+		return mstb->port_parent;
+
+	return drm_dp_get_last_connected_port_to_mstb(mstb->port_parent->parent);
+}
+
+static struct drm_dp_mst_branch *drm_dp_get_last_connected_port_and_mstb(struct drm_dp_mst_topology_mgr *mgr,
+									 struct drm_dp_mst_branch *mstb,
+									 int *port_num)
+{
+	struct drm_dp_mst_branch *rmstb = NULL;
+	struct drm_dp_mst_port *found_port;
+	mutex_lock(&mgr->lock);
+	if (mgr->mst_primary) {
+		found_port = drm_dp_get_last_connected_port_to_mstb(mstb);
+
+		if (found_port) {
+			rmstb = found_port->parent;
+			kref_get(&rmstb->kref);
+			*port_num = found_port->port_num;
+		}
+	}
+	mutex_unlock(&mgr->lock);
+	return rmstb;
+}
+
 static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
 				   struct drm_dp_mst_port *port,
 				   int id,
@@ -1561,11 +1668,18 @@ static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
 {
 	struct drm_dp_sideband_msg_tx *txmsg;
 	struct drm_dp_mst_branch *mstb;
-	int len, ret;
+	int len, ret, port_num;
+	u8 sinks[DRM_DP_MAX_SDP_STREAMS];
+	int i;
 
+	port_num = port->port_num;
 	mstb = drm_dp_get_validated_mstb_ref(mgr, port->parent);
-	if (!mstb)
-		return -EINVAL;
+	if (!mstb) {
+		mstb = drm_dp_get_last_connected_port_and_mstb(mgr, port->parent, &port_num);
+
+		if (!mstb)
+			return -EINVAL;
+	}
 
 	txmsg = kzalloc(sizeof(*txmsg), GFP_KERNEL);
 	if (!txmsg) {
@@ -1573,10 +1687,13 @@ static int drm_dp_payload_send_msg(struct drm_dp_mst_topology_mgr *mgr,
 		goto fail_put;
 	}
 
+	for (i = 0; i < port->num_sdp_streams; i++)
+		sinks[i] = i;
+
 	txmsg->dst = mstb;
-	len = build_allocate_payload(txmsg, port->port_num,
+	len = build_allocate_payload(txmsg, port_num,
 				     id,
-				     pbn);
+				     pbn, port->num_sdp_streams, sinks);
 
 	drm_dp_queue_down_tx(mgr, txmsg);
 
@@ -1673,6 +1790,7 @@ int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
 		if (mgr->proposed_vcpis[i]) {
 			port = container_of(mgr->proposed_vcpis[i], struct drm_dp_mst_port, vcpi);
 			req_payload.num_slots = mgr->proposed_vcpis[i]->num_slots;
+			req_payload.vcpi = mgr->proposed_vcpis[i]->vcpi;
 		} else {
 			port = NULL;
 			req_payload.num_slots = 0;
@@ -1688,6 +1806,7 @@ int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
 			if (req_payload.num_slots) {
 				drm_dp_create_payload_step1(mgr, mgr->proposed_vcpis[i]->vcpi, &req_payload);
 				mgr->payloads[i].num_slots = req_payload.num_slots;
+				mgr->payloads[i].vcpi = req_payload.vcpi;
 			} else if (mgr->payloads[i].num_slots) {
 				mgr->payloads[i].num_slots = 0;
 				drm_dp_destroy_payload_step1(mgr, port, port->vcpi.vcpi, &mgr->payloads[i]);
@@ -1823,7 +1942,7 @@ static int drm_dp_encode_up_ack_reply(struct drm_dp_sideband_msg_tx *msg, u8 req
 {
 	struct drm_dp_sideband_msg_reply_body reply;
 
-	reply.reply_type = 1;
+	reply.reply_type = 0;
 	reply.req_type = req_type;
 	drm_dp_encode_sideband_reply(&reply, msg);
 	return 0;
@@ -1844,11 +1963,12 @@ static int drm_dp_send_up_ack_reply(struct drm_dp_mst_topology_mgr *mgr,
 	drm_dp_encode_up_ack_reply(txmsg, req_type);
 
 	mutex_lock(&mgr->qlock);
-	list_add_tail(&txmsg->next, &mgr->tx_msg_upq);
-	if (!mgr->tx_up_in_progress) {
-		process_single_up_tx_qlock(mgr);
-	}
+
+	process_single_up_tx_qlock(mgr, txmsg);
+
 	mutex_unlock(&mgr->qlock);
+
+	kfree(txmsg);
 	return 0;
 }
 
@@ -1927,31 +2047,17 @@ int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool ms
 		mgr->mst_primary = mstb;
 		kref_get(&mgr->mst_primary->kref);
 
-		{
-			struct drm_dp_payload reset_pay;
-			reset_pay.start_slot = 0;
-			reset_pay.num_slots = 0x3f;
-			drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
-		}
-
 		ret = drm_dp_dpcd_writeb(mgr->aux, DP_MSTM_CTRL,
-					 DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
+							 DP_MST_EN | DP_UP_REQ_EN | DP_UPSTREAM_IS_SRC);
 		if (ret < 0) {
 			goto out_unlock;
 		}
 
-
-		/* sort out guid */
-		ret = drm_dp_dpcd_read(mgr->aux, DP_GUID, mgr->guid, 16);
-		if (ret != 16) {
-			DRM_DEBUG_KMS("failed to read DP GUID %d\n", ret);
-			goto out_unlock;
-		}
-
-		mgr->guid_valid = drm_dp_validate_guid(mgr, mgr->guid);
-		if (!mgr->guid_valid) {
-			ret = drm_dp_dpcd_write(mgr->aux, DP_GUID, mgr->guid, 16);
-			mgr->guid_valid = true;
+		{
+			struct drm_dp_payload reset_pay;
+			reset_pay.start_slot = 0;
+			reset_pay.num_slots = 0x3f;
+			drm_dp_dpcd_write_payload(mgr, 0, &reset_pay);
 		}
 
 		queue_work(system_long_wq, &mgr->work);
@@ -2145,28 +2251,51 @@ static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
 
 	if (mgr->up_req_recv.have_eomt) {
 		struct drm_dp_sideband_msg_req_body msg;
-		struct drm_dp_mst_branch *mstb;
+		struct drm_dp_mst_branch *mstb = NULL;
 		bool seqno;
-		mstb = drm_dp_get_mst_branch_device(mgr,
-						    mgr->up_req_recv.initial_hdr.lct,
-						    mgr->up_req_recv.initial_hdr.rad);
-		if (!mstb) {
-			DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
-			memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
-			return 0;
+
+		if (!mgr->up_req_recv.initial_hdr.broadcast) {
+			mstb = drm_dp_get_mst_branch_device(mgr,
+							    mgr->up_req_recv.initial_hdr.lct,
+							    mgr->up_req_recv.initial_hdr.rad);
+			if (!mstb) {
+				DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
+				memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
+				return 0;
+			}
 		}
 
 		seqno = mgr->up_req_recv.initial_hdr.seqno;
 		drm_dp_sideband_parse_req(&mgr->up_req_recv, &msg);
 
 		if (msg.req_type == DP_CONNECTION_STATUS_NOTIFY) {
-			drm_dp_send_up_ack_reply(mgr, mstb, msg.req_type, seqno, false);
+			drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
+
+			if (!mstb)
+				mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.conn_stat.guid);
+
+			if (!mstb) {
+				DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
+				memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
+				return 0;
+			}
+
 			drm_dp_update_port(mstb, &msg.u.conn_stat);
+
 			DRM_DEBUG_KMS("Got CSN: pn: %d ldps:%d ddps: %d mcs: %d ip: %d pdt: %d\n", msg.u.conn_stat.port_number, msg.u.conn_stat.legacy_device_plug_status, msg.u.conn_stat.displayport_device_plug_status, msg.u.conn_stat.message_capability_status, msg.u.conn_stat.input_port, msg.u.conn_stat.peer_device_type);
 			(*mgr->cbs->hotplug)(mgr);
 
 		} else if (msg.req_type == DP_RESOURCE_STATUS_NOTIFY) {
-			drm_dp_send_up_ack_reply(mgr, mstb, msg.req_type, seqno, false);
+			drm_dp_send_up_ack_reply(mgr, mgr->mst_primary, msg.req_type, seqno, false);
+			if (!mstb)
+				mstb = drm_dp_get_mst_branch_device_by_guid(mgr, msg.u.resource_stat.guid);
+
+			if (!mstb) {
+				DRM_DEBUG_KMS("Got MST reply from unknown device %d\n", mgr->up_req_recv.initial_hdr.lct);
+				memset(&mgr->up_req_recv, 0, sizeof(struct drm_dp_sideband_msg_rx));
+				return 0;
+			}
+
 			DRM_DEBUG_KMS("Got RSN: pn: %d avail_pbn %d\n", msg.u.resource_stat.port_number, msg.u.resource_stat.available_pbn);
 		}
 
@@ -2259,6 +2388,27 @@ out:
 EXPORT_SYMBOL(drm_dp_mst_detect_port);
 
 /**
+ * drm_dp_mst_port_has_audio() - Check whether port has audio capability or not
+ * @mgr: manager for this port
+ * @port: unverified pointer to a port.
+ *
+ * This returns whether the port supports audio or not.
+ */
+bool drm_dp_mst_port_has_audio(struct drm_dp_mst_topology_mgr *mgr,
+					struct drm_dp_mst_port *port)
+{
+	bool ret = false;
+
+	port = drm_dp_get_validated_port_ref(mgr, port);
+	if (!port)
+		return ret;
+	ret = port->has_audio;
+	drm_dp_put_port(port);
+	return ret;
+}
+EXPORT_SYMBOL(drm_dp_mst_port_has_audio);
+
+/**
  * drm_dp_mst_get_edid() - get EDID for an MST port
  * @connector: toplevel connector to get EDID for
  * @mgr: manager for this port
@@ -2283,6 +2433,7 @@ struct edid *drm_dp_mst_get_edid(struct drm_connector *connector, struct drm_dp_
 		edid = drm_get_edid(connector, &port->aux.ddc);
 		drm_mode_connector_set_tile_property(connector);
 	}
+	port->has_audio = drm_detect_monitor_audio(edid);
 	drm_dp_put_port(port);
 	return edid;
 }
@@ -2346,6 +2497,7 @@ bool drm_dp_mst_allocate_vcpi(struct drm_dp_mst_topology_mgr *mgr, struct drm_dp
 		DRM_DEBUG_KMS("payload: vcpi %d already allocated for pbn %d - requested pbn %d\n", port->vcpi.vcpi, port->vcpi.pbn, pbn);
 		if (pbn == port->vcpi.pbn) {
 			*slots = port->vcpi.num_slots;
+			drm_dp_put_port(port);
 			return true;
 		}
 	}
@@ -2505,32 +2657,31 @@ EXPORT_SYMBOL(drm_dp_check_act_status);
  */
 int drm_dp_calc_pbn_mode(int clock, int bpp)
 {
-	fixed20_12 pix_bw;
-	fixed20_12 fbpp;
-	fixed20_12 result;
-	fixed20_12 margin, tmp;
-	u32 res;
-
-	pix_bw.full = dfixed_const(clock);
-	fbpp.full = dfixed_const(bpp);
-	tmp.full = dfixed_const(8);
-	fbpp.full = dfixed_div(fbpp, tmp);
-
-	result.full = dfixed_mul(pix_bw, fbpp);
-	margin.full = dfixed_const(54);
-	tmp.full = dfixed_const(64);
-	margin.full = dfixed_div(margin, tmp);
-	result.full = dfixed_div(result, margin);
-
-	margin.full = dfixed_const(1006);
-	tmp.full = dfixed_const(1000);
-	margin.full = dfixed_div(margin, tmp);
-	result.full = dfixed_mul(result, margin);
-
-	result.full = dfixed_div(result, tmp);
-	result.full = dfixed_ceil(result);
-	res = dfixed_trunc(result);
-	return res;
+	u64 kbps;
+	s64 peak_kbps;
+	u32 numerator;
+	u32 denominator;
+
+	kbps = clock * bpp;
+
+	/*
+	 * margin 5300ppm + 300ppm ~ 0.6% as per spec, factor is 1.006
+	 * The unit of 54/64Mbytes/sec is an arbitrary unit chosen based on
+	 * common multiplier to render an integer PBN for all link rate/lane
+	 * counts combinations
+	 * calculate
+	 * peak_kbps *= (1006/1000)
+	 * peak_kbps *= (64/54)
+	 * peak_kbps *= 8    convert to bytes
+	 */
+
+	numerator = 64 * 1006;
+	denominator = 54 * 8 * 1000 * 1000;
+
+	kbps *= numerator;
+	peak_kbps = drm_fixp_from_fraction(kbps, denominator);
+
+	return drm_fixp2int_ceil(peak_kbps);
 }
 EXPORT_SYMBOL(drm_dp_calc_pbn_mode);
 
@@ -2538,11 +2689,23 @@ static int test_calc_pbn_mode(void)
 {
 	int ret;
 	ret = drm_dp_calc_pbn_mode(154000, 30);
-	if (ret != 689)
+	if (ret != 689) {
+		DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
+				154000, 30, 689, ret);
 		return -EINVAL;
+	}
 	ret = drm_dp_calc_pbn_mode(234000, 30);
-	if (ret != 1047)
+	if (ret != 1047) {
+		DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
+				234000, 30, 1047, ret);
+		return -EINVAL;
+	}
+	ret = drm_dp_calc_pbn_mode(297000, 24);
+	if (ret != 1063) {
+		DRM_ERROR("PBN calculation test failed - clock %d, bpp %d, expected PBN %d, actual PBN %d.\n",
+				297000, 24, 1063, ret);
 		return -EINVAL;
+	}
 	return 0;
 }
 
@@ -2566,7 +2729,7 @@ static void drm_dp_mst_dump_mstb(struct seq_file *m,
 
 	seq_printf(m, "%smst: %p, %d\n", prefix, mstb, mstb->num_ports);
 	list_for_each_entry(port, &mstb->ports, next) {
-		seq_printf(m, "%sport: %d: ddps: %d ldps: %d, %p, conn: %p\n", prefix, port->port_num, port->ddps, port->ldps, port, port->connector);
+		seq_printf(m, "%sport: %d: ddps: %d ldps: %d, sdp: %d/%d, %p, conn: %p\n", prefix, port->port_num, port->ddps, port->ldps, port->num_sdp_streams, port->num_sdp_stream_sinks, port, port->connector);
 		if (port->mstb)
 			drm_dp_mst_dump_mstb(m, port->mstb);
 	}
@@ -2683,6 +2846,13 @@ static void drm_dp_tx_work(struct work_struct *work)
 	mutex_unlock(&mgr->qlock);
 }
 
+static void drm_dp_free_mst_port(struct kref *kref)
+{
+	struct drm_dp_mst_port *port = container_of(kref, struct drm_dp_mst_port, kref);
+	kref_put(&port->parent->kref, drm_dp_free_mst_branch_device);
+	kfree(port);
+}
+
 static void drm_dp_destroy_connector_work(struct work_struct *work)
 {
 	struct drm_dp_mst_topology_mgr *mgr = container_of(work, struct drm_dp_mst_topology_mgr, destroy_connector_work);
@@ -2703,13 +2873,22 @@ static void drm_dp_destroy_connector_work(struct work_struct *work)
 		list_del(&port->next);
 		mutex_unlock(&mgr->destroy_connector_lock);
 
+		kref_init(&port->kref);
+		INIT_LIST_HEAD(&port->next);
+
 		mgr->cbs->destroy_connector(mgr, port->connector);
 
 		drm_dp_port_teardown_pdt(port, port->pdt);
 
-		if (!port->input && port->vcpi.vcpi > 0)
-			drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
-		kfree(port);
+		if (!port->input && port->vcpi.vcpi > 0) {
+			if (mgr->mst_state) {
+				drm_dp_mst_reset_vcpi_slots(mgr, port);
+				drm_dp_update_payload_part1(mgr);
+				drm_dp_mst_put_payload_id(mgr, port->vcpi.vcpi);
+			}
+		}
+
+		kref_put(&port->kref, drm_dp_free_mst_port);
 		send_hotplug = true;
 	}
 	if (send_hotplug)
@@ -2736,7 +2915,6 @@ int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
 	mutex_init(&mgr->qlock);
 	mutex_init(&mgr->payload_lock);
 	mutex_init(&mgr->destroy_connector_lock);
-	INIT_LIST_HEAD(&mgr->tx_msg_upq);
 	INIT_LIST_HEAD(&mgr->tx_msg_downq);
 	INIT_LIST_HEAD(&mgr->destroy_connector_list);
 	INIT_WORK(&mgr->work, drm_dp_mst_link_probe_work);
@@ -2748,6 +2926,9 @@ int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
 	mgr->max_dpcd_transaction_bytes = max_dpcd_transaction_bytes;
 	mgr->max_payloads = max_payloads;
 	mgr->conn_base_id = conn_base_id;
+	if (max_payloads + 1 > sizeof(mgr->payload_mask) * 8 ||
+	    max_payloads + 1 > sizeof(mgr->vcpi_mask) * 8)
+		return -EINVAL;
 	mgr->payloads = kcalloc(max_payloads, sizeof(struct drm_dp_payload), GFP_KERNEL);
 	if (!mgr->payloads)
 		return -ENOMEM;
@@ -2755,7 +2936,9 @@ int drm_dp_mst_topology_mgr_init(struct drm_dp_mst_topology_mgr *mgr,
 	if (!mgr->proposed_vcpis)
 		return -ENOMEM;
 	set_bit(0, &mgr->payload_mask);
-	test_calc_pbn_mode();
+	if (test_calc_pbn_mode() < 0)
+		DRM_ERROR("MST PBN self-test failed\n");
+
 	return 0;
 }
 EXPORT_SYMBOL(drm_dp_mst_topology_mgr_init);
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 7dd6728dd092..167c8d3d4a31 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -44,10 +44,6 @@ MODULE_AUTHOR(CORE_AUTHOR);
 MODULE_DESCRIPTION(CORE_DESC);
 MODULE_LICENSE("GPL and additional rights");
 MODULE_PARM_DESC(debug, "Enable debug output");
-MODULE_PARM_DESC(vblankoffdelay, "Delay until vblank irq auto-disable [msecs] (0: never disable, <0: disable immediately)");
-MODULE_PARM_DESC(timestamp_precision_usec, "Max. error on timestamps [usecs]");
-MODULE_PARM_DESC(timestamp_monotonic, "Use monotonic timestamps");
-
 module_param_named(debug, drm_debug, int, 0600);
 
 static DEFINE_SPINLOCK(drm_minor_lock);
@@ -633,8 +629,17 @@ struct drm_device *drm_dev_alloc(struct drm_driver *driver,
 		}
 	}
 
+	if (parent) {
+		ret = drm_dev_set_unique(dev, dev_name(parent));
+		if (ret)
+			goto err_setunique;
+	}
+
 	return dev;
 
+err_setunique:
+	if (drm_core_check_feature(dev, DRIVER_GEM))
+		drm_gem_destroy(dev);
 err_ctxbitmap:
 	drm_legacy_ctxbitmap_cleanup(dev);
 	drm_ht_remove(&dev->map_hash);
@@ -797,23 +802,18 @@ EXPORT_SYMBOL(drm_dev_unregister);
 /**
  * drm_dev_set_unique - Set the unique name of a DRM device
  * @dev: device of which to set the unique name
- * @fmt: format string for unique name
+ * @name: unique name
  *
- * Sets the unique name of a DRM device using the specified format string and
- * a variable list of arguments. Drivers can use this at driver probe time if
- * the unique name of the devices they drive is static.
+ * Sets the unique name of a DRM device using the specified string. Drivers
+ * can use this at driver probe time if the unique name of the devices they
+ * drive is static.
  *
  * Return: 0 on success or a negative error code on failure.
  */
-int drm_dev_set_unique(struct drm_device *dev, const char *fmt, ...)
+int drm_dev_set_unique(struct drm_device *dev, const char *name)
 {
-	va_list ap;
-
 	kfree(dev->unique);
-
-	va_start(ap, fmt);
-	dev->unique = kvasprintf(GFP_KERNEL, fmt, ap);
-	va_end(ap);
+	dev->unique = kstrdup(name, GFP_KERNEL);
 
 	return dev->unique ? 0 : -ENOMEM;
 }
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index d5d2c03fd136..04cb4877fabd 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -637,8 +637,12 @@ static const struct minimode extra_modes[] = {
 /*
  * Probably taken from CEA-861 spec.
  * This table is converted from xorg's hw/xfree86/modes/xf86EdidModes.c.
+ *
+ * Index using the VIC.
  */
 static const struct drm_display_mode edid_cea_modes[] = {
+	/* 0 - dummy, VICs start at 1 */
+	{ },
 	/* 1 - 640x480@60Hz */
 	{ DRM_MODE("640x480", DRM_MODE_TYPE_DRIVER, 25175, 640, 656,
 		   752, 800, 0, 480, 490, 492, 525, 0,
@@ -987,9 +991,11 @@ static const struct drm_display_mode edid_cea_modes[] = {
 };
 
 /*
- * HDMI 1.4 4k modes.
+ * HDMI 1.4 4k modes. Index using the VIC.
  */
 static const struct drm_display_mode edid_4k_modes[] = {
+	/* 0 - dummy, VICs start at 1 */
+	{ },
 	/* 1 - 3840x2160@30Hz */
 	{ DRM_MODE("3840x2160", DRM_MODE_TYPE_DRIVER, 297000,
 		   3840, 4016, 4104, 4400, 0,
@@ -2545,6 +2551,33 @@ cea_mode_alternate_clock(const struct drm_display_mode *cea_mode)
 	return clock;
 }
 
+static u8 drm_match_cea_mode_clock_tolerance(const struct drm_display_mode *to_match,
+					     unsigned int clock_tolerance)
+{
+	u8 vic;
+
+	if (!to_match->clock)
+		return 0;
+
+	for (vic = 1; vic < ARRAY_SIZE(edid_cea_modes); vic++) {
+		const struct drm_display_mode *cea_mode = &edid_cea_modes[vic];
+		unsigned int clock1, clock2;
+
+		/* Check both 60Hz and 59.94Hz */
+		clock1 = cea_mode->clock;
+		clock2 = cea_mode_alternate_clock(cea_mode);
+
+		if (abs(to_match->clock - clock1) > clock_tolerance &&
+		    abs(to_match->clock - clock2) > clock_tolerance)
+			continue;
+
+		if (drm_mode_equal_no_clocks(to_match, cea_mode))
+			return vic;
+	}
+
+	return 0;
+}
+
 /**
  * drm_match_cea_mode - look for a CEA mode matching given mode
  * @to_match: display mode
@@ -2554,13 +2587,13 @@ cea_mode_alternate_clock(const struct drm_display_mode *cea_mode)
  */
 u8 drm_match_cea_mode(const struct drm_display_mode *to_match)
 {
-	u8 mode;
+	u8 vic;
 
 	if (!to_match->clock)
 		return 0;
 
-	for (mode = 0; mode < ARRAY_SIZE(edid_cea_modes); mode++) {
-		const struct drm_display_mode *cea_mode = &edid_cea_modes[mode];
+	for (vic = 1; vic < ARRAY_SIZE(edid_cea_modes); vic++) {
+		const struct drm_display_mode *cea_mode = &edid_cea_modes[vic];
 		unsigned int clock1, clock2;
 
 		/* Check both 60Hz and 59.94Hz */
@@ -2570,12 +2603,17 @@ u8 drm_match_cea_mode(const struct drm_display_mode *to_match)
 		if ((KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock1) ||
 		     KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock2)) &&
 		    drm_mode_equal_no_clocks_no_stereo(to_match, cea_mode))
-			return mode + 1;
+			return vic;
 	}
 	return 0;
 }
 EXPORT_SYMBOL(drm_match_cea_mode);
 
+static bool drm_valid_cea_vic(u8 vic)
+{
+	return vic > 0 && vic < ARRAY_SIZE(edid_cea_modes);
+}
+
 /**
  * drm_get_cea_aspect_ratio - get the picture aspect ratio corresponding to
  * the input VIC from the CEA mode list
@@ -2585,10 +2623,7 @@ EXPORT_SYMBOL(drm_match_cea_mode);
  */
 enum hdmi_picture_aspect drm_get_cea_aspect_ratio(const u8 video_code)
 {
-	/* return picture aspect ratio for video_code - 1 to access the
-	 * right array element
-	*/
-	return edid_cea_modes[video_code-1].picture_aspect_ratio;
+	return edid_cea_modes[video_code].picture_aspect_ratio;
 }
 EXPORT_SYMBOL(drm_get_cea_aspect_ratio);
 
@@ -2609,6 +2644,33 @@ hdmi_mode_alternate_clock(const struct drm_display_mode *hdmi_mode)
 	return cea_mode_alternate_clock(hdmi_mode);
 }
 
+static u8 drm_match_hdmi_mode_clock_tolerance(const struct drm_display_mode *to_match,
+					      unsigned int clock_tolerance)
+{
+	u8 vic;
+
+	if (!to_match->clock)
+		return 0;
+
+	for (vic = 1; vic < ARRAY_SIZE(edid_4k_modes); vic++) {
+		const struct drm_display_mode *hdmi_mode = &edid_4k_modes[vic];
+		unsigned int clock1, clock2;
+
+		/* Make sure to also match alternate clocks */
+		clock1 = hdmi_mode->clock;
+		clock2 = hdmi_mode_alternate_clock(hdmi_mode);
+
+		if (abs(to_match->clock - clock1) > clock_tolerance &&
+		    abs(to_match->clock - clock2) > clock_tolerance)
+			continue;
+
+		if (drm_mode_equal_no_clocks(to_match, hdmi_mode))
+			return vic;
+	}
+
+	return 0;
+}
+
 /*
  * drm_match_hdmi_mode - look for a HDMI mode matching given mode
  * @to_match: display mode
@@ -2619,13 +2681,13 @@ hdmi_mode_alternate_clock(const struct drm_display_mode *hdmi_mode)
  */
 static u8 drm_match_hdmi_mode(const struct drm_display_mode *to_match)
 {
-	u8 mode;
+	u8 vic;
 
 	if (!to_match->clock)
 		return 0;
 
-	for (mode = 0; mode < ARRAY_SIZE(edid_4k_modes); mode++) {
-		const struct drm_display_mode *hdmi_mode = &edid_4k_modes[mode];
+	for (vic = 1; vic < ARRAY_SIZE(edid_4k_modes); vic++) {
+		const struct drm_display_mode *hdmi_mode = &edid_4k_modes[vic];
 		unsigned int clock1, clock2;
 
 		/* Make sure to also match alternate clocks */
@@ -2635,11 +2697,16 @@ static u8 drm_match_hdmi_mode(const struct drm_display_mode *to_match)
 		if ((KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock1) ||
 		     KHZ2PICOS(to_match->clock) == KHZ2PICOS(clock2)) &&
 		    drm_mode_equal_no_clocks_no_stereo(to_match, hdmi_mode))
-			return mode + 1;
+			return vic;
 	}
 	return 0;
 }
 
+static bool drm_valid_hdmi_vic(u8 vic)
+{
+	return vic > 0 && vic < ARRAY_SIZE(edid_4k_modes);
+}
+
 static int
 add_alternate_cea_modes(struct drm_connector *connector, struct edid *edid)
 {
@@ -2659,16 +2726,16 @@ add_alternate_cea_modes(struct drm_connector *connector, struct edid *edid)
 	list_for_each_entry(mode, &connector->probed_modes, head) {
 		const struct drm_display_mode *cea_mode = NULL;
 		struct drm_display_mode *newmode;
-		u8 mode_idx = drm_match_cea_mode(mode) - 1;
+		u8 vic = drm_match_cea_mode(mode);
 		unsigned int clock1, clock2;
 
-		if (mode_idx < ARRAY_SIZE(edid_cea_modes)) {
-			cea_mode = &edid_cea_modes[mode_idx];
+		if (drm_valid_cea_vic(vic)) {
+			cea_mode = &edid_cea_modes[vic];
 			clock2 = cea_mode_alternate_clock(cea_mode);
 		} else {
-			mode_idx = drm_match_hdmi_mode(mode) - 1;
-			if (mode_idx < ARRAY_SIZE(edid_4k_modes)) {
-				cea_mode = &edid_4k_modes[mode_idx];
+			vic = drm_match_hdmi_mode(mode);
+			if (drm_valid_hdmi_vic(vic)) {
+				cea_mode = &edid_4k_modes[vic];
 				clock2 = hdmi_mode_alternate_clock(cea_mode);
 			}
 		}
@@ -2719,17 +2786,17 @@ drm_display_mode_from_vic_index(struct drm_connector *connector,
 {
 	struct drm_device *dev = connector->dev;
 	struct drm_display_mode *newmode;
-	u8 cea_mode;
+	u8 vic;
 
 	if (video_db == NULL || video_index >= video_len)
 		return NULL;
 
 	/* CEA modes are numbered 1..127 */
-	cea_mode = (video_db[video_index] & 127) - 1;
-	if (cea_mode >= ARRAY_SIZE(edid_cea_modes))
+	vic = (video_db[video_index] & 127);
+	if (!drm_valid_cea_vic(vic))
 		return NULL;
 
-	newmode = drm_mode_duplicate(dev, &edid_cea_modes[cea_mode]);
+	newmode = drm_mode_duplicate(dev, &edid_cea_modes[vic]);
 	if (!newmode)
 		return NULL;
 
@@ -2824,8 +2891,7 @@ static int add_hdmi_mode(struct drm_connector *connector, u8 vic)
 	struct drm_device *dev = connector->dev;
 	struct drm_display_mode *newmode;
 
-	vic--; /* VICs start at 1 */
-	if (vic >= ARRAY_SIZE(edid_4k_modes)) {
+	if (!drm_valid_hdmi_vic(vic)) {
 		DRM_ERROR("Unknown HDMI VIC: %d\n", vic);
 		return 0;
 	}
@@ -3116,20 +3182,24 @@ static void fixup_detailed_cea_mode_clock(struct drm_display_mode *mode)
 {
 	const struct drm_display_mode *cea_mode;
 	int clock1, clock2, clock;
-	u8 mode_idx;
+	u8 vic;
 	const char *type;
 
-	mode_idx = drm_match_cea_mode(mode) - 1;
-	if (mode_idx < ARRAY_SIZE(edid_cea_modes)) {
+	/*
+	 * allow 5kHz clock difference either way to account for
+	 * the 10kHz clock resolution limit of detailed timings.
+	 */
+	vic = drm_match_cea_mode_clock_tolerance(mode, 5);
+	if (drm_valid_cea_vic(vic)) {
 		type = "CEA";
-		cea_mode = &edid_cea_modes[mode_idx];
+		cea_mode = &edid_cea_modes[vic];
 		clock1 = cea_mode->clock;
 		clock2 = cea_mode_alternate_clock(cea_mode);
 	} else {
-		mode_idx = drm_match_hdmi_mode(mode) - 1;
-		if (mode_idx < ARRAY_SIZE(edid_4k_modes)) {
+		vic = drm_match_hdmi_mode_clock_tolerance(mode, 5);
+		if (drm_valid_hdmi_vic(vic)) {
 			type = "HDMI";
-			cea_mode = &edid_4k_modes[mode_idx];
+			cea_mode = &edid_4k_modes[vic];
 			clock1 = cea_mode->clock;
 			clock2 = hdmi_mode_alternate_clock(cea_mode);
 		} else {
@@ -3147,7 +3217,7 @@ static void fixup_detailed_cea_mode_clock(struct drm_display_mode *mode)
 		return;
 
 	DRM_DEBUG("detailed mode matches %s VIC %d, adjusting clock %d -> %d\n",
-		  type, mode_idx + 1, mode->clock, clock);
+		  type, vic, mode->clock, clock);
 	mode->clock = clock;
 }
 
diff --git a/drivers/gpu/drm/drm_encoder_slave.c b/drivers/gpu/drm/drm_encoder_slave.c
index d18b88b755c3..e8629076de32 100644
--- a/drivers/gpu/drm/drm_encoder_slave.c
+++ b/drivers/gpu/drm/drm_encoder_slave.c
@@ -124,7 +124,7 @@ EXPORT_SYMBOL(drm_i2c_encoder_destroy);
  * Wrapper fxns which can be plugged in to drm_encoder_helper_funcs:
  */
 
-static inline struct drm_encoder_slave_funcs *
+static inline const struct drm_encoder_slave_funcs *
 get_slave_funcs(struct drm_encoder *enc)
 {
 	return to_encoder_slave(enc)->slave_funcs;
diff --git a/drivers/gpu/drm/drm_fb_cma_helper.c b/drivers/gpu/drm/drm_fb_cma_helper.c
index c19a62561183..c895b6fddbd8 100644
--- a/drivers/gpu/drm/drm_fb_cma_helper.c
+++ b/drivers/gpu/drm/drm_fb_cma_helper.c
@@ -74,7 +74,7 @@ static struct drm_framebuffer_funcs drm_fb_cma_funcs = {
 };
 
 static struct drm_fb_cma *drm_fb_cma_alloc(struct drm_device *dev,
-	struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_cma_object **obj,
+	const const struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_cma_object **obj,
 	unsigned int num_planes)
 {
 	struct drm_fb_cma *fb_cma;
@@ -107,7 +107,7 @@ static struct drm_fb_cma *drm_fb_cma_alloc(struct drm_device *dev,
  * checked before calling this function.
  */
 struct drm_framebuffer *drm_fb_cma_create(struct drm_device *dev,
-	struct drm_file *file_priv, struct drm_mode_fb_cmd2 *mode_cmd)
+	struct drm_file *file_priv, const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_fb_cma *fb_cma;
 	struct drm_gem_cma_object *objs[4];
@@ -266,7 +266,7 @@ static int drm_fbdev_cma_create(struct drm_fb_helper *helper,
 	fbi = drm_fb_helper_alloc_fbi(helper);
 	if (IS_ERR(fbi)) {
 		ret = PTR_ERR(fbi);
-		goto err_drm_gem_cma_free_object;
+		goto err_gem_free_object;
 	}
 
 	fbdev_cma->fb = drm_fb_cma_alloc(dev, &mode_cmd, &obj, 1);
@@ -299,8 +299,8 @@ static int drm_fbdev_cma_create(struct drm_fb_helper *helper,
 
 err_fb_info_destroy:
 	drm_fb_helper_release_fbi(helper);
-err_drm_gem_cma_free_object:
-	drm_gem_cma_free_object(&obj->base);
+err_gem_free_object:
+	dev->driver->gem_free_object(&obj->base);
 	return ret;
 }
 
@@ -348,9 +348,6 @@ struct drm_fbdev_cma *drm_fbdev_cma_init(struct drm_device *dev,
 
 	}
 
-	/* disable all the possible outputs/crtcs before entering KMS mode */
-	drm_helper_disable_unused_functions(dev);
-
 	ret = drm_fb_helper_initial_config(helper, preferred_bpp);
 	if (ret < 0) {
 		dev_err(dev->dev, "Failed to set initial hw configuration.\n");
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 69cbab5e5c81..1e103c4c6ee0 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -1251,7 +1251,7 @@ retry:
 			goto fail;
 
 		plane = mode_set->crtc->primary;
-		plane_mask |= drm_plane_index(plane);
+		plane_mask |= (1 << drm_plane_index(plane));
 		plane->old_fb = plane->fb;
 	}
 
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 6b5625e66119..1ea8790e5090 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -226,6 +226,8 @@ static int drm_open_helper(struct file *filp, struct drm_minor *minor)
 	init_waitqueue_head(&priv->event_wait);
 	priv->event_space = 4096; /* set aside 4k for event buffer */
 
+	mutex_init(&priv->event_read_lock);
+
 	if (drm_core_check_feature(dev, DRIVER_GEM))
 		drm_gem_open(dev, priv);
 
@@ -511,14 +513,28 @@ ssize_t drm_read(struct file *filp, char __user *buffer,
 {
 	struct drm_file *file_priv = filp->private_data;
 	struct drm_device *dev = file_priv->minor->dev;
-	ssize_t ret = 0;
+	ssize_t ret;
 
 	if (!access_ok(VERIFY_WRITE, buffer, count))
 		return -EFAULT;
 
-	spin_lock_irq(&dev->event_lock);
+	ret = mutex_lock_interruptible(&file_priv->event_read_lock);
+	if (ret)
+		return ret;
+
 	for (;;) {
-		if (list_empty(&file_priv->event_list)) {
+		struct drm_pending_event *e = NULL;
+
+		spin_lock_irq(&dev->event_lock);
+		if (!list_empty(&file_priv->event_list)) {
+			e = list_first_entry(&file_priv->event_list,
+					struct drm_pending_event, link);
+			file_priv->event_space += e->event->length;
+			list_del(&e->link);
+		}
+		spin_unlock_irq(&dev->event_lock);
+
+		if (e == NULL) {
 			if (ret)
 				break;
 
@@ -527,36 +543,36 @@ ssize_t drm_read(struct file *filp, char __user *buffer,
 				break;
 			}
 
-			spin_unlock_irq(&dev->event_lock);
+			mutex_unlock(&file_priv->event_read_lock);
 			ret = wait_event_interruptible(file_priv->event_wait,
 						       !list_empty(&file_priv->event_list));
-			spin_lock_irq(&dev->event_lock);
-			if (ret < 0)
-				break;
-
-			ret = 0;
+			if (ret >= 0)
+				ret = mutex_lock_interruptible(&file_priv->event_read_lock);
+			if (ret)
+				return ret;
 		} else {
-			struct drm_pending_event *e;
-
-			e = list_first_entry(&file_priv->event_list,
-					     struct drm_pending_event, link);
-			if (e->event->length + ret > count)
+			unsigned length = e->event->length;
+
+			if (length > count - ret) {
+put_back_event:
+				spin_lock_irq(&dev->event_lock);
+				file_priv->event_space -= length;
+				list_add(&e->link, &file_priv->event_list);
+				spin_unlock_irq(&dev->event_lock);
 				break;
+			}
 
-			if (__copy_to_user_inatomic(buffer + ret,
-						    e->event, e->event->length)) {
+			if (copy_to_user(buffer + ret, e->event, length)) {
 				if (ret == 0)
 					ret = -EFAULT;
-				break;
+				goto put_back_event;
 			}
 
-			file_priv->event_space += e->event->length;
-			ret += e->event->length;
-			list_del(&e->link);
+			ret += length;
 			e->destroy(e);
 		}
 	}
-	spin_unlock_irq(&dev->event_lock);
+	mutex_unlock(&file_priv->event_read_lock);
 
 	return ret;
 }
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index c7de454e8e88..2e8c77e71e1f 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -220,6 +220,9 @@ static void drm_gem_object_exported_dma_buf_free(struct drm_gem_object *obj)
 static void
 drm_gem_object_handle_unreference_unlocked(struct drm_gem_object *obj)
 {
+	struct drm_device *dev = obj->dev;
+	bool final = false;
+
 	if (WARN_ON(obj->handle_count == 0))
 		return;
 
@@ -229,14 +232,39 @@ drm_gem_object_handle_unreference_unlocked(struct drm_gem_object *obj)
 	* checked for a name
 	*/
 
-	mutex_lock(&obj->dev->object_name_lock);
+	mutex_lock(&dev->object_name_lock);
 	if (--obj->handle_count == 0) {
 		drm_gem_object_handle_free(obj);
 		drm_gem_object_exported_dma_buf_free(obj);
+		final = true;
 	}
-	mutex_unlock(&obj->dev->object_name_lock);
+	mutex_unlock(&dev->object_name_lock);
 
-	drm_gem_object_unreference_unlocked(obj);
+	if (final)
+		drm_gem_object_unreference_unlocked(obj);
+}
+
+/*
+ * Called at device or object close to release the file's
+ * handle references on objects.
+ */
+static int
+drm_gem_object_release_handle(int id, void *ptr, void *data)
+{
+	struct drm_file *file_priv = data;
+	struct drm_gem_object *obj = ptr;
+	struct drm_device *dev = obj->dev;
+
+	if (drm_core_check_feature(dev, DRIVER_PRIME))
+		drm_gem_remove_prime_handles(obj, file_priv);
+	drm_vma_node_revoke(&obj->vma_node, file_priv->filp);
+
+	if (dev->driver->gem_close_object)
+		dev->driver->gem_close_object(obj, file_priv);
+
+	drm_gem_object_handle_unreference_unlocked(obj);
+
+	return 0;
 }
 
 /**
@@ -244,8 +272,9 @@ drm_gem_object_handle_unreference_unlocked(struct drm_gem_object *obj)
  * @filp: drm file-private structure to use for the handle look up
  * @handle: userspace handle to delete
  *
- * Removes the GEM handle from the @filp lookup table and if this is the last
- * handle also cleans up linked resources like GEM names.
+ * Removes the GEM handle from the @filp lookup table which has been added with
+ * drm_gem_handle_create(). If this is the last handle also cleans up linked
+ * resources like GEM names.
  */
 int
 drm_gem_handle_delete(struct drm_file *filp, u32 handle)
@@ -276,14 +305,7 @@ drm_gem_handle_delete(struct drm_file *filp, u32 handle)
 	idr_remove(&filp->object_idr, handle);
 	spin_unlock(&filp->table_lock);
 
-	if (drm_core_check_feature(dev, DRIVER_PRIME))
-		drm_gem_remove_prime_handles(obj, filp);
-	drm_vma_node_revoke(&obj->vma_node, filp->filp);
-
-	if (dev->driver->gem_close_object)
-		dev->driver->gem_close_object(obj, filp);
-	drm_gem_object_handle_unreference_unlocked(obj);
-
+	drm_gem_object_release_handle(handle, obj, filp);
 	return 0;
 }
 EXPORT_SYMBOL(drm_gem_handle_delete);
@@ -314,6 +336,10 @@ EXPORT_SYMBOL(drm_gem_dumb_destroy);
  * This expects the dev->object_name_lock to be held already and will drop it
  * before returning. Used to avoid races in establishing new handles when
  * importing an object from either an flink name or a dma-buf.
+ *
+ * Handles must be release again through drm_gem_handle_delete(). This is done
+ * when userspace closes @file_priv for all attached handles, or through the
+ * GEM_CLOSE ioctl for individual handles.
  */
 int
 drm_gem_handle_create_tail(struct drm_file *file_priv,
@@ -321,9 +347,12 @@ drm_gem_handle_create_tail(struct drm_file *file_priv,
 			   u32 *handlep)
 {
 	struct drm_device *dev = obj->dev;
+	u32 handle;
 	int ret;
 
 	WARN_ON(!mutex_is_locked(&dev->object_name_lock));
+	if (obj->handle_count++ == 0)
+		drm_gem_object_reference(obj);
 
 	/*
 	 * Get the user-visible handle using idr.  Preload and perform
@@ -333,32 +362,38 @@ drm_gem_handle_create_tail(struct drm_file *file_priv,
 	spin_lock(&file_priv->table_lock);
 
 	ret = idr_alloc(&file_priv->object_idr, obj, 1, 0, GFP_NOWAIT);
-	drm_gem_object_reference(obj);
-	obj->handle_count++;
+
 	spin_unlock(&file_priv->table_lock);
 	idr_preload_end();
+
 	mutex_unlock(&dev->object_name_lock);
-	if (ret < 0) {
-		drm_gem_object_handle_unreference_unlocked(obj);
-		return ret;
-	}
-	*handlep = ret;
+	if (ret < 0)
+		goto err_unref;
+
+	handle = ret;
 
 	ret = drm_vma_node_allow(&obj->vma_node, file_priv->filp);
-	if (ret) {
-		drm_gem_handle_delete(file_priv, *handlep);
-		return ret;
-	}
+	if (ret)
+		goto err_remove;
 
 	if (dev->driver->gem_open_object) {
 		ret = dev->driver->gem_open_object(obj, file_priv);
-		if (ret) {
-			drm_gem_handle_delete(file_priv, *handlep);
-			return ret;
-		}
+		if (ret)
+			goto err_revoke;
 	}
 
+	*handlep = handle;
 	return 0;
+
+err_revoke:
+	drm_vma_node_revoke(&obj->vma_node, file_priv->filp);
+err_remove:
+	spin_lock(&file_priv->table_lock);
+	idr_remove(&file_priv->object_idr, handle);
+	spin_unlock(&file_priv->table_lock);
+err_unref:
+	drm_gem_object_handle_unreference_unlocked(obj);
+	return ret;
 }
 
 /**
@@ -541,7 +576,17 @@ void drm_gem_put_pages(struct drm_gem_object *obj, struct page **pages,
 }
 EXPORT_SYMBOL(drm_gem_put_pages);
 
-/** Returns a reference to the object named by the handle. */
+/**
+ * drm_gem_object_lookup - look up a GEM object from it's handle
+ * @dev: DRM device
+ * @filp: DRM file private date
+ * @handle: userspace handle
+ *
+ * Returns:
+ *
+ * A reference to the object named by the handle if such exists on @filp, NULL
+ * otherwise.
+ */
 struct drm_gem_object *
 drm_gem_object_lookup(struct drm_device *dev, struct drm_file *filp,
 		      u32 handle)
@@ -615,7 +660,6 @@ drm_gem_flink_ioctl(struct drm_device *dev, void *data,
 		return -ENOENT;
 
 	mutex_lock(&dev->object_name_lock);
-	idr_preload(GFP_KERNEL);
 	/* prevent races with concurrent gem_close. */
 	if (obj->handle_count == 0) {
 		ret = -ENOENT;
@@ -623,7 +667,7 @@ drm_gem_flink_ioctl(struct drm_device *dev, void *data,
 	}
 
 	if (!obj->name) {
-		ret = idr_alloc(&dev->object_name_idr, obj, 1, 0, GFP_NOWAIT);
+		ret = idr_alloc(&dev->object_name_idr, obj, 1, 0, GFP_KERNEL);
 		if (ret < 0)
 			goto err;
 
@@ -634,7 +678,6 @@ drm_gem_flink_ioctl(struct drm_device *dev, void *data,
 	ret = 0;
 
 err:
-	idr_preload_end();
 	mutex_unlock(&dev->object_name_lock);
 	drm_gem_object_unreference_unlocked(obj);
 	return ret;
@@ -699,29 +742,6 @@ drm_gem_open(struct drm_device *dev, struct drm_file *file_private)
 	spin_lock_init(&file_private->table_lock);
 }
 
-/*
- * Called at device close to release the file's
- * handle references on objects.
- */
-static int
-drm_gem_object_release_handle(int id, void *ptr, void *data)
-{
-	struct drm_file *file_priv = data;
-	struct drm_gem_object *obj = ptr;
-	struct drm_device *dev = obj->dev;
-
-	if (drm_core_check_feature(dev, DRIVER_PRIME))
-		drm_gem_remove_prime_handles(obj, file_priv);
-	drm_vma_node_revoke(&obj->vma_node, file_priv->filp);
-
-	if (dev->driver->gem_close_object)
-		dev->driver->gem_close_object(obj, file_priv);
-
-	drm_gem_object_handle_unreference_unlocked(obj);
-
-	return 0;
-}
-
 /**
  * drm_gem_release - release file-private GEM resources
  * @dev: drm_device which is being closed by userspace
@@ -774,6 +794,13 @@ drm_gem_object_free(struct kref *kref)
 }
 EXPORT_SYMBOL(drm_gem_object_free);
 
+/**
+ * drm_gem_vm_open - vma->ops->open implementation for GEM
+ * @vma: VM area structure
+ *
+ * This function implements the #vm_operations_struct open() callback for GEM
+ * drivers. This must be used together with drm_gem_vm_close().
+ */
 void drm_gem_vm_open(struct vm_area_struct *vma)
 {
 	struct drm_gem_object *obj = vma->vm_private_data;
@@ -782,6 +809,13 @@ void drm_gem_vm_open(struct vm_area_struct *vma)
 }
 EXPORT_SYMBOL(drm_gem_vm_open);
 
+/**
+ * drm_gem_vm_close - vma->ops->close implementation for GEM
+ * @vma: VM area structure
+ *
+ * This function implements the #vm_operations_struct close() callback for GEM
+ * drivers. This must be used together with drm_gem_vm_open().
+ */
 void drm_gem_vm_close(struct vm_area_struct *vma)
 {
 	struct drm_gem_object *obj = vma->vm_private_data;
diff --git a/drivers/gpu/drm/drm_gem_cma_helper.c b/drivers/gpu/drm/drm_gem_cma_helper.c
index e109b49cd25d..e5df53b6e229 100644
--- a/drivers/gpu/drm/drm_gem_cma_helper.c
+++ b/drivers/gpu/drm/drm_gem_cma_helper.c
@@ -59,11 +59,13 @@ __drm_gem_cma_create(struct drm_device *drm, size_t size)
 	struct drm_gem_object *gem_obj;
 	int ret;
 
-	cma_obj = kzalloc(sizeof(*cma_obj), GFP_KERNEL);
-	if (!cma_obj)
+	if (drm->driver->gem_create_object)
+		gem_obj = drm->driver->gem_create_object(drm, size);
+	else
+		gem_obj = kzalloc(sizeof(*cma_obj), GFP_KERNEL);
+	if (!gem_obj)
 		return ERR_PTR(-ENOMEM);
-
-	gem_obj = &cma_obj->base;
+	cma_obj = container_of(gem_obj, struct drm_gem_cma_object, base);
 
 	ret = drm_gem_object_init(drm, gem_obj, size);
 	if (ret)
@@ -119,7 +121,7 @@ struct drm_gem_cma_object *drm_gem_cma_create(struct drm_device *drm,
 	return cma_obj;
 
 error:
-	drm_gem_cma_free_object(&cma_obj->base);
+	drm->driver->gem_free_object(&cma_obj->base);
 	return ERR_PTR(ret);
 }
 EXPORT_SYMBOL_GPL(drm_gem_cma_create);
@@ -169,7 +171,7 @@ drm_gem_cma_create_with_handle(struct drm_file *file_priv,
 	return cma_obj;
 
 err_handle_create:
-	drm_gem_cma_free_object(gem_obj);
+	drm->driver->gem_free_object(gem_obj);
 
 	return ERR_PTR(ret);
 }
diff --git a/drivers/gpu/drm/drm_hashtab.c b/drivers/gpu/drm/drm_hashtab.c
index c3b80fd65d62..7b30b307674b 100644
--- a/drivers/gpu/drm/drm_hashtab.c
+++ b/drivers/gpu/drm/drm_hashtab.c
@@ -198,10 +198,7 @@ EXPORT_SYMBOL(drm_ht_remove_item);
 void drm_ht_remove(struct drm_open_hash *ht)
 {
 	if (ht->table) {
-		if ((PAGE_SIZE / sizeof(*ht->table)) >> ht->order)
-			kfree(ht->table);
-		else
-			vfree(ht->table);
+		kvfree(ht->table);
 		ht->table = NULL;
 	}
 }
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index 607f493ae801..1fe14579e8c9 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -73,6 +73,9 @@ static int drm_vblank_offdelay = 5000;    /* Default to 5000 msecs. */
 module_param_named(vblankoffdelay, drm_vblank_offdelay, int, 0600);
 module_param_named(timestamp_precision_usec, drm_timestamp_precision, int, 0600);
 module_param_named(timestamp_monotonic, drm_timestamp_monotonic, int, 0600);
+MODULE_PARM_DESC(vblankoffdelay, "Delay until vblank irq auto-disable [msecs] (0: never disable, <0: disable immediately)");
+MODULE_PARM_DESC(timestamp_precision_usec, "Max. error on timestamps [usecs]");
+MODULE_PARM_DESC(timestamp_monotonic, "Use monotonic timestamps");
 
 static void store_vblank(struct drm_device *dev, unsigned int pipe,
 			 u32 vblank_count_inc,
@@ -221,6 +224,64 @@ static void drm_update_vblank_count(struct drm_device *dev, unsigned int pipe,
 		diff = (flags & DRM_CALLED_FROM_VBLIRQ) != 0;
 	}
 
+	/*
+	 * Within a drm_vblank_pre_modeset - drm_vblank_post_modeset
+	 * interval? If so then vblank irqs keep running and it will likely
+	 * happen that the hardware vblank counter is not trustworthy as it
+	 * might reset at some point in that interval and vblank timestamps
+	 * are not trustworthy either in that interval. Iow. this can result
+	 * in a bogus diff >> 1 which must be avoided as it would cause
+	 * random large forward jumps of the software vblank counter.
+	 */
+	if (diff > 1 && (vblank->inmodeset & 0x2)) {
+		DRM_DEBUG_VBL("clamping vblank bump to 1 on crtc %u: diffr=%u"
+			      " due to pre-modeset.\n", pipe, diff);
+		diff = 1;
+	}
+
+	/*
+	 * FIMXE: Need to replace this hack with proper seqlocks.
+	 *
+	 * Restrict the bump of the software vblank counter to a safe maximum
+	 * value of +1 whenever there is the possibility that concurrent readers
+	 * of vblank timestamps could be active at the moment, as the current
+	 * implementation of the timestamp caching and updating is not safe
+	 * against concurrent readers for calls to store_vblank() with a bump
+	 * of anything but +1. A bump != 1 would very likely return corrupted
+	 * timestamps to userspace, because the same slot in the cache could
+	 * be concurrently written by store_vblank() and read by one of those
+	 * readers without the read-retry logic detecting the collision.
+	 *
+	 * Concurrent readers can exist when we are called from the
+	 * drm_vblank_off() or drm_vblank_on() functions and other non-vblank-
+	 * irq callers. However, all those calls to us are happening with the
+	 * vbl_lock locked to prevent drm_vblank_get(), so the vblank refcount
+	 * can't increase while we are executing. Therefore a zero refcount at
+	 * this point is safe for arbitrary counter bumps if we are called
+	 * outside vblank irq, a non-zero count is not 100% safe. Unfortunately
+	 * we must also accept a refcount of 1, as whenever we are called from
+	 * drm_vblank_get() -> drm_vblank_enable() the refcount will be 1 and
+	 * we must let that one pass through in order to not lose vblank counts
+	 * during vblank irq off - which would completely defeat the whole
+	 * point of this routine.
+	 *
+	 * Whenever we are called from vblank irq, we have to assume concurrent
+	 * readers exist or can show up any time during our execution, even if
+	 * the refcount is currently zero, as vblank irqs are usually only
+	 * enabled due to the presence of readers, and because when we are called
+	 * from vblank irq we can't hold the vbl_lock to protect us from sudden
+	 * bumps in vblank refcount. Therefore also restrict bumps to +1 when
+	 * called from vblank irq.
+	 */
+	if ((diff > 1) && (atomic_read(&vblank->refcount) > 1 ||
+	    (flags & DRM_CALLED_FROM_VBLIRQ))) {
+		DRM_DEBUG_VBL("clamping vblank bump to 1 on crtc %u: diffr=%u "
+			      "refcount %u, vblirq %u\n", pipe, diff,
+			      atomic_read(&vblank->refcount),
+			      (flags & DRM_CALLED_FROM_VBLIRQ) != 0);
+		diff = 1;
+	}
+
 	DRM_DEBUG_VBL("updating vblank count on crtc %u:"
 		      " current=%u, diff=%u, hw=%u hw_last=%u\n",
 		      pipe, vblank->count, diff, cur_vblank, vblank->last);
@@ -1313,7 +1374,13 @@ void drm_vblank_off(struct drm_device *dev, unsigned int pipe)
 	spin_lock_irqsave(&dev->event_lock, irqflags);
 
 	spin_lock(&dev->vbl_lock);
-	vblank_disable_and_save(dev, pipe);
+	DRM_DEBUG_VBL("crtc %d, vblank enabled %d, inmodeset %d\n",
+		      pipe, vblank->enabled, vblank->inmodeset);
+
+	/* Avoid redundant vblank disables without previous drm_vblank_on(). */
+	if (drm_core_check_feature(dev, DRIVER_ATOMIC) || !vblank->inmodeset)
+		vblank_disable_and_save(dev, pipe);
+
 	wake_up(&vblank->queue);
 
 	/*
@@ -1415,6 +1482,9 @@ void drm_vblank_on(struct drm_device *dev, unsigned int pipe)
 		return;
 
 	spin_lock_irqsave(&dev->vbl_lock, irqflags);
+	DRM_DEBUG_VBL("crtc %d, vblank enabled %d, inmodeset %d\n",
+		      pipe, vblank->enabled, vblank->inmodeset);
+
 	/* Drop our private "prevent drm_vblank_get" refcount */
 	if (vblank->inmodeset) {
 		atomic_dec(&vblank->refcount);
@@ -1427,8 +1497,7 @@ void drm_vblank_on(struct drm_device *dev, unsigned int pipe)
 	 * re-enable interrupts if there are users left, or the
 	 * user wishes vblank interrupts to be enabled all the time.
 	 */
-	if (atomic_read(&vblank->refcount) != 0 ||
-	    (!dev->vblank_disable_immediate && drm_vblank_offdelay == 0))
+	if (atomic_read(&vblank->refcount) != 0 || drm_vblank_offdelay == 0)
 		WARN_ON(drm_vblank_enable(dev, pipe));
 	spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
 }
@@ -1523,6 +1592,7 @@ void drm_vblank_post_modeset(struct drm_device *dev, unsigned int pipe)
 	if (vblank->inmodeset) {
 		spin_lock_irqsave(&dev->vbl_lock, irqflags);
 		dev->vblank_disable_allowed = true;
+		drm_reset_vblank_timestamp(dev, pipe);
 		spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
 
 		if (vblank->inmodeset & 0x2)
diff --git a/drivers/gpu/drm/drm_mipi_dsi.c b/drivers/gpu/drm/drm_mipi_dsi.c
index 2d5ca8eec13a..6e6a9c58d404 100644
--- a/drivers/gpu/drm/drm_mipi_dsi.c
+++ b/drivers/gpu/drm/drm_mipi_dsi.c
@@ -365,6 +365,44 @@ int mipi_dsi_create_packet(struct mipi_dsi_packet *packet,
 }
 EXPORT_SYMBOL(mipi_dsi_create_packet);
 
+/**
+ * mipi_dsi_shutdown_peripheral() - sends a Shutdown Peripheral command
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_shutdown_peripheral(struct mipi_dsi_device *dsi)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.type = MIPI_DSI_SHUTDOWN_PERIPHERAL,
+		.tx_buf = (u8 [2]) { 0, 0 },
+		.tx_len = 2,
+	};
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_shutdown_peripheral);
+
+/**
+ * mipi_dsi_turn_on_peripheral() - sends a Turn On Peripheral command
+ * @dsi: DSI peripheral device
+ *
+ * Return: 0 on success or a negative error code on failure.
+ */
+int mipi_dsi_turn_on_peripheral(struct mipi_dsi_device *dsi)
+{
+	struct mipi_dsi_msg msg = {
+		.channel = dsi->channel,
+		.type = MIPI_DSI_TURN_ON_PERIPHERAL,
+		.tx_buf = (u8 [2]) { 0, 0 },
+		.tx_len = 2,
+	};
+
+	return mipi_dsi_device_transfer(dsi, &msg);
+}
+EXPORT_SYMBOL(mipi_dsi_turn_on_peripheral);
+
 /*
  * mipi_dsi_set_maximum_return_packet_size() - specify the maximum size of the
  *    the payload in a long packet transmitted from the peripheral back to the
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index cd74a0953f42..20775c05235a 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -553,10 +553,10 @@ EXPORT_SYMBOL(drm_gtf_mode_complex);
  * drivers/video/fbmon.c
  *
  * Standard GTF parameters:
- * M = 600
- * C = 40
- * K = 128
- * J = 20
+ *     M = 600
+ *     C = 40
+ *     K = 128
+ *     J = 20
  *
  * Returns:
  * The modeline based on the GTF algorithm stored in a drm_display_mode object.
@@ -708,7 +708,8 @@ void drm_mode_set_name(struct drm_display_mode *mode)
 }
 EXPORT_SYMBOL(drm_mode_set_name);
 
-/** drm_mode_hsync - get the hsync of a mode
+/**
+ * drm_mode_hsync - get the hsync of a mode
  * @mode: mode
  *
  * Returns:
@@ -917,13 +918,30 @@ bool drm_mode_equal(const struct drm_display_mode *mode1, const struct drm_displ
 	} else if (mode1->clock != mode2->clock)
 		return false;
 
+	return drm_mode_equal_no_clocks(mode1, mode2);
+}
+EXPORT_SYMBOL(drm_mode_equal);
+
+/**
+ * drm_mode_equal_no_clocks - test modes for equality
+ * @mode1: first mode
+ * @mode2: second mode
+ *
+ * Check to see if @mode1 and @mode2 are equivalent, but
+ * don't check the pixel clocks.
+ *
+ * Returns:
+ * True if the modes are equal, false otherwise.
+ */
+bool drm_mode_equal_no_clocks(const struct drm_display_mode *mode1, const struct drm_display_mode *mode2)
+{
 	if ((mode1->flags & DRM_MODE_FLAG_3D_MASK) !=
 	    (mode2->flags & DRM_MODE_FLAG_3D_MASK))
 		return false;
 
 	return drm_mode_equal_no_clocks_no_stereo(mode1, mode2);
 }
-EXPORT_SYMBOL(drm_mode_equal);
+EXPORT_SYMBOL(drm_mode_equal_no_clocks);
 
 /**
  * drm_mode_equal_no_clocks_no_stereo - test modes for equality
@@ -1056,7 +1074,7 @@ static const char * const drm_mode_status_names[] = {
 	MODE_STATUS(ONE_SIZE),
 	MODE_STATUS(NO_REDUCED),
 	MODE_STATUS(NO_STEREO),
-	MODE_STATUS(UNVERIFIED),
+	MODE_STATUS(STALE),
 	MODE_STATUS(BAD),
 	MODE_STATUS(ERROR),
 };
@@ -1154,7 +1172,6 @@ EXPORT_SYMBOL(drm_mode_sort);
 /**
  * drm_mode_connector_list_update - update the mode list for the connector
  * @connector: the connector to update
- * @merge_type_bits: whether to merge or overwrite type bits
  *
  * This moves the modes from the @connector probed_modes list
  * to the actual mode list. It compares the probed mode against the current
@@ -1163,33 +1180,48 @@ EXPORT_SYMBOL(drm_mode_sort);
  * This is just a helper functions doesn't validate any modes itself and also
  * doesn't prune any invalid modes. Callers need to do that themselves.
  */
-void drm_mode_connector_list_update(struct drm_connector *connector,
-				    bool merge_type_bits)
+void drm_mode_connector_list_update(struct drm_connector *connector)
 {
-	struct drm_display_mode *mode;
 	struct drm_display_mode *pmode, *pt;
-	int found_it;
 
 	WARN_ON(!mutex_is_locked(&connector->dev->mode_config.mutex));
 
-	list_for_each_entry_safe(pmode, pt, &connector->probed_modes,
-				 head) {
-		found_it = 0;
+	list_for_each_entry_safe(pmode, pt, &connector->probed_modes, head) {
+		struct drm_display_mode *mode;
+		bool found_it = false;
+
 		/* go through current modes checking for the new probed mode */
 		list_for_each_entry(mode, &connector->modes, head) {
-			if (drm_mode_equal(pmode, mode)) {
-				found_it = 1;
-				/* if equal delete the probed mode */
-				mode->status = pmode->status;
-				/* Merge type bits together */
-				if (merge_type_bits)
-					mode->type |= pmode->type;
-				else
-					mode->type = pmode->type;
-				list_del(&pmode->head);
-				drm_mode_destroy(connector->dev, pmode);
-				break;
+			if (!drm_mode_equal(pmode, mode))
+				continue;
+
+			found_it = true;
+
+			/*
+			 * If the old matching mode is stale (ie. left over
+			 * from a previous probe) just replace it outright.
+			 * Otherwise just merge the type bits between all
+			 * equal probed modes.
+			 *
+			 * If two probed modes are considered equal, pick the
+			 * actual timings from the one that's marked as
+			 * preferred (in case the match isn't 100%). If
+			 * multiple or zero preferred modes are present, favor
+			 * the mode added to the probed_modes list first.
+			 */
+			if (mode->status == MODE_STALE) {
+				drm_mode_copy(mode, pmode);
+			} else if ((mode->type & DRM_MODE_TYPE_PREFERRED) == 0 &&
+				   (pmode->type & DRM_MODE_TYPE_PREFERRED) != 0) {
+				pmode->type |= mode->type;
+				drm_mode_copy(mode, pmode);
+			} else {
+				mode->type |= pmode->type;
 			}
+
+			list_del(&pmode->head);
+			drm_mode_destroy(connector->dev, pmode);
+			break;
 		}
 
 		if (!found_it) {
@@ -1212,7 +1244,7 @@ EXPORT_SYMBOL(drm_mode_connector_list_update);
  * This uses the same parameters as the fb modedb.c, except for an extra
  * force-enable, force-enable-digital and force-disable bit at the end:
  *
- *	<xres>x<yres>[M][R][-<bpp>][@<refresh>][i][m][eDd]
+ * <xres>x<yres>[M][R][-<bpp>][@<refresh>][i][m][eDd]
  *
  * The intermediate drm_cmdline_mode structure is required to store additional
  * options from the command line modline like the force-enable/disable flag.
@@ -1491,4 +1523,4 @@ int drm_mode_convert_umode(struct drm_display_mode *out,
 
 out:
 	return ret;
-}
\ No newline at end of file
+}
diff --git a/drivers/gpu/drm/drm_modeset_lock.c b/drivers/gpu/drm/drm_modeset_lock.c
index 6675b1428410..e3a4adf03e7b 100644
--- a/drivers/gpu/drm/drm_modeset_lock.c
+++ b/drivers/gpu/drm/drm_modeset_lock.c
@@ -40,28 +40,33 @@
  * The basic usage pattern is to:
  *
  *     drm_modeset_acquire_init(&ctx)
- *   retry:
+ *     retry:
  *     foreach (lock in random_ordered_set_of_locks) {
- *       ret = drm_modeset_lock(lock, &ctx)
- *       if (ret == -EDEADLK) {
- *          drm_modeset_backoff(&ctx);
- *          goto retry;
- *       }
+ *         ret = drm_modeset_lock(lock, &ctx)
+ *         if (ret == -EDEADLK) {
+ *             drm_modeset_backoff(&ctx);
+ *             goto retry;
+ *         }
  *     }
- *
  *     ... do stuff ...
- *
  *     drm_modeset_drop_locks(&ctx);
  *     drm_modeset_acquire_fini(&ctx);
  */
 
 /**
  * drm_modeset_lock_all - take all modeset locks
- * @dev: drm device
+ * @dev: DRM device
  *
  * This function takes all modeset locks, suitable where a more fine-grained
- * scheme isn't (yet) implemented. Locks must be dropped with
- * drm_modeset_unlock_all.
+ * scheme isn't (yet) implemented. Locks must be dropped by calling the
+ * drm_modeset_unlock_all() function.
+ *
+ * This function is deprecated. It allocates a lock acquisition context and
+ * stores it in the DRM device's ->mode_config. This facilitate conversion of
+ * existing code because it removes the need to manually deal with the
+ * acquisition context, but it is also brittle because the context is global
+ * and care must be taken not to nest calls. New code should use the
+ * drm_modeset_lock_all_ctx() function and pass in the context explicitly.
  */
 void drm_modeset_lock_all(struct drm_device *dev)
 {
@@ -78,39 +83,43 @@ void drm_modeset_lock_all(struct drm_device *dev)
 	drm_modeset_acquire_init(ctx, 0);
 
 retry:
-	ret = drm_modeset_lock(&config->connection_mutex, ctx);
-	if (ret)
-		goto fail;
-	ret = drm_modeset_lock_all_crtcs(dev, ctx);
-	if (ret)
-		goto fail;
+	ret = drm_modeset_lock_all_ctx(dev, ctx);
+	if (ret < 0) {
+		if (ret == -EDEADLK) {
+			drm_modeset_backoff(ctx);
+			goto retry;
+		}
+
+		drm_modeset_acquire_fini(ctx);
+		kfree(ctx);
+		return;
+	}
 
 	WARN_ON(config->acquire_ctx);
 
-	/* now we hold the locks, so now that it is safe, stash the
-	 * ctx for drm_modeset_unlock_all():
+	/*
+	 * We hold the locks now, so it is safe to stash the acquisition
+	 * context for drm_modeset_unlock_all().
 	 */
 	config->acquire_ctx = ctx;
 
 	drm_warn_on_modeset_not_all_locked(dev);
-
-	return;
-
-fail:
-	if (ret == -EDEADLK) {
-		drm_modeset_backoff(ctx);
-		goto retry;
-	}
-
-	kfree(ctx);
 }
 EXPORT_SYMBOL(drm_modeset_lock_all);
 
 /**
  * drm_modeset_unlock_all - drop all modeset locks
- * @dev: device
+ * @dev: DRM device
  *
- * This function drop all modeset locks taken by drm_modeset_lock_all.
+ * This function drops all modeset locks taken by a previous call to the
+ * drm_modeset_lock_all() function.
+ *
+ * This function is deprecated. It uses the lock acquisition context stored
+ * in the DRM device's ->mode_config. This facilitates conversion of existing
+ * code because it removes the need to manually deal with the acquisition
+ * context, but it is also brittle because the context is global and care must
+ * be taken not to nest calls. New code should pass the acquisition context
+ * directly to the drm_modeset_drop_locks() function.
  */
 void drm_modeset_unlock_all(struct drm_device *dev)
 {
@@ -431,14 +440,34 @@ void drm_modeset_unlock(struct drm_modeset_lock *lock)
 }
 EXPORT_SYMBOL(drm_modeset_unlock);
 
-/* In some legacy codepaths it's convenient to just grab all the crtc and plane
- * related locks. */
-int drm_modeset_lock_all_crtcs(struct drm_device *dev,
-		struct drm_modeset_acquire_ctx *ctx)
+/**
+ * drm_modeset_lock_all_ctx - take all modeset locks
+ * @dev: DRM device
+ * @ctx: lock acquisition context
+ *
+ * This function takes all modeset locks, suitable where a more fine-grained
+ * scheme isn't (yet) implemented.
+ *
+ * Unlike drm_modeset_lock_all(), it doesn't take the dev->mode_config.mutex
+ * since that lock isn't required for modeset state changes. Callers which
+ * need to grab that lock too need to do so outside of the acquire context
+ * @ctx.
+ *
+ * Locks acquired with this function should be released by calling the
+ * drm_modeset_drop_locks() function on @ctx.
+ *
+ * Returns: 0 on success or a negative error-code on failure.
+ */
+int drm_modeset_lock_all_ctx(struct drm_device *dev,
+			     struct drm_modeset_acquire_ctx *ctx)
 {
 	struct drm_crtc *crtc;
 	struct drm_plane *plane;
-	int ret = 0;
+	int ret;
+
+	ret = drm_modeset_lock(&dev->mode_config.connection_mutex, ctx);
+	if (ret)
+		return ret;
 
 	drm_for_each_crtc(crtc, dev) {
 		ret = drm_modeset_lock(&crtc->mutex, ctx);
@@ -454,4 +483,4 @@ int drm_modeset_lock_all_crtcs(struct drm_device *dev,
 
 	return 0;
 }
-EXPORT_SYMBOL(drm_modeset_lock_all_crtcs);
+EXPORT_SYMBOL(drm_modeset_lock_all_ctx);
diff --git a/drivers/gpu/drm/drm_pci.c b/drivers/gpu/drm/drm_pci.c
index fcd2a86acd2c..a1fff1179a97 100644
--- a/drivers/gpu/drm/drm_pci.c
+++ b/drivers/gpu/drm/drm_pci.c
@@ -410,6 +410,26 @@ int drm_pcie_get_speed_cap_mask(struct drm_device *dev, u32 *mask)
 }
 EXPORT_SYMBOL(drm_pcie_get_speed_cap_mask);
 
+int drm_pcie_get_max_link_width(struct drm_device *dev, u32 *mlw)
+{
+	struct pci_dev *root;
+	u32 lnkcap;
+
+	*mlw = 0;
+	if (!dev->pdev)
+		return -EINVAL;
+
+	root = dev->pdev->bus->self;
+
+	pcie_capability_read_dword(root, PCI_EXP_LNKCAP, &lnkcap);
+
+	*mlw = (lnkcap & PCI_EXP_LNKCAP_MLW) >> 4;
+
+	DRM_INFO("probing mlw for device %x:%x = %x\n", root->vendor, root->device, lnkcap);
+	return 0;
+}
+EXPORT_SYMBOL(drm_pcie_get_max_link_width);
+
 #else
 
 int drm_pci_init(struct drm_driver *driver, struct pci_driver *pdriver)
diff --git a/drivers/gpu/drm/drm_plane_helper.c b/drivers/gpu/drm/drm_plane_helper.c
index d384ebcf0aaf..369d2898ff9e 100644
--- a/drivers/gpu/drm/drm_plane_helper.c
+++ b/drivers/gpu/drm/drm_plane_helper.c
@@ -57,6 +57,10 @@
  * by the atomic helpers.
  *
  * Again drivers are strongly urged to switch to the new interfaces.
+ *
+ * The plane helpers share the function table structures with other helpers,
+ * specifically also the atomic helpers. See struct &drm_plane_helper_funcs for
+ * the details.
  */
 
 /*
@@ -164,6 +168,8 @@ int drm_plane_helper_check_update(struct drm_plane *plane,
 	vscale = drm_rect_calc_vscale(src, dest, min_scale, max_scale);
 	if (hscale < 0 || vscale < 0) {
 		DRM_DEBUG_KMS("Invalid scaling of plane\n");
+		drm_rect_debug_print("src: ", src, true);
+		drm_rect_debug_print("dst: ", dest, false);
 		return -ERANGE;
 	}
 
@@ -180,6 +186,8 @@ int drm_plane_helper_check_update(struct drm_plane *plane,
 
 	if (!can_position && !drm_rect_equals(dest, clip)) {
 		DRM_DEBUG_KMS("Plane must cover entire CRTC\n");
+		drm_rect_debug_print("dst: ", dest, false);
+		drm_rect_debug_print("clip: ", clip, false);
 		return -EINVAL;
 	}
 
@@ -367,7 +375,7 @@ static struct drm_plane *create_primary_plane(struct drm_device *dev)
 				       &drm_primary_helper_funcs,
 				       safe_modeset_formats,
 				       ARRAY_SIZE(safe_modeset_formats),
-				       DRM_PLANE_TYPE_PRIMARY);
+				       DRM_PLANE_TYPE_PRIMARY, NULL);
 	if (ret) {
 		kfree(primary);
 		primary = NULL;
@@ -394,7 +402,8 @@ int drm_crtc_init(struct drm_device *dev, struct drm_crtc *crtc,
 	struct drm_plane *primary;
 
 	primary = create_primary_plane(dev);
-	return drm_crtc_init_with_planes(dev, crtc, primary, NULL, funcs);
+	return drm_crtc_init_with_planes(dev, crtc, primary, NULL, funcs,
+					 NULL);
 }
 EXPORT_SYMBOL(drm_crtc_init);
 
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 9f935f55d74c..27aa7183b20b 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -313,19 +313,15 @@ static const struct dma_buf_ops drm_gem_prime_dmabuf_ops =  {
  *
  * Export callbacks:
  *
- *  - @gem_prime_pin (optional): prepare a GEM object for exporting
- *
- *  - @gem_prime_get_sg_table: provide a scatter/gather table of pinned pages
- *
- *  - @gem_prime_vmap: vmap a buffer exported by your driver
- *
- *  - @gem_prime_vunmap: vunmap a buffer exported by your driver
- *
- *  - @gem_prime_mmap (optional): mmap a buffer exported by your driver
+ *  * @gem_prime_pin (optional): prepare a GEM object for exporting
+ *  * @gem_prime_get_sg_table: provide a scatter/gather table of pinned pages
+ *  * @gem_prime_vmap: vmap a buffer exported by your driver
+ *  * @gem_prime_vunmap: vunmap a buffer exported by your driver
+ *  * @gem_prime_mmap (optional): mmap a buffer exported by your driver
  *
  * Import callback:
  *
- *  - @gem_prime_import_sg_table (import): produce a GEM object from another
+ *  * @gem_prime_import_sg_table (import): produce a GEM object from another
  *    driver's scatter/gather table
  */
 
diff --git a/drivers/gpu/drm/drm_probe_helper.c b/drivers/gpu/drm/drm_probe_helper.c
index f8b5fcfa91a2..e714b5a7955f 100644
--- a/drivers/gpu/drm/drm_probe_helper.c
+++ b/drivers/gpu/drm/drm_probe_helper.c
@@ -53,6 +53,9 @@
  * This helper library can be used independently of the modeset helper library.
  * Drivers can also overwrite different parts e.g. use their own hotplug
  * handling code to avoid probing unrelated outputs.
+ *
+ * The probe helpers share the function table structures with other display
+ * helper libraries. See struct &drm_connector_helper_funcs for the details.
  */
 
 static bool drm_kms_helper_poll = true;
@@ -126,9 +129,64 @@ void drm_kms_helper_poll_enable_locked(struct drm_device *dev)
 }
 EXPORT_SYMBOL(drm_kms_helper_poll_enable_locked);
 
-
-static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connector *connector,
-							      uint32_t maxX, uint32_t maxY, bool merge_type_bits)
+/**
+ * drm_helper_probe_single_connector_modes - get complete set of display modes
+ * @connector: connector to probe
+ * @maxX: max width for modes
+ * @maxY: max height for modes
+ *
+ * Based on the helper callbacks implemented by @connector in struct
+ * &drm_connector_helper_funcs try to detect all valid modes.  Modes will first
+ * be added to the connector's probed_modes list, then culled (based on validity
+ * and the @maxX, @maxY parameters) and put into the normal modes list.
+ *
+ * Intended to be used as a generic implementation of the ->fill_modes()
+ * @connector vfunc for drivers that use the CRTC helpers for output mode
+ * filtering and detection.
+ *
+ * The basic procedure is as follows
+ *
+ * 1. All modes currently on the connector's modes list are marked as stale
+ *
+ * 2. New modes are added to the connector's probed_modes list with
+ *    drm_mode_probed_add(). New modes start their life with status as OK.
+ *    Modes are added from a single source using the following priority order.
+ *
+ *    - debugfs 'override_edid' (used for testing only)
+ *    - firmware EDID (drm_load_edid_firmware())
+ *    - connector helper ->get_modes() vfunc
+ *    - if the connector status is connector_status_connected, standard
+ *      VESA DMT modes up to 1024x768 are automatically added
+ *      (drm_add_modes_noedid())
+ *
+ *    Finally modes specified via the kernel command line (video=...) are
+ *    added in addition to what the earlier probes produced
+ *    (drm_helper_probe_add_cmdline_mode()). These modes are generated
+ *    using the VESA GTF/CVT formulas.
+ *
+ * 3. Modes are moved from the probed_modes list to the modes list. Potential
+ *    duplicates are merged together (see drm_mode_connector_list_update()).
+ *    After this step the probed_modes list will be empty again.
+ *
+ * 4. Any non-stale mode on the modes list then undergoes validation
+ *
+ *    - drm_mode_validate_basic() performs basic sanity checks
+ *    - drm_mode_validate_size() filters out modes larger than @maxX and @maxY
+ *      (if specified)
+ *    - drm_mode_validate_flag() checks the modes againt basic connector
+ *      capabilites (interlace_allowed,doublescan_allowed,stereo_allowed)
+ *    - the optional connector ->mode_valid() helper can perform driver and/or
+ *      hardware specific checks
+ *
+ * 5. Any mode whose status is not OK is pruned from the connector's modes list,
+ *    accompanied by a debug message indicating the reason for the mode's
+ *    rejection (see drm_mode_prune_invalid()).
+ *
+ * Returns:
+ * The number of modes found on @connector.
+ */
+int drm_helper_probe_single_connector_modes(struct drm_connector *connector,
+					    uint32_t maxX, uint32_t maxY)
 {
 	struct drm_device *dev = connector->dev;
 	struct drm_display_mode *mode;
@@ -143,9 +201,11 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n", connector->base.id,
 			connector->name);
-	/* set all modes to the unverified state */
+	/* set all old modes to the stale state */
 	list_for_each_entry(mode, &connector->modes, head)
-		mode->status = MODE_UNVERIFIED;
+		mode->status = MODE_STALE;
+
+	old_status = connector->status;
 
 	if (connector->force) {
 		if (connector->force == DRM_FORCE_ON ||
@@ -156,33 +216,32 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 		if (connector->funcs->force)
 			connector->funcs->force(connector);
 	} else {
-		old_status = connector->status;
-
 		connector->status = connector->funcs->detect(connector, true);
+	}
+
+	/*
+	 * Normally either the driver's hpd code or the poll loop should
+	 * pick up any changes and fire the hotplug event. But if
+	 * userspace sneaks in a probe, we might miss a change. Hence
+	 * check here, and if anything changed start the hotplug code.
+	 */
+	if (old_status != connector->status) {
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] status updated from %s to %s\n",
+			      connector->base.id,
+			      connector->name,
+			      drm_get_connector_status_name(old_status),
+			      drm_get_connector_status_name(connector->status));
 
 		/*
-		 * Normally either the driver's hpd code or the poll loop should
-		 * pick up any changes and fire the hotplug event. But if
-		 * userspace sneaks in a probe, we might miss a change. Hence
-		 * check here, and if anything changed start the hotplug code.
+		 * The hotplug event code might call into the fb
+		 * helpers, and so expects that we do not hold any
+		 * locks. Fire up the poll struct instead, it will
+		 * disable itself again.
 		 */
-		if (old_status != connector->status) {
-			DRM_DEBUG_KMS("[CONNECTOR:%d:%s] status updated from %d to %d\n",
-				      connector->base.id,
-				      connector->name,
-				      old_status, connector->status);
-
-			/*
-			 * The hotplug event code might call into the fb
-			 * helpers, and so expects that we do not hold any
-			 * locks. Fire up the poll struct instead, it will
-			 * disable itself again.
-			 */
-			dev->mode_config.delayed_event = true;
-			if (dev->mode_config.poll_enabled)
-				schedule_delayed_work(&dev->mode_config.output_poll_work,
-						      0);
-		}
+		dev->mode_config.delayed_event = true;
+		if (dev->mode_config.poll_enabled)
+			schedule_delayed_work(&dev->mode_config.output_poll_work,
+					      0);
 	}
 
 	/* Re-enable polling in case the global poll config changed. */
@@ -199,17 +258,16 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 		goto prune;
 	}
 
+	if (connector->override_edid) {
+		struct edid *edid = (struct edid *) connector->edid_blob_ptr->data;
+
+		count = drm_add_edid_modes(connector, edid);
+		drm_edid_to_eld(connector, edid);
+	} else {
 #ifdef CONFIG_DRM_LOAD_EDID_FIRMWARE
-	count = drm_load_edid_firmware(connector);
-	if (count == 0)
+		count = drm_load_edid_firmware(connector);
+		if (count == 0)
 #endif
-	{
-		if (connector->override_edid) {
-			struct edid *edid = (struct edid *) connector->edid_blob_ptr->data;
-
-			count = drm_add_edid_modes(connector, edid);
-			drm_edid_to_eld(connector, edid);
-		} else
 			count = (*connector_funcs->get_modes)(connector);
 	}
 
@@ -219,7 +277,7 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 	if (count == 0)
 		goto prune;
 
-	drm_mode_connector_list_update(connector, merge_type_bits);
+	drm_mode_connector_list_update(connector);
 
 	if (connector->interlace_allowed)
 		mode_flags |= DRM_MODE_FLAG_INTERLACE;
@@ -263,49 +321,9 @@ prune:
 
 	return count;
 }
-
-/**
- * drm_helper_probe_single_connector_modes - get complete set of display modes
- * @connector: connector to probe
- * @maxX: max width for modes
- * @maxY: max height for modes
- *
- * Based on the helper callbacks implemented by @connector try to detect all
- * valid modes.  Modes will first be added to the connector's probed_modes list,
- * then culled (based on validity and the @maxX, @maxY parameters) and put into
- * the normal modes list.
- *
- * Intended to be use as a generic implementation of the ->fill_modes()
- * @connector vfunc for drivers that use the crtc helpers for output mode
- * filtering and detection.
- *
- * Returns:
- * The number of modes found on @connector.
- */
-int drm_helper_probe_single_connector_modes(struct drm_connector *connector,
-					    uint32_t maxX, uint32_t maxY)
-{
-	return drm_helper_probe_single_connector_modes_merge_bits(connector, maxX, maxY, true);
-}
 EXPORT_SYMBOL(drm_helper_probe_single_connector_modes);
 
 /**
- * drm_helper_probe_single_connector_modes_nomerge - get complete set of display modes
- * @connector: connector to probe
- * @maxX: max width for modes
- * @maxY: max height for modes
- *
- * This operates like drm_hehlper_probe_single_connector_modes except it
- * replaces the mode bits instead of merging them for preferred modes.
- */
-int drm_helper_probe_single_connector_modes_nomerge(struct drm_connector *connector,
-					    uint32_t maxX, uint32_t maxY)
-{
-	return drm_helper_probe_single_connector_modes_merge_bits(connector, maxX, maxY, false);
-}
-EXPORT_SYMBOL(drm_helper_probe_single_connector_modes_nomerge);
-
-/**
  * drm_kms_helper_hotplug_event - fire off KMS hotplug events
  * @dev: drm_device whose connector state changed
  *
diff --git a/drivers/gpu/drm/drm_rect.c b/drivers/gpu/drm/drm_rect.c
index 531ac4cc9756..a8e2c8603945 100644
--- a/drivers/gpu/drm/drm_rect.c
+++ b/drivers/gpu/drm/drm_rect.c
@@ -275,22 +275,23 @@ EXPORT_SYMBOL(drm_rect_calc_vscale_relaxed);
 
 /**
  * drm_rect_debug_print - print the rectangle information
+ * @prefix: prefix string
  * @r: rectangle to print
  * @fixed_point: rectangle is in 16.16 fixed point format
  */
-void drm_rect_debug_print(const struct drm_rect *r, bool fixed_point)
+void drm_rect_debug_print(const char *prefix, const struct drm_rect *r, bool fixed_point)
 {
 	int w = drm_rect_width(r);
 	int h = drm_rect_height(r);
 
 	if (fixed_point)
-		DRM_DEBUG_KMS("%d.%06ux%d.%06u%+d.%06u%+d.%06u\n",
+		DRM_DEBUG_KMS("%s%d.%06ux%d.%06u%+d.%06u%+d.%06u\n", prefix,
 			      w >> 16, ((w & 0xffff) * 15625) >> 10,
 			      h >> 16, ((h & 0xffff) * 15625) >> 10,
 			      r->x1 >> 16, ((r->x1 & 0xffff) * 15625) >> 10,
 			      r->y1 >> 16, ((r->y1 & 0xffff) * 15625) >> 10);
 	else
-		DRM_DEBUG_KMS("%dx%d%+d%+d\n", w, h, r->x1, r->y1);
+		DRM_DEBUG_KMS("%s%dx%d%+d%+d\n", prefix, w, h, r->x1, r->y1);
 }
 EXPORT_SYMBOL(drm_rect_debug_print);
 
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index 615b7e667320..d503f8e8c2d1 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -167,47 +167,35 @@ static ssize_t status_store(struct device *device,
 {
 	struct drm_connector *connector = to_drm_connector(device);
 	struct drm_device *dev = connector->dev;
-	enum drm_connector_status old_status;
+	enum drm_connector_force old_force;
 	int ret;
 
 	ret = mutex_lock_interruptible(&dev->mode_config.mutex);
 	if (ret)
 		return ret;
 
-	old_status = connector->status;
+	old_force = connector->force;
 
-	if (sysfs_streq(buf, "detect")) {
+	if (sysfs_streq(buf, "detect"))
 		connector->force = 0;
-		connector->status = connector->funcs->detect(connector, true);
-	} else if (sysfs_streq(buf, "on")) {
+	else if (sysfs_streq(buf, "on"))
 		connector->force = DRM_FORCE_ON;
-	} else if (sysfs_streq(buf, "on-digital")) {
+	else if (sysfs_streq(buf, "on-digital"))
 		connector->force = DRM_FORCE_ON_DIGITAL;
-	} else if (sysfs_streq(buf, "off")) {
+	else if (sysfs_streq(buf, "off"))
 		connector->force = DRM_FORCE_OFF;
-	} else
+	else
 		ret = -EINVAL;
 
-	if (ret == 0 && connector->force) {
-		if (connector->force == DRM_FORCE_ON ||
-		    connector->force == DRM_FORCE_ON_DIGITAL)
-			connector->status = connector_status_connected;
-		else
-			connector->status = connector_status_disconnected;
-		if (connector->funcs->force)
-			connector->funcs->force(connector);
-	}
-
-	if (old_status != connector->status) {
-		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] status updated from %d to %d\n",
+	if (old_force != connector->force || !connector->force) {
+		DRM_DEBUG_KMS("[CONNECTOR:%d:%s] force updated from %d to %d or reprobing\n",
 			      connector->base.id,
 			      connector->name,
-			      old_status, connector->status);
+			      old_force, connector->force);
 
-		dev->mode_config.delayed_event = true;
-		if (dev->mode_config.poll_enabled)
-			schedule_delayed_work(&dev->mode_config.output_poll_work,
-					      0);
+		connector->funcs->fill_modes(connector,
+					     dev->mode_config.max_width,
+					     dev->mode_config.max_height);
 	}
 
 	mutex_unlock(&dev->mode_config.mutex);
@@ -252,27 +240,33 @@ static ssize_t edid_show(struct file *filp, struct kobject *kobj,
 			 struct bin_attribute *attr, char *buf, loff_t off,
 			 size_t count)
 {
-	struct device *connector_dev = container_of(kobj, struct device, kobj);
+	struct device *connector_dev = kobj_to_dev(kobj);
 	struct drm_connector *connector = to_drm_connector(connector_dev);
 	unsigned char *edid;
 	size_t size;
+	ssize_t ret = 0;
 
+	mutex_lock(&connector->dev->mode_config.mutex);
 	if (!connector->edid_blob_ptr)
-		return 0;
+		goto unlock;
 
 	edid = connector->edid_blob_ptr->data;
 	size = connector->edid_blob_ptr->length;
 	if (!edid)
-		return 0;
+		goto unlock;
 
 	if (off >= size)
-		return 0;
+		goto unlock;
 
 	if (off + count > size)
 		count = size - off;
 	memcpy(buf, edid + off, count);
 
-	return count;
+	ret = count;
+unlock:
+	mutex_unlock(&connector->dev->mode_config.mutex);
+
+	return ret;
 }
 
 static ssize_t modes_show(struct device *device,
@@ -283,10 +277,12 @@ static ssize_t modes_show(struct device *device,
 	struct drm_display_mode *mode;
 	int written = 0;
 
+	mutex_lock(&connector->dev->mode_config.mutex);
 	list_for_each_entry(mode, &connector->modes, head) {
 		written += snprintf(buf + written, PAGE_SIZE - written, "%s\n",
 				    mode->name);
 	}
+	mutex_unlock(&connector->dev->mode_config.mutex);
 
 	return written;
 }
diff --git a/drivers/gpu/drm/etnaviv/Kconfig b/drivers/gpu/drm/etnaviv/Kconfig
new file mode 100644
index 000000000000..2cde7a5442fb
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/Kconfig
@@ -0,0 +1,20 @@
+
+config DRM_ETNAVIV
+	tristate "ETNAVIV (DRM support for Vivante GPU IP cores)"
+	depends on DRM
+	depends on ARCH_MXC || ARCH_DOVE
+	select SHMEM
+	select TMPFS
+	select IOMMU_API
+	select IOMMU_SUPPORT
+	select WANT_DEV_COREDUMP
+	help
+	  DRM driver for Vivante GPUs.
+
+config DRM_ETNAVIV_REGISTER_LOGGING
+	bool "enable ETNAVIV register logging"
+	depends on DRM_ETNAVIV
+	help
+	  Compile in support for logging register reads/writes in a format
+	  that can be parsed by envytools demsm tool.  If enabled, register
+	  logging can be switched on via etnaviv.reglog=y module param.
diff --git a/drivers/gpu/drm/etnaviv/Makefile b/drivers/gpu/drm/etnaviv/Makefile
new file mode 100644
index 000000000000..1086e9876f91
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/Makefile
@@ -0,0 +1,14 @@
+etnaviv-y := \
+	etnaviv_buffer.o \
+	etnaviv_cmd_parser.o \
+	etnaviv_drv.o \
+	etnaviv_dump.o \
+	etnaviv_gem_prime.o \
+	etnaviv_gem_submit.o \
+	etnaviv_gem.o \
+	etnaviv_gpu.o \
+	etnaviv_iommu_v2.o \
+	etnaviv_iommu.o \
+	etnaviv_mmu.o
+
+obj-$(CONFIG_DRM_ETNAVIV)	+= etnaviv.o
diff --git a/drivers/gpu/drm/etnaviv/cmdstream.xml.h b/drivers/gpu/drm/etnaviv/cmdstream.xml.h
new file mode 100644
index 000000000000..8c44ba9a694e
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/cmdstream.xml.h
@@ -0,0 +1,218 @@
+#ifndef CMDSTREAM_XML
+#define CMDSTREAM_XML
+
+/* Autogenerated file, DO NOT EDIT manually!
+
+This file was generated by the rules-ng-ng headergen tool in this git repository:
+http://0x04.net/cgit/index.cgi/rules-ng-ng
+git clone git://0x04.net/rules-ng-ng
+
+The rules-ng-ng source files this header was generated from are:
+- cmdstream.xml (  12589 bytes, from 2014-02-17 14:57:56)
+- common.xml    (  18437 bytes, from 2015-03-25 11:27:41)
+
+Copyright (C) 2014
+*/
+
+
+#define FE_OPCODE_LOAD_STATE					0x00000001
+#define FE_OPCODE_END						0x00000002
+#define FE_OPCODE_NOP						0x00000003
+#define FE_OPCODE_DRAW_2D					0x00000004
+#define FE_OPCODE_DRAW_PRIMITIVES				0x00000005
+#define FE_OPCODE_DRAW_INDEXED_PRIMITIVES			0x00000006
+#define FE_OPCODE_WAIT						0x00000007
+#define FE_OPCODE_LINK						0x00000008
+#define FE_OPCODE_STALL						0x00000009
+#define FE_OPCODE_CALL						0x0000000a
+#define FE_OPCODE_RETURN					0x0000000b
+#define FE_OPCODE_CHIP_SELECT					0x0000000d
+#define PRIMITIVE_TYPE_POINTS					0x00000001
+#define PRIMITIVE_TYPE_LINES					0x00000002
+#define PRIMITIVE_TYPE_LINE_STRIP				0x00000003
+#define PRIMITIVE_TYPE_TRIANGLES				0x00000004
+#define PRIMITIVE_TYPE_TRIANGLE_STRIP				0x00000005
+#define PRIMITIVE_TYPE_TRIANGLE_FAN				0x00000006
+#define PRIMITIVE_TYPE_LINE_LOOP				0x00000007
+#define PRIMITIVE_TYPE_QUADS					0x00000008
+#define VIV_FE_LOAD_STATE					0x00000000
+
+#define VIV_FE_LOAD_STATE_HEADER				0x00000000
+#define VIV_FE_LOAD_STATE_HEADER_OP__MASK			0xf8000000
+#define VIV_FE_LOAD_STATE_HEADER_OP__SHIFT			27
+#define VIV_FE_LOAD_STATE_HEADER_OP_LOAD_STATE			0x08000000
+#define VIV_FE_LOAD_STATE_HEADER_FIXP				0x04000000
+#define VIV_FE_LOAD_STATE_HEADER_COUNT__MASK			0x03ff0000
+#define VIV_FE_LOAD_STATE_HEADER_COUNT__SHIFT			16
+#define VIV_FE_LOAD_STATE_HEADER_COUNT(x)			(((x) << VIV_FE_LOAD_STATE_HEADER_COUNT__SHIFT) & VIV_FE_LOAD_STATE_HEADER_COUNT__MASK)
+#define VIV_FE_LOAD_STATE_HEADER_OFFSET__MASK			0x0000ffff
+#define VIV_FE_LOAD_STATE_HEADER_OFFSET__SHIFT			0
+#define VIV_FE_LOAD_STATE_HEADER_OFFSET(x)			(((x) << VIV_FE_LOAD_STATE_HEADER_OFFSET__SHIFT) & VIV_FE_LOAD_STATE_HEADER_OFFSET__MASK)
+#define VIV_FE_LOAD_STATE_HEADER_OFFSET__SHR			2
+
+#define VIV_FE_END						0x00000000
+
+#define VIV_FE_END_HEADER					0x00000000
+#define VIV_FE_END_HEADER_EVENT_ID__MASK			0x0000001f
+#define VIV_FE_END_HEADER_EVENT_ID__SHIFT			0
+#define VIV_FE_END_HEADER_EVENT_ID(x)				(((x) << VIV_FE_END_HEADER_EVENT_ID__SHIFT) & VIV_FE_END_HEADER_EVENT_ID__MASK)
+#define VIV_FE_END_HEADER_EVENT_ENABLE				0x00000100
+#define VIV_FE_END_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_END_HEADER_OP__SHIFT				27
+#define VIV_FE_END_HEADER_OP_END				0x10000000
+
+#define VIV_FE_NOP						0x00000000
+
+#define VIV_FE_NOP_HEADER					0x00000000
+#define VIV_FE_NOP_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_NOP_HEADER_OP__SHIFT				27
+#define VIV_FE_NOP_HEADER_OP_NOP				0x18000000
+
+#define VIV_FE_DRAW_2D						0x00000000
+
+#define VIV_FE_DRAW_2D_HEADER					0x00000000
+#define VIV_FE_DRAW_2D_HEADER_COUNT__MASK			0x0000ff00
+#define VIV_FE_DRAW_2D_HEADER_COUNT__SHIFT			8
+#define VIV_FE_DRAW_2D_HEADER_COUNT(x)				(((x) << VIV_FE_DRAW_2D_HEADER_COUNT__SHIFT) & VIV_FE_DRAW_2D_HEADER_COUNT__MASK)
+#define VIV_FE_DRAW_2D_HEADER_DATA_COUNT__MASK			0x07ff0000
+#define VIV_FE_DRAW_2D_HEADER_DATA_COUNT__SHIFT			16
+#define VIV_FE_DRAW_2D_HEADER_DATA_COUNT(x)			(((x) << VIV_FE_DRAW_2D_HEADER_DATA_COUNT__SHIFT) & VIV_FE_DRAW_2D_HEADER_DATA_COUNT__MASK)
+#define VIV_FE_DRAW_2D_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_DRAW_2D_HEADER_OP__SHIFT				27
+#define VIV_FE_DRAW_2D_HEADER_OP_DRAW_2D			0x20000000
+
+#define VIV_FE_DRAW_2D_TOP_LEFT					0x00000008
+#define VIV_FE_DRAW_2D_TOP_LEFT_X__MASK				0x0000ffff
+#define VIV_FE_DRAW_2D_TOP_LEFT_X__SHIFT			0
+#define VIV_FE_DRAW_2D_TOP_LEFT_X(x)				(((x) << VIV_FE_DRAW_2D_TOP_LEFT_X__SHIFT) & VIV_FE_DRAW_2D_TOP_LEFT_X__MASK)
+#define VIV_FE_DRAW_2D_TOP_LEFT_Y__MASK				0xffff0000
+#define VIV_FE_DRAW_2D_TOP_LEFT_Y__SHIFT			16
+#define VIV_FE_DRAW_2D_TOP_LEFT_Y(x)				(((x) << VIV_FE_DRAW_2D_TOP_LEFT_Y__SHIFT) & VIV_FE_DRAW_2D_TOP_LEFT_Y__MASK)
+
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT				0x0000000c
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_X__MASK			0x0000ffff
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_X__SHIFT			0
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_X(x)			(((x) << VIV_FE_DRAW_2D_BOTTOM_RIGHT_X__SHIFT) & VIV_FE_DRAW_2D_BOTTOM_RIGHT_X__MASK)
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_Y__MASK			0xffff0000
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_Y__SHIFT			16
+#define VIV_FE_DRAW_2D_BOTTOM_RIGHT_Y(x)			(((x) << VIV_FE_DRAW_2D_BOTTOM_RIGHT_Y__SHIFT) & VIV_FE_DRAW_2D_BOTTOM_RIGHT_Y__MASK)
+
+#define VIV_FE_DRAW_PRIMITIVES					0x00000000
+
+#define VIV_FE_DRAW_PRIMITIVES_HEADER				0x00000000
+#define VIV_FE_DRAW_PRIMITIVES_HEADER_OP__MASK			0xf8000000
+#define VIV_FE_DRAW_PRIMITIVES_HEADER_OP__SHIFT			27
+#define VIV_FE_DRAW_PRIMITIVES_HEADER_OP_DRAW_PRIMITIVES	0x28000000
+
+#define VIV_FE_DRAW_PRIMITIVES_COMMAND				0x00000004
+#define VIV_FE_DRAW_PRIMITIVES_COMMAND_TYPE__MASK		0x000000ff
+#define VIV_FE_DRAW_PRIMITIVES_COMMAND_TYPE__SHIFT		0
+#define VIV_FE_DRAW_PRIMITIVES_COMMAND_TYPE(x)			(((x) << VIV_FE_DRAW_PRIMITIVES_COMMAND_TYPE__SHIFT) & VIV_FE_DRAW_PRIMITIVES_COMMAND_TYPE__MASK)
+
+#define VIV_FE_DRAW_PRIMITIVES_START				0x00000008
+
+#define VIV_FE_DRAW_PRIMITIVES_COUNT				0x0000000c
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES				0x00000000
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_HEADER			0x00000000
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_HEADER_OP__MASK		0xf8000000
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_HEADER_OP__SHIFT		27
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_HEADER_OP_DRAW_INDEXED_PRIMITIVES	0x30000000
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND			0x00000004
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND_TYPE__MASK	0x000000ff
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND_TYPE__SHIFT	0
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND_TYPE(x)		(((x) << VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND_TYPE__SHIFT) & VIV_FE_DRAW_INDEXED_PRIMITIVES_COMMAND_TYPE__MASK)
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_START			0x00000008
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_COUNT			0x0000000c
+
+#define VIV_FE_DRAW_INDEXED_PRIMITIVES_OFFSET			0x00000010
+
+#define VIV_FE_WAIT						0x00000000
+
+#define VIV_FE_WAIT_HEADER					0x00000000
+#define VIV_FE_WAIT_HEADER_DELAY__MASK				0x0000ffff
+#define VIV_FE_WAIT_HEADER_DELAY__SHIFT				0
+#define VIV_FE_WAIT_HEADER_DELAY(x)				(((x) << VIV_FE_WAIT_HEADER_DELAY__SHIFT) & VIV_FE_WAIT_HEADER_DELAY__MASK)
+#define VIV_FE_WAIT_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_WAIT_HEADER_OP__SHIFT				27
+#define VIV_FE_WAIT_HEADER_OP_WAIT				0x38000000
+
+#define VIV_FE_LINK						0x00000000
+
+#define VIV_FE_LINK_HEADER					0x00000000
+#define VIV_FE_LINK_HEADER_PREFETCH__MASK			0x0000ffff
+#define VIV_FE_LINK_HEADER_PREFETCH__SHIFT			0
+#define VIV_FE_LINK_HEADER_PREFETCH(x)				(((x) << VIV_FE_LINK_HEADER_PREFETCH__SHIFT) & VIV_FE_LINK_HEADER_PREFETCH__MASK)
+#define VIV_FE_LINK_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_LINK_HEADER_OP__SHIFT				27
+#define VIV_FE_LINK_HEADER_OP_LINK				0x40000000
+
+#define VIV_FE_LINK_ADDRESS					0x00000004
+
+#define VIV_FE_STALL						0x00000000
+
+#define VIV_FE_STALL_HEADER					0x00000000
+#define VIV_FE_STALL_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_STALL_HEADER_OP__SHIFT				27
+#define VIV_FE_STALL_HEADER_OP_STALL				0x48000000
+
+#define VIV_FE_STALL_TOKEN					0x00000004
+#define VIV_FE_STALL_TOKEN_FROM__MASK				0x0000001f
+#define VIV_FE_STALL_TOKEN_FROM__SHIFT				0
+#define VIV_FE_STALL_TOKEN_FROM(x)				(((x) << VIV_FE_STALL_TOKEN_FROM__SHIFT) & VIV_FE_STALL_TOKEN_FROM__MASK)
+#define VIV_FE_STALL_TOKEN_TO__MASK				0x00001f00
+#define VIV_FE_STALL_TOKEN_TO__SHIFT				8
+#define VIV_FE_STALL_TOKEN_TO(x)				(((x) << VIV_FE_STALL_TOKEN_TO__SHIFT) & VIV_FE_STALL_TOKEN_TO__MASK)
+
+#define VIV_FE_CALL						0x00000000
+
+#define VIV_FE_CALL_HEADER					0x00000000
+#define VIV_FE_CALL_HEADER_PREFETCH__MASK			0x0000ffff
+#define VIV_FE_CALL_HEADER_PREFETCH__SHIFT			0
+#define VIV_FE_CALL_HEADER_PREFETCH(x)				(((x) << VIV_FE_CALL_HEADER_PREFETCH__SHIFT) & VIV_FE_CALL_HEADER_PREFETCH__MASK)
+#define VIV_FE_CALL_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_CALL_HEADER_OP__SHIFT				27
+#define VIV_FE_CALL_HEADER_OP_CALL				0x50000000
+
+#define VIV_FE_CALL_ADDRESS					0x00000004
+
+#define VIV_FE_CALL_RETURN_PREFETCH				0x00000008
+
+#define VIV_FE_CALL_RETURN_ADDRESS				0x0000000c
+
+#define VIV_FE_RETURN						0x00000000
+
+#define VIV_FE_RETURN_HEADER					0x00000000
+#define VIV_FE_RETURN_HEADER_OP__MASK				0xf8000000
+#define VIV_FE_RETURN_HEADER_OP__SHIFT				27
+#define VIV_FE_RETURN_HEADER_OP_RETURN				0x58000000
+
+#define VIV_FE_CHIP_SELECT					0x00000000
+
+#define VIV_FE_CHIP_SELECT_HEADER				0x00000000
+#define VIV_FE_CHIP_SELECT_HEADER_OP__MASK			0xf8000000
+#define VIV_FE_CHIP_SELECT_HEADER_OP__SHIFT			27
+#define VIV_FE_CHIP_SELECT_HEADER_OP_CHIP_SELECT		0x68000000
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP15			0x00008000
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP14			0x00004000
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP13			0x00002000
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP12			0x00001000
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP11			0x00000800
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP10			0x00000400
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP9			0x00000200
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP8			0x00000100
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP7			0x00000080
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP6			0x00000040
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP5			0x00000020
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP4			0x00000010
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP3			0x00000008
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP2			0x00000004
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP1			0x00000002
+#define VIV_FE_CHIP_SELECT_HEADER_ENABLE_CHIP0			0x00000001
+
+
+#endif /* CMDSTREAM_XML */
diff --git a/drivers/gpu/drm/etnaviv/common.xml.h b/drivers/gpu/drm/etnaviv/common.xml.h
new file mode 100644
index 000000000000..e881482b5971
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/common.xml.h
@@ -0,0 +1,292 @@
+#ifndef COMMON_XML
+#define COMMON_XML
+
+/* Autogenerated file, DO NOT EDIT manually!
+
+This file was generated by the rules-ng-ng headergen tool in this git repository:
+http://0x04.net/cgit/index.cgi/rules-ng-ng
+git clone git://0x04.net/rules-ng-ng
+
+The rules-ng-ng source files this header was generated from are:
+- state_hi.xml (  24309 bytes, from 2015-12-12 09:02:53)
+- common.xml   (  18379 bytes, from 2015-12-12 09:02:53)
+
+Copyright (C) 2015
+*/
+
+
+#define PIPE_ID_PIPE_3D						0x00000000
+#define PIPE_ID_PIPE_2D						0x00000001
+#define SYNC_RECIPIENT_FE					0x00000001
+#define SYNC_RECIPIENT_RA					0x00000005
+#define SYNC_RECIPIENT_PE					0x00000007
+#define SYNC_RECIPIENT_DE					0x0000000b
+#define SYNC_RECIPIENT_VG					0x0000000f
+#define SYNC_RECIPIENT_TESSELATOR				0x00000010
+#define SYNC_RECIPIENT_VG2					0x00000011
+#define SYNC_RECIPIENT_TESSELATOR2				0x00000012
+#define SYNC_RECIPIENT_VG3					0x00000013
+#define SYNC_RECIPIENT_TESSELATOR3				0x00000014
+#define ENDIAN_MODE_NO_SWAP					0x00000000
+#define ENDIAN_MODE_SWAP_16					0x00000001
+#define ENDIAN_MODE_SWAP_32					0x00000002
+#define chipModel_GC200						0x00000200
+#define chipModel_GC300						0x00000300
+#define chipModel_GC320						0x00000320
+#define chipModel_GC328						0x00000328
+#define chipModel_GC350						0x00000350
+#define chipModel_GC355						0x00000355
+#define chipModel_GC400						0x00000400
+#define chipModel_GC410						0x00000410
+#define chipModel_GC420						0x00000420
+#define chipModel_GC428						0x00000428
+#define chipModel_GC450						0x00000450
+#define chipModel_GC500						0x00000500
+#define chipModel_GC520						0x00000520
+#define chipModel_GC530						0x00000530
+#define chipModel_GC600						0x00000600
+#define chipModel_GC700						0x00000700
+#define chipModel_GC800						0x00000800
+#define chipModel_GC860						0x00000860
+#define chipModel_GC880						0x00000880
+#define chipModel_GC1000					0x00001000
+#define chipModel_GC1500					0x00001500
+#define chipModel_GC2000					0x00002000
+#define chipModel_GC2100					0x00002100
+#define chipModel_GC2200					0x00002200
+#define chipModel_GC2500					0x00002500
+#define chipModel_GC3000					0x00003000
+#define chipModel_GC4000					0x00004000
+#define chipModel_GC5000					0x00005000
+#define chipModel_GC5200					0x00005200
+#define chipModel_GC6400					0x00006400
+#define RGBA_BITS_R						0x00000001
+#define RGBA_BITS_G						0x00000002
+#define RGBA_BITS_B						0x00000004
+#define RGBA_BITS_A						0x00000008
+#define chipFeatures_FAST_CLEAR					0x00000001
+#define chipFeatures_SPECIAL_ANTI_ALIASING			0x00000002
+#define chipFeatures_PIPE_3D					0x00000004
+#define chipFeatures_DXT_TEXTURE_COMPRESSION			0x00000008
+#define chipFeatures_DEBUG_MODE					0x00000010
+#define chipFeatures_Z_COMPRESSION				0x00000020
+#define chipFeatures_YUV420_SCALER				0x00000040
+#define chipFeatures_MSAA					0x00000080
+#define chipFeatures_DC						0x00000100
+#define chipFeatures_PIPE_2D					0x00000200
+#define chipFeatures_ETC1_TEXTURE_COMPRESSION			0x00000400
+#define chipFeatures_FAST_SCALER				0x00000800
+#define chipFeatures_HIGH_DYNAMIC_RANGE				0x00001000
+#define chipFeatures_YUV420_TILER				0x00002000
+#define chipFeatures_MODULE_CG					0x00004000
+#define chipFeatures_MIN_AREA					0x00008000
+#define chipFeatures_NO_EARLY_Z					0x00010000
+#define chipFeatures_NO_422_TEXTURE				0x00020000
+#define chipFeatures_BUFFER_INTERLEAVING			0x00040000
+#define chipFeatures_BYTE_WRITE_2D				0x00080000
+#define chipFeatures_NO_SCALER					0x00100000
+#define chipFeatures_YUY2_AVERAGING				0x00200000
+#define chipFeatures_HALF_PE_CACHE				0x00400000
+#define chipFeatures_HALF_TX_CACHE				0x00800000
+#define chipFeatures_YUY2_RENDER_TARGET				0x01000000
+#define chipFeatures_MEM32					0x02000000
+#define chipFeatures_PIPE_VG					0x04000000
+#define chipFeatures_VGTS					0x08000000
+#define chipFeatures_FE20					0x10000000
+#define chipFeatures_BYTE_WRITE_3D				0x20000000
+#define chipFeatures_RS_YUV_TARGET				0x40000000
+#define chipFeatures_32_BIT_INDICES				0x80000000
+#define chipMinorFeatures0_FLIP_Y				0x00000001
+#define chipMinorFeatures0_DUAL_RETURN_BUS			0x00000002
+#define chipMinorFeatures0_ENDIANNESS_CONFIG			0x00000004
+#define chipMinorFeatures0_TEXTURE_8K				0x00000008
+#define chipMinorFeatures0_CORRECT_TEXTURE_CONVERTER		0x00000010
+#define chipMinorFeatures0_SPECIAL_MSAA_LOD			0x00000020
+#define chipMinorFeatures0_FAST_CLEAR_FLUSH			0x00000040
+#define chipMinorFeatures0_2DPE20				0x00000080
+#define chipMinorFeatures0_CORRECT_AUTO_DISABLE			0x00000100
+#define chipMinorFeatures0_RENDERTARGET_8K			0x00000200
+#define chipMinorFeatures0_2BITPERTILE				0x00000400
+#define chipMinorFeatures0_SEPARATE_TILE_STATUS_WHEN_INTERLEAVED	0x00000800
+#define chipMinorFeatures0_SUPER_TILED				0x00001000
+#define chipMinorFeatures0_VG_20				0x00002000
+#define chipMinorFeatures0_TS_EXTENDED_COMMANDS			0x00004000
+#define chipMinorFeatures0_COMPRESSION_FIFO_FIXED		0x00008000
+#define chipMinorFeatures0_HAS_SIGN_FLOOR_CEIL			0x00010000
+#define chipMinorFeatures0_VG_FILTER				0x00020000
+#define chipMinorFeatures0_VG_21				0x00040000
+#define chipMinorFeatures0_SHADER_HAS_W				0x00080000
+#define chipMinorFeatures0_HAS_SQRT_TRIG			0x00100000
+#define chipMinorFeatures0_MORE_MINOR_FEATURES			0x00200000
+#define chipMinorFeatures0_MC20					0x00400000
+#define chipMinorFeatures0_MSAA_SIDEBAND			0x00800000
+#define chipMinorFeatures0_BUG_FIXES0				0x01000000
+#define chipMinorFeatures0_VAA					0x02000000
+#define chipMinorFeatures0_BYPASS_IN_MSAA			0x04000000
+#define chipMinorFeatures0_HZ					0x08000000
+#define chipMinorFeatures0_NEW_TEXTURE				0x10000000
+#define chipMinorFeatures0_2D_A8_TARGET				0x20000000
+#define chipMinorFeatures0_CORRECT_STENCIL			0x40000000
+#define chipMinorFeatures0_ENHANCE_VR				0x80000000
+#define chipMinorFeatures1_RSUV_SWIZZLE				0x00000001
+#define chipMinorFeatures1_V2_COMPRESSION			0x00000002
+#define chipMinorFeatures1_VG_DOUBLE_BUFFER			0x00000004
+#define chipMinorFeatures1_EXTRA_EVENT_STATES			0x00000008
+#define chipMinorFeatures1_NO_STRIPING_NEEDED			0x00000010
+#define chipMinorFeatures1_TEXTURE_STRIDE			0x00000020
+#define chipMinorFeatures1_BUG_FIXES3				0x00000040
+#define chipMinorFeatures1_AUTO_DISABLE				0x00000080
+#define chipMinorFeatures1_AUTO_RESTART_TS			0x00000100
+#define chipMinorFeatures1_DISABLE_PE_GATING			0x00000200
+#define chipMinorFeatures1_L2_WINDOWING				0x00000400
+#define chipMinorFeatures1_HALF_FLOAT				0x00000800
+#define chipMinorFeatures1_PIXEL_DITHER				0x00001000
+#define chipMinorFeatures1_TWO_STENCIL_REFERENCE		0x00002000
+#define chipMinorFeatures1_EXTENDED_PIXEL_FORMAT		0x00004000
+#define chipMinorFeatures1_CORRECT_MIN_MAX_DEPTH		0x00008000
+#define chipMinorFeatures1_2D_DITHER				0x00010000
+#define chipMinorFeatures1_BUG_FIXES5				0x00020000
+#define chipMinorFeatures1_NEW_2D				0x00040000
+#define chipMinorFeatures1_NEW_FP				0x00080000
+#define chipMinorFeatures1_TEXTURE_HALIGN			0x00100000
+#define chipMinorFeatures1_NON_POWER_OF_TWO			0x00200000
+#define chipMinorFeatures1_LINEAR_TEXTURE_SUPPORT		0x00400000
+#define chipMinorFeatures1_HALTI0				0x00800000
+#define chipMinorFeatures1_CORRECT_OVERFLOW_VG			0x01000000
+#define chipMinorFeatures1_NEGATIVE_LOG_FIX			0x02000000
+#define chipMinorFeatures1_RESOLVE_OFFSET			0x04000000
+#define chipMinorFeatures1_OK_TO_GATE_AXI_CLOCK			0x08000000
+#define chipMinorFeatures1_MMU_VERSION				0x10000000
+#define chipMinorFeatures1_WIDE_LINE				0x20000000
+#define chipMinorFeatures1_BUG_FIXES6				0x40000000
+#define chipMinorFeatures1_FC_FLUSH_STALL			0x80000000
+#define chipMinorFeatures2_LINE_LOOP				0x00000001
+#define chipMinorFeatures2_LOGIC_OP				0x00000002
+#define chipMinorFeatures2_UNK2					0x00000004
+#define chipMinorFeatures2_SUPERTILED_TEXTURE			0x00000008
+#define chipMinorFeatures2_UNK4					0x00000010
+#define chipMinorFeatures2_RECT_PRIMITIVE			0x00000020
+#define chipMinorFeatures2_COMPOSITION				0x00000040
+#define chipMinorFeatures2_CORRECT_AUTO_DISABLE_COUNT		0x00000080
+#define chipMinorFeatures2_UNK8					0x00000100
+#define chipMinorFeatures2_UNK9					0x00000200
+#define chipMinorFeatures2_UNK10				0x00000400
+#define chipMinorFeatures2_HALTI1				0x00000800
+#define chipMinorFeatures2_UNK12				0x00001000
+#define chipMinorFeatures2_UNK13				0x00002000
+#define chipMinorFeatures2_UNK14				0x00004000
+#define chipMinorFeatures2_EXTRA_TEXTURE_STATE			0x00008000
+#define chipMinorFeatures2_FULL_DIRECTFB			0x00010000
+#define chipMinorFeatures2_2D_TILING				0x00020000
+#define chipMinorFeatures2_THREAD_WALKER_IN_PS			0x00040000
+#define chipMinorFeatures2_TILE_FILLER				0x00080000
+#define chipMinorFeatures2_UNK20				0x00100000
+#define chipMinorFeatures2_2D_MULTI_SOURCE_BLIT			0x00200000
+#define chipMinorFeatures2_UNK22				0x00400000
+#define chipMinorFeatures2_UNK23				0x00800000
+#define chipMinorFeatures2_UNK24				0x01000000
+#define chipMinorFeatures2_MIXED_STREAMS			0x02000000
+#define chipMinorFeatures2_2D_420_L2CACHE			0x04000000
+#define chipMinorFeatures2_UNK27				0x08000000
+#define chipMinorFeatures2_2D_NO_INDEX8_BRUSH			0x10000000
+#define chipMinorFeatures2_TEXTURE_TILED_READ			0x20000000
+#define chipMinorFeatures2_UNK30				0x40000000
+#define chipMinorFeatures2_UNK31				0x80000000
+#define chipMinorFeatures3_ROTATION_STALL_FIX			0x00000001
+#define chipMinorFeatures3_UNK1					0x00000002
+#define chipMinorFeatures3_2D_MULTI_SOURCE_BLT_EX		0x00000004
+#define chipMinorFeatures3_UNK3					0x00000008
+#define chipMinorFeatures3_UNK4					0x00000010
+#define chipMinorFeatures3_UNK5					0x00000020
+#define chipMinorFeatures3_UNK6					0x00000040
+#define chipMinorFeatures3_UNK7					0x00000080
+#define chipMinorFeatures3_FAST_MSAA				0x00000100
+#define chipMinorFeatures3_UNK9					0x00000200
+#define chipMinorFeatures3_BUG_FIXES10				0x00000400
+#define chipMinorFeatures3_UNK11				0x00000800
+#define chipMinorFeatures3_BUG_FIXES11				0x00001000
+#define chipMinorFeatures3_UNK13				0x00002000
+#define chipMinorFeatures3_UNK14				0x00004000
+#define chipMinorFeatures3_UNK15				0x00008000
+#define chipMinorFeatures3_UNK16				0x00010000
+#define chipMinorFeatures3_UNK17				0x00020000
+#define chipMinorFeatures3_ACE					0x00040000
+#define chipMinorFeatures3_UNK19				0x00080000
+#define chipMinorFeatures3_UNK20				0x00100000
+#define chipMinorFeatures3_UNK21				0x00200000
+#define chipMinorFeatures3_UNK22				0x00400000
+#define chipMinorFeatures3_UNK23				0x00800000
+#define chipMinorFeatures3_UNK24				0x01000000
+#define chipMinorFeatures3_UNK25				0x02000000
+#define chipMinorFeatures3_NEW_HZ				0x04000000
+#define chipMinorFeatures3_UNK27				0x08000000
+#define chipMinorFeatures3_UNK28				0x10000000
+#define chipMinorFeatures3_UNK29				0x20000000
+#define chipMinorFeatures3_UNK30				0x40000000
+#define chipMinorFeatures3_UNK31				0x80000000
+#define chipMinorFeatures4_UNK0					0x00000001
+#define chipMinorFeatures4_UNK1					0x00000002
+#define chipMinorFeatures4_UNK2					0x00000004
+#define chipMinorFeatures4_UNK3					0x00000008
+#define chipMinorFeatures4_UNK4					0x00000010
+#define chipMinorFeatures4_UNK5					0x00000020
+#define chipMinorFeatures4_UNK6					0x00000040
+#define chipMinorFeatures4_UNK7					0x00000080
+#define chipMinorFeatures4_UNK8					0x00000100
+#define chipMinorFeatures4_UNK9					0x00000200
+#define chipMinorFeatures4_UNK10				0x00000400
+#define chipMinorFeatures4_UNK11				0x00000800
+#define chipMinorFeatures4_UNK12				0x00001000
+#define chipMinorFeatures4_UNK13				0x00002000
+#define chipMinorFeatures4_UNK14				0x00004000
+#define chipMinorFeatures4_UNK15				0x00008000
+#define chipMinorFeatures4_HALTI2				0x00010000
+#define chipMinorFeatures4_UNK17				0x00020000
+#define chipMinorFeatures4_SMALL_MSAA				0x00040000
+#define chipMinorFeatures4_UNK19				0x00080000
+#define chipMinorFeatures4_UNK20				0x00100000
+#define chipMinorFeatures4_UNK21				0x00200000
+#define chipMinorFeatures4_UNK22				0x00400000
+#define chipMinorFeatures4_UNK23				0x00800000
+#define chipMinorFeatures4_UNK24				0x01000000
+#define chipMinorFeatures4_UNK25				0x02000000
+#define chipMinorFeatures4_UNK26				0x04000000
+#define chipMinorFeatures4_UNK27				0x08000000
+#define chipMinorFeatures4_UNK28				0x10000000
+#define chipMinorFeatures4_UNK29				0x20000000
+#define chipMinorFeatures4_UNK30				0x40000000
+#define chipMinorFeatures4_UNK31				0x80000000
+#define chipMinorFeatures5_UNK0					0x00000001
+#define chipMinorFeatures5_UNK1					0x00000002
+#define chipMinorFeatures5_UNK2					0x00000004
+#define chipMinorFeatures5_UNK3					0x00000008
+#define chipMinorFeatures5_UNK4					0x00000010
+#define chipMinorFeatures5_UNK5					0x00000020
+#define chipMinorFeatures5_UNK6					0x00000040
+#define chipMinorFeatures5_UNK7					0x00000080
+#define chipMinorFeatures5_UNK8					0x00000100
+#define chipMinorFeatures5_HALTI3				0x00000200
+#define chipMinorFeatures5_UNK10				0x00000400
+#define chipMinorFeatures5_UNK11				0x00000800
+#define chipMinorFeatures5_UNK12				0x00001000
+#define chipMinorFeatures5_UNK13				0x00002000
+#define chipMinorFeatures5_UNK14				0x00004000
+#define chipMinorFeatures5_UNK15				0x00008000
+#define chipMinorFeatures5_UNK16				0x00010000
+#define chipMinorFeatures5_UNK17				0x00020000
+#define chipMinorFeatures5_UNK18				0x00040000
+#define chipMinorFeatures5_UNK19				0x00080000
+#define chipMinorFeatures5_UNK20				0x00100000
+#define chipMinorFeatures5_UNK21				0x00200000
+#define chipMinorFeatures5_UNK22				0x00400000
+#define chipMinorFeatures5_UNK23				0x00800000
+#define chipMinorFeatures5_UNK24				0x01000000
+#define chipMinorFeatures5_UNK25				0x02000000
+#define chipMinorFeatures5_UNK26				0x04000000
+#define chipMinorFeatures5_UNK27				0x08000000
+#define chipMinorFeatures5_UNK28				0x10000000
+#define chipMinorFeatures5_UNK29				0x20000000
+#define chipMinorFeatures5_UNK30				0x40000000
+#define chipMinorFeatures5_UNK31				0x80000000
+
+#endif /* COMMON_XML */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_buffer.c b/drivers/gpu/drm/etnaviv/etnaviv_buffer.c
new file mode 100644
index 000000000000..332c55ebba6d
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_buffer.c
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2014 Etnaviv Project
+ * Author: Christian Gmeiner <christian.gmeiner@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "etnaviv_gpu.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_mmu.h"
+
+#include "common.xml.h"
+#include "state.xml.h"
+#include "cmdstream.xml.h"
+
+/*
+ * Command Buffer helper:
+ */
+
+
+static inline void OUT(struct etnaviv_cmdbuf *buffer, u32 data)
+{
+	u32 *vaddr = (u32 *)buffer->vaddr;
+
+	BUG_ON(buffer->user_size >= buffer->size);
+
+	vaddr[buffer->user_size / 4] = data;
+	buffer->user_size += 4;
+}
+
+static inline void CMD_LOAD_STATE(struct etnaviv_cmdbuf *buffer,
+	u32 reg, u32 value)
+{
+	u32 index = reg >> VIV_FE_LOAD_STATE_HEADER_OFFSET__SHR;
+
+	buffer->user_size = ALIGN(buffer->user_size, 8);
+
+	/* write a register via cmd stream */
+	OUT(buffer, VIV_FE_LOAD_STATE_HEADER_OP_LOAD_STATE |
+		    VIV_FE_LOAD_STATE_HEADER_COUNT(1) |
+		    VIV_FE_LOAD_STATE_HEADER_OFFSET(index));
+	OUT(buffer, value);
+}
+
+static inline void CMD_END(struct etnaviv_cmdbuf *buffer)
+{
+	buffer->user_size = ALIGN(buffer->user_size, 8);
+
+	OUT(buffer, VIV_FE_END_HEADER_OP_END);
+}
+
+static inline void CMD_WAIT(struct etnaviv_cmdbuf *buffer)
+{
+	buffer->user_size = ALIGN(buffer->user_size, 8);
+
+	OUT(buffer, VIV_FE_WAIT_HEADER_OP_WAIT | 200);
+}
+
+static inline void CMD_LINK(struct etnaviv_cmdbuf *buffer,
+	u16 prefetch, u32 address)
+{
+	buffer->user_size = ALIGN(buffer->user_size, 8);
+
+	OUT(buffer, VIV_FE_LINK_HEADER_OP_LINK |
+		    VIV_FE_LINK_HEADER_PREFETCH(prefetch));
+	OUT(buffer, address);
+}
+
+static inline void CMD_STALL(struct etnaviv_cmdbuf *buffer,
+	u32 from, u32 to)
+{
+	buffer->user_size = ALIGN(buffer->user_size, 8);
+
+	OUT(buffer, VIV_FE_STALL_HEADER_OP_STALL);
+	OUT(buffer, VIV_FE_STALL_TOKEN_FROM(from) | VIV_FE_STALL_TOKEN_TO(to));
+}
+
+static void etnaviv_cmd_select_pipe(struct etnaviv_cmdbuf *buffer, u8 pipe)
+{
+	u32 flush;
+	u32 stall;
+
+	/*
+	 * This assumes that if we're switching to 2D, we're switching
+	 * away from 3D, and vice versa.  Hence, if we're switching to
+	 * the 2D core, we need to flush the 3D depth and color caches,
+	 * otherwise we need to flush the 2D pixel engine cache.
+	 */
+	if (pipe == ETNA_PIPE_2D)
+		flush = VIVS_GL_FLUSH_CACHE_DEPTH | VIVS_GL_FLUSH_CACHE_COLOR;
+	else
+		flush = VIVS_GL_FLUSH_CACHE_PE2D;
+
+	stall = VIVS_GL_SEMAPHORE_TOKEN_FROM(SYNC_RECIPIENT_FE) |
+		VIVS_GL_SEMAPHORE_TOKEN_TO(SYNC_RECIPIENT_PE);
+
+	CMD_LOAD_STATE(buffer, VIVS_GL_FLUSH_CACHE, flush);
+	CMD_LOAD_STATE(buffer, VIVS_GL_SEMAPHORE_TOKEN, stall);
+
+	CMD_STALL(buffer, SYNC_RECIPIENT_FE, SYNC_RECIPIENT_PE);
+
+	CMD_LOAD_STATE(buffer, VIVS_GL_PIPE_SELECT,
+		       VIVS_GL_PIPE_SELECT_PIPE(pipe));
+}
+
+static u32 gpu_va(struct etnaviv_gpu *gpu, struct etnaviv_cmdbuf *buf)
+{
+	return buf->paddr - gpu->memory_base;
+}
+
+static void etnaviv_buffer_dump(struct etnaviv_gpu *gpu,
+	struct etnaviv_cmdbuf *buf, u32 off, u32 len)
+{
+	u32 size = buf->size;
+	u32 *ptr = buf->vaddr + off;
+
+	dev_info(gpu->dev, "virt %p phys 0x%08x free 0x%08x\n",
+			ptr, gpu_va(gpu, buf) + off, size - len * 4 - off);
+
+	print_hex_dump(KERN_INFO, "cmd ", DUMP_PREFIX_OFFSET, 16, 4,
+			ptr, len * 4, 0);
+}
+
+u16 etnaviv_buffer_init(struct etnaviv_gpu *gpu)
+{
+	struct etnaviv_cmdbuf *buffer = gpu->buffer;
+
+	/* initialize buffer */
+	buffer->user_size = 0;
+
+	CMD_WAIT(buffer);
+	CMD_LINK(buffer, 2, gpu_va(gpu, buffer) + buffer->user_size - 4);
+
+	return buffer->user_size / 8;
+}
+
+void etnaviv_buffer_end(struct etnaviv_gpu *gpu)
+{
+	struct etnaviv_cmdbuf *buffer = gpu->buffer;
+
+	/* Replace the last WAIT with an END */
+	buffer->user_size -= 16;
+
+	CMD_END(buffer);
+	mb();
+}
+
+void etnaviv_buffer_queue(struct etnaviv_gpu *gpu, unsigned int event,
+	struct etnaviv_cmdbuf *cmdbuf)
+{
+	struct etnaviv_cmdbuf *buffer = gpu->buffer;
+	u32 *lw = buffer->vaddr + buffer->user_size - 16;
+	u32 back, link_target, link_size, reserve_size, extra_size = 0;
+
+	if (drm_debug & DRM_UT_DRIVER)
+		etnaviv_buffer_dump(gpu, buffer, 0, 0x50);
+
+	/*
+	 * If we need to flush the MMU prior to submitting this buffer, we
+	 * will need to append a mmu flush load state, followed by a new
+	 * link to this buffer - a total of four additional words.
+	 */
+	if (gpu->mmu->need_flush || gpu->switch_context) {
+		/* link command */
+		extra_size += 2;
+		/* flush command */
+		if (gpu->mmu->need_flush)
+			extra_size += 2;
+		/* pipe switch commands */
+		if (gpu->switch_context)
+			extra_size += 8;
+	}
+
+	reserve_size = (6 + extra_size) * 4;
+
+	/*
+	 * if we are going to completely overflow the buffer, we need to wrap.
+	 */
+	if (buffer->user_size + reserve_size > buffer->size)
+		buffer->user_size = 0;
+
+	/* save offset back into main buffer */
+	back = buffer->user_size + reserve_size - 6 * 4;
+	link_target = gpu_va(gpu, buffer) + buffer->user_size;
+	link_size = 6;
+
+	/* Skip over any extra instructions */
+	link_target += extra_size * sizeof(u32);
+
+	if (drm_debug & DRM_UT_DRIVER)
+		pr_info("stream link to 0x%08x @ 0x%08x %p\n",
+			link_target, gpu_va(gpu, cmdbuf), cmdbuf->vaddr);
+
+	/* jump back from cmd to main buffer */
+	CMD_LINK(cmdbuf, link_size, link_target);
+
+	link_target = gpu_va(gpu, cmdbuf);
+	link_size = cmdbuf->size / 8;
+
+
+
+	if (drm_debug & DRM_UT_DRIVER) {
+		print_hex_dump(KERN_INFO, "cmd ", DUMP_PREFIX_OFFSET, 16, 4,
+			       cmdbuf->vaddr, cmdbuf->size, 0);
+
+		pr_info("link op: %p\n", lw);
+		pr_info("link addr: %p\n", lw + 1);
+		pr_info("addr: 0x%08x\n", link_target);
+		pr_info("back: 0x%08x\n", gpu_va(gpu, buffer) + back);
+		pr_info("event: %d\n", event);
+	}
+
+	if (gpu->mmu->need_flush || gpu->switch_context) {
+		u32 new_target = gpu_va(gpu, buffer) + buffer->user_size;
+
+		if (gpu->mmu->need_flush) {
+			/* Add the MMU flush */
+			CMD_LOAD_STATE(buffer, VIVS_GL_FLUSH_MMU,
+				       VIVS_GL_FLUSH_MMU_FLUSH_FEMMU |
+				       VIVS_GL_FLUSH_MMU_FLUSH_UNK1 |
+				       VIVS_GL_FLUSH_MMU_FLUSH_UNK2 |
+				       VIVS_GL_FLUSH_MMU_FLUSH_PEMMU |
+				       VIVS_GL_FLUSH_MMU_FLUSH_UNK4);
+
+			gpu->mmu->need_flush = false;
+		}
+
+		if (gpu->switch_context) {
+			etnaviv_cmd_select_pipe(buffer, cmdbuf->exec_state);
+			gpu->switch_context = false;
+		}
+
+		/* And the link to the first buffer */
+		CMD_LINK(buffer, link_size, link_target);
+
+		/* Update the link target to point to above instructions */
+		link_target = new_target;
+		link_size = extra_size;
+	}
+
+	/* trigger event */
+	CMD_LOAD_STATE(buffer, VIVS_GL_EVENT, VIVS_GL_EVENT_EVENT_ID(event) |
+		       VIVS_GL_EVENT_FROM_PE);
+
+	/* append WAIT/LINK to main buffer */
+	CMD_WAIT(buffer);
+	CMD_LINK(buffer, 2, gpu_va(gpu, buffer) + (buffer->user_size - 4));
+
+	/* Change WAIT into a LINK command; write the address first. */
+	*(lw + 1) = link_target;
+	mb();
+	*(lw) = VIV_FE_LINK_HEADER_OP_LINK |
+		VIV_FE_LINK_HEADER_PREFETCH(link_size);
+	mb();
+
+	if (drm_debug & DRM_UT_DRIVER)
+		etnaviv_buffer_dump(gpu, buffer, 0, 0x50);
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_cmd_parser.c b/drivers/gpu/drm/etnaviv/etnaviv_cmd_parser.c
new file mode 100644
index 000000000000..dcfd565c88d1
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_cmd_parser.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+
+#include "etnaviv_gem.h"
+#include "etnaviv_gpu.h"
+
+#include "cmdstream.xml.h"
+
+#define EXTRACT(val, field) (((val) & field##__MASK) >> field##__SHIFT)
+
+struct etna_validation_state {
+	struct etnaviv_gpu *gpu;
+	const struct drm_etnaviv_gem_submit_reloc *relocs;
+	unsigned int num_relocs;
+	u32 *start;
+};
+
+static const struct {
+	u16 offset;
+	u16 size;
+} etnaviv_sensitive_states[] __initconst = {
+#define ST(start, num) { (start) >> 2, (num) }
+	/* 2D */
+	ST(0x1200, 1),
+	ST(0x1228, 1),
+	ST(0x1238, 1),
+	ST(0x1284, 1),
+	ST(0x128c, 1),
+	ST(0x1304, 1),
+	ST(0x1310, 1),
+	ST(0x1318, 1),
+	ST(0x12800, 4),
+	ST(0x128a0, 4),
+	ST(0x128c0, 4),
+	ST(0x12970, 4),
+	ST(0x12a00, 8),
+	ST(0x12b40, 8),
+	ST(0x12b80, 8),
+	ST(0x12ce0, 8),
+	/* 3D */
+	ST(0x0644, 1),
+	ST(0x064c, 1),
+	ST(0x0680, 8),
+	ST(0x1410, 1),
+	ST(0x1430, 1),
+	ST(0x1458, 1),
+	ST(0x1460, 8),
+	ST(0x1480, 8),
+	ST(0x1500, 8),
+	ST(0x1520, 8),
+	ST(0x1608, 1),
+	ST(0x1610, 1),
+	ST(0x1658, 1),
+	ST(0x165c, 1),
+	ST(0x1664, 1),
+	ST(0x1668, 1),
+	ST(0x16a4, 1),
+	ST(0x16c0, 8),
+	ST(0x16e0, 8),
+	ST(0x1740, 8),
+	ST(0x2400, 14 * 16),
+	ST(0x10800, 32 * 16),
+#undef ST
+};
+
+#define ETNAVIV_STATES_SIZE (VIV_FE_LOAD_STATE_HEADER_OFFSET__MASK + 1u)
+static DECLARE_BITMAP(etnaviv_states, ETNAVIV_STATES_SIZE);
+
+void __init etnaviv_validate_init(void)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(etnaviv_sensitive_states); i++)
+		bitmap_set(etnaviv_states, etnaviv_sensitive_states[i].offset,
+			   etnaviv_sensitive_states[i].size);
+}
+
+static void etnaviv_warn_if_non_sensitive(struct etna_validation_state *state,
+	unsigned int buf_offset, unsigned int state_addr)
+{
+	if (state->num_relocs && state->relocs->submit_offset < buf_offset) {
+		dev_warn_once(state->gpu->dev,
+			      "%s: relocation for non-sensitive state 0x%x at offset %u\n",
+			      __func__, state_addr,
+			      state->relocs->submit_offset);
+		while (state->num_relocs &&
+		       state->relocs->submit_offset < buf_offset) {
+			state->relocs++;
+			state->num_relocs--;
+		}
+	}
+}
+
+static bool etnaviv_validate_load_state(struct etna_validation_state *state,
+	u32 *ptr, unsigned int state_offset, unsigned int num)
+{
+	unsigned int size = min(ETNAVIV_STATES_SIZE, state_offset + num);
+	unsigned int st_offset = state_offset, buf_offset;
+
+	for_each_set_bit_from(st_offset, etnaviv_states, size) {
+		buf_offset = (ptr - state->start +
+			      st_offset - state_offset) * 4;
+
+		etnaviv_warn_if_non_sensitive(state, buf_offset, st_offset * 4);
+		if (state->num_relocs &&
+		    state->relocs->submit_offset == buf_offset) {
+			state->relocs++;
+			state->num_relocs--;
+			continue;
+		}
+
+		dev_warn_ratelimited(state->gpu->dev,
+				     "%s: load state touches restricted state 0x%x at offset %u\n",
+				     __func__, st_offset * 4, buf_offset);
+		return false;
+	}
+
+	if (state->num_relocs) {
+		buf_offset = (ptr - state->start + num) * 4;
+		etnaviv_warn_if_non_sensitive(state, buf_offset, st_offset * 4 +
+					      state->relocs->submit_offset -
+					      buf_offset);
+	}
+
+	return true;
+}
+
+static uint8_t cmd_length[32] = {
+	[FE_OPCODE_DRAW_PRIMITIVES] = 4,
+	[FE_OPCODE_DRAW_INDEXED_PRIMITIVES] = 6,
+	[FE_OPCODE_NOP] = 2,
+	[FE_OPCODE_STALL] = 2,
+};
+
+bool etnaviv_cmd_validate_one(struct etnaviv_gpu *gpu, u32 *stream,
+			      unsigned int size,
+			      struct drm_etnaviv_gem_submit_reloc *relocs,
+			      unsigned int reloc_size)
+{
+	struct etna_validation_state state;
+	u32 *buf = stream;
+	u32 *end = buf + size;
+
+	state.gpu = gpu;
+	state.relocs = relocs;
+	state.num_relocs = reloc_size;
+	state.start = stream;
+
+	while (buf < end) {
+		u32 cmd = *buf;
+		unsigned int len, n, off;
+		unsigned int op = cmd >> 27;
+
+		switch (op) {
+		case FE_OPCODE_LOAD_STATE:
+			n = EXTRACT(cmd, VIV_FE_LOAD_STATE_HEADER_COUNT);
+			len = ALIGN(1 + n, 2);
+			if (buf + len > end)
+				break;
+
+			off = EXTRACT(cmd, VIV_FE_LOAD_STATE_HEADER_OFFSET);
+			if (!etnaviv_validate_load_state(&state, buf + 1,
+							 off, n))
+				return false;
+			break;
+
+		case FE_OPCODE_DRAW_2D:
+			n = EXTRACT(cmd, VIV_FE_DRAW_2D_HEADER_COUNT);
+			if (n == 0)
+				n = 256;
+			len = 2 + n * 2;
+			break;
+
+		default:
+			len = cmd_length[op];
+			if (len == 0) {
+				dev_err(gpu->dev, "%s: op %u not permitted at offset %tu\n",
+					__func__, op, buf - state.start);
+				return false;
+			}
+			break;
+		}
+
+		buf += len;
+	}
+
+	if (buf > end) {
+		dev_err(gpu->dev, "%s: commands overflow end of buffer: %tu > %u\n",
+			__func__, buf - state.start, size);
+		return false;
+	}
+
+	return true;
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_drv.c b/drivers/gpu/drm/etnaviv/etnaviv_drv.c
new file mode 100644
index 000000000000..e8858985f01e
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_drv.c
@@ -0,0 +1,706 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/component.h>
+#include <linux/of_platform.h>
+
+#include "etnaviv_drv.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_mmu.h"
+#include "etnaviv_gem.h"
+
+#ifdef CONFIG_DRM_ETNAVIV_REGISTER_LOGGING
+static bool reglog;
+MODULE_PARM_DESC(reglog, "Enable register read/write logging");
+module_param(reglog, bool, 0600);
+#else
+#define reglog 0
+#endif
+
+void __iomem *etnaviv_ioremap(struct platform_device *pdev, const char *name,
+		const char *dbgname)
+{
+	struct resource *res;
+	void __iomem *ptr;
+
+	if (name)
+		res = platform_get_resource_byname(pdev, IORESOURCE_MEM, name);
+	else
+		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	ptr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ptr)) {
+		dev_err(&pdev->dev, "failed to ioremap %s: %ld\n", name,
+			PTR_ERR(ptr));
+		return ptr;
+	}
+
+	if (reglog)
+		dev_printk(KERN_DEBUG, &pdev->dev, "IO:region %s 0x%p %08zx\n",
+			   dbgname, ptr, (size_t)resource_size(res));
+
+	return ptr;
+}
+
+void etnaviv_writel(u32 data, void __iomem *addr)
+{
+	if (reglog)
+		printk(KERN_DEBUG "IO:W %p %08x\n", addr, data);
+
+	writel(data, addr);
+}
+
+u32 etnaviv_readl(const void __iomem *addr)
+{
+	u32 val = readl(addr);
+
+	if (reglog)
+		printk(KERN_DEBUG "IO:R %p %08x\n", addr, val);
+
+	return val;
+}
+
+/*
+ * DRM operations:
+ */
+
+
+static void load_gpu(struct drm_device *dev)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	unsigned int i;
+
+	for (i = 0; i < ETNA_MAX_PIPES; i++) {
+		struct etnaviv_gpu *g = priv->gpu[i];
+
+		if (g) {
+			int ret;
+
+			ret = etnaviv_gpu_init(g);
+			if (ret) {
+				dev_err(g->dev, "hw init failed: %d\n", ret);
+				priv->gpu[i] = NULL;
+			}
+		}
+	}
+}
+
+static int etnaviv_open(struct drm_device *dev, struct drm_file *file)
+{
+	struct etnaviv_file_private *ctx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	file->driver_priv = ctx;
+
+	return 0;
+}
+
+static void etnaviv_preclose(struct drm_device *dev, struct drm_file *file)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct etnaviv_file_private *ctx = file->driver_priv;
+	unsigned int i;
+
+	for (i = 0; i < ETNA_MAX_PIPES; i++) {
+		struct etnaviv_gpu *gpu = priv->gpu[i];
+
+		if (gpu) {
+			mutex_lock(&gpu->lock);
+			if (gpu->lastctx == ctx)
+				gpu->lastctx = NULL;
+			mutex_unlock(&gpu->lock);
+		}
+	}
+
+	kfree(ctx);
+}
+
+/*
+ * DRM debugfs:
+ */
+
+#ifdef CONFIG_DEBUG_FS
+static int etnaviv_gem_show(struct drm_device *dev, struct seq_file *m)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+
+	etnaviv_gem_describe_objects(priv, m);
+
+	return 0;
+}
+
+static int etnaviv_mm_show(struct drm_device *dev, struct seq_file *m)
+{
+	int ret;
+
+	read_lock(&dev->vma_offset_manager->vm_lock);
+	ret = drm_mm_dump_table(m, &dev->vma_offset_manager->vm_addr_space_mm);
+	read_unlock(&dev->vma_offset_manager->vm_lock);
+
+	return ret;
+}
+
+static int etnaviv_mmu_show(struct etnaviv_gpu *gpu, struct seq_file *m)
+{
+	seq_printf(m, "Active Objects (%s):\n", dev_name(gpu->dev));
+
+	mutex_lock(&gpu->mmu->lock);
+	drm_mm_dump_table(m, &gpu->mmu->mm);
+	mutex_unlock(&gpu->mmu->lock);
+
+	return 0;
+}
+
+static void etnaviv_buffer_dump(struct etnaviv_gpu *gpu, struct seq_file *m)
+{
+	struct etnaviv_cmdbuf *buf = gpu->buffer;
+	u32 size = buf->size;
+	u32 *ptr = buf->vaddr;
+	u32 i;
+
+	seq_printf(m, "virt %p - phys 0x%llx - free 0x%08x\n",
+			buf->vaddr, (u64)buf->paddr, size - buf->user_size);
+
+	for (i = 0; i < size / 4; i++) {
+		if (i && !(i % 4))
+			seq_puts(m, "\n");
+		if (i % 4 == 0)
+			seq_printf(m, "\t0x%p: ", ptr + i);
+		seq_printf(m, "%08x ", *(ptr + i));
+	}
+	seq_puts(m, "\n");
+}
+
+static int etnaviv_ring_show(struct etnaviv_gpu *gpu, struct seq_file *m)
+{
+	seq_printf(m, "Ring Buffer (%s): ", dev_name(gpu->dev));
+
+	mutex_lock(&gpu->lock);
+	etnaviv_buffer_dump(gpu, m);
+	mutex_unlock(&gpu->lock);
+
+	return 0;
+}
+
+static int show_unlocked(struct seq_file *m, void *arg)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	int (*show)(struct drm_device *dev, struct seq_file *m) =
+			node->info_ent->data;
+
+	return show(dev, m);
+}
+
+static int show_each_gpu(struct seq_file *m, void *arg)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct etnaviv_gpu *gpu;
+	int (*show)(struct etnaviv_gpu *gpu, struct seq_file *m) =
+			node->info_ent->data;
+	unsigned int i;
+	int ret = 0;
+
+	for (i = 0; i < ETNA_MAX_PIPES; i++) {
+		gpu = priv->gpu[i];
+		if (!gpu)
+			continue;
+
+		ret = show(gpu, m);
+		if (ret < 0)
+			break;
+	}
+
+	return ret;
+}
+
+static struct drm_info_list etnaviv_debugfs_list[] = {
+		{"gpu", show_each_gpu, 0, etnaviv_gpu_debugfs},
+		{"gem", show_unlocked, 0, etnaviv_gem_show},
+		{ "mm", show_unlocked, 0, etnaviv_mm_show },
+		{"mmu", show_each_gpu, 0, etnaviv_mmu_show},
+		{"ring", show_each_gpu, 0, etnaviv_ring_show},
+};
+
+static int etnaviv_debugfs_init(struct drm_minor *minor)
+{
+	struct drm_device *dev = minor->dev;
+	int ret;
+
+	ret = drm_debugfs_create_files(etnaviv_debugfs_list,
+			ARRAY_SIZE(etnaviv_debugfs_list),
+			minor->debugfs_root, minor);
+
+	if (ret) {
+		dev_err(dev->dev, "could not install etnaviv_debugfs_list\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static void etnaviv_debugfs_cleanup(struct drm_minor *minor)
+{
+	drm_debugfs_remove_files(etnaviv_debugfs_list,
+			ARRAY_SIZE(etnaviv_debugfs_list), minor);
+}
+#endif
+
+/*
+ * DRM ioctls:
+ */
+
+static int etnaviv_ioctl_get_param(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct drm_etnaviv_param *args = data;
+	struct etnaviv_gpu *gpu;
+
+	if (args->pipe >= ETNA_MAX_PIPES)
+		return -EINVAL;
+
+	gpu = priv->gpu[args->pipe];
+	if (!gpu)
+		return -ENXIO;
+
+	return etnaviv_gpu_get_param(gpu, args->param, &args->value);
+}
+
+static int etnaviv_ioctl_gem_new(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct drm_etnaviv_gem_new *args = data;
+
+	if (args->flags & ~(ETNA_BO_CACHED | ETNA_BO_WC | ETNA_BO_UNCACHED |
+			    ETNA_BO_FORCE_MMU))
+		return -EINVAL;
+
+	return etnaviv_gem_new_handle(dev, file, args->size,
+			args->flags, &args->handle);
+}
+
+#define TS(t) ((struct timespec){ \
+	.tv_sec = (t).tv_sec, \
+	.tv_nsec = (t).tv_nsec \
+})
+
+static int etnaviv_ioctl_gem_cpu_prep(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct drm_etnaviv_gem_cpu_prep *args = data;
+	struct drm_gem_object *obj;
+	int ret;
+
+	if (args->op & ~(ETNA_PREP_READ | ETNA_PREP_WRITE | ETNA_PREP_NOSYNC))
+		return -EINVAL;
+
+	obj = drm_gem_object_lookup(dev, file, args->handle);
+	if (!obj)
+		return -ENOENT;
+
+	ret = etnaviv_gem_cpu_prep(obj, args->op, &TS(args->timeout));
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static int etnaviv_ioctl_gem_cpu_fini(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct drm_etnaviv_gem_cpu_fini *args = data;
+	struct drm_gem_object *obj;
+	int ret;
+
+	if (args->flags)
+		return -EINVAL;
+
+	obj = drm_gem_object_lookup(dev, file, args->handle);
+	if (!obj)
+		return -ENOENT;
+
+	ret = etnaviv_gem_cpu_fini(obj);
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static int etnaviv_ioctl_gem_info(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct drm_etnaviv_gem_info *args = data;
+	struct drm_gem_object *obj;
+	int ret;
+
+	if (args->pad)
+		return -EINVAL;
+
+	obj = drm_gem_object_lookup(dev, file, args->handle);
+	if (!obj)
+		return -ENOENT;
+
+	ret = etnaviv_gem_mmap_offset(obj, &args->offset);
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static int etnaviv_ioctl_wait_fence(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct drm_etnaviv_wait_fence *args = data;
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct timespec *timeout = &TS(args->timeout);
+	struct etnaviv_gpu *gpu;
+
+	if (args->flags & ~(ETNA_WAIT_NONBLOCK))
+		return -EINVAL;
+
+	if (args->pipe >= ETNA_MAX_PIPES)
+		return -EINVAL;
+
+	gpu = priv->gpu[args->pipe];
+	if (!gpu)
+		return -ENXIO;
+
+	if (args->flags & ETNA_WAIT_NONBLOCK)
+		timeout = NULL;
+
+	return etnaviv_gpu_wait_fence_interruptible(gpu, args->fence,
+						    timeout);
+}
+
+static int etnaviv_ioctl_gem_userptr(struct drm_device *dev, void *data,
+	struct drm_file *file)
+{
+	struct drm_etnaviv_gem_userptr *args = data;
+	int access;
+
+	if (args->flags & ~(ETNA_USERPTR_READ|ETNA_USERPTR_WRITE) ||
+	    args->flags == 0)
+		return -EINVAL;
+
+	if (offset_in_page(args->user_ptr | args->user_size) ||
+	    (uintptr_t)args->user_ptr != args->user_ptr ||
+	    (u32)args->user_size != args->user_size ||
+	    args->user_ptr & ~PAGE_MASK)
+		return -EINVAL;
+
+	if (args->flags & ETNA_USERPTR_WRITE)
+		access = VERIFY_WRITE;
+	else
+		access = VERIFY_READ;
+
+	if (!access_ok(access, (void __user *)(unsigned long)args->user_ptr,
+		       args->user_size))
+		return -EFAULT;
+
+	return etnaviv_gem_new_userptr(dev, file, args->user_ptr,
+				       args->user_size, args->flags,
+				       &args->handle);
+}
+
+static int etnaviv_ioctl_gem_wait(struct drm_device *dev, void *data,
+	struct drm_file *file)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct drm_etnaviv_gem_wait *args = data;
+	struct timespec *timeout = &TS(args->timeout);
+	struct drm_gem_object *obj;
+	struct etnaviv_gpu *gpu;
+	int ret;
+
+	if (args->flags & ~(ETNA_WAIT_NONBLOCK))
+		return -EINVAL;
+
+	if (args->pipe >= ETNA_MAX_PIPES)
+		return -EINVAL;
+
+	gpu = priv->gpu[args->pipe];
+	if (!gpu)
+		return -ENXIO;
+
+	obj = drm_gem_object_lookup(dev, file, args->handle);
+	if (!obj)
+		return -ENOENT;
+
+	if (args->flags & ETNA_WAIT_NONBLOCK)
+		timeout = NULL;
+
+	ret = etnaviv_gem_wait_bo(gpu, obj, timeout);
+
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+static const struct drm_ioctl_desc etnaviv_ioctls[] = {
+#define ETNA_IOCTL(n, func, flags) \
+	DRM_IOCTL_DEF_DRV(ETNAVIV_##n, etnaviv_ioctl_##func, flags)
+	ETNA_IOCTL(GET_PARAM,    get_param,    DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_NEW,      gem_new,      DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_INFO,     gem_info,     DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_CPU_PREP, gem_cpu_prep, DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_CPU_FINI, gem_cpu_fini, DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_SUBMIT,   gem_submit,   DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(WAIT_FENCE,   wait_fence,   DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_USERPTR,  gem_userptr,  DRM_AUTH|DRM_RENDER_ALLOW),
+	ETNA_IOCTL(GEM_WAIT,     gem_wait,     DRM_AUTH|DRM_RENDER_ALLOW),
+};
+
+static const struct vm_operations_struct vm_ops = {
+	.fault = etnaviv_gem_fault,
+	.open = drm_gem_vm_open,
+	.close = drm_gem_vm_close,
+};
+
+static const struct file_operations fops = {
+	.owner              = THIS_MODULE,
+	.open               = drm_open,
+	.release            = drm_release,
+	.unlocked_ioctl     = drm_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl       = drm_compat_ioctl,
+#endif
+	.poll               = drm_poll,
+	.read               = drm_read,
+	.llseek             = no_llseek,
+	.mmap               = etnaviv_gem_mmap,
+};
+
+static struct drm_driver etnaviv_drm_driver = {
+	.driver_features    = DRIVER_HAVE_IRQ |
+				DRIVER_GEM |
+				DRIVER_PRIME |
+				DRIVER_RENDER,
+	.open               = etnaviv_open,
+	.preclose           = etnaviv_preclose,
+	.set_busid          = drm_platform_set_busid,
+	.gem_free_object    = etnaviv_gem_free_object,
+	.gem_vm_ops         = &vm_ops,
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export   = drm_gem_prime_export,
+	.gem_prime_import   = drm_gem_prime_import,
+	.gem_prime_pin      = etnaviv_gem_prime_pin,
+	.gem_prime_unpin    = etnaviv_gem_prime_unpin,
+	.gem_prime_get_sg_table = etnaviv_gem_prime_get_sg_table,
+	.gem_prime_import_sg_table = etnaviv_gem_prime_import_sg_table,
+	.gem_prime_vmap     = etnaviv_gem_prime_vmap,
+	.gem_prime_vunmap   = etnaviv_gem_prime_vunmap,
+#ifdef CONFIG_DEBUG_FS
+	.debugfs_init       = etnaviv_debugfs_init,
+	.debugfs_cleanup    = etnaviv_debugfs_cleanup,
+#endif
+	.ioctls             = etnaviv_ioctls,
+	.num_ioctls         = DRM_ETNAVIV_NUM_IOCTLS,
+	.fops               = &fops,
+	.name               = "etnaviv",
+	.desc               = "etnaviv DRM",
+	.date               = "20151214",
+	.major              = 1,
+	.minor              = 0,
+};
+
+/*
+ * Platform driver:
+ */
+static int etnaviv_bind(struct device *dev)
+{
+	struct etnaviv_drm_private *priv;
+	struct drm_device *drm;
+	int ret;
+
+	drm = drm_dev_alloc(&etnaviv_drm_driver, dev);
+	if (!drm)
+		return -ENOMEM;
+
+	drm->platformdev = to_platform_device(dev);
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(dev, "failed to allocate private data\n");
+		ret = -ENOMEM;
+		goto out_unref;
+	}
+	drm->dev_private = priv;
+
+	priv->wq = alloc_ordered_workqueue("etnaviv", 0);
+	if (!priv->wq) {
+		ret = -ENOMEM;
+		goto out_wq;
+	}
+
+	mutex_init(&priv->gem_lock);
+	INIT_LIST_HEAD(&priv->gem_list);
+	priv->num_gpus = 0;
+
+	dev_set_drvdata(dev, drm);
+
+	ret = component_bind_all(dev, drm);
+	if (ret < 0)
+		goto out_bind;
+
+	load_gpu(drm);
+
+	ret = drm_dev_register(drm, 0);
+	if (ret)
+		goto out_register;
+
+	return 0;
+
+out_register:
+	component_unbind_all(dev, drm);
+out_bind:
+	flush_workqueue(priv->wq);
+	destroy_workqueue(priv->wq);
+out_wq:
+	kfree(priv);
+out_unref:
+	drm_dev_unref(drm);
+
+	return ret;
+}
+
+static void etnaviv_unbind(struct device *dev)
+{
+	struct drm_device *drm = dev_get_drvdata(dev);
+	struct etnaviv_drm_private *priv = drm->dev_private;
+
+	drm_dev_unregister(drm);
+
+	flush_workqueue(priv->wq);
+	destroy_workqueue(priv->wq);
+
+	component_unbind_all(dev, drm);
+
+	drm->dev_private = NULL;
+	kfree(priv);
+
+	drm_put_dev(drm);
+}
+
+static const struct component_master_ops etnaviv_master_ops = {
+	.bind = etnaviv_bind,
+	.unbind = etnaviv_unbind,
+};
+
+static int compare_of(struct device *dev, void *data)
+{
+	struct device_node *np = data;
+
+	return dev->of_node == np;
+}
+
+static int compare_str(struct device *dev, void *data)
+{
+	return !strcmp(dev_name(dev), data);
+}
+
+static int etnaviv_pdev_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct component_match *match = NULL;
+
+	dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+
+	if (node) {
+		struct device_node *core_node;
+		int i;
+
+		for (i = 0; ; i++) {
+			core_node = of_parse_phandle(node, "cores", i);
+			if (!core_node)
+				break;
+
+			component_match_add(&pdev->dev, &match, compare_of,
+					    core_node);
+			of_node_put(core_node);
+		}
+	} else if (dev->platform_data) {
+		char **names = dev->platform_data;
+		unsigned i;
+
+		for (i = 0; names[i]; i++)
+			component_match_add(dev, &match, compare_str, names[i]);
+	}
+
+	return component_master_add_with_match(dev, &etnaviv_master_ops, match);
+}
+
+static int etnaviv_pdev_remove(struct platform_device *pdev)
+{
+	component_master_del(&pdev->dev, &etnaviv_master_ops);
+
+	return 0;
+}
+
+static const struct of_device_id dt_match[] = {
+	{ .compatible = "fsl,imx-gpu-subsystem" },
+	{ .compatible = "marvell,dove-gpu-subsystem" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, dt_match);
+
+static struct platform_driver etnaviv_platform_driver = {
+	.probe      = etnaviv_pdev_probe,
+	.remove     = etnaviv_pdev_remove,
+	.driver     = {
+		.name   = "etnaviv",
+		.of_match_table = dt_match,
+	},
+};
+
+static int __init etnaviv_init(void)
+{
+	int ret;
+
+	etnaviv_validate_init();
+
+	ret = platform_driver_register(&etnaviv_gpu_driver);
+	if (ret != 0)
+		return ret;
+
+	ret = platform_driver_register(&etnaviv_platform_driver);
+	if (ret != 0)
+		platform_driver_unregister(&etnaviv_gpu_driver);
+
+	return ret;
+}
+module_init(etnaviv_init);
+
+static void __exit etnaviv_exit(void)
+{
+	platform_driver_unregister(&etnaviv_gpu_driver);
+	platform_driver_unregister(&etnaviv_platform_driver);
+}
+module_exit(etnaviv_exit);
+
+MODULE_AUTHOR("Christian Gmeiner <christian.gmeiner@gmail.com>");
+MODULE_AUTHOR("Russell King <rmk+kernel@arm.linux.org.uk>");
+MODULE_AUTHOR("Lucas Stach <l.stach@pengutronix.de>");
+MODULE_DESCRIPTION("etnaviv DRM Driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:etnaviv");
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_drv.h b/drivers/gpu/drm/etnaviv/etnaviv_drv.h
new file mode 100644
index 000000000000..1cd6046e76b1
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_drv.h
@@ -0,0 +1,161 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_DRV_H__
+#define __ETNAVIV_DRV_H__
+
+#include <linux/kernel.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/iommu.h>
+#include <linux/types.h>
+#include <linux/sizes.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
+#include <drm/drm_gem.h>
+#include <drm/etnaviv_drm.h>
+
+struct etnaviv_cmdbuf;
+struct etnaviv_gpu;
+struct etnaviv_mmu;
+struct etnaviv_gem_object;
+struct etnaviv_gem_submit;
+
+struct etnaviv_file_private {
+	/* currently we don't do anything useful with this.. but when
+	 * per-context address spaces are supported we'd keep track of
+	 * the context's page-tables here.
+	 */
+	int dummy;
+};
+
+struct etnaviv_drm_private {
+	int num_gpus;
+	struct etnaviv_gpu *gpu[ETNA_MAX_PIPES];
+
+	/* list of GEM objects: */
+	struct mutex gem_lock;
+	struct list_head gem_list;
+
+	struct workqueue_struct *wq;
+};
+
+static inline void etnaviv_queue_work(struct drm_device *dev,
+	struct work_struct *w)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+
+	queue_work(priv->wq, w);
+}
+
+int etnaviv_ioctl_gem_submit(struct drm_device *dev, void *data,
+		struct drm_file *file);
+
+int etnaviv_gem_mmap(struct file *filp, struct vm_area_struct *vma);
+int etnaviv_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
+int etnaviv_gem_mmap_offset(struct drm_gem_object *obj, u64 *offset);
+int etnaviv_gem_get_iova(struct etnaviv_gpu *gpu,
+	struct drm_gem_object *obj, u32 *iova);
+void etnaviv_gem_put_iova(struct etnaviv_gpu *gpu, struct drm_gem_object *obj);
+struct sg_table *etnaviv_gem_prime_get_sg_table(struct drm_gem_object *obj);
+void *etnaviv_gem_prime_vmap(struct drm_gem_object *obj);
+void etnaviv_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr);
+struct drm_gem_object *etnaviv_gem_prime_import_sg_table(struct drm_device *dev,
+	struct dma_buf_attachment *attach, struct sg_table *sg);
+int etnaviv_gem_prime_pin(struct drm_gem_object *obj);
+void etnaviv_gem_prime_unpin(struct drm_gem_object *obj);
+void *etnaviv_gem_vmap(struct drm_gem_object *obj);
+int etnaviv_gem_cpu_prep(struct drm_gem_object *obj, u32 op,
+		struct timespec *timeout);
+int etnaviv_gem_cpu_fini(struct drm_gem_object *obj);
+void etnaviv_gem_free_object(struct drm_gem_object *obj);
+int etnaviv_gem_new_handle(struct drm_device *dev, struct drm_file *file,
+		u32 size, u32 flags, u32 *handle);
+struct drm_gem_object *etnaviv_gem_new_locked(struct drm_device *dev,
+		u32 size, u32 flags);
+struct drm_gem_object *etnaviv_gem_new(struct drm_device *dev,
+		u32 size, u32 flags);
+int etnaviv_gem_new_userptr(struct drm_device *dev, struct drm_file *file,
+	uintptr_t ptr, u32 size, u32 flags, u32 *handle);
+u16 etnaviv_buffer_init(struct etnaviv_gpu *gpu);
+void etnaviv_buffer_end(struct etnaviv_gpu *gpu);
+void etnaviv_buffer_queue(struct etnaviv_gpu *gpu, unsigned int event,
+	struct etnaviv_cmdbuf *cmdbuf);
+void etnaviv_validate_init(void);
+bool etnaviv_cmd_validate_one(struct etnaviv_gpu *gpu,
+	u32 *stream, unsigned int size,
+	struct drm_etnaviv_gem_submit_reloc *relocs, unsigned int reloc_size);
+
+#ifdef CONFIG_DEBUG_FS
+void etnaviv_gem_describe_objects(struct etnaviv_drm_private *priv,
+	struct seq_file *m);
+#endif
+
+void __iomem *etnaviv_ioremap(struct platform_device *pdev, const char *name,
+		const char *dbgname);
+void etnaviv_writel(u32 data, void __iomem *addr);
+u32 etnaviv_readl(const void __iomem *addr);
+
+#define DBG(fmt, ...) DRM_DEBUG(fmt"\n", ##__VA_ARGS__)
+#define VERB(fmt, ...) if (0) DRM_DEBUG(fmt"\n", ##__VA_ARGS__)
+
+/*
+ * Return the storage size of a structure with a variable length array.
+ * The array is nelem elements of elem_size, where the base structure
+ * is defined by base.  If the size overflows size_t, return zero.
+ */
+static inline size_t size_vstruct(size_t nelem, size_t elem_size, size_t base)
+{
+	if (elem_size && nelem > (SIZE_MAX - base) / elem_size)
+		return 0;
+	return base + nelem * elem_size;
+}
+
+/* returns true if fence a comes after fence b */
+static inline bool fence_after(u32 a, u32 b)
+{
+	return (s32)(a - b) > 0;
+}
+
+static inline bool fence_after_eq(u32 a, u32 b)
+{
+	return (s32)(a - b) >= 0;
+}
+
+static inline unsigned long etnaviv_timeout_to_jiffies(
+	const struct timespec *timeout)
+{
+	unsigned long timeout_jiffies = timespec_to_jiffies(timeout);
+	unsigned long start_jiffies = jiffies;
+	unsigned long remaining_jiffies;
+
+	if (time_after(start_jiffies, timeout_jiffies))
+		remaining_jiffies = 0;
+	else
+		remaining_jiffies = timeout_jiffies - start_jiffies;
+
+	return remaining_jiffies;
+}
+
+#endif /* __ETNAVIV_DRV_H__ */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_dump.c b/drivers/gpu/drm/etnaviv/etnaviv_dump.c
new file mode 100644
index 000000000000..4a29eeadbf1e
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_dump.c
@@ -0,0 +1,229 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/devcoredump.h>
+#include "etnaviv_dump.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_mmu.h"
+#include "state.xml.h"
+#include "state_hi.xml.h"
+
+struct core_dump_iterator {
+	void *start;
+	struct etnaviv_dump_object_header *hdr;
+	void *data;
+};
+
+static const unsigned short etnaviv_dump_registers[] = {
+	VIVS_HI_AXI_STATUS,
+	VIVS_HI_CLOCK_CONTROL,
+	VIVS_HI_IDLE_STATE,
+	VIVS_HI_AXI_CONFIG,
+	VIVS_HI_INTR_ENBL,
+	VIVS_HI_CHIP_IDENTITY,
+	VIVS_HI_CHIP_FEATURE,
+	VIVS_HI_CHIP_MODEL,
+	VIVS_HI_CHIP_REV,
+	VIVS_HI_CHIP_DATE,
+	VIVS_HI_CHIP_TIME,
+	VIVS_HI_CHIP_MINOR_FEATURE_0,
+	VIVS_HI_CACHE_CONTROL,
+	VIVS_HI_AXI_CONTROL,
+	VIVS_PM_POWER_CONTROLS,
+	VIVS_PM_MODULE_CONTROLS,
+	VIVS_PM_MODULE_STATUS,
+	VIVS_PM_PULSE_EATER,
+	VIVS_MC_MMU_FE_PAGE_TABLE,
+	VIVS_MC_MMU_TX_PAGE_TABLE,
+	VIVS_MC_MMU_PE_PAGE_TABLE,
+	VIVS_MC_MMU_PEZ_PAGE_TABLE,
+	VIVS_MC_MMU_RA_PAGE_TABLE,
+	VIVS_MC_DEBUG_MEMORY,
+	VIVS_MC_MEMORY_BASE_ADDR_RA,
+	VIVS_MC_MEMORY_BASE_ADDR_FE,
+	VIVS_MC_MEMORY_BASE_ADDR_TX,
+	VIVS_MC_MEMORY_BASE_ADDR_PEZ,
+	VIVS_MC_MEMORY_BASE_ADDR_PE,
+	VIVS_MC_MEMORY_TIMING_CONTROL,
+	VIVS_MC_BUS_CONFIG,
+	VIVS_FE_DMA_STATUS,
+	VIVS_FE_DMA_DEBUG_STATE,
+	VIVS_FE_DMA_ADDRESS,
+	VIVS_FE_DMA_LOW,
+	VIVS_FE_DMA_HIGH,
+	VIVS_FE_AUTO_FLUSH,
+};
+
+static void etnaviv_core_dump_header(struct core_dump_iterator *iter,
+	u32 type, void *data_end)
+{
+	struct etnaviv_dump_object_header *hdr = iter->hdr;
+
+	hdr->magic = cpu_to_le32(ETDUMP_MAGIC);
+	hdr->type = cpu_to_le32(type);
+	hdr->file_offset = cpu_to_le32(iter->data - iter->start);
+	hdr->file_size = cpu_to_le32(data_end - iter->data);
+
+	iter->hdr++;
+	iter->data += hdr->file_size;
+}
+
+static void etnaviv_core_dump_registers(struct core_dump_iterator *iter,
+	struct etnaviv_gpu *gpu)
+{
+	struct etnaviv_dump_registers *reg = iter->data;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(etnaviv_dump_registers); i++, reg++) {
+		reg->reg = etnaviv_dump_registers[i];
+		reg->value = gpu_read(gpu, etnaviv_dump_registers[i]);
+	}
+
+	etnaviv_core_dump_header(iter, ETDUMP_BUF_REG, reg);
+}
+
+static void etnaviv_core_dump_mmu(struct core_dump_iterator *iter,
+	struct etnaviv_gpu *gpu, size_t mmu_size)
+{
+	etnaviv_iommu_dump(gpu->mmu, iter->data);
+
+	etnaviv_core_dump_header(iter, ETDUMP_BUF_MMU, iter->data + mmu_size);
+}
+
+static void etnaviv_core_dump_mem(struct core_dump_iterator *iter, u32 type,
+	void *ptr, size_t size, u64 iova)
+{
+	memcpy(iter->data, ptr, size);
+
+	iter->hdr->iova = cpu_to_le64(iova);
+
+	etnaviv_core_dump_header(iter, type, iter->data + size);
+}
+
+void etnaviv_core_dump(struct etnaviv_gpu *gpu)
+{
+	struct core_dump_iterator iter;
+	struct etnaviv_vram_mapping *vram;
+	struct etnaviv_gem_object *obj;
+	struct etnaviv_cmdbuf *cmd;
+	unsigned int n_obj, n_bomap_pages;
+	size_t file_size, mmu_size;
+	__le64 *bomap, *bomap_start;
+
+	mmu_size = etnaviv_iommu_dump_size(gpu->mmu);
+
+	/* We always dump registers, mmu, ring and end marker */
+	n_obj = 4;
+	n_bomap_pages = 0;
+	file_size = ARRAY_SIZE(etnaviv_dump_registers) *
+			sizeof(struct etnaviv_dump_registers) +
+		    mmu_size + gpu->buffer->size;
+
+	/* Add in the active command buffers */
+	list_for_each_entry(cmd, &gpu->active_cmd_list, node) {
+		file_size += cmd->size;
+		n_obj++;
+	}
+
+	/* Add in the active buffer objects */
+	list_for_each_entry(vram, &gpu->mmu->mappings, mmu_node) {
+		if (!vram->use)
+			continue;
+
+		obj = vram->object;
+		file_size += obj->base.size;
+		n_bomap_pages += obj->base.size >> PAGE_SHIFT;
+		n_obj++;
+	}
+
+	/* If we have any buffer objects, add a bomap object */
+	if (n_bomap_pages) {
+		file_size += n_bomap_pages * sizeof(__le64);
+		n_obj++;
+	}
+
+	/* Add the size of the headers */
+	file_size += sizeof(*iter.hdr) * n_obj;
+
+	/* Allocate the file in vmalloc memory, it's likely to be big */
+	iter.start = vmalloc(file_size);
+	if (!iter.start) {
+		dev_warn(gpu->dev, "failed to allocate devcoredump file\n");
+		return;
+	}
+
+	/* Point the data member after the headers */
+	iter.hdr = iter.start;
+	iter.data = &iter.hdr[n_obj];
+
+	memset(iter.hdr, 0, iter.data - iter.start);
+
+	etnaviv_core_dump_registers(&iter, gpu);
+	etnaviv_core_dump_mmu(&iter, gpu, mmu_size);
+	etnaviv_core_dump_mem(&iter, ETDUMP_BUF_RING, gpu->buffer->vaddr,
+			      gpu->buffer->size, gpu->buffer->paddr);
+
+	list_for_each_entry(cmd, &gpu->active_cmd_list, node)
+		etnaviv_core_dump_mem(&iter, ETDUMP_BUF_CMD, cmd->vaddr,
+				      cmd->size, cmd->paddr);
+
+	/* Reserve space for the bomap */
+	if (n_bomap_pages) {
+		bomap_start = bomap = iter.data;
+		memset(bomap, 0, sizeof(*bomap) * n_bomap_pages);
+		etnaviv_core_dump_header(&iter, ETDUMP_BUF_BOMAP,
+					 bomap + n_bomap_pages);
+	} else {
+		/* Silence warning */
+		bomap_start = bomap = NULL;
+	}
+
+	list_for_each_entry(vram, &gpu->mmu->mappings, mmu_node) {
+		struct page **pages;
+		void *vaddr;
+
+		if (vram->use == 0)
+			continue;
+
+		obj = vram->object;
+
+		mutex_lock(&obj->lock);
+		pages = etnaviv_gem_get_pages(obj);
+		mutex_unlock(&obj->lock);
+		if (pages) {
+			int j;
+
+			iter.hdr->data[0] = bomap - bomap_start;
+
+			for (j = 0; j < obj->base.size >> PAGE_SHIFT; j++)
+				*bomap++ = cpu_to_le64(page_to_phys(*pages++));
+		}
+
+		iter.hdr->iova = cpu_to_le64(vram->iova);
+
+		vaddr = etnaviv_gem_vmap(&obj->base);
+		if (vaddr)
+			memcpy(iter.data, vaddr, obj->base.size);
+
+		etnaviv_core_dump_header(&iter, ETDUMP_BUF_BO, iter.data +
+					 obj->base.size);
+	}
+
+	etnaviv_core_dump_header(&iter, ETDUMP_BUF_END, iter.data);
+
+	dev_coredumpv(gpu->dev, iter.start, iter.data - iter.start, GFP_KERNEL);
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_dump.h b/drivers/gpu/drm/etnaviv/etnaviv_dump.h
new file mode 100644
index 000000000000..97f2f8db9133
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_dump.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Etnaviv devcoredump file definitions
+ */
+#ifndef ETNAVIV_DUMP_H
+#define ETNAVIV_DUMP_H
+
+#include <linux/types.h>
+
+enum {
+	ETDUMP_MAGIC = 0x414e5445,
+	ETDUMP_BUF_REG = 0,
+	ETDUMP_BUF_MMU,
+	ETDUMP_BUF_RING,
+	ETDUMP_BUF_CMD,
+	ETDUMP_BUF_BOMAP,
+	ETDUMP_BUF_BO,
+	ETDUMP_BUF_END,
+};
+
+struct etnaviv_dump_object_header {
+	__le32 magic;
+	__le32 type;
+	__le32 file_offset;
+	__le32 file_size;
+	__le64 iova;
+	__le32 data[2];
+};
+
+/* Registers object, an array of these */
+struct etnaviv_dump_registers {
+	__le32 reg;
+	__le32 value;
+};
+
+#ifdef __KERNEL__
+struct etnaviv_gpu;
+void etnaviv_core_dump(struct etnaviv_gpu *gpu);
+#endif
+
+#endif
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem.c b/drivers/gpu/drm/etnaviv/etnaviv_gem.c
new file mode 100644
index 000000000000..4b519e4309b2
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem.c
@@ -0,0 +1,915 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/spinlock.h>
+#include <linux/shmem_fs.h>
+
+#include "etnaviv_drv.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_mmu.h"
+
+static void etnaviv_gem_scatter_map(struct etnaviv_gem_object *etnaviv_obj)
+{
+	struct drm_device *dev = etnaviv_obj->base.dev;
+	struct sg_table *sgt = etnaviv_obj->sgt;
+
+	/*
+	 * For non-cached buffers, ensure the new pages are clean
+	 * because display controller, GPU, etc. are not coherent.
+	 */
+	if (etnaviv_obj->flags & ETNA_BO_CACHE_MASK)
+		dma_map_sg(dev->dev, sgt->sgl, sgt->nents, DMA_BIDIRECTIONAL);
+}
+
+static void etnaviv_gem_scatterlist_unmap(struct etnaviv_gem_object *etnaviv_obj)
+{
+	struct drm_device *dev = etnaviv_obj->base.dev;
+	struct sg_table *sgt = etnaviv_obj->sgt;
+
+	/*
+	 * For non-cached buffers, ensure the new pages are clean
+	 * because display controller, GPU, etc. are not coherent:
+	 *
+	 * WARNING: The DMA API does not support concurrent CPU
+	 * and device access to the memory area.  With BIDIRECTIONAL,
+	 * we will clean the cache lines which overlap the region,
+	 * and invalidate all cache lines (partially) contained in
+	 * the region.
+	 *
+	 * If you have dirty data in the overlapping cache lines,
+	 * that will corrupt the GPU-written data.  If you have
+	 * written into the remainder of the region, this can
+	 * discard those writes.
+	 */
+	if (etnaviv_obj->flags & ETNA_BO_CACHE_MASK)
+		dma_unmap_sg(dev->dev, sgt->sgl, sgt->nents, DMA_BIDIRECTIONAL);
+}
+
+/* called with etnaviv_obj->lock held */
+static int etnaviv_gem_shmem_get_pages(struct etnaviv_gem_object *etnaviv_obj)
+{
+	struct drm_device *dev = etnaviv_obj->base.dev;
+	struct page **p = drm_gem_get_pages(&etnaviv_obj->base);
+
+	if (IS_ERR(p)) {
+		dev_err(dev->dev, "could not get pages: %ld\n", PTR_ERR(p));
+		return PTR_ERR(p);
+	}
+
+	etnaviv_obj->pages = p;
+
+	return 0;
+}
+
+static void put_pages(struct etnaviv_gem_object *etnaviv_obj)
+{
+	if (etnaviv_obj->sgt) {
+		etnaviv_gem_scatterlist_unmap(etnaviv_obj);
+		sg_free_table(etnaviv_obj->sgt);
+		kfree(etnaviv_obj->sgt);
+		etnaviv_obj->sgt = NULL;
+	}
+	if (etnaviv_obj->pages) {
+		drm_gem_put_pages(&etnaviv_obj->base, etnaviv_obj->pages,
+				  true, false);
+
+		etnaviv_obj->pages = NULL;
+	}
+}
+
+struct page **etnaviv_gem_get_pages(struct etnaviv_gem_object *etnaviv_obj)
+{
+	int ret;
+
+	lockdep_assert_held(&etnaviv_obj->lock);
+
+	if (!etnaviv_obj->pages) {
+		ret = etnaviv_obj->ops->get_pages(etnaviv_obj);
+		if (ret < 0)
+			return ERR_PTR(ret);
+	}
+
+	if (!etnaviv_obj->sgt) {
+		struct drm_device *dev = etnaviv_obj->base.dev;
+		int npages = etnaviv_obj->base.size >> PAGE_SHIFT;
+		struct sg_table *sgt;
+
+		sgt = drm_prime_pages_to_sg(etnaviv_obj->pages, npages);
+		if (IS_ERR(sgt)) {
+			dev_err(dev->dev, "failed to allocate sgt: %ld\n",
+				PTR_ERR(sgt));
+			return ERR_CAST(sgt);
+		}
+
+		etnaviv_obj->sgt = sgt;
+
+		etnaviv_gem_scatter_map(etnaviv_obj);
+	}
+
+	return etnaviv_obj->pages;
+}
+
+void etnaviv_gem_put_pages(struct etnaviv_gem_object *etnaviv_obj)
+{
+	lockdep_assert_held(&etnaviv_obj->lock);
+	/* when we start tracking the pin count, then do something here */
+}
+
+static int etnaviv_gem_mmap_obj(struct drm_gem_object *obj,
+		struct vm_area_struct *vma)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	pgprot_t vm_page_prot;
+
+	vma->vm_flags &= ~VM_PFNMAP;
+	vma->vm_flags |= VM_MIXEDMAP;
+
+	vm_page_prot = vm_get_page_prot(vma->vm_flags);
+
+	if (etnaviv_obj->flags & ETNA_BO_WC) {
+		vma->vm_page_prot = pgprot_writecombine(vm_page_prot);
+	} else if (etnaviv_obj->flags & ETNA_BO_UNCACHED) {
+		vma->vm_page_prot = pgprot_noncached(vm_page_prot);
+	} else {
+		/*
+		 * Shunt off cached objs to shmem file so they have their own
+		 * address_space (so unmap_mapping_range does what we want,
+		 * in particular in the case of mmap'd dmabufs)
+		 */
+		fput(vma->vm_file);
+		get_file(obj->filp);
+		vma->vm_pgoff = 0;
+		vma->vm_file  = obj->filp;
+
+		vma->vm_page_prot = vm_page_prot;
+	}
+
+	return 0;
+}
+
+int etnaviv_gem_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct etnaviv_gem_object *obj;
+	int ret;
+
+	ret = drm_gem_mmap(filp, vma);
+	if (ret) {
+		DBG("mmap failed: %d", ret);
+		return ret;
+	}
+
+	obj = to_etnaviv_bo(vma->vm_private_data);
+	return etnaviv_gem_mmap_obj(vma->vm_private_data, vma);
+}
+
+int etnaviv_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct drm_gem_object *obj = vma->vm_private_data;
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct page **pages, *page;
+	pgoff_t pgoff;
+	int ret;
+
+	/*
+	 * Make sure we don't parallel update on a fault, nor move or remove
+	 * something from beneath our feet.  Note that vm_insert_page() is
+	 * specifically coded to take care of this, so we don't have to.
+	 */
+	ret = mutex_lock_interruptible(&etnaviv_obj->lock);
+	if (ret)
+		goto out;
+
+	/* make sure we have pages attached now */
+	pages = etnaviv_gem_get_pages(etnaviv_obj);
+	mutex_unlock(&etnaviv_obj->lock);
+
+	if (IS_ERR(pages)) {
+		ret = PTR_ERR(pages);
+		goto out;
+	}
+
+	/* We don't use vmf->pgoff since that has the fake offset: */
+	pgoff = ((unsigned long)vmf->virtual_address -
+			vma->vm_start) >> PAGE_SHIFT;
+
+	page = pages[pgoff];
+
+	VERB("Inserting %p pfn %lx, pa %lx", vmf->virtual_address,
+	     page_to_pfn(page), page_to_pfn(page) << PAGE_SHIFT);
+
+	ret = vm_insert_page(vma, (unsigned long)vmf->virtual_address, page);
+
+out:
+	switch (ret) {
+	case -EAGAIN:
+	case 0:
+	case -ERESTARTSYS:
+	case -EINTR:
+	case -EBUSY:
+		/*
+		 * EBUSY is ok: this just means that another thread
+		 * already did the job.
+		 */
+		return VM_FAULT_NOPAGE;
+	case -ENOMEM:
+		return VM_FAULT_OOM;
+	default:
+		return VM_FAULT_SIGBUS;
+	}
+}
+
+int etnaviv_gem_mmap_offset(struct drm_gem_object *obj, u64 *offset)
+{
+	int ret;
+
+	/* Make it mmapable */
+	ret = drm_gem_create_mmap_offset(obj);
+	if (ret)
+		dev_err(obj->dev->dev, "could not allocate mmap offset\n");
+	else
+		*offset = drm_vma_node_offset_addr(&obj->vma_node);
+
+	return ret;
+}
+
+static struct etnaviv_vram_mapping *
+etnaviv_gem_get_vram_mapping(struct etnaviv_gem_object *obj,
+			     struct etnaviv_iommu *mmu)
+{
+	struct etnaviv_vram_mapping *mapping;
+
+	list_for_each_entry(mapping, &obj->vram_list, obj_node) {
+		if (mapping->mmu == mmu)
+			return mapping;
+	}
+
+	return NULL;
+}
+
+int etnaviv_gem_get_iova(struct etnaviv_gpu *gpu,
+	struct drm_gem_object *obj, u32 *iova)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct etnaviv_vram_mapping *mapping;
+	struct page **pages;
+	int ret = 0;
+
+	mutex_lock(&etnaviv_obj->lock);
+	mapping = etnaviv_gem_get_vram_mapping(etnaviv_obj, gpu->mmu);
+	if (mapping) {
+		/*
+		 * Holding the object lock prevents the use count changing
+		 * beneath us.  If the use count is zero, the MMU might be
+		 * reaping this object, so take the lock and re-check that
+		 * the MMU owns this mapping to close this race.
+		 */
+		if (mapping->use == 0) {
+			mutex_lock(&gpu->mmu->lock);
+			if (mapping->mmu == gpu->mmu)
+				mapping->use += 1;
+			else
+				mapping = NULL;
+			mutex_unlock(&gpu->mmu->lock);
+			if (mapping)
+				goto out;
+		} else {
+			mapping->use += 1;
+			goto out;
+		}
+	}
+
+	pages = etnaviv_gem_get_pages(etnaviv_obj);
+	if (IS_ERR(pages)) {
+		ret = PTR_ERR(pages);
+		goto out;
+	}
+
+	/*
+	 * See if we have a reaped vram mapping we can re-use before
+	 * allocating a fresh mapping.
+	 */
+	mapping = etnaviv_gem_get_vram_mapping(etnaviv_obj, NULL);
+	if (!mapping) {
+		mapping = kzalloc(sizeof(*mapping), GFP_KERNEL);
+		if (!mapping) {
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		INIT_LIST_HEAD(&mapping->scan_node);
+		mapping->object = etnaviv_obj;
+	} else {
+		list_del(&mapping->obj_node);
+	}
+
+	mapping->mmu = gpu->mmu;
+	mapping->use = 1;
+
+	ret = etnaviv_iommu_map_gem(gpu->mmu, etnaviv_obj, gpu->memory_base,
+				    mapping);
+	if (ret < 0)
+		kfree(mapping);
+	else
+		list_add_tail(&mapping->obj_node, &etnaviv_obj->vram_list);
+
+out:
+	mutex_unlock(&etnaviv_obj->lock);
+
+	if (!ret) {
+		/* Take a reference on the object */
+		drm_gem_object_reference(obj);
+		*iova = mapping->iova;
+	}
+
+	return ret;
+}
+
+void etnaviv_gem_put_iova(struct etnaviv_gpu *gpu, struct drm_gem_object *obj)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct etnaviv_vram_mapping *mapping;
+
+	mutex_lock(&etnaviv_obj->lock);
+	mapping = etnaviv_gem_get_vram_mapping(etnaviv_obj, gpu->mmu);
+
+	WARN_ON(mapping->use == 0);
+	mapping->use -= 1;
+	mutex_unlock(&etnaviv_obj->lock);
+
+	drm_gem_object_unreference_unlocked(obj);
+}
+
+void *etnaviv_gem_vmap(struct drm_gem_object *obj)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+	if (etnaviv_obj->vaddr)
+		return etnaviv_obj->vaddr;
+
+	mutex_lock(&etnaviv_obj->lock);
+	/*
+	 * Need to check again, as we might have raced with another thread
+	 * while waiting for the mutex.
+	 */
+	if (!etnaviv_obj->vaddr)
+		etnaviv_obj->vaddr = etnaviv_obj->ops->vmap(etnaviv_obj);
+	mutex_unlock(&etnaviv_obj->lock);
+
+	return etnaviv_obj->vaddr;
+}
+
+static void *etnaviv_gem_vmap_impl(struct etnaviv_gem_object *obj)
+{
+	struct page **pages;
+
+	lockdep_assert_held(&obj->lock);
+
+	pages = etnaviv_gem_get_pages(obj);
+	if (IS_ERR(pages))
+		return NULL;
+
+	return vmap(pages, obj->base.size >> PAGE_SHIFT,
+			VM_MAP, pgprot_writecombine(PAGE_KERNEL));
+}
+
+static inline enum dma_data_direction etnaviv_op_to_dma_dir(u32 op)
+{
+	if (op & ETNA_PREP_READ)
+		return DMA_FROM_DEVICE;
+	else if (op & ETNA_PREP_WRITE)
+		return DMA_TO_DEVICE;
+	else
+		return DMA_BIDIRECTIONAL;
+}
+
+int etnaviv_gem_cpu_prep(struct drm_gem_object *obj, u32 op,
+		struct timespec *timeout)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct drm_device *dev = obj->dev;
+	bool write = !!(op & ETNA_PREP_WRITE);
+	int ret;
+
+	if (op & ETNA_PREP_NOSYNC) {
+		if (!reservation_object_test_signaled_rcu(etnaviv_obj->resv,
+							  write))
+			return -EBUSY;
+	} else {
+		unsigned long remain = etnaviv_timeout_to_jiffies(timeout);
+
+		ret = reservation_object_wait_timeout_rcu(etnaviv_obj->resv,
+							  write, true, remain);
+		if (ret <= 0)
+			return ret == 0 ? -ETIMEDOUT : ret;
+	}
+
+	if (etnaviv_obj->flags & ETNA_BO_CACHED) {
+		if (!etnaviv_obj->sgt) {
+			void *ret;
+
+			mutex_lock(&etnaviv_obj->lock);
+			ret = etnaviv_gem_get_pages(etnaviv_obj);
+			mutex_unlock(&etnaviv_obj->lock);
+			if (IS_ERR(ret))
+				return PTR_ERR(ret);
+		}
+
+		dma_sync_sg_for_cpu(dev->dev, etnaviv_obj->sgt->sgl,
+				    etnaviv_obj->sgt->nents,
+				    etnaviv_op_to_dma_dir(op));
+		etnaviv_obj->last_cpu_prep_op = op;
+	}
+
+	return 0;
+}
+
+int etnaviv_gem_cpu_fini(struct drm_gem_object *obj)
+{
+	struct drm_device *dev = obj->dev;
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+	if (etnaviv_obj->flags & ETNA_BO_CACHED) {
+		/* fini without a prep is almost certainly a userspace error */
+		WARN_ON(etnaviv_obj->last_cpu_prep_op == 0);
+		dma_sync_sg_for_device(dev->dev, etnaviv_obj->sgt->sgl,
+			etnaviv_obj->sgt->nents,
+			etnaviv_op_to_dma_dir(etnaviv_obj->last_cpu_prep_op));
+		etnaviv_obj->last_cpu_prep_op = 0;
+	}
+
+	return 0;
+}
+
+int etnaviv_gem_wait_bo(struct etnaviv_gpu *gpu, struct drm_gem_object *obj,
+	struct timespec *timeout)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+	return etnaviv_gpu_wait_obj_inactive(gpu, etnaviv_obj, timeout);
+}
+
+#ifdef CONFIG_DEBUG_FS
+static void etnaviv_gem_describe_fence(struct fence *fence,
+	const char *type, struct seq_file *m)
+{
+	if (!test_bit(FENCE_FLAG_SIGNALED_BIT, &fence->flags))
+		seq_printf(m, "\t%9s: %s %s seq %u\n",
+			   type,
+			   fence->ops->get_driver_name(fence),
+			   fence->ops->get_timeline_name(fence),
+			   fence->seqno);
+}
+
+static void etnaviv_gem_describe(struct drm_gem_object *obj, struct seq_file *m)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct reservation_object *robj = etnaviv_obj->resv;
+	struct reservation_object_list *fobj;
+	struct fence *fence;
+	unsigned long off = drm_vma_node_start(&obj->vma_node);
+
+	seq_printf(m, "%08x: %c %2d (%2d) %08lx %p %zd\n",
+			etnaviv_obj->flags, is_active(etnaviv_obj) ? 'A' : 'I',
+			obj->name, obj->refcount.refcount.counter,
+			off, etnaviv_obj->vaddr, obj->size);
+
+	rcu_read_lock();
+	fobj = rcu_dereference(robj->fence);
+	if (fobj) {
+		unsigned int i, shared_count = fobj->shared_count;
+
+		for (i = 0; i < shared_count; i++) {
+			fence = rcu_dereference(fobj->shared[i]);
+			etnaviv_gem_describe_fence(fence, "Shared", m);
+		}
+	}
+
+	fence = rcu_dereference(robj->fence_excl);
+	if (fence)
+		etnaviv_gem_describe_fence(fence, "Exclusive", m);
+	rcu_read_unlock();
+}
+
+void etnaviv_gem_describe_objects(struct etnaviv_drm_private *priv,
+	struct seq_file *m)
+{
+	struct etnaviv_gem_object *etnaviv_obj;
+	int count = 0;
+	size_t size = 0;
+
+	mutex_lock(&priv->gem_lock);
+	list_for_each_entry(etnaviv_obj, &priv->gem_list, gem_node) {
+		struct drm_gem_object *obj = &etnaviv_obj->base;
+
+		seq_puts(m, "   ");
+		etnaviv_gem_describe(obj, m);
+		count++;
+		size += obj->size;
+	}
+	mutex_unlock(&priv->gem_lock);
+
+	seq_printf(m, "Total %d objects, %zu bytes\n", count, size);
+}
+#endif
+
+static void etnaviv_gem_shmem_release(struct etnaviv_gem_object *etnaviv_obj)
+{
+	if (etnaviv_obj->vaddr)
+		vunmap(etnaviv_obj->vaddr);
+	put_pages(etnaviv_obj);
+}
+
+static const struct etnaviv_gem_ops etnaviv_gem_shmem_ops = {
+	.get_pages = etnaviv_gem_shmem_get_pages,
+	.release = etnaviv_gem_shmem_release,
+	.vmap = etnaviv_gem_vmap_impl,
+};
+
+void etnaviv_gem_free_object(struct drm_gem_object *obj)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+	struct etnaviv_vram_mapping *mapping, *tmp;
+
+	/* object should not be active */
+	WARN_ON(is_active(etnaviv_obj));
+
+	list_del(&etnaviv_obj->gem_node);
+
+	list_for_each_entry_safe(mapping, tmp, &etnaviv_obj->vram_list,
+				 obj_node) {
+		struct etnaviv_iommu *mmu = mapping->mmu;
+
+		WARN_ON(mapping->use);
+
+		if (mmu)
+			etnaviv_iommu_unmap_gem(mmu, mapping);
+
+		list_del(&mapping->obj_node);
+		kfree(mapping);
+	}
+
+	drm_gem_free_mmap_offset(obj);
+	etnaviv_obj->ops->release(etnaviv_obj);
+	if (etnaviv_obj->resv == &etnaviv_obj->_resv)
+		reservation_object_fini(&etnaviv_obj->_resv);
+	drm_gem_object_release(obj);
+
+	kfree(etnaviv_obj);
+}
+
+int etnaviv_gem_obj_add(struct drm_device *dev, struct drm_gem_object *obj)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+	mutex_lock(&priv->gem_lock);
+	list_add_tail(&etnaviv_obj->gem_node, &priv->gem_list);
+	mutex_unlock(&priv->gem_lock);
+
+	return 0;
+}
+
+static int etnaviv_gem_new_impl(struct drm_device *dev, u32 size, u32 flags,
+	struct reservation_object *robj, const struct etnaviv_gem_ops *ops,
+	struct drm_gem_object **obj)
+{
+	struct etnaviv_gem_object *etnaviv_obj;
+	unsigned sz = sizeof(*etnaviv_obj);
+	bool valid = true;
+
+	/* validate flags */
+	switch (flags & ETNA_BO_CACHE_MASK) {
+	case ETNA_BO_UNCACHED:
+	case ETNA_BO_CACHED:
+	case ETNA_BO_WC:
+		break;
+	default:
+		valid = false;
+	}
+
+	if (!valid) {
+		dev_err(dev->dev, "invalid cache flag: %x\n",
+			(flags & ETNA_BO_CACHE_MASK));
+		return -EINVAL;
+	}
+
+	etnaviv_obj = kzalloc(sz, GFP_KERNEL);
+	if (!etnaviv_obj)
+		return -ENOMEM;
+
+	etnaviv_obj->flags = flags;
+	etnaviv_obj->ops = ops;
+	if (robj) {
+		etnaviv_obj->resv = robj;
+	} else {
+		etnaviv_obj->resv = &etnaviv_obj->_resv;
+		reservation_object_init(&etnaviv_obj->_resv);
+	}
+
+	mutex_init(&etnaviv_obj->lock);
+	INIT_LIST_HEAD(&etnaviv_obj->vram_list);
+
+	*obj = &etnaviv_obj->base;
+
+	return 0;
+}
+
+static struct drm_gem_object *__etnaviv_gem_new(struct drm_device *dev,
+		u32 size, u32 flags)
+{
+	struct drm_gem_object *obj = NULL;
+	int ret;
+
+	size = PAGE_ALIGN(size);
+
+	ret = etnaviv_gem_new_impl(dev, size, flags, NULL,
+				   &etnaviv_gem_shmem_ops, &obj);
+	if (ret)
+		goto fail;
+
+	ret = drm_gem_object_init(dev, obj, size);
+	if (ret == 0) {
+		struct address_space *mapping;
+
+		/*
+		 * Our buffers are kept pinned, so allocating them
+		 * from the MOVABLE zone is a really bad idea, and
+		 * conflicts with CMA.  See coments above new_inode()
+		 * why this is required _and_ expected if you're
+		 * going to pin these pages.
+		 */
+		mapping = file_inode(obj->filp)->i_mapping;
+		mapping_set_gfp_mask(mapping, GFP_HIGHUSER);
+	}
+
+	if (ret)
+		goto fail;
+
+	return obj;
+
+fail:
+	if (obj)
+		drm_gem_object_unreference_unlocked(obj);
+
+	return ERR_PTR(ret);
+}
+
+/* convenience method to construct a GEM buffer object, and userspace handle */
+int etnaviv_gem_new_handle(struct drm_device *dev, struct drm_file *file,
+		u32 size, u32 flags, u32 *handle)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	obj = __etnaviv_gem_new(dev, size, flags);
+	if (IS_ERR(obj))
+		return PTR_ERR(obj);
+
+	ret = etnaviv_gem_obj_add(dev, obj);
+	if (ret < 0) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ret;
+	}
+
+	ret = drm_gem_handle_create(file, obj, handle);
+
+	/* drop reference from allocate - handle holds it now */
+	drm_gem_object_unreference_unlocked(obj);
+
+	return ret;
+}
+
+struct drm_gem_object *etnaviv_gem_new(struct drm_device *dev,
+		u32 size, u32 flags)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	obj = __etnaviv_gem_new(dev, size, flags);
+	if (IS_ERR(obj))
+		return obj;
+
+	ret = etnaviv_gem_obj_add(dev, obj);
+	if (ret < 0) {
+		drm_gem_object_unreference_unlocked(obj);
+		return ERR_PTR(ret);
+	}
+
+	return obj;
+}
+
+int etnaviv_gem_new_private(struct drm_device *dev, size_t size, u32 flags,
+	struct reservation_object *robj, const struct etnaviv_gem_ops *ops,
+	struct etnaviv_gem_object **res)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	ret = etnaviv_gem_new_impl(dev, size, flags, robj, ops, &obj);
+	if (ret)
+		return ret;
+
+	drm_gem_private_object_init(dev, obj, size);
+
+	*res = to_etnaviv_bo(obj);
+
+	return 0;
+}
+
+struct get_pages_work {
+	struct work_struct work;
+	struct mm_struct *mm;
+	struct task_struct *task;
+	struct etnaviv_gem_object *etnaviv_obj;
+};
+
+static struct page **etnaviv_gem_userptr_do_get_pages(
+	struct etnaviv_gem_object *etnaviv_obj, struct mm_struct *mm, struct task_struct *task)
+{
+	int ret = 0, pinned, npages = etnaviv_obj->base.size >> PAGE_SHIFT;
+	struct page **pvec;
+	uintptr_t ptr;
+
+	pvec = drm_malloc_ab(npages, sizeof(struct page *));
+	if (!pvec)
+		return ERR_PTR(-ENOMEM);
+
+	pinned = 0;
+	ptr = etnaviv_obj->userptr.ptr;
+
+	down_read(&mm->mmap_sem);
+	while (pinned < npages) {
+		ret = get_user_pages(task, mm, ptr, npages - pinned,
+				     !etnaviv_obj->userptr.ro, 0,
+				     pvec + pinned, NULL);
+		if (ret < 0)
+			break;
+
+		ptr += ret * PAGE_SIZE;
+		pinned += ret;
+	}
+	up_read(&mm->mmap_sem);
+
+	if (ret < 0) {
+		release_pages(pvec, pinned, 0);
+		drm_free_large(pvec);
+		return ERR_PTR(ret);
+	}
+
+	return pvec;
+}
+
+static void __etnaviv_gem_userptr_get_pages(struct work_struct *_work)
+{
+	struct get_pages_work *work = container_of(_work, typeof(*work), work);
+	struct etnaviv_gem_object *etnaviv_obj = work->etnaviv_obj;
+	struct page **pvec;
+
+	pvec = etnaviv_gem_userptr_do_get_pages(etnaviv_obj, work->mm, work->task);
+
+	mutex_lock(&etnaviv_obj->lock);
+	if (IS_ERR(pvec)) {
+		etnaviv_obj->userptr.work = ERR_CAST(pvec);
+	} else {
+		etnaviv_obj->userptr.work = NULL;
+		etnaviv_obj->pages = pvec;
+	}
+
+	mutex_unlock(&etnaviv_obj->lock);
+	drm_gem_object_unreference_unlocked(&etnaviv_obj->base);
+
+	mmput(work->mm);
+	put_task_struct(work->task);
+	kfree(work);
+}
+
+static int etnaviv_gem_userptr_get_pages(struct etnaviv_gem_object *etnaviv_obj)
+{
+	struct page **pvec = NULL;
+	struct get_pages_work *work;
+	struct mm_struct *mm;
+	int ret, pinned, npages = etnaviv_obj->base.size >> PAGE_SHIFT;
+
+	if (etnaviv_obj->userptr.work) {
+		if (IS_ERR(etnaviv_obj->userptr.work)) {
+			ret = PTR_ERR(etnaviv_obj->userptr.work);
+			etnaviv_obj->userptr.work = NULL;
+		} else {
+			ret = -EAGAIN;
+		}
+		return ret;
+	}
+
+	mm = get_task_mm(etnaviv_obj->userptr.task);
+	pinned = 0;
+	if (mm == current->mm) {
+		pvec = drm_malloc_ab(npages, sizeof(struct page *));
+		if (!pvec) {
+			mmput(mm);
+			return -ENOMEM;
+		}
+
+		pinned = __get_user_pages_fast(etnaviv_obj->userptr.ptr, npages,
+					       !etnaviv_obj->userptr.ro, pvec);
+		if (pinned < 0) {
+			drm_free_large(pvec);
+			mmput(mm);
+			return pinned;
+		}
+
+		if (pinned == npages) {
+			etnaviv_obj->pages = pvec;
+			mmput(mm);
+			return 0;
+		}
+	}
+
+	release_pages(pvec, pinned, 0);
+	drm_free_large(pvec);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work) {
+		mmput(mm);
+		return -ENOMEM;
+	}
+
+	get_task_struct(current);
+	drm_gem_object_reference(&etnaviv_obj->base);
+
+	work->mm = mm;
+	work->task = current;
+	work->etnaviv_obj = etnaviv_obj;
+
+	etnaviv_obj->userptr.work = &work->work;
+	INIT_WORK(&work->work, __etnaviv_gem_userptr_get_pages);
+
+	etnaviv_queue_work(etnaviv_obj->base.dev, &work->work);
+
+	return -EAGAIN;
+}
+
+static void etnaviv_gem_userptr_release(struct etnaviv_gem_object *etnaviv_obj)
+{
+	if (etnaviv_obj->sgt) {
+		etnaviv_gem_scatterlist_unmap(etnaviv_obj);
+		sg_free_table(etnaviv_obj->sgt);
+		kfree(etnaviv_obj->sgt);
+	}
+	if (etnaviv_obj->pages) {
+		int npages = etnaviv_obj->base.size >> PAGE_SHIFT;
+
+		release_pages(etnaviv_obj->pages, npages, 0);
+		drm_free_large(etnaviv_obj->pages);
+	}
+	put_task_struct(etnaviv_obj->userptr.task);
+}
+
+static const struct etnaviv_gem_ops etnaviv_gem_userptr_ops = {
+	.get_pages = etnaviv_gem_userptr_get_pages,
+	.release = etnaviv_gem_userptr_release,
+	.vmap = etnaviv_gem_vmap_impl,
+};
+
+int etnaviv_gem_new_userptr(struct drm_device *dev, struct drm_file *file,
+	uintptr_t ptr, u32 size, u32 flags, u32 *handle)
+{
+	struct etnaviv_gem_object *etnaviv_obj;
+	int ret;
+
+	ret = etnaviv_gem_new_private(dev, size, ETNA_BO_CACHED, NULL,
+				      &etnaviv_gem_userptr_ops, &etnaviv_obj);
+	if (ret)
+		return ret;
+
+	etnaviv_obj->userptr.ptr = ptr;
+	etnaviv_obj->userptr.task = current;
+	etnaviv_obj->userptr.ro = !(flags & ETNA_USERPTR_WRITE);
+	get_task_struct(current);
+
+	ret = etnaviv_gem_obj_add(dev, &etnaviv_obj->base);
+	if (ret) {
+		drm_gem_object_unreference_unlocked(&etnaviv_obj->base);
+		return ret;
+	}
+
+	ret = drm_gem_handle_create(file, &etnaviv_obj->base, handle);
+
+	/* drop reference from allocate - handle holds it now */
+	drm_gem_object_unreference_unlocked(&etnaviv_obj->base);
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem.h b/drivers/gpu/drm/etnaviv/etnaviv_gem.h
new file mode 100644
index 000000000000..ab5df8147a5f
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem.h
@@ -0,0 +1,118 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_GEM_H__
+#define __ETNAVIV_GEM_H__
+
+#include <linux/reservation.h>
+#include "etnaviv_drv.h"
+
+struct etnaviv_gem_ops;
+struct etnaviv_gem_object;
+
+struct etnaviv_gem_userptr {
+	uintptr_t ptr;
+	struct task_struct *task;
+	struct work_struct *work;
+	bool ro;
+};
+
+struct etnaviv_vram_mapping {
+	struct list_head obj_node;
+	struct list_head scan_node;
+	struct list_head mmu_node;
+	struct etnaviv_gem_object *object;
+	struct etnaviv_iommu *mmu;
+	struct drm_mm_node vram_node;
+	unsigned int use;
+	u32 iova;
+};
+
+struct etnaviv_gem_object {
+	struct drm_gem_object base;
+	const struct etnaviv_gem_ops *ops;
+	struct mutex lock;
+
+	u32 flags;
+
+	struct list_head gem_node;
+	struct etnaviv_gpu *gpu;     /* non-null if active */
+	atomic_t gpu_active;
+	u32 access;
+
+	struct page **pages;
+	struct sg_table *sgt;
+	void *vaddr;
+
+	/* normally (resv == &_resv) except for imported bo's */
+	struct reservation_object *resv;
+	struct reservation_object _resv;
+
+	struct list_head vram_list;
+
+	/* cache maintenance */
+	u32 last_cpu_prep_op;
+
+	struct etnaviv_gem_userptr userptr;
+};
+
+static inline
+struct etnaviv_gem_object *to_etnaviv_bo(struct drm_gem_object *obj)
+{
+	return container_of(obj, struct etnaviv_gem_object, base);
+}
+
+struct etnaviv_gem_ops {
+	int (*get_pages)(struct etnaviv_gem_object *);
+	void (*release)(struct etnaviv_gem_object *);
+	void *(*vmap)(struct etnaviv_gem_object *);
+};
+
+static inline bool is_active(struct etnaviv_gem_object *etnaviv_obj)
+{
+	return atomic_read(&etnaviv_obj->gpu_active) != 0;
+}
+
+#define MAX_CMDS 4
+
+/* Created per submit-ioctl, to track bo's and cmdstream bufs, etc,
+ * associated with the cmdstream submission for synchronization (and
+ * make it easier to unwind when things go wrong, etc).  This only
+ * lasts for the duration of the submit-ioctl.
+ */
+struct etnaviv_gem_submit {
+	struct drm_device *dev;
+	struct etnaviv_gpu *gpu;
+	struct ww_acquire_ctx ticket;
+	u32 fence;
+	unsigned int nr_bos;
+	struct {
+		u32 flags;
+		struct etnaviv_gem_object *obj;
+		u32 iova;
+	} bos[0];
+};
+
+int etnaviv_gem_wait_bo(struct etnaviv_gpu *gpu, struct drm_gem_object *obj,
+	struct timespec *timeout);
+int etnaviv_gem_new_private(struct drm_device *dev, size_t size, u32 flags,
+	struct reservation_object *robj, const struct etnaviv_gem_ops *ops,
+	struct etnaviv_gem_object **res);
+int etnaviv_gem_obj_add(struct drm_device *dev, struct drm_gem_object *obj);
+struct page **etnaviv_gem_get_pages(struct etnaviv_gem_object *obj);
+void etnaviv_gem_put_pages(struct etnaviv_gem_object *obj);
+
+#endif /* __ETNAVIV_GEM_H__ */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem_prime.c b/drivers/gpu/drm/etnaviv/etnaviv_gem_prime.c
new file mode 100644
index 000000000000..4e67395f5fa1
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem_prime.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright (C) 2013 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/dma-buf.h>
+#include "etnaviv_drv.h"
+#include "etnaviv_gem.h"
+
+
+struct sg_table *etnaviv_gem_prime_get_sg_table(struct drm_gem_object *obj)
+{
+	struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+	BUG_ON(!etnaviv_obj->sgt);  /* should have already pinned! */
+
+	return etnaviv_obj->sgt;
+}
+
+void *etnaviv_gem_prime_vmap(struct drm_gem_object *obj)
+{
+	return etnaviv_gem_vmap(obj);
+}
+
+void etnaviv_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
+{
+	/* TODO msm_gem_vunmap() */
+}
+
+int etnaviv_gem_prime_pin(struct drm_gem_object *obj)
+{
+	if (!obj->import_attach) {
+		struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+		mutex_lock(&etnaviv_obj->lock);
+		etnaviv_gem_get_pages(etnaviv_obj);
+		mutex_unlock(&etnaviv_obj->lock);
+	}
+	return 0;
+}
+
+void etnaviv_gem_prime_unpin(struct drm_gem_object *obj)
+{
+	if (!obj->import_attach) {
+		struct etnaviv_gem_object *etnaviv_obj = to_etnaviv_bo(obj);
+
+		mutex_lock(&etnaviv_obj->lock);
+		etnaviv_gem_put_pages(to_etnaviv_bo(obj));
+		mutex_unlock(&etnaviv_obj->lock);
+	}
+}
+
+static void etnaviv_gem_prime_release(struct etnaviv_gem_object *etnaviv_obj)
+{
+	if (etnaviv_obj->vaddr)
+		dma_buf_vunmap(etnaviv_obj->base.import_attach->dmabuf,
+			       etnaviv_obj->vaddr);
+
+	/* Don't drop the pages for imported dmabuf, as they are not
+	 * ours, just free the array we allocated:
+	 */
+	if (etnaviv_obj->pages)
+		drm_free_large(etnaviv_obj->pages);
+
+	drm_prime_gem_destroy(&etnaviv_obj->base, etnaviv_obj->sgt);
+}
+
+static void *etnaviv_gem_prime_vmap_impl(struct etnaviv_gem_object *etnaviv_obj)
+{
+	lockdep_assert_held(&etnaviv_obj->lock);
+
+	return dma_buf_vmap(etnaviv_obj->base.import_attach->dmabuf);
+}
+
+static const struct etnaviv_gem_ops etnaviv_gem_prime_ops = {
+	/* .get_pages should never be called */
+	.release = etnaviv_gem_prime_release,
+	.vmap = etnaviv_gem_prime_vmap_impl,
+};
+
+struct drm_gem_object *etnaviv_gem_prime_import_sg_table(struct drm_device *dev,
+	struct dma_buf_attachment *attach, struct sg_table *sgt)
+{
+	struct etnaviv_gem_object *etnaviv_obj;
+	size_t size = PAGE_ALIGN(attach->dmabuf->size);
+	int ret, npages;
+
+	ret = etnaviv_gem_new_private(dev, size, ETNA_BO_WC,
+				      attach->dmabuf->resv,
+				      &etnaviv_gem_prime_ops, &etnaviv_obj);
+	if (ret < 0)
+		return ERR_PTR(ret);
+
+	npages = size / PAGE_SIZE;
+
+	etnaviv_obj->sgt = sgt;
+	etnaviv_obj->pages = drm_malloc_ab(npages, sizeof(struct page *));
+	if (!etnaviv_obj->pages) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	ret = drm_prime_sg_to_page_addr_arrays(sgt, etnaviv_obj->pages,
+					       NULL, npages);
+	if (ret)
+		goto fail;
+
+	ret = etnaviv_gem_obj_add(dev, &etnaviv_obj->base);
+	if (ret)
+		goto fail;
+
+	return &etnaviv_obj->base;
+
+fail:
+	drm_gem_object_unreference_unlocked(&etnaviv_obj->base);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
new file mode 100644
index 000000000000..1aba01a999df
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gem_submit.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/reservation.h>
+#include "etnaviv_drv.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_gem.h"
+
+/*
+ * Cmdstream submission:
+ */
+
+#define BO_INVALID_FLAGS ~(ETNA_SUBMIT_BO_READ | ETNA_SUBMIT_BO_WRITE)
+/* make sure these don't conflict w/ ETNAVIV_SUBMIT_BO_x */
+#define BO_LOCKED   0x4000
+#define BO_PINNED   0x2000
+
+static inline void __user *to_user_ptr(u64 address)
+{
+	return (void __user *)(uintptr_t)address;
+}
+
+static struct etnaviv_gem_submit *submit_create(struct drm_device *dev,
+		struct etnaviv_gpu *gpu, size_t nr)
+{
+	struct etnaviv_gem_submit *submit;
+	size_t sz = size_vstruct(nr, sizeof(submit->bos[0]), sizeof(*submit));
+
+	submit = kmalloc(sz, GFP_TEMPORARY | __GFP_NOWARN | __GFP_NORETRY);
+	if (submit) {
+		submit->dev = dev;
+		submit->gpu = gpu;
+
+		/* initially, until copy_from_user() and bo lookup succeeds: */
+		submit->nr_bos = 0;
+
+		ww_acquire_init(&submit->ticket, &reservation_ww_class);
+	}
+
+	return submit;
+}
+
+static int submit_lookup_objects(struct etnaviv_gem_submit *submit,
+	struct drm_file *file, struct drm_etnaviv_gem_submit_bo *submit_bos,
+	unsigned nr_bos)
+{
+	struct drm_etnaviv_gem_submit_bo *bo;
+	unsigned i;
+	int ret = 0;
+
+	spin_lock(&file->table_lock);
+
+	for (i = 0, bo = submit_bos; i < nr_bos; i++, bo++) {
+		struct drm_gem_object *obj;
+
+		if (bo->flags & BO_INVALID_FLAGS) {
+			DRM_ERROR("invalid flags: %x\n", bo->flags);
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
+		submit->bos[i].flags = bo->flags;
+
+		/* normally use drm_gem_object_lookup(), but for bulk lookup
+		 * all under single table_lock just hit object_idr directly:
+		 */
+		obj = idr_find(&file->object_idr, bo->handle);
+		if (!obj) {
+			DRM_ERROR("invalid handle %u at index %u\n",
+				  bo->handle, i);
+			ret = -EINVAL;
+			goto out_unlock;
+		}
+
+		/*
+		 * Take a refcount on the object. The file table lock
+		 * prevents the object_idr's refcount on this being dropped.
+		 */
+		drm_gem_object_reference(obj);
+
+		submit->bos[i].obj = to_etnaviv_bo(obj);
+	}
+
+out_unlock:
+	submit->nr_bos = i;
+	spin_unlock(&file->table_lock);
+
+	return ret;
+}
+
+static void submit_unlock_object(struct etnaviv_gem_submit *submit, int i)
+{
+	if (submit->bos[i].flags & BO_LOCKED) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+
+		ww_mutex_unlock(&etnaviv_obj->resv->lock);
+		submit->bos[i].flags &= ~BO_LOCKED;
+	}
+}
+
+static int submit_lock_objects(struct etnaviv_gem_submit *submit)
+{
+	int contended, slow_locked = -1, i, ret = 0;
+
+retry:
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+
+		if (slow_locked == i)
+			slow_locked = -1;
+
+		contended = i;
+
+		if (!(submit->bos[i].flags & BO_LOCKED)) {
+			ret = ww_mutex_lock_interruptible(&etnaviv_obj->resv->lock,
+					&submit->ticket);
+			if (ret == -EALREADY)
+				DRM_ERROR("BO at index %u already on submit list\n",
+					  i);
+			if (ret)
+				goto fail;
+			submit->bos[i].flags |= BO_LOCKED;
+		}
+	}
+
+	ww_acquire_done(&submit->ticket);
+
+	return 0;
+
+fail:
+	for (; i >= 0; i--)
+		submit_unlock_object(submit, i);
+
+	if (slow_locked > 0)
+		submit_unlock_object(submit, slow_locked);
+
+	if (ret == -EDEADLK) {
+		struct etnaviv_gem_object *etnaviv_obj;
+
+		etnaviv_obj = submit->bos[contended].obj;
+
+		/* we lost out in a seqno race, lock and retry.. */
+		ret = ww_mutex_lock_slow_interruptible(&etnaviv_obj->resv->lock,
+				&submit->ticket);
+		if (!ret) {
+			submit->bos[contended].flags |= BO_LOCKED;
+			slow_locked = contended;
+			goto retry;
+		}
+	}
+
+	return ret;
+}
+
+static int submit_fence_sync(const struct etnaviv_gem_submit *submit)
+{
+	unsigned int context = submit->gpu->fence_context;
+	int i, ret = 0;
+
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+		bool write = submit->bos[i].flags & ETNA_SUBMIT_BO_WRITE;
+
+		ret = etnaviv_gpu_fence_sync_obj(etnaviv_obj, context, write);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static void submit_unpin_objects(struct etnaviv_gem_submit *submit)
+{
+	int i;
+
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+
+		if (submit->bos[i].flags & BO_PINNED)
+			etnaviv_gem_put_iova(submit->gpu, &etnaviv_obj->base);
+
+		submit->bos[i].iova = 0;
+		submit->bos[i].flags &= ~BO_PINNED;
+	}
+}
+
+static int submit_pin_objects(struct etnaviv_gem_submit *submit)
+{
+	int i, ret = 0;
+
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+		u32 iova;
+
+		ret = etnaviv_gem_get_iova(submit->gpu, &etnaviv_obj->base,
+					   &iova);
+		if (ret)
+			break;
+
+		submit->bos[i].flags |= BO_PINNED;
+		submit->bos[i].iova = iova;
+	}
+
+	return ret;
+}
+
+static int submit_bo(struct etnaviv_gem_submit *submit, u32 idx,
+		struct etnaviv_gem_object **obj, u32 *iova)
+{
+	if (idx >= submit->nr_bos) {
+		DRM_ERROR("invalid buffer index: %u (out of %u)\n",
+				idx, submit->nr_bos);
+		return -EINVAL;
+	}
+
+	if (obj)
+		*obj = submit->bos[idx].obj;
+	if (iova)
+		*iova = submit->bos[idx].iova;
+
+	return 0;
+}
+
+/* process the reloc's and patch up the cmdstream as needed: */
+static int submit_reloc(struct etnaviv_gem_submit *submit, void *stream,
+		u32 size, const struct drm_etnaviv_gem_submit_reloc *relocs,
+		u32 nr_relocs)
+{
+	u32 i, last_offset = 0;
+	u32 *ptr = stream;
+	int ret;
+
+	for (i = 0; i < nr_relocs; i++) {
+		const struct drm_etnaviv_gem_submit_reloc *r = relocs + i;
+		struct etnaviv_gem_object *bobj;
+		u32 iova, off;
+
+		if (unlikely(r->flags)) {
+			DRM_ERROR("invalid reloc flags\n");
+			return -EINVAL;
+		}
+
+		if (r->submit_offset % 4) {
+			DRM_ERROR("non-aligned reloc offset: %u\n",
+				  r->submit_offset);
+			return -EINVAL;
+		}
+
+		/* offset in dwords: */
+		off = r->submit_offset / 4;
+
+		if ((off >= size ) ||
+				(off < last_offset)) {
+			DRM_ERROR("invalid offset %u at reloc %u\n", off, i);
+			return -EINVAL;
+		}
+
+		ret = submit_bo(submit, r->reloc_idx, &bobj, &iova);
+		if (ret)
+			return ret;
+
+		if (r->reloc_offset >=
+		    bobj->base.size - sizeof(*ptr)) {
+			DRM_ERROR("relocation %u outside object", i);
+			return -EINVAL;
+		}
+
+		ptr[off] = iova + r->reloc_offset;
+
+		last_offset = off;
+	}
+
+	return 0;
+}
+
+static void submit_cleanup(struct etnaviv_gem_submit *submit)
+{
+	unsigned i;
+
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+
+		submit_unlock_object(submit, i);
+		drm_gem_object_unreference_unlocked(&etnaviv_obj->base);
+	}
+
+	ww_acquire_fini(&submit->ticket);
+	kfree(submit);
+}
+
+int etnaviv_ioctl_gem_submit(struct drm_device *dev, void *data,
+		struct drm_file *file)
+{
+	struct etnaviv_drm_private *priv = dev->dev_private;
+	struct drm_etnaviv_gem_submit *args = data;
+	struct drm_etnaviv_gem_submit_reloc *relocs;
+	struct drm_etnaviv_gem_submit_bo *bos;
+	struct etnaviv_gem_submit *submit;
+	struct etnaviv_cmdbuf *cmdbuf;
+	struct etnaviv_gpu *gpu;
+	void *stream;
+	int ret;
+
+	if (args->pipe >= ETNA_MAX_PIPES)
+		return -EINVAL;
+
+	gpu = priv->gpu[args->pipe];
+	if (!gpu)
+		return -ENXIO;
+
+	if (args->stream_size % 4) {
+		DRM_ERROR("non-aligned cmdstream buffer size: %u\n",
+			  args->stream_size);
+		return -EINVAL;
+	}
+
+	if (args->exec_state != ETNA_PIPE_3D &&
+	    args->exec_state != ETNA_PIPE_2D &&
+	    args->exec_state != ETNA_PIPE_VG) {
+		DRM_ERROR("invalid exec_state: 0x%x\n", args->exec_state);
+		return -EINVAL;
+	}
+
+	/*
+	 * Copy the command submission and bo array to kernel space in
+	 * one go, and do this outside of any locks.
+	 */
+	bos = drm_malloc_ab(args->nr_bos, sizeof(*bos));
+	relocs = drm_malloc_ab(args->nr_relocs, sizeof(*relocs));
+	stream = drm_malloc_ab(1, args->stream_size);
+	cmdbuf = etnaviv_gpu_cmdbuf_new(gpu, ALIGN(args->stream_size, 8) + 8,
+					args->nr_bos);
+	if (!bos || !relocs || !stream || !cmdbuf) {
+		ret = -ENOMEM;
+		goto err_submit_cmds;
+	}
+
+	cmdbuf->exec_state = args->exec_state;
+	cmdbuf->ctx = file->driver_priv;
+
+	ret = copy_from_user(bos, to_user_ptr(args->bos),
+			     args->nr_bos * sizeof(*bos));
+	if (ret) {
+		ret = -EFAULT;
+		goto err_submit_cmds;
+	}
+
+	ret = copy_from_user(relocs, to_user_ptr(args->relocs),
+			     args->nr_relocs * sizeof(*relocs));
+	if (ret) {
+		ret = -EFAULT;
+		goto err_submit_cmds;
+	}
+
+	ret = copy_from_user(stream, to_user_ptr(args->stream),
+			     args->stream_size);
+	if (ret) {
+		ret = -EFAULT;
+		goto err_submit_cmds;
+	}
+
+	submit = submit_create(dev, gpu, args->nr_bos);
+	if (!submit) {
+		ret = -ENOMEM;
+		goto err_submit_cmds;
+	}
+
+	ret = submit_lookup_objects(submit, file, bos, args->nr_bos);
+	if (ret)
+		goto err_submit_objects;
+
+	ret = submit_lock_objects(submit);
+	if (ret)
+		goto err_submit_objects;
+
+	if (!etnaviv_cmd_validate_one(gpu, stream, args->stream_size / 4,
+				      relocs, args->nr_relocs)) {
+		ret = -EINVAL;
+		goto err_submit_objects;
+	}
+
+	ret = submit_fence_sync(submit);
+	if (ret)
+		goto err_submit_objects;
+
+	ret = submit_pin_objects(submit);
+	if (ret)
+		goto out;
+
+	ret = submit_reloc(submit, stream, args->stream_size / 4,
+			   relocs, args->nr_relocs);
+	if (ret)
+		goto out;
+
+	memcpy(cmdbuf->vaddr, stream, args->stream_size);
+	cmdbuf->user_size = ALIGN(args->stream_size, 8);
+
+	ret = etnaviv_gpu_submit(gpu, submit, cmdbuf);
+	if (ret == 0)
+		cmdbuf = NULL;
+
+	args->fence = submit->fence;
+
+out:
+	submit_unpin_objects(submit);
+
+	/*
+	 * If we're returning -EAGAIN, it may be due to the userptr code
+	 * wanting to run its workqueue outside of any locks. Flush our
+	 * workqueue to ensure that it is run in a timely manner.
+	 */
+	if (ret == -EAGAIN)
+		flush_workqueue(priv->wq);
+
+err_submit_objects:
+	submit_cleanup(submit);
+
+err_submit_cmds:
+	/* if we still own the cmdbuf */
+	if (cmdbuf)
+		etnaviv_gpu_cmdbuf_free(cmdbuf);
+	if (stream)
+		drm_free_large(stream);
+	if (bos)
+		drm_free_large(bos);
+	if (relocs)
+		drm_free_large(relocs);
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gpu.c b/drivers/gpu/drm/etnaviv/etnaviv_gpu.c
new file mode 100644
index 000000000000..a33162cf4f4c
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gpu.c
@@ -0,0 +1,1712 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/component.h>
+#include <linux/fence.h>
+#include <linux/moduleparam.h>
+#include <linux/of_device.h>
+#include "etnaviv_dump.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_mmu.h"
+#include "etnaviv_iommu.h"
+#include "etnaviv_iommu_v2.h"
+#include "common.xml.h"
+#include "state.xml.h"
+#include "state_hi.xml.h"
+#include "cmdstream.xml.h"
+
+static const struct platform_device_id gpu_ids[] = {
+	{ .name = "etnaviv-gpu,2d" },
+	{ },
+};
+
+static bool etnaviv_dump_core = true;
+module_param_named(dump_core, etnaviv_dump_core, bool, 0600);
+
+/*
+ * Driver functions:
+ */
+
+int etnaviv_gpu_get_param(struct etnaviv_gpu *gpu, u32 param, u64 *value)
+{
+	switch (param) {
+	case ETNAVIV_PARAM_GPU_MODEL:
+		*value = gpu->identity.model;
+		break;
+
+	case ETNAVIV_PARAM_GPU_REVISION:
+		*value = gpu->identity.revision;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_0:
+		*value = gpu->identity.features;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_1:
+		*value = gpu->identity.minor_features0;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_2:
+		*value = gpu->identity.minor_features1;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_3:
+		*value = gpu->identity.minor_features2;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_4:
+		*value = gpu->identity.minor_features3;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_5:
+		*value = gpu->identity.minor_features4;
+		break;
+
+	case ETNAVIV_PARAM_GPU_FEATURES_6:
+		*value = gpu->identity.minor_features5;
+		break;
+
+	case ETNAVIV_PARAM_GPU_STREAM_COUNT:
+		*value = gpu->identity.stream_count;
+		break;
+
+	case ETNAVIV_PARAM_GPU_REGISTER_MAX:
+		*value = gpu->identity.register_max;
+		break;
+
+	case ETNAVIV_PARAM_GPU_THREAD_COUNT:
+		*value = gpu->identity.thread_count;
+		break;
+
+	case ETNAVIV_PARAM_GPU_VERTEX_CACHE_SIZE:
+		*value = gpu->identity.vertex_cache_size;
+		break;
+
+	case ETNAVIV_PARAM_GPU_SHADER_CORE_COUNT:
+		*value = gpu->identity.shader_core_count;
+		break;
+
+	case ETNAVIV_PARAM_GPU_PIXEL_PIPES:
+		*value = gpu->identity.pixel_pipes;
+		break;
+
+	case ETNAVIV_PARAM_GPU_VERTEX_OUTPUT_BUFFER_SIZE:
+		*value = gpu->identity.vertex_output_buffer_size;
+		break;
+
+	case ETNAVIV_PARAM_GPU_BUFFER_SIZE:
+		*value = gpu->identity.buffer_size;
+		break;
+
+	case ETNAVIV_PARAM_GPU_INSTRUCTION_COUNT:
+		*value = gpu->identity.instruction_count;
+		break;
+
+	case ETNAVIV_PARAM_GPU_NUM_CONSTANTS:
+		*value = gpu->identity.num_constants;
+		break;
+
+	case ETNAVIV_PARAM_GPU_NUM_VARYINGS:
+		*value = gpu->identity.varyings_count;
+		break;
+
+	default:
+		DBG("%s: invalid param: %u", dev_name(gpu->dev), param);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+#define etnaviv_is_model_rev(gpu, mod, rev) \
+	((gpu)->identity.model == chipModel_##mod && \
+	 (gpu)->identity.revision == rev)
+#define etnaviv_field(val, field) \
+	(((val) & field##__MASK) >> field##__SHIFT)
+
+static void etnaviv_hw_specs(struct etnaviv_gpu *gpu)
+{
+	if (gpu->identity.minor_features0 &
+	    chipMinorFeatures0_MORE_MINOR_FEATURES) {
+		u32 specs[4];
+		unsigned int streams;
+
+		specs[0] = gpu_read(gpu, VIVS_HI_CHIP_SPECS);
+		specs[1] = gpu_read(gpu, VIVS_HI_CHIP_SPECS_2);
+		specs[2] = gpu_read(gpu, VIVS_HI_CHIP_SPECS_3);
+		specs[3] = gpu_read(gpu, VIVS_HI_CHIP_SPECS_4);
+
+		gpu->identity.stream_count = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_STREAM_COUNT);
+		gpu->identity.register_max = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_REGISTER_MAX);
+		gpu->identity.thread_count = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_THREAD_COUNT);
+		gpu->identity.vertex_cache_size = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE);
+		gpu->identity.shader_core_count = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT);
+		gpu->identity.pixel_pipes = etnaviv_field(specs[0],
+					VIVS_HI_CHIP_SPECS_PIXEL_PIPES);
+		gpu->identity.vertex_output_buffer_size =
+			etnaviv_field(specs[0],
+				VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE);
+
+		gpu->identity.buffer_size = etnaviv_field(specs[1],
+					VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE);
+		gpu->identity.instruction_count = etnaviv_field(specs[1],
+					VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT);
+		gpu->identity.num_constants = etnaviv_field(specs[1],
+					VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS);
+
+		gpu->identity.varyings_count = etnaviv_field(specs[2],
+					VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT);
+
+		/* This overrides the value from older register if non-zero */
+		streams = etnaviv_field(specs[3],
+					VIVS_HI_CHIP_SPECS_4_STREAM_COUNT);
+		if (streams)
+			gpu->identity.stream_count = streams;
+	}
+
+	/* Fill in the stream count if not specified */
+	if (gpu->identity.stream_count == 0) {
+		if (gpu->identity.model >= 0x1000)
+			gpu->identity.stream_count = 4;
+		else
+			gpu->identity.stream_count = 1;
+	}
+
+	/* Convert the register max value */
+	if (gpu->identity.register_max)
+		gpu->identity.register_max = 1 << gpu->identity.register_max;
+	else if (gpu->identity.model == chipModel_GC400)
+		gpu->identity.register_max = 32;
+	else
+		gpu->identity.register_max = 64;
+
+	/* Convert thread count */
+	if (gpu->identity.thread_count)
+		gpu->identity.thread_count = 1 << gpu->identity.thread_count;
+	else if (gpu->identity.model == chipModel_GC400)
+		gpu->identity.thread_count = 64;
+	else if (gpu->identity.model == chipModel_GC500 ||
+		 gpu->identity.model == chipModel_GC530)
+		gpu->identity.thread_count = 128;
+	else
+		gpu->identity.thread_count = 256;
+
+	if (gpu->identity.vertex_cache_size == 0)
+		gpu->identity.vertex_cache_size = 8;
+
+	if (gpu->identity.shader_core_count == 0) {
+		if (gpu->identity.model >= 0x1000)
+			gpu->identity.shader_core_count = 2;
+		else
+			gpu->identity.shader_core_count = 1;
+	}
+
+	if (gpu->identity.pixel_pipes == 0)
+		gpu->identity.pixel_pipes = 1;
+
+	/* Convert virtex buffer size */
+	if (gpu->identity.vertex_output_buffer_size) {
+		gpu->identity.vertex_output_buffer_size =
+			1 << gpu->identity.vertex_output_buffer_size;
+	} else if (gpu->identity.model == chipModel_GC400) {
+		if (gpu->identity.revision < 0x4000)
+			gpu->identity.vertex_output_buffer_size = 512;
+		else if (gpu->identity.revision < 0x4200)
+			gpu->identity.vertex_output_buffer_size = 256;
+		else
+			gpu->identity.vertex_output_buffer_size = 128;
+	} else {
+		gpu->identity.vertex_output_buffer_size = 512;
+	}
+
+	switch (gpu->identity.instruction_count) {
+	case 0:
+		if (etnaviv_is_model_rev(gpu, GC2000, 0x5108) ||
+		    gpu->identity.model == chipModel_GC880)
+			gpu->identity.instruction_count = 512;
+		else
+			gpu->identity.instruction_count = 256;
+		break;
+
+	case 1:
+		gpu->identity.instruction_count = 1024;
+		break;
+
+	case 2:
+		gpu->identity.instruction_count = 2048;
+		break;
+
+	default:
+		gpu->identity.instruction_count = 256;
+		break;
+	}
+
+	if (gpu->identity.num_constants == 0)
+		gpu->identity.num_constants = 168;
+
+	if (gpu->identity.varyings_count == 0) {
+		if (gpu->identity.minor_features1 & chipMinorFeatures1_HALTI0)
+			gpu->identity.varyings_count = 12;
+		else
+			gpu->identity.varyings_count = 8;
+	}
+
+	/*
+	 * For some cores, two varyings are consumed for position, so the
+	 * maximum varying count needs to be reduced by one.
+	 */
+	if (etnaviv_is_model_rev(gpu, GC5000, 0x5434) ||
+	    etnaviv_is_model_rev(gpu, GC4000, 0x5222) ||
+	    etnaviv_is_model_rev(gpu, GC4000, 0x5245) ||
+	    etnaviv_is_model_rev(gpu, GC4000, 0x5208) ||
+	    etnaviv_is_model_rev(gpu, GC3000, 0x5435) ||
+	    etnaviv_is_model_rev(gpu, GC2200, 0x5244) ||
+	    etnaviv_is_model_rev(gpu, GC2100, 0x5108) ||
+	    etnaviv_is_model_rev(gpu, GC2000, 0x5108) ||
+	    etnaviv_is_model_rev(gpu, GC1500, 0x5246) ||
+	    etnaviv_is_model_rev(gpu, GC880, 0x5107) ||
+	    etnaviv_is_model_rev(gpu, GC880, 0x5106))
+		gpu->identity.varyings_count -= 1;
+}
+
+static void etnaviv_hw_identify(struct etnaviv_gpu *gpu)
+{
+	u32 chipIdentity;
+
+	chipIdentity = gpu_read(gpu, VIVS_HI_CHIP_IDENTITY);
+
+	/* Special case for older graphic cores. */
+	if (etnaviv_field(chipIdentity, VIVS_HI_CHIP_IDENTITY_FAMILY) == 0x01) {
+		gpu->identity.model    = chipModel_GC500;
+		gpu->identity.revision = etnaviv_field(chipIdentity,
+					 VIVS_HI_CHIP_IDENTITY_REVISION);
+	} else {
+
+		gpu->identity.model = gpu_read(gpu, VIVS_HI_CHIP_MODEL);
+		gpu->identity.revision = gpu_read(gpu, VIVS_HI_CHIP_REV);
+
+		/*
+		 * !!!! HACK ALERT !!!!
+		 * Because people change device IDs without letting software
+		 * know about it - here is the hack to make it all look the
+		 * same.  Only for GC400 family.
+		 */
+		if ((gpu->identity.model & 0xff00) == 0x0400 &&
+		    gpu->identity.model != chipModel_GC420) {
+			gpu->identity.model = gpu->identity.model & 0x0400;
+		}
+
+		/* Another special case */
+		if (etnaviv_is_model_rev(gpu, GC300, 0x2201)) {
+			u32 chipDate = gpu_read(gpu, VIVS_HI_CHIP_DATE);
+			u32 chipTime = gpu_read(gpu, VIVS_HI_CHIP_TIME);
+
+			if (chipDate == 0x20080814 && chipTime == 0x12051100) {
+				/*
+				 * This IP has an ECO; put the correct
+				 * revision in it.
+				 */
+				gpu->identity.revision = 0x1051;
+			}
+		}
+	}
+
+	dev_info(gpu->dev, "model: GC%x, revision: %x\n",
+		 gpu->identity.model, gpu->identity.revision);
+
+	gpu->identity.features = gpu_read(gpu, VIVS_HI_CHIP_FEATURE);
+
+	/* Disable fast clear on GC700. */
+	if (gpu->identity.model == chipModel_GC700)
+		gpu->identity.features &= ~chipFeatures_FAST_CLEAR;
+
+	if ((gpu->identity.model == chipModel_GC500 &&
+	     gpu->identity.revision < 2) ||
+	    (gpu->identity.model == chipModel_GC300 &&
+	     gpu->identity.revision < 0x2000)) {
+
+		/*
+		 * GC500 rev 1.x and GC300 rev < 2.0 doesn't have these
+		 * registers.
+		 */
+		gpu->identity.minor_features0 = 0;
+		gpu->identity.minor_features1 = 0;
+		gpu->identity.minor_features2 = 0;
+		gpu->identity.minor_features3 = 0;
+		gpu->identity.minor_features4 = 0;
+		gpu->identity.minor_features5 = 0;
+	} else
+		gpu->identity.minor_features0 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_0);
+
+	if (gpu->identity.minor_features0 &
+	    chipMinorFeatures0_MORE_MINOR_FEATURES) {
+		gpu->identity.minor_features1 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_1);
+		gpu->identity.minor_features2 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_2);
+		gpu->identity.minor_features3 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_3);
+		gpu->identity.minor_features4 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_4);
+		gpu->identity.minor_features5 =
+				gpu_read(gpu, VIVS_HI_CHIP_MINOR_FEATURE_5);
+	}
+
+	/* GC600 idle register reports zero bits where modules aren't present */
+	if (gpu->identity.model == chipModel_GC600) {
+		gpu->idle_mask = VIVS_HI_IDLE_STATE_TX |
+				 VIVS_HI_IDLE_STATE_RA |
+				 VIVS_HI_IDLE_STATE_SE |
+				 VIVS_HI_IDLE_STATE_PA |
+				 VIVS_HI_IDLE_STATE_SH |
+				 VIVS_HI_IDLE_STATE_PE |
+				 VIVS_HI_IDLE_STATE_DE |
+				 VIVS_HI_IDLE_STATE_FE;
+	} else {
+		gpu->idle_mask = ~VIVS_HI_IDLE_STATE_AXI_LP;
+	}
+
+	etnaviv_hw_specs(gpu);
+}
+
+static void etnaviv_gpu_load_clock(struct etnaviv_gpu *gpu, u32 clock)
+{
+	gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, clock |
+		  VIVS_HI_CLOCK_CONTROL_FSCALE_CMD_LOAD);
+	gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, clock);
+}
+
+static int etnaviv_hw_reset(struct etnaviv_gpu *gpu)
+{
+	u32 control, idle;
+	unsigned long timeout;
+	bool failed = true;
+
+	/* TODO
+	 *
+	 * - clock gating
+	 * - puls eater
+	 * - what about VG?
+	 */
+
+	/* We hope that the GPU resets in under one second */
+	timeout = jiffies + msecs_to_jiffies(1000);
+
+	while (time_is_after_jiffies(timeout)) {
+		control = VIVS_HI_CLOCK_CONTROL_DISABLE_DEBUG_REGISTERS |
+			  VIVS_HI_CLOCK_CONTROL_FSCALE_VAL(0x40);
+
+		/* enable clock */
+		etnaviv_gpu_load_clock(gpu, control);
+
+		/* Wait for stable clock.  Vivante's code waited for 1ms */
+		usleep_range(1000, 10000);
+
+		/* isolate the GPU. */
+		control |= VIVS_HI_CLOCK_CONTROL_ISOLATE_GPU;
+		gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, control);
+
+		/* set soft reset. */
+		control |= VIVS_HI_CLOCK_CONTROL_SOFT_RESET;
+		gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, control);
+
+		/* wait for reset. */
+		msleep(1);
+
+		/* reset soft reset bit. */
+		control &= ~VIVS_HI_CLOCK_CONTROL_SOFT_RESET;
+		gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, control);
+
+		/* reset GPU isolation. */
+		control &= ~VIVS_HI_CLOCK_CONTROL_ISOLATE_GPU;
+		gpu_write(gpu, VIVS_HI_CLOCK_CONTROL, control);
+
+		/* read idle register. */
+		idle = gpu_read(gpu, VIVS_HI_IDLE_STATE);
+
+		/* try reseting again if FE it not idle */
+		if ((idle & VIVS_HI_IDLE_STATE_FE) == 0) {
+			dev_dbg(gpu->dev, "FE is not idle\n");
+			continue;
+		}
+
+		/* read reset register. */
+		control = gpu_read(gpu, VIVS_HI_CLOCK_CONTROL);
+
+		/* is the GPU idle? */
+		if (((control & VIVS_HI_CLOCK_CONTROL_IDLE_3D) == 0) ||
+		    ((control & VIVS_HI_CLOCK_CONTROL_IDLE_2D) == 0)) {
+			dev_dbg(gpu->dev, "GPU is not idle\n");
+			continue;
+		}
+
+		failed = false;
+		break;
+	}
+
+	if (failed) {
+		idle = gpu_read(gpu, VIVS_HI_IDLE_STATE);
+		control = gpu_read(gpu, VIVS_HI_CLOCK_CONTROL);
+
+		dev_err(gpu->dev, "GPU failed to reset: FE %sidle, 3D %sidle, 2D %sidle\n",
+			idle & VIVS_HI_IDLE_STATE_FE ? "" : "not ",
+			control & VIVS_HI_CLOCK_CONTROL_IDLE_3D ? "" : "not ",
+			control & VIVS_HI_CLOCK_CONTROL_IDLE_2D ? "" : "not ");
+
+		return -EBUSY;
+	}
+
+	/* We rely on the GPU running, so program the clock */
+	control = VIVS_HI_CLOCK_CONTROL_DISABLE_DEBUG_REGISTERS |
+		  VIVS_HI_CLOCK_CONTROL_FSCALE_VAL(0x40);
+
+	/* enable clock */
+	etnaviv_gpu_load_clock(gpu, control);
+
+	return 0;
+}
+
+static void etnaviv_gpu_hw_init(struct etnaviv_gpu *gpu)
+{
+	u16 prefetch;
+
+	if ((etnaviv_is_model_rev(gpu, GC320, 0x5007) ||
+	     etnaviv_is_model_rev(gpu, GC320, 0x5220)) &&
+	    gpu_read(gpu, VIVS_HI_CHIP_TIME) != 0x2062400) {
+		u32 mc_memory_debug;
+
+		mc_memory_debug = gpu_read(gpu, VIVS_MC_DEBUG_MEMORY) & ~0xff;
+
+		if (gpu->identity.revision == 0x5007)
+			mc_memory_debug |= 0x0c;
+		else
+			mc_memory_debug |= 0x08;
+
+		gpu_write(gpu, VIVS_MC_DEBUG_MEMORY, mc_memory_debug);
+	}
+
+	/*
+	 * Update GPU AXI cache atttribute to "cacheable, no allocate".
+	 * This is necessary to prevent the iMX6 SoC locking up.
+	 */
+	gpu_write(gpu, VIVS_HI_AXI_CONFIG,
+		  VIVS_HI_AXI_CONFIG_AWCACHE(2) |
+		  VIVS_HI_AXI_CONFIG_ARCACHE(2));
+
+	/* GC2000 rev 5108 needs a special bus config */
+	if (etnaviv_is_model_rev(gpu, GC2000, 0x5108)) {
+		u32 bus_config = gpu_read(gpu, VIVS_MC_BUS_CONFIG);
+		bus_config &= ~(VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG__MASK |
+				VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG__MASK);
+		bus_config |= VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG(1) |
+			      VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG(0);
+		gpu_write(gpu, VIVS_MC_BUS_CONFIG, bus_config);
+	}
+
+	/* set base addresses */
+	gpu_write(gpu, VIVS_MC_MEMORY_BASE_ADDR_RA, gpu->memory_base);
+	gpu_write(gpu, VIVS_MC_MEMORY_BASE_ADDR_FE, gpu->memory_base);
+	gpu_write(gpu, VIVS_MC_MEMORY_BASE_ADDR_TX, gpu->memory_base);
+	gpu_write(gpu, VIVS_MC_MEMORY_BASE_ADDR_PEZ, gpu->memory_base);
+	gpu_write(gpu, VIVS_MC_MEMORY_BASE_ADDR_PE, gpu->memory_base);
+
+	/* setup the MMU page table pointers */
+	etnaviv_iommu_domain_restore(gpu, gpu->mmu->domain);
+
+	/* Start command processor */
+	prefetch = etnaviv_buffer_init(gpu);
+
+	gpu_write(gpu, VIVS_HI_INTR_ENBL, ~0U);
+	gpu_write(gpu, VIVS_FE_COMMAND_ADDRESS,
+		  gpu->buffer->paddr - gpu->memory_base);
+	gpu_write(gpu, VIVS_FE_COMMAND_CONTROL,
+		  VIVS_FE_COMMAND_CONTROL_ENABLE |
+		  VIVS_FE_COMMAND_CONTROL_PREFETCH(prefetch));
+}
+
+int etnaviv_gpu_init(struct etnaviv_gpu *gpu)
+{
+	int ret, i;
+	struct iommu_domain *iommu;
+	enum etnaviv_iommu_version version;
+	bool mmuv2;
+
+	ret = pm_runtime_get_sync(gpu->dev);
+	if (ret < 0)
+		return ret;
+
+	etnaviv_hw_identify(gpu);
+
+	if (gpu->identity.model == 0) {
+		dev_err(gpu->dev, "Unknown GPU model\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	/* Exclude VG cores with FE2.0 */
+	if (gpu->identity.features & chipFeatures_PIPE_VG &&
+	    gpu->identity.features & chipFeatures_FE20) {
+		dev_info(gpu->dev, "Ignoring GPU with VG and FE2.0\n");
+		ret = -ENXIO;
+		goto fail;
+	}
+
+	ret = etnaviv_hw_reset(gpu);
+	if (ret)
+		goto fail;
+
+	/* Setup IOMMU.. eventually we will (I think) do this once per context
+	 * and have separate page tables per context.  For now, to keep things
+	 * simple and to get something working, just use a single address space:
+	 */
+	mmuv2 = gpu->identity.minor_features1 & chipMinorFeatures1_MMU_VERSION;
+	dev_dbg(gpu->dev, "mmuv2: %d\n", mmuv2);
+
+	if (!mmuv2) {
+		iommu = etnaviv_iommu_domain_alloc(gpu);
+		version = ETNAVIV_IOMMU_V1;
+	} else {
+		iommu = etnaviv_iommu_v2_domain_alloc(gpu);
+		version = ETNAVIV_IOMMU_V2;
+	}
+
+	if (!iommu) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	gpu->mmu = etnaviv_iommu_new(gpu, iommu, version);
+	if (!gpu->mmu) {
+		iommu_domain_free(iommu);
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	/* Create buffer: */
+	gpu->buffer = etnaviv_gpu_cmdbuf_new(gpu, PAGE_SIZE, 0);
+	if (!gpu->buffer) {
+		ret = -ENOMEM;
+		dev_err(gpu->dev, "could not create command buffer\n");
+		goto destroy_iommu;
+	}
+	if (gpu->buffer->paddr - gpu->memory_base > 0x80000000) {
+		ret = -EINVAL;
+		dev_err(gpu->dev,
+			"command buffer outside valid memory window\n");
+		goto free_buffer;
+	}
+
+	/* Setup event management */
+	spin_lock_init(&gpu->event_spinlock);
+	init_completion(&gpu->event_free);
+	for (i = 0; i < ARRAY_SIZE(gpu->event); i++) {
+		gpu->event[i].used = false;
+		complete(&gpu->event_free);
+	}
+
+	/* Now program the hardware */
+	mutex_lock(&gpu->lock);
+	etnaviv_gpu_hw_init(gpu);
+	mutex_unlock(&gpu->lock);
+
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+
+	return 0;
+
+free_buffer:
+	etnaviv_gpu_cmdbuf_free(gpu->buffer);
+	gpu->buffer = NULL;
+destroy_iommu:
+	etnaviv_iommu_destroy(gpu->mmu);
+	gpu->mmu = NULL;
+fail:
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+
+	return ret;
+}
+
+#ifdef CONFIG_DEBUG_FS
+struct dma_debug {
+	u32 address[2];
+	u32 state[2];
+};
+
+static void verify_dma(struct etnaviv_gpu *gpu, struct dma_debug *debug)
+{
+	u32 i;
+
+	debug->address[0] = gpu_read(gpu, VIVS_FE_DMA_ADDRESS);
+	debug->state[0]   = gpu_read(gpu, VIVS_FE_DMA_DEBUG_STATE);
+
+	for (i = 0; i < 500; i++) {
+		debug->address[1] = gpu_read(gpu, VIVS_FE_DMA_ADDRESS);
+		debug->state[1]   = gpu_read(gpu, VIVS_FE_DMA_DEBUG_STATE);
+
+		if (debug->address[0] != debug->address[1])
+			break;
+
+		if (debug->state[0] != debug->state[1])
+			break;
+	}
+}
+
+int etnaviv_gpu_debugfs(struct etnaviv_gpu *gpu, struct seq_file *m)
+{
+	struct dma_debug debug;
+	u32 dma_lo, dma_hi, axi, idle;
+	int ret;
+
+	seq_printf(m, "%s Status:\n", dev_name(gpu->dev));
+
+	ret = pm_runtime_get_sync(gpu->dev);
+	if (ret < 0)
+		return ret;
+
+	dma_lo = gpu_read(gpu, VIVS_FE_DMA_LOW);
+	dma_hi = gpu_read(gpu, VIVS_FE_DMA_HIGH);
+	axi = gpu_read(gpu, VIVS_HI_AXI_STATUS);
+	idle = gpu_read(gpu, VIVS_HI_IDLE_STATE);
+
+	verify_dma(gpu, &debug);
+
+	seq_puts(m, "\tfeatures\n");
+	seq_printf(m, "\t minor_features0: 0x%08x\n",
+		   gpu->identity.minor_features0);
+	seq_printf(m, "\t minor_features1: 0x%08x\n",
+		   gpu->identity.minor_features1);
+	seq_printf(m, "\t minor_features2: 0x%08x\n",
+		   gpu->identity.minor_features2);
+	seq_printf(m, "\t minor_features3: 0x%08x\n",
+		   gpu->identity.minor_features3);
+	seq_printf(m, "\t minor_features4: 0x%08x\n",
+		   gpu->identity.minor_features4);
+	seq_printf(m, "\t minor_features5: 0x%08x\n",
+		   gpu->identity.minor_features5);
+
+	seq_puts(m, "\tspecs\n");
+	seq_printf(m, "\t stream_count:  %d\n",
+			gpu->identity.stream_count);
+	seq_printf(m, "\t register_max: %d\n",
+			gpu->identity.register_max);
+	seq_printf(m, "\t thread_count: %d\n",
+			gpu->identity.thread_count);
+	seq_printf(m, "\t vertex_cache_size: %d\n",
+			gpu->identity.vertex_cache_size);
+	seq_printf(m, "\t shader_core_count: %d\n",
+			gpu->identity.shader_core_count);
+	seq_printf(m, "\t pixel_pipes: %d\n",
+			gpu->identity.pixel_pipes);
+	seq_printf(m, "\t vertex_output_buffer_size: %d\n",
+			gpu->identity.vertex_output_buffer_size);
+	seq_printf(m, "\t buffer_size: %d\n",
+			gpu->identity.buffer_size);
+	seq_printf(m, "\t instruction_count: %d\n",
+			gpu->identity.instruction_count);
+	seq_printf(m, "\t num_constants: %d\n",
+			gpu->identity.num_constants);
+	seq_printf(m, "\t varyings_count: %d\n",
+			gpu->identity.varyings_count);
+
+	seq_printf(m, "\taxi: 0x%08x\n", axi);
+	seq_printf(m, "\tidle: 0x%08x\n", idle);
+	idle |= ~gpu->idle_mask & ~VIVS_HI_IDLE_STATE_AXI_LP;
+	if ((idle & VIVS_HI_IDLE_STATE_FE) == 0)
+		seq_puts(m, "\t FE is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_DE) == 0)
+		seq_puts(m, "\t DE is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_PE) == 0)
+		seq_puts(m, "\t PE is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_SH) == 0)
+		seq_puts(m, "\t SH is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_PA) == 0)
+		seq_puts(m, "\t PA is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_SE) == 0)
+		seq_puts(m, "\t SE is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_RA) == 0)
+		seq_puts(m, "\t RA is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_TX) == 0)
+		seq_puts(m, "\t TX is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_VG) == 0)
+		seq_puts(m, "\t VG is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_IM) == 0)
+		seq_puts(m, "\t IM is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_FP) == 0)
+		seq_puts(m, "\t FP is not idle\n");
+	if ((idle & VIVS_HI_IDLE_STATE_TS) == 0)
+		seq_puts(m, "\t TS is not idle\n");
+	if (idle & VIVS_HI_IDLE_STATE_AXI_LP)
+		seq_puts(m, "\t AXI low power mode\n");
+
+	if (gpu->identity.features & chipFeatures_DEBUG_MODE) {
+		u32 read0 = gpu_read(gpu, VIVS_MC_DEBUG_READ0);
+		u32 read1 = gpu_read(gpu, VIVS_MC_DEBUG_READ1);
+		u32 write = gpu_read(gpu, VIVS_MC_DEBUG_WRITE);
+
+		seq_puts(m, "\tMC\n");
+		seq_printf(m, "\t read0: 0x%08x\n", read0);
+		seq_printf(m, "\t read1: 0x%08x\n", read1);
+		seq_printf(m, "\t write: 0x%08x\n", write);
+	}
+
+	seq_puts(m, "\tDMA ");
+
+	if (debug.address[0] == debug.address[1] &&
+	    debug.state[0] == debug.state[1]) {
+		seq_puts(m, "seems to be stuck\n");
+	} else if (debug.address[0] == debug.address[1]) {
+		seq_puts(m, "adress is constant\n");
+	} else {
+		seq_puts(m, "is runing\n");
+	}
+
+	seq_printf(m, "\t address 0: 0x%08x\n", debug.address[0]);
+	seq_printf(m, "\t address 1: 0x%08x\n", debug.address[1]);
+	seq_printf(m, "\t state 0: 0x%08x\n", debug.state[0]);
+	seq_printf(m, "\t state 1: 0x%08x\n", debug.state[1]);
+	seq_printf(m, "\t last fetch 64 bit word: 0x%08x 0x%08x\n",
+		   dma_lo, dma_hi);
+
+	ret = 0;
+
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+
+	return ret;
+}
+#endif
+
+/*
+ * Power Management:
+ */
+static int enable_clk(struct etnaviv_gpu *gpu)
+{
+	if (gpu->clk_core)
+		clk_prepare_enable(gpu->clk_core);
+	if (gpu->clk_shader)
+		clk_prepare_enable(gpu->clk_shader);
+
+	return 0;
+}
+
+static int disable_clk(struct etnaviv_gpu *gpu)
+{
+	if (gpu->clk_core)
+		clk_disable_unprepare(gpu->clk_core);
+	if (gpu->clk_shader)
+		clk_disable_unprepare(gpu->clk_shader);
+
+	return 0;
+}
+
+static int enable_axi(struct etnaviv_gpu *gpu)
+{
+	if (gpu->clk_bus)
+		clk_prepare_enable(gpu->clk_bus);
+
+	return 0;
+}
+
+static int disable_axi(struct etnaviv_gpu *gpu)
+{
+	if (gpu->clk_bus)
+		clk_disable_unprepare(gpu->clk_bus);
+
+	return 0;
+}
+
+/*
+ * Hangcheck detection for locked gpu:
+ */
+static void recover_worker(struct work_struct *work)
+{
+	struct etnaviv_gpu *gpu = container_of(work, struct etnaviv_gpu,
+					       recover_work);
+	unsigned long flags;
+	unsigned int i;
+
+	dev_err(gpu->dev, "hangcheck recover!\n");
+
+	if (pm_runtime_get_sync(gpu->dev) < 0)
+		return;
+
+	mutex_lock(&gpu->lock);
+
+	/* Only catch the first event, or when manually re-armed */
+	if (etnaviv_dump_core) {
+		etnaviv_core_dump(gpu);
+		etnaviv_dump_core = false;
+	}
+
+	etnaviv_hw_reset(gpu);
+
+	/* complete all events, the GPU won't do it after the reset */
+	spin_lock_irqsave(&gpu->event_spinlock, flags);
+	for (i = 0; i < ARRAY_SIZE(gpu->event); i++) {
+		if (!gpu->event[i].used)
+			continue;
+		fence_signal(gpu->event[i].fence);
+		gpu->event[i].fence = NULL;
+		gpu->event[i].used = false;
+		complete(&gpu->event_free);
+		/*
+		 * Decrement the PM count for each stuck event. This is safe
+		 * even in atomic context as we use ASYNC RPM here.
+		 */
+		pm_runtime_put_autosuspend(gpu->dev);
+	}
+	spin_unlock_irqrestore(&gpu->event_spinlock, flags);
+	gpu->completed_fence = gpu->active_fence;
+
+	etnaviv_gpu_hw_init(gpu);
+	gpu->switch_context = true;
+
+	mutex_unlock(&gpu->lock);
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+
+	/* Retire the buffer objects in a work */
+	etnaviv_queue_work(gpu->drm, &gpu->retire_work);
+}
+
+static void hangcheck_timer_reset(struct etnaviv_gpu *gpu)
+{
+	DBG("%s", dev_name(gpu->dev));
+	mod_timer(&gpu->hangcheck_timer,
+		  round_jiffies_up(jiffies + DRM_ETNAVIV_HANGCHECK_JIFFIES));
+}
+
+static void hangcheck_handler(unsigned long data)
+{
+	struct etnaviv_gpu *gpu = (struct etnaviv_gpu *)data;
+	u32 fence = gpu->completed_fence;
+	bool progress = false;
+
+	if (fence != gpu->hangcheck_fence) {
+		gpu->hangcheck_fence = fence;
+		progress = true;
+	}
+
+	if (!progress) {
+		u32 dma_addr = gpu_read(gpu, VIVS_FE_DMA_ADDRESS);
+		int change = dma_addr - gpu->hangcheck_dma_addr;
+
+		if (change < 0 || change > 16) {
+			gpu->hangcheck_dma_addr = dma_addr;
+			progress = true;
+		}
+	}
+
+	if (!progress && fence_after(gpu->active_fence, fence)) {
+		dev_err(gpu->dev, "hangcheck detected gpu lockup!\n");
+		dev_err(gpu->dev, "     completed fence: %u\n", fence);
+		dev_err(gpu->dev, "     active fence: %u\n",
+			gpu->active_fence);
+		etnaviv_queue_work(gpu->drm, &gpu->recover_work);
+	}
+
+	/* if still more pending work, reset the hangcheck timer: */
+	if (fence_after(gpu->active_fence, gpu->hangcheck_fence))
+		hangcheck_timer_reset(gpu);
+}
+
+static void hangcheck_disable(struct etnaviv_gpu *gpu)
+{
+	del_timer_sync(&gpu->hangcheck_timer);
+	cancel_work_sync(&gpu->recover_work);
+}
+
+/* fence object management */
+struct etnaviv_fence {
+	struct etnaviv_gpu *gpu;
+	struct fence base;
+};
+
+static inline struct etnaviv_fence *to_etnaviv_fence(struct fence *fence)
+{
+	return container_of(fence, struct etnaviv_fence, base);
+}
+
+static const char *etnaviv_fence_get_driver_name(struct fence *fence)
+{
+	return "etnaviv";
+}
+
+static const char *etnaviv_fence_get_timeline_name(struct fence *fence)
+{
+	struct etnaviv_fence *f = to_etnaviv_fence(fence);
+
+	return dev_name(f->gpu->dev);
+}
+
+static bool etnaviv_fence_enable_signaling(struct fence *fence)
+{
+	return true;
+}
+
+static bool etnaviv_fence_signaled(struct fence *fence)
+{
+	struct etnaviv_fence *f = to_etnaviv_fence(fence);
+
+	return fence_completed(f->gpu, f->base.seqno);
+}
+
+static void etnaviv_fence_release(struct fence *fence)
+{
+	struct etnaviv_fence *f = to_etnaviv_fence(fence);
+
+	kfree_rcu(f, base.rcu);
+}
+
+static const struct fence_ops etnaviv_fence_ops = {
+	.get_driver_name = etnaviv_fence_get_driver_name,
+	.get_timeline_name = etnaviv_fence_get_timeline_name,
+	.enable_signaling = etnaviv_fence_enable_signaling,
+	.signaled = etnaviv_fence_signaled,
+	.wait = fence_default_wait,
+	.release = etnaviv_fence_release,
+};
+
+static struct fence *etnaviv_gpu_fence_alloc(struct etnaviv_gpu *gpu)
+{
+	struct etnaviv_fence *f;
+
+	f = kzalloc(sizeof(*f), GFP_KERNEL);
+	if (!f)
+		return NULL;
+
+	f->gpu = gpu;
+
+	fence_init(&f->base, &etnaviv_fence_ops, &gpu->fence_spinlock,
+		   gpu->fence_context, ++gpu->next_fence);
+
+	return &f->base;
+}
+
+int etnaviv_gpu_fence_sync_obj(struct etnaviv_gem_object *etnaviv_obj,
+	unsigned int context, bool exclusive)
+{
+	struct reservation_object *robj = etnaviv_obj->resv;
+	struct reservation_object_list *fobj;
+	struct fence *fence;
+	int i, ret;
+
+	if (!exclusive) {
+		ret = reservation_object_reserve_shared(robj);
+		if (ret)
+			return ret;
+	}
+
+	/*
+	 * If we have any shared fences, then the exclusive fence
+	 * should be ignored as it will already have been signalled.
+	 */
+	fobj = reservation_object_get_list(robj);
+	if (!fobj || fobj->shared_count == 0) {
+		/* Wait on any existing exclusive fence which isn't our own */
+		fence = reservation_object_get_excl(robj);
+		if (fence && fence->context != context) {
+			ret = fence_wait(fence, true);
+			if (ret)
+				return ret;
+		}
+	}
+
+	if (!exclusive || !fobj)
+		return 0;
+
+	for (i = 0; i < fobj->shared_count; i++) {
+		fence = rcu_dereference_protected(fobj->shared[i],
+						reservation_object_held(robj));
+		if (fence->context != context) {
+			ret = fence_wait(fence, true);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * event management:
+ */
+
+static unsigned int event_alloc(struct etnaviv_gpu *gpu)
+{
+	unsigned long ret, flags;
+	unsigned int i, event = ~0U;
+
+	ret = wait_for_completion_timeout(&gpu->event_free,
+					  msecs_to_jiffies(10 * 10000));
+	if (!ret)
+		dev_err(gpu->dev, "wait_for_completion_timeout failed");
+
+	spin_lock_irqsave(&gpu->event_spinlock, flags);
+
+	/* find first free event */
+	for (i = 0; i < ARRAY_SIZE(gpu->event); i++) {
+		if (gpu->event[i].used == false) {
+			gpu->event[i].used = true;
+			event = i;
+			break;
+		}
+	}
+
+	spin_unlock_irqrestore(&gpu->event_spinlock, flags);
+
+	return event;
+}
+
+static void event_free(struct etnaviv_gpu *gpu, unsigned int event)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&gpu->event_spinlock, flags);
+
+	if (gpu->event[event].used == false) {
+		dev_warn(gpu->dev, "event %u is already marked as free",
+			 event);
+		spin_unlock_irqrestore(&gpu->event_spinlock, flags);
+	} else {
+		gpu->event[event].used = false;
+		spin_unlock_irqrestore(&gpu->event_spinlock, flags);
+
+		complete(&gpu->event_free);
+	}
+}
+
+/*
+ * Cmdstream submission/retirement:
+ */
+
+struct etnaviv_cmdbuf *etnaviv_gpu_cmdbuf_new(struct etnaviv_gpu *gpu, u32 size,
+	size_t nr_bos)
+{
+	struct etnaviv_cmdbuf *cmdbuf;
+	size_t sz = size_vstruct(nr_bos, sizeof(cmdbuf->bo[0]),
+				 sizeof(*cmdbuf));
+
+	cmdbuf = kzalloc(sz, GFP_KERNEL);
+	if (!cmdbuf)
+		return NULL;
+
+	cmdbuf->vaddr = dma_alloc_writecombine(gpu->dev, size, &cmdbuf->paddr,
+					       GFP_KERNEL);
+	if (!cmdbuf->vaddr) {
+		kfree(cmdbuf);
+		return NULL;
+	}
+
+	cmdbuf->gpu = gpu;
+	cmdbuf->size = size;
+
+	return cmdbuf;
+}
+
+void etnaviv_gpu_cmdbuf_free(struct etnaviv_cmdbuf *cmdbuf)
+{
+	dma_free_writecombine(cmdbuf->gpu->dev, cmdbuf->size,
+			      cmdbuf->vaddr, cmdbuf->paddr);
+	kfree(cmdbuf);
+}
+
+static void retire_worker(struct work_struct *work)
+{
+	struct etnaviv_gpu *gpu = container_of(work, struct etnaviv_gpu,
+					       retire_work);
+	u32 fence = gpu->completed_fence;
+	struct etnaviv_cmdbuf *cmdbuf, *tmp;
+	unsigned int i;
+
+	mutex_lock(&gpu->lock);
+	list_for_each_entry_safe(cmdbuf, tmp, &gpu->active_cmd_list, node) {
+		if (!fence_is_signaled(cmdbuf->fence))
+			break;
+
+		list_del(&cmdbuf->node);
+		fence_put(cmdbuf->fence);
+
+		for (i = 0; i < cmdbuf->nr_bos; i++) {
+			struct etnaviv_gem_object *etnaviv_obj = cmdbuf->bo[i];
+
+			atomic_dec(&etnaviv_obj->gpu_active);
+			/* drop the refcount taken in etnaviv_gpu_submit */
+			etnaviv_gem_put_iova(gpu, &etnaviv_obj->base);
+		}
+
+		etnaviv_gpu_cmdbuf_free(cmdbuf);
+	}
+
+	gpu->retired_fence = fence;
+
+	mutex_unlock(&gpu->lock);
+
+	wake_up_all(&gpu->fence_event);
+}
+
+int etnaviv_gpu_wait_fence_interruptible(struct etnaviv_gpu *gpu,
+	u32 fence, struct timespec *timeout)
+{
+	int ret;
+
+	if (fence_after(fence, gpu->next_fence)) {
+		DRM_ERROR("waiting on invalid fence: %u (of %u)\n",
+				fence, gpu->next_fence);
+		return -EINVAL;
+	}
+
+	if (!timeout) {
+		/* No timeout was requested: just test for completion */
+		ret = fence_completed(gpu, fence) ? 0 : -EBUSY;
+	} else {
+		unsigned long remaining = etnaviv_timeout_to_jiffies(timeout);
+
+		ret = wait_event_interruptible_timeout(gpu->fence_event,
+						fence_completed(gpu, fence),
+						remaining);
+		if (ret == 0) {
+			DBG("timeout waiting for fence: %u (retired: %u completed: %u)",
+				fence, gpu->retired_fence,
+				gpu->completed_fence);
+			ret = -ETIMEDOUT;
+		} else if (ret != -ERESTARTSYS) {
+			ret = 0;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * Wait for an object to become inactive.  This, on it's own, is not race
+ * free: the object is moved by the retire worker off the active list, and
+ * then the iova is put.  Moreover, the object could be re-submitted just
+ * after we notice that it's become inactive.
+ *
+ * Although the retirement happens under the gpu lock, we don't want to hold
+ * that lock in this function while waiting.
+ */
+int etnaviv_gpu_wait_obj_inactive(struct etnaviv_gpu *gpu,
+	struct etnaviv_gem_object *etnaviv_obj, struct timespec *timeout)
+{
+	unsigned long remaining;
+	long ret;
+
+	if (!timeout)
+		return !is_active(etnaviv_obj) ? 0 : -EBUSY;
+
+	remaining = etnaviv_timeout_to_jiffies(timeout);
+
+	ret = wait_event_interruptible_timeout(gpu->fence_event,
+					       !is_active(etnaviv_obj),
+					       remaining);
+	if (ret > 0) {
+		struct etnaviv_drm_private *priv = gpu->drm->dev_private;
+
+		/* Synchronise with the retire worker */
+		flush_workqueue(priv->wq);
+		return 0;
+	} else if (ret == -ERESTARTSYS) {
+		return -ERESTARTSYS;
+	} else {
+		return -ETIMEDOUT;
+	}
+}
+
+int etnaviv_gpu_pm_get_sync(struct etnaviv_gpu *gpu)
+{
+	return pm_runtime_get_sync(gpu->dev);
+}
+
+void etnaviv_gpu_pm_put(struct etnaviv_gpu *gpu)
+{
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+}
+
+/* add bo's to gpu's ring, and kick gpu: */
+int etnaviv_gpu_submit(struct etnaviv_gpu *gpu,
+	struct etnaviv_gem_submit *submit, struct etnaviv_cmdbuf *cmdbuf)
+{
+	struct fence *fence;
+	unsigned int event, i;
+	int ret;
+
+	ret = etnaviv_gpu_pm_get_sync(gpu);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&gpu->lock);
+
+	/*
+	 * TODO
+	 *
+	 * - flush
+	 * - data endian
+	 * - prefetch
+	 *
+	 */
+
+	event = event_alloc(gpu);
+	if (unlikely(event == ~0U)) {
+		DRM_ERROR("no free event\n");
+		ret = -EBUSY;
+		goto out_unlock;
+	}
+
+	fence = etnaviv_gpu_fence_alloc(gpu);
+	if (!fence) {
+		event_free(gpu, event);
+		ret = -ENOMEM;
+		goto out_unlock;
+	}
+
+	gpu->event[event].fence = fence;
+	submit->fence = fence->seqno;
+	gpu->active_fence = submit->fence;
+
+	if (gpu->lastctx != cmdbuf->ctx) {
+		gpu->mmu->need_flush = true;
+		gpu->switch_context = true;
+		gpu->lastctx = cmdbuf->ctx;
+	}
+
+	etnaviv_buffer_queue(gpu, event, cmdbuf);
+
+	cmdbuf->fence = fence;
+	list_add_tail(&cmdbuf->node, &gpu->active_cmd_list);
+
+	/* We're committed to adding this command buffer, hold a PM reference */
+	pm_runtime_get_noresume(gpu->dev);
+
+	for (i = 0; i < submit->nr_bos; i++) {
+		struct etnaviv_gem_object *etnaviv_obj = submit->bos[i].obj;
+		u32 iova;
+
+		/* Each cmdbuf takes a refcount on the iova */
+		etnaviv_gem_get_iova(gpu, &etnaviv_obj->base, &iova);
+		cmdbuf->bo[i] = etnaviv_obj;
+		atomic_inc(&etnaviv_obj->gpu_active);
+
+		if (submit->bos[i].flags & ETNA_SUBMIT_BO_WRITE)
+			reservation_object_add_excl_fence(etnaviv_obj->resv,
+							  fence);
+		else
+			reservation_object_add_shared_fence(etnaviv_obj->resv,
+							    fence);
+	}
+	cmdbuf->nr_bos = submit->nr_bos;
+	hangcheck_timer_reset(gpu);
+	ret = 0;
+
+out_unlock:
+	mutex_unlock(&gpu->lock);
+
+	etnaviv_gpu_pm_put(gpu);
+
+	return ret;
+}
+
+/*
+ * Init/Cleanup:
+ */
+static irqreturn_t irq_handler(int irq, void *data)
+{
+	struct etnaviv_gpu *gpu = data;
+	irqreturn_t ret = IRQ_NONE;
+
+	u32 intr = gpu_read(gpu, VIVS_HI_INTR_ACKNOWLEDGE);
+
+	if (intr != 0) {
+		int event;
+
+		pm_runtime_mark_last_busy(gpu->dev);
+
+		dev_dbg(gpu->dev, "intr 0x%08x\n", intr);
+
+		if (intr & VIVS_HI_INTR_ACKNOWLEDGE_AXI_BUS_ERROR) {
+			dev_err(gpu->dev, "AXI bus error\n");
+			intr &= ~VIVS_HI_INTR_ACKNOWLEDGE_AXI_BUS_ERROR;
+		}
+
+		while ((event = ffs(intr)) != 0) {
+			struct fence *fence;
+
+			event -= 1;
+
+			intr &= ~(1 << event);
+
+			dev_dbg(gpu->dev, "event %u\n", event);
+
+			fence = gpu->event[event].fence;
+			gpu->event[event].fence = NULL;
+			fence_signal(fence);
+
+			/*
+			 * Events can be processed out of order.  Eg,
+			 * - allocate and queue event 0
+			 * - allocate event 1
+			 * - event 0 completes, we process it
+			 * - allocate and queue event 0
+			 * - event 1 and event 0 complete
+			 * we can end up processing event 0 first, then 1.
+			 */
+			if (fence_after(fence->seqno, gpu->completed_fence))
+				gpu->completed_fence = fence->seqno;
+
+			event_free(gpu, event);
+
+			/*
+			 * We need to balance the runtime PM count caused by
+			 * each submission.  Upon submission, we increment
+			 * the runtime PM counter, and allocate one event.
+			 * So here, we put the runtime PM count for each
+			 * completed event.
+			 */
+			pm_runtime_put_autosuspend(gpu->dev);
+		}
+
+		/* Retire the buffer objects in a work */
+		etnaviv_queue_work(gpu->drm, &gpu->retire_work);
+
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static int etnaviv_gpu_clk_enable(struct etnaviv_gpu *gpu)
+{
+	int ret;
+
+	ret = enable_clk(gpu);
+	if (ret)
+		return ret;
+
+	ret = enable_axi(gpu);
+	if (ret) {
+		disable_clk(gpu);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int etnaviv_gpu_clk_disable(struct etnaviv_gpu *gpu)
+{
+	int ret;
+
+	ret = disable_axi(gpu);
+	if (ret)
+		return ret;
+
+	ret = disable_clk(gpu);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int etnaviv_gpu_hw_suspend(struct etnaviv_gpu *gpu)
+{
+	if (gpu->buffer) {
+		unsigned long timeout;
+
+		/* Replace the last WAIT with END */
+		etnaviv_buffer_end(gpu);
+
+		/*
+		 * We know that only the FE is busy here, this should
+		 * happen quickly (as the WAIT is only 200 cycles).  If
+		 * we fail, just warn and continue.
+		 */
+		timeout = jiffies + msecs_to_jiffies(100);
+		do {
+			u32 idle = gpu_read(gpu, VIVS_HI_IDLE_STATE);
+
+			if ((idle & gpu->idle_mask) == gpu->idle_mask)
+				break;
+
+			if (time_is_before_jiffies(timeout)) {
+				dev_warn(gpu->dev,
+					 "timed out waiting for idle: idle=0x%x\n",
+					 idle);
+				break;
+			}
+
+			udelay(5);
+		} while (1);
+	}
+
+	return etnaviv_gpu_clk_disable(gpu);
+}
+
+#ifdef CONFIG_PM
+static int etnaviv_gpu_hw_resume(struct etnaviv_gpu *gpu)
+{
+	u32 clock;
+	int ret;
+
+	ret = mutex_lock_killable(&gpu->lock);
+	if (ret)
+		return ret;
+
+	clock = VIVS_HI_CLOCK_CONTROL_DISABLE_DEBUG_REGISTERS |
+		VIVS_HI_CLOCK_CONTROL_FSCALE_VAL(0x40);
+
+	etnaviv_gpu_load_clock(gpu, clock);
+	etnaviv_gpu_hw_init(gpu);
+
+	gpu->switch_context = true;
+
+	mutex_unlock(&gpu->lock);
+
+	return 0;
+}
+#endif
+
+static int etnaviv_gpu_bind(struct device *dev, struct device *master,
+	void *data)
+{
+	struct drm_device *drm = data;
+	struct etnaviv_drm_private *priv = drm->dev_private;
+	struct etnaviv_gpu *gpu = dev_get_drvdata(dev);
+	int ret;
+
+#ifdef CONFIG_PM
+	ret = pm_runtime_get_sync(gpu->dev);
+#else
+	ret = etnaviv_gpu_clk_enable(gpu);
+#endif
+	if (ret < 0)
+		return ret;
+
+	gpu->drm = drm;
+	gpu->fence_context = fence_context_alloc(1);
+	spin_lock_init(&gpu->fence_spinlock);
+
+	INIT_LIST_HEAD(&gpu->active_cmd_list);
+	INIT_WORK(&gpu->retire_work, retire_worker);
+	INIT_WORK(&gpu->recover_work, recover_worker);
+	init_waitqueue_head(&gpu->fence_event);
+
+	setup_timer(&gpu->hangcheck_timer, hangcheck_handler,
+			(unsigned long)gpu);
+
+	priv->gpu[priv->num_gpus++] = gpu;
+
+	pm_runtime_mark_last_busy(gpu->dev);
+	pm_runtime_put_autosuspend(gpu->dev);
+
+	return 0;
+}
+
+static void etnaviv_gpu_unbind(struct device *dev, struct device *master,
+	void *data)
+{
+	struct etnaviv_gpu *gpu = dev_get_drvdata(dev);
+
+	DBG("%s", dev_name(gpu->dev));
+
+	hangcheck_disable(gpu);
+
+#ifdef CONFIG_PM
+	pm_runtime_get_sync(gpu->dev);
+	pm_runtime_put_sync_suspend(gpu->dev);
+#else
+	etnaviv_gpu_hw_suspend(gpu);
+#endif
+
+	if (gpu->buffer) {
+		etnaviv_gpu_cmdbuf_free(gpu->buffer);
+		gpu->buffer = NULL;
+	}
+
+	if (gpu->mmu) {
+		etnaviv_iommu_destroy(gpu->mmu);
+		gpu->mmu = NULL;
+	}
+
+	gpu->drm = NULL;
+}
+
+static const struct component_ops gpu_ops = {
+	.bind = etnaviv_gpu_bind,
+	.unbind = etnaviv_gpu_unbind,
+};
+
+static const struct of_device_id etnaviv_gpu_match[] = {
+	{
+		.compatible = "vivante,gc"
+	},
+	{ /* sentinel */ }
+};
+
+static int etnaviv_gpu_platform_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct etnaviv_gpu *gpu;
+	int err = 0;
+
+	gpu = devm_kzalloc(dev, sizeof(*gpu), GFP_KERNEL);
+	if (!gpu)
+		return -ENOMEM;
+
+	gpu->dev = &pdev->dev;
+	mutex_init(&gpu->lock);
+
+	/*
+	 * Set the GPU base address to the start of physical memory.  This
+	 * ensures that if we have up to 2GB, the v1 MMU can address the
+	 * highest memory.  This is important as command buffers may be
+	 * allocated outside of this limit.
+	 */
+	gpu->memory_base = PHYS_OFFSET;
+
+	/* Map registers: */
+	gpu->mmio = etnaviv_ioremap(pdev, NULL, dev_name(gpu->dev));
+	if (IS_ERR(gpu->mmio))
+		return PTR_ERR(gpu->mmio);
+
+	/* Get Interrupt: */
+	gpu->irq = platform_get_irq(pdev, 0);
+	if (gpu->irq < 0) {
+		err = gpu->irq;
+		dev_err(dev, "failed to get irq: %d\n", err);
+		goto fail;
+	}
+
+	err = devm_request_irq(&pdev->dev, gpu->irq, irq_handler, 0,
+			       dev_name(gpu->dev), gpu);
+	if (err) {
+		dev_err(dev, "failed to request IRQ%u: %d\n", gpu->irq, err);
+		goto fail;
+	}
+
+	/* Get Clocks: */
+	gpu->clk_bus = devm_clk_get(&pdev->dev, "bus");
+	DBG("clk_bus: %p", gpu->clk_bus);
+	if (IS_ERR(gpu->clk_bus))
+		gpu->clk_bus = NULL;
+
+	gpu->clk_core = devm_clk_get(&pdev->dev, "core");
+	DBG("clk_core: %p", gpu->clk_core);
+	if (IS_ERR(gpu->clk_core))
+		gpu->clk_core = NULL;
+
+	gpu->clk_shader = devm_clk_get(&pdev->dev, "shader");
+	DBG("clk_shader: %p", gpu->clk_shader);
+	if (IS_ERR(gpu->clk_shader))
+		gpu->clk_shader = NULL;
+
+	/* TODO: figure out max mapped size */
+	dev_set_drvdata(dev, gpu);
+
+	/*
+	 * We treat the device as initially suspended.  The runtime PM
+	 * autosuspend delay is rather arbitary: no measurements have
+	 * yet been performed to determine an appropriate value.
+	 */
+	pm_runtime_use_autosuspend(gpu->dev);
+	pm_runtime_set_autosuspend_delay(gpu->dev, 200);
+	pm_runtime_enable(gpu->dev);
+
+	err = component_add(&pdev->dev, &gpu_ops);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to register component: %d\n", err);
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+	return err;
+}
+
+static int etnaviv_gpu_platform_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &gpu_ops);
+	pm_runtime_disable(&pdev->dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int etnaviv_gpu_rpm_suspend(struct device *dev)
+{
+	struct etnaviv_gpu *gpu = dev_get_drvdata(dev);
+	u32 idle, mask;
+
+	/* If we have outstanding fences, we're not idle */
+	if (gpu->completed_fence != gpu->active_fence)
+		return -EBUSY;
+
+	/* Check whether the hardware (except FE) is idle */
+	mask = gpu->idle_mask & ~VIVS_HI_IDLE_STATE_FE;
+	idle = gpu_read(gpu, VIVS_HI_IDLE_STATE) & mask;
+	if (idle != mask)
+		return -EBUSY;
+
+	return etnaviv_gpu_hw_suspend(gpu);
+}
+
+static int etnaviv_gpu_rpm_resume(struct device *dev)
+{
+	struct etnaviv_gpu *gpu = dev_get_drvdata(dev);
+	int ret;
+
+	ret = etnaviv_gpu_clk_enable(gpu);
+	if (ret)
+		return ret;
+
+	/* Re-initialise the basic hardware state */
+	if (gpu->drm && gpu->buffer) {
+		ret = etnaviv_gpu_hw_resume(gpu);
+		if (ret) {
+			etnaviv_gpu_clk_disable(gpu);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops etnaviv_gpu_pm_ops = {
+	SET_RUNTIME_PM_OPS(etnaviv_gpu_rpm_suspend, etnaviv_gpu_rpm_resume,
+			   NULL)
+};
+
+struct platform_driver etnaviv_gpu_driver = {
+	.driver = {
+		.name = "etnaviv-gpu",
+		.owner = THIS_MODULE,
+		.pm = &etnaviv_gpu_pm_ops,
+		.of_match_table = etnaviv_gpu_match,
+	},
+	.probe = etnaviv_gpu_platform_probe,
+	.remove = etnaviv_gpu_platform_remove,
+	.id_table = gpu_ids,
+};
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_gpu.h b/drivers/gpu/drm/etnaviv/etnaviv_gpu.h
new file mode 100644
index 000000000000..f233ac4c7c1c
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_gpu.h
@@ -0,0 +1,218 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_GPU_H__
+#define __ETNAVIV_GPU_H__
+
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+
+#include "etnaviv_drv.h"
+
+struct etnaviv_gem_submit;
+
+struct etnaviv_chip_identity {
+	/* Chip model. */
+	u32 model;
+
+	/* Revision value.*/
+	u32 revision;
+
+	/* Supported feature fields. */
+	u32 features;
+
+	/* Supported minor feature fields. */
+	u32 minor_features0;
+
+	/* Supported minor feature 1 fields. */
+	u32 minor_features1;
+
+	/* Supported minor feature 2 fields. */
+	u32 minor_features2;
+
+	/* Supported minor feature 3 fields. */
+	u32 minor_features3;
+
+	/* Supported minor feature 4 fields. */
+	u32 minor_features4;
+
+	/* Supported minor feature 5 fields. */
+	u32 minor_features5;
+
+	/* Number of streams supported. */
+	u32 stream_count;
+
+	/* Total number of temporary registers per thread. */
+	u32 register_max;
+
+	/* Maximum number of threads. */
+	u32 thread_count;
+
+	/* Number of shader cores. */
+	u32 shader_core_count;
+
+	/* Size of the vertex cache. */
+	u32 vertex_cache_size;
+
+	/* Number of entries in the vertex output buffer. */
+	u32 vertex_output_buffer_size;
+
+	/* Number of pixel pipes. */
+	u32 pixel_pipes;
+
+	/* Number of instructions. */
+	u32 instruction_count;
+
+	/* Number of constants. */
+	u32 num_constants;
+
+	/* Buffer size */
+	u32 buffer_size;
+
+	/* Number of varyings */
+	u8 varyings_count;
+};
+
+struct etnaviv_event {
+	bool used;
+	struct fence *fence;
+};
+
+struct etnaviv_cmdbuf;
+
+struct etnaviv_gpu {
+	struct drm_device *drm;
+	struct device *dev;
+	struct mutex lock;
+	struct etnaviv_chip_identity identity;
+	struct etnaviv_file_private *lastctx;
+	bool switch_context;
+
+	/* 'ring'-buffer: */
+	struct etnaviv_cmdbuf *buffer;
+
+	/* bus base address of memory  */
+	u32 memory_base;
+
+	/* event management: */
+	struct etnaviv_event event[30];
+	struct completion event_free;
+	spinlock_t event_spinlock;
+
+	/* list of currently in-flight command buffers */
+	struct list_head active_cmd_list;
+
+	u32 idle_mask;
+
+	/* Fencing support */
+	u32 next_fence;
+	u32 active_fence;
+	u32 completed_fence;
+	u32 retired_fence;
+	wait_queue_head_t fence_event;
+	unsigned int fence_context;
+	spinlock_t fence_spinlock;
+
+	/* worker for handling active-list retiring: */
+	struct work_struct retire_work;
+
+	void __iomem *mmio;
+	int irq;
+
+	struct etnaviv_iommu *mmu;
+
+	/* Power Control: */
+	struct clk *clk_bus;
+	struct clk *clk_core;
+	struct clk *clk_shader;
+
+	/* Hang Detction: */
+#define DRM_ETNAVIV_HANGCHECK_PERIOD 500 /* in ms */
+#define DRM_ETNAVIV_HANGCHECK_JIFFIES msecs_to_jiffies(DRM_ETNAVIV_HANGCHECK_PERIOD)
+	struct timer_list hangcheck_timer;
+	u32 hangcheck_fence;
+	u32 hangcheck_dma_addr;
+	struct work_struct recover_work;
+};
+
+struct etnaviv_cmdbuf {
+	/* device this cmdbuf is allocated for */
+	struct etnaviv_gpu *gpu;
+	/* user context key, must be unique between all active users */
+	struct etnaviv_file_private *ctx;
+	/* cmdbuf properties */
+	void *vaddr;
+	dma_addr_t paddr;
+	u32 size;
+	u32 user_size;
+	/* fence after which this buffer is to be disposed */
+	struct fence *fence;
+	/* target exec state */
+	u32 exec_state;
+	/* per GPU in-flight list */
+	struct list_head node;
+	/* BOs attached to this command buffer */
+	unsigned int nr_bos;
+	struct etnaviv_gem_object *bo[0];
+};
+
+static inline void gpu_write(struct etnaviv_gpu *gpu, u32 reg, u32 data)
+{
+	etnaviv_writel(data, gpu->mmio + reg);
+}
+
+static inline u32 gpu_read(struct etnaviv_gpu *gpu, u32 reg)
+{
+	return etnaviv_readl(gpu->mmio + reg);
+}
+
+static inline bool fence_completed(struct etnaviv_gpu *gpu, u32 fence)
+{
+	return fence_after_eq(gpu->completed_fence, fence);
+}
+
+static inline bool fence_retired(struct etnaviv_gpu *gpu, u32 fence)
+{
+	return fence_after_eq(gpu->retired_fence, fence);
+}
+
+int etnaviv_gpu_get_param(struct etnaviv_gpu *gpu, u32 param, u64 *value);
+
+int etnaviv_gpu_init(struct etnaviv_gpu *gpu);
+
+#ifdef CONFIG_DEBUG_FS
+int etnaviv_gpu_debugfs(struct etnaviv_gpu *gpu, struct seq_file *m);
+#endif
+
+int etnaviv_gpu_fence_sync_obj(struct etnaviv_gem_object *etnaviv_obj,
+	unsigned int context, bool exclusive);
+
+void etnaviv_gpu_retire(struct etnaviv_gpu *gpu);
+int etnaviv_gpu_wait_fence_interruptible(struct etnaviv_gpu *gpu,
+	u32 fence, struct timespec *timeout);
+int etnaviv_gpu_wait_obj_inactive(struct etnaviv_gpu *gpu,
+	struct etnaviv_gem_object *etnaviv_obj, struct timespec *timeout);
+int etnaviv_gpu_submit(struct etnaviv_gpu *gpu,
+	struct etnaviv_gem_submit *submit, struct etnaviv_cmdbuf *cmdbuf);
+struct etnaviv_cmdbuf *etnaviv_gpu_cmdbuf_new(struct etnaviv_gpu *gpu,
+					      u32 size, size_t nr_bos);
+void etnaviv_gpu_cmdbuf_free(struct etnaviv_cmdbuf *cmdbuf);
+int etnaviv_gpu_pm_get_sync(struct etnaviv_gpu *gpu);
+void etnaviv_gpu_pm_put(struct etnaviv_gpu *gpu);
+
+extern struct platform_driver etnaviv_gpu_driver;
+
+#endif /* __ETNAVIV_GPU_H__ */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_iommu.c b/drivers/gpu/drm/etnaviv/etnaviv_iommu.c
new file mode 100644
index 000000000000..522cfd447892
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_iommu.c
@@ -0,0 +1,240 @@
+/*
+ * Copyright (C) 2014 Christian Gmeiner <christian.gmeiner@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/iommu.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/bitops.h>
+
+#include "etnaviv_gpu.h"
+#include "etnaviv_mmu.h"
+#include "etnaviv_iommu.h"
+#include "state_hi.xml.h"
+
+#define PT_SIZE		SZ_2M
+#define PT_ENTRIES	(PT_SIZE / sizeof(u32))
+
+#define GPU_MEM_START	0x80000000
+
+struct etnaviv_iommu_domain_pgtable {
+	u32 *pgtable;
+	dma_addr_t paddr;
+};
+
+struct etnaviv_iommu_domain {
+	struct iommu_domain domain;
+	struct device *dev;
+	void *bad_page_cpu;
+	dma_addr_t bad_page_dma;
+	struct etnaviv_iommu_domain_pgtable pgtable;
+	spinlock_t map_lock;
+};
+
+static struct etnaviv_iommu_domain *to_etnaviv_domain(struct iommu_domain *domain)
+{
+	return container_of(domain, struct etnaviv_iommu_domain, domain);
+}
+
+static int pgtable_alloc(struct etnaviv_iommu_domain_pgtable *pgtable,
+			 size_t size)
+{
+	pgtable->pgtable = dma_alloc_coherent(NULL, size, &pgtable->paddr, GFP_KERNEL);
+	if (!pgtable->pgtable)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void pgtable_free(struct etnaviv_iommu_domain_pgtable *pgtable,
+			 size_t size)
+{
+	dma_free_coherent(NULL, size, pgtable->pgtable, pgtable->paddr);
+}
+
+static u32 pgtable_read(struct etnaviv_iommu_domain_pgtable *pgtable,
+			   unsigned long iova)
+{
+	/* calcuate index into page table */
+	unsigned int index = (iova - GPU_MEM_START) / SZ_4K;
+	phys_addr_t paddr;
+
+	paddr = pgtable->pgtable[index];
+
+	return paddr;
+}
+
+static void pgtable_write(struct etnaviv_iommu_domain_pgtable *pgtable,
+			  unsigned long iova, phys_addr_t paddr)
+{
+	/* calcuate index into page table */
+	unsigned int index = (iova - GPU_MEM_START) / SZ_4K;
+
+	pgtable->pgtable[index] = paddr;
+}
+
+static int __etnaviv_iommu_init(struct etnaviv_iommu_domain *etnaviv_domain)
+{
+	u32 *p;
+	int ret, i;
+
+	etnaviv_domain->bad_page_cpu = dma_alloc_coherent(etnaviv_domain->dev,
+						  SZ_4K,
+						  &etnaviv_domain->bad_page_dma,
+						  GFP_KERNEL);
+	if (!etnaviv_domain->bad_page_cpu)
+		return -ENOMEM;
+
+	p = etnaviv_domain->bad_page_cpu;
+	for (i = 0; i < SZ_4K / 4; i++)
+		*p++ = 0xdead55aa;
+
+	ret = pgtable_alloc(&etnaviv_domain->pgtable, PT_SIZE);
+	if (ret < 0) {
+		dma_free_coherent(etnaviv_domain->dev, SZ_4K,
+				  etnaviv_domain->bad_page_cpu,
+				  etnaviv_domain->bad_page_dma);
+		return ret;
+	}
+
+	for (i = 0; i < PT_ENTRIES; i++)
+		etnaviv_domain->pgtable.pgtable[i] =
+			etnaviv_domain->bad_page_dma;
+
+	spin_lock_init(&etnaviv_domain->map_lock);
+
+	return 0;
+}
+
+static void etnaviv_domain_free(struct iommu_domain *domain)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+
+	pgtable_free(&etnaviv_domain->pgtable, PT_SIZE);
+
+	dma_free_coherent(etnaviv_domain->dev, SZ_4K,
+			  etnaviv_domain->bad_page_cpu,
+			  etnaviv_domain->bad_page_dma);
+
+	kfree(etnaviv_domain);
+}
+
+static int etnaviv_iommuv1_map(struct iommu_domain *domain, unsigned long iova,
+	   phys_addr_t paddr, size_t size, int prot)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+
+	if (size != SZ_4K)
+		return -EINVAL;
+
+	spin_lock(&etnaviv_domain->map_lock);
+	pgtable_write(&etnaviv_domain->pgtable, iova, paddr);
+	spin_unlock(&etnaviv_domain->map_lock);
+
+	return 0;
+}
+
+static size_t etnaviv_iommuv1_unmap(struct iommu_domain *domain,
+	unsigned long iova, size_t size)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+
+	if (size != SZ_4K)
+		return -EINVAL;
+
+	spin_lock(&etnaviv_domain->map_lock);
+	pgtable_write(&etnaviv_domain->pgtable, iova,
+		      etnaviv_domain->bad_page_dma);
+	spin_unlock(&etnaviv_domain->map_lock);
+
+	return SZ_4K;
+}
+
+static phys_addr_t etnaviv_iommu_iova_to_phys(struct iommu_domain *domain,
+	dma_addr_t iova)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+
+	return pgtable_read(&etnaviv_domain->pgtable, iova);
+}
+
+static size_t etnaviv_iommuv1_dump_size(struct iommu_domain *domain)
+{
+	return PT_SIZE;
+}
+
+static void etnaviv_iommuv1_dump(struct iommu_domain *domain, void *buf)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+
+	memcpy(buf, etnaviv_domain->pgtable.pgtable, PT_SIZE);
+}
+
+static struct etnaviv_iommu_ops etnaviv_iommu_ops = {
+	.ops = {
+		.domain_free = etnaviv_domain_free,
+		.map = etnaviv_iommuv1_map,
+		.unmap = etnaviv_iommuv1_unmap,
+		.iova_to_phys = etnaviv_iommu_iova_to_phys,
+		.pgsize_bitmap = SZ_4K,
+	},
+	.dump_size = etnaviv_iommuv1_dump_size,
+	.dump = etnaviv_iommuv1_dump,
+};
+
+void etnaviv_iommu_domain_restore(struct etnaviv_gpu *gpu,
+	struct iommu_domain *domain)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain = to_etnaviv_domain(domain);
+	u32 pgtable;
+
+	/* set page table address in MC */
+	pgtable = (u32)etnaviv_domain->pgtable.paddr;
+
+	gpu_write(gpu, VIVS_MC_MMU_FE_PAGE_TABLE, pgtable);
+	gpu_write(gpu, VIVS_MC_MMU_TX_PAGE_TABLE, pgtable);
+	gpu_write(gpu, VIVS_MC_MMU_PE_PAGE_TABLE, pgtable);
+	gpu_write(gpu, VIVS_MC_MMU_PEZ_PAGE_TABLE, pgtable);
+	gpu_write(gpu, VIVS_MC_MMU_RA_PAGE_TABLE, pgtable);
+}
+
+struct iommu_domain *etnaviv_iommu_domain_alloc(struct etnaviv_gpu *gpu)
+{
+	struct etnaviv_iommu_domain *etnaviv_domain;
+	int ret;
+
+	etnaviv_domain = kzalloc(sizeof(*etnaviv_domain), GFP_KERNEL);
+	if (!etnaviv_domain)
+		return NULL;
+
+	etnaviv_domain->dev = gpu->dev;
+
+	etnaviv_domain->domain.type = __IOMMU_DOMAIN_PAGING;
+	etnaviv_domain->domain.ops = &etnaviv_iommu_ops.ops;
+	etnaviv_domain->domain.geometry.aperture_start = GPU_MEM_START;
+	etnaviv_domain->domain.geometry.aperture_end = GPU_MEM_START + PT_ENTRIES * SZ_4K - 1;
+
+	ret = __etnaviv_iommu_init(etnaviv_domain);
+	if (ret)
+		goto out_free;
+
+	return &etnaviv_domain->domain;
+
+out_free:
+	kfree(etnaviv_domain);
+	return NULL;
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_iommu.h b/drivers/gpu/drm/etnaviv/etnaviv_iommu.h
new file mode 100644
index 000000000000..cf45503f6b6f
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_iommu.h
@@ -0,0 +1,28 @@
+/*
+ * Copyright (C) 2014 Christian Gmeiner <christian.gmeiner@gmail.com>
+  *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_IOMMU_H__
+#define __ETNAVIV_IOMMU_H__
+
+#include <linux/iommu.h>
+struct etnaviv_gpu;
+
+struct iommu_domain *etnaviv_iommu_domain_alloc(struct etnaviv_gpu *gpu);
+void etnaviv_iommu_domain_restore(struct etnaviv_gpu *gpu,
+	struct iommu_domain *domain);
+struct iommu_domain *etnaviv_iommu_v2_domain_alloc(struct etnaviv_gpu *gpu);
+
+#endif /* __ETNAVIV_IOMMU_H__ */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.c b/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.c
new file mode 100644
index 000000000000..fbb4aed3dc80
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2014 Christian Gmeiner <christian.gmeiner@gmail.com>
+  *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/iommu.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/dma-mapping.h>
+#include <linux/bitops.h>
+
+#include "etnaviv_gpu.h"
+#include "etnaviv_iommu.h"
+#include "state_hi.xml.h"
+
+
+struct iommu_domain *etnaviv_iommu_v2_domain_alloc(struct etnaviv_gpu *gpu)
+{
+	/* TODO */
+	return NULL;
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.h b/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.h
new file mode 100644
index 000000000000..603ea41c5389
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_iommu_v2.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (C) 2014 Christian Gmeiner <christian.gmeiner@gmail.com>
+  *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_IOMMU_V2_H__
+#define __ETNAVIV_IOMMU_V2_H__
+
+#include <linux/iommu.h>
+struct etnaviv_gpu;
+
+struct iommu_domain *etnaviv_iommu_v2_domain_alloc(struct etnaviv_gpu *gpu);
+
+#endif /* __ETNAVIV_IOMMU_V2_H__ */
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_mmu.c b/drivers/gpu/drm/etnaviv/etnaviv_mmu.c
new file mode 100644
index 000000000000..6743bc648dc8
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_mmu.c
@@ -0,0 +1,299 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "etnaviv_drv.h"
+#include "etnaviv_gem.h"
+#include "etnaviv_gpu.h"
+#include "etnaviv_mmu.h"
+
+static int etnaviv_fault_handler(struct iommu_domain *iommu, struct device *dev,
+		unsigned long iova, int flags, void *arg)
+{
+	DBG("*** fault: iova=%08lx, flags=%d", iova, flags);
+	return 0;
+}
+
+int etnaviv_iommu_map(struct etnaviv_iommu *iommu, u32 iova,
+		struct sg_table *sgt, unsigned len, int prot)
+{
+	struct iommu_domain *domain = iommu->domain;
+	struct scatterlist *sg;
+	unsigned int da = iova;
+	unsigned int i, j;
+	int ret;
+
+	if (!domain || !sgt)
+		return -EINVAL;
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		u32 pa = sg_dma_address(sg) - sg->offset;
+		size_t bytes = sg_dma_len(sg) + sg->offset;
+
+		VERB("map[%d]: %08x %08x(%zx)", i, iova, pa, bytes);
+
+		ret = iommu_map(domain, da, pa, bytes, prot);
+		if (ret)
+			goto fail;
+
+		da += bytes;
+	}
+
+	return 0;
+
+fail:
+	da = iova;
+
+	for_each_sg(sgt->sgl, sg, i, j) {
+		size_t bytes = sg_dma_len(sg) + sg->offset;
+
+		iommu_unmap(domain, da, bytes);
+		da += bytes;
+	}
+	return ret;
+}
+
+int etnaviv_iommu_unmap(struct etnaviv_iommu *iommu, u32 iova,
+		struct sg_table *sgt, unsigned len)
+{
+	struct iommu_domain *domain = iommu->domain;
+	struct scatterlist *sg;
+	unsigned int da = iova;
+	int i;
+
+	for_each_sg(sgt->sgl, sg, sgt->nents, i) {
+		size_t bytes = sg_dma_len(sg) + sg->offset;
+		size_t unmapped;
+
+		unmapped = iommu_unmap(domain, da, bytes);
+		if (unmapped < bytes)
+			return unmapped;
+
+		VERB("unmap[%d]: %08x(%zx)", i, iova, bytes);
+
+		BUG_ON(!PAGE_ALIGNED(bytes));
+
+		da += bytes;
+	}
+
+	return 0;
+}
+
+static void etnaviv_iommu_remove_mapping(struct etnaviv_iommu *mmu,
+	struct etnaviv_vram_mapping *mapping)
+{
+	struct etnaviv_gem_object *etnaviv_obj = mapping->object;
+
+	etnaviv_iommu_unmap(mmu, mapping->vram_node.start,
+			    etnaviv_obj->sgt, etnaviv_obj->base.size);
+	drm_mm_remove_node(&mapping->vram_node);
+}
+
+int etnaviv_iommu_map_gem(struct etnaviv_iommu *mmu,
+	struct etnaviv_gem_object *etnaviv_obj, u32 memory_base,
+	struct etnaviv_vram_mapping *mapping)
+{
+	struct etnaviv_vram_mapping *free = NULL;
+	struct sg_table *sgt = etnaviv_obj->sgt;
+	struct drm_mm_node *node;
+	int ret;
+
+	lockdep_assert_held(&etnaviv_obj->lock);
+
+	mutex_lock(&mmu->lock);
+
+	/* v1 MMU can optimize single entry (contiguous) scatterlists */
+	if (sgt->nents == 1 && !(etnaviv_obj->flags & ETNA_BO_FORCE_MMU)) {
+		u32 iova;
+
+		iova = sg_dma_address(sgt->sgl) - memory_base;
+		if (iova < 0x80000000 - sg_dma_len(sgt->sgl)) {
+			mapping->iova = iova;
+			list_add_tail(&mapping->mmu_node, &mmu->mappings);
+			mutex_unlock(&mmu->lock);
+			return 0;
+		}
+	}
+
+	node = &mapping->vram_node;
+	while (1) {
+		struct etnaviv_vram_mapping *m, *n;
+		struct list_head list;
+		bool found;
+
+		ret = drm_mm_insert_node_in_range(&mmu->mm, node,
+			etnaviv_obj->base.size, 0, mmu->last_iova, ~0UL,
+			DRM_MM_SEARCH_DEFAULT);
+
+		if (ret != -ENOSPC)
+			break;
+
+		/*
+		 * If we did not search from the start of the MMU region,
+		 * try again in case there are free slots.
+		 */
+		if (mmu->last_iova) {
+			mmu->last_iova = 0;
+			mmu->need_flush = true;
+			continue;
+		}
+
+		/* Try to retire some entries */
+		drm_mm_init_scan(&mmu->mm, etnaviv_obj->base.size, 0, 0);
+
+		found = 0;
+		INIT_LIST_HEAD(&list);
+		list_for_each_entry(free, &mmu->mappings, mmu_node) {
+			/* If this vram node has not been used, skip this. */
+			if (!free->vram_node.mm)
+				continue;
+
+			/*
+			 * If the iova is pinned, then it's in-use,
+			 * so we must keep its mapping.
+			 */
+			if (free->use)
+				continue;
+
+			list_add(&free->scan_node, &list);
+			if (drm_mm_scan_add_block(&free->vram_node)) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			/* Nothing found, clean up and fail */
+			list_for_each_entry_safe(m, n, &list, scan_node)
+				BUG_ON(drm_mm_scan_remove_block(&m->vram_node));
+			break;
+		}
+
+		/*
+		 * drm_mm does not allow any other operations while
+		 * scanning, so we have to remove all blocks first.
+		 * If drm_mm_scan_remove_block() returns false, we
+		 * can leave the block pinned.
+		 */
+		list_for_each_entry_safe(m, n, &list, scan_node)
+			if (!drm_mm_scan_remove_block(&m->vram_node))
+				list_del_init(&m->scan_node);
+
+		/*
+		 * Unmap the blocks which need to be reaped from the MMU.
+		 * Clear the mmu pointer to prevent the get_iova finding
+		 * this mapping.
+		 */
+		list_for_each_entry_safe(m, n, &list, scan_node) {
+			etnaviv_iommu_remove_mapping(mmu, m);
+			m->mmu = NULL;
+			list_del_init(&m->mmu_node);
+			list_del_init(&m->scan_node);
+		}
+
+		/*
+		 * We removed enough mappings so that the new allocation will
+		 * succeed.  Ensure that the MMU will be flushed before the
+		 * associated commit requesting this mapping, and retry the
+		 * allocation one more time.
+		 */
+		mmu->need_flush = true;
+	}
+
+	if (ret < 0) {
+		mutex_unlock(&mmu->lock);
+		return ret;
+	}
+
+	mmu->last_iova = node->start + etnaviv_obj->base.size;
+	mapping->iova = node->start;
+	ret = etnaviv_iommu_map(mmu, node->start, sgt, etnaviv_obj->base.size,
+				IOMMU_READ | IOMMU_WRITE);
+
+	if (ret < 0) {
+		drm_mm_remove_node(node);
+		mutex_unlock(&mmu->lock);
+		return ret;
+	}
+
+	list_add_tail(&mapping->mmu_node, &mmu->mappings);
+	mutex_unlock(&mmu->lock);
+
+	return ret;
+}
+
+void etnaviv_iommu_unmap_gem(struct etnaviv_iommu *mmu,
+	struct etnaviv_vram_mapping *mapping)
+{
+	WARN_ON(mapping->use);
+
+	mutex_lock(&mmu->lock);
+
+	/* If the vram node is on the mm, unmap and remove the node */
+	if (mapping->vram_node.mm == &mmu->mm)
+		etnaviv_iommu_remove_mapping(mmu, mapping);
+
+	list_del(&mapping->mmu_node);
+	mutex_unlock(&mmu->lock);
+}
+
+void etnaviv_iommu_destroy(struct etnaviv_iommu *mmu)
+{
+	drm_mm_takedown(&mmu->mm);
+	iommu_domain_free(mmu->domain);
+	kfree(mmu);
+}
+
+struct etnaviv_iommu *etnaviv_iommu_new(struct etnaviv_gpu *gpu,
+	struct iommu_domain *domain, enum etnaviv_iommu_version version)
+{
+	struct etnaviv_iommu *mmu;
+
+	mmu = kzalloc(sizeof(*mmu), GFP_KERNEL);
+	if (!mmu)
+		return ERR_PTR(-ENOMEM);
+
+	mmu->domain = domain;
+	mmu->gpu = gpu;
+	mmu->version = version;
+	mutex_init(&mmu->lock);
+	INIT_LIST_HEAD(&mmu->mappings);
+
+	drm_mm_init(&mmu->mm, domain->geometry.aperture_start,
+		    domain->geometry.aperture_end -
+		      domain->geometry.aperture_start + 1);
+
+	iommu_set_fault_handler(domain, etnaviv_fault_handler, gpu->dev);
+
+	return mmu;
+}
+
+size_t etnaviv_iommu_dump_size(struct etnaviv_iommu *iommu)
+{
+	struct etnaviv_iommu_ops *ops;
+
+	ops = container_of(iommu->domain->ops, struct etnaviv_iommu_ops, ops);
+
+	return ops->dump_size(iommu->domain);
+}
+
+void etnaviv_iommu_dump(struct etnaviv_iommu *iommu, void *buf)
+{
+	struct etnaviv_iommu_ops *ops;
+
+	ops = container_of(iommu->domain->ops, struct etnaviv_iommu_ops, ops);
+
+	ops->dump(iommu->domain, buf);
+}
diff --git a/drivers/gpu/drm/etnaviv/etnaviv_mmu.h b/drivers/gpu/drm/etnaviv/etnaviv_mmu.h
new file mode 100644
index 000000000000..fff215a47630
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/etnaviv_mmu.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2015 Etnaviv Project
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __ETNAVIV_MMU_H__
+#define __ETNAVIV_MMU_H__
+
+#include <linux/iommu.h>
+
+enum etnaviv_iommu_version {
+	ETNAVIV_IOMMU_V1 = 0,
+	ETNAVIV_IOMMU_V2,
+};
+
+struct etnaviv_gpu;
+struct etnaviv_vram_mapping;
+
+struct etnaviv_iommu_ops {
+	struct iommu_ops ops;
+	size_t (*dump_size)(struct iommu_domain *);
+	void (*dump)(struct iommu_domain *, void *);
+};
+
+struct etnaviv_iommu {
+	struct etnaviv_gpu *gpu;
+	struct iommu_domain *domain;
+
+	enum etnaviv_iommu_version version;
+
+	/* memory manager for GPU address area */
+	struct mutex lock;
+	struct list_head mappings;
+	struct drm_mm mm;
+	u32 last_iova;
+	bool need_flush;
+};
+
+struct etnaviv_gem_object;
+
+int etnaviv_iommu_attach(struct etnaviv_iommu *iommu, const char **names,
+	int cnt);
+int etnaviv_iommu_map(struct etnaviv_iommu *iommu, u32 iova,
+	struct sg_table *sgt, unsigned len, int prot);
+int etnaviv_iommu_unmap(struct etnaviv_iommu *iommu, u32 iova,
+	struct sg_table *sgt, unsigned len);
+int etnaviv_iommu_map_gem(struct etnaviv_iommu *mmu,
+	struct etnaviv_gem_object *etnaviv_obj, u32 memory_base,
+	struct etnaviv_vram_mapping *mapping);
+void etnaviv_iommu_unmap_gem(struct etnaviv_iommu *mmu,
+	struct etnaviv_vram_mapping *mapping);
+void etnaviv_iommu_destroy(struct etnaviv_iommu *iommu);
+
+size_t etnaviv_iommu_dump_size(struct etnaviv_iommu *iommu);
+void etnaviv_iommu_dump(struct etnaviv_iommu *iommu, void *buf);
+
+struct etnaviv_iommu *etnaviv_iommu_new(struct etnaviv_gpu *gpu,
+	struct iommu_domain *domain, enum etnaviv_iommu_version version);
+
+#endif /* __ETNAVIV_MMU_H__ */
diff --git a/drivers/gpu/drm/etnaviv/state.xml.h b/drivers/gpu/drm/etnaviv/state.xml.h
new file mode 100644
index 000000000000..368218304566
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/state.xml.h
@@ -0,0 +1,351 @@
+#ifndef STATE_XML
+#define STATE_XML
+
+/* Autogenerated file, DO NOT EDIT manually!
+
+This file was generated by the rules-ng-ng headergen tool in this git repository:
+http://0x04.net/cgit/index.cgi/rules-ng-ng
+git clone git://0x04.net/rules-ng-ng
+
+The rules-ng-ng source files this header was generated from are:
+- state.xml    (  18882 bytes, from 2015-03-25 11:42:32)
+- common.xml   (  18437 bytes, from 2015-03-25 11:27:41)
+- state_hi.xml (  23420 bytes, from 2015-03-25 11:47:21)
+- state_2d.xml (  51549 bytes, from 2015-03-25 11:25:06)
+- state_3d.xml (  54600 bytes, from 2015-03-25 11:25:19)
+- state_vg.xml (   5973 bytes, from 2015-03-25 11:26:01)
+
+Copyright (C) 2015
+*/
+
+
+#define VARYING_COMPONENT_USE_UNUSED				0x00000000
+#define VARYING_COMPONENT_USE_USED				0x00000001
+#define VARYING_COMPONENT_USE_POINTCOORD_X			0x00000002
+#define VARYING_COMPONENT_USE_POINTCOORD_Y			0x00000003
+#define FE_VERTEX_STREAM_CONTROL_VERTEX_STRIDE__MASK		0x000000ff
+#define FE_VERTEX_STREAM_CONTROL_VERTEX_STRIDE__SHIFT		0
+#define FE_VERTEX_STREAM_CONTROL_VERTEX_STRIDE(x)		(((x) << FE_VERTEX_STREAM_CONTROL_VERTEX_STRIDE__SHIFT) & FE_VERTEX_STREAM_CONTROL_VERTEX_STRIDE__MASK)
+#define VIVS_FE							0x00000000
+
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG(i0)		       (0x00000600 + 0x4*(i0))
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG__ESIZE			0x00000004
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG__LEN			0x00000010
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE__MASK		0x0000000f
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE__SHIFT		0
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_BYTE			0x00000000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_UNSIGNED_BYTE	0x00000001
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_SHORT		0x00000002
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_UNSIGNED_SHORT	0x00000003
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_INT			0x00000004
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_UNSIGNED_INT		0x00000005
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_FLOAT		0x00000008
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_HALF_FLOAT		0x00000009
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_FIXED		0x0000000b
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_INT_10_10_10_2	0x0000000c
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_TYPE_UNSIGNED_INT_10_10_10_2	0x0000000d
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_ENDIAN__MASK		0x00000030
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_ENDIAN__SHIFT		4
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_ENDIAN(x)			(((x) << VIVS_FE_VERTEX_ELEMENT_CONFIG_ENDIAN__SHIFT) & VIVS_FE_VERTEX_ELEMENT_CONFIG_ENDIAN__MASK)
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NONCONSECUTIVE		0x00000080
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_STREAM__MASK		0x00000700
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_STREAM__SHIFT		8
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_STREAM(x)			(((x) << VIVS_FE_VERTEX_ELEMENT_CONFIG_STREAM__SHIFT) & VIVS_FE_VERTEX_ELEMENT_CONFIG_STREAM__MASK)
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NUM__MASK			0x00003000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NUM__SHIFT		12
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NUM(x)			(((x) << VIVS_FE_VERTEX_ELEMENT_CONFIG_NUM__SHIFT) & VIVS_FE_VERTEX_ELEMENT_CONFIG_NUM__MASK)
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NORMALIZE__MASK		0x0000c000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NORMALIZE__SHIFT		14
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NORMALIZE_OFF		0x00000000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_NORMALIZE_ON		0x00008000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_START__MASK		0x00ff0000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_START__SHIFT		16
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_START(x)			(((x) << VIVS_FE_VERTEX_ELEMENT_CONFIG_START__SHIFT) & VIVS_FE_VERTEX_ELEMENT_CONFIG_START__MASK)
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_END__MASK			0xff000000
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_END__SHIFT		24
+#define VIVS_FE_VERTEX_ELEMENT_CONFIG_END(x)			(((x) << VIVS_FE_VERTEX_ELEMENT_CONFIG_END__SHIFT) & VIVS_FE_VERTEX_ELEMENT_CONFIG_END__MASK)
+
+#define VIVS_FE_CMD_STREAM_BASE_ADDR				0x00000640
+
+#define VIVS_FE_INDEX_STREAM_BASE_ADDR				0x00000644
+
+#define VIVS_FE_INDEX_STREAM_CONTROL				0x00000648
+#define VIVS_FE_INDEX_STREAM_CONTROL_TYPE__MASK			0x00000003
+#define VIVS_FE_INDEX_STREAM_CONTROL_TYPE__SHIFT		0
+#define VIVS_FE_INDEX_STREAM_CONTROL_TYPE_UNSIGNED_CHAR		0x00000000
+#define VIVS_FE_INDEX_STREAM_CONTROL_TYPE_UNSIGNED_SHORT	0x00000001
+#define VIVS_FE_INDEX_STREAM_CONTROL_TYPE_UNSIGNED_INT		0x00000002
+
+#define VIVS_FE_VERTEX_STREAM_BASE_ADDR				0x0000064c
+
+#define VIVS_FE_VERTEX_STREAM_CONTROL				0x00000650
+
+#define VIVS_FE_COMMAND_ADDRESS					0x00000654
+
+#define VIVS_FE_COMMAND_CONTROL					0x00000658
+#define VIVS_FE_COMMAND_CONTROL_PREFETCH__MASK			0x0000ffff
+#define VIVS_FE_COMMAND_CONTROL_PREFETCH__SHIFT			0
+#define VIVS_FE_COMMAND_CONTROL_PREFETCH(x)			(((x) << VIVS_FE_COMMAND_CONTROL_PREFETCH__SHIFT) & VIVS_FE_COMMAND_CONTROL_PREFETCH__MASK)
+#define VIVS_FE_COMMAND_CONTROL_ENABLE				0x00010000
+
+#define VIVS_FE_DMA_STATUS					0x0000065c
+
+#define VIVS_FE_DMA_DEBUG_STATE					0x00000660
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE__MASK			0x0000001f
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE__SHIFT		0
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_IDLE			0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_DEC			0x00000001
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_ADR0			0x00000002
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_LOAD0			0x00000003
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_ADR1			0x00000004
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_LOAD1			0x00000005
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_3DADR			0x00000006
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_3DCMD			0x00000007
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_3DCNTL		0x00000008
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_3DIDXCNTL		0x00000009
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_INITREQDMA		0x0000000a
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_DRAWIDX		0x0000000b
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_DRAW			0x0000000c
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_2DRECT0		0x0000000d
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_2DRECT1		0x0000000e
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_2DDATA0		0x0000000f
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_2DDATA1		0x00000010
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_WAITFIFO		0x00000011
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_WAIT			0x00000012
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_LINK			0x00000013
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_END			0x00000014
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_STATE_STALL			0x00000015
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE__MASK		0x00000300
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE__SHIFT		8
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE_IDLE		0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE_START		0x00000100
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE_REQ		0x00000200
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_DMA_STATE_END		0x00000300
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_FETCH_STATE__MASK		0x00000c00
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_FETCH_STATE__SHIFT		10
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_FETCH_STATE_IDLE		0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_FETCH_STATE_RAMVALID	0x00000400
+#define VIVS_FE_DMA_DEBUG_STATE_CMD_FETCH_STATE_VALID		0x00000800
+#define VIVS_FE_DMA_DEBUG_STATE_REQ_DMA_STATE__MASK		0x00003000
+#define VIVS_FE_DMA_DEBUG_STATE_REQ_DMA_STATE__SHIFT		12
+#define VIVS_FE_DMA_DEBUG_STATE_REQ_DMA_STATE_IDLE		0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_REQ_DMA_STATE_WAITIDX		0x00001000
+#define VIVS_FE_DMA_DEBUG_STATE_REQ_DMA_STATE_CAL		0x00002000
+#define VIVS_FE_DMA_DEBUG_STATE_CAL_STATE__MASK			0x0000c000
+#define VIVS_FE_DMA_DEBUG_STATE_CAL_STATE__SHIFT		14
+#define VIVS_FE_DMA_DEBUG_STATE_CAL_STATE_IDLE			0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_CAL_STATE_LDADR			0x00004000
+#define VIVS_FE_DMA_DEBUG_STATE_CAL_STATE_IDXCALC		0x00008000
+#define VIVS_FE_DMA_DEBUG_STATE_VE_REQ_STATE__MASK		0x00030000
+#define VIVS_FE_DMA_DEBUG_STATE_VE_REQ_STATE__SHIFT		16
+#define VIVS_FE_DMA_DEBUG_STATE_VE_REQ_STATE_IDLE		0x00000000
+#define VIVS_FE_DMA_DEBUG_STATE_VE_REQ_STATE_CKCACHE		0x00010000
+#define VIVS_FE_DMA_DEBUG_STATE_VE_REQ_STATE_MISS		0x00020000
+
+#define VIVS_FE_DMA_ADDRESS					0x00000664
+
+#define VIVS_FE_DMA_LOW						0x00000668
+
+#define VIVS_FE_DMA_HIGH					0x0000066c
+
+#define VIVS_FE_AUTO_FLUSH					0x00000670
+
+#define VIVS_FE_UNK00678					0x00000678
+
+#define VIVS_FE_UNK0067C					0x0000067c
+
+#define VIVS_FE_VERTEX_STREAMS(i0)			       (0x00000000 + 0x4*(i0))
+#define VIVS_FE_VERTEX_STREAMS__ESIZE				0x00000004
+#define VIVS_FE_VERTEX_STREAMS__LEN				0x00000008
+
+#define VIVS_FE_VERTEX_STREAMS_BASE_ADDR(i0)		       (0x00000680 + 0x4*(i0))
+
+#define VIVS_FE_VERTEX_STREAMS_CONTROL(i0)		       (0x000006a0 + 0x4*(i0))
+
+#define VIVS_FE_UNK00700(i0)				       (0x00000700 + 0x4*(i0))
+#define VIVS_FE_UNK00700__ESIZE					0x00000004
+#define VIVS_FE_UNK00700__LEN					0x00000010
+
+#define VIVS_FE_UNK00740(i0)				       (0x00000740 + 0x4*(i0))
+#define VIVS_FE_UNK00740__ESIZE					0x00000004
+#define VIVS_FE_UNK00740__LEN					0x00000010
+
+#define VIVS_FE_UNK00780(i0)				       (0x00000780 + 0x4*(i0))
+#define VIVS_FE_UNK00780__ESIZE					0x00000004
+#define VIVS_FE_UNK00780__LEN					0x00000010
+
+#define VIVS_GL							0x00000000
+
+#define VIVS_GL_PIPE_SELECT					0x00003800
+#define VIVS_GL_PIPE_SELECT_PIPE__MASK				0x00000001
+#define VIVS_GL_PIPE_SELECT_PIPE__SHIFT				0
+#define VIVS_GL_PIPE_SELECT_PIPE(x)				(((x) << VIVS_GL_PIPE_SELECT_PIPE__SHIFT) & VIVS_GL_PIPE_SELECT_PIPE__MASK)
+
+#define VIVS_GL_EVENT						0x00003804
+#define VIVS_GL_EVENT_EVENT_ID__MASK				0x0000001f
+#define VIVS_GL_EVENT_EVENT_ID__SHIFT				0
+#define VIVS_GL_EVENT_EVENT_ID(x)				(((x) << VIVS_GL_EVENT_EVENT_ID__SHIFT) & VIVS_GL_EVENT_EVENT_ID__MASK)
+#define VIVS_GL_EVENT_FROM_FE					0x00000020
+#define VIVS_GL_EVENT_FROM_PE					0x00000040
+#define VIVS_GL_EVENT_SOURCE__MASK				0x00001f00
+#define VIVS_GL_EVENT_SOURCE__SHIFT				8
+#define VIVS_GL_EVENT_SOURCE(x)					(((x) << VIVS_GL_EVENT_SOURCE__SHIFT) & VIVS_GL_EVENT_SOURCE__MASK)
+
+#define VIVS_GL_SEMAPHORE_TOKEN					0x00003808
+#define VIVS_GL_SEMAPHORE_TOKEN_FROM__MASK			0x0000001f
+#define VIVS_GL_SEMAPHORE_TOKEN_FROM__SHIFT			0
+#define VIVS_GL_SEMAPHORE_TOKEN_FROM(x)				(((x) << VIVS_GL_SEMAPHORE_TOKEN_FROM__SHIFT) & VIVS_GL_SEMAPHORE_TOKEN_FROM__MASK)
+#define VIVS_GL_SEMAPHORE_TOKEN_TO__MASK			0x00001f00
+#define VIVS_GL_SEMAPHORE_TOKEN_TO__SHIFT			8
+#define VIVS_GL_SEMAPHORE_TOKEN_TO(x)				(((x) << VIVS_GL_SEMAPHORE_TOKEN_TO__SHIFT) & VIVS_GL_SEMAPHORE_TOKEN_TO__MASK)
+
+#define VIVS_GL_FLUSH_CACHE					0x0000380c
+#define VIVS_GL_FLUSH_CACHE_DEPTH				0x00000001
+#define VIVS_GL_FLUSH_CACHE_COLOR				0x00000002
+#define VIVS_GL_FLUSH_CACHE_TEXTURE				0x00000004
+#define VIVS_GL_FLUSH_CACHE_PE2D				0x00000008
+#define VIVS_GL_FLUSH_CACHE_TEXTUREVS				0x00000010
+#define VIVS_GL_FLUSH_CACHE_SHADER_L1				0x00000020
+#define VIVS_GL_FLUSH_CACHE_SHADER_L2				0x00000040
+
+#define VIVS_GL_FLUSH_MMU					0x00003810
+#define VIVS_GL_FLUSH_MMU_FLUSH_FEMMU				0x00000001
+#define VIVS_GL_FLUSH_MMU_FLUSH_UNK1				0x00000002
+#define VIVS_GL_FLUSH_MMU_FLUSH_UNK2				0x00000004
+#define VIVS_GL_FLUSH_MMU_FLUSH_PEMMU				0x00000008
+#define VIVS_GL_FLUSH_MMU_FLUSH_UNK4				0x00000010
+
+#define VIVS_GL_VERTEX_ELEMENT_CONFIG				0x00003814
+
+#define VIVS_GL_MULTI_SAMPLE_CONFIG				0x00003818
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES__MASK		0x00000003
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES__SHIFT		0
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES_NONE		0x00000000
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES_2X		0x00000001
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES_4X		0x00000002
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_SAMPLES_MASK		0x00000008
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES__MASK		0x000000f0
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES__SHIFT		4
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES(x)		(((x) << VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES__SHIFT) & VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES__MASK)
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_MSAA_ENABLES_MASK		0x00000100
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12__MASK			0x00007000
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12__SHIFT		12
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12(x)			(((x) << VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12__SHIFT) & VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12__MASK)
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK12_MASK			0x00008000
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16__MASK			0x00030000
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16__SHIFT		16
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16(x)			(((x) << VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16__SHIFT) & VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16__MASK)
+#define VIVS_GL_MULTI_SAMPLE_CONFIG_UNK16_MASK			0x00080000
+
+#define VIVS_GL_VARYING_TOTAL_COMPONENTS			0x0000381c
+#define VIVS_GL_VARYING_TOTAL_COMPONENTS_NUM__MASK		0x000000ff
+#define VIVS_GL_VARYING_TOTAL_COMPONENTS_NUM__SHIFT		0
+#define VIVS_GL_VARYING_TOTAL_COMPONENTS_NUM(x)			(((x) << VIVS_GL_VARYING_TOTAL_COMPONENTS_NUM__SHIFT) & VIVS_GL_VARYING_TOTAL_COMPONENTS_NUM__MASK)
+
+#define VIVS_GL_VARYING_NUM_COMPONENTS				0x00003820
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR0__MASK		0x00000007
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR0__SHIFT		0
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR0(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR0__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR0__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR1__MASK		0x00000070
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR1__SHIFT		4
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR1(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR1__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR1__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR2__MASK		0x00000700
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR2__SHIFT		8
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR2(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR2__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR2__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR3__MASK		0x00007000
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR3__SHIFT		12
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR3(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR3__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR3__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR4__MASK		0x00070000
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR4__SHIFT		16
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR4(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR4__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR4__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR5__MASK		0x00700000
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR5__SHIFT		20
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR5(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR5__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR5__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR6__MASK		0x07000000
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR6__SHIFT		24
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR6(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR6__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR6__MASK)
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR7__MASK		0x70000000
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR7__SHIFT		28
+#define VIVS_GL_VARYING_NUM_COMPONENTS_VAR7(x)			(((x) << VIVS_GL_VARYING_NUM_COMPONENTS_VAR7__SHIFT) & VIVS_GL_VARYING_NUM_COMPONENTS_VAR7__MASK)
+
+#define VIVS_GL_VARYING_COMPONENT_USE(i0)		       (0x00003828 + 0x4*(i0))
+#define VIVS_GL_VARYING_COMPONENT_USE__ESIZE			0x00000004
+#define VIVS_GL_VARYING_COMPONENT_USE__LEN			0x00000002
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP0__MASK		0x00000003
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP0__SHIFT		0
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP0(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP0__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP0__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP1__MASK		0x0000000c
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP1__SHIFT		2
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP1(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP1__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP1__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP2__MASK		0x00000030
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP2__SHIFT		4
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP2(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP2__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP2__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP3__MASK		0x000000c0
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP3__SHIFT		6
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP3(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP3__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP3__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP4__MASK		0x00000300
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP4__SHIFT		8
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP4(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP4__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP4__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP5__MASK		0x00000c00
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP5__SHIFT		10
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP5(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP5__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP5__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP6__MASK		0x00003000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP6__SHIFT		12
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP6(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP6__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP6__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP7__MASK		0x0000c000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP7__SHIFT		14
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP7(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP7__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP7__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP8__MASK		0x00030000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP8__SHIFT		16
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP8(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP8__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP8__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP9__MASK		0x000c0000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP9__SHIFT		18
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP9(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP9__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP9__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP10__MASK		0x00300000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP10__SHIFT		20
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP10(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP10__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP10__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP11__MASK		0x00c00000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP11__SHIFT		22
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP11(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP11__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP11__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP12__MASK		0x03000000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP12__SHIFT		24
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP12(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP12__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP12__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP13__MASK		0x0c000000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP13__SHIFT		26
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP13(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP13__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP13__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP14__MASK		0x30000000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP14__SHIFT		28
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP14(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP14__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP14__MASK)
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP15__MASK		0xc0000000
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP15__SHIFT		30
+#define VIVS_GL_VARYING_COMPONENT_USE_COMP15(x)			(((x) << VIVS_GL_VARYING_COMPONENT_USE_COMP15__SHIFT) & VIVS_GL_VARYING_COMPONENT_USE_COMP15__MASK)
+
+#define VIVS_GL_UNK03834					0x00003834
+
+#define VIVS_GL_UNK03838					0x00003838
+
+#define VIVS_GL_API_MODE					0x0000384c
+#define VIVS_GL_API_MODE_OPENGL					0x00000000
+#define VIVS_GL_API_MODE_OPENVG					0x00000001
+#define VIVS_GL_API_MODE_OPENCL					0x00000002
+
+#define VIVS_GL_CONTEXT_POINTER					0x00003850
+
+#define VIVS_GL_UNK03A00					0x00003a00
+
+#define VIVS_GL_STALL_TOKEN					0x00003c00
+#define VIVS_GL_STALL_TOKEN_FROM__MASK				0x0000001f
+#define VIVS_GL_STALL_TOKEN_FROM__SHIFT				0
+#define VIVS_GL_STALL_TOKEN_FROM(x)				(((x) << VIVS_GL_STALL_TOKEN_FROM__SHIFT) & VIVS_GL_STALL_TOKEN_FROM__MASK)
+#define VIVS_GL_STALL_TOKEN_TO__MASK				0x00001f00
+#define VIVS_GL_STALL_TOKEN_TO__SHIFT				8
+#define VIVS_GL_STALL_TOKEN_TO(x)				(((x) << VIVS_GL_STALL_TOKEN_TO__SHIFT) & VIVS_GL_STALL_TOKEN_TO__MASK)
+#define VIVS_GL_STALL_TOKEN_FLIP0				0x40000000
+#define VIVS_GL_STALL_TOKEN_FLIP1				0x80000000
+
+#define VIVS_DUMMY						0x00000000
+
+#define VIVS_DUMMY_DUMMY					0x0003fffc
+
+
+#endif /* STATE_XML */
diff --git a/drivers/gpu/drm/etnaviv/state_hi.xml.h b/drivers/gpu/drm/etnaviv/state_hi.xml.h
new file mode 100644
index 000000000000..6a7de5f1454a
--- /dev/null
+++ b/drivers/gpu/drm/etnaviv/state_hi.xml.h
@@ -0,0 +1,429 @@
+#ifndef STATE_HI_XML
+#define STATE_HI_XML
+
+/* Autogenerated file, DO NOT EDIT manually!
+
+This file was generated by the rules-ng-ng headergen tool in this git repository:
+http://0x04.net/cgit/index.cgi/rules-ng-ng
+git clone git://0x04.net/rules-ng-ng
+
+The rules-ng-ng source files this header was generated from are:
+- state_hi.xml (  24309 bytes, from 2015-12-12 09:02:53)
+- common.xml   (  18437 bytes, from 2015-12-12 09:02:53)
+
+Copyright (C) 2015
+*/
+
+
+#define MMU_EXCEPTION_SLAVE_NOT_PRESENT				0x00000001
+#define MMU_EXCEPTION_PAGE_NOT_PRESENT				0x00000002
+#define MMU_EXCEPTION_WRITE_VIOLATION				0x00000003
+#define VIVS_HI							0x00000000
+
+#define VIVS_HI_CLOCK_CONTROL					0x00000000
+#define VIVS_HI_CLOCK_CONTROL_CLK3D_DIS				0x00000001
+#define VIVS_HI_CLOCK_CONTROL_CLK2D_DIS				0x00000002
+#define VIVS_HI_CLOCK_CONTROL_FSCALE_VAL__MASK			0x000001fc
+#define VIVS_HI_CLOCK_CONTROL_FSCALE_VAL__SHIFT			2
+#define VIVS_HI_CLOCK_CONTROL_FSCALE_VAL(x)			(((x) << VIVS_HI_CLOCK_CONTROL_FSCALE_VAL__SHIFT) & VIVS_HI_CLOCK_CONTROL_FSCALE_VAL__MASK)
+#define VIVS_HI_CLOCK_CONTROL_FSCALE_CMD_LOAD			0x00000200
+#define VIVS_HI_CLOCK_CONTROL_DISABLE_RAM_CLK_GATING		0x00000400
+#define VIVS_HI_CLOCK_CONTROL_DISABLE_DEBUG_REGISTERS		0x00000800
+#define VIVS_HI_CLOCK_CONTROL_SOFT_RESET			0x00001000
+#define VIVS_HI_CLOCK_CONTROL_IDLE_3D				0x00010000
+#define VIVS_HI_CLOCK_CONTROL_IDLE_2D				0x00020000
+#define VIVS_HI_CLOCK_CONTROL_IDLE_VG				0x00040000
+#define VIVS_HI_CLOCK_CONTROL_ISOLATE_GPU			0x00080000
+#define VIVS_HI_CLOCK_CONTROL_DEBUG_PIXEL_PIPE__MASK		0x00f00000
+#define VIVS_HI_CLOCK_CONTROL_DEBUG_PIXEL_PIPE__SHIFT		20
+#define VIVS_HI_CLOCK_CONTROL_DEBUG_PIXEL_PIPE(x)		(((x) << VIVS_HI_CLOCK_CONTROL_DEBUG_PIXEL_PIPE__SHIFT) & VIVS_HI_CLOCK_CONTROL_DEBUG_PIXEL_PIPE__MASK)
+
+#define VIVS_HI_IDLE_STATE					0x00000004
+#define VIVS_HI_IDLE_STATE_FE					0x00000001
+#define VIVS_HI_IDLE_STATE_DE					0x00000002
+#define VIVS_HI_IDLE_STATE_PE					0x00000004
+#define VIVS_HI_IDLE_STATE_SH					0x00000008
+#define VIVS_HI_IDLE_STATE_PA					0x00000010
+#define VIVS_HI_IDLE_STATE_SE					0x00000020
+#define VIVS_HI_IDLE_STATE_RA					0x00000040
+#define VIVS_HI_IDLE_STATE_TX					0x00000080
+#define VIVS_HI_IDLE_STATE_VG					0x00000100
+#define VIVS_HI_IDLE_STATE_IM					0x00000200
+#define VIVS_HI_IDLE_STATE_FP					0x00000400
+#define VIVS_HI_IDLE_STATE_TS					0x00000800
+#define VIVS_HI_IDLE_STATE_AXI_LP				0x80000000
+
+#define VIVS_HI_AXI_CONFIG					0x00000008
+#define VIVS_HI_AXI_CONFIG_AWID__MASK				0x0000000f
+#define VIVS_HI_AXI_CONFIG_AWID__SHIFT				0
+#define VIVS_HI_AXI_CONFIG_AWID(x)				(((x) << VIVS_HI_AXI_CONFIG_AWID__SHIFT) & VIVS_HI_AXI_CONFIG_AWID__MASK)
+#define VIVS_HI_AXI_CONFIG_ARID__MASK				0x000000f0
+#define VIVS_HI_AXI_CONFIG_ARID__SHIFT				4
+#define VIVS_HI_AXI_CONFIG_ARID(x)				(((x) << VIVS_HI_AXI_CONFIG_ARID__SHIFT) & VIVS_HI_AXI_CONFIG_ARID__MASK)
+#define VIVS_HI_AXI_CONFIG_AWCACHE__MASK			0x00000f00
+#define VIVS_HI_AXI_CONFIG_AWCACHE__SHIFT			8
+#define VIVS_HI_AXI_CONFIG_AWCACHE(x)				(((x) << VIVS_HI_AXI_CONFIG_AWCACHE__SHIFT) & VIVS_HI_AXI_CONFIG_AWCACHE__MASK)
+#define VIVS_HI_AXI_CONFIG_ARCACHE__MASK			0x0000f000
+#define VIVS_HI_AXI_CONFIG_ARCACHE__SHIFT			12
+#define VIVS_HI_AXI_CONFIG_ARCACHE(x)				(((x) << VIVS_HI_AXI_CONFIG_ARCACHE__SHIFT) & VIVS_HI_AXI_CONFIG_ARCACHE__MASK)
+
+#define VIVS_HI_AXI_STATUS					0x0000000c
+#define VIVS_HI_AXI_STATUS_WR_ERR_ID__MASK			0x0000000f
+#define VIVS_HI_AXI_STATUS_WR_ERR_ID__SHIFT			0
+#define VIVS_HI_AXI_STATUS_WR_ERR_ID(x)				(((x) << VIVS_HI_AXI_STATUS_WR_ERR_ID__SHIFT) & VIVS_HI_AXI_STATUS_WR_ERR_ID__MASK)
+#define VIVS_HI_AXI_STATUS_RD_ERR_ID__MASK			0x000000f0
+#define VIVS_HI_AXI_STATUS_RD_ERR_ID__SHIFT			4
+#define VIVS_HI_AXI_STATUS_RD_ERR_ID(x)				(((x) << VIVS_HI_AXI_STATUS_RD_ERR_ID__SHIFT) & VIVS_HI_AXI_STATUS_RD_ERR_ID__MASK)
+#define VIVS_HI_AXI_STATUS_DET_WR_ERR				0x00000100
+#define VIVS_HI_AXI_STATUS_DET_RD_ERR				0x00000200
+
+#define VIVS_HI_INTR_ACKNOWLEDGE				0x00000010
+#define VIVS_HI_INTR_ACKNOWLEDGE_INTR_VEC__MASK			0x7fffffff
+#define VIVS_HI_INTR_ACKNOWLEDGE_INTR_VEC__SHIFT		0
+#define VIVS_HI_INTR_ACKNOWLEDGE_INTR_VEC(x)			(((x) << VIVS_HI_INTR_ACKNOWLEDGE_INTR_VEC__SHIFT) & VIVS_HI_INTR_ACKNOWLEDGE_INTR_VEC__MASK)
+#define VIVS_HI_INTR_ACKNOWLEDGE_AXI_BUS_ERROR			0x80000000
+
+#define VIVS_HI_INTR_ENBL					0x00000014
+#define VIVS_HI_INTR_ENBL_INTR_ENBL_VEC__MASK			0xffffffff
+#define VIVS_HI_INTR_ENBL_INTR_ENBL_VEC__SHIFT			0
+#define VIVS_HI_INTR_ENBL_INTR_ENBL_VEC(x)			(((x) << VIVS_HI_INTR_ENBL_INTR_ENBL_VEC__SHIFT) & VIVS_HI_INTR_ENBL_INTR_ENBL_VEC__MASK)
+
+#define VIVS_HI_CHIP_IDENTITY					0x00000018
+#define VIVS_HI_CHIP_IDENTITY_FAMILY__MASK			0xff000000
+#define VIVS_HI_CHIP_IDENTITY_FAMILY__SHIFT			24
+#define VIVS_HI_CHIP_IDENTITY_FAMILY(x)				(((x) << VIVS_HI_CHIP_IDENTITY_FAMILY__SHIFT) & VIVS_HI_CHIP_IDENTITY_FAMILY__MASK)
+#define VIVS_HI_CHIP_IDENTITY_PRODUCT__MASK			0x00ff0000
+#define VIVS_HI_CHIP_IDENTITY_PRODUCT__SHIFT			16
+#define VIVS_HI_CHIP_IDENTITY_PRODUCT(x)			(((x) << VIVS_HI_CHIP_IDENTITY_PRODUCT__SHIFT) & VIVS_HI_CHIP_IDENTITY_PRODUCT__MASK)
+#define VIVS_HI_CHIP_IDENTITY_REVISION__MASK			0x0000f000
+#define VIVS_HI_CHIP_IDENTITY_REVISION__SHIFT			12
+#define VIVS_HI_CHIP_IDENTITY_REVISION(x)			(((x) << VIVS_HI_CHIP_IDENTITY_REVISION__SHIFT) & VIVS_HI_CHIP_IDENTITY_REVISION__MASK)
+
+#define VIVS_HI_CHIP_FEATURE					0x0000001c
+
+#define VIVS_HI_CHIP_MODEL					0x00000020
+
+#define VIVS_HI_CHIP_REV					0x00000024
+
+#define VIVS_HI_CHIP_DATE					0x00000028
+
+#define VIVS_HI_CHIP_TIME					0x0000002c
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_0				0x00000034
+
+#define VIVS_HI_CACHE_CONTROL					0x00000038
+
+#define VIVS_HI_MEMORY_COUNTER_RESET				0x0000003c
+
+#define VIVS_HI_PROFILE_READ_BYTES8				0x00000040
+
+#define VIVS_HI_PROFILE_WRITE_BYTES8				0x00000044
+
+#define VIVS_HI_CHIP_SPECS					0x00000048
+#define VIVS_HI_CHIP_SPECS_STREAM_COUNT__MASK			0x0000000f
+#define VIVS_HI_CHIP_SPECS_STREAM_COUNT__SHIFT			0
+#define VIVS_HI_CHIP_SPECS_STREAM_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_STREAM_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_STREAM_COUNT__MASK)
+#define VIVS_HI_CHIP_SPECS_REGISTER_MAX__MASK			0x000000f0
+#define VIVS_HI_CHIP_SPECS_REGISTER_MAX__SHIFT			4
+#define VIVS_HI_CHIP_SPECS_REGISTER_MAX(x)			(((x) << VIVS_HI_CHIP_SPECS_REGISTER_MAX__SHIFT) & VIVS_HI_CHIP_SPECS_REGISTER_MAX__MASK)
+#define VIVS_HI_CHIP_SPECS_THREAD_COUNT__MASK			0x00000f00
+#define VIVS_HI_CHIP_SPECS_THREAD_COUNT__SHIFT			8
+#define VIVS_HI_CHIP_SPECS_THREAD_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_THREAD_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_THREAD_COUNT__MASK)
+#define VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE__MASK		0x0001f000
+#define VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE__SHIFT		12
+#define VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE(x)			(((x) << VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE__SHIFT) & VIVS_HI_CHIP_SPECS_VERTEX_CACHE_SIZE__MASK)
+#define VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT__MASK		0x01f00000
+#define VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT__SHIFT		20
+#define VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_SHADER_CORE_COUNT__MASK)
+#define VIVS_HI_CHIP_SPECS_PIXEL_PIPES__MASK			0x0e000000
+#define VIVS_HI_CHIP_SPECS_PIXEL_PIPES__SHIFT			25
+#define VIVS_HI_CHIP_SPECS_PIXEL_PIPES(x)			(((x) << VIVS_HI_CHIP_SPECS_PIXEL_PIPES__SHIFT) & VIVS_HI_CHIP_SPECS_PIXEL_PIPES__MASK)
+#define VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE__MASK	0xf0000000
+#define VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE__SHIFT	28
+#define VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE(x)		(((x) << VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE__SHIFT) & VIVS_HI_CHIP_SPECS_VERTEX_OUTPUT_BUFFER_SIZE__MASK)
+
+#define VIVS_HI_PROFILE_WRITE_BURSTS				0x0000004c
+
+#define VIVS_HI_PROFILE_WRITE_REQUESTS				0x00000050
+
+#define VIVS_HI_PROFILE_READ_BURSTS				0x00000058
+
+#define VIVS_HI_PROFILE_READ_REQUESTS				0x0000005c
+
+#define VIVS_HI_PROFILE_READ_LASTS				0x00000060
+
+#define VIVS_HI_GP_OUT0						0x00000064
+
+#define VIVS_HI_GP_OUT1						0x00000068
+
+#define VIVS_HI_GP_OUT2						0x0000006c
+
+#define VIVS_HI_AXI_CONTROL					0x00000070
+#define VIVS_HI_AXI_CONTROL_WR_FULL_BURST_MODE			0x00000001
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_1				0x00000074
+
+#define VIVS_HI_PROFILE_TOTAL_CYCLES				0x00000078
+
+#define VIVS_HI_PROFILE_IDLE_CYCLES				0x0000007c
+
+#define VIVS_HI_CHIP_SPECS_2					0x00000080
+#define VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE__MASK			0x000000ff
+#define VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE__SHIFT			0
+#define VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE(x)			(((x) << VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE__SHIFT) & VIVS_HI_CHIP_SPECS_2_BUFFER_SIZE__MASK)
+#define VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT__MASK		0x0000ff00
+#define VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT__SHIFT		8
+#define VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT(x)		(((x) << VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_2_INSTRUCTION_COUNT__MASK)
+#define VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS__MASK		0xffff0000
+#define VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS__SHIFT		16
+#define VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS(x)			(((x) << VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS__SHIFT) & VIVS_HI_CHIP_SPECS_2_NUM_CONSTANTS__MASK)
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_2				0x00000084
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_3				0x00000088
+
+#define VIVS_HI_CHIP_SPECS_3					0x0000008c
+#define VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT__MASK		0x000001f0
+#define VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT__SHIFT		4
+#define VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_3_VARYINGS_COUNT__MASK)
+#define VIVS_HI_CHIP_SPECS_3_GPU_CORE_COUNT__MASK		0x00000007
+#define VIVS_HI_CHIP_SPECS_3_GPU_CORE_COUNT__SHIFT		0
+#define VIVS_HI_CHIP_SPECS_3_GPU_CORE_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_3_GPU_CORE_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_3_GPU_CORE_COUNT__MASK)
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_4				0x00000094
+
+#define VIVS_HI_CHIP_SPECS_4					0x0000009c
+#define VIVS_HI_CHIP_SPECS_4_STREAM_COUNT__MASK			0x0001f000
+#define VIVS_HI_CHIP_SPECS_4_STREAM_COUNT__SHIFT		12
+#define VIVS_HI_CHIP_SPECS_4_STREAM_COUNT(x)			(((x) << VIVS_HI_CHIP_SPECS_4_STREAM_COUNT__SHIFT) & VIVS_HI_CHIP_SPECS_4_STREAM_COUNT__MASK)
+
+#define VIVS_HI_CHIP_MINOR_FEATURE_5				0x000000a0
+
+#define VIVS_HI_CHIP_PRODUCT_ID					0x000000a8
+
+#define VIVS_PM							0x00000000
+
+#define VIVS_PM_POWER_CONTROLS					0x00000100
+#define VIVS_PM_POWER_CONTROLS_ENABLE_MODULE_CLOCK_GATING	0x00000001
+#define VIVS_PM_POWER_CONTROLS_DISABLE_STALL_MODULE_CLOCK_GATING	0x00000002
+#define VIVS_PM_POWER_CONTROLS_DISABLE_STARVE_MODULE_CLOCK_GATING	0x00000004
+#define VIVS_PM_POWER_CONTROLS_TURN_ON_COUNTER__MASK		0x000000f0
+#define VIVS_PM_POWER_CONTROLS_TURN_ON_COUNTER__SHIFT		4
+#define VIVS_PM_POWER_CONTROLS_TURN_ON_COUNTER(x)		(((x) << VIVS_PM_POWER_CONTROLS_TURN_ON_COUNTER__SHIFT) & VIVS_PM_POWER_CONTROLS_TURN_ON_COUNTER__MASK)
+#define VIVS_PM_POWER_CONTROLS_TURN_OFF_COUNTER__MASK		0xffff0000
+#define VIVS_PM_POWER_CONTROLS_TURN_OFF_COUNTER__SHIFT		16
+#define VIVS_PM_POWER_CONTROLS_TURN_OFF_COUNTER(x)		(((x) << VIVS_PM_POWER_CONTROLS_TURN_OFF_COUNTER__SHIFT) & VIVS_PM_POWER_CONTROLS_TURN_OFF_COUNTER__MASK)
+
+#define VIVS_PM_MODULE_CONTROLS					0x00000104
+#define VIVS_PM_MODULE_CONTROLS_DISABLE_MODULE_CLOCK_GATING_FE	0x00000001
+#define VIVS_PM_MODULE_CONTROLS_DISABLE_MODULE_CLOCK_GATING_DE	0x00000002
+#define VIVS_PM_MODULE_CONTROLS_DISABLE_MODULE_CLOCK_GATING_PE	0x00000004
+
+#define VIVS_PM_MODULE_STATUS					0x00000108
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_FE		0x00000001
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_DE		0x00000002
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_PE		0x00000004
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_SH		0x00000008
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_PA		0x00000010
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_SE		0x00000020
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_RA		0x00000040
+#define VIVS_PM_MODULE_STATUS_MODULE_CLOCK_GATED_TX		0x00000080
+
+#define VIVS_PM_PULSE_EATER					0x0000010c
+
+#define VIVS_MMUv2						0x00000000
+
+#define VIVS_MMUv2_SAFE_ADDRESS					0x00000180
+
+#define VIVS_MMUv2_CONFIGURATION				0x00000184
+#define VIVS_MMUv2_CONFIGURATION_MODE__MASK			0x00000001
+#define VIVS_MMUv2_CONFIGURATION_MODE__SHIFT			0
+#define VIVS_MMUv2_CONFIGURATION_MODE_MODE4_K			0x00000000
+#define VIVS_MMUv2_CONFIGURATION_MODE_MODE1_K			0x00000001
+#define VIVS_MMUv2_CONFIGURATION_MODE_MASK			0x00000008
+#define VIVS_MMUv2_CONFIGURATION_FLUSH__MASK			0x00000010
+#define VIVS_MMUv2_CONFIGURATION_FLUSH__SHIFT			4
+#define VIVS_MMUv2_CONFIGURATION_FLUSH_FLUSH			0x00000010
+#define VIVS_MMUv2_CONFIGURATION_FLUSH_MASK			0x00000080
+#define VIVS_MMUv2_CONFIGURATION_ADDRESS_MASK			0x00000100
+#define VIVS_MMUv2_CONFIGURATION_ADDRESS__MASK			0xfffffc00
+#define VIVS_MMUv2_CONFIGURATION_ADDRESS__SHIFT			10
+#define VIVS_MMUv2_CONFIGURATION_ADDRESS(x)			(((x) << VIVS_MMUv2_CONFIGURATION_ADDRESS__SHIFT) & VIVS_MMUv2_CONFIGURATION_ADDRESS__MASK)
+
+#define VIVS_MMUv2_STATUS					0x00000188
+#define VIVS_MMUv2_STATUS_EXCEPTION0__MASK			0x00000003
+#define VIVS_MMUv2_STATUS_EXCEPTION0__SHIFT			0
+#define VIVS_MMUv2_STATUS_EXCEPTION0(x)				(((x) << VIVS_MMUv2_STATUS_EXCEPTION0__SHIFT) & VIVS_MMUv2_STATUS_EXCEPTION0__MASK)
+#define VIVS_MMUv2_STATUS_EXCEPTION1__MASK			0x00000030
+#define VIVS_MMUv2_STATUS_EXCEPTION1__SHIFT			4
+#define VIVS_MMUv2_STATUS_EXCEPTION1(x)				(((x) << VIVS_MMUv2_STATUS_EXCEPTION1__SHIFT) & VIVS_MMUv2_STATUS_EXCEPTION1__MASK)
+#define VIVS_MMUv2_STATUS_EXCEPTION2__MASK			0x00000300
+#define VIVS_MMUv2_STATUS_EXCEPTION2__SHIFT			8
+#define VIVS_MMUv2_STATUS_EXCEPTION2(x)				(((x) << VIVS_MMUv2_STATUS_EXCEPTION2__SHIFT) & VIVS_MMUv2_STATUS_EXCEPTION2__MASK)
+#define VIVS_MMUv2_STATUS_EXCEPTION3__MASK			0x00003000
+#define VIVS_MMUv2_STATUS_EXCEPTION3__SHIFT			12
+#define VIVS_MMUv2_STATUS_EXCEPTION3(x)				(((x) << VIVS_MMUv2_STATUS_EXCEPTION3__SHIFT) & VIVS_MMUv2_STATUS_EXCEPTION3__MASK)
+
+#define VIVS_MMUv2_CONTROL					0x0000018c
+#define VIVS_MMUv2_CONTROL_ENABLE				0x00000001
+
+#define VIVS_MMUv2_EXCEPTION_ADDR(i0)			       (0x00000190 + 0x4*(i0))
+#define VIVS_MMUv2_EXCEPTION_ADDR__ESIZE			0x00000004
+#define VIVS_MMUv2_EXCEPTION_ADDR__LEN				0x00000004
+
+#define VIVS_MC							0x00000000
+
+#define VIVS_MC_MMU_FE_PAGE_TABLE				0x00000400
+
+#define VIVS_MC_MMU_TX_PAGE_TABLE				0x00000404
+
+#define VIVS_MC_MMU_PE_PAGE_TABLE				0x00000408
+
+#define VIVS_MC_MMU_PEZ_PAGE_TABLE				0x0000040c
+
+#define VIVS_MC_MMU_RA_PAGE_TABLE				0x00000410
+
+#define VIVS_MC_DEBUG_MEMORY					0x00000414
+#define VIVS_MC_DEBUG_MEMORY_SPECIAL_PATCH_GC320		0x00000008
+#define VIVS_MC_DEBUG_MEMORY_FAST_CLEAR_BYPASS			0x00100000
+#define VIVS_MC_DEBUG_MEMORY_COMPRESSION_BYPASS			0x00200000
+
+#define VIVS_MC_MEMORY_BASE_ADDR_RA				0x00000418
+
+#define VIVS_MC_MEMORY_BASE_ADDR_FE				0x0000041c
+
+#define VIVS_MC_MEMORY_BASE_ADDR_TX				0x00000420
+
+#define VIVS_MC_MEMORY_BASE_ADDR_PEZ				0x00000424
+
+#define VIVS_MC_MEMORY_BASE_ADDR_PE				0x00000428
+
+#define VIVS_MC_MEMORY_TIMING_CONTROL				0x0000042c
+
+#define VIVS_MC_MEMORY_FLUSH					0x00000430
+
+#define VIVS_MC_PROFILE_CYCLE_COUNTER				0x00000438
+
+#define VIVS_MC_DEBUG_READ0					0x0000043c
+
+#define VIVS_MC_DEBUG_READ1					0x00000440
+
+#define VIVS_MC_DEBUG_WRITE					0x00000444
+
+#define VIVS_MC_PROFILE_RA_READ					0x00000448
+
+#define VIVS_MC_PROFILE_TX_READ					0x0000044c
+
+#define VIVS_MC_PROFILE_FE_READ					0x00000450
+
+#define VIVS_MC_PROFILE_PE_READ					0x00000454
+
+#define VIVS_MC_PROFILE_DE_READ					0x00000458
+
+#define VIVS_MC_PROFILE_SH_READ					0x0000045c
+
+#define VIVS_MC_PROFILE_PA_READ					0x00000460
+
+#define VIVS_MC_PROFILE_SE_READ					0x00000464
+
+#define VIVS_MC_PROFILE_MC_READ					0x00000468
+
+#define VIVS_MC_PROFILE_HI_READ					0x0000046c
+
+#define VIVS_MC_PROFILE_CONFIG0					0x00000470
+#define VIVS_MC_PROFILE_CONFIG0_FE__MASK			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG0_FE__SHIFT			0
+#define VIVS_MC_PROFILE_CONFIG0_FE_RESET			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG0_DE__MASK			0x00000f00
+#define VIVS_MC_PROFILE_CONFIG0_DE__SHIFT			8
+#define VIVS_MC_PROFILE_CONFIG0_DE_RESET			0x00000f00
+#define VIVS_MC_PROFILE_CONFIG0_PE__MASK			0x000f0000
+#define VIVS_MC_PROFILE_CONFIG0_PE__SHIFT			16
+#define VIVS_MC_PROFILE_CONFIG0_PE_PIXEL_COUNT_KILLED_BY_COLOR_PIPE	0x00000000
+#define VIVS_MC_PROFILE_CONFIG0_PE_PIXEL_COUNT_KILLED_BY_DEPTH_PIPE	0x00010000
+#define VIVS_MC_PROFILE_CONFIG0_PE_PIXEL_COUNT_DRAWN_BY_COLOR_PIPE	0x00020000
+#define VIVS_MC_PROFILE_CONFIG0_PE_PIXEL_COUNT_DRAWN_BY_DEPTH_PIPE	0x00030000
+#define VIVS_MC_PROFILE_CONFIG0_PE_PIXELS_RENDERED_2D		0x000b0000
+#define VIVS_MC_PROFILE_CONFIG0_PE_RESET			0x000f0000
+#define VIVS_MC_PROFILE_CONFIG0_SH__MASK			0x0f000000
+#define VIVS_MC_PROFILE_CONFIG0_SH__SHIFT			24
+#define VIVS_MC_PROFILE_CONFIG0_SH_SHADER_CYCLES		0x04000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_PS_INST_COUNTER		0x07000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_RENDERED_PIXEL_COUNTER	0x08000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_VS_INST_COUNTER		0x09000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_RENDERED_VERTICE_COUNTER	0x0a000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_VTX_BRANCH_INST_COUNTER	0x0b000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_VTX_TEXLD_INST_COUNTER	0x0c000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_PXL_BRANCH_INST_COUNTER	0x0d000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_PXL_TEXLD_INST_COUNTER	0x0e000000
+#define VIVS_MC_PROFILE_CONFIG0_SH_RESET			0x0f000000
+
+#define VIVS_MC_PROFILE_CONFIG1					0x00000474
+#define VIVS_MC_PROFILE_CONFIG1_PA__MASK			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG1_PA__SHIFT			0
+#define VIVS_MC_PROFILE_CONFIG1_PA_INPUT_VTX_COUNTER		0x00000003
+#define VIVS_MC_PROFILE_CONFIG1_PA_INPUT_PRIM_COUNTER		0x00000004
+#define VIVS_MC_PROFILE_CONFIG1_PA_OUTPUT_PRIM_COUNTER		0x00000005
+#define VIVS_MC_PROFILE_CONFIG1_PA_DEPTH_CLIPPED_COUNTER	0x00000006
+#define VIVS_MC_PROFILE_CONFIG1_PA_TRIVIAL_REJECTED_COUNTER	0x00000007
+#define VIVS_MC_PROFILE_CONFIG1_PA_CULLED_COUNTER		0x00000008
+#define VIVS_MC_PROFILE_CONFIG1_PA_RESET			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG1_SE__MASK			0x00000f00
+#define VIVS_MC_PROFILE_CONFIG1_SE__SHIFT			8
+#define VIVS_MC_PROFILE_CONFIG1_SE_CULLED_TRIANGLE_COUNT	0x00000000
+#define VIVS_MC_PROFILE_CONFIG1_SE_CULLED_LINES_COUNT		0x00000100
+#define VIVS_MC_PROFILE_CONFIG1_SE_RESET			0x00000f00
+#define VIVS_MC_PROFILE_CONFIG1_RA__MASK			0x000f0000
+#define VIVS_MC_PROFILE_CONFIG1_RA__SHIFT			16
+#define VIVS_MC_PROFILE_CONFIG1_RA_VALID_PIXEL_COUNT		0x00000000
+#define VIVS_MC_PROFILE_CONFIG1_RA_TOTAL_QUAD_COUNT		0x00010000
+#define VIVS_MC_PROFILE_CONFIG1_RA_VALID_QUAD_COUNT_AFTER_EARLY_Z	0x00020000
+#define VIVS_MC_PROFILE_CONFIG1_RA_TOTAL_PRIMITIVE_COUNT	0x00030000
+#define VIVS_MC_PROFILE_CONFIG1_RA_PIPE_CACHE_MISS_COUNTER	0x00090000
+#define VIVS_MC_PROFILE_CONFIG1_RA_PREFETCH_CACHE_MISS_COUNTER	0x000a0000
+#define VIVS_MC_PROFILE_CONFIG1_RA_CULLED_QUAD_COUNT		0x000b0000
+#define VIVS_MC_PROFILE_CONFIG1_RA_RESET			0x000f0000
+#define VIVS_MC_PROFILE_CONFIG1_TX__MASK			0x0f000000
+#define VIVS_MC_PROFILE_CONFIG1_TX__SHIFT			24
+#define VIVS_MC_PROFILE_CONFIG1_TX_TOTAL_BILINEAR_REQUESTS	0x00000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_TOTAL_TRILINEAR_REQUESTS	0x01000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_TOTAL_DISCARDED_TEXTURE_REQUESTS	0x02000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_TOTAL_TEXTURE_REQUESTS	0x03000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_UNKNOWN			0x04000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_MEM_READ_COUNT		0x05000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_MEM_READ_IN_8B_COUNT		0x06000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_CACHE_MISS_COUNT		0x07000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_CACHE_HIT_TEXEL_COUNT	0x08000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_CACHE_MISS_TEXEL_COUNT	0x09000000
+#define VIVS_MC_PROFILE_CONFIG1_TX_RESET			0x0f000000
+
+#define VIVS_MC_PROFILE_CONFIG2					0x00000478
+#define VIVS_MC_PROFILE_CONFIG2_MC__MASK			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG2_MC__SHIFT			0
+#define VIVS_MC_PROFILE_CONFIG2_MC_TOTAL_READ_REQ_8B_FROM_PIPELINE	0x00000001
+#define VIVS_MC_PROFILE_CONFIG2_MC_TOTAL_READ_REQ_8B_FROM_IP	0x00000002
+#define VIVS_MC_PROFILE_CONFIG2_MC_TOTAL_WRITE_REQ_8B_FROM_PIPELINE	0x00000003
+#define VIVS_MC_PROFILE_CONFIG2_MC_RESET			0x0000000f
+#define VIVS_MC_PROFILE_CONFIG2_HI__MASK			0x00000f00
+#define VIVS_MC_PROFILE_CONFIG2_HI__SHIFT			8
+#define VIVS_MC_PROFILE_CONFIG2_HI_AXI_CYCLES_READ_REQUEST_STALLED	0x00000000
+#define VIVS_MC_PROFILE_CONFIG2_HI_AXI_CYCLES_WRITE_REQUEST_STALLED	0x00000100
+#define VIVS_MC_PROFILE_CONFIG2_HI_AXI_CYCLES_WRITE_DATA_STALLED	0x00000200
+#define VIVS_MC_PROFILE_CONFIG2_HI_RESET			0x00000f00
+
+#define VIVS_MC_PROFILE_CONFIG3					0x0000047c
+
+#define VIVS_MC_BUS_CONFIG					0x00000480
+#define VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG__MASK			0x0000000f
+#define VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG__SHIFT			0
+#define VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG(x)			(((x) << VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG__SHIFT) & VIVS_MC_BUS_CONFIG_FE_BUS_CONFIG__MASK)
+#define VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG__MASK			0x000000f0
+#define VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG__SHIFT			4
+#define VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG(x)			(((x) << VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG__SHIFT) & VIVS_MC_BUS_CONFIG_TX_BUS_CONFIG__MASK)
+
+#define VIVS_MC_START_COMPOSITION				0x00000554
+
+#define VIVS_MC_128B_MERGE					0x00000558
+
+
+#endif /* STATE_HI_XML */
diff --git a/drivers/gpu/drm/exynos/Kconfig b/drivers/gpu/drm/exynos/Kconfig
index 96e86cf4455b..f17d39279596 100644
--- a/drivers/gpu/drm/exynos/Kconfig
+++ b/drivers/gpu/drm/exynos/Kconfig
@@ -1,6 +1,6 @@
 config DRM_EXYNOS
 	tristate "DRM Support for Samsung SoC EXYNOS Series"
-	depends on OF && DRM && (PLAT_SAMSUNG || ARCH_MULTIPLATFORM)
+	depends on OF && DRM && (ARCH_S3C64XX || ARCH_EXYNOS || ARCH_MULTIPLATFORM)
 	select DRM_KMS_HELPER
 	select DRM_KMS_FB_HELPER
 	select FB_CFB_FILLRECT
@@ -118,7 +118,7 @@ config DRM_EXYNOS_ROTATOR
 
 config DRM_EXYNOS_GSC
 	bool "GScaler"
-	depends on DRM_EXYNOS_IPP && ARCH_EXYNOS5 && !ARCH_MULTIPLATFORM
+	depends on DRM_EXYNOS_IPP && ARCH_EXYNOS5 && !VIDEO_SAMSUNG_EXYNOS_GSC
 	help
 	  Choose this option if you want to use Exynos GSC for DRM.
 
diff --git a/drivers/gpu/drm/exynos/exynos5433_drm_decon.c b/drivers/gpu/drm/exynos/exynos5433_drm_decon.c
index fbe1b3174f75..162ab93e99cb 100644
--- a/drivers/gpu/drm/exynos/exynos5433_drm_decon.c
+++ b/drivers/gpu/drm/exynos/exynos5433_drm_decon.c
@@ -21,11 +21,11 @@
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_crtc.h"
+#include "exynos_drm_fb.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_iommu.h"
 
 #define WINDOWS_NR	3
-#define CURSOR_WIN	2
 #define MIN_FB_WIDTH_FOR_16WORD_BURST	128
 
 static const char * const decon_clks_name[] = {
@@ -56,6 +56,7 @@ struct decon_context {
 	struct drm_device		*drm_dev;
 	struct exynos_drm_crtc		*crtc;
 	struct exynos_drm_plane		planes[WINDOWS_NR];
+	struct exynos_drm_plane_config	configs[WINDOWS_NR];
 	void __iomem			*addr;
 	struct clk			*clks[ARRAY_SIZE(decon_clks_name)];
 	int				pipe;
@@ -71,6 +72,12 @@ static const uint32_t decon_formats[] = {
 	DRM_FORMAT_ARGB8888,
 };
 
+static const enum drm_plane_type decon_win_types[WINDOWS_NR] = {
+	DRM_PLANE_TYPE_PRIMARY,
+	DRM_PLANE_TYPE_OVERLAY,
+	DRM_PLANE_TYPE_CURSOR,
+};
+
 static inline void decon_set_bits(struct decon_context *ctx, u32 reg, u32 mask,
 				  u32 val)
 {
@@ -86,7 +93,7 @@ static int decon_enable_vblank(struct exynos_drm_crtc *crtc)
 	if (test_bit(BIT_SUSPENDED, &ctx->flags))
 		return -EPERM;
 
-	if (test_and_set_bit(BIT_IRQS_ENABLED, &ctx->flags)) {
+	if (!test_and_set_bit(BIT_IRQS_ENABLED, &ctx->flags)) {
 		val = VIDINTCON0_INTEN;
 		if (ctx->out_type == IFTYPE_I80)
 			val |= VIDINTCON0_FRAMEDONE;
@@ -241,15 +248,16 @@ static void decon_shadow_protect_win(struct decon_context *ctx, int win,
 		       protect ? ~0 : 0);
 }
 
-static void decon_atomic_begin(struct exynos_drm_crtc *crtc,
-					struct exynos_drm_plane *plane)
+static void decon_atomic_begin(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
+	int i;
 
 	if (test_bit(BIT_SUSPENDED, &ctx->flags))
 		return;
 
-	decon_shadow_protect_win(ctx, plane->zpos, true);
+	for (i = ctx->first_win; i < WINDOWS_NR; i++)
+		decon_shadow_protect_win(ctx, i, true);
 }
 
 #define BIT_VAL(x, e, s) (((x) & ((1 << ((e) - (s) + 1)) - 1)) << (s))
@@ -259,21 +267,24 @@ static void decon_atomic_begin(struct exynos_drm_crtc *crtc,
 static void decon_update_plane(struct exynos_drm_crtc *crtc,
 			       struct exynos_drm_plane *plane)
 {
+	struct exynos_drm_plane_state *state =
+				to_exynos_plane_state(plane->base.state);
 	struct decon_context *ctx = crtc->ctx;
-	struct drm_plane_state *state = plane->base.state;
-	unsigned int win = plane->zpos;
-	unsigned int bpp = state->fb->bits_per_pixel >> 3;
-	unsigned int pitch = state->fb->pitches[0];
+	struct drm_framebuffer *fb = state->base.fb;
+	unsigned int win = plane->index;
+	unsigned int bpp = fb->bits_per_pixel >> 3;
+	unsigned int pitch = fb->pitches[0];
+	dma_addr_t dma_addr = exynos_drm_fb_dma_addr(fb, 0);
 	u32 val;
 
 	if (test_bit(BIT_SUSPENDED, &ctx->flags))
 		return;
 
-	val = COORDINATE_X(plane->crtc_x) | COORDINATE_Y(plane->crtc_y);
+	val = COORDINATE_X(state->crtc.x) | COORDINATE_Y(state->crtc.y);
 	writel(val, ctx->addr + DECON_VIDOSDxA(win));
 
-	val = COORDINATE_X(plane->crtc_x + plane->crtc_w - 1) |
-		COORDINATE_Y(plane->crtc_y + plane->crtc_h - 1);
+	val = COORDINATE_X(state->crtc.x + state->crtc.w - 1) |
+		COORDINATE_Y(state->crtc.y + state->crtc.h - 1);
 	writel(val, ctx->addr + DECON_VIDOSDxB(win));
 
 	val = VIDOSD_Wx_ALPHA_R_F(0x0) | VIDOSD_Wx_ALPHA_G_F(0x0) |
@@ -284,20 +295,20 @@ static void decon_update_plane(struct exynos_drm_crtc *crtc,
 		VIDOSD_Wx_ALPHA_B_F(0x0);
 	writel(val, ctx->addr + DECON_VIDOSDxD(win));
 
-	writel(plane->dma_addr[0], ctx->addr + DECON_VIDW0xADD0B0(win));
+	writel(dma_addr, ctx->addr + DECON_VIDW0xADD0B0(win));
 
-	val = plane->dma_addr[0] + pitch * plane->crtc_h;
+	val = dma_addr + pitch * state->src.h;
 	writel(val, ctx->addr + DECON_VIDW0xADD1B0(win));
 
 	if (ctx->out_type != IFTYPE_HDMI)
-		val = BIT_VAL(pitch - plane->crtc_w * bpp, 27, 14)
-			| BIT_VAL(plane->crtc_w * bpp, 13, 0);
+		val = BIT_VAL(pitch - state->crtc.w * bpp, 27, 14)
+			| BIT_VAL(state->crtc.w * bpp, 13, 0);
 	else
-		val = BIT_VAL(pitch - plane->crtc_w * bpp, 29, 15)
-			| BIT_VAL(plane->crtc_w * bpp, 14, 0);
+		val = BIT_VAL(pitch - state->crtc.w * bpp, 29, 15)
+			| BIT_VAL(state->crtc.w * bpp, 14, 0);
 	writel(val, ctx->addr + DECON_VIDW0xADD2(win));
 
-	decon_win_set_pixfmt(ctx, win, state->fb);
+	decon_win_set_pixfmt(ctx, win, fb);
 
 	/* window enable */
 	decon_set_bits(ctx, DECON_WINCONx(win), WINCONx_ENWIN_F, ~0);
@@ -310,7 +321,7 @@ static void decon_disable_plane(struct exynos_drm_crtc *crtc,
 				struct exynos_drm_plane *plane)
 {
 	struct decon_context *ctx = crtc->ctx;
-	unsigned int win = plane->zpos;
+	unsigned int win = plane->index;
 
 	if (test_bit(BIT_SUSPENDED, &ctx->flags))
 		return;
@@ -326,15 +337,16 @@ static void decon_disable_plane(struct exynos_drm_crtc *crtc,
 	decon_set_bits(ctx, DECON_UPDATE, STANDALONE_UPDATE_F, ~0);
 }
 
-static void decon_atomic_flush(struct exynos_drm_crtc *crtc,
-				struct exynos_drm_plane *plane)
+static void decon_atomic_flush(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
+	int i;
 
 	if (test_bit(BIT_SUSPENDED, &ctx->flags))
 		return;
 
-	decon_shadow_protect_win(ctx, plane->zpos, false);
+	for (i = ctx->first_win; i < WINDOWS_NR; i++)
+		decon_shadow_protect_win(ctx, i, false);
 
 	if (ctx->out_type == IFTYPE_I80)
 		set_bit(BIT_WIN_UPDATED, &ctx->flags);
@@ -377,20 +389,12 @@ static void decon_swreset(struct decon_context *ctx)
 static void decon_enable(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
-	int ret;
-	int i;
 
 	if (!test_and_clear_bit(BIT_SUSPENDED, &ctx->flags))
 		return;
 
 	pm_runtime_get_sync(ctx->dev);
 
-	for (i = 0; i < ARRAY_SIZE(decon_clks_name); i++) {
-		ret = clk_prepare_enable(ctx->clks[i]);
-		if (ret < 0)
-			goto err;
-	}
-
 	set_bit(BIT_CLKS_ENABLED, &ctx->flags);
 
 	/* if vblank was enabled status, enable it again. */
@@ -398,13 +402,6 @@ static void decon_enable(struct exynos_drm_crtc *crtc)
 		decon_enable_vblank(ctx->crtc);
 
 	decon_commit(ctx->crtc);
-
-	return;
-err:
-	while (--i >= 0)
-		clk_disable_unprepare(ctx->clks[i]);
-
-	set_bit(BIT_SUSPENDED, &ctx->flags);
 }
 
 static void decon_disable(struct exynos_drm_crtc *crtc)
@@ -425,9 +422,6 @@ static void decon_disable(struct exynos_drm_crtc *crtc)
 
 	decon_swreset(ctx);
 
-	for (i = 0; i < ARRAY_SIZE(decon_clks_name); i++)
-		clk_disable_unprepare(ctx->clks[i]);
-
 	clear_bit(BIT_CLKS_ENABLED, &ctx->flags);
 
 	pm_runtime_put_sync(ctx->dev);
@@ -478,7 +472,6 @@ err:
 static struct exynos_drm_crtc_ops decon_crtc_ops = {
 	.enable			= decon_enable,
 	.disable		= decon_disable,
-	.commit			= decon_commit,
 	.enable_vblank		= decon_enable_vblank,
 	.disable_vblank		= decon_disable_vblank,
 	.atomic_begin		= decon_atomic_begin,
@@ -495,7 +488,6 @@ static int decon_bind(struct device *dev, struct device *master, void *data)
 	struct exynos_drm_private *priv = drm_dev->dev_private;
 	struct exynos_drm_plane *exynos_plane;
 	enum exynos_drm_output_type out_type;
-	enum drm_plane_type type;
 	unsigned int win;
 	int ret;
 
@@ -505,10 +497,13 @@ static int decon_bind(struct device *dev, struct device *master, void *data)
 	for (win = ctx->first_win; win < WINDOWS_NR; win++) {
 		int tmp = (win == ctx->first_win) ? 0 : win;
 
-		type = exynos_plane_get_type(tmp, CURSOR_WIN);
-		ret = exynos_plane_init(drm_dev, &ctx->planes[win],
-				1 << ctx->pipe, type, decon_formats,
-				ARRAY_SIZE(decon_formats), win);
+		ctx->configs[win].pixel_formats = decon_formats;
+		ctx->configs[win].num_pixel_formats = ARRAY_SIZE(decon_formats);
+		ctx->configs[win].zpos = win;
+		ctx->configs[win].type = decon_win_types[tmp];
+
+		ret = exynos_plane_init(drm_dev, &ctx->planes[win], win,
+					1 << ctx->pipe, &ctx->configs[win]);
 		if (ret)
 			return ret;
 	}
@@ -581,6 +576,44 @@ out:
 	return IRQ_HANDLED;
 }
 
+#ifdef CONFIG_PM
+static int exynos5433_decon_suspend(struct device *dev)
+{
+	struct decon_context *ctx = dev_get_drvdata(dev);
+	int i = ARRAY_SIZE(decon_clks_name);
+
+	while (--i >= 0)
+		clk_disable_unprepare(ctx->clks[i]);
+
+	return 0;
+}
+
+static int exynos5433_decon_resume(struct device *dev)
+{
+	struct decon_context *ctx = dev_get_drvdata(dev);
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(decon_clks_name); i++) {
+		ret = clk_prepare_enable(ctx->clks[i]);
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+
+err:
+	while (--i >= 0)
+		clk_disable_unprepare(ctx->clks[i]);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops exynos5433_decon_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos5433_decon_suspend, exynos5433_decon_resume,
+			   NULL)
+};
+
 static const struct of_device_id exynos5433_decon_driver_dt_match[] = {
 	{
 		.compatible = "samsung,exynos5433-decon",
@@ -684,6 +717,7 @@ struct platform_driver exynos5433_decon_driver = {
 	.remove		= exynos5433_decon_remove,
 	.driver		= {
 		.name	= "exynos5433-decon",
+		.pm	= &exynos5433_decon_pm_ops,
 		.of_match_table = exynos5433_decon_driver_dt_match,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos7_drm_decon.c b/drivers/gpu/drm/exynos/exynos7_drm_decon.c
index ead2b16e237d..52bda3b42fe0 100644
--- a/drivers/gpu/drm/exynos/exynos7_drm_decon.c
+++ b/drivers/gpu/drm/exynos/exynos7_drm_decon.c
@@ -30,6 +30,7 @@
 #include "exynos_drm_crtc.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_drv.h"
+#include "exynos_drm_fb.h"
 #include "exynos_drm_fbdev.h"
 #include "exynos_drm_iommu.h"
 
@@ -40,13 +41,13 @@
 #define MIN_FB_WIDTH_FOR_16WORD_BURST 128
 
 #define WINDOWS_NR	2
-#define CURSOR_WIN	1
 
 struct decon_context {
 	struct device			*dev;
 	struct drm_device		*drm_dev;
 	struct exynos_drm_crtc		*crtc;
 	struct exynos_drm_plane		planes[WINDOWS_NR];
+	struct exynos_drm_plane_config	configs[WINDOWS_NR];
 	struct clk			*pclk;
 	struct clk			*aclk;
 	struct clk			*eclk;
@@ -81,6 +82,11 @@ static const uint32_t decon_formats[] = {
 	DRM_FORMAT_BGRA8888,
 };
 
+static const enum drm_plane_type decon_win_types[WINDOWS_NR] = {
+	DRM_PLANE_TYPE_PRIMARY,
+	DRM_PLANE_TYPE_CURSOR,
+};
+
 static void decon_wait_for_vblank(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
@@ -119,13 +125,8 @@ static void decon_clear_channels(struct exynos_drm_crtc *crtc)
 	}
 
 	/* Wait for vsync, as disable channel takes effect at next vsync */
-	if (ch_enabled) {
-		unsigned int state = ctx->suspended;
-
-		ctx->suspended = 0;
+	if (ch_enabled)
 		decon_wait_for_vblank(ctx->crtc);
-		ctx->suspended = state;
-	}
 }
 
 static int decon_ctx_initialize(struct decon_context *ctx,
@@ -384,30 +385,32 @@ static void decon_shadow_protect_win(struct decon_context *ctx,
 	writel(val, ctx->regs + SHADOWCON);
 }
 
-static void decon_atomic_begin(struct exynos_drm_crtc *crtc,
-					struct exynos_drm_plane *plane)
+static void decon_atomic_begin(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
+	int i;
 
 	if (ctx->suspended)
 		return;
 
-	decon_shadow_protect_win(ctx, plane->zpos, true);
+	for (i = 0; i < WINDOWS_NR; i++)
+		decon_shadow_protect_win(ctx, i, true);
 }
 
 static void decon_update_plane(struct exynos_drm_crtc *crtc,
 			       struct exynos_drm_plane *plane)
 {
+	struct exynos_drm_plane_state *state =
+				to_exynos_plane_state(plane->base.state);
 	struct decon_context *ctx = crtc->ctx;
-	struct drm_display_mode *mode = &crtc->base.state->adjusted_mode;
-	struct drm_plane_state *state = plane->base.state;
+	struct drm_framebuffer *fb = state->base.fb;
 	int padding;
 	unsigned long val, alpha;
 	unsigned int last_x;
 	unsigned int last_y;
-	unsigned int win = plane->zpos;
-	unsigned int bpp = state->fb->bits_per_pixel >> 3;
-	unsigned int pitch = state->fb->pitches[0];
+	unsigned int win = plane->index;
+	unsigned int bpp = fb->bits_per_pixel >> 3;
+	unsigned int pitch = fb->pitches[0];
 
 	if (ctx->suspended)
 		return;
@@ -423,41 +426,32 @@ static void decon_update_plane(struct exynos_drm_crtc *crtc,
 	 */
 
 	/* buffer start address */
-	val = (unsigned long)plane->dma_addr[0];
+	val = (unsigned long)exynos_drm_fb_dma_addr(fb, 0);
 	writel(val, ctx->regs + VIDW_BUF_START(win));
 
-	padding = (pitch / bpp) - state->fb->width;
+	padding = (pitch / bpp) - fb->width;
 
 	/* buffer size */
-	writel(state->fb->width + padding, ctx->regs + VIDW_WHOLE_X(win));
-	writel(state->fb->height, ctx->regs + VIDW_WHOLE_Y(win));
+	writel(fb->width + padding, ctx->regs + VIDW_WHOLE_X(win));
+	writel(fb->height, ctx->regs + VIDW_WHOLE_Y(win));
 
 	/* offset from the start of the buffer to read */
-	writel(plane->src_x, ctx->regs + VIDW_OFFSET_X(win));
-	writel(plane->src_y, ctx->regs + VIDW_OFFSET_Y(win));
+	writel(state->src.x, ctx->regs + VIDW_OFFSET_X(win));
+	writel(state->src.y, ctx->regs + VIDW_OFFSET_Y(win));
 
 	DRM_DEBUG_KMS("start addr = 0x%lx\n",
 			(unsigned long)val);
 	DRM_DEBUG_KMS("ovl_width = %d, ovl_height = %d\n",
-			plane->crtc_w, plane->crtc_h);
-
-	/*
-	 * OSD position.
-	 * In case the window layout goes of LCD layout, DECON fails.
-	 */
-	if ((plane->crtc_x + plane->crtc_w) > mode->hdisplay)
-		plane->crtc_x = mode->hdisplay - plane->crtc_w;
-	if ((plane->crtc_y + plane->crtc_h) > mode->vdisplay)
-		plane->crtc_y = mode->vdisplay - plane->crtc_h;
+			state->crtc.w, state->crtc.h);
 
-	val = VIDOSDxA_TOPLEFT_X(plane->crtc_x) |
-		VIDOSDxA_TOPLEFT_Y(plane->crtc_y);
+	val = VIDOSDxA_TOPLEFT_X(state->crtc.x) |
+		VIDOSDxA_TOPLEFT_Y(state->crtc.y);
 	writel(val, ctx->regs + VIDOSD_A(win));
 
-	last_x = plane->crtc_x + plane->crtc_w;
+	last_x = state->crtc.x + state->crtc.w;
 	if (last_x)
 		last_x--;
-	last_y = plane->crtc_y + plane->crtc_h;
+	last_y = state->crtc.y + state->crtc.h;
 	if (last_y)
 		last_y--;
 
@@ -466,7 +460,7 @@ static void decon_update_plane(struct exynos_drm_crtc *crtc,
 	writel(val, ctx->regs + VIDOSD_B(win));
 
 	DRM_DEBUG_KMS("osd pos: tx = %d, ty = %d, bx = %d, by = %d\n",
-			plane->crtc_x, plane->crtc_y, last_x, last_y);
+			state->crtc.x, state->crtc.y, last_x, last_y);
 
 	/* OSD alpha */
 	alpha = VIDOSDxC_ALPHA0_R_F(0x0) |
@@ -481,7 +475,7 @@ static void decon_update_plane(struct exynos_drm_crtc *crtc,
 
 	writel(alpha, ctx->regs + VIDOSD_D(win));
 
-	decon_win_set_pixfmt(ctx, win, state->fb);
+	decon_win_set_pixfmt(ctx, win, fb);
 
 	/* hardware window 0 doesn't support color key. */
 	if (win != 0)
@@ -505,7 +499,7 @@ static void decon_disable_plane(struct exynos_drm_crtc *crtc,
 				struct exynos_drm_plane *plane)
 {
 	struct decon_context *ctx = crtc->ctx;
-	unsigned int win = plane->zpos;
+	unsigned int win = plane->index;
 	u32 val;
 
 	if (ctx->suspended)
@@ -524,15 +518,16 @@ static void decon_disable_plane(struct exynos_drm_crtc *crtc,
 	writel(val, ctx->regs + DECON_UPDATE);
 }
 
-static void decon_atomic_flush(struct exynos_drm_crtc *crtc,
-					struct exynos_drm_plane *plane)
+static void decon_atomic_flush(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
+	int i;
 
 	if (ctx->suspended)
 		return;
 
-	decon_shadow_protect_win(ctx, plane->zpos, false);
+	for (i = 0; i < WINDOWS_NR; i++)
+		decon_shadow_protect_win(ctx, i, false);
 }
 
 static void decon_init(struct decon_context *ctx)
@@ -555,39 +550,12 @@ static void decon_init(struct decon_context *ctx)
 static void decon_enable(struct exynos_drm_crtc *crtc)
 {
 	struct decon_context *ctx = crtc->ctx;
-	int ret;
 
 	if (!ctx->suspended)
 		return;
 
-	ctx->suspended = false;
-
 	pm_runtime_get_sync(ctx->dev);
 
-	ret = clk_prepare_enable(ctx->pclk);
-	if (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the pclk [%d]\n", ret);
-		return;
-	}
-
-	ret = clk_prepare_enable(ctx->aclk);
-	if (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the aclk [%d]\n", ret);
-		return;
-	}
-
-	ret = clk_prepare_enable(ctx->eclk);
-	if  (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the eclk [%d]\n", ret);
-		return;
-	}
-
-	ret = clk_prepare_enable(ctx->vclk);
-	if  (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the vclk [%d]\n", ret);
-		return;
-	}
-
 	decon_init(ctx);
 
 	/* if vblank was enabled status, enable it again. */
@@ -595,6 +563,8 @@ static void decon_enable(struct exynos_drm_crtc *crtc)
 		decon_enable_vblank(ctx->crtc);
 
 	decon_commit(ctx->crtc);
+
+	ctx->suspended = false;
 }
 
 static void decon_disable(struct exynos_drm_crtc *crtc)
@@ -613,11 +583,6 @@ static void decon_disable(struct exynos_drm_crtc *crtc)
 	for (i = 0; i < WINDOWS_NR; i++)
 		decon_disable_plane(crtc, &ctx->planes[i]);
 
-	clk_disable_unprepare(ctx->vclk);
-	clk_disable_unprepare(ctx->eclk);
-	clk_disable_unprepare(ctx->aclk);
-	clk_disable_unprepare(ctx->pclk);
-
 	pm_runtime_put_sync(ctx->dev);
 
 	ctx->suspended = true;
@@ -679,8 +644,7 @@ static int decon_bind(struct device *dev, struct device *master, void *data)
 	struct decon_context *ctx = dev_get_drvdata(dev);
 	struct drm_device *drm_dev = data;
 	struct exynos_drm_plane *exynos_plane;
-	enum drm_plane_type type;
-	unsigned int zpos;
+	unsigned int i;
 	int ret;
 
 	ret = decon_ctx_initialize(ctx, drm_dev);
@@ -689,11 +653,14 @@ static int decon_bind(struct device *dev, struct device *master, void *data)
 		return ret;
 	}
 
-	for (zpos = 0; zpos < WINDOWS_NR; zpos++) {
-		type = exynos_plane_get_type(zpos, CURSOR_WIN);
-		ret = exynos_plane_init(drm_dev, &ctx->planes[zpos],
-					1 << ctx->pipe, type, decon_formats,
-					ARRAY_SIZE(decon_formats), zpos);
+	for (i = 0; i < WINDOWS_NR; i++) {
+		ctx->configs[i].pixel_formats = decon_formats;
+		ctx->configs[i].num_pixel_formats = ARRAY_SIZE(decon_formats);
+		ctx->configs[i].zpos = i;
+		ctx->configs[i].type = decon_win_types[i];
+
+		ret = exynos_plane_init(drm_dev, &ctx->planes[i], i,
+					1 << ctx->pipe, &ctx->configs[i]);
 		if (ret)
 			return ret;
 	}
@@ -843,11 +810,63 @@ static int decon_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int exynos7_decon_suspend(struct device *dev)
+{
+	struct decon_context *ctx = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(ctx->vclk);
+	clk_disable_unprepare(ctx->eclk);
+	clk_disable_unprepare(ctx->aclk);
+	clk_disable_unprepare(ctx->pclk);
+
+	return 0;
+}
+
+static int exynos7_decon_resume(struct device *dev)
+{
+	struct decon_context *ctx = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(ctx->pclk);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the pclk [%d]\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(ctx->aclk);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the aclk [%d]\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(ctx->eclk);
+	if  (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the eclk [%d]\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(ctx->vclk);
+	if  (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the vclk [%d]\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops exynos7_decon_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos7_decon_suspend, exynos7_decon_resume,
+			   NULL)
+};
+
 struct platform_driver decon_driver = {
 	.probe		= decon_probe,
 	.remove		= decon_remove,
 	.driver		= {
 		.name	= "exynos-decon",
+		.pm	= &exynos7_decon_pm_ops,
 		.of_match_table = decon_driver_dt_match,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_dp_core.c b/drivers/gpu/drm/exynos/exynos_dp_core.c
index 124fb9a56f02..673164b331c8 100644
--- a/drivers/gpu/drm/exynos/exynos_dp_core.c
+++ b/drivers/gpu/drm/exynos/exynos_dp_core.c
@@ -953,7 +953,7 @@ static void exynos_dp_connector_destroy(struct drm_connector *connector)
 	drm_connector_cleanup(connector);
 }
 
-static struct drm_connector_funcs exynos_dp_connector_funcs = {
+static const struct drm_connector_funcs exynos_dp_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = exynos_dp_detect,
@@ -998,7 +998,7 @@ static struct drm_encoder *exynos_dp_best_encoder(
 	return &dp->encoder;
 }
 
-static struct drm_connector_helper_funcs exynos_dp_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs exynos_dp_connector_helper_funcs = {
 	.get_modes = exynos_dp_get_modes,
 	.best_encoder = exynos_dp_best_encoder,
 };
@@ -1009,9 +1009,9 @@ static int exynos_drm_attach_lcd_bridge(struct exynos_dp_device *dp,
 {
 	int ret;
 
-	encoder->bridge = dp->bridge;
-	dp->bridge->encoder = encoder;
-	ret = drm_bridge_attach(encoder->dev, dp->bridge);
+	encoder->bridge->next = dp->ptn_bridge;
+	dp->ptn_bridge->encoder = encoder;
+	ret = drm_bridge_attach(encoder->dev, dp->ptn_bridge);
 	if (ret) {
 		DRM_ERROR("Failed to attach bridge to drm\n");
 		return ret;
@@ -1020,14 +1020,15 @@ static int exynos_drm_attach_lcd_bridge(struct exynos_dp_device *dp,
 	return 0;
 }
 
-static int exynos_dp_create_connector(struct drm_encoder *encoder)
+static int exynos_dp_bridge_attach(struct drm_bridge *bridge)
 {
-	struct exynos_dp_device *dp = encoder_to_dp(encoder);
+	struct exynos_dp_device *dp = bridge->driver_private;
+	struct drm_encoder *encoder = &dp->encoder;
 	struct drm_connector *connector = &dp->connector;
 	int ret;
 
 	/* Pre-empt DP connector creation if there's a bridge */
-	if (dp->bridge) {
+	if (dp->ptn_bridge) {
 		ret = exynos_drm_attach_lcd_bridge(dp, encoder);
 		if (!ret)
 			return 0;
@@ -1052,27 +1053,16 @@ static int exynos_dp_create_connector(struct drm_encoder *encoder)
 	return ret;
 }
 
-static bool exynos_dp_mode_fixup(struct drm_encoder *encoder,
-				 const struct drm_display_mode *mode,
-				 struct drm_display_mode *adjusted_mode)
-{
-	return true;
-}
-
-static void exynos_dp_mode_set(struct drm_encoder *encoder,
-			       struct drm_display_mode *mode,
-			       struct drm_display_mode *adjusted_mode)
+static void exynos_dp_bridge_enable(struct drm_bridge *bridge)
 {
-}
-
-static void exynos_dp_enable(struct drm_encoder *encoder)
-{
-	struct exynos_dp_device *dp = encoder_to_dp(encoder);
+	struct exynos_dp_device *dp = bridge->driver_private;
 	struct exynos_drm_crtc *crtc = dp_to_crtc(dp);
 
 	if (dp->dpms_mode == DRM_MODE_DPMS_ON)
 		return;
 
+	pm_runtime_get_sync(dp->dev);
+
 	if (dp->panel) {
 		if (drm_panel_prepare(dp->panel)) {
 			DRM_ERROR("failed to setup the panel\n");
@@ -1083,7 +1073,6 @@ static void exynos_dp_enable(struct drm_encoder *encoder)
 	if (crtc->ops->clock_enable)
 		crtc->ops->clock_enable(dp_to_crtc(dp), true);
 
-	clk_prepare_enable(dp->clock);
 	phy_power_on(dp->phy);
 	exynos_dp_init_dp(dp);
 	enable_irq(dp->irq);
@@ -1092,9 +1081,9 @@ static void exynos_dp_enable(struct drm_encoder *encoder)
 	dp->dpms_mode = DRM_MODE_DPMS_ON;
 }
 
-static void exynos_dp_disable(struct drm_encoder *encoder)
+static void exynos_dp_bridge_disable(struct drm_bridge *bridge)
 {
-	struct exynos_dp_device *dp = encoder_to_dp(encoder);
+	struct exynos_dp_device *dp = bridge->driver_private;
 	struct exynos_drm_crtc *crtc = dp_to_crtc(dp);
 
 	if (dp->dpms_mode != DRM_MODE_DPMS_ON)
@@ -1110,7 +1099,6 @@ static void exynos_dp_disable(struct drm_encoder *encoder)
 	disable_irq(dp->irq);
 	flush_work(&dp->hotplug_work);
 	phy_power_off(dp->phy);
-	clk_disable_unprepare(dp->clock);
 
 	if (crtc->ops->clock_enable)
 		crtc->ops->clock_enable(dp_to_crtc(dp), false);
@@ -1120,17 +1108,82 @@ static void exynos_dp_disable(struct drm_encoder *encoder)
 			DRM_ERROR("failed to turnoff the panel\n");
 	}
 
+	pm_runtime_put_sync(dp->dev);
+
 	dp->dpms_mode = DRM_MODE_DPMS_OFF;
 }
 
-static struct drm_encoder_helper_funcs exynos_dp_encoder_helper_funcs = {
+static void exynos_dp_bridge_nop(struct drm_bridge *bridge)
+{
+	/* do nothing */
+}
+
+static const struct drm_bridge_funcs exynos_dp_bridge_funcs = {
+	.enable = exynos_dp_bridge_enable,
+	.disable = exynos_dp_bridge_disable,
+	.pre_enable = exynos_dp_bridge_nop,
+	.post_disable = exynos_dp_bridge_nop,
+	.attach = exynos_dp_bridge_attach,
+};
+
+static int exynos_dp_create_connector(struct drm_encoder *encoder)
+{
+	struct exynos_dp_device *dp = encoder_to_dp(encoder);
+	struct drm_device *drm_dev = dp->drm_dev;
+	struct drm_bridge *bridge;
+	int ret;
+
+	bridge = devm_kzalloc(drm_dev->dev, sizeof(*bridge), GFP_KERNEL);
+	if (!bridge) {
+		DRM_ERROR("failed to allocate for drm bridge\n");
+		return -ENOMEM;
+	}
+
+	dp->bridge = bridge;
+
+	encoder->bridge = bridge;
+	bridge->driver_private = dp;
+	bridge->encoder = encoder;
+	bridge->funcs = &exynos_dp_bridge_funcs;
+
+	ret = drm_bridge_attach(drm_dev, bridge);
+	if (ret) {
+		DRM_ERROR("failed to attach drm bridge\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static bool exynos_dp_mode_fixup(struct drm_encoder *encoder,
+				 const struct drm_display_mode *mode,
+				 struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void exynos_dp_mode_set(struct drm_encoder *encoder,
+			       struct drm_display_mode *mode,
+			       struct drm_display_mode *adjusted_mode)
+{
+}
+
+static void exynos_dp_enable(struct drm_encoder *encoder)
+{
+}
+
+static void exynos_dp_disable(struct drm_encoder *encoder)
+{
+}
+
+static const struct drm_encoder_helper_funcs exynos_dp_encoder_helper_funcs = {
 	.mode_fixup = exynos_dp_mode_fixup,
 	.mode_set = exynos_dp_mode_set,
 	.enable = exynos_dp_enable,
 	.disable = exynos_dp_disable,
 };
 
-static struct drm_encoder_funcs exynos_dp_encoder_funcs = {
+static const struct drm_encoder_funcs exynos_dp_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -1238,7 +1291,7 @@ static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 		}
 	}
 
-	if (!dp->panel && !dp->bridge) {
+	if (!dp->panel && !dp->ptn_bridge) {
 		ret = exynos_dp_dt_parse_panel(dp);
 		if (ret)
 			return ret;
@@ -1289,10 +1342,6 @@ static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 
 	INIT_WORK(&dp->hotplug_work, exynos_dp_hotplug);
 
-	phy_power_on(dp->phy);
-
-	exynos_dp_init_dp(dp);
-
 	ret = devm_request_irq(&pdev->dev, dp->irq, exynos_dp_irq_handler,
 			irq_flags, "exynos-dp", dp);
 	if (ret) {
@@ -1313,7 +1362,7 @@ static int exynos_dp_bind(struct device *dev, struct device *master, void *data)
 	DRM_DEBUG_KMS("possible_crtcs = 0x%x\n", encoder->possible_crtcs);
 
 	drm_encoder_init(drm_dev, encoder, &exynos_dp_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(encoder, &exynos_dp_encoder_helper_funcs);
 
@@ -1343,8 +1392,9 @@ static const struct component_ops exynos_dp_ops = {
 static int exynos_dp_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct device_node *panel_node, *bridge_node, *endpoint;
+	struct device_node *np = NULL, *endpoint = NULL;
 	struct exynos_dp_device *dp;
+	int ret;
 
 	dp = devm_kzalloc(&pdev->dev, sizeof(struct exynos_dp_device),
 				GFP_KERNEL);
@@ -1353,36 +1403,91 @@ static int exynos_dp_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, dp);
 
-	panel_node = of_parse_phandle(dev->of_node, "panel", 0);
-	if (panel_node) {
-		dp->panel = of_drm_find_panel(panel_node);
-		of_node_put(panel_node);
+	/* This is for the backward compatibility. */
+	np = of_parse_phandle(dev->of_node, "panel", 0);
+	if (np) {
+		dp->panel = of_drm_find_panel(np);
+		of_node_put(np);
 		if (!dp->panel)
 			return -EPROBE_DEFER;
+		goto out;
 	}
 
 	endpoint = of_graph_get_next_endpoint(dev->of_node, NULL);
 	if (endpoint) {
-		bridge_node = of_graph_get_remote_port_parent(endpoint);
-		if (bridge_node) {
-			dp->bridge = of_drm_find_bridge(bridge_node);
-			of_node_put(bridge_node);
-			if (!dp->bridge)
-				return -EPROBE_DEFER;
-		} else
-			return -EPROBE_DEFER;
+		np = of_graph_get_remote_port_parent(endpoint);
+		if (np) {
+			/* The remote port can be either a panel or a bridge */
+			dp->panel = of_drm_find_panel(np);
+			if (!dp->panel) {
+				dp->ptn_bridge = of_drm_find_bridge(np);
+				if (!dp->ptn_bridge) {
+					of_node_put(np);
+					return -EPROBE_DEFER;
+				}
+			}
+			of_node_put(np);
+		} else {
+			DRM_ERROR("no remote endpoint device node found.\n");
+			return -EINVAL;
+		}
+	} else {
+		DRM_ERROR("no port endpoint subnode found.\n");
+		return -EINVAL;
 	}
 
-	return component_add(&pdev->dev, &exynos_dp_ops);
+out:
+	pm_runtime_enable(dev);
+
+	ret = component_add(&pdev->dev, &exynos_dp_ops);
+	if (ret)
+		goto err_disable_pm_runtime;
+
+	return ret;
+
+err_disable_pm_runtime:
+	pm_runtime_disable(dev);
+
+	return ret;
 }
 
 static int exynos_dp_remove(struct platform_device *pdev)
 {
+	pm_runtime_disable(&pdev->dev);
 	component_del(&pdev->dev, &exynos_dp_ops);
 
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int exynos_dp_suspend(struct device *dev)
+{
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(dp->clock);
+
+	return 0;
+}
+
+static int exynos_dp_resume(struct device *dev)
+{
+	struct exynos_dp_device *dp = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(dp->clock);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the clock clk [%d]\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops exynos_dp_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos_dp_suspend, exynos_dp_resume, NULL)
+};
+
 static const struct of_device_id exynos_dp_match[] = {
 	{ .compatible = "samsung,exynos5-dp" },
 	{},
@@ -1395,6 +1500,7 @@ struct platform_driver dp_driver = {
 	.driver		= {
 		.name	= "exynos-dp",
 		.owner	= THIS_MODULE,
+		.pm	= &exynos_dp_pm_ops,
 		.of_match_table = exynos_dp_match,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_dp_core.h b/drivers/gpu/drm/exynos/exynos_dp_core.h
index e413b6f7b0e7..66eec4b2d5c6 100644
--- a/drivers/gpu/drm/exynos/exynos_dp_core.h
+++ b/drivers/gpu/drm/exynos/exynos_dp_core.h
@@ -153,6 +153,7 @@ struct exynos_dp_device {
 	struct drm_connector	connector;
 	struct drm_panel	*panel;
 	struct drm_bridge	*bridge;
+	struct drm_bridge	*ptn_bridge;
 	struct clk		*clock;
 	unsigned int		irq;
 	void __iomem		*reg_base;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index e69357172ffb..e36579c1c025 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -68,35 +68,23 @@ static void exynos_crtc_atomic_begin(struct drm_crtc *crtc,
 				     struct drm_crtc_state *old_crtc_state)
 {
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
-	struct drm_plane *plane;
 
 	exynos_crtc->event = crtc->state->event;
 
-	drm_atomic_crtc_for_each_plane(plane, crtc) {
-		struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
-
-		if (exynos_crtc->ops->atomic_begin)
-			exynos_crtc->ops->atomic_begin(exynos_crtc,
-							exynos_plane);
-	}
+	if (exynos_crtc->ops->atomic_begin)
+		exynos_crtc->ops->atomic_begin(exynos_crtc);
 }
 
 static void exynos_crtc_atomic_flush(struct drm_crtc *crtc,
 				     struct drm_crtc_state *old_crtc_state)
 {
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
-	struct drm_plane *plane;
 
-	drm_atomic_crtc_for_each_plane(plane, crtc) {
-		struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
-
-		if (exynos_crtc->ops->atomic_flush)
-			exynos_crtc->ops->atomic_flush(exynos_crtc,
-							exynos_plane);
-	}
+	if (exynos_crtc->ops->atomic_flush)
+		exynos_crtc->ops->atomic_flush(exynos_crtc);
 }
 
-static struct drm_crtc_helper_funcs exynos_crtc_helper_funcs = {
+static const struct drm_crtc_helper_funcs exynos_crtc_helper_funcs = {
 	.enable		= exynos_drm_crtc_enable,
 	.disable	= exynos_drm_crtc_disable,
 	.mode_set_nofb	= exynos_drm_crtc_mode_set_nofb,
@@ -116,7 +104,7 @@ static void exynos_drm_crtc_destroy(struct drm_crtc *crtc)
 	kfree(exynos_crtc);
 }
 
-static struct drm_crtc_funcs exynos_crtc_funcs = {
+static const struct drm_crtc_funcs exynos_crtc_funcs = {
 	.set_config	= drm_atomic_helper_set_config,
 	.page_flip	= drm_atomic_helper_page_flip,
 	.destroy	= exynos_drm_crtc_destroy,
@@ -153,7 +141,7 @@ struct exynos_drm_crtc *exynos_drm_crtc_create(struct drm_device *drm_dev,
 	private->crtc[pipe] = crtc;
 
 	ret = drm_crtc_init_with_planes(drm_dev, crtc, plane, NULL,
-					&exynos_crtc_funcs);
+					&exynos_crtc_funcs, NULL);
 	if (ret < 0)
 		goto err_crtc;
 
@@ -215,29 +203,6 @@ void exynos_drm_crtc_finish_update(struct exynos_drm_crtc *exynos_crtc,
 	spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
 }
 
-void exynos_drm_crtc_complete_scanout(struct drm_framebuffer *fb)
-{
-	struct exynos_drm_crtc *exynos_crtc;
-	struct drm_device *dev = fb->dev;
-	struct drm_crtc *crtc;
-
-	/*
-	 * make sure that overlay data are updated to real hardware
-	 * for all encoders.
-	 */
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
-		exynos_crtc = to_exynos_crtc(crtc);
-
-		/*
-		 * wait for vblank interrupt
-		 * - this makes sure that overlay data are updated to
-		 *	real hardware.
-		 */
-		if (exynos_crtc->ops->wait_for_vblank)
-			exynos_crtc->ops->wait_for_vblank(exynos_crtc);
-	}
-}
-
 int exynos_drm_crtc_get_pipe_from_type(struct drm_device *drm_dev,
 				       enum exynos_drm_output_type out_type)
 {
@@ -261,3 +226,29 @@ void exynos_drm_crtc_te_handler(struct drm_crtc *crtc)
 	if (exynos_crtc->ops->te_handler)
 		exynos_crtc->ops->te_handler(exynos_crtc);
 }
+
+void exynos_drm_crtc_cancel_page_flip(struct drm_crtc *crtc,
+					struct drm_file *file)
+{
+	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
+	struct drm_pending_vblank_event *e;
+	unsigned long flags;
+
+	spin_lock_irqsave(&crtc->dev->event_lock, flags);
+	e = exynos_crtc->event;
+	if (e && e->base.file_priv == file) {
+		exynos_crtc->event = NULL;
+		/*
+		 * event will be destroyed by core part
+		 * so below line should be removed later with core changes
+		 */
+		e->base.destroy(&e->base);
+		/*
+		 * event_space will be increased by core part
+		 * so below line should be removed later with core changes.
+		 */
+		file->event_space += sizeof(e->event);
+		atomic_dec(&exynos_crtc->pending_update);
+	}
+	spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
+}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.h b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
index f9f365bd0257..cfdcf3e4eb1b 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.h
@@ -28,7 +28,6 @@ void exynos_drm_crtc_disable_vblank(struct drm_device *dev, unsigned int pipe);
 void exynos_drm_crtc_wait_pending_update(struct exynos_drm_crtc *exynos_crtc);
 void exynos_drm_crtc_finish_update(struct exynos_drm_crtc *exynos_crtc,
 				   struct exynos_drm_plane *exynos_plane);
-void exynos_drm_crtc_complete_scanout(struct drm_framebuffer *fb);
 
 /* This function gets pipe value to crtc device matched with out_type. */
 int exynos_drm_crtc_get_pipe_from_type(struct drm_device *drm_dev,
@@ -41,4 +40,8 @@ int exynos_drm_crtc_get_pipe_from_type(struct drm_device *drm_dev,
  */
 void exynos_drm_crtc_te_handler(struct drm_crtc *crtc);
 
+/* This function cancels a page flip request. */
+void exynos_drm_crtc_cancel_page_flip(struct drm_crtc *crtc,
+					struct drm_file *file);
+
 #endif
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dpi.c b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
index c748b8790de3..05350ae0785b 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dpi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
@@ -57,7 +57,7 @@ static void exynos_dpi_connector_destroy(struct drm_connector *connector)
 	drm_connector_cleanup(connector);
 }
 
-static struct drm_connector_funcs exynos_dpi_connector_funcs = {
+static const struct drm_connector_funcs exynos_dpi_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = exynos_dpi_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -100,7 +100,7 @@ exynos_dpi_best_encoder(struct drm_connector *connector)
 	return &ctx->encoder;
 }
 
-static struct drm_connector_helper_funcs exynos_dpi_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs exynos_dpi_connector_helper_funcs = {
 	.get_modes = exynos_dpi_get_modes,
 	.best_encoder = exynos_dpi_best_encoder,
 };
@@ -161,14 +161,14 @@ static void exynos_dpi_disable(struct drm_encoder *encoder)
 	}
 }
 
-static struct drm_encoder_helper_funcs exynos_dpi_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs exynos_dpi_encoder_helper_funcs = {
 	.mode_fixup = exynos_dpi_mode_fixup,
 	.mode_set = exynos_dpi_mode_set,
 	.enable = exynos_dpi_enable,
 	.disable = exynos_dpi_disable,
 };
 
-static struct drm_encoder_funcs exynos_dpi_encoder_funcs = {
+static const struct drm_encoder_funcs exynos_dpi_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -309,7 +309,7 @@ int exynos_dpi_bind(struct drm_device *dev, struct drm_encoder *encoder)
 	DRM_DEBUG_KMS("possible_crtcs = 0x%x\n", encoder->possible_crtcs);
 
 	drm_encoder_init(dev, encoder, &exynos_dpi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(encoder, &exynos_dpi_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index 2c6019d6a205..68f0f36f6e7e 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -304,45 +304,6 @@ int exynos_atomic_commit(struct drm_device *dev, struct drm_atomic_state *state,
 	return 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-static int exynos_drm_suspend(struct drm_device *dev, pm_message_t state)
-{
-	struct drm_connector *connector;
-
-	drm_modeset_lock_all(dev);
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		int old_dpms = connector->dpms;
-
-		if (connector->funcs->dpms)
-			connector->funcs->dpms(connector, DRM_MODE_DPMS_OFF);
-
-		/* Set the old mode back to the connector for resume */
-		connector->dpms = old_dpms;
-	}
-	drm_modeset_unlock_all(dev);
-
-	return 0;
-}
-
-static int exynos_drm_resume(struct drm_device *dev)
-{
-	struct drm_connector *connector;
-
-	drm_modeset_lock_all(dev);
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		if (connector->funcs->dpms) {
-			int dpms = connector->dpms;
-
-			connector->dpms = DRM_MODE_DPMS_OFF;
-			connector->funcs->dpms(connector, dpms);
-		}
-	}
-	drm_modeset_unlock_all(dev);
-
-	return 0;
-}
-#endif
-
 static int exynos_drm_open(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_exynos_file_private *file_priv;
@@ -369,7 +330,12 @@ err_file_priv_free:
 static void exynos_drm_preclose(struct drm_device *dev,
 					struct drm_file *file)
 {
+	struct drm_crtc *crtc;
+
 	exynos_drm_subdrv_close(dev, file);
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		exynos_drm_crtc_cancel_page_flip(crtc, file);
 }
 
 static void exynos_drm_postclose(struct drm_device *dev, struct drm_file *file)
@@ -476,31 +442,54 @@ static struct drm_driver exynos_drm_driver = {
 };
 
 #ifdef CONFIG_PM_SLEEP
-static int exynos_drm_sys_suspend(struct device *dev)
+static int exynos_drm_suspend(struct device *dev)
 {
 	struct drm_device *drm_dev = dev_get_drvdata(dev);
-	pm_message_t message;
+	struct drm_connector *connector;
 
 	if (pm_runtime_suspended(dev) || !drm_dev)
 		return 0;
 
-	message.event = PM_EVENT_SUSPEND;
-	return exynos_drm_suspend(drm_dev, message);
+	drm_modeset_lock_all(drm_dev);
+	drm_for_each_connector(connector, drm_dev) {
+		int old_dpms = connector->dpms;
+
+		if (connector->funcs->dpms)
+			connector->funcs->dpms(connector, DRM_MODE_DPMS_OFF);
+
+		/* Set the old mode back to the connector for resume */
+		connector->dpms = old_dpms;
+	}
+	drm_modeset_unlock_all(drm_dev);
+
+	return 0;
 }
 
-static int exynos_drm_sys_resume(struct device *dev)
+static int exynos_drm_resume(struct device *dev)
 {
 	struct drm_device *drm_dev = dev_get_drvdata(dev);
+	struct drm_connector *connector;
 
 	if (pm_runtime_suspended(dev) || !drm_dev)
 		return 0;
 
-	return exynos_drm_resume(drm_dev);
+	drm_modeset_lock_all(drm_dev);
+	drm_for_each_connector(connector, drm_dev) {
+		if (connector->funcs->dpms) {
+			int dpms = connector->dpms;
+
+			connector->dpms = DRM_MODE_DPMS_OFF;
+			connector->funcs->dpms(connector, dpms);
+		}
+	}
+	drm_modeset_unlock_all(drm_dev);
+
+	return 0;
 }
 #endif
 
 static const struct dev_pm_ops exynos_drm_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(exynos_drm_sys_suspend, exynos_drm_sys_resume)
+	SET_SYSTEM_SLEEP_PM_OPS(exynos_drm_suspend, exynos_drm_resume)
 };
 
 /* forward declaration */
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index f1eda7fa4e3c..17b5ded72ff1 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -38,25 +38,46 @@ enum exynos_drm_output_type {
 	EXYNOS_DISPLAY_TYPE_VIDI,
 };
 
+struct exynos_drm_rect {
+	unsigned int x, y;
+	unsigned int w, h;
+};
+
 /*
- * Exynos drm common overlay structure.
+ * Exynos drm plane state structure.
  *
- * @base: plane object
- * @src_x: offset x on a framebuffer to be displayed.
- *	- the unit is screen coordinates.
- * @src_y: offset y on a framebuffer to be displayed.
- *	- the unit is screen coordinates.
- * @src_w: width of a partial image to be displayed from framebuffer.
- * @src_h: height of a partial image to be displayed from framebuffer.
- * @crtc_x: offset x on hardware screen.
- * @crtc_y: offset y on hardware screen.
- * @crtc_w: window width to be displayed (hardware screen).
- * @crtc_h: window height to be displayed (hardware screen).
+ * @base: plane_state object (contains drm_framebuffer pointer)
+ * @src: rectangle of the source image data to be displayed (clipped to
+ *       visible part).
+ * @crtc: rectangle of the target image position on hardware screen
+ *       (clipped to visible part).
  * @h_ratio: horizontal scaling ratio, 16.16 fixed point
  * @v_ratio: vertical scaling ratio, 16.16 fixed point
- * @dma_addr: array of bus(accessed by dma) address to the memory region
- *	      allocated for a overlay.
- * @zpos: order of overlay layer(z position).
+ *
+ * this structure consists plane state data that will be applied to hardware
+ * specific overlay info.
+ */
+
+struct exynos_drm_plane_state {
+	struct drm_plane_state base;
+	struct exynos_drm_rect crtc;
+	struct exynos_drm_rect src;
+	unsigned int h_ratio;
+	unsigned int v_ratio;
+	unsigned int zpos;
+};
+
+static inline struct exynos_drm_plane_state *
+to_exynos_plane_state(struct drm_plane_state *state)
+{
+	return container_of(state, struct exynos_drm_plane_state, base);
+}
+
+/*
+ * Exynos drm common overlay structure.
+ *
+ * @base: plane object
+ * @index: hardware index of the overlay layer
  *
  * this structure is common to exynos SoC and its contents would be copied
  * to hardware specific overlay info.
@@ -64,21 +85,33 @@ enum exynos_drm_output_type {
 
 struct exynos_drm_plane {
 	struct drm_plane base;
-	unsigned int src_x;
-	unsigned int src_y;
-	unsigned int src_w;
-	unsigned int src_h;
-	unsigned int crtc_x;
-	unsigned int crtc_y;
-	unsigned int crtc_w;
-	unsigned int crtc_h;
-	unsigned int h_ratio;
-	unsigned int v_ratio;
-	dma_addr_t dma_addr[MAX_FB_BUFFER];
-	unsigned int zpos;
+	const struct exynos_drm_plane_config *config;
+	unsigned int index;
 	struct drm_framebuffer *pending_fb;
 };
 
+#define EXYNOS_DRM_PLANE_CAP_DOUBLE	(1 << 0)
+#define EXYNOS_DRM_PLANE_CAP_SCALE	(1 << 1)
+#define EXYNOS_DRM_PLANE_CAP_ZPOS	(1 << 2)
+
+/*
+ * Exynos DRM plane configuration structure.
+ *
+ * @zpos: initial z-position of the plane.
+ * @type: type of the plane (primary, cursor or overlay).
+ * @pixel_formats: supported pixel formats.
+ * @num_pixel_formats: number of elements in 'pixel_formats'.
+ * @capabilities: supported features (see EXYNOS_DRM_PLANE_CAP_*)
+ */
+
+struct exynos_drm_plane_config {
+	unsigned int zpos;
+	enum drm_plane_type type;
+	const uint32_t *pixel_formats;
+	unsigned int num_pixel_formats;
+	unsigned int capabilities;
+};
+
 /*
  * Exynos drm crtc ops
  *
@@ -90,8 +123,8 @@ struct exynos_drm_plane {
  * @wait_for_vblank: wait for vblank interrupt to make sure that
  *	hardware overlay is updated.
  * @atomic_check: validate state
- * @atomic_begin: prepare a window to receive a update
- * @atomic_flush: mark the end of a window update
+ * @atomic_begin: prepare device to receive an update
+ * @atomic_flush: mark the end of device update
  * @update_plane: apply hardware specific overlay data to registers.
  * @disable_plane: disable hardware specific overlay.
  * @te_handler: trigger to transfer video image at the tearing effect
@@ -111,14 +144,12 @@ struct exynos_drm_crtc_ops {
 	void (*wait_for_vblank)(struct exynos_drm_crtc *crtc);
 	int (*atomic_check)(struct exynos_drm_crtc *crtc,
 			    struct drm_crtc_state *state);
-	void (*atomic_begin)(struct exynos_drm_crtc *crtc,
-			      struct exynos_drm_plane *plane);
+	void (*atomic_begin)(struct exynos_drm_crtc *crtc);
 	void (*update_plane)(struct exynos_drm_crtc *crtc,
 			     struct exynos_drm_plane *plane);
 	void (*disable_plane)(struct exynos_drm_crtc *crtc,
 			      struct exynos_drm_plane *plane);
-	void (*atomic_flush)(struct exynos_drm_crtc *crtc,
-			      struct exynos_drm_plane *plane);
+	void (*atomic_flush)(struct exynos_drm_crtc *crtc);
 	void (*te_handler)(struct exynos_drm_crtc *crtc);
 	void (*clock_enable)(struct exynos_drm_crtc *crtc, bool enable);
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dsi.c b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
index 12b03b364703..26e81d191f56 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dsi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
@@ -1458,66 +1458,6 @@ static const struct mipi_dsi_host_ops exynos_dsi_ops = {
 	.transfer = exynos_dsi_host_transfer,
 };
 
-static int exynos_dsi_poweron(struct exynos_dsi *dsi)
-{
-	struct exynos_dsi_driver_data *driver_data = dsi->driver_data;
-	int ret, i;
-
-	ret = regulator_bulk_enable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
-	if (ret < 0) {
-		dev_err(dsi->dev, "cannot enable regulators %d\n", ret);
-		return ret;
-	}
-
-	for (i = 0; i < driver_data->num_clks; i++) {
-		ret = clk_prepare_enable(dsi->clks[i]);
-		if (ret < 0)
-			goto err_clk;
-	}
-
-	ret = phy_power_on(dsi->phy);
-	if (ret < 0) {
-		dev_err(dsi->dev, "cannot enable phy %d\n", ret);
-		goto err_clk;
-	}
-
-	return 0;
-
-err_clk:
-	while (--i > -1)
-		clk_disable_unprepare(dsi->clks[i]);
-	regulator_bulk_disable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
-
-	return ret;
-}
-
-static void exynos_dsi_poweroff(struct exynos_dsi *dsi)
-{
-	struct exynos_dsi_driver_data *driver_data = dsi->driver_data;
-	int ret, i;
-
-	usleep_range(10000, 20000);
-
-	if (dsi->state & DSIM_STATE_INITIALIZED) {
-		dsi->state &= ~DSIM_STATE_INITIALIZED;
-
-		exynos_dsi_disable_clock(dsi);
-
-		exynos_dsi_disable_irq(dsi);
-	}
-
-	dsi->state &= ~DSIM_STATE_CMD_LPM;
-
-	phy_power_off(dsi->phy);
-
-	for (i = driver_data->num_clks - 1; i > -1; i--)
-		clk_disable_unprepare(dsi->clks[i]);
-
-	ret = regulator_bulk_disable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
-	if (ret < 0)
-		dev_err(dsi->dev, "cannot disable regulators %d\n", ret);
-}
-
 static void exynos_dsi_enable(struct drm_encoder *encoder)
 {
 	struct exynos_dsi *dsi = encoder_to_dsi(encoder);
@@ -1526,16 +1466,14 @@ static void exynos_dsi_enable(struct drm_encoder *encoder)
 	if (dsi->state & DSIM_STATE_ENABLED)
 		return;
 
-	ret = exynos_dsi_poweron(dsi);
-	if (ret < 0)
-		return;
+	pm_runtime_get_sync(dsi->dev);
 
 	dsi->state |= DSIM_STATE_ENABLED;
 
 	ret = drm_panel_prepare(dsi->panel);
 	if (ret < 0) {
 		dsi->state &= ~DSIM_STATE_ENABLED;
-		exynos_dsi_poweroff(dsi);
+		pm_runtime_put_sync(dsi->dev);
 		return;
 	}
 
@@ -1547,7 +1485,7 @@ static void exynos_dsi_enable(struct drm_encoder *encoder)
 		dsi->state &= ~DSIM_STATE_ENABLED;
 		exynos_dsi_set_display_enable(dsi, false);
 		drm_panel_unprepare(dsi->panel);
-		exynos_dsi_poweroff(dsi);
+		pm_runtime_put_sync(dsi->dev);
 		return;
 	}
 
@@ -1569,7 +1507,7 @@ static void exynos_dsi_disable(struct drm_encoder *encoder)
 
 	dsi->state &= ~DSIM_STATE_ENABLED;
 
-	exynos_dsi_poweroff(dsi);
+	pm_runtime_put_sync(dsi->dev);
 }
 
 static enum drm_connector_status
@@ -1603,7 +1541,7 @@ static void exynos_dsi_connector_destroy(struct drm_connector *connector)
 	connector->dev = NULL;
 }
 
-static struct drm_connector_funcs exynos_dsi_connector_funcs = {
+static const struct drm_connector_funcs exynos_dsi_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.detect = exynos_dsi_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -1631,7 +1569,7 @@ exynos_dsi_best_encoder(struct drm_connector *connector)
 	return &dsi->encoder;
 }
 
-static struct drm_connector_helper_funcs exynos_dsi_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs exynos_dsi_connector_helper_funcs = {
 	.get_modes = exynos_dsi_get_modes,
 	.best_encoder = exynos_dsi_best_encoder,
 };
@@ -1684,14 +1622,14 @@ static void exynos_dsi_mode_set(struct drm_encoder *encoder,
 	vm->hsync_len = m->hsync_end - m->hsync_start;
 }
 
-static struct drm_encoder_helper_funcs exynos_dsi_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs exynos_dsi_encoder_helper_funcs = {
 	.mode_fixup = exynos_dsi_mode_fixup,
 	.mode_set = exynos_dsi_mode_set,
 	.enable = exynos_dsi_enable,
 	.disable = exynos_dsi_disable,
 };
 
-static struct drm_encoder_funcs exynos_dsi_encoder_funcs = {
+static const struct drm_encoder_funcs exynos_dsi_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -1797,13 +1735,13 @@ static int exynos_dsi_parse_dt(struct exynos_dsi *dsi)
 
 	ep = of_graph_get_next_endpoint(node, NULL);
 	if (!ep) {
-		ret = -ENXIO;
+		ret = -EINVAL;
 		goto end;
 	}
 
 	dsi->bridge_node = of_graph_get_remote_port_parent(ep);
 	if (!dsi->bridge_node) {
-		ret = -ENXIO;
+		ret = -EINVAL;
 		goto end;
 	}
 end:
@@ -1831,7 +1769,7 @@ static int exynos_dsi_bind(struct device *dev, struct device *master,
 	DRM_DEBUG_KMS("possible_crtcs = 0x%x\n", encoder->possible_crtcs);
 
 	drm_encoder_init(drm_dev, encoder, &exynos_dsi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(encoder, &exynos_dsi_encoder_helper_funcs);
 
@@ -1844,6 +1782,7 @@ static int exynos_dsi_bind(struct device *dev, struct device *master,
 
 	bridge = of_drm_find_bridge(dsi->bridge_node);
 	if (bridge) {
+		encoder->bridge = bridge;
 		drm_bridge_attach(drm_dev, bridge);
 	}
 
@@ -1954,22 +1893,97 @@ static int exynos_dsi_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, &dsi->encoder);
 
+	pm_runtime_enable(dev);
+
 	return component_add(dev, &exynos_dsi_component_ops);
 }
 
 static int exynos_dsi_remove(struct platform_device *pdev)
 {
+	pm_runtime_disable(&pdev->dev);
+
 	component_del(&pdev->dev, &exynos_dsi_component_ops);
 
 	return 0;
 }
 
+static int __maybe_unused exynos_dsi_suspend(struct device *dev)
+{
+	struct drm_encoder *encoder = dev_get_drvdata(dev);
+	struct exynos_dsi *dsi = encoder_to_dsi(encoder);
+	struct exynos_dsi_driver_data *driver_data = dsi->driver_data;
+	int ret, i;
+
+	usleep_range(10000, 20000);
+
+	if (dsi->state & DSIM_STATE_INITIALIZED) {
+		dsi->state &= ~DSIM_STATE_INITIALIZED;
+
+		exynos_dsi_disable_clock(dsi);
+
+		exynos_dsi_disable_irq(dsi);
+	}
+
+	dsi->state &= ~DSIM_STATE_CMD_LPM;
+
+	phy_power_off(dsi->phy);
+
+	for (i = driver_data->num_clks - 1; i > -1; i--)
+		clk_disable_unprepare(dsi->clks[i]);
+
+	ret = regulator_bulk_disable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
+	if (ret < 0)
+		dev_err(dsi->dev, "cannot disable regulators %d\n", ret);
+
+	return 0;
+}
+
+static int __maybe_unused exynos_dsi_resume(struct device *dev)
+{
+	struct drm_encoder *encoder = dev_get_drvdata(dev);
+	struct exynos_dsi *dsi = encoder_to_dsi(encoder);
+	struct exynos_dsi_driver_data *driver_data = dsi->driver_data;
+	int ret, i;
+
+	ret = regulator_bulk_enable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
+	if (ret < 0) {
+		dev_err(dsi->dev, "cannot enable regulators %d\n", ret);
+		return ret;
+	}
+
+	for (i = 0; i < driver_data->num_clks; i++) {
+		ret = clk_prepare_enable(dsi->clks[i]);
+		if (ret < 0)
+			goto err_clk;
+	}
+
+	ret = phy_power_on(dsi->phy);
+	if (ret < 0) {
+		dev_err(dsi->dev, "cannot enable phy %d\n", ret);
+		goto err_clk;
+	}
+
+	return 0;
+
+err_clk:
+	while (--i > -1)
+		clk_disable_unprepare(dsi->clks[i]);
+	regulator_bulk_disable(ARRAY_SIZE(dsi->supplies), dsi->supplies);
+
+	return ret;
+}
+
+static const struct dev_pm_ops exynos_dsi_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos_dsi_suspend, exynos_dsi_resume, NULL)
+};
+
 struct platform_driver dsi_driver = {
 	.probe = exynos_dsi_probe,
 	.remove = exynos_dsi_remove,
 	.driver = {
 		   .name = "exynos-dsi",
 		   .owner = THIS_MODULE,
+		   .pm = &exynos_dsi_pm_ops,
 		   .of_match_table = exynos_dsi_of_match,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index fcea28bdbc42..d614194644c8 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -37,6 +37,7 @@
 struct exynos_drm_fb {
 	struct drm_framebuffer	fb;
 	struct exynos_drm_gem	*exynos_gem[MAX_FB_BUFFER];
+	dma_addr_t			dma_addr[MAX_FB_BUFFER];
 };
 
 static int check_fb_gem_memory_type(struct drm_device *drm_dev,
@@ -70,9 +71,6 @@ static void exynos_drm_fb_destroy(struct drm_framebuffer *fb)
 	struct exynos_drm_fb *exynos_fb = to_exynos_fb(fb);
 	unsigned int i;
 
-	/* make sure that overlay data are updated before relesing fb. */
-	exynos_drm_crtc_complete_scanout(fb);
-
 	drm_framebuffer_cleanup(fb);
 
 	for (i = 0; i < ARRAY_SIZE(exynos_fb->exynos_gem); i++) {
@@ -109,7 +107,7 @@ static int exynos_drm_fb_dirty(struct drm_framebuffer *fb,
 	return 0;
 }
 
-static struct drm_framebuffer_funcs exynos_drm_fb_funcs = {
+static const struct drm_framebuffer_funcs exynos_drm_fb_funcs = {
 	.destroy	= exynos_drm_fb_destroy,
 	.create_handle	= exynos_drm_fb_create_handle,
 	.dirty		= exynos_drm_fb_dirty,
@@ -117,7 +115,7 @@ static struct drm_framebuffer_funcs exynos_drm_fb_funcs = {
 
 struct drm_framebuffer *
 exynos_drm_framebuffer_init(struct drm_device *dev,
-			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    const struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct exynos_drm_gem **exynos_gem,
 			    int count)
 {
@@ -135,6 +133,8 @@ exynos_drm_framebuffer_init(struct drm_device *dev,
 			goto err;
 
 		exynos_fb->exynos_gem[i] = exynos_gem[i];
+		exynos_fb->dma_addr[i] = exynos_gem[i]->dma_addr
+						+ mode_cmd->offsets[i];
 	}
 
 	drm_helper_mode_fill_fb_struct(&exynos_fb->fb, mode_cmd);
@@ -154,7 +154,7 @@ err:
 
 static struct drm_framebuffer *
 exynos_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
-		      struct drm_mode_fb_cmd2 *mode_cmd)
+		      const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct exynos_drm_gem *exynos_gem[MAX_FB_BUFFER];
 	struct drm_gem_object *obj;
@@ -189,21 +189,14 @@ err:
 	return ERR_PTR(ret);
 }
 
-struct exynos_drm_gem *exynos_drm_fb_gem(struct drm_framebuffer *fb, int index)
+dma_addr_t exynos_drm_fb_dma_addr(struct drm_framebuffer *fb, int index)
 {
 	struct exynos_drm_fb *exynos_fb = to_exynos_fb(fb);
-	struct exynos_drm_gem *exynos_gem;
 
 	if (index >= MAX_FB_BUFFER)
-		return NULL;
-
-	exynos_gem = exynos_fb->exynos_gem[index];
-	if (!exynos_gem)
-		return NULL;
-
-	DRM_DEBUG_KMS("dma_addr: 0x%lx\n", (unsigned long)exynos_gem->dma_addr);
+		return DMA_ERROR_CODE;
 
-	return exynos_gem;
+	return exynos_fb->dma_addr[index];
 }
 
 static void exynos_drm_output_poll_changed(struct drm_device *dev)
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.h b/drivers/gpu/drm/exynos/exynos_drm_fb.h
index 726a2d44371f..3a9e75b2cf6b 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.h
@@ -12,18 +12,17 @@
  */
 
 #ifndef _EXYNOS_DRM_FB_H_
-#define _EXYNOS_DRM_FB_H
+#define _EXYNOS_DRM_FB_H_
 
 #include "exynos_drm_gem.h"
 
 struct drm_framebuffer *
 exynos_drm_framebuffer_init(struct drm_device *dev,
-			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    const struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct exynos_drm_gem **exynos_gem,
 			    int count);
 
-/* get gem object of a drm framebuffer */
-struct exynos_drm_gem *exynos_drm_fb_gem(struct drm_framebuffer *fb, int index);
+dma_addr_t exynos_drm_fb_dma_addr(struct drm_framebuffer *fb, int index);
 
 void exynos_drm_mode_config_init(struct drm_device *dev);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
index f6118baa8e3e..8baabd813ff5 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
@@ -50,7 +50,7 @@ static int exynos_drm_fb_mmap(struct fb_info *info,
 	if (vm_size > exynos_gem->size)
 		return -EINVAL;
 
-	ret = dma_mmap_attrs(helper->dev->dev, vma, exynos_gem->pages,
+	ret = dma_mmap_attrs(helper->dev->dev, vma, exynos_gem->cookie,
 			     exynos_gem->dma_addr, exynos_gem->size,
 			     &exynos_gem->dma_attrs);
 	if (ret < 0) {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimc.c b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
index c747824f3c98..8a4f4a0211d0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
@@ -1723,7 +1723,7 @@ static int fimc_probe(struct platform_device *pdev)
 		goto err_put_clk;
 	}
 
-	DRM_DEBUG_KMS("id[%d]ippdrv[0x%x]\n", ctx->id, (int)ippdrv);
+	DRM_DEBUG_KMS("id[%d]ippdrv[%p]\n", ctx->id, ippdrv);
 
 	spin_lock_init(&ctx->lock);
 	platform_set_drvdata(pdev, ctx);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index bd75c1531cac..70194d0e4fe4 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -29,6 +29,7 @@
 #include <drm/exynos_drm.h>
 
 #include "exynos_drm_drv.h"
+#include "exynos_drm_fb.h"
 #include "exynos_drm_fbdev.h"
 #include "exynos_drm_crtc.h"
 #include "exynos_drm_plane.h"
@@ -87,7 +88,6 @@
 
 /* FIMD has totally five hardware windows. */
 #define WINDOWS_NR	5
-#define CURSOR_WIN	4
 
 struct fimd_driver_data {
 	unsigned int timing_base;
@@ -150,6 +150,7 @@ struct fimd_context {
 	struct drm_device		*drm_dev;
 	struct exynos_drm_crtc		*crtc;
 	struct exynos_drm_plane		planes[WINDOWS_NR];
+	struct exynos_drm_plane_config	configs[WINDOWS_NR];
 	struct clk			*bus_clk;
 	struct clk			*lcd_clk;
 	void __iomem			*regs;
@@ -187,6 +188,14 @@ static const struct of_device_id fimd_driver_dt_match[] = {
 };
 MODULE_DEVICE_TABLE(of, fimd_driver_dt_match);
 
+static const enum drm_plane_type fimd_win_types[WINDOWS_NR] = {
+	DRM_PLANE_TYPE_PRIMARY,
+	DRM_PLANE_TYPE_OVERLAY,
+	DRM_PLANE_TYPE_OVERLAY,
+	DRM_PLANE_TYPE_OVERLAY,
+	DRM_PLANE_TYPE_CURSOR,
+};
+
 static const uint32_t fimd_formats[] = {
 	DRM_FORMAT_C8,
 	DRM_FORMAT_XRGB1555,
@@ -478,7 +487,7 @@ static void fimd_commit(struct exynos_drm_crtc *crtc)
 
 
 static void fimd_win_set_pixfmt(struct fimd_context *ctx, unsigned int win,
-				struct drm_framebuffer *fb)
+				uint32_t pixel_format, int width)
 {
 	unsigned long val;
 
@@ -489,11 +498,11 @@ static void fimd_win_set_pixfmt(struct fimd_context *ctx, unsigned int win,
 	 * So the request format is ARGB8888 then change it to XRGB8888.
 	 */
 	if (ctx->driver_data->has_limited_fmt && !win) {
-		if (fb->pixel_format == DRM_FORMAT_ARGB8888)
-			fb->pixel_format = DRM_FORMAT_XRGB8888;
+		if (pixel_format == DRM_FORMAT_ARGB8888)
+			pixel_format = DRM_FORMAT_XRGB8888;
 	}
 
-	switch (fb->pixel_format) {
+	switch (pixel_format) {
 	case DRM_FORMAT_C8:
 		val |= WINCON0_BPPMODE_8BPP_PALETTE;
 		val |= WINCONx_BURSTLEN_8WORD;
@@ -529,17 +538,15 @@ static void fimd_win_set_pixfmt(struct fimd_context *ctx, unsigned int win,
 		break;
 	}
 
-	DRM_DEBUG_KMS("bpp = %d\n", fb->bits_per_pixel);
-
 	/*
-	 * In case of exynos, setting dma-burst to 16Word causes permanent
-	 * tearing for very small buffers, e.g. cursor buffer. Burst Mode
-	 * switching which is based on plane size is not recommended as
-	 * plane size varies alot towards the end of the screen and rapid
-	 * movement causes unstable DMA which results into iommu crash/tear.
+	 * Setting dma-burst to 16Word causes permanent tearing for very small
+	 * buffers, e.g. cursor buffer. Burst Mode switching which based on
+	 * plane size is not recommended as plane size varies alot towards the
+	 * end of the screen and rapid movement causes unstable DMA, but it is
+	 * still better to change dma-burst than displaying garbage.
 	 */
 
-	if (fb->width < MIN_FB_WIDTH_FOR_16WORD_BURST) {
+	if (width < MIN_FB_WIDTH_FOR_16WORD_BURST) {
 		val &= ~WINCONx_BURSTLEN_MASK;
 		val |= WINCONx_BURSTLEN_4WORD;
 	}
@@ -615,64 +622,68 @@ static void fimd_shadow_protect_win(struct fimd_context *ctx,
 	writel(val, ctx->regs + reg);
 }
 
-static void fimd_atomic_begin(struct exynos_drm_crtc *crtc,
-			       struct exynos_drm_plane *plane)
+static void fimd_atomic_begin(struct exynos_drm_crtc *crtc)
 {
 	struct fimd_context *ctx = crtc->ctx;
+	int i;
 
 	if (ctx->suspended)
 		return;
 
-	fimd_shadow_protect_win(ctx, plane->zpos, true);
+	for (i = 0; i < WINDOWS_NR; i++)
+		fimd_shadow_protect_win(ctx, i, true);
 }
 
-static void fimd_atomic_flush(struct exynos_drm_crtc *crtc,
-			       struct exynos_drm_plane *plane)
+static void fimd_atomic_flush(struct exynos_drm_crtc *crtc)
 {
 	struct fimd_context *ctx = crtc->ctx;
+	int i;
 
 	if (ctx->suspended)
 		return;
 
-	fimd_shadow_protect_win(ctx, plane->zpos, false);
+	for (i = 0; i < WINDOWS_NR; i++)
+		fimd_shadow_protect_win(ctx, i, false);
 }
 
 static void fimd_update_plane(struct exynos_drm_crtc *crtc,
 			      struct exynos_drm_plane *plane)
 {
+	struct exynos_drm_plane_state *state =
+				to_exynos_plane_state(plane->base.state);
 	struct fimd_context *ctx = crtc->ctx;
-	struct drm_plane_state *state = plane->base.state;
+	struct drm_framebuffer *fb = state->base.fb;
 	dma_addr_t dma_addr;
 	unsigned long val, size, offset;
 	unsigned int last_x, last_y, buf_offsize, line_size;
-	unsigned int win = plane->zpos;
-	unsigned int bpp = state->fb->bits_per_pixel >> 3;
-	unsigned int pitch = state->fb->pitches[0];
+	unsigned int win = plane->index;
+	unsigned int bpp = fb->bits_per_pixel >> 3;
+	unsigned int pitch = fb->pitches[0];
 
 	if (ctx->suspended)
 		return;
 
-	offset = plane->src_x * bpp;
-	offset += plane->src_y * pitch;
+	offset = state->src.x * bpp;
+	offset += state->src.y * pitch;
 
 	/* buffer start address */
-	dma_addr = plane->dma_addr[0] + offset;
+	dma_addr = exynos_drm_fb_dma_addr(fb, 0) + offset;
 	val = (unsigned long)dma_addr;
 	writel(val, ctx->regs + VIDWx_BUF_START(win, 0));
 
 	/* buffer end address */
-	size = pitch * plane->crtc_h;
+	size = pitch * state->crtc.h;
 	val = (unsigned long)(dma_addr + size);
 	writel(val, ctx->regs + VIDWx_BUF_END(win, 0));
 
 	DRM_DEBUG_KMS("start addr = 0x%lx, end addr = 0x%lx, size = 0x%lx\n",
 			(unsigned long)dma_addr, val, size);
 	DRM_DEBUG_KMS("ovl_width = %d, ovl_height = %d\n",
-			plane->crtc_w, plane->crtc_h);
+			state->crtc.w, state->crtc.h);
 
 	/* buffer size */
-	buf_offsize = pitch - (plane->crtc_w * bpp);
-	line_size = plane->crtc_w * bpp;
+	buf_offsize = pitch - (state->crtc.w * bpp);
+	line_size = state->crtc.w * bpp;
 	val = VIDW_BUF_SIZE_OFFSET(buf_offsize) |
 		VIDW_BUF_SIZE_PAGEWIDTH(line_size) |
 		VIDW_BUF_SIZE_OFFSET_E(buf_offsize) |
@@ -680,16 +691,16 @@ static void fimd_update_plane(struct exynos_drm_crtc *crtc,
 	writel(val, ctx->regs + VIDWx_BUF_SIZE(win, 0));
 
 	/* OSD position */
-	val = VIDOSDxA_TOPLEFT_X(plane->crtc_x) |
-		VIDOSDxA_TOPLEFT_Y(plane->crtc_y) |
-		VIDOSDxA_TOPLEFT_X_E(plane->crtc_x) |
-		VIDOSDxA_TOPLEFT_Y_E(plane->crtc_y);
+	val = VIDOSDxA_TOPLEFT_X(state->crtc.x) |
+		VIDOSDxA_TOPLEFT_Y(state->crtc.y) |
+		VIDOSDxA_TOPLEFT_X_E(state->crtc.x) |
+		VIDOSDxA_TOPLEFT_Y_E(state->crtc.y);
 	writel(val, ctx->regs + VIDOSD_A(win));
 
-	last_x = plane->crtc_x + plane->crtc_w;
+	last_x = state->crtc.x + state->crtc.w;
 	if (last_x)
 		last_x--;
-	last_y = plane->crtc_y + plane->crtc_h;
+	last_y = state->crtc.y + state->crtc.h;
 	if (last_y)
 		last_y--;
 
@@ -699,20 +710,20 @@ static void fimd_update_plane(struct exynos_drm_crtc *crtc,
 	writel(val, ctx->regs + VIDOSD_B(win));
 
 	DRM_DEBUG_KMS("osd pos: tx = %d, ty = %d, bx = %d, by = %d\n",
-			plane->crtc_x, plane->crtc_y, last_x, last_y);
+			state->crtc.x, state->crtc.y, last_x, last_y);
 
 	/* OSD size */
 	if (win != 3 && win != 4) {
 		u32 offset = VIDOSD_D(win);
 		if (win == 0)
 			offset = VIDOSD_C(win);
-		val = plane->crtc_w * plane->crtc_h;
+		val = state->crtc.w * state->crtc.h;
 		writel(val, ctx->regs + offset);
 
 		DRM_DEBUG_KMS("osd size = 0x%x\n", (unsigned int)val);
 	}
 
-	fimd_win_set_pixfmt(ctx, win, state->fb);
+	fimd_win_set_pixfmt(ctx, win, fb->pixel_format, state->src.w);
 
 	/* hardware window 0 doesn't support color key. */
 	if (win != 0)
@@ -731,7 +742,7 @@ static void fimd_disable_plane(struct exynos_drm_crtc *crtc,
 			       struct exynos_drm_plane *plane)
 {
 	struct fimd_context *ctx = crtc->ctx;
-	unsigned int win = plane->zpos;
+	unsigned int win = plane->index;
 
 	if (ctx->suspended)
 		return;
@@ -745,7 +756,6 @@ static void fimd_disable_plane(struct exynos_drm_crtc *crtc,
 static void fimd_enable(struct exynos_drm_crtc *crtc)
 {
 	struct fimd_context *ctx = crtc->ctx;
-	int ret;
 
 	if (!ctx->suspended)
 		return;
@@ -754,18 +764,6 @@ static void fimd_enable(struct exynos_drm_crtc *crtc)
 
 	pm_runtime_get_sync(ctx->dev);
 
-	ret = clk_prepare_enable(ctx->bus_clk);
-	if (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the bus clk [%d]\n", ret);
-		return;
-	}
-
-	ret = clk_prepare_enable(ctx->lcd_clk);
-	if  (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the lcd clk [%d]\n", ret);
-		return;
-	}
-
 	/* if vblank was enabled status, enable it again. */
 	if (test_and_clear_bit(0, &ctx->irq_flags))
 		fimd_enable_vblank(ctx->crtc);
@@ -795,11 +793,7 @@ static void fimd_disable(struct exynos_drm_crtc *crtc)
 
 	writel(0, ctx->regs + VIDCON0);
 
-	clk_disable_unprepare(ctx->lcd_clk);
-	clk_disable_unprepare(ctx->bus_clk);
-
 	pm_runtime_put_sync(ctx->dev);
-
 	ctx->suspended = true;
 }
 
@@ -941,18 +935,19 @@ static int fimd_bind(struct device *dev, struct device *master, void *data)
 	struct drm_device *drm_dev = data;
 	struct exynos_drm_private *priv = drm_dev->dev_private;
 	struct exynos_drm_plane *exynos_plane;
-	enum drm_plane_type type;
-	unsigned int zpos;
+	unsigned int i;
 	int ret;
 
 	ctx->drm_dev = drm_dev;
 	ctx->pipe = priv->pipe++;
 
-	for (zpos = 0; zpos < WINDOWS_NR; zpos++) {
-		type = exynos_plane_get_type(zpos, CURSOR_WIN);
-		ret = exynos_plane_init(drm_dev, &ctx->planes[zpos],
-					1 << ctx->pipe, type, fimd_formats,
-					ARRAY_SIZE(fimd_formats), zpos);
+	for (i = 0; i < WINDOWS_NR; i++) {
+		ctx->configs[i].pixel_formats = fimd_formats;
+		ctx->configs[i].num_pixel_formats = ARRAY_SIZE(fimd_formats);
+		ctx->configs[i].zpos = i;
+		ctx->configs[i].type = fimd_win_types[i];
+		ret = exynos_plane_init(drm_dev, &ctx->planes[i], i,
+					1 << ctx->pipe, &ctx->configs[i]);
 		if (ret)
 			return ret;
 	}
@@ -1121,12 +1116,49 @@ static int fimd_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int exynos_fimd_suspend(struct device *dev)
+{
+	struct fimd_context *ctx = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(ctx->lcd_clk);
+	clk_disable_unprepare(ctx->bus_clk);
+
+	return 0;
+}
+
+static int exynos_fimd_resume(struct device *dev)
+{
+	struct fimd_context *ctx = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(ctx->bus_clk);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the bus clk [%d]\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(ctx->lcd_clk);
+	if  (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the lcd clk [%d]\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops exynos_fimd_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos_fimd_suspend, exynos_fimd_resume, NULL)
+};
+
 struct platform_driver fimd_driver = {
 	.probe		= fimd_probe,
 	.remove		= fimd_remove,
 	.driver		= {
 		.name	= "exynos4-fb",
 		.owner	= THIS_MODULE,
+		.pm	= &exynos_fimd_pm_ops,
 		.of_match_table = fimd_driver_dt_match,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_drm_g2d.c b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
index c17efdb238a6..8dfe6e113a88 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_g2d.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_g2d.c
@@ -1166,7 +1166,7 @@ int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
 		goto err_free_event;
 	}
 
-	cmd = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd;
+	cmd = (struct drm_exynos_g2d_cmd *)(unsigned long)req->cmd;
 
 	if (copy_from_user(cmdlist->data + cmdlist->last,
 				(void __user *)cmd,
@@ -1184,7 +1184,8 @@ int exynos_g2d_set_cmdlist_ioctl(struct drm_device *drm_dev, void *data,
 	if (req->cmd_buf_nr) {
 		struct drm_exynos_g2d_cmd *cmd_buf;
 
-		cmd_buf = (struct drm_exynos_g2d_cmd *)(uint32_t)req->cmd_buf;
+		cmd_buf = (struct drm_exynos_g2d_cmd *)
+				(unsigned long)req->cmd_buf;
 
 		if (copy_from_user(cmdlist->data + cmdlist->last,
 					(void __user *)cmd_buf,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 252eb301470c..26b5e4bd55b6 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -14,6 +14,7 @@
 
 #include <linux/shmem_fs.h>
 #include <linux/dma-buf.h>
+#include <linux/pfn_t.h>
 #include <drm/exynos_drm.h>
 
 #include "exynos_drm_drv.h"
@@ -217,7 +218,7 @@ static struct exynos_drm_gem *exynos_drm_gem_init(struct drm_device *dev,
 		return ERR_PTR(ret);
 	}
 
-	DRM_DEBUG_KMS("created file object = 0x%x\n", (unsigned int)obj->filp);
+	DRM_DEBUG_KMS("created file object = %p\n", obj->filp);
 
 	return exynos_gem;
 }
@@ -334,7 +335,7 @@ static int exynos_drm_gem_mmap_buffer(struct exynos_drm_gem *exynos_gem,
 	if (vm_size > exynos_gem->size)
 		return -EINVAL;
 
-	ret = dma_mmap_attrs(drm_dev->dev, vma, exynos_gem->pages,
+	ret = dma_mmap_attrs(drm_dev->dev, vma, exynos_gem->cookie,
 			     exynos_gem->dma_addr, exynos_gem->size,
 			     &exynos_gem->dma_attrs);
 	if (ret < 0) {
@@ -490,7 +491,8 @@ int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	}
 
 	pfn = page_to_pfn(exynos_gem->pages[page_offset]);
-	ret = vm_insert_mixed(vma, (unsigned long)vmf->virtual_address, pfn);
+	ret = vm_insert_mixed(vma, (unsigned long)vmf->virtual_address,
+			__pfn_to_pfn_t(pfn, PFN_DEV));
 
 out:
 	switch (ret) {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 37ab8b282db6..9ca5047959ec 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -55,8 +55,6 @@ struct exynos_drm_gem {
 	struct sg_table		*sgt;
 };
 
-struct page **exynos_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask);
-
 /* destroy a buffer with gem object */
 void exynos_drm_gem_destroy(struct exynos_drm_gem *exynos_gem);
 
@@ -91,10 +89,6 @@ void exynos_drm_gem_put_dma_addr(struct drm_device *dev,
 					unsigned int gem_handle,
 					struct drm_file *filp);
 
-/* map user space allocated by malloc to pages. */
-int exynos_drm_gem_userptr_ioctl(struct drm_device *dev, void *data,
-				      struct drm_file *file_priv);
-
 /* get buffer information to memory region allocated by gem. */
 int exynos_drm_gem_get_ioctl(struct drm_device *dev, void *data,
 				      struct drm_file *file_priv);
@@ -123,28 +117,6 @@ int exynos_drm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf);
 /* set vm_flags and we can change the vm attribute to other one at here. */
 int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma);
 
-static inline int vma_is_io(struct vm_area_struct *vma)
-{
-	return !!(vma->vm_flags & (VM_IO | VM_PFNMAP));
-}
-
-/* get a copy of a virtual memory region. */
-struct vm_area_struct *exynos_gem_get_vma(struct vm_area_struct *vma);
-
-/* release a userspace virtual memory area. */
-void exynos_gem_put_vma(struct vm_area_struct *vma);
-
-/* get pages from user space. */
-int exynos_gem_get_pages_from_userptr(unsigned long start,
-						unsigned int npages,
-						struct page **pages,
-						struct vm_area_struct *vma);
-
-/* drop the reference to pages. */
-void exynos_gem_put_pages_to_userptr(struct page **pages,
-					unsigned int npages,
-					struct vm_area_struct *vma);
-
 /* map sgt with dma region. */
 int exynos_gem_map_sgt_with_dma(struct drm_device *drm_dev,
 				struct sg_table *sgt,
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gsc.c b/drivers/gpu/drm/exynos/exynos_drm_gsc.c
index 11b87d2a7913..5d20da8f957e 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gsc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gsc.c
@@ -15,7 +15,8 @@
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
-#include <plat/map-base.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 
 #include <drm/drmP.h>
 #include <drm/exynos_drm.h>
@@ -126,6 +127,7 @@ struct gsc_capability {
  * @ippdrv: prepare initialization using ippdrv.
  * @regs_res: register resources.
  * @regs: memory mapped io registers.
+ * @sysreg: handle to SYSREG block regmap.
  * @lock: locking of operations.
  * @gsc_clk: gsc gate clock.
  * @sc: scaler infomations.
@@ -138,6 +140,7 @@ struct gsc_context {
 	struct exynos_drm_ippdrv	ippdrv;
 	struct resource	*regs_res;
 	void __iomem	*regs;
+	struct regmap	*sysreg;
 	struct mutex	lock;
 	struct clk	*gsc_clk;
 	struct gsc_scaler	sc;
@@ -437,9 +440,12 @@ static int gsc_sw_reset(struct gsc_context *ctx)
 
 static void gsc_set_gscblk_fimd_wb(struct gsc_context *ctx, bool enable)
 {
-	u32 gscblk_cfg;
+	unsigned int gscblk_cfg;
 
-	gscblk_cfg = readl(SYSREG_GSCBLK_CFG1);
+	if (!ctx->sysreg)
+		return;
+
+	regmap_read(ctx->sysreg, SYSREG_GSCBLK_CFG1, &gscblk_cfg);
 
 	if (enable)
 		gscblk_cfg |= GSC_BLK_DISP1WB_DEST(ctx->id) |
@@ -448,7 +454,7 @@ static void gsc_set_gscblk_fimd_wb(struct gsc_context *ctx, bool enable)
 	else
 		gscblk_cfg |= GSC_BLK_PXLASYNC_LO_MASK_WB(ctx->id);
 
-	writel(gscblk_cfg, SYSREG_GSCBLK_CFG1);
+	regmap_write(ctx->sysreg, SYSREG_GSCBLK_CFG1, gscblk_cfg);
 }
 
 static void gsc_handle_irq(struct gsc_context *ctx, bool enable,
@@ -1215,10 +1221,10 @@ static int gsc_clk_ctrl(struct gsc_context *ctx, bool enable)
 	DRM_DEBUG_KMS("enable[%d]\n", enable);
 
 	if (enable) {
-		clk_enable(ctx->gsc_clk);
+		clk_prepare_enable(ctx->gsc_clk);
 		ctx->suspended = false;
 	} else {
-		clk_disable(ctx->gsc_clk);
+		clk_disable_unprepare(ctx->gsc_clk);
 		ctx->suspended = true;
 	}
 
@@ -1663,6 +1669,15 @@ static int gsc_probe(struct platform_device *pdev)
 	if (!ctx)
 		return -ENOMEM;
 
+	if (dev->of_node) {
+		ctx->sysreg = syscon_regmap_lookup_by_phandle(dev->of_node,
+							"samsung,sysreg");
+		if (IS_ERR(ctx->sysreg)) {
+			dev_warn(dev, "failed to get system register.\n");
+			ctx->sysreg = NULL;
+		}
+	}
+
 	/* clock control */
 	ctx->gsc_clk = devm_clk_get(dev, "gscl");
 	if (IS_ERR(ctx->gsc_clk)) {
@@ -1708,12 +1723,11 @@ static int gsc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	DRM_DEBUG_KMS("id[%d]ippdrv[0x%x]\n", ctx->id, (int)ippdrv);
+	DRM_DEBUG_KMS("id[%d]ippdrv[%p]\n", ctx->id, ippdrv);
 
 	mutex_init(&ctx->lock);
 	platform_set_drvdata(pdev, ctx);
 
-	pm_runtime_set_active(dev);
 	pm_runtime_enable(dev);
 
 	ret = exynos_drm_ippdrv_register(ippdrv);
@@ -1797,6 +1811,12 @@ static const struct dev_pm_ops gsc_pm_ops = {
 	SET_RUNTIME_PM_OPS(gsc_runtime_suspend, gsc_runtime_resume, NULL)
 };
 
+static const struct of_device_id exynos_drm_gsc_of_match[] = {
+	{ .compatible = "samsung,exynos5-gsc" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, exynos_drm_gsc_of_match);
+
 struct platform_driver gsc_driver = {
 	.probe		= gsc_probe,
 	.remove		= gsc_remove,
@@ -1804,6 +1824,7 @@ struct platform_driver gsc_driver = {
 		.name	= "exynos-drm-gsc",
 		.owner	= THIS_MODULE,
 		.pm	= &gsc_pm_ops,
+		.of_match_table = of_match_ptr(exynos_drm_gsc_of_match),
 	},
 };
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.c b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
index 67d24236e745..95eeb9116f10 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
@@ -208,7 +208,7 @@ static struct exynos_drm_ippdrv *ipp_find_drv_by_handle(u32 prop_id)
 	 * e.g PAUSE state, queue buf, command control.
 	 */
 	list_for_each_entry(ippdrv, &exynos_drm_ippdrv_list, drv_list) {
-		DRM_DEBUG_KMS("count[%d]ippdrv[0x%x]\n", count++, (int)ippdrv);
+		DRM_DEBUG_KMS("count[%d]ippdrv[%p]\n", count++, ippdrv);
 
 		mutex_lock(&ippdrv->cmd_lock);
 		list_for_each_entry(c_node, &ippdrv->cmd_list, list) {
@@ -388,8 +388,8 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 	}
 	property->prop_id = ret;
 
-	DRM_DEBUG_KMS("created prop_id[%d]cmd[%d]ippdrv[0x%x]\n",
-		property->prop_id, property->cmd, (int)ippdrv);
+	DRM_DEBUG_KMS("created prop_id[%d]cmd[%d]ippdrv[%p]\n",
+		property->prop_id, property->cmd, ippdrv);
 
 	/* stored property information and ippdrv in private data */
 	c_node->property = *property;
@@ -518,7 +518,7 @@ static int ipp_put_mem_node(struct drm_device *drm_dev,
 {
 	int i;
 
-	DRM_DEBUG_KMS("node[0x%x]\n", (int)m_node);
+	DRM_DEBUG_KMS("node[%p]\n", m_node);
 
 	if (!m_node) {
 		DRM_ERROR("invalid dequeue node.\n");
@@ -562,7 +562,7 @@ static struct drm_exynos_ipp_mem_node
 	m_node->buf_id = qbuf->buf_id;
 	INIT_LIST_HEAD(&m_node->list);
 
-	DRM_DEBUG_KMS("m_node[0x%x]ops_id[%d]\n", (int)m_node, qbuf->ops_id);
+	DRM_DEBUG_KMS("m_node[%p]ops_id[%d]\n", m_node, qbuf->ops_id);
 	DRM_DEBUG_KMS("prop_id[%d]buf_id[%d]\n", qbuf->prop_id, m_node->buf_id);
 
 	for_each_ipp_planar(i) {
@@ -582,8 +582,8 @@ static struct drm_exynos_ipp_mem_node
 
 			buf_info->handles[i] = qbuf->handle[i];
 			buf_info->base[i] = *addr;
-			DRM_DEBUG_KMS("i[%d]base[0x%x]hd[0x%lx]\n", i,
-				      buf_info->base[i], buf_info->handles[i]);
+			DRM_DEBUG_KMS("i[%d]base[%pad]hd[0x%lx]\n", i,
+				      &buf_info->base[i], buf_info->handles[i]);
 		}
 	}
 
@@ -664,7 +664,7 @@ static void ipp_put_event(struct drm_exynos_ipp_cmd_node *c_node,
 
 	mutex_lock(&c_node->event_lock);
 	list_for_each_entry_safe(e, te, &c_node->event_list, base.link) {
-		DRM_DEBUG_KMS("count[%d]e[0x%x]\n", count++, (int)e);
+		DRM_DEBUG_KMS("count[%d]e[%p]\n", count++, e);
 
 		/*
 		 * qbuf == NULL condition means all event deletion.
@@ -755,7 +755,7 @@ static struct drm_exynos_ipp_mem_node
 
 	/* find memory node from memory list */
 	list_for_each_entry(m_node, head, list) {
-		DRM_DEBUG_KMS("count[%d]m_node[0x%x]\n", count++, (int)m_node);
+		DRM_DEBUG_KMS("count[%d]m_node[%p]\n", count++, m_node);
 
 		/* compare buffer id */
 		if (m_node->buf_id == qbuf->buf_id)
@@ -772,7 +772,7 @@ static int ipp_set_mem_node(struct exynos_drm_ippdrv *ippdrv,
 	struct exynos_drm_ipp_ops *ops = NULL;
 	int ret = 0;
 
-	DRM_DEBUG_KMS("node[0x%x]\n", (int)m_node);
+	DRM_DEBUG_KMS("node[%p]\n", m_node);
 
 	if (!m_node) {
 		DRM_ERROR("invalid queue node.\n");
@@ -1237,7 +1237,7 @@ static int ipp_start_property(struct exynos_drm_ippdrv *ippdrv,
 			m_node = list_first_entry(head,
 				struct drm_exynos_ipp_mem_node, list);
 
-			DRM_DEBUG_KMS("m_node[0x%x]\n", (int)m_node);
+			DRM_DEBUG_KMS("m_node[%p]\n", m_node);
 
 			ret = ipp_set_mem_node(ippdrv, c_node, m_node);
 			if (ret) {
@@ -1610,8 +1610,8 @@ static int ipp_subdrv_probe(struct drm_device *drm_dev, struct device *dev)
 		}
 		ippdrv->prop_list.ipp_id = ret;
 
-		DRM_DEBUG_KMS("count[%d]ippdrv[0x%x]ipp_id[%d]\n",
-			count++, (int)ippdrv, ret);
+		DRM_DEBUG_KMS("count[%d]ippdrv[%p]ipp_id[%d]\n",
+			count++, ippdrv, ret);
 
 		/* store parent device for node */
 		ippdrv->parent_dev = dev;
@@ -1668,7 +1668,7 @@ static int ipp_subdrv_open(struct drm_device *drm_dev, struct device *dev,
 
 	file_priv->ipp_dev = dev;
 
-	DRM_DEBUG_KMS("done priv[0x%x]\n", (int)dev);
+	DRM_DEBUG_KMS("done priv[%p]\n", dev);
 
 	return 0;
 }
@@ -1685,8 +1685,8 @@ static void ipp_subdrv_close(struct drm_device *drm_dev, struct device *dev,
 		mutex_lock(&ippdrv->cmd_lock);
 		list_for_each_entry_safe(c_node, tc_node,
 			&ippdrv->cmd_list, list) {
-			DRM_DEBUG_KMS("count[%d]ippdrv[0x%x]\n",
-				count++, (int)ippdrv);
+			DRM_DEBUG_KMS("count[%d]ippdrv[%p]\n",
+				count++, ippdrv);
 
 			if (c_node->filp == file) {
 				/*
diff --git a/drivers/gpu/drm/exynos/exynos_drm_mic.c b/drivers/gpu/drm/exynos/exynos_drm_mic.c
index 8994eab56ba8..9869d70e9e54 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_mic.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_mic.c
@@ -18,6 +18,7 @@
 #include <linux/of.h>
 #include <linux/of_graph.h>
 #include <linux/clk.h>
+#include <linux/component.h>
 #include <drm/drmP.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
@@ -306,9 +307,9 @@ exit:
 	return ret;
 }
 
-void mic_disable(struct drm_bridge *bridge) { }
+static void mic_disable(struct drm_bridge *bridge) { }
 
-void mic_post_disable(struct drm_bridge *bridge)
+static void mic_post_disable(struct drm_bridge *bridge)
 {
 	struct exynos_mic *mic = bridge->driver_private;
 	int i;
@@ -328,7 +329,7 @@ already_disabled:
 	mutex_unlock(&mic_mutex);
 }
 
-void mic_pre_enable(struct drm_bridge *bridge)
+static void mic_pre_enable(struct drm_bridge *bridge)
 {
 	struct exynos_mic *mic = bridge->driver_private;
 	int ret, i;
@@ -371,11 +372,35 @@ already_enabled:
 	mutex_unlock(&mic_mutex);
 }
 
-void mic_enable(struct drm_bridge *bridge) { }
+static void mic_enable(struct drm_bridge *bridge) { }
 
-void mic_destroy(struct drm_bridge *bridge)
+static const struct drm_bridge_funcs mic_bridge_funcs = {
+	.disable = mic_disable,
+	.post_disable = mic_post_disable,
+	.pre_enable = mic_pre_enable,
+	.enable = mic_enable,
+};
+
+static int exynos_mic_bind(struct device *dev, struct device *master,
+			   void *data)
 {
-	struct exynos_mic *mic = bridge->driver_private;
+	struct exynos_mic *mic = dev_get_drvdata(dev);
+	int ret;
+
+	mic->bridge.funcs = &mic_bridge_funcs;
+	mic->bridge.of_node = dev->of_node;
+	mic->bridge.driver_private = mic;
+	ret = drm_bridge_add(&mic->bridge);
+	if (ret)
+		DRM_ERROR("mic: Failed to add MIC to the global bridge list\n");
+
+	return ret;
+}
+
+static void exynos_mic_unbind(struct device *dev, struct device *master,
+			      void *data)
+{
+	struct exynos_mic *mic = dev_get_drvdata(dev);
 	int i;
 
 	mutex_lock(&mic_mutex);
@@ -387,16 +412,16 @@ void mic_destroy(struct drm_bridge *bridge)
 
 already_disabled:
 	mutex_unlock(&mic_mutex);
+
+	drm_bridge_remove(&mic->bridge);
 }
 
-struct drm_bridge_funcs mic_bridge_funcs = {
-	.disable = mic_disable,
-	.post_disable = mic_post_disable,
-	.pre_enable = mic_pre_enable,
-	.enable = mic_enable,
+static const struct component_ops exynos_mic_component_ops = {
+	.bind	= exynos_mic_bind,
+	.unbind	= exynos_mic_unbind,
 };
 
-int exynos_mic_probe(struct platform_device *pdev)
+static int exynos_mic_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct exynos_mic *mic;
@@ -435,17 +460,8 @@ int exynos_mic_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	mic->bridge.funcs = &mic_bridge_funcs;
-	mic->bridge.of_node = dev->of_node;
-	mic->bridge.driver_private = mic;
-	ret = drm_bridge_add(&mic->bridge);
-	if (ret) {
-		DRM_ERROR("mic: Failed to add MIC to the global bridge list\n");
-		goto err;
-	}
-
 	for (i = 0; i < NUM_CLKS; i++) {
-		mic->clks[i] = of_clk_get_by_name(dev->of_node, clk_names[i]);
+		mic->clks[i] = devm_clk_get(dev, clk_names[i]);
 		if (IS_ERR(mic->clks[i])) {
 			DRM_ERROR("mic: Failed to get clock (%s)\n",
 								clk_names[i]);
@@ -454,7 +470,10 @@ int exynos_mic_probe(struct platform_device *pdev)
 		}
 	}
 
+	platform_set_drvdata(pdev, mic);
+
 	DRM_DEBUG_KMS("MIC has been probed\n");
+	return component_add(dev, &exynos_mic_component_ops);
 
 err:
 	return ret;
@@ -462,14 +481,7 @@ err:
 
 static int exynos_mic_remove(struct platform_device *pdev)
 {
-	struct exynos_mic *mic = platform_get_drvdata(pdev);
-	int i;
-
-	drm_bridge_remove(&mic->bridge);
-
-	for (i = NUM_CLKS - 1; i > -1; i--)
-		clk_put(mic->clks[i]);
-
+	component_del(&pdev->dev, &exynos_mic_component_ops);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index 179311760bb7..d86227236f55 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -56,93 +56,213 @@ static int exynos_plane_get_size(int start, unsigned length, unsigned last)
 	return size;
 }
 
-static void exynos_plane_mode_set(struct drm_plane *plane,
-				  struct drm_crtc *crtc,
-				  struct drm_framebuffer *fb,
-				  int crtc_x, int crtc_y,
-				  unsigned int crtc_w, unsigned int crtc_h,
-				  uint32_t src_x, uint32_t src_y,
-				  uint32_t src_w, uint32_t src_h)
+static void exynos_plane_mode_set(struct exynos_drm_plane_state *exynos_state)
+
 {
-	struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
+	struct drm_plane_state *state = &exynos_state->base;
+	struct drm_crtc *crtc = exynos_state->base.crtc;
 	struct drm_display_mode *mode = &crtc->state->adjusted_mode;
+	int crtc_x, crtc_y;
+	unsigned int crtc_w, crtc_h;
+	unsigned int src_x, src_y;
+	unsigned int src_w, src_h;
 	unsigned int actual_w;
 	unsigned int actual_h;
 
+	/*
+	 * The original src/dest coordinates are stored in exynos_state->base,
+	 * but we want to keep another copy internal to our driver that we can
+	 * clip/modify ourselves.
+	 */
+
+	crtc_x = state->crtc_x;
+	crtc_y = state->crtc_y;
+	crtc_w = state->crtc_w;
+	crtc_h = state->crtc_h;
+
+	src_x = state->src_x >> 16;
+	src_y = state->src_y >> 16;
+	src_w = state->src_w >> 16;
+	src_h = state->src_h >> 16;
+
+	/* set ratio */
+	exynos_state->h_ratio = (src_w << 16) / crtc_w;
+	exynos_state->v_ratio = (src_h << 16) / crtc_h;
+
+	/* clip to visible area */
 	actual_w = exynos_plane_get_size(crtc_x, crtc_w, mode->hdisplay);
 	actual_h = exynos_plane_get_size(crtc_y, crtc_h, mode->vdisplay);
 
 	if (crtc_x < 0) {
 		if (actual_w)
-			src_x -= crtc_x;
+			src_x += ((-crtc_x) * exynos_state->h_ratio) >> 16;
 		crtc_x = 0;
 	}
 
 	if (crtc_y < 0) {
 		if (actual_h)
-			src_y -= crtc_y;
+			src_y += ((-crtc_y) * exynos_state->v_ratio) >> 16;
 		crtc_y = 0;
 	}
 
-	/* set ratio */
-	exynos_plane->h_ratio = (src_w << 16) / crtc_w;
-	exynos_plane->v_ratio = (src_h << 16) / crtc_h;
-
 	/* set drm framebuffer data. */
-	exynos_plane->src_x = src_x;
-	exynos_plane->src_y = src_y;
-	exynos_plane->src_w = (actual_w * exynos_plane->h_ratio) >> 16;
-	exynos_plane->src_h = (actual_h * exynos_plane->v_ratio) >> 16;
+	exynos_state->src.x = src_x;
+	exynos_state->src.y = src_y;
+	exynos_state->src.w = (actual_w * exynos_state->h_ratio) >> 16;
+	exynos_state->src.h = (actual_h * exynos_state->v_ratio) >> 16;
 
 	/* set plane range to be displayed. */
-	exynos_plane->crtc_x = crtc_x;
-	exynos_plane->crtc_y = crtc_y;
-	exynos_plane->crtc_w = actual_w;
-	exynos_plane->crtc_h = actual_h;
+	exynos_state->crtc.x = crtc_x;
+	exynos_state->crtc.y = crtc_y;
+	exynos_state->crtc.w = actual_w;
+	exynos_state->crtc.h = actual_h;
 
 	DRM_DEBUG_KMS("plane : offset_x/y(%d,%d), width/height(%d,%d)",
-			exynos_plane->crtc_x, exynos_plane->crtc_y,
-			exynos_plane->crtc_w, exynos_plane->crtc_h);
+			exynos_state->crtc.x, exynos_state->crtc.y,
+			exynos_state->crtc.w, exynos_state->crtc.h);
+}
+
+static void exynos_drm_plane_reset(struct drm_plane *plane)
+{
+	struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_plane_state *exynos_state;
+
+	if (plane->state) {
+		exynos_state = to_exynos_plane_state(plane->state);
+		if (exynos_state->base.fb)
+			drm_framebuffer_unreference(exynos_state->base.fb);
+		kfree(exynos_state);
+		plane->state = NULL;
+	}
+
+	exynos_state = kzalloc(sizeof(*exynos_state), GFP_KERNEL);
+	if (exynos_state) {
+		exynos_state->zpos = exynos_plane->config->zpos;
+		plane->state = &exynos_state->base;
+		plane->state->plane = plane;
+	}
+}
+
+static struct drm_plane_state *
+exynos_drm_plane_duplicate_state(struct drm_plane *plane)
+{
+	struct exynos_drm_plane_state *exynos_state;
+	struct exynos_drm_plane_state *copy;
+
+	exynos_state = to_exynos_plane_state(plane->state);
+	copy = kzalloc(sizeof(*exynos_state), GFP_KERNEL);
+	if (!copy)
+		return NULL;
+
+	__drm_atomic_helper_plane_duplicate_state(plane, &copy->base);
+	copy->zpos = exynos_state->zpos;
+	return &copy->base;
+}
+
+static void exynos_drm_plane_destroy_state(struct drm_plane *plane,
+					   struct drm_plane_state *old_state)
+{
+	struct exynos_drm_plane_state *old_exynos_state =
+					to_exynos_plane_state(old_state);
+	__drm_atomic_helper_plane_destroy_state(plane, old_state);
+	kfree(old_exynos_state);
+}
+
+static int exynos_drm_plane_atomic_set_property(struct drm_plane *plane,
+						struct drm_plane_state *state,
+						struct drm_property *property,
+						uint64_t val)
+{
+	struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
+	struct exynos_drm_plane_state *exynos_state =
+					to_exynos_plane_state(state);
+	struct exynos_drm_private *dev_priv = plane->dev->dev_private;
+	const struct exynos_drm_plane_config *config = exynos_plane->config;
 
-	plane->crtc = crtc;
+	if (property == dev_priv->plane_zpos_property &&
+	    (config->capabilities & EXYNOS_DRM_PLANE_CAP_ZPOS))
+		exynos_state->zpos = val;
+	else
+		return -EINVAL;
+
+	return 0;
+}
+
+static int exynos_drm_plane_atomic_get_property(struct drm_plane *plane,
+					  const struct drm_plane_state *state,
+					  struct drm_property *property,
+					  uint64_t *val)
+{
+	const struct exynos_drm_plane_state *exynos_state =
+		container_of(state, const struct exynos_drm_plane_state, base);
+	struct exynos_drm_private *dev_priv = plane->dev->dev_private;
+
+	if (property == dev_priv->plane_zpos_property)
+		*val = exynos_state->zpos;
+	else
+		return -EINVAL;
+
+	return 0;
 }
 
 static struct drm_plane_funcs exynos_plane_funcs = {
 	.update_plane	= drm_atomic_helper_update_plane,
 	.disable_plane	= drm_atomic_helper_disable_plane,
 	.destroy	= drm_plane_cleanup,
-	.reset = drm_atomic_helper_plane_reset,
-	.atomic_duplicate_state = drm_atomic_helper_plane_duplicate_state,
-	.atomic_destroy_state = drm_atomic_helper_plane_destroy_state,
+	.set_property	= drm_atomic_helper_plane_set_property,
+	.reset		= exynos_drm_plane_reset,
+	.atomic_duplicate_state = exynos_drm_plane_duplicate_state,
+	.atomic_destroy_state = exynos_drm_plane_destroy_state,
+	.atomic_set_property = exynos_drm_plane_atomic_set_property,
+	.atomic_get_property = exynos_drm_plane_atomic_get_property,
 };
 
+static int
+exynos_drm_plane_check_size(const struct exynos_drm_plane_config *config,
+			    struct exynos_drm_plane_state *state)
+{
+	bool width_ok = false, height_ok = false;
+
+	if (config->capabilities & EXYNOS_DRM_PLANE_CAP_SCALE)
+		return 0;
+
+	if (state->src.w == state->crtc.w)
+		width_ok = true;
+
+	if (state->src.h == state->crtc.h)
+		height_ok = true;
+
+	if ((config->capabilities & EXYNOS_DRM_PLANE_CAP_DOUBLE) &&
+	    state->h_ratio == (1 << 15))
+		width_ok = true;
+
+	if ((config->capabilities & EXYNOS_DRM_PLANE_CAP_DOUBLE) &&
+	    state->v_ratio == (1 << 15))
+		height_ok = true;
+
+	if (width_ok & height_ok)
+		return 0;
+
+	DRM_DEBUG_KMS("scaling mode is not supported");
+	return -ENOTSUPP;
+}
+
 static int exynos_plane_atomic_check(struct drm_plane *plane,
 				     struct drm_plane_state *state)
 {
 	struct exynos_drm_plane *exynos_plane = to_exynos_plane(plane);
-	int nr;
-	int i;
+	struct exynos_drm_plane_state *exynos_state =
+						to_exynos_plane_state(state);
+	int ret = 0;
 
-	if (!state->fb)
+	if (!state->crtc || !state->fb)
 		return 0;
 
-	nr = drm_format_num_planes(state->fb->pixel_format);
-	for (i = 0; i < nr; i++) {
-		struct exynos_drm_gem *exynos_gem =
-					exynos_drm_fb_gem(state->fb, i);
-		if (!exynos_gem) {
-			DRM_DEBUG_KMS("gem object is null\n");
-			return -EFAULT;
-		}
-
-		exynos_plane->dma_addr[i] = exynos_gem->dma_addr +
-					    state->fb->offsets[i];
+	/* translate state into exynos_state */
+	exynos_plane_mode_set(exynos_state);
 
-		DRM_DEBUG_KMS("buffer: %d, dma_addr = 0x%lx\n",
-				i, (unsigned long)exynos_plane->dma_addr[i]);
-	}
-
-	return 0;
+	ret = exynos_drm_plane_check_size(exynos_plane->config, exynos_state);
+	return ret;
 }
 
 static void exynos_plane_atomic_update(struct drm_plane *plane,
@@ -155,12 +275,7 @@ static void exynos_plane_atomic_update(struct drm_plane *plane,
 	if (!state->crtc)
 		return;
 
-	exynos_plane_mode_set(plane, state->crtc, state->fb,
-			      state->crtc_x, state->crtc_y,
-			      state->crtc_w, state->crtc_h,
-			      state->src_x >> 16, state->src_y >> 16,
-			      state->src_w >> 16, state->src_h >> 16);
-
+	plane->crtc = state->crtc;
 	exynos_plane->pending_fb = state->fb;
 
 	if (exynos_crtc->ops->update_plane)
@@ -177,8 +292,7 @@ static void exynos_plane_atomic_disable(struct drm_plane *plane,
 		return;
 
 	if (exynos_crtc->ops->disable_plane)
-		exynos_crtc->ops->disable_plane(exynos_crtc,
-						exynos_plane);
+		exynos_crtc->ops->disable_plane(exynos_crtc, exynos_plane);
 }
 
 static const struct drm_plane_helper_funcs plane_helper_funcs = {
@@ -196,8 +310,8 @@ static void exynos_plane_attach_zpos_property(struct drm_plane *plane,
 
 	prop = dev_priv->plane_zpos_property;
 	if (!prop) {
-		prop = drm_property_create_range(dev, DRM_MODE_PROP_IMMUTABLE,
-						 "zpos", 0, MAX_PLANE - 1);
+		prop = drm_property_create_range(dev, 0, "zpos",
+						 0, MAX_PLANE - 1);
 		if (!prop)
 			return;
 
@@ -207,28 +321,19 @@ static void exynos_plane_attach_zpos_property(struct drm_plane *plane,
 	drm_object_attach_property(&plane->base, prop, zpos);
 }
 
-enum drm_plane_type exynos_plane_get_type(unsigned int zpos,
-					  unsigned int cursor_win)
-{
-		if (zpos == DEFAULT_WIN)
-			return DRM_PLANE_TYPE_PRIMARY;
-		else if (zpos == cursor_win)
-			return DRM_PLANE_TYPE_CURSOR;
-		else
-			return DRM_PLANE_TYPE_OVERLAY;
-}
-
 int exynos_plane_init(struct drm_device *dev,
 		      struct exynos_drm_plane *exynos_plane,
-		      unsigned long possible_crtcs, enum drm_plane_type type,
-		      const uint32_t *formats, unsigned int fcount,
-		      unsigned int zpos)
+		      unsigned int index, unsigned long possible_crtcs,
+		      const struct exynos_drm_plane_config *config)
 {
 	int err;
 
-	err = drm_universal_plane_init(dev, &exynos_plane->base, possible_crtcs,
-				       &exynos_plane_funcs, formats, fcount,
-				       type);
+	err = drm_universal_plane_init(dev, &exynos_plane->base,
+				       possible_crtcs,
+				       &exynos_plane_funcs,
+				       config->pixel_formats,
+				       config->num_pixel_formats,
+				       config->type, NULL);
 	if (err) {
 		DRM_ERROR("failed to initialize plane\n");
 		return err;
@@ -236,10 +341,10 @@ int exynos_plane_init(struct drm_device *dev,
 
 	drm_plane_helper_add(&exynos_plane->base, &plane_helper_funcs);
 
-	exynos_plane->zpos = zpos;
+	exynos_plane->index = index;
+	exynos_plane->config = config;
 
-	if (type == DRM_PLANE_TYPE_OVERLAY)
-		exynos_plane_attach_zpos_property(&exynos_plane->base, zpos);
+	exynos_plane_attach_zpos_property(&exynos_plane->base, config->zpos);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.h b/drivers/gpu/drm/exynos/exynos_drm_plane.h
index abb641e64c23..9aafad164cdf 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.h
@@ -9,10 +9,7 @@
  *
  */
 
-enum drm_plane_type exynos_plane_get_type(unsigned int zpos,
-					  unsigned int cursor_win);
 int exynos_plane_init(struct drm_device *dev,
-		      struct exynos_drm_plane *exynos_plane,
-		      unsigned long possible_crtcs, enum drm_plane_type type,
-		      const uint32_t *formats, unsigned int fcount,
-		      unsigned int zpos);
+		      struct exynos_drm_plane *exynos_plane, unsigned int index,
+		      unsigned long possible_crtcs,
+		      const struct exynos_drm_plane_config *config);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_rotator.c b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
index 2f5c118f4c8e..ce59f4443394 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_rotator.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
@@ -754,7 +754,7 @@ static int rotator_probe(struct platform_device *pdev)
 		goto err_ippdrv_register;
 	}
 
-	DRM_DEBUG_KMS("ippdrv[0x%x]\n", (int)ippdrv);
+	DRM_DEBUG_KMS("ippdrv[%p]\n", ippdrv);
 
 	platform_set_drvdata(pdev, rot);
 
@@ -790,10 +790,10 @@ static int rotator_remove(struct platform_device *pdev)
 static int rotator_clk_crtl(struct rot_context *rot, bool enable)
 {
 	if (enable) {
-		clk_enable(rot->clock);
+		clk_prepare_enable(rot->clock);
 		rot->suspended = false;
 	} else {
-		clk_disable(rot->clock);
+		clk_disable_unprepare(rot->clock);
 		rot->suspended = true;
 	}
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 669362c53f49..b605bd7395ec 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -24,12 +24,12 @@
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_crtc.h"
+#include "exynos_drm_fb.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_vidi.h"
 
 /* vidi has totally three virtual windows. */
 #define WINDOWS_NR		3
-#define CURSOR_WIN		2
 
 #define ctx_from_connector(c)	container_of(c, struct vidi_context, \
 					connector)
@@ -89,6 +89,12 @@ static const uint32_t formats[] = {
 	DRM_FORMAT_NV12,
 };
 
+static const enum drm_plane_type vidi_win_types[WINDOWS_NR] = {
+	DRM_PLANE_TYPE_PRIMARY,
+	DRM_PLANE_TYPE_OVERLAY,
+	DRM_PLANE_TYPE_CURSOR,
+};
+
 static int vidi_enable_vblank(struct exynos_drm_crtc *crtc)
 {
 	struct vidi_context *ctx = crtc->ctx;
@@ -125,12 +131,15 @@ static void vidi_disable_vblank(struct exynos_drm_crtc *crtc)
 static void vidi_update_plane(struct exynos_drm_crtc *crtc,
 			      struct exynos_drm_plane *plane)
 {
+	struct drm_plane_state *state = plane->base.state;
 	struct vidi_context *ctx = crtc->ctx;
+	dma_addr_t addr;
 
 	if (ctx->suspended)
 		return;
 
-	DRM_DEBUG_KMS("dma_addr = %pad\n", plane->dma_addr);
+	addr = exynos_drm_fb_dma_addr(state->fb, 0);
+	DRM_DEBUG_KMS("dma_addr = %pad\n", &addr);
 
 	if (ctx->vblank_on)
 		schedule_work(&ctx->work);
@@ -214,7 +223,7 @@ static void vidi_fake_vblank_handler(struct work_struct *work)
 	}
 }
 
-static int vidi_show_connection(struct device *dev,
+static ssize_t vidi_show_connection(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
 	struct vidi_context *ctx = dev_get_drvdata(dev);
@@ -229,7 +238,7 @@ static int vidi_show_connection(struct device *dev,
 	return rc;
 }
 
-static int vidi_store_connection(struct device *dev,
+static ssize_t vidi_store_connection(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t len)
 {
@@ -285,7 +294,9 @@ int vidi_connection_ioctl(struct drm_device *drm_dev, void *data,
 	}
 
 	if (vidi->connection) {
-		struct edid *raw_edid  = (struct edid *)(uint32_t)vidi->edid;
+		struct edid *raw_edid;
+
+		raw_edid = (struct edid *)(unsigned long)vidi->edid;
 		if (!drm_edid_is_valid(raw_edid)) {
 			DRM_DEBUG_KMS("edid data is invalid.\n");
 			return -EINVAL;
@@ -330,7 +341,7 @@ static void vidi_connector_destroy(struct drm_connector *connector)
 {
 }
 
-static struct drm_connector_funcs vidi_connector_funcs = {
+static const struct drm_connector_funcs vidi_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = vidi_detect,
@@ -374,7 +385,7 @@ static struct drm_encoder *vidi_best_encoder(struct drm_connector *connector)
 	return &ctx->encoder;
 }
 
-static struct drm_connector_helper_funcs vidi_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs vidi_connector_helper_funcs = {
 	.get_modes = vidi_get_modes,
 	.best_encoder = vidi_best_encoder,
 };
@@ -422,14 +433,14 @@ static void exynos_vidi_disable(struct drm_encoder *encoder)
 {
 }
 
-static struct drm_encoder_helper_funcs exynos_vidi_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs exynos_vidi_encoder_helper_funcs = {
 	.mode_fixup = exynos_vidi_mode_fixup,
 	.mode_set = exynos_vidi_mode_set,
 	.enable = exynos_vidi_enable,
 	.disable = exynos_vidi_disable,
 };
 
-static struct drm_encoder_funcs exynos_vidi_encoder_funcs = {
+static const struct drm_encoder_funcs exynos_vidi_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -439,17 +450,21 @@ static int vidi_bind(struct device *dev, struct device *master, void *data)
 	struct drm_device *drm_dev = data;
 	struct drm_encoder *encoder = &ctx->encoder;
 	struct exynos_drm_plane *exynos_plane;
-	enum drm_plane_type type;
-	unsigned int zpos;
+	struct exynos_drm_plane_config plane_config = { 0 };
+	unsigned int i;
 	int pipe, ret;
 
 	vidi_ctx_initialize(ctx, drm_dev);
 
-	for (zpos = 0; zpos < WINDOWS_NR; zpos++) {
-		type = exynos_plane_get_type(zpos, CURSOR_WIN);
-		ret = exynos_plane_init(drm_dev, &ctx->planes[zpos],
-					1 << ctx->pipe, type, formats,
-					ARRAY_SIZE(formats), zpos);
+	plane_config.pixel_formats = formats;
+	plane_config.num_pixel_formats = ARRAY_SIZE(formats);
+
+	for (i = 0; i < WINDOWS_NR; i++) {
+		plane_config.zpos = i;
+		plane_config.type = vidi_win_types[i];
+
+		ret = exynos_plane_init(drm_dev, &ctx->planes[i], i,
+					1 << ctx->pipe, &plane_config);
 		if (ret)
 			return ret;
 	}
@@ -473,7 +488,7 @@ static int vidi_bind(struct device *dev, struct device *master, void *data)
 	DRM_DEBUG_KMS("possible_crtcs = 0x%x\n", encoder->possible_crtcs);
 
 	drm_encoder_init(drm_dev, encoder, &exynos_vidi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(encoder, &exynos_vidi_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index 57b675563e94..21a29dbce18c 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -113,7 +113,7 @@ struct hdmi_context {
 	void __iomem			*regs_hdmiphy;
 	struct i2c_client		*hdmiphy_port;
 	struct i2c_adapter		*ddc_adpt;
-	struct gpio_desc 		*hpd_gpio;
+	struct gpio_desc		*hpd_gpio;
 	int				irq;
 	struct regmap			*pmureg;
 	struct clk			*hdmi;
@@ -956,7 +956,7 @@ static void hdmi_connector_destroy(struct drm_connector *connector)
 	drm_connector_cleanup(connector);
 }
 
-static struct drm_connector_funcs hdmi_connector_funcs = {
+static const struct drm_connector_funcs hdmi_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = hdmi_detect,
@@ -1030,7 +1030,7 @@ static struct drm_encoder *hdmi_best_encoder(struct drm_connector *connector)
 	return &hdata->encoder;
 }
 
-static struct drm_connector_helper_funcs hdmi_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs hdmi_connector_helper_funcs = {
 	.get_modes = hdmi_get_modes,
 	.mode_valid = hdmi_mode_valid,
 	.best_encoder = hdmi_best_encoder,
@@ -1588,8 +1588,6 @@ static void hdmi_enable(struct drm_encoder *encoder)
 	if (hdata->powered)
 		return;
 
-	hdata->powered = true;
-
 	pm_runtime_get_sync(hdata->dev);
 
 	if (regulator_bulk_enable(ARRAY_SIZE(supply), hdata->regul_bulk))
@@ -1599,10 +1597,9 @@ static void hdmi_enable(struct drm_encoder *encoder)
 	regmap_update_bits(hdata->pmureg, PMU_HDMI_PHY_CONTROL,
 			PMU_HDMI_PHY_ENABLE_BIT, 1);
 
-	clk_prepare_enable(hdata->hdmi);
-	clk_prepare_enable(hdata->sclk_hdmi);
-
 	hdmi_conf_apply(hdata);
+
+	hdata->powered = true;
 }
 
 static void hdmi_disable(struct drm_encoder *encoder)
@@ -1633,9 +1630,6 @@ static void hdmi_disable(struct drm_encoder *encoder)
 
 	cancel_delayed_work(&hdata->hotplug_work);
 
-	clk_disable_unprepare(hdata->sclk_hdmi);
-	clk_disable_unprepare(hdata->hdmi);
-
 	/* reset pmu hdmiphy control bit to disable hdmiphy */
 	regmap_update_bits(hdata->pmureg, PMU_HDMI_PHY_CONTROL,
 			PMU_HDMI_PHY_ENABLE_BIT, 0);
@@ -1647,14 +1641,14 @@ static void hdmi_disable(struct drm_encoder *encoder)
 	hdata->powered = false;
 }
 
-static struct drm_encoder_helper_funcs exynos_hdmi_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs exynos_hdmi_encoder_helper_funcs = {
 	.mode_fixup	= hdmi_mode_fixup,
 	.mode_set	= hdmi_mode_set,
 	.enable		= hdmi_enable,
 	.disable	= hdmi_disable,
 };
 
-static struct drm_encoder_funcs exynos_hdmi_encoder_funcs = {
+static const struct drm_encoder_funcs exynos_hdmi_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -1793,7 +1787,7 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 	DRM_DEBUG_KMS("possible_crtcs = 0x%x\n", encoder->possible_crtcs);
 
 	drm_encoder_init(drm_dev, encoder, &exynos_hdmi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(encoder, &exynos_hdmi_encoder_helper_funcs);
 
@@ -1978,12 +1972,49 @@ static int hdmi_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM
+static int exynos_hdmi_suspend(struct device *dev)
+{
+	struct hdmi_context *hdata = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(hdata->sclk_hdmi);
+	clk_disable_unprepare(hdata->hdmi);
+
+	return 0;
+}
+
+static int exynos_hdmi_resume(struct device *dev)
+{
+	struct hdmi_context *hdata = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(hdata->hdmi);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the hdmi clk [%d]\n", ret);
+		return ret;
+	}
+	ret = clk_prepare_enable(hdata->sclk_hdmi);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the sclk_mixer clk [%d]\n",
+			  ret);
+		return ret;
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops exynos_hdmi_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos_hdmi_suspend, exynos_hdmi_resume, NULL)
+};
+
 struct platform_driver hdmi_driver = {
 	.probe		= hdmi_probe,
 	.remove		= hdmi_remove,
 	.driver		= {
 		.name	= "exynos-hdmi",
 		.owner	= THIS_MODULE,
+		.pm	= &exynos_hdmi_pm_ops,
 		.of_match_table = hdmi_match_types,
 	},
 };
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index d09f8f9a8939..0a5a60005f7e 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -37,12 +37,12 @@
 
 #include "exynos_drm_drv.h"
 #include "exynos_drm_crtc.h"
+#include "exynos_drm_fb.h"
 #include "exynos_drm_plane.h"
 #include "exynos_drm_iommu.h"
 
 #define MIXER_WIN_NR		3
 #define VP_DEFAULT_WIN		2
-#define CURSOR_WIN		1
 
 /* The pixelformats that are natively supported by the mixer. */
 #define MXR_FORMAT_RGB565	4
@@ -76,7 +76,9 @@ enum mixer_flag_bits {
 
 static const uint32_t mixer_formats[] = {
 	DRM_FORMAT_XRGB4444,
+	DRM_FORMAT_ARGB4444,
 	DRM_FORMAT_XRGB1555,
+	DRM_FORMAT_ARGB1555,
 	DRM_FORMAT_RGB565,
 	DRM_FORMAT_XRGB8888,
 	DRM_FORMAT_ARGB8888,
@@ -111,6 +113,31 @@ struct mixer_drv_data {
 	bool					has_sclk;
 };
 
+static const struct exynos_drm_plane_config plane_configs[MIXER_WIN_NR] = {
+	{
+		.zpos = 0,
+		.type = DRM_PLANE_TYPE_PRIMARY,
+		.pixel_formats = mixer_formats,
+		.num_pixel_formats = ARRAY_SIZE(mixer_formats),
+		.capabilities = EXYNOS_DRM_PLANE_CAP_DOUBLE |
+				EXYNOS_DRM_PLANE_CAP_ZPOS,
+	}, {
+		.zpos = 1,
+		.type = DRM_PLANE_TYPE_CURSOR,
+		.pixel_formats = mixer_formats,
+		.num_pixel_formats = ARRAY_SIZE(mixer_formats),
+		.capabilities = EXYNOS_DRM_PLANE_CAP_DOUBLE |
+				EXYNOS_DRM_PLANE_CAP_ZPOS,
+	}, {
+		.zpos = 2,
+		.type = DRM_PLANE_TYPE_OVERLAY,
+		.pixel_formats = vp_formats,
+		.num_pixel_formats = ARRAY_SIZE(vp_formats),
+		.capabilities = EXYNOS_DRM_PLANE_CAP_SCALE |
+				EXYNOS_DRM_PLANE_CAP_ZPOS,
+	},
+};
+
 static const u8 filter_y_horiz_tap8[] = {
 	0,	-1,	-1,	-1,	-1,	-1,	-1,	-1,
 	-1,	-1,	-1,	-1,	-1,	0,	0,	0,
@@ -140,6 +167,18 @@ static const u8 filter_cr_horiz_tap4[] = {
 	70,	59,	48,	37,	27,	19,	11,	5,
 };
 
+static inline bool is_alpha_format(unsigned int pixel_format)
+{
+	switch (pixel_format) {
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_ARGB1555:
+	case DRM_FORMAT_ARGB4444:
+		return true;
+	default:
+		return false;
+	}
+}
+
 static inline u32 vp_reg_read(struct mixer_resources *res, u32 reg_id)
 {
 	return readl(res->vp_regs + reg_id);
@@ -269,6 +308,37 @@ static void vp_default_filter(struct mixer_resources *res)
 		filter_cr_horiz_tap4, sizeof(filter_cr_horiz_tap4));
 }
 
+static void mixer_cfg_gfx_blend(struct mixer_context *ctx, unsigned int win,
+				bool alpha)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val;
+
+	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
+	if (alpha) {
+		/* blending based on pixel alpha */
+		val |= MXR_GRP_CFG_BLEND_PRE_MUL;
+		val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
+	}
+	mixer_reg_writemask(res, MXR_GRAPHIC_CFG(win),
+			    val, MXR_GRP_CFG_MISC_MASK);
+}
+
+static void mixer_cfg_vp_blend(struct mixer_context *ctx)
+{
+	struct mixer_resources *res = &ctx->mixer_res;
+	u32 val;
+
+	/*
+	 * No blending at the moment since the NV12/NV21 pixelformats don't
+	 * have an alpha channel. However the mixer supports a global alpha
+	 * value for a layer. Once this functionality is exposed, we can
+	 * support blending of the video layer through this.
+	 */
+	val = 0;
+	mixer_reg_write(res, MXR_VIDEO_CFG, val);
+}
+
 static void mixer_vsync_set_update(struct mixer_context *ctx, bool enable)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
@@ -350,7 +420,7 @@ static void mixer_cfg_rgb_fmt(struct mixer_context *ctx, unsigned int height)
 }
 
 static void mixer_cfg_layer(struct mixer_context *ctx, unsigned int win,
-				bool enable)
+			    unsigned int priority, bool enable)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
 	u32 val = enable ? ~0 : 0;
@@ -358,20 +428,24 @@ static void mixer_cfg_layer(struct mixer_context *ctx, unsigned int win,
 	switch (win) {
 	case 0:
 		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_GRP0_ENABLE);
+		mixer_reg_writemask(res, MXR_LAYER_CFG,
+				    MXR_LAYER_CFG_GRP0_VAL(priority),
+				    MXR_LAYER_CFG_GRP0_MASK);
 		break;
 	case 1:
 		mixer_reg_writemask(res, MXR_CFG, val, MXR_CFG_GRP1_ENABLE);
+		mixer_reg_writemask(res, MXR_LAYER_CFG,
+				    MXR_LAYER_CFG_GRP1_VAL(priority),
+				    MXR_LAYER_CFG_GRP1_MASK);
 		break;
-	case 2:
+	case VP_DEFAULT_WIN:
 		if (ctx->vp_enabled) {
 			vp_reg_writemask(res, VP_ENABLE, val, VP_ENABLE_ON);
 			mixer_reg_writemask(res, MXR_CFG, val,
 				MXR_CFG_VP_ENABLE);
-
-			/* control blending of graphic layer 0 */
-			mixer_reg_writemask(res, MXR_GRAPHIC_CFG(0), val,
-					MXR_GRP_CFG_BLEND_PRE_MUL |
-					MXR_GRP_CFG_PIXEL_BLEND_EN);
+			mixer_reg_writemask(res, MXR_LAYER_CFG,
+					    MXR_LAYER_CFG_VP_VAL(priority),
+					    MXR_LAYER_CFG_VP_MASK);
 		}
 		break;
 	}
@@ -399,10 +473,11 @@ static void mixer_stop(struct mixer_context *ctx)
 static void vp_video_buffer(struct mixer_context *ctx,
 			    struct exynos_drm_plane *plane)
 {
+	struct exynos_drm_plane_state *state =
+				to_exynos_plane_state(plane->base.state);
+	struct drm_display_mode *mode = &state->base.crtc->state->adjusted_mode;
 	struct mixer_resources *res = &ctx->mixer_res;
-	struct drm_plane_state *state = plane->base.state;
-	struct drm_framebuffer *fb = state->fb;
-	struct drm_display_mode *mode = &state->crtc->mode;
+	struct drm_framebuffer *fb = state->base.fb;
 	unsigned long flags;
 	dma_addr_t luma_addr[2], chroma_addr[2];
 	bool tiled_mode = false;
@@ -422,8 +497,8 @@ static void vp_video_buffer(struct mixer_context *ctx,
 		return;
 	}
 
-	luma_addr[0] = plane->dma_addr[0];
-	chroma_addr[0] = plane->dma_addr[1];
+	luma_addr[0] = exynos_drm_fb_dma_addr(fb, 0);
+	chroma_addr[0] = exynos_drm_fb_dma_addr(fb, 1);
 
 	if (mode->flags & DRM_MODE_FLAG_INTERLACE) {
 		ctx->interlace = true;
@@ -441,7 +516,6 @@ static void vp_video_buffer(struct mixer_context *ctx,
 	}
 
 	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(ctx, false);
 
 	/* interlace or progressive scan mode */
 	val = (ctx->interlace ? ~0 : 0);
@@ -459,24 +533,24 @@ static void vp_video_buffer(struct mixer_context *ctx,
 	vp_reg_write(res, VP_IMG_SIZE_C, VP_IMG_HSIZE(fb->pitches[0]) |
 		VP_IMG_VSIZE(fb->height / 2));
 
-	vp_reg_write(res, VP_SRC_WIDTH, plane->src_w);
-	vp_reg_write(res, VP_SRC_HEIGHT, plane->src_h);
+	vp_reg_write(res, VP_SRC_WIDTH, state->src.w);
+	vp_reg_write(res, VP_SRC_HEIGHT, state->src.h);
 	vp_reg_write(res, VP_SRC_H_POSITION,
-			VP_SRC_H_POSITION_VAL(plane->src_x));
-	vp_reg_write(res, VP_SRC_V_POSITION, plane->src_y);
+			VP_SRC_H_POSITION_VAL(state->src.x));
+	vp_reg_write(res, VP_SRC_V_POSITION, state->src.y);
 
-	vp_reg_write(res, VP_DST_WIDTH, plane->crtc_w);
-	vp_reg_write(res, VP_DST_H_POSITION, plane->crtc_x);
+	vp_reg_write(res, VP_DST_WIDTH, state->crtc.w);
+	vp_reg_write(res, VP_DST_H_POSITION, state->crtc.x);
 	if (ctx->interlace) {
-		vp_reg_write(res, VP_DST_HEIGHT, plane->crtc_h / 2);
-		vp_reg_write(res, VP_DST_V_POSITION, plane->crtc_y / 2);
+		vp_reg_write(res, VP_DST_HEIGHT, state->crtc.h / 2);
+		vp_reg_write(res, VP_DST_V_POSITION, state->crtc.y / 2);
 	} else {
-		vp_reg_write(res, VP_DST_HEIGHT, plane->crtc_h);
-		vp_reg_write(res, VP_DST_V_POSITION, plane->crtc_y);
+		vp_reg_write(res, VP_DST_HEIGHT, state->crtc.h);
+		vp_reg_write(res, VP_DST_V_POSITION, state->crtc.y);
 	}
 
-	vp_reg_write(res, VP_H_RATIO, plane->h_ratio);
-	vp_reg_write(res, VP_V_RATIO, plane->v_ratio);
+	vp_reg_write(res, VP_H_RATIO, state->h_ratio);
+	vp_reg_write(res, VP_V_RATIO, state->v_ratio);
 
 	vp_reg_write(res, VP_ENDIAN_MODE, VP_ENDIAN_MODE_LITTLE);
 
@@ -488,10 +562,10 @@ static void vp_video_buffer(struct mixer_context *ctx,
 
 	mixer_cfg_scan(ctx, mode->vdisplay);
 	mixer_cfg_rgb_fmt(ctx, mode->vdisplay);
-	mixer_cfg_layer(ctx, plane->zpos, true);
+	mixer_cfg_layer(ctx, plane->index, state->zpos + 1, true);
+	mixer_cfg_vp_blend(ctx);
 	mixer_run(ctx);
 
-	mixer_vsync_set_update(ctx, true);
 	spin_unlock_irqrestore(&res->reg_slock, flags);
 
 	mixer_regs_dump(ctx);
@@ -505,39 +579,16 @@ static void mixer_layer_update(struct mixer_context *ctx)
 	mixer_reg_writemask(res, MXR_CFG, ~0, MXR_CFG_LAYER_UPDATE);
 }
 
-static int mixer_setup_scale(const struct exynos_drm_plane *plane,
-		unsigned int *x_ratio, unsigned int *y_ratio)
-{
-	if (plane->crtc_w != plane->src_w) {
-		if (plane->crtc_w == 2 * plane->src_w)
-			*x_ratio = 1;
-		else
-			goto fail;
-	}
-
-	if (plane->crtc_h != plane->src_h) {
-		if (plane->crtc_h == 2 * plane->src_h)
-			*y_ratio = 1;
-		else
-			goto fail;
-	}
-
-	return 0;
-
-fail:
-	DRM_DEBUG_KMS("only 2x width/height scaling of plane supported\n");
-	return -ENOTSUPP;
-}
-
 static void mixer_graph_buffer(struct mixer_context *ctx,
 			       struct exynos_drm_plane *plane)
 {
+	struct exynos_drm_plane_state *state =
+				to_exynos_plane_state(plane->base.state);
+	struct drm_display_mode *mode = &state->base.crtc->state->adjusted_mode;
 	struct mixer_resources *res = &ctx->mixer_res;
-	struct drm_plane_state *state = plane->base.state;
-	struct drm_framebuffer *fb = state->fb;
-	struct drm_display_mode *mode = &state->crtc->mode;
+	struct drm_framebuffer *fb = state->base.fb;
 	unsigned long flags;
-	unsigned int win = plane->zpos;
+	unsigned int win = plane->index;
 	unsigned int x_ratio = 0, y_ratio = 0;
 	unsigned int src_x_offset, src_y_offset, dst_x_offset, dst_y_offset;
 	dma_addr_t dma_addr;
@@ -546,10 +597,12 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_XRGB4444:
+	case DRM_FORMAT_ARGB4444:
 		fmt = MXR_FORMAT_ARGB4444;
 		break;
 
 	case DRM_FORMAT_XRGB1555:
+	case DRM_FORMAT_ARGB1555:
 		fmt = MXR_FORMAT_ARGB1555;
 		break;
 
@@ -567,17 +620,17 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 		return;
 	}
 
-	/* check if mixer supports requested scaling setup */
-	if (mixer_setup_scale(plane, &x_ratio, &y_ratio))
-		return;
+	/* ratio is already checked by common plane code */
+	x_ratio = state->h_ratio == (1 << 15);
+	y_ratio = state->v_ratio == (1 << 15);
 
-	dst_x_offset = plane->crtc_x;
-	dst_y_offset = plane->crtc_y;
+	dst_x_offset = state->crtc.x;
+	dst_y_offset = state->crtc.y;
 
 	/* converting dma address base and source offset */
-	dma_addr = plane->dma_addr[0]
-		+ (plane->src_x * fb->bits_per_pixel >> 3)
-		+ (plane->src_y * fb->pitches[0]);
+	dma_addr = exynos_drm_fb_dma_addr(fb, 0)
+		+ (state->src.x * fb->bits_per_pixel >> 3)
+		+ (state->src.y * fb->pitches[0]);
 	src_x_offset = 0;
 	src_y_offset = 0;
 
@@ -587,7 +640,6 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 		ctx->interlace = false;
 
 	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(ctx, false);
 
 	/* setup format */
 	mixer_reg_writemask(res, MXR_GRAPHIC_CFG(win),
@@ -605,8 +657,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 		mixer_reg_write(res, MXR_RESOLUTION, val);
 	}
 
-	val  = MXR_GRP_WH_WIDTH(plane->src_w);
-	val |= MXR_GRP_WH_HEIGHT(plane->src_h);
+	val  = MXR_GRP_WH_WIDTH(state->src.w);
+	val |= MXR_GRP_WH_HEIGHT(state->src.h);
 	val |= MXR_GRP_WH_H_SCALE(x_ratio);
 	val |= MXR_GRP_WH_V_SCALE(y_ratio);
 	mixer_reg_write(res, MXR_GRAPHIC_WH(win), val);
@@ -626,7 +678,8 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 
 	mixer_cfg_scan(ctx, mode->vdisplay);
 	mixer_cfg_rgb_fmt(ctx, mode->vdisplay);
-	mixer_cfg_layer(ctx, win, true);
+	mixer_cfg_layer(ctx, win, state->zpos + 1, true);
+	mixer_cfg_gfx_blend(ctx, win, is_alpha_format(fb->pixel_format));
 
 	/* layer update mandatory for mixer 16.0.33.0 */
 	if (ctx->mxr_ver == MXR_VER_16_0_33_0 ||
@@ -635,7 +688,6 @@ static void mixer_graph_buffer(struct mixer_context *ctx,
 
 	mixer_run(ctx);
 
-	mixer_vsync_set_update(ctx, true);
 	spin_unlock_irqrestore(&res->reg_slock, flags);
 
 	mixer_regs_dump(ctx);
@@ -660,10 +712,8 @@ static void mixer_win_reset(struct mixer_context *ctx)
 {
 	struct mixer_resources *res = &ctx->mixer_res;
 	unsigned long flags;
-	u32 val; /* value stored to register */
 
 	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(ctx, false);
 
 	mixer_reg_writemask(res, MXR_CFG, MXR_CFG_DST_HDMI, MXR_CFG_DST_MASK);
 
@@ -674,40 +724,14 @@ static void mixer_win_reset(struct mixer_context *ctx)
 	mixer_reg_writemask(res, MXR_STATUS, MXR_STATUS_16_BURST,
 		MXR_STATUS_BURST_MASK);
 
-	/* setting default layer priority: layer1 > layer0 > video
-	 * because typical usage scenario would be
-	 * layer1 - OSD
-	 * layer0 - framebuffer
-	 * video - video overlay
-	 */
-	val = MXR_LAYER_CFG_GRP1_VAL(3);
-	val |= MXR_LAYER_CFG_GRP0_VAL(2);
-	if (ctx->vp_enabled)
-		val |= MXR_LAYER_CFG_VP_VAL(1);
-	mixer_reg_write(res, MXR_LAYER_CFG, val);
+	/* reset default layer priority */
+	mixer_reg_write(res, MXR_LAYER_CFG, 0);
 
 	/* setting background color */
 	mixer_reg_write(res, MXR_BG_COLOR0, 0x008080);
 	mixer_reg_write(res, MXR_BG_COLOR1, 0x008080);
 	mixer_reg_write(res, MXR_BG_COLOR2, 0x008080);
 
-	/* setting graphical layers */
-	val  = MXR_GRP_CFG_COLOR_KEY_DISABLE; /* no blank key */
-	val |= MXR_GRP_CFG_WIN_BLEND_EN;
-	val |= MXR_GRP_CFG_ALPHA_VAL(0xff); /* non-transparent alpha */
-
-	/* Don't blend layer 0 onto the mixer background */
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(0), val);
-
-	/* Blend layer 1 into layer 0 */
-	val |= MXR_GRP_CFG_BLEND_PRE_MUL;
-	val |= MXR_GRP_CFG_PIXEL_BLEND_EN;
-	mixer_reg_write(res, MXR_GRAPHIC_CFG(1), val);
-
-	/* setting video layers */
-	val = MXR_GRP_CFG_ALPHA_VAL(0);
-	mixer_reg_write(res, MXR_VIDEO_CFG, val);
-
 	if (ctx->vp_enabled) {
 		/* configuration of Video Processor Registers */
 		vp_win_reset(ctx);
@@ -720,7 +744,6 @@ static void mixer_win_reset(struct mixer_context *ctx)
 	if (ctx->vp_enabled)
 		mixer_reg_writemask(res, MXR_CFG, 0, MXR_CFG_VP_ENABLE);
 
-	mixer_vsync_set_update(ctx, true);
 	spin_unlock_irqrestore(&res->reg_slock, flags);
 }
 
@@ -951,17 +974,27 @@ static void mixer_disable_vblank(struct exynos_drm_crtc *crtc)
 	mixer_reg_writemask(res, MXR_INT_EN, 0, MXR_INT_EN_VSYNC);
 }
 
+static void mixer_atomic_begin(struct exynos_drm_crtc *crtc)
+{
+	struct mixer_context *mixer_ctx = crtc->ctx;
+
+	if (!test_bit(MXR_BIT_POWERED, &mixer_ctx->flags))
+		return;
+
+	mixer_vsync_set_update(mixer_ctx, false);
+}
+
 static void mixer_update_plane(struct exynos_drm_crtc *crtc,
 			       struct exynos_drm_plane *plane)
 {
 	struct mixer_context *mixer_ctx = crtc->ctx;
 
-	DRM_DEBUG_KMS("win: %d\n", plane->zpos);
+	DRM_DEBUG_KMS("win: %d\n", plane->index);
 
 	if (!test_bit(MXR_BIT_POWERED, &mixer_ctx->flags))
 		return;
 
-	if (plane->zpos > 1 && mixer_ctx->vp_enabled)
+	if (plane->index == VP_DEFAULT_WIN)
 		vp_video_buffer(mixer_ctx, plane);
 	else
 		mixer_graph_buffer(mixer_ctx, plane);
@@ -974,18 +1007,24 @@ static void mixer_disable_plane(struct exynos_drm_crtc *crtc,
 	struct mixer_resources *res = &mixer_ctx->mixer_res;
 	unsigned long flags;
 
-	DRM_DEBUG_KMS("win: %d\n", plane->zpos);
+	DRM_DEBUG_KMS("win: %d\n", plane->index);
 
 	if (!test_bit(MXR_BIT_POWERED, &mixer_ctx->flags))
 		return;
 
 	spin_lock_irqsave(&res->reg_slock, flags);
-	mixer_vsync_set_update(mixer_ctx, false);
+	mixer_cfg_layer(mixer_ctx, plane->index, 0, false);
+	spin_unlock_irqrestore(&res->reg_slock, flags);
+}
 
-	mixer_cfg_layer(mixer_ctx, plane->zpos, false);
+static void mixer_atomic_flush(struct exynos_drm_crtc *crtc)
+{
+	struct mixer_context *mixer_ctx = crtc->ctx;
+
+	if (!test_bit(MXR_BIT_POWERED, &mixer_ctx->flags))
+		return;
 
 	mixer_vsync_set_update(mixer_ctx, true);
-	spin_unlock_irqrestore(&res->reg_slock, flags);
 }
 
 static void mixer_wait_for_vblank(struct exynos_drm_crtc *crtc)
@@ -1020,42 +1059,13 @@ static void mixer_enable(struct exynos_drm_crtc *crtc)
 {
 	struct mixer_context *ctx = crtc->ctx;
 	struct mixer_resources *res = &ctx->mixer_res;
-	int ret;
 
 	if (test_bit(MXR_BIT_POWERED, &ctx->flags))
 		return;
 
 	pm_runtime_get_sync(ctx->dev);
 
-	ret = clk_prepare_enable(res->mixer);
-	if (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the mixer clk [%d]\n", ret);
-		return;
-	}
-	ret = clk_prepare_enable(res->hdmi);
-	if (ret < 0) {
-		DRM_ERROR("Failed to prepare_enable the hdmi clk [%d]\n", ret);
-		return;
-	}
-	if (ctx->vp_enabled) {
-		ret = clk_prepare_enable(res->vp);
-		if (ret < 0) {
-			DRM_ERROR("Failed to prepare_enable the vp clk [%d]\n",
-				  ret);
-			return;
-		}
-		if (ctx->has_sclk) {
-			ret = clk_prepare_enable(res->sclk_mixer);
-			if (ret < 0) {
-				DRM_ERROR("Failed to prepare_enable the " \
-					   "sclk_mixer clk [%d]\n",
-					  ret);
-				return;
-			}
-		}
-	}
-
-	set_bit(MXR_BIT_POWERED, &ctx->flags);
+	mixer_vsync_set_update(ctx, false);
 
 	mixer_reg_writemask(res, MXR_STATUS, ~0, MXR_STATUS_SOFT_RESET);
 
@@ -1064,12 +1074,15 @@ static void mixer_enable(struct exynos_drm_crtc *crtc)
 		mixer_reg_writemask(res, MXR_INT_EN, ~0, MXR_INT_EN_VSYNC);
 	}
 	mixer_win_reset(ctx);
+
+	mixer_vsync_set_update(ctx, true);
+
+	set_bit(MXR_BIT_POWERED, &ctx->flags);
 }
 
 static void mixer_disable(struct exynos_drm_crtc *crtc)
 {
 	struct mixer_context *ctx = crtc->ctx;
-	struct mixer_resources *res = &ctx->mixer_res;
 	int i;
 
 	if (!test_bit(MXR_BIT_POWERED, &ctx->flags))
@@ -1081,17 +1094,9 @@ static void mixer_disable(struct exynos_drm_crtc *crtc)
 	for (i = 0; i < MIXER_WIN_NR; i++)
 		mixer_disable_plane(crtc, &ctx->planes[i]);
 
-	clear_bit(MXR_BIT_POWERED, &ctx->flags);
+	pm_runtime_put(ctx->dev);
 
-	clk_disable_unprepare(res->hdmi);
-	clk_disable_unprepare(res->mixer);
-	if (ctx->vp_enabled) {
-		clk_disable_unprepare(res->vp);
-		if (ctx->has_sclk)
-			clk_disable_unprepare(res->sclk_mixer);
-	}
-
-	pm_runtime_put_sync(ctx->dev);
+	clear_bit(MXR_BIT_POWERED, &ctx->flags);
 }
 
 /* Only valid for Mixer version 16.0.33.0 */
@@ -1122,8 +1127,10 @@ static const struct exynos_drm_crtc_ops mixer_crtc_ops = {
 	.enable_vblank		= mixer_enable_vblank,
 	.disable_vblank		= mixer_disable_vblank,
 	.wait_for_vblank	= mixer_wait_for_vblank,
+	.atomic_begin		= mixer_atomic_begin,
 	.update_plane		= mixer_update_plane,
 	.disable_plane		= mixer_disable_plane,
+	.atomic_flush		= mixer_atomic_flush,
 	.atomic_check		= mixer_atomic_check,
 };
 
@@ -1187,30 +1194,19 @@ static int mixer_bind(struct device *dev, struct device *manager, void *data)
 	struct mixer_context *ctx = dev_get_drvdata(dev);
 	struct drm_device *drm_dev = data;
 	struct exynos_drm_plane *exynos_plane;
-	unsigned int zpos;
+	unsigned int i;
 	int ret;
 
 	ret = mixer_initialize(ctx, drm_dev);
 	if (ret)
 		return ret;
 
-	for (zpos = 0; zpos < MIXER_WIN_NR; zpos++) {
-		enum drm_plane_type type;
-		const uint32_t *formats;
-		unsigned int fcount;
-
-		if (zpos < VP_DEFAULT_WIN) {
-			formats = mixer_formats;
-			fcount = ARRAY_SIZE(mixer_formats);
-		} else {
-			formats = vp_formats;
-			fcount = ARRAY_SIZE(vp_formats);
-		}
+	for (i = 0; i < MIXER_WIN_NR; i++) {
+		if (i == VP_DEFAULT_WIN && !ctx->vp_enabled)
+			continue;
 
-		type = exynos_plane_get_type(zpos, CURSOR_WIN);
-		ret = exynos_plane_init(drm_dev, &ctx->planes[zpos],
-					1 << ctx->pipe, type, formats, fcount,
-					zpos);
+		ret = exynos_plane_init(drm_dev, &ctx->planes[i], i,
+					1 << ctx->pipe, &plane_configs[i]);
 		if (ret)
 			return ret;
 	}
@@ -1293,10 +1289,68 @@ static int mixer_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int __maybe_unused exynos_mixer_suspend(struct device *dev)
+{
+	struct mixer_context *ctx = dev_get_drvdata(dev);
+	struct mixer_resources *res = &ctx->mixer_res;
+
+	clk_disable_unprepare(res->hdmi);
+	clk_disable_unprepare(res->mixer);
+	if (ctx->vp_enabled) {
+		clk_disable_unprepare(res->vp);
+		if (ctx->has_sclk)
+			clk_disable_unprepare(res->sclk_mixer);
+	}
+
+	return 0;
+}
+
+static int __maybe_unused exynos_mixer_resume(struct device *dev)
+{
+	struct mixer_context *ctx = dev_get_drvdata(dev);
+	struct mixer_resources *res = &ctx->mixer_res;
+	int ret;
+
+	ret = clk_prepare_enable(res->mixer);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the mixer clk [%d]\n", ret);
+		return ret;
+	}
+	ret = clk_prepare_enable(res->hdmi);
+	if (ret < 0) {
+		DRM_ERROR("Failed to prepare_enable the hdmi clk [%d]\n", ret);
+		return ret;
+	}
+	if (ctx->vp_enabled) {
+		ret = clk_prepare_enable(res->vp);
+		if (ret < 0) {
+			DRM_ERROR("Failed to prepare_enable the vp clk [%d]\n",
+				  ret);
+			return ret;
+		}
+		if (ctx->has_sclk) {
+			ret = clk_prepare_enable(res->sclk_mixer);
+			if (ret < 0) {
+				DRM_ERROR("Failed to prepare_enable the " \
+					   "sclk_mixer clk [%d]\n",
+					  ret);
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static const struct dev_pm_ops exynos_mixer_pm_ops = {
+	SET_RUNTIME_PM_OPS(exynos_mixer_suspend, exynos_mixer_resume, NULL)
+};
+
 struct platform_driver mixer_driver = {
 	.driver = {
 		.name = "exynos-mixer",
 		.owner = THIS_MODULE,
+		.pm = &exynos_mixer_pm_ops,
 		.of_match_table = mixer_match_types,
 	},
 	.probe = mixer_probe,
diff --git a/drivers/gpu/drm/exynos/regs-gsc.h b/drivers/gpu/drm/exynos/regs-gsc.h
index 9ad592707aaf..4704a993cbb7 100644
--- a/drivers/gpu/drm/exynos/regs-gsc.h
+++ b/drivers/gpu/drm/exynos/regs-gsc.h
@@ -273,12 +273,12 @@
 #define GSC_CLK_GATE_MODE_SNOOP_CNT(x)	((x) << 0)
 
 /* SYSCON. GSCBLK_CFG */
-#define SYSREG_GSCBLK_CFG1		(S3C_VA_SYS + 0x0224)
+#define SYSREG_GSCBLK_CFG1		0x0224
 #define GSC_BLK_DISP1WB_DEST(x)		(x << 10)
 #define GSC_BLK_SW_RESET_WB_DEST(x)	(1 << (18 + x))
 #define GSC_BLK_PXLASYNC_LO_MASK_WB(x)	(0 << (14 + x))
 #define GSC_BLK_GSCL_WB_IN_SRC_SEL(x)	(1 << (2 * x))
-#define SYSREG_GSCBLK_CFG2		(S3C_VA_SYS + 0x2000)
+#define SYSREG_GSCBLK_CFG2		0x2000
 #define PXLASYNC_LO_MASK_CAMIF_GSCL(x)	(1 << (x))
 
 #endif /* EXYNOS_REGS_GSC_H_ */
diff --git a/drivers/gpu/drm/exynos/regs-mixer.h b/drivers/gpu/drm/exynos/regs-mixer.h
index ac60260c2389..7f22df5bf707 100644
--- a/drivers/gpu/drm/exynos/regs-mixer.h
+++ b/drivers/gpu/drm/exynos/regs-mixer.h
@@ -113,6 +113,7 @@
 #define MXR_GRP_CFG_BLEND_PRE_MUL	(1 << 20)
 #define MXR_GRP_CFG_WIN_BLEND_EN	(1 << 17)
 #define MXR_GRP_CFG_PIXEL_BLEND_EN	(1 << 16)
+#define MXR_GRP_CFG_MISC_MASK		((3 << 16) | (3 << 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)
@@ -145,8 +146,11 @@
 
 /* bit for MXR_LAYER_CFG */
 #define MXR_LAYER_CFG_GRP1_VAL(x)	MXR_MASK_VAL(x, 11, 8)
+#define MXR_LAYER_CFG_GRP1_MASK		MXR_LAYER_CFG_GRP1_VAL(~0)
 #define MXR_LAYER_CFG_GRP0_VAL(x)	MXR_MASK_VAL(x, 7, 4)
+#define MXR_LAYER_CFG_GRP0_MASK		MXR_LAYER_CFG_GRP0_VAL(~0)
 #define MXR_LAYER_CFG_VP_VAL(x)		MXR_MASK_VAL(x, 3, 0)
+#define MXR_LAYER_CFG_VP_MASK		MXR_LAYER_CFG_VP_VAL(~0)
 
 #endif /* SAMSUNG_REGS_MIXER_H */
 
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 82a3d311e164..d8ab8f0af10c 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_crtc.c
@@ -175,7 +175,7 @@ int fsl_dcu_drm_crtc_create(struct fsl_dcu_drm_device *fsl_dev)
 
 	primary = fsl_dcu_drm_primary_create_plane(fsl_dev->drm);
 	ret = drm_crtc_init_with_planes(fsl_dev->drm, crtc, primary, NULL,
-					&fsl_dcu_drm_crtc_funcs);
+					&fsl_dcu_drm_crtc_funcs, NULL);
 	if (ret < 0)
 		return ret;
 
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 1930234ba5f1..fca97d3fc846 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_drv.c
@@ -363,7 +363,6 @@ static int fsl_dcu_drm_probe(struct platform_device *pdev)
 	fsl_dev->np = dev->of_node;
 	drm->dev_private = fsl_dev;
 	dev_set_drvdata(dev, fsl_dev);
-	drm_dev_set_unique(drm, dev_name(dev));
 
 	ret = drm_dev_register(drm, 0);
 	if (ret < 0)
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 51daaea40b4d..4b13cf919575 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_plane.c
@@ -249,7 +249,7 @@ struct drm_plane *fsl_dcu_drm_primary_create_plane(struct drm_device *dev)
 				       &fsl_dcu_drm_plane_funcs,
 				       fsl_dcu_drm_plane_formats,
 				       ARRAY_SIZE(fsl_dcu_drm_plane_formats),
-				       DRM_PLANE_TYPE_PRIMARY);
+				       DRM_PLANE_TYPE_PRIMARY, NULL);
 	if (ret) {
 		kfree(primary);
 		primary = NULL;
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 fe8ab5da04fb..8780deba5e8a 100644
--- a/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c
+++ b/drivers/gpu/drm/fsl-dcu/fsl_dcu_drm_rgb.c
@@ -57,7 +57,7 @@ int fsl_dcu_drm_encoder_create(struct fsl_dcu_drm_device *fsl_dev,
 
 	encoder->possible_crtcs = 1;
 	ret = drm_encoder_init(fsl_dev->drm, encoder, &encoder_funcs,
-			       DRM_MODE_ENCODER_LVDS);
+			       DRM_MODE_ENCODER_LVDS, NULL);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/gpu/drm/gma500/cdv_device.c b/drivers/gpu/drm/gma500/cdv_device.c
index 3531f90e53d0..8745971a7680 100644
--- a/drivers/gpu/drm/gma500/cdv_device.c
+++ b/drivers/gpu/drm/gma500/cdv_device.c
@@ -619,6 +619,8 @@ const struct psb_ops cdv_chip_ops = {
 	.init_pm = cdv_init_pm,
 	.save_regs = cdv_save_display_registers,
 	.restore_regs = cdv_restore_display_registers,
+	.save_crtc = gma_crtc_save,
+	.restore_crtc = gma_crtc_restore,
 	.power_down = cdv_power_down,
 	.power_up = cdv_power_up,
 	.update_wm = cdv_update_wm,
diff --git a/drivers/gpu/drm/gma500/cdv_intel_crt.c b/drivers/gpu/drm/gma500/cdv_intel_crt.c
index 248c33a35ebf..d0717a85c7ec 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_crt.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_crt.c
@@ -273,7 +273,7 @@ void cdv_intel_crt_init(struct drm_device *dev,
 
 	encoder = &gma_encoder->base;
 	drm_encoder_init(dev, encoder,
-		&cdv_intel_crt_enc_funcs, DRM_MODE_ENCODER_DAC);
+		&cdv_intel_crt_enc_funcs, DRM_MODE_ENCODER_DAC, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_display.c b/drivers/gpu/drm/gma500/cdv_intel_display.c
index 7d47b3d5cc0d..6126546295e9 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_display.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_display.c
@@ -983,8 +983,6 @@ const struct drm_crtc_helper_funcs cdv_intel_helper_funcs = {
 };
 
 const struct drm_crtc_funcs cdv_intel_crtc_funcs = {
-	.save = gma_crtc_save,
-	.restore = gma_crtc_restore,
 	.cursor_set = gma_crtc_cursor_set,
 	.cursor_move = gma_crtc_cursor_move,
 	.gamma_set = gma_crtc_gamma_set,
diff --git a/drivers/gpu/drm/gma500/cdv_intel_dp.c b/drivers/gpu/drm/gma500/cdv_intel_dp.c
index 17cea400ae32..7bb1f1aff932 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_dp.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_dp.c
@@ -2020,7 +2020,8 @@ cdv_intel_dp_init(struct drm_device *dev, struct psb_intel_mode_device *mode_dev
 	encoder = &gma_encoder->base;
 
 	drm_connector_init(dev, connector, &cdv_intel_dp_connector_funcs, type);
-	drm_encoder_init(dev, encoder, &cdv_intel_dp_enc_funcs, DRM_MODE_ENCODER_TMDS);
+	drm_encoder_init(dev, encoder, &cdv_intel_dp_enc_funcs,
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 
diff --git a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
index 6b1d3340ba14..ddf2d7700759 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_hdmi.c
@@ -270,8 +270,6 @@ static const struct drm_connector_helper_funcs
 
 static const struct drm_connector_funcs cdv_hdmi_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = cdv_hdmi_save,
-	.restore = cdv_hdmi_restore,
 	.detect = cdv_hdmi_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = cdv_hdmi_set_property,
@@ -306,13 +304,16 @@ void cdv_hdmi_init(struct drm_device *dev,
 
 	connector = &gma_connector->base;
 	connector->polled = DRM_CONNECTOR_POLL_HPD;
+	gma_connector->save = cdv_hdmi_save;
+	gma_connector->restore = cdv_hdmi_restore;
+
 	encoder = &gma_encoder->base;
 	drm_connector_init(dev, connector,
 			   &cdv_hdmi_connector_funcs,
 			   DRM_MODE_CONNECTOR_DVID);
 
 	drm_encoder_init(dev, encoder, &psb_intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 	gma_encoder->type = INTEL_OUTPUT_HDMI;
diff --git a/drivers/gpu/drm/gma500/cdv_intel_lvds.c b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
index 211069b2b951..813ef23a8054 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_lvds.c
@@ -530,8 +530,6 @@ static const struct drm_connector_helper_funcs
 
 static const struct drm_connector_funcs cdv_intel_lvds_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = cdv_intel_lvds_save,
-	.restore = cdv_intel_lvds_restore,
 	.detect = cdv_intel_lvds_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = cdv_intel_lvds_set_property,
@@ -643,6 +641,8 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 	gma_encoder->dev_priv = lvds_priv;
 
 	connector = &gma_connector->base;
+	gma_connector->save = cdv_intel_lvds_save;
+	gma_connector->restore = cdv_intel_lvds_restore;
 	encoder = &gma_encoder->base;
 
 
@@ -652,7 +652,7 @@ void cdv_intel_lvds_init(struct drm_device *dev,
 
 	drm_encoder_init(dev, encoder,
 			 &cdv_intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index 2eaf1b31c7bd..cb95765050cc 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -21,6 +21,7 @@
 #include <linux/kernel.h>
 #include <linux/errno.h>
 #include <linux/string.h>
+#include <linux/pfn_t.h>
 #include <linux/mm.h>
 #include <linux/tty.h>
 #include <linux/slab.h>
@@ -132,7 +133,8 @@ static int psbfb_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	for (i = 0; i < page_num; i++) {
 		pfn = (phys_addr >> PAGE_SHIFT);
 
-		ret = vm_insert_mixed(vma, address, pfn);
+		ret = vm_insert_mixed(vma, address,
+				__pfn_to_pfn_t(pfn, PFN_DEV));
 		if (unlikely((ret == -EBUSY) || (ret != 0 && i > 0)))
 			break;
 		else if (unlikely(ret != 0)) {
@@ -241,7 +243,7 @@ static struct fb_ops psbfb_unaccel_ops = {
  */
 static int psb_framebuffer_init(struct drm_device *dev,
 					struct psb_framebuffer *fb,
-					struct drm_mode_fb_cmd2 *mode_cmd,
+					const struct drm_mode_fb_cmd2 *mode_cmd,
 					struct gtt_range *gt)
 {
 	u32 bpp, depth;
@@ -284,7 +286,7 @@ static int psb_framebuffer_init(struct drm_device *dev,
 
 static struct drm_framebuffer *psb_framebuffer_create
 			(struct drm_device *dev,
-			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 const struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct gtt_range *gt)
 {
 	struct psb_framebuffer *fb;
@@ -406,8 +408,6 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 
 	memset(dev_priv->vram_addr + backing->offset, 0, size);
 
-	mutex_lock(&dev->struct_mutex);
-
 	info = drm_fb_helper_alloc_fbi(&fbdev->psb_fb_helper);
 	if (IS_ERR(info)) {
 		ret = PTR_ERR(info);
@@ -463,17 +463,15 @@ static int psbfb_create(struct psb_fbdev *fbdev,
 	dev_dbg(dev->dev, "allocated %dx%d fb\n",
 					psbfb->base.width, psbfb->base.height);
 
-	mutex_unlock(&dev->struct_mutex);
 	return 0;
 out_unref:
 	if (backing->stolen)
 		psb_gtt_free_range(dev, backing);
 	else
-		drm_gem_object_unreference(&backing->gem);
+		drm_gem_object_unreference_unlocked(&backing->gem);
 
 	drm_fb_helper_release_fbi(&fbdev->psb_fb_helper);
 out_err1:
-	mutex_unlock(&dev->struct_mutex);
 	psb_gtt_free_range(dev, backing);
 	return ret;
 }
@@ -488,7 +486,7 @@ out_err1:
  */
 static struct drm_framebuffer *psb_user_framebuffer_create
 			(struct drm_device *dev, struct drm_file *filp,
-			 struct drm_mode_fb_cmd2 *cmd)
+			 const struct drm_mode_fb_cmd2 *cmd)
 {
 	struct gtt_range *r;
 	struct drm_gem_object *obj;
@@ -569,7 +567,7 @@ static int psb_fbdev_destroy(struct drm_device *dev, struct psb_fbdev *fbdev)
 	drm_framebuffer_cleanup(&psbfb->base);
 
 	if (psbfb->gtt)
-		drm_gem_object_unreference(&psbfb->gtt->gem);
+		drm_gem_object_unreference_unlocked(&psbfb->gtt->gem);
 	return 0;
 }
 
@@ -784,12 +782,8 @@ void psb_modeset_cleanup(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	if (dev_priv->modeset) {
-		mutex_lock(&dev->struct_mutex);
-
 		drm_kms_helper_poll_fini(dev);
 		psb_fbdev_fini(dev);
 		drm_mode_config_cleanup(dev);
-
-		mutex_unlock(&dev->struct_mutex);
 	}
 }
diff --git a/drivers/gpu/drm/gma500/gem.c b/drivers/gpu/drm/gma500/gem.c
index c707fa6fca85..506224b3a0ad 100644
--- a/drivers/gpu/drm/gma500/gem.c
+++ b/drivers/gpu/drm/gma500/gem.c
@@ -62,15 +62,10 @@ int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
 	int ret = 0;
 	struct drm_gem_object *obj;
 
-	mutex_lock(&dev->struct_mutex);
-
 	/* GEM does all our handle to object mapping */
 	obj = drm_gem_object_lookup(dev, file, handle);
-	if (obj == NULL) {
-		ret = -ENOENT;
-		goto unlock;
-	}
-	/* What validation is needed here ? */
+	if (obj == NULL)
+		return -ENOENT;
 
 	/* Make it mmapable */
 	ret = drm_gem_create_mmap_offset(obj);
@@ -78,9 +73,7 @@ int psb_gem_dumb_map_gtt(struct drm_file *file, struct drm_device *dev,
 		goto out;
 	*offset = drm_vma_node_offset_addr(&obj->vma_node);
 out:
-	drm_gem_object_unreference(obj);
-unlock:
-	mutex_unlock(&dev->struct_mutex);
+	drm_gem_object_unreference_unlocked(obj);
 	return ret;
 }
 
@@ -130,7 +123,7 @@ int psb_gem_create(struct drm_file *file, struct drm_device *dev, u64 size,
 		return ret;
 	}
 	/* We have the initial and handle reference but need only one now */
-	drm_gem_object_unreference(&r->gem);
+	drm_gem_object_unreference_unlocked(&r->gem);
 	*handlep = handle;
 	return 0;
 }
@@ -189,7 +182,7 @@ int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 
 	/* Make sure we don't parallel update on a fault, nor move or remove
 	   something from beneath our feet */
-	mutex_lock(&dev->struct_mutex);
+	mutex_lock(&dev_priv->mmap_mutex);
 
 	/* For now the mmap pins the object and it stays pinned. As things
 	   stand that will do us no harm */
@@ -215,7 +208,7 @@ int psb_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	ret = vm_insert_pfn(vma, (unsigned long)vmf->virtual_address, pfn);
 
 fail:
-	mutex_unlock(&dev->struct_mutex);
+	mutex_unlock(&dev_priv->mmap_mutex);
 	switch (ret) {
 	case 0:
 	case -ERESTARTSYS:
diff --git a/drivers/gpu/drm/gma500/gma_display.c b/drivers/gpu/drm/gma500/gma_display.c
index 001b450b27b3..ff17af4cfc64 100644
--- a/drivers/gpu/drm/gma500/gma_display.c
+++ b/drivers/gpu/drm/gma500/gma_display.c
@@ -349,8 +349,6 @@ int gma_crtc_cursor_set(struct drm_crtc *crtc,
 	/* If we didn't get a handle then turn the cursor off */
 	if (!handle) {
 		temp = CURSOR_MODE_DISABLE;
-		mutex_lock(&dev->struct_mutex);
-
 		if (gma_power_begin(dev, false)) {
 			REG_WRITE(control, temp);
 			REG_WRITE(base, 0);
@@ -362,11 +360,9 @@ int gma_crtc_cursor_set(struct drm_crtc *crtc,
 			gt = container_of(gma_crtc->cursor_obj,
 					  struct gtt_range, gem);
 			psb_gtt_unpin(gt);
-			drm_gem_object_unreference(gma_crtc->cursor_obj);
+			drm_gem_object_unreference_unlocked(gma_crtc->cursor_obj);
 			gma_crtc->cursor_obj = NULL;
 		}
-
-		mutex_unlock(&dev->struct_mutex);
 		return 0;
 	}
 
@@ -376,7 +372,6 @@ int gma_crtc_cursor_set(struct drm_crtc *crtc,
 		return -EINVAL;
 	}
 
-	mutex_lock(&dev->struct_mutex);
 	obj = drm_gem_object_lookup(dev, file_priv, handle);
 	if (!obj) {
 		ret = -ENOENT;
@@ -441,17 +436,15 @@ int gma_crtc_cursor_set(struct drm_crtc *crtc,
 	if (gma_crtc->cursor_obj) {
 		gt = container_of(gma_crtc->cursor_obj, struct gtt_range, gem);
 		psb_gtt_unpin(gt);
-		drm_gem_object_unreference(gma_crtc->cursor_obj);
+		drm_gem_object_unreference_unlocked(gma_crtc->cursor_obj);
 	}
 
 	gma_crtc->cursor_obj = obj;
 unlock:
-	mutex_unlock(&dev->struct_mutex);
 	return ret;
 
 unref_cursor:
-	drm_gem_object_unreference(obj);
-	mutex_unlock(&dev->struct_mutex);
+	drm_gem_object_unreference_unlocked(obj);
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/gma500/gtt.c b/drivers/gpu/drm/gma500/gtt.c
index ce015db59dc6..8f69225ce2b4 100644
--- a/drivers/gpu/drm/gma500/gtt.c
+++ b/drivers/gpu/drm/gma500/gtt.c
@@ -425,6 +425,7 @@ int psb_gtt_init(struct drm_device *dev, int resume)
 
 	if (!resume) {
 		mutex_init(&dev_priv->gtt_mutex);
+		mutex_init(&dev_priv->mmap_mutex);
 		psb_gtt_alloc(dev);
 	}
 
diff --git a/drivers/gpu/drm/gma500/mdfld_device.c b/drivers/gpu/drm/gma500/mdfld_device.c
index 265ad0de44a6..e2ab858122f9 100644
--- a/drivers/gpu/drm/gma500/mdfld_device.c
+++ b/drivers/gpu/drm/gma500/mdfld_device.c
@@ -546,6 +546,8 @@ const struct psb_ops mdfld_chip_ops = {
 
 	.save_regs = mdfld_save_registers,
 	.restore_regs = mdfld_restore_registers,
+	.save_crtc = gma_crtc_save,
+	.restore_crtc = gma_crtc_restore,
 	.power_down = mdfld_power_down,
 	.power_up = mdfld_power_up,
 };
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
index d4813e03f5ee..7cd87a0c2385 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_dpi.c
@@ -821,14 +821,18 @@ void mdfld_dsi_dpi_mode_set(struct drm_encoder *encoder,
 	struct drm_device *dev = dsi_config->dev;
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int pipe = mdfld_dsi_encoder_get_pipe(dsi_encoder);
-
 	u32 pipeconf_reg = PIPEACONF;
 	u32 dspcntr_reg = DSPACNTR;
+	u32 pipeconf, dspcntr;
 
-	u32 pipeconf = dev_priv->pipeconf[pipe];
-	u32 dspcntr = dev_priv->dspcntr[pipe];
 	u32 mipi = MIPI_PORT_EN | PASS_FROM_SPHY_TO_AFE | SEL_FLOPPED_HSTX;
 
+	if (WARN_ON(pipe < 0))
+		return;
+
+	pipeconf = dev_priv->pipeconf[pipe];
+	dspcntr = dev_priv->dspcntr[pipe];
+
 	if (pipe) {
 		pipeconf_reg = PIPECCONF;
 		dspcntr_reg = DSPCCNTR;
@@ -994,7 +998,7 @@ struct mdfld_dsi_encoder *mdfld_dsi_dpi_init(struct drm_device *dev,
 	drm_encoder_init(dev,
 			encoder,
 			p_funcs->encoder_funcs,
-			DRM_MODE_ENCODER_LVDS);
+			DRM_MODE_ENCODER_LVDS, NULL);
 	drm_encoder_helper_add(encoder,
 				p_funcs->encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/gma500/mdfld_dsi_output.c b/drivers/gpu/drm/gma500/mdfld_dsi_output.c
index 89f705c3a5eb..d758f4cc6805 100644
--- a/drivers/gpu/drm/gma500/mdfld_dsi_output.c
+++ b/drivers/gpu/drm/gma500/mdfld_dsi_output.c
@@ -405,8 +405,6 @@ static struct drm_encoder *mdfld_dsi_connector_best_encoder(
 /*DSI connector funcs*/
 static const struct drm_connector_funcs mdfld_dsi_connector_funcs = {
 	.dpms = /*drm_helper_connector_dpms*/mdfld_dsi_connector_dpms,
-	.save = mdfld_dsi_connector_save,
-	.restore = mdfld_dsi_connector_restore,
 	.detect = mdfld_dsi_connector_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = mdfld_dsi_connector_set_property,
@@ -563,6 +561,9 @@ void mdfld_dsi_output_init(struct drm_device *dev,
 
 
 	connector = &dsi_connector->base.base;
+	dsi_connector->base.save = mdfld_dsi_connector_save;
+	dsi_connector->base.restore = mdfld_dsi_connector_restore;
+
 	drm_connector_init(dev, connector, &mdfld_dsi_connector_funcs,
 						DRM_MODE_CONNECTOR_LVDS);
 	drm_connector_helper_add(connector, &mdfld_dsi_connector_helper_funcs);
diff --git a/drivers/gpu/drm/gma500/oaktrail_device.c b/drivers/gpu/drm/gma500/oaktrail_device.c
index 368a03ae3010..ba30b43a3412 100644
--- a/drivers/gpu/drm/gma500/oaktrail_device.c
+++ b/drivers/gpu/drm/gma500/oaktrail_device.c
@@ -568,6 +568,8 @@ const struct psb_ops oaktrail_chip_ops = {
 
 	.save_regs = oaktrail_save_display_registers,
 	.restore_regs = oaktrail_restore_display_registers,
+	.save_crtc = gma_crtc_save,
+	.restore_crtc = gma_crtc_restore,
 	.power_down = oaktrail_power_down,
 	.power_up = oaktrail_power_up,
 
diff --git a/drivers/gpu/drm/gma500/oaktrail_hdmi.c b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
index 2310d879cdc2..2d18499d6060 100644
--- a/drivers/gpu/drm/gma500/oaktrail_hdmi.c
+++ b/drivers/gpu/drm/gma500/oaktrail_hdmi.c
@@ -654,7 +654,7 @@ void oaktrail_hdmi_init(struct drm_device *dev,
 
 	drm_encoder_init(dev, encoder,
 			 &oaktrail_hdmi_enc_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 
diff --git a/drivers/gpu/drm/gma500/oaktrail_lvds.c b/drivers/gpu/drm/gma500/oaktrail_lvds.c
index 83bbc271bcfb..f7038f12ac76 100644
--- a/drivers/gpu/drm/gma500/oaktrail_lvds.c
+++ b/drivers/gpu/drm/gma500/oaktrail_lvds.c
@@ -323,7 +323,7 @@ void oaktrail_lvds_init(struct drm_device *dev,
 			   DRM_MODE_CONNECTOR_LVDS);
 
 	drm_encoder_init(dev, encoder, &psb_intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 	gma_encoder->type = INTEL_OUTPUT_LVDS;
diff --git a/drivers/gpu/drm/gma500/power.c b/drivers/gpu/drm/gma500/power.c
index b6b135fcd59c..bea8578846d1 100644
--- a/drivers/gpu/drm/gma500/power.c
+++ b/drivers/gpu/drm/gma500/power.c
@@ -187,7 +187,7 @@ static bool gma_resume_pci(struct pci_dev *pdev)
  */
 int gma_power_suspend(struct device *_dev)
 {
-	struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(_dev);
 	struct drm_device *dev = pci_get_drvdata(pdev);
 	struct drm_psb_private *dev_priv = dev->dev_private;
 
@@ -214,7 +214,7 @@ int gma_power_suspend(struct device *_dev)
  */
 int gma_power_resume(struct device *_dev)
 {
-	struct pci_dev *pdev = container_of(_dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(_dev);
 	struct drm_device *dev = pci_get_drvdata(pdev);
 
 	mutex_lock(&power_mutex);
diff --git a/drivers/gpu/drm/gma500/psb_device.c b/drivers/gpu/drm/gma500/psb_device.c
index 07df7d4eea72..dc0f8527570c 100644
--- a/drivers/gpu/drm/gma500/psb_device.c
+++ b/drivers/gpu/drm/gma500/psb_device.c
@@ -181,7 +181,7 @@ static int psb_save_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
-	struct drm_connector *connector;
+	struct gma_connector *connector;
 	struct psb_state *regs = &dev_priv->regs.psb;
 
 	/* Display arbitration control + watermarks */
@@ -198,12 +198,12 @@ static int psb_save_display_registers(struct drm_device *dev)
 	drm_modeset_lock_all(dev);
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		if (drm_helper_crtc_in_use(crtc))
-			crtc->funcs->save(crtc);
+			dev_priv->ops->save_crtc(crtc);
 	}
 
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		if (connector->funcs->save)
-			connector->funcs->save(connector);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+		if (connector->save)
+			connector->save(&connector->base);
 
 	drm_modeset_unlock_all(dev);
 	return 0;
@@ -219,7 +219,7 @@ static int psb_restore_display_registers(struct drm_device *dev)
 {
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
-	struct drm_connector *connector;
+	struct gma_connector *connector;
 	struct psb_state *regs = &dev_priv->regs.psb;
 
 	/* Display arbitration + watermarks */
@@ -238,11 +238,11 @@ static int psb_restore_display_registers(struct drm_device *dev)
 	drm_modeset_lock_all(dev);
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
 		if (drm_helper_crtc_in_use(crtc))
-			crtc->funcs->restore(crtc);
+			dev_priv->ops->restore_crtc(crtc);
 
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
-		if (connector->funcs->restore)
-			connector->funcs->restore(connector);
+	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+		if (connector->restore)
+			connector->restore(&connector->base);
 
 	drm_modeset_unlock_all(dev);
 	return 0;
@@ -354,6 +354,8 @@ const struct psb_ops psb_chip_ops = {
 	.init_pm = psb_init_pm,
 	.save_regs = psb_save_display_registers,
 	.restore_regs = psb_restore_display_registers,
+	.save_crtc = gma_crtc_save,
+	.restore_crtc = gma_crtc_restore,
 	.power_down = psb_power_down,
 	.power_up = psb_power_up,
 };
diff --git a/drivers/gpu/drm/gma500/psb_drv.h b/drivers/gpu/drm/gma500/psb_drv.h
index e21726ecac32..b74372760d7f 100644
--- a/drivers/gpu/drm/gma500/psb_drv.h
+++ b/drivers/gpu/drm/gma500/psb_drv.h
@@ -465,6 +465,8 @@ struct drm_psb_private {
 	struct mutex gtt_mutex;
 	struct resource *gtt_mem;	/* Our PCI resource */
 
+	struct mutex mmap_mutex;
+
 	struct psb_mmu_driver *mmu;
 	struct psb_mmu_pd *pf_pd;
 
@@ -651,6 +653,8 @@ struct psb_ops {
 	void (*init_pm)(struct drm_device *dev);
 	int (*save_regs)(struct drm_device *dev);
 	int (*restore_regs)(struct drm_device *dev);
+	void (*save_crtc)(struct drm_crtc *crtc);
+	void (*restore_crtc)(struct drm_crtc *crtc);
 	int (*power_up)(struct drm_device *dev);
 	int (*power_down)(struct drm_device *dev);
 	void (*update_wm)(struct drm_device *dev, struct drm_crtc *crtc);
diff --git a/drivers/gpu/drm/gma500/psb_intel_display.c b/drivers/gpu/drm/gma500/psb_intel_display.c
index 6659da88fe5b..dcdbc37e55e1 100644
--- a/drivers/gpu/drm/gma500/psb_intel_display.c
+++ b/drivers/gpu/drm/gma500/psb_intel_display.c
@@ -439,8 +439,6 @@ const struct drm_crtc_helper_funcs psb_intel_helper_funcs = {
 };
 
 const struct drm_crtc_funcs psb_intel_crtc_funcs = {
-	.save = gma_crtc_save,
-	.restore = gma_crtc_restore,
 	.cursor_set = gma_crtc_cursor_set,
 	.cursor_move = gma_crtc_cursor_move,
 	.gamma_set = gma_crtc_gamma_set,
diff --git a/drivers/gpu/drm/gma500/psb_intel_drv.h b/drivers/gpu/drm/gma500/psb_intel_drv.h
index 860dd2177ca1..2a3b7c684db2 100644
--- a/drivers/gpu/drm/gma500/psb_intel_drv.h
+++ b/drivers/gpu/drm/gma500/psb_intel_drv.h
@@ -140,6 +140,9 @@ struct gma_encoder {
 struct gma_connector {
 	struct drm_connector base;
 	struct gma_encoder *encoder;
+
+	void (*save)(struct drm_connector *connector);
+	void (*restore)(struct drm_connector *connector);
 };
 
 struct psb_intel_crtc_state {
diff --git a/drivers/gpu/drm/gma500/psb_intel_lvds.c b/drivers/gpu/drm/gma500/psb_intel_lvds.c
index ce0645d0c1e5..b1b93317d054 100644
--- a/drivers/gpu/drm/gma500/psb_intel_lvds.c
+++ b/drivers/gpu/drm/gma500/psb_intel_lvds.c
@@ -653,8 +653,6 @@ const struct drm_connector_helper_funcs
 
 const struct drm_connector_funcs psb_intel_lvds_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = psb_intel_lvds_save,
-	.restore = psb_intel_lvds_restore,
 	.detect = psb_intel_lvds_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = psb_intel_lvds_set_property,
@@ -715,6 +713,9 @@ void psb_intel_lvds_init(struct drm_device *dev,
 	gma_encoder->dev_priv = lvds_priv;
 
 	connector = &gma_connector->base;
+	gma_connector->save = psb_intel_lvds_save;
+	gma_connector->restore = psb_intel_lvds_restore;
+
 	encoder = &gma_encoder->base;
 	drm_connector_init(dev, connector,
 			   &psb_intel_lvds_connector_funcs,
@@ -722,7 +723,7 @@ void psb_intel_lvds_init(struct drm_device *dev,
 
 	drm_encoder_init(dev, encoder,
 			 &psb_intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 
 	gma_connector_attach_encoder(gma_connector, gma_encoder);
 	gma_encoder->type = INTEL_OUTPUT_LVDS;
diff --git a/drivers/gpu/drm/gma500/psb_intel_sdvo.c b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
index 58529cea575d..e787d376ba67 100644
--- a/drivers/gpu/drm/gma500/psb_intel_sdvo.c
+++ b/drivers/gpu/drm/gma500/psb_intel_sdvo.c
@@ -1837,8 +1837,6 @@ static const struct drm_encoder_helper_funcs psb_intel_sdvo_helper_funcs = {
 
 static const struct drm_connector_funcs psb_intel_sdvo_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = psb_intel_sdvo_save,
-	.restore = psb_intel_sdvo_restore,
 	.detect = psb_intel_sdvo_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = psb_intel_sdvo_set_property,
@@ -2021,6 +2019,9 @@ psb_intel_sdvo_connector_init(struct psb_intel_sdvo_connector *connector,
 	connector->base.base.doublescan_allowed = 0;
 	connector->base.base.display_info.subpixel_order = SubPixelHorizontalRGB;
 
+	connector->base.save = psb_intel_sdvo_save;
+	connector->base.restore = psb_intel_sdvo_restore;
+
 	gma_connector_attach_encoder(&connector->base, &encoder->base);
 	drm_connector_register(&connector->base.base);
 }
@@ -2525,7 +2526,8 @@ bool psb_intel_sdvo_init(struct drm_device *dev, int sdvo_reg)
 	/* encoder type will be decided later */
 	gma_encoder = &psb_intel_sdvo->base;
 	gma_encoder->type = INTEL_OUTPUT_SDVO;
-	drm_encoder_init(dev, &gma_encoder->base, &psb_intel_sdvo_enc_funcs, 0);
+	drm_encoder_init(dev, &gma_encoder->base, &psb_intel_sdvo_enc_funcs,
+			 0, NULL);
 
 	/* Read the regs to test if we can talk to the device */
 	for (i = 0; i < 0x40; i++) {
diff --git a/drivers/gpu/drm/i2c/adv7511.c b/drivers/gpu/drm/i2c/adv7511.c
index 00416f23b5cb..a02112ba1c3d 100644
--- a/drivers/gpu/drm/i2c/adv7511.c
+++ b/drivers/gpu/drm/i2c/adv7511.c
@@ -136,6 +136,7 @@ static bool adv7511_register_volatile(struct device *dev, unsigned int reg)
 	case ADV7511_REG_BKSV(3):
 	case ADV7511_REG_BKSV(4):
 	case ADV7511_REG_DDC_STATUS:
+	case ADV7511_REG_EDID_READ_CTRL:
 	case ADV7511_REG_BSTATUS(0):
 	case ADV7511_REG_BSTATUS(1):
 	case ADV7511_REG_CHIP_ID_HIGH:
@@ -362,24 +363,31 @@ static void adv7511_power_on(struct adv7511 *adv7511)
 {
 	adv7511->current_edid_segment = -1;
 
-	regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
-		     ADV7511_INT0_EDID_READY);
-	regmap_write(adv7511->regmap, ADV7511_REG_INT(1),
-		     ADV7511_INT1_DDC_ERROR);
 	regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
 			   ADV7511_POWER_POWER_DOWN, 0);
+	if (adv7511->i2c_main->irq) {
+		/*
+		 * Documentation says the INT_ENABLE registers are reset in
+		 * POWER_DOWN mode. My 7511w preserved the bits, however.
+		 * Still, let's be safe and stick to the documentation.
+		 */
+		regmap_write(adv7511->regmap, ADV7511_REG_INT_ENABLE(0),
+			     ADV7511_INT0_EDID_READY);
+		regmap_write(adv7511->regmap, ADV7511_REG_INT_ENABLE(1),
+			     ADV7511_INT1_DDC_ERROR);
+	}
 
 	/*
-	 * Per spec it is allowed to pulse the HDP signal to indicate that the
+	 * Per spec it is allowed to pulse the HPD signal to indicate that the
 	 * EDID information has changed. Some monitors do this when they wakeup
-	 * from standby or are enabled. When the HDP goes low the adv7511 is
+	 * from standby or are enabled. When the HPD goes low the adv7511 is
 	 * reset and the outputs are disabled which might cause the monitor to
-	 * go to standby again. To avoid this we ignore the HDP pin for the
+	 * go to standby again. To avoid this we ignore the HPD pin for the
 	 * first few seconds after enabling the output.
 	 */
 	regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER2,
-			   ADV7511_REG_POWER2_HDP_SRC_MASK,
-			   ADV7511_REG_POWER2_HDP_SRC_NONE);
+			   ADV7511_REG_POWER2_HPD_SRC_MASK,
+			   ADV7511_REG_POWER2_HPD_SRC_NONE);
 
 	/*
 	 * Most of the registers are reset during power down or when HPD is low.
@@ -413,9 +421,9 @@ static bool adv7511_hpd(struct adv7511 *adv7511)
 	if (ret < 0)
 		return false;
 
-	if (irq0 & ADV7511_INT0_HDP) {
+	if (irq0 & ADV7511_INT0_HPD) {
 		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
-			     ADV7511_INT0_HDP);
+			     ADV7511_INT0_HPD);
 		return true;
 	}
 
@@ -438,7 +446,7 @@ static int adv7511_irq_process(struct adv7511 *adv7511)
 	regmap_write(adv7511->regmap, ADV7511_REG_INT(0), irq0);
 	regmap_write(adv7511->regmap, ADV7511_REG_INT(1), irq1);
 
-	if (irq0 & ADV7511_INT0_HDP && adv7511->encoder)
+	if (irq0 & ADV7511_INT0_HPD && adv7511->encoder)
 		drm_helper_hpd_irq_event(adv7511->encoder->dev);
 
 	if (irq0 & ADV7511_INT0_EDID_READY || irq1 & ADV7511_INT1_DDC_ERROR) {
@@ -567,12 +575,14 @@ static int adv7511_get_modes(struct drm_encoder *encoder,
 
 	/* Reading the EDID only works if the device is powered */
 	if (!adv7511->powered) {
-		regmap_write(adv7511->regmap, ADV7511_REG_INT(0),
-			     ADV7511_INT0_EDID_READY);
-		regmap_write(adv7511->regmap, ADV7511_REG_INT(1),
-			     ADV7511_INT1_DDC_ERROR);
 		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER,
 				   ADV7511_POWER_POWER_DOWN, 0);
+		if (adv7511->i2c_main->irq) {
+			regmap_write(adv7511->regmap, ADV7511_REG_INT_ENABLE(0),
+				     ADV7511_INT0_EDID_READY);
+			regmap_write(adv7511->regmap, ADV7511_REG_INT_ENABLE(1),
+				     ADV7511_INT1_DDC_ERROR);
+		}
 		adv7511->current_edid_segment = -1;
 	}
 
@@ -638,10 +648,10 @@ adv7511_encoder_detect(struct drm_encoder *encoder,
 		if (adv7511->status == connector_status_connected)
 			status = connector_status_disconnected;
 	} else {
-		/* Renable HDP sensing */
+		/* Renable HPD sensing */
 		regmap_update_bits(adv7511->regmap, ADV7511_REG_POWER2,
-				   ADV7511_REG_POWER2_HDP_SRC_MASK,
-				   ADV7511_REG_POWER2_HDP_SRC_BOTH);
+				   ADV7511_REG_POWER2_HPD_SRC_MASK,
+				   ADV7511_REG_POWER2_HPD_SRC_BOTH);
 	}
 
 	adv7511->status = status;
@@ -752,7 +762,7 @@ static void adv7511_encoder_mode_set(struct drm_encoder *encoder,
 	adv7511->f_tmds = mode->clock;
 }
 
-static struct drm_encoder_slave_funcs adv7511_encoder_funcs = {
+static const struct drm_encoder_slave_funcs adv7511_encoder_funcs = {
 	.dpms = adv7511_encoder_dpms,
 	.mode_valid = adv7511_encoder_mode_valid,
 	.mode_set = adv7511_encoder_mode_set,
diff --git a/drivers/gpu/drm/i2c/adv7511.h b/drivers/gpu/drm/i2c/adv7511.h
index 6599ed538426..38515b30cedf 100644
--- a/drivers/gpu/drm/i2c/adv7511.h
+++ b/drivers/gpu/drm/i2c/adv7511.h
@@ -90,7 +90,7 @@
 #define ADV7511_CSC_ENABLE			BIT(7)
 #define ADV7511_CSC_UPDATE_MODE			BIT(5)
 
-#define ADV7511_INT0_HDP			BIT(7)
+#define ADV7511_INT0_HPD			BIT(7)
 #define ADV7511_INT0_VSYNC			BIT(5)
 #define ADV7511_INT0_AUDIO_FIFO_FULL		BIT(4)
 #define ADV7511_INT0_EDID_READY			BIT(2)
@@ -157,11 +157,11 @@
 #define ADV7511_PACKET_ENABLE_SPARE2		BIT(1)
 #define ADV7511_PACKET_ENABLE_SPARE1		BIT(0)
 
-#define ADV7511_REG_POWER2_HDP_SRC_MASK		0xc0
-#define ADV7511_REG_POWER2_HDP_SRC_BOTH		0x00
-#define ADV7511_REG_POWER2_HDP_SRC_HDP		0x40
-#define ADV7511_REG_POWER2_HDP_SRC_CEC		0x80
-#define ADV7511_REG_POWER2_HDP_SRC_NONE		0xc0
+#define ADV7511_REG_POWER2_HPD_SRC_MASK		0xc0
+#define ADV7511_REG_POWER2_HPD_SRC_BOTH		0x00
+#define ADV7511_REG_POWER2_HPD_SRC_HPD		0x40
+#define ADV7511_REG_POWER2_HPD_SRC_CEC		0x80
+#define ADV7511_REG_POWER2_HPD_SRC_NONE		0xc0
 #define ADV7511_REG_POWER2_TDMS_ENABLE		BIT(4)
 #define ADV7511_REG_POWER2_GATE_INPUT_CLK	BIT(0)
 
diff --git a/drivers/gpu/drm/i2c/ch7006_drv.c b/drivers/gpu/drm/i2c/ch7006_drv.c
index d9a72c96e56c..90db5f4dcce5 100644
--- a/drivers/gpu/drm/i2c/ch7006_drv.c
+++ b/drivers/gpu/drm/i2c/ch7006_drv.c
@@ -371,7 +371,7 @@ static int ch7006_encoder_set_property(struct drm_encoder *encoder,
 	return 0;
 }
 
-static struct drm_encoder_slave_funcs ch7006_encoder_funcs = {
+static const struct drm_encoder_slave_funcs ch7006_encoder_funcs = {
 	.set_config = ch7006_encoder_set_config,
 	.destroy = ch7006_encoder_destroy,
 	.dpms = ch7006_encoder_dpms,
diff --git a/drivers/gpu/drm/i2c/sil164_drv.c b/drivers/gpu/drm/i2c/sil164_drv.c
index 002ce7874332..c400428f6c8c 100644
--- a/drivers/gpu/drm/i2c/sil164_drv.c
+++ b/drivers/gpu/drm/i2c/sil164_drv.c
@@ -341,7 +341,7 @@ sil164_encoder_destroy(struct drm_encoder *encoder)
 	drm_i2c_encoder_destroy(encoder);
 }
 
-static struct drm_encoder_slave_funcs sil164_encoder_funcs = {
+static const struct drm_encoder_slave_funcs sil164_encoder_funcs = {
 	.set_config = sil164_encoder_set_config,
 	.destroy = sil164_encoder_destroy,
 	.dpms = sil164_encoder_dpms,
diff --git a/drivers/gpu/drm/i2c/tda998x_drv.c b/drivers/gpu/drm/i2c/tda998x_drv.c
index 896b6aaf8c4d..f8ee740c0e26 100644
--- a/drivers/gpu/drm/i2c/tda998x_drv.c
+++ b/drivers/gpu/drm/i2c/tda998x_drv.c
@@ -22,6 +22,7 @@
 #include <sound/asoundef.h>
 
 #include <drm/drmP.h>
+#include <drm/drm_atomic_helper.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_edid.h>
 #include <drm/drm_of.h>
@@ -855,18 +856,6 @@ static void tda998x_encoder_dpms(struct drm_encoder *encoder, int mode)
 	priv->dpms = mode;
 }
 
-static void
-tda998x_encoder_save(struct drm_encoder *encoder)
-{
-	DBG("");
-}
-
-static void
-tda998x_encoder_restore(struct drm_encoder *encoder)
-{
-	DBG("");
-}
-
 static bool
 tda998x_encoder_mode_fixup(struct drm_encoder *encoder,
 			  const struct drm_display_mode *mode,
@@ -878,7 +867,10 @@ tda998x_encoder_mode_fixup(struct drm_encoder *encoder,
 static int tda998x_connector_mode_valid(struct drm_connector *connector,
 					struct drm_display_mode *mode)
 {
-	if (mode->clock > 150000)
+	/* TDA19988 dotclock can go up to 165MHz */
+	struct tda998x_priv *priv = conn_to_tda998x_priv(connector);
+
+	if (mode->clock > ((priv->rev == TDA19988) ? 165000 : 150000))
 		return MODE_CLOCK_HIGH;
 	if (mode->htotal >= BIT(13))
 		return MODE_BAD_HVALUE;
@@ -1351,8 +1343,6 @@ static void tda998x_encoder_commit(struct drm_encoder *encoder)
 
 static const struct drm_encoder_helper_funcs tda998x_encoder_helper_funcs = {
 	.dpms = tda998x_encoder_dpms,
-	.save = tda998x_encoder_save,
-	.restore = tda998x_encoder_restore,
 	.mode_fixup = tda998x_encoder_mode_fixup,
 	.prepare = tda998x_encoder_prepare,
 	.commit = tda998x_encoder_commit,
@@ -1392,11 +1382,22 @@ static void tda998x_connector_destroy(struct drm_connector *connector)
 	drm_connector_cleanup(connector);
 }
 
+static int tda998x_connector_dpms(struct drm_connector *connector, int mode)
+{
+	if (drm_core_check_feature(connector->dev, DRIVER_ATOMIC))
+		return drm_atomic_helper_connector_dpms(connector, mode);
+	else
+		return drm_helper_connector_dpms(connector, mode);
+}
+
 static const struct drm_connector_funcs tda998x_connector_funcs = {
-	.dpms = drm_helper_connector_dpms,
+	.dpms = tda998x_connector_dpms,
+	.reset = drm_atomic_helper_connector_reset,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = tda998x_connector_detect,
 	.destroy = tda998x_connector_destroy,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
 };
 
 static int tda998x_bind(struct device *dev, struct device *master, void *data)
@@ -1437,7 +1438,7 @@ static int tda998x_bind(struct device *dev, struct device *master, void *data)
 
 	drm_encoder_helper_add(&priv->encoder, &tda998x_encoder_helper_funcs);
 	ret = drm_encoder_init(drm, &priv->encoder, &tda998x_encoder_funcs,
-			       DRM_MODE_ENCODER_TMDS);
+			       DRM_MODE_ENCODER_TMDS, NULL);
 	if (ret)
 		goto err_encoder;
 
@@ -1453,7 +1454,6 @@ static int tda998x_bind(struct device *dev, struct device *master, void *data)
 	if (ret)
 		goto err_sysfs;
 
-	priv->connector.encoder = &priv->encoder;
 	drm_mode_connector_attach_encoder(&priv->connector, &priv->encoder);
 
 	return 0;
@@ -1472,6 +1472,7 @@ static void tda998x_unbind(struct device *dev, struct device *master,
 {
 	struct tda998x_priv *priv = dev_get_drvdata(dev);
 
+	drm_connector_unregister(&priv->connector);
 	drm_connector_cleanup(&priv->connector);
 	drm_encoder_cleanup(&priv->encoder);
 	tda998x_destroy(priv);
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 44d290ae1999..0851de07bd13 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -77,6 +77,7 @@ i915-y += dvo_ch7017.o \
 	  dvo_tfp410.o \
 	  intel_crt.o \
 	  intel_ddi.o \
+	  intel_dp_link_training.o \
 	  intel_dp_mst.o \
 	  intel_dp.o \
 	  intel_dsi.o \
diff --git a/drivers/gpu/drm/i915/dvo.h b/drivers/gpu/drm/i915/dvo.h
index 0e2c1b9648a7..5e6a3013da49 100644
--- a/drivers/gpu/drm/i915/dvo.h
+++ b/drivers/gpu/drm/i915/dvo.h
@@ -32,7 +32,8 @@ struct intel_dvo_device {
 	const char *name;
 	int type;
 	/* DVOA/B/C output register */
-	u32 dvo_reg;
+	i915_reg_t dvo_reg;
+	i915_reg_t dvo_srcdim_reg;
 	/* GPIO register used for i2c bus to control this device */
 	u32 gpio;
 	int slave_addr;
@@ -128,11 +129,11 @@ struct intel_dvo_dev_ops {
 	void (*dump_regs)(struct intel_dvo_device *dvo);
 };
 
-extern struct intel_dvo_dev_ops sil164_ops;
-extern struct intel_dvo_dev_ops ch7xxx_ops;
-extern struct intel_dvo_dev_ops ivch_ops;
-extern struct intel_dvo_dev_ops tfp410_ops;
-extern struct intel_dvo_dev_ops ch7017_ops;
-extern struct intel_dvo_dev_ops ns2501_ops;
+extern const struct intel_dvo_dev_ops sil164_ops;
+extern const struct intel_dvo_dev_ops ch7xxx_ops;
+extern const struct intel_dvo_dev_ops ivch_ops;
+extern const struct intel_dvo_dev_ops tfp410_ops;
+extern const struct intel_dvo_dev_ops ch7017_ops;
+extern const struct intel_dvo_dev_ops ns2501_ops;
 
 #endif /* _INTEL_DVO_H */
diff --git a/drivers/gpu/drm/i915/dvo_ch7017.c b/drivers/gpu/drm/i915/dvo_ch7017.c
index cbb22027a3ce..b3c7c199200c 100644
--- a/drivers/gpu/drm/i915/dvo_ch7017.c
+++ b/drivers/gpu/drm/i915/dvo_ch7017.c
@@ -402,7 +402,7 @@ static void ch7017_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops ch7017_ops = {
+const struct intel_dvo_dev_ops ch7017_ops = {
 	.init = ch7017_init,
 	.detect = ch7017_detect,
 	.mode_valid = ch7017_mode_valid,
diff --git a/drivers/gpu/drm/i915/dvo_ch7xxx.c b/drivers/gpu/drm/i915/dvo_ch7xxx.c
index 4b4acc1a06fe..44b3159f2fe8 100644
--- a/drivers/gpu/drm/i915/dvo_ch7xxx.c
+++ b/drivers/gpu/drm/i915/dvo_ch7xxx.c
@@ -356,7 +356,7 @@ static void ch7xxx_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops ch7xxx_ops = {
+const struct intel_dvo_dev_ops ch7xxx_ops = {
 	.init = ch7xxx_init,
 	.detect = ch7xxx_detect,
 	.mode_valid = ch7xxx_mode_valid,
diff --git a/drivers/gpu/drm/i915/dvo_ivch.c b/drivers/gpu/drm/i915/dvo_ivch.c
index ff9f1b077d83..4950b82f5b49 100644
--- a/drivers/gpu/drm/i915/dvo_ivch.c
+++ b/drivers/gpu/drm/i915/dvo_ivch.c
@@ -490,7 +490,7 @@ static void ivch_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops ivch_ops = {
+const struct intel_dvo_dev_ops ivch_ops = {
 	.init = ivch_init,
 	.dpms = ivch_dpms,
 	.get_hw_state = ivch_get_hw_state,
diff --git a/drivers/gpu/drm/i915/dvo_ns2501.c b/drivers/gpu/drm/i915/dvo_ns2501.c
index 063859fff0f0..2379c33cfe51 100644
--- a/drivers/gpu/drm/i915/dvo_ns2501.c
+++ b/drivers/gpu/drm/i915/dvo_ns2501.c
@@ -698,7 +698,7 @@ static void ns2501_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops ns2501_ops = {
+const struct intel_dvo_dev_ops ns2501_ops = {
 	.init = ns2501_init,
 	.detect = ns2501_detect,
 	.mode_valid = ns2501_mode_valid,
diff --git a/drivers/gpu/drm/i915/dvo_sil164.c b/drivers/gpu/drm/i915/dvo_sil164.c
index 26f13eb634f9..1c1a0674dbab 100644
--- a/drivers/gpu/drm/i915/dvo_sil164.c
+++ b/drivers/gpu/drm/i915/dvo_sil164.c
@@ -267,7 +267,7 @@ static void sil164_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops sil164_ops = {
+const struct intel_dvo_dev_ops sil164_ops = {
 	.init = sil164_init,
 	.detect = sil164_detect,
 	.mode_valid = sil164_mode_valid,
diff --git a/drivers/gpu/drm/i915/dvo_tfp410.c b/drivers/gpu/drm/i915/dvo_tfp410.c
index 6f1a0a6d4e22..31e181da93db 100644
--- a/drivers/gpu/drm/i915/dvo_tfp410.c
+++ b/drivers/gpu/drm/i915/dvo_tfp410.c
@@ -306,7 +306,7 @@ static void tfp410_destroy(struct intel_dvo_device *dvo)
 	}
 }
 
-struct intel_dvo_dev_ops tfp410_ops = {
+const struct intel_dvo_dev_ops tfp410_ops = {
 	.init = tfp410_init,
 	.detect = tfp410_detect,
 	.mode_valid = tfp410_mode_valid,
diff --git a/drivers/gpu/drm/i915/i915_cmd_parser.c b/drivers/gpu/drm/i915/i915_cmd_parser.c
index db58c8d664c2..814d894ed925 100644
--- a/drivers/gpu/drm/i915/i915_cmd_parser.c
+++ b/drivers/gpu/drm/i915/i915_cmd_parser.c
@@ -407,14 +407,14 @@ static const struct drm_i915_cmd_table hsw_blt_ring_cmds[] = {
  * LRI.
  */
 struct drm_i915_reg_descriptor {
-	u32 addr;
+	i915_reg_t addr;
 	u32 mask;
 	u32 value;
 };
 
 /* Convenience macro for adding 32-bit registers. */
-#define REG32(address, ...)                             \
-	{ .addr = address, __VA_ARGS__ }
+#define REG32(_reg, ...) \
+	{ .addr = (_reg), __VA_ARGS__ }
 
 /*
  * Convenience macro for adding 64-bit registers.
@@ -423,8 +423,13 @@ struct drm_i915_reg_descriptor {
  * access commands only allow 32-bit accesses. Hence, we have to include
  * entries for both halves of the 64-bit registers.
  */
-#define REG64(addr)                                     \
-	REG32(addr), REG32(addr + sizeof(u32))
+#define REG64(_reg) \
+	{ .addr = _reg }, \
+	{ .addr = _reg ## _UDW }
+
+#define REG64_IDX(_reg, idx) \
+	{ .addr = _reg(idx) }, \
+	{ .addr = _reg ## _UDW(idx) }
 
 static const struct drm_i915_reg_descriptor gen7_render_regs[] = {
 	REG64(GPGPU_THREADS_DISPATCHED),
@@ -451,14 +456,14 @@ static const struct drm_i915_reg_descriptor gen7_render_regs[] = {
 	REG32(GEN7_GPGPU_DISPATCHDIMX),
 	REG32(GEN7_GPGPU_DISPATCHDIMY),
 	REG32(GEN7_GPGPU_DISPATCHDIMZ),
-	REG64(GEN7_SO_NUM_PRIMS_WRITTEN(0)),
-	REG64(GEN7_SO_NUM_PRIMS_WRITTEN(1)),
-	REG64(GEN7_SO_NUM_PRIMS_WRITTEN(2)),
-	REG64(GEN7_SO_NUM_PRIMS_WRITTEN(3)),
-	REG64(GEN7_SO_PRIM_STORAGE_NEEDED(0)),
-	REG64(GEN7_SO_PRIM_STORAGE_NEEDED(1)),
-	REG64(GEN7_SO_PRIM_STORAGE_NEEDED(2)),
-	REG64(GEN7_SO_PRIM_STORAGE_NEEDED(3)),
+	REG64_IDX(GEN7_SO_NUM_PRIMS_WRITTEN, 0),
+	REG64_IDX(GEN7_SO_NUM_PRIMS_WRITTEN, 1),
+	REG64_IDX(GEN7_SO_NUM_PRIMS_WRITTEN, 2),
+	REG64_IDX(GEN7_SO_NUM_PRIMS_WRITTEN, 3),
+	REG64_IDX(GEN7_SO_PRIM_STORAGE_NEEDED, 0),
+	REG64_IDX(GEN7_SO_PRIM_STORAGE_NEEDED, 1),
+	REG64_IDX(GEN7_SO_PRIM_STORAGE_NEEDED, 2),
+	REG64_IDX(GEN7_SO_PRIM_STORAGE_NEEDED, 3),
 	REG32(GEN7_SO_WRITE_OFFSET(0)),
 	REG32(GEN7_SO_WRITE_OFFSET(1)),
 	REG32(GEN7_SO_WRITE_OFFSET(2)),
@@ -592,7 +597,7 @@ static bool check_sorted(int ring_id,
 	bool ret = true;
 
 	for (i = 0; i < reg_count; i++) {
-		u32 curr = reg_table[i].addr;
+		u32 curr = i915_mmio_reg_offset(reg_table[i].addr);
 
 		if (curr < previous) {
 			DRM_ERROR("CMD: table not sorted ring=%d entry=%d reg=0x%08X prev=0x%08X\n",
@@ -847,7 +852,7 @@ find_reg(const struct drm_i915_reg_descriptor *table,
 		int i;
 
 		for (i = 0; i < count; i++) {
-			if (table[i].addr == addr)
+			if (i915_mmio_reg_offset(table[i].addr) == addr)
 				return &table[i];
 		}
 	}
@@ -1023,7 +1028,7 @@ static bool check_cmd(const struct intel_engine_cs *ring,
 			 * to the register. Hence, limit OACONTROL writes to
 			 * only MI_LOAD_REGISTER_IMM commands.
 			 */
-			if (reg_addr == OACONTROL) {
+			if (reg_addr == i915_mmio_reg_offset(OACONTROL)) {
 				if (desc->cmd.value == MI_LOAD_REGISTER_MEM) {
 					DRM_DEBUG_DRIVER("CMD: Rejected LRM to OACONTROL\n");
 					return false;
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 8aab974b0564..cf39ed3133d6 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -825,8 +825,11 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 		}
 
 		for_each_pipe(dev_priv, pipe) {
-			if (!intel_display_power_is_enabled(dev_priv,
-						POWER_DOMAIN_PIPE(pipe))) {
+			enum intel_display_power_domain power_domain;
+
+			power_domain = POWER_DOMAIN_PIPE(pipe);
+			if (!intel_display_power_get_if_enabled(dev_priv,
+								power_domain)) {
 				seq_printf(m, "Pipe %c power disabled\n",
 					   pipe_name(pipe));
 				continue;
@@ -840,6 +843,8 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 			seq_printf(m, "Pipe %c IER:\t%08x\n",
 				   pipe_name(pipe),
 				   I915_READ(GEN8_DE_PIPE_IER(pipe)));
+
+			intel_display_power_put(dev_priv, power_domain);
 		}
 
 		seq_printf(m, "Display Engine port interrupt mask:\t%08x\n",
@@ -1142,8 +1147,34 @@ static int i915_frequency_info(struct seq_file *m, void *unused)
 			   MEMSTAT_VID_SHIFT);
 		seq_printf(m, "Current P-state: %d\n",
 			   (rgvstat & MEMSTAT_PSTATE_MASK) >> MEMSTAT_PSTATE_SHIFT);
-	} else if (IS_GEN6(dev) || (IS_GEN7(dev) && !IS_VALLEYVIEW(dev)) ||
-		   IS_BROADWELL(dev) || IS_GEN9(dev)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
+		u32 freq_sts;
+
+		mutex_lock(&dev_priv->rps.hw_lock);
+		freq_sts = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
+		seq_printf(m, "PUNIT_REG_GPU_FREQ_STS: 0x%08x\n", freq_sts);
+		seq_printf(m, "DDR freq: %d MHz\n", dev_priv->mem_freq);
+
+		seq_printf(m, "actual GPU freq: %d MHz\n",
+			   intel_gpu_freq(dev_priv, (freq_sts >> 8) & 0xff));
+
+		seq_printf(m, "current GPU freq: %d MHz\n",
+			   intel_gpu_freq(dev_priv, dev_priv->rps.cur_freq));
+
+		seq_printf(m, "max GPU freq: %d MHz\n",
+			   intel_gpu_freq(dev_priv, dev_priv->rps.max_freq));
+
+		seq_printf(m, "min GPU freq: %d MHz\n",
+			   intel_gpu_freq(dev_priv, dev_priv->rps.min_freq));
+
+		seq_printf(m, "idle GPU freq: %d MHz\n",
+			   intel_gpu_freq(dev_priv, dev_priv->rps.idle_freq));
+
+		seq_printf(m,
+			   "efficient (RPe) frequency: %d MHz\n",
+			   intel_gpu_freq(dev_priv, dev_priv->rps.efficient_freq));
+		mutex_unlock(&dev_priv->rps.hw_lock);
+	} else if (INTEL_INFO(dev)->gen >= 6) {
 		u32 rp_state_limits;
 		u32 gt_perf_status;
 		u32 rp_state_cap;
@@ -1252,18 +1283,21 @@ static int i915_frequency_info(struct seq_file *m, void *unused)
 
 		max_freq = (IS_BROXTON(dev) ? rp_state_cap >> 0 :
 			    rp_state_cap >> 16) & 0xff;
-		max_freq *= (IS_SKYLAKE(dev) ? GEN9_FREQ_SCALER : 1);
+		max_freq *= (IS_SKYLAKE(dev) || IS_KABYLAKE(dev) ?
+			     GEN9_FREQ_SCALER : 1);
 		seq_printf(m, "Lowest (RPN) frequency: %dMHz\n",
 			   intel_gpu_freq(dev_priv, max_freq));
 
 		max_freq = (rp_state_cap & 0xff00) >> 8;
-		max_freq *= (IS_SKYLAKE(dev) ? GEN9_FREQ_SCALER : 1);
+		max_freq *= (IS_SKYLAKE(dev) || IS_KABYLAKE(dev) ?
+			     GEN9_FREQ_SCALER : 1);
 		seq_printf(m, "Nominal (RP1) frequency: %dMHz\n",
 			   intel_gpu_freq(dev_priv, max_freq));
 
 		max_freq = (IS_BROXTON(dev) ? rp_state_cap >> 16 :
 			    rp_state_cap >> 0) & 0xff;
-		max_freq *= (IS_SKYLAKE(dev) ? GEN9_FREQ_SCALER : 1);
+		max_freq *= (IS_SKYLAKE(dev) || IS_KABYLAKE(dev) ?
+			     GEN9_FREQ_SCALER : 1);
 		seq_printf(m, "Max non-overclocked (RP0) frequency: %dMHz\n",
 			   intel_gpu_freq(dev_priv, max_freq));
 		seq_printf(m, "Max overclocked frequency: %dMHz\n",
@@ -1281,33 +1315,6 @@ static int i915_frequency_info(struct seq_file *m, void *unused)
 		seq_printf(m,
 			   "efficient (RPe) frequency: %d MHz\n",
 			   intel_gpu_freq(dev_priv, dev_priv->rps.efficient_freq));
-	} else if (IS_VALLEYVIEW(dev)) {
-		u32 freq_sts;
-
-		mutex_lock(&dev_priv->rps.hw_lock);
-		freq_sts = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
-		seq_printf(m, "PUNIT_REG_GPU_FREQ_STS: 0x%08x\n", freq_sts);
-		seq_printf(m, "DDR freq: %d MHz\n", dev_priv->mem_freq);
-
-		seq_printf(m, "actual GPU freq: %d MHz\n",
-			   intel_gpu_freq(dev_priv, (freq_sts >> 8) & 0xff));
-
-		seq_printf(m, "current GPU freq: %d MHz\n",
-			   intel_gpu_freq(dev_priv, dev_priv->rps.cur_freq));
-
-		seq_printf(m, "max GPU freq: %d MHz\n",
-			   intel_gpu_freq(dev_priv, dev_priv->rps.max_freq));
-
-		seq_printf(m, "min GPU freq: %d MHz\n",
-			   intel_gpu_freq(dev_priv, dev_priv->rps.min_freq));
-
-		seq_printf(m, "idle GPU freq: %d MHz\n",
-			   intel_gpu_freq(dev_priv, dev_priv->rps.idle_freq));
-
-		seq_printf(m,
-			   "efficient (RPe) frequency: %d MHz\n",
-			   intel_gpu_freq(dev_priv, dev_priv->rps.efficient_freq));
-		mutex_unlock(&dev_priv->rps.hw_lock);
 	} else {
 		seq_puts(m, "no P-state info available\n");
 	}
@@ -1523,7 +1530,7 @@ static int gen6_drpc_info(struct seq_file *m)
 		seq_printf(m, "RC information accurate: %s\n", yesno(count < 51));
 	}
 
-	gt_core_status = readl(dev_priv->regs + GEN6_GT_CORE_STATUS);
+	gt_core_status = I915_READ_FW(GEN6_GT_CORE_STATUS);
 	trace_i915_reg_rw(false, GEN6_GT_CORE_STATUS, gt_core_status, 4, true);
 
 	rpmodectl1 = I915_READ(GEN6_RP_CONTROL);
@@ -1599,7 +1606,7 @@ static int i915_drpc_info(struct seq_file *m, void *unused)
 	struct drm_info_node *node = m->private;
 	struct drm_device *dev = node->minor->dev;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		return vlv_drpc_info(m);
 	else if (INTEL_INFO(dev)->gen >= 6)
 		return gen6_drpc_info(m);
@@ -1636,11 +1643,11 @@ static int i915_fbc_status(struct seq_file *m, void *unused)
 	intel_runtime_pm_get(dev_priv);
 	mutex_lock(&dev_priv->fbc.lock);
 
-	if (intel_fbc_enabled(dev_priv))
+	if (intel_fbc_is_active(dev_priv))
 		seq_puts(m, "FBC enabled\n");
 	else
 		seq_printf(m, "FBC disabled: %s\n",
-			  intel_no_fbc_reason_str(dev_priv->fbc.no_fbc_reason));
+			   dev_priv->fbc.no_fbc_reason);
 
 	if (INTEL_INFO(dev_priv)->gen >= 7)
 		seq_printf(m, "Compressing: %s\n",
@@ -1740,7 +1747,7 @@ static int i915_sr_status(struct seq_file *m, void *unused)
 		sr_enabled = I915_READ(INSTPM) & INSTPM_SELF_EN;
 	else if (IS_PINEVIEW(dev))
 		sr_enabled = I915_READ(DSPFW3) & PINEVIEW_SELF_REFRESH_EN;
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		sr_enabled = I915_READ(FW_BLC_SELF_VLV) & FW_CSPWRDWNEN;
 
 	intel_runtime_pm_put(dev_priv);
@@ -1801,7 +1808,7 @@ static int i915_ring_freq_table(struct seq_file *m, void *unused)
 	if (ret)
 		goto out;
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		/* Convert GT frequency to 50 HZ units */
 		min_gpu_freq =
 			dev_priv->rps.min_freq_softlimit / GEN9_FREQ_SCALER;
@@ -1821,7 +1828,8 @@ static int i915_ring_freq_table(struct seq_file *m, void *unused)
 				       &ia_freq);
 		seq_printf(m, "%d\t\t%d\t\t\t\t%d\n",
 			   intel_gpu_freq(dev_priv, (gpu_freq *
-				(IS_SKYLAKE(dev) ? GEN9_FREQ_SCALER : 1))),
+				(IS_SKYLAKE(dev) || IS_KABYLAKE(dev) ?
+				 GEN9_FREQ_SCALER : 1))),
 			   ((ia_freq >> 0) & 0xff) * 100,
 			   ((ia_freq >> 8) & 0xff) * 100);
 	}
@@ -1839,25 +1847,31 @@ static int i915_opregion(struct seq_file *m, void *unused)
 	struct drm_device *dev = node->minor->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_opregion *opregion = &dev_priv->opregion;
-	void *data = kmalloc(OPREGION_SIZE, GFP_KERNEL);
 	int ret;
 
-	if (data == NULL)
-		return -ENOMEM;
-
 	ret = mutex_lock_interruptible(&dev->struct_mutex);
 	if (ret)
 		goto out;
 
-	if (opregion->header) {
-		memcpy(data, opregion->header, OPREGION_SIZE);
-		seq_write(m, data, OPREGION_SIZE);
-	}
+	if (opregion->header)
+		seq_write(m, opregion->header, OPREGION_SIZE);
 
 	mutex_unlock(&dev->struct_mutex);
 
 out:
-	kfree(data);
+	return 0;
+}
+
+static int i915_vbt(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_opregion *opregion = &dev_priv->opregion;
+
+	if (opregion->vbt)
+		seq_write(m, opregion->vbt, opregion->vbt_size);
+
 	return 0;
 }
 
@@ -1865,31 +1879,29 @@ static int i915_gem_framebuffer_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = m->private;
 	struct drm_device *dev = node->minor->dev;
-	struct intel_fbdev *ifbdev = NULL;
-	struct intel_framebuffer *fb;
+	struct intel_framebuffer *fbdev_fb = NULL;
 	struct drm_framebuffer *drm_fb;
 
 #ifdef CONFIG_DRM_FBDEV_EMULATION
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	ifbdev = dev_priv->fbdev;
-	fb = to_intel_framebuffer(ifbdev->helper.fb);
-
-	seq_printf(m, "fbcon size: %d x %d, depth %d, %d bpp, modifier 0x%llx, refcount %d, obj ",
-		   fb->base.width,
-		   fb->base.height,
-		   fb->base.depth,
-		   fb->base.bits_per_pixel,
-		   fb->base.modifier[0],
-		   atomic_read(&fb->base.refcount.refcount));
-	describe_obj(m, fb->obj);
-	seq_putc(m, '\n');
+       if (to_i915(dev)->fbdev) {
+               fbdev_fb = to_intel_framebuffer(to_i915(dev)->fbdev->helper.fb);
+
+               seq_printf(m, "fbcon size: %d x %d, depth %d, %d bpp, modifier 0x%llx, refcount %d, obj ",
+                         fbdev_fb->base.width,
+                         fbdev_fb->base.height,
+                         fbdev_fb->base.depth,
+                         fbdev_fb->base.bits_per_pixel,
+                         fbdev_fb->base.modifier[0],
+                         atomic_read(&fbdev_fb->base.refcount.refcount));
+               describe_obj(m, fbdev_fb->obj);
+               seq_putc(m, '\n');
+       }
 #endif
 
 	mutex_lock(&dev->mode_config.fb_lock);
 	drm_for_each_fb(drm_fb, dev) {
-		fb = to_intel_framebuffer(drm_fb);
-		if (ifbdev && &fb->base == ifbdev->helper.fb)
+		struct intel_framebuffer *fb = to_intel_framebuffer(drm_fb);
+		if (fb == fbdev_fb)
 			continue;
 
 		seq_printf(m, "user size: %d x %d, depth %d, %d bpp, modifier 0x%llx, refcount %d, obj ",
@@ -2402,6 +2414,12 @@ static int i915_guc_load_status_info(struct seq_file *m, void *data)
 		guc_fw->guc_fw_major_wanted, guc_fw->guc_fw_minor_wanted);
 	seq_printf(m, "\tversion found: %d.%d\n",
 		guc_fw->guc_fw_major_found, guc_fw->guc_fw_minor_found);
+	seq_printf(m, "\theader: offset is %d; size = %d\n",
+		guc_fw->header_offset, guc_fw->header_size);
+	seq_printf(m, "\tuCode: offset is %d; size = %d\n",
+		guc_fw->ucode_offset, guc_fw->ucode_size);
+	seq_printf(m, "\tRSA: offset is %d; size = %d\n",
+		guc_fw->rsa_offset, guc_fw->rsa_size);
 
 	tmp = I915_READ(GUC_STATUS);
 
@@ -2461,15 +2479,15 @@ static int i915_guc_info(struct seq_file *m, void *data)
 	if (!HAS_GUC_SCHED(dev_priv->dev))
 		return 0;
 
+	if (mutex_lock_interruptible(&dev->struct_mutex))
+		return 0;
+
 	/* Take a local copy of the GuC data, so we can dump it at leisure */
-	spin_lock(&dev_priv->guc.host2guc_lock);
 	guc = dev_priv->guc;
-	if (guc.execbuf_client) {
-		spin_lock(&guc.execbuf_client->wq_lock);
+	if (guc.execbuf_client)
 		client = *guc.execbuf_client;
-		spin_unlock(&guc.execbuf_client->wq_lock);
-	}
-	spin_unlock(&dev_priv->guc.host2guc_lock);
+
+	mutex_unlock(&dev->struct_mutex);
 
 	seq_printf(m, "GuC total action count: %llu\n", guc.action_count);
 	seq_printf(m, "GuC action failure count: %u\n", guc.action_fail);
@@ -2550,7 +2568,7 @@ static int i915_edp_psr_status(struct seq_file *m, void *data)
 		   yesno(work_busy(&dev_priv->psr.work.work)));
 
 	if (HAS_DDI(dev))
-		enabled = I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE;
+		enabled = I915_READ(EDP_PSR_CTL) & EDP_PSR_ENABLE;
 	else {
 		for_each_pipe(dev_priv, pipe) {
 			stat[pipe] = I915_READ(VLV_PSRSTAT(pipe)) &
@@ -2570,9 +2588,12 @@ static int i915_edp_psr_status(struct seq_file *m, void *data)
 		}
 	seq_puts(m, "\n");
 
-	/* CHV PSR has no kind of performance counter */
-	if (HAS_DDI(dev)) {
-		psrperf = I915_READ(EDP_PSR_PERF_CNT(dev)) &
+	/*
+	 * VLV/CHV PSR has no kind of performance counter
+	 * SKL+ Perf counter is reset to 0 everytime DC state is entered
+	 */
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+		psrperf = I915_READ(EDP_PSR_PERF_CNT) &
 			EDP_PSR_PERF_CNT_MASK;
 
 		seq_printf(m, "Performance_Counter: %u\n", psrperf);
@@ -2673,77 +2694,6 @@ static int i915_runtime_pm_status(struct seq_file *m, void *unused)
 	return 0;
 }
 
-static const char *power_domain_str(enum intel_display_power_domain domain)
-{
-	switch (domain) {
-	case POWER_DOMAIN_PIPE_A:
-		return "PIPE_A";
-	case POWER_DOMAIN_PIPE_B:
-		return "PIPE_B";
-	case POWER_DOMAIN_PIPE_C:
-		return "PIPE_C";
-	case POWER_DOMAIN_PIPE_A_PANEL_FITTER:
-		return "PIPE_A_PANEL_FITTER";
-	case POWER_DOMAIN_PIPE_B_PANEL_FITTER:
-		return "PIPE_B_PANEL_FITTER";
-	case POWER_DOMAIN_PIPE_C_PANEL_FITTER:
-		return "PIPE_C_PANEL_FITTER";
-	case POWER_DOMAIN_TRANSCODER_A:
-		return "TRANSCODER_A";
-	case POWER_DOMAIN_TRANSCODER_B:
-		return "TRANSCODER_B";
-	case POWER_DOMAIN_TRANSCODER_C:
-		return "TRANSCODER_C";
-	case POWER_DOMAIN_TRANSCODER_EDP:
-		return "TRANSCODER_EDP";
-	case POWER_DOMAIN_PORT_DDI_A_2_LANES:
-		return "PORT_DDI_A_2_LANES";
-	case POWER_DOMAIN_PORT_DDI_A_4_LANES:
-		return "PORT_DDI_A_4_LANES";
-	case POWER_DOMAIN_PORT_DDI_B_2_LANES:
-		return "PORT_DDI_B_2_LANES";
-	case POWER_DOMAIN_PORT_DDI_B_4_LANES:
-		return "PORT_DDI_B_4_LANES";
-	case POWER_DOMAIN_PORT_DDI_C_2_LANES:
-		return "PORT_DDI_C_2_LANES";
-	case POWER_DOMAIN_PORT_DDI_C_4_LANES:
-		return "PORT_DDI_C_4_LANES";
-	case POWER_DOMAIN_PORT_DDI_D_2_LANES:
-		return "PORT_DDI_D_2_LANES";
-	case POWER_DOMAIN_PORT_DDI_D_4_LANES:
-		return "PORT_DDI_D_4_LANES";
-	case POWER_DOMAIN_PORT_DDI_E_2_LANES:
-		return "PORT_DDI_E_2_LANES";
-	case POWER_DOMAIN_PORT_DSI:
-		return "PORT_DSI";
-	case POWER_DOMAIN_PORT_CRT:
-		return "PORT_CRT";
-	case POWER_DOMAIN_PORT_OTHER:
-		return "PORT_OTHER";
-	case POWER_DOMAIN_VGA:
-		return "VGA";
-	case POWER_DOMAIN_AUDIO:
-		return "AUDIO";
-	case POWER_DOMAIN_PLLS:
-		return "PLLS";
-	case POWER_DOMAIN_AUX_A:
-		return "AUX_A";
-	case POWER_DOMAIN_AUX_B:
-		return "AUX_B";
-	case POWER_DOMAIN_AUX_C:
-		return "AUX_C";
-	case POWER_DOMAIN_AUX_D:
-		return "AUX_D";
-	case POWER_DOMAIN_GMBUS:
-		return "GMBUS";
-	case POWER_DOMAIN_INIT:
-		return "INIT";
-	default:
-		MISSING_CASE(domain);
-		return "?";
-	}
-}
-
 static int i915_power_domain_info(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = m->private;
@@ -2769,7 +2719,7 @@ static int i915_power_domain_info(struct seq_file *m, void *unused)
 				continue;
 
 			seq_printf(m, "  %-23s %d\n",
-				 power_domain_str(power_domain),
+				 intel_display_power_domain_str(power_domain),
 				 power_domains->domain_use_count[power_domain]);
 		}
 	}
@@ -2779,6 +2729,51 @@ static int i915_power_domain_info(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int i915_dmc_info(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_csr *csr;
+
+	if (!HAS_CSR(dev)) {
+		seq_puts(m, "not supported\n");
+		return 0;
+	}
+
+	csr = &dev_priv->csr;
+
+	intel_runtime_pm_get(dev_priv);
+
+	seq_printf(m, "fw loaded: %s\n", yesno(csr->dmc_payload != NULL));
+	seq_printf(m, "path: %s\n", csr->fw_path);
+
+	if (!csr->dmc_payload)
+		goto out;
+
+	seq_printf(m, "version: %d.%d\n", CSR_VERSION_MAJOR(csr->version),
+		   CSR_VERSION_MINOR(csr->version));
+
+	if (IS_SKYLAKE(dev) && csr->version >= CSR_VERSION(1, 6)) {
+		seq_printf(m, "DC3 -> DC5 count: %d\n",
+			   I915_READ(SKL_CSR_DC3_DC5_COUNT));
+		seq_printf(m, "DC5 -> DC6 count: %d\n",
+			   I915_READ(SKL_CSR_DC5_DC6_COUNT));
+	} else if (IS_BROXTON(dev) && csr->version >= CSR_VERSION(1, 4)) {
+		seq_printf(m, "DC3 -> DC5 count: %d\n",
+			   I915_READ(BXT_CSR_DC3_DC5_COUNT));
+	}
+
+out:
+	seq_printf(m, "program base: 0x%08x\n", I915_READ(CSR_PROGRAM(0)));
+	seq_printf(m, "ssp base: 0x%08x\n", I915_READ(CSR_SSP_BASE));
+	seq_printf(m, "htp: 0x%08x\n", I915_READ(CSR_HTP_SKL));
+
+	intel_runtime_pm_put(dev_priv);
+
+	return 0;
+}
+
 static void intel_seq_print_mode(struct seq_file *m, int tabs,
 				 struct drm_display_mode *mode)
 {
@@ -2865,6 +2860,20 @@ static void intel_dp_info(struct seq_file *m,
 		intel_panel_info(m, &intel_connector->panel);
 }
 
+static void intel_dp_mst_info(struct seq_file *m,
+			  struct intel_connector *intel_connector)
+{
+	struct intel_encoder *intel_encoder = intel_connector->encoder;
+	struct intel_dp_mst_encoder *intel_mst =
+		enc_to_mst(&intel_encoder->base);
+	struct intel_digital_port *intel_dig_port = intel_mst->primary;
+	struct intel_dp *intel_dp = &intel_dig_port->dp;
+	bool has_audio = drm_dp_mst_port_has_audio(&intel_dp->mst_mgr,
+					intel_connector->port);
+
+	seq_printf(m, "\taudio support: %s\n", yesno(has_audio));
+}
+
 static void intel_hdmi_info(struct seq_file *m,
 			    struct intel_connector *intel_connector)
 {
@@ -2908,6 +2917,8 @@ static void intel_connector_info(struct seq_file *m,
 			intel_hdmi_info(m, intel_connector);
 		else if (intel_encoder->type == INTEL_OUTPUT_LVDS)
 			intel_lvds_info(m, intel_connector);
+		else if (intel_encoder->type == INTEL_OUTPUT_DP_MST)
+			intel_dp_mst_info(m, intel_connector);
 	}
 
 	seq_printf(m, "\tmodes:\n");
@@ -2946,6 +2957,107 @@ static bool cursor_position(struct drm_device *dev, int pipe, int *x, int *y)
 	return cursor_active(dev, pipe);
 }
 
+static const char *plane_type(enum drm_plane_type type)
+{
+	switch (type) {
+	case DRM_PLANE_TYPE_OVERLAY:
+		return "OVL";
+	case DRM_PLANE_TYPE_PRIMARY:
+		return "PRI";
+	case DRM_PLANE_TYPE_CURSOR:
+		return "CUR";
+	/*
+	 * Deliberately omitting default: to generate compiler warnings
+	 * when a new drm_plane_type gets added.
+	 */
+	}
+
+	return "unknown";
+}
+
+static const char *plane_rotation(unsigned int rotation)
+{
+	static char buf[48];
+	/*
+	 * According to doc only one DRM_ROTATE_ is allowed but this
+	 * will print them all to visualize if the values are misused
+	 */
+	snprintf(buf, sizeof(buf),
+		 "%s%s%s%s%s%s(0x%08x)",
+		 (rotation & BIT(DRM_ROTATE_0)) ? "0 " : "",
+		 (rotation & BIT(DRM_ROTATE_90)) ? "90 " : "",
+		 (rotation & BIT(DRM_ROTATE_180)) ? "180 " : "",
+		 (rotation & BIT(DRM_ROTATE_270)) ? "270 " : "",
+		 (rotation & BIT(DRM_REFLECT_X)) ? "FLIPX " : "",
+		 (rotation & BIT(DRM_REFLECT_Y)) ? "FLIPY " : "",
+		 rotation);
+
+	return buf;
+}
+
+static void intel_plane_info(struct seq_file *m, struct intel_crtc *intel_crtc)
+{
+	struct drm_info_node *node = m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct intel_plane *intel_plane;
+
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		struct drm_plane_state *state;
+		struct drm_plane *plane = &intel_plane->base;
+
+		if (!plane->state) {
+			seq_puts(m, "plane->state is NULL!\n");
+			continue;
+		}
+
+		state = plane->state;
+
+		seq_printf(m, "\t--Plane id %d: type=%s, crtc_pos=%4dx%4d, crtc_size=%4dx%4d, src_pos=%d.%04ux%d.%04u, src_size=%d.%04ux%d.%04u, format=%s, rotation=%s\n",
+			   plane->base.id,
+			   plane_type(intel_plane->base.type),
+			   state->crtc_x, state->crtc_y,
+			   state->crtc_w, state->crtc_h,
+			   (state->src_x >> 16),
+			   ((state->src_x & 0xffff) * 15625) >> 10,
+			   (state->src_y >> 16),
+			   ((state->src_y & 0xffff) * 15625) >> 10,
+			   (state->src_w >> 16),
+			   ((state->src_w & 0xffff) * 15625) >> 10,
+			   (state->src_h >> 16),
+			   ((state->src_h & 0xffff) * 15625) >> 10,
+			   state->fb ? drm_get_format_name(state->fb->pixel_format) : "N/A",
+			   plane_rotation(state->rotation));
+	}
+}
+
+static void intel_scaler_info(struct seq_file *m, struct intel_crtc *intel_crtc)
+{
+	struct intel_crtc_state *pipe_config;
+	int num_scalers = intel_crtc->num_scalers;
+	int i;
+
+	pipe_config = to_intel_crtc_state(intel_crtc->base.state);
+
+	/* Not all platformas have a scaler */
+	if (num_scalers) {
+		seq_printf(m, "\tnum_scalers=%d, scaler_users=%x scaler_id=%d",
+			   num_scalers,
+			   pipe_config->scaler_state.scaler_users,
+			   pipe_config->scaler_state.scaler_id);
+
+		for (i = 0; i < SKL_NUM_SCALERS; i++) {
+			struct intel_scaler *sc =
+					&pipe_config->scaler_state.scalers[i];
+
+			seq_printf(m, ", scalers[%d]: use=%s, mode=%x",
+				   i, yesno(sc->in_use), sc->mode);
+		}
+		seq_puts(m, "\n");
+	} else {
+		seq_puts(m, "\tNo scalers available on this platform\n");
+	}
+}
+
 static int i915_display_info(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = m->private;
@@ -2965,10 +3077,12 @@ static int i915_display_info(struct seq_file *m, void *unused)
 
 		pipe_config = to_intel_crtc_state(crtc->base.state);
 
-		seq_printf(m, "CRTC %d: pipe: %c, active=%s (size=%dx%d)\n",
+		seq_printf(m, "CRTC %d: pipe: %c, active=%s, (size=%dx%d), dither=%s, bpp=%d\n",
 			   crtc->base.base.id, pipe_name(crtc->pipe),
 			   yesno(pipe_config->base.active),
-			   pipe_config->pipe_src_w, pipe_config->pipe_src_h);
+			   pipe_config->pipe_src_w, pipe_config->pipe_src_h,
+			   yesno(pipe_config->dither), pipe_config->pipe_bpp);
+
 		if (pipe_config->base.active) {
 			intel_crtc_info(m, crtc);
 
@@ -2978,6 +3092,8 @@ static int i915_display_info(struct seq_file *m, void *unused)
 				   x, y, crtc->base.cursor->state->crtc_w,
 				   crtc->base.cursor->state->crtc_h,
 				   crtc->cursor_addr, yesno(active));
+			intel_scaler_info(m, crtc);
+			intel_plane_info(m, crtc);
 		}
 
 		seq_printf(m, "\tunderrun reporting: cpu=%s pch=%s \n",
@@ -3112,7 +3228,8 @@ static int i915_wa_registers(struct seq_file *m, void *unused)
 
 	seq_printf(m, "Workarounds applied: %d\n", dev_priv->workarounds.count);
 	for (i = 0; i < dev_priv->workarounds.count; ++i) {
-		u32 addr, mask, value, read;
+		i915_reg_t addr;
+		u32 mask, value, read;
 		bool ok;
 
 		addr = dev_priv->workarounds.reg[i].addr;
@@ -3121,7 +3238,7 @@ static int i915_wa_registers(struct seq_file *m, void *unused)
 		read = I915_READ(addr);
 		ok = (value & mask) == (read & mask);
 		seq_printf(m, "0x%X: 0x%08X, mask: 0x%08X, read: 0x%08x, status: %s\n",
-			   addr, value, mask, read, ok ? "OK" : "FAIL");
+			   i915_mmio_reg_offset(addr), value, mask, read, ok ? "OK" : "FAIL");
 	}
 
 	intel_runtime_pm_put(dev_priv);
@@ -3873,6 +3990,7 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 	struct intel_pipe_crc *pipe_crc = &dev_priv->pipe_crc[pipe];
 	struct intel_crtc *crtc = to_intel_crtc(intel_get_crtc_for_pipe(dev,
 									pipe));
+	enum intel_display_power_domain power_domain;
 	u32 val = 0; /* shut up gcc */
 	int ret;
 
@@ -3883,7 +4001,8 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 	if (pipe_crc->source && source)
 		return -EINVAL;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PIPE(pipe))) {
+	power_domain = POWER_DOMAIN_PIPE(pipe);
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain)) {
 		DRM_DEBUG_KMS("Trying to capture CRC while pipe is off\n");
 		return -EIO;
 	}
@@ -3892,7 +4011,7 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 		ret = i8xx_pipe_crc_ctl_reg(&source, &val);
 	else if (INTEL_INFO(dev)->gen < 5)
 		ret = i9xx_pipe_crc_ctl_reg(dev, pipe, &source, &val);
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		ret = vlv_pipe_crc_ctl_reg(dev, pipe, &source, &val);
 	else if (IS_GEN5(dev) || IS_GEN6(dev))
 		ret = ilk_pipe_crc_ctl_reg(&source, &val);
@@ -3900,7 +4019,7 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 		ret = ivb_pipe_crc_ctl_reg(dev, pipe, &source, &val);
 
 	if (ret != 0)
-		return ret;
+		goto out;
 
 	/* none -> real source transition */
 	if (source) {
@@ -3912,8 +4031,10 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 		entries = kcalloc(INTEL_PIPE_CRC_ENTRIES_NR,
 				  sizeof(pipe_crc->entries[0]),
 				  GFP_KERNEL);
-		if (!entries)
-			return -ENOMEM;
+		if (!entries) {
+			ret = -ENOMEM;
+			goto out;
+		}
 
 		/*
 		 * When IPS gets enabled, the pipe CRC changes. Since IPS gets
@@ -3961,7 +4082,7 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 
 		if (IS_G4X(dev))
 			g4x_undo_pipe_scramble_reset(dev, pipe);
-		else if (IS_VALLEYVIEW(dev))
+		else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 			vlv_undo_pipe_scramble_reset(dev, pipe);
 		else if (IS_HASWELL(dev) && pipe == PIPE_A)
 			hsw_trans_edp_pipe_A_crc_wa(dev, false);
@@ -3969,7 +4090,12 @@ static int pipe_crc_set_source(struct drm_device *dev, enum pipe pipe,
 		hsw_enable_ips(crtc);
 	}
 
-	return 0;
+	ret = 0;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 /*
@@ -4351,7 +4477,8 @@ static void wm_latency_show(struct seq_file *m, const uint16_t wm[8])
 		 * - WM1+ latency values in 0.5us units
 		 * - latencies are in us on gen9/vlv/chv
 		 */
-		if (INTEL_INFO(dev)->gen >= 9 || IS_VALLEYVIEW(dev))
+		if (INTEL_INFO(dev)->gen >= 9 || IS_VALLEYVIEW(dev) ||
+		    IS_CHERRYVIEW(dev))
 			latency *= 10;
 		else if (level > 0)
 			latency *= 5;
@@ -5025,7 +5152,7 @@ static void gen9_sseu_device_status(struct drm_device *dev,
 
 		stat->slice_total++;
 
-		if (IS_SKYLAKE(dev))
+		if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 			ss_cnt = INTEL_INFO(dev)->subslice_per_slice;
 
 		for (ss = 0; ss < ss_max; ss++) {
@@ -5225,6 +5352,7 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_ips_status", i915_ips_status, 0},
 	{"i915_sr_status", i915_sr_status, 0},
 	{"i915_opregion", i915_opregion, 0},
+	{"i915_vbt", i915_vbt, 0},
 	{"i915_gem_framebuffer", i915_gem_framebuffer_info, 0},
 	{"i915_context_status", i915_context_status, 0},
 	{"i915_dump_lrc", i915_dump_lrc, 0},
@@ -5238,6 +5366,7 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_energy_uJ", i915_energy_uJ, 0},
 	{"i915_runtime_pm_status", i915_runtime_pm_status, 0},
 	{"i915_power_domain_info", i915_power_domain_info, 0},
+	{"i915_dmc_info", i915_dmc_info, 0},
 	{"i915_display_info", i915_display_info, 0},
 	{"i915_semaphore_status", i915_semaphore_status, 0},
 	{"i915_shared_dplls_info", i915_shared_dplls_info, 0},
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index b4741d121a74..d70d96fe553b 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -28,7 +28,6 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/async.h>
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
@@ -170,6 +169,9 @@ static int i915_getparam(struct drm_device *dev, void *data,
 	case I915_PARAM_HAS_RESOURCE_STREAMER:
 		value = HAS_RESOURCE_STREAMER(dev);
 		break;
+	case I915_PARAM_HAS_EXEC_SOFTPIN:
+		value = 1;
+		break;
 	default:
 		DRM_DEBUG("Unknown parameter %d\n", param->param);
 		return -EINVAL;
@@ -257,7 +259,7 @@ intel_setup_mchbar(struct drm_device *dev)
 	u32 temp;
 	bool enabled;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		return;
 
 	dev_priv->mchbar_need_disable = false;
@@ -338,7 +340,7 @@ static void i915_switcheroo_set_state(struct pci_dev *pdev, enum vga_switcheroo_
 		i915_resume_switcheroo(dev);
 		dev->switch_power_state = DRM_SWITCH_POWER_ON;
 	} else {
-		pr_err("switched off\n");
+		pr_info("switched off\n");
 		dev->switch_power_state = DRM_SWITCH_POWER_CHANGING;
 		i915_suspend_switcheroo(dev, pmm);
 		dev->switch_power_state = DRM_SWITCH_POWER_OFF;
@@ -368,7 +370,7 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	ret = intel_parse_bios(dev);
+	ret = intel_bios_init(dev_priv);
 	if (ret)
 		DRM_INFO("failed to find VBIOS tables\n");
 
@@ -396,12 +398,16 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	if (ret)
 		goto cleanup_vga_switcheroo;
 
-	intel_power_domains_init_hw(dev_priv);
+	intel_power_domains_init_hw(dev_priv, false);
+
+	intel_csr_ucode_init(dev_priv);
 
 	ret = intel_irq_install(dev_priv);
 	if (ret)
 		goto cleanup_gem_stolen;
 
+	intel_setup_gmbus(dev);
+
 	/* Important: The output setup functions called by modeset_init need
 	 * working irqs for e.g. gmbus and dp aux transfers. */
 	intel_modeset_init(dev);
@@ -437,7 +443,7 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	 * scanning against hotplug events. Hence do this first and ignore the
 	 * tiny window where we will loose hotplug notifactions.
 	 */
-	async_schedule(intel_fbdev_initial_config, dev_priv);
+	intel_fbdev_initial_config_async(dev);
 
 	drm_kms_helper_poll_init(dev);
 
@@ -451,6 +457,7 @@ cleanup_gem:
 cleanup_irq:
 	intel_guc_ucode_fini(dev);
 	drm_irq_uninstall(dev);
+	intel_teardown_gmbus(dev);
 cleanup_gem_stolen:
 	i915_gem_cleanup_stolen(dev);
 cleanup_vga_switcheroo:
@@ -663,7 +670,8 @@ static void gen9_sseu_info_init(struct drm_device *dev)
 	 * supports EU power gating on devices with more than one EU
 	 * pair per subslice.
 	*/
-	info->has_slice_pg = (IS_SKYLAKE(dev) && (info->slice_total > 1));
+	info->has_slice_pg = ((IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) &&
+			       (info->slice_total > 1));
 	info->has_subslice_pg = (IS_BROXTON(dev) && (info->subslice_total > 1));
 	info->has_eu_pg = (info->eu_per_subslice > 2);
 }
@@ -777,7 +785,7 @@ static void intel_device_info_runtime_init(struct drm_device *dev)
 		info->num_sprites[PIPE_A] = 2;
 		info->num_sprites[PIPE_B] = 2;
 		info->num_sprites[PIPE_C] = 1;
-	} else if (IS_VALLEYVIEW(dev))
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 2;
 	else
@@ -789,7 +797,7 @@ static void intel_device_info_runtime_init(struct drm_device *dev)
 		info->num_pipes = 0;
 	} else if (info->num_pipes > 0 &&
 		   (INTEL_INFO(dev)->gen == 7 || INTEL_INFO(dev)->gen == 8) &&
-		   !IS_VALLEYVIEW(dev)) {
+		   HAS_PCH_SPLIT(dev)) {
 		u32 fuse_strap = I915_READ(FUSE_STRAP);
 		u32 sfuse_strap = I915_READ(SFUSE_STRAP);
 
@@ -834,9 +842,6 @@ static void intel_device_info_runtime_init(struct drm_device *dev)
 
 static void intel_init_dpio(struct drm_i915_private *dev_priv)
 {
-	if (!IS_VALLEYVIEW(dev_priv))
-		return;
-
 	/*
 	 * IOSF_PORT_DPIO is used for VLV x2 PHY (DP/HDMI B and C),
 	 * CHV x1 PHY (DP/HDMI D)
@@ -845,7 +850,7 @@ static void intel_init_dpio(struct drm_i915_private *dev_priv)
 	if (IS_CHERRYVIEW(dev_priv)) {
 		DPIO_PHY_IOSF_PORT(DPIO_PHY0) = IOSF_PORT_DPIO_2;
 		DPIO_PHY_IOSF_PORT(DPIO_PHY1) = IOSF_PORT_DPIO;
-	} else {
+	} else if (IS_VALLEYVIEW(dev_priv)) {
 		DPIO_PHY_IOSF_PORT(DPIO_PHY0) = IOSF_PORT_DPIO;
 	}
 }
@@ -890,11 +895,12 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	spin_lock_init(&dev_priv->mmio_flip_lock);
 	mutex_init(&dev_priv->sb_lock);
 	mutex_init(&dev_priv->modeset_restore_lock);
-	mutex_init(&dev_priv->csr_lock);
 	mutex_init(&dev_priv->av_mutex);
 
 	intel_pm_setup(dev);
 
+	intel_runtime_pm_get(dev_priv);
+
 	intel_display_crc_init(dev);
 
 	i915_dump_device_info(dev_priv);
@@ -937,9 +943,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	intel_uncore_init(dev);
 
-	/* Load CSR Firmware for SKL */
-	intel_csr_ucode_init(dev);
-
 	ret = i915_gem_gtt_init(dev);
 	if (ret)
 		goto out_freecsr;
@@ -1028,7 +1031,6 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	/* Try to make sure MCHBAR is enabled before poking at it */
 	intel_setup_mchbar(dev);
-	intel_setup_gmbus(dev);
 	intel_opregion_setup(dev);
 
 	i915_gem_load(dev);
@@ -1087,6 +1089,8 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 
 	i915_audio_component_init(dev_priv);
 
+	intel_runtime_pm_put(dev_priv);
+
 	return 0;
 
 out_power_well:
@@ -1099,7 +1103,6 @@ out_gem_unload:
 	if (dev->pdev->msi_enabled)
 		pci_disable_msi(dev->pdev);
 
-	intel_teardown_gmbus(dev);
 	intel_teardown_mchbar(dev);
 	pm_qos_remove_request(&dev_priv->pm_qos);
 	destroy_workqueue(dev_priv->gpu_error.hangcheck_wq);
@@ -1113,7 +1116,7 @@ out_mtrrfree:
 out_gtt:
 	i915_global_gtt_cleanup(dev);
 out_freecsr:
-	intel_csr_ucode_fini(dev);
+	intel_csr_ucode_fini(dev_priv);
 	intel_uncore_fini(dev);
 	pci_iounmap(dev->pdev, dev_priv->regs);
 put_bridge:
@@ -1122,6 +1125,9 @@ free_priv:
 	kmem_cache_destroy(dev_priv->requests);
 	kmem_cache_destroy(dev_priv->vmas);
 	kmem_cache_destroy(dev_priv->objects);
+
+	intel_runtime_pm_put(dev_priv);
+
 	kfree(dev_priv);
 	return ret;
 }
@@ -1131,6 +1137,8 @@ int i915_driver_unload(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
+	intel_fbdev_fini(dev);
+
 	i915_audio_component_cleanup(dev_priv);
 
 	ret = i915_gem_suspend(dev);
@@ -1153,8 +1161,6 @@ int i915_driver_unload(struct drm_device *dev)
 
 	acpi_video_unregister();
 
-	intel_fbdev_fini(dev);
-
 	drm_vblank_cleanup(dev);
 
 	intel_modeset_cleanup(dev);
@@ -1196,9 +1202,8 @@ int i915_driver_unload(struct drm_device *dev)
 	intel_fbc_cleanup_cfb(dev_priv);
 	i915_gem_cleanup_stolen(dev);
 
-	intel_csr_ucode_fini(dev);
+	intel_csr_ucode_fini(dev_priv);
 
-	intel_teardown_gmbus(dev);
 	intel_teardown_mchbar(dev);
 
 	destroy_workqueue(dev_priv->hotplug.dp_wq);
@@ -1264,8 +1269,6 @@ void i915_driver_postclose(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_i915_file_private *file_priv = file->driver_priv;
 
-	if (file_priv && file_priv->bsd_ring)
-		file_priv->bsd_ring = NULL;
 	kfree(file_priv);
 }
 
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index 760e0ce4aa26..f357058c74d9 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -228,161 +228,111 @@ static const struct intel_device_info intel_sandybridge_m_info = {
 	.need_gfx_hws = 1, .has_hotplug = 1, \
 	.has_fbc = 1, \
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING, \
-	.has_llc = 1
+	.has_llc = 1, \
+	GEN_DEFAULT_PIPEOFFSETS, \
+	IVB_CURSOR_OFFSETS
 
 static const struct intel_device_info intel_ivybridge_d_info = {
 	GEN7_FEATURES,
 	.is_ivybridge = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_ivybridge_m_info = {
 	GEN7_FEATURES,
 	.is_ivybridge = 1,
 	.is_mobile = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_ivybridge_q_info = {
 	GEN7_FEATURES,
 	.is_ivybridge = 1,
 	.num_pipes = 0, /* legal, last one wins */
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
+#define VLV_FEATURES  \
+	.gen = 7, .num_pipes = 2, \
+	.need_gfx_hws = 1, .has_hotplug = 1, \
+	.ring_mask = RENDER_RING | BSD_RING | BLT_RING, \
+	.display_mmio_offset = VLV_DISPLAY_BASE, \
+	GEN_DEFAULT_PIPEOFFSETS, \
+	CURSOR_OFFSETS
+
 static const struct intel_device_info intel_valleyview_m_info = {
-	GEN7_FEATURES,
-	.is_mobile = 1,
-	.num_pipes = 2,
+	VLV_FEATURES,
 	.is_valleyview = 1,
-	.display_mmio_offset = VLV_DISPLAY_BASE,
-	.has_fbc = 0, /* legal, last one wins */
-	.has_llc = 0, /* legal, last one wins */
-	GEN_DEFAULT_PIPEOFFSETS,
-	CURSOR_OFFSETS,
+	.is_mobile = 1,
 };
 
 static const struct intel_device_info intel_valleyview_d_info = {
-	GEN7_FEATURES,
-	.num_pipes = 2,
+	VLV_FEATURES,
 	.is_valleyview = 1,
-	.display_mmio_offset = VLV_DISPLAY_BASE,
-	.has_fbc = 0, /* legal, last one wins */
-	.has_llc = 0, /* legal, last one wins */
-	GEN_DEFAULT_PIPEOFFSETS,
-	CURSOR_OFFSETS,
 };
 
+#define HSW_FEATURES  \
+	GEN7_FEATURES, \
+	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING, \
+	.has_ddi = 1, \
+	.has_fpga_dbg = 1
+
 static const struct intel_device_info intel_haswell_d_info = {
-	GEN7_FEATURES,
+	HSW_FEATURES,
 	.is_haswell = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_haswell_m_info = {
-	GEN7_FEATURES,
+	HSW_FEATURES,
 	.is_haswell = 1,
 	.is_mobile = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_broadwell_d_info = {
-	.gen = 8, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
+	HSW_FEATURES,
+	.gen = 8,
 };
 
 static const struct intel_device_info intel_broadwell_m_info = {
-	.gen = 8, .is_mobile = 1, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
+	HSW_FEATURES,
+	.gen = 8, .is_mobile = 1,
 };
 
 static const struct intel_device_info intel_broadwell_gt3d_info = {
-	.gen = 8, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
+	HSW_FEATURES,
+	.gen = 8,
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING | BSD2_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_broadwell_gt3m_info = {
-	.gen = 8, .is_mobile = 1, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
+	HSW_FEATURES,
+	.gen = 8, .is_mobile = 1,
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING | BSD2_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_cherryview_info = {
 	.gen = 8, .num_pipes = 3,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	.is_valleyview = 1,
+	.is_cherryview = 1,
 	.display_mmio_offset = VLV_DISPLAY_BASE,
 	GEN_CHV_PIPEOFFSETS,
 	CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_skylake_info = {
+	HSW_FEATURES,
 	.is_skylake = 1,
-	.gen = 9, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
-	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
+	.gen = 9,
 };
 
 static const struct intel_device_info intel_skylake_gt3_info = {
+	HSW_FEATURES,
 	.is_skylake = 1,
-	.gen = 9, .num_pipes = 3,
-	.need_gfx_hws = 1, .has_hotplug = 1,
+	.gen = 9,
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING | BSD2_RING,
-	.has_llc = 1,
-	.has_ddi = 1,
-	.has_fpga_dbg = 1,
-	.has_fbc = 1,
-	GEN_DEFAULT_PIPEOFFSETS,
-	IVB_CURSOR_OFFSETS,
 };
 
 static const struct intel_device_info intel_broxton_info = {
 	.is_preliminary = 1,
+	.is_broxton = 1,
 	.gen = 9,
 	.need_gfx_hws = 1, .has_hotplug = 1,
 	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING,
@@ -394,50 +344,67 @@ static const struct intel_device_info intel_broxton_info = {
 	IVB_CURSOR_OFFSETS,
 };
 
+static const struct intel_device_info intel_kabylake_info = {
+	HSW_FEATURES,
+	.is_preliminary = 1,
+	.is_kabylake = 1,
+	.gen = 9,
+};
+
+static const struct intel_device_info intel_kabylake_gt3_info = {
+	HSW_FEATURES,
+	.is_preliminary = 1,
+	.is_kabylake = 1,
+	.gen = 9,
+	.ring_mask = RENDER_RING | BSD_RING | BLT_RING | VEBOX_RING | BSD2_RING,
+};
+
 /*
  * Make sure any device matches here are from most specific to most
  * general.  For example, since the Quanta match is based on the subsystem
  * and subvendor IDs, we need it to come before the more general IVB
  * PCI ID matches, otherwise we'll use the wrong info struct above.
  */
-#define INTEL_PCI_IDS \
-	INTEL_I830_IDS(&intel_i830_info),	\
-	INTEL_I845G_IDS(&intel_845g_info),	\
-	INTEL_I85X_IDS(&intel_i85x_info),	\
-	INTEL_I865G_IDS(&intel_i865g_info),	\
-	INTEL_I915G_IDS(&intel_i915g_info),	\
-	INTEL_I915GM_IDS(&intel_i915gm_info),	\
-	INTEL_I945G_IDS(&intel_i945g_info),	\
-	INTEL_I945GM_IDS(&intel_i945gm_info),	\
-	INTEL_I965G_IDS(&intel_i965g_info),	\
-	INTEL_G33_IDS(&intel_g33_info),		\
-	INTEL_I965GM_IDS(&intel_i965gm_info),	\
-	INTEL_GM45_IDS(&intel_gm45_info), 	\
-	INTEL_G45_IDS(&intel_g45_info), 	\
-	INTEL_PINEVIEW_IDS(&intel_pineview_info),	\
-	INTEL_IRONLAKE_D_IDS(&intel_ironlake_d_info),	\
-	INTEL_IRONLAKE_M_IDS(&intel_ironlake_m_info),	\
-	INTEL_SNB_D_IDS(&intel_sandybridge_d_info),	\
-	INTEL_SNB_M_IDS(&intel_sandybridge_m_info),	\
-	INTEL_IVB_Q_IDS(&intel_ivybridge_q_info), /* must be first IVB */ \
-	INTEL_IVB_M_IDS(&intel_ivybridge_m_info),	\
-	INTEL_IVB_D_IDS(&intel_ivybridge_d_info),	\
-	INTEL_HSW_D_IDS(&intel_haswell_d_info), \
-	INTEL_HSW_M_IDS(&intel_haswell_m_info), \
-	INTEL_VLV_M_IDS(&intel_valleyview_m_info),	\
-	INTEL_VLV_D_IDS(&intel_valleyview_d_info),	\
-	INTEL_BDW_GT12M_IDS(&intel_broadwell_m_info),	\
-	INTEL_BDW_GT12D_IDS(&intel_broadwell_d_info),	\
-	INTEL_BDW_GT3M_IDS(&intel_broadwell_gt3m_info),	\
-	INTEL_BDW_GT3D_IDS(&intel_broadwell_gt3d_info), \
-	INTEL_CHV_IDS(&intel_cherryview_info),	\
-	INTEL_SKL_GT1_IDS(&intel_skylake_info),	\
-	INTEL_SKL_GT2_IDS(&intel_skylake_info),	\
-	INTEL_SKL_GT3_IDS(&intel_skylake_gt3_info),	\
-	INTEL_BXT_IDS(&intel_broxton_info)
-
-static const struct pci_device_id pciidlist[] = {		/* aka */
-	INTEL_PCI_IDS,
+static const struct pci_device_id pciidlist[] = {
+	INTEL_I830_IDS(&intel_i830_info),
+	INTEL_I845G_IDS(&intel_845g_info),
+	INTEL_I85X_IDS(&intel_i85x_info),
+	INTEL_I865G_IDS(&intel_i865g_info),
+	INTEL_I915G_IDS(&intel_i915g_info),
+	INTEL_I915GM_IDS(&intel_i915gm_info),
+	INTEL_I945G_IDS(&intel_i945g_info),
+	INTEL_I945GM_IDS(&intel_i945gm_info),
+	INTEL_I965G_IDS(&intel_i965g_info),
+	INTEL_G33_IDS(&intel_g33_info),
+	INTEL_I965GM_IDS(&intel_i965gm_info),
+	INTEL_GM45_IDS(&intel_gm45_info),
+	INTEL_G45_IDS(&intel_g45_info),
+	INTEL_PINEVIEW_IDS(&intel_pineview_info),
+	INTEL_IRONLAKE_D_IDS(&intel_ironlake_d_info),
+	INTEL_IRONLAKE_M_IDS(&intel_ironlake_m_info),
+	INTEL_SNB_D_IDS(&intel_sandybridge_d_info),
+	INTEL_SNB_M_IDS(&intel_sandybridge_m_info),
+	INTEL_IVB_Q_IDS(&intel_ivybridge_q_info), /* must be first IVB */
+	INTEL_IVB_M_IDS(&intel_ivybridge_m_info),
+	INTEL_IVB_D_IDS(&intel_ivybridge_d_info),
+	INTEL_HSW_D_IDS(&intel_haswell_d_info),
+	INTEL_HSW_M_IDS(&intel_haswell_m_info),
+	INTEL_VLV_M_IDS(&intel_valleyview_m_info),
+	INTEL_VLV_D_IDS(&intel_valleyview_d_info),
+	INTEL_BDW_GT12M_IDS(&intel_broadwell_m_info),
+	INTEL_BDW_GT12D_IDS(&intel_broadwell_d_info),
+	INTEL_BDW_GT3M_IDS(&intel_broadwell_gt3m_info),
+	INTEL_BDW_GT3D_IDS(&intel_broadwell_gt3d_info),
+	INTEL_CHV_IDS(&intel_cherryview_info),
+	INTEL_SKL_GT1_IDS(&intel_skylake_info),
+	INTEL_SKL_GT2_IDS(&intel_skylake_info),
+	INTEL_SKL_GT3_IDS(&intel_skylake_gt3_info),
+	INTEL_SKL_GT4_IDS(&intel_skylake_gt3_info),
+	INTEL_BXT_IDS(&intel_broxton_info),
+	INTEL_KBL_GT1_IDS(&intel_kabylake_info),
+	INTEL_KBL_GT2_IDS(&intel_kabylake_info),
+	INTEL_KBL_GT3_IDS(&intel_kabylake_gt3_info),
+	INTEL_KBL_GT4_IDS(&intel_kabylake_gt3_info),
 	{0, 0, 0}
 };
 
@@ -463,7 +430,7 @@ static enum intel_pch intel_virt_detect_pch(struct drm_device *dev)
 	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		ret = PCH_LPT;
 		DRM_DEBUG_KMS("Assuming LynxPoint PCH\n");
-	} else if (IS_SKYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		ret = PCH_SPT;
 		DRM_DEBUG_KMS("Assuming SunrisePoint PCH\n");
 	}
@@ -526,12 +493,17 @@ void intel_detect_pch(struct drm_device *dev)
 			} else if (id == INTEL_PCH_SPT_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_SPT;
 				DRM_DEBUG_KMS("Found SunrisePoint PCH\n");
-				WARN_ON(!IS_SKYLAKE(dev));
+				WARN_ON(!IS_SKYLAKE(dev) &&
+					!IS_KABYLAKE(dev));
 			} else if (id == INTEL_PCH_SPT_LP_DEVICE_ID_TYPE) {
 				dev_priv->pch_type = PCH_SPT;
 				DRM_DEBUG_KMS("Found SunrisePoint LP PCH\n");
-				WARN_ON(!IS_SKYLAKE(dev));
-			} else if (id == INTEL_PCH_P2X_DEVICE_ID_TYPE) {
+				WARN_ON(!IS_SKYLAKE(dev) &&
+					!IS_KABYLAKE(dev));
+			} else if ((id == INTEL_PCH_P2X_DEVICE_ID_TYPE) ||
+				   ((id == INTEL_PCH_QEMU_DEVICE_ID_TYPE) &&
+				    pch->subsystem_vendor == 0x1af4 &&
+				    pch->subsystem_device == 0x1100)) {
 				dev_priv->pch_type = intel_virt_detect_pch(dev);
 			} else
 				continue;
@@ -570,47 +542,31 @@ bool i915_semaphore_is_enabled(struct drm_device *dev)
 	return true;
 }
 
-void i915_firmware_load_error_print(const char *fw_path, int err)
-{
-	DRM_ERROR("failed to load firmware %s (%d)\n", fw_path, err);
-
-	/*
-	 * If the reason is not known assume -ENOENT since that's the most
-	 * usual failure mode.
-	 */
-	if (!err)
-		err = -ENOENT;
-
-	if (!(IS_BUILTIN(CONFIG_DRM_I915) && err == -ENOENT))
-		return;
-
-	DRM_ERROR(
-	  "The driver is built-in, so to load the firmware you need to\n"
-	  "include it either in the kernel (see CONFIG_EXTRA_FIRMWARE) or\n"
-	  "in your initrd/initramfs image.\n");
-}
-
 static void intel_suspend_encoders(struct drm_i915_private *dev_priv)
 {
 	struct drm_device *dev = dev_priv->dev;
-	struct drm_encoder *encoder;
+	struct intel_encoder *encoder;
 
 	drm_modeset_lock_all(dev);
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
-
-		if (intel_encoder->suspend)
-			intel_encoder->suspend(intel_encoder);
-	}
+	for_each_intel_encoder(dev, encoder)
+		if (encoder->suspend)
+			encoder->suspend(encoder);
 	drm_modeset_unlock_all(dev);
 }
 
 static int intel_suspend_complete(struct drm_i915_private *dev_priv);
 static int vlv_resume_prepare(struct drm_i915_private *dev_priv,
 			      bool rpm_resume);
-static int skl_resume_prepare(struct drm_i915_private *dev_priv);
 static int bxt_resume_prepare(struct drm_i915_private *dev_priv);
 
+static bool suspend_to_idle(struct drm_i915_private *dev_priv)
+{
+#if IS_ENABLED(CONFIG_ACPI_SLEEP)
+	if (acpi_target_system_state() < ACPI_STATE_S3)
+		return true;
+#endif
+	return false;
+}
 
 static int i915_drm_suspend(struct drm_device *dev)
 {
@@ -623,6 +579,8 @@ static int i915_drm_suspend(struct drm_device *dev)
 	dev_priv->modeset_restore = MODESET_SUSPENDED;
 	mutex_unlock(&dev_priv->modeset_restore_lock);
 
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	/* We do a lot of poking in a lot of registers, make sure they work
 	 * properly. */
 	intel_display_set_init_power(dev_priv, true);
@@ -635,7 +593,7 @@ static int i915_drm_suspend(struct drm_device *dev)
 	if (error) {
 		dev_err(&dev->pdev->dev,
 			"GEM idle failed, resume might fail\n");
-		return error;
+		goto out;
 	}
 
 	intel_guc_suspend(dev);
@@ -663,11 +621,7 @@ static int i915_drm_suspend(struct drm_device *dev)
 
 	i915_save_state(dev);
 
-	opregion_target_state = PCI_D3cold;
-#if IS_ENABLED(CONFIG_ACPI_SLEEP)
-	if (acpi_target_system_state() < ACPI_STATE_S3)
-		opregion_target_state = PCI_D1;
-#endif
+	opregion_target_state = suspend_to_idle(dev_priv) ? PCI_D1 : PCI_D3cold;
 	intel_opregion_notify_adapter(dev, opregion_target_state);
 
 	intel_uncore_forcewake_reset(dev, false);
@@ -679,20 +633,42 @@ static int i915_drm_suspend(struct drm_device *dev)
 
 	intel_display_set_init_power(dev_priv, false);
 
-	return 0;
+	if (HAS_CSR(dev_priv))
+		flush_work(&dev_priv->csr.work);
+
+out:
+	enable_rpm_wakeref_asserts(dev_priv);
+
+	return error;
 }
 
 static int i915_drm_suspend_late(struct drm_device *drm_dev, bool hibernation)
 {
 	struct drm_i915_private *dev_priv = drm_dev->dev_private;
+	bool fw_csr;
 	int ret;
 
+	disable_rpm_wakeref_asserts(dev_priv);
+
+	fw_csr = suspend_to_idle(dev_priv) && dev_priv->csr.dmc_payload;
+	/*
+	 * In case of firmware assisted context save/restore don't manually
+	 * deinit the power domains. This also means the CSR/DMC firmware will
+	 * stay active, it will power down any HW resources as required and
+	 * also enable deeper system power states that would be blocked if the
+	 * firmware was inactive.
+	 */
+	if (!fw_csr)
+		intel_power_domains_suspend(dev_priv);
+
 	ret = intel_suspend_complete(dev_priv);
 
 	if (ret) {
 		DRM_ERROR("Suspend complete failed: %d\n", ret);
+		if (!fw_csr)
+			intel_power_domains_init_hw(dev_priv, true);
 
-		return ret;
+		goto out;
 	}
 
 	pci_disable_device(drm_dev->pdev);
@@ -711,7 +687,12 @@ static int i915_drm_suspend_late(struct drm_device *drm_dev, bool hibernation)
 	if (!(hibernation && INTEL_INFO(dev_priv)->gen < 6))
 		pci_set_power_state(drm_dev->pdev, PCI_D3hot);
 
-	return 0;
+	dev_priv->suspended_to_idle = suspend_to_idle(dev_priv);
+
+out:
+	enable_rpm_wakeref_asserts(dev_priv);
+
+	return ret;
 }
 
 int i915_suspend_switcheroo(struct drm_device *dev, pm_message_t state)
@@ -742,6 +723,8 @@ static int i915_drm_resume(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	mutex_lock(&dev->struct_mutex);
 	i915_gem_restore_gtt_mappings(dev);
 	mutex_unlock(&dev->struct_mutex);
@@ -806,6 +789,8 @@ static int i915_drm_resume(struct drm_device *dev)
 
 	drm_kms_helper_poll_enable(dev);
 
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return 0;
 }
 
@@ -823,12 +808,16 @@ static int i915_drm_resume_early(struct drm_device *dev)
 	 * FIXME: This should be solved with a special hdmi sink device or
 	 * similar so that power domains can be employed.
 	 */
-	if (pci_enable_device(dev->pdev))
-		return -EIO;
+	if (pci_enable_device(dev->pdev)) {
+		ret = -EIO;
+		goto out;
+	}
 
 	pci_set_master(dev->pdev);
 
-	if (IS_VALLEYVIEW(dev_priv))
+	disable_rpm_wakeref_asserts(dev_priv);
+
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		ret = vlv_resume_prepare(dev_priv, false);
 	if (ret)
 		DRM_ERROR("Resume prepare failed: %d, continuing anyway\n",
@@ -838,13 +827,18 @@ static int i915_drm_resume_early(struct drm_device *dev)
 
 	if (IS_BROXTON(dev))
 		ret = bxt_resume_prepare(dev_priv);
-	else if (IS_SKYLAKE(dev_priv))
-		ret = skl_resume_prepare(dev_priv);
 	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		hsw_disable_pc8(dev_priv);
 
 	intel_uncore_sanitize(dev);
-	intel_power_domains_init_hw(dev_priv);
+
+	if (!(dev_priv->suspended_to_idle && dev_priv->csr.dmc_payload))
+		intel_power_domains_init_hw(dev_priv, true);
+
+out:
+	dev_priv->suspended_to_idle = false;
+
+	enable_rpm_wakeref_asserts(dev_priv);
 
 	return ret;
 }
@@ -1051,15 +1045,6 @@ static int i915_pm_resume(struct device *dev)
 	return i915_drm_resume(drm_dev);
 }
 
-static int skl_suspend_complete(struct drm_i915_private *dev_priv)
-{
-	/* Enabling DC6 is not a hard requirement to enter runtime D3 */
-
-	skl_uninit_cdclk(dev_priv);
-
-	return 0;
-}
-
 static int hsw_suspend_complete(struct drm_i915_private *dev_priv)
 {
 	hsw_enable_pc8(dev_priv);
@@ -1099,16 +1084,6 @@ static int bxt_resume_prepare(struct drm_i915_private *dev_priv)
 	return 0;
 }
 
-static int skl_resume_prepare(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-
-	skl_init_cdclk(dev_priv);
-	intel_csr_load_program(dev);
-
-	return 0;
-}
-
 /*
  * Save all Gunit registers that may be lost after a D3 and a subsequent
  * S0i[R123] transition. The list of registers needing a save/restore is
@@ -1497,6 +1472,9 @@ static int intel_runtime_suspend(struct device *device)
 
 		return -EAGAIN;
 	}
+
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	/*
 	 * We are safe here against re-faults, since the fault handler takes
 	 * an RPM reference.
@@ -1504,6 +1482,8 @@ static int intel_runtime_suspend(struct device *device)
 	i915_gem_release_all_mmaps(dev_priv);
 	mutex_unlock(&dev->struct_mutex);
 
+	cancel_delayed_work_sync(&dev_priv->gpu_error.hangcheck_work);
+
 	intel_guc_suspend(dev);
 
 	intel_suspend_gt_powersave(dev);
@@ -1514,11 +1494,15 @@ static int intel_runtime_suspend(struct device *device)
 		DRM_ERROR("Runtime suspend failed, disabling it (%d)\n", ret);
 		intel_runtime_pm_enable_interrupts(dev_priv);
 
+		enable_rpm_wakeref_asserts(dev_priv);
+
 		return ret;
 	}
 
-	cancel_delayed_work_sync(&dev_priv->gpu_error.hangcheck_work);
 	intel_uncore_forcewake_reset(dev, false);
+
+	enable_rpm_wakeref_asserts(dev_priv);
+	WARN_ON_ONCE(atomic_read(&dev_priv->pm.wakeref_count));
 	dev_priv->pm.suspended = true;
 
 	/*
@@ -1562,6 +1546,9 @@ static int intel_runtime_resume(struct device *device)
 
 	DRM_DEBUG_KMS("Resuming device\n");
 
+	WARN_ON_ONCE(atomic_read(&dev_priv->pm.wakeref_count));
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	intel_opregion_notify_adapter(dev, PCI_D0);
 	dev_priv->pm.suspended = false;
 
@@ -1572,11 +1559,9 @@ static int intel_runtime_resume(struct device *device)
 
 	if (IS_BROXTON(dev))
 		ret = bxt_resume_prepare(dev_priv);
-	else if (IS_SKYLAKE(dev))
-		ret = skl_resume_prepare(dev_priv);
 	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		hsw_disable_pc8(dev_priv);
-	else if (IS_VALLEYVIEW(dev_priv))
+	else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		ret = vlv_resume_prepare(dev_priv, true);
 
 	/*
@@ -1593,11 +1578,13 @@ static int intel_runtime_resume(struct device *device)
 	 * power well, so hpd is reinitialized from there. For
 	 * everyone else do it here.
 	 */
-	if (!IS_VALLEYVIEW(dev_priv))
+	if (!IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv))
 		intel_hpd_init(dev_priv);
 
 	intel_enable_gt_powersave(dev);
 
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	if (ret)
 		DRM_ERROR("Runtime resume failed, disabling it (%d)\n", ret);
 	else
@@ -1616,11 +1603,9 @@ static int intel_suspend_complete(struct drm_i915_private *dev_priv)
 
 	if (IS_BROXTON(dev_priv))
 		ret = bxt_suspend_complete(dev_priv);
-	else if (IS_SKYLAKE(dev_priv))
-		ret = skl_suspend_complete(dev_priv);
 	else if (IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv))
 		ret = hsw_suspend_complete(dev_priv);
-	else if (IS_VALLEYVIEW(dev_priv))
+	else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		ret = vlv_suspend_complete(dev_priv);
 	else
 		ret = 0;
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index f4af19a0d569..b0847b915545 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -33,6 +33,7 @@
 #include <uapi/drm/i915_drm.h>
 #include <uapi/drm/drm_fourcc.h>
 
+#include <drm/drmP.h>
 #include "i915_reg.h"
 #include "intel_bios.h"
 #include "intel_ringbuffer.h"
@@ -57,7 +58,7 @@
 
 #define DRIVER_NAME		"i915"
 #define DRIVER_DESC		"Intel Graphics"
-#define DRIVER_DATE		"20151010"
+#define DRIVER_DATE		"20151218"
 
 #undef WARN_ON
 /* Many gcc seem to no see through this and fall over :( */
@@ -180,15 +181,11 @@ enum intel_display_power_domain {
 	POWER_DOMAIN_TRANSCODER_B,
 	POWER_DOMAIN_TRANSCODER_C,
 	POWER_DOMAIN_TRANSCODER_EDP,
-	POWER_DOMAIN_PORT_DDI_A_2_LANES,
-	POWER_DOMAIN_PORT_DDI_A_4_LANES,
-	POWER_DOMAIN_PORT_DDI_B_2_LANES,
-	POWER_DOMAIN_PORT_DDI_B_4_LANES,
-	POWER_DOMAIN_PORT_DDI_C_2_LANES,
-	POWER_DOMAIN_PORT_DDI_C_4_LANES,
-	POWER_DOMAIN_PORT_DDI_D_2_LANES,
-	POWER_DOMAIN_PORT_DDI_D_4_LANES,
-	POWER_DOMAIN_PORT_DDI_E_2_LANES,
+	POWER_DOMAIN_PORT_DDI_A_LANES,
+	POWER_DOMAIN_PORT_DDI_B_LANES,
+	POWER_DOMAIN_PORT_DDI_C_LANES,
+	POWER_DOMAIN_PORT_DDI_D_LANES,
+	POWER_DOMAIN_PORT_DDI_E_LANES,
 	POWER_DOMAIN_PORT_DSI,
 	POWER_DOMAIN_PORT_CRT,
 	POWER_DOMAIN_PORT_OTHER,
@@ -200,6 +197,7 @@ enum intel_display_power_domain {
 	POWER_DOMAIN_AUX_C,
 	POWER_DOMAIN_AUX_D,
 	POWER_DOMAIN_GMBUS,
+	POWER_DOMAIN_MODESET,
 	POWER_DOMAIN_INIT,
 
 	POWER_DOMAIN_NUM,
@@ -289,7 +287,7 @@ struct i915_hotplug {
 	list_for_each_entry(intel_plane,				\
 			    &(dev)->mode_config.plane_list,		\
 			    base.head)					\
-		if ((intel_plane)->pipe == (intel_crtc)->pipe)
+		for_each_if ((intel_plane)->pipe == (intel_crtc)->pipe)
 
 #define for_each_intel_crtc(dev, intel_crtc) \
 	list_for_each_entry(intel_crtc, &dev->mode_config.crtc_list, base.head)
@@ -306,15 +304,15 @@ struct i915_hotplug {
 
 #define for_each_encoder_on_crtc(dev, __crtc, intel_encoder) \
 	list_for_each_entry((intel_encoder), &(dev)->mode_config.encoder_list, base.head) \
-		if ((intel_encoder)->base.crtc == (__crtc))
+		for_each_if ((intel_encoder)->base.crtc == (__crtc))
 
 #define for_each_connector_on_encoder(dev, __encoder, intel_connector) \
 	list_for_each_entry((intel_connector), &(dev)->mode_config.connector_list, base.head) \
-		if ((intel_connector)->base.encoder == (__encoder))
+		for_each_if ((intel_connector)->base.encoder == (__encoder))
 
 #define for_each_power_domain(domain, mask)				\
 	for ((domain) = 0; (domain) < POWER_DOMAIN_NUM; (domain)++)	\
-		if ((1 << (domain)) & (mask))
+		for_each_if ((1 << (domain)) & (mask))
 
 struct drm_i915_private;
 struct i915_mm_struct;
@@ -460,7 +458,9 @@ struct intel_opregion {
 	u32 swsci_gbda_sub_functions;
 	u32 swsci_sbcb_sub_functions;
 	struct opregion_asle *asle;
-	void *vbt;
+	void *rvda;
+	const void *vbt;
+	u32 vbt_size;
 	u32 *lid_state;
 	struct work_struct asle_work;
 };
@@ -631,11 +631,9 @@ struct drm_i915_display_funcs {
 			  int target, int refclk,
 			  struct dpll *match_clock,
 			  struct dpll *best_clock);
+	int (*compute_pipe_wm)(struct intel_crtc *crtc,
+			       struct drm_atomic_state *state);
 	void (*update_wm)(struct drm_crtc *crtc);
-	void (*update_sprite_wm)(struct drm_plane *plane,
-				 struct drm_crtc *crtc,
-				 uint32_t sprite_width, uint32_t sprite_height,
-				 int pixel_size, bool enable, bool scaled);
 	int (*modeset_calc_cdclk)(struct drm_atomic_state *state);
 	void (*modeset_commit_cdclk)(struct drm_atomic_state *state);
 	/* Returns the active state of the crtc, and if the crtc is active,
@@ -693,18 +691,18 @@ struct intel_uncore_funcs {
 	void (*force_wake_put)(struct drm_i915_private *dev_priv,
 							enum forcewake_domains domains);
 
-	uint8_t  (*mmio_readb)(struct drm_i915_private *dev_priv, off_t offset, bool trace);
-	uint16_t (*mmio_readw)(struct drm_i915_private *dev_priv, off_t offset, bool trace);
-	uint32_t (*mmio_readl)(struct drm_i915_private *dev_priv, off_t offset, bool trace);
-	uint64_t (*mmio_readq)(struct drm_i915_private *dev_priv, off_t offset, bool trace);
+	uint8_t  (*mmio_readb)(struct drm_i915_private *dev_priv, i915_reg_t r, bool trace);
+	uint16_t (*mmio_readw)(struct drm_i915_private *dev_priv, i915_reg_t r, bool trace);
+	uint32_t (*mmio_readl)(struct drm_i915_private *dev_priv, i915_reg_t r, bool trace);
+	uint64_t (*mmio_readq)(struct drm_i915_private *dev_priv, i915_reg_t r, bool trace);
 
-	void (*mmio_writeb)(struct drm_i915_private *dev_priv, off_t offset,
+	void (*mmio_writeb)(struct drm_i915_private *dev_priv, i915_reg_t r,
 				uint8_t val, bool trace);
-	void (*mmio_writew)(struct drm_i915_private *dev_priv, off_t offset,
+	void (*mmio_writew)(struct drm_i915_private *dev_priv, i915_reg_t r,
 				uint16_t val, bool trace);
-	void (*mmio_writel)(struct drm_i915_private *dev_priv, off_t offset,
+	void (*mmio_writel)(struct drm_i915_private *dev_priv, i915_reg_t r,
 				uint32_t val, bool trace);
-	void (*mmio_writeq)(struct drm_i915_private *dev_priv, off_t offset,
+	void (*mmio_writeq)(struct drm_i915_private *dev_priv, i915_reg_t r,
 				uint64_t val, bool trace);
 };
 
@@ -721,11 +719,11 @@ struct intel_uncore {
 		enum forcewake_domain_id id;
 		unsigned wake_count;
 		struct timer_list timer;
-		u32 reg_set;
+		i915_reg_t reg_set;
 		u32 val_set;
 		u32 val_clear;
-		u32 reg_ack;
-		u32 reg_post;
+		i915_reg_t reg_ack;
+		i915_reg_t reg_post;
 		u32 val_reset;
 	} fw_domain[FW_DOMAIN_ID_COUNT];
 };
@@ -735,25 +733,25 @@ struct intel_uncore {
 	for ((i__) = 0, (domain__) = &(dev_priv__)->uncore.fw_domain[0]; \
 	     (i__) < FW_DOMAIN_ID_COUNT; \
 	     (i__)++, (domain__) = &(dev_priv__)->uncore.fw_domain[i__]) \
-		if (((mask__) & (dev_priv__)->uncore.fw_domains) & (1 << (i__)))
+		for_each_if (((mask__) & (dev_priv__)->uncore.fw_domains) & (1 << (i__)))
 
 #define for_each_fw_domain(domain__, dev_priv__, i__) \
 	for_each_fw_domain_mask(domain__, FORCEWAKE_ALL, dev_priv__, i__)
 
-enum csr_state {
-	FW_UNINITIALIZED = 0,
-	FW_LOADED,
-	FW_FAILED
-};
+#define CSR_VERSION(major, minor)	((major) << 16 | (minor))
+#define CSR_VERSION_MAJOR(version)	((version) >> 16)
+#define CSR_VERSION_MINOR(version)	((version) & 0xffff)
 
 struct intel_csr {
+	struct work_struct work;
 	const char *fw_path;
 	uint32_t *dmc_payload;
 	uint32_t dmc_fw_size;
+	uint32_t version;
 	uint32_t mmio_count;
-	uint32_t mmioaddr[8];
+	i915_reg_t mmioaddr[8];
 	uint32_t mmiodata[8];
-	enum csr_state state;
+	uint32_t dc_state;
 };
 
 #define DEV_INFO_FOR_EACH_FLAG(func, sep) \
@@ -769,8 +767,11 @@ struct intel_csr {
 	func(is_crestline) sep \
 	func(is_ivybridge) sep \
 	func(is_valleyview) sep \
+	func(is_cherryview) sep \
 	func(is_haswell) sep \
 	func(is_skylake) sep \
+	func(is_broxton) sep \
+	func(is_kabylake) sep \
 	func(is_preliminary) sep \
 	func(has_fbc) sep \
 	func(has_pipe_cxsr) sep \
@@ -906,7 +907,6 @@ struct i915_fbc {
 	/* This is always the inner lock when overlapping with struct_mutex and
 	 * it's the outer lock when overlapping with stolen_lock. */
 	struct mutex lock;
-	unsigned long uncompressed_size;
 	unsigned threshold;
 	unsigned int fb_id;
 	unsigned int possible_framebuffer_bits;
@@ -919,38 +919,21 @@ struct i915_fbc {
 
 	bool false_color;
 
-	/* Tracks whether the HW is actually enabled, not whether the feature is
-	 * possible. */
 	bool enabled;
+	bool active;
 
 	struct intel_fbc_work {
-		struct delayed_work work;
-		struct intel_crtc *crtc;
+		bool scheduled;
+		struct work_struct work;
 		struct drm_framebuffer *fb;
-	} *fbc_work;
-
-	enum no_fbc_reason {
-		FBC_OK, /* FBC is enabled */
-		FBC_UNSUPPORTED, /* FBC is not supported by this chipset */
-		FBC_NO_OUTPUT, /* no outputs enabled to compress */
-		FBC_STOLEN_TOO_SMALL, /* not enough space for buffers */
-		FBC_UNSUPPORTED_MODE, /* interlace or doublescanned mode */
-		FBC_MODE_TOO_LARGE, /* mode too large for compression */
-		FBC_BAD_PLANE, /* fbc not supported on plane */
-		FBC_NOT_TILED, /* buffer not tiled */
-		FBC_MULTIPLE_PIPES, /* more than one pipe active */
-		FBC_MODULE_PARAM,
-		FBC_CHIP_DEFAULT, /* disabled by default on this chip */
-		FBC_ROTATION, /* rotation is not supported */
-		FBC_IN_DBG_MASTER, /* kernel debugger is active */
-		FBC_BAD_STRIDE, /* stride is not supported */
-		FBC_PIXEL_RATE, /* pixel rate is too big */
-		FBC_PIXEL_FORMAT /* pixel format is invalid */
-	} no_fbc_reason;
-
-	bool (*fbc_enabled)(struct drm_i915_private *dev_priv);
-	void (*enable_fbc)(struct intel_crtc *crtc);
-	void (*disable_fbc)(struct drm_i915_private *dev_priv);
+		unsigned long enable_jiffies;
+	} work;
+
+	const char *no_fbc_reason;
+
+	bool (*is_active)(struct drm_i915_private *dev_priv);
+	void (*activate)(struct intel_crtc *crtc);
+	void (*deactivate)(struct drm_i915_private *dev_priv);
 };
 
 /**
@@ -1020,7 +1003,7 @@ struct intel_gmbus {
 	struct i2c_adapter adapter;
 	u32 force_bit;
 	u32 reg0;
-	u32 gpio_reg;
+	i915_reg_t gpio_reg;
 	struct i2c_algo_bit_data bit_algo;
 	struct drm_i915_private *dev_priv;
 };
@@ -1623,6 +1606,8 @@ struct skl_wm_level {
  * For more, read the Documentation/power/runtime_pm.txt.
  */
 struct i915_runtime_pm {
+	atomic_t wakeref_count;
+	atomic_t atomic_seq;
 	bool suspended;
 	bool irqs_enabled;
 };
@@ -1669,7 +1654,7 @@ struct i915_frontbuffer_tracking {
 };
 
 struct i915_wa_reg {
-	u32 addr;
+	i915_reg_t addr;
 	u32 value;
 	/* bitmask representing WA bits */
 	u32 mask;
@@ -1698,6 +1683,13 @@ struct i915_execbuffer_params {
 	struct drm_i915_gem_request     *request;
 };
 
+/* used in computing the new watermarks state */
+struct intel_wm_config {
+	unsigned int num_pipes_active;
+	bool sprites_enabled;
+	bool sprites_scaled;
+};
+
 struct drm_i915_private {
 	struct drm_device *dev;
 	struct kmem_cache *objects;
@@ -1718,9 +1710,6 @@ struct drm_i915_private {
 
 	struct intel_csr csr;
 
-	/* Display CSR-related protection */
-	struct mutex csr_lock;
-
 	struct intel_gmbus gmbus[GMBUS_NUM_PINS];
 
 	/** gmbus_mutex protects against concurrent usage of the single hw gmbus
@@ -1735,6 +1724,8 @@ struct drm_i915_private {
 	/* MMIO base address for MIPI regs */
 	uint32_t mipi_mmio_base;
 
+	uint32_t psr_mmio_base;
+
 	wait_queue_head_t gmbus_wait_queue;
 
 	struct pci_dev *bridge_dev;
@@ -1900,6 +1891,7 @@ struct drm_i915_private {
 	u32 chv_phy_control;
 
 	u32 suspend_count;
+	bool suspended_to_idle;
 	struct i915_suspend_saved_registers regfile;
 	struct vlv_s0ix_state vlv_s0ix_state;
 
@@ -1922,6 +1914,9 @@ struct drm_i915_private {
 		 */
 		uint16_t skl_latency[8];
 
+		/* Committed wm config */
+		struct intel_wm_config config;
+
 		/*
 		 * The skl_wm_values structure is a bit too big for stack
 		 * allocation, so we keep the staging struct where we store
@@ -1956,6 +1951,8 @@ struct drm_i915_private {
 	/* perform PHY state sanity checks? */
 	bool chv_phy_assert[2];
 
+	struct intel_encoder *dig_port_map[I915_MAX_PORTS];
+
 	/*
 	 * NOTE: This is the dri1/ums dungeon, don't add stuff here. Your patch
 	 * will be rejected. Instead look for a better place.
@@ -1980,7 +1977,7 @@ static inline struct drm_i915_private *guc_to_i915(struct intel_guc *guc)
 /* Iterate over initialised rings */
 #define for_each_ring(ring__, dev_priv__, i__) \
 	for ((i__) = 0; (i__) < I915_NUM_RINGS; (i__)++) \
-		if (((ring__) = &(dev_priv__)->ring[(i__)]), intel_ring_initialized((ring__)))
+		for_each_if ((((ring__) = &(dev_priv__)->ring[(i__)]), intel_ring_initialized((ring__))))
 
 enum hdmi_force_audio {
 	HDMI_AUDIO_OFF_DVI = -2,	/* no aux data for HDMI-DVI converter */
@@ -1992,6 +1989,9 @@ enum hdmi_force_audio {
 #define I915_GTT_OFFSET_NONE ((u32)-1)
 
 struct drm_i915_gem_object_ops {
+	unsigned int flags;
+#define I915_GEM_OBJECT_HAS_STRUCT_PAGE 0x1
+
 	/* Interface between the GEM object and its backing storage.
 	 * get_pages() is called once prior to the use of the associated set
 	 * of pages before to binding them into the GTT, and put_pages() is
@@ -2007,6 +2007,7 @@ struct drm_i915_gem_object_ops {
 	 */
 	int (*get_pages)(struct drm_i915_gem_object *);
 	void (*put_pages)(struct drm_i915_gem_object *);
+
 	int (*dmabuf_export)(struct drm_i915_gem_object *);
 	void (*release)(struct drm_i915_gem_object *);
 };
@@ -2445,6 +2446,15 @@ struct drm_i915_cmd_table {
 #define INTEL_DEVID(p)	(INTEL_INFO(p)->device_id)
 #define INTEL_REVID(p)	(__I915__(p)->dev->pdev->revision)
 
+#define REVID_FOREVER		0xff
+/*
+ * Return true if revision is in range [since,until] inclusive.
+ *
+ * Use 0 for open-ended since, and REVID_FOREVER for open-ended until.
+ */
+#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_I85X(dev)		(INTEL_INFO(dev)->is_i85x)
@@ -2467,11 +2477,12 @@ struct drm_i915_cmd_table {
 				 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_valleyview && IS_GEN8(dev))
+#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_valleyview && IS_GEN8(dev))
+#define IS_BROADWELL(dev)	(!INTEL_INFO(dev)->is_cherryview && IS_GEN8(dev))
 #define IS_SKYLAKE(dev)	(INTEL_INFO(dev)->is_skylake)
-#define IS_BROXTON(dev)	(!INTEL_INFO(dev)->is_skylake && IS_GEN9(dev))
+#define IS_BROXTON(dev)		(INTEL_INFO(dev)->is_broxton)
+#define IS_KABYLAKE(dev)	(INTEL_INFO(dev)->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)
@@ -2499,6 +2510,14 @@ struct drm_i915_cmd_table {
 #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) && \
@@ -2506,16 +2525,21 @@ struct drm_i915_cmd_table {
 
 #define IS_PRELIMINARY_HW(intel_info) ((intel_info)->is_preliminary)
 
-#define SKL_REVID_A0		(0x0)
-#define SKL_REVID_B0		(0x1)
-#define SKL_REVID_C0		(0x2)
-#define SKL_REVID_D0		(0x3)
-#define SKL_REVID_E0		(0x4)
-#define SKL_REVID_F0		(0x5)
+#define SKL_REVID_A0		0x0
+#define SKL_REVID_B0		0x1
+#define SKL_REVID_C0		0x2
+#define SKL_REVID_D0		0x3
+#define SKL_REVID_E0		0x4
+#define SKL_REVID_F0		0x5
+
+#define IS_SKL_REVID(p, since, until) (IS_SKYLAKE(p) && IS_REVID(p, since, until))
 
-#define BXT_REVID_A0		(0x0)
-#define BXT_REVID_B0		(0x3)
-#define BXT_REVID_C0		(0x9)
+#define BXT_REVID_A0		0x0
+#define BXT_REVID_A1		0x1
+#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))
 
 /*
  * The genX designation typically refers to the render engine, so render
@@ -2587,23 +2611,25 @@ struct drm_i915_cmd_table {
 #define HAS_FPGA_DBG_UNCLAIMED(dev)	(INTEL_INFO(dev)->has_fpga_dbg)
 #define HAS_PSR(dev)		(IS_HASWELL(dev) || IS_BROADWELL(dev) || \
 				 IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev) || \
-				 IS_SKYLAKE(dev))
+				 IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 #define HAS_RUNTIME_PM(dev)	(IS_GEN6(dev) || IS_HASWELL(dev) || \
 				 IS_BROADWELL(dev) || IS_VALLEYVIEW(dev) || \
-				 IS_SKYLAKE(dev))
+				 IS_CHERRYVIEW(dev) || IS_SKYLAKE(dev) || \
+				 IS_KABYLAKE(dev))
 #define HAS_RC6(dev)		(INTEL_INFO(dev)->gen >= 6)
 #define HAS_RC6p(dev)		(INTEL_INFO(dev)->gen == 6 || IS_IVYBRIDGE(dev))
 
 #define HAS_CSR(dev)	(IS_GEN9(dev))
 
-#define HAS_GUC_UCODE(dev)	(IS_GEN9(dev))
-#define HAS_GUC_SCHED(dev)	(IS_GEN9(dev))
+#define HAS_GUC_UCODE(dev)	(IS_GEN9(dev) && !IS_KABYLAKE(dev))
+#define HAS_GUC_SCHED(dev)	(IS_GEN9(dev) && !IS_KABYLAKE(dev))
 
 #define HAS_RESOURCE_STREAMER(dev) (IS_HASWELL(dev) || \
 				    INTEL_INFO(dev)->gen >= 8)
 
 #define HAS_CORE_RING_FREQ(dev)	(INTEL_INFO(dev)->gen >= 6 && \
-				 !IS_VALLEYVIEW(dev) && !IS_BROXTON(dev))
+				 !IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) && \
+				 !IS_BROXTON(dev))
 
 #define INTEL_PCH_DEVICE_ID_MASK		0xff00
 #define INTEL_PCH_IBX_DEVICE_ID_TYPE		0x3b00
@@ -2614,17 +2640,20 @@ struct drm_i915_cmd_table {
 #define INTEL_PCH_SPT_DEVICE_ID_TYPE		0xA100
 #define INTEL_PCH_SPT_LP_DEVICE_ID_TYPE		0x9D00
 #define INTEL_PCH_P2X_DEVICE_ID_TYPE		0x7100
+#define INTEL_PCH_QEMU_DEVICE_ID_TYPE		0x2900 /* qemu q35 has 2918 */
 
 #define INTEL_PCH_TYPE(dev) (__I915__(dev)->pch_type)
 #define HAS_PCH_SPT(dev) (INTEL_PCH_TYPE(dev) == PCH_SPT)
 #define HAS_PCH_LPT(dev) (INTEL_PCH_TYPE(dev) == PCH_LPT)
 #define HAS_PCH_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 HAS_GMCH_DISPLAY(dev) (INTEL_INFO(dev)->gen < 5 || IS_VALLEYVIEW(dev))
+#define HAS_GMCH_DISPLAY(dev) (INTEL_INFO(dev)->gen < 5 || \
+			       IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 
 /* DPF == dynamic parity feature */
 #define HAS_L3_DPF(dev) (IS_IVYBRIDGE(dev) || IS_HASWELL(dev))
@@ -2650,6 +2679,7 @@ struct i915_params {
 	int panel_use_ssc;
 	int vbt_sdvo_panel_type;
 	int enable_rc6;
+	int enable_dc;
 	int enable_fbc;
 	int enable_ppgtt;
 	int enable_execlists;
@@ -2698,7 +2728,6 @@ extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
 extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
 int vlv_force_gfx_clock(struct drm_i915_private *dev_priv, bool on);
-void i915_firmware_load_error_print(const char *fw_path, int err);
 
 /* intel_hotplug.c */
 void intel_hpd_irq_handler(struct drm_device *dev, u32 pin_mask, u32 long_mask);
@@ -2755,17 +2784,47 @@ void valleyview_disable_display_irqs(struct drm_i915_private *dev_priv);
 void i915_hotplug_interrupt_update(struct drm_i915_private *dev_priv,
 				   uint32_t mask,
 				   uint32_t bits);
-void
-ironlake_enable_display_irq(struct drm_i915_private *dev_priv, u32 mask);
-void
-ironlake_disable_display_irq(struct drm_i915_private *dev_priv, u32 mask);
+void ilk_update_display_irq(struct drm_i915_private *dev_priv,
+			    uint32_t interrupt_mask,
+			    uint32_t enabled_irq_mask);
+static inline void
+ilk_enable_display_irq(struct drm_i915_private *dev_priv, uint32_t bits)
+{
+	ilk_update_display_irq(dev_priv, bits, bits);
+}
+static inline void
+ilk_disable_display_irq(struct drm_i915_private *dev_priv, uint32_t bits)
+{
+	ilk_update_display_irq(dev_priv, bits, 0);
+}
+void bdw_update_pipe_irq(struct drm_i915_private *dev_priv,
+			 enum pipe pipe,
+			 uint32_t interrupt_mask,
+			 uint32_t enabled_irq_mask);
+static inline void bdw_enable_pipe_irq(struct drm_i915_private *dev_priv,
+				       enum pipe pipe, uint32_t bits)
+{
+	bdw_update_pipe_irq(dev_priv, pipe, bits, bits);
+}
+static inline void bdw_disable_pipe_irq(struct drm_i915_private *dev_priv,
+					enum pipe pipe, uint32_t bits)
+{
+	bdw_update_pipe_irq(dev_priv, pipe, bits, 0);
+}
 void ibx_display_interrupt_update(struct drm_i915_private *dev_priv,
 				  uint32_t interrupt_mask,
 				  uint32_t enabled_irq_mask);
-#define ibx_enable_display_interrupt(dev_priv, bits) \
-	ibx_display_interrupt_update((dev_priv), (bits), (bits))
-#define ibx_disable_display_interrupt(dev_priv, bits) \
-	ibx_display_interrupt_update((dev_priv), (bits), 0)
+static inline void
+ibx_enable_display_interrupt(struct drm_i915_private *dev_priv, uint32_t bits)
+{
+	ibx_display_interrupt_update(dev_priv, bits, bits);
+}
+static inline void
+ibx_disable_display_interrupt(struct drm_i915_private *dev_priv, uint32_t bits)
+{
+	ibx_display_interrupt_update(dev_priv, bits, 0);
+}
+
 
 /* i915_gem.c */
 int i915_gem_create_ioctl(struct drm_device *dev, void *data,
@@ -2834,6 +2893,7 @@ void i915_gem_vma_destroy(struct i915_vma *vma);
 #define PIN_UPDATE	(1<<5)
 #define PIN_ZONE_4G	(1<<6)
 #define PIN_HIGH	(1<<7)
+#define PIN_OFFSET_FIXED	(1<<8)
 #define PIN_OFFSET_MASK (~4095)
 int __must_check
 i915_gem_object_pin(struct drm_i915_gem_object *obj,
@@ -2869,6 +2929,9 @@ static inline int __sg_page_count(struct scatterlist *sg)
 	return sg->length >> PAGE_SHIFT;
 }
 
+struct page *
+i915_gem_object_get_dirty_page(struct drm_i915_gem_object *obj, int n);
+
 static inline struct page *
 i915_gem_object_get_page(struct drm_i915_gem_object *obj, int n)
 {
@@ -3008,8 +3071,6 @@ i915_gem_object_set_to_cpu_domain(struct drm_i915_gem_object *obj, bool write);
 int __must_check
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
-				     struct intel_engine_cs *pipelined,
-				     struct drm_i915_gem_request **pipelined_request,
 				     const struct i915_ggtt_view *view);
 void i915_gem_object_unpin_from_display_plane(struct drm_i915_gem_object *obj,
 					      const struct i915_ggtt_view *view);
@@ -3184,6 +3245,7 @@ int __must_check i915_gem_evict_something(struct drm_device *dev,
 					  unsigned long start,
 					  unsigned long end,
 					  unsigned flags);
+int __must_check i915_gem_evict_for_vma(struct i915_vma *target);
 int i915_gem_evict_vm(struct i915_address_space *vm, bool do_idle);
 
 /* belongs in i915_gem_gtt.h */
@@ -3312,6 +3374,10 @@ static inline bool intel_gmbus_is_forced_bit(struct i2c_adapter *adapter)
 }
 extern void intel_i2c_reset(struct drm_device *dev);
 
+/* intel_bios.c */
+int intel_bios_init(struct drm_i915_private *dev_priv);
+bool intel_bios_is_valid_vbt(const void *buf, size_t size);
+
 /* intel_opregion.c */
 #ifdef CONFIG_ACPI
 extern int intel_opregion_setup(struct drm_device *dev);
@@ -3364,7 +3430,6 @@ extern void intel_set_rps(struct drm_device *dev, u8 val);
 extern void intel_set_memory_cxsr(struct drm_i915_private *dev_priv,
 				  bool enable);
 extern void intel_detect_pch(struct drm_device *dev);
-extern int intel_trans_dp_port_sel(struct drm_crtc *crtc);
 extern int intel_enable_rc6(const struct drm_device *dev);
 
 extern bool i915_semaphore_is_enabled(struct drm_device *dev);
@@ -3447,6 +3512,32 @@ int intel_freq_opcode(struct drm_i915_private *dev_priv, int val);
 #define POSTING_READ(reg)	(void)I915_READ_NOTRACE(reg)
 #define POSTING_READ16(reg)	(void)I915_READ16_NOTRACE(reg)
 
+#define __raw_read(x, s) \
+static inline uint##x##_t __raw_i915_read##x(struct drm_i915_private *dev_priv, \
+					     i915_reg_t reg) \
+{ \
+	return read##s(dev_priv->regs + i915_mmio_reg_offset(reg)); \
+}
+
+#define __raw_write(x, s) \
+static inline void __raw_i915_write##x(struct drm_i915_private *dev_priv, \
+				       i915_reg_t reg, uint##x##_t val) \
+{ \
+	write##s(val, dev_priv->regs + i915_mmio_reg_offset(reg)); \
+}
+__raw_read(8, b)
+__raw_read(16, w)
+__raw_read(32, l)
+__raw_read(64, q)
+
+__raw_write(8, b)
+__raw_write(16, w)
+__raw_write(32, l)
+__raw_write(64, q)
+
+#undef __raw_read
+#undef __raw_write
+
 /* These are untraced mmio-accessors that are only valid to be used inside
  * criticial sections inside IRQ handlers where forcewake is explicitly
  * controlled.
@@ -3454,8 +3545,8 @@ int intel_freq_opcode(struct drm_i915_private *dev_priv, int val);
  * Note: Should only be used between intel_uncore_forcewake_irqlock() and
  * intel_uncore_forcewake_irqunlock().
  */
-#define I915_READ_FW(reg__) readl(dev_priv->regs + (reg__))
-#define I915_WRITE_FW(reg__, val__) writel(val__, dev_priv->regs + (reg__))
+#define I915_READ_FW(reg__) __raw_i915_read32(dev_priv, (reg__))
+#define I915_WRITE_FW(reg__, val__) __raw_i915_write32(dev_priv, (reg__), (val__))
 #define POSTING_READ_FW(reg__) (void)I915_READ_FW(reg__)
 
 /* "Broadcast RGB" property */
@@ -3463,9 +3554,9 @@ int intel_freq_opcode(struct drm_i915_private *dev_priv, int val);
 #define INTEL_BROADCAST_RGB_FULL 1
 #define INTEL_BROADCAST_RGB_LIMITED 2
 
-static inline uint32_t i915_vgacntrl_reg(struct drm_device *dev)
+static inline i915_reg_t i915_vgacntrl_reg(struct drm_device *dev)
 {
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		return VLV_VGACNTRL;
 	else if (INTEL_INFO(dev)->gen >= 5)
 		return CPU_VGACNTRL;
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index f56af0aaafde..bb44bad15403 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -2797,6 +2797,8 @@ static void i915_gem_reset_ring_status(struct drm_i915_private *dev_priv,
 static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 					struct intel_engine_cs *ring)
 {
+	struct intel_ringbuffer *buffer;
+
 	while (!list_empty(&ring->active_list)) {
 		struct drm_i915_gem_object *obj;
 
@@ -2812,18 +2814,16 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 	 * are the ones that keep the context and ringbuffer backing objects
 	 * pinned in place.
 	 */
-	while (!list_empty(&ring->execlist_queue)) {
-		struct drm_i915_gem_request *submit_req;
 
-		submit_req = list_first_entry(&ring->execlist_queue,
-				struct drm_i915_gem_request,
-				execlist_link);
-		list_del(&submit_req->execlist_link);
+	if (i915.enable_execlists) {
+		spin_lock_irq(&ring->execlist_lock);
 
-		if (submit_req->ctx != ring->default_context)
-			intel_lr_context_unpin(submit_req);
+		/* list_splice_tail_init checks for empty lists */
+		list_splice_tail_init(&ring->execlist_queue,
+				      &ring->execlist_retired_req_list);
 
-		i915_gem_request_unreference(submit_req);
+		spin_unlock_irq(&ring->execlist_lock);
+		intel_execlists_retire_requests(ring);
 	}
 
 	/*
@@ -2842,6 +2842,18 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 
 		i915_gem_request_retire(request);
 	}
+
+	/* Having flushed all requests from all queues, we know that all
+	 * ringbuffers must now be empty. However, since we do not reclaim
+	 * all space when retiring the request (to prevent HEADs colliding
+	 * with rapid ringbuffer wraparound) the amount of available space
+	 * upon reset is less than when we start. Do one more pass over
+	 * all the ringbuffers to reset last_retired_head.
+	 */
+	list_for_each_entry(buffer, &ring->buffers, link) {
+		buffer->last_retired_head = buffer->tail;
+		intel_ring_update_space(buffer);
+	}
 }
 
 void i915_gem_reset(struct drm_device *dev)
@@ -2982,6 +2994,10 @@ i915_gem_idle_work_handler(struct work_struct *work)
 		if (!list_empty(&ring->request_list))
 			return;
 
+	/* we probably should sync with hangcheck here, using cancel_work_sync.
+	 * Also locking seems to be fubar here, ring->request_list is protected
+	 * by dev->struct_mutex. */
+
 	intel_mark_idle(dev);
 
 	if (mutex_trylock(&dev->struct_mutex)) {
@@ -3106,7 +3122,7 @@ i915_gem_wait_ioctl(struct drm_device *dev, void *data, struct drm_file *file)
 		if (ret == 0)
 			ret = __i915_wait_request(req[i], reset_counter, true,
 						  args->timeout_ns > 0 ? &args->timeout_ns : NULL,
-						  file->driver_priv);
+						  to_rps_client(file));
 		i915_gem_request_unreference__unlocked(req[i]);
 	}
 	return ret;
@@ -3472,7 +3488,7 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 	if (flags & PIN_MAPPABLE)
 		end = min_t(u64, end, dev_priv->gtt.mappable_end);
 	if (flags & PIN_ZONE_4G)
-		end = min_t(u64, end, (1ULL << 32));
+		end = min_t(u64, end, (1ULL << 32) - PAGE_SIZE);
 
 	if (alignment == 0)
 		alignment = flags & PIN_MAPPABLE ? fence_alignment :
@@ -3509,30 +3525,50 @@ i915_gem_object_bind_to_vm(struct drm_i915_gem_object *obj,
 	if (IS_ERR(vma))
 		goto err_unpin;
 
-	if (flags & PIN_HIGH) {
-		search_flag = DRM_MM_SEARCH_BELOW;
-		alloc_flag = DRM_MM_CREATE_TOP;
+	if (flags & PIN_OFFSET_FIXED) {
+		uint64_t offset = flags & PIN_OFFSET_MASK;
+
+		if (offset & (alignment - 1) || offset + size > end) {
+			ret = -EINVAL;
+			goto err_free_vma;
+		}
+		vma->node.start = offset;
+		vma->node.size = size;
+		vma->node.color = obj->cache_level;
+		ret = drm_mm_reserve_node(&vm->mm, &vma->node);
+		if (ret) {
+			ret = i915_gem_evict_for_vma(vma);
+			if (ret == 0)
+				ret = drm_mm_reserve_node(&vm->mm, &vma->node);
+		}
+		if (ret)
+			goto err_free_vma;
 	} else {
-		search_flag = DRM_MM_SEARCH_DEFAULT;
-		alloc_flag = DRM_MM_CREATE_DEFAULT;
-	}
+		if (flags & PIN_HIGH) {
+			search_flag = DRM_MM_SEARCH_BELOW;
+			alloc_flag = DRM_MM_CREATE_TOP;
+		} else {
+			search_flag = DRM_MM_SEARCH_DEFAULT;
+			alloc_flag = DRM_MM_CREATE_DEFAULT;
+		}
 
 search_free:
-	ret = drm_mm_insert_node_in_range_generic(&vm->mm, &vma->node,
-						  size, alignment,
-						  obj->cache_level,
-						  start, end,
-						  search_flag,
-						  alloc_flag);
-	if (ret) {
-		ret = i915_gem_evict_something(dev, vm, size, alignment,
-					       obj->cache_level,
-					       start, end,
-					       flags);
-		if (ret == 0)
-			goto search_free;
+		ret = drm_mm_insert_node_in_range_generic(&vm->mm, &vma->node,
+							  size, alignment,
+							  obj->cache_level,
+							  start, end,
+							  search_flag,
+							  alloc_flag);
+		if (ret) {
+			ret = i915_gem_evict_something(dev, vm, size, alignment,
+						       obj->cache_level,
+						       start, end,
+						       flags);
+			if (ret == 0)
+				goto search_free;
 
-		goto err_free_vma;
+			goto err_free_vma;
+		}
 	}
 	if (WARN_ON(!i915_gem_valid_gtt_space(vma, obj->cache_level))) {
 		ret = -EINVAL;
@@ -3886,7 +3922,7 @@ int i915_gem_set_caching_ioctl(struct drm_device *dev, void *data,
 		 * cacheline, whereas normally such cachelines would get
 		 * invalidated.
 		 */
-		if (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0)
+		if (IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 			return -ENODEV;
 
 		level = I915_CACHE_LLC;
@@ -3929,17 +3965,11 @@ rpm_put:
 int
 i915_gem_object_pin_to_display_plane(struct drm_i915_gem_object *obj,
 				     u32 alignment,
-				     struct intel_engine_cs *pipelined,
-				     struct drm_i915_gem_request **pipelined_request,
 				     const struct i915_ggtt_view *view)
 {
 	u32 old_read_domains, old_write_domain;
 	int ret;
 
-	ret = i915_gem_object_sync(obj, pipelined, pipelined_request);
-	if (ret)
-		return ret;
-
 	/* Mark the pin_display early so that we account for the
 	 * display coherency whilst setting up the cache domains.
 	 */
@@ -4129,6 +4159,10 @@ i915_vma_misplaced(struct i915_vma *vma, uint32_t alignment, uint64_t flags)
 	    vma->node.start < (flags & PIN_OFFSET_MASK))
 		return true;
 
+	if (flags & PIN_OFFSET_FIXED &&
+	    vma->node.start != (flags & PIN_OFFSET_MASK))
+		return true;
+
 	return false;
 }
 
@@ -4391,6 +4425,7 @@ void i915_gem_object_init(struct drm_i915_gem_object *obj,
 }
 
 static const struct drm_i915_gem_object_ops i915_gem_object_ops = {
+	.flags = I915_GEM_OBJECT_HAS_STRUCT_PAGE,
 	.get_pages = i915_gem_object_get_pages_gtt,
 	.put_pages = i915_gem_object_put_pages_gtt,
 };
@@ -4541,10 +4576,8 @@ struct i915_vma *i915_gem_obj_to_vma(struct drm_i915_gem_object *obj,
 {
 	struct i915_vma *vma;
 	list_for_each_entry(vma, &obj->vma_list, vma_link) {
-		if (i915_is_ggtt(vma->vm) &&
-		    vma->ggtt_view.type != I915_GGTT_VIEW_NORMAL)
-			continue;
-		if (vma->vm == vm)
+		if (vma->ggtt_view.type == I915_GGTT_VIEW_NORMAL &&
+		    vma->vm == vm)
 			return vma;
 	}
 	return NULL;
@@ -4633,7 +4666,6 @@ int i915_gem_l3_remap(struct drm_i915_gem_request *req, int slice)
 	struct intel_engine_cs *ring = req->ring;
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 reg_base = GEN7_L3LOG_BASE + (slice * 0x200);
 	u32 *remap_info = dev_priv->l3_parity.remap_info[slice];
 	int i, ret;
 
@@ -4649,10 +4681,10 @@ int i915_gem_l3_remap(struct drm_i915_gem_request *req, int slice)
 	 * here because no other code should access these registers other than
 	 * at initialization time.
 	 */
-	for (i = 0; i < GEN7_L3LOG_SIZE; i += 4) {
+	for (i = 0; i < GEN7_L3LOG_SIZE / 4; i++) {
 		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(ring, reg_base + i);
-		intel_ring_emit(ring, remap_info[i/4]);
+		intel_ring_emit_reg(ring, GEN7_L3LOG(slice, i));
+		intel_ring_emit(ring, remap_info[i]);
 	}
 
 	intel_ring_advance(ring);
@@ -4820,18 +4852,9 @@ i915_gem_init_hw(struct drm_device *dev)
 	if (HAS_GUC_UCODE(dev)) {
 		ret = intel_guc_ucode_load(dev);
 		if (ret) {
-			/*
-			 * If we got an error and GuC submission is enabled, map
-			 * the error to -EIO so the GPU will be declared wedged.
-			 * OTOH, if we didn't intend to use the GuC anyway, just
-			 * discard the error and carry on.
-			 */
-			DRM_ERROR("Failed to initialize GuC, error %d%s\n", ret,
-				  i915.enable_guc_submission ? "" :
-				  " (ignored)");
-			ret = i915.enable_guc_submission ? -EIO : 0;
-			if (ret)
-				goto out;
+			DRM_ERROR("Failed to initialize GuC, error %d\n", ret);
+			ret = -EIO;
+			goto out;
 		}
 	}
 
@@ -4894,14 +4917,6 @@ int i915_gem_init(struct drm_device *dev)
 
 	mutex_lock(&dev->struct_mutex);
 
-	if (IS_VALLEYVIEW(dev)) {
-		/* VLVA0 (potential hack), BIOS isn't actually waking us */
-		I915_WRITE(VLV_GTLC_WAKE_CTRL, VLV_GTLC_ALLOWWAKEREQ);
-		if (wait_for((I915_READ(VLV_GTLC_PW_STATUS) &
-			      VLV_GTLC_ALLOWWAKEACK), 10))
-			DRM_DEBUG_DRIVER("allow wake ack timed out\n");
-	}
-
 	if (!i915.enable_execlists) {
 		dev_priv->gt.execbuf_submit = i915_gem_ringbuffer_submission;
 		dev_priv->gt.init_rings = i915_gem_init_rings;
@@ -5019,7 +5034,7 @@ i915_gem_load(struct drm_device *dev)
 
 	dev_priv->relative_constants_mode = I915_EXEC_CONSTANTS_REL_GENERAL;
 
-	if (INTEL_INFO(dev)->gen >= 7 && !IS_VALLEYVIEW(dev))
+	if (INTEL_INFO(dev)->gen >= 7 && !IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev))
 		dev_priv->num_fence_regs = 32;
 	else if (INTEL_INFO(dev)->gen >= 4 || IS_I945G(dev) || IS_I945GM(dev) || IS_G33(dev))
 		dev_priv->num_fence_regs = 16;
@@ -5240,6 +5255,21 @@ bool i915_gem_obj_is_pinned(struct drm_i915_gem_object *obj)
 	return false;
 }
 
+/* Like i915_gem_object_get_page(), but mark the returned page dirty */
+struct page *
+i915_gem_object_get_dirty_page(struct drm_i915_gem_object *obj, int n)
+{
+	struct page *page;
+
+	/* Only default objects have per-page dirty tracking */
+	if (WARN_ON((obj->ops->flags & I915_GEM_OBJECT_HAS_STRUCT_PAGE) == 0))
+		return NULL;
+
+	page = i915_gem_object_get_page(obj, n);
+	set_page_dirty(page);
+	return page;
+}
+
 /* Allocate a new GEM object and fill it with the supplied data */
 struct drm_i915_gem_object *
 i915_gem_object_create_from_data(struct drm_device *dev,
@@ -5265,6 +5295,7 @@ i915_gem_object_create_from_data(struct drm_device *dev,
 	i915_gem_object_pin_pages(obj);
 	sg = obj->pages;
 	bytes = sg_copy_from_buffer(sg->sgl, sg->nents, (void *)data, size);
+	obj->dirty = 1;		/* Backing store is now out of date */
 	i915_gem_object_unpin_pages(obj);
 
 	if (WARN_ON(bytes != size)) {
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 02ceb7a4b481..c25083c78ba7 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -189,8 +189,15 @@ i915_gem_alloc_context_obj(struct drm_device *dev, size_t size)
 	 * shouldn't touch the cache level, especially as that
 	 * would make the object snooped which might have a
 	 * negative performance impact.
+	 *
+	 * Snooping is required on non-llc platforms in execlist
+	 * mode, but since all GGTT accesses use PAT entry 0 we
+	 * get snooping anyway regardless of cache_level.
+	 *
+	 * This is only applicable for Ivy Bridge devices since
+	 * later platforms don't have L3 control bits in the PTE.
 	 */
-	if (INTEL_INFO(dev)->gen >= 7 && !IS_VALLEYVIEW(dev)) {
+	if (IS_IVYBRIDGE(dev)) {
 		ret = i915_gem_object_set_cache_level(obj, I915_CACHE_L3_LLC);
 		/* Failure shouldn't ever happen this early */
 		if (WARN_ON(ret)) {
@@ -340,6 +347,10 @@ void i915_gem_context_reset(struct drm_device *dev)
 			i915_gem_context_unreference(lctx);
 			ring->last_context = NULL;
 		}
+
+		/* Force the GPU state to be reinitialised on enabling */
+		if (ring->default_context)
+			ring->default_context->legacy_hw_ctx.initialized = false;
 	}
 }
 
@@ -554,7 +565,7 @@ mi_set_context(struct drm_i915_gem_request *req, u32 hw_flags)
 				if (signaller == ring)
 					continue;
 
-				intel_ring_emit(ring, RING_PSMI_CTL(signaller->mmio_base));
+				intel_ring_emit_reg(ring, RING_PSMI_CTL(signaller->mmio_base));
 				intel_ring_emit(ring, _MASKED_BIT_ENABLE(GEN6_PSMI_SLEEP_MSG_DISABLE));
 			}
 		}
@@ -579,7 +590,7 @@ mi_set_context(struct drm_i915_gem_request *req, u32 hw_flags)
 				if (signaller == ring)
 					continue;
 
-				intel_ring_emit(ring, RING_PSMI_CTL(signaller->mmio_base));
+				intel_ring_emit_reg(ring, RING_PSMI_CTL(signaller->mmio_base));
 				intel_ring_emit(ring, _MASKED_BIT_DISABLE(GEN6_PSMI_SLEEP_MSG_DISABLE));
 			}
 		}
@@ -708,7 +719,7 @@ static int do_switch(struct drm_i915_gem_request *req)
 	if (ret)
 		goto unpin_out;
 
-	if (!to->legacy_hw_ctx.initialized) {
+	if (!to->legacy_hw_ctx.initialized || i915_gem_context_is_default(to)) {
 		hw_flags |= MI_RESTORE_INHIBIT;
 		/* NB: If we inhibit the restore, the context is not allowed to
 		 * die because future work may end up depending on valid address
@@ -923,6 +934,14 @@ int i915_gem_context_getparam_ioctl(struct drm_device *dev, void *data,
 	case I915_CONTEXT_PARAM_NO_ZEROMAP:
 		args->value = ctx->flags & CONTEXT_NO_ZEROMAP;
 		break;
+	case I915_CONTEXT_PARAM_GTT_SIZE:
+		if (ctx->ppgtt)
+			args->value = ctx->ppgtt->base.total;
+		else if (to_i915(dev)->mm.aliasing_ppgtt)
+			args->value = to_i915(dev)->mm.aliasing_ppgtt->base.total;
+		else
+			args->value = to_i915(dev)->gtt.base.total;
+		break;
 	default:
 		ret = -EINVAL;
 		break;
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index d71a133ceff5..07c6e4d320c9 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -199,6 +199,45 @@ found:
 	return ret;
 }
 
+int
+i915_gem_evict_for_vma(struct i915_vma *target)
+{
+	struct drm_mm_node *node, *next;
+
+	list_for_each_entry_safe(node, next,
+			&target->vm->mm.head_node.node_list,
+			node_list) {
+		struct i915_vma *vma;
+		int ret;
+
+		if (node->start + node->size <= target->node.start)
+			continue;
+		if (node->start >= target->node.start + target->node.size)
+			break;
+
+		vma = container_of(node, typeof(*vma), node);
+
+		if (vma->pin_count) {
+			if (!vma->exec_entry || (vma->pin_count > 1))
+				/* Object is pinned for some other use */
+				return -EBUSY;
+
+			/* We need to evict a buffer in the same batch */
+			if (vma->exec_entry->flags & EXEC_OBJECT_PINNED)
+				/* Overlapping fixed objects in the same batch */
+				return -EINVAL;
+
+			return -ENOSPC;
+		}
+
+		ret = i915_vma_unbind(vma);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 /**
  * i915_gem_evict_vm - Evict all idle vmas from a vm
  * @vm: Address space to cleanse
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 6ed7d63a0688..dccb517361b3 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -249,6 +249,31 @@ static inline int use_cpu_reloc(struct drm_i915_gem_object *obj)
 		obj->cache_level != I915_CACHE_NONE);
 }
 
+/* Used to convert any address to canonical form.
+ * Starting from gen8, some commands (e.g. STATE_BASE_ADDRESS,
+ * MI_LOAD_REGISTER_MEM and others, see Broadwell PRM Vol2a) require the
+ * addresses to be in a canonical form:
+ * "GraphicsAddress[63:48] are ignored by the HW and assumed to be in correct
+ * canonical form [63:48] == [47]."
+ */
+#define GEN8_HIGH_ADDRESS_BIT 47
+static inline uint64_t gen8_canonical_addr(uint64_t address)
+{
+	return sign_extend64(address, GEN8_HIGH_ADDRESS_BIT);
+}
+
+static inline uint64_t gen8_noncanonical_addr(uint64_t address)
+{
+	return address & ((1ULL << (GEN8_HIGH_ADDRESS_BIT + 1)) - 1);
+}
+
+static inline uint64_t
+relocation_target(struct drm_i915_gem_relocation_entry *reloc,
+		  uint64_t target_offset)
+{
+	return gen8_canonical_addr((int)reloc->delta + target_offset);
+}
+
 static int
 relocate_entry_cpu(struct drm_i915_gem_object *obj,
 		   struct drm_i915_gem_relocation_entry *reloc,
@@ -256,7 +281,7 @@ relocate_entry_cpu(struct drm_i915_gem_object *obj,
 {
 	struct drm_device *dev = obj->base.dev;
 	uint32_t page_offset = offset_in_page(reloc->offset);
-	uint64_t delta = reloc->delta + target_offset;
+	uint64_t delta = relocation_target(reloc, target_offset);
 	char *vaddr;
 	int ret;
 
@@ -264,7 +289,7 @@ relocate_entry_cpu(struct drm_i915_gem_object *obj,
 	if (ret)
 		return ret;
 
-	vaddr = kmap_atomic(i915_gem_object_get_page(obj,
+	vaddr = kmap_atomic(i915_gem_object_get_dirty_page(obj,
 				reloc->offset >> PAGE_SHIFT));
 	*(uint32_t *)(vaddr + page_offset) = lower_32_bits(delta);
 
@@ -273,7 +298,7 @@ relocate_entry_cpu(struct drm_i915_gem_object *obj,
 
 		if (page_offset == 0) {
 			kunmap_atomic(vaddr);
-			vaddr = kmap_atomic(i915_gem_object_get_page(obj,
+			vaddr = kmap_atomic(i915_gem_object_get_dirty_page(obj,
 			    (reloc->offset + sizeof(uint32_t)) >> PAGE_SHIFT));
 		}
 
@@ -292,7 +317,7 @@ relocate_entry_gtt(struct drm_i915_gem_object *obj,
 {
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint64_t delta = reloc->delta + target_offset;
+	uint64_t delta = relocation_target(reloc, target_offset);
 	uint64_t offset;
 	void __iomem *reloc_page;
 	int ret;
@@ -347,7 +372,7 @@ relocate_entry_clflush(struct drm_i915_gem_object *obj,
 {
 	struct drm_device *dev = obj->base.dev;
 	uint32_t page_offset = offset_in_page(reloc->offset);
-	uint64_t delta = (int)reloc->delta + target_offset;
+	uint64_t delta = relocation_target(reloc, target_offset);
 	char *vaddr;
 	int ret;
 
@@ -355,7 +380,7 @@ relocate_entry_clflush(struct drm_i915_gem_object *obj,
 	if (ret)
 		return ret;
 
-	vaddr = kmap_atomic(i915_gem_object_get_page(obj,
+	vaddr = kmap_atomic(i915_gem_object_get_dirty_page(obj,
 				reloc->offset >> PAGE_SHIFT));
 	clflush_write32(vaddr + page_offset, lower_32_bits(delta));
 
@@ -364,7 +389,7 @@ relocate_entry_clflush(struct drm_i915_gem_object *obj,
 
 		if (page_offset == 0) {
 			kunmap_atomic(vaddr);
-			vaddr = kmap_atomic(i915_gem_object_get_page(obj,
+			vaddr = kmap_atomic(i915_gem_object_get_dirty_page(obj,
 			    (reloc->offset + sizeof(uint32_t)) >> PAGE_SHIFT));
 		}
 
@@ -395,7 +420,7 @@ i915_gem_execbuffer_relocate_entry(struct drm_i915_gem_object *obj,
 	target_i915_obj = target_vma->obj;
 	target_obj = &target_vma->obj->base;
 
-	target_offset = target_vma->node.start;
+	target_offset = gen8_canonical_addr(target_vma->node.start);
 
 	/* Sandybridge PPGTT errata: We need a global gtt mapping for MI and
 	 * pipe_control writes because the gpu doesn't properly redirect them
@@ -599,6 +624,8 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 			flags |= PIN_GLOBAL | PIN_MAPPABLE;
 		if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS)
 			flags |= BATCH_OFFSET_BIAS | PIN_OFFSET_BIAS;
+		if (entry->flags & EXEC_OBJECT_PINNED)
+			flags |= entry->offset | PIN_OFFSET_FIXED;
 		if ((flags & PIN_MAPPABLE) == 0)
 			flags |= PIN_HIGH;
 	}
@@ -670,6 +697,10 @@ eb_vma_misplaced(struct i915_vma *vma)
 	    vma->node.start & (entry->alignment - 1))
 		return true;
 
+	if (entry->flags & EXEC_OBJECT_PINNED &&
+	    vma->node.start != entry->offset)
+		return true;
+
 	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS &&
 	    vma->node.start < BATCH_OFFSET_BIAS)
 		return true;
@@ -695,6 +726,7 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 	struct i915_vma *vma;
 	struct i915_address_space *vm;
 	struct list_head ordered_vmas;
+	struct list_head pinned_vmas;
 	bool has_fenced_gpu_access = INTEL_INFO(ring->dev)->gen < 4;
 	int retry;
 
@@ -703,6 +735,7 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 	vm = list_first_entry(vmas, struct i915_vma, exec_list)->vm;
 
 	INIT_LIST_HEAD(&ordered_vmas);
+	INIT_LIST_HEAD(&pinned_vmas);
 	while (!list_empty(vmas)) {
 		struct drm_i915_gem_exec_object2 *entry;
 		bool need_fence, need_mappable;
@@ -721,7 +754,9 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 			obj->tiling_mode != I915_TILING_NONE;
 		need_mappable = need_fence || need_reloc_mappable(vma);
 
-		if (need_mappable) {
+		if (entry->flags & EXEC_OBJECT_PINNED)
+			list_move_tail(&vma->exec_list, &pinned_vmas);
+		else if (need_mappable) {
 			entry->flags |= __EXEC_OBJECT_NEEDS_MAP;
 			list_move(&vma->exec_list, &ordered_vmas);
 		} else
@@ -731,6 +766,7 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 		obj->base.pending_write_domain = 0;
 	}
 	list_splice(&ordered_vmas, vmas);
+	list_splice(&pinned_vmas, vmas);
 
 	/* Attempt to pin all of the buffers into the GTT.
 	 * This is done in 3 phases:
@@ -983,6 +1019,21 @@ validate_exec_list(struct drm_device *dev,
 		if (exec[i].flags & invalid_flags)
 			return -EINVAL;
 
+		/* Offset can be used as input (EXEC_OBJECT_PINNED), reject
+		 * any non-page-aligned or non-canonical addresses.
+		 */
+		if (exec[i].flags & EXEC_OBJECT_PINNED) {
+			if (exec[i].offset !=
+			    gen8_canonical_addr(exec[i].offset & PAGE_MASK))
+				return -EINVAL;
+
+			/* From drm_mm perspective address space is continuous,
+			 * so from this point we're always using non-canonical
+			 * form internally.
+			 */
+			exec[i].offset = gen8_noncanonical_addr(exec[i].offset);
+		}
+
 		if (exec[i].alignment && !is_power_of_2(exec[i].alignment))
 			return -EINVAL;
 
@@ -1114,7 +1165,7 @@ i915_reset_gen7_sol_offsets(struct drm_device *dev,
 
 	for (i = 0; i < 4; i++) {
 		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(ring, GEN7_SO_WRITE_OFFSET(i));
+		intel_ring_emit_reg(ring, GEN7_SO_WRITE_OFFSET(i));
 		intel_ring_emit(ring, 0);
 	}
 
@@ -1241,7 +1292,7 @@ i915_gem_ringbuffer_submission(struct i915_execbuffer_params *params,
 
 		intel_ring_emit(ring, MI_NOOP);
 		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(ring, INSTPM);
+		intel_ring_emit_reg(ring, INSTPM);
 		intel_ring_emit(ring, instp_mask << 16 | instp_mode);
 		intel_ring_advance(ring);
 
@@ -1317,7 +1368,8 @@ eb_get_batch(struct eb_vmas *eb)
 	 * Note that actual hangs have only been observed on gen7, but for
 	 * paranoia do it everywhere.
 	 */
-	vma->exec_entry->flags |= __EXEC_OBJECT_NEEDS_BIAS;
+	if ((vma->exec_entry->flags & EXEC_OBJECT_PINNED) == 0)
+		vma->exec_entry->flags |= __EXEC_OBJECT_NEEDS_BIAS;
 
 	return vma->obj;
 }
@@ -1675,6 +1727,8 @@ i915_gem_execbuffer(struct drm_device *dev, void *data,
 
 		/* Copy the new buffer offsets back to the user's exec list. */
 		for (i = 0; i < args->buffer_count; i++) {
+			exec2_list[i].offset =
+				gen8_canonical_addr(exec2_list[i].offset);
 			ret = __copy_to_user(&user_exec_list[i].offset,
 					     &exec2_list[i].offset,
 					     sizeof(user_exec_list[i].offset));
@@ -1740,6 +1794,8 @@ i915_gem_execbuffer2(struct drm_device *dev, void *data,
 		int i;
 
 		for (i = 0; i < args->buffer_count; i++) {
+			exec2_list[i].offset =
+				gen8_canonical_addr(exec2_list[i].offset);
 			ret = __copy_to_user(&user_exec_list[i].offset,
 					     &exec2_list[i].offset,
 					     sizeof(user_exec_list[i].offset));
diff --git a/drivers/gpu/drm/i915/i915_gem_fence.c b/drivers/gpu/drm/i915/i915_gem_fence.c
index f010391b87f5..598198543dcd 100644
--- a/drivers/gpu/drm/i915/i915_gem_fence.c
+++ b/drivers/gpu/drm/i915/i915_gem_fence.c
@@ -59,7 +59,7 @@ static void i965_write_fence_reg(struct drm_device *dev, int reg,
 				 struct drm_i915_gem_object *obj)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int fence_reg_lo, fence_reg_hi;
+	i915_reg_t fence_reg_lo, fence_reg_hi;
 	int fence_pitch_shift;
 
 	if (INTEL_INFO(dev)->gen >= 6) {
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 86c7500454b4..56f4f2e58d53 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -24,6 +24,7 @@
  */
 
 #include <linux/seq_file.h>
+#include <linux/stop_machine.h>
 #include <drm/drmP.h>
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
@@ -104,9 +105,11 @@ static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 {
 	bool has_aliasing_ppgtt;
 	bool has_full_ppgtt;
+	bool has_full_48bit_ppgtt;
 
 	has_aliasing_ppgtt = INTEL_INFO(dev)->gen >= 6;
 	has_full_ppgtt = INTEL_INFO(dev)->gen >= 7;
+	has_full_48bit_ppgtt = IS_BROADWELL(dev) || INTEL_INFO(dev)->gen >= 9;
 
 	if (intel_vgpu_active(dev))
 		has_full_ppgtt = false; /* emulation is too hard */
@@ -125,6 +128,9 @@ static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 	if (enable_ppgtt == 2 && has_full_ppgtt)
 		return 2;
 
+	if (enable_ppgtt == 3 && has_full_48bit_ppgtt)
+		return 3;
+
 #ifdef CONFIG_INTEL_IOMMU
 	/* Disable ppgtt on SNB if VT-d is on. */
 	if (INTEL_INFO(dev)->gen == 6 && intel_iommu_gfx_mapped) {
@@ -134,14 +140,13 @@ static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 #endif
 
 	/* Early VLV doesn't have this */
-	if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
-	    dev->pdev->revision < 0xb) {
+	if (IS_VALLEYVIEW(dev) && dev->pdev->revision < 0xb) {
 		DRM_DEBUG_DRIVER("disabling PPGTT on pre-B3 step VLV\n");
 		return 0;
 	}
 
 	if (INTEL_INFO(dev)->gen >= 8 && i915.enable_execlists)
-		return 2;
+		return has_full_48bit_ppgtt ? 3 : 2;
 	else
 		return has_aliasing_ppgtt ? 1 : 0;
 }
@@ -661,10 +666,10 @@ static int gen8_write_pdp(struct drm_i915_gem_request *req,
 		return ret;
 
 	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-	intel_ring_emit(ring, GEN8_RING_PDP_UDW(ring, entry));
+	intel_ring_emit_reg(ring, GEN8_RING_PDP_UDW(ring, entry));
 	intel_ring_emit(ring, upper_32_bits(addr));
 	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-	intel_ring_emit(ring, GEN8_RING_PDP_LDW(ring, entry));
+	intel_ring_emit_reg(ring, GEN8_RING_PDP_LDW(ring, entry));
 	intel_ring_emit(ring, lower_32_bits(addr));
 	intel_ring_advance(ring);
 
@@ -764,10 +769,10 @@ static void gen8_ppgtt_clear_range(struct i915_address_space *vm,
 		gen8_ppgtt_clear_pte_range(vm, &ppgtt->pdp, start, length,
 					   scratch_pte);
 	} else {
-		uint64_t templ4, pml4e;
+		uint64_t pml4e;
 		struct i915_page_directory_pointer *pdp;
 
-		gen8_for_each_pml4e(pdp, &ppgtt->pml4, start, length, templ4, pml4e) {
+		gen8_for_each_pml4e(pdp, &ppgtt->pml4, start, length, pml4e) {
 			gen8_ppgtt_clear_pte_range(vm, pdp, start, length,
 						   scratch_pte);
 		}
@@ -833,10 +838,10 @@ static void gen8_ppgtt_insert_entries(struct i915_address_space *vm,
 					      cache_level);
 	} else {
 		struct i915_page_directory_pointer *pdp;
-		uint64_t templ4, pml4e;
+		uint64_t pml4e;
 		uint64_t length = (uint64_t)pages->orig_nents << PAGE_SHIFT;
 
-		gen8_for_each_pml4e(pdp, &ppgtt->pml4, start, length, templ4, pml4e) {
+		gen8_for_each_pml4e(pdp, &ppgtt->pml4, start, length, pml4e) {
 			gen8_ppgtt_insert_pte_entries(vm, pdp, &sg_iter,
 						      start, cache_level);
 		}
@@ -904,14 +909,13 @@ static int gen8_ppgtt_notify_vgt(struct i915_hw_ppgtt *ppgtt, bool create)
 	enum vgt_g2v_type msg;
 	struct drm_device *dev = ppgtt->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned int offset = vgtif_reg(pdp0_lo);
 	int i;
 
 	if (USES_FULL_48BIT_PPGTT(dev)) {
 		u64 daddr = px_dma(&ppgtt->pml4);
 
-		I915_WRITE(offset, lower_32_bits(daddr));
-		I915_WRITE(offset + 4, upper_32_bits(daddr));
+		I915_WRITE(vgtif_reg(pdp[0].lo), lower_32_bits(daddr));
+		I915_WRITE(vgtif_reg(pdp[0].hi), upper_32_bits(daddr));
 
 		msg = (create ? VGT_G2V_PPGTT_L4_PAGE_TABLE_CREATE :
 				VGT_G2V_PPGTT_L4_PAGE_TABLE_DESTROY);
@@ -919,10 +923,8 @@ static int gen8_ppgtt_notify_vgt(struct i915_hw_ppgtt *ppgtt, bool create)
 		for (i = 0; i < GEN8_LEGACY_PDPES; i++) {
 			u64 daddr = i915_page_dir_dma_addr(ppgtt, i);
 
-			I915_WRITE(offset, lower_32_bits(daddr));
-			I915_WRITE(offset + 4, upper_32_bits(daddr));
-
-			offset += 8;
+			I915_WRITE(vgtif_reg(pdp[i].lo), lower_32_bits(daddr));
+			I915_WRITE(vgtif_reg(pdp[i].hi), upper_32_bits(daddr));
 		}
 
 		msg = (create ? VGT_G2V_PPGTT_L3_PAGE_TABLE_CREATE :
@@ -1017,10 +1019,9 @@ static int gen8_ppgtt_alloc_pagetabs(struct i915_address_space *vm,
 {
 	struct drm_device *dev = vm->dev;
 	struct i915_page_table *pt;
-	uint64_t temp;
 	uint32_t pde;
 
-	gen8_for_each_pde(pt, pd, start, length, temp, pde) {
+	gen8_for_each_pde(pt, pd, start, length, pde) {
 		/* Don't reallocate page tables */
 		if (test_bit(pde, pd->used_pdes)) {
 			/* Scratch is never allocated this way */
@@ -1079,13 +1080,12 @@ gen8_ppgtt_alloc_page_directories(struct i915_address_space *vm,
 {
 	struct drm_device *dev = vm->dev;
 	struct i915_page_directory *pd;
-	uint64_t temp;
 	uint32_t pdpe;
 	uint32_t pdpes = I915_PDPES_PER_PDP(dev);
 
 	WARN_ON(!bitmap_empty(new_pds, pdpes));
 
-	gen8_for_each_pdpe(pd, pdp, start, length, temp, pdpe) {
+	gen8_for_each_pdpe(pd, pdp, start, length, pdpe) {
 		if (test_bit(pdpe, pdp->used_pdpes))
 			continue;
 
@@ -1133,12 +1133,11 @@ gen8_ppgtt_alloc_page_dirpointers(struct i915_address_space *vm,
 {
 	struct drm_device *dev = vm->dev;
 	struct i915_page_directory_pointer *pdp;
-	uint64_t temp;
 	uint32_t pml4e;
 
 	WARN_ON(!bitmap_empty(new_pdps, GEN8_PML4ES_PER_PML4));
 
-	gen8_for_each_pml4e(pdp, pml4, start, length, temp, pml4e) {
+	gen8_for_each_pml4e(pdp, pml4, start, length, pml4e) {
 		if (!test_bit(pml4e, pml4->used_pml4es)) {
 			pdp = alloc_pdp(dev);
 			if (IS_ERR(pdp))
@@ -1222,7 +1221,6 @@ static int gen8_alloc_va_range_3lvl(struct i915_address_space *vm,
 	struct i915_page_directory *pd;
 	const uint64_t orig_start = start;
 	const uint64_t orig_length = length;
-	uint64_t temp;
 	uint32_t pdpe;
 	uint32_t pdpes = I915_PDPES_PER_PDP(dev);
 	int ret;
@@ -1249,7 +1247,7 @@ static int gen8_alloc_va_range_3lvl(struct i915_address_space *vm,
 	}
 
 	/* For every page directory referenced, allocate page tables */
-	gen8_for_each_pdpe(pd, pdp, start, length, temp, pdpe) {
+	gen8_for_each_pdpe(pd, pdp, start, length, pdpe) {
 		ret = gen8_ppgtt_alloc_pagetabs(vm, pd, start, length,
 						new_page_tables + pdpe * BITS_TO_LONGS(I915_PDES));
 		if (ret)
@@ -1261,7 +1259,7 @@ static int gen8_alloc_va_range_3lvl(struct i915_address_space *vm,
 
 	/* Allocations have completed successfully, so set the bitmaps, and do
 	 * the mappings. */
-	gen8_for_each_pdpe(pd, pdp, start, length, temp, pdpe) {
+	gen8_for_each_pdpe(pd, pdp, start, length, pdpe) {
 		gen8_pde_t *const page_directory = kmap_px(pd);
 		struct i915_page_table *pt;
 		uint64_t pd_len = length;
@@ -1271,7 +1269,7 @@ static int gen8_alloc_va_range_3lvl(struct i915_address_space *vm,
 		/* Every pd should be allocated, we just did that above. */
 		WARN_ON(!pd);
 
-		gen8_for_each_pde(pt, pd, pd_start, pd_len, temp, pde) {
+		gen8_for_each_pde(pt, pd, pd_start, pd_len, pde) {
 			/* Same reasoning as pd */
 			WARN_ON(!pt);
 			WARN_ON(!pd_len);
@@ -1308,6 +1306,8 @@ static int gen8_alloc_va_range_3lvl(struct i915_address_space *vm,
 
 err_out:
 	while (pdpe--) {
+		unsigned long temp;
+
 		for_each_set_bit(temp, new_page_tables + pdpe *
 				BITS_TO_LONGS(I915_PDES), I915_PDES)
 			free_pt(dev, pdp->page_directory[pdpe]->page_table[temp]);
@@ -1330,7 +1330,7 @@ static int gen8_alloc_va_range_4lvl(struct i915_address_space *vm,
 	struct i915_hw_ppgtt *ppgtt =
 			container_of(vm, struct i915_hw_ppgtt, base);
 	struct i915_page_directory_pointer *pdp;
-	uint64_t temp, pml4e;
+	uint64_t pml4e;
 	int ret = 0;
 
 	/* Do the pml4 allocations first, so we don't need to track the newly
@@ -1349,7 +1349,7 @@ static int gen8_alloc_va_range_4lvl(struct i915_address_space *vm,
 	     "The allocation has spanned more than 512GB. "
 	     "It is highly likely this is incorrect.");
 
-	gen8_for_each_pml4e(pdp, pml4, start, length, temp, pml4e) {
+	gen8_for_each_pml4e(pdp, pml4, start, length, pml4e) {
 		WARN_ON(!pdp);
 
 		ret = gen8_alloc_va_range_3lvl(vm, pdp, start, length);
@@ -1389,10 +1389,9 @@ static void gen8_dump_pdp(struct i915_page_directory_pointer *pdp,
 			  struct seq_file *m)
 {
 	struct i915_page_directory *pd;
-	uint64_t temp;
 	uint32_t pdpe;
 
-	gen8_for_each_pdpe(pd, pdp, start, length, temp, pdpe) {
+	gen8_for_each_pdpe(pd, pdp, start, length, pdpe) {
 		struct i915_page_table *pt;
 		uint64_t pd_len = length;
 		uint64_t pd_start = start;
@@ -1402,7 +1401,7 @@ static void gen8_dump_pdp(struct i915_page_directory_pointer *pdp,
 			continue;
 
 		seq_printf(m, "\tPDPE #%d\n", pdpe);
-		gen8_for_each_pde(pt, pd, pd_start, pd_len, temp, pde) {
+		gen8_for_each_pde(pt, pd, pd_start, pd_len, pde) {
 			uint32_t  pte;
 			gen8_pte_t *pt_vaddr;
 
@@ -1452,11 +1451,11 @@ static void gen8_dump_ppgtt(struct i915_hw_ppgtt *ppgtt, struct seq_file *m)
 	if (!USES_FULL_48BIT_PPGTT(vm->dev)) {
 		gen8_dump_pdp(&ppgtt->pdp, start, length, scratch_pte, m);
 	} else {
-		uint64_t templ4, pml4e;
+		uint64_t pml4e;
 		struct i915_pml4 *pml4 = &ppgtt->pml4;
 		struct i915_page_directory_pointer *pdp;
 
-		gen8_for_each_pml4e(pdp, pml4, start, length, templ4, pml4e) {
+		gen8_for_each_pml4e(pdp, pml4, start, length, pml4e) {
 			if (!test_bit(pml4e, pml4->used_pml4es))
 				continue;
 
@@ -1662,9 +1661,9 @@ static int hsw_mm_switch(struct i915_hw_ppgtt *ppgtt,
 		return ret;
 
 	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(2));
-	intel_ring_emit(ring, RING_PP_DIR_DCLV(ring));
+	intel_ring_emit_reg(ring, RING_PP_DIR_DCLV(ring));
 	intel_ring_emit(ring, PP_DIR_DCLV_2G);
-	intel_ring_emit(ring, RING_PP_DIR_BASE(ring));
+	intel_ring_emit_reg(ring, RING_PP_DIR_BASE(ring));
 	intel_ring_emit(ring, get_pd_offset(ppgtt));
 	intel_ring_emit(ring, MI_NOOP);
 	intel_ring_advance(ring);
@@ -1699,9 +1698,9 @@ static int gen7_mm_switch(struct i915_hw_ppgtt *ppgtt,
 		return ret;
 
 	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(2));
-	intel_ring_emit(ring, RING_PP_DIR_DCLV(ring));
+	intel_ring_emit_reg(ring, RING_PP_DIR_DCLV(ring));
 	intel_ring_emit(ring, PP_DIR_DCLV_2G);
-	intel_ring_emit(ring, RING_PP_DIR_BASE(ring));
+	intel_ring_emit_reg(ring, RING_PP_DIR_BASE(ring));
 	intel_ring_emit(ring, get_pd_offset(ppgtt));
 	intel_ring_emit(ring, MI_NOOP);
 	intel_ring_advance(ring);
@@ -2352,6 +2351,9 @@ static void gen8_ggtt_insert_entries(struct i915_address_space *vm,
 	int i = 0;
 	struct sg_page_iter sg_iter;
 	dma_addr_t addr = 0; /* shut up gcc */
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
 	for_each_sg_page(st->sgl, &sg_iter, st->nents, 0) {
 		addr = sg_dma_address(sg_iter.sg) +
@@ -2378,6 +2380,34 @@ static void gen8_ggtt_insert_entries(struct i915_address_space *vm,
 	 */
 	I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
 	POSTING_READ(GFX_FLSH_CNTL_GEN6);
+
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
+}
+
+struct insert_entries {
+	struct i915_address_space *vm;
+	struct sg_table *st;
+	uint64_t start;
+	enum i915_cache_level level;
+	u32 flags;
+};
+
+static int gen8_ggtt_insert_entries__cb(void *_arg)
+{
+	struct insert_entries *arg = _arg;
+	gen8_ggtt_insert_entries(arg->vm, arg->st,
+				 arg->start, arg->level, arg->flags);
+	return 0;
+}
+
+static void gen8_ggtt_insert_entries__BKL(struct i915_address_space *vm,
+					  struct sg_table *st,
+					  uint64_t start,
+					  enum i915_cache_level level,
+					  u32 flags)
+{
+	struct insert_entries arg = { vm, st, start, level, flags };
+	stop_machine(gen8_ggtt_insert_entries__cb, &arg, NULL);
 }
 
 /*
@@ -2398,6 +2428,9 @@ static void gen6_ggtt_insert_entries(struct i915_address_space *vm,
 	int i = 0;
 	struct sg_page_iter sg_iter;
 	dma_addr_t addr = 0;
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
 	for_each_sg_page(st->sgl, &sg_iter, st->nents, 0) {
 		addr = sg_page_iter_dma_address(&sg_iter);
@@ -2422,6 +2455,8 @@ static void gen6_ggtt_insert_entries(struct i915_address_space *vm,
 	 */
 	I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
 	POSTING_READ(GFX_FLSH_CNTL_GEN6);
+
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
 }
 
 static void gen8_ggtt_clear_range(struct i915_address_space *vm,
@@ -2436,6 +2471,9 @@ static void gen8_ggtt_clear_range(struct i915_address_space *vm,
 		(gen8_pte_t __iomem *) dev_priv->gtt.gsm + first_entry;
 	const int max_entries = gtt_total_entries(dev_priv->gtt) - first_entry;
 	int i;
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
 	if (WARN(num_entries > max_entries,
 		 "First entry = %d; Num entries = %d (max=%d)\n",
@@ -2448,6 +2486,8 @@ static void gen8_ggtt_clear_range(struct i915_address_space *vm,
 	for (i = 0; i < num_entries; i++)
 		gen8_set_pte(&gtt_base[i], scratch_pte);
 	readl(gtt_base);
+
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
 }
 
 static void gen6_ggtt_clear_range(struct i915_address_space *vm,
@@ -2462,6 +2502,9 @@ static void gen6_ggtt_clear_range(struct i915_address_space *vm,
 		(gen6_pte_t __iomem *) dev_priv->gtt.gsm + first_entry;
 	const int max_entries = gtt_total_entries(dev_priv->gtt) - first_entry;
 	int i;
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
 	if (WARN(num_entries > max_entries,
 		 "First entry = %d; Num entries = %d (max=%d)\n",
@@ -2474,6 +2517,8 @@ static void gen6_ggtt_clear_range(struct i915_address_space *vm,
 	for (i = 0; i < num_entries; i++)
 		iowrite32(scratch_pte, &gtt_base[i]);
 	readl(gtt_base);
+
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
 }
 
 static void i915_ggtt_insert_entries(struct i915_address_space *vm,
@@ -2481,11 +2526,17 @@ static void i915_ggtt_insert_entries(struct i915_address_space *vm,
 				     uint64_t start,
 				     enum i915_cache_level cache_level, u32 unused)
 {
+	struct drm_i915_private *dev_priv = vm->dev->dev_private;
 	unsigned int flags = (cache_level == I915_CACHE_NONE) ?
 		AGP_USER_MEMORY : AGP_USER_CACHED_MEMORY;
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
 
 	intel_gtt_insert_sg_entries(pages, start >> PAGE_SHIFT, flags);
 
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
+
 }
 
 static void i915_ggtt_clear_range(struct i915_address_space *vm,
@@ -2493,9 +2544,16 @@ static void i915_ggtt_clear_range(struct i915_address_space *vm,
 				  uint64_t length,
 				  bool unused)
 {
+	struct drm_i915_private *dev_priv = vm->dev->dev_private;
 	unsigned first_entry = start >> PAGE_SHIFT;
 	unsigned num_entries = length >> PAGE_SHIFT;
+	int rpm_atomic_seq;
+
+	rpm_atomic_seq = assert_rpm_atomic_begin(dev_priv);
+
 	intel_gtt_clear_range(first_entry, num_entries);
+
+	assert_rpm_atomic_end(dev_priv, rpm_atomic_seq);
 }
 
 static int ggtt_bind_vma(struct i915_vma *vma,
@@ -2996,6 +3054,9 @@ static int gen8_gmch_probe(struct drm_device *dev,
 	dev_priv->gtt.base.bind_vma = ggtt_bind_vma;
 	dev_priv->gtt.base.unbind_vma = ggtt_unbind_vma;
 
+	if (IS_CHERRYVIEW(dev_priv))
+		dev_priv->gtt.base.insert_entries = gen8_ggtt_insert_entries__BKL;
+
 	return ret;
 }
 
@@ -3303,7 +3364,7 @@ static struct sg_table *
 intel_rotate_fb_obj_pages(struct i915_ggtt_view *ggtt_view,
 			  struct drm_i915_gem_object *obj)
 {
-	struct intel_rotation_info *rot_info = &ggtt_view->rotation_info;
+	struct intel_rotation_info *rot_info = &ggtt_view->params.rotation_info;
 	unsigned int size_pages = rot_info->size >> PAGE_SHIFT;
 	unsigned int size_pages_uv;
 	struct sg_page_iter sg_iter;
@@ -3535,7 +3596,7 @@ i915_ggtt_view_size(struct drm_i915_gem_object *obj,
 	if (view->type == I915_GGTT_VIEW_NORMAL) {
 		return obj->base.size;
 	} else if (view->type == I915_GGTT_VIEW_ROTATED) {
-		return view->rotation_info.size;
+		return view->params.rotation_info.size;
 	} else if (view->type == I915_GGTT_VIEW_PARTIAL) {
 		return view->params.partial.size << PAGE_SHIFT;
 	} else {
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.h b/drivers/gpu/drm/i915/i915_gem_gtt.h
index a216397ead52..b448ad832dcf 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.h
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.h
@@ -156,13 +156,10 @@ struct i915_ggtt_view {
 			u64 offset;
 			unsigned int size;
 		} partial;
+		struct intel_rotation_info rotation_info;
 	} params;
 
 	struct sg_table *pages;
-
-	union {
-		struct intel_rotation_info rotation_info;
-	};
 };
 
 extern const struct i915_ggtt_view i915_ggtt_view_normal;
@@ -458,32 +455,29 @@ static inline uint32_t gen6_pde_index(uint32_t addr)
  * between from start until start + length. On gen8+ it simply iterates
  * over every page directory entry in a page directory.
  */
-#define gen8_for_each_pde(pt, pd, start, length, temp, iter)		\
-	for (iter = gen8_pde_index(start); \
-	     length > 0 && iter < I915_PDES ? \
-			(pt = (pd)->page_table[iter]), 1 : 0; \
-	     iter++,				\
-	     temp = ALIGN(start+1, 1 << GEN8_PDE_SHIFT) - start,	\
-	     temp = min(temp, length),					\
-	     start += temp, length -= temp)
-
-#define gen8_for_each_pdpe(pd, pdp, start, length, temp, iter)	\
-	for (iter = gen8_pdpe_index(start); \
-	     length > 0 && (iter < I915_PDPES_PER_PDP(dev)) ? \
-			(pd = (pdp)->page_directory[iter]), 1 : 0; \
-	     iter++,				\
-	     temp = ALIGN(start+1, 1 << GEN8_PDPE_SHIFT) - start,	\
-	     temp = min(temp, length),					\
-	     start += temp, length -= temp)
-
-#define gen8_for_each_pml4e(pdp, pml4, start, length, temp, iter)	\
-	for (iter = gen8_pml4e_index(start);	\
-	     length > 0 && iter < GEN8_PML4ES_PER_PML4 ? \
-			(pdp = (pml4)->pdps[iter]), 1 : 0; \
-	     iter++,				\
-	     temp = ALIGN(start+1, 1ULL << GEN8_PML4E_SHIFT) - start,	\
-	     temp = min(temp, length),					\
-	     start += temp, length -= temp)
+#define gen8_for_each_pde(pt, pd, start, length, iter)			\
+	for (iter = gen8_pde_index(start);				\
+	     length > 0 && iter < I915_PDES &&				\
+		(pt = (pd)->page_table[iter], true);			\
+	     ({ u64 temp = ALIGN(start+1, 1 << GEN8_PDE_SHIFT);		\
+		    temp = min(temp - start, length);			\
+		    start += temp, length -= temp; }), ++iter)
+
+#define gen8_for_each_pdpe(pd, pdp, start, length, iter)		\
+	for (iter = gen8_pdpe_index(start);				\
+	     length > 0 && iter < I915_PDPES_PER_PDP(dev) &&		\
+		(pd = (pdp)->page_directory[iter], true);		\
+	     ({ u64 temp = ALIGN(start+1, 1 << GEN8_PDPE_SHIFT);	\
+		    temp = min(temp - start, length);			\
+		    start += temp, length -= temp; }), ++iter)
+
+#define gen8_for_each_pml4e(pdp, pml4, start, length, iter)		\
+	for (iter = gen8_pml4e_index(start);				\
+	     length > 0 && iter < GEN8_PML4ES_PER_PML4 &&		\
+		(pdp = (pml4)->pdps[iter], true);			\
+	     ({ u64 temp = ALIGN(start+1, 1ULL << GEN8_PML4E_SHIFT);	\
+		    temp = min(temp - start, length);			\
+		    start += temp, length -= temp; }), ++iter)
 
 static inline uint32_t gen8_pte_index(uint64_t address)
 {
@@ -556,7 +550,7 @@ i915_ggtt_view_equal(const struct i915_ggtt_view *a,
 
 	if (a->type != b->type)
 		return false;
-	if (a->type == I915_GGTT_VIEW_PARTIAL)
+	if (a->type != I915_GGTT_VIEW_NORMAL)
 		return !memcmp(&a->params, &b->params, sizeof(a->params));
 	return true;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_render_state.c b/drivers/gpu/drm/i915/i915_gem_render_state.c
index 5026a6267a88..fc7e6d5c6251 100644
--- a/drivers/gpu/drm/i915/i915_gem_render_state.c
+++ b/drivers/gpu/drm/i915/i915_gem_render_state.c
@@ -103,7 +103,7 @@ static int render_state_setup(struct render_state *so)
 	if (ret)
 		return ret;
 
-	page = sg_page(so->obj->pages->sgl);
+	page = i915_gem_object_get_dirty_page(so->obj, 0);
 	d = kmap(page);
 
 	while (i < rodata->batch_items) {
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index 87e919a06b27..3476877fc0d6 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -433,7 +433,8 @@ int i915_gem_init_stolen(struct drm_device *dev)
 					 &reserved_size);
 		break;
 	default:
-		if (IS_BROADWELL(dev_priv) || IS_SKYLAKE(dev_priv))
+		if (IS_BROADWELL(dev_priv) ||
+		    IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev))
 			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 8a6717cc265c..7410f6c962e7 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -176,6 +176,8 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 		return -EINVAL;
 	}
 
+	intel_runtime_pm_get(dev_priv);
+
 	mutex_lock(&dev->struct_mutex);
 	if (obj->pin_display || obj->framebuffer_references) {
 		ret = -EBUSY;
@@ -269,6 +271,8 @@ err:
 	drm_gem_object_unreference(&obj->base);
 	mutex_unlock(&dev->struct_mutex);
 
+	intel_runtime_pm_put(dev_priv);
+
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/i915/i915_gem_userptr.c b/drivers/gpu/drm/i915/i915_gem_userptr.c
index 19fb0bddc1cd..59e45b3a6937 100644
--- a/drivers/gpu/drm/i915/i915_gem_userptr.c
+++ b/drivers/gpu/drm/i915/i915_gem_userptr.c
@@ -789,9 +789,10 @@ i915_gem_userptr_dmabuf_export(struct drm_i915_gem_object *obj)
 }
 
 static const struct drm_i915_gem_object_ops i915_gem_userptr_ops = {
-	.dmabuf_export = i915_gem_userptr_dmabuf_export,
+	.flags = I915_GEM_OBJECT_HAS_STRUCT_PAGE,
 	.get_pages = i915_gem_userptr_get_pages,
 	.put_pages = i915_gem_userptr_put_pages,
+	.dmabuf_export = i915_gem_userptr_dmabuf_export,
 	.release = i915_gem_userptr_release,
 };
 
diff --git a/drivers/gpu/drm/i915/i915_gpu_error.c b/drivers/gpu/drm/i915/i915_gpu_error.c
index 2f04e4f2ff35..06ca4082735b 100644
--- a/drivers/gpu/drm/i915/i915_gpu_error.c
+++ b/drivers/gpu/drm/i915/i915_gpu_error.c
@@ -366,6 +366,17 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 	err_printf(m, "Suspend count: %u\n", error->suspend_count);
 	err_printf(m, "PCI ID: 0x%04x\n", dev->pdev->device);
 	err_printf(m, "IOMMU enabled?: %d\n", error->iommu);
+
+	if (HAS_CSR(dev)) {
+		struct intel_csr *csr = &dev_priv->csr;
+
+		err_printf(m, "DMC loaded: %s\n",
+			   yesno(csr->dmc_payload != NULL));
+		err_printf(m, "DMC fw version: %d.%d\n",
+			   CSR_VERSION_MAJOR(csr->version),
+			   CSR_VERSION_MINOR(csr->version));
+	}
+
 	err_printf(m, "EIR: 0x%08x\n", error->eir);
 	err_printf(m, "IER: 0x%08x\n", error->ier);
 	if (INTEL_INFO(dev)->gen >= 8) {
@@ -862,7 +873,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (INTEL_INFO(dev)->gen >= 6) {
-		ering->rc_psmi = I915_READ(ring->mmio_base + 0x50);
+		ering->rc_psmi = I915_READ(RING_PSMI_CTL(ring->mmio_base));
 		ering->fault_reg = I915_READ(RING_FAULT_REG(ring));
 		if (INTEL_INFO(dev)->gen >= 8)
 			gen8_record_semaphore_state(dev_priv, error, ring, ering);
@@ -899,7 +910,7 @@ static void i915_record_ring_state(struct drm_device *dev,
 	ering->ctl = I915_READ_CTL(ring);
 
 	if (I915_NEED_GFX_HWS(dev)) {
-		int mmio;
+		i915_reg_t mmio;
 
 		if (IS_GEN7(dev)) {
 			switch (ring->id) {
@@ -1071,6 +1082,25 @@ static void i915_gem_record_rings(struct drm_device *dev,
 		list_for_each_entry(request, &ring->request_list, list) {
 			struct drm_i915_error_request *erq;
 
+			if (count >= error->ring[i].num_requests) {
+				/*
+				 * If the ring request list was changed in
+				 * between the point where the error request
+				 * list was created and dimensioned and this
+				 * point then just exit early to avoid crashes.
+				 *
+				 * We don't need to communicate that the
+				 * request list changed state during error
+				 * state capture and that the error state is
+				 * slightly incorrect as a consequence since we
+				 * are typically only interested in the request
+				 * list state at the point of error state
+				 * capture, not in any changes happening during
+				 * the capture.
+				 */
+				break;
+			}
+
 			erq = &error->ring[i].requests[count++];
 			erq->seqno = request->seqno;
 			erq->jiffies = request->emitted_jiffies;
@@ -1181,7 +1211,7 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 	if (IS_VALLEYVIEW(dev)) {
 		error->gtier[0] = I915_READ(GTIER);
 		error->ier = I915_READ(VLV_IER);
-		error->forcewake = I915_READ(FORCEWAKE_VLV);
+		error->forcewake = I915_READ_FW(FORCEWAKE_VLV);
 	}
 
 	if (IS_GEN7(dev))
@@ -1193,14 +1223,14 @@ static void i915_capture_reg_state(struct drm_i915_private *dev_priv,
 	}
 
 	if (IS_GEN6(dev)) {
-		error->forcewake = I915_READ(FORCEWAKE);
+		error->forcewake = I915_READ_FW(FORCEWAKE);
 		error->gab_ctl = I915_READ(GAB_CTL);
 		error->gfx_mode = I915_READ(GFX_MODE);
 	}
 
 	/* 2: Registers which belong to multiple generations */
 	if (INTEL_INFO(dev)->gen >= 7)
-		error->forcewake = I915_READ(FORCEWAKE_MT);
+		error->forcewake = I915_READ_FW(FORCEWAKE_MT);
 
 	if (INTEL_INFO(dev)->gen >= 6) {
 		error->derrmr = I915_READ(DERRMR);
diff --git a/drivers/gpu/drm/i915/i915_guc_reg.h b/drivers/gpu/drm/i915/i915_guc_reg.h
index c4cb1c0c4d0d..685c7991e24f 100644
--- a/drivers/gpu/drm/i915/i915_guc_reg.h
+++ b/drivers/gpu/drm/i915/i915_guc_reg.h
@@ -26,7 +26,7 @@
 
 /* Definitions of GuC H/W registers, bits, etc */
 
-#define GUC_STATUS			0xc000
+#define GUC_STATUS			_MMIO(0xc000)
 #define   GS_BOOTROM_SHIFT		1
 #define   GS_BOOTROM_MASK		  (0x7F << GS_BOOTROM_SHIFT)
 #define   GS_BOOTROM_RSA_FAILED		  (0x50 << GS_BOOTROM_SHIFT)
@@ -39,40 +39,41 @@
 #define   GS_MIA_MASK			  (0x07 << GS_MIA_SHIFT)
 #define   GS_MIA_CORE_STATE		  (1 << GS_MIA_SHIFT)
 
-#define SOFT_SCRATCH(n)			(0xc180 + ((n) * 4))
+#define SOFT_SCRATCH(n)			_MMIO(0xc180 + (n) * 4)
 
-#define UOS_RSA_SCRATCH(i)		(0xc200 + (i) * 4)
-#define DMA_ADDR_0_LOW			0xc300
-#define DMA_ADDR_0_HIGH			0xc304
-#define DMA_ADDR_1_LOW			0xc308
-#define DMA_ADDR_1_HIGH			0xc30c
+#define UOS_RSA_SCRATCH(i)		_MMIO(0xc200 + (i) * 4)
+#define   UOS_RSA_SCRATCH_MAX_COUNT	  64
+#define DMA_ADDR_0_LOW			_MMIO(0xc300)
+#define DMA_ADDR_0_HIGH			_MMIO(0xc304)
+#define DMA_ADDR_1_LOW			_MMIO(0xc308)
+#define DMA_ADDR_1_HIGH			_MMIO(0xc30c)
 #define   DMA_ADDRESS_SPACE_WOPCM	  (7 << 16)
 #define   DMA_ADDRESS_SPACE_GTT		  (8 << 16)
-#define DMA_COPY_SIZE			0xc310
-#define DMA_CTRL			0xc314
+#define DMA_COPY_SIZE			_MMIO(0xc310)
+#define DMA_CTRL			_MMIO(0xc314)
 #define   UOS_MOVE			  (1<<4)
 #define   START_DMA			  (1<<0)
-#define DMA_GUC_WOPCM_OFFSET		0xc340
+#define DMA_GUC_WOPCM_OFFSET		_MMIO(0xc340)
 #define   GUC_WOPCM_OFFSET_VALUE	  0x80000	/* 512KB */
-#define GUC_MAX_IDLE_COUNT		0xC3E4
+#define GUC_MAX_IDLE_COUNT		_MMIO(0xC3E4)
 
-#define GUC_WOPCM_SIZE			0xc050
+#define GUC_WOPCM_SIZE			_MMIO(0xc050)
 #define   GUC_WOPCM_SIZE_VALUE  	  (0x80 << 12)	/* 512KB */
 
 /* GuC addresses below GUC_WOPCM_TOP don't map through the GTT */
 #define	GUC_WOPCM_TOP			(GUC_WOPCM_SIZE_VALUE)
 
-#define GEN8_GT_PM_CONFIG		0x138140
-#define GEN9LP_GT_PM_CONFIG		0x138140
-#define GEN9_GT_PM_CONFIG		0x13816c
+#define GEN8_GT_PM_CONFIG		_MMIO(0x138140)
+#define GEN9LP_GT_PM_CONFIG		_MMIO(0x138140)
+#define GEN9_GT_PM_CONFIG		_MMIO(0x13816c)
 #define   GT_DOORBELL_ENABLE		  (1<<0)
 
-#define GEN8_GTCR			0x4274
+#define GEN8_GTCR			_MMIO(0x4274)
 #define   GEN8_GTCR_INVALIDATE		  (1<<0)
 
-#define GUC_ARAT_C6DIS			0xA178
+#define GUC_ARAT_C6DIS			_MMIO(0xA178)
 
-#define GUC_SHIM_CONTROL		0xc064
+#define GUC_SHIM_CONTROL		_MMIO(0xc064)
 #define   GUC_DISABLE_SRAM_INIT_TO_ZEROES	(1<<0)
 #define   GUC_ENABLE_READ_CACHE_LOGIC		(1<<1)
 #define   GUC_ENABLE_MIA_CACHING		(1<<2)
@@ -89,21 +90,21 @@
 				 GUC_ENABLE_READ_CACHE_FOR_WOPCM_DATA	| \
 				 GUC_ENABLE_MIA_CLOCK_GATING)
 
-#define HOST2GUC_INTERRUPT		0xc4c8
+#define HOST2GUC_INTERRUPT		_MMIO(0xc4c8)
 #define   HOST2GUC_TRIGGER		  (1<<0)
 
 #define DRBMISC1			0x1984
 #define   DOORBELL_ENABLE		  (1<<0)
 
-#define GEN8_DRBREGL(x)			(0x1000 + (x) * 8)
+#define GEN8_DRBREGL(x)			_MMIO(0x1000 + (x) * 8)
 #define   GEN8_DRB_VALID		  (1<<0)
-#define GEN8_DRBREGU(x)			(GEN8_DRBREGL(x) + 4)
+#define GEN8_DRBREGU(x)			_MMIO(0x1000 + (x) * 8 + 4)
 
-#define DE_GUCRMR			0x44054
+#define DE_GUCRMR			_MMIO(0x44054)
 
-#define GUC_BCS_RCS_IER			0xC550
-#define GUC_VCS2_VCS1_IER		0xC554
-#define GUC_WD_VECS_IER			0xC558
-#define GUC_PM_P24C_IER			0xC55C
+#define GUC_BCS_RCS_IER			_MMIO(0xC550)
+#define GUC_VCS2_VCS1_IER		_MMIO(0xC554)
+#define GUC_WD_VECS_IER			_MMIO(0xC558)
+#define GUC_PM_P24C_IER			_MMIO(0xC55C)
 
 #endif
diff --git a/drivers/gpu/drm/i915/i915_guc_submission.c b/drivers/gpu/drm/i915/i915_guc_submission.c
index 036b42bae827..05aa7e61cbe0 100644
--- a/drivers/gpu/drm/i915/i915_guc_submission.c
+++ b/drivers/gpu/drm/i915/i915_guc_submission.c
@@ -27,7 +27,7 @@
 #include "intel_guc.h"
 
 /**
- * DOC: GuC Client
+ * DOC: GuC-based command submission
  *
  * i915_guc_client:
  * We use the term client to avoid confusion with contexts. A i915_guc_client is
@@ -86,7 +86,6 @@ static int host2guc_action(struct intel_guc *guc, u32 *data, u32 len)
 		return -EINVAL;
 
 	intel_uncore_forcewake_get(dev_priv, FORCEWAKE_ALL);
-	spin_lock(&dev_priv->guc.host2guc_lock);
 
 	dev_priv->guc.action_count += 1;
 	dev_priv->guc.action_cmd = data[0];
@@ -119,7 +118,6 @@ static int host2guc_action(struct intel_guc *guc, u32 *data, u32 len)
 	}
 	dev_priv->guc.action_status = status;
 
-	spin_unlock(&dev_priv->guc.host2guc_lock);
 	intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 
 	return ret;
@@ -161,9 +159,9 @@ static int host2guc_sample_forcewake(struct intel_guc *guc,
 	data[0] = HOST2GUC_ACTION_SAMPLE_FORCEWAKE;
 	/* WaRsDisableCoarsePowerGating:skl,bxt */
 	if (!intel_enable_rc6(dev_priv->dev) ||
-	    (IS_BROXTON(dev) && (INTEL_REVID(dev) < BXT_REVID_B0)) ||
-	    (IS_SKL_GT3(dev) && (INTEL_REVID(dev) <= SKL_REVID_E0)) ||
-	    (IS_SKL_GT4(dev) && (INTEL_REVID(dev) <= SKL_REVID_E0)))
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1) ||
+	    (IS_SKL_GT3(dev) && IS_SKL_REVID(dev, 0, SKL_REVID_E0)) ||
+	    (IS_SKL_GT4(dev) && IS_SKL_REVID(dev, 0, SKL_REVID_E0)))
 		data[1] = 0;
 	else
 		/* bit 0 and 1 are for Render and Media domain separately */
@@ -258,7 +256,7 @@ static void guc_disable_doorbell(struct intel_guc *guc,
 	struct drm_i915_private *dev_priv = guc_to_i915(guc);
 	struct guc_doorbell_info *doorbell;
 	void *base;
-	int drbreg = GEN8_DRBREGL(client->doorbell_id);
+	i915_reg_t drbreg = GEN8_DRBREGL(client->doorbell_id);
 	int value;
 
 	base = kmap_atomic(i915_gem_object_get_page(client->client_obj, 0));
@@ -292,16 +290,12 @@ static uint32_t select_doorbell_cacheline(struct intel_guc *guc)
 	const uint32_t cacheline_size = cache_line_size();
 	uint32_t offset;
 
-	spin_lock(&guc->host2guc_lock);
-
 	/* Doorbell uses a single cache line within a page */
 	offset = offset_in_page(guc->db_cacheline);
 
 	/* Moving to next cache line to reduce contention */
 	guc->db_cacheline += cacheline_size;
 
-	spin_unlock(&guc->host2guc_lock);
-
 	DRM_DEBUG_DRIVER("selected doorbell cacheline 0x%x, next 0x%x, linesize %u\n",
 			offset, guc->db_cacheline, cacheline_size);
 
@@ -322,13 +316,11 @@ static uint16_t assign_doorbell(struct intel_guc *guc, uint32_t priority)
 	const uint16_t end = start + half;
 	uint16_t id;
 
-	spin_lock(&guc->host2guc_lock);
 	id = find_next_zero_bit(guc->doorbell_bitmap, end, start);
 	if (id == end)
 		id = GUC_INVALID_DOORBELL_ID;
 	else
 		bitmap_set(guc->doorbell_bitmap, id, 1);
-	spin_unlock(&guc->host2guc_lock);
 
 	DRM_DEBUG_DRIVER("assigned %s priority doorbell id 0x%x\n",
 			hi_pri ? "high" : "normal", id);
@@ -338,9 +330,7 @@ static uint16_t assign_doorbell(struct intel_guc *guc, uint32_t priority)
 
 static void release_doorbell(struct intel_guc *guc, uint16_t id)
 {
-	spin_lock(&guc->host2guc_lock);
 	bitmap_clear(guc->doorbell_bitmap, id, 1);
-	spin_unlock(&guc->host2guc_lock);
 }
 
 /*
@@ -487,16 +477,13 @@ static int guc_get_workqueue_space(struct i915_guc_client *gc, u32 *offset)
 	struct guc_process_desc *desc;
 	void *base;
 	u32 size = sizeof(struct guc_wq_item);
-	int ret = 0, timeout_counter = 200;
+	int ret = -ETIMEDOUT, timeout_counter = 200;
 
 	base = kmap_atomic(i915_gem_object_get_page(gc->client_obj, 0));
 	desc = base + gc->proc_desc_offset;
 
 	while (timeout_counter-- > 0) {
-		ret = wait_for_atomic(CIRC_SPACE(gc->wq_tail, desc->head,
-				gc->wq_size) >= size, 1);
-
-		if (!ret) {
+		if (CIRC_SPACE(gc->wq_tail, desc->head, gc->wq_size) >= size) {
 			*offset = gc->wq_tail;
 
 			/* advance the tail for next workqueue item */
@@ -505,7 +492,11 @@ static int guc_get_workqueue_space(struct i915_guc_client *gc, u32 *offset)
 
 			/* this will break the loop */
 			timeout_counter = 0;
+			ret = 0;
 		}
+
+		if (timeout_counter)
+			usleep_range(1000, 2000);
 	};
 
 	kunmap_atomic(base);
@@ -577,7 +568,7 @@ static void lr_context_update(struct drm_i915_gem_request *rq)
 	WARN_ON(!i915_gem_obj_is_pinned(ctx_obj));
 	WARN_ON(!i915_gem_obj_is_pinned(rb_obj));
 
-	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	page = i915_gem_object_get_dirty_page(ctx_obj, LRC_STATE_PN);
 	reg_state = kmap_atomic(page);
 
 	reg_state[CTX_RING_BUFFER_START+1] = i915_gem_obj_ggtt_offset(rb_obj);
@@ -588,8 +579,7 @@ static void lr_context_update(struct drm_i915_gem_request *rq)
 /**
  * i915_guc_submit() - Submit commands through GuC
  * @client:	the guc client where commands will go through
- * @ctx:	LRC where commands come from
- * @ring:	HW engine that will excute the commands
+ * @rq:		request associated with the commands
  *
  * Return:	0 if succeed
  */
@@ -598,15 +588,12 @@ int i915_guc_submit(struct i915_guc_client *client,
 {
 	struct intel_guc *guc = client->guc;
 	enum intel_ring_id ring_id = rq->ring->id;
-	unsigned long flags;
 	int q_ret, b_ret;
 
 	/* Need this because of the deferred pin ctx and ring */
 	/* Shall we move this right after ring is pinned? */
 	lr_context_update(rq);
 
-	spin_lock_irqsave(&client->wq_lock, flags);
-
 	q_ret = guc_add_workqueue_item(client, rq);
 	if (q_ret == 0)
 		b_ret = guc_ring_doorbell(client);
@@ -621,12 +608,8 @@ int i915_guc_submit(struct i915_guc_client *client,
 	} else {
 		client->retcode = 0;
 	}
-	spin_unlock_irqrestore(&client->wq_lock, flags);
-
-	spin_lock(&guc->host2guc_lock);
 	guc->submissions[ring_id] += 1;
 	guc->last_seqno[ring_id] = rq->seqno;
-	spin_unlock(&guc->host2guc_lock);
 
 	return q_ret;
 }
@@ -678,7 +661,7 @@ static struct drm_i915_gem_object *gem_allocate_guc_obj(struct drm_device *dev,
 /**
  * gem_release_guc_obj() - Release gem object allocated for GuC usage
  * @obj:	gem obj to be released
-  */
+ */
 static void gem_release_guc_obj(struct drm_i915_gem_object *obj)
 {
 	if (!obj)
@@ -731,7 +714,8 @@ static void guc_client_free(struct drm_device *dev,
  * 		The kernel client to replace ExecList submission is created with
  * 		NORMAL priority. Priority of a client for scheduler can be HIGH,
  * 		while a preemption context can use CRITICAL.
- * @ctx		the context to own the client (we use the default render context)
+ * @ctx:	the context that owns the client (we use the default render
+ * 		context)
  *
  * Return:	An i915_guc_client object if success.
  */
@@ -768,7 +752,6 @@ static struct i915_guc_client *guc_client_alloc(struct drm_device *dev,
 	client->client_obj = obj;
 	client->wq_offset = GUC_DB_SIZE;
 	client->wq_size = GUC_WQ_SIZE;
-	spin_lock_init(&client->wq_lock);
 
 	client->doorbell_offset = select_doorbell_cacheline(guc);
 
@@ -871,8 +854,6 @@ int i915_guc_submission_init(struct drm_device *dev)
 	if (!guc->ctx_pool_obj)
 		return -ENOMEM;
 
-	spin_lock_init(&dev_priv->guc.host2guc_lock);
-
 	ida_init(&guc->ctx_ids);
 
 	guc_create_log(guc);
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 0d228f909dcb..fa8afa7860ae 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -139,7 +139,8 @@ static const u32 hpd_bxt[HPD_NUM_PINS] = {
 /*
  * We should clear IMR at preinstall/uninstall, and just check at postinstall.
  */
-static void gen5_assert_iir_is_zero(struct drm_i915_private *dev_priv, u32 reg)
+static void gen5_assert_iir_is_zero(struct drm_i915_private *dev_priv,
+				    i915_reg_t reg)
 {
 	u32 val = I915_READ(reg);
 
@@ -147,7 +148,7 @@ static void gen5_assert_iir_is_zero(struct drm_i915_private *dev_priv, u32 reg)
 		return;
 
 	WARN(1, "Interrupt register 0x%x is not zero: 0x%08x\n",
-	     reg, val);
+	     i915_mmio_reg_offset(reg), val);
 	I915_WRITE(reg, 0xffffffff);
 	POSTING_READ(reg);
 	I915_WRITE(reg, 0xffffffff);
@@ -214,9 +215,9 @@ void i915_hotplug_interrupt_update(struct drm_i915_private *dev_priv,
  * @interrupt_mask: mask of interrupt bits to update
  * @enabled_irq_mask: mask of interrupt bits to enable
  */
-static void ilk_update_display_irq(struct drm_i915_private *dev_priv,
-				   uint32_t interrupt_mask,
-				   uint32_t enabled_irq_mask)
+void ilk_update_display_irq(struct drm_i915_private *dev_priv,
+			    uint32_t interrupt_mask,
+			    uint32_t enabled_irq_mask)
 {
 	uint32_t new_val;
 
@@ -238,18 +239,6 @@ static void ilk_update_display_irq(struct drm_i915_private *dev_priv,
 	}
 }
 
-void
-ironlake_enable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
-{
-	ilk_update_display_irq(dev_priv, mask, mask);
-}
-
-void
-ironlake_disable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
-{
-	ilk_update_display_irq(dev_priv, mask, 0);
-}
-
 /**
  * ilk_update_gt_irq - update GTIMR
  * @dev_priv: driver private
@@ -283,27 +272,27 @@ void gen5_disable_gt_irq(struct drm_i915_private *dev_priv, uint32_t mask)
 	ilk_update_gt_irq(dev_priv, mask, 0);
 }
 
-static u32 gen6_pm_iir(struct drm_i915_private *dev_priv)
+static i915_reg_t gen6_pm_iir(struct drm_i915_private *dev_priv)
 {
 	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IIR(2) : GEN6_PMIIR;
 }
 
-static u32 gen6_pm_imr(struct drm_i915_private *dev_priv)
+static i915_reg_t gen6_pm_imr(struct drm_i915_private *dev_priv)
 {
 	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IMR(2) : GEN6_PMIMR;
 }
 
-static u32 gen6_pm_ier(struct drm_i915_private *dev_priv)
+static i915_reg_t gen6_pm_ier(struct drm_i915_private *dev_priv)
 {
 	return INTEL_INFO(dev_priv)->gen >= 8 ? GEN8_GT_IER(2) : GEN6_PMIER;
 }
 
 /**
-  * snb_update_pm_irq - update GEN6_PMIMR
-  * @dev_priv: driver private
-  * @interrupt_mask: mask of interrupt bits to update
-  * @enabled_irq_mask: mask of interrupt bits to enable
-  */
+ * snb_update_pm_irq - update GEN6_PMIMR
+ * @dev_priv: driver private
+ * @interrupt_mask: mask of interrupt bits to update
+ * @enabled_irq_mask: mask of interrupt bits to enable
+ */
 static void snb_update_pm_irq(struct drm_i915_private *dev_priv,
 			      uint32_t interrupt_mask,
 			      uint32_t enabled_irq_mask)
@@ -350,7 +339,7 @@ void gen6_disable_pm_irq(struct drm_i915_private *dev_priv, uint32_t mask)
 void gen6_reset_rps_interrupts(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t reg = gen6_pm_iir(dev_priv);
+	i915_reg_t reg = gen6_pm_iir(dev_priv);
 
 	spin_lock_irq(&dev_priv->irq_lock);
 	I915_WRITE(reg, dev_priv->pm_rps_events);
@@ -417,11 +406,11 @@ void gen6_disable_rps_interrupts(struct drm_device *dev)
 }
 
 /**
-  * bdw_update_port_irq - update DE port interrupt
-  * @dev_priv: driver private
-  * @interrupt_mask: mask of interrupt bits to update
-  * @enabled_irq_mask: mask of interrupt bits to enable
-  */
+ * bdw_update_port_irq - update DE port interrupt
+ * @dev_priv: driver private
+ * @interrupt_mask: mask of interrupt bits to update
+ * @enabled_irq_mask: mask of interrupt bits to enable
+ */
 static void bdw_update_port_irq(struct drm_i915_private *dev_priv,
 				uint32_t interrupt_mask,
 				uint32_t enabled_irq_mask)
@@ -449,6 +438,38 @@ static void bdw_update_port_irq(struct drm_i915_private *dev_priv,
 }
 
 /**
+ * bdw_update_pipe_irq - update DE pipe interrupt
+ * @dev_priv: driver private
+ * @pipe: pipe whose interrupt to update
+ * @interrupt_mask: mask of interrupt bits to update
+ * @enabled_irq_mask: mask of interrupt bits to enable
+ */
+void bdw_update_pipe_irq(struct drm_i915_private *dev_priv,
+			 enum pipe pipe,
+			 uint32_t interrupt_mask,
+			 uint32_t enabled_irq_mask)
+{
+	uint32_t new_val;
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	WARN_ON(enabled_irq_mask & ~interrupt_mask);
+
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
+		return;
+
+	new_val = dev_priv->de_irq_mask[pipe];
+	new_val &= ~interrupt_mask;
+	new_val |= (~enabled_irq_mask & interrupt_mask);
+
+	if (new_val != dev_priv->de_irq_mask[pipe]) {
+		dev_priv->de_irq_mask[pipe] = new_val;
+		I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
+		POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+	}
+}
+
+/**
  * ibx_display_interrupt_update - update SDEIMR
  * @dev_priv: driver private
  * @interrupt_mask: mask of interrupt bits to update
@@ -477,7 +498,7 @@ static void
 __i915_enable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 		       u32 enable_mask, u32 status_mask)
 {
-	u32 reg = PIPESTAT(pipe);
+	i915_reg_t reg = PIPESTAT(pipe);
 	u32 pipestat = I915_READ(reg) & PIPESTAT_INT_ENABLE_MASK;
 
 	assert_spin_locked(&dev_priv->irq_lock);
@@ -504,7 +525,7 @@ static void
 __i915_disable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 		        u32 enable_mask, u32 status_mask)
 {
-	u32 reg = PIPESTAT(pipe);
+	i915_reg_t reg = PIPESTAT(pipe);
 	u32 pipestat = I915_READ(reg) & PIPESTAT_INT_ENABLE_MASK;
 
 	assert_spin_locked(&dev_priv->irq_lock);
@@ -560,7 +581,7 @@ i915_enable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 {
 	u32 enable_mask;
 
-	if (IS_VALLEYVIEW(dev_priv->dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		enable_mask = vlv_get_pipestat_enable_mask(dev_priv->dev,
 							   status_mask);
 	else
@@ -574,7 +595,7 @@ i915_disable_pipestat(struct drm_i915_private *dev_priv, enum pipe pipe,
 {
 	u32 enable_mask;
 
-	if (IS_VALLEYVIEW(dev_priv->dev))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		enable_mask = vlv_get_pipestat_enable_mask(dev_priv->dev,
 							   status_mask);
 	else
@@ -665,8 +686,7 @@ static u32 i8xx_get_vblank_counter(struct drm_device *dev, unsigned int pipe)
 static u32 i915_get_vblank_counter(struct drm_device *dev, unsigned int pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	unsigned long high_frame;
-	unsigned long low_frame;
+	i915_reg_t high_frame, low_frame;
 	u32 high1, high2, low, pixel, vbl_start, hsync_start, htotal;
 	struct intel_crtc *intel_crtc =
 		to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
@@ -717,9 +737,7 @@ static u32 g4x_get_vblank_counter(struct drm_device *dev, unsigned int pipe)
 	return I915_READ(PIPE_FRMCOUNT_G4X(pipe));
 }
 
-/* raw reads, only for fast reads of display block, no need for forcewake etc. */
-#define __raw_i915_read32(dev_priv__, reg__) readl((dev_priv__)->regs + (reg__))
-
+/* I915_READ_FW, only for fast reads of display block, no need for forcewake etc. */
 static int __intel_get_crtc_scanline(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
@@ -733,9 +751,9 @@ static int __intel_get_crtc_scanline(struct intel_crtc *crtc)
 		vtotal /= 2;
 
 	if (IS_GEN2(dev))
-		position = __raw_i915_read32(dev_priv, PIPEDSL(pipe)) & DSL_LINEMASK_GEN2;
+		position = I915_READ_FW(PIPEDSL(pipe)) & DSL_LINEMASK_GEN2;
 	else
-		position = __raw_i915_read32(dev_priv, PIPEDSL(pipe)) & DSL_LINEMASK_GEN3;
+		position = I915_READ_FW(PIPEDSL(pipe)) & DSL_LINEMASK_GEN3;
 
 	/*
 	 * On HSW, the DSL reg (0x70000) appears to return 0 if we
@@ -827,7 +845,7 @@ static int i915_get_crtc_scanoutpos(struct drm_device *dev, unsigned int pipe,
 		 * We can split this into vertical and horizontal
 		 * scanout position.
 		 */
-		position = (__raw_i915_read32(dev_priv, PIPEFRAMEPIXEL(pipe)) & PIPE_PIXEL_MASK) >> PIPE_PIXEL_SHIFT;
+		position = (I915_READ_FW(PIPEFRAMEPIXEL(pipe)) & PIPE_PIXEL_MASK) >> PIPE_PIXEL_SHIFT;
 
 		/* convert to pixel counts */
 		vbl_start *= htotal;
@@ -1085,6 +1103,14 @@ static void gen6_pm_rps_work(struct work_struct *work)
 		spin_unlock_irq(&dev_priv->irq_lock);
 		return;
 	}
+
+	/*
+	 * The RPS work is synced during runtime suspend, we don't require a
+	 * wakeref. TODO: instead of disabling the asserts make sure that we
+	 * always hold an RPM reference while the work is running.
+	 */
+	DISABLE_RPM_WAKEREF_ASSERTS(dev_priv);
+
 	pm_iir = dev_priv->rps.pm_iir;
 	dev_priv->rps.pm_iir = 0;
 	/* Make sure not to corrupt PMIMR state used by ringbuffer on GEN6 */
@@ -1097,7 +1123,7 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	WARN_ON(pm_iir & ~dev_priv->pm_rps_events);
 
 	if ((pm_iir & dev_priv->pm_rps_events) == 0 && !client_boost)
-		return;
+		goto out;
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 
@@ -1152,6 +1178,8 @@ static void gen6_pm_rps_work(struct work_struct *work)
 	intel_set_rps(dev_priv->dev, new_delay);
 
 	mutex_unlock(&dev_priv->rps.hw_lock);
+out:
+	ENABLE_RPM_WAKEREF_ASSERTS(dev_priv);
 }
 
 
@@ -1188,7 +1216,7 @@ static void ivybridge_parity_work(struct work_struct *work)
 	POSTING_READ(GEN7_MISCCPCTL);
 
 	while ((slice = ffs(dev_priv->l3_parity.which_slice)) != 0) {
-		u32 reg;
+		i915_reg_t reg;
 
 		slice--;
 		if (WARN_ON_ONCE(slice >= NUM_L3_SLICES(dev_priv->dev)))
@@ -1196,7 +1224,7 @@ static void ivybridge_parity_work(struct work_struct *work)
 
 		dev_priv->l3_parity.which_slice &= ~(1<<slice);
 
-		reg = GEN7_L3CDERRST1 + (slice * 0x200);
+		reg = GEN7_L3CDERRST1(slice);
 
 		error_status = I915_READ(reg);
 		row = GEN7_PARITY_ERROR_ROW(error_status);
@@ -1290,70 +1318,69 @@ static void snb_gt_irq_handler(struct drm_device *dev,
 		ivybridge_parity_error_irq_handler(dev, gt_iir);
 }
 
+static __always_inline void
+gen8_cs_irq_handler(struct intel_engine_cs *ring, u32 iir, int test_shift)
+{
+	if (iir & (GT_RENDER_USER_INTERRUPT << test_shift))
+		notify_ring(ring);
+	if (iir & (GT_CONTEXT_SWITCH_INTERRUPT << test_shift))
+		intel_lrc_irq_handler(ring);
+}
+
 static irqreturn_t gen8_gt_irq_handler(struct drm_i915_private *dev_priv,
 				       u32 master_ctl)
 {
 	irqreturn_t ret = IRQ_NONE;
 
 	if (master_ctl & (GEN8_GT_RCS_IRQ | GEN8_GT_BCS_IRQ)) {
-		u32 tmp = I915_READ_FW(GEN8_GT_IIR(0));
-		if (tmp) {
-			I915_WRITE_FW(GEN8_GT_IIR(0), tmp);
+		u32 iir = I915_READ_FW(GEN8_GT_IIR(0));
+		if (iir) {
+			I915_WRITE_FW(GEN8_GT_IIR(0), iir);
 			ret = IRQ_HANDLED;
 
-			if (tmp & (GT_CONTEXT_SWITCH_INTERRUPT << GEN8_RCS_IRQ_SHIFT))
-				intel_lrc_irq_handler(&dev_priv->ring[RCS]);
-			if (tmp & (GT_RENDER_USER_INTERRUPT << GEN8_RCS_IRQ_SHIFT))
-				notify_ring(&dev_priv->ring[RCS]);
+			gen8_cs_irq_handler(&dev_priv->ring[RCS],
+					iir, GEN8_RCS_IRQ_SHIFT);
 
-			if (tmp & (GT_CONTEXT_SWITCH_INTERRUPT << GEN8_BCS_IRQ_SHIFT))
-				intel_lrc_irq_handler(&dev_priv->ring[BCS]);
-			if (tmp & (GT_RENDER_USER_INTERRUPT << GEN8_BCS_IRQ_SHIFT))
-				notify_ring(&dev_priv->ring[BCS]);
+			gen8_cs_irq_handler(&dev_priv->ring[BCS],
+					iir, GEN8_BCS_IRQ_SHIFT);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT0)!\n");
 	}
 
 	if (master_ctl & (GEN8_GT_VCS1_IRQ | GEN8_GT_VCS2_IRQ)) {
-		u32 tmp = I915_READ_FW(GEN8_GT_IIR(1));
-		if (tmp) {
-			I915_WRITE_FW(GEN8_GT_IIR(1), tmp);
+		u32 iir = I915_READ_FW(GEN8_GT_IIR(1));
+		if (iir) {
+			I915_WRITE_FW(GEN8_GT_IIR(1), iir);
 			ret = IRQ_HANDLED;
 
-			if (tmp & (GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS1_IRQ_SHIFT))
-				intel_lrc_irq_handler(&dev_priv->ring[VCS]);
-			if (tmp & (GT_RENDER_USER_INTERRUPT << GEN8_VCS1_IRQ_SHIFT))
-				notify_ring(&dev_priv->ring[VCS]);
+			gen8_cs_irq_handler(&dev_priv->ring[VCS],
+					iir, GEN8_VCS1_IRQ_SHIFT);
 
-			if (tmp & (GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS2_IRQ_SHIFT))
-				intel_lrc_irq_handler(&dev_priv->ring[VCS2]);
-			if (tmp & (GT_RENDER_USER_INTERRUPT << GEN8_VCS2_IRQ_SHIFT))
-				notify_ring(&dev_priv->ring[VCS2]);
+			gen8_cs_irq_handler(&dev_priv->ring[VCS2],
+					iir, GEN8_VCS2_IRQ_SHIFT);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT1)!\n");
 	}
 
 	if (master_ctl & GEN8_GT_VECS_IRQ) {
-		u32 tmp = I915_READ_FW(GEN8_GT_IIR(3));
-		if (tmp) {
-			I915_WRITE_FW(GEN8_GT_IIR(3), tmp);
+		u32 iir = I915_READ_FW(GEN8_GT_IIR(3));
+		if (iir) {
+			I915_WRITE_FW(GEN8_GT_IIR(3), iir);
 			ret = IRQ_HANDLED;
 
-			if (tmp & (GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VECS_IRQ_SHIFT))
-				intel_lrc_irq_handler(&dev_priv->ring[VECS]);
-			if (tmp & (GT_RENDER_USER_INTERRUPT << GEN8_VECS_IRQ_SHIFT))
-				notify_ring(&dev_priv->ring[VECS]);
+			gen8_cs_irq_handler(&dev_priv->ring[VECS],
+					iir, GEN8_VECS_IRQ_SHIFT);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT3)!\n");
 	}
 
 	if (master_ctl & GEN8_GT_PM_IRQ) {
-		u32 tmp = I915_READ_FW(GEN8_GT_IIR(2));
-		if (tmp & dev_priv->pm_rps_events) {
+		u32 iir = I915_READ_FW(GEN8_GT_IIR(2));
+		if (iir & dev_priv->pm_rps_events) {
 			I915_WRITE_FW(GEN8_GT_IIR(2),
-				      tmp & dev_priv->pm_rps_events);
+				      iir & dev_priv->pm_rps_events);
 			ret = IRQ_HANDLED;
-			gen6_rps_irq_handler(dev_priv, tmp);
+			gen6_rps_irq_handler(dev_priv, iir);
 		} else
 			DRM_ERROR("The master control interrupt lied (PM)!\n");
 	}
@@ -1625,7 +1652,7 @@ static void valleyview_pipestat_irq_handler(struct drm_device *dev, u32 iir)
 
 	spin_lock(&dev_priv->irq_lock);
 	for_each_pipe(dev_priv, pipe) {
-		int reg;
+		i915_reg_t reg;
 		u32 mask, iir_bit = 0;
 
 		/*
@@ -1706,7 +1733,7 @@ static void i9xx_hpd_irq_handler(struct drm_device *dev)
 	 */
 	POSTING_READ(PORT_HOTPLUG_STAT);
 
-	if (IS_G4X(dev) || IS_VALLEYVIEW(dev)) {
+	if (IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		u32 hotplug_trigger = hotplug_status & HOTPLUG_INT_STATUS_G4X;
 
 		if (hotplug_trigger) {
@@ -1741,6 +1768,9 @@ static irqreturn_t valleyview_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	while (true) {
 		/* Find, clear, then process each source of interrupt */
 
@@ -1775,6 +1805,8 @@ static irqreturn_t valleyview_irq_handler(int irq, void *arg)
 	}
 
 out:
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -1788,6 +1820,9 @@ static irqreturn_t cherryview_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	for (;;) {
 		master_ctl = I915_READ(GEN8_MASTER_IRQ) & ~GEN8_MASTER_IRQ_CONTROL;
 		iir = I915_READ(VLV_IIR);
@@ -1818,6 +1853,8 @@ static irqreturn_t cherryview_irq_handler(int irq, void *arg)
 		POSTING_READ(GEN8_MASTER_IRQ);
 	}
 
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -1827,8 +1864,24 @@ static void ibx_hpd_irq_handler(struct drm_device *dev, u32 hotplug_trigger,
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	u32 dig_hotplug_reg, pin_mask = 0, long_mask = 0;
 
+	/*
+	 * Somehow the PCH doesn't seem to really ack the interrupt to the CPU
+	 * unless we touch the hotplug register, even if hotplug_trigger is
+	 * zero. Not acking leads to "The master control interrupt lied (SDE)!"
+	 * errors.
+	 */
 	dig_hotplug_reg = I915_READ(PCH_PORT_HOTPLUG);
+	if (!hotplug_trigger) {
+		u32 mask = PORTA_HOTPLUG_STATUS_MASK |
+			PORTD_HOTPLUG_STATUS_MASK |
+			PORTC_HOTPLUG_STATUS_MASK |
+			PORTB_HOTPLUG_STATUS_MASK;
+		dig_hotplug_reg &= ~mask;
+	}
+
 	I915_WRITE(PCH_PORT_HOTPLUG, dig_hotplug_reg);
+	if (!hotplug_trigger)
+		return;
 
 	intel_get_hpd_pins(&pin_mask, &long_mask, hotplug_trigger,
 			   dig_hotplug_reg, hpd,
@@ -1843,8 +1896,7 @@ static void ibx_irq_handler(struct drm_device *dev, u32 pch_iir)
 	int pipe;
 	u32 hotplug_trigger = pch_iir & SDE_HOTPLUG_MASK;
 
-	if (hotplug_trigger)
-		ibx_hpd_irq_handler(dev, hotplug_trigger, hpd_ibx);
+	ibx_hpd_irq_handler(dev, hotplug_trigger, hpd_ibx);
 
 	if (pch_iir & SDE_AUDIO_POWER_MASK) {
 		int port = ffs((pch_iir & SDE_AUDIO_POWER_MASK) >>
@@ -1937,8 +1989,7 @@ static void cpt_irq_handler(struct drm_device *dev, u32 pch_iir)
 	int pipe;
 	u32 hotplug_trigger = pch_iir & SDE_HOTPLUG_MASK_CPT;
 
-	if (hotplug_trigger)
-		ibx_hpd_irq_handler(dev, hotplug_trigger, hpd_cpt);
+	ibx_hpd_irq_handler(dev, hotplug_trigger, hpd_cpt);
 
 	if (pch_iir & SDE_AUDIO_POWER_MASK_CPT) {
 		int port = ffs((pch_iir & SDE_AUDIO_POWER_MASK_CPT) >>
@@ -2134,6 +2185,9 @@ static irqreturn_t ironlake_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	/* We get interrupts on unclaimed registers, so check for this before we
 	 * do any I915_{READ,WRITE}. */
 	intel_uncore_check_errors(dev);
@@ -2192,6 +2246,9 @@ static irqreturn_t ironlake_irq_handler(int irq, void *arg)
 		POSTING_READ(SDEIER);
 	}
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -2224,6 +2281,9 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	if (INTEL_INFO(dev_priv)->gen >= 9)
 		aux_mask |=  GEN9_AUX_CHANNEL_B | GEN9_AUX_CHANNEL_C |
 			GEN9_AUX_CHANNEL_D;
@@ -2231,7 +2291,7 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 	master_ctl = I915_READ_FW(GEN8_MASTER_IRQ);
 	master_ctl &= ~GEN8_MASTER_IRQ_CONTROL;
 	if (!master_ctl)
-		return IRQ_NONE;
+		goto out;
 
 	I915_WRITE_FW(GEN8_MASTER_IRQ, 0);
 
@@ -2354,14 +2414,21 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 				spt_irq_handler(dev, pch_iir);
 			else
 				cpt_irq_handler(dev, pch_iir);
-		} else
-			DRM_ERROR("The master control interrupt lied (SDE)!\n");
-
+		} else {
+			/*
+			 * Like on previous PCH there seems to be something
+			 * fishy going on with forwarding PCH interrupts.
+			 */
+			DRM_DEBUG_DRIVER("The master control interrupt lied (SDE)!\n");
+		}
 	}
 
 	I915_WRITE_FW(GEN8_MASTER_IRQ, GEN8_MASTER_IRQ_CONTROL);
 	POSTING_READ_FW(GEN8_MASTER_IRQ);
 
+out:
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -2644,7 +2711,7 @@ static int ironlake_enable_vblank(struct drm_device *dev, unsigned int pipe)
 						     DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_enable_display_irq(dev_priv, bit);
+	ilk_enable_display_irq(dev_priv, bit);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
 	return 0;
@@ -2669,10 +2736,9 @@ static int gen8_enable_vblank(struct drm_device *dev, unsigned int pipe)
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	dev_priv->de_irq_mask[pipe] &= ~GEN8_PIPE_VBLANK;
-	I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
-	POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+	bdw_enable_pipe_irq(dev_priv, pipe, GEN8_PIPE_VBLANK);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
 	return 0;
 }
 
@@ -2699,7 +2765,7 @@ static void ironlake_disable_vblank(struct drm_device *dev, unsigned int pipe)
 						     DE_PIPE_VBLANK(pipe);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	ironlake_disable_display_irq(dev_priv, bit);
+	ilk_disable_display_irq(dev_priv, bit);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
@@ -2720,9 +2786,7 @@ static void gen8_disable_vblank(struct drm_device *dev, unsigned int pipe)
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
-	dev_priv->de_irq_mask[pipe] |= GEN8_PIPE_VBLANK;
-	I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
-	POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+	bdw_disable_pipe_irq(dev_priv, pipe, GEN8_PIPE_VBLANK);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
@@ -2961,6 +3025,13 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 	if (!i915.enable_hangcheck)
 		return;
 
+	/*
+	 * The hangcheck work is synced during runtime suspend, we don't
+	 * require a wakeref. TODO: instead of disabling the asserts make
+	 * sure that we hold a reference when this work is running.
+	 */
+	DISABLE_RPM_WAKEREF_ASSERTS(dev_priv);
+
 	for_each_ring(ring, dev_priv, i) {
 		u64 acthd;
 		u32 seqno;
@@ -3052,13 +3123,18 @@ static void i915_hangcheck_elapsed(struct work_struct *work)
 		}
 	}
 
-	if (rings_hung)
-		return i915_handle_error(dev, true, "Ring hung");
+	if (rings_hung) {
+		i915_handle_error(dev, true, "Ring hung");
+		goto out;
+	}
 
 	if (busy_count)
 		/* Reset timer case chip hangs without another request
 		 * being added */
 		i915_queue_hangcheck(dev);
+
+out:
+	ENABLE_RPM_WAKEREF_ASSERTS(dev_priv);
 }
 
 void i915_queue_hangcheck(struct drm_device *dev)
@@ -3451,7 +3527,7 @@ static int ironlake_irq_postinstall(struct drm_device *dev)
 		 * setup is guaranteed to run in single-threaded context. But we
 		 * need it to make the assert_spin_locked happy. */
 		spin_lock_irq(&dev_priv->irq_lock);
-		ironlake_enable_display_irq(dev_priv, DE_PCU_EVENT);
+		ilk_enable_display_irq(dev_priv, DE_PCU_EVENT);
 		spin_unlock_irq(&dev_priv->irq_lock);
 	}
 
@@ -3850,13 +3926,18 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 	u16 flip_mask =
 		I915_DISPLAY_PLANE_A_FLIP_PENDING_INTERRUPT |
 		I915_DISPLAY_PLANE_B_FLIP_PENDING_INTERRUPT;
+	irqreturn_t ret;
 
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
+	ret = IRQ_NONE;
 	iir = I915_READ16(IIR);
 	if (iir == 0)
-		return IRQ_NONE;
+		goto out;
 
 	while (iir & ~flip_mask) {
 		/* Can't rely on pipestat interrupt bit in iir as it might
@@ -3869,7 +3950,7 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
-			int reg = PIPESTAT(pipe);
+			i915_reg_t reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
 			/*
@@ -3905,8 +3986,12 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 
 		iir = new_iir;
 	}
+	ret = IRQ_HANDLED;
+
+out:
+	enable_rpm_wakeref_asserts(dev_priv);
 
-	return IRQ_HANDLED;
+	return ret;
 }
 
 static void i8xx_irq_uninstall(struct drm_device * dev)
@@ -4035,6 +4120,9 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	iir = I915_READ(IIR);
 	do {
 		bool irq_received = (iir & ~flip_mask) != 0;
@@ -4050,7 +4138,7 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
-			int reg = PIPESTAT(pipe);
+			i915_reg_t reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
 			/* Clear the PIPE*STAT regs before the IIR */
@@ -4117,6 +4205,8 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		iir = new_iir;
 	} while (iir & ~flip_mask);
 
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -4256,6 +4346,9 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 	if (!intel_irqs_enabled(dev_priv))
 		return IRQ_NONE;
 
+	/* IRQs are synced during runtime_suspend, we don't require a wakeref */
+	disable_rpm_wakeref_asserts(dev_priv);
+
 	iir = I915_READ(IIR);
 
 	for (;;) {
@@ -4272,7 +4365,7 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 			DRM_DEBUG("Command parser error, iir 0x%08x\n", iir);
 
 		for_each_pipe(dev_priv, pipe) {
-			int reg = PIPESTAT(pipe);
+			i915_reg_t reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
 			/*
@@ -4341,6 +4434,8 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		iir = new_iir;
 	}
 
+	enable_rpm_wakeref_asserts(dev_priv);
+
 	return ret;
 }
 
@@ -4384,7 +4479,7 @@ void intel_irq_init(struct drm_i915_private *dev_priv)
 	INIT_WORK(&dev_priv->l3_parity.error_work, ivybridge_parity_work);
 
 	/* Let's track the enabled rps events */
-	if (IS_VALLEYVIEW(dev_priv) && !IS_CHERRYVIEW(dev_priv))
+	if (IS_VALLEYVIEW(dev_priv))
 		/* WaGsvRC0ResidencyMethod:vlv */
 		dev_priv->pm_rps_events = GEN6_PM_RP_DOWN_EI_EXPIRED | GEN6_PM_RP_UP_EI_EXPIRED;
 	else
diff --git a/drivers/gpu/drm/i915/i915_params.c b/drivers/gpu/drm/i915/i915_params.c
index 4be13a5eb932..835d6099c769 100644
--- a/drivers/gpu/drm/i915/i915_params.c
+++ b/drivers/gpu/drm/i915/i915_params.c
@@ -32,6 +32,7 @@ struct i915_params i915 __read_mostly = {
 	.panel_use_ssc = -1,
 	.vbt_sdvo_panel_type = -1,
 	.enable_rc6 = -1,
+	.enable_dc = -1,
 	.enable_fbc = -1,
 	.enable_execlists = -1,
 	.enable_hangcheck = true,
@@ -80,6 +81,11 @@ MODULE_PARM_DESC(enable_rc6,
 	"For example, 3 would enable rc6 and deep rc6, and 7 would enable everything. "
 	"default: -1 (use per-chip default)");
 
+module_param_named_unsafe(enable_dc, i915.enable_dc, int, 0400);
+MODULE_PARM_DESC(enable_dc,
+	"Enable power-saving display C-states. "
+	"(-1=auto [default]; 0=disable; 1=up to DC5; 2=up to DC6)");
+
 module_param_named_unsafe(enable_fbc, i915.enable_fbc, int, 0600);
 MODULE_PARM_DESC(enable_fbc,
 	"Enable frame buffer compression for power savings "
@@ -112,7 +118,7 @@ MODULE_PARM_DESC(enable_hangcheck,
 module_param_named_unsafe(enable_ppgtt, i915.enable_ppgtt, int, 0400);
 MODULE_PARM_DESC(enable_ppgtt,
 	"Override PPGTT usage. "
-	"(-1=auto [default], 0=disabled, 1=aliasing, 2=full)");
+	"(-1=auto [default], 0=disabled, 1=aliasing, 2=full, 3=full with extended address space)");
 
 module_param_named_unsafe(enable_execlists, i915.enable_execlists, int, 0400);
 MODULE_PARM_DESC(enable_execlists,
@@ -126,7 +132,7 @@ module_param_named_unsafe(preliminary_hw_support, i915.preliminary_hw_support, i
 MODULE_PARM_DESC(preliminary_hw_support,
 	"Enable preliminary hardware support.");
 
-module_param_named_unsafe(disable_power_well, i915.disable_power_well, int, 0600);
+module_param_named_unsafe(disable_power_well, i915.disable_power_well, int, 0400);
 MODULE_PARM_DESC(disable_power_well,
 	"Disable display power wells when possible "
 	"(-1=auto [default], 0=power wells always on, 1=power wells disabled when possible)");
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index bc7b8faba84d..4897728713f6 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -25,14 +25,43 @@
 #ifndef _I915_REG_H_
 #define _I915_REG_H_
 
+typedef struct {
+	uint32_t reg;
+} i915_reg_t;
+
+#define _MMIO(r) ((const i915_reg_t){ .reg = (r) })
+
+#define INVALID_MMIO_REG _MMIO(0)
+
+static inline uint32_t i915_mmio_reg_offset(i915_reg_t reg)
+{
+	return reg.reg;
+}
+
+static inline bool i915_mmio_reg_equal(i915_reg_t a, i915_reg_t b)
+{
+	return i915_mmio_reg_offset(a) == i915_mmio_reg_offset(b);
+}
+
+static inline bool i915_mmio_reg_valid(i915_reg_t reg)
+{
+	return !i915_mmio_reg_equal(reg, INVALID_MMIO_REG);
+}
+
 #define _PIPE(pipe, a, b) ((a) + (pipe)*((b)-(a)))
+#define _MMIO_PIPE(pipe, a, b) _MMIO(_PIPE(pipe, a, b))
 #define _PLANE(plane, a, b) _PIPE(plane, a, b)
-#define _TRANSCODER(tran, a, b) ((a) + (tran)*((b)-(a)))
+#define _MMIO_PLANE(plane, a, b) _MMIO_PIPE(plane, a, b)
+#define _TRANS(tran, a, b) ((a) + (tran)*((b)-(a)))
+#define _MMIO_TRANS(tran, a, b) _MMIO(_TRANS(tran, a, b))
 #define _PORT(port, a, b) ((a) + (port)*((b)-(a)))
+#define _MMIO_PORT(port, a, b) _MMIO(_PORT(port, a, b))
 #define _PIPE3(pipe, a, b, c) ((pipe) == PIPE_A ? (a) : \
 			       (pipe) == PIPE_B ? (b) : (c))
+#define _MMIO_PIPE3(pipe, a, b, c) _MMIO(_PIPE3(pipe, a, b, c))
 #define _PORT3(port, a, b, c) ((port) == PORT_A ? (a) : \
 			       (port) == PORT_B ? (b) : (c))
+#define _MMIO_PORT3(pipe, a, b, c) _MMIO(_PORT3(pipe, a, b, c))
 
 #define _MASKED_FIELD(mask, value) ({					   \
 	if (__builtin_constant_p(mask))					   \
@@ -105,14 +134,14 @@
 #define  GRDOM_RESET_STATUS (1<<1)
 #define  GRDOM_RESET_ENABLE (1<<0)
 
-#define ILK_GDSR (MCHBAR_MIRROR_BASE + 0x2ca4)
+#define ILK_GDSR _MMIO(MCHBAR_MIRROR_BASE + 0x2ca4)
 #define  ILK_GRDOM_FULL		(0<<1)
 #define  ILK_GRDOM_RENDER	(1<<1)
 #define  ILK_GRDOM_MEDIA	(3<<1)
 #define  ILK_GRDOM_MASK		(3<<1)
 #define  ILK_GRDOM_RESET_ENABLE (1<<0)
 
-#define GEN6_MBCUNIT_SNPCR	0x900c /* for LLC config */
+#define GEN6_MBCUNIT_SNPCR	_MMIO(0x900c) /* for LLC config */
 #define   GEN6_MBC_SNPCR_SHIFT	21
 #define   GEN6_MBC_SNPCR_MASK	(3<<21)
 #define   GEN6_MBC_SNPCR_MAX	(0<<21)
@@ -120,31 +149,31 @@
 #define   GEN6_MBC_SNPCR_LOW	(2<<21)
 #define   GEN6_MBC_SNPCR_MIN	(3<<21) /* only 1/16th of the cache is shared */
 
-#define VLV_G3DCTL		0x9024
-#define VLV_GSCKGCTL		0x9028
+#define VLV_G3DCTL		_MMIO(0x9024)
+#define VLV_GSCKGCTL		_MMIO(0x9028)
 
-#define GEN6_MBCTL		0x0907c
+#define GEN6_MBCTL		_MMIO(0x0907c)
 #define   GEN6_MBCTL_ENABLE_BOOT_FETCH	(1 << 4)
 #define   GEN6_MBCTL_CTX_FETCH_NEEDED	(1 << 3)
 #define   GEN6_MBCTL_BME_UPDATE_ENABLE	(1 << 2)
 #define   GEN6_MBCTL_MAE_UPDATE_ENABLE	(1 << 1)
 #define   GEN6_MBCTL_BOOT_FETCH_MECH	(1 << 0)
 
-#define GEN6_GDRST	0x941c
+#define GEN6_GDRST	_MMIO(0x941c)
 #define  GEN6_GRDOM_FULL		(1 << 0)
 #define  GEN6_GRDOM_RENDER		(1 << 1)
 #define  GEN6_GRDOM_MEDIA		(1 << 2)
 #define  GEN6_GRDOM_BLT			(1 << 3)
 
-#define RING_PP_DIR_BASE(ring)		((ring)->mmio_base+0x228)
-#define RING_PP_DIR_BASE_READ(ring)	((ring)->mmio_base+0x518)
-#define RING_PP_DIR_DCLV(ring)		((ring)->mmio_base+0x220)
+#define RING_PP_DIR_BASE(ring)		_MMIO((ring)->mmio_base+0x228)
+#define RING_PP_DIR_BASE_READ(ring)	_MMIO((ring)->mmio_base+0x518)
+#define RING_PP_DIR_DCLV(ring)		_MMIO((ring)->mmio_base+0x220)
 #define   PP_DIR_DCLV_2G		0xffffffff
 
-#define GEN8_RING_PDP_UDW(ring, n)	((ring)->mmio_base+0x270 + ((n) * 8 + 4))
-#define GEN8_RING_PDP_LDW(ring, n)	((ring)->mmio_base+0x270 + (n) * 8)
+#define GEN8_RING_PDP_UDW(ring, n)	_MMIO((ring)->mmio_base+0x270 + (n) * 8 + 4)
+#define GEN8_RING_PDP_LDW(ring, n)	_MMIO((ring)->mmio_base+0x270 + (n) * 8)
 
-#define GEN8_R_PWR_CLK_STATE		0x20C8
+#define GEN8_R_PWR_CLK_STATE		_MMIO(0x20C8)
 #define   GEN8_RPCS_ENABLE		(1 << 31)
 #define   GEN8_RPCS_S_CNT_ENABLE	(1 << 18)
 #define   GEN8_RPCS_S_CNT_SHIFT		15
@@ -157,7 +186,7 @@
 #define   GEN8_RPCS_EU_MIN_SHIFT	0
 #define   GEN8_RPCS_EU_MIN_MASK		(0xf << GEN8_RPCS_EU_MIN_SHIFT)
 
-#define GAM_ECOCHK			0x4090
+#define GAM_ECOCHK			_MMIO(0x4090)
 #define   BDW_DISABLE_HDC_INVALIDATION	(1<<25)
 #define   ECOCHK_SNB_BIT		(1<<10)
 #define   ECOCHK_DIS_TLB		(1<<8)
@@ -170,15 +199,15 @@
 #define   ECOCHK_PPGTT_WT_HSW		(0x2<<3)
 #define   ECOCHK_PPGTT_WB_HSW		(0x3<<3)
 
-#define GAC_ECO_BITS			0x14090
+#define GAC_ECO_BITS			_MMIO(0x14090)
 #define   ECOBITS_SNB_BIT		(1<<13)
 #define   ECOBITS_PPGTT_CACHE64B	(3<<8)
 #define   ECOBITS_PPGTT_CACHE4B		(0<<8)
 
-#define GAB_CTL				0x24000
+#define GAB_CTL				_MMIO(0x24000)
 #define   GAB_CTL_CONT_AFTER_PAGEFAULT	(1<<8)
 
-#define GEN6_STOLEN_RESERVED		0x1082C0
+#define GEN6_STOLEN_RESERVED		_MMIO(0x1082C0)
 #define GEN6_STOLEN_RESERVED_ADDR_MASK	(0xFFF << 20)
 #define GEN7_STOLEN_RESERVED_ADDR_MASK	(0x3FFF << 18)
 #define GEN6_STOLEN_RESERVED_SIZE_MASK	(3 << 4)
@@ -200,6 +229,7 @@
 #define VGA_ST01_MDA 0x3ba
 #define VGA_ST01_CGA 0x3da
 
+#define _VGA_MSR_WRITE _MMIO(0x3c2)
 #define VGA_MSR_WRITE 0x3c2
 #define VGA_MSR_READ 0x3cc
 #define   VGA_MSR_MEM_EN (1<<1)
@@ -377,10 +407,12 @@
 #define MI_BATCH_BUFFER_START_GEN8	MI_INSTR(0x31, 1)
 #define   MI_BATCH_RESOURCE_STREAMER (1<<10)
 
-#define MI_PREDICATE_SRC0	(0x2400)
-#define MI_PREDICATE_SRC1	(0x2408)
+#define MI_PREDICATE_SRC0	_MMIO(0x2400)
+#define MI_PREDICATE_SRC0_UDW	_MMIO(0x2400 + 4)
+#define MI_PREDICATE_SRC1	_MMIO(0x2408)
+#define MI_PREDICATE_SRC1_UDW	_MMIO(0x2408 + 4)
 
-#define MI_PREDICATE_RESULT_2	(0x2214)
+#define MI_PREDICATE_RESULT_2	_MMIO(0x2214)
 #define  LOWER_SLICE_ENABLED	(1<<0)
 #define  LOWER_SLICE_DISABLED	(0<<0)
 
@@ -509,49 +541,61 @@
 /*
  * Registers used only by the command parser
  */
-#define BCS_SWCTRL 0x22200
-
-#define GPGPU_THREADS_DISPATCHED        0x2290
-#define HS_INVOCATION_COUNT             0x2300
-#define DS_INVOCATION_COUNT             0x2308
-#define IA_VERTICES_COUNT               0x2310
-#define IA_PRIMITIVES_COUNT             0x2318
-#define VS_INVOCATION_COUNT             0x2320
-#define GS_INVOCATION_COUNT             0x2328
-#define GS_PRIMITIVES_COUNT             0x2330
-#define CL_INVOCATION_COUNT             0x2338
-#define CL_PRIMITIVES_COUNT             0x2340
-#define PS_INVOCATION_COUNT             0x2348
-#define PS_DEPTH_COUNT                  0x2350
+#define BCS_SWCTRL _MMIO(0x22200)
+
+#define GPGPU_THREADS_DISPATCHED        _MMIO(0x2290)
+#define GPGPU_THREADS_DISPATCHED_UDW	_MMIO(0x2290 + 4)
+#define HS_INVOCATION_COUNT             _MMIO(0x2300)
+#define HS_INVOCATION_COUNT_UDW		_MMIO(0x2300 + 4)
+#define DS_INVOCATION_COUNT             _MMIO(0x2308)
+#define DS_INVOCATION_COUNT_UDW		_MMIO(0x2308 + 4)
+#define IA_VERTICES_COUNT               _MMIO(0x2310)
+#define IA_VERTICES_COUNT_UDW		_MMIO(0x2310 + 4)
+#define IA_PRIMITIVES_COUNT             _MMIO(0x2318)
+#define IA_PRIMITIVES_COUNT_UDW		_MMIO(0x2318 + 4)
+#define VS_INVOCATION_COUNT             _MMIO(0x2320)
+#define VS_INVOCATION_COUNT_UDW		_MMIO(0x2320 + 4)
+#define GS_INVOCATION_COUNT             _MMIO(0x2328)
+#define GS_INVOCATION_COUNT_UDW		_MMIO(0x2328 + 4)
+#define GS_PRIMITIVES_COUNT             _MMIO(0x2330)
+#define GS_PRIMITIVES_COUNT_UDW		_MMIO(0x2330 + 4)
+#define CL_INVOCATION_COUNT             _MMIO(0x2338)
+#define CL_INVOCATION_COUNT_UDW		_MMIO(0x2338 + 4)
+#define CL_PRIMITIVES_COUNT             _MMIO(0x2340)
+#define CL_PRIMITIVES_COUNT_UDW		_MMIO(0x2340 + 4)
+#define PS_INVOCATION_COUNT             _MMIO(0x2348)
+#define PS_INVOCATION_COUNT_UDW		_MMIO(0x2348 + 4)
+#define PS_DEPTH_COUNT                  _MMIO(0x2350)
+#define PS_DEPTH_COUNT_UDW		_MMIO(0x2350 + 4)
 
 /* There are the 4 64-bit counter registers, one for each stream output */
-#define GEN7_SO_NUM_PRIMS_WRITTEN(n) (0x5200 + (n) * 8)
+#define GEN7_SO_NUM_PRIMS_WRITTEN(n)		_MMIO(0x5200 + (n) * 8)
+#define GEN7_SO_NUM_PRIMS_WRITTEN_UDW(n)	_MMIO(0x5200 + (n) * 8 + 4)
 
-#define GEN7_SO_PRIM_STORAGE_NEEDED(n)  (0x5240 + (n) * 8)
+#define GEN7_SO_PRIM_STORAGE_NEEDED(n)		_MMIO(0x5240 + (n) * 8)
+#define GEN7_SO_PRIM_STORAGE_NEEDED_UDW(n)	_MMIO(0x5240 + (n) * 8 + 4)
 
-#define GEN7_3DPRIM_END_OFFSET          0x2420
-#define GEN7_3DPRIM_START_VERTEX        0x2430
-#define GEN7_3DPRIM_VERTEX_COUNT        0x2434
-#define GEN7_3DPRIM_INSTANCE_COUNT      0x2438
-#define GEN7_3DPRIM_START_INSTANCE      0x243C
-#define GEN7_3DPRIM_BASE_VERTEX         0x2440
+#define GEN7_3DPRIM_END_OFFSET          _MMIO(0x2420)
+#define GEN7_3DPRIM_START_VERTEX        _MMIO(0x2430)
+#define GEN7_3DPRIM_VERTEX_COUNT        _MMIO(0x2434)
+#define GEN7_3DPRIM_INSTANCE_COUNT      _MMIO(0x2438)
+#define GEN7_3DPRIM_START_INSTANCE      _MMIO(0x243C)
+#define GEN7_3DPRIM_BASE_VERTEX         _MMIO(0x2440)
 
-#define GEN7_GPGPU_DISPATCHDIMX         0x2500
-#define GEN7_GPGPU_DISPATCHDIMY         0x2504
-#define GEN7_GPGPU_DISPATCHDIMZ         0x2508
+#define GEN7_GPGPU_DISPATCHDIMX         _MMIO(0x2500)
+#define GEN7_GPGPU_DISPATCHDIMY         _MMIO(0x2504)
+#define GEN7_GPGPU_DISPATCHDIMZ         _MMIO(0x2508)
 
-#define OACONTROL 0x2360
+#define OACONTROL _MMIO(0x2360)
 
 #define _GEN7_PIPEA_DE_LOAD_SL	0x70068
 #define _GEN7_PIPEB_DE_LOAD_SL	0x71068
-#define GEN7_PIPE_DE_LOAD_SL(pipe) _PIPE(pipe, \
-					 _GEN7_PIPEA_DE_LOAD_SL, \
-					 _GEN7_PIPEB_DE_LOAD_SL)
+#define GEN7_PIPE_DE_LOAD_SL(pipe) _MMIO_PIPE(pipe, _GEN7_PIPEA_DE_LOAD_SL, _GEN7_PIPEB_DE_LOAD_SL)
 
 /*
  * Reset registers
  */
-#define DEBUG_RESET_I830		0x6070
+#define DEBUG_RESET_I830		_MMIO(0x6070)
 #define  DEBUG_RESET_FULL		(1<<7)
 #define  DEBUG_RESET_RENDER		(1<<8)
 #define  DEBUG_RESET_DISPLAY		(1<<9)
@@ -559,7 +603,7 @@
 /*
  * IOSF sideband
  */
-#define VLV_IOSF_DOORBELL_REQ			(VLV_DISPLAY_BASE + 0x2100)
+#define VLV_IOSF_DOORBELL_REQ			_MMIO(VLV_DISPLAY_BASE + 0x2100)
 #define   IOSF_DEVFN_SHIFT			24
 #define   IOSF_OPCODE_SHIFT			16
 #define   IOSF_PORT_SHIFT			8
@@ -576,8 +620,8 @@
 #define   IOSF_PORT_CCU				0xA9
 #define   IOSF_PORT_GPS_CORE			0x48
 #define   IOSF_PORT_FLISDSI			0x1B
-#define VLV_IOSF_DATA				(VLV_DISPLAY_BASE + 0x2104)
-#define VLV_IOSF_ADDR				(VLV_DISPLAY_BASE + 0x2108)
+#define VLV_IOSF_DATA				_MMIO(VLV_DISPLAY_BASE + 0x2104)
+#define VLV_IOSF_ADDR				_MMIO(VLV_DISPLAY_BASE + 0x2108)
 
 /* See configdb bunit SB addr map */
 #define BUNIT_REG_BISOC				0x11
@@ -609,6 +653,7 @@
 
 /* See the PUNIT HAS v0.8 for the below bits */
 enum punit_power_well {
+	/* These numbers are fixed and must match the position of the pw bits */
 	PUNIT_POWER_WELL_RENDER			= 0,
 	PUNIT_POWER_WELL_MEDIA			= 1,
 	PUNIT_POWER_WELL_DISP2D			= 3,
@@ -621,10 +666,12 @@ enum punit_power_well {
 	PUNIT_POWER_WELL_DPIO_RX1		= 11,
 	PUNIT_POWER_WELL_DPIO_CMN_D		= 12,
 
-	PUNIT_POWER_WELL_NUM,
+	/* Not actual bit groups. Used as IDs for lookup_power_well() */
+	PUNIT_POWER_WELL_ALWAYS_ON,
 };
 
 enum skl_disp_power_wells {
+	/* These numbers are fixed and must match the position of the pw bits */
 	SKL_DISP_PW_MISC_IO,
 	SKL_DISP_PW_DDI_A_E,
 	SKL_DISP_PW_DDI_B,
@@ -632,6 +679,10 @@ enum skl_disp_power_wells {
 	SKL_DISP_PW_DDI_D,
 	SKL_DISP_PW_1 = 14,
 	SKL_DISP_PW_2,
+
+	/* Not actual bit groups. Used as IDs for lookup_power_well() */
+	SKL_DISP_PW_ALWAYS_ON,
+	SKL_DISP_PW_DC_OFF,
 };
 
 #define SKL_POWER_WELL_STATE(pw) (1 << ((pw) * 2))
@@ -804,35 +855,35 @@ enum skl_disp_power_wells {
  *
  * Note: DDI0 is digital port B, DD1 is digital port C, and DDI2 is
  * digital port D (CHV) or port A (BXT).
- */
-/*
- * Dual channel PHY (VLV/CHV/BXT)
- * ---------------------------------
- * |      CH0      |      CH1      |
- * |  CMN/PLL/REF  |  CMN/PLL/REF  |
- * |---------------|---------------| Display PHY
- * | PCS01 | PCS23 | PCS01 | PCS23 |
- * |-------|-------|-------|-------|
- * |TX0|TX1|TX2|TX3|TX0|TX1|TX2|TX3|
- * ---------------------------------
- * |     DDI0      |     DDI1      | DP/HDMI ports
- * ---------------------------------
  *
- * Single channel PHY (CHV/BXT)
- * -----------------
- * |      CH0      |
- * |  CMN/PLL/REF  |
- * |---------------| Display PHY
- * | PCS01 | PCS23 |
- * |-------|-------|
- * |TX0|TX1|TX2|TX3|
- * -----------------
- * |     DDI2      | DP/HDMI port
- * -----------------
+ *
+ *     Dual channel PHY (VLV/CHV/BXT)
+ *     ---------------------------------
+ *     |      CH0      |      CH1      |
+ *     |  CMN/PLL/REF  |  CMN/PLL/REF  |
+ *     |---------------|---------------| Display PHY
+ *     | PCS01 | PCS23 | PCS01 | PCS23 |
+ *     |-------|-------|-------|-------|
+ *     |TX0|TX1|TX2|TX3|TX0|TX1|TX2|TX3|
+ *     ---------------------------------
+ *     |     DDI0      |     DDI1      | DP/HDMI ports
+ *     ---------------------------------
+ *
+ *     Single channel PHY (CHV/BXT)
+ *     -----------------
+ *     |      CH0      |
+ *     |  CMN/PLL/REF  |
+ *     |---------------| Display PHY
+ *     | PCS01 | PCS23 |
+ *     |-------|-------|
+ *     |TX0|TX1|TX2|TX3|
+ *     -----------------
+ *     |     DDI2      | DP/HDMI port
+ *     -----------------
  */
 #define DPIO_DEVFN			0
 
-#define DPIO_CTL			(VLV_DISPLAY_BASE + 0x2110)
+#define DPIO_CTL			_MMIO(VLV_DISPLAY_BASE + 0x2110)
 #define  DPIO_MODSEL1			(1<<3) /* if ref clk b == 27 */
 #define  DPIO_MODSEL0			(1<<2) /* if ref clk a == 27 */
 #define  DPIO_SFR_BYPASS		(1<<1)
@@ -1185,9 +1236,9 @@ enum skl_disp_power_wells {
 #define   DPIO_UPAR_SHIFT		30
 
 /* BXT PHY registers */
-#define _BXT_PHY(phy, a, b)		_PIPE((phy), (a), (b))
+#define _BXT_PHY(phy, a, b)		_MMIO_PIPE((phy), (a), (b))
 
-#define BXT_P_CR_GT_DISP_PWRON		0x138090
+#define BXT_P_CR_GT_DISP_PWRON		_MMIO(0x138090)
 #define   GT_DISPLAY_POWER_ON(phy)	(1 << (phy))
 
 #define _PHY_CTL_FAMILY_EDP		0x64C80
@@ -1203,7 +1254,7 @@ enum skl_disp_power_wells {
 #define   PORT_PLL_ENABLE		(1 << 31)
 #define   PORT_PLL_LOCK			(1 << 30)
 #define   PORT_PLL_REF_SEL		(1 << 27)
-#define BXT_PORT_PLL_ENABLE(port)	_PORT(port, _PORT_PLL_A, _PORT_PLL_B)
+#define BXT_PORT_PLL_ENABLE(port)	_MMIO_PORT(port, _PORT_PLL_A, _PORT_PLL_B)
 
 #define _PORT_PLL_EBB_0_A		0x162034
 #define _PORT_PLL_EBB_0_B		0x6C034
@@ -1214,7 +1265,7 @@ enum skl_disp_power_wells {
 #define   PORT_PLL_P2_SHIFT		8
 #define   PORT_PLL_P2_MASK		(0x1f << PORT_PLL_P2_SHIFT)
 #define   PORT_PLL_P2(x)		((x)  << PORT_PLL_P2_SHIFT)
-#define BXT_PORT_PLL_EBB_0(port)	_PORT3(port, _PORT_PLL_EBB_0_A, \
+#define BXT_PORT_PLL_EBB_0(port)	_MMIO_PORT3(port, _PORT_PLL_EBB_0_A, \
 						_PORT_PLL_EBB_0_B,	\
 						_PORT_PLL_EBB_0_C)
 
@@ -1223,7 +1274,7 @@ enum skl_disp_power_wells {
 #define _PORT_PLL_EBB_4_C		0x6C344
 #define   PORT_PLL_10BIT_CLK_ENABLE	(1 << 13)
 #define   PORT_PLL_RECALIBRATE		(1 << 14)
-#define BXT_PORT_PLL_EBB_4(port)	_PORT3(port, _PORT_PLL_EBB_4_A, \
+#define BXT_PORT_PLL_EBB_4(port)	_MMIO_PORT3(port, _PORT_PLL_EBB_4_A, \
 						_PORT_PLL_EBB_4_B,	\
 						_PORT_PLL_EBB_4_C)
 
@@ -1259,7 +1310,7 @@ enum skl_disp_power_wells {
 #define _PORT_PLL_BASE(port)		_PORT3(port, _PORT_PLL_0_A,	\
 						_PORT_PLL_0_B,		\
 						_PORT_PLL_0_C)
-#define BXT_PORT_PLL(port, idx)		(_PORT_PLL_BASE(port) + (idx) * 4)
+#define BXT_PORT_PLL(port, idx)		_MMIO(_PORT_PLL_BASE(port) + (idx) * 4)
 
 /* BXT PHY common lane registers */
 #define _PORT_CL1CM_DW0_A		0x162000
@@ -1297,7 +1348,7 @@ enum skl_disp_power_wells {
 							_PORT_CL1CM_DW30_A)
 
 /* Defined for PHY0 only */
-#define BXT_PORT_CL2CM_DW6_BC		0x6C358
+#define BXT_PORT_CL2CM_DW6_BC		_MMIO(0x6C358)
 #define   DW6_OLDO_DYN_PWR_DOWN_EN	(1 << 28)
 
 /* BXT PHY Ref registers */
@@ -1337,10 +1388,10 @@ enum skl_disp_power_wells {
 #define _PORT_PCS_DW10_GRP_A		0x162C28
 #define _PORT_PCS_DW10_GRP_B		0x6CC28
 #define _PORT_PCS_DW10_GRP_C		0x6CE28
-#define BXT_PORT_PCS_DW10_LN01(port)	_PORT3(port, _PORT_PCS_DW10_LN01_A, \
+#define BXT_PORT_PCS_DW10_LN01(port)	_MMIO_PORT3(port, _PORT_PCS_DW10_LN01_A, \
 						     _PORT_PCS_DW10_LN01_B, \
 						     _PORT_PCS_DW10_LN01_C)
-#define BXT_PORT_PCS_DW10_GRP(port)	_PORT3(port, _PORT_PCS_DW10_GRP_A,  \
+#define BXT_PORT_PCS_DW10_GRP(port)	_MMIO_PORT3(port, _PORT_PCS_DW10_GRP_A,  \
 						     _PORT_PCS_DW10_GRP_B,  \
 						     _PORT_PCS_DW10_GRP_C)
 #define   TX2_SWING_CALC_INIT		(1 << 31)
@@ -1357,13 +1408,13 @@ enum skl_disp_power_wells {
 #define _PORT_PCS_DW12_GRP_C		0x6CE30
 #define   LANESTAGGER_STRAP_OVRD	(1 << 6)
 #define   LANE_STAGGER_MASK		0x1F
-#define BXT_PORT_PCS_DW12_LN01(port)	_PORT3(port, _PORT_PCS_DW12_LN01_A, \
+#define BXT_PORT_PCS_DW12_LN01(port)	_MMIO_PORT3(port, _PORT_PCS_DW12_LN01_A, \
 						     _PORT_PCS_DW12_LN01_B, \
 						     _PORT_PCS_DW12_LN01_C)
-#define BXT_PORT_PCS_DW12_LN23(port)	_PORT3(port, _PORT_PCS_DW12_LN23_A, \
+#define BXT_PORT_PCS_DW12_LN23(port)	_MMIO_PORT3(port, _PORT_PCS_DW12_LN23_A, \
 						     _PORT_PCS_DW12_LN23_B, \
 						     _PORT_PCS_DW12_LN23_C)
-#define BXT_PORT_PCS_DW12_GRP(port)	_PORT3(port, _PORT_PCS_DW12_GRP_A, \
+#define BXT_PORT_PCS_DW12_GRP(port)	_MMIO_PORT3(port, _PORT_PCS_DW12_GRP_A, \
 						     _PORT_PCS_DW12_GRP_B, \
 						     _PORT_PCS_DW12_GRP_C)
 
@@ -1377,10 +1428,10 @@ enum skl_disp_power_wells {
 #define _PORT_TX_DW2_GRP_A		0x162D08
 #define _PORT_TX_DW2_GRP_B		0x6CD08
 #define _PORT_TX_DW2_GRP_C		0x6CF08
-#define BXT_PORT_TX_DW2_GRP(port)	_PORT3(port, _PORT_TX_DW2_GRP_A,  \
+#define BXT_PORT_TX_DW2_GRP(port)	_MMIO_PORT3(port, _PORT_TX_DW2_GRP_A,  \
 						     _PORT_TX_DW2_GRP_B,  \
 						     _PORT_TX_DW2_GRP_C)
-#define BXT_PORT_TX_DW2_LN0(port)	_PORT3(port, _PORT_TX_DW2_LN0_A,  \
+#define BXT_PORT_TX_DW2_LN0(port)	_MMIO_PORT3(port, _PORT_TX_DW2_LN0_A,  \
 						     _PORT_TX_DW2_LN0_B,  \
 						     _PORT_TX_DW2_LN0_C)
 #define   MARGIN_000_SHIFT		16
@@ -1394,10 +1445,10 @@ enum skl_disp_power_wells {
 #define _PORT_TX_DW3_GRP_A		0x162D0C
 #define _PORT_TX_DW3_GRP_B		0x6CD0C
 #define _PORT_TX_DW3_GRP_C		0x6CF0C
-#define BXT_PORT_TX_DW3_GRP(port)	_PORT3(port, _PORT_TX_DW3_GRP_A,  \
+#define BXT_PORT_TX_DW3_GRP(port)	_MMIO_PORT3(port, _PORT_TX_DW3_GRP_A,  \
 						     _PORT_TX_DW3_GRP_B,  \
 						     _PORT_TX_DW3_GRP_C)
-#define BXT_PORT_TX_DW3_LN0(port)	_PORT3(port, _PORT_TX_DW3_LN0_A,  \
+#define BXT_PORT_TX_DW3_LN0(port)	_MMIO_PORT3(port, _PORT_TX_DW3_LN0_A,  \
 						     _PORT_TX_DW3_LN0_B,  \
 						     _PORT_TX_DW3_LN0_C)
 #define   SCALE_DCOMP_METHOD		(1 << 26)
@@ -1409,10 +1460,10 @@ enum skl_disp_power_wells {
 #define _PORT_TX_DW4_GRP_A		0x162D10
 #define _PORT_TX_DW4_GRP_B		0x6CD10
 #define _PORT_TX_DW4_GRP_C		0x6CF10
-#define BXT_PORT_TX_DW4_LN0(port)	_PORT3(port, _PORT_TX_DW4_LN0_A,  \
+#define BXT_PORT_TX_DW4_LN0(port)	_MMIO_PORT3(port, _PORT_TX_DW4_LN0_A,  \
 						     _PORT_TX_DW4_LN0_B,  \
 						     _PORT_TX_DW4_LN0_C)
-#define BXT_PORT_TX_DW4_GRP(port)	_PORT3(port, _PORT_TX_DW4_GRP_A,  \
+#define BXT_PORT_TX_DW4_GRP(port)	_MMIO_PORT3(port, _PORT_TX_DW4_GRP_A,  \
 						     _PORT_TX_DW4_GRP_B,  \
 						     _PORT_TX_DW4_GRP_C)
 #define   DEEMPH_SHIFT			24
@@ -1423,17 +1474,17 @@ enum skl_disp_power_wells {
 #define _PORT_TX_DW14_LN0_C		0x6C938
 #define   LATENCY_OPTIM_SHIFT		30
 #define   LATENCY_OPTIM			(1 << LATENCY_OPTIM_SHIFT)
-#define BXT_PORT_TX_DW14_LN(port, lane)	(_PORT3((port), _PORT_TX_DW14_LN0_A,   \
+#define BXT_PORT_TX_DW14_LN(port, lane)	_MMIO(_PORT3((port), _PORT_TX_DW14_LN0_A,   \
 							_PORT_TX_DW14_LN0_B,   \
 							_PORT_TX_DW14_LN0_C) + \
 					 _BXT_LANE_OFFSET(lane))
 
 /* UAIMI scratch pad register 1 */
-#define UAIMI_SPR1			0x4F074
+#define UAIMI_SPR1			_MMIO(0x4F074)
 /* SKL VccIO mask */
 #define SKL_VCCIO_MASK			0x1
 /* SKL balance leg register */
-#define DISPIO_CR_TX_BMU_CR0		0x6C00C
+#define DISPIO_CR_TX_BMU_CR0		_MMIO(0x6C00C)
 /* I_boost values */
 #define BALANCE_LEG_SHIFT(port)		(8+3*(port))
 #define BALANCE_LEG_MASK(port)		(7<<(8+3*(port)))
@@ -1450,7 +1501,7 @@ enum skl_disp_power_wells {
  * [0-15] @ 0x100000 gen6,vlv,chv
  * [0-31] @ 0x100000 gen7+
  */
-#define FENCE_REG(i)			(0x2000 + (((i) & 8) << 9) + ((i) & 7) * 4)
+#define FENCE_REG(i)			_MMIO(0x2000 + (((i) & 8) << 9) + ((i) & 7) * 4)
 #define   I830_FENCE_START_MASK		0x07f80000
 #define   I830_FENCE_TILING_Y_SHIFT	12
 #define   I830_FENCE_SIZE_BITS(size)	((ffs((size) >> 19) - 1) << 8)
@@ -1463,21 +1514,21 @@ enum skl_disp_power_wells {
 #define   I915_FENCE_START_MASK		0x0ff00000
 #define   I915_FENCE_SIZE_BITS(size)	((ffs((size) >> 20) - 1) << 8)
 
-#define FENCE_REG_965_LO(i)		(0x03000 + (i) * 8)
-#define FENCE_REG_965_HI(i)		(0x03000 + (i) * 8 + 4)
+#define FENCE_REG_965_LO(i)		_MMIO(0x03000 + (i) * 8)
+#define FENCE_REG_965_HI(i)		_MMIO(0x03000 + (i) * 8 + 4)
 #define   I965_FENCE_PITCH_SHIFT	2
 #define   I965_FENCE_TILING_Y_SHIFT	1
 #define   I965_FENCE_REG_VALID		(1<<0)
 #define   I965_FENCE_MAX_PITCH_VAL	0x0400
 
-#define FENCE_REG_GEN6_LO(i)	(0x100000 + (i) * 8)
-#define FENCE_REG_GEN6_HI(i)	(0x100000 + (i) * 8 + 4)
+#define FENCE_REG_GEN6_LO(i)		_MMIO(0x100000 + (i) * 8)
+#define FENCE_REG_GEN6_HI(i)		_MMIO(0x100000 + (i) * 8 + 4)
 #define   GEN6_FENCE_PITCH_SHIFT	32
 #define   GEN7_FENCE_MAX_PITCH_VAL	0x0800
 
 
 /* control register for cpu gtt access */
-#define TILECTL				0x101000
+#define TILECTL				_MMIO(0x101000)
 #define   TILECTL_SWZCTL			(1 << 0)
 #define   TILECTL_TLBPF			(1 << 1)
 #define   TILECTL_TLB_PREFETCH_DIS	(1 << 2)
@@ -1486,30 +1537,30 @@ enum skl_disp_power_wells {
 /*
  * Instruction and interrupt control regs
  */
-#define PGTBL_CTL	0x02020
+#define PGTBL_CTL	_MMIO(0x02020)
 #define   PGTBL_ADDRESS_LO_MASK	0xfffff000 /* bits [31:12] */
 #define   PGTBL_ADDRESS_HI_MASK	0x000000f0 /* bits [35:32] (gen4) */
-#define PGTBL_ER	0x02024
-#define PRB0_BASE (0x2030-0x30)
-#define PRB1_BASE (0x2040-0x30) /* 830,gen3 */
-#define PRB2_BASE (0x2050-0x30) /* gen3 */
-#define SRB0_BASE (0x2100-0x30) /* gen2 */
-#define SRB1_BASE (0x2110-0x30) /* gen2 */
-#define SRB2_BASE (0x2120-0x30) /* 830 */
-#define SRB3_BASE (0x2130-0x30) /* 830 */
+#define PGTBL_ER	_MMIO(0x02024)
+#define PRB0_BASE	(0x2030-0x30)
+#define PRB1_BASE	(0x2040-0x30) /* 830,gen3 */
+#define PRB2_BASE	(0x2050-0x30) /* gen3 */
+#define SRB0_BASE	(0x2100-0x30) /* gen2 */
+#define SRB1_BASE	(0x2110-0x30) /* gen2 */
+#define SRB2_BASE	(0x2120-0x30) /* 830 */
+#define SRB3_BASE	(0x2130-0x30) /* 830 */
 #define RENDER_RING_BASE	0x02000
 #define BSD_RING_BASE		0x04000
 #define GEN6_BSD_RING_BASE	0x12000
 #define GEN8_BSD2_RING_BASE	0x1c000
 #define VEBOX_RING_BASE		0x1a000
 #define BLT_RING_BASE		0x22000
-#define RING_TAIL(base)		((base)+0x30)
-#define RING_HEAD(base)		((base)+0x34)
-#define RING_START(base)	((base)+0x38)
-#define RING_CTL(base)		((base)+0x3c)
-#define RING_SYNC_0(base)	((base)+0x40)
-#define RING_SYNC_1(base)	((base)+0x44)
-#define RING_SYNC_2(base)	((base)+0x48)
+#define RING_TAIL(base)		_MMIO((base)+0x30)
+#define RING_HEAD(base)		_MMIO((base)+0x34)
+#define RING_START(base)	_MMIO((base)+0x38)
+#define RING_CTL(base)		_MMIO((base)+0x3c)
+#define RING_SYNC_0(base)	_MMIO((base)+0x40)
+#define RING_SYNC_1(base)	_MMIO((base)+0x44)
+#define RING_SYNC_2(base)	_MMIO((base)+0x48)
 #define GEN6_RVSYNC	(RING_SYNC_0(RENDER_RING_BASE))
 #define GEN6_RBSYNC	(RING_SYNC_1(RENDER_RING_BASE))
 #define GEN6_RVESYNC	(RING_SYNC_2(RENDER_RING_BASE))
@@ -1522,51 +1573,52 @@ enum skl_disp_power_wells {
 #define GEN6_VEBSYNC	(RING_SYNC_0(VEBOX_RING_BASE))
 #define GEN6_VERSYNC	(RING_SYNC_1(VEBOX_RING_BASE))
 #define GEN6_VEVSYNC	(RING_SYNC_2(VEBOX_RING_BASE))
-#define GEN6_NOSYNC 0
-#define RING_PSMI_CTL(base)	((base)+0x50)
-#define RING_MAX_IDLE(base)	((base)+0x54)
-#define RING_HWS_PGA(base)	((base)+0x80)
-#define RING_HWS_PGA_GEN6(base)	((base)+0x2080)
-#define RING_RESET_CTL(base)	((base)+0xd0)
+#define GEN6_NOSYNC	INVALID_MMIO_REG
+#define RING_PSMI_CTL(base)	_MMIO((base)+0x50)
+#define RING_MAX_IDLE(base)	_MMIO((base)+0x54)
+#define RING_HWS_PGA(base)	_MMIO((base)+0x80)
+#define RING_HWS_PGA_GEN6(base)	_MMIO((base)+0x2080)
+#define RING_RESET_CTL(base)	_MMIO((base)+0xd0)
 #define   RESET_CTL_REQUEST_RESET  (1 << 0)
 #define   RESET_CTL_READY_TO_RESET (1 << 1)
 
-#define HSW_GTT_CACHE_EN	0x4024
+#define HSW_GTT_CACHE_EN	_MMIO(0x4024)
 #define   GTT_CACHE_EN_ALL	0xF0007FFF
-#define GEN7_WR_WATERMARK	0x4028
-#define GEN7_GFX_PRIO_CTRL	0x402C
-#define ARB_MODE		0x4030
+#define GEN7_WR_WATERMARK	_MMIO(0x4028)
+#define GEN7_GFX_PRIO_CTRL	_MMIO(0x402C)
+#define ARB_MODE		_MMIO(0x4030)
 #define   ARB_MODE_SWIZZLE_SNB	(1<<4)
 #define   ARB_MODE_SWIZZLE_IVB	(1<<5)
-#define GEN7_GFX_PEND_TLB0	0x4034
-#define GEN7_GFX_PEND_TLB1	0x4038
+#define GEN7_GFX_PEND_TLB0	_MMIO(0x4034)
+#define GEN7_GFX_PEND_TLB1	_MMIO(0x4038)
 /* L3, CVS, ZTLB, RCC, CASC LRA min, max values */
-#define GEN7_LRA_LIMITS(i)	(0x403C + (i) * 4)
+#define GEN7_LRA_LIMITS(i)	_MMIO(0x403C + (i) * 4)
 #define GEN7_LRA_LIMITS_REG_NUM	13
-#define GEN7_MEDIA_MAX_REQ_COUNT	0x4070
-#define GEN7_GFX_MAX_REQ_COUNT		0x4074
+#define GEN7_MEDIA_MAX_REQ_COUNT	_MMIO(0x4070)
+#define GEN7_GFX_MAX_REQ_COUNT		_MMIO(0x4074)
 
-#define GAMTARBMODE		0x04a08
+#define GAMTARBMODE		_MMIO(0x04a08)
 #define   ARB_MODE_BWGTLB_DISABLE (1<<9)
 #define   ARB_MODE_SWIZZLE_BDW	(1<<1)
-#define RENDER_HWS_PGA_GEN7	(0x04080)
-#define RING_FAULT_REG(ring)	(0x4094 + 0x100*(ring)->id)
+#define RENDER_HWS_PGA_GEN7	_MMIO(0x04080)
+#define RING_FAULT_REG(ring)	_MMIO(0x4094 + 0x100*(ring)->id)
 #define   RING_FAULT_GTTSEL_MASK (1<<11)
 #define   RING_FAULT_SRCID(x)	(((x) >> 3) & 0xff)
 #define   RING_FAULT_FAULT_TYPE(x) (((x) >> 1) & 0x3)
 #define   RING_FAULT_VALID	(1<<0)
-#define DONE_REG		0x40b0
-#define GEN8_PRIVATE_PAT_LO	0x40e0
-#define GEN8_PRIVATE_PAT_HI	(0x40e0 + 4)
-#define BSD_HWS_PGA_GEN7	(0x04180)
-#define BLT_HWS_PGA_GEN7	(0x04280)
-#define VEBOX_HWS_PGA_GEN7	(0x04380)
-#define RING_ACTHD(base)	((base)+0x74)
-#define RING_ACTHD_UDW(base)	((base)+0x5c)
-#define RING_NOPID(base)	((base)+0x94)
-#define RING_IMR(base)		((base)+0xa8)
-#define RING_HWSTAM(base)	((base)+0x98)
-#define RING_TIMESTAMP(base)	((base)+0x358)
+#define DONE_REG		_MMIO(0x40b0)
+#define GEN8_PRIVATE_PAT_LO	_MMIO(0x40e0)
+#define GEN8_PRIVATE_PAT_HI	_MMIO(0x40e0 + 4)
+#define BSD_HWS_PGA_GEN7	_MMIO(0x04180)
+#define BLT_HWS_PGA_GEN7	_MMIO(0x04280)
+#define VEBOX_HWS_PGA_GEN7	_MMIO(0x04380)
+#define RING_ACTHD(base)	_MMIO((base)+0x74)
+#define RING_ACTHD_UDW(base)	_MMIO((base)+0x5c)
+#define RING_NOPID(base)	_MMIO((base)+0x94)
+#define RING_IMR(base)		_MMIO((base)+0xa8)
+#define RING_HWSTAM(base)	_MMIO((base)+0x98)
+#define RING_TIMESTAMP(base)		_MMIO((base)+0x358)
+#define RING_TIMESTAMP_UDW(base)	_MMIO((base)+0x358 + 4)
 #define   TAIL_ADDR		0x001FFFF8
 #define   HEAD_WRAP_COUNT	0xFFE00000
 #define   HEAD_WRAP_ONE		0x00200000
@@ -1583,57 +1635,65 @@ enum skl_disp_power_wells {
 #define   RING_WAIT		(1<<11) /* gen3+, PRBx_CTL */
 #define   RING_WAIT_SEMAPHORE	(1<<10) /* gen6+ */
 
-#define GEN7_TLB_RD_ADDR	0x4700
+#define GEN7_TLB_RD_ADDR	_MMIO(0x4700)
 
 #if 0
-#define PRB0_TAIL	0x02030
-#define PRB0_HEAD	0x02034
-#define PRB0_START	0x02038
-#define PRB0_CTL	0x0203c
-#define PRB1_TAIL	0x02040 /* 915+ only */
-#define PRB1_HEAD	0x02044 /* 915+ only */
-#define PRB1_START	0x02048 /* 915+ only */
-#define PRB1_CTL	0x0204c /* 915+ only */
+#define PRB0_TAIL	_MMIO(0x2030)
+#define PRB0_HEAD	_MMIO(0x2034)
+#define PRB0_START	_MMIO(0x2038)
+#define PRB0_CTL	_MMIO(0x203c)
+#define PRB1_TAIL	_MMIO(0x2040) /* 915+ only */
+#define PRB1_HEAD	_MMIO(0x2044) /* 915+ only */
+#define PRB1_START	_MMIO(0x2048) /* 915+ only */
+#define PRB1_CTL	_MMIO(0x204c) /* 915+ only */
 #endif
-#define IPEIR_I965	0x02064
-#define IPEHR_I965	0x02068
-#define GEN7_SC_INSTDONE	0x07100
-#define GEN7_SAMPLER_INSTDONE	0x0e160
-#define GEN7_ROW_INSTDONE	0x0e164
+#define IPEIR_I965	_MMIO(0x2064)
+#define IPEHR_I965	_MMIO(0x2068)
+#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 RING_IPEIR(base)	((base)+0x64)
-#define RING_IPEHR(base)	((base)+0x68)
+#define RING_IPEIR(base)	_MMIO((base)+0x64)
+#define RING_IPEHR(base)	_MMIO((base)+0x68)
 /*
  * On GEN4, only the render ring INSTDONE exists and has a different
  * layout than the GEN7+ version.
  * The GEN2 counterpart of this register is GEN2_INSTDONE.
  */
-#define RING_INSTDONE(base)	((base)+0x6c)
-#define RING_INSTPS(base)	((base)+0x70)
-#define RING_DMA_FADD(base)	((base)+0x78)
-#define RING_DMA_FADD_UDW(base)	((base)+0x60) /* gen8+ */
-#define RING_INSTPM(base)	((base)+0xc0)
-#define RING_MI_MODE(base)	((base)+0x9c)
-#define INSTPS		0x02070 /* 965+ only */
-#define GEN4_INSTDONE1	0x0207c /* 965+ only, aka INSTDONE_2 on SNB */
-#define ACTHD_I965	0x02074
-#define HWS_PGA		0x02080
+#define RING_INSTDONE(base)	_MMIO((base)+0x6c)
+#define RING_INSTPS(base)	_MMIO((base)+0x70)
+#define RING_DMA_FADD(base)	_MMIO((base)+0x78)
+#define RING_DMA_FADD_UDW(base)	_MMIO((base)+0x60) /* gen8+ */
+#define RING_INSTPM(base)	_MMIO((base)+0xc0)
+#define RING_MI_MODE(base)	_MMIO((base)+0x9c)
+#define INSTPS		_MMIO(0x2070) /* 965+ only */
+#define GEN4_INSTDONE1	_MMIO(0x207c) /* 965+ only, aka INSTDONE_2 on SNB */
+#define ACTHD_I965	_MMIO(0x2074)
+#define HWS_PGA		_MMIO(0x2080)
 #define HWS_ADDRESS_MASK	0xfffff000
 #define HWS_START_ADDRESS_SHIFT	4
-#define PWRCTXA		0x2088 /* 965GM+ only */
+#define PWRCTXA		_MMIO(0x2088) /* 965GM+ only */
 #define   PWRCTX_EN	(1<<0)
-#define IPEIR		0x02088
-#define IPEHR		0x0208c
-#define GEN2_INSTDONE	0x02090
-#define NOPID		0x02094
-#define HWSTAM		0x02098
-#define DMA_FADD_I8XX	0x020d0
-#define RING_BBSTATE(base)	((base)+0x110)
-#define RING_BBADDR(base)	((base)+0x140)
-#define RING_BBADDR_UDW(base)	((base)+0x168) /* gen8+ */
-
-#define ERROR_GEN6	0x040a0
-#define GEN7_ERR_INT	0x44040
+#define IPEIR		_MMIO(0x2088)
+#define IPEHR		_MMIO(0x208c)
+#define GEN2_INSTDONE	_MMIO(0x2090)
+#define NOPID		_MMIO(0x2094)
+#define HWSTAM		_MMIO(0x2098)
+#define DMA_FADD_I8XX	_MMIO(0x20d0)
+#define RING_BBSTATE(base)	_MMIO((base)+0x110)
+#define   RING_BB_PPGTT		(1 << 5)
+#define RING_SBBADDR(base)	_MMIO((base)+0x114) /* hsw+ */
+#define RING_SBBSTATE(base)	_MMIO((base)+0x118) /* hsw+ */
+#define RING_SBBADDR_UDW(base)	_MMIO((base)+0x11c) /* gen8+ */
+#define RING_BBADDR(base)	_MMIO((base)+0x140)
+#define RING_BBADDR_UDW(base)	_MMIO((base)+0x168) /* gen8+ */
+#define RING_BB_PER_CTX_PTR(base)	_MMIO((base)+0x1c0) /* gen8+ */
+#define RING_INDIRECT_CTX(base)		_MMIO((base)+0x1c4) /* gen8+ */
+#define RING_INDIRECT_CTX_OFFSET(base)	_MMIO((base)+0x1c8) /* gen8+ */
+#define RING_CTX_TIMESTAMP(base)	_MMIO((base)+0x3a8) /* gen8+ */
+
+#define ERROR_GEN6	_MMIO(0x40a0)
+#define GEN7_ERR_INT	_MMIO(0x44040)
 #define   ERR_INT_POISON		(1<<31)
 #define   ERR_INT_MMIO_UNCLAIMED	(1<<13)
 #define   ERR_INT_PIPE_CRC_DONE_C	(1<<8)
@@ -1645,13 +1705,13 @@ enum skl_disp_power_wells {
 #define   ERR_INT_FIFO_UNDERRUN_A	(1<<0)
 #define   ERR_INT_FIFO_UNDERRUN(pipe)	(1<<((pipe)*3))
 
-#define GEN8_FAULT_TLB_DATA0		0x04b10
-#define GEN8_FAULT_TLB_DATA1		0x04b14
+#define GEN8_FAULT_TLB_DATA0		_MMIO(0x4b10)
+#define GEN8_FAULT_TLB_DATA1		_MMIO(0x4b14)
 
-#define FPGA_DBG		0x42300
+#define FPGA_DBG		_MMIO(0x42300)
 #define   FPGA_DBG_RM_NOCLAIM	(1<<31)
 
-#define DERRMR		0x44050
+#define DERRMR		_MMIO(0x44050)
 /* Note that HBLANK events are reserved on bdw+ */
 #define   DERRMR_PIPEA_SCANLINE		(1<<0)
 #define   DERRMR_PIPEA_PRI_FLIP_DONE	(1<<1)
@@ -1675,29 +1735,29 @@ enum skl_disp_power_wells {
  * for various sorts of correct behavior.  The top 16 bits of each are
  * the enables for writing to the corresponding low bit.
  */
-#define _3D_CHICKEN	0x02084
+#define _3D_CHICKEN	_MMIO(0x2084)
 #define  _3D_CHICKEN_HIZ_PLANE_DISABLE_MSAA_4X_SNB	(1 << 10)
-#define _3D_CHICKEN2	0x0208c
+#define _3D_CHICKEN2	_MMIO(0x208c)
 /* Disables pipelining of read flushes past the SF-WIZ interface.
  * Required on all Ironlake steppings according to the B-Spec, but the
  * particular danger of not doing so is not specified.
  */
 # define _3D_CHICKEN2_WM_READ_PIPELINED			(1 << 14)
-#define _3D_CHICKEN3	0x02090
+#define _3D_CHICKEN3	_MMIO(0x2090)
 #define  _3D_CHICKEN_SF_DISABLE_OBJEND_CULL		(1 << 10)
 #define  _3D_CHICKEN3_SF_DISABLE_FASTCLIP_CULL		(1 << 5)
 #define  _3D_CHICKEN_SDE_LIMIT_FIFO_POLY_DEPTH(x)	((x)<<1) /* gen8+ */
 #define  _3D_CHICKEN3_SF_DISABLE_PIPELINED_ATTR_FETCH	(1 << 1) /* gen6 */
 
-#define MI_MODE		0x0209c
+#define MI_MODE		_MMIO(0x209c)
 # define VS_TIMER_DISPATCH				(1 << 6)
 # define MI_FLUSH_ENABLE				(1 << 12)
 # define ASYNC_FLIP_PERF_DISABLE			(1 << 14)
 # define MODE_IDLE					(1 << 9)
 # define STOP_RING					(1 << 8)
 
-#define GEN6_GT_MODE	0x20d0
-#define GEN7_GT_MODE	0x7008
+#define GEN6_GT_MODE	_MMIO(0x20d0)
+#define GEN7_GT_MODE	_MMIO(0x7008)
 #define   GEN6_WIZ_HASHING(hi, lo)			(((hi) << 9) | ((lo) << 7))
 #define   GEN6_WIZ_HASHING_8x8				GEN6_WIZ_HASHING(0, 0)
 #define   GEN6_WIZ_HASHING_8x4				GEN6_WIZ_HASHING(0, 1)
@@ -1707,9 +1767,9 @@ enum skl_disp_power_wells {
 #define   GEN9_IZ_HASHING_MASK(slice)			(0x3 << ((slice) * 2))
 #define   GEN9_IZ_HASHING(slice, val)			((val) << ((slice) * 2))
 
-#define GFX_MODE	0x02520
-#define GFX_MODE_GEN7	0x0229c
-#define RING_MODE_GEN7(ring)	((ring)->mmio_base+0x29c)
+#define GFX_MODE	_MMIO(0x2520)
+#define GFX_MODE_GEN7	_MMIO(0x229c)
+#define RING_MODE_GEN7(ring)	_MMIO((ring)->mmio_base+0x29c)
 #define   GFX_RUN_LIST_ENABLE		(1<<15)
 #define   GFX_INTERRUPT_STEERING	(1<<14)
 #define   GFX_TLB_INVALIDATE_EXPLICIT	(1<<13)
@@ -1727,36 +1787,36 @@ enum skl_disp_power_wells {
 #define VLV_DISPLAY_BASE 0x180000
 #define VLV_MIPI_BASE VLV_DISPLAY_BASE
 
-#define VLV_GU_CTL0	(VLV_DISPLAY_BASE + 0x2030)
-#define VLV_GU_CTL1	(VLV_DISPLAY_BASE + 0x2034)
-#define SCPD0		0x0209c /* 915+ only */
-#define IER		0x020a0
-#define IIR		0x020a4
-#define IMR		0x020a8
-#define ISR		0x020ac
-#define VLV_GUNIT_CLOCK_GATE	(VLV_DISPLAY_BASE + 0x2060)
+#define VLV_GU_CTL0	_MMIO(VLV_DISPLAY_BASE + 0x2030)
+#define VLV_GU_CTL1	_MMIO(VLV_DISPLAY_BASE + 0x2034)
+#define SCPD0		_MMIO(0x209c) /* 915+ only */
+#define IER		_MMIO(0x20a0)
+#define IIR		_MMIO(0x20a4)
+#define IMR		_MMIO(0x20a8)
+#define ISR		_MMIO(0x20ac)
+#define VLV_GUNIT_CLOCK_GATE	_MMIO(VLV_DISPLAY_BASE + 0x2060)
 #define   GINT_DIS		(1<<22)
 #define   GCFG_DIS		(1<<8)
-#define VLV_GUNIT_CLOCK_GATE2	(VLV_DISPLAY_BASE + 0x2064)
-#define VLV_IIR_RW	(VLV_DISPLAY_BASE + 0x2084)
-#define VLV_IER		(VLV_DISPLAY_BASE + 0x20a0)
-#define VLV_IIR		(VLV_DISPLAY_BASE + 0x20a4)
-#define VLV_IMR		(VLV_DISPLAY_BASE + 0x20a8)
-#define VLV_ISR		(VLV_DISPLAY_BASE + 0x20ac)
-#define VLV_PCBR	(VLV_DISPLAY_BASE + 0x2120)
+#define VLV_GUNIT_CLOCK_GATE2	_MMIO(VLV_DISPLAY_BASE + 0x2064)
+#define VLV_IIR_RW	_MMIO(VLV_DISPLAY_BASE + 0x2084)
+#define VLV_IER		_MMIO(VLV_DISPLAY_BASE + 0x20a0)
+#define VLV_IIR		_MMIO(VLV_DISPLAY_BASE + 0x20a4)
+#define VLV_IMR		_MMIO(VLV_DISPLAY_BASE + 0x20a8)
+#define VLV_ISR		_MMIO(VLV_DISPLAY_BASE + 0x20ac)
+#define VLV_PCBR	_MMIO(VLV_DISPLAY_BASE + 0x2120)
 #define VLV_PCBR_ADDR_SHIFT	12
 
 #define   DISPLAY_PLANE_FLIP_PENDING(plane) (1<<(11-(plane))) /* A and B only */
-#define EIR		0x020b0
-#define EMR		0x020b4
-#define ESR		0x020b8
+#define EIR		_MMIO(0x20b0)
+#define EMR		_MMIO(0x20b4)
+#define ESR		_MMIO(0x20b8)
 #define   GM45_ERROR_PAGE_TABLE				(1<<5)
 #define   GM45_ERROR_MEM_PRIV				(1<<4)
 #define   I915_ERROR_PAGE_TABLE				(1<<4)
 #define   GM45_ERROR_CP_PRIV				(1<<3)
 #define   I915_ERROR_MEMORY_REFRESH			(1<<1)
 #define   I915_ERROR_INSTRUCTION			(1<<0)
-#define INSTPM	        0x020c0
+#define INSTPM	        _MMIO(0x20c0)
 #define   INSTPM_SELF_EN (1<<12) /* 915GM only */
 #define   INSTPM_AGPBUSY_INT_EN (1<<11) /* gen3: when disabled, pending interrupts
 					will not assert AGPBUSY# and will only
@@ -1764,14 +1824,14 @@ enum skl_disp_power_wells {
 #define   INSTPM_FORCE_ORDERING				(1<<7) /* GEN6+ */
 #define   INSTPM_TLB_INVALIDATE	(1<<9)
 #define   INSTPM_SYNC_FLUSH	(1<<5)
-#define ACTHD	        0x020c8
-#define MEM_MODE	0x020cc
+#define ACTHD	        _MMIO(0x20c8)
+#define MEM_MODE	_MMIO(0x20cc)
 #define   MEM_DISPLAY_B_TRICKLE_FEED_DISABLE (1<<3) /* 830 only */
 #define   MEM_DISPLAY_A_TRICKLE_FEED_DISABLE (1<<2) /* 830/845 only */
 #define   MEM_DISPLAY_TRICKLE_FEED_DISABLE (1<<2) /* 85x only */
-#define FW_BLC		0x020d8
-#define FW_BLC2		0x020dc
-#define FW_BLC_SELF	0x020e0 /* 915+ only */
+#define FW_BLC		_MMIO(0x20d8)
+#define FW_BLC2		_MMIO(0x20dc)
+#define FW_BLC_SELF	_MMIO(0x20e0) /* 915+ only */
 #define   FW_BLC_SELF_EN_MASK      (1<<31)
 #define   FW_BLC_SELF_FIFO_MASK    (1<<16) /* 945 only */
 #define   FW_BLC_SELF_EN           (1<<15) /* 945 only */
@@ -1779,7 +1839,7 @@ enum skl_disp_power_wells {
 #define MM_FIFO_WATERMARK   0x0001F000
 #define LM_BURST_LENGTH     0x00000700
 #define LM_FIFO_WATERMARK   0x0000001F
-#define MI_ARB_STATE	0x020e4 /* 915+ only */
+#define MI_ARB_STATE	_MMIO(0x20e4) /* 915+ only */
 
 /* Make render/texture TLB fetches lower priorty than associated data
  *   fetches. This is not turned on by default
@@ -1843,11 +1903,11 @@ enum skl_disp_power_wells {
 #define   MI_ARB_DISPLAY_PRIORITY_A_B		(0 << 0)	/* display A > display B */
 #define   MI_ARB_DISPLAY_PRIORITY_B_A		(1 << 0)	/* display B > display A */
 
-#define MI_STATE	0x020e4 /* gen2 only */
+#define MI_STATE	_MMIO(0x20e4) /* gen2 only */
 #define   MI_AGPBUSY_INT_EN			(1 << 1) /* 85x only */
 #define   MI_AGPBUSY_830_MODE			(1 << 0) /* 85x only */
 
-#define CACHE_MODE_0	0x02120 /* 915+ only */
+#define CACHE_MODE_0	_MMIO(0x2120) /* 915+ only */
 #define   CM0_PIPELINED_RENDER_FLUSH_DISABLE (1<<8)
 #define   CM0_IZ_OPT_DISABLE      (1<<6)
 #define   CM0_ZR_OPT_DISABLE      (1<<5)
@@ -1856,32 +1916,32 @@ enum skl_disp_power_wells {
 #define   CM0_COLOR_EVICT_DISABLE (1<<3)
 #define   CM0_DEPTH_WRITE_DISABLE (1<<1)
 #define   CM0_RC_OP_FLUSH_DISABLE (1<<0)
-#define GFX_FLSH_CNTL	0x02170 /* 915+ only */
-#define GFX_FLSH_CNTL_GEN6	0x101008
+#define GFX_FLSH_CNTL	_MMIO(0x2170) /* 915+ only */
+#define GFX_FLSH_CNTL_GEN6	_MMIO(0x101008)
 #define   GFX_FLSH_CNTL_EN	(1<<0)
-#define ECOSKPD		0x021d0
+#define ECOSKPD		_MMIO(0x21d0)
 #define   ECO_GATING_CX_ONLY	(1<<3)
 #define   ECO_FLIP_DONE		(1<<0)
 
-#define CACHE_MODE_0_GEN7	0x7000 /* IVB+ */
+#define CACHE_MODE_0_GEN7	_MMIO(0x7000) /* IVB+ */
 #define RC_OP_FLUSH_ENABLE (1<<0)
 #define   HIZ_RAW_STALL_OPT_DISABLE (1<<2)
-#define CACHE_MODE_1		0x7004 /* IVB+ */
+#define CACHE_MODE_1		_MMIO(0x7004) /* IVB+ */
 #define   PIXEL_SUBSPAN_COLLECT_OPT_DISABLE	(1<<6)
 #define   GEN8_4x4_STC_OPTIMIZATION_DISABLE	(1<<6)
 #define   GEN9_PARTIAL_RESOLVE_IN_VC_DISABLE	(1<<1)
 
-#define GEN6_BLITTER_ECOSKPD	0x221d0
+#define GEN6_BLITTER_ECOSKPD	_MMIO(0x221d0)
 #define   GEN6_BLITTER_LOCK_SHIFT			16
 #define   GEN6_BLITTER_FBC_NOTIFY			(1<<3)
 
-#define GEN6_RC_SLEEP_PSMI_CONTROL	0x2050
+#define GEN6_RC_SLEEP_PSMI_CONTROL	_MMIO(0x2050)
 #define   GEN6_PSMI_SLEEP_MSG_DISABLE	(1 << 0)
 #define   GEN8_RC_SEMA_IDLE_MSG_DISABLE	(1 << 12)
 #define   GEN8_FF_DOP_CLOCK_GATE_DISABLE	(1<<10)
 
 /* Fuse readout registers for GT */
-#define CHV_FUSE_GT			(VLV_DISPLAY_BASE + 0x2168)
+#define CHV_FUSE_GT			_MMIO(VLV_DISPLAY_BASE + 0x2168)
 #define   CHV_FGT_DISABLE_SS0		(1 << 10)
 #define   CHV_FGT_DISABLE_SS1		(1 << 11)
 #define   CHV_FGT_EU_DIS_SS0_R0_SHIFT	16
@@ -1893,7 +1953,7 @@ enum skl_disp_power_wells {
 #define   CHV_FGT_EU_DIS_SS1_R1_SHIFT	28
 #define   CHV_FGT_EU_DIS_SS1_R1_MASK	(0xf << CHV_FGT_EU_DIS_SS1_R1_SHIFT)
 
-#define GEN8_FUSE2			0x9120
+#define GEN8_FUSE2			_MMIO(0x9120)
 #define   GEN8_F2_SS_DIS_SHIFT		21
 #define   GEN8_F2_SS_DIS_MASK		(0x7 << GEN8_F2_SS_DIS_SHIFT)
 #define   GEN8_F2_S_ENA_SHIFT		25
@@ -1902,22 +1962,22 @@ enum skl_disp_power_wells {
 #define   GEN9_F2_SS_DIS_SHIFT		20
 #define   GEN9_F2_SS_DIS_MASK		(0xf << GEN9_F2_SS_DIS_SHIFT)
 
-#define GEN8_EU_DISABLE0		0x9134
+#define GEN8_EU_DISABLE0		_MMIO(0x9134)
 #define   GEN8_EU_DIS0_S0_MASK		0xffffff
 #define   GEN8_EU_DIS0_S1_SHIFT		24
 #define   GEN8_EU_DIS0_S1_MASK		(0xff << GEN8_EU_DIS0_S1_SHIFT)
 
-#define GEN8_EU_DISABLE1		0x9138
+#define GEN8_EU_DISABLE1		_MMIO(0x9138)
 #define   GEN8_EU_DIS1_S1_MASK		0xffff
 #define   GEN8_EU_DIS1_S2_SHIFT		16
 #define   GEN8_EU_DIS1_S2_MASK		(0xffff << GEN8_EU_DIS1_S2_SHIFT)
 
-#define GEN8_EU_DISABLE2		0x913c
+#define GEN8_EU_DISABLE2		_MMIO(0x913c)
 #define   GEN8_EU_DIS2_S2_MASK		0xff
 
-#define GEN9_EU_DISABLE(slice)		(0x9134 + (slice)*0x4)
+#define GEN9_EU_DISABLE(slice)		_MMIO(0x9134 + (slice)*0x4)
 
-#define GEN6_BSD_SLEEP_PSMI_CONTROL	0x12050
+#define GEN6_BSD_SLEEP_PSMI_CONTROL	_MMIO(0x12050)
 #define   GEN6_BSD_SLEEP_MSG_DISABLE	(1 << 0)
 #define   GEN6_BSD_SLEEP_FLUSH_DISABLE	(1 << 2)
 #define   GEN6_BSD_SLEEP_INDICATOR	(1 << 3)
@@ -1995,9 +2055,9 @@ enum skl_disp_power_wells {
 #define I915_ASLE_INTERRUPT				(1<<0)
 #define I915_BSD_USER_INTERRUPT				(1<<25)
 
-#define GEN6_BSD_RNCID			0x12198
+#define GEN6_BSD_RNCID			_MMIO(0x12198)
 
-#define GEN7_FF_THREAD_MODE		0x20a0
+#define GEN7_FF_THREAD_MODE		_MMIO(0x20a0)
 #define   GEN7_FF_SCHED_MASK		0x0077070
 #define   GEN8_FF_DS_REF_CNT_FFME	(1 << 19)
 #define   GEN7_FF_TS_SCHED_HS1		(0x5<<16)
@@ -2018,9 +2078,9 @@ enum skl_disp_power_wells {
  * Framebuffer compression (915+ only)
  */
 
-#define FBC_CFB_BASE		0x03200 /* 4k page aligned */
-#define FBC_LL_BASE		0x03204 /* 4k page aligned */
-#define FBC_CONTROL		0x03208
+#define FBC_CFB_BASE		_MMIO(0x3200) /* 4k page aligned */
+#define FBC_LL_BASE		_MMIO(0x3204) /* 4k page aligned */
+#define FBC_CONTROL		_MMIO(0x3208)
 #define   FBC_CTL_EN		(1<<31)
 #define   FBC_CTL_PERIODIC	(1<<30)
 #define   FBC_CTL_INTERVAL_SHIFT (16)
@@ -2028,14 +2088,14 @@ enum skl_disp_power_wells {
 #define   FBC_CTL_C3_IDLE	(1<<13)
 #define   FBC_CTL_STRIDE_SHIFT	(5)
 #define   FBC_CTL_FENCENO_SHIFT	(0)
-#define FBC_COMMAND		0x0320c
+#define FBC_COMMAND		_MMIO(0x320c)
 #define   FBC_CMD_COMPRESS	(1<<0)
-#define FBC_STATUS		0x03210
+#define FBC_STATUS		_MMIO(0x3210)
 #define   FBC_STAT_COMPRESSING	(1<<31)
 #define   FBC_STAT_COMPRESSED	(1<<30)
 #define   FBC_STAT_MODIFIED	(1<<29)
 #define   FBC_STAT_CURRENT_LINE_SHIFT	(0)
-#define FBC_CONTROL2		0x03214
+#define FBC_CONTROL2		_MMIO(0x3214)
 #define   FBC_CTL_FENCE_DBL	(0<<4)
 #define   FBC_CTL_IDLE_IMM	(0<<2)
 #define   FBC_CTL_IDLE_FULL	(1<<2)
@@ -2043,17 +2103,17 @@ enum skl_disp_power_wells {
 #define   FBC_CTL_IDLE_DEBUG	(3<<2)
 #define   FBC_CTL_CPU_FENCE	(1<<1)
 #define   FBC_CTL_PLANE(plane)	((plane)<<0)
-#define FBC_FENCE_OFF		0x03218 /* BSpec typo has 321Bh */
-#define FBC_TAG(i)		(0x03300 + (i) * 4)
+#define FBC_FENCE_OFF		_MMIO(0x3218) /* BSpec typo has 321Bh */
+#define FBC_TAG(i)		_MMIO(0x3300 + (i) * 4)
 
-#define FBC_STATUS2		0x43214
+#define FBC_STATUS2		_MMIO(0x43214)
 #define  FBC_COMPRESSION_MASK	0x7ff
 
 #define FBC_LL_SIZE		(1536)
 
 /* Framebuffer compression for GM45+ */
-#define DPFC_CB_BASE		0x3200
-#define DPFC_CONTROL		0x3208
+#define DPFC_CB_BASE		_MMIO(0x3200)
+#define DPFC_CONTROL		_MMIO(0x3208)
 #define   DPFC_CTL_EN		(1<<31)
 #define   DPFC_CTL_PLANE(plane)	((plane)<<30)
 #define   IVB_DPFC_CTL_PLANE(plane)	((plane)<<29)
@@ -2064,37 +2124,37 @@ enum skl_disp_power_wells {
 #define   DPFC_CTL_LIMIT_1X	(0<<6)
 #define   DPFC_CTL_LIMIT_2X	(1<<6)
 #define   DPFC_CTL_LIMIT_4X	(2<<6)
-#define DPFC_RECOMP_CTL		0x320c
+#define DPFC_RECOMP_CTL		_MMIO(0x320c)
 #define   DPFC_RECOMP_STALL_EN	(1<<27)
 #define   DPFC_RECOMP_STALL_WM_SHIFT (16)
 #define   DPFC_RECOMP_STALL_WM_MASK (0x07ff0000)
 #define   DPFC_RECOMP_TIMER_COUNT_SHIFT (0)
 #define   DPFC_RECOMP_TIMER_COUNT_MASK (0x0000003f)
-#define DPFC_STATUS		0x3210
+#define DPFC_STATUS		_MMIO(0x3210)
 #define   DPFC_INVAL_SEG_SHIFT  (16)
 #define   DPFC_INVAL_SEG_MASK	(0x07ff0000)
 #define   DPFC_COMP_SEG_SHIFT	(0)
 #define   DPFC_COMP_SEG_MASK	(0x000003ff)
-#define DPFC_STATUS2		0x3214
-#define DPFC_FENCE_YOFF		0x3218
-#define DPFC_CHICKEN		0x3224
+#define DPFC_STATUS2		_MMIO(0x3214)
+#define DPFC_FENCE_YOFF		_MMIO(0x3218)
+#define DPFC_CHICKEN		_MMIO(0x3224)
 #define   DPFC_HT_MODIFY	(1<<31)
 
 /* Framebuffer compression for Ironlake */
-#define ILK_DPFC_CB_BASE	0x43200
-#define ILK_DPFC_CONTROL	0x43208
+#define ILK_DPFC_CB_BASE	_MMIO(0x43200)
+#define ILK_DPFC_CONTROL	_MMIO(0x43208)
 #define   FBC_CTL_FALSE_COLOR	(1<<10)
 /* The bit 28-8 is reserved */
 #define   DPFC_RESERVED		(0x1FFFFF00)
-#define ILK_DPFC_RECOMP_CTL	0x4320c
-#define ILK_DPFC_STATUS		0x43210
-#define ILK_DPFC_FENCE_YOFF	0x43218
-#define ILK_DPFC_CHICKEN	0x43224
-#define ILK_FBC_RT_BASE		0x2128
+#define ILK_DPFC_RECOMP_CTL	_MMIO(0x4320c)
+#define ILK_DPFC_STATUS		_MMIO(0x43210)
+#define ILK_DPFC_FENCE_YOFF	_MMIO(0x43218)
+#define ILK_DPFC_CHICKEN	_MMIO(0x43224)
+#define ILK_FBC_RT_BASE		_MMIO(0x2128)
 #define   ILK_FBC_RT_VALID	(1<<0)
 #define   SNB_FBC_FRONT_BUFFER	(1<<1)
 
-#define ILK_DISPLAY_CHICKEN1	0x42000
+#define ILK_DISPLAY_CHICKEN1	_MMIO(0x42000)
 #define   ILK_FBCQ_DIS		(1<<22)
 #define	  ILK_PABSTRETCH_DIS	(1<<21)
 
@@ -2104,31 +2164,31 @@ enum skl_disp_power_wells {
  *
  * The following two registers are of type GTTMMADR
  */
-#define SNB_DPFC_CTL_SA		0x100100
+#define SNB_DPFC_CTL_SA		_MMIO(0x100100)
 #define   SNB_CPU_FENCE_ENABLE	(1<<29)
-#define DPFC_CPU_FENCE_OFFSET	0x100104
+#define DPFC_CPU_FENCE_OFFSET	_MMIO(0x100104)
 
 /* Framebuffer compression for Ivybridge */
-#define IVB_FBC_RT_BASE			0x7020
+#define IVB_FBC_RT_BASE			_MMIO(0x7020)
 
-#define IPS_CTL		0x43408
+#define IPS_CTL		_MMIO(0x43408)
 #define   IPS_ENABLE	(1 << 31)
 
-#define MSG_FBC_REND_STATE	0x50380
+#define MSG_FBC_REND_STATE	_MMIO(0x50380)
 #define   FBC_REND_NUKE		(1<<2)
 #define   FBC_REND_CACHE_CLEAN	(1<<1)
 
 /*
  * GPIO regs
  */
-#define GPIOA			0x5010
-#define GPIOB			0x5014
-#define GPIOC			0x5018
-#define GPIOD			0x501c
-#define GPIOE			0x5020
-#define GPIOF			0x5024
-#define GPIOG			0x5028
-#define GPIOH			0x502c
+#define GPIOA			_MMIO(0x5010)
+#define GPIOB			_MMIO(0x5014)
+#define GPIOC			_MMIO(0x5018)
+#define GPIOD			_MMIO(0x501c)
+#define GPIOE			_MMIO(0x5020)
+#define GPIOF			_MMIO(0x5024)
+#define GPIOG			_MMIO(0x5028)
+#define GPIOH			_MMIO(0x502c)
 # define GPIO_CLOCK_DIR_MASK		(1 << 0)
 # define GPIO_CLOCK_DIR_IN		(0 << 1)
 # define GPIO_CLOCK_DIR_OUT		(1 << 1)
@@ -2144,7 +2204,7 @@ enum skl_disp_power_wells {
 # define GPIO_DATA_VAL_IN		(1 << 12)
 # define GPIO_DATA_PULLUP_DISABLE	(1 << 13)
 
-#define GMBUS0			(dev_priv->gpio_mmio_base + 0x5100) /* clock/port select */
+#define GMBUS0			_MMIO(dev_priv->gpio_mmio_base + 0x5100) /* clock/port select */
 #define   GMBUS_RATE_100KHZ	(0<<8)
 #define   GMBUS_RATE_50KHZ	(1<<8)
 #define   GMBUS_RATE_400KHZ	(2<<8) /* reserved on Pineview */
@@ -2163,7 +2223,7 @@ enum skl_disp_power_wells {
 #define   GMBUS_PIN_2_BXT	2
 #define   GMBUS_PIN_3_BXT	3
 #define   GMBUS_NUM_PINS	7 /* including 0 */
-#define GMBUS1			(dev_priv->gpio_mmio_base + 0x5104) /* command/status */
+#define GMBUS1			_MMIO(dev_priv->gpio_mmio_base + 0x5104) /* command/status */
 #define   GMBUS_SW_CLR_INT	(1<<31)
 #define   GMBUS_SW_RDY		(1<<30)
 #define   GMBUS_ENT		(1<<29) /* enable timeout */
@@ -2177,7 +2237,7 @@ enum skl_disp_power_wells {
 #define   GMBUS_SLAVE_ADDR_SHIFT 1
 #define   GMBUS_SLAVE_READ	(1<<0)
 #define   GMBUS_SLAVE_WRITE	(0<<0)
-#define GMBUS2			(dev_priv->gpio_mmio_base + 0x5108) /* status */
+#define GMBUS2			_MMIO(dev_priv->gpio_mmio_base + 0x5108) /* status */
 #define   GMBUS_INUSE		(1<<15)
 #define   GMBUS_HW_WAIT_PHASE	(1<<14)
 #define   GMBUS_STALL_TIMEOUT	(1<<13)
@@ -2185,14 +2245,14 @@ enum skl_disp_power_wells {
 #define   GMBUS_HW_RDY		(1<<11)
 #define   GMBUS_SATOER		(1<<10)
 #define   GMBUS_ACTIVE		(1<<9)
-#define GMBUS3			(dev_priv->gpio_mmio_base + 0x510c) /* data buffer bytes 3-0 */
-#define GMBUS4			(dev_priv->gpio_mmio_base + 0x5110) /* interrupt mask (Pineview+) */
+#define GMBUS3			_MMIO(dev_priv->gpio_mmio_base + 0x510c) /* data buffer bytes 3-0 */
+#define GMBUS4			_MMIO(dev_priv->gpio_mmio_base + 0x5110) /* interrupt mask (Pineview+) */
 #define   GMBUS_SLAVE_TIMEOUT_EN (1<<4)
 #define   GMBUS_NAK_EN		(1<<3)
 #define   GMBUS_IDLE_EN		(1<<2)
 #define   GMBUS_HW_WAIT_EN	(1<<1)
 #define   GMBUS_HW_RDY_EN	(1<<0)
-#define GMBUS5			(dev_priv->gpio_mmio_base + 0x5120) /* byte index */
+#define GMBUS5			_MMIO(dev_priv->gpio_mmio_base + 0x5120) /* byte index */
 #define   GMBUS_2BYTE_INDEX_EN	(1<<31)
 
 /*
@@ -2201,11 +2261,11 @@ enum skl_disp_power_wells {
 #define _DPLL_A (dev_priv->info.display_mmio_offset + 0x6014)
 #define _DPLL_B (dev_priv->info.display_mmio_offset + 0x6018)
 #define _CHV_DPLL_C (dev_priv->info.display_mmio_offset + 0x6030)
-#define DPLL(pipe) _PIPE3((pipe), _DPLL_A, _DPLL_B, _CHV_DPLL_C)
+#define DPLL(pipe) _MMIO_PIPE3((pipe), _DPLL_A, _DPLL_B, _CHV_DPLL_C)
 
-#define VGA0	0x6000
-#define VGA1	0x6004
-#define VGA_PD	0x6010
+#define VGA0	_MMIO(0x6000)
+#define VGA1	_MMIO(0x6004)
+#define VGA_PD	_MMIO(0x6010)
 #define   VGA0_PD_P2_DIV_4	(1 << 7)
 #define   VGA0_PD_P1_DIV_2	(1 << 5)
 #define   VGA0_PD_P1_SHIFT	0
@@ -2241,9 +2301,9 @@ enum skl_disp_power_wells {
 #define   DPLL_FPA01_P1_POST_DIV_MASK_I830	0x001f0000
 
 /* Additional CHV pll/phy registers */
-#define DPIO_PHY_STATUS			(VLV_DISPLAY_BASE + 0x6240)
+#define DPIO_PHY_STATUS			_MMIO(VLV_DISPLAY_BASE + 0x6240)
 #define   DPLL_PORTD_READY_MASK		(0xf)
-#define DISPLAY_PHY_CONTROL (VLV_DISPLAY_BASE + 0x60100)
+#define DISPLAY_PHY_CONTROL _MMIO(VLV_DISPLAY_BASE + 0x60100)
 #define   PHY_CH_POWER_DOWN_OVRD_EN(phy, ch)	(1 << (2*(phy)+(ch)+27))
 #define   PHY_LDO_DELAY_0NS			0x0
 #define   PHY_LDO_DELAY_200NS			0x1
@@ -2254,7 +2314,7 @@ enum skl_disp_power_wells {
 #define   PHY_CH_DEEP_PSR			0x7
 #define   PHY_CH_POWER_MODE(mode, phy, ch)	((mode) << (6*(phy)+3*(ch)+2))
 #define   PHY_COM_LANE_RESET_DEASSERT(phy)	(1 << (phy))
-#define DISPLAY_PHY_STATUS (VLV_DISPLAY_BASE + 0x60104)
+#define DISPLAY_PHY_STATUS _MMIO(VLV_DISPLAY_BASE + 0x60104)
 #define   PHY_POWERGOOD(phy)	(((phy) == DPIO_PHY0) ? (1<<31) : (1<<30))
 #define   PHY_STATUS_CMN_LDO(phy, ch)                   (1 << (6-(6*(phy)+3*(ch))))
 #define   PHY_STATUS_SPLINE_LDO(phy, ch, spline)        (1 << (8-(6*(phy)+3*(ch)+(spline))))
@@ -2300,7 +2360,7 @@ enum skl_disp_power_wells {
 #define _DPLL_A_MD (dev_priv->info.display_mmio_offset + 0x601c)
 #define _DPLL_B_MD (dev_priv->info.display_mmio_offset + 0x6020)
 #define _CHV_DPLL_C_MD (dev_priv->info.display_mmio_offset + 0x603c)
-#define DPLL_MD(pipe) _PIPE3((pipe), _DPLL_A_MD, _DPLL_B_MD, _CHV_DPLL_C_MD)
+#define DPLL_MD(pipe) _MMIO_PIPE3((pipe), _DPLL_A_MD, _DPLL_B_MD, _CHV_DPLL_C_MD)
 
 /*
  * UDI pixel divider, controlling how many pixels are stuffed into a packet.
@@ -2339,12 +2399,12 @@ enum skl_disp_power_wells {
 #define   DPLL_MD_VGA_UDI_MULTIPLIER_MASK	0x0000003f
 #define   DPLL_MD_VGA_UDI_MULTIPLIER_SHIFT	0
 
-#define _FPA0	0x06040
-#define _FPA1	0x06044
-#define _FPB0	0x06048
-#define _FPB1	0x0604c
-#define FP0(pipe) _PIPE(pipe, _FPA0, _FPB0)
-#define FP1(pipe) _PIPE(pipe, _FPA1, _FPB1)
+#define _FPA0	0x6040
+#define _FPA1	0x6044
+#define _FPB0	0x6048
+#define _FPB1	0x604c
+#define FP0(pipe) _MMIO_PIPE(pipe, _FPA0, _FPB0)
+#define FP1(pipe) _MMIO_PIPE(pipe, _FPA1, _FPB1)
 #define   FP_N_DIV_MASK		0x003f0000
 #define   FP_N_PINEVIEW_DIV_MASK	0x00ff0000
 #define   FP_N_DIV_SHIFT		16
@@ -2353,7 +2413,7 @@ enum skl_disp_power_wells {
 #define   FP_M2_DIV_MASK	0x0000003f
 #define   FP_M2_PINEVIEW_DIV_MASK	0x000000ff
 #define   FP_M2_DIV_SHIFT		 0
-#define DPLL_TEST	0x606c
+#define DPLL_TEST	_MMIO(0x606c)
 #define   DPLLB_TEST_SDVO_DIV_1		(0 << 22)
 #define   DPLLB_TEST_SDVO_DIV_2		(1 << 22)
 #define   DPLLB_TEST_SDVO_DIV_4		(2 << 22)
@@ -2364,12 +2424,12 @@ enum skl_disp_power_wells {
 #define   DPLLA_TEST_N_BYPASS		(1 << 3)
 #define   DPLLA_TEST_M_BYPASS		(1 << 2)
 #define   DPLLA_INPUT_BUFFER_ENABLE	(1 << 0)
-#define D_STATE		0x6104
+#define D_STATE		_MMIO(0x6104)
 #define  DSTATE_GFX_RESET_I830			(1<<6)
 #define  DSTATE_PLL_D3_OFF			(1<<3)
 #define  DSTATE_GFX_CLOCK_GATING		(1<<1)
 #define  DSTATE_DOT_CLOCK_GATING		(1<<0)
-#define DSPCLK_GATE_D	(dev_priv->info.display_mmio_offset + 0x6200)
+#define DSPCLK_GATE_D	_MMIO(dev_priv->info.display_mmio_offset + 0x6200)
 # define DPUNIT_B_CLOCK_GATE_DISABLE		(1 << 30) /* 965 */
 # define VSUNIT_CLOCK_GATE_DISABLE		(1 << 29) /* 965 */
 # define VRHUNIT_CLOCK_GATE_DISABLE		(1 << 28) /* 965 */
@@ -2408,7 +2468,7 @@ enum skl_disp_power_wells {
 # define ZVUNIT_CLOCK_GATE_DISABLE		(1 << 0) /* 830 */
 # define OVLUNIT_CLOCK_GATE_DISABLE		(1 << 0) /* 845,865 */
 
-#define RENCLK_GATE_D1		0x6204
+#define RENCLK_GATE_D1		_MMIO(0x6204)
 # define BLITTER_CLOCK_GATE_DISABLE		(1 << 13) /* 945GM only */
 # define MPEG_CLOCK_GATE_DISABLE		(1 << 12) /* 945GM only */
 # define PC_FE_CLOCK_GATE_DISABLE		(1 << 11)
@@ -2472,35 +2532,35 @@ enum skl_disp_power_wells {
 # define I965_FT_CLOCK_GATE_DISABLE		(1 << 1)
 # define I965_DM_CLOCK_GATE_DISABLE		(1 << 0)
 
-#define RENCLK_GATE_D2		0x6208
+#define RENCLK_GATE_D2		_MMIO(0x6208)
 #define VF_UNIT_CLOCK_GATE_DISABLE		(1 << 9)
 #define GS_UNIT_CLOCK_GATE_DISABLE		(1 << 7)
 #define CL_UNIT_CLOCK_GATE_DISABLE		(1 << 6)
 
-#define VDECCLK_GATE_D		0x620C		/* g4x only */
+#define VDECCLK_GATE_D		_MMIO(0x620C)		/* g4x only */
 #define  VCP_UNIT_CLOCK_GATE_DISABLE		(1 << 4)
 
-#define RAMCLK_GATE_D		0x6210		/* CRL only */
-#define DEUC			0x6214          /* CRL only */
+#define RAMCLK_GATE_D		_MMIO(0x6210)		/* CRL only */
+#define DEUC			_MMIO(0x6214)          /* CRL only */
 
-#define FW_BLC_SELF_VLV		(VLV_DISPLAY_BASE + 0x6500)
+#define FW_BLC_SELF_VLV		_MMIO(VLV_DISPLAY_BASE + 0x6500)
 #define  FW_CSPWRDWNEN		(1<<15)
 
-#define MI_ARB_VLV		(VLV_DISPLAY_BASE + 0x6504)
+#define MI_ARB_VLV		_MMIO(VLV_DISPLAY_BASE + 0x6504)
 
-#define CZCLK_CDCLK_FREQ_RATIO	(VLV_DISPLAY_BASE + 0x6508)
+#define CZCLK_CDCLK_FREQ_RATIO	_MMIO(VLV_DISPLAY_BASE + 0x6508)
 #define   CDCLK_FREQ_SHIFT	4
 #define   CDCLK_FREQ_MASK	(0x1f << CDCLK_FREQ_SHIFT)
 #define   CZCLK_FREQ_MASK	0xf
 
-#define GCI_CONTROL		(VLV_DISPLAY_BASE + 0x650C)
+#define GCI_CONTROL		_MMIO(VLV_DISPLAY_BASE + 0x650C)
 #define   PFI_CREDIT_63		(9 << 28)		/* chv only */
 #define   PFI_CREDIT_31		(8 << 28)		/* chv only */
 #define   PFI_CREDIT(x)		(((x) - 8) << 28)	/* 8-15 */
 #define   PFI_CREDIT_RESEND	(1 << 27)
 #define   VGA_FAST_MODE_DISABLE	(1 << 14)
 
-#define GMBUSFREQ_VLV		(VLV_DISPLAY_BASE + 0x6510)
+#define GMBUSFREQ_VLV		_MMIO(VLV_DISPLAY_BASE + 0x6510)
 
 /*
  * Palette regs
@@ -2508,8 +2568,8 @@ enum skl_disp_power_wells {
 #define PALETTE_A_OFFSET 0xa000
 #define PALETTE_B_OFFSET 0xa800
 #define CHV_PALETTE_C_OFFSET 0xc000
-#define PALETTE(pipe, i) (dev_priv->info.palette_offsets[pipe] + \
-			  dev_priv->info.display_mmio_offset + (i) * 4)
+#define PALETTE(pipe, i) _MMIO(dev_priv->info.palette_offsets[pipe] +	\
+			      dev_priv->info.display_mmio_offset + (i) * 4)
 
 /* MCH MMIO space */
 
@@ -2527,37 +2587,37 @@ enum skl_disp_power_wells {
 
 #define MCHBAR_MIRROR_BASE_SNB	0x140000
 
-#define CTG_STOLEN_RESERVED		(MCHBAR_MIRROR_BASE + 0x34)
-#define ELK_STOLEN_RESERVED		(MCHBAR_MIRROR_BASE + 0x48)
+#define CTG_STOLEN_RESERVED		_MMIO(MCHBAR_MIRROR_BASE + 0x34)
+#define ELK_STOLEN_RESERVED		_MMIO(MCHBAR_MIRROR_BASE + 0x48)
 #define G4X_STOLEN_RESERVED_ADDR1_MASK	(0xFFFF << 16)
 #define G4X_STOLEN_RESERVED_ADDR2_MASK	(0xFFF << 4)
 
 /* Memory controller frequency in MCHBAR for Haswell (possible SNB+) */
-#define DCLK (MCHBAR_MIRROR_BASE_SNB + 0x5e04)
+#define DCLK _MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5e04)
 
 /* 915-945 and GM965 MCH register controlling DRAM channel access */
-#define DCC			0x10200
+#define DCC			_MMIO(MCHBAR_MIRROR_BASE + 0x200)
 #define DCC_ADDRESSING_MODE_SINGLE_CHANNEL		(0 << 0)
 #define DCC_ADDRESSING_MODE_DUAL_CHANNEL_ASYMMETRIC	(1 << 0)
 #define DCC_ADDRESSING_MODE_DUAL_CHANNEL_INTERLEAVED	(2 << 0)
 #define DCC_ADDRESSING_MODE_MASK			(3 << 0)
 #define DCC_CHANNEL_XOR_DISABLE				(1 << 10)
 #define DCC_CHANNEL_XOR_BIT_17				(1 << 9)
-#define DCC2			0x10204
+#define DCC2			_MMIO(MCHBAR_MIRROR_BASE + 0x204)
 #define DCC2_MODIFIED_ENHANCED_DISABLE			(1 << 20)
 
 /* Pineview MCH register contains DDR3 setting */
-#define CSHRDDR3CTL            0x101a8
+#define CSHRDDR3CTL            _MMIO(MCHBAR_MIRROR_BASE + 0x1a8)
 #define CSHRDDR3CTL_DDR3       (1 << 2)
 
 /* 965 MCH register controlling DRAM channel configuration */
-#define C0DRB3			0x10206
-#define C1DRB3			0x10606
+#define C0DRB3			_MMIO(MCHBAR_MIRROR_BASE + 0x206)
+#define C1DRB3			_MMIO(MCHBAR_MIRROR_BASE + 0x606)
 
 /* snb MCH registers for reading the DRAM channel configuration */
-#define MAD_DIMM_C0			(MCHBAR_MIRROR_BASE_SNB + 0x5004)
-#define MAD_DIMM_C1			(MCHBAR_MIRROR_BASE_SNB + 0x5008)
-#define MAD_DIMM_C2			(MCHBAR_MIRROR_BASE_SNB + 0x500C)
+#define MAD_DIMM_C0			_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5004)
+#define MAD_DIMM_C1			_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5008)
+#define MAD_DIMM_C2			_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x500C)
 #define   MAD_DIMM_ECC_MASK		(0x3 << 24)
 #define   MAD_DIMM_ECC_OFF		(0x0 << 24)
 #define   MAD_DIMM_ECC_IO_ON_LOGIC_OFF	(0x1 << 24)
@@ -2577,14 +2637,14 @@ enum skl_disp_power_wells {
 #define   MAD_DIMM_A_SIZE_MASK		(0xff << MAD_DIMM_A_SIZE_SHIFT)
 
 /* snb MCH registers for priority tuning */
-#define MCH_SSKPD			(MCHBAR_MIRROR_BASE_SNB + 0x5d10)
+#define MCH_SSKPD			_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5d10)
 #define   MCH_SSKPD_WM0_MASK		0x3f
 #define   MCH_SSKPD_WM0_VAL		0xc
 
-#define MCH_SECP_NRG_STTS		(MCHBAR_MIRROR_BASE_SNB + 0x592c)
+#define MCH_SECP_NRG_STTS		_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x592c)
 
 /* Clocking configuration register */
-#define CLKCFG			0x10c00
+#define CLKCFG			_MMIO(MCHBAR_MIRROR_BASE + 0xc00)
 #define CLKCFG_FSB_400					(5 << 0)	/* hrawclk 100 */
 #define CLKCFG_FSB_533					(1 << 0)	/* hrawclk 133 */
 #define CLKCFG_FSB_667					(3 << 0)	/* hrawclk 166 */
@@ -2600,26 +2660,26 @@ enum skl_disp_power_wells {
 #define CLKCFG_MEM_800					(3 << 4)
 #define CLKCFG_MEM_MASK					(7 << 4)
 
-#define HPLLVCO                 (MCHBAR_MIRROR_BASE + 0xc38)
-#define HPLLVCO_MOBILE          (MCHBAR_MIRROR_BASE + 0xc0f)
+#define HPLLVCO                 _MMIO(MCHBAR_MIRROR_BASE + 0xc38)
+#define HPLLVCO_MOBILE          _MMIO(MCHBAR_MIRROR_BASE + 0xc0f)
 
-#define TSC1			0x11001
+#define TSC1			_MMIO(0x11001)
 #define   TSE			(1<<0)
-#define TR1			0x11006
-#define TSFS			0x11020
+#define TR1			_MMIO(0x11006)
+#define TSFS			_MMIO(0x11020)
 #define   TSFS_SLOPE_MASK	0x0000ff00
 #define   TSFS_SLOPE_SHIFT	8
 #define   TSFS_INTR_MASK	0x000000ff
 
-#define CRSTANDVID		0x11100
-#define PXVFREQ(i)		(0x11110 + (i) * 4) /* P[0-15]VIDFREQ (0x1114c) (Ironlake) */
+#define CRSTANDVID		_MMIO(0x11100)
+#define PXVFREQ(fstart)		_MMIO(0x11110 + (fstart) * 4)  /* P[0-15]VIDFREQ (0x1114c) (Ironlake) */
 #define   PXVFREQ_PX_MASK	0x7f000000
 #define   PXVFREQ_PX_SHIFT	24
-#define VIDFREQ_BASE		0x11110
-#define VIDFREQ1		0x11110 /* VIDFREQ1-4 (0x1111c) (Cantiga) */
-#define VIDFREQ2		0x11114
-#define VIDFREQ3		0x11118
-#define VIDFREQ4		0x1111c
+#define VIDFREQ_BASE		_MMIO(0x11110)
+#define VIDFREQ1		_MMIO(0x11110) /* VIDFREQ1-4 (0x1111c) (Cantiga) */
+#define VIDFREQ2		_MMIO(0x11114)
+#define VIDFREQ3		_MMIO(0x11118)
+#define VIDFREQ4		_MMIO(0x1111c)
 #define   VIDFREQ_P0_MASK	0x1f000000
 #define   VIDFREQ_P0_SHIFT	24
 #define   VIDFREQ_P0_CSCLK_MASK	0x00f00000
@@ -2631,8 +2691,8 @@ enum skl_disp_power_wells {
 #define   VIDFREQ_P1_CSCLK_MASK	0x000000f0
 #define   VIDFREQ_P1_CSCLK_SHIFT 4
 #define   VIDFREQ_P1_CRCLK_MASK	0x0000000f
-#define INTTOEXT_BASE_ILK	0x11300
-#define INTTOEXT_BASE		0x11120 /* INTTOEXT1-8 (0x1113c) */
+#define INTTOEXT_BASE_ILK	_MMIO(0x11300)
+#define INTTOEXT_BASE		_MMIO(0x11120) /* INTTOEXT1-8 (0x1113c) */
 #define   INTTOEXT_MAP3_SHIFT	24
 #define   INTTOEXT_MAP3_MASK	(0x1f << INTTOEXT_MAP3_SHIFT)
 #define   INTTOEXT_MAP2_SHIFT	16
@@ -2641,7 +2701,7 @@ enum skl_disp_power_wells {
 #define   INTTOEXT_MAP1_MASK	(0x1f << INTTOEXT_MAP1_SHIFT)
 #define   INTTOEXT_MAP0_SHIFT	0
 #define   INTTOEXT_MAP0_MASK	(0x1f << INTTOEXT_MAP0_SHIFT)
-#define MEMSWCTL		0x11170 /* Ironlake only */
+#define MEMSWCTL		_MMIO(0x11170) /* Ironlake only */
 #define   MEMCTL_CMD_MASK	0xe000
 #define   MEMCTL_CMD_SHIFT	13
 #define   MEMCTL_CMD_RCLK_OFF	0
@@ -2656,8 +2716,8 @@ enum skl_disp_power_wells {
 #define   MEMCTL_FREQ_SHIFT	8
 #define   MEMCTL_SFCAVM		(1<<7)
 #define   MEMCTL_TGT_VID_MASK	0x007f
-#define MEMIHYST		0x1117c
-#define MEMINTREN		0x11180 /* 16 bits */
+#define MEMIHYST		_MMIO(0x1117c)
+#define MEMINTREN		_MMIO(0x11180) /* 16 bits */
 #define   MEMINT_RSEXIT_EN	(1<<8)
 #define   MEMINT_CX_SUPR_EN	(1<<7)
 #define   MEMINT_CONT_BUSY_EN	(1<<6)
@@ -2667,7 +2727,7 @@ enum skl_disp_power_wells {
 #define   MEMINT_UP_EVAL_EN	(1<<2)
 #define   MEMINT_DOWN_EVAL_EN	(1<<1)
 #define   MEMINT_SW_CMD_EN	(1<<0)
-#define MEMINTRSTR		0x11182 /* 16 bits */
+#define MEMINTRSTR		_MMIO(0x11182) /* 16 bits */
 #define   MEM_RSEXIT_MASK	0xc000
 #define   MEM_RSEXIT_SHIFT	14
 #define   MEM_CONT_BUSY_MASK	0x3000
@@ -2687,7 +2747,7 @@ enum skl_disp_power_wells {
 #define   MEM_INT_STEER_CMR	1
 #define   MEM_INT_STEER_SMI	2
 #define   MEM_INT_STEER_SCI	3
-#define MEMINTRSTS		0x11184
+#define MEMINTRSTS		_MMIO(0x11184)
 #define   MEMINT_RSEXIT		(1<<7)
 #define   MEMINT_CONT_BUSY	(1<<6)
 #define   MEMINT_AVG_BUSY	(1<<5)
@@ -2696,7 +2756,7 @@ enum skl_disp_power_wells {
 #define   MEMINT_UP_EVAL	(1<<2)
 #define   MEMINT_DOWN_EVAL	(1<<1)
 #define   MEMINT_SW_CMD		(1<<0)
-#define MEMMODECTL		0x11190
+#define MEMMODECTL		_MMIO(0x11190)
 #define   MEMMODE_BOOST_EN	(1<<31)
 #define   MEMMODE_BOOST_FREQ_MASK 0x0f000000 /* jitter for boost, 0-15 */
 #define   MEMMODE_BOOST_FREQ_SHIFT 24
@@ -2713,8 +2773,8 @@ enum skl_disp_power_wells {
 #define   MEMMODE_FMAX_MASK	0x000000f0 /* max jitter, 0-15 */
 #define   MEMMODE_FMAX_SHIFT	4
 #define   MEMMODE_FMIN_MASK	0x0000000f /* min jitter, 0-15 */
-#define RCBMAXAVG		0x1119c
-#define MEMSWCTL2		0x1119e /* Cantiga only */
+#define RCBMAXAVG		_MMIO(0x1119c)
+#define MEMSWCTL2		_MMIO(0x1119e) /* Cantiga only */
 #define   SWMEMCMD_RENDER_OFF	(0 << 13)
 #define   SWMEMCMD_RENDER_ON	(1 << 13)
 #define   SWMEMCMD_SWFREQ	(2 << 13)
@@ -2726,11 +2786,11 @@ enum skl_disp_power_wells {
 #define   SWFREQ_MASK		0x0380 /* P0-7 */
 #define   SWFREQ_SHIFT		7
 #define   TARVID_MASK		0x001f
-#define MEMSTAT_CTG		0x111a0
-#define RCBMINAVG		0x111a0
-#define RCUPEI			0x111b0
-#define RCDNEI			0x111b4
-#define RSTDBYCTL		0x111b8
+#define MEMSTAT_CTG		_MMIO(0x111a0)
+#define RCBMINAVG		_MMIO(0x111a0)
+#define RCUPEI			_MMIO(0x111b0)
+#define RCDNEI			_MMIO(0x111b4)
+#define RSTDBYCTL		_MMIO(0x111b8)
 #define   RS1EN			(1<<31)
 #define   RS2EN			(1<<30)
 #define   RS3EN			(1<<29)
@@ -2774,10 +2834,10 @@ enum skl_disp_power_wells {
 #define   RS_CSTATE_C367_RS2	(3<<4)
 #define   REDSAVES		(1<<3) /* no context save if was idle during rs0 */
 #define   REDRESTORES		(1<<2) /* no restore if was idle during rs0 */
-#define VIDCTL			0x111c0
-#define VIDSTS			0x111c8
-#define VIDSTART		0x111cc /* 8 bits */
-#define MEMSTAT_ILK			0x111f8
+#define VIDCTL			_MMIO(0x111c0)
+#define VIDSTS			_MMIO(0x111c8)
+#define VIDSTART		_MMIO(0x111cc) /* 8 bits */
+#define MEMSTAT_ILK		_MMIO(0x111f8)
 #define   MEMSTAT_VID_MASK	0x7f00
 #define   MEMSTAT_VID_SHIFT	8
 #define   MEMSTAT_PSTATE_MASK	0x00f8
@@ -2788,55 +2848,55 @@ enum skl_disp_power_wells {
 #define   MEMSTAT_SRC_CTL_TRB	1
 #define   MEMSTAT_SRC_CTL_THM	2
 #define   MEMSTAT_SRC_CTL_STDBY 3
-#define RCPREVBSYTUPAVG		0x113b8
-#define RCPREVBSYTDNAVG		0x113bc
-#define PMMISC			0x11214
+#define RCPREVBSYTUPAVG		_MMIO(0x113b8)
+#define RCPREVBSYTDNAVG		_MMIO(0x113bc)
+#define PMMISC			_MMIO(0x11214)
 #define   MCPPCE_EN		(1<<0) /* enable PM_MSG from PCH->MPC */
-#define SDEW			0x1124c
-#define CSIEW0			0x11250
-#define CSIEW1			0x11254
-#define CSIEW2			0x11258
-#define PEW(i)			(0x1125c + (i) * 4) /* 5 registers */
-#define DEW(i)			(0x11270 + (i) * 4) /* 3 registers */
-#define MCHAFE			0x112c0
-#define CSIEC			0x112e0
-#define DMIEC			0x112e4
-#define DDREC			0x112e8
-#define PEG0EC			0x112ec
-#define PEG1EC			0x112f0
-#define GFXEC			0x112f4
-#define RPPREVBSYTUPAVG		0x113b8
-#define RPPREVBSYTDNAVG		0x113bc
-#define ECR			0x11600
+#define SDEW			_MMIO(0x1124c)
+#define CSIEW0			_MMIO(0x11250)
+#define CSIEW1			_MMIO(0x11254)
+#define CSIEW2			_MMIO(0x11258)
+#define PEW(i)			_MMIO(0x1125c + (i) * 4) /* 5 registers */
+#define DEW(i)			_MMIO(0x11270 + (i) * 4) /* 3 registers */
+#define MCHAFE			_MMIO(0x112c0)
+#define CSIEC			_MMIO(0x112e0)
+#define DMIEC			_MMIO(0x112e4)
+#define DDREC			_MMIO(0x112e8)
+#define PEG0EC			_MMIO(0x112ec)
+#define PEG1EC			_MMIO(0x112f0)
+#define GFXEC			_MMIO(0x112f4)
+#define RPPREVBSYTUPAVG		_MMIO(0x113b8)
+#define RPPREVBSYTDNAVG		_MMIO(0x113bc)
+#define ECR			_MMIO(0x11600)
 #define   ECR_GPFE		(1<<31)
 #define   ECR_IMONE		(1<<30)
 #define   ECR_CAP_MASK		0x0000001f /* Event range, 0-31 */
-#define OGW0			0x11608
-#define OGW1			0x1160c
-#define EG0			0x11610
-#define EG1			0x11614
-#define EG2			0x11618
-#define EG3			0x1161c
-#define EG4			0x11620
-#define EG5			0x11624
-#define EG6			0x11628
-#define EG7			0x1162c
-#define PXW(i)			(0x11664 + (i) * 4) /* 4 registers */
-#define PXWL(i)			(0x11680 + (i) * 4) /* 8 registers */
-#define LCFUSE02		0x116c0
+#define OGW0			_MMIO(0x11608)
+#define OGW1			_MMIO(0x1160c)
+#define EG0			_MMIO(0x11610)
+#define EG1			_MMIO(0x11614)
+#define EG2			_MMIO(0x11618)
+#define EG3			_MMIO(0x1161c)
+#define EG4			_MMIO(0x11620)
+#define EG5			_MMIO(0x11624)
+#define EG6			_MMIO(0x11628)
+#define EG7			_MMIO(0x1162c)
+#define PXW(i)			_MMIO(0x11664 + (i) * 4) /* 4 registers */
+#define PXWL(i)			_MMIO(0x11680 + (i) * 8) /* 8 registers */
+#define LCFUSE02		_MMIO(0x116c0)
 #define   LCFUSE_HIV_MASK	0x000000ff
-#define CSIPLL0			0x12c10
-#define DDRMPLL1		0X12c20
-#define PEG_BAND_GAP_DATA	0x14d68
+#define CSIPLL0			_MMIO(0x12c10)
+#define DDRMPLL1		_MMIO(0X12c20)
+#define PEG_BAND_GAP_DATA	_MMIO(0x14d68)
 
-#define GEN6_GT_THREAD_STATUS_REG 0x13805c
+#define GEN6_GT_THREAD_STATUS_REG _MMIO(0x13805c)
 #define GEN6_GT_THREAD_STATUS_CORE_MASK 0x7
 
-#define GEN6_GT_PERF_STATUS	(MCHBAR_MIRROR_BASE_SNB + 0x5948)
-#define BXT_GT_PERF_STATUS      (MCHBAR_MIRROR_BASE_SNB + 0x7070)
-#define GEN6_RP_STATE_LIMITS	(MCHBAR_MIRROR_BASE_SNB + 0x5994)
-#define GEN6_RP_STATE_CAP	(MCHBAR_MIRROR_BASE_SNB + 0x5998)
-#define BXT_RP_STATE_CAP        0x138170
+#define GEN6_GT_PERF_STATUS	_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5948)
+#define BXT_GT_PERF_STATUS      _MMIO(MCHBAR_MIRROR_BASE_SNB + 0x7070)
+#define GEN6_RP_STATE_LIMITS	_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5994)
+#define GEN6_RP_STATE_CAP	_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5998)
+#define BXT_RP_STATE_CAP        _MMIO(0x138170)
 
 #define INTERVAL_1_28_US(us)	(((us) * 100) >> 7)
 #define INTERVAL_1_33_US(us)	(((us) * 3)   >> 2)
@@ -2850,7 +2910,7 @@ enum skl_disp_power_wells {
 /*
  * Logical Context regs
  */
-#define CCID			0x2180
+#define CCID			_MMIO(0x2180)
 #define   CCID_EN		(1<<0)
 /*
  * Notes on SNB/IVB/VLV context size:
@@ -2865,7 +2925,7 @@ enum skl_disp_power_wells {
  * - GT1 size just indicates how much of render context
  *   doesn't need saving on GT1
  */
-#define CXT_SIZE		0x21a0
+#define CXT_SIZE		_MMIO(0x21a0)
 #define GEN6_CXT_POWER_SIZE(cxt_reg)	(((cxt_reg) >> 24) & 0x3f)
 #define GEN6_CXT_RING_SIZE(cxt_reg)	(((cxt_reg) >> 18) & 0x3f)
 #define GEN6_CXT_RENDER_SIZE(cxt_reg)	(((cxt_reg) >> 12) & 0x3f)
@@ -2874,7 +2934,7 @@ enum skl_disp_power_wells {
 #define GEN6_CXT_TOTAL_SIZE(cxt_reg)	(GEN6_CXT_RING_SIZE(cxt_reg) + \
 					GEN6_CXT_EXTENDED_SIZE(cxt_reg) + \
 					GEN6_CXT_PIPELINE_SIZE(cxt_reg))
-#define GEN7_CXT_SIZE		0x21a8
+#define GEN7_CXT_SIZE		_MMIO(0x21a8)
 #define GEN7_CXT_POWER_SIZE(ctx_reg)	(((ctx_reg) >> 25) & 0x7f)
 #define GEN7_CXT_RING_SIZE(ctx_reg)	(((ctx_reg) >> 22) & 0x7)
 #define GEN7_CXT_RENDER_SIZE(ctx_reg)	(((ctx_reg) >> 16) & 0x3f)
@@ -2894,23 +2954,30 @@ enum skl_disp_power_wells {
 /* Same as Haswell, but 72064 bytes now. */
 #define GEN8_CXT_TOTAL_SIZE		(18 * PAGE_SIZE)
 
-#define CHV_CLK_CTL1			0x101100
-#define VLV_CLK_CTL2			0x101104
+#define CHV_CLK_CTL1			_MMIO(0x101100)
+#define VLV_CLK_CTL2			_MMIO(0x101104)
 #define   CLK_CTL2_CZCOUNT_30NS_SHIFT	28
 
 /*
  * Overlay regs
  */
 
-#define OVADD			0x30000
-#define DOVSTA			0x30008
+#define OVADD			_MMIO(0x30000)
+#define DOVSTA			_MMIO(0x30008)
 #define OC_BUF			(0x3<<20)
-#define OGAMC5			0x30010
-#define OGAMC4			0x30014
-#define OGAMC3			0x30018
-#define OGAMC2			0x3001c
-#define OGAMC1			0x30020
-#define OGAMC0			0x30024
+#define OGAMC5			_MMIO(0x30010)
+#define OGAMC4			_MMIO(0x30014)
+#define OGAMC3			_MMIO(0x30018)
+#define OGAMC2			_MMIO(0x3001c)
+#define OGAMC1			_MMIO(0x30020)
+#define OGAMC0			_MMIO(0x30024)
+
+/*
+ * GEN9 clock gating regs
+ */
+#define GEN9_CLKGATE_DIS_0		_MMIO(0x46530)
+#define   PWM2_GATING_DIS		(1 << 14)
+#define   PWM1_GATING_DIS		(1 << 13)
 
 /*
  * Display engine regs
@@ -2970,28 +3037,18 @@ enum skl_disp_power_wells {
 #define _PIPE_CRC_RES_4_B_IVB		0x61070
 #define _PIPE_CRC_RES_5_B_IVB		0x61074
 
-#define PIPE_CRC_CTL(pipe) _TRANSCODER2(pipe, _PIPE_CRC_CTL_A)
-#define PIPE_CRC_RES_1_IVB(pipe)	\
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_1_A_IVB)
-#define PIPE_CRC_RES_2_IVB(pipe)	\
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_2_A_IVB)
-#define PIPE_CRC_RES_3_IVB(pipe)	\
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_3_A_IVB)
-#define PIPE_CRC_RES_4_IVB(pipe)	\
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_4_A_IVB)
-#define PIPE_CRC_RES_5_IVB(pipe)	\
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_5_A_IVB)
-
-#define PIPE_CRC_RES_RED(pipe) \
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_RED_A)
-#define PIPE_CRC_RES_GREEN(pipe) \
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_GREEN_A)
-#define PIPE_CRC_RES_BLUE(pipe) \
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_BLUE_A)
-#define PIPE_CRC_RES_RES1_I915(pipe) \
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_RES1_A_I915)
-#define PIPE_CRC_RES_RES2_G4X(pipe) \
-	_TRANSCODER2(pipe, _PIPE_CRC_RES_RES2_A_G4X)
+#define PIPE_CRC_CTL(pipe)		_MMIO_TRANS2(pipe, _PIPE_CRC_CTL_A)
+#define PIPE_CRC_RES_1_IVB(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_1_A_IVB)
+#define PIPE_CRC_RES_2_IVB(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_2_A_IVB)
+#define PIPE_CRC_RES_3_IVB(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_3_A_IVB)
+#define PIPE_CRC_RES_4_IVB(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_4_A_IVB)
+#define PIPE_CRC_RES_5_IVB(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_5_A_IVB)
+
+#define PIPE_CRC_RES_RED(pipe)		_MMIO_TRANS2(pipe, _PIPE_CRC_RES_RED_A)
+#define PIPE_CRC_RES_GREEN(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_GREEN_A)
+#define PIPE_CRC_RES_BLUE(pipe)		_MMIO_TRANS2(pipe, _PIPE_CRC_RES_BLUE_A)
+#define PIPE_CRC_RES_RES1_I915(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_RES1_A_I915)
+#define PIPE_CRC_RES_RES2_G4X(pipe)	_MMIO_TRANS2(pipe, _PIPE_CRC_RES_RES2_A_G4X)
 
 /* Pipe A timing regs */
 #define _HTOTAL_A	0x60000
@@ -3023,20 +3080,20 @@ enum skl_disp_power_wells {
 #define CHV_TRANSCODER_C_OFFSET 0x63000
 #define TRANSCODER_EDP_OFFSET 0x6f000
 
-#define _TRANSCODER2(pipe, reg) (dev_priv->info.trans_offsets[(pipe)] - \
+#define _MMIO_TRANS2(pipe, reg) _MMIO(dev_priv->info.trans_offsets[(pipe)] - \
 	dev_priv->info.trans_offsets[TRANSCODER_A] + (reg) + \
 	dev_priv->info.display_mmio_offset)
 
-#define HTOTAL(trans) _TRANSCODER2(trans, _HTOTAL_A)
-#define HBLANK(trans) _TRANSCODER2(trans, _HBLANK_A)
-#define HSYNC(trans) _TRANSCODER2(trans, _HSYNC_A)
-#define VTOTAL(trans) _TRANSCODER2(trans, _VTOTAL_A)
-#define VBLANK(trans) _TRANSCODER2(trans, _VBLANK_A)
-#define VSYNC(trans) _TRANSCODER2(trans, _VSYNC_A)
-#define BCLRPAT(trans) _TRANSCODER2(trans, _BCLRPAT_A)
-#define VSYNCSHIFT(trans) _TRANSCODER2(trans, _VSYNCSHIFT_A)
-#define PIPESRC(trans) _TRANSCODER2(trans, _PIPEASRC)
-#define PIPE_MULT(trans) _TRANSCODER2(trans, _PIPE_MULT_A)
+#define HTOTAL(trans)		_MMIO_TRANS2(trans, _HTOTAL_A)
+#define HBLANK(trans)		_MMIO_TRANS2(trans, _HBLANK_A)
+#define HSYNC(trans)		_MMIO_TRANS2(trans, _HSYNC_A)
+#define VTOTAL(trans)		_MMIO_TRANS2(trans, _VTOTAL_A)
+#define VBLANK(trans)		_MMIO_TRANS2(trans, _VBLANK_A)
+#define VSYNC(trans)		_MMIO_TRANS2(trans, _VSYNC_A)
+#define BCLRPAT(trans)		_MMIO_TRANS2(trans, _BCLRPAT_A)
+#define VSYNCSHIFT(trans)	_MMIO_TRANS2(trans, _VSYNCSHIFT_A)
+#define PIPESRC(trans)		_MMIO_TRANS2(trans, _PIPEASRC)
+#define PIPE_MULT(trans)	_MMIO_TRANS2(trans, _PIPE_MULT_A)
 
 /* VLV eDP PSR registers */
 #define _PSRCTLA				(VLV_DISPLAY_BASE + 0x60090)
@@ -3052,14 +3109,14 @@ enum skl_disp_power_wells {
 #define  VLV_EDP_PSR_DBL_FRAME			(1<<10)
 #define  VLV_EDP_PSR_FRAME_COUNT_MASK		(0xff<<16)
 #define  VLV_EDP_PSR_IDLE_FRAME_SHIFT		16
-#define VLV_PSRCTL(pipe) _PIPE(pipe, _PSRCTLA, _PSRCTLB)
+#define VLV_PSRCTL(pipe)	_MMIO_PIPE(pipe, _PSRCTLA, _PSRCTLB)
 
 #define _VSCSDPA			(VLV_DISPLAY_BASE + 0x600a0)
 #define _VSCSDPB			(VLV_DISPLAY_BASE + 0x610a0)
 #define  VLV_EDP_PSR_SDP_FREQ_MASK	(3<<30)
 #define  VLV_EDP_PSR_SDP_FREQ_ONCE	(1<<31)
 #define  VLV_EDP_PSR_SDP_FREQ_EVFRAME	(1<<30)
-#define VLV_VSCSDP(pipe)	_PIPE(pipe, _VSCSDPA, _VSCSDPB)
+#define VLV_VSCSDP(pipe)	_MMIO_PIPE(pipe, _VSCSDPA, _VSCSDPB)
 
 #define _PSRSTATA			(VLV_DISPLAY_BASE + 0x60094)
 #define _PSRSTATB			(VLV_DISPLAY_BASE + 0x61094)
@@ -3072,11 +3129,12 @@ enum skl_disp_power_wells {
 #define  VLV_EDP_PSR_ACTIVE_SF_UPDATE	(4<<0)
 #define  VLV_EDP_PSR_EXIT		(5<<0)
 #define  VLV_EDP_PSR_IN_TRANS		(1<<7)
-#define VLV_PSRSTAT(pipe) _PIPE(pipe, _PSRSTATA, _PSRSTATB)
+#define VLV_PSRSTAT(pipe)	_MMIO_PIPE(pipe, _PSRSTATA, _PSRSTATB)
 
 /* HSW+ eDP PSR registers */
-#define EDP_PSR_BASE(dev)                       (IS_HASWELL(dev) ? 0x64800 : 0x6f800)
-#define EDP_PSR_CTL(dev)			(EDP_PSR_BASE(dev) + 0)
+#define HSW_EDP_PSR_BASE	0x64800
+#define BDW_EDP_PSR_BASE	0x6f800
+#define EDP_PSR_CTL				_MMIO(dev_priv->psr_mmio_base + 0)
 #define   EDP_PSR_ENABLE			(1<<31)
 #define   BDW_PSR_SINGLE_FRAME			(1<<30)
 #define   EDP_PSR_LINK_STANDBY			(1<<27)
@@ -3099,14 +3157,10 @@ enum skl_disp_power_wells {
 #define   EDP_PSR_TP1_TIME_0us			(3<<4)
 #define   EDP_PSR_IDLE_FRAME_SHIFT		0
 
-#define EDP_PSR_AUX_CTL(dev)			(EDP_PSR_BASE(dev) + 0x10)
-#define EDP_PSR_AUX_DATA1(dev)			(EDP_PSR_BASE(dev) + 0x14)
-#define EDP_PSR_AUX_DATA2(dev)			(EDP_PSR_BASE(dev) + 0x18)
-#define EDP_PSR_AUX_DATA3(dev)			(EDP_PSR_BASE(dev) + 0x1c)
-#define EDP_PSR_AUX_DATA4(dev)			(EDP_PSR_BASE(dev) + 0x20)
-#define EDP_PSR_AUX_DATA5(dev)			(EDP_PSR_BASE(dev) + 0x24)
+#define EDP_PSR_AUX_CTL				_MMIO(dev_priv->psr_mmio_base + 0x10)
+#define EDP_PSR_AUX_DATA(i)			_MMIO(dev_priv->psr_mmio_base + 0x14 + (i) * 4) /* 5 registers */
 
-#define EDP_PSR_STATUS_CTL(dev)			(EDP_PSR_BASE(dev) + 0x40)
+#define EDP_PSR_STATUS_CTL			_MMIO(dev_priv->psr_mmio_base + 0x40)
 #define   EDP_PSR_STATUS_STATE_MASK		(7<<29)
 #define   EDP_PSR_STATUS_STATE_IDLE		(0<<29)
 #define   EDP_PSR_STATUS_STATE_SRDONACK		(1<<29)
@@ -3130,15 +3184,15 @@ enum skl_disp_power_wells {
 #define   EDP_PSR_STATUS_SENDING_TP1		(1<<4)
 #define   EDP_PSR_STATUS_IDLE_MASK		0xf
 
-#define EDP_PSR_PERF_CNT(dev)		(EDP_PSR_BASE(dev) + 0x44)
+#define EDP_PSR_PERF_CNT		_MMIO(dev_priv->psr_mmio_base + 0x44)
 #define   EDP_PSR_PERF_CNT_MASK		0xffffff
 
-#define EDP_PSR_DEBUG_CTL(dev)		(EDP_PSR_BASE(dev) + 0x60)
+#define EDP_PSR_DEBUG_CTL		_MMIO(dev_priv->psr_mmio_base + 0x60)
 #define   EDP_PSR_DEBUG_MASK_LPSP	(1<<27)
 #define   EDP_PSR_DEBUG_MASK_MEMUP	(1<<26)
 #define   EDP_PSR_DEBUG_MASK_HPD	(1<<25)
 
-#define EDP_PSR2_CTL			0x6f900
+#define EDP_PSR2_CTL			_MMIO(0x6f900)
 #define   EDP_PSR2_ENABLE		(1<<31)
 #define   EDP_SU_TRACK_ENABLE		(1<<30)
 #define   EDP_MAX_SU_DISABLE_TIME(t)	((t)<<20)
@@ -3153,9 +3207,9 @@ enum skl_disp_power_wells {
 #define   EDP_PSR2_IDLE_MASK		0xf
 
 /* VGA port control */
-#define ADPA			0x61100
-#define PCH_ADPA                0xe1100
-#define VLV_ADPA		(VLV_DISPLAY_BASE + ADPA)
+#define ADPA			_MMIO(0x61100)
+#define PCH_ADPA                _MMIO(0xe1100)
+#define VLV_ADPA		_MMIO(VLV_DISPLAY_BASE + 0x61100)
 
 #define   ADPA_DAC_ENABLE	(1<<31)
 #define   ADPA_DAC_DISABLE	0
@@ -3201,7 +3255,7 @@ enum skl_disp_power_wells {
 
 
 /* Hotplug control (945+ only) */
-#define PORT_HOTPLUG_EN		(dev_priv->info.display_mmio_offset + 0x61110)
+#define PORT_HOTPLUG_EN		_MMIO(dev_priv->info.display_mmio_offset + 0x61110)
 #define   PORTB_HOTPLUG_INT_EN			(1 << 29)
 #define   PORTC_HOTPLUG_INT_EN			(1 << 28)
 #define   PORTD_HOTPLUG_INT_EN			(1 << 27)
@@ -3231,21 +3285,22 @@ enum skl_disp_power_wells {
 #define CRT_HOTPLUG_DETECT_VOLTAGE_325MV	(0 << 2)
 #define CRT_HOTPLUG_DETECT_VOLTAGE_475MV	(1 << 2)
 
-#define PORT_HOTPLUG_STAT	(dev_priv->info.display_mmio_offset + 0x61114)
+#define PORT_HOTPLUG_STAT	_MMIO(dev_priv->info.display_mmio_offset + 0x61114)
 /*
- * HDMI/DP bits are gen4+
+ * HDMI/DP bits are g4x+
  *
  * WARNING: Bspec for hpd status bits on gen4 seems to be completely confused.
  * Please check the detailed lore in the commit message for for experimental
  * evidence.
  */
-#define   PORTD_HOTPLUG_LIVE_STATUS_G4X		(1 << 29)
+/* Bspec says GM45 should match G4X/VLV/CHV, but reality disagrees */
+#define   PORTD_HOTPLUG_LIVE_STATUS_GM45	(1 << 29)
+#define   PORTC_HOTPLUG_LIVE_STATUS_GM45	(1 << 28)
+#define   PORTB_HOTPLUG_LIVE_STATUS_GM45	(1 << 27)
+/* G4X/VLV/CHV DP/HDMI bits again match Bspec */
+#define   PORTD_HOTPLUG_LIVE_STATUS_G4X		(1 << 27)
 #define   PORTC_HOTPLUG_LIVE_STATUS_G4X		(1 << 28)
-#define   PORTB_HOTPLUG_LIVE_STATUS_G4X		(1 << 27)
-/* VLV DP/HDMI bits again match Bspec */
-#define   PORTD_HOTPLUG_LIVE_STATUS_VLV		(1 << 27)
-#define   PORTC_HOTPLUG_LIVE_STATUS_VLV		(1 << 28)
-#define   PORTB_HOTPLUG_LIVE_STATUS_VLV		(1 << 29)
+#define   PORTB_HOTPLUG_LIVE_STATUS_G4X		(1 << 29)
 #define   PORTD_HOTPLUG_INT_STATUS		(3 << 21)
 #define   PORTD_HOTPLUG_INT_LONG_PULSE		(2 << 21)
 #define   PORTD_HOTPLUG_INT_SHORT_PULSE		(1 << 21)
@@ -3296,21 +3351,23 @@ enum skl_disp_power_wells {
 
 /* SDVO and HDMI port control.
  * The same register may be used for SDVO or HDMI */
-#define GEN3_SDVOB	0x61140
-#define GEN3_SDVOC	0x61160
+#define _GEN3_SDVOB	0x61140
+#define _GEN3_SDVOC	0x61160
+#define GEN3_SDVOB	_MMIO(_GEN3_SDVOB)
+#define GEN3_SDVOC	_MMIO(_GEN3_SDVOC)
 #define GEN4_HDMIB	GEN3_SDVOB
 #define GEN4_HDMIC	GEN3_SDVOC
-#define VLV_HDMIB	(VLV_DISPLAY_BASE + GEN4_HDMIB)
-#define VLV_HDMIC	(VLV_DISPLAY_BASE + GEN4_HDMIC)
-#define CHV_HDMID	(VLV_DISPLAY_BASE + 0x6116C)
-#define PCH_SDVOB	0xe1140
+#define VLV_HDMIB	_MMIO(VLV_DISPLAY_BASE + 0x61140)
+#define VLV_HDMIC	_MMIO(VLV_DISPLAY_BASE + 0x61160)
+#define CHV_HDMID	_MMIO(VLV_DISPLAY_BASE + 0x6116C)
+#define PCH_SDVOB	_MMIO(0xe1140)
 #define PCH_HDMIB	PCH_SDVOB
-#define PCH_HDMIC	0xe1150
-#define PCH_HDMID	0xe1160
+#define PCH_HDMIC	_MMIO(0xe1150)
+#define PCH_HDMID	_MMIO(0xe1160)
 
-#define PORT_DFT_I9XX				0x61150
+#define PORT_DFT_I9XX				_MMIO(0x61150)
 #define   DC_BALANCE_RESET			(1 << 25)
-#define PORT_DFT2_G4X		(dev_priv->info.display_mmio_offset + 0x61154)
+#define PORT_DFT2_G4X		_MMIO(dev_priv->info.display_mmio_offset + 0x61154)
 #define   DC_BALANCE_RESET_VLV			(1 << 31)
 #define   PIPE_SCRAMBLE_RESET_MASK		((1 << 14) | (0x3 << 0))
 #define   PIPE_C_SCRAMBLE_RESET			(1 << 14) /* chv */
@@ -3370,9 +3427,12 @@ enum skl_disp_power_wells {
 
 
 /* DVO port control */
-#define DVOA			0x61120
-#define DVOB			0x61140
-#define DVOC			0x61160
+#define _DVOA			0x61120
+#define DVOA			_MMIO(_DVOA)
+#define _DVOB			0x61140
+#define DVOB			_MMIO(_DVOB)
+#define _DVOC			0x61160
+#define DVOC			_MMIO(_DVOC)
 #define   DVO_ENABLE			(1 << 31)
 #define   DVO_PIPE_B_SELECT		(1 << 30)
 #define   DVO_PIPE_STALL_UNUSED		(0 << 28)
@@ -3397,14 +3457,14 @@ enum skl_disp_power_wells {
 #define   DVO_OUTPUT_CSTATE_PIXELS	(1 << 1)	/* SDG only */
 #define   DVO_OUTPUT_SOURCE_SIZE_PIXELS	(1 << 0)	/* SDG only */
 #define   DVO_PRESERVE_MASK		(0x7<<24)
-#define DVOA_SRCDIM		0x61124
-#define DVOB_SRCDIM		0x61144
-#define DVOC_SRCDIM		0x61164
+#define DVOA_SRCDIM		_MMIO(0x61124)
+#define DVOB_SRCDIM		_MMIO(0x61144)
+#define DVOC_SRCDIM		_MMIO(0x61164)
 #define   DVO_SRCDIM_HORIZONTAL_SHIFT	12
 #define   DVO_SRCDIM_VERTICAL_SHIFT	0
 
 /* LVDS port control */
-#define LVDS			0x61180
+#define LVDS			_MMIO(0x61180)
 /*
  * Enables the LVDS port.  This bit must be set before DPLLs are enabled, as
  * the DPLL semantics change when the LVDS is assigned to that pipe.
@@ -3454,13 +3514,13 @@ enum skl_disp_power_wells {
 #define   LVDS_B0B3_POWER_UP		(3 << 2)
 
 /* Video Data Island Packet control */
-#define VIDEO_DIP_DATA		0x61178
+#define VIDEO_DIP_DATA		_MMIO(0x61178)
 /* Read the description of VIDEO_DIP_DATA (before Haswell) or VIDEO_DIP_ECC
  * (Haswell and newer) to see which VIDEO_DIP_DATA byte corresponds to each byte
  * of the infoframe structure specified by CEA-861. */
 #define   VIDEO_DIP_DATA_SIZE	32
 #define   VIDEO_DIP_VSC_DATA_SIZE	36
-#define VIDEO_DIP_CTL		0x61170
+#define VIDEO_DIP_CTL		_MMIO(0x61170)
 /* Pre HSW: */
 #define   VIDEO_DIP_ENABLE		(1 << 31)
 #define   VIDEO_DIP_PORT(port)		((port) << 29)
@@ -3487,7 +3547,7 @@ enum skl_disp_power_wells {
 #define   VIDEO_DIP_ENABLE_SPD_HSW	(1 << 0)
 
 /* Panel power sequencing */
-#define PP_STATUS	0x61200
+#define PP_STATUS	_MMIO(0x61200)
 #define   PP_ON		(1 << 31)
 /*
  * Indicates that all dependencies of the panel are on:
@@ -3513,14 +3573,14 @@ enum skl_disp_power_wells {
 #define   PP_SEQUENCE_STATE_ON_S1_2	(0xa << 0)
 #define   PP_SEQUENCE_STATE_ON_S1_3	(0xb << 0)
 #define   PP_SEQUENCE_STATE_RESET	(0xf << 0)
-#define PP_CONTROL	0x61204
+#define PP_CONTROL	_MMIO(0x61204)
 #define   POWER_TARGET_ON	(1 << 0)
-#define PP_ON_DELAYS	0x61208
-#define PP_OFF_DELAYS	0x6120c
-#define PP_DIVISOR	0x61210
+#define PP_ON_DELAYS	_MMIO(0x61208)
+#define PP_OFF_DELAYS	_MMIO(0x6120c)
+#define PP_DIVISOR	_MMIO(0x61210)
 
 /* Panel fitting */
-#define PFIT_CONTROL	(dev_priv->info.display_mmio_offset + 0x61230)
+#define PFIT_CONTROL	_MMIO(dev_priv->info.display_mmio_offset + 0x61230)
 #define   PFIT_ENABLE		(1 << 31)
 #define   PFIT_PIPE_MASK	(3 << 29)
 #define   PFIT_PIPE_SHIFT	29
@@ -3538,7 +3598,7 @@ enum skl_disp_power_wells {
 #define   PFIT_SCALING_PROGRAMMED (1 << 26)
 #define   PFIT_SCALING_PILLAR	(2 << 26)
 #define   PFIT_SCALING_LETTER	(3 << 26)
-#define PFIT_PGM_RATIOS	(dev_priv->info.display_mmio_offset + 0x61234)
+#define PFIT_PGM_RATIOS _MMIO(dev_priv->info.display_mmio_offset + 0x61234)
 /* Pre-965 */
 #define		PFIT_VERT_SCALE_SHIFT		20
 #define		PFIT_VERT_SCALE_MASK		0xfff00000
@@ -3550,25 +3610,25 @@ enum skl_disp_power_wells {
 #define		PFIT_HORIZ_SCALE_SHIFT_965	0
 #define		PFIT_HORIZ_SCALE_MASK_965	0x00001fff
 
-#define PFIT_AUTO_RATIOS (dev_priv->info.display_mmio_offset + 0x61238)
+#define PFIT_AUTO_RATIOS _MMIO(dev_priv->info.display_mmio_offset + 0x61238)
 
 #define _VLV_BLC_PWM_CTL2_A (dev_priv->info.display_mmio_offset + 0x61250)
 #define _VLV_BLC_PWM_CTL2_B (dev_priv->info.display_mmio_offset + 0x61350)
-#define VLV_BLC_PWM_CTL2(pipe) _PIPE(pipe, _VLV_BLC_PWM_CTL2_A, \
-				     _VLV_BLC_PWM_CTL2_B)
+#define VLV_BLC_PWM_CTL2(pipe) _MMIO_PIPE(pipe, _VLV_BLC_PWM_CTL2_A, \
+					 _VLV_BLC_PWM_CTL2_B)
 
 #define _VLV_BLC_PWM_CTL_A (dev_priv->info.display_mmio_offset + 0x61254)
 #define _VLV_BLC_PWM_CTL_B (dev_priv->info.display_mmio_offset + 0x61354)
-#define VLV_BLC_PWM_CTL(pipe) _PIPE(pipe, _VLV_BLC_PWM_CTL_A, \
-				    _VLV_BLC_PWM_CTL_B)
+#define VLV_BLC_PWM_CTL(pipe) _MMIO_PIPE(pipe, _VLV_BLC_PWM_CTL_A, \
+					_VLV_BLC_PWM_CTL_B)
 
 #define _VLV_BLC_HIST_CTL_A (dev_priv->info.display_mmio_offset + 0x61260)
 #define _VLV_BLC_HIST_CTL_B (dev_priv->info.display_mmio_offset + 0x61360)
-#define VLV_BLC_HIST_CTL(pipe) _PIPE(pipe, _VLV_BLC_HIST_CTL_A, \
-				     _VLV_BLC_HIST_CTL_B)
+#define VLV_BLC_HIST_CTL(pipe) _MMIO_PIPE(pipe, _VLV_BLC_HIST_CTL_A, \
+					 _VLV_BLC_HIST_CTL_B)
 
 /* Backlight control */
-#define BLC_PWM_CTL2	(dev_priv->info.display_mmio_offset + 0x61250) /* 965+ only */
+#define BLC_PWM_CTL2	_MMIO(dev_priv->info.display_mmio_offset + 0x61250) /* 965+ only */
 #define   BLM_PWM_ENABLE		(1 << 31)
 #define   BLM_COMBINATION_MODE		(1 << 30) /* gen4 only */
 #define   BLM_PIPE_SELECT		(1 << 29)
@@ -3591,7 +3651,7 @@ enum skl_disp_power_wells {
 #define   BLM_PHASE_IN_COUNT_MASK	(0xff << 8)
 #define   BLM_PHASE_IN_INCR_SHIFT	(0)
 #define   BLM_PHASE_IN_INCR_MASK	(0xff << 0)
-#define BLC_PWM_CTL	(dev_priv->info.display_mmio_offset + 0x61254)
+#define BLC_PWM_CTL	_MMIO(dev_priv->info.display_mmio_offset + 0x61254)
 /*
  * This is the most significant 15 bits of the number of backlight cycles in a
  * complete cycle of the modulated backlight control.
@@ -3613,25 +3673,25 @@ enum skl_disp_power_wells {
 #define   BACKLIGHT_DUTY_CYCLE_MASK_PNV		(0xfffe)
 #define   BLM_POLARITY_PNV			(1 << 0) /* pnv only */
 
-#define BLC_HIST_CTL	(dev_priv->info.display_mmio_offset + 0x61260)
+#define BLC_HIST_CTL	_MMIO(dev_priv->info.display_mmio_offset + 0x61260)
 #define  BLM_HISTOGRAM_ENABLE			(1 << 31)
 
 /* New registers for PCH-split platforms. Safe where new bits show up, the
  * register layout machtes with gen4 BLC_PWM_CTL[12]. */
-#define BLC_PWM_CPU_CTL2	0x48250
-#define BLC_PWM_CPU_CTL		0x48254
+#define BLC_PWM_CPU_CTL2	_MMIO(0x48250)
+#define BLC_PWM_CPU_CTL		_MMIO(0x48254)
 
-#define HSW_BLC_PWM2_CTL	0x48350
+#define HSW_BLC_PWM2_CTL	_MMIO(0x48350)
 
 /* PCH CTL1 is totally different, all but the below bits are reserved. CTL2 is
  * like the normal CTL from gen4 and earlier. Hooray for confusing naming. */
-#define BLC_PWM_PCH_CTL1	0xc8250
+#define BLC_PWM_PCH_CTL1	_MMIO(0xc8250)
 #define   BLM_PCH_PWM_ENABLE			(1 << 31)
 #define   BLM_PCH_OVERRIDE_ENABLE		(1 << 30)
 #define   BLM_PCH_POLARITY			(1 << 29)
-#define BLC_PWM_PCH_CTL2	0xc8254
+#define BLC_PWM_PCH_CTL2	_MMIO(0xc8254)
 
-#define UTIL_PIN_CTL		0x48400
+#define UTIL_PIN_CTL		_MMIO(0x48400)
 #define   UTIL_PIN_ENABLE	(1 << 31)
 
 #define   UTIL_PIN_PIPE(x)     ((x) << 29)
@@ -3651,18 +3711,18 @@ enum skl_disp_power_wells {
 #define _BXT_BLC_PWM_FREQ2			0xC8354
 #define _BXT_BLC_PWM_DUTY2			0xC8358
 
-#define BXT_BLC_PWM_CTL(controller)    _PIPE(controller, \
+#define BXT_BLC_PWM_CTL(controller)    _MMIO_PIPE(controller,		\
 					_BXT_BLC_PWM_CTL1, _BXT_BLC_PWM_CTL2)
-#define BXT_BLC_PWM_FREQ(controller)   _PIPE(controller, \
+#define BXT_BLC_PWM_FREQ(controller)   _MMIO_PIPE(controller, \
 					_BXT_BLC_PWM_FREQ1, _BXT_BLC_PWM_FREQ2)
-#define BXT_BLC_PWM_DUTY(controller)   _PIPE(controller, \
+#define BXT_BLC_PWM_DUTY(controller)   _MMIO_PIPE(controller, \
 					_BXT_BLC_PWM_DUTY1, _BXT_BLC_PWM_DUTY2)
 
-#define PCH_GTC_CTL		0xe7000
+#define PCH_GTC_CTL		_MMIO(0xe7000)
 #define   PCH_GTC_ENABLE	(1 << 31)
 
 /* TV port control */
-#define TV_CTL			0x68000
+#define TV_CTL			_MMIO(0x68000)
 /* Enables the TV encoder */
 # define TV_ENC_ENABLE			(1 << 31)
 /* Sources the TV encoder input from pipe B instead of A. */
@@ -3729,7 +3789,7 @@ enum skl_disp_power_wells {
 # define TV_TEST_MODE_MONITOR_DETECT	(7 << 0)
 # define TV_TEST_MODE_MASK		(7 << 0)
 
-#define TV_DAC			0x68004
+#define TV_DAC			_MMIO(0x68004)
 # define TV_DAC_SAVE		0x00ffff00
 /*
  * Reports that DAC state change logic has reported change (RO).
@@ -3780,13 +3840,13 @@ enum skl_disp_power_wells {
  * where 2-bit exponents are unsigned n, and 3-bit exponents are signed n with
  * -1 (0x3) being the only legal negative value.
  */
-#define TV_CSC_Y		0x68010
+#define TV_CSC_Y		_MMIO(0x68010)
 # define TV_RY_MASK			0x07ff0000
 # define TV_RY_SHIFT			16
 # define TV_GY_MASK			0x00000fff
 # define TV_GY_SHIFT			0
 
-#define TV_CSC_Y2		0x68014
+#define TV_CSC_Y2		_MMIO(0x68014)
 # define TV_BY_MASK			0x07ff0000
 # define TV_BY_SHIFT			16
 /*
@@ -3797,13 +3857,13 @@ enum skl_disp_power_wells {
 # define TV_AY_MASK			0x000003ff
 # define TV_AY_SHIFT			0
 
-#define TV_CSC_U		0x68018
+#define TV_CSC_U		_MMIO(0x68018)
 # define TV_RU_MASK			0x07ff0000
 # define TV_RU_SHIFT			16
 # define TV_GU_MASK			0x000007ff
 # define TV_GU_SHIFT			0
 
-#define TV_CSC_U2		0x6801c
+#define TV_CSC_U2		_MMIO(0x6801c)
 # define TV_BU_MASK			0x07ff0000
 # define TV_BU_SHIFT			16
 /*
@@ -3814,13 +3874,13 @@ enum skl_disp_power_wells {
 # define TV_AU_MASK			0x000003ff
 # define TV_AU_SHIFT			0
 
-#define TV_CSC_V		0x68020
+#define TV_CSC_V		_MMIO(0x68020)
 # define TV_RV_MASK			0x0fff0000
 # define TV_RV_SHIFT			16
 # define TV_GV_MASK			0x000007ff
 # define TV_GV_SHIFT			0
 
-#define TV_CSC_V2		0x68024
+#define TV_CSC_V2		_MMIO(0x68024)
 # define TV_BV_MASK			0x07ff0000
 # define TV_BV_SHIFT			16
 /*
@@ -3831,7 +3891,7 @@ enum skl_disp_power_wells {
 # define TV_AV_MASK			0x000007ff
 # define TV_AV_SHIFT			0
 
-#define TV_CLR_KNOBS		0x68028
+#define TV_CLR_KNOBS		_MMIO(0x68028)
 /* 2s-complement brightness adjustment */
 # define TV_BRIGHTNESS_MASK		0xff000000
 # define TV_BRIGHTNESS_SHIFT		24
@@ -3845,7 +3905,7 @@ enum skl_disp_power_wells {
 # define TV_HUE_MASK			0x000000ff
 # define TV_HUE_SHIFT			0
 
-#define TV_CLR_LEVEL		0x6802c
+#define TV_CLR_LEVEL		_MMIO(0x6802c)
 /* Controls the DAC level for black */
 # define TV_BLACK_LEVEL_MASK		0x01ff0000
 # define TV_BLACK_LEVEL_SHIFT		16
@@ -3853,7 +3913,7 @@ enum skl_disp_power_wells {
 # define TV_BLANK_LEVEL_MASK		0x000001ff
 # define TV_BLANK_LEVEL_SHIFT		0
 
-#define TV_H_CTL_1		0x68030
+#define TV_H_CTL_1		_MMIO(0x68030)
 /* Number of pixels in the hsync. */
 # define TV_HSYNC_END_MASK		0x1fff0000
 # define TV_HSYNC_END_SHIFT		16
@@ -3861,7 +3921,7 @@ enum skl_disp_power_wells {
 # define TV_HTOTAL_MASK			0x00001fff
 # define TV_HTOTAL_SHIFT		0
 
-#define TV_H_CTL_2		0x68034
+#define TV_H_CTL_2		_MMIO(0x68034)
 /* Enables the colorburst (needed for non-component color) */
 # define TV_BURST_ENA			(1 << 31)
 /* Offset of the colorburst from the start of hsync, in pixels minus one. */
@@ -3871,7 +3931,7 @@ enum skl_disp_power_wells {
 # define TV_HBURST_LEN_SHIFT		0
 # define TV_HBURST_LEN_MASK		0x0001fff
 
-#define TV_H_CTL_3		0x68038
+#define TV_H_CTL_3		_MMIO(0x68038)
 /* End of hblank, measured in pixels minus one from start of hsync */
 # define TV_HBLANK_END_SHIFT		16
 # define TV_HBLANK_END_MASK		0x1fff0000
@@ -3879,7 +3939,7 @@ enum skl_disp_power_wells {
 # define TV_HBLANK_START_SHIFT		0
 # define TV_HBLANK_START_MASK		0x0001fff
 
-#define TV_V_CTL_1		0x6803c
+#define TV_V_CTL_1		_MMIO(0x6803c)
 /* XXX */
 # define TV_NBR_END_SHIFT		16
 # define TV_NBR_END_MASK		0x07ff0000
@@ -3890,7 +3950,7 @@ enum skl_disp_power_wells {
 # define TV_VI_END_F2_SHIFT		0
 # define TV_VI_END_F2_MASK		0x0000003f
 
-#define TV_V_CTL_2		0x68040
+#define TV_V_CTL_2		_MMIO(0x68040)
 /* Length of vsync, in half lines */
 # define TV_VSYNC_LEN_MASK		0x07ff0000
 # define TV_VSYNC_LEN_SHIFT		16
@@ -3906,7 +3966,7 @@ enum skl_disp_power_wells {
 # define TV_VSYNC_START_F2_MASK		0x0000007f
 # define TV_VSYNC_START_F2_SHIFT	0
 
-#define TV_V_CTL_3		0x68044
+#define TV_V_CTL_3		_MMIO(0x68044)
 /* Enables generation of the equalization signal */
 # define TV_EQUAL_ENA			(1 << 31)
 /* Length of vsync, in half lines */
@@ -3924,7 +3984,7 @@ enum skl_disp_power_wells {
 # define TV_VEQ_START_F2_MASK		0x000007f
 # define TV_VEQ_START_F2_SHIFT		0
 
-#define TV_V_CTL_4		0x68048
+#define TV_V_CTL_4		_MMIO(0x68048)
 /*
  * Offset to start of vertical colorburst, measured in one less than the
  * number of lines from vertical start.
@@ -3938,7 +3998,7 @@ enum skl_disp_power_wells {
 # define TV_VBURST_END_F1_MASK		0x000000ff
 # define TV_VBURST_END_F1_SHIFT		0
 
-#define TV_V_CTL_5		0x6804c
+#define TV_V_CTL_5		_MMIO(0x6804c)
 /*
  * Offset to start of vertical colorburst, measured in one less than the
  * number of lines from vertical start.
@@ -3952,7 +4012,7 @@ enum skl_disp_power_wells {
 # define TV_VBURST_END_F2_MASK		0x000000ff
 # define TV_VBURST_END_F2_SHIFT		0
 
-#define TV_V_CTL_6		0x68050
+#define TV_V_CTL_6		_MMIO(0x68050)
 /*
  * Offset to start of vertical colorburst, measured in one less than the
  * number of lines from vertical start.
@@ -3966,7 +4026,7 @@ enum skl_disp_power_wells {
 # define TV_VBURST_END_F3_MASK		0x000000ff
 # define TV_VBURST_END_F3_SHIFT		0
 
-#define TV_V_CTL_7		0x68054
+#define TV_V_CTL_7		_MMIO(0x68054)
 /*
  * Offset to start of vertical colorburst, measured in one less than the
  * number of lines from vertical start.
@@ -3980,7 +4040,7 @@ enum skl_disp_power_wells {
 # define TV_VBURST_END_F4_MASK		0x000000ff
 # define TV_VBURST_END_F4_SHIFT		0
 
-#define TV_SC_CTL_1		0x68060
+#define TV_SC_CTL_1		_MMIO(0x68060)
 /* Turns on the first subcarrier phase generation DDA */
 # define TV_SC_DDA1_EN			(1 << 31)
 /* Turns on the first subcarrier phase generation DDA */
@@ -4002,7 +4062,7 @@ enum skl_disp_power_wells {
 # define TV_SCDDA1_INC_MASK		0x00000fff
 # define TV_SCDDA1_INC_SHIFT		0
 
-#define TV_SC_CTL_2		0x68064
+#define TV_SC_CTL_2		_MMIO(0x68064)
 /* Sets the rollover for the second subcarrier phase generation DDA */
 # define TV_SCDDA2_SIZE_MASK		0x7fff0000
 # define TV_SCDDA2_SIZE_SHIFT		16
@@ -4010,7 +4070,7 @@ enum skl_disp_power_wells {
 # define TV_SCDDA2_INC_MASK		0x00007fff
 # define TV_SCDDA2_INC_SHIFT		0
 
-#define TV_SC_CTL_3		0x68068
+#define TV_SC_CTL_3		_MMIO(0x68068)
 /* Sets the rollover for the third subcarrier phase generation DDA */
 # define TV_SCDDA3_SIZE_MASK		0x7fff0000
 # define TV_SCDDA3_SIZE_SHIFT		16
@@ -4018,7 +4078,7 @@ enum skl_disp_power_wells {
 # define TV_SCDDA3_INC_MASK		0x00007fff
 # define TV_SCDDA3_INC_SHIFT		0
 
-#define TV_WIN_POS		0x68070
+#define TV_WIN_POS		_MMIO(0x68070)
 /* X coordinate of the display from the start of horizontal active */
 # define TV_XPOS_MASK			0x1fff0000
 # define TV_XPOS_SHIFT			16
@@ -4026,7 +4086,7 @@ enum skl_disp_power_wells {
 # define TV_YPOS_MASK			0x00000fff
 # define TV_YPOS_SHIFT			0
 
-#define TV_WIN_SIZE		0x68074
+#define TV_WIN_SIZE		_MMIO(0x68074)
 /* Horizontal size of the display window, measured in pixels*/
 # define TV_XSIZE_MASK			0x1fff0000
 # define TV_XSIZE_SHIFT			16
@@ -4038,7 +4098,7 @@ enum skl_disp_power_wells {
 # define TV_YSIZE_MASK			0x00000fff
 # define TV_YSIZE_SHIFT			0
 
-#define TV_FILTER_CTL_1		0x68080
+#define TV_FILTER_CTL_1		_MMIO(0x68080)
 /*
  * Enables automatic scaling calculation.
  *
@@ -4071,7 +4131,7 @@ enum skl_disp_power_wells {
 # define TV_HSCALE_FRAC_MASK		0x00003fff
 # define TV_HSCALE_FRAC_SHIFT		0
 
-#define TV_FILTER_CTL_2		0x68084
+#define TV_FILTER_CTL_2		_MMIO(0x68084)
 /*
  * Sets the integer part of the 3.15 fixed-point vertical scaling factor.
  *
@@ -4087,7 +4147,7 @@ enum skl_disp_power_wells {
 # define TV_VSCALE_FRAC_MASK		0x00007fff
 # define TV_VSCALE_FRAC_SHIFT		0
 
-#define TV_FILTER_CTL_3		0x68088
+#define TV_FILTER_CTL_3		_MMIO(0x68088)
 /*
  * Sets the integer part of the 3.15 fixed-point vertical scaling factor.
  *
@@ -4107,7 +4167,7 @@ enum skl_disp_power_wells {
 # define TV_VSCALE_IP_FRAC_MASK		0x00007fff
 # define TV_VSCALE_IP_FRAC_SHIFT		0
 
-#define TV_CC_CONTROL		0x68090
+#define TV_CC_CONTROL		_MMIO(0x68090)
 # define TV_CC_ENABLE			(1 << 31)
 /*
  * Specifies which field to send the CC data in.
@@ -4123,7 +4183,7 @@ enum skl_disp_power_wells {
 # define TV_CC_LINE_MASK		0x0000003f
 # define TV_CC_LINE_SHIFT		0
 
-#define TV_CC_DATA		0x68094
+#define TV_CC_DATA		_MMIO(0x68094)
 # define TV_CC_RDY			(1 << 31)
 /* Second word of CC data to be transmitted. */
 # define TV_CC_DATA_2_MASK		0x007f0000
@@ -4132,20 +4192,20 @@ enum skl_disp_power_wells {
 # define TV_CC_DATA_1_MASK		0x0000007f
 # define TV_CC_DATA_1_SHIFT		0
 
-#define TV_H_LUMA(i)		(0x68100 + (i) * 4) /* 60 registers */
-#define TV_H_CHROMA(i)		(0x68200 + (i) * 4) /* 60 registers */
-#define TV_V_LUMA(i)		(0x68300 + (i) * 4) /* 43 registers */
-#define TV_V_CHROMA(i)		(0x68400 + (i) * 4) /* 43 registers */
+#define TV_H_LUMA(i)		_MMIO(0x68100 + (i) * 4) /* 60 registers */
+#define TV_H_CHROMA(i)		_MMIO(0x68200 + (i) * 4) /* 60 registers */
+#define TV_V_LUMA(i)		_MMIO(0x68300 + (i) * 4) /* 43 registers */
+#define TV_V_CHROMA(i)		_MMIO(0x68400 + (i) * 4) /* 43 registers */
 
 /* Display Port */
-#define DP_A				0x64000 /* eDP */
-#define DP_B				0x64100
-#define DP_C				0x64200
-#define DP_D				0x64300
+#define DP_A			_MMIO(0x64000) /* eDP */
+#define DP_B			_MMIO(0x64100)
+#define DP_C			_MMIO(0x64200)
+#define DP_D			_MMIO(0x64300)
 
-#define VLV_DP_B			(VLV_DISPLAY_BASE + DP_B)
-#define VLV_DP_C			(VLV_DISPLAY_BASE + DP_C)
-#define CHV_DP_D			(VLV_DISPLAY_BASE + DP_D)
+#define VLV_DP_B		_MMIO(VLV_DISPLAY_BASE + 0x64100)
+#define VLV_DP_C		_MMIO(VLV_DISPLAY_BASE + 0x64200)
+#define CHV_DP_D		_MMIO(VLV_DISPLAY_BASE + 0x64300)
 
 #define   DP_PORT_EN			(1 << 31)
 #define   DP_PIPEB_SELECT		(1 << 30)
@@ -4199,7 +4259,7 @@ enum skl_disp_power_wells {
 
 /* eDP */
 #define   DP_PLL_FREQ_270MHZ		(0 << 16)
-#define   DP_PLL_FREQ_160MHZ		(1 << 16)
+#define   DP_PLL_FREQ_162MHZ		(1 << 16)
 #define   DP_PLL_FREQ_MASK		(3 << 16)
 
 /* locked once port is enabled */
@@ -4232,33 +4292,36 @@ enum skl_disp_power_wells {
  * is 20 bytes in each direction, hence the 5 fixed
  * data registers
  */
-#define DPA_AUX_CH_CTL			0x64010
-#define DPA_AUX_CH_DATA1		0x64014
-#define DPA_AUX_CH_DATA2		0x64018
-#define DPA_AUX_CH_DATA3		0x6401c
-#define DPA_AUX_CH_DATA4		0x64020
-#define DPA_AUX_CH_DATA5		0x64024
-
-#define DPB_AUX_CH_CTL			0x64110
-#define DPB_AUX_CH_DATA1		0x64114
-#define DPB_AUX_CH_DATA2		0x64118
-#define DPB_AUX_CH_DATA3		0x6411c
-#define DPB_AUX_CH_DATA4		0x64120
-#define DPB_AUX_CH_DATA5		0x64124
-
-#define DPC_AUX_CH_CTL			0x64210
-#define DPC_AUX_CH_DATA1		0x64214
-#define DPC_AUX_CH_DATA2		0x64218
-#define DPC_AUX_CH_DATA3		0x6421c
-#define DPC_AUX_CH_DATA4		0x64220
-#define DPC_AUX_CH_DATA5		0x64224
-
-#define DPD_AUX_CH_CTL			0x64310
-#define DPD_AUX_CH_DATA1		0x64314
-#define DPD_AUX_CH_DATA2		0x64318
-#define DPD_AUX_CH_DATA3		0x6431c
-#define DPD_AUX_CH_DATA4		0x64320
-#define DPD_AUX_CH_DATA5		0x64324
+#define _DPA_AUX_CH_CTL		(dev_priv->info.display_mmio_offset + 0x64010)
+#define _DPA_AUX_CH_DATA1	(dev_priv->info.display_mmio_offset + 0x64014)
+#define _DPA_AUX_CH_DATA2	(dev_priv->info.display_mmio_offset + 0x64018)
+#define _DPA_AUX_CH_DATA3	(dev_priv->info.display_mmio_offset + 0x6401c)
+#define _DPA_AUX_CH_DATA4	(dev_priv->info.display_mmio_offset + 0x64020)
+#define _DPA_AUX_CH_DATA5	(dev_priv->info.display_mmio_offset + 0x64024)
+
+#define _DPB_AUX_CH_CTL		(dev_priv->info.display_mmio_offset + 0x64110)
+#define _DPB_AUX_CH_DATA1	(dev_priv->info.display_mmio_offset + 0x64114)
+#define _DPB_AUX_CH_DATA2	(dev_priv->info.display_mmio_offset + 0x64118)
+#define _DPB_AUX_CH_DATA3	(dev_priv->info.display_mmio_offset + 0x6411c)
+#define _DPB_AUX_CH_DATA4	(dev_priv->info.display_mmio_offset + 0x64120)
+#define _DPB_AUX_CH_DATA5	(dev_priv->info.display_mmio_offset + 0x64124)
+
+#define _DPC_AUX_CH_CTL		(dev_priv->info.display_mmio_offset + 0x64210)
+#define _DPC_AUX_CH_DATA1	(dev_priv->info.display_mmio_offset + 0x64214)
+#define _DPC_AUX_CH_DATA2	(dev_priv->info.display_mmio_offset + 0x64218)
+#define _DPC_AUX_CH_DATA3	(dev_priv->info.display_mmio_offset + 0x6421c)
+#define _DPC_AUX_CH_DATA4	(dev_priv->info.display_mmio_offset + 0x64220)
+#define _DPC_AUX_CH_DATA5	(dev_priv->info.display_mmio_offset + 0x64224)
+
+#define _DPD_AUX_CH_CTL		(dev_priv->info.display_mmio_offset + 0x64310)
+#define _DPD_AUX_CH_DATA1	(dev_priv->info.display_mmio_offset + 0x64314)
+#define _DPD_AUX_CH_DATA2	(dev_priv->info.display_mmio_offset + 0x64318)
+#define _DPD_AUX_CH_DATA3	(dev_priv->info.display_mmio_offset + 0x6431c)
+#define _DPD_AUX_CH_DATA4	(dev_priv->info.display_mmio_offset + 0x64320)
+#define _DPD_AUX_CH_DATA5	(dev_priv->info.display_mmio_offset + 0x64324)
+
+#define DP_AUX_CH_CTL(port)	_MMIO_PORT(port, _DPA_AUX_CH_CTL, _DPB_AUX_CH_CTL)
+#define DP_AUX_CH_DATA(port, i)	_MMIO(_PORT(port, _DPA_AUX_CH_DATA1, _DPB_AUX_CH_DATA1) + (i) * 4) /* 5 registers */
 
 #define   DP_AUX_CH_CTL_SEND_BUSY	    (1 << 31)
 #define   DP_AUX_CH_CTL_DONE		    (1 << 30)
@@ -4335,10 +4398,10 @@ enum skl_disp_power_wells {
 #define _PIPEB_LINK_N_G4X	0x71064
 #define   PIPEA_DP_LINK_N_MASK			(0xffffff)
 
-#define PIPE_DATA_M_G4X(pipe) _PIPE(pipe, _PIPEA_DATA_M_G4X, _PIPEB_DATA_M_G4X)
-#define PIPE_DATA_N_G4X(pipe) _PIPE(pipe, _PIPEA_DATA_N_G4X, _PIPEB_DATA_N_G4X)
-#define PIPE_LINK_M_G4X(pipe) _PIPE(pipe, _PIPEA_LINK_M_G4X, _PIPEB_LINK_M_G4X)
-#define PIPE_LINK_N_G4X(pipe) _PIPE(pipe, _PIPEA_LINK_N_G4X, _PIPEB_LINK_N_G4X)
+#define PIPE_DATA_M_G4X(pipe) _MMIO_PIPE(pipe, _PIPEA_DATA_M_G4X, _PIPEB_DATA_M_G4X)
+#define PIPE_DATA_N_G4X(pipe) _MMIO_PIPE(pipe, _PIPEA_DATA_N_G4X, _PIPEB_DATA_N_G4X)
+#define PIPE_LINK_M_G4X(pipe) _MMIO_PIPE(pipe, _PIPEA_LINK_M_G4X, _PIPEB_LINK_M_G4X)
+#define PIPE_LINK_N_G4X(pipe) _MMIO_PIPE(pipe, _PIPEA_LINK_N_G4X, _PIPEB_LINK_N_G4X)
 
 /* Display & cursor control */
 
@@ -4454,15 +4517,15 @@ enum skl_disp_power_wells {
  */
 #define PIPE_EDP_OFFSET	0x7f000
 
-#define _PIPE2(pipe, reg) (dev_priv->info.pipe_offsets[pipe] - \
+#define _MMIO_PIPE2(pipe, reg) _MMIO(dev_priv->info.pipe_offsets[pipe] - \
 	dev_priv->info.pipe_offsets[PIPE_A] + (reg) + \
 	dev_priv->info.display_mmio_offset)
 
-#define PIPECONF(pipe) _PIPE2(pipe, _PIPEACONF)
-#define PIPEDSL(pipe)  _PIPE2(pipe, _PIPEADSL)
-#define PIPEFRAME(pipe) _PIPE2(pipe, _PIPEAFRAMEHIGH)
-#define PIPEFRAMEPIXEL(pipe)  _PIPE2(pipe, _PIPEAFRAMEPIXEL)
-#define PIPESTAT(pipe) _PIPE2(pipe, _PIPEASTAT)
+#define PIPECONF(pipe)		_MMIO_PIPE2(pipe, _PIPEACONF)
+#define PIPEDSL(pipe)		_MMIO_PIPE2(pipe, _PIPEADSL)
+#define PIPEFRAME(pipe)		_MMIO_PIPE2(pipe, _PIPEAFRAMEHIGH)
+#define PIPEFRAMEPIXEL(pipe)	_MMIO_PIPE2(pipe, _PIPEAFRAMEPIXEL)
+#define PIPESTAT(pipe)		_MMIO_PIPE2(pipe, _PIPEASTAT)
 
 #define _PIPE_MISC_A			0x70030
 #define _PIPE_MISC_B			0x71030
@@ -4474,9 +4537,9 @@ enum skl_disp_power_wells {
 #define   PIPEMISC_DITHER_ENABLE	(1<<4)
 #define   PIPEMISC_DITHER_TYPE_MASK	(3<<2)
 #define   PIPEMISC_DITHER_TYPE_SP	(0<<2)
-#define PIPEMISC(pipe) _PIPE2(pipe, _PIPE_MISC_A)
+#define PIPEMISC(pipe)			_MMIO_PIPE2(pipe, _PIPE_MISC_A)
 
-#define VLV_DPFLIPSTAT				(VLV_DISPLAY_BASE + 0x70028)
+#define VLV_DPFLIPSTAT				_MMIO(VLV_DISPLAY_BASE + 0x70028)
 #define   PIPEB_LINE_COMPARE_INT_EN		(1<<29)
 #define   PIPEB_HLINE_INT_EN			(1<<28)
 #define   PIPEB_VBLANK_INT_EN			(1<<27)
@@ -4497,7 +4560,7 @@ enum skl_disp_power_wells {
 #define   SPRITEE_FLIPDONE_INT_EN		(1<<9)
 #define   PLANEC_FLIPDONE_INT_EN		(1<<8)
 
-#define DPINVGTT				(VLV_DISPLAY_BASE + 0x7002c) /* VLV/CHV only */
+#define DPINVGTT				_MMIO(VLV_DISPLAY_BASE + 0x7002c) /* VLV/CHV only */
 #define   SPRITEF_INVALID_GTT_INT_EN		(1<<27)
 #define   SPRITEE_INVALID_GTT_INT_EN		(1<<26)
 #define   PLANEC_INVALID_GTT_INT_EN		(1<<25)
@@ -4527,7 +4590,7 @@ enum skl_disp_power_wells {
 #define   DPINVGTT_STATUS_MASK			0xff
 #define   DPINVGTT_STATUS_MASK_CHV		0xfff
 
-#define DSPARB			(dev_priv->info.display_mmio_offset + 0x70030)
+#define DSPARB			_MMIO(dev_priv->info.display_mmio_offset + 0x70030)
 #define   DSPARB_CSTART_MASK	(0x7f << 7)
 #define   DSPARB_CSTART_SHIFT	7
 #define   DSPARB_BSTART_MASK	(0x7f)
@@ -4542,7 +4605,7 @@ enum skl_disp_power_wells {
 #define   DSPARB_SPRITEC_MASK_VLV	(0xff << 16)
 #define   DSPARB_SPRITED_SHIFT_VLV	24
 #define   DSPARB_SPRITED_MASK_VLV	(0xff << 24)
-#define DSPARB2			(VLV_DISPLAY_BASE + 0x70060) /* vlv/chv */
+#define DSPARB2				_MMIO(VLV_DISPLAY_BASE + 0x70060) /* vlv/chv */
 #define   DSPARB_SPRITEA_HI_SHIFT_VLV	0
 #define   DSPARB_SPRITEA_HI_MASK_VLV	(0x1 << 0)
 #define   DSPARB_SPRITEB_HI_SHIFT_VLV	4
@@ -4555,14 +4618,14 @@ enum skl_disp_power_wells {
 #define   DSPARB_SPRITEE_HI_MASK_VLV	(0x1 << 16)
 #define   DSPARB_SPRITEF_HI_SHIFT_VLV	20
 #define   DSPARB_SPRITEF_HI_MASK_VLV	(0x1 << 20)
-#define DSPARB3			(VLV_DISPLAY_BASE + 0x7006c) /* chv */
+#define DSPARB3				_MMIO(VLV_DISPLAY_BASE + 0x7006c) /* chv */
 #define   DSPARB_SPRITEE_SHIFT_VLV	0
 #define   DSPARB_SPRITEE_MASK_VLV	(0xff << 0)
 #define   DSPARB_SPRITEF_SHIFT_VLV	8
 #define   DSPARB_SPRITEF_MASK_VLV	(0xff << 8)
 
 /* pnv/gen4/g4x/vlv/chv */
-#define DSPFW1			(dev_priv->info.display_mmio_offset + 0x70034)
+#define DSPFW1		_MMIO(dev_priv->info.display_mmio_offset + 0x70034)
 #define   DSPFW_SR_SHIFT		23
 #define   DSPFW_SR_MASK			(0x1ff<<23)
 #define   DSPFW_CURSORB_SHIFT		16
@@ -4573,7 +4636,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_PLANEA_SHIFT		0
 #define   DSPFW_PLANEA_MASK		(0x7f<<0)
 #define   DSPFW_PLANEA_MASK_VLV		(0xff<<0) /* vlv/chv */
-#define DSPFW2			(dev_priv->info.display_mmio_offset + 0x70038)
+#define DSPFW2		_MMIO(dev_priv->info.display_mmio_offset + 0x70038)
 #define   DSPFW_FBC_SR_EN		(1<<31)	  /* g4x */
 #define   DSPFW_FBC_SR_SHIFT		28
 #define   DSPFW_FBC_SR_MASK		(0x7<<28) /* g4x */
@@ -4589,7 +4652,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_SPRITEA_SHIFT		0
 #define   DSPFW_SPRITEA_MASK		(0x7f<<0) /* g4x */
 #define   DSPFW_SPRITEA_MASK_VLV	(0xff<<0) /* vlv/chv */
-#define DSPFW3			(dev_priv->info.display_mmio_offset + 0x7003c)
+#define DSPFW3		_MMIO(dev_priv->info.display_mmio_offset + 0x7003c)
 #define   DSPFW_HPLL_SR_EN		(1<<31)
 #define   PINEVIEW_SELF_REFRESH_EN	(1<<30)
 #define   DSPFW_CURSOR_SR_SHIFT		24
@@ -4600,14 +4663,14 @@ enum skl_disp_power_wells {
 #define   DSPFW_HPLL_SR_MASK		(0x1ff<<0)
 
 /* vlv/chv */
-#define DSPFW4			(VLV_DISPLAY_BASE + 0x70070)
+#define DSPFW4		_MMIO(VLV_DISPLAY_BASE + 0x70070)
 #define   DSPFW_SPRITEB_WM1_SHIFT	16
 #define   DSPFW_SPRITEB_WM1_MASK	(0xff<<16)
 #define   DSPFW_CURSORA_WM1_SHIFT	8
 #define   DSPFW_CURSORA_WM1_MASK	(0x3f<<8)
 #define   DSPFW_SPRITEA_WM1_SHIFT	0
 #define   DSPFW_SPRITEA_WM1_MASK	(0xff<<0)
-#define DSPFW5			(VLV_DISPLAY_BASE + 0x70074)
+#define DSPFW5		_MMIO(VLV_DISPLAY_BASE + 0x70074)
 #define   DSPFW_PLANEB_WM1_SHIFT	24
 #define   DSPFW_PLANEB_WM1_MASK		(0xff<<24)
 #define   DSPFW_PLANEA_WM1_SHIFT	16
@@ -4616,11 +4679,11 @@ enum skl_disp_power_wells {
 #define   DSPFW_CURSORB_WM1_MASK	(0x3f<<8)
 #define   DSPFW_CURSOR_SR_WM1_SHIFT	0
 #define   DSPFW_CURSOR_SR_WM1_MASK	(0x3f<<0)
-#define DSPFW6			(VLV_DISPLAY_BASE + 0x70078)
+#define DSPFW6		_MMIO(VLV_DISPLAY_BASE + 0x70078)
 #define   DSPFW_SR_WM1_SHIFT		0
 #define   DSPFW_SR_WM1_MASK		(0x1ff<<0)
-#define DSPFW7			(VLV_DISPLAY_BASE + 0x7007c)
-#define DSPFW7_CHV		(VLV_DISPLAY_BASE + 0x700b4) /* wtf #1? */
+#define DSPFW7		_MMIO(VLV_DISPLAY_BASE + 0x7007c)
+#define DSPFW7_CHV	_MMIO(VLV_DISPLAY_BASE + 0x700b4) /* wtf #1? */
 #define   DSPFW_SPRITED_WM1_SHIFT	24
 #define   DSPFW_SPRITED_WM1_MASK	(0xff<<24)
 #define   DSPFW_SPRITED_SHIFT		16
@@ -4629,7 +4692,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_SPRITEC_WM1_MASK	(0xff<<8)
 #define   DSPFW_SPRITEC_SHIFT		0
 #define   DSPFW_SPRITEC_MASK_VLV	(0xff<<0)
-#define DSPFW8_CHV		(VLV_DISPLAY_BASE + 0x700b8)
+#define DSPFW8_CHV	_MMIO(VLV_DISPLAY_BASE + 0x700b8)
 #define   DSPFW_SPRITEF_WM1_SHIFT	24
 #define   DSPFW_SPRITEF_WM1_MASK	(0xff<<24)
 #define   DSPFW_SPRITEF_SHIFT		16
@@ -4638,7 +4701,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_SPRITEE_WM1_MASK	(0xff<<8)
 #define   DSPFW_SPRITEE_SHIFT		0
 #define   DSPFW_SPRITEE_MASK_VLV	(0xff<<0)
-#define DSPFW9_CHV		(VLV_DISPLAY_BASE + 0x7007c) /* wtf #2? */
+#define DSPFW9_CHV	_MMIO(VLV_DISPLAY_BASE + 0x7007c) /* wtf #2? */
 #define   DSPFW_PLANEC_WM1_SHIFT	24
 #define   DSPFW_PLANEC_WM1_MASK		(0xff<<24)
 #define   DSPFW_PLANEC_SHIFT		16
@@ -4649,7 +4712,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_CURSORC_MASK		(0x3f<<0)
 
 /* vlv/chv high order bits */
-#define DSPHOWM			(VLV_DISPLAY_BASE + 0x70064)
+#define DSPHOWM		_MMIO(VLV_DISPLAY_BASE + 0x70064)
 #define   DSPFW_SR_HI_SHIFT		24
 #define   DSPFW_SR_HI_MASK		(3<<24) /* 2 bits for chv, 1 for vlv */
 #define   DSPFW_SPRITEF_HI_SHIFT	23
@@ -4670,7 +4733,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_SPRITEA_HI_MASK		(1<<4)
 #define   DSPFW_PLANEA_HI_SHIFT		0
 #define   DSPFW_PLANEA_HI_MASK		(1<<0)
-#define DSPHOWM1		(VLV_DISPLAY_BASE + 0x70068)
+#define DSPHOWM1	_MMIO(VLV_DISPLAY_BASE + 0x70068)
 #define   DSPFW_SR_WM1_HI_SHIFT		24
 #define   DSPFW_SR_WM1_HI_MASK		(3<<24) /* 2 bits for chv, 1 for vlv */
 #define   DSPFW_SPRITEF_WM1_HI_SHIFT	23
@@ -4693,7 +4756,7 @@ enum skl_disp_power_wells {
 #define   DSPFW_PLANEA_WM1_HI_MASK	(1<<0)
 
 /* drain latency register values*/
-#define VLV_DDL(pipe)			(VLV_DISPLAY_BASE + 0x70050 + 4 * (pipe))
+#define VLV_DDL(pipe)			_MMIO(VLV_DISPLAY_BASE + 0x70050 + 4 * (pipe))
 #define DDL_CURSOR_SHIFT		24
 #define DDL_SPRITE_SHIFT(sprite)	(8+8*(sprite))
 #define DDL_PLANE_SHIFT			0
@@ -4701,7 +4764,7 @@ enum skl_disp_power_wells {
 #define DDL_PRECISION_LOW		(0<<7)
 #define DRAIN_LATENCY_MASK		0x7f
 
-#define CBR1_VLV			(VLV_DISPLAY_BASE + 0x70400)
+#define CBR1_VLV			_MMIO(VLV_DISPLAY_BASE + 0x70400)
 #define  CBR_PND_DEADLINE_DISABLE	(1<<31)
 #define  CBR_PWM_CLOCK_MUX_SELECT	(1<<30)
 
@@ -4739,51 +4802,51 @@ enum skl_disp_power_wells {
 #define I965_CURSOR_DFT_WM	8
 
 /* Watermark register definitions for SKL */
-#define CUR_WM_A_0		0x70140
-#define CUR_WM_B_0		0x71140
-#define PLANE_WM_1_A_0		0x70240
-#define PLANE_WM_1_B_0		0x71240
-#define PLANE_WM_2_A_0		0x70340
-#define PLANE_WM_2_B_0		0x71340
-#define PLANE_WM_TRANS_1_A_0	0x70268
-#define PLANE_WM_TRANS_1_B_0	0x71268
-#define PLANE_WM_TRANS_2_A_0	0x70368
-#define PLANE_WM_TRANS_2_B_0	0x71368
-#define CUR_WM_TRANS_A_0	0x70168
-#define CUR_WM_TRANS_B_0	0x71168
+#define _CUR_WM_A_0		0x70140
+#define _CUR_WM_B_0		0x71140
+#define _PLANE_WM_1_A_0		0x70240
+#define _PLANE_WM_1_B_0		0x71240
+#define _PLANE_WM_2_A_0		0x70340
+#define _PLANE_WM_2_B_0		0x71340
+#define _PLANE_WM_TRANS_1_A_0	0x70268
+#define _PLANE_WM_TRANS_1_B_0	0x71268
+#define _PLANE_WM_TRANS_2_A_0	0x70368
+#define _PLANE_WM_TRANS_2_B_0	0x71368
+#define _CUR_WM_TRANS_A_0	0x70168
+#define _CUR_WM_TRANS_B_0	0x71168
 #define   PLANE_WM_EN		(1 << 31)
 #define   PLANE_WM_LINES_SHIFT	14
 #define   PLANE_WM_LINES_MASK	0x1f
 #define   PLANE_WM_BLOCKS_MASK	0x3ff
 
-#define CUR_WM_0(pipe) _PIPE(pipe, CUR_WM_A_0, CUR_WM_B_0)
-#define CUR_WM(pipe, level) (CUR_WM_0(pipe) + ((4) * (level)))
-#define CUR_WM_TRANS(pipe) _PIPE(pipe, CUR_WM_TRANS_A_0, CUR_WM_TRANS_B_0)
+#define _CUR_WM_0(pipe) _PIPE(pipe, _CUR_WM_A_0, _CUR_WM_B_0)
+#define CUR_WM(pipe, level) _MMIO(_CUR_WM_0(pipe) + ((4) * (level)))
+#define CUR_WM_TRANS(pipe) _MMIO_PIPE(pipe, _CUR_WM_TRANS_A_0, _CUR_WM_TRANS_B_0)
 
-#define _PLANE_WM_1(pipe) _PIPE(pipe, PLANE_WM_1_A_0, PLANE_WM_1_B_0)
-#define _PLANE_WM_2(pipe) _PIPE(pipe, PLANE_WM_2_A_0, PLANE_WM_2_B_0)
+#define _PLANE_WM_1(pipe) _PIPE(pipe, _PLANE_WM_1_A_0, _PLANE_WM_1_B_0)
+#define _PLANE_WM_2(pipe) _PIPE(pipe, _PLANE_WM_2_A_0, _PLANE_WM_2_B_0)
 #define _PLANE_WM_BASE(pipe, plane)	\
 			_PLANE(plane, _PLANE_WM_1(pipe), _PLANE_WM_2(pipe))
 #define PLANE_WM(pipe, plane, level)	\
-			(_PLANE_WM_BASE(pipe, plane) + ((4) * (level)))
+			_MMIO(_PLANE_WM_BASE(pipe, plane) + ((4) * (level)))
 #define _PLANE_WM_TRANS_1(pipe)	\
-			_PIPE(pipe, PLANE_WM_TRANS_1_A_0, PLANE_WM_TRANS_1_B_0)
+			_PIPE(pipe, _PLANE_WM_TRANS_1_A_0, _PLANE_WM_TRANS_1_B_0)
 #define _PLANE_WM_TRANS_2(pipe)	\
-			_PIPE(pipe, PLANE_WM_TRANS_2_A_0, PLANE_WM_TRANS_2_B_0)
+			_PIPE(pipe, _PLANE_WM_TRANS_2_A_0, _PLANE_WM_TRANS_2_B_0)
 #define PLANE_WM_TRANS(pipe, plane)	\
-		_PLANE(plane, _PLANE_WM_TRANS_1(pipe), _PLANE_WM_TRANS_2(pipe))
+	_MMIO(_PLANE(plane, _PLANE_WM_TRANS_1(pipe), _PLANE_WM_TRANS_2(pipe)))
 
 /* define the Watermark register on Ironlake */
-#define WM0_PIPEA_ILK		0x45100
+#define WM0_PIPEA_ILK		_MMIO(0x45100)
 #define  WM0_PIPE_PLANE_MASK	(0xffff<<16)
 #define  WM0_PIPE_PLANE_SHIFT	16
 #define  WM0_PIPE_SPRITE_MASK	(0xff<<8)
 #define  WM0_PIPE_SPRITE_SHIFT	8
 #define  WM0_PIPE_CURSOR_MASK	(0xff)
 
-#define WM0_PIPEB_ILK		0x45104
-#define WM0_PIPEC_IVB		0x45200
-#define WM1_LP_ILK		0x45108
+#define WM0_PIPEB_ILK		_MMIO(0x45104)
+#define WM0_PIPEC_IVB		_MMIO(0x45200)
+#define WM1_LP_ILK		_MMIO(0x45108)
 #define  WM1_LP_SR_EN		(1<<31)
 #define  WM1_LP_LATENCY_SHIFT	24
 #define  WM1_LP_LATENCY_MASK	(0x7f<<24)
@@ -4793,13 +4856,13 @@ enum skl_disp_power_wells {
 #define  WM1_LP_SR_MASK		(0x7ff<<8)
 #define  WM1_LP_SR_SHIFT	8
 #define  WM1_LP_CURSOR_MASK	(0xff)
-#define WM2_LP_ILK		0x4510c
+#define WM2_LP_ILK		_MMIO(0x4510c)
 #define  WM2_LP_EN		(1<<31)
-#define WM3_LP_ILK		0x45110
+#define WM3_LP_ILK		_MMIO(0x45110)
 #define  WM3_LP_EN		(1<<31)
-#define WM1S_LP_ILK		0x45120
-#define WM2S_LP_IVB		0x45124
-#define WM3S_LP_IVB		0x45128
+#define WM1S_LP_ILK		_MMIO(0x45120)
+#define WM2S_LP_IVB		_MMIO(0x45124)
+#define WM3S_LP_IVB		_MMIO(0x45128)
 #define  WM1S_LP_EN		(1<<31)
 
 #define HSW_WM_LP_VAL(lat, fbc, pri, cur) \
@@ -4807,7 +4870,7 @@ enum skl_disp_power_wells {
 	 ((fbc) << WM1_LP_FBC_SHIFT) | ((pri) << WM1_LP_SR_SHIFT) | (cur))
 
 /* Memory latency timer register */
-#define MLTR_ILK		0x11222
+#define MLTR_ILK		_MMIO(0x11222)
 #define  MLTR_WM1_SHIFT		0
 #define  MLTR_WM2_SHIFT		8
 /* the unit of memory self-refresh latency time is 0.5us */
@@ -4815,7 +4878,7 @@ enum skl_disp_power_wells {
 
 
 /* the address where we get all kinds of latency value */
-#define SSKPD			0x5d10
+#define SSKPD			_MMIO(0x5d10)
 #define SSKPD_WM_MASK		0x3f
 #define SSKPD_WM0_SHIFT		0
 #define SSKPD_WM1_SHIFT		8
@@ -4848,8 +4911,8 @@ enum skl_disp_power_wells {
 /* GM45+ just has to be different */
 #define _PIPEA_FRMCOUNT_G4X	0x70040
 #define _PIPEA_FLIPCOUNT_G4X	0x70044
-#define PIPE_FRMCOUNT_G4X(pipe) _PIPE2(pipe, _PIPEA_FRMCOUNT_G4X)
-#define PIPE_FLIPCOUNT_G4X(pipe) _PIPE2(pipe, _PIPEA_FLIPCOUNT_G4X)
+#define PIPE_FRMCOUNT_G4X(pipe) _MMIO_PIPE2(pipe, _PIPEA_FRMCOUNT_G4X)
+#define PIPE_FLIPCOUNT_G4X(pipe) _MMIO_PIPE2(pipe, _PIPEA_FLIPCOUNT_G4X)
 
 /* Cursor A & B regs */
 #define _CURACNTR		0x70080
@@ -4887,7 +4950,7 @@ enum skl_disp_power_wells {
 #define   CURSOR_POS_SIGN       0x8000
 #define   CURSOR_X_SHIFT        0
 #define   CURSOR_Y_SHIFT        16
-#define CURSIZE			0x700a0
+#define CURSIZE			_MMIO(0x700a0)
 #define _CURBCNTR		0x700c0
 #define _CURBBASE		0x700c4
 #define _CURBPOS		0x700c8
@@ -4896,7 +4959,7 @@ enum skl_disp_power_wells {
 #define _CURBBASE_IVB		0x71084
 #define _CURBPOS_IVB		0x71088
 
-#define _CURSOR2(pipe, reg) (dev_priv->info.cursor_offsets[(pipe)] - \
+#define _CURSOR2(pipe, reg) _MMIO(dev_priv->info.cursor_offsets[(pipe)] - \
 	dev_priv->info.cursor_offsets[PIPE_A] + (reg) + \
 	dev_priv->info.display_mmio_offset)
 
@@ -4957,16 +5020,16 @@ enum skl_disp_power_wells {
 #define _DSPAOFFSET				0x701A4 /* HSW */
 #define _DSPASURFLIVE				0x701AC
 
-#define DSPCNTR(plane) _PIPE2(plane, _DSPACNTR)
-#define DSPADDR(plane) _PIPE2(plane, _DSPAADDR)
-#define DSPSTRIDE(plane) _PIPE2(plane, _DSPASTRIDE)
-#define DSPPOS(plane) _PIPE2(plane, _DSPAPOS)
-#define DSPSIZE(plane) _PIPE2(plane, _DSPASIZE)
-#define DSPSURF(plane) _PIPE2(plane, _DSPASURF)
-#define DSPTILEOFF(plane) _PIPE2(plane, _DSPATILEOFF)
-#define DSPLINOFF(plane) DSPADDR(plane)
-#define DSPOFFSET(plane) _PIPE2(plane, _DSPAOFFSET)
-#define DSPSURFLIVE(plane) _PIPE2(plane, _DSPASURFLIVE)
+#define DSPCNTR(plane)		_MMIO_PIPE2(plane, _DSPACNTR)
+#define DSPADDR(plane)		_MMIO_PIPE2(plane, _DSPAADDR)
+#define DSPSTRIDE(plane)	_MMIO_PIPE2(plane, _DSPASTRIDE)
+#define DSPPOS(plane)		_MMIO_PIPE2(plane, _DSPAPOS)
+#define DSPSIZE(plane)		_MMIO_PIPE2(plane, _DSPASIZE)
+#define DSPSURF(plane)		_MMIO_PIPE2(plane, _DSPASURF)
+#define DSPTILEOFF(plane)	_MMIO_PIPE2(plane, _DSPATILEOFF)
+#define DSPLINOFF(plane)	DSPADDR(plane)
+#define DSPOFFSET(plane)	_MMIO_PIPE2(plane, _DSPAOFFSET)
+#define DSPSURFLIVE(plane)	_MMIO_PIPE2(plane, _DSPASURFLIVE)
 
 /* CHV pipe B blender and primary plane */
 #define _CHV_BLEND_A		0x60a00
@@ -4980,11 +5043,11 @@ enum skl_disp_power_wells {
 #define _PRIMCNSTALPHA_A	0x60a10
 #define   PRIM_CONST_ALPHA_ENABLE	(1<<31)
 
-#define CHV_BLEND(pipe) _TRANSCODER2(pipe, _CHV_BLEND_A)
-#define CHV_CANVAS(pipe) _TRANSCODER2(pipe, _CHV_CANVAS_A)
-#define PRIMPOS(plane) _TRANSCODER2(plane, _PRIMPOS_A)
-#define PRIMSIZE(plane) _TRANSCODER2(plane, _PRIMSIZE_A)
-#define PRIMCNSTALPHA(plane) _TRANSCODER2(plane, _PRIMCNSTALPHA_A)
+#define CHV_BLEND(pipe)		_MMIO_TRANS2(pipe, _CHV_BLEND_A)
+#define CHV_CANVAS(pipe)	_MMIO_TRANS2(pipe, _CHV_CANVAS_A)
+#define PRIMPOS(plane)		_MMIO_TRANS2(plane, _PRIMPOS_A)
+#define PRIMSIZE(plane)		_MMIO_TRANS2(plane, _PRIMSIZE_A)
+#define PRIMCNSTALPHA(plane)	_MMIO_TRANS2(plane, _PRIMCNSTALPHA_A)
 
 /* Display/Sprite base address macros */
 #define DISP_BASEADDR_MASK	(0xfffff000)
@@ -5002,9 +5065,10 @@ enum skl_disp_power_wells {
  * [10:1f] all
  * [30:32] all
  */
-#define SWF0(i)	(dev_priv->info.display_mmio_offset + 0x70410 + (i) * 4)
-#define SWF1(i)	(dev_priv->info.display_mmio_offset + 0x71410 + (i) * 4)
-#define SWF3(i)	(dev_priv->info.display_mmio_offset + 0x72414 + (i) * 4)
+#define SWF0(i)	_MMIO(dev_priv->info.display_mmio_offset + 0x70410 + (i) * 4)
+#define SWF1(i)	_MMIO(dev_priv->info.display_mmio_offset + 0x71410 + (i) * 4)
+#define SWF3(i)	_MMIO(dev_priv->info.display_mmio_offset + 0x72414 + (i) * 4)
+#define SWF_ILK(i)	_MMIO(0x4F000 + (i) * 4)
 
 /* Pipe B */
 #define _PIPEBDSL		(dev_priv->info.display_mmio_offset + 0x71000)
@@ -5086,18 +5150,18 @@ enum skl_disp_power_wells {
 #define _DVSBSCALE		0x73204
 #define _DVSBGAMC		0x73300
 
-#define DVSCNTR(pipe) _PIPE(pipe, _DVSACNTR, _DVSBCNTR)
-#define DVSLINOFF(pipe) _PIPE(pipe, _DVSALINOFF, _DVSBLINOFF)
-#define DVSSTRIDE(pipe) _PIPE(pipe, _DVSASTRIDE, _DVSBSTRIDE)
-#define DVSPOS(pipe) _PIPE(pipe, _DVSAPOS, _DVSBPOS)
-#define DVSSURF(pipe) _PIPE(pipe, _DVSASURF, _DVSBSURF)
-#define DVSKEYMAX(pipe) _PIPE(pipe, _DVSAKEYMAXVAL, _DVSBKEYMAXVAL)
-#define DVSSIZE(pipe) _PIPE(pipe, _DVSASIZE, _DVSBSIZE)
-#define DVSSCALE(pipe) _PIPE(pipe, _DVSASCALE, _DVSBSCALE)
-#define DVSTILEOFF(pipe) _PIPE(pipe, _DVSATILEOFF, _DVSBTILEOFF)
-#define DVSKEYVAL(pipe) _PIPE(pipe, _DVSAKEYVAL, _DVSBKEYVAL)
-#define DVSKEYMSK(pipe) _PIPE(pipe, _DVSAKEYMSK, _DVSBKEYMSK)
-#define DVSSURFLIVE(pipe) _PIPE(pipe, _DVSASURFLIVE, _DVSBSURFLIVE)
+#define DVSCNTR(pipe) _MMIO_PIPE(pipe, _DVSACNTR, _DVSBCNTR)
+#define DVSLINOFF(pipe) _MMIO_PIPE(pipe, _DVSALINOFF, _DVSBLINOFF)
+#define DVSSTRIDE(pipe) _MMIO_PIPE(pipe, _DVSASTRIDE, _DVSBSTRIDE)
+#define DVSPOS(pipe) _MMIO_PIPE(pipe, _DVSAPOS, _DVSBPOS)
+#define DVSSURF(pipe) _MMIO_PIPE(pipe, _DVSASURF, _DVSBSURF)
+#define DVSKEYMAX(pipe) _MMIO_PIPE(pipe, _DVSAKEYMAXVAL, _DVSBKEYMAXVAL)
+#define DVSSIZE(pipe) _MMIO_PIPE(pipe, _DVSASIZE, _DVSBSIZE)
+#define DVSSCALE(pipe) _MMIO_PIPE(pipe, _DVSASCALE, _DVSBSCALE)
+#define DVSTILEOFF(pipe) _MMIO_PIPE(pipe, _DVSATILEOFF, _DVSBTILEOFF)
+#define DVSKEYVAL(pipe) _MMIO_PIPE(pipe, _DVSAKEYVAL, _DVSBKEYVAL)
+#define DVSKEYMSK(pipe) _MMIO_PIPE(pipe, _DVSAKEYMSK, _DVSBKEYMSK)
+#define DVSSURFLIVE(pipe) _MMIO_PIPE(pipe, _DVSASURFLIVE, _DVSBSURFLIVE)
 
 #define _SPRA_CTL		0x70280
 #define   SPRITE_ENABLE			(1<<31)
@@ -5160,20 +5224,20 @@ enum skl_disp_power_wells {
 #define _SPRB_SCALE		0x71304
 #define _SPRB_GAMC		0x71400
 
-#define SPRCTL(pipe) _PIPE(pipe, _SPRA_CTL, _SPRB_CTL)
-#define SPRLINOFF(pipe) _PIPE(pipe, _SPRA_LINOFF, _SPRB_LINOFF)
-#define SPRSTRIDE(pipe) _PIPE(pipe, _SPRA_STRIDE, _SPRB_STRIDE)
-#define SPRPOS(pipe) _PIPE(pipe, _SPRA_POS, _SPRB_POS)
-#define SPRSIZE(pipe) _PIPE(pipe, _SPRA_SIZE, _SPRB_SIZE)
-#define SPRKEYVAL(pipe) _PIPE(pipe, _SPRA_KEYVAL, _SPRB_KEYVAL)
-#define SPRKEYMSK(pipe) _PIPE(pipe, _SPRA_KEYMSK, _SPRB_KEYMSK)
-#define SPRSURF(pipe) _PIPE(pipe, _SPRA_SURF, _SPRB_SURF)
-#define SPRKEYMAX(pipe) _PIPE(pipe, _SPRA_KEYMAX, _SPRB_KEYMAX)
-#define SPRTILEOFF(pipe) _PIPE(pipe, _SPRA_TILEOFF, _SPRB_TILEOFF)
-#define SPROFFSET(pipe) _PIPE(pipe, _SPRA_OFFSET, _SPRB_OFFSET)
-#define SPRSCALE(pipe) _PIPE(pipe, _SPRA_SCALE, _SPRB_SCALE)
-#define SPRGAMC(pipe) _PIPE(pipe, _SPRA_GAMC, _SPRB_GAMC)
-#define SPRSURFLIVE(pipe) _PIPE(pipe, _SPRA_SURFLIVE, _SPRB_SURFLIVE)
+#define SPRCTL(pipe) _MMIO_PIPE(pipe, _SPRA_CTL, _SPRB_CTL)
+#define SPRLINOFF(pipe) _MMIO_PIPE(pipe, _SPRA_LINOFF, _SPRB_LINOFF)
+#define SPRSTRIDE(pipe) _MMIO_PIPE(pipe, _SPRA_STRIDE, _SPRB_STRIDE)
+#define SPRPOS(pipe) _MMIO_PIPE(pipe, _SPRA_POS, _SPRB_POS)
+#define SPRSIZE(pipe) _MMIO_PIPE(pipe, _SPRA_SIZE, _SPRB_SIZE)
+#define SPRKEYVAL(pipe) _MMIO_PIPE(pipe, _SPRA_KEYVAL, _SPRB_KEYVAL)
+#define SPRKEYMSK(pipe) _MMIO_PIPE(pipe, _SPRA_KEYMSK, _SPRB_KEYMSK)
+#define SPRSURF(pipe) _MMIO_PIPE(pipe, _SPRA_SURF, _SPRB_SURF)
+#define SPRKEYMAX(pipe) _MMIO_PIPE(pipe, _SPRA_KEYMAX, _SPRB_KEYMAX)
+#define SPRTILEOFF(pipe) _MMIO_PIPE(pipe, _SPRA_TILEOFF, _SPRB_TILEOFF)
+#define SPROFFSET(pipe) _MMIO_PIPE(pipe, _SPRA_OFFSET, _SPRB_OFFSET)
+#define SPRSCALE(pipe) _MMIO_PIPE(pipe, _SPRA_SCALE, _SPRB_SCALE)
+#define SPRGAMC(pipe) _MMIO_PIPE(pipe, _SPRA_GAMC, _SPRB_GAMC)
+#define SPRSURFLIVE(pipe) _MMIO_PIPE(pipe, _SPRA_SURFLIVE, _SPRB_SURFLIVE)
 
 #define _SPACNTR		(VLV_DISPLAY_BASE + 0x72180)
 #define   SP_ENABLE			(1<<31)
@@ -5223,18 +5287,18 @@ enum skl_disp_power_wells {
 #define _SPBCONSTALPHA		(VLV_DISPLAY_BASE + 0x722a8)
 #define _SPBGAMC		(VLV_DISPLAY_BASE + 0x722f4)
 
-#define SPCNTR(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPACNTR, _SPBCNTR)
-#define SPLINOFF(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPALINOFF, _SPBLINOFF)
-#define SPSTRIDE(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPASTRIDE, _SPBSTRIDE)
-#define SPPOS(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPAPOS, _SPBPOS)
-#define SPSIZE(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPASIZE, _SPBSIZE)
-#define SPKEYMINVAL(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPAKEYMINVAL, _SPBKEYMINVAL)
-#define SPKEYMSK(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPAKEYMSK, _SPBKEYMSK)
-#define SPSURF(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPASURF, _SPBSURF)
-#define SPKEYMAXVAL(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPAKEYMAXVAL, _SPBKEYMAXVAL)
-#define SPTILEOFF(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPATILEOFF, _SPBTILEOFF)
-#define SPCONSTALPHA(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPACONSTALPHA, _SPBCONSTALPHA)
-#define SPGAMC(pipe, plane) _PIPE((pipe) * 2 + (plane), _SPAGAMC, _SPBGAMC)
+#define SPCNTR(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPACNTR, _SPBCNTR)
+#define SPLINOFF(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPALINOFF, _SPBLINOFF)
+#define SPSTRIDE(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPASTRIDE, _SPBSTRIDE)
+#define SPPOS(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPAPOS, _SPBPOS)
+#define SPSIZE(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPASIZE, _SPBSIZE)
+#define SPKEYMINVAL(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPAKEYMINVAL, _SPBKEYMINVAL)
+#define SPKEYMSK(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPAKEYMSK, _SPBKEYMSK)
+#define SPSURF(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPASURF, _SPBSURF)
+#define SPKEYMAXVAL(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPAKEYMAXVAL, _SPBKEYMAXVAL)
+#define SPTILEOFF(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPATILEOFF, _SPBTILEOFF)
+#define SPCONSTALPHA(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPACONSTALPHA, _SPBCONSTALPHA)
+#define SPGAMC(pipe, plane) _MMIO_PIPE((pipe) * 2 + (plane), _SPAGAMC, _SPBGAMC)
 
 /*
  * CHV pipe B sprite CSC
@@ -5243,29 +5307,29 @@ enum skl_disp_power_wells {
  * |yg| = |c3 c4 c5| x |yg + yg_ioff| + |yg_ooff|
  * |cb|   |c6 c7 c8|   |cb + cr_ioff|   |cb_ooff|
  */
-#define SPCSCYGOFF(sprite)	(VLV_DISPLAY_BASE + 0x6d900 + (sprite) * 0x1000)
-#define SPCSCCBOFF(sprite)	(VLV_DISPLAY_BASE + 0x6d904 + (sprite) * 0x1000)
-#define SPCSCCROFF(sprite)	(VLV_DISPLAY_BASE + 0x6d908 + (sprite) * 0x1000)
+#define SPCSCYGOFF(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d900 + (sprite) * 0x1000)
+#define SPCSCCBOFF(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d904 + (sprite) * 0x1000)
+#define SPCSCCROFF(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d908 + (sprite) * 0x1000)
 #define  SPCSC_OOFF(x)		(((x) & 0x7ff) << 16) /* s11 */
 #define  SPCSC_IOFF(x)		(((x) & 0x7ff) << 0) /* s11 */
 
-#define SPCSCC01(sprite)	(VLV_DISPLAY_BASE + 0x6d90c + (sprite) * 0x1000)
-#define SPCSCC23(sprite)	(VLV_DISPLAY_BASE + 0x6d910 + (sprite) * 0x1000)
-#define SPCSCC45(sprite)	(VLV_DISPLAY_BASE + 0x6d914 + (sprite) * 0x1000)
-#define SPCSCC67(sprite)	(VLV_DISPLAY_BASE + 0x6d918 + (sprite) * 0x1000)
-#define SPCSCC8(sprite)		(VLV_DISPLAY_BASE + 0x6d91c + (sprite) * 0x1000)
+#define SPCSCC01(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d90c + (sprite) * 0x1000)
+#define SPCSCC23(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d910 + (sprite) * 0x1000)
+#define SPCSCC45(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d914 + (sprite) * 0x1000)
+#define SPCSCC67(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d918 + (sprite) * 0x1000)
+#define SPCSCC8(sprite)		_MMIO(VLV_DISPLAY_BASE + 0x6d91c + (sprite) * 0x1000)
 #define  SPCSC_C1(x)		(((x) & 0x7fff) << 16) /* s3.12 */
 #define  SPCSC_C0(x)		(((x) & 0x7fff) << 0) /* s3.12 */
 
-#define SPCSCYGICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d920 + (sprite) * 0x1000)
-#define SPCSCCBICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d924 + (sprite) * 0x1000)
-#define SPCSCCRICLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d928 + (sprite) * 0x1000)
+#define SPCSCYGICLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d920 + (sprite) * 0x1000)
+#define SPCSCCBICLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d924 + (sprite) * 0x1000)
+#define SPCSCCRICLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d928 + (sprite) * 0x1000)
 #define  SPCSC_IMAX(x)		(((x) & 0x7ff) << 16) /* s11 */
 #define  SPCSC_IMIN(x)		(((x) & 0x7ff) << 0) /* s11 */
 
-#define SPCSCYGOCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d92c + (sprite) * 0x1000)
-#define SPCSCCBOCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d930 + (sprite) * 0x1000)
-#define SPCSCCROCLAMP(sprite)	(VLV_DISPLAY_BASE + 0x6d934 + (sprite) * 0x1000)
+#define SPCSCYGOCLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d92c + (sprite) * 0x1000)
+#define SPCSCCBOCLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d930 + (sprite) * 0x1000)
+#define SPCSCCROCLAMP(sprite)	_MMIO(VLV_DISPLAY_BASE + 0x6d934 + (sprite) * 0x1000)
 #define  SPCSC_OMAX(x)		((x) << 16) /* u10 */
 #define  SPCSC_OMIN(x)		((x) << 0) /* u10 */
 
@@ -5346,7 +5410,7 @@ enum skl_disp_power_wells {
 #define _PLANE_CTL_2(pipe)	_PIPE(pipe, _PLANE_CTL_2_A, _PLANE_CTL_2_B)
 #define _PLANE_CTL_3(pipe)	_PIPE(pipe, _PLANE_CTL_3_A, _PLANE_CTL_3_B)
 #define PLANE_CTL(pipe, plane)	\
-	_PLANE(plane, _PLANE_CTL_1(pipe), _PLANE_CTL_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_CTL_1(pipe), _PLANE_CTL_2(pipe))
 
 #define _PLANE_STRIDE_1_B			0x71188
 #define _PLANE_STRIDE_2_B			0x71288
@@ -5358,7 +5422,7 @@ enum skl_disp_power_wells {
 #define _PLANE_STRIDE_3(pipe)	\
 	_PIPE(pipe, _PLANE_STRIDE_3_A, _PLANE_STRIDE_3_B)
 #define PLANE_STRIDE(pipe, plane)	\
-	_PLANE(plane, _PLANE_STRIDE_1(pipe), _PLANE_STRIDE_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_STRIDE_1(pipe), _PLANE_STRIDE_2(pipe))
 
 #define _PLANE_POS_1_B				0x7118c
 #define _PLANE_POS_2_B				0x7128c
@@ -5367,7 +5431,7 @@ enum skl_disp_power_wells {
 #define _PLANE_POS_2(pipe)	_PIPE(pipe, _PLANE_POS_2_A, _PLANE_POS_2_B)
 #define _PLANE_POS_3(pipe)	_PIPE(pipe, _PLANE_POS_3_A, _PLANE_POS_3_B)
 #define PLANE_POS(pipe, plane)	\
-	_PLANE(plane, _PLANE_POS_1(pipe), _PLANE_POS_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_POS_1(pipe), _PLANE_POS_2(pipe))
 
 #define _PLANE_SIZE_1_B				0x71190
 #define _PLANE_SIZE_2_B				0x71290
@@ -5376,7 +5440,7 @@ enum skl_disp_power_wells {
 #define _PLANE_SIZE_2(pipe)	_PIPE(pipe, _PLANE_SIZE_2_A, _PLANE_SIZE_2_B)
 #define _PLANE_SIZE_3(pipe)	_PIPE(pipe, _PLANE_SIZE_3_A, _PLANE_SIZE_3_B)
 #define PLANE_SIZE(pipe, plane)	\
-	_PLANE(plane, _PLANE_SIZE_1(pipe), _PLANE_SIZE_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_SIZE_1(pipe), _PLANE_SIZE_2(pipe))
 
 #define _PLANE_SURF_1_B				0x7119c
 #define _PLANE_SURF_2_B				0x7129c
@@ -5385,35 +5449,35 @@ enum skl_disp_power_wells {
 #define _PLANE_SURF_2(pipe)	_PIPE(pipe, _PLANE_SURF_2_A, _PLANE_SURF_2_B)
 #define _PLANE_SURF_3(pipe)	_PIPE(pipe, _PLANE_SURF_3_A, _PLANE_SURF_3_B)
 #define PLANE_SURF(pipe, plane)	\
-	_PLANE(plane, _PLANE_SURF_1(pipe), _PLANE_SURF_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_SURF_1(pipe), _PLANE_SURF_2(pipe))
 
 #define _PLANE_OFFSET_1_B			0x711a4
 #define _PLANE_OFFSET_2_B			0x712a4
 #define _PLANE_OFFSET_1(pipe) _PIPE(pipe, _PLANE_OFFSET_1_A, _PLANE_OFFSET_1_B)
 #define _PLANE_OFFSET_2(pipe) _PIPE(pipe, _PLANE_OFFSET_2_A, _PLANE_OFFSET_2_B)
 #define PLANE_OFFSET(pipe, plane)	\
-	_PLANE(plane, _PLANE_OFFSET_1(pipe), _PLANE_OFFSET_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_OFFSET_1(pipe), _PLANE_OFFSET_2(pipe))
 
 #define _PLANE_KEYVAL_1_B			0x71194
 #define _PLANE_KEYVAL_2_B			0x71294
 #define _PLANE_KEYVAL_1(pipe) _PIPE(pipe, _PLANE_KEYVAL_1_A, _PLANE_KEYVAL_1_B)
 #define _PLANE_KEYVAL_2(pipe) _PIPE(pipe, _PLANE_KEYVAL_2_A, _PLANE_KEYVAL_2_B)
 #define PLANE_KEYVAL(pipe, plane)	\
-	_PLANE(plane, _PLANE_KEYVAL_1(pipe), _PLANE_KEYVAL_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_KEYVAL_1(pipe), _PLANE_KEYVAL_2(pipe))
 
 #define _PLANE_KEYMSK_1_B			0x71198
 #define _PLANE_KEYMSK_2_B			0x71298
 #define _PLANE_KEYMSK_1(pipe) _PIPE(pipe, _PLANE_KEYMSK_1_A, _PLANE_KEYMSK_1_B)
 #define _PLANE_KEYMSK_2(pipe) _PIPE(pipe, _PLANE_KEYMSK_2_A, _PLANE_KEYMSK_2_B)
 #define PLANE_KEYMSK(pipe, plane)	\
-	_PLANE(plane, _PLANE_KEYMSK_1(pipe), _PLANE_KEYMSK_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_KEYMSK_1(pipe), _PLANE_KEYMSK_2(pipe))
 
 #define _PLANE_KEYMAX_1_B			0x711a0
 #define _PLANE_KEYMAX_2_B			0x712a0
 #define _PLANE_KEYMAX_1(pipe) _PIPE(pipe, _PLANE_KEYMAX_1_A, _PLANE_KEYMAX_1_B)
 #define _PLANE_KEYMAX_2(pipe) _PIPE(pipe, _PLANE_KEYMAX_2_A, _PLANE_KEYMAX_2_B)
 #define PLANE_KEYMAX(pipe, plane)	\
-	_PLANE(plane, _PLANE_KEYMAX_1(pipe), _PLANE_KEYMAX_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_KEYMAX_1(pipe), _PLANE_KEYMAX_2(pipe))
 
 #define _PLANE_BUF_CFG_1_B			0x7127c
 #define _PLANE_BUF_CFG_2_B			0x7137c
@@ -5422,7 +5486,7 @@ enum skl_disp_power_wells {
 #define _PLANE_BUF_CFG_2(pipe)	\
 	_PIPE(pipe, _PLANE_BUF_CFG_2_A, _PLANE_BUF_CFG_2_B)
 #define PLANE_BUF_CFG(pipe, plane)	\
-	_PLANE(plane, _PLANE_BUF_CFG_1(pipe), _PLANE_BUF_CFG_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_BUF_CFG_1(pipe), _PLANE_BUF_CFG_2(pipe))
 
 #define _PLANE_NV12_BUF_CFG_1_B		0x71278
 #define _PLANE_NV12_BUF_CFG_2_B		0x71378
@@ -5431,26 +5495,26 @@ enum skl_disp_power_wells {
 #define _PLANE_NV12_BUF_CFG_2(pipe)	\
 	_PIPE(pipe, _PLANE_NV12_BUF_CFG_2_A, _PLANE_NV12_BUF_CFG_2_B)
 #define PLANE_NV12_BUF_CFG(pipe, plane)	\
-	_PLANE(plane, _PLANE_NV12_BUF_CFG_1(pipe), _PLANE_NV12_BUF_CFG_2(pipe))
+	_MMIO_PLANE(plane, _PLANE_NV12_BUF_CFG_1(pipe), _PLANE_NV12_BUF_CFG_2(pipe))
 
 /* SKL new cursor registers */
 #define _CUR_BUF_CFG_A				0x7017c
 #define _CUR_BUF_CFG_B				0x7117c
-#define CUR_BUF_CFG(pipe)	_PIPE(pipe, _CUR_BUF_CFG_A, _CUR_BUF_CFG_B)
+#define CUR_BUF_CFG(pipe)	_MMIO_PIPE(pipe, _CUR_BUF_CFG_A, _CUR_BUF_CFG_B)
 
 /* VBIOS regs */
-#define VGACNTRL		0x71400
+#define VGACNTRL		_MMIO(0x71400)
 # define VGA_DISP_DISABLE			(1 << 31)
 # define VGA_2X_MODE				(1 << 30)
 # define VGA_PIPE_B_SELECT			(1 << 29)
 
-#define VLV_VGACNTRL		(VLV_DISPLAY_BASE + 0x71400)
+#define VLV_VGACNTRL		_MMIO(VLV_DISPLAY_BASE + 0x71400)
 
 /* Ironlake */
 
-#define CPU_VGACNTRL	0x41000
+#define CPU_VGACNTRL	_MMIO(0x41000)
 
-#define DIGITAL_PORT_HOTPLUG_CNTRL	0x44030
+#define DIGITAL_PORT_HOTPLUG_CNTRL	_MMIO(0x44030)
 #define  DIGITAL_PORTA_HOTPLUG_ENABLE		(1 << 4)
 #define  DIGITAL_PORTA_PULSE_DURATION_2ms	(0 << 2) /* pre-HSW */
 #define  DIGITAL_PORTA_PULSE_DURATION_4_5ms	(1 << 2) /* pre-HSW */
@@ -5463,26 +5527,26 @@ enum skl_disp_power_wells {
 #define  DIGITAL_PORTA_HOTPLUG_LONG_DETECT	(2 << 0)
 
 /* refresh rate hardware control */
-#define RR_HW_CTL       0x45300
+#define RR_HW_CTL       _MMIO(0x45300)
 #define  RR_HW_LOW_POWER_FRAMES_MASK    0xff
 #define  RR_HW_HIGH_POWER_FRAMES_MASK   0xff00
 
-#define FDI_PLL_BIOS_0  0x46000
+#define FDI_PLL_BIOS_0  _MMIO(0x46000)
 #define  FDI_PLL_FB_CLOCK_MASK  0xff
-#define FDI_PLL_BIOS_1  0x46004
-#define FDI_PLL_BIOS_2  0x46008
-#define DISPLAY_PORT_PLL_BIOS_0         0x4600c
-#define DISPLAY_PORT_PLL_BIOS_1         0x46010
-#define DISPLAY_PORT_PLL_BIOS_2         0x46014
+#define FDI_PLL_BIOS_1  _MMIO(0x46004)
+#define FDI_PLL_BIOS_2  _MMIO(0x46008)
+#define DISPLAY_PORT_PLL_BIOS_0         _MMIO(0x4600c)
+#define DISPLAY_PORT_PLL_BIOS_1         _MMIO(0x46010)
+#define DISPLAY_PORT_PLL_BIOS_2         _MMIO(0x46014)
 
-#define PCH_3DCGDIS0		0x46020
+#define PCH_3DCGDIS0		_MMIO(0x46020)
 # define MARIUNIT_CLOCK_GATE_DISABLE		(1 << 18)
 # define SVSMUNIT_CLOCK_GATE_DISABLE		(1 << 1)
 
-#define PCH_3DCGDIS1		0x46024
+#define PCH_3DCGDIS1		_MMIO(0x46024)
 # define VFMUNIT_CLOCK_GATE_DISABLE		(1 << 11)
 
-#define FDI_PLL_FREQ_CTL        0x46030
+#define FDI_PLL_FREQ_CTL        _MMIO(0x46030)
 #define  FDI_PLL_FREQ_CHANGE_REQUEST    (1<<24)
 #define  FDI_PLL_FREQ_LOCK_LIMIT_MASK   0xfff00
 #define  FDI_PLL_FREQ_DISABLE_COUNT_LIMIT_MASK  0xff
@@ -5519,14 +5583,14 @@ enum skl_disp_power_wells {
 #define _PIPEB_LINK_M2		0x61048
 #define _PIPEB_LINK_N2		0x6104c
 
-#define PIPE_DATA_M1(tran) _TRANSCODER2(tran, _PIPEA_DATA_M1)
-#define PIPE_DATA_N1(tran) _TRANSCODER2(tran, _PIPEA_DATA_N1)
-#define PIPE_DATA_M2(tran) _TRANSCODER2(tran, _PIPEA_DATA_M2)
-#define PIPE_DATA_N2(tran) _TRANSCODER2(tran, _PIPEA_DATA_N2)
-#define PIPE_LINK_M1(tran) _TRANSCODER2(tran, _PIPEA_LINK_M1)
-#define PIPE_LINK_N1(tran) _TRANSCODER2(tran, _PIPEA_LINK_N1)
-#define PIPE_LINK_M2(tran) _TRANSCODER2(tran, _PIPEA_LINK_M2)
-#define PIPE_LINK_N2(tran) _TRANSCODER2(tran, _PIPEA_LINK_N2)
+#define PIPE_DATA_M1(tran) _MMIO_TRANS2(tran, _PIPEA_DATA_M1)
+#define PIPE_DATA_N1(tran) _MMIO_TRANS2(tran, _PIPEA_DATA_N1)
+#define PIPE_DATA_M2(tran) _MMIO_TRANS2(tran, _PIPEA_DATA_M2)
+#define PIPE_DATA_N2(tran) _MMIO_TRANS2(tran, _PIPEA_DATA_N2)
+#define PIPE_LINK_M1(tran) _MMIO_TRANS2(tran, _PIPEA_LINK_M1)
+#define PIPE_LINK_N1(tran) _MMIO_TRANS2(tran, _PIPEA_LINK_N1)
+#define PIPE_LINK_M2(tran) _MMIO_TRANS2(tran, _PIPEA_LINK_M2)
+#define PIPE_LINK_N2(tran) _MMIO_TRANS2(tran, _PIPEA_LINK_N2)
 
 /* CPU panel fitter */
 /* IVB+ has 3 fitters, 0 is 7x5 capable, the other two only 3x3 */
@@ -5549,11 +5613,11 @@ enum skl_disp_power_wells {
 #define _PFA_HSCALE		0x68090
 #define _PFB_HSCALE		0x68890
 
-#define PF_CTL(pipe)		_PIPE(pipe, _PFA_CTL_1, _PFB_CTL_1)
-#define PF_WIN_SZ(pipe)		_PIPE(pipe, _PFA_WIN_SZ, _PFB_WIN_SZ)
-#define PF_WIN_POS(pipe)	_PIPE(pipe, _PFA_WIN_POS, _PFB_WIN_POS)
-#define PF_VSCALE(pipe)		_PIPE(pipe, _PFA_VSCALE, _PFB_VSCALE)
-#define PF_HSCALE(pipe)		_PIPE(pipe, _PFA_HSCALE, _PFB_HSCALE)
+#define PF_CTL(pipe)		_MMIO_PIPE(pipe, _PFA_CTL_1, _PFB_CTL_1)
+#define PF_WIN_SZ(pipe)		_MMIO_PIPE(pipe, _PFA_WIN_SZ, _PFB_WIN_SZ)
+#define PF_WIN_POS(pipe)	_MMIO_PIPE(pipe, _PFA_WIN_POS, _PFB_WIN_POS)
+#define PF_VSCALE(pipe)		_MMIO_PIPE(pipe, _PFA_VSCALE, _PFB_VSCALE)
+#define PF_HSCALE(pipe)		_MMIO_PIPE(pipe, _PFA_HSCALE, _PFB_HSCALE)
 
 #define _PSA_CTL		0x68180
 #define _PSB_CTL		0x68980
@@ -5563,9 +5627,9 @@ enum skl_disp_power_wells {
 #define _PSA_WIN_POS		0x68170
 #define _PSB_WIN_POS		0x68970
 
-#define PS_CTL(pipe)		_PIPE(pipe, _PSA_CTL, _PSB_CTL)
-#define PS_WIN_SZ(pipe)		_PIPE(pipe, _PSA_WIN_SZ, _PSB_WIN_SZ)
-#define PS_WIN_POS(pipe)	_PIPE(pipe, _PSA_WIN_POS, _PSB_WIN_POS)
+#define PS_CTL(pipe)		_MMIO_PIPE(pipe, _PSA_CTL, _PSB_CTL)
+#define PS_WIN_SZ(pipe)		_MMIO_PIPE(pipe, _PSA_WIN_SZ, _PSB_WIN_SZ)
+#define PS_WIN_POS(pipe)	_MMIO_PIPE(pipe, _PSA_WIN_POS, _PSB_WIN_POS)
 
 /*
  * Skylake scalers
@@ -5654,48 +5718,63 @@ enum skl_disp_power_wells {
 #define _PS_ECC_STAT_1C     0x691D0
 
 #define _ID(id, a, b) ((a) + (id)*((b)-(a)))
-#define SKL_PS_CTRL(pipe, id) _PIPE(pipe,        \
+#define SKL_PS_CTRL(pipe, id) _MMIO_PIPE(pipe,        \
 			_ID(id, _PS_1A_CTRL, _PS_2A_CTRL),       \
 			_ID(id, _PS_1B_CTRL, _PS_2B_CTRL))
-#define SKL_PS_PWR_GATE(pipe, id) _PIPE(pipe,    \
+#define SKL_PS_PWR_GATE(pipe, id) _MMIO_PIPE(pipe,    \
 			_ID(id, _PS_PWR_GATE_1A, _PS_PWR_GATE_2A), \
 			_ID(id, _PS_PWR_GATE_1B, _PS_PWR_GATE_2B))
-#define SKL_PS_WIN_POS(pipe, id) _PIPE(pipe,     \
+#define SKL_PS_WIN_POS(pipe, id) _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_WIN_POS_1A, _PS_WIN_POS_2A), \
 			_ID(id, _PS_WIN_POS_1B, _PS_WIN_POS_2B))
-#define SKL_PS_WIN_SZ(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_WIN_SZ(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_WIN_SZ_1A, _PS_WIN_SZ_2A),   \
 			_ID(id, _PS_WIN_SZ_1B, _PS_WIN_SZ_2B))
-#define SKL_PS_VSCALE(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_VSCALE(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_VSCALE_1A, _PS_VSCALE_2A),   \
 			_ID(id, _PS_VSCALE_1B, _PS_VSCALE_2B))
-#define SKL_PS_HSCALE(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_HSCALE(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_HSCALE_1A, _PS_HSCALE_2A),   \
 			_ID(id, _PS_HSCALE_1B, _PS_HSCALE_2B))
-#define SKL_PS_VPHASE(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_VPHASE(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_VPHASE_1A, _PS_VPHASE_2A),   \
 			_ID(id, _PS_VPHASE_1B, _PS_VPHASE_2B))
-#define SKL_PS_HPHASE(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_HPHASE(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_HPHASE_1A, _PS_HPHASE_2A),   \
 			_ID(id, _PS_HPHASE_1B, _PS_HPHASE_2B))
-#define SKL_PS_ECC_STAT(pipe, id)  _PIPE(pipe,     \
+#define SKL_PS_ECC_STAT(pipe, id)  _MMIO_PIPE(pipe,     \
 			_ID(id, _PS_ECC_STAT_1A, _PS_ECC_STAT_2A),   \
-			_ID(id, _PS_ECC_STAT_1B, _PS_ECC_STAT_2B)
+			_ID(id, _PS_ECC_STAT_1B, _PS_ECC_STAT_2B))
 
 /* legacy palette */
 #define _LGC_PALETTE_A           0x4a000
 #define _LGC_PALETTE_B           0x4a800
-#define LGC_PALETTE(pipe, i) (_PIPE(pipe, _LGC_PALETTE_A, _LGC_PALETTE_B) + (i) * 4)
+#define LGC_PALETTE(pipe, i) _MMIO(_PIPE(pipe, _LGC_PALETTE_A, _LGC_PALETTE_B) + (i) * 4)
 
 #define _GAMMA_MODE_A		0x4a480
 #define _GAMMA_MODE_B		0x4ac80
-#define GAMMA_MODE(pipe) _PIPE(pipe, _GAMMA_MODE_A, _GAMMA_MODE_B)
+#define GAMMA_MODE(pipe) _MMIO_PIPE(pipe, _GAMMA_MODE_A, _GAMMA_MODE_B)
 #define GAMMA_MODE_MODE_MASK	(3 << 0)
 #define GAMMA_MODE_MODE_8BIT	(0 << 0)
 #define GAMMA_MODE_MODE_10BIT	(1 << 0)
 #define GAMMA_MODE_MODE_12BIT	(2 << 0)
 #define GAMMA_MODE_MODE_SPLIT	(3 << 0)
 
+/* DMC/CSR */
+#define CSR_PROGRAM(i)		_MMIO(0x80000 + (i) * 4)
+#define CSR_SSP_BASE_ADDR_GEN9	0x00002FC0
+#define CSR_HTP_ADDR_SKL	0x00500034
+#define CSR_SSP_BASE		_MMIO(0x8F074)
+#define CSR_HTP_SKL		_MMIO(0x8F004)
+#define CSR_LAST_WRITE		_MMIO(0x8F034)
+#define CSR_LAST_WRITE_VALUE	0xc003b400
+/* MMIO address range for CSR program (0x80000 - 0x82FFF) */
+#define CSR_MMIO_START_RANGE	0x80000
+#define CSR_MMIO_END_RANGE	0x8FFFF
+#define SKL_CSR_DC3_DC5_COUNT	_MMIO(0x80030)
+#define SKL_CSR_DC5_DC6_COUNT	_MMIO(0x8002C)
+#define BXT_CSR_DC3_DC5_COUNT	_MMIO(0x80038)
+
 /* interrupts */
 #define DE_MASTER_IRQ_CONTROL   (1 << 31)
 #define DE_SPRITEB_FLIP_DONE    (1 << 29)
@@ -5747,20 +5826,20 @@ enum skl_disp_power_wells {
 #define DE_PIPEA_VBLANK_IVB		(1<<0)
 #define DE_PIPE_VBLANK_IVB(pipe)	(1 << ((pipe) * 5))
 
-#define VLV_MASTER_IER			0x4400c /* Gunit master IER */
+#define VLV_MASTER_IER			_MMIO(0x4400c) /* Gunit master IER */
 #define   MASTER_INTERRUPT_ENABLE	(1<<31)
 
-#define DEISR   0x44000
-#define DEIMR   0x44004
-#define DEIIR   0x44008
-#define DEIER   0x4400c
+#define DEISR   _MMIO(0x44000)
+#define DEIMR   _MMIO(0x44004)
+#define DEIIR   _MMIO(0x44008)
+#define DEIER   _MMIO(0x4400c)
 
-#define GTISR   0x44010
-#define GTIMR   0x44014
-#define GTIIR   0x44018
-#define GTIER   0x4401c
+#define GTISR   _MMIO(0x44010)
+#define GTIMR   _MMIO(0x44014)
+#define GTIIR   _MMIO(0x44018)
+#define GTIER   _MMIO(0x4401c)
 
-#define GEN8_MASTER_IRQ			0x44200
+#define GEN8_MASTER_IRQ			_MMIO(0x44200)
 #define  GEN8_MASTER_IRQ_CONTROL	(1<<31)
 #define  GEN8_PCU_IRQ			(1<<30)
 #define  GEN8_DE_PCH_IRQ		(1<<23)
@@ -5777,10 +5856,10 @@ enum skl_disp_power_wells {
 #define  GEN8_GT_BCS_IRQ		(1<<1)
 #define  GEN8_GT_RCS_IRQ		(1<<0)
 
-#define GEN8_GT_ISR(which) (0x44300 + (0x10 * (which)))
-#define GEN8_GT_IMR(which) (0x44304 + (0x10 * (which)))
-#define GEN8_GT_IIR(which) (0x44308 + (0x10 * (which)))
-#define GEN8_GT_IER(which) (0x4430c + (0x10 * (which)))
+#define GEN8_GT_ISR(which) _MMIO(0x44300 + (0x10 * (which)))
+#define GEN8_GT_IMR(which) _MMIO(0x44304 + (0x10 * (which)))
+#define GEN8_GT_IIR(which) _MMIO(0x44308 + (0x10 * (which)))
+#define GEN8_GT_IER(which) _MMIO(0x4430c + (0x10 * (which)))
 
 #define GEN8_RCS_IRQ_SHIFT 0
 #define GEN8_BCS_IRQ_SHIFT 16
@@ -5789,10 +5868,10 @@ enum skl_disp_power_wells {
 #define GEN8_VECS_IRQ_SHIFT 0
 #define GEN8_WD_IRQ_SHIFT 16
 
-#define GEN8_DE_PIPE_ISR(pipe) (0x44400 + (0x10 * (pipe)))
-#define GEN8_DE_PIPE_IMR(pipe) (0x44404 + (0x10 * (pipe)))
-#define GEN8_DE_PIPE_IIR(pipe) (0x44408 + (0x10 * (pipe)))
-#define GEN8_DE_PIPE_IER(pipe) (0x4440c + (0x10 * (pipe)))
+#define GEN8_DE_PIPE_ISR(pipe) _MMIO(0x44400 + (0x10 * (pipe)))
+#define GEN8_DE_PIPE_IMR(pipe) _MMIO(0x44404 + (0x10 * (pipe)))
+#define GEN8_DE_PIPE_IIR(pipe) _MMIO(0x44408 + (0x10 * (pipe)))
+#define GEN8_DE_PIPE_IER(pipe) _MMIO(0x4440c + (0x10 * (pipe)))
 #define  GEN8_PIPE_FIFO_UNDERRUN	(1 << 31)
 #define  GEN8_PIPE_CDCLK_CRC_ERROR	(1 << 29)
 #define  GEN8_PIPE_CDCLK_CRC_DONE	(1 << 28)
@@ -5825,10 +5904,10 @@ enum skl_disp_power_wells {
 	 GEN9_PIPE_PLANE2_FAULT | \
 	 GEN9_PIPE_PLANE1_FAULT)
 
-#define GEN8_DE_PORT_ISR 0x44440
-#define GEN8_DE_PORT_IMR 0x44444
-#define GEN8_DE_PORT_IIR 0x44448
-#define GEN8_DE_PORT_IER 0x4444c
+#define GEN8_DE_PORT_ISR _MMIO(0x44440)
+#define GEN8_DE_PORT_IMR _MMIO(0x44444)
+#define GEN8_DE_PORT_IIR _MMIO(0x44448)
+#define GEN8_DE_PORT_IER _MMIO(0x4444c)
 #define  GEN9_AUX_CHANNEL_D		(1 << 27)
 #define  GEN9_AUX_CHANNEL_C		(1 << 26)
 #define  GEN9_AUX_CHANNEL_B		(1 << 25)
@@ -5842,23 +5921,23 @@ enum skl_disp_power_wells {
 #define  BXT_DE_PORT_GMBUS		(1 << 1)
 #define  GEN8_AUX_CHANNEL_A		(1 << 0)
 
-#define GEN8_DE_MISC_ISR 0x44460
-#define GEN8_DE_MISC_IMR 0x44464
-#define GEN8_DE_MISC_IIR 0x44468
-#define GEN8_DE_MISC_IER 0x4446c
+#define GEN8_DE_MISC_ISR _MMIO(0x44460)
+#define GEN8_DE_MISC_IMR _MMIO(0x44464)
+#define GEN8_DE_MISC_IIR _MMIO(0x44468)
+#define GEN8_DE_MISC_IER _MMIO(0x4446c)
 #define  GEN8_DE_MISC_GSE		(1 << 27)
 
-#define GEN8_PCU_ISR 0x444e0
-#define GEN8_PCU_IMR 0x444e4
-#define GEN8_PCU_IIR 0x444e8
-#define GEN8_PCU_IER 0x444ec
+#define GEN8_PCU_ISR _MMIO(0x444e0)
+#define GEN8_PCU_IMR _MMIO(0x444e4)
+#define GEN8_PCU_IIR _MMIO(0x444e8)
+#define GEN8_PCU_IER _MMIO(0x444ec)
 
-#define ILK_DISPLAY_CHICKEN2	0x42004
+#define ILK_DISPLAY_CHICKEN2	_MMIO(0x42004)
 /* Required on all Ironlake and Sandybridge according to the B-Spec. */
 #define  ILK_ELPIN_409_SELECT	(1 << 25)
 #define  ILK_DPARB_GATE	(1<<22)
 #define  ILK_VSDPFD_FULL	(1<<21)
-#define FUSE_STRAP			0x42014
+#define FUSE_STRAP			_MMIO(0x42014)
 #define  ILK_INTERNAL_GRAPHICS_DISABLE	(1 << 31)
 #define  ILK_INTERNAL_DISPLAY_DISABLE	(1 << 30)
 #define  ILK_DISPLAY_DEBUG_DISABLE	(1 << 29)
@@ -5867,18 +5946,18 @@ enum skl_disp_power_wells {
 #define  HSW_CDCLK_LIMIT		(1 << 24)
 #define  ILK_DESKTOP			(1 << 23)
 
-#define ILK_DSPCLK_GATE_D			0x42020
+#define ILK_DSPCLK_GATE_D			_MMIO(0x42020)
 #define   ILK_VRHUNIT_CLOCK_GATE_DISABLE	(1 << 28)
 #define   ILK_DPFCUNIT_CLOCK_GATE_DISABLE	(1 << 9)
 #define   ILK_DPFCRUNIT_CLOCK_GATE_DISABLE	(1 << 8)
 #define   ILK_DPFDUNIT_CLOCK_GATE_ENABLE	(1 << 7)
 #define   ILK_DPARBUNIT_CLOCK_GATE_ENABLE	(1 << 5)
 
-#define IVB_CHICKEN3	0x4200c
+#define IVB_CHICKEN3	_MMIO(0x4200c)
 # define CHICKEN3_DGMG_REQ_OUT_FIX_DISABLE	(1 << 5)
 # define CHICKEN3_DGMG_DONE_FIX_DISABLE		(1 << 2)
 
-#define CHICKEN_PAR1_1		0x42080
+#define CHICKEN_PAR1_1		_MMIO(0x42080)
 #define  DPA_MASK_VBLANK_SRD	(1 << 15)
 #define  FORCE_ARB_IDLE_PLANES	(1 << 14)
 
@@ -5886,70 +5965,70 @@ enum skl_disp_power_wells {
 #define _CHICKEN_PIPESL_1_B	0x420b4
 #define  HSW_FBCQ_DIS			(1 << 22)
 #define  BDW_DPRS_MASK_VBLANK_SRD	(1 << 0)
-#define CHICKEN_PIPESL_1(pipe) _PIPE(pipe, _CHICKEN_PIPESL_1_A, _CHICKEN_PIPESL_1_B)
+#define CHICKEN_PIPESL_1(pipe) _MMIO_PIPE(pipe, _CHICKEN_PIPESL_1_A, _CHICKEN_PIPESL_1_B)
 
-#define DISP_ARB_CTL	0x45000
+#define DISP_ARB_CTL	_MMIO(0x45000)
 #define  DISP_TILE_SURFACE_SWIZZLING	(1<<13)
 #define  DISP_FBC_WM_DIS		(1<<15)
-#define DISP_ARB_CTL2	0x45004
+#define DISP_ARB_CTL2	_MMIO(0x45004)
 #define  DISP_DATA_PARTITION_5_6	(1<<6)
-#define DBUF_CTL	0x45008
+#define DBUF_CTL	_MMIO(0x45008)
 #define  DBUF_POWER_REQUEST		(1<<31)
 #define  DBUF_POWER_STATE		(1<<30)
-#define GEN7_MSG_CTL	0x45010
+#define GEN7_MSG_CTL	_MMIO(0x45010)
 #define  WAIT_FOR_PCH_RESET_ACK		(1<<1)
 #define  WAIT_FOR_PCH_FLR_ACK		(1<<0)
-#define HSW_NDE_RSTWRN_OPT	0x46408
+#define HSW_NDE_RSTWRN_OPT	_MMIO(0x46408)
 #define  RESET_PCH_HANDSHAKE_ENABLE	(1<<4)
 
-#define SKL_DFSM			0x51000
+#define SKL_DFSM			_MMIO(0x51000)
 #define SKL_DFSM_CDCLK_LIMIT_MASK	(3 << 23)
 #define SKL_DFSM_CDCLK_LIMIT_675	(0 << 23)
 #define SKL_DFSM_CDCLK_LIMIT_540	(1 << 23)
 #define SKL_DFSM_CDCLK_LIMIT_450	(2 << 23)
 #define SKL_DFSM_CDCLK_LIMIT_337_5	(3 << 23)
 
-#define FF_SLICE_CS_CHICKEN2			0x20e4
+#define FF_SLICE_CS_CHICKEN2			_MMIO(0x20e4)
 #define  GEN9_TSG_BARRIER_ACK_DISABLE		(1<<8)
 
 /* GEN7 chicken */
-#define GEN7_COMMON_SLICE_CHICKEN1		0x7010
+#define GEN7_COMMON_SLICE_CHICKEN1		_MMIO(0x7010)
 # define GEN7_CSC1_RHWO_OPT_DISABLE_IN_RCC	((1<<10) | (1<<26))
 # define GEN9_RHWO_OPTIMIZATION_DISABLE		(1<<14)
-#define COMMON_SLICE_CHICKEN2			0x7014
+#define COMMON_SLICE_CHICKEN2			_MMIO(0x7014)
 # define GEN8_CSC2_SBE_VUE_CACHE_CONSERVATIVE	(1<<0)
 
-#define HIZ_CHICKEN					0x7018
+#define HIZ_CHICKEN					_MMIO(0x7018)
 # define CHV_HZ_8X8_MODE_IN_1X				(1<<15)
 # define BDW_HIZ_POWER_COMPILER_CLOCK_GATING_DISABLE	(1<<3)
 
-#define GEN9_SLICE_COMMON_ECO_CHICKEN0		0x7308
+#define GEN9_SLICE_COMMON_ECO_CHICKEN0		_MMIO(0x7308)
 #define  DISABLE_PIXEL_MASK_CAMMING		(1<<14)
 
-#define GEN7_L3SQCREG1				0xB010
+#define GEN7_L3SQCREG1				_MMIO(0xB010)
 #define  VLV_B0_WA_L3SQCREG1_VALUE		0x00D30000
 
-#define GEN8_L3SQCREG1				0xB100
+#define GEN8_L3SQCREG1				_MMIO(0xB100)
 #define  BDW_WA_L3SQCREG1_DEFAULT		0x784000
 
-#define GEN7_L3CNTLREG1				0xB01C
+#define GEN7_L3CNTLREG1				_MMIO(0xB01C)
 #define  GEN7_WA_FOR_GEN7_L3_CONTROL			0x3C47FF8C
 #define  GEN7_L3AGDIS				(1<<19)
-#define GEN7_L3CNTLREG2				0xB020
-#define GEN7_L3CNTLREG3				0xB024
+#define GEN7_L3CNTLREG2				_MMIO(0xB020)
+#define GEN7_L3CNTLREG3				_MMIO(0xB024)
 
-#define GEN7_L3_CHICKEN_MODE_REGISTER		0xB030
+#define GEN7_L3_CHICKEN_MODE_REGISTER		_MMIO(0xB030)
 #define  GEN7_WA_L3_CHICKEN_MODE				0x20000000
 
-#define GEN7_L3SQCREG4				0xb034
+#define GEN7_L3SQCREG4				_MMIO(0xb034)
 #define  L3SQ_URB_READ_CAM_MATCH_DISABLE	(1<<27)
 
-#define GEN8_L3SQCREG4				0xb118
+#define GEN8_L3SQCREG4				_MMIO(0xb118)
 #define  GEN8_LQSC_RO_PERF_DIS			(1<<27)
 #define  GEN8_LQSC_FLUSH_COHERENT_LINES		(1<<21)
 
 /* GEN8 chicken */
-#define HDC_CHICKEN0				0x7300
+#define HDC_CHICKEN0				_MMIO(0x7300)
 #define  HDC_FORCE_CSR_NON_COHERENT_OVR_DISABLE	(1<<15)
 #define  HDC_FENCE_DEST_SLM_DISABLE		(1<<14)
 #define  HDC_DONOT_FETCH_MEM_WHEN_MASKED	(1<<11)
@@ -5958,17 +6037,17 @@ enum skl_disp_power_wells {
 #define  HDC_BARRIER_PERFORMANCE_DISABLE	(1<<10)
 
 /* GEN9 chicken */
-#define SLICE_ECO_CHICKEN0			0x7308
+#define SLICE_ECO_CHICKEN0			_MMIO(0x7308)
 #define   PIXEL_MASK_CAMMING_DISABLE		(1 << 14)
 
 /* WaCatErrorRejectionIssue */
-#define GEN7_SQ_CHICKEN_MBCUNIT_CONFIG		0x9030
+#define GEN7_SQ_CHICKEN_MBCUNIT_CONFIG		_MMIO(0x9030)
 #define  GEN7_SQ_CHICKEN_MBCUNIT_SQINTMOB	(1<<11)
 
-#define HSW_SCRATCH1				0xb038
+#define HSW_SCRATCH1				_MMIO(0xb038)
 #define  HSW_SCRATCH1_L3_DATA_ATOMICS_DISABLE	(1<<27)
 
-#define BDW_SCRATCH1					0xb11c
+#define BDW_SCRATCH1					_MMIO(0xb11c)
 #define  GEN9_LBS_SLA_RETRY_TIMER_DECREMENT_ENABLE	(1<<2)
 
 /* PCH */
@@ -6062,12 +6141,12 @@ enum skl_disp_power_wells {
 				 SDE_FDI_RXB_CPT | \
 				 SDE_FDI_RXA_CPT)
 
-#define SDEISR  0xc4000
-#define SDEIMR  0xc4004
-#define SDEIIR  0xc4008
-#define SDEIER  0xc400c
+#define SDEISR  _MMIO(0xc4000)
+#define SDEIMR  _MMIO(0xc4004)
+#define SDEIIR  _MMIO(0xc4008)
+#define SDEIER  _MMIO(0xc400c)
 
-#define SERR_INT			0xc4040
+#define SERR_INT			_MMIO(0xc4040)
 #define  SERR_INT_POISON		(1<<31)
 #define  SERR_INT_TRANS_C_FIFO_UNDERRUN	(1<<6)
 #define  SERR_INT_TRANS_B_FIFO_UNDERRUN	(1<<3)
@@ -6075,7 +6154,7 @@ enum skl_disp_power_wells {
 #define  SERR_INT_TRANS_FIFO_UNDERRUN(pipe)	(1<<((pipe)*3))
 
 /* digital port hotplug */
-#define PCH_PORT_HOTPLUG		0xc4030	/* SHOTPLUG_CTL */
+#define PCH_PORT_HOTPLUG		_MMIO(0xc4030)	/* SHOTPLUG_CTL */
 #define  PORTA_HOTPLUG_ENABLE		(1 << 28) /* LPT:LP+ & BXT */
 #define  PORTA_HOTPLUG_STATUS_MASK	(3 << 24) /* SPT+ & BXT */
 #define  PORTA_HOTPLUG_NO_DETECT	(0 << 24) /* SPT+ & BXT */
@@ -6112,42 +6191,42 @@ enum skl_disp_power_wells {
 #define  PORTB_HOTPLUG_SHORT_DETECT	(1 << 0)
 #define  PORTB_HOTPLUG_LONG_DETECT	(2 << 0)
 
-#define PCH_PORT_HOTPLUG2		0xc403C	/* SHOTPLUG_CTL2 SPT+ */
+#define PCH_PORT_HOTPLUG2		_MMIO(0xc403C)	/* SHOTPLUG_CTL2 SPT+ */
 #define  PORTE_HOTPLUG_ENABLE		(1 << 4)
 #define  PORTE_HOTPLUG_STATUS_MASK	(3 << 0)
 #define  PORTE_HOTPLUG_NO_DETECT	(0 << 0)
 #define  PORTE_HOTPLUG_SHORT_DETECT	(1 << 0)
 #define  PORTE_HOTPLUG_LONG_DETECT	(2 << 0)
 
-#define PCH_GPIOA               0xc5010
-#define PCH_GPIOB               0xc5014
-#define PCH_GPIOC               0xc5018
-#define PCH_GPIOD               0xc501c
-#define PCH_GPIOE               0xc5020
-#define PCH_GPIOF               0xc5024
+#define PCH_GPIOA               _MMIO(0xc5010)
+#define PCH_GPIOB               _MMIO(0xc5014)
+#define PCH_GPIOC               _MMIO(0xc5018)
+#define PCH_GPIOD               _MMIO(0xc501c)
+#define PCH_GPIOE               _MMIO(0xc5020)
+#define PCH_GPIOF               _MMIO(0xc5024)
 
-#define PCH_GMBUS0		0xc5100
-#define PCH_GMBUS1		0xc5104
-#define PCH_GMBUS2		0xc5108
-#define PCH_GMBUS3		0xc510c
-#define PCH_GMBUS4		0xc5110
-#define PCH_GMBUS5		0xc5120
+#define PCH_GMBUS0		_MMIO(0xc5100)
+#define PCH_GMBUS1		_MMIO(0xc5104)
+#define PCH_GMBUS2		_MMIO(0xc5108)
+#define PCH_GMBUS3		_MMIO(0xc510c)
+#define PCH_GMBUS4		_MMIO(0xc5110)
+#define PCH_GMBUS5		_MMIO(0xc5120)
 
 #define _PCH_DPLL_A              0xc6014
 #define _PCH_DPLL_B              0xc6018
-#define PCH_DPLL(pll) (pll == 0 ? _PCH_DPLL_A : _PCH_DPLL_B)
+#define PCH_DPLL(pll) _MMIO(pll == 0 ? _PCH_DPLL_A : _PCH_DPLL_B)
 
 #define _PCH_FPA0                0xc6040
 #define  FP_CB_TUNE		(0x3<<22)
 #define _PCH_FPA1                0xc6044
 #define _PCH_FPB0                0xc6048
 #define _PCH_FPB1                0xc604c
-#define PCH_FP0(pll) (pll == 0 ? _PCH_FPA0 : _PCH_FPB0)
-#define PCH_FP1(pll) (pll == 0 ? _PCH_FPA1 : _PCH_FPB1)
+#define PCH_FP0(pll) _MMIO(pll == 0 ? _PCH_FPA0 : _PCH_FPB0)
+#define PCH_FP1(pll) _MMIO(pll == 0 ? _PCH_FPA1 : _PCH_FPB1)
 
-#define PCH_DPLL_TEST           0xc606c
+#define PCH_DPLL_TEST           _MMIO(0xc606c)
 
-#define PCH_DREF_CONTROL        0xC6200
+#define PCH_DREF_CONTROL        _MMIO(0xC6200)
 #define  DREF_CONTROL_MASK      0x7fc3
 #define  DREF_CPU_SOURCE_OUTPUT_DISABLE         (0<<13)
 #define  DREF_CPU_SOURCE_OUTPUT_DOWNSPREAD      (2<<13)
@@ -6170,19 +6249,19 @@ enum skl_disp_power_wells {
 #define  DREF_SSC4_DISABLE                      (0)
 #define  DREF_SSC4_ENABLE                       (1)
 
-#define PCH_RAWCLK_FREQ         0xc6204
+#define PCH_RAWCLK_FREQ         _MMIO(0xc6204)
 #define  FDL_TP1_TIMER_SHIFT    12
 #define  FDL_TP1_TIMER_MASK     (3<<12)
 #define  FDL_TP2_TIMER_SHIFT    10
 #define  FDL_TP2_TIMER_MASK     (3<<10)
 #define  RAWCLK_FREQ_MASK       0x3ff
 
-#define PCH_DPLL_TMR_CFG        0xc6208
+#define PCH_DPLL_TMR_CFG        _MMIO(0xc6208)
 
-#define PCH_SSC4_PARMS          0xc6210
-#define PCH_SSC4_AUX_PARMS      0xc6214
+#define PCH_SSC4_PARMS          _MMIO(0xc6210)
+#define PCH_SSC4_AUX_PARMS      _MMIO(0xc6214)
 
-#define PCH_DPLL_SEL		0xc7000
+#define PCH_DPLL_SEL		_MMIO(0xc7000)
 #define	 TRANS_DPLLB_SEL(pipe)		(1 << ((pipe) * 4))
 #define	 TRANS_DPLLA_SEL(pipe)		0
 #define  TRANS_DPLL_ENABLE(pipe)	(1 << ((pipe) * 4 + 3))
@@ -6230,79 +6309,73 @@ enum skl_disp_power_wells {
 #define _VIDEO_DIP_DATA_B        0xe1208
 #define _VIDEO_DIP_GCP_B         0xe1210
 
-#define TVIDEO_DIP_CTL(pipe) _PIPE(pipe, _VIDEO_DIP_CTL_A, _VIDEO_DIP_CTL_B)
-#define TVIDEO_DIP_DATA(pipe) _PIPE(pipe, _VIDEO_DIP_DATA_A, _VIDEO_DIP_DATA_B)
-#define TVIDEO_DIP_GCP(pipe) _PIPE(pipe, _VIDEO_DIP_GCP_A, _VIDEO_DIP_GCP_B)
+#define TVIDEO_DIP_CTL(pipe) _MMIO_PIPE(pipe, _VIDEO_DIP_CTL_A, _VIDEO_DIP_CTL_B)
+#define TVIDEO_DIP_DATA(pipe) _MMIO_PIPE(pipe, _VIDEO_DIP_DATA_A, _VIDEO_DIP_DATA_B)
+#define TVIDEO_DIP_GCP(pipe) _MMIO_PIPE(pipe, _VIDEO_DIP_GCP_A, _VIDEO_DIP_GCP_B)
 
 /* Per-transcoder DIP controls (VLV) */
-#define VLV_VIDEO_DIP_CTL_A		(VLV_DISPLAY_BASE + 0x60200)
-#define VLV_VIDEO_DIP_DATA_A		(VLV_DISPLAY_BASE + 0x60208)
-#define VLV_VIDEO_DIP_GDCP_PAYLOAD_A	(VLV_DISPLAY_BASE + 0x60210)
+#define _VLV_VIDEO_DIP_CTL_A		(VLV_DISPLAY_BASE + 0x60200)
+#define _VLV_VIDEO_DIP_DATA_A		(VLV_DISPLAY_BASE + 0x60208)
+#define _VLV_VIDEO_DIP_GDCP_PAYLOAD_A	(VLV_DISPLAY_BASE + 0x60210)
 
-#define VLV_VIDEO_DIP_CTL_B		(VLV_DISPLAY_BASE + 0x61170)
-#define VLV_VIDEO_DIP_DATA_B		(VLV_DISPLAY_BASE + 0x61174)
-#define VLV_VIDEO_DIP_GDCP_PAYLOAD_B	(VLV_DISPLAY_BASE + 0x61178)
+#define _VLV_VIDEO_DIP_CTL_B		(VLV_DISPLAY_BASE + 0x61170)
+#define _VLV_VIDEO_DIP_DATA_B		(VLV_DISPLAY_BASE + 0x61174)
+#define _VLV_VIDEO_DIP_GDCP_PAYLOAD_B	(VLV_DISPLAY_BASE + 0x61178)
 
-#define CHV_VIDEO_DIP_CTL_C		(VLV_DISPLAY_BASE + 0x611f0)
-#define CHV_VIDEO_DIP_DATA_C		(VLV_DISPLAY_BASE + 0x611f4)
-#define CHV_VIDEO_DIP_GDCP_PAYLOAD_C	(VLV_DISPLAY_BASE + 0x611f8)
+#define _CHV_VIDEO_DIP_CTL_C		(VLV_DISPLAY_BASE + 0x611f0)
+#define _CHV_VIDEO_DIP_DATA_C		(VLV_DISPLAY_BASE + 0x611f4)
+#define _CHV_VIDEO_DIP_GDCP_PAYLOAD_C	(VLV_DISPLAY_BASE + 0x611f8)
 
 #define VLV_TVIDEO_DIP_CTL(pipe) \
-	_PIPE3((pipe), VLV_VIDEO_DIP_CTL_A, \
-	       VLV_VIDEO_DIP_CTL_B, CHV_VIDEO_DIP_CTL_C)
+	_MMIO_PIPE3((pipe), _VLV_VIDEO_DIP_CTL_A, \
+	       _VLV_VIDEO_DIP_CTL_B, _CHV_VIDEO_DIP_CTL_C)
 #define VLV_TVIDEO_DIP_DATA(pipe) \
-	_PIPE3((pipe), VLV_VIDEO_DIP_DATA_A, \
-	       VLV_VIDEO_DIP_DATA_B, CHV_VIDEO_DIP_DATA_C)
+	_MMIO_PIPE3((pipe), _VLV_VIDEO_DIP_DATA_A, \
+	       _VLV_VIDEO_DIP_DATA_B, _CHV_VIDEO_DIP_DATA_C)
 #define VLV_TVIDEO_DIP_GCP(pipe) \
-	_PIPE3((pipe), VLV_VIDEO_DIP_GDCP_PAYLOAD_A, \
-		VLV_VIDEO_DIP_GDCP_PAYLOAD_B, CHV_VIDEO_DIP_GDCP_PAYLOAD_C)
+	_MMIO_PIPE3((pipe), _VLV_VIDEO_DIP_GDCP_PAYLOAD_A, \
+		_VLV_VIDEO_DIP_GDCP_PAYLOAD_B, _CHV_VIDEO_DIP_GDCP_PAYLOAD_C)
 
 /* Haswell DIP controls */
-#define HSW_VIDEO_DIP_CTL_A		0x60200
-#define HSW_VIDEO_DIP_AVI_DATA_A	0x60220
-#define HSW_VIDEO_DIP_VS_DATA_A		0x60260
-#define HSW_VIDEO_DIP_SPD_DATA_A	0x602A0
-#define HSW_VIDEO_DIP_GMP_DATA_A	0x602E0
-#define HSW_VIDEO_DIP_VSC_DATA_A	0x60320
-#define HSW_VIDEO_DIP_AVI_ECC_A		0x60240
-#define HSW_VIDEO_DIP_VS_ECC_A		0x60280
-#define HSW_VIDEO_DIP_SPD_ECC_A		0x602C0
-#define HSW_VIDEO_DIP_GMP_ECC_A		0x60300
-#define HSW_VIDEO_DIP_VSC_ECC_A		0x60344
-#define HSW_VIDEO_DIP_GCP_A		0x60210
-
-#define HSW_VIDEO_DIP_CTL_B		0x61200
-#define HSW_VIDEO_DIP_AVI_DATA_B	0x61220
-#define HSW_VIDEO_DIP_VS_DATA_B		0x61260
-#define HSW_VIDEO_DIP_SPD_DATA_B	0x612A0
-#define HSW_VIDEO_DIP_GMP_DATA_B	0x612E0
-#define HSW_VIDEO_DIP_VSC_DATA_B	0x61320
-#define HSW_VIDEO_DIP_BVI_ECC_B		0x61240
-#define HSW_VIDEO_DIP_VS_ECC_B		0x61280
-#define HSW_VIDEO_DIP_SPD_ECC_B		0x612C0
-#define HSW_VIDEO_DIP_GMP_ECC_B		0x61300
-#define HSW_VIDEO_DIP_VSC_ECC_B		0x61344
-#define HSW_VIDEO_DIP_GCP_B		0x61210
-
-#define HSW_TVIDEO_DIP_CTL(trans) \
-	 _TRANSCODER2(trans, HSW_VIDEO_DIP_CTL_A)
-#define HSW_TVIDEO_DIP_AVI_DATA(trans, i) \
-	(_TRANSCODER2(trans, HSW_VIDEO_DIP_AVI_DATA_A) + (i) * 4)
-#define HSW_TVIDEO_DIP_VS_DATA(trans, i) \
-	(_TRANSCODER2(trans, HSW_VIDEO_DIP_VS_DATA_A) + (i) * 4)
-#define HSW_TVIDEO_DIP_SPD_DATA(trans, i) \
-	(_TRANSCODER2(trans, HSW_VIDEO_DIP_SPD_DATA_A) + (i) * 4)
-#define HSW_TVIDEO_DIP_GCP(trans) \
-	_TRANSCODER2(trans, HSW_VIDEO_DIP_GCP_A)
-#define HSW_TVIDEO_DIP_VSC_DATA(trans, i) \
-	(_TRANSCODER2(trans, HSW_VIDEO_DIP_VSC_DATA_A) + (i) * 4)
-
-#define HSW_STEREO_3D_CTL_A	0x70020
-#define   S3D_ENABLE		(1<<31)
-#define HSW_STEREO_3D_CTL_B	0x71020
-
-#define HSW_STEREO_3D_CTL(trans) \
-	_PIPE2(trans, HSW_STEREO_3D_CTL_A)
+
+#define _HSW_VIDEO_DIP_CTL_A		0x60200
+#define _HSW_VIDEO_DIP_AVI_DATA_A	0x60220
+#define _HSW_VIDEO_DIP_VS_DATA_A	0x60260
+#define _HSW_VIDEO_DIP_SPD_DATA_A	0x602A0
+#define _HSW_VIDEO_DIP_GMP_DATA_A	0x602E0
+#define _HSW_VIDEO_DIP_VSC_DATA_A	0x60320
+#define _HSW_VIDEO_DIP_AVI_ECC_A	0x60240
+#define _HSW_VIDEO_DIP_VS_ECC_A		0x60280
+#define _HSW_VIDEO_DIP_SPD_ECC_A	0x602C0
+#define _HSW_VIDEO_DIP_GMP_ECC_A	0x60300
+#define _HSW_VIDEO_DIP_VSC_ECC_A	0x60344
+#define _HSW_VIDEO_DIP_GCP_A		0x60210
+
+#define _HSW_VIDEO_DIP_CTL_B		0x61200
+#define _HSW_VIDEO_DIP_AVI_DATA_B	0x61220
+#define _HSW_VIDEO_DIP_VS_DATA_B	0x61260
+#define _HSW_VIDEO_DIP_SPD_DATA_B	0x612A0
+#define _HSW_VIDEO_DIP_GMP_DATA_B	0x612E0
+#define _HSW_VIDEO_DIP_VSC_DATA_B	0x61320
+#define _HSW_VIDEO_DIP_BVI_ECC_B	0x61240
+#define _HSW_VIDEO_DIP_VS_ECC_B		0x61280
+#define _HSW_VIDEO_DIP_SPD_ECC_B	0x612C0
+#define _HSW_VIDEO_DIP_GMP_ECC_B	0x61300
+#define _HSW_VIDEO_DIP_VSC_ECC_B	0x61344
+#define _HSW_VIDEO_DIP_GCP_B		0x61210
+
+#define HSW_TVIDEO_DIP_CTL(trans)		_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_CTL_A)
+#define HSW_TVIDEO_DIP_AVI_DATA(trans, i)	_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_AVI_DATA_A + (i) * 4)
+#define HSW_TVIDEO_DIP_VS_DATA(trans, i)	_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_VS_DATA_A + (i) * 4)
+#define HSW_TVIDEO_DIP_SPD_DATA(trans, i)	_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_SPD_DATA_A + (i) * 4)
+#define HSW_TVIDEO_DIP_GCP(trans)		_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_GCP_A)
+#define HSW_TVIDEO_DIP_VSC_DATA(trans, i)	_MMIO_TRANS2(trans, _HSW_VIDEO_DIP_VSC_DATA_A + (i) * 4)
+
+#define _HSW_STEREO_3D_CTL_A		0x70020
+#define   S3D_ENABLE			(1<<31)
+#define _HSW_STEREO_3D_CTL_B		0x71020
+
+#define HSW_STEREO_3D_CTL(trans)	_MMIO_PIPE2(trans, _HSW_STEREO_3D_CTL_A)
 
 #define _PCH_TRANS_HTOTAL_B          0xe1000
 #define _PCH_TRANS_HBLANK_B          0xe1004
@@ -6310,16 +6383,15 @@ enum skl_disp_power_wells {
 #define _PCH_TRANS_VTOTAL_B          0xe100c
 #define _PCH_TRANS_VBLANK_B          0xe1010
 #define _PCH_TRANS_VSYNC_B           0xe1014
-#define _PCH_TRANS_VSYNCSHIFT_B	 0xe1028
+#define _PCH_TRANS_VSYNCSHIFT_B 0xe1028
 
-#define PCH_TRANS_HTOTAL(pipe) _PIPE(pipe, _PCH_TRANS_HTOTAL_A, _PCH_TRANS_HTOTAL_B)
-#define PCH_TRANS_HBLANK(pipe) _PIPE(pipe, _PCH_TRANS_HBLANK_A, _PCH_TRANS_HBLANK_B)
-#define PCH_TRANS_HSYNC(pipe) _PIPE(pipe, _PCH_TRANS_HSYNC_A, _PCH_TRANS_HSYNC_B)
-#define PCH_TRANS_VTOTAL(pipe) _PIPE(pipe, _PCH_TRANS_VTOTAL_A, _PCH_TRANS_VTOTAL_B)
-#define PCH_TRANS_VBLANK(pipe) _PIPE(pipe, _PCH_TRANS_VBLANK_A, _PCH_TRANS_VBLANK_B)
-#define PCH_TRANS_VSYNC(pipe) _PIPE(pipe, _PCH_TRANS_VSYNC_A, _PCH_TRANS_VSYNC_B)
-#define PCH_TRANS_VSYNCSHIFT(pipe) _PIPE(pipe, _PCH_TRANS_VSYNCSHIFT_A, \
-					 _PCH_TRANS_VSYNCSHIFT_B)
+#define PCH_TRANS_HTOTAL(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_HTOTAL_A, _PCH_TRANS_HTOTAL_B)
+#define PCH_TRANS_HBLANK(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_HBLANK_A, _PCH_TRANS_HBLANK_B)
+#define PCH_TRANS_HSYNC(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_HSYNC_A, _PCH_TRANS_HSYNC_B)
+#define PCH_TRANS_VTOTAL(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_VTOTAL_A, _PCH_TRANS_VTOTAL_B)
+#define PCH_TRANS_VBLANK(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_VBLANK_A, _PCH_TRANS_VBLANK_B)
+#define PCH_TRANS_VSYNC(pipe)		_MMIO_PIPE(pipe, _PCH_TRANS_VSYNC_A, _PCH_TRANS_VSYNC_B)
+#define PCH_TRANS_VSYNCSHIFT(pipe)	_MMIO_PIPE(pipe, _PCH_TRANS_VSYNCSHIFT_A, _PCH_TRANS_VSYNCSHIFT_B)
 
 #define _PCH_TRANSB_DATA_M1	0xe1030
 #define _PCH_TRANSB_DATA_N1	0xe1034
@@ -6330,19 +6402,19 @@ enum skl_disp_power_wells {
 #define _PCH_TRANSB_LINK_M2	0xe1048
 #define _PCH_TRANSB_LINK_N2	0xe104c
 
-#define PCH_TRANS_DATA_M1(pipe) _PIPE(pipe, _PCH_TRANSA_DATA_M1, _PCH_TRANSB_DATA_M1)
-#define PCH_TRANS_DATA_N1(pipe) _PIPE(pipe, _PCH_TRANSA_DATA_N1, _PCH_TRANSB_DATA_N1)
-#define PCH_TRANS_DATA_M2(pipe) _PIPE(pipe, _PCH_TRANSA_DATA_M2, _PCH_TRANSB_DATA_M2)
-#define PCH_TRANS_DATA_N2(pipe) _PIPE(pipe, _PCH_TRANSA_DATA_N2, _PCH_TRANSB_DATA_N2)
-#define PCH_TRANS_LINK_M1(pipe) _PIPE(pipe, _PCH_TRANSA_LINK_M1, _PCH_TRANSB_LINK_M1)
-#define PCH_TRANS_LINK_N1(pipe) _PIPE(pipe, _PCH_TRANSA_LINK_N1, _PCH_TRANSB_LINK_N1)
-#define PCH_TRANS_LINK_M2(pipe) _PIPE(pipe, _PCH_TRANSA_LINK_M2, _PCH_TRANSB_LINK_M2)
-#define PCH_TRANS_LINK_N2(pipe) _PIPE(pipe, _PCH_TRANSA_LINK_N2, _PCH_TRANSB_LINK_N2)
+#define PCH_TRANS_DATA_M1(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_DATA_M1, _PCH_TRANSB_DATA_M1)
+#define PCH_TRANS_DATA_N1(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_DATA_N1, _PCH_TRANSB_DATA_N1)
+#define PCH_TRANS_DATA_M2(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_DATA_M2, _PCH_TRANSB_DATA_M2)
+#define PCH_TRANS_DATA_N2(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_DATA_N2, _PCH_TRANSB_DATA_N2)
+#define PCH_TRANS_LINK_M1(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_LINK_M1, _PCH_TRANSB_LINK_M1)
+#define PCH_TRANS_LINK_N1(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_LINK_N1, _PCH_TRANSB_LINK_N1)
+#define PCH_TRANS_LINK_M2(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_LINK_M2, _PCH_TRANSB_LINK_M2)
+#define PCH_TRANS_LINK_N2(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSA_LINK_N2, _PCH_TRANSB_LINK_N2)
 
 #define _PCH_TRANSACONF              0xf0008
 #define _PCH_TRANSBCONF              0xf1008
-#define PCH_TRANSCONF(pipe) _PIPE(pipe, _PCH_TRANSACONF, _PCH_TRANSBCONF)
-#define LPT_TRANSCONF		_PCH_TRANSACONF /* lpt has only one transcoder */
+#define PCH_TRANSCONF(pipe)	_MMIO_PIPE(pipe, _PCH_TRANSACONF, _PCH_TRANSBCONF)
+#define LPT_TRANSCONF		PCH_TRANSCONF(PIPE_A) /* lpt has only one transcoder */
 #define  TRANS_DISABLE          (0<<31)
 #define  TRANS_ENABLE           (1<<31)
 #define  TRANS_STATE_MASK       (1<<30)
@@ -6363,47 +6435,47 @@ enum skl_disp_power_wells {
 
 #define _TRANSA_CHICKEN1	 0xf0060
 #define _TRANSB_CHICKEN1	 0xf1060
-#define TRANS_CHICKEN1(pipe) _PIPE(pipe, _TRANSA_CHICKEN1, _TRANSB_CHICKEN1)
+#define TRANS_CHICKEN1(pipe)	_MMIO_PIPE(pipe, _TRANSA_CHICKEN1, _TRANSB_CHICKEN1)
 #define  TRANS_CHICKEN1_HDMIUNIT_GC_DISABLE	(1<<10)
 #define  TRANS_CHICKEN1_DP0UNIT_GC_DISABLE	(1<<4)
 #define _TRANSA_CHICKEN2	 0xf0064
 #define _TRANSB_CHICKEN2	 0xf1064
-#define TRANS_CHICKEN2(pipe) _PIPE(pipe, _TRANSA_CHICKEN2, _TRANSB_CHICKEN2)
+#define TRANS_CHICKEN2(pipe)	_MMIO_PIPE(pipe, _TRANSA_CHICKEN2, _TRANSB_CHICKEN2)
 #define  TRANS_CHICKEN2_TIMING_OVERRIDE			(1<<31)
 #define  TRANS_CHICKEN2_FDI_POLARITY_REVERSED		(1<<29)
 #define  TRANS_CHICKEN2_FRAME_START_DELAY_MASK		(3<<27)
 #define  TRANS_CHICKEN2_DISABLE_DEEP_COLOR_COUNTER	(1<<26)
 #define  TRANS_CHICKEN2_DISABLE_DEEP_COLOR_MODESWITCH	(1<<25)
 
-#define SOUTH_CHICKEN1		0xc2000
+#define SOUTH_CHICKEN1		_MMIO(0xc2000)
 #define  FDIA_PHASE_SYNC_SHIFT_OVR	19
 #define  FDIA_PHASE_SYNC_SHIFT_EN	18
 #define  FDI_PHASE_SYNC_OVR(pipe) (1<<(FDIA_PHASE_SYNC_SHIFT_OVR - ((pipe) * 2)))
 #define  FDI_PHASE_SYNC_EN(pipe) (1<<(FDIA_PHASE_SYNC_SHIFT_EN - ((pipe) * 2)))
 #define  FDI_BC_BIFURCATION_SELECT	(1 << 12)
 #define  SPT_PWM_GRANULARITY		(1<<0)
-#define SOUTH_CHICKEN2		0xc2004
+#define SOUTH_CHICKEN2		_MMIO(0xc2004)
 #define  FDI_MPHY_IOSFSB_RESET_STATUS	(1<<13)
 #define  FDI_MPHY_IOSFSB_RESET_CTL	(1<<12)
 #define  LPT_PWM_GRANULARITY		(1<<5)
 #define  DPLS_EDP_PPS_FIX_DIS		(1<<0)
 
-#define _FDI_RXA_CHICKEN         0xc200c
-#define _FDI_RXB_CHICKEN         0xc2010
+#define _FDI_RXA_CHICKEN        0xc200c
+#define _FDI_RXB_CHICKEN        0xc2010
 #define  FDI_RX_PHASE_SYNC_POINTER_OVR	(1<<1)
 #define  FDI_RX_PHASE_SYNC_POINTER_EN	(1<<0)
-#define FDI_RX_CHICKEN(pipe) _PIPE(pipe, _FDI_RXA_CHICKEN, _FDI_RXB_CHICKEN)
+#define FDI_RX_CHICKEN(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_CHICKEN, _FDI_RXB_CHICKEN)
 
-#define SOUTH_DSPCLK_GATE_D	0xc2020
+#define SOUTH_DSPCLK_GATE_D	_MMIO(0xc2020)
 #define  PCH_DPLUNIT_CLOCK_GATE_DISABLE (1<<30)
 #define  PCH_DPLSUNIT_CLOCK_GATE_DISABLE (1<<29)
 #define  PCH_CPUNIT_CLOCK_GATE_DISABLE (1<<14)
 #define  PCH_LP_PARTITION_LEVEL_DISABLE  (1<<12)
 
 /* CPU: FDI_TX */
-#define _FDI_TXA_CTL             0x60100
-#define _FDI_TXB_CTL             0x61100
-#define FDI_TX_CTL(pipe) _PIPE(pipe, _FDI_TXA_CTL, _FDI_TXB_CTL)
+#define _FDI_TXA_CTL            0x60100
+#define _FDI_TXB_CTL            0x61100
+#define FDI_TX_CTL(pipe)	_MMIO_PIPE(pipe, _FDI_TXA_CTL, _FDI_TXB_CTL)
 #define  FDI_TX_DISABLE         (0<<31)
 #define  FDI_TX_ENABLE          (1<<31)
 #define  FDI_LINK_TRAIN_PATTERN_1       (0<<28)
@@ -6453,7 +6525,7 @@ enum skl_disp_power_wells {
 /* FDI_RX, FDI_X is hard-wired to Transcoder_X */
 #define _FDI_RXA_CTL             0xf000c
 #define _FDI_RXB_CTL             0xf100c
-#define FDI_RX_CTL(pipe) _PIPE(pipe, _FDI_RXA_CTL, _FDI_RXB_CTL)
+#define FDI_RX_CTL(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_CTL, _FDI_RXB_CTL)
 #define  FDI_RX_ENABLE          (1<<31)
 /* train, dp width same as FDI_TX */
 #define  FDI_FS_ERRC_ENABLE		(1<<27)
@@ -6489,14 +6561,14 @@ enum skl_disp_power_wells {
 #define  FDI_RX_TP1_TO_TP2_48		(2<<20)
 #define  FDI_RX_TP1_TO_TP2_64		(3<<20)
 #define  FDI_RX_FDI_DELAY_90		(0x90<<0)
-#define FDI_RX_MISC(pipe) _PIPE(pipe, _FDI_RXA_MISC, _FDI_RXB_MISC)
+#define FDI_RX_MISC(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_MISC, _FDI_RXB_MISC)
 
-#define _FDI_RXA_TUSIZE1         0xf0030
-#define _FDI_RXA_TUSIZE2         0xf0038
-#define _FDI_RXB_TUSIZE1         0xf1030
-#define _FDI_RXB_TUSIZE2         0xf1038
-#define FDI_RX_TUSIZE1(pipe) _PIPE(pipe, _FDI_RXA_TUSIZE1, _FDI_RXB_TUSIZE1)
-#define FDI_RX_TUSIZE2(pipe) _PIPE(pipe, _FDI_RXA_TUSIZE2, _FDI_RXB_TUSIZE2)
+#define _FDI_RXA_TUSIZE1        0xf0030
+#define _FDI_RXA_TUSIZE2        0xf0038
+#define _FDI_RXB_TUSIZE1        0xf1030
+#define _FDI_RXB_TUSIZE2        0xf1038
+#define FDI_RX_TUSIZE1(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_TUSIZE1, _FDI_RXB_TUSIZE1)
+#define FDI_RX_TUSIZE2(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_TUSIZE2, _FDI_RXB_TUSIZE2)
 
 /* FDI_RX interrupt register format */
 #define FDI_RX_INTER_LANE_ALIGN         (1<<10)
@@ -6511,44 +6583,41 @@ enum skl_disp_power_wells {
 #define FDI_RX_CROSS_CLOCK_OVERFLOW     (1<<1)
 #define FDI_RX_SYMBOL_QUEUE_OVERFLOW    (1<<0)
 
-#define _FDI_RXA_IIR             0xf0014
-#define _FDI_RXA_IMR             0xf0018
-#define _FDI_RXB_IIR             0xf1014
-#define _FDI_RXB_IMR             0xf1018
-#define FDI_RX_IIR(pipe) _PIPE(pipe, _FDI_RXA_IIR, _FDI_RXB_IIR)
-#define FDI_RX_IMR(pipe) _PIPE(pipe, _FDI_RXA_IMR, _FDI_RXB_IMR)
+#define _FDI_RXA_IIR            0xf0014
+#define _FDI_RXA_IMR            0xf0018
+#define _FDI_RXB_IIR            0xf1014
+#define _FDI_RXB_IMR            0xf1018
+#define FDI_RX_IIR(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_IIR, _FDI_RXB_IIR)
+#define FDI_RX_IMR(pipe)	_MMIO_PIPE(pipe, _FDI_RXA_IMR, _FDI_RXB_IMR)
 
-#define FDI_PLL_CTL_1           0xfe000
-#define FDI_PLL_CTL_2           0xfe004
+#define FDI_PLL_CTL_1           _MMIO(0xfe000)
+#define FDI_PLL_CTL_2           _MMIO(0xfe004)
 
-#define PCH_LVDS	0xe1180
+#define PCH_LVDS	_MMIO(0xe1180)
 #define  LVDS_DETECTED	(1 << 1)
 
 /* vlv has 2 sets of panel control regs. */
-#define PIPEA_PP_STATUS         (VLV_DISPLAY_BASE + 0x61200)
-#define PIPEA_PP_CONTROL        (VLV_DISPLAY_BASE + 0x61204)
-#define PIPEA_PP_ON_DELAYS      (VLV_DISPLAY_BASE + 0x61208)
+#define _PIPEA_PP_STATUS         (VLV_DISPLAY_BASE + 0x61200)
+#define _PIPEA_PP_CONTROL        (VLV_DISPLAY_BASE + 0x61204)
+#define _PIPEA_PP_ON_DELAYS      (VLV_DISPLAY_BASE + 0x61208)
 #define  PANEL_PORT_SELECT_VLV(port)	((port) << 30)
-#define PIPEA_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6120c)
-#define PIPEA_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61210)
-
-#define PIPEB_PP_STATUS         (VLV_DISPLAY_BASE + 0x61300)
-#define PIPEB_PP_CONTROL        (VLV_DISPLAY_BASE + 0x61304)
-#define PIPEB_PP_ON_DELAYS      (VLV_DISPLAY_BASE + 0x61308)
-#define PIPEB_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6130c)
-#define PIPEB_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61310)
-
-#define VLV_PIPE_PP_STATUS(pipe) _PIPE(pipe, PIPEA_PP_STATUS, PIPEB_PP_STATUS)
-#define VLV_PIPE_PP_CONTROL(pipe) _PIPE(pipe, PIPEA_PP_CONTROL, PIPEB_PP_CONTROL)
-#define VLV_PIPE_PP_ON_DELAYS(pipe) \
-		_PIPE(pipe, PIPEA_PP_ON_DELAYS, PIPEB_PP_ON_DELAYS)
-#define VLV_PIPE_PP_OFF_DELAYS(pipe) \
-		_PIPE(pipe, PIPEA_PP_OFF_DELAYS, PIPEB_PP_OFF_DELAYS)
-#define VLV_PIPE_PP_DIVISOR(pipe) \
-		_PIPE(pipe, PIPEA_PP_DIVISOR, PIPEB_PP_DIVISOR)
-
-#define PCH_PP_STATUS		0xc7200
-#define PCH_PP_CONTROL		0xc7204
+#define _PIPEA_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6120c)
+#define _PIPEA_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61210)
+
+#define _PIPEB_PP_STATUS         (VLV_DISPLAY_BASE + 0x61300)
+#define _PIPEB_PP_CONTROL        (VLV_DISPLAY_BASE + 0x61304)
+#define _PIPEB_PP_ON_DELAYS      (VLV_DISPLAY_BASE + 0x61308)
+#define _PIPEB_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6130c)
+#define _PIPEB_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61310)
+
+#define VLV_PIPE_PP_STATUS(pipe)	_MMIO_PIPE(pipe, _PIPEA_PP_STATUS, _PIPEB_PP_STATUS)
+#define VLV_PIPE_PP_CONTROL(pipe)	_MMIO_PIPE(pipe, _PIPEA_PP_CONTROL, _PIPEB_PP_CONTROL)
+#define VLV_PIPE_PP_ON_DELAYS(pipe)	_MMIO_PIPE(pipe, _PIPEA_PP_ON_DELAYS, _PIPEB_PP_ON_DELAYS)
+#define VLV_PIPE_PP_OFF_DELAYS(pipe)	_MMIO_PIPE(pipe, _PIPEA_PP_OFF_DELAYS, _PIPEB_PP_OFF_DELAYS)
+#define VLV_PIPE_PP_DIVISOR(pipe)	_MMIO_PIPE(pipe, _PIPEA_PP_DIVISOR, _PIPEB_PP_DIVISOR)
+
+#define _PCH_PP_STATUS		0xc7200
+#define _PCH_PP_CONTROL		0xc7204
 #define  PANEL_UNLOCK_REGS	(0xabcd << 16)
 #define  PANEL_UNLOCK_MASK	(0xffff << 16)
 #define  BXT_POWER_CYCLE_DELAY_MASK	(0x1f0)
@@ -6558,7 +6627,7 @@ enum skl_disp_power_wells {
 #define  PANEL_POWER_RESET	(1 << 1)
 #define  PANEL_POWER_OFF	(0 << 0)
 #define  PANEL_POWER_ON		(1 << 0)
-#define PCH_PP_ON_DELAYS	0xc7208
+#define _PCH_PP_ON_DELAYS	0xc7208
 #define  PANEL_PORT_SELECT_MASK	(3 << 30)
 #define  PANEL_PORT_SELECT_LVDS	(0 << 30)
 #define  PANEL_PORT_SELECT_DPA	(1 << 30)
@@ -6569,52 +6638,64 @@ enum skl_disp_power_wells {
 #define  PANEL_LIGHT_ON_DELAY_MASK	(0x1fff)
 #define  PANEL_LIGHT_ON_DELAY_SHIFT	0
 
-#define PCH_PP_OFF_DELAYS	0xc720c
+#define _PCH_PP_OFF_DELAYS		0xc720c
 #define  PANEL_POWER_DOWN_DELAY_MASK	(0x1fff0000)
 #define  PANEL_POWER_DOWN_DELAY_SHIFT	16
 #define  PANEL_LIGHT_OFF_DELAY_MASK	(0x1fff)
 #define  PANEL_LIGHT_OFF_DELAY_SHIFT	0
 
-#define PCH_PP_DIVISOR		0xc7210
+#define _PCH_PP_DIVISOR			0xc7210
 #define  PP_REFERENCE_DIVIDER_MASK	(0xffffff00)
 #define  PP_REFERENCE_DIVIDER_SHIFT	8
 #define  PANEL_POWER_CYCLE_DELAY_MASK	(0x1f)
 #define  PANEL_POWER_CYCLE_DELAY_SHIFT	0
 
+#define PCH_PP_STATUS			_MMIO(_PCH_PP_STATUS)
+#define PCH_PP_CONTROL			_MMIO(_PCH_PP_CONTROL)
+#define PCH_PP_ON_DELAYS		_MMIO(_PCH_PP_ON_DELAYS)
+#define PCH_PP_OFF_DELAYS		_MMIO(_PCH_PP_OFF_DELAYS)
+#define PCH_PP_DIVISOR			_MMIO(_PCH_PP_DIVISOR)
+
 /* BXT PPS changes - 2nd set of PPS registers */
 #define _BXT_PP_STATUS2 	0xc7300
 #define _BXT_PP_CONTROL2 	0xc7304
 #define _BXT_PP_ON_DELAYS2	0xc7308
 #define _BXT_PP_OFF_DELAYS2	0xc730c
 
-#define BXT_PP_STATUS(n)	_PIPE(n, PCH_PP_STATUS, _BXT_PP_STATUS2)
-#define BXT_PP_CONTROL(n)	_PIPE(n, PCH_PP_CONTROL, _BXT_PP_CONTROL2)
-#define BXT_PP_ON_DELAYS(n)	_PIPE(n, PCH_PP_ON_DELAYS, _BXT_PP_ON_DELAYS2)
-#define BXT_PP_OFF_DELAYS(n)	_PIPE(n, PCH_PP_OFF_DELAYS, _BXT_PP_OFF_DELAYS2)
-
-#define PCH_DP_B		0xe4100
-#define PCH_DPB_AUX_CH_CTL	0xe4110
-#define PCH_DPB_AUX_CH_DATA1	0xe4114
-#define PCH_DPB_AUX_CH_DATA2	0xe4118
-#define PCH_DPB_AUX_CH_DATA3	0xe411c
-#define PCH_DPB_AUX_CH_DATA4	0xe4120
-#define PCH_DPB_AUX_CH_DATA5	0xe4124
-
-#define PCH_DP_C		0xe4200
-#define PCH_DPC_AUX_CH_CTL	0xe4210
-#define PCH_DPC_AUX_CH_DATA1	0xe4214
-#define PCH_DPC_AUX_CH_DATA2	0xe4218
-#define PCH_DPC_AUX_CH_DATA3	0xe421c
-#define PCH_DPC_AUX_CH_DATA4	0xe4220
-#define PCH_DPC_AUX_CH_DATA5	0xe4224
-
-#define PCH_DP_D		0xe4300
-#define PCH_DPD_AUX_CH_CTL	0xe4310
-#define PCH_DPD_AUX_CH_DATA1	0xe4314
-#define PCH_DPD_AUX_CH_DATA2	0xe4318
-#define PCH_DPD_AUX_CH_DATA3	0xe431c
-#define PCH_DPD_AUX_CH_DATA4	0xe4320
-#define PCH_DPD_AUX_CH_DATA5	0xe4324
+#define BXT_PP_STATUS(n)	_MMIO_PIPE(n, _PCH_PP_STATUS, _BXT_PP_STATUS2)
+#define BXT_PP_CONTROL(n)	_MMIO_PIPE(n, _PCH_PP_CONTROL, _BXT_PP_CONTROL2)
+#define BXT_PP_ON_DELAYS(n)	_MMIO_PIPE(n, _PCH_PP_ON_DELAYS, _BXT_PP_ON_DELAYS2)
+#define BXT_PP_OFF_DELAYS(n)	_MMIO_PIPE(n, _PCH_PP_OFF_DELAYS, _BXT_PP_OFF_DELAYS2)
+
+#define _PCH_DP_B		0xe4100
+#define PCH_DP_B		_MMIO(_PCH_DP_B)
+#define _PCH_DPB_AUX_CH_CTL	0xe4110
+#define _PCH_DPB_AUX_CH_DATA1	0xe4114
+#define _PCH_DPB_AUX_CH_DATA2	0xe4118
+#define _PCH_DPB_AUX_CH_DATA3	0xe411c
+#define _PCH_DPB_AUX_CH_DATA4	0xe4120
+#define _PCH_DPB_AUX_CH_DATA5	0xe4124
+
+#define _PCH_DP_C		0xe4200
+#define PCH_DP_C		_MMIO(_PCH_DP_C)
+#define _PCH_DPC_AUX_CH_CTL	0xe4210
+#define _PCH_DPC_AUX_CH_DATA1	0xe4214
+#define _PCH_DPC_AUX_CH_DATA2	0xe4218
+#define _PCH_DPC_AUX_CH_DATA3	0xe421c
+#define _PCH_DPC_AUX_CH_DATA4	0xe4220
+#define _PCH_DPC_AUX_CH_DATA5	0xe4224
+
+#define _PCH_DP_D		0xe4300
+#define PCH_DP_D		_MMIO(_PCH_DP_D)
+#define _PCH_DPD_AUX_CH_CTL	0xe4310
+#define _PCH_DPD_AUX_CH_DATA1	0xe4314
+#define _PCH_DPD_AUX_CH_DATA2	0xe4318
+#define _PCH_DPD_AUX_CH_DATA3	0xe431c
+#define _PCH_DPD_AUX_CH_DATA4	0xe4320
+#define _PCH_DPD_AUX_CH_DATA5	0xe4324
+
+#define PCH_DP_AUX_CH_CTL(port)		_MMIO_PORT((port) - PORT_B, _PCH_DPB_AUX_CH_CTL, _PCH_DPC_AUX_CH_CTL)
+#define PCH_DP_AUX_CH_DATA(port, i)	_MMIO(_PORT((port) - PORT_B, _PCH_DPB_AUX_CH_DATA1, _PCH_DPC_AUX_CH_DATA1) + (i) * 4) /* 5 registers */
 
 /* CPT */
 #define  PORT_TRANS_A_SEL_CPT	0
@@ -6627,10 +6708,10 @@ enum skl_disp_power_wells {
 #define  SDVO_PORT_TO_PIPE_CHV(val)	(((val) & (3<<24)) >> 24)
 #define  DP_PORT_TO_PIPE_CHV(val)	(((val) & (3<<16)) >> 16)
 
-#define TRANS_DP_CTL_A		0xe0300
-#define TRANS_DP_CTL_B		0xe1300
-#define TRANS_DP_CTL_C		0xe2300
-#define TRANS_DP_CTL(pipe)	_PIPE(pipe, TRANS_DP_CTL_A, TRANS_DP_CTL_B)
+#define _TRANS_DP_CTL_A		0xe0300
+#define _TRANS_DP_CTL_B		0xe1300
+#define _TRANS_DP_CTL_C		0xe2300
+#define TRANS_DP_CTL(pipe)	_MMIO_PIPE(pipe, _TRANS_DP_CTL_A, _TRANS_DP_CTL_B)
 #define  TRANS_DP_OUTPUT_ENABLE	(1<<31)
 #define  TRANS_DP_PORT_SEL_B	(0<<29)
 #define  TRANS_DP_PORT_SEL_C	(1<<29)
@@ -6683,40 +6764,40 @@ enum skl_disp_power_wells {
 
 #define  EDP_LINK_TRAIN_VOL_EMP_MASK_IVB	(0x3f<<22)
 
-#define  VLV_PMWGICZ				0x1300a4
+#define  VLV_PMWGICZ				_MMIO(0x1300a4)
 
-#define  FORCEWAKE				0xA18C
-#define  FORCEWAKE_VLV				0x1300b0
-#define  FORCEWAKE_ACK_VLV			0x1300b4
-#define  FORCEWAKE_MEDIA_VLV			0x1300b8
-#define  FORCEWAKE_ACK_MEDIA_VLV		0x1300bc
-#define  FORCEWAKE_ACK_HSW			0x130044
-#define  FORCEWAKE_ACK				0x130090
-#define  VLV_GTLC_WAKE_CTRL			0x130090
+#define  FORCEWAKE				_MMIO(0xA18C)
+#define  FORCEWAKE_VLV				_MMIO(0x1300b0)
+#define  FORCEWAKE_ACK_VLV			_MMIO(0x1300b4)
+#define  FORCEWAKE_MEDIA_VLV			_MMIO(0x1300b8)
+#define  FORCEWAKE_ACK_MEDIA_VLV		_MMIO(0x1300bc)
+#define  FORCEWAKE_ACK_HSW			_MMIO(0x130044)
+#define  FORCEWAKE_ACK				_MMIO(0x130090)
+#define  VLV_GTLC_WAKE_CTRL			_MMIO(0x130090)
 #define   VLV_GTLC_RENDER_CTX_EXISTS		(1 << 25)
 #define   VLV_GTLC_MEDIA_CTX_EXISTS		(1 << 24)
 #define   VLV_GTLC_ALLOWWAKEREQ			(1 << 0)
 
-#define  VLV_GTLC_PW_STATUS			0x130094
+#define  VLV_GTLC_PW_STATUS			_MMIO(0x130094)
 #define   VLV_GTLC_ALLOWWAKEACK			(1 << 0)
 #define   VLV_GTLC_ALLOWWAKEERR			(1 << 1)
 #define   VLV_GTLC_PW_MEDIA_STATUS_MASK		(1 << 5)
 #define   VLV_GTLC_PW_RENDER_STATUS_MASK	(1 << 7)
-#define  FORCEWAKE_MT				0xa188 /* multi-threaded */
-#define  FORCEWAKE_MEDIA_GEN9			0xa270
-#define  FORCEWAKE_RENDER_GEN9			0xa278
-#define  FORCEWAKE_BLITTER_GEN9			0xa188
-#define  FORCEWAKE_ACK_MEDIA_GEN9		0x0D88
-#define  FORCEWAKE_ACK_RENDER_GEN9		0x0D84
-#define  FORCEWAKE_ACK_BLITTER_GEN9		0x130044
+#define  FORCEWAKE_MT				_MMIO(0xa188) /* multi-threaded */
+#define  FORCEWAKE_MEDIA_GEN9			_MMIO(0xa270)
+#define  FORCEWAKE_RENDER_GEN9			_MMIO(0xa278)
+#define  FORCEWAKE_BLITTER_GEN9			_MMIO(0xa188)
+#define  FORCEWAKE_ACK_MEDIA_GEN9		_MMIO(0x0D88)
+#define  FORCEWAKE_ACK_RENDER_GEN9		_MMIO(0x0D84)
+#define  FORCEWAKE_ACK_BLITTER_GEN9		_MMIO(0x130044)
 #define   FORCEWAKE_KERNEL			0x1
 #define   FORCEWAKE_USER			0x2
-#define  FORCEWAKE_MT_ACK			0x130040
-#define  ECOBUS					0xa180
+#define  FORCEWAKE_MT_ACK			_MMIO(0x130040)
+#define  ECOBUS					_MMIO(0xa180)
 #define    FORCEWAKE_MT_ENABLE			(1<<5)
-#define  VLV_SPAREG2H				0xA194
+#define  VLV_SPAREG2H				_MMIO(0xA194)
 
-#define  GTFIFODBG				0x120000
+#define  GTFIFODBG				_MMIO(0x120000)
 #define    GT_FIFO_SBDROPERR			(1<<6)
 #define    GT_FIFO_BLOBDROPERR			(1<<5)
 #define    GT_FIFO_SB_READ_ABORTERR		(1<<4)
@@ -6725,23 +6806,23 @@ enum skl_disp_power_wells {
 #define    GT_FIFO_IAWRERR			(1<<1)
 #define    GT_FIFO_IARDERR			(1<<0)
 
-#define  GTFIFOCTL				0x120008
+#define  GTFIFOCTL				_MMIO(0x120008)
 #define    GT_FIFO_FREE_ENTRIES_MASK		0x7f
 #define    GT_FIFO_NUM_RESERVED_ENTRIES		20
 #define    GT_FIFO_CTL_BLOCK_ALL_POLICY_STALL	(1 << 12)
 #define    GT_FIFO_CTL_RC6_POLICY_STALL		(1 << 11)
 
-#define  HSW_IDICR				0x9008
+#define  HSW_IDICR				_MMIO(0x9008)
 #define    IDIHASHMSK(x)			(((x) & 0x3f) << 16)
-#define  HSW_EDRAM_PRESENT			0x120010
+#define  HSW_EDRAM_PRESENT			_MMIO(0x120010)
 #define    EDRAM_ENABLED			0x1
 
-#define GEN6_UCGCTL1				0x9400
+#define GEN6_UCGCTL1				_MMIO(0x9400)
 # define GEN6_EU_TCUNIT_CLOCK_GATE_DISABLE		(1 << 16)
 # define GEN6_BLBUNIT_CLOCK_GATE_DISABLE		(1 << 5)
 # define GEN6_CSUNIT_CLOCK_GATE_DISABLE			(1 << 7)
 
-#define GEN6_UCGCTL2				0x9404
+#define GEN6_UCGCTL2				_MMIO(0x9404)
 # define GEN6_VFUNIT_CLOCK_GATE_DISABLE			(1 << 31)
 # define GEN7_VDSUNIT_CLOCK_GATE_DISABLE		(1 << 30)
 # define GEN7_TDLUNIT_CLOCK_GATE_DISABLE		(1 << 22)
@@ -6749,30 +6830,30 @@ enum skl_disp_power_wells {
 # define GEN6_RCPBUNIT_CLOCK_GATE_DISABLE		(1 << 12)
 # define GEN6_RCCUNIT_CLOCK_GATE_DISABLE		(1 << 11)
 
-#define GEN6_UCGCTL3				0x9408
+#define GEN6_UCGCTL3				_MMIO(0x9408)
 
-#define GEN7_UCGCTL4				0x940c
+#define GEN7_UCGCTL4				_MMIO(0x940c)
 #define  GEN7_L3BANK2X_CLOCK_GATE_DISABLE	(1<<25)
 
-#define GEN6_RCGCTL1				0x9410
-#define GEN6_RCGCTL2				0x9414
-#define GEN6_RSTCTL				0x9420
+#define GEN6_RCGCTL1				_MMIO(0x9410)
+#define GEN6_RCGCTL2				_MMIO(0x9414)
+#define GEN6_RSTCTL				_MMIO(0x9420)
 
-#define GEN8_UCGCTL6				0x9430
+#define GEN8_UCGCTL6				_MMIO(0x9430)
 #define   GEN8_GAPSUNIT_CLOCK_GATE_DISABLE	(1<<24)
 #define   GEN8_SDEUNIT_CLOCK_GATE_DISABLE	(1<<14)
 #define   GEN8_HDCUNIT_CLOCK_GATE_DISABLE_HDCREQ (1<<28)
 
-#define GEN6_GFXPAUSE				0xA000
-#define GEN6_RPNSWREQ				0xA008
+#define GEN6_GFXPAUSE				_MMIO(0xA000)
+#define GEN6_RPNSWREQ				_MMIO(0xA008)
 #define   GEN6_TURBO_DISABLE			(1<<31)
 #define   GEN6_FREQUENCY(x)			((x)<<25)
 #define   HSW_FREQUENCY(x)			((x)<<24)
 #define   GEN9_FREQUENCY(x)			((x)<<23)
 #define   GEN6_OFFSET(x)			((x)<<19)
 #define   GEN6_AGGRESSIVE_TURBO			(0<<15)
-#define GEN6_RC_VIDEO_FREQ			0xA00C
-#define GEN6_RC_CONTROL				0xA090
+#define GEN6_RC_VIDEO_FREQ			_MMIO(0xA00C)
+#define GEN6_RC_CONTROL				_MMIO(0xA090)
 #define   GEN6_RC_CTL_RC6pp_ENABLE		(1<<16)
 #define   GEN6_RC_CTL_RC6p_ENABLE		(1<<17)
 #define   GEN6_RC_CTL_RC6_ENABLE		(1<<18)
@@ -6782,16 +6863,16 @@ enum skl_disp_power_wells {
 #define   GEN7_RC_CTL_TO_MODE			(1<<28)
 #define   GEN6_RC_CTL_EI_MODE(x)		((x)<<27)
 #define   GEN6_RC_CTL_HW_ENABLE			(1<<31)
-#define GEN6_RP_DOWN_TIMEOUT			0xA010
-#define GEN6_RP_INTERRUPT_LIMITS		0xA014
-#define GEN6_RPSTAT1				0xA01C
+#define GEN6_RP_DOWN_TIMEOUT			_MMIO(0xA010)
+#define GEN6_RP_INTERRUPT_LIMITS		_MMIO(0xA014)
+#define GEN6_RPSTAT1				_MMIO(0xA01C)
 #define   GEN6_CAGF_SHIFT			8
 #define   HSW_CAGF_SHIFT			7
 #define   GEN9_CAGF_SHIFT			23
 #define   GEN6_CAGF_MASK			(0x7f << GEN6_CAGF_SHIFT)
 #define   HSW_CAGF_MASK				(0x7f << HSW_CAGF_SHIFT)
 #define   GEN9_CAGF_MASK			(0x1ff << GEN9_CAGF_SHIFT)
-#define GEN6_RP_CONTROL				0xA024
+#define GEN6_RP_CONTROL				_MMIO(0xA024)
 #define   GEN6_RP_MEDIA_TURBO			(1<<11)
 #define   GEN6_RP_MEDIA_MODE_MASK		(3<<9)
 #define   GEN6_RP_MEDIA_HW_TURBO_MODE		(3<<9)
@@ -6805,53 +6886,53 @@ enum skl_disp_power_wells {
 #define   GEN6_RP_UP_BUSY_CONT			(0x4<<3)
 #define   GEN6_RP_DOWN_IDLE_AVG			(0x2<<0)
 #define   GEN6_RP_DOWN_IDLE_CONT		(0x1<<0)
-#define GEN6_RP_UP_THRESHOLD			0xA02C
-#define GEN6_RP_DOWN_THRESHOLD			0xA030
-#define GEN6_RP_CUR_UP_EI			0xA050
+#define GEN6_RP_UP_THRESHOLD			_MMIO(0xA02C)
+#define GEN6_RP_DOWN_THRESHOLD			_MMIO(0xA030)
+#define GEN6_RP_CUR_UP_EI			_MMIO(0xA050)
 #define   GEN6_CURICONT_MASK			0xffffff
-#define GEN6_RP_CUR_UP				0xA054
+#define GEN6_RP_CUR_UP				_MMIO(0xA054)
 #define   GEN6_CURBSYTAVG_MASK			0xffffff
-#define GEN6_RP_PREV_UP				0xA058
-#define GEN6_RP_CUR_DOWN_EI			0xA05C
+#define GEN6_RP_PREV_UP				_MMIO(0xA058)
+#define GEN6_RP_CUR_DOWN_EI			_MMIO(0xA05C)
 #define   GEN6_CURIAVG_MASK			0xffffff
-#define GEN6_RP_CUR_DOWN			0xA060
-#define GEN6_RP_PREV_DOWN			0xA064
-#define GEN6_RP_UP_EI				0xA068
-#define GEN6_RP_DOWN_EI				0xA06C
-#define GEN6_RP_IDLE_HYSTERSIS			0xA070
-#define GEN6_RPDEUHWTC				0xA080
-#define GEN6_RPDEUC				0xA084
-#define GEN6_RPDEUCSW				0xA088
-#define GEN6_RC_STATE				0xA094
-#define GEN6_RC1_WAKE_RATE_LIMIT		0xA098
-#define GEN6_RC6_WAKE_RATE_LIMIT		0xA09C
-#define GEN6_RC6pp_WAKE_RATE_LIMIT		0xA0A0
-#define GEN6_RC_EVALUATION_INTERVAL		0xA0A8
-#define GEN6_RC_IDLE_HYSTERSIS			0xA0AC
-#define GEN6_RC_SLEEP				0xA0B0
-#define GEN6_RCUBMABDTMR			0xA0B0
-#define GEN6_RC1e_THRESHOLD			0xA0B4
-#define GEN6_RC6_THRESHOLD			0xA0B8
-#define GEN6_RC6p_THRESHOLD			0xA0BC
-#define VLV_RCEDATA				0xA0BC
-#define GEN6_RC6pp_THRESHOLD			0xA0C0
-#define GEN6_PMINTRMSK				0xA168
+#define GEN6_RP_CUR_DOWN			_MMIO(0xA060)
+#define GEN6_RP_PREV_DOWN			_MMIO(0xA064)
+#define GEN6_RP_UP_EI				_MMIO(0xA068)
+#define GEN6_RP_DOWN_EI				_MMIO(0xA06C)
+#define GEN6_RP_IDLE_HYSTERSIS			_MMIO(0xA070)
+#define GEN6_RPDEUHWTC				_MMIO(0xA080)
+#define GEN6_RPDEUC				_MMIO(0xA084)
+#define GEN6_RPDEUCSW				_MMIO(0xA088)
+#define GEN6_RC_STATE				_MMIO(0xA094)
+#define GEN6_RC1_WAKE_RATE_LIMIT		_MMIO(0xA098)
+#define GEN6_RC6_WAKE_RATE_LIMIT		_MMIO(0xA09C)
+#define GEN6_RC6pp_WAKE_RATE_LIMIT		_MMIO(0xA0A0)
+#define GEN6_RC_EVALUATION_INTERVAL		_MMIO(0xA0A8)
+#define GEN6_RC_IDLE_HYSTERSIS			_MMIO(0xA0AC)
+#define GEN6_RC_SLEEP				_MMIO(0xA0B0)
+#define GEN6_RCUBMABDTMR			_MMIO(0xA0B0)
+#define GEN6_RC1e_THRESHOLD			_MMIO(0xA0B4)
+#define GEN6_RC6_THRESHOLD			_MMIO(0xA0B8)
+#define GEN6_RC6p_THRESHOLD			_MMIO(0xA0BC)
+#define VLV_RCEDATA				_MMIO(0xA0BC)
+#define GEN6_RC6pp_THRESHOLD			_MMIO(0xA0C0)
+#define GEN6_PMINTRMSK				_MMIO(0xA168)
 #define GEN8_PMINTR_REDIRECT_TO_NON_DISP	(1<<31)
-#define VLV_PWRDWNUPCTL				0xA294
-#define GEN9_MEDIA_PG_IDLE_HYSTERESIS		0xA0C4
-#define GEN9_RENDER_PG_IDLE_HYSTERESIS		0xA0C8
-#define GEN9_PG_ENABLE				0xA210
+#define VLV_PWRDWNUPCTL				_MMIO(0xA294)
+#define GEN9_MEDIA_PG_IDLE_HYSTERESIS		_MMIO(0xA0C4)
+#define GEN9_RENDER_PG_IDLE_HYSTERESIS		_MMIO(0xA0C8)
+#define GEN9_PG_ENABLE				_MMIO(0xA210)
 #define GEN9_RENDER_PG_ENABLE			(1<<0)
 #define GEN9_MEDIA_PG_ENABLE			(1<<1)
 
-#define VLV_CHICKEN_3				(VLV_DISPLAY_BASE + 0x7040C)
+#define VLV_CHICKEN_3				_MMIO(VLV_DISPLAY_BASE + 0x7040C)
 #define  PIXEL_OVERLAP_CNT_MASK			(3 << 30)
 #define  PIXEL_OVERLAP_CNT_SHIFT		30
 
-#define GEN6_PMISR				0x44020
-#define GEN6_PMIMR				0x44024 /* rps_lock */
-#define GEN6_PMIIR				0x44028
-#define GEN6_PMIER				0x4402C
+#define GEN6_PMISR				_MMIO(0x44020)
+#define GEN6_PMIMR				_MMIO(0x44024) /* rps_lock */
+#define GEN6_PMIIR				_MMIO(0x44028)
+#define GEN6_PMIER				_MMIO(0x4402C)
 #define  GEN6_PM_MBOX_EVENT			(1<<25)
 #define  GEN6_PM_THERMAL_EVENT			(1<<24)
 #define  GEN6_PM_RP_DOWN_TIMEOUT		(1<<6)
@@ -6863,30 +6944,30 @@ enum skl_disp_power_wells {
 						 GEN6_PM_RP_DOWN_THRESHOLD | \
 						 GEN6_PM_RP_DOWN_TIMEOUT)
 
-#define GEN7_GT_SCRATCH(i)			(0x4F100 + (i) * 4)
+#define GEN7_GT_SCRATCH(i)			_MMIO(0x4F100 + (i) * 4)
 #define GEN7_GT_SCRATCH_REG_NUM			8
 
-#define VLV_GTLC_SURVIVABILITY_REG              0x130098
+#define VLV_GTLC_SURVIVABILITY_REG              _MMIO(0x130098)
 #define VLV_GFX_CLK_STATUS_BIT			(1<<3)
 #define VLV_GFX_CLK_FORCE_ON_BIT		(1<<2)
 
-#define GEN6_GT_GFX_RC6_LOCKED			0x138104
-#define VLV_COUNTER_CONTROL			0x138104
+#define GEN6_GT_GFX_RC6_LOCKED			_MMIO(0x138104)
+#define VLV_COUNTER_CONTROL			_MMIO(0x138104)
 #define   VLV_COUNT_RANGE_HIGH			(1<<15)
 #define   VLV_MEDIA_RC0_COUNT_EN		(1<<5)
 #define   VLV_RENDER_RC0_COUNT_EN		(1<<4)
 #define   VLV_MEDIA_RC6_COUNT_EN		(1<<1)
 #define   VLV_RENDER_RC6_COUNT_EN		(1<<0)
-#define GEN6_GT_GFX_RC6				0x138108
-#define VLV_GT_RENDER_RC6			0x138108
-#define VLV_GT_MEDIA_RC6			0x13810C
+#define GEN6_GT_GFX_RC6				_MMIO(0x138108)
+#define VLV_GT_RENDER_RC6			_MMIO(0x138108)
+#define VLV_GT_MEDIA_RC6			_MMIO(0x13810C)
 
-#define GEN6_GT_GFX_RC6p			0x13810C
-#define GEN6_GT_GFX_RC6pp			0x138110
-#define VLV_RENDER_C0_COUNT			0x138118
-#define VLV_MEDIA_C0_COUNT			0x13811C
+#define GEN6_GT_GFX_RC6p			_MMIO(0x13810C)
+#define GEN6_GT_GFX_RC6pp			_MMIO(0x138110)
+#define VLV_RENDER_C0_COUNT			_MMIO(0x138118)
+#define VLV_MEDIA_C0_COUNT			_MMIO(0x13811C)
 
-#define GEN6_PCODE_MAILBOX			0x138124
+#define GEN6_PCODE_MAILBOX			_MMIO(0x138124)
 #define   GEN6_PCODE_READY			(1<<31)
 #define	  GEN6_PCODE_WRITE_RC6VIDS		0x4
 #define	  GEN6_PCODE_READ_RC6VIDS		0x5
@@ -6909,12 +6990,12 @@ enum skl_disp_power_wells {
 #define   HSW_PCODE_DE_WRITE_FREQ_REQ		0x17
 #define   DISPLAY_IPS_CONTROL			0x19
 #define	  HSW_PCODE_DYNAMIC_DUTY_CYCLE_CONTROL	0x1A
-#define GEN6_PCODE_DATA				0x138128
+#define GEN6_PCODE_DATA				_MMIO(0x138128)
 #define   GEN6_PCODE_FREQ_IA_RATIO_SHIFT	8
 #define   GEN6_PCODE_FREQ_RING_RATIO_SHIFT	16
-#define GEN6_PCODE_DATA1			0x13812C
+#define GEN6_PCODE_DATA1			_MMIO(0x13812C)
 
-#define GEN6_GT_CORE_STATUS		0x138060
+#define GEN6_GT_CORE_STATUS		_MMIO(0x138060)
 #define   GEN6_CORE_CPD_STATE_MASK	(7<<4)
 #define   GEN6_RCn_MASK			7
 #define   GEN6_RC0			0
@@ -6922,26 +7003,26 @@ enum skl_disp_power_wells {
 #define   GEN6_RC6			3
 #define   GEN6_RC7			4
 
-#define GEN8_GT_SLICE_INFO		0x138064
+#define GEN8_GT_SLICE_INFO		_MMIO(0x138064)
 #define   GEN8_LSLICESTAT_MASK		0x7
 
-#define CHV_POWER_SS0_SIG1		0xa720
-#define CHV_POWER_SS1_SIG1		0xa728
+#define CHV_POWER_SS0_SIG1		_MMIO(0xa720)
+#define CHV_POWER_SS1_SIG1		_MMIO(0xa728)
 #define   CHV_SS_PG_ENABLE		(1<<1)
 #define   CHV_EU08_PG_ENABLE		(1<<9)
 #define   CHV_EU19_PG_ENABLE		(1<<17)
 #define   CHV_EU210_PG_ENABLE		(1<<25)
 
-#define CHV_POWER_SS0_SIG2		0xa724
-#define CHV_POWER_SS1_SIG2		0xa72c
+#define CHV_POWER_SS0_SIG2		_MMIO(0xa724)
+#define CHV_POWER_SS1_SIG2		_MMIO(0xa72c)
 #define   CHV_EU311_PG_ENABLE		(1<<1)
 
-#define GEN9_SLICE_PGCTL_ACK(slice)	(0x804c + (slice)*0x4)
+#define GEN9_SLICE_PGCTL_ACK(slice)	_MMIO(0x804c + (slice)*0x4)
 #define   GEN9_PGCTL_SLICE_ACK		(1 << 0)
 #define   GEN9_PGCTL_SS_ACK(subslice)	(1 << (2 + (subslice)*2))
 
-#define GEN9_SS01_EU_PGCTL_ACK(slice)	(0x805c + (slice)*0x8)
-#define GEN9_SS23_EU_PGCTL_ACK(slice)	(0x8060 + (slice)*0x8)
+#define GEN9_SS01_EU_PGCTL_ACK(slice)	_MMIO(0x805c + (slice)*0x8)
+#define GEN9_SS23_EU_PGCTL_ACK(slice)	_MMIO(0x8060 + (slice)*0x8)
 #define   GEN9_PGCTL_SSA_EU08_ACK	(1 << 0)
 #define   GEN9_PGCTL_SSA_EU19_ACK	(1 << 2)
 #define   GEN9_PGCTL_SSA_EU210_ACK	(1 << 4)
@@ -6951,18 +7032,17 @@ enum skl_disp_power_wells {
 #define   GEN9_PGCTL_SSB_EU210_ACK	(1 << 12)
 #define   GEN9_PGCTL_SSB_EU311_ACK	(1 << 14)
 
-#define GEN7_MISCCPCTL			(0x9424)
+#define GEN7_MISCCPCTL				_MMIO(0x9424)
 #define   GEN7_DOP_CLOCK_GATE_ENABLE		(1<<0)
 #define   GEN8_DOP_CLOCK_GATE_CFCLK_ENABLE	(1<<2)
 #define   GEN8_DOP_CLOCK_GATE_GUC_ENABLE	(1<<4)
 #define   GEN8_DOP_CLOCK_GATE_MEDIA_ENABLE     (1<<6)
 
-#define GEN8_GARBCNTL                   0xB004
+#define GEN8_GARBCNTL                   _MMIO(0xB004)
 #define   GEN9_GAPS_TSV_CREDIT_DISABLE  (1<<7)
 
 /* IVYBRIDGE DPF */
-#define GEN7_L3CDERRST1			0xB008 /* L3CD Error Status 1 */
-#define HSW_L3CDERRST11			0xB208 /* L3CD Error Status register 1 slice 1 */
+#define GEN7_L3CDERRST1(slice)		_MMIO(0xB008 + (slice) * 0x200) /* L3CD Error Status 1 */
 #define   GEN7_L3CDERRST1_ROW_MASK	(0x7ff<<14)
 #define   GEN7_PARITY_ERROR_VALID	(1<<13)
 #define   GEN7_L3CDERRST1_BANK_MASK	(3<<11)
@@ -6975,119 +7055,102 @@ enum skl_disp_power_wells {
 		((reg & GEN7_L3CDERRST1_SUBBANK_MASK) >> 8)
 #define   GEN7_L3CDERRST1_ENABLE	(1<<7)
 
-#define GEN7_L3LOG_BASE			0xB070
-#define HSW_L3LOG_BASE_SLICE1		0xB270
+#define GEN7_L3LOG(slice, i)		_MMIO(0xB070 + (slice) * 0x200 + (i) * 4)
 #define GEN7_L3LOG_SIZE			0x80
 
-#define GEN7_HALF_SLICE_CHICKEN1	0xe100 /* IVB GT1 + VLV */
-#define GEN7_HALF_SLICE_CHICKEN1_GT2	0xf100
+#define GEN7_HALF_SLICE_CHICKEN1	_MMIO(0xe100) /* IVB GT1 + VLV */
+#define GEN7_HALF_SLICE_CHICKEN1_GT2	_MMIO(0xf100)
 #define   GEN7_MAX_PS_THREAD_DEP		(8<<12)
 #define   GEN7_SINGLE_SUBSCAN_DISPATCH_ENABLE	(1<<10)
 #define   GEN7_SBE_SS_CACHE_DISPATCH_PORT_SHARING_DISABLE	(1<<4)
 #define   GEN7_PSD_SINGLE_PORT_DISPATCH_ENABLE	(1<<3)
 
-#define GEN9_HALF_SLICE_CHICKEN5	0xe188
+#define GEN9_HALF_SLICE_CHICKEN5	_MMIO(0xe188)
 #define   GEN9_DG_MIRROR_FIX_ENABLE	(1<<5)
 #define   GEN9_CCS_TLB_PREFETCH_ENABLE	(1<<3)
 
-#define GEN8_ROW_CHICKEN		0xe4f0
+#define GEN8_ROW_CHICKEN		_MMIO(0xe4f0)
 #define   PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE	(1<<8)
 #define   STALL_DOP_GATING_DISABLE		(1<<5)
 
-#define GEN7_ROW_CHICKEN2		0xe4f4
-#define GEN7_ROW_CHICKEN2_GT2		0xf4f4
+#define GEN7_ROW_CHICKEN2		_MMIO(0xe4f4)
+#define GEN7_ROW_CHICKEN2_GT2		_MMIO(0xf4f4)
 #define   DOP_CLOCK_GATING_DISABLE	(1<<0)
 
-#define HSW_ROW_CHICKEN3		0xe49c
+#define HSW_ROW_CHICKEN3		_MMIO(0xe49c)
 #define  HSW_ROW_CHICKEN3_L3_GLOBAL_ATOMICS_DISABLE    (1 << 6)
 
-#define HALF_SLICE_CHICKEN2		0xe180
+#define HALF_SLICE_CHICKEN2		_MMIO(0xe180)
 #define   GEN8_ST_PO_DISABLE		(1<<13)
 
-#define HALF_SLICE_CHICKEN3		0xe184
+#define HALF_SLICE_CHICKEN3		_MMIO(0xe184)
 #define   HSW_SAMPLE_C_PERFORMANCE	(1<<9)
 #define   GEN8_CENTROID_PIXEL_OPT_DIS	(1<<8)
 #define   GEN9_DISABLE_OCL_OOB_SUPPRESS_LOGIC	(1<<5)
 #define   GEN8_SAMPLER_POWER_BYPASS_DIS	(1<<1)
 
-#define GEN9_HALF_SLICE_CHICKEN7	0xe194
+#define GEN9_HALF_SLICE_CHICKEN7	_MMIO(0xe194)
 #define   GEN9_ENABLE_YV12_BUGFIX	(1<<4)
 
 /* Audio */
-#define G4X_AUD_VID_DID			(dev_priv->info.display_mmio_offset + 0x62020)
+#define G4X_AUD_VID_DID			_MMIO(dev_priv->info.display_mmio_offset + 0x62020)
 #define   INTEL_AUDIO_DEVCL		0x808629FB
 #define   INTEL_AUDIO_DEVBLC		0x80862801
 #define   INTEL_AUDIO_DEVCTG		0x80862802
 
-#define G4X_AUD_CNTL_ST			0x620B4
+#define G4X_AUD_CNTL_ST			_MMIO(0x620B4)
 #define   G4X_ELDV_DEVCL_DEVBLC		(1 << 13)
 #define   G4X_ELDV_DEVCTG		(1 << 14)
 #define   G4X_ELD_ADDR_MASK		(0xf << 5)
 #define   G4X_ELD_ACK			(1 << 4)
-#define G4X_HDMIW_HDMIEDID		0x6210C
+#define G4X_HDMIW_HDMIEDID		_MMIO(0x6210C)
 
 #define _IBX_HDMIW_HDMIEDID_A		0xE2050
 #define _IBX_HDMIW_HDMIEDID_B		0xE2150
-#define IBX_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					_IBX_HDMIW_HDMIEDID_A, \
-					_IBX_HDMIW_HDMIEDID_B)
+#define IBX_HDMIW_HDMIEDID(pipe)	_MMIO_PIPE(pipe, _IBX_HDMIW_HDMIEDID_A, \
+						  _IBX_HDMIW_HDMIEDID_B)
 #define _IBX_AUD_CNTL_ST_A		0xE20B4
 #define _IBX_AUD_CNTL_ST_B		0xE21B4
-#define IBX_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					_IBX_AUD_CNTL_ST_A, \
-					_IBX_AUD_CNTL_ST_B)
+#define IBX_AUD_CNTL_ST(pipe)		_MMIO_PIPE(pipe, _IBX_AUD_CNTL_ST_A, \
+						  _IBX_AUD_CNTL_ST_B)
 #define   IBX_ELD_BUFFER_SIZE_MASK	(0x1f << 10)
 #define   IBX_ELD_ADDRESS_MASK		(0x1f << 5)
 #define   IBX_ELD_ACK			(1 << 4)
-#define IBX_AUD_CNTL_ST2		0xE20C0
+#define IBX_AUD_CNTL_ST2		_MMIO(0xE20C0)
 #define   IBX_CP_READY(port)		((1 << 1) << (((port) - 1) * 4))
 #define   IBX_ELD_VALID(port)		((1 << 0) << (((port) - 1) * 4))
 
 #define _CPT_HDMIW_HDMIEDID_A		0xE5050
 #define _CPT_HDMIW_HDMIEDID_B		0xE5150
-#define CPT_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					_CPT_HDMIW_HDMIEDID_A, \
-					_CPT_HDMIW_HDMIEDID_B)
+#define CPT_HDMIW_HDMIEDID(pipe)	_MMIO_PIPE(pipe, _CPT_HDMIW_HDMIEDID_A, _CPT_HDMIW_HDMIEDID_B)
 #define _CPT_AUD_CNTL_ST_A		0xE50B4
 #define _CPT_AUD_CNTL_ST_B		0xE51B4
-#define CPT_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					_CPT_AUD_CNTL_ST_A, \
-					_CPT_AUD_CNTL_ST_B)
-#define CPT_AUD_CNTRL_ST2		0xE50C0
+#define CPT_AUD_CNTL_ST(pipe)		_MMIO_PIPE(pipe, _CPT_AUD_CNTL_ST_A, _CPT_AUD_CNTL_ST_B)
+#define CPT_AUD_CNTRL_ST2		_MMIO(0xE50C0)
 
 #define _VLV_HDMIW_HDMIEDID_A		(VLV_DISPLAY_BASE + 0x62050)
 #define _VLV_HDMIW_HDMIEDID_B		(VLV_DISPLAY_BASE + 0x62150)
-#define VLV_HDMIW_HDMIEDID(pipe) _PIPE(pipe, \
-					_VLV_HDMIW_HDMIEDID_A, \
-					_VLV_HDMIW_HDMIEDID_B)
+#define VLV_HDMIW_HDMIEDID(pipe)	_MMIO_PIPE(pipe, _VLV_HDMIW_HDMIEDID_A, _VLV_HDMIW_HDMIEDID_B)
 #define _VLV_AUD_CNTL_ST_A		(VLV_DISPLAY_BASE + 0x620B4)
 #define _VLV_AUD_CNTL_ST_B		(VLV_DISPLAY_BASE + 0x621B4)
-#define VLV_AUD_CNTL_ST(pipe) _PIPE(pipe, \
-					_VLV_AUD_CNTL_ST_A, \
-					_VLV_AUD_CNTL_ST_B)
-#define VLV_AUD_CNTL_ST2		(VLV_DISPLAY_BASE + 0x620C0)
+#define VLV_AUD_CNTL_ST(pipe)		_MMIO_PIPE(pipe, _VLV_AUD_CNTL_ST_A, _VLV_AUD_CNTL_ST_B)
+#define VLV_AUD_CNTL_ST2		_MMIO(VLV_DISPLAY_BASE + 0x620C0)
 
 /* These are the 4 32-bit write offset registers for each stream
  * output buffer.  It determines the offset from the
  * 3DSTATE_SO_BUFFERs that the next streamed vertex output goes to.
  */
-#define GEN7_SO_WRITE_OFFSET(n)		(0x5280 + (n) * 4)
+#define GEN7_SO_WRITE_OFFSET(n)		_MMIO(0x5280 + (n) * 4)
 
 #define _IBX_AUD_CONFIG_A		0xe2000
 #define _IBX_AUD_CONFIG_B		0xe2100
-#define IBX_AUD_CFG(pipe) _PIPE(pipe, \
-					_IBX_AUD_CONFIG_A, \
-					_IBX_AUD_CONFIG_B)
+#define IBX_AUD_CFG(pipe)		_MMIO_PIPE(pipe, _IBX_AUD_CONFIG_A, _IBX_AUD_CONFIG_B)
 #define _CPT_AUD_CONFIG_A		0xe5000
 #define _CPT_AUD_CONFIG_B		0xe5100
-#define CPT_AUD_CFG(pipe) _PIPE(pipe, \
-					_CPT_AUD_CONFIG_A, \
-					_CPT_AUD_CONFIG_B)
+#define CPT_AUD_CFG(pipe)		_MMIO_PIPE(pipe, _CPT_AUD_CONFIG_A, _CPT_AUD_CONFIG_B)
 #define _VLV_AUD_CONFIG_A		(VLV_DISPLAY_BASE + 0x62000)
 #define _VLV_AUD_CONFIG_B		(VLV_DISPLAY_BASE + 0x62100)
-#define VLV_AUD_CFG(pipe) _PIPE(pipe, \
-					_VLV_AUD_CONFIG_A, \
-					_VLV_AUD_CONFIG_B)
+#define VLV_AUD_CFG(pipe)		_MMIO_PIPE(pipe, _VLV_AUD_CONFIG_A, _VLV_AUD_CONFIG_B)
 
 #define   AUD_CONFIG_N_VALUE_INDEX		(1 << 29)
 #define   AUD_CONFIG_N_PROG_ENABLE		(1 << 28)
@@ -7112,72 +7175,62 @@ enum skl_disp_power_wells {
 /* HSW Audio */
 #define _HSW_AUD_CONFIG_A		0x65000
 #define _HSW_AUD_CONFIG_B		0x65100
-#define HSW_AUD_CFG(pipe) _PIPE(pipe, \
-					_HSW_AUD_CONFIG_A, \
-					_HSW_AUD_CONFIG_B)
+#define HSW_AUD_CFG(pipe)		_MMIO_PIPE(pipe, _HSW_AUD_CONFIG_A, _HSW_AUD_CONFIG_B)
 
 #define _HSW_AUD_MISC_CTRL_A		0x65010
 #define _HSW_AUD_MISC_CTRL_B		0x65110
-#define HSW_AUD_MISC_CTRL(pipe) _PIPE(pipe, \
-					_HSW_AUD_MISC_CTRL_A, \
-					_HSW_AUD_MISC_CTRL_B)
+#define HSW_AUD_MISC_CTRL(pipe)		_MMIO_PIPE(pipe, _HSW_AUD_MISC_CTRL_A, _HSW_AUD_MISC_CTRL_B)
 
 #define _HSW_AUD_DIP_ELD_CTRL_ST_A	0x650b4
 #define _HSW_AUD_DIP_ELD_CTRL_ST_B	0x651b4
-#define HSW_AUD_DIP_ELD_CTRL(pipe) _PIPE(pipe, \
-					_HSW_AUD_DIP_ELD_CTRL_ST_A, \
-					_HSW_AUD_DIP_ELD_CTRL_ST_B)
+#define HSW_AUD_DIP_ELD_CTRL(pipe)	_MMIO_PIPE(pipe, _HSW_AUD_DIP_ELD_CTRL_ST_A, _HSW_AUD_DIP_ELD_CTRL_ST_B)
 
 /* Audio Digital Converter */
 #define _HSW_AUD_DIG_CNVT_1		0x65080
 #define _HSW_AUD_DIG_CNVT_2		0x65180
-#define AUD_DIG_CNVT(pipe) _PIPE(pipe, \
-					_HSW_AUD_DIG_CNVT_1, \
-					_HSW_AUD_DIG_CNVT_2)
+#define AUD_DIG_CNVT(pipe)		_MMIO_PIPE(pipe, _HSW_AUD_DIG_CNVT_1, _HSW_AUD_DIG_CNVT_2)
 #define DIP_PORT_SEL_MASK		0x3
 
 #define _HSW_AUD_EDID_DATA_A		0x65050
 #define _HSW_AUD_EDID_DATA_B		0x65150
-#define HSW_AUD_EDID_DATA(pipe) _PIPE(pipe, \
-					_HSW_AUD_EDID_DATA_A, \
-					_HSW_AUD_EDID_DATA_B)
+#define HSW_AUD_EDID_DATA(pipe)		_MMIO_PIPE(pipe, _HSW_AUD_EDID_DATA_A, _HSW_AUD_EDID_DATA_B)
 
-#define HSW_AUD_PIPE_CONV_CFG		0x6507c
-#define HSW_AUD_PIN_ELD_CP_VLD		0x650c0
+#define HSW_AUD_PIPE_CONV_CFG		_MMIO(0x6507c)
+#define HSW_AUD_PIN_ELD_CP_VLD		_MMIO(0x650c0)
 #define   AUDIO_INACTIVE(trans)		((1 << 3) << ((trans) * 4))
 #define   AUDIO_OUTPUT_ENABLE(trans)	((1 << 2) << ((trans) * 4))
 #define   AUDIO_CP_READY(trans)		((1 << 1) << ((trans) * 4))
 #define   AUDIO_ELD_VALID(trans)	((1 << 0) << ((trans) * 4))
 
-#define HSW_AUD_CHICKENBIT			0x65f10
+#define HSW_AUD_CHICKENBIT			_MMIO(0x65f10)
 #define   SKL_AUD_CODEC_WAKE_SIGNAL		(1 << 15)
 
 /* HSW Power Wells */
-#define HSW_PWR_WELL_BIOS			0x45400 /* CTL1 */
-#define HSW_PWR_WELL_DRIVER			0x45404 /* CTL2 */
-#define HSW_PWR_WELL_KVMR			0x45408 /* CTL3 */
-#define HSW_PWR_WELL_DEBUG			0x4540C /* CTL4 */
+#define HSW_PWR_WELL_BIOS			_MMIO(0x45400) /* CTL1 */
+#define HSW_PWR_WELL_DRIVER			_MMIO(0x45404) /* CTL2 */
+#define HSW_PWR_WELL_KVMR			_MMIO(0x45408) /* CTL3 */
+#define HSW_PWR_WELL_DEBUG			_MMIO(0x4540C) /* CTL4 */
 #define   HSW_PWR_WELL_ENABLE_REQUEST		(1<<31)
 #define   HSW_PWR_WELL_STATE_ENABLED		(1<<30)
-#define HSW_PWR_WELL_CTL5			0x45410
+#define HSW_PWR_WELL_CTL5			_MMIO(0x45410)
 #define   HSW_PWR_WELL_ENABLE_SINGLE_STEP	(1<<31)
 #define   HSW_PWR_WELL_PWR_GATE_OVERRIDE	(1<<20)
 #define   HSW_PWR_WELL_FORCE_ON			(1<<19)
-#define HSW_PWR_WELL_CTL6			0x45414
+#define HSW_PWR_WELL_CTL6			_MMIO(0x45414)
 
 /* SKL Fuse Status */
-#define SKL_FUSE_STATUS				0x42000
+#define SKL_FUSE_STATUS				_MMIO(0x42000)
 #define  SKL_FUSE_DOWNLOAD_STATUS              (1<<31)
 #define  SKL_FUSE_PG0_DIST_STATUS              (1<<27)
 #define  SKL_FUSE_PG1_DIST_STATUS              (1<<26)
 #define  SKL_FUSE_PG2_DIST_STATUS              (1<<25)
 
 /* Per-pipe DDI Function Control */
-#define TRANS_DDI_FUNC_CTL_A		0x60400
-#define TRANS_DDI_FUNC_CTL_B		0x61400
-#define TRANS_DDI_FUNC_CTL_C		0x62400
-#define TRANS_DDI_FUNC_CTL_EDP		0x6F400
-#define TRANS_DDI_FUNC_CTL(tran) _TRANSCODER2(tran, TRANS_DDI_FUNC_CTL_A)
+#define _TRANS_DDI_FUNC_CTL_A		0x60400
+#define _TRANS_DDI_FUNC_CTL_B		0x61400
+#define _TRANS_DDI_FUNC_CTL_C		0x62400
+#define _TRANS_DDI_FUNC_CTL_EDP		0x6F400
+#define TRANS_DDI_FUNC_CTL(tran) _MMIO_TRANS2(tran, _TRANS_DDI_FUNC_CTL_A)
 
 #define  TRANS_DDI_FUNC_ENABLE		(1<<31)
 /* Those bits are ignored by pipe EDP since it can only connect to DDI A */
@@ -7207,9 +7260,9 @@ enum skl_disp_power_wells {
 #define  TRANS_DDI_BFI_ENABLE		(1<<4)
 
 /* DisplayPort Transport Control */
-#define DP_TP_CTL_A			0x64040
-#define DP_TP_CTL_B			0x64140
-#define DP_TP_CTL(port) _PORT(port, DP_TP_CTL_A, DP_TP_CTL_B)
+#define _DP_TP_CTL_A			0x64040
+#define _DP_TP_CTL_B			0x64140
+#define DP_TP_CTL(port) _MMIO_PORT(port, _DP_TP_CTL_A, _DP_TP_CTL_B)
 #define  DP_TP_CTL_ENABLE			(1<<31)
 #define  DP_TP_CTL_MODE_SST			(0<<27)
 #define  DP_TP_CTL_MODE_MST			(1<<27)
@@ -7225,9 +7278,9 @@ enum skl_disp_power_wells {
 #define  DP_TP_CTL_SCRAMBLE_DISABLE		(1<<7)
 
 /* DisplayPort Transport Status */
-#define DP_TP_STATUS_A			0x64044
-#define DP_TP_STATUS_B			0x64144
-#define DP_TP_STATUS(port) _PORT(port, DP_TP_STATUS_A, DP_TP_STATUS_B)
+#define _DP_TP_STATUS_A			0x64044
+#define _DP_TP_STATUS_B			0x64144
+#define DP_TP_STATUS(port) _MMIO_PORT(port, _DP_TP_STATUS_A, _DP_TP_STATUS_B)
 #define  DP_TP_STATUS_IDLE_DONE			(1<<25)
 #define  DP_TP_STATUS_ACT_SENT			(1<<24)
 #define  DP_TP_STATUS_MODE_STATUS_MST		(1<<23)
@@ -7237,9 +7290,9 @@ enum skl_disp_power_wells {
 #define  DP_TP_STATUS_PAYLOAD_MAPPING_VC0	(3 << 0)
 
 /* DDI Buffer Control */
-#define DDI_BUF_CTL_A				0x64000
-#define DDI_BUF_CTL_B				0x64100
-#define DDI_BUF_CTL(port) _PORT(port, DDI_BUF_CTL_A, DDI_BUF_CTL_B)
+#define _DDI_BUF_CTL_A				0x64000
+#define _DDI_BUF_CTL_B				0x64100
+#define DDI_BUF_CTL(port) _MMIO_PORT(port, _DDI_BUF_CTL_A, _DDI_BUF_CTL_B)
 #define  DDI_BUF_CTL_ENABLE			(1<<31)
 #define  DDI_BUF_TRANS_SELECT(n)	((n) << 24)
 #define  DDI_BUF_EMP_MASK			(0xf<<24)
@@ -7252,17 +7305,17 @@ enum skl_disp_power_wells {
 #define  DDI_INIT_DISPLAY_DETECTED		(1<<0)
 
 /* DDI Buffer Translations */
-#define DDI_BUF_TRANS_A				0x64E00
-#define DDI_BUF_TRANS_B				0x64E60
-#define DDI_BUF_TRANS_LO(port, i) (_PORT(port, DDI_BUF_TRANS_A, DDI_BUF_TRANS_B) + (i) * 8)
-#define DDI_BUF_TRANS_HI(port, i) (_PORT(port, DDI_BUF_TRANS_A, DDI_BUF_TRANS_B) + (i) * 8 + 4)
+#define _DDI_BUF_TRANS_A		0x64E00
+#define _DDI_BUF_TRANS_B		0x64E60
+#define DDI_BUF_TRANS_LO(port, i)	_MMIO(_PORT(port, _DDI_BUF_TRANS_A, _DDI_BUF_TRANS_B) + (i) * 8)
+#define DDI_BUF_TRANS_HI(port, i)	_MMIO(_PORT(port, _DDI_BUF_TRANS_A, _DDI_BUF_TRANS_B) + (i) * 8 + 4)
 
 /* Sideband Interface (SBI) is programmed indirectly, via
  * SBI_ADDR, which contains the register offset; and SBI_DATA,
  * which contains the payload */
-#define SBI_ADDR			0xC6000
-#define SBI_DATA			0xC6004
-#define SBI_CTL_STAT			0xC6008
+#define SBI_ADDR			_MMIO(0xC6000)
+#define SBI_DATA			_MMIO(0xC6004)
+#define SBI_CTL_STAT			_MMIO(0xC6008)
 #define  SBI_CTL_DEST_ICLK		(0x0<<16)
 #define  SBI_CTL_DEST_MPHY		(0x1<<16)
 #define  SBI_CTL_OP_IORD		(0x2<<8)
@@ -7275,6 +7328,7 @@ enum skl_disp_power_wells {
 #define  SBI_READY			(0x0<<0)
 
 /* SBI offsets */
+#define  SBI_SSCDIVINTPHASE			0x0200
 #define  SBI_SSCDIVINTPHASE6			0x0600
 #define   SBI_SSCDIVINTPHASE_DIVSEL_MASK	((0x7f)<<1)
 #define   SBI_SSCDIVINTPHASE_DIVSEL(x)		((x)<<1)
@@ -7282,6 +7336,7 @@ enum skl_disp_power_wells {
 #define   SBI_SSCDIVINTPHASE_INCVAL(x)		((x)<<8)
 #define   SBI_SSCDIVINTPHASE_DIR(x)		((x)<<15)
 #define   SBI_SSCDIVINTPHASE_PROPAGATE		(1<<0)
+#define  SBI_SSCDITHPHASE			0x0204
 #define  SBI_SSCCTL				0x020c
 #define  SBI_SSCCTL6				0x060C
 #define   SBI_SSCCTL_PATHALT			(1<<3)
@@ -7293,12 +7348,12 @@ enum skl_disp_power_wells {
 #define   SBI_GEN0_CFG_BUFFENABLE_DISABLE	(1<<0)
 
 /* LPT PIXCLK_GATE */
-#define PIXCLK_GATE			0xC6020
+#define PIXCLK_GATE			_MMIO(0xC6020)
 #define  PIXCLK_GATE_UNGATE		(1<<0)
 #define  PIXCLK_GATE_GATE		(0<<0)
 
 /* SPLL */
-#define SPLL_CTL			0x46020
+#define SPLL_CTL			_MMIO(0x46020)
 #define  SPLL_PLL_ENABLE		(1<<31)
 #define  SPLL_PLL_SSC			(1<<28)
 #define  SPLL_PLL_NON_SSC		(2<<28)
@@ -7310,9 +7365,9 @@ enum skl_disp_power_wells {
 #define  SPLL_PLL_FREQ_MASK		(3<<26)
 
 /* WRPLL */
-#define WRPLL_CTL1			0x46040
-#define WRPLL_CTL2			0x46060
-#define WRPLL_CTL(pll)			(pll == 0 ? WRPLL_CTL1 : WRPLL_CTL2)
+#define _WRPLL_CTL1			0x46040
+#define _WRPLL_CTL2			0x46060
+#define WRPLL_CTL(pll)			_MMIO_PIPE(pll, _WRPLL_CTL1, _WRPLL_CTL2)
 #define  WRPLL_PLL_ENABLE		(1<<31)
 #define  WRPLL_PLL_SSC			(1<<28)
 #define  WRPLL_PLL_NON_SSC		(2<<28)
@@ -7329,9 +7384,9 @@ enum skl_disp_power_wells {
 #define  WRPLL_DIVIDER_FB_MASK		(0xff<<16)
 
 /* Port clock selection */
-#define PORT_CLK_SEL_A			0x46100
-#define PORT_CLK_SEL_B			0x46104
-#define PORT_CLK_SEL(port) _PORT(port, PORT_CLK_SEL_A, PORT_CLK_SEL_B)
+#define _PORT_CLK_SEL_A			0x46100
+#define _PORT_CLK_SEL_B			0x46104
+#define PORT_CLK_SEL(port) _MMIO_PORT(port, _PORT_CLK_SEL_A, _PORT_CLK_SEL_B)
 #define  PORT_CLK_SEL_LCPLL_2700	(0<<29)
 #define  PORT_CLK_SEL_LCPLL_1350	(1<<29)
 #define  PORT_CLK_SEL_LCPLL_810		(2<<29)
@@ -7343,18 +7398,18 @@ enum skl_disp_power_wells {
 #define  PORT_CLK_SEL_MASK		(7<<29)
 
 /* Transcoder clock selection */
-#define TRANS_CLK_SEL_A			0x46140
-#define TRANS_CLK_SEL_B			0x46144
-#define TRANS_CLK_SEL(tran) _TRANSCODER(tran, TRANS_CLK_SEL_A, TRANS_CLK_SEL_B)
+#define _TRANS_CLK_SEL_A		0x46140
+#define _TRANS_CLK_SEL_B		0x46144
+#define TRANS_CLK_SEL(tran) _MMIO_TRANS(tran, _TRANS_CLK_SEL_A, _TRANS_CLK_SEL_B)
 /* For each transcoder, we need to select the corresponding port clock */
 #define  TRANS_CLK_SEL_DISABLED		(0x0<<29)
 #define  TRANS_CLK_SEL_PORT(x)		(((x)+1)<<29)
 
-#define TRANSA_MSA_MISC			0x60410
-#define TRANSB_MSA_MISC			0x61410
-#define TRANSC_MSA_MISC			0x62410
-#define TRANS_EDP_MSA_MISC		0x6f410
-#define TRANS_MSA_MISC(tran) _TRANSCODER2(tran, TRANSA_MSA_MISC)
+#define _TRANSA_MSA_MISC		0x60410
+#define _TRANSB_MSA_MISC		0x61410
+#define _TRANSC_MSA_MISC		0x62410
+#define _TRANS_EDP_MSA_MISC		0x6f410
+#define TRANS_MSA_MISC(tran) _MMIO_TRANS2(tran, _TRANSA_MSA_MISC)
 
 #define  TRANS_MSA_SYNC_CLK		(1<<0)
 #define  TRANS_MSA_6_BPC		(0<<5)
@@ -7364,7 +7419,7 @@ enum skl_disp_power_wells {
 #define  TRANS_MSA_16_BPC		(4<<5)
 
 /* LCPLL Control */
-#define LCPLL_CTL			0x130040
+#define LCPLL_CTL			_MMIO(0x130040)
 #define  LCPLL_PLL_DISABLE		(1<<31)
 #define  LCPLL_PLL_LOCK			(1<<30)
 #define  LCPLL_CLK_FREQ_MASK		(3<<26)
@@ -7384,7 +7439,7 @@ enum skl_disp_power_wells {
  */
 
 /* CDCLK_CTL */
-#define CDCLK_CTL			0x46000
+#define CDCLK_CTL			_MMIO(0x46000)
 #define  CDCLK_FREQ_SEL_MASK		(3<<26)
 #define  CDCLK_FREQ_450_432		(0<<26)
 #define  CDCLK_FREQ_540			(1<<26)
@@ -7400,12 +7455,12 @@ enum skl_disp_power_wells {
 #define  BXT_CDCLK_SSA_PRECHARGE_ENABLE	(1<<16)
 
 /* LCPLL_CTL */
-#define LCPLL1_CTL		0x46010
-#define LCPLL2_CTL		0x46014
+#define LCPLL1_CTL		_MMIO(0x46010)
+#define LCPLL2_CTL		_MMIO(0x46014)
 #define  LCPLL_PLL_ENABLE	(1<<31)
 
 /* DPLL control1 */
-#define DPLL_CTRL1		0x6C058
+#define DPLL_CTRL1		_MMIO(0x6C058)
 #define  DPLL_CTRL1_HDMI_MODE(id)		(1<<((id)*6+5))
 #define  DPLL_CTRL1_SSC(id)			(1<<((id)*6+4))
 #define  DPLL_CTRL1_LINK_RATE_MASK(id)		(7<<((id)*6+1))
@@ -7420,7 +7475,7 @@ enum skl_disp_power_wells {
 #define  DPLL_CTRL1_LINK_RATE_2160		5
 
 /* DPLL control2 */
-#define DPLL_CTRL2				0x6C05C
+#define DPLL_CTRL2				_MMIO(0x6C05C)
 #define  DPLL_CTRL2_DDI_CLK_OFF(port)		(1<<((port)+15))
 #define  DPLL_CTRL2_DDI_CLK_SEL_MASK(port)	(3<<((port)*3+1))
 #define  DPLL_CTRL2_DDI_CLK_SEL_SHIFT(port)    ((port)*3+1)
@@ -7428,21 +7483,21 @@ enum skl_disp_power_wells {
 #define  DPLL_CTRL2_DDI_SEL_OVERRIDE(port)     (1<<((port)*3))
 
 /* DPLL Status */
-#define DPLL_STATUS	0x6C060
+#define DPLL_STATUS	_MMIO(0x6C060)
 #define  DPLL_LOCK(id) (1<<((id)*8))
 
 /* DPLL cfg */
-#define DPLL1_CFGCR1	0x6C040
-#define DPLL2_CFGCR1	0x6C048
-#define DPLL3_CFGCR1	0x6C050
+#define _DPLL1_CFGCR1	0x6C040
+#define _DPLL2_CFGCR1	0x6C048
+#define _DPLL3_CFGCR1	0x6C050
 #define  DPLL_CFGCR1_FREQ_ENABLE	(1<<31)
 #define  DPLL_CFGCR1_DCO_FRACTION_MASK	(0x7fff<<9)
 #define  DPLL_CFGCR1_DCO_FRACTION(x)	((x)<<9)
 #define  DPLL_CFGCR1_DCO_INTEGER_MASK	(0x1ff)
 
-#define DPLL1_CFGCR2	0x6C044
-#define DPLL2_CFGCR2	0x6C04C
-#define DPLL3_CFGCR2	0x6C054
+#define _DPLL1_CFGCR2	0x6C044
+#define _DPLL2_CFGCR2	0x6C04C
+#define _DPLL3_CFGCR2	0x6C054
 #define  DPLL_CFGCR2_QDIV_RATIO_MASK	(0xff<<8)
 #define  DPLL_CFGCR2_QDIV_RATIO(x)	((x)<<8)
 #define  DPLL_CFGCR2_QDIV_MODE(x)	((x)<<7)
@@ -7460,58 +7515,59 @@ enum skl_disp_power_wells {
 #define  DPLL_CFGCR2_PDIV_7 (4<<2)
 #define  DPLL_CFGCR2_CENTRAL_FREQ_MASK	(3)
 
-#define DPLL_CFGCR1(id) (DPLL1_CFGCR1 + ((id) - SKL_DPLL1) * 8)
-#define DPLL_CFGCR2(id) (DPLL1_CFGCR2 + ((id) - SKL_DPLL1) * 8)
+#define DPLL_CFGCR1(id)	_MMIO_PIPE((id) - SKL_DPLL1, _DPLL1_CFGCR1, _DPLL2_CFGCR1)
+#define DPLL_CFGCR2(id)	_MMIO_PIPE((id) - SKL_DPLL1, _DPLL1_CFGCR2, _DPLL2_CFGCR2)
 
 /* BXT display engine PLL */
-#define BXT_DE_PLL_CTL			0x6d000
+#define BXT_DE_PLL_CTL			_MMIO(0x6d000)
 #define   BXT_DE_PLL_RATIO(x)		(x)	/* {60,65,100} * 19.2MHz */
 #define   BXT_DE_PLL_RATIO_MASK		0xff
 
-#define BXT_DE_PLL_ENABLE		0x46070
+#define BXT_DE_PLL_ENABLE		_MMIO(0x46070)
 #define   BXT_DE_PLL_PLL_ENABLE		(1 << 31)
 #define   BXT_DE_PLL_LOCK		(1 << 30)
 
 /* GEN9 DC */
-#define DC_STATE_EN			0x45504
+#define DC_STATE_EN			_MMIO(0x45504)
+#define  DC_STATE_DISABLE		0
 #define  DC_STATE_EN_UPTO_DC5		(1<<0)
 #define  DC_STATE_EN_DC9		(1<<3)
 #define  DC_STATE_EN_UPTO_DC6		(2<<0)
 #define  DC_STATE_EN_UPTO_DC5_DC6_MASK   0x3
 
-#define  DC_STATE_DEBUG                  0x45520
+#define  DC_STATE_DEBUG                  _MMIO(0x45520)
 #define  DC_STATE_DEBUG_MASK_MEMORY_UP	(1<<1)
 
 /* Please see hsw_read_dcomp() and hsw_write_dcomp() before using this register,
  * since on HSW we can't write to it using I915_WRITE. */
-#define D_COMP_HSW			(MCHBAR_MIRROR_BASE_SNB + 0x5F0C)
-#define D_COMP_BDW			0x138144
+#define D_COMP_HSW			_MMIO(MCHBAR_MIRROR_BASE_SNB + 0x5F0C)
+#define D_COMP_BDW			_MMIO(0x138144)
 #define  D_COMP_RCOMP_IN_PROGRESS	(1<<9)
 #define  D_COMP_COMP_FORCE		(1<<8)
 #define  D_COMP_COMP_DISABLE		(1<<0)
 
 /* Pipe WM_LINETIME - watermark line time */
-#define PIPE_WM_LINETIME_A		0x45270
-#define PIPE_WM_LINETIME_B		0x45274
-#define PIPE_WM_LINETIME(pipe) _PIPE(pipe, PIPE_WM_LINETIME_A, \
-					   PIPE_WM_LINETIME_B)
+#define _PIPE_WM_LINETIME_A		0x45270
+#define _PIPE_WM_LINETIME_B		0x45274
+#define PIPE_WM_LINETIME(pipe) _MMIO_PIPE(pipe, _PIPE_WM_LINETIME_A, _PIPE_WM_LINETIME_B)
 #define   PIPE_WM_LINETIME_MASK			(0x1ff)
 #define   PIPE_WM_LINETIME_TIME(x)		((x))
 #define   PIPE_WM_LINETIME_IPS_LINETIME_MASK	(0x1ff<<16)
 #define   PIPE_WM_LINETIME_IPS_LINETIME(x)	((x)<<16)
 
 /* SFUSE_STRAP */
-#define SFUSE_STRAP			0xc2014
+#define SFUSE_STRAP			_MMIO(0xc2014)
 #define  SFUSE_STRAP_FUSE_LOCK		(1<<13)
 #define  SFUSE_STRAP_DISPLAY_DISABLED	(1<<7)
+#define  SFUSE_STRAP_CRT_DISABLED	(1<<6)
 #define  SFUSE_STRAP_DDIB_DETECTED	(1<<2)
 #define  SFUSE_STRAP_DDIC_DETECTED	(1<<1)
 #define  SFUSE_STRAP_DDID_DETECTED	(1<<0)
 
-#define WM_MISC				0x45260
+#define WM_MISC				_MMIO(0x45260)
 #define  WM_MISC_DATA_PARTITION_5_6	(1 << 0)
 
-#define WM_DBG				0x45280
+#define WM_DBG				_MMIO(0x45280)
 #define  WM_DBG_DISALLOW_MULTIPLE_LP	(1<<0)
 #define  WM_DBG_DISALLOW_MAXFIFO	(1<<1)
 #define  WM_DBG_DISALLOW_SPRITE		(1<<2)
@@ -7548,28 +7604,29 @@ enum skl_disp_power_wells {
 #define _PIPE_B_CSC_POSTOFF_ME	0x49144
 #define _PIPE_B_CSC_POSTOFF_LO	0x49148
 
-#define PIPE_CSC_COEFF_RY_GY(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_RY_GY, _PIPE_B_CSC_COEFF_RY_GY)
-#define PIPE_CSC_COEFF_BY(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_BY, _PIPE_B_CSC_COEFF_BY)
-#define PIPE_CSC_COEFF_RU_GU(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_RU_GU, _PIPE_B_CSC_COEFF_RU_GU)
-#define PIPE_CSC_COEFF_BU(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_BU, _PIPE_B_CSC_COEFF_BU)
-#define PIPE_CSC_COEFF_RV_GV(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_RV_GV, _PIPE_B_CSC_COEFF_RV_GV)
-#define PIPE_CSC_COEFF_BV(pipe) _PIPE(pipe, _PIPE_A_CSC_COEFF_BV, _PIPE_B_CSC_COEFF_BV)
-#define PIPE_CSC_MODE(pipe) _PIPE(pipe, _PIPE_A_CSC_MODE, _PIPE_B_CSC_MODE)
-#define PIPE_CSC_PREOFF_HI(pipe) _PIPE(pipe, _PIPE_A_CSC_PREOFF_HI, _PIPE_B_CSC_PREOFF_HI)
-#define PIPE_CSC_PREOFF_ME(pipe) _PIPE(pipe, _PIPE_A_CSC_PREOFF_ME, _PIPE_B_CSC_PREOFF_ME)
-#define PIPE_CSC_PREOFF_LO(pipe) _PIPE(pipe, _PIPE_A_CSC_PREOFF_LO, _PIPE_B_CSC_PREOFF_LO)
-#define PIPE_CSC_POSTOFF_HI(pipe) _PIPE(pipe, _PIPE_A_CSC_POSTOFF_HI, _PIPE_B_CSC_POSTOFF_HI)
-#define PIPE_CSC_POSTOFF_ME(pipe) _PIPE(pipe, _PIPE_A_CSC_POSTOFF_ME, _PIPE_B_CSC_POSTOFF_ME)
-#define PIPE_CSC_POSTOFF_LO(pipe) _PIPE(pipe, _PIPE_A_CSC_POSTOFF_LO, _PIPE_B_CSC_POSTOFF_LO)
+#define PIPE_CSC_COEFF_RY_GY(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_RY_GY, _PIPE_B_CSC_COEFF_RY_GY)
+#define PIPE_CSC_COEFF_BY(pipe)		_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_BY, _PIPE_B_CSC_COEFF_BY)
+#define PIPE_CSC_COEFF_RU_GU(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_RU_GU, _PIPE_B_CSC_COEFF_RU_GU)
+#define PIPE_CSC_COEFF_BU(pipe)		_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_BU, _PIPE_B_CSC_COEFF_BU)
+#define PIPE_CSC_COEFF_RV_GV(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_RV_GV, _PIPE_B_CSC_COEFF_RV_GV)
+#define PIPE_CSC_COEFF_BV(pipe)		_MMIO_PIPE(pipe, _PIPE_A_CSC_COEFF_BV, _PIPE_B_CSC_COEFF_BV)
+#define PIPE_CSC_MODE(pipe)		_MMIO_PIPE(pipe, _PIPE_A_CSC_MODE, _PIPE_B_CSC_MODE)
+#define PIPE_CSC_PREOFF_HI(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_PREOFF_HI, _PIPE_B_CSC_PREOFF_HI)
+#define PIPE_CSC_PREOFF_ME(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_PREOFF_ME, _PIPE_B_CSC_PREOFF_ME)
+#define PIPE_CSC_PREOFF_LO(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_PREOFF_LO, _PIPE_B_CSC_PREOFF_LO)
+#define PIPE_CSC_POSTOFF_HI(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_POSTOFF_HI, _PIPE_B_CSC_POSTOFF_HI)
+#define PIPE_CSC_POSTOFF_ME(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_POSTOFF_ME, _PIPE_B_CSC_POSTOFF_ME)
+#define PIPE_CSC_POSTOFF_LO(pipe)	_MMIO_PIPE(pipe, _PIPE_A_CSC_POSTOFF_LO, _PIPE_B_CSC_POSTOFF_LO)
 
 /* MIPI DSI registers */
 
 #define _MIPI_PORT(port, a, c)	_PORT3(port, a, 0, c)	/* ports A and C only */
+#define _MMIO_MIPI(port, a, c)	_MMIO(_MIPI_PORT(port, a, c))
 
 /* BXT MIPI clock controls */
 #define BXT_MAX_VAR_OUTPUT_KHZ			39500
 
-#define BXT_MIPI_CLOCK_CTL			0x46090
+#define BXT_MIPI_CLOCK_CTL			_MMIO(0x46090)
 #define  BXT_MIPI1_DIV_SHIFT			26
 #define  BXT_MIPI2_DIV_SHIFT			10
 #define  BXT_MIPI_DIV_SHIFT(port)		\
@@ -7631,20 +7688,20 @@ enum skl_disp_power_wells {
 /* BXT MIPI mode configure */
 #define  _BXT_MIPIA_TRANS_HACTIVE			0x6B0F8
 #define  _BXT_MIPIC_TRANS_HACTIVE			0x6B8F8
-#define  BXT_MIPI_TRANS_HACTIVE(tc)	_MIPI_PORT(tc, \
+#define  BXT_MIPI_TRANS_HACTIVE(tc)	_MMIO_MIPI(tc, \
 		_BXT_MIPIA_TRANS_HACTIVE, _BXT_MIPIC_TRANS_HACTIVE)
 
 #define  _BXT_MIPIA_TRANS_VACTIVE			0x6B0FC
 #define  _BXT_MIPIC_TRANS_VACTIVE			0x6B8FC
-#define  BXT_MIPI_TRANS_VACTIVE(tc)	_MIPI_PORT(tc, \
+#define  BXT_MIPI_TRANS_VACTIVE(tc)	_MMIO_MIPI(tc, \
 		_BXT_MIPIA_TRANS_VACTIVE, _BXT_MIPIC_TRANS_VACTIVE)
 
 #define  _BXT_MIPIA_TRANS_VTOTAL			0x6B100
 #define  _BXT_MIPIC_TRANS_VTOTAL			0x6B900
-#define  BXT_MIPI_TRANS_VTOTAL(tc)	_MIPI_PORT(tc, \
+#define  BXT_MIPI_TRANS_VTOTAL(tc)	_MMIO_MIPI(tc, \
 		_BXT_MIPIA_TRANS_VTOTAL, _BXT_MIPIC_TRANS_VTOTAL)
 
-#define BXT_DSI_PLL_CTL			0x161000
+#define BXT_DSI_PLL_CTL			_MMIO(0x161000)
 #define  BXT_DSI_PLL_PVD_RATIO_SHIFT	16
 #define  BXT_DSI_PLL_PVD_RATIO_MASK	(3 << BXT_DSI_PLL_PVD_RATIO_SHIFT)
 #define  BXT_DSI_PLL_PVD_RATIO_1	(1 << BXT_DSI_PLL_PVD_RATIO_SHIFT)
@@ -7660,21 +7717,20 @@ enum skl_disp_power_wells {
 #define BXT_DSI_PLL_RATIO_MAX		0x7D
 #define BXT_DSI_PLL_RATIO_MIN		0x22
 #define BXT_DSI_PLL_RATIO_MASK		0xFF
-#define BXT_REF_CLOCK_KHZ		19500
+#define BXT_REF_CLOCK_KHZ		19200
 
-#define BXT_DSI_PLL_ENABLE		0x46080
+#define BXT_DSI_PLL_ENABLE		_MMIO(0x46080)
 #define  BXT_DSI_PLL_DO_ENABLE		(1 << 31)
 #define  BXT_DSI_PLL_LOCKED		(1 << 30)
 
 #define _MIPIA_PORT_CTRL			(VLV_DISPLAY_BASE + 0x61190)
 #define _MIPIC_PORT_CTRL			(VLV_DISPLAY_BASE + 0x61700)
-#define MIPI_PORT_CTRL(port)	_MIPI_PORT(port, _MIPIA_PORT_CTRL, _MIPIC_PORT_CTRL)
+#define MIPI_PORT_CTRL(port)	_MMIO_MIPI(port, _MIPIA_PORT_CTRL, _MIPIC_PORT_CTRL)
 
  /* BXT port control */
 #define _BXT_MIPIA_PORT_CTRL				0x6B0C0
 #define _BXT_MIPIC_PORT_CTRL				0x6B8C0
-#define BXT_MIPI_PORT_CTRL(tc)	_MIPI_PORT(tc, _BXT_MIPIA_PORT_CTRL, \
-						_BXT_MIPIC_PORT_CTRL)
+#define BXT_MIPI_PORT_CTRL(tc)	_MMIO_MIPI(tc, _BXT_MIPIA_PORT_CTRL, _BXT_MIPIC_PORT_CTRL)
 
 #define  DPI_ENABLE					(1 << 31) /* A + C */
 #define  MIPIA_MIPI4DPHY_DELAY_COUNT_SHIFT		27
@@ -7718,8 +7774,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_TEARING_CTRL			(VLV_DISPLAY_BASE + 0x61194)
 #define _MIPIC_TEARING_CTRL			(VLV_DISPLAY_BASE + 0x61704)
-#define MIPI_TEARING_CTRL(port)			_MIPI_PORT(port, \
-				_MIPIA_TEARING_CTRL, _MIPIC_TEARING_CTRL)
+#define MIPI_TEARING_CTRL(port)			_MMIO_MIPI(port, _MIPIA_TEARING_CTRL, _MIPIC_TEARING_CTRL)
 #define  TEARING_EFFECT_DELAY_SHIFT			0
 #define  TEARING_EFFECT_DELAY_MASK			(0xffff << 0)
 
@@ -7730,8 +7785,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_DEVICE_READY		(dev_priv->mipi_mmio_base + 0xb000)
 #define _MIPIC_DEVICE_READY		(dev_priv->mipi_mmio_base + 0xb800)
-#define MIPI_DEVICE_READY(port)		_MIPI_PORT(port, _MIPIA_DEVICE_READY, \
-						_MIPIC_DEVICE_READY)
+#define MIPI_DEVICE_READY(port)		_MMIO_MIPI(port, _MIPIA_DEVICE_READY, _MIPIC_DEVICE_READY)
 #define  BUS_POSSESSION					(1 << 3) /* set to give bus to receiver */
 #define  ULPS_STATE_MASK				(3 << 1)
 #define  ULPS_STATE_ENTER				(2 << 1)
@@ -7741,12 +7795,10 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_INTR_STAT		(dev_priv->mipi_mmio_base + 0xb004)
 #define _MIPIC_INTR_STAT		(dev_priv->mipi_mmio_base + 0xb804)
-#define MIPI_INTR_STAT(port)		_MIPI_PORT(port, _MIPIA_INTR_STAT, \
-					_MIPIC_INTR_STAT)
+#define MIPI_INTR_STAT(port)		_MMIO_MIPI(port, _MIPIA_INTR_STAT, _MIPIC_INTR_STAT)
 #define _MIPIA_INTR_EN			(dev_priv->mipi_mmio_base + 0xb008)
 #define _MIPIC_INTR_EN			(dev_priv->mipi_mmio_base + 0xb808)
-#define MIPI_INTR_EN(port)		_MIPI_PORT(port, _MIPIA_INTR_EN, \
-					_MIPIC_INTR_EN)
+#define MIPI_INTR_EN(port)		_MMIO_MIPI(port, _MIPIA_INTR_EN, _MIPIC_INTR_EN)
 #define  TEARING_EFFECT					(1 << 31)
 #define  SPL_PKT_SENT_INTERRUPT				(1 << 30)
 #define  GEN_READ_DATA_AVAIL				(1 << 29)
@@ -7782,8 +7834,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_DSI_FUNC_PRG		(dev_priv->mipi_mmio_base + 0xb00c)
 #define _MIPIC_DSI_FUNC_PRG		(dev_priv->mipi_mmio_base + 0xb80c)
-#define MIPI_DSI_FUNC_PRG(port)		_MIPI_PORT(port, _MIPIA_DSI_FUNC_PRG, \
-						_MIPIC_DSI_FUNC_PRG)
+#define MIPI_DSI_FUNC_PRG(port)		_MMIO_MIPI(port, _MIPIA_DSI_FUNC_PRG, _MIPIC_DSI_FUNC_PRG)
 #define  CMD_MODE_DATA_WIDTH_MASK			(7 << 13)
 #define  CMD_MODE_NOT_SUPPORTED				(0 << 13)
 #define  CMD_MODE_DATA_WIDTH_16_BIT			(1 << 13)
@@ -7806,32 +7857,27 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_HS_TX_TIMEOUT		(dev_priv->mipi_mmio_base + 0xb010)
 #define _MIPIC_HS_TX_TIMEOUT		(dev_priv->mipi_mmio_base + 0xb810)
-#define MIPI_HS_TX_TIMEOUT(port)	_MIPI_PORT(port, _MIPIA_HS_TX_TIMEOUT, \
-					_MIPIC_HS_TX_TIMEOUT)
+#define MIPI_HS_TX_TIMEOUT(port)	_MMIO_MIPI(port, _MIPIA_HS_TX_TIMEOUT, _MIPIC_HS_TX_TIMEOUT)
 #define  HIGH_SPEED_TX_TIMEOUT_COUNTER_MASK		0xffffff
 
 #define _MIPIA_LP_RX_TIMEOUT		(dev_priv->mipi_mmio_base + 0xb014)
 #define _MIPIC_LP_RX_TIMEOUT		(dev_priv->mipi_mmio_base + 0xb814)
-#define MIPI_LP_RX_TIMEOUT(port)	_MIPI_PORT(port, _MIPIA_LP_RX_TIMEOUT, \
-					_MIPIC_LP_RX_TIMEOUT)
+#define MIPI_LP_RX_TIMEOUT(port)	_MMIO_MIPI(port, _MIPIA_LP_RX_TIMEOUT, _MIPIC_LP_RX_TIMEOUT)
 #define  LOW_POWER_RX_TIMEOUT_COUNTER_MASK		0xffffff
 
 #define _MIPIA_TURN_AROUND_TIMEOUT	(dev_priv->mipi_mmio_base + 0xb018)
 #define _MIPIC_TURN_AROUND_TIMEOUT	(dev_priv->mipi_mmio_base + 0xb818)
-#define MIPI_TURN_AROUND_TIMEOUT(port)	_MIPI_PORT(port, \
-			_MIPIA_TURN_AROUND_TIMEOUT, _MIPIC_TURN_AROUND_TIMEOUT)
+#define MIPI_TURN_AROUND_TIMEOUT(port)	_MMIO_MIPI(port, _MIPIA_TURN_AROUND_TIMEOUT, _MIPIC_TURN_AROUND_TIMEOUT)
 #define  TURN_AROUND_TIMEOUT_MASK			0x3f
 
 #define _MIPIA_DEVICE_RESET_TIMER	(dev_priv->mipi_mmio_base + 0xb01c)
 #define _MIPIC_DEVICE_RESET_TIMER	(dev_priv->mipi_mmio_base + 0xb81c)
-#define MIPI_DEVICE_RESET_TIMER(port)	_MIPI_PORT(port, \
-			_MIPIA_DEVICE_RESET_TIMER, _MIPIC_DEVICE_RESET_TIMER)
+#define MIPI_DEVICE_RESET_TIMER(port)	_MMIO_MIPI(port, _MIPIA_DEVICE_RESET_TIMER, _MIPIC_DEVICE_RESET_TIMER)
 #define  DEVICE_RESET_TIMER_MASK			0xffff
 
 #define _MIPIA_DPI_RESOLUTION		(dev_priv->mipi_mmio_base + 0xb020)
 #define _MIPIC_DPI_RESOLUTION		(dev_priv->mipi_mmio_base + 0xb820)
-#define MIPI_DPI_RESOLUTION(port)	_MIPI_PORT(port, _MIPIA_DPI_RESOLUTION, \
-					_MIPIC_DPI_RESOLUTION)
+#define MIPI_DPI_RESOLUTION(port)	_MMIO_MIPI(port, _MIPIA_DPI_RESOLUTION, _MIPIC_DPI_RESOLUTION)
 #define  VERTICAL_ADDRESS_SHIFT				16
 #define  VERTICAL_ADDRESS_MASK				(0xffff << 16)
 #define  HORIZONTAL_ADDRESS_SHIFT			0
@@ -7839,8 +7885,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_DBI_FIFO_THROTTLE	(dev_priv->mipi_mmio_base + 0xb024)
 #define _MIPIC_DBI_FIFO_THROTTLE	(dev_priv->mipi_mmio_base + 0xb824)
-#define MIPI_DBI_FIFO_THROTTLE(port)	_MIPI_PORT(port, \
-			_MIPIA_DBI_FIFO_THROTTLE, _MIPIC_DBI_FIFO_THROTTLE)
+#define MIPI_DBI_FIFO_THROTTLE(port)	_MMIO_MIPI(port, _MIPIA_DBI_FIFO_THROTTLE, _MIPIC_DBI_FIFO_THROTTLE)
 #define  DBI_FIFO_EMPTY_HALF				(0 << 0)
 #define  DBI_FIFO_EMPTY_QUARTER				(1 << 0)
 #define  DBI_FIFO_EMPTY_7_LOCATIONS			(2 << 0)
@@ -7848,50 +7893,41 @@ enum skl_disp_power_wells {
 /* regs below are bits 15:0 */
 #define _MIPIA_HSYNC_PADDING_COUNT	(dev_priv->mipi_mmio_base + 0xb028)
 #define _MIPIC_HSYNC_PADDING_COUNT	(dev_priv->mipi_mmio_base + 0xb828)
-#define MIPI_HSYNC_PADDING_COUNT(port)	_MIPI_PORT(port, \
-			_MIPIA_HSYNC_PADDING_COUNT, _MIPIC_HSYNC_PADDING_COUNT)
+#define MIPI_HSYNC_PADDING_COUNT(port)	_MMIO_MIPI(port, _MIPIA_HSYNC_PADDING_COUNT, _MIPIC_HSYNC_PADDING_COUNT)
 
 #define _MIPIA_HBP_COUNT		(dev_priv->mipi_mmio_base + 0xb02c)
 #define _MIPIC_HBP_COUNT		(dev_priv->mipi_mmio_base + 0xb82c)
-#define MIPI_HBP_COUNT(port)		_MIPI_PORT(port, _MIPIA_HBP_COUNT, \
-					_MIPIC_HBP_COUNT)
+#define MIPI_HBP_COUNT(port)		_MMIO_MIPI(port, _MIPIA_HBP_COUNT, _MIPIC_HBP_COUNT)
 
 #define _MIPIA_HFP_COUNT		(dev_priv->mipi_mmio_base + 0xb030)
 #define _MIPIC_HFP_COUNT		(dev_priv->mipi_mmio_base + 0xb830)
-#define MIPI_HFP_COUNT(port)		_MIPI_PORT(port, _MIPIA_HFP_COUNT, \
-					_MIPIC_HFP_COUNT)
+#define MIPI_HFP_COUNT(port)		_MMIO_MIPI(port, _MIPIA_HFP_COUNT, _MIPIC_HFP_COUNT)
 
 #define _MIPIA_HACTIVE_AREA_COUNT	(dev_priv->mipi_mmio_base + 0xb034)
 #define _MIPIC_HACTIVE_AREA_COUNT	(dev_priv->mipi_mmio_base + 0xb834)
-#define MIPI_HACTIVE_AREA_COUNT(port)	_MIPI_PORT(port, \
-			_MIPIA_HACTIVE_AREA_COUNT, _MIPIC_HACTIVE_AREA_COUNT)
+#define MIPI_HACTIVE_AREA_COUNT(port)	_MMIO_MIPI(port, _MIPIA_HACTIVE_AREA_COUNT, _MIPIC_HACTIVE_AREA_COUNT)
 
 #define _MIPIA_VSYNC_PADDING_COUNT	(dev_priv->mipi_mmio_base + 0xb038)
 #define _MIPIC_VSYNC_PADDING_COUNT	(dev_priv->mipi_mmio_base + 0xb838)
-#define MIPI_VSYNC_PADDING_COUNT(port)	_MIPI_PORT(port, \
-			_MIPIA_VSYNC_PADDING_COUNT, _MIPIC_VSYNC_PADDING_COUNT)
+#define MIPI_VSYNC_PADDING_COUNT(port)	_MMIO_MIPI(port, _MIPIA_VSYNC_PADDING_COUNT, _MIPIC_VSYNC_PADDING_COUNT)
 
 #define _MIPIA_VBP_COUNT		(dev_priv->mipi_mmio_base + 0xb03c)
 #define _MIPIC_VBP_COUNT		(dev_priv->mipi_mmio_base + 0xb83c)
-#define MIPI_VBP_COUNT(port)		_MIPI_PORT(port, _MIPIA_VBP_COUNT, \
-					_MIPIC_VBP_COUNT)
+#define MIPI_VBP_COUNT(port)		_MMIO_MIPI(port, _MIPIA_VBP_COUNT, _MIPIC_VBP_COUNT)
 
 #define _MIPIA_VFP_COUNT		(dev_priv->mipi_mmio_base + 0xb040)
 #define _MIPIC_VFP_COUNT		(dev_priv->mipi_mmio_base + 0xb840)
-#define MIPI_VFP_COUNT(port)		_MIPI_PORT(port, _MIPIA_VFP_COUNT, \
-					_MIPIC_VFP_COUNT)
+#define MIPI_VFP_COUNT(port)		_MMIO_MIPI(port, _MIPIA_VFP_COUNT, _MIPIC_VFP_COUNT)
 
 #define _MIPIA_HIGH_LOW_SWITCH_COUNT	(dev_priv->mipi_mmio_base + 0xb044)
 #define _MIPIC_HIGH_LOW_SWITCH_COUNT	(dev_priv->mipi_mmio_base + 0xb844)
-#define MIPI_HIGH_LOW_SWITCH_COUNT(port)	_MIPI_PORT(port,	\
-		_MIPIA_HIGH_LOW_SWITCH_COUNT, _MIPIC_HIGH_LOW_SWITCH_COUNT)
+#define MIPI_HIGH_LOW_SWITCH_COUNT(port)	_MMIO_MIPI(port,	_MIPIA_HIGH_LOW_SWITCH_COUNT, _MIPIC_HIGH_LOW_SWITCH_COUNT)
 
 /* regs above are bits 15:0 */
 
 #define _MIPIA_DPI_CONTROL		(dev_priv->mipi_mmio_base + 0xb048)
 #define _MIPIC_DPI_CONTROL		(dev_priv->mipi_mmio_base + 0xb848)
-#define MIPI_DPI_CONTROL(port)		_MIPI_PORT(port, _MIPIA_DPI_CONTROL, \
-					_MIPIC_DPI_CONTROL)
+#define MIPI_DPI_CONTROL(port)		_MMIO_MIPI(port, _MIPIA_DPI_CONTROL, _MIPIC_DPI_CONTROL)
 #define  DPI_LP_MODE					(1 << 6)
 #define  BACKLIGHT_OFF					(1 << 5)
 #define  BACKLIGHT_ON					(1 << 4)
@@ -7902,29 +7938,26 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_DPI_DATA			(dev_priv->mipi_mmio_base + 0xb04c)
 #define _MIPIC_DPI_DATA			(dev_priv->mipi_mmio_base + 0xb84c)
-#define MIPI_DPI_DATA(port)		_MIPI_PORT(port, _MIPIA_DPI_DATA, \
-					_MIPIC_DPI_DATA)
+#define MIPI_DPI_DATA(port)		_MMIO_MIPI(port, _MIPIA_DPI_DATA, _MIPIC_DPI_DATA)
 #define  COMMAND_BYTE_SHIFT				0
 #define  COMMAND_BYTE_MASK				(0x3f << 0)
 
 #define _MIPIA_INIT_COUNT		(dev_priv->mipi_mmio_base + 0xb050)
 #define _MIPIC_INIT_COUNT		(dev_priv->mipi_mmio_base + 0xb850)
-#define MIPI_INIT_COUNT(port)		_MIPI_PORT(port, _MIPIA_INIT_COUNT, \
-					_MIPIC_INIT_COUNT)
+#define MIPI_INIT_COUNT(port)		_MMIO_MIPI(port, _MIPIA_INIT_COUNT, _MIPIC_INIT_COUNT)
 #define  MASTER_INIT_TIMER_SHIFT			0
 #define  MASTER_INIT_TIMER_MASK				(0xffff << 0)
 
 #define _MIPIA_MAX_RETURN_PKT_SIZE	(dev_priv->mipi_mmio_base + 0xb054)
 #define _MIPIC_MAX_RETURN_PKT_SIZE	(dev_priv->mipi_mmio_base + 0xb854)
-#define MIPI_MAX_RETURN_PKT_SIZE(port)	_MIPI_PORT(port, \
+#define MIPI_MAX_RETURN_PKT_SIZE(port)	_MMIO_MIPI(port, \
 			_MIPIA_MAX_RETURN_PKT_SIZE, _MIPIC_MAX_RETURN_PKT_SIZE)
 #define  MAX_RETURN_PKT_SIZE_SHIFT			0
 #define  MAX_RETURN_PKT_SIZE_MASK			(0x3ff << 0)
 
 #define _MIPIA_VIDEO_MODE_FORMAT	(dev_priv->mipi_mmio_base + 0xb058)
 #define _MIPIC_VIDEO_MODE_FORMAT	(dev_priv->mipi_mmio_base + 0xb858)
-#define MIPI_VIDEO_MODE_FORMAT(port)	_MIPI_PORT(port, \
-			_MIPIA_VIDEO_MODE_FORMAT, _MIPIC_VIDEO_MODE_FORMAT)
+#define MIPI_VIDEO_MODE_FORMAT(port)	_MMIO_MIPI(port, _MIPIA_VIDEO_MODE_FORMAT, _MIPIC_VIDEO_MODE_FORMAT)
 #define  RANDOM_DPI_DISPLAY_RESOLUTION			(1 << 4)
 #define  DISABLE_VIDEO_BTA				(1 << 3)
 #define  IP_TG_CONFIG					(1 << 2)
@@ -7934,8 +7967,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_EOT_DISABLE		(dev_priv->mipi_mmio_base + 0xb05c)
 #define _MIPIC_EOT_DISABLE		(dev_priv->mipi_mmio_base + 0xb85c)
-#define MIPI_EOT_DISABLE(port)		_MIPI_PORT(port, _MIPIA_EOT_DISABLE, \
-					_MIPIC_EOT_DISABLE)
+#define MIPI_EOT_DISABLE(port)		_MMIO_MIPI(port, _MIPIA_EOT_DISABLE, _MIPIC_EOT_DISABLE)
 #define  LP_RX_TIMEOUT_ERROR_RECOVERY_DISABLE		(1 << 7)
 #define  HS_RX_TIMEOUT_ERROR_RECOVERY_DISABLE		(1 << 6)
 #define  LOW_CONTENTION_RECOVERY_DISABLE		(1 << 5)
@@ -7947,31 +7979,26 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_LP_BYTECLK		(dev_priv->mipi_mmio_base + 0xb060)
 #define _MIPIC_LP_BYTECLK		(dev_priv->mipi_mmio_base + 0xb860)
-#define MIPI_LP_BYTECLK(port)		_MIPI_PORT(port, _MIPIA_LP_BYTECLK, \
-					_MIPIC_LP_BYTECLK)
+#define MIPI_LP_BYTECLK(port)		_MMIO_MIPI(port, _MIPIA_LP_BYTECLK, _MIPIC_LP_BYTECLK)
 #define  LP_BYTECLK_SHIFT				0
 #define  LP_BYTECLK_MASK				(0xffff << 0)
 
 /* bits 31:0 */
 #define _MIPIA_LP_GEN_DATA		(dev_priv->mipi_mmio_base + 0xb064)
 #define _MIPIC_LP_GEN_DATA		(dev_priv->mipi_mmio_base + 0xb864)
-#define MIPI_LP_GEN_DATA(port)		_MIPI_PORT(port, _MIPIA_LP_GEN_DATA, \
-					_MIPIC_LP_GEN_DATA)
+#define MIPI_LP_GEN_DATA(port)		_MMIO_MIPI(port, _MIPIA_LP_GEN_DATA, _MIPIC_LP_GEN_DATA)
 
 /* bits 31:0 */
 #define _MIPIA_HS_GEN_DATA		(dev_priv->mipi_mmio_base + 0xb068)
 #define _MIPIC_HS_GEN_DATA		(dev_priv->mipi_mmio_base + 0xb868)
-#define MIPI_HS_GEN_DATA(port)		_MIPI_PORT(port, _MIPIA_HS_GEN_DATA, \
-					_MIPIC_HS_GEN_DATA)
+#define MIPI_HS_GEN_DATA(port)		_MMIO_MIPI(port, _MIPIA_HS_GEN_DATA, _MIPIC_HS_GEN_DATA)
 
 #define _MIPIA_LP_GEN_CTRL		(dev_priv->mipi_mmio_base + 0xb06c)
 #define _MIPIC_LP_GEN_CTRL		(dev_priv->mipi_mmio_base + 0xb86c)
-#define MIPI_LP_GEN_CTRL(port)		_MIPI_PORT(port, _MIPIA_LP_GEN_CTRL, \
-					_MIPIC_LP_GEN_CTRL)
+#define MIPI_LP_GEN_CTRL(port)		_MMIO_MIPI(port, _MIPIA_LP_GEN_CTRL, _MIPIC_LP_GEN_CTRL)
 #define _MIPIA_HS_GEN_CTRL		(dev_priv->mipi_mmio_base + 0xb070)
 #define _MIPIC_HS_GEN_CTRL		(dev_priv->mipi_mmio_base + 0xb870)
-#define MIPI_HS_GEN_CTRL(port)		_MIPI_PORT(port, _MIPIA_HS_GEN_CTRL, \
-					_MIPIC_HS_GEN_CTRL)
+#define MIPI_HS_GEN_CTRL(port)		_MMIO_MIPI(port, _MIPIA_HS_GEN_CTRL, _MIPIC_HS_GEN_CTRL)
 #define  LONG_PACKET_WORD_COUNT_SHIFT			8
 #define  LONG_PACKET_WORD_COUNT_MASK			(0xffff << 8)
 #define  SHORT_PACKET_PARAM_SHIFT			8
@@ -7984,8 +8011,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_GEN_FIFO_STAT		(dev_priv->mipi_mmio_base + 0xb074)
 #define _MIPIC_GEN_FIFO_STAT		(dev_priv->mipi_mmio_base + 0xb874)
-#define MIPI_GEN_FIFO_STAT(port)	_MIPI_PORT(port, _MIPIA_GEN_FIFO_STAT, \
-					_MIPIC_GEN_FIFO_STAT)
+#define MIPI_GEN_FIFO_STAT(port)	_MMIO_MIPI(port, _MIPIA_GEN_FIFO_STAT, _MIPIC_GEN_FIFO_STAT)
 #define  DPI_FIFO_EMPTY					(1 << 28)
 #define  DBI_FIFO_EMPTY					(1 << 27)
 #define  LP_CTRL_FIFO_EMPTY				(1 << 26)
@@ -8003,16 +8029,14 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_HS_LS_DBI_ENABLE		(dev_priv->mipi_mmio_base + 0xb078)
 #define _MIPIC_HS_LS_DBI_ENABLE		(dev_priv->mipi_mmio_base + 0xb878)
-#define MIPI_HS_LP_DBI_ENABLE(port)	_MIPI_PORT(port, \
-			_MIPIA_HS_LS_DBI_ENABLE, _MIPIC_HS_LS_DBI_ENABLE)
+#define MIPI_HS_LP_DBI_ENABLE(port)	_MMIO_MIPI(port, _MIPIA_HS_LS_DBI_ENABLE, _MIPIC_HS_LS_DBI_ENABLE)
 #define  DBI_HS_LP_MODE_MASK				(1 << 0)
 #define  DBI_LP_MODE					(1 << 0)
 #define  DBI_HS_MODE					(0 << 0)
 
 #define _MIPIA_DPHY_PARAM		(dev_priv->mipi_mmio_base + 0xb080)
 #define _MIPIC_DPHY_PARAM		(dev_priv->mipi_mmio_base + 0xb880)
-#define MIPI_DPHY_PARAM(port)		_MIPI_PORT(port, _MIPIA_DPHY_PARAM, \
-					_MIPIC_DPHY_PARAM)
+#define MIPI_DPHY_PARAM(port)		_MMIO_MIPI(port, _MIPIA_DPHY_PARAM, _MIPIC_DPHY_PARAM)
 #define  EXIT_ZERO_COUNT_SHIFT				24
 #define  EXIT_ZERO_COUNT_MASK				(0x3f << 24)
 #define  TRAIL_COUNT_SHIFT				16
@@ -8025,15 +8049,11 @@ enum skl_disp_power_wells {
 /* bits 31:0 */
 #define _MIPIA_DBI_BW_CTRL		(dev_priv->mipi_mmio_base + 0xb084)
 #define _MIPIC_DBI_BW_CTRL		(dev_priv->mipi_mmio_base + 0xb884)
-#define MIPI_DBI_BW_CTRL(port)		_MIPI_PORT(port, _MIPIA_DBI_BW_CTRL, \
-					_MIPIC_DBI_BW_CTRL)
-
-#define _MIPIA_CLK_LANE_SWITCH_TIME_CNT		(dev_priv->mipi_mmio_base \
-							+ 0xb088)
-#define _MIPIC_CLK_LANE_SWITCH_TIME_CNT		(dev_priv->mipi_mmio_base \
-							+ 0xb888)
-#define MIPI_CLK_LANE_SWITCH_TIME_CNT(port)	_MIPI_PORT(port, \
-	_MIPIA_CLK_LANE_SWITCH_TIME_CNT, _MIPIC_CLK_LANE_SWITCH_TIME_CNT)
+#define MIPI_DBI_BW_CTRL(port)		_MMIO_MIPI(port, _MIPIA_DBI_BW_CTRL, _MIPIC_DBI_BW_CTRL)
+
+#define _MIPIA_CLK_LANE_SWITCH_TIME_CNT		(dev_priv->mipi_mmio_base + 0xb088)
+#define _MIPIC_CLK_LANE_SWITCH_TIME_CNT		(dev_priv->mipi_mmio_base + 0xb888)
+#define MIPI_CLK_LANE_SWITCH_TIME_CNT(port)	_MMIO_MIPI(port, _MIPIA_CLK_LANE_SWITCH_TIME_CNT, _MIPIC_CLK_LANE_SWITCH_TIME_CNT)
 #define  LP_HS_SSW_CNT_SHIFT				16
 #define  LP_HS_SSW_CNT_MASK				(0xffff << 16)
 #define  HS_LP_PWR_SW_CNT_SHIFT				0
@@ -8041,19 +8061,16 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_STOP_STATE_STALL		(dev_priv->mipi_mmio_base + 0xb08c)
 #define _MIPIC_STOP_STATE_STALL		(dev_priv->mipi_mmio_base + 0xb88c)
-#define MIPI_STOP_STATE_STALL(port)	_MIPI_PORT(port, \
-			_MIPIA_STOP_STATE_STALL, _MIPIC_STOP_STATE_STALL)
+#define MIPI_STOP_STATE_STALL(port)	_MMIO_MIPI(port, _MIPIA_STOP_STATE_STALL, _MIPIC_STOP_STATE_STALL)
 #define  STOP_STATE_STALL_COUNTER_SHIFT			0
 #define  STOP_STATE_STALL_COUNTER_MASK			(0xff << 0)
 
 #define _MIPIA_INTR_STAT_REG_1		(dev_priv->mipi_mmio_base + 0xb090)
 #define _MIPIC_INTR_STAT_REG_1		(dev_priv->mipi_mmio_base + 0xb890)
-#define MIPI_INTR_STAT_REG_1(port)	_MIPI_PORT(port, \
-				_MIPIA_INTR_STAT_REG_1, _MIPIC_INTR_STAT_REG_1)
+#define MIPI_INTR_STAT_REG_1(port)	_MMIO_MIPI(port, _MIPIA_INTR_STAT_REG_1, _MIPIC_INTR_STAT_REG_1)
 #define _MIPIA_INTR_EN_REG_1		(dev_priv->mipi_mmio_base + 0xb094)
 #define _MIPIC_INTR_EN_REG_1		(dev_priv->mipi_mmio_base + 0xb894)
-#define MIPI_INTR_EN_REG_1(port)	_MIPI_PORT(port, _MIPIA_INTR_EN_REG_1, \
-					_MIPIC_INTR_EN_REG_1)
+#define MIPI_INTR_EN_REG_1(port)	_MMIO_MIPI(port, _MIPIA_INTR_EN_REG_1, _MIPIC_INTR_EN_REG_1)
 #define  RX_CONTENTION_DETECTED				(1 << 0)
 
 /* XXX: only pipe A ?!? */
@@ -8073,8 +8090,7 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_CTRL			(dev_priv->mipi_mmio_base + 0xb104)
 #define _MIPIC_CTRL			(dev_priv->mipi_mmio_base + 0xb904)
-#define MIPI_CTRL(port)			_MIPI_PORT(port, _MIPIA_CTRL, \
-					_MIPIC_CTRL)
+#define MIPI_CTRL(port)			_MMIO_MIPI(port, _MIPIA_CTRL, _MIPIC_CTRL)
 #define  ESCAPE_CLOCK_DIVIDER_SHIFT			5 /* A only */
 #define  ESCAPE_CLOCK_DIVIDER_MASK			(3 << 5)
 #define  ESCAPE_CLOCK_DIVIDER_1				(0 << 5)
@@ -8087,29 +8103,24 @@ enum skl_disp_power_wells {
 #define  RGB_FLIP_TO_BGR				(1 << 2)
 
 #define  BXT_PIPE_SELECT_MASK				(7 << 7)
-#define  BXT_PIPE_SELECT_C				(2 << 7)
-#define  BXT_PIPE_SELECT_B				(1 << 7)
-#define  BXT_PIPE_SELECT_A				(0 << 7)
+#define  BXT_PIPE_SELECT(pipe)				((pipe) << 7)
 
 #define _MIPIA_DATA_ADDRESS		(dev_priv->mipi_mmio_base + 0xb108)
 #define _MIPIC_DATA_ADDRESS		(dev_priv->mipi_mmio_base + 0xb908)
-#define MIPI_DATA_ADDRESS(port)		_MIPI_PORT(port, _MIPIA_DATA_ADDRESS, \
-					_MIPIC_DATA_ADDRESS)
+#define MIPI_DATA_ADDRESS(port)		_MMIO_MIPI(port, _MIPIA_DATA_ADDRESS, _MIPIC_DATA_ADDRESS)
 #define  DATA_MEM_ADDRESS_SHIFT				5
 #define  DATA_MEM_ADDRESS_MASK				(0x7ffffff << 5)
 #define  DATA_VALID					(1 << 0)
 
 #define _MIPIA_DATA_LENGTH		(dev_priv->mipi_mmio_base + 0xb10c)
 #define _MIPIC_DATA_LENGTH		(dev_priv->mipi_mmio_base + 0xb90c)
-#define MIPI_DATA_LENGTH(port)		_MIPI_PORT(port, _MIPIA_DATA_LENGTH, \
-					_MIPIC_DATA_LENGTH)
+#define MIPI_DATA_LENGTH(port)		_MMIO_MIPI(port, _MIPIA_DATA_LENGTH, _MIPIC_DATA_LENGTH)
 #define  DATA_LENGTH_SHIFT				0
 #define  DATA_LENGTH_MASK				(0xfffff << 0)
 
 #define _MIPIA_COMMAND_ADDRESS		(dev_priv->mipi_mmio_base + 0xb110)
 #define _MIPIC_COMMAND_ADDRESS		(dev_priv->mipi_mmio_base + 0xb910)
-#define MIPI_COMMAND_ADDRESS(port)	_MIPI_PORT(port, \
-				_MIPIA_COMMAND_ADDRESS, _MIPIC_COMMAND_ADDRESS)
+#define MIPI_COMMAND_ADDRESS(port)	_MMIO_MIPI(port, _MIPIA_COMMAND_ADDRESS, _MIPIC_COMMAND_ADDRESS)
 #define  COMMAND_MEM_ADDRESS_SHIFT			5
 #define  COMMAND_MEM_ADDRESS_MASK			(0x7ffffff << 5)
 #define  AUTO_PWG_ENABLE				(1 << 2)
@@ -8118,21 +8129,17 @@ enum skl_disp_power_wells {
 
 #define _MIPIA_COMMAND_LENGTH		(dev_priv->mipi_mmio_base + 0xb114)
 #define _MIPIC_COMMAND_LENGTH		(dev_priv->mipi_mmio_base + 0xb914)
-#define MIPI_COMMAND_LENGTH(port)	_MIPI_PORT(port, _MIPIA_COMMAND_LENGTH, \
-					_MIPIC_COMMAND_LENGTH)
+#define MIPI_COMMAND_LENGTH(port)	_MMIO_MIPI(port, _MIPIA_COMMAND_LENGTH, _MIPIC_COMMAND_LENGTH)
 #define  COMMAND_LENGTH_SHIFT(n)			(8 * (n)) /* n: 0...3 */
 #define  COMMAND_LENGTH_MASK(n)				(0xff << (8 * (n)))
 
 #define _MIPIA_READ_DATA_RETURN0	(dev_priv->mipi_mmio_base + 0xb118)
 #define _MIPIC_READ_DATA_RETURN0	(dev_priv->mipi_mmio_base + 0xb918)
-#define MIPI_READ_DATA_RETURN(port, n) \
-	(_MIPI_PORT(port, _MIPIA_READ_DATA_RETURN0, _MIPIC_READ_DATA_RETURN0) \
-					+ 4 * (n)) /* n: 0...7 */
+#define MIPI_READ_DATA_RETURN(port, n) _MMIO(_MIPI(port, _MIPIA_READ_DATA_RETURN0, _MIPIC_READ_DATA_RETURN0) + 4 * (n)) /* n: 0...7 */
 
 #define _MIPIA_READ_DATA_VALID		(dev_priv->mipi_mmio_base + 0xb138)
 #define _MIPIC_READ_DATA_VALID		(dev_priv->mipi_mmio_base + 0xb938)
-#define MIPI_READ_DATA_VALID(port)	_MIPI_PORT(port, \
-				_MIPIA_READ_DATA_VALID, _MIPIC_READ_DATA_VALID)
+#define MIPI_READ_DATA_VALID(port)	_MMIO_MIPI(port, _MIPIA_READ_DATA_VALID, _MIPIC_READ_DATA_VALID)
 #define  READ_DATA_VALID(n)				(1 << (n))
 
 /* For UMS only (deprecated): */
@@ -8140,12 +8147,12 @@ enum skl_disp_power_wells {
 #define _PALETTE_B (dev_priv->info.display_mmio_offset + 0xa800)
 
 /* MOCS (Memory Object Control State) registers */
-#define GEN9_LNCFCMOCS0		0xb020	/* L3 Cache Control base */
+#define GEN9_LNCFCMOCS(i)	_MMIO(0xb020 + (i) * 4)	/* L3 Cache Control */
 
-#define GEN9_GFX_MOCS_0		0xc800	/* Graphics MOCS base register*/
-#define GEN9_MFX0_MOCS_0	0xc900	/* Media 0 MOCS base register*/
-#define GEN9_MFX1_MOCS_0	0xca00	/* Media 1 MOCS base register*/
-#define GEN9_VEBOX_MOCS_0	0xcb00	/* Video MOCS base register*/
-#define GEN9_BLT_MOCS_0		0xcc00	/* Blitter MOCS base register*/
+#define GEN9_GFX_MOCS(i)	_MMIO(0xc800 + (i) * 4)	/* Graphics MOCS registers */
+#define GEN9_MFX0_MOCS(i)	_MMIO(0xc900 + (i) * 4)	/* Media 0 MOCS registers */
+#define GEN9_MFX1_MOCS(i)	_MMIO(0xca00 + (i) * 4)	/* Media 1 MOCS registers */
+#define GEN9_VEBOX_MOCS(i)	_MMIO(0xcb00 + (i) * 4)	/* Video MOCS registers */
+#define GEN9_BLT_MOCS(i)	_MMIO(0xcc00 + (i) * 4)	/* Blitter MOCS registers */
 
 #endif /* _I915_REG_H_ */
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 2d9182189422..a8af594fbd00 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -49,7 +49,7 @@ static void i915_save_display(struct drm_device *dev)
 		dev_priv->regfile.savePP_ON_DELAYS = I915_READ(PCH_PP_ON_DELAYS);
 		dev_priv->regfile.savePP_OFF_DELAYS = I915_READ(PCH_PP_OFF_DELAYS);
 		dev_priv->regfile.savePP_DIVISOR = I915_READ(PCH_PP_DIVISOR);
-	} else if (!IS_VALLEYVIEW(dev)) {
+	} else if (INTEL_INFO(dev)->gen <= 4) {
 		dev_priv->regfile.savePP_CONTROL = I915_READ(PP_CONTROL);
 		dev_priv->regfile.savePP_ON_DELAYS = I915_READ(PP_ON_DELAYS);
 		dev_priv->regfile.savePP_OFF_DELAYS = I915_READ(PP_OFF_DELAYS);
@@ -84,7 +84,7 @@ static void i915_restore_display(struct drm_device *dev)
 		I915_WRITE(PCH_PP_OFF_DELAYS, dev_priv->regfile.savePP_OFF_DELAYS);
 		I915_WRITE(PCH_PP_DIVISOR, dev_priv->regfile.savePP_DIVISOR);
 		I915_WRITE(PCH_PP_CONTROL, dev_priv->regfile.savePP_CONTROL);
-	} else if (!IS_VALLEYVIEW(dev)) {
+	} else if (INTEL_INFO(dev)->gen <= 4) {
 		I915_WRITE(PP_ON_DELAYS, dev_priv->regfile.savePP_ON_DELAYS);
 		I915_WRITE(PP_OFF_DELAYS, dev_priv->regfile.savePP_OFF_DELAYS);
 		I915_WRITE(PP_DIVISOR, dev_priv->regfile.savePP_DIVISOR);
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index 50ce9ce2b269..37e3f0ddf8e0 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -35,7 +35,8 @@
 #define dev_to_drm_minor(d) dev_get_drvdata((d))
 
 #ifdef CONFIG_PM
-static u32 calc_residency(struct drm_device *dev, const u32 reg)
+static u32 calc_residency(struct drm_device *dev,
+			  i915_reg_t reg)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u64 raw_time; /* 32b value may overflow during fixed point math */
@@ -48,7 +49,7 @@ static u32 calc_residency(struct drm_device *dev, const u32 reg)
 	intel_runtime_pm_get(dev_priv);
 
 	/* On VLV and CHV, residency time is in CZ units rather than 1.28us */
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		units = 1;
 		div = dev_priv->czclk_freq;
 
@@ -283,7 +284,7 @@ static ssize_t gt_act_freq_mhz_show(struct device *kdev,
 	intel_runtime_pm_get(dev_priv);
 
 	mutex_lock(&dev_priv->rps.hw_lock);
-	if (IS_VALLEYVIEW(dev_priv->dev)) {
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		u32 freq;
 		freq = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
 		ret = intel_gpu_freq(dev_priv, (freq >> 8) & 0xff);
@@ -597,7 +598,7 @@ void i915_setup_sysfs(struct drm_device *dev)
 		if (ret)
 			DRM_ERROR("RC6p residency sysfs setup failed\n");
 	}
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		ret = sysfs_merge_group(&dev->primary->kdev->kobj,
 					&media_rc6_attr_group);
 		if (ret)
@@ -618,7 +619,7 @@ void i915_setup_sysfs(struct drm_device *dev)
 	}
 
 	ret = 0;
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		ret = sysfs_create_files(&dev->primary->kdev->kobj, vlv_attrs);
 	else if (INTEL_INFO(dev)->gen >= 6)
 		ret = sysfs_create_files(&dev->primary->kdev->kobj, gen6_attrs);
@@ -634,7 +635,7 @@ void i915_setup_sysfs(struct drm_device *dev)
 void i915_teardown_sysfs(struct drm_device *dev)
 {
 	sysfs_remove_bin_file(&dev->primary->kdev->kobj, &error_state_attr);
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		sysfs_remove_files(&dev->primary->kdev->kobj, vlv_attrs);
 	else
 		sysfs_remove_files(&dev->primary->kdev->kobj, gen6_attrs);
diff --git a/drivers/gpu/drm/i915/i915_trace.h b/drivers/gpu/drm/i915/i915_trace.h
index 04fe8491c8b6..52b2d409945d 100644
--- a/drivers/gpu/drm/i915/i915_trace.h
+++ b/drivers/gpu/drm/i915/i915_trace.h
@@ -664,7 +664,7 @@ TRACE_EVENT(i915_flip_complete,
 );
 
 TRACE_EVENT_CONDITION(i915_reg_rw,
-	TP_PROTO(bool write, u32 reg, u64 val, int len, bool trace),
+	TP_PROTO(bool write, i915_reg_t reg, u64 val, int len, bool trace),
 
 	TP_ARGS(write, reg, val, len, trace),
 
@@ -679,7 +679,7 @@ TRACE_EVENT_CONDITION(i915_reg_rw,
 
 	TP_fast_assign(
 		__entry->val = (u64)val;
-		__entry->reg = reg;
+		__entry->reg = i915_mmio_reg_offset(reg);
 		__entry->write = write;
 		__entry->len = len;
 		),
diff --git a/drivers/gpu/drm/i915/i915_vgpu.c b/drivers/gpu/drm/i915/i915_vgpu.c
index 5eee75bff170..dea7429be4d0 100644
--- a/drivers/gpu/drm/i915/i915_vgpu.c
+++ b/drivers/gpu/drm/i915/i915_vgpu.c
@@ -69,13 +69,13 @@ void i915_check_vgpu(struct drm_device *dev)
 	if (!IS_HASWELL(dev))
 		return;
 
-	magic = readq(dev_priv->regs + vgtif_reg(magic));
+	magic = __raw_i915_read64(dev_priv, vgtif_reg(magic));
 	if (magic != VGT_MAGIC)
 		return;
 
 	version = INTEL_VGT_IF_VERSION_ENCODE(
-		readw(dev_priv->regs + vgtif_reg(version_major)),
-		readw(dev_priv->regs + vgtif_reg(version_minor)));
+		__raw_i915_read16(dev_priv, vgtif_reg(version_major)),
+		__raw_i915_read16(dev_priv, vgtif_reg(version_minor)));
 	if (version != INTEL_VGT_IF_VERSION) {
 		DRM_INFO("VGT interface version mismatch!\n");
 		return;
diff --git a/drivers/gpu/drm/i915/i915_vgpu.h b/drivers/gpu/drm/i915/i915_vgpu.h
index 21c97f44d637..3c83b47b5f69 100644
--- a/drivers/gpu/drm/i915/i915_vgpu.h
+++ b/drivers/gpu/drm/i915/i915_vgpu.h
@@ -92,14 +92,10 @@ struct vgt_if {
 	uint32_t g2v_notify;
 	uint32_t rsv6[7];
 
-	uint32_t pdp0_lo;
-	uint32_t pdp0_hi;
-	uint32_t pdp1_lo;
-	uint32_t pdp1_hi;
-	uint32_t pdp2_lo;
-	uint32_t pdp2_hi;
-	uint32_t pdp3_lo;
-	uint32_t pdp3_hi;
+	struct {
+		uint32_t lo;
+		uint32_t hi;
+	} pdp[4];
 
 	uint32_t execlist_context_descriptor_lo;
 	uint32_t execlist_context_descriptor_hi;
@@ -108,7 +104,7 @@ struct vgt_if {
 } __packed;
 
 #define vgtif_reg(x) \
-	(VGT_PVINFO_PAGE + (long)&((struct vgt_if *)NULL)->x)
+	_MMIO((VGT_PVINFO_PAGE + (long)&((struct vgt_if *)NULL)->x))
 
 /* vGPU display status to be used by the host side */
 #define VGT_DRV_DISPLAY_NOT_READY 0
diff --git a/drivers/gpu/drm/i915/intel_atomic.c b/drivers/gpu/drm/i915/intel_atomic.c
index f1975f267710..d0b1c9afa35e 100644
--- a/drivers/gpu/drm/i915/intel_atomic.c
+++ b/drivers/gpu/drm/i915/intel_atomic.c
@@ -94,6 +94,9 @@ intel_crtc_duplicate_state(struct drm_crtc *crtc)
 	__drm_atomic_helper_crtc_duplicate_state(crtc, &crtc_state->base);
 
 	crtc_state->update_pipe = false;
+	crtc_state->disable_lp_wm = false;
+	crtc_state->disable_cxsr = false;
+	crtc_state->wm_changed = false;
 
 	return &crtc_state->base;
 }
@@ -205,8 +208,6 @@ int intel_atomic_setup_scalers(struct drm_device *dev,
 				 * but since this plane is unchanged just do the
 				 * minimum required validation.
 				 */
-				if (plane->type == DRM_PLANE_TYPE_PRIMARY)
-					intel_crtc->atomic.wait_for_flips = true;
 				crtc_state->base.planes_changed = true;
 			}
 
diff --git a/drivers/gpu/drm/i915/intel_atomic_plane.c b/drivers/gpu/drm/i915/intel_atomic_plane.c
index a11980696595..c6bb0fc1edfb 100644
--- a/drivers/gpu/drm/i915/intel_atomic_plane.c
+++ b/drivers/gpu/drm/i915/intel_atomic_plane.c
@@ -84,6 +84,7 @@ intel_plane_duplicate_state(struct drm_plane *plane)
 	state = &intel_state->base;
 
 	__drm_atomic_helper_plane_duplicate_state(plane, state);
+	intel_state->wait_req = NULL;
 
 	return state;
 }
@@ -100,6 +101,7 @@ void
 intel_plane_destroy_state(struct drm_plane *plane,
 			  struct drm_plane_state *state)
 {
+	WARN_ON(state && to_intel_plane_state(state)->wait_req);
 	drm_atomic_helper_plane_destroy_state(plane, state);
 }
 
diff --git a/drivers/gpu/drm/i915/intel_audio.c b/drivers/gpu/drm/i915/intel_audio.c
index 4dccd9b003a1..30f921421b0c 100644
--- a/drivers/gpu/drm/i915/intel_audio.c
+++ b/drivers/gpu/drm/i915/intel_audio.c
@@ -161,9 +161,9 @@ static bool audio_rate_need_prog(struct intel_crtc *crtc,
 }
 
 static bool intel_eld_uptodate(struct drm_connector *connector,
-			       int reg_eldv, uint32_t bits_eldv,
-			       int reg_elda, uint32_t bits_elda,
-			       int reg_edid)
+			       i915_reg_t reg_eldv, uint32_t bits_eldv,
+			       i915_reg_t reg_elda, uint32_t bits_elda,
+			       i915_reg_t reg_edid)
 {
 	struct drm_i915_private *dev_priv = connector->dev->dev_private;
 	uint8_t *eld = connector->eld;
@@ -262,7 +262,8 @@ static void hsw_audio_codec_disable(struct intel_encoder *encoder)
 	tmp |= AUD_CONFIG_N_PROG_ENABLE;
 	tmp &= ~AUD_CONFIG_UPPER_N_MASK;
 	tmp &= ~AUD_CONFIG_LOWER_N_MASK;
-	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT) ||
+	    intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DP_MST))
 		tmp |= AUD_CONFIG_N_VALUE_INDEX;
 	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
 
@@ -364,8 +365,7 @@ static void ilk_audio_codec_disable(struct intel_encoder *encoder)
 	enum port port = intel_dig_port->port;
 	enum pipe pipe = intel_crtc->pipe;
 	uint32_t tmp, eldv;
-	int aud_config;
-	int aud_cntrl_st2;
+	i915_reg_t aud_config, aud_cntrl_st2;
 
 	DRM_DEBUG_KMS("Disable audio codec on port %c, pipe %c\n",
 		      port_name(port), pipe_name(pipe));
@@ -376,7 +376,7 @@ static void ilk_audio_codec_disable(struct intel_encoder *encoder)
 	if (HAS_PCH_IBX(dev_priv->dev)) {
 		aud_config = IBX_AUD_CFG(pipe);
 		aud_cntrl_st2 = IBX_AUD_CNTL_ST2;
-	} else if (IS_VALLEYVIEW(dev_priv)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		aud_config = VLV_AUD_CFG(pipe);
 		aud_cntrl_st2 = VLV_AUD_CNTL_ST2;
 	} else {
@@ -416,10 +416,7 @@ static void ilk_audio_codec_enable(struct drm_connector *connector,
 	uint32_t eldv;
 	uint32_t tmp;
 	int len, i;
-	int hdmiw_hdmiedid;
-	int aud_config;
-	int aud_cntl_st;
-	int aud_cntrl_st2;
+	i915_reg_t hdmiw_hdmiedid, aud_config, aud_cntl_st, aud_cntrl_st2;
 
 	DRM_DEBUG_KMS("Enable audio codec on port %c, pipe %c, %u bytes ELD\n",
 		      port_name(port), pipe_name(pipe), drm_eld_size(eld));
@@ -439,7 +436,8 @@ static void ilk_audio_codec_enable(struct drm_connector *connector,
 		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)) {
+	} else if (IS_VALLEYVIEW(connector->dev) ||
+		   IS_CHERRYVIEW(connector->dev)) {
 		hdmiw_hdmiedid = VLV_HDMIW_HDMIEDID(pipe);
 		aud_config = VLV_AUD_CFG(pipe);
 		aud_cntl_st = VLV_AUD_CNTL_ST(pipe);
@@ -478,7 +476,8 @@ static void ilk_audio_codec_enable(struct drm_connector *connector,
 	tmp &= ~AUD_CONFIG_N_VALUE_INDEX;
 	tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
 	tmp &= ~AUD_CONFIG_PIXEL_CLOCK_HDMI_MASK;
-	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT))
+	if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DISPLAYPORT) ||
+	    intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DP_MST))
 		tmp |= AUD_CONFIG_N_VALUE_INDEX;
 	else
 		tmp |= audio_config_hdmi_pixel_clock(adjusted_mode);
@@ -516,7 +515,8 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
 
 	/* ELD Conn_Type */
 	connector->eld[5] &= ~(3 << 2);
-	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT))
+	if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DISPLAYPORT) ||
+	    intel_pipe_has_type(crtc, INTEL_OUTPUT_DP_MST))
 		connector->eld[5] |= (1 << 2);
 
 	connector->eld[6] = drm_av_sync_delay(connector, adjusted_mode) / 2;
@@ -525,6 +525,12 @@ void intel_audio_codec_enable(struct intel_encoder *intel_encoder)
 		dev_priv->display.audio_codec_enable(connector, intel_encoder,
 						     adjusted_mode);
 
+	mutex_lock(&dev_priv->av_mutex);
+	intel_dig_port->audio_connector = connector;
+	/* referred in audio callbacks */
+	dev_priv->dig_port_map[port] = intel_encoder;
+	mutex_unlock(&dev_priv->av_mutex);
+
 	if (acomp && acomp->audio_ops && acomp->audio_ops->pin_eld_notify)
 		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr, (int) port);
 }
@@ -548,6 +554,11 @@ void intel_audio_codec_disable(struct intel_encoder *intel_encoder)
 	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;
+	mutex_unlock(&dev_priv->av_mutex);
+
 	if (acomp && acomp->audio_ops && acomp->audio_ops->pin_eld_notify)
 		acomp->audio_ops->pin_eld_notify(acomp->audio_ops->audio_ptr, (int) port);
 }
@@ -563,7 +574,7 @@ void intel_init_audio(struct drm_device *dev)
 	if (IS_G4X(dev)) {
 		dev_priv->display.audio_codec_enable = g4x_audio_codec_enable;
 		dev_priv->display.audio_codec_disable = g4x_audio_codec_disable;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		dev_priv->display.audio_codec_enable = ilk_audio_codec_enable;
 		dev_priv->display.audio_codec_disable = ilk_audio_codec_disable;
 	} else if (IS_HASWELL(dev) || INTEL_INFO(dev)->gen >= 8) {
@@ -591,7 +602,7 @@ static void i915_audio_component_codec_wake_override(struct device *dev,
 	struct drm_i915_private *dev_priv = dev_to_i915(dev);
 	u32 tmp;
 
-	if (!IS_SKYLAKE(dev_priv))
+	if (!IS_SKYLAKE(dev_priv) && !IS_KABYLAKE(dev_priv))
 		return;
 
 	/*
@@ -632,44 +643,40 @@ static int i915_audio_component_sync_audio_rate(struct device *dev,
 						int port, int rate)
 {
 	struct drm_i915_private *dev_priv = dev_to_i915(dev);
-	struct drm_device *drm_dev = dev_priv->dev;
 	struct intel_encoder *intel_encoder;
-	struct intel_digital_port *intel_dig_port;
 	struct intel_crtc *crtc;
 	struct drm_display_mode *mode;
 	struct i915_audio_component *acomp = dev_priv->audio_component;
-	enum pipe pipe = -1;
+	enum pipe pipe = INVALID_PIPE;
 	u32 tmp;
 	int n;
+	int err = 0;
 
-	/* HSW, BDW SKL need this fix */
+	/* HSW, BDW, SKL, KBL need this fix */
 	if (!IS_SKYLAKE(dev_priv) &&
-		!IS_BROADWELL(dev_priv) &&
-		!IS_HASWELL(dev_priv))
+	    !IS_KABYLAKE(dev_priv) &&
+	    !IS_BROADWELL(dev_priv) &&
+	    !IS_HASWELL(dev_priv))
 		return 0;
 
 	mutex_lock(&dev_priv->av_mutex);
 	/* 1. get the pipe */
-	for_each_intel_encoder(drm_dev, intel_encoder) {
-		if (intel_encoder->type != INTEL_OUTPUT_HDMI)
-			continue;
-		intel_dig_port = enc_to_dig_port(&intel_encoder->base);
-		if (port == intel_dig_port->port) {
-			crtc = to_intel_crtc(intel_encoder->base.crtc);
-			if (!crtc) {
-				DRM_DEBUG_KMS("%s: crtc is NULL\n", __func__);
-				continue;
-			}
-			pipe = crtc->pipe;
-			break;
-		}
+	intel_encoder = dev_priv->dig_port_map[port];
+	/* intel_encoder might be NULL for DP MST */
+	if (!intel_encoder || !intel_encoder->base.crtc ||
+	    intel_encoder->type != INTEL_OUTPUT_HDMI) {
+		DRM_DEBUG_KMS("no valid port %c\n", port_name(port));
+		err = -ENODEV;
+		goto unlock;
 	}
-
+	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));
-		mutex_unlock(&dev_priv->av_mutex);
-		return -ENODEV;
+		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;
@@ -682,8 +689,7 @@ static int i915_audio_component_sync_audio_rate(struct device *dev,
 		tmp = I915_READ(HSW_AUD_CFG(pipe));
 		tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
 		I915_WRITE(HSW_AUD_CFG(pipe), tmp);
-		mutex_unlock(&dev_priv->av_mutex);
-		return 0;
+		goto unlock;
 	}
 
 	n = audio_config_get_n(mode, rate);
@@ -693,8 +699,7 @@ static int i915_audio_component_sync_audio_rate(struct device *dev,
 		tmp = I915_READ(HSW_AUD_CFG(pipe));
 		tmp &= ~AUD_CONFIG_N_PROG_ENABLE;
 		I915_WRITE(HSW_AUD_CFG(pipe), tmp);
-		mutex_unlock(&dev_priv->av_mutex);
-		return 0;
+		goto unlock;
 	}
 
 	/* 3. set the N/CTS/M */
@@ -702,8 +707,37 @@ static int i915_audio_component_sync_audio_rate(struct device *dev,
 	tmp = audio_config_setup_n_reg(n, tmp);
 	I915_WRITE(HSW_AUD_CFG(pipe), tmp);
 
+ unlock:
 	mutex_unlock(&dev_priv->av_mutex);
-	return 0;
+	return err;
+}
+
+static int i915_audio_component_get_eld(struct device *dev, int port,
+					bool *enabled,
+					unsigned char *buf, int max_bytes)
+{
+	struct drm_i915_private *dev_priv = dev_to_i915(dev);
+	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));
+		}
+	}
+
+	mutex_unlock(&dev_priv->av_mutex);
+	return ret;
 }
 
 static const struct i915_audio_component_ops i915_audio_component_ops = {
@@ -713,6 +747,7 @@ static const struct i915_audio_component_ops i915_audio_component_ops = {
 	.codec_wake_override = i915_audio_component_codec_wake_override,
 	.get_cdclk_freq	= i915_audio_component_get_cdclk_freq,
 	.sync_audio_rate = i915_audio_component_sync_audio_rate,
+	.get_eld	= i915_audio_component_get_eld,
 };
 
 static int i915_audio_component_bind(struct device *i915_dev,
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index ce82f9c7df24..eba3e0f87181 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -24,7 +24,7 @@
  *    Eric Anholt <eric@anholt.net>
  *
  */
-#include <linux/dmi.h>
+
 #include <drm/drm_dp_helper.h>
 #include <drm/drmP.h>
 #include <drm/i915_drm.h>
@@ -332,10 +332,10 @@ parse_sdvo_panel_data(struct drm_i915_private *dev_priv,
 	drm_mode_debug_printmodeline(panel_fixed_mode);
 }
 
-static int intel_bios_ssc_frequency(struct drm_device *dev,
+static int intel_bios_ssc_frequency(struct drm_i915_private *dev_priv,
 				    bool alternate)
 {
-	switch (INTEL_INFO(dev)->gen) {
+	switch (INTEL_INFO(dev_priv)->gen) {
 	case 2:
 		return alternate ? 66667 : 48000;
 	case 3:
@@ -350,26 +350,29 @@ static void
 parse_general_features(struct drm_i915_private *dev_priv,
 		       const struct bdb_header *bdb)
 {
-	struct drm_device *dev = dev_priv->dev;
 	const struct bdb_general_features *general;
 
 	general = find_section(bdb, BDB_GENERAL_FEATURES);
-	if (general) {
-		dev_priv->vbt.int_tv_support = general->int_tv_support;
+	if (!general)
+		return;
+
+	dev_priv->vbt.int_tv_support = general->int_tv_support;
+	/* int_crt_support can't be trusted on earlier platforms */
+	if (bdb->version >= 155 &&
+	    (HAS_DDI(dev_priv) || IS_VALLEYVIEW(dev_priv)))
 		dev_priv->vbt.int_crt_support = general->int_crt_support;
-		dev_priv->vbt.lvds_use_ssc = general->enable_ssc;
-		dev_priv->vbt.lvds_ssc_freq =
-			intel_bios_ssc_frequency(dev, general->ssc_freq);
-		dev_priv->vbt.display_clock_mode = general->display_clock_mode;
-		dev_priv->vbt.fdi_rx_polarity_inverted = general->fdi_rx_polarity_inverted;
-		DRM_DEBUG_KMS("BDB_GENERAL_FEATURES int_tv_support %d int_crt_support %d lvds_use_ssc %d lvds_ssc_freq %d display_clock_mode %d fdi_rx_polarity_inverted %d\n",
-			      dev_priv->vbt.int_tv_support,
-			      dev_priv->vbt.int_crt_support,
-			      dev_priv->vbt.lvds_use_ssc,
-			      dev_priv->vbt.lvds_ssc_freq,
-			      dev_priv->vbt.display_clock_mode,
-			      dev_priv->vbt.fdi_rx_polarity_inverted);
-	}
+	dev_priv->vbt.lvds_use_ssc = general->enable_ssc;
+	dev_priv->vbt.lvds_ssc_freq =
+		intel_bios_ssc_frequency(dev_priv, general->ssc_freq);
+	dev_priv->vbt.display_clock_mode = general->display_clock_mode;
+	dev_priv->vbt.fdi_rx_polarity_inverted = general->fdi_rx_polarity_inverted;
+	DRM_DEBUG_KMS("BDB_GENERAL_FEATURES int_tv_support %d int_crt_support %d lvds_use_ssc %d lvds_ssc_freq %d display_clock_mode %d fdi_rx_polarity_inverted %d\n",
+		      dev_priv->vbt.int_tv_support,
+		      dev_priv->vbt.int_crt_support,
+		      dev_priv->vbt.lvds_use_ssc,
+		      dev_priv->vbt.lvds_ssc_freq,
+		      dev_priv->vbt.display_clock_mode,
+		      dev_priv->vbt.fdi_rx_polarity_inverted);
 }
 
 static void
@@ -1054,10 +1057,9 @@ static void parse_ddi_port(struct drm_i915_private *dev_priv, enum port port,
 static void parse_ddi_ports(struct drm_i915_private *dev_priv,
 			    const struct bdb_header *bdb)
 {
-	struct drm_device *dev = dev_priv->dev;
 	enum port port;
 
-	if (!HAS_DDI(dev))
+	if (!HAS_DDI(dev_priv))
 		return;
 
 	if (!dev_priv->vbt.child_dev_num)
@@ -1170,7 +1172,6 @@ parse_device_mapping(struct drm_i915_private *dev_priv,
 static void
 init_vbt_defaults(struct drm_i915_private *dev_priv)
 {
-	struct drm_device *dev = dev_priv->dev;
 	enum port port;
 
 	dev_priv->vbt.crt_ddc_pin = GMBUS_PIN_VGADDC;
@@ -1195,8 +1196,8 @@ init_vbt_defaults(struct drm_i915_private *dev_priv)
 	 * Core/SandyBridge/IvyBridge use alternative (120MHz) reference
 	 * clock for LVDS.
 	 */
-	dev_priv->vbt.lvds_ssc_freq = intel_bios_ssc_frequency(dev,
-			!HAS_PCH_SPLIT(dev));
+	dev_priv->vbt.lvds_ssc_freq = intel_bios_ssc_frequency(dev_priv,
+			!HAS_PCH_SPLIT(dev_priv));
 	DRM_DEBUG_KMS("Set default to SSC at %d kHz\n", dev_priv->vbt.lvds_ssc_freq);
 
 	for (port = PORT_A; port < I915_MAX_PORTS; port++) {
@@ -1211,88 +1212,79 @@ init_vbt_defaults(struct drm_i915_private *dev_priv)
 	}
 }
 
-static int intel_no_opregion_vbt_callback(const struct dmi_system_id *id)
+static const struct bdb_header *get_bdb_header(const struct vbt_header *vbt)
 {
-	DRM_DEBUG_KMS("Falling back to manually reading VBT from "
-		      "VBIOS ROM for %s\n",
-		      id->ident);
-	return 1;
+	const void *_vbt = vbt;
+
+	return _vbt + vbt->bdb_offset;
 }
 
-static const struct dmi_system_id intel_no_opregion_vbt[] = {
-	{
-		.callback = intel_no_opregion_vbt_callback,
-		.ident = "ThinkCentre A57",
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "97027RG"),
-		},
-	},
-	{ }
-};
-
-static const struct bdb_header *validate_vbt(const void *base,
-					     size_t size,
-					     const void *_vbt,
-					     const char *source)
+/**
+ * intel_bios_is_valid_vbt - does the given buffer contain a valid VBT
+ * @buf:	pointer to a buffer to validate
+ * @size:	size of the buffer
+ *
+ * Returns true on valid VBT.
+ */
+bool intel_bios_is_valid_vbt(const void *buf, size_t size)
 {
-	size_t offset = _vbt - base;
-	const struct vbt_header *vbt = _vbt;
+	const struct vbt_header *vbt = buf;
 	const struct bdb_header *bdb;
 
-	if (offset + sizeof(struct vbt_header) > size) {
+	if (!vbt)
+		return false;
+
+	if (sizeof(struct vbt_header) > size) {
 		DRM_DEBUG_DRIVER("VBT header incomplete\n");
-		return NULL;
+		return false;
 	}
 
 	if (memcmp(vbt->signature, "$VBT", 4)) {
 		DRM_DEBUG_DRIVER("VBT invalid signature\n");
-		return NULL;
+		return false;
 	}
 
-	offset += vbt->bdb_offset;
-	if (offset + sizeof(struct bdb_header) > size) {
+	if (vbt->bdb_offset + sizeof(struct bdb_header) > size) {
 		DRM_DEBUG_DRIVER("BDB header incomplete\n");
-		return NULL;
+		return false;
 	}
 
-	bdb = base + offset;
-	if (offset + bdb->bdb_size > size) {
+	bdb = get_bdb_header(vbt);
+	if (vbt->bdb_offset + bdb->bdb_size > size) {
 		DRM_DEBUG_DRIVER("BDB incomplete\n");
-		return NULL;
+		return false;
 	}
 
-	DRM_DEBUG_KMS("Using VBT from %s: %20s\n",
-		      source, vbt->signature);
-	return bdb;
+	return vbt;
 }
 
-static const struct bdb_header *find_vbt(void __iomem *bios, size_t size)
+static const struct vbt_header *find_vbt(void __iomem *bios, size_t size)
 {
-	const struct bdb_header *bdb = NULL;
 	size_t i;
 
 	/* Scour memory looking for the VBT signature. */
 	for (i = 0; i + 4 < size; i++) {
-		if (ioread32(bios + i) == *((const u32 *) "$VBT")) {
-			/*
-			 * This is the one place where we explicitly discard the
-			 * address space (__iomem) of the BIOS/VBT. From now on
-			 * everything is based on 'base', and treated as regular
-			 * memory.
-			 */
-			void *_bios = (void __force *) bios;
+		void *vbt;
 
-			bdb = validate_vbt(_bios, size, _bios + i, "PCI ROM");
-			break;
-		}
+		if (ioread32(bios + i) != *((const u32 *) "$VBT"))
+			continue;
+
+		/*
+		 * This is the one place where we explicitly discard the address
+		 * space (__iomem) of the BIOS/VBT.
+		 */
+		vbt = (void __force *) bios + i;
+		if (intel_bios_is_valid_vbt(vbt, size - i))
+			return vbt;
+
+		break;
 	}
 
-	return bdb;
+	return NULL;
 }
 
 /**
- * intel_parse_bios - find VBT and initialize settings from the BIOS
+ * intel_bios_init - find VBT and initialize settings from the BIOS
  * @dev: DRM device
  *
  * Loads the Video BIOS and checks that the VBT exists.  Sets scratch registers
@@ -1301,37 +1293,39 @@ static const struct bdb_header *find_vbt(void __iomem *bios, size_t size)
  * Returns 0 on success, nonzero on failure.
  */
 int
-intel_parse_bios(struct drm_device *dev)
+intel_bios_init(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct pci_dev *pdev = dev->pdev;
-	const struct bdb_header *bdb = NULL;
+	struct pci_dev *pdev = dev_priv->dev->pdev;
+	const struct vbt_header *vbt = dev_priv->opregion.vbt;
+	const struct bdb_header *bdb;
 	u8 __iomem *bios = NULL;
 
-	if (HAS_PCH_NOP(dev))
+	if (HAS_PCH_NOP(dev_priv))
 		return -ENODEV;
 
 	init_vbt_defaults(dev_priv);
 
-	/* XXX Should this validation be moved to intel_opregion.c? */
-	if (!dmi_check_system(intel_no_opregion_vbt) && dev_priv->opregion.vbt)
-		bdb = validate_vbt(dev_priv->opregion.header, OPREGION_SIZE,
-				   dev_priv->opregion.vbt, "OpRegion");
-
-	if (bdb == NULL) {
+	if (!vbt) {
 		size_t size;
 
 		bios = pci_map_rom(pdev, &size);
 		if (!bios)
 			return -1;
 
-		bdb = find_vbt(bios, size);
-		if (!bdb) {
+		vbt = find_vbt(bios, size);
+		if (!vbt) {
 			pci_unmap_rom(pdev, bios);
 			return -1;
 		}
+
+		DRM_DEBUG_KMS("Found valid VBT in PCI ROM\n");
 	}
 
+	bdb = get_bdb_header(vbt);
+
+	DRM_DEBUG_KMS("VBT signature \"%.*s\", BDB version %d\n",
+		      (int)sizeof(vbt->signature), vbt->signature, bdb->version);
+
 	/* Grab useful general definitions */
 	parse_general_features(dev_priv, bdb);
 	parse_general_definitions(dev_priv, bdb);
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index 7ec8c9aefb84..54eac1003a1e 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -28,8 +28,6 @@
 #ifndef _I830_BIOS_H_
 #define _I830_BIOS_H_
 
-#include <drm/drmP.h>
-
 struct vbt_header {
 	u8 signature[20];		/**< Always starts with 'VBT$' */
 	u16 version;			/**< decimal */
@@ -588,8 +586,6 @@ struct bdb_psr {
 	struct psr_table psr_table[16];
 } __packed;
 
-int intel_parse_bios(struct drm_device *dev);
-
 /*
  * Driver<->VBIOS interaction occurs through scratch bits in
  * GR18 & SWF*.
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 6a2c76e367a5..a7b4a524fadd 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -50,7 +50,7 @@ struct intel_crt {
 	 * encoder's enable/disable callbacks */
 	struct intel_connector *connector;
 	bool force_hotplug_required;
-	u32 adpa_reg;
+	i915_reg_t adpa_reg;
 };
 
 static struct intel_crt *intel_encoder_to_crt(struct intel_encoder *encoder)
@@ -71,22 +71,29 @@ static bool intel_crt_get_hw_state(struct intel_encoder *encoder,
 	struct intel_crt *crt = intel_encoder_to_crt(encoder);
 	enum intel_display_power_domain power_domain;
 	u32 tmp;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	tmp = I915_READ(crt->adpa_reg);
 
 	if (!(tmp & ADPA_DAC_ENABLE))
-		return false;
+		goto out;
 
 	if (HAS_PCH_CPT(dev))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
 
-	return true;
+	ret = true;
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static unsigned int intel_crt_get_flags(struct intel_encoder *encoder)
@@ -480,12 +487,8 @@ intel_crt_load_detect(struct intel_crt *crt)
 	uint32_t vsample;
 	uint32_t vblank, vblank_start, vblank_end;
 	uint32_t dsl;
-	uint32_t bclrpat_reg;
-	uint32_t vtotal_reg;
-	uint32_t vblank_reg;
-	uint32_t vsync_reg;
-	uint32_t pipeconf_reg;
-	uint32_t pipe_dsl_reg;
+	i915_reg_t bclrpat_reg, vtotal_reg,
+		vblank_reg, vsync_reg, pipeconf_reg, pipe_dsl_reg;
 	uint8_t	st00;
 	enum drm_connector_status status;
 
@@ -518,7 +521,7 @@ intel_crt_load_detect(struct intel_crt *crt)
 		/* Wait for next Vblank to substitue
 		 * border color for Color info */
 		intel_wait_for_vblank(dev, pipe);
-		st00 = I915_READ8(VGA_MSR_WRITE);
+		st00 = I915_READ8(_VGA_MSR_WRITE);
 		status = ((st00 & (1 << 4)) != 0) ?
 			connector_status_connected :
 			connector_status_disconnected;
@@ -563,7 +566,7 @@ intel_crt_load_detect(struct intel_crt *crt)
 		do {
 			count++;
 			/* Read the ST00 VGA status register */
-			st00 = I915_READ8(VGA_MSR_WRITE);
+			st00 = I915_READ8(_VGA_MSR_WRITE);
 			if (st00 & (1 << 4))
 				detect++;
 		} while ((I915_READ(pipe_dsl_reg) == dsl));
@@ -781,11 +784,37 @@ void intel_crt_init(struct drm_device *dev)
 	struct intel_crt *crt;
 	struct intel_connector *intel_connector;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	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))
+		adpa_reg = PCH_ADPA;
+	else if (IS_VALLEYVIEW(dev))
+		adpa_reg = VLV_ADPA;
+	else
+		adpa_reg = ADPA;
+
+	adpa = I915_READ(adpa_reg);
+	if ((adpa & ADPA_DAC_ENABLE) == 0) {
+		/*
+		 * On some machines (some IVB at least) CRT can be
+		 * fused off, but there's no known fuse bit to
+		 * indicate that. On these machine the ADPA register
+		 * works normally, except the DAC enable bit won't
+		 * take. So the only way to tell is attempt to enable
+		 * it and see what happens.
+		 */
+		I915_WRITE(adpa_reg, adpa | ADPA_DAC_ENABLE |
+			   ADPA_HSYNC_CNTL_DISABLE | ADPA_VSYNC_CNTL_DISABLE);
+		if ((I915_READ(adpa_reg) & ADPA_DAC_ENABLE) == 0)
+			return;
+		I915_WRITE(adpa_reg, adpa);
+	}
+
 	crt = kzalloc(sizeof(struct intel_crt), GFP_KERNEL);
 	if (!crt)
 		return;
@@ -802,7 +831,7 @@ void intel_crt_init(struct drm_device *dev)
 			   &intel_crt_connector_funcs, DRM_MODE_CONNECTOR_VGA);
 
 	drm_encoder_init(dev, &crt->base.base, &intel_crt_enc_funcs,
-			 DRM_MODE_ENCODER_DAC);
+			 DRM_MODE_ENCODER_DAC, NULL);
 
 	intel_connector_attach_encoder(intel_connector, &crt->base);
 
@@ -819,15 +848,10 @@ void intel_crt_init(struct drm_device *dev)
 		connector->interlace_allowed = 1;
 	connector->doublescan_allowed = 0;
 
-	if (HAS_PCH_SPLIT(dev))
-		crt->adpa_reg = PCH_ADPA;
-	else if (IS_VALLEYVIEW(dev))
-		crt->adpa_reg = VLV_ADPA;
-	else
-		crt->adpa_reg = ADPA;
+	crt->adpa_reg = adpa_reg;
 
 	crt->base.compute_config = intel_crt_compute_config;
-	if (HAS_PCH_SPLIT(dev) && !HAS_DDI(dev)) {
+	if (HAS_PCH_SPLIT(dev)) {
 		crt->base.disable = pch_disable_crt;
 		crt->base.post_disable = pch_post_disable_crt;
 	} else {
diff --git a/drivers/gpu/drm/i915/intel_csr.c b/drivers/gpu/drm/i915/intel_csr.c
index 9e530a739354..647d85e77c2f 100644
--- a/drivers/gpu/drm/i915/intel_csr.c
+++ b/drivers/gpu/drm/i915/intel_csr.c
@@ -47,21 +47,10 @@
 MODULE_FIRMWARE(I915_CSR_SKL);
 MODULE_FIRMWARE(I915_CSR_BXT);
 
-/*
-* SKL CSR registers for DC5 and DC6
-*/
-#define CSR_PROGRAM(i)			(0x80000 + (i) * 4)
-#define CSR_SSP_BASE_ADDR_GEN9		0x00002FC0
-#define CSR_HTP_ADDR_SKL		0x00500034
-#define CSR_SSP_BASE			0x8F074
-#define CSR_HTP_SKL			0x8F004
-#define CSR_LAST_WRITE			0x8F034
-#define CSR_LAST_WRITE_VALUE		0xc003b400
-/* MMIO address range for CSR program (0x80000 - 0x82FFF) */
+#define SKL_CSR_VERSION_REQUIRED	CSR_VERSION(1, 23)
+
 #define CSR_MAX_FW_SIZE			0x2FFF
 #define CSR_DEFAULT_FW_OFFSET		0xFFFFFFFF
-#define CSR_MMIO_START_RANGE	0x80000
-#define CSR_MMIO_END_RANGE		0x8FFFF
 
 struct intel_css_header {
 	/* 0x09 for DMC */
@@ -177,167 +166,148 @@ struct stepping_info {
 	char substepping;
 };
 
+/*
+ * Kabylake derivated from Skylake H0, so SKL H0
+ * is the right firmware for KBL A0 (revid 0).
+ */
+static const struct stepping_info kbl_stepping_info[] = {
+	{'H', '0'}, {'I', '0'}
+};
+
 static const struct stepping_info skl_stepping_info[] = {
-		{'A', '0'}, {'B', '0'}, {'C', '0'},
-		{'D', '0'}, {'E', '0'}, {'F', '0'},
-		{'G', '0'}, {'H', '0'}, {'I', '0'}
+	{'A', '0'}, {'B', '0'}, {'C', '0'},
+	{'D', '0'}, {'E', '0'}, {'F', '0'},
+	{'G', '0'}, {'H', '0'}, {'I', '0'}
 };
 
-static struct stepping_info bxt_stepping_info[] = {
+static const struct stepping_info bxt_stepping_info[] = {
 	{'A', '0'}, {'A', '1'}, {'A', '2'},
 	{'B', '0'}, {'B', '1'}, {'B', '2'}
 };
 
-static char intel_get_stepping(struct drm_device *dev)
-{
-	if (IS_SKYLAKE(dev) && (dev->pdev->revision <
-			ARRAY_SIZE(skl_stepping_info)))
-		return skl_stepping_info[dev->pdev->revision].stepping;
-	else if (IS_BROXTON(dev) && (dev->pdev->revision <
-				ARRAY_SIZE(bxt_stepping_info)))
-		return bxt_stepping_info[dev->pdev->revision].stepping;
-	else
-		return -ENODATA;
-}
-
-static char intel_get_substepping(struct drm_device *dev)
-{
-	if (IS_SKYLAKE(dev) && (dev->pdev->revision <
-			ARRAY_SIZE(skl_stepping_info)))
-		return skl_stepping_info[dev->pdev->revision].substepping;
-	else if (IS_BROXTON(dev) && (dev->pdev->revision <
-			ARRAY_SIZE(bxt_stepping_info)))
-		return bxt_stepping_info[dev->pdev->revision].substepping;
-	else
-		return -ENODATA;
-}
-
-/**
- * intel_csr_load_status_get() - to get firmware loading status.
- * @dev_priv: i915 device.
- *
- * This function helps to get the firmware loading status.
- *
- * Return: Firmware loading status.
- */
-enum csr_state intel_csr_load_status_get(struct drm_i915_private *dev_priv)
+static const struct stepping_info *intel_get_stepping_info(struct drm_device *dev)
 {
-	enum csr_state state;
-
-	mutex_lock(&dev_priv->csr_lock);
-	state = dev_priv->csr.state;
-	mutex_unlock(&dev_priv->csr_lock);
+	const struct stepping_info *si;
+	unsigned int size;
+
+	if (IS_KABYLAKE(dev)) {
+		size = ARRAY_SIZE(kbl_stepping_info);
+		si = kbl_stepping_info;
+	} else if (IS_SKYLAKE(dev)) {
+		size = ARRAY_SIZE(skl_stepping_info);
+		si = skl_stepping_info;
+	} else if (IS_BROXTON(dev)) {
+		size = ARRAY_SIZE(bxt_stepping_info);
+		si = bxt_stepping_info;
+	} else {
+		return NULL;
+	}
 
-	return state;
-}
+	if (INTEL_REVID(dev) < size)
+		return si + INTEL_REVID(dev);
 
-/**
- * intel_csr_load_status_set() - help to set firmware loading status.
- * @dev_priv: i915 device.
- * @state: enumeration of firmware loading status.
- *
- * Set the firmware loading status.
- */
-void intel_csr_load_status_set(struct drm_i915_private *dev_priv,
-			enum csr_state state)
-{
-	mutex_lock(&dev_priv->csr_lock);
-	dev_priv->csr.state = state;
-	mutex_unlock(&dev_priv->csr_lock);
+	return NULL;
 }
 
 /**
  * intel_csr_load_program() - write the firmware from memory to register.
- * @dev: drm device.
+ * @dev_priv: i915 drm device.
  *
  * CSR firmware is read from a .bin file and kept in internal memory one time.
  * Everytime display comes back from low power state this function is called to
  * copy the firmware from internal memory to registers.
  */
-void intel_csr_load_program(struct drm_device *dev)
+void intel_csr_load_program(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 *payload = dev_priv->csr.dmc_payload;
 	uint32_t i, fw_size;
 
-	if (!IS_GEN9(dev)) {
+	if (!IS_GEN9(dev_priv)) {
 		DRM_ERROR("No CSR support available for this platform\n");
 		return;
 	}
 
-	/*
-	 * FIXME: Firmware gets lost on S3/S4, but not when entering system
-	 * standby or suspend-to-idle (which is just like forced runtime pm).
-	 * Unfortunately the ACPI subsystem doesn't yet give us a way to
-	 * differentiate this, hence figure it out with this hack.
-	 */
-	if (I915_READ(CSR_PROGRAM(0)))
+	if (!dev_priv->csr.dmc_payload) {
+		DRM_ERROR("Tried to program CSR with empty payload\n");
 		return;
+	}
 
-	mutex_lock(&dev_priv->csr_lock);
 	fw_size = dev_priv->csr.dmc_fw_size;
 	for (i = 0; i < fw_size; i++)
 		I915_WRITE(CSR_PROGRAM(i), payload[i]);
 
 	for (i = 0; i < dev_priv->csr.mmio_count; i++) {
 		I915_WRITE(dev_priv->csr.mmioaddr[i],
-			dev_priv->csr.mmiodata[i]);
+			   dev_priv->csr.mmiodata[i]);
 	}
 
-	dev_priv->csr.state = FW_LOADED;
-	mutex_unlock(&dev_priv->csr_lock);
+	dev_priv->csr.dc_state = 0;
 }
 
-static void finish_csr_load(const struct firmware *fw, void *context)
+static uint32_t *parse_csr_fw(struct drm_i915_private *dev_priv,
+			      const struct firmware *fw)
 {
-	struct drm_i915_private *dev_priv = context;
 	struct drm_device *dev = dev_priv->dev;
 	struct intel_css_header *css_header;
 	struct intel_package_header *package_header;
 	struct intel_dmc_header *dmc_header;
 	struct intel_csr *csr = &dev_priv->csr;
-	char stepping = intel_get_stepping(dev);
-	char substepping = intel_get_substepping(dev);
+	const struct stepping_info *stepping_info = intel_get_stepping_info(dev);
+	char stepping, substepping;
 	uint32_t dmc_offset = CSR_DEFAULT_FW_OFFSET, readcount = 0, nbytes;
 	uint32_t i;
 	uint32_t *dmc_payload;
-	bool fw_loaded = false;
 
-	if (!fw) {
-		i915_firmware_load_error_print(csr->fw_path, 0);
-		goto out;
-	}
+	if (!fw)
+		return NULL;
 
-	if ((stepping == -ENODATA) || (substepping == -ENODATA)) {
+	if (!stepping_info) {
 		DRM_ERROR("Unknown stepping info, firmware loading failed\n");
-		goto out;
+		return NULL;
 	}
 
+	stepping = stepping_info->stepping;
+	substepping = stepping_info->substepping;
+
 	/* Extract CSS Header information*/
 	css_header = (struct intel_css_header *)fw->data;
 	if (sizeof(struct intel_css_header) !=
-		(css_header->header_len * 4)) {
+	    (css_header->header_len * 4)) {
 		DRM_ERROR("Firmware has wrong CSS header length %u bytes\n",
-			(css_header->header_len * 4));
-		goto out;
+			  (css_header->header_len * 4));
+		return NULL;
+	}
+
+	csr->version = css_header->version;
+
+	if (IS_SKYLAKE(dev) && csr->version < SKL_CSR_VERSION_REQUIRED) {
+		DRM_INFO("Refusing to load old Skylake DMC firmware v%u.%u,"
+			 " please upgrade to v%u.%u or later"
+			 " [https://01.org/linuxgraphics/intel-linux-graphics-firmwares].\n",
+			 CSR_VERSION_MAJOR(csr->version),
+			 CSR_VERSION_MINOR(csr->version),
+			 CSR_VERSION_MAJOR(SKL_CSR_VERSION_REQUIRED),
+			 CSR_VERSION_MINOR(SKL_CSR_VERSION_REQUIRED));
+		return NULL;
 	}
+
 	readcount += sizeof(struct intel_css_header);
 
 	/* Extract Package Header information*/
 	package_header = (struct intel_package_header *)
-					&fw->data[readcount];
+		&fw->data[readcount];
 	if (sizeof(struct intel_package_header) !=
-		(package_header->header_len * 4)) {
+	    (package_header->header_len * 4)) {
 		DRM_ERROR("Firmware has wrong package header length %u bytes\n",
-			(package_header->header_len * 4));
-		goto out;
+			  (package_header->header_len * 4));
+		return NULL;
 	}
 	readcount += sizeof(struct intel_package_header);
 
 	/* Search for dmc_offset to find firware binary. */
 	for (i = 0; i < package_header->num_entries; i++) {
 		if (package_header->fw_info[i].substepping == '*' &&
-			stepping == package_header->fw_info[i].stepping) {
+		    stepping == package_header->fw_info[i].stepping) {
 			dmc_offset = package_header->fw_info[i].offset;
 			break;
 		} else if (stepping == package_header->fw_info[i].stepping &&
@@ -345,12 +315,12 @@ static void finish_csr_load(const struct firmware *fw, void *context)
 			dmc_offset = package_header->fw_info[i].offset;
 			break;
 		} else if (package_header->fw_info[i].stepping == '*' &&
-			package_header->fw_info[i].substepping == '*')
+			   package_header->fw_info[i].substepping == '*')
 			dmc_offset = package_header->fw_info[i].offset;
 	}
 	if (dmc_offset == CSR_DEFAULT_FW_OFFSET) {
 		DRM_ERROR("Firmware not supported for %c stepping\n", stepping);
-		goto out;
+		return NULL;
 	}
 	readcount += dmc_offset;
 
@@ -358,26 +328,26 @@ static void finish_csr_load(const struct firmware *fw, void *context)
 	dmc_header = (struct intel_dmc_header *)&fw->data[readcount];
 	if (sizeof(struct intel_dmc_header) != (dmc_header->header_len)) {
 		DRM_ERROR("Firmware has wrong dmc header length %u bytes\n",
-				(dmc_header->header_len));
-		goto out;
+			  (dmc_header->header_len));
+		return NULL;
 	}
 	readcount += sizeof(struct intel_dmc_header);
 
 	/* Cache the dmc header info. */
 	if (dmc_header->mmio_count > ARRAY_SIZE(csr->mmioaddr)) {
 		DRM_ERROR("Firmware has wrong mmio count %u\n",
-						dmc_header->mmio_count);
-		goto out;
+			  dmc_header->mmio_count);
+		return NULL;
 	}
 	csr->mmio_count = dmc_header->mmio_count;
 	for (i = 0; i < dmc_header->mmio_count; i++) {
 		if (dmc_header->mmioaddr[i] < CSR_MMIO_START_RANGE ||
-			dmc_header->mmioaddr[i] > CSR_MMIO_END_RANGE) {
+		    dmc_header->mmioaddr[i] > CSR_MMIO_END_RANGE) {
 			DRM_ERROR(" Firmware has wrong mmio address 0x%x\n",
-						dmc_header->mmioaddr[i]);
-			goto out;
+				  dmc_header->mmioaddr[i]);
+			return NULL;
 		}
-		csr->mmioaddr[i] = dmc_header->mmioaddr[i];
+		csr->mmioaddr[i] = _MMIO(dmc_header->mmioaddr[i]);
 		csr->mmiodata[i] = dmc_header->mmiodata[i];
 	}
 
@@ -385,56 +355,80 @@ static void finish_csr_load(const struct firmware *fw, void *context)
 	nbytes = dmc_header->fw_size * 4;
 	if (nbytes > CSR_MAX_FW_SIZE) {
 		DRM_ERROR("CSR firmware too big (%u) bytes\n", nbytes);
-		goto out;
+		return NULL;
 	}
 	csr->dmc_fw_size = dmc_header->fw_size;
 
-	csr->dmc_payload = kmalloc(nbytes, GFP_KERNEL);
-	if (!csr->dmc_payload) {
+	dmc_payload = kmalloc(nbytes, GFP_KERNEL);
+	if (!dmc_payload) {
 		DRM_ERROR("Memory allocation failed for dmc payload\n");
-		goto out;
+		return NULL;
 	}
 
-	dmc_payload = csr->dmc_payload;
 	memcpy(dmc_payload, &fw->data[readcount], nbytes);
 
+	return dmc_payload;
+}
+
+static void csr_load_work_fn(struct work_struct *work)
+{
+	struct drm_i915_private *dev_priv;
+	struct intel_csr *csr;
+	const struct firmware *fw;
+	int ret;
+
+	dev_priv = container_of(work, typeof(*dev_priv), csr.work);
+	csr = &dev_priv->csr;
+
+	ret = request_firmware(&fw, dev_priv->csr.fw_path,
+			       &dev_priv->dev->pdev->dev);
+	if (!fw)
+		goto out;
+
+	dev_priv->csr.dmc_payload = parse_csr_fw(dev_priv, fw);
+	if (!dev_priv->csr.dmc_payload)
+		goto out;
+
 	/* load csr program during system boot, as needed for DC states */
-	intel_csr_load_program(dev);
-	fw_loaded = true;
+	intel_csr_load_program(dev_priv);
 
-	DRM_DEBUG_KMS("Finished loading %s\n", dev_priv->csr.fw_path);
 out:
-	if (fw_loaded)
-		intel_runtime_pm_put(dev_priv);
-	else
-		intel_csr_load_status_set(dev_priv, FW_FAILED);
+	if (dev_priv->csr.dmc_payload) {
+		intel_display_power_put(dev_priv, POWER_DOMAIN_INIT);
+
+		DRM_INFO("Finished loading %s (v%u.%u)\n",
+			 dev_priv->csr.fw_path,
+			 CSR_VERSION_MAJOR(csr->version),
+			 CSR_VERSION_MINOR(csr->version));
+	} else {
+		DRM_ERROR("Failed to load DMC firmware, disabling rpm\n");
+	}
 
 	release_firmware(fw);
 }
 
 /**
  * intel_csr_ucode_init() - initialize the firmware loading.
- * @dev: drm device.
+ * @dev_priv: i915 drm device.
  *
  * This function is called at the time of loading the display driver to read
  * firmware from a .bin file and copied into a internal memory.
  */
-void intel_csr_ucode_init(struct drm_device *dev)
+void intel_csr_ucode_init(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_csr *csr = &dev_priv->csr;
-	int ret;
 
-	if (!HAS_CSR(dev))
+	INIT_WORK(&dev_priv->csr.work, csr_load_work_fn);
+
+	if (!HAS_CSR(dev_priv))
 		return;
 
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev_priv))
 		csr->fw_path = I915_CSR_SKL;
 	else if (IS_BROXTON(dev_priv))
 		csr->fw_path = I915_CSR_BXT;
 	else {
 		DRM_ERROR("Unexpected: no known CSR firmware for platform\n");
-		intel_csr_load_status_set(dev_priv, FW_FAILED);
 		return;
 	}
 
@@ -444,43 +438,24 @@ void intel_csr_ucode_init(struct drm_device *dev)
 	 * Obtain a runtime pm reference, until CSR is loaded,
 	 * to avoid entering runtime-suspend.
 	 */
-	intel_runtime_pm_get(dev_priv);
-
-	/* CSR supported for platform, load firmware */
-	ret = request_firmware_nowait(THIS_MODULE, true, csr->fw_path,
-				&dev_priv->dev->pdev->dev,
-				GFP_KERNEL, dev_priv,
-				finish_csr_load);
-	if (ret) {
-		i915_firmware_load_error_print(csr->fw_path, ret);
-		intel_csr_load_status_set(dev_priv, FW_FAILED);
-	}
+	intel_display_power_get(dev_priv, POWER_DOMAIN_INIT);
+
+	schedule_work(&dev_priv->csr.work);
 }
 
 /**
  * intel_csr_ucode_fini() - unload the CSR firmware.
- * @dev: drm device.
+ * @dev_priv: i915 drm device.
  *
  * Firmmware unloading includes freeing the internal momory and reset the
  * firmware loading status.
  */
-void intel_csr_ucode_fini(struct drm_device *dev)
+void intel_csr_ucode_fini(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (!HAS_CSR(dev))
+	if (!HAS_CSR(dev_priv))
 		return;
 
-	intel_csr_load_status_set(dev_priv, FW_FAILED);
-	kfree(dev_priv->csr.dmc_payload);
-}
+	flush_work(&dev_priv->csr.work);
 
-void assert_csr_loaded(struct drm_i915_private *dev_priv)
-{
-	WARN_ONCE(intel_csr_load_status_get(dev_priv) != FW_LOADED,
-		  "CSR is not loaded.\n");
-	WARN_ONCE(!I915_READ(CSR_PROGRAM(0)),
-		  "CSR program storage start is NULL\n");
-	WARN_ONCE(!I915_READ(CSR_SSP_BASE), "CSR SSP Base Not fine\n");
-	WARN_ONCE(!I915_READ(CSR_HTP_SKL), "CSR HTP Not fine\n");
+	kfree(dev_priv->csr.dmc_payload);
 }
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index a6752a61d99f..084d5586585d 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -133,12 +133,12 @@ static const struct ddi_buf_trans skl_ddi_translations_dp[] = {
 	{ 0x00002016, 0x000000A0, 0x0 },
 	{ 0x00005012, 0x0000009B, 0x0 },
 	{ 0x00007011, 0x00000088, 0x0 },
-	{ 0x00009010, 0x000000C7, 0x0 },
+	{ 0x80009010, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 	{ 0x00002016, 0x0000009B, 0x0 },
 	{ 0x00005012, 0x00000088, 0x0 },
-	{ 0x00007011, 0x000000C7, 0x0 },
+	{ 0x80007011, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 	{ 0x00002016, 0x000000DF, 0x0 },
-	{ 0x00005012, 0x000000C7, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 };
 
 /* Skylake U */
@@ -146,12 +146,12 @@ static const struct ddi_buf_trans skl_u_ddi_translations_dp[] = {
 	{ 0x0000201B, 0x000000A2, 0x0 },
 	{ 0x00005012, 0x00000088, 0x0 },
 	{ 0x00007011, 0x00000087, 0x0 },
-	{ 0x80009010, 0x000000C7, 0x1 },	/* Uses I_boost level 0x1 */
+	{ 0x80009010, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 	{ 0x0000201B, 0x0000009D, 0x0 },
-	{ 0x00005012, 0x000000C7, 0x0 },
-	{ 0x00007011, 0x000000C7, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
+	{ 0x80007011, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 	{ 0x00002016, 0x00000088, 0x0 },
-	{ 0x00005012, 0x000000C7, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x1 },	/* Uses I_boost level 0x1 */
 };
 
 /* Skylake Y */
@@ -159,12 +159,12 @@ static const struct ddi_buf_trans skl_y_ddi_translations_dp[] = {
 	{ 0x00000018, 0x000000A2, 0x0 },
 	{ 0x00005012, 0x00000088, 0x0 },
 	{ 0x00007011, 0x00000087, 0x0 },
-	{ 0x80009010, 0x000000C7, 0x3 },	/* Uses I_boost level 0x3 */
+	{ 0x80009010, 0x000000C0, 0x3 },	/* Uses I_boost level 0x3 */
 	{ 0x00000018, 0x0000009D, 0x0 },
-	{ 0x00005012, 0x000000C7, 0x0 },
-	{ 0x00007011, 0x000000C7, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },	/* Uses I_boost level 0x3 */
+	{ 0x80007011, 0x000000C0, 0x3 },	/* Uses I_boost level 0x3 */
 	{ 0x00000018, 0x00000088, 0x0 },
-	{ 0x00005012, 0x000000C7, 0x0 },
+	{ 0x80005012, 0x000000C0, 0x3 },	/* Uses I_boost level 0x3 */
 };
 
 /*
@@ -345,7 +345,7 @@ enum port intel_ddi_get_encoder_port(struct intel_encoder *intel_encoder)
 static bool
 intel_dig_port_supports_hdmi(const struct intel_digital_port *intel_dig_port)
 {
-	return intel_dig_port->hdmi.hdmi_reg;
+	return i915_mmio_reg_valid(intel_dig_port->hdmi.hdmi_reg);
 }
 
 static const struct ddi_buf_trans *skl_get_buf_trans_dp(struct drm_device *dev,
@@ -353,10 +353,10 @@ static const struct ddi_buf_trans *skl_get_buf_trans_dp(struct drm_device *dev,
 {
 	const struct ddi_buf_trans *ddi_translations;
 
-	if (IS_SKL_ULX(dev)) {
+	if (IS_SKL_ULX(dev) || IS_KBL_ULX(dev)) {
 		ddi_translations = skl_y_ddi_translations_dp;
 		*n_entries = ARRAY_SIZE(skl_y_ddi_translations_dp);
-	} else if (IS_SKL_ULT(dev)) {
+	} else if (IS_SKL_ULT(dev) || IS_KBL_ULT(dev)) {
 		ddi_translations = skl_u_ddi_translations_dp;
 		*n_entries = ARRAY_SIZE(skl_u_ddi_translations_dp);
 	} else {
@@ -373,7 +373,7 @@ static const struct ddi_buf_trans *skl_get_buf_trans_edp(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	const struct ddi_buf_trans *ddi_translations;
 
-	if (IS_SKL_ULX(dev)) {
+	if (IS_SKL_ULX(dev) || IS_KBL_ULX(dev)) {
 		if (dev_priv->edp_low_vswing) {
 			ddi_translations = skl_y_ddi_translations_edp;
 			*n_entries = ARRAY_SIZE(skl_y_ddi_translations_edp);
@@ -381,7 +381,7 @@ static const struct ddi_buf_trans *skl_get_buf_trans_edp(struct drm_device *dev,
 			ddi_translations = skl_y_ddi_translations_dp;
 			*n_entries = ARRAY_SIZE(skl_y_ddi_translations_dp);
 		}
-	} else if (IS_SKL_ULT(dev)) {
+	} else if (IS_SKL_ULT(dev) || IS_KBL_ULT(dev)) {
 		if (dev_priv->edp_low_vswing) {
 			ddi_translations = skl_u_ddi_translations_edp;
 			*n_entries = ARRAY_SIZE(skl_u_ddi_translations_edp);
@@ -408,7 +408,7 @@ skl_get_buf_trans_hdmi(struct drm_device *dev,
 {
 	const struct ddi_buf_trans *ddi_translations;
 
-	if (IS_SKL_ULX(dev)) {
+	if (IS_SKL_ULX(dev) || IS_KBL_ULX(dev)) {
 		ddi_translations = skl_y_ddi_translations_hdmi;
 		*n_entries = ARRAY_SIZE(skl_y_ddi_translations_hdmi);
 	} else {
@@ -448,7 +448,7 @@ static void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port,
 		bxt_ddi_vswing_sequence(dev, hdmi_level, port,
 					INTEL_OUTPUT_HDMI);
 		return;
-	} else if (IS_SKYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		ddi_translations_fdi = NULL;
 		ddi_translations_dp =
 				skl_get_buf_trans_dp(dev, &n_dp_entries);
@@ -576,7 +576,7 @@ void intel_prepare_ddi(struct drm_device *dev)
 static void intel_wait_ddi_buf_idle(struct drm_i915_private *dev_priv,
 				    enum port port)
 {
-	uint32_t reg = DDI_BUF_CTL(port);
+	i915_reg_t reg = DDI_BUF_CTL(port);
 	int i;
 
 	for (i = 0; i < 16; i++) {
@@ -675,15 +675,16 @@ void hsw_fdi_link_train(struct drm_crtc *crtc)
 		temp = I915_READ(DP_TP_STATUS(PORT_E));
 		if (temp & DP_TP_STATUS_AUTOTRAIN_DONE) {
 			DRM_DEBUG_KMS("FDI link training done on step %d\n", i);
+			break;
+		}
 
-			/* Enable normal pixel sending for FDI */
-			I915_WRITE(DP_TP_CTL(PORT_E),
-				   DP_TP_CTL_FDI_AUTOTRAIN |
-				   DP_TP_CTL_LINK_TRAIN_NORMAL |
-				   DP_TP_CTL_ENHANCED_FRAME_ENABLE |
-				   DP_TP_CTL_ENABLE);
-
-			return;
+		/*
+		 * Leave things enabled even if we failed to train FDI.
+		 * Results in less fireworks from the state checker.
+		 */
+		if (i == ARRAY_SIZE(hsw_ddi_translations_fdi) * 2 - 1) {
+			DRM_ERROR("FDI link training failed!\n");
+			break;
 		}
 
 		temp = I915_READ(DDI_BUF_CTL(PORT_E));
@@ -712,7 +713,12 @@ void hsw_fdi_link_train(struct drm_crtc *crtc)
 		POSTING_READ(FDI_RX_MISC(PIPE_A));
 	}
 
-	DRM_ERROR("FDI link training failed!\n");
+	/* Enable normal pixel sending for FDI */
+	I915_WRITE(DP_TP_CTL(PORT_E),
+		   DP_TP_CTL_FDI_AUTOTRAIN |
+		   DP_TP_CTL_LINK_TRAIN_NORMAL |
+		   DP_TP_CTL_ENHANCED_FRAME_ENABLE |
+		   DP_TP_CTL_ENABLE);
 }
 
 void intel_ddi_init_dp_buf_reg(struct intel_encoder *encoder)
@@ -931,7 +937,8 @@ static void hsw_wrpll_update_rnp(uint64_t freq2k, unsigned budget,
 	/* Otherwise a < c && b >= d, do nothing */
 }
 
-static int hsw_ddi_calc_wrpll_link(struct drm_i915_private *dev_priv, int reg)
+static int hsw_ddi_calc_wrpll_link(struct drm_i915_private *dev_priv,
+				   i915_reg_t reg)
 {
 	int refclk = LC_FREQ;
 	int n, p, r;
@@ -967,7 +974,7 @@ static int hsw_ddi_calc_wrpll_link(struct drm_i915_private *dev_priv, int reg)
 static int skl_calc_wrpll_link(struct drm_i915_private *dev_priv,
 			       uint32_t dpll)
 {
-	uint32_t cfgcr1_reg, cfgcr2_reg;
+	i915_reg_t cfgcr1_reg, cfgcr2_reg;
 	uint32_t cfgcr1_val, cfgcr2_val;
 	uint32_t p0, p1, p2, dco_freq;
 
@@ -1112,10 +1119,10 @@ static void hsw_ddi_clock_get(struct intel_encoder *encoder,
 		link_clock = 270000;
 		break;
 	case PORT_CLK_SEL_WRPLL1:
-		link_clock = hsw_ddi_calc_wrpll_link(dev_priv, WRPLL_CTL1);
+		link_clock = hsw_ddi_calc_wrpll_link(dev_priv, WRPLL_CTL(0));
 		break;
 	case PORT_CLK_SEL_WRPLL2:
-		link_clock = hsw_ddi_calc_wrpll_link(dev_priv, WRPLL_CTL2);
+		link_clock = hsw_ddi_calc_wrpll_link(dev_priv, WRPLL_CTL(1));
 		break;
 	case PORT_CLK_SEL_SPLL:
 		pll = I915_READ(SPLL_CTL) & SPLL_PLL_FREQ_MASK;
@@ -1184,7 +1191,7 @@ void intel_ddi_clock_get(struct intel_encoder *encoder,
 
 	if (INTEL_INFO(dev)->gen <= 8)
 		hsw_ddi_clock_get(encoder, pipe_config);
-	else if (IS_SKYLAKE(dev))
+	else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		skl_ddi_clock_get(encoder, pipe_config);
 	else if (IS_BROXTON(dev))
 		bxt_ddi_clock_get(encoder, pipe_config);
@@ -1582,7 +1589,8 @@ skl_ddi_pll_select(struct intel_crtc *intel_crtc,
 			 DPLL_CFGCR2_KDIV(wrpll_params.kdiv) |
 			 DPLL_CFGCR2_PDIV(wrpll_params.pdiv) |
 			 wrpll_params.central_freq;
-	} else if (intel_encoder->type == INTEL_OUTPUT_DISPLAYPORT) {
+	} else if (intel_encoder->type == INTEL_OUTPUT_DISPLAYPORT ||
+		   intel_encoder->type == INTEL_OUTPUT_DP_MST) {
 		switch (crtc_state->port_clock / 2) {
 		case 81000:
 			ctrl1 |= DPLL_CTRL1_LINK_RATE(DPLL_CTRL1_LINK_RATE_810, 0);
@@ -1780,7 +1788,7 @@ bool intel_ddi_pll_select(struct intel_crtc *intel_crtc,
 	struct intel_encoder *intel_encoder =
 		intel_ddi_get_crtc_new_encoder(crtc_state);
 
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		return skl_ddi_pll_select(intel_crtc, crtc_state,
 					  intel_encoder);
 	else if (IS_BROXTON(dev))
@@ -1942,7 +1950,7 @@ void intel_ddi_enable_transcoder_func(struct drm_crtc *crtc)
 void intel_ddi_disable_transcoder_func(struct drm_i915_private *dev_priv,
 				       enum transcoder cpu_transcoder)
 {
-	uint32_t reg = TRANS_DDI_FUNC_CTL(cpu_transcoder);
+	i915_reg_t reg = TRANS_DDI_FUNC_CTL(cpu_transcoder);
 	uint32_t val = I915_READ(reg);
 
 	val &= ~(TRANS_DDI_FUNC_ENABLE | TRANS_DDI_PORT_MASK | TRANS_DDI_DP_VC_PAYLOAD_ALLOC);
@@ -1961,13 +1969,16 @@ bool intel_ddi_connector_get_hw_state(struct intel_connector *intel_connector)
 	enum transcoder cpu_transcoder;
 	enum intel_display_power_domain power_domain;
 	uint32_t tmp;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(intel_encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
-	if (!intel_encoder->get_hw_state(intel_encoder, &pipe))
-		return false;
+	if (!intel_encoder->get_hw_state(intel_encoder, &pipe)) {
+		ret = false;
+		goto out;
+	}
 
 	if (port == PORT_A)
 		cpu_transcoder = TRANSCODER_EDP;
@@ -1979,23 +1990,33 @@ bool intel_ddi_connector_get_hw_state(struct intel_connector *intel_connector)
 	switch (tmp & TRANS_DDI_MODE_SELECT_MASK) {
 	case TRANS_DDI_MODE_SELECT_HDMI:
 	case TRANS_DDI_MODE_SELECT_DVI:
-		return (type == DRM_MODE_CONNECTOR_HDMIA);
+		ret = type == DRM_MODE_CONNECTOR_HDMIA;
+		break;
 
 	case TRANS_DDI_MODE_SELECT_DP_SST:
-		if (type == DRM_MODE_CONNECTOR_eDP)
-			return true;
-		return (type == DRM_MODE_CONNECTOR_DisplayPort);
+		ret = type == DRM_MODE_CONNECTOR_eDP ||
+		      type == DRM_MODE_CONNECTOR_DisplayPort;
+		break;
+
 	case TRANS_DDI_MODE_SELECT_DP_MST:
 		/* if the transcoder is in MST state then
 		 * connector isn't connected */
-		return false;
+		ret = false;
+		break;
 
 	case TRANS_DDI_MODE_SELECT_FDI:
-		return (type == DRM_MODE_CONNECTOR_VGA);
+		ret = type == DRM_MODE_CONNECTOR_VGA;
+		break;
 
 	default:
-		return false;
+		ret = false;
+		break;
 	}
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
@@ -2007,15 +2028,18 @@ bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
 	enum intel_display_power_domain power_domain;
 	u32 tmp;
 	int i;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	tmp = I915_READ(DDI_BUF_CTL(port));
 
 	if (!(tmp & DDI_BUF_CTL_ENABLE))
-		return false;
+		goto out;
 
 	if (port == PORT_A) {
 		tmp = I915_READ(TRANS_DDI_FUNC_CTL(TRANSCODER_EDP));
@@ -2033,25 +2057,32 @@ bool intel_ddi_get_hw_state(struct intel_encoder *encoder,
 			break;
 		}
 
-		return true;
-	} else {
-		for (i = TRANSCODER_A; i <= TRANSCODER_C; i++) {
-			tmp = I915_READ(TRANS_DDI_FUNC_CTL(i));
+		ret = true;
+
+		goto out;
+	}
 
-			if ((tmp & TRANS_DDI_PORT_MASK)
-			    == TRANS_DDI_SELECT_PORT(port)) {
-				if ((tmp & TRANS_DDI_MODE_SELECT_MASK) == TRANS_DDI_MODE_SELECT_DP_MST)
-					return false;
+	for (i = TRANSCODER_A; i <= TRANSCODER_C; i++) {
+		tmp = I915_READ(TRANS_DDI_FUNC_CTL(i));
 
-				*pipe = i;
-				return true;
-			}
+		if ((tmp & TRANS_DDI_PORT_MASK) == TRANS_DDI_SELECT_PORT(port)) {
+			if ((tmp & TRANS_DDI_MODE_SELECT_MASK) ==
+			    TRANS_DDI_MODE_SELECT_DP_MST)
+				goto out;
+
+			*pipe = i;
+			ret = true;
+
+			goto out;
 		}
 	}
 
 	DRM_DEBUG_KMS("No pipe for ddi port %c found\n", port_name(port));
 
-	return false;
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 void intel_ddi_enable_pipe_clock(struct intel_crtc *intel_crtc)
@@ -2097,21 +2128,21 @@ static void skl_ddi_set_iboost(struct drm_device *dev, u32 level,
 			iboost = dp_iboost;
 		} else {
 			ddi_translations = skl_get_buf_trans_dp(dev, &n_entries);
-			iboost = ddi_translations[port].i_boost;
+			iboost = ddi_translations[level].i_boost;
 		}
 	} else if (type == INTEL_OUTPUT_EDP) {
 		if (dp_iboost) {
 			iboost = dp_iboost;
 		} else {
 			ddi_translations = skl_get_buf_trans_edp(dev, &n_entries);
-			iboost = ddi_translations[port].i_boost;
+			iboost = ddi_translations[level].i_boost;
 		}
 	} else if (type == INTEL_OUTPUT_HDMI) {
 		if (hdmi_iboost) {
 			iboost = hdmi_iboost;
 		} else {
 			ddi_translations = skl_get_buf_trans_hdmi(dev, &n_entries);
-			iboost = ddi_translations[port].i_boost;
+			iboost = ddi_translations[level].i_boost;
 		}
 	} else {
 		return;
@@ -2263,7 +2294,7 @@ uint32_t ddi_signal_levels(struct intel_dp *intel_dp)
 
 	level = translate_signal_level(signal_levels);
 
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		skl_ddi_set_iboost(dev, level, port, encoder->type);
 	else if (IS_BROXTON(dev))
 		bxt_ddi_vswing_sequence(dev, level, port, encoder->type);
@@ -2271,30 +2302,21 @@ uint32_t ddi_signal_levels(struct intel_dp *intel_dp)
 	return DDI_BUF_TRANS_SELECT(level);
 }
 
-static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
+void intel_ddi_clk_select(struct intel_encoder *encoder,
+			  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_encoder *encoder = &intel_encoder->base;
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
-	enum port port = intel_ddi_get_encoder_port(intel_encoder);
-	int type = intel_encoder->type;
-	int hdmi_level;
-
-	if (type == INTEL_OUTPUT_EDP) {
-		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
-		intel_edp_panel_on(intel_dp);
-	}
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
+	enum port port = intel_ddi_get_encoder_port(encoder);
 
-	if (IS_SKYLAKE(dev)) {
-		uint32_t dpll = crtc->config->ddi_pll_sel;
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv)) {
+		uint32_t dpll = pipe_config->ddi_pll_sel;
 		uint32_t val;
 
 		/*
 		 * DPLL0 is used for eDP and is the only "private" DPLL (as
 		 * opposed to shared) on SKL
 		 */
-		if (type == INTEL_OUTPUT_EDP) {
+		if (encoder->type == INTEL_OUTPUT_EDP) {
 			WARN_ON(dpll != SKL_DPLL0);
 
 			val = I915_READ(DPLL_CTRL1);
@@ -2302,7 +2324,7 @@ static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
 			val &= ~(DPLL_CTRL1_HDMI_MODE(dpll) |
 				 DPLL_CTRL1_SSC(dpll) |
 				 DPLL_CTRL1_LINK_RATE_MASK(dpll));
-			val |= crtc->config->dpll_hw_state.ctrl1 << (dpll * 6);
+			val |= pipe_config->dpll_hw_state.ctrl1 << (dpll * 6);
 
 			I915_WRITE(DPLL_CTRL1, val);
 			POSTING_READ(DPLL_CTRL1);
@@ -2318,11 +2340,29 @@ static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
 
 		I915_WRITE(DPLL_CTRL2, val);
 
-	} else if (INTEL_INFO(dev)->gen < 9) {
-		WARN_ON(crtc->config->ddi_pll_sel == PORT_CLK_SEL_NONE);
-		I915_WRITE(PORT_CLK_SEL(port), crtc->config->ddi_pll_sel);
+	} else if (INTEL_INFO(dev_priv)->gen < 9) {
+		WARN_ON(pipe_config->ddi_pll_sel == PORT_CLK_SEL_NONE);
+		I915_WRITE(PORT_CLK_SEL(port), pipe_config->ddi_pll_sel);
+	}
+}
+
+static void intel_ddi_pre_enable(struct intel_encoder *intel_encoder)
+{
+	struct drm_encoder *encoder = &intel_encoder->base;
+	struct drm_device *dev = encoder->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
+	enum port port = intel_ddi_get_encoder_port(intel_encoder);
+	int type = intel_encoder->type;
+	int hdmi_level;
+
+	if (type == INTEL_OUTPUT_EDP) {
+		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
+		intel_edp_panel_on(intel_dp);
 	}
 
+	intel_ddi_clk_select(intel_encoder, crtc->config);
+
 	if (type == INTEL_OUTPUT_DISPLAYPORT || type == INTEL_OUTPUT_EDP) {
 		struct intel_dp *intel_dp = enc_to_intel_dp(encoder);
 
@@ -2381,7 +2421,7 @@ static void intel_ddi_post_disable(struct intel_encoder *intel_encoder)
 		intel_edp_panel_off(intel_dp);
 	}
 
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		I915_WRITE(DPLL_CTRL2, (I915_READ(DPLL_CTRL2) |
 					DPLL_CTRL2_DDI_CLK_OFF(port)));
 	else if (INTEL_INFO(dev)->gen < 9)
@@ -2491,12 +2531,14 @@ static bool hsw_ddi_wrpll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	uint32_t val;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
 	val = I915_READ(WRPLL_CTL(pll->id));
 	hw_state->wrpll = val;
 
+	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+
 	return val & WRPLL_PLL_ENABLE;
 }
 
@@ -2506,12 +2548,14 @@ static bool hsw_ddi_spll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	uint32_t val;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
 	val = I915_READ(SPLL_CTL);
 	hw_state->spll = val;
 
+	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+
 	return val & SPLL_PLL_ENABLE;
 }
 
@@ -2553,7 +2597,7 @@ static const char * const skl_ddi_pll_names[] = {
 };
 
 struct skl_dpll_regs {
-	u32 ctl, cfgcr1, cfgcr2;
+	i915_reg_t ctl, cfgcr1, cfgcr2;
 };
 
 /* this array is indexed by the *shared* pll id */
@@ -2566,13 +2610,13 @@ static const struct skl_dpll_regs skl_dpll_regs[3] = {
 	},
 	{
 		/* DPLL 2 */
-		.ctl = WRPLL_CTL1,
+		.ctl = WRPLL_CTL(0),
 		.cfgcr1 = DPLL_CFGCR1(SKL_DPLL2),
 		.cfgcr2 = DPLL_CFGCR2(SKL_DPLL2),
 	},
 	{
 		/* DPLL 3 */
-		.ctl = WRPLL_CTL2,
+		.ctl = WRPLL_CTL(1),
 		.cfgcr1 = DPLL_CFGCR1(SKL_DPLL3),
 		.cfgcr2 = DPLL_CFGCR2(SKL_DPLL3),
 	},
@@ -2628,16 +2672,19 @@ static bool skl_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
 	uint32_t val;
 	unsigned int dpll;
 	const struct skl_dpll_regs *regs = skl_dpll_regs;
+	bool ret;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
+	ret = false;
+
 	/* DPLL0 is not part of the shared DPLLs, so pll->id is 0 for DPLL1 */
 	dpll = pll->id + 1;
 
 	val = I915_READ(regs[pll->id].ctl);
 	if (!(val & LCPLL_PLL_ENABLE))
-		return false;
+		goto out;
 
 	val = I915_READ(DPLL_CTRL1);
 	hw_state->ctrl1 = (val >> (dpll * 6)) & 0x3f;
@@ -2647,8 +2694,12 @@ static bool skl_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
 		hw_state->cfgcr1 = I915_READ(regs[pll->id].cfgcr1);
 		hw_state->cfgcr2 = I915_READ(regs[pll->id].cfgcr2);
 	}
+	ret = true;
 
-	return true;
+out:
+	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+
+	return ret;
 }
 
 static void skl_shared_dplls_init(struct drm_i915_private *dev_priv)
@@ -2915,13 +2966,16 @@ static bool bxt_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	enum port port = (enum port)pll->id;	/* 1:1 port->PLL mapping */
 	uint32_t val;
+	bool ret;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
+	ret = false;
+
 	val = I915_READ(BXT_PORT_PLL_ENABLE(port));
 	if (!(val & PORT_PLL_ENABLE))
-		return false;
+		goto out;
 
 	hw_state->ebb0 = I915_READ(BXT_PORT_PLL_EBB_0(port));
 	hw_state->ebb0 &= PORT_PLL_P1_MASK | PORT_PLL_P2_MASK;
@@ -2968,7 +3022,12 @@ static bool bxt_ddi_pll_get_hw_state(struct drm_i915_private *dev_priv,
 				 I915_READ(BXT_PORT_PCS_DW12_LN23(port)));
 	hw_state->pcsdw12 &= LANE_STAGGER_MASK | LANESTAGGER_STRAP_OVRD;
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+
+	return ret;
 }
 
 static void bxt_shared_dplls_init(struct drm_i915_private *dev_priv)
@@ -2992,22 +3051,22 @@ void intel_ddi_pll_init(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t val = I915_READ(LCPLL_CTL);
 
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		skl_shared_dplls_init(dev_priv);
 	else if (IS_BROXTON(dev))
 		bxt_shared_dplls_init(dev_priv);
 	else
 		hsw_shared_dplls_init(dev_priv);
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		int cdclk_freq;
 
 		cdclk_freq = dev_priv->display.get_display_clock_speed(dev);
 		dev_priv->skl_boot_cdclk = cdclk_freq;
+		if (skl_sanitize_cdclk(dev_priv))
+			DRM_DEBUG_KMS("Sanitized cdclk programmed by pre-os\n");
 		if (!(I915_READ(LCPLL1_CTL) & LCPLL_PLL_ENABLE))
 			DRM_ERROR("LCPLL1 is disabled\n");
-		else
-			intel_display_power_get(dev_priv, POWER_DOMAIN_PLLS);
 	} else if (IS_BROXTON(dev)) {
 		broxton_init_cdclk(dev);
 		broxton_ddi_phy_init(dev);
@@ -3026,11 +3085,11 @@ void intel_ddi_pll_init(struct drm_device *dev)
 	}
 }
 
-void intel_ddi_prepare_link_retrain(struct drm_encoder *encoder)
+void intel_ddi_prepare_link_retrain(struct intel_dp *intel_dp)
 {
-	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
-	struct intel_dp *intel_dp = &intel_dig_port->dp;
-	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_i915_private *dev_priv =
+		to_i915(intel_dig_port->base.base.dev);
 	enum port port = intel_dig_port->port;
 	uint32_t val;
 	bool wait = false;
@@ -3098,6 +3157,23 @@ void intel_ddi_fdi_disable(struct drm_crtc *crtc)
 	I915_WRITE(FDI_RX_CTL(PIPE_A), val);
 }
 
+bool intel_ddi_is_audio_enabled(struct drm_i915_private *dev_priv,
+				 struct intel_crtc *intel_crtc)
+{
+	u32 temp;
+
+	if (intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_AUDIO)) {
+		temp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
+
+		intel_display_power_put(dev_priv, POWER_DOMAIN_AUDIO);
+
+		if (temp & AUDIO_OUTPUT_ENABLE(intel_crtc->pipe))
+			return true;
+	}
+
+	return false;
+}
+
 void intel_ddi_get_config(struct intel_encoder *encoder,
 			  struct intel_crtc_state *pipe_config)
 {
@@ -3141,7 +3217,7 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 		pipe_config->has_hdmi_sink = true;
 		intel_hdmi = enc_to_intel_hdmi(&encoder->base);
 
-		if (intel_hdmi->infoframe_enabled(&encoder->base))
+		if (intel_hdmi->infoframe_enabled(&encoder->base, pipe_config))
 			pipe_config->has_infoframe = true;
 		break;
 	case TRANS_DDI_MODE_SELECT_DVI:
@@ -3158,11 +3234,8 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 		break;
 	}
 
-	if (intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_AUDIO)) {
-		temp = I915_READ(HSW_AUD_PIN_ELD_CP_VLD);
-		if (temp & AUDIO_OUTPUT_ENABLE(intel_crtc->pipe))
-			pipe_config->has_audio = true;
-	}
+	pipe_config->has_audio =
+		intel_ddi_is_audio_enabled(dev_priv, intel_crtc);
 
 	if (encoder->type == INTEL_OUTPUT_EDP && dev_priv->vbt.edp_bpp &&
 	    pipe_config->pipe_bpp > dev_priv->vbt.edp_bpp) {
@@ -3274,7 +3347,7 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 	encoder = &intel_encoder->base;
 
 	drm_encoder_init(dev, encoder, &intel_ddi_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	intel_encoder->compute_config = intel_ddi_compute_config;
 	intel_encoder->enable = intel_enable_ddi;
@@ -3289,6 +3362,20 @@ void intel_ddi_init(struct drm_device *dev, enum port port)
 					  (DDI_BUF_PORT_REVERSAL |
 					   DDI_A_4_LANES);
 
+	/*
+	 * Bspec says that DDI_A_4_LANES is the only supported configuration
+	 * for Broxton.  Yet some BIOS fail to set this bit on port A if eDP
+	 * wasn't lit up at boot.  Force this bit on in our internal
+	 * configuration so that we use the proper lane count for our
+	 * calculations.
+	 */
+	if (IS_BROXTON(dev) && 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;
+		}
+	}
+
 	intel_encoder->type = INTEL_OUTPUT_UNKNOWN;
 	intel_encoder->crtc_mask = (1 << 0) | (1 << 1) | (1 << 2);
 	intel_encoder->cloneable = 0;
@@ -3302,8 +3389,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_BROXTON(dev_priv) && (INTEL_REVID(dev) < BXT_REVID_B0)
-					 && port == PORT_B)
+		if (IS_BXT_REVID(dev, 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;
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 32cf97346978..46947fffd599 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -44,6 +44,8 @@
 #include <drm/drm_plane_helper.h>
 #include <drm/drm_rect.h>
 #include <linux/dma_remapping.h>
+#include <linux/reservation.h>
+#include <linux/dma-buf.h>
 
 /* Primary plane formats for gen <= 3 */
 static const uint32_t i8xx_primary_formats[] = {
@@ -186,7 +188,7 @@ int intel_hrawclk(struct drm_device *dev)
 	uint32_t clkcfg;
 
 	/* There is no CLKCFG reg in Valleyview. VLV hrawclk is 200 MHz */
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		return 200;
 
 	clkcfg = I915_READ(CLKCFG);
@@ -214,7 +216,7 @@ int intel_hrawclk(struct drm_device *dev)
 
 static void intel_update_czclk(struct drm_i915_private *dev_priv)
 {
-	if (!IS_VALLEYVIEW(dev_priv))
+	if (!(IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)))
 		return;
 
 	dev_priv->czclk_freq = vlv_get_cck_clock_hpll(dev_priv, "czclk",
@@ -715,11 +717,12 @@ 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_BROXTON(dev))
+	if (!IS_PINEVIEW(dev) && !IS_VALLEYVIEW(dev) &&
+	    !IS_CHERRYVIEW(dev) && !IS_BROXTON(dev))
 		if (clock->m1 <= clock->m2)
 			INTELPllInvalid("m1 <= m2\n");
 
-	if (!IS_VALLEYVIEW(dev) && !IS_BROXTON(dev)) {
+	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) && !IS_BROXTON(dev)) {
 		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)
@@ -1096,7 +1099,7 @@ enum transcoder intel_pipe_to_cpu_transcoder(struct drm_i915_private *dev_priv,
 static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 reg = PIPEDSL(pipe);
+	i915_reg_t reg = PIPEDSL(pipe);
 	u32 line1, line2;
 	u32 line_mask;
 
@@ -1136,7 +1139,7 @@ static void intel_wait_for_pipe_off(struct intel_crtc *crtc)
 	enum pipe pipe = crtc->pipe;
 
 	if (INTEL_INFO(dev)->gen >= 4) {
-		int reg = PIPECONF(cpu_transcoder);
+		i915_reg_t reg = PIPECONF(cpu_transcoder);
 
 		/* Wait for the Pipe State to go off */
 		if (wait_for((I915_READ(reg) & I965_PIPECONF_ACTIVE) == 0,
@@ -1286,7 +1289,7 @@ void assert_panel_unlocked(struct drm_i915_private *dev_priv,
 			   enum pipe pipe)
 {
 	struct drm_device *dev = dev_priv->dev;
-	int pp_reg;
+	i915_reg_t pp_reg;
 	u32 val;
 	enum pipe panel_pipe = PIPE_A;
 	bool locked = true;
@@ -1304,7 +1307,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)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		/* presumably write lock depends on pipe, not port select */
 		pp_reg = VLV_PIPE_PP_CONTROL(pipe);
 		panel_pipe = pipe;
@@ -1348,18 +1351,21 @@ void assert_pipe(struct drm_i915_private *dev_priv,
 	bool cur_state;
 	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
 								      pipe);
+	enum intel_display_power_domain power_domain;
 
 	/* if we need the pipe quirk it must be always on */
 	if ((pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
 	    (pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
 		state = true;
 
-	if (!intel_display_power_is_enabled(dev_priv,
-				POWER_DOMAIN_TRANSCODER(cpu_transcoder))) {
-		cur_state = false;
-	} else {
+	power_domain = POWER_DOMAIN_TRANSCODER(cpu_transcoder);
+	if (intel_display_power_get_if_enabled(dev_priv, power_domain)) {
 		u32 val = I915_READ(PIPECONF(cpu_transcoder));
 		cur_state = !!(val & PIPECONF_ENABLE);
+
+		intel_display_power_put(dev_priv, power_domain);
+	} else {
+		cur_state = false;
 	}
 
 	I915_STATE_WARN(cur_state != state,
@@ -1422,7 +1428,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)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		for_each_sprite(dev_priv, pipe, sprite) {
 			u32 val = I915_READ(SPCNTR(pipe, sprite));
 			I915_STATE_WARN(val & SP_ENABLE,
@@ -1481,8 +1487,7 @@ static bool dp_pipe_enabled(struct drm_i915_private *dev_priv,
 		return false;
 
 	if (HAS_PCH_CPT(dev_priv->dev)) {
-		u32	trans_dp_ctl_reg = TRANS_DP_CTL(pipe);
-		u32	trans_dp_ctl = I915_READ(trans_dp_ctl_reg);
+		u32 trans_dp_ctl = I915_READ(TRANS_DP_CTL(pipe));
 		if ((trans_dp_ctl & TRANS_DP_PORT_SEL_MASK) != port_sel)
 			return false;
 	} else if (IS_CHERRYVIEW(dev_priv->dev)) {
@@ -1546,12 +1551,13 @@ static bool adpa_pipe_enabled(struct drm_i915_private *dev_priv,
 }
 
 static void assert_pch_dp_disabled(struct drm_i915_private *dev_priv,
-				   enum pipe pipe, int reg, u32 port_sel)
+				   enum pipe pipe, i915_reg_t reg,
+				   u32 port_sel)
 {
 	u32 val = I915_READ(reg);
 	I915_STATE_WARN(dp_pipe_enabled(dev_priv, pipe, port_sel, val),
 	     "PCH DP (0x%08x) enabled on transcoder %c, should be disabled\n",
-	     reg, pipe_name(pipe));
+	     i915_mmio_reg_offset(reg), pipe_name(pipe));
 
 	I915_STATE_WARN(HAS_PCH_IBX(dev_priv->dev) && (val & DP_PORT_EN) == 0
 	     && (val & DP_PIPEB_SELECT),
@@ -1559,12 +1565,12 @@ static void assert_pch_dp_disabled(struct drm_i915_private *dev_priv,
 }
 
 static void assert_pch_hdmi_disabled(struct drm_i915_private *dev_priv,
-				     enum pipe pipe, int reg)
+				     enum pipe pipe, i915_reg_t reg)
 {
 	u32 val = I915_READ(reg);
 	I915_STATE_WARN(hdmi_pipe_enabled(dev_priv, pipe, val),
 	     "PCH HDMI (0x%08x) enabled on transcoder %c, should be disabled\n",
-	     reg, pipe_name(pipe));
+	     i915_mmio_reg_offset(reg), pipe_name(pipe));
 
 	I915_STATE_WARN(HAS_PCH_IBX(dev_priv->dev) && (val & SDVO_ENABLE) == 0
 	     && (val & SDVO_PIPE_B_SELECT),
@@ -1600,14 +1606,11 @@ static void vlv_enable_pll(struct intel_crtc *crtc,
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int reg = DPLL(crtc->pipe);
+	i915_reg_t reg = DPLL(crtc->pipe);
 	u32 dpll = pipe_config->dpll_hw_state.dpll;
 
 	assert_pipe_disabled(dev_priv, crtc->pipe);
 
-	/* No really, not for ILK+ */
-	BUG_ON(!IS_VALLEYVIEW(dev_priv->dev));
-
 	/* PLL is protected by panel, make sure we can write it */
 	if (IS_MOBILE(dev_priv->dev))
 		assert_panel_unlocked(dev_priv, crtc->pipe);
@@ -1645,8 +1648,6 @@ static void chv_enable_pll(struct intel_crtc *crtc,
 
 	assert_pipe_disabled(dev_priv, crtc->pipe);
 
-	BUG_ON(!IS_CHERRYVIEW(dev_priv->dev));
-
 	mutex_lock(&dev_priv->sb_lock);
 
 	/* Enable back the 10bit clock to display controller */
@@ -1689,7 +1690,7 @@ static void i9xx_enable_pll(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int reg = DPLL(crtc->pipe);
+	i915_reg_t reg = DPLL(crtc->pipe);
 	u32 dpll = crtc->config->dpll_hw_state.dpll;
 
 	assert_pipe_disabled(dev_priv, crtc->pipe);
@@ -1838,7 +1839,7 @@ void vlv_wait_port_ready(struct drm_i915_private *dev_priv,
 			 unsigned int expected_mask)
 {
 	u32 port_mask;
-	int dpll_reg;
+	i915_reg_t dpll_reg;
 
 	switch (dport->port) {
 	case PORT_B:
@@ -1963,7 +1964,8 @@ static void ironlake_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 	struct drm_device *dev = dev_priv->dev;
 	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	uint32_t reg, val, pipeconf_val;
+	i915_reg_t reg;
+	uint32_t val, pipeconf_val;
 
 	/* PCH only available on ILK+ */
 	BUG_ON(!HAS_PCH_SPLIT(dev));
@@ -2052,7 +2054,8 @@ static void ironlake_disable_pch_transcoder(struct drm_i915_private *dev_priv,
 					    enum pipe pipe)
 {
 	struct drm_device *dev = dev_priv->dev;
-	uint32_t reg, val;
+	i915_reg_t reg;
+	uint32_t val;
 
 	/* FDI relies on the transcoder */
 	assert_fdi_tx_disabled(dev_priv, pipe);
@@ -2069,7 +2072,7 @@ static void ironlake_disable_pch_transcoder(struct drm_i915_private *dev_priv,
 	if (wait_for((I915_READ(reg) & TRANS_STATE_ENABLE) == 0, 50))
 		DRM_ERROR("failed to disable transcoder %c\n", pipe_name(pipe));
 
-	if (!HAS_PCH_IBX(dev)) {
+	if (HAS_PCH_CPT(dev)) {
 		/* Workaround: Clear the timing override chicken bit again. */
 		reg = TRANS_CHICKEN2(pipe);
 		val = I915_READ(reg);
@@ -2107,10 +2110,9 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe = crtc->pipe;
-	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
-								      pipe);
+	enum transcoder cpu_transcoder = crtc->config->cpu_transcoder;
 	enum pipe pch_transcoder;
-	int reg;
+	i915_reg_t reg;
 	u32 val;
 
 	DRM_DEBUG_KMS("enabling pipe %c\n", pipe_name(pipe));
@@ -2130,7 +2132,7 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	 * need the check.
 	 */
 	if (HAS_GMCH_DISPLAY(dev_priv->dev))
-		if (intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI))
+		if (crtc->config->has_dsi_encoder)
 			assert_dsi_pll_enabled(dev_priv);
 		else
 			assert_pll_enabled(dev_priv, pipe);
@@ -2171,7 +2173,7 @@ static void intel_disable_pipe(struct intel_crtc *crtc)
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	enum transcoder cpu_transcoder = crtc->config->cpu_transcoder;
 	enum pipe pipe = crtc->pipe;
-	int reg;
+	i915_reg_t reg;
 	u32 val;
 
 	DRM_DEBUG_KMS("disabling pipe %c\n", pipe_name(pipe));
@@ -2270,20 +2272,20 @@ intel_fb_align_height(struct drm_device *dev, unsigned int height,
 					       fb_format_modifier, 0));
 }
 
-static int
+static void
 intel_fill_fb_ggtt_view(struct i915_ggtt_view *view, struct drm_framebuffer *fb,
 			const struct drm_plane_state *plane_state)
 {
-	struct intel_rotation_info *info = &view->rotation_info;
+	struct intel_rotation_info *info = &view->params.rotation_info;
 	unsigned int tile_height, tile_pitch;
 
 	*view = i915_ggtt_view_normal;
 
 	if (!plane_state)
-		return 0;
+		return;
 
 	if (!intel_rotation_90_or_270(plane_state->rotation))
-		return 0;
+		return;
 
 	*view = i915_ggtt_view_rotated;
 
@@ -2310,8 +2312,6 @@ intel_fill_fb_ggtt_view(struct i915_ggtt_view *view, struct drm_framebuffer *fb,
 		info->size_uv = info->width_pages_uv * info->height_pages_uv *
 				PAGE_SIZE;
 	}
-
-	return 0;
 }
 
 static unsigned int intel_linear_alignment(struct drm_i915_private *dev_priv)
@@ -2319,7 +2319,7 @@ static unsigned int intel_linear_alignment(struct drm_i915_private *dev_priv)
 	if (INTEL_INFO(dev_priv)->gen >= 9)
 		return 256 * 1024;
 	else if (IS_BROADWATER(dev_priv) || IS_CRESTLINE(dev_priv) ||
-		 IS_VALLEYVIEW(dev_priv))
+		 IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		return 128 * 1024;
 	else if (INTEL_INFO(dev_priv)->gen >= 4)
 		return 4 * 1024;
@@ -2330,9 +2330,7 @@ static unsigned int intel_linear_alignment(struct drm_i915_private *dev_priv)
 int
 intel_pin_and_fence_fb_obj(struct drm_plane *plane,
 			   struct drm_framebuffer *fb,
-			   const struct drm_plane_state *plane_state,
-			   struct intel_engine_cs *pipelined,
-			   struct drm_i915_gem_request **pipelined_request)
+			   const struct drm_plane_state *plane_state)
 {
 	struct drm_device *dev = fb->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -2367,9 +2365,7 @@ intel_pin_and_fence_fb_obj(struct drm_plane *plane,
 		return -EINVAL;
 	}
 
-	ret = intel_fill_fb_ggtt_view(&view, fb, plane_state);
-	if (ret)
-		return ret;
+	intel_fill_fb_ggtt_view(&view, fb, plane_state);
 
 	/* Note that the w/a also requires 64 PTE of padding following the
 	 * bo. We currently fill all unused PTE with the shadow page and so
@@ -2388,11 +2384,10 @@ intel_pin_and_fence_fb_obj(struct drm_plane *plane,
 	 */
 	intel_runtime_pm_get(dev_priv);
 
-	dev_priv->mm.interruptible = false;
-	ret = i915_gem_object_pin_to_display_plane(obj, alignment, pipelined,
-						   pipelined_request, &view);
+	ret = i915_gem_object_pin_to_display_plane(obj, alignment,
+						   &view);
 	if (ret)
-		goto err_interruptible;
+		goto err_pm;
 
 	/* Install a fence for tiled scan-out. Pre-i965 always needs a
 	 * fence, whereas 965+ only requires a fence if using
@@ -2418,14 +2413,12 @@ intel_pin_and_fence_fb_obj(struct drm_plane *plane,
 		i915_gem_object_pin_fence(obj);
 	}
 
-	dev_priv->mm.interruptible = true;
 	intel_runtime_pm_put(dev_priv);
 	return 0;
 
 err_unpin:
 	i915_gem_object_unpin_from_display_plane(obj, &view);
-err_interruptible:
-	dev_priv->mm.interruptible = true;
+err_pm:
 	intel_runtime_pm_put(dev_priv);
 	return ret;
 }
@@ -2435,12 +2428,10 @@ static void intel_unpin_fb_obj(struct drm_framebuffer *fb,
 {
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	struct i915_ggtt_view view;
-	int ret;
 
 	WARN_ON(!mutex_is_locked(&obj->base.dev->struct_mutex));
 
-	ret = intel_fill_fb_ggtt_view(&view, fb, plane_state);
-	WARN_ONCE(ret, "Couldn't get view from plane state!");
+	intel_fill_fb_ggtt_view(&view, fb, plane_state);
 
 	if (view.type == I915_GGTT_VIEW_NORMAL)
 		i915_gem_object_unpin_fence(obj);
@@ -2695,7 +2686,7 @@ static void i9xx_update_primary_plane(struct drm_crtc *crtc,
 	int plane = intel_crtc->plane;
 	unsigned long linear_offset;
 	u32 dspcntr;
-	u32 reg = DSPCNTR(plane);
+	i915_reg_t reg = DSPCNTR(plane);
 	int pixel_size;
 
 	if (!visible || !fb) {
@@ -2825,7 +2816,7 @@ static void ironlake_update_primary_plane(struct drm_crtc *crtc,
 	int plane = intel_crtc->plane;
 	unsigned long linear_offset;
 	u32 dspcntr;
-	u32 reg = DSPCNTR(plane);
+	i915_reg_t reg = DSPCNTR(plane);
 	int pixel_size;
 
 	if (!visible || !fb) {
@@ -2950,30 +2941,32 @@ u32 intel_fb_stride_alignment(struct drm_device *dev, uint64_t fb_modifier,
 	}
 }
 
-unsigned long intel_plane_obj_offset(struct intel_plane *intel_plane,
-				     struct drm_i915_gem_object *obj,
-				     unsigned int plane)
+u32 intel_plane_obj_offset(struct intel_plane *intel_plane,
+			   struct drm_i915_gem_object *obj,
+			   unsigned int plane)
 {
-	const struct i915_ggtt_view *view = &i915_ggtt_view_normal;
+	struct i915_ggtt_view view;
 	struct i915_vma *vma;
-	unsigned char *offset;
+	u64 offset;
 
-	if (intel_rotation_90_or_270(intel_plane->base.state->rotation))
-		view = &i915_ggtt_view_rotated;
+	intel_fill_fb_ggtt_view(&view, intel_plane->base.state->fb,
+				intel_plane->base.state);
 
-	vma = i915_gem_obj_to_ggtt_view(obj, view);
+	vma = i915_gem_obj_to_ggtt_view(obj, &view);
 	if (WARN(!vma, "ggtt vma for display object not found! (view=%u)\n",
-		view->type))
+		view.type))
 		return -1;
 
-	offset = (unsigned char *)vma->node.start;
+	offset = vma->node.start;
 
 	if (plane == 1) {
-		offset += vma->ggtt_view.rotation_info.uv_start_page *
+		offset += vma->ggtt_view.params.rotation_info.uv_start_page *
 			  PAGE_SIZE;
 	}
 
-	return (unsigned long)offset;
+	WARN_ON(upper_32_bits(offset));
+
+	return lower_32_bits(offset);
 }
 
 static void skl_detach_scaler(struct intel_crtc *intel_crtc, int id)
@@ -3099,7 +3092,7 @@ static void skylake_update_primary_plane(struct drm_crtc *crtc,
 	u32 tile_height, plane_offset, plane_size;
 	unsigned int rotation;
 	int x_offset, y_offset;
-	unsigned long surf_addr;
+	u32 surf_addr;
 	struct intel_crtc_state *crtc_state = intel_crtc->config;
 	struct intel_plane_state *plane_state;
 	int src_x = 0, src_y = 0, src_w = 0, src_h = 0;
@@ -3197,8 +3190,8 @@ intel_pipe_set_base_atomic(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (dev_priv->fbc.disable_fbc)
-		dev_priv->fbc.disable_fbc(dev_priv);
+	if (dev_priv->fbc.deactivate)
+		dev_priv->fbc.deactivate(dev_priv);
 
 	dev_priv->display.update_primary_plane(crtc, fb, x, y);
 
@@ -3227,10 +3220,9 @@ static void intel_update_primary_planes(struct drm_device *dev)
 		struct intel_plane_state *plane_state;
 
 		drm_modeset_lock_crtc(crtc, &plane->base);
-
 		plane_state = to_intel_plane_state(plane->base.state);
 
-		if (plane_state->base.fb)
+		if (crtc->state->active && plane_state->base.fb)
 			plane->commit_plane(&plane->base, plane_state);
 
 		drm_modeset_unlock_crtc(crtc);
@@ -3306,32 +3298,6 @@ void intel_finish_reset(struct drm_device *dev)
 	drm_modeset_unlock_all(dev);
 }
 
-static void
-intel_finish_fb(struct drm_framebuffer *old_fb)
-{
-	struct drm_i915_gem_object *obj = intel_fb_obj(old_fb);
-	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
-	bool was_interruptible = dev_priv->mm.interruptible;
-	int ret;
-
-	/* Big Hammer, we also need to ensure that any pending
-	 * MI_WAIT_FOR_EVENT inside a user batch buffer on the
-	 * current scanout is retired before unpinning the old
-	 * framebuffer. Note that we rely on userspace rendering
-	 * into the buffer attached to the pipe they are waiting
-	 * on. If not, userspace generates a GPU hang with IPEHR
-	 * point to the MI_WAIT_FOR_EVENT.
-	 *
-	 * This should only fail upon a hung GPU, in which case we
-	 * can safely continue.
-	 */
-	dev_priv->mm.interruptible = false;
-	ret = i915_gem_object_wait_rendering(obj, true);
-	dev_priv->mm.interruptible = was_interruptible;
-
-	WARN_ON(ret);
-}
-
 static bool intel_crtc_has_pending_flip(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -3401,7 +3367,8 @@ static void intel_fdi_normal_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
+	i915_reg_t reg;
+	u32 temp;
 
 	/* enable normal train */
 	reg = FDI_TX_CTL(pipe);
@@ -3443,7 +3410,8 @@ static void ironlake_fdi_link_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, tries;
+	i915_reg_t reg;
+	u32 temp, tries;
 
 	/* FDI needs bits from pipe first */
 	assert_pipe_enabled(dev_priv, pipe);
@@ -3543,7 +3511,8 @@ static void gen6_fdi_link_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, i, retry;
+	i915_reg_t reg;
+	u32 temp, i, retry;
 
 	/* Train 1: umask FDI RX Interrupt symbol_lock and bit_lock bit
 	   for train result */
@@ -3675,7 +3644,8 @@ static void ivb_manual_fdi_link_train(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp, i, j;
+	i915_reg_t reg;
+	u32 temp, i, j;
 
 	/* Train 1: umask FDI RX Interrupt symbol_lock and bit_lock bit
 	   for train result */
@@ -3792,8 +3762,8 @@ static void ironlake_fdi_pll_enable(struct intel_crtc *intel_crtc)
 	struct drm_device *dev = intel_crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
-
+	i915_reg_t reg;
+	u32 temp;
 
 	/* enable PCH FDI RX PLL, wait warmup plus DMI latency */
 	reg = FDI_RX_CTL(pipe);
@@ -3829,7 +3799,8 @@ static void ironlake_fdi_pll_disable(struct intel_crtc *intel_crtc)
 	struct drm_device *dev = intel_crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
+	i915_reg_t reg;
+	u32 temp;
 
 	/* Switch from PCDclk to Rawclk */
 	reg = FDI_RX_CTL(pipe);
@@ -3859,7 +3830,8 @@ static void ironlake_fdi_disable(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
+	i915_reg_t reg;
+	u32 temp;
 
 	/* disable CPU FDI tx and PCH FDI rx */
 	reg = FDI_TX_CTL(pipe);
@@ -3952,15 +3924,23 @@ static void page_flip_completed(struct intel_crtc *intel_crtc)
 				 work->pending_flip_obj);
 }
 
-void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
+static int intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	long ret;
 
 	WARN_ON(waitqueue_active(&dev_priv->pending_flip_queue));
-	if (WARN_ON(wait_event_timeout(dev_priv->pending_flip_queue,
-				       !intel_crtc_has_pending_flip(crtc),
-				       60*HZ) == 0)) {
+
+	ret = wait_event_interruptible_timeout(
+					dev_priv->pending_flip_queue,
+					!intel_crtc_has_pending_flip(crtc),
+					60*HZ);
+
+	if (ret < 0)
+		return ret;
+
+	if (ret == 0) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
 		spin_lock_irq(&dev->event_lock);
@@ -3971,11 +3951,22 @@ void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 		spin_unlock_irq(&dev->event_lock);
 	}
 
-	if (crtc->primary->fb) {
-		mutex_lock(&dev->struct_mutex);
-		intel_finish_fb(crtc->primary->fb);
-		mutex_unlock(&dev->struct_mutex);
-	}
+	return 0;
+}
+
+static void lpt_disable_iclkip(struct drm_i915_private *dev_priv)
+{
+	u32 temp;
+
+	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_GATE);
+
+	mutex_lock(&dev_priv->sb_lock);
+
+	temp = intel_sbi_read(dev_priv, SBI_SSCCTL6, SBI_ICLK);
+	temp |= SBI_SSCCTL_DISABLE;
+	intel_sbi_write(dev_priv, SBI_SSCCTL6, temp, SBI_ICLK);
+
+	mutex_unlock(&dev_priv->sb_lock);
 }
 
 /* Program iCLKIP clock to the desired frequency */
@@ -3987,18 +3978,7 @@ static void lpt_program_iclkip(struct drm_crtc *crtc)
 	u32 divsel, phaseinc, auxdiv, phasedir = 0;
 	u32 temp;
 
-	mutex_lock(&dev_priv->sb_lock);
-
-	/* It is necessary to ungate the pixclk gate prior to programming
-	 * the divisors, and gate it back when it is done.
-	 */
-	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_GATE);
-
-	/* Disable SSCCTL */
-	intel_sbi_write(dev_priv, SBI_SSCCTL6,
-			intel_sbi_read(dev_priv, SBI_SSCCTL6, SBI_ICLK) |
-				SBI_SSCCTL_DISABLE,
-			SBI_ICLK);
+	lpt_disable_iclkip(dev_priv);
 
 	/* 20MHz is a corner case which is out of range for the 7-bit divisor */
 	if (clock == 20000) {
@@ -4016,7 +3996,7 @@ static void lpt_program_iclkip(struct drm_crtc *crtc)
 		u32 iclk_pi_range = 64;
 		u32 desired_divisor, msb_divisor_value, pi_value;
 
-		desired_divisor = (iclk_virtual_root_freq / clock);
+		desired_divisor = DIV_ROUND_CLOSEST(iclk_virtual_root_freq, clock);
 		msb_divisor_value = desired_divisor / iclk_pi_range;
 		pi_value = desired_divisor % iclk_pi_range;
 
@@ -4038,6 +4018,8 @@ static void lpt_program_iclkip(struct drm_crtc *crtc)
 			phasedir,
 			phaseinc);
 
+	mutex_lock(&dev_priv->sb_lock);
+
 	/* Program SSCDIVINTPHASE6 */
 	temp = intel_sbi_read(dev_priv, SBI_SSCDIVINTPHASE6, SBI_ICLK);
 	temp &= ~SBI_SSCDIVINTPHASE_DIVSEL_MASK;
@@ -4059,12 +4041,12 @@ static void lpt_program_iclkip(struct drm_crtc *crtc)
 	temp &= ~SBI_SSCCTL_DISABLE;
 	intel_sbi_write(dev_priv, SBI_SSCCTL6, temp, SBI_ICLK);
 
+	mutex_unlock(&dev_priv->sb_lock);
+
 	/* Wait for initialization time */
 	udelay(24);
 
 	I915_WRITE(PIXCLK_GATE, PIXCLK_GATE_UNGATE);
-
-	mutex_unlock(&dev_priv->sb_lock);
 }
 
 static void ironlake_pch_transcoder_set_timings(struct intel_crtc *crtc,
@@ -4135,6 +4117,22 @@ static void ivybridge_update_fdi_bc_bifurcation(struct intel_crtc *intel_crtc)
 	}
 }
 
+/* Return which DP Port should be selected for Transcoder DP control */
+static enum port
+intel_trans_dp_port_sel(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct intel_encoder *encoder;
+
+	for_each_encoder_on_crtc(dev, crtc, encoder) {
+		if (encoder->type == INTEL_OUTPUT_DISPLAYPORT ||
+		    encoder->type == INTEL_OUTPUT_EDP)
+			return enc_to_dig_port(&encoder->base)->port;
+	}
+
+	return -1;
+}
+
 /*
  * Enable PCH resources required for PCH ports:
  *   - PCH PLLs
@@ -4149,7 +4147,7 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
+	u32 temp;
 
 	assert_pch_transcoder_disabled(dev_priv, pipe);
 
@@ -4161,6 +4159,12 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 	I915_WRITE(FDI_RX_TUSIZE1(pipe),
 		   I915_READ(PIPE_DATA_M1(pipe)) & TU_SIZE_MASK);
 
+	/*
+	 * Sometimes spurious CPU pipe underruns happen during FDI
+	 * training, at least with VGA+HDMI cloning. Suppress them.
+	 */
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
+
 	/* For PCH output, training FDI link */
 	dev_priv->display.fdi_link_train(crtc);
 
@@ -4194,10 +4198,14 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 
 	intel_fdi_normal_train(crtc);
 
+	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+
 	/* For PCH DP, enable TRANS_DP_CTL */
 	if (HAS_PCH_CPT(dev) && intel_crtc->config->has_dp_encoder) {
+		const struct drm_display_mode *adjusted_mode =
+			&intel_crtc->config->base.adjusted_mode;
 		u32 bpc = (I915_READ(PIPECONF(pipe)) & PIPECONF_BPC_MASK) >> 5;
-		reg = TRANS_DP_CTL(pipe);
+		i915_reg_t reg = TRANS_DP_CTL(pipe);
 		temp = I915_READ(reg);
 		temp &= ~(TRANS_DP_PORT_SEL_MASK |
 			  TRANS_DP_SYNC_MASK |
@@ -4205,19 +4213,19 @@ static void ironlake_pch_enable(struct drm_crtc *crtc)
 		temp |= TRANS_DP_OUTPUT_ENABLE;
 		temp |= bpc << 9; /* same format but at 11:9 */
 
-		if (crtc->mode.flags & DRM_MODE_FLAG_PHSYNC)
+		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
 			temp |= TRANS_DP_HSYNC_ACTIVE_HIGH;
-		if (crtc->mode.flags & DRM_MODE_FLAG_PVSYNC)
+		if (adjusted_mode->flags & DRM_MODE_FLAG_PVSYNC)
 			temp |= TRANS_DP_VSYNC_ACTIVE_HIGH;
 
 		switch (intel_trans_dp_port_sel(crtc)) {
-		case PCH_DP_B:
+		case PORT_B:
 			temp |= TRANS_DP_PORT_SEL_B;
 			break;
-		case PCH_DP_C:
+		case PORT_C:
 			temp |= TRANS_DP_PORT_SEL_C;
 			break;
-		case PCH_DP_D:
+		case PORT_D:
 			temp |= TRANS_DP_PORT_SEL_D;
 			break;
 		default:
@@ -4357,7 +4365,7 @@ static void intel_shared_dpll_commit(struct drm_atomic_state *state)
 static void cpt_verify_modeset(struct drm_device *dev, int pipe)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int dslreg = PIPEDSL(pipe);
+	i915_reg_t dslreg = PIPEDSL(pipe);
 	u32 temp;
 
 	temp = I915_READ(dslreg);
@@ -4650,7 +4658,7 @@ static void intel_crtc_load_lut(struct drm_crtc *crtc)
 		return;
 
 	if (HAS_GMCH_DISPLAY(dev_priv->dev)) {
-		if (intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI))
+		if (intel_crtc->config->has_dsi_encoder)
 			assert_dsi_pll_enabled(dev_priv);
 		else
 			assert_pll_enabled(dev_priv, pipe);
@@ -4667,7 +4675,7 @@ static void intel_crtc_load_lut(struct drm_crtc *crtc)
 	}
 
 	for (i = 0; i < 256; i++) {
-		u32 palreg;
+		i915_reg_t palreg;
 
 		if (HAS_GMCH_DISPLAY(dev))
 			palreg = PALETTE(pipe, i);
@@ -4721,14 +4729,6 @@ intel_post_enable_primary(struct drm_crtc *crtc)
 	int pipe = intel_crtc->pipe;
 
 	/*
-	 * BDW signals flip done immediately if the plane
-	 * is disabled, even if the plane enable is already
-	 * armed to occur at the next vblank :(
-	 */
-	if (IS_BROADWELL(dev))
-		intel_wait_for_vblank(dev, pipe);
-
-	/*
 	 * FIXME IPS should be fine as long as one plane is
 	 * enabled, but in practice it seems to have problems
 	 * when going from primary only to sprite only and vice
@@ -4746,9 +4746,9 @@ intel_post_enable_primary(struct drm_crtc *crtc)
 	if (IS_GEN2(dev))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
 
-	/* Underruns don't raise interrupts, so check manually. */
-	if (HAS_GMCH_DISPLAY(dev))
-		i9xx_check_fifo_underruns(dev_priv);
+	/* Underruns don't always raise interrupts, so check manually. */
+	intel_check_cpu_fifo_underruns(dev_priv);
+	intel_check_pch_fifo_underruns(dev_priv);
 }
 
 /**
@@ -4805,31 +4805,26 @@ intel_pre_disable_primary(struct drm_crtc *crtc)
 static void intel_post_plane_update(struct intel_crtc *crtc)
 {
 	struct intel_crtc_atomic_commit *atomic = &crtc->atomic;
+	struct intel_crtc_state *pipe_config =
+		to_intel_crtc_state(crtc->base.state);
 	struct drm_device *dev = crtc->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_plane *plane;
 
 	if (atomic->wait_vblank)
 		intel_wait_for_vblank(dev, crtc->pipe);
 
 	intel_frontbuffer_flip(dev, atomic->fb_bits);
 
-	if (atomic->disable_cxsr)
-		crtc->wm.cxsr_allowed = true;
+	crtc->wm.cxsr_allowed = true;
 
-	if (crtc->atomic.update_wm_post)
+	if (pipe_config->wm_changed && pipe_config->base.active)
 		intel_update_watermarks(&crtc->base);
 
 	if (atomic->update_fbc)
-		intel_fbc_update(dev_priv);
+		intel_fbc_update(crtc);
 
 	if (atomic->post_enable_primary)
 		intel_post_enable_primary(&crtc->base);
 
-	drm_for_each_plane_mask(plane, dev, atomic->update_sprite_watermarks)
-		intel_update_sprite_watermarks(plane, &crtc->base,
-					       0, 0, 0, false, false);
-
 	memset(atomic, 0, sizeof(*atomic));
 }
 
@@ -4838,23 +4833,11 @@ static void intel_pre_plane_update(struct intel_crtc *crtc)
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc_atomic_commit *atomic = &crtc->atomic;
-	struct drm_plane *p;
-
-	/* Track fb's for any planes being disabled */
-	drm_for_each_plane_mask(p, dev, atomic->disabled_planes) {
-		struct intel_plane *plane = to_intel_plane(p);
-
-		mutex_lock(&dev->struct_mutex);
-		i915_gem_track_fb(intel_fb_obj(plane->base.fb), NULL,
-				  plane->frontbuffer_bit);
-		mutex_unlock(&dev->struct_mutex);
-	}
-
-	if (atomic->wait_for_flips)
-		intel_crtc_wait_for_pending_flips(&crtc->base);
+	struct intel_crtc_state *pipe_config =
+		to_intel_crtc_state(crtc->base.state);
 
 	if (atomic->disable_fbc)
-		intel_fbc_disable_crtc(crtc);
+		intel_fbc_deactivate(crtc);
 
 	if (crtc->atomic.disable_ips)
 		hsw_disable_ips(crtc);
@@ -4862,10 +4845,13 @@ static void intel_pre_plane_update(struct intel_crtc *crtc)
 	if (atomic->pre_disable_primary)
 		intel_pre_disable_primary(&crtc->base);
 
-	if (atomic->disable_cxsr) {
+	if (pipe_config->disable_cxsr) {
 		crtc->wm.cxsr_allowed = false;
 		intel_set_memory_cxsr(dev_priv, false);
 	}
+
+	if (!needs_modeset(&pipe_config->base) && pipe_config->wm_changed)
+		intel_update_watermarks(&crtc->base);
 }
 
 static void intel_crtc_disable_planes(struct drm_crtc *crtc, unsigned plane_mask)
@@ -4900,6 +4886,9 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 		return;
 
 	if (intel_crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, false);
+
+	if (intel_crtc->config->has_pch_encoder)
 		intel_prepare_shared_dpll(intel_crtc);
 
 	if (intel_crtc->config->has_dp_encoder)
@@ -4917,7 +4906,6 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	intel_crtc->active = true;
 
 	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
-	intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, true);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->pre_enable)
@@ -4955,6 +4943,13 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 
 	if (HAS_PCH_CPT(dev))
 		cpt_verify_modeset(dev, intel_crtc->pipe);
+
+	/* Must wait for vblank to avoid spurious PCH FIFO underruns */
+	if (intel_crtc->config->has_pch_encoder)
+		intel_wait_for_vblank(dev, pipe);
+	intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, true);
+
+	intel_fbc_enable(intel_crtc);
 }
 
 /* IPS only exists on ULT machines and is tied to pipe A. */
@@ -4972,11 +4967,14 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	int pipe = intel_crtc->pipe, hsw_workaround_pipe;
 	struct intel_crtc_state *pipe_config =
 		to_intel_crtc_state(crtc->state);
-	bool is_dsi = intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI);
 
 	if (WARN_ON(intel_crtc->active))
 		return;
 
+	if (intel_crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      false);
+
 	if (intel_crtc_to_shared_dpll(intel_crtc))
 		intel_enable_shared_dpll(intel_crtc);
 
@@ -5001,21 +4999,20 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 
 	intel_crtc->active = true;
 
-	intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+	if (intel_crtc->config->has_pch_encoder)
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
+	else
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+
 	for_each_encoder_on_crtc(dev, crtc, encoder) {
-		if (encoder->pre_pll_enable)
-			encoder->pre_pll_enable(encoder);
 		if (encoder->pre_enable)
 			encoder->pre_enable(encoder);
 	}
 
-	if (intel_crtc->config->has_pch_encoder) {
-		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
-						      true);
+	if (intel_crtc->config->has_pch_encoder)
 		dev_priv->display.fdi_link_train(crtc);
-	}
 
-	if (!is_dsi)
+	if (!intel_crtc->config->has_dsi_encoder)
 		intel_ddi_enable_pipe_clock(intel_crtc);
 
 	if (INTEL_INFO(dev)->gen >= 9)
@@ -5030,7 +5027,7 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	intel_crtc_load_lut(crtc);
 
 	intel_ddi_set_pipe_settings(crtc);
-	if (!is_dsi)
+	if (!intel_crtc->config->has_dsi_encoder)
 		intel_ddi_enable_transcoder_func(crtc);
 
 	intel_update_watermarks(crtc);
@@ -5039,7 +5036,7 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	if (intel_crtc->config->has_pch_encoder)
 		lpt_pch_enable(crtc);
 
-	if (intel_crtc->config->dp_encoder_is_mst && !is_dsi)
+	if (intel_crtc->config->dp_encoder_is_mst)
 		intel_ddi_set_vc_payload_alloc(crtc, true);
 
 	assert_vblank_disabled(crtc);
@@ -5050,6 +5047,14 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 		intel_opregion_notify_encoder(encoder, true);
 	}
 
+	if (intel_crtc->config->has_pch_encoder) {
+		intel_wait_for_vblank(dev, pipe);
+		intel_wait_for_vblank(dev, pipe);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      true);
+	}
+
 	/* If we change the relative order between pipe/planes enabling, we need
 	 * to change the workaround. */
 	hsw_workaround_pipe = pipe_config->hsw_workaround_pipe;
@@ -5057,6 +5062,8 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 		intel_wait_for_vblank(dev, hsw_workaround_pipe);
 		intel_wait_for_vblank(dev, hsw_workaround_pipe);
 	}
+
+	intel_fbc_enable(intel_crtc);
 }
 
 static void ironlake_pfit_disable(struct intel_crtc *crtc, bool force)
@@ -5081,7 +5088,9 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	u32 reg, temp;
+
+	if (intel_crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->disable(encoder);
@@ -5089,15 +5098,22 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 	drm_crtc_vblank_off(crtc);
 	assert_vblank_disabled(crtc);
 
+	/*
+	 * Sometimes spurious CPU pipe underruns happen when the
+	 * pipe is already disabled, but FDI RX/TX is still enabled.
+	 * Happens at least with VGA+HDMI cloning. Suppress them.
+	 */
 	if (intel_crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, false);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
 
 	intel_disable_pipe(intel_crtc);
 
 	ironlake_pfit_disable(intel_crtc, false);
 
-	if (intel_crtc->config->has_pch_encoder)
+	if (intel_crtc->config->has_pch_encoder) {
 		ironlake_fdi_disable(crtc);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+	}
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		if (encoder->post_disable)
@@ -5107,6 +5123,9 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 		ironlake_disable_pch_transcoder(dev_priv, pipe);
 
 		if (HAS_PCH_CPT(dev)) {
+			i915_reg_t reg;
+			u32 temp;
+
 			/* disable TRANS_DP_CTL */
 			reg = TRANS_DP_CTL(pipe);
 			temp = I915_READ(reg);
@@ -5123,6 +5142,10 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 
 		ironlake_fdi_pll_disable(intel_crtc);
 	}
+
+	intel_set_pch_fifo_underrun_reporting(dev_priv, pipe, true);
+
+	intel_fbc_disable_crtc(intel_crtc);
 }
 
 static void haswell_crtc_disable(struct drm_crtc *crtc)
@@ -5132,7 +5155,10 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	enum transcoder cpu_transcoder = intel_crtc->config->cpu_transcoder;
-	bool is_dsi = intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI);
+
+	if (intel_crtc->config->has_pch_encoder)
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      false);
 
 	for_each_encoder_on_crtc(dev, crtc, encoder) {
 		intel_opregion_notify_encoder(encoder, false);
@@ -5142,15 +5168,12 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 	drm_crtc_vblank_off(crtc);
 	assert_vblank_disabled(crtc);
 
-	if (intel_crtc->config->has_pch_encoder)
-		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
-						      false);
 	intel_disable_pipe(intel_crtc);
 
 	if (intel_crtc->config->dp_encoder_is_mst)
 		intel_ddi_set_vc_payload_alloc(crtc, false);
 
-	if (!is_dsi)
+	if (!intel_crtc->config->has_dsi_encoder)
 		intel_ddi_disable_transcoder_func(dev_priv, cpu_transcoder);
 
 	if (INTEL_INFO(dev)->gen >= 9)
@@ -5158,17 +5181,23 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 	else
 		ironlake_pfit_disable(intel_crtc, false);
 
-	if (!is_dsi)
+	if (!intel_crtc->config->has_dsi_encoder)
 		intel_ddi_disable_pipe_clock(intel_crtc);
 
+	for_each_encoder_on_crtc(dev, crtc, encoder)
+		if (encoder->post_disable)
+			encoder->post_disable(encoder);
+
 	if (intel_crtc->config->has_pch_encoder) {
 		lpt_disable_pch_transcoder(dev_priv);
+		lpt_disable_iclkip(dev_priv);
 		intel_ddi_fdi_disable(crtc);
+
+		intel_set_pch_fifo_underrun_reporting(dev_priv, TRANSCODER_A,
+						      true);
 	}
 
-	for_each_encoder_on_crtc(dev, crtc, encoder)
-		if (encoder->post_disable)
-			encoder->post_disable(encoder);
+	intel_fbc_disable_crtc(intel_crtc);
 }
 
 static void i9xx_pfit_enable(struct intel_crtc *crtc)
@@ -5199,15 +5228,15 @@ static enum intel_display_power_domain port_to_power_domain(enum port port)
 {
 	switch (port) {
 	case PORT_A:
-		return POWER_DOMAIN_PORT_DDI_A_4_LANES;
+		return POWER_DOMAIN_PORT_DDI_A_LANES;
 	case PORT_B:
-		return POWER_DOMAIN_PORT_DDI_B_4_LANES;
+		return POWER_DOMAIN_PORT_DDI_B_LANES;
 	case PORT_C:
-		return POWER_DOMAIN_PORT_DDI_C_4_LANES;
+		return POWER_DOMAIN_PORT_DDI_C_LANES;
 	case PORT_D:
-		return POWER_DOMAIN_PORT_DDI_D_4_LANES;
+		return POWER_DOMAIN_PORT_DDI_D_LANES;
 	case PORT_E:
-		return POWER_DOMAIN_PORT_DDI_E_2_LANES;
+		return POWER_DOMAIN_PORT_DDI_E_LANES;
 	default:
 		MISSING_CASE(port);
 		return POWER_DOMAIN_PORT_OTHER;
@@ -5234,10 +5263,6 @@ static enum intel_display_power_domain port_to_aux_power_domain(enum port port)
 	}
 }
 
-#define for_each_power_domain(domain, mask)				\
-	for ((domain) = 0; (domain) < POWER_DOMAIN_NUM; (domain)++)	\
-		if ((1 << (domain)) & (mask))
-
 enum intel_display_power_domain
 intel_display_port_power_domain(struct intel_encoder *intel_encoder)
 {
@@ -5302,13 +5327,11 @@ static unsigned long get_crtc_power_domains(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	enum pipe pipe = intel_crtc->pipe;
 	unsigned long mask;
-	enum transcoder transcoder;
+	enum transcoder transcoder = intel_crtc->config->cpu_transcoder;
 
 	if (!crtc->state->active)
 		return 0;
 
-	transcoder = intel_pipe_to_cpu_transcoder(dev->dev_private, pipe);
-
 	mask = BIT(POWER_DOMAIN_PIPE(pipe));
 	mask |= BIT(POWER_DOMAIN_TRANSCODER(transcoder));
 	if (intel_crtc->config->pch_pfit.enabled ||
@@ -5395,7 +5418,7 @@ static void intel_update_max_cdclk(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		u32 limit = I915_READ(SKL_DFSM) & SKL_DFSM_CDCLK_LIMIT_MASK;
 
 		if (limit == SKL_DFSM_CDCLK_LIMIT_675)
@@ -5452,7 +5475,7 @@ static void intel_update_cdclk(struct drm_device *dev)
 	 * BSpec erroneously claims we should aim for 4MHz, but
 	 * in fact 1MHz is the correct frequency.
 	 */
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		/*
 		 * Program the gmbus_freq based on the cdclk frequency.
 		 * BSpec erroneously claims we should aim for 4MHz, but
@@ -5812,32 +5835,16 @@ void skl_uninit_cdclk(struct drm_i915_private *dev_priv)
 	if (I915_READ(DBUF_CTL) & DBUF_POWER_STATE)
 		DRM_ERROR("DBuf power disable timeout\n");
 
-	/*
-	 * DMC assumes ownership of LCPLL and will get confused if we touch it.
-	 */
-	if (dev_priv->csr.dmc_payload) {
-		/* disable DPLL0 */
-		I915_WRITE(LCPLL1_CTL, I915_READ(LCPLL1_CTL) &
-					~LCPLL_PLL_ENABLE);
-		if (wait_for(!(I915_READ(LCPLL1_CTL) & LCPLL_PLL_LOCK), 1))
-			DRM_ERROR("Couldn't disable DPLL0\n");
-	}
-
-	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+	/* disable DPLL0 */
+	I915_WRITE(LCPLL1_CTL, I915_READ(LCPLL1_CTL) & ~LCPLL_PLL_ENABLE);
+	if (wait_for(!(I915_READ(LCPLL1_CTL) & LCPLL_PLL_LOCK), 1))
+		DRM_ERROR("Couldn't disable DPLL0\n");
 }
 
 void skl_init_cdclk(struct drm_i915_private *dev_priv)
 {
-	u32 val;
 	unsigned int required_vco;
 
-	/* enable PCH reset handshake */
-	val = I915_READ(HSW_NDE_RSTWRN_OPT);
-	I915_WRITE(HSW_NDE_RSTWRN_OPT, val | RESET_PCH_HANDSHAKE_ENABLE);
-
-	/* enable PG1 and Misc I/O */
-	intel_display_power_get(dev_priv, POWER_DOMAIN_PLLS);
-
 	/* DPLL0 not enabled (happens on early BIOS versions) */
 	if (!(I915_READ(LCPLL1_CTL) & LCPLL_PLL_ENABLE)) {
 		/* enable DPLL0 */
@@ -5858,6 +5865,45 @@ void skl_init_cdclk(struct drm_i915_private *dev_priv)
 		DRM_ERROR("DBuf power enable timeout\n");
 }
 
+int skl_sanitize_cdclk(struct drm_i915_private *dev_priv)
+{
+	uint32_t lcpll1 = I915_READ(LCPLL1_CTL);
+	uint32_t cdctl = I915_READ(CDCLK_CTL);
+	int freq = dev_priv->skl_boot_cdclk;
+
+	/*
+	 * check if the pre-os intialized the display
+	 * There is SWF18 scratchpad register defined which is set by the
+	 * pre-os which can be used by the OS drivers to check the status
+	 */
+	if ((I915_READ(SWF_ILK(0x18)) & 0x00FFFFFF) == 0)
+		goto sanitize;
+
+	/* Is PLL enabled and locked ? */
+	if (!((lcpll1 & LCPLL_PLL_ENABLE) && (lcpll1 & LCPLL_PLL_LOCK)))
+		goto sanitize;
+
+	/* DPLL okay; verify the cdclock
+	 *
+	 * Noticed in some instances that the freq selection is correct but
+	 * decimal part is programmed wrong from BIOS where pre-os does not
+	 * enable display. Verify the same as well.
+	 */
+	if (cdctl == ((cdctl & CDCLK_FREQ_SEL_MASK) | skl_cdclk_decimal(freq)))
+		/* All well; nothing to sanitize */
+		return false;
+sanitize:
+	/*
+	 * As of now initialize with max cdclk till
+	 * we get dynamic cdclk support
+	 * */
+	dev_priv->skl_boot_cdclk = dev_priv->max_cdclk_freq;
+	skl_init_cdclk(dev_priv);
+
+	/* we did have to sanitize */
+	return true;
+}
+
 /* Adjust CDclk dividers to allow high res or save power if possible */
 static void valleyview_set_cdclk(struct drm_device *dev, int cdclk)
 {
@@ -6139,13 +6185,10 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	bool is_dsi;
 
 	if (WARN_ON(intel_crtc->active))
 		return;
 
-	is_dsi = intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI);
-
 	if (intel_crtc->config->has_dp_encoder)
 		intel_dp_set_m_n(intel_crtc, M1_N1);
 
@@ -6168,7 +6211,7 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 		if (encoder->pre_pll_enable)
 			encoder->pre_pll_enable(encoder);
 
-	if (!is_dsi) {
+	if (!intel_crtc->config->has_dsi_encoder) {
 		if (IS_CHERRYVIEW(dev)) {
 			chv_prepare_pll(intel_crtc, intel_crtc->config);
 			chv_enable_pll(intel_crtc, intel_crtc->config);
@@ -6247,6 +6290,8 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 
 	for_each_encoder_on_crtc(dev, crtc, encoder)
 		encoder->enable(encoder);
+
+	intel_fbc_enable(intel_crtc);
 }
 
 static void i9xx_pfit_disable(struct intel_crtc *crtc)
@@ -6294,7 +6339,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 		if (encoder->post_disable)
 			encoder->post_disable(encoder);
 
-	if (!intel_pipe_has_type(intel_crtc, INTEL_OUTPUT_DSI)) {
+	if (!intel_crtc->config->has_dsi_encoder) {
 		if (IS_CHERRYVIEW(dev))
 			chv_disable_pll(dev_priv, pipe);
 		else if (IS_VALLEYVIEW(dev))
@@ -6309,6 +6354,8 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 
 	if (!IS_GEN2(dev))
 		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
+
+	intel_fbc_disable_crtc(intel_crtc);
 }
 
 static void intel_crtc_disable_noatomic(struct drm_crtc *crtc)
@@ -6322,7 +6369,8 @@ static void intel_crtc_disable_noatomic(struct drm_crtc *crtc)
 		return;
 
 	if (to_intel_plane_state(crtc->primary->state)->visible) {
-		intel_crtc_wait_for_pending_flips(crtc);
+		WARN_ON(intel_crtc->unpin_work);
+
 		intel_pre_disable_primary(crtc);
 
 		intel_crtc_disable_planes(crtc, 1 << drm_plane_index(crtc->primary));
@@ -6447,13 +6495,11 @@ static void intel_connector_check_state(struct intel_connector *connector)
 
 int intel_connector_init(struct intel_connector *connector)
 {
-	struct drm_connector_state *connector_state;
+	drm_atomic_helper_connector_reset(&connector->base);
 
-	connector_state = kzalloc(sizeof *connector_state, GFP_KERNEL);
-	if (!connector_state)
+	if (!connector->base.state)
 		return -ENOMEM;
 
-	connector->base.state = connector_state;
 	return 0;
 }
 
@@ -6642,6 +6688,15 @@ static void hsw_compute_ips_config(struct intel_crtc *crtc,
 		pipe_config_supports_ips(dev_priv, pipe_config);
 }
 
+static bool intel_crtc_supports_double_wide(const struct intel_crtc *crtc)
+{
+	const struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
+
+	/* GDG double wide on either pipe, otherwise pipe A only */
+	return INTEL_INFO(dev_priv)->gen < 4 &&
+		(crtc->pipe == PIPE_A || IS_I915G(dev_priv));
+}
+
 static int intel_crtc_compute_config(struct intel_crtc *crtc,
 				     struct intel_crtc_state *pipe_config)
 {
@@ -6651,23 +6706,24 @@ static int intel_crtc_compute_config(struct intel_crtc *crtc,
 
 	/* FIXME should check pixel clock limits on all platforms */
 	if (INTEL_INFO(dev)->gen < 4) {
-		int clock_limit = dev_priv->max_cdclk_freq;
+		int clock_limit = dev_priv->max_cdclk_freq * 9 / 10;
 
 		/*
-		 * Enable pixel doubling when the dot clock
+		 * Enable double wide mode when the dot clock
 		 * is > 90% of the (display) core speed.
-		 *
-		 * GDG double wide on either pipe,
-		 * otherwise pipe A only.
 		 */
-		if ((crtc->pipe == PIPE_A || IS_I915G(dev)) &&
-		    adjusted_mode->crtc_clock > clock_limit * 9 / 10) {
+		if (intel_crtc_supports_double_wide(crtc) &&
+		    adjusted_mode->crtc_clock > clock_limit) {
 			clock_limit *= 2;
 			pipe_config->double_wide = true;
 		}
 
-		if (adjusted_mode->crtc_clock > clock_limit * 9 / 10)
+		if (adjusted_mode->crtc_clock > clock_limit) {
+			DRM_DEBUG_KMS("requested pixel clock (%d kHz) too high (max: %d kHz, double wide: %s)\n",
+				      adjusted_mode->crtc_clock, clock_limit,
+				      yesno(pipe_config->double_wide));
 			return -EINVAL;
+		}
 	}
 
 	/*
@@ -7144,7 +7200,7 @@ static int i9xx_get_refclk(const struct intel_crtc_state *crtc_state,
 
 	WARN_ON(!crtc_state->base.state);
 
-	if (IS_VALLEYVIEW(dev) || IS_BROXTON(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev) || IS_BROXTON(dev)) {
 		refclk = 100000;
 	} else if (intel_pipe_will_have_type(crtc_state, INTEL_OUTPUT_LVDS) &&
 	    intel_panel_use_ssc(dev_priv) && num_connectors < 2) {
@@ -7432,7 +7488,7 @@ static void chv_prepare_pll(struct intel_crtc *crtc,
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe = crtc->pipe;
-	int dpll_reg = DPLL(crtc->pipe);
+	i915_reg_t dpll_reg = DPLL(crtc->pipe);
 	enum dpio_channel port = vlv_pipe_to_channel(pipe);
 	u32 loopfilter, tribuf_calcntr;
 	u32 bestn, bestm1, bestm2, bestp1, bestp2, bestm2_frac;
@@ -7843,7 +7899,7 @@ 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)) {
+	if (IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		/* 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 |
@@ -7883,7 +7939,8 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 	} else
 		pipeconf |= PIPECONF_PROGRESSIVE;
 
-	if (IS_VALLEYVIEW(dev) && intel_crtc->config->limited_color_range)
+	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	     intel_crtc->config->limited_color_range)
 		pipeconf |= PIPECONF_COLOR_RANGE_SELECT;
 
 	I915_WRITE(PIPECONF(intel_crtc->pipe), pipeconf);
@@ -7898,8 +7955,6 @@ static int i9xx_crtc_compute_clock(struct intel_crtc *crtc,
 	int refclk, num_connectors = 0;
 	intel_clock_t clock;
 	bool ok;
-	bool is_dsi = false;
-	struct intel_encoder *encoder;
 	const intel_limit_t *limit;
 	struct drm_atomic_state *state = crtc_state->base.state;
 	struct drm_connector *connector;
@@ -7909,26 +7964,14 @@ static int i9xx_crtc_compute_clock(struct intel_crtc *crtc,
 	memset(&crtc_state->dpll_hw_state, 0,
 	       sizeof(crtc_state->dpll_hw_state));
 
-	for_each_connector_in_state(state, connector, connector_state, i) {
-		if (connector_state->crtc != &crtc->base)
-			continue;
-
-		encoder = to_intel_encoder(connector_state->best_encoder);
-
-		switch (encoder->type) {
-		case INTEL_OUTPUT_DSI:
-			is_dsi = true;
-			break;
-		default:
-			break;
-		}
+	if (crtc_state->has_dsi_encoder)
+		return 0;
 
-		num_connectors++;
+	for_each_connector_in_state(state, connector, connector_state, i) {
+		if (connector_state->crtc == &crtc->base)
+			num_connectors++;
 	}
 
-	if (is_dsi)
-		return 0;
-
 	if (!crtc_state->clock_set) {
 		refclk = i9xx_get_refclk(crtc_state, num_connectors);
 
@@ -8131,20 +8174,24 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum intel_display_power_domain power_domain;
 	uint32_t tmp;
+	bool ret;
 
-	if (!intel_display_power_is_enabled(dev_priv,
-					    POWER_DOMAIN_PIPE(crtc->pipe)))
+	power_domain = POWER_DOMAIN_PIPE(crtc->pipe);
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
 	pipe_config->cpu_transcoder = (enum transcoder) crtc->pipe;
 	pipe_config->shared_dpll = DPLL_ID_PRIVATE;
 
+	ret = false;
+
 	tmp = I915_READ(PIPECONF(crtc->pipe));
 	if (!(tmp & PIPECONF_ENABLE))
-		return false;
+		goto out;
 
-	if (IS_G4X(dev) || IS_VALLEYVIEW(dev)) {
+	if (IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		switch (tmp & PIPECONF_BPC_MASK) {
 		case PIPECONF_6BPC:
 			pipe_config->pipe_bpp = 18;
@@ -8160,7 +8207,8 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 		}
 	}
 
-	if (IS_VALLEYVIEW(dev) && (tmp & PIPECONF_COLOR_RANGE_SELECT))
+	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+	    (tmp & PIPECONF_COLOR_RANGE_SELECT))
 		pipe_config->limited_color_range = true;
 
 	if (INTEL_INFO(dev)->gen < 4)
@@ -8188,7 +8236,7 @@ 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)) {
+	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
 		/*
 		 * DPLL_DVO_2X_MODE must be enabled for both DPLLs
 		 * on 830. Filter it out here so that we don't
@@ -8221,7 +8269,12 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 	pipe_config->base.adjusted_mode.crtc_clock =
 		pipe_config->port_clock / pipe_config->pixel_multiplier;
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void ironlake_init_pch_refclk(struct drm_device *dev)
@@ -8540,6 +8593,67 @@ static void lpt_disable_clkout_dp(struct drm_device *dev)
 	mutex_unlock(&dev_priv->sb_lock);
 }
 
+#define BEND_IDX(steps) ((50 + (steps)) / 5)
+
+static const uint16_t sscdivintphase[] = {
+	[BEND_IDX( 50)] = 0x3B23,
+	[BEND_IDX( 45)] = 0x3B23,
+	[BEND_IDX( 40)] = 0x3C23,
+	[BEND_IDX( 35)] = 0x3C23,
+	[BEND_IDX( 30)] = 0x3D23,
+	[BEND_IDX( 25)] = 0x3D23,
+	[BEND_IDX( 20)] = 0x3E23,
+	[BEND_IDX( 15)] = 0x3E23,
+	[BEND_IDX( 10)] = 0x3F23,
+	[BEND_IDX(  5)] = 0x3F23,
+	[BEND_IDX(  0)] = 0x0025,
+	[BEND_IDX( -5)] = 0x0025,
+	[BEND_IDX(-10)] = 0x0125,
+	[BEND_IDX(-15)] = 0x0125,
+	[BEND_IDX(-20)] = 0x0225,
+	[BEND_IDX(-25)] = 0x0225,
+	[BEND_IDX(-30)] = 0x0325,
+	[BEND_IDX(-35)] = 0x0325,
+	[BEND_IDX(-40)] = 0x0425,
+	[BEND_IDX(-45)] = 0x0425,
+	[BEND_IDX(-50)] = 0x0525,
+};
+
+/*
+ * Bend CLKOUT_DP
+ * steps -50 to 50 inclusive, in steps of 5
+ * < 0 slow down the clock, > 0 speed up the clock, 0 == no bend (135MHz)
+ * change in clock period = -(steps / 10) * 5.787 ps
+ */
+static void lpt_bend_clkout_dp(struct drm_i915_private *dev_priv, int steps)
+{
+	uint32_t tmp;
+	int idx = BEND_IDX(steps);
+
+	if (WARN_ON(steps % 5 != 0))
+		return;
+
+	if (WARN_ON(idx >= ARRAY_SIZE(sscdivintphase)))
+		return;
+
+	mutex_lock(&dev_priv->sb_lock);
+
+	if (steps % 10 != 0)
+		tmp = 0xAAAAAAAB;
+	else
+		tmp = 0x00000000;
+	intel_sbi_write(dev_priv, SBI_SSCDITHPHASE, tmp, SBI_ICLK);
+
+	tmp = intel_sbi_read(dev_priv, SBI_SSCDIVINTPHASE, SBI_ICLK);
+	tmp &= 0xffff0000;
+	tmp |= sscdivintphase[idx];
+	intel_sbi_write(dev_priv, SBI_SSCDIVINTPHASE, tmp, SBI_ICLK);
+
+	mutex_unlock(&dev_priv->sb_lock);
+}
+
+#undef BEND_IDX
+
 static void lpt_init_pch_refclk(struct drm_device *dev)
 {
 	struct intel_encoder *encoder;
@@ -8555,10 +8669,12 @@ static void lpt_init_pch_refclk(struct drm_device *dev)
 		}
 	}
 
-	if (has_vga)
+	if (has_vga) {
+		lpt_bend_clkout_dp(to_i915(dev), 0);
 		lpt_enable_clkout_dp(dev, true, true);
-	else
+	} else {
 		lpt_disable_clkout_dp(dev);
+	}
 }
 
 /*
@@ -8921,7 +9037,7 @@ static int ironlake_crtc_compute_clock(struct intel_crtc *crtc,
 	memset(&crtc_state->dpll_hw_state, 0,
 	       sizeof(crtc_state->dpll_hw_state));
 
-	is_lvds = intel_pipe_has_type(crtc, INTEL_OUTPUT_LVDS);
+	is_lvds = intel_pipe_will_have_type(crtc_state, INTEL_OUTPUT_LVDS);
 
 	WARN(!(HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)),
 	     "Unexpected PCH type %d\n", INTEL_PCH_TYPE(dev));
@@ -9262,18 +9378,21 @@ static bool ironlake_get_pipe_config(struct intel_crtc *crtc,
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum intel_display_power_domain power_domain;
 	uint32_t tmp;
+	bool ret;
 
-	if (!intel_display_power_is_enabled(dev_priv,
-					    POWER_DOMAIN_PIPE(crtc->pipe)))
+	power_domain = POWER_DOMAIN_PIPE(crtc->pipe);
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
 	pipe_config->cpu_transcoder = (enum transcoder) crtc->pipe;
 	pipe_config->shared_dpll = DPLL_ID_PRIVATE;
 
+	ret = false;
 	tmp = I915_READ(PIPECONF(crtc->pipe));
 	if (!(tmp & PIPECONF_ENABLE))
-		return false;
+		goto out;
 
 	switch (tmp & PIPECONF_BPC_MASK) {
 	case PIPECONF_6BPC:
@@ -9336,7 +9455,12 @@ static bool ironlake_get_pipe_config(struct intel_crtc *crtc,
 
 	ironlake_get_pfit_config(crtc, pipe_config);
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void assert_can_disable_lcpll(struct drm_i915_private *dev_priv)
@@ -9350,8 +9474,8 @@ static void assert_can_disable_lcpll(struct drm_i915_private *dev_priv)
 
 	I915_STATE_WARN(I915_READ(HSW_PWR_WELL_DRIVER), "Power well on\n");
 	I915_STATE_WARN(I915_READ(SPLL_CTL) & SPLL_PLL_ENABLE, "SPLL enabled\n");
-	I915_STATE_WARN(I915_READ(WRPLL_CTL1) & WRPLL_PLL_ENABLE, "WRPLL1 enabled\n");
-	I915_STATE_WARN(I915_READ(WRPLL_CTL2) & WRPLL_PLL_ENABLE, "WRPLL2 enabled\n");
+	I915_STATE_WARN(I915_READ(WRPLL_CTL(0)) & WRPLL_PLL_ENABLE, "WRPLL1 enabled\n");
+	I915_STATE_WARN(I915_READ(WRPLL_CTL(1)) & WRPLL_PLL_ENABLE, "WRPLL2 enabled\n");
 	I915_STATE_WARN(I915_READ(PCH_PP_STATUS) & PP_ON, "Panel power on\n");
 	I915_STATE_WARN(I915_READ(BLC_PWM_CPU_CTL2) & BLM_PWM_ENABLE,
 	     "CPU PWM1 enabled\n");
@@ -9695,14 +9819,10 @@ static int broadwell_modeset_calc_cdclk(struct drm_atomic_state *state)
 	else
 		cdclk = 337500;
 
-	/*
-	 * FIXME move the cdclk caclulation to
-	 * compute_config() so we can fail gracegully.
-	 */
 	if (cdclk > dev_priv->max_cdclk_freq) {
-		DRM_ERROR("requested cdclk (%d kHz) exceeds max (%d kHz)\n",
-			  cdclk, dev_priv->max_cdclk_freq);
-		cdclk = dev_priv->max_cdclk_freq;
+		DRM_DEBUG_KMS("requested cdclk (%d kHz) exceeds max (%d kHz)\n",
+			      cdclk, dev_priv->max_cdclk_freq);
+		return -EINVAL;
 	}
 
 	to_intel_atomic_state(state)->cdclk = cdclk;
@@ -9797,6 +9917,7 @@ static void haswell_get_ddi_pll(struct drm_i915_private *dev_priv,
 		break;
 	case PORT_CLK_SEL_SPLL:
 		pipe_config->shared_dpll = DPLL_ID_SPLL;
+		break;
 	}
 }
 
@@ -9813,7 +9934,7 @@ 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))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		skylake_get_ddi_pll(dev_priv, port, pipe_config);
 	else if (IS_BROXTON(dev))
 		bxt_get_ddi_pll(dev_priv, port, pipe_config);
@@ -9849,12 +9970,17 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum intel_display_power_domain pfit_domain;
+	enum intel_display_power_domain power_domain;
+	unsigned long power_domain_mask;
 	uint32_t tmp;
+	bool ret;
 
-	if (!intel_display_power_is_enabled(dev_priv,
-					 POWER_DOMAIN_PIPE(crtc->pipe)))
+	power_domain = POWER_DOMAIN_PIPE(crtc->pipe);
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
+	power_domain_mask = BIT(power_domain);
+
+	ret = false;
 
 	pipe_config->cpu_transcoder = (enum transcoder) crtc->pipe;
 	pipe_config->shared_dpll = DPLL_ID_PRIVATE;
@@ -9881,13 +10007,14 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 			pipe_config->cpu_transcoder = TRANSCODER_EDP;
 	}
 
-	if (!intel_display_power_is_enabled(dev_priv,
-			POWER_DOMAIN_TRANSCODER(pipe_config->cpu_transcoder)))
-		return false;
+	power_domain = POWER_DOMAIN_TRANSCODER(pipe_config->cpu_transcoder);
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
+		goto out;
+	power_domain_mask |= BIT(power_domain);
 
 	tmp = I915_READ(PIPECONF(pipe_config->cpu_transcoder));
 	if (!(tmp & PIPECONF_ENABLE))
-		return false;
+		goto out;
 
 	haswell_get_ddi_port_state(crtc, pipe_config);
 
@@ -9897,14 +10024,14 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 		skl_init_scalers(dev, crtc, pipe_config);
 	}
 
-	pfit_domain = POWER_DOMAIN_PIPE_PANEL_FITTER(crtc->pipe);
-
 	if (INTEL_INFO(dev)->gen >= 9) {
 		pipe_config->scaler_state.scaler_id = -1;
 		pipe_config->scaler_state.scaler_users &= ~(1 << SKL_CRTC_INDEX);
 	}
 
-	if (intel_display_power_is_enabled(dev_priv, pfit_domain)) {
+	power_domain = POWER_DOMAIN_PIPE_PANEL_FITTER(crtc->pipe);
+	if (intel_display_power_get_if_enabled(dev_priv, power_domain)) {
+		power_domain_mask |= BIT(power_domain);
 		if (INTEL_INFO(dev)->gen >= 9)
 			skylake_get_pfit_config(crtc, pipe_config);
 		else
@@ -9922,7 +10049,13 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 		pipe_config->pixel_multiplier = 1;
 	}
 
-	return true;
+	ret = true;
+
+out:
+	for_each_power_domain(power_domain, power_domain_mask)
+		intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void i845_update_cursor(struct drm_crtc *crtc, u32 base, bool on)
@@ -10154,20 +10287,17 @@ __intel_framebuffer_create(struct drm_device *dev,
 	int ret;
 
 	intel_fb = kzalloc(sizeof(*intel_fb), GFP_KERNEL);
-	if (!intel_fb) {
-		drm_gem_object_unreference(&obj->base);
+	if (!intel_fb)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	ret = intel_framebuffer_init(dev, intel_fb, mode_cmd, obj);
 	if (ret)
 		goto err;
 
 	return &intel_fb->base;
+
 err:
-	drm_gem_object_unreference(&obj->base);
 	kfree(intel_fb);
-
 	return ERR_PTR(ret);
 }
 
@@ -10207,6 +10337,7 @@ intel_framebuffer_create_for_mode(struct drm_device *dev,
 				  struct drm_display_mode *mode,
 				  int depth, int bpp)
 {
+	struct drm_framebuffer *fb;
 	struct drm_i915_gem_object *obj;
 	struct drm_mode_fb_cmd2 mode_cmd = { 0 };
 
@@ -10221,7 +10352,11 @@ intel_framebuffer_create_for_mode(struct drm_device *dev,
 								bpp);
 	mode_cmd.pixel_format = drm_mode_legacy_fb_format(bpp, depth);
 
-	return intel_framebuffer_create(dev, &mode_cmd, obj);
+	fb = intel_framebuffer_create(dev, &mode_cmd, obj);
+	if (IS_ERR(fb))
+		drm_gem_object_unreference_unlocked(&obj->base);
+
+	return fb;
 }
 
 static struct drm_framebuffer *
@@ -11124,7 +11259,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 	 */
 	if (ring->id == RCS) {
 		intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
-		intel_ring_emit(ring, DERRMR);
+		intel_ring_emit_reg(ring, DERRMR);
 		intel_ring_emit(ring, ~(DERRMR_PIPEA_PRI_FLIP_DONE |
 					DERRMR_PIPEB_PRI_FLIP_DONE |
 					DERRMR_PIPEC_PRI_FLIP_DONE));
@@ -11134,7 +11269,7 @@ static int intel_gen7_queue_flip(struct drm_device *dev,
 		else
 			intel_ring_emit(ring, MI_STORE_REGISTER_MEM |
 					      MI_SRM_LRM_GLOBAL_GTT);
-		intel_ring_emit(ring, DERRMR);
+		intel_ring_emit_reg(ring, DERRMR);
 		intel_ring_emit(ring, ring->scratch.gtt_offset + 256);
 		if (IS_GEN8(dev)) {
 			intel_ring_emit(ring, 0);
@@ -11174,18 +11309,23 @@ static bool use_mmio_flip(struct intel_engine_cs *ring,
 		return true;
 	else if (i915.enable_execlists)
 		return true;
+	else if (obj->base.dma_buf &&
+		 !reservation_object_test_signaled_rcu(obj->base.dma_buf->resv,
+						       false))
+		return true;
 	else
 		return ring != i915_gem_request_get_ring(obj->last_write_req);
 }
 
 static void skl_do_mmio_flip(struct intel_crtc *intel_crtc,
+			     unsigned int rotation,
 			     struct intel_unpin_work *work)
 {
 	struct drm_device *dev = intel_crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_framebuffer *fb = intel_crtc->base.primary->fb;
 	const enum pipe pipe = intel_crtc->pipe;
-	u32 ctl, stride;
+	u32 ctl, stride, tile_height;
 
 	ctl = I915_READ(PLANE_CTL(pipe, 0));
 	ctl &= ~PLANE_CTL_TILED_MASK;
@@ -11209,9 +11349,16 @@ static void skl_do_mmio_flip(struct intel_crtc *intel_crtc,
 	 * The stride is either expressed as a multiple of 64 bytes chunks for
 	 * linear buffers or in number of tiles for tiled buffers.
 	 */
-	stride = fb->pitches[0] /
-		 intel_fb_stride_alignment(dev, fb->modifier[0],
-					   fb->pixel_format);
+	if (intel_rotation_90_or_270(rotation)) {
+		/* stride = Surface height in tiles */
+		tile_height = intel_tile_height(dev, fb->pixel_format,
+						fb->modifier[0], 0);
+		stride = DIV_ROUND_UP(fb->height, tile_height);
+	} else {
+		stride = fb->pitches[0] /
+				intel_fb_stride_alignment(dev, fb->modifier[0],
+							  fb->pixel_format);
+	}
 
 	/*
 	 * Both PLANE_CTL and PLANE_STRIDE are not updated on vblank but on
@@ -11232,10 +11379,9 @@ static void ilk_do_mmio_flip(struct intel_crtc *intel_crtc,
 	struct intel_framebuffer *intel_fb =
 		to_intel_framebuffer(intel_crtc->base.primary->fb);
 	struct drm_i915_gem_object *obj = intel_fb->obj;
+	i915_reg_t reg = DSPCNTR(intel_crtc->plane);
 	u32 dspcntr;
-	u32 reg;
 
-	reg = DSPCNTR(intel_crtc->plane);
 	dspcntr = I915_READ(reg);
 
 	if (obj->tiling_mode != I915_TILING_NONE)
@@ -11269,7 +11415,7 @@ static void intel_do_mmio_flip(struct intel_mmio_flip *mmio_flip)
 	intel_pipe_update_start(crtc);
 
 	if (INTEL_INFO(mmio_flip->i915)->gen >= 9)
-		skl_do_mmio_flip(crtc, work);
+		skl_do_mmio_flip(crtc, mmio_flip->rotation, work);
 	else
 		/* use_mmio_flip() retricts MMIO flips to ilk+ */
 		ilk_do_mmio_flip(crtc, work);
@@ -11281,6 +11427,9 @@ static void intel_mmio_flip_work_func(struct work_struct *work)
 {
 	struct intel_mmio_flip *mmio_flip =
 		container_of(work, struct intel_mmio_flip, work);
+	struct intel_framebuffer *intel_fb =
+		to_intel_framebuffer(mmio_flip->crtc->base.primary->fb);
+	struct drm_i915_gem_object *obj = intel_fb->obj;
 
 	if (mmio_flip->req) {
 		WARN_ON(__i915_wait_request(mmio_flip->req,
@@ -11290,16 +11439,19 @@ static void intel_mmio_flip_work_func(struct work_struct *work)
 		i915_gem_request_unreference__unlocked(mmio_flip->req);
 	}
 
+	/* For framebuffer backed by dmabuf, wait for fence */
+	if (obj->base.dma_buf)
+		WARN_ON(reservation_object_wait_timeout_rcu(obj->base.dma_buf->resv,
+							    false, false,
+							    MAX_SCHEDULE_TIMEOUT) < 0);
+
 	intel_do_mmio_flip(mmio_flip);
 	kfree(mmio_flip);
 }
 
 static int intel_queue_mmio_flip(struct drm_device *dev,
 				 struct drm_crtc *crtc,
-				 struct drm_framebuffer *fb,
-				 struct drm_i915_gem_object *obj,
-				 struct intel_engine_cs *ring,
-				 uint32_t flags)
+				 struct drm_i915_gem_object *obj)
 {
 	struct intel_mmio_flip *mmio_flip;
 
@@ -11310,6 +11462,7 @@ static int intel_queue_mmio_flip(struct drm_device *dev,
 	mmio_flip->i915 = to_i915(dev);
 	mmio_flip->req = i915_gem_request_reference(obj->last_write_req);
 	mmio_flip->crtc = to_intel_crtc(crtc);
+	mmio_flip->rotation = crtc->primary->state->rotation;
 
 	INIT_WORK(&mmio_flip->work, intel_mmio_flip_work_func);
 	schedule_work(&mmio_flip->work);
@@ -11493,7 +11646,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	if (INTEL_INFO(dev)->gen >= 5 || IS_G4X(dev))
 		work->flip_count = I915_READ(PIPE_FLIPCOUNT_G4X(pipe)) + 1;
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		ring = &dev_priv->ring[BCS];
 		if (obj->tiling_mode != intel_fb_obj(work->old_fb)->tiling_mode)
 			/* vlv: DISPLAY_FLIP fails to change tiling */
@@ -11515,9 +11668,14 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	 * synchronisation, so all we want here is to pin the framebuffer
 	 * into the display plane and skip any waits.
 	 */
+	if (!mmio_flip) {
+		ret = i915_gem_object_sync(obj, ring, &request);
+		if (ret)
+			goto cleanup_pending;
+	}
+
 	ret = intel_pin_and_fence_fb_obj(crtc->primary, fb,
-					 crtc->primary->state,
-					 mmio_flip ? i915_gem_request_get_ring(obj->last_write_req) : ring, &request);
+					 crtc->primary->state);
 	if (ret)
 		goto cleanup_pending;
 
@@ -11526,8 +11684,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	work->gtt_offset += intel_crtc->dspaddr_offset;
 
 	if (mmio_flip) {
-		ret = intel_queue_mmio_flip(dev, crtc, fb, obj, ring,
-					    page_flip_flags);
+		ret = intel_queue_mmio_flip(dev, crtc, obj);
 		if (ret)
 			goto cleanup_unpin;
 
@@ -11558,7 +11715,7 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 			  to_intel_plane(primary)->frontbuffer_bit);
 	mutex_unlock(&dev->struct_mutex);
 
-	intel_fbc_disable_crtc(intel_crtc);
+	intel_fbc_deactivate(intel_crtc);
 	intel_frontbuffer_flip_prepare(dev,
 				       to_intel_plane(primary)->frontbuffer_bit);
 
@@ -11641,21 +11798,41 @@ retry:
 static bool intel_wm_need_update(struct drm_plane *plane,
 				 struct drm_plane_state *state)
 {
-	/* Update watermarks on tiling changes. */
-	if (!plane->state->fb || !state->fb ||
-	    plane->state->fb->modifier[0] != state->fb->modifier[0] ||
-	    plane->state->rotation != state->rotation)
+	struct intel_plane_state *new = to_intel_plane_state(state);
+	struct intel_plane_state *cur = to_intel_plane_state(plane->state);
+
+	/* Update watermarks on tiling or size changes. */
+	if (new->visible != cur->visible)
 		return true;
 
-	if (plane->state->crtc_w != state->crtc_w)
+	if (!cur->base.fb || !new->base.fb)
+		return false;
+
+	if (cur->base.fb->modifier[0] != new->base.fb->modifier[0] ||
+	    cur->base.rotation != new->base.rotation ||
+	    drm_rect_width(&new->src) != drm_rect_width(&cur->src) ||
+	    drm_rect_height(&new->src) != drm_rect_height(&cur->src) ||
+	    drm_rect_width(&new->dst) != drm_rect_width(&cur->dst) ||
+	    drm_rect_height(&new->dst) != drm_rect_height(&cur->dst))
 		return true;
 
 	return false;
 }
 
+static bool needs_scaling(struct intel_plane_state *state)
+{
+	int src_w = drm_rect_width(&state->src) >> 16;
+	int src_h = drm_rect_height(&state->src) >> 16;
+	int dst_w = drm_rect_width(&state->dst);
+	int dst_h = drm_rect_height(&state->dst);
+
+	return (src_w != dst_w || src_h != dst_h);
+}
+
 int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 				    struct drm_plane_state *plane_state)
 {
+	struct intel_crtc_state *pipe_config = to_intel_crtc_state(crtc_state);
 	struct drm_crtc *crtc = crtc_state->crtc;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct drm_plane *plane = plane_state->plane;
@@ -11668,7 +11845,6 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 	bool mode_changed = needs_modeset(crtc_state);
 	bool was_crtc_enabled = crtc->state->active;
 	bool is_crtc_enabled = crtc_state->active;
-
 	bool turn_off, turn_on, visible, was_visible;
 	struct drm_framebuffer *fb = plane_state->fb;
 
@@ -11681,14 +11857,6 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 			return ret;
 	}
 
-	/*
-	 * Disabling a plane is always okay; we just need to update
-	 * fb tracking in a special way since cleanup_fb() won't
-	 * get called by the plane helpers.
-	 */
-	if (old_plane_state->base.fb && !fb)
-		intel_crtc->atomic.disabled_planes |= 1 << i;
-
 	was_visible = old_plane_state->visible;
 	visible = to_intel_plane_state(plane_state)->visible;
 
@@ -11711,25 +11879,17 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 			 plane->base.id, was_visible, visible,
 			 turn_off, turn_on, mode_changed);
 
-	if (turn_on) {
-		intel_crtc->atomic.update_wm_pre = true;
-		/* must disable cxsr around plane enable/disable */
-		if (plane->type != DRM_PLANE_TYPE_CURSOR) {
-			intel_crtc->atomic.disable_cxsr = true;
-			/* to potentially re-enable cxsr */
-			intel_crtc->atomic.wait_vblank = true;
-			intel_crtc->atomic.update_wm_post = true;
-		}
-	} else if (turn_off) {
-		intel_crtc->atomic.update_wm_post = true;
+	if (turn_on || turn_off) {
+		pipe_config->wm_changed = true;
+
 		/* must disable cxsr around plane enable/disable */
 		if (plane->type != DRM_PLANE_TYPE_CURSOR) {
 			if (is_crtc_enabled)
 				intel_crtc->atomic.wait_vblank = true;
-			intel_crtc->atomic.disable_cxsr = true;
+			pipe_config->disable_cxsr = true;
 		}
 	} else if (intel_wm_need_update(plane, plane_state)) {
-		intel_crtc->atomic.update_wm_pre = true;
+		pipe_config->wm_changed = true;
 	}
 
 	if (visible || was_visible)
@@ -11738,7 +11898,6 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 
 	switch (plane->type) {
 	case DRM_PLANE_TYPE_PRIMARY:
-		intel_crtc->atomic.wait_for_flips = true;
 		intel_crtc->atomic.pre_disable_primary = turn_off;
 		intel_crtc->atomic.post_enable_primary = turn_on;
 
@@ -11786,11 +11945,23 @@ int intel_plane_atomic_calc_changes(struct drm_crtc_state *crtc_state,
 	case DRM_PLANE_TYPE_CURSOR:
 		break;
 	case DRM_PLANE_TYPE_OVERLAY:
-		if (turn_off && !mode_changed) {
+		/*
+		 * WaCxSRDisabledForSpriteScaling:ivb
+		 *
+		 * cstate->update_wm was already set above, so this flag will
+		 * take effect when we commit and program watermarks.
+		 */
+		if (IS_IVYBRIDGE(dev) &&
+		    needs_scaling(to_intel_plane_state(plane_state)) &&
+		    !needs_scaling(old_plane_state)) {
+			to_intel_crtc_state(crtc_state)->disable_lp_wm = true;
+		} else if (turn_off && !mode_changed) {
 			intel_crtc->atomic.wait_vblank = true;
 			intel_crtc->atomic.update_sprite_watermarks |=
 				1 << i;
 		}
+
+		break;
 	}
 	return 0;
 }
@@ -11863,7 +12034,7 @@ static int intel_crtc_atomic_check(struct drm_crtc *crtc,
 	}
 
 	if (mode_changed && !crtc_state->active)
-		intel_crtc->atomic.update_wm_post = true;
+		pipe_config->wm_changed = true;
 
 	if (mode_changed && crtc_state->enable &&
 	    dev_priv->display.crtc_compute_clock &&
@@ -11875,6 +12046,12 @@ static int intel_crtc_atomic_check(struct drm_crtc *crtc,
 	}
 
 	ret = 0;
+	if (dev_priv->display.compute_pipe_wm) {
+		ret = dev_priv->display.compute_pipe_wm(intel_crtc, state);
+		if (ret)
+			return ret;
+	}
+
 	if (INTEL_INFO(dev)->gen >= 9) {
 		if (mode_changed)
 			ret = skl_update_scaler_crtc(pipe_config);
@@ -11930,11 +12107,21 @@ connected_sink_compute_bpp(struct intel_connector *connector,
 		pipe_config->pipe_bpp = connector->base.display_info.bpc*3;
 	}
 
-	/* Clamp bpp to 8 on screens without EDID 1.4 */
-	if (connector->base.display_info.bpc == 0 && bpp > 24) {
-		DRM_DEBUG_KMS("clamping display bpp (was %d) to default limit of 24\n",
-			      bpp);
-		pipe_config->pipe_bpp = 24;
+	/* Clamp bpp to default limit on screens without EDID 1.4 */
+	if (connector->base.display_info.bpc == 0) {
+		int type = connector->base.connector_type;
+		int clamp_bpp = 24;
+
+		/* Fall back to 18 bpp when DP sink capability is unknown. */
+		if (type == DRM_MODE_CONNECTOR_DisplayPort ||
+		    type == DRM_MODE_CONNECTOR_eDP)
+			clamp_bpp = 18;
+
+		if (bpp > clamp_bpp) {
+			DRM_DEBUG_KMS("clamping display bpp (was %d) to default limit of %d\n",
+				      bpp, clamp_bpp);
+			pipe_config->pipe_bpp = clamp_bpp;
+		}
 	}
 }
 
@@ -11948,7 +12135,7 @@ compute_baseline_pipe_bpp(struct intel_crtc *crtc,
 	struct drm_connector_state *connector_state;
 	int bpp, i;
 
-	if ((IS_G4X(dev) || IS_VALLEYVIEW(dev)))
+	if ((IS_G4X(dev) || IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)))
 		bpp = 10*3;
 	else if (INTEL_INFO(dev)->gen >= 5)
 		bpp = 12*3;
@@ -12064,7 +12251,7 @@ 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)) {
+	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		DRM_DEBUG_KMS("ddi_pll_sel: %u; dpll_hw_state: "
 			      "ctrl1: 0x%x, cfgcr1: 0x%x, cfgcr2: 0x%x\n",
 			      pipe_config->ddi_pll_sel,
@@ -12322,6 +12509,18 @@ intel_modeset_update_crtc_state(struct drm_atomic_state *state)
 			crtc->hwmode = crtc->state->adjusted_mode;
 		else
 			crtc->hwmode.crtc_clock = 0;
+
+		/*
+		 * Update legacy state to satisfy fbc code. This can
+		 * be removed when fbc uses the atomic state.
+		 */
+		if (drm_atomic_get_existing_plane_state(state, crtc->primary)) {
+			struct drm_plane_state *plane_state = crtc->primary->state;
+
+			crtc->primary->fb = plane_state->fb;
+			crtc->x = plane_state->src_x >> 16;
+			crtc->y = plane_state->src_y >> 16;
+		}
 	}
 }
 
@@ -12347,7 +12546,7 @@ static bool intel_fuzzy_clock_check(int clock1, int clock2)
 	list_for_each_entry((intel_crtc), \
 			    &(dev)->mode_config.crtc_list, \
 			    base.head) \
-		if (mask & (1 <<(intel_crtc)->pipe))
+		for_each_if (mask & (1 <<(intel_crtc)->pipe))
 
 static bool
 intel_compare_m_n(unsigned int m, unsigned int n,
@@ -12531,6 +12730,8 @@ intel_pipe_config_compare(struct drm_device *dev,
 	} else
 		PIPE_CONF_CHECK_M_N_ALT(dp_m_n, dp_m2_n2);
 
+	PIPE_CONF_CHECK_I(has_dsi_encoder);
+
 	PIPE_CONF_CHECK_I(base.adjusted_mode.crtc_hdisplay);
 	PIPE_CONF_CHECK_I(base.adjusted_mode.crtc_htotal);
 	PIPE_CONF_CHECK_I(base.adjusted_mode.crtc_hblank_start);
@@ -12548,7 +12749,7 @@ 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_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		PIPE_CONF_CHECK_I(limited_color_range);
 	PIPE_CONF_CHECK_I(has_infoframe);
 
@@ -13085,6 +13286,45 @@ static int intel_modeset_checks(struct drm_atomic_state *state)
 	return 0;
 }
 
+/*
+ * Handle calculation of various watermark data at the end of the atomic check
+ * phase.  The code here should be run after the per-crtc and per-plane 'check'
+ * handlers to ensure that all derived state has been updated.
+ */
+static void calc_watermark_data(struct drm_atomic_state *state)
+{
+	struct drm_device *dev = state->dev;
+	struct intel_atomic_state *intel_state = to_intel_atomic_state(state);
+	struct drm_crtc *crtc;
+	struct drm_crtc_state *cstate;
+	struct drm_plane *plane;
+	struct drm_plane_state *pstate;
+
+	/*
+	 * Calculate watermark configuration details now that derived
+	 * plane/crtc state is all properly updated.
+	 */
+	drm_for_each_crtc(crtc, dev) {
+		cstate = drm_atomic_get_existing_crtc_state(state, crtc) ?:
+			crtc->state;
+
+		if (cstate->active)
+			intel_state->wm_config.num_pipes_active++;
+	}
+	drm_for_each_legacy_plane(plane, dev) {
+		pstate = drm_atomic_get_existing_plane_state(state, plane) ?:
+			plane->state;
+
+		if (!to_intel_plane_state(pstate)->visible)
+			continue;
+
+		intel_state->wm_config.sprites_enabled = true;
+		if (pstate->crtc_w != pstate->src_w >> 16 ||
+		    pstate->crtc_h != pstate->src_h >> 16)
+			intel_state->wm_config.sprites_scaled = true;
+	}
+}
+
 /**
  * intel_atomic_check - validate state object
  * @dev: drm device
@@ -13093,6 +13333,7 @@ static int intel_modeset_checks(struct drm_atomic_state *state)
 static int intel_atomic_check(struct drm_device *dev,
 			      struct drm_atomic_state *state)
 {
+	struct intel_atomic_state *intel_state = to_intel_atomic_state(state);
 	struct drm_crtc *crtc;
 	struct drm_crtc_state *crtc_state;
 	int ret, i;
@@ -13160,10 +13401,81 @@ static int intel_atomic_check(struct drm_device *dev,
 		if (ret)
 			return ret;
 	} else
-		to_intel_atomic_state(state)->cdclk =
-			to_i915(state->dev)->cdclk_freq;
+		intel_state->cdclk = to_i915(state->dev)->cdclk_freq;
+
+	ret = drm_atomic_helper_check_planes(state->dev, state);
+	if (ret)
+		return ret;
+
+	calc_watermark_data(state);
+
+	return 0;
+}
+
+static int intel_atomic_prepare_commit(struct drm_device *dev,
+				       struct drm_atomic_state *state,
+				       bool async)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_plane_state *plane_state;
+	struct drm_crtc_state *crtc_state;
+	struct drm_plane *plane;
+	struct drm_crtc *crtc;
+	int i, ret;
+
+	if (async) {
+		DRM_DEBUG_KMS("i915 does not yet support async commit\n");
+		return -EINVAL;
+	}
+
+	for_each_crtc_in_state(state, crtc, crtc_state, i) {
+		ret = intel_crtc_wait_for_pending_flips(crtc);
+		if (ret)
+			return ret;
+
+		if (atomic_read(&to_intel_crtc(crtc)->unpin_work_count) >= 2)
+			flush_workqueue(dev_priv->wq);
+	}
+
+	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	if (ret)
+		return ret;
+
+	ret = drm_atomic_helper_prepare_planes(dev, state);
+	if (!ret && !async && !i915_reset_in_progress(&dev_priv->gpu_error)) {
+		u32 reset_counter;
+
+		reset_counter = atomic_read(&dev_priv->gpu_error.reset_counter);
+		mutex_unlock(&dev->struct_mutex);
+
+		for_each_plane_in_state(state, plane, plane_state, i) {
+			struct intel_plane_state *intel_plane_state =
+				to_intel_plane_state(plane_state);
+
+			if (!intel_plane_state->wait_req)
+				continue;
+
+			ret = __i915_wait_request(intel_plane_state->wait_req,
+						  reset_counter, true,
+						  NULL, NULL);
+
+			/* Swallow -EIO errors to allow updates during hw lockup. */
+			if (ret == -EIO)
+				ret = 0;
+
+			if (ret)
+				break;
+		}
 
-	return drm_atomic_helper_check_planes(state->dev, state);
+		if (!ret)
+			return 0;
+
+		mutex_lock(&dev->struct_mutex);
+		drm_atomic_helper_cleanup_planes(dev, state);
+	}
+
+	mutex_unlock(&dev->struct_mutex);
+	return ret;
 }
 
 /**
@@ -13187,22 +13499,20 @@ static int intel_atomic_commit(struct drm_device *dev,
 			       bool async)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc;
 	struct drm_crtc_state *crtc_state;
+	struct drm_crtc *crtc;
 	int ret = 0;
 	int i;
 	bool any_ms = false;
 
-	if (async) {
-		DRM_DEBUG_KMS("i915 does not yet support async commit\n");
-		return -EINVAL;
-	}
-
-	ret = drm_atomic_helper_prepare_planes(dev, state);
-	if (ret)
+	ret = intel_atomic_prepare_commit(dev, state, async);
+	if (ret) {
+		DRM_DEBUG_ATOMIC("Preparing state failed with %i\n", ret);
 		return ret;
+	}
 
 	drm_atomic_helper_swap_state(dev, state);
+	dev_priv->wm.config = to_intel_atomic_state(state)->wm_config;
 
 	for_each_crtc_in_state(state, crtc, crtc_state, i) {
 		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
@@ -13218,6 +13528,16 @@ static int intel_atomic_commit(struct drm_device *dev,
 			dev_priv->display.crtc_disable(crtc);
 			intel_crtc->active = false;
 			intel_disable_shared_dpll(intel_crtc);
+
+			/*
+			 * Underruns don't always raise
+			 * interrupts, so check manually.
+			 */
+			intel_check_cpu_fifo_underruns(dev_priv);
+			intel_check_pch_fifo_underruns(dev_priv);
+
+			if (!crtc->state->active)
+				intel_update_watermarks(crtc);
 		}
 	}
 
@@ -13240,6 +13560,9 @@ static int intel_atomic_commit(struct drm_device *dev,
 			to_intel_crtc_state(crtc->state)->update_pipe;
 		unsigned long put_domains = 0;
 
+		if (modeset)
+			intel_display_power_get(dev_priv, POWER_DOMAIN_MODESET);
+
 		if (modeset && crtc->state->active) {
 			update_scanline_offset(to_intel_crtc(crtc));
 			dev_priv->display.crtc_enable(crtc);
@@ -13255,18 +13578,26 @@ static int intel_atomic_commit(struct drm_device *dev,
 		if (!modeset)
 			intel_pre_plane_update(intel_crtc);
 
-		drm_atomic_helper_commit_planes_on_crtc(crtc_state);
+		if (crtc->state->active &&
+		    (crtc->state->planes_changed || update_pipe))
+			drm_atomic_helper_commit_planes_on_crtc(crtc_state);
 
 		if (put_domains)
 			modeset_put_power_domains(dev_priv, put_domains);
 
 		intel_post_plane_update(intel_crtc);
+
+		if (modeset)
+			intel_display_power_put(dev_priv, POWER_DOMAIN_MODESET);
 	}
 
 	/* FIXME: add subpixel order */
 
 	drm_atomic_helper_wait_for_vblanks(dev, state);
+
+	mutex_lock(&dev->struct_mutex);
 	drm_atomic_helper_cleanup_planes(dev, state);
+	mutex_unlock(&dev->struct_mutex);
 
 	if (any_ms)
 		intel_modeset_check_state(dev, state);
@@ -13331,7 +13662,7 @@ static bool ibx_pch_dpll_get_hw_state(struct drm_i915_private *dev_priv,
 {
 	uint32_t val;
 
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PLLS))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_PLLS))
 		return false;
 
 	val = I915_READ(PCH_DPLL(pll->id));
@@ -13339,6 +13670,8 @@ static bool ibx_pch_dpll_get_hw_state(struct drm_i915_private *dev_priv,
 	hw_state->fp0 = I915_READ(PCH_FP0(pll->id));
 	hw_state->fp1 = I915_READ(PCH_FP1(pll->id));
 
+	intel_display_power_put(dev_priv, POWER_DOMAIN_PLLS);
+
 	return val & DPLL_VCO_ENABLE;
 }
 
@@ -13435,6 +13768,8 @@ static void intel_shared_dpll_init(struct drm_device *dev)
  * bits.  Some older platforms need special physical address handling for
  * cursor planes.
  *
+ * Must be called with struct_mutex held.
+ *
  * Returns 0 on success, negative error code on failure.
  */
 int
@@ -13445,28 +13780,71 @@ intel_prepare_plane_fb(struct drm_plane *plane,
 	struct drm_framebuffer *fb = new_state->fb;
 	struct intel_plane *intel_plane = to_intel_plane(plane);
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
-	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->fb);
+	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->state->fb);
 	int ret = 0;
 
-	if (!obj)
+	if (!obj && !old_obj)
 		return 0;
 
-	mutex_lock(&dev->struct_mutex);
+	if (old_obj) {
+		struct drm_crtc_state *crtc_state =
+			drm_atomic_get_existing_crtc_state(new_state->state, plane->state->crtc);
+
+		/* Big Hammer, we also need to ensure that any pending
+		 * MI_WAIT_FOR_EVENT inside a user batch buffer on the
+		 * current scanout is retired before unpinning the old
+		 * framebuffer. Note that we rely on userspace rendering
+		 * into the buffer attached to the pipe they are waiting
+		 * on. If not, userspace generates a GPU hang with IPEHR
+		 * point to the MI_WAIT_FOR_EVENT.
+		 *
+		 * This should only fail upon a hung GPU, in which case we
+		 * can safely continue.
+		 */
+		if (needs_modeset(crtc_state))
+			ret = i915_gem_object_wait_rendering(old_obj, true);
+
+		/* Swallow -EIO errors to allow updates during hw lockup. */
+		if (ret && ret != -EIO)
+			return ret;
+	}
+
+	/* For framebuffer backed by dmabuf, wait for fence */
+	if (obj && obj->base.dma_buf) {
+		long lret;
 
-	if (plane->type == DRM_PLANE_TYPE_CURSOR &&
+		lret = reservation_object_wait_timeout_rcu(obj->base.dma_buf->resv,
+							   false, true,
+							   MAX_SCHEDULE_TIMEOUT);
+		if (lret == -ERESTARTSYS)
+			return lret;
+
+		WARN(lret < 0, "waiting returns %li\n", lret);
+	}
+
+	if (!obj) {
+		ret = 0;
+	} else if (plane->type == DRM_PLANE_TYPE_CURSOR &&
 	    INTEL_INFO(dev)->cursor_needs_physical) {
 		int align = IS_I830(dev) ? 16 * 1024 : 256;
 		ret = i915_gem_object_attach_phys(obj, align);
 		if (ret)
 			DRM_DEBUG_KMS("failed to attach phys object\n");
 	} else {
-		ret = intel_pin_and_fence_fb_obj(plane, fb, new_state, NULL, NULL);
+		ret = intel_pin_and_fence_fb_obj(plane, fb, new_state);
 	}
 
-	if (ret == 0)
-		i915_gem_track_fb(old_obj, obj, intel_plane->frontbuffer_bit);
+	if (ret == 0) {
+		if (obj) {
+			struct intel_plane_state *plane_state =
+				to_intel_plane_state(new_state);
 
-	mutex_unlock(&dev->struct_mutex);
+			i915_gem_request_assign(&plane_state->wait_req,
+						obj->last_write_req);
+		}
+
+		i915_gem_track_fb(old_obj, obj, intel_plane->frontbuffer_bit);
+	}
 
 	return ret;
 }
@@ -13477,23 +13855,35 @@ intel_prepare_plane_fb(struct drm_plane *plane,
  * @fb: old framebuffer that was on plane
  *
  * Cleans up a framebuffer that has just been removed from a plane.
+ *
+ * Must be called with struct_mutex held.
  */
 void
 intel_cleanup_plane_fb(struct drm_plane *plane,
 		       const struct drm_plane_state *old_state)
 {
 	struct drm_device *dev = plane->dev;
-	struct drm_i915_gem_object *obj = intel_fb_obj(old_state->fb);
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct intel_plane_state *old_intel_state;
+	struct drm_i915_gem_object *old_obj = intel_fb_obj(old_state->fb);
+	struct drm_i915_gem_object *obj = intel_fb_obj(plane->state->fb);
 
-	if (!obj)
+	old_intel_state = to_intel_plane_state(old_state);
+
+	if (!obj && !old_obj)
 		return;
 
-	if (plane->type != DRM_PLANE_TYPE_CURSOR ||
-	    !INTEL_INFO(dev)->cursor_needs_physical) {
-		mutex_lock(&dev->struct_mutex);
+	if (old_obj && (plane->type != DRM_PLANE_TYPE_CURSOR ||
+	    !INTEL_INFO(dev)->cursor_needs_physical))
 		intel_unpin_fb_obj(old_state->fb, old_state);
-		mutex_unlock(&dev->struct_mutex);
-	}
+
+	/* prepare_fb aborted? */
+	if ((old_obj && (old_obj->frontbuffer_bits & intel_plane->frontbuffer_bit)) ||
+	    (obj && !(obj->frontbuffer_bits & intel_plane->frontbuffer_bit)))
+		i915_gem_track_fb(old_obj, obj, intel_plane->frontbuffer_bit);
+
+	i915_gem_request_assign(&old_intel_state->wait_req, NULL);
+
 }
 
 int
@@ -13512,7 +13902,7 @@ skl_max_scale(struct intel_crtc *intel_crtc, struct intel_crtc_state *crtc_state
 	crtc_clock = crtc_state->base.adjusted_mode.crtc_clock;
 	cdclk = to_intel_atomic_state(crtc_state->base.state)->cdclk;
 
-	if (!crtc_clock || !cdclk)
+	if (WARN_ON_ONCE(!crtc_clock || cdclk < crtc_clock))
 		return DRM_PLANE_HELPER_NO_SCALING;
 
 	/*
@@ -13537,11 +13927,12 @@ intel_check_primary_plane(struct drm_plane *plane,
 	int max_scale = DRM_PLANE_HELPER_NO_SCALING;
 	bool can_position = false;
 
-	/* use scaler when colorkey is not required */
-	if (INTEL_INFO(plane->dev)->gen >= 9 &&
-	    state->ckey.flags == I915_SET_COLORKEY_NONE) {
-		min_scale = 1;
-		max_scale = skl_max_scale(to_intel_crtc(crtc), crtc_state);
+	if (INTEL_INFO(plane->dev)->gen >= 9) {
+		/* use scaler when colorkey is not required */
+		if (state->ckey.flags == I915_SET_COLORKEY_NONE) {
+			min_scale = 1;
+			max_scale = skl_max_scale(to_intel_crtc(crtc), crtc_state);
+		}
 		can_position = true;
 	}
 
@@ -13560,18 +13951,8 @@ intel_commit_primary_plane(struct drm_plane *plane,
 	struct drm_framebuffer *fb = state->base.fb;
 	struct drm_device *dev = plane->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc;
-	struct drm_rect *src = &state->src;
 
 	crtc = crtc ? crtc : plane->crtc;
-	intel_crtc = to_intel_crtc(crtc);
-
-	plane->fb = fb;
-	crtc->x = src->x1 >> 16;
-	crtc->y = src->y1 >> 16;
-
-	if (!crtc->state->active)
-		return;
 
 	dev_priv->display.update_primary_plane(crtc, fb,
 					       state->src.x1 >> 16,
@@ -13597,12 +13978,8 @@ static void intel_begin_crtc_commit(struct drm_crtc *crtc,
 		to_intel_crtc_state(old_crtc_state);
 	bool modeset = needs_modeset(crtc->state);
 
-	if (intel_crtc->atomic.update_wm_pre)
-		intel_update_watermarks(crtc);
-
 	/* Perform vblank evasion around commit operation */
-	if (crtc->state->active)
-		intel_pipe_update_start(intel_crtc);
+	intel_pipe_update_start(intel_crtc);
 
 	if (modeset)
 		return;
@@ -13618,8 +13995,7 @@ static void intel_finish_crtc_commit(struct drm_crtc *crtc,
 {
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
-	if (crtc->state->active)
-		intel_pipe_update_end(intel_crtc);
+	intel_pipe_update_end(intel_crtc);
 }
 
 /**
@@ -13696,7 +14072,7 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 	drm_universal_plane_init(dev, &primary->base, 0,
 				 &intel_plane_funcs,
 				 intel_primary_formats, num_formats,
-				 DRM_PLANE_TYPE_PRIMARY);
+				 DRM_PLANE_TYPE_PRIMARY, NULL);
 
 	if (INTEL_INFO(dev)->gen >= 4)
 		intel_create_rotation_property(dev, primary);
@@ -13848,7 +14224,7 @@ static struct drm_plane *intel_cursor_plane_create(struct drm_device *dev,
 				 &intel_plane_funcs,
 				 intel_cursor_formats,
 				 ARRAY_SIZE(intel_cursor_formats),
-				 DRM_PLANE_TYPE_CURSOR);
+				 DRM_PLANE_TYPE_CURSOR, NULL);
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		if (!dev->mode_config.rotation_property)
@@ -13925,7 +14301,7 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 		goto fail;
 
 	ret = drm_crtc_init_with_planes(dev, &intel_crtc->base, primary,
-					cursor, &intel_crtc_funcs);
+					cursor, &intel_crtc_funcs, NULL);
 	if (ret)
 		goto fail;
 
@@ -14051,7 +14427,14 @@ static bool intel_crt_present(struct drm_device *dev)
 	if (IS_CHERRYVIEW(dev))
 		return false;
 
-	if (IS_VALLEYVIEW(dev) && !dev_priv->vbt.int_crt_support)
+	if (HAS_PCH_LPT_H(dev) && 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)
+		return false;
+
+	if (!dev_priv->vbt.int_crt_support)
 		return false;
 
 	return true;
@@ -14087,7 +14470,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))
+		if (found || IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 			intel_ddi_init(dev, PORT_A);
 
 		/* DDI B, C and D detection is indicated by the SFUSE_STRAP
@@ -14103,7 +14486,7 @@ 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) &&
+		if ((IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) &&
 		    (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))
@@ -14118,7 +14501,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(PCH_HDMIB) & SDVO_DETECTED) {
 			/* PCH SDVOB multiplex with HDMIB */
-			found = intel_sdvo_init(dev, PCH_SDVOB, true);
+			found = intel_sdvo_init(dev, PCH_SDVOB, PORT_B);
 			if (!found)
 				intel_hdmi_init(dev, PCH_HDMIB, PORT_B);
 			if (!found && (I915_READ(PCH_DP_B) & DP_DETECTED))
@@ -14136,7 +14519,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)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		/*
 		 * The DP_DETECTED bit is the latched state of the DDC
 		 * SDA pin at boot. However since eDP doesn't require DDC
@@ -14174,7 +14557,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(GEN3_SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOB\n");
-			found = intel_sdvo_init(dev, GEN3_SDVOB, true);
+			found = intel_sdvo_init(dev, GEN3_SDVOB, PORT_B);
 			if (!found && IS_G4X(dev)) {
 				DRM_DEBUG_KMS("probing HDMI on SDVOB\n");
 				intel_hdmi_init(dev, GEN4_HDMIB, PORT_B);
@@ -14188,7 +14571,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 		if (I915_READ(GEN3_SDVOB) & SDVO_DETECTED) {
 			DRM_DEBUG_KMS("probing SDVOC\n");
-			found = intel_sdvo_init(dev, GEN3_SDVOC, false);
+			found = intel_sdvo_init(dev, GEN3_SDVOC, PORT_C);
 		}
 
 		if (!found && (I915_READ(GEN3_SDVOC) & SDVO_DETECTED)) {
@@ -14285,7 +14668,7 @@ u32 intel_fb_pitch_limit(struct drm_device *dev, uint64_t fb_modifier,
 		 *  pixels and 32K bytes."
 		 */
 		 return min(8192*drm_format_plane_cpp(pixel_format, 0), 32768);
-	} else if (gen >= 5 && !IS_VALLEYVIEW(dev)) {
+	} else if (gen >= 5 && !IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
 		return 32*1024;
 	} else if (gen >= 4) {
 		if (fb_modifier == I915_FORMAT_MOD_X_TILED)
@@ -14389,7 +14772,8 @@ static int intel_framebuffer_init(struct drm_device *dev,
 		}
 		break;
 	case DRM_FORMAT_ABGR8888:
-		if (!IS_VALLEYVIEW(dev) && INTEL_INFO(dev)->gen < 9) {
+		if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
+		    INTEL_INFO(dev)->gen < 9) {
 			DRM_DEBUG("unsupported pixel format: %s\n",
 				  drm_get_format_name(mode_cmd->pixel_format));
 			return -EINVAL;
@@ -14405,7 +14789,7 @@ static int intel_framebuffer_init(struct drm_device *dev,
 		}
 		break;
 	case DRM_FORMAT_ABGR2101010:
-		if (!IS_VALLEYVIEW(dev)) {
+		if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)) {
 			DRM_DEBUG("unsupported pixel format: %s\n",
 				  drm_get_format_name(mode_cmd->pixel_format));
 			return -EINVAL;
@@ -14454,8 +14838,9 @@ static int intel_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 intel_user_framebuffer_create(struct drm_device *dev,
 			      struct drm_file *filp,
-			      struct drm_mode_fb_cmd2 *user_mode_cmd)
+			      const struct drm_mode_fb_cmd2 *user_mode_cmd)
 {
+	struct drm_framebuffer *fb;
 	struct drm_i915_gem_object *obj;
 	struct drm_mode_fb_cmd2 mode_cmd = *user_mode_cmd;
 
@@ -14464,7 +14849,11 @@ intel_user_framebuffer_create(struct drm_device *dev,
 	if (&obj->base == NULL)
 		return ERR_PTR(-ENOENT);
 
-	return intel_framebuffer_create(dev, &mode_cmd, obj);
+	fb = intel_framebuffer_create(dev, &mode_cmd, obj);
+	if (IS_ERR(fb))
+		drm_gem_object_unreference_unlocked(&obj->base);
+
+	return fb;
 }
 
 #ifndef CONFIG_DRM_FBDEV_EMULATION
@@ -14528,7 +14917,7 @@ static void intel_init_display(struct drm_device *dev)
 		dev_priv->display.crtc_disable = ironlake_crtc_disable;
 		dev_priv->display.update_primary_plane =
 			ironlake_update_primary_plane;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		dev_priv->display.get_pipe_config = i9xx_get_pipe_config;
 		dev_priv->display.get_initial_plane_config =
 			i9xx_get_initial_plane_config;
@@ -14549,7 +14938,7 @@ static void intel_init_display(struct drm_device *dev)
 	}
 
 	/* Returns the core display clock speed */
-	if (IS_SKYLAKE(dev))
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 		dev_priv->display.get_display_clock_speed =
 			skylake_get_display_clock_speed;
 	else if (IS_BROXTON(dev))
@@ -14561,7 +14950,7 @@ static void intel_init_display(struct drm_device *dev)
 	else if (IS_HASWELL(dev))
 		dev_priv->display.get_display_clock_speed =
 			haswell_get_display_clock_speed;
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		dev_priv->display.get_display_clock_speed =
 			valleyview_get_display_clock_speed;
 	else if (IS_GEN5(dev))
@@ -14589,9 +14978,6 @@ static void intel_init_display(struct drm_device *dev)
 	else if (IS_I945GM(dev) || IS_845G(dev))
 		dev_priv->display.get_display_clock_speed =
 			i9xx_misc_get_display_clock_speed;
-	else if (IS_PINEVIEW(dev))
-		dev_priv->display.get_display_clock_speed =
-			pnv_get_display_clock_speed;
 	else if (IS_I915GM(dev))
 		dev_priv->display.get_display_clock_speed =
 			i915gm_get_display_clock_speed;
@@ -14622,7 +15008,7 @@ static void intel_init_display(struct drm_device *dev)
 			dev_priv->display.modeset_calc_cdclk =
 				broadwell_modeset_calc_cdclk;
 		}
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		dev_priv->display.modeset_commit_cdclk =
 			valleyview_modeset_commit_cdclk;
 		dev_priv->display.modeset_calc_cdclk =
@@ -14838,7 +15224,7 @@ static void i915_disable_vga(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u8 sr1;
-	u32 vga_reg = i915_vgacntrl_reg(dev);
+	i915_reg_t vga_reg = i915_vgacntrl_reg(dev);
 
 	/* WaEnableVGAAccessThroughIOPort:ctg,elk,ilk,snb,ivb,vlv,hsw */
 	vga_get_uninterruptible(dev->pdev, VGA_RSRC_LEGACY_IO);
@@ -14954,9 +15340,6 @@ void intel_modeset_init(struct drm_device *dev)
 	i915_disable_vga(dev);
 	intel_setup_outputs(dev);
 
-	/* Just in case the BIOS is doing something questionable. */
-	intel_fbc_disable(dev_priv);
-
 	drm_modeset_lock_all(dev);
 	intel_modeset_setup_hw_state(dev);
 	drm_modeset_unlock_all(dev);
@@ -15043,10 +15426,9 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 reg;
+	i915_reg_t reg = PIPECONF(crtc->config->cpu_transcoder);
 
 	/* Clear any frame start delays used for debugging left by the BIOS */
-	reg = PIPECONF(crtc->config->cpu_transcoder);
 	I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 
 	/* restore vblank interrupts to correct state */
@@ -15113,6 +15495,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 		WARN_ON(drm_atomic_set_mode_for_crtc(crtc->base.state, NULL) < 0);
 		crtc->base.state->active = crtc->active;
 		crtc->base.enabled = crtc->active;
+		crtc->base.state->connector_mask = 0;
 
 		/* Because we only establish the connector -> encoder ->
 		 * crtc links if something is active, this means the
@@ -15200,7 +15583,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 = dev->dev_private;
-	u32 vga_reg = i915_vgacntrl_reg(dev);
+	i915_reg_t vga_reg = i915_vgacntrl_reg(dev);
 
 	if (!(I915_READ(vga_reg) & VGA_DISP_DISABLE)) {
 		DRM_DEBUG_KMS("Something enabled VGA plane, disabling it\n");
@@ -15219,10 +15602,12 @@ void i915_redisable_vga(struct drm_device *dev)
 	 * level, just check if the power well is enabled instead of trying to
 	 * follow the "don't touch the power well if we don't need it" policy
 	 * the rest of the driver uses. */
-	if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_VGA))
+	if (!intel_display_power_get_if_enabled(dev_priv, POWER_DOMAIN_VGA))
 		return;
 
 	i915_redisable_vga_power_on(dev);
+
+	intel_display_power_put(dev_priv, POWER_DOMAIN_VGA);
 }
 
 static bool primary_get_hw_state(struct intel_plane *plane)
@@ -15239,7 +15624,7 @@ static void readout_plane_state(struct intel_crtc *crtc)
 	struct intel_plane_state *plane_state =
 		to_intel_plane_state(primary->state);
 
-	plane_state->visible =
+	plane_state->visible = crtc->active &&
 		primary_get_hw_state(to_intel_plane(primary));
 
 	if (plane_state->visible)
@@ -15315,7 +15700,21 @@ static void intel_modeset_readout_hw_state(struct drm_device *dev)
 	for_each_intel_connector(dev, connector) {
 		if (connector->get_hw_state(connector)) {
 			connector->base.dpms = DRM_MODE_DPMS_ON;
-			connector->base.encoder = &connector->encoder->base;
+
+			encoder = connector->encoder;
+			connector->base.encoder = &encoder->base;
+
+			if (encoder->base.crtc &&
+			    encoder->base.crtc->state->active) {
+				/*
+				 * This has to be done during hardware readout
+				 * because anything calling .crtc_disable may
+				 * rely on the connector_mask being accurate.
+				 */
+				encoder->base.crtc->state->connector_mask |=
+					1 << drm_connector_index(&connector->base);
+			}
+
 		} else {
 			connector->base.dpms = DRM_MODE_DPMS_OFF;
 			connector->base.encoder = NULL;
@@ -15400,7 +15799,7 @@ intel_modeset_setup_hw_state(struct drm_device *dev)
 		pll->on = false;
 	}
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		vlv_wm_get_hw_state(dev);
 	else if (IS_GEN9(dev))
 		skl_wm_get_hw_state(dev);
@@ -15496,8 +15895,7 @@ void intel_modeset_gem_init(struct drm_device *dev)
 		mutex_lock(&dev->struct_mutex);
 		ret = intel_pin_and_fence_fb_obj(c->primary,
 						 c->primary->fb,
-						 c->primary->state,
-						 NULL, NULL);
+						 c->primary->state);
 		mutex_unlock(&dev->struct_mutex);
 		if (ret) {
 			DRM_ERROR("failed to pin boot fb on pipe %d\n",
@@ -15524,7 +15922,7 @@ void intel_connector_unregister(struct intel_connector *intel_connector)
 void intel_modeset_cleanup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_connector *connector;
+	struct intel_connector *connector;
 
 	intel_disable_gt_powersave(dev);
 
@@ -15551,12 +15949,8 @@ void intel_modeset_cleanup(struct drm_device *dev)
 	flush_scheduled_work();
 
 	/* destroy the backlight and sysfs files before encoders/connectors */
-	list_for_each_entry(connector, &dev->mode_config.connector_list, head) {
-		struct intel_connector *intel_connector;
-
-		intel_connector = to_intel_connector(connector);
-		intel_connector->unregister(intel_connector);
-	}
+	for_each_intel_connector(dev, connector)
+		connector->unregister(connector);
 
 	drm_mode_config_cleanup(dev);
 
@@ -15565,6 +15959,8 @@ void intel_modeset_cleanup(struct drm_device *dev)
 	mutex_lock(&dev->struct_mutex);
 	intel_cleanup_gt_powersave(dev);
 	mutex_unlock(&dev->struct_mutex);
+
+	intel_teardown_gmbus(dev);
 }
 
 /*
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index 78b8ec84d576..cdc2c15873dc 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -389,8 +389,7 @@ vlv_power_sequencer_pipe(struct intel_dp *intel_dp)
 	 * We don't have power sequencer currently.
 	 * Pick one that's not used by other ports.
 	 */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		struct intel_dp *tmp;
 
 		if (encoder->type != INTEL_OUTPUT_EDP)
@@ -517,7 +516,7 @@ void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = dev_priv->dev;
 	struct intel_encoder *encoder;
 
-	if (WARN_ON(!IS_VALLEYVIEW(dev)))
+	if (WARN_ON(!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev)))
 		return;
 
 	/*
@@ -530,7 +529,7 @@ void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv)
 	 * should use them always.
 	 */
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		struct intel_dp *intel_dp;
 
 		if (encoder->type != INTEL_OUTPUT_EDP)
@@ -541,7 +540,8 @@ void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv)
 	}
 }
 
-static u32 _pp_ctrl_reg(struct intel_dp *intel_dp)
+static i915_reg_t
+_pp_ctrl_reg(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
@@ -553,7 +553,8 @@ static u32 _pp_ctrl_reg(struct intel_dp *intel_dp)
 		return VLV_PIPE_PP_CONTROL(vlv_power_sequencer_pipe(intel_dp));
 }
 
-static u32 _pp_stat_reg(struct intel_dp *intel_dp)
+static i915_reg_t
+_pp_stat_reg(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
@@ -580,9 +581,9 @@ static int edp_notify_handler(struct notifier_block *this, unsigned long code,
 
 	pps_lock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		enum pipe pipe = vlv_power_sequencer_pipe(intel_dp);
-		u32 pp_ctrl_reg, pp_div_reg;
+		i915_reg_t pp_ctrl_reg, pp_div_reg;
 		u32 pp_div;
 
 		pp_ctrl_reg = VLV_PIPE_PP_CONTROL(pipe);
@@ -608,7 +609,7 @@ static bool edp_have_panel_power(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	if (IS_VALLEYVIEW(dev) &&
+	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
 	    intel_dp->pps_pipe == INVALID_PIPE)
 		return false;
 
@@ -622,7 +623,7 @@ static bool edp_have_panel_vdd(struct intel_dp *intel_dp)
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	if (IS_VALLEYVIEW(dev) &&
+	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
 	    intel_dp->pps_pipe == INVALID_PIPE)
 		return false;
 
@@ -652,7 +653,7 @@ intel_dp_aux_wait_done(struct intel_dp *intel_dp, bool has_aux_irq)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t ch_ctl = intel_dp->aux_ch_ctl_reg;
+	i915_reg_t ch_ctl = intel_dp->aux_ch_ctl_reg;
 	uint32_t status;
 	bool done;
 
@@ -679,7 +680,7 @@ static uint32_t i9xx_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
 	 * The clock divider is based off the hrawclk, and would like to run at
 	 * 2MHz.  So, take the hrawclk value and divide by 2 and use that
 	 */
-	return index ? 0 : intel_hrawclk(dev) / 2;
+	return index ? 0 : DIV_ROUND_CLOSEST(intel_hrawclk(dev), 2);
 }
 
 static uint32_t ilk_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
@@ -692,10 +693,10 @@ static uint32_t ilk_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
 		return 0;
 
 	if (intel_dig_port->port == PORT_A) {
-		return DIV_ROUND_UP(dev_priv->cdclk_freq, 2000);
+		return DIV_ROUND_CLOSEST(dev_priv->cdclk_freq, 2000);
 
 	} else {
-		return DIV_ROUND_UP(intel_pch_rawclk(dev), 2);
+		return DIV_ROUND_CLOSEST(intel_pch_rawclk(dev), 2);
 	}
 }
 
@@ -709,7 +710,7 @@ static uint32_t hsw_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
 		if (index)
 			return 0;
 		return DIV_ROUND_CLOSEST(dev_priv->cdclk_freq, 2000);
-	} else if (dev_priv->pch_id == INTEL_PCH_LPT_DEVICE_ID_TYPE) {
+	} else if (HAS_PCH_LPT_H(dev_priv)) {
 		/* Workaround for non-ULT HSW */
 		switch (index) {
 		case 0: return 63;
@@ -717,7 +718,7 @@ static uint32_t hsw_get_aux_clock_divider(struct intel_dp *intel_dp, int index)
 		default: return 0;
 		}
 	} else  {
-		return index ? 0 : DIV_ROUND_UP(intel_pch_rawclk(dev), 2);
+		return index ? 0 : DIV_ROUND_CLOSEST(intel_pch_rawclk(dev), 2);
 	}
 }
 
@@ -750,7 +751,7 @@ static uint32_t i9xx_get_aux_send_ctl(struct intel_dp *intel_dp,
 	else
 		precharge = 5;
 
-	if (IS_BROADWELL(dev) && intel_dp->aux_ch_ctl_reg == DPA_AUX_CH_CTL)
+	if (IS_BROADWELL(dev) && intel_dig_port->port == PORT_A)
 		timeout = DP_AUX_CH_CTL_TIME_OUT_600us;
 	else
 		timeout = DP_AUX_CH_CTL_TIME_OUT_400us;
@@ -789,8 +790,7 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t ch_ctl = intel_dp->aux_ch_ctl_reg;
-	uint32_t ch_data = ch_ctl + 4;
+	i915_reg_t ch_ctl = intel_dp->aux_ch_ctl_reg;
 	uint32_t aux_clock_divider;
 	int i, ret, recv_bytes;
 	uint32_t status;
@@ -854,7 +854,7 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 		for (try = 0; try < 5; try++) {
 			/* Load the send data into the aux channel data registers */
 			for (i = 0; i < send_bytes; i += 4)
-				I915_WRITE(ch_data + i,
+				I915_WRITE(intel_dp->aux_ch_data_reg[i >> 2],
 					   intel_dp_pack_aux(send + i,
 							     send_bytes - i));
 
@@ -914,11 +914,32 @@ done:
 	/* Unload any bytes sent back from the other side */
 	recv_bytes = ((status & DP_AUX_CH_CTL_MESSAGE_SIZE_MASK) >>
 		      DP_AUX_CH_CTL_MESSAGE_SIZE_SHIFT);
+
+	/*
+	 * By BSpec: "Message sizes of 0 or >20 are not allowed."
+	 * We have no idea of what happened so we return -EBUSY so
+	 * drm layer takes care for the necessary retries.
+	 */
+	if (recv_bytes == 0 || recv_bytes > 20) {
+		DRM_DEBUG_KMS("Forbidden recv_bytes = %d on aux transaction\n",
+			      recv_bytes);
+		/*
+		 * FIXME: This patch was created on top of a series that
+		 * organize the retries at drm level. There EBUSY should
+		 * also take care for 1ms wait before retrying.
+		 * That aux retries re-org is still needed and after that is
+		 * merged we remove this sleep from here.
+		 */
+		usleep_range(1000, 1500);
+		ret = -EBUSY;
+		goto out;
+	}
+
 	if (recv_bytes > recv_size)
 		recv_bytes = recv_size;
 
 	for (i = 0; i < recv_bytes; i += 4)
-		intel_dp_unpack_aux(I915_READ(ch_data + i),
+		intel_dp_unpack_aux(I915_READ(intel_dp->aux_ch_data_reg[i >> 2]),
 				    recv + i, recv_bytes - i);
 
 	ret = recv_bytes;
@@ -1005,96 +1026,206 @@ intel_dp_aux_transfer(struct drm_dp_aux *aux, struct drm_dp_aux_msg *msg)
 	return ret;
 }
 
-static void
-intel_dp_aux_init(struct intel_dp *intel_dp, struct intel_connector *connector)
+static i915_reg_t g4x_aux_ctl_reg(struct drm_i915_private *dev_priv,
+				       enum port port)
 {
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	enum port port = intel_dig_port->port;
-	struct ddi_vbt_port_info *info = &dev_priv->vbt.ddi_port_info[port];
-	const char *name = NULL;
-	uint32_t porte_aux_ctl_reg = DPA_AUX_CH_CTL;
-	int ret;
+	switch (port) {
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		return DP_AUX_CH_CTL(port);
+	default:
+		MISSING_CASE(port);
+		return DP_AUX_CH_CTL(PORT_B);
+	}
+}
 
-	/* On SKL we don't have Aux for port E so we rely on VBT to set
-	 * a proper alternate aux channel.
-	 */
-	if (IS_SKYLAKE(dev) && port == PORT_E) {
-		switch (info->alternate_aux_channel) {
-		case DP_AUX_B:
-			porte_aux_ctl_reg = DPB_AUX_CH_CTL;
-			break;
-		case DP_AUX_C:
-			porte_aux_ctl_reg = DPC_AUX_CH_CTL;
-			break;
-		case DP_AUX_D:
-			porte_aux_ctl_reg = DPD_AUX_CH_CTL;
-			break;
-		case DP_AUX_A:
-		default:
-			porte_aux_ctl_reg = DPA_AUX_CH_CTL;
-		}
+static i915_reg_t g4x_aux_data_reg(struct drm_i915_private *dev_priv,
+					enum port port, int index)
+{
+	switch (port) {
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		return DP_AUX_CH_DATA(port, index);
+	default:
+		MISSING_CASE(port);
+		return DP_AUX_CH_DATA(PORT_B, index);
 	}
+}
 
+static i915_reg_t ilk_aux_ctl_reg(struct drm_i915_private *dev_priv,
+				       enum port port)
+{
 	switch (port) {
 	case PORT_A:
-		intel_dp->aux_ch_ctl_reg = DPA_AUX_CH_CTL;
-		name = "DPDDC-A";
-		break;
+		return DP_AUX_CH_CTL(port);
 	case PORT_B:
-		intel_dp->aux_ch_ctl_reg = PCH_DPB_AUX_CH_CTL;
-		name = "DPDDC-B";
-		break;
 	case PORT_C:
-		intel_dp->aux_ch_ctl_reg = PCH_DPC_AUX_CH_CTL;
-		name = "DPDDC-C";
-		break;
 	case PORT_D:
-		intel_dp->aux_ch_ctl_reg = PCH_DPD_AUX_CH_CTL;
-		name = "DPDDC-D";
-		break;
-	case PORT_E:
-		intel_dp->aux_ch_ctl_reg = porte_aux_ctl_reg;
-		name = "DPDDC-E";
-		break;
+		return PCH_DP_AUX_CH_CTL(port);
 	default:
-		BUG();
+		MISSING_CASE(port);
+		return DP_AUX_CH_CTL(PORT_A);
 	}
+}
 
-	/*
-	 * The AUX_CTL register is usually DP_CTL + 0x10.
-	 *
-	 * On Haswell and Broadwell though:
-	 *   - Both port A DDI_BUF_CTL and DDI_AUX_CTL are on the CPU
-	 *   - Port B/C/D AUX channels are on the PCH, DDI_BUF_CTL on the CPU
-	 *
-	 * Skylake moves AUX_CTL back next to DDI_BUF_CTL, on the CPU.
-	 */
-	if (!IS_HASWELL(dev) && !IS_BROADWELL(dev) && port != PORT_E)
-		intel_dp->aux_ch_ctl_reg = intel_dp->output_reg + 0x10;
+static i915_reg_t ilk_aux_data_reg(struct drm_i915_private *dev_priv,
+					enum port port, int index)
+{
+	switch (port) {
+	case PORT_A:
+		return DP_AUX_CH_DATA(port, index);
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		return PCH_DP_AUX_CH_DATA(port, index);
+	default:
+		MISSING_CASE(port);
+		return DP_AUX_CH_DATA(PORT_A, index);
+	}
+}
+
+/*
+ * 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)
+{
+	if (port == PORT_E)
+		port = skl_porte_aux_port(dev_priv);
+
+	switch (port) {
+	case PORT_A:
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		return DP_AUX_CH_CTL(port);
+	default:
+		MISSING_CASE(port);
+		return DP_AUX_CH_CTL(PORT_A);
+	}
+}
+
+static i915_reg_t skl_aux_data_reg(struct drm_i915_private *dev_priv,
+					enum port port, int index)
+{
+	if (port == PORT_E)
+		port = skl_porte_aux_port(dev_priv);
+
+	switch (port) {
+	case PORT_A:
+	case PORT_B:
+	case PORT_C:
+	case PORT_D:
+		return DP_AUX_CH_DATA(port, index);
+	default:
+		MISSING_CASE(port);
+		return DP_AUX_CH_DATA(PORT_A, index);
+	}
+}
+
+static i915_reg_t intel_aux_ctl_reg(struct drm_i915_private *dev_priv,
+					 enum port port)
+{
+	if (INTEL_INFO(dev_priv)->gen >= 9)
+		return skl_aux_ctl_reg(dev_priv, port);
+	else if (HAS_PCH_SPLIT(dev_priv))
+		return ilk_aux_ctl_reg(dev_priv, port);
+	else
+		return g4x_aux_ctl_reg(dev_priv, port);
+}
+
+static i915_reg_t intel_aux_data_reg(struct drm_i915_private *dev_priv,
+					  enum port port, int index)
+{
+	if (INTEL_INFO(dev_priv)->gen >= 9)
+		return skl_aux_data_reg(dev_priv, port, index);
+	else if (HAS_PCH_SPLIT(dev_priv))
+		return ilk_aux_data_reg(dev_priv, port, index);
+	else
+		return g4x_aux_data_reg(dev_priv, port, index);
+}
+
+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;
+	int i;
+
+	intel_dp->aux_ch_ctl_reg = intel_aux_ctl_reg(dev_priv, port);
+	for (i = 0; i < ARRAY_SIZE(intel_dp->aux_ch_data_reg); i++)
+		intel_dp->aux_ch_data_reg[i] = intel_aux_data_reg(dev_priv, port, i);
+}
+
+static void
+intel_dp_aux_fini(struct intel_dp *intel_dp)
+{
+	drm_dp_aux_unregister(&intel_dp->aux);
+	kfree(intel_dp->aux.name);
+}
+
+static int
+intel_dp_aux_init(struct intel_dp *intel_dp, struct intel_connector *connector)
+{
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	enum port port = intel_dig_port->port;
+	int ret;
+
+	intel_aux_reg_init(intel_dp);
+
+	intel_dp->aux.name = kasprintf(GFP_KERNEL, "DPDDC-%c", port_name(port));
+	if (!intel_dp->aux.name)
+		return -ENOMEM;
 
-	intel_dp->aux.name = name;
 	intel_dp->aux.dev = dev->dev;
 	intel_dp->aux.transfer = intel_dp_aux_transfer;
 
-	DRM_DEBUG_KMS("registering %s bus for %s\n", name,
+	DRM_DEBUG_KMS("registering %s bus for %s\n",
+		      intel_dp->aux.name,
 		      connector->base.kdev->kobj.name);
 
 	ret = drm_dp_aux_register(&intel_dp->aux);
 	if (ret < 0) {
 		DRM_ERROR("drm_dp_aux_register() for %s failed (%d)\n",
-			  name, ret);
-		return;
+			  intel_dp->aux.name, ret);
+		kfree(intel_dp->aux.name);
+		return ret;
 	}
 
 	ret = sysfs_create_link(&connector->base.kdev->kobj,
 				&intel_dp->aux.ddc.dev.kobj,
 				intel_dp->aux.ddc.dev.kobj.name);
 	if (ret < 0) {
-		DRM_ERROR("sysfs_create_link() for %s failed (%d)\n", name, ret);
-		drm_dp_aux_unregister(&intel_dp->aux);
+		DRM_ERROR("sysfs_create_link() for %s failed (%d)\n",
+			  intel_dp->aux.name, ret);
+		intel_dp_aux_fini(intel_dp);
+		return ret;
 	}
+
+	return 0;
 }
 
 static void
@@ -1186,10 +1317,13 @@ intel_dp_sink_rates(struct intel_dp *intel_dp, const int **sink_rates)
 	return (intel_dp_max_link_bw(intel_dp) >> 3) + 1;
 }
 
-static bool intel_dp_source_supports_hbr2(struct drm_device *dev)
+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_SKYLAKE(dev) && INTEL_REVID(dev) <= SKL_REVID_B0)
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0))
 		return false;
 
 	if ((IS_HASWELL(dev) && !IS_HSW_ULX(dev)) || IS_BROADWELL(dev) ||
@@ -1200,14 +1334,16 @@ static bool intel_dp_source_supports_hbr2(struct drm_device *dev)
 }
 
 static int
-intel_dp_source_rates(struct drm_device *dev, const int **source_rates)
+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;
 	int size;
 
 	if (IS_BROXTON(dev)) {
 		*source_rates = bxt_rates;
 		size = ARRAY_SIZE(bxt_rates);
-	} else if (IS_SKYLAKE(dev)) {
+	} else if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		*source_rates = skl_rates;
 		size = ARRAY_SIZE(skl_rates);
 	} else {
@@ -1216,7 +1352,7 @@ intel_dp_source_rates(struct drm_device *dev, const int **source_rates)
 	}
 
 	/* This depends on the fact that 5.4 is last value in the array */
-	if (!intel_dp_source_supports_hbr2(dev))
+	if (!intel_dp_source_supports_hbr2(intel_dp))
 		size--;
 
 	return size;
@@ -1281,12 +1417,11 @@ static int intersect_rates(const int *source_rates, int source_len,
 static int intel_dp_common_rates(struct intel_dp *intel_dp,
 				 int *common_rates)
 {
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	const int *source_rates, *sink_rates;
 	int source_len, sink_len;
 
 	sink_len = intel_dp_sink_rates(intel_dp, &sink_rates);
-	source_len = intel_dp_source_rates(dev, &source_rates);
+	source_len = intel_dp_source_rates(intel_dp, &source_rates);
 
 	return intersect_rates(source_rates, source_len,
 			       sink_rates, sink_len,
@@ -1311,7 +1446,6 @@ static void snprintf_int_array(char *str, size_t len,
 
 static void intel_dp_print_rates(struct intel_dp *intel_dp)
 {
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	const int *source_rates, *sink_rates;
 	int source_len, sink_len, common_len;
 	int common_rates[DP_MAX_SUPPORTED_RATES];
@@ -1320,7 +1454,7 @@ static void intel_dp_print_rates(struct intel_dp *intel_dp)
 	if ((drm_debug & DRM_UT_KMS) == 0)
 		return;
 
-	source_len = intel_dp_source_rates(dev, &source_rates);
+	source_len = intel_dp_source_rates(intel_dp, &source_rates);
 	snprintf_int_array(str, sizeof(str), source_rates, source_len);
 	DRM_DEBUG_KMS("source rates: %s\n", str);
 
@@ -1362,8 +1496,8 @@ int intel_dp_rate_select(struct intel_dp *intel_dp, int rate)
 	return rate_to_index(rate, intel_dp->sink_rates);
 }
 
-static void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
-				  uint8_t *link_bw, uint8_t *rate_select)
+void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
+			   uint8_t *link_bw, uint8_t *rate_select)
 {
 	if (intel_dp->num_sink_rates) {
 		*link_bw = 0;
@@ -1423,7 +1557,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 				return ret;
 		}
 
-		if (!HAS_PCH_SPLIT(dev))
+		if (HAS_GMCH_DISPLAY(dev))
 			intel_gmch_panel_fitting(intel_crtc, pipe_config,
 						 intel_connector->panel.fitting_mode);
 		else
@@ -1527,7 +1661,7 @@ found:
 				&pipe_config->dp_m2_n2);
 	}
 
-	if (IS_SKYLAKE(dev) && is_edp(intel_dp))
+	if ((IS_SKYLAKE(dev)  || IS_KABYLAKE(dev)) && is_edp(intel_dp))
 		skl_edp_set_pll_config(pipe_config);
 	else if (IS_BROXTON(dev))
 		/* handled in ddi */;
@@ -1539,37 +1673,6 @@ found:
 	return true;
 }
 
-static void ironlake_set_pll_cpu_edp(struct intel_dp *intel_dp)
-{
-	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
-	struct intel_crtc *crtc = to_intel_crtc(dig_port->base.base.crtc);
-	struct drm_device *dev = crtc->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpa_ctl;
-
-	DRM_DEBUG_KMS("eDP PLL enable for clock %d\n",
-		      crtc->config->port_clock);
-	dpa_ctl = I915_READ(DP_A);
-	dpa_ctl &= ~DP_PLL_FREQ_MASK;
-
-	if (crtc->config->port_clock == 162000) {
-		/* For a long time we've carried around a ILK-DevA w/a for the
-		 * 160MHz clock. If we're really unlucky, it's still required.
-		 */
-		DRM_DEBUG_KMS("160MHz cpu eDP clock, might need ilk devA w/a\n");
-		dpa_ctl |= DP_PLL_FREQ_160MHZ;
-		intel_dp->DP |= DP_PLL_FREQ_160MHZ;
-	} else {
-		dpa_ctl |= DP_PLL_FREQ_270MHZ;
-		intel_dp->DP |= DP_PLL_FREQ_270MHZ;
-	}
-
-	I915_WRITE(DP_A, dpa_ctl);
-
-	POSTING_READ(DP_A);
-	udelay(500);
-}
-
 void intel_dp_set_link_params(struct intel_dp *intel_dp,
 			      const struct intel_crtc_state *pipe_config)
 {
@@ -1614,9 +1717,6 @@ static void intel_dp_prepare(struct intel_encoder *encoder)
 	intel_dp->DP |= DP_VOLTAGE_0_4 | DP_PRE_EMPHASIS_0;
 	intel_dp->DP |= DP_PORT_WIDTH(crtc->config->lane_count);
 
-	if (crtc->config->has_audio)
-		intel_dp->DP |= DP_AUDIO_OUTPUT_ENABLE;
-
 	/* Split out the IBX/CPU vs CPT settings */
 
 	if (IS_GEN7(dev) && port == PORT_A) {
@@ -1643,7 +1743,7 @@ static void intel_dp_prepare(struct intel_encoder *encoder)
 		I915_WRITE(TRANS_DP_CTL(crtc->pipe), trans_dp);
 	} else {
 		if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev) &&
-		    crtc->config->limited_color_range)
+		    !IS_CHERRYVIEW(dev) && crtc->config->limited_color_range)
 			intel_dp->DP |= DP_COLOR_RANGE_16_235;
 
 		if (adjusted_mode->flags & DRM_MODE_FLAG_PHSYNC)
@@ -1677,7 +1777,7 @@ static void wait_panel_status(struct intel_dp *intel_dp,
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 pp_stat_reg, pp_ctrl_reg;
+	i915_reg_t pp_stat_reg, pp_ctrl_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -1767,7 +1867,7 @@ static bool edp_panel_vdd_on(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum intel_display_power_domain power_domain;
 	u32 pp;
-	u32 pp_stat_reg, pp_ctrl_reg;
+	i915_reg_t pp_stat_reg, pp_ctrl_reg;
 	bool need_to_disable = !intel_dp->want_panel_vdd;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
@@ -1843,7 +1943,7 @@ static void edp_panel_vdd_off_sync(struct intel_dp *intel_dp)
 	struct intel_encoder *intel_encoder = &intel_dig_port->base;
 	enum intel_display_power_domain power_domain;
 	u32 pp;
-	u32 pp_stat_reg, pp_ctrl_reg;
+	i915_reg_t pp_stat_reg, pp_ctrl_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -1930,7 +2030,7 @@ static void edp_panel_on(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
-	u32 pp_ctrl_reg;
+	i915_reg_t pp_ctrl_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -1992,7 +2092,7 @@ static void edp_panel_off(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum intel_display_power_domain power_domain;
 	u32 pp;
-	u32 pp_ctrl_reg;
+	i915_reg_t pp_ctrl_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -2043,7 +2143,7 @@ static void _intel_edp_backlight_on(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
-	u32 pp_ctrl_reg;
+	i915_reg_t pp_ctrl_reg;
 
 	/*
 	 * If we enable the backlight right away following a panel power
@@ -2084,7 +2184,7 @@ static void _intel_edp_backlight_off(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp;
-	u32 pp_ctrl_reg;
+	i915_reg_t pp_ctrl_reg;
 
 	if (!is_edp(intel_dp))
 		return;
@@ -2143,27 +2243,61 @@ static void intel_edp_backlight_power(struct intel_connector *connector,
 		_intel_edp_backlight_off(intel_dp);
 }
 
+static const char *state_string(bool enabled)
+{
+	return enabled ? "on" : "off";
+}
+
+static void assert_dp_port(struct intel_dp *intel_dp, bool state)
+{
+	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_i915_private *dev_priv = to_i915(dig_port->base.base.dev);
+	bool cur_state = I915_READ(intel_dp->output_reg) & DP_PORT_EN;
+
+	I915_STATE_WARN(cur_state != state,
+			"DP port %c state assertion failure (expected %s, current %s)\n",
+			port_name(dig_port->port),
+			state_string(state), state_string(cur_state));
+}
+#define assert_dp_port_disabled(d) assert_dp_port((d), false)
+
+static void assert_edp_pll(struct drm_i915_private *dev_priv, bool state)
+{
+	bool cur_state = I915_READ(DP_A) & DP_PLL_ENABLE;
+
+	I915_STATE_WARN(cur_state != state,
+			"eDP PLL state assertion failure (expected %s, current %s)\n",
+			state_string(state), state_string(cur_state));
+}
+#define assert_edp_pll_enabled(d) assert_edp_pll((d), true)
+#define assert_edp_pll_disabled(d) assert_edp_pll((d), false)
+
 static void ironlake_edp_pll_on(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_crtc *crtc = intel_dig_port->base.base.crtc;
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpa_ctl;
+	struct intel_crtc *crtc = to_intel_crtc(intel_dig_port->base.base.crtc);
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 
-	assert_pipe_disabled(dev_priv,
-			     to_intel_crtc(crtc)->pipe);
+	assert_pipe_disabled(dev_priv, crtc->pipe);
+	assert_dp_port_disabled(intel_dp);
+	assert_edp_pll_disabled(dev_priv);
+
+	DRM_DEBUG_KMS("enabling eDP PLL for clock %d\n",
+		      crtc->config->port_clock);
+
+	intel_dp->DP &= ~DP_PLL_FREQ_MASK;
+
+	if (crtc->config->port_clock == 162000)
+		intel_dp->DP |= DP_PLL_FREQ_162MHZ;
+	else
+		intel_dp->DP |= DP_PLL_FREQ_270MHZ;
+
+	I915_WRITE(DP_A, intel_dp->DP);
+	POSTING_READ(DP_A);
+	udelay(500);
 
-	DRM_DEBUG_KMS("\n");
-	dpa_ctl = I915_READ(DP_A);
-	WARN(dpa_ctl & DP_PLL_ENABLE, "dp pll on, should be off\n");
-	WARN(dpa_ctl & DP_PORT_EN, "dp port still on, should be off\n");
-
-	/* We don't adjust intel_dp->DP while tearing down the link, to
-	 * facilitate link retraining (e.g. after hotplug). Hence clear all
-	 * enable bits here to ensure that we don't enable too much. */
-	intel_dp->DP &= ~(DP_PORT_EN | DP_AUDIO_OUTPUT_ENABLE);
 	intel_dp->DP |= DP_PLL_ENABLE;
+
 	I915_WRITE(DP_A, intel_dp->DP);
 	POSTING_READ(DP_A);
 	udelay(200);
@@ -2172,24 +2306,18 @@ static void ironlake_edp_pll_on(struct intel_dp *intel_dp)
 static void ironlake_edp_pll_off(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_crtc *crtc = intel_dig_port->base.base.crtc;
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 dpa_ctl;
+	struct intel_crtc *crtc = to_intel_crtc(intel_dig_port->base.base.crtc);
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
 
-	assert_pipe_disabled(dev_priv,
-			     to_intel_crtc(crtc)->pipe);
+	assert_pipe_disabled(dev_priv, crtc->pipe);
+	assert_dp_port_disabled(intel_dp);
+	assert_edp_pll_enabled(dev_priv);
 
-	dpa_ctl = I915_READ(DP_A);
-	WARN((dpa_ctl & DP_PLL_ENABLE) == 0,
-	     "dp pll off, should be on\n");
-	WARN(dpa_ctl & DP_PORT_EN, "dp port still on, should be off\n");
+	DRM_DEBUG_KMS("disabling eDP PLL\n");
 
-	/* We can't rely on the value tracked for the DP register in
-	 * intel_dp->DP because link_down must not change that (otherwise link
-	 * re-training will fail. */
-	dpa_ctl &= ~DP_PLL_ENABLE;
-	I915_WRITE(DP_A, dpa_ctl);
+	intel_dp->DP &= ~DP_PLL_ENABLE;
+
+	I915_WRITE(DP_A, intel_dp->DP);
 	POSTING_READ(DP_A);
 	udelay(200);
 }
@@ -2234,15 +2362,18 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum intel_display_power_domain power_domain;
 	u32 tmp;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	tmp = I915_READ(intel_dp->output_reg);
 
 	if (!(tmp & DP_PORT_EN))
-		return false;
+		goto out;
 
 	if (IS_GEN7(dev) && port == PORT_A) {
 		*pipe = PORT_TO_PIPE_CPT(tmp);
@@ -2253,19 +2384,26 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 			u32 trans_dp = I915_READ(TRANS_DP_CTL(p));
 			if (TRANS_DP_PIPE_TO_PORT(trans_dp) == port) {
 				*pipe = p;
-				return true;
+				ret = true;
+
+				goto out;
 			}
 		}
 
 		DRM_DEBUG_KMS("No pipe for dp port 0x%x found\n",
-			      intel_dp->output_reg);
+			      i915_mmio_reg_offset(intel_dp->output_reg));
 	} else if (IS_CHERRYVIEW(dev)) {
 		*pipe = DP_PORT_TO_PIPE_CHV(tmp);
 	} else {
 		*pipe = PORT_TO_PIPE(tmp);
 	}
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void intel_dp_get_config(struct intel_encoder *encoder,
@@ -2310,7 +2448,7 @@ 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) &&
-	    tmp & DP_COLOR_RANGE_16_235)
+	    !IS_CHERRYVIEW(dev) && tmp & DP_COLOR_RANGE_16_235)
 		pipe_config->limited_color_range = true;
 
 	pipe_config->has_dp_encoder = true;
@@ -2321,7 +2459,7 @@ static void intel_dp_get_config(struct intel_encoder *encoder,
 	intel_dp_get_m_n(crtc, pipe_config);
 
 	if (port == PORT_A) {
-		if ((I915_READ(DP_A) & DP_PLL_FREQ_MASK) == DP_PLL_FREQ_160MHZ)
+		if ((I915_READ(DP_A) & DP_PLL_FREQ_MASK) == DP_PLL_FREQ_162MHZ)
 			pipe_config->port_clock = 162000;
 		else
 			pipe_config->port_clock = 270000;
@@ -2386,6 +2524,8 @@ static void ilk_post_disable_dp(struct intel_encoder *encoder)
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
 	intel_dp_link_down(intel_dp);
+
+	/* Only ilk+ has port A */
 	if (port == PORT_A)
 		ironlake_edp_pll_off(intel_dp);
 }
@@ -2545,6 +2685,8 @@ static void intel_dp_enable_port(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc =
+		to_intel_crtc(dp_to_dig_port(intel_dp)->base.base.crtc);
 
 	/* enable with pattern 1 (as per spec) */
 	_intel_dp_set_link_train(intel_dp, &intel_dp->DP,
@@ -2560,6 +2702,8 @@ static void intel_dp_enable_port(struct intel_dp *intel_dp)
 	 * fail when the power sequencer is freshly used for this port.
 	 */
 	intel_dp->DP |= DP_PORT_EN;
+	if (crtc->config->has_audio)
+		intel_dp->DP |= DP_AUDIO_OUTPUT_ENABLE;
 
 	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
 	POSTING_READ(intel_dp->output_reg);
@@ -2572,24 +2716,49 @@ static void intel_enable_dp(struct intel_encoder *encoder)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
 	uint32_t dp_reg = I915_READ(intel_dp->output_reg);
+	enum port port = dp_to_dig_port(intel_dp)->port;
+	enum pipe pipe = crtc->pipe;
 
 	if (WARN_ON(dp_reg & DP_PORT_EN))
 		return;
 
 	pps_lock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		vlv_init_panel_power_sequencer(intel_dp);
 
+	/*
+	 * We get an occasional spurious underrun between the port
+	 * enable and vdd enable, when enabling port A eDP.
+	 *
+	 * FIXME: Not sure if this applies to (PCH) port D eDP as well
+	 */
+	if (port == PORT_A)
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, false);
+
 	intel_dp_enable_port(intel_dp);
 
+	if (port == PORT_A && IS_GEN5(dev_priv)) {
+		/*
+		 * Underrun reporting for the other pipe was disabled in
+		 * g4x_pre_enable_dp(). The eDP PLL and port have now been
+		 * enabled, so it's now safe to re-enable underrun reporting.
+		 */
+		intel_wait_for_vblank_if_active(dev_priv->dev, !pipe);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, !pipe, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, !pipe, true);
+	}
+
 	edp_panel_vdd_on(intel_dp);
 	edp_panel_on(intel_dp);
 	edp_panel_vdd_off(intel_dp, true);
 
+	if (port == PORT_A)
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, pipe, true);
+
 	pps_unlock(intel_dp);
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		unsigned int lane_mask = 0x0;
 
 		if (IS_CHERRYVIEW(dev))
@@ -2605,7 +2774,7 @@ static void intel_enable_dp(struct intel_encoder *encoder)
 
 	if (crtc->config->has_audio) {
 		DRM_DEBUG_DRIVER("Enabling DP audio on pipe %c\n",
-				 pipe_name(crtc->pipe));
+				 pipe_name(pipe));
 		intel_audio_codec_enable(encoder);
 	}
 }
@@ -2628,16 +2797,29 @@ static void vlv_enable_dp(struct intel_encoder *encoder)
 
 static void g4x_pre_enable_dp(struct intel_encoder *encoder)
 {
+	struct drm_i915_private *dev_priv = to_i915(encoder->base.dev);
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
-	struct intel_digital_port *dport = dp_to_dig_port(intel_dp);
+	enum port port = dp_to_dig_port(intel_dp)->port;
+	enum pipe pipe = to_intel_crtc(encoder->base.crtc)->pipe;
 
 	intel_dp_prepare(encoder);
 
+	if (port == PORT_A && IS_GEN5(dev_priv)) {
+		/*
+		 * We get FIFO underruns on the other pipe when
+		 * enabling the CPU eDP PLL, and when enabling CPU
+		 * eDP port. We could potentially avoid the PLL
+		 * underrun with a vblank wait just prior to enabling
+		 * the PLL, but that doesn't appear to help the port
+		 * enable case. Just sweep it all under the rug.
+		 */
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, !pipe, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, !pipe, false);
+	}
+
 	/* Only ilk+ has port A */
-	if (dport->port == PORT_A) {
-		ironlake_set_pll_cpu_edp(intel_dp);
+	if (port == PORT_A)
 		ironlake_edp_pll_on(intel_dp);
-	}
 }
 
 static void vlv_detach_power_sequencer(struct intel_dp *intel_dp)
@@ -2645,7 +2827,7 @@ static void vlv_detach_power_sequencer(struct intel_dp *intel_dp)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_i915_private *dev_priv = intel_dig_port->base.base.dev->dev_private;
 	enum pipe pipe = intel_dp->pps_pipe;
-	int pp_on_reg = VLV_PIPE_PP_ON_DELAYS(pipe);
+	i915_reg_t pp_on_reg = VLV_PIPE_PP_ON_DELAYS(pipe);
 
 	edp_panel_vdd_off_sync(intel_dp);
 
@@ -2677,8 +2859,7 @@ static void vlv_steal_power_sequencer(struct drm_device *dev,
 	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
 		return;
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		struct intel_dp *intel_dp;
 		enum port port;
 
@@ -3043,7 +3224,7 @@ intel_dp_dpcd_read_wake(struct drm_dp_aux *aux, unsigned int offset,
  * Fetch AUX CH registers 0x202 - 0x207 which contain
  * link status information
  */
-static bool
+bool
 intel_dp_get_link_status(struct intel_dp *intel_dp, uint8_t link_status[DP_LINK_STATUS_SIZE])
 {
 	return intel_dp_dpcd_read_wake(&intel_dp->aux,
@@ -3053,7 +3234,7 @@ intel_dp_get_link_status(struct intel_dp *intel_dp, uint8_t link_status[DP_LINK_
 }
 
 /* These are source-specific values. */
-static uint8_t
+uint8_t
 intel_dp_voltage_max(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
@@ -3066,7 +3247,7 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 		if (dev_priv->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))
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else if (IS_GEN7(dev) && port == PORT_A)
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
@@ -3076,7 +3257,7 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
 }
 
-static uint8_t
+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);
@@ -3107,7 +3288,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)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
 		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			return DP_TRAIN_PRE_EMPH_LEVEL_3;
@@ -3418,38 +3599,6 @@ static uint32_t chv_signal_levels(struct intel_dp *intel_dp)
 	return 0;
 }
 
-static void
-intel_get_adjust_train(struct intel_dp *intel_dp,
-		       const uint8_t link_status[DP_LINK_STATUS_SIZE])
-{
-	uint8_t v = 0;
-	uint8_t p = 0;
-	int lane;
-	uint8_t voltage_max;
-	uint8_t preemph_max;
-
-	for (lane = 0; lane < intel_dp->lane_count; lane++) {
-		uint8_t this_v = drm_dp_get_adjust_request_voltage(link_status, lane);
-		uint8_t this_p = drm_dp_get_adjust_request_pre_emphasis(link_status, lane);
-
-		if (this_v > v)
-			v = this_v;
-		if (this_p > p)
-			p = this_p;
-	}
-
-	voltage_max = intel_dp_voltage_max(intel_dp);
-	if (v >= voltage_max)
-		v = voltage_max | DP_TRAIN_MAX_SWING_REACHED;
-
-	preemph_max = intel_dp_pre_emphasis_max(intel_dp, v);
-	if (p >= preemph_max)
-		p = preemph_max | DP_TRAIN_MAX_PRE_EMPHASIS_REACHED;
-
-	for (lane = 0; lane < 4; lane++)
-		intel_dp->train_set[lane] = v | p;
-}
-
 static uint32_t
 gen4_signal_levels(uint8_t train_set)
 {
@@ -3547,13 +3696,13 @@ gen7_edp_signal_levels(uint8_t train_set)
 	}
 }
 
-/* Properly updates "DP" with the correct signal levels. */
-static void
-intel_dp_set_signal_levels(struct intel_dp *intel_dp, uint32_t *DP)
+void
+intel_dp_set_signal_levels(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	enum port port = intel_dig_port->port;
 	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = to_i915(dev);
 	uint32_t signal_levels, mask = 0;
 	uint8_t train_set = intel_dp->train_set[0];
 
@@ -3588,74 +3737,27 @@ intel_dp_set_signal_levels(struct intel_dp *intel_dp, uint32_t *DP)
 		(train_set & DP_TRAIN_PRE_EMPHASIS_MASK) >>
 			DP_TRAIN_PRE_EMPHASIS_SHIFT);
 
-	*DP = (*DP & ~mask) | signal_levels;
-}
-
-static bool
-intel_dp_set_link_train(struct intel_dp *intel_dp,
-			uint32_t *DP,
-			uint8_t dp_train_pat)
-{
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_i915_private *dev_priv =
-		to_i915(intel_dig_port->base.base.dev);
-	uint8_t buf[sizeof(intel_dp->train_set) + 1];
-	int ret, len;
-
-	_intel_dp_set_link_train(intel_dp, DP, dp_train_pat);
+	intel_dp->DP = (intel_dp->DP & ~mask) | signal_levels;
 
-	I915_WRITE(intel_dp->output_reg, *DP);
+	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
 	POSTING_READ(intel_dp->output_reg);
-
-	buf[0] = dp_train_pat;
-	if ((dp_train_pat & DP_TRAINING_PATTERN_MASK) ==
-	    DP_TRAINING_PATTERN_DISABLE) {
-		/* don't write DP_TRAINING_LANEx_SET on disable */
-		len = 1;
-	} else {
-		/* DP_TRAINING_LANEx_SET follow DP_TRAINING_PATTERN_SET */
-		memcpy(buf + 1, intel_dp->train_set, intel_dp->lane_count);
-		len = intel_dp->lane_count + 1;
-	}
-
-	ret = drm_dp_dpcd_write(&intel_dp->aux, DP_TRAINING_PATTERN_SET,
-				buf, len);
-
-	return ret == len;
-}
-
-static bool
-intel_dp_reset_link_train(struct intel_dp *intel_dp, uint32_t *DP,
-			uint8_t dp_train_pat)
-{
-	if (!intel_dp->train_set_valid)
-		memset(intel_dp->train_set, 0, sizeof(intel_dp->train_set));
-	intel_dp_set_signal_levels(intel_dp, DP);
-	return intel_dp_set_link_train(intel_dp, DP, dp_train_pat);
 }
 
-static bool
-intel_dp_update_link_train(struct intel_dp *intel_dp, uint32_t *DP,
-			   const uint8_t link_status[DP_LINK_STATUS_SIZE])
+void
+intel_dp_program_link_training_pattern(struct intel_dp *intel_dp,
+				       uint8_t dp_train_pat)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_i915_private *dev_priv =
 		to_i915(intel_dig_port->base.base.dev);
-	int ret;
 
-	intel_get_adjust_train(intel_dp, link_status);
-	intel_dp_set_signal_levels(intel_dp, DP);
+	_intel_dp_set_link_train(intel_dp, &intel_dp->DP, dp_train_pat);
 
-	I915_WRITE(intel_dp->output_reg, *DP);
+	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
 	POSTING_READ(intel_dp->output_reg);
-
-	ret = drm_dp_dpcd_write(&intel_dp->aux, DP_TRAINING_LANE0_SET,
-				intel_dp->train_set, intel_dp->lane_count);
-
-	return ret == intel_dp->lane_count;
 }
 
-static void intel_dp_set_idle_link_train(struct intel_dp *intel_dp)
+void intel_dp_set_idle_link_train(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
@@ -3686,232 +3788,6 @@ static void intel_dp_set_idle_link_train(struct intel_dp *intel_dp)
 		DRM_ERROR("Timed out waiting for DP idle patterns\n");
 }
 
-/* Enable corresponding port and start training pattern 1 */
-static void
-intel_dp_link_training_clock_recovery(struct intel_dp *intel_dp)
-{
-	struct drm_encoder *encoder = &dp_to_dig_port(intel_dp)->base.base;
-	struct drm_device *dev = encoder->dev;
-	int i;
-	uint8_t voltage;
-	int voltage_tries, loop_tries;
-	uint32_t DP = intel_dp->DP;
-	uint8_t link_config[2];
-	uint8_t link_bw, rate_select;
-
-	if (HAS_DDI(dev))
-		intel_ddi_prepare_link_retrain(encoder);
-
-	intel_dp_compute_rate(intel_dp, intel_dp->link_rate,
-			      &link_bw, &rate_select);
-
-	/* Write the link configuration data */
-	link_config[0] = link_bw;
-	link_config[1] = intel_dp->lane_count;
-	if (drm_dp_enhanced_frame_cap(intel_dp->dpcd))
-		link_config[1] |= DP_LANE_COUNT_ENHANCED_FRAME_EN;
-	drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_BW_SET, link_config, 2);
-	if (intel_dp->num_sink_rates)
-		drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_RATE_SET,
-				  &rate_select, 1);
-
-	link_config[0] = 0;
-	link_config[1] = DP_SET_ANSI_8B10B;
-	drm_dp_dpcd_write(&intel_dp->aux, DP_DOWNSPREAD_CTRL, link_config, 2);
-
-	DP |= DP_PORT_EN;
-
-	/* clock recovery */
-	if (!intel_dp_reset_link_train(intel_dp, &DP,
-				       DP_TRAINING_PATTERN_1 |
-				       DP_LINK_SCRAMBLING_DISABLE)) {
-		DRM_ERROR("failed to enable link training\n");
-		return;
-	}
-
-	voltage = 0xff;
-	voltage_tries = 0;
-	loop_tries = 0;
-	for (;;) {
-		uint8_t link_status[DP_LINK_STATUS_SIZE];
-
-		drm_dp_link_train_clock_recovery_delay(intel_dp->dpcd);
-		if (!intel_dp_get_link_status(intel_dp, link_status)) {
-			DRM_ERROR("failed to get link status\n");
-			break;
-		}
-
-		if (drm_dp_clock_recovery_ok(link_status, intel_dp->lane_count)) {
-			DRM_DEBUG_KMS("clock recovery OK\n");
-			break;
-		}
-
-		/*
-		 * if we used previously trained voltage and pre-emphasis values
-		 * and we don't get clock recovery, reset link training values
-		 */
-		if (intel_dp->train_set_valid) {
-			DRM_DEBUG_KMS("clock recovery not ok, reset");
-			/* clear the flag as we are not reusing train set */
-			intel_dp->train_set_valid = false;
-			if (!intel_dp_reset_link_train(intel_dp, &DP,
-						       DP_TRAINING_PATTERN_1 |
-						       DP_LINK_SCRAMBLING_DISABLE)) {
-				DRM_ERROR("failed to enable link training\n");
-				return;
-			}
-			continue;
-		}
-
-		/* Check to see if we've tried the max voltage */
-		for (i = 0; i < intel_dp->lane_count; i++)
-			if ((intel_dp->train_set[i] & DP_TRAIN_MAX_SWING_REACHED) == 0)
-				break;
-		if (i == intel_dp->lane_count) {
-			++loop_tries;
-			if (loop_tries == 5) {
-				DRM_ERROR("too many full retries, give up\n");
-				break;
-			}
-			intel_dp_reset_link_train(intel_dp, &DP,
-						  DP_TRAINING_PATTERN_1 |
-						  DP_LINK_SCRAMBLING_DISABLE);
-			voltage_tries = 0;
-			continue;
-		}
-
-		/* Check to see if we've tried the same voltage 5 times */
-		if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) == voltage) {
-			++voltage_tries;
-			if (voltage_tries == 5) {
-				DRM_ERROR("too many voltage retries, give up\n");
-				break;
-			}
-		} else
-			voltage_tries = 0;
-		voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
-
-		/* Update training set as requested by target */
-		if (!intel_dp_update_link_train(intel_dp, &DP, link_status)) {
-			DRM_ERROR("failed to update link training\n");
-			break;
-		}
-	}
-
-	intel_dp->DP = DP;
-}
-
-static void
-intel_dp_link_training_channel_equalization(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;
-	bool channel_eq = false;
-	int tries, cr_tries;
-	uint32_t DP = intel_dp->DP;
-	uint32_t training_pattern = DP_TRAINING_PATTERN_2;
-
-	/*
-	 * Training Pattern 3 for HBR2 or 1.2 devices that support it.
-	 *
-	 * Intel platforms that support HBR2 also support TPS3. TPS3 support is
-	 * also mandatory for downstream devices that support HBR2.
-	 *
-	 * Due to WaDisableHBR2 SKL < B0 is the only exception where TPS3 is
-	 * supported but still not enabled.
-	 */
-	if (intel_dp_source_supports_hbr2(dev) &&
-	    drm_dp_tps3_supported(intel_dp->dpcd))
-		training_pattern = DP_TRAINING_PATTERN_3;
-	else if (intel_dp->link_rate == 540000)
-		DRM_ERROR("5.4 Gbps link rate without HBR2/TPS3 support\n");
-
-	/* channel equalization */
-	if (!intel_dp_set_link_train(intel_dp, &DP,
-				     training_pattern |
-				     DP_LINK_SCRAMBLING_DISABLE)) {
-		DRM_ERROR("failed to start channel equalization\n");
-		return;
-	}
-
-	tries = 0;
-	cr_tries = 0;
-	channel_eq = false;
-	for (;;) {
-		uint8_t link_status[DP_LINK_STATUS_SIZE];
-
-		if (cr_tries > 5) {
-			DRM_ERROR("failed to train DP, aborting\n");
-			break;
-		}
-
-		drm_dp_link_train_channel_eq_delay(intel_dp->dpcd);
-		if (!intel_dp_get_link_status(intel_dp, link_status)) {
-			DRM_ERROR("failed to get link status\n");
-			break;
-		}
-
-		/* Make sure clock is still ok */
-		if (!drm_dp_clock_recovery_ok(link_status,
-					      intel_dp->lane_count)) {
-			intel_dp->train_set_valid = false;
-			intel_dp_link_training_clock_recovery(intel_dp);
-			intel_dp_set_link_train(intel_dp, &DP,
-						training_pattern |
-						DP_LINK_SCRAMBLING_DISABLE);
-			cr_tries++;
-			continue;
-		}
-
-		if (drm_dp_channel_eq_ok(link_status,
-					 intel_dp->lane_count)) {
-			channel_eq = true;
-			break;
-		}
-
-		/* Try 5 times, then try clock recovery if that fails */
-		if (tries > 5) {
-			intel_dp->train_set_valid = false;
-			intel_dp_link_training_clock_recovery(intel_dp);
-			intel_dp_set_link_train(intel_dp, &DP,
-						training_pattern |
-						DP_LINK_SCRAMBLING_DISABLE);
-			tries = 0;
-			cr_tries++;
-			continue;
-		}
-
-		/* Update training set as requested by target */
-		if (!intel_dp_update_link_train(intel_dp, &DP, link_status)) {
-			DRM_ERROR("failed to update link training\n");
-			break;
-		}
-		++tries;
-	}
-
-	intel_dp_set_idle_link_train(intel_dp);
-
-	intel_dp->DP = DP;
-
-	if (channel_eq) {
-		intel_dp->train_set_valid = true;
-		DRM_DEBUG_KMS("Channel EQ done. DP Training successful\n");
-	}
-}
-
-void intel_dp_stop_link_train(struct intel_dp *intel_dp)
-{
-	intel_dp_set_link_train(intel_dp, &intel_dp->DP,
-				DP_TRAINING_PATTERN_DISABLE);
-}
-
-void
-intel_dp_start_link_train(struct intel_dp *intel_dp)
-{
-	intel_dp_link_training_clock_recovery(intel_dp);
-	intel_dp_link_training_channel_equalization(intel_dp);
-}
-
 static void
 intel_dp_link_down(struct intel_dp *intel_dp)
 {
@@ -3954,6 +3830,13 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 	 * matching HDMI port to be enabled on transcoder A.
 	 */
 	if (HAS_PCH_IBX(dev) && 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.
+		 */
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+
 		/* always enable with pattern 1 (as per spec) */
 		DP &= ~(DP_PIPEB_SELECT | DP_LINK_TRAIN_MASK);
 		DP |= DP_PORT_EN | DP_LINK_TRAIN_PAT_1;
@@ -3963,9 +3846,15 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 		DP &= ~DP_PORT_EN;
 		I915_WRITE(intel_dp->output_reg, DP);
 		POSTING_READ(intel_dp->output_reg);
+
+		intel_wait_for_vblank_if_active(dev_priv->dev, PIPE_A);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, true);
 	}
 
 	msleep(intel_dp->panel_power_down_delay);
+
+	intel_dp->DP = DP;
 }
 
 static bool
@@ -4013,7 +3902,7 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	}
 
 	DRM_DEBUG_KMS("Display Port TPS3 support: source %s, sink %s\n",
-		      yesno(intel_dp_source_supports_hbr2(dev)),
+		      yesno(intel_dp_source_supports_hbr2(intel_dp)),
 		      yesno(drm_dp_tps3_supported(intel_dp->dpcd)));
 
 	/* Intermediate frequency support */
@@ -4103,9 +3992,12 @@ intel_dp_probe_mst(struct intel_dp *intel_dp)
 static int intel_dp_sink_crc_stop(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
 	struct intel_crtc *intel_crtc = to_intel_crtc(dig_port->base.base.crtc);
 	u8 buf;
 	int ret = 0;
+	int count = 0;
+	int attempts = 10;
 
 	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK, &buf) < 0) {
 		DRM_DEBUG_KMS("Sink CRC couldn't be stopped properly\n");
@@ -4120,7 +4012,22 @@ static int intel_dp_sink_crc_stop(struct intel_dp *intel_dp)
 		goto out;
 	}
 
-	intel_dp->sink_crc.started = false;
+	do {
+		intel_wait_for_vblank(dev, intel_crtc->pipe);
+
+		if (drm_dp_dpcd_readb(&intel_dp->aux,
+				      DP_TEST_SINK_MISC, &buf) < 0) {
+			ret = -EIO;
+			goto out;
+		}
+		count = buf & DP_TEST_COUNT_MASK;
+	} while (--attempts && count);
+
+	if (attempts == 0) {
+		DRM_ERROR("TIMEOUT: Sink CRC counter is not zeroed\n");
+		ret = -ETIMEDOUT;
+	}
+
  out:
 	hsw_enable_ips(intel_crtc);
 	return ret;
@@ -4129,27 +4036,26 @@ static int intel_dp_sink_crc_stop(struct intel_dp *intel_dp)
 static int intel_dp_sink_crc_start(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = dig_port->base.base.dev;
 	struct intel_crtc *intel_crtc = to_intel_crtc(dig_port->base.base.crtc);
 	u8 buf;
 	int ret;
 
-	if (intel_dp->sink_crc.started) {
-		ret = intel_dp_sink_crc_stop(intel_dp);
-		if (ret)
-			return ret;
-	}
-
 	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK_MISC, &buf) < 0)
 		return -EIO;
 
 	if (!(buf & DP_TEST_CRC_SUPPORTED))
 		return -ENOTTY;
 
-	intel_dp->sink_crc.last_count = buf & DP_TEST_COUNT_MASK;
-
 	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK, &buf) < 0)
 		return -EIO;
 
+	if (buf & DP_TEST_SINK_START) {
+		ret = intel_dp_sink_crc_stop(intel_dp);
+		if (ret)
+			return ret;
+	}
+
 	hsw_disable_ips(intel_crtc);
 
 	if (drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK,
@@ -4158,7 +4064,7 @@ static int intel_dp_sink_crc_start(struct intel_dp *intel_dp)
 		return -EIO;
 	}
 
-	intel_dp->sink_crc.started = true;
+	intel_wait_for_vblank(dev, intel_crtc->pipe);
 	return 0;
 }
 
@@ -4170,7 +4076,6 @@ int intel_dp_sink_crc(struct intel_dp *intel_dp, u8 *crc)
 	u8 buf;
 	int count, ret;
 	int attempts = 6;
-	bool old_equal_new;
 
 	ret = intel_dp_sink_crc_start(intel_dp);
 	if (ret)
@@ -4186,35 +4091,17 @@ int intel_dp_sink_crc(struct intel_dp *intel_dp, u8 *crc)
 		}
 		count = buf & DP_TEST_COUNT_MASK;
 
-		/*
-		 * Count might be reset during the loop. In this case
-		 * last known count needs to be reset as well.
-		 */
-		if (count == 0)
-			intel_dp->sink_crc.last_count = 0;
-
-		if (drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_CRC_R_CR, crc, 6) < 0) {
-			ret = -EIO;
-			goto stop;
-		}
-
-		old_equal_new = (count == intel_dp->sink_crc.last_count &&
-				 !memcmp(intel_dp->sink_crc.last_crc, crc,
-					 6 * sizeof(u8)));
-
-	} while (--attempts && (count == 0 || old_equal_new));
-
-	intel_dp->sink_crc.last_count = buf & DP_TEST_COUNT_MASK;
-	memcpy(intel_dp->sink_crc.last_crc, crc, 6 * sizeof(u8));
+	} while (--attempts && count == 0);
 
 	if (attempts == 0) {
-		if (old_equal_new) {
-			DRM_DEBUG_KMS("Unreliable Sink CRC counter: Current returned CRC is identical to the previous one\n");
-		} else {
-			DRM_ERROR("Panel is unable to calculate any CRC after 6 vblanks\n");
-			ret = -ETIMEDOUT;
-			goto stop;
-		}
+		DRM_ERROR("Panel is unable to calculate any CRC after 6 vblanks\n");
+		ret = -ETIMEDOUT;
+		goto stop;
+	}
+
+	if (drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_CRC_R_CR, crc, 6) < 0) {
+		ret = -EIO;
+		goto stop;
 	}
 
 stop:
@@ -4314,13 +4201,6 @@ static void intel_dp_handle_test_request(struct intel_dp *intel_dp)
 	uint8_t rxdata = 0;
 	int status = 0;
 
-	intel_dp->compliance_test_active = 0;
-	intel_dp->compliance_test_type = 0;
-	intel_dp->compliance_test_data = 0;
-
-	intel_dp->aux.i2c_nack_count = 0;
-	intel_dp->aux.i2c_defer_count = 0;
-
 	status = drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_REQUEST, &rxdata, 1);
 	if (status <= 0) {
 		DRM_DEBUG_KMS("Could not read test request from sink\n");
@@ -4436,6 +4316,14 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 
 	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
 
+	/*
+	 * Clearing compliance test variables to allow capturing
+	 * of values for next automated test request.
+	 */
+	intel_dp->compliance_test_active = 0;
+	intel_dp->compliance_test_type = 0;
+	intel_dp->compliance_test_data = 0;
+
 	if (!intel_encoder->base.crtc)
 		return;
 
@@ -4466,7 +4354,9 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 			DRM_DEBUG_DRIVER("CP or sink specific irq unhandled\n");
 	}
 
-	if (!drm_dp_channel_eq_ok(link_status, intel_dp->lane_count)) {
+	/* 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);
@@ -4613,20 +4503,20 @@ static bool g4x_digital_port_connected(struct drm_i915_private *dev_priv,
 	return I915_READ(PORT_HOTPLUG_STAT) & bit;
 }
 
-static bool vlv_digital_port_connected(struct drm_i915_private *dev_priv,
-				       struct intel_digital_port *port)
+static bool gm45_digital_port_connected(struct drm_i915_private *dev_priv,
+					struct intel_digital_port *port)
 {
 	u32 bit;
 
 	switch (port->port) {
 	case PORT_B:
-		bit = PORTB_HOTPLUG_LIVE_STATUS_VLV;
+		bit = PORTB_HOTPLUG_LIVE_STATUS_GM45;
 		break;
 	case PORT_C:
-		bit = PORTC_HOTPLUG_LIVE_STATUS_VLV;
+		bit = PORTC_HOTPLUG_LIVE_STATUS_GM45;
 		break;
 	case PORT_D:
-		bit = PORTD_HOTPLUG_LIVE_STATUS_VLV;
+		bit = PORTD_HOTPLUG_LIVE_STATUS_GM45;
 		break;
 	default:
 		MISSING_CASE(port->port);
@@ -4678,47 +4568,12 @@ bool intel_digital_port_connected(struct drm_i915_private *dev_priv,
 		return cpt_digital_port_connected(dev_priv, port);
 	else if (IS_BROXTON(dev_priv))
 		return bxt_digital_port_connected(dev_priv, port);
-	else if (IS_VALLEYVIEW(dev_priv))
-		return vlv_digital_port_connected(dev_priv, port);
+	else if (IS_GM45(dev_priv))
+		return gm45_digital_port_connected(dev_priv, port);
 	else
 		return g4x_digital_port_connected(dev_priv, port);
 }
 
-static enum drm_connector_status
-ironlake_dp_detect(struct intel_dp *intel_dp)
-{
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-
-	if (!intel_digital_port_connected(dev_priv, intel_dig_port))
-		return connector_status_disconnected;
-
-	return intel_dp_detect_dpcd(intel_dp);
-}
-
-static enum drm_connector_status
-g4x_dp_detect(struct intel_dp *intel_dp)
-{
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-
-	/* Can't disconnect eDP, but you can close the lid... */
-	if (is_edp(intel_dp)) {
-		enum drm_connector_status status;
-
-		status = intel_panel_detect(dev);
-		if (status == connector_status_unknown)
-			status = connector_status_connected;
-		return status;
-	}
-
-	if (!intel_digital_port_connected(dev->dev_private, intel_dig_port))
-		return connector_status_disconnected;
-
-	return intel_dp_detect_dpcd(intel_dp);
-}
-
 static struct edid *
 intel_dp_get_edid(struct intel_dp *intel_dp)
 {
@@ -4791,12 +4646,19 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	/* Can't disconnect eDP, but you can close the lid... */
 	if (is_edp(intel_dp))
 		status = edp_detect(intel_dp);
-	else if (HAS_PCH_SPLIT(dev))
-		status = ironlake_dp_detect(intel_dp);
+	else if (intel_digital_port_connected(to_i915(dev),
+					      dp_to_dig_port(intel_dp)))
+		status = intel_dp_detect_dpcd(intel_dp);
 	else
-		status = g4x_dp_detect(intel_dp);
-	if (status != connector_status_connected)
+		status = connector_status_disconnected;
+
+	if (status != connector_status_connected) {
+		intel_dp->compliance_test_active = 0;
+		intel_dp->compliance_test_type = 0;
+		intel_dp->compliance_test_data = 0;
+
 		goto out;
+	}
 
 	intel_dp_probe_oui(intel_dp);
 
@@ -4810,6 +4672,14 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 		goto out;
 	}
 
+	/*
+	 * Clearing NACK and defer counts to get their exact values
+	 * while reading EDID which are required by Compliance tests
+	 * 4.2.2.4 and 4.2.2.5
+	 */
+	intel_dp->aux.i2c_nack_count = 0;
+	intel_dp->aux.i2c_defer_count = 0;
+
 	intel_dp_set_edid(intel_dp);
 
 	if (intel_encoder->type != INTEL_OUTPUT_EDP)
@@ -5014,7 +4884,7 @@ void intel_dp_encoder_destroy(struct drm_encoder *encoder)
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
 
-	drm_dp_aux_unregister(&intel_dp->aux);
+	intel_dp_aux_fini(intel_dp);
 	intel_dp_mst_encoder_cleanup(intel_dig_port);
 	if (is_edp(intel_dp)) {
 		cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
@@ -5092,7 +4962,7 @@ static void intel_dp_encoder_reset(struct drm_encoder *encoder)
 	 * Read out the current power sequencer assignment,
 	 * in case the BIOS did something with it.
 	 */
-	if (IS_VALLEYVIEW(encoder->dev))
+	if (IS_VALLEYVIEW(encoder->dev) || IS_CHERRYVIEW(encoder->dev))
 		vlv_initial_power_sequencer_setup(intel_dp);
 
 	intel_edp_panel_vdd_sanitize(intel_dp);
@@ -5204,25 +5074,6 @@ put_power:
 	return ret;
 }
 
-/* Return which DP Port should be selected for Transcoder DP control */
-int
-intel_trans_dp_port_sel(struct drm_crtc *crtc)
-{
-	struct drm_device *dev = crtc->dev;
-	struct intel_encoder *intel_encoder;
-	struct intel_dp *intel_dp;
-
-	for_each_encoder_on_crtc(dev, crtc, intel_encoder) {
-		intel_dp = enc_to_intel_dp(&intel_encoder->base);
-
-		if (intel_encoder->type == INTEL_OUTPUT_DISPLAYPORT ||
-		    intel_encoder->type == INTEL_OUTPUT_EDP)
-			return intel_dp->output_reg;
-	}
-
-	return -1;
-}
-
 /* check the VBT to see whether the eDP is on another port */
 bool intel_dp_is_edp(struct drm_device *dev, enum port port)
 {
@@ -5294,7 +5145,7 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 	struct edp_power_seq cur, vbt, spec,
 		*final = &intel_dp->pps_delays;
 	u32 pp_on, pp_off, pp_div = 0, pp_ctl = 0;
-	int pp_ctrl_reg, pp_on_reg, pp_off_reg, pp_div_reg = 0;
+	i915_reg_t pp_ctrl_reg, pp_on_reg, pp_off_reg, pp_div_reg;
 
 	lockdep_assert_held(&dev_priv->pps_mutex);
 
@@ -5416,7 +5267,7 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp_on, pp_off, pp_div, port_sel = 0;
 	int div = HAS_PCH_SPLIT(dev) ? intel_pch_rawclk(dev) : intel_hrawclk(dev);
-	int pp_on_reg, pp_off_reg, pp_div_reg = 0, pp_ctrl_reg;
+	i915_reg_t pp_on_reg, pp_off_reg, pp_div_reg, pp_ctrl_reg;
 	enum port port = dp_to_dig_port(intel_dp)->port;
 	const struct edp_power_seq *seq = &intel_dp->pps_delays;
 
@@ -5471,7 +5322,7 @@ 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)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		port_sel = PANEL_PORT_SELECT_VLV(port);
 	} else if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)) {
 		if (port == PORT_A)
@@ -5578,17 +5429,17 @@ static void intel_dp_set_drrs_state(struct drm_device *dev, int refresh_rate)
 			DRM_ERROR("Unsupported refreshrate type\n");
 		}
 	} else if (INTEL_INFO(dev)->gen > 6) {
-		u32 reg = PIPECONF(intel_crtc->config->cpu_transcoder);
+		i915_reg_t reg = PIPECONF(intel_crtc->config->cpu_transcoder);
 		u32 val;
 
 		val = I915_READ(reg);
 		if (index > DRRS_HIGH_RR) {
-			if (IS_VALLEYVIEW(dev))
+			if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 				val |= PIPECONF_EDP_RR_MODE_SWITCH_VLV;
 			else
 				val |= PIPECONF_EDP_RR_MODE_SWITCH;
 		} else {
-			if (IS_VALLEYVIEW(dev))
+			if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 				val &= ~PIPECONF_EDP_RR_MODE_SWITCH_VLV;
 			else
 				val &= ~PIPECONF_EDP_RR_MODE_SWITCH;
@@ -5955,7 +5806,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	}
 	mutex_unlock(&dev->mode_config.mutex);
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		intel_dp->edp_notifier.notifier_call = edp_notify_handler;
 		register_reboot_notifier(&intel_dp->edp_notifier);
 
@@ -5996,14 +5847,14 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	struct drm_device *dev = intel_encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum port port = intel_dig_port->port;
-	int type;
+	int type, ret;
 
 	intel_dp->pps_pipe = INVALID_PIPE;
 
 	/* intel_dp vfuncs */
 	if (INTEL_INFO(dev)->gen >= 9)
 		intel_dp->get_aux_clock_divider = skl_get_aux_clock_divider;
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		intel_dp->get_aux_clock_divider = vlv_get_aux_clock_divider;
 	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		intel_dp->get_aux_clock_divider = hsw_get_aux_clock_divider;
@@ -6017,6 +5868,9 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	else
 		intel_dp->get_aux_send_ctl = i9xx_get_aux_send_ctl;
 
+	if (HAS_DDI(dev))
+		intel_dp->prepare_link_retrain = intel_ddi_prepare_link_retrain;
+
 	/* Preserve the current hw state. */
 	intel_dp->DP = I915_READ(intel_dp->output_reg);
 	intel_dp->attached_connector = intel_connector;
@@ -6035,8 +5889,8 @@ 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_edp(intel_dp) &&
-		    port != PORT_B && port != PORT_C))
+	if (WARN_ON((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
+		    is_edp(intel_dp) && port != PORT_B && port != PORT_C))
 		return false;
 
 	DRM_DEBUG_KMS("Adding %s connector on port %c\n",
@@ -6068,7 +5922,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_BROXTON(dev_priv) && (INTEL_REVID(dev) < BXT_REVID_B0))
+		if (IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 			intel_encoder->hpd_pin = HPD_PORT_A;
 		break;
 	case PORT_C:
@@ -6087,14 +5941,16 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	if (is_edp(intel_dp)) {
 		pps_lock(intel_dp);
 		intel_dp_init_panel_power_timestamps(intel_dp);
-		if (IS_VALLEYVIEW(dev))
+		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 			vlv_initial_power_sequencer_setup(intel_dp);
 		else
 			intel_dp_init_panel_power_sequencer(dev, intel_dp);
 		pps_unlock(intel_dp);
 	}
 
-	intel_dp_aux_init(intel_dp, intel_connector);
+	ret = intel_dp_aux_init(intel_dp, intel_connector);
+	if (ret)
+		goto fail;
 
 	/* init MST on ports that can support it */
 	if (HAS_DP_MST(dev) &&
@@ -6103,20 +5959,9 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 					  intel_connector->base.base.id);
 
 	if (!intel_edp_init_connector(intel_dp, intel_connector)) {
-		drm_dp_aux_unregister(&intel_dp->aux);
-		if (is_edp(intel_dp)) {
-			cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
-			/*
-			 * vdd might still be enabled do to the delayed vdd off.
-			 * Make sure vdd is actually turned off here.
-			 */
-			pps_lock(intel_dp);
-			edp_panel_vdd_off_sync(intel_dp);
-			pps_unlock(intel_dp);
-		}
-		drm_connector_unregister(connector);
-		drm_connector_cleanup(connector);
-		return false;
+		intel_dp_aux_fini(intel_dp);
+		intel_dp_mst_encoder_cleanup(intel_dig_port);
+		goto fail;
 	}
 
 	intel_dp_add_properties(intel_dp, connector);
@@ -6133,10 +5978,27 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	i915_debugfs_connector_add(connector);
 
 	return true;
+
+fail:
+	if (is_edp(intel_dp)) {
+		cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
+		/*
+		 * vdd might still be enabled do to the delayed vdd off.
+		 * Make sure vdd is actually turned off here.
+		 */
+		pps_lock(intel_dp);
+		edp_panel_vdd_off_sync(intel_dp);
+		pps_unlock(intel_dp);
+	}
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
+
+	return false;
 }
 
 void
-intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
+intel_dp_init(struct drm_device *dev,
+	      i915_reg_t output_reg, enum port port)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_digital_port *intel_dig_port;
@@ -6155,8 +6017,9 @@ intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
 	intel_encoder = &intel_dig_port->base;
 	encoder = &intel_encoder->base;
 
-	drm_encoder_init(dev, &intel_encoder->base, &intel_dp_enc_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+	if (drm_encoder_init(dev, &intel_encoder->base, &intel_dp_enc_funcs,
+			     DRM_MODE_ENCODER_TMDS, NULL))
+		goto err_encoder_init;
 
 	intel_encoder->compute_config = intel_dp_compute_config;
 	intel_encoder->disable = intel_disable_dp;
@@ -6205,6 +6068,7 @@ intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
 
 err_init_connector:
 	drm_encoder_cleanup(encoder);
+err_encoder_init:
 	kfree(intel_connector);
 err_connector_alloc:
 	kfree(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
new file mode 100644
index 000000000000..0b8eefc2acc5
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_dp_link_training.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright © 2008-2015 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 "intel_drv.h"
+
+static void
+intel_get_adjust_train(struct intel_dp *intel_dp,
+		       const uint8_t link_status[DP_LINK_STATUS_SIZE])
+{
+	uint8_t v = 0;
+	uint8_t p = 0;
+	int lane;
+	uint8_t voltage_max;
+	uint8_t preemph_max;
+
+	for (lane = 0; lane < intel_dp->lane_count; lane++) {
+		uint8_t this_v = drm_dp_get_adjust_request_voltage(link_status, lane);
+		uint8_t this_p = drm_dp_get_adjust_request_pre_emphasis(link_status, lane);
+
+		if (this_v > v)
+			v = this_v;
+		if (this_p > p)
+			p = this_p;
+	}
+
+	voltage_max = intel_dp_voltage_max(intel_dp);
+	if (v >= voltage_max)
+		v = voltage_max | DP_TRAIN_MAX_SWING_REACHED;
+
+	preemph_max = intel_dp_pre_emphasis_max(intel_dp, v);
+	if (p >= preemph_max)
+		p = preemph_max | DP_TRAIN_MAX_PRE_EMPHASIS_REACHED;
+
+	for (lane = 0; lane < 4; lane++)
+		intel_dp->train_set[lane] = v | p;
+}
+
+static bool
+intel_dp_set_link_train(struct intel_dp *intel_dp,
+			uint8_t dp_train_pat)
+{
+	uint8_t buf[sizeof(intel_dp->train_set) + 1];
+	int ret, len;
+
+	intel_dp_program_link_training_pattern(intel_dp, dp_train_pat);
+
+	buf[0] = dp_train_pat;
+	if ((dp_train_pat & DP_TRAINING_PATTERN_MASK) ==
+	    DP_TRAINING_PATTERN_DISABLE) {
+		/* don't write DP_TRAINING_LANEx_SET on disable */
+		len = 1;
+	} else {
+		/* DP_TRAINING_LANEx_SET follow DP_TRAINING_PATTERN_SET */
+		memcpy(buf + 1, intel_dp->train_set, intel_dp->lane_count);
+		len = intel_dp->lane_count + 1;
+	}
+
+	ret = drm_dp_dpcd_write(&intel_dp->aux, DP_TRAINING_PATTERN_SET,
+				buf, len);
+
+	return ret == len;
+}
+
+static bool
+intel_dp_reset_link_train(struct intel_dp *intel_dp,
+			uint8_t dp_train_pat)
+{
+	if (!intel_dp->train_set_valid)
+		memset(intel_dp->train_set, 0, sizeof(intel_dp->train_set));
+	intel_dp_set_signal_levels(intel_dp);
+	return intel_dp_set_link_train(intel_dp, dp_train_pat);
+}
+
+static bool
+intel_dp_update_link_train(struct intel_dp *intel_dp)
+{
+	int ret;
+
+	intel_dp_set_signal_levels(intel_dp);
+
+	ret = drm_dp_dpcd_write(&intel_dp->aux, DP_TRAINING_LANE0_SET,
+				intel_dp->train_set, intel_dp->lane_count);
+
+	return ret == intel_dp->lane_count;
+}
+
+/* Enable corresponding port and start training pattern 1 */
+static void
+intel_dp_link_training_clock_recovery(struct intel_dp *intel_dp)
+{
+	int i;
+	uint8_t voltage;
+	int voltage_tries, loop_tries;
+	uint8_t link_config[2];
+	uint8_t link_bw, rate_select;
+
+	if (intel_dp->prepare_link_retrain)
+		intel_dp->prepare_link_retrain(intel_dp);
+
+	intel_dp_compute_rate(intel_dp, intel_dp->link_rate,
+			      &link_bw, &rate_select);
+
+	/* Write the link configuration data */
+	link_config[0] = link_bw;
+	link_config[1] = intel_dp->lane_count;
+	if (drm_dp_enhanced_frame_cap(intel_dp->dpcd))
+		link_config[1] |= DP_LANE_COUNT_ENHANCED_FRAME_EN;
+	drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_BW_SET, link_config, 2);
+	if (intel_dp->num_sink_rates)
+		drm_dp_dpcd_write(&intel_dp->aux, DP_LINK_RATE_SET,
+				  &rate_select, 1);
+
+	link_config[0] = 0;
+	link_config[1] = DP_SET_ANSI_8B10B;
+	drm_dp_dpcd_write(&intel_dp->aux, DP_DOWNSPREAD_CTRL, link_config, 2);
+
+	intel_dp->DP |= DP_PORT_EN;
+
+	/* clock recovery */
+	if (!intel_dp_reset_link_train(intel_dp,
+				       DP_TRAINING_PATTERN_1 |
+				       DP_LINK_SCRAMBLING_DISABLE)) {
+		DRM_ERROR("failed to enable link training\n");
+		return;
+	}
+
+	voltage = 0xff;
+	voltage_tries = 0;
+	loop_tries = 0;
+	for (;;) {
+		uint8_t link_status[DP_LINK_STATUS_SIZE];
+
+		drm_dp_link_train_clock_recovery_delay(intel_dp->dpcd);
+		if (!intel_dp_get_link_status(intel_dp, link_status)) {
+			DRM_ERROR("failed to get link status\n");
+			break;
+		}
+
+		if (drm_dp_clock_recovery_ok(link_status, intel_dp->lane_count)) {
+			DRM_DEBUG_KMS("clock recovery OK\n");
+			break;
+		}
+
+		/*
+		 * if we used previously trained voltage and pre-emphasis values
+		 * and we don't get clock recovery, reset link training values
+		 */
+		if (intel_dp->train_set_valid) {
+			DRM_DEBUG_KMS("clock recovery not ok, reset");
+			/* clear the flag as we are not reusing train set */
+			intel_dp->train_set_valid = false;
+			if (!intel_dp_reset_link_train(intel_dp,
+						       DP_TRAINING_PATTERN_1 |
+						       DP_LINK_SCRAMBLING_DISABLE)) {
+				DRM_ERROR("failed to enable link training\n");
+				return;
+			}
+			continue;
+		}
+
+		/* Check to see if we've tried the max voltage */
+		for (i = 0; i < intel_dp->lane_count; i++)
+			if ((intel_dp->train_set[i] & DP_TRAIN_MAX_SWING_REACHED) == 0)
+				break;
+		if (i == intel_dp->lane_count) {
+			++loop_tries;
+			if (loop_tries == 5) {
+				DRM_ERROR("too many full retries, give up\n");
+				break;
+			}
+			intel_dp_reset_link_train(intel_dp,
+						  DP_TRAINING_PATTERN_1 |
+						  DP_LINK_SCRAMBLING_DISABLE);
+			voltage_tries = 0;
+			continue;
+		}
+
+		/* Check to see if we've tried the same voltage 5 times */
+		if ((intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK) == voltage) {
+			++voltage_tries;
+			if (voltage_tries == 5) {
+				DRM_ERROR("too many voltage retries, give up\n");
+				break;
+			}
+		} else
+			voltage_tries = 0;
+		voltage = intel_dp->train_set[0] & DP_TRAIN_VOLTAGE_SWING_MASK;
+
+		/* Update training set as requested by target */
+		intel_get_adjust_train(intel_dp, link_status);
+		if (!intel_dp_update_link_train(intel_dp)) {
+			DRM_ERROR("failed to update link training\n");
+			break;
+		}
+	}
+}
+
+/*
+ * Pick training pattern for channel equalization. Training Pattern 3 for HBR2
+ * or 1.2 devices that support it, Training Pattern 2 otherwise.
+ */
+static u32 intel_dp_training_pattern(struct intel_dp *intel_dp)
+{
+	u32 training_pattern = DP_TRAINING_PATTERN_2;
+	bool source_tps3, sink_tps3;
+
+	/*
+	 * 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);
+
+	if (source_tps3 && sink_tps3) {
+		training_pattern = DP_TRAINING_PATTERN_3;
+	} else if (intel_dp->link_rate == 540000) {
+		if (!source_tps3)
+			DRM_DEBUG_KMS("5.4 Gbps link rate without source HBR2/TPS3 support\n");
+		if (!sink_tps3)
+			DRM_DEBUG_KMS("5.4 Gbps link rate without sink TPS3 support\n");
+	}
+
+	return training_pattern;
+}
+
+static void
+intel_dp_link_training_channel_equalization(struct intel_dp *intel_dp)
+{
+	bool channel_eq = false;
+	int tries, cr_tries;
+	u32 training_pattern;
+
+	training_pattern = intel_dp_training_pattern(intel_dp);
+
+	/* channel equalization */
+	if (!intel_dp_set_link_train(intel_dp,
+				     training_pattern |
+				     DP_LINK_SCRAMBLING_DISABLE)) {
+		DRM_ERROR("failed to start channel equalization\n");
+		return;
+	}
+
+	tries = 0;
+	cr_tries = 0;
+	channel_eq = false;
+	for (;;) {
+		uint8_t link_status[DP_LINK_STATUS_SIZE];
+
+		if (cr_tries > 5) {
+			DRM_ERROR("failed to train DP, aborting\n");
+			break;
+		}
+
+		drm_dp_link_train_channel_eq_delay(intel_dp->dpcd);
+		if (!intel_dp_get_link_status(intel_dp, link_status)) {
+			DRM_ERROR("failed to get link status\n");
+			break;
+		}
+
+		/* Make sure clock is still ok */
+		if (!drm_dp_clock_recovery_ok(link_status,
+					      intel_dp->lane_count)) {
+			intel_dp->train_set_valid = false;
+			intel_dp_link_training_clock_recovery(intel_dp);
+			intel_dp_set_link_train(intel_dp,
+						training_pattern |
+						DP_LINK_SCRAMBLING_DISABLE);
+			cr_tries++;
+			continue;
+		}
+
+		if (drm_dp_channel_eq_ok(link_status,
+					 intel_dp->lane_count)) {
+			channel_eq = true;
+			break;
+		}
+
+		/* Try 5 times, then try clock recovery if that fails */
+		if (tries > 5) {
+			intel_dp->train_set_valid = false;
+			intel_dp_link_training_clock_recovery(intel_dp);
+			intel_dp_set_link_train(intel_dp,
+						training_pattern |
+						DP_LINK_SCRAMBLING_DISABLE);
+			tries = 0;
+			cr_tries++;
+			continue;
+		}
+
+		/* Update training set as requested by target */
+		intel_get_adjust_train(intel_dp, link_status);
+		if (!intel_dp_update_link_train(intel_dp)) {
+			DRM_ERROR("failed to update link training\n");
+			break;
+		}
+		++tries;
+	}
+
+	intel_dp_set_idle_link_train(intel_dp);
+
+	if (channel_eq) {
+		intel_dp->train_set_valid = true;
+		DRM_DEBUG_KMS("Channel EQ done. DP Training successful\n");
+	}
+}
+
+void intel_dp_stop_link_train(struct intel_dp *intel_dp)
+{
+	intel_dp_set_link_train(intel_dp,
+				DP_TRAINING_PATTERN_DISABLE);
+}
+
+void
+intel_dp_start_link_train(struct intel_dp *intel_dp)
+{
+	intel_dp_link_training_clock_recovery(intel_dp);
+	intel_dp_link_training_channel_equalization(intel_dp);
+}
diff --git a/drivers/gpu/drm/i915/intel_dp_mst.c b/drivers/gpu/drm/i915/intel_dp_mst.c
index 0639275fc471..fa0dabf578dc 100644
--- a/drivers/gpu/drm/i915/intel_dp_mst.c
+++ b/drivers/gpu/drm/i915/intel_dp_mst.c
@@ -78,6 +78,8 @@ static bool intel_dp_mst_compute_config(struct intel_encoder *encoder,
 		return false;
 	}
 
+	if (drm_dp_mst_port_has_audio(&intel_dp->mst_mgr, found->port))
+		pipe_config->has_audio = true;
 	mst_pbn = drm_dp_calc_pbn_mode(adjusted_mode->crtc_clock, bpp);
 
 	pipe_config->pbn = mst_pbn;
@@ -102,6 +104,11 @@ static void intel_mst_disable_dp(struct intel_encoder *encoder)
 	struct intel_dp_mst_encoder *intel_mst = enc_to_mst(&encoder->base);
 	struct intel_digital_port *intel_dig_port = intel_mst->primary;
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = encoder->base.crtc;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
 	int ret;
 
 	DRM_DEBUG_KMS("%d\n", intel_dp->active_mst_links);
@@ -112,6 +119,10 @@ static void intel_mst_disable_dp(struct intel_encoder *encoder)
 	if (ret) {
 		DRM_ERROR("failed to update payload %d\n", ret);
 	}
+	if (intel_crtc->config->has_audio) {
+		intel_audio_codec_disable(encoder);
+		intel_display_power_put(dev_priv, POWER_DOMAIN_AUDIO);
+	}
 }
 
 static void intel_mst_post_disable_dp(struct intel_encoder *encoder)
@@ -173,20 +184,14 @@ static void intel_mst_pre_enable_dp(struct intel_encoder *encoder)
 	intel_mst->port = found->port;
 
 	if (intel_dp->active_mst_links == 0) {
-		enum port port = intel_ddi_get_encoder_port(encoder);
+		intel_ddi_clk_select(encoder, intel_crtc->config);
 
 		intel_dp_set_link_params(intel_dp, intel_crtc->config);
 
-		/* FIXME: add support for SKL */
-		if (INTEL_INFO(dev)->gen < 9)
-			I915_WRITE(PORT_CLK_SEL(port),
-				   intel_crtc->config->ddi_pll_sel);
-
 		intel_ddi_init_dp_buf_reg(&intel_dig_port->base);
 
 		intel_dp_sink_dpms(intel_dp, DRM_MODE_DPMS_ON);
 
-
 		intel_dp_start_link_train(intel_dp);
 		intel_dp_stop_link_train(intel_dp);
 	}
@@ -214,6 +219,7 @@ static void intel_mst_enable_dp(struct intel_encoder *encoder)
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
 	enum port port = intel_dig_port->port;
 	int ret;
 
@@ -226,6 +232,13 @@ static void intel_mst_enable_dp(struct intel_encoder *encoder)
 	ret = drm_dp_check_act_status(&intel_dp->mst_mgr);
 
 	ret = drm_dp_update_payload_part2(&intel_dp->mst_mgr);
+
+	if (crtc->config->has_audio) {
+		DRM_DEBUG_DRIVER("Enabling DP audio on pipe %c\n",
+				 pipe_name(crtc->pipe));
+		intel_display_power_get(dev_priv, POWER_DOMAIN_AUDIO);
+		intel_audio_codec_enable(encoder);
+	}
 }
 
 static bool intel_dp_mst_enc_get_hw_state(struct intel_encoder *encoder,
@@ -251,6 +264,9 @@ static void intel_dp_mst_enc_get_config(struct intel_encoder *encoder,
 
 	pipe_config->has_dp_encoder = true;
 
+	pipe_config->has_audio =
+		intel_ddi_is_audio_enabled(dev_priv, crtc);
+
 	temp = I915_READ(TRANS_DDI_FUNC_CTL(cpu_transcoder));
 	if (temp & TRANS_DDI_PHSYNC)
 		flags |= DRM_MODE_FLAG_PHSYNC;
@@ -414,7 +430,10 @@ static void intel_connector_add_to_fbdev(struct intel_connector *connector)
 {
 #ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
-	drm_fb_helper_add_one_connector(&dev_priv->fbdev->helper, &connector->base);
+
+	if (dev_priv->fbdev)
+		drm_fb_helper_add_one_connector(&dev_priv->fbdev->helper,
+						&connector->base);
 #endif
 }
 
@@ -422,7 +441,10 @@ static void intel_connector_remove_from_fbdev(struct intel_connector *connector)
 {
 #ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
-	drm_fb_helper_remove_one_connector(&dev_priv->fbdev->helper, &connector->base);
+
+	if (dev_priv->fbdev)
+		drm_fb_helper_remove_one_connector(&dev_priv->fbdev->helper,
+						   &connector->base);
 #endif
 }
 
@@ -512,7 +534,7 @@ static void intel_dp_mst_hotplug(struct drm_dp_mst_topology_mgr *mgr)
 	drm_kms_helper_hotplug_event(dev);
 }
 
-static struct drm_dp_mst_topology_cbs mst_cbs = {
+static const struct drm_dp_mst_topology_cbs mst_cbs = {
 	.add_connector = intel_dp_add_mst_connector,
 	.register_connector = intel_dp_register_mst_connector,
 	.destroy_connector = intel_dp_destroy_mst_connector,
@@ -536,7 +558,7 @@ intel_dp_create_fake_mst_encoder(struct intel_digital_port *intel_dig_port, enum
 	intel_mst->primary = intel_dig_port;
 
 	drm_encoder_init(dev, &intel_encoder->base, &intel_dp_mst_enc_funcs,
-			 DRM_MODE_ENCODER_DPMST);
+			 DRM_MODE_ENCODER_DPMST, NULL);
 
 	intel_encoder->type = INTEL_OUTPUT_DP_MST;
 	intel_encoder->crtc_mask = 0x7;
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 0d00f07b7163..df7f3cb66056 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -123,8 +123,6 @@ struct intel_framebuffer {
 struct intel_fbdev {
 	struct drm_fb_helper helper;
 	struct intel_framebuffer *fb;
-	struct list_head fbdev_list;
-	struct drm_display_mode *our_mode;
 	int preferred_bpp;
 };
 
@@ -250,6 +248,7 @@ struct intel_atomic_state {
 	unsigned int cdclk;
 	bool dpll_set;
 	struct intel_shared_dpll_config shared_dpll[I915_NUM_PLLS];
+	struct intel_wm_config wm_config;
 };
 
 struct intel_plane_state {
@@ -280,6 +279,9 @@ struct intel_plane_state {
 	int scaler_id;
 
 	struct drm_intel_sprite_colorkey ckey;
+
+	/* async flip related structures */
+	struct drm_i915_gem_request *wait_req;
 };
 
 struct intel_initial_plane_config {
@@ -334,6 +336,21 @@ struct intel_crtc_scaler_state {
 /* drm_mode->private_flags */
 #define I915_MODE_FLAG_INHERITED 1
 
+struct intel_pipe_wm {
+	struct intel_wm_level wm[5];
+	uint32_t linetime;
+	bool fbc_wm_enabled;
+	bool pipe_enabled;
+	bool sprites_enabled;
+	bool sprites_scaled;
+};
+
+struct skl_pipe_wm {
+	struct skl_wm_level wm[8];
+	struct skl_wm_level trans_wm;
+	uint32_t linetime;
+};
+
 struct intel_crtc_state {
 	struct drm_crtc_state base;
 
@@ -348,7 +365,9 @@ struct intel_crtc_state {
 #define PIPE_CONFIG_QUIRK_MODE_SYNC_FLAGS	(1<<0) /* unreliable sync mode.flags */
 	unsigned long quirks;
 
-	bool update_pipe;
+	bool update_pipe; /* can a fast modeset be performed? */
+	bool disable_cxsr;
+	bool wm_changed; /* watermarks are updated */
 
 	/* Pipe source size (ie. panel fitter input size)
 	 * All planes will be positioned inside this space,
@@ -376,6 +395,9 @@ struct intel_crtc_state {
 	 * accordingly. */
 	bool has_dp_encoder;
 
+	/* DSI has special cases */
+	bool has_dsi_encoder;
+
 	/* Whether we should send NULL infoframes. Required for audio. */
 	bool has_hdmi_sink;
 
@@ -468,6 +490,20 @@ struct intel_crtc_state {
 
 	/* w/a for waiting 2 vblanks during crtc enable */
 	enum pipe hsw_workaround_pipe;
+
+	/* IVB sprite scaling w/a (WaCxSRDisabledForSpriteScaling:ivb) */
+	bool disable_lp_wm;
+
+	struct {
+		/*
+		 * optimal watermarks, programmed post-vblank when this state
+		 * is committed
+		 */
+		union {
+			struct intel_pipe_wm ilk;
+			struct skl_pipe_wm skl;
+		} optimal;
+	} wm;
 };
 
 struct vlv_wm_state {
@@ -479,26 +515,12 @@ struct vlv_wm_state {
 	bool cxsr;
 };
 
-struct intel_pipe_wm {
-	struct intel_wm_level wm[5];
-	uint32_t linetime;
-	bool fbc_wm_enabled;
-	bool pipe_enabled;
-	bool sprites_enabled;
-	bool sprites_scaled;
-};
-
 struct intel_mmio_flip {
 	struct work_struct work;
 	struct drm_i915_private *i915;
 	struct drm_i915_gem_request *req;
 	struct intel_crtc *crtc;
-};
-
-struct skl_pipe_wm {
-	struct skl_wm_level wm[8];
-	struct skl_wm_level trans_wm;
-	uint32_t linetime;
+	unsigned int rotation;
 };
 
 /*
@@ -509,13 +531,9 @@ struct skl_pipe_wm {
  */
 struct intel_crtc_atomic_commit {
 	/* Sleepable operations to perform before commit */
-	bool wait_for_flips;
 	bool disable_fbc;
 	bool disable_ips;
-	bool disable_cxsr;
 	bool pre_disable_primary;
-	bool update_wm_pre, update_wm_post;
-	unsigned disabled_planes;
 
 	/* Sleepable operations to perform after commit */
 	unsigned fb_bits;
@@ -567,9 +585,10 @@ struct intel_crtc {
 	/* per-pipe watermark state */
 	struct {
 		/* watermarks currently being used  */
-		struct intel_pipe_wm active;
-		/* SKL wm values currently in use */
-		struct skl_pipe_wm skl_active;
+		union {
+			struct intel_pipe_wm ilk;
+			struct skl_pipe_wm skl;
+		} active;
 		/* allow CxSR on this pipe */
 		bool cxsr_allowed;
 	} wm;
@@ -677,7 +696,7 @@ struct cxsr_latency {
 #define intel_fb_obj(x) (x ? to_intel_framebuffer(x)->obj : NULL)
 
 struct intel_hdmi {
-	u32 hdmi_reg;
+	i915_reg_t hdmi_reg;
 	int ddc_bus;
 	bool limited_color_range;
 	bool color_range_auto;
@@ -693,7 +712,8 @@ struct intel_hdmi {
 	void (*set_infoframes)(struct drm_encoder *encoder,
 			       bool enable,
 			       const struct drm_display_mode *adjusted_mode);
-	bool (*infoframe_enabled)(struct drm_encoder *encoder);
+	bool (*infoframe_enabled)(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config);
 };
 
 struct intel_dp_mst_encoder;
@@ -719,15 +739,10 @@ enum link_m_n_set {
 	M2_N2
 };
 
-struct sink_crc {
-	bool started;
-	u8 last_crc[6];
-	int last_count;
-};
-
 struct intel_dp {
-	uint32_t output_reg;
-	uint32_t aux_ch_ctl_reg;
+	i915_reg_t output_reg;
+	i915_reg_t aux_ch_ctl_reg;
+	i915_reg_t aux_ch_data_reg[5];
 	uint32_t DP;
 	int link_rate;
 	uint8_t lane_count;
@@ -741,7 +756,6 @@ struct intel_dp {
 	/* sink rates as reported by DP_SUPPORTED_LINK_RATES */
 	uint8_t num_sink_rates;
 	int sink_rates[DP_MAX_SUPPORTED_RATES];
-	struct sink_crc sink_crc;
 	struct drm_dp_aux aux;
 	uint8_t train_set[4];
 	int panel_power_up_delay;
@@ -783,6 +797,10 @@ struct intel_dp {
 				     bool has_aux_irq,
 				     int send_bytes,
 				     uint32_t aux_clock_divider);
+
+	/* This is called before a link training is starterd */
+	void (*prepare_link_retrain)(struct intel_dp *intel_dp);
+
 	bool train_set_valid;
 
 	/* Displayport compliance testing */
@@ -799,6 +817,8 @@ struct intel_digital_port {
 	struct intel_hdmi hdmi;
 	enum irqreturn (*hpd_pulse)(struct intel_digital_port *, bool);
 	bool release_cl2_override;
+	/* for communication with audio component; protected by av_mutex */
+	const struct drm_connector *audio_connector;
 };
 
 struct intel_dp_mst_encoder {
@@ -942,7 +962,8 @@ void intel_cpu_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
 					 enum pipe pipe);
 void intel_pch_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
 					 enum transcoder pch_transcoder);
-void i9xx_check_fifo_underruns(struct drm_i915_private *dev_priv);
+void intel_check_cpu_fifo_underruns(struct drm_i915_private *dev_priv);
+void intel_check_pch_fifo_underruns(struct drm_i915_private *dev_priv);
 
 /* i915_irq.c */
 void gen5_enable_gt_irq(struct drm_i915_private *dev_priv, uint32_t mask);
@@ -973,6 +994,8 @@ void intel_crt_init(struct drm_device *dev);
 
 
 /* intel_ddi.c */
+void intel_ddi_clk_select(struct intel_encoder *encoder,
+			  const struct intel_crtc_state *pipe_config);
 void intel_prepare_ddi(struct drm_device *dev);
 void hsw_fdi_link_train(struct drm_crtc *crtc);
 void intel_ddi_init(struct drm_device *dev, enum port port);
@@ -987,9 +1010,11 @@ void intel_ddi_disable_pipe_clock(struct intel_crtc *intel_crtc);
 bool intel_ddi_pll_select(struct intel_crtc *crtc,
 			  struct intel_crtc_state *crtc_state);
 void intel_ddi_set_pipe_settings(struct drm_crtc *crtc);
-void intel_ddi_prepare_link_retrain(struct drm_encoder *encoder);
+void intel_ddi_prepare_link_retrain(struct intel_dp *intel_dp);
 bool intel_ddi_connector_get_hw_state(struct intel_connector *intel_connector);
 void intel_ddi_fdi_disable(struct drm_crtc *crtc);
+bool intel_ddi_is_audio_enabled(struct drm_i915_private *dev_priv,
+				 struct intel_crtc *intel_crtc);
 void intel_ddi_get_config(struct intel_encoder *encoder,
 			  struct intel_crtc_state *pipe_config);
 struct intel_encoder *
@@ -1055,6 +1080,15 @@ intel_wait_for_vblank(struct drm_device *dev, int pipe)
 {
 	drm_wait_one_vblank(dev, pipe);
 }
+static inline void
+intel_wait_for_vblank_if_active(struct drm_device *dev, int pipe)
+{
+	const struct intel_crtc *crtc =
+		to_intel_crtc(intel_get_crtc_for_pipe(dev, pipe));
+
+	if (crtc->active)
+		intel_wait_for_vblank(dev, pipe);
+}
 int ironlake_get_lanes_required(int target_clock, int link_bw, int bpp);
 void vlv_wait_port_ready(struct drm_i915_private *dev_priv,
 			 struct intel_digital_port *dport,
@@ -1068,9 +1102,7 @@ void intel_release_load_detect_pipe(struct drm_connector *connector,
 				    struct drm_modeset_acquire_ctx *ctx);
 int intel_pin_and_fence_fb_obj(struct drm_plane *plane,
 			       struct drm_framebuffer *fb,
-			       const struct drm_plane_state *plane_state,
-			       struct intel_engine_cs *pipelined,
-			       struct drm_i915_gem_request **pipelined_request);
+			       const struct drm_plane_state *plane_state);
 struct drm_framebuffer *
 __intel_framebuffer_create(struct drm_device *dev,
 			   struct drm_mode_fb_cmd2 *mode_cmd,
@@ -1151,7 +1183,10 @@ void broxton_ddi_phy_uninit(struct drm_device *dev);
 void bxt_enable_dc9(struct drm_i915_private *dev_priv);
 void bxt_disable_dc9(struct drm_i915_private *dev_priv);
 void skl_init_cdclk(struct drm_i915_private *dev_priv);
+int skl_sanitize_cdclk(struct drm_i915_private *dev_priv);
 void skl_uninit_cdclk(struct drm_i915_private *dev_priv);
+void skl_enable_dc6(struct drm_i915_private *dev_priv);
+void skl_disable_dc6(struct drm_i915_private *dev_priv);
 void intel_dp_get_m_n(struct intel_crtc *crtc,
 		      struct intel_crtc_state *pipe_config);
 void intel_dp_set_m_n(struct intel_crtc *crtc, enum link_m_n_set m_n);
@@ -1172,31 +1207,26 @@ enum intel_display_power_domain
 intel_display_port_aux_power_domain(struct intel_encoder *intel_encoder);
 void intel_mode_from_pipe_config(struct drm_display_mode *mode,
 				 struct intel_crtc_state *pipe_config);
-void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc);
 void intel_modeset_preclose(struct drm_device *dev, struct drm_file *file);
 
 int skl_update_scaler_crtc(struct intel_crtc_state *crtc_state);
 int skl_max_scale(struct intel_crtc *crtc, struct intel_crtc_state *crtc_state);
 
-unsigned long intel_plane_obj_offset(struct intel_plane *intel_plane,
-				     struct drm_i915_gem_object *obj,
-				     unsigned int plane);
+u32 intel_plane_obj_offset(struct intel_plane *intel_plane,
+			   struct drm_i915_gem_object *obj,
+			   unsigned int plane);
 
 u32 skl_plane_ctl_format(uint32_t pixel_format);
 u32 skl_plane_ctl_tiling(uint64_t fb_modifier);
 u32 skl_plane_ctl_rotation(unsigned int rotation);
 
 /* intel_csr.c */
-void intel_csr_ucode_init(struct drm_device *dev);
-enum csr_state intel_csr_load_status_get(struct drm_i915_private *dev_priv);
-void intel_csr_load_status_set(struct drm_i915_private *dev_priv,
-					enum csr_state state);
-void intel_csr_load_program(struct drm_device *dev);
-void intel_csr_ucode_fini(struct drm_device *dev);
-void assert_csr_loaded(struct drm_i915_private *dev_priv);
+void intel_csr_ucode_init(struct drm_i915_private *);
+void intel_csr_load_program(struct drm_i915_private *);
+void intel_csr_ucode_fini(struct drm_i915_private *);
 
 /* intel_dp.c */
-void intel_dp_init(struct drm_device *dev, int output_reg, enum port port);
+void intel_dp_init(struct drm_device *dev, i915_reg_t output_reg, enum port port);
 bool intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 			     struct intel_connector *intel_connector);
 void intel_dp_set_link_params(struct intel_dp *intel_dp,
@@ -1234,6 +1264,22 @@ bool intel_digital_port_connected(struct drm_i915_private *dev_priv,
 					 struct intel_digital_port *port);
 void hsw_dp_set_ddi_pll_sel(struct intel_crtc_state *pipe_config);
 
+void
+intel_dp_program_link_training_pattern(struct intel_dp *intel_dp,
+				       uint8_t dp_train_pat);
+void
+intel_dp_set_signal_levels(struct intel_dp *intel_dp);
+void intel_dp_set_idle_link_train(struct intel_dp *intel_dp);
+uint8_t
+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);
+void intel_dp_compute_rate(struct intel_dp *intel_dp, int port_clock,
+			   uint8_t *link_bw, uint8_t *rate_select);
+bool intel_dp_source_supports_hbr2(struct intel_dp *intel_dp);
+bool
+intel_dp_get_link_status(struct intel_dp *intel_dp, uint8_t link_status[DP_LINK_STATUS_SIZE]);
+
 /* intel_dp_mst.c */
 int intel_dp_mst_encoder_init(struct intel_digital_port *intel_dig_port, int conn_id);
 void intel_dp_mst_encoder_cleanup(struct intel_digital_port *intel_dig_port);
@@ -1248,7 +1294,7 @@ void intel_dvo_init(struct drm_device *dev);
 /* legacy fbdev emulation in intel_fbdev.c */
 #ifdef CONFIG_DRM_FBDEV_EMULATION
 extern int intel_fbdev_init(struct drm_device *dev);
-extern void intel_fbdev_initial_config(void *data, async_cookie_t cookie);
+extern void intel_fbdev_initial_config_async(struct drm_device *dev);
 extern void intel_fbdev_fini(struct drm_device *dev);
 extern void intel_fbdev_set_suspend(struct drm_device *dev, int state, bool synchronous);
 extern void intel_fbdev_output_poll_changed(struct drm_device *dev);
@@ -1259,7 +1305,7 @@ static inline int intel_fbdev_init(struct drm_device *dev)
 	return 0;
 }
 
-static inline void intel_fbdev_initial_config(void *data, async_cookie_t cookie)
+static inline void intel_fbdev_initial_config_async(struct drm_device *dev)
 {
 }
 
@@ -1277,9 +1323,11 @@ static inline void intel_fbdev_restore_mode(struct drm_device *dev)
 #endif
 
 /* intel_fbc.c */
-bool intel_fbc_enabled(struct drm_i915_private *dev_priv);
-void intel_fbc_update(struct drm_i915_private *dev_priv);
+bool intel_fbc_is_active(struct drm_i915_private *dev_priv);
+void intel_fbc_deactivate(struct intel_crtc *crtc);
+void intel_fbc_update(struct intel_crtc *crtc);
 void intel_fbc_init(struct drm_i915_private *dev_priv);
+void intel_fbc_enable(struct intel_crtc *crtc);
 void intel_fbc_disable(struct drm_i915_private *dev_priv);
 void intel_fbc_disable_crtc(struct intel_crtc *crtc);
 void intel_fbc_invalidate(struct drm_i915_private *dev_priv,
@@ -1287,11 +1335,10 @@ void intel_fbc_invalidate(struct drm_i915_private *dev_priv,
 			  enum fb_op_origin origin);
 void intel_fbc_flush(struct drm_i915_private *dev_priv,
 		     unsigned int frontbuffer_bits, enum fb_op_origin origin);
-const char *intel_no_fbc_reason_str(enum no_fbc_reason reason);
 void intel_fbc_cleanup_cfb(struct drm_i915_private *dev_priv);
 
 /* intel_hdmi.c */
-void intel_hdmi_init(struct drm_device *dev, int hdmi_reg, enum port port);
+void intel_hdmi_init(struct drm_device *dev, i915_reg_t hdmi_reg, enum port port);
 void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 			       struct intel_connector *intel_connector);
 struct intel_hdmi *enc_to_intel_hdmi(struct drm_encoder *encoder);
@@ -1367,8 +1414,13 @@ void intel_psr_single_frame_update(struct drm_device *dev,
 /* intel_runtime_pm.c */
 int intel_power_domains_init(struct drm_i915_private *);
 void intel_power_domains_fini(struct drm_i915_private *);
-void intel_power_domains_init_hw(struct drm_i915_private *dev_priv);
+void intel_power_domains_init_hw(struct drm_i915_private *dev_priv, bool resume);
+void intel_power_domains_suspend(struct drm_i915_private *dev_priv);
+void skl_pw1_misc_io_init(struct drm_i915_private *dev_priv);
+void skl_pw1_misc_io_fini(struct drm_i915_private *dev_priv);
 void intel_runtime_pm_enable(struct drm_i915_private *dev_priv);
+const char *
+intel_display_power_domain_str(enum intel_display_power_domain domain);
 
 bool intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
 				    enum intel_display_power_domain domain);
@@ -1376,9 +1428,95 @@ bool __intel_display_power_is_enabled(struct drm_i915_private *dev_priv,
 				      enum intel_display_power_domain domain);
 void intel_display_power_get(struct drm_i915_private *dev_priv,
 			     enum intel_display_power_domain domain);
+bool intel_display_power_get_if_enabled(struct drm_i915_private *dev_priv,
+					enum intel_display_power_domain domain);
 void intel_display_power_put(struct drm_i915_private *dev_priv,
 			     enum intel_display_power_domain domain);
+
+static inline void
+assert_rpm_device_not_suspended(struct drm_i915_private *dev_priv)
+{
+	WARN_ONCE(dev_priv->pm.suspended,
+		  "Device suspended during HW access\n");
+}
+
+static inline void
+assert_rpm_wakelock_held(struct drm_i915_private *dev_priv)
+{
+	assert_rpm_device_not_suspended(dev_priv);
+	/* FIXME: Needs to be converted back to WARN_ONCE, but currently causes
+	 * too much noise. */
+	if (!atomic_read(&dev_priv->pm.wakeref_count))
+		DRM_DEBUG_DRIVER("RPM wakelock ref not held during HW access");
+}
+
+static inline int
+assert_rpm_atomic_begin(struct drm_i915_private *dev_priv)
+{
+	int seq = atomic_read(&dev_priv->pm.atomic_seq);
+
+	assert_rpm_wakelock_held(dev_priv);
+
+	return seq;
+}
+
+static inline void
+assert_rpm_atomic_end(struct drm_i915_private *dev_priv, int begin_seq)
+{
+	WARN_ONCE(atomic_read(&dev_priv->pm.atomic_seq) != begin_seq,
+		  "HW access outside of RPM atomic section\n");
+}
+
+/**
+ * disable_rpm_wakeref_asserts - disable the RPM assert checks
+ * @dev_priv: i915 device instance
+ *
+ * This function disable asserts that check if we hold an RPM wakelock
+ * reference, while keeping the device-not-suspended checks still enabled.
+ * It's meant to be used only in special circumstances where our rule about
+ * the wakelock refcount wrt. the device power state doesn't hold. According
+ * to this rule at any point where we access the HW or want to keep the HW in
+ * an active state we must hold an RPM wakelock reference acquired via one of
+ * the intel_runtime_pm_get() helpers. Currently there are a few special spots
+ * where this rule doesn't hold: the IRQ and suspend/resume handlers, the
+ * forcewake release timer, and the GPU RPS and hangcheck works. All other
+ * users should avoid using this function.
+ *
+ * Any calls to this function must have a symmetric call to
+ * enable_rpm_wakeref_asserts().
+ */
+static inline void
+disable_rpm_wakeref_asserts(struct drm_i915_private *dev_priv)
+{
+	atomic_inc(&dev_priv->pm.wakeref_count);
+}
+
+/**
+ * enable_rpm_wakeref_asserts - re-enable the RPM assert checks
+ * @dev_priv: i915 device instance
+ *
+ * This function re-enables the RPM assert checks after disabling them with
+ * disable_rpm_wakeref_asserts. It's meant to be used only in special
+ * circumstances otherwise its use should be avoided.
+ *
+ * Any calls to this function must have a symmetric call to
+ * disable_rpm_wakeref_asserts().
+ */
+static inline void
+enable_rpm_wakeref_asserts(struct drm_i915_private *dev_priv)
+{
+	atomic_dec(&dev_priv->pm.wakeref_count);
+}
+
+/* TODO: convert users of these to rely instead on proper RPM refcounting */
+#define DISABLE_RPM_WAKEREF_ASSERTS(dev_priv)	\
+	disable_rpm_wakeref_asserts(dev_priv)
+
+#define ENABLE_RPM_WAKEREF_ASSERTS(dev_priv)	\
+	enable_rpm_wakeref_asserts(dev_priv)
+
 void intel_runtime_pm_get(struct drm_i915_private *dev_priv);
+bool intel_runtime_pm_get_if_in_use(struct drm_i915_private *dev_priv);
 void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv);
 void intel_runtime_pm_put(struct drm_i915_private *dev_priv);
 
@@ -1395,12 +1533,6 @@ 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);
 void intel_update_watermarks(struct drm_crtc *crtc);
-void intel_update_sprite_watermarks(struct drm_plane *plane,
-				    struct drm_crtc *crtc,
-				    uint32_t sprite_width,
-				    uint32_t sprite_height,
-				    int pixel_size,
-				    bool enabled, bool scaled);
 void intel_init_pm(struct drm_device *dev);
 void intel_pm_setup(struct drm_device *dev);
 void intel_gpu_ips_init(struct drm_i915_private *dev_priv);
@@ -1428,7 +1560,8 @@ void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
 uint32_t ilk_pipe_pixel_rate(const struct intel_crtc_state *pipe_config);
 
 /* intel_sdvo.c */
-bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob);
+bool intel_sdvo_init(struct drm_device *dev,
+		     i915_reg_t reg, enum port port);
 
 
 /* intel_sprite.c */
diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index 170ae6f4866e..0193c62a53ef 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -60,7 +60,8 @@ static void wait_for_dsi_fifo_empty(struct intel_dsi *intel_dsi, enum port port)
 		DRM_ERROR("DPI FIFOs are not empty\n");
 }
 
-static void write_data(struct drm_i915_private *dev_priv, u32 reg,
+static void write_data(struct drm_i915_private *dev_priv,
+		       i915_reg_t reg,
 		       const u8 *data, u32 len)
 {
 	u32 i, j;
@@ -75,7 +76,8 @@ static void write_data(struct drm_i915_private *dev_priv, u32 reg,
 	}
 }
 
-static void read_data(struct drm_i915_private *dev_priv, u32 reg,
+static void read_data(struct drm_i915_private *dev_priv,
+		      i915_reg_t reg,
 		      u8 *data, u32 len)
 {
 	u32 i, j;
@@ -98,7 +100,8 @@ static ssize_t intel_dsi_host_transfer(struct mipi_dsi_host *host,
 	struct mipi_dsi_packet packet;
 	ssize_t ret;
 	const u8 *header, *data;
-	u32 data_reg, data_mask, ctrl_reg, ctrl_mask;
+	i915_reg_t data_reg, ctrl_reg;
+	u32 data_mask, ctrl_mask;
 
 	ret = mipi_dsi_create_packet(&packet, msg);
 	if (ret < 0)
@@ -263,16 +266,18 @@ static inline bool is_cmd_mode(struct intel_dsi *intel_dsi)
 }
 
 static bool intel_dsi_compute_config(struct intel_encoder *encoder,
-				     struct intel_crtc_state *config)
+				     struct intel_crtc_state *pipe_config)
 {
 	struct intel_dsi *intel_dsi = container_of(encoder, struct intel_dsi,
 						   base);
 	struct intel_connector *intel_connector = intel_dsi->attached_connector;
 	struct drm_display_mode *fixed_mode = intel_connector->panel.fixed_mode;
-	struct drm_display_mode *adjusted_mode = &config->base.adjusted_mode;
+	struct drm_display_mode *adjusted_mode = &pipe_config->base.adjusted_mode;
 
 	DRM_DEBUG_KMS("\n");
 
+	pipe_config->has_dsi_encoder = true;
+
 	if (fixed_mode)
 		intel_fixed_panel_mode(fixed_mode, adjusted_mode);
 
@@ -364,7 +369,7 @@ static void intel_dsi_device_ready(struct intel_encoder *encoder)
 {
 	struct drm_device *dev = encoder->base.dev;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		vlv_dsi_device_ready(encoder);
 	else if (IS_BROXTON(dev))
 		bxt_dsi_device_ready(encoder);
@@ -377,10 +382,10 @@ static void intel_dsi_port_enable(struct intel_encoder *encoder)
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	enum port port;
-	u32 temp;
-	u32 port_ctrl;
 
 	if (intel_dsi->dual_link == DSI_DUAL_LINK_FRONT_BACK) {
+		u32 temp;
+
 		temp = I915_READ(VLV_CHICKEN_3);
 		temp &= ~PIXEL_OVERLAP_CNT_MASK |
 					intel_dsi->pixel_overlap <<
@@ -389,8 +394,9 @@ static void intel_dsi_port_enable(struct intel_encoder *encoder)
 	}
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		port_ctrl = IS_BROXTON(dev) ? BXT_MIPI_PORT_CTRL(port) :
-						MIPI_PORT_CTRL(port);
+		i915_reg_t port_ctrl = IS_BROXTON(dev) ?
+			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
+		u32 temp;
 
 		temp = I915_READ(port_ctrl);
 
@@ -416,13 +422,13 @@ static void intel_dsi_port_disable(struct intel_encoder *encoder)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	enum port port;
-	u32 temp;
-	u32 port_ctrl;
 
 	for_each_dsi_port(port, intel_dsi->ports) {
+		i915_reg_t port_ctrl = IS_BROXTON(dev) ?
+			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
+		u32 temp;
+
 		/* de-assert ip_tg_enable signal */
-		port_ctrl = IS_BROXTON(dev) ? BXT_MIPI_PORT_CTRL(port) :
-						MIPI_PORT_CTRL(port);
 		temp = I915_READ(port_ctrl);
 		I915_WRITE(port_ctrl, temp & ~DPI_ENABLE);
 		POSTING_READ(port_ctrl);
@@ -458,6 +464,8 @@ static void intel_dsi_enable(struct intel_encoder *encoder)
 	intel_panel_enable_backlight(intel_dsi->attached_connector);
 }
 
+static void intel_dsi_prepare(struct intel_encoder *intel_encoder);
+
 static void intel_dsi_pre_enable(struct intel_encoder *encoder)
 {
 	struct drm_device *dev = encoder->base.dev;
@@ -470,13 +478,16 @@ static void intel_dsi_pre_enable(struct intel_encoder *encoder)
 
 	DRM_DEBUG_KMS("\n");
 
+	intel_dsi_prepare(encoder);
+	intel_enable_dsi_pll(encoder);
+
 	/* Panel Enable over CRC PMIC */
 	if (intel_dsi->gpio_panel)
 		gpiod_set_value_cansleep(intel_dsi->gpio_panel, 1);
 
 	msleep(intel_dsi->panel_on_delay);
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		/*
 		 * Disable DPOunit clock gating, can stall pipe
 		 * and we need DPLL REFA always enabled
@@ -580,11 +591,13 @@ static void intel_dsi_clear_device_ready(struct intel_encoder *encoder)
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	enum port port;
-	u32 val;
-	u32 port_ctrl = 0;
 
 	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) ?
+			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(PORT_A);
+		u32 val;
 
 		I915_WRITE(MIPI_DEVICE_READY(port), DEVICE_READY |
 							ULPS_STATE_ENTER);
@@ -598,12 +611,6 @@ static void intel_dsi_clear_device_ready(struct intel_encoder *encoder)
 							ULPS_STATE_ENTER);
 		usleep_range(2000, 2500);
 
-		if (IS_BROXTON(dev))
-			port_ctrl = BXT_MIPI_PORT_CTRL(port);
-		else if (IS_VALLEYVIEW(dev))
-			/* Common bit for both MIPI Port A & MIPI Port C */
-			port_ctrl = MIPI_PORT_CTRL(PORT_A);
-
 		/* Wait till Clock lanes are in LP-00 state for MIPI Port A
 		 * only. MIPI Port C has no similar bit for checking
 		 */
@@ -656,40 +663,47 @@ static bool intel_dsi_get_hw_state(struct intel_encoder *encoder,
 	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;
-	u32 dpi_enabled, func, ctrl_reg;
 	enum port port;
+	bool ret;
 
 	DRM_DEBUG_KMS("\n");
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	/* XXX: this only works for one DSI output */
 	for_each_dsi_port(port, intel_dsi->ports) {
+		i915_reg_t ctrl_reg = IS_BROXTON(dev) ?
+			BXT_MIPI_PORT_CTRL(port) : MIPI_PORT_CTRL(port);
+		u32 dpi_enabled, func;
+
 		func = I915_READ(MIPI_DSI_FUNC_PRG(port));
-		ctrl_reg = IS_BROXTON(dev) ? BXT_MIPI_PORT_CTRL(port) :
-						MIPI_PORT_CTRL(port);
 		dpi_enabled = I915_READ(ctrl_reg) & DPI_ENABLE;
 
 		/* Due to some hardware limitations on BYT, MIPI Port C DPI
 		 * Enable bit does not get set. To check whether DSI Port C
 		 * was enabled in BIOS, check the Pipe B enable bit
 		 */
-		if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
-		    (port == PORT_C))
+		if (IS_VALLEYVIEW(dev) && port == PORT_C)
 			dpi_enabled = I915_READ(PIPECONF(PIPE_B)) &
 							PIPECONF_ENABLE;
 
 		if (dpi_enabled || (func & CMD_MODE_DATA_WIDTH_MASK)) {
 			if (I915_READ(MIPI_DEVICE_READY(port)) & DEVICE_READY) {
 				*pipe = port == PORT_A ? PIPE_A : PIPE_B;
-				return true;
+				ret = true;
+
+				goto out;
 			}
 		}
 	}
+out:
+	intel_display_power_put(dev_priv, power_domain);
 
-	return false;
+	return ret;
 }
 
 static void intel_dsi_get_config(struct intel_encoder *encoder,
@@ -698,6 +712,8 @@ static void intel_dsi_get_config(struct intel_encoder *encoder,
 	u32 pclk = 0;
 	DRM_DEBUG_KMS("\n");
 
+	pipe_config->has_dsi_encoder = true;
+
 	/*
 	 * DPLL_MD is not used in case of DSI, reading will get some default value
 	 * set dpll_md = 0
@@ -706,7 +722,8 @@ static void intel_dsi_get_config(struct intel_encoder *encoder,
 
 	if (IS_BROXTON(encoder->base.dev))
 		pclk = bxt_get_dsi_pclk(encoder, pipe_config->pipe_bpp);
-	else if (IS_VALLEYVIEW(encoder->base.dev))
+	else if (IS_VALLEYVIEW(encoder->base.dev) ||
+		 IS_CHERRYVIEW(encoder->base.dev))
 		pclk = vlv_get_dsi_pclk(encoder, pipe_config->pipe_bpp);
 
 	if (!pclk)
@@ -859,7 +876,7 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 	}
 
 	for_each_dsi_port(port, intel_dsi->ports) {
-		if (IS_VALLEYVIEW(dev)) {
+		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 			/*
 			 * escape clock divider, 20MHz, shared for A and C.
 			 * device ready must be off when doing this! txclkesc?
@@ -875,21 +892,12 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 			I915_WRITE(MIPI_CTRL(port), tmp |
 					READ_REQUEST_PRIORITY_HIGH);
 		} else if (IS_BROXTON(dev)) {
-			/*
-			 * FIXME:
-			 * BXT can connect any PIPE to any MIPI port.
-			 * Select the pipe based on the MIPI port read from
-			 * VBT for now. Pick PIPE A for MIPI port A and C
-			 * for port C.
-			 */
+			enum pipe pipe = intel_crtc->pipe;
+
 			tmp = I915_READ(MIPI_CTRL(port));
 			tmp &= ~BXT_PIPE_SELECT_MASK;
 
-			if (port == PORT_A)
-				tmp |= BXT_PIPE_SELECT_A;
-			else if (port == PORT_C)
-				tmp |= BXT_PIPE_SELECT_C;
-
+			tmp |= BXT_PIPE_SELECT(pipe);
 			I915_WRITE(MIPI_CTRL(port), tmp);
 		}
 
@@ -1025,15 +1033,6 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 	}
 }
 
-static void intel_dsi_pre_pll_enable(struct intel_encoder *encoder)
-{
-	DRM_DEBUG_KMS("\n");
-
-	intel_dsi_prepare(encoder);
-	intel_enable_dsi_pll(encoder);
-
-}
-
 static enum drm_connector_status
 intel_dsi_detect(struct drm_connector *connector, bool force)
 {
@@ -1128,7 +1127,7 @@ void intel_dsi_init(struct drm_device *dev)
 	if (!dev_priv->vbt.has_mipi)
 		return;
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		dev_priv->mipi_mmio_base = VLV_MIPI_BASE;
 	} else {
 		DRM_ERROR("Unsupported Mipi device to reg base");
@@ -1151,11 +1150,10 @@ void intel_dsi_init(struct drm_device *dev)
 
 	connector = &intel_connector->base;
 
-	drm_encoder_init(dev, encoder, &intel_dsi_funcs, DRM_MODE_ENCODER_DSI);
+	drm_encoder_init(dev, encoder, &intel_dsi_funcs, DRM_MODE_ENCODER_DSI,
+			 NULL);
 
-	/* XXX: very likely not all of these are needed */
 	intel_encoder->compute_config = intel_dsi_compute_config;
-	intel_encoder->pre_pll_enable = intel_dsi_pre_pll_enable;
 	intel_encoder->pre_enable = intel_dsi_pre_enable;
 	intel_encoder->enable = intel_dsi_enable_nop;
 	intel_encoder->disable = intel_dsi_pre_disable;
diff --git a/drivers/gpu/drm/i915/intel_dsi.h b/drivers/gpu/drm/i915/intel_dsi.h
index e6cb25239941..02551ff228c2 100644
--- a/drivers/gpu/drm/i915/intel_dsi.h
+++ b/drivers/gpu/drm/i915/intel_dsi.h
@@ -117,7 +117,7 @@ static inline struct intel_dsi_host *to_intel_dsi_host(struct mipi_dsi_host *h)
 
 #define for_each_dsi_port(__port, __ports_mask) \
 	for ((__port) = PORT_A; (__port) < I915_MAX_PORTS; (__port)++)	\
-		if ((__ports_mask) & (1 << (__port)))
+		for_each_if ((__ports_mask) & (1 << (__port)))
 
 static inline struct intel_dsi *enc_to_intel_dsi(struct drm_encoder *encoder)
 {
diff --git a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
index a5e99ac305da..e8113ad65477 100644
--- a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
+++ b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
@@ -204,10 +204,28 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 	struct drm_device *dev = intel_dsi->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	if (dev_priv->vbt.dsi.seq_version >= 3)
+		data++;
+
 	gpio = *data++;
 
 	/* pull up/down */
-	action = *data++;
+	action = *data++ & 1;
+
+	if (gpio >= ARRAY_SIZE(gtable)) {
+		DRM_DEBUG_KMS("unknown gpio %u\n", gpio);
+		goto out;
+	}
+
+	if (!IS_VALLEYVIEW(dev_priv)) {
+		DRM_DEBUG_KMS("GPIO element not supported on this platform\n");
+		goto out;
+	}
+
+	if (dev_priv->vbt.dsi.seq_version >= 3) {
+		DRM_DEBUG_KMS("GPIO element v3 not supported\n");
+		goto out;
+	}
 
 	function = gtable[gpio].function_reg;
 	pad = gtable[gpio].pad_reg;
@@ -226,6 +244,7 @@ static const u8 *mipi_exec_gpio(struct intel_dsi *intel_dsi, const u8 *data)
 	vlv_gpio_nc_write(dev_priv, pad, val);
 	mutex_unlock(&dev_priv->sb_lock);
 
+out:
 	return data;
 }
 
diff --git a/drivers/gpu/drm/i915/intel_dsi_pll.c b/drivers/gpu/drm/i915/intel_dsi_pll.c
index cb3cf3986212..fbd2b51810ca 100644
--- a/drivers/gpu/drm/i915/intel_dsi_pll.c
+++ b/drivers/gpu/drm/i915/intel_dsi_pll.c
@@ -561,7 +561,7 @@ void intel_enable_dsi_pll(struct intel_encoder *encoder)
 {
 	struct drm_device *dev = encoder->base.dev;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		vlv_enable_dsi_pll(encoder);
 	else if (IS_BROXTON(dev))
 		bxt_enable_dsi_pll(encoder);
@@ -571,7 +571,7 @@ void intel_disable_dsi_pll(struct intel_encoder *encoder)
 {
 	struct drm_device *dev = encoder->base.dev;
 
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		vlv_disable_dsi_pll(encoder);
 	else if (IS_BROXTON(dev))
 		bxt_disable_dsi_pll(encoder);
@@ -599,6 +599,6 @@ void intel_dsi_reset_clocks(struct intel_encoder *encoder, enum port port)
 
 	if (IS_BROXTON(dev))
 		bxt_dsi_reset_clocks(encoder, port);
-	else if (IS_VALLEYVIEW(dev))
+	else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		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 8492053e0ff0..286baec979c8 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -44,6 +44,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_TMDS,
 		.name = "sil164",
 		.dvo_reg = DVOC,
+		.dvo_srcdim_reg = DVOC_SRCDIM,
 		.slave_addr = SIL164_ADDR,
 		.dev_ops = &sil164_ops,
 	},
@@ -51,6 +52,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_TMDS,
 		.name = "ch7xxx",
 		.dvo_reg = DVOC,
+		.dvo_srcdim_reg = DVOC_SRCDIM,
 		.slave_addr = CH7xxx_ADDR,
 		.dev_ops = &ch7xxx_ops,
 	},
@@ -58,6 +60,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_TMDS,
 		.name = "ch7xxx",
 		.dvo_reg = DVOC,
+		.dvo_srcdim_reg = DVOC_SRCDIM,
 		.slave_addr = 0x75, /* For some ch7010 */
 		.dev_ops = &ch7xxx_ops,
 	},
@@ -65,6 +68,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_LVDS,
 		.name = "ivch",
 		.dvo_reg = DVOA,
+		.dvo_srcdim_reg = DVOA_SRCDIM,
 		.slave_addr = 0x02, /* Might also be 0x44, 0x84, 0xc4 */
 		.dev_ops = &ivch_ops,
 	},
@@ -72,6 +76,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_TMDS,
 		.name = "tfp410",
 		.dvo_reg = DVOC,
+		.dvo_srcdim_reg = DVOC_SRCDIM,
 		.slave_addr = TFP410_ADDR,
 		.dev_ops = &tfp410_ops,
 	},
@@ -79,6 +84,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 		.type = INTEL_DVO_CHIP_LVDS,
 		.name = "ch7017",
 		.dvo_reg = DVOC,
+		.dvo_srcdim_reg = DVOC_SRCDIM,
 		.slave_addr = 0x75,
 		.gpio = GMBUS_PIN_DPB,
 		.dev_ops = &ch7017_ops,
@@ -87,6 +93,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 	        .type = INTEL_DVO_CHIP_TMDS,
 		.name = "ns2501",
 		.dvo_reg = DVOB,
+		.dvo_srcdim_reg = DVOB_SRCDIM,
 		.slave_addr = NS2501_ADDR,
 		.dev_ops = &ns2501_ops,
        }
@@ -171,7 +178,7 @@ static void intel_disable_dvo(struct intel_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
 	struct intel_dvo *intel_dvo = enc_to_dvo(encoder);
-	u32 dvo_reg = intel_dvo->dev.dvo_reg;
+	i915_reg_t dvo_reg = intel_dvo->dev.dvo_reg;
 	u32 temp = I915_READ(dvo_reg);
 
 	intel_dvo->dev.dev_ops->dpms(&intel_dvo->dev, false);
@@ -184,7 +191,7 @@ static void intel_enable_dvo(struct intel_encoder *encoder)
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
 	struct intel_dvo *intel_dvo = enc_to_dvo(encoder);
 	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
-	u32 dvo_reg = intel_dvo->dev.dvo_reg;
+	i915_reg_t dvo_reg = intel_dvo->dev.dvo_reg;
 	u32 temp = I915_READ(dvo_reg);
 
 	intel_dvo->dev.dev_ops->mode_set(&intel_dvo->dev,
@@ -255,20 +262,8 @@ static void intel_dvo_pre_enable(struct intel_encoder *encoder)
 	struct intel_dvo *intel_dvo = enc_to_dvo(encoder);
 	int pipe = crtc->pipe;
 	u32 dvo_val;
-	u32 dvo_reg = intel_dvo->dev.dvo_reg, dvo_srcdim_reg;
-
-	switch (dvo_reg) {
-	case DVOA:
-	default:
-		dvo_srcdim_reg = DVOA_SRCDIM;
-		break;
-	case DVOB:
-		dvo_srcdim_reg = DVOB_SRCDIM;
-		break;
-	case DVOC:
-		dvo_srcdim_reg = DVOC_SRCDIM;
-		break;
-	}
+	i915_reg_t dvo_reg = intel_dvo->dev.dvo_reg;
+	i915_reg_t dvo_srcdim_reg = intel_dvo->dev.dvo_srcdim_reg;
 
 	/* Save the data order, since I don't know what it should be set to. */
 	dvo_val = I915_READ(dvo_reg) &
@@ -434,7 +429,7 @@ void intel_dvo_init(struct drm_device *dev)
 
 	intel_encoder = &intel_dvo->base;
 	drm_encoder_init(dev, &intel_encoder->base,
-			 &intel_dvo_enc_funcs, encoder_type);
+			 &intel_dvo_enc_funcs, encoder_type, NULL);
 
 	intel_encoder->disable = intel_disable_dvo;
 	intel_encoder->enable = intel_enable_dvo;
diff --git a/drivers/gpu/drm/i915/intel_fbc.c b/drivers/gpu/drm/i915/intel_fbc.c
index cf47352b7b8e..a1988a486b92 100644
--- a/drivers/gpu/drm/i915/intel_fbc.c
+++ b/drivers/gpu/drm/i915/intel_fbc.c
@@ -43,7 +43,17 @@
 
 static inline bool fbc_supported(struct drm_i915_private *dev_priv)
 {
-	return dev_priv->fbc.enable_fbc != NULL;
+	return dev_priv->fbc.activate != NULL;
+}
+
+static inline bool fbc_on_pipe_a_only(struct drm_i915_private *dev_priv)
+{
+	return IS_HASWELL(dev_priv) || INTEL_INFO(dev_priv)->gen >= 8;
+}
+
+static inline bool fbc_on_plane_a_only(struct drm_i915_private *dev_priv)
+{
+	return INTEL_INFO(dev_priv)->gen < 4;
 }
 
 /*
@@ -59,11 +69,51 @@ static unsigned int get_crtc_fence_y_offset(struct intel_crtc *crtc)
 	return crtc->base.y - crtc->adjusted_y;
 }
 
-static void i8xx_fbc_disable(struct drm_i915_private *dev_priv)
+/*
+ * For SKL+, the plane source size used by the hardware is based on the value we
+ * write to the PLANE_SIZE register. For BDW-, the hardware looks at the value
+ * we wrote to PIPESRC.
+ */
+static void intel_fbc_get_plane_source_size(struct intel_crtc *crtc,
+					    int *width, int *height)
+{
+	struct intel_plane_state *plane_state =
+			to_intel_plane_state(crtc->base.primary->state);
+	int w, h;
+
+	if (intel_rotation_90_or_270(plane_state->base.rotation)) {
+		w = drm_rect_height(&plane_state->src) >> 16;
+		h = drm_rect_width(&plane_state->src) >> 16;
+	} else {
+		w = drm_rect_width(&plane_state->src) >> 16;
+		h = drm_rect_height(&plane_state->src) >> 16;
+	}
+
+	if (width)
+		*width = w;
+	if (height)
+		*height = h;
+}
+
+static int intel_fbc_calculate_cfb_size(struct intel_crtc *crtc,
+					struct drm_framebuffer *fb)
+{
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+	int lines;
+
+	intel_fbc_get_plane_source_size(crtc, NULL, &lines);
+	if (INTEL_INFO(dev_priv)->gen >= 7)
+		lines = min(lines, 2048);
+
+	/* Hardware needs the full buffer stride, not just the active area. */
+	return lines * fb->pitches[0];
+}
+
+static void i8xx_fbc_deactivate(struct drm_i915_private *dev_priv)
 {
 	u32 fbc_ctl;
 
-	dev_priv->fbc.enabled = false;
+	dev_priv->fbc.active = false;
 
 	/* Disable compression */
 	fbc_ctl = I915_READ(FBC_CONTROL);
@@ -78,11 +128,9 @@ static void i8xx_fbc_disable(struct drm_i915_private *dev_priv)
 		DRM_DEBUG_KMS("FBC idle timed out\n");
 		return;
 	}
-
-	DRM_DEBUG_KMS("disabled FBC\n");
 }
 
-static void i8xx_fbc_enable(struct intel_crtc *crtc)
+static void i8xx_fbc_activate(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	struct drm_framebuffer *fb = crtc->base.primary->fb;
@@ -91,10 +139,10 @@ static void i8xx_fbc_enable(struct intel_crtc *crtc)
 	int i;
 	u32 fbc_ctl;
 
-	dev_priv->fbc.enabled = true;
+	dev_priv->fbc.active = true;
 
 	/* Note: fbc.threshold == 1 for i8xx */
-	cfb_pitch = dev_priv->fbc.uncompressed_size / FBC_LL_SIZE;
+	cfb_pitch = intel_fbc_calculate_cfb_size(crtc, fb) / FBC_LL_SIZE;
 	if (fb->pitches[0] < cfb_pitch)
 		cfb_pitch = fb->pitches[0];
 
@@ -127,24 +175,21 @@ static void i8xx_fbc_enable(struct intel_crtc *crtc)
 	fbc_ctl |= (cfb_pitch & 0xff) << FBC_CTL_STRIDE_SHIFT;
 	fbc_ctl |= obj->fence_reg;
 	I915_WRITE(FBC_CONTROL, fbc_ctl);
-
-	DRM_DEBUG_KMS("enabled FBC, pitch %d, yoff %d, plane %c\n",
-		      cfb_pitch, crtc->base.y, plane_name(crtc->plane));
 }
 
-static bool i8xx_fbc_enabled(struct drm_i915_private *dev_priv)
+static bool i8xx_fbc_is_active(struct drm_i915_private *dev_priv)
 {
 	return I915_READ(FBC_CONTROL) & FBC_CTL_EN;
 }
 
-static void g4x_fbc_enable(struct intel_crtc *crtc)
+static void g4x_fbc_activate(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	struct drm_framebuffer *fb = crtc->base.primary->fb;
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	u32 dpfc_ctl;
 
-	dev_priv->fbc.enabled = true;
+	dev_priv->fbc.active = true;
 
 	dpfc_ctl = DPFC_CTL_PLANE(crtc->plane) | DPFC_SR_EN;
 	if (drm_format_plane_cpp(fb->pixel_format, 0) == 2)
@@ -157,38 +202,35 @@ static void g4x_fbc_enable(struct intel_crtc *crtc)
 
 	/* enable it... */
 	I915_WRITE(DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
-
-	DRM_DEBUG_KMS("enabled fbc on plane %c\n", plane_name(crtc->plane));
 }
 
-static void g4x_fbc_disable(struct drm_i915_private *dev_priv)
+static void g4x_fbc_deactivate(struct drm_i915_private *dev_priv)
 {
 	u32 dpfc_ctl;
 
-	dev_priv->fbc.enabled = false;
+	dev_priv->fbc.active = false;
 
 	/* Disable compression */
 	dpfc_ctl = I915_READ(DPFC_CONTROL);
 	if (dpfc_ctl & DPFC_CTL_EN) {
 		dpfc_ctl &= ~DPFC_CTL_EN;
 		I915_WRITE(DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
 	}
 }
 
-static bool g4x_fbc_enabled(struct drm_i915_private *dev_priv)
+static bool g4x_fbc_is_active(struct drm_i915_private *dev_priv)
 {
 	return I915_READ(DPFC_CONTROL) & DPFC_CTL_EN;
 }
 
-static void intel_fbc_nuke(struct drm_i915_private *dev_priv)
+/* This function forces a CFB recompression through the nuke operation. */
+static void intel_fbc_recompress(struct drm_i915_private *dev_priv)
 {
 	I915_WRITE(MSG_FBC_REND_STATE, FBC_REND_NUKE);
 	POSTING_READ(MSG_FBC_REND_STATE);
 }
 
-static void ilk_fbc_enable(struct intel_crtc *crtc)
+static void ilk_fbc_activate(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	struct drm_framebuffer *fb = crtc->base.primary->fb;
@@ -197,7 +239,7 @@ static void ilk_fbc_enable(struct intel_crtc *crtc)
 	int threshold = dev_priv->fbc.threshold;
 	unsigned int y_offset;
 
-	dev_priv->fbc.enabled = true;
+	dev_priv->fbc.active = true;
 
 	dpfc_ctl = DPFC_CTL_PLANE(crtc->plane);
 	if (drm_format_plane_cpp(fb->pixel_format, 0) == 2)
@@ -231,33 +273,29 @@ static void ilk_fbc_enable(struct intel_crtc *crtc)
 		I915_WRITE(DPFC_CPU_FENCE_OFFSET, y_offset);
 	}
 
-	intel_fbc_nuke(dev_priv);
-
-	DRM_DEBUG_KMS("enabled fbc on plane %c\n", plane_name(crtc->plane));
+	intel_fbc_recompress(dev_priv);
 }
 
-static void ilk_fbc_disable(struct drm_i915_private *dev_priv)
+static void ilk_fbc_deactivate(struct drm_i915_private *dev_priv)
 {
 	u32 dpfc_ctl;
 
-	dev_priv->fbc.enabled = false;
+	dev_priv->fbc.active = false;
 
 	/* Disable compression */
 	dpfc_ctl = I915_READ(ILK_DPFC_CONTROL);
 	if (dpfc_ctl & DPFC_CTL_EN) {
 		dpfc_ctl &= ~DPFC_CTL_EN;
 		I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl);
-
-		DRM_DEBUG_KMS("disabled FBC\n");
 	}
 }
 
-static bool ilk_fbc_enabled(struct drm_i915_private *dev_priv)
+static bool ilk_fbc_is_active(struct drm_i915_private *dev_priv)
 {
 	return I915_READ(ILK_DPFC_CONTROL) & DPFC_CTL_EN;
 }
 
-static void gen7_fbc_enable(struct intel_crtc *crtc)
+static void gen7_fbc_activate(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	struct drm_framebuffer *fb = crtc->base.primary->fb;
@@ -265,7 +303,7 @@ static void gen7_fbc_enable(struct intel_crtc *crtc)
 	u32 dpfc_ctl;
 	int threshold = dev_priv->fbc.threshold;
 
-	dev_priv->fbc.enabled = true;
+	dev_priv->fbc.active = true;
 
 	dpfc_ctl = 0;
 	if (IS_IVYBRIDGE(dev_priv))
@@ -310,155 +348,120 @@ static void gen7_fbc_enable(struct intel_crtc *crtc)
 		   SNB_CPU_FENCE_ENABLE | obj->fence_reg);
 	I915_WRITE(DPFC_CPU_FENCE_OFFSET, get_crtc_fence_y_offset(crtc));
 
-	intel_fbc_nuke(dev_priv);
-
-	DRM_DEBUG_KMS("enabled fbc on plane %c\n", plane_name(crtc->plane));
+	intel_fbc_recompress(dev_priv);
 }
 
 /**
- * intel_fbc_enabled - Is FBC enabled?
+ * intel_fbc_is_active - Is FBC active?
  * @dev_priv: i915 device instance
  *
  * This function is used to verify the current state of FBC.
  * FIXME: This should be tracked in the plane config eventually
  *        instead of queried at runtime for most callers.
  */
-bool intel_fbc_enabled(struct drm_i915_private *dev_priv)
+bool intel_fbc_is_active(struct drm_i915_private *dev_priv)
 {
-	return dev_priv->fbc.enabled;
+	return dev_priv->fbc.active;
 }
 
-static void intel_fbc_enable(struct intel_crtc *crtc,
-			     const struct drm_framebuffer *fb)
+static void intel_fbc_activate(const struct drm_framebuffer *fb)
 {
-	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+	struct drm_i915_private *dev_priv = fb->dev->dev_private;
+	struct intel_crtc *crtc = dev_priv->fbc.crtc;
 
-	dev_priv->fbc.enable_fbc(crtc);
+	dev_priv->fbc.activate(crtc);
 
-	dev_priv->fbc.crtc = crtc;
 	dev_priv->fbc.fb_id = fb->base.id;
 	dev_priv->fbc.y = crtc->base.y;
 }
 
 static void intel_fbc_work_fn(struct work_struct *__work)
 {
-	struct intel_fbc_work *work =
-		container_of(to_delayed_work(__work),
-			     struct intel_fbc_work, work);
-	struct drm_i915_private *dev_priv = work->crtc->base.dev->dev_private;
-	struct drm_framebuffer *crtc_fb = work->crtc->base.primary->fb;
+	struct drm_i915_private *dev_priv =
+		container_of(__work, struct drm_i915_private, fbc.work.work);
+	struct intel_fbc_work *work = &dev_priv->fbc.work;
+	struct intel_crtc *crtc = dev_priv->fbc.crtc;
+	int delay_ms = 50;
+
+retry:
+	/* Delay the actual enabling to let pageflipping cease and the
+	 * display to settle before starting the compression. Note that
+	 * this delay also serves a second purpose: it allows for a
+	 * vblank to pass after disabling the FBC before we attempt
+	 * to modify the control registers.
+	 *
+	 * A more complicated solution would involve tracking vblanks
+	 * following the termination of the page-flipping sequence
+	 * and indeed performing the enable as a co-routine and not
+	 * waiting synchronously upon the vblank.
+	 *
+	 * WaFbcWaitForVBlankBeforeEnable:ilk,snb
+	 */
+	wait_remaining_ms_from_jiffies(work->enable_jiffies, delay_ms);
 
 	mutex_lock(&dev_priv->fbc.lock);
-	if (work == dev_priv->fbc.fbc_work) {
-		/* Double check that we haven't switched fb without cancelling
-		 * the prior work.
-		 */
-		if (crtc_fb == work->fb)
-			intel_fbc_enable(work->crtc, work->fb);
 
-		dev_priv->fbc.fbc_work = NULL;
+	/* Were we cancelled? */
+	if (!work->scheduled)
+		goto out;
+
+	/* Were we delayed again while this function was sleeping? */
+	if (time_after(work->enable_jiffies + msecs_to_jiffies(delay_ms),
+		       jiffies)) {
+		mutex_unlock(&dev_priv->fbc.lock);
+		goto retry;
 	}
-	mutex_unlock(&dev_priv->fbc.lock);
 
-	kfree(work);
+	if (crtc->base.primary->fb == work->fb)
+		intel_fbc_activate(work->fb);
+
+	work->scheduled = false;
+
+out:
+	mutex_unlock(&dev_priv->fbc.lock);
 }
 
 static void intel_fbc_cancel_work(struct drm_i915_private *dev_priv)
 {
 	WARN_ON(!mutex_is_locked(&dev_priv->fbc.lock));
-
-	if (dev_priv->fbc.fbc_work == NULL)
-		return;
-
-	DRM_DEBUG_KMS("cancelling pending FBC enable\n");
-
-	/* Synchronisation is provided by struct_mutex and checking of
-	 * dev_priv->fbc.fbc_work, so we can perform the cancellation
-	 * entirely asynchronously.
-	 */
-	if (cancel_delayed_work(&dev_priv->fbc.fbc_work->work))
-		/* tasklet was killed before being run, clean up */
-		kfree(dev_priv->fbc.fbc_work);
-
-	/* Mark the work as no longer wanted so that if it does
-	 * wake-up (because the work was already running and waiting
-	 * for our mutex), it will discover that is no longer
-	 * necessary to run.
-	 */
-	dev_priv->fbc.fbc_work = NULL;
+	dev_priv->fbc.work.scheduled = false;
 }
 
-static void intel_fbc_schedule_enable(struct intel_crtc *crtc)
+static void intel_fbc_schedule_activation(struct intel_crtc *crtc)
 {
-	struct intel_fbc_work *work;
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+	struct intel_fbc_work *work = &dev_priv->fbc.work;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->fbc.lock));
 
-	intel_fbc_cancel_work(dev_priv);
-
-	work = kzalloc(sizeof(*work), GFP_KERNEL);
-	if (work == NULL) {
-		DRM_ERROR("Failed to allocate FBC work structure\n");
-		intel_fbc_enable(crtc, crtc->base.primary->fb);
-		return;
-	}
-
-	work->crtc = crtc;
+	/* It is useless to call intel_fbc_cancel_work() in this function since
+	 * we're not releasing fbc.lock, so it won't have an opportunity to grab
+	 * it to discover that it was cancelled. So we just update the expected
+	 * jiffy count. */
 	work->fb = crtc->base.primary->fb;
-	INIT_DELAYED_WORK(&work->work, intel_fbc_work_fn);
-
-	dev_priv->fbc.fbc_work = work;
+	work->scheduled = true;
+	work->enable_jiffies = jiffies;
 
-	/* Delay the actual enabling to let pageflipping cease and the
-	 * display to settle before starting the compression. Note that
-	 * this delay also serves a second purpose: it allows for a
-	 * vblank to pass after disabling the FBC before we attempt
-	 * to modify the control registers.
-	 *
-	 * A more complicated solution would involve tracking vblanks
-	 * following the termination of the page-flipping sequence
-	 * and indeed performing the enable as a co-routine and not
-	 * waiting synchronously upon the vblank.
-	 *
-	 * WaFbcWaitForVBlankBeforeEnable:ilk,snb
-	 */
-	schedule_delayed_work(&work->work, msecs_to_jiffies(50));
+	schedule_work(&work->work);
 }
 
-static void __intel_fbc_disable(struct drm_i915_private *dev_priv)
+static void __intel_fbc_deactivate(struct drm_i915_private *dev_priv)
 {
 	WARN_ON(!mutex_is_locked(&dev_priv->fbc.lock));
 
 	intel_fbc_cancel_work(dev_priv);
 
-	dev_priv->fbc.disable_fbc(dev_priv);
-	dev_priv->fbc.crtc = NULL;
-}
-
-/**
- * intel_fbc_disable - disable FBC
- * @dev_priv: i915 device instance
- *
- * This function disables FBC.
- */
-void intel_fbc_disable(struct drm_i915_private *dev_priv)
-{
-	if (!fbc_supported(dev_priv))
-		return;
-
-	mutex_lock(&dev_priv->fbc.lock);
-	__intel_fbc_disable(dev_priv);
-	mutex_unlock(&dev_priv->fbc.lock);
+	if (dev_priv->fbc.active)
+		dev_priv->fbc.deactivate(dev_priv);
 }
 
 /*
- * intel_fbc_disable_crtc - disable FBC if it's associated with crtc
+ * intel_fbc_deactivate - deactivate FBC if it's associated with crtc
  * @crtc: the CRTC
  *
- * This function disables FBC if it's associated with the provided CRTC.
+ * This function deactivates FBC if it's associated with the provided CRTC.
  */
-void intel_fbc_disable_crtc(struct intel_crtc *crtc)
+void intel_fbc_deactivate(struct intel_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 
@@ -467,85 +470,42 @@ void intel_fbc_disable_crtc(struct intel_crtc *crtc)
 
 	mutex_lock(&dev_priv->fbc.lock);
 	if (dev_priv->fbc.crtc == crtc)
-		__intel_fbc_disable(dev_priv);
+		__intel_fbc_deactivate(dev_priv);
 	mutex_unlock(&dev_priv->fbc.lock);
 }
 
-const char *intel_no_fbc_reason_str(enum no_fbc_reason reason)
-{
-	switch (reason) {
-	case FBC_OK:
-		return "FBC enabled but currently disabled in hardware";
-	case FBC_UNSUPPORTED:
-		return "unsupported by this chipset";
-	case FBC_NO_OUTPUT:
-		return "no output";
-	case FBC_STOLEN_TOO_SMALL:
-		return "not enough stolen memory";
-	case FBC_UNSUPPORTED_MODE:
-		return "mode incompatible with compression";
-	case FBC_MODE_TOO_LARGE:
-		return "mode too large for compression";
-	case FBC_BAD_PLANE:
-		return "FBC unsupported on plane";
-	case FBC_NOT_TILED:
-		return "framebuffer not tiled or fenced";
-	case FBC_MULTIPLE_PIPES:
-		return "more than one pipe active";
-	case FBC_MODULE_PARAM:
-		return "disabled per module param";
-	case FBC_CHIP_DEFAULT:
-		return "disabled per chip default";
-	case FBC_ROTATION:
-		return "rotation unsupported";
-	case FBC_IN_DBG_MASTER:
-		return "Kernel debugger is active";
-	case FBC_BAD_STRIDE:
-		return "framebuffer stride not supported";
-	case FBC_PIXEL_RATE:
-		return "pixel rate is too big";
-	case FBC_PIXEL_FORMAT:
-		return "pixel format is invalid";
-	default:
-		MISSING_CASE(reason);
-		return "unknown reason";
-	}
-}
-
 static void set_no_fbc_reason(struct drm_i915_private *dev_priv,
-			      enum no_fbc_reason reason)
+			      const char *reason)
 {
 	if (dev_priv->fbc.no_fbc_reason == reason)
 		return;
 
 	dev_priv->fbc.no_fbc_reason = reason;
-	DRM_DEBUG_KMS("Disabling FBC: %s\n", intel_no_fbc_reason_str(reason));
+	DRM_DEBUG_KMS("Disabling FBC: %s\n", reason);
 }
 
-static struct drm_crtc *intel_fbc_find_crtc(struct drm_i915_private *dev_priv)
+static bool crtc_can_fbc(struct intel_crtc *crtc)
 {
-	struct drm_crtc *crtc = NULL, *tmp_crtc;
-	enum pipe pipe;
-	bool pipe_a_only = false;
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 
-	if (IS_HASWELL(dev_priv) || INTEL_INFO(dev_priv)->gen >= 8)
-		pipe_a_only = true;
+	if (fbc_on_pipe_a_only(dev_priv) && crtc->pipe != PIPE_A)
+		return false;
 
-	for_each_pipe(dev_priv, pipe) {
-		tmp_crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	if (fbc_on_plane_a_only(dev_priv) && crtc->plane != PLANE_A)
+		return false;
 
-		if (intel_crtc_active(tmp_crtc) &&
-		    to_intel_plane_state(tmp_crtc->primary->state)->visible)
-			crtc = tmp_crtc;
+	return true;
+}
 
-		if (pipe_a_only)
-			break;
-	}
+static bool crtc_is_valid(struct intel_crtc *crtc)
+{
+	if (!intel_crtc_active(&crtc->base))
+		return false;
 
-	if (!crtc || crtc->primary->fb == NULL)
-		return NULL;
+	if (!to_intel_plane_state(crtc->base.primary->state)->visible)
+		return false;
 
-	return crtc;
+	return true;
 }
 
 static bool multiple_pipes_ok(struct drm_i915_private *dev_priv)
@@ -581,7 +541,8 @@ static int find_compression_threshold(struct drm_i915_private *dev_priv,
 	 * reserved range size, so it always assumes the maximum (8mb) is used.
 	 * If we enable FBC using a CFB on that memory range we'll get FIFO
 	 * underruns, even if that range is not reserved by the BIOS. */
-	if (IS_BROADWELL(dev_priv) || IS_SKYLAKE(dev_priv))
+	if (IS_BROADWELL(dev_priv) ||
+	    IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		end = dev_priv->gtt.stolen_size - 8 * 1024 * 1024;
 	else
 		end = dev_priv->gtt.stolen_usable_size;
@@ -617,11 +578,17 @@ again:
 	}
 }
 
-static int intel_fbc_alloc_cfb(struct drm_i915_private *dev_priv, int size,
-			       int fb_cpp)
+static int intel_fbc_alloc_cfb(struct intel_crtc *crtc)
 {
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+	struct drm_framebuffer *fb = crtc->base.primary->state->fb;
 	struct drm_mm_node *uninitialized_var(compressed_llb);
-	int ret;
+	int size, fb_cpp, ret;
+
+	WARN_ON(drm_mm_node_allocated(&dev_priv->fbc.compressed_fb));
+
+	size = intel_fbc_calculate_cfb_size(crtc, fb);
+	fb_cpp = drm_format_plane_cpp(fb->pixel_format, 0);
 
 	ret = find_compression_threshold(dev_priv, &dev_priv->fbc.compressed_fb,
 					 size, fb_cpp);
@@ -656,8 +623,6 @@ static int intel_fbc_alloc_cfb(struct drm_i915_private *dev_priv, int size,
 			   dev_priv->mm.stolen_base + compressed_llb->start);
 	}
 
-	dev_priv->fbc.uncompressed_size = size;
-
 	DRM_DEBUG_KMS("reserved %llu bytes of contiguous stolen space for FBC, threshold: %d\n",
 		      dev_priv->fbc.compressed_fb.size,
 		      dev_priv->fbc.threshold);
@@ -674,18 +639,15 @@ err_llb:
 
 static void __intel_fbc_cleanup_cfb(struct drm_i915_private *dev_priv)
 {
-	if (dev_priv->fbc.uncompressed_size == 0)
-		return;
-
-	i915_gem_stolen_remove_node(dev_priv, &dev_priv->fbc.compressed_fb);
+	if (drm_mm_node_allocated(&dev_priv->fbc.compressed_fb))
+		i915_gem_stolen_remove_node(dev_priv,
+					    &dev_priv->fbc.compressed_fb);
 
 	if (dev_priv->fbc.compressed_llb) {
 		i915_gem_stolen_remove_node(dev_priv,
 					    dev_priv->fbc.compressed_llb);
 		kfree(dev_priv->fbc.compressed_llb);
 	}
-
-	dev_priv->fbc.uncompressed_size = 0;
 }
 
 void intel_fbc_cleanup_cfb(struct drm_i915_private *dev_priv)
@@ -698,63 +660,6 @@ void intel_fbc_cleanup_cfb(struct drm_i915_private *dev_priv)
 	mutex_unlock(&dev_priv->fbc.lock);
 }
 
-/*
- * For SKL+, the plane source size used by the hardware is based on the value we
- * write to the PLANE_SIZE register. For BDW-, the hardware looks at the value
- * we wrote to PIPESRC.
- */
-static void intel_fbc_get_plane_source_size(struct intel_crtc *crtc,
-					    int *width, int *height)
-{
-	struct intel_plane_state *plane_state =
-			to_intel_plane_state(crtc->base.primary->state);
-	int w, h;
-
-	if (intel_rotation_90_or_270(plane_state->base.rotation)) {
-		w = drm_rect_height(&plane_state->src) >> 16;
-		h = drm_rect_width(&plane_state->src) >> 16;
-	} else {
-		w = drm_rect_width(&plane_state->src) >> 16;
-		h = drm_rect_height(&plane_state->src) >> 16;
-	}
-
-	if (width)
-		*width = w;
-	if (height)
-		*height = h;
-}
-
-static int intel_fbc_calculate_cfb_size(struct intel_crtc *crtc)
-{
-	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
-	struct drm_framebuffer *fb = crtc->base.primary->fb;
-	int lines;
-
-	intel_fbc_get_plane_source_size(crtc, NULL, &lines);
-	if (INTEL_INFO(dev_priv)->gen >= 7)
-		lines = min(lines, 2048);
-
-	return lines * fb->pitches[0];
-}
-
-static int intel_fbc_setup_cfb(struct intel_crtc *crtc)
-{
-	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
-	struct drm_framebuffer *fb = crtc->base.primary->fb;
-	int size, cpp;
-
-	size = intel_fbc_calculate_cfb_size(crtc);
-	cpp = drm_format_plane_cpp(fb->pixel_format, 0);
-
-	if (size <= dev_priv->fbc.uncompressed_size)
-		return 0;
-
-	/* Release any current block */
-	__intel_fbc_cleanup_cfb(dev_priv);
-
-	return intel_fbc_alloc_cfb(dev_priv, size, cpp);
-}
-
 static bool stride_is_valid(struct drm_i915_private *dev_priv,
 			    unsigned int stride)
 {
@@ -829,87 +734,46 @@ static bool intel_fbc_hw_tracking_covers_screen(struct intel_crtc *crtc)
 }
 
 /**
- * __intel_fbc_update - enable/disable FBC as needed, unlocked
- * @dev_priv: i915 device instance
- *
- * Set up the framebuffer compression hardware at mode set time.  We
- * enable it if possible:
- *   - plane A only (on pre-965)
- *   - no pixel mulitply/line duplication
- *   - no alpha buffer discard
- *   - no dual wide
- *   - framebuffer <= max_hdisplay in width, max_vdisplay in height
- *
- * We can't assume that any compression will take place (worst case),
- * so the compressed buffer has to be the same size as the uncompressed
- * one.  It also must reside (along with the line length buffer) in
- * stolen memory.
+ * __intel_fbc_update - activate/deactivate FBC as needed, unlocked
+ * @crtc: the CRTC that triggered the update
  *
- * We need to enable/disable FBC on a global basis.
+ * This function completely reevaluates the status of FBC, then activates,
+ * deactivates or maintains it on the same state.
  */
-static void __intel_fbc_update(struct drm_i915_private *dev_priv)
+static void __intel_fbc_update(struct intel_crtc *crtc)
 {
-	struct drm_crtc *crtc = NULL;
-	struct intel_crtc *intel_crtc;
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
 	struct drm_framebuffer *fb;
 	struct drm_i915_gem_object *obj;
 	const struct drm_display_mode *adjusted_mode;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->fbc.lock));
 
-	/* disable framebuffer compression in vGPU */
-	if (intel_vgpu_active(dev_priv->dev))
-		i915.enable_fbc = 0;
-
-	if (i915.enable_fbc < 0) {
-		set_no_fbc_reason(dev_priv, FBC_CHIP_DEFAULT);
+	if (!multiple_pipes_ok(dev_priv)) {
+		set_no_fbc_reason(dev_priv, "more than one pipe active");
 		goto out_disable;
 	}
 
-	if (!i915.enable_fbc) {
-		set_no_fbc_reason(dev_priv, FBC_MODULE_PARAM);
-		goto out_disable;
-	}
+	if (!dev_priv->fbc.enabled || dev_priv->fbc.crtc != crtc)
+		return;
 
-	/*
-	 * If FBC is already on, we just have to verify that we can
-	 * keep it that way...
-	 * Need to disable if:
-	 *   - more than one pipe is active
-	 *   - changing FBC params (stride, fence, mode)
-	 *   - new fb is too large to fit in compressed buffer
-	 *   - going to an unsupported config (interlace, pixel multiply, etc.)
-	 */
-	crtc = intel_fbc_find_crtc(dev_priv);
-	if (!crtc) {
-		set_no_fbc_reason(dev_priv, FBC_NO_OUTPUT);
+	if (!crtc_is_valid(crtc)) {
+		set_no_fbc_reason(dev_priv, "no output");
 		goto out_disable;
 	}
 
-	if (!multiple_pipes_ok(dev_priv)) {
-		set_no_fbc_reason(dev_priv, FBC_MULTIPLE_PIPES);
-		goto out_disable;
-	}
-
-	intel_crtc = to_intel_crtc(crtc);
-	fb = crtc->primary->fb;
+	fb = crtc->base.primary->fb;
 	obj = intel_fb_obj(fb);
-	adjusted_mode = &intel_crtc->config->base.adjusted_mode;
+	adjusted_mode = &crtc->config->base.adjusted_mode;
 
 	if ((adjusted_mode->flags & DRM_MODE_FLAG_INTERLACE) ||
 	    (adjusted_mode->flags & DRM_MODE_FLAG_DBLSCAN)) {
-		set_no_fbc_reason(dev_priv, FBC_UNSUPPORTED_MODE);
+		set_no_fbc_reason(dev_priv, "incompatible mode");
 		goto out_disable;
 	}
 
-	if (!intel_fbc_hw_tracking_covers_screen(intel_crtc)) {
-		set_no_fbc_reason(dev_priv, FBC_MODE_TOO_LARGE);
-		goto out_disable;
-	}
-
-	if ((INTEL_INFO(dev_priv)->gen < 4 || HAS_DDI(dev_priv)) &&
-	    intel_crtc->plane != PLANE_A) {
-		set_no_fbc_reason(dev_priv, FBC_BAD_PLANE);
+	if (!intel_fbc_hw_tracking_covers_screen(crtc)) {
+		set_no_fbc_reason(dev_priv, "mode too large for compression");
 		goto out_disable;
 	}
 
@@ -918,41 +782,46 @@ static void __intel_fbc_update(struct drm_i915_private *dev_priv)
 	 */
 	if (obj->tiling_mode != I915_TILING_X ||
 	    obj->fence_reg == I915_FENCE_REG_NONE) {
-		set_no_fbc_reason(dev_priv, FBC_NOT_TILED);
+		set_no_fbc_reason(dev_priv, "framebuffer not tiled or fenced");
 		goto out_disable;
 	}
 	if (INTEL_INFO(dev_priv)->gen <= 4 && !IS_G4X(dev_priv) &&
-	    crtc->primary->state->rotation != BIT(DRM_ROTATE_0)) {
-		set_no_fbc_reason(dev_priv, FBC_ROTATION);
+	    crtc->base.primary->state->rotation != BIT(DRM_ROTATE_0)) {
+		set_no_fbc_reason(dev_priv, "rotation unsupported");
 		goto out_disable;
 	}
 
 	if (!stride_is_valid(dev_priv, fb->pitches[0])) {
-		set_no_fbc_reason(dev_priv, FBC_BAD_STRIDE);
+		set_no_fbc_reason(dev_priv, "framebuffer stride not supported");
 		goto out_disable;
 	}
 
 	if (!pixel_format_is_valid(fb)) {
-		set_no_fbc_reason(dev_priv, FBC_PIXEL_FORMAT);
-		goto out_disable;
-	}
-
-	/* If the kernel debugger is active, always disable compression */
-	if (in_dbg_master()) {
-		set_no_fbc_reason(dev_priv, FBC_IN_DBG_MASTER);
+		set_no_fbc_reason(dev_priv, "pixel format is invalid");
 		goto out_disable;
 	}
 
 	/* WaFbcExceedCdClockThreshold:hsw,bdw */
 	if ((IS_HASWELL(dev_priv) || IS_BROADWELL(dev_priv)) &&
-	    ilk_pipe_pixel_rate(intel_crtc->config) >=
+	    ilk_pipe_pixel_rate(crtc->config) >=
 	    dev_priv->cdclk_freq * 95 / 100) {
-		set_no_fbc_reason(dev_priv, FBC_PIXEL_RATE);
+		set_no_fbc_reason(dev_priv, "pixel rate is too big");
 		goto out_disable;
 	}
 
-	if (intel_fbc_setup_cfb(intel_crtc)) {
-		set_no_fbc_reason(dev_priv, FBC_STOLEN_TOO_SMALL);
+	/* It is possible for the required CFB size change without a
+	 * crtc->disable + crtc->enable since it is possible to change the
+	 * stride without triggering a full modeset. Since we try to
+	 * over-allocate the CFB, there's a chance we may keep FBC enabled even
+	 * if this happens, but if we exceed the current CFB size we'll have to
+	 * disable FBC. Notice that it would be possible to disable FBC, wait
+	 * for a frame, free the stolen node, then try to reenable FBC in case
+	 * we didn't get any invalidate/deactivate calls, but this would require
+	 * a lot of tracking just for a specific case. If we conclude it's an
+	 * important case, we can implement it later. */
+	if (intel_fbc_calculate_cfb_size(crtc, fb) >
+	    dev_priv->fbc.compressed_fb.size * dev_priv->fbc.threshold) {
+		set_no_fbc_reason(dev_priv, "CFB requirements changed");
 		goto out_disable;
 	}
 
@@ -961,12 +830,13 @@ static void __intel_fbc_update(struct drm_i915_private *dev_priv)
 	 * cannot be unpinned (and have its GTT offset and fence revoked)
 	 * without first being decoupled from the scanout and FBC disabled.
 	 */
-	if (dev_priv->fbc.crtc == intel_crtc &&
+	if (dev_priv->fbc.crtc == crtc &&
 	    dev_priv->fbc.fb_id == fb->base.id &&
-	    dev_priv->fbc.y == crtc->y)
+	    dev_priv->fbc.y == crtc->base.y &&
+	    dev_priv->fbc.active)
 		return;
 
-	if (intel_fbc_enabled(dev_priv)) {
+	if (intel_fbc_is_active(dev_priv)) {
 		/* We update FBC along two paths, after changing fb/crtc
 		 * configuration (modeswitching) and after page-flipping
 		 * finishes. For the latter, we know that not only did
@@ -990,36 +860,37 @@ static void __intel_fbc_update(struct drm_i915_private *dev_priv)
 		 * disabling paths we do need to wait for a vblank at
 		 * some point. And we wait before enabling FBC anyway.
 		 */
-		DRM_DEBUG_KMS("disabling active FBC for update\n");
-		__intel_fbc_disable(dev_priv);
+		DRM_DEBUG_KMS("deactivating FBC for update\n");
+		__intel_fbc_deactivate(dev_priv);
 	}
 
-	intel_fbc_schedule_enable(intel_crtc);
-	dev_priv->fbc.no_fbc_reason = FBC_OK;
+	intel_fbc_schedule_activation(crtc);
+	dev_priv->fbc.no_fbc_reason = "FBC enabled (not necessarily active)";
 	return;
 
 out_disable:
 	/* Multiple disables should be harmless */
-	if (intel_fbc_enabled(dev_priv)) {
-		DRM_DEBUG_KMS("unsupported config, disabling FBC\n");
-		__intel_fbc_disable(dev_priv);
+	if (intel_fbc_is_active(dev_priv)) {
+		DRM_DEBUG_KMS("unsupported config, deactivating FBC\n");
+		__intel_fbc_deactivate(dev_priv);
 	}
-	__intel_fbc_cleanup_cfb(dev_priv);
 }
 
 /*
- * intel_fbc_update - enable/disable FBC as needed
- * @dev_priv: i915 device instance
+ * intel_fbc_update - activate/deactivate FBC as needed
+ * @crtc: the CRTC that triggered the update
  *
- * This function reevaluates the overall state and enables or disables FBC.
+ * This function reevaluates the overall state and activates or deactivates FBC.
  */
-void intel_fbc_update(struct drm_i915_private *dev_priv)
+void intel_fbc_update(struct intel_crtc *crtc)
 {
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+
 	if (!fbc_supported(dev_priv))
 		return;
 
 	mutex_lock(&dev_priv->fbc.lock);
-	__intel_fbc_update(dev_priv);
+	__intel_fbc_update(crtc);
 	mutex_unlock(&dev_priv->fbc.lock);
 }
 
@@ -1039,16 +910,13 @@ void intel_fbc_invalidate(struct drm_i915_private *dev_priv,
 
 	if (dev_priv->fbc.enabled)
 		fbc_bits = INTEL_FRONTBUFFER_PRIMARY(dev_priv->fbc.crtc->pipe);
-	else if (dev_priv->fbc.fbc_work)
-		fbc_bits = INTEL_FRONTBUFFER_PRIMARY(
-					dev_priv->fbc.fbc_work->crtc->pipe);
 	else
 		fbc_bits = dev_priv->fbc.possible_framebuffer_bits;
 
 	dev_priv->fbc.busy_bits |= (fbc_bits & frontbuffer_bits);
 
 	if (dev_priv->fbc.busy_bits)
-		__intel_fbc_disable(dev_priv);
+		__intel_fbc_deactivate(dev_priv);
 
 	mutex_unlock(&dev_priv->fbc.lock);
 }
@@ -1066,11 +934,136 @@ void intel_fbc_flush(struct drm_i915_private *dev_priv,
 
 	dev_priv->fbc.busy_bits &= ~frontbuffer_bits;
 
-	if (!dev_priv->fbc.busy_bits) {
+	if (!dev_priv->fbc.busy_bits && dev_priv->fbc.enabled) {
+		if (origin != ORIGIN_FLIP && dev_priv->fbc.active) {
+			intel_fbc_recompress(dev_priv);
+		} else {
+			__intel_fbc_deactivate(dev_priv);
+			__intel_fbc_update(dev_priv->fbc.crtc);
+		}
+	}
+
+	mutex_unlock(&dev_priv->fbc.lock);
+}
+
+/**
+ * intel_fbc_enable: tries to enable FBC on the CRTC
+ * @crtc: the CRTC
+ *
+ * This function checks if it's possible to enable FBC on the following CRTC,
+ * then enables it. Notice that it doesn't activate FBC.
+ */
+void intel_fbc_enable(struct intel_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+
+	if (!fbc_supported(dev_priv))
+		return;
+
+	mutex_lock(&dev_priv->fbc.lock);
+
+	if (dev_priv->fbc.enabled) {
+		WARN_ON(dev_priv->fbc.crtc == crtc);
+		goto out;
+	}
+
+	WARN_ON(dev_priv->fbc.active);
+	WARN_ON(dev_priv->fbc.crtc != NULL);
+
+	if (intel_vgpu_active(dev_priv->dev)) {
+		set_no_fbc_reason(dev_priv, "VGPU is active");
+		goto out;
+	}
+
+	if (i915.enable_fbc < 0) {
+		set_no_fbc_reason(dev_priv, "disabled per chip default");
+		goto out;
+	}
+
+	if (!i915.enable_fbc) {
+		set_no_fbc_reason(dev_priv, "disabled per module param");
+		goto out;
+	}
+
+	if (!crtc_can_fbc(crtc)) {
+		set_no_fbc_reason(dev_priv, "no enabled pipes can have FBC");
+		goto out;
+	}
+
+	if (intel_fbc_alloc_cfb(crtc)) {
+		set_no_fbc_reason(dev_priv, "not enough stolen memory");
+		goto out;
+	}
+
+	DRM_DEBUG_KMS("Enabling FBC on pipe %c\n", pipe_name(crtc->pipe));
+	dev_priv->fbc.no_fbc_reason = "FBC enabled but not active yet\n";
+
+	dev_priv->fbc.enabled = true;
+	dev_priv->fbc.crtc = crtc;
+out:
+	mutex_unlock(&dev_priv->fbc.lock);
+}
+
+/**
+ * __intel_fbc_disable - disable FBC
+ * @dev_priv: i915 device instance
+ *
+ * This is the low level function that actually disables FBC. Callers should
+ * grab the FBC lock.
+ */
+static void __intel_fbc_disable(struct drm_i915_private *dev_priv)
+{
+	struct intel_crtc *crtc = dev_priv->fbc.crtc;
+
+	WARN_ON(!mutex_is_locked(&dev_priv->fbc.lock));
+	WARN_ON(!dev_priv->fbc.enabled);
+	WARN_ON(dev_priv->fbc.active);
+	assert_pipe_disabled(dev_priv, crtc->pipe);
+
+	DRM_DEBUG_KMS("Disabling FBC on pipe %c\n", pipe_name(crtc->pipe));
+
+	__intel_fbc_cleanup_cfb(dev_priv);
+
+	dev_priv->fbc.enabled = false;
+	dev_priv->fbc.crtc = NULL;
+}
+
+/**
+ * intel_fbc_disable_crtc - disable FBC if it's associated with crtc
+ * @crtc: the CRTC
+ *
+ * This function disables FBC if it's associated with the provided CRTC.
+ */
+void intel_fbc_disable_crtc(struct intel_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+
+	if (!fbc_supported(dev_priv))
+		return;
+
+	mutex_lock(&dev_priv->fbc.lock);
+	if (dev_priv->fbc.crtc == crtc) {
+		WARN_ON(!dev_priv->fbc.enabled);
+		WARN_ON(dev_priv->fbc.active);
 		__intel_fbc_disable(dev_priv);
-		__intel_fbc_update(dev_priv);
 	}
+	mutex_unlock(&dev_priv->fbc.lock);
+}
 
+/**
+ * intel_fbc_disable - globally disable FBC
+ * @dev_priv: i915 device instance
+ *
+ * This function disables FBC regardless of which CRTC is associated with it.
+ */
+void intel_fbc_disable(struct drm_i915_private *dev_priv)
+{
+	if (!fbc_supported(dev_priv))
+		return;
+
+	mutex_lock(&dev_priv->fbc.lock);
+	if (dev_priv->fbc.enabled)
+		__intel_fbc_disable(dev_priv);
 	mutex_unlock(&dev_priv->fbc.lock);
 }
 
@@ -1084,11 +1077,14 @@ void intel_fbc_init(struct drm_i915_private *dev_priv)
 {
 	enum pipe pipe;
 
+	INIT_WORK(&dev_priv->fbc.work.work, intel_fbc_work_fn);
 	mutex_init(&dev_priv->fbc.lock);
+	dev_priv->fbc.enabled = false;
+	dev_priv->fbc.active = false;
+	dev_priv->fbc.work.scheduled = false;
 
 	if (!HAS_FBC(dev_priv)) {
-		dev_priv->fbc.enabled = false;
-		dev_priv->fbc.no_fbc_reason = FBC_UNSUPPORTED;
+		dev_priv->fbc.no_fbc_reason = "unsupported by this chipset";
 		return;
 	}
 
@@ -1096,30 +1092,34 @@ void intel_fbc_init(struct drm_i915_private *dev_priv)
 		dev_priv->fbc.possible_framebuffer_bits |=
 				INTEL_FRONTBUFFER_PRIMARY(pipe);
 
-		if (IS_HASWELL(dev_priv) || INTEL_INFO(dev_priv)->gen >= 8)
+		if (fbc_on_pipe_a_only(dev_priv))
 			break;
 	}
 
 	if (INTEL_INFO(dev_priv)->gen >= 7) {
-		dev_priv->fbc.fbc_enabled = ilk_fbc_enabled;
-		dev_priv->fbc.enable_fbc = gen7_fbc_enable;
-		dev_priv->fbc.disable_fbc = ilk_fbc_disable;
+		dev_priv->fbc.is_active = ilk_fbc_is_active;
+		dev_priv->fbc.activate = gen7_fbc_activate;
+		dev_priv->fbc.deactivate = ilk_fbc_deactivate;
 	} else if (INTEL_INFO(dev_priv)->gen >= 5) {
-		dev_priv->fbc.fbc_enabled = ilk_fbc_enabled;
-		dev_priv->fbc.enable_fbc = ilk_fbc_enable;
-		dev_priv->fbc.disable_fbc = ilk_fbc_disable;
+		dev_priv->fbc.is_active = ilk_fbc_is_active;
+		dev_priv->fbc.activate = ilk_fbc_activate;
+		dev_priv->fbc.deactivate = ilk_fbc_deactivate;
 	} else if (IS_GM45(dev_priv)) {
-		dev_priv->fbc.fbc_enabled = g4x_fbc_enabled;
-		dev_priv->fbc.enable_fbc = g4x_fbc_enable;
-		dev_priv->fbc.disable_fbc = g4x_fbc_disable;
+		dev_priv->fbc.is_active = g4x_fbc_is_active;
+		dev_priv->fbc.activate = g4x_fbc_activate;
+		dev_priv->fbc.deactivate = g4x_fbc_deactivate;
 	} else {
-		dev_priv->fbc.fbc_enabled = i8xx_fbc_enabled;
-		dev_priv->fbc.enable_fbc = i8xx_fbc_enable;
-		dev_priv->fbc.disable_fbc = i8xx_fbc_disable;
+		dev_priv->fbc.is_active = i8xx_fbc_is_active;
+		dev_priv->fbc.activate = i8xx_fbc_activate;
+		dev_priv->fbc.deactivate = i8xx_fbc_deactivate;
 
 		/* This value was pulled out of someone's hat */
 		I915_WRITE(FBC_CONTROL, 500 << FBC_CTL_INTERVAL_SHIFT);
 	}
 
-	dev_priv->fbc.enabled = dev_priv->fbc.fbc_enabled(dev_priv);
+	/* We still don't have any sort of hardware state readout for FBC, so
+	 * deactivate it in case the BIOS activated it to make sure software
+	 * matches the hardware state. */
+	if (dev_priv->fbc.is_active(dev_priv))
+		dev_priv->fbc.deactivate(dev_priv);
 }
diff --git a/drivers/gpu/drm/i915/intel_fbdev.c b/drivers/gpu/drm/i915/intel_fbdev.c
index 4fd5fdfef6bd..bea75cafc623 100644
--- a/drivers/gpu/drm/i915/intel_fbdev.c
+++ b/drivers/gpu/drm/i915/intel_fbdev.c
@@ -119,7 +119,7 @@ static int intelfb_alloc(struct drm_fb_helper *helper,
 {
 	struct intel_fbdev *ifbdev =
 		container_of(helper, struct intel_fbdev, helper);
-	struct drm_framebuffer *fb;
+	struct drm_framebuffer *fb = NULL;
 	struct drm_device *dev = helper->dev;
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_mode_fb_cmd2 mode_cmd = {};
@@ -138,6 +138,8 @@ static int intelfb_alloc(struct drm_fb_helper *helper,
 	mode_cmd.pixel_format = drm_mode_legacy_fb_format(sizes->surface_bpp,
 							  sizes->surface_depth);
 
+	mutex_lock(&dev->struct_mutex);
+
 	size = mode_cmd.pitches[0] * mode_cmd.height;
 	size = PAGE_ALIGN(size);
 
@@ -156,26 +158,21 @@ static int intelfb_alloc(struct drm_fb_helper *helper,
 
 	fb = __intel_framebuffer_create(dev, &mode_cmd, obj);
 	if (IS_ERR(fb)) {
+		drm_gem_object_unreference(&obj->base);
 		ret = PTR_ERR(fb);
-		goto out_unref;
+		goto out;
 	}
 
-	/* Flush everything out, we'll be doing GTT only from now on */
-	ret = intel_pin_and_fence_fb_obj(NULL, fb, NULL, NULL, NULL);
-	if (ret) {
-		DRM_ERROR("failed to pin obj: %d\n", ret);
-		goto out_fb;
-	}
+	mutex_unlock(&dev->struct_mutex);
 
 	ifbdev->fb = to_intel_framebuffer(fb);
 
 	return 0;
 
-out_fb:
-	drm_framebuffer_remove(fb);
-out_unref:
-	drm_gem_object_unreference(&obj->base);
 out:
+	mutex_unlock(&dev->struct_mutex);
+	if (!IS_ERR_OR_NULL(fb))
+		drm_framebuffer_unreference(fb);
 	return ret;
 }
 
@@ -193,8 +190,6 @@ static int intelfb_create(struct drm_fb_helper *helper,
 	int size, ret;
 	bool prealloc = false;
 
-	mutex_lock(&dev->struct_mutex);
-
 	if (intel_fb &&
 	    (sizes->fb_width > intel_fb->base.width ||
 	     sizes->fb_height > intel_fb->base.height)) {
@@ -209,7 +204,7 @@ static int intelfb_create(struct drm_fb_helper *helper,
 		DRM_DEBUG_KMS("no BIOS fb, allocating a new one\n");
 		ret = intelfb_alloc(helper, sizes);
 		if (ret)
-			goto out_unlock;
+			return ret;
 		intel_fb = ifbdev->fb;
 	} else {
 		DRM_DEBUG_KMS("re-using BIOS fb\n");
@@ -221,8 +216,19 @@ static int intelfb_create(struct drm_fb_helper *helper,
 	obj = intel_fb->obj;
 	size = obj->base.size;
 
+	mutex_lock(&dev->struct_mutex);
+
+	/* Pin the GGTT vma for our access via info->screen_base.
+	 * This also validates that any existing fb inherited from the
+	 * BIOS is suitable for own access.
+	 */
+	ret = intel_pin_and_fence_fb_obj(NULL, &ifbdev->fb->base, NULL);
+	if (ret)
+		goto out_unlock;
+
 	info = drm_fb_helper_alloc_fbi(helper);
 	if (IS_ERR(info)) {
+		DRM_ERROR("Failed to allocate fb_info\n");
 		ret = PTR_ERR(info);
 		goto out_unpin;
 	}
@@ -249,6 +255,7 @@ static int intelfb_create(struct drm_fb_helper *helper,
 		ioremap_wc(dev_priv->gtt.mappable_base + i915_gem_obj_ggtt_offset(obj),
 			   size);
 	if (!info->screen_base) {
+		DRM_ERROR("Failed to remap framebuffer into virtual memory\n");
 		ret = -ENOSPC;
 		goto out_destroy_fbi;
 	}
@@ -281,7 +288,6 @@ out_destroy_fbi:
 	drm_fb_helper_release_fbi(helper);
 out_unpin:
 	i915_gem_object_ggtt_unpin(obj);
-	drm_gem_object_unreference(&obj->base);
 out_unlock:
 	mutex_unlock(&dev->struct_mutex);
 	return ret;
@@ -520,14 +526,20 @@ static const struct drm_fb_helper_funcs intel_fb_helper_funcs = {
 static void intel_fbdev_destroy(struct drm_device *dev,
 				struct intel_fbdev *ifbdev)
 {
+	/* We rely on the object-free to release the VMA pinning for
+	 * the info->screen_base mmaping. Leaking the VMA is simpler than
+	 * trying to rectify all the possible error paths leading here.
+	 */
 
 	drm_fb_helper_unregister_fbi(&ifbdev->helper);
 	drm_fb_helper_release_fbi(&ifbdev->helper);
 
 	drm_fb_helper_fini(&ifbdev->helper);
 
-	drm_framebuffer_unregister_private(&ifbdev->fb->base);
-	drm_framebuffer_remove(&ifbdev->fb->base);
+	if (ifbdev->fb) {
+		drm_framebuffer_unregister_private(&ifbdev->fb->base);
+		drm_framebuffer_remove(&ifbdev->fb->base);
+	}
 }
 
 /*
@@ -702,13 +714,20 @@ int intel_fbdev_init(struct drm_device *dev)
 	return 0;
 }
 
-void intel_fbdev_initial_config(void *data, async_cookie_t cookie)
+static void intel_fbdev_initial_config(void *data, async_cookie_t cookie)
 {
 	struct drm_i915_private *dev_priv = data;
 	struct intel_fbdev *ifbdev = dev_priv->fbdev;
 
 	/* Due to peculiar init order wrt to hpd handling this is separate. */
-	drm_fb_helper_initial_config(&ifbdev->helper, ifbdev->preferred_bpp);
+	if (drm_fb_helper_initial_config(&ifbdev->helper,
+					 ifbdev->preferred_bpp))
+		intel_fbdev_fini(dev_priv->dev);
+}
+
+void intel_fbdev_initial_config_async(struct drm_device *dev)
+{
+	async_schedule(intel_fbdev_initial_config, to_i915(dev));
 }
 
 void intel_fbdev_fini(struct drm_device *dev)
@@ -719,7 +738,8 @@ void intel_fbdev_fini(struct drm_device *dev)
 
 	flush_work(&dev_priv->fbdev_suspend_work);
 
-	async_synchronize_full();
+	if (!current_is_async())
+		async_synchronize_full();
 	intel_fbdev_destroy(dev, dev_priv->fbdev);
 	kfree(dev_priv->fbdev);
 	dev_priv->fbdev = NULL;
diff --git a/drivers/gpu/drm/i915/intel_fifo_underrun.c b/drivers/gpu/drm/i915/intel_fifo_underrun.c
index 54daa66c6970..bda526660e20 100644
--- a/drivers/gpu/drm/i915/intel_fifo_underrun.c
+++ b/drivers/gpu/drm/i915/intel_fifo_underrun.c
@@ -84,38 +84,21 @@ static bool cpt_can_enable_serr_int(struct drm_device *dev)
 	return true;
 }
 
-/**
- * i9xx_check_fifo_underruns - check for fifo underruns
- * @dev_priv: i915 device instance
- *
- * This function checks for fifo underruns on GMCH platforms. This needs to be
- * done manually on modeset to make sure that we catch all underruns since they
- * do not generate an interrupt by themselves on these platforms.
- */
-void i9xx_check_fifo_underruns(struct drm_i915_private *dev_priv)
+static void i9xx_check_fifo_underruns(struct intel_crtc *crtc)
 {
-	struct intel_crtc *crtc;
-
-	spin_lock_irq(&dev_priv->irq_lock);
-
-	for_each_intel_crtc(dev_priv->dev, crtc) {
-		u32 reg = PIPESTAT(crtc->pipe);
-		u32 pipestat;
-
-		if (crtc->cpu_fifo_underrun_disabled)
-			continue;
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
+	i915_reg_t reg = PIPESTAT(crtc->pipe);
+	u32 pipestat = I915_READ(reg) & 0xffff0000;
 
-		pipestat = I915_READ(reg) & 0xffff0000;
-		if ((pipestat & PIPE_FIFO_UNDERRUN_STATUS) == 0)
-			continue;
+	assert_spin_locked(&dev_priv->irq_lock);
 
-		I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
-		POSTING_READ(reg);
+	if ((pipestat & PIPE_FIFO_UNDERRUN_STATUS) == 0)
+		return;
 
-		DRM_ERROR("pipe %c underrun\n", pipe_name(crtc->pipe));
-	}
+	I915_WRITE(reg, pipestat | PIPE_FIFO_UNDERRUN_STATUS);
+	POSTING_READ(reg);
 
-	spin_unlock_irq(&dev_priv->irq_lock);
+	DRM_ERROR("pipe %c underrun\n", pipe_name(crtc->pipe));
 }
 
 static void i9xx_set_fifo_underrun_reporting(struct drm_device *dev,
@@ -123,7 +106,7 @@ static void i9xx_set_fifo_underrun_reporting(struct drm_device *dev,
 					     bool enable, bool old)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 reg = PIPESTAT(pipe);
+	i915_reg_t reg = PIPESTAT(pipe);
 	u32 pipestat = I915_READ(reg) & 0xffff0000;
 
 	assert_spin_locked(&dev_priv->irq_lock);
@@ -145,9 +128,26 @@ static void ironlake_set_fifo_underrun_reporting(struct drm_device *dev,
 					  DE_PIPEB_FIFO_UNDERRUN;
 
 	if (enable)
-		ironlake_enable_display_irq(dev_priv, bit);
+		ilk_enable_display_irq(dev_priv, bit);
 	else
-		ironlake_disable_display_irq(dev_priv, bit);
+		ilk_disable_display_irq(dev_priv, bit);
+}
+
+static void ivybridge_check_fifo_underruns(struct intel_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
+	enum pipe pipe = crtc->pipe;
+	uint32_t err_int = I915_READ(GEN7_ERR_INT);
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	if ((err_int & ERR_INT_FIFO_UNDERRUN(pipe)) == 0)
+		return;
+
+	I915_WRITE(GEN7_ERR_INT, ERR_INT_FIFO_UNDERRUN(pipe));
+	POSTING_READ(GEN7_ERR_INT);
+
+	DRM_ERROR("fifo underrun on pipe %c\n", pipe_name(pipe));
 }
 
 static void ivybridge_set_fifo_underrun_reporting(struct drm_device *dev,
@@ -161,9 +161,9 @@ static void ivybridge_set_fifo_underrun_reporting(struct drm_device *dev,
 		if (!ivb_can_enable_err_int(dev))
 			return;
 
-		ironlake_enable_display_irq(dev_priv, DE_ERR_INT_IVB);
+		ilk_enable_display_irq(dev_priv, DE_ERR_INT_IVB);
 	} else {
-		ironlake_disable_display_irq(dev_priv, DE_ERR_INT_IVB);
+		ilk_disable_display_irq(dev_priv, DE_ERR_INT_IVB);
 
 		if (old &&
 		    I915_READ(GEN7_ERR_INT) & ERR_INT_FIFO_UNDERRUN(pipe)) {
@@ -178,14 +178,10 @@ static void broadwell_set_fifo_underrun_reporting(struct drm_device *dev,
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	assert_spin_locked(&dev_priv->irq_lock);
-
 	if (enable)
-		dev_priv->de_irq_mask[pipe] &= ~GEN8_PIPE_FIFO_UNDERRUN;
+		bdw_enable_pipe_irq(dev_priv, pipe, GEN8_PIPE_FIFO_UNDERRUN);
 	else
-		dev_priv->de_irq_mask[pipe] |= GEN8_PIPE_FIFO_UNDERRUN;
-	I915_WRITE(GEN8_DE_PIPE_IMR(pipe), dev_priv->de_irq_mask[pipe]);
-	POSTING_READ(GEN8_DE_PIPE_IMR(pipe));
+		bdw_disable_pipe_irq(dev_priv, pipe, GEN8_PIPE_FIFO_UNDERRUN);
 }
 
 static void ibx_set_fifo_underrun_reporting(struct drm_device *dev,
@@ -202,6 +198,24 @@ static void ibx_set_fifo_underrun_reporting(struct drm_device *dev,
 		ibx_disable_display_interrupt(dev_priv, bit);
 }
 
+static void cpt_check_pch_fifo_underruns(struct intel_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = to_i915(crtc->base.dev);
+	enum transcoder pch_transcoder = (enum transcoder) crtc->pipe;
+	uint32_t serr_int = I915_READ(SERR_INT);
+
+	assert_spin_locked(&dev_priv->irq_lock);
+
+	if ((serr_int & SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder)) == 0)
+		return;
+
+	I915_WRITE(SERR_INT, SERR_INT_TRANS_FIFO_UNDERRUN(pch_transcoder));
+	POSTING_READ(SERR_INT);
+
+	DRM_ERROR("pch fifo underrun on pch transcoder %c\n",
+		  transcoder_name(pch_transcoder));
+}
+
 static void cpt_set_fifo_underrun_reporting(struct drm_device *dev,
 					    enum transcoder pch_transcoder,
 					    bool enable, bool old)
@@ -375,3 +389,56 @@ void intel_pch_fifo_underrun_irq_handler(struct drm_i915_private *dev_priv,
 		DRM_ERROR("PCH transcoder %c FIFO underrun\n",
 			  transcoder_name(pch_transcoder));
 }
+
+/**
+ * intel_check_cpu_fifo_underruns - check for CPU fifo underruns immediately
+ * @dev_priv: i915 device instance
+ *
+ * Check for CPU fifo underruns immediately. Useful on IVB/HSW where the shared
+ * error interrupt may have been disabled, and so CPU fifo underruns won't
+ * necessarily raise an interrupt, and on GMCH platforms where underruns never
+ * raise an interrupt.
+ */
+void intel_check_cpu_fifo_underruns(struct drm_i915_private *dev_priv)
+{
+	struct intel_crtc *crtc;
+
+	spin_lock_irq(&dev_priv->irq_lock);
+
+	for_each_intel_crtc(dev_priv->dev, crtc) {
+		if (crtc->cpu_fifo_underrun_disabled)
+			continue;
+
+		if (HAS_GMCH_DISPLAY(dev_priv))
+			i9xx_check_fifo_underruns(crtc);
+		else if (IS_GEN7(dev_priv))
+			ivybridge_check_fifo_underruns(crtc);
+	}
+
+	spin_unlock_irq(&dev_priv->irq_lock);
+}
+
+/**
+ * intel_check_pch_fifo_underruns - check for PCH fifo underruns immediately
+ * @dev_priv: i915 device instance
+ *
+ * Check for PCH fifo underruns immediately. Useful on CPT/PPT where the shared
+ * error interrupt may have been disabled, and so PCH fifo underruns won't
+ * necessarily raise an interrupt.
+ */
+void intel_check_pch_fifo_underruns(struct drm_i915_private *dev_priv)
+{
+	struct intel_crtc *crtc;
+
+	spin_lock_irq(&dev_priv->irq_lock);
+
+	for_each_intel_crtc(dev_priv->dev, crtc) {
+		if (crtc->pch_fifo_underrun_disabled)
+			continue;
+
+		if (HAS_PCH_CPT(dev_priv))
+			cpt_check_pch_fifo_underruns(crtc);
+	}
+
+	spin_unlock_irq(&dev_priv->irq_lock);
+}
diff --git a/drivers/gpu/drm/i915/intel_guc.h b/drivers/gpu/drm/i915/intel_guc.h
index 081d5f648d26..822952235dcf 100644
--- a/drivers/gpu/drm/i915/intel_guc.h
+++ b/drivers/gpu/drm/i915/intel_guc.h
@@ -42,8 +42,6 @@ struct i915_guc_client {
 
 	uint32_t wq_offset;
 	uint32_t wq_size;
-
-	spinlock_t wq_lock;		/* Protects all data below	*/
 	uint32_t wq_tail;
 
 	/* GuC submission statistics & status */
@@ -76,11 +74,17 @@ struct intel_guc_fw {
 	uint16_t			guc_fw_minor_wanted;
 	uint16_t			guc_fw_major_found;
 	uint16_t			guc_fw_minor_found;
+
+	uint32_t header_size;
+	uint32_t header_offset;
+	uint32_t rsa_size;
+	uint32_t rsa_offset;
+	uint32_t ucode_size;
+	uint32_t ucode_offset;
 };
 
 struct intel_guc {
 	struct intel_guc_fw guc_fw;
-
 	uint32_t log_flags;
 	struct drm_i915_gem_object *log_obj;
 
@@ -89,8 +93,6 @@ struct intel_guc {
 
 	struct i915_guc_client *execbuf_client;
 
-	spinlock_t host2guc_lock;	/* Protects all data below	*/
-
 	DECLARE_BITMAP(doorbell_bitmap, GUC_MAX_DOORBELLS);
 	uint32_t db_cacheline;		/* Cyclic counter mod pagesize	*/
 
diff --git a/drivers/gpu/drm/i915/intel_guc_fwif.h b/drivers/gpu/drm/i915/intel_guc_fwif.h
index 593d2f585978..40b2ea572e16 100644
--- a/drivers/gpu/drm/i915/intel_guc_fwif.h
+++ b/drivers/gpu/drm/i915/intel_guc_fwif.h
@@ -122,6 +122,78 @@
 
 #define GUC_CTL_MAX_DWORDS		(GUC_CTL_RSRVD + 1)
 
+/**
+ * DOC: GuC Firmware Layout
+ *
+ * The GuC firmware layout looks like this:
+ *
+ *     +-------------------------------+
+ *     |        guc_css_header         |
+ *     | contains major/minor version  |
+ *     +-------------------------------+
+ *     |             uCode             |
+ *     +-------------------------------+
+ *     |         RSA signature         |
+ *     +-------------------------------+
+ *     |          modulus key          |
+ *     +-------------------------------+
+ *     |          exponent val         |
+ *     +-------------------------------+
+ *
+ * The firmware may or may not have modulus key and exponent data. The header,
+ * uCode and RSA signature are must-have components that will be used by driver.
+ * Length of each components, which is all in dwords, can be found in header.
+ * In the case that modulus and exponent are not present in fw, a.k.a truncated
+ * image, the length value still appears in header.
+ *
+ * Driver will do some basic fw size validation based on the following rules:
+ *
+ * 1. Header, uCode and RSA are must-have components.
+ * 2. All firmware components, if they present, are in the sequence illustrated
+ * in the layout table above.
+ * 3. Length info of each component can be found in header, in dwords.
+ * 4. Modulus and exponent key are not required by driver. They may not appear
+ * in fw. So driver will load a truncated firmware in this case.
+ */
+
+struct guc_css_header {
+	uint32_t module_type;
+	/* header_size includes all non-uCode bits, including css_header, rsa
+	 * key, modulus key and exponent data. */
+	uint32_t header_size_dw;
+	uint32_t header_version;
+	uint32_t module_id;
+	uint32_t module_vendor;
+	union {
+		struct {
+			uint8_t day;
+			uint8_t month;
+			uint16_t year;
+		};
+		uint32_t date;
+	};
+	uint32_t size_dw; /* uCode plus header_size_dw */
+	uint32_t key_size_dw;
+	uint32_t modulus_size_dw;
+	uint32_t exponent_size_dw;
+	union {
+		struct {
+			uint8_t hour;
+			uint8_t min;
+			uint16_t sec;
+		};
+		uint32_t time;
+	};
+
+	char username[8];
+	char buildnumber[12];
+	uint32_t device_id;
+	uint32_t guc_sw_version;
+	uint32_t prod_preprod_fw;
+	uint32_t reserved[12];
+	uint32_t header_info;
+} __packed;
+
 struct guc_doorbell_info {
 	u32 db_status;
 	u32 cookie;
diff --git a/drivers/gpu/drm/i915/intel_guc_loader.c b/drivers/gpu/drm/i915/intel_guc_loader.c
index 3541f76c65a7..550921f2ef7d 100644
--- a/drivers/gpu/drm/i915/intel_guc_loader.c
+++ b/drivers/gpu/drm/i915/intel_guc_loader.c
@@ -31,7 +31,7 @@
 #include "intel_guc.h"
 
 /**
- * DOC: GuC
+ * DOC: GuC-specific firmware loader
  *
  * intel_guc:
  * Top level structure of guc. It handles firmware loading and manages client
@@ -208,16 +208,6 @@ static inline bool guc_ucode_response(struct drm_i915_private *dev_priv,
 /*
  * Transfer the firmware image to RAM for execution by the microcontroller.
  *
- * GuC Firmware layout:
- * +-------------------------------+  ----
- * |          CSS header           |  128B
- * | contains major/minor version  |
- * +-------------------------------+  ----
- * |             uCode             |
- * +-------------------------------+  ----
- * |         RSA signature         |  256B
- * +-------------------------------+  ----
- *
  * Architecturally, the DMA engine is bidirectional, and can potentially even
  * transfer between GTT locations. This functionality is left out of the API
  * for now as there is no need for it.
@@ -225,33 +215,29 @@ static inline bool guc_ucode_response(struct drm_i915_private *dev_priv,
  * Note that GuC needs the CSS header plus uKernel code to be copied by the
  * DMA engine in one operation, whereas the RSA signature is loaded via MMIO.
  */
-
-#define UOS_CSS_HEADER_OFFSET		0
-#define UOS_VER_MINOR_OFFSET		0x44
-#define UOS_VER_MAJOR_OFFSET		0x46
-#define UOS_CSS_HEADER_SIZE		0x80
-#define UOS_RSA_SIG_SIZE		0x100
-
 static int guc_ucode_xfer_dma(struct drm_i915_private *dev_priv)
 {
 	struct intel_guc_fw *guc_fw = &dev_priv->guc.guc_fw;
 	struct drm_i915_gem_object *fw_obj = guc_fw->guc_fw_obj;
 	unsigned long offset;
 	struct sg_table *sg = fw_obj->pages;
-	u32 status, ucode_size, rsa[UOS_RSA_SIG_SIZE / sizeof(u32)];
+	u32 status, rsa[UOS_RSA_SCRATCH_MAX_COUNT];
 	int i, ret = 0;
 
-	/* uCode size, also is where RSA signature starts */
-	offset = ucode_size = guc_fw->guc_fw_size - UOS_RSA_SIG_SIZE;
-	I915_WRITE(DMA_COPY_SIZE, ucode_size);
+	/* where RSA signature starts */
+	offset = guc_fw->rsa_offset;
 
 	/* Copy RSA signature from the fw image to HW for verification */
-	sg_pcopy_to_buffer(sg->sgl, sg->nents, rsa, UOS_RSA_SIG_SIZE, offset);
-	for (i = 0; i < UOS_RSA_SIG_SIZE / sizeof(u32); i++)
+	sg_pcopy_to_buffer(sg->sgl, sg->nents, rsa, sizeof(rsa), offset);
+	for (i = 0; i < UOS_RSA_SCRATCH_MAX_COUNT; i++)
 		I915_WRITE(UOS_RSA_SCRATCH(i), rsa[i]);
 
+	/* The header plus uCode will be copied to WOPCM via DMA, excluding any
+	 * other components */
+	I915_WRITE(DMA_COPY_SIZE, guc_fw->header_size + guc_fw->ucode_size);
+
 	/* Set the source address for the new blob */
-	offset = i915_gem_obj_ggtt_offset(fw_obj);
+	offset = i915_gem_obj_ggtt_offset(fw_obj) + guc_fw->header_offset;
 	I915_WRITE(DMA_ADDR_0_LOW, lower_32_bits(offset));
 	I915_WRITE(DMA_ADDR_0_HIGH, upper_32_bits(offset) & 0xFFFF);
 
@@ -322,8 +308,8 @@ static int guc_ucode_xfer(struct drm_i915_private *dev_priv)
 	I915_WRITE(GUC_SHIM_CONTROL, GUC_SHIM_CONTROL_VALUE);
 
 	/* WaDisableMinuteIaClockGating:skl,bxt */
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) <= SKL_REVID_B0) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) == BXT_REVID_A0)) {
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		I915_WRITE(GUC_SHIM_CONTROL, (I915_READ(GUC_SHIM_CONTROL) &
 					      ~GUC_ENABLE_MIA_CLOCK_GATING));
 	}
@@ -378,6 +364,9 @@ int intel_guc_ucode_load(struct drm_device *dev)
 	struct intel_guc_fw *guc_fw = &dev_priv->guc.guc_fw;
 	int err = 0;
 
+	if (!i915.enable_guc_submission)
+		return 0;
+
 	DRM_DEBUG_DRIVER("GuC fw status: fetch %s, load %s\n",
 		intel_guc_fw_status_repr(guc_fw->guc_fw_fetch_status),
 		intel_guc_fw_status_repr(guc_fw->guc_fw_load_status));
@@ -457,10 +446,8 @@ static void guc_fw_fetch(struct drm_device *dev, struct intel_guc_fw *guc_fw)
 {
 	struct drm_i915_gem_object *obj;
 	const struct firmware *fw;
-	const u8 *css_header;
-	const size_t minsize = UOS_CSS_HEADER_SIZE + UOS_RSA_SIG_SIZE;
-	const size_t maxsize = GUC_WOPCM_SIZE_VALUE + UOS_RSA_SIG_SIZE
-			- 0x8000; /* 32k reserved (8K stack + 24k context) */
+	struct guc_css_header *css;
+	size_t size;
 	int err;
 
 	DRM_DEBUG_DRIVER("before requesting firmware: GuC fw fetch status %s\n",
@@ -474,12 +461,52 @@ static void guc_fw_fetch(struct drm_device *dev, struct intel_guc_fw *guc_fw)
 
 	DRM_DEBUG_DRIVER("fetch GuC fw from %s succeeded, fw %p\n",
 		guc_fw->guc_fw_path, fw);
-	DRM_DEBUG_DRIVER("firmware file size %zu (minimum %zu, maximum %zu)\n",
-		fw->size, minsize, maxsize);
 
-	/* Check the size of the blob befoe examining buffer contents */
-	if (fw->size < minsize || fw->size > maxsize)
+	/* Check the size of the blob before examining buffer contents */
+	if (fw->size < sizeof(struct guc_css_header)) {
+		DRM_ERROR("Firmware header is missing\n");
 		goto fail;
+	}
+
+	css = (struct guc_css_header *)fw->data;
+
+	/* Firmware bits always start from header */
+	guc_fw->header_offset = 0;
+	guc_fw->header_size = (css->header_size_dw - css->modulus_size_dw -
+		css->key_size_dw - css->exponent_size_dw) * sizeof(u32);
+
+	if (guc_fw->header_size != sizeof(struct guc_css_header)) {
+		DRM_ERROR("CSS header definition mismatch\n");
+		goto fail;
+	}
+
+	/* then, uCode */
+	guc_fw->ucode_offset = guc_fw->header_offset + guc_fw->header_size;
+	guc_fw->ucode_size = (css->size_dw - css->header_size_dw) * sizeof(u32);
+
+	/* now RSA */
+	if (css->key_size_dw != UOS_RSA_SCRATCH_MAX_COUNT) {
+		DRM_ERROR("RSA key size is bad\n");
+		goto fail;
+	}
+	guc_fw->rsa_offset = guc_fw->ucode_offset + guc_fw->ucode_size;
+	guc_fw->rsa_size = css->key_size_dw * sizeof(u32);
+
+	/* At least, it should have header, uCode and RSA. Size of all three. */
+	size = guc_fw->header_size + guc_fw->ucode_size + guc_fw->rsa_size;
+	if (fw->size < size) {
+		DRM_ERROR("Missing firmware components\n");
+		goto fail;
+	}
+
+	/* Header and uCode will be loaded to WOPCM. Size of the two. */
+	size = guc_fw->header_size + guc_fw->ucode_size;
+
+	/* Top 32k of WOPCM is reserved (8K stack + 24k RC6 context). */
+	if (size > GUC_WOPCM_SIZE_VALUE - 0x8000) {
+		DRM_ERROR("Firmware is too large to fit in WOPCM\n");
+		goto fail;
+	}
 
 	/*
 	 * The GuC firmware image has the version number embedded at a well-known
@@ -487,9 +514,8 @@ static void guc_fw_fetch(struct drm_device *dev, struct intel_guc_fw *guc_fw)
 	 * TWO bytes each (i.e. u16), although all pointers and offsets are defined
 	 * in terms of bytes (u8).
 	 */
-	css_header = fw->data + UOS_CSS_HEADER_OFFSET;
-	guc_fw->guc_fw_major_found = *(u16 *)(css_header + UOS_VER_MAJOR_OFFSET);
-	guc_fw->guc_fw_minor_found = *(u16 *)(css_header + UOS_VER_MINOR_OFFSET);
+	guc_fw->guc_fw_major_found = css->guc_sw_version >> 16;
+	guc_fw->guc_fw_minor_found = css->guc_sw_version & 0xFFFF;
 
 	if (guc_fw->guc_fw_major_found != guc_fw->guc_fw_major_wanted ||
 	    guc_fw->guc_fw_minor_found < guc_fw->guc_fw_minor_wanted) {
@@ -566,6 +592,9 @@ void intel_guc_ucode_init(struct drm_device *dev)
 		fw_path = "";	/* unknown device */
 	}
 
+	if (!i915.enable_guc_submission)
+		return;
+
 	guc_fw->guc_dev = dev;
 	guc_fw->guc_fw_path = fw_path;
 	guc_fw->guc_fw_fetch_status = GUC_FIRMWARE_NONE;
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index e6c035b0fc1c..616108c4bc3e 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -78,7 +78,7 @@ static u32 g4x_infoframe_index(enum hdmi_infoframe_type type)
 	case HDMI_INFOFRAME_TYPE_VENDOR:
 		return VIDEO_DIP_SELECT_VENDOR;
 	default:
-		DRM_DEBUG_DRIVER("unknown info frame type %d\n", type);
+		MISSING_CASE(type);
 		return 0;
 	}
 }
@@ -93,7 +93,7 @@ static u32 g4x_infoframe_enable(enum hdmi_infoframe_type type)
 	case HDMI_INFOFRAME_TYPE_VENDOR:
 		return VIDEO_DIP_ENABLE_VENDOR;
 	default:
-		DRM_DEBUG_DRIVER("unknown info frame type %d\n", type);
+		MISSING_CASE(type);
 		return 0;
 	}
 }
@@ -108,15 +108,16 @@ static u32 hsw_infoframe_enable(enum hdmi_infoframe_type type)
 	case HDMI_INFOFRAME_TYPE_VENDOR:
 		return VIDEO_DIP_ENABLE_VS_HSW;
 	default:
-		DRM_DEBUG_DRIVER("unknown info frame type %d\n", type);
+		MISSING_CASE(type);
 		return 0;
 	}
 }
 
-static u32 hsw_dip_data_reg(struct drm_i915_private *dev_priv,
-			    enum transcoder cpu_transcoder,
-			    enum hdmi_infoframe_type type,
-			    int i)
+static i915_reg_t
+hsw_dip_data_reg(struct drm_i915_private *dev_priv,
+		 enum transcoder cpu_transcoder,
+		 enum hdmi_infoframe_type type,
+		 int i)
 {
 	switch (type) {
 	case HDMI_INFOFRAME_TYPE_AVI:
@@ -126,8 +127,8 @@ static u32 hsw_dip_data_reg(struct drm_i915_private *dev_priv,
 	case HDMI_INFOFRAME_TYPE_VENDOR:
 		return HSW_TVIDEO_DIP_VS_DATA(cpu_transcoder, i);
 	default:
-		DRM_DEBUG_DRIVER("unknown info frame type %d\n", type);
-		return 0;
+		MISSING_CASE(type);
+		return INVALID_MMIO_REG;
 	}
 }
 
@@ -168,10 +169,10 @@ static void g4x_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(VIDEO_DIP_CTL);
 }
 
-static bool g4x_infoframe_enabled(struct drm_encoder *encoder)
+static bool g4x_infoframe_enabled(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	u32 val = I915_READ(VIDEO_DIP_CTL);
 
@@ -193,8 +194,9 @@ static void ibx_write_infoframe(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	int i, reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
+	int i;
 
 	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
@@ -223,13 +225,13 @@ static void ibx_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
-static bool ibx_infoframe_enabled(struct drm_encoder *encoder)
+static bool ibx_infoframe_enabled(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
-	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	enum pipe pipe = to_intel_crtc(pipe_config->base.crtc)->pipe;
+	i915_reg_t reg = TVIDEO_DIP_CTL(pipe);
 	u32 val = I915_READ(reg);
 
 	if ((val & VIDEO_DIP_ENABLE) == 0)
@@ -251,8 +253,9 @@ static void cpt_write_infoframe(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	int i, reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
+	int i;
 
 	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
@@ -284,13 +287,12 @@ static void cpt_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
-static bool cpt_infoframe_enabled(struct drm_encoder *encoder)
+static bool cpt_infoframe_enabled(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	int reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
-	u32 val = I915_READ(reg);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
+	enum pipe pipe = to_intel_crtc(pipe_config->base.crtc)->pipe;
+	u32 val = I915_READ(TVIDEO_DIP_CTL(pipe));
 
 	if ((val & VIDEO_DIP_ENABLE) == 0)
 		return false;
@@ -308,8 +310,9 @@ static void vlv_write_infoframe(struct drm_encoder *encoder,
 	struct drm_device *dev = encoder->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	int i, reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
+	int i;
 
 	WARN(!(val & VIDEO_DIP_ENABLE), "Writing DIP with CTL reg disabled\n");
 
@@ -338,14 +341,13 @@ static void vlv_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(reg);
 }
 
-static bool vlv_infoframe_enabled(struct drm_encoder *encoder)
+static bool vlv_infoframe_enabled(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
-	int reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
-	u32 val = I915_READ(reg);
+	enum pipe pipe = to_intel_crtc(pipe_config->base.crtc)->pipe;
+	u32 val = I915_READ(VLV_TVIDEO_DIP_CTL(pipe));
 
 	if ((val & VIDEO_DIP_ENABLE) == 0)
 		return false;
@@ -367,14 +369,12 @@ static void hsw_write_infoframe(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	enum transcoder cpu_transcoder = intel_crtc->config->cpu_transcoder;
-	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(cpu_transcoder);
-	u32 data_reg;
+	i915_reg_t ctl_reg = HSW_TVIDEO_DIP_CTL(cpu_transcoder);
+	i915_reg_t data_reg;
 	int i;
 	u32 val = I915_READ(ctl_reg);
 
 	data_reg = hsw_dip_data_reg(dev_priv, cpu_transcoder, type, 0);
-	if (data_reg == 0)
-		return;
 
 	val &= ~hsw_infoframe_enable(type);
 	I915_WRITE(ctl_reg, val);
@@ -396,13 +396,11 @@ static void hsw_write_infoframe(struct drm_encoder *encoder,
 	POSTING_READ(ctl_reg);
 }
 
-static bool hsw_infoframe_enabled(struct drm_encoder *encoder)
+static bool hsw_infoframe_enabled(struct drm_encoder *encoder,
+				  const struct intel_crtc_state *pipe_config)
 {
-	struct drm_device *dev = encoder->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
-	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config->cpu_transcoder);
-	u32 val = I915_READ(ctl_reg);
+	struct drm_i915_private *dev_priv = to_i915(encoder->dev);
+	u32 val = I915_READ(HSW_TVIDEO_DIP_CTL(pipe_config->cpu_transcoder));
 
 	return val & (VIDEO_DIP_ENABLE_VSC_HSW | VIDEO_DIP_ENABLE_AVI_HSW |
 		      VIDEO_DIP_ENABLE_GCP_HSW | VIDEO_DIP_ENABLE_VS_HSW |
@@ -513,7 +511,7 @@ static void g4x_set_infoframes(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_hdmi *intel_hdmi = &intel_dig_port->hdmi;
-	u32 reg = VIDEO_DIP_CTL;
+	i915_reg_t reg = VIDEO_DIP_CTL;
 	u32 val = I915_READ(reg);
 	u32 port = VIDEO_DIP_PORT(intel_dig_port->port);
 
@@ -633,11 +631,12 @@ static bool intel_hdmi_set_gcp_infoframe(struct drm_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
 	struct intel_crtc *crtc = to_intel_crtc(encoder->crtc);
-	u32 reg, val = 0;
+	i915_reg_t reg;
+	u32 val = 0;
 
 	if (HAS_DDI(dev_priv))
 		reg = HSW_TVIDEO_DIP_GCP(crtc->config->cpu_transcoder);
-	else if (IS_VALLEYVIEW(dev_priv))
+	else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		reg = VLV_TVIDEO_DIP_GCP(crtc->pipe);
 	else if (HAS_PCH_SPLIT(dev_priv->dev))
 		reg = TVIDEO_DIP_GCP(crtc->pipe);
@@ -666,7 +665,7 @@ static void ibx_set_infoframes(struct drm_encoder *encoder,
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_hdmi *intel_hdmi = &intel_dig_port->hdmi;
-	u32 reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
 	u32 port = VIDEO_DIP_PORT(intel_dig_port->port);
 
@@ -717,7 +716,7 @@ static void cpt_set_infoframes(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
 
 	assert_hdmi_port_disabled(intel_hdmi);
@@ -760,7 +759,7 @@ static void vlv_set_infoframes(struct drm_encoder *encoder,
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
+	i915_reg_t reg = VLV_TVIDEO_DIP_CTL(intel_crtc->pipe);
 	u32 val = I915_READ(reg);
 	u32 port = VIDEO_DIP_PORT(intel_dig_port->port);
 
@@ -811,7 +810,7 @@ static void hsw_set_infoframes(struct drm_encoder *encoder,
 	struct drm_i915_private *dev_priv = encoder->dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->crtc);
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(encoder);
-	u32 reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config->cpu_transcoder);
+	i915_reg_t reg = HSW_TVIDEO_DIP_CTL(intel_crtc->config->cpu_transcoder);
 	u32 val = I915_READ(reg);
 
 	assert_hdmi_port_disabled(intel_hdmi);
@@ -881,15 +880,18 @@ static bool intel_hdmi_get_hw_state(struct intel_encoder *encoder,
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
 	enum intel_display_power_domain power_domain;
 	u32 tmp;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	tmp = I915_READ(intel_hdmi->hdmi_reg);
 
 	if (!(tmp & SDVO_ENABLE))
-		return false;
+		goto out;
 
 	if (HAS_PCH_CPT(dev))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
@@ -898,7 +900,12 @@ static bool intel_hdmi_get_hw_state(struct intel_encoder *encoder,
 	else
 		*pipe = PORT_TO_PIPE(tmp);
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void intel_hdmi_get_config(struct intel_encoder *encoder,
@@ -925,7 +932,7 @@ static void intel_hdmi_get_config(struct intel_encoder *encoder,
 	if (tmp & HDMI_MODE_SELECT_HDMI)
 		pipe_config->has_hdmi_sink = true;
 
-	if (intel_hdmi->infoframe_enabled(&encoder->base))
+	if (intel_hdmi->infoframe_enabled(&encoder->base, pipe_config))
 		pipe_config->has_infoframe = true;
 
 	if (tmp & SDVO_AUDIO_ENABLE)
@@ -1108,6 +1115,13 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 	 * matching DP port to be enabled on transcoder A.
 	 */
 	if (HAS_PCH_IBX(dev) && crtc->pipe == PIPE_B) {
+		/*
+		 * We get CPU/PCH FIFO underruns on the other pipe when
+		 * doing the workaround. Sweep them under the rug.
+		 */
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+
 		temp &= ~SDVO_PIPE_B_SELECT;
 		temp |= SDVO_ENABLE;
 		/*
@@ -1122,6 +1136,10 @@ static void intel_disable_hdmi(struct intel_encoder *encoder)
 		temp &= ~SDVO_ENABLE;
 		I915_WRITE(intel_hdmi->hdmi_reg, temp);
 		POSTING_READ(intel_hdmi->hdmi_reg);
+
+		intel_wait_for_vblank_if_active(dev_priv->dev, PIPE_A);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, true);
 	}
 
 	intel_hdmi->set_infoframes(&encoder->base, false, NULL);
@@ -1338,14 +1356,15 @@ intel_hdmi_set_edid(struct drm_connector *connector, bool force)
 	struct edid *edid = NULL;
 	bool connected = false;
 
-	intel_display_power_get(dev_priv, POWER_DOMAIN_GMBUS);
+	if (force) {
+		intel_display_power_get(dev_priv, POWER_DOMAIN_GMBUS);
 
-	if (force)
 		edid = drm_get_edid(connector,
 				    intel_gmbus_get_adapter(dev_priv,
 				    intel_hdmi->ddc_bus));
 
-	intel_display_power_put(dev_priv, POWER_DOMAIN_GMBUS);
+		intel_display_power_put(dev_priv, POWER_DOMAIN_GMBUS);
+	}
 
 	to_intel_connector(connector)->detect_edid = edid;
 	if (edid && edid->input & DRM_EDID_INPUT_DIGITAL) {
@@ -2040,7 +2059,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 		 * On BXT A0/A1, sw needs to activate DDIA HPD logic and
 		 * interrupts to check the external panel connection.
 		 */
-		if (IS_BROXTON(dev_priv) && (INTEL_REVID(dev) < BXT_REVID_B0))
+		if (IS_BXT_REVID(dev_priv, 0, BXT_REVID_A1))
 			intel_encoder->hpd_pin = HPD_PORT_A;
 		else
 			intel_encoder->hpd_pin = HPD_PORT_B;
@@ -2088,7 +2107,7 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 		BUG();
 	}
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		intel_hdmi->write_infoframe = vlv_write_infoframe;
 		intel_hdmi->set_infoframes = vlv_set_infoframes;
 		intel_hdmi->infoframe_enabled = vlv_infoframe_enabled;
@@ -2132,7 +2151,8 @@ void intel_hdmi_init_connector(struct intel_digital_port *intel_dig_port,
 	}
 }
 
-void intel_hdmi_init(struct drm_device *dev, int hdmi_reg, enum port port)
+void intel_hdmi_init(struct drm_device *dev,
+		     i915_reg_t hdmi_reg, enum port port)
 {
 	struct intel_digital_port *intel_dig_port;
 	struct intel_encoder *intel_encoder;
@@ -2151,7 +2171,7 @@ void intel_hdmi_init(struct drm_device *dev, int hdmi_reg, enum port port)
 	intel_encoder = &intel_dig_port->base;
 
 	drm_encoder_init(dev, &intel_encoder->base, &intel_hdmi_enc_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	intel_encoder->compute_config = intel_hdmi_compute_config;
 	if (HAS_PCH_SPLIT(dev)) {
@@ -2203,7 +2223,7 @@ void intel_hdmi_init(struct drm_device *dev, int hdmi_reg, enum port port)
 
 	intel_dig_port->port = port;
 	intel_dig_port->hdmi.hdmi_reg = hdmi_reg;
-	intel_dig_port->dp.output_reg = 0;
+	intel_dig_port->dp.output_reg = INVALID_MMIO_REG;
 
 	intel_hdmi_init_connector(intel_dig_port, intel_connector);
 }
diff --git a/drivers/gpu/drm/i915/intel_hotplug.c b/drivers/gpu/drm/i915/intel_hotplug.c
index b17785719598..bee673005d48 100644
--- a/drivers/gpu/drm/i915/intel_hotplug.c
+++ b/drivers/gpu/drm/i915/intel_hotplug.c
@@ -407,7 +407,7 @@ void intel_hpd_irq_handler(struct drm_device *dev,
 			 * hotplug bits itself. So only WARN about unexpected
 			 * interrupts on saner platforms.
 			 */
-			WARN_ONCE(INTEL_INFO(dev)->gen >= 5 && !IS_VALLEYVIEW(dev),
+			WARN_ONCE(!HAS_GMCH_DISPLAY(dev),
 				  "Received HPD interrupt on pin %d although disabled\n", i);
 			continue;
 		}
@@ -468,9 +468,14 @@ void intel_hpd_init(struct drm_i915_private *dev_priv)
 	list_for_each_entry(connector, &mode_config->connector_list, head) {
 		struct intel_connector *intel_connector = to_intel_connector(connector);
 		connector->polled = intel_connector->polled;
-		if (connector->encoder && !connector->polled && I915_HAS_HOTPLUG(dev) && intel_connector->encoder->hpd_pin > HPD_NONE)
-			connector->polled = DRM_CONNECTOR_POLL_HPD;
+
+		/* MST has a dynamic intel_connector->encoder and it's reprobing
+		 * is all handled by the MST helpers. */
 		if (intel_connector->mst_port)
+			continue;
+
+		if (!connector->polled && I915_HAS_HOTPLUG(dev) &&
+		    intel_connector->encoder->hpd_pin > HPD_NONE)
 			connector->polled = DRM_CONNECTOR_POLL_HPD;
 	}
 
diff --git a/drivers/gpu/drm/i915/intel_i2c.c b/drivers/gpu/drm/i915/intel_i2c.c
index 8324654037b6..52fbe530fc9e 100644
--- a/drivers/gpu/drm/i915/intel_i2c.c
+++ b/drivers/gpu/drm/i915/intel_i2c.c
@@ -36,7 +36,7 @@
 
 struct gmbus_pin {
 	const char *name;
-	int reg;
+	i915_reg_t reg;
 };
 
 /* Map gmbus pin pairs to names and registers. */
@@ -63,9 +63,9 @@ static const struct gmbus_pin gmbus_pins_skl[] = {
 };
 
 static const struct gmbus_pin gmbus_pins_bxt[] = {
-	[GMBUS_PIN_1_BXT] = { "dpb", PCH_GPIOB },
-	[GMBUS_PIN_2_BXT] = { "dpc", PCH_GPIOC },
-	[GMBUS_PIN_3_BXT] = { "misc", PCH_GPIOD },
+	[GMBUS_PIN_1_BXT] = { "dpb", GPIOB },
+	[GMBUS_PIN_2_BXT] = { "dpc", GPIOC },
+	[GMBUS_PIN_3_BXT] = { "misc", GPIOD },
 };
 
 /* pin is expected to be valid */
@@ -74,7 +74,7 @@ static const struct gmbus_pin *get_gmbus_pin(struct drm_i915_private *dev_priv,
 {
 	if (IS_BROXTON(dev_priv))
 		return &gmbus_pins_bxt[pin];
-	else if (IS_SKYLAKE(dev_priv))
+	else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		return &gmbus_pins_skl[pin];
 	else if (IS_BROADWELL(dev_priv))
 		return &gmbus_pins_bdw[pin];
@@ -89,14 +89,15 @@ bool intel_gmbus_is_valid_pin(struct drm_i915_private *dev_priv,
 
 	if (IS_BROXTON(dev_priv))
 		size = ARRAY_SIZE(gmbus_pins_bxt);
-	else if (IS_SKYLAKE(dev_priv))
+	else if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
 		size = ARRAY_SIZE(gmbus_pins_skl);
 	else if (IS_BROADWELL(dev_priv))
 		size = ARRAY_SIZE(gmbus_pins_bdw);
 	else
 		size = ARRAY_SIZE(gmbus_pins);
 
-	return pin < size && get_gmbus_pin(dev_priv, pin)->reg;
+	return pin < size &&
+		i915_mmio_reg_valid(get_gmbus_pin(dev_priv, pin)->reg);
 }
 
 /* Intel GPIO access functions */
@@ -240,9 +241,8 @@ intel_gpio_setup(struct intel_gmbus *bus, unsigned int pin)
 
 	algo = &bus->bit_algo;
 
-	bus->gpio_reg = dev_priv->gpio_mmio_base +
-		get_gmbus_pin(dev_priv, pin)->reg;
-
+	bus->gpio_reg = _MMIO(dev_priv->gpio_mmio_base +
+			      i915_mmio_reg_offset(get_gmbus_pin(dev_priv, pin)->reg));
 	bus->adapter.algo_data = algo;
 	algo->setsda = set_data;
 	algo->setscl = set_clock;
@@ -472,9 +472,7 @@ gmbus_xfer_index_read(struct drm_i915_private *dev_priv, struct i2c_msg *msgs)
 }
 
 static int
-gmbus_xfer(struct i2c_adapter *adapter,
-	   struct i2c_msg *msgs,
-	   int num)
+do_gmbus_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs, int num)
 {
 	struct intel_gmbus *bus = container_of(adapter,
 					       struct intel_gmbus,
@@ -483,14 +481,6 @@ gmbus_xfer(struct i2c_adapter *adapter,
 	int i = 0, inc, try = 0;
 	int ret = 0;
 
-	intel_display_power_get(dev_priv, POWER_DOMAIN_GMBUS);
-	mutex_lock(&dev_priv->gmbus_mutex);
-
-	if (bus->force_bit) {
-		ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
-		goto out;
-	}
-
 retry:
 	I915_WRITE(GMBUS0, bus->reg0);
 
@@ -505,17 +495,13 @@ retry:
 			ret = gmbus_xfer_write(dev_priv, &msgs[i]);
 		}
 
+		if (!ret)
+			ret = gmbus_wait_hw_status(dev_priv, GMBUS_HW_WAIT_PHASE,
+						   GMBUS_HW_WAIT_EN);
 		if (ret == -ETIMEDOUT)
 			goto timeout;
-		if (ret == -ENXIO)
-			goto clear_err;
-
-		ret = gmbus_wait_hw_status(dev_priv, GMBUS_HW_WAIT_PHASE,
-					   GMBUS_HW_WAIT_EN);
-		if (ret == -ENXIO)
+		else if (ret)
 			goto clear_err;
-		if (ret)
-			goto timeout;
 	}
 
 	/* Generate a STOP condition on the bus. Note that gmbus can't generata
@@ -589,13 +575,34 @@ timeout:
 		 bus->adapter.name, bus->reg0 & 0xff);
 	I915_WRITE(GMBUS0, 0);
 
-	/* Hardware may not support GMBUS over these pins? Try GPIO bitbanging instead. */
+	/*
+	 * Hardware may not support GMBUS over these pins? Try GPIO bitbanging
+	 * instead. Use EAGAIN to have i2c core retry.
+	 */
 	bus->force_bit = 1;
-	ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
+	ret = -EAGAIN;
 
 out:
-	mutex_unlock(&dev_priv->gmbus_mutex);
+	return ret;
+}
 
+static int
+gmbus_xfer(struct i2c_adapter *adapter, struct i2c_msg *msgs, int num)
+{
+	struct intel_gmbus *bus = container_of(adapter, struct intel_gmbus,
+					       adapter);
+	struct drm_i915_private *dev_priv = bus->dev_priv;
+	int ret;
+
+	intel_display_power_get(dev_priv, POWER_DOMAIN_GMBUS);
+	mutex_lock(&dev_priv->gmbus_mutex);
+
+	if (bus->force_bit)
+		ret = i2c_bit_algo.master_xfer(adapter, msgs, num);
+	else
+		ret = do_gmbus_xfer(adapter, msgs, num);
+
+	mutex_unlock(&dev_priv->gmbus_mutex);
 	intel_display_power_put(dev_priv, POWER_DOMAIN_GMBUS);
 
 	return ret;
@@ -628,12 +635,13 @@ int intel_setup_gmbus(struct drm_device *dev)
 
 	if (HAS_PCH_NOP(dev))
 		return 0;
-	else if (HAS_PCH_SPLIT(dev))
-		dev_priv->gpio_mmio_base = PCH_GPIOA - GPIOA;
-	else if (IS_VALLEYVIEW(dev))
+
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		dev_priv->gpio_mmio_base = VLV_DISPLAY_BASE;
-	else
-		dev_priv->gpio_mmio_base = 0;
+	else if (!HAS_GMCH_DISPLAY(dev_priv))
+		dev_priv->gpio_mmio_base =
+			i915_mmio_reg_offset(PCH_GPIOA) -
+			i915_mmio_reg_offset(GPIOA);
 
 	mutex_init(&dev_priv->gmbus_mutex);
 	init_waitqueue_head(&dev_priv->gmbus_wait_queue);
@@ -656,6 +664,12 @@ int intel_setup_gmbus(struct drm_device *dev)
 
 		bus->adapter.algo = &gmbus_algorithm;
 
+		/*
+		 * We wish to retry with bit banging
+		 * after a timed out GMBUS attempt.
+		 */
+		bus->adapter.retries = 1;
+
 		/* By default use a conservative clock rate */
 		bus->reg0 = pin | GMBUS_RATE_100KHZ;
 
@@ -675,7 +689,7 @@ int intel_setup_gmbus(struct drm_device *dev)
 	return 0;
 
 err:
-	while (--pin) {
+	while (pin--) {
 		if (!intel_gmbus_is_valid_pin(dev_priv, pin))
 			continue;
 
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
index 88e12bdf79e2..f1fa756c5d5d 100644
--- a/drivers/gpu/drm/i915/intel_lrc.c
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -190,16 +190,21 @@
 #define GEN8_CTX_L3LLC_COHERENT (1<<5)
 #define GEN8_CTX_PRIVILEGE (1<<8)
 
-#define ASSIGN_CTX_PDP(ppgtt, reg_state, n) { \
+#define ASSIGN_CTX_REG(reg_state, pos, reg, val) do { \
+	(reg_state)[(pos)+0] = i915_mmio_reg_offset(reg); \
+	(reg_state)[(pos)+1] = (val); \
+} while (0)
+
+#define ASSIGN_CTX_PDP(ppgtt, reg_state, n) do {		\
 	const u64 _addr = i915_page_dir_dma_addr((ppgtt), (n));	\
 	reg_state[CTX_PDP ## n ## _UDW+1] = upper_32_bits(_addr); \
 	reg_state[CTX_PDP ## n ## _LDW+1] = lower_32_bits(_addr); \
-}
+} while (0)
 
-#define ASSIGN_CTX_PML4(ppgtt, reg_state) { \
+#define ASSIGN_CTX_PML4(ppgtt, reg_state) do { \
 	reg_state[CTX_PDP0_UDW + 1] = upper_32_bits(px_dma(&ppgtt->pml4)); \
 	reg_state[CTX_PDP0_LDW + 1] = lower_32_bits(px_dma(&ppgtt->pml4)); \
-}
+} while (0)
 
 enum {
 	ADVANCED_CONTEXT = 0,
@@ -284,8 +289,8 @@ static bool disable_lite_restore_wa(struct intel_engine_cs *ring)
 {
 	struct drm_device *dev = ring->dev;
 
-	return ((IS_SKYLAKE(dev) && INTEL_REVID(dev) <= SKL_REVID_B0) ||
-		(IS_BROXTON(dev) && INTEL_REVID(dev) == BXT_REVID_A0)) &&
+	return (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
+		IS_BXT_REVID(dev, 0, BXT_REVID_A1)) &&
 	       (ring->id == VCS || ring->id == VCS2);
 }
 
@@ -367,7 +372,7 @@ static int execlists_update_context(struct drm_i915_gem_request *rq)
 	WARN_ON(!i915_gem_obj_is_pinned(ctx_obj));
 	WARN_ON(!i915_gem_obj_is_pinned(rb_obj));
 
-	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	page = i915_gem_object_get_dirty_page(ctx_obj, LRC_STATE_PN);
 	reg_state = kmap_atomic(page);
 
 	reg_state[CTX_RING_TAIL+1] = rq->tail;
@@ -921,7 +926,7 @@ int intel_execlists_submission(struct i915_execbuffer_params *params,
 
 		intel_logical_ring_emit(ringbuf, MI_NOOP);
 		intel_logical_ring_emit(ringbuf, MI_LOAD_REGISTER_IMM(1));
-		intel_logical_ring_emit(ringbuf, INSTPM);
+		intel_logical_ring_emit_reg(ringbuf, INSTPM);
 		intel_logical_ring_emit(ringbuf, instp_mask << 16 | instp_mode);
 		intel_logical_ring_advance(ringbuf);
 
@@ -1096,7 +1101,7 @@ static int intel_logical_ring_workarounds_emit(struct drm_i915_gem_request *req)
 
 	intel_logical_ring_emit(ringbuf, MI_LOAD_REGISTER_IMM(w->count));
 	for (i = 0; i < w->count; i++) {
-		intel_logical_ring_emit(ringbuf, w->reg[i].addr);
+		intel_logical_ring_emit_reg(ringbuf, w->reg[i].addr);
 		intel_logical_ring_emit(ringbuf, w->reg[i].value);
 	}
 	intel_logical_ring_emit(ringbuf, MI_NOOP);
@@ -1120,6 +1125,8 @@ static int intel_logical_ring_workarounds_emit(struct drm_i915_gem_request *req)
 		batch[__index] = (cmd);					\
 	} while (0)
 
+#define wa_ctx_emit_reg(batch, index, reg) \
+	wa_ctx_emit((batch), (index), i915_mmio_reg_offset(reg))
 
 /*
  * In this WA we need to set GEN8_L3SQCREG4[21:21] and reset it after
@@ -1149,17 +1156,17 @@ static inline int gen8_emit_flush_coherentl3_wa(struct intel_engine_cs *ring,
 	 * this batch updates GEN8_L3SQCREG4 with default value we need to
 	 * set this bit here to retain the WA during flush.
 	 */
-	if (IS_SKYLAKE(ring->dev) && INTEL_REVID(ring->dev) <= SKL_REVID_E0)
+	if (IS_SKL_REVID(ring->dev, 0, SKL_REVID_E0))
 		l3sqc4_flush |= GEN8_LQSC_RO_PERF_DIS;
 
 	wa_ctx_emit(batch, index, (MI_STORE_REGISTER_MEM_GEN8 |
 				   MI_SRM_LRM_GLOBAL_GTT));
-	wa_ctx_emit(batch, index, GEN8_L3SQCREG4);
+	wa_ctx_emit_reg(batch, index, GEN8_L3SQCREG4);
 	wa_ctx_emit(batch, index, ring->scratch.gtt_offset + 256);
 	wa_ctx_emit(batch, index, 0);
 
 	wa_ctx_emit(batch, index, MI_LOAD_REGISTER_IMM(1));
-	wa_ctx_emit(batch, index, GEN8_L3SQCREG4);
+	wa_ctx_emit_reg(batch, index, GEN8_L3SQCREG4);
 	wa_ctx_emit(batch, index, l3sqc4_flush);
 
 	wa_ctx_emit(batch, index, GFX_OP_PIPE_CONTROL(6));
@@ -1172,7 +1179,7 @@ static inline int gen8_emit_flush_coherentl3_wa(struct intel_engine_cs *ring,
 
 	wa_ctx_emit(batch, index, (MI_LOAD_REGISTER_MEM_GEN8 |
 				   MI_SRM_LRM_GLOBAL_GTT));
-	wa_ctx_emit(batch, index, GEN8_L3SQCREG4);
+	wa_ctx_emit_reg(batch, index, GEN8_L3SQCREG4);
 	wa_ctx_emit(batch, index, ring->scratch.gtt_offset + 256);
 	wa_ctx_emit(batch, index, 0);
 
@@ -1314,8 +1321,8 @@ static int gen9_init_indirectctx_bb(struct intel_engine_cs *ring,
 	uint32_t index = wa_ctx_start(wa_ctx, *offset, CACHELINE_DWORDS);
 
 	/* WaDisableCtxRestoreArbitration:skl,bxt */
-	if ((IS_SKYLAKE(dev) && (INTEL_REVID(dev) <= SKL_REVID_D0)) ||
-	    (IS_BROXTON(dev) && (INTEL_REVID(dev) == BXT_REVID_A0)))
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_D0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 		wa_ctx_emit(batch, index, MI_ARB_ON_OFF | MI_ARB_DISABLE);
 
 	/* WaFlushCoherentL3CacheLinesAtContextSwitch:skl,bxt */
@@ -1340,18 +1347,18 @@ static int gen9_init_perctx_bb(struct intel_engine_cs *ring,
 	uint32_t index = wa_ctx_start(wa_ctx, *offset, CACHELINE_DWORDS);
 
 	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:skl,bxt */
-	if ((IS_SKYLAKE(dev) && (INTEL_REVID(dev) <= SKL_REVID_B0)) ||
-	    (IS_BROXTON(dev) && (INTEL_REVID(dev) == BXT_REVID_A0))) {
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		wa_ctx_emit(batch, index, MI_LOAD_REGISTER_IMM(1));
-		wa_ctx_emit(batch, index, GEN9_SLICE_COMMON_ECO_CHICKEN0);
+		wa_ctx_emit_reg(batch, index, GEN9_SLICE_COMMON_ECO_CHICKEN0);
 		wa_ctx_emit(batch, index,
 			    _MASKED_BIT_ENABLE(DISABLE_PIXEL_MASK_CAMMING));
 		wa_ctx_emit(batch, index, MI_NOOP);
 	}
 
 	/* WaDisableCtxRestoreArbitration:skl,bxt */
-	if ((IS_SKYLAKE(dev) && (INTEL_REVID(dev) <= SKL_REVID_D0)) ||
-	    (IS_BROXTON(dev) && (INTEL_REVID(dev) == BXT_REVID_A0)))
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_D0) ||
+	    IS_BXT_REVID(dev, 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);
@@ -1418,7 +1425,7 @@ static int intel_init_workaround_bb(struct intel_engine_cs *ring)
 		return ret;
 	}
 
-	page = i915_gem_object_get_page(wa_ctx->obj, 0);
+	page = i915_gem_object_get_dirty_page(wa_ctx->obj, 0);
 	batch = kmap_atomic(page);
 	offset = 0;
 
@@ -1472,12 +1479,6 @@ static int gen8_init_common_ring(struct intel_engine_cs *ring)
 	I915_WRITE_IMR(ring, ~(ring->irq_enable_mask | ring->irq_keep_mask));
 	I915_WRITE(RING_HWSTAM(ring->mmio_base), 0xffffffff);
 
-	if (ring->status_page.obj) {
-		I915_WRITE(RING_HWS_PGA(ring->mmio_base),
-			   (u32)ring->status_page.gfx_addr);
-		POSTING_READ(RING_HWS_PGA(ring->mmio_base));
-	}
-
 	I915_WRITE(RING_MODE_GEN7(ring),
 		   _MASKED_BIT_DISABLE(GFX_REPLAY_MODE) |
 		   _MASKED_BIT_ENABLE(GFX_RUN_LIST_ENABLE));
@@ -1562,9 +1563,9 @@ static int intel_logical_ring_emit_pdps(struct drm_i915_gem_request *req)
 	for (i = GEN8_LEGACY_PDPES - 1; i >= 0; i--) {
 		const dma_addr_t pd_daddr = i915_page_dir_dma_addr(ppgtt, i);
 
-		intel_logical_ring_emit(ringbuf, GEN8_RING_PDP_UDW(ring, i));
+		intel_logical_ring_emit_reg(ringbuf, GEN8_RING_PDP_UDW(ring, i));
 		intel_logical_ring_emit(ringbuf, upper_32_bits(pd_daddr));
-		intel_logical_ring_emit(ringbuf, GEN8_RING_PDP_LDW(ring, i));
+		intel_logical_ring_emit_reg(ringbuf, GEN8_RING_PDP_LDW(ring, i));
 		intel_logical_ring_emit(ringbuf, lower_32_bits(pd_daddr));
 	}
 
@@ -1706,6 +1707,7 @@ static int gen8_emit_flush_render(struct drm_i915_gem_request *request,
 	if (flush_domains) {
 		flags |= PIPE_CONTROL_RENDER_TARGET_CACHE_FLUSH;
 		flags |= PIPE_CONTROL_DEPTH_CACHE_FLUSH;
+		flags |= PIPE_CONTROL_DC_FLUSH_ENABLE;
 		flags |= PIPE_CONTROL_FLUSH_ENABLE;
 	}
 
@@ -1893,8 +1895,10 @@ void intel_logical_ring_cleanup(struct intel_engine_cs *ring)
 
 	dev_priv = ring->dev->dev_private;
 
-	intel_logical_ring_stop(ring);
-	WARN_ON((I915_READ_MODE(ring) & MODE_IDLE) == 0);
+	if (ring->buffer) {
+		intel_logical_ring_stop(ring);
+		WARN_ON((I915_READ_MODE(ring) & MODE_IDLE) == 0);
+	}
 
 	if (ring->cleanup)
 		ring->cleanup(ring);
@@ -1908,6 +1912,7 @@ void intel_logical_ring_cleanup(struct intel_engine_cs *ring)
 	}
 
 	lrc_destroy_wa_ctx_obj(ring);
+	ring->dev = NULL;
 }
 
 static int logical_ring_init(struct drm_device *dev, struct intel_engine_cs *ring)
@@ -1923,17 +1928,18 @@ static int logical_ring_init(struct drm_device *dev, struct intel_engine_cs *rin
 	i915_gem_batch_pool_init(dev, &ring->batch_pool);
 	init_waitqueue_head(&ring->irq_queue);
 
+	INIT_LIST_HEAD(&ring->buffers);
 	INIT_LIST_HEAD(&ring->execlist_queue);
 	INIT_LIST_HEAD(&ring->execlist_retired_req_list);
 	spin_lock_init(&ring->execlist_lock);
 
 	ret = i915_cmd_parser_init_ring(ring);
 	if (ret)
-		return ret;
+		goto error;
 
 	ret = intel_lr_context_deferred_alloc(ring->default_context, ring);
 	if (ret)
-		return ret;
+		goto error;
 
 	/* As this is the default context, always pin it */
 	ret = intel_lr_context_do_pin(
@@ -1944,9 +1950,13 @@ static int logical_ring_init(struct drm_device *dev, struct intel_engine_cs *rin
 		DRM_ERROR(
 			"Failed to pin and map ringbuffer %s: %d\n",
 			ring->name, ret);
-		return ret;
+		goto error;
 	}
 
+	return 0;
+
+error:
+	intel_logical_ring_cleanup(ring);
 	return ret;
 }
 
@@ -1972,7 +1982,7 @@ static int logical_render_ring_init(struct drm_device *dev)
 		ring->init_hw = gen8_init_render_ring;
 	ring->init_context = gen8_init_rcs_context;
 	ring->cleanup = intel_fini_pipe_control;
-	if (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		ring->get_seqno = bxt_a_get_seqno;
 		ring->set_seqno = bxt_a_set_seqno;
 	} else {
@@ -2024,7 +2034,7 @@ static int logical_bsd_ring_init(struct drm_device *dev)
 		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS1_IRQ_SHIFT;
 
 	ring->init_hw = gen8_init_common_ring;
-	if (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		ring->get_seqno = bxt_a_get_seqno;
 		ring->set_seqno = bxt_a_set_seqno;
 	} else {
@@ -2079,7 +2089,7 @@ static int logical_blt_ring_init(struct drm_device *dev)
 		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_BCS_IRQ_SHIFT;
 
 	ring->init_hw = gen8_init_common_ring;
-	if (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		ring->get_seqno = bxt_a_get_seqno;
 		ring->set_seqno = bxt_a_set_seqno;
 	} else {
@@ -2109,7 +2119,7 @@ static int logical_vebox_ring_init(struct drm_device *dev)
 		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VECS_IRQ_SHIFT;
 
 	ring->init_hw = gen8_init_common_ring;
-	if (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		ring->get_seqno = bxt_a_get_seqno;
 		ring->set_seqno = bxt_a_set_seqno;
 	} else {
@@ -2255,7 +2265,7 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 
 	/* The second page of the context object contains some fields which must
 	 * be set up prior to the first execution. */
-	page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+	page = i915_gem_object_get_dirty_page(ctx_obj, LRC_STATE_PN);
 	reg_state = kmap_atomic(page);
 
 	/* A context is actually a big batch buffer with several MI_LOAD_REGISTER_IMM
@@ -2263,46 +2273,31 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 	 * only for the first context restore: on a subsequent save, the GPU will
 	 * recreate this batchbuffer with new values (including all the missing
 	 * MI_LOAD_REGISTER_IMM commands that we are not initializing here). */
-	if (ring->id == RCS)
-		reg_state[CTX_LRI_HEADER_0] = MI_LOAD_REGISTER_IMM(14);
-	else
-		reg_state[CTX_LRI_HEADER_0] = MI_LOAD_REGISTER_IMM(11);
-	reg_state[CTX_LRI_HEADER_0] |= MI_LRI_FORCE_POSTED;
-	reg_state[CTX_CONTEXT_CONTROL] = RING_CONTEXT_CONTROL(ring);
-	reg_state[CTX_CONTEXT_CONTROL+1] =
-		_MASKED_BIT_ENABLE(CTX_CTRL_INHIBIT_SYN_CTX_SWITCH |
-				   CTX_CTRL_ENGINE_CTX_RESTORE_INHIBIT |
-				   CTX_CTRL_RS_CTX_ENABLE);
-	reg_state[CTX_RING_HEAD] = RING_HEAD(ring->mmio_base);
-	reg_state[CTX_RING_HEAD+1] = 0;
-	reg_state[CTX_RING_TAIL] = RING_TAIL(ring->mmio_base);
-	reg_state[CTX_RING_TAIL+1] = 0;
-	reg_state[CTX_RING_BUFFER_START] = RING_START(ring->mmio_base);
+	reg_state[CTX_LRI_HEADER_0] =
+		MI_LOAD_REGISTER_IMM(ring->id == RCS ? 14 : 11) | MI_LRI_FORCE_POSTED;
+	ASSIGN_CTX_REG(reg_state, CTX_CONTEXT_CONTROL, RING_CONTEXT_CONTROL(ring),
+		       _MASKED_BIT_ENABLE(CTX_CTRL_INHIBIT_SYN_CTX_SWITCH |
+					  CTX_CTRL_ENGINE_CTX_RESTORE_INHIBIT |
+					  CTX_CTRL_RS_CTX_ENABLE));
+	ASSIGN_CTX_REG(reg_state, CTX_RING_HEAD, RING_HEAD(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_RING_TAIL, RING_TAIL(ring->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()
 	 */
-	reg_state[CTX_RING_BUFFER_CONTROL] = RING_CTL(ring->mmio_base);
-	reg_state[CTX_RING_BUFFER_CONTROL+1] =
-			((ringbuf->size - PAGE_SIZE) & RING_NR_PAGES) | RING_VALID;
-	reg_state[CTX_BB_HEAD_U] = ring->mmio_base + 0x168;
-	reg_state[CTX_BB_HEAD_U+1] = 0;
-	reg_state[CTX_BB_HEAD_L] = ring->mmio_base + 0x140;
-	reg_state[CTX_BB_HEAD_L+1] = 0;
-	reg_state[CTX_BB_STATE] = ring->mmio_base + 0x110;
-	reg_state[CTX_BB_STATE+1] = (1<<5);
-	reg_state[CTX_SECOND_BB_HEAD_U] = ring->mmio_base + 0x11c;
-	reg_state[CTX_SECOND_BB_HEAD_U+1] = 0;
-	reg_state[CTX_SECOND_BB_HEAD_L] = ring->mmio_base + 0x114;
-	reg_state[CTX_SECOND_BB_HEAD_L+1] = 0;
-	reg_state[CTX_SECOND_BB_STATE] = ring->mmio_base + 0x118;
-	reg_state[CTX_SECOND_BB_STATE+1] = 0;
+	ASSIGN_CTX_REG(reg_state, CTX_RING_BUFFER_START, RING_START(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_RING_BUFFER_CONTROL, RING_CTL(ring->mmio_base),
+		       ((ringbuf->size - PAGE_SIZE) & RING_NR_PAGES) | RING_VALID);
+	ASSIGN_CTX_REG(reg_state, CTX_BB_HEAD_U, RING_BBADDR_UDW(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_BB_HEAD_L, RING_BBADDR(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_BB_STATE, RING_BBSTATE(ring->mmio_base),
+		       RING_BB_PPGTT);
+	ASSIGN_CTX_REG(reg_state, CTX_SECOND_BB_HEAD_U, RING_SBBADDR_UDW(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_SECOND_BB_HEAD_L, RING_SBBADDR(ring->mmio_base), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_SECOND_BB_STATE, RING_SBBSTATE(ring->mmio_base), 0);
 	if (ring->id == RCS) {
-		reg_state[CTX_BB_PER_CTX_PTR] = ring->mmio_base + 0x1c0;
-		reg_state[CTX_BB_PER_CTX_PTR+1] = 0;
-		reg_state[CTX_RCS_INDIRECT_CTX] = ring->mmio_base + 0x1c4;
-		reg_state[CTX_RCS_INDIRECT_CTX+1] = 0;
-		reg_state[CTX_RCS_INDIRECT_CTX_OFFSET] = ring->mmio_base + 0x1c8;
-		reg_state[CTX_RCS_INDIRECT_CTX_OFFSET+1] = 0;
+		ASSIGN_CTX_REG(reg_state, CTX_BB_PER_CTX_PTR, RING_BB_PER_CTX_PTR(ring->mmio_base), 0);
+		ASSIGN_CTX_REG(reg_state, CTX_RCS_INDIRECT_CTX, RING_INDIRECT_CTX(ring->mmio_base), 0);
+		ASSIGN_CTX_REG(reg_state, CTX_RCS_INDIRECT_CTX_OFFSET, RING_INDIRECT_CTX_OFFSET(ring->mmio_base), 0);
 		if (ring->wa_ctx.obj) {
 			struct i915_ctx_workarounds *wa_ctx = &ring->wa_ctx;
 			uint32_t ggtt_offset = i915_gem_obj_ggtt_offset(wa_ctx->obj);
@@ -2319,18 +2314,17 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 				0x01;
 		}
 	}
-	reg_state[CTX_LRI_HEADER_1] = MI_LOAD_REGISTER_IMM(9);
-	reg_state[CTX_LRI_HEADER_1] |= MI_LRI_FORCE_POSTED;
-	reg_state[CTX_CTX_TIMESTAMP] = ring->mmio_base + 0x3a8;
-	reg_state[CTX_CTX_TIMESTAMP+1] = 0;
-	reg_state[CTX_PDP3_UDW] = GEN8_RING_PDP_UDW(ring, 3);
-	reg_state[CTX_PDP3_LDW] = GEN8_RING_PDP_LDW(ring, 3);
-	reg_state[CTX_PDP2_UDW] = GEN8_RING_PDP_UDW(ring, 2);
-	reg_state[CTX_PDP2_LDW] = GEN8_RING_PDP_LDW(ring, 2);
-	reg_state[CTX_PDP1_UDW] = GEN8_RING_PDP_UDW(ring, 1);
-	reg_state[CTX_PDP1_LDW] = GEN8_RING_PDP_LDW(ring, 1);
-	reg_state[CTX_PDP0_UDW] = GEN8_RING_PDP_UDW(ring, 0);
-	reg_state[CTX_PDP0_LDW] = GEN8_RING_PDP_LDW(ring, 0);
+	reg_state[CTX_LRI_HEADER_1] = MI_LOAD_REGISTER_IMM(9) | MI_LRI_FORCE_POSTED;
+	ASSIGN_CTX_REG(reg_state, CTX_CTX_TIMESTAMP, RING_CTX_TIMESTAMP(ring->mmio_base), 0);
+	/* PDP values well be assigned later if needed */
+	ASSIGN_CTX_REG(reg_state, CTX_PDP3_UDW, GEN8_RING_PDP_UDW(ring, 3), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP3_LDW, GEN8_RING_PDP_LDW(ring, 3), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP2_UDW, GEN8_RING_PDP_UDW(ring, 2), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP2_LDW, GEN8_RING_PDP_LDW(ring, 2), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP1_UDW, GEN8_RING_PDP_UDW(ring, 1), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP1_LDW, GEN8_RING_PDP_LDW(ring, 1), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP0_UDW, GEN8_RING_PDP_UDW(ring, 0), 0);
+	ASSIGN_CTX_REG(reg_state, CTX_PDP0_LDW, GEN8_RING_PDP_LDW(ring, 0), 0);
 
 	if (USES_FULL_48BIT_PPGTT(ppgtt->base.dev)) {
 		/* 64b PPGTT (48bit canonical)
@@ -2352,14 +2346,11 @@ populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_o
 
 	if (ring->id == RCS) {
 		reg_state[CTX_LRI_HEADER_2] = MI_LOAD_REGISTER_IMM(1);
-		reg_state[CTX_R_PWR_CLK_STATE] = GEN8_R_PWR_CLK_STATE;
-		reg_state[CTX_R_PWR_CLK_STATE+1] = make_rpcs(dev);
+		ASSIGN_CTX_REG(reg_state, CTX_R_PWR_CLK_STATE, GEN8_R_PWR_CLK_STATE,
+			       make_rpcs(dev));
 	}
 
 	kunmap_atomic(reg_state);
-
-	ctx_obj->dirty = 1;
-	set_page_dirty(page);
 	i915_gem_object_unpin_pages(ctx_obj);
 
 	return 0;
@@ -2543,7 +2534,7 @@ void intel_lr_context_reset(struct drm_device *dev,
 			WARN(1, "Failed get_pages for context obj\n");
 			continue;
 		}
-		page = i915_gem_object_get_page(ctx_obj, LRC_STATE_PN);
+		page = i915_gem_object_get_dirty_page(ctx_obj, LRC_STATE_PN);
 		reg_state = kmap_atomic(page);
 
 		reg_state[CTX_RING_HEAD+1] = 0;
diff --git a/drivers/gpu/drm/i915/intel_lrc.h b/drivers/gpu/drm/i915/intel_lrc.h
index 4e60d54ba66d..0b821b91723a 100644
--- a/drivers/gpu/drm/i915/intel_lrc.h
+++ b/drivers/gpu/drm/i915/intel_lrc.h
@@ -29,16 +29,16 @@
 #define GEN8_CSB_PTR_MASK 0x07
 
 /* Execlists regs */
-#define RING_ELSP(ring)			((ring)->mmio_base+0x230)
-#define RING_EXECLIST_STATUS_LO(ring)	((ring)->mmio_base+0x234)
-#define RING_EXECLIST_STATUS_HI(ring)	((ring)->mmio_base+0x234 + 4)
-#define RING_CONTEXT_CONTROL(ring)	((ring)->mmio_base+0x244)
+#define RING_ELSP(ring)				_MMIO((ring)->mmio_base + 0x230)
+#define RING_EXECLIST_STATUS_LO(ring)		_MMIO((ring)->mmio_base + 0x234)
+#define RING_EXECLIST_STATUS_HI(ring)		_MMIO((ring)->mmio_base + 0x234 + 4)
+#define RING_CONTEXT_CONTROL(ring)		_MMIO((ring)->mmio_base + 0x244)
 #define	  CTX_CTRL_INHIBIT_SYN_CTX_SWITCH	(1 << 3)
 #define	  CTX_CTRL_ENGINE_CTX_RESTORE_INHIBIT	(1 << 0)
 #define   CTX_CTRL_RS_CTX_ENABLE                (1 << 1)
-#define RING_CONTEXT_STATUS_BUF_LO(ring, i)	((ring)->mmio_base+0x370 + (i) * 8)
-#define RING_CONTEXT_STATUS_BUF_HI(ring, i)	((ring)->mmio_base+0x370 + (i) * 8 + 4)
-#define RING_CONTEXT_STATUS_PTR(ring)	((ring)->mmio_base+0x3a0)
+#define RING_CONTEXT_STATUS_BUF_LO(ring, i)	_MMIO((ring)->mmio_base + 0x370 + (i) * 8)
+#define RING_CONTEXT_STATUS_BUF_HI(ring, i)	_MMIO((ring)->mmio_base + 0x370 + (i) * 8 + 4)
+#define RING_CONTEXT_STATUS_PTR(ring)		_MMIO((ring)->mmio_base + 0x3a0)
 
 /* Logical Rings */
 int intel_logical_ring_alloc_request_extras(struct drm_i915_gem_request *request);
@@ -70,6 +70,11 @@ static inline void intel_logical_ring_emit(struct intel_ringbuffer *ringbuf,
 	iowrite32(data, ringbuf->virtual_start + ringbuf->tail);
 	ringbuf->tail += 4;
 }
+static inline void intel_logical_ring_emit_reg(struct intel_ringbuffer *ringbuf,
+					       i915_reg_t reg)
+{
+	intel_logical_ring_emit(ringbuf, i915_mmio_reg_offset(reg));
+}
 
 /* Logical Ring Contexts */
 
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 7f39b8ad88ae..bc04d8d29acb 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -51,7 +51,7 @@ struct intel_lvds_encoder {
 	struct intel_encoder base;
 
 	bool is_dual_link;
-	u32 reg;
+	i915_reg_t reg;
 	u32 a3_power;
 
 	struct intel_lvds_connector *attached_connector;
@@ -75,22 +75,30 @@ static bool intel_lvds_get_hw_state(struct intel_encoder *encoder,
 	struct intel_lvds_encoder *lvds_encoder = to_lvds_encoder(&encoder->base);
 	enum intel_display_power_domain power_domain;
 	u32 tmp;
+	bool ret;
 
 	power_domain = intel_display_port_power_domain(encoder);
-	if (!intel_display_power_is_enabled(dev_priv, power_domain))
+	if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 		return false;
 
+	ret = false;
+
 	tmp = I915_READ(lvds_encoder->reg);
 
 	if (!(tmp & LVDS_PORT_EN))
-		return false;
+		goto out;
 
 	if (HAS_PCH_CPT(dev))
 		*pipe = PORT_TO_PIPE_CPT(tmp);
 	else
 		*pipe = PORT_TO_PIPE(tmp);
 
-	return true;
+	ret = true;
+
+out:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 static void intel_lvds_get_config(struct intel_encoder *encoder,
@@ -210,7 +218,7 @@ static void intel_enable_lvds(struct intel_encoder *encoder)
 	struct intel_connector *intel_connector =
 		&lvds_encoder->attached_connector->base;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 ctl_reg, stat_reg;
+	i915_reg_t ctl_reg, stat_reg;
 
 	if (HAS_PCH_SPLIT(dev)) {
 		ctl_reg = PCH_PP_CONTROL;
@@ -235,7 +243,7 @@ static void intel_disable_lvds(struct intel_encoder *encoder)
 	struct drm_device *dev = encoder->base.dev;
 	struct intel_lvds_encoder *lvds_encoder = to_lvds_encoder(&encoder->base);
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 ctl_reg, stat_reg;
+	i915_reg_t ctl_reg, stat_reg;
 
 	if (HAS_PCH_SPLIT(dev)) {
 		ctl_reg = PCH_PP_CONTROL;
@@ -939,7 +947,7 @@ void intel_lvds_init(struct drm_device *dev)
 	struct drm_display_mode *downclock_mode = NULL;
 	struct edid *edid;
 	struct drm_crtc *crtc;
-	u32 lvds_reg;
+	i915_reg_t lvds_reg;
 	u32 lvds;
 	int pipe;
 	u8 pin;
@@ -1025,7 +1033,7 @@ void intel_lvds_init(struct drm_device *dev)
 			   DRM_MODE_CONNECTOR_LVDS);
 
 	drm_encoder_init(dev, &intel_encoder->base, &intel_lvds_enc_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 
 	intel_encoder->enable = intel_enable_lvds;
 	intel_encoder->pre_enable = intel_pre_enable_lvds;
@@ -1164,8 +1172,7 @@ out:
 	DRM_DEBUG_KMS("detected %s-link lvds configuration\n",
 		      lvds_encoder->is_dual_link ? "dual" : "single");
 
-	lvds_encoder->a3_power = I915_READ(lvds_encoder->reg) &
-				 LVDS_A3_POWER_MASK;
+	lvds_encoder->a3_power = lvds & LVDS_A3_POWER_MASK;
 
 	lvds_connector->lid_notifier.notifier_call = intel_lid_notify;
 	if (acpi_lid_notifier_register(&lvds_connector->lid_notifier)) {
diff --git a/drivers/gpu/drm/i915/intel_mocs.c b/drivers/gpu/drm/i915/intel_mocs.c
index 6d3c6c0a5c62..fed7bea19cc9 100644
--- a/drivers/gpu/drm/i915/intel_mocs.c
+++ b/drivers/gpu/drm/i915/intel_mocs.c
@@ -143,7 +143,7 @@ static bool get_mocs_settings(struct drm_device *dev,
 {
 	bool result = false;
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		table->size  = ARRAY_SIZE(skylake_mocs_table);
 		table->table = skylake_mocs_table;
 		result = true;
@@ -159,11 +159,30 @@ static bool get_mocs_settings(struct drm_device *dev,
 	return result;
 }
 
+static i915_reg_t mocs_register(enum intel_ring_id ring, int index)
+{
+	switch (ring) {
+	case RCS:
+		return GEN9_GFX_MOCS(index);
+	case VCS:
+		return GEN9_MFX0_MOCS(index);
+	case BCS:
+		return GEN9_BLT_MOCS(index);
+	case VECS:
+		return GEN9_VEBOX_MOCS(index);
+	case VCS2:
+		return GEN9_MFX1_MOCS(index);
+	default:
+		MISSING_CASE(ring);
+		return INVALID_MMIO_REG;
+	}
+}
+
 /**
  * emit_mocs_control_table() - emit the mocs control table
  * @req:	Request to set up the MOCS table for.
  * @table:	The values to program into the control regs.
- * @reg_base:	The base for the engine that needs to be programmed.
+ * @ring:	The engine for whom to emit the registers.
  *
  * This function simply emits a MI_LOAD_REGISTER_IMM command for the
  * given table starting at the given address.
@@ -172,7 +191,7 @@ static bool get_mocs_settings(struct drm_device *dev,
  */
 static int emit_mocs_control_table(struct drm_i915_gem_request *req,
 				   const struct drm_i915_mocs_table *table,
-				   u32 reg_base)
+				   enum intel_ring_id ring)
 {
 	struct intel_ringbuffer *ringbuf = req->ringbuf;
 	unsigned int index;
@@ -191,7 +210,7 @@ static int emit_mocs_control_table(struct drm_i915_gem_request *req,
 				MI_LOAD_REGISTER_IMM(GEN9_NUM_MOCS_ENTRIES));
 
 	for (index = 0; index < table->size; index++) {
-		intel_logical_ring_emit(ringbuf, reg_base + index * 4);
+		intel_logical_ring_emit_reg(ringbuf, mocs_register(ring, index));
 		intel_logical_ring_emit(ringbuf,
 					table->table[index].control_value);
 	}
@@ -205,7 +224,7 @@ static int emit_mocs_control_table(struct drm_i915_gem_request *req,
 	 * that value to all the used entries.
 	 */
 	for (; index < GEN9_NUM_MOCS_ENTRIES; index++) {
-		intel_logical_ring_emit(ringbuf, reg_base + index * 4);
+		intel_logical_ring_emit_reg(ringbuf, mocs_register(ring, index));
 		intel_logical_ring_emit(ringbuf, table->table[0].control_value);
 	}
 
@@ -253,7 +272,7 @@ static int emit_mocs_l3cc_table(struct drm_i915_gem_request *req,
 		value = (table->table[count].l3cc_value & 0xffff) |
 			((table->table[count + 1].l3cc_value & 0xffff) << 16);
 
-		intel_logical_ring_emit(ringbuf, GEN9_LNCFCMOCS0 + i * 4);
+		intel_logical_ring_emit_reg(ringbuf, GEN9_LNCFCMOCS(i));
 		intel_logical_ring_emit(ringbuf, value);
 	}
 
@@ -270,7 +289,7 @@ static int emit_mocs_l3cc_table(struct drm_i915_gem_request *req,
 	 * they are reserved by the hardware.
 	 */
 	for (; i < GEN9_NUM_MOCS_ENTRIES / 2; i++) {
-		intel_logical_ring_emit(ringbuf, GEN9_LNCFCMOCS0 + i * 4);
+		intel_logical_ring_emit_reg(ringbuf, GEN9_LNCFCMOCS(i));
 		intel_logical_ring_emit(ringbuf, value);
 
 		value = filler;
@@ -304,26 +323,16 @@ int intel_rcs_context_init_mocs(struct drm_i915_gem_request *req)
 	int ret;
 
 	if (get_mocs_settings(req->ring->dev, &t)) {
-		/* Program the control registers */
-		ret = emit_mocs_control_table(req, &t, GEN9_GFX_MOCS_0);
-		if (ret)
-			return ret;
-
-		ret = emit_mocs_control_table(req, &t, GEN9_MFX0_MOCS_0);
-		if (ret)
-			return ret;
+		struct drm_i915_private *dev_priv = req->i915;
+		struct intel_engine_cs *ring;
+		enum intel_ring_id ring_id;
 
-		ret = emit_mocs_control_table(req, &t, GEN9_MFX1_MOCS_0);
-		if (ret)
-			return ret;
-
-		ret = emit_mocs_control_table(req, &t, GEN9_VEBOX_MOCS_0);
-		if (ret)
-			return ret;
-
-		ret = emit_mocs_control_table(req, &t, GEN9_BLT_MOCS_0);
-		if (ret)
-			return ret;
+		/* Program the control registers */
+		for_each_ring(ring, dev_priv, ring_id) {
+			ret = emit_mocs_control_table(req, &t, ring_id);
+			if (ret)
+				return ret;
+		}
 
 		/* Now program the l3cc registers */
 		ret = emit_mocs_l3cc_table(req, &t);
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index 6dc13c02c28e..c15718b4862a 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -26,6 +26,7 @@
  */
 
 #include <linux/acpi.h>
+#include <linux/dmi.h>
 #include <acpi/video.h>
 
 #include <drm/drmP.h>
@@ -46,6 +47,7 @@
 #define OPREGION_SWSCI_OFFSET  0x200
 #define OPREGION_ASLE_OFFSET   0x300
 #define OPREGION_VBT_OFFSET    0x400
+#define OPREGION_ASLE_EXT_OFFSET	0x1C00
 
 #define OPREGION_SIGNATURE "IntelGraphicsMem"
 #define MBOX_ACPI      (1<<0)
@@ -120,7 +122,16 @@ struct opregion_asle {
 	u64 fdss;
 	u32 fdsp;
 	u32 stat;
-	u8 rsvd[70];
+	u64 rvda;	/* Physical address of raw vbt data */
+	u32 rvds;	/* Size of raw vbt data */
+	u8 rsvd[58];
+} __packed;
+
+/* OpRegion mailbox #5: ASLE ext */
+struct opregion_asle_ext {
+	u32 phed;	/* Panel Header */
+	u8 bddc[256];	/* Panel EDID */
+	u8 rsvd[764];
 } __packed;
 
 /* Driver readiness indicator */
@@ -411,7 +422,7 @@ int intel_opregion_notify_adapter(struct drm_device *dev, pci_power_t state)
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_connector *intel_connector;
+	struct intel_connector *connector;
 	struct opregion_asle *asle = dev_priv->opregion.asle;
 
 	DRM_DEBUG_DRIVER("bclp = 0x%08x\n", bclp);
@@ -435,8 +446,8 @@ static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 	 * only one).
 	 */
 	DRM_DEBUG_KMS("updating opregion backlight %d/255\n", bclp);
-	list_for_each_entry(intel_connector, &dev->mode_config.connector_list, base.head)
-		intel_panel_set_backlight_acpi(intel_connector, bclp, 255);
+	for_each_intel_connector(dev, connector)
+		intel_panel_set_backlight_acpi(connector, bclp, 255);
 	asle->cblv = DIV_ROUND_UP(bclp * 100, 255) | ASLE_CBLV_VALID;
 
 	drm_modeset_unlock(&dev->mode_config.connection_mutex);
@@ -682,7 +693,7 @@ static void intel_didl_outputs(struct drm_device *dev)
 	}
 
 	if (!acpi_video_bus) {
-		DRM_ERROR("No ACPI video bus found\n");
+		DRM_DEBUG_KMS("No ACPI video bus found\n");
 		return;
 	}
 
@@ -826,6 +837,10 @@ void intel_opregion_fini(struct drm_device *dev)
 
 	/* just clear all opregion memory pointers now */
 	memunmap(opregion->header);
+	if (opregion->rvda) {
+		memunmap(opregion->rvda);
+		opregion->rvda = NULL;
+	}
 	opregion->header = NULL;
 	opregion->acpi = NULL;
 	opregion->swsci = NULL;
@@ -894,6 +909,25 @@ static void swsci_setup(struct drm_device *dev)
 static inline void swsci_setup(struct drm_device *dev) {}
 #endif  /* CONFIG_ACPI */
 
+static int intel_no_opregion_vbt_callback(const struct dmi_system_id *id)
+{
+	DRM_DEBUG_KMS("Falling back to manually reading VBT from "
+		      "VBIOS ROM for %s\n", id->ident);
+	return 1;
+}
+
+static const struct dmi_system_id intel_no_opregion_vbt[] = {
+	{
+		.callback = intel_no_opregion_vbt_callback,
+		.ident = "ThinkCentre A57",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "97027RG"),
+		},
+	},
+	{ }
+};
+
 int intel_opregion_setup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -907,6 +941,7 @@ int intel_opregion_setup(struct drm_device *dev)
 	BUILD_BUG_ON(sizeof(struct opregion_acpi) != 0x100);
 	BUILD_BUG_ON(sizeof(struct opregion_swsci) != 0x100);
 	BUILD_BUG_ON(sizeof(struct opregion_asle) != 0x100);
+	BUILD_BUG_ON(sizeof(struct opregion_asle_ext) != 0x400);
 
 	pci_read_config_dword(dev->pdev, PCI_ASLS, &asls);
 	DRM_DEBUG_DRIVER("graphic opregion physical addr: 0x%x\n", asls);
@@ -931,8 +966,6 @@ int intel_opregion_setup(struct drm_device *dev)
 		goto err_out;
 	}
 	opregion->header = base;
-	opregion->vbt = base + OPREGION_VBT_OFFSET;
-
 	opregion->lid_state = base + ACPI_CLID;
 
 	mboxes = opregion->header->mboxes;
@@ -946,6 +979,7 @@ int intel_opregion_setup(struct drm_device *dev)
 		opregion->swsci = base + OPREGION_SWSCI_OFFSET;
 		swsci_setup(dev);
 	}
+
 	if (mboxes & MBOX_ASLE) {
 		DRM_DEBUG_DRIVER("ASLE supported\n");
 		opregion->asle = base + OPREGION_ASLE_OFFSET;
@@ -953,6 +987,37 @@ int intel_opregion_setup(struct drm_device *dev)
 		opregion->asle->ardy = ASLE_ARDY_NOT_READY;
 	}
 
+	if (mboxes & MBOX_ASLE_EXT)
+		DRM_DEBUG_DRIVER("ASLE extension supported\n");
+
+	if (!dmi_check_system(intel_no_opregion_vbt)) {
+		const void *vbt = NULL;
+		u32 vbt_size = 0;
+
+		if (opregion->header->opregion_ver >= 2 && opregion->asle &&
+		    opregion->asle->rvda && opregion->asle->rvds) {
+			opregion->rvda = memremap(opregion->asle->rvda,
+						  opregion->asle->rvds,
+						  MEMREMAP_WB);
+			vbt = opregion->rvda;
+			vbt_size = opregion->asle->rvds;
+		}
+
+		if (intel_bios_is_valid_vbt(vbt, vbt_size)) {
+			DRM_DEBUG_KMS("Found valid VBT in ACPI OpRegion (RVDA)\n");
+			opregion->vbt = vbt;
+			opregion->vbt_size = vbt_size;
+		} else {
+			vbt = base + OPREGION_VBT_OFFSET;
+			vbt_size = OPREGION_ASLE_EXT_OFFSET - OPREGION_VBT_OFFSET;
+			if (intel_bios_is_valid_vbt(vbt, vbt_size)) {
+				DRM_DEBUG_KMS("Found valid VBT in ACPI OpRegion (Mailbox #4)\n");
+				opregion->vbt = vbt;
+				opregion->vbt_size = vbt_size;
+			}
+		}
+	}
+
 	return 0;
 
 err_out:
diff --git a/drivers/gpu/drm/i915/intel_overlay.c b/drivers/gpu/drm/i915/intel_overlay.c
index 444542696a2c..76f1980a7541 100644
--- a/drivers/gpu/drm/i915/intel_overlay.c
+++ b/drivers/gpu/drm/i915/intel_overlay.c
@@ -749,7 +749,7 @@ static int intel_overlay_do_put_image(struct intel_overlay *overlay,
 	if (ret != 0)
 		return ret;
 
-	ret = i915_gem_object_pin_to_display_plane(new_bo, 0, NULL, NULL,
+	ret = i915_gem_object_pin_to_display_plane(new_bo, 0,
 						   &i915_ggtt_view_normal);
 	if (ret != 0)
 		return ret;
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index a24df35e11e7..21ee6477bf98 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -461,8 +461,7 @@ static inline u32 scale_hw_to_user(struct intel_connector *connector,
 static u32 intel_panel_compute_brightness(struct intel_connector *connector,
 					  u32 val)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 
 	WARN_ON(panel->backlight.max == 0);
@@ -480,45 +479,40 @@ static u32 intel_panel_compute_brightness(struct intel_connector *connector,
 
 static u32 lpt_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 
 	return I915_READ(BLC_PWM_PCH_CTL2) & BACKLIGHT_DUTY_CYCLE_MASK;
 }
 
 static u32 pch_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 
 	return I915_READ(BLC_PWM_CPU_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
 }
 
 static u32 i9xx_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 val;
 
 	val = I915_READ(BLC_PWM_CTL) & BACKLIGHT_DUTY_CYCLE_MASK;
-	if (INTEL_INFO(dev)->gen < 4)
+	if (INTEL_INFO(dev_priv)->gen < 4)
 		val >>= 1;
 
 	if (panel->backlight.combination_mode) {
 		u8 lbpc;
 
-		pci_read_config_byte(dev->pdev, PCI_LBPC, &lbpc);
+		pci_read_config_byte(dev_priv->dev->pdev, PCI_LBPC, &lbpc);
 		val *= lbpc;
 	}
 
 	return val;
 }
 
-static u32 _vlv_get_backlight(struct drm_device *dev, enum pipe pipe)
+static u32 _vlv_get_backlight(struct drm_i915_private *dev_priv, enum pipe pipe)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
 	if (WARN_ON(pipe != PIPE_A && pipe != PIPE_B))
 		return 0;
 
@@ -527,17 +521,16 @@ static u32 _vlv_get_backlight(struct drm_device *dev, enum pipe pipe)
 
 static u32 vlv_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 
-	return _vlv_get_backlight(dev, pipe);
+	return _vlv_get_backlight(dev_priv, pipe);
 }
 
 static u32 bxt_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	return I915_READ(BXT_BLC_PWM_DUTY(panel->backlight.controller));
 }
@@ -553,8 +546,7 @@ static u32 pwm_get_backlight(struct intel_connector *connector)
 
 static u32 intel_panel_get_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 val = 0;
 
@@ -573,16 +565,14 @@ static u32 intel_panel_get_backlight(struct intel_connector *connector)
 
 static void lpt_set_backlight(struct intel_connector *connector, u32 level)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 val = I915_READ(BLC_PWM_PCH_CTL2) & ~BACKLIGHT_DUTY_CYCLE_MASK;
 	I915_WRITE(BLC_PWM_PCH_CTL2, val | level);
 }
 
 static void pch_set_backlight(struct intel_connector *connector, u32 level)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 tmp;
 
 	tmp = I915_READ(BLC_PWM_CPU_CTL) & ~BACKLIGHT_DUTY_CYCLE_MASK;
@@ -591,8 +581,7 @@ static void pch_set_backlight(struct intel_connector *connector, u32 level)
 
 static void i9xx_set_backlight(struct intel_connector *connector, u32 level)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 tmp, mask;
 
@@ -603,10 +592,10 @@ static void i9xx_set_backlight(struct intel_connector *connector, u32 level)
 
 		lbpc = level * 0xfe / panel->backlight.max + 1;
 		level /= lbpc;
-		pci_write_config_byte(dev->pdev, PCI_LBPC, lbpc);
+		pci_write_config_byte(dev_priv->dev->pdev, PCI_LBPC, lbpc);
 	}
 
-	if (IS_GEN4(dev)) {
+	if (IS_GEN4(dev_priv)) {
 		mask = BACKLIGHT_DUTY_CYCLE_MASK;
 	} else {
 		level <<= 1;
@@ -619,8 +608,7 @@ static void i9xx_set_backlight(struct intel_connector *connector, u32 level)
 
 static void vlv_set_backlight(struct intel_connector *connector, u32 level)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 tmp;
 
@@ -633,8 +621,7 @@ static void vlv_set_backlight(struct intel_connector *connector, u32 level)
 
 static void bxt_set_backlight(struct intel_connector *connector, u32 level)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 
 	I915_WRITE(BXT_BLC_PWM_DUTY(panel->backlight.controller), level);
@@ -663,8 +650,7 @@ intel_panel_actually_set_backlight(struct intel_connector *connector, u32 level)
 static void intel_panel_set_backlight(struct intel_connector *connector,
 				      u32 user_level, u32 user_max)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 hw_level;
 
@@ -690,8 +676,7 @@ static void intel_panel_set_backlight(struct intel_connector *connector,
 void intel_panel_set_backlight_acpi(struct intel_connector *connector,
 				    u32 user_level, u32 user_max)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 hw_level;
@@ -726,8 +711,7 @@ void intel_panel_set_backlight_acpi(struct intel_connector *connector,
 
 static void lpt_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 tmp;
 
 	intel_panel_actually_set_backlight(connector, 0);
@@ -752,8 +736,7 @@ static void lpt_disable_backlight(struct intel_connector *connector)
 
 static void pch_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 tmp;
 
 	intel_panel_actually_set_backlight(connector, 0);
@@ -772,8 +755,7 @@ static void i9xx_disable_backlight(struct intel_connector *connector)
 
 static void i965_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 tmp;
 
 	intel_panel_actually_set_backlight(connector, 0);
@@ -784,8 +766,7 @@ static void i965_disable_backlight(struct intel_connector *connector)
 
 static void vlv_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 tmp;
 
@@ -800,8 +781,7 @@ static void vlv_disable_backlight(struct intel_connector *connector)
 
 static void bxt_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 tmp, val;
 
@@ -830,8 +810,7 @@ static void pwm_disable_backlight(struct intel_connector *connector)
 
 void intel_panel_disable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 
 	if (!panel->backlight.present)
@@ -843,7 +822,7 @@ void intel_panel_disable_backlight(struct intel_connector *connector)
 	 * backlight. This will leave the backlight on unnecessarily when
 	 * another client is not activated.
 	 */
-	if (dev->switch_power_state == DRM_SWITCH_POWER_CHANGING) {
+	if (dev_priv->dev->switch_power_state == DRM_SWITCH_POWER_CHANGING) {
 		DRM_DEBUG_DRIVER("Skipping backlight disable on vga switch\n");
 		return;
 	}
@@ -860,8 +839,7 @@ void intel_panel_disable_backlight(struct intel_connector *connector)
 
 static void lpt_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 pch_ctl1, pch_ctl2;
 
@@ -893,8 +871,7 @@ static void lpt_enable_backlight(struct intel_connector *connector)
 
 static void pch_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	enum transcoder cpu_transcoder =
@@ -940,8 +917,7 @@ static void pch_enable_backlight(struct intel_connector *connector)
 
 static void i9xx_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 ctl, freq;
 
@@ -958,7 +934,7 @@ static void i9xx_enable_backlight(struct intel_connector *connector)
 	ctl = freq << 17;
 	if (panel->backlight.combination_mode)
 		ctl |= BLM_LEGACY_MODE;
-	if (IS_PINEVIEW(dev) && panel->backlight.active_low_pwm)
+	if (IS_PINEVIEW(dev_priv) && panel->backlight.active_low_pwm)
 		ctl |= BLM_POLARITY_PNV;
 
 	I915_WRITE(BLC_PWM_CTL, ctl);
@@ -972,14 +948,13 @@ static void i9xx_enable_backlight(struct intel_connector *connector)
 	 * 855gm only, but checking for gen2 is safe, as 855gm is the only gen2
 	 * that has backlight.
 	 */
-	if (IS_GEN2(dev))
+	if (IS_GEN2(dev_priv))
 		I915_WRITE(BLC_HIST_CTL, BLM_HISTOGRAM_ENABLE);
 }
 
 static void i965_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 ctl, ctl2, freq;
@@ -1012,8 +987,7 @@ static void i965_enable_backlight(struct intel_connector *connector)
 
 static void vlv_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 ctl, ctl2;
@@ -1044,8 +1018,7 @@ static void vlv_enable_backlight(struct intel_connector *connector)
 
 static void bxt_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 	u32 pwm_ctl, val;
@@ -1102,8 +1075,7 @@ static void pwm_enable_backlight(struct intel_connector *connector)
 
 void intel_panel_enable_backlight(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	enum pipe pipe = intel_get_pipe_from_connector(connector);
 
@@ -1264,14 +1236,21 @@ static void intel_backlight_device_unregister(struct intel_connector *connector)
 #endif /* CONFIG_BACKLIGHT_CLASS_DEVICE */
 
 /*
+ * BXT: PWM clock frequency = 19.2 MHz.
+ */
+static u32 bxt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
+{
+	return KHz(19200) / pwm_freq_hz;
+}
+
+/*
  * SPT: This value represents the period of the PWM stream in clock periods
  * multiplied by 16 (default increment) or 128 (alternate increment selected in
  * SCHICKEN_1 bit 0). PWM clock is 24 MHz.
  */
 static u32 spt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 mul, clock;
 
 	if (I915_READ(SOUTH_CHICKEN1) & SPT_PWM_GRANULARITY)
@@ -1291,8 +1270,7 @@ 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_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	u32 mul, clock;
 
 	if (I915_READ(SOUTH_CHICKEN2) & LPT_PWM_GRANULARITY)
@@ -1300,7 +1278,7 @@ static u32 lpt_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 	else
 		mul = 128;
 
-	if (dev_priv->pch_id == INTEL_PCH_LPT_DEVICE_ID_TYPE)
+	if (HAS_PCH_LPT_H(dev_priv))
 		clock = MHz(135); /* LPT:H */
 	else
 		clock = MHz(24); /* LPT:LP */
@@ -1335,22 +1313,28 @@ static u32 i9xx_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 	int clock;
 
 	if (IS_PINEVIEW(dev))
-		clock = intel_hrawclk(dev);
+		clock = MHz(intel_hrawclk(dev));
 	else
-		clock = 1000 * dev_priv->display.get_display_clock_speed(dev);
+		clock = 1000 * dev_priv->cdclk_freq;
 
 	return clock / (pwm_freq_hz * 32);
 }
 
 /*
  * Gen4: This value represents the period of the PWM stream in display core
- * clocks multiplied by 128.
+ * clocks ([DevCTG] HRAW clocks) multiplied by 128.
+ *
  */
 static u32 i965_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 {
 	struct drm_device *dev = connector->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int clock = 1000 * dev_priv->display.get_display_clock_speed(dev);
+	int clock;
+
+	if (IS_G4X(dev_priv))
+		clock = MHz(intel_hrawclk(dev));
+	else
+		clock = 1000 * dev_priv->cdclk_freq;
 
 	return clock / (pwm_freq_hz * 128);
 }
@@ -1379,20 +1363,23 @@ static u32 vlv_hz_to_pwm(struct intel_connector *connector, u32 pwm_freq_hz)
 
 static u32 get_backlight_max_vbt(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u16 pwm_freq_hz = dev_priv->vbt.backlight.pwm_freq_hz;
 	u32 pwm;
 
-	if (!pwm_freq_hz) {
-		DRM_DEBUG_KMS("backlight frequency not specified in VBT\n");
+	if (!panel->backlight.hz_to_pwm) {
+		DRM_DEBUG_KMS("backlight frequency conversion not supported\n");
 		return 0;
 	}
 
-	if (!panel->backlight.hz_to_pwm) {
-		DRM_DEBUG_KMS("backlight frequency setting from VBT currently not supported on this platform\n");
-		return 0;
+	if (pwm_freq_hz) {
+		DRM_DEBUG_KMS("VBT defined backlight frequency %u Hz\n",
+			      pwm_freq_hz);
+	} else {
+		pwm_freq_hz = 200;
+		DRM_DEBUG_KMS("default backlight frequency %u Hz\n",
+			      pwm_freq_hz);
 	}
 
 	pwm = panel->backlight.hz_to_pwm(connector, pwm_freq_hz);
@@ -1401,8 +1388,6 @@ static u32 get_backlight_max_vbt(struct intel_connector *connector)
 		return 0;
 	}
 
-	DRM_DEBUG_KMS("backlight frequency %u Hz from VBT\n", pwm_freq_hz);
-
 	return pwm;
 }
 
@@ -1411,8 +1396,7 @@ static u32 get_backlight_max_vbt(struct intel_connector *connector)
  */
 static u32 get_backlight_min_vbt(struct intel_connector *connector)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	int min;
 
@@ -1437,8 +1421,7 @@ static u32 get_backlight_min_vbt(struct intel_connector *connector)
 
 static int lpt_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 pch_ctl1, pch_ctl2, val;
 
@@ -1467,8 +1450,7 @@ static int lpt_setup_backlight(struct intel_connector *connector, enum pipe unus
 
 static int pch_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 cpu_ctl2, pch_ctl1, pch_ctl2, val;
 
@@ -1498,17 +1480,16 @@ static int pch_setup_backlight(struct intel_connector *connector, enum pipe unus
 
 static int i9xx_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 ctl, val;
 
 	ctl = I915_READ(BLC_PWM_CTL);
 
-	if (IS_GEN2(dev) || IS_I915GM(dev) || IS_I945GM(dev))
+	if (IS_GEN2(dev_priv) || IS_I915GM(dev_priv) || IS_I945GM(dev_priv))
 		panel->backlight.combination_mode = ctl & BLM_LEGACY_MODE;
 
-	if (IS_PINEVIEW(dev))
+	if (IS_PINEVIEW(dev_priv))
 		panel->backlight.active_low_pwm = ctl & BLM_POLARITY_PNV;
 
 	panel->backlight.max = ctl >> 17;
@@ -1536,8 +1517,7 @@ static int i9xx_setup_backlight(struct intel_connector *connector, enum pipe unu
 
 static int i965_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 ctl, ctl2, val;
 
@@ -1570,8 +1550,7 @@ static int i965_setup_backlight(struct intel_connector *connector, enum pipe unu
 
 static int vlv_setup_backlight(struct intel_connector *connector, enum pipe pipe)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 ctl, ctl2, val;
 
@@ -1592,7 +1571,7 @@ static int vlv_setup_backlight(struct intel_connector *connector, enum pipe pipe
 
 	panel->backlight.min = get_backlight_min_vbt(connector);
 
-	val = _vlv_get_backlight(dev, pipe);
+	val = _vlv_get_backlight(dev_priv, pipe);
 	panel->backlight.level = intel_panel_compute_brightness(connector, val);
 
 	panel->backlight.enabled = (ctl2 & BLM_PWM_ENABLE) &&
@@ -1604,8 +1583,7 @@ static int vlv_setup_backlight(struct intel_connector *connector, enum pipe pipe
 static int
 bxt_setup_backlight(struct intel_connector *connector, enum pipe unused)
 {
-	struct drm_device *dev = connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 	struct intel_panel *panel = &connector->panel;
 	u32 pwm_ctl, val;
 
@@ -1683,8 +1661,7 @@ static int pwm_setup_backlight(struct intel_connector *connector,
 
 int intel_panel_setup_backlight(struct drm_connector *connector, enum pipe pipe)
 {
-	struct drm_device *dev = connector->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->dev);
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 	struct intel_panel *panel = &intel_connector->panel;
 	int ret;
@@ -1739,35 +1716,35 @@ void intel_panel_destroy_backlight(struct drm_connector *connector)
 static void
 intel_panel_init_backlight_funcs(struct intel_panel *panel)
 {
-	struct intel_connector *intel_connector =
+	struct intel_connector *connector =
 		container_of(panel, struct intel_connector, panel);
-	struct drm_device *dev = intel_connector->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(connector->base.dev);
 
-	if (IS_BROXTON(dev)) {
+	if (IS_BROXTON(dev_priv)) {
 		panel->backlight.setup = bxt_setup_backlight;
 		panel->backlight.enable = bxt_enable_backlight;
 		panel->backlight.disable = bxt_disable_backlight;
 		panel->backlight.set = bxt_set_backlight;
 		panel->backlight.get = bxt_get_backlight;
-	} else if (HAS_PCH_LPT(dev) || HAS_PCH_SPT(dev)) {
+		panel->backlight.hz_to_pwm = bxt_hz_to_pwm;
+	} else if (HAS_PCH_LPT(dev_priv) || HAS_PCH_SPT(dev_priv)) {
 		panel->backlight.setup = lpt_setup_backlight;
 		panel->backlight.enable = lpt_enable_backlight;
 		panel->backlight.disable = lpt_disable_backlight;
 		panel->backlight.set = lpt_set_backlight;
 		panel->backlight.get = lpt_get_backlight;
-		if (HAS_PCH_LPT(dev))
+		if (HAS_PCH_LPT(dev_priv))
 			panel->backlight.hz_to_pwm = lpt_hz_to_pwm;
 		else
 			panel->backlight.hz_to_pwm = spt_hz_to_pwm;
-	} else if (HAS_PCH_SPLIT(dev)) {
+	} else if (HAS_PCH_SPLIT(dev_priv)) {
 		panel->backlight.setup = pch_setup_backlight;
 		panel->backlight.enable = pch_enable_backlight;
 		panel->backlight.disable = pch_disable_backlight;
 		panel->backlight.set = pch_set_backlight;
 		panel->backlight.get = pch_get_backlight;
 		panel->backlight.hz_to_pwm = pch_hz_to_pwm;
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv)) {
 		if (dev_priv->vbt.has_mipi) {
 			panel->backlight.setup = pwm_setup_backlight;
 			panel->backlight.enable = pwm_enable_backlight;
@@ -1782,7 +1759,7 @@ intel_panel_init_backlight_funcs(struct intel_panel *panel)
 			panel->backlight.get = vlv_get_backlight;
 			panel->backlight.hz_to_pwm = vlv_hz_to_pwm;
 		}
-	} else if (IS_GEN4(dev)) {
+	} else if (IS_GEN4(dev_priv)) {
 		panel->backlight.setup = i965_setup_backlight;
 		panel->backlight.enable = i965_enable_backlight;
 		panel->backlight.disable = i965_disable_backlight;
@@ -1828,7 +1805,7 @@ void intel_backlight_register(struct drm_device *dev)
 {
 	struct intel_connector *connector;
 
-	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+	for_each_intel_connector(dev, connector)
 		intel_backlight_device_register(connector);
 }
 
@@ -1836,6 +1813,6 @@ void intel_backlight_unregister(struct drm_device *dev)
 {
 	struct intel_connector *connector;
 
-	list_for_each_entry(connector, &dev->mode_config.connector_list, base.head)
+	for_each_intel_connector(dev, connector)
 		intel_backlight_device_unregister(connector);
 }
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index f091ad12d694..b28c29f20e75 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -66,6 +66,14 @@ static void bxt_init_clock_gating(struct drm_device *dev)
 	 */
 	I915_WRITE(GEN8_UCGCTL6, I915_READ(GEN8_UCGCTL6) |
 		   GEN8_HDCUNIT_CLOCK_GATE_DISABLE_HDCREQ);
+
+	/*
+	 * Wa: Backlight PWM may stop in the asserted state, causing backlight
+	 * to stay fully on.
+	 */
+	if (IS_BXT_REVID(dev_priv, BXT_REVID_B0, REVID_FOREVER))
+		I915_WRITE(GEN9_CLKGATE_DIS_0, I915_READ(GEN9_CLKGATE_DIS_0) |
+			   PWM1_GATING_DIS | PWM2_GATING_DIS);
 }
 
 static void i915_pineview_get_mem_freq(struct drm_device *dev)
@@ -283,7 +291,7 @@ void intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enable)
 	struct drm_device *dev = dev_priv->dev;
 	u32 val;
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		I915_WRITE(FW_BLC_SELF_VLV, enable ? FW_CSPWRDWNEN : 0);
 		POSTING_READ(FW_BLC_SELF_VLV);
 		dev_priv->wm.vlv.cxsr = enable;
@@ -1708,13 +1716,6 @@ static uint32_t ilk_wm_fbc(uint32_t pri_val, uint32_t horiz_pixels,
 	return DIV_ROUND_UP(pri_val * 64, horiz_pixels * bytes_per_pixel) + 2;
 }
 
-struct skl_pipe_wm_parameters {
-	bool active;
-	uint32_t pipe_htotal;
-	uint32_t pixel_rate; /* in KHz */
-	struct intel_plane_wm_parameters plane[I915_MAX_PLANES];
-};
-
 struct ilk_wm_maximums {
 	uint16_t pri;
 	uint16_t spr;
@@ -1722,13 +1723,6 @@ struct ilk_wm_maximums {
 	uint16_t fbc;
 };
 
-/* used in computing the new watermarks state */
-struct intel_wm_config {
-	unsigned int num_pipes_active;
-	bool sprites_enabled;
-	bool sprites_scaled;
-};
-
 /*
  * For both WM_PIPE and WM_LP.
  * mem_value must be in 0.1us units.
@@ -1789,16 +1783,20 @@ static uint32_t ilk_compute_cur_wm(const struct intel_crtc_state *cstate,
 				   const struct intel_plane_state *pstate,
 				   uint32_t mem_value)
 {
-	int bpp = pstate->base.fb ? pstate->base.fb->bits_per_pixel / 8 : 0;
+	/*
+	 * We treat the cursor plane as always-on for the purposes of watermark
+	 * calculation.  Until we have two-stage watermark programming merged,
+	 * this is necessary to avoid flickering.
+	 */
+	int cpp = 4;
+	int width = pstate->visible ? pstate->base.crtc_w : 64;
 
-	if (!cstate->base.active || !pstate->visible)
+	if (!cstate->base.active)
 		return 0;
 
 	return ilk_wm_method2(ilk_pipe_pixel_rate(cstate),
 			      cstate->base.adjusted_mode.crtc_htotal,
-			      drm_rect_width(&pstate->dst),
-			      bpp,
-			      mem_value);
+			      width, cpp, mem_value);
 }
 
 /* Only for WM_LP. */
@@ -1979,9 +1977,11 @@ static void ilk_compute_wm_level(const struct drm_i915_private *dev_priv,
 				 const struct intel_crtc *intel_crtc,
 				 int level,
 				 struct intel_crtc_state *cstate,
+				 struct intel_plane_state *pristate,
+				 struct intel_plane_state *sprstate,
+				 struct intel_plane_state *curstate,
 				 struct intel_wm_level *result)
 {
-	struct intel_plane *intel_plane;
 	uint16_t pri_latency = dev_priv->wm.pri_latency[level];
 	uint16_t spr_latency = dev_priv->wm.spr_latency[level];
 	uint16_t cur_latency = dev_priv->wm.cur_latency[level];
@@ -1993,29 +1993,11 @@ static void ilk_compute_wm_level(const struct drm_i915_private *dev_priv,
 		cur_latency *= 5;
 	}
 
-	for_each_intel_plane_on_crtc(dev_priv->dev, intel_crtc, intel_plane) {
-		struct intel_plane_state *pstate =
-			to_intel_plane_state(intel_plane->base.state);
-
-		switch (intel_plane->base.type) {
-		case DRM_PLANE_TYPE_PRIMARY:
-			result->pri_val = ilk_compute_pri_wm(cstate, pstate,
-							     pri_latency,
-							     level);
-			result->fbc_val = ilk_compute_fbc_wm(cstate, pstate,
-							     result->pri_val);
-			break;
-		case DRM_PLANE_TYPE_OVERLAY:
-			result->spr_val = ilk_compute_spr_wm(cstate, pstate,
-							     spr_latency);
-			break;
-		case DRM_PLANE_TYPE_CURSOR:
-			result->cur_val = ilk_compute_cur_wm(cstate, pstate,
-							     cur_latency);
-			break;
-		}
-	}
-
+	result->pri_val = ilk_compute_pri_wm(cstate, pristate,
+					     pri_latency, level);
+	result->spr_val = ilk_compute_spr_wm(cstate, sprstate, spr_latency);
+	result->cur_val = ilk_compute_cur_wm(cstate, curstate, cur_latency);
+	result->fbc_val = ilk_compute_fbc_wm(cstate, pristate, result->pri_val);
 	result->enable = true;
 }
 
@@ -2274,34 +2256,19 @@ static void skl_setup_wm_latency(struct drm_device *dev)
 	intel_print_wm_latency(dev, "Gen9 Plane", dev_priv->wm.skl_latency);
 }
 
-static void ilk_compute_wm_config(struct drm_device *dev,
-				  struct intel_wm_config *config)
-{
-	struct intel_crtc *intel_crtc;
-
-	/* Compute the currently _active_ config */
-	for_each_intel_crtc(dev, intel_crtc) {
-		const struct intel_pipe_wm *wm = &intel_crtc->wm.active;
-
-		if (!wm->pipe_enabled)
-			continue;
-
-		config->sprites_enabled |= wm->sprites_enabled;
-		config->sprites_scaled |= wm->sprites_scaled;
-		config->num_pipes_active++;
-	}
-}
-
 /* Compute new watermarks for the pipe */
-static bool intel_compute_pipe_wm(struct intel_crtc_state *cstate,
-				  struct intel_pipe_wm *pipe_wm)
+static int ilk_compute_pipe_wm(struct intel_crtc *intel_crtc,
+			       struct drm_atomic_state *state)
 {
-	struct drm_crtc *crtc = cstate->base.crtc;
-	struct drm_device *dev = crtc->dev;
+	struct intel_pipe_wm *pipe_wm;
+	struct drm_device *dev = intel_crtc->base.dev;
 	const struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *cstate = NULL;
 	struct intel_plane *intel_plane;
+	struct drm_plane_state *ps;
+	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);
 	/* LP0 watermark maximums depend on this pipe alone */
 	struct intel_wm_config config = {
@@ -2309,11 +2276,24 @@ static bool intel_compute_pipe_wm(struct intel_crtc_state *cstate,
 	};
 	struct ilk_wm_maximums max;
 
+	cstate = intel_atomic_get_crtc_state(state, intel_crtc);
+	if (IS_ERR(cstate))
+		return PTR_ERR(cstate);
+
+	pipe_wm = &cstate->wm.optimal.ilk;
+
 	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
-		if (intel_plane->base.type == DRM_PLANE_TYPE_OVERLAY) {
-			sprstate = to_intel_plane_state(intel_plane->base.state);
-			break;
-		}
+		ps = drm_atomic_get_plane_state(state,
+						&intel_plane->base);
+		if (IS_ERR(ps))
+			return PTR_ERR(ps);
+
+		if (intel_plane->base.type == DRM_PLANE_TYPE_PRIMARY)
+			pristate = to_intel_plane_state(ps);
+		else if (intel_plane->base.type == DRM_PLANE_TYPE_OVERLAY)
+			sprstate = to_intel_plane_state(ps);
+		else if (intel_plane->base.type == DRM_PLANE_TYPE_CURSOR)
+			curstate = to_intel_plane_state(ps);
 	}
 
 	config.sprites_enabled = sprstate->visible;
@@ -2322,7 +2302,7 @@ static bool intel_compute_pipe_wm(struct intel_crtc_state *cstate,
 		drm_rect_height(&sprstate->dst) != drm_rect_height(&sprstate->src) >> 16);
 
 	pipe_wm->pipe_enabled = cstate->base.active;
-	pipe_wm->sprites_enabled = sprstate->visible;
+	pipe_wm->sprites_enabled = config.sprites_enabled;
 	pipe_wm->sprites_scaled = config.sprites_scaled;
 
 	/* ILK/SNB: LP2+ watermarks only w/o sprites */
@@ -2333,24 +2313,27 @@ static bool intel_compute_pipe_wm(struct intel_crtc_state *cstate,
 	if (config.sprites_scaled)
 		max_level = 0;
 
-	ilk_compute_wm_level(dev_priv, intel_crtc, 0, cstate, &pipe_wm->wm[0]);
+	ilk_compute_wm_level(dev_priv, intel_crtc, 0, cstate,
+			     pristate, sprstate, curstate, &pipe_wm->wm[0]);
 
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
-		pipe_wm->linetime = hsw_compute_linetime_wm(dev, crtc);
+		pipe_wm->linetime = hsw_compute_linetime_wm(dev,
+							    &intel_crtc->base);
 
 	/* LP0 watermarks always use 1/2 DDB partitioning */
 	ilk_compute_wm_maximums(dev, 0, &config, INTEL_DDB_PART_1_2, &max);
 
 	/* At least LP0 must be valid */
 	if (!ilk_validate_wm_level(0, &max, &pipe_wm->wm[0]))
-		return false;
+		return -EINVAL;
 
 	ilk_compute_wm_reg_maximums(dev, 1, &max);
 
 	for (level = 1; level <= max_level; level++) {
 		struct intel_wm_level wm = {};
 
-		ilk_compute_wm_level(dev_priv, intel_crtc, level, cstate, &wm);
+		ilk_compute_wm_level(dev_priv, intel_crtc, level, cstate,
+				     pristate, sprstate, curstate, &wm);
 
 		/*
 		 * Disable any watermark level that exceeds the
@@ -2363,7 +2346,7 @@ static bool intel_compute_pipe_wm(struct intel_crtc_state *cstate,
 		pipe_wm->wm[level] = wm;
 	}
 
-	return true;
+	return 0;
 }
 
 /*
@@ -2378,7 +2361,9 @@ static void ilk_merge_wm_level(struct drm_device *dev,
 	ret_wm->enable = true;
 
 	for_each_intel_crtc(dev, intel_crtc) {
-		const struct intel_pipe_wm *active = &intel_crtc->wm.active;
+		const struct intel_crtc_state *cstate =
+			to_intel_crtc_state(intel_crtc->base.state);
+		const struct intel_pipe_wm *active = &cstate->wm.optimal.ilk;
 		const struct intel_wm_level *wm = &active->wm[level];
 
 		if (!active->pipe_enabled)
@@ -2449,7 +2434,7 @@ static void ilk_wm_merge(struct drm_device *dev,
 	 * enabled sometime later.
 	 */
 	if (IS_GEN5(dev) && !merged->fbc_wm_enabled &&
-	    intel_fbc_enabled(dev_priv)) {
+	    intel_fbc_is_active(dev_priv)) {
 		for (level = 2; level <= max_level; level++) {
 			struct intel_wm_level *wm = &merged->wm[level];
 
@@ -2526,14 +2511,15 @@ static void ilk_compute_wm_results(struct drm_device *dev,
 
 	/* LP0 register values */
 	for_each_intel_crtc(dev, intel_crtc) {
+		const struct intel_crtc_state *cstate =
+			to_intel_crtc_state(intel_crtc->base.state);
 		enum pipe pipe = intel_crtc->pipe;
-		const struct intel_wm_level *r =
-			&intel_crtc->wm.active.wm[0];
+		const struct intel_wm_level *r = &cstate->wm.optimal.ilk.wm[0];
 
 		if (WARN_ON(!r->enable))
 			continue;
 
-		results->wm_linetime[pipe] = intel_crtc->wm.active.linetime;
+		results->wm_linetime[pipe] = cstate->wm.optimal.ilk.linetime;
 
 		results->wm_pipe[pipe] =
 			(r->pri_val << WM0_PIPE_PLANE_SHIFT) |
@@ -2755,18 +2741,40 @@ static bool ilk_disable_lp_wm(struct drm_device *dev)
 #define SKL_DDB_SIZE		896	/* in blocks */
 #define BXT_DDB_SIZE		512
 
+/*
+ * Return the index of a plane in the SKL DDB and wm result arrays.  Primary
+ * plane is always in slot 0, cursor is always in slot I915_MAX_PLANES-1, and
+ * other universal planes are in indices 1..n.  Note that this may leave unused
+ * indices between the top "sprite" plane and the cursor.
+ */
+static int
+skl_wm_plane_id(const struct intel_plane *plane)
+{
+	switch (plane->base.type) {
+	case DRM_PLANE_TYPE_PRIMARY:
+		return 0;
+	case DRM_PLANE_TYPE_CURSOR:
+		return PLANE_CURSOR;
+	case DRM_PLANE_TYPE_OVERLAY:
+		return plane->plane + 1;
+	default:
+		MISSING_CASE(plane->base.type);
+		return plane->plane;
+	}
+}
+
 static void
 skl_ddb_get_pipe_allocation_limits(struct drm_device *dev,
-				   struct drm_crtc *for_crtc,
+				   const struct intel_crtc_state *cstate,
 				   const struct intel_wm_config *config,
-				   const struct skl_pipe_wm_parameters *params,
 				   struct skl_ddb_entry *alloc /* out */)
 {
+	struct drm_crtc *for_crtc = cstate->base.crtc;
 	struct drm_crtc *crtc;
 	unsigned int pipe_size, ddb_size;
 	int nth_active_pipe;
 
-	if (!params->active) {
+	if (!cstate->base.active) {
 		alloc->start = 0;
 		alloc->end = 0;
 		return;
@@ -2821,7 +2829,10 @@ void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
 	memset(ddb, 0, sizeof(*ddb));
 
 	for_each_pipe(dev_priv, pipe) {
-		if (!intel_display_power_is_enabled(dev_priv, POWER_DOMAIN_PIPE(pipe)))
+		enum intel_display_power_domain power_domain;
+
+		power_domain = POWER_DOMAIN_PIPE(pipe);
+		if (!intel_display_power_get_if_enabled(dev_priv, power_domain))
 			continue;
 
 		for_each_plane(dev_priv, pipe, plane) {
@@ -2833,23 +2844,35 @@ void skl_ddb_get_hw_state(struct drm_i915_private *dev_priv,
 		val = I915_READ(CUR_BUF_CFG(pipe));
 		skl_ddb_entry_init_from_hw(&ddb->plane[pipe][PLANE_CURSOR],
 					   val);
+
+		intel_display_power_put(dev_priv, power_domain);
 	}
 }
 
 static unsigned int
-skl_plane_relative_data_rate(const struct intel_plane_wm_parameters *p, int y)
+skl_plane_relative_data_rate(const struct intel_crtc_state *cstate,
+			     const struct drm_plane_state *pstate,
+			     int y)
 {
+	struct intel_crtc *intel_crtc = to_intel_crtc(cstate->base.crtc);
+	struct drm_framebuffer *fb = pstate->fb;
 
 	/* for planar format */
-	if (p->y_bytes_per_pixel) {
+	if (fb->pixel_format == DRM_FORMAT_NV12) {
 		if (y)  /* y-plane data rate */
-			return p->horiz_pixels * p->vert_pixels * p->y_bytes_per_pixel;
+			return intel_crtc->config->pipe_src_w *
+				intel_crtc->config->pipe_src_h *
+				drm_format_plane_cpp(fb->pixel_format, 0);
 		else    /* uv-plane data rate */
-			return (p->horiz_pixels/2) * (p->vert_pixels/2) * p->bytes_per_pixel;
+			return (intel_crtc->config->pipe_src_w/2) *
+				(intel_crtc->config->pipe_src_h/2) *
+				drm_format_plane_cpp(fb->pixel_format, 1);
 	}
 
 	/* for packed formats */
-	return p->horiz_pixels * p->vert_pixels * p->bytes_per_pixel;
+	return intel_crtc->config->pipe_src_w *
+		intel_crtc->config->pipe_src_h *
+		drm_format_plane_cpp(fb->pixel_format, 0);
 }
 
 /*
@@ -2858,46 +2881,55 @@ skl_plane_relative_data_rate(const struct intel_plane_wm_parameters *p, int y)
  *   3 * 4096 * 8192  * 4 < 2^32
  */
 static unsigned int
-skl_get_total_relative_data_rate(struct intel_crtc *intel_crtc,
-				 const struct skl_pipe_wm_parameters *params)
+skl_get_total_relative_data_rate(const struct intel_crtc_state *cstate)
 {
+	struct intel_crtc *intel_crtc = to_intel_crtc(cstate->base.crtc);
+	struct drm_device *dev = intel_crtc->base.dev;
+	const struct intel_plane *intel_plane;
 	unsigned int total_data_rate = 0;
-	int plane;
 
-	for (plane = 0; plane < intel_num_planes(intel_crtc); plane++) {
-		const struct intel_plane_wm_parameters *p;
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		const struct drm_plane_state *pstate = intel_plane->base.state;
 
-		p = &params->plane[plane];
-		if (!p->enabled)
+		if (pstate->fb == NULL)
 			continue;
 
-		total_data_rate += skl_plane_relative_data_rate(p, 0); /* packed/uv */
-		if (p->y_bytes_per_pixel) {
-			total_data_rate += skl_plane_relative_data_rate(p, 1); /* y-plane */
-		}
+		if (intel_plane->base.type == DRM_PLANE_TYPE_CURSOR)
+			continue;
+
+		/* packed/uv */
+		total_data_rate += skl_plane_relative_data_rate(cstate,
+								pstate,
+								0);
+
+		if (pstate->fb->pixel_format == DRM_FORMAT_NV12)
+			/* y-plane */
+			total_data_rate += skl_plane_relative_data_rate(cstate,
+									pstate,
+									1);
 	}
 
 	return total_data_rate;
 }
 
 static void
-skl_allocate_pipe_ddb(struct drm_crtc *crtc,
-		      const struct intel_wm_config *config,
-		      const struct skl_pipe_wm_parameters *params,
+skl_allocate_pipe_ddb(struct intel_crtc_state *cstate,
 		      struct skl_ddb_allocation *ddb /* out */)
 {
+	struct drm_crtc *crtc = cstate->base.crtc;
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = to_i915(dev);
+	struct intel_wm_config *config = &dev_priv->wm.config;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_plane *intel_plane;
 	enum pipe pipe = intel_crtc->pipe;
 	struct skl_ddb_entry *alloc = &ddb->pipe[pipe];
 	uint16_t alloc_size, start, cursor_blocks;
 	uint16_t minimum[I915_MAX_PLANES];
 	uint16_t y_minimum[I915_MAX_PLANES];
 	unsigned int total_data_rate;
-	int plane;
 
-	skl_ddb_get_pipe_allocation_limits(dev, crtc, config, params, alloc);
+	skl_ddb_get_pipe_allocation_limits(dev, cstate, config, alloc);
 	alloc_size = skl_ddb_entry_size(alloc);
 	if (alloc_size == 0) {
 		memset(ddb->plane[pipe], 0, sizeof(ddb->plane[pipe]));
@@ -2914,17 +2946,20 @@ skl_allocate_pipe_ddb(struct drm_crtc *crtc,
 	alloc->end -= cursor_blocks;
 
 	/* 1. Allocate the mininum required blocks for each active plane */
-	for_each_plane(dev_priv, pipe, plane) {
-		const struct intel_plane_wm_parameters *p;
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		struct drm_plane *plane = &intel_plane->base;
+		struct drm_framebuffer *fb = plane->state->fb;
+		int id = skl_wm_plane_id(intel_plane);
 
-		p = &params->plane[plane];
-		if (!p->enabled)
+		if (fb == NULL)
+			continue;
+		if (plane->type == DRM_PLANE_TYPE_CURSOR)
 			continue;
 
-		minimum[plane] = 8;
-		alloc_size -= minimum[plane];
-		y_minimum[plane] = p->y_bytes_per_pixel ? 8 : 0;
-		alloc_size -= y_minimum[plane];
+		minimum[id] = 8;
+		alloc_size -= minimum[id];
+		y_minimum[id] = (fb->pixel_format == DRM_FORMAT_NV12) ? 8 : 0;
+		alloc_size -= y_minimum[id];
 	}
 
 	/*
@@ -2933,45 +2968,50 @@ skl_allocate_pipe_ddb(struct drm_crtc *crtc,
 	 *
 	 * FIXME: we may not allocate every single block here.
 	 */
-	total_data_rate = skl_get_total_relative_data_rate(intel_crtc, params);
+	total_data_rate = skl_get_total_relative_data_rate(cstate);
 
 	start = alloc->start;
-	for (plane = 0; plane < intel_num_planes(intel_crtc); plane++) {
-		const struct intel_plane_wm_parameters *p;
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		struct drm_plane *plane = &intel_plane->base;
+		struct drm_plane_state *pstate = intel_plane->base.state;
 		unsigned int data_rate, y_data_rate;
 		uint16_t plane_blocks, y_plane_blocks = 0;
+		int id = skl_wm_plane_id(intel_plane);
 
-		p = &params->plane[plane];
-		if (!p->enabled)
+		if (pstate->fb == NULL)
+			continue;
+		if (plane->type == DRM_PLANE_TYPE_CURSOR)
 			continue;
 
-		data_rate = skl_plane_relative_data_rate(p, 0);
+		data_rate = skl_plane_relative_data_rate(cstate, pstate, 0);
 
 		/*
 		 * allocation for (packed formats) or (uv-plane part of planar format):
 		 * promote the expression to 64 bits to avoid overflowing, the
 		 * result is < available as data_rate / total_data_rate < 1
 		 */
-		plane_blocks = minimum[plane];
+		plane_blocks = minimum[id];
 		plane_blocks += div_u64((uint64_t)alloc_size * data_rate,
 					total_data_rate);
 
-		ddb->plane[pipe][plane].start = start;
-		ddb->plane[pipe][plane].end = start + plane_blocks;
+		ddb->plane[pipe][id].start = start;
+		ddb->plane[pipe][id].end = start + plane_blocks;
 
 		start += plane_blocks;
 
 		/*
 		 * allocation for y_plane part of planar format:
 		 */
-		if (p->y_bytes_per_pixel) {
-			y_data_rate = skl_plane_relative_data_rate(p, 1);
-			y_plane_blocks = y_minimum[plane];
+		if (pstate->fb->pixel_format == DRM_FORMAT_NV12) {
+			y_data_rate = skl_plane_relative_data_rate(cstate,
+								   pstate,
+								   1);
+			y_plane_blocks = y_minimum[id];
 			y_plane_blocks += div_u64((uint64_t)alloc_size * y_data_rate,
 						total_data_rate);
 
-			ddb->y_plane[pipe][plane].start = start;
-			ddb->y_plane[pipe][plane].end = start + y_plane_blocks;
+			ddb->y_plane[pipe][id].start = start;
+			ddb->y_plane[pipe][id].end = start + y_plane_blocks;
 
 			start += y_plane_blocks;
 		}
@@ -3041,104 +3081,27 @@ static bool skl_ddb_allocation_changed(const struct skl_ddb_allocation *new_ddb,
 	struct drm_device *dev = intel_crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	const struct skl_ddb_allocation *cur_ddb = &dev_priv->wm.skl_hw.ddb;
-	enum pipe pipe = intel_crtc->pipe;
-
-	if (memcmp(new_ddb->plane[pipe], cur_ddb->plane[pipe],
-		   sizeof(new_ddb->plane[pipe])))
-		return true;
 
-	if (memcmp(&new_ddb->plane[pipe][PLANE_CURSOR], &cur_ddb->plane[pipe][PLANE_CURSOR],
-		    sizeof(new_ddb->plane[pipe][PLANE_CURSOR])))
+	/*
+	 * If ddb allocation of pipes changed, it may require recalculation of
+	 * watermarks
+	 */
+	if (memcmp(new_ddb->pipe, cur_ddb->pipe, sizeof(new_ddb->pipe)))
 		return true;
 
 	return false;
 }
 
-static void skl_compute_wm_global_parameters(struct drm_device *dev,
-					     struct intel_wm_config *config)
-{
-	struct drm_crtc *crtc;
-	struct drm_plane *plane;
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		config->num_pipes_active += to_intel_crtc(crtc)->active;
-
-	/* FIXME: I don't think we need those two global parameters on SKL */
-	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
-		struct intel_plane *intel_plane = to_intel_plane(plane);
-
-		config->sprites_enabled |= intel_plane->wm.enabled;
-		config->sprites_scaled |= intel_plane->wm.scaled;
-	}
-}
-
-static void skl_compute_wm_pipe_parameters(struct drm_crtc *crtc,
-					   struct skl_pipe_wm_parameters *p)
-{
-	struct drm_device *dev = crtc->dev;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	enum pipe pipe = intel_crtc->pipe;
-	struct drm_plane *plane;
-	struct drm_framebuffer *fb;
-	int i = 1; /* Index for sprite planes start */
-
-	p->active = intel_crtc->active;
-	if (p->active) {
-		p->pipe_htotal = intel_crtc->config->base.adjusted_mode.crtc_htotal;
-		p->pixel_rate = skl_pipe_pixel_rate(intel_crtc->config);
-
-		fb = crtc->primary->state->fb;
-		/* For planar: Bpp is for uv plane, y_Bpp is for y plane */
-		if (fb) {
-			p->plane[0].enabled = true;
-			p->plane[0].bytes_per_pixel = fb->pixel_format == DRM_FORMAT_NV12 ?
-				drm_format_plane_cpp(fb->pixel_format, 1) :
-				drm_format_plane_cpp(fb->pixel_format, 0);
-			p->plane[0].y_bytes_per_pixel = fb->pixel_format == DRM_FORMAT_NV12 ?
-				drm_format_plane_cpp(fb->pixel_format, 0) : 0;
-			p->plane[0].tiling = fb->modifier[0];
-		} else {
-			p->plane[0].enabled = false;
-			p->plane[0].bytes_per_pixel = 0;
-			p->plane[0].y_bytes_per_pixel = 0;
-			p->plane[0].tiling = DRM_FORMAT_MOD_NONE;
-		}
-		p->plane[0].horiz_pixels = intel_crtc->config->pipe_src_w;
-		p->plane[0].vert_pixels = intel_crtc->config->pipe_src_h;
-		p->plane[0].rotation = crtc->primary->state->rotation;
-
-		fb = crtc->cursor->state->fb;
-		p->plane[PLANE_CURSOR].y_bytes_per_pixel = 0;
-		if (fb) {
-			p->plane[PLANE_CURSOR].enabled = true;
-			p->plane[PLANE_CURSOR].bytes_per_pixel = fb->bits_per_pixel / 8;
-			p->plane[PLANE_CURSOR].horiz_pixels = crtc->cursor->state->crtc_w;
-			p->plane[PLANE_CURSOR].vert_pixels = crtc->cursor->state->crtc_h;
-		} else {
-			p->plane[PLANE_CURSOR].enabled = false;
-			p->plane[PLANE_CURSOR].bytes_per_pixel = 0;
-			p->plane[PLANE_CURSOR].horiz_pixels = 64;
-			p->plane[PLANE_CURSOR].vert_pixels = 64;
-		}
-	}
-
-	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
-		struct intel_plane *intel_plane = to_intel_plane(plane);
-
-		if (intel_plane->pipe == pipe &&
-			plane->type == DRM_PLANE_TYPE_OVERLAY)
-			p->plane[i++] = intel_plane->wm;
-	}
-}
-
 static bool skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
-				 struct skl_pipe_wm_parameters *p,
-				 struct intel_plane_wm_parameters *p_params,
+				 struct intel_crtc_state *cstate,
+				 struct intel_plane *intel_plane,
 				 uint16_t ddb_allocation,
 				 int level,
 				 uint16_t *out_blocks, /* out */
 				 uint8_t *out_lines /* out */)
 {
+	struct drm_plane *plane = &intel_plane->base;
+	struct drm_framebuffer *fb = plane->state->fb;
 	uint32_t latency = dev_priv->wm.skl_latency[level];
 	uint32_t method1, method2;
 	uint32_t plane_bytes_per_line, plane_blocks_per_line;
@@ -3146,31 +3109,33 @@ static bool skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	uint32_t selected_result;
 	uint8_t bytes_per_pixel;
 
-	if (latency == 0 || !p->active || !p_params->enabled)
+	if (latency == 0 || !cstate->base.active || !fb)
 		return false;
 
-	bytes_per_pixel = p_params->y_bytes_per_pixel ?
-		p_params->y_bytes_per_pixel :
-		p_params->bytes_per_pixel;
-	method1 = skl_wm_method1(p->pixel_rate,
+	bytes_per_pixel = drm_format_plane_cpp(fb->pixel_format, 0);
+	method1 = skl_wm_method1(skl_pipe_pixel_rate(cstate),
 				 bytes_per_pixel,
 				 latency);
-	method2 = skl_wm_method2(p->pixel_rate,
-				 p->pipe_htotal,
-				 p_params->horiz_pixels,
+	method2 = skl_wm_method2(skl_pipe_pixel_rate(cstate),
+				 cstate->base.adjusted_mode.crtc_htotal,
+				 cstate->pipe_src_w,
 				 bytes_per_pixel,
-				 p_params->tiling,
+				 fb->modifier[0],
 				 latency);
 
-	plane_bytes_per_line = p_params->horiz_pixels * bytes_per_pixel;
+	plane_bytes_per_line = cstate->pipe_src_w * bytes_per_pixel;
 	plane_blocks_per_line = DIV_ROUND_UP(plane_bytes_per_line, 512);
 
-	if (p_params->tiling == I915_FORMAT_MOD_Y_TILED ||
-	    p_params->tiling == I915_FORMAT_MOD_Yf_TILED) {
+	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 (intel_rotation_90_or_270(p_params->rotation)) {
-			switch (p_params->bytes_per_pixel) {
+		if (intel_rotation_90_or_270(plane->state->rotation)) {
+			int bpp = (fb->pixel_format == DRM_FORMAT_NV12) ?
+				drm_format_plane_cpp(fb->pixel_format, 1) :
+				drm_format_plane_cpp(fb->pixel_format, 0);
+
+			switch (bpp) {
 			case 1:
 				min_scanlines = 16;
 				break;
@@ -3194,8 +3159,8 @@ static bool skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 	res_lines = DIV_ROUND_UP(selected_result, plane_blocks_per_line);
 
 	if (level >= 1 && level <= 7) {
-		if (p_params->tiling == I915_FORMAT_MOD_Y_TILED ||
-		    p_params->tiling == I915_FORMAT_MOD_Yf_TILED)
+		if (fb->modifier[0] == I915_FORMAT_MOD_Y_TILED ||
+		    fb->modifier[0] == I915_FORMAT_MOD_Yf_TILED)
 			res_lines += 4;
 		else
 			res_blocks++;
@@ -3212,84 +3177,80 @@ static bool skl_compute_plane_wm(const struct drm_i915_private *dev_priv,
 
 static void skl_compute_wm_level(const struct drm_i915_private *dev_priv,
 				 struct skl_ddb_allocation *ddb,
-				 struct skl_pipe_wm_parameters *p,
-				 enum pipe pipe,
+				 struct intel_crtc_state *cstate,
 				 int level,
-				 int num_planes,
 				 struct skl_wm_level *result)
 {
+	struct drm_device *dev = dev_priv->dev;
+	struct intel_crtc *intel_crtc = to_intel_crtc(cstate->base.crtc);
+	struct intel_plane *intel_plane;
 	uint16_t ddb_blocks;
-	int i;
+	enum pipe pipe = intel_crtc->pipe;
+
+	for_each_intel_plane_on_crtc(dev, intel_crtc, intel_plane) {
+		int i = skl_wm_plane_id(intel_plane);
 
-	for (i = 0; i < num_planes; i++) {
 		ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][i]);
 
 		result->plane_en[i] = skl_compute_plane_wm(dev_priv,
-						p, &p->plane[i],
+						cstate,
+						intel_plane,
 						ddb_blocks,
 						level,
 						&result->plane_res_b[i],
 						&result->plane_res_l[i]);
 	}
-
-	ddb_blocks = skl_ddb_entry_size(&ddb->plane[pipe][PLANE_CURSOR]);
-	result->plane_en[PLANE_CURSOR] = skl_compute_plane_wm(dev_priv, p,
-						 &p->plane[PLANE_CURSOR],
-						 ddb_blocks, level,
-						 &result->plane_res_b[PLANE_CURSOR],
-						 &result->plane_res_l[PLANE_CURSOR]);
 }
 
 static uint32_t
-skl_compute_linetime_wm(struct drm_crtc *crtc, struct skl_pipe_wm_parameters *p)
+skl_compute_linetime_wm(struct intel_crtc_state *cstate)
 {
-	if (!to_intel_crtc(crtc)->active)
+	if (!cstate->base.active)
 		return 0;
 
-	if (WARN_ON(p->pixel_rate == 0))
+	if (WARN_ON(skl_pipe_pixel_rate(cstate) == 0))
 		return 0;
 
-	return DIV_ROUND_UP(8 * p->pipe_htotal * 1000, p->pixel_rate);
+	return DIV_ROUND_UP(8 * cstate->base.adjusted_mode.crtc_htotal * 1000,
+			    skl_pipe_pixel_rate(cstate));
 }
 
-static void skl_compute_transition_wm(struct drm_crtc *crtc,
-				      struct skl_pipe_wm_parameters *params,
+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);
-	int i;
+	struct intel_plane *intel_plane;
 
-	if (!params->active)
+	if (!cstate->base.active)
 		return;
 
 	/* Until we know more, just disable transition WMs */
-	for (i = 0; i < intel_num_planes(intel_crtc); i++)
+	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[PLANE_CURSOR] = false;
+	}
 }
 
-static void skl_compute_pipe_wm(struct drm_crtc *crtc,
+static void skl_compute_pipe_wm(struct intel_crtc_state *cstate,
 				struct skl_ddb_allocation *ddb,
-				struct skl_pipe_wm_parameters *params,
 				struct skl_pipe_wm *pipe_wm)
 {
-	struct drm_device *dev = crtc->dev;
+	struct drm_device *dev = cstate->base.crtc->dev;
 	const struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int level, max_level = ilk_wm_max_level(dev);
 
 	for (level = 0; level <= max_level; level++) {
-		skl_compute_wm_level(dev_priv, ddb, params, intel_crtc->pipe,
-				     level, intel_num_planes(intel_crtc),
-				     &pipe_wm->wm[level]);
+		skl_compute_wm_level(dev_priv, ddb, cstate,
+				     level, &pipe_wm->wm[level]);
 	}
-	pipe_wm->linetime = skl_compute_linetime_wm(crtc, params);
+	pipe_wm->linetime = skl_compute_linetime_wm(cstate);
 
-	skl_compute_transition_wm(crtc, params, &pipe_wm->trans_wm);
+	skl_compute_transition_wm(cstate, &pipe_wm->trans_wm);
 }
 
 static void skl_compute_wm_results(struct drm_device *dev,
-				   struct skl_pipe_wm_parameters *p,
 				   struct skl_pipe_wm *p_wm,
 				   struct skl_wm_values *r,
 				   struct intel_crtc *intel_crtc)
@@ -3346,7 +3307,8 @@ static void skl_compute_wm_results(struct drm_device *dev,
 	r->wm_linetime[pipe] = p_wm->linetime;
 }
 
-static void skl_ddb_entry_write(struct drm_i915_private *dev_priv, uint32_t reg,
+static void skl_ddb_entry_write(struct drm_i915_private *dev_priv,
+				i915_reg_t reg,
 				const struct skl_ddb_entry *entry)
 {
 	if (entry->end)
@@ -3361,7 +3323,7 @@ static void skl_write_wm_values(struct drm_i915_private *dev_priv,
 	struct drm_device *dev = dev_priv->dev;
 	struct intel_crtc *crtc;
 
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, base.head) {
+	for_each_intel_crtc(dev, crtc) {
 		int i, level, max_level = ilk_wm_max_level(dev);
 		enum pipe pipe = crtc->pipe;
 
@@ -3533,28 +3495,25 @@ static void skl_flush_wm_values(struct drm_i915_private *dev_priv,
 }
 
 static bool skl_update_pipe_wm(struct drm_crtc *crtc,
-			       struct skl_pipe_wm_parameters *params,
-			       struct intel_wm_config *config,
 			       struct skl_ddb_allocation *ddb, /* out */
 			       struct skl_pipe_wm *pipe_wm /* out */)
 {
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
 
-	skl_compute_wm_pipe_parameters(crtc, params);
-	skl_allocate_pipe_ddb(crtc, config, params, ddb);
-	skl_compute_pipe_wm(crtc, ddb, params, pipe_wm);
+	skl_allocate_pipe_ddb(cstate, ddb);
+	skl_compute_pipe_wm(cstate, ddb, pipe_wm);
 
-	if (!memcmp(&intel_crtc->wm.skl_active, pipe_wm, sizeof(*pipe_wm)))
+	if (!memcmp(&intel_crtc->wm.active.skl, pipe_wm, sizeof(*pipe_wm)))
 		return false;
 
-	intel_crtc->wm.skl_active = *pipe_wm;
+	intel_crtc->wm.active.skl = *pipe_wm;
 
 	return true;
 }
 
 static void skl_update_other_pipe_wm(struct drm_device *dev,
 				     struct drm_crtc *crtc,
-				     struct intel_wm_config *config,
 				     struct skl_wm_values *r)
 {
 	struct intel_crtc *intel_crtc;
@@ -3573,9 +3532,7 @@ static void skl_update_other_pipe_wm(struct drm_device *dev,
 	 * Otherwise, because of this_crtc being freshly enabled/disabled, the
 	 * other active pipes need new DDB allocation and WM values.
 	 */
-	list_for_each_entry(intel_crtc, &dev->mode_config.crtc_list,
-				base.head) {
-		struct skl_pipe_wm_parameters params = {};
+	for_each_intel_crtc(dev, intel_crtc) {
 		struct skl_pipe_wm pipe_wm = {};
 		bool wm_changed;
 
@@ -3586,7 +3543,6 @@ static void skl_update_other_pipe_wm(struct drm_device *dev,
 			continue;
 
 		wm_changed = skl_update_pipe_wm(&intel_crtc->base,
-						&params, config,
 						&r->ddb, &pipe_wm);
 
 		/*
@@ -3596,7 +3552,7 @@ static void skl_update_other_pipe_wm(struct drm_device *dev,
 		 */
 		WARN_ON(!wm_changed);
 
-		skl_compute_wm_results(dev, &params, &pipe_wm, r, intel_crtc);
+		skl_compute_wm_results(dev, &pipe_wm, r, intel_crtc);
 		r->dirty[intel_crtc->pipe] = true;
 	}
 }
@@ -3626,10 +3582,9 @@ static void skl_update_wm(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct skl_pipe_wm_parameters params = {};
 	struct skl_wm_values *results = &dev_priv->wm.skl_results;
-	struct skl_pipe_wm pipe_wm = {};
-	struct intel_wm_config config = {};
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
+	struct skl_pipe_wm *pipe_wm = &cstate->wm.optimal.skl;
 
 
 	/* Clear all dirty flags */
@@ -3637,16 +3592,13 @@ static void skl_update_wm(struct drm_crtc *crtc)
 
 	skl_clear_wm(results, intel_crtc->pipe);
 
-	skl_compute_wm_global_parameters(dev, &config);
-
-	if (!skl_update_pipe_wm(crtc, &params, &config,
-				&results->ddb, &pipe_wm))
+	if (!skl_update_pipe_wm(crtc, &results->ddb, pipe_wm))
 		return;
 
-	skl_compute_wm_results(dev, &params, &pipe_wm, results, intel_crtc);
+	skl_compute_wm_results(dev, pipe_wm, results, intel_crtc);
 	results->dirty[intel_crtc->pipe] = true;
 
-	skl_update_other_pipe_wm(dev, crtc, &config, results);
+	skl_update_other_pipe_wm(dev, crtc, results);
 	skl_write_wm_values(dev_priv, results);
 	skl_flush_wm_values(dev_priv, results);
 
@@ -3654,71 +3606,23 @@ static void skl_update_wm(struct drm_crtc *crtc)
 	dev_priv->wm.skl_hw = *results;
 }
 
-static void
-skl_update_sprite_wm(struct drm_plane *plane, struct drm_crtc *crtc,
-		     uint32_t sprite_width, uint32_t sprite_height,
-		     int pixel_size, bool enabled, bool scaled)
+static void ilk_program_watermarks(struct drm_i915_private *dev_priv)
 {
-	struct intel_plane *intel_plane = to_intel_plane(plane);
-	struct drm_framebuffer *fb = plane->state->fb;
-
-	intel_plane->wm.enabled = enabled;
-	intel_plane->wm.scaled = scaled;
-	intel_plane->wm.horiz_pixels = sprite_width;
-	intel_plane->wm.vert_pixels = sprite_height;
-	intel_plane->wm.tiling = DRM_FORMAT_MOD_NONE;
-
-	/* For planar: Bpp is for UV plane, y_Bpp is for Y plane */
-	intel_plane->wm.bytes_per_pixel =
-		(fb && fb->pixel_format == DRM_FORMAT_NV12) ?
-		drm_format_plane_cpp(plane->state->fb->pixel_format, 1) : pixel_size;
-	intel_plane->wm.y_bytes_per_pixel =
-		(fb && fb->pixel_format == DRM_FORMAT_NV12) ?
-		drm_format_plane_cpp(plane->state->fb->pixel_format, 0) : 0;
-
-	/*
-	 * Framebuffer can be NULL on plane disable, but it does not
-	 * matter for watermarks if we assume no tiling in that case.
-	 */
-	if (fb)
-		intel_plane->wm.tiling = fb->modifier[0];
-	intel_plane->wm.rotation = plane->state->rotation;
-
-	skl_update_wm(crtc);
-}
-
-static void ilk_update_wm(struct drm_crtc *crtc)
-{
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_device *dev = dev_priv->dev;
+	struct intel_pipe_wm lp_wm_1_2 = {}, lp_wm_5_6 = {}, *best_lp_wm;
 	struct ilk_wm_maximums max;
+	struct intel_wm_config *config = &dev_priv->wm.config;
 	struct ilk_wm_values results = {};
 	enum intel_ddb_partitioning partitioning;
-	struct intel_pipe_wm pipe_wm = {};
-	struct intel_pipe_wm lp_wm_1_2 = {}, lp_wm_5_6 = {}, *best_lp_wm;
-	struct intel_wm_config config = {};
-
-	WARN_ON(cstate->base.active != intel_crtc->active);
 
-	intel_compute_pipe_wm(cstate, &pipe_wm);
-
-	if (!memcmp(&intel_crtc->wm.active, &pipe_wm, sizeof(pipe_wm)))
-		return;
-
-	intel_crtc->wm.active = pipe_wm;
-
-	ilk_compute_wm_config(dev, &config);
-
-	ilk_compute_wm_maximums(dev, 1, &config, INTEL_DDB_PART_1_2, &max);
-	ilk_wm_merge(dev, &config, &max, &lp_wm_1_2);
+	ilk_compute_wm_maximums(dev, 1, config, INTEL_DDB_PART_1_2, &max);
+	ilk_wm_merge(dev, config, &max, &lp_wm_1_2);
 
 	/* 5/6 split only in single pipe config on IVB+ */
 	if (INTEL_INFO(dev)->gen >= 7 &&
-	    config.num_pipes_active == 1 && config.sprites_enabled) {
-		ilk_compute_wm_maximums(dev, 1, &config, INTEL_DDB_PART_5_6, &max);
-		ilk_wm_merge(dev, &config, &max, &lp_wm_5_6);
+	    config->num_pipes_active == 1 && config->sprites_enabled) {
+		ilk_compute_wm_maximums(dev, 1, config, INTEL_DDB_PART_5_6, &max);
+		ilk_wm_merge(dev, config, &max, &lp_wm_5_6);
 
 		best_lp_wm = ilk_find_best_result(dev, &lp_wm_1_2, &lp_wm_5_6);
 	} else {
@@ -3733,14 +3637,13 @@ static void ilk_update_wm(struct drm_crtc *crtc)
 	ilk_write_wm_values(dev_priv, &results);
 }
 
-static void
-ilk_update_sprite_wm(struct drm_plane *plane,
-		     struct drm_crtc *crtc,
-		     uint32_t sprite_width, uint32_t sprite_height,
-		     int pixel_size, bool enabled, bool scaled)
+static void ilk_update_wm(struct drm_crtc *crtc)
 {
-	struct drm_device *dev = plane->dev;
-	struct intel_plane *intel_plane = to_intel_plane(plane);
+	struct drm_i915_private *dev_priv = to_i915(crtc->dev);
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
+
+	WARN_ON(cstate->base.active != intel_crtc->active);
 
 	/*
 	 * IVB workaround: must disable low power watermarks for at least
@@ -3749,10 +3652,14 @@ ilk_update_sprite_wm(struct drm_plane *plane,
 	 *
 	 * WaCxSRDisabledForSpriteScaling:ivb
 	 */
-	if (IS_IVYBRIDGE(dev) && scaled && ilk_disable_lp_wm(dev))
-		intel_wait_for_vblank(dev, intel_plane->pipe);
+	if (cstate->disable_lp_wm) {
+		ilk_disable_lp_wm(crtc->dev);
+		intel_wait_for_vblank(crtc->dev, intel_crtc->pipe);
+	}
+
+	intel_crtc->wm.active.ilk = cstate->wm.optimal.ilk;
 
-	ilk_update_wm(crtc);
+	ilk_program_watermarks(dev_priv);
 }
 
 static void skl_pipe_wm_active_state(uint32_t val,
@@ -3805,7 +3712,8 @@ static void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct skl_wm_values *hw = &dev_priv->wm.skl_hw;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct skl_pipe_wm *active = &intel_crtc->wm.skl_active;
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
+	struct skl_pipe_wm *active = &cstate->wm.optimal.skl;
 	enum pipe pipe = intel_crtc->pipe;
 	int level, i, max_level;
 	uint32_t temp;
@@ -3849,6 +3757,8 @@ static void skl_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 
 	temp = hw->plane_trans[pipe][PLANE_CURSOR];
 	skl_pipe_wm_active_state(temp, active, true, true, i, 0);
+
+	intel_crtc->wm.active.skl = *active;
 }
 
 void skl_wm_get_hw_state(struct drm_device *dev)
@@ -3868,9 +3778,10 @@ static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct ilk_wm_values *hw = &dev_priv->wm.hw;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_pipe_wm *active = &intel_crtc->wm.active;
+	struct intel_crtc_state *cstate = to_intel_crtc_state(crtc->state);
+	struct intel_pipe_wm *active = &cstate->wm.optimal.ilk;
 	enum pipe pipe = intel_crtc->pipe;
-	static const unsigned int wm0_pipe_reg[] = {
+	static const i915_reg_t wm0_pipe_reg[] = {
 		[PIPE_A] = WM0_PIPEA_ILK,
 		[PIPE_B] = WM0_PIPEB_ILK,
 		[PIPE_C] = WM0_PIPEC_IVB,
@@ -3907,6 +3818,8 @@ static void ilk_pipe_wm_get_hw_state(struct drm_crtc *crtc)
 		for (level = 0; level <= max_level; level++)
 			active->wm[level].enable = true;
 	}
+
+	intel_crtc->wm.active.ilk = *active;
 }
 
 #define _FW_WM(value, plane) \
@@ -4132,21 +4045,6 @@ void intel_update_watermarks(struct drm_crtc *crtc)
 		dev_priv->display.update_wm(crtc);
 }
 
-void intel_update_sprite_watermarks(struct drm_plane *plane,
-				    struct drm_crtc *crtc,
-				    uint32_t sprite_width,
-				    uint32_t sprite_height,
-				    int pixel_size,
-				    bool enabled, bool scaled)
-{
-	struct drm_i915_private *dev_priv = plane->dev->dev_private;
-
-	if (dev_priv->display.update_sprite_wm)
-		dev_priv->display.update_sprite_wm(plane, crtc,
-						   sprite_width, sprite_height,
-						   pixel_size, enabled, scaled);
-}
-
 /**
  * Lock protecting IPS related data structures
  */
@@ -4414,7 +4312,7 @@ static void gen6_set_rps(struct drm_device *dev, u8 val)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	/* WaGsvDisableTurbo: Workaround to disable turbo on BXT A* */
-	if (IS_BROXTON(dev) && (INTEL_REVID(dev) < BXT_REVID_B0))
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 		return;
 
 	WARN_ON(!mutex_is_locked(&dev_priv->rps.hw_lock));
@@ -4515,7 +4413,7 @@ void gen6_rps_idle(struct drm_i915_private *dev_priv)
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 	if (dev_priv->rps.enabled) {
-		if (IS_VALLEYVIEW(dev))
+		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 			vlv_set_rps_idle(dev_priv);
 		else
 			gen6_set_rps(dev_priv->dev, dev_priv->rps.idle_freq);
@@ -4568,7 +4466,7 @@ void gen6_rps_boost(struct drm_i915_private *dev_priv,
 
 void intel_set_rps(struct drm_device *dev, u8 val)
 {
-	if (IS_VALLEYVIEW(dev))
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev))
 		valleyview_set_rps(dev, val);
 	else
 		gen6_set_rps(dev, val);
@@ -4612,7 +4510,7 @@ static void valleyview_disable_rps(struct drm_device *dev)
 
 static void intel_print_rc6_info(struct drm_device *dev, u32 mode)
 {
-	if (IS_VALLEYVIEW(dev)) {
+	if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		if (mode & (GEN7_RC_CTL_TO_MODE | GEN6_RC_CTL_EI_MODE(1)))
 			mode = GEN6_RC_CTL_RC6_ENABLE;
 		else
@@ -4689,7 +4587,8 @@ static void gen6_init_rps_frequencies(struct drm_device *dev)
 	dev_priv->rps.max_freq		= dev_priv->rps.rp0_freq;
 
 	dev_priv->rps.efficient_freq = dev_priv->rps.rp1_freq;
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev) || IS_SKYLAKE(dev)) {
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev) ||
+	    IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		ret = sandybridge_pcode_read(dev_priv,
 					HSW_PCODE_DYNAMIC_DUTY_CYCLE_CONTROL,
 					&ddcc_status);
@@ -4701,7 +4600,7 @@ static void gen6_init_rps_frequencies(struct drm_device *dev)
 					dev_priv->rps.max_freq);
 	}
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		/* Store the frequency values in 16.66 MHZ units, which is
 		   the natural hardware unit for SKL */
 		dev_priv->rps.rp0_freq *= GEN9_FREQ_SCALER;
@@ -4738,7 +4637,7 @@ static void gen9_enable_rps(struct drm_device *dev)
 	gen6_init_rps_frequencies(dev);
 
 	/* WaGsvDisableTurbo: Workaround to disable turbo on BXT A* */
-	if (IS_BROXTON(dev) && (INTEL_REVID(dev) < BXT_REVID_B0)) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		intel_uncore_forcewake_put(dev_priv, FORCEWAKE_ALL);
 		return;
 	}
@@ -4806,8 +4705,8 @@ static void gen9_enable_rc6(struct drm_device *dev)
 	DRM_INFO("RC6 %s\n", (rc6_mask & GEN6_RC_CTL_RC6_ENABLE) ?
 			"on" : "off");
 	/* WaRsUseTimeoutMode */
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) <= SKL_REVID_D0) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) <= BXT_REVID_A0)) {
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_D0) ||
+	    IS_BXT_REVID(dev, 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 |
@@ -5055,7 +4954,7 @@ static void __gen6_update_ring_freq(struct drm_device *dev)
 	/* convert DDR frequency from units of 266.6MHz to bandwidth */
 	min_ring_freq = mult_frac(min_ring_freq, 8, 3);
 
-	if (IS_SKYLAKE(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 		/* Convert GT frequency to 50 HZ units */
 		min_gpu_freq = dev_priv->rps.min_freq / GEN9_FREQ_SCALER;
 		max_gpu_freq = dev_priv->rps.max_freq / GEN9_FREQ_SCALER;
@@ -5073,7 +4972,7 @@ static void __gen6_update_ring_freq(struct drm_device *dev)
 		int diff = max_gpu_freq - gpu_freq;
 		unsigned int ia_freq = 0, ring_freq = 0;
 
-		if (IS_SKYLAKE(dev)) {
+		if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
 			/*
 			 * ring_freq = 2 * GT. ring_freq is in 100MHz units
 			 * No floor required for ring frequency on SKL.
@@ -5208,7 +5107,17 @@ static int valleyview_rps_rpe_freq(struct drm_i915_private *dev_priv)
 
 static int valleyview_rps_min_freq(struct drm_i915_private *dev_priv)
 {
-	return vlv_punit_read(dev_priv, PUNIT_REG_GPU_LFM) & 0xff;
+	u32 val;
+
+	val = vlv_punit_read(dev_priv, PUNIT_REG_GPU_LFM) & 0xff;
+	/*
+	 * According to the BYT Punit GPU turbo HAS 1.1.6.3 the minimum value
+	 * for the minimum frequency in GPLL mode is 0xc1. Contrary to this on
+	 * a BYT-M B0 the above register contains 0xbf. Moreover when setting
+	 * a frequency Punit will not allow values below 0xc0. Clamp it 0xc0
+	 * to make sure it matches what Punit accepts.
+	 */
+	return max_t(u32, val, 0xc0);
 }
 
 /* Check that the pctx buffer wasn't move under us. */
@@ -6113,7 +6022,17 @@ static void intel_init_emon(struct drm_device *dev)
 
 void intel_init_gt_powersave(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
 	i915.enable_rc6 = sanitize_rc6_option(dev, i915.enable_rc6);
+	/*
+	 * RPM depends on RC6 to save restore the GT HW context, so make RC6 a
+	 * requirement.
+	 */
+	if (!i915.enable_rc6) {
+		DRM_INFO("RC6 disabled, disabling runtime PM support\n");
+		intel_runtime_pm_get(dev_priv);
+	}
 
 	if (IS_CHERRYVIEW(dev))
 		cherryview_init_gt_powersave(dev);
@@ -6123,10 +6042,15 @@ void intel_init_gt_powersave(struct drm_device *dev)
 
 void intel_cleanup_gt_powersave(struct drm_device *dev)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
 	if (IS_CHERRYVIEW(dev))
 		return;
 	else if (IS_VALLEYVIEW(dev))
 		valleyview_cleanup_gt_powersave(dev);
+
+	if (!i915.enable_rc6)
+		intel_runtime_pm_put(dev_priv);
 }
 
 static void gen6_suspend_rps(struct drm_device *dev)
@@ -6201,7 +6125,7 @@ static void intel_gen6_powersave_work(struct work_struct *work)
 	} else if (INTEL_INFO(dev)->gen >= 9) {
 		gen9_enable_rc6(dev);
 		gen9_enable_rps(dev);
-		if (IS_SKYLAKE(dev))
+		if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev))
 			__gen6_update_ring_freq(dev);
 	} else if (IS_BROADWELL(dev)) {
 		gen8_enable_rps(dev);
@@ -7057,7 +6981,6 @@ void intel_init_pm(struct drm_device *dev)
 			dev_priv->display.init_clock_gating =
 				bxt_init_clock_gating;
 		dev_priv->display.update_wm = skl_update_wm;
-		dev_priv->display.update_sprite_wm = skl_update_sprite_wm;
 	} else if (HAS_PCH_SPLIT(dev)) {
 		ilk_setup_wm_latency(dev);
 
@@ -7066,7 +6989,7 @@ void intel_init_pm(struct drm_device *dev)
 		    (!IS_GEN5(dev) && dev_priv->wm.pri_latency[0] &&
 		     dev_priv->wm.spr_latency[0] && dev_priv->wm.cur_latency[0])) {
 			dev_priv->display.update_wm = ilk_update_wm;
-			dev_priv->display.update_sprite_wm = ilk_update_sprite_wm;
+			dev_priv->display.compute_pipe_wm = ilk_compute_pipe_wm;
 		} else {
 			DRM_DEBUG_KMS("Failed to read display plane latency. "
 				      "Disable CxSR\n");
@@ -7331,4 +7254,6 @@ void intel_pm_setup(struct drm_device *dev)
 	INIT_LIST_HEAD(&dev_priv->rps.mmioflips.link);
 
 	dev_priv->pm.suspended = false;
+	atomic_set(&dev_priv->pm.wakeref_count, 0);
+	atomic_set(&dev_priv->pm.atomic_seq, 0);
 }
diff --git a/drivers/gpu/drm/i915/intel_psr.c b/drivers/gpu/drm/i915/intel_psr.c
index 213581c215b3..9ccff3011523 100644
--- a/drivers/gpu/drm/i915/intel_psr.c
+++ b/drivers/gpu/drm/i915/intel_psr.c
@@ -80,7 +80,7 @@ static void intel_psr_write_vsc(struct intel_dp *intel_dp,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *crtc = to_intel_crtc(dig_port->base.base.crtc);
 	enum transcoder cpu_transcoder = crtc->config->cpu_transcoder;
-	u32 ctl_reg = HSW_TVIDEO_DIP_CTL(cpu_transcoder);
+	i915_reg_t ctl_reg = HSW_TVIDEO_DIP_CTL(cpu_transcoder);
 	uint32_t *data = (uint32_t *) vsc_psr;
 	unsigned int i;
 
@@ -151,13 +151,31 @@ static void vlv_psr_enable_sink(struct intel_dp *intel_dp)
 			   DP_PSR_ENABLE | DP_PSR_MAIN_LINK_ACTIVE);
 }
 
+static i915_reg_t psr_aux_ctl_reg(struct drm_i915_private *dev_priv,
+				       enum port port)
+{
+	if (INTEL_INFO(dev_priv)->gen >= 9)
+		return DP_AUX_CH_CTL(port);
+	else
+		return EDP_PSR_AUX_CTL;
+}
+
+static i915_reg_t psr_aux_data_reg(struct drm_i915_private *dev_priv,
+					enum port port, int index)
+{
+	if (INTEL_INFO(dev_priv)->gen >= 9)
+		return DP_AUX_CH_DATA(port, index);
+	else
+		return EDP_PSR_AUX_DATA(index);
+}
+
 static void hsw_psr_enable_sink(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint32_t aux_clock_divider;
-	uint32_t aux_data_reg, aux_ctl_reg;
+	i915_reg_t aux_ctl_reg;
 	int precharge = 0x3;
 	static const uint8_t aux_msg[] = {
 		[0] = DP_AUX_NATIVE_WRITE << 4,
@@ -166,29 +184,24 @@ static void hsw_psr_enable_sink(struct intel_dp *intel_dp)
 		[3] = 1 - 1,
 		[4] = DP_SET_POWER_D0,
 	};
+	enum port port = dig_port->port;
 	int i;
 
 	BUILD_BUG_ON(sizeof(aux_msg) > 20);
 
 	aux_clock_divider = intel_dp->get_aux_clock_divider(intel_dp, 0);
 
-	drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG,
-			   DP_PSR_ENABLE & ~DP_PSR_MAIN_LINK_ACTIVE);
-
 	/* Enable AUX frame sync at sink */
 	if (dev_priv->psr.aux_frame_sync)
 		drm_dp_dpcd_writeb(&intel_dp->aux,
 				DP_SINK_DEVICE_AUX_FRAME_SYNC_CONF,
 				DP_AUX_FRAME_SYNC_ENABLE);
 
-	aux_data_reg = (INTEL_INFO(dev)->gen >= 9) ?
-				DPA_AUX_CH_DATA1 : EDP_PSR_AUX_DATA1(dev);
-	aux_ctl_reg = (INTEL_INFO(dev)->gen >= 9) ?
-				DPA_AUX_CH_CTL : EDP_PSR_AUX_CTL(dev);
+	aux_ctl_reg = psr_aux_ctl_reg(dev_priv, port);
 
 	/* Setup AUX registers */
 	for (i = 0; i < sizeof(aux_msg); i += 4)
-		I915_WRITE(aux_data_reg + i,
+		I915_WRITE(psr_aux_data_reg(dev_priv, port, i >> 2),
 			   intel_dp_pack_aux(&aux_msg[i], sizeof(aux_msg) - i));
 
 	if (INTEL_INFO(dev)->gen >= 9) {
@@ -254,30 +267,20 @@ static void hsw_psr_enable_source(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	uint32_t max_sleep_time = 0x1f;
-	/* Lately it was identified that depending on panel idle frame count
-	 * calculated at HW can be off by 1. So let's use what came
-	 * from VBT + 1.
-	 * There are also other cases where panel demands at least 4
-	 * but VBT is not being set. To cover these 2 cases lets use
-	 * at least 5 when VBT isn't set to be on the safest side.
+	/*
+	 * Let's respect VBT in case VBT asks a higher idle_frame value.
+	 * Let's use 6 as the minimum to cover all known cases including
+	 * the off-by-one issue that HW has in some cases. Also there are
+	 * cases where sink should be able to train
+	 * with the 5 or 6 idle patterns.
 	 */
-	uint32_t idle_frames = dev_priv->vbt.psr.idle_frames ?
-			       dev_priv->vbt.psr.idle_frames + 1 : 5;
+	uint32_t idle_frames = max(6, dev_priv->vbt.psr.idle_frames);
 	uint32_t val = 0x0;
-	const uint32_t link_entry_time = EDP_PSR_MIN_LINK_ENTRY_TIME_8_LINES;
-
-	if (intel_dp->psr_dpcd[1] & DP_PSR_NO_TRAIN_ON_EXIT) {
-		/* It doesn't mean we shouldn't send TPS patters, so let's
-		   send the minimal TP1 possible and skip TP2. */
-		val |= EDP_PSR_TP1_TIME_100us;
-		val |= EDP_PSR_TP2_TP3_TIME_0us;
-		val |= EDP_PSR_SKIP_AUX_EXIT;
-		/* Sink should be able to train with the 5 or 6 idle patterns */
-		idle_frames += 4;
-	}
 
-	I915_WRITE(EDP_PSR_CTL(dev), val |
-		   (IS_BROADWELL(dev) ? 0 : link_entry_time) |
+	if (IS_HASWELL(dev))
+		val |= EDP_PSR_MIN_LINK_ENTRY_TIME_8_LINES;
+
+	I915_WRITE(EDP_PSR_CTL, val |
 		   max_sleep_time << EDP_PSR_MAX_SLEEP_TIME_SHIFT |
 		   idle_frames << EDP_PSR_IDLE_FRAME_SHIFT |
 		   EDP_PSR_ENABLE);
@@ -324,8 +327,8 @@ static bool intel_psr_match_conditions(struct intel_dp *intel_dp)
 		return false;
 	}
 
-	if (!IS_VALLEYVIEW(dev) && ((dev_priv->vbt.psr.full_link) ||
-				    (dig_port->port != PORT_A))) {
+	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev) &&
+	    ((dev_priv->vbt.psr.full_link) || (dig_port->port != PORT_A))) {
 		DRM_DEBUG_KMS("PSR condition failed: Link Standby requested/needed but not supported on this platform\n");
 		return false;
 	}
@@ -340,7 +343,7 @@ static void intel_psr_activate(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
+	WARN_ON(I915_READ(EDP_PSR_CTL) & EDP_PSR_ENABLE);
 	WARN_ON(dev_priv->psr.active);
 	lockdep_assert_held(&dev_priv->psr.lock);
 
@@ -403,9 +406,14 @@ void intel_psr_enable(struct intel_dp *intel_dp)
 				skl_psr_setup_su_vsc(intel_dp);
 		}
 
-		/* Avoid continuous PSR exit by masking memup and hpd */
-		I915_WRITE(EDP_PSR_DEBUG_CTL(dev), EDP_PSR_DEBUG_MASK_MEMUP |
-			   EDP_PSR_DEBUG_MASK_HPD);
+		/*
+		 * Per Spec: Avoid continuous PSR exit by masking MEMUP and HPD.
+		 * Also mask LPSP to avoid dependency on other drivers that
+		 * might block runtime_pm besides preventing other hw tracking
+		 * issues now we can rely on frontbuffer tracking.
+		 */
+		I915_WRITE(EDP_PSR_DEBUG_CTL, EDP_PSR_DEBUG_MASK_MEMUP |
+			   EDP_PSR_DEBUG_MASK_HPD | EDP_PSR_DEBUG_MASK_LPSP);
 
 		/* Enable PSR on the panel */
 		hsw_psr_enable_sink(intel_dp);
@@ -427,6 +435,19 @@ void intel_psr_enable(struct intel_dp *intel_dp)
 		vlv_psr_enable_source(intel_dp);
 	}
 
+	/*
+	 * FIXME: Activation should happen immediately since this function
+	 * is just called after pipe is fully trained and enabled.
+	 * However on every platform we face issues when first activation
+	 * follows a modeset so quickly.
+	 *     - On VLV/CHV we get bank screen on first activation
+	 *     - On HSW/BDW we get a recoverable frozen screen until next
+	 *       exit-activate sequence.
+	 */
+	if (INTEL_INFO(dev)->gen < 9)
+		schedule_delayed_work(&dev_priv->psr.work,
+				      msecs_to_jiffies(intel_dp->panel_power_cycle_delay * 5));
+
 	dev_priv->psr.enabled = intel_dp;
 unlock:
 	mutex_unlock(&dev_priv->psr.lock);
@@ -466,17 +487,17 @@ static void hsw_psr_disable(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	if (dev_priv->psr.active) {
-		I915_WRITE(EDP_PSR_CTL(dev),
-			   I915_READ(EDP_PSR_CTL(dev)) & ~EDP_PSR_ENABLE);
+		I915_WRITE(EDP_PSR_CTL,
+			   I915_READ(EDP_PSR_CTL) & ~EDP_PSR_ENABLE);
 
 		/* Wait till PSR is idle */
-		if (_wait_for((I915_READ(EDP_PSR_STATUS_CTL(dev)) &
+		if (_wait_for((I915_READ(EDP_PSR_STATUS_CTL) &
 			       EDP_PSR_STATUS_STATE_MASK) == 0, 2000, 10))
 			DRM_ERROR("Timed out waiting for PSR Idle State\n");
 
 		dev_priv->psr.active = false;
 	} else {
-		WARN_ON(I915_READ(EDP_PSR_CTL(dev)) & EDP_PSR_ENABLE);
+		WARN_ON(I915_READ(EDP_PSR_CTL) & EDP_PSR_ENABLE);
 	}
 }
 
@@ -498,11 +519,15 @@ void intel_psr_disable(struct intel_dp *intel_dp)
 		return;
 	}
 
+	/* Disable PSR on Source */
 	if (HAS_DDI(dev))
 		hsw_psr_disable(intel_dp);
 	else
 		vlv_psr_disable(intel_dp);
 
+	/* Disable PSR on Sink */
+	drm_dp_dpcd_writeb(&intel_dp->aux, DP_PSR_EN_CFG, 0);
+
 	dev_priv->psr.enabled = NULL;
 	mutex_unlock(&dev_priv->psr.lock);
 
@@ -523,7 +548,7 @@ static void intel_psr_work(struct work_struct *work)
 	 * and be ready for re-enable.
 	 */
 	if (HAS_DDI(dev_priv->dev)) {
-		if (wait_for((I915_READ(EDP_PSR_STATUS_CTL(dev_priv->dev)) &
+		if (wait_for((I915_READ(EDP_PSR_STATUS_CTL) &
 			      EDP_PSR_STATUS_STATE_MASK) == 0, 50)) {
 			DRM_ERROR("Timed out waiting for PSR Idle for re-enable\n");
 			return;
@@ -566,11 +591,11 @@ static void intel_psr_exit(struct drm_device *dev)
 		return;
 
 	if (HAS_DDI(dev)) {
-		val = I915_READ(EDP_PSR_CTL(dev));
+		val = I915_READ(EDP_PSR_CTL);
 
 		WARN_ON(!(val & EDP_PSR_ENABLE));
 
-		I915_WRITE(EDP_PSR_CTL(dev), val & ~EDP_PSR_ENABLE);
+		I915_WRITE(EDP_PSR_CTL, val & ~EDP_PSR_ENABLE);
 	} else {
 		val = I915_READ(VLV_PSRCTL(pipe));
 
@@ -620,7 +645,7 @@ void intel_psr_single_frame_update(struct drm_device *dev,
 	 * Single frame update is already supported on BDW+ but it requires
 	 * many W/A and it isn't really needed.
 	 */
-	if (!IS_VALLEYVIEW(dev))
+	if (!IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev))
 		return;
 
 	mutex_lock(&dev_priv->psr.lock);
@@ -700,7 +725,6 @@ void intel_psr_flush(struct drm_device *dev,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct drm_crtc *crtc;
 	enum pipe pipe;
-	int delay_ms = HAS_DDI(dev) ? 100 : 500;
 
 	mutex_lock(&dev_priv->psr.lock);
 	if (!dev_priv->psr.enabled) {
@@ -714,29 +738,14 @@ void intel_psr_flush(struct drm_device *dev,
 	frontbuffer_bits &= INTEL_FRONTBUFFER_ALL_MASK(pipe);
 	dev_priv->psr.busy_frontbuffer_bits &= ~frontbuffer_bits;
 
-	if (HAS_DDI(dev)) {
-		/*
-		 * By definition every flush should mean invalidate + flush,
-		 * however on core platforms let's minimize the
-		 * disable/re-enable so we can avoid the invalidate when flip
-		 * originated the flush.
-		 */
-		if (frontbuffer_bits && origin != ORIGIN_FLIP)
-			intel_psr_exit(dev);
-	} else {
-		/*
-		 * On Valleyview and Cherryview we don't use hardware tracking
-		 * so any plane updates or cursor moves don't result in a PSR
-		 * invalidating. Which means we need to manually fake this in
-		 * software for all flushes.
-		 */
-		if (frontbuffer_bits)
-			intel_psr_exit(dev);
-	}
+	/* By definition flush = invalidate + flush */
+	if (frontbuffer_bits)
+		intel_psr_exit(dev);
 
 	if (!dev_priv->psr.active && !dev_priv->psr.busy_frontbuffer_bits)
-		schedule_delayed_work(&dev_priv->psr.work,
-				      msecs_to_jiffies(delay_ms));
+		if (!work_busy(&dev_priv->psr.work.work))
+			schedule_delayed_work(&dev_priv->psr.work,
+					      msecs_to_jiffies(100));
 	mutex_unlock(&dev_priv->psr.lock);
 }
 
@@ -751,6 +760,9 @@ void intel_psr_init(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	dev_priv->psr_mmio_base = IS_HASWELL(dev_priv) ?
+		HSW_EDP_PSR_BASE : BDW_EDP_PSR_BASE;
+
 	INIT_DELAYED_WORK(&dev_priv->psr.work, intel_psr_work);
 	mutex_init(&dev_priv->psr.lock);
 }
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 9461a238f5d5..40c6aff57256 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -27,29 +27,13 @@
  *
  */
 
+#include <linux/log2.h>
 #include <drm/drmP.h>
 #include "i915_drv.h"
 #include <drm/i915_drm.h>
 #include "i915_trace.h"
 #include "intel_drv.h"
 
-bool
-intel_ring_initialized(struct intel_engine_cs *ring)
-{
-	struct drm_device *dev = ring->dev;
-
-	if (!dev)
-		return false;
-
-	if (i915.enable_execlists) {
-		struct intel_context *dctx = ring->default_context;
-		struct intel_ringbuffer *ringbuf = dctx->engine[ring->id].ringbuf;
-
-		return ringbuf->obj;
-	} else
-		return ring->buffer && ring->buffer->obj;
-}
-
 int __intel_ring_space(int head, int tail, int size)
 {
 	int space = head - tail;
@@ -347,6 +331,7 @@ gen7_render_ring_flush(struct drm_i915_gem_request *req,
 	if (flush_domains) {
 		flags |= PIPE_CONTROL_RENDER_TARGET_CACHE_FLUSH;
 		flags |= PIPE_CONTROL_DEPTH_CACHE_FLUSH;
+		flags |= PIPE_CONTROL_DC_FLUSH_ENABLE;
 		flags |= PIPE_CONTROL_FLUSH_ENABLE;
 	}
 	if (invalidate_domains) {
@@ -419,6 +404,7 @@ gen8_render_ring_flush(struct drm_i915_gem_request *req,
 	if (flush_domains) {
 		flags |= PIPE_CONTROL_RENDER_TARGET_CACHE_FLUSH;
 		flags |= PIPE_CONTROL_DEPTH_CACHE_FLUSH;
+		flags |= PIPE_CONTROL_DC_FLUSH_ENABLE;
 		flags |= PIPE_CONTROL_FLUSH_ENABLE;
 	}
 	if (invalidate_domains) {
@@ -481,7 +467,7 @@ static void intel_ring_setup_status_page(struct intel_engine_cs *ring)
 {
 	struct drm_device *dev = ring->dev;
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
-	u32 mmio = 0;
+	i915_reg_t mmio;
 
 	/* The ring status page addresses are no longer next to the rest of
 	 * the ring registers as of gen7.
@@ -524,7 +510,7 @@ static void intel_ring_setup_status_page(struct intel_engine_cs *ring)
 	 * invalidating the TLB?
 	 */
 	if (INTEL_INFO(dev)->gen >= 6 && INTEL_INFO(dev)->gen < 8) {
-		u32 reg = RING_INSTPM(ring->mmio_base);
+		i915_reg_t reg = RING_INSTPM(ring->mmio_base);
 
 		/* ring should be idle before issuing a sync flush*/
 		WARN_ON((I915_READ_MODE(ring) & MODE_IDLE) == 0);
@@ -733,7 +719,7 @@ static int intel_ring_workarounds_emit(struct drm_i915_gem_request *req)
 
 	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(w->count));
 	for (i = 0; i < w->count; i++) {
-		intel_ring_emit(ring, w->reg[i].addr);
+		intel_ring_emit_reg(ring, w->reg[i].addr);
 		intel_ring_emit(ring, w->reg[i].value);
 	}
 	intel_ring_emit(ring, MI_NOOP);
@@ -766,7 +752,8 @@ static int intel_rcs_ctx_init(struct drm_i915_gem_request *req)
 }
 
 static int wa_add(struct drm_i915_private *dev_priv,
-		  const u32 addr, const u32 mask, const u32 val)
+		  i915_reg_t addr,
+		  const u32 mask, const u32 val)
 {
 	const u32 idx = dev_priv->workarounds.count;
 
@@ -924,17 +911,15 @@ static int gen9_init_workarounds(struct intel_engine_cs *ring)
 	WA_SET_BIT_MASKED(HALF_SLICE_CHICKEN3,
 			  GEN9_DISABLE_OCL_OOB_SUPPRESS_LOGIC);
 
-	if ((IS_SKYLAKE(dev) && (INTEL_REVID(dev) == SKL_REVID_A0 ||
-	    INTEL_REVID(dev) == SKL_REVID_B0)) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0)) {
-		/* WaDisableDgMirrorFixInHalfSliceChicken5:skl,bxt */
+	/* WaDisableDgMirrorFixInHalfSliceChicken5:skl,bxt */
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 		WA_CLR_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN5,
 				  GEN9_DG_MIRROR_FIX_ENABLE);
-	}
 
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) <= SKL_REVID_B0) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0)) {
-		/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:skl,bxt */
+	/* WaSetDisablePixMaskCammingAndRhwoInCommonSliceChicken:skl,bxt */
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_B0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		WA_SET_BIT_MASKED(GEN7_COMMON_SLICE_CHICKEN1,
 				  GEN9_RHWO_OPTIMIZATION_DISABLE);
 		/*
@@ -944,12 +929,10 @@ static int gen9_init_workarounds(struct intel_engine_cs *ring)
 		 */
 	}
 
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) >= SKL_REVID_C0) ||
-	    IS_BROXTON(dev)) {
-		/* WaEnableYV12BugFixInHalfSliceChicken7:skl,bxt */
+	/* WaEnableYV12BugFixInHalfSliceChicken7:skl,bxt */
+	if (IS_SKL_REVID(dev, SKL_REVID_C0, REVID_FOREVER) || IS_BROXTON(dev))
 		WA_SET_BIT_MASKED(GEN9_HALF_SLICE_CHICKEN7,
 				  GEN9_ENABLE_YV12_BUGFIX);
-	}
 
 	/* Wa4x4STCOptimizationDisable:skl,bxt */
 	/* WaDisablePartialResolveInVc:skl,bxt */
@@ -961,24 +944,22 @@ static int gen9_init_workarounds(struct intel_engine_cs *ring)
 			  GEN9_CCS_TLB_PREFETCH_ENABLE);
 
 	/* WaDisableMaskBasedCammingInRCC:skl,bxt */
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) == SKL_REVID_C0) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) < BXT_REVID_B0))
+	if (IS_SKL_REVID(dev, SKL_REVID_C0, SKL_REVID_C0) ||
+	    IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 		WA_SET_BIT_MASKED(SLICE_ECO_CHICKEN0,
 				  PIXEL_MASK_CAMMING_DISABLE);
 
 	/* WaForceContextSaveRestoreNonCoherent:skl,bxt */
 	tmp = HDC_FORCE_CONTEXT_SAVE_RESTORE_NON_COHERENT;
-	if ((IS_SKYLAKE(dev) && INTEL_REVID(dev) == SKL_REVID_F0) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) >= BXT_REVID_B0))
+	if (IS_SKL_REVID(dev, SKL_REVID_F0, SKL_REVID_F0) ||
+	    IS_BXT_REVID(dev, BXT_REVID_B0, REVID_FOREVER))
 		tmp |= HDC_FORCE_CSR_NON_COHERENT_OVR_DISABLE;
 	WA_SET_BIT_MASKED(HDC_CHICKEN0, tmp);
 
 	/* WaDisableSamplerPowerBypassForSOPingPong:skl,bxt */
-	if (IS_SKYLAKE(dev) ||
-	    (IS_BROXTON(dev) && INTEL_REVID(dev) <= BXT_REVID_B0)) {
+	if (IS_SKYLAKE(dev) || IS_BXT_REVID(dev, 0, BXT_REVID_B0))
 		WA_SET_BIT_MASKED(HALF_SLICE_CHICKEN3,
 				  GEN8_SAMPLER_POWER_BYPASS_DIS);
-	}
 
 	/* WaDisableSTUnitPowerOptimization:skl,bxt */
 	WA_SET_BIT_MASKED(HALF_SLICE_CHICKEN2, GEN8_ST_PO_DISABLE);
@@ -1000,7 +981,7 @@ static int skl_tune_iz_hashing(struct intel_engine_cs *ring)
 		 * Only consider slices where one, and only one, subslice has 7
 		 * EUs
 		 */
-		if (hweight8(dev_priv->info.subslice_7eu[i]) != 1)
+		if (!is_power_of_2(dev_priv->info.subslice_7eu[i]))
 			continue;
 
 		/*
@@ -1038,11 +1019,7 @@ static int skl_init_workarounds(struct intel_engine_cs *ring)
 	if (ret)
 		return ret;
 
-	if (INTEL_REVID(dev) <= SKL_REVID_D0) {
-		/* WaDisableHDCInvalidation:skl */
-		I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) |
-			   BDW_DISABLE_HDC_INVALIDATION);
-
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_D0)) {
 		/* WaDisableChickenBitTSGBarrierAckForFFSliceCS:skl */
 		I915_WRITE(FF_SLICE_CS_CHICKEN2,
 			   _MASKED_BIT_ENABLE(GEN9_TSG_BARRIER_ACK_DISABLE));
@@ -1051,23 +1028,23 @@ static int skl_init_workarounds(struct intel_engine_cs *ring)
 	/* 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 (INTEL_REVID(dev) <= SKL_REVID_E0)
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_E0))
 		/* WaDisableLSQCROPERFforOCL:skl */
 		I915_WRITE(GEN8_L3SQCREG4, I915_READ(GEN8_L3SQCREG4) |
 			   GEN8_LQSC_RO_PERF_DIS);
 
 	/* WaEnableGapsTsvCreditFix:skl */
-	if (IS_SKYLAKE(dev) && (INTEL_REVID(dev) >= SKL_REVID_C0)) {
+	if (IS_SKL_REVID(dev, SKL_REVID_C0, REVID_FOREVER)) {
 		I915_WRITE(GEN8_GARBCNTL, (I915_READ(GEN8_GARBCNTL) |
 					   GEN9_GAPS_TSV_CREDIT_DISABLE));
 	}
 
 	/* WaDisablePowerCompilerClockGating:skl */
-	if (INTEL_REVID(dev) == SKL_REVID_B0)
+	if (IS_SKL_REVID(dev, SKL_REVID_B0, SKL_REVID_B0))
 		WA_SET_BIT_MASKED(HIZ_CHICKEN,
 				  BDW_HIZ_POWER_COMPILER_CLOCK_GATING_DISABLE);
 
-	if (INTEL_REVID(dev) <= SKL_REVID_D0) {
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_F0)) {
 		/*
 		 *Use Force Non-Coherent whenever executing a 3D context. This
 		 * is a workaround for a possible hang in the unlikely event
@@ -1076,21 +1053,23 @@ static int skl_init_workarounds(struct intel_engine_cs *ring)
 		/* WaForceEnableNonCoherent:skl */
 		WA_SET_BIT_MASKED(HDC_CHICKEN0,
 				  HDC_FORCE_NON_COHERENT);
+
+		/* WaDisableHDCInvalidation:skl */
+		I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) |
+			   BDW_DISABLE_HDC_INVALIDATION);
 	}
 
-	if (INTEL_REVID(dev) == SKL_REVID_C0 ||
-	    INTEL_REVID(dev) == SKL_REVID_D0)
-		/* WaBarrierPerformanceFixDisable:skl */
+	/* WaBarrierPerformanceFixDisable:skl */
+	if (IS_SKL_REVID(dev, SKL_REVID_C0, SKL_REVID_D0))
 		WA_SET_BIT_MASKED(HDC_CHICKEN0,
 				  HDC_FENCE_DEST_SLM_DISABLE |
 				  HDC_BARRIER_PERFORMANCE_DISABLE);
 
 	/* WaDisableSbeCacheDispatchPortSharing:skl */
-	if (INTEL_REVID(dev) <= SKL_REVID_F0) {
+	if (IS_SKL_REVID(dev, 0, SKL_REVID_F0))
 		WA_SET_BIT_MASKED(
 			GEN7_HALF_SLICE_CHICKEN1,
 			GEN7_SBE_SS_CACHE_DISPATCH_PORT_SHARING_DISABLE);
-	}
 
 	return skl_tune_iz_hashing(ring);
 }
@@ -1107,11 +1086,11 @@ static int bxt_init_workarounds(struct intel_engine_cs *ring)
 
 	/* WaStoreMultiplePTEenable:bxt */
 	/* This is a requirement according to Hardware specification */
-	if (INTEL_REVID(dev) == BXT_REVID_A0)
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1))
 		I915_WRITE(TILECTL, I915_READ(TILECTL) | TILECTL_TLBPF);
 
 	/* WaSetClckGatingDisableMedia:bxt */
-	if (INTEL_REVID(dev) == BXT_REVID_A0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_A1)) {
 		I915_WRITE(GEN7_MISCCPCTL, (I915_READ(GEN7_MISCCPCTL) &
 					    ~GEN8_DOP_CLOCK_GATE_MEDIA_ENABLE));
 	}
@@ -1121,7 +1100,7 @@ static int bxt_init_workarounds(struct intel_engine_cs *ring)
 			  STALL_DOP_GATING_DISABLE);
 
 	/* WaDisableSbeCacheDispatchPortSharing:bxt */
-	if (INTEL_REVID(dev) <= BXT_REVID_B0) {
+	if (IS_BXT_REVID(dev, 0, BXT_REVID_B0)) {
 		WA_SET_BIT_MASKED(
 			GEN7_HALF_SLICE_CHICKEN1,
 			GEN7_SBE_SS_CACHE_DISPATCH_PORT_SHARING_DISABLE);
@@ -1319,11 +1298,13 @@ static int gen6_signal(struct drm_i915_gem_request *signaller_req,
 		return ret;
 
 	for_each_ring(useless, dev_priv, i) {
-		u32 mbox_reg = signaller->semaphore.mbox.signal[i];
-		if (mbox_reg != GEN6_NOSYNC) {
+		i915_reg_t mbox_reg = signaller->semaphore.mbox.signal[i];
+
+		if (i915_mmio_reg_valid(mbox_reg)) {
 			u32 seqno = i915_gem_request_get_seqno(signaller_req);
+
 			intel_ring_emit(signaller, MI_LOAD_REGISTER_IMM(1));
-			intel_ring_emit(signaller, mbox_reg);
+			intel_ring_emit_reg(signaller, mbox_reg);
 			intel_ring_emit(signaller, seqno);
 		}
 	}
@@ -2004,11 +1985,35 @@ static int init_phys_status_page(struct intel_engine_cs *ring)
 
 void intel_unpin_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
 {
-	iounmap(ringbuf->virtual_start);
+	if (HAS_LLC(ringbuf->obj->base.dev) && !ringbuf->obj->stolen)
+		vunmap(ringbuf->virtual_start);
+	else
+		iounmap(ringbuf->virtual_start);
 	ringbuf->virtual_start = NULL;
 	i915_gem_object_ggtt_unpin(ringbuf->obj);
 }
 
+static u32 *vmap_obj(struct drm_i915_gem_object *obj)
+{
+	struct sg_page_iter sg_iter;
+	struct page **pages;
+	void *addr;
+	int i;
+
+	pages = drm_malloc_ab(obj->base.size >> PAGE_SHIFT, sizeof(*pages));
+	if (pages == NULL)
+		return NULL;
+
+	i = 0;
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0)
+		pages[i++] = sg_page_iter_page(&sg_iter);
+
+	addr = vmap(pages, i, 0, PAGE_KERNEL);
+	drm_free_large(pages);
+
+	return addr;
+}
+
 int intel_pin_and_map_ringbuffer_obj(struct drm_device *dev,
 				     struct intel_ringbuffer *ringbuf)
 {
@@ -2016,21 +2021,39 @@ int intel_pin_and_map_ringbuffer_obj(struct drm_device *dev,
 	struct drm_i915_gem_object *obj = ringbuf->obj;
 	int ret;
 
-	ret = i915_gem_obj_ggtt_pin(obj, PAGE_SIZE, PIN_MAPPABLE);
-	if (ret)
-		return ret;
+	if (HAS_LLC(dev_priv) && !obj->stolen) {
+		ret = i915_gem_obj_ggtt_pin(obj, PAGE_SIZE, 0);
+		if (ret)
+			return ret;
 
-	ret = i915_gem_object_set_to_gtt_domain(obj, true);
-	if (ret) {
-		i915_gem_object_ggtt_unpin(obj);
-		return ret;
-	}
+		ret = i915_gem_object_set_to_cpu_domain(obj, true);
+		if (ret) {
+			i915_gem_object_ggtt_unpin(obj);
+			return ret;
+		}
 
-	ringbuf->virtual_start = ioremap_wc(dev_priv->gtt.mappable_base +
-			i915_gem_obj_ggtt_offset(obj), ringbuf->size);
-	if (ringbuf->virtual_start == NULL) {
-		i915_gem_object_ggtt_unpin(obj);
-		return -EINVAL;
+		ringbuf->virtual_start = vmap_obj(obj);
+		if (ringbuf->virtual_start == NULL) {
+			i915_gem_object_ggtt_unpin(obj);
+			return -ENOMEM;
+		}
+	} else {
+		ret = i915_gem_obj_ggtt_pin(obj, PAGE_SIZE, PIN_MAPPABLE);
+		if (ret)
+			return ret;
+
+		ret = i915_gem_object_set_to_gtt_domain(obj, true);
+		if (ret) {
+			i915_gem_object_ggtt_unpin(obj);
+			return ret;
+		}
+
+		ringbuf->virtual_start = ioremap_wc(dev_priv->gtt.mappable_base +
+						    i915_gem_obj_ggtt_offset(obj), ringbuf->size);
+		if (ringbuf->virtual_start == NULL) {
+			i915_gem_object_ggtt_unpin(obj);
+			return -EINVAL;
+		}
 	}
 
 	return 0;
@@ -2070,10 +2093,14 @@ intel_engine_create_ringbuffer(struct intel_engine_cs *engine, int size)
 	int ret;
 
 	ring = kzalloc(sizeof(*ring), GFP_KERNEL);
-	if (ring == NULL)
+	if (ring == NULL) {
+		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer %s\n",
+				 engine->name);
 		return ERR_PTR(-ENOMEM);
+	}
 
 	ring->ring = engine;
+	list_add(&ring->link, &engine->buffers);
 
 	ring->size = size;
 	/* Workaround an erratum on the i830 which causes a hang if
@@ -2089,8 +2116,9 @@ intel_engine_create_ringbuffer(struct intel_engine_cs *engine, int size)
 
 	ret = intel_alloc_ringbuffer_obj(engine->dev, ring);
 	if (ret) {
-		DRM_ERROR("Failed to allocate ringbuffer %s: %d\n",
-			  engine->name, ret);
+		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer %s: %d\n",
+				 engine->name, ret);
+		list_del(&ring->link);
 		kfree(ring);
 		return ERR_PTR(ret);
 	}
@@ -2102,6 +2130,7 @@ void
 intel_ringbuffer_free(struct intel_ringbuffer *ring)
 {
 	intel_destroy_ringbuffer_obj(ring);
+	list_del(&ring->link);
 	kfree(ring);
 }
 
@@ -2117,14 +2146,17 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 	INIT_LIST_HEAD(&ring->active_list);
 	INIT_LIST_HEAD(&ring->request_list);
 	INIT_LIST_HEAD(&ring->execlist_queue);
+	INIT_LIST_HEAD(&ring->buffers);
 	i915_gem_batch_pool_init(dev, &ring->batch_pool);
 	memset(ring->semaphore.sync_seqno, 0, sizeof(ring->semaphore.sync_seqno));
 
 	init_waitqueue_head(&ring->irq_queue);
 
 	ringbuf = intel_engine_create_ringbuffer(ring, 32 * PAGE_SIZE);
-	if (IS_ERR(ringbuf))
-		return PTR_ERR(ringbuf);
+	if (IS_ERR(ringbuf)) {
+		ret = PTR_ERR(ringbuf);
+		goto error;
+	}
 	ring->buffer = ringbuf;
 
 	if (I915_NEED_GFX_HWS(dev)) {
@@ -2153,8 +2185,7 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 	return 0;
 
 error:
-	intel_ringbuffer_free(ringbuf);
-	ring->buffer = NULL;
+	intel_cleanup_ring_buffer(ring);
 	return ret;
 }
 
@@ -2167,12 +2198,14 @@ void intel_cleanup_ring_buffer(struct intel_engine_cs *ring)
 
 	dev_priv = to_i915(ring->dev);
 
-	intel_stop_ring_buffer(ring);
-	WARN_ON(!IS_GEN2(ring->dev) && (I915_READ_MODE(ring) & MODE_IDLE) == 0);
+	if (ring->buffer) {
+		intel_stop_ring_buffer(ring);
+		WARN_ON(!IS_GEN2(ring->dev) && (I915_READ_MODE(ring) & MODE_IDLE) == 0);
 
-	intel_unpin_ringbuffer_obj(ring->buffer);
-	intel_ringbuffer_free(ring->buffer);
-	ring->buffer = NULL;
+		intel_unpin_ringbuffer_obj(ring->buffer);
+		intel_ringbuffer_free(ring->buffer);
+		ring->buffer = NULL;
+	}
 
 	if (ring->cleanup)
 		ring->cleanup(ring);
@@ -2181,6 +2214,7 @@ void intel_cleanup_ring_buffer(struct intel_engine_cs *ring)
 
 	i915_cmd_parser_fini_ring(ring);
 	i915_gem_batch_pool_fini(&ring->batch_pool);
+	ring->dev = NULL;
 }
 
 static int ring_wait_for_space(struct intel_engine_cs *ring, int n)
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 49fa41dc0eb6..49574ffe54bc 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -100,6 +100,7 @@ struct intel_ringbuffer {
 	void __iomem *virtual_start;
 
 	struct intel_engine_cs *ring;
+	struct list_head link;
 
 	u32 head;
 	u32 tail;
@@ -157,6 +158,7 @@ struct  intel_engine_cs {
 	u32		mmio_base;
 	struct		drm_device *dev;
 	struct intel_ringbuffer *buffer;
+	struct list_head buffers;
 
 	/*
 	 * A pool of objects to use as shadow copies of client batch buffers
@@ -247,7 +249,7 @@ struct  intel_engine_cs {
 				/* our mbox written by others */
 				u32		wait[I915_NUM_RINGS];
 				/* mboxes this ring signals to */
-				u32		signal[I915_NUM_RINGS];
+				i915_reg_t	signal[I915_NUM_RINGS];
 			} mbox;
 			u64		signal_ggtt[I915_NUM_RINGS];
 		};
@@ -348,7 +350,11 @@ struct  intel_engine_cs {
 	u32 (*get_cmd_length_mask)(u32 cmd_header);
 };
 
-bool intel_ring_initialized(struct intel_engine_cs *ring);
+static inline bool
+intel_ring_initialized(struct intel_engine_cs *ring)
+{
+	return ring->dev != NULL;
+}
 
 static inline unsigned
 intel_ring_flag(struct intel_engine_cs *ring)
@@ -441,6 +447,11 @@ static inline void intel_ring_emit(struct intel_engine_cs *ring,
 	iowrite32(data, ringbuf->virtual_start + ringbuf->tail);
 	ringbuf->tail += 4;
 }
+static inline void intel_ring_emit_reg(struct intel_engine_cs *ring,
+				       i915_reg_t reg)
+{
+	intel_ring_emit(ring, i915_mmio_reg_offset(reg));
+}
 static inline void intel_ring_advance(struct intel_engine_cs *ring)
 {
 	struct intel_ringbuffer *ringbuf = ring->buffer;
diff --git a/drivers/gpu/drm/i915/intel_runtime_pm.c b/drivers/gpu/drm/i915/intel_runtime_pm.c
index 7e23d65c9b24..4f43d9b32e66 100644
--- a/drivers/gpu/drm/i915/intel_runtime_pm.c
+++ b/drivers/gpu/drm/i915/intel_runtime_pm.c
@@ -49,25 +49,88 @@
  * present for a given platform.
  */
 
-#define GEN9_ENABLE_DC5(dev) 0
-#define SKL_ENABLE_DC6(dev) IS_SKYLAKE(dev)
-
 #define for_each_power_well(i, power_well, domain_mask, power_domains)	\
 	for (i = 0;							\
 	     i < (power_domains)->power_well_count &&			\
 		 ((power_well) = &(power_domains)->power_wells[i]);	\
 	     i++)							\
-		if ((power_well)->domains & (domain_mask))
+		for_each_if ((power_well)->domains & (domain_mask))
 
 #define for_each_power_well_rev(i, power_well, domain_mask, power_domains) \
 	for (i = (power_domains)->power_well_count - 1;			 \
 	     i >= 0 && ((power_well) = &(power_domains)->power_wells[i]);\
 	     i--)							 \
-		if ((power_well)->domains & (domain_mask))
+		for_each_if ((power_well)->domains & (domain_mask))
 
 bool intel_display_power_well_is_enabled(struct drm_i915_private *dev_priv,
 				    int power_well_id);
 
+const char *
+intel_display_power_domain_str(enum intel_display_power_domain domain)
+{
+	switch (domain) {
+	case POWER_DOMAIN_PIPE_A:
+		return "PIPE_A";
+	case POWER_DOMAIN_PIPE_B:
+		return "PIPE_B";
+	case POWER_DOMAIN_PIPE_C:
+		return "PIPE_C";
+	case POWER_DOMAIN_PIPE_A_PANEL_FITTER:
+		return "PIPE_A_PANEL_FITTER";
+	case POWER_DOMAIN_PIPE_B_PANEL_FITTER:
+		return "PIPE_B_PANEL_FITTER";
+	case POWER_DOMAIN_PIPE_C_PANEL_FITTER:
+		return "PIPE_C_PANEL_FITTER";
+	case POWER_DOMAIN_TRANSCODER_A:
+		return "TRANSCODER_A";
+	case POWER_DOMAIN_TRANSCODER_B:
+		return "TRANSCODER_B";
+	case POWER_DOMAIN_TRANSCODER_C:
+		return "TRANSCODER_C";
+	case POWER_DOMAIN_TRANSCODER_EDP:
+		return "TRANSCODER_EDP";
+	case POWER_DOMAIN_PORT_DDI_A_LANES:
+		return "PORT_DDI_A_LANES";
+	case POWER_DOMAIN_PORT_DDI_B_LANES:
+		return "PORT_DDI_B_LANES";
+	case POWER_DOMAIN_PORT_DDI_C_LANES:
+		return "PORT_DDI_C_LANES";
+	case POWER_DOMAIN_PORT_DDI_D_LANES:
+		return "PORT_DDI_D_LANES";
+	case POWER_DOMAIN_PORT_DDI_E_LANES:
+		return "PORT_DDI_E_LANES";
+	case POWER_DOMAIN_PORT_DSI:
+		return "PORT_DSI";
+	case POWER_DOMAIN_PORT_CRT:
+		return "PORT_CRT";
+	case POWER_DOMAIN_PORT_OTHER:
+		return "PORT_OTHER";
+	case POWER_DOMAIN_VGA:
+		return "VGA";
+	case POWER_DOMAIN_AUDIO:
+		return "AUDIO";
+	case POWER_DOMAIN_PLLS:
+		return "PLLS";
+	case POWER_DOMAIN_AUX_A:
+		return "AUX_A";
+	case POWER_DOMAIN_AUX_B:
+		return "AUX_B";
+	case POWER_DOMAIN_AUX_C:
+		return "AUX_C";
+	case POWER_DOMAIN_AUX_D:
+		return "AUX_D";
+	case POWER_DOMAIN_GMBUS:
+		return "GMBUS";
+	case POWER_DOMAIN_INIT:
+		return "INIT";
+	case POWER_DOMAIN_MODESET:
+		return "MODESET";
+	default:
+		MISSING_CASE(domain);
+		return "?";
+	}
+}
+
 static void intel_power_well_enable(struct drm_i915_private *dev_priv,
 				    struct i915_power_well *power_well)
 {
@@ -244,12 +307,6 @@ static void skl_power_well_post_enable(struct drm_i915_private *dev_priv,
 		gen8_irq_power_well_post_enable(dev_priv,
 						1 << PIPE_C | 1 << PIPE_B);
 	}
-
-	if (power_well->data == SKL_DISP_PW_1) {
-		if (!dev_priv->power_domains.initializing)
-			intel_prepare_ddi(dev);
-		gen8_irq_power_well_post_enable(dev_priv, 1 << PIPE_A);
-	}
 }
 
 static void hsw_set_power_well(struct drm_i915_private *dev_priv,
@@ -292,58 +349,38 @@ static void hsw_set_power_well(struct drm_i915_private *dev_priv,
 	BIT(POWER_DOMAIN_TRANSCODER_C) |		\
 	BIT(POWER_DOMAIN_PIPE_B_PANEL_FITTER) |		\
 	BIT(POWER_DOMAIN_PIPE_C_PANEL_FITTER) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_E_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_E_LANES) |		\
 	BIT(POWER_DOMAIN_AUX_B) |                       \
 	BIT(POWER_DOMAIN_AUX_C) |			\
 	BIT(POWER_DOMAIN_AUX_D) |			\
 	BIT(POWER_DOMAIN_AUDIO) |			\
 	BIT(POWER_DOMAIN_VGA) |				\
 	BIT(POWER_DOMAIN_INIT))
-#define SKL_DISPLAY_POWERWELL_1_POWER_DOMAINS (		\
-	SKL_DISPLAY_POWERWELL_2_POWER_DOMAINS |		\
-	BIT(POWER_DOMAIN_PLLS) |			\
-	BIT(POWER_DOMAIN_PIPE_A) |			\
-	BIT(POWER_DOMAIN_TRANSCODER_EDP) |		\
-	BIT(POWER_DOMAIN_PIPE_A_PANEL_FITTER) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
-	BIT(POWER_DOMAIN_AUX_A) |			\
-	BIT(POWER_DOMAIN_INIT))
 #define SKL_DISPLAY_DDI_A_E_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_E_2_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_A_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_E_LANES) |		\
 	BIT(POWER_DOMAIN_INIT))
 #define SKL_DISPLAY_DDI_B_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |		\
 	BIT(POWER_DOMAIN_INIT))
 #define SKL_DISPLAY_DDI_C_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |		\
 	BIT(POWER_DOMAIN_INIT))
 #define SKL_DISPLAY_DDI_D_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_LANES) |		\
 	BIT(POWER_DOMAIN_INIT))
-#define SKL_DISPLAY_MISC_IO_POWER_DOMAINS (		\
-	SKL_DISPLAY_POWERWELL_1_POWER_DOMAINS |		\
-	BIT(POWER_DOMAIN_PLLS) |			\
+#define SKL_DISPLAY_DC_OFF_POWER_DOMAINS (		\
+	SKL_DISPLAY_POWERWELL_2_POWER_DOMAINS |		\
+	BIT(POWER_DOMAIN_MODESET) |			\
+	BIT(POWER_DOMAIN_AUX_A) |			\
 	BIT(POWER_DOMAIN_INIT))
 #define SKL_DISPLAY_ALWAYS_ON_POWER_DOMAINS (		\
-	(POWER_DOMAIN_MASK & ~(SKL_DISPLAY_POWERWELL_1_POWER_DOMAINS |	\
+	(POWER_DOMAIN_MASK & ~(				\
 	SKL_DISPLAY_POWERWELL_2_POWER_DOMAINS |		\
-	SKL_DISPLAY_DDI_A_E_POWER_DOMAINS |		\
-	SKL_DISPLAY_DDI_B_POWER_DOMAINS |		\
-	SKL_DISPLAY_DDI_C_POWER_DOMAINS |		\
-	SKL_DISPLAY_DDI_D_POWER_DOMAINS |		\
-	SKL_DISPLAY_MISC_IO_POWER_DOMAINS)) |		\
+	SKL_DISPLAY_DC_OFF_POWER_DOMAINS)) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define BXT_DISPLAY_POWERWELL_2_POWER_DOMAINS (		\
@@ -354,10 +391,8 @@ static void hsw_set_power_well(struct drm_i915_private *dev_priv,
 	BIT(POWER_DOMAIN_TRANSCODER_C) |		\
 	BIT(POWER_DOMAIN_PIPE_B_PANEL_FITTER) |		\
 	BIT(POWER_DOMAIN_PIPE_C_PANEL_FITTER) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |		\
 	BIT(POWER_DOMAIN_AUX_B) |			\
 	BIT(POWER_DOMAIN_AUX_C) |			\
 	BIT(POWER_DOMAIN_AUDIO) |			\
@@ -369,11 +404,15 @@ static void hsw_set_power_well(struct drm_i915_private *dev_priv,
 	BIT(POWER_DOMAIN_PIPE_A) |			\
 	BIT(POWER_DOMAIN_TRANSCODER_EDP) |		\
 	BIT(POWER_DOMAIN_PIPE_A_PANEL_FITTER) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_A_LANES) |		\
 	BIT(POWER_DOMAIN_AUX_A) |			\
 	BIT(POWER_DOMAIN_PLLS) |			\
 	BIT(POWER_DOMAIN_INIT))
+#define BXT_DISPLAY_DC_OFF_POWER_DOMAINS (		\
+	BXT_DISPLAY_POWERWELL_2_POWER_DOMAINS |		\
+	BIT(POWER_DOMAIN_MODESET) |			\
+	BIT(POWER_DOMAIN_AUX_A) |			\
+	BIT(POWER_DOMAIN_INIT))
 #define BXT_DISPLAY_ALWAYS_ON_POWER_DOMAINS (		\
 	(POWER_DOMAIN_MASK & ~(BXT_DISPLAY_POWERWELL_1_POWER_DOMAINS |	\
 	BXT_DISPLAY_POWERWELL_2_POWER_DOMAINS)) |	\
@@ -417,46 +456,119 @@ static void assert_can_disable_dc9(struct drm_i915_private *dev_priv)
 	  */
 }
 
-void bxt_enable_dc9(struct drm_i915_private *dev_priv)
+static void gen9_set_dc_state_debugmask_memory_up(
+			struct drm_i915_private *dev_priv)
 {
 	uint32_t val;
 
+	/* The below bit doesn't need to be cleared ever afterwards */
+	val = I915_READ(DC_STATE_DEBUG);
+	if (!(val & DC_STATE_DEBUG_MASK_MEMORY_UP)) {
+		val |= DC_STATE_DEBUG_MASK_MEMORY_UP;
+		I915_WRITE(DC_STATE_DEBUG, val);
+		POSTING_READ(DC_STATE_DEBUG);
+	}
+}
+
+static void gen9_write_dc_state(struct drm_i915_private *dev_priv,
+				u32 state)
+{
+	int rewrites = 0;
+	int rereads = 0;
+	u32 v;
+
+	I915_WRITE(DC_STATE_EN, state);
+
+	/* It has been observed that disabling the dc6 state sometimes
+	 * doesn't stick and dmc keeps returning old value. Make sure
+	 * the write really sticks enough times and also force rewrite until
+	 * we are confident that state is exactly what we want.
+	 */
+	do  {
+		v = I915_READ(DC_STATE_EN);
+
+		if (v != state) {
+			I915_WRITE(DC_STATE_EN, state);
+			rewrites++;
+			rereads = 0;
+		} else if (rereads++ > 5) {
+			break;
+		}
+
+	} while (rewrites < 100);
+
+	if (v != state)
+		DRM_ERROR("Writing dc state to 0x%x failed, now 0x%x\n",
+			  state, v);
+
+	/* Most of the times we need one retry, avoid spam */
+	if (rewrites > 1)
+		DRM_DEBUG_KMS("Rewrote dc state to 0x%x %d times\n",
+			      state, rewrites);
+}
+
+static void gen9_set_dc_state(struct drm_i915_private *dev_priv, uint32_t state)
+{
+	uint32_t val;
+	uint32_t mask;
+
+	mask = DC_STATE_EN_UPTO_DC5;
+	if (IS_BROXTON(dev_priv))
+		mask |= DC_STATE_EN_DC9;
+	else
+		mask |= DC_STATE_EN_UPTO_DC6;
+
+	WARN_ON_ONCE(state & ~mask);
+
+	if (i915.enable_dc == 0)
+		state = DC_STATE_DISABLE;
+	else if (i915.enable_dc == 1 && state > DC_STATE_EN_UPTO_DC5)
+		state = DC_STATE_EN_UPTO_DC5;
+
+	if (state & DC_STATE_EN_UPTO_DC5_DC6_MASK)
+		gen9_set_dc_state_debugmask_memory_up(dev_priv);
+
+	val = I915_READ(DC_STATE_EN);
+	DRM_DEBUG_KMS("Setting DC state from %02x to %02x\n",
+		      val & mask, state);
+
+	/* Check if DMC is ignoring our DC state requests */
+	if ((val & mask) != dev_priv->csr.dc_state)
+		DRM_ERROR("DC state mismatch (0x%x -> 0x%x)\n",
+			  dev_priv->csr.dc_state, val & mask);
+
+	val &= ~mask;
+	val |= state;
+
+	gen9_write_dc_state(dev_priv, val);
+
+	dev_priv->csr.dc_state = val & mask;
+}
+
+void bxt_enable_dc9(struct drm_i915_private *dev_priv)
+{
 	assert_can_enable_dc9(dev_priv);
 
 	DRM_DEBUG_KMS("Enabling DC9\n");
 
-	val = I915_READ(DC_STATE_EN);
-	val |= DC_STATE_EN_DC9;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
+	gen9_set_dc_state(dev_priv, DC_STATE_EN_DC9);
 }
 
 void bxt_disable_dc9(struct drm_i915_private *dev_priv)
 {
-	uint32_t val;
-
 	assert_can_disable_dc9(dev_priv);
 
 	DRM_DEBUG_KMS("Disabling DC9\n");
 
-	val = I915_READ(DC_STATE_EN);
-	val &= ~DC_STATE_EN_DC9;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
+	gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
 }
 
-static void gen9_set_dc_state_debugmask_memory_up(
-			struct drm_i915_private *dev_priv)
+static void assert_csr_loaded(struct drm_i915_private *dev_priv)
 {
-	uint32_t val;
-
-	/* The below bit doesn't need to be cleared ever afterwards */
-	val = I915_READ(DC_STATE_DEBUG);
-	if (!(val & DC_STATE_DEBUG_MASK_MEMORY_UP)) {
-		val |= DC_STATE_DEBUG_MASK_MEMORY_UP;
-		I915_WRITE(DC_STATE_DEBUG, val);
-		POSTING_READ(DC_STATE_DEBUG);
-	}
+	WARN_ONCE(!I915_READ(CSR_PROGRAM(0)),
+		  "CSR program storage start is NULL\n");
+	WARN_ONCE(!I915_READ(CSR_SSP_BASE), "CSR SSP Base Not fine\n");
+	WARN_ONCE(!I915_READ(CSR_HTP_SKL), "CSR HTP Not fine\n");
 }
 
 static void assert_can_enable_dc5(struct drm_i915_private *dev_priv)
@@ -471,16 +583,13 @@ static void assert_can_enable_dc5(struct drm_i915_private *dev_priv)
 
 	WARN_ONCE((I915_READ(DC_STATE_EN) & DC_STATE_EN_UPTO_DC5),
 		  "DC5 already programmed to be enabled.\n");
-	WARN_ONCE(dev_priv->pm.suspended,
-		  "DC5 cannot be enabled, if platform is runtime-suspended.\n");
+	assert_rpm_wakelock_held(dev_priv);
 
 	assert_csr_loaded(dev_priv);
 }
 
 static void assert_can_disable_dc5(struct drm_i915_private *dev_priv)
 {
-	bool pg2_enabled = intel_display_power_well_is_enabled(dev_priv,
-					SKL_DISP_PW_2);
 	/*
 	 * During initialization, the firmware may not be loaded yet.
 	 * We still want to make sure that the DC enabling flag is cleared.
@@ -488,40 +597,16 @@ static void assert_can_disable_dc5(struct drm_i915_private *dev_priv)
 	if (dev_priv->power_domains.initializing)
 		return;
 
-	WARN_ONCE(!pg2_enabled, "PG2 not enabled to disable DC5.\n");
-	WARN_ONCE(dev_priv->pm.suspended,
-		"Disabling of DC5 while platform is runtime-suspended should never happen.\n");
+	assert_rpm_wakelock_held(dev_priv);
 }
 
 static void gen9_enable_dc5(struct drm_i915_private *dev_priv)
 {
-	uint32_t val;
-
 	assert_can_enable_dc5(dev_priv);
 
 	DRM_DEBUG_KMS("Enabling DC5\n");
 
-	gen9_set_dc_state_debugmask_memory_up(dev_priv);
-
-	val = I915_READ(DC_STATE_EN);
-	val &= ~DC_STATE_EN_UPTO_DC5_DC6_MASK;
-	val |= DC_STATE_EN_UPTO_DC5;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
-}
-
-static void gen9_disable_dc5(struct drm_i915_private *dev_priv)
-{
-	uint32_t val;
-
-	assert_can_disable_dc5(dev_priv);
-
-	DRM_DEBUG_KMS("Disabling DC5\n");
-
-	val = I915_READ(DC_STATE_EN);
-	val &= ~DC_STATE_EN_UPTO_DC5;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
+	gen9_set_dc_state(dev_priv, DC_STATE_EN_UPTO_DC5);
 }
 
 static void assert_can_enable_dc6(struct drm_i915_private *dev_priv)
@@ -547,40 +632,37 @@ static void assert_can_disable_dc6(struct drm_i915_private *dev_priv)
 	if (dev_priv->power_domains.initializing)
 		return;
 
-	assert_csr_loaded(dev_priv);
 	WARN_ONCE(!(I915_READ(DC_STATE_EN) & DC_STATE_EN_UPTO_DC6),
 		  "DC6 already programmed to be disabled.\n");
 }
 
-static void skl_enable_dc6(struct drm_i915_private *dev_priv)
+static void gen9_disable_dc5_dc6(struct drm_i915_private *dev_priv)
 {
-	uint32_t val;
+	assert_can_disable_dc5(dev_priv);
+
+	if (IS_SKYLAKE(dev_priv) && i915.enable_dc != 0 && i915.enable_dc != 1)
+		assert_can_disable_dc6(dev_priv);
 
+	gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
+}
+
+void skl_enable_dc6(struct drm_i915_private *dev_priv)
+{
 	assert_can_enable_dc6(dev_priv);
 
 	DRM_DEBUG_KMS("Enabling DC6\n");
 
-	gen9_set_dc_state_debugmask_memory_up(dev_priv);
+	gen9_set_dc_state(dev_priv, DC_STATE_EN_UPTO_DC6);
 
-	val = I915_READ(DC_STATE_EN);
-	val &= ~DC_STATE_EN_UPTO_DC5_DC6_MASK;
-	val |= DC_STATE_EN_UPTO_DC6;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
 }
 
-static void skl_disable_dc6(struct drm_i915_private *dev_priv)
+void skl_disable_dc6(struct drm_i915_private *dev_priv)
 {
-	uint32_t val;
-
 	assert_can_disable_dc6(dev_priv);
 
 	DRM_DEBUG_KMS("Disabling DC6\n");
 
-	val = I915_READ(DC_STATE_EN);
-	val &= ~DC_STATE_EN_UPTO_DC6;
-	I915_WRITE(DC_STATE_EN, val);
-	POSTING_READ(DC_STATE_EN);
+	gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
 }
 
 static void skl_set_power_well(struct drm_i915_private *dev_priv,
@@ -630,20 +712,16 @@ static void skl_set_power_well(struct drm_i915_private *dev_priv,
 				!I915_READ(HSW_PWR_WELL_BIOS),
 				"Invalid for power well status to be enabled, unless done by the BIOS, \
 				when request is to disable!\n");
-			if ((GEN9_ENABLE_DC5(dev) || SKL_ENABLE_DC6(dev)) &&
-				power_well->data == SKL_DISP_PW_2) {
-				if (SKL_ENABLE_DC6(dev)) {
-					skl_disable_dc6(dev_priv);
-					/*
-					 * DDI buffer programming unnecessary during driver-load/resume
-					 * as it's already done during modeset initialization then.
-					 * It's also invalid here as encoder list is still uninitialized.
-					 */
-					if (!dev_priv->power_domains.initializing)
-						intel_prepare_ddi(dev);
-				} else {
-					gen9_disable_dc5(dev_priv);
-				}
+			if (power_well->data == SKL_DISP_PW_2) {
+				/*
+				 * DDI buffer programming unnecessary during
+				 * driver-load/resume as it's already done
+				 * during modeset initialization then. It's
+				 * also invalid here as encoder list is still
+				 * uninitialized.
+				 */
+				if (!dev_priv->power_domains.initializing)
+					intel_prepare_ddi(dev);
 			}
 			I915_WRITE(HSW_PWR_WELL_DRIVER, tmp | req_mask);
 		}
@@ -658,34 +736,9 @@ static void skl_set_power_well(struct drm_i915_private *dev_priv,
 		}
 	} else {
 		if (enable_requested) {
-			if (IS_SKYLAKE(dev) &&
-				(power_well->data == SKL_DISP_PW_1) &&
-				(intel_csr_load_status_get(dev_priv) == FW_LOADED))
-				DRM_DEBUG_KMS("Not Disabling PW1, dmc will handle\n");
-			else {
-				I915_WRITE(HSW_PWR_WELL_DRIVER,	tmp & ~req_mask);
-				POSTING_READ(HSW_PWR_WELL_DRIVER);
-				DRM_DEBUG_KMS("Disabling %s\n", power_well->name);
-			}
-
-			if ((GEN9_ENABLE_DC5(dev) || SKL_ENABLE_DC6(dev)) &&
-				power_well->data == SKL_DISP_PW_2) {
-				enum csr_state state;
-				/* TODO: wait for a completion event or
-				 * similar here instead of busy
-				 * waiting using wait_for function.
-				 */
-				wait_for((state = intel_csr_load_status_get(dev_priv)) !=
-						FW_UNINITIALIZED, 1000);
-				if (state != FW_LOADED)
-					DRM_DEBUG("CSR firmware not ready (%d)\n",
-							state);
-				else
-					if (SKL_ENABLE_DC6(dev))
-						skl_enable_dc6(dev_priv);
-					else
-						gen9_enable_dc5(dev_priv);
-			}
+			I915_WRITE(HSW_PWR_WELL_DRIVER,	tmp & ~req_mask);
+			POSTING_READ(HSW_PWR_WELL_DRIVER);
+			DRM_DEBUG_KMS("Disabling %s\n", power_well->name);
 		}
 	}
 
@@ -760,6 +813,41 @@ static void skl_power_well_disable(struct drm_i915_private *dev_priv,
 	skl_set_power_well(dev_priv, power_well, false);
 }
 
+static bool gen9_dc_off_power_well_enabled(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	return (I915_READ(DC_STATE_EN) & DC_STATE_EN_UPTO_DC5_DC6_MASK) == 0;
+}
+
+static void gen9_dc_off_power_well_enable(struct drm_i915_private *dev_priv,
+					  struct i915_power_well *power_well)
+{
+	gen9_disable_dc5_dc6(dev_priv);
+}
+
+static void gen9_dc_off_power_well_disable(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	if (IS_SKYLAKE(dev_priv) && i915.enable_dc != 0 && i915.enable_dc != 1)
+		skl_enable_dc6(dev_priv);
+	else
+		gen9_enable_dc5(dev_priv);
+}
+
+static void gen9_dc_off_power_well_sync_hw(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	if (power_well->count > 0) {
+		gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
+	} else {
+		if (IS_SKYLAKE(dev_priv) && i915.enable_dc != 0 &&
+		    i915.enable_dc != 1)
+			gen9_set_dc_state(dev_priv, DC_STATE_EN_UPTO_DC6);
+		else
+			gen9_set_dc_state(dev_priv, DC_STATE_EN_UPTO_DC5);
+	}
+}
+
 static void i9xx_always_on_power_well_noop(struct drm_i915_private *dev_priv,
 					   struct i915_power_well *power_well)
 {
@@ -974,10 +1062,12 @@ static struct i915_power_well *lookup_power_well(struct drm_i915_private *dev_pr
 						 int power_well_id)
 {
 	struct i915_power_domains *power_domains = &dev_priv->power_domains;
-	struct i915_power_well *power_well;
 	int i;
 
-	for_each_power_well(i, power_well, POWER_DOMAIN_MASK, power_domains) {
+	for (i = 0; i < power_domains->power_well_count; i++) {
+		struct i915_power_well *power_well;
+
+		power_well = &power_domains->power_wells[i];
 		if (power_well->data == power_well_id)
 			return power_well;
 	}
@@ -1397,6 +1487,22 @@ static void chv_pipe_power_well_disable(struct drm_i915_private *dev_priv,
 	chv_set_pipe_power_well(dev_priv, power_well, false);
 }
 
+static void
+__intel_display_power_get_domain(struct drm_i915_private *dev_priv,
+				 enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+	struct i915_power_well *power_well;
+	int i;
+
+	for_each_power_well(i, power_well, BIT(domain), power_domains) {
+		if (!power_well->count++)
+			intel_power_well_enable(dev_priv, power_well);
+	}
+
+	power_domains->domain_use_count[domain]++;
+}
+
 /**
  * intel_display_power_get - grab a power domain reference
  * @dev_priv: i915 device instance
@@ -1412,24 +1518,53 @@ static void chv_pipe_power_well_disable(struct drm_i915_private *dev_priv,
 void intel_display_power_get(struct drm_i915_private *dev_priv,
 			     enum intel_display_power_domain domain)
 {
-	struct i915_power_domains *power_domains;
-	struct i915_power_well *power_well;
-	int i;
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
 
 	intel_runtime_pm_get(dev_priv);
 
-	power_domains = &dev_priv->power_domains;
+	mutex_lock(&power_domains->lock);
+
+	__intel_display_power_get_domain(dev_priv, domain);
+
+	mutex_unlock(&power_domains->lock);
+}
+
+/**
+ * intel_display_power_get_if_enabled - grab a reference for an enabled display power domain
+ * @dev_priv: i915 device instance
+ * @domain: power domain to reference
+ *
+ * This function grabs a power domain reference for @domain and ensures that the
+ * power domain and all its parents are powered up. Therefore users should only
+ * grab a reference to the innermost power domain they need.
+ *
+ * Any power domain reference obtained by this function must have a symmetric
+ * call to intel_display_power_put() to release the reference again.
+ */
+bool intel_display_power_get_if_enabled(struct drm_i915_private *dev_priv,
+					enum intel_display_power_domain domain)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+	bool is_enabled;
+
+	if (!intel_runtime_pm_get_if_in_use(dev_priv))
+		return false;
 
 	mutex_lock(&power_domains->lock);
 
-	for_each_power_well(i, power_well, BIT(domain), power_domains) {
-		if (!power_well->count++)
-			intel_power_well_enable(dev_priv, power_well);
+	if (__intel_display_power_is_enabled(dev_priv, domain)) {
+		__intel_display_power_get_domain(dev_priv, domain);
+		is_enabled = true;
+	} else {
+		is_enabled = false;
 	}
 
-	power_domains->domain_use_count[domain]++;
-
 	mutex_unlock(&power_domains->lock);
+
+	if (!is_enabled)
+		intel_runtime_pm_put(dev_priv);
+
+	return is_enabled;
 }
 
 /**
@@ -1452,13 +1587,17 @@ void intel_display_power_put(struct drm_i915_private *dev_priv,
 
 	mutex_lock(&power_domains->lock);
 
-	WARN_ON(!power_domains->domain_use_count[domain]);
+	WARN(!power_domains->domain_use_count[domain],
+	     "Use count on domain %s is already zero\n",
+	     intel_display_power_domain_str(domain));
 	power_domains->domain_use_count[domain]--;
 
 	for_each_power_well_rev(i, power_well, BIT(domain), power_domains) {
-		WARN_ON(!power_well->count);
+		WARN(!power_well->count,
+		     "Use count on power well %s is already zero",
+		     power_well->name);
 
-		if (!--power_well->count && i915.disable_power_well)
+		if (!--power_well->count)
 			intel_power_well_disable(dev_priv, power_well);
 	}
 
@@ -1470,14 +1609,10 @@ void intel_display_power_put(struct drm_i915_private *dev_priv,
 #define HSW_ALWAYS_ON_POWER_DOMAINS (			\
 	BIT(POWER_DOMAIN_PIPE_A) |			\
 	BIT(POWER_DOMAIN_TRANSCODER_EDP) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_A_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_A_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_LANES) |		\
 	BIT(POWER_DOMAIN_PORT_CRT) |			\
 	BIT(POWER_DOMAIN_PLLS) |			\
 	BIT(POWER_DOMAIN_AUX_A) |			\
@@ -1501,49 +1636,42 @@ void intel_display_power_put(struct drm_i915_private *dev_priv,
 #define VLV_DISPLAY_POWER_DOMAINS	POWER_DOMAIN_MASK
 
 #define VLV_DPIO_CMN_BC_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |	\
 	BIT(POWER_DOMAIN_PORT_CRT) |		\
 	BIT(POWER_DOMAIN_AUX_B) |		\
 	BIT(POWER_DOMAIN_AUX_C) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_B) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_B) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_C) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS (	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_C) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define CHV_DPIO_CMN_BC_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_B) |		\
 	BIT(POWER_DOMAIN_AUX_C) |		\
 	BIT(POWER_DOMAIN_INIT))
 
 #define CHV_DPIO_CMN_D_POWER_DOMAINS (		\
-	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
-	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_LANES) |	\
 	BIT(POWER_DOMAIN_AUX_D) |		\
 	BIT(POWER_DOMAIN_INIT))
 
@@ -1591,6 +1719,13 @@ static const struct i915_power_well_ops skl_power_well_ops = {
 	.is_enabled = skl_power_well_enabled,
 };
 
+static const struct i915_power_well_ops gen9_dc_off_power_well_ops = {
+	.sync_hw = gen9_dc_off_power_well_sync_hw,
+	.enable = gen9_dc_off_power_well_enable,
+	.disable = gen9_dc_off_power_well_disable,
+	.is_enabled = gen9_dc_off_power_well_enabled,
+};
+
 static struct i915_power_well hsw_power_wells[] = {
 	{
 		.name = "always-on",
@@ -1646,6 +1781,7 @@ static struct i915_power_well vlv_power_wells[] = {
 		.always_on = 1,
 		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
 		.ops = &i9xx_always_on_power_well_ops,
+		.data = PUNIT_POWER_WELL_ALWAYS_ON,
 	},
 	{
 		.name = "display",
@@ -1747,20 +1883,29 @@ static struct i915_power_well skl_power_wells[] = {
 		.always_on = 1,
 		.domains = SKL_DISPLAY_ALWAYS_ON_POWER_DOMAINS,
 		.ops = &i9xx_always_on_power_well_ops,
+		.data = SKL_DISP_PW_ALWAYS_ON,
 	},
 	{
 		.name = "power well 1",
-		.domains = SKL_DISPLAY_POWERWELL_1_POWER_DOMAINS,
+		/* Handled by the DMC firmware */
+		.domains = 0,
 		.ops = &skl_power_well_ops,
 		.data = SKL_DISP_PW_1,
 	},
 	{
 		.name = "MISC IO power well",
-		.domains = SKL_DISPLAY_MISC_IO_POWER_DOMAINS,
+		/* Handled by the DMC firmware */
+		.domains = 0,
 		.ops = &skl_power_well_ops,
 		.data = SKL_DISP_PW_MISC_IO,
 	},
 	{
+		.name = "DC off",
+		.domains = SKL_DISPLAY_DC_OFF_POWER_DOMAINS,
+		.ops = &gen9_dc_off_power_well_ops,
+		.data = SKL_DISP_PW_DC_OFF,
+	},
+	{
 		.name = "power well 2",
 		.domains = SKL_DISPLAY_POWERWELL_2_POWER_DOMAINS,
 		.ops = &skl_power_well_ops,
@@ -1792,6 +1937,34 @@ static struct i915_power_well skl_power_wells[] = {
 	},
 };
 
+void skl_pw1_misc_io_init(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_well *well;
+
+	if (!IS_SKYLAKE(dev_priv))
+		return;
+
+	well = lookup_power_well(dev_priv, SKL_DISP_PW_1);
+	intel_power_well_enable(dev_priv, well);
+
+	well = lookup_power_well(dev_priv, SKL_DISP_PW_MISC_IO);
+	intel_power_well_enable(dev_priv, well);
+}
+
+void skl_pw1_misc_io_fini(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_well *well;
+
+	if (!IS_SKYLAKE(dev_priv))
+		return;
+
+	well = lookup_power_well(dev_priv, SKL_DISP_PW_1);
+	intel_power_well_disable(dev_priv, well);
+
+	well = lookup_power_well(dev_priv, SKL_DISP_PW_MISC_IO);
+	intel_power_well_disable(dev_priv, well);
+}
+
 static struct i915_power_well bxt_power_wells[] = {
 	{
 		.name = "always-on",
@@ -1806,11 +1979,17 @@ static struct i915_power_well bxt_power_wells[] = {
 		.data = SKL_DISP_PW_1,
 	},
 	{
+		.name = "DC off",
+		.domains = BXT_DISPLAY_DC_OFF_POWER_DOMAINS,
+		.ops = &gen9_dc_off_power_well_ops,
+		.data = SKL_DISP_PW_DC_OFF,
+	},
+	{
 		.name = "power well 2",
 		.domains = BXT_DISPLAY_POWERWELL_2_POWER_DOMAINS,
 		.ops = &skl_power_well_ops,
 		.data = SKL_DISP_PW_2,
-	}
+	},
 };
 
 static int
@@ -1820,7 +1999,7 @@ sanitize_disable_power_well_option(const struct drm_i915_private *dev_priv,
 	if (disable_power_well >= 0)
 		return !!disable_power_well;
 
-	if (IS_SKYLAKE(dev_priv)) {
+	if (IS_BROXTON(dev_priv)) {
 		DRM_DEBUG_KMS("Disabling display power well support\n");
 		return 0;
 	}
@@ -1859,7 +2038,7 @@ int intel_power_domains_init(struct drm_i915_private *dev_priv)
 		set_power_wells(power_domains, hsw_power_wells);
 	} else if (IS_BROADWELL(dev_priv->dev)) {
 		set_power_wells(power_domains, bdw_power_wells);
-	} else if (IS_SKYLAKE(dev_priv->dev)) {
+	} else if (IS_SKYLAKE(dev_priv->dev) || IS_KABYLAKE(dev_priv->dev)) {
 		set_power_wells(power_domains, skl_power_wells);
 	} else if (IS_BROXTON(dev_priv->dev)) {
 		set_power_wells(power_domains, bxt_power_wells);
@@ -1874,21 +2053,6 @@ int intel_power_domains_init(struct drm_i915_private *dev_priv)
 	return 0;
 }
 
-static void intel_runtime_pm_disable(struct drm_i915_private *dev_priv)
-{
-	struct drm_device *dev = dev_priv->dev;
-	struct device *device = &dev->pdev->dev;
-
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	if (!intel_enable_rc6(dev))
-		return;
-
-	/* Make sure we're not suspended first. */
-	pm_runtime_get_sync(device);
-}
-
 /**
  * intel_power_domains_fini - finalizes the power domain structures
  * @dev_priv: i915 device instance
@@ -1899,15 +2063,32 @@ static void intel_runtime_pm_disable(struct drm_i915_private *dev_priv)
  */
 void intel_power_domains_fini(struct drm_i915_private *dev_priv)
 {
-	intel_runtime_pm_disable(dev_priv);
+	struct device *device = &dev_priv->dev->pdev->dev;
 
-	/* The i915.ko module is still not prepared to be loaded when
+	/*
+	 * The i915.ko module is still not prepared to be loaded when
 	 * the power well is not enabled, so just enable it in case
-	 * we're going to unload/reload. */
+	 * we're going to unload/reload.
+	 * The following also reacquires the RPM reference the core passed
+	 * to the driver during loading, which is dropped in
+	 * intel_runtime_pm_enable(). We have to hand back the control of the
+	 * device to the core with this reference held.
+	 */
 	intel_display_set_init_power(dev_priv, true);
+
+	/* Remove the refcount we took to keep power well support disabled. */
+	if (!i915.disable_power_well)
+		intel_display_power_put(dev_priv, POWER_DOMAIN_INIT);
+
+	/*
+	 * Remove the refcount we took in intel_runtime_pm_enable() in case
+	 * the platform doesn't support runtime PM.
+	 */
+	if (!HAS_RUNTIME_PM(dev_priv))
+		pm_runtime_put(device);
 }
 
-static void intel_power_domains_resume(struct drm_i915_private *dev_priv)
+static void intel_power_domains_sync_hw(struct drm_i915_private *dev_priv)
 {
 	struct i915_power_domains *power_domains = &dev_priv->power_domains;
 	struct i915_power_well *power_well;
@@ -1922,6 +2103,47 @@ static void intel_power_domains_resume(struct drm_i915_private *dev_priv)
 	mutex_unlock(&power_domains->lock);
 }
 
+static void skl_display_core_init(struct drm_i915_private *dev_priv,
+				  bool resume)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+	uint32_t val;
+
+	gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
+
+	/* enable PCH reset handshake */
+	val = I915_READ(HSW_NDE_RSTWRN_OPT);
+	I915_WRITE(HSW_NDE_RSTWRN_OPT, val | RESET_PCH_HANDSHAKE_ENABLE);
+
+	/* enable PG1 and Misc I/O */
+	mutex_lock(&power_domains->lock);
+	skl_pw1_misc_io_init(dev_priv);
+	mutex_unlock(&power_domains->lock);
+
+	if (!resume)
+		return;
+
+	skl_init_cdclk(dev_priv);
+
+	if (dev_priv->csr.dmc_payload)
+		intel_csr_load_program(dev_priv);
+}
+
+static void skl_display_core_uninit(struct drm_i915_private *dev_priv)
+{
+	struct i915_power_domains *power_domains = &dev_priv->power_domains;
+
+	gen9_set_dc_state(dev_priv, DC_STATE_DISABLE);
+
+	skl_uninit_cdclk(dev_priv);
+
+	/* The spec doesn't call for removing the reset handshake flag */
+	/* disable PG1 and Misc I/O */
+	mutex_lock(&power_domains->lock);
+	skl_pw1_misc_io_fini(dev_priv);
+	mutex_unlock(&power_domains->lock);
+}
+
 static void chv_phy_control_init(struct drm_i915_private *dev_priv)
 {
 	struct i915_power_well *cmn_bc =
@@ -2044,14 +2266,16 @@ static void vlv_cmnlane_wa(struct drm_i915_private *dev_priv)
  * This function initializes the hardware power domain state and enables all
  * power domains using intel_display_set_init_power().
  */
-void intel_power_domains_init_hw(struct drm_i915_private *dev_priv)
+void intel_power_domains_init_hw(struct drm_i915_private *dev_priv, bool resume)
 {
 	struct drm_device *dev = dev_priv->dev;
 	struct i915_power_domains *power_domains = &dev_priv->power_domains;
 
 	power_domains->initializing = true;
 
-	if (IS_CHERRYVIEW(dev)) {
+	if (IS_SKYLAKE(dev) || IS_KABYLAKE(dev)) {
+		skl_display_core_init(dev_priv, resume);
+	} else if (IS_CHERRYVIEW(dev)) {
 		mutex_lock(&power_domains->lock);
 		chv_phy_control_init(dev_priv);
 		mutex_unlock(&power_domains->lock);
@@ -2063,11 +2287,34 @@ void intel_power_domains_init_hw(struct drm_i915_private *dev_priv)
 
 	/* For now, we need the power well to be always enabled. */
 	intel_display_set_init_power(dev_priv, true);
-	intel_power_domains_resume(dev_priv);
+	/* Disable power support if the user asked so. */
+	if (!i915.disable_power_well)
+		intel_display_power_get(dev_priv, POWER_DOMAIN_INIT);
+	intel_power_domains_sync_hw(dev_priv);
 	power_domains->initializing = false;
 }
 
 /**
+ * intel_power_domains_suspend - suspend power domain state
+ * @dev_priv: i915 device instance
+ *
+ * This function prepares the hardware power domain state before entering
+ * system suspend. It must be paired with intel_power_domains_init_hw().
+ */
+void intel_power_domains_suspend(struct drm_i915_private *dev_priv)
+{
+	/*
+	 * Even if power well support was disabled we still want to disable
+	 * power wells while we are system suspended.
+	 */
+	if (!i915.disable_power_well)
+		intel_display_power_put(dev_priv, POWER_DOMAIN_INIT);
+
+	if (IS_SKYLAKE(dev_priv) || IS_KABYLAKE(dev_priv))
+		skl_display_core_uninit(dev_priv);
+}
+
+/**
  * intel_runtime_pm_get - grab a runtime pm reference
  * @dev_priv: i915 device instance
  *
@@ -2082,11 +2329,45 @@ void intel_runtime_pm_get(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = dev_priv->dev;
 	struct device *device = &dev->pdev->dev;
 
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
 	pm_runtime_get_sync(device);
-	WARN(dev_priv->pm.suspended, "Device still suspended.\n");
+
+	atomic_inc(&dev_priv->pm.wakeref_count);
+	assert_rpm_wakelock_held(dev_priv);
+}
+
+/**
+ * intel_runtime_pm_get_if_in_use - grab a runtime pm reference if device in use
+ * @dev_priv: i915 device instance
+ *
+ * This function grabs a device-level runtime pm reference if the device is
+ * already in use and ensures that it is powered up.
+ *
+ * Any runtime pm reference obtained by this function must have a symmetric
+ * call to intel_runtime_pm_put() to release the reference again.
+ */
+bool intel_runtime_pm_get_if_in_use(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct device *device = &dev->pdev->dev;
+
+	if (IS_ENABLED(CONFIG_PM)) {
+		int ret = pm_runtime_get_if_in_use(device);
+
+		/*
+		 * In cases runtime PM is disabled by the RPM core and we get
+		 * an -EINVAL return value we are not supposed to call this
+		 * function, since the power state is undefined. This applies
+		 * atm to the late/early system suspend/resume handlers.
+		 */
+		WARN_ON_ONCE(ret < 0);
+		if (ret <= 0)
+			return false;
+	}
+
+	atomic_inc(&dev_priv->pm.wakeref_count);
+	assert_rpm_wakelock_held(dev_priv);
+
+	return true;
 }
 
 /**
@@ -2111,11 +2392,10 @@ void intel_runtime_pm_get_noresume(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = dev_priv->dev;
 	struct device *device = &dev->pdev->dev;
 
-	if (!HAS_RUNTIME_PM(dev))
-		return;
-
-	WARN(dev_priv->pm.suspended, "Getting nosync-ref while suspended.\n");
+	assert_rpm_wakelock_held(dev_priv);
 	pm_runtime_get_noresume(device);
+
+	atomic_inc(&dev_priv->pm.wakeref_count);
 }
 
 /**
@@ -2131,8 +2411,9 @@ void intel_runtime_pm_put(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = dev_priv->dev;
 	struct device *device = &dev->pdev->dev;
 
-	if (!HAS_RUNTIME_PM(dev))
-		return;
+	assert_rpm_wakelock_held(dev_priv);
+	if (atomic_dec_and_test(&dev_priv->pm.wakeref_count))
+		atomic_inc(&dev_priv->pm.atomic_seq);
 
 	pm_runtime_mark_last_busy(device);
 	pm_runtime_put_autosuspend(device);
@@ -2153,22 +2434,27 @@ void intel_runtime_pm_enable(struct drm_i915_private *dev_priv)
 	struct drm_device *dev = dev_priv->dev;
 	struct device *device = &dev->pdev->dev;
 
-	if (!HAS_RUNTIME_PM(dev))
-		return;
+	pm_runtime_set_autosuspend_delay(device, 10000); /* 10s */
+	pm_runtime_mark_last_busy(device);
 
 	/*
-	 * RPM depends on RC6 to save restore the GT HW context, so make RC6 a
-	 * requirement.
+	 * Take a permanent reference to disable the RPM functionality and drop
+	 * it only when unloading the driver. Use the low level get/put helpers,
+	 * so the driver's own RPM reference tracking asserts also work on
+	 * platforms without RPM support.
 	 */
-	if (!intel_enable_rc6(dev)) {
-		DRM_INFO("RC6 disabled, disabling runtime PM support\n");
-		return;
+	if (!HAS_RUNTIME_PM(dev)) {
+		pm_runtime_dont_use_autosuspend(device);
+		pm_runtime_get_sync(device);
+	} else {
+		pm_runtime_use_autosuspend(device);
 	}
 
-	pm_runtime_set_autosuspend_delay(device, 10000); /* 10s */
-	pm_runtime_mark_last_busy(device);
-	pm_runtime_use_autosuspend(device);
-
+	/*
+	 * The core calls the driver load handler with an RPM reference held.
+	 * We drop that here and will reacquire it during unloading in
+	 * intel_power_domains_fini().
+	 */
 	pm_runtime_put_autosuspend(device);
 }
 
diff --git a/drivers/gpu/drm/i915/intel_sdvo.c b/drivers/gpu/drm/i915/intel_sdvo.c
index c42b636c2087..2e1da060b0e1 100644
--- a/drivers/gpu/drm/i915/intel_sdvo.c
+++ b/drivers/gpu/drm/i915/intel_sdvo.c
@@ -74,7 +74,7 @@ struct intel_sdvo {
 	struct i2c_adapter ddc;
 
 	/* Register for the SDVO device: SDVOB or SDVOC */
-	uint32_t sdvo_reg;
+	i915_reg_t sdvo_reg;
 
 	/* Active outputs controlled by this SDVO output */
 	uint16_t controlled_output;
@@ -120,8 +120,7 @@ struct intel_sdvo {
 	 */
 	bool is_tv;
 
-	/* On different gens SDVOB is at different places. */
-	bool is_sdvob;
+	enum port port;
 
 	/* This is for current tv format name */
 	int tv_format_index;
@@ -245,7 +244,7 @@ static void intel_sdvo_write_sdvox(struct intel_sdvo *intel_sdvo, u32 val)
 	u32 bval = val, cval = val;
 	int i;
 
-	if (intel_sdvo->sdvo_reg == PCH_SDVOB) {
+	if (HAS_PCH_SPLIT(dev_priv)) {
 		I915_WRITE(intel_sdvo->sdvo_reg, val);
 		POSTING_READ(intel_sdvo->sdvo_reg);
 		/*
@@ -259,7 +258,7 @@ static void intel_sdvo_write_sdvox(struct intel_sdvo *intel_sdvo, u32 val)
 		return;
 	}
 
-	if (intel_sdvo->sdvo_reg == GEN3_SDVOB)
+	if (intel_sdvo->port == PORT_B)
 		cval = I915_READ(GEN3_SDVOC);
 	else
 		bval = I915_READ(GEN3_SDVOB);
@@ -422,7 +421,7 @@ static const struct _sdvo_cmd_name {
 	SDVO_CMD_NAME_ENTRY(SDVO_CMD_GET_HBUF_DATA),
 };
 
-#define SDVO_NAME(svdo) ((svdo)->is_sdvob ? "SDVOB" : "SDVOC")
+#define SDVO_NAME(svdo) ((svdo)->port == PORT_B ? "SDVOB" : "SDVOC")
 
 static void intel_sdvo_debug_write(struct intel_sdvo *intel_sdvo, u8 cmd,
 				   const void *args, int args_len)
@@ -1282,14 +1281,10 @@ static void intel_sdvo_pre_enable(struct intel_encoder *intel_encoder)
 			sdvox |= SDVO_BORDER_ENABLE;
 	} else {
 		sdvox = I915_READ(intel_sdvo->sdvo_reg);
-		switch (intel_sdvo->sdvo_reg) {
-		case GEN3_SDVOB:
+		if (intel_sdvo->port == PORT_B)
 			sdvox &= SDVOB_PRESERVE_MASK;
-			break;
-		case GEN3_SDVOC:
+		else
 			sdvox &= SDVOC_PRESERVE_MASK;
-			break;
-		}
 		sdvox |= (9 << 19) | SDVO_BORDER_ENABLE;
 	}
 
@@ -1464,12 +1459,23 @@ static void intel_disable_sdvo(struct intel_encoder *encoder)
 	 * matching DP port to be enabled on transcoder A.
 	 */
 	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.
+		 */
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, false);
+
 		temp &= ~SDVO_PIPE_B_SELECT;
 		temp |= SDVO_ENABLE;
 		intel_sdvo_write_sdvox(intel_sdvo, temp);
 
 		temp &= ~SDVO_ENABLE;
 		intel_sdvo_write_sdvox(intel_sdvo, temp);
+
+		intel_wait_for_vblank_if_active(dev_priv->dev, PIPE_A);
+		intel_set_cpu_fifo_underrun_reporting(dev_priv, PIPE_A, true);
+		intel_set_pch_fifo_underrun_reporting(dev_priv, PIPE_A, true);
 	}
 }
 
@@ -2251,7 +2257,7 @@ intel_sdvo_select_ddc_bus(struct drm_i915_private *dev_priv,
 {
 	struct sdvo_device_mapping *mapping;
 
-	if (sdvo->is_sdvob)
+	if (sdvo->port == PORT_B)
 		mapping = &(dev_priv->sdvo_mappings[0]);
 	else
 		mapping = &(dev_priv->sdvo_mappings[1]);
@@ -2269,7 +2275,7 @@ intel_sdvo_select_i2c_bus(struct drm_i915_private *dev_priv,
 	struct sdvo_device_mapping *mapping;
 	u8 pin;
 
-	if (sdvo->is_sdvob)
+	if (sdvo->port == PORT_B)
 		mapping = &dev_priv->sdvo_mappings[0];
 	else
 		mapping = &dev_priv->sdvo_mappings[1];
@@ -2307,7 +2313,7 @@ intel_sdvo_get_slave_addr(struct drm_device *dev, struct intel_sdvo *sdvo)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct sdvo_device_mapping *my_mapping, *other_mapping;
 
-	if (sdvo->is_sdvob) {
+	if (sdvo->port == PORT_B) {
 		my_mapping = &dev_priv->sdvo_mappings[0];
 		other_mapping = &dev_priv->sdvo_mappings[1];
 	} else {
@@ -2332,7 +2338,7 @@ intel_sdvo_get_slave_addr(struct drm_device *dev, struct intel_sdvo *sdvo)
 	/* No SDVO device info is found for another DVO port,
 	 * so use mapping assumption we had before BIOS parsing.
 	 */
-	if (sdvo->is_sdvob)
+	if (sdvo->port == PORT_B)
 		return 0x70;
 	else
 		return 0x72;
@@ -2939,18 +2945,31 @@ intel_sdvo_init_ddc_proxy(struct intel_sdvo *sdvo,
 	return i2c_add_adapter(&sdvo->ddc) == 0;
 }
 
-bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
+static void assert_sdvo_port_valid(const struct drm_i915_private *dev_priv,
+				   enum port port)
+{
+	if (HAS_PCH_SPLIT(dev_priv))
+		WARN_ON(port != PORT_B);
+	else
+		WARN_ON(port != PORT_B && port != PORT_C);
+}
+
+bool intel_sdvo_init(struct drm_device *dev,
+		     i915_reg_t sdvo_reg, enum port port)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_encoder *intel_encoder;
 	struct intel_sdvo *intel_sdvo;
 	int i;
+
+	assert_sdvo_port_valid(dev_priv, port);
+
 	intel_sdvo = kzalloc(sizeof(*intel_sdvo), GFP_KERNEL);
 	if (!intel_sdvo)
 		return false;
 
 	intel_sdvo->sdvo_reg = sdvo_reg;
-	intel_sdvo->is_sdvob = is_sdvob;
+	intel_sdvo->port = port;
 	intel_sdvo->slave_addr = intel_sdvo_get_slave_addr(dev, intel_sdvo) >> 1;
 	intel_sdvo_select_i2c_bus(dev_priv, intel_sdvo);
 	if (!intel_sdvo_init_ddc_proxy(intel_sdvo, dev))
@@ -2959,7 +2978,8 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 	/* encoder type will be decided later */
 	intel_encoder = &intel_sdvo->base;
 	intel_encoder->type = INTEL_OUTPUT_SDVO;
-	drm_encoder_init(dev, &intel_encoder->base, &intel_sdvo_enc_funcs, 0);
+	drm_encoder_init(dev, &intel_encoder->base, &intel_sdvo_enc_funcs, 0,
+			 NULL);
 
 	/* Read the regs to test if we can talk to the device */
 	for (i = 0; i < 0x40; i++) {
@@ -3000,8 +3020,10 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob)
 	 * hotplug lines.
 	 */
 	if (intel_sdvo->hotplug_active) {
-		intel_encoder->hpd_pin =
-			intel_sdvo->is_sdvob ?  HPD_SDVO_B : HPD_SDVO_C;
+		if (intel_sdvo->port == PORT_B)
+			intel_encoder->hpd_pin = HPD_SDVO_B;
+		else
+			intel_encoder->hpd_pin = HPD_SDVO_C;
 	}
 
 	/*
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 56dc132e8e20..4ff7a1f4183e 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -192,10 +192,9 @@ skl_update_plane(struct drm_plane *drm_plane, struct drm_crtc *crtc,
 	const int pipe = intel_plane->pipe;
 	const int plane = intel_plane->plane + 1;
 	u32 plane_ctl, stride_div, stride;
-	int pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
 	const struct drm_intel_sprite_colorkey *key =
 		&to_intel_plane_state(drm_plane->state)->ckey;
-	unsigned long surf_addr;
+	u32 surf_addr;
 	u32 tile_height, plane_offset, plane_size;
 	unsigned int rotation;
 	int x_offset, y_offset;
@@ -212,10 +211,6 @@ skl_update_plane(struct drm_plane *drm_plane, struct drm_crtc *crtc,
 	rotation = drm_plane->state->rotation;
 	plane_ctl |= skl_plane_ctl_rotation(rotation);
 
-	intel_update_sprite_watermarks(drm_plane, crtc, src_w, src_h,
-				       pixel_size, true,
-				       src_w != crtc_w || src_h != crtc_h);
-
 	stride_div = intel_fb_stride_alignment(dev, fb->modifier[0],
 					       fb->pixel_format);
 
@@ -297,8 +292,6 @@ skl_disable_plane(struct drm_plane *dplane, struct drm_crtc *crtc)
 
 	I915_WRITE(PLANE_SURF(pipe, plane), 0);
 	POSTING_READ(PLANE_SURF(pipe, plane));
-
-	intel_update_sprite_watermarks(dplane, crtc, 0, 0, 0, false, false);
 }
 
 static void
@@ -541,10 +534,6 @@ ivb_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		sprctl |= SPRITE_PIPE_CSC_ENABLE;
 
-	intel_update_sprite_watermarks(plane, crtc, src_w, src_h, pixel_size,
-				       true,
-				       src_w != crtc_w || src_h != crtc_h);
-
 	/* Sizes are 0 based */
 	src_w--;
 	src_h--;
@@ -678,10 +667,6 @@ ilk_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	if (IS_GEN6(dev))
 		dvscntr |= DVS_TRICKLE_FEED_DISABLE; /* must disable */
 
-	intel_update_sprite_watermarks(plane, crtc, src_w, src_h,
-				       pixel_size, true,
-				       src_w != crtc_w || src_h != crtc_h);
-
 	/* Sizes are 0 based */
 	src_w--;
 	src_h--;
@@ -832,8 +817,8 @@ intel_check_sprite_plane(struct drm_plane *plane,
 		hscale = drm_rect_calc_hscale(src, dst, min_scale, max_scale);
 		if (hscale < 0) {
 			DRM_DEBUG_KMS("Horizontal scaling factor out of limits\n");
-			drm_rect_debug_print(src, true);
-			drm_rect_debug_print(dst, false);
+			drm_rect_debug_print("src: ", src, true);
+			drm_rect_debug_print("dst: ", dst, false);
 
 			return hscale;
 		}
@@ -841,8 +826,8 @@ intel_check_sprite_plane(struct drm_plane *plane,
 		vscale = drm_rect_calc_vscale(src, dst, min_scale, max_scale);
 		if (vscale < 0) {
 			DRM_DEBUG_KMS("Vertical scaling factor out of limits\n");
-			drm_rect_debug_print(src, true);
-			drm_rect_debug_print(dst, false);
+			drm_rect_debug_print("src: ", src, true);
+			drm_rect_debug_print("dst: ", dst, false);
 
 			return vscale;
 		}
@@ -938,9 +923,6 @@ intel_commit_sprite_plane(struct drm_plane *plane,
 
 	crtc = crtc ? crtc : plane->crtc;
 
-	if (!crtc->state->active)
-		return;
-
 	if (state->visible) {
 		intel_plane->update_plane(plane, crtc, fb,
 					  state->dst.x1, state->dst.y1,
@@ -969,7 +951,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) &&
+	if ((IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) &&
 	    set->flags & I915_SET_COLORKEY_DESTINATION)
 		return -EINVAL;
 
@@ -1104,7 +1086,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 			intel_plane->max_downscale = 1;
 		}
 
-		if (IS_VALLEYVIEW(dev)) {
+		if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 			intel_plane->update_plane = vlv_update_plane;
 			intel_plane->disable_plane = vlv_disable_plane;
 
@@ -1141,7 +1123,7 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 	ret = drm_universal_plane_init(dev, &intel_plane->base, possible_crtcs,
 				       &intel_plane_funcs,
 				       plane_formats, num_plane_formats,
-				       DRM_PLANE_TYPE_OVERLAY);
+				       DRM_PLANE_TYPE_OVERLAY, NULL);
 	if (ret) {
 		kfree(intel_plane);
 		goto out;
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index 6bea78944cd6..948cbff6c62e 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -1645,7 +1645,7 @@ intel_tv_init(struct drm_device *dev)
 			   DRM_MODE_CONNECTOR_SVIDEO);
 
 	drm_encoder_init(dev, &intel_encoder->base, &intel_tv_enc_funcs,
-			 DRM_MODE_ENCODER_TVDAC);
+			 DRM_MODE_ENCODER_TVDAC, NULL);
 
 	intel_encoder->compute_config = intel_tv_compute_config;
 	intel_encoder->get_config = intel_tv_get_config;
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index 43cba129a0c0..277e60ae0e47 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -29,19 +29,7 @@
 
 #define FORCEWAKE_ACK_TIMEOUT_MS 50
 
-#define __raw_i915_read8(dev_priv__, reg__) readb((dev_priv__)->regs + (reg__))
-#define __raw_i915_write8(dev_priv__, reg__, val__) writeb(val__, (dev_priv__)->regs + (reg__))
-
-#define __raw_i915_read16(dev_priv__, reg__) readw((dev_priv__)->regs + (reg__))
-#define __raw_i915_write16(dev_priv__, reg__, val__) writew(val__, (dev_priv__)->regs + (reg__))
-
-#define __raw_i915_read32(dev_priv__, reg__) readl((dev_priv__)->regs + (reg__))
-#define __raw_i915_write32(dev_priv__, reg__, val__) writel(val__, (dev_priv__)->regs + (reg__))
-
-#define __raw_i915_read64(dev_priv__, reg__) readq((dev_priv__)->regs + (reg__))
-#define __raw_i915_write64(dev_priv__, reg__, val__) writeq(val__, (dev_priv__)->regs + (reg__))
-
-#define __raw_posting_read(dev_priv__, reg__) (void)__raw_i915_read32(dev_priv__, reg__)
+#define __raw_posting_read(dev_priv__, reg__) (void)__raw_i915_read32((dev_priv__), (reg__))
 
 static const char * const forcewake_domain_names[] = {
 	"render",
@@ -62,17 +50,10 @@ intel_uncore_forcewake_domain_to_str(const enum forcewake_domain_id id)
 	return "unknown";
 }
 
-static void
-assert_device_not_suspended(struct drm_i915_private *dev_priv)
-{
-	WARN_ONCE(HAS_RUNTIME_PM(dev_priv->dev) && dev_priv->pm.suspended,
-		  "Device suspended\n");
-}
-
 static inline void
 fw_domain_reset(const struct intel_uncore_forcewake_domain *d)
 {
-	WARN_ON(d->reg_set == 0);
+	WARN_ON(!i915_mmio_reg_valid(d->reg_set));
 	__raw_i915_write32(d->i915, d->reg_set, d->val_reset);
 }
 
@@ -118,7 +99,7 @@ static inline void
 fw_domain_posting_read(const struct intel_uncore_forcewake_domain *d)
 {
 	/* something from same cacheline, but not from the set register */
-	if (d->reg_post)
+	if (i915_mmio_reg_valid(d->reg_post))
 		__raw_posting_read(d->i915, d->reg_post);
 }
 
@@ -248,7 +229,7 @@ static void intel_uncore_fw_release_timer(unsigned long arg)
 	struct intel_uncore_forcewake_domain *domain = (void *)arg;
 	unsigned long irqflags;
 
-	assert_device_not_suspended(domain->i915);
+	assert_rpm_device_not_suspended(domain->i915);
 
 	spin_lock_irqsave(&domain->i915->uncore.lock, irqflags);
 	if (WARN_ON(domain->wake_count == 0))
@@ -423,7 +404,7 @@ void intel_uncore_forcewake_get(struct drm_i915_private *dev_priv,
 	if (!dev_priv->uncore.funcs.force_wake_get)
 		return;
 
-	WARN_ON(dev_priv->pm.suspended);
+	assert_rpm_wakelock_held(dev_priv);
 
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags);
 	__intel_uncore_forcewake_get(dev_priv, fw_domains);
@@ -525,8 +506,7 @@ void assert_forcewakes_inactive(struct drm_i915_private *dev_priv)
 }
 
 /* We give fast paths for the really cool registers */
-#define NEEDS_FORCE_WAKE(reg) \
-	 ((reg) < 0x40000 && (reg) != FORCEWAKE)
+#define NEEDS_FORCE_WAKE(reg) ((reg) < 0x40000)
 
 #define REG_RANGE(reg, start, end) ((reg) >= (start) && (reg) < (end))
 
@@ -589,7 +569,7 @@ void assert_forcewakes_inactive(struct drm_i915_private *dev_priv)
 	REG_RANGE((reg), 0x9400, 0x9800)
 
 #define FORCEWAKE_GEN9_BLITTER_RANGE_OFFSET(reg) \
-	((reg) < 0x40000 &&\
+	((reg) < 0x40000 && \
 	 !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg) && \
 	 !FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg) && \
 	 !FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg) && \
@@ -605,8 +585,8 @@ ilk_dummy_write(struct drm_i915_private *dev_priv)
 }
 
 static void
-hsw_unclaimed_reg_debug(struct drm_i915_private *dev_priv, u32 reg, bool read,
-			bool before)
+hsw_unclaimed_reg_debug(struct drm_i915_private *dev_priv,
+			i915_reg_t reg, bool read, bool before)
 {
 	const char *op = read ? "reading" : "writing to";
 	const char *when = before ? "before" : "after";
@@ -616,7 +596,7 @@ hsw_unclaimed_reg_debug(struct drm_i915_private *dev_priv, u32 reg, bool read,
 
 	if (__raw_i915_read32(dev_priv, FPGA_DBG) & FPGA_DBG_RM_NOCLAIM) {
 		WARN(1, "Unclaimed register detected %s %s register 0x%x\n",
-		     when, op, reg);
+		     when, op, i915_mmio_reg_offset(reg));
 		__raw_i915_write32(dev_priv, FPGA_DBG, FPGA_DBG_RM_NOCLAIM);
 		i915.mmio_debug--; /* Only report the first N failures */
 	}
@@ -641,7 +621,7 @@ hsw_unclaimed_reg_detect(struct drm_i915_private *dev_priv)
 
 #define GEN2_READ_HEADER(x) \
 	u##x val = 0; \
-	assert_device_not_suspended(dev_priv);
+	assert_rpm_wakelock_held(dev_priv);
 
 #define GEN2_READ_FOOTER \
 	trace_i915_reg_rw(false, reg, val, sizeof(val), trace); \
@@ -649,7 +629,7 @@ hsw_unclaimed_reg_detect(struct drm_i915_private *dev_priv)
 
 #define __gen2_read(x) \
 static u##x \
-gen2_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+gen2_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	GEN2_READ_HEADER(x); \
 	val = __raw_i915_read##x(dev_priv, reg); \
 	GEN2_READ_FOOTER; \
@@ -657,7 +637,7 @@ gen2_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
 
 #define __gen5_read(x) \
 static u##x \
-gen5_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+gen5_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	GEN2_READ_HEADER(x); \
 	ilk_dummy_write(dev_priv); \
 	val = __raw_i915_read##x(dev_priv, reg); \
@@ -680,9 +660,10 @@ __gen2_read(64)
 #undef GEN2_READ_HEADER
 
 #define GEN6_READ_HEADER(x) \
+	u32 offset = i915_mmio_reg_offset(reg); \
 	unsigned long irqflags; \
 	u##x val = 0; \
-	assert_device_not_suspended(dev_priv); \
+	assert_rpm_wakelock_held(dev_priv); \
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags)
 
 #define GEN6_READ_FOOTER \
@@ -714,20 +695,12 @@ static inline void __force_wake_get(struct drm_i915_private *dev_priv,
 		dev_priv->uncore.funcs.force_wake_get(dev_priv, fw_domains);
 }
 
-#define __vgpu_read(x) \
-static u##x \
-vgpu_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
-	GEN6_READ_HEADER(x); \
-	val = __raw_i915_read##x(dev_priv, reg); \
-	GEN6_READ_FOOTER; \
-}
-
 #define __gen6_read(x) \
 static u##x \
-gen6_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+gen6_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	GEN6_READ_HEADER(x); \
 	hsw_unclaimed_reg_debug(dev_priv, reg, true, true); \
-	if (NEEDS_FORCE_WAKE(reg)) \
+	if (NEEDS_FORCE_WAKE(offset)) \
 		__force_wake_get(dev_priv, FORCEWAKE_RENDER); \
 	val = __raw_i915_read##x(dev_priv, reg); \
 	hsw_unclaimed_reg_debug(dev_priv, reg, true, false); \
@@ -736,47 +709,56 @@ gen6_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
 
 #define __vlv_read(x) \
 static u##x \
-vlv_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+vlv_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
+	enum forcewake_domains fw_engine = 0; \
 	GEN6_READ_HEADER(x); \
-	if (FORCEWAKE_VLV_RENDER_RANGE_OFFSET(reg)) \
-		__force_wake_get(dev_priv, FORCEWAKE_RENDER); \
-	else if (FORCEWAKE_VLV_MEDIA_RANGE_OFFSET(reg)) \
-		__force_wake_get(dev_priv, FORCEWAKE_MEDIA); \
+	if (!NEEDS_FORCE_WAKE(offset)) \
+		fw_engine = 0; \
+	else if (FORCEWAKE_VLV_RENDER_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_RENDER; \
+	else if (FORCEWAKE_VLV_MEDIA_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_MEDIA; \
+	if (fw_engine) \
+		__force_wake_get(dev_priv, fw_engine); \
 	val = __raw_i915_read##x(dev_priv, reg); \
 	GEN6_READ_FOOTER; \
 }
 
 #define __chv_read(x) \
 static u##x \
-chv_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+chv_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
+	enum forcewake_domains fw_engine = 0; \
 	GEN6_READ_HEADER(x); \
-	if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(reg)) \
-		__force_wake_get(dev_priv, FORCEWAKE_RENDER); \
-	else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(reg)) \
-		__force_wake_get(dev_priv, FORCEWAKE_MEDIA); \
-	else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(reg)) \
-		__force_wake_get(dev_priv, \
-				 FORCEWAKE_RENDER | FORCEWAKE_MEDIA); \
+	if (!NEEDS_FORCE_WAKE(offset)) \
+		fw_engine = 0; \
+	else if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_RENDER; \
+	else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_MEDIA; \
+	else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
+	if (fw_engine) \
+		__force_wake_get(dev_priv, fw_engine); \
 	val = __raw_i915_read##x(dev_priv, reg); \
 	GEN6_READ_FOOTER; \
 }
 
 #define SKL_NEEDS_FORCE_WAKE(reg) \
-	 ((reg) < 0x40000 && !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg))
+	((reg) < 0x40000 && !FORCEWAKE_GEN9_UNCORE_RANGE_OFFSET(reg))
 
 #define __gen9_read(x) \
 static u##x \
-gen9_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
+gen9_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
 	enum forcewake_domains fw_engine; \
 	GEN6_READ_HEADER(x); \
 	hsw_unclaimed_reg_debug(dev_priv, reg, true, true); \
-	if (!SKL_NEEDS_FORCE_WAKE(reg)) \
+	if (!SKL_NEEDS_FORCE_WAKE(offset)) \
 		fw_engine = 0; \
-	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
 	else \
 		fw_engine = FORCEWAKE_BLITTER; \
@@ -787,10 +769,6 @@ gen9_read##x(struct drm_i915_private *dev_priv, off_t reg, bool trace) { \
 	GEN6_READ_FOOTER; \
 }
 
-__vgpu_read(8)
-__vgpu_read(16)
-__vgpu_read(32)
-__vgpu_read(64)
 __gen9_read(8)
 __gen9_read(16)
 __gen9_read(32)
@@ -812,19 +790,46 @@ __gen6_read(64)
 #undef __chv_read
 #undef __vlv_read
 #undef __gen6_read
-#undef __vgpu_read
 #undef GEN6_READ_FOOTER
 #undef GEN6_READ_HEADER
 
+#define VGPU_READ_HEADER(x) \
+	unsigned long irqflags; \
+	u##x val = 0; \
+	assert_rpm_device_not_suspended(dev_priv); \
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags)
+
+#define VGPU_READ_FOOTER \
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags); \
+	trace_i915_reg_rw(false, reg, val, sizeof(val), trace); \
+	return val
+
+#define __vgpu_read(x) \
+static u##x \
+vgpu_read##x(struct drm_i915_private *dev_priv, i915_reg_t reg, bool trace) { \
+	VGPU_READ_HEADER(x); \
+	val = __raw_i915_read##x(dev_priv, reg); \
+	VGPU_READ_FOOTER; \
+}
+
+__vgpu_read(8)
+__vgpu_read(16)
+__vgpu_read(32)
+__vgpu_read(64)
+
+#undef __vgpu_read
+#undef VGPU_READ_FOOTER
+#undef VGPU_READ_HEADER
+
 #define GEN2_WRITE_HEADER \
 	trace_i915_reg_rw(true, reg, val, sizeof(val), trace); \
-	assert_device_not_suspended(dev_priv); \
+	assert_rpm_wakelock_held(dev_priv); \
 
 #define GEN2_WRITE_FOOTER
 
 #define __gen2_write(x) \
 static void \
-gen2_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
+gen2_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
 	GEN2_WRITE_HEADER; \
 	__raw_i915_write##x(dev_priv, reg, val); \
 	GEN2_WRITE_FOOTER; \
@@ -832,7 +837,7 @@ gen2_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace
 
 #define __gen5_write(x) \
 static void \
-gen5_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
+gen5_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
 	GEN2_WRITE_HEADER; \
 	ilk_dummy_write(dev_priv); \
 	__raw_i915_write##x(dev_priv, reg, val); \
@@ -855,9 +860,10 @@ __gen2_write(64)
 #undef GEN2_WRITE_HEADER
 
 #define GEN6_WRITE_HEADER \
+	u32 offset = i915_mmio_reg_offset(reg); \
 	unsigned long irqflags; \
 	trace_i915_reg_rw(true, reg, val, sizeof(val), trace); \
-	assert_device_not_suspended(dev_priv); \
+	assert_rpm_wakelock_held(dev_priv); \
 	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags)
 
 #define GEN6_WRITE_FOOTER \
@@ -865,10 +871,10 @@ __gen2_write(64)
 
 #define __gen6_write(x) \
 static void \
-gen6_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
+gen6_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(reg)) { \
+	if (NEEDS_FORCE_WAKE(offset)) { \
 		__fifo_ret = __gen6_gt_wait_for_fifo(dev_priv); \
 	} \
 	__raw_i915_write##x(dev_priv, reg, val); \
@@ -880,10 +886,10 @@ gen6_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace
 
 #define __hsw_write(x) \
 static void \
-hsw_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
+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(reg)) { \
+	if (NEEDS_FORCE_WAKE(offset)) { \
 		__fifo_ret = __gen6_gt_wait_for_fifo(dev_priv); \
 	} \
 	hsw_unclaimed_reg_debug(dev_priv, reg, false, true); \
@@ -896,15 +902,7 @@ hsw_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace)
 	GEN6_WRITE_FOOTER; \
 }
 
-#define __vgpu_write(x) \
-static void vgpu_write##x(struct drm_i915_private *dev_priv, \
-			  off_t reg, u##x val, bool trace) { \
-	GEN6_WRITE_HEADER; \
-	__raw_i915_write##x(dev_priv, reg, val); \
-	GEN6_WRITE_FOOTER; \
-}
-
-static const u32 gen8_shadowed_regs[] = {
+static const i915_reg_t gen8_shadowed_regs[] = {
 	FORCEWAKE_MT,
 	GEN6_RPNSWREQ,
 	GEN6_RC_VIDEO_FREQ,
@@ -915,11 +913,12 @@ static const u32 gen8_shadowed_regs[] = {
 	/* TODO: Other registers are not yet used */
 };
 
-static bool is_gen8_shadowed(struct drm_i915_private *dev_priv, u32 reg)
+static bool is_gen8_shadowed(struct drm_i915_private *dev_priv,
+			     i915_reg_t reg)
 {
 	int i;
 	for (i = 0; i < ARRAY_SIZE(gen8_shadowed_regs); i++)
-		if (reg == gen8_shadowed_regs[i])
+		if (i915_mmio_reg_equal(reg, gen8_shadowed_regs[i]))
 			return true;
 
 	return false;
@@ -927,10 +926,10 @@ static bool is_gen8_shadowed(struct drm_i915_private *dev_priv, u32 reg)
 
 #define __gen8_write(x) \
 static void \
-gen8_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
+gen8_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
 	GEN6_WRITE_HEADER; \
 	hsw_unclaimed_reg_debug(dev_priv, reg, false, true); \
-	if (reg < 0x40000 && !is_gen8_shadowed(dev_priv, reg)) \
+	if (NEEDS_FORCE_WAKE(offset) && !is_gen8_shadowed(dev_priv, reg)) \
 		__force_wake_get(dev_priv, FORCEWAKE_RENDER); \
 	__raw_i915_write##x(dev_priv, reg, val); \
 	hsw_unclaimed_reg_debug(dev_priv, reg, false, false); \
@@ -940,22 +939,25 @@ gen8_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace
 
 #define __chv_write(x) \
 static void \
-chv_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, bool trace) { \
-	bool shadowed = is_gen8_shadowed(dev_priv, reg); \
+chv_write##x(struct drm_i915_private *dev_priv, i915_reg_t reg, u##x val, bool trace) { \
+	enum forcewake_domains fw_engine = 0; \
 	GEN6_WRITE_HEADER; \
-	if (!shadowed) { \
-		if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(reg)) \
-			__force_wake_get(dev_priv, FORCEWAKE_RENDER); \
-		else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(reg)) \
-			__force_wake_get(dev_priv, FORCEWAKE_MEDIA); \
-		else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(reg)) \
-			__force_wake_get(dev_priv, FORCEWAKE_RENDER | FORCEWAKE_MEDIA); \
-	} \
+	if (!NEEDS_FORCE_WAKE(offset) || \
+	    is_gen8_shadowed(dev_priv, reg)) \
+		fw_engine = 0; \
+	else if (FORCEWAKE_CHV_RENDER_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_RENDER; \
+	else if (FORCEWAKE_CHV_MEDIA_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_MEDIA; \
+	else if (FORCEWAKE_CHV_COMMON_RANGE_OFFSET(offset)) \
+		fw_engine = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
+	if (fw_engine) \
+		__force_wake_get(dev_priv, fw_engine); \
 	__raw_i915_write##x(dev_priv, reg, val); \
 	GEN6_WRITE_FOOTER; \
 }
 
-static const u32 gen9_shadowed_regs[] = {
+static const i915_reg_t gen9_shadowed_regs[] = {
 	RING_TAIL(RENDER_RING_BASE),
 	RING_TAIL(GEN6_BSD_RING_BASE),
 	RING_TAIL(VEBOX_RING_BASE),
@@ -968,11 +970,12 @@ static const u32 gen9_shadowed_regs[] = {
 	/* TODO: Other registers are not yet used */
 };
 
-static bool is_gen9_shadowed(struct drm_i915_private *dev_priv, u32 reg)
+static bool is_gen9_shadowed(struct drm_i915_private *dev_priv,
+			     i915_reg_t reg)
 {
 	int i;
 	for (i = 0; i < ARRAY_SIZE(gen9_shadowed_regs); i++)
-		if (reg == gen9_shadowed_regs[i])
+		if (i915_mmio_reg_equal(reg, gen9_shadowed_regs[i]))
 			return true;
 
 	return false;
@@ -980,19 +983,19 @@ static bool is_gen9_shadowed(struct drm_i915_private *dev_priv, u32 reg)
 
 #define __gen9_write(x) \
 static void \
-gen9_write##x(struct drm_i915_private *dev_priv, off_t reg, u##x val, \
+gen9_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; \
 	hsw_unclaimed_reg_debug(dev_priv, reg, false, true); \
-	if (!SKL_NEEDS_FORCE_WAKE(reg) || \
+	if (!SKL_NEEDS_FORCE_WAKE(offset) || \
 	    is_gen9_shadowed(dev_priv, reg)) \
 		fw_engine = 0; \
-	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_RENDER_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_RENDER; \
-	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_MEDIA_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_MEDIA; \
-	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(reg)) \
+	else if (FORCEWAKE_GEN9_COMMON_RANGE_OFFSET(offset)) \
 		fw_engine = FORCEWAKE_RENDER | FORCEWAKE_MEDIA; \
 	else \
 		fw_engine = FORCEWAKE_BLITTER; \
@@ -1024,20 +1027,41 @@ __gen6_write(8)
 __gen6_write(16)
 __gen6_write(32)
 __gen6_write(64)
-__vgpu_write(8)
-__vgpu_write(16)
-__vgpu_write(32)
-__vgpu_write(64)
 
 #undef __gen9_write
 #undef __chv_write
 #undef __gen8_write
 #undef __hsw_write
 #undef __gen6_write
-#undef __vgpu_write
 #undef GEN6_WRITE_FOOTER
 #undef GEN6_WRITE_HEADER
 
+#define VGPU_WRITE_HEADER \
+	unsigned long irqflags; \
+	trace_i915_reg_rw(true, reg, val, sizeof(val), trace); \
+	assert_rpm_device_not_suspended(dev_priv); \
+	spin_lock_irqsave(&dev_priv->uncore.lock, irqflags)
+
+#define VGPU_WRITE_FOOTER \
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, irqflags)
+
+#define __vgpu_write(x) \
+static void vgpu_write##x(struct drm_i915_private *dev_priv, \
+			  i915_reg_t reg, u##x val, bool trace) { \
+	VGPU_WRITE_HEADER; \
+	__raw_i915_write##x(dev_priv, reg, val); \
+	VGPU_WRITE_FOOTER; \
+}
+
+__vgpu_write(8)
+__vgpu_write(16)
+__vgpu_write(32)
+__vgpu_write(64)
+
+#undef __vgpu_write
+#undef VGPU_WRITE_FOOTER
+#undef VGPU_WRITE_HEADER
+
 #define ASSIGN_WRITE_MMIO_VFUNCS(x) \
 do { \
 	dev_priv->uncore.funcs.mmio_writeb = x##_write8; \
@@ -1057,7 +1081,8 @@ do { \
 
 static void fw_domain_init(struct drm_i915_private *dev_priv,
 			   enum forcewake_domain_id domain_id,
-			   u32 reg_set, u32 reg_ack)
+			   i915_reg_t reg_set,
+			   i915_reg_t reg_ack)
 {
 	struct intel_uncore_forcewake_domain *d;
 
@@ -1083,12 +1108,10 @@ static void fw_domain_init(struct drm_i915_private *dev_priv,
 		d->val_clear = _MASKED_BIT_DISABLE(FORCEWAKE_KERNEL);
 	}
 
-	if (IS_VALLEYVIEW(dev_priv))
+	if (IS_VALLEYVIEW(dev_priv) || IS_CHERRYVIEW(dev_priv))
 		d->reg_post = FORCEWAKE_ACK_VLV;
 	else if (IS_GEN6(dev_priv) || IS_GEN7(dev_priv) || IS_GEN8(dev_priv))
 		d->reg_post = ECOBUS;
-	else
-		d->reg_post = 0;
 
 	d->i915 = dev_priv;
 	d->id = domain_id;
@@ -1118,7 +1141,7 @@ static void intel_uncore_fw_domains_init(struct drm_device *dev)
 			       FORCEWAKE_ACK_BLITTER_GEN9);
 		fw_domain_init(dev_priv, FW_DOMAIN_ID_MEDIA,
 			       FORCEWAKE_MEDIA_GEN9, FORCEWAKE_ACK_MEDIA_GEN9);
-	} else if (IS_VALLEYVIEW(dev)) {
+	} else if (IS_VALLEYVIEW(dev) || IS_CHERRYVIEW(dev)) {
 		dev_priv->uncore.funcs.force_wake_get = fw_domains_get;
 		if (!IS_CHERRYVIEW(dev))
 			dev_priv->uncore.funcs.force_wake_put =
@@ -1262,12 +1285,14 @@ void intel_uncore_fini(struct drm_device *dev)
 #define GEN_RANGE(l, h) GENMASK(h, l)
 
 static const struct register_whitelist {
-	uint64_t offset;
+	i915_reg_t offset_ldw, offset_udw;
 	uint32_t size;
 	/* supported gens, 0x10 for 4, 0x30 for 4 and 5, etc. */
 	uint32_t gen_bitmask;
 } whitelist[] = {
-	{ RING_TIMESTAMP(RENDER_RING_BASE), 8, GEN_RANGE(4, 9) },
+	{ .offset_ldw = RING_TIMESTAMP(RENDER_RING_BASE),
+	  .offset_udw = RING_TIMESTAMP_UDW(RENDER_RING_BASE),
+	  .size = 8, .gen_bitmask = GEN_RANGE(4, 9) },
 };
 
 int i915_reg_read_ioctl(struct drm_device *dev,
@@ -1277,11 +1302,11 @@ int i915_reg_read_ioctl(struct drm_device *dev,
 	struct drm_i915_reg_read *reg = data;
 	struct register_whitelist const *entry = whitelist;
 	unsigned size;
-	u64 offset;
+	i915_reg_t offset_ldw, offset_udw;
 	int i, ret = 0;
 
 	for (i = 0; i < ARRAY_SIZE(whitelist); i++, entry++) {
-		if (entry->offset == (reg->offset & -entry->size) &&
+		if (i915_mmio_reg_offset(entry->offset_ldw) == (reg->offset & -entry->size) &&
 		    (1 << INTEL_INFO(dev)->gen & entry->gen_bitmask))
 			break;
 	}
@@ -1293,27 +1318,28 @@ int i915_reg_read_ioctl(struct drm_device *dev,
 	 * be naturally aligned (and those that are not so aligned merely
 	 * limit the available flags for that register).
 	 */
-	offset = entry->offset;
+	offset_ldw = entry->offset_ldw;
+	offset_udw = entry->offset_udw;
 	size = entry->size;
-	size |= reg->offset ^ offset;
+	size |= reg->offset ^ i915_mmio_reg_offset(offset_ldw);
 
 	intel_runtime_pm_get(dev_priv);
 
 	switch (size) {
 	case 8 | 1:
-		reg->val = I915_READ64_2x32(offset, offset+4);
+		reg->val = I915_READ64_2x32(offset_ldw, offset_udw);
 		break;
 	case 8:
-		reg->val = I915_READ64(offset);
+		reg->val = I915_READ64(offset_ldw);
 		break;
 	case 4:
-		reg->val = I915_READ(offset);
+		reg->val = I915_READ(offset_ldw);
 		break;
 	case 2:
-		reg->val = I915_READ16(offset);
+		reg->val = I915_READ16(offset_ldw);
 		break;
 	case 1:
-		reg->val = I915_READ8(offset);
+		reg->val = I915_READ8(offset_ldw);
 		break;
 	default:
 		ret = -EINVAL;
@@ -1470,7 +1496,7 @@ static int gen6_do_reset(struct drm_device *dev)
 }
 
 static int wait_for_register(struct drm_i915_private *dev_priv,
-			     const u32 reg,
+			     i915_reg_t reg,
 			     const u32 mask,
 			     const u32 value,
 			     const unsigned long timeout_ms)
diff --git a/drivers/gpu/drm/imx/Kconfig b/drivers/gpu/drm/imx/Kconfig
index 2b81a417cf29..a1844b50546c 100644
--- a/drivers/gpu/drm/imx/Kconfig
+++ b/drivers/gpu/drm/imx/Kconfig
@@ -5,20 +5,11 @@ config DRM_IMX
 	select VIDEOMODE_HELPERS
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_CMA_HELPER
-	depends on DRM && (ARCH_MXC || ARCH_MULTIPLATFORM) && HAVE_DMA_ATTRS
+	depends on DRM && (ARCH_MXC || ARCH_MULTIPLATFORM)
 	depends on IMX_IPUV3_CORE
 	help
 	  enable i.MX graphics support
 
-config DRM_IMX_FB_HELPER
-	tristate "provide legacy framebuffer /dev/fb0"
-	select DRM_KMS_CMA_HELPER
-	depends on DRM_IMX
-	help
-	  The DRM framework can provide a legacy /dev/fb0 framebuffer
-	  for your device. This is necessary to get a framebuffer console
-	  and also for applications using the legacy framebuffer API
-
 config DRM_IMX_PARALLEL_DISPLAY
 	tristate "Support for parallel displays"
 	select DRM_PANEL
diff --git a/drivers/gpu/drm/imx/dw_hdmi-imx.c b/drivers/gpu/drm/imx/dw_hdmi-imx.c
index 98605ea2ad9d..063825fecbe2 100644
--- a/drivers/gpu/drm/imx/dw_hdmi-imx.c
+++ b/drivers/gpu/drm/imx/dw_hdmi-imx.c
@@ -137,7 +137,7 @@ static void dw_hdmi_imx_encoder_prepare(struct drm_encoder *encoder)
 	imx_drm_set_bus_format(encoder, MEDIA_BUS_FMT_RGB888_1X24);
 }
 
-static struct drm_encoder_helper_funcs dw_hdmi_imx_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs dw_hdmi_imx_encoder_helper_funcs = {
 	.mode_fixup = dw_hdmi_imx_encoder_mode_fixup,
 	.mode_set   = dw_hdmi_imx_encoder_mode_set,
 	.prepare    = dw_hdmi_imx_encoder_prepare,
@@ -145,7 +145,7 @@ static struct drm_encoder_helper_funcs dw_hdmi_imx_encoder_helper_funcs = {
 	.disable    = dw_hdmi_imx_encoder_disable,
 };
 
-static struct drm_encoder_funcs dw_hdmi_imx_encoder_funcs = {
+static const struct drm_encoder_funcs dw_hdmi_imx_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -251,7 +251,7 @@ static int dw_hdmi_imx_bind(struct device *dev, struct device *master,
 
 	drm_encoder_helper_add(encoder, &dw_hdmi_imx_encoder_helper_funcs);
 	drm_encoder_init(drm, encoder, &dw_hdmi_imx_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	return dw_hdmi_bind(dev, master, data, encoder, iores, irq, plat_data);
 }
diff --git a/drivers/gpu/drm/imx/imx-drm-core.c b/drivers/gpu/drm/imx/imx-drm-core.c
index 7b990b4e96d2..2f57d7967417 100644
--- a/drivers/gpu/drm/imx/imx-drm-core.c
+++ b/drivers/gpu/drm/imx/imx-drm-core.c
@@ -39,46 +39,41 @@ struct imx_drm_component {
 struct imx_drm_device {
 	struct drm_device			*drm;
 	struct imx_drm_crtc			*crtc[MAX_CRTC];
-	int					pipes;
+	unsigned int				pipes;
 	struct drm_fbdev_cma			*fbhelper;
 };
 
 struct imx_drm_crtc {
 	struct drm_crtc				*crtc;
-	int					pipe;
 	struct imx_drm_crtc_helper_funcs	imx_drm_helper_funcs;
 };
 
+#if IS_ENABLED(CONFIG_DRM_FBDEV_EMULATION)
 static int legacyfb_depth = 16;
 module_param(legacyfb_depth, int, 0444);
+#endif
 
-int imx_drm_crtc_id(struct imx_drm_crtc *crtc)
+unsigned int imx_drm_crtc_id(struct imx_drm_crtc *crtc)
 {
-	return crtc->pipe;
+	return drm_crtc_index(crtc->crtc);
 }
 EXPORT_SYMBOL_GPL(imx_drm_crtc_id);
 
 static void imx_drm_driver_lastclose(struct drm_device *drm)
 {
-#if IS_ENABLED(CONFIG_DRM_IMX_FB_HELPER)
 	struct imx_drm_device *imxdrm = drm->dev_private;
 
 	drm_fbdev_cma_restore_mode(imxdrm->fbhelper);
-#endif
 }
 
 static int imx_drm_driver_unload(struct drm_device *drm)
 {
-#if IS_ENABLED(CONFIG_DRM_IMX_FB_HELPER)
 	struct imx_drm_device *imxdrm = drm->dev_private;
-#endif
 
 	drm_kms_helper_poll_fini(drm);
 
-#if IS_ENABLED(CONFIG_DRM_IMX_FB_HELPER)
 	if (imxdrm->fbhelper)
 		drm_fbdev_cma_fini(imxdrm->fbhelper);
-#endif
 
 	component_unbind_all(drm->dev, drm);
 
@@ -128,19 +123,19 @@ EXPORT_SYMBOL_GPL(imx_drm_set_bus_format);
 
 int imx_drm_crtc_vblank_get(struct imx_drm_crtc *imx_drm_crtc)
 {
-	return drm_vblank_get(imx_drm_crtc->crtc->dev, imx_drm_crtc->pipe);
+	return drm_crtc_vblank_get(imx_drm_crtc->crtc);
 }
 EXPORT_SYMBOL_GPL(imx_drm_crtc_vblank_get);
 
 void imx_drm_crtc_vblank_put(struct imx_drm_crtc *imx_drm_crtc)
 {
-	drm_vblank_put(imx_drm_crtc->crtc->dev, imx_drm_crtc->pipe);
+	drm_crtc_vblank_put(imx_drm_crtc->crtc);
 }
 EXPORT_SYMBOL_GPL(imx_drm_crtc_vblank_put);
 
 void imx_drm_handle_vblank(struct imx_drm_crtc *imx_drm_crtc)
 {
-	drm_handle_vblank(imx_drm_crtc->crtc->dev, imx_drm_crtc->pipe);
+	drm_crtc_handle_vblank(imx_drm_crtc->crtc);
 }
 EXPORT_SYMBOL_GPL(imx_drm_handle_vblank);
 
@@ -214,14 +209,12 @@ EXPORT_SYMBOL_GPL(imx_drm_encoder_destroy);
 
 static void imx_drm_output_poll_changed(struct drm_device *drm)
 {
-#if IS_ENABLED(CONFIG_DRM_IMX_FB_HELPER)
 	struct imx_drm_device *imxdrm = drm->dev_private;
 
 	drm_fbdev_cma_hotplug_event(imxdrm->fbhelper);
-#endif
 }
 
-static struct drm_mode_config_funcs imx_drm_mode_config_funcs = {
+static const struct drm_mode_config_funcs imx_drm_mode_config_funcs = {
 	.fb_create = drm_fb_cma_create,
 	.output_poll_changed = imx_drm_output_poll_changed,
 };
@@ -307,11 +300,12 @@ static int imx_drm_driver_load(struct drm_device *drm, unsigned long flags)
 	 * The fb helper takes copies of key hardware information, so the
 	 * crtcs/connectors/encoders must not change after this point.
 	 */
-#if IS_ENABLED(CONFIG_DRM_IMX_FB_HELPER)
+#if IS_ENABLED(CONFIG_DRM_FBDEV_EMULATION)
 	if (legacyfb_depth != 16 && legacyfb_depth != 32) {
 		dev_warn(drm->dev, "Invalid legacyfb_depth.  Defaulting to 16bpp\n");
 		legacyfb_depth = 16;
 	}
+	drm_helper_disable_unused_functions(drm);
 	imxdrm->fbhelper = drm_fbdev_cma_init(drm, legacyfb_depth,
 				drm->mode_config.num_crtc, MAX_CRTC);
 	if (IS_ERR(imxdrm->fbhelper)) {
@@ -362,12 +356,11 @@ int imx_drm_add_crtc(struct drm_device *drm, struct drm_crtc *crtc,
 		return -ENOMEM;
 
 	imx_drm_crtc->imx_drm_helper_funcs = *imx_drm_helper_funcs;
-	imx_drm_crtc->pipe = imxdrm->pipes++;
 	imx_drm_crtc->crtc = crtc;
 
 	crtc->port = port;
 
-	imxdrm->crtc[imx_drm_crtc->pipe] = imx_drm_crtc;
+	imxdrm->crtc[imxdrm->pipes++] = imx_drm_crtc;
 
 	*new_crtc = imx_drm_crtc;
 
@@ -379,12 +372,12 @@ int imx_drm_add_crtc(struct drm_device *drm, struct drm_crtc *crtc,
 			imx_drm_crtc->imx_drm_helper_funcs.crtc_helper_funcs);
 
 	drm_crtc_init_with_planes(drm, crtc, primary_plane, NULL,
-			imx_drm_crtc->imx_drm_helper_funcs.crtc_funcs);
+			imx_drm_crtc->imx_drm_helper_funcs.crtc_funcs, NULL);
 
 	return 0;
 
 err_register:
-	imxdrm->crtc[imx_drm_crtc->pipe] = NULL;
+	imxdrm->crtc[--imxdrm->pipes] = NULL;
 	kfree(imx_drm_crtc);
 	return ret;
 }
@@ -396,10 +389,11 @@ EXPORT_SYMBOL_GPL(imx_drm_add_crtc);
 int imx_drm_remove_crtc(struct imx_drm_crtc *imx_drm_crtc)
 {
 	struct imx_drm_device *imxdrm = imx_drm_crtc->crtc->dev->dev_private;
+	unsigned int pipe = drm_crtc_index(imx_drm_crtc->crtc);
 
 	drm_crtc_cleanup(imx_drm_crtc->crtc);
 
-	imxdrm->crtc[imx_drm_crtc->pipe] = NULL;
+	imxdrm->crtc[pipe] = NULL;
 
 	kfree(imx_drm_crtc);
 
diff --git a/drivers/gpu/drm/imx/imx-drm.h b/drivers/gpu/drm/imx/imx-drm.h
index 83284b4d4be1..71cf6d9c714f 100644
--- a/drivers/gpu/drm/imx/imx-drm.h
+++ b/drivers/gpu/drm/imx/imx-drm.h
@@ -13,7 +13,7 @@ struct drm_plane;
 struct imx_drm_crtc;
 struct platform_device;
 
-int imx_drm_crtc_id(struct imx_drm_crtc *crtc);
+unsigned int imx_drm_crtc_id(struct imx_drm_crtc *crtc);
 
 struct imx_drm_crtc_helper_funcs {
 	int (*enable_vblank)(struct drm_crtc *crtc);
diff --git a/drivers/gpu/drm/imx/imx-ldb.c b/drivers/gpu/drm/imx/imx-ldb.c
index abacc8f67469..22ac482231ed 100644
--- a/drivers/gpu/drm/imx/imx-ldb.c
+++ b/drivers/gpu/drm/imx/imx-ldb.c
@@ -358,23 +358,23 @@ static void imx_ldb_encoder_disable(struct drm_encoder *encoder)
 	drm_panel_unprepare(imx_ldb_ch->panel);
 }
 
-static struct drm_connector_funcs imx_ldb_connector_funcs = {
+static const struct drm_connector_funcs imx_ldb_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = imx_ldb_connector_detect,
 	.destroy = imx_drm_connector_destroy,
 };
 
-static struct drm_connector_helper_funcs imx_ldb_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs imx_ldb_connector_helper_funcs = {
 	.get_modes = imx_ldb_connector_get_modes,
 	.best_encoder = imx_ldb_connector_best_encoder,
 };
 
-static struct drm_encoder_funcs imx_ldb_encoder_funcs = {
+static const struct drm_encoder_funcs imx_ldb_encoder_funcs = {
 	.destroy = imx_drm_encoder_destroy,
 };
 
-static struct drm_encoder_helper_funcs imx_ldb_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs imx_ldb_encoder_helper_funcs = {
 	.dpms = imx_ldb_encoder_dpms,
 	.mode_fixup = imx_ldb_encoder_mode_fixup,
 	.prepare = imx_ldb_encoder_prepare,
@@ -422,7 +422,7 @@ static int imx_ldb_register(struct drm_device *drm,
 	drm_encoder_helper_add(&imx_ldb_ch->encoder,
 			&imx_ldb_encoder_helper_funcs);
 	drm_encoder_init(drm, &imx_ldb_ch->encoder, &imx_ldb_encoder_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 
 	drm_connector_helper_add(&imx_ldb_ch->connector,
 			&imx_ldb_connector_helper_funcs);
diff --git a/drivers/gpu/drm/imx/imx-tve.c b/drivers/gpu/drm/imx/imx-tve.c
index f9597146dc67..292349f0b132 100644
--- a/drivers/gpu/drm/imx/imx-tve.c
+++ b/drivers/gpu/drm/imx/imx-tve.c
@@ -360,24 +360,24 @@ static void imx_tve_encoder_disable(struct drm_encoder *encoder)
 	tve_disable(tve);
 }
 
-static struct drm_connector_funcs imx_tve_connector_funcs = {
+static const struct drm_connector_funcs imx_tve_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = imx_tve_connector_detect,
 	.destroy = imx_drm_connector_destroy,
 };
 
-static struct drm_connector_helper_funcs imx_tve_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs imx_tve_connector_helper_funcs = {
 	.get_modes = imx_tve_connector_get_modes,
 	.best_encoder = imx_tve_connector_best_encoder,
 	.mode_valid = imx_tve_connector_mode_valid,
 };
 
-static struct drm_encoder_funcs imx_tve_encoder_funcs = {
+static const struct drm_encoder_funcs imx_tve_encoder_funcs = {
 	.destroy = imx_drm_encoder_destroy,
 };
 
-static struct drm_encoder_helper_funcs imx_tve_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs imx_tve_encoder_helper_funcs = {
 	.dpms = imx_tve_encoder_dpms,
 	.mode_fixup = imx_tve_encoder_mode_fixup,
 	.prepare = imx_tve_encoder_prepare,
@@ -508,7 +508,7 @@ static int imx_tve_register(struct drm_device *drm, struct imx_tve *tve)
 
 	drm_encoder_helper_add(&tve->encoder, &imx_tve_encoder_helper_funcs);
 	drm_encoder_init(drm, &tve->encoder, &imx_tve_encoder_funcs,
-			 encoder_type);
+			 encoder_type, NULL);
 
 	drm_connector_helper_add(&tve->connector,
 			&imx_tve_connector_helper_funcs);
diff --git a/drivers/gpu/drm/imx/ipuv3-crtc.c b/drivers/gpu/drm/imx/ipuv3-crtc.c
index 4ab841eebee1..287226311413 100644
--- a/drivers/gpu/drm/imx/ipuv3-crtc.c
+++ b/drivers/gpu/drm/imx/ipuv3-crtc.c
@@ -64,6 +64,7 @@ static void ipu_fb_enable(struct ipu_crtc *ipu_crtc)
 	/* Start DC channel and DI after IDMAC */
 	ipu_dc_enable_channel(ipu_crtc->dc);
 	ipu_di_enable(ipu_crtc->di);
+	drm_crtc_vblank_on(&ipu_crtc->base);
 
 	ipu_crtc->enabled = 1;
 }
@@ -80,6 +81,7 @@ static void ipu_fb_disable(struct ipu_crtc *ipu_crtc)
 	ipu_di_disable(ipu_crtc->di);
 	ipu_plane_disable(ipu_crtc->plane[0]);
 	ipu_dc_disable(ipu);
+	drm_crtc_vblank_off(&ipu_crtc->base);
 
 	ipu_crtc->enabled = 0;
 }
@@ -270,7 +272,7 @@ static void ipu_crtc_commit(struct drm_crtc *crtc)
 	ipu_fb_enable(ipu_crtc);
 }
 
-static struct drm_crtc_helper_funcs ipu_helper_funcs = {
+static const struct drm_crtc_helper_funcs ipu_helper_funcs = {
 	.dpms = ipu_crtc_dpms,
 	.mode_fixup = ipu_crtc_mode_fixup,
 	.mode_set = ipu_crtc_mode_set,
diff --git a/drivers/gpu/drm/imx/ipuv3-plane.c b/drivers/gpu/drm/imx/ipuv3-plane.c
index e2ff410bab74..26bb1b626fe3 100644
--- a/drivers/gpu/drm/imx/ipuv3-plane.c
+++ b/drivers/gpu/drm/imx/ipuv3-plane.c
@@ -42,6 +42,7 @@ static const uint32_t ipu_plane_formats[] = {
 	DRM_FORMAT_YVYU,
 	DRM_FORMAT_YUV420,
 	DRM_FORMAT_YVU420,
+	DRM_FORMAT_RGB565,
 };
 
 int ipu_plane_irq(struct ipu_plane *ipu_plane)
@@ -401,7 +402,8 @@ struct ipu_plane *ipu_plane_init(struct drm_device *dev, struct ipu_soc *ipu,
 
 	ret = drm_universal_plane_init(dev, &ipu_plane->base, possible_crtcs,
 				       &ipu_plane_funcs, ipu_plane_formats,
-				       ARRAY_SIZE(ipu_plane_formats), type);
+				       ARRAY_SIZE(ipu_plane_formats), type,
+				       NULL);
 	if (ret) {
 		DRM_ERROR("failed to initialize plane\n");
 		kfree(ipu_plane);
diff --git a/drivers/gpu/drm/imx/parallel-display.c b/drivers/gpu/drm/imx/parallel-display.c
index 2e9b9f1b5cd2..0ffef172afb4 100644
--- a/drivers/gpu/drm/imx/parallel-display.c
+++ b/drivers/gpu/drm/imx/parallel-display.c
@@ -148,23 +148,23 @@ static void imx_pd_encoder_disable(struct drm_encoder *encoder)
 	drm_panel_unprepare(imxpd->panel);
 }
 
-static struct drm_connector_funcs imx_pd_connector_funcs = {
+static const struct drm_connector_funcs imx_pd_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.detect = imx_pd_connector_detect,
 	.destroy = imx_drm_connector_destroy,
 };
 
-static struct drm_connector_helper_funcs imx_pd_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs imx_pd_connector_helper_funcs = {
 	.get_modes = imx_pd_connector_get_modes,
 	.best_encoder = imx_pd_connector_best_encoder,
 };
 
-static struct drm_encoder_funcs imx_pd_encoder_funcs = {
+static const struct drm_encoder_funcs imx_pd_encoder_funcs = {
 	.destroy = imx_drm_encoder_destroy,
 };
 
-static struct drm_encoder_helper_funcs imx_pd_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs imx_pd_encoder_helper_funcs = {
 	.dpms = imx_pd_encoder_dpms,
 	.mode_fixup = imx_pd_encoder_mode_fixup,
 	.prepare = imx_pd_encoder_prepare,
@@ -192,7 +192,7 @@ static int imx_pd_register(struct drm_device *drm,
 
 	drm_encoder_helper_add(&imxpd->encoder, &imx_pd_encoder_helper_funcs);
 	drm_encoder_init(drm, &imxpd->encoder, &imx_pd_encoder_funcs,
-			 DRM_MODE_ENCODER_NONE);
+			 DRM_MODE_ENCODER_NONE, NULL);
 
 	drm_connector_helper_add(&imxpd->connector,
 			&imx_pd_connector_helper_funcs);
@@ -204,8 +204,6 @@ static int imx_pd_register(struct drm_device *drm,
 
 	drm_mode_connector_attach_encoder(&imxpd->connector, &imxpd->encoder);
 
-	imxpd->connector.encoder = &imxpd->encoder;
-
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.h b/drivers/gpu/drm/mgag200/mgag200_drv.h
index 912151c36d59..205b2801d3b8 100644
--- a/drivers/gpu/drm/mgag200/mgag200_drv.h
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.h
@@ -252,7 +252,7 @@ void mgag200_fbdev_fini(struct mga_device *mdev);
 				/* mgag200_main.c */
 int mgag200_framebuffer_init(struct drm_device *dev,
 			     struct mga_framebuffer *mfb,
-			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     const struct drm_mode_fb_cmd2 *mode_cmd,
 			     struct drm_gem_object *obj);
 
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
index b35b5b2db4ec..d9b04b008feb 100644
--- a/drivers/gpu/drm/mgag200/mgag200_fb.c
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -138,7 +138,7 @@ static struct fb_ops mgag200fb_ops = {
 };
 
 static int mgag200fb_create_object(struct mga_fbdev *afbdev,
-				   struct drm_mode_fb_cmd2 *mode_cmd,
+				   const struct drm_mode_fb_cmd2 *mode_cmd,
 				   struct drm_gem_object **gobj_p)
 {
 	struct drm_device *dev = afbdev->helper.dev;
diff --git a/drivers/gpu/drm/mgag200/mgag200_main.c b/drivers/gpu/drm/mgag200/mgag200_main.c
index b1a0f5656175..9147444d5bf2 100644
--- a/drivers/gpu/drm/mgag200/mgag200_main.c
+++ b/drivers/gpu/drm/mgag200/mgag200_main.c
@@ -29,7 +29,7 @@ static const struct drm_framebuffer_funcs mga_fb_funcs = {
 
 int mgag200_framebuffer_init(struct drm_device *dev,
 			     struct mga_framebuffer *gfb,
-			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     const struct drm_mode_fb_cmd2 *mode_cmd,
 			     struct drm_gem_object *obj)
 {
 	int ret;
@@ -47,7 +47,7 @@ int mgag200_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 mgag200_user_framebuffer_create(struct drm_device *dev,
 				struct drm_file *filp,
-				struct drm_mode_fb_cmd2 *mode_cmd)
+				const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct mga_framebuffer *mga_fb;
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index c99d3fe12881..dc13c4857e6f 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1538,7 +1538,7 @@ static struct drm_encoder *mga_encoder_init(struct drm_device *dev)
 	encoder->possible_crtcs = 0x1;
 
 	drm_encoder_init(dev, encoder, &mga_encoder_encoder_funcs,
-			 DRM_MODE_ENCODER_DAC);
+			 DRM_MODE_ENCODER_DAC, NULL);
 	drm_encoder_helper_add(encoder, &mga_encoder_helper_funcs);
 
 	return encoder;
@@ -1564,7 +1564,7 @@ static uint32_t mga_vga_calculate_mode_bandwidth(struct drm_display_mode *mode,
 							int bits_per_pixel)
 {
 	uint32_t total_area, divisor;
-	int64_t active_area, pixels_per_second, bandwidth;
+	uint64_t active_area, pixels_per_second, bandwidth;
 	uint64_t bytes_per_pixel = (bits_per_pixel + 7) / 8;
 
 	divisor = 1024;
@@ -1684,13 +1684,13 @@ static void mga_connector_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
-struct drm_connector_helper_funcs mga_vga_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs mga_vga_connector_helper_funcs = {
 	.get_modes = mga_vga_get_modes,
 	.mode_valid = mga_vga_mode_valid,
 	.best_encoder = mga_connector_best_encoder,
 };
 
-struct drm_connector_funcs mga_vga_connector_funcs = {
+static const struct drm_connector_funcs mga_vga_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = mga_vga_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
diff --git a/drivers/gpu/drm/msm/Kconfig b/drivers/gpu/drm/msm/Kconfig
index 84d3ec98e6b9..215495c2780c 100644
--- a/drivers/gpu/drm/msm/Kconfig
+++ b/drivers/gpu/drm/msm/Kconfig
@@ -54,3 +54,11 @@ config DRM_MSM_DSI_20NM_PHY
 	default y
 	help
 	  Choose this option if the 20nm DSI PHY is used on the platform.
+
+config DRM_MSM_DSI_28NM_8960_PHY
+	bool "Enable DSI 28nm 8960 PHY driver in MSM DRM"
+	depends on DRM_MSM_DSI
+	default y
+	help
+	  Choose this option if the 28nm DSI PHY 8960 variant is used on the
+	  platform.
diff --git a/drivers/gpu/drm/msm/Makefile b/drivers/gpu/drm/msm/Makefile
index 1c90290be716..065ad4138799 100644
--- a/drivers/gpu/drm/msm/Makefile
+++ b/drivers/gpu/drm/msm/Makefile
@@ -54,6 +54,7 @@ msm-$(CONFIG_DRM_FBDEV_EMULATION) += msm_fbdev.o
 msm-$(CONFIG_COMMON_CLK) += mdp/mdp4/mdp4_lvds_pll.o
 
 msm-$(CONFIG_DRM_MSM_DSI) += dsi/dsi.o \
+			mdp/mdp4/mdp4_dsi_encoder.o \
 			dsi/dsi_cfg.o \
 			dsi/dsi_host.o \
 			dsi/dsi_manager.o \
@@ -62,10 +63,12 @@ msm-$(CONFIG_DRM_MSM_DSI) += dsi/dsi.o \
 
 msm-$(CONFIG_DRM_MSM_DSI_28NM_PHY) += dsi/phy/dsi_phy_28nm.o
 msm-$(CONFIG_DRM_MSM_DSI_20NM_PHY) += dsi/phy/dsi_phy_20nm.o
+msm-$(CONFIG_DRM_MSM_DSI_28NM_8960_PHY) += dsi/phy/dsi_phy_28nm_8960.o
 
 ifeq ($(CONFIG_DRM_MSM_DSI_PLL),y)
 msm-y += dsi/pll/dsi_pll.o
 msm-$(CONFIG_DRM_MSM_DSI_28NM_PHY) += dsi/pll/dsi_pll_28nm.o
+msm-$(CONFIG_DRM_MSM_DSI_28NM_8960_PHY) += dsi/pll/dsi_pll_28nm_8960.o
 endif
 
 obj-$(CONFIG_DRM_MSM)	+= msm.o
diff --git a/drivers/gpu/drm/msm/adreno/adreno_device.c b/drivers/gpu/drm/msm/adreno/adreno_device.c
index 1ea2df524fac..950d27d26b30 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_device.c
+++ b/drivers/gpu/drm/msm/adreno/adreno_device.c
@@ -19,10 +19,6 @@
 
 #include "adreno_gpu.h"
 
-#if defined(DOWNSTREAM_CONFIG_MSM_BUS_SCALING) && !defined(CONFIG_OF)
-#  include <mach/kgsl.h>
-#endif
-
 #define ANY_ID 0xff
 
 bool hang_debug = false;
@@ -168,7 +164,6 @@ static void set_gpu_pdev(struct drm_device *dev,
 static int adreno_bind(struct device *dev, struct device *master, void *data)
 {
 	static struct adreno_platform_config config = {};
-#ifdef CONFIG_OF
 	struct device_node *child, *node = dev->of_node;
 	u32 val;
 	int ret;
@@ -205,53 +200,6 @@ static int adreno_bind(struct device *dev, struct device *master, void *data)
 		return -ENXIO;
 	}
 
-#else
-	struct kgsl_device_platform_data *pdata = dev->platform_data;
-	uint32_t version = socinfo_get_version();
-	if (cpu_is_apq8064ab()) {
-		config.fast_rate = 450000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 4;
-		config.rev = ADRENO_REV(3, 2, 1, 0);
-	} else if (cpu_is_apq8064()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 4;
-
-		if (SOCINFO_VERSION_MAJOR(version) == 2)
-			config.rev = ADRENO_REV(3, 2, 0, 2);
-		else if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
-				(SOCINFO_VERSION_MINOR(version) == 1))
-			config.rev = ADRENO_REV(3, 2, 0, 1);
-		else
-			config.rev = ADRENO_REV(3, 2, 0, 0);
-
-	} else if (cpu_is_msm8960ab()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 320000000;
-		config.bus_freq  = 4;
-
-		if (SOCINFO_VERSION_MINOR(version) == 0)
-			config.rev = ADRENO_REV(3, 2, 1, 0);
-		else
-			config.rev = ADRENO_REV(3, 2, 1, 1);
-
-	} else if (cpu_is_msm8930()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 3;
-
-		if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
-			(SOCINFO_VERSION_MINOR(version) == 2))
-			config.rev = ADRENO_REV(3, 0, 5, 2);
-		else
-			config.rev = ADRENO_REV(3, 0, 5, 0);
-
-	}
-#  ifdef DOWNSTREAM_CONFIG_MSM_BUS_SCALING
-	config.bus_scale_table = pdata->bus_scale_table;
-#  endif
-#endif
 	dev->platform_data = &config;
 	set_gpu_pdev(dev_get_drvdata(master), to_platform_device(dev));
 	return 0;
diff --git a/drivers/gpu/drm/msm/dsi/dsi.h b/drivers/gpu/drm/msm/dsi/dsi.h
index 5f5a3732cdf6..749fbb28ec3d 100644
--- a/drivers/gpu/drm/msm/dsi/dsi.h
+++ b/drivers/gpu/drm/msm/dsi/dsi.h
@@ -31,10 +31,12 @@ enum msm_dsi_phy_type {
 	MSM_DSI_PHY_28NM_HPM,
 	MSM_DSI_PHY_28NM_LP,
 	MSM_DSI_PHY_20NM,
+	MSM_DSI_PHY_28NM_8960,
 	MSM_DSI_PHY_MAX
 };
 
 #define DSI_DEV_REGULATOR_MAX	8
+#define DSI_BUS_CLK_MAX		4
 
 /* Regulators for DSI devices */
 struct dsi_reg_entry {
@@ -89,7 +91,7 @@ int msm_dsi_manager_phy_enable(int id,
 		u32 *clk_pre, u32 *clk_post);
 void msm_dsi_manager_phy_disable(int id);
 int msm_dsi_manager_cmd_xfer(int id, const struct mipi_dsi_msg *msg);
-bool msm_dsi_manager_cmd_xfer_trigger(int id, u32 iova, u32 len);
+bool msm_dsi_manager_cmd_xfer_trigger(int id, u32 dma_base, u32 len);
 int msm_dsi_manager_register(struct msm_dsi *msm_dsi);
 void msm_dsi_manager_unregister(struct msm_dsi *msm_dsi);
 
@@ -143,7 +145,7 @@ int msm_dsi_host_cmd_tx(struct mipi_dsi_host *host,
 int msm_dsi_host_cmd_rx(struct mipi_dsi_host *host,
 					const struct mipi_dsi_msg *msg);
 void msm_dsi_host_cmd_xfer_commit(struct mipi_dsi_host *host,
-					u32 iova, u32 len);
+					u32 dma_base, u32 len);
 int msm_dsi_host_enable(struct mipi_dsi_host *host);
 int msm_dsi_host_disable(struct mipi_dsi_host *host);
 int msm_dsi_host_power_on(struct mipi_dsi_host *host);
diff --git a/drivers/gpu/drm/msm/dsi/dsi_cfg.c b/drivers/gpu/drm/msm/dsi/dsi_cfg.c
index 5872d5e5934f..2a827d8093a2 100644
--- a/drivers/gpu/drm/msm/dsi/dsi_cfg.c
+++ b/drivers/gpu/drm/msm/dsi/dsi_cfg.c
@@ -13,9 +13,26 @@
 
 #include "dsi_cfg.h"
 
-/* DSI v2 has not been supported by now */
-static const struct msm_dsi_config dsi_v2_cfg = {
+static const char * const dsi_v2_bus_clk_names[] = {
+	"core_mmss_clk", "iface_clk", "bus_clk",
+};
+
+static const struct msm_dsi_config apq8064_dsi_cfg = {
 	.io_offset = 0,
+	.reg_cfg = {
+		.num = 3,
+		.regs = {
+			{"vdda", 1200000, 1200000, 100000, 100},
+			{"avdd", 3000000, 3000000, 110000, 100},
+			{"vddio", 1800000, 1800000, 100000, 100},
+		},
+	},
+	.bus_clk_names = dsi_v2_bus_clk_names,
+	.num_bus_clks = ARRAY_SIZE(dsi_v2_bus_clk_names),
+};
+
+static const char * const dsi_6g_bus_clk_names[] = {
+	"mdp_core_clk", "iface_clk", "bus_clk", "core_mmss_clk",
 };
 
 static const struct msm_dsi_config msm8974_apq8084_dsi_cfg = {
@@ -29,6 +46,12 @@ static const struct msm_dsi_config msm8974_apq8084_dsi_cfg = {
 			{"vddio", 1800000, 1800000, 100000, 100},
 		},
 	},
+	.bus_clk_names = dsi_6g_bus_clk_names,
+	.num_bus_clks = ARRAY_SIZE(dsi_6g_bus_clk_names),
+};
+
+static const char * const dsi_8916_bus_clk_names[] = {
+	"mdp_core_clk", "iface_clk", "bus_clk",
 };
 
 static const struct msm_dsi_config msm8916_dsi_cfg = {
@@ -42,6 +65,8 @@ static const struct msm_dsi_config msm8916_dsi_cfg = {
 			{"vddio", 1800000, 1800000, 100000, 100},
 		},
 	},
+	.bus_clk_names = dsi_8916_bus_clk_names,
+	.num_bus_clks = ARRAY_SIZE(dsi_8916_bus_clk_names),
 };
 
 static const struct msm_dsi_config msm8994_dsi_cfg = {
@@ -57,11 +82,13 @@ static const struct msm_dsi_config msm8994_dsi_cfg = {
 			{"lab_reg", -1, -1, -1, -1},
 			{"ibb_reg", -1, -1, -1, -1},
 		},
-	}
+	},
+	.bus_clk_names = dsi_6g_bus_clk_names,
+	.num_bus_clks = ARRAY_SIZE(dsi_6g_bus_clk_names),
 };
 
 static const struct msm_dsi_cfg_handler dsi_cfg_handlers[] = {
-	{MSM_DSI_VER_MAJOR_V2, U32_MAX, &dsi_v2_cfg},
+	{MSM_DSI_VER_MAJOR_V2, MSM_DSI_V2_VER_MINOR_8064, &apq8064_dsi_cfg},
 	{MSM_DSI_VER_MAJOR_6G, MSM_DSI_6G_VER_MINOR_V1_0,
 						&msm8974_apq8084_dsi_cfg},
 	{MSM_DSI_VER_MAJOR_6G, MSM_DSI_6G_VER_MINOR_V1_1,
diff --git a/drivers/gpu/drm/msm/dsi/dsi_cfg.h b/drivers/gpu/drm/msm/dsi/dsi_cfg.h
index 4cf887240177..a68c836744a3 100644
--- a/drivers/gpu/drm/msm/dsi/dsi_cfg.h
+++ b/drivers/gpu/drm/msm/dsi/dsi_cfg.h
@@ -25,11 +25,15 @@
 #define MSM_DSI_6G_VER_MINOR_V1_3	0x10030000
 #define MSM_DSI_6G_VER_MINOR_V1_3_1	0x10030001
 
+#define MSM_DSI_V2_VER_MINOR_8064	0x0
+
 #define DSI_6G_REG_SHIFT	4
 
 struct msm_dsi_config {
 	u32 io_offset;
 	struct dsi_reg_config reg_cfg;
+	const char * const *bus_clk_names;
+	const int num_bus_clks;
 };
 
 struct msm_dsi_cfg_handler {
diff --git a/drivers/gpu/drm/msm/dsi/dsi_host.c b/drivers/gpu/drm/msm/dsi/dsi_host.c
index 4c49868efcda..48f9967b4a1b 100644
--- a/drivers/gpu/drm/msm/dsi/dsi_host.c
+++ b/drivers/gpu/drm/msm/dsi/dsi_host.c
@@ -24,26 +24,36 @@
 #include <linux/of_graph.h>
 #include <linux/regulator/consumer.h>
 #include <linux/spinlock.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 #include <video/mipi_display.h>
 
 #include "dsi.h"
 #include "dsi.xml.h"
+#include "sfpb.xml.h"
 #include "dsi_cfg.h"
 
 static int dsi_get_version(const void __iomem *base, u32 *major, u32 *minor)
 {
 	u32 ver;
-	u32 ver_6g;
 
 	if (!major || !minor)
 		return -EINVAL;
 
-	/* From DSI6G(v3), addition of a 6G_HW_VERSION register at offset 0
+	/*
+	 * From DSI6G(v3), addition of a 6G_HW_VERSION register at offset 0
 	 * makes all other registers 4-byte shifted down.
+	 *
+	 * In order to identify between DSI6G(v3) and beyond, and DSIv2 and
+	 * older, we read the DSI_VERSION register without any shift(offset
+	 * 0x1f0). In the case of DSIv2, this hast to be a non-zero value. In
+	 * the case of DSI6G, this has to be zero (the offset points to a
+	 * scratch register which we never touch)
 	 */
-	ver_6g = msm_readl(base + REG_DSI_6G_HW_VERSION);
-	if (ver_6g == 0) {
-		ver = msm_readl(base + REG_DSI_VERSION);
+
+	ver = msm_readl(base + REG_DSI_VERSION);
+	if (ver) {
+		/* older dsi host, there is no register shift */
 		ver = FIELD(ver, DSI_VERSION_MAJOR);
 		if (ver <= MSM_DSI_VER_MAJOR_V2) {
 			/* old versions */
@@ -54,12 +64,17 @@ static int dsi_get_version(const void __iomem *base, u32 *major, u32 *minor)
 			return -EINVAL;
 		}
 	} else {
+		/*
+		 * newer host, offset 0 has 6G_HW_VERSION, the rest of the
+		 * registers are shifted down, read DSI_VERSION again with
+		 * the shifted offset
+		 */
 		ver = msm_readl(base + DSI_6G_REG_SHIFT + REG_DSI_VERSION);
 		ver = FIELD(ver, DSI_VERSION_MAJOR);
 		if (ver == MSM_DSI_VER_MAJOR_6G) {
 			/* 6G version */
 			*major = ver;
-			*minor = ver_6g;
+			*minor = msm_readl(base + REG_DSI_6G_HW_VERSION);
 			return 0;
 		} else {
 			return -EINVAL;
@@ -91,10 +106,9 @@ struct msm_dsi_host {
 
 	void __iomem *ctrl_base;
 	struct regulator_bulk_data supplies[DSI_DEV_REGULATOR_MAX];
-	struct clk *mdp_core_clk;
-	struct clk *ahb_clk;
-	struct clk *axi_clk;
-	struct clk *mmss_misc_ahb_clk;
+
+	struct clk *bus_clks[DSI_BUS_CLK_MAX];
+
 	struct clk *byte_clk;
 	struct clk *esc_clk;
 	struct clk *pixel_clk;
@@ -102,6 +116,14 @@ struct msm_dsi_host {
 	struct clk *pixel_clk_src;
 
 	u32 byte_clk_rate;
+	u32 esc_clk_rate;
+
+	/* DSI v2 specific clocks */
+	struct clk *src_clk;
+	struct clk *esc_clk_src;
+	struct clk *dsi_clk_src;
+
+	u32 src_clk_rate;
 
 	struct gpio_desc *disp_en_gpio;
 	struct gpio_desc *te_gpio;
@@ -119,9 +141,19 @@ struct msm_dsi_host {
 	struct work_struct err_work;
 	struct workqueue_struct *workqueue;
 
+	/* DSI 6G TX buffer*/
 	struct drm_gem_object *tx_gem_obj;
+
+	/* DSI v2 TX buffer */
+	void *tx_buf;
+	dma_addr_t tx_buf_paddr;
+
+	int tx_size;
+
 	u8 *rx_buf;
 
+	struct regmap *sfpb;
+
 	struct drm_display_mode *mode;
 
 	/* connected device info */
@@ -165,21 +197,31 @@ static const struct msm_dsi_cfg_handler *dsi_get_config(
 						struct msm_dsi_host *msm_host)
 {
 	const struct msm_dsi_cfg_handler *cfg_hnd = NULL;
+	struct device *dev = &msm_host->pdev->dev;
 	struct regulator *gdsc_reg;
+	struct clk *ahb_clk;
 	int ret;
 	u32 major = 0, minor = 0;
 
-	gdsc_reg = regulator_get(&msm_host->pdev->dev, "gdsc");
+	gdsc_reg = regulator_get(dev, "gdsc");
 	if (IS_ERR(gdsc_reg)) {
 		pr_err("%s: cannot get gdsc\n", __func__);
 		goto exit;
 	}
+
+	ahb_clk = clk_get(dev, "iface_clk");
+	if (IS_ERR(ahb_clk)) {
+		pr_err("%s: cannot get interface clock\n", __func__);
+		goto put_gdsc;
+	}
+
 	ret = regulator_enable(gdsc_reg);
 	if (ret) {
 		pr_err("%s: unable to enable gdsc\n", __func__);
-		goto put_gdsc;
+		goto put_clk;
 	}
-	ret = clk_prepare_enable(msm_host->ahb_clk);
+
+	ret = clk_prepare_enable(ahb_clk);
 	if (ret) {
 		pr_err("%s: unable to enable ahb_clk\n", __func__);
 		goto disable_gdsc;
@@ -196,9 +238,11 @@ static const struct msm_dsi_cfg_handler *dsi_get_config(
 	DBG("%s: Version %x:%x\n", __func__, major, minor);
 
 disable_clks:
-	clk_disable_unprepare(msm_host->ahb_clk);
+	clk_disable_unprepare(ahb_clk);
 disable_gdsc:
 	regulator_disable(gdsc_reg);
+put_clk:
+	clk_put(ahb_clk);
 put_gdsc:
 	regulator_put(gdsc_reg);
 exit:
@@ -295,40 +339,23 @@ static int dsi_regulator_init(struct msm_dsi_host *msm_host)
 static int dsi_clk_init(struct msm_dsi_host *msm_host)
 {
 	struct device *dev = &msm_host->pdev->dev;
-	int ret = 0;
-
-	msm_host->mdp_core_clk = devm_clk_get(dev, "mdp_core_clk");
-	if (IS_ERR(msm_host->mdp_core_clk)) {
-		ret = PTR_ERR(msm_host->mdp_core_clk);
-		pr_err("%s: Unable to get mdp core clk. ret=%d\n",
-			__func__, ret);
-		goto exit;
-	}
-
-	msm_host->ahb_clk = devm_clk_get(dev, "iface_clk");
-	if (IS_ERR(msm_host->ahb_clk)) {
-		ret = PTR_ERR(msm_host->ahb_clk);
-		pr_err("%s: Unable to get mdss ahb clk. ret=%d\n",
-			__func__, ret);
-		goto exit;
-	}
-
-	msm_host->axi_clk = devm_clk_get(dev, "bus_clk");
-	if (IS_ERR(msm_host->axi_clk)) {
-		ret = PTR_ERR(msm_host->axi_clk);
-		pr_err("%s: Unable to get axi bus clk. ret=%d\n",
-			__func__, ret);
-		goto exit;
-	}
-
-	msm_host->mmss_misc_ahb_clk = devm_clk_get(dev, "core_mmss_clk");
-	if (IS_ERR(msm_host->mmss_misc_ahb_clk)) {
-		ret = PTR_ERR(msm_host->mmss_misc_ahb_clk);
-		pr_err("%s: Unable to get mmss misc ahb clk. ret=%d\n",
-			__func__, ret);
-		goto exit;
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
+	const struct msm_dsi_config *cfg = cfg_hnd->cfg;
+	int i, ret = 0;
+
+	/* get bus clocks */
+	for (i = 0; i < cfg->num_bus_clks; i++) {
+		msm_host->bus_clks[i] = devm_clk_get(dev,
+						cfg->bus_clk_names[i]);
+		if (IS_ERR(msm_host->bus_clks[i])) {
+			ret = PTR_ERR(msm_host->bus_clks[i]);
+			pr_err("%s: Unable to get %s, ret = %d\n",
+				__func__, cfg->bus_clk_names[i], ret);
+			goto exit;
+		}
 	}
 
+	/* get link and source clocks */
 	msm_host->byte_clk = devm_clk_get(dev, "byte_clk");
 	if (IS_ERR(msm_host->byte_clk)) {
 		ret = PTR_ERR(msm_host->byte_clk);
@@ -356,80 +383,85 @@ static int dsi_clk_init(struct msm_dsi_host *msm_host)
 		goto exit;
 	}
 
-	msm_host->byte_clk_src = devm_clk_get(dev, "byte_clk_src");
-	if (IS_ERR(msm_host->byte_clk_src)) {
-		ret = PTR_ERR(msm_host->byte_clk_src);
+	msm_host->byte_clk_src = clk_get_parent(msm_host->byte_clk);
+	if (!msm_host->byte_clk_src) {
+		ret = -ENODEV;
 		pr_err("%s: can't find byte_clk_src. ret=%d\n", __func__, ret);
-		msm_host->byte_clk_src = NULL;
 		goto exit;
 	}
 
-	msm_host->pixel_clk_src = devm_clk_get(dev, "pixel_clk_src");
-	if (IS_ERR(msm_host->pixel_clk_src)) {
-		ret = PTR_ERR(msm_host->pixel_clk_src);
+	msm_host->pixel_clk_src = clk_get_parent(msm_host->pixel_clk);
+	if (!msm_host->pixel_clk_src) {
+		ret = -ENODEV;
 		pr_err("%s: can't find pixel_clk_src. ret=%d\n", __func__, ret);
-		msm_host->pixel_clk_src = NULL;
 		goto exit;
 	}
 
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_V2) {
+		msm_host->src_clk = devm_clk_get(dev, "src_clk");
+		if (IS_ERR(msm_host->src_clk)) {
+			ret = PTR_ERR(msm_host->src_clk);
+			pr_err("%s: can't find dsi_src_clk. ret=%d\n",
+				__func__, ret);
+			msm_host->src_clk = NULL;
+			goto exit;
+		}
+
+		msm_host->esc_clk_src = clk_get_parent(msm_host->esc_clk);
+		if (!msm_host->esc_clk_src) {
+			ret = -ENODEV;
+			pr_err("%s: can't get esc_clk_src. ret=%d\n",
+				__func__, ret);
+			goto exit;
+		}
+
+		msm_host->dsi_clk_src = clk_get_parent(msm_host->src_clk);
+		if (!msm_host->dsi_clk_src) {
+			ret = -ENODEV;
+			pr_err("%s: can't get dsi_clk_src. ret=%d\n",
+				__func__, ret);
+		}
+	}
 exit:
 	return ret;
 }
 
 static int dsi_bus_clk_enable(struct msm_dsi_host *msm_host)
 {
-	int ret;
+	const struct msm_dsi_config *cfg = msm_host->cfg_hnd->cfg;
+	int i, ret;
 
 	DBG("id=%d", msm_host->id);
 
-	ret = clk_prepare_enable(msm_host->mdp_core_clk);
-	if (ret) {
-		pr_err("%s: failed to enable mdp_core_clock, %d\n",
-							 __func__, ret);
-		goto core_clk_err;
-	}
-
-	ret = clk_prepare_enable(msm_host->ahb_clk);
-	if (ret) {
-		pr_err("%s: failed to enable ahb clock, %d\n", __func__, ret);
-		goto ahb_clk_err;
-	}
-
-	ret = clk_prepare_enable(msm_host->axi_clk);
-	if (ret) {
-		pr_err("%s: failed to enable ahb clock, %d\n", __func__, ret);
-		goto axi_clk_err;
-	}
-
-	ret = clk_prepare_enable(msm_host->mmss_misc_ahb_clk);
-	if (ret) {
-		pr_err("%s: failed to enable mmss misc ahb clk, %d\n",
-			__func__, ret);
-		goto misc_ahb_clk_err;
+	for (i = 0; i < cfg->num_bus_clks; i++) {
+		ret = clk_prepare_enable(msm_host->bus_clks[i]);
+		if (ret) {
+			pr_err("%s: failed to enable bus clock %d ret %d\n",
+				__func__, i, ret);
+			goto err;
+		}
 	}
 
 	return 0;
+err:
+	for (; i > 0; i--)
+		clk_disable_unprepare(msm_host->bus_clks[i]);
 
-misc_ahb_clk_err:
-	clk_disable_unprepare(msm_host->axi_clk);
-axi_clk_err:
-	clk_disable_unprepare(msm_host->ahb_clk);
-ahb_clk_err:
-	clk_disable_unprepare(msm_host->mdp_core_clk);
-core_clk_err:
 	return ret;
 }
 
 static void dsi_bus_clk_disable(struct msm_dsi_host *msm_host)
 {
+	const struct msm_dsi_config *cfg = msm_host->cfg_hnd->cfg;
+	int i;
+
 	DBG("");
-	clk_disable_unprepare(msm_host->mmss_misc_ahb_clk);
-	clk_disable_unprepare(msm_host->axi_clk);
-	clk_disable_unprepare(msm_host->ahb_clk);
-	clk_disable_unprepare(msm_host->mdp_core_clk);
+
+	for (i = cfg->num_bus_clks - 1; i >= 0; i--)
+		clk_disable_unprepare(msm_host->bus_clks[i]);
 }
 
-static int dsi_link_clk_enable(struct msm_dsi_host *msm_host)
+static int dsi_link_clk_enable_6g(struct msm_dsi_host *msm_host)
 {
 	int ret;
 
@@ -476,11 +508,98 @@ error:
 	return ret;
 }
 
-static void dsi_link_clk_disable(struct msm_dsi_host *msm_host)
+static int dsi_link_clk_enable_v2(struct msm_dsi_host *msm_host)
 {
+	int ret;
+
+	DBG("Set clk rates: pclk=%d, byteclk=%d, esc_clk=%d, dsi_src_clk=%d",
+		msm_host->mode->clock, msm_host->byte_clk_rate,
+		msm_host->esc_clk_rate, msm_host->src_clk_rate);
+
+	ret = clk_set_rate(msm_host->byte_clk, msm_host->byte_clk_rate);
+	if (ret) {
+		pr_err("%s: Failed to set rate byte clk, %d\n", __func__, ret);
+		goto error;
+	}
+
+	ret = clk_set_rate(msm_host->esc_clk, msm_host->esc_clk_rate);
+	if (ret) {
+		pr_err("%s: Failed to set rate esc clk, %d\n", __func__, ret);
+		goto error;
+	}
+
+	ret = clk_set_rate(msm_host->src_clk, msm_host->src_clk_rate);
+	if (ret) {
+		pr_err("%s: Failed to set rate src clk, %d\n", __func__, ret);
+		goto error;
+	}
+
+	ret = clk_set_rate(msm_host->pixel_clk, msm_host->mode->clock * 1000);
+	if (ret) {
+		pr_err("%s: Failed to set rate pixel clk, %d\n", __func__, ret);
+		goto error;
+	}
+
+	ret = clk_prepare_enable(msm_host->byte_clk);
+	if (ret) {
+		pr_err("%s: Failed to enable dsi byte clk\n", __func__);
+		goto error;
+	}
+
+	ret = clk_prepare_enable(msm_host->esc_clk);
+	if (ret) {
+		pr_err("%s: Failed to enable dsi esc clk\n", __func__);
+		goto esc_clk_err;
+	}
+
+	ret = clk_prepare_enable(msm_host->src_clk);
+	if (ret) {
+		pr_err("%s: Failed to enable dsi src clk\n", __func__);
+		goto src_clk_err;
+	}
+
+	ret = clk_prepare_enable(msm_host->pixel_clk);
+	if (ret) {
+		pr_err("%s: Failed to enable dsi pixel clk\n", __func__);
+		goto pixel_clk_err;
+	}
+
+	return 0;
+
+pixel_clk_err:
+	clk_disable_unprepare(msm_host->src_clk);
+src_clk_err:
 	clk_disable_unprepare(msm_host->esc_clk);
-	clk_disable_unprepare(msm_host->pixel_clk);
+esc_clk_err:
 	clk_disable_unprepare(msm_host->byte_clk);
+error:
+	return ret;
+}
+
+static int dsi_link_clk_enable(struct msm_dsi_host *msm_host)
+{
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
+
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_6G)
+		return dsi_link_clk_enable_6g(msm_host);
+	else
+		return dsi_link_clk_enable_v2(msm_host);
+}
+
+static void dsi_link_clk_disable(struct msm_dsi_host *msm_host)
+{
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
+
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_6G) {
+		clk_disable_unprepare(msm_host->esc_clk);
+		clk_disable_unprepare(msm_host->pixel_clk);
+		clk_disable_unprepare(msm_host->byte_clk);
+	} else {
+		clk_disable_unprepare(msm_host->pixel_clk);
+		clk_disable_unprepare(msm_host->src_clk);
+		clk_disable_unprepare(msm_host->esc_clk);
+		clk_disable_unprepare(msm_host->byte_clk);
+	}
 }
 
 static int dsi_clk_ctrl(struct msm_dsi_host *msm_host, bool enable)
@@ -515,6 +634,7 @@ unlock_ret:
 static int dsi_calc_clk_rate(struct msm_dsi_host *msm_host)
 {
 	struct drm_display_mode *mode = msm_host->mode;
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
 	u8 lanes = msm_host->lanes;
 	u32 bpp = dsi_get_bpp(msm_host->format);
 	u32 pclk_rate;
@@ -534,6 +654,47 @@ static int dsi_calc_clk_rate(struct msm_dsi_host *msm_host)
 
 	DBG("pclk=%d, bclk=%d", pclk_rate, msm_host->byte_clk_rate);
 
+	msm_host->esc_clk_rate = clk_get_rate(msm_host->esc_clk);
+
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_V2) {
+		unsigned int esc_mhz, esc_div;
+		unsigned long byte_mhz;
+
+		msm_host->src_clk_rate = (pclk_rate * bpp) / 8;
+
+		/*
+		 * esc clock is byte clock followed by a 4 bit divider,
+		 * we need to find an escape clock frequency within the
+		 * mipi DSI spec range within the maximum divider limit
+		 * We iterate here between an escape clock frequencey
+		 * between 20 Mhz to 5 Mhz and pick up the first one
+		 * that can be supported by our divider
+		 */
+
+		byte_mhz = msm_host->byte_clk_rate / 1000000;
+
+		for (esc_mhz = 20; esc_mhz >= 5; esc_mhz--) {
+			esc_div = DIV_ROUND_UP(byte_mhz, esc_mhz);
+
+			/*
+			 * TODO: Ideally, we shouldn't know what sort of divider
+			 * is available in mmss_cc, we're just assuming that
+			 * it'll always be a 4 bit divider. Need to come up with
+			 * a better way here.
+			 */
+			if (esc_div >= 1 && esc_div <= 16)
+				break;
+		}
+
+		if (esc_mhz < 5)
+			return -EINVAL;
+
+		msm_host->esc_clk_rate = msm_host->byte_clk_rate / esc_div;
+
+		DBG("esc=%d, src=%d", msm_host->esc_clk_rate,
+			msm_host->src_clk_rate);
+	}
+
 	return 0;
 }
 
@@ -835,29 +996,46 @@ static void dsi_wait4video_eng_busy(struct msm_dsi_host *msm_host)
 static int dsi_tx_buf_alloc(struct msm_dsi_host *msm_host, int size)
 {
 	struct drm_device *dev = msm_host->dev;
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
 	int ret;
 	u32 iova;
 
-	mutex_lock(&dev->struct_mutex);
-	msm_host->tx_gem_obj = msm_gem_new(dev, size, MSM_BO_UNCACHED);
-	if (IS_ERR(msm_host->tx_gem_obj)) {
-		ret = PTR_ERR(msm_host->tx_gem_obj);
-		pr_err("%s: failed to allocate gem, %d\n", __func__, ret);
-		msm_host->tx_gem_obj = NULL;
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_6G) {
+		mutex_lock(&dev->struct_mutex);
+		msm_host->tx_gem_obj = msm_gem_new(dev, size, MSM_BO_UNCACHED);
+		if (IS_ERR(msm_host->tx_gem_obj)) {
+			ret = PTR_ERR(msm_host->tx_gem_obj);
+			pr_err("%s: failed to allocate gem, %d\n",
+				__func__, ret);
+			msm_host->tx_gem_obj = NULL;
+			mutex_unlock(&dev->struct_mutex);
+			return ret;
+		}
+
+		ret = msm_gem_get_iova_locked(msm_host->tx_gem_obj, 0, &iova);
 		mutex_unlock(&dev->struct_mutex);
-		return ret;
-	}
+		if (ret) {
+			pr_err("%s: failed to get iova, %d\n", __func__, ret);
+			return ret;
+		}
 
-	ret = msm_gem_get_iova_locked(msm_host->tx_gem_obj, 0, &iova);
-	if (ret) {
-		pr_err("%s: failed to get iova, %d\n", __func__, ret);
-		return ret;
-	}
-	mutex_unlock(&dev->struct_mutex);
+		if (iova & 0x07) {
+			pr_err("%s: buf NOT 8 bytes aligned\n", __func__);
+			return -EINVAL;
+		}
 
-	if (iova & 0x07) {
-		pr_err("%s: buf NOT 8 bytes aligned\n", __func__);
-		return -EINVAL;
+		msm_host->tx_size = msm_host->tx_gem_obj->size;
+	} else {
+		msm_host->tx_buf = dma_alloc_coherent(dev->dev, size,
+					&msm_host->tx_buf_paddr, GFP_KERNEL);
+		if (!msm_host->tx_buf) {
+			ret = -ENOMEM;
+			pr_err("%s: failed to allocate tx buf, %d\n",
+				__func__, ret);
+			return ret;
+		}
+
+		msm_host->tx_size = size;
 	}
 
 	return 0;
@@ -874,14 +1052,19 @@ static void dsi_tx_buf_free(struct msm_dsi_host *msm_host)
 		msm_host->tx_gem_obj = NULL;
 		mutex_unlock(&dev->struct_mutex);
 	}
+
+	if (msm_host->tx_buf)
+		dma_free_coherent(dev->dev, msm_host->tx_size, msm_host->tx_buf,
+			msm_host->tx_buf_paddr);
 }
 
 /*
  * prepare cmd buffer to be txed
  */
-static int dsi_cmd_dma_add(struct drm_gem_object *tx_gem,
-			const struct mipi_dsi_msg *msg)
+static int dsi_cmd_dma_add(struct msm_dsi_host *msm_host,
+			   const struct mipi_dsi_msg *msg)
 {
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
 	struct mipi_dsi_packet packet;
 	int len;
 	int ret;
@@ -894,17 +1077,20 @@ static int dsi_cmd_dma_add(struct drm_gem_object *tx_gem,
 	}
 	len = (packet.size + 3) & (~0x3);
 
-	if (len > tx_gem->size) {
+	if (len > msm_host->tx_size) {
 		pr_err("%s: packet size is too big\n", __func__);
 		return -EINVAL;
 	}
 
-	data = msm_gem_vaddr(tx_gem);
-
-	if (IS_ERR(data)) {
-		ret = PTR_ERR(data);
-		pr_err("%s: get vaddr failed, %d\n", __func__, ret);
-		return ret;
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_6G) {
+		data = msm_gem_vaddr(msm_host->tx_gem_obj);
+		if (IS_ERR(data)) {
+			ret = PTR_ERR(data);
+			pr_err("%s: get vaddr failed, %d\n", __func__, ret);
+			return ret;
+		}
+	} else {
+		data = msm_host->tx_buf;
 	}
 
 	/* MSM specific command format in memory */
@@ -970,17 +1156,21 @@ static int dsi_long_read_resp(u8 *buf, const struct mipi_dsi_msg *msg)
 	return msg->rx_len;
 }
 
-
 static int dsi_cmd_dma_tx(struct msm_dsi_host *msm_host, int len)
 {
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
 	int ret;
-	u32 iova;
+	u32 dma_base;
 	bool triggered;
 
-	ret = msm_gem_get_iova(msm_host->tx_gem_obj, 0, &iova);
-	if (ret) {
-		pr_err("%s: failed to get iova: %d\n", __func__, ret);
-		return ret;
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_6G) {
+		ret = msm_gem_get_iova(msm_host->tx_gem_obj, 0, &dma_base);
+		if (ret) {
+			pr_err("%s: failed to get iova: %d\n", __func__, ret);
+			return ret;
+		}
+	} else {
+		dma_base = msm_host->tx_buf_paddr;
 	}
 
 	reinit_completion(&msm_host->dma_comp);
@@ -988,7 +1178,7 @@ static int dsi_cmd_dma_tx(struct msm_dsi_host *msm_host, int len)
 	dsi_wait4video_eng_busy(msm_host);
 
 	triggered = msm_dsi_manager_cmd_xfer_trigger(
-						msm_host->id, iova, len);
+						msm_host->id, dma_base, len);
 	if (triggered) {
 		ret = wait_for_completion_timeout(&msm_host->dma_comp,
 					msecs_to_jiffies(200));
@@ -1060,7 +1250,7 @@ static int dsi_cmds2buf_tx(struct msm_dsi_host *msm_host,
 	int bllp_len = msm_host->mode->hdisplay *
 			dsi_get_bpp(msm_host->format) / 8;
 
-	len = dsi_cmd_dma_add(msm_host->tx_gem_obj, msg);
+	len = dsi_cmd_dma_add(msm_host, msg);
 	if (!len) {
 		pr_err("%s: failed to add cmd type = 0x%x\n",
 			__func__,  msg->type);
@@ -1383,6 +1573,16 @@ static int dsi_host_parse_dt(struct msm_dsi_host *msm_host)
 
 	msm_host->device_node = device_node;
 
+	if (of_property_read_bool(np, "syscon-sfpb")) {
+		msm_host->sfpb = syscon_regmap_lookup_by_phandle(np,
+					"syscon-sfpb");
+		if (IS_ERR(msm_host->sfpb)) {
+			dev_err(dev, "%s: failed to get sfpb regmap\n",
+				__func__);
+			return PTR_ERR(msm_host->sfpb);
+		}
+	}
+
 	return 0;
 }
 
@@ -1408,12 +1608,6 @@ int msm_dsi_host_init(struct msm_dsi *msm_dsi)
 		goto fail;
 	}
 
-	ret = dsi_clk_init(msm_host);
-	if (ret) {
-		pr_err("%s: unable to initialize dsi clks\n", __func__);
-		goto fail;
-	}
-
 	msm_host->ctrl_base = msm_ioremap(pdev, "dsi_ctrl", "DSI CTRL");
 	if (IS_ERR(msm_host->ctrl_base)) {
 		pr_err("%s: unable to map Dsi ctrl base\n", __func__);
@@ -1437,6 +1631,12 @@ int msm_dsi_host_init(struct msm_dsi *msm_dsi)
 		goto fail;
 	}
 
+	ret = dsi_clk_init(msm_host);
+	if (ret) {
+		pr_err("%s: unable to initialize dsi clks\n", __func__);
+		goto fail;
+	}
+
 	msm_host->rx_buf = devm_kzalloc(&pdev->dev, SZ_4K, GFP_KERNEL);
 	if (!msm_host->rx_buf) {
 		pr_err("%s: alloc rx temp buf failed\n", __func__);
@@ -1750,11 +1950,12 @@ int msm_dsi_host_cmd_rx(struct mipi_dsi_host *host,
 	return ret;
 }
 
-void msm_dsi_host_cmd_xfer_commit(struct mipi_dsi_host *host, u32 iova, u32 len)
+void msm_dsi_host_cmd_xfer_commit(struct mipi_dsi_host *host, u32 dma_base,
+				  u32 len)
 {
 	struct msm_dsi_host *msm_host = to_msm_dsi_host(host);
 
-	dsi_write(msm_host, REG_DSI_DMA_BASE, iova);
+	dsi_write(msm_host, REG_DSI_DMA_BASE, dma_base);
 	dsi_write(msm_host, REG_DSI_DMA_LEN, len);
 	dsi_write(msm_host, REG_DSI_TRIG_DMA, 1);
 
@@ -1766,6 +1967,7 @@ int msm_dsi_host_set_src_pll(struct mipi_dsi_host *host,
 	struct msm_dsi_pll *src_pll)
 {
 	struct msm_dsi_host *msm_host = to_msm_dsi_host(host);
+	const struct msm_dsi_cfg_handler *cfg_hnd = msm_host->cfg_hnd;
 	struct clk *byte_clk_provider, *pixel_clk_provider;
 	int ret;
 
@@ -1791,6 +1993,22 @@ int msm_dsi_host_set_src_pll(struct mipi_dsi_host *host,
 		goto exit;
 	}
 
+	if (cfg_hnd->major == MSM_DSI_VER_MAJOR_V2) {
+		ret = clk_set_parent(msm_host->dsi_clk_src, pixel_clk_provider);
+		if (ret) {
+			pr_err("%s: can't set parent to dsi_clk_src. ret=%d\n",
+				__func__, ret);
+			goto exit;
+		}
+
+		ret = clk_set_parent(msm_host->esc_clk_src, byte_clk_provider);
+		if (ret) {
+			pr_err("%s: can't set parent to esc_clk_src. ret=%d\n",
+				__func__, ret);
+			goto exit;
+		}
+	}
+
 exit:
 	return ret;
 }
@@ -1828,6 +2046,20 @@ int msm_dsi_host_disable(struct mipi_dsi_host *host)
 	return 0;
 }
 
+static void msm_dsi_sfpb_config(struct msm_dsi_host *msm_host, bool enable)
+{
+	enum sfpb_ahb_arb_master_port_en en;
+
+	if (!msm_host->sfpb)
+		return;
+
+	en = enable ? SFPB_MASTER_PORT_ENABLE : SFPB_MASTER_PORT_DISABLE;
+
+	regmap_update_bits(msm_host->sfpb, REG_SFPB_GPREG,
+			SFPB_GPREG_MASTER_PORT_EN__MASK,
+			SFPB_GPREG_MASTER_PORT_EN(en));
+}
+
 int msm_dsi_host_power_on(struct mipi_dsi_host *host)
 {
 	struct msm_dsi_host *msm_host = to_msm_dsi_host(host);
@@ -1840,6 +2072,8 @@ int msm_dsi_host_power_on(struct mipi_dsi_host *host)
 		goto unlock_ret;
 	}
 
+	msm_dsi_sfpb_config(msm_host, true);
+
 	ret = dsi_calc_clk_rate(msm_host);
 	if (ret) {
 		pr_err("%s: unable to calc clk rate, %d\n", __func__, ret);
@@ -1862,7 +2096,7 @@ int msm_dsi_host_power_on(struct mipi_dsi_host *host)
 	dsi_phy_sw_reset(msm_host);
 	ret = msm_dsi_manager_phy_enable(msm_host->id,
 					msm_host->byte_clk_rate * 8,
-					clk_get_rate(msm_host->esc_clk),
+					msm_host->esc_clk_rate,
 					&clk_pre, &clk_post);
 	dsi_bus_clk_disable(msm_host);
 	if (ret) {
@@ -1927,6 +2161,8 @@ int msm_dsi_host_power_off(struct mipi_dsi_host *host)
 
 	dsi_host_regulator_disable(msm_host);
 
+	msm_dsi_sfpb_config(msm_host, false);
+
 	DBG("-");
 
 	msm_host->power_on = false;
diff --git a/drivers/gpu/drm/msm/dsi/dsi_manager.c b/drivers/gpu/drm/msm/dsi/dsi_manager.c
index 0455ff75074a..58ba7ec17f51 100644
--- a/drivers/gpu/drm/msm/dsi/dsi_manager.c
+++ b/drivers/gpu/drm/msm/dsi/dsi_manager.c
@@ -774,7 +774,7 @@ restore_host0:
 	return ret;
 }
 
-bool msm_dsi_manager_cmd_xfer_trigger(int id, u32 iova, u32 len)
+bool msm_dsi_manager_cmd_xfer_trigger(int id, u32 dma_base, u32 len)
 {
 	struct msm_dsi *msm_dsi = dsi_mgr_get_dsi(id);
 	struct msm_dsi *msm_dsi0 = dsi_mgr_get_dsi(DSI_0);
@@ -784,9 +784,9 @@ bool msm_dsi_manager_cmd_xfer_trigger(int id, u32 iova, u32 len)
 		return false;
 
 	if (IS_SYNC_NEEDED() && msm_dsi0)
-		msm_dsi_host_cmd_xfer_commit(msm_dsi0->host, iova, len);
+		msm_dsi_host_cmd_xfer_commit(msm_dsi0->host, dma_base, len);
 
-	msm_dsi_host_cmd_xfer_commit(host, iova, len);
+	msm_dsi_host_cmd_xfer_commit(host, dma_base, len);
 
 	return true;
 }
diff --git a/drivers/gpu/drm/msm/dsi/phy/dsi_phy.c b/drivers/gpu/drm/msm/dsi/phy/dsi_phy.c
index f1f955f571fa..91a95fb04a4a 100644
--- a/drivers/gpu/drm/msm/dsi/phy/dsi_phy.c
+++ b/drivers/gpu/drm/msm/dsi/phy/dsi_phy.c
@@ -277,6 +277,10 @@ static const struct of_device_id dsi_phy_dt_match[] = {
 	{ .compatible = "qcom,dsi-phy-20nm",
 	  .data = &dsi_phy_20nm_cfgs },
 #endif
+#ifdef CONFIG_DRM_MSM_DSI_28NM_8960_PHY
+	{ .compatible = "qcom,dsi-phy-28nm-8960",
+	  .data = &dsi_phy_28nm_8960_cfgs },
+#endif
 	{}
 };
 
diff --git a/drivers/gpu/drm/msm/dsi/phy/dsi_phy.h b/drivers/gpu/drm/msm/dsi/phy/dsi_phy.h
index 0456b253239f..0d54ed00386d 100644
--- a/drivers/gpu/drm/msm/dsi/phy/dsi_phy.h
+++ b/drivers/gpu/drm/msm/dsi/phy/dsi_phy.h
@@ -43,6 +43,7 @@ struct msm_dsi_phy_cfg {
 extern const struct msm_dsi_phy_cfg dsi_phy_28nm_hpm_cfgs;
 extern const struct msm_dsi_phy_cfg dsi_phy_28nm_lp_cfgs;
 extern const struct msm_dsi_phy_cfg dsi_phy_20nm_cfgs;
+extern const struct msm_dsi_phy_cfg dsi_phy_28nm_8960_cfgs;
 
 struct msm_dsi_dphy_timing {
 	u32 clk_pre;
diff --git a/drivers/gpu/drm/msm/dsi/phy/dsi_phy_28nm_8960.c b/drivers/gpu/drm/msm/dsi/phy/dsi_phy_28nm_8960.c
new file mode 100644
index 000000000000..197b039ca1f1
--- /dev/null
+++ b/drivers/gpu/drm/msm/dsi/phy/dsi_phy_28nm_8960.c
@@ -0,0 +1,195 @@
+/*
+ * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "dsi_phy.h"
+#include "dsi.xml.h"
+
+static void dsi_28nm_dphy_set_timing(struct msm_dsi_phy *phy,
+		struct msm_dsi_dphy_timing *timing)
+{
+	void __iomem *base = phy->base;
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_0,
+		DSI_28nm_8960_PHY_TIMING_CTRL_0_CLK_ZERO(timing->clk_zero));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_1,
+		DSI_28nm_8960_PHY_TIMING_CTRL_1_CLK_TRAIL(timing->clk_trail));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_2,
+		DSI_28nm_8960_PHY_TIMING_CTRL_2_CLK_PREPARE(timing->clk_prepare));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_3, 0x0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_4,
+		DSI_28nm_8960_PHY_TIMING_CTRL_4_HS_EXIT(timing->hs_exit));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_5,
+		DSI_28nm_8960_PHY_TIMING_CTRL_5_HS_ZERO(timing->hs_zero));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_6,
+		DSI_28nm_8960_PHY_TIMING_CTRL_6_HS_PREPARE(timing->hs_prepare));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_7,
+		DSI_28nm_8960_PHY_TIMING_CTRL_7_HS_TRAIL(timing->hs_trail));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_8,
+		DSI_28nm_8960_PHY_TIMING_CTRL_8_HS_RQST(timing->hs_rqst));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_9,
+		DSI_28nm_8960_PHY_TIMING_CTRL_9_TA_GO(timing->ta_go) |
+		DSI_28nm_8960_PHY_TIMING_CTRL_9_TA_SURE(timing->ta_sure));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_10,
+		DSI_28nm_8960_PHY_TIMING_CTRL_10_TA_GET(timing->ta_get));
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_TIMING_CTRL_11,
+		DSI_28nm_8960_PHY_TIMING_CTRL_11_TRIG3_CMD(0));
+}
+
+static void dsi_28nm_phy_regulator_init(struct msm_dsi_phy *phy)
+{
+	void __iomem *base = phy->reg_base;
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_0, 0x3);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_1, 1);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_2, 1);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_3, 0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_4,
+		0x100);
+}
+
+static void dsi_28nm_phy_regulator_ctrl(struct msm_dsi_phy *phy)
+{
+	void __iomem *base = phy->reg_base;
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_0, 0x3);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_1, 0xa);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_2, 0x4);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_3, 0x0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CTRL_4, 0x20);
+}
+
+static void dsi_28nm_phy_calibration(struct msm_dsi_phy *phy)
+{
+	void __iomem *base = phy->reg_base;
+	u32 status;
+	int i = 5000;
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_REGULATOR_CAL_PWR_CFG,
+			0x3);
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_SW_CFG_2, 0x0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_CFG_1, 0x5a);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_CFG_3, 0x10);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_CFG_4, 0x1);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_CFG_0, 0x1);
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_TRIGGER, 0x1);
+	usleep_range(5000, 6000);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_MISC_CAL_HW_TRIGGER, 0x0);
+
+	do {
+		status = dsi_phy_read(base +
+				REG_DSI_28nm_8960_PHY_MISC_CAL_STATUS);
+
+		if (!(status & DSI_28nm_8960_PHY_MISC_CAL_STATUS_CAL_BUSY))
+			break;
+
+		udelay(1);
+	} while (--i > 0);
+}
+
+static void dsi_28nm_phy_lane_config(struct msm_dsi_phy *phy)
+{
+	void __iomem *base = phy->base;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_CFG_0(i), 0x80);
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_CFG_1(i), 0x45);
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_CFG_2(i), 0x00);
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_TEST_DATAPATH(i),
+			0x00);
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_TEST_STR_0(i),
+			0x01);
+		dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LN_TEST_STR_1(i),
+			0x66);
+	}
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_CFG_0, 0x40);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_CFG_1, 0x67);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_CFG_2, 0x0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_TEST_DATAPATH, 0x0);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_TEST_STR0, 0x1);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LNCK_TEST_STR1, 0x88);
+}
+
+static int dsi_28nm_phy_enable(struct msm_dsi_phy *phy, int src_pll_id,
+		const unsigned long bit_rate, const unsigned long esc_rate)
+{
+	struct msm_dsi_dphy_timing *timing = &phy->timing;
+	void __iomem *base = phy->base;
+
+	DBG("");
+
+	if (msm_dsi_dphy_timing_calc(timing, bit_rate, esc_rate)) {
+		dev_err(&phy->pdev->dev,
+			"%s: D-PHY timing calculation failed\n", __func__);
+		return -EINVAL;
+	}
+
+	dsi_28nm_phy_regulator_init(phy);
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_LDO_CTRL, 0x04);
+
+	/* strength control */
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_STRENGTH_0, 0xff);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_STRENGTH_1, 0x00);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_STRENGTH_2, 0x06);
+
+	/* phy ctrl */
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_CTRL_0, 0x5f);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_CTRL_1, 0x00);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_CTRL_2, 0x00);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_CTRL_3, 0x10);
+
+	dsi_28nm_phy_regulator_ctrl(phy);
+
+	dsi_28nm_phy_calibration(phy);
+
+	dsi_28nm_phy_lane_config(phy);
+
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_BIST_CTRL_4, 0x0f);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_BIST_CTRL_1, 0x03);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_BIST_CTRL_0, 0x03);
+	dsi_phy_write(base + REG_DSI_28nm_8960_PHY_BIST_CTRL_4, 0x0);
+
+	dsi_28nm_dphy_set_timing(phy, timing);
+
+	return 0;
+}
+
+static void dsi_28nm_phy_disable(struct msm_dsi_phy *phy)
+{
+	dsi_phy_write(phy->base + REG_DSI_28nm_8960_PHY_CTRL_0, 0x0);
+
+	/*
+	 * Wait for the registers writes to complete in order to
+	 * ensure that the phy is completely disabled
+	 */
+	wmb();
+}
+
+const struct msm_dsi_phy_cfg dsi_phy_28nm_8960_cfgs = {
+	.type = MSM_DSI_PHY_28NM_8960,
+	.src_pll_truthtable = { {true, true}, {false, true} },
+	.reg_cfg = {
+		.num = 1,
+		.regs = {
+			{"vddio", 1800000, 1800000, 100000, 100},
+		},
+	},
+	.ops = {
+		.enable = dsi_28nm_phy_enable,
+		.disable = dsi_28nm_phy_disable,
+	},
+};
diff --git a/drivers/gpu/drm/msm/dsi/pll/dsi_pll.c b/drivers/gpu/drm/msm/dsi/pll/dsi_pll.c
index 5104fc9f9a53..5cd438f91afe 100644
--- a/drivers/gpu/drm/msm/dsi/pll/dsi_pll.c
+++ b/drivers/gpu/drm/msm/dsi/pll/dsi_pll.c
@@ -151,6 +151,9 @@ struct msm_dsi_pll *msm_dsi_pll_init(struct platform_device *pdev,
 	case MSM_DSI_PHY_28NM_LP:
 		pll = msm_dsi_pll_28nm_init(pdev, type, id);
 		break;
+	case MSM_DSI_PHY_28NM_8960:
+		pll = msm_dsi_pll_28nm_8960_init(pdev, id);
+		break;
 	default:
 		pll = ERR_PTR(-ENXIO);
 		break;
diff --git a/drivers/gpu/drm/msm/dsi/pll/dsi_pll.h b/drivers/gpu/drm/msm/dsi/pll/dsi_pll.h
index 063caa2c5740..80b6038334a6 100644
--- a/drivers/gpu/drm/msm/dsi/pll/dsi_pll.h
+++ b/drivers/gpu/drm/msm/dsi/pll/dsi_pll.h
@@ -93,6 +93,16 @@ static inline struct msm_dsi_pll *msm_dsi_pll_28nm_init(
 	return ERR_PTR(-ENODEV);
 }
 #endif
+#ifdef CONFIG_DRM_MSM_DSI_28NM_8960_PHY
+struct msm_dsi_pll *msm_dsi_pll_28nm_8960_init(struct platform_device *pdev,
+					       int id);
+#else
+struct msm_dsi_pll *msm_dsi_pll_28nm_8960_init(struct platform_device *pdev,
+					       int id)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif
 
 #endif /* __DSI_PLL_H__ */
 
diff --git a/drivers/gpu/drm/msm/dsi/pll/dsi_pll_28nm_8960.c b/drivers/gpu/drm/msm/dsi/pll/dsi_pll_28nm_8960.c
new file mode 100644
index 000000000000..38c90e1eb002
--- /dev/null
+++ b/drivers/gpu/drm/msm/dsi/pll/dsi_pll_28nm_8960.c
@@ -0,0 +1,533 @@
+/*
+ * Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+
+#include "dsi_pll.h"
+#include "dsi.xml.h"
+
+/*
+ * DSI PLL 28nm (8960/A family) - clock diagram (eg: DSI1):
+ *
+ *
+ *                        +------+
+ *  dsi1vco_clk ----o-----| DIV1 |---dsi1pllbit (not exposed as clock)
+ *  F * byte_clk    |     +------+
+ *                  | bit clock divider (F / 8)
+ *                  |
+ *                  |     +------+
+ *                  o-----| DIV2 |---dsi0pllbyte---o---> To byte RCG
+ *                  |     +------+                 | (sets parent rate)
+ *                  | byte clock divider (F)       |
+ *                  |                              |
+ *                  |                              o---> To esc RCG
+ *                  |                                (doesn't set parent rate)
+ *                  |
+ *                  |     +------+
+ *                  o-----| DIV3 |----dsi0pll------o---> To dsi RCG
+ *                        +------+                 | (sets parent rate)
+ *                  dsi clock divider (F * magic)  |
+ *                                                 |
+ *                                                 o---> To pixel rcg
+ *                                                  (doesn't set parent rate)
+ */
+
+#define POLL_MAX_READS		8000
+#define POLL_TIMEOUT_US		1
+
+#define NUM_PROVIDED_CLKS	2
+
+#define VCO_REF_CLK_RATE	27000000
+#define VCO_MIN_RATE		600000000
+#define VCO_MAX_RATE		1200000000
+
+#define DSI_BYTE_PLL_CLK	0
+#define DSI_PIXEL_PLL_CLK	1
+
+#define VCO_PREF_DIV_RATIO	27
+
+struct pll_28nm_cached_state {
+	unsigned long vco_rate;
+	u8 postdiv3;
+	u8 postdiv2;
+	u8 postdiv1;
+};
+
+struct clk_bytediv {
+	struct clk_hw hw;
+	void __iomem *reg;
+};
+
+struct dsi_pll_28nm {
+	struct msm_dsi_pll base;
+
+	int id;
+	struct platform_device *pdev;
+	void __iomem *mmio;
+
+	/* custom byte clock divider */
+	struct clk_bytediv *bytediv;
+
+	/* private clocks: */
+	struct clk *clks[NUM_DSI_CLOCKS_MAX];
+	u32 num_clks;
+
+	/* clock-provider: */
+	struct clk *provided_clks[NUM_PROVIDED_CLKS];
+	struct clk_onecell_data clk_data;
+
+	struct pll_28nm_cached_state cached_state;
+};
+
+#define to_pll_28nm(x)	container_of(x, struct dsi_pll_28nm, base)
+
+static bool pll_28nm_poll_for_ready(struct dsi_pll_28nm *pll_28nm,
+				    int nb_tries, int timeout_us)
+{
+	bool pll_locked = false;
+	u32 val;
+
+	while (nb_tries--) {
+		val = pll_read(pll_28nm->mmio + REG_DSI_28nm_8960_PHY_PLL_RDY);
+		pll_locked = !!(val & DSI_28nm_8960_PHY_PLL_RDY_PLL_RDY);
+
+		if (pll_locked)
+			break;
+
+		udelay(timeout_us);
+	}
+	DBG("DSI PLL is %slocked", pll_locked ? "" : "*not* ");
+
+	return pll_locked;
+}
+
+/*
+ * Clock Callbacks
+ */
+static int dsi_pll_28nm_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+				     unsigned long parent_rate)
+{
+	struct msm_dsi_pll *pll = hw_clk_to_pll(hw);
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+	void __iomem *base = pll_28nm->mmio;
+	u32 val, temp, fb_divider;
+
+	DBG("rate=%lu, parent's=%lu", rate, parent_rate);
+
+	temp = rate / 10;
+	val = VCO_REF_CLK_RATE / 10;
+	fb_divider = (temp * VCO_PREF_DIV_RATIO) / val;
+	fb_divider = fb_divider / 2 - 1;
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_1,
+			fb_divider & 0xff);
+
+	val = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_2);
+
+	val |= (fb_divider >> 8) & 0x07;
+
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_2,
+			val);
+
+	val = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_3);
+
+	val |= (VCO_PREF_DIV_RATIO - 1) & 0x3f;
+
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_3,
+			val);
+
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_6,
+			0xf);
+
+	val = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8);
+	val |= 0x7 << 4;
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8,
+			val);
+
+	return 0;
+}
+
+static int dsi_pll_28nm_clk_is_enabled(struct clk_hw *hw)
+{
+	struct msm_dsi_pll *pll = hw_clk_to_pll(hw);
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+
+	return pll_28nm_poll_for_ready(pll_28nm, POLL_MAX_READS,
+					POLL_TIMEOUT_US);
+}
+
+static unsigned long dsi_pll_28nm_clk_recalc_rate(struct clk_hw *hw,
+						  unsigned long parent_rate)
+{
+	struct msm_dsi_pll *pll = hw_clk_to_pll(hw);
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+	void __iomem *base = pll_28nm->mmio;
+	unsigned long vco_rate;
+	u32 status, fb_divider, temp, ref_divider;
+
+	VERB("parent_rate=%lu", parent_rate);
+
+	status = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_0);
+
+	if (status & DSI_28nm_8960_PHY_PLL_CTRL_0_ENABLE) {
+		fb_divider = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_1);
+		fb_divider &= 0xff;
+		temp = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_2) & 0x07;
+		fb_divider = (temp << 8) | fb_divider;
+		fb_divider += 1;
+
+		ref_divider = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_3);
+		ref_divider &= 0x3f;
+		ref_divider += 1;
+
+		/* multiply by 2 */
+		vco_rate = (parent_rate / ref_divider) * fb_divider * 2;
+	} else {
+		vco_rate = 0;
+	}
+
+	DBG("returning vco rate = %lu", vco_rate);
+
+	return vco_rate;
+}
+
+static const struct clk_ops clk_ops_dsi_pll_28nm_vco = {
+	.round_rate = msm_dsi_pll_helper_clk_round_rate,
+	.set_rate = dsi_pll_28nm_clk_set_rate,
+	.recalc_rate = dsi_pll_28nm_clk_recalc_rate,
+	.prepare = msm_dsi_pll_helper_clk_prepare,
+	.unprepare = msm_dsi_pll_helper_clk_unprepare,
+	.is_enabled = dsi_pll_28nm_clk_is_enabled,
+};
+
+/*
+ * Custom byte clock divier clk_ops
+ *
+ * This clock is the entry point to configuring the PLL. The user (dsi host)
+ * will set this clock's rate to the desired byte clock rate. The VCO lock
+ * frequency is a multiple of the byte clock rate. The multiplication factor
+ * (shown as F in the diagram above) is a function of the byte clock rate.
+ *
+ * This custom divider clock ensures that its parent (VCO) is set to the
+ * desired rate, and that the byte clock postdivider (POSTDIV2) is configured
+ * accordingly
+ */
+#define to_clk_bytediv(_hw) container_of(_hw, struct clk_bytediv, hw)
+
+static unsigned long clk_bytediv_recalc_rate(struct clk_hw *hw,
+		unsigned long parent_rate)
+{
+	struct clk_bytediv *bytediv = to_clk_bytediv(hw);
+	unsigned int div;
+
+	div = pll_read(bytediv->reg) & 0xff;
+
+	return parent_rate / (div + 1);
+}
+
+/* find multiplication factor(wrt byte clock) at which the VCO should be set */
+static unsigned int get_vco_mul_factor(unsigned long byte_clk_rate)
+{
+	unsigned long bit_mhz;
+
+	/* convert to bit clock in Mhz */
+	bit_mhz = (byte_clk_rate * 8) / 1000000;
+
+	if (bit_mhz < 125)
+		return 64;
+	else if (bit_mhz < 250)
+		return 32;
+	else if (bit_mhz < 600)
+		return 16;
+	else
+		return 8;
+}
+
+static long clk_bytediv_round_rate(struct clk_hw *hw, unsigned long rate,
+				   unsigned long *prate)
+{
+	unsigned long best_parent;
+	unsigned int factor;
+
+	factor = get_vco_mul_factor(rate);
+
+	best_parent = rate * factor;
+	*prate = clk_hw_round_rate(clk_hw_get_parent(hw), best_parent);
+
+	return *prate / factor;
+}
+
+static int clk_bytediv_set_rate(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct clk_bytediv *bytediv = to_clk_bytediv(hw);
+	u32 val;
+	unsigned int factor;
+
+	factor = get_vco_mul_factor(rate);
+
+	val = pll_read(bytediv->reg);
+	val |= (factor - 1) & 0xff;
+	pll_write(bytediv->reg, val);
+
+	return 0;
+}
+
+/* Our special byte clock divider ops */
+static const struct clk_ops clk_bytediv_ops = {
+	.round_rate = clk_bytediv_round_rate,
+	.set_rate = clk_bytediv_set_rate,
+	.recalc_rate = clk_bytediv_recalc_rate,
+};
+
+/*
+ * PLL Callbacks
+ */
+static int dsi_pll_28nm_enable_seq(struct msm_dsi_pll *pll)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+	struct device *dev = &pll_28nm->pdev->dev;
+	void __iomem *base = pll_28nm->mmio;
+	bool locked;
+	unsigned int bit_div, byte_div;
+	int max_reads = 1000, timeout_us = 100;
+	u32 val;
+
+	DBG("id=%d", pll_28nm->id);
+
+	/*
+	 * before enabling the PLL, configure the bit clock divider since we
+	 * don't expose it as a clock to the outside world
+	 * 1: read back the byte clock divider that should already be set
+	 * 2: divide by 8 to get bit clock divider
+	 * 3: write it to POSTDIV1
+	 */
+	val = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_9);
+	byte_div = val + 1;
+	bit_div = byte_div / 8;
+
+	val = pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8);
+	val &= ~0xf;
+	val |= (bit_div - 1);
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8, val);
+
+	/* enable the PLL */
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_0,
+			DSI_28nm_8960_PHY_PLL_CTRL_0_ENABLE);
+
+	locked = pll_28nm_poll_for_ready(pll_28nm, max_reads, timeout_us);
+
+	if (unlikely(!locked))
+		dev_err(dev, "DSI PLL lock failed\n");
+	else
+		DBG("DSI PLL lock success");
+
+	return locked ? 0 : -EINVAL;
+}
+
+static void dsi_pll_28nm_disable_seq(struct msm_dsi_pll *pll)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+
+	DBG("id=%d", pll_28nm->id);
+	pll_write(pll_28nm->mmio + REG_DSI_28nm_8960_PHY_PLL_CTRL_0, 0x00);
+}
+
+static void dsi_pll_28nm_save_state(struct msm_dsi_pll *pll)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+	struct pll_28nm_cached_state *cached_state = &pll_28nm->cached_state;
+	void __iomem *base = pll_28nm->mmio;
+
+	cached_state->postdiv3 =
+			pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_10);
+	cached_state->postdiv2 =
+			pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_9);
+	cached_state->postdiv1 =
+			pll_read(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8);
+
+	cached_state->vco_rate = clk_hw_get_rate(&pll->clk_hw);
+}
+
+static int dsi_pll_28nm_restore_state(struct msm_dsi_pll *pll)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+	struct pll_28nm_cached_state *cached_state = &pll_28nm->cached_state;
+	void __iomem *base = pll_28nm->mmio;
+	int ret;
+
+	ret = dsi_pll_28nm_clk_set_rate(&pll->clk_hw,
+					cached_state->vco_rate, 0);
+	if (ret) {
+		dev_err(&pll_28nm->pdev->dev,
+			"restore vco rate failed. ret=%d\n", ret);
+		return ret;
+	}
+
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_10,
+			cached_state->postdiv3);
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_9,
+			cached_state->postdiv2);
+	pll_write(base + REG_DSI_28nm_8960_PHY_PLL_CTRL_8,
+			cached_state->postdiv1);
+
+	return 0;
+}
+
+static int dsi_pll_28nm_get_provider(struct msm_dsi_pll *pll,
+				struct clk **byte_clk_provider,
+				struct clk **pixel_clk_provider)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+
+	if (byte_clk_provider)
+		*byte_clk_provider = pll_28nm->provided_clks[DSI_BYTE_PLL_CLK];
+	if (pixel_clk_provider)
+		*pixel_clk_provider =
+				pll_28nm->provided_clks[DSI_PIXEL_PLL_CLK];
+
+	return 0;
+}
+
+static void dsi_pll_28nm_destroy(struct msm_dsi_pll *pll)
+{
+	struct dsi_pll_28nm *pll_28nm = to_pll_28nm(pll);
+
+	msm_dsi_pll_helper_unregister_clks(pll_28nm->pdev,
+					pll_28nm->clks, pll_28nm->num_clks);
+}
+
+static int pll_28nm_register(struct dsi_pll_28nm *pll_28nm)
+{
+	char *clk_name, *parent_name, *vco_name;
+	struct clk_init_data vco_init = {
+		.parent_names = (const char *[]){ "pxo" },
+		.num_parents = 1,
+		.ops = &clk_ops_dsi_pll_28nm_vco,
+	};
+	struct device *dev = &pll_28nm->pdev->dev;
+	struct clk **clks = pll_28nm->clks;
+	struct clk **provided_clks = pll_28nm->provided_clks;
+	struct clk_bytediv *bytediv;
+	struct clk_init_data bytediv_init = { };
+	int ret, num = 0;
+
+	DBG("%d", pll_28nm->id);
+
+	bytediv = devm_kzalloc(dev, sizeof(*bytediv), GFP_KERNEL);
+	if (!bytediv)
+		return -ENOMEM;
+
+	vco_name = devm_kzalloc(dev, 32, GFP_KERNEL);
+	if (!vco_name)
+		return -ENOMEM;
+
+	parent_name = devm_kzalloc(dev, 32, GFP_KERNEL);
+	if (!parent_name)
+		return -ENOMEM;
+
+	clk_name = devm_kzalloc(dev, 32, GFP_KERNEL);
+	if (!clk_name)
+		return -ENOMEM;
+
+	pll_28nm->bytediv = bytediv;
+
+	snprintf(vco_name, 32, "dsi%dvco_clk", pll_28nm->id);
+	vco_init.name = vco_name;
+
+	pll_28nm->base.clk_hw.init = &vco_init;
+
+	clks[num++] = clk_register(dev, &pll_28nm->base.clk_hw);
+
+	/* prepare and register bytediv */
+	bytediv->hw.init = &bytediv_init;
+	bytediv->reg = pll_28nm->mmio + REG_DSI_28nm_8960_PHY_PLL_CTRL_9;
+
+	snprintf(parent_name, 32, "dsi%dvco_clk", pll_28nm->id);
+	snprintf(clk_name, 32, "dsi%dpllbyte", pll_28nm->id);
+
+	bytediv_init.name = clk_name;
+	bytediv_init.ops = &clk_bytediv_ops;
+	bytediv_init.flags = CLK_SET_RATE_PARENT;
+	bytediv_init.parent_names = (const char * const *) &parent_name;
+	bytediv_init.num_parents = 1;
+
+	/* DIV2 */
+	clks[num++] = provided_clks[DSI_BYTE_PLL_CLK] =
+			clk_register(dev, &bytediv->hw);
+
+	snprintf(clk_name, 32, "dsi%dpll", pll_28nm->id);
+	/* DIV3 */
+	clks[num++] = provided_clks[DSI_PIXEL_PLL_CLK] =
+			clk_register_divider(dev, clk_name,
+				parent_name, 0, pll_28nm->mmio +
+				REG_DSI_28nm_8960_PHY_PLL_CTRL_10,
+				0, 8, 0, NULL);
+
+	pll_28nm->num_clks = num;
+
+	pll_28nm->clk_data.clk_num = NUM_PROVIDED_CLKS;
+	pll_28nm->clk_data.clks = provided_clks;
+
+	ret = of_clk_add_provider(dev->of_node,
+			of_clk_src_onecell_get, &pll_28nm->clk_data);
+	if (ret) {
+		dev_err(dev, "failed to register clk provider: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+struct msm_dsi_pll *msm_dsi_pll_28nm_8960_init(struct platform_device *pdev,
+					       int id)
+{
+	struct dsi_pll_28nm *pll_28nm;
+	struct msm_dsi_pll *pll;
+	int ret;
+
+	if (!pdev)
+		return ERR_PTR(-ENODEV);
+
+	pll_28nm = devm_kzalloc(&pdev->dev, sizeof(*pll_28nm), GFP_KERNEL);
+	if (!pll_28nm)
+		return ERR_PTR(-ENOMEM);
+
+	pll_28nm->pdev = pdev;
+	pll_28nm->id = id + 1;
+
+	pll_28nm->mmio = msm_ioremap(pdev, "dsi_pll", "DSI_PLL");
+	if (IS_ERR_OR_NULL(pll_28nm->mmio)) {
+		dev_err(&pdev->dev, "%s: failed to map pll base\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	pll = &pll_28nm->base;
+	pll->min_rate = VCO_MIN_RATE;
+	pll->max_rate = VCO_MAX_RATE;
+	pll->get_provider = dsi_pll_28nm_get_provider;
+	pll->destroy = dsi_pll_28nm_destroy;
+	pll->disable_seq = dsi_pll_28nm_disable_seq;
+	pll->save_state = dsi_pll_28nm_save_state;
+	pll->restore_state = dsi_pll_28nm_restore_state;
+
+	pll->en_seq_cnt = 1;
+	pll->enable_seqs[0] = dsi_pll_28nm_enable_seq;
+
+	ret = pll_28nm_register(pll_28nm);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register PLL: %d\n", ret);
+		return ERR_PTR(ret);
+	}
+
+	return pll;
+}
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.c b/drivers/gpu/drm/msm/hdmi/hdmi.c
index 1f4a95eeb348..9a0989c0b4de 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.c
@@ -17,6 +17,8 @@
  */
 
 #include <linux/of_irq.h>
+#include <linux/of_gpio.h>
+
 #include "hdmi.h"
 
 void hdmi_set_mode(struct hdmi *hdmi, bool power_on)
@@ -322,8 +324,6 @@ fail:
  * The hdmi device:
  */
 
-#include <linux/of_gpio.h>
-
 #define HDMI_CFG(item, entry) \
 	.item ## _names = item ##_names_ ## entry, \
 	.item ## _cnt   = ARRAY_SIZE(item ## _names_ ## entry)
@@ -388,17 +388,6 @@ static struct hdmi_platform_config hdmi_tx_8996_config = {
 		.hpd_freq      = hpd_clk_freq_8x74,
 };
 
-static const struct of_device_id dt_match[] = {
-	{ .compatible = "qcom,hdmi-tx-8996", .data = &hdmi_tx_8996_config },
-	{ .compatible = "qcom,hdmi-tx-8994", .data = &hdmi_tx_8994_config },
-	{ .compatible = "qcom,hdmi-tx-8084", .data = &hdmi_tx_8084_config },
-	{ .compatible = "qcom,hdmi-tx-8974", .data = &hdmi_tx_8974_config },
-	{ .compatible = "qcom,hdmi-tx-8960", .data = &hdmi_tx_8960_config },
-	{ .compatible = "qcom,hdmi-tx-8660", .data = &hdmi_tx_8660_config },
-	{}
-};
-
-#ifdef CONFIG_OF
 static int get_gpio(struct device *dev, struct device_node *of_node, const char *name)
 {
 	int gpio = of_get_named_gpio(of_node, name, 0);
@@ -413,7 +402,6 @@ static int get_gpio(struct device *dev, struct device_node *of_node, const char
 	}
 	return gpio;
 }
-#endif
 
 static int hdmi_bind(struct device *dev, struct device *master, void *data)
 {
@@ -421,16 +409,12 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 	struct msm_drm_private *priv = drm->dev_private;
 	static struct hdmi_platform_config *hdmi_cfg;
 	struct hdmi *hdmi;
-#ifdef CONFIG_OF
 	struct device_node *of_node = dev->of_node;
-	const struct of_device_id *match;
 
-	match = of_match_node(dt_match, of_node);
-	if (match && match->data) {
-		hdmi_cfg = (struct hdmi_platform_config *)match->data;
-		DBG("hdmi phy: %s", match->compatible);
-	} else {
-		dev_err(dev, "unknown phy: %s\n", of_node->name);
+	hdmi_cfg = (struct hdmi_platform_config *)
+			of_device_get_match_data(dev);
+	if (!hdmi_cfg) {
+		dev_err(dev, "unknown hdmi_cfg: %s\n", of_node->name);
 		return -ENXIO;
 	}
 
@@ -443,55 +427,6 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 	hdmi_cfg->mux_sel_gpio  = get_gpio(dev, of_node, "qcom,hdmi-tx-mux-sel");
 	hdmi_cfg->mux_lpm_gpio  = get_gpio(dev, of_node, "qcom,hdmi-tx-mux-lpm");
 
-#else
-	static struct hdmi_platform_config config = {};
-	static const char *hpd_clk_names[] = {
-			"core_clk", "master_iface_clk", "slave_iface_clk",
-	};
-	if (cpu_is_apq8064()) {
-		static const char *hpd_reg_names[] = {"8921_hdmi_mvs"};
-		config.phy_init      = hdmi_phy_8960_init;
-		config.hpd_reg_names = hpd_reg_names;
-		config.hpd_reg_cnt   = ARRAY_SIZE(hpd_reg_names);
-		config.hpd_clk_names = hpd_clk_names;
-		config.hpd_clk_cnt   = ARRAY_SIZE(hpd_clk_names);
-		config.ddc_clk_gpio  = 70;
-		config.ddc_data_gpio = 71;
-		config.hpd_gpio      = 72;
-		config.mux_en_gpio   = -1;
-		config.mux_sel_gpio  = -1;
-	} else if (cpu_is_msm8960() || cpu_is_msm8960ab()) {
-		static const char *hpd_reg_names[] = {"8921_hdmi_mvs"};
-		config.phy_init      = hdmi_phy_8960_init;
-		config.hpd_reg_names = hpd_reg_names;
-		config.hpd_reg_cnt   = ARRAY_SIZE(hpd_reg_names);
-		config.hpd_clk_names = hpd_clk_names;
-		config.hpd_clk_cnt   = ARRAY_SIZE(hpd_clk_names);
-		config.ddc_clk_gpio  = 100;
-		config.ddc_data_gpio = 101;
-		config.hpd_gpio      = 102;
-		config.mux_en_gpio   = -1;
-		config.mux_sel_gpio  = -1;
-	} else if (cpu_is_msm8x60()) {
-		static const char *hpd_reg_names[] = {
-				"8901_hdmi_mvs", "8901_mpp0"
-		};
-		config.phy_init      = hdmi_phy_8x60_init;
-		config.hpd_reg_names = hpd_reg_names;
-		config.hpd_reg_cnt   = ARRAY_SIZE(hpd_reg_names);
-		config.hpd_clk_names = hpd_clk_names;
-		config.hpd_clk_cnt   = ARRAY_SIZE(hpd_clk_names);
-		config.ddc_clk_gpio  = 170;
-		config.ddc_data_gpio = 171;
-		config.hpd_gpio      = 172;
-		config.mux_en_gpio   = -1;
-		config.mux_sel_gpio  = -1;
-	}
-	config.mmio_name     = "hdmi_msm_hdmi_addr";
-	config.qfprom_mmio_name = "hdmi_msm_qfprom_addr";
-
-	hdmi_cfg = &config;
-#endif
 	dev->platform_data = hdmi_cfg;
 
 	hdmi = hdmi_init(to_platform_device(dev));
@@ -529,6 +464,16 @@ static int hdmi_dev_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct of_device_id dt_match[] = {
+	{ .compatible = "qcom,hdmi-tx-8996", .data = &hdmi_tx_8996_config },
+	{ .compatible = "qcom,hdmi-tx-8994", .data = &hdmi_tx_8994_config },
+	{ .compatible = "qcom,hdmi-tx-8084", .data = &hdmi_tx_8084_config },
+	{ .compatible = "qcom,hdmi-tx-8974", .data = &hdmi_tx_8974_config },
+	{ .compatible = "qcom,hdmi-tx-8960", .data = &hdmi_tx_8960_config },
+	{ .compatible = "qcom,hdmi-tx-8660", .data = &hdmi_tx_8660_config },
+	{}
+};
+
 static struct platform_driver hdmi_driver = {
 	.probe = hdmi_dev_probe,
 	.remove = hdmi_dev_remove,
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
index 6ac9aa165768..28df397c3b04 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
@@ -678,7 +678,8 @@ struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
 	drm_flip_work_init(&mdp4_crtc->unref_cursor_work,
 			"unref cursor", unref_cursor_worker);
 
-	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp4_crtc_funcs);
+	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp4_crtc_funcs,
+				  NULL);
 	drm_crtc_helper_add(crtc, &mdp4_crtc_helper_funcs);
 	plane->crtc = crtc;
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dsi_encoder.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dsi_encoder.c
new file mode 100644
index 000000000000..2f57e9453b67
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dsi_encoder.c
@@ -0,0 +1,198 @@
+/*
+ * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2014, Inforce Computing. All rights reserved.
+ *
+ * Author: Vinay Simha <vinaysimha@inforcecomputing.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "mdp4_kms.h"
+
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+struct mdp4_dsi_encoder {
+	struct drm_encoder base;
+	struct drm_panel *panel;
+	bool enabled;
+};
+#define to_mdp4_dsi_encoder(x) container_of(x, struct mdp4_dsi_encoder, base)
+
+static struct mdp4_kms *get_kms(struct drm_encoder *encoder)
+{
+	struct msm_drm_private *priv = encoder->dev->dev_private;
+	return to_mdp4_kms(to_mdp_kms(priv->kms));
+}
+
+static void mdp4_dsi_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct mdp4_dsi_encoder *mdp4_dsi_encoder = to_mdp4_dsi_encoder(encoder);
+
+	drm_encoder_cleanup(encoder);
+	kfree(mdp4_dsi_encoder);
+}
+
+static const struct drm_encoder_funcs mdp4_dsi_encoder_funcs = {
+	.destroy = mdp4_dsi_encoder_destroy,
+};
+
+static bool mdp4_dsi_encoder_mode_fixup(struct drm_encoder *encoder,
+					const struct drm_display_mode *mode,
+					struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void mdp4_dsi_encoder_mode_set(struct drm_encoder *encoder,
+				      struct drm_display_mode *mode,
+				      struct drm_display_mode *adjusted_mode)
+{
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+	uint32_t dsi_hsync_skew, vsync_period, vsync_len, ctrl_pol;
+	uint32_t display_v_start, display_v_end;
+	uint32_t hsync_start_x, hsync_end_x;
+
+	mode = adjusted_mode;
+
+	DBG("set mode: %d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
+			mode->base.id, mode->name,
+			mode->vrefresh, mode->clock,
+			mode->hdisplay, mode->hsync_start,
+			mode->hsync_end, mode->htotal,
+			mode->vdisplay, mode->vsync_start,
+			mode->vsync_end, mode->vtotal,
+			mode->type, mode->flags);
+
+	ctrl_pol = 0;
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		ctrl_pol |= MDP4_DSI_CTRL_POLARITY_HSYNC_LOW;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		ctrl_pol |= MDP4_DSI_CTRL_POLARITY_VSYNC_LOW;
+	/* probably need to get DATA_EN polarity from panel.. */
+
+	dsi_hsync_skew = 0;  /* get this from panel? */
+
+	hsync_start_x = (mode->htotal - mode->hsync_start);
+	hsync_end_x = mode->htotal - (mode->hsync_start - mode->hdisplay) - 1;
+
+	vsync_period = mode->vtotal * mode->htotal;
+	vsync_len = (mode->vsync_end - mode->vsync_start) * mode->htotal;
+	display_v_start = (mode->vtotal - mode->vsync_start) * mode->htotal + dsi_hsync_skew;
+	display_v_end = vsync_period - ((mode->vsync_start - mode->vdisplay) * mode->htotal) + dsi_hsync_skew - 1;
+
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_HSYNC_CTRL,
+			MDP4_DSI_HSYNC_CTRL_PULSEW(mode->hsync_end - mode->hsync_start) |
+			MDP4_DSI_HSYNC_CTRL_PERIOD(mode->htotal));
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_VSYNC_PERIOD, vsync_period);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_VSYNC_LEN, vsync_len);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_DISPLAY_HCTRL,
+			MDP4_DSI_DISPLAY_HCTRL_START(hsync_start_x) |
+			MDP4_DSI_DISPLAY_HCTRL_END(hsync_end_x));
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_DISPLAY_VSTART, display_v_start);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_DISPLAY_VEND, display_v_end);
+
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_CTRL_POLARITY, ctrl_pol);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_UNDERFLOW_CLR,
+			MDP4_DSI_UNDERFLOW_CLR_ENABLE_RECOVERY |
+			MDP4_DSI_UNDERFLOW_CLR_COLOR(0xff));
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_ACTIVE_HCTL,
+			MDP4_DSI_ACTIVE_HCTL_START(0) |
+			MDP4_DSI_ACTIVE_HCTL_END(0));
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_HSYNC_SKEW, dsi_hsync_skew);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_BORDER_CLR, 0);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_ACTIVE_VSTART, 0);
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_ACTIVE_VEND, 0);
+}
+
+static void mdp4_dsi_encoder_disable(struct drm_encoder *encoder)
+{
+	struct mdp4_dsi_encoder *mdp4_dsi_encoder = to_mdp4_dsi_encoder(encoder);
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+
+	if (!mdp4_dsi_encoder->enabled)
+		return;
+
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_ENABLE, 0);
+
+	/*
+	 * Wait for a vsync so we know the ENABLE=0 latched before
+	 * the (connector) source of the vsync's gets disabled,
+	 * otherwise we end up in a funny state if we re-enable
+	 * before the disable latches, which results that some of
+	 * the settings changes for the new modeset (like new
+	 * scanout buffer) don't latch properly..
+	 */
+	mdp_irq_wait(&mdp4_kms->base, MDP4_IRQ_PRIMARY_VSYNC);
+
+	mdp4_dsi_encoder->enabled = false;
+}
+
+static void mdp4_dsi_encoder_enable(struct drm_encoder *encoder)
+{
+	struct mdp4_dsi_encoder *mdp4_dsi_encoder = to_mdp4_dsi_encoder(encoder);
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+
+	if (mdp4_dsi_encoder->enabled)
+		return;
+
+	 mdp4_crtc_set_config(encoder->crtc,
+			MDP4_DMA_CONFIG_PACK_ALIGN_MSB |
+			MDP4_DMA_CONFIG_DEFLKR_EN |
+			MDP4_DMA_CONFIG_DITHER_EN |
+			MDP4_DMA_CONFIG_R_BPC(BPC8) |
+			MDP4_DMA_CONFIG_G_BPC(BPC8) |
+			MDP4_DMA_CONFIG_B_BPC(BPC8) |
+			MDP4_DMA_CONFIG_PACK(0x21));
+
+	mdp4_crtc_set_intf(encoder->crtc, INTF_DSI_VIDEO, 0);
+
+	mdp4_write(mdp4_kms, REG_MDP4_DSI_ENABLE, 1);
+
+	mdp4_dsi_encoder->enabled = true;
+}
+
+static const struct drm_encoder_helper_funcs mdp4_dsi_encoder_helper_funcs = {
+	.mode_fixup = mdp4_dsi_encoder_mode_fixup,
+	.mode_set = mdp4_dsi_encoder_mode_set,
+	.disable = mdp4_dsi_encoder_disable,
+	.enable = mdp4_dsi_encoder_enable,
+};
+
+/* initialize encoder */
+struct drm_encoder *mdp4_dsi_encoder_init(struct drm_device *dev)
+{
+	struct drm_encoder *encoder = NULL;
+	struct mdp4_dsi_encoder *mdp4_dsi_encoder;
+	int ret;
+
+	mdp4_dsi_encoder = kzalloc(sizeof(*mdp4_dsi_encoder), GFP_KERNEL);
+	if (!mdp4_dsi_encoder) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	encoder = &mdp4_dsi_encoder->base;
+
+	drm_encoder_init(dev, encoder, &mdp4_dsi_encoder_funcs,
+			 DRM_MODE_ENCODER_DSI, NULL);
+	drm_encoder_helper_add(encoder, &mdp4_dsi_encoder_helper_funcs);
+
+	return encoder;
+
+fail:
+	if (encoder)
+		mdp4_dsi_encoder_destroy(encoder);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
index 89614c6a6c1b..a21df54cb50f 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
@@ -262,7 +262,7 @@ struct drm_encoder *mdp4_dtv_encoder_init(struct drm_device *dev)
 	encoder = &mdp4_dtv_encoder->base;
 
 	drm_encoder_init(dev, encoder, &mdp4_dtv_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 	drm_encoder_helper_add(encoder, &mdp4_dtv_encoder_helper_funcs);
 
 	mdp4_dtv_encoder->src_clk = devm_clk_get(dev->dev, "src_clk");
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
index 5ed38cf548a1..a521207db8a1 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_irq.c
@@ -29,7 +29,7 @@ void mdp4_set_irqmask(struct mdp_kms *mdp_kms, uint32_t irqmask,
 
 static void mdp4_irq_error_handler(struct mdp_irq *irq, uint32_t irqstatus)
 {
-	DRM_ERROR("errors: %08x\n", irqstatus);
+	DRM_ERROR_RATELIMITED("errors: %08x\n", irqstatus);
 }
 
 void mdp4_irq_preinstall(struct msm_kms *kms)
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
index 077f7521a971..5a8e3d6bcbff 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
@@ -169,7 +169,14 @@ static long mdp4_round_pixclk(struct msm_kms *kms, unsigned long rate,
 		struct drm_encoder *encoder)
 {
 	/* if we had >1 encoder, we'd need something more clever: */
-	return mdp4_dtv_round_pixclk(encoder, rate);
+	switch (encoder->encoder_type) {
+	case DRM_MODE_ENCODER_TMDS:
+		return mdp4_dtv_round_pixclk(encoder, rate);
+	case DRM_MODE_ENCODER_LVDS:
+	case DRM_MODE_ENCODER_DSI:
+	default:
+		return rate;
+	}
 }
 
 static void mdp4_preclose(struct msm_kms *kms, struct drm_file *file)
@@ -240,19 +247,18 @@ int mdp4_enable(struct mdp4_kms *mdp4_kms)
 	return 0;
 }
 
-#ifdef CONFIG_OF
-static struct drm_panel *detect_panel(struct drm_device *dev)
+static struct device_node *mdp4_detect_lcdc_panel(struct drm_device *dev)
 {
 	struct device_node *endpoint, *panel_node;
 	struct device_node *np = dev->dev->of_node;
-	struct drm_panel *panel = NULL;
 
 	endpoint = of_graph_get_next_endpoint(np, NULL);
 	if (!endpoint) {
-		dev_err(dev->dev, "no valid endpoint\n");
-		return ERR_PTR(-ENODEV);
+		DBG("no endpoint in MDP4 to fetch LVDS panel\n");
+		return NULL;
 	}
 
+	/* don't proceed if we have an endpoint but no panel_node tied to it */
 	panel_node = of_graph_get_remote_port_parent(endpoint);
 	if (!panel_node) {
 		dev_err(dev->dev, "no valid panel node\n");
@@ -262,132 +268,185 @@ static struct drm_panel *detect_panel(struct drm_device *dev)
 
 	of_node_put(endpoint);
 
-	panel = of_drm_find_panel(panel_node);
-	if (!panel) {
-		of_node_put(panel_node);
-		return ERR_PTR(-EPROBE_DEFER);
-	}
-
-	return panel;
+	return panel_node;
 }
-#else
-static struct drm_panel *detect_panel(struct drm_device *dev)
-{
-	// ??? maybe use a module param to specify which panel is attached?
-}
-#endif
 
-static int modeset_init(struct mdp4_kms *mdp4_kms)
+static int mdp4_modeset_init_intf(struct mdp4_kms *mdp4_kms,
+				  int intf_type)
 {
 	struct drm_device *dev = mdp4_kms->dev;
 	struct msm_drm_private *priv = dev->dev_private;
-	struct drm_plane *plane;
-	struct drm_crtc *crtc;
 	struct drm_encoder *encoder;
 	struct drm_connector *connector;
-	struct drm_panel *panel;
+	struct device_node *panel_node;
+	struct drm_encoder *dsi_encs[MSM_DSI_ENCODER_NUM];
+	int i, dsi_id;
 	int ret;
 
-	/* construct non-private planes: */
-	plane = mdp4_plane_init(dev, VG1, false);
-	if (IS_ERR(plane)) {
-		dev_err(dev->dev, "failed to construct plane for VG1\n");
-		ret = PTR_ERR(plane);
-		goto fail;
-	}
-	priv->planes[priv->num_planes++] = plane;
+	switch (intf_type) {
+	case DRM_MODE_ENCODER_LVDS:
+		/*
+		 * bail out early if:
+		 * - there is no panel node (no need to initialize lcdc
+		 *   encoder and lvds connector), or
+		 * - panel node is a bad pointer
+		 */
+		panel_node = mdp4_detect_lcdc_panel(dev);
+		if (IS_ERR_OR_NULL(panel_node))
+			return PTR_ERR(panel_node);
+
+		encoder = mdp4_lcdc_encoder_init(dev, panel_node);
+		if (IS_ERR(encoder)) {
+			dev_err(dev->dev, "failed to construct LCDC encoder\n");
+			return PTR_ERR(encoder);
+		}
 
-	plane = mdp4_plane_init(dev, VG2, false);
-	if (IS_ERR(plane)) {
-		dev_err(dev->dev, "failed to construct plane for VG2\n");
-		ret = PTR_ERR(plane);
-		goto fail;
-	}
-	priv->planes[priv->num_planes++] = plane;
+		/* LCDC can be hooked to DMA_P (TODO: Add DMA_S later?) */
+		encoder->possible_crtcs = 1 << DMA_P;
 
-	/*
-	 * Setup the LCDC/LVDS path: RGB2 -> DMA_P -> LCDC -> LVDS:
-	 */
+		connector = mdp4_lvds_connector_init(dev, panel_node, encoder);
+		if (IS_ERR(connector)) {
+			dev_err(dev->dev, "failed to initialize LVDS connector\n");
+			return PTR_ERR(connector);
+		}
 
-	panel = detect_panel(dev);
-	if (IS_ERR(panel)) {
-		ret = PTR_ERR(panel);
-		dev_err(dev->dev, "failed to detect LVDS panel: %d\n", ret);
-		goto fail;
-	}
+		priv->encoders[priv->num_encoders++] = encoder;
+		priv->connectors[priv->num_connectors++] = connector;
 
-	plane = mdp4_plane_init(dev, RGB2, true);
-	if (IS_ERR(plane)) {
-		dev_err(dev->dev, "failed to construct plane for RGB2\n");
-		ret = PTR_ERR(plane);
-		goto fail;
-	}
+		break;
+	case DRM_MODE_ENCODER_TMDS:
+		encoder = mdp4_dtv_encoder_init(dev);
+		if (IS_ERR(encoder)) {
+			dev_err(dev->dev, "failed to construct DTV encoder\n");
+			return PTR_ERR(encoder);
+		}
 
-	crtc  = mdp4_crtc_init(dev, plane, priv->num_crtcs, 0, DMA_P);
-	if (IS_ERR(crtc)) {
-		dev_err(dev->dev, "failed to construct crtc for DMA_P\n");
-		ret = PTR_ERR(crtc);
-		goto fail;
-	}
+		/* DTV can be hooked to DMA_E: */
+		encoder->possible_crtcs = 1 << 1;
 
-	encoder = mdp4_lcdc_encoder_init(dev, panel);
-	if (IS_ERR(encoder)) {
-		dev_err(dev->dev, "failed to construct LCDC encoder\n");
-		ret = PTR_ERR(encoder);
-		goto fail;
-	}
+		if (priv->hdmi) {
+			/* Construct bridge/connector for HDMI: */
+			ret = hdmi_modeset_init(priv->hdmi, dev, encoder);
+			if (ret) {
+				dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
+				return ret;
+			}
+		}
 
-	/* LCDC can be hooked to DMA_P: */
-	encoder->possible_crtcs = 1 << priv->num_crtcs;
+		priv->encoders[priv->num_encoders++] = encoder;
 
-	priv->crtcs[priv->num_crtcs++] = crtc;
-	priv->encoders[priv->num_encoders++] = encoder;
+		break;
+	case DRM_MODE_ENCODER_DSI:
+		/* only DSI1 supported for now */
+		dsi_id = 0;
 
-	connector = mdp4_lvds_connector_init(dev, panel, encoder);
-	if (IS_ERR(connector)) {
-		ret = PTR_ERR(connector);
-		dev_err(dev->dev, "failed to initialize LVDS connector: %d\n", ret);
-		goto fail;
-	}
+		if (!priv->dsi[dsi_id])
+			break;
 
-	priv->connectors[priv->num_connectors++] = connector;
+		for (i = 0; i < MSM_DSI_ENCODER_NUM; i++) {
+			dsi_encs[i] = mdp4_dsi_encoder_init(dev);
+			if (IS_ERR(dsi_encs[i])) {
+				ret = PTR_ERR(dsi_encs[i]);
+				dev_err(dev->dev,
+					"failed to construct DSI encoder: %d\n",
+					ret);
+				return ret;
+			}
 
-	/*
-	 * Setup DTV/HDMI path: RGB1 -> DMA_E -> DTV -> HDMI:
-	 */
+			/* TODO: Add DMA_S later? */
+			dsi_encs[i]->possible_crtcs = 1 << DMA_P;
+			priv->encoders[priv->num_encoders++] = dsi_encs[i];
+		}
 
-	plane = mdp4_plane_init(dev, RGB1, true);
-	if (IS_ERR(plane)) {
-		dev_err(dev->dev, "failed to construct plane for RGB1\n");
-		ret = PTR_ERR(plane);
-		goto fail;
-	}
+		ret = msm_dsi_modeset_init(priv->dsi[dsi_id], dev, dsi_encs);
+		if (ret) {
+			dev_err(dev->dev, "failed to initialize DSI: %d\n",
+				ret);
+			return ret;
+		}
 
-	crtc  = mdp4_crtc_init(dev, plane, priv->num_crtcs, 1, DMA_E);
-	if (IS_ERR(crtc)) {
-		dev_err(dev->dev, "failed to construct crtc for DMA_E\n");
-		ret = PTR_ERR(crtc);
-		goto fail;
+		break;
+	default:
+		dev_err(dev->dev, "Invalid or unsupported interface\n");
+		return -EINVAL;
 	}
 
-	encoder = mdp4_dtv_encoder_init(dev);
-	if (IS_ERR(encoder)) {
-		dev_err(dev->dev, "failed to construct DTV encoder\n");
-		ret = PTR_ERR(encoder);
-		goto fail;
+	return 0;
+}
+
+static int modeset_init(struct mdp4_kms *mdp4_kms)
+{
+	struct drm_device *dev = mdp4_kms->dev;
+	struct msm_drm_private *priv = dev->dev_private;
+	struct drm_plane *plane;
+	struct drm_crtc *crtc;
+	int i, ret;
+	static const enum mdp4_pipe rgb_planes[] = {
+		RGB1, RGB2,
+	};
+	static const enum mdp4_pipe vg_planes[] = {
+		VG1, VG2,
+	};
+	static const enum mdp4_dma mdp4_crtcs[] = {
+		DMA_P, DMA_E,
+	};
+	static const char * const mdp4_crtc_names[] = {
+		"DMA_P", "DMA_E",
+	};
+	static const int mdp4_intfs[] = {
+		DRM_MODE_ENCODER_LVDS,
+		DRM_MODE_ENCODER_DSI,
+		DRM_MODE_ENCODER_TMDS,
+	};
+
+	/* construct non-private planes: */
+	for (i = 0; i < ARRAY_SIZE(vg_planes); i++) {
+		plane = mdp4_plane_init(dev, vg_planes[i], false);
+		if (IS_ERR(plane)) {
+			dev_err(dev->dev,
+				"failed to construct plane for VG%d\n", i + 1);
+			ret = PTR_ERR(plane);
+			goto fail;
+		}
+		priv->planes[priv->num_planes++] = plane;
 	}
 
-	/* DTV can be hooked to DMA_E: */
-	encoder->possible_crtcs = 1 << priv->num_crtcs;
+	for (i = 0; i < ARRAY_SIZE(mdp4_crtcs); i++) {
+		plane = mdp4_plane_init(dev, rgb_planes[i], true);
+		if (IS_ERR(plane)) {
+			dev_err(dev->dev,
+				"failed to construct plane for RGB%d\n", i + 1);
+			ret = PTR_ERR(plane);
+			goto fail;
+		}
+
+		crtc  = mdp4_crtc_init(dev, plane, priv->num_crtcs, i,
+				mdp4_crtcs[i]);
+		if (IS_ERR(crtc)) {
+			dev_err(dev->dev, "failed to construct crtc for %s\n",
+				mdp4_crtc_names[i]);
+			ret = PTR_ERR(crtc);
+			goto fail;
+		}
+
+		priv->crtcs[priv->num_crtcs++] = crtc;
+	}
 
-	priv->crtcs[priv->num_crtcs++] = crtc;
-	priv->encoders[priv->num_encoders++] = encoder;
+	/*
+	 * we currently set up two relatively fixed paths:
+	 *
+	 * LCDC/LVDS path: RGB1 -> DMA_P -> LCDC -> LVDS
+	 *			or
+	 * DSI path: RGB1 -> DMA_P -> DSI1 -> DSI Panel
+	 *
+	 * DTV/HDMI path: RGB2 -> DMA_E -> DTV -> HDMI
+	 */
 
-	if (priv->hdmi) {
-		/* Construct bridge/connector for HDMI: */
-		ret = hdmi_modeset_init(priv->hdmi, dev, encoder);
+	for (i = 0; i < ARRAY_SIZE(mdp4_intfs); i++) {
+		ret = mdp4_modeset_init_intf(mdp4_kms, mdp4_intfs[i]);
 		if (ret) {
-			dev_err(dev->dev, "failed to initialize HDMI: %d\n", ret);
+			dev_err(dev->dev, "failed to initialize intf: %d, %d\n",
+				i, ret);
 			goto fail;
 		}
 	}
@@ -558,17 +617,10 @@ fail:
 static struct mdp4_platform_config *mdp4_get_config(struct platform_device *dev)
 {
 	static struct mdp4_platform_config config = {};
-#ifdef CONFIG_OF
-	/* TODO */
+
+	/* TODO: Chips that aren't apq8064 have a 200 Mhz max_clk */
 	config.max_clk = 266667000;
 	config.iommu = iommu_domain_alloc(&platform_bus_type);
-#else
-	if (cpu_is_apq8064())
-		config.max_clk = 266667000;
-	else
-		config.max_clk = 200000000;
-
-	config.iommu = msm_get_iommu_domain(DISPLAY_READ_DOMAIN);
-#endif
+
 	return &config;
 }
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
index 8a7f6e1e2bca..d2c96ef431f4 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
@@ -157,7 +157,7 @@ static inline uint32_t mixercfg(uint32_t mixer_cfg, int mixer,
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE6_MIXER1);
 		break;
 	default:
-		WARN_ON("invalid pipe");
+		WARN(1, "invalid pipe");
 		break;
 	}
 
@@ -212,10 +212,19 @@ struct drm_encoder *mdp4_dtv_encoder_init(struct drm_device *dev);
 
 long mdp4_lcdc_round_pixclk(struct drm_encoder *encoder, unsigned long rate);
 struct drm_encoder *mdp4_lcdc_encoder_init(struct drm_device *dev,
-		struct drm_panel *panel);
+		struct device_node *panel_node);
 
 struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
-		struct drm_panel *panel, struct drm_encoder *encoder);
+		struct device_node *panel_node, struct drm_encoder *encoder);
+
+#ifdef CONFIG_DRM_MSM_DSI
+struct drm_encoder *mdp4_dsi_encoder_init(struct drm_device *dev);
+#else
+static inline struct drm_encoder *mdp4_dsi_encoder_init(struct drm_device *dev)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif
 
 #ifdef CONFIG_COMMON_CLK
 struct clk *mpd4_lvds_pll_init(struct drm_device *dev);
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c
index 4cd6e721aa0a..cd63fedb67cc 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c
@@ -23,6 +23,7 @@
 
 struct mdp4_lcdc_encoder {
 	struct drm_encoder base;
+	struct device_node *panel_node;
 	struct drm_panel *panel;
 	struct clk *lcdc_clk;
 	unsigned long int pixclock;
@@ -338,7 +339,7 @@ static void mdp4_lcdc_encoder_disable(struct drm_encoder *encoder)
 	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder =
 			to_mdp4_lcdc_encoder(encoder);
 	struct mdp4_kms *mdp4_kms = get_kms(encoder);
-	struct drm_panel *panel = mdp4_lcdc_encoder->panel;
+	struct drm_panel *panel;
 	int i, ret;
 
 	if (WARN_ON(!mdp4_lcdc_encoder->enabled))
@@ -346,6 +347,7 @@ static void mdp4_lcdc_encoder_disable(struct drm_encoder *encoder)
 
 	mdp4_write(mdp4_kms, REG_MDP4_LCDC_ENABLE, 0);
 
+	panel = of_drm_find_panel(mdp4_lcdc_encoder->panel_node);
 	if (panel) {
 		drm_panel_disable(panel);
 		drm_panel_unprepare(panel);
@@ -381,7 +383,7 @@ static void mdp4_lcdc_encoder_enable(struct drm_encoder *encoder)
 			to_mdp4_lcdc_encoder(encoder);
 	unsigned long pc = mdp4_lcdc_encoder->pixclock;
 	struct mdp4_kms *mdp4_kms = get_kms(encoder);
-	struct drm_panel *panel = mdp4_lcdc_encoder->panel;
+	struct drm_panel *panel;
 	int i, ret;
 
 	if (WARN_ON(mdp4_lcdc_encoder->enabled))
@@ -414,6 +416,7 @@ static void mdp4_lcdc_encoder_enable(struct drm_encoder *encoder)
 	if (ret)
 		dev_err(dev->dev, "failed to enable lcdc_clk: %d\n", ret);
 
+	panel = of_drm_find_panel(mdp4_lcdc_encoder->panel_node);
 	if (panel) {
 		drm_panel_prepare(panel);
 		drm_panel_enable(panel);
@@ -442,7 +445,7 @@ long mdp4_lcdc_round_pixclk(struct drm_encoder *encoder, unsigned long rate)
 
 /* initialize encoder */
 struct drm_encoder *mdp4_lcdc_encoder_init(struct drm_device *dev,
-		struct drm_panel *panel)
+		struct device_node *panel_node)
 {
 	struct drm_encoder *encoder = NULL;
 	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder;
@@ -455,12 +458,12 @@ struct drm_encoder *mdp4_lcdc_encoder_init(struct drm_device *dev,
 		goto fail;
 	}
 
-	mdp4_lcdc_encoder->panel = panel;
+	mdp4_lcdc_encoder->panel_node = panel_node;
 
 	encoder = &mdp4_lcdc_encoder->base;
 
 	drm_encoder_init(dev, encoder, &mdp4_lcdc_encoder_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 	drm_encoder_helper_add(encoder, &mdp4_lcdc_encoder_helper_funcs);
 
 	/* TODO: do we need different pll in other cases? */
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
index 921185133d38..e73e1742b250 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
@@ -23,6 +23,7 @@
 struct mdp4_lvds_connector {
 	struct drm_connector base;
 	struct drm_encoder *encoder;
+	struct device_node *panel_node;
 	struct drm_panel *panel;
 };
 #define to_mdp4_lvds_connector(x) container_of(x, struct mdp4_lvds_connector, base)
@@ -33,6 +34,10 @@ static enum drm_connector_status mdp4_lvds_connector_detect(
 	struct mdp4_lvds_connector *mdp4_lvds_connector =
 			to_mdp4_lvds_connector(connector);
 
+	if (!mdp4_lvds_connector->panel)
+		mdp4_lvds_connector->panel =
+			of_drm_find_panel(mdp4_lvds_connector->panel_node);
+
 	return mdp4_lvds_connector->panel ?
 			connector_status_connected :
 			connector_status_disconnected;
@@ -42,10 +47,6 @@ static void mdp4_lvds_connector_destroy(struct drm_connector *connector)
 {
 	struct mdp4_lvds_connector *mdp4_lvds_connector =
 			to_mdp4_lvds_connector(connector);
-	struct drm_panel *panel = mdp4_lvds_connector->panel;
-
-	if (panel)
-		drm_panel_detach(panel);
 
 	drm_connector_unregister(connector);
 	drm_connector_cleanup(connector);
@@ -60,9 +61,14 @@ static int mdp4_lvds_connector_get_modes(struct drm_connector *connector)
 	struct drm_panel *panel = mdp4_lvds_connector->panel;
 	int ret = 0;
 
-	if (panel)
+	if (panel) {
+		drm_panel_attach(panel, connector);
+
 		ret = panel->funcs->get_modes(panel);
 
+		drm_panel_detach(panel);
+	}
+
 	return ret;
 }
 
@@ -111,7 +117,7 @@ static const struct drm_connector_helper_funcs mdp4_lvds_connector_helper_funcs
 
 /* initialize connector */
 struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
-		struct drm_panel *panel, struct drm_encoder *encoder)
+		struct device_node *panel_node, struct drm_encoder *encoder)
 {
 	struct drm_connector *connector = NULL;
 	struct mdp4_lvds_connector *mdp4_lvds_connector;
@@ -124,7 +130,7 @@ struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
 	}
 
 	mdp4_lvds_connector->encoder = encoder;
-	mdp4_lvds_connector->panel = panel;
+	mdp4_lvds_connector->panel_node = panel_node;
 
 	connector = &mdp4_lvds_connector->base;
 
@@ -141,9 +147,6 @@ struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
 
 	drm_mode_connector_attach_encoder(connector, encoder);
 
-	if (panel)
-		drm_panel_attach(panel, connector);
-
 	return connector;
 
 fail:
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
index 30d57e74c42f..9f96dfe67769 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_plane.c
@@ -397,7 +397,8 @@ struct drm_plane *mdp4_plane_init(struct drm_device *dev,
 
 	type = private_plane ? DRM_PLANE_TYPE_PRIMARY : DRM_PLANE_TYPE_OVERLAY;
 	ret = drm_universal_plane_init(dev, plane, 0xff, &mdp4_plane_funcs,
-				 mdp4_plane->formats, mdp4_plane->nformats, type);
+				 mdp4_plane->formats, mdp4_plane->nformats,
+				 type, NULL);
 	if (ret)
 		goto fail;
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c
index bb1225aa2f75..57f73f0c120d 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cfg.c
@@ -553,9 +553,7 @@ fail:
 static struct mdp5_cfg_platform *mdp5_get_config(struct platform_device *dev)
 {
 	static struct mdp5_cfg_platform config = {};
-#ifdef CONFIG_OF
-	/* TODO */
-#endif
+
 	config.iommu = iommu_domain_alloc(&platform_bus_type);
 
 	return &config;
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cmd_encoder.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cmd_encoder.c
index 8e6c9b598a57..1aa21dba663d 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cmd_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_cmd_encoder.c
@@ -326,7 +326,7 @@ struct drm_encoder *mdp5_cmd_encoder_init(struct drm_device *dev,
 	mdp5_cmd_enc->ctl = ctl;
 
 	drm_encoder_init(dev, encoder, &mdp5_cmd_encoder_funcs,
-			DRM_MODE_ENCODER_DSI);
+			DRM_MODE_ENCODER_DSI, NULL);
 
 	drm_encoder_helper_add(encoder, &mdp5_cmd_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
index 7f9f4ac88029..20cee5ce4071 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_crtc.c
@@ -797,7 +797,8 @@ struct drm_crtc *mdp5_crtc_init(struct drm_device *dev,
 	snprintf(mdp5_crtc->name, sizeof(mdp5_crtc->name), "%s:%d",
 			pipe2name(mdp5_plane_pipe(plane)), id);
 
-	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp5_crtc_funcs);
+	drm_crtc_init_with_planes(dev, crtc, plane, NULL, &mdp5_crtc_funcs,
+				  NULL);
 
 	drm_flip_work_init(&mdp5_crtc->unref_cursor_work,
 			"unref cursor", unref_cursor_worker);
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
index c9e32b08a7a0..0d737cad03a6 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_encoder.c
@@ -293,6 +293,24 @@ static const struct drm_encoder_helper_funcs mdp5_encoder_helper_funcs = {
 	.enable = mdp5_encoder_enable,
 };
 
+int mdp5_encoder_get_linecount(struct drm_encoder *encoder)
+{
+	struct mdp5_encoder *mdp5_encoder = to_mdp5_encoder(encoder);
+	struct mdp5_kms *mdp5_kms = get_kms(encoder);
+	int intf = mdp5_encoder->intf.num;
+
+	return mdp5_read(mdp5_kms, REG_MDP5_INTF_LINE_COUNT(intf));
+}
+
+u32 mdp5_encoder_get_framecount(struct drm_encoder *encoder)
+{
+	struct mdp5_encoder *mdp5_encoder = to_mdp5_encoder(encoder);
+	struct mdp5_kms *mdp5_kms = get_kms(encoder);
+	int intf = mdp5_encoder->intf.num;
+
+	return mdp5_read(mdp5_kms, REG_MDP5_INTF_FRAME_COUNT(intf));
+}
+
 int mdp5_encoder_set_split_display(struct drm_encoder *encoder,
 					struct drm_encoder *slave_encoder)
 {
@@ -354,7 +372,7 @@ struct drm_encoder *mdp5_encoder_init(struct drm_device *dev,
 
 	spin_lock_init(&mdp5_encoder->intf_lock);
 
-	drm_encoder_init(dev, encoder, &mdp5_encoder_funcs, enc_type);
+	drm_encoder_init(dev, encoder, &mdp5_encoder_funcs, enc_type, NULL);
 
 	drm_encoder_helper_add(encoder, &mdp5_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
index b0d4b53b97f4..73bc3e312fd4 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_irq.c
@@ -31,7 +31,7 @@ void mdp5_set_irqmask(struct mdp_kms *mdp_kms, uint32_t irqmask,
 
 static void mdp5_irq_error_handler(struct mdp_irq *irq, uint32_t irqstatus)
 {
-	DRM_ERROR("errors: %08x\n", irqstatus);
+	DRM_ERROR_RATELIMITED("errors: %08x\n", irqstatus);
 }
 
 void mdp5_irq_preinstall(struct msm_kms *kms)
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
index b532faa8026d..e115318402bd 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.c
@@ -468,6 +468,127 @@ static int get_clk(struct platform_device *pdev, struct clk **clkp,
 	return 0;
 }
 
+static struct drm_encoder *get_encoder_from_crtc(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_encoder *encoder;
+
+	drm_for_each_encoder(encoder, dev)
+		if (encoder->crtc == crtc)
+			return encoder;
+
+	return NULL;
+}
+
+static int mdp5_get_scanoutpos(struct drm_device *dev, unsigned int pipe,
+			       unsigned int flags, int *vpos, int *hpos,
+			       ktime_t *stime, ktime_t *etime,
+			       const struct drm_display_mode *mode)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	struct drm_encoder *encoder;
+	int line, vsw, vbp, vactive_start, vactive_end, vfp_end;
+	int ret = 0;
+
+	crtc = priv->crtcs[pipe];
+	if (!crtc) {
+		DRM_ERROR("Invalid crtc %d\n", pipe);
+		return 0;
+	}
+
+	encoder = get_encoder_from_crtc(crtc);
+	if (!encoder) {
+		DRM_ERROR("no encoder found for crtc %d\n", pipe);
+		return 0;
+	}
+
+	ret |= DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_ACCURATE;
+
+	vsw = mode->crtc_vsync_end - mode->crtc_vsync_start;
+	vbp = mode->crtc_vtotal - mode->crtc_vsync_end;
+
+	/*
+	 * the line counter is 1 at the start of the VSYNC pulse and VTOTAL at
+	 * the end of VFP. Translate the porch values relative to the line
+	 * counter positions.
+	 */
+
+	vactive_start = vsw + vbp + 1;
+
+	vactive_end = vactive_start + mode->crtc_vdisplay;
+
+	/* last scan line before VSYNC */
+	vfp_end = mode->crtc_vtotal;
+
+	if (stime)
+		*stime = ktime_get();
+
+	line = mdp5_encoder_get_linecount(encoder);
+
+	if (line < vactive_start) {
+		line -= vactive_start;
+		ret |= DRM_SCANOUTPOS_IN_VBLANK;
+	} else if (line > vactive_end) {
+		line = line - vfp_end - vactive_start;
+		ret |= DRM_SCANOUTPOS_IN_VBLANK;
+	} else {
+		line -= vactive_start;
+	}
+
+	*vpos = line;
+	*hpos = 0;
+
+	if (etime)
+		*etime = ktime_get();
+
+	return ret;
+}
+
+static int mdp5_get_vblank_timestamp(struct drm_device *dev, unsigned int pipe,
+				     int *max_error,
+				     struct timeval *vblank_time,
+				     unsigned flags)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc;
+
+	if (pipe < 0 || pipe >= priv->num_crtcs) {
+		DRM_ERROR("Invalid crtc %d\n", pipe);
+		return -EINVAL;
+	}
+
+	crtc = priv->crtcs[pipe];
+	if (!crtc) {
+		DRM_ERROR("Invalid crtc %d\n", pipe);
+		return -EINVAL;
+	}
+
+	return drm_calc_vbltimestamp_from_scanoutpos(dev, pipe, max_error,
+						     vblank_time, flags,
+						     &crtc->mode);
+}
+
+static u32 mdp5_get_vblank_counter(struct drm_device *dev, unsigned int pipe)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	struct drm_crtc *crtc;
+	struct drm_encoder *encoder;
+
+	if (pipe < 0 || pipe >= priv->num_crtcs)
+		return 0;
+
+	crtc = priv->crtcs[pipe];
+	if (!crtc)
+		return 0;
+
+	encoder = get_encoder_from_crtc(crtc);
+	if (!encoder)
+		return 0;
+
+	return mdp5_encoder_get_framecount(encoder);
+}
+
 struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 {
 	struct platform_device *pdev = dev->platformdev;
@@ -590,6 +711,8 @@ struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 				!config->hw->intf.base[i])
 			continue;
 		mdp5_write(mdp5_kms, REG_MDP5_INTF_TIMING_ENGINE_EN(i), 0);
+
+		mdp5_write(mdp5_kms, REG_MDP5_INTF_FRAME_LINE_COUNT_EN(i), 0x3);
 	}
 	mdp5_disable(mdp5_kms);
 	mdelay(16);
@@ -635,6 +758,12 @@ struct msm_kms *mdp5_kms_init(struct drm_device *dev)
 	dev->mode_config.max_width = config->hw->lm.max_width;
 	dev->mode_config.max_height = config->hw->lm.max_height;
 
+	dev->driver->get_vblank_timestamp = mdp5_get_vblank_timestamp;
+	dev->driver->get_scanout_position = mdp5_get_scanoutpos;
+	dev->driver->get_vblank_counter = mdp5_get_vblank_counter;
+	dev->max_vblank_count = 0xffffffff;
+	dev->vblank_disable_immediate = true;
+
 	return kms;
 
 fail:
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
index 84f65d415598..00730ba08a60 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_kms.h
@@ -222,6 +222,8 @@ struct drm_encoder *mdp5_encoder_init(struct drm_device *dev,
 		struct mdp5_interface *intf, struct mdp5_ctl *ctl);
 int mdp5_encoder_set_split_display(struct drm_encoder *encoder,
 					struct drm_encoder *slave_encoder);
+int mdp5_encoder_get_linecount(struct drm_encoder *encoder);
+u32 mdp5_encoder_get_framecount(struct drm_encoder *encoder);
 
 #ifdef CONFIG_DRM_MSM_DSI
 struct drm_encoder *mdp5_cmd_encoder_init(struct drm_device *dev,
diff --git a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
index 81cd49045ffc..432c09836b0e 100644
--- a/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
+++ b/drivers/gpu/drm/msm/mdp/mdp5/mdp5_plane.c
@@ -904,7 +904,7 @@ struct drm_plane *mdp5_plane_init(struct drm_device *dev,
 	type = private_plane ? DRM_PLANE_TYPE_PRIMARY : DRM_PLANE_TYPE_OVERLAY;
 	ret = drm_universal_plane_init(dev, plane, 0xff, &mdp5_plane_funcs,
 				 mdp5_plane->formats, mdp5_plane->nformats,
-				 type);
+				 type, NULL);
 	if (ret)
 		goto fail;
 
diff --git a/drivers/gpu/drm/msm/msm_drv.c b/drivers/gpu/drm/msm/msm_drv.c
index b88ce514eb8e..9a30807b900b 100644
--- a/drivers/gpu/drm/msm/msm_drv.c
+++ b/drivers/gpu/drm/msm/msm_drv.c
@@ -237,20 +237,9 @@ static int msm_unload(struct drm_device *dev)
 
 static int get_mdp_ver(struct platform_device *pdev)
 {
-#ifdef CONFIG_OF
-	static const struct of_device_id match_types[] = { {
-		.compatible = "qcom,mdss_mdp",
-		.data	= (void	*)5,
-	}, {
-		/* end node */
-	} };
 	struct device *dev = &pdev->dev;
-	const struct of_device_id *match;
-	match = of_match_node(match_types, dev->of_node);
-	if (match)
-		return (int)(unsigned long)match->data;
-#endif
-	return 4;
+
+	return (int) (unsigned long) of_device_get_match_data(dev);
 }
 
 #include <linux/of_address.h>
@@ -258,10 +247,10 @@ static int get_mdp_ver(struct platform_device *pdev)
 static int msm_init_vram(struct drm_device *dev)
 {
 	struct msm_drm_private *priv = dev->dev_private;
+	struct device_node *node;
 	unsigned long size = 0;
 	int ret = 0;
 
-#ifdef CONFIG_OF
 	/* In the device-tree world, we could have a 'memory-region'
 	 * phandle, which gives us a link to our "vram".  Allocating
 	 * is all nicely abstracted behind the dma api, but we need
@@ -278,7 +267,6 @@ static int msm_init_vram(struct drm_device *dev)
 	 *     as corruption on screen before we have a chance to
 	 *     load and do initial modeset)
 	 */
-	struct device_node *node;
 
 	node = of_parse_phandle(dev->dev->of_node, "memory-region", 0);
 	if (node) {
@@ -288,14 +276,12 @@ static int msm_init_vram(struct drm_device *dev)
 			return ret;
 		size = r.end - r.start;
 		DRM_INFO("using VRAM carveout: %lx@%pa\n", size, &r.start);
-	} else
-#endif
 
-	/* if we have no IOMMU, then we need to use carveout allocator.
-	 * Grab the entire CMA chunk carved out in early startup in
-	 * mach-msm:
-	 */
-	if (!iommu_present(&platform_bus_type)) {
+		/* if we have no IOMMU, then we need to use carveout allocator.
+		 * Grab the entire CMA chunk carved out in early startup in
+		 * mach-msm:
+		 */
+	} else if (!iommu_present(&platform_bus_type)) {
 		DRM_INFO("using %s VRAM carveout\n", vram);
 		size = memparse(vram, NULL);
 	}
@@ -1035,9 +1021,9 @@ static const struct dev_pm_ops msm_pm_ops = {
  * Componentized driver support:
  */
 
-#ifdef CONFIG_OF
-/* NOTE: the CONFIG_OF case duplicates the same code as exynos or imx
- * (or probably any other).. so probably some room for some helpers
+/*
+ * NOTE: duplication of the same code as exynos or imx (or probably any other).
+ * so probably some room for some helpers
  */
 static int compare_of(struct device *dev, void *data)
 {
@@ -1062,12 +1048,6 @@ static int add_components(struct device *dev, struct component_match **matchptr,
 
 	return 0;
 }
-#else
-static int compare_dev(struct device *dev, void *data)
-{
-	return dev == data;
-}
-#endif
 
 static int msm_drm_bind(struct device *dev)
 {
@@ -1091,35 +1071,9 @@ static const struct component_master_ops msm_drm_ops = {
 static int msm_pdev_probe(struct platform_device *pdev)
 {
 	struct component_match *match = NULL;
-#ifdef CONFIG_OF
+
 	add_components(&pdev->dev, &match, "connectors");
 	add_components(&pdev->dev, &match, "gpus");
-#else
-	/* For non-DT case, it kinda sucks.  We don't actually have a way
-	 * to know whether or not we are waiting for certain devices (or if
-	 * they are simply not present).  But for non-DT we only need to
-	 * care about apq8064/apq8060/etc (all mdp4/a3xx):
-	 */
-	static const char *devnames[] = {
-			"hdmi_msm.0", "kgsl-3d0.0",
-	};
-	int i;
-
-	DBG("Adding components..");
-
-	for (i = 0; i < ARRAY_SIZE(devnames); i++) {
-		struct device *dev;
-
-		dev = bus_find_device_by_name(&platform_bus_type,
-				NULL, devnames[i]);
-		if (!dev) {
-			dev_info(&pdev->dev, "still waiting for %s\n", devnames[i]);
-			return -EPROBE_DEFER;
-		}
-
-		component_match_add(&pdev->dev, &match, compare_dev, dev);
-	}
-#endif
 
 	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
 	return component_master_add_with_match(&pdev->dev, &msm_drm_ops, match);
@@ -1138,8 +1092,10 @@ static const struct platform_device_id msm_id[] = {
 };
 
 static const struct of_device_id dt_match[] = {
-	{ .compatible = "qcom,mdp" },      /* mdp4 */
-	{ .compatible = "qcom,mdss_mdp" }, /* mdp5 */
+	{ .compatible = "qcom,mdp4", .data = (void *) 4 },	/* mdp4 */
+	{ .compatible = "qcom,mdp5", .data = (void *) 5 },	/* mdp5 */
+	/* to support downstream DT files */
+	{ .compatible = "qcom,mdss_mdp", .data = (void *) 5 },  /* mdp5 */
 	{}
 };
 MODULE_DEVICE_TABLE(of, dt_match);
diff --git a/drivers/gpu/drm/msm/msm_drv.h b/drivers/gpu/drm/msm/msm_drv.h
index 3be7a56b14f1..c1e7bba2fdb7 100644
--- a/drivers/gpu/drm/msm/msm_drv.h
+++ b/drivers/gpu/drm/msm/msm_drv.h
@@ -31,14 +31,9 @@
 #include <linux/iommu.h>
 #include <linux/types.h>
 #include <linux/of_graph.h>
+#include <linux/of_device.h>
 #include <asm/sizes.h>
 
-#ifndef CONFIG_OF
-#include <mach/board.h>
-#include <mach/socinfo.h>
-#include <mach/iommu_domains.h>
-#endif
-
 #include <drm/drmP.h>
 #include <drm/drm_atomic.h>
 #include <drm/drm_atomic_helper.h>
@@ -240,9 +235,9 @@ uint32_t msm_framebuffer_iova(struct drm_framebuffer *fb, int id, int plane);
 struct drm_gem_object *msm_framebuffer_bo(struct drm_framebuffer *fb, int plane);
 const struct msm_format *msm_framebuffer_format(struct drm_framebuffer *fb);
 struct drm_framebuffer *msm_framebuffer_init(struct drm_device *dev,
-		struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos);
+		const struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos);
 struct drm_framebuffer *msm_framebuffer_create(struct drm_device *dev,
-		struct drm_file *file, struct drm_mode_fb_cmd2 *mode_cmd);
+		struct drm_file *file, const struct drm_mode_fb_cmd2 *mode_cmd);
 
 struct drm_fb_helper *msm_fbdev_init(struct drm_device *dev);
 
diff --git a/drivers/gpu/drm/msm/msm_fb.c b/drivers/gpu/drm/msm/msm_fb.c
index 121713281417..a474d6cf5d9f 100644
--- a/drivers/gpu/drm/msm/msm_fb.c
+++ b/drivers/gpu/drm/msm/msm_fb.c
@@ -138,7 +138,7 @@ const struct msm_format *msm_framebuffer_format(struct drm_framebuffer *fb)
 }
 
 struct drm_framebuffer *msm_framebuffer_create(struct drm_device *dev,
-		struct drm_file *file, struct drm_mode_fb_cmd2 *mode_cmd)
+		struct drm_file *file, const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *bos[4] = {0};
 	struct drm_framebuffer *fb;
@@ -168,7 +168,7 @@ out_unref:
 }
 
 struct drm_framebuffer *msm_framebuffer_init(struct drm_device *dev,
-		struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos)
+		const struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos)
 {
 	struct msm_drm_private *priv = dev->dev_private;
 	struct msm_kms *kms = priv->kms;
diff --git a/drivers/gpu/drm/msm/msm_fbdev.c b/drivers/gpu/drm/msm/msm_fbdev.c
index 3f6ec077b51d..d95af6eba602 100644
--- a/drivers/gpu/drm/msm/msm_fbdev.c
+++ b/drivers/gpu/drm/msm/msm_fbdev.c
@@ -121,7 +121,7 @@ static int msm_fbdev_create(struct drm_fb_helper *helper,
 		/* note: if fb creation failed, we can't rely on fb destroy
 		 * to unref the bo:
 		 */
-		drm_gem_object_unreference(fbdev->bo);
+		drm_gem_object_unreference_unlocked(fbdev->bo);
 		ret = PTR_ERR(fb);
 		goto fail;
 	}
diff --git a/drivers/gpu/drm/msm/msm_gem.c b/drivers/gpu/drm/msm/msm_gem.c
index c76cc853b08a..3cedb8d5c855 100644
--- a/drivers/gpu/drm/msm/msm_gem.c
+++ b/drivers/gpu/drm/msm/msm_gem.c
@@ -18,6 +18,7 @@
 #include <linux/spinlock.h>
 #include <linux/shmem_fs.h>
 #include <linux/dma-buf.h>
+#include <linux/pfn_t.h>
 
 #include "msm_drv.h"
 #include "msm_gem.h"
@@ -222,7 +223,8 @@ int msm_gem_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 	VERB("Inserting %p pfn %lx, pa %lx", vmf->virtual_address,
 			pfn, pfn << PAGE_SHIFT);
 
-	ret = vm_insert_mixed(vma, (unsigned long)vmf->virtual_address, pfn);
+	ret = vm_insert_mixed(vma, (unsigned long)vmf->virtual_address,
+			__pfn_to_pfn_t(pfn, PFN_DEV));
 
 out_unlock:
 	mutex_unlock(&dev->struct_mutex);
diff --git a/drivers/gpu/drm/nouveau/Kbuild b/drivers/gpu/drm/nouveau/Kbuild
index a34b437dbc8f..2527bf4ca5d9 100644
--- a/drivers/gpu/drm/nouveau/Kbuild
+++ b/drivers/gpu/drm/nouveau/Kbuild
@@ -24,7 +24,6 @@ nouveau-y += nouveau_hwmon.o
 nouveau-$(CONFIG_COMPAT) += nouveau_ioc32.o
 nouveau-y += nouveau_nvif.o
 nouveau-$(CONFIG_NOUVEAU_PLATFORM_DRIVER) += nouveau_platform.o
-nouveau-y += nouveau_sysfs.o
 nouveau-y += nouveau_usif.o # userspace <-> nvif
 nouveau-y += nouveau_vga.o
 
diff --git a/drivers/gpu/drm/nouveau/dispnv04/crtc.c b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
index 3d96b49fe662..6f04397d43a7 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/crtc.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
@@ -1081,8 +1081,6 @@ nouveau_crtc_set_config(struct drm_mode_set *set)
 }
 
 static const struct drm_crtc_funcs nv04_crtc_funcs = {
-	.save = nv_crtc_save,
-	.restore = nv_crtc_restore,
 	.cursor_set = nv04_crtc_cursor_set,
 	.cursor_move = nv04_crtc_cursor_move,
 	.gamma_set = nv_crtc_gamma_set,
@@ -1123,6 +1121,9 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	nv_crtc->index = crtc_num;
 	nv_crtc->last_dpms = NV_DPMS_CLEARED;
 
+	nv_crtc->save = nv_crtc_save;
+	nv_crtc->restore = nv_crtc_restore;
+
 	drm_crtc_init(dev, &nv_crtc->base, &nv04_crtc_funcs);
 	drm_crtc_helper_add(&nv_crtc->base, &nv04_crtc_helper_funcs);
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
diff --git a/drivers/gpu/drm/nouveau/dispnv04/dac.c b/drivers/gpu/drm/nouveau/dispnv04/dac.c
index 78cb033bc015..b48eec395f07 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/dac.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/dac.c
@@ -504,8 +504,6 @@ static void nv04_dac_destroy(struct drm_encoder *encoder)
 
 static const struct drm_encoder_helper_funcs nv04_dac_helper_funcs = {
 	.dpms = nv04_dac_dpms,
-	.save = nv04_dac_save,
-	.restore = nv04_dac_restore,
 	.mode_fixup = nv04_dac_mode_fixup,
 	.prepare = nv04_dac_prepare,
 	.commit = nv04_dac_commit,
@@ -515,8 +513,6 @@ static const struct drm_encoder_helper_funcs nv04_dac_helper_funcs = {
 
 static const struct drm_encoder_helper_funcs nv17_dac_helper_funcs = {
 	.dpms = nv04_dac_dpms,
-	.save = nv04_dac_save,
-	.restore = nv04_dac_restore,
 	.mode_fixup = nv04_dac_mode_fixup,
 	.prepare = nv04_dac_prepare,
 	.commit = nv04_dac_commit,
@@ -545,12 +541,16 @@ nv04_dac_create(struct drm_connector *connector, struct dcb_output *entry)
 	nv_encoder->dcb = entry;
 	nv_encoder->or = ffs(entry->or) - 1;
 
+	nv_encoder->enc_save = nv04_dac_save;
+	nv_encoder->enc_restore = nv04_dac_restore;
+
 	if (nv_gf4_disp_arch(dev))
 		helper = &nv17_dac_helper_funcs;
 	else
 		helper = &nv04_dac_helper_funcs;
 
-	drm_encoder_init(dev, encoder, &nv04_dac_funcs, DRM_MODE_ENCODER_DAC);
+	drm_encoder_init(dev, encoder, &nv04_dac_funcs, DRM_MODE_ENCODER_DAC,
+			 NULL);
 	drm_encoder_helper_add(encoder, helper);
 
 	encoder->possible_crtcs = entry->heads;
diff --git a/drivers/gpu/drm/nouveau/dispnv04/dfp.c b/drivers/gpu/drm/nouveau/dispnv04/dfp.c
index 429ab5e3025a..05bfd151d1d8 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/dfp.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/dfp.c
@@ -652,8 +652,6 @@ static void nv04_tmds_slave_init(struct drm_encoder *encoder)
 
 static const struct drm_encoder_helper_funcs nv04_lvds_helper_funcs = {
 	.dpms = nv04_lvds_dpms,
-	.save = nv04_dfp_save,
-	.restore = nv04_dfp_restore,
 	.mode_fixup = nv04_dfp_mode_fixup,
 	.prepare = nv04_dfp_prepare,
 	.commit = nv04_dfp_commit,
@@ -663,8 +661,6 @@ static const struct drm_encoder_helper_funcs nv04_lvds_helper_funcs = {
 
 static const struct drm_encoder_helper_funcs nv04_tmds_helper_funcs = {
 	.dpms = nv04_tmds_dpms,
-	.save = nv04_dfp_save,
-	.restore = nv04_dfp_restore,
 	.mode_fixup = nv04_dfp_mode_fixup,
 	.prepare = nv04_dfp_prepare,
 	.commit = nv04_dfp_commit,
@@ -701,12 +697,15 @@ nv04_dfp_create(struct drm_connector *connector, struct dcb_output *entry)
 	if (!nv_encoder)
 		return -ENOMEM;
 
+	nv_encoder->enc_save = nv04_dfp_save;
+	nv_encoder->enc_restore = nv04_dfp_restore;
+
 	encoder = to_drm_encoder(nv_encoder);
 
 	nv_encoder->dcb = entry;
 	nv_encoder->or = ffs(entry->or) - 1;
 
-	drm_encoder_init(connector->dev, encoder, &nv04_dfp_funcs, type);
+	drm_encoder_init(connector->dev, encoder, &nv04_dfp_funcs, type, NULL);
 	drm_encoder_helper_add(encoder, helper);
 
 	encoder->possible_crtcs = entry->heads;
diff --git a/drivers/gpu/drm/nouveau/dispnv04/disp.c b/drivers/gpu/drm/nouveau/dispnv04/disp.c
index 9e650081c357..b4a6bc433ef5 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/disp.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/disp.c
@@ -39,7 +39,8 @@ nv04_display_create(struct drm_device *dev)
 	struct dcb_table *dcb = &drm->vbios.dcb;
 	struct drm_connector *connector, *ct;
 	struct drm_encoder *encoder;
-	struct drm_crtc *crtc;
+	struct nouveau_encoder *nv_encoder;
+	struct nouveau_crtc *crtc;
 	struct nv04_display *disp;
 	int i, ret;
 
@@ -107,14 +108,11 @@ nv04_display_create(struct drm_device *dev)
 	}
 
 	/* Save previous state */
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		crtc->funcs->save(crtc);
-
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		const struct drm_encoder_helper_funcs *func = encoder->helper_private;
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, base.head)
+		crtc->save(&crtc->base);
 
-		func->save(encoder);
-	}
+	list_for_each_entry(nv_encoder, &dev->mode_config.encoder_list, base.base.head)
+		nv_encoder->enc_save(&nv_encoder->base.base);
 
 	nouveau_overlay_init(dev);
 
@@ -126,8 +124,9 @@ nv04_display_destroy(struct drm_device *dev)
 {
 	struct nv04_display *disp = nv04_display(dev);
 	struct nouveau_drm *drm = nouveau_drm(dev);
-	struct drm_encoder *encoder;
+	struct nouveau_encoder *encoder;
 	struct drm_crtc *crtc;
+	struct nouveau_crtc *nv_crtc;
 
 	/* Turn every CRTC off. */
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
@@ -139,14 +138,11 @@ nv04_display_destroy(struct drm_device *dev)
 	}
 
 	/* Restore state */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		const struct drm_encoder_helper_funcs *func = encoder->helper_private;
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.base.head)
+		encoder->enc_restore(&encoder->base.base);
 
-		func->restore(encoder);
-	}
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		crtc->funcs->restore(crtc);
+	list_for_each_entry(nv_crtc, &dev->mode_config.crtc_list, base.head)
+		nv_crtc->restore(&nv_crtc->base);
 
 	nouveau_hw_save_vga_fonts(dev, 0);
 
@@ -159,8 +155,8 @@ nv04_display_destroy(struct drm_device *dev)
 int
 nv04_display_init(struct drm_device *dev)
 {
-	struct drm_encoder *encoder;
-	struct drm_crtc *crtc;
+	struct nouveau_encoder *encoder;
+	struct nouveau_crtc *crtc;
 
 	/* meh.. modeset apparently doesn't setup all the regs and depends
 	 * on pre-existing state, for now load the state of the card *before*
@@ -170,14 +166,11 @@ nv04_display_init(struct drm_device *dev)
 	 * save/restore "pre-load" state, but more general so we can save
 	 * on suspend too.
 	 */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		const struct drm_encoder_helper_funcs *func = encoder->helper_private;
-
-		func->restore(encoder);
-	}
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, base.head)
+		crtc->save(&crtc->base);
 
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		crtc->funcs->restore(crtc);
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.base.head)
+		encoder->enc_save(&encoder->base.base);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c b/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c
index 5345eb5378a8..54e9fb9eb5c0 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvnv04.c
@@ -192,8 +192,6 @@ static const struct drm_encoder_funcs nv04_tv_funcs = {
 
 static const struct drm_encoder_helper_funcs nv04_tv_helper_funcs = {
 	.dpms = nv04_tv_dpms,
-	.save = drm_i2c_encoder_save,
-	.restore = drm_i2c_encoder_restore,
 	.mode_fixup = drm_i2c_encoder_mode_fixup,
 	.prepare = nv04_tv_prepare,
 	.commit = nv04_tv_commit,
@@ -225,9 +223,13 @@ nv04_tv_create(struct drm_connector *connector, struct dcb_output *entry)
 	/* Initialize the common members */
 	encoder = to_drm_encoder(nv_encoder);
 
-	drm_encoder_init(dev, encoder, &nv04_tv_funcs, DRM_MODE_ENCODER_TVDAC);
+	drm_encoder_init(dev, encoder, &nv04_tv_funcs, DRM_MODE_ENCODER_TVDAC,
+			 NULL);
 	drm_encoder_helper_add(encoder, &nv04_tv_helper_funcs);
 
+	nv_encoder->enc_save = drm_i2c_encoder_save;
+	nv_encoder->enc_restore = drm_i2c_encoder_restore;
+
 	encoder->possible_crtcs = entry->heads;
 	encoder->possible_clones = 0;
 	nv_encoder->dcb = entry;
diff --git a/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c
index b734195d80a0..163317d26de9 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/tvnv17.c
@@ -769,10 +769,8 @@ static void nv17_tv_destroy(struct drm_encoder *encoder)
 	kfree(tv_enc);
 }
 
-static struct drm_encoder_helper_funcs nv17_tv_helper_funcs = {
+static const struct drm_encoder_helper_funcs nv17_tv_helper_funcs = {
 	.dpms = nv17_tv_dpms,
-	.save = nv17_tv_save,
-	.restore = nv17_tv_restore,
 	.mode_fixup = nv17_tv_mode_fixup,
 	.prepare = nv17_tv_prepare,
 	.commit = nv17_tv_commit,
@@ -780,14 +778,14 @@ static struct drm_encoder_helper_funcs nv17_tv_helper_funcs = {
 	.detect = nv17_tv_detect,
 };
 
-static struct drm_encoder_slave_funcs nv17_tv_slave_funcs = {
+static const struct drm_encoder_slave_funcs nv17_tv_slave_funcs = {
 	.get_modes = nv17_tv_get_modes,
 	.mode_valid = nv17_tv_mode_valid,
 	.create_resources = nv17_tv_create_resources,
 	.set_property = nv17_tv_set_property,
 };
 
-static struct drm_encoder_funcs nv17_tv_funcs = {
+static const struct drm_encoder_funcs nv17_tv_funcs = {
 	.destroy = nv17_tv_destroy,
 };
 
@@ -816,10 +814,14 @@ nv17_tv_create(struct drm_connector *connector, struct dcb_output *entry)
 	tv_enc->base.dcb = entry;
 	tv_enc->base.or = ffs(entry->or) - 1;
 
-	drm_encoder_init(dev, encoder, &nv17_tv_funcs, DRM_MODE_ENCODER_TVDAC);
+	drm_encoder_init(dev, encoder, &nv17_tv_funcs, DRM_MODE_ENCODER_TVDAC,
+			 NULL);
 	drm_encoder_helper_add(encoder, &nv17_tv_helper_funcs);
 	to_encoder_slave(encoder)->slave_funcs = &nv17_tv_slave_funcs;
 
+	tv_enc->base.enc_save = nv17_tv_save;
+	tv_enc->base.enc_restore = nv17_tv_restore;
+
 	encoder->possible_crtcs = entry->heads;
 	encoder->possible_clones = 0;
 
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl0002.h b/drivers/gpu/drm/nouveau/include/nvif/cl0002.h
new file mode 100644
index 000000000000..6d72ed38da32
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl0002.h
@@ -0,0 +1,66 @@
+#ifndef __NVIF_CL0002_H__
+#define __NVIF_CL0002_H__
+
+struct nv_dma_v0 {
+	__u8  version;
+#define NV_DMA_V0_TARGET_VM                                                0x00
+#define NV_DMA_V0_TARGET_VRAM                                              0x01
+#define NV_DMA_V0_TARGET_PCI                                               0x02
+#define NV_DMA_V0_TARGET_PCI_US                                            0x03
+#define NV_DMA_V0_TARGET_AGP                                               0x04
+	__u8  target;
+#define NV_DMA_V0_ACCESS_VM                                                0x00
+#define NV_DMA_V0_ACCESS_RD                                                0x01
+#define NV_DMA_V0_ACCESS_WR                                                0x02
+#define NV_DMA_V0_ACCESS_RDWR                 (NV_DMA_V0_ACCESS_RD | NV_DMA_V0_ACCESS_WR)
+	__u8  access;
+	__u8  pad03[5];
+	__u64 start;
+	__u64 limit;
+	/* ... chipset-specific class data */
+};
+
+struct nv50_dma_v0 {
+	__u8  version;
+#define NV50_DMA_V0_PRIV_VM                                                0x00
+#define NV50_DMA_V0_PRIV_US                                                0x01
+#define NV50_DMA_V0_PRIV__S                                                0x02
+	__u8  priv;
+#define NV50_DMA_V0_PART_VM                                                0x00
+#define NV50_DMA_V0_PART_256                                               0x01
+#define NV50_DMA_V0_PART_1KB                                               0x02
+	__u8  part;
+#define NV50_DMA_V0_COMP_NONE                                              0x00
+#define NV50_DMA_V0_COMP_1                                                 0x01
+#define NV50_DMA_V0_COMP_2                                                 0x02
+#define NV50_DMA_V0_COMP_VM                                                0x03
+	__u8  comp;
+#define NV50_DMA_V0_KIND_PITCH                                             0x00
+#define NV50_DMA_V0_KIND_VM                                                0x7f
+	__u8  kind;
+	__u8  pad05[3];
+};
+
+struct gf100_dma_v0 {
+	__u8  version;
+#define GF100_DMA_V0_PRIV_VM                                               0x00
+#define GF100_DMA_V0_PRIV_US                                               0x01
+#define GF100_DMA_V0_PRIV__S                                               0x02
+	__u8  priv;
+#define GF100_DMA_V0_KIND_PITCH                                            0x00
+#define GF100_DMA_V0_KIND_VM                                               0xff
+	__u8  kind;
+	__u8  pad03[5];
+};
+
+struct gf119_dma_v0 {
+	__u8  version;
+#define GF119_DMA_V0_PAGE_LP                                               0x00
+#define GF119_DMA_V0_PAGE_SP                                               0x01
+	__u8  page;
+#define GF119_DMA_V0_KIND_PITCH                                            0x00
+#define GF119_DMA_V0_KIND_VM                                               0xff
+	__u8  kind;
+	__u8  pad03[5];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl0046.h b/drivers/gpu/drm/nouveau/include/nvif/cl0046.h
new file mode 100644
index 000000000000..a6a71f4ad91e
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl0046.h
@@ -0,0 +1,28 @@
+#ifndef __NVIF_CL0046_H__
+#define __NVIF_CL0046_H__
+
+#define NV04_DISP_NTFY_VBLANK                                              0x00
+#define NV04_DISP_NTFY_CONN                                                0x01
+
+struct nv04_disp_mthd_v0 {
+	__u8  version;
+#define NV04_DISP_SCANOUTPOS                                               0x00
+	__u8  method;
+	__u8  head;
+	__u8  pad03[5];
+};
+
+struct nv04_disp_scanoutpos_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__s64 time[2];
+	__u16 vblanks;
+	__u16 vblanke;
+	__u16 vtotal;
+	__u16 vline;
+	__u16 hblanks;
+	__u16 hblanke;
+	__u16 htotal;
+	__u16 hline;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl006b.h b/drivers/gpu/drm/nouveau/include/nvif/cl006b.h
new file mode 100644
index 000000000000..309ab8a3d9e8
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl006b.h
@@ -0,0 +1,11 @@
+#ifndef __NVIF_CL006B_H__
+#define __NVIF_CL006B_H__
+
+struct nv03_channel_dma_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[2];
+	__u32 offset;
+	__u64 pushbuf;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl0080.h b/drivers/gpu/drm/nouveau/include/nvif/cl0080.h
new file mode 100644
index 000000000000..331620a52afa
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl0080.h
@@ -0,0 +1,45 @@
+#ifndef __NVIF_CL0080_H__
+#define __NVIF_CL0080_H__
+
+struct nv_device_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__u64 device;	/* device identifier, ~0 for client default */
+};
+
+#define NV_DEVICE_V0_INFO                                                  0x00
+#define NV_DEVICE_V0_TIME                                                  0x01
+
+struct nv_device_info_v0 {
+	__u8  version;
+#define NV_DEVICE_INFO_V0_IGP                                              0x00
+#define NV_DEVICE_INFO_V0_PCI                                              0x01
+#define NV_DEVICE_INFO_V0_AGP                                              0x02
+#define NV_DEVICE_INFO_V0_PCIE                                             0x03
+#define NV_DEVICE_INFO_V0_SOC                                              0x04
+	__u8  platform;
+	__u16 chipset;	/* from NV_PMC_BOOT_0 */
+	__u8  revision;	/* from NV_PMC_BOOT_0 */
+#define NV_DEVICE_INFO_V0_TNT                                              0x01
+#define NV_DEVICE_INFO_V0_CELSIUS                                          0x02
+#define NV_DEVICE_INFO_V0_KELVIN                                           0x03
+#define NV_DEVICE_INFO_V0_RANKINE                                          0x04
+#define NV_DEVICE_INFO_V0_CURIE                                            0x05
+#define NV_DEVICE_INFO_V0_TESLA                                            0x06
+#define NV_DEVICE_INFO_V0_FERMI                                            0x07
+#define NV_DEVICE_INFO_V0_KEPLER                                           0x08
+#define NV_DEVICE_INFO_V0_MAXWELL                                          0x09
+	__u8  family;
+	__u8  pad06[2];
+	__u64 ram_size;
+	__u64 ram_user;
+	char  chip[16];
+	char  name[64];
+};
+
+struct nv_device_time_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__u64 time;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl506e.h b/drivers/gpu/drm/nouveau/include/nvif/cl506e.h
new file mode 100644
index 000000000000..aa94b8cf9679
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl506e.h
@@ -0,0 +1,12 @@
+#ifndef __NVIF_CL506E_H__
+#define __NVIF_CL506E_H__
+
+struct nv50_channel_dma_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[6];
+	__u64 vm;
+	__u64 pushbuf;
+	__u64 offset;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl506f.h b/drivers/gpu/drm/nouveau/include/nvif/cl506f.h
new file mode 100644
index 000000000000..3b7101966de4
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl506f.h
@@ -0,0 +1,13 @@
+#ifndef __NVIF_CL506F_H__
+#define __NVIF_CL506F_H__
+
+struct nv50_channel_gpfifo_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[2];
+	__u32 ilength;
+	__u64 ioffset;
+	__u64 pushbuf;
+	__u64 vm;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl5070.h b/drivers/gpu/drm/nouveau/include/nvif/cl5070.h
new file mode 100644
index 000000000000..d15c296b5f33
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl5070.h
@@ -0,0 +1,99 @@
+#ifndef __NVIF_CL5070_H__
+#define __NVIF_CL5070_H__
+
+#define NV50_DISP_MTHD                                                     0x00
+
+struct nv50_disp_mthd_v0 {
+	__u8  version;
+#define NV50_DISP_SCANOUTPOS                                               0x00
+	__u8  method;
+	__u8  head;
+	__u8  pad03[5];
+};
+
+struct nv50_disp_scanoutpos_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__s64 time[2];
+	__u16 vblanks;
+	__u16 vblanke;
+	__u16 vtotal;
+	__u16 vline;
+	__u16 hblanks;
+	__u16 hblanke;
+	__u16 htotal;
+	__u16 hline;
+};
+
+struct nv50_disp_mthd_v1 {
+	__u8  version;
+#define NV50_DISP_MTHD_V1_DAC_PWR                                          0x10
+#define NV50_DISP_MTHD_V1_DAC_LOAD                                         0x11
+#define NV50_DISP_MTHD_V1_SOR_PWR                                          0x20
+#define NV50_DISP_MTHD_V1_SOR_HDA_ELD                                      0x21
+#define NV50_DISP_MTHD_V1_SOR_HDMI_PWR                                     0x22
+#define NV50_DISP_MTHD_V1_SOR_LVDS_SCRIPT                                  0x23
+#define NV50_DISP_MTHD_V1_SOR_DP_PWR                                       0x24
+#define NV50_DISP_MTHD_V1_PIOR_PWR                                         0x30
+	__u8  method;
+	__u16 hasht;
+	__u16 hashm;
+	__u8  pad06[2];
+};
+
+struct nv50_disp_dac_pwr_v0 {
+	__u8  version;
+	__u8  state;
+	__u8  data;
+	__u8  vsync;
+	__u8  hsync;
+	__u8  pad05[3];
+};
+
+struct nv50_disp_dac_load_v0 {
+	__u8  version;
+	__u8  load;
+	__u8  pad02[2];
+	__u32 data;
+};
+
+struct nv50_disp_sor_pwr_v0 {
+	__u8  version;
+	__u8  state;
+	__u8  pad02[6];
+};
+
+struct nv50_disp_sor_hda_eld_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__u8  data[];
+};
+
+struct nv50_disp_sor_hdmi_pwr_v0 {
+	__u8  version;
+	__u8  state;
+	__u8  max_ac_packet;
+	__u8  rekey;
+	__u8  pad04[4];
+};
+
+struct nv50_disp_sor_lvds_script_v0 {
+	__u8  version;
+	__u8  pad01[1];
+	__u16 script;
+	__u8  pad04[4];
+};
+
+struct nv50_disp_sor_dp_pwr_v0 {
+	__u8  version;
+	__u8  state;
+	__u8  pad02[6];
+};
+
+struct nv50_disp_pior_pwr_v0 {
+	__u8  version;
+	__u8  state;
+	__u8  type;
+	__u8  pad03[5];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl507a.h b/drivers/gpu/drm/nouveau/include/nvif/cl507a.h
new file mode 100644
index 000000000000..12e0643b78bd
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl507a.h
@@ -0,0 +1,11 @@
+#ifndef __NVIF_CL507A_H__
+#define __NVIF_CL507A_H__
+
+struct nv50_disp_cursor_v0 {
+	__u8  version;
+	__u8  head;
+	__u8  pad02[6];
+};
+
+#define NV50_DISP_CURSOR_V0_NTFY_UEVENT                                    0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl507b.h b/drivers/gpu/drm/nouveau/include/nvif/cl507b.h
new file mode 100644
index 000000000000..99e9d8c47f60
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl507b.h
@@ -0,0 +1,11 @@
+#ifndef __NVIF_CL507B_H__
+#define __NVIF_CL507B_H__
+
+struct nv50_disp_overlay_v0 {
+	__u8  version;
+	__u8  head;
+	__u8  pad02[6];
+};
+
+#define NV50_DISP_OVERLAY_V0_NTFY_UEVENT                                   0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl507c.h b/drivers/gpu/drm/nouveau/include/nvif/cl507c.h
new file mode 100644
index 000000000000..6af70dbdfd9f
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl507c.h
@@ -0,0 +1,12 @@
+#ifndef __NVIF_CL507C_H__
+#define __NVIF_CL507C_H__
+
+struct nv50_disp_base_channel_dma_v0 {
+	__u8  version;
+	__u8  head;
+	__u8  pad02[6];
+	__u64 pushbuf;
+};
+
+#define NV50_DISP_BASE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl507d.h b/drivers/gpu/drm/nouveau/include/nvif/cl507d.h
new file mode 100644
index 000000000000..5ab0c9e4c6a3
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl507d.h
@@ -0,0 +1,11 @@
+#ifndef __NVIF_CL507D_H__
+#define __NVIF_CL507D_H__
+
+struct nv50_disp_core_channel_dma_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__u64 pushbuf;
+};
+
+#define NV50_DISP_CORE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl507e.h b/drivers/gpu/drm/nouveau/include/nvif/cl507e.h
new file mode 100644
index 000000000000..c06209f3cac4
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl507e.h
@@ -0,0 +1,12 @@
+#ifndef __NVIF_CL507E_H__
+#define __NVIF_CL507E_H__
+
+struct nv50_disp_overlay_channel_dma_v0 {
+	__u8  version;
+	__u8  head;
+	__u8  pad02[6];
+	__u64 pushbuf;
+};
+
+#define NV50_DISP_OVERLAY_CHANNEL_DMA_V0_NTFY_UEVENT                       0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl826e.h b/drivers/gpu/drm/nouveau/include/nvif/cl826e.h
new file mode 100644
index 000000000000..05e6ef7cd190
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl826e.h
@@ -0,0 +1,14 @@
+#ifndef __NVIF_CL826E_H__
+#define __NVIF_CL826E_H__
+
+struct g82_channel_dma_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[6];
+	__u64 vm;
+	__u64 pushbuf;
+	__u64 offset;
+};
+
+#define G82_CHANNEL_DMA_V0_NTFY_UEVENT                                     0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl826f.h b/drivers/gpu/drm/nouveau/include/nvif/cl826f.h
new file mode 100644
index 000000000000..cecafcb1e954
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl826f.h
@@ -0,0 +1,15 @@
+#ifndef __NVIF_CL826F_H__
+#define __NVIF_CL826F_H__
+
+struct g82_channel_gpfifo_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[2];
+	__u32 ilength;
+	__u64 ioffset;
+	__u64 pushbuf;
+	__u64 vm;
+};
+
+#define G82_CHANNEL_GPFIFO_V0_NTFY_UEVENT                                  0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl906f.h b/drivers/gpu/drm/nouveau/include/nvif/cl906f.h
new file mode 100644
index 000000000000..2caf0838fcfd
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl906f.h
@@ -0,0 +1,14 @@
+#ifndef __NVIF_CL906F_H__
+#define __NVIF_CL906F_H__
+
+struct fermi_channel_gpfifo_v0 {
+	__u8  version;
+	__u8  chid;
+	__u8  pad02[2];
+	__u32 ilength;
+	__u64 ioffset;
+	__u64 vm;
+};
+
+#define FERMI_CHANNEL_GPFIFO_V0_NTFY_UEVENT                                0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cl9097.h b/drivers/gpu/drm/nouveau/include/nvif/cl9097.h
new file mode 100644
index 000000000000..4057676d2981
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cl9097.h
@@ -0,0 +1,44 @@
+#ifndef __NVIF_CL9097_H__
+#define __NVIF_CL9097_H__
+
+#define FERMI_A_ZBC_COLOR                                                  0x00
+#define FERMI_A_ZBC_DEPTH                                                  0x01
+
+struct fermi_a_zbc_color_v0 {
+	__u8  version;
+#define FERMI_A_ZBC_COLOR_V0_FMT_ZERO                                      0x01
+#define FERMI_A_ZBC_COLOR_V0_FMT_UNORM_ONE                                 0x02
+#define FERMI_A_ZBC_COLOR_V0_FMT_RF32_GF32_BF32_AF32                       0x04
+#define FERMI_A_ZBC_COLOR_V0_FMT_R16_G16_B16_A16                           0x08
+#define FERMI_A_ZBC_COLOR_V0_FMT_RN16_GN16_BN16_AN16                       0x0c
+#define FERMI_A_ZBC_COLOR_V0_FMT_RS16_GS16_BS16_AS16                       0x10
+#define FERMI_A_ZBC_COLOR_V0_FMT_RU16_GU16_BU16_AU16                       0x14
+#define FERMI_A_ZBC_COLOR_V0_FMT_RF16_GF16_BF16_AF16                       0x16
+#define FERMI_A_ZBC_COLOR_V0_FMT_A8R8G8B8                                  0x18
+#define FERMI_A_ZBC_COLOR_V0_FMT_A8RL8GL8BL8                               0x1c
+#define FERMI_A_ZBC_COLOR_V0_FMT_A2B10G10R10                               0x20
+#define FERMI_A_ZBC_COLOR_V0_FMT_AU2BU10GU10RU10                           0x24
+#define FERMI_A_ZBC_COLOR_V0_FMT_A8B8G8R8                                  0x28
+#define FERMI_A_ZBC_COLOR_V0_FMT_A8BL8GL8RL8                               0x2c
+#define FERMI_A_ZBC_COLOR_V0_FMT_AN8BN8GN8RN8                              0x30
+#define FERMI_A_ZBC_COLOR_V0_FMT_AS8BS8GS8RS8                              0x34
+#define FERMI_A_ZBC_COLOR_V0_FMT_AU8BU8GU8RU8                              0x38
+#define FERMI_A_ZBC_COLOR_V0_FMT_A2R10G10B10                               0x3c
+#define FERMI_A_ZBC_COLOR_V0_FMT_BF10GF11RF11                              0x40
+	__u8  format;
+	__u8  index;
+	__u8  pad03[5];
+	__u32 ds[4];
+	__u32 l2[4];
+};
+
+struct fermi_a_zbc_depth_v0 {
+	__u8  version;
+#define FERMI_A_ZBC_DEPTH_V0_FMT_FP32                                      0x01
+	__u8  format;
+	__u8  index;
+	__u8  pad03[5];
+	__u32 ds;
+	__u32 l2;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/cla06f.h b/drivers/gpu/drm/nouveau/include/nvif/cla06f.h
new file mode 100644
index 000000000000..85b7827eb782
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/cla06f.h
@@ -0,0 +1,21 @@
+#ifndef __NVIF_CLA06F_H__
+#define __NVIF_CLA06F_H__
+
+struct kepler_channel_gpfifo_a_v0 {
+	__u8  version;
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_GR                               0x01
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSPDEC                           0x02
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSPPP                            0x04
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSVLD                            0x08
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_CE0                              0x10
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_CE1                              0x20
+#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_ENC                              0x40
+	__u8  engine;
+	__u16 chid;
+	__u32 ilength;
+	__u64 ioffset;
+	__u64 vm;
+};
+
+#define KEPLER_CHANNEL_GPFIFO_A_V0_NTFY_UEVENT                             0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/class.h b/drivers/gpu/drm/nouveau/include/nvif/class.h
index 95a64d89547c..4179cd65ac0a 100644
--- a/drivers/gpu/drm/nouveau/include/nvif/class.h
+++ b/drivers/gpu/drm/nouveau/include/nvif/class.h
@@ -1,16 +1,21 @@
 #ifndef __NVIF_CLASS_H__
 #define __NVIF_CLASS_H__
 
-/*******************************************************************************
- * class identifiers
- ******************************************************************************/
+/* these class numbers are made up by us, and not nvidia-assigned */
+#define NVIF_CLASS_CONTROL                                    /* if0001.h */ -1
+#define NVIF_CLASS_PERFMON                                    /* if0002.h */ -2
+#define NVIF_CLASS_PERFDOM                                    /* if0003.h */ -3
+#define NVIF_CLASS_SW_NV04                                    /* if0004.h */ -4
+#define NVIF_CLASS_SW_NV10                                    /* if0005.h */ -5
+#define NVIF_CLASS_SW_NV50                                    /* if0005.h */ -6
+#define NVIF_CLASS_SW_GF100                                   /* if0005.h */ -7
 
 /* the below match nvidia-assigned (either in hw, or sw) class numbers */
-#define NV_DEVICE                                                    0x00000080
+#define NV_DEVICE                                     /* cl0080.h */ 0x00000080
 
-#define NV_DMA_FROM_MEMORY                                           0x00000002
-#define NV_DMA_TO_MEMORY                                             0x00000003
-#define NV_DMA_IN_MEMORY                                             0x0000003d
+#define NV_DMA_FROM_MEMORY                            /* cl0002.h */ 0x00000002
+#define NV_DMA_TO_MEMORY                              /* cl0002.h */ 0x00000003
+#define NV_DMA_IN_MEMORY                              /* cl0002.h */ 0x0000003d
 
 #define FERMI_TWOD_A                                                 0x0000902d
 
@@ -19,85 +24,85 @@
 #define KEPLER_INLINE_TO_MEMORY_A                                    0x0000a040
 #define KEPLER_INLINE_TO_MEMORY_B                                    0x0000a140
 
-#define NV04_DISP                                                    0x00000046
-
-#define NV03_CHANNEL_DMA                                             0x0000006b
-#define NV10_CHANNEL_DMA                                             0x0000006e
-#define NV17_CHANNEL_DMA                                             0x0000176e
-#define NV40_CHANNEL_DMA                                             0x0000406e
-#define NV50_CHANNEL_DMA                                             0x0000506e
-#define G82_CHANNEL_DMA                                              0x0000826e
-
-#define NV50_CHANNEL_GPFIFO                                          0x0000506f
-#define G82_CHANNEL_GPFIFO                                           0x0000826f
-#define FERMI_CHANNEL_GPFIFO                                         0x0000906f
-#define KEPLER_CHANNEL_GPFIFO_A                                      0x0000a06f
-#define MAXWELL_CHANNEL_GPFIFO_A                                     0x0000b06f
-
-#define NV50_DISP                                                    0x00005070
-#define G82_DISP                                                     0x00008270
-#define GT200_DISP                                                   0x00008370
-#define GT214_DISP                                                   0x00008570
-#define GT206_DISP                                                   0x00008870
-#define GF110_DISP                                                   0x00009070
-#define GK104_DISP                                                   0x00009170
-#define GK110_DISP                                                   0x00009270
-#define GM107_DISP                                                   0x00009470
-#define GM204_DISP                                                   0x00009570
+#define NV04_DISP                                     /* cl0046.h */ 0x00000046
+
+#define NV03_CHANNEL_DMA                              /* cl506b.h */ 0x0000006b
+#define NV10_CHANNEL_DMA                              /* cl506b.h */ 0x0000006e
+#define NV17_CHANNEL_DMA                              /* cl506b.h */ 0x0000176e
+#define NV40_CHANNEL_DMA                              /* cl506b.h */ 0x0000406e
+#define NV50_CHANNEL_DMA                              /* cl506e.h */ 0x0000506e
+#define G82_CHANNEL_DMA                               /* cl826e.h */ 0x0000826e
+
+#define NV50_CHANNEL_GPFIFO                           /* cl506f.h */ 0x0000506f
+#define G82_CHANNEL_GPFIFO                            /* cl826f.h */ 0x0000826f
+#define FERMI_CHANNEL_GPFIFO                          /* cl906f.h */ 0x0000906f
+#define KEPLER_CHANNEL_GPFIFO_A                       /* cla06f.h */ 0x0000a06f
+#define MAXWELL_CHANNEL_GPFIFO_A                      /* cla06f.h */ 0x0000b06f
+
+#define NV50_DISP                                     /* cl5070.h */ 0x00005070
+#define G82_DISP                                      /* cl5070.h */ 0x00008270
+#define GT200_DISP                                    /* cl5070.h */ 0x00008370
+#define GT214_DISP                                    /* cl5070.h */ 0x00008570
+#define GT206_DISP                                    /* cl5070.h */ 0x00008870
+#define GF110_DISP                                    /* cl5070.h */ 0x00009070
+#define GK104_DISP                                    /* cl5070.h */ 0x00009170
+#define GK110_DISP                                    /* cl5070.h */ 0x00009270
+#define GM107_DISP                                    /* cl5070.h */ 0x00009470
+#define GM204_DISP                                    /* cl5070.h */ 0x00009570
 
 #define NV31_MPEG                                                    0x00003174
 #define G82_MPEG                                                     0x00008274
 
 #define NV74_VP2                                                     0x00007476
 
-#define NV50_DISP_CURSOR                                             0x0000507a
-#define G82_DISP_CURSOR                                              0x0000827a
-#define GT214_DISP_CURSOR                                            0x0000857a
-#define GF110_DISP_CURSOR                                            0x0000907a
-#define GK104_DISP_CURSOR                                            0x0000917a
-
-#define NV50_DISP_OVERLAY                                            0x0000507b
-#define G82_DISP_OVERLAY                                             0x0000827b
-#define GT214_DISP_OVERLAY                                           0x0000857b
-#define GF110_DISP_OVERLAY                                           0x0000907b
-#define GK104_DISP_OVERLAY                                           0x0000917b
-
-#define NV50_DISP_BASE_CHANNEL_DMA                                   0x0000507c
-#define G82_DISP_BASE_CHANNEL_DMA                                    0x0000827c
-#define GT200_DISP_BASE_CHANNEL_DMA                                  0x0000837c
-#define GT214_DISP_BASE_CHANNEL_DMA                                  0x0000857c
-#define GF110_DISP_BASE_CHANNEL_DMA                                  0x0000907c
-#define GK104_DISP_BASE_CHANNEL_DMA                                  0x0000917c
-#define GK110_DISP_BASE_CHANNEL_DMA                                  0x0000927c
-
-#define NV50_DISP_CORE_CHANNEL_DMA                                   0x0000507d
-#define G82_DISP_CORE_CHANNEL_DMA                                    0x0000827d
-#define GT200_DISP_CORE_CHANNEL_DMA                                  0x0000837d
-#define GT214_DISP_CORE_CHANNEL_DMA                                  0x0000857d
-#define GT206_DISP_CORE_CHANNEL_DMA                                  0x0000887d
-#define GF110_DISP_CORE_CHANNEL_DMA                                  0x0000907d
-#define GK104_DISP_CORE_CHANNEL_DMA                                  0x0000917d
-#define GK110_DISP_CORE_CHANNEL_DMA                                  0x0000927d
-#define GM107_DISP_CORE_CHANNEL_DMA                                  0x0000947d
-#define GM204_DISP_CORE_CHANNEL_DMA                                  0x0000957d
-
-#define NV50_DISP_OVERLAY_CHANNEL_DMA                                0x0000507e
-#define G82_DISP_OVERLAY_CHANNEL_DMA                                 0x0000827e
-#define GT200_DISP_OVERLAY_CHANNEL_DMA                               0x0000837e
-#define GT214_DISP_OVERLAY_CHANNEL_DMA                               0x0000857e
-#define GF110_DISP_OVERLAY_CONTROL_DMA                               0x0000907e
-#define GK104_DISP_OVERLAY_CONTROL_DMA                               0x0000917e
-
-#define FERMI_A                                                      0x00009097
-#define FERMI_B                                                      0x00009197
-#define FERMI_C                                                      0x00009297
-
-#define KEPLER_A                                                     0x0000a097
-#define KEPLER_B                                                     0x0000a197
-#define KEPLER_C                                                     0x0000a297
-
-#define MAXWELL_A                                                    0x0000b097
-#define MAXWELL_B                                                    0x0000b197
+#define NV50_DISP_CURSOR                              /* cl507a.h */ 0x0000507a
+#define G82_DISP_CURSOR                               /* cl507a.h */ 0x0000827a
+#define GT214_DISP_CURSOR                             /* cl507a.h */ 0x0000857a
+#define GF110_DISP_CURSOR                             /* cl507a.h */ 0x0000907a
+#define GK104_DISP_CURSOR                             /* cl507a.h */ 0x0000917a
+
+#define NV50_DISP_OVERLAY                             /* cl507b.h */ 0x0000507b
+#define G82_DISP_OVERLAY                              /* cl507b.h */ 0x0000827b
+#define GT214_DISP_OVERLAY                            /* cl507b.h */ 0x0000857b
+#define GF110_DISP_OVERLAY                            /* cl507b.h */ 0x0000907b
+#define GK104_DISP_OVERLAY                            /* cl507b.h */ 0x0000917b
+
+#define NV50_DISP_BASE_CHANNEL_DMA                    /* cl507c.h */ 0x0000507c
+#define G82_DISP_BASE_CHANNEL_DMA                     /* cl507c.h */ 0x0000827c
+#define GT200_DISP_BASE_CHANNEL_DMA                   /* cl507c.h */ 0x0000837c
+#define GT214_DISP_BASE_CHANNEL_DMA                   /* cl507c.h */ 0x0000857c
+#define GF110_DISP_BASE_CHANNEL_DMA                   /* cl507c.h */ 0x0000907c
+#define GK104_DISP_BASE_CHANNEL_DMA                   /* cl507c.h */ 0x0000917c
+#define GK110_DISP_BASE_CHANNEL_DMA                   /* cl507c.h */ 0x0000927c
+
+#define NV50_DISP_CORE_CHANNEL_DMA                    /* cl507d.h */ 0x0000507d
+#define G82_DISP_CORE_CHANNEL_DMA                     /* cl507d.h */ 0x0000827d
+#define GT200_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000837d
+#define GT214_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000857d
+#define GT206_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000887d
+#define GF110_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000907d
+#define GK104_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000917d
+#define GK110_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000927d
+#define GM107_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000947d
+#define GM204_DISP_CORE_CHANNEL_DMA                   /* cl507d.h */ 0x0000957d
+
+#define NV50_DISP_OVERLAY_CHANNEL_DMA                 /* cl507e.h */ 0x0000507e
+#define G82_DISP_OVERLAY_CHANNEL_DMA                  /* cl507e.h */ 0x0000827e
+#define GT200_DISP_OVERLAY_CHANNEL_DMA                /* cl507e.h */ 0x0000837e
+#define GT214_DISP_OVERLAY_CHANNEL_DMA                /* cl507e.h */ 0x0000857e
+#define GF110_DISP_OVERLAY_CONTROL_DMA                /* cl507e.h */ 0x0000907e
+#define GK104_DISP_OVERLAY_CONTROL_DMA                /* cl507e.h */ 0x0000917e
+
+#define FERMI_A                                       /* cl9097.h */ 0x00009097
+#define FERMI_B                                       /* cl9097.h */ 0x00009197
+#define FERMI_C                                       /* cl9097.h */ 0x00009297
+
+#define KEPLER_A                                      /* cl9097.h */ 0x0000a097
+#define KEPLER_B                                      /* cl9097.h */ 0x0000a197
+#define KEPLER_C                                      /* cl9097.h */ 0x0000a297
+
+#define MAXWELL_A                                     /* cl9097.h */ 0x0000b097
+#define MAXWELL_B                                     /* cl9097.h */ 0x0000b197
 
 #define NV74_BSP                                                     0x000074b0
 
@@ -133,540 +138,4 @@
 #define MAXWELL_COMPUTE_B                                            0x0000b1c0
 
 #define NV74_CIPHER                                                  0x000074c1
-
-/*******************************************************************************
- * client
- ******************************************************************************/
-
-#define NV_CLIENT_DEVLIST                                                  0x00
-
-struct nv_client_devlist_v0 {
-	__u8  version;
-	__u8  count;
-	__u8  pad02[6];
-	__u64 device[];
-};
-
-
-/*******************************************************************************
- * device
- ******************************************************************************/
-
-struct nv_device_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__u64 device;	/* device identifier, ~0 for client default */
-};
-
-#define NV_DEVICE_V0_INFO                                                  0x00
-#define NV_DEVICE_V0_TIME                                                  0x01
-
-struct nv_device_info_v0 {
-	__u8  version;
-#define NV_DEVICE_INFO_V0_IGP                                              0x00
-#define NV_DEVICE_INFO_V0_PCI                                              0x01
-#define NV_DEVICE_INFO_V0_AGP                                              0x02
-#define NV_DEVICE_INFO_V0_PCIE                                             0x03
-#define NV_DEVICE_INFO_V0_SOC                                              0x04
-	__u8  platform;
-	__u16 chipset;	/* from NV_PMC_BOOT_0 */
-	__u8  revision;	/* from NV_PMC_BOOT_0 */
-#define NV_DEVICE_INFO_V0_TNT                                              0x01
-#define NV_DEVICE_INFO_V0_CELSIUS                                          0x02
-#define NV_DEVICE_INFO_V0_KELVIN                                           0x03
-#define NV_DEVICE_INFO_V0_RANKINE                                          0x04
-#define NV_DEVICE_INFO_V0_CURIE                                            0x05
-#define NV_DEVICE_INFO_V0_TESLA                                            0x06
-#define NV_DEVICE_INFO_V0_FERMI                                            0x07
-#define NV_DEVICE_INFO_V0_KEPLER                                           0x08
-#define NV_DEVICE_INFO_V0_MAXWELL                                          0x09
-	__u8  family;
-	__u8  pad06[2];
-	__u64 ram_size;
-	__u64 ram_user;
-	char  chip[16];
-	char  name[64];
-};
-
-struct nv_device_time_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__u64 time;
-};
-
-
-/*******************************************************************************
- * context dma
- ******************************************************************************/
-
-struct nv_dma_v0 {
-	__u8  version;
-#define NV_DMA_V0_TARGET_VM                                                0x00
-#define NV_DMA_V0_TARGET_VRAM                                              0x01
-#define NV_DMA_V0_TARGET_PCI                                               0x02
-#define NV_DMA_V0_TARGET_PCI_US                                            0x03
-#define NV_DMA_V0_TARGET_AGP                                               0x04
-	__u8  target;
-#define NV_DMA_V0_ACCESS_VM                                                0x00
-#define NV_DMA_V0_ACCESS_RD                                                0x01
-#define NV_DMA_V0_ACCESS_WR                                                0x02
-#define NV_DMA_V0_ACCESS_RDWR                 (NV_DMA_V0_ACCESS_RD | NV_DMA_V0_ACCESS_WR)
-	__u8  access;
-	__u8  pad03[5];
-	__u64 start;
-	__u64 limit;
-	/* ... chipset-specific class data */
-};
-
-struct nv50_dma_v0 {
-	__u8  version;
-#define NV50_DMA_V0_PRIV_VM                                                0x00
-#define NV50_DMA_V0_PRIV_US                                                0x01
-#define NV50_DMA_V0_PRIV__S                                                0x02
-	__u8  priv;
-#define NV50_DMA_V0_PART_VM                                                0x00
-#define NV50_DMA_V0_PART_256                                               0x01
-#define NV50_DMA_V0_PART_1KB                                               0x02
-	__u8  part;
-#define NV50_DMA_V0_COMP_NONE                                              0x00
-#define NV50_DMA_V0_COMP_1                                                 0x01
-#define NV50_DMA_V0_COMP_2                                                 0x02
-#define NV50_DMA_V0_COMP_VM                                                0x03
-	__u8  comp;
-#define NV50_DMA_V0_KIND_PITCH                                             0x00
-#define NV50_DMA_V0_KIND_VM                                                0x7f
-	__u8  kind;
-	__u8  pad05[3];
-};
-
-struct gf100_dma_v0 {
-	__u8  version;
-#define GF100_DMA_V0_PRIV_VM                                               0x00
-#define GF100_DMA_V0_PRIV_US                                               0x01
-#define GF100_DMA_V0_PRIV__S                                               0x02
-	__u8  priv;
-#define GF100_DMA_V0_KIND_PITCH                                            0x00
-#define GF100_DMA_V0_KIND_VM                                               0xff
-	__u8  kind;
-	__u8  pad03[5];
-};
-
-struct gf119_dma_v0 {
-	__u8  version;
-#define GF119_DMA_V0_PAGE_LP                                               0x00
-#define GF119_DMA_V0_PAGE_SP                                               0x01
-	__u8  page;
-#define GF119_DMA_V0_KIND_PITCH                                            0x00
-#define GF119_DMA_V0_KIND_VM                                               0xff
-	__u8  kind;
-	__u8  pad03[5];
-};
-
-
-/*******************************************************************************
- * perfmon
- ******************************************************************************/
-
-#define NVIF_PERFMON_V0_QUERY_DOMAIN                                       0x00
-#define NVIF_PERFMON_V0_QUERY_SIGNAL                                       0x01
-#define NVIF_PERFMON_V0_QUERY_SOURCE                                       0x02
-
-struct nvif_perfmon_query_domain_v0 {
-	__u8  version;
-	__u8  id;
-	__u8  counter_nr;
-	__u8  iter;
-	__u16 signal_nr;
-	__u8  pad05[2];
-	char  name[64];
-};
-
-struct nvif_perfmon_query_signal_v0 {
-	__u8  version;
-	__u8  domain;
-	__u16 iter;
-	__u8  signal;
-	__u8  source_nr;
-	__u8  pad05[2];
-	char  name[64];
-};
-
-struct nvif_perfmon_query_source_v0 {
-	__u8  version;
-	__u8  domain;
-	__u8  signal;
-	__u8  iter;
-	__u8  pad04[4];
-	__u32 source;
-	__u32 mask;
-	char  name[64];
-};
-
-
-/*******************************************************************************
- * perfdom
- ******************************************************************************/
-
-struct nvif_perfdom_v0 {
-	__u8  version;
-	__u8  domain;
-	__u8  mode;
-	__u8  pad03[1];
-	struct {
-		__u8  signal[4];
-		__u64 source[4][8];
-		__u16 logic_op;
-	} ctr[4];
-};
-
-#define NVIF_PERFDOM_V0_INIT                                               0x00
-#define NVIF_PERFDOM_V0_SAMPLE                                             0x01
-#define NVIF_PERFDOM_V0_READ                                               0x02
-
-struct nvif_perfdom_init {
-};
-
-struct nvif_perfdom_sample {
-};
-
-struct nvif_perfdom_read_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__u32 ctr[4];
-	__u32 clk;
-	__u8  pad04[4];
-};
-
-
-/*******************************************************************************
- * device control
- ******************************************************************************/
-
-#define NVIF_CONTROL_PSTATE_INFO                                           0x00
-#define NVIF_CONTROL_PSTATE_ATTR                                           0x01
-#define NVIF_CONTROL_PSTATE_USER                                           0x02
-
-struct nvif_control_pstate_info_v0 {
-	__u8  version;
-	__u8  count; /* out: number of power states */
-#define NVIF_CONTROL_PSTATE_INFO_V0_USTATE_DISABLE                         (-1)
-#define NVIF_CONTROL_PSTATE_INFO_V0_USTATE_PERFMON                         (-2)
-	__s8  ustate_ac; /* out: target pstate index */
-	__s8  ustate_dc; /* out: target pstate index */
-	__s8  pwrsrc; /* out: current power source */
-#define NVIF_CONTROL_PSTATE_INFO_V0_PSTATE_UNKNOWN                         (-1)
-#define NVIF_CONTROL_PSTATE_INFO_V0_PSTATE_PERFMON                         (-2)
-	__s8  pstate; /* out: current pstate index */
-	__u8  pad06[2];
-};
-
-struct nvif_control_pstate_attr_v0 {
-	__u8  version;
-#define NVIF_CONTROL_PSTATE_ATTR_V0_STATE_CURRENT                          (-1)
-	__s8  state; /*  in: index of pstate to query
-		      * out: pstate identifier
-		      */
-	__u8  index; /*  in: index of attribute to query
-		      * out: index of next attribute, or 0 if no more
-		      */
-	__u8  pad03[5];
-	__u32 min;
-	__u32 max;
-	char  name[32];
-	char  unit[16];
-};
-
-struct nvif_control_pstate_user_v0 {
-	__u8  version;
-#define NVIF_CONTROL_PSTATE_USER_V0_STATE_UNKNOWN                          (-1)
-#define NVIF_CONTROL_PSTATE_USER_V0_STATE_PERFMON                          (-2)
-	__s8  ustate; /*  in: pstate identifier */
-	__s8  pwrsrc; /*  in: target power source */
-	__u8  pad03[5];
-};
-
-
-/*******************************************************************************
- * DMA FIFO channels
- ******************************************************************************/
-
-struct nv03_channel_dma_v0 {
-	__u8  version;
-	__u8  chid;
-	__u8  pad02[2];
-	__u32 offset;
-	__u64 pushbuf;
-};
-
-struct nv50_channel_dma_v0 {
-	__u8  version;
-	__u8  chid;
-	__u8  pad02[6];
-	__u64 vm;
-	__u64 pushbuf;
-	__u64 offset;
-};
-
-#define G82_CHANNEL_DMA_V0_NTFY_UEVENT                                     0x00
-
-/*******************************************************************************
- * GPFIFO channels
- ******************************************************************************/
-
-struct nv50_channel_gpfifo_v0 {
-	__u8  version;
-	__u8  chid;
-	__u8  pad02[2];
-	__u32 ilength;
-	__u64 ioffset;
-	__u64 pushbuf;
-	__u64 vm;
-};
-
-struct fermi_channel_gpfifo_v0 {
-	__u8  version;
-	__u8  chid;
-	__u8  pad02[2];
-	__u32 ilength;
-	__u64 ioffset;
-	__u64 vm;
-};
-
-struct kepler_channel_gpfifo_a_v0 {
-	__u8  version;
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_GR                               0x01
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSPDEC                           0x02
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSPPP                            0x04
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_MSVLD                            0x08
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_CE0                              0x10
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_CE1                              0x20
-#define KEPLER_CHANNEL_GPFIFO_A_V0_ENGINE_ENC                              0x40
-	__u8  engine;
-	__u16 chid;
-	__u32 ilength;
-	__u64 ioffset;
-	__u64 vm;
-};
-
-/*******************************************************************************
- * legacy display
- ******************************************************************************/
-
-#define NV04_DISP_NTFY_VBLANK                                              0x00
-#define NV04_DISP_NTFY_CONN                                                0x01
-
-struct nv04_disp_mthd_v0 {
-	__u8  version;
-#define NV04_DISP_SCANOUTPOS                                               0x00
-	__u8  method;
-	__u8  head;
-	__u8  pad03[5];
-};
-
-struct nv04_disp_scanoutpos_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__s64 time[2];
-	__u16 vblanks;
-	__u16 vblanke;
-	__u16 vtotal;
-	__u16 vline;
-	__u16 hblanks;
-	__u16 hblanke;
-	__u16 htotal;
-	__u16 hline;
-};
-
-/*******************************************************************************
- * display
- ******************************************************************************/
-
-#define NV50_DISP_MTHD                                                     0x00
-
-struct nv50_disp_mthd_v0 {
-	__u8  version;
-#define NV50_DISP_SCANOUTPOS                                               0x00
-	__u8  method;
-	__u8  head;
-	__u8  pad03[5];
-};
-
-struct nv50_disp_mthd_v1 {
-	__u8  version;
-#define NV50_DISP_MTHD_V1_DAC_PWR                                          0x10
-#define NV50_DISP_MTHD_V1_DAC_LOAD                                         0x11
-#define NV50_DISP_MTHD_V1_SOR_PWR                                          0x20
-#define NV50_DISP_MTHD_V1_SOR_HDA_ELD                                      0x21
-#define NV50_DISP_MTHD_V1_SOR_HDMI_PWR                                     0x22
-#define NV50_DISP_MTHD_V1_SOR_LVDS_SCRIPT                                  0x23
-#define NV50_DISP_MTHD_V1_SOR_DP_PWR                                       0x24
-#define NV50_DISP_MTHD_V1_PIOR_PWR                                         0x30
-	__u8  method;
-	__u16 hasht;
-	__u16 hashm;
-	__u8  pad06[2];
-};
-
-struct nv50_disp_dac_pwr_v0 {
-	__u8  version;
-	__u8  state;
-	__u8  data;
-	__u8  vsync;
-	__u8  hsync;
-	__u8  pad05[3];
-};
-
-struct nv50_disp_dac_load_v0 {
-	__u8  version;
-	__u8  load;
-	__u8  pad02[2];
-	__u32 data;
-};
-
-struct nv50_disp_sor_pwr_v0 {
-	__u8  version;
-	__u8  state;
-	__u8  pad02[6];
-};
-
-struct nv50_disp_sor_hda_eld_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__u8  data[];
-};
-
-struct nv50_disp_sor_hdmi_pwr_v0 {
-	__u8  version;
-	__u8  state;
-	__u8  max_ac_packet;
-	__u8  rekey;
-	__u8  pad04[4];
-};
-
-struct nv50_disp_sor_lvds_script_v0 {
-	__u8  version;
-	__u8  pad01[1];
-	__u16 script;
-	__u8  pad04[4];
-};
-
-struct nv50_disp_sor_dp_pwr_v0 {
-	__u8  version;
-	__u8  state;
-	__u8  pad02[6];
-};
-
-struct nv50_disp_pior_pwr_v0 {
-	__u8  version;
-	__u8  state;
-	__u8  type;
-	__u8  pad03[5];
-};
-
-/* core */
-struct nv50_disp_core_channel_dma_v0 {
-	__u8  version;
-	__u8  pad01[7];
-	__u64 pushbuf;
-};
-
-#define NV50_DISP_CORE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
-
-/* cursor immediate */
-struct nv50_disp_cursor_v0 {
-	__u8  version;
-	__u8  head;
-	__u8  pad02[6];
-};
-
-#define NV50_DISP_CURSOR_V0_NTFY_UEVENT                                    0x00
-
-/* base */
-struct nv50_disp_base_channel_dma_v0 {
-	__u8  version;
-	__u8  head;
-	__u8  pad02[6];
-	__u64 pushbuf;
-};
-
-#define NV50_DISP_BASE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
-
-/* overlay */
-struct nv50_disp_overlay_channel_dma_v0 {
-	__u8  version;
-	__u8  head;
-	__u8  pad02[6];
-	__u64 pushbuf;
-};
-
-#define NV50_DISP_OVERLAY_CHANNEL_DMA_V0_NTFY_UEVENT                       0x00
-
-/* overlay immediate */
-struct nv50_disp_overlay_v0 {
-	__u8  version;
-	__u8  head;
-	__u8  pad02[6];
-};
-
-#define NV50_DISP_OVERLAY_V0_NTFY_UEVENT                                   0x00
-
-/*******************************************************************************
- * software
- ******************************************************************************/
-
-#define NVSW_NTFY_UEVENT                                                   0x00
-
-#define NV04_NVSW_GET_REF                                                  0x00
-
-struct nv04_nvsw_get_ref_v0 {
-	__u8  version;
-	__u8  pad01[3];
-	__u32 ref;
-};
-
-/*******************************************************************************
- * fermi
- ******************************************************************************/
-
-#define FERMI_A_ZBC_COLOR                                                  0x00
-#define FERMI_A_ZBC_DEPTH                                                  0x01
-
-struct fermi_a_zbc_color_v0 {
-	__u8  version;
-#define FERMI_A_ZBC_COLOR_V0_FMT_ZERO                                      0x01
-#define FERMI_A_ZBC_COLOR_V0_FMT_UNORM_ONE                                 0x02
-#define FERMI_A_ZBC_COLOR_V0_FMT_RF32_GF32_BF32_AF32                       0x04
-#define FERMI_A_ZBC_COLOR_V0_FMT_R16_G16_B16_A16                           0x08
-#define FERMI_A_ZBC_COLOR_V0_FMT_RN16_GN16_BN16_AN16                       0x0c
-#define FERMI_A_ZBC_COLOR_V0_FMT_RS16_GS16_BS16_AS16                       0x10
-#define FERMI_A_ZBC_COLOR_V0_FMT_RU16_GU16_BU16_AU16                       0x14
-#define FERMI_A_ZBC_COLOR_V0_FMT_RF16_GF16_BF16_AF16                       0x16
-#define FERMI_A_ZBC_COLOR_V0_FMT_A8R8G8B8                                  0x18
-#define FERMI_A_ZBC_COLOR_V0_FMT_A8RL8GL8BL8                               0x1c
-#define FERMI_A_ZBC_COLOR_V0_FMT_A2B10G10R10                               0x20
-#define FERMI_A_ZBC_COLOR_V0_FMT_AU2BU10GU10RU10                           0x24
-#define FERMI_A_ZBC_COLOR_V0_FMT_A8B8G8R8                                  0x28
-#define FERMI_A_ZBC_COLOR_V0_FMT_A8BL8GL8RL8                               0x2c
-#define FERMI_A_ZBC_COLOR_V0_FMT_AN8BN8GN8RN8                              0x30
-#define FERMI_A_ZBC_COLOR_V0_FMT_AS8BS8GS8RS8                              0x34
-#define FERMI_A_ZBC_COLOR_V0_FMT_AU8BU8GU8RU8                              0x38
-#define FERMI_A_ZBC_COLOR_V0_FMT_A2R10G10B10                               0x3c
-#define FERMI_A_ZBC_COLOR_V0_FMT_BF10GF11RF11                              0x40
-	__u8  format;
-	__u8  index;
-	__u8  pad03[5];
-	__u32 ds[4];
-	__u32 l2[4];
-};
-
-struct fermi_a_zbc_depth_v0 {
-	__u8  version;
-#define FERMI_A_ZBC_DEPTH_V0_FMT_FP32                                      0x01
-	__u8  format;
-	__u8  index;
-	__u8  pad03[5];
-	__u32 ds;
-	__u32 l2;
-};
-
 #endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/device.h b/drivers/gpu/drm/nouveau/include/nvif/device.h
index 700a9b206726..e0ed2f4b2f43 100644
--- a/drivers/gpu/drm/nouveau/include/nvif/device.h
+++ b/drivers/gpu/drm/nouveau/include/nvif/device.h
@@ -2,7 +2,7 @@
 #define __NVIF_DEVICE_H__
 
 #include <nvif/object.h>
-#include <nvif/class.h>
+#include <nvif/cl0080.h>
 
 struct nvif_device {
 	struct nvif_object object;
@@ -63,6 +63,7 @@ u64  nvif_device_time(struct nvif_device *);
 #define nvxx_clk(a) nvxx_device(a)->clk
 #define nvxx_i2c(a) nvxx_device(a)->i2c
 #define nvxx_therm(a) nvxx_device(a)->therm
+#define nvxx_volt(a) nvxx_device(a)->volt
 
 #include <core/device.h>
 #include <engine/fifo.h>
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0000.h b/drivers/gpu/drm/nouveau/include/nvif/if0000.h
new file mode 100644
index 000000000000..85c44e8a1201
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0000.h
@@ -0,0 +1,12 @@
+#ifndef __NVIF_IF0000_H__
+#define __NVIF_IF0000_H__
+
+#define NV_CLIENT_DEVLIST                                                  0x00
+
+struct nv_client_devlist_v0 {
+	__u8  version;
+	__u8  count;
+	__u8  pad02[6];
+	__u64 device[];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0001.h b/drivers/gpu/drm/nouveau/include/nvif/if0001.h
new file mode 100644
index 000000000000..bd5b64125eed
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0001.h
@@ -0,0 +1,46 @@
+#ifndef __NVIF_IF0001_H__
+#define __NVIF_IF0001_H__
+
+#define NVIF_CONTROL_PSTATE_INFO                                           0x00
+#define NVIF_CONTROL_PSTATE_ATTR                                           0x01
+#define NVIF_CONTROL_PSTATE_USER                                           0x02
+
+struct nvif_control_pstate_info_v0 {
+	__u8  version;
+	__u8  count; /* out: number of power states */
+#define NVIF_CONTROL_PSTATE_INFO_V0_USTATE_DISABLE                         (-1)
+#define NVIF_CONTROL_PSTATE_INFO_V0_USTATE_PERFMON                         (-2)
+	__s8  ustate_ac; /* out: target pstate index */
+	__s8  ustate_dc; /* out: target pstate index */
+	__s8  pwrsrc; /* out: current power source */
+#define NVIF_CONTROL_PSTATE_INFO_V0_PSTATE_UNKNOWN                         (-1)
+#define NVIF_CONTROL_PSTATE_INFO_V0_PSTATE_PERFMON                         (-2)
+	__s8  pstate; /* out: current pstate index */
+	__u8  pad06[2];
+};
+
+struct nvif_control_pstate_attr_v0 {
+	__u8  version;
+#define NVIF_CONTROL_PSTATE_ATTR_V0_STATE_CURRENT                          (-1)
+	__s8  state; /*  in: index of pstate to query
+		      * out: pstate identifier
+		      */
+	__u8  index; /*  in: index of attribute to query
+		      * out: index of next attribute, or 0 if no more
+		      */
+	__u8  pad03[5];
+	__u32 min;
+	__u32 max;
+	char  name[32];
+	char  unit[16];
+};
+
+struct nvif_control_pstate_user_v0 {
+	__u8  version;
+#define NVIF_CONTROL_PSTATE_USER_V0_STATE_UNKNOWN                          (-1)
+#define NVIF_CONTROL_PSTATE_USER_V0_STATE_PERFMON                          (-2)
+	__s8  ustate; /*  in: pstate identifier */
+	__s8  pwrsrc; /*  in: target power source */
+	__u8  pad03[5];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0002.h b/drivers/gpu/drm/nouveau/include/nvif/if0002.h
new file mode 100644
index 000000000000..c04c91d0b818
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0002.h
@@ -0,0 +1,38 @@
+#ifndef __NVIF_IF0002_H__
+#define __NVIF_IF0002_H__
+
+#define NVIF_PERFMON_V0_QUERY_DOMAIN                                       0x00
+#define NVIF_PERFMON_V0_QUERY_SIGNAL                                       0x01
+#define NVIF_PERFMON_V0_QUERY_SOURCE                                       0x02
+
+struct nvif_perfmon_query_domain_v0 {
+	__u8  version;
+	__u8  id;
+	__u8  counter_nr;
+	__u8  iter;
+	__u16 signal_nr;
+	__u8  pad05[2];
+	char  name[64];
+};
+
+struct nvif_perfmon_query_signal_v0 {
+	__u8  version;
+	__u8  domain;
+	__u16 iter;
+	__u8  signal;
+	__u8  source_nr;
+	__u8  pad05[2];
+	char  name[64];
+};
+
+struct nvif_perfmon_query_source_v0 {
+	__u8  version;
+	__u8  domain;
+	__u8  signal;
+	__u8  iter;
+	__u8  pad04[4];
+	__u32 source;
+	__u32 mask;
+	char  name[64];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0003.h b/drivers/gpu/drm/nouveau/include/nvif/if0003.h
new file mode 100644
index 000000000000..0cd03efb80a1
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0003.h
@@ -0,0 +1,33 @@
+#ifndef __NVIF_IF0003_H__
+#define __NVIF_IF0003_H__
+
+struct nvif_perfdom_v0 {
+	__u8  version;
+	__u8  domain;
+	__u8  mode;
+	__u8  pad03[1];
+	struct {
+		__u8  signal[4];
+		__u64 source[4][8];
+		__u16 logic_op;
+	} ctr[4];
+};
+
+#define NVIF_PERFDOM_V0_INIT                                               0x00
+#define NVIF_PERFDOM_V0_SAMPLE                                             0x01
+#define NVIF_PERFDOM_V0_READ                                               0x02
+
+struct nvif_perfdom_init {
+};
+
+struct nvif_perfdom_sample {
+};
+
+struct nvif_perfdom_read_v0 {
+	__u8  version;
+	__u8  pad01[7];
+	__u32 ctr[4];
+	__u32 clk;
+	__u8  pad04[4];
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0004.h b/drivers/gpu/drm/nouveau/include/nvif/if0004.h
new file mode 100644
index 000000000000..bd5cd428cfd7
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0004.h
@@ -0,0 +1,13 @@
+#ifndef __NVIF_IF0004_H__
+#define __NVIF_IF0004_H__
+
+#define NV04_NVSW_NTFY_UEVENT                                              0x00
+
+#define NV04_NVSW_GET_REF                                                  0x00
+
+struct nv04_nvsw_get_ref_v0 {
+	__u8  version;
+	__u8  pad01[3];
+	__u32 ref;
+};
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/if0005.h b/drivers/gpu/drm/nouveau/include/nvif/if0005.h
new file mode 100644
index 000000000000..abfd373bb68b
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/include/nvif/if0005.h
@@ -0,0 +1,4 @@
+#ifndef __NVIF_IF0005_H__
+#define __NVIF_IF0005_H__
+#define NV10_NVSW_NTFY_UEVENT                                              0x00
+#endif
diff --git a/drivers/gpu/drm/nouveau/include/nvif/ioctl.h b/drivers/gpu/drm/nouveau/include/nvif/ioctl.h
index b0ac0215ebf9..c5f5eb83a594 100644
--- a/drivers/gpu/drm/nouveau/include/nvif/ioctl.h
+++ b/drivers/gpu/drm/nouveau/include/nvif/ioctl.h
@@ -55,14 +55,6 @@ struct nvif_ioctl_new_v0 {
 	__u64 token;
 	__u64 object;
 	__u32 handle;
-/* these class numbers are made up by us, and not nvidia-assigned */
-#define NVIF_IOCTL_NEW_V0_CONTROL                                            -1
-#define NVIF_IOCTL_NEW_V0_PERFMON                                            -2
-#define NVIF_IOCTL_NEW_V0_PERFDOM                                            -3
-#define NVIF_IOCTL_NEW_V0_SW_NV04                                            -4
-#define NVIF_IOCTL_NEW_V0_SW_NV10                                            -5
-#define NVIF_IOCTL_NEW_V0_SW_NV50                                            -6
-#define NVIF_IOCTL_NEW_V0_SW_GF100                                           -7
 	__s32 oclass;
 	__u8  data[];		/* class data (class.h) */
 };
diff --git a/drivers/gpu/drm/nouveau/include/nvif/unpack.h b/drivers/gpu/drm/nouveau/include/nvif/unpack.h
index 5933188b4a77..751bcf4930a7 100644
--- a/drivers/gpu/drm/nouveau/include/nvif/unpack.h
+++ b/drivers/gpu/drm/nouveau/include/nvif/unpack.h
@@ -1,24 +1,28 @@
 #ifndef __NVIF_UNPACK_H__
 #define __NVIF_UNPACK_H__
 
-#define nvif_unvers(d) ({                                                      \
-	ret = (size == sizeof(d)) ? 0 : -ENOSYS;                               \
-	(ret == 0);                                                            \
+#define nvif_unvers(r,d,s,m) ({                                                \
+	void **_data = (d); __u32 *_size = (s); int _ret = (r);                \
+	if (_ret == -ENOSYS && *_size == sizeof(m)) {                          \
+		*_data = NULL;                                                 \
+		*_size = _ret = 0;                                             \
+	}                                                                      \
+	_ret;                                                                  \
 })
 
-#define nvif_unpack(d,vl,vh,m) ({                                              \
-	if ((vl) == 0 || ret == -ENOSYS) {                                     \
-		int _size = sizeof(d);                                         \
-		if (_size <= size && (d).version >= (vl) &&                    \
-				     (d).version <= (vh)) {                    \
-			data = (u8 *)data + _size;                             \
-			size = size - _size;                                   \
-			ret = ((m) || !size) ? 0 : -E2BIG;                     \
-		} else {                                                       \
-			ret = -ENOSYS;                                         \
+#define nvif_unpack(r,d,s,m,vl,vh,x) ({                                        \
+	void **_data = (d); __u32 *_size = (s);                                \
+	int _ret = (r), _vl = (vl), _vh = (vh);                                \
+	if (_ret == -ENOSYS && *_size >= sizeof(m) &&                          \
+	    (m).version >= _vl && (m).version <= _vh) {                        \
+		*_data = (__u8 *)*_data + sizeof(m);                           \
+		*_size = *_size - sizeof(m);                                   \
+		if (_ret = 0, !(x)) {                                          \
+			_ret = *_size ? -E2BIG : 0;                            \
+			*_data = NULL;                                         \
+			*_size = 0;                                            \
 		}                                                              \
 	}                                                                      \
-	(ret == 0);                                                            \
+	_ret;                                                                  \
 })
-
 #endif
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/subdev/bios/perf.h b/drivers/gpu/drm/nouveau/include/nvkm/subdev/bios/perf.h
index 7cc2becabc69..d3bd250103d5 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/subdev/bios/perf.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/subdev/bios/perf.h
@@ -13,6 +13,8 @@ struct nvbios_perfE {
 	u32 vdec;
 	u32 disp;
 	u32 script;
+	u8  pcie_speed;
+	u8  pcie_width;
 };
 
 u16 nvbios_perf_entry(struct nvkm_bios *, int idx,
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/subdev/clk.h b/drivers/gpu/drm/nouveau/include/nvkm/subdev/clk.h
index 8708f0a4e188..6b33bc058924 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/subdev/clk.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/subdev/clk.h
@@ -2,6 +2,7 @@
 #define __NVKM_CLK_H__
 #include <core/subdev.h>
 #include <core/notify.h>
+#include <subdev/pci.h>
 struct nvbios_pll;
 struct nvkm_pll_vals;
 
@@ -38,7 +39,7 @@ enum nv_clk_src {
 	nv_clk_src_hubk06,
 	nv_clk_src_hubk07,
 	nv_clk_src_copy,
-	nv_clk_src_daemon,
+	nv_clk_src_pmu,
 	nv_clk_src_disp,
 	nv_clk_src_vdec,
 
@@ -59,6 +60,8 @@ struct nvkm_pstate {
 	struct nvkm_cstate base;
 	u8 pstate;
 	u8 fanspeed;
+	enum nvkm_pcie_speed pcie_speed;
+	u8 pcie_width;
 };
 
 struct nvkm_domain {
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/subdev/ibus.h b/drivers/gpu/drm/nouveau/include/nvkm/subdev/ibus.h
index c4dcd2680fe1..ea23e24a246c 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/subdev/ibus.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/subdev/ibus.h
@@ -6,4 +6,5 @@ int gf100_ibus_new(struct nvkm_device *, int, struct nvkm_subdev **);
 int gf117_ibus_new(struct nvkm_device *, int, struct nvkm_subdev **);
 int gk104_ibus_new(struct nvkm_device *, int, struct nvkm_subdev **);
 int gk20a_ibus_new(struct nvkm_device *, int, struct nvkm_subdev **);
+int gm204_ibus_new(struct nvkm_device *, int, struct nvkm_subdev **);
 #endif
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/subdev/ltc.h b/drivers/gpu/drm/nouveau/include/nvkm/subdev/ltc.h
index 3d4dbbf9aab3..0ffa2ec106d6 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/subdev/ltc.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/subdev/ltc.h
@@ -37,4 +37,5 @@ int gf100_ltc_new(struct nvkm_device *, int, struct nvkm_ltc **);
 int gk104_ltc_new(struct nvkm_device *, int, struct nvkm_ltc **);
 int gk20a_ltc_new(struct nvkm_device *, int, struct nvkm_ltc **);
 int gm107_ltc_new(struct nvkm_device *, int, struct nvkm_ltc **);
+int gm204_ltc_new(struct nvkm_device *, int, struct nvkm_ltc **);
 #endif
diff --git a/drivers/gpu/drm/nouveau/include/nvkm/subdev/pci.h b/drivers/gpu/drm/nouveau/include/nvkm/subdev/pci.h
index fee0a97c44c5..ddb913889d7e 100644
--- a/drivers/gpu/drm/nouveau/include/nvkm/subdev/pci.h
+++ b/drivers/gpu/drm/nouveau/include/nvkm/subdev/pci.h
@@ -2,6 +2,12 @@
 #define __NVKM_PCI_H__
 #include <core/subdev.h>
 
+enum nvkm_pcie_speed {
+	NVKM_PCIE_SPEED_2_5,
+	NVKM_PCIE_SPEED_5_0,
+	NVKM_PCIE_SPEED_8_0,
+};
+
 struct nvkm_pci {
 	const struct nvkm_pci_func *func;
 	struct nvkm_subdev subdev;
@@ -18,6 +24,11 @@ struct nvkm_pci {
 		bool acquired;
 	} agp;
 
+	struct {
+		enum nvkm_pcie_speed speed;
+		u8 width;
+	} pcie;
+
 	bool msi;
 };
 
@@ -34,4 +45,9 @@ int nv4c_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
 int g84_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
 int g94_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
 int gf100_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
+int gf106_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
+int gk104_pci_new(struct nvkm_device *, int, struct nvkm_pci **);
+
+/* pcie functions */
+int nvkm_pcie_set_link(struct nvkm_pci *, enum nvkm_pcie_speed, u8 width);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index 7f50cf5f929e..50f52ffe5b0c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -25,6 +25,8 @@
 #include <nvif/driver.h>
 #include <nvif/ioctl.h>
 #include <nvif/class.h>
+#include <nvif/cl0002.h>
+#include <nvif/cla06f.h>
 #include <nvif/unpack.h>
 
 #include "nouveau_drm.h"
@@ -87,18 +89,18 @@ nouveau_abi16_swclass(struct nouveau_drm *drm)
 {
 	switch (drm->device.info.family) {
 	case NV_DEVICE_INFO_V0_TNT:
-		return NVIF_IOCTL_NEW_V0_SW_NV04;
+		return NVIF_CLASS_SW_NV04;
 	case NV_DEVICE_INFO_V0_CELSIUS:
 	case NV_DEVICE_INFO_V0_KELVIN:
 	case NV_DEVICE_INFO_V0_RANKINE:
 	case NV_DEVICE_INFO_V0_CURIE:
-		return NVIF_IOCTL_NEW_V0_SW_NV10;
+		return NVIF_CLASS_SW_NV10;
 	case NV_DEVICE_INFO_V0_TESLA:
-		return NVIF_IOCTL_NEW_V0_SW_NV50;
+		return NVIF_CLASS_SW_NV50;
 	case NV_DEVICE_INFO_V0_FERMI:
 	case NV_DEVICE_INFO_V0_KEPLER:
 	case NV_DEVICE_INFO_V0_MAXWELL:
-		return NVIF_IOCTL_NEW_V0_SW_GF100;
+		return NVIF_CLASS_SW_GF100;
 	}
 
 	return 0x0000;
@@ -355,9 +357,9 @@ nouveau_abi16_usif(struct drm_file *file_priv, void *data, u32 size)
 	} *args = data;
 	struct nouveau_abi16_chan *chan;
 	struct nouveau_abi16 *abi16;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		switch (args->v0.type) {
 		case NVIF_IOCTL_V0_NEW:
 		case NVIF_IOCTL_V0_MTHD:
@@ -433,10 +435,10 @@ nouveau_abi16_ioctl_grobj_alloc(ABI16_IOCTL_ARGS)
 		/* nvsw: compatibility with older 0x*6e class identifier */
 		for (i = 0; !oclass && i < ret; i++) {
 			switch (sclass[i].oclass) {
-			case NVIF_IOCTL_NEW_V0_SW_NV04:
-			case NVIF_IOCTL_NEW_V0_SW_NV10:
-			case NVIF_IOCTL_NEW_V0_SW_NV50:
-			case NVIF_IOCTL_NEW_V0_SW_GF100:
+			case NVIF_CLASS_SW_NV04:
+			case NVIF_CLASS_SW_NV10:
+			case NVIF_CLASS_SW_NV50:
+			case NVIF_CLASS_SW_GF100:
 				oclass = sclass[i].oclass;
 				break;
 			default:
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 78f520d05de9..e3acc35e3805 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -1520,7 +1520,7 @@ nouveau_ttm_tt_populate(struct ttm_tt *ttm)
 				    DMA_BIDIRECTIONAL);
 
 		if (dma_mapping_error(pdev, addr)) {
-			while (--i) {
+			while (i--) {
 				dma_unmap_page(pdev, ttm_dma->dma_address[i],
 					       PAGE_SIZE, DMA_BIDIRECTIONAL);
 				ttm_dma->dma_address[i] = 0;
diff --git a/drivers/gpu/drm/nouveau/nouveau_chan.c b/drivers/gpu/drm/nouveau/nouveau_chan.c
index 1860f389f21f..3f804a8c590c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_chan.c
+++ b/drivers/gpu/drm/nouveau/nouveau_chan.c
@@ -24,6 +24,11 @@
 
 #include <nvif/os.h>
 #include <nvif/class.h>
+#include <nvif/cl0002.h>
+#include <nvif/cl006b.h>
+#include <nvif/cl506f.h>
+#include <nvif/cl906f.h>
+#include <nvif/cla06f.h>
 #include <nvif/ioctl.h>
 
 /*XXX*/
@@ -378,7 +383,7 @@ nouveau_channel_init(struct nouveau_channel *chan, u32 vram, u32 gart)
 	/* allocate software object class (used for fences on <= nv05) */
 	if (device->info.family < NV_DEVICE_INFO_V0_CELSIUS) {
 		ret = nvif_object_init(&chan->user, 0x006e,
-				       NVIF_IOCTL_NEW_V0_SW_NV04,
+				       NVIF_CLASS_SW_NV04,
 				       NULL, 0, &chan->nvsw);
 		if (ret)
 			return ret;
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index 2e7cbe933533..fcebfae5d426 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -42,6 +42,8 @@
 #include "nouveau_encoder.h"
 #include "nouveau_crtc.h"
 
+#include <nvif/class.h>
+#include <nvif/cl0046.h>
 #include <nvif/event.h>
 
 MODULE_PARM_DESC(tv_disable, "Disable TV-out detection");
@@ -56,6 +58,10 @@ MODULE_PARM_DESC(duallink, "Allow dual-link TMDS (default: enabled)");
 int nouveau_duallink = 1;
 module_param_named(duallink, nouveau_duallink, int, 0400);
 
+MODULE_PARM_DESC(hdmimhz, "Force a maximum HDMI pixel clock (in MHz)");
+int nouveau_hdmimhz = 0;
+module_param_named(hdmimhz, nouveau_hdmimhz, int, 0400);
+
 struct nouveau_encoder *
 find_encoder(struct drm_connector *connector, int type)
 {
@@ -809,12 +815,23 @@ nouveau_connector_get_modes(struct drm_connector *connector)
 }
 
 static unsigned
-get_tmds_link_bandwidth(struct drm_connector *connector)
+get_tmds_link_bandwidth(struct drm_connector *connector, bool hdmi)
 {
 	struct nouveau_connector *nv_connector = nouveau_connector(connector);
 	struct nouveau_drm *drm = nouveau_drm(connector->dev);
 	struct dcb_output *dcb = nv_connector->detected_encoder->dcb;
 
+	if (hdmi) {
+		if (nouveau_hdmimhz > 0)
+			return nouveau_hdmimhz * 1000;
+		/* Note: these limits are conservative, some Fermi's
+		 * can do 297 MHz. Unclear how this can be determined.
+		 */
+		if (drm->device.info.family >= NV_DEVICE_INFO_V0_KEPLER)
+			return 297000;
+		if (drm->device.info.family >= NV_DEVICE_INFO_V0_FERMI)
+			return 225000;
+	}
 	if (dcb->location != DCB_LOC_ON_CHIP ||
 	    drm->device.info.chipset >= 0x46)
 		return 165000;
@@ -835,6 +852,7 @@ nouveau_connector_mode_valid(struct drm_connector *connector,
 	struct drm_encoder *encoder = to_drm_encoder(nv_encoder);
 	unsigned min_clock = 25000, max_clock = min_clock;
 	unsigned clock = mode->clock;
+	bool hdmi;
 
 	switch (nv_encoder->dcb->type) {
 	case DCB_OUTPUT_LVDS:
@@ -847,8 +865,10 @@ nouveau_connector_mode_valid(struct drm_connector *connector,
 		max_clock = 400000;
 		break;
 	case DCB_OUTPUT_TMDS:
-		max_clock = get_tmds_link_bandwidth(connector);
-		if (nouveau_duallink && nv_encoder->dcb->duallink_possible)
+		hdmi = drm_detect_hdmi_monitor(nv_connector->edid);
+		max_clock = get_tmds_link_bandwidth(connector, hdmi);
+		if (!hdmi && nouveau_duallink &&
+		    nv_encoder->dcb->duallink_possible)
 			max_clock *= 2;
 		break;
 	case DCB_OUTPUT_ANALOG:
@@ -898,8 +918,6 @@ nouveau_connector_helper_funcs = {
 static const struct drm_connector_funcs
 nouveau_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = NULL,
-	.restore = NULL,
 	.detect = nouveau_connector_detect,
 	.destroy = nouveau_connector_destroy,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -910,8 +928,6 @@ nouveau_connector_funcs = {
 static const struct drm_connector_funcs
 nouveau_connector_funcs_lvds = {
 	.dpms = drm_helper_connector_dpms,
-	.save = NULL,
-	.restore = NULL,
 	.detect = nouveau_connector_detect_lvds,
 	.destroy = nouveau_connector_destroy,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -944,8 +960,6 @@ nouveau_connector_dp_dpms(struct drm_connector *connector, int mode)
 static const struct drm_connector_funcs
 nouveau_connector_funcs_dp = {
 	.dpms = nouveau_connector_dp_dpms,
-	.save = NULL,
-	.restore = NULL,
 	.detect = nouveau_connector_detect,
 	.destroy = nouveau_connector_destroy,
 	.fill_modes = drm_helper_probe_single_connector_modes,
@@ -969,10 +983,13 @@ nouveau_connector_hotplug(struct nvif_notify *notify)
 
 		NV_DEBUG(drm, "%splugged %s\n", plugged ? "" : "un", name);
 
+		mutex_lock(&drm->dev->mode_config.mutex);
 		if (plugged)
 			drm_helper_connector_dpms(connector, DRM_MODE_DPMS_ON);
 		else
 			drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
+		mutex_unlock(&drm->dev->mode_config.mutex);
+
 		drm_helper_hpd_irq_event(connector->dev);
 	}
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_crtc.h b/drivers/gpu/drm/nouveau/nouveau_crtc.h
index f19cb1c5fc5a..863f10b8d818 100644
--- a/drivers/gpu/drm/nouveau/nouveau_crtc.h
+++ b/drivers/gpu/drm/nouveau/nouveau_crtc.h
@@ -73,6 +73,9 @@ struct nouveau_crtc {
 	int (*set_dither)(struct nouveau_crtc *crtc, bool update);
 	int (*set_scale)(struct nouveau_crtc *crtc, bool update);
 	int (*set_color_vibrance)(struct nouveau_crtc *crtc, bool update);
+
+	void (*save)(struct drm_crtc *crtc);
+	void (*restore)(struct drm_crtc *crtc);
 };
 
 static inline struct nouveau_crtc *nouveau_crtc(struct drm_crtc *crtc)
diff --git a/drivers/gpu/drm/nouveau/nouveau_debugfs.c b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
index 5392e07edfc6..3d0dc199b253 100644
--- a/drivers/gpu/drm/nouveau/nouveau_debugfs.c
+++ b/drivers/gpu/drm/nouveau/nouveau_debugfs.c
@@ -28,6 +28,9 @@
  *  Ben Skeggs <bskeggs@redhat.com>
  */
 
+#include <linux/debugfs.h>
+#include <nvif/class.h>
+#include <nvif/if0001.h>
 #include "nouveau_debugfs.h"
 #include "nouveau_drm.h"
 
@@ -43,22 +46,233 @@ nouveau_debugfs_vbios_image(struct seq_file *m, void *data)
 	return 0;
 }
 
+static int
+nouveau_debugfs_pstate_get(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct nouveau_debugfs *debugfs = nouveau_debugfs(node->minor->dev);
+	struct nvif_object *ctrl = &debugfs->ctrl;
+	struct nvif_control_pstate_info_v0 info = {};
+	int ret, i;
+
+	if (!debugfs)
+		return -ENODEV;
+
+	ret = nvif_mthd(ctrl, NVIF_CONTROL_PSTATE_INFO, &info, sizeof(info));
+	if (ret)
+		return ret;
+
+	for (i = 0; i < info.count + 1; i++) {
+		const s32 state = i < info.count ? i :
+			NVIF_CONTROL_PSTATE_ATTR_V0_STATE_CURRENT;
+		struct nvif_control_pstate_attr_v0 attr = {
+			.state = state,
+			.index = 0,
+		};
+
+		ret = nvif_mthd(ctrl, NVIF_CONTROL_PSTATE_ATTR,
+				&attr, sizeof(attr));
+		if (ret)
+			return ret;
+
+		if (i < info.count)
+			seq_printf(m, "%02x:", attr.state);
+		else
+			seq_printf(m, "%s:", info.pwrsrc == 0 ? "DC" :
+					     info.pwrsrc == 1 ? "AC" : "--");
+
+		attr.index = 0;
+		do {
+			attr.state = state;
+			ret = nvif_mthd(ctrl, NVIF_CONTROL_PSTATE_ATTR,
+					&attr, sizeof(attr));
+			if (ret)
+				return ret;
+
+			seq_printf(m, " %s %d", attr.name, attr.min);
+			if (attr.min != attr.max)
+				seq_printf(m, "-%d", attr.max);
+			seq_printf(m, " %s", attr.unit);
+		} while (attr.index);
+
+		if (state >= 0) {
+			if (info.ustate_ac == state)
+				seq_printf(m, " AC");
+			if (info.ustate_dc == state)
+				seq_printf(m, " DC");
+			if (info.pstate == state)
+				seq_printf(m, " *");
+		} else {
+			if (info.ustate_ac < -1)
+				seq_printf(m, " AC");
+			if (info.ustate_dc < -1)
+				seq_printf(m, " DC");
+		}
+
+		seq_printf(m, "\n");
+	}
+
+	return 0;
+}
+
+static ssize_t
+nouveau_debugfs_pstate_set(struct file *file, const char __user *ubuf,
+			   size_t len, loff_t *offp)
+{
+	struct seq_file *m = file->private_data;
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct nouveau_debugfs *debugfs = nouveau_debugfs(node->minor->dev);
+	struct nvif_object *ctrl = &debugfs->ctrl;
+	struct nvif_control_pstate_user_v0 args = { .pwrsrc = -EINVAL };
+	char buf[32] = {}, *tmp, *cur = buf;
+	long value, ret;
+
+	if (!debugfs)
+		return -ENODEV;
+
+	if (len >= sizeof(buf))
+		return -EINVAL;
+
+	if (copy_from_user(buf, ubuf, len))
+		return -EFAULT;
+
+	if ((tmp = strchr(buf, '\n')))
+		*tmp = '\0';
+
+	if (!strncasecmp(cur, "dc:", 3)) {
+		args.pwrsrc = 0;
+		cur += 3;
+	} else
+	if (!strncasecmp(cur, "ac:", 3)) {
+		args.pwrsrc = 1;
+		cur += 3;
+	}
+
+	if (!strcasecmp(cur, "none"))
+		args.ustate = NVIF_CONTROL_PSTATE_USER_V0_STATE_UNKNOWN;
+	else
+	if (!strcasecmp(cur, "auto"))
+		args.ustate = NVIF_CONTROL_PSTATE_USER_V0_STATE_PERFMON;
+	else {
+		ret = kstrtol(cur, 16, &value);
+		if (ret)
+			return ret;
+		args.ustate = value;
+	}
+
+	ret = nvif_mthd(ctrl, NVIF_CONTROL_PSTATE_USER, &args, sizeof(args));
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+static int
+nouveau_debugfs_pstate_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, nouveau_debugfs_pstate_get, inode->i_private);
+}
+
+static const struct file_operations nouveau_pstate_fops = {
+	.owner = THIS_MODULE,
+	.open = nouveau_debugfs_pstate_open,
+	.read = seq_read,
+	.write = nouveau_debugfs_pstate_set,
+};
+
 static struct drm_info_list nouveau_debugfs_list[] = {
 	{ "vbios.rom", nouveau_debugfs_vbios_image, 0, NULL },
 };
 #define NOUVEAU_DEBUGFS_ENTRIES ARRAY_SIZE(nouveau_debugfs_list)
 
-int
-nouveau_debugfs_init(struct drm_minor *minor)
+static const struct nouveau_debugfs_files {
+	const char *name;
+	const struct file_operations *fops;
+} nouveau_debugfs_files[] = {
+	{"pstate", &nouveau_pstate_fops},
+};
+
+static int
+nouveau_debugfs_create_file(struct drm_minor *minor,
+		const struct nouveau_debugfs_files *ndf)
 {
-	drm_debugfs_create_files(nouveau_debugfs_list, NOUVEAU_DEBUGFS_ENTRIES,
-				 minor->debugfs_root, minor);
+	struct drm_info_node *node;
+
+	node = kmalloc(sizeof(*node), GFP_KERNEL);
+	if (node == NULL)
+		return -ENOMEM;
+
+	node->minor = minor;
+	node->info_ent = (const void *)ndf->fops;
+	node->dent = debugfs_create_file(ndf->name, S_IRUGO | S_IWUSR,
+					 minor->debugfs_root, node, ndf->fops);
+	if (!node->dent) {
+		kfree(node);
+		return -ENOMEM;
+	}
+
+	mutex_lock(&minor->debugfs_lock);
+	list_add(&node->list, &minor->debugfs_list);
+	mutex_unlock(&minor->debugfs_lock);
 	return 0;
 }
 
+int
+nouveau_drm_debugfs_init(struct drm_minor *minor)
+{
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(nouveau_debugfs_files); i++) {
+		ret = nouveau_debugfs_create_file(minor,
+						  &nouveau_debugfs_files[i]);
+
+		if (ret)
+			return ret;
+	}
+
+	return drm_debugfs_create_files(nouveau_debugfs_list,
+					NOUVEAU_DEBUGFS_ENTRIES,
+					minor->debugfs_root, minor);
+}
+
 void
-nouveau_debugfs_takedown(struct drm_minor *minor)
+nouveau_drm_debugfs_cleanup(struct drm_minor *minor)
 {
+	int i;
+
 	drm_debugfs_remove_files(nouveau_debugfs_list, NOUVEAU_DEBUGFS_ENTRIES,
 				 minor);
+
+	for (i = 0; i < ARRAY_SIZE(nouveau_debugfs_files); i++) {
+		drm_debugfs_remove_files((struct drm_info_list *)
+					 nouveau_debugfs_files[i].fops,
+					 1, minor);
+	}
+}
+
+int
+nouveau_debugfs_init(struct nouveau_drm *drm)
+{
+	int ret;
+
+	drm->debugfs = kzalloc(sizeof(*drm->debugfs), GFP_KERNEL);
+	if (!drm->debugfs)
+		return -ENOMEM;
+
+	ret = nvif_object_init(&drm->device.object, 0, NVIF_CLASS_CONTROL,
+			       NULL, 0, &drm->debugfs->ctrl);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+void
+nouveau_debugfs_fini(struct nouveau_drm *drm)
+{
+	if (drm->debugfs && drm->debugfs->ctrl.priv)
+		nvif_object_fini(&drm->debugfs->ctrl);
+
+	kfree(drm->debugfs);
+	drm->debugfs = NULL;
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_debugfs.h b/drivers/gpu/drm/nouveau/nouveau_debugfs.h
index a62af6fb5f99..b8c03ff5bf05 100644
--- a/drivers/gpu/drm/nouveau/nouveau_debugfs.h
+++ b/drivers/gpu/drm/nouveau/nouveau_debugfs.h
@@ -4,16 +4,43 @@
 #include <drm/drmP.h>
 
 #if defined(CONFIG_DEBUG_FS)
-extern int  nouveau_debugfs_init(struct drm_minor *);
-extern void nouveau_debugfs_takedown(struct drm_minor *);
+
+#include "nouveau_drm.h"
+
+struct nouveau_debugfs {
+	struct nvif_object ctrl;
+};
+
+static inline struct nouveau_debugfs *
+nouveau_debugfs(struct drm_device *dev)
+{
+	return nouveau_drm(dev)->debugfs;
+}
+
+extern int  nouveau_drm_debugfs_init(struct drm_minor *);
+extern void nouveau_drm_debugfs_cleanup(struct drm_minor *);
+extern int  nouveau_debugfs_init(struct nouveau_drm *);
+extern void nouveau_debugfs_fini(struct nouveau_drm *);
 #else
 static inline int
-nouveau_debugfs_init(struct drm_minor *minor)
+nouveau_drm_debugfs_init(struct drm_minor *minor)
 {
        return 0;
 }
 
-static inline void nouveau_debugfs_takedown(struct drm_minor *minor)
+static inline void
+nouveau_drm_debugfs_cleanup(struct drm_minor *minor)
+{
+}
+
+static inline int
+nouveau_debugfs_init(struct nouveau_drm *drm)
+{
+	return 0;
+}
+
+static inline void
+nouveau_debugfs_fini(struct nouveau_drm *drm)
 {
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 64c8d932d5f1..20935eb2a09e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -39,6 +39,7 @@
 
 #include "nouveau_fence.h"
 
+#include <nvif/cl0046.h>
 #include <nvif/event.h>
 
 static int
@@ -246,7 +247,7 @@ static const struct drm_framebuffer_funcs nouveau_framebuffer_funcs = {
 int
 nouveau_framebuffer_init(struct drm_device *dev,
 			 struct nouveau_framebuffer *nv_fb,
-			 struct drm_mode_fb_cmd2 *mode_cmd,
+			 const struct drm_mode_fb_cmd2 *mode_cmd,
 			 struct nouveau_bo *nvbo)
 {
 	struct nouveau_display *disp = nouveau_display(dev);
@@ -272,7 +273,7 @@ nouveau_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 nouveau_user_framebuffer_create(struct drm_device *dev,
 				struct drm_file *file_priv,
-				struct drm_mode_fb_cmd2 *mode_cmd)
+				const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct nouveau_framebuffer *nouveau_fb;
 	struct drm_gem_object *gem;
@@ -634,10 +635,6 @@ nouveau_display_resume(struct drm_device *dev, bool runtime)
 		nv_crtc->lut.depth = 0;
 	}
 
-	/* Make sure that drm and hw vblank irqs get resumed if needed. */
-	for (head = 0; head < dev->mode_config.num_crtc; head++)
-		drm_vblank_on(dev, head);
-
 	/* This should ensure we don't hit a locking problem when someone
 	 * wakes us up via a connector.  We should never go into suspend
 	 * while the display is on anyways.
@@ -647,6 +644,10 @@ nouveau_display_resume(struct drm_device *dev, bool runtime)
 
 	drm_helper_resume_force_mode(dev);
 
+	/* Make sure that drm and hw vblank irqs get resumed if needed. */
+	for (head = 0; head < dev->mode_config.num_crtc; head++)
+		drm_vblank_on(dev, head);
+
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.h b/drivers/gpu/drm/nouveau/nouveau_display.h
index 856abe0f070d..5a57d8b472c4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.h
+++ b/drivers/gpu/drm/nouveau/nouveau_display.h
@@ -23,7 +23,7 @@ nouveau_framebuffer(struct drm_framebuffer *fb)
 }
 
 int nouveau_framebuffer_init(struct drm_device *, struct nouveau_framebuffer *,
-			     struct drm_mode_fb_cmd2 *, struct nouveau_bo *);
+			     const struct drm_mode_fb_cmd2 *, struct nouveau_bo *);
 
 struct nouveau_page_flip_state {
 	struct list_head head;
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index 1d3ee5179ab8..2f2f252e3fb6 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -37,12 +37,16 @@
 #include <core/pci.h>
 #include <core/tegra.h>
 
+#include <nvif/class.h>
+#include <nvif/cl0002.h>
+#include <nvif/cla06f.h>
+#include <nvif/if0004.h>
+
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
 #include "nouveau_ttm.h"
 #include "nouveau_gem.h"
 #include "nouveau_vga.h"
-#include "nouveau_sysfs.h"
 #include "nouveau_hwmon.h"
 #include "nouveau_acpi.h"
 #include "nouveau_bios.h"
@@ -256,8 +260,8 @@ nouveau_accel_init(struct nouveau_drm *drm)
 		}
 
 		ret = nvif_notify_init(&drm->nvsw, nouveau_flip_complete,
-				       false, NVSW_NTFY_UEVENT, NULL, 0, 0,
-				       &drm->flip);
+				       false, NV04_NVSW_NTFY_UEVENT,
+				       NULL, 0, 0, &drm->flip);
 		if (ret == 0)
 			ret = nvif_notify_get(&drm->flip);
 		if (ret) {
@@ -448,7 +452,7 @@ nouveau_drm_load(struct drm_device *dev, unsigned long flags)
 			goto fail_dispinit;
 	}
 
-	nouveau_sysfs_init(dev);
+	nouveau_debugfs_init(drm);
 	nouveau_hwmon_init(dev);
 	nouveau_accel_init(drm);
 	nouveau_fbcon_init(dev);
@@ -486,7 +490,7 @@ nouveau_drm_unload(struct drm_device *dev)
 	nouveau_fbcon_fini(dev);
 	nouveau_accel_fini(drm);
 	nouveau_hwmon_fini(dev);
-	nouveau_sysfs_fini(dev);
+	nouveau_debugfs_fini(drm);
 
 	if (dev->mode_config.num_crtc)
 		nouveau_display_fini(dev);
@@ -928,8 +932,8 @@ driver_stub = {
 	.lastclose = nouveau_vga_lastclose,
 
 #if defined(CONFIG_DEBUG_FS)
-	.debugfs_init = nouveau_debugfs_init,
-	.debugfs_cleanup = nouveau_debugfs_takedown,
+	.debugfs_init = nouveau_drm_debugfs_init,
+	.debugfs_cleanup = nouveau_drm_debugfs_cleanup,
 #endif
 
 	.get_vblank_counter = drm_vblank_no_hw_counter,
@@ -1003,7 +1007,6 @@ static void nouveau_display_options(void)
 	DRM_DEBUG_DRIVER("... modeset      : %d\n", nouveau_modeset);
 	DRM_DEBUG_DRIVER("... runpm        : %d\n", nouveau_runtime_pm);
 	DRM_DEBUG_DRIVER("... vram_pushbuf : %d\n", nouveau_vram_pushbuf);
-	DRM_DEBUG_DRIVER("... pstate       : %d\n", nouveau_pstate);
 }
 
 static const struct dev_pm_ops nouveau_pm_ops = {
@@ -1046,10 +1049,6 @@ nouveau_platform_device_create(const struct nvkm_device_tegra_func *func,
 		goto err_free;
 	}
 
-	err = drm_dev_set_unique(drm, "%s", dev_name(&pdev->dev));
-	if (err < 0)
-		goto err_free;
-
 	drm->platformdev = pdev;
 	platform_set_drvdata(pdev, drm);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.h b/drivers/gpu/drm/nouveau/nouveau_drm.h
index a02813e994ec..5c363ed1c842 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.h
@@ -164,7 +164,7 @@ struct nouveau_drm {
 
 	/* power management */
 	struct nouveau_hwmon *hwmon;
-	struct nouveau_sysfs *sysfs;
+	struct nouveau_debugfs *debugfs;
 
 	/* display power reference */
 	bool have_disp_power_ref;
diff --git a/drivers/gpu/drm/nouveau/nouveau_encoder.h b/drivers/gpu/drm/nouveau/nouveau_encoder.h
index b37da95105b0..ee6a6d3fc80f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_encoder.h
+++ b/drivers/gpu/drm/nouveau/nouveau_encoder.h
@@ -63,6 +63,9 @@ struct nouveau_encoder {
 			u32 datarate;
 		} dp;
 	};
+
+	void (*enc_save)(struct drm_encoder *encoder);
+	void (*enc_restore)(struct drm_encoder *encoder);
 };
 
 struct nouveau_encoder *
@@ -80,7 +83,7 @@ static inline struct drm_encoder *to_drm_encoder(struct nouveau_encoder *enc)
 	return &enc->base.base;
 }
 
-static inline struct drm_encoder_slave_funcs *
+static inline const struct drm_encoder_slave_funcs *
 get_slave_funcs(struct drm_encoder *enc)
 {
 	return to_encoder_slave(enc)->slave_funcs;
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.h b/drivers/gpu/drm/nouveau/nouveau_fbcon.h
index 1e2e9e27a03b..ca77ad001978 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.h
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.h
@@ -34,7 +34,6 @@
 struct nouveau_fbdev {
 	struct drm_fb_helper helper;
 	struct nouveau_framebuffer nouveau_fb;
-	struct list_head fbdev_list;
 	struct drm_device *dev;
 	unsigned int saved_flags;
 	struct nvif_object surf2d;
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index 574c36b492ee..9a8c5b727f59 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -30,6 +30,7 @@
 #include <linux/hrtimer.h>
 #include <trace/events/fence.h>
 
+#include <nvif/cl826e.h>
 #include <nvif/notify.h>
 #include <nvif/event.h>
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_hwmon.c b/drivers/gpu/drm/nouveau/nouveau_hwmon.c
index 491c7149d197..8e13467d0ddb 100644
--- a/drivers/gpu/drm/nouveau/nouveau_hwmon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_hwmon.c
@@ -34,6 +34,8 @@
 #include "nouveau_drm.h"
 #include "nouveau_hwmon.h"
 
+#include <nvkm/subdev/volt.h>
+
 #if defined(CONFIG_HWMON) || (defined(MODULE) && defined(CONFIG_HWMON_MODULE))
 static ssize_t
 nouveau_hwmon_show_temp(struct device *d, struct device_attribute *a, char *buf)
@@ -512,6 +514,35 @@ static SENSOR_DEVICE_ATTR(pwm1_max, S_IRUGO | S_IWUSR,
 			  nouveau_hwmon_get_pwm1_max,
 			  nouveau_hwmon_set_pwm1_max, 0);
 
+static ssize_t
+nouveau_hwmon_get_in0_input(struct device *d,
+			    struct device_attribute *a, char *buf)
+{
+	struct drm_device *dev = dev_get_drvdata(d);
+	struct nouveau_drm *drm = nouveau_drm(dev);
+	struct nvkm_volt *volt = nvxx_volt(&drm->device);
+	int ret;
+
+	ret = nvkm_volt_get(volt);
+	if (ret < 0)
+		return ret;
+
+	return sprintf(buf, "%i\n", ret / 1000);
+}
+
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO,
+			  nouveau_hwmon_get_in0_input, NULL, 0);
+
+static ssize_t
+nouveau_hwmon_get_in0_label(struct device *d,
+			    struct device_attribute *a, char *buf)
+{
+	return sprintf(buf, "GPU core\n");
+}
+
+static SENSOR_DEVICE_ATTR(in0_label, S_IRUGO,
+			  nouveau_hwmon_get_in0_label, NULL, 0);
+
 static struct attribute *hwmon_default_attributes[] = {
 	&sensor_dev_attr_name.dev_attr.attr,
 	&sensor_dev_attr_update_rate.dev_attr.attr,
@@ -542,6 +573,12 @@ static struct attribute *hwmon_pwm_fan_attributes[] = {
 	NULL
 };
 
+static struct attribute *hwmon_in0_attributes[] = {
+	&sensor_dev_attr_in0_input.dev_attr.attr,
+	&sensor_dev_attr_in0_label.dev_attr.attr,
+	NULL
+};
+
 static const struct attribute_group hwmon_default_attrgroup = {
 	.attrs = hwmon_default_attributes,
 };
@@ -554,6 +591,9 @@ static const struct attribute_group hwmon_fan_rpm_attrgroup = {
 static const struct attribute_group hwmon_pwm_fan_attrgroup = {
 	.attrs = hwmon_pwm_fan_attributes,
 };
+static const struct attribute_group hwmon_in0_attrgroup = {
+	.attrs = hwmon_in0_attributes,
+};
 #endif
 
 int
@@ -562,6 +602,7 @@ nouveau_hwmon_init(struct drm_device *dev)
 #if defined(CONFIG_HWMON) || (defined(MODULE) && defined(CONFIG_HWMON_MODULE))
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nvkm_therm *therm = nvxx_therm(&drm->device);
+	struct nvkm_volt *volt = nvxx_volt(&drm->device);
 	struct nouveau_hwmon *hwmon;
 	struct device *hwmon_dev;
 	int ret = 0;
@@ -613,6 +654,14 @@ nouveau_hwmon_init(struct drm_device *dev)
 			goto error;
 	}
 
+	if (volt && nvkm_volt_get(volt) >= 0) {
+		ret = sysfs_create_group(&hwmon_dev->kobj,
+					 &hwmon_in0_attrgroup);
+
+		if (ret)
+			goto error;
+	}
+
 	hwmon->hwmon = hwmon_dev;
 
 	return 0;
@@ -638,6 +687,7 @@ nouveau_hwmon_fini(struct drm_device *dev)
 		sysfs_remove_group(&hwmon->hwmon->kobj, &hwmon_temp_attrgroup);
 		sysfs_remove_group(&hwmon->hwmon->kobj, &hwmon_pwm_fan_attrgroup);
 		sysfs_remove_group(&hwmon->hwmon->kobj, &hwmon_fan_rpm_attrgroup);
+		sysfs_remove_group(&hwmon->hwmon->kobj, &hwmon_in0_attrgroup);
 
 		hwmon_device_unregister(hwmon->hwmon);
 	}
diff --git a/drivers/gpu/drm/nouveau/nouveau_platform.c b/drivers/gpu/drm/nouveau/nouveau_platform.c
index 60e32c4e4e49..2dfe58af12e4 100644
--- a/drivers/gpu/drm/nouveau/nouveau_platform.c
+++ b/drivers/gpu/drm/nouveau/nouveau_platform.c
@@ -24,7 +24,7 @@
 static int nouveau_platform_probe(struct platform_device *pdev)
 {
 	const struct nvkm_device_tegra_func *func;
-	struct nvkm_device *device;
+	struct nvkm_device *device = NULL;
 	struct drm_device *drm;
 	int ret;
 
@@ -78,3 +78,14 @@ struct platform_driver nouveau_platform_driver = {
 	.probe = nouveau_platform_probe,
 	.remove = nouveau_platform_remove,
 };
+
+#if IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC) || IS_ENABLED(CONFIG_ARCH_TEGRA_132_SOC)
+MODULE_FIRMWARE("nvidia/gk20a/fecs_data.bin");
+MODULE_FIRMWARE("nvidia/gk20a/fecs_inst.bin");
+MODULE_FIRMWARE("nvidia/gk20a/gpccs_data.bin");
+MODULE_FIRMWARE("nvidia/gk20a/gpccs_inst.bin");
+MODULE_FIRMWARE("nvidia/gk20a/sw_bundle_init.bin");
+MODULE_FIRMWARE("nvidia/gk20a/sw_ctx.bin");
+MODULE_FIRMWARE("nvidia/gk20a/sw_method_init.bin");
+MODULE_FIRMWARE("nvidia/gk20a/sw_nonctx.bin");
+#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_sysfs.c b/drivers/gpu/drm/nouveau/nouveau_sysfs.c
deleted file mode 100644
index 5dac3546c1b8..000000000000
--- a/drivers/gpu/drm/nouveau/nouveau_sysfs.c
+++ /dev/null
@@ -1,197 +0,0 @@
-/*
- * Copyright 2013 Red Hat Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- * Authors: Ben Skeggs <bskeggs@redhat.com>
- */
-
-#include <nvif/os.h>
-#include <nvif/class.h>
-#include <nvif/ioctl.h>
-
-#include "nouveau_sysfs.h"
-
-MODULE_PARM_DESC(pstate, "enable sysfs pstate file, which will be moved in the future");
-int nouveau_pstate;
-module_param_named(pstate, nouveau_pstate, int, 0400);
-
-static inline struct drm_device *
-drm_device(struct device *d)
-{
-	return dev_get_drvdata(d);
-}
-
-#define snappendf(p,r,f,a...) do {                                             \
-	snprintf(p, r, f, ##a);                                                \
-	r -= strlen(p);                                                        \
-	p += strlen(p);                                                        \
-} while(0)
-
-static ssize_t
-nouveau_sysfs_pstate_get(struct device *d, struct device_attribute *a, char *b)
-{
-	struct nouveau_sysfs *sysfs = nouveau_sysfs(drm_device(d));
-	struct nvif_control_pstate_info_v0 info = {};
-	size_t cnt = PAGE_SIZE;
-	char *buf = b;
-	int ret, i;
-
-	ret = nvif_mthd(&sysfs->ctrl, NVIF_CONTROL_PSTATE_INFO,
-			&info, sizeof(info));
-	if (ret)
-		return ret;
-
-	for (i = 0; i < info.count + 1; i++) {
-		const s32 state = i < info.count ? i :
-			NVIF_CONTROL_PSTATE_ATTR_V0_STATE_CURRENT;
-		struct nvif_control_pstate_attr_v0 attr = {
-			.state = state,
-			.index = 0,
-		};
-
-		ret = nvif_mthd(&sysfs->ctrl, NVIF_CONTROL_PSTATE_ATTR,
-				&attr, sizeof(attr));
-		if (ret)
-			return ret;
-
-		if (i < info.count)
-			snappendf(buf, cnt, "%02x:", attr.state);
-		else
-			snappendf(buf, cnt, "%s:", info.pwrsrc == 0 ? "DC" :
-						   info.pwrsrc == 1 ? "AC" :
-						   "--");
-
-		attr.index = 0;
-		do {
-			attr.state = state;
-			ret = nvif_mthd(&sysfs->ctrl,
-					NVIF_CONTROL_PSTATE_ATTR,
-					&attr, sizeof(attr));
-			if (ret)
-				return ret;
-
-			snappendf(buf, cnt, " %s %d", attr.name, attr.min);
-			if (attr.min != attr.max)
-				snappendf(buf, cnt, "-%d", attr.max);
-			snappendf(buf, cnt, " %s", attr.unit);
-		} while (attr.index);
-
-		if (state >= 0) {
-			if (info.ustate_ac == state)
-				snappendf(buf, cnt, " AC");
-			if (info.ustate_dc == state)
-				snappendf(buf, cnt, " DC");
-			if (info.pstate == state)
-				snappendf(buf, cnt, " *");
-		} else {
-			if (info.ustate_ac < -1)
-				snappendf(buf, cnt, " AC");
-			if (info.ustate_dc < -1)
-				snappendf(buf, cnt, " DC");
-		}
-
-		snappendf(buf, cnt, "\n");
-	}
-
-	return strlen(b);
-}
-
-static ssize_t
-nouveau_sysfs_pstate_set(struct device *d, struct device_attribute *a,
-			 const char *buf, size_t count)
-{
-	struct nouveau_sysfs *sysfs = nouveau_sysfs(drm_device(d));
-	struct nvif_control_pstate_user_v0 args = { .pwrsrc = -EINVAL };
-	long value, ret;
-	char *tmp;
-
-	if ((tmp = strchr(buf, '\n')))
-		*tmp = '\0';
-
-	if (!strncasecmp(buf, "dc:", 3)) {
-		args.pwrsrc = 0;
-		buf += 3;
-	} else
-	if (!strncasecmp(buf, "ac:", 3)) {
-		args.pwrsrc = 1;
-		buf += 3;
-	}
-
-	if (!strcasecmp(buf, "none"))
-		args.ustate = NVIF_CONTROL_PSTATE_USER_V0_STATE_UNKNOWN;
-	else
-	if (!strcasecmp(buf, "auto"))
-		args.ustate = NVIF_CONTROL_PSTATE_USER_V0_STATE_PERFMON;
-	else {
-		ret = kstrtol(buf, 16, &value);
-		if (ret)
-			return ret;
-		args.ustate = value;
-	}
-
-	ret = nvif_mthd(&sysfs->ctrl, NVIF_CONTROL_PSTATE_USER,
-			&args, sizeof(args));
-	if (ret < 0)
-		return ret;
-
-	return count;
-}
-
-static DEVICE_ATTR(pstate, S_IRUGO | S_IWUSR,
-		   nouveau_sysfs_pstate_get, nouveau_sysfs_pstate_set);
-
-void
-nouveau_sysfs_fini(struct drm_device *dev)
-{
-	struct nouveau_sysfs *sysfs = nouveau_sysfs(dev);
-	struct nouveau_drm *drm = nouveau_drm(dev);
-	struct nvif_device *device = &drm->device;
-
-	if (sysfs && sysfs->ctrl.priv) {
-		device_remove_file(nvxx_device(device)->dev, &dev_attr_pstate);
-		nvif_object_fini(&sysfs->ctrl);
-	}
-
-	drm->sysfs = NULL;
-	kfree(sysfs);
-}
-
-int
-nouveau_sysfs_init(struct drm_device *dev)
-{
-	struct nouveau_drm *drm = nouveau_drm(dev);
-	struct nvif_device *device = &drm->device;
-	struct nouveau_sysfs *sysfs;
-	int ret;
-
-	if (!nouveau_pstate)
-		return 0;
-
-	sysfs = drm->sysfs = kzalloc(sizeof(*sysfs), GFP_KERNEL);
-	if (!sysfs)
-		return -ENOMEM;
-
-	ret = nvif_object_init(&device->object, 0, NVIF_IOCTL_NEW_V0_CONTROL,
-			       NULL, 0, &sysfs->ctrl);
-	if (ret == 0)
-		device_create_file(nvxx_device(device)->dev, &dev_attr_pstate);
-
-	return 0;
-}
diff --git a/drivers/gpu/drm/nouveau/nouveau_sysfs.h b/drivers/gpu/drm/nouveau/nouveau_sysfs.h
deleted file mode 100644
index 4e5ea9241b28..000000000000
--- a/drivers/gpu/drm/nouveau/nouveau_sysfs.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef __NOUVEAU_SYSFS_H__
-#define __NOUVEAU_SYSFS_H__
-
-#include "nouveau_drm.h"
-
-struct nouveau_sysfs {
-	struct nvif_object ctrl;
-};
-
-static inline struct nouveau_sysfs *
-nouveau_sysfs(struct drm_device *dev)
-{
-	return nouveau_drm(dev)->sysfs;
-}
-
-int  nouveau_sysfs_init(struct drm_device *);
-void nouveau_sysfs_fini(struct drm_device *);
-
-extern int nouveau_pstate;
-
-#endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_usif.c b/drivers/gpu/drm/nouveau/nouveau_usif.c
index 6ae1b3494bcd..e9f52ef0be83 100644
--- a/drivers/gpu/drm/nouveau/nouveau_usif.c
+++ b/drivers/gpu/drm/nouveau/nouveau_usif.c
@@ -130,20 +130,21 @@ usif_notify_new(struct drm_file *f, void *data, u32 size, void *argv, u32 argc)
 		struct nvif_notify_req_v0 v0;
 	} *req;
 	struct usif_notify *ntfy;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		if (usif_notify_find(f, args->v0.index))
 			return -EEXIST;
 	} else
 		return ret;
 	req = data;
+	ret = -ENOSYS;
 
 	if (!(ntfy = kmalloc(sizeof(*ntfy), GFP_KERNEL)))
 		return -ENOMEM;
 	atomic_set(&ntfy->enabled, 0);
 
-	if (nvif_unpack(req->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, req->v0, 0, 0, true))) {
 		ntfy->reply = sizeof(struct nvif_notify_rep_v0) + req->v0.reply;
 		ntfy->route = req->v0.route;
 		ntfy->token = req->v0.token;
@@ -171,9 +172,9 @@ usif_notify_del(struct drm_file *f, void *data, u32 size, void *argv, u32 argc)
 		struct nvif_ioctl_ntfy_del_v0 v0;
 	} *args = data;
 	struct usif_notify *ntfy;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		if (!(ntfy = usif_notify_find(f, args->v0.index)))
 			return -ENOENT;
 	} else
@@ -194,9 +195,9 @@ usif_notify_get(struct drm_file *f, void *data, u32 size, void *argv, u32 argc)
 		struct nvif_ioctl_ntfy_del_v0 v0;
 	} *args = data;
 	struct usif_notify *ntfy;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		if (!(ntfy = usif_notify_find(f, args->v0.index)))
 			return -ENOENT;
 	} else
@@ -233,9 +234,9 @@ usif_notify_put(struct drm_file *f, void *data, u32 size, void *argv, u32 argc)
 		struct nvif_ioctl_ntfy_put_v0 v0;
 	} *args = data;
 	struct usif_notify *ntfy;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		if (!(ntfy = usif_notify_find(f, args->v0.index)))
 			return -ENOENT;
 	} else
@@ -270,13 +271,13 @@ usif_object_new(struct drm_file *f, void *data, u32 size, void *argv, u32 argc)
 		struct nvif_ioctl_new_v0 v0;
 	} *args = data;
 	struct usif_object *object;
-	int ret;
+	int ret = -ENOSYS;
 
 	if (!(object = kmalloc(sizeof(*object), GFP_KERNEL)))
 		return -ENOMEM;
 	list_add(&object->head, &cli->objects);
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		object->route = args->v0.route;
 		object->token = args->v0.token;
 		args->v0.route = NVDRM_OBJECT_USIF;
@@ -310,7 +311,7 @@ usif_ioctl(struct drm_file *filp, void __user *user, u32 argc)
 	if (ret = -EFAULT, copy_from_user(argv, user, size))
 		goto done;
 
-	if (nvif_unpack(argv->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(-ENOSYS, &data, &size, argv->v0, 0, 0, true))) {
 		/* block access to objects not created via this interface */
 		owner = argv->v0.owner;
 		if (argv->v0.object == 0ULL)
diff --git a/drivers/gpu/drm/nouveau/nv04_fence.c b/drivers/gpu/drm/nouveau/nv04_fence.c
index f3d705d67738..3022d24ed88b 100644
--- a/drivers/gpu/drm/nouveau/nv04_fence.c
+++ b/drivers/gpu/drm/nouveau/nv04_fence.c
@@ -26,6 +26,8 @@
 #include "nouveau_dma.h"
 #include "nouveau_fence.h"
 
+#include <nvif/if0004.h>
+
 struct nv04_fence_chan {
 	struct nouveau_fence_chan base;
 };
diff --git a/drivers/gpu/drm/nouveau/nv17_fence.c b/drivers/gpu/drm/nouveau/nv17_fence.c
index 80b6eb8b3d02..6a141c9bf5b7 100644
--- a/drivers/gpu/drm/nouveau/nv17_fence.c
+++ b/drivers/gpu/drm/nouveau/nv17_fence.c
@@ -24,6 +24,7 @@
 
 #include <nvif/os.h>
 #include <nvif/class.h>
+#include <nvif/cl0002.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index c053c50b346a..ea3921652449 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -28,8 +28,16 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_plane_helper.h>
 #include <drm/drm_dp_helper.h>
+#include <drm/drm_fb_helper.h>
 
 #include <nvif/class.h>
+#include <nvif/cl0002.h>
+#include <nvif/cl5070.h>
+#include <nvif/cl507a.h>
+#include <nvif/cl507b.h>
+#include <nvif/cl507c.h>
+#include <nvif/cl507d.h>
+#include <nvif/cl507e.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
@@ -773,7 +781,6 @@ nv50_crtc_set_scale(struct nouveau_crtc *nv_crtc, bool update)
 	 */
 	if (nv_connector && ( nv_connector->underscan == UNDERSCAN_ON ||
 			     (nv_connector->underscan == UNDERSCAN_AUTO &&
-			      nv_connector->edid &&
 			      drm_detect_hdmi_monitor(nv_connector->edid)))) {
 		u32 bX = nv_connector->underscan_hborder;
 		u32 bY = nv_connector->underscan_vborder;
@@ -1717,7 +1724,7 @@ nv50_dac_create(struct drm_connector *connector, struct dcb_output *dcbe)
 	encoder = to_drm_encoder(nv_encoder);
 	encoder->possible_crtcs = dcbe->heads;
 	encoder->possible_clones = 0;
-	drm_encoder_init(connector->dev, encoder, &nv50_dac_func, type);
+	drm_encoder_init(connector->dev, encoder, &nv50_dac_func, type, NULL);
 	drm_encoder_helper_add(encoder, &nv50_dac_hfunc);
 
 	drm_mode_connector_attach_encoder(connector, encoder);
@@ -1961,10 +1968,17 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 	switch (nv_encoder->dcb->type) {
 	case DCB_OUTPUT_TMDS:
 		if (nv_encoder->dcb->sorconf.link & 1) {
-			if (mode->clock < 165000)
-				proto = 0x1;
-			else
-				proto = 0x5;
+			proto = 0x1;
+			/* Only enable dual-link if:
+			 *  - Need to (i.e. rate > 165MHz)
+			 *  - DCB says we can
+			 *  - Not an HDMI monitor, since there's no dual-link
+			 *    on HDMI.
+			 */
+			if (mode->clock >= 165000 &&
+			    nv_encoder->dcb->duallink_possible &&
+			    !drm_detect_hdmi_monitor(nv_connector->edid))
+				proto |= 0x4;
 		} else {
 			proto = 0x2;
 		}
@@ -2125,7 +2139,7 @@ nv50_sor_create(struct drm_connector *connector, struct dcb_output *dcbe)
 	encoder = to_drm_encoder(nv_encoder);
 	encoder->possible_crtcs = dcbe->heads;
 	encoder->possible_clones = 0;
-	drm_encoder_init(connector->dev, encoder, &nv50_sor_func, type);
+	drm_encoder_init(connector->dev, encoder, &nv50_sor_func, type, NULL);
 	drm_encoder_helper_add(encoder, &nv50_sor_hfunc);
 
 	drm_mode_connector_attach_encoder(connector, encoder);
@@ -2305,7 +2319,7 @@ nv50_pior_create(struct drm_connector *connector, struct dcb_output *dcbe)
 	encoder = to_drm_encoder(nv_encoder);
 	encoder->possible_crtcs = dcbe->heads;
 	encoder->possible_clones = 0;
-	drm_encoder_init(connector->dev, encoder, &nv50_pior_func, type);
+	drm_encoder_init(connector->dev, encoder, &nv50_pior_func, type, NULL);
 	drm_encoder_helper_add(encoder, &nv50_pior_hfunc);
 
 	drm_mode_connector_attach_encoder(connector, encoder);
diff --git a/drivers/gpu/drm/nouveau/nv50_fence.c b/drivers/gpu/drm/nouveau/nv50_fence.c
index f0d96e5da6b4..3695ccce68c7 100644
--- a/drivers/gpu/drm/nouveau/nv50_fence.c
+++ b/drivers/gpu/drm/nouveau/nv50_fence.c
@@ -24,6 +24,7 @@
 
 #include <nvif/os.h>
 #include <nvif/class.h>
+#include <nvif/cl0002.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_dma.h"
diff --git a/drivers/gpu/drm/nouveau/nvkm/core/client.c b/drivers/gpu/drm/nouveau/nvkm/core/client.c
index 297e1e953fa6..e1943910858e 100644
--- a/drivers/gpu/drm/nouveau/nvkm/core/client.c
+++ b/drivers/gpu/drm/nouveau/nvkm/core/client.c
@@ -28,6 +28,7 @@
 
 #include <nvif/class.h>
 #include <nvif/event.h>
+#include <nvif/if0000.h>
 #include <nvif/unpack.h>
 
 struct nvkm_client_notify {
@@ -96,7 +97,7 @@ nvkm_client_notify_new(struct nvkm_object *object,
 		struct nvif_notify_req_v0 v0;
 	} *req = data;
 	u8  index, reply;
-	int ret;
+	int ret = -ENOSYS;
 
 	for (index = 0; index < ARRAY_SIZE(client->notify); index++) {
 		if (!client->notify[index])
@@ -111,7 +112,7 @@ nvkm_client_notify_new(struct nvkm_object *object,
 		return -ENOMEM;
 
 	nvif_ioctl(object, "notify new size %d\n", size);
-	if (nvif_unpack(req->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, req->v0, 0, 0, true))) {
 		nvif_ioctl(object, "notify new vers %d reply %d route %02x "
 				   "token %llx\n", req->v0.version,
 			   req->v0.reply, req->v0.route, req->v0.token);
@@ -143,10 +144,10 @@ nvkm_client_mthd_devlist(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nv_client_devlist_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "client devlist size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "client devlist vers %d count %d\n",
 			   args->v0.version, args->v0.count);
 		if (size == sizeof(args->v0.device[0]) * args->v0.count) {
diff --git a/drivers/gpu/drm/nouveau/nvkm/core/ioctl.c b/drivers/gpu/drm/nouveau/nvkm/core/ioctl.c
index d87d6ab03cc7..b0db51847c36 100644
--- a/drivers/gpu/drm/nouveau/nvkm/core/ioctl.c
+++ b/drivers/gpu/drm/nouveau/nvkm/core/ioctl.c
@@ -34,10 +34,10 @@ nvkm_ioctl_nop(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_nop_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "nop size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "nop vers %lld\n", args->v0.version);
 		args->v0.version = NVIF_VERSION_LATEST;
 	}
@@ -52,10 +52,10 @@ nvkm_ioctl_sclass(struct nvkm_object *object, void *data, u32 size)
 		struct nvif_ioctl_sclass_v0 v0;
 	} *args = data;
 	struct nvkm_oclass oclass;
-	int ret, i = 0;
+	int ret = -ENOSYS, i = 0;
 
 	nvif_ioctl(object, "sclass size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "sclass vers %d count %d\n",
 			   args->v0.version, args->v0.count);
 		if (size != args->v0.count * sizeof(args->v0.oclass[0]))
@@ -86,10 +86,10 @@ nvkm_ioctl_new(struct nvkm_object *parent, void *data, u32 size)
 	struct nvkm_client *client = parent->client;
 	struct nvkm_object *object = NULL;
 	struct nvkm_oclass oclass;
-	int ret, i = 0;
+	int ret = -ENOSYS, i = 0;
 
 	nvif_ioctl(parent, "new size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(parent, "new vers %d handle %08x class %08x "
 				   "route %02x token %llx object %016llx\n",
 			   args->v0.version, args->v0.handle, args->v0.oclass,
@@ -147,10 +147,10 @@ nvkm_ioctl_del(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_del none;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "delete size %d\n", size);
-	if (nvif_unvers(args->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, args->none))) {
 		nvif_ioctl(object, "delete\n");
 		nvkm_object_fini(object, false);
 		nvkm_object_del(&object);
@@ -165,10 +165,10 @@ nvkm_ioctl_mthd(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_mthd_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "mthd size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "mthd vers %d mthd %02x\n",
 			   args->v0.version, args->v0.method);
 		ret = nvkm_object_mthd(object, args->v0.method, data, size);
@@ -189,10 +189,10 @@ nvkm_ioctl_rd(struct nvkm_object *object, void *data, u32 size)
 		u16 b16;
 		u32 b32;
 	} v;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "rd size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "rd vers %d size %d addr %016llx\n",
 			   args->v0.version, args->v0.size, args->v0.addr);
 		switch (args->v0.size) {
@@ -223,10 +223,10 @@ nvkm_ioctl_wr(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_wr_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "wr size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object,
 			   "wr vers %d size %d addr %016llx data %08x\n",
 			   args->v0.version, args->v0.size, args->v0.addr,
@@ -251,10 +251,10 @@ nvkm_ioctl_map(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_map_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "map size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "map vers %d\n", args->v0.version);
 		ret = nvkm_object_map(object, &args->v0.handle,
 					      &args->v0.length);
@@ -269,10 +269,10 @@ nvkm_ioctl_unmap(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_unmap none;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "unmap size %d\n", size);
-	if (nvif_unvers(args->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, args->none))) {
 		nvif_ioctl(object, "unmap\n");
 	}
 
@@ -286,10 +286,10 @@ nvkm_ioctl_ntfy_new(struct nvkm_object *object, void *data, u32 size)
 		struct nvif_ioctl_ntfy_new_v0 v0;
 	} *args = data;
 	struct nvkm_event *event;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "ntfy new size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "ntfy new vers %d event %02x\n",
 			   args->v0.version, args->v0.event);
 		ret = nvkm_object_ntfy(object, args->v0.event, &event);
@@ -312,10 +312,10 @@ nvkm_ioctl_ntfy_del(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_ntfy_del_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "ntfy del size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "ntfy del vers %d index %d\n",
 			   args->v0.version, args->v0.index);
 		ret = nvkm_client_notify_del(client, args->v0.index);
@@ -331,10 +331,10 @@ nvkm_ioctl_ntfy_get(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_ntfy_get_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "ntfy get size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "ntfy get vers %d index %d\n",
 			   args->v0.version, args->v0.index);
 		ret = nvkm_client_notify_get(client, args->v0.index);
@@ -350,10 +350,10 @@ nvkm_ioctl_ntfy_put(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct nvif_ioctl_ntfy_put_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "ntfy put size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "ntfy put vers %d index %d\n",
 			   args->v0.version, args->v0.index);
 		ret = nvkm_client_notify_put(client, args->v0.index);
@@ -421,12 +421,12 @@ nvkm_ioctl(struct nvkm_client *client, bool supervisor,
 	union {
 		struct nvif_ioctl_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	client->super = supervisor;
 	nvif_ioctl(object, "size %d\n", size);
 
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object,
 			   "vers %d type %02x object %016llx owner %02x\n",
 			   args->v0.version, args->v0.type, args->v0.object,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/ce/gk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/ce/gk104.c
index c541a1c012dc..e2b944dce9b8 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/ce/gk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/ce/gk104.c
@@ -22,19 +22,65 @@
  * Authors: Ben Skeggs
  */
 #include "priv.h"
+#include <core/enum.h>
 
 #include <nvif/class.h>
 
+static const struct nvkm_enum
+gk104_ce_launcherr_report[] = {
+	{ 0x0, "NO_ERR" },
+	{ 0x1, "2D_LAYER_EXCEEDS_DEPTH" },
+	{ 0x2, "INVALID_ARGUMENT" },
+	{ 0x3, "MEM2MEM_RECT_OUT_OF_BOUNDS" },
+	{ 0x4, "SRC_LINE_EXCEEDS_PITCH" },
+	{ 0x5, "SRC_LINE_EXCEEDS_NEG_PITCH" },
+	{ 0x6, "DST_LINE_EXCEEDS_PITCH" },
+	{ 0x7, "DST_LINE_EXCEEDS_NEG_PITCH" },
+	{ 0x8, "BAD_SRC_PIXEL_COMP_REF" },
+	{ 0x9, "INVALID_VALUE" },
+	{ 0xa, "UNUSED_FIELD" },
+	{ 0xb, "INVALID_OPERATION" },
+	{}
+};
+
+static void
+gk104_ce_intr_launcherr(struct nvkm_engine *ce, const u32 base)
+{
+	struct nvkm_subdev *subdev = &ce->subdev;
+	struct nvkm_device *device = subdev->device;
+	u32 stat = nvkm_rd32(device, 0x104f14 + base);
+	const struct nvkm_enum *en =
+		nvkm_enum_find(gk104_ce_launcherr_report, stat & 0x0000000f);
+	nvkm_warn(subdev, "LAUNCHERR %08x [%s]\n", stat, en ? en->name : "");
+	nvkm_wr32(device, 0x104f14 + base, 0x00000000);
+}
+
 void
 gk104_ce_intr(struct nvkm_engine *ce)
 {
 	const u32 base = (ce->subdev.index - NVKM_ENGINE_CE0) * 0x1000;
 	struct nvkm_subdev *subdev = &ce->subdev;
 	struct nvkm_device *device = subdev->device;
-	u32 stat = nvkm_rd32(device, 0x104908 + base);
-	if (stat) {
-		nvkm_warn(subdev, "intr %08x\n", stat);
-		nvkm_wr32(device, 0x104908 + base, stat);
+	u32 mask = nvkm_rd32(device, 0x104904 + base);
+	u32 intr = nvkm_rd32(device, 0x104908 + base) & mask;
+	if (intr & 0x00000001) {
+		nvkm_warn(subdev, "BLOCKPIPE\n");
+		nvkm_wr32(device, 0x104908 + base, 0x00000001);
+		intr &= ~0x00000001;
+	}
+	if (intr & 0x00000002) {
+		nvkm_warn(subdev, "NONBLOCKPIPE\n");
+		nvkm_wr32(device, 0x104908 + base, 0x00000002);
+		intr &= ~0x00000002;
+	}
+	if (intr & 0x00000004) {
+		gk104_ce_intr_launcherr(ce, base);
+		nvkm_wr32(device, 0x104908 + base, 0x00000004);
+		intr &= ~0x00000004;
+	}
+	if (intr) {
+		nvkm_warn(subdev, "intr %08x\n", intr);
+		nvkm_wr32(device, 0x104908 + base, intr);
 	}
 }
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
index bbc9824af6e0..b1ba1c782a2b 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/base.c
@@ -1388,7 +1388,7 @@ nvc1_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gf106_pci_new,
 	.pmu = gf100_pmu_new,
 	.therm = gt215_therm_new,
 	.timer = nv41_timer_new,
@@ -1423,7 +1423,7 @@ nvc3_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gf106_pci_new,
 	.pmu = gf100_pmu_new,
 	.therm = gt215_therm_new,
 	.timer = nv41_timer_new,
@@ -1566,7 +1566,7 @@ nvcf_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gf106_pci_new,
 	.pmu = gf100_pmu_new,
 	.therm = gt215_therm_new,
 	.timer = nv41_timer_new,
@@ -1601,7 +1601,7 @@ nvd7_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gf106_pci_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
 	.ce[0] = gf100_ce_new,
@@ -1634,7 +1634,7 @@ nvd9_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gf106_pci_new,
 	.pmu = gf119_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1669,7 +1669,7 @@ nve4_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk104_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1706,7 +1706,7 @@ nve6_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk104_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1743,7 +1743,7 @@ nve7_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk104_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1804,7 +1804,7 @@ nvf0_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk110_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1840,7 +1840,7 @@ nvf1_chipset = {
 	.mc = gf100_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk110_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1876,7 +1876,7 @@ nv106_chipset = {
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk208_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1912,7 +1912,7 @@ nv108_chipset = {
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gk208_pmu_new,
 	.therm = gf119_therm_new,
 	.timer = nv41_timer_new,
@@ -1948,7 +1948,7 @@ nv117_chipset = {
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gm107_pmu_new,
 	.therm = gm107_therm_new,
 	.timer = gk20a_timer_new,
@@ -1973,13 +1973,13 @@ nv124_chipset = {
 	.fuse = gm107_fuse_new,
 	.gpio = gk104_gpio_new,
 	.i2c = gm204_i2c_new,
-	.ibus = gk104_ibus_new,
+	.ibus = gm204_ibus_new,
 	.imem = nv50_instmem_new,
-	.ltc = gm107_ltc_new,
+	.ltc = gm204_ltc_new,
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gm107_pmu_new,
 	.timer = gk20a_timer_new,
 	.volt = gk104_volt_new,
@@ -2004,13 +2004,13 @@ nv126_chipset = {
 	.fuse = gm107_fuse_new,
 	.gpio = gk104_gpio_new,
 	.i2c = gm204_i2c_new,
-	.ibus = gk104_ibus_new,
+	.ibus = gm204_ibus_new,
 	.imem = nv50_instmem_new,
-	.ltc = gm107_ltc_new,
+	.ltc = gm204_ltc_new,
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.mxm = nv50_mxm_new,
-	.pci = g94_pci_new,
+	.pci = gk104_pci_new,
 	.pmu = gm107_pmu_new,
 	.timer = gk20a_timer_new,
 	.volt = gk104_volt_new,
@@ -2033,7 +2033,7 @@ nv12b_chipset = {
 	.fuse = gm107_fuse_new,
 	.ibus = gk20a_ibus_new,
 	.imem = gk20a_instmem_new,
-	.ltc = gm107_ltc_new,
+	.ltc = gm204_ltc_new,
 	.mc = gk20a_mc_new,
 	.mmu = gf100_mmu_new,
 	.timer = gk20a_timer_new,
@@ -2494,7 +2494,8 @@ nvkm_device_ctor(const struct nvkm_device_func *func,
 		device->pri = ioremap(mmio_base, mmio_size);
 		if (!device->pri) {
 			nvdev_error(device, "unable to map PRI\n");
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto done;
 		}
 	}
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/ctrl.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/ctrl.c
index cf8bc068e9b7..b0ece71aefde 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/ctrl.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/ctrl.c
@@ -27,6 +27,7 @@
 #include <subdev/clk.h>
 
 #include <nvif/class.h>
+#include <nvif/if0001.h>
 #include <nvif/ioctl.h>
 #include <nvif/unpack.h>
 
@@ -37,10 +38,10 @@ nvkm_control_mthd_pstate_info(struct nvkm_control *ctrl, void *data, u32 size)
 		struct nvif_control_pstate_info_v0 v0;
 	} *args = data;
 	struct nvkm_clk *clk = ctrl->device->clk;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(&ctrl->object, "control pstate info size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(&ctrl->object, "control pstate info vers %d\n",
 			   args->v0.version);
 	} else
@@ -75,10 +76,10 @@ nvkm_control_mthd_pstate_attr(struct nvkm_control *ctrl, void *data, u32 size)
 	struct nvkm_cstate *cstate;
 	int i = 0, j = -1;
 	u32 lo, hi;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(&ctrl->object, "control pstate attr size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(&ctrl->object,
 			   "control pstate attr vers %d state %d index %d\n",
 			   args->v0.version, args->v0.state, args->v0.index);
@@ -143,10 +144,10 @@ nvkm_control_mthd_pstate_user(struct nvkm_control *ctrl, void *data, u32 size)
 		struct nvif_control_pstate_user_v0 v0;
 	} *args = data;
 	struct nvkm_clk *clk = ctrl->device->clk;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(&ctrl->object, "control pstate user size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(&ctrl->object,
 			   "control pstate user vers %d ustate %d pwrsrc %d\n",
 			   args->v0.version, args->v0.ustate, args->v0.pwrsrc);
@@ -204,7 +205,7 @@ nvkm_control_new(struct nvkm_device *device, const struct nvkm_oclass *oclass,
 
 const struct nvkm_device_oclass
 nvkm_control_oclass = {
-	.base.oclass = NVIF_IOCTL_NEW_V0_CONTROL,
+	.base.oclass = NVIF_CLASS_CONTROL,
 	.base.minver = -1,
 	.base.maxver = -1,
 	.ctor = nvkm_control_new,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
index 7f8a42721eb2..e7e581d6a8ff 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/tegra.c
@@ -252,32 +252,40 @@ nvkm_device_tegra_new(const struct nvkm_device_tegra_func *func,
 
 	if (!(tdev = kzalloc(sizeof(*tdev), GFP_KERNEL)))
 		return -ENOMEM;
-	*pdevice = &tdev->device;
+
 	tdev->func = func;
 	tdev->pdev = pdev;
 	tdev->irq = -1;
 
 	tdev->vdd = devm_regulator_get(&pdev->dev, "vdd");
-	if (IS_ERR(tdev->vdd))
-		return PTR_ERR(tdev->vdd);
+	if (IS_ERR(tdev->vdd)) {
+		ret = PTR_ERR(tdev->vdd);
+		goto free;
+	}
 
 	tdev->rst = devm_reset_control_get(&pdev->dev, "gpu");
-	if (IS_ERR(tdev->rst))
-		return PTR_ERR(tdev->rst);
+	if (IS_ERR(tdev->rst)) {
+		ret = PTR_ERR(tdev->rst);
+		goto free;
+	}
 
 	tdev->clk = devm_clk_get(&pdev->dev, "gpu");
-	if (IS_ERR(tdev->clk))
-		return PTR_ERR(tdev->clk);
+	if (IS_ERR(tdev->clk)) {
+		ret = PTR_ERR(tdev->clk);
+		goto free;
+	}
 
 	tdev->clk_pwr = devm_clk_get(&pdev->dev, "pwr");
-	if (IS_ERR(tdev->clk_pwr))
-		return PTR_ERR(tdev->clk_pwr);
+	if (IS_ERR(tdev->clk_pwr)) {
+		ret = PTR_ERR(tdev->clk_pwr);
+		goto free;
+	}
 
 	nvkm_device_tegra_probe_iommu(tdev);
 
 	ret = nvkm_device_tegra_power_up(tdev);
 	if (ret)
-		return ret;
+		goto remove;
 
 	tdev->gpu_speedo = tegra_sku_info.gpu_speedo_value;
 	ret = nvkm_device_ctor(&nvkm_device_tegra_func, NULL, &pdev->dev,
@@ -285,9 +293,19 @@ nvkm_device_tegra_new(const struct nvkm_device_tegra_func *func,
 			       cfg, dbg, detect, mmio, subdev_mask,
 			       &tdev->device);
 	if (ret)
-		return ret;
+		goto powerdown;
+
+	*pdevice = &tdev->device;
 
 	return 0;
+
+powerdown:
+	nvkm_device_tegra_power_down(tdev);
+remove:
+	nvkm_device_tegra_remove_iommu(tdev);
+free:
+	kfree(tdev);
+	return ret;
 }
 #else
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/device/user.c b/drivers/gpu/drm/nouveau/nvkm/engine/device/user.c
index 1ae48f27029d..137066426ed7 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/device/user.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/device/user.c
@@ -31,6 +31,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cl0080.h>
 #include <nvif/unpack.h>
 
 struct nvkm_udevice {
@@ -48,10 +49,10 @@ nvkm_udevice_info(struct nvkm_udevice *udev, void *data, u32 size)
 	union {
 		struct nv_device_info_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "device info size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "device info vers %d\n", args->v0.version);
 	} else
 		return ret;
@@ -123,13 +124,16 @@ nvkm_udevice_info(struct nvkm_udevice *udev, void *data, u32 size)
 static int
 nvkm_udevice_time(struct nvkm_udevice *udev, void *data, u32 size)
 {
+	struct nvkm_object *object = &udev->object;
 	struct nvkm_device *device = udev->device;
 	union {
 		struct nv_device_time_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	nvif_ioctl(object, "device time size %d\n", size);
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
+		nvif_ioctl(object, "device time vers %d\n", args->v0.version);
 		args->v0.time = nvkm_timer_read(device->timer);
 	}
 
@@ -140,6 +144,7 @@ static int
 nvkm_udevice_mthd(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 {
 	struct nvkm_udevice *udev = nvkm_udevice(object);
+	nvif_ioctl(object, "device mthd %08x\n", mthd);
 	switch (mthd) {
 	case NV_DEVICE_V0_INFO:
 		return nvkm_udevice_info(udev, data, size);
@@ -331,10 +336,10 @@ nvkm_udevice_new(const struct nvkm_oclass *oclass, void *data, u32 size,
 	struct nvkm_object *parent = &client->object;
 	const struct nvkm_object_func *func;
 	struct nvkm_udevice *udev;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create device size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create device v%d device %016llx\n",
 			   args->v0.version, args->v0.device);
 	} else
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/base.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/base.c
index 44b67719f64d..785fa76d0fbf 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/base.c
@@ -32,6 +32,7 @@
 #include <subdev/bios/dcb.h>
 
 #include <nvif/class.h>
+#include <nvif/cl0046.h>
 #include <nvif/event.h>
 #include <nvif/unpack.h>
 
@@ -58,9 +59,9 @@ nvkm_disp_vblank_ctor(struct nvkm_object *object, void *data, u32 size,
 	union {
 		struct nvif_notify_head_req_v0 v0;
 	} *req = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(req->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, req->v0, 0, 0, false))) {
 		notify->size = sizeof(struct nvif_notify_head_rep_v0);
 		if (ret = -ENXIO, req->v0.head <= disp->vblank.index_nr) {
 			notify->types = 1;
@@ -96,9 +97,9 @@ nvkm_disp_hpd_ctor(struct nvkm_object *object, void *data, u32 size,
 		struct nvif_notify_conn_req_v0 v0;
 	} *req = data;
 	struct nvkm_output *outp;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(req->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, req->v0, 0, 0, false))) {
 		notify->size = sizeof(struct nvif_notify_conn_rep_v0);
 		list_for_each_entry(outp, &disp->outp, head) {
 			if (ret = -ENXIO, outp->conn->index == req->v0.conn) {
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/basenv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/basenv50.c
index 1fd89edefc26..83f152300ec0 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/basenv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/basenv50.c
@@ -27,6 +27,7 @@
 #include <core/client.h>
 
 #include <nvif/class.h>
+#include <nvif/cl507c.h>
 #include <nvif/unpack.h>
 
 int
@@ -41,11 +42,11 @@ nv50_disp_base_new(const struct nv50_disp_dmac_func *func,
 	} *args = data;
 	struct nvkm_object *parent = oclass->parent;
 	struct nv50_disp *disp = root->disp;
-	int head, ret;
+	int head, ret = -ENOSYS;
 	u64 push;
 
 	nvif_ioctl(parent, "create disp base channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create disp base channel dma vers %d "
 				   "pushbuf %016llx head %d\n",
 			   args->v0.version, args->v0.pushbuf, args->v0.head);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/channv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/channv50.c
index 01803c0679b6..dd2953bc9264 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/channv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/channv50.c
@@ -28,7 +28,7 @@
 #include <core/ramht.h>
 #include <engine/dma.h>
 
-#include <nvif/class.h>
+#include <nvif/cl507d.h>
 #include <nvif/event.h>
 #include <nvif/unpack.h>
 
@@ -134,9 +134,9 @@ nv50_disp_chan_uevent_ctor(struct nvkm_object *object, void *data, u32 size,
 	union {
 		struct nvif_notify_uevent_req none;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unvers(args->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, args->none))) {
 		notify->size  = sizeof(struct nvif_notify_uevent_rep);
 		notify->types = 1;
 		notify->index = chan->chid;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/corenv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/corenv50.c
index db4a9b3e0e09..b547c8b833ca 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/corenv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/corenv50.c
@@ -28,6 +28,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cl507d.h>
 #include <nvif/unpack.h>
 
 int
@@ -42,10 +43,10 @@ nv50_disp_core_new(const struct nv50_disp_dmac_func *func,
 	} *args = data;
 	struct nvkm_object *parent = oclass->parent;
 	u64 push;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create disp core channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create disp core channel dma vers %d "
 				   "pushbuf %016llx\n",
 			   args->v0.version, args->v0.pushbuf);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/cursnv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/cursnv50.c
index 225858e62cf6..8b1320499a0f 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/cursnv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/cursnv50.c
@@ -27,6 +27,7 @@
 #include <core/client.h>
 
 #include <nvif/class.h>
+#include <nvif/cl507a.h>
 #include <nvif/unpack.h>
 
 int
@@ -41,10 +42,10 @@ nv50_disp_curs_new(const struct nv50_disp_chan_func *func,
 	} *args = data;
 	struct nvkm_object *parent = oclass->parent;
 	struct nv50_disp *disp = root->disp;
-	int head, ret;
+	int head, ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create disp cursor size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create disp cursor vers %d head %d\n",
 			   args->v0.version, args->v0.head);
 		if (args->v0.head > disp->base.head.nr)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dacnv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dacnv50.c
index 9bfa9e7dc161..c9b78b8f9c87 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dacnv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dacnv50.c
@@ -27,7 +27,7 @@
 #include <core/client.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -39,10 +39,10 @@ nv50_dac_power(NV50_DISP_MTHD_V1)
 		struct nv50_disp_dac_pwr_v0 v0;
 	} *args = data;
 	u32 stat;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp dac pwr size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp dac pwr vers %d state %d data %d "
 				   "vsync %d hsync %d\n",
 			   args->v0.version, args->v0.state, args->v0.data,
@@ -76,10 +76,10 @@ nv50_dac_sense(NV50_DISP_MTHD_V1)
 	} *args = data;
 	const u32 doff = outp->or * 0x800;
 	u32 loadval;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp dac load size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp dac load vers %d data %08x\n",
 			   args->v0.version, args->v0.data);
 		if (args->v0.data & 0xfff00000)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.c
index 74e2f7c6c07e..9688970eca47 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.c
@@ -328,6 +328,7 @@ nvkm_dp_train(struct work_struct *w)
 		.outp = outp,
 	}, *dp = &_dp;
 	u32 datarate = 0;
+	u8  pwr;
 	int ret;
 
 	if (!outp->base.info.location && disp->func->sor.magic)
@@ -355,6 +356,15 @@ nvkm_dp_train(struct work_struct *w)
 	/* disable link interrupt handling during link training */
 	nvkm_notify_put(&outp->irq);
 
+	/* ensure sink is not in a low-power state */
+	if (!nvkm_rdaux(outp->aux, DPCD_SC00, &pwr, 1)) {
+		if ((pwr & DPCD_SC00_SET_POWER) != DPCD_SC00_SET_POWER_D0) {
+			pwr &= ~DPCD_SC00_SET_POWER;
+			pwr |=  DPCD_SC00_SET_POWER_D0;
+			nvkm_wraux(outp->aux, DPCD_SC00, &pwr, 1);
+		}
+	}
+
 	/* enable down-spreading and execute pre-train script from vbios */
 	dp_link_train_init(dp, outp->dpcd[3] & 0x01);
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.h b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.h
index 9596290329c7..6e10c5e0ef11 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.h
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/dport.h
@@ -71,5 +71,11 @@
 #define DPCD_LS0C_LANE1_POST_CURSOR2                                       0x0c
 #define DPCD_LS0C_LANE0_POST_CURSOR2                                       0x03
 
+/* DPCD Sink Control */
+#define DPCD_SC00                                                       0x00600
+#define DPCD_SC00_SET_POWER                                                0x03
+#define DPCD_SC00_SET_POWER_D0                                             0x01
+#define DPCD_SC00_SET_POWER_D3                                             0x03
+
 void nvkm_dp_train(struct work_struct *);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/gf119.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/gf119.c
index 186fd3ac78f6..f0314664349c 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/gf119.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/gf119.c
@@ -158,7 +158,7 @@ exec_clkcmp(struct nv50_disp *disp, int head, int id, u32 pclk, u32 *conf)
 	switch (outp->info.type) {
 	case DCB_OUTPUT_TMDS:
 		*conf = (ctrl & 0x00000f00) >> 8;
-		if (pclk >= 165000)
+		if (*conf == 5)
 			*conf |= 0x0100;
 		break;
 	case DCB_OUTPUT_LVDS:
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagf119.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagf119.c
index af99efbd63f7..da6129b2b78f 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagf119.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagf119.c
@@ -29,7 +29,7 @@
 #include <subdev/bios/dcb.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -41,10 +41,10 @@ gf119_hda_eld(NV50_DISP_MTHD_V1)
 	} *args = data;
 	const u32 soff = outp->or * 0x030;
 	const u32 hoff = head * 0x800;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(object, "disp sor hda eld size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "disp sor hda eld vers %d\n",
 			   args->v0.version);
 		if (size > 0x60)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagt215.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagt215.c
index c1590b746f13..6f0436df0219 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagt215.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdagt215.c
@@ -27,7 +27,7 @@
 #include <core/client.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -38,10 +38,10 @@ gt215_hda_eld(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hda_eld_v0 v0;
 	} *args = data;
 	const u32 soff = outp->or * 0x800;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(object, "disp sor hda eld size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "disp sor hda eld vers %d\n",
 			   args->v0.version);
 		if (size > 0x60)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmig84.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmig84.c
index ee9e800a8f06..1c4256e8cbd6 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmig84.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmig84.c
@@ -25,7 +25,7 @@
 
 #include <core/client.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -37,10 +37,10 @@ g84_hdmi_ctrl(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hdmi_pwr_v0 v0;
 	} *args = data;
 	u32 ctrl;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp sor hdmi ctrl size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp sor hdmi ctrl vers %d state %d "
 				   "max_ac_packet %d rekey %d\n",
 			   args->v0.version, args->v0.state,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigf119.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigf119.c
index b5af025d3b04..632f02da1382 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigf119.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigf119.c
@@ -25,7 +25,7 @@
 
 #include <core/client.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -37,10 +37,10 @@ gf119_hdmi_ctrl(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hdmi_pwr_v0 v0;
 	} *args = data;
 	u32 ctrl;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp sor hdmi ctrl size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp sor hdmi ctrl vers %d state %d "
 				   "max_ac_packet %d rekey %d\n",
 			   args->v0.version, args->v0.state,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigk104.c
index 110dc19e4f67..4e8067d511d7 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigk104.c
@@ -25,7 +25,7 @@
 
 #include <core/client.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -38,10 +38,10 @@ gk104_hdmi_ctrl(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hdmi_pwr_v0 v0;
 	} *args = data;
 	u32 ctrl;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp sor hdmi ctrl size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp sor hdmi ctrl vers %d state %d "
 				   "max_ac_packet %d rekey %d\n",
 			   args->v0.version, args->v0.state,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigt215.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigt215.c
index 61237dbfa35a..f1afc16494b6 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigt215.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/hdmigt215.c
@@ -26,7 +26,7 @@
 
 #include <core/client.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -38,10 +38,10 @@ gt215_hdmi_ctrl(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hdmi_pwr_v0 v0;
 	} *args = data;
 	u32 ctrl;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp sor hdmi ctrl size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp sor hdmi ctrl vers %d state %d "
 				   "max_ac_packet %d rekey %d\n",
 			   args->v0.version, args->v0.state,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/nv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/nv50.c
index 32e73a975b58..4226d2153b9c 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/nv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/nv50.c
@@ -391,7 +391,7 @@ exec_clkcmp(struct nv50_disp *disp, int head, int id, u32 pclk, u32 *conf)
 		switch (outp->info.type) {
 		case DCB_OUTPUT_TMDS:
 			*conf = (ctrl & 0x00000f00) >> 8;
-			if (pclk >= 165000)
+			if (*conf == 5)
 				*conf |= 0x0100;
 			break;
 		case DCB_OUTPUT_LVDS:
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/oimmnv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/oimmnv50.c
index cd888a1e443c..3940b9c966ec 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/oimmnv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/oimmnv50.c
@@ -27,6 +27,7 @@
 #include <core/client.h>
 
 #include <nvif/class.h>
+#include <nvif/cl507b.h>
 #include <nvif/unpack.h>
 
 int
@@ -41,10 +42,10 @@ nv50_disp_oimm_new(const struct nv50_disp_chan_func *func,
 	} *args = data;
 	struct nvkm_object *parent = oclass->parent;
 	struct nv50_disp *disp = root->disp;
-	int head, ret;
+	int head, ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create disp overlay size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create disp overlay vers %d head %d\n",
 			   args->v0.version, args->v0.head);
 		if (args->v0.head > disp->base.head.nr)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/ovlynv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/ovlynv50.c
index 6fa296c047b8..2a49c46425cd 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/ovlynv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/ovlynv50.c
@@ -27,6 +27,7 @@
 #include <core/client.h>
 
 #include <nvif/class.h>
+#include <nvif/cl507e.h>
 #include <nvif/unpack.h>
 
 int
@@ -41,11 +42,11 @@ nv50_disp_ovly_new(const struct nv50_disp_dmac_func *func,
 	} *args = data;
 	struct nvkm_object *parent = oclass->parent;
 	struct nv50_disp *disp = root->disp;
-	int head, ret;
+	int head, ret = -ENOSYS;
 	u64 push;
 
 	nvif_ioctl(parent, "create disp overlay channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create disp overlay channel dma vers %d "
 				   "pushbuf %016llx head %d\n",
 			   args->v0.version, args->v0.pushbuf, args->v0.head);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/piornv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/piornv50.c
index ab524bde7795..6c532eadba17 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/piornv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/piornv50.c
@@ -28,7 +28,7 @@
 #include <subdev/i2c.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -40,10 +40,10 @@ nv50_pior_power(NV50_DISP_MTHD_V1)
 		struct nv50_disp_pior_pwr_v0 v0;
 	} *args = data;
 	u32 ctrl, type;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp pior pwr size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp pior pwr vers %d state %d type %x\n",
 			   args->v0.version, args->v0.state, args->v0.type);
 		if (args->v0.type > 0x0f)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootgf119.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootgf119.c
index 8591726871ac..335d88823c22 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootgf119.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootgf119.c
@@ -29,6 +29,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -39,12 +40,12 @@ gf119_disp_root_scanoutpos(NV50_DISP_MTHD_V0)
 	const u32 blanke = nvkm_rd32(device, 0x64041c + (head * 0x300));
 	const u32 blanks = nvkm_rd32(device, 0x640420 + (head * 0x300));
 	union {
-		struct nv04_disp_scanoutpos_v0 v0;
+		struct nv50_disp_scanoutpos_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp scanoutpos size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp scanoutpos vers %d\n",
 			   args->v0.version);
 		args->v0.vblanke = (blanke & 0xffff0000) >> 16;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv04.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv04.c
index 2be846374d39..f535f43231e2 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv04.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv04.c
@@ -27,6 +27,7 @@
 #include <core/client.h>
 
 #include <nvif/class.h>
+#include <nvif/cl0046.h>
 #include <nvif/unpack.h>
 
 struct nv04_disp_root {
@@ -45,10 +46,10 @@ nv04_disp_scanoutpos(struct nv04_disp_root *root,
 		struct nv04_disp_scanoutpos_v0 v0;
 	} *args = data;
 	u32 line;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp scanoutpos size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp scanoutpos vers %d\n",
 			   args->v0.version);
 		args->v0.vblanks = nvkm_rd32(device, 0x680800 + hoff) & 0xffff;
@@ -85,10 +86,10 @@ nv04_disp_mthd(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 	union {
 		struct nv04_disp_mthd_v0 v0;
 	} *args = data;
-	int head, ret;
+	int head, ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp mthd size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "disp mthd vers %d mthd %02x head %d\n",
 			   args->v0.version, args->v0.method, args->v0.head);
 		mthd = args->v0.method;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv50.c
index 06fb24d88702..2f9cecd81d04 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/rootnv50.c
@@ -29,6 +29,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -39,12 +40,12 @@ nv50_disp_root_scanoutpos(NV50_DISP_MTHD_V0)
 	const u32 blanks = nvkm_rd32(device, 0x610af4 + (head * 0x540));
 	const u32 total  = nvkm_rd32(device, 0x610afc + (head * 0x540));
 	union {
-		struct nv04_disp_scanoutpos_v0 v0;
+		struct nv50_disp_scanoutpos_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp scanoutpos size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp scanoutpos vers %d\n",
 			   args->v0.version);
 		args->v0.vblanke = (blanke & 0xffff0000) >> 16;
@@ -78,19 +79,19 @@ nv50_disp_root_mthd_(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 	struct nvkm_output *outp = NULL;
 	struct nvkm_output *temp;
 	u16 type, mask = 0;
-	int head, ret;
+	int head, ret = -ENOSYS;
 
 	if (mthd != NV50_DISP_MTHD)
 		return -EINVAL;
 
 	nvif_ioctl(object, "disp mthd size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(object, "disp mthd vers %d mthd %02x head %d\n",
 			   args->v0.version, args->v0.method, args->v0.head);
 		mthd = args->v0.method;
 		head = args->v0.head;
 	} else
-	if (nvif_unpack(args->v1, 1, 1, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v1, 1, 1, true))) {
 		nvif_ioctl(object, "disp mthd vers %d mthd %02x "
 				   "type %04x mask %04x\n",
 			   args->v1.version, args->v1.method,
@@ -143,8 +144,9 @@ nv50_disp_root_mthd_(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 		union {
 			struct nv50_disp_sor_lvds_script_v0 v0;
 		} *args = data;
+		int ret = -ENOSYS;
 		nvif_ioctl(object, "disp sor lvds script size %d\n", size);
-		if (nvif_unpack(args->v0, 0, 0, false)) {
+		if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 			nvif_ioctl(object, "disp sor lvds script "
 					   "vers %d name %04x\n",
 				   args->v0.version, args->v0.script);
@@ -159,8 +161,9 @@ nv50_disp_root_mthd_(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 		union {
 			struct nv50_disp_sor_dp_pwr_v0 v0;
 		} *args = data;
+		int ret = -ENOSYS;
 		nvif_ioctl(object, "disp sor dp pwr size %d\n", size);
-		if (nvif_unpack(args->v0, 0, 0, false)) {
+		if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 			nvif_ioctl(object, "disp sor dp pwr vers %d state %d\n",
 				   args->v0.version, args->v0.state);
 			if (args->v0.state == 0) {
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/disp/sornv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/disp/sornv50.c
index 29e0d2a9a839..53596bed3c36 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/disp/sornv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/disp/sornv50.c
@@ -27,7 +27,7 @@
 #include <core/client.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl5070.h>
 #include <nvif/unpack.h>
 
 int
@@ -39,10 +39,10 @@ nv50_sor_power(NV50_DISP_MTHD_V1)
 	} *args = data;
 	const u32 soff = outp->or * 0x800;
 	u32 stat;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "disp sor pwr size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "disp sor pwr vers %d state %d\n",
 			   args->v0.version, args->v0.state);
 		stat = !!args->v0.state;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/dma/user.c b/drivers/gpu/drm/nouveau/nvkm/engine/dma/user.c
index 45ab062661a4..13c661b1ef14 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/dma/user.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/dma/user.c
@@ -28,7 +28,7 @@
 #include <subdev/fb.h>
 #include <subdev/instmem.h>
 
-#include <nvif/class.h>
+#include <nvif/cl0002.h>
 #include <nvif/unpack.h>
 
 static int
@@ -69,7 +69,7 @@ nvkm_dmaobj_ctor(const struct nvkm_dmaobj_func *func, struct nvkm_dma *dma,
 	struct nvkm_fb *fb = device->fb;
 	void *data = *pdata;
 	u32 size = *psize;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvkm_object_ctor(&nvkm_dmaobj_func, oclass, &dmaobj->object);
 	dmaobj->func = func;
@@ -77,7 +77,7 @@ nvkm_dmaobj_ctor(const struct nvkm_dmaobj_func *func, struct nvkm_dma *dma,
 	RB_CLEAR_NODE(&dmaobj->rb);
 
 	nvif_ioctl(parent, "create dma size %d\n", *psize);
-	if (nvif_unpack(args->v0, 0, 0, true)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, true))) {
 		nvif_ioctl(parent, "create dma vers %d target %d access %d "
 				   "start %016llx limit %016llx\n",
 			   args->v0.version, args->v0.target, args->v0.access,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf100.c
index 13e341cc4e32..ef7ac360101e 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf100.c
@@ -28,7 +28,7 @@
 #include <core/gpuobj.h>
 #include <subdev/fb.h>
 
-#include <nvif/class.h>
+#include <nvif/cl0002.h>
 #include <nvif/unpack.h>
 
 struct gf100_dmaobj {
@@ -87,10 +87,11 @@ gf100_dmaobj_new(struct nvkm_dma *dma, const struct nvkm_oclass *oclass,
 	if (ret)
 		return ret;
 
+	ret  = -ENOSYS;
 	args = data;
 
 	nvif_ioctl(parent, "create gf100 dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent,
 			   "create gf100 dma vers %d priv %d kind %02x\n",
 			   args->v0.version, args->v0.priv, args->v0.kind);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf119.c b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf119.c
index 0e1af8b4db84..c068cee34588 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf119.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usergf119.c
@@ -28,7 +28,7 @@
 #include <core/gpuobj.h>
 #include <subdev/fb.h>
 
-#include <nvif/class.h>
+#include <nvif/cl0002.h>
 #include <nvif/unpack.h>
 
 struct gf119_dmaobj {
@@ -85,10 +85,11 @@ gf119_dmaobj_new(struct nvkm_dma *dma, const struct nvkm_oclass *oclass,
 	if (ret)
 		return ret;
 
+	ret  = -ENOSYS;
 	args = data;
 
 	nvif_ioctl(parent, "create gf119 dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent,
 			   "create gf100 dma vers %d page %d kind %02x\n",
 			   args->v0.version, args->v0.page, args->v0.kind);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usernv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usernv50.c
index 5b7ce313ea14..6a85b5dea643 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/dma/usernv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/dma/usernv50.c
@@ -28,7 +28,7 @@
 #include <core/gpuobj.h>
 #include <subdev/fb.h>
 
-#include <nvif/class.h>
+#include <nvif/cl0002.h>
 #include <nvif/unpack.h>
 
 struct nv50_dmaobj {
@@ -87,10 +87,11 @@ nv50_dmaobj_new(struct nvkm_dma *dma, const struct nvkm_oclass *oclass,
 	if (ret)
 		return ret;
 
+	ret  = -ENOSYS;
 	args = data;
 
 	nvif_ioctl(parent, "create nv50 dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create nv50 dma vers %d priv %d part %d "
 				   "comp %d kind %02x\n", args->v0.version,
 			   args->v0.priv, args->v0.part, args->v0.comp,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/base.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/base.c
index 1fbbfbe6ca9c..cfc7d5725a61 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/base.c
@@ -129,9 +129,9 @@ nvkm_fifo_uevent_ctor(struct nvkm_object *object, void *data, u32 size,
 	union {
 		struct nvif_notify_uevent_req none;
 	} *req = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unvers(req->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, req->none))) {
 		notify->size  = sizeof(struct nvif_notify_uevent_rep);
 		notify->types = 1;
 		notify->index = 0;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/chang84.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/chang84.c
index 04305241ceed..aeb3387a3fb0 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/chang84.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/chang84.c
@@ -28,7 +28,7 @@
 #include <subdev/mmu.h>
 #include <subdev/timer.h>
 
-#include <nvif/class.h>
+#include <nvif/cl826e.h>
 
 int
 g84_fifo_chan_ntfy(struct nvkm_fifo_chan *chan, u32 type,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmag84.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmag84.c
index a5ca52c7b74f..caa914074752 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmag84.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmag84.c
@@ -27,6 +27,7 @@
 #include <core/ramht.h>
 
 #include <nvif/class.h>
+#include <nvif/cl826e.h>
 #include <nvif/unpack.h>
 
 static int
@@ -35,14 +36,14 @@ g84_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 {
 	struct nvkm_object *parent = oclass->parent;
 	union {
-		struct nv50_channel_dma_v0 v0;
+		struct g82_channel_dma_v0 v0;
 	} *args = data;
 	struct nv50_fifo *fifo = nv50_fifo(base);
 	struct nv50_fifo_chan *chan;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d vm %llx "
 				   "pushbuf %llx offset %016llx\n",
 			   args->v0.version, args->v0.vm, args->v0.pushbuf,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
index bfcc6408a772..edec30fd3ecd 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv04.c
@@ -29,6 +29,7 @@
 #include <subdev/instmem.h>
 
 #include <nvif/class.h>
+#include <nvif/cl006b.h>
 #include <nvif/unpack.h>
 
 void
@@ -167,10 +168,10 @@ nv04_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nv04_fifo_chan *chan = NULL;
 	struct nvkm_device *device = fifo->base.engine.subdev.device;
 	struct nvkm_instmem *imem = device->imem;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d pushbuf %llx "
 				   "offset %08x\n", args->v0.version,
 			   args->v0.pushbuf, args->v0.offset);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv10.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv10.c
index 34f68e5bd040..f5f355ff005d 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv10.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv10.c
@@ -29,6 +29,7 @@
 #include <subdev/instmem.h>
 
 #include <nvif/class.h>
+#include <nvif/cl006b.h>
 #include <nvif/unpack.h>
 
 static int
@@ -43,10 +44,10 @@ nv10_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nv04_fifo_chan *chan = NULL;
 	struct nvkm_device *device = fifo->base.engine.subdev.device;
 	struct nvkm_instmem *imem = device->imem;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d pushbuf %llx "
 				   "offset %08x\n", args->v0.version,
 			   args->v0.pushbuf, args->v0.offset);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv17.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv17.c
index ed7cc9f2b540..7edc6a564b5d 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv17.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv17.c
@@ -29,6 +29,7 @@
 #include <subdev/instmem.h>
 
 #include <nvif/class.h>
+#include <nvif/cl006b.h>
 #include <nvif/unpack.h>
 
 static int
@@ -43,10 +44,10 @@ nv17_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nv04_fifo_chan *chan = NULL;
 	struct nvkm_device *device = fifo->base.engine.subdev.device;
 	struct nvkm_instmem *imem = device->imem;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d pushbuf %llx "
 				   "offset %08x\n", args->v0.version,
 			   args->v0.pushbuf, args->v0.offset);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv40.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv40.c
index 043b6c325949..0ec179fc40a1 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv40.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv40.c
@@ -29,6 +29,7 @@
 #include <subdev/instmem.h>
 
 #include <nvif/class.h>
+#include <nvif/cl006b.h>
 #include <nvif/unpack.h>
 
 static bool
@@ -188,10 +189,10 @@ nv40_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nv04_fifo_chan *chan = NULL;
 	struct nvkm_device *device = fifo->base.engine.subdev.device;
 	struct nvkm_instmem *imem = device->imem;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d pushbuf %llx "
 				   "offset %08x\n", args->v0.version,
 			   args->v0.pushbuf, args->v0.offset);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv50.c
index 6b3b15f12c39..480bc3777be5 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/dmanv50.c
@@ -27,6 +27,7 @@
 #include <core/ramht.h>
 
 #include <nvif/class.h>
+#include <nvif/cl506e.h>
 #include <nvif/unpack.h>
 
 static int
@@ -39,10 +40,10 @@ nv50_fifo_dma_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	} *args = data;
 	struct nv50_fifo *fifo = nv50_fifo(base);
 	struct nv50_fifo_chan *chan;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel dma size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel dma vers %d vm %llx "
 				   "pushbuf %llx offset %016llx\n",
 			   args->v0.version, args->v0.vm, args->v0.pushbuf,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.c
index ff6fcbda615b..36a39c7fd8d2 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.c
@@ -47,7 +47,7 @@ gf100_fifo_uevent_fini(struct nvkm_fifo *fifo)
 }
 
 void
-gf100_fifo_runlist_update(struct gf100_fifo *fifo)
+gf100_fifo_runlist_commit(struct gf100_fifo *fifo)
 {
 	struct gf100_fifo_chan *chan;
 	struct nvkm_subdev *subdev = &fifo->base.engine.subdev;
@@ -77,6 +77,22 @@ gf100_fifo_runlist_update(struct gf100_fifo *fifo)
 	mutex_unlock(&subdev->mutex);
 }
 
+void
+gf100_fifo_runlist_remove(struct gf100_fifo *fifo, struct gf100_fifo_chan *chan)
+{
+	mutex_lock(&fifo->base.engine.subdev.mutex);
+	list_del_init(&chan->head);
+	mutex_unlock(&fifo->base.engine.subdev.mutex);
+}
+
+void
+gf100_fifo_runlist_insert(struct gf100_fifo *fifo, struct gf100_fifo_chan *chan)
+{
+	mutex_lock(&fifo->base.engine.subdev.mutex);
+	list_add_tail(&chan->head, &fifo->chan);
+	mutex_unlock(&fifo->base.engine.subdev.mutex);
+}
+
 static inline int
 gf100_fifo_engidx(struct gf100_fifo *fifo, u32 engn)
 {
@@ -139,7 +155,7 @@ gf100_fifo_recover_work(struct work_struct *work)
 		}
 	}
 
-	gf100_fifo_runlist_update(fifo);
+	gf100_fifo_runlist_commit(fifo);
 	nvkm_wr32(device, 0x00262c, engm);
 	nvkm_mask(device, 0x002630, engm, 0x00000000);
 }
@@ -239,7 +255,7 @@ gf100_fifo_fault_engine[] = {
 	{ 0x14, "PMSPDEC", NULL, NVKM_ENGINE_MSPDEC },
 	{ 0x15, "PCE0", NULL, NVKM_ENGINE_CE0 },
 	{ 0x16, "PCE1", NULL, NVKM_ENGINE_CE1 },
-	{ 0x17, "PDAEMON" },
+	{ 0x17, "PMU" },
 	{}
 };
 
@@ -270,7 +286,7 @@ gf100_fifo_fault_hubclient[] = {
 	{ 0x0c, "PMSPPP" },
 	{ 0x0d, "PMSVLD" },
 	{ 0x11, "PCOUNTER" },
-	{ 0x12, "PDAEMON" },
+	{ 0x12, "PMU" },
 	{ 0x14, "CCACHE" },
 	{ 0x15, "CCACHE_POST" },
 	{}
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.h b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.h
index c649ca9b53e3..08c33c3ceaf7 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.h
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gf100.h
@@ -5,6 +5,7 @@
 
 #include <subdev/mmu.h>
 
+struct gf100_fifo_chan;
 struct gf100_fifo {
 	struct nvkm_fifo base;
 
@@ -27,5 +28,7 @@ struct gf100_fifo {
 };
 
 void gf100_fifo_intr_engine(struct gf100_fifo *);
-void gf100_fifo_runlist_update(struct gf100_fifo *);
+void gf100_fifo_runlist_insert(struct gf100_fifo *, struct gf100_fifo_chan *);
+void gf100_fifo_runlist_remove(struct gf100_fifo *, struct gf100_fifo_chan *);
+void gf100_fifo_runlist_commit(struct gf100_fifo *);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
index 98970a0b7a66..4fcd147d43c8 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.c
@@ -47,7 +47,7 @@ gk104_fifo_uevent_init(struct nvkm_fifo *fifo)
 }
 
 void
-gk104_fifo_runlist_update(struct gk104_fifo *fifo, u32 engine)
+gk104_fifo_runlist_commit(struct gk104_fifo *fifo, u32 engine)
 {
 	struct gk104_fifo_engn *engn = &fifo->engine[engine];
 	struct gk104_fifo_chan *chan;
@@ -78,6 +78,22 @@ gk104_fifo_runlist_update(struct gk104_fifo *fifo, u32 engine)
 	mutex_unlock(&subdev->mutex);
 }
 
+void
+gk104_fifo_runlist_remove(struct gk104_fifo *fifo, struct gk104_fifo_chan *chan)
+{
+	mutex_lock(&fifo->base.engine.subdev.mutex);
+	list_del_init(&chan->head);
+	mutex_unlock(&fifo->base.engine.subdev.mutex);
+}
+
+void
+gk104_fifo_runlist_insert(struct gk104_fifo *fifo, struct gk104_fifo_chan *chan)
+{
+	mutex_lock(&fifo->base.engine.subdev.mutex);
+	list_add_tail(&chan->head, &fifo->engine[chan->engine].chan);
+	mutex_unlock(&fifo->base.engine.subdev.mutex);
+}
+
 static inline struct nvkm_engine *
 gk104_fifo_engine(struct gk104_fifo *fifo, u32 engn)
 {
@@ -112,7 +128,7 @@ gk104_fifo_recover_work(struct work_struct *work)
 			nvkm_subdev_fini(&engine->subdev, false);
 			WARN_ON(nvkm_subdev_init(&engine->subdev));
 		}
-		gk104_fifo_runlist_update(fifo, gk104_fifo_subdev_engine(engn));
+		gk104_fifo_runlist_commit(fifo, gk104_fifo_subdev_engine(engn));
 	}
 
 	nvkm_wr32(device, 0x00262c, engm);
@@ -180,7 +196,7 @@ gk104_fifo_intr_sched_ctxsw(struct gk104_fifo *fifo)
 
 	spin_lock_irqsave(&fifo->base.lock, flags);
 	for (engn = 0; engn < ARRAY_SIZE(fifo->engine); engn++) {
-		u32 stat = nvkm_rd32(device, 0x002640 + (engn * 0x04));
+		u32 stat = nvkm_rd32(device, 0x002640 + (engn * 0x08));
 		u32 busy = (stat & 0x80000000);
 		u32 next = (stat & 0x07ff0000) >> 16;
 		u32 chsw = (stat & 0x00008000);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.h b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.h
index 5afd9b5ec5d1..bec519d8f91e 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.h
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gk104.h
@@ -5,6 +5,7 @@
 
 #include <subdev/mmu.h>
 
+struct gk104_fifo_chan;
 struct gk104_fifo_engn {
 	struct nvkm_memory *runlist[2];
 	int cur_runlist;
@@ -35,7 +36,9 @@ void gk104_fifo_fini(struct nvkm_fifo *);
 void gk104_fifo_intr(struct nvkm_fifo *);
 void gk104_fifo_uevent_init(struct nvkm_fifo *);
 void gk104_fifo_uevent_fini(struct nvkm_fifo *);
-void gk104_fifo_runlist_update(struct gk104_fifo *, u32 engine);
+void gk104_fifo_runlist_insert(struct gk104_fifo *, struct gk104_fifo_chan *);
+void gk104_fifo_runlist_remove(struct gk104_fifo *, struct gk104_fifo_chan *);
+void gk104_fifo_runlist_commit(struct gk104_fifo *, u32 engine);
 
 static inline u64
 gk104_fifo_engine_subdev(int engine)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifog84.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifog84.c
index 820132363f68..77c2f2a28bf3 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifog84.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifog84.c
@@ -27,6 +27,7 @@
 #include <core/ramht.h>
 
 #include <nvif/class.h>
+#include <nvif/cl826f.h>
 #include <nvif/unpack.h>
 
 static int
@@ -35,15 +36,15 @@ g84_fifo_gpfifo_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 {
 	struct nvkm_object *parent = oclass->parent;
 	union {
-		struct nv50_channel_gpfifo_v0 v0;
+		struct g82_channel_gpfifo_v0 v0;
 	} *args = data;
 	struct nv50_fifo *fifo = nv50_fifo(base);
 	struct nv50_fifo_chan *chan;
 	u64 ioffset, ilength;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel gpfifo size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel gpfifo vers %d vm %llx "
 				   "pushbuf %llx ioffset %016llx "
 				   "ilength %08x\n",
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogf100.c
index e7cbc139c1d4..cbc67f262322 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogf100.c
@@ -29,6 +29,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cl906f.h>
 #include <nvif/unpack.h>
 
 static u32
@@ -138,9 +139,9 @@ gf100_fifo_gpfifo_fini(struct nvkm_fifo_chan *base)
 	u32 coff = chan->base.chid * 8;
 
 	if (!list_empty(&chan->head) && !chan->killed) {
-		list_del_init(&chan->head);
+		gf100_fifo_runlist_remove(fifo, chan);
 		nvkm_mask(device, 0x003004 + coff, 0x00000001, 0x00000000);
-		gf100_fifo_runlist_update(fifo);
+		gf100_fifo_runlist_commit(fifo);
 	}
 
 	gf100_fifo_intr_engine(fifo);
@@ -160,9 +161,9 @@ gf100_fifo_gpfifo_init(struct nvkm_fifo_chan *base)
 	nvkm_wr32(device, 0x003000 + coff, 0xc0000000 | addr);
 
 	if (list_empty(&chan->head) && !chan->killed) {
-		list_add_tail(&chan->head, &fifo->chan);
+		gf100_fifo_runlist_insert(fifo, chan);
 		nvkm_wr32(device, 0x003004 + coff, 0x001f0001);
-		gf100_fifo_runlist_update(fifo);
+		gf100_fifo_runlist_commit(fifo);
 	}
 }
 
@@ -199,10 +200,10 @@ gf100_fifo_gpfifo_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nvkm_object *parent = oclass->parent;
 	struct gf100_fifo_chan *chan;
 	u64 usermem, ioffset, ilength;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(parent, "create channel gpfifo size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel gpfifo vers %d vm %llx "
 				   "ioffset %016llx ilength %08x\n",
 			   args->v0.version, args->v0.vm, args->v0.ioffset,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogk104.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogk104.c
index 0b817540a9e4..2e1df01bd928 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifogk104.c
@@ -30,6 +30,7 @@
 #include <subdev/timer.h>
 
 #include <nvif/class.h>
+#include <nvif/cla06f.h>
 #include <nvif/unpack.h>
 
 static int
@@ -151,9 +152,9 @@ gk104_fifo_gpfifo_fini(struct nvkm_fifo_chan *base)
 	u32 coff = chan->base.chid * 8;
 
 	if (!list_empty(&chan->head)) {
-		list_del_init(&chan->head);
+		gk104_fifo_runlist_remove(fifo, chan);
 		nvkm_mask(device, 0x800004 + coff, 0x00000800, 0x00000800);
-		gk104_fifo_runlist_update(fifo, chan->engine);
+		gk104_fifo_runlist_commit(fifo, chan->engine);
 	}
 
 	nvkm_wr32(device, 0x800000 + coff, 0x00000000);
@@ -172,9 +173,9 @@ gk104_fifo_gpfifo_init(struct nvkm_fifo_chan *base)
 	nvkm_wr32(device, 0x800000 + coff, 0x80000000 | addr);
 
 	if (list_empty(&chan->head) && !chan->killed) {
-		list_add_tail(&chan->head, &fifo->engine[chan->engine].chan);
+		gk104_fifo_runlist_insert(fifo, chan);
 		nvkm_mask(device, 0x800004 + coff, 0x00000400, 0x00000400);
-		gk104_fifo_runlist_update(fifo, chan->engine);
+		gk104_fifo_runlist_commit(fifo, chan->engine);
 		nvkm_mask(device, 0x800004 + coff, 0x00000400, 0x00000400);
 	}
 }
@@ -213,10 +214,10 @@ gk104_fifo_gpfifo_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct gk104_fifo_chan *chan;
 	u64 usermem, ioffset, ilength;
 	u32 engines;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(parent, "create channel gpfifo size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel gpfifo vers %d vm %llx "
 				   "ioffset %016llx ilength %08x engine %08x\n",
 			   args->v0.version, args->v0.vm, args->v0.ioffset,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifonv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifonv50.c
index a8c69f878221..c5a7de9db259 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifonv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/fifo/gpfifonv50.c
@@ -27,6 +27,7 @@
 #include <core/ramht.h>
 
 #include <nvif/class.h>
+#include <nvif/cl506f.h>
 #include <nvif/unpack.h>
 
 static int
@@ -40,10 +41,10 @@ nv50_fifo_gpfifo_new(struct nvkm_fifo *base, const struct nvkm_oclass *oclass,
 	struct nv50_fifo *fifo = nv50_fifo(base);
 	struct nv50_fifo_chan *chan;
 	u64 ioffset, ilength;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create channel gpfifo size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create channel gpfifo vers %d vm %llx "
 				   "pushbuf %llx ioffset %016llx "
 				   "ilength %08x\n",
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk20a.c b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk20a.c
index ddaa16a71c84..ad0a6cfe7580 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk20a.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/ctxgk20a.c
@@ -55,7 +55,7 @@ gk20a_grctx_generate_main(struct gf100_gr *gr, struct gf100_grctx *info)
 
 	gk104_grctx_generate_rop_active_fbps(gr);
 
-	nvkm_mask(device, 0x5044b0, 0x8000000, 0x8000000);
+	nvkm_mask(device, 0x5044b0, 0x08000000, 0x08000000);
 
 	gf100_gr_wait_idle(gr);
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpc.fuc b/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpc.fuc
index 7dacb3cc0668..e168b83a10c9 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpc.fuc
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpc.fuc
@@ -247,10 +247,7 @@ init:
 	tpc_strand_info(-1);
 
 	ld b32 $r4 D[$r0 + #tpc_count]
-	mov $r5 NV_PGRAPH_GPC0_TPC0
-	ld b32 $r6 D[$r0 + #gpc_id]
-	shl b32 $r6 15
-	add b32 $r5 $r6
+	gpc_addr($r5, NV_PGRAPH_GPC0_TPC0)
 	tpc_strand_init_tpc_loop:
 		add b32 $r14 $r5 NV_TPC_STRAND_CNT
 		call(nv_rd32)
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpcgm107.fuc5.h b/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpcgm107.fuc5.h
index 11bf363a6ae9..5136f9161706 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpcgm107.fuc5.h
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/fuc/gpcgm107.fuc5.h
@@ -289,7 +289,7 @@ uint32_t gm107_grgpc_code[] = {
 	0x020014fe,
 	0x12004002,
 	0xbd0002f6,
-	0x05b34104,
+	0x05ad4104,
 	0x400010fe,
 	0x00f60700,
 	0x0204bd00,
@@ -387,180 +387,180 @@ uint32_t gm107_grgpc_code[] = {
 	0x7e00008f,
 	0x98000314,
 	0x00850504,
-	0x06985040,
-	0x0f64b604,
-/* 0x04e3: tpc_strand_init_tpc_loop */
-	0xb80056bb,
-	0x0005705e,
-	0x0000657e,
-	0x74bdf6b2,
-/* 0x04f0: tpc_strand_init_idx_loop */
-	0x05605eb8,
-	0x7e7fb200,
-	0xb800008f,
-	0x0005885e,
-	0x7e082f95,
-	0xb800008f,
-	0x00058c5e,
-	0x7e082f95,
-	0xb800008f,
-	0x0005905e,
-	0x0000657e,
-	0xb606f5b6,
-	0xf4b601f0,
-	0x002fbb08,
-	0xb6003fbb,
-	0x62b60170,
-	0xbf1bf401,
-	0x080050b7,
-	0xf40142b6,
-	0x3f0fa81b,
-	0x501d608e,
-	0xb201e5f0,
-	0x008f7eff,
-	0x8e0d0f00,
-	0xf0501da8,
-	0xffb201e5,
-	0x00008f7e,
-	0x0003147e,
-	0x02010080,
-	0xbd0003f6,
-	0xf024bd04,
-	0x00801f29,
-	0x02f60230,
-/* 0x0577: main */
-	0xf404bd00,
-	0x28f40031,
-	0x7e240d00,
-	0xf4000037,
-	0xe4b0f401,
-	0x1d18f404,
-	0x020181fe,
-	0xfd20bd06,
-	0xe4b60412,
-	0x051efd01,
-	0x7e0018fe,
-	0xf400064a,
-/* 0x05a6: main_not_ctx_xfer */
-	0xef94d40e,
-	0x01f5f010,
-	0x0002f87e,
-/* 0x05b3: ih */
-	0xf9c70ef4,
-	0x0188fe80,
-	0x90f980f9,
-	0xb0f9a0f9,
-	0xe0f9d0f9,
-	0x04bdf0f9,
-	0xcf02004a,
-	0xabc400aa,
-	0x1f0bf404,
-	0x004e240d,
-	0x00eecf1a,
-	0xcf19004f,
-	0x047e00ff,
-	0x010e0000,
-	0xf61d0040,
-	0x04bd000e,
-/* 0x05f0: ih_no_fifo */
-	0xf6010040,
-	0x04bd000a,
-	0xe0fcf0fc,
-	0xb0fcd0fc,
-	0x90fca0fc,
-	0x88fe80fc,
-	0xf480fc00,
-	0x01f80032,
-/* 0x0610: hub_barrier_done */
-	0x0e98010f,
-	0x04febb04,
-	0x188effb2,
-	0x8f7e4094,
-	0x00f80000,
-/* 0x0624: ctx_redswitch */
-	0x0080200f,
-	0x0ff60185,
-	0x0e04bd00,
-/* 0x0631: ctx_redswitch_delay */
-	0x01e2b608,
-	0xf1fd1bf4,
-	0xf10800f5,
-	0x800200f5,
-	0xf6018500,
-	0x04bd000f,
-/* 0x064a: ctx_xfer */
-	0x008000f8,
-	0x0ff60281,
-	0x8e04bd00,
-	0xf0501dc4,
-	0xffb201e5,
-	0x00008f7e,
-	0x7e0711f4,
-/* 0x0667: ctx_xfer_not_load */
-	0x7e000624,
-	0xbd000216,
-	0x47fc8024,
-	0x0002f602,
-	0x2cf004bd,
-	0x0320b601,
-	0x024afc80,
-	0xbd0002f6,
-	0x8e0c0f04,
-	0xf0501da8,
-	0xffb201e5,
-	0x00008f7e,
-	0x0003147e,
+	0x55f05040,
+/* 0x04dd: tpc_strand_init_tpc_loop */
+	0x705eb801,
+	0x657e0005,
+	0xf6b20000,
+/* 0x04ea: tpc_strand_init_idx_loop */
+	0x5eb874bd,
+	0xb2000560,
+	0x008f7e7f,
+	0x885eb800,
+	0x2f950005,
+	0x008f7e08,
+	0x8c5eb800,
+	0x2f950005,
+	0x008f7e08,
+	0x905eb800,
+	0x657e0005,
+	0xf5b60000,
+	0x01f0b606,
+	0xbb08f4b6,
+	0x3fbb002f,
+	0x0170b600,
+	0xf40162b6,
+	0x50b7bf1b,
+	0x42b60800,
+	0xa81bf401,
 	0x608e3f0f,
 	0xe5f0501d,
 	0x7effb201,
 	0x0f00008f,
-	0x1d9c8e00,
+	0x1da88e0d,
 	0x01e5f050,
 	0x8f7effb2,
-	0x010f0000,
-	0x0003147e,
-	0xb601fcf0,
-	0xa88e03f0,
-	0xe5f0501d,
-	0x7effb201,
-	0xf000008f,
-	0xa5f001ac,
-	0x00008b02,
-	0x040c9850,
-	0xbb0fc4b6,
-	0x0c9800bc,
-	0x010d9800,
-	0x3d7e000e,
-	0xacf00001,
-	0x40008b01,
-	0x040c9850,
-	0xbb0fc4b6,
-	0x0c9800bc,
-	0x020d9801,
-	0x4e060f98,
-	0x3d7e0800,
-	0xacf00001,
-	0x04a5f001,
-	0x5030008b,
-	0xb6040c98,
-	0xbcbb0fc4,
-	0x020c9800,
-	0x98030d98,
-	0x004e080f,
-	0x013d7e02,
-	0x020a7e00,
-	0x03147e00,
-	0x0601f400,
-/* 0x073f: ctx_xfer_post */
-	0x7e1a12f4,
-	0x0f000227,
-	0x1da88e0d,
+	0x147e0000,
+	0x00800003,
+	0x03f60201,
+	0xbd04bd00,
+	0x1f29f024,
+	0x02300080,
+	0xbd0002f6,
+/* 0x0571: main */
+	0x0031f404,
+	0x0d0028f4,
+	0x00377e24,
+	0xf401f400,
+	0xf404e4b0,
+	0x81fe1d18,
+	0xbd060201,
+	0x0412fd20,
+	0xfd01e4b6,
+	0x18fe051e,
+	0x06447e00,
+	0xd40ef400,
+/* 0x05a0: main_not_ctx_xfer */
+	0xf010ef94,
+	0xf87e01f5,
+	0x0ef40002,
+/* 0x05ad: ih */
+	0xfe80f9c7,
+	0x80f90188,
+	0xa0f990f9,
+	0xd0f9b0f9,
+	0xf0f9e0f9,
+	0x004a04bd,
+	0x00aacf02,
+	0xf404abc4,
+	0x240d1f0b,
+	0xcf1a004e,
+	0x004f00ee,
+	0x00ffcf19,
+	0x0000047e,
+	0x0040010e,
+	0x000ef61d,
+/* 0x05ea: ih_no_fifo */
+	0x004004bd,
+	0x000af601,
+	0xf0fc04bd,
+	0xd0fce0fc,
+	0xa0fcb0fc,
+	0x80fc90fc,
+	0xfc0088fe,
+	0x0032f480,
+/* 0x060a: hub_barrier_done */
+	0x010f01f8,
+	0xbb040e98,
+	0xffb204fe,
+	0x4094188e,
+	0x00008f7e,
+/* 0x061e: ctx_redswitch */
+	0x200f00f8,
+	0x01850080,
+	0xbd000ff6,
+/* 0x062b: ctx_redswitch_delay */
+	0xb6080e04,
+	0x1bf401e2,
+	0x00f5f1fd,
+	0x00f5f108,
+	0x85008002,
+	0x000ff601,
+	0x00f804bd,
+/* 0x0644: ctx_xfer */
+	0x02810080,
+	0xbd000ff6,
+	0x1dc48e04,
+	0x01e5f050,
+	0x8f7effb2,
+	0x11f40000,
+	0x061e7e07,
+/* 0x0661: ctx_xfer_not_load */
+	0x02167e00,
+	0x8024bd00,
+	0xf60247fc,
+	0x04bd0002,
+	0xb6012cf0,
+	0xfc800320,
+	0x02f6024a,
+	0x0f04bd00,
+	0x1da88e0c,
 	0x01e5f050,
 	0x8f7effb2,
 	0x147e0000,
-/* 0x0756: ctx_xfer_done */
-	0x107e0003,
-	0x00f80006,
+	0x3f0f0003,
+	0x501d608e,
+	0xb201e5f0,
+	0x008f7eff,
+	0x8e000f00,
+	0xf0501d9c,
+	0xffb201e5,
+	0x00008f7e,
+	0x147e010f,
+	0xfcf00003,
+	0x03f0b601,
+	0x501da88e,
+	0xb201e5f0,
+	0x008f7eff,
+	0x01acf000,
+	0x8b02a5f0,
+	0x98500000,
+	0xc4b6040c,
+	0x00bcbb0f,
+	0x98000c98,
+	0x000e010d,
+	0x00013d7e,
+	0x8b01acf0,
+	0x98504000,
+	0xc4b6040c,
+	0x00bcbb0f,
+	0x98010c98,
+	0x0f98020d,
+	0x08004e06,
+	0x00013d7e,
+	0xf001acf0,
+	0x008b04a5,
+	0x0c985030,
+	0x0fc4b604,
+	0x9800bcbb,
+	0x0d98020c,
+	0x080f9803,
+	0x7e02004e,
+	0x7e00013d,
+	0x7e00020a,
+	0xf4000314,
+	0x12f40601,
+/* 0x0739: ctx_xfer_post */
+	0x02277e1a,
+	0x8e0d0f00,
+	0xf0501da8,
+	0xffb201e5,
+	0x00008f7e,
+	0x0003147e,
+/* 0x0750: ctx_xfer_done */
+	0x00060a7e,
+	0x000000f8,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/gr/gf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/gr/gf100.c
index 9f5dfc85147a..1f81069edc58 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/gr/gf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/gr/gf100.c
@@ -34,6 +34,7 @@
 #include <engine/fifo.h>
 
 #include <nvif/class.h>
+#include <nvif/cl9097.h>
 #include <nvif/unpack.h>
 
 /*******************************************************************************
@@ -139,6 +140,12 @@ gf100_gr_zbc_depth_get(struct gf100_gr *gr, int format,
 /*******************************************************************************
  * Graphics object classes
  ******************************************************************************/
+#define gf100_gr_object(p) container_of((p), struct gf100_gr_object, object)
+
+struct gf100_gr_object {
+	struct nvkm_object object;
+	struct gf100_gr_chan *chan;
+};
 
 static int
 gf100_fermi_mthd_zbc_color(struct nvkm_object *object, void *data, u32 size)
@@ -147,9 +154,9 @@ gf100_fermi_mthd_zbc_color(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct fermi_a_zbc_color_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		switch (args->v0.format) {
 		case FERMI_A_ZBC_COLOR_V0_FMT_ZERO:
 		case FERMI_A_ZBC_COLOR_V0_FMT_UNORM_ONE:
@@ -193,9 +200,9 @@ gf100_fermi_mthd_zbc_depth(struct nvkm_object *object, void *data, u32 size)
 	union {
 		struct fermi_a_zbc_depth_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		switch (args->v0.format) {
 		case FERMI_A_ZBC_DEPTH_V0_FMT_FP32:
 			ret = gf100_gr_zbc_depth_get(gr, args->v0.format,
@@ -213,6 +220,7 @@ gf100_fermi_mthd_zbc_depth(struct nvkm_object *object, void *data, u32 size)
 static int
 gf100_fermi_mthd(struct nvkm_object *object, u32 mthd, void *data, u32 size)
 {
+	nvif_ioctl(object, "fermi mthd %08x\n", mthd);
 	switch (mthd) {
 	case FERMI_A_ZBC_COLOR:
 		return gf100_fermi_mthd_zbc_color(object, data, size);
@@ -256,6 +264,27 @@ gf100_gr_mthd_sw(struct nvkm_device *device, u16 class, u32 mthd, u32 data)
 	return false;
 }
 
+static const struct nvkm_object_func
+gf100_gr_object_func = {
+};
+
+static int
+gf100_gr_object_new(const struct nvkm_oclass *oclass, void *data, u32 size,
+		    struct nvkm_object **pobject)
+{
+	struct gf100_gr_chan *chan = gf100_gr_chan(oclass->parent);
+	struct gf100_gr_object *object;
+
+	if (!(object = kzalloc(sizeof(*object), GFP_KERNEL)))
+		return -ENOMEM;
+	*pobject = &object->object;
+
+	nvkm_object_ctor(oclass->base.func ? oclass->base.func :
+			 &gf100_gr_object_func, oclass, &object->object);
+	object->chan = chan;
+	return 0;
+}
+
 static int
 gf100_gr_object_get(struct nvkm_gr *base, int index, struct nvkm_sclass *sclass)
 {
@@ -265,6 +294,7 @@ gf100_gr_object_get(struct nvkm_gr *base, int index, struct nvkm_sclass *sclass)
 	while (gr->func->sclass[c].oclass) {
 		if (c++ == index) {
 			*sclass = gr->func->sclass[index];
+			sclass->ctor = gf100_gr_object_new;
 			return index;
 		}
 	}
@@ -826,7 +856,41 @@ gf100_gr_units(struct nvkm_gr *base)
 	return cfg;
 }
 
+static const struct nvkm_bitfield gf100_dispatch_error[] = {
+	{ 0x00000001, "INJECTED_BUNDLE_ERROR" },
+	{ 0x00000002, "CLASS_SUBCH_MISMATCH" },
+	{ 0x00000004, "SUBCHSW_DURING_NOTIFY" },
+	{}
+};
+
+static const struct nvkm_bitfield gf100_m2mf_error[] = {
+	{ 0x00000001, "PUSH_TOO_MUCH_DATA" },
+	{ 0x00000002, "PUSH_NOT_ENOUGH_DATA" },
+	{}
+};
+
+static const struct nvkm_bitfield gf100_unk6_error[] = {
+	{ 0x00000001, "TEMP_TOO_SMALL" },
+	{}
+};
+
+static const struct nvkm_bitfield gf100_ccache_error[] = {
+	{ 0x00000001, "INTR" },
+	{ 0x00000002, "LDCONST_OOB" },
+	{}
+};
+
+static const struct nvkm_bitfield gf100_macro_error[] = {
+	{ 0x00000001, "TOO_FEW_PARAMS" },
+	{ 0x00000002, "TOO_MANY_PARAMS" },
+	{ 0x00000004, "ILLEGAL_OPCODE" },
+	{ 0x00000008, "DOUBLE_BRANCH" },
+	{ 0x00000010, "WATCHDOG" },
+	{}
+};
+
 static const struct nvkm_bitfield gk104_sked_error[] = {
+	{ 0x00000040, "CTA_RESUME" },
 	{ 0x00000080, "CONSTANT_BUFFER_SIZE" },
 	{ 0x00000200, "LOCAL_MEMORY_SIZE_POS" },
 	{ 0x00000400, "LOCAL_MEMORY_SIZE_NEG" },
@@ -836,6 +900,8 @@ static const struct nvkm_bitfield gk104_sked_error[] = {
 	{ 0x00040000, "TOTAL_THREADS" },
 	{ 0x00100000, "PROGRAM_OFFSET" },
 	{ 0x00200000, "SHARED_MEMORY_SIZE" },
+	{ 0x00800000, "CTA_THREAD_DIMENSION_ZERO" },
+	{ 0x01000000, "MEMORY_WINDOW_OVERLAP" },
 	{ 0x02000000, "SHARED_CONFIG_TOO_SMALL" },
 	{ 0x04000000, "TOTAL_REGISTER_COUNT" },
 	{}
@@ -1005,12 +1071,16 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 {
 	struct nvkm_subdev *subdev = &gr->base.engine.subdev;
 	struct nvkm_device *device = subdev->device;
+	char error[128];
 	u32 trap = nvkm_rd32(device, 0x400108);
 	int rop, gpc;
 
 	if (trap & 0x00000001) {
 		u32 stat = nvkm_rd32(device, 0x404000);
-		nvkm_error(subdev, "DISPATCH %08x\n", stat);
+
+		nvkm_snprintbf(error, sizeof(error), gf100_dispatch_error,
+			       stat & 0x3fffffff);
+		nvkm_error(subdev, "DISPATCH %08x [%s]\n", stat, error);
 		nvkm_wr32(device, 0x404000, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000001);
 		trap &= ~0x00000001;
@@ -1018,7 +1088,11 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000002) {
 		u32 stat = nvkm_rd32(device, 0x404600);
-		nvkm_error(subdev, "M2MF %08x\n", stat);
+
+		nvkm_snprintbf(error, sizeof(error), gf100_m2mf_error,
+			       stat & 0x3fffffff);
+		nvkm_error(subdev, "M2MF %08x [%s]\n", stat, error);
+
 		nvkm_wr32(device, 0x404600, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000002);
 		trap &= ~0x00000002;
@@ -1026,7 +1100,10 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000008) {
 		u32 stat = nvkm_rd32(device, 0x408030);
-		nvkm_error(subdev, "CCACHE %08x\n", stat);
+
+		nvkm_snprintbf(error, sizeof(error), gf100_m2mf_error,
+			       stat & 0x3fffffff);
+		nvkm_error(subdev, "CCACHE %08x [%s]\n", stat, error);
 		nvkm_wr32(device, 0x408030, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000008);
 		trap &= ~0x00000008;
@@ -1034,7 +1111,8 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000010) {
 		u32 stat = nvkm_rd32(device, 0x405840);
-		nvkm_error(subdev, "SHADER %08x\n", stat);
+		nvkm_error(subdev, "SHADER %08x, sph: 0x%06x, stage: 0x%02x\n",
+			   stat, stat & 0xffffff, (stat >> 24) & 0x3f);
 		nvkm_wr32(device, 0x405840, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000010);
 		trap &= ~0x00000010;
@@ -1042,7 +1120,11 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000040) {
 		u32 stat = nvkm_rd32(device, 0x40601c);
-		nvkm_error(subdev, "UNK6 %08x\n", stat);
+
+		nvkm_snprintbf(error, sizeof(error), gf100_unk6_error,
+			       stat & 0x3fffffff);
+		nvkm_error(subdev, "UNK6 %08x [%s]\n", stat, error);
+
 		nvkm_wr32(device, 0x40601c, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000040);
 		trap &= ~0x00000040;
@@ -1050,7 +1132,16 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000080) {
 		u32 stat = nvkm_rd32(device, 0x404490);
-		nvkm_error(subdev, "MACRO %08x\n", stat);
+		u32 pc = nvkm_rd32(device, 0x404494);
+		u32 op = nvkm_rd32(device, 0x40449c);
+
+		nvkm_snprintbf(error, sizeof(error), gf100_macro_error,
+			       stat & 0x1fffffff);
+		nvkm_error(subdev, "MACRO %08x [%s], pc: 0x%03x%s, op: 0x%08x\n",
+			   stat, error, pc & 0x7ff,
+			   (pc & 0x10000000) ? "" : " (invalid)",
+			   op);
+
 		nvkm_wr32(device, 0x404490, 0xc0000000);
 		nvkm_wr32(device, 0x400108, 0x00000080);
 		trap &= ~0x00000080;
@@ -1058,10 +1149,9 @@ gf100_gr_trap_intr(struct gf100_gr *gr)
 
 	if (trap & 0x00000100) {
 		u32 stat = nvkm_rd32(device, 0x407020) & 0x3fffffff;
-		char sked[128];
 
-		nvkm_snprintbf(sked, sizeof(sked), gk104_sked_error, stat);
-		nvkm_error(subdev, "SKED: %08x [%s]\n", stat, sked);
+		nvkm_snprintbf(error, sizeof(error), gk104_sked_error, stat);
+		nvkm_error(subdev, "SKED: %08x [%s]\n", stat, error);
 
 		if (stat)
 			nvkm_wr32(device, 0x407020, 0x40000000);
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/pm/base.c b/drivers/gpu/drm/nouveau/nvkm/engine/pm/base.c
index 2721592d3031..f19fabef8d73 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/pm/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/pm/base.c
@@ -27,6 +27,8 @@
 #include <core/option.h>
 
 #include <nvif/class.h>
+#include <nvif/if0002.h>
+#include <nvif/if0003.h>
 #include <nvif/ioctl.h>
 #include <nvif/unpack.h>
 
@@ -210,10 +212,10 @@ nvkm_perfdom_init(struct nvkm_perfdom *dom, void *data, u32 size)
 	} *args = data;
 	struct nvkm_object *object = &dom->object;
 	struct nvkm_pm *pm = dom->perfmon->pm;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(object, "perfdom init size %d\n", size);
-	if (nvif_unvers(args->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, args->none))) {
 		nvif_ioctl(object, "perfdom init\n");
 	} else
 		return ret;
@@ -240,10 +242,10 @@ nvkm_perfdom_sample(struct nvkm_perfdom *dom, void *data, u32 size)
 	} *args = data;
 	struct nvkm_object *object = &dom->object;
 	struct nvkm_pm *pm = dom->perfmon->pm;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(object, "perfdom sample size %d\n", size);
-	if (nvif_unvers(args->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, args->none))) {
 		nvif_ioctl(object, "perfdom sample\n");
 	} else
 		return ret;
@@ -264,10 +266,10 @@ nvkm_perfdom_read(struct nvkm_perfdom *dom, void *data, u32 size)
 	} *args = data;
 	struct nvkm_object *object = &dom->object;
 	struct nvkm_pm *pm = dom->perfmon->pm;
-	int ret, i;
+	int ret = -ENOSYS, i;
 
 	nvif_ioctl(object, "perfdom read size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "perfdom read vers %d\n", args->v0.version);
 	} else
 		return ret;
@@ -374,10 +376,10 @@ nvkm_perfdom_new_(struct nvkm_perfmon *perfmon,
 	struct nvkm_perfctr *ctr[4] = {};
 	struct nvkm_perfdom *dom;
 	int c, s, m;
-	int ret;
+	int ret = -ENOSYS;
 
 	nvif_ioctl(parent, "create perfdom size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(parent, "create perfdom vers %d dom %d mode %02x\n",
 			   args->v0.version, args->v0.domain, args->v0.mode);
 	} else
@@ -439,10 +441,10 @@ nvkm_perfmon_mthd_query_domain(struct nvkm_perfmon *perfmon,
 	struct nvkm_pm *pm = perfmon->pm;
 	struct nvkm_perfdom *dom;
 	u8 domain_nr;
-	int di, ret;
+	int di, ret = -ENOSYS;
 
 	nvif_ioctl(object, "perfmon query domain size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object, "perfmon domain vers %d iter %02x\n",
 			   args->v0.version, args->v0.iter);
 		di = (args->v0.iter & 0xff) - 1;
@@ -490,10 +492,10 @@ nvkm_perfmon_mthd_query_signal(struct nvkm_perfmon *perfmon,
 	struct nvkm_perfsig *sig;
 	const bool all = nvkm_boolopt(device->cfgopt, "NvPmShowAll", false);
 	const bool raw = nvkm_boolopt(device->cfgopt, "NvPmUnnamed", all);
-	int ret, si;
+	int ret = -ENOSYS, si;
 
 	nvif_ioctl(object, "perfmon query signal size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object,
 			   "perfmon query signal vers %d dom %d iter %04x\n",
 			   args->v0.version, args->v0.domain, args->v0.iter);
@@ -543,10 +545,10 @@ nvkm_perfmon_mthd_query_source(struct nvkm_perfmon *perfmon,
 	struct nvkm_perfsig *sig;
 	struct nvkm_perfsrc *src;
 	u8 source_nr = 0;
-	int si, ret;
+	int si, ret = -ENOSYS;
 
 	nvif_ioctl(object, "perfmon query source size %d\n", size);
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		nvif_ioctl(object,
 			   "perfmon source vers %d dom %d sig %02x iter %02x\n",
 			   args->v0.version, args->v0.domain, args->v0.signal,
@@ -612,7 +614,7 @@ nvkm_perfmon_child_get(struct nvkm_object *object, int index,
 		       struct nvkm_oclass *oclass)
 {
 	if (index == 0) {
-		oclass->base.oclass = NVIF_IOCTL_NEW_V0_PERFDOM;
+		oclass->base.oclass = NVIF_CLASS_PERFDOM;
 		oclass->base.minver = 0;
 		oclass->base.maxver = 0;
 		oclass->ctor = nvkm_perfmon_child_new;
@@ -679,7 +681,7 @@ nvkm_pm_oclass_new(struct nvkm_device *device, const struct nvkm_oclass *oclass,
 
 static const struct nvkm_device_oclass
 nvkm_pm_oclass = {
-	.base.oclass = NVIF_IOCTL_NEW_V0_PERFMON,
+	.base.oclass = NVIF_CLASS_PERFMON,
 	.base.minver = -1,
 	.base.maxver = -1,
 	.ctor = nvkm_pm_oclass_new,
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/chan.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/chan.c
index d082f4f73a80..f28967065639 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/chan.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/chan.c
@@ -53,9 +53,9 @@ nvkm_sw_chan_event_ctor(struct nvkm_object *object, void *data, u32 size,
 	union {
 		struct nvif_notify_uevent_req none;
 	} *req = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unvers(req->none)) {
+	if (!(ret = nvif_unvers(ret, &data, &size, req->none))) {
 		notify->size  = sizeof(struct nvif_notify_uevent_rep);
 		notify->types = 1;
 		notify->index = 0;
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/gf100.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/gf100.c
index b01ef7eca906..ea8f4247b628 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/gf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/gf100.c
@@ -28,8 +28,8 @@
 #include <engine/disp.h>
 #include <engine/fifo.h>
 
+#include <nvif/class.h>
 #include <nvif/event.h>
-#include <nvif/ioctl.h>
 
 /*******************************************************************************
  * software context
@@ -143,7 +143,7 @@ static const struct nvkm_sw_func
 gf100_sw = {
 	.chan_new = gf100_sw_chan_new,
 	.sclass = {
-		{ nvkm_nvsw_new, { -1, -1, NVIF_IOCTL_NEW_V0_SW_GF100 } },
+		{ nvkm_nvsw_new, { -1, -1, NVIF_CLASS_SW_GF100 } },
 		{}
 	}
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv04.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv04.c
index 445217ffa791..b6675fe1b0ce 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv04.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv04.c
@@ -27,6 +27,7 @@
 #include "nvsw.h"
 
 #include <nvif/class.h>
+#include <nvif/if0004.h>
 #include <nvif/ioctl.h>
 #include <nvif/unpack.h>
 
@@ -46,9 +47,9 @@ nv04_nvsw_mthd_get_ref(struct nvkm_nvsw *nvsw, void *data, u32 size)
 	union {
 		struct nv04_nvsw_get_ref_v0 v0;
 	} *args = data;
-	int ret;
+	int ret = -ENOSYS;
 
-	if (nvif_unpack(args->v0, 0, 0, false)) {
+	if (!(ret = nvif_unpack(ret, &data, &size, args->v0, 0, 0, false))) {
 		args->v0.ref = atomic_read(&chan->ref);
 	}
 
@@ -126,7 +127,7 @@ static const struct nvkm_sw_func
 nv04_sw = {
 	.chan_new = nv04_sw_chan_new,
 	.sclass = {
-		{ nv04_nvsw_new, { -1, -1, NVIF_IOCTL_NEW_V0_SW_NV04 } },
+		{ nv04_nvsw_new, { -1, -1, NVIF_CLASS_SW_NV04 } },
 		{}
 	}
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv10.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv10.c
index adf70d92b244..09d22fcd194c 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv10.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv10.c
@@ -25,7 +25,7 @@
 #include "chan.h"
 #include "nvsw.h"
 
-#include <nvif/ioctl.h>
+#include <nvif/class.h>
 
 /*******************************************************************************
  * software context
@@ -56,7 +56,7 @@ static const struct nvkm_sw_func
 nv10_sw = {
 	.chan_new = nv10_sw_chan_new,
 	.sclass = {
-		{ nvkm_nvsw_new, { -1, -1, NVIF_IOCTL_NEW_V0_SW_NV10 } },
+		{ nvkm_nvsw_new, { -1, -1, NVIF_CLASS_SW_NV10 } },
 		{}
 	}
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv50.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv50.c
index a381196af69d..01573d187f2c 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nv50.c
@@ -28,8 +28,8 @@
 #include <engine/fifo/chan.h>
 #include <subdev/bar.h>
 
+#include <nvif/class.h>
 #include <nvif/event.h>
-#include <nvif/ioctl.h>
 
 /*******************************************************************************
  * software context
@@ -136,7 +136,7 @@ static const struct nvkm_sw_func
 nv50_sw = {
 	.chan_new = nv50_sw_chan_new,
 	.sclass = {
-		{ nvkm_nvsw_new, { -1, -1, NVIF_IOCTL_NEW_V0_SW_NV50 } },
+		{ nvkm_nvsw_new, { -1, -1, NVIF_CLASS_SW_NV50 } },
 		{}
 	}
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nvsw.c b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nvsw.c
index 66cf986b9572..33dd03fff3c4 100644
--- a/drivers/gpu/drm/nouveau/nvkm/engine/sw/nvsw.c
+++ b/drivers/gpu/drm/nouveau/nvkm/engine/sw/nvsw.c
@@ -24,7 +24,7 @@
 #include "nvsw.h"
 #include "chan.h"
 
-#include <nvif/class.h>
+#include <nvif/if0004.h>
 
 static int
 nvkm_nvsw_mthd_(struct nvkm_object *object, u32 mthd, void *data, u32 size)
@@ -41,7 +41,7 @@ nvkm_nvsw_ntfy_(struct nvkm_object *object, u32 mthd,
 {
 	struct nvkm_nvsw *nvsw = nvkm_nvsw(object);
 	switch (mthd) {
-	case NVSW_NTFY_UEVENT:
+	case NV04_NVSW_NTFY_UEVENT:
 		*pevent = &nvsw->chan->event;
 		return 0;
 	default:
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/dcb.c b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/dcb.c
index 8304b806f2a6..a8d5d67feeaf 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/dcb.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/dcb.c
@@ -143,16 +143,19 @@ dcb_outp_parse(struct nvkm_bios *bios, u8 idx, u8 *ver, u8 *len,
 			switch (outp->type) {
 			case DCB_OUTPUT_DP:
 				switch (conf & 0x00e00000) {
-				case 0x00000000:
+				case 0x00000000: /* 1.62 */
 					outp->dpconf.link_bw = 0x06;
 					break;
-				case 0x00200000:
+				case 0x00200000: /* 2.7 */
 					outp->dpconf.link_bw = 0x0a;
 					break;
-				case 0x00400000:
-				default:
+				case 0x00400000: /* 5.4 */
 					outp->dpconf.link_bw = 0x14;
 					break;
+				case 0x00600000: /* 8.1 */
+				default:
+					outp->dpconf.link_bw = 0x1e;
+					break;
 				}
 
 				switch ((conf & 0x0f000000) >> 24) {
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/perf.c b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/perf.c
index aa7e33b42b30..636bfb665bb9 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/bios/perf.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/bios/perf.c
@@ -24,6 +24,7 @@
 #include <subdev/bios.h>
 #include <subdev/bios/bit.h>
 #include <subdev/bios/perf.h>
+#include <subdev/pci.h>
 
 u16
 nvbios_perf_table(struct nvkm_bios *bios, u8 *ver, u8 *hdr,
@@ -145,6 +146,21 @@ nvbios_perfEp(struct nvkm_bios *bios, int idx,
 		break;
 	case 0x40:
 		info->voltage  = nvbios_rd08(bios, perf + 0x02);
+		switch (nvbios_rd08(bios, perf + 0xb) & 0x3) {
+		case 0:
+			info->pcie_speed = NVKM_PCIE_SPEED_5_0;
+			break;
+		case 3:
+		case 1:
+			info->pcie_speed = NVKM_PCIE_SPEED_2_5;
+			break;
+		case 2:
+			info->pcie_speed = NVKM_PCIE_SPEED_8_0;
+			break;
+		default:
+			break;
+		}
+		info->pcie_width = 0xff;
 		break;
 	default:
 		return 0x0000;
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/base.c b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/base.c
index dc8682c91cc7..889cce2eb727 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/base.c
@@ -176,6 +176,7 @@ nvkm_pstate_prog(struct nvkm_clk *clk, int pstatei)
 {
 	struct nvkm_subdev *subdev = &clk->subdev;
 	struct nvkm_ram *ram = subdev->device->fb->ram;
+	struct nvkm_pci *pci = subdev->device->pci;
 	struct nvkm_pstate *pstate;
 	int ret, idx = 0;
 
@@ -187,6 +188,8 @@ nvkm_pstate_prog(struct nvkm_clk *clk, int pstatei)
 	nvkm_debug(subdev, "setting performance state %d\n", pstatei);
 	clk->pstate = pstatei;
 
+	nvkm_pcie_set_link(pci, pstate->pcie_speed, pstate->pcie_width);
+
 	if (ram && ram->func->calc) {
 		int khz = pstate->base.domain[nv_clk_src_mem];
 		do {
@@ -330,6 +333,8 @@ nvkm_pstate_new(struct nvkm_clk *clk, int idx)
 
 	pstate->pstate = perfE.pstate;
 	pstate->fanspeed = perfE.fanspeed;
+	pstate->pcie_speed = perfE.pcie_speed;
+	pstate->pcie_width = perfE.pcie_width;
 	cstate->voltage = perfE.voltage;
 	cstate->domain[nv_clk_src_core] = perfE.core;
 	cstate->domain[nv_clk_src_shader] = perfE.shader;
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gf100.c b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gf100.c
index a52b7e7fce41..78c449b417b7 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gf100.c
@@ -188,7 +188,7 @@ gf100_clk_read(struct nvkm_clk *base, enum nv_clk_src src)
 		return read_clk(clk, 0x08);
 	case nv_clk_src_copy:
 		return read_clk(clk, 0x09);
-	case nv_clk_src_daemon:
+	case nv_clk_src_pmu:
 		return read_clk(clk, 0x0c);
 	case nv_clk_src_vdec:
 		return read_clk(clk, 0x0e);
@@ -325,7 +325,7 @@ gf100_clk_calc(struct nvkm_clk *base, struct nvkm_cstate *cstate)
 	    (ret = calc_clk(clk, cstate, 0x07, nv_clk_src_hubk06)) ||
 	    (ret = calc_clk(clk, cstate, 0x08, nv_clk_src_hubk01)) ||
 	    (ret = calc_clk(clk, cstate, 0x09, nv_clk_src_copy)) ||
-	    (ret = calc_clk(clk, cstate, 0x0c, nv_clk_src_daemon)) ||
+	    (ret = calc_clk(clk, cstate, 0x0c, nv_clk_src_pmu)) ||
 	    (ret = calc_clk(clk, cstate, 0x0e, nv_clk_src_vdec)))
 		return ret;
 
@@ -447,7 +447,7 @@ gf100_clk = {
 		{ nv_clk_src_rop    , 0x04 },
 		{ nv_clk_src_mem    , 0x05, 0, "memory", 1000 },
 		{ nv_clk_src_vdec   , 0x06 },
-		{ nv_clk_src_daemon , 0x0a },
+		{ nv_clk_src_pmu    , 0x0a },
 		{ nv_clk_src_hubk07 , 0x0b },
 		{ nv_clk_src_max }
 	}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk104.c b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk104.c
index 396f7e4dad0a..975c401bccab 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk104.c
@@ -209,7 +209,7 @@ gk104_clk_read(struct nvkm_clk *base, enum nv_clk_src src)
 		return read_clk(clk, 0x07);
 	case nv_clk_src_hubk01:
 		return read_clk(clk, 0x08);
-	case nv_clk_src_daemon:
+	case nv_clk_src_pmu:
 		return read_clk(clk, 0x0c);
 	case nv_clk_src_vdec:
 		return read_clk(clk, 0x0e);
@@ -346,7 +346,7 @@ gk104_clk_calc(struct nvkm_clk *base, struct nvkm_cstate *cstate)
 	    (ret = calc_clk(clk, cstate, 0x02, nv_clk_src_hubk07)) ||
 	    (ret = calc_clk(clk, cstate, 0x07, nv_clk_src_hubk06)) ||
 	    (ret = calc_clk(clk, cstate, 0x08, nv_clk_src_hubk01)) ||
-	    (ret = calc_clk(clk, cstate, 0x0c, nv_clk_src_daemon)) ||
+	    (ret = calc_clk(clk, cstate, 0x0c, nv_clk_src_pmu)) ||
 	    (ret = calc_clk(clk, cstate, 0x0e, nv_clk_src_vdec)))
 		return ret;
 
@@ -492,7 +492,7 @@ gk104_clk = {
 		{ nv_clk_src_hubk06 , 0x04, NVKM_CLK_DOM_FLAG_CORE },
 		{ nv_clk_src_hubk01 , 0x05 },
 		{ nv_clk_src_vdec   , 0x06 },
-		{ nv_clk_src_daemon , 0x07 },
+		{ nv_clk_src_pmu    , 0x07 },
 		{ nv_clk_src_max }
 	}
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk20a.c b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk20a.c
index 254094ab7fb8..5da2aa8cc333 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk20a.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gk20a.c
@@ -141,9 +141,8 @@ gk20a_pllg_calc_rate(struct gk20a_clk *clk)
 
 	rate = clk->parent_rate * clk->n;
 	divider = clk->m * pl_to_div[clk->pl];
-	do_div(rate, divider);
 
-	return rate / 2;
+	return rate / divider / 2;
 }
 
 static int
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gt215.c b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gt215.c
index c233e3f653ce..056702ef69aa 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gt215.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/clk/gt215.c
@@ -158,7 +158,7 @@ gt215_clk_read(struct nvkm_clk *base, enum nv_clk_src src)
 		return read_clk(clk, 0x20, false);
 	case nv_clk_src_vdec:
 		return read_clk(clk, 0x21, false);
-	case nv_clk_src_daemon:
+	case nv_clk_src_pmu:
 		return read_clk(clk, 0x25, false);
 	case nv_clk_src_host:
 		hsrc = (nvkm_rd32(device, 0xc040) & 0x30000000) >> 28;
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/fb/nv50.c b/drivers/gpu/drm/nouveau/nvkm/subdev/fb/nv50.c
index f5edfadb5b46..1b5fb02eab2a 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/fb/nv50.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/fb/nv50.c
@@ -132,7 +132,7 @@ static const struct nvkm_enum vm_engine[] = {
 	{ 0x0000000b, "PCOUNTER" },
 	{ 0x0000000c, "SEMAPHORE_BG" },
 	{ 0x0000000d, "PCE0" },
-	{ 0x0000000e, "PDAEMON" },
+	{ 0x0000000e, "PMU" },
 	{}
 };
 
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/fb/ramgk104.c b/drivers/gpu/drm/nouveau/nvkm/subdev/fb/ramgk104.c
index 9df45030ff9f..1fa3ade468ae 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/fb/ramgk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/fb/ramgk104.c
@@ -216,11 +216,11 @@ r1373f4_fini(struct gk104_ramfuc *fuc)
 	ram_wr32(fuc, 0x1373ec, tmp | (v1 << 16));
 	ram_mask(fuc, 0x1373f0, (~ram->mode & 3), 0x00000000);
 	if (ram->mode == 2) {
-		ram_mask(fuc, 0x1373f4, 0x00000003, 0x000000002);
-		ram_mask(fuc, 0x1373f4, 0x00001100, 0x000000000);
+		ram_mask(fuc, 0x1373f4, 0x00000003, 0x00000002);
+		ram_mask(fuc, 0x1373f4, 0x00001100, 0x00000000);
 	} else {
-		ram_mask(fuc, 0x1373f4, 0x00000003, 0x000000001);
-		ram_mask(fuc, 0x1373f4, 0x00010000, 0x000000000);
+		ram_mask(fuc, 0x1373f4, 0x00000003, 0x00000001);
+		ram_mask(fuc, 0x1373f4, 0x00010000, 0x00000000);
 	}
 	ram_mask(fuc, 0x10f800, 0x00000030, (v0 ^ v1) << 4);
 }
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/Kbuild b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/Kbuild
index de888fa62b3e..7e77a7466992 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/Kbuild
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/Kbuild
@@ -2,3 +2,4 @@ nvkm-y += nvkm/subdev/ibus/gf100.o
 nvkm-y += nvkm/subdev/ibus/gf117.o
 nvkm-y += nvkm/subdev/ibus/gk104.o
 nvkm-y += nvkm/subdev/ibus/gk20a.o
+nvkm-y += nvkm/subdev/ibus/gm204.o
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gk104.c b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gk104.c
index ba33609f643c..b5cee3f89aaa 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gk104.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gk104.c
@@ -21,7 +21,7 @@
  *
  * Authors: Ben Skeggs
  */
-#include <subdev/ibus.h>
+#include "priv.h"
 
 static void
 gk104_ibus_intr_hub(struct nvkm_subdev *ibus, int i)
@@ -56,7 +56,7 @@ gk104_ibus_intr_gpc(struct nvkm_subdev *ibus, int i)
 	nvkm_mask(device, 0x128128 + (i * 0x0800), 0x00000200, 0x00000000);
 }
 
-static void
+void
 gk104_ibus_intr(struct nvkm_subdev *ibus)
 {
 	struct nvkm_device *device = ibus->device;
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gm204.c b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gm204.c
new file mode 100644
index 000000000000..b3839dc254ee
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/gm204.c
@@ -0,0 +1,40 @@
+/*
+ * Copyright 2015 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs <bskeggs@redhat.com>
+ */
+#include "priv.h"
+
+static const struct nvkm_subdev_func
+gm204_ibus = {
+	.intr = gk104_ibus_intr,
+};
+
+int
+gm204_ibus_new(struct nvkm_device *device, int index,
+	       struct nvkm_subdev **pibus)
+{
+	struct nvkm_subdev *ibus;
+	if (!(ibus = *pibus = kzalloc(sizeof(*ibus), GFP_KERNEL)))
+		return -ENOMEM;
+	nvkm_subdev_ctor(&gm204_ibus, device, index, 0, ibus);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/priv.h b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/priv.h
index 48e1b6365ce6..01caf798cf31 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/priv.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ibus/priv.h
@@ -4,4 +4,5 @@
 #include <subdev/ibus.h>
 
 void gf100_ibus_intr(struct nvkm_subdev *);
+void gk104_ibus_intr(struct nvkm_subdev *);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/instmem/gk20a.c b/drivers/gpu/drm/nouveau/nvkm/subdev/instmem/gk20a.c
index 14107b5b7811..4c20fec64d96 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/instmem/gk20a.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/instmem/gk20a.c
@@ -56,7 +56,6 @@ struct gk20a_instobj {
 
 	/* CPU mapping */
 	u32 *vaddr;
-	struct list_head vaddr_node;
 };
 #define gk20a_instobj(p) container_of((p), struct gk20a_instobj, memory)
 
@@ -66,7 +65,6 @@ struct gk20a_instobj {
 struct gk20a_instobj_dma {
 	struct gk20a_instobj base;
 
-	u32 *cpuaddr;
 	dma_addr_t handle;
 	struct nvkm_mm_node r;
 };
@@ -79,6 +77,11 @@ struct gk20a_instobj_dma {
 struct gk20a_instobj_iommu {
 	struct gk20a_instobj base;
 
+	/* to link into gk20a_instmem::vaddr_lru */
+	struct list_head vaddr_node;
+	/* how many clients are using vaddr? */
+	u32 use_cpt;
+
 	/* will point to the higher half of pages */
 	dma_addr_t *dma_addrs;
 	/* array of base.mem->size pages (+ dma_addr_ts) */
@@ -107,8 +110,6 @@ struct gk20a_instmem {
 
 	/* Only used by DMA API */
 	struct dma_attrs attrs;
-
-	void __iomem * (*cpu_map)(struct nvkm_memory *);
 };
 #define gk20a_instmem(p) container_of((p), struct gk20a_instmem, base)
 
@@ -130,70 +131,58 @@ gk20a_instobj_size(struct nvkm_memory *memory)
 	return (u64)gk20a_instobj(memory)->mem.size << 12;
 }
 
-static void __iomem *
-gk20a_instobj_cpu_map_dma(struct nvkm_memory *memory)
-{
-#if defined(CONFIG_ARM) || defined(CONFIG_ARM64)
-	struct gk20a_instobj_dma *node = gk20a_instobj_dma(memory);
-	struct device *dev = node->base.imem->base.subdev.device->dev;
-	int npages = nvkm_memory_size(memory) >> 12;
-	struct page *pages[npages];
-	int i;
-
-	/* we shouldn't see a gk20a on anything but arm/arm64 anyways */
-	/* phys_to_page does not exist on all platforms... */
-	pages[0] = pfn_to_page(dma_to_phys(dev, node->handle) >> PAGE_SHIFT);
-	for (i = 1; i < npages; i++)
-		pages[i] = pages[0] + i;
-
-	return vmap(pages, npages, VM_MAP, pgprot_writecombine(PAGE_KERNEL));
-#else
-	BUG();
-	return NULL;
-#endif
-}
-
-static void __iomem *
-gk20a_instobj_cpu_map_iommu(struct nvkm_memory *memory)
+/*
+ * Recycle the vaddr of obj. Must be called with gk20a_instmem::lock held.
+ */
+static void
+gk20a_instobj_iommu_recycle_vaddr(struct gk20a_instobj_iommu *obj)
 {
-	struct gk20a_instobj_iommu *node = gk20a_instobj_iommu(memory);
-	int npages = nvkm_memory_size(memory) >> 12;
-
-	return vmap(node->pages, npages, VM_MAP,
-		    pgprot_writecombine(PAGE_KERNEL));
+	struct gk20a_instmem *imem = obj->base.imem;
+	/* there should not be any user left... */
+	WARN_ON(obj->use_cpt);
+	list_del(&obj->vaddr_node);
+	vunmap(obj->base.vaddr);
+	obj->base.vaddr = NULL;
+	imem->vaddr_use -= nvkm_memory_size(&obj->base.memory);
+	nvkm_debug(&imem->base.subdev, "vaddr used: %x/%x\n", imem->vaddr_use,
+		   imem->vaddr_max);
 }
 
 /*
- * Must be called while holding gk20a_instmem_lock
+ * Must be called while holding gk20a_instmem::lock
  */
 static void
 gk20a_instmem_vaddr_gc(struct gk20a_instmem *imem, const u64 size)
 {
 	while (imem->vaddr_use + size > imem->vaddr_max) {
-		struct gk20a_instobj *obj;
-
 		/* no candidate that can be unmapped, abort... */
 		if (list_empty(&imem->vaddr_lru))
 			break;
 
-		obj = list_first_entry(&imem->vaddr_lru, struct gk20a_instobj,
-				       vaddr_node);
-		list_del(&obj->vaddr_node);
-		vunmap(obj->vaddr);
-		obj->vaddr = NULL;
-		imem->vaddr_use -= nvkm_memory_size(&obj->memory);
-		nvkm_debug(&imem->base.subdev, "(GC) vaddr used: %x/%x\n",
-			   imem->vaddr_use, imem->vaddr_max);
-
+		gk20a_instobj_iommu_recycle_vaddr(
+				list_first_entry(&imem->vaddr_lru,
+				struct gk20a_instobj_iommu, vaddr_node));
 	}
 }
 
 static void __iomem *
-gk20a_instobj_acquire(struct nvkm_memory *memory)
+gk20a_instobj_acquire_dma(struct nvkm_memory *memory)
 {
 	struct gk20a_instobj *node = gk20a_instobj(memory);
 	struct gk20a_instmem *imem = node->imem;
 	struct nvkm_ltc *ltc = imem->base.subdev.device->ltc;
+
+	nvkm_ltc_flush(ltc);
+
+	return node->vaddr;
+}
+
+static void __iomem *
+gk20a_instobj_acquire_iommu(struct nvkm_memory *memory)
+{
+	struct gk20a_instobj_iommu *node = gk20a_instobj_iommu(memory);
+	struct gk20a_instmem *imem = node->base.imem;
+	struct nvkm_ltc *ltc = imem->base.subdev.device->ltc;
 	const u64 size = nvkm_memory_size(memory);
 	unsigned long flags;
 
@@ -201,19 +190,21 @@ gk20a_instobj_acquire(struct nvkm_memory *memory)
 
 	spin_lock_irqsave(&imem->lock, flags);
 
-	if (node->vaddr) {
-		/* remove us from the LRU list since we cannot be unmapped */
-		list_del(&node->vaddr_node);
-
+	if (node->base.vaddr) {
+		if (!node->use_cpt) {
+			/* remove from LRU list since mapping in use again */
+			list_del(&node->vaddr_node);
+		}
 		goto out;
 	}
 
 	/* try to free some address space if we reached the limit */
 	gk20a_instmem_vaddr_gc(imem, size);
 
-	node->vaddr = imem->cpu_map(memory);
-
-	if (!node->vaddr) {
+	/* map the pages */
+	node->base.vaddr = vmap(node->pages, size >> PAGE_SHIFT, VM_MAP,
+				pgprot_writecombine(PAGE_KERNEL));
+	if (!node->base.vaddr) {
 		nvkm_error(&imem->base.subdev, "cannot map instobj - "
 			   "this is not going to end well...\n");
 		goto out;
@@ -224,24 +215,41 @@ gk20a_instobj_acquire(struct nvkm_memory *memory)
 		   imem->vaddr_use, imem->vaddr_max);
 
 out:
+	node->use_cpt++;
 	spin_unlock_irqrestore(&imem->lock, flags);
 
-	return node->vaddr;
+	return node->base.vaddr;
 }
 
 static void
-gk20a_instobj_release(struct nvkm_memory *memory)
+gk20a_instobj_release_dma(struct nvkm_memory *memory)
 {
 	struct gk20a_instobj *node = gk20a_instobj(memory);
 	struct gk20a_instmem *imem = node->imem;
 	struct nvkm_ltc *ltc = imem->base.subdev.device->ltc;
+
+	nvkm_ltc_invalidate(ltc);
+}
+
+static void
+gk20a_instobj_release_iommu(struct nvkm_memory *memory)
+{
+	struct gk20a_instobj_iommu *node = gk20a_instobj_iommu(memory);
+	struct gk20a_instmem *imem = node->base.imem;
+	struct nvkm_ltc *ltc = imem->base.subdev.device->ltc;
 	unsigned long flags;
 
 	spin_lock_irqsave(&imem->lock, flags);
 
-	/* add ourselves to the LRU list so our CPU mapping can be freed */
-	list_add_tail(&node->vaddr_node, &imem->vaddr_lru);
+	/* we should at least have one user to release... */
+	if (WARN_ON(node->use_cpt == 0))
+		goto out;
+
+	/* add unused objs to the LRU list to recycle their mapping */
+	if (--node->use_cpt == 0)
+		list_add_tail(&node->vaddr_node, &imem->vaddr_lru);
 
+out:
 	spin_unlock_irqrestore(&imem->lock, flags);
 
 	wmb();
@@ -272,37 +280,6 @@ gk20a_instobj_map(struct nvkm_memory *memory, struct nvkm_vma *vma, u64 offset)
 	nvkm_vm_map_at(vma, offset, &node->mem);
 }
 
-/*
- * Clear the CPU mapping of an instobj if it exists
- */
-static void
-gk20a_instobj_dtor(struct gk20a_instobj *node)
-{
-	struct gk20a_instmem *imem = node->imem;
-	struct gk20a_instobj *obj;
-	unsigned long flags;
-
-	spin_lock_irqsave(&imem->lock, flags);
-
-	if (!node->vaddr)
-		goto out;
-
-	list_for_each_entry(obj, &imem->vaddr_lru, vaddr_node) {
-		if (obj == node) {
-			list_del(&obj->vaddr_node);
-			break;
-		}
-	}
-	vunmap(node->vaddr);
-	node->vaddr = NULL;
-	imem->vaddr_use -= nvkm_memory_size(&node->memory);
-	nvkm_debug(&imem->base.subdev, "vaddr used: %x/%x\n",
-		   imem->vaddr_use, imem->vaddr_max);
-
-out:
-	spin_unlock_irqrestore(&imem->lock, flags);
-}
-
 static void *
 gk20a_instobj_dtor_dma(struct nvkm_memory *memory)
 {
@@ -310,12 +287,10 @@ gk20a_instobj_dtor_dma(struct nvkm_memory *memory)
 	struct gk20a_instmem *imem = node->base.imem;
 	struct device *dev = imem->base.subdev.device->dev;
 
-	gk20a_instobj_dtor(&node->base);
-
-	if (unlikely(!node->cpuaddr))
+	if (unlikely(!node->base.vaddr))
 		goto out;
 
-	dma_free_attrs(dev, node->base.mem.size << PAGE_SHIFT, node->cpuaddr,
+	dma_free_attrs(dev, node->base.mem.size << PAGE_SHIFT, node->base.vaddr,
 		       node->handle, &imem->attrs);
 
 out:
@@ -329,13 +304,20 @@ gk20a_instobj_dtor_iommu(struct nvkm_memory *memory)
 	struct gk20a_instmem *imem = node->base.imem;
 	struct device *dev = imem->base.subdev.device->dev;
 	struct nvkm_mm_node *r;
+	unsigned long flags;
 	int i;
 
-	gk20a_instobj_dtor(&node->base);
-
 	if (unlikely(list_empty(&node->base.mem.regions)))
 		goto out;
 
+	spin_lock_irqsave(&imem->lock, flags);
+
+	/* vaddr has already been recycled */
+	if (node->base.vaddr)
+		gk20a_instobj_iommu_recycle_vaddr(node);
+
+	spin_unlock_irqrestore(&imem->lock, flags);
+
 	r = list_first_entry(&node->base.mem.regions, struct nvkm_mm_node,
 			     rl_entry);
 
@@ -366,8 +348,8 @@ gk20a_instobj_func_dma = {
 	.target = gk20a_instobj_target,
 	.addr = gk20a_instobj_addr,
 	.size = gk20a_instobj_size,
-	.acquire = gk20a_instobj_acquire,
-	.release = gk20a_instobj_release,
+	.acquire = gk20a_instobj_acquire_dma,
+	.release = gk20a_instobj_release_dma,
 	.rd32 = gk20a_instobj_rd32,
 	.wr32 = gk20a_instobj_wr32,
 	.map = gk20a_instobj_map,
@@ -379,8 +361,8 @@ gk20a_instobj_func_iommu = {
 	.target = gk20a_instobj_target,
 	.addr = gk20a_instobj_addr,
 	.size = gk20a_instobj_size,
-	.acquire = gk20a_instobj_acquire,
-	.release = gk20a_instobj_release,
+	.acquire = gk20a_instobj_acquire_iommu,
+	.release = gk20a_instobj_release_iommu,
 	.rd32 = gk20a_instobj_rd32,
 	.wr32 = gk20a_instobj_wr32,
 	.map = gk20a_instobj_map,
@@ -400,10 +382,10 @@ gk20a_instobj_ctor_dma(struct gk20a_instmem *imem, u32 npages, u32 align,
 
 	nvkm_memory_ctor(&gk20a_instobj_func_dma, &node->base.memory);
 
-	node->cpuaddr = dma_alloc_attrs(dev, npages << PAGE_SHIFT,
-					&node->handle, GFP_KERNEL,
-					&imem->attrs);
-	if (!node->cpuaddr) {
+	node->base.vaddr = dma_alloc_attrs(dev, npages << PAGE_SHIFT,
+					   &node->handle, GFP_KERNEL,
+					   &imem->attrs);
+	if (!node->base.vaddr) {
 		nvkm_error(subdev, "cannot allocate DMA memory\n");
 		return -ENOMEM;
 	}
@@ -609,18 +591,14 @@ gk20a_instmem_new(struct nvkm_device *device, int index,
 		imem->mm = &tdev->iommu.mm;
 		imem->domain = tdev->iommu.domain;
 		imem->iommu_pgshift = tdev->iommu.pgshift;
-		imem->cpu_map = gk20a_instobj_cpu_map_iommu;
 		imem->iommu_bit = tdev->func->iommu_bit;
 
 		nvkm_info(&imem->base.subdev, "using IOMMU\n");
 	} else {
 		init_dma_attrs(&imem->attrs);
-		/* We will access the memory through our own mapping */
 		dma_set_attr(DMA_ATTR_NON_CONSISTENT, &imem->attrs);
 		dma_set_attr(DMA_ATTR_WEAK_ORDERING, &imem->attrs);
 		dma_set_attr(DMA_ATTR_WRITE_COMBINE, &imem->attrs);
-		dma_set_attr(DMA_ATTR_NO_KERNEL_MAPPING, &imem->attrs);
-		imem->cpu_map = gk20a_instobj_cpu_map_dma;
 
 		nvkm_info(&imem->base.subdev, "using DMA API\n");
 	}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/Kbuild b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/Kbuild
index e5df3d865f0c..f8108df3cb38 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/Kbuild
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/Kbuild
@@ -2,3 +2,4 @@ nvkm-y += nvkm/subdev/ltc/base.o
 nvkm-y += nvkm/subdev/ltc/gf100.o
 nvkm-y += nvkm/subdev/ltc/gk104.o
 nvkm-y += nvkm/subdev/ltc/gm107.o
+nvkm-y += nvkm/subdev/ltc/gm204.o
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm107.c b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm107.c
index 3043bbfd7384..2af1f9e100fc 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm107.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm107.c
@@ -26,16 +26,16 @@
 #include <subdev/fb.h>
 #include <subdev/timer.h>
 
-static void
+void
 gm107_ltc_cbc_clear(struct nvkm_ltc *ltc, u32 start, u32 limit)
 {
 	struct nvkm_device *device = ltc->subdev.device;
 	nvkm_wr32(device, 0x17e270, start);
 	nvkm_wr32(device, 0x17e274, limit);
-	nvkm_wr32(device, 0x17e26c, 0x00000004);
+	nvkm_mask(device, 0x17e26c, 0x00000000, 0x00000004);
 }
 
-static void
+void
 gm107_ltc_cbc_wait(struct nvkm_ltc *ltc)
 {
 	struct nvkm_device *device = ltc->subdev.device;
@@ -51,7 +51,7 @@ gm107_ltc_cbc_wait(struct nvkm_ltc *ltc)
 	}
 }
 
-static void
+void
 gm107_ltc_zbc_clear_color(struct nvkm_ltc *ltc, int i, const u32 color[4])
 {
 	struct nvkm_device *device = ltc->subdev.device;
@@ -62,7 +62,7 @@ gm107_ltc_zbc_clear_color(struct nvkm_ltc *ltc, int i, const u32 color[4])
 	nvkm_wr32(device, 0x17e348, color[3]);
 }
 
-static void
+void
 gm107_ltc_zbc_clear_depth(struct nvkm_ltc *ltc, int i, const u32 depth)
 {
 	struct nvkm_device *device = ltc->subdev.device;
@@ -84,7 +84,7 @@ gm107_ltc_lts_isr(struct nvkm_ltc *ltc, int c, int s)
 	}
 }
 
-static void
+void
 gm107_ltc_intr(struct nvkm_ltc *ltc)
 {
 	struct nvkm_device *device = ltc->subdev.device;
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm204.c b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm204.c
new file mode 100644
index 000000000000..5ad6fb9d022d
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/gm204.c
@@ -0,0 +1,63 @@
+/*
+ * Copyright 2015 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs <bskeggs@redhat.com>
+ */
+#include "priv.h"
+
+#include <subdev/fb.h>
+#include <subdev/timer.h>
+
+static int
+gm204_ltc_oneinit(struct nvkm_ltc *ltc)
+{
+	struct nvkm_device *device = ltc->subdev.device;
+
+	ltc->ltc_nr = nvkm_rd32(device, 0x12006c);
+	ltc->lts_nr = nvkm_rd32(device, 0x17e280) >> 28;
+
+	return gf100_ltc_oneinit_tag_ram(ltc);
+}
+static void
+gm204_ltc_init(struct nvkm_ltc *ltc)
+{
+	nvkm_wr32(ltc->subdev.device, 0x17e278, ltc->tag_base);
+}
+
+static const struct nvkm_ltc_func
+gm204_ltc = {
+	.oneinit = gm204_ltc_oneinit,
+	.init = gm204_ltc_init,
+	.intr = gm107_ltc_intr, /*XXX: not validated */
+	.cbc_clear = gm107_ltc_cbc_clear,
+	.cbc_wait = gm107_ltc_cbc_wait,
+	.zbc = 16,
+	.zbc_clear_color = gm107_ltc_zbc_clear_color,
+	.zbc_clear_depth = gm107_ltc_zbc_clear_depth,
+	.invalidate = gf100_ltc_invalidate,
+	.flush = gf100_ltc_flush,
+};
+
+int
+gm204_ltc_new(struct nvkm_device *device, int index, struct nvkm_ltc **pltc)
+{
+	return nvkm_ltc_new_(&gm204_ltc, device, index, pltc);
+}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/priv.h b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/priv.h
index 4e3755b82769..6d81c695ed0d 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/priv.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/ltc/priv.h
@@ -31,4 +31,10 @@ void gf100_ltc_zbc_clear_color(struct nvkm_ltc *, int, const u32[4]);
 void gf100_ltc_zbc_clear_depth(struct nvkm_ltc *, int, const u32);
 void gf100_ltc_invalidate(struct nvkm_ltc *);
 void gf100_ltc_flush(struct nvkm_ltc *);
+
+void gm107_ltc_intr(struct nvkm_ltc *);
+void gm107_ltc_cbc_clear(struct nvkm_ltc *, u32, u32);
+void gm107_ltc_cbc_wait(struct nvkm_ltc *);
+void gm107_ltc_zbc_clear_color(struct nvkm_ltc *, int, const u32[4]);
+void gm107_ltc_zbc_clear_depth(struct nvkm_ltc *, int, const u32);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/Kbuild b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/Kbuild
index 4476ef75acd6..3c2519fdeb81 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/Kbuild
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/Kbuild
@@ -1,5 +1,6 @@
 nvkm-y += nvkm/subdev/pci/agp.o
 nvkm-y += nvkm/subdev/pci/base.o
+nvkm-y += nvkm/subdev/pci/pcie.o
 nvkm-y += nvkm/subdev/pci/nv04.o
 nvkm-y += nvkm/subdev/pci/nv40.o
 nvkm-y += nvkm/subdev/pci/nv46.o
@@ -7,3 +8,5 @@ nvkm-y += nvkm/subdev/pci/nv4c.o
 nvkm-y += nvkm/subdev/pci/g84.o
 nvkm-y += nvkm/subdev/pci/g94.o
 nvkm-y += nvkm/subdev/pci/gf100.o
+nvkm-y += nvkm/subdev/pci/gf106.o
+nvkm-y += nvkm/subdev/pci/gk104.o
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/base.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/base.c
index d671dcfaff3c..65057c8310a2 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/base.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/base.c
@@ -107,6 +107,15 @@ nvkm_pci_preinit(struct nvkm_subdev *subdev)
 }
 
 static int
+nvkm_pci_oneinit(struct nvkm_subdev *subdev)
+{
+	struct nvkm_pci *pci = nvkm_pci(subdev);
+	if (pci_is_pcie(pci->pdev))
+		return nvkm_pcie_oneinit(pci);
+	return 0;
+}
+
+static int
 nvkm_pci_init(struct nvkm_subdev *subdev)
 {
 	struct nvkm_pci *pci = nvkm_pci(subdev);
@@ -117,6 +126,8 @@ nvkm_pci_init(struct nvkm_subdev *subdev)
 		ret = nvkm_agp_init(pci);
 		if (ret)
 			return ret;
+	} else if (pci_is_pcie(pci->pdev)) {
+		nvkm_pcie_init(pci);
 	}
 
 	if (pci->func->init)
@@ -143,6 +154,7 @@ nvkm_pci_dtor(struct nvkm_subdev *subdev)
 static const struct nvkm_subdev_func
 nvkm_pci_func = {
 	.dtor = nvkm_pci_dtor,
+	.oneinit = nvkm_pci_oneinit,
 	.preinit = nvkm_pci_preinit,
 	.init = nvkm_pci_init,
 	.fini = nvkm_pci_fini,
@@ -160,6 +172,8 @@ nvkm_pci_new_(const struct nvkm_pci_func *func, struct nvkm_device *device,
 	pci->func = func;
 	pci->pdev = device->func->pci(device)->pdev;
 	pci->irq = -1;
+	pci->pcie.speed = -1;
+	pci->pcie.width = -1;
 
 	if (device->type == NVKM_DEVICE_AGP)
 		nvkm_agp_ctor(pci);
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g84.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g84.c
index 3faa6bfb895b..62438d892f42 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g84.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g84.c
@@ -25,6 +25,80 @@
 
 #include <core/pci.h>
 
+static int
+g84_pcie_version_supported(struct nvkm_pci *pci)
+{
+	/* g84 and g86 report wrong information about what they support */
+	return 1;
+}
+
+int
+g84_pcie_version(struct nvkm_pci *pci)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	return (nvkm_rd32(device, 0x00154c) & 0x1) + 1;
+}
+
+void
+g84_pcie_set_version(struct nvkm_pci *pci, u8 ver)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	nvkm_mask(device, 0x00154c, 0x1, (ver >= 2 ? 0x1 : 0x0));
+}
+
+static void
+g84_pcie_set_cap_speed(struct nvkm_pci *pci, bool full_speed)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	nvkm_mask(device, 0x00154c, 0x80, full_speed ? 0x80 : 0x0);
+}
+
+enum nvkm_pcie_speed
+g84_pcie_cur_speed(struct nvkm_pci *pci)
+{
+	u32 reg_v = nvkm_pci_rd32(pci, 0x88) & 0x30000;
+	switch (reg_v) {
+	case 0x30000:
+		return NVKM_PCIE_SPEED_8_0;
+	case 0x20000:
+		return NVKM_PCIE_SPEED_5_0;
+	case 0x10000:
+	default:
+		return NVKM_PCIE_SPEED_2_5;
+	}
+}
+
+enum nvkm_pcie_speed
+g84_pcie_max_speed(struct nvkm_pci *pci)
+{
+	u32 reg_v = nvkm_pci_rd32(pci, 0x460) & 0x3300;
+	if (reg_v == 0x2200)
+		return NVKM_PCIE_SPEED_5_0;
+	return NVKM_PCIE_SPEED_2_5;
+}
+
+void
+g84_pcie_set_link_speed(struct nvkm_pci *pci, enum nvkm_pcie_speed speed)
+{
+	u32 mask_value;
+
+	if (speed == NVKM_PCIE_SPEED_5_0)
+		mask_value = 0x20;
+	else
+		mask_value = 0x10;
+
+	nvkm_pci_mask(pci, 0x460, 0x30, mask_value);
+	nvkm_pci_mask(pci, 0x460, 0x1, 0x1);
+}
+
+int
+g84_pcie_set_link(struct nvkm_pci *pci, enum nvkm_pcie_speed speed, u8 width)
+{
+	g84_pcie_set_cap_speed(pci, speed == NVKM_PCIE_SPEED_5_0);
+	g84_pcie_set_link_speed(pci, speed);
+	return 0;
+}
+
 void
 g84_pci_init(struct nvkm_pci *pci)
 {
@@ -48,6 +122,14 @@ g84_pci_init(struct nvkm_pci *pci)
 		nvkm_pci_mask(pci, 0x041c, 0x00000060, 0x00000000);
 }
 
+int
+g84_pcie_init(struct nvkm_pci *pci)
+{
+	bool full_speed = g84_pcie_cur_speed(pci) == NVKM_PCIE_SPEED_5_0;
+	g84_pcie_set_cap_speed(pci, full_speed);
+	return 0;
+}
+
 static const struct nvkm_pci_func
 g84_pci_func = {
 	.init = g84_pci_init,
@@ -55,6 +137,16 @@ g84_pci_func = {
 	.wr08 = nv40_pci_wr08,
 	.wr32 = nv40_pci_wr32,
 	.msi_rearm = nv46_pci_msi_rearm,
+
+	.pcie.init = g84_pcie_init,
+	.pcie.set_link = g84_pcie_set_link,
+
+	.pcie.max_speed = g84_pcie_max_speed,
+	.pcie.cur_speed = g84_pcie_cur_speed,
+
+	.pcie.set_version = g84_pcie_set_version,
+	.pcie.version = g84_pcie_version,
+	.pcie.version_supported = g84_pcie_version_supported,
 };
 
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g94.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g94.c
index cd311ee311cc..43444123bc04 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g94.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/g94.c
@@ -23,6 +23,14 @@
  */
 #include "priv.h"
 
+int
+g94_pcie_version_supported(struct nvkm_pci *pci)
+{
+	if ((nvkm_pci_rd32(pci, 0x460) & 0x200) == 0x200)
+		return 2;
+	return 1;
+}
+
 static const struct nvkm_pci_func
 g94_pci_func = {
 	.init = g84_pci_init,
@@ -30,6 +38,16 @@ g94_pci_func = {
 	.wr08 = nv40_pci_wr08,
 	.wr32 = nv40_pci_wr32,
 	.msi_rearm = nv40_pci_msi_rearm,
+
+	.pcie.init = g84_pcie_init,
+	.pcie.set_link = g84_pcie_set_link,
+
+	.pcie.max_speed = g84_pcie_max_speed,
+	.pcie.cur_speed = g84_pcie_cur_speed,
+
+	.pcie.set_version = g84_pcie_set_version,
+	.pcie.version = g84_pcie_version,
+	.pcie.version_supported = g94_pcie_version_supported,
 };
 
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf100.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf100.c
index 25e1ae70867f..e30ea676baf6 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf100.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf100.c
@@ -29,6 +29,53 @@ gf100_pci_msi_rearm(struct nvkm_pci *pci)
 	nvkm_pci_wr08(pci, 0x0704, 0xff);
 }
 
+void
+gf100_pcie_set_version(struct nvkm_pci *pci, u8 ver)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	nvkm_mask(device, 0x02241c, 0x1, ver > 1 ? 1 : 0);
+}
+
+int
+gf100_pcie_version(struct nvkm_pci *pci)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	return (nvkm_rd32(device, 0x02241c) & 0x1) + 1;
+}
+
+void
+gf100_pcie_set_cap_speed(struct nvkm_pci *pci, bool full_speed)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	nvkm_mask(device, 0x02241c, 0x80, full_speed ? 0x80 : 0x0);
+}
+
+int
+gf100_pcie_cap_speed(struct nvkm_pci *pci)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	u8 punits_pci_cap_speed = nvkm_rd32(device, 0x02241c) & 0x80;
+	if (punits_pci_cap_speed == 0x80)
+		return 1;
+	return 0;
+}
+
+int
+gf100_pcie_init(struct nvkm_pci *pci)
+{
+	bool full_speed = g84_pcie_cur_speed(pci) == NVKM_PCIE_SPEED_5_0;
+	gf100_pcie_set_cap_speed(pci, full_speed);
+	return 0;
+}
+
+int
+gf100_pcie_set_link(struct nvkm_pci *pci, enum nvkm_pcie_speed speed, u8 width)
+{
+	gf100_pcie_set_cap_speed(pci, speed == NVKM_PCIE_SPEED_5_0);
+	g84_pcie_set_link_speed(pci, speed);
+	return 0;
+}
+
 static const struct nvkm_pci_func
 gf100_pci_func = {
 	.init = g84_pci_init,
@@ -36,6 +83,16 @@ gf100_pci_func = {
 	.wr08 = nv40_pci_wr08,
 	.wr32 = nv40_pci_wr32,
 	.msi_rearm = gf100_pci_msi_rearm,
+
+	.pcie.init = gf100_pcie_init,
+	.pcie.set_link = gf100_pcie_set_link,
+
+	.pcie.max_speed = g84_pcie_max_speed,
+	.pcie.cur_speed = g84_pcie_cur_speed,
+
+	.pcie.set_version = gf100_pcie_set_version,
+	.pcie.version = gf100_pcie_version,
+	.pcie.version_supported = g94_pcie_version_supported,
 };
 
 int
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf106.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf106.c
new file mode 100644
index 000000000000..c3b798c5c6dd
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gf106.c
@@ -0,0 +1,49 @@
+/*
+ * Copyright 2015 Karol Herbst <nouveau@karolherbst.de>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Karol Herbst <nouveau@karolherbst.de>
+ */
+#include "priv.h"
+
+static const struct nvkm_pci_func
+gf106_pci_func = {
+	.init = g84_pci_init,
+	.rd32 = nv40_pci_rd32,
+	.wr08 = nv40_pci_wr08,
+	.wr32 = nv40_pci_wr32,
+	.msi_rearm = nv40_pci_msi_rearm,
+
+	.pcie.init = gf100_pcie_init,
+	.pcie.set_link = gf100_pcie_set_link,
+
+	.pcie.max_speed = g84_pcie_max_speed,
+	.pcie.cur_speed = g84_pcie_cur_speed,
+
+	.pcie.set_version = gf100_pcie_set_version,
+	.pcie.version = gf100_pcie_version,
+	.pcie.version_supported = g94_pcie_version_supported,
+};
+
+int
+gf106_pci_new(struct nvkm_device *device, int index, struct nvkm_pci **ppci)
+{
+	return nvkm_pci_new_(&gf106_pci_func, device, index, ppci);
+}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gk104.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gk104.c
new file mode 100644
index 000000000000..e68030507d88
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/gk104.c
@@ -0,0 +1,228 @@
+/*
+ * Copyright 2015 Karol Herbst <nouveau@karolherbst.de>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Karol Herbst <nouveau@karolherbst.de>
+ */
+#include "priv.h"
+
+static int
+gk104_pcie_version_supported(struct nvkm_pci *pci)
+{
+	return (nvkm_rd32(pci->subdev.device, 0x8c1c0) & 0x4) == 0x4 ? 2 : 1;
+}
+
+static void
+gk104_pcie_set_cap_speed(struct nvkm_pci *pci, enum nvkm_pcie_speed speed)
+{
+	struct nvkm_device *device = pci->subdev.device;
+
+	switch (speed) {
+	case NVKM_PCIE_SPEED_2_5:
+		gf100_pcie_set_cap_speed(pci, false);
+		nvkm_mask(device, 0x8c1c0, 0x30000, 0x10000);
+		break;
+	case NVKM_PCIE_SPEED_5_0:
+		gf100_pcie_set_cap_speed(pci, true);
+		nvkm_mask(device, 0x8c1c0, 0x30000, 0x20000);
+		break;
+	case NVKM_PCIE_SPEED_8_0:
+		gf100_pcie_set_cap_speed(pci, true);
+		nvkm_mask(device, 0x8c1c0, 0x30000, 0x30000);
+		break;
+	}
+}
+
+static enum nvkm_pcie_speed
+gk104_pcie_cap_speed(struct nvkm_pci *pci)
+{
+	int speed = gf100_pcie_cap_speed(pci);
+
+	if (speed == 0)
+		return NVKM_PCIE_SPEED_2_5;
+
+	if (speed >= 1) {
+		int speed2 = nvkm_rd32(pci->subdev.device, 0x8c1c0) & 0x30000;
+		switch (speed2) {
+		case 0x00000:
+		case 0x10000:
+			return NVKM_PCIE_SPEED_2_5;
+		case 0x20000:
+			return NVKM_PCIE_SPEED_5_0;
+		case 0x30000:
+			return NVKM_PCIE_SPEED_8_0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void
+gk104_pcie_set_lnkctl_speed(struct nvkm_pci *pci, enum nvkm_pcie_speed speed)
+{
+	u8 reg_v = 0;
+	switch (speed) {
+	case NVKM_PCIE_SPEED_2_5:
+		reg_v = 1;
+		break;
+	case NVKM_PCIE_SPEED_5_0:
+		reg_v = 2;
+		break;
+	case NVKM_PCIE_SPEED_8_0:
+		reg_v = 3;
+		break;
+	}
+	nvkm_pci_mask(pci, 0xa8, 0x3, reg_v);
+}
+
+static enum nvkm_pcie_speed
+gk104_pcie_lnkctl_speed(struct nvkm_pci *pci)
+{
+	u8 reg_v = nvkm_pci_rd32(pci, 0xa8) & 0x3;
+	switch (reg_v) {
+	case 0:
+	case 1:
+		return NVKM_PCIE_SPEED_2_5;
+	case 2:
+		return NVKM_PCIE_SPEED_5_0;
+	case 3:
+		return NVKM_PCIE_SPEED_8_0;
+	}
+	return -1;
+}
+
+static enum nvkm_pcie_speed
+gk104_pcie_max_speed(struct nvkm_pci *pci)
+{
+	u32 max_speed = nvkm_rd32(pci->subdev.device, 0x8c1c0) & 0x300000;
+	switch (max_speed) {
+	case 0x000000:
+		return NVKM_PCIE_SPEED_8_0;
+	case 0x100000:
+		return NVKM_PCIE_SPEED_5_0;
+	case 0x200000:
+		return NVKM_PCIE_SPEED_2_5;
+	}
+	return NVKM_PCIE_SPEED_2_5;
+}
+
+static void
+gk104_pcie_set_link_speed(struct nvkm_pci *pci, enum nvkm_pcie_speed speed)
+{
+	struct nvkm_device *device = pci->subdev.device;
+	u32 mask_value;
+
+	switch (speed) {
+	case NVKM_PCIE_SPEED_8_0:
+		mask_value = 0x00000;
+		break;
+	case NVKM_PCIE_SPEED_5_0:
+		mask_value = 0x40000;
+		break;
+	case NVKM_PCIE_SPEED_2_5:
+	default:
+		mask_value = 0x80000;
+		break;
+	}
+
+	nvkm_mask(device, 0x8c040, 0xc0000, mask_value);
+	nvkm_mask(device, 0x8c040, 0x1, 0x1);
+}
+
+static int
+gk104_pcie_init(struct nvkm_pci * pci)
+{
+	enum nvkm_pcie_speed lnkctl_speed, max_speed, cap_speed;
+	struct nvkm_subdev *subdev = &pci->subdev;
+
+	if (gf100_pcie_version(pci) < 2)
+		return 0;
+
+	lnkctl_speed = gk104_pcie_lnkctl_speed(pci);
+	max_speed = gk104_pcie_max_speed(pci);
+	cap_speed = gk104_pcie_cap_speed(pci);
+
+	if (cap_speed != max_speed) {
+		nvkm_trace(subdev, "adjusting cap to max speed\n");
+		gk104_pcie_set_cap_speed(pci, max_speed);
+		cap_speed = gk104_pcie_cap_speed(pci);
+		if (cap_speed != max_speed)
+			nvkm_warn(subdev, "failed to adjust cap speed\n");
+	}
+
+	if (lnkctl_speed != max_speed) {
+		nvkm_debug(subdev, "adjusting lnkctl to max speed\n");
+		gk104_pcie_set_lnkctl_speed(pci, max_speed);
+		lnkctl_speed = gk104_pcie_lnkctl_speed(pci);
+		if (lnkctl_speed != max_speed)
+			nvkm_error(subdev, "failed to adjust lnkctl speed\n");
+	}
+
+	return 0;
+}
+
+static int
+gk104_pcie_set_link(struct nvkm_pci *pci, enum nvkm_pcie_speed speed, u8 width)
+{
+	struct nvkm_subdev *subdev = &pci->subdev;
+	enum nvkm_pcie_speed lnk_ctl_speed = gk104_pcie_lnkctl_speed(pci);
+	enum nvkm_pcie_speed lnk_cap_speed = gk104_pcie_cap_speed(pci);
+
+	if (speed > lnk_cap_speed) {
+		speed = lnk_cap_speed;
+		nvkm_warn(subdev, "dropping requested speed due too low cap"
+			  " speed\n");
+	}
+
+	if (speed > lnk_ctl_speed) {
+		speed = lnk_ctl_speed;
+		nvkm_warn(subdev, "dropping requested speed due too low"
+			  " lnkctl speed\n");
+	}
+
+	gk104_pcie_set_link_speed(pci, speed);
+	return 0;
+}
+
+
+static const struct nvkm_pci_func
+gk104_pci_func = {
+	.init = g84_pci_init,
+	.rd32 = nv40_pci_rd32,
+	.wr08 = nv40_pci_wr08,
+	.wr32 = nv40_pci_wr32,
+	.msi_rearm = nv40_pci_msi_rearm,
+
+	.pcie.init = gk104_pcie_init,
+	.pcie.set_link = gk104_pcie_set_link,
+
+	.pcie.max_speed = gk104_pcie_max_speed,
+	.pcie.cur_speed = g84_pcie_cur_speed,
+
+	.pcie.set_version = gf100_pcie_set_version,
+	.pcie.version = gf100_pcie_version,
+	.pcie.version_supported = gk104_pcie_version_supported,
+};
+
+int
+gk104_pci_new(struct nvkm_device *device, int index, struct nvkm_pci **ppci)
+{
+	return nvkm_pci_new_(&gk104_pci_func, device, index, ppci);
+}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/pcie.c b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/pcie.c
new file mode 100644
index 000000000000..d71e5db5028a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/pcie.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright 2015 Karol Herbst <nouveau@karolherbst.de>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Karol Herbst <git@karolherbst.de>
+ */
+#include "priv.h"
+
+static char *nvkm_pcie_speeds[] = {
+	"2.5GT/s",
+	"5.0GT/s",
+	"8.0GT/s",
+};
+
+static enum nvkm_pcie_speed
+nvkm_pcie_speed(enum pci_bus_speed speed)
+{
+	switch (speed) {
+	case PCIE_SPEED_2_5GT:
+		return NVKM_PCIE_SPEED_2_5;
+	case PCIE_SPEED_5_0GT:
+		return NVKM_PCIE_SPEED_5_0;
+	case PCIE_SPEED_8_0GT:
+		return NVKM_PCIE_SPEED_8_0;
+	default:
+		/* XXX 0x16 is 8_0, assume 0x17 will be 16_0 for now */
+		if (speed == 0x17)
+			return NVKM_PCIE_SPEED_8_0;
+		return -1;
+	}
+}
+
+static int
+nvkm_pcie_get_version(struct nvkm_pci *pci)
+{
+	if (!pci->func->pcie.version)
+		return -ENOSYS;
+
+	return pci->func->pcie.version(pci);
+}
+
+static int
+nvkm_pcie_get_max_version(struct nvkm_pci *pci)
+{
+	if (!pci->func->pcie.version_supported)
+		return -ENOSYS;
+
+	return pci->func->pcie.version_supported(pci);
+}
+
+static int
+nvkm_pcie_set_version(struct nvkm_pci *pci, int version)
+{
+	if (!pci->func->pcie.set_version)
+		return -ENOSYS;
+
+	nvkm_trace(&pci->subdev, "set to version %i\n", version);
+	pci->func->pcie.set_version(pci, version);
+	return nvkm_pcie_get_version(pci);
+}
+
+int
+nvkm_pcie_oneinit(struct nvkm_pci *pci)
+{
+	if (pci->func->pcie.max_speed)
+		nvkm_debug(&pci->subdev, "pcie max speed: %s\n",
+			   nvkm_pcie_speeds[pci->func->pcie.max_speed(pci)]);
+	return 0;
+}
+
+int
+nvkm_pcie_init(struct nvkm_pci *pci)
+{
+	struct nvkm_subdev *subdev = &pci->subdev;
+	int ret;
+
+	/* raise pcie version first */
+	ret = nvkm_pcie_get_version(pci);
+	if (ret > 0) {
+		int max_version = nvkm_pcie_get_max_version(pci);
+		if (max_version > 0 && max_version > ret)
+			ret = nvkm_pcie_set_version(pci, max_version);
+
+		if (ret < max_version)
+			nvkm_error(subdev, "couldn't raise version: %i\n", ret);
+	}
+
+	if (pci->func->pcie.init)
+		pci->func->pcie.init(pci);
+
+	if (pci->pcie.speed != -1)
+		nvkm_pcie_set_link(pci, pci->pcie.speed, pci->pcie.width);
+
+	return 0;
+}
+
+int
+nvkm_pcie_set_link(struct nvkm_pci *pci, enum nvkm_pcie_speed speed, u8 width)
+{
+	struct nvkm_subdev *subdev = &pci->subdev;
+	enum nvkm_pcie_speed cur_speed, max_speed;
+	struct pci_bus *pbus;
+	int ret;
+
+	if (!pci || !pci_is_pcie(pci->pdev))
+		return 0;
+	pbus = pci->pdev->bus;
+
+	if (!pci->func->pcie.set_link)
+		return -ENOSYS;
+
+	nvkm_trace(subdev, "requested %s\n", nvkm_pcie_speeds[speed]);
+
+	if (pci->func->pcie.version(pci) < 2) {
+		nvkm_error(subdev, "setting link failed due to low version\n");
+		return -ENODEV;
+	}
+
+	cur_speed = pci->func->pcie.cur_speed(pci);
+	max_speed = min(nvkm_pcie_speed(pbus->max_bus_speed),
+			pci->func->pcie.max_speed(pci));
+
+	nvkm_trace(subdev, "current speed: %s\n", nvkm_pcie_speeds[cur_speed]);
+
+	if (speed > max_speed) {
+		nvkm_debug(subdev, "%s not supported by bus or card, dropping"
+			   "requested speed to %s", nvkm_pcie_speeds[speed],
+			   nvkm_pcie_speeds[max_speed]);
+		speed = max_speed;
+	}
+
+	pci->pcie.speed = speed;
+	pci->pcie.width = width;
+
+	if (speed == cur_speed) {
+		nvkm_debug(subdev, "requested matches current speed\n");
+		return speed;
+	}
+
+	nvkm_debug(subdev, "set link to %s x%i\n",
+		   nvkm_pcie_speeds[speed], width);
+
+	ret = pci->func->pcie.set_link(pci, speed, width);
+	if (ret < 0)
+		nvkm_error(subdev, "setting link failed: %i\n", ret);
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/priv.h b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/priv.h
index cf46d38d0b0a..23de3180aae5 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pci/priv.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pci/priv.h
@@ -12,6 +12,18 @@ struct nvkm_pci_func {
 	void (*wr08)(struct nvkm_pci *, u16 addr, u8 data);
 	void (*wr32)(struct nvkm_pci *, u16 addr, u32 data);
 	void (*msi_rearm)(struct nvkm_pci *);
+
+	struct {
+		int (*init)(struct nvkm_pci *);
+		int (*set_link)(struct nvkm_pci *, enum nvkm_pcie_speed, u8);
+
+		enum nvkm_pcie_speed (*max_speed)(struct nvkm_pci *);
+		enum nvkm_pcie_speed (*cur_speed)(struct nvkm_pci *);
+
+		void (*set_version)(struct nvkm_pci *, u8);
+		int (*version)(struct nvkm_pci *);
+		int (*version_supported)(struct nvkm_pci *);
+	} pcie;
 };
 
 u32 nv40_pci_rd32(struct nvkm_pci *, u16);
@@ -22,4 +34,25 @@ void nv40_pci_msi_rearm(struct nvkm_pci *);
 void nv46_pci_msi_rearm(struct nvkm_pci *);
 
 void g84_pci_init(struct nvkm_pci *pci);
+
+/* pcie functions */
+void g84_pcie_set_version(struct nvkm_pci *, u8);
+int g84_pcie_version(struct nvkm_pci *);
+void g84_pcie_set_link_speed(struct nvkm_pci *, enum nvkm_pcie_speed);
+enum nvkm_pcie_speed g84_pcie_cur_speed(struct nvkm_pci *);
+enum nvkm_pcie_speed g84_pcie_max_speed(struct nvkm_pci *);
+int g84_pcie_init(struct nvkm_pci *);
+int g84_pcie_set_link(struct nvkm_pci *, enum nvkm_pcie_speed, u8);
+
+int g94_pcie_version_supported(struct nvkm_pci *);
+
+void gf100_pcie_set_version(struct nvkm_pci *, u8);
+int gf100_pcie_version(struct nvkm_pci *);
+void gf100_pcie_set_cap_speed(struct nvkm_pci *, bool);
+int gf100_pcie_cap_speed(struct nvkm_pci *);
+int gf100_pcie_init(struct nvkm_pci *);
+int gf100_pcie_set_link(struct nvkm_pci *, enum nvkm_pcie_speed, u8);
+
+int nvkm_pcie_oneinit(struct nvkm_pci *);
+int nvkm_pcie_init(struct nvkm_pci *);
 #endif
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf100.fuc3.h b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf100.fuc3.h
index 302557c52d03..770294457274 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf100.fuc3.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf100.fuc3.h
@@ -24,8 +24,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000512,
-	0x000004af,
+	0x00000507,
+	0x000004a4,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x0000075e,
-	0x00000750,
+	0x00000753,
+	0x00000745,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000762,
-	0x00000760,
+	0x00000757,
+	0x00000755,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000b92,
-	0x00000a35,
+	0x00000b87,
+	0x00000a2a,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000bbb,
-	0x00000b94,
+	0x00000bb0,
+	0x00000b89,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t gf100_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000bc7,
-	0x00000bc5,
+	0x00000bbc,
+	0x00000bba,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -229,26 +229,26 @@ uint32_t gf100_pmu_data[] = {
 /* 0x0370: memx_func_head */
 	0x00000001,
 	0x00000000,
-	0x00000551,
+	0x00000546,
 /* 0x037c: memx_func_next */
 	0x00000002,
 	0x00000000,
-	0x000005db,
+	0x000005d0,
 	0x00000003,
 	0x00000002,
-	0x000006a5,
+	0x0000069a,
 	0x00040004,
 	0x00000000,
-	0x000006c1,
+	0x000006b6,
 	0x00010005,
 	0x00000000,
-	0x000006de,
+	0x000006d3,
 	0x00010006,
 	0x00000000,
-	0x00000663,
+	0x00000658,
 	0x00000007,
 	0x00000000,
-	0x000006e9,
+	0x000006de,
 /* 0x03c4: memx_func_tail */
 /* 0x03c4: memx_ts_start */
 	0x00000000,
@@ -917,7 +917,7 @@ uint32_t gf100_pmu_data[] = {
 };
 
 uint32_t gf100_pmu_code[] = {
-	0x039e0ef5,
+	0x03930ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xd00604b6,
@@ -987,7 +987,7 @@ uint32_t gf100_pmu_code[] = {
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x02dd21f5,
+	0x02d221f5,
 	0x0ef494bd,
 /* 0x00f9: intr_watchdog_next_time */
 	0x9b0a9815,
@@ -1039,7 +1039,7 @@ uint32_t gf100_pmu_code[] = {
 	0x48e7f1c0,
 	0x53e3f14f,
 	0x00d7f054,
-	0x034221f5,
+	0x033721f5,
 	0x07f1c0fc,
 	0x04b604c0,
 	0x000cd006,
@@ -1048,760 +1048,758 @@ uint32_t gf100_pmu_code[] = {
 	0x04b60688,
 	0x0009d006,
 /* 0x01ca: intr_skip_subintr */
-	0x89c404bd,
-	0x070bf420,
-	0xffbfa4f1,
-/* 0x01d4: intr_skip_pause */
-	0xf44089c4,
-	0xa4f1070b,
-/* 0x01de: intr_skip_user0 */
-	0x07f0ffbf,
-	0x0604b604,
-	0xbd0008d0,
-	0xfe80fc04,
-	0xf0fc0088,
-	0xd0fce0fc,
-	0xb0fcc0fc,
-	0x90fca0fc,
-	0x00fc80fc,
-	0xf80032f4,
-/* 0x0205: ticks_from_ns */
-	0xf9c0f901,
-	0xcbd7f1b0,
-	0x00d3f000,
-	0x041321f5,
-	0x03e8ccec,
-	0xf400b4b0,
-	0xeeec120b,
-	0xd7f103e8,
-	0xd3f000cb,
-	0x1321f500,
-/* 0x022d: ticks_from_ns_quit */
-	0x02ceb904,
-	0xc0fcb0fc,
-/* 0x0236: ticks_from_us */
-	0xc0f900f8,
+	0x97f104bd,
+	0x90bd00e0,
+	0xf00489fd,
+	0x04b60407,
+	0x0008d006,
+	0x80fc04bd,
+	0xfc0088fe,
+	0xfce0fcf0,
+	0xfcc0fcd0,
+	0xfca0fcb0,
+	0xfc80fc90,
+	0x0032f400,
+/* 0x01fa: ticks_from_ns */
+	0xc0f901f8,
 	0xd7f1b0f9,
 	0xd3f000cb,
-	0x1321f500,
-	0x02ceb904,
-	0xf400b4b0,
-	0xe4bd050b,
-/* 0x0250: ticks_from_us_quit */
-	0xc0fcb0fc,
-/* 0x0256: ticks_to_us */
-	0xd7f100f8,
-	0xd3f000cb,
-	0xecedff00,
-/* 0x0262: timer */
-	0x90f900f8,
-	0x32f480f9,
-	0x03f89810,
-	0xf40086b0,
-	0x84bd651c,
-	0xb63807f0,
-	0x08d00604,
-	0xf004bd00,
-	0x84b63487,
-	0x0088cf06,
-	0xbb9a0998,
-	0xe9bb0298,
-	0x03fe8000,
-	0xb60887f0,
-	0x88cf0684,
-	0x0284f000,
-	0xf0261bf4,
-	0x84b63487,
-	0x0088cf06,
-	0xf406e0b8,
-	0xe8b8090b,
-	0x111cf406,
-/* 0x02b8: timer_reset */
-	0xb63407f0,
-	0x0ed00604,
-	0x8004bd00,
-/* 0x02c6: timer_enable */
-	0x87f09a0e,
-	0x3807f001,
+	0x0821f500,
+	0xe8ccec04,
+	0x00b4b003,
+	0xec120bf4,
+	0xf103e8ee,
+	0xf000cbd7,
+	0x21f500d3,
+/* 0x0222: ticks_from_ns_quit */
+	0xceb90408,
+	0xfcb0fc02,
+/* 0x022b: ticks_from_us */
+	0xf900f8c0,
+	0xf1b0f9c0,
+	0xf000cbd7,
+	0x21f500d3,
+	0xceb90408,
+	0x00b4b002,
+	0xbd050bf4,
+/* 0x0245: ticks_from_us_quit */
+	0xfcb0fce4,
+/* 0x024b: ticks_to_us */
+	0xf100f8c0,
+	0xf000cbd7,
+	0xedff00d3,
+/* 0x0257: timer */
+	0xf900f8ec,
+	0xf480f990,
+	0xf8981032,
+	0x0086b003,
+	0xbd651cf4,
+	0x3807f084,
 	0xd00604b6,
 	0x04bd0008,
-/* 0x02d4: timer_done */
-	0xfc1031f4,
-	0xf890fc80,
-/* 0x02dd: send_proc */
-	0xf980f900,
-	0x05e89890,
-	0xf004e998,
-	0x89b80486,
-	0x2a0bf406,
-	0x940398c4,
-	0x80b60488,
-	0x008ebb18,
-	0x8000fa98,
-	0x8d80008a,
-	0x028c8001,
-	0xb6038b80,
-	0x94f00190,
-	0x04e98007,
-/* 0x0317: send_done */
-	0xfc0231f4,
-	0xf880fc90,
-/* 0x031d: find */
-	0xf080f900,
-	0x31f45887,
-/* 0x0325: find_loop */
-	0x008a9801,
-	0xf406aeb8,
-	0x80b6100b,
-	0x6886b158,
-	0xf01bf402,
-/* 0x033b: find_done */
-	0xb90132f4,
-	0x80fc028e,
-/* 0x0342: send */
-	0x21f500f8,
-	0x01f4031d,
-/* 0x034b: recv */
-	0xf900f897,
-	0x9880f990,
-	0xe99805e8,
-	0x0132f404,
-	0xf40689b8,
-	0x89c43d0b,
-	0x0180b603,
-	0x800784f0,
-	0xea9805e8,
-	0xfef0f902,
-	0xf0f9018f,
-	0x9402efb9,
-	0xe9bb0499,
-	0x18e0b600,
-	0x9803eb98,
-	0xed9802ec,
-	0x00ee9801,
-	0xf0fca5f9,
-	0xf400f8fe,
-	0xf0fc0131,
-/* 0x0398: recv_done */
-	0x90fc80fc,
-/* 0x039e: init */
-	0x17f100f8,
-	0x14b60108,
-	0x0011cf06,
-	0x010911e7,
-	0xfe0814b6,
-	0x17f10014,
-	0x13f000e0,
-	0x1c07f000,
-	0xd00604b6,
-	0x04bd0001,
-	0xf0ff17f0,
-	0x04b61407,
-	0x0001d006,
-	0x17f004bd,
-	0x0015f102,
-	0x1007f008,
+	0xb63487f0,
+	0x88cf0684,
+	0x9a099800,
+	0xbb0298bb,
+	0xfe8000e9,
+	0x0887f003,
+	0xcf0684b6,
+	0x84f00088,
+	0x261bf402,
+	0xb63487f0,
+	0x88cf0684,
+	0x06e0b800,
+	0xb8090bf4,
+	0x1cf406e8,
+/* 0x02ad: timer_reset */
+	0x3407f011,
 	0xd00604b6,
-	0x04bd0001,
-	0x011a17f1,
-	0xfe0013f0,
-	0x31f40010,
-	0x0117f010,
-	0xb63807f0,
+	0x04bd000e,
+/* 0x02bb: timer_enable */
+	0xf09a0e80,
+	0x07f00187,
+	0x0604b638,
+	0xbd0008d0,
+/* 0x02c9: timer_done */
+	0x1031f404,
+	0x90fc80fc,
+/* 0x02d2: send_proc */
+	0x80f900f8,
+	0xe89890f9,
+	0x04e99805,
+	0xb80486f0,
+	0x0bf40689,
+	0x0398c42a,
+	0xb6048894,
+	0x8ebb1880,
+	0x00fa9800,
+	0x80008a80,
+	0x8c80018d,
+	0x038b8002,
+	0xf00190b6,
+	0xe9800794,
+	0x0231f404,
+/* 0x030c: send_done */
+	0x80fc90fc,
+/* 0x0312: find */
+	0x80f900f8,
+	0xf45887f0,
+/* 0x031a: find_loop */
+	0x8a980131,
+	0x06aeb800,
+	0xb6100bf4,
+	0x86b15880,
+	0x1bf40268,
+	0x0132f4f0,
+/* 0x0330: find_done */
+	0xfc028eb9,
+/* 0x0337: send */
+	0xf500f880,
+	0xf4031221,
+	0x00f89701,
+/* 0x0340: recv */
+	0x80f990f9,
+	0x9805e898,
+	0x32f404e9,
+	0x0689b801,
+	0xc43d0bf4,
+	0x80b60389,
+	0x0784f001,
+	0x9805e880,
+	0xf0f902ea,
+	0xf9018ffe,
+	0x02efb9f0,
+	0xbb049994,
+	0xe0b600e9,
+	0x03eb9818,
+	0x9802ec98,
+	0xee9801ed,
+	0xfca5f900,
+	0x00f8fef0,
+	0xfc0131f4,
+/* 0x038d: recv_done */
+	0xfc80fcf0,
+/* 0x0393: init */
+	0xf100f890,
+	0xb6010817,
+	0x11cf0614,
+	0x0911e700,
+	0x0814b601,
+	0xf10014fe,
+	0xf000e017,
+	0x07f00013,
+	0x0604b61c,
+	0xbd0001d0,
+	0xff17f004,
+	0xb61407f0,
 	0x01d00604,
 	0xf004bd00,
-/* 0x0402: init_proc */
-	0xf19858f7,
-	0x0016b001,
-	0xf9fa0bf4,
-	0x58f0b615,
-/* 0x0413: mulu32_32_64 */
-	0xf9f20ef4,
-	0xf920f910,
-	0x9540f930,
-	0xd29510e1,
-	0xbdc4bd10,
-	0xc0edffb4,
-	0xb9301dff,
-	0x34f10234,
-	0x34b6ffff,
-	0x1045b610,
-	0xbb00c3bb,
-	0xe2ff01b4,
-	0x0234b930,
-	0xffff34f1,
-	0xb61034b6,
-	0xc3bb1045,
-	0x01b4bb00,
-	0xbb3012ff,
-	0x40fc00b3,
-	0x20fc30fc,
-	0x00f810fc,
-/* 0x0464: host_send */
-	0x04b017f1,
-	0xcf0614b6,
-	0x27f10011,
-	0x24b604a0,
-	0x0022cf06,
-	0xf40612b8,
-	0x1ec4320b,
-	0x04ee9407,
-	0x0270e0b7,
-	0x9803eb98,
-	0xed9802ec,
-	0x00ee9801,
-	0x034221f5,
-	0xc40110b6,
-	0x07f10f1e,
-	0x04b604b0,
-	0x000ed006,
-	0x0ef404bd,
-/* 0x04ad: host_send_done */
-/* 0x04af: host_recv */
-	0xf100f8ba,
-	0xf14e4917,
-	0xb8525413,
-	0x0bf406e1,
-/* 0x04bd: host_recv_wait */
-	0xcc17f1aa,
+	0x15f10217,
+	0x07f00800,
+	0x0604b610,
+	0xbd0001d0,
+	0x1a17f104,
+	0x0013f001,
+	0xf40010fe,
+	0x17f01031,
+	0x3807f001,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x03f7: init_proc */
+	0x9858f7f0,
+	0x16b001f1,
+	0xfa0bf400,
+	0xf0b615f9,
+	0xf20ef458,
+/* 0x0408: mulu32_32_64 */
+	0x20f910f9,
+	0x40f930f9,
+	0x9510e195,
+	0xc4bd10d2,
+	0xedffb4bd,
+	0x301dffc0,
+	0xf10234b9,
+	0xb6ffff34,
+	0x45b61034,
+	0x00c3bb10,
+	0xff01b4bb,
+	0x34b930e2,
+	0xff34f102,
+	0x1034b6ff,
+	0xbb1045b6,
+	0xb4bb00c3,
+	0x3012ff01,
+	0xfc00b3bb,
+	0xfc30fc40,
+	0xf810fc20,
+/* 0x0459: host_send */
+	0xb017f100,
 	0x0614b604,
 	0xf10011cf,
-	0xb604c827,
+	0xb604a027,
 	0x22cf0624,
-	0x0816f000,
-	0xf40612b8,
-	0x23c4e60b,
-	0x0434b607,
-	0x02f030b7,
-	0x80033b80,
-	0x3d80023c,
-	0x003e8001,
-	0xf00120b6,
-	0x07f10f24,
-	0x04b604c8,
-	0x0002d006,
-	0x27f004bd,
-	0x0007f040,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x0512: host_init */
-	0x17f100f8,
+	0x0612b800,
+	0xc4320bf4,
+	0xee94071e,
+	0x70e0b704,
+	0x03eb9802,
+	0x9802ec98,
+	0xee9801ed,
+	0x3721f500,
+	0x0110b603,
+	0xf10f1ec4,
+	0xb604b007,
+	0x0ed00604,
+	0xf404bd00,
+/* 0x04a2: host_send_done */
+	0x00f8ba0e,
+/* 0x04a4: host_recv */
+	0x4e4917f1,
+	0x525413f1,
+	0xf406e1b8,
+/* 0x04b2: host_recv_wait */
+	0x17f1aa0b,
+	0x14b604cc,
+	0x0011cf06,
+	0x04c827f1,
+	0xcf0624b6,
+	0x16f00022,
+	0x0612b808,
+	0xc4e60bf4,
+	0x34b60723,
+	0xf030b704,
+	0x033b8002,
+	0x80023c80,
+	0x3e80013d,
+	0x0120b600,
+	0xf10f24f0,
+	0xb604c807,
+	0x02d00604,
+	0xf004bd00,
+	0x07f04027,
+	0x0604b600,
+	0xbd0002d0,
+/* 0x0507: host_init */
+	0xf100f804,
+	0xb6008017,
+	0x15f11014,
+	0x07f10270,
+	0x04b604d0,
+	0x0001d006,
+	0x17f104bd,
 	0x14b60080,
-	0x7015f110,
-	0xd007f102,
+	0xf015f110,
+	0xdc07f102,
 	0x0604b604,
 	0xbd0001d0,
-	0x8017f104,
-	0x1014b600,
-	0x02f015f1,
-	0x04dc07f1,
+	0x0117f004,
+	0x04c407f1,
 	0xd00604b6,
 	0x04bd0001,
-	0xf10117f0,
-	0xb604c407,
-	0x01d00604,
-	0xf804bd00,
-/* 0x0551: memx_func_enter */
-	0x2067f100,
-	0x5d77f116,
-	0xff73f1f5,
+/* 0x0546: memx_func_enter */
+	0x67f100f8,
+	0x77f11620,
+	0x73f1f55d,
+	0x6eb9ffff,
+	0x0421f402,
+	0xfd02d8b9,
+	0x60f90487,
+	0xd0fc80f9,
+	0x21f4e0fc,
+	0xfe77f13f,
+	0xff73f1ff,
 	0x026eb9ff,
 	0xb90421f4,
 	0x87fd02d8,
 	0xf960f904,
 	0xfcd0fc80,
 	0x3f21f4e0,
-	0xfffe77f1,
-	0xffff73f1,
+	0x26f067f1,
 	0xf4026eb9,
 	0xd8b90421,
 	0x0487fd02,
 	0x80f960f9,
 	0xe0fcd0fc,
-	0xf13f21f4,
-	0xb926f067,
-	0x21f4026e,
-	0x02d8b904,
-	0xf90487fd,
-	0xfc80f960,
-	0xf4e0fcd0,
-	0x67f03f21,
-	0xe007f104,
-	0x0604b607,
-	0xbd0006d0,
-/* 0x05bd: memx_func_enter_wait */
-	0xc067f104,
-	0x0664b607,
-	0xf00066cf,
-	0x0bf40464,
-	0x2c67f0f3,
-	0xcf0664b6,
-	0x06800066,
-/* 0x05db: memx_func_leave */
-	0xf000f8f1,
-	0x64b62c67,
-	0x0066cf06,
-	0xf0f20680,
+	0xf03f21f4,
 	0x07f10467,
-	0x04b607e4,
+	0x04b607e0,
 	0x0006d006,
-/* 0x05f6: memx_func_leave_wait */
+/* 0x05b2: memx_func_enter_wait */
 	0x67f104bd,
 	0x64b607c0,
 	0x0066cf06,
 	0xf40464f0,
-	0x67f1f31b,
-	0x77f126f0,
-	0x73f00001,
-	0x026eb900,
-	0xb90421f4,
-	0x87fd02d8,
-	0xf960f905,
-	0xfcd0fc80,
-	0x3f21f4e0,
-	0x162067f1,
-	0xf4026eb9,
-	0xd8b90421,
-	0x0587fd02,
-	0x80f960f9,
-	0xe0fcd0fc,
-	0xf13f21f4,
-	0xf00aa277,
+	0x67f0f30b,
+	0x0664b62c,
+	0x800066cf,
+	0x00f8f106,
+/* 0x05d0: memx_func_leave */
+	0xb62c67f0,
+	0x66cf0664,
+	0xf2068000,
+	0xf10467f0,
+	0xb607e407,
+	0x06d00604,
+/* 0x05eb: memx_func_leave_wait */
+	0xf104bd00,
+	0xb607c067,
+	0x66cf0664,
+	0x0464f000,
+	0xf1f31bf4,
+	0xf126f067,
+	0xf0000177,
 	0x6eb90073,
 	0x0421f402,
 	0xfd02d8b9,
 	0x60f90587,
 	0xd0fc80f9,
 	0x21f4e0fc,
-/* 0x0663: memx_func_wait_vblank */
-	0x9800f83f,
-	0x66b00016,
-	0x130bf400,
-	0xf40166b0,
-	0x0ef4060b,
-/* 0x0675: memx_func_wait_vblank_head1 */
-	0x2077f12e,
-	0x070ef400,
-/* 0x067c: memx_func_wait_vblank_head0 */
-	0x000877f1,
-/* 0x0680: memx_func_wait_vblank_0 */
-	0x07c467f1,
-	0xcf0664b6,
-	0x67fd0066,
-	0xf31bf404,
-/* 0x0690: memx_func_wait_vblank_1 */
-	0x07c467f1,
-	0xcf0664b6,
-	0x67fd0066,
-	0xf30bf404,
-/* 0x06a0: memx_func_wait_vblank_fini */
-	0xf80410b6,
-/* 0x06a5: memx_func_wr32 */
-	0x00169800,
-	0xb6011598,
-	0x60f90810,
-	0xd0fc50f9,
-	0x21f4e0fc,
-	0x0242b63f,
-	0xf8e91bf4,
-/* 0x06c1: memx_func_wait */
-	0x2c87f000,
-	0xcf0684b6,
-	0x1e980088,
-	0x011d9800,
-	0x98021c98,
-	0x10b6031b,
-	0xa421f410,
-/* 0x06de: memx_func_delay */
-	0x1e9800f8,
+	0x2067f13f,
+	0x026eb916,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f905,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0x0aa277f1,
+	0xb90073f0,
+	0x21f4026e,
+	0x02d8b904,
+	0xf90587fd,
+	0xfc80f960,
+	0xf4e0fcd0,
+	0x00f83f21,
+/* 0x0658: memx_func_wait_vblank */
+	0xb0001698,
+	0x0bf40066,
+	0x0166b013,
+	0xf4060bf4,
+/* 0x066a: memx_func_wait_vblank_head1 */
+	0x77f12e0e,
+	0x0ef40020,
+/* 0x0671: memx_func_wait_vblank_head0 */
+	0x0877f107,
+/* 0x0675: memx_func_wait_vblank_0 */
+	0xc467f100,
+	0x0664b607,
+	0xfd0066cf,
+	0x1bf40467,
+/* 0x0685: memx_func_wait_vblank_1 */
+	0xc467f1f3,
+	0x0664b607,
+	0xfd0066cf,
+	0x0bf40467,
+/* 0x0695: memx_func_wait_vblank_fini */
+	0x0410b6f3,
+/* 0x069a: memx_func_wr32 */
+	0x169800f8,
+	0x01159800,
+	0xf90810b6,
+	0xfc50f960,
+	0xf4e0fcd0,
+	0x42b63f21,
+	0xe91bf402,
+/* 0x06b6: memx_func_wait */
+	0x87f000f8,
+	0x0684b62c,
+	0x980088cf,
+	0x1d98001e,
+	0x021c9801,
+	0xb6031b98,
+	0x21f41010,
+/* 0x06d3: memx_func_delay */
+	0x9800f8a4,
+	0x10b6001e,
+	0x7f21f404,
+/* 0x06de: memx_func_train */
+	0x00f800f8,
+/* 0x06e0: memx_exec */
+	0xd0f9e0f9,
+	0xb902c1b9,
+/* 0x06ea: memx_exec_next */
+	0x139802b2,
 	0x0410b600,
+	0x01f034e7,
+	0x01e033e7,
+	0xf00132b6,
+	0x35980c30,
+	0xb855f9de,
+	0x1ef40612,
+	0xf10b98e4,
+	0xbbf20c98,
+	0xb7f102cb,
+	0xb4b607c4,
+	0x00bbcf06,
+	0xe0fcd0fc,
+	0x033721f5,
+/* 0x0726: memx_info */
+	0xc67000f8,
+	0x0e0bf401,
+/* 0x072c: memx_info_data */
+	0x03ccc7f1,
+	0x0800b7f1,
+/* 0x0737: memx_info_train */
+	0xf10b0ef4,
+	0xf10bccc7,
+/* 0x073f: memx_info_send */
+	0xf50100b7,
+	0xf8033721,
+/* 0x0745: memx_recv */
+	0x01d6b000,
+	0xb0980bf4,
+	0x0bf400d6,
+/* 0x0753: memx_init */
+	0xf800f8d8,
+/* 0x0755: perf_recv */
+/* 0x0757: perf_init */
+	0xf800f800,
+/* 0x0759: i2c_drive_scl */
+	0x0036b000,
+	0xf1110bf4,
+	0xb607e007,
+	0x01d00604,
+	0xf804bd00,
+/* 0x076d: i2c_drive_scl_lo */
+	0xe407f100,
+	0x0604b607,
+	0xbd0001d0,
+/* 0x077b: i2c_drive_sda */
+	0xb000f804,
+	0x0bf40036,
+	0xe007f111,
+	0x0604b607,
+	0xbd0002d0,
+/* 0x078f: i2c_drive_sda_lo */
+	0xf100f804,
+	0xb607e407,
+	0x02d00604,
+	0xf804bd00,
+/* 0x079d: i2c_sense_scl */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x31fd0033,
+	0x060bf404,
+/* 0x07b3: i2c_sense_scl_done */
+	0xf80131f4,
+/* 0x07b5: i2c_sense_sda */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x32fd0033,
+	0x060bf404,
+/* 0x07cb: i2c_sense_sda_done */
+	0xf80131f4,
+/* 0x07cd: i2c_raise_scl */
+	0xf140f900,
+	0xf0089847,
+	0x21f50137,
+/* 0x07da: i2c_raise_scl_wait */
+	0xe7f10759,
+	0x21f403e8,
+	0x9d21f57f,
+	0x0901f407,
+	0xf40142b6,
+/* 0x07ee: i2c_raise_scl_done */
+	0x40fcef1b,
+/* 0x07f2: i2c_start */
+	0x21f500f8,
+	0x11f4079d,
+	0xb521f50d,
+	0x0611f407,
+/* 0x0803: i2c_start_rep */
+	0xf0300ef4,
+	0x21f50037,
+	0x37f00759,
+	0x7b21f501,
+	0x0076bb07,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b607cd,
+	0x1f11f404,
+/* 0x0830: i2c_start_send */
+	0xf50037f0,
+	0xf1077b21,
+	0xf41388e7,
+	0x37f07f21,
+	0x5921f500,
+	0x88e7f107,
+	0x7f21f413,
+/* 0x084c: i2c_start_out */
+/* 0x084e: i2c_stop */
+	0x37f000f8,
+	0x5921f500,
+	0x0037f007,
+	0x077b21f5,
+	0x03e8e7f1,
+	0xf07f21f4,
+	0x21f50137,
+	0xe7f10759,
+	0x21f41388,
+	0x0137f07f,
+	0x077b21f5,
+	0x1388e7f1,
 	0xf87f21f4,
-/* 0x06e9: memx_func_train */
-/* 0x06eb: memx_exec */
-	0xf900f800,
-	0xb9d0f9e0,
-	0xb2b902c1,
-/* 0x06f5: memx_exec_next */
-	0x00139802,
-	0xe70410b6,
-	0xe701f034,
-	0xb601e033,
-	0x30f00132,
-	0xde35980c,
-	0x12b855f9,
-	0xe41ef406,
-	0x98f10b98,
-	0xcbbbf20c,
-	0xc4b7f102,
-	0x06b4b607,
-	0xfc00bbcf,
-	0xf5e0fcd0,
-	0xf8034221,
-/* 0x0731: memx_info */
-	0x01c67000,
-/* 0x0737: memx_info_data */
-	0xf10e0bf4,
-	0xf103ccc7,
-	0xf40800b7,
-/* 0x0742: memx_info_train */
-	0xc7f10b0e,
-	0xb7f10bcc,
-/* 0x074a: memx_info_send */
-	0x21f50100,
-	0x00f80342,
-/* 0x0750: memx_recv */
-	0xf401d6b0,
-	0xd6b0980b,
-	0xd80bf400,
-/* 0x075e: memx_init */
-	0x00f800f8,
-/* 0x0760: perf_recv */
-/* 0x0762: perf_init */
-	0x00f800f8,
-/* 0x0764: i2c_drive_scl */
-	0xf40036b0,
-	0x07f1110b,
-	0x04b607e0,
-	0x0001d006,
-	0x00f804bd,
-/* 0x0778: i2c_drive_scl_lo */
-	0x07e407f1,
-	0xd00604b6,
-	0x04bd0001,
-/* 0x0786: i2c_drive_sda */
-	0x36b000f8,
-	0x110bf400,
-	0x07e007f1,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x079a: i2c_drive_sda_lo */
-	0x07f100f8,
-	0x04b607e4,
-	0x0002d006,
-	0x00f804bd,
-/* 0x07a8: i2c_sense_scl */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0431fd00,
-	0xf4060bf4,
-/* 0x07be: i2c_sense_scl_done */
-	0x00f80131,
-/* 0x07c0: i2c_sense_sda */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0432fd00,
-	0xf4060bf4,
-/* 0x07d6: i2c_sense_sda_done */
-	0x00f80131,
-/* 0x07d8: i2c_raise_scl */
-	0x47f140f9,
-	0x37f00898,
-	0x6421f501,
-/* 0x07e5: i2c_raise_scl_wait */
+/* 0x0881: i2c_bitw */
+	0x7b21f500,
 	0xe8e7f107,
 	0x7f21f403,
-	0x07a821f5,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x07f9: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x07fd: i2c_start */
-	0xa821f500,
-	0x0d11f407,
-	0x07c021f5,
-	0xf40611f4,
-/* 0x080e: i2c_start_rep */
-	0x37f0300e,
-	0x6421f500,
-	0x0137f007,
-	0x078621f5,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xd821f550,
+	0xcd21f550,
 	0x0464b607,
-/* 0x083b: i2c_start_send */
-	0xf01f11f4,
-	0x21f50037,
-	0xe7f10786,
-	0x21f41388,
-	0x0037f07f,
-	0x076421f5,
-	0x1388e7f1,
-/* 0x0857: i2c_start_out */
-	0xf87f21f4,
-/* 0x0859: i2c_stop */
-	0x0037f000,
-	0x076421f5,
-	0xf50037f0,
-	0xf1078621,
-	0xf403e8e7,
+	0xf11811f4,
+	0xf41388e7,
 	0x37f07f21,
-	0x6421f501,
+	0x5921f500,
 	0x88e7f107,
 	0x7f21f413,
-	0xf50137f0,
-	0xf1078621,
-	0xf41388e7,
-	0x00f87f21,
-/* 0x088c: i2c_bitw */
-	0x078621f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x07d821f5,
-	0xf40464b6,
-	0xe7f11811,
+/* 0x08c0: i2c_bitw_out */
+/* 0x08c2: i2c_bitr */
+	0x37f000f8,
+	0x7b21f501,
+	0xe8e7f107,
+	0x7f21f403,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xcd21f550,
+	0x0464b607,
+	0xf51b11f4,
+	0xf007b521,
+	0x21f50037,
+	0xe7f10759,
 	0x21f41388,
-	0x0037f07f,
-	0x076421f5,
-	0x1388e7f1,
-/* 0x08cb: i2c_bitw_out */
-	0xf87f21f4,
-/* 0x08cd: i2c_bitr */
-	0x0137f000,
-	0x078621f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x07d821f5,
-	0xf40464b6,
-	0x21f51b11,
-	0x37f007c0,
-	0x6421f500,
-	0x88e7f107,
-	0x7f21f413,
-	0xf4013cf0,
-/* 0x0912: i2c_bitr_done */
-	0x00f80131,
-/* 0x0914: i2c_get_byte */
-	0xf00057f0,
-/* 0x091a: i2c_get_byte_next */
-	0x54b60847,
+	0x013cf07f,
+/* 0x0907: i2c_bitr_done */
+	0xf80131f4,
+/* 0x0909: i2c_get_byte */
+	0x0057f000,
+/* 0x090f: i2c_get_byte_next */
+	0xb60847f0,
+	0x76bb0154,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb608c221,
+	0x11f40464,
+	0x0553fd2b,
+	0xf40142b6,
+	0x37f0d81b,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b608cd,
-	0x2b11f404,
-	0xb60553fd,
-	0x1bf40142,
-	0x0137f0d8,
+	0x64b60881,
+/* 0x0959: i2c_get_byte_done */
+/* 0x095b: i2c_put_byte */
+	0xf000f804,
+/* 0x095e: i2c_put_byte_next */
+	0x42b60847,
+	0x3854ff01,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x8c21f550,
+	0x8121f550,
 	0x0464b608,
-/* 0x0964: i2c_get_byte_done */
-/* 0x0966: i2c_put_byte */
-	0x47f000f8,
-/* 0x0969: i2c_put_byte_next */
-	0x0142b608,
-	0xbb3854ff,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x088c21f5,
-	0xf40464b6,
-	0x46b03411,
-	0xd81bf400,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xcd21f550,
-	0x0464b608,
-	0xbb0f11f4,
-	0x36b00076,
-	0x061bf401,
-/* 0x09bf: i2c_put_byte_done */
-	0xf80132f4,
-/* 0x09c1: i2c_addr */
-	0x0076bb00,
+	0xb03411f4,
+	0x1bf40046,
+	0x0076bbd8,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b607fd,
-	0x2911f404,
-	0x012ec3e7,
-	0xfd0134b6,
-	0x76bb0553,
+	0x64b608c2,
+	0x0f11f404,
+	0xb00076bb,
+	0x1bf40136,
+	0x0132f406,
+/* 0x09b4: i2c_put_byte_done */
+/* 0x09b6: i2c_addr */
+	0x76bb00f8,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6096621,
-/* 0x0a06: i2c_addr_done */
-	0x00f80464,
-/* 0x0a08: i2c_acquire_addr */
-	0xb6f8cec7,
-	0xe0b702e4,
-	0xee980d1c,
-/* 0x0a17: i2c_acquire */
-	0xf500f800,
-	0xf40a0821,
-	0xd9f00421,
-	0x3f21f403,
-/* 0x0a26: i2c_release */
-	0x21f500f8,
-	0x21f40a08,
-	0x03daf004,
-	0xf83f21f4,
-/* 0x0a35: i2c_recv */
-	0x0132f400,
-	0xb6f8c1c7,
-	0x16b00214,
-	0x3a1ff528,
-	0xf413a001,
-	0x0032980c,
-	0x0ccc13a0,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
+	0xb607f221,
+	0x11f40464,
+	0x2ec3e729,
+	0x0134b601,
+	0xbb0553fd,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0a1721f5,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b31bf5,
-	0xbb0057f0,
+	0x095b21f5,
+/* 0x09fb: i2c_addr_done */
+	0xf80464b6,
+/* 0x09fd: i2c_acquire_addr */
+	0xf8cec700,
+	0xb702e4b6,
+	0x980d1ce0,
+	0x00f800ee,
+/* 0x0a0c: i2c_acquire */
+	0x09fd21f5,
+	0xf00421f4,
+	0x21f403d9,
+/* 0x0a1b: i2c_release */
+	0xf500f83f,
+	0xf409fd21,
+	0xdaf00421,
+	0x3f21f403,
+/* 0x0a2a: i2c_recv */
+	0x32f400f8,
+	0xf8c1c701,
+	0xb00214b6,
+	0x1ff52816,
+	0x13a0013a,
+	0x32980cf4,
+	0xcc13a000,
+	0x0031980c,
+	0xf90231f4,
+	0xf9e0f9d0,
+	0x0067f1d0,
+	0x0063f100,
+	0x01679210,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x0c21f550,
+	0x0464b60a,
+	0xd6b0d0fc,
+	0xb31bf500,
+	0x0057f000,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xb621f550,
+	0x0464b609,
+	0x00d011f5,
+	0xbbe0c5c7,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x09c121f5,
+	0x095b21f5,
 	0xf50464b6,
-	0xc700d011,
-	0x76bbe0c5,
+	0xf000ad11,
+	0x76bb0157,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6096621,
+	0xb609b621,
 	0x11f50464,
-	0x57f000ad,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b609c1,
-	0x8a11f504,
-	0x0076bb00,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b60914,
-	0x6a11f404,
-	0xbbe05bcb,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x085921f5,
-	0xb90464b6,
-	0x74bd025b,
-/* 0x0b3b: i2c_recv_not_rd08 */
-	0xb0430ef4,
-	0x1bf401d6,
-	0x0057f03d,
-	0x09c121f5,
-	0xc73311f4,
-	0x21f5e0c5,
-	0x11f40966,
-	0x0057f029,
-	0x09c121f5,
-	0xc71f11f4,
-	0x21f5e0b5,
-	0x11f40966,
-	0x5921f515,
-	0xc774bd08,
-	0x1bf408c5,
-	0x0232f409,
-/* 0x0b7b: i2c_recv_not_wr08 */
-/* 0x0b7b: i2c_recv_done */
-	0xc7030ef4,
-	0x21f5f8ce,
-	0xe0fc0a26,
-	0x12f4d0fc,
-	0x027cb90a,
-	0x034221f5,
-/* 0x0b90: i2c_recv_exit */
-/* 0x0b92: i2c_init */
+	0x76bb008a,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6090921,
+	0x11f40464,
+	0xe05bcb6a,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x4e21f550,
+	0x0464b608,
+	0xbd025bb9,
+	0x430ef474,
+/* 0x0b30: i2c_recv_not_rd08 */
+	0xf401d6b0,
+	0x57f03d1b,
+	0xb621f500,
+	0x3311f409,
+	0xf5e0c5c7,
+	0xf4095b21,
+	0x57f02911,
+	0xb621f500,
+	0x1f11f409,
+	0xf5e0b5c7,
+	0xf4095b21,
+	0x21f51511,
+	0x74bd084e,
+	0xf408c5c7,
+	0x32f4091b,
+	0x030ef402,
+/* 0x0b70: i2c_recv_not_wr08 */
+/* 0x0b70: i2c_recv_done */
+	0xf5f8cec7,
+	0xfc0a1b21,
+	0xf4d0fce0,
+	0x7cb90a12,
+	0x3721f502,
+/* 0x0b85: i2c_recv_exit */
+/* 0x0b87: i2c_init */
+	0xf800f803,
+/* 0x0b89: test_recv */
+	0xd817f100,
+	0x0614b605,
+	0xb60011cf,
+	0x07f10110,
+	0x04b605d8,
+	0x0001d006,
+	0xe7f104bd,
+	0xe3f1d900,
+	0x21f5134f,
+	0x00f80257,
+/* 0x0bb0: test_init */
+	0x0800e7f1,
+	0x025721f5,
+/* 0x0bba: idle_recv */
 	0x00f800f8,
-/* 0x0b94: test_recv */
-	0x05d817f1,
-	0xcf0614b6,
-	0x10b60011,
-	0xd807f101,
-	0x0604b605,
-	0xbd0001d0,
-	0x00e7f104,
-	0x4fe3f1d9,
-	0x6221f513,
-/* 0x0bbb: test_init */
-	0xf100f802,
-	0xf50800e7,
-	0xf8026221,
-/* 0x0bc5: idle_recv */
-/* 0x0bc7: idle */
-	0xf400f800,
-	0x17f10031,
-	0x14b605d4,
-	0x0011cf06,
-	0xf10110b6,
-	0xb605d407,
-	0x01d00604,
-/* 0x0be3: idle_loop */
-	0xf004bd00,
-	0x32f45817,
-/* 0x0be9: idle_proc */
-/* 0x0be9: idle_proc_exec */
-	0xb910f902,
-	0x21f5021e,
-	0x10fc034b,
-	0xf40911f4,
-	0x0ef40231,
-/* 0x0bfd: idle_proc_next */
-	0x5810b6ef,
-	0xf4061fb8,
-	0x02f4e61b,
-	0x0028f4dd,
-	0x00bb0ef4,
+/* 0x0bbc: idle */
+	0xf10031f4,
+	0xb605d417,
+	0x11cf0614,
+	0x0110b600,
+	0x05d407f1,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x0bd8: idle_loop */
+	0xf45817f0,
+/* 0x0bde: idle_proc */
+/* 0x0bde: idle_proc_exec */
+	0x10f90232,
+	0xf5021eb9,
+	0xfc034021,
+	0x0911f410,
+	0xf40231f4,
+/* 0x0bf2: idle_proc_next */
+	0x10b6ef0e,
+	0x061fb858,
+	0xf4e61bf4,
+	0x28f4dd02,
+	0xbb0ef400,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf119.fuc4.h b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf119.fuc4.h
index 31552af9b06e..7bf6b39ed205 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf119.fuc4.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gf119.fuc4.h
@@ -24,8 +24,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x0000049d,
-	0x00000446,
+	0x00000492,
+	0x0000043b,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x0000068b,
-	0x0000067d,
+	0x00000680,
+	0x00000672,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x0000068f,
-	0x0000068d,
+	0x00000684,
+	0x00000682,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000aaa,
-	0x0000094d,
+	0x00000a9f,
+	0x00000942,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000acd,
-	0x00000aac,
+	0x00000ac2,
+	0x00000aa1,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t gf119_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000ad9,
-	0x00000ad7,
+	0x00000ace,
+	0x00000acc,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -229,26 +229,26 @@ uint32_t gf119_pmu_data[] = {
 /* 0x0370: memx_func_head */
 	0x00000001,
 	0x00000000,
-	0x000004d3,
+	0x000004c8,
 /* 0x037c: memx_func_next */
 	0x00000002,
 	0x00000000,
-	0x00000554,
+	0x00000549,
 	0x00000003,
 	0x00000002,
-	0x000005d8,
+	0x000005cd,
 	0x00040004,
 	0x00000000,
-	0x000005f4,
+	0x000005e9,
 	0x00010005,
 	0x00000000,
-	0x0000060e,
+	0x00000603,
 	0x00010006,
 	0x00000000,
-	0x000005d3,
+	0x000005c8,
 	0x00000007,
 	0x00000000,
-	0x00000619,
+	0x0000060e,
 /* 0x03c4: memx_func_tail */
 /* 0x03c4: memx_ts_start */
 	0x00000000,
@@ -916,7 +916,7 @@ uint32_t gf119_pmu_data[] = {
 };
 
 uint32_t gf119_pmu_code[] = {
-	0x034d0ef5,
+	0x03420ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xbd000ed0,
@@ -977,7 +977,7 @@ uint32_t gf119_pmu_code[] = {
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x028c21f5,
+	0x028121f5,
 	0x0ef494bd,
 /* 0x00d5: intr_watchdog_next_time */
 	0x9b0a9815,
@@ -1025,716 +1025,714 @@ uint32_t gf119_pmu_code[] = {
 	0xf14f48e7,
 	0xf05453e3,
 	0x21f500d7,
-	0xc0fc02f1,
+	0xc0fc02e6,
 	0x04c007f1,
 	0xbd000cd0,
 /* 0x0185: intr_subintr_skip_fifo */
 	0x8807f104,
 	0x0009d006,
 /* 0x018e: intr_skip_subintr */
-	0x89c404bd,
-	0x070bf420,
-	0xffbfa4f1,
-/* 0x0198: intr_skip_pause */
-	0xf44089c4,
-	0xa4f1070b,
-/* 0x01a2: intr_skip_user0 */
-	0x07f0ffbf,
-	0x0008d004,
-	0x80fc04bd,
-	0xfc0088fe,
-	0xfce0fcf0,
-	0xfcc0fcd0,
-	0xfca0fcb0,
-	0xfc80fc90,
-	0x0032f400,
-/* 0x01c6: ticks_from_ns */
-	0xc0f901f8,
-	0xd7f1b0f9,
-	0xd3f00144,
-	0xb321f500,
-	0xe8ccec03,
-	0x00b4b003,
-	0xec120bf4,
-	0xf103e8ee,
-	0xf00144d7,
-	0x21f500d3,
-/* 0x01ee: ticks_from_ns_quit */
-	0xceb903b3,
-	0xfcb0fc02,
-/* 0x01f7: ticks_from_us */
-	0xf900f8c0,
+	0x97f104bd,
+	0x90bd00e0,
+	0xf00489fd,
+	0x08d00407,
+	0xfc04bd00,
+	0x0088fe80,
+	0xe0fcf0fc,
+	0xc0fcd0fc,
+	0xa0fcb0fc,
+	0x80fc90fc,
+	0x32f400fc,
+/* 0x01bb: ticks_from_ns */
+	0xf901f800,
 	0xf1b0f9c0,
 	0xf00144d7,
 	0x21f500d3,
-	0xceb903b3,
-	0x00b4b002,
-	0xbd050bf4,
-/* 0x0211: ticks_from_us_quit */
-	0xfcb0fce4,
-/* 0x0217: ticks_to_us */
-	0xf100f8c0,
-	0xf00144d7,
-	0xedff00d3,
-/* 0x0223: timer */
-	0xf900f8ec,
-	0xf480f990,
-	0xf8981032,
-	0x0086b003,
-	0xbd531cf4,
-	0x3807f084,
-	0xbd0008d0,
-	0x3487f004,
-	0x980088cf,
-	0x98bb9a09,
-	0x00e9bb02,
-	0xf003fe80,
-	0x88cf0887,
-	0x0284f000,
-	0xf0201bf4,
-	0x88cf3487,
-	0x06e0b800,
-	0xb8090bf4,
-	0x1cf406e8,
-/* 0x026d: timer_reset */
-	0x3407f00e,
-	0xbd000ed0,
-	0x9a0e8004,
-/* 0x0278: timer_enable */
-	0xf00187f0,
-	0x08d03807,
-/* 0x0283: timer_done */
-	0xf404bd00,
-	0x80fc1031,
-	0x00f890fc,
-/* 0x028c: send_proc */
-	0x90f980f9,
-	0x9805e898,
-	0x86f004e9,
-	0x0689b804,
-	0xc42a0bf4,
-	0x88940398,
-	0x1880b604,
-	0x98008ebb,
-	0x8a8000fa,
-	0x018d8000,
-	0x80028c80,
-	0x90b6038b,
-	0x0794f001,
-	0xf404e980,
-/* 0x02c6: send_done */
-	0x90fc0231,
-	0x00f880fc,
-/* 0x02cc: find */
-	0x87f080f9,
-	0x0131f458,
-/* 0x02d4: find_loop */
-	0xb8008a98,
-	0x0bf406ae,
-	0x5880b610,
-	0x026886b1,
-	0xf4f01bf4,
-/* 0x02ea: find_done */
-	0x8eb90132,
-	0xf880fc02,
-/* 0x02f1: send */
-	0xcc21f500,
-	0x9701f402,
-/* 0x02fa: recv */
-	0x90f900f8,
-	0xe89880f9,
-	0x04e99805,
-	0xb80132f4,
-	0x0bf40689,
-	0x0389c43d,
-	0xf00180b6,
-	0xe8800784,
-	0x02ea9805,
-	0x8ffef0f9,
-	0xb9f0f901,
-	0x999402ef,
-	0x00e9bb04,
-	0x9818e0b6,
-	0xec9803eb,
-	0x01ed9802,
-	0xf900ee98,
-	0xfef0fca5,
-	0x31f400f8,
-/* 0x0347: recv_done */
-	0xfcf0fc01,
+	0xccec03a8,
+	0xb4b003e8,
+	0x120bf400,
+	0x03e8eeec,
+	0x0144d7f1,
+	0xf500d3f0,
+/* 0x01e3: ticks_from_ns_quit */
+	0xb903a821,
+	0xb0fc02ce,
+	0x00f8c0fc,
+/* 0x01ec: ticks_from_us */
+	0xb0f9c0f9,
+	0x0144d7f1,
+	0xf500d3f0,
+	0xb903a821,
+	0xb4b002ce,
+	0x050bf400,
+/* 0x0206: ticks_from_us_quit */
+	0xb0fce4bd,
+	0x00f8c0fc,
+/* 0x020c: ticks_to_us */
+	0x0144d7f1,
+	0xff00d3f0,
+	0x00f8eced,
+/* 0x0218: timer */
+	0x80f990f9,
+	0x981032f4,
+	0x86b003f8,
+	0x531cf400,
+	0x07f084bd,
+	0x0008d038,
+	0x87f004bd,
+	0x0088cf34,
+	0xbb9a0998,
+	0xe9bb0298,
+	0x03fe8000,
+	0xcf0887f0,
+	0x84f00088,
+	0x201bf402,
+	0xcf3487f0,
+	0xe0b80088,
+	0x090bf406,
+	0xf406e8b8,
+/* 0x0262: timer_reset */
+	0x07f00e1c,
+	0x000ed034,
+	0x0e8004bd,
+/* 0x026d: timer_enable */
+	0x0187f09a,
+	0xd03807f0,
+	0x04bd0008,
+/* 0x0278: timer_done */
+	0xfc1031f4,
 	0xf890fc80,
-/* 0x034d: init */
-	0x0817f100,
-	0x0011cf01,
-	0x010911e7,
-	0xfe0814b6,
-	0x17f10014,
-	0x13f000e0,
-	0x1c07f000,
+/* 0x0281: send_proc */
+	0xf980f900,
+	0x05e89890,
+	0xf004e998,
+	0x89b80486,
+	0x2a0bf406,
+	0x940398c4,
+	0x80b60488,
+	0x008ebb18,
+	0x8000fa98,
+	0x8d80008a,
+	0x028c8001,
+	0xb6038b80,
+	0x94f00190,
+	0x04e98007,
+/* 0x02bb: send_done */
+	0xfc0231f4,
+	0xf880fc90,
+/* 0x02c1: find */
+	0xf080f900,
+	0x31f45887,
+/* 0x02c9: find_loop */
+	0x008a9801,
+	0xf406aeb8,
+	0x80b6100b,
+	0x6886b158,
+	0xf01bf402,
+/* 0x02df: find_done */
+	0xb90132f4,
+	0x80fc028e,
+/* 0x02e6: send */
+	0x21f500f8,
+	0x01f402c1,
+/* 0x02ef: recv */
+	0xf900f897,
+	0x9880f990,
+	0xe99805e8,
+	0x0132f404,
+	0xf40689b8,
+	0x89c43d0b,
+	0x0180b603,
+	0x800784f0,
+	0xea9805e8,
+	0xfef0f902,
+	0xf0f9018f,
+	0x9402efb9,
+	0xe9bb0499,
+	0x18e0b600,
+	0x9803eb98,
+	0xed9802ec,
+	0x00ee9801,
+	0xf0fca5f9,
+	0xf400f8fe,
+	0xf0fc0131,
+/* 0x033c: recv_done */
+	0x90fc80fc,
+/* 0x0342: init */
+	0x17f100f8,
+	0x11cf0108,
+	0x0911e700,
+	0x0814b601,
+	0xf10014fe,
+	0xf000e017,
+	0x07f00013,
+	0x0001d01c,
+	0x17f004bd,
+	0x1407f0ff,
 	0xbd0001d0,
-	0xff17f004,
-	0xd01407f0,
+	0x0217f004,
+	0x080015f1,
+	0xd01007f0,
 	0x04bd0001,
-	0xf10217f0,
-	0xf0080015,
-	0x01d01007,
-	0xf104bd00,
-	0xf000f617,
-	0x10fe0013,
-	0x1031f400,
-	0xf00117f0,
-	0x01d03807,
-	0xf004bd00,
-/* 0x03a2: init_proc */
-	0xf19858f7,
-	0x0016b001,
-	0xf9fa0bf4,
-	0x58f0b615,
-/* 0x03b3: mulu32_32_64 */
-	0xf9f20ef4,
-	0xf920f910,
-	0x9540f930,
-	0xd29510e1,
-	0xbdc4bd10,
-	0xc0edffb4,
-	0xb9301dff,
-	0x34f10234,
-	0x34b6ffff,
-	0x1045b610,
-	0xbb00c3bb,
-	0xe2ff01b4,
-	0x0234b930,
-	0xffff34f1,
-	0xb61034b6,
-	0xc3bb1045,
-	0x01b4bb00,
-	0xbb3012ff,
-	0x40fc00b3,
-	0x20fc30fc,
-	0x00f810fc,
-/* 0x0404: host_send */
-	0x04b017f1,
-	0xf10011cf,
-	0xcf04a027,
-	0x12b80022,
-	0x2f0bf406,
-	0x94071ec4,
-	0xe0b704ee,
-	0xeb980270,
-	0x02ec9803,
-	0x9801ed98,
-	0x21f500ee,
-	0x10b602f1,
-	0x0f1ec401,
-	0x04b007f1,
-	0xbd000ed0,
-	0xc30ef404,
-/* 0x0444: host_send_done */
-/* 0x0446: host_recv */
-	0x17f100f8,
-	0x13f14e49,
-	0xe1b85254,
-	0xb30bf406,
-/* 0x0454: host_recv_wait */
-	0x04cc17f1,
-	0xf10011cf,
-	0xcf04c827,
-	0x16f00022,
-	0x0612b808,
-	0xc4ec0bf4,
-	0x34b60723,
-	0xf030b704,
-	0x033b8002,
-	0x80023c80,
-	0x3e80013d,
-	0x0120b600,
-	0xf10f24f0,
-	0xd004c807,
+	0x00f617f1,
+	0xfe0013f0,
+	0x31f40010,
+	0x0117f010,
+	0xd03807f0,
+	0x04bd0001,
+/* 0x0397: init_proc */
+	0x9858f7f0,
+	0x16b001f1,
+	0xfa0bf400,
+	0xf0b615f9,
+	0xf20ef458,
+/* 0x03a8: mulu32_32_64 */
+	0x20f910f9,
+	0x40f930f9,
+	0x9510e195,
+	0xc4bd10d2,
+	0xedffb4bd,
+	0x301dffc0,
+	0xf10234b9,
+	0xb6ffff34,
+	0x45b61034,
+	0x00c3bb10,
+	0xff01b4bb,
+	0x34b930e2,
+	0xff34f102,
+	0x1034b6ff,
+	0xbb1045b6,
+	0xb4bb00c3,
+	0x3012ff01,
+	0xfc00b3bb,
+	0xfc30fc40,
+	0xf810fc20,
+/* 0x03f9: host_send */
+	0xb017f100,
+	0x0011cf04,
+	0x04a027f1,
+	0xb80022cf,
+	0x0bf40612,
+	0x071ec42f,
+	0xb704ee94,
+	0x980270e0,
+	0xec9803eb,
+	0x01ed9802,
+	0xf500ee98,
+	0xb602e621,
+	0x1ec40110,
+	0xb007f10f,
+	0x000ed004,
+	0x0ef404bd,
+/* 0x0439: host_send_done */
+/* 0x043b: host_recv */
+	0xf100f8c3,
+	0xf14e4917,
+	0xb8525413,
+	0x0bf406e1,
+/* 0x0449: host_recv_wait */
+	0xcc17f1b3,
+	0x0011cf04,
+	0x04c827f1,
+	0xf00022cf,
+	0x12b80816,
+	0xec0bf406,
+	0xb60723c4,
+	0x30b70434,
+	0x3b8002f0,
+	0x023c8003,
+	0x80013d80,
+	0x20b6003e,
+	0x0f24f001,
+	0x04c807f1,
+	0xbd0002d0,
+	0x4027f004,
+	0xd00007f0,
 	0x04bd0002,
-	0xf04027f0,
-	0x02d00007,
-	0xf804bd00,
-/* 0x049d: host_init */
-	0x8017f100,
-	0x1014b600,
-	0x027015f1,
-	0x04d007f1,
-	0xbd0001d0,
-	0x8017f104,
-	0x1014b600,
-	0x02f015f1,
-	0x04dc07f1,
-	0xbd0001d0,
-	0x0117f004,
-	0x04c407f1,
-	0xbd0001d0,
-/* 0x04d3: memx_func_enter */
-	0xf100f804,
-	0xf1162067,
-	0xf1f55d77,
+/* 0x0492: host_init */
+	0x17f100f8,
+	0x14b60080,
+	0x7015f110,
+	0xd007f102,
+	0x0001d004,
+	0x17f104bd,
+	0x14b60080,
+	0xf015f110,
+	0xdc07f102,
+	0x0001d004,
+	0x17f004bd,
+	0xc407f101,
+	0x0001d004,
+	0x00f804bd,
+/* 0x04c8: memx_func_enter */
+	0x162067f1,
+	0xf55d77f1,
+	0xffff73f1,
+	0xf4026eb9,
+	0xd8b90421,
+	0x0487fd02,
+	0x80f960f9,
+	0xe0fcd0fc,
+	0xf13321f4,
+	0xf1fffe77,
 	0xb9ffff73,
 	0x21f4026e,
 	0x02d8b904,
 	0xf90487fd,
 	0xfc80f960,
 	0xf4e0fcd0,
-	0x77f13321,
-	0x73f1fffe,
-	0x6eb9ffff,
+	0x67f13321,
+	0x6eb926f0,
 	0x0421f402,
 	0xfd02d8b9,
 	0x60f90487,
 	0xd0fc80f9,
 	0x21f4e0fc,
-	0xf067f133,
-	0x026eb926,
-	0xb90421f4,
-	0x87fd02d8,
-	0xf960f904,
-	0xfcd0fc80,
-	0x3321f4e0,
-	0xf10467f0,
-	0xd007e007,
-	0x04bd0006,
-/* 0x053c: memx_func_enter_wait */
-	0x07c067f1,
-	0xf00066cf,
-	0x0bf40464,
-	0x2c67f0f6,
-	0x800066cf,
-	0x00f8f106,
-/* 0x0554: memx_func_leave */
-	0xcf2c67f0,
-	0x06800066,
-	0x0467f0f2,
-	0x07e407f1,
+	0x0467f033,
+	0x07e007f1,
 	0xbd0006d0,
-/* 0x0569: memx_func_leave_wait */
+/* 0x0531: memx_func_enter_wait */
 	0xc067f104,
 	0x0066cf07,
 	0xf40464f0,
-	0x67f1f61b,
-	0x77f126f0,
-	0x73f00001,
-	0x026eb900,
-	0xb90421f4,
-	0x87fd02d8,
-	0xf960f905,
-	0xfcd0fc80,
-	0x3321f4e0,
-	0x162067f1,
-	0xf4026eb9,
-	0xd8b90421,
-	0x0587fd02,
-	0x80f960f9,
-	0xe0fcd0fc,
-	0xf13321f4,
-	0xf00aa277,
+	0x67f0f60b,
+	0x0066cf2c,
+	0xf8f10680,
+/* 0x0549: memx_func_leave */
+	0x2c67f000,
+	0x800066cf,
+	0x67f0f206,
+	0xe407f104,
+	0x0006d007,
+/* 0x055e: memx_func_leave_wait */
+	0x67f104bd,
+	0x66cf07c0,
+	0x0464f000,
+	0xf1f61bf4,
+	0xf126f067,
+	0xf0000177,
 	0x6eb90073,
 	0x0421f402,
 	0xfd02d8b9,
 	0x60f90587,
 	0xd0fc80f9,
 	0x21f4e0fc,
-/* 0x05d3: memx_func_wait_vblank */
-	0xb600f833,
-	0x00f80410,
-/* 0x05d8: memx_func_wr32 */
-	0x98001698,
-	0x10b60115,
-	0xf960f908,
-	0xfcd0fc50,
+	0x2067f133,
+	0x026eb916,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f905,
+	0xfcd0fc80,
 	0x3321f4e0,
-	0xf40242b6,
-	0x00f8e91b,
-/* 0x05f4: memx_func_wait */
-	0xcf2c87f0,
-	0x1e980088,
-	0x011d9800,
-	0x98021c98,
-	0x10b6031b,
-	0x8621f410,
-/* 0x060e: memx_func_delay */
-	0x1e9800f8,
+	0x0aa277f1,
+	0xb90073f0,
+	0x21f4026e,
+	0x02d8b904,
+	0xf90587fd,
+	0xfc80f960,
+	0xf4e0fcd0,
+	0x00f83321,
+/* 0x05c8: memx_func_wait_vblank */
+	0xf80410b6,
+/* 0x05cd: memx_func_wr32 */
+	0x00169800,
+	0xb6011598,
+	0x60f90810,
+	0xd0fc50f9,
+	0x21f4e0fc,
+	0x0242b633,
+	0xf8e91bf4,
+/* 0x05e9: memx_func_wait */
+	0x2c87f000,
+	0x980088cf,
+	0x1d98001e,
+	0x021c9801,
+	0xb6031b98,
+	0x21f41010,
+/* 0x0603: memx_func_delay */
+	0x9800f886,
+	0x10b6001e,
+	0x6721f404,
+/* 0x060e: memx_func_train */
+	0x00f800f8,
+/* 0x0610: memx_exec */
+	0xd0f9e0f9,
+	0xb902c1b9,
+/* 0x061a: memx_exec_next */
+	0x139802b2,
 	0x0410b600,
-	0xf86721f4,
-/* 0x0619: memx_func_train */
-/* 0x061b: memx_exec */
-	0xf900f800,
-	0xb9d0f9e0,
-	0xb2b902c1,
-/* 0x0625: memx_exec_next */
-	0x00139802,
-	0xe70410b6,
-	0xe701f034,
-	0xb601e033,
-	0x30f00132,
-	0xde35980c,
-	0x12b855f9,
-	0xe41ef406,
-	0x98f10b98,
-	0xcbbbf20c,
-	0xc4b7f102,
-	0x00bbcf07,
-	0xe0fcd0fc,
-	0x02f121f5,
-/* 0x065e: memx_info */
-	0xc67000f8,
-	0x0e0bf401,
-/* 0x0664: memx_info_data */
-	0x03ccc7f1,
-	0x0800b7f1,
-/* 0x066f: memx_info_train */
-	0xf10b0ef4,
-	0xf10bccc7,
-/* 0x0677: memx_info_send */
-	0xf50100b7,
-	0xf802f121,
-/* 0x067d: memx_recv */
-	0x01d6b000,
-	0xb09b0bf4,
-	0x0bf400d6,
-/* 0x068b: memx_init */
-	0xf800f8d8,
-/* 0x068d: perf_recv */
-/* 0x068f: perf_init */
-	0xf800f800,
-/* 0x0691: i2c_drive_scl */
-	0x0036b000,
-	0xf10e0bf4,
-	0xd007e007,
+	0x01f034e7,
+	0x01e033e7,
+	0xf00132b6,
+	0x35980c30,
+	0xb855f9de,
+	0x1ef40612,
+	0xf10b98e4,
+	0xbbf20c98,
+	0xb7f102cb,
+	0xbbcf07c4,
+	0xfcd0fc00,
+	0xe621f5e0,
+/* 0x0653: memx_info */
+	0x7000f802,
+	0x0bf401c6,
+/* 0x0659: memx_info_data */
+	0xccc7f10e,
+	0x00b7f103,
+	0x0b0ef408,
+/* 0x0664: memx_info_train */
+	0x0bccc7f1,
+	0x0100b7f1,
+/* 0x066c: memx_info_send */
+	0x02e621f5,
+/* 0x0672: memx_recv */
+	0xd6b000f8,
+	0x9b0bf401,
+	0xf400d6b0,
+	0x00f8d80b,
+/* 0x0680: memx_init */
+/* 0x0682: perf_recv */
+	0x00f800f8,
+/* 0x0684: perf_init */
+/* 0x0686: i2c_drive_scl */
+	0x36b000f8,
+	0x0e0bf400,
+	0x07e007f1,
+	0xbd0001d0,
+/* 0x0697: i2c_drive_scl_lo */
+	0xf100f804,
+	0xd007e407,
 	0x04bd0001,
-/* 0x06a2: i2c_drive_scl_lo */
-	0x07f100f8,
-	0x01d007e4,
-	0xf804bd00,
-/* 0x06ad: i2c_drive_sda */
-	0x0036b000,
-	0xf10e0bf4,
-	0xd007e007,
+/* 0x06a2: i2c_drive_sda */
+	0x36b000f8,
+	0x0e0bf400,
+	0x07e007f1,
+	0xbd0002d0,
+/* 0x06b3: i2c_drive_sda_lo */
+	0xf100f804,
+	0xd007e407,
 	0x04bd0002,
-/* 0x06be: i2c_drive_sda_lo */
-	0x07f100f8,
-	0x02d007e4,
-	0xf804bd00,
-/* 0x06c9: i2c_sense_scl */
-	0x0132f400,
-	0x07c437f1,
-	0xfd0033cf,
-	0x0bf40431,
-	0x0131f406,
-/* 0x06dc: i2c_sense_scl_done */
-/* 0x06de: i2c_sense_sda */
+/* 0x06be: i2c_sense_scl */
 	0x32f400f8,
 	0xc437f101,
 	0x0033cf07,
-	0xf40432fd,
+	0xf40431fd,
 	0x31f4060b,
-/* 0x06f1: i2c_sense_sda_done */
-/* 0x06f3: i2c_raise_scl */
-	0xf900f801,
-	0x9847f140,
-	0x0137f008,
-	0x069121f5,
-/* 0x0700: i2c_raise_scl_wait */
-	0x03e8e7f1,
-	0xf56721f4,
-	0xf406c921,
-	0x42b60901,
-	0xef1bf401,
-/* 0x0714: i2c_raise_scl_done */
-	0x00f840fc,
-/* 0x0718: i2c_start */
-	0x06c921f5,
-	0xf50d11f4,
-	0xf406de21,
-	0x0ef40611,
-/* 0x0729: i2c_start_rep */
-	0x0037f030,
-	0x069121f5,
+/* 0x06d1: i2c_sense_scl_done */
+/* 0x06d3: i2c_sense_sda */
+	0xf400f801,
+	0x37f10132,
+	0x33cf07c4,
+	0x0432fd00,
+	0xf4060bf4,
+/* 0x06e6: i2c_sense_sda_done */
+	0x00f80131,
+/* 0x06e8: i2c_raise_scl */
+	0x47f140f9,
+	0x37f00898,
+	0x8621f501,
+/* 0x06f5: i2c_raise_scl_wait */
+	0xe8e7f106,
+	0x6721f403,
+	0x06be21f5,
+	0xb60901f4,
+	0x1bf40142,
+/* 0x0709: i2c_raise_scl_done */
+	0xf840fcef,
+/* 0x070d: i2c_start */
+	0xbe21f500,
+	0x0d11f406,
+	0x06d321f5,
+	0xf40611f4,
+/* 0x071e: i2c_start_rep */
+	0x37f0300e,
+	0x8621f500,
+	0x0137f006,
+	0x06a221f5,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xe821f550,
+	0x0464b606,
+/* 0x074b: i2c_start_send */
+	0xf01f11f4,
+	0x21f50037,
+	0xe7f106a2,
+	0x21f41388,
+	0x0037f067,
+	0x068621f5,
+	0x1388e7f1,
+/* 0x0767: i2c_start_out */
+	0xf86721f4,
+/* 0x0769: i2c_stop */
+	0x0037f000,
+	0x068621f5,
+	0xf50037f0,
+	0xf106a221,
+	0xf403e8e7,
+	0x37f06721,
+	0x8621f501,
+	0x88e7f106,
+	0x6721f413,
 	0xf50137f0,
-	0xbb06ad21,
+	0xf106a221,
+	0xf41388e7,
+	0x00f86721,
+/* 0x079c: i2c_bitw */
+	0x06a221f5,
+	0x03e8e7f1,
+	0xbb6721f4,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x06f321f5,
+	0x06e821f5,
 	0xf40464b6,
-/* 0x0756: i2c_start_send */
-	0x37f01f11,
-	0xad21f500,
-	0x88e7f106,
-	0x6721f413,
-	0xf50037f0,
-	0xf1069121,
-	0xf41388e7,
-/* 0x0772: i2c_start_out */
-	0x00f86721,
-/* 0x0774: i2c_stop */
-	0xf50037f0,
-	0xf0069121,
-	0x21f50037,
-	0xe7f106ad,
-	0x21f403e8,
-	0x0137f067,
-	0x069121f5,
-	0x1388e7f1,
-	0xf06721f4,
-	0x21f50137,
-	0xe7f106ad,
+	0xe7f11811,
 	0x21f41388,
-/* 0x07a7: i2c_bitw */
-	0xf500f867,
-	0xf106ad21,
-	0xf403e8e7,
-	0x76bb6721,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb606f321,
-	0x11f40464,
-	0x88e7f118,
-	0x6721f413,
-	0xf50037f0,
-	0xf1069121,
-	0xf41388e7,
-/* 0x07e6: i2c_bitw_out */
-	0x00f86721,
-/* 0x07e8: i2c_bitr */
-	0xf50137f0,
-	0xf106ad21,
-	0xf403e8e7,
-	0x76bb6721,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb606f321,
-	0x11f40464,
-	0xde21f51b,
-	0x0037f006,
-	0x069121f5,
+	0x0037f067,
+	0x068621f5,
 	0x1388e7f1,
-	0xf06721f4,
-	0x31f4013c,
-/* 0x082d: i2c_bitr_done */
-/* 0x082f: i2c_get_byte */
-	0xf000f801,
-	0x47f00057,
-/* 0x0835: i2c_get_byte_next */
-	0x0154b608,
+/* 0x07db: i2c_bitw_out */
+	0xf86721f4,
+/* 0x07dd: i2c_bitr */
+	0x0137f000,
+	0x06a221f5,
+	0x03e8e7f1,
+	0xbb6721f4,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x06e821f5,
+	0xf40464b6,
+	0x21f51b11,
+	0x37f006d3,
+	0x8621f500,
+	0x88e7f106,
+	0x6721f413,
+	0xf4013cf0,
+/* 0x0822: i2c_bitr_done */
+	0x00f80131,
+/* 0x0824: i2c_get_byte */
+	0xf00057f0,
+/* 0x082a: i2c_get_byte_next */
+	0x54b60847,
+	0x0076bb01,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b607dd,
+	0x2b11f404,
+	0xb60553fd,
+	0x1bf40142,
+	0x0137f0d8,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xe821f550,
+	0x9c21f550,
 	0x0464b607,
-	0xfd2b11f4,
-	0x42b60553,
-	0xd81bf401,
-	0xbb0137f0,
+/* 0x0874: i2c_get_byte_done */
+/* 0x0876: i2c_put_byte */
+	0x47f000f8,
+/* 0x0879: i2c_put_byte_next */
+	0x0142b608,
+	0xbb3854ff,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x07a721f5,
-/* 0x087f: i2c_get_byte_done */
-	0xf80464b6,
-/* 0x0881: i2c_put_byte */
-	0x0847f000,
-/* 0x0884: i2c_put_byte_next */
-	0xff0142b6,
-	0x76bb3854,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb607a721,
-	0x11f40464,
-	0x0046b034,
-	0xbbd81bf4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x07e821f5,
+	0x079c21f5,
 	0xf40464b6,
-	0x76bb0f11,
-	0x0136b000,
-	0xf4061bf4,
-/* 0x08da: i2c_put_byte_done */
-	0x00f80132,
-/* 0x08dc: i2c_addr */
+	0x46b03411,
+	0xd81bf400,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x1821f550,
+	0xdd21f550,
 	0x0464b607,
-	0xe72911f4,
-	0xb6012ec3,
-	0x53fd0134,
-	0x0076bb05,
+	0xbb0f11f4,
+	0x36b00076,
+	0x061bf401,
+/* 0x08cf: i2c_put_byte_done */
+	0xf80132f4,
+/* 0x08d1: i2c_addr */
+	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60881,
-/* 0x0921: i2c_addr_done */
-/* 0x0923: i2c_acquire_addr */
-	0xc700f804,
-	0xe4b6f8ce,
-	0x14e0b705,
-/* 0x092f: i2c_acquire */
-	0xf500f8d0,
-	0xf4092321,
-	0xd9f00421,
+	0x64b6070d,
+	0x2911f404,
+	0x012ec3e7,
+	0xfd0134b6,
+	0x76bb0553,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6087621,
+/* 0x0916: i2c_addr_done */
+	0x00f80464,
+/* 0x0918: i2c_acquire_addr */
+	0xb6f8cec7,
+	0xe0b705e4,
+	0x00f8d014,
+/* 0x0924: i2c_acquire */
+	0x091821f5,
+	0xf00421f4,
+	0x21f403d9,
+/* 0x0933: i2c_release */
+	0xf500f833,
+	0xf4091821,
+	0xdaf00421,
 	0x3321f403,
-/* 0x093e: i2c_release */
-	0x21f500f8,
-	0x21f40923,
-	0x03daf004,
-	0xf83321f4,
-/* 0x094d: i2c_recv */
-	0x0132f400,
-	0xb6f8c1c7,
-	0x16b00214,
-	0x3a1ff528,
-	0xf413a001,
-	0x0032980c,
-	0x0ccc13a0,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x092f21f5,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b31bf5,
-	0xbb0057f0,
+/* 0x0942: i2c_recv */
+	0x32f400f8,
+	0xf8c1c701,
+	0xb00214b6,
+	0x1ff52816,
+	0x13a0013a,
+	0x32980cf4,
+	0xcc13a000,
+	0x0031980c,
+	0xf90231f4,
+	0xf9e0f9d0,
+	0x0067f1d0,
+	0x0063f100,
+	0x01679210,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x2421f550,
+	0x0464b609,
+	0xd6b0d0fc,
+	0xb31bf500,
+	0x0057f000,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xd121f550,
+	0x0464b608,
+	0x00d011f5,
+	0xbbe0c5c7,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x08dc21f5,
+	0x087621f5,
 	0xf50464b6,
-	0xc700d011,
-	0x76bbe0c5,
+	0xf000ad11,
+	0x76bb0157,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6088121,
+	0xb608d121,
 	0x11f50464,
-	0x57f000ad,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b608dc,
-	0x8a11f504,
-	0x0076bb00,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b6082f,
-	0x6a11f404,
-	0xbbe05bcb,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x077421f5,
-	0xb90464b6,
-	0x74bd025b,
-/* 0x0a53: i2c_recv_not_rd08 */
-	0xb0430ef4,
-	0x1bf401d6,
-	0x0057f03d,
-	0x08dc21f5,
-	0xc73311f4,
-	0x21f5e0c5,
-	0x11f40881,
-	0x0057f029,
-	0x08dc21f5,
-	0xc71f11f4,
-	0x21f5e0b5,
-	0x11f40881,
-	0x7421f515,
-	0xc774bd07,
-	0x1bf408c5,
-	0x0232f409,
-/* 0x0a93: i2c_recv_not_wr08 */
-/* 0x0a93: i2c_recv_done */
-	0xc7030ef4,
-	0x21f5f8ce,
-	0xe0fc093e,
-	0x12f4d0fc,
-	0x027cb90a,
-	0x02f121f5,
-/* 0x0aa8: i2c_recv_exit */
-/* 0x0aaa: i2c_init */
-	0x00f800f8,
-/* 0x0aac: test_recv */
-	0x05d817f1,
-	0xb60011cf,
-	0x07f10110,
-	0x01d005d8,
-	0xf104bd00,
-	0xf1d900e7,
-	0xf5134fe3,
-	0xf8022321,
-/* 0x0acd: test_init */
-	0x00e7f100,
-	0x2321f508,
-/* 0x0ad7: idle_recv */
+	0x76bb008a,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6082421,
+	0x11f40464,
+	0xe05bcb6a,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x6921f550,
+	0x0464b607,
+	0xbd025bb9,
+	0x430ef474,
+/* 0x0a48: i2c_recv_not_rd08 */
+	0xf401d6b0,
+	0x57f03d1b,
+	0xd121f500,
+	0x3311f408,
+	0xf5e0c5c7,
+	0xf4087621,
+	0x57f02911,
+	0xd121f500,
+	0x1f11f408,
+	0xf5e0b5c7,
+	0xf4087621,
+	0x21f51511,
+	0x74bd0769,
+	0xf408c5c7,
+	0x32f4091b,
+	0x030ef402,
+/* 0x0a88: i2c_recv_not_wr08 */
+/* 0x0a88: i2c_recv_done */
+	0xf5f8cec7,
+	0xfc093321,
+	0xf4d0fce0,
+	0x7cb90a12,
+	0xe621f502,
+/* 0x0a9d: i2c_recv_exit */
+/* 0x0a9f: i2c_init */
 	0xf800f802,
-/* 0x0ad9: idle */
-	0x0031f400,
-	0x05d417f1,
-	0xb60011cf,
-	0x07f10110,
-	0x01d005d4,
-/* 0x0aef: idle_loop */
-	0xf004bd00,
-	0x32f45817,
-/* 0x0af5: idle_proc */
-/* 0x0af5: idle_proc_exec */
-	0xb910f902,
-	0x21f5021e,
-	0x10fc02fa,
-	0xf40911f4,
-	0x0ef40231,
-/* 0x0b09: idle_proc_next */
-	0x5810b6ef,
-	0xf4061fb8,
-	0x02f4e61b,
-	0x0028f4dd,
-	0x00c10ef4,
+/* 0x0aa1: test_recv */
+	0xd817f100,
+	0x0011cf05,
+	0xf10110b6,
+	0xd005d807,
+	0x04bd0001,
+	0xd900e7f1,
+	0x134fe3f1,
+	0x021821f5,
+/* 0x0ac2: test_init */
+	0xe7f100f8,
+	0x21f50800,
+	0x00f80218,
+/* 0x0acc: idle_recv */
+/* 0x0ace: idle */
+	0x31f400f8,
+	0xd417f100,
+	0x0011cf05,
+	0xf10110b6,
+	0xd005d407,
+	0x04bd0001,
+/* 0x0ae4: idle_loop */
+	0xf45817f0,
+/* 0x0aea: idle_proc */
+/* 0x0aea: idle_proc_exec */
+	0x10f90232,
+	0xf5021eb9,
+	0xfc02ef21,
+	0x0911f410,
+	0xf40231f4,
+/* 0x0afe: idle_proc_next */
+	0x10b6ef0e,
+	0x061fb858,
+	0xf4e61bf4,
+	0x28f4dd02,
+	0xc10ef400,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gk208.fuc5.h b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gk208.fuc5.h
index fe4f63deeaab..8a2b628642ac 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gk208.fuc5.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gk208.fuc5.h
@@ -24,8 +24,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000453,
-	0x00000404,
+	0x00000447,
+	0x000003f8,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x0000062d,
-	0x0000061f,
+	0x00000621,
+	0x00000613,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000631,
-	0x0000062f,
+	0x00000625,
+	0x00000623,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000a35,
-	0x000008dc,
+	0x00000a29,
+	0x000008d0,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000a56,
-	0x00000a37,
+	0x00000a4a,
+	0x00000a2b,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t gk208_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000a61,
-	0x00000a5f,
+	0x00000a55,
+	0x00000a53,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -229,26 +229,26 @@ uint32_t gk208_pmu_data[] = {
 /* 0x0370: memx_func_head */
 	0x00000001,
 	0x00000000,
-	0x00000483,
+	0x00000477,
 /* 0x037c: memx_func_next */
 	0x00000002,
 	0x00000000,
-	0x00000500,
+	0x000004f4,
 	0x00000003,
 	0x00000002,
-	0x00000580,
+	0x00000574,
 	0x00040004,
 	0x00000000,
-	0x0000059d,
+	0x00000591,
 	0x00010005,
 	0x00000000,
-	0x000005b7,
+	0x000005ab,
 	0x00010006,
 	0x00000000,
-	0x0000057b,
+	0x0000056f,
 	0x00000007,
 	0x00000000,
-	0x000005c3,
+	0x000005b7,
 /* 0x03c4: memx_func_tail */
 /* 0x03c4: memx_ts_start */
 	0x00000000,
@@ -916,7 +916,7 @@ uint32_t gk208_pmu_data[] = {
 };
 
 uint32_t gk208_pmu_code[] = {
-	0x031c0ef5,
+	0x03100ef5,
 /* 0x0004: rd32 */
 	0xf607a040,
 	0x04bd000e,
@@ -972,7 +972,7 @@ uint32_t gk208_pmu_code[] = {
 	0x0a98280b,
 	0x029abb9a,
 	0x0d0e1cf4,
-	0x02617e01,
+	0x02557e01,
 	0xf494bd00,
 /* 0x00c2: intr_watchdog_next_time */
 	0x0a98140e,
@@ -1017,21 +1017,16 @@ uint32_t gk208_pmu_code[] = {
 	0xc0f900cc,
 	0xf14f484e,
 	0x0d5453e3,
-	0x02c27e00,
+	0x02b67e00,
 	0x40c0fc00,
 	0x0cf604c0,
 /* 0x0167: intr_subintr_skip_fifo */
 	0x4004bd00,
 	0x09f60688,
 /* 0x016f: intr_skip_subintr */
-	0xc404bd00,
-	0x0bf42089,
-	0xbfa4f107,
-/* 0x0179: intr_skip_pause */
-	0x4089c4ff,
-	0xf1070bf4,
-/* 0x0183: intr_skip_user0 */
-	0x00ffbfa4,
+	0x4904bd00,
+	0x90bd00e0,
+	0x000489fd,
 	0x0008f604,
 	0x80fc04bd,
 	0xfc0088fe,
@@ -1040,35 +1035,35 @@ uint32_t gk208_pmu_code[] = {
 	0xfca0fcb0,
 	0xfc80fc90,
 	0x0032f400,
-/* 0x01a6: ticks_from_ns */
+/* 0x019a: ticks_from_ns */
 	0xc0f901f8,
 	0xd7f1b0f9,
 	0xd3f00144,
-	0x7721f500,
+	0x6b21f500,
 	0xe8ccec03,
 	0x00b4b003,
 	0xec120bf4,
 	0xf103e8ee,
 	0xf00144d7,
 	0x21f500d3,
-/* 0x01ce: ticks_from_ns_quit */
-	0xceb20377,
+/* 0x01c2: ticks_from_ns_quit */
+	0xceb2036b,
 	0xc0fcb0fc,
-/* 0x01d6: ticks_from_us */
+/* 0x01ca: ticks_from_us */
 	0xc0f900f8,
 	0xd7f1b0f9,
 	0xd3f00144,
-	0x7721f500,
+	0x6b21f500,
 	0xb0ceb203,
 	0x0bf400b4,
-/* 0x01ef: ticks_from_us_quit */
+/* 0x01e3: ticks_from_us_quit */
 	0xfce4bd05,
 	0xf8c0fcb0,
-/* 0x01f5: ticks_to_us */
+/* 0x01e9: ticks_to_us */
 	0x44d7f100,
 	0x00d3f001,
 	0xf8ecedff,
-/* 0x0201: timer */
+/* 0x01f5: timer */
 	0xf990f900,
 	0x1032f480,
 	0xb003f898,
@@ -1086,17 +1081,17 @@ uint32_t gk208_pmu_code[] = {
 	0xa60088cf,
 	0x080bf4e0,
 	0x1cf4e8a6,
-/* 0x0245: timer_reset */
+/* 0x0239: timer_reset */
 	0xf634000d,
 	0x04bd000e,
-/* 0x024f: timer_enable */
+/* 0x0243: timer_enable */
 	0x089a0eb5,
 	0xf6380001,
 	0x04bd0008,
-/* 0x0258: timer_done */
+/* 0x024c: timer_done */
 	0xfc1031f4,
 	0xf890fc80,
-/* 0x0261: send_proc */
+/* 0x0255: send_proc */
 	0xf980f900,
 	0x05e89890,
 	0xf004e998,
@@ -1111,24 +1106,24 @@ uint32_t gk208_pmu_code[] = {
 	0x90b6038b,
 	0x0794f001,
 	0xf404e9b5,
-/* 0x029a: send_done */
+/* 0x028e: send_done */
 	0x90fc0231,
 	0x00f880fc,
-/* 0x02a0: find */
+/* 0x0294: find */
 	0x580880f9,
-/* 0x02a7: find_loop */
+/* 0x029b: find_loop */
 	0x980131f4,
 	0xaea6008a,
 	0xb6100bf4,
 	0x86b15880,
 	0x1bf40268,
 	0x0132f4f1,
-/* 0x02bc: find_done */
+/* 0x02b0: find_done */
 	0x80fc8eb2,
-/* 0x02c2: send */
-	0xa07e00f8,
+/* 0x02b6: send */
+	0x947e00f8,
 	0x01f40002,
-/* 0x02cb: recv */
+/* 0x02bf: recv */
 	0xf900f89b,
 	0x9880f990,
 	0xe99805e8,
@@ -1148,10 +1143,10 @@ uint32_t gk208_pmu_code[] = {
 	0xa5f900ee,
 	0xf8fef0fc,
 	0x0131f400,
-/* 0x0316: recv_done */
+/* 0x030a: recv_done */
 	0x80fcf0fc,
 	0x00f890fc,
-/* 0x031c: init */
+/* 0x0310: init */
 	0xcf010841,
 	0x11e70011,
 	0x14b60109,
@@ -1170,12 +1165,12 @@ uint32_t gk208_pmu_code[] = {
 	0x011031f4,
 	0xf6380001,
 	0x04bd0001,
-/* 0x0366: init_proc */
+/* 0x035a: init_proc */
 	0xf198580f,
 	0x0016b001,
 	0xf9fa0bf4,
 	0x58f0b615,
-/* 0x0377: mulu32_32_64 */
+/* 0x036b: mulu32_32_64 */
 	0xf9f20ef4,
 	0xf920f910,
 	0x9540f930,
@@ -1196,7 +1191,7 @@ uint32_t gk208_pmu_code[] = {
 	0x00b3bb30,
 	0x30fc40fc,
 	0x10fc20fc,
-/* 0x03c6: host_send */
+/* 0x03ba: host_send */
 	0xb04100f8,
 	0x0011cf04,
 	0xcf04a042,
@@ -1207,18 +1202,18 @@ uint32_t gk208_pmu_code[] = {
 	0x03eb9802,
 	0x9802ec98,
 	0xee9801ed,
-	0x02c27e00,
+	0x02b67e00,
 	0x0110b600,
 	0x400f1ec4,
 	0x0ef604b0,
 	0xf404bd00,
-/* 0x0402: host_send_done */
+/* 0x03f6: host_send_done */
 	0x00f8c70e,
-/* 0x0404: host_recv */
+/* 0x03f8: host_recv */
 	0xf14e4941,
 	0xa6525413,
 	0xb90bf4e1,
-/* 0x0410: host_recv_wait */
+/* 0x0404: host_recv_wait */
 	0xcf04cc41,
 	0xc8420011,
 	0x0022cf04,
@@ -1235,7 +1230,7 @@ uint32_t gk208_pmu_code[] = {
 	0x04bd0002,
 	0x00004002,
 	0xbd0002f6,
-/* 0x0453: host_init */
+/* 0x0447: host_init */
 	0x4100f804,
 	0x14b60080,
 	0x7015f110,
@@ -1248,7 +1243,7 @@ uint32_t gk208_pmu_code[] = {
 	0x0104bd00,
 	0x04c44001,
 	0xbd0001f6,
-/* 0x0483: memx_func_enter */
+/* 0x0477: memx_func_enter */
 	0xf100f804,
 	0xf1162067,
 	0xf1f55d77,
@@ -1275,19 +1270,19 @@ uint32_t gk208_pmu_code[] = {
 	0x00002e7e,
 	0xe0400406,
 	0x0006f607,
-/* 0x04ea: memx_func_enter_wait */
+/* 0x04de: memx_func_enter_wait */
 	0xc04604bd,
 	0x0066cf07,
 	0xf40464f0,
 	0x2c06f70b,
 	0xb50066cf,
 	0x00f8f106,
-/* 0x0500: memx_func_leave */
+/* 0x04f4: memx_func_leave */
 	0x66cf2c06,
 	0xf206b500,
 	0xe4400406,
 	0x0006f607,
-/* 0x0512: memx_func_leave_wait */
+/* 0x0506: memx_func_leave_wait */
 	0xc04604bd,
 	0x0066cf07,
 	0xf40464f0,
@@ -1314,10 +1309,10 @@ uint32_t gk208_pmu_code[] = {
 	0xf960f905,
 	0xfcd0fc80,
 	0x002e7ee0,
-/* 0x057b: memx_func_wait_vblank */
+/* 0x056f: memx_func_wait_vblank */
 	0xb600f800,
 	0x00f80410,
-/* 0x0580: memx_func_wr32 */
+/* 0x0574: memx_func_wr32 */
 	0x98001698,
 	0x10b60115,
 	0xf960f908,
@@ -1325,23 +1320,23 @@ uint32_t gk208_pmu_code[] = {
 	0x002e7ee0,
 	0x0242b600,
 	0xf8e81bf4,
-/* 0x059d: memx_func_wait */
+/* 0x0591: memx_func_wait */
 	0xcf2c0800,
 	0x1e980088,
 	0x011d9800,
 	0x98021c98,
 	0x10b6031b,
 	0x00797e10,
-/* 0x05b7: memx_func_delay */
+/* 0x05ab: memx_func_delay */
 	0x9800f800,
 	0x10b6001e,
 	0x005d7e04,
-/* 0x05c3: memx_func_train */
+/* 0x05b7: memx_func_train */
 	0xf800f800,
-/* 0x05c5: memx_exec */
+/* 0x05b9: memx_exec */
 	0xf9e0f900,
 	0xb2c1b2d0,
-/* 0x05cd: memx_exec_next */
+/* 0x05c1: memx_exec_next */
 	0x001398b2,
 	0xe70410b6,
 	0xe701f034,
@@ -1354,111 +1349,111 @@ uint32_t gk208_pmu_code[] = {
 	0x02cbbbf2,
 	0xcf07c44b,
 	0xd0fc00bb,
-	0xc27ee0fc,
+	0xb67ee0fc,
 	0x00f80002,
-/* 0x0604: memx_info */
+/* 0x05f8: memx_info */
 	0xf401c670,
-/* 0x060a: memx_info_data */
+/* 0x05fe: memx_info_data */
 	0xcc4c0c0b,
 	0x08004b03,
-/* 0x0613: memx_info_train */
+/* 0x0607: memx_info_train */
 	0x4c090ef4,
 	0x004b0bcc,
-/* 0x0619: memx_info_send */
-	0x02c27e01,
-/* 0x061f: memx_recv */
+/* 0x060d: memx_info_send */
+	0x02b67e01,
+/* 0x0613: memx_recv */
 	0xb000f800,
 	0x0bf401d6,
 	0x00d6b0a3,
 	0xf8dc0bf4,
-/* 0x062d: memx_init */
-/* 0x062f: perf_recv */
+/* 0x0621: memx_init */
+/* 0x0623: perf_recv */
 	0xf800f800,
-/* 0x0631: perf_init */
-/* 0x0633: i2c_drive_scl */
+/* 0x0625: perf_init */
+/* 0x0627: i2c_drive_scl */
 	0xb000f800,
 	0x0bf40036,
 	0x07e0400d,
 	0xbd0001f6,
-/* 0x0643: i2c_drive_scl_lo */
+/* 0x0637: i2c_drive_scl_lo */
 	0x4000f804,
 	0x01f607e4,
 	0xf804bd00,
-/* 0x064d: i2c_drive_sda */
+/* 0x0641: i2c_drive_sda */
 	0x0036b000,
 	0x400d0bf4,
 	0x02f607e0,
 	0xf804bd00,
-/* 0x065d: i2c_drive_sda_lo */
+/* 0x0651: i2c_drive_sda_lo */
 	0x07e44000,
 	0xbd0002f6,
-/* 0x0667: i2c_sense_scl */
+/* 0x065b: i2c_sense_scl */
 	0xf400f804,
 	0xc4430132,
 	0x0033cf07,
 	0xf40431fd,
 	0x31f4060b,
-/* 0x0679: i2c_sense_scl_done */
-/* 0x067b: i2c_sense_sda */
+/* 0x066d: i2c_sense_scl_done */
+/* 0x066f: i2c_sense_sda */
 	0xf400f801,
 	0xc4430132,
 	0x0033cf07,
 	0xf40432fd,
 	0x31f4060b,
-/* 0x068d: i2c_sense_sda_done */
-/* 0x068f: i2c_raise_scl */
+/* 0x0681: i2c_sense_sda_done */
+/* 0x0683: i2c_raise_scl */
 	0xf900f801,
 	0x08984440,
-	0x337e0103,
-/* 0x069a: i2c_raise_scl_wait */
+	0x277e0103,
+/* 0x068e: i2c_raise_scl_wait */
 	0xe84e0006,
 	0x005d7e03,
-	0x06677e00,
+	0x065b7e00,
 	0x0901f400,
 	0xf40142b6,
-/* 0x06ae: i2c_raise_scl_done */
+/* 0x06a2: i2c_raise_scl_done */
 	0x40fcef1b,
-/* 0x06b2: i2c_start */
-	0x677e00f8,
+/* 0x06a6: i2c_start */
+	0x5b7e00f8,
 	0x11f40006,
-	0x067b7e0d,
+	0x066f7e0d,
 	0x0611f400,
-/* 0x06c3: i2c_start_rep */
+/* 0x06b7: i2c_start_rep */
 	0x032e0ef4,
-	0x06337e00,
+	0x06277e00,
 	0x7e010300,
-	0xbb00064d,
+	0xbb000641,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x00068f7e,
+	0x0006837e,
 	0xf40464b6,
-/* 0x06ee: i2c_start_send */
+/* 0x06e2: i2c_start_send */
 	0x00031d11,
-	0x00064d7e,
+	0x0006417e,
 	0x7e13884e,
 	0x0300005d,
-	0x06337e00,
+	0x06277e00,
 	0x13884e00,
 	0x00005d7e,
-/* 0x0708: i2c_start_out */
-/* 0x070a: i2c_stop */
+/* 0x06fc: i2c_start_out */
+/* 0x06fe: i2c_stop */
 	0x000300f8,
-	0x0006337e,
-	0x4d7e0003,
+	0x0006277e,
+	0x417e0003,
 	0xe84e0006,
 	0x005d7e03,
 	0x7e010300,
-	0x4e000633,
+	0x4e000627,
 	0x5d7e1388,
 	0x01030000,
-	0x00064d7e,
+	0x0006417e,
 	0x7e13884e,
 	0xf800005d,
-/* 0x0739: i2c_bitw */
-	0x064d7e00,
+/* 0x072d: i2c_bitw */
+	0x06417e00,
 	0x03e84e00,
 	0x00005d7e,
 	0xb60076bb,
@@ -1466,18 +1461,18 @@ uint32_t gk208_pmu_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x068f7e50,
+	0x06837e50,
 	0x0464b600,
 	0x4e1711f4,
 	0x5d7e1388,
 	0x00030000,
-	0x0006337e,
+	0x0006277e,
 	0x7e13884e,
-/* 0x0777: i2c_bitw_out */
+/* 0x076b: i2c_bitw_out */
 	0xf800005d,
-/* 0x0779: i2c_bitr */
+/* 0x076d: i2c_bitr */
 	0x7e010300,
-	0x4e00064d,
+	0x4e000641,
 	0x5d7e03e8,
 	0x76bb0000,
 	0x0465b600,
@@ -1485,25 +1480,25 @@ uint32_t gk208_pmu_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb600068f,
+	0xb6000683,
 	0x11f40464,
-	0x067b7e1a,
+	0x066f7e1a,
 	0x7e000300,
-	0x4e000633,
+	0x4e000627,
 	0x5d7e1388,
 	0x3cf00000,
 	0x0131f401,
-/* 0x07bc: i2c_bitr_done */
-/* 0x07be: i2c_get_byte */
+/* 0x07b0: i2c_bitr_done */
+/* 0x07b2: i2c_get_byte */
 	0x000500f8,
-/* 0x07c2: i2c_get_byte_next */
+/* 0x07b6: i2c_get_byte_next */
 	0x54b60804,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0x797e50fc,
+	0x6d7e50fc,
 	0x64b60007,
 	0x2a11f404,
 	0xb60553fd,
@@ -1514,11 +1509,11 @@ uint32_t gk208_pmu_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0007397e,
-/* 0x080b: i2c_get_byte_done */
+	0x00072d7e,
+/* 0x07ff: i2c_get_byte_done */
 	0xf80464b6,
-/* 0x080d: i2c_put_byte */
-/* 0x080f: i2c_put_byte_next */
+/* 0x0801: i2c_put_byte */
+/* 0x0803: i2c_put_byte_next */
 	0xb6080400,
 	0x54ff0142,
 	0x0076bb38,
@@ -1526,7 +1521,7 @@ uint32_t gk208_pmu_code[] = {
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0x397e50fc,
+	0x2d7e50fc,
 	0x64b60007,
 	0x3411f404,
 	0xf40046b0,
@@ -1536,20 +1531,20 @@ uint32_t gk208_pmu_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb6000779,
+	0xb600076d,
 	0x11f40464,
 	0x0076bb0f,
 	0xf40136b0,
 	0x32f4061b,
-/* 0x0865: i2c_put_byte_done */
-/* 0x0867: i2c_addr */
+/* 0x0859: i2c_put_byte_done */
+/* 0x085b: i2c_addr */
 	0xbb00f801,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0006b27e,
+	0x0006a67e,
 	0xf40464b6,
 	0xc3e72911,
 	0x34b6012e,
@@ -1559,25 +1554,25 @@ uint32_t gk208_pmu_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x080d7e50,
+	0x08017e50,
 	0x0464b600,
-/* 0x08ac: i2c_addr_done */
-/* 0x08ae: i2c_acquire_addr */
+/* 0x08a0: i2c_addr_done */
+/* 0x08a2: i2c_acquire_addr */
 	0xcec700f8,
 	0x05e4b6f8,
 	0xd014e0b7,
-/* 0x08ba: i2c_acquire */
-	0xae7e00f8,
+/* 0x08ae: i2c_acquire */
+	0xa27e00f8,
 	0x047e0008,
 	0xd9f00000,
 	0x002e7e03,
-/* 0x08cb: i2c_release */
+/* 0x08bf: i2c_release */
 	0x7e00f800,
-	0x7e0008ae,
+	0x7e0008a2,
 	0xf0000004,
 	0x2e7e03da,
 	0x00f80000,
-/* 0x08dc: i2c_recv */
+/* 0x08d0: i2c_recv */
 	0xc70132f4,
 	0x14b6f8c1,
 	0x2816b002,
@@ -1596,7 +1591,7 @@ uint32_t gk208_pmu_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x08ba7e50,
+	0x08ae7e50,
 	0x0464b600,
 	0xd6b0d0fc,
 	0xb01bf500,
@@ -1606,7 +1601,7 @@ uint32_t gk208_pmu_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0008677e,
+	0x00085b7e,
 	0xf50464b6,
 	0xc700cc11,
 	0x76bbe0c5,
@@ -1615,7 +1610,7 @@ uint32_t gk208_pmu_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb600080d,
+	0xb6000801,
 	0x11f50464,
 	0x010500a9,
 	0xb60076bb,
@@ -1623,7 +1618,7 @@ uint32_t gk208_pmu_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x08677e50,
+	0x085b7e50,
 	0x0464b600,
 	0x008711f5,
 	0xb60076bb,
@@ -1631,7 +1626,7 @@ uint32_t gk208_pmu_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x07be7e50,
+	0x07b27e50,
 	0x0464b600,
 	0xcb6711f4,
 	0x76bbe05b,
@@ -1640,36 +1635,36 @@ uint32_t gk208_pmu_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb600070a,
+	0xb60006fe,
 	0x5bb20464,
 	0x0ef474bd,
-/* 0x09e1: i2c_recv_not_rd08 */
+/* 0x09d5: i2c_recv_not_rd08 */
 	0x01d6b041,
 	0x053b1bf4,
-	0x08677e00,
+	0x085b7e00,
 	0x3211f400,
 	0x7ee0c5c7,
-	0xf400080d,
+	0xf4000801,
 	0x00052811,
-	0x0008677e,
+	0x00085b7e,
 	0xc71f11f4,
-	0x0d7ee0b5,
+	0x017ee0b5,
 	0x11f40008,
-	0x070a7e15,
+	0x06fe7e15,
 	0xc774bd00,
 	0x1bf408c5,
 	0x0232f409,
-/* 0x0a1f: i2c_recv_not_wr08 */
-/* 0x0a1f: i2c_recv_done */
+/* 0x0a13: i2c_recv_not_wr08 */
+/* 0x0a13: i2c_recv_done */
 	0xc7030ef4,
-	0xcb7ef8ce,
+	0xbf7ef8ce,
 	0xe0fc0008,
 	0x12f4d0fc,
 	0x7e7cb209,
-/* 0x0a33: i2c_recv_exit */
-	0xf80002c2,
-/* 0x0a35: i2c_init */
-/* 0x0a37: test_recv */
+/* 0x0a27: i2c_recv_exit */
+	0xf80002b6,
+/* 0x0a29: i2c_init */
+/* 0x0a2b: test_recv */
 	0x4100f800,
 	0x11cf0458,
 	0x0110b600,
@@ -1677,28 +1672,28 @@ uint32_t gk208_pmu_code[] = {
 	0x04bd0001,
 	0xd900e7f1,
 	0x134fe3f1,
-	0x0002017e,
-/* 0x0a56: test_init */
+	0x0001f57e,
+/* 0x0a4a: test_init */
 	0x004e00f8,
-	0x02017e08,
-/* 0x0a5f: idle_recv */
+	0x01f57e08,
+/* 0x0a53: idle_recv */
 	0xf800f800,
-/* 0x0a61: idle */
+/* 0x0a55: idle */
 	0x0031f400,
 	0xcf045441,
 	0x10b60011,
 	0x04544001,
 	0xbd0001f6,
-/* 0x0a75: idle_loop */
+/* 0x0a69: idle_loop */
 	0xf4580104,
-/* 0x0a7a: idle_proc */
-/* 0x0a7a: idle_proc_exec */
+/* 0x0a6e: idle_proc */
+/* 0x0a6e: idle_proc_exec */
 	0x10f90232,
-	0xcb7e1eb2,
+	0xbf7e1eb2,
 	0x10fc0002,
 	0xf40911f4,
 	0x0ef40231,
-/* 0x0a8d: idle_proc_next */
+/* 0x0a81: idle_proc_next */
 	0x5810b6f0,
 	0x1bf41fa6,
 	0xe002f4e8,
@@ -1728,4 +1723,7 @@ uint32_t gk208_pmu_code[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 };
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gt215.fuc3.h b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gt215.fuc3.h
index 2686f8fad0f5..516569270bac 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gt215.fuc3.h
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/gt215.fuc3.h
@@ -24,8 +24,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000512,
-	0x000004af,
+	0x00000507,
+	0x000004a4,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x00000842,
-	0x00000834,
+	0x00000837,
+	0x00000829,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000846,
-	0x00000844,
+	0x0000083b,
+	0x00000839,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000c76,
-	0x00000b19,
+	0x00000c6b,
+	0x00000b0e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x00000c9f,
-	0x00000c78,
+	0x00000c94,
+	0x00000c6d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t gt215_pmu_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000cab,
-	0x00000ca9,
+	0x00000ca0,
+	0x00000c9e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -229,26 +229,26 @@ uint32_t gt215_pmu_data[] = {
 /* 0x0370: memx_func_head */
 	0x00000001,
 	0x00000000,
-	0x00000551,
+	0x00000546,
 /* 0x037c: memx_func_next */
 	0x00000002,
 	0x00000000,
-	0x000005a8,
+	0x0000059d,
 	0x00000003,
 	0x00000002,
-	0x0000063a,
+	0x0000062f,
 	0x00040004,
 	0x00000000,
-	0x00000656,
+	0x0000064b,
 	0x00010005,
 	0x00000000,
-	0x00000673,
+	0x00000668,
 	0x00010006,
 	0x00000000,
-	0x000005f8,
+	0x000005ed,
 	0x00000007,
 	0x00000000,
-	0x0000067e,
+	0x00000673,
 /* 0x03c4: memx_func_tail */
 /* 0x03c4: memx_ts_start */
 	0x00000000,
@@ -917,7 +917,7 @@ uint32_t gt215_pmu_data[] = {
 };
 
 uint32_t gt215_pmu_code[] = {
-	0x039e0ef5,
+	0x03930ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xd00604b6,
@@ -987,7 +987,7 @@ uint32_t gt215_pmu_code[] = {
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x02dd21f5,
+	0x02d221f5,
 	0x0ef494bd,
 /* 0x00f9: intr_watchdog_next_time */
 	0x9b0a9815,
@@ -1039,7 +1039,7 @@ uint32_t gt215_pmu_code[] = {
 	0x48e7f1c0,
 	0x53e3f14f,
 	0x00d7f054,
-	0x034221f5,
+	0x033721f5,
 	0x07f1c0fc,
 	0x04b604c0,
 	0x000cd006,
@@ -1048,820 +1048,818 @@ uint32_t gt215_pmu_code[] = {
 	0x04b60688,
 	0x0009d006,
 /* 0x01ca: intr_skip_subintr */
-	0x89c404bd,
-	0x070bf420,
-	0xffbfa4f1,
-/* 0x01d4: intr_skip_pause */
-	0xf44089c4,
-	0xa4f1070b,
-/* 0x01de: intr_skip_user0 */
-	0x07f0ffbf,
-	0x0604b604,
-	0xbd0008d0,
-	0xfe80fc04,
-	0xf0fc0088,
-	0xd0fce0fc,
-	0xb0fcc0fc,
-	0x90fca0fc,
-	0x00fc80fc,
-	0xf80032f4,
-/* 0x0205: ticks_from_ns */
-	0xf9c0f901,
-	0xcbd7f1b0,
-	0x00d3f000,
-	0x041321f5,
-	0x03e8ccec,
-	0xf400b4b0,
-	0xeeec120b,
-	0xd7f103e8,
-	0xd3f000cb,
-	0x1321f500,
-/* 0x022d: ticks_from_ns_quit */
-	0x02ceb904,
-	0xc0fcb0fc,
-/* 0x0236: ticks_from_us */
-	0xc0f900f8,
+	0x97f104bd,
+	0x90bd00e0,
+	0xf00489fd,
+	0x04b60407,
+	0x0008d006,
+	0x80fc04bd,
+	0xfc0088fe,
+	0xfce0fcf0,
+	0xfcc0fcd0,
+	0xfca0fcb0,
+	0xfc80fc90,
+	0x0032f400,
+/* 0x01fa: ticks_from_ns */
+	0xc0f901f8,
 	0xd7f1b0f9,
 	0xd3f000cb,
-	0x1321f500,
-	0x02ceb904,
-	0xf400b4b0,
-	0xe4bd050b,
-/* 0x0250: ticks_from_us_quit */
-	0xc0fcb0fc,
-/* 0x0256: ticks_to_us */
-	0xd7f100f8,
-	0xd3f000cb,
-	0xecedff00,
-/* 0x0262: timer */
-	0x90f900f8,
-	0x32f480f9,
-	0x03f89810,
-	0xf40086b0,
-	0x84bd651c,
-	0xb63807f0,
-	0x08d00604,
-	0xf004bd00,
-	0x84b63487,
-	0x0088cf06,
-	0xbb9a0998,
-	0xe9bb0298,
-	0x03fe8000,
-	0xb60887f0,
-	0x88cf0684,
-	0x0284f000,
-	0xf0261bf4,
-	0x84b63487,
-	0x0088cf06,
-	0xf406e0b8,
-	0xe8b8090b,
-	0x111cf406,
-/* 0x02b8: timer_reset */
-	0xb63407f0,
-	0x0ed00604,
-	0x8004bd00,
-/* 0x02c6: timer_enable */
-	0x87f09a0e,
-	0x3807f001,
+	0x0821f500,
+	0xe8ccec04,
+	0x00b4b003,
+	0xec120bf4,
+	0xf103e8ee,
+	0xf000cbd7,
+	0x21f500d3,
+/* 0x0222: ticks_from_ns_quit */
+	0xceb90408,
+	0xfcb0fc02,
+/* 0x022b: ticks_from_us */
+	0xf900f8c0,
+	0xf1b0f9c0,
+	0xf000cbd7,
+	0x21f500d3,
+	0xceb90408,
+	0x00b4b002,
+	0xbd050bf4,
+/* 0x0245: ticks_from_us_quit */
+	0xfcb0fce4,
+/* 0x024b: ticks_to_us */
+	0xf100f8c0,
+	0xf000cbd7,
+	0xedff00d3,
+/* 0x0257: timer */
+	0xf900f8ec,
+	0xf480f990,
+	0xf8981032,
+	0x0086b003,
+	0xbd651cf4,
+	0x3807f084,
 	0xd00604b6,
 	0x04bd0008,
-/* 0x02d4: timer_done */
-	0xfc1031f4,
-	0xf890fc80,
-/* 0x02dd: send_proc */
-	0xf980f900,
-	0x05e89890,
-	0xf004e998,
-	0x89b80486,
-	0x2a0bf406,
-	0x940398c4,
-	0x80b60488,
-	0x008ebb18,
-	0x8000fa98,
-	0x8d80008a,
-	0x028c8001,
-	0xb6038b80,
-	0x94f00190,
-	0x04e98007,
-/* 0x0317: send_done */
-	0xfc0231f4,
-	0xf880fc90,
-/* 0x031d: find */
-	0xf080f900,
-	0x31f45887,
-/* 0x0325: find_loop */
-	0x008a9801,
-	0xf406aeb8,
-	0x80b6100b,
-	0x6886b158,
-	0xf01bf402,
-/* 0x033b: find_done */
-	0xb90132f4,
-	0x80fc028e,
-/* 0x0342: send */
-	0x21f500f8,
-	0x01f4031d,
-/* 0x034b: recv */
-	0xf900f897,
-	0x9880f990,
-	0xe99805e8,
-	0x0132f404,
-	0xf40689b8,
-	0x89c43d0b,
-	0x0180b603,
-	0x800784f0,
-	0xea9805e8,
-	0xfef0f902,
-	0xf0f9018f,
-	0x9402efb9,
-	0xe9bb0499,
-	0x18e0b600,
-	0x9803eb98,
-	0xed9802ec,
-	0x00ee9801,
-	0xf0fca5f9,
-	0xf400f8fe,
-	0xf0fc0131,
-/* 0x0398: recv_done */
-	0x90fc80fc,
-/* 0x039e: init */
-	0x17f100f8,
-	0x14b60108,
-	0x0011cf06,
-	0x010911e7,
-	0xfe0814b6,
-	0x17f10014,
-	0x13f000e0,
-	0x1c07f000,
-	0xd00604b6,
-	0x04bd0001,
-	0xf0ff17f0,
-	0x04b61407,
-	0x0001d006,
-	0x17f004bd,
-	0x0015f102,
-	0x1007f008,
+	0xb63487f0,
+	0x88cf0684,
+	0x9a099800,
+	0xbb0298bb,
+	0xfe8000e9,
+	0x0887f003,
+	0xcf0684b6,
+	0x84f00088,
+	0x261bf402,
+	0xb63487f0,
+	0x88cf0684,
+	0x06e0b800,
+	0xb8090bf4,
+	0x1cf406e8,
+/* 0x02ad: timer_reset */
+	0x3407f011,
 	0xd00604b6,
-	0x04bd0001,
-	0x011a17f1,
-	0xfe0013f0,
-	0x31f40010,
-	0x0117f010,
-	0xb63807f0,
+	0x04bd000e,
+/* 0x02bb: timer_enable */
+	0xf09a0e80,
+	0x07f00187,
+	0x0604b638,
+	0xbd0008d0,
+/* 0x02c9: timer_done */
+	0x1031f404,
+	0x90fc80fc,
+/* 0x02d2: send_proc */
+	0x80f900f8,
+	0xe89890f9,
+	0x04e99805,
+	0xb80486f0,
+	0x0bf40689,
+	0x0398c42a,
+	0xb6048894,
+	0x8ebb1880,
+	0x00fa9800,
+	0x80008a80,
+	0x8c80018d,
+	0x038b8002,
+	0xf00190b6,
+	0xe9800794,
+	0x0231f404,
+/* 0x030c: send_done */
+	0x80fc90fc,
+/* 0x0312: find */
+	0x80f900f8,
+	0xf45887f0,
+/* 0x031a: find_loop */
+	0x8a980131,
+	0x06aeb800,
+	0xb6100bf4,
+	0x86b15880,
+	0x1bf40268,
+	0x0132f4f0,
+/* 0x0330: find_done */
+	0xfc028eb9,
+/* 0x0337: send */
+	0xf500f880,
+	0xf4031221,
+	0x00f89701,
+/* 0x0340: recv */
+	0x80f990f9,
+	0x9805e898,
+	0x32f404e9,
+	0x0689b801,
+	0xc43d0bf4,
+	0x80b60389,
+	0x0784f001,
+	0x9805e880,
+	0xf0f902ea,
+	0xf9018ffe,
+	0x02efb9f0,
+	0xbb049994,
+	0xe0b600e9,
+	0x03eb9818,
+	0x9802ec98,
+	0xee9801ed,
+	0xfca5f900,
+	0x00f8fef0,
+	0xfc0131f4,
+/* 0x038d: recv_done */
+	0xfc80fcf0,
+/* 0x0393: init */
+	0xf100f890,
+	0xb6010817,
+	0x11cf0614,
+	0x0911e700,
+	0x0814b601,
+	0xf10014fe,
+	0xf000e017,
+	0x07f00013,
+	0x0604b61c,
+	0xbd0001d0,
+	0xff17f004,
+	0xb61407f0,
 	0x01d00604,
 	0xf004bd00,
-/* 0x0402: init_proc */
-	0xf19858f7,
-	0x0016b001,
-	0xf9fa0bf4,
-	0x58f0b615,
-/* 0x0413: mulu32_32_64 */
-	0xf9f20ef4,
-	0xf920f910,
-	0x9540f930,
-	0xd29510e1,
-	0xbdc4bd10,
-	0xc0edffb4,
-	0xb9301dff,
-	0x34f10234,
-	0x34b6ffff,
-	0x1045b610,
-	0xbb00c3bb,
-	0xe2ff01b4,
-	0x0234b930,
-	0xffff34f1,
-	0xb61034b6,
-	0xc3bb1045,
-	0x01b4bb00,
-	0xbb3012ff,
-	0x40fc00b3,
-	0x20fc30fc,
-	0x00f810fc,
-/* 0x0464: host_send */
-	0x04b017f1,
-	0xcf0614b6,
-	0x27f10011,
-	0x24b604a0,
-	0x0022cf06,
-	0xf40612b8,
-	0x1ec4320b,
-	0x04ee9407,
-	0x0270e0b7,
-	0x9803eb98,
-	0xed9802ec,
-	0x00ee9801,
-	0x034221f5,
-	0xc40110b6,
-	0x07f10f1e,
-	0x04b604b0,
-	0x000ed006,
-	0x0ef404bd,
-/* 0x04ad: host_send_done */
-/* 0x04af: host_recv */
-	0xf100f8ba,
-	0xf14e4917,
-	0xb8525413,
-	0x0bf406e1,
-/* 0x04bd: host_recv_wait */
-	0xcc17f1aa,
+	0x15f10217,
+	0x07f00800,
+	0x0604b610,
+	0xbd0001d0,
+	0x1a17f104,
+	0x0013f001,
+	0xf40010fe,
+	0x17f01031,
+	0x3807f001,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x03f7: init_proc */
+	0x9858f7f0,
+	0x16b001f1,
+	0xfa0bf400,
+	0xf0b615f9,
+	0xf20ef458,
+/* 0x0408: mulu32_32_64 */
+	0x20f910f9,
+	0x40f930f9,
+	0x9510e195,
+	0xc4bd10d2,
+	0xedffb4bd,
+	0x301dffc0,
+	0xf10234b9,
+	0xb6ffff34,
+	0x45b61034,
+	0x00c3bb10,
+	0xff01b4bb,
+	0x34b930e2,
+	0xff34f102,
+	0x1034b6ff,
+	0xbb1045b6,
+	0xb4bb00c3,
+	0x3012ff01,
+	0xfc00b3bb,
+	0xfc30fc40,
+	0xf810fc20,
+/* 0x0459: host_send */
+	0xb017f100,
 	0x0614b604,
 	0xf10011cf,
-	0xb604c827,
+	0xb604a027,
 	0x22cf0624,
-	0x0816f000,
-	0xf40612b8,
-	0x23c4e60b,
-	0x0434b607,
-	0x02f030b7,
-	0x80033b80,
-	0x3d80023c,
-	0x003e8001,
-	0xf00120b6,
-	0x07f10f24,
-	0x04b604c8,
-	0x0002d006,
-	0x27f004bd,
-	0x0007f040,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x0512: host_init */
-	0x17f100f8,
+	0x0612b800,
+	0xc4320bf4,
+	0xee94071e,
+	0x70e0b704,
+	0x03eb9802,
+	0x9802ec98,
+	0xee9801ed,
+	0x3721f500,
+	0x0110b603,
+	0xf10f1ec4,
+	0xb604b007,
+	0x0ed00604,
+	0xf404bd00,
+/* 0x04a2: host_send_done */
+	0x00f8ba0e,
+/* 0x04a4: host_recv */
+	0x4e4917f1,
+	0x525413f1,
+	0xf406e1b8,
+/* 0x04b2: host_recv_wait */
+	0x17f1aa0b,
+	0x14b604cc,
+	0x0011cf06,
+	0x04c827f1,
+	0xcf0624b6,
+	0x16f00022,
+	0x0612b808,
+	0xc4e60bf4,
+	0x34b60723,
+	0xf030b704,
+	0x033b8002,
+	0x80023c80,
+	0x3e80013d,
+	0x0120b600,
+	0xf10f24f0,
+	0xb604c807,
+	0x02d00604,
+	0xf004bd00,
+	0x07f04027,
+	0x0604b600,
+	0xbd0002d0,
+/* 0x0507: host_init */
+	0xf100f804,
+	0xb6008017,
+	0x15f11014,
+	0x07f10270,
+	0x04b604d0,
+	0x0001d006,
+	0x17f104bd,
 	0x14b60080,
-	0x7015f110,
-	0xd007f102,
+	0xf015f110,
+	0xdc07f102,
 	0x0604b604,
 	0xbd0001d0,
-	0x8017f104,
-	0x1014b600,
-	0x02f015f1,
-	0x04dc07f1,
+	0x0117f004,
+	0x04c407f1,
 	0xd00604b6,
 	0x04bd0001,
-	0xf10117f0,
-	0xb604c407,
-	0x01d00604,
-	0xf804bd00,
-/* 0x0551: memx_func_enter */
-	0x1087f100,
-	0x028eb916,
-	0xb90421f4,
-	0x67f102d7,
-	0x63f1fffc,
-	0x76fdffff,
-	0x0267f104,
-	0x0576fd00,
-	0x70f980f9,
-	0xe0fcd0fc,
-	0xf03f21f4,
-	0x07f10467,
-	0x04b607e0,
-	0x0006d006,
-/* 0x058a: memx_func_enter_wait */
-	0x67f104bd,
-	0x64b607c0,
-	0x0066cf06,
-	0xf40464f0,
-	0x67f0f30b,
-	0x0664b62c,
-	0x800066cf,
-	0x00f8f106,
-/* 0x05a8: memx_func_leave */
-	0xb62c67f0,
-	0x66cf0664,
-	0xf2068000,
+/* 0x0546: memx_func_enter */
+	0x87f100f8,
+	0x8eb91610,
+	0x0421f402,
+	0xf102d7b9,
+	0xf1fffc67,
+	0xfdffff63,
+	0x67f10476,
+	0x76fd0002,
+	0xf980f905,
+	0xfcd0fc70,
+	0x3f21f4e0,
 	0xf10467f0,
-	0xb607e407,
+	0xb607e007,
 	0x06d00604,
-/* 0x05c3: memx_func_leave_wait */
+/* 0x057f: memx_func_enter_wait */
 	0xf104bd00,
 	0xb607c067,
 	0x66cf0664,
 	0x0464f000,
-	0xf1f31bf4,
-	0xb9161087,
-	0x21f4028e,
-	0x02d7b904,
-	0xffcc67f1,
-	0xffff63f1,
-	0xf90476fd,
-	0xfc70f980,
-	0xf4e0fcd0,
-	0x00f83f21,
-/* 0x05f8: memx_func_wait_vblank */
-	0xb0001698,
-	0x0bf40066,
-	0x0166b013,
-	0xf4060bf4,
-/* 0x060a: memx_func_wait_vblank_head1 */
-	0x77f12e0e,
-	0x0ef40020,
-/* 0x0611: memx_func_wait_vblank_head0 */
-	0x0877f107,
-/* 0x0615: memx_func_wait_vblank_0 */
-	0xc467f100,
-	0x0664b607,
-	0xfd0066cf,
-	0x1bf40467,
-/* 0x0625: memx_func_wait_vblank_1 */
-	0xc467f1f3,
-	0x0664b607,
-	0xfd0066cf,
-	0x0bf40467,
-/* 0x0635: memx_func_wait_vblank_fini */
-	0x0410b6f3,
-/* 0x063a: memx_func_wr32 */
-	0x169800f8,
-	0x01159800,
-	0xf90810b6,
-	0xfc50f960,
-	0xf4e0fcd0,
-	0x42b63f21,
-	0xe91bf402,
-/* 0x0656: memx_func_wait */
-	0x87f000f8,
-	0x0684b62c,
-	0x980088cf,
-	0x1d98001e,
-	0x021c9801,
-	0xb6031b98,
-	0x21f41010,
-/* 0x0673: memx_func_delay */
-	0x9800f8a4,
-	0x10b6001e,
-	0x7f21f404,
-/* 0x067e: memx_func_train */
-	0x57f100f8,
-	0x77f10003,
-	0x97f10000,
-	0x93f00000,
-	0x029eb970,
-	0xb90421f4,
-	0xe7f102d8,
-	0x21f42710,
-/* 0x069d: memx_func_train_loop_outer */
-	0x0158e07f,
-	0x0083f101,
-	0xe097f102,
-	0x1193f011,
-	0x80f990f9,
+	0xf0f30bf4,
+	0x64b62c67,
+	0x0066cf06,
+	0xf8f10680,
+/* 0x059d: memx_func_leave */
+	0x2c67f000,
+	0xcf0664b6,
+	0x06800066,
+	0x0467f0f2,
+	0x07e407f1,
+	0xd00604b6,
+	0x04bd0006,
+/* 0x05b8: memx_func_leave_wait */
+	0x07c067f1,
+	0xcf0664b6,
+	0x64f00066,
+	0xf31bf404,
+	0x161087f1,
+	0xf4028eb9,
+	0xd7b90421,
+	0xcc67f102,
+	0xff63f1ff,
+	0x0476fdff,
+	0x70f980f9,
 	0xe0fcd0fc,
-	0xf93f21f4,
-	0x0067f150,
-/* 0x06bd: memx_func_train_loop_inner */
-	0x1187f100,
-	0x9068ff11,
-	0xfd109894,
-	0x97f10589,
-	0x93f00720,
-	0xf990f910,
-	0xfcd0fc80,
-	0x3f21f4e0,
-	0x008097f1,
-	0xb91093f0,
-	0x21f4029e,
-	0x02d8b904,
-	0xf92088c5,
-	0xfc80f990,
-	0xf4e0fcd0,
-	0x97f13f21,
-	0x93f0053c,
-	0x0287f110,
-	0x0083f130,
-	0xf990f980,
-	0xfcd0fc80,
-	0x3f21f4e0,
-	0x0560e7f1,
-	0xf110e3f0,
-	0xf10000d7,
-	0x908000d3,
-	0xb7f100dc,
-	0xb3f08480,
-	0xa421f41e,
-	0x000057f1,
-	0xffff97f1,
-	0x830093f1,
-/* 0x073c: memx_func_train_loop_4x */
-	0x0080a7f1,
-	0xb910a3f0,
-	0x21f402ae,
-	0x02d8b904,
-	0xffdfb7f1,
-	0xffffb3f1,
-	0xf9048bfd,
-	0xfc80f9a0,
-	0xf4e0fcd0,
-	0xa7f13f21,
-	0xa3f0053c,
-	0x0287f110,
-	0x0083f130,
-	0xf9a0f980,
+	0xf83f21f4,
+/* 0x05ed: memx_func_wait_vblank */
+	0x00169800,
+	0xf40066b0,
+	0x66b0130b,
+	0x060bf401,
+/* 0x05ff: memx_func_wait_vblank_head1 */
+	0xf12e0ef4,
+	0xf4002077,
+/* 0x0606: memx_func_wait_vblank_head0 */
+	0x77f1070e,
+/* 0x060a: memx_func_wait_vblank_0 */
+	0x67f10008,
+	0x64b607c4,
+	0x0066cf06,
+	0xf40467fd,
+/* 0x061a: memx_func_wait_vblank_1 */
+	0x67f1f31b,
+	0x64b607c4,
+	0x0066cf06,
+	0xf40467fd,
+/* 0x062a: memx_func_wait_vblank_fini */
+	0x10b6f30b,
+/* 0x062f: memx_func_wr32 */
+	0x9800f804,
+	0x15980016,
+	0x0810b601,
+	0x50f960f9,
+	0xe0fcd0fc,
+	0xb63f21f4,
+	0x1bf40242,
+/* 0x064b: memx_func_wait */
+	0xf000f8e9,
+	0x84b62c87,
+	0x0088cf06,
+	0x98001e98,
+	0x1c98011d,
+	0x031b9802,
+	0xf41010b6,
+	0x00f8a421,
+/* 0x0668: memx_func_delay */
+	0xb6001e98,
+	0x21f40410,
+/* 0x0673: memx_func_train */
+	0xf100f87f,
+	0xf1000357,
+	0xf1000077,
+	0xf0000097,
+	0x9eb97093,
+	0x0421f402,
+	0xf102d8b9,
+	0xf42710e7,
+/* 0x0692: memx_func_train_loop_outer */
+	0x58e07f21,
+	0x83f10101,
+	0x97f10200,
+	0x93f011e0,
+	0xf990f911,
 	0xfcd0fc80,
 	0x3f21f4e0,
-	0x0560e7f1,
-	0xf110e3f0,
-	0xf10000d7,
-	0xb98000d3,
-	0xb7f102dc,
-	0xb3f02710,
-	0xa421f400,
-	0xf402eeb9,
-	0xddb90421,
-	0x949dff02,
+	0x67f150f9,
+/* 0x06b2: memx_func_train_loop_inner */
+	0x87f10000,
+	0x68ff1111,
+	0x10989490,
+	0xf10589fd,
+	0xf0072097,
+	0x90f91093,
+	0xd0fc80f9,
+	0x21f4e0fc,
+	0x8097f13f,
+	0x1093f000,
+	0xf4029eb9,
+	0xd8b90421,
+	0x2088c502,
+	0x80f990f9,
+	0xe0fcd0fc,
+	0xf13f21f4,
+	0xf0053c97,
+	0x87f11093,
+	0x83f13002,
+	0x90f98000,
+	0xd0fc80f9,
+	0x21f4e0fc,
+	0x60e7f13f,
+	0x10e3f005,
+	0x0000d7f1,
+	0x8000d3f1,
+	0xf100dc90,
+	0xf08480b7,
+	0x21f41eb3,
+	0x0057f1a4,
+	0xff97f100,
+	0x0093f1ff,
+/* 0x0731: memx_func_train_loop_4x */
+	0x80a7f183,
+	0x10a3f000,
+	0xf402aeb9,
+	0xd8b90421,
+	0xdfb7f102,
+	0xffb3f1ff,
+	0x048bfdff,
+	0x80f9a0f9,
+	0xe0fcd0fc,
+	0xf13f21f4,
+	0xf0053ca7,
+	0x87f110a3,
+	0x83f13002,
+	0xa0f98000,
+	0xd0fc80f9,
+	0x21f4e0fc,
+	0x60e7f13f,
+	0x10e3f005,
+	0x0000d7f1,
+	0x8000d3f1,
+	0xf102dcb9,
+	0xf02710b7,
+	0x21f400b3,
+	0x02eeb9a4,
+	0xb90421f4,
+	0x9dff02dd,
+	0x0150b694,
+	0xf4045670,
+	0x7aa0921e,
+	0xa9800bcc,
+	0x0160b600,
+	0x700470b6,
+	0x1ef51066,
+	0x50fcff00,
 	0x700150b6,
-	0x1ef40456,
-	0xcc7aa092,
-	0x00a9800b,
-	0xb60160b6,
-	0x66700470,
-	0x001ef510,
-	0xb650fcff,
-	0x56700150,
-	0xd41ef507,
-/* 0x07cf: memx_exec */
-	0xf900f8fe,
-	0xb9d0f9e0,
-	0xb2b902c1,
-/* 0x07d9: memx_exec_next */
-	0x00139802,
-	0xe70410b6,
-	0xe701f034,
-	0xb601e033,
-	0x30f00132,
-	0xde35980c,
-	0x12b855f9,
-	0xe41ef406,
-	0x98f10b98,
-	0xcbbbf20c,
-	0xc4b7f102,
-	0x06b4b607,
-	0xfc00bbcf,
-	0xf5e0fcd0,
-	0xf8034221,
-/* 0x0815: memx_info */
-	0x01c67000,
-/* 0x081b: memx_info_data */
-	0xf10e0bf4,
-	0xf103ccc7,
-	0xf40800b7,
-/* 0x0826: memx_info_train */
-	0xc7f10b0e,
-	0xb7f10bcc,
-/* 0x082e: memx_info_send */
-	0x21f50100,
-	0x00f80342,
-/* 0x0834: memx_recv */
-	0xf401d6b0,
-	0xd6b0980b,
-	0xd80bf400,
-/* 0x0842: memx_init */
-	0x00f800f8,
-/* 0x0844: perf_recv */
-/* 0x0846: perf_init */
-	0x00f800f8,
-/* 0x0848: i2c_drive_scl */
-	0xf40036b0,
-	0x07f1110b,
-	0x04b607e0,
-	0x0001d006,
-	0x00f804bd,
-/* 0x085c: i2c_drive_scl_lo */
-	0x07e407f1,
-	0xd00604b6,
-	0x04bd0001,
-/* 0x086a: i2c_drive_sda */
-	0x36b000f8,
-	0x110bf400,
-	0x07e007f1,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x087e: i2c_drive_sda_lo */
-	0x07f100f8,
-	0x04b607e4,
-	0x0002d006,
-	0x00f804bd,
-/* 0x088c: i2c_sense_scl */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0431fd00,
-	0xf4060bf4,
-/* 0x08a2: i2c_sense_scl_done */
-	0x00f80131,
-/* 0x08a4: i2c_sense_sda */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0432fd00,
-	0xf4060bf4,
-/* 0x08ba: i2c_sense_sda_done */
-	0x00f80131,
-/* 0x08bc: i2c_raise_scl */
-	0x47f140f9,
-	0x37f00898,
-	0x4821f501,
-/* 0x08c9: i2c_raise_scl_wait */
+	0x1ef50756,
+	0x00f8fed4,
+/* 0x07c4: memx_exec */
+	0xd0f9e0f9,
+	0xb902c1b9,
+/* 0x07ce: memx_exec_next */
+	0x139802b2,
+	0x0410b600,
+	0x01f034e7,
+	0x01e033e7,
+	0xf00132b6,
+	0x35980c30,
+	0xb855f9de,
+	0x1ef40612,
+	0xf10b98e4,
+	0xbbf20c98,
+	0xb7f102cb,
+	0xb4b607c4,
+	0x00bbcf06,
+	0xe0fcd0fc,
+	0x033721f5,
+/* 0x080a: memx_info */
+	0xc67000f8,
+	0x0e0bf401,
+/* 0x0810: memx_info_data */
+	0x03ccc7f1,
+	0x0800b7f1,
+/* 0x081b: memx_info_train */
+	0xf10b0ef4,
+	0xf10bccc7,
+/* 0x0823: memx_info_send */
+	0xf50100b7,
+	0xf8033721,
+/* 0x0829: memx_recv */
+	0x01d6b000,
+	0xb0980bf4,
+	0x0bf400d6,
+/* 0x0837: memx_init */
+	0xf800f8d8,
+/* 0x0839: perf_recv */
+/* 0x083b: perf_init */
+	0xf800f800,
+/* 0x083d: i2c_drive_scl */
+	0x0036b000,
+	0xf1110bf4,
+	0xb607e007,
+	0x01d00604,
+	0xf804bd00,
+/* 0x0851: i2c_drive_scl_lo */
+	0xe407f100,
+	0x0604b607,
+	0xbd0001d0,
+/* 0x085f: i2c_drive_sda */
+	0xb000f804,
+	0x0bf40036,
+	0xe007f111,
+	0x0604b607,
+	0xbd0002d0,
+/* 0x0873: i2c_drive_sda_lo */
+	0xf100f804,
+	0xb607e407,
+	0x02d00604,
+	0xf804bd00,
+/* 0x0881: i2c_sense_scl */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x31fd0033,
+	0x060bf404,
+/* 0x0897: i2c_sense_scl_done */
+	0xf80131f4,
+/* 0x0899: i2c_sense_sda */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x32fd0033,
+	0x060bf404,
+/* 0x08af: i2c_sense_sda_done */
+	0xf80131f4,
+/* 0x08b1: i2c_raise_scl */
+	0xf140f900,
+	0xf0089847,
+	0x21f50137,
+/* 0x08be: i2c_raise_scl_wait */
+	0xe7f1083d,
+	0x21f403e8,
+	0x8121f57f,
+	0x0901f408,
+	0xf40142b6,
+/* 0x08d2: i2c_raise_scl_done */
+	0x40fcef1b,
+/* 0x08d6: i2c_start */
+	0x21f500f8,
+	0x11f40881,
+	0x9921f50d,
+	0x0611f408,
+/* 0x08e7: i2c_start_rep */
+	0xf0300ef4,
+	0x21f50037,
+	0x37f0083d,
+	0x5f21f501,
+	0x0076bb08,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b608b1,
+	0x1f11f404,
+/* 0x0914: i2c_start_send */
+	0xf50037f0,
+	0xf1085f21,
+	0xf41388e7,
+	0x37f07f21,
+	0x3d21f500,
+	0x88e7f108,
+	0x7f21f413,
+/* 0x0930: i2c_start_out */
+/* 0x0932: i2c_stop */
+	0x37f000f8,
+	0x3d21f500,
+	0x0037f008,
+	0x085f21f5,
+	0x03e8e7f1,
+	0xf07f21f4,
+	0x21f50137,
+	0xe7f1083d,
+	0x21f41388,
+	0x0137f07f,
+	0x085f21f5,
+	0x1388e7f1,
+	0xf87f21f4,
+/* 0x0965: i2c_bitw */
+	0x5f21f500,
 	0xe8e7f108,
 	0x7f21f403,
-	0x088c21f5,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x08dd: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x08e1: i2c_start */
-	0x8c21f500,
-	0x0d11f408,
-	0x08a421f5,
-	0xf40611f4,
-/* 0x08f2: i2c_start_rep */
-	0x37f0300e,
-	0x4821f500,
-	0x0137f008,
-	0x086a21f5,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xbc21f550,
+	0xb121f550,
 	0x0464b608,
-/* 0x091f: i2c_start_send */
-	0xf01f11f4,
-	0x21f50037,
-	0xe7f1086a,
-	0x21f41388,
-	0x0037f07f,
-	0x084821f5,
-	0x1388e7f1,
-/* 0x093b: i2c_start_out */
-	0xf87f21f4,
-/* 0x093d: i2c_stop */
-	0x0037f000,
-	0x084821f5,
-	0xf50037f0,
-	0xf1086a21,
-	0xf403e8e7,
+	0xf11811f4,
+	0xf41388e7,
 	0x37f07f21,
-	0x4821f501,
+	0x3d21f500,
 	0x88e7f108,
 	0x7f21f413,
-	0xf50137f0,
-	0xf1086a21,
-	0xf41388e7,
-	0x00f87f21,
-/* 0x0970: i2c_bitw */
-	0x086a21f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x08bc21f5,
-	0xf40464b6,
-	0xe7f11811,
+/* 0x09a4: i2c_bitw_out */
+/* 0x09a6: i2c_bitr */
+	0x37f000f8,
+	0x5f21f501,
+	0xe8e7f108,
+	0x7f21f403,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xb121f550,
+	0x0464b608,
+	0xf51b11f4,
+	0xf0089921,
+	0x21f50037,
+	0xe7f1083d,
 	0x21f41388,
-	0x0037f07f,
-	0x084821f5,
-	0x1388e7f1,
-/* 0x09af: i2c_bitw_out */
-	0xf87f21f4,
-/* 0x09b1: i2c_bitr */
-	0x0137f000,
-	0x086a21f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x08bc21f5,
-	0xf40464b6,
-	0x21f51b11,
-	0x37f008a4,
-	0x4821f500,
-	0x88e7f108,
-	0x7f21f413,
-	0xf4013cf0,
-/* 0x09f6: i2c_bitr_done */
-	0x00f80131,
-/* 0x09f8: i2c_get_byte */
-	0xf00057f0,
-/* 0x09fe: i2c_get_byte_next */
-	0x54b60847,
+	0x013cf07f,
+/* 0x09eb: i2c_bitr_done */
+	0xf80131f4,
+/* 0x09ed: i2c_get_byte */
+	0x0057f000,
+/* 0x09f3: i2c_get_byte_next */
+	0xb60847f0,
+	0x76bb0154,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb609a621,
+	0x11f40464,
+	0x0553fd2b,
+	0xf40142b6,
+	0x37f0d81b,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b609b1,
-	0x2b11f404,
-	0xb60553fd,
-	0x1bf40142,
-	0x0137f0d8,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x7021f550,
-	0x0464b609,
-/* 0x0a48: i2c_get_byte_done */
-/* 0x0a4a: i2c_put_byte */
-	0x47f000f8,
-/* 0x0a4d: i2c_put_byte_next */
-	0x0142b608,
-	0xbb3854ff,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x097021f5,
-	0xf40464b6,
-	0x46b03411,
-	0xd81bf400,
+	0x64b60965,
+/* 0x0a3d: i2c_get_byte_done */
+/* 0x0a3f: i2c_put_byte */
+	0xf000f804,
+/* 0x0a42: i2c_put_byte_next */
+	0x42b60847,
+	0x3854ff01,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xb121f550,
+	0x6521f550,
 	0x0464b609,
-	0xbb0f11f4,
-	0x36b00076,
-	0x061bf401,
-/* 0x0aa3: i2c_put_byte_done */
-	0xf80132f4,
-/* 0x0aa5: i2c_addr */
-	0x0076bb00,
+	0xb03411f4,
+	0x1bf40046,
+	0x0076bbd8,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b608e1,
-	0x2911f404,
-	0x012ec3e7,
-	0xfd0134b6,
-	0x76bb0553,
+	0x64b609a6,
+	0x0f11f404,
+	0xb00076bb,
+	0x1bf40136,
+	0x0132f406,
+/* 0x0a98: i2c_put_byte_done */
+/* 0x0a9a: i2c_addr */
+	0x76bb00f8,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb60a4a21,
-/* 0x0aea: i2c_addr_done */
-	0x00f80464,
-/* 0x0aec: i2c_acquire_addr */
-	0xb6f8cec7,
-	0xe0b702e4,
-	0xee980d1c,
-/* 0x0afb: i2c_acquire */
-	0xf500f800,
-	0xf40aec21,
-	0xd9f00421,
-	0x3f21f403,
-/* 0x0b0a: i2c_release */
-	0x21f500f8,
-	0x21f40aec,
-	0x03daf004,
-	0xf83f21f4,
-/* 0x0b19: i2c_recv */
-	0x0132f400,
-	0xb6f8c1c7,
-	0x16b00214,
-	0x3a1ff528,
-	0xf413a001,
-	0x0032980c,
-	0x0ccc13a0,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
+	0xb608d621,
+	0x11f40464,
+	0x2ec3e729,
+	0x0134b601,
+	0xbb0553fd,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0afb21f5,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b31bf5,
-	0xbb0057f0,
+	0x0a3f21f5,
+/* 0x0adf: i2c_addr_done */
+	0xf80464b6,
+/* 0x0ae1: i2c_acquire_addr */
+	0xf8cec700,
+	0xb702e4b6,
+	0x980d1ce0,
+	0x00f800ee,
+/* 0x0af0: i2c_acquire */
+	0x0ae121f5,
+	0xf00421f4,
+	0x21f403d9,
+/* 0x0aff: i2c_release */
+	0xf500f83f,
+	0xf40ae121,
+	0xdaf00421,
+	0x3f21f403,
+/* 0x0b0e: i2c_recv */
+	0x32f400f8,
+	0xf8c1c701,
+	0xb00214b6,
+	0x1ff52816,
+	0x13a0013a,
+	0x32980cf4,
+	0xcc13a000,
+	0x0031980c,
+	0xf90231f4,
+	0xf9e0f9d0,
+	0x0067f1d0,
+	0x0063f100,
+	0x01679210,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xf021f550,
+	0x0464b60a,
+	0xd6b0d0fc,
+	0xb31bf500,
+	0x0057f000,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x9a21f550,
+	0x0464b60a,
+	0x00d011f5,
+	0xbbe0c5c7,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0aa521f5,
+	0x0a3f21f5,
 	0xf50464b6,
-	0xc700d011,
-	0x76bbe0c5,
+	0xf000ad11,
+	0x76bb0157,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb60a4a21,
+	0xb60a9a21,
 	0x11f50464,
-	0x57f000ad,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b60aa5,
-	0x8a11f504,
-	0x0076bb00,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b609f8,
-	0x6a11f404,
-	0xbbe05bcb,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x093d21f5,
-	0xb90464b6,
-	0x74bd025b,
-/* 0x0c1f: i2c_recv_not_rd08 */
-	0xb0430ef4,
-	0x1bf401d6,
-	0x0057f03d,
-	0x0aa521f5,
-	0xc73311f4,
-	0x21f5e0c5,
-	0x11f40a4a,
-	0x0057f029,
-	0x0aa521f5,
-	0xc71f11f4,
-	0x21f5e0b5,
-	0x11f40a4a,
-	0x3d21f515,
-	0xc774bd09,
-	0x1bf408c5,
-	0x0232f409,
-/* 0x0c5f: i2c_recv_not_wr08 */
-/* 0x0c5f: i2c_recv_done */
-	0xc7030ef4,
-	0x21f5f8ce,
-	0xe0fc0b0a,
-	0x12f4d0fc,
-	0x027cb90a,
-	0x034221f5,
-/* 0x0c74: i2c_recv_exit */
-/* 0x0c76: i2c_init */
+	0x76bb008a,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb609ed21,
+	0x11f40464,
+	0xe05bcb6a,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x3221f550,
+	0x0464b609,
+	0xbd025bb9,
+	0x430ef474,
+/* 0x0c14: i2c_recv_not_rd08 */
+	0xf401d6b0,
+	0x57f03d1b,
+	0x9a21f500,
+	0x3311f40a,
+	0xf5e0c5c7,
+	0xf40a3f21,
+	0x57f02911,
+	0x9a21f500,
+	0x1f11f40a,
+	0xf5e0b5c7,
+	0xf40a3f21,
+	0x21f51511,
+	0x74bd0932,
+	0xf408c5c7,
+	0x32f4091b,
+	0x030ef402,
+/* 0x0c54: i2c_recv_not_wr08 */
+/* 0x0c54: i2c_recv_done */
+	0xf5f8cec7,
+	0xfc0aff21,
+	0xf4d0fce0,
+	0x7cb90a12,
+	0x3721f502,
+/* 0x0c69: i2c_recv_exit */
+/* 0x0c6b: i2c_init */
+	0xf800f803,
+/* 0x0c6d: test_recv */
+	0xd817f100,
+	0x0614b605,
+	0xb60011cf,
+	0x07f10110,
+	0x04b605d8,
+	0x0001d006,
+	0xe7f104bd,
+	0xe3f1d900,
+	0x21f5134f,
+	0x00f80257,
+/* 0x0c94: test_init */
+	0x0800e7f1,
+	0x025721f5,
+/* 0x0c9e: idle_recv */
 	0x00f800f8,
-/* 0x0c78: test_recv */
-	0x05d817f1,
-	0xcf0614b6,
-	0x10b60011,
-	0xd807f101,
-	0x0604b605,
-	0xbd0001d0,
-	0x00e7f104,
-	0x4fe3f1d9,
-	0x6221f513,
-/* 0x0c9f: test_init */
-	0xf100f802,
-	0xf50800e7,
-	0xf8026221,
-/* 0x0ca9: idle_recv */
-/* 0x0cab: idle */
-	0xf400f800,
-	0x17f10031,
-	0x14b605d4,
-	0x0011cf06,
-	0xf10110b6,
-	0xb605d407,
-	0x01d00604,
-/* 0x0cc7: idle_loop */
-	0xf004bd00,
-	0x32f45817,
-/* 0x0ccd: idle_proc */
-/* 0x0ccd: idle_proc_exec */
-	0xb910f902,
-	0x21f5021e,
-	0x10fc034b,
-	0xf40911f4,
-	0x0ef40231,
-/* 0x0ce1: idle_proc_next */
-	0x5810b6ef,
-	0xf4061fb8,
-	0x02f4e61b,
-	0x0028f4dd,
-	0x00bb0ef4,
+/* 0x0ca0: idle */
+	0xf10031f4,
+	0xb605d417,
+	0x11cf0614,
+	0x0110b600,
+	0x05d407f1,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x0cbc: idle_loop */
+	0xf45817f0,
+/* 0x0cc2: idle_proc */
+/* 0x0cc2: idle_proc_exec */
+	0x10f90232,
+	0xf5021eb9,
+	0xfc034021,
+	0x0911f410,
+	0xf40231f4,
+/* 0x0cd6: idle_proc_next */
+	0x10b6ef0e,
+	0x061fb858,
+	0xf4e61bf4,
+	0x28f4dd02,
+	0xbb0ef400,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/kernel.fuc b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/kernel.fuc
index 5cf5be63cbef..ad35fa57be94 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/kernel.fuc
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/pmu/fuc/kernel.fuc
@@ -225,17 +225,11 @@ intr:
 		nv_iowr(NV_PPWR_SUBINTR, $r9)
 
 	intr_skip_subintr:
-	and $r9 $r8 NV_PPWR_INTR_PAUSE
-	bra z #intr_skip_pause
-		and $r10 0xffbf
-
-	intr_skip_pause:
-	and $r9 $r8 NV_PPWR_INTR_USER0
-	bra z #intr_skip_user0
-		and $r10 0xffbf
-
-	intr_skip_user0:
+	mov $r9 (NV_PPWR_INTR_USER0 | NV_PPWR_INTR_USER1 | NV_PPWR_INTR_PAUSE)
+	not b32 $r9
+	and $r8 $r9
 	nv_iowr(NV_PPWR_INTR_ACK, $r8)
+
 	pop $r8
 	mov $flags $r8
 	pop $r15
diff --git a/drivers/gpu/drm/nouveau/nvkm/subdev/therm/nv40.c b/drivers/gpu/drm/nouveau/nvkm/subdev/therm/nv40.c
index 6326fdc5a48d..2c92ffb5f9d0 100644
--- a/drivers/gpu/drm/nouveau/nvkm/subdev/therm/nv40.c
+++ b/drivers/gpu/drm/nouveau/nvkm/subdev/therm/nv40.c
@@ -107,7 +107,7 @@ nv40_fan_pwm_ctrl(struct nvkm_therm *therm, int line, bool enable)
 {
 	struct nvkm_subdev *subdev = &therm->subdev;
 	struct nvkm_device *device = subdev->device;
-	u32 mask = enable ? 0x80000000 : 0x0000000;
+	u32 mask = enable ? 0x80000000 : 0x00000000;
 	if      (line == 2) nvkm_mask(device, 0x0010f0, 0x80000000, mask);
 	else if (line == 9) nvkm_mask(device, 0x0015f4, 0x80000000, mask);
 	else {
diff --git a/drivers/gpu/drm/omapdrm/Kconfig b/drivers/gpu/drm/omapdrm/Kconfig
index 6c220cd3497a..336ad4de9981 100644
--- a/drivers/gpu/drm/omapdrm/Kconfig
+++ b/drivers/gpu/drm/omapdrm/Kconfig
@@ -1,9 +1,8 @@
-
 config DRM_OMAP
 	tristate "OMAP DRM"
 	depends on DRM
 	depends on ARCH_OMAP2PLUS || ARCH_MULTIPLATFORM
-	depends on OMAP2_DSS
+	select OMAP2_DSS
 	select DRM_KMS_HELPER
 	select DRM_KMS_FB_HELPER
 	select FB_SYS_FILLRECT
@@ -14,13 +13,18 @@ config DRM_OMAP
 	help
 	  DRM display driver for OMAP2/3/4 based boards.
 
+if DRM_OMAP
+
 config DRM_OMAP_NUM_CRTCS
 	int "Number of CRTCs"
 	range 1 10
 	default 1  if ARCH_OMAP2 || ARCH_OMAP3
 	default 2  if ARCH_OMAP4
-	depends on DRM_OMAP
 	help
 	  Select the number of video overlays which can be used as framebuffers.
 	  The remaining overlays are reserved for video.
 
+source "drivers/gpu/drm/omapdrm/dss/Kconfig"
+source "drivers/gpu/drm/omapdrm/displays/Kconfig"
+
+endif
diff --git a/drivers/gpu/drm/omapdrm/Makefile b/drivers/gpu/drm/omapdrm/Makefile
index 778372b062ad..fe4c2228bc18 100644
--- a/drivers/gpu/drm/omapdrm/Makefile
+++ b/drivers/gpu/drm/omapdrm/Makefile
@@ -3,6 +3,9 @@
 # Direct Rendering Infrastructure (DRI)
 #
 
+obj-y += dss/
+obj-y += displays/
+
 ccflags-y := -Iinclude/drm -Werror
 omapdrm-y := omap_drv.o \
 	omap_irq.o \
@@ -12,10 +15,11 @@ omapdrm-y := omap_drv.o \
 	omap_encoder.o \
 	omap_connector.o \
 	omap_fb.o \
-	omap_fbdev.o \
 	omap_gem.o \
 	omap_gem_dmabuf.o \
 	omap_dmm_tiler.o \
 	tcm-sita.o
 
+omapdrm-$(CONFIG_DRM_FBDEV_EMULATION) += omap_fbdev.o
+
 obj-$(CONFIG_DRM_OMAP)	+= omapdrm.o
diff --git a/drivers/video/fbdev/omap2/displays-new/Kconfig b/drivers/gpu/drm/omapdrm/displays/Kconfig
index 574710141a61..2a618afe0f53 100644
--- a/drivers/video/fbdev/omap2/displays-new/Kconfig
+++ b/drivers/gpu/drm/omapdrm/displays/Kconfig
@@ -1,5 +1,4 @@
-menu "OMAP Display Device Drivers (new device model)"
-        depends on OMAP2_DSS
+menu "OMAPDRM External Display Device Drivers"
 
 config DISPLAY_ENCODER_OPA362
 	tristate "OPA362 external analog amplifier"
diff --git a/drivers/video/fbdev/omap2/displays-new/Makefile b/drivers/gpu/drm/omapdrm/displays/Makefile
index 9aa176bfbf2e..9aa176bfbf2e 100644
--- a/drivers/video/fbdev/omap2/displays-new/Makefile
+++ b/drivers/gpu/drm/omapdrm/displays/Makefile
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c b/drivers/gpu/drm/omapdrm/displays/connector-analog-tv.c
index 8511c648a15c..8511c648a15c 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-analog-tv.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-analog-tv.c
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c b/drivers/gpu/drm/omapdrm/displays/connector-dvi.c
index d811e6dcaef7..d811e6dcaef7 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-dvi.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-dvi.c
diff --git a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c b/drivers/gpu/drm/omapdrm/displays/connector-hdmi.c
index 6ee4129bc0c0..6ee4129bc0c0 100644
--- a/drivers/video/fbdev/omap2/displays-new/connector-hdmi.c
+++ b/drivers/gpu/drm/omapdrm/displays/connector-hdmi.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c b/drivers/gpu/drm/omapdrm/displays/encoder-opa362.c
index 8c246c213e06..8c246c213e06 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-opa362.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-opa362.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c b/drivers/gpu/drm/omapdrm/displays/encoder-tfp410.c
index d9048b3df495..d9048b3df495 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tfp410.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-tfp410.c
diff --git a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c b/drivers/gpu/drm/omapdrm/displays/encoder-tpd12s015.c
index 990af6baeb0f..990af6baeb0f 100644
--- a/drivers/video/fbdev/omap2/displays-new/encoder-tpd12s015.c
+++ b/drivers/gpu/drm/omapdrm/displays/encoder-tpd12s015.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c b/drivers/gpu/drm/omapdrm/displays/panel-dpi.c
index f7be3489f744..e780fd4f8b46 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dpi.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-dpi.c
@@ -83,8 +83,7 @@ static int panel_dpi_enable(struct omap_dss_device *dssdev)
 	if (r)
 		return r;
 
-	if (ddata->enable_gpio)
-		gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+	gpiod_set_value_cansleep(ddata->enable_gpio, 1);
 
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 1);
@@ -102,12 +101,11 @@ static void panel_dpi_disable(struct omap_dss_device *dssdev)
 	if (!omapdss_device_is_enabled(dssdev))
 		return;
 
-	if (ddata->enable_gpio)
-		gpiod_set_value_cansleep(ddata->enable_gpio, 0);
-
 	if (gpio_is_valid(ddata->backlight_gpio))
 		gpio_set_value_cansleep(ddata->backlight_gpio, 0);
 
+	gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
 	in->ops.dpi->disable(in);
 
 	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c b/drivers/gpu/drm/omapdrm/displays/panel-dsi-cm.c
index 3414c2609320..3414c2609320 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-dsi-cm.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-dsi-cm.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c b/drivers/gpu/drm/omapdrm/displays/panel-lgphilips-lb035q02.c
index 18eb60e9c9ec..18eb60e9c9ec 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-lgphilips-lb035q02.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-lgphilips-lb035q02.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c b/drivers/gpu/drm/omapdrm/displays/panel-nec-nl8048hl11.c
index 8a928c9a2fc9..8a928c9a2fc9 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-nec-nl8048hl11.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-nec-nl8048hl11.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c b/drivers/gpu/drm/omapdrm/displays/panel-sharp-ls037v7dw01.c
index abfd1f6e3327..abfd1f6e3327 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sharp-ls037v7dw01.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-sharp-ls037v7dw01.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-sony-acx565akm.c b/drivers/gpu/drm/omapdrm/displays/panel-sony-acx565akm.c
index 31efcca801bd..31efcca801bd 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-sony-acx565akm.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-sony-acx565akm.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td028ttec1.c
index 4d657f3ab679..4d657f3ab679 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td028ttec1.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td028ttec1.c
diff --git a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td043mtea1.c
index 68e3b68a2920..68e3b68a2920 100644
--- a/drivers/video/fbdev/omap2/displays-new/panel-tpo-td043mtea1.c
+++ b/drivers/gpu/drm/omapdrm/displays/panel-tpo-td043mtea1.c
diff --git a/drivers/video/fbdev/omap2/dss/Kconfig b/drivers/gpu/drm/omapdrm/dss/Kconfig
index d1fa730c7d54..d1fa730c7d54 100644
--- a/drivers/video/fbdev/omap2/dss/Kconfig
+++ b/drivers/gpu/drm/omapdrm/dss/Kconfig
diff --git a/drivers/video/fbdev/omap2/dss/Makefile b/drivers/gpu/drm/omapdrm/dss/Makefile
index b5136d3d4b77..b5136d3d4b77 100644
--- a/drivers/video/fbdev/omap2/dss/Makefile
+++ b/drivers/gpu/drm/omapdrm/dss/Makefile
diff --git a/drivers/video/fbdev/omap2/dss/apply.c b/drivers/gpu/drm/omapdrm/dss/apply.c
index 663ccc3bf4e5..663ccc3bf4e5 100644
--- a/drivers/video/fbdev/omap2/dss/apply.c
+++ b/drivers/gpu/drm/omapdrm/dss/apply.c
diff --git a/drivers/video/fbdev/omap2/dss/core.c b/drivers/gpu/drm/omapdrm/dss/core.c
index 54eeb507f9b3..54eeb507f9b3 100644
--- a/drivers/video/fbdev/omap2/dss/core.c
+++ b/drivers/gpu/drm/omapdrm/dss/core.c
diff --git a/drivers/video/fbdev/omap2/dss/dispc-compat.c b/drivers/gpu/drm/omapdrm/dss/dispc-compat.c
index 633c461fbc6e..0918b3bfe82a 100644
--- a/drivers/video/fbdev/omap2/dss/dispc-compat.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc-compat.c
@@ -476,7 +476,7 @@ static void dispc_mgr_disable_lcd_out(enum omap_channel channel)
 	int r;
 	u32 irq;
 
-	if (dispc_mgr_is_enabled(channel) == false)
+	if (!dispc_mgr_is_enabled(channel))
 		return;
 
 	/*
@@ -524,7 +524,7 @@ static void dispc_mgr_enable_digit_out(void)
 	int r;
 	u32 irq_mask;
 
-	if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT) == true)
+	if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
 		return;
 
 	/*
@@ -562,7 +562,7 @@ static void dispc_mgr_disable_digit_out(void)
 	u32 irq_mask;
 	int num_irqs;
 
-	if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT) == false)
+	if (!dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
 		return;
 
 	/*
diff --git a/drivers/video/fbdev/omap2/dss/dispc-compat.h b/drivers/gpu/drm/omapdrm/dss/dispc-compat.h
index 14a69b3d4fb0..14a69b3d4fb0 100644
--- a/drivers/video/fbdev/omap2/dss/dispc-compat.h
+++ b/drivers/gpu/drm/omapdrm/dss/dispc-compat.h
diff --git a/drivers/video/fbdev/omap2/dss/dispc.c b/drivers/gpu/drm/omapdrm/dss/dispc.c
index be716c9ffb88..6b50476ec669 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.c
@@ -99,6 +99,11 @@ struct dispc_features {
 
 	/* PIXEL_INC is not added to the last pixel of a line */
 	bool last_pixel_inc_missing:1;
+
+	/* POL_FREQ has ALIGN bit */
+	bool supports_sync_align:1;
+
+	bool has_writeback:1;
 };
 
 #define DISPC_MAX_NR_FIFOS 5
@@ -246,6 +251,11 @@ struct color_conv_coef {
 	int full_range;
 };
 
+static unsigned long dispc_fclk_rate(void);
+static unsigned long dispc_core_clk_rate(void);
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
+
 static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
 static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
 
@@ -571,7 +581,7 @@ EXPORT_SYMBOL(dispc_mgr_go_busy);
 
 void dispc_mgr_go(enum omap_channel channel)
 {
-	WARN_ON(dispc_mgr_is_enabled(channel) == false);
+	WARN_ON(!dispc_mgr_is_enabled(channel));
 	WARN_ON(dispc_mgr_go_busy(channel));
 
 	DSSDBG("GO %s\n", mgr_desc[channel].name);
@@ -707,19 +717,20 @@ static void dispc_setup_color_conv_coef(void)
 {
 	int i;
 	int num_ovl = dss_feat_get_num_ovls();
-	int num_wb = dss_feat_get_num_wbs();
 	const struct color_conv_coef ctbl_bt601_5_ovl = {
+		/* YUV -> RGB */
 		298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
 	};
 	const struct color_conv_coef ctbl_bt601_5_wb = {
-		66, 112, -38, 129, -94, -74, 25, -18, 112, 0,
+		/* RGB -> YUV */
+		66, 129, 25, 112, -94, -18, -38, -74, 112, 0,
 	};
 
 	for (i = 1; i < num_ovl; i++)
 		dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
 
-	for (; i < num_wb; i++)
-		dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_wb);
+	if (dispc.feat->has_writeback)
+		dispc_ovl_write_color_conv_coef(OMAP_DSS_WB, &ctbl_bt601_5_wb);
 }
 
 static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
@@ -970,6 +981,10 @@ void dispc_ovl_set_channel_out(enum omap_plane plane, enum omap_channel channel)
 				return;
 			}
 			break;
+		case OMAP_DSS_CHANNEL_WB:
+			chan = 0;
+			chan2 = 3;
+			break;
 		default:
 			BUG();
 			return;
@@ -988,7 +1003,6 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 {
 	int shift;
 	u32 val;
-	enum omap_channel channel;
 
 	switch (plane) {
 	case OMAP_DSS_GFX:
@@ -1006,23 +1020,23 @@ static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
 
 	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 
-	if (dss_has_feature(FEAT_MGR_LCD3)) {
-		if (FLD_GET(val, 31, 30) == 0)
-			channel = FLD_GET(val, shift, shift);
-		else if (FLD_GET(val, 31, 30) == 1)
-			channel = OMAP_DSS_CHANNEL_LCD2;
-		else
-			channel = OMAP_DSS_CHANNEL_LCD3;
-	} else if (dss_has_feature(FEAT_MGR_LCD2)) {
-		if (FLD_GET(val, 31, 30) == 0)
-			channel = FLD_GET(val, shift, shift);
-		else
-			channel = OMAP_DSS_CHANNEL_LCD2;
-	} else {
-		channel = FLD_GET(val, shift, shift);
-	}
+	if (FLD_GET(val, shift, shift) == 1)
+		return OMAP_DSS_CHANNEL_DIGIT;
+
+	if (!dss_has_feature(FEAT_MGR_LCD2))
+		return OMAP_DSS_CHANNEL_LCD;
 
-	return channel;
+	switch (FLD_GET(val, 31, 30)) {
+	case 0:
+	default:
+		return OMAP_DSS_CHANNEL_LCD;
+	case 1:
+		return OMAP_DSS_CHANNEL_LCD2;
+	case 2:
+		return OMAP_DSS_CHANNEL_LCD3;
+	case 3:
+		return OMAP_DSS_CHANNEL_WB;
+	}
 }
 
 void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
@@ -1050,6 +1064,8 @@ static void dispc_configure_burst_sizes(void)
 	/* Configure burst size always to maximum size */
 	for (i = 0; i < dss_feat_get_num_ovls(); ++i)
 		dispc_ovl_set_burst_size(i, burst_size);
+	if (dispc.feat->has_writeback)
+		dispc_ovl_set_burst_size(OMAP_DSS_WB, burst_size);
 }
 
 static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
@@ -1196,6 +1212,17 @@ static void dispc_init_fifos(void)
 
 		dispc_ovl_set_fifo_threshold(i, low, high);
 	}
+
+	if (dispc.feat->has_writeback) {
+		u32 low, high;
+		const bool use_fifomerge = false;
+		const bool manual_update = false;
+
+		dispc_ovl_compute_fifo_thresholds(OMAP_DSS_WB, &low, &high,
+			use_fifomerge, manual_update);
+
+		dispc_ovl_set_fifo_threshold(OMAP_DSS_WB, low, high);
+	}
 }
 
 static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
@@ -1248,7 +1275,6 @@ void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
 			plane != OMAP_DSS_WB)
 		dispc_write_reg(DISPC_OVL_PRELOAD(plane), min(high, 0xfffu));
 }
-EXPORT_SYMBOL(dispc_ovl_set_fifo_threshold);
 
 void dispc_enable_fifomerge(bool enable)
 {
@@ -1307,7 +1333,6 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
 		*fifo_high = total_fifo_size - buf_unit;
 	}
 }
-EXPORT_SYMBOL(dispc_ovl_compute_fifo_thresholds);
 
 static void dispc_ovl_set_mflag(enum omap_plane plane, bool enable)
 {
@@ -1364,6 +1389,25 @@ static void dispc_init_mflag(void)
 
 		dispc_ovl_set_mflag_threshold(i, low, high);
 	}
+
+	if (dispc.feat->has_writeback) {
+		u32 size = dispc_ovl_get_fifo_size(OMAP_DSS_WB);
+		u32 unit = dss_feat_get_buffer_size_unit();
+		u32 low, high;
+
+		dispc_ovl_set_mflag(OMAP_DSS_WB, true);
+
+		/*
+		 * Simulation team suggests below thesholds:
+		 * HT = fifosize * 5 / 8;
+		 * LT = fifosize * 4 / 8;
+		 */
+
+		low = size * 4 / 8 / unit;
+		high = size * 5 / 8 / unit;
+
+		dispc_ovl_set_mflag_threshold(OMAP_DSS_WB, low, high);
+	}
 }
 
 static void dispc_ovl_set_fir(enum omap_plane plane,
@@ -2438,7 +2482,7 @@ static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
 	if (width == out_width && height == out_height)
 		return 0;
 
-	if (pclk == 0 || mgr_timings->pixelclock == 0) {
+	if (!mem_to_mem && (pclk == 0 || mgr_timings->pixelclock == 0)) {
 		DSSERR("cannot calculate scaling settings: pclk is zero\n");
 		return -EINVAL;
 	}
@@ -2816,8 +2860,25 @@ int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
 	l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
 	l = FLD_MOD(l, truncation, 10, 10);	/* TRUNCATIONENABLE */
 	l = FLD_MOD(l, mem_to_mem, 19, 19);	/* WRITEBACKMODE */
+	if (mem_to_mem)
+		l = FLD_MOD(l, 1, 26, 24);	/* CAPTUREMODE */
+	else
+		l = FLD_MOD(l, 0, 26, 24);	/* CAPTUREMODE */
 	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
 
+	if (mem_to_mem) {
+		/* WBDELAYCOUNT */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 7, 0);
+	} else {
+		int wbdelay;
+
+		wbdelay = min(mgr_timings->vfp + mgr_timings->vsw +
+			mgr_timings->vbp, 255);
+
+		/* WBDELAYCOUNT */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), wbdelay, 7, 0);
+	}
+
 	return r;
 }
 
@@ -2896,7 +2957,7 @@ static void dispc_mgr_set_lcd_type_tft(enum omap_channel channel)
 	mgr_fld_write(channel, DISPC_MGR_FLD_STNTFT, 1);
 }
 
-void dispc_set_loadmode(enum omap_dss_load_mode mode)
+static void dispc_set_loadmode(enum omap_dss_load_mode mode)
 {
 	REG_FLD_MOD(DISPC_CONFIG, mode, 2, 1);
 }
@@ -3163,6 +3224,10 @@ static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
 		FLD_VAL(hs, 13, 13) |
 		FLD_VAL(vs, 12, 12);
 
+	/* always set ALIGN bit when available */
+	if (dispc.feat->supports_sync_align)
+		l |= (1 << 18);
+
 	dispc_write_reg(DISPC_POL_FREQ(channel), l);
 
 	if (dispc.syscon_pol) {
@@ -3220,7 +3285,7 @@ void dispc_mgr_set_timings(enum omap_channel channel,
 
 		DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
 	} else {
-		if (t.interlace == true)
+		if (t.interlace)
 			t.y_res /= 2;
 	}
 
@@ -3237,7 +3302,7 @@ static void dispc_mgr_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
 	dispc_write_reg(DISPC_DIVISORo(channel),
 			FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
 
-	if (dss_has_feature(FEAT_CORE_CLK_DIV) == false &&
+	if (!dss_has_feature(FEAT_CORE_CLK_DIV) &&
 			channel == OMAP_DSS_CHANNEL_LCD)
 		dispc.core_clk_rate = dispc_fclk_rate() / lck_div;
 }
@@ -3251,7 +3316,7 @@ static void dispc_mgr_get_lcd_divisor(enum omap_channel channel, int *lck_div,
 	*pck_div = FLD_GET(l, 7, 0);
 }
 
-unsigned long dispc_fclk_rate(void)
+static unsigned long dispc_fclk_rate(void)
 {
 	struct dss_pll *pll;
 	unsigned long r = 0;
@@ -3282,7 +3347,7 @@ unsigned long dispc_fclk_rate(void)
 	return r;
 }
 
-unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
 {
 	struct dss_pll *pll;
 	int lcd;
@@ -3323,7 +3388,7 @@ unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
 	}
 }
 
-unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
 {
 	unsigned long r;
 
@@ -3348,7 +3413,7 @@ void dispc_set_tv_pclk(unsigned long pclk)
 	dispc.tv_pclk_rate = pclk;
 }
 
-unsigned long dispc_core_clk_rate(void)
+static unsigned long dispc_core_clk_rate(void)
 {
 	return dispc.core_clk_rate;
 }
@@ -3448,6 +3513,7 @@ static void dispc_dump_regs(struct seq_file *s)
 		[OMAP_DSS_VIDEO1]	= "VID1",
 		[OMAP_DSS_VIDEO2]	= "VID2",
 		[OMAP_DSS_VIDEO3]	= "VID3",
+		[OMAP_DSS_WB]		= "WB",
 	};
 	const char **p_names;
 
@@ -3554,6 +3620,35 @@ static void dispc_dump_regs(struct seq_file *s)
 			DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
 	}
 
+	if (dispc.feat->has_writeback) {
+		i = OMAP_DSS_WB;
+		DUMPREG(i, DISPC_OVL_BA0);
+		DUMPREG(i, DISPC_OVL_BA1);
+		DUMPREG(i, DISPC_OVL_SIZE);
+		DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+		DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+		DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+		DUMPREG(i, DISPC_OVL_ROW_INC);
+		DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+		if (dss_has_feature(FEAT_MFLAG))
+			DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+		DUMPREG(i, DISPC_OVL_FIR);
+		DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+		DUMPREG(i, DISPC_OVL_ACCU0);
+		DUMPREG(i, DISPC_OVL_ACCU1);
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			DUMPREG(i, DISPC_OVL_BA0_UV);
+			DUMPREG(i, DISPC_OVL_BA1_UV);
+			DUMPREG(i, DISPC_OVL_FIR2);
+			DUMPREG(i, DISPC_OVL_ACCU2_0);
+			DUMPREG(i, DISPC_OVL_ACCU2_1);
+		}
+		if (dss_has_feature(FEAT_ATTR2))
+			DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+	}
+
 #undef DISPC_REG
 #undef DUMPREG
 
@@ -3854,6 +3949,8 @@ static const struct dispc_features omap44xx_dispc_feats = {
 	.num_fifos		=	5,
 	.gfx_fifo_workaround	=	true,
 	.set_max_preload	=	true,
+	.supports_sync_align	=	true,
+	.has_writeback		=	true,
 };
 
 static const struct dispc_features omap54xx_dispc_feats = {
@@ -3875,6 +3972,8 @@ static const struct dispc_features omap54xx_dispc_feats = {
 	.gfx_fifo_workaround	=	true,
 	.mstandby_workaround	=	true,
 	.set_max_preload	=	true,
+	.supports_sync_align	=	true,
+	.has_writeback		=	true,
 };
 
 static int dispc_init_features(struct platform_device *pdev)
diff --git a/drivers/video/fbdev/omap2/dss/dispc.h b/drivers/gpu/drm/omapdrm/dss/dispc.h
index 3043d6e0a5f9..483744223dd1 100644
--- a/drivers/video/fbdev/omap2/dss/dispc.h
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.h
@@ -908,6 +908,8 @@ static inline u16 DISPC_MFLAG_THRESHOLD_OFFSET(enum omap_plane plane)
 		return 0x0868;
 	case OMAP_DSS_VIDEO3:
 		return 0x086c;
+	case OMAP_DSS_WB:
+		return 0x0870;
 	default:
 		BUG();
 		return 0;
diff --git a/drivers/video/fbdev/omap2/dss/dispc_coefs.c b/drivers/gpu/drm/omapdrm/dss/dispc_coefs.c
index 038c15b04215..038c15b04215 100644
--- a/drivers/video/fbdev/omap2/dss/dispc_coefs.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc_coefs.c
diff --git a/drivers/video/fbdev/omap2/dss/display-sysfs.c b/drivers/gpu/drm/omapdrm/dss/display-sysfs.c
index 6ad0991f8259..6ad0991f8259 100644
--- a/drivers/video/fbdev/omap2/dss/display-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/display-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/display.c b/drivers/gpu/drm/omapdrm/dss/display.c
index ef5b9027985d..ef5b9027985d 100644
--- a/drivers/video/fbdev/omap2/dss/display.c
+++ b/drivers/gpu/drm/omapdrm/dss/display.c
diff --git a/drivers/video/fbdev/omap2/dss/dpi.c b/drivers/gpu/drm/omapdrm/dss/dpi.c
index fb45b6432968..7953e6a52346 100644
--- a/drivers/video/fbdev/omap2/dss/dpi.c
+++ b/drivers/gpu/drm/omapdrm/dss/dpi.c
@@ -395,7 +395,7 @@ static int dpi_display_enable(struct omap_dss_device *dssdev)
 		goto err_no_reg;
 	}
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err_no_out_mgr;
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/gpu/drm/omapdrm/dss/dsi.c
index b3606def5b7b..43be4b2a7b05 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/gpu/drm/omapdrm/dss/dsi.c
@@ -144,7 +144,7 @@ struct dsi_reg { u16 module; u16 idx; };
 #define DSI_IRQ_TA_TIMEOUT	(1 << 20)
 #define DSI_IRQ_ERROR_MASK \
 	(DSI_IRQ_HS_TX_TIMEOUT | DSI_IRQ_LP_RX_TIMEOUT | DSI_IRQ_SYNC_LOST | \
-	DSI_IRQ_TA_TIMEOUT | DSI_IRQ_SYNC_LOST)
+	DSI_IRQ_TA_TIMEOUT)
 #define DSI_IRQ_CHANNEL_MASK	0xf
 
 /* Virtual channel interrupts */
@@ -3833,7 +3833,7 @@ static int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
 	u16 word_count;
 	int r;
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		return -ENODEV;
 	}
diff --git a/drivers/video/fbdev/omap2/dss/dss-of.c b/drivers/gpu/drm/omapdrm/dss/dss-of.c
index bf407b6ba15c..bf407b6ba15c 100644
--- a/drivers/video/fbdev/omap2/dss/dss-of.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss-of.c
diff --git a/drivers/video/fbdev/omap2/dss/dss.c b/drivers/gpu/drm/omapdrm/dss/dss.c
index 9200a8668b49..f95ff319e68e 100644
--- a/drivers/video/fbdev/omap2/dss/dss.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss.c
@@ -1264,12 +1264,18 @@ static int dss_runtime_suspend(struct device *dev)
 {
 	dss_save_context();
 	dss_set_min_bus_tput(dev, 0);
+
+	pinctrl_pm_select_sleep_state(dev);
+
 	return 0;
 }
 
 static int dss_runtime_resume(struct device *dev)
 {
 	int r;
+
+	pinctrl_pm_select_default_state(dev);
+
 	/*
 	 * Set an arbitrarily high tput request to ensure OPP100.
 	 * What we should really do is to make a request to stay in OPP100,
diff --git a/drivers/video/fbdev/omap2/dss/dss.h b/drivers/gpu/drm/omapdrm/dss/dss.h
index 2406bcdb831a..9a6453235585 100644
--- a/drivers/video/fbdev/omap2/dss/dss.h
+++ b/drivers/gpu/drm/omapdrm/dss/dss.h
@@ -343,7 +343,8 @@ u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
 #else
 static inline u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
 {
-	WARN("%s: DSI not compiled in, returning pixel_size as 0\n", __func__);
+	WARN(1, "%s: DSI not compiled in, returning pixel_size as 0\n",
+	     __func__);
 	return 0;
 }
 #endif
@@ -378,7 +379,6 @@ void dispc_lcd_enable_signal(bool enable);
 void dispc_pck_free_enable(bool enable);
 void dispc_enable_fifomerge(bool enable);
 void dispc_enable_gamma_table(bool enable);
-void dispc_set_loadmode(enum omap_dss_load_mode mode);
 
 typedef bool (*dispc_div_calc_func)(int lckd, int pckd, unsigned long lck,
 		unsigned long pck, void *data);
@@ -388,7 +388,6 @@ bool dispc_div_calc(unsigned long dispc,
 
 bool dispc_mgr_timings_ok(enum omap_channel channel,
 		const struct omap_video_timings *timings);
-unsigned long dispc_fclk_rate(void);
 int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
 		struct dispc_clock_info *cinfo);
 
@@ -398,9 +397,6 @@ void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
 		u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
 		bool manual_update);
 
-unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
-unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
-unsigned long dispc_core_clk_rate(void);
 void dispc_mgr_set_clock_div(enum omap_channel channel,
 		const struct dispc_clock_info *cinfo);
 int dispc_mgr_get_clock_div(enum omap_channel channel,
diff --git a/drivers/video/fbdev/omap2/dss/dss_features.c b/drivers/gpu/drm/omapdrm/dss/dss_features.c
index b0b6dfd657bf..c886a2927f73 100644
--- a/drivers/video/fbdev/omap2/dss/dss_features.c
+++ b/drivers/gpu/drm/omapdrm/dss/dss_features.c
@@ -46,7 +46,6 @@ struct omap_dss_features {
 
 	const int num_mgrs;
 	const int num_ovls;
-	const int num_wbs;
 	const enum omap_display_type *supported_displays;
 	const enum omap_dss_output_id *supported_outputs;
 	const enum omap_color_mode *supported_color_modes;
@@ -735,7 +734,6 @@ static const struct omap_dss_features omap4430_es1_0_dss_features  = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
-	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
 	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
@@ -757,7 +755,6 @@ static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
-	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
 	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
@@ -779,7 +776,6 @@ static const struct omap_dss_features omap4_dss_features = {
 
 	.num_mgrs = 3,
 	.num_ovls = 4,
-	.num_wbs = 1,
 	.supported_displays = omap4_dss_supported_displays,
 	.supported_outputs = omap4_dss_supported_outputs,
 	.supported_color_modes = omap4_dss_supported_color_modes,
@@ -825,11 +821,6 @@ int dss_feat_get_num_ovls(void)
 }
 EXPORT_SYMBOL(dss_feat_get_num_ovls);
 
-int dss_feat_get_num_wbs(void)
-{
-	return omap_current_dss_features->num_wbs;
-}
-
 unsigned long dss_feat_get_param_min(enum dss_range_param param)
 {
 	return omap_current_dss_features->dss_params[param].min;
@@ -844,13 +835,11 @@ enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel
 {
 	return omap_current_dss_features->supported_displays[channel];
 }
-EXPORT_SYMBOL(dss_feat_get_supported_displays);
 
 enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
 {
 	return omap_current_dss_features->supported_outputs[channel];
 }
-EXPORT_SYMBOL(dss_feat_get_supported_outputs);
 
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
 {
diff --git a/drivers/video/fbdev/omap2/dss/dss_features.h b/drivers/gpu/drm/omapdrm/dss/dss_features.h
index 100f7a2d0638..3d67d39f192f 100644
--- a/drivers/video/fbdev/omap2/dss/dss_features.h
+++ b/drivers/gpu/drm/omapdrm/dss/dss_features.h
@@ -86,7 +86,6 @@ enum dss_range_param {
 };
 
 /* DSS Feature Functions */
-int dss_feat_get_num_wbs(void);
 unsigned long dss_feat_get_param_min(enum dss_range_param param);
 unsigned long dss_feat_get_param_max(enum dss_range_param param);
 enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
@@ -102,4 +101,8 @@ bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type);
 bool dss_has_feature(enum dss_feat_id id);
 void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end);
 void dss_features_init(enum omapdss_version version);
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
+
 #endif
diff --git a/drivers/video/fbdev/omap2/dss/hdmi.h b/drivers/gpu/drm/omapdrm/dss/hdmi.h
index 53616b02b613..53616b02b613 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4.c b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
index 94c8d5549b4c..7103c659a534 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
@@ -343,7 +343,7 @@ static int hdmi_display_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&hdmi.lock);
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.c b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.c
index fa72e735dad2..fa72e735dad2 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi4_core.h b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.h
index a069f96ec6f6..a069f96ec6f6 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi4_core.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4_core.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5.c b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
index b59ba7902be1..a955a2c4c061 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
@@ -373,7 +373,7 @@ static int hdmi_display_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&hdmi.lock);
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.c b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.c
index 8ea531d2652c..8ea531d2652c 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5_core.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi5_core.h b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.h
index f2f1022c5516..f2f1022c5516 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi5_core.h
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5_core.h
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_common.c b/drivers/gpu/drm/omapdrm/dss/hdmi_common.c
index 1b8fcc6c4ba1..1b8fcc6c4ba1 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_common.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_common.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_phy.c b/drivers/gpu/drm/omapdrm/dss/hdmi_phy.c
index 1f5d19c119ce..1f5d19c119ce 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_phy.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_phy.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_pll.c b/drivers/gpu/drm/omapdrm/dss/hdmi_pll.c
index 06e23a7c432c..06e23a7c432c 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_pll.c
diff --git a/drivers/video/fbdev/omap2/dss/hdmi_wp.c b/drivers/gpu/drm/omapdrm/dss/hdmi_wp.c
index 7c544bc56fb5..7c544bc56fb5 100644
--- a/drivers/video/fbdev/omap2/dss/hdmi_wp.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi_wp.c
diff --git a/drivers/video/fbdev/omap2/dss/manager-sysfs.c b/drivers/gpu/drm/omapdrm/dss/manager-sysfs.c
index a7414fb12830..a7414fb12830 100644
--- a/drivers/video/fbdev/omap2/dss/manager-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/manager-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/manager.c b/drivers/gpu/drm/omapdrm/dss/manager.c
index 1aac9b4191a9..08a67f4f6a20 100644
--- a/drivers/video/fbdev/omap2/dss/manager.c
+++ b/drivers/gpu/drm/omapdrm/dss/manager.c
@@ -210,7 +210,7 @@ static int dss_mgr_check_lcd_config(struct omap_overlay_manager *mgr,
 		return -EINVAL;
 
 	/* fifohandcheck should be used only with stallmode */
-	if (stallmode == false && fifohandcheck == true)
+	if (!stallmode && fifohandcheck)
 		return -EINVAL;
 
 	/*
diff --git a/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c b/drivers/gpu/drm/omapdrm/dss/omapdss-boot-init.c
index 8b6f6d5fdd68..136d30484d02 100644
--- a/drivers/video/fbdev/omap2/dss/omapdss-boot-init.c
+++ b/drivers/gpu/drm/omapdrm/dss/omapdss-boot-init.c
@@ -199,10 +199,8 @@ static int __init omapdss_boot_init(void)
 	omapdss_walk_device(dss, true);
 
 	for_each_available_child_of_node(dss, child) {
-		if (!of_find_property(child, "compatible", NULL)) {
-			of_node_put(child);
+		if (!of_find_property(child, "compatible", NULL))
 			continue;
-		}
 
 		omapdss_walk_device(child, true);
 	}
diff --git a/drivers/video/fbdev/omap2/dss/output.c b/drivers/gpu/drm/omapdrm/dss/output.c
index 16072159bd24..16072159bd24 100644
--- a/drivers/video/fbdev/omap2/dss/output.c
+++ b/drivers/gpu/drm/omapdrm/dss/output.c
diff --git a/drivers/video/fbdev/omap2/dss/overlay-sysfs.c b/drivers/gpu/drm/omapdrm/dss/overlay-sysfs.c
index 4cc5ddebfb34..4cc5ddebfb34 100644
--- a/drivers/video/fbdev/omap2/dss/overlay-sysfs.c
+++ b/drivers/gpu/drm/omapdrm/dss/overlay-sysfs.c
diff --git a/drivers/video/fbdev/omap2/dss/overlay.c b/drivers/gpu/drm/omapdrm/dss/overlay.c
index 2f7cee985cdd..2f7cee985cdd 100644
--- a/drivers/video/fbdev/omap2/dss/overlay.c
+++ b/drivers/gpu/drm/omapdrm/dss/overlay.c
diff --git a/drivers/video/fbdev/omap2/dss/pll.c b/drivers/gpu/drm/omapdrm/dss/pll.c
index f974ddcd3b6e..f974ddcd3b6e 100644
--- a/drivers/video/fbdev/omap2/dss/pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/pll.c
diff --git a/drivers/video/fbdev/omap2/dss/rfbi.c b/drivers/gpu/drm/omapdrm/dss/rfbi.c
index 1525a494d057..aea6a1d0fb20 100644
--- a/drivers/video/fbdev/omap2/dss/rfbi.c
+++ b/drivers/gpu/drm/omapdrm/dss/rfbi.c
@@ -880,7 +880,7 @@ static int rfbi_display_enable(struct omap_dss_device *dssdev)
 	struct omap_dss_device *out = &rfbi.output;
 	int r;
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		return -ENODEV;
 	}
diff --git a/drivers/video/fbdev/omap2/dss/sdi.c b/drivers/gpu/drm/omapdrm/dss/sdi.c
index 5843580a1deb..d747cc6b59e1 100644
--- a/drivers/video/fbdev/omap2/dss/sdi.c
+++ b/drivers/gpu/drm/omapdrm/dss/sdi.c
@@ -136,7 +136,7 @@ static int sdi_display_enable(struct omap_dss_device *dssdev)
 	unsigned long pck;
 	int r;
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("failed to enable display: no output/manager\n");
 		return -ENODEV;
 	}
diff --git a/drivers/video/fbdev/omap2/dss/venc.c b/drivers/gpu/drm/omapdrm/dss/venc.c
index d05a54922ba6..08f9def76e27 100644
--- a/drivers/video/fbdev/omap2/dss/venc.c
+++ b/drivers/gpu/drm/omapdrm/dss/venc.c
@@ -515,7 +515,7 @@ static int venc_display_enable(struct omap_dss_device *dssdev)
 
 	mutex_lock(&venc.venc_lock);
 
-	if (out == NULL || out->manager == NULL) {
+	if (out->manager == NULL) {
 		DSSERR("Failed to enable display: no output/manager\n");
 		r = -ENODEV;
 		goto err0;
diff --git a/drivers/video/fbdev/omap2/dss/video-pll.c b/drivers/gpu/drm/omapdrm/dss/video-pll.c
index b1ec59e42940..b1ec59e42940 100644
--- a/drivers/video/fbdev/omap2/dss/video-pll.c
+++ b/drivers/gpu/drm/omapdrm/dss/video-pll.c
diff --git a/drivers/gpu/drm/omapdrm/omap_crtc.c b/drivers/gpu/drm/omapdrm/omap_crtc.c
index ad09590e8a46..2ed0754ed19e 100644
--- a/drivers/gpu/drm/omapdrm/omap_crtc.c
+++ b/drivers/gpu/drm/omapdrm/omap_crtc.c
@@ -524,7 +524,7 @@ struct drm_crtc *omap_crtc_init(struct drm_device *dev,
 	omap_crtc->mgr = omap_dss_get_overlay_manager(channel);
 
 	ret = drm_crtc_init_with_planes(dev, crtc, plane, NULL,
-					&omap_crtc_funcs);
+					&omap_crtc_funcs, NULL);
 	if (ret < 0) {
 		kfree(omap_crtc);
 		return NULL;
diff --git a/drivers/gpu/drm/omapdrm/omap_debugfs.c b/drivers/gpu/drm/omapdrm/omap_debugfs.c
index ee91a25127f9..6f5fc14fc015 100644
--- a/drivers/gpu/drm/omapdrm/omap_debugfs.c
+++ b/drivers/gpu/drm/omapdrm/omap_debugfs.c
@@ -51,6 +51,7 @@ static int mm_show(struct seq_file *m, void *arg)
 	return drm_mm_dump_table(m, &dev->vma_offset_manager->vm_addr_space_mm);
 }
 
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 static int fb_show(struct seq_file *m, void *arg)
 {
 	struct drm_info_node *node = (struct drm_info_node *) m->private;
@@ -73,12 +74,15 @@ static int fb_show(struct seq_file *m, void *arg)
 
 	return 0;
 }
+#endif
 
 /* list of debufs files that are applicable to all devices */
 static struct drm_info_list omap_debugfs_list[] = {
 	{"gem", gem_show, 0},
 	{"mm", mm_show, 0},
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	{"fb", fb_show, 0},
+#endif
 };
 
 /* list of debugfs files that are specific to devices with dmm/tiler */
diff --git a/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c b/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
index 7841970de48d..dfebdc4aa0f2 100644
--- a/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
+++ b/drivers/gpu/drm/omapdrm/omap_dmm_tiler.c
@@ -363,6 +363,7 @@ struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w,
 	u32 min_align = 128;
 	int ret;
 	unsigned long flags;
+	size_t slot_bytes;
 
 	BUG_ON(!validfmt(fmt));
 
@@ -371,13 +372,15 @@ struct tiler_block *tiler_reserve_2d(enum tiler_fmt fmt, uint16_t w,
 	h = DIV_ROUND_UP(h, geom[fmt].slot_h);
 
 	/* convert alignment to slots */
-	min_align = max(min_align, (geom[fmt].slot_w * geom[fmt].cpp));
-	align = ALIGN(align, min_align);
-	align /= geom[fmt].slot_w * geom[fmt].cpp;
+	slot_bytes = geom[fmt].slot_w * geom[fmt].cpp;
+	min_align = max(min_align, slot_bytes);
+	align = (align > min_align) ? ALIGN(align, min_align) : min_align;
+	align /= slot_bytes;
 
 	block->fmt = fmt;
 
-	ret = tcm_reserve_2d(containers[fmt], w, h, align, &block->area);
+	ret = tcm_reserve_2d(containers[fmt], w, h, align, -1, slot_bytes,
+			&block->area);
 	if (ret) {
 		kfree(block);
 		return ERR_PTR(-ENOMEM);
@@ -739,8 +742,7 @@ static int omap_dmm_probe(struct platform_device *dev)
 	   programming during reill operations */
 	for (i = 0; i < omap_dmm->num_lut; i++) {
 		omap_dmm->tcm[i] = sita_init(omap_dmm->container_width,
-						omap_dmm->container_height,
-						NULL);
+						omap_dmm->container_height);
 
 		if (!omap_dmm->tcm[i]) {
 			dev_err(&dev->dev, "failed to allocate container\n");
@@ -1030,4 +1032,3 @@ struct platform_driver omap_dmm_driver = {
 MODULE_LICENSE("GPL v2");
 MODULE_AUTHOR("Andy Gross <andy.gross@ti.com>");
 MODULE_DESCRIPTION("OMAP DMM/Tiler Driver");
-MODULE_ALIAS("platform:" DMM_DRIVER_NAME);
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.c b/drivers/gpu/drm/omapdrm/omap_drv.c
index 5c6609cbb6a2..dfafdb602ad2 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.c
+++ b/drivers/gpu/drm/omapdrm/omap_drv.c
@@ -547,14 +547,19 @@ static int ioctl_set_param(struct drm_device *dev, void *data,
 	return 0;
 }
 
+#define OMAP_BO_USER_MASK	0x00ffffff	/* flags settable by userspace */
+
 static int ioctl_gem_new(struct drm_device *dev, void *data,
 		struct drm_file *file_priv)
 {
 	struct drm_omap_gem_new *args = data;
+	u32 flags = args->flags & OMAP_BO_USER_MASK;
+
 	VERB("%p:%p: size=0x%08x, flags=%08x", dev, file_priv,
-			args->size.bytes, args->flags);
-	return omap_gem_new_handle(dev, file_priv, args->size,
-			args->flags, &args->handle);
+	     args->size.bytes, flags);
+
+	return omap_gem_new_handle(dev, file_priv, args->size, flags,
+				   &args->handle);
 }
 
 static int ioctl_gem_cpu_prep(struct drm_device *dev, void *data,
@@ -692,10 +697,6 @@ static int dev_load(struct drm_device *dev, unsigned long flags)
 		drm_crtc_vblank_off(priv->crtcs[i]);
 
 	priv->fbdev = omap_fbdev_init(dev);
-	if (!priv->fbdev) {
-		dev_warn(dev->dev, "omap_fbdev_init failed\n");
-		/* well, limp along without an fbdev.. maybe X11 will work? */
-	}
 
 	/* store off drm_device for use in pm ops */
 	dev_set_drvdata(dev->dev, dev);
@@ -831,7 +832,8 @@ static const struct file_operations omapdriver_fops = {
 };
 
 static struct drm_driver omap_drm_driver = {
-	.driver_features = DRIVER_MODESET | DRIVER_GEM  | DRIVER_PRIME,
+	.driver_features = DRIVER_MODESET | DRIVER_GEM  | DRIVER_PRIME |
+		DRIVER_ATOMIC,
 	.load = dev_load,
 	.unload = dev_unload,
 	.open = dev_open,
@@ -928,35 +930,23 @@ static struct platform_driver pdev = {
 	.remove = pdev_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&omap_dmm_driver,
+	&pdev,
+};
+
 static int __init omap_drm_init(void)
 {
-	int r;
-
 	DBG("init");
 
-	r = platform_driver_register(&omap_dmm_driver);
-	if (r) {
-		pr_err("DMM driver registration failed\n");
-		return r;
-	}
-
-	r = platform_driver_register(&pdev);
-	if (r) {
-		pr_err("omapdrm driver registration failed\n");
-		platform_driver_unregister(&omap_dmm_driver);
-		return r;
-	}
-
-	return 0;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit omap_drm_fini(void)
 {
 	DBG("fini");
 
-	platform_driver_unregister(&pdev);
-
-	platform_driver_unregister(&omap_dmm_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 /* need late_initcall() so we load after dss_driver's are loaded */
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.h b/drivers/gpu/drm/omapdrm/omap_drv.h
index 5c367aad8a6e..9e0030731c37 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.h
+++ b/drivers/gpu/drm/omapdrm/omap_drv.h
@@ -36,11 +36,7 @@
 
 #define MODULE_NAME     "omapdrm"
 
-/* max # of mapper-id's that can be assigned.. todo, come up with a better
- * (but still inexpensive) way to store/access per-buffer mapper private
- * data..
- */
-#define MAX_MAPPERS 2
+struct omap_drm_usergart;
 
 /* parameters which describe (unrotated) coordinates of scanout within a fb: */
 struct omap_drm_window {
@@ -97,6 +93,7 @@ struct omap_drm_private {
 	/* list of GEM objects: */
 	struct list_head obj_list;
 
+	struct omap_drm_usergart *usergart;
 	bool has_dmm;
 
 	/* properties: */
@@ -138,8 +135,18 @@ void omap_irq_unregister(struct drm_device *dev, struct omap_drm_irq *irq);
 void omap_drm_irq_uninstall(struct drm_device *dev);
 int omap_drm_irq_install(struct drm_device *dev);
 
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 struct drm_fb_helper *omap_fbdev_init(struct drm_device *dev);
 void omap_fbdev_free(struct drm_device *dev);
+#else
+static inline struct drm_fb_helper *omap_fbdev_init(struct drm_device *dev)
+{
+	return NULL;
+}
+static inline void omap_fbdev_free(struct drm_device *dev)
+{
+}
+#endif
 
 struct omap_video_timings *omap_crtc_timings(struct drm_crtc *crtc);
 enum omap_channel omap_crtc_channel(struct drm_crtc *crtc);
@@ -172,9 +179,9 @@ void copy_timings_drm_to_omap(struct omap_video_timings *timings,
 uint32_t omap_framebuffer_get_formats(uint32_t *pixel_formats,
 		uint32_t max_formats, enum omap_color_mode supported_modes);
 struct drm_framebuffer *omap_framebuffer_create(struct drm_device *dev,
-		struct drm_file *file, struct drm_mode_fb_cmd2 *mode_cmd);
+		struct drm_file *file, const struct drm_mode_fb_cmd2 *mode_cmd);
 struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
-		struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos);
+		const struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos);
 struct drm_gem_object *omap_framebuffer_bo(struct drm_framebuffer *fb, int p);
 int omap_framebuffer_pin(struct drm_framebuffer *fb);
 void omap_framebuffer_unpin(struct drm_framebuffer *fb);
@@ -248,7 +255,7 @@ struct omap_dss_device *omap_encoder_get_dssdev(struct drm_encoder *encoder);
 
 static inline int objects_lookup(struct drm_device *dev,
 		struct drm_file *filp, uint32_t pixel_format,
-		struct drm_gem_object **bos, uint32_t *handles)
+		struct drm_gem_object **bos, const uint32_t *handles)
 {
 	int i, n = drm_format_num_planes(pixel_format);
 
diff --git a/drivers/gpu/drm/omapdrm/omap_encoder.c b/drivers/gpu/drm/omapdrm/omap_encoder.c
index 7d9b32a0eb43..61714e9670ae 100644
--- a/drivers/gpu/drm/omapdrm/omap_encoder.c
+++ b/drivers/gpu/drm/omapdrm/omap_encoder.c
@@ -110,8 +110,6 @@ static int omap_encoder_update(struct drm_encoder *encoder,
 	struct omap_dss_driver *dssdrv = dssdev->driver;
 	int ret;
 
-	dssdev->src->manager = omap_dss_get_overlay_manager(channel);
-
 	if (dssdrv->check_timings) {
 		ret = dssdrv->check_timings(dssdev, timings);
 	} else {
@@ -178,7 +176,7 @@ struct drm_encoder *omap_encoder_init(struct drm_device *dev,
 	encoder = &omap_encoder->base;
 
 	drm_encoder_init(dev, encoder, &omap_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 	drm_encoder_helper_add(encoder, &omap_encoder_helper_funcs);
 
 	return encoder;
diff --git a/drivers/gpu/drm/omapdrm/omap_fb.c b/drivers/gpu/drm/omapdrm/omap_fb.c
index 636a1f921569..ad202dfc1a49 100644
--- a/drivers/gpu/drm/omapdrm/omap_fb.c
+++ b/drivers/gpu/drm/omapdrm/omap_fb.c
@@ -364,7 +364,7 @@ void omap_framebuffer_describe(struct drm_framebuffer *fb, struct seq_file *m)
 #endif
 
 struct drm_framebuffer *omap_framebuffer_create(struct drm_device *dev,
-		struct drm_file *file, struct drm_mode_fb_cmd2 *mode_cmd)
+		struct drm_file *file, const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *bos[4];
 	struct drm_framebuffer *fb;
@@ -386,7 +386,7 @@ struct drm_framebuffer *omap_framebuffer_create(struct drm_device *dev,
 }
 
 struct drm_framebuffer *omap_framebuffer_init(struct drm_device *dev,
-		struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos)
+		const struct drm_mode_fb_cmd2 *mode_cmd, struct drm_gem_object **bos)
 {
 	struct omap_framebuffer *omap_fb = NULL;
 	struct drm_framebuffer *fb = NULL;
diff --git a/drivers/gpu/drm/omapdrm/omap_fbdev.c b/drivers/gpu/drm/omapdrm/omap_fbdev.c
index 24f92bea39c7..3cb16f0cf381 100644
--- a/drivers/gpu/drm/omapdrm/omap_fbdev.c
+++ b/drivers/gpu/drm/omapdrm/omap_fbdev.c
@@ -295,6 +295,10 @@ fini:
 	drm_fb_helper_fini(helper);
 fail:
 	kfree(fbdev);
+
+	dev_warn(dev->dev, "omap_fbdev_init failed\n");
+	/* well, limp along without an fbdev.. maybe X11 will work? */
+
 	return NULL;
 }
 
diff --git a/drivers/gpu/drm/omapdrm/omap_gem.c b/drivers/gpu/drm/omapdrm/omap_gem.c
index 7ed08fdc4c42..8495a1a4b617 100644
--- a/drivers/gpu/drm/omapdrm/omap_gem.c
+++ b/drivers/gpu/drm/omapdrm/omap_gem.c
@@ -19,30 +19,22 @@
 
 #include <linux/shmem_fs.h>
 #include <linux/spinlock.h>
+#include <linux/pfn_t.h>
 
 #include <drm/drm_vma_manager.h>
 
 #include "omap_drv.h"
 #include "omap_dmm_tiler.h"
 
-/* remove these once drm core helpers are merged */
-struct page **_drm_gem_get_pages(struct drm_gem_object *obj, gfp_t gfpmask);
-void _drm_gem_put_pages(struct drm_gem_object *obj, struct page **pages,
-		bool dirty, bool accessed);
-int _drm_gem_create_mmap_offset_size(struct drm_gem_object *obj, size_t size);
-
 /*
  * GEM buffer object implementation.
  */
 
-#define to_omap_bo(x) container_of(x, struct omap_gem_object, base)
-
 /* note: we use upper 8 bits of flags for driver-internal flags: */
-#define OMAP_BO_DMA			0x01000000	/* actually is physically contiguous */
+#define OMAP_BO_DMA		0x01000000	/* actually is physically contiguous */
 #define OMAP_BO_EXT_SYNC	0x02000000	/* externally allocated sync object */
 #define OMAP_BO_EXT_MEM		0x04000000	/* externally allocated memory */
 
-
 struct omap_gem_object {
 	struct drm_gem_object base;
 
@@ -119,8 +111,7 @@ struct omap_gem_object {
 	} *sync;
 };
 
-static int get_pages(struct drm_gem_object *obj, struct page ***pages);
-static uint64_t mmap_offset(struct drm_gem_object *obj);
+#define to_omap_bo(x) container_of(x, struct omap_gem_object, base)
 
 /* To deal with userspace mmap'ings of 2d tiled buffers, which (a) are
  * not necessarily pinned in TILER all the time, and (b) when they are
@@ -134,27 +125,69 @@ static uint64_t mmap_offset(struct drm_gem_object *obj);
  * for later..
  */
 #define NUM_USERGART_ENTRIES 2
-struct usergart_entry {
+struct omap_drm_usergart_entry {
 	struct tiler_block *block;	/* the reserved tiler block */
 	dma_addr_t paddr;
 	struct drm_gem_object *obj;	/* the current pinned obj */
 	pgoff_t obj_pgoff;		/* page offset of obj currently
 					   mapped in */
 };
-static struct {
-	struct usergart_entry entry[NUM_USERGART_ENTRIES];
+
+struct omap_drm_usergart {
+	struct omap_drm_usergart_entry entry[NUM_USERGART_ENTRIES];
 	int height;				/* height in rows */
 	int height_shift;		/* ilog2(height in rows) */
 	int slot_shift;			/* ilog2(width per slot) */
 	int stride_pfn;			/* stride in pages */
 	int last;				/* index of last used entry */
-} *usergart;
+};
+
+/* -----------------------------------------------------------------------------
+ * Helpers
+ */
+
+/** get mmap offset */
+static uint64_t mmap_offset(struct drm_gem_object *obj)
+{
+	struct drm_device *dev = obj->dev;
+	int ret;
+	size_t size;
+
+	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
+
+	/* Make it mmapable */
+	size = omap_gem_mmap_size(obj);
+	ret = drm_gem_create_mmap_offset_size(obj, size);
+	if (ret) {
+		dev_err(dev->dev, "could not allocate mmap offset\n");
+		return 0;
+	}
+
+	return drm_vma_node_offset_addr(&obj->vma_node);
+}
+
+/* GEM objects can either be allocated from contiguous memory (in which
+ * case obj->filp==NULL), or w/ shmem backing (obj->filp!=NULL).  But non
+ * contiguous buffers can be remapped in TILER/DMM if they need to be
+ * contiguous... but we don't do this all the time to reduce pressure
+ * on TILER/DMM space when we know at allocation time that the buffer
+ * will need to be scanned out.
+ */
+static inline bool is_shmem(struct drm_gem_object *obj)
+{
+	return obj->filp != NULL;
+}
+
+/* -----------------------------------------------------------------------------
+ * Eviction
+ */
 
 static void evict_entry(struct drm_gem_object *obj,
-		enum tiler_fmt fmt, struct usergart_entry *entry)
+		enum tiler_fmt fmt, struct omap_drm_usergart_entry *entry)
 {
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
-	int n = usergart[fmt].height;
+	struct omap_drm_private *priv = obj->dev->dev_private;
+	int n = priv->usergart[fmt].height;
 	size_t size = PAGE_SIZE * n;
 	loff_t off = mmap_offset(obj) +
 			(entry->obj_pgoff << PAGE_SHIFT);
@@ -180,46 +213,25 @@ static void evict_entry(struct drm_gem_object *obj,
 static void evict(struct drm_gem_object *obj)
 {
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	struct omap_drm_private *priv = obj->dev->dev_private;
 
 	if (omap_obj->flags & OMAP_BO_TILED) {
 		enum tiler_fmt fmt = gem2fmt(omap_obj->flags);
 		int i;
 
-		if (!usergart)
-			return;
-
 		for (i = 0; i < NUM_USERGART_ENTRIES; i++) {
-			struct usergart_entry *entry = &usergart[fmt].entry[i];
+			struct omap_drm_usergart_entry *entry =
+				&priv->usergart[fmt].entry[i];
+
 			if (entry->obj == obj)
 				evict_entry(obj, fmt, entry);
 		}
 	}
 }
 
-/* GEM objects can either be allocated from contiguous memory (in which
- * case obj->filp==NULL), or w/ shmem backing (obj->filp!=NULL).  But non
- * contiguous buffers can be remapped in TILER/DMM if they need to be
- * contiguous... but we don't do this all the time to reduce pressure
- * on TILER/DMM space when we know at allocation time that the buffer
- * will need to be scanned out.
- */
-static inline bool is_shmem(struct drm_gem_object *obj)
-{
-	return obj->filp != NULL;
-}
-
-/**
- * shmem buffers that are mapped cached can simulate coherency via using
- * page faulting to keep track of dirty pages
+/* -----------------------------------------------------------------------------
+ * Page Management
  */
-static inline bool is_cached_coherent(struct drm_gem_object *obj)
-{
-	struct omap_gem_object *omap_obj = to_omap_bo(obj);
-	return is_shmem(obj) &&
-		((omap_obj->flags & OMAP_BO_CACHE_MASK) == OMAP_BO_CACHED);
-}
-
-static DEFINE_SPINLOCK(sync_lock);
 
 /** ensure backing pages are allocated */
 static int omap_gem_attach_pages(struct drm_gem_object *obj)
@@ -272,6 +284,28 @@ free_pages:
 	return ret;
 }
 
+/* acquire pages when needed (for example, for DMA where physically
+ * contiguous buffer is not required
+ */
+static int get_pages(struct drm_gem_object *obj, struct page ***pages)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	int ret = 0;
+
+	if (is_shmem(obj) && !omap_obj->pages) {
+		ret = omap_gem_attach_pages(obj);
+		if (ret) {
+			dev_err(obj->dev->dev, "could not attach pages\n");
+			return ret;
+		}
+	}
+
+	/* TODO: even phys-contig.. we should have a list of pages? */
+	*pages = omap_obj->pages;
+
+	return 0;
+}
+
 /** release backing pages */
 static void omap_gem_detach_pages(struct drm_gem_object *obj)
 {
@@ -301,26 +335,6 @@ uint32_t omap_gem_flags(struct drm_gem_object *obj)
 	return to_omap_bo(obj)->flags;
 }
 
-/** get mmap offset */
-static uint64_t mmap_offset(struct drm_gem_object *obj)
-{
-	struct drm_device *dev = obj->dev;
-	int ret;
-	size_t size;
-
-	WARN_ON(!mutex_is_locked(&dev->struct_mutex));
-
-	/* Make it mmapable */
-	size = omap_gem_mmap_size(obj);
-	ret = drm_gem_create_mmap_offset_size(obj, size);
-	if (ret) {
-		dev_err(dev->dev, "could not allocate mmap offset\n");
-		return 0;
-	}
-
-	return drm_vma_node_offset_addr(&obj->vma_node);
-}
-
 uint64_t omap_gem_mmap_offset(struct drm_gem_object *obj)
 {
 	uint64_t offset;
@@ -362,6 +376,10 @@ int omap_gem_tiled_size(struct drm_gem_object *obj, uint16_t *w, uint16_t *h)
 	return -EINVAL;
 }
 
+/* -----------------------------------------------------------------------------
+ * Fault Handling
+ */
+
 /* Normal handling for the case of faulting in non-tiled buffers */
 static int fault_1d(struct drm_gem_object *obj,
 		struct vm_area_struct *vma, struct vm_fault *vmf)
@@ -385,7 +403,8 @@ static int fault_1d(struct drm_gem_object *obj,
 	VERB("Inserting %p pfn %lx, pa %lx", vmf->virtual_address,
 			pfn, pfn << PAGE_SHIFT);
 
-	return vm_insert_mixed(vma, (unsigned long)vmf->virtual_address, pfn);
+	return vm_insert_mixed(vma, (unsigned long)vmf->virtual_address,
+			__pfn_to_pfn_t(pfn, PFN_DEV));
 }
 
 /* Special handling for the case of faulting in 2d tiled buffers */
@@ -393,7 +412,8 @@ static int fault_2d(struct drm_gem_object *obj,
 		struct vm_area_struct *vma, struct vm_fault *vmf)
 {
 	struct omap_gem_object *omap_obj = to_omap_bo(obj);
-	struct usergart_entry *entry;
+	struct omap_drm_private *priv = obj->dev->dev_private;
+	struct omap_drm_usergart_entry *entry;
 	enum tiler_fmt fmt = gem2fmt(omap_obj->flags);
 	struct page *pages[64];  /* XXX is this too much to have on stack? */
 	unsigned long pfn;
@@ -406,8 +426,8 @@ static int fault_2d(struct drm_gem_object *obj,
 	 * that need to be mapped in to fill 4kb wide CPU page.  If the slot
 	 * height is 64, then 64 pages fill a 4kb wide by 64 row region.
 	 */
-	const int n = usergart[fmt].height;
-	const int n_shift = usergart[fmt].height_shift;
+	const int n = priv->usergart[fmt].height;
+	const int n_shift = priv->usergart[fmt].height_shift;
 
 	/*
 	 * If buffer width in bytes > PAGE_SIZE then the virtual stride is
@@ -428,11 +448,11 @@ static int fault_2d(struct drm_gem_object *obj,
 	base_pgoff = round_down(pgoff, m << n_shift);
 
 	/* figure out buffer width in slots */
-	slots = omap_obj->width >> usergart[fmt].slot_shift;
+	slots = omap_obj->width >> priv->usergart[fmt].slot_shift;
 
 	vaddr = vmf->virtual_address - ((pgoff - base_pgoff) << PAGE_SHIFT);
 
-	entry = &usergart[fmt].entry[usergart[fmt].last];
+	entry = &priv->usergart[fmt].entry[priv->usergart[fmt].last];
 
 	/* evict previous buffer using this usergart entry, if any: */
 	if (entry->obj)
@@ -478,13 +498,15 @@ static int fault_2d(struct drm_gem_object *obj,
 			pfn, pfn << PAGE_SHIFT);
 
 	for (i = n; i > 0; i--) {
-		vm_insert_mixed(vma, (unsigned long)vaddr, pfn);
-		pfn += usergart[fmt].stride_pfn;
+		vm_insert_mixed(vma, (unsigned long)vaddr,
+				__pfn_to_pfn_t(pfn, PFN_DEV));
+		pfn += priv->usergart[fmt].stride_pfn;
 		vaddr += PAGE_SIZE * m;
 	}
 
 	/* simple round-robin: */
-	usergart[fmt].last = (usergart[fmt].last + 1) % NUM_USERGART_ENTRIES;
+	priv->usergart[fmt].last = (priv->usergart[fmt].last + 1)
+				 % NUM_USERGART_ENTRIES;
 
 	return 0;
 }
@@ -596,6 +618,9 @@ int omap_gem_mmap_obj(struct drm_gem_object *obj,
 	return 0;
 }
 
+/* -----------------------------------------------------------------------------
+ * Dumb Buffers
+ */
 
 /**
  * omap_gem_dumb_create	-	create a dumb buffer
@@ -653,6 +678,7 @@ fail:
 	return ret;
 }
 
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 /* Set scrolling position.  This allows us to implement fast scrolling
  * for console.
  *
@@ -689,6 +715,22 @@ fail:
 
 	return ret;
 }
+#endif
+
+/* -----------------------------------------------------------------------------
+ * Memory Management & DMA Sync
+ */
+
+/**
+ * shmem buffers that are mapped cached can simulate coherency via using
+ * page faulting to keep track of dirty pages
+ */
+static inline bool is_cached_coherent(struct drm_gem_object *obj)
+{
+	struct omap_gem_object *omap_obj = to_omap_bo(obj);
+	return is_shmem(obj) &&
+		((omap_obj->flags & OMAP_BO_CACHE_MASK) == OMAP_BO_CACHED);
+}
 
 /* Sync the buffer for CPU access.. note pages should already be
  * attached, ie. omap_gem_get_pages()
@@ -865,28 +907,6 @@ int omap_gem_tiled_stride(struct drm_gem_object *obj, uint32_t orient)
 	return ret;
 }
 
-/* acquire pages when needed (for example, for DMA where physically
- * contiguous buffer is not required
- */
-static int get_pages(struct drm_gem_object *obj, struct page ***pages)
-{
-	struct omap_gem_object *omap_obj = to_omap_bo(obj);
-	int ret = 0;
-
-	if (is_shmem(obj) && !omap_obj->pages) {
-		ret = omap_gem_attach_pages(obj);
-		if (ret) {
-			dev_err(obj->dev->dev, "could not attach pages\n");
-			return ret;
-		}
-	}
-
-	/* TODO: even phys-contig.. we should have a list of pages? */
-	*pages = omap_obj->pages;
-
-	return 0;
-}
-
 /* if !remap, and we don't have pages backing, then fail, rather than
  * increasing the pin count (which we don't really do yet anyways,
  * because we don't support swapping pages back out).  And 'remap'
@@ -924,6 +944,7 @@ int omap_gem_put_pages(struct drm_gem_object *obj)
 	return 0;
 }
 
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 /* Get kernel virtual address for CPU access.. this more or less only
  * exists for omap_fbdev.  This should be called with struct_mutex
  * held.
@@ -942,6 +963,11 @@ void *omap_gem_vaddr(struct drm_gem_object *obj)
 	}
 	return omap_obj->vaddr;
 }
+#endif
+
+/* -----------------------------------------------------------------------------
+ * Power Management
+ */
 
 #ifdef CONFIG_PM
 /* re-pin objects in DMM in resume path: */
@@ -971,6 +997,10 @@ int omap_gem_resume(struct device *dev)
 }
 #endif
 
+/* -----------------------------------------------------------------------------
+ * DebugFS
+ */
+
 #ifdef CONFIG_DEBUG_FS
 void omap_gem_describe(struct drm_gem_object *obj, struct seq_file *m)
 {
@@ -1017,9 +1047,12 @@ void omap_gem_describe_objects(struct list_head *list, struct seq_file *m)
 }
 #endif
 
-/* Buffer Synchronization:
+/* -----------------------------------------------------------------------------
+ * Buffer Synchronization
  */
 
+static DEFINE_SPINLOCK(sync_lock);
+
 struct omap_gem_sync_waiter {
 	struct list_head list;
 	struct omap_gem_object *omap_obj;
@@ -1265,6 +1298,10 @@ unlock:
 	return ret;
 }
 
+/* -----------------------------------------------------------------------------
+ * Constructor & Destructor
+ */
+
 /* don't call directly.. called from GEM core when it is time to actually
  * free the object..
  */
@@ -1282,8 +1319,6 @@ void omap_gem_free_object(struct drm_gem_object *obj)
 	list_del(&omap_obj->mm_list);
 	spin_unlock(&priv->list_lock);
 
-	drm_gem_free_mmap_offset(obj);
-
 	/* this means the object is still pinned.. which really should
 	 * not happen.  I think..
 	 */
@@ -1308,31 +1343,7 @@ void omap_gem_free_object(struct drm_gem_object *obj)
 
 	drm_gem_object_release(obj);
 
-	kfree(obj);
-}
-
-/* convenience method to construct a GEM buffer object, and userspace handle */
-int omap_gem_new_handle(struct drm_device *dev, struct drm_file *file,
-		union omap_gem_size gsize, uint32_t flags, uint32_t *handle)
-{
-	struct drm_gem_object *obj;
-	int ret;
-
-	obj = omap_gem_new(dev, gsize, flags);
-	if (!obj)
-		return -ENOMEM;
-
-	ret = drm_gem_handle_create(file, obj, handle);
-	if (ret) {
-		drm_gem_object_release(obj);
-		kfree(obj); /* TODO isn't there a dtor to call? just copying i915 */
-		return ret;
-	}
-
-	/* drop reference from allocate - handle holds it now */
-	drm_gem_object_unreference_unlocked(obj);
-
-	return 0;
+	kfree(omap_obj);
 }
 
 /* GEM buffer object constructor */
@@ -1341,15 +1352,15 @@ struct drm_gem_object *omap_gem_new(struct drm_device *dev,
 {
 	struct omap_drm_private *priv = dev->dev_private;
 	struct omap_gem_object *omap_obj;
-	struct drm_gem_object *obj = NULL;
+	struct drm_gem_object *obj;
 	struct address_space *mapping;
 	size_t size;
 	int ret;
 
 	if (flags & OMAP_BO_TILED) {
-		if (!usergart) {
+		if (!priv->usergart) {
 			dev_err(dev->dev, "Tiled buffers require DMM\n");
-			goto fail;
+			return NULL;
 		}
 
 		/* tiled buffers are always shmem paged backed.. when they are
@@ -1420,16 +1431,42 @@ struct drm_gem_object *omap_gem_new(struct drm_device *dev,
 	return obj;
 
 fail:
-	if (obj)
+	omap_gem_free_object(obj);
+	return NULL;
+}
+
+/* convenience method to construct a GEM buffer object, and userspace handle */
+int omap_gem_new_handle(struct drm_device *dev, struct drm_file *file,
+		union omap_gem_size gsize, uint32_t flags, uint32_t *handle)
+{
+	struct drm_gem_object *obj;
+	int ret;
+
+	obj = omap_gem_new(dev, gsize, flags);
+	if (!obj)
+		return -ENOMEM;
+
+	ret = drm_gem_handle_create(file, obj, handle);
+	if (ret) {
 		omap_gem_free_object(obj);
+		return ret;
+	}
 
-	return NULL;
+	/* drop reference from allocate - handle holds it now */
+	drm_gem_object_unreference_unlocked(obj);
+
+	return 0;
 }
 
-/* init/cleanup.. if DMM is used, we need to set some stuff up.. */
+/* -----------------------------------------------------------------------------
+ * Init & Cleanup
+ */
+
+/* If DMM is used, we need to set some stuff up.. */
 void omap_gem_init(struct drm_device *dev)
 {
 	struct omap_drm_private *priv = dev->dev_private;
+	struct omap_drm_usergart *usergart;
 	const enum tiler_fmt fmts[] = {
 			TILFMT_8BIT, TILFMT_16BIT, TILFMT_32BIT
 	};
@@ -1458,10 +1495,11 @@ void omap_gem_init(struct drm_device *dev)
 		usergart[i].stride_pfn = tiler_stride(fmts[i], 0) >> PAGE_SHIFT;
 		usergart[i].slot_shift = ilog2((PAGE_SIZE / h) >> i);
 		for (j = 0; j < NUM_USERGART_ENTRIES; j++) {
-			struct usergart_entry *entry = &usergart[i].entry[j];
-			struct tiler_block *block =
-					tiler_reserve_2d(fmts[i], w, h,
-							PAGE_SIZE);
+			struct omap_drm_usergart_entry *entry;
+			struct tiler_block *block;
+
+			entry = &usergart[i].entry[j];
+			block = tiler_reserve_2d(fmts[i], w, h, PAGE_SIZE);
 			if (IS_ERR(block)) {
 				dev_err(dev->dev,
 						"reserve failed: %d, %d, %ld\n",
@@ -1477,13 +1515,16 @@ void omap_gem_init(struct drm_device *dev)
 		}
 	}
 
+	priv->usergart = usergart;
 	priv->has_dmm = true;
 }
 
 void omap_gem_deinit(struct drm_device *dev)
 {
+	struct omap_drm_private *priv = dev->dev_private;
+
 	/* I believe we can rely on there being no more outstanding GEM
 	 * objects which could depend on usergart/dmm at this point.
 	 */
-	kfree(usergart);
+	kfree(priv->usergart);
 }
diff --git a/drivers/gpu/drm/omapdrm/omap_plane.c b/drivers/gpu/drm/omapdrm/omap_plane.c
index 3054bda72688..d75b197eff46 100644
--- a/drivers/gpu/drm/omapdrm/omap_plane.c
+++ b/drivers/gpu/drm/omapdrm/omap_plane.c
@@ -188,33 +188,6 @@ static const struct drm_plane_helper_funcs omap_plane_helper_funcs = {
 	.atomic_disable = omap_plane_atomic_disable,
 };
 
-static void omap_plane_reset(struct drm_plane *plane)
-{
-	struct omap_plane *omap_plane = to_omap_plane(plane);
-	struct omap_plane_state *omap_state;
-
-	if (plane->state && plane->state->fb)
-		drm_framebuffer_unreference(plane->state->fb);
-
-	kfree(plane->state);
-	plane->state = NULL;
-
-	omap_state = kzalloc(sizeof(*omap_state), GFP_KERNEL);
-	if (omap_state == NULL)
-		return;
-
-	/*
-	 * Set defaults depending on whether we are a primary or overlay
-	 * plane.
-	 */
-	omap_state->zorder = plane->type == DRM_PLANE_TYPE_PRIMARY
-			   ? 0 : omap_plane->id;
-	omap_state->base.rotation = BIT(DRM_ROTATE_0);
-
-	plane->state = &omap_state->base;
-	plane->state->plane = plane;
-}
-
 static void omap_plane_destroy(struct drm_plane *plane)
 {
 	struct omap_plane *omap_plane = to_omap_plane(plane);
@@ -270,6 +243,32 @@ static void omap_plane_atomic_destroy_state(struct drm_plane *plane,
 	kfree(to_omap_plane_state(state));
 }
 
+static void omap_plane_reset(struct drm_plane *plane)
+{
+	struct omap_plane *omap_plane = to_omap_plane(plane);
+	struct omap_plane_state *omap_state;
+
+	if (plane->state) {
+		omap_plane_atomic_destroy_state(plane, plane->state);
+		plane->state = NULL;
+	}
+
+	omap_state = kzalloc(sizeof(*omap_state), GFP_KERNEL);
+	if (omap_state == NULL)
+		return;
+
+	/*
+	 * Set defaults depending on whether we are a primary or overlay
+	 * plane.
+	 */
+	omap_state->zorder = plane->type == DRM_PLANE_TYPE_PRIMARY
+			   ? 0 : omap_plane->id;
+	omap_state->base.rotation = BIT(DRM_ROTATE_0);
+
+	plane->state = &omap_state->base;
+	plane->state->plane = plane;
+}
+
 static int omap_plane_atomic_set_property(struct drm_plane *plane,
 					  struct drm_plane_state *state,
 					  struct drm_property *property,
@@ -366,7 +365,7 @@ struct drm_plane *omap_plane_init(struct drm_device *dev,
 
 	ret = drm_universal_plane_init(dev, plane, (1 << priv->num_crtcs) - 1,
 				       &omap_plane_funcs, omap_plane->formats,
-				       omap_plane->nformats, type);
+				       omap_plane->nformats, type, NULL);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/gpu/drm/omapdrm/tcm-sita.c b/drivers/gpu/drm/omapdrm/tcm-sita.c
index efb609510540..c10fdfc0930f 100644
--- a/drivers/gpu/drm/omapdrm/tcm-sita.c
+++ b/drivers/gpu/drm/omapdrm/tcm-sita.c
@@ -5,8 +5,9 @@
  *
  * Authors: Ravi Ramachandra <r.ramachandra@ti.com>,
  *          Lajos Molnar <molnar@ti.com>
+ *          Andy Gross <andy.gross@ti.com>
  *
- * Copyright (C) 2009-2010 Texas Instruments, Inc.
+ * Copyright (C) 2012 Texas Instruments, Inc.
  *
  * This package is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -17,687 +18,244 @@
  * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
  *
  */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/bitmap.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
+#include "tcm.h"
 
-#include "tcm-sita.h"
-
-#define ALIGN_DOWN(value, align) ((value) & ~((align) - 1))
-
-/* Individual selection criteria for different scan areas */
-static s32 CR_L2R_T2B = CR_BIAS_HORIZONTAL;
-static s32 CR_R2L_T2B = CR_DIAGONAL_BALANCE;
-
-/*********************************************
- *	TCM API - Sita Implementation
- *********************************************/
-static s32 sita_reserve_2d(struct tcm *tcm, u16 h, u16 w, u8 align,
-			   struct tcm_area *area);
-static s32 sita_reserve_1d(struct tcm *tcm, u32 slots, struct tcm_area *area);
-static s32 sita_free(struct tcm *tcm, struct tcm_area *area);
-static void sita_deinit(struct tcm *tcm);
-
-/*********************************************
- *	Main Scanner functions
- *********************************************/
-static s32 scan_areas_and_find_fit(struct tcm *tcm, u16 w, u16 h, u16 align,
-				   struct tcm_area *area);
-
-static s32 scan_l2r_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
-			struct tcm_area *field, struct tcm_area *area);
-
-static s32 scan_r2l_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
-			struct tcm_area *field, struct tcm_area *area);
-
-static s32 scan_r2l_b2t_one_dim(struct tcm *tcm, u32 num_slots,
-			struct tcm_area *field, struct tcm_area *area);
-
-/*********************************************
- *	Support Infrastructure Methods
- *********************************************/
-static s32 is_area_free(struct tcm_area ***map, u16 x0, u16 y0, u16 w, u16 h);
-
-static s32 update_candidate(struct tcm *tcm, u16 x0, u16 y0, u16 w, u16 h,
-			    struct tcm_area *field, s32 criteria,
-			    struct score *best);
-
-static void get_nearness_factor(struct tcm_area *field,
-				struct tcm_area *candidate,
-				struct nearness_factor *nf);
-
-static void get_neighbor_stats(struct tcm *tcm, struct tcm_area *area,
-			       struct neighbor_stats *stat);
-
-static void fill_area(struct tcm *tcm,
-				struct tcm_area *area, struct tcm_area *parent);
-
-
-/*********************************************/
-
-/*********************************************
- *	Utility Methods
- *********************************************/
-struct tcm *sita_init(u16 width, u16 height, struct tcm_pt *attr)
-{
-	struct tcm *tcm;
-	struct sita_pvt *pvt;
-	struct tcm_area area = {0};
-	s32 i;
-
-	if (width == 0 || height == 0)
-		return NULL;
-
-	tcm = kmalloc(sizeof(*tcm), GFP_KERNEL);
-	pvt = kmalloc(sizeof(*pvt), GFP_KERNEL);
-	if (!tcm || !pvt)
-		goto error;
-
-	memset(tcm, 0, sizeof(*tcm));
-	memset(pvt, 0, sizeof(*pvt));
-
-	/* Updating the pointers to SiTA implementation APIs */
-	tcm->height = height;
-	tcm->width = width;
-	tcm->reserve_2d = sita_reserve_2d;
-	tcm->reserve_1d = sita_reserve_1d;
-	tcm->free = sita_free;
-	tcm->deinit = sita_deinit;
-	tcm->pvt = (void *)pvt;
-
-	spin_lock_init(&(pvt->lock));
-
-	/* Creating tam map */
-	pvt->map = kmalloc(sizeof(*pvt->map) * tcm->width, GFP_KERNEL);
-	if (!pvt->map)
-		goto error;
-
-	for (i = 0; i < tcm->width; i++) {
-		pvt->map[i] =
-			kmalloc(sizeof(**pvt->map) * tcm->height,
-								GFP_KERNEL);
-		if (pvt->map[i] == NULL) {
-			while (i--)
-				kfree(pvt->map[i]);
-			kfree(pvt->map);
-			goto error;
-		}
-	}
-
-	if (attr && attr->x <= tcm->width && attr->y <= tcm->height) {
-		pvt->div_pt.x = attr->x;
-		pvt->div_pt.y = attr->y;
-
-	} else {
-		/* Defaulting to 3:1 ratio on width for 2D area split */
-		/* Defaulting to 3:1 ratio on height for 2D and 1D split */
-		pvt->div_pt.x = (tcm->width * 3) / 4;
-		pvt->div_pt.y = (tcm->height * 3) / 4;
-	}
-
-	spin_lock(&(pvt->lock));
-	assign(&area, 0, 0, width - 1, height - 1);
-	fill_area(tcm, &area, NULL);
-	spin_unlock(&(pvt->lock));
-	return tcm;
-
-error:
-	kfree(tcm);
-	kfree(pvt);
-	return NULL;
-}
-
-static void sita_deinit(struct tcm *tcm)
-{
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-	struct tcm_area area = {0};
-	s32 i;
-
-	area.p1.x = tcm->width - 1;
-	area.p1.y = tcm->height - 1;
-
-	spin_lock(&(pvt->lock));
-	fill_area(tcm, &area, NULL);
-	spin_unlock(&(pvt->lock));
-
-	for (i = 0; i < tcm->height; i++)
-		kfree(pvt->map[i]);
-	kfree(pvt->map);
-	kfree(pvt);
-}
-
-/**
- * Reserve a 1D area in the container
- *
- * @param num_slots	size of 1D area
- * @param area		pointer to the area that will be populated with the
- *			reserved area
- *
- * @return 0 on success, non-0 error value on failure.
+static unsigned long mask[8];
+/*
+ * pos		position in bitmap
+ * w		width in slots
+ * h		height in slots
+ * map		ptr to bitmap
+ * stride		slots in a row
  */
-static s32 sita_reserve_1d(struct tcm *tcm, u32 num_slots,
-			   struct tcm_area *area)
+static void free_slots(unsigned long pos, uint16_t w, uint16_t h,
+		unsigned long *map, uint16_t stride)
 {
-	s32 ret;
-	struct tcm_area field = {0};
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-
-	spin_lock(&(pvt->lock));
-
-	/* Scanning entire container */
-	assign(&field, tcm->width - 1, tcm->height - 1, 0, 0);
+	int i;
 
-	ret = scan_r2l_b2t_one_dim(tcm, num_slots, &field, area);
-	if (!ret)
-		/* update map */
-		fill_area(tcm, area, area);
-
-	spin_unlock(&(pvt->lock));
-	return ret;
+	for (i = 0; i < h; i++, pos += stride)
+		bitmap_clear(map, pos, w);
 }
 
-/**
- * Reserve a 2D area in the container
- *
- * @param w	width
- * @param h	height
- * @param area	pointer to the area that will be populated with the reserved
- *		area
- *
- * @return 0 on success, non-0 error value on failure.
+/*
+ * w		width in slots
+ * pos		ptr to position
+ * map		ptr to bitmap
+ * num_bits	number of bits in bitmap
  */
-static s32 sita_reserve_2d(struct tcm *tcm, u16 h, u16 w, u8 align,
-			   struct tcm_area *area)
+static int r2l_b2t_1d(uint16_t w, unsigned long *pos, unsigned long *map,
+		size_t num_bits)
 {
-	s32 ret;
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
+	unsigned long search_count = 0;
+	unsigned long bit;
+	bool area_found = false;
 
-	/* not supporting more than 64 as alignment */
-	if (align > 64)
-		return -EINVAL;
+	*pos = num_bits - w;
 
-	/* we prefer 1, 32 and 64 as alignment */
-	align = align <= 1 ? 1 : align <= 32 ? 32 : 64;
+	while (search_count < num_bits) {
+		bit = find_next_bit(map, num_bits, *pos);
 
-	spin_lock(&(pvt->lock));
-	ret = scan_areas_and_find_fit(tcm, w, h, align, area);
-	if (!ret)
-		/* update map */
-		fill_area(tcm, area, area);
+		if (bit - *pos >= w) {
+			/* found a long enough free area */
+			bitmap_set(map, *pos, w);
+			area_found = true;
+			break;
+		}
 
-	spin_unlock(&(pvt->lock));
-	return ret;
+		search_count = num_bits - bit + w;
+		*pos = bit - w;
+	}
+
+	return (area_found) ? 0 : -ENOMEM;
 }
 
-/**
- * Unreserve a previously allocated 2D or 1D area
- * @param area	area to be freed
- * @return 0 - success
+/*
+ * w = width in slots
+ * h = height in slots
+ * a = align in slots	(mask, 2^n-1, 0 is unaligned)
+ * offset = offset in bytes from 4KiB
+ * pos = position in bitmap for buffer
+ * map = bitmap ptr
+ * num_bits = size of bitmap
+ * stride = bits in one row of container
  */
-static s32 sita_free(struct tcm *tcm, struct tcm_area *area)
+static int l2r_t2b(uint16_t w, uint16_t h, uint16_t a, int16_t offset,
+		unsigned long *pos, unsigned long slot_bytes,
+		unsigned long *map, size_t num_bits, size_t slot_stride)
 {
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-
-	spin_lock(&(pvt->lock));
+	int i;
+	unsigned long index;
+	bool area_free;
+	unsigned long slots_per_band = PAGE_SIZE / slot_bytes;
+	unsigned long bit_offset = (offset > 0) ? offset / slot_bytes : 0;
+	unsigned long curr_bit = bit_offset;
+
+	/* reset alignment to 1 if we are matching a specific offset */
+	/* adjust alignment - 1 to get to the format expected in bitmaps */
+	a = (offset > 0) ? 0 : a - 1;
+
+	/* FIXME Return error if slots_per_band > stride */
+
+	while (curr_bit < num_bits) {
+		*pos = bitmap_find_next_zero_area(map, num_bits, curr_bit, w,
+				a);
+
+		/* skip forward if we are not at right offset */
+		if (bit_offset > 0 && (*pos % slots_per_band != bit_offset)) {
+			curr_bit = ALIGN(*pos, slots_per_band) + bit_offset;
+			continue;
+		}
 
-	/* check that this is in fact an existing area */
-	WARN_ON(pvt->map[area->p0.x][area->p0.y] != area ||
-		pvt->map[area->p1.x][area->p1.y] != area);
+		/* skip forward to next row if we overlap end of row */
+		if ((*pos % slot_stride) + w > slot_stride) {
+			curr_bit = ALIGN(*pos, slot_stride) + bit_offset;
+			continue;
+		}
 
-	/* Clear the contents of the associated tiles in the map */
-	fill_area(tcm, area, NULL);
+		/* TODO: Handle overlapping 4K boundaries */
 
-	spin_unlock(&(pvt->lock));
+		/* break out of look if we will go past end of container */
+		if ((*pos + slot_stride * h) > num_bits)
+			break;
 
-	return 0;
-}
+		/* generate mask that represents out matching pattern */
+		bitmap_clear(mask, 0, slot_stride);
+		bitmap_set(mask, (*pos % BITS_PER_LONG), w);
 
-/**
- * Note: In general the cordinates in the scan field area relevant to the can
- * sweep directions. The scan origin (e.g. top-left corner) will always be
- * the p0 member of the field.  Therfore, for a scan from top-left p0.x <= p1.x
- * and p0.y <= p1.y; whereas, for a scan from bottom-right p1.x <= p0.x and p1.y
- * <= p0.y
- */
+		/* assume the area is free until we find an overlap */
+		area_free = true;
 
-/**
- * Raster scan horizontally right to left from top to bottom to find a place for
- * a 2D area of given size inside a scan field.
- *
- * @param w	width of desired area
- * @param h	height of desired area
- * @param align	desired area alignment
- * @param area	pointer to the area that will be set to the best position
- * @param field	area to scan (inclusive)
- *
- * @return 0 on success, non-0 error value on failure.
- */
-static s32 scan_r2l_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
-			struct tcm_area *field, struct tcm_area *area)
-{
-	s32 x, y;
-	s16 start_x, end_x, start_y, end_y, found_x = -1;
-	struct tcm_area ***map = ((struct sita_pvt *)tcm->pvt)->map;
-	struct score best = {{0}, {0}, {0}, 0};
-
-	start_x = field->p0.x;
-	end_x = field->p1.x;
-	start_y = field->p0.y;
-	end_y = field->p1.y;
-
-	/* check scan area co-ordinates */
-	if (field->p0.x < field->p1.x ||
-	    field->p1.y < field->p0.y)
-		return -EINVAL;
-
-	/* check if allocation would fit in scan area */
-	if (w > LEN(start_x, end_x) || h > LEN(end_y, start_y))
-		return -ENOSPC;
-
-	/* adjust start_x and end_y, as allocation would not fit beyond */
-	start_x = ALIGN_DOWN(start_x - w + 1, align); /* - 1 to be inclusive */
-	end_y = end_y - h + 1;
-
-	/* check if allocation would still fit in scan area */
-	if (start_x < end_x)
-		return -ENOSPC;
-
-	/* scan field top-to-bottom, right-to-left */
-	for (y = start_y; y <= end_y; y++) {
-		for (x = start_x; x >= end_x; x -= align) {
-			if (is_area_free(map, x, y, w, h)) {
-				found_x = x;
-
-				/* update best candidate */
-				if (update_candidate(tcm, x, y, w, h, field,
-							CR_R2L_T2B, &best))
-					goto done;
-
-				/* change upper x bound */
-				end_x = x + 1;
+		/* check subsequent rows to see if complete area is free */
+		for (i = 1; i < h; i++) {
+			index = *pos / BITS_PER_LONG + i * 8;
+			if (bitmap_intersects(&map[index], mask,
+				(*pos % BITS_PER_LONG) + w)) {
+				area_free = false;
 				break;
-			} else if (map[x][y] && map[x][y]->is2d) {
-				/* step over 2D areas */
-				x = ALIGN(map[x][y]->p0.x - w + 1, align);
 			}
 		}
 
-		/* break if you find a free area shouldering the scan field */
-		if (found_x == start_x)
+		if (area_free)
 			break;
-	}
-
-	if (!best.a.tcm)
-		return -ENOSPC;
-done:
-	assign(area, best.a.p0.x, best.a.p0.y, best.a.p1.x, best.a.p1.y);
-	return 0;
-}
-
-/**
- * Raster scan horizontally left to right from top to bottom to find a place for
- * a 2D area of given size inside a scan field.
- *
- * @param w	width of desired area
- * @param h	height of desired area
- * @param align	desired area alignment
- * @param area	pointer to the area that will be set to the best position
- * @param field	area to scan (inclusive)
- *
- * @return 0 on success, non-0 error value on failure.
- */
-static s32 scan_l2r_t2b(struct tcm *tcm, u16 w, u16 h, u16 align,
-			struct tcm_area *field, struct tcm_area *area)
-{
-	s32 x, y;
-	s16 start_x, end_x, start_y, end_y, found_x = -1;
-	struct tcm_area ***map = ((struct sita_pvt *)tcm->pvt)->map;
-	struct score best = {{0}, {0}, {0}, 0};
-
-	start_x = field->p0.x;
-	end_x = field->p1.x;
-	start_y = field->p0.y;
-	end_y = field->p1.y;
-
-	/* check scan area co-ordinates */
-	if (field->p1.x < field->p0.x ||
-	    field->p1.y < field->p0.y)
-		return -EINVAL;
-
-	/* check if allocation would fit in scan area */
-	if (w > LEN(end_x, start_x) || h > LEN(end_y, start_y))
-		return -ENOSPC;
-
-	start_x = ALIGN(start_x, align);
-
-	/* check if allocation would still fit in scan area */
-	if (w > LEN(end_x, start_x))
-		return -ENOSPC;
-
-	/* adjust end_x and end_y, as allocation would not fit beyond */
-	end_x = end_x - w + 1; /* + 1 to be inclusive */
-	end_y = end_y - h + 1;
-
-	/* scan field top-to-bottom, left-to-right */
-	for (y = start_y; y <= end_y; y++) {
-		for (x = start_x; x <= end_x; x += align) {
-			if (is_area_free(map, x, y, w, h)) {
-				found_x = x;
-
-				/* update best candidate */
-				if (update_candidate(tcm, x, y, w, h, field,
-							CR_L2R_T2B, &best))
-					goto done;
-				/* change upper x bound */
-				end_x = x - 1;
 
-				break;
-			} else if (map[x][y] && map[x][y]->is2d) {
-				/* step over 2D areas */
-				x = ALIGN_DOWN(map[x][y]->p1.x, align);
-			}
-		}
+		/* go forward past this match */
+		if (bit_offset > 0)
+			curr_bit = ALIGN(*pos, slots_per_band) + bit_offset;
+		else
+			curr_bit = *pos + a + 1;
+	}
 
-		/* break if you find a free area shouldering the scan field */
-		if (found_x == start_x)
-			break;
+	if (area_free) {
+		/* set area as in-use. iterate over rows */
+		for (i = 0, index = *pos; i < h; i++, index += slot_stride)
+			bitmap_set(map, index, w);
 	}
 
-	if (!best.a.tcm)
-		return -ENOSPC;
-done:
-	assign(area, best.a.p0.x, best.a.p0.y, best.a.p1.x, best.a.p1.y);
-	return 0;
+	return (area_free) ? 0 : -ENOMEM;
 }
 
-/**
- * Raster scan horizontally right to left from bottom to top to find a place
- * for a 1D area of given size inside a scan field.
- *
- * @param num_slots	size of desired area
- * @param align		desired area alignment
- * @param area		pointer to the area that will be set to the best
- *			position
- * @param field		area to scan (inclusive)
- *
- * @return 0 on success, non-0 error value on failure.
- */
-static s32 scan_r2l_b2t_one_dim(struct tcm *tcm, u32 num_slots,
-				struct tcm_area *field, struct tcm_area *area)
+static s32 sita_reserve_1d(struct tcm *tcm, u32 num_slots,
+			   struct tcm_area *area)
 {
-	s32 found = 0;
-	s16 x, y;
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-	struct tcm_area *p;
-
-	/* check scan area co-ordinates */
-	if (field->p0.y < field->p1.y)
-		return -EINVAL;
-
-	/**
-	 * Currently we only support full width 1D scan field, which makes sense
-	 * since 1D slot-ordering spans the full container width.
-	 */
-	if (tcm->width != field->p0.x - field->p1.x + 1)
-		return -EINVAL;
-
-	/* check if allocation would fit in scan area */
-	if (num_slots > tcm->width * LEN(field->p0.y, field->p1.y))
-		return -ENOSPC;
-
-	x = field->p0.x;
-	y = field->p0.y;
-
-	/* find num_slots consecutive free slots to the left */
-	while (found < num_slots) {
-		if (y < 0)
-			return -ENOSPC;
-
-		/* remember bottom-right corner */
-		if (found == 0) {
-			area->p1.x = x;
-			area->p1.y = y;
-		}
-
-		/* skip busy regions */
-		p = pvt->map[x][y];
-		if (p) {
-			/* move to left of 2D areas, top left of 1D */
-			x = p->p0.x;
-			if (!p->is2d)
-				y = p->p0.y;
-
-			/* start over */
-			found = 0;
-		} else {
-			/* count consecutive free slots */
-			found++;
-			if (found == num_slots)
-				break;
-		}
-
-		/* move to the left */
-		if (x == 0)
-			y--;
-		x = (x ? : tcm->width) - 1;
-
+	unsigned long pos;
+	int ret;
+
+	spin_lock(&(tcm->lock));
+	ret = r2l_b2t_1d(num_slots, &pos, tcm->bitmap, tcm->map_size);
+	if (!ret) {
+		area->p0.x = pos % tcm->width;
+		area->p0.y = pos / tcm->width;
+		area->p1.x = (pos + num_slots - 1) % tcm->width;
+		area->p1.y = (pos + num_slots - 1) / tcm->width;
 	}
+	spin_unlock(&(tcm->lock));
 
-	/* set top-left corner */
-	area->p0.x = x;
-	area->p0.y = y;
-	return 0;
+	return ret;
 }
 
-/**
- * Find a place for a 2D area of given size inside a scan field based on its
- * alignment needs.
- *
- * @param w	width of desired area
- * @param h	height of desired area
- * @param align	desired area alignment
- * @param area	pointer to the area that will be set to the best position
- *
- * @return 0 on success, non-0 error value on failure.
- */
-static s32 scan_areas_and_find_fit(struct tcm *tcm, u16 w, u16 h, u16 align,
-				   struct tcm_area *area)
+static s32 sita_reserve_2d(struct tcm *tcm, u16 h, u16 w, u16 align,
+				int16_t offset, uint16_t slot_bytes,
+				struct tcm_area *area)
 {
-	s32 ret = 0;
-	struct tcm_area field = {0};
-	u16 boundary_x, boundary_y;
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-
-	if (align > 1) {
-		/* prefer top-left corner */
-		boundary_x = pvt->div_pt.x - 1;
-		boundary_y = pvt->div_pt.y - 1;
-
-		/* expand width and height if needed */
-		if (w > pvt->div_pt.x)
-			boundary_x = tcm->width - 1;
-		if (h > pvt->div_pt.y)
-			boundary_y = tcm->height - 1;
-
-		assign(&field, 0, 0, boundary_x, boundary_y);
-		ret = scan_l2r_t2b(tcm, w, h, align, &field, area);
-
-		/* scan whole container if failed, but do not scan 2x */
-		if (ret != 0 && (boundary_x != tcm->width - 1 ||
-				 boundary_y != tcm->height - 1)) {
-			/* scan the entire container if nothing found */
-			assign(&field, 0, 0, tcm->width - 1, tcm->height - 1);
-			ret = scan_l2r_t2b(tcm, w, h, align, &field, area);
-		}
-	} else if (align == 1) {
-		/* prefer top-right corner */
-		boundary_x = pvt->div_pt.x;
-		boundary_y = pvt->div_pt.y - 1;
-
-		/* expand width and height if needed */
-		if (w > (tcm->width - pvt->div_pt.x))
-			boundary_x = 0;
-		if (h > pvt->div_pt.y)
-			boundary_y = tcm->height - 1;
-
-		assign(&field, tcm->width - 1, 0, boundary_x, boundary_y);
-		ret = scan_r2l_t2b(tcm, w, h, align, &field, area);
-
-		/* scan whole container if failed, but do not scan 2x */
-		if (ret != 0 && (boundary_x != 0 ||
-				 boundary_y != tcm->height - 1)) {
-			/* scan the entire container if nothing found */
-			assign(&field, tcm->width - 1, 0, 0, tcm->height - 1);
-			ret = scan_r2l_t2b(tcm, w, h, align, &field,
-					   area);
-		}
+	unsigned long pos;
+	int ret;
+
+	spin_lock(&(tcm->lock));
+	ret = l2r_t2b(w, h, align, offset, &pos, slot_bytes, tcm->bitmap,
+			tcm->map_size, tcm->width);
+
+	if (!ret) {
+		area->p0.x = pos % tcm->width;
+		area->p0.y = pos / tcm->width;
+		area->p1.x = area->p0.x + w - 1;
+		area->p1.y = area->p0.y + h - 1;
 	}
+	spin_unlock(&(tcm->lock));
 
 	return ret;
 }
 
-/* check if an entire area is free */
-static s32 is_area_free(struct tcm_area ***map, u16 x0, u16 y0, u16 w, u16 h)
+static void sita_deinit(struct tcm *tcm)
 {
-	u16 x = 0, y = 0;
-	for (y = y0; y < y0 + h; y++) {
-		for (x = x0; x < x0 + w; x++) {
-			if (map[x][y])
-				return false;
-		}
-	}
-	return true;
+	kfree(tcm);
 }
 
-/* fills an area with a parent tcm_area */
-static void fill_area(struct tcm *tcm, struct tcm_area *area,
-			struct tcm_area *parent)
+static s32 sita_free(struct tcm *tcm, struct tcm_area *area)
 {
-	s32 x, y;
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-	struct tcm_area a, a_;
-
-	/* set area's tcm; otherwise, enumerator considers it invalid */
-	area->tcm = tcm;
-
-	tcm_for_each_slice(a, *area, a_) {
-		for (x = a.p0.x; x <= a.p1.x; ++x)
-			for (y = a.p0.y; y <= a.p1.y; ++y)
-				pvt->map[x][y] = parent;
+	unsigned long pos;
+	uint16_t w, h;
 
+	pos = area->p0.x + area->p0.y * tcm->width;
+	if (area->is2d) {
+		w = area->p1.x - area->p0.x + 1;
+		h = area->p1.y - area->p0.y + 1;
+	} else {
+		w = area->p1.x + area->p1.y * tcm->width - pos + 1;
+		h = 1;
 	}
+
+	spin_lock(&(tcm->lock));
+	free_slots(pos, w, h, tcm->bitmap, tcm->width);
+	spin_unlock(&(tcm->lock));
+	return 0;
 }
 
-/**
- * Compares a candidate area to the current best area, and if it is a better
- * fit, it updates the best to this one.
- *
- * @param x0, y0, w, h		top, left, width, height of candidate area
- * @param field			scan field
- * @param criteria		scan criteria
- * @param best			best candidate and its scores
- *
- * @return 1 (true) if the candidate area is known to be the final best, so no
- * more searching should be performed
- */
-static s32 update_candidate(struct tcm *tcm, u16 x0, u16 y0, u16 w, u16 h,
-			    struct tcm_area *field, s32 criteria,
-			    struct score *best)
+struct tcm *sita_init(u16 width, u16 height)
 {
-	struct score me;	/* score for area */
-
-	/*
-	 * NOTE: For horizontal bias we always give the first found, because our
-	 * scan is horizontal-raster-based and the first candidate will always
-	 * have the horizontal bias.
-	 */
-	bool first = criteria & CR_BIAS_HORIZONTAL;
-
-	assign(&me.a, x0, y0, x0 + w - 1, y0 + h - 1);
-
-	/* calculate score for current candidate */
-	if (!first) {
-		get_neighbor_stats(tcm, &me.a, &me.n);
-		me.neighs = me.n.edge + me.n.busy;
-		get_nearness_factor(field, &me.a, &me.f);
-	}
-
-	/* the 1st candidate is always the best */
-	if (!best->a.tcm)
-		goto better;
+	struct tcm *tcm;
+	size_t map_size = BITS_TO_LONGS(width*height) * sizeof(unsigned long);
 
-	BUG_ON(first);
+	if (width == 0 || height == 0)
+		return NULL;
 
-	/* diagonal balance check */
-	if ((criteria & CR_DIAGONAL_BALANCE) &&
-		best->neighs <= me.neighs &&
-		(best->neighs < me.neighs ||
-		 /* this implies that neighs and occupied match */
-		 best->n.busy < me.n.busy ||
-		 (best->n.busy == me.n.busy &&
-		  /* check the nearness factor */
-		  best->f.x + best->f.y > me.f.x + me.f.y)))
-		goto better;
+	tcm = kzalloc(sizeof(*tcm) + map_size, GFP_KERNEL);
+	if (!tcm)
+		goto error;
 
-	/* not better, keep going */
-	return 0;
+	/* Updating the pointers to SiTA implementation APIs */
+	tcm->height = height;
+	tcm->width = width;
+	tcm->reserve_2d = sita_reserve_2d;
+	tcm->reserve_1d = sita_reserve_1d;
+	tcm->free = sita_free;
+	tcm->deinit = sita_deinit;
 
-better:
-	/* save current area as best */
-	memcpy(best, &me, sizeof(me));
-	best->a.tcm = tcm;
-	return first;
-}
+	spin_lock_init(&tcm->lock);
+	tcm->bitmap = (unsigned long *)(tcm + 1);
+	bitmap_clear(tcm->bitmap, 0, width*height);
 
-/**
- * Calculate the nearness factor of an area in a search field.  The nearness
- * factor is smaller if the area is closer to the search origin.
- */
-static void get_nearness_factor(struct tcm_area *field, struct tcm_area *area,
-				struct nearness_factor *nf)
-{
-	/**
-	 * Using signed math as field coordinates may be reversed if
-	 * search direction is right-to-left or bottom-to-top.
-	 */
-	nf->x = (s32)(area->p0.x - field->p0.x) * 1000 /
-		(field->p1.x - field->p0.x);
-	nf->y = (s32)(area->p0.y - field->p0.y) * 1000 /
-		(field->p1.y - field->p0.y);
-}
+	tcm->map_size = width*height;
 
-/* get neighbor statistics */
-static void get_neighbor_stats(struct tcm *tcm, struct tcm_area *area,
-			 struct neighbor_stats *stat)
-{
-	s16 x = 0, y = 0;
-	struct sita_pvt *pvt = (struct sita_pvt *)tcm->pvt;
-
-	/* Clearing any exisiting values */
-	memset(stat, 0, sizeof(*stat));
-
-	/* process top & bottom edges */
-	for (x = area->p0.x; x <= area->p1.x; x++) {
-		if (area->p0.y == 0)
-			stat->edge++;
-		else if (pvt->map[x][area->p0.y - 1])
-			stat->busy++;
-
-		if (area->p1.y == tcm->height - 1)
-			stat->edge++;
-		else if (pvt->map[x][area->p1.y + 1])
-			stat->busy++;
-	}
+	return tcm;
 
-	/* process left & right edges */
-	for (y = area->p0.y; y <= area->p1.y; ++y) {
-		if (area->p0.x == 0)
-			stat->edge++;
-		else if (pvt->map[area->p0.x - 1][y])
-			stat->busy++;
-
-		if (area->p1.x == tcm->width - 1)
-			stat->edge++;
-		else if (pvt->map[area->p1.x + 1][y])
-			stat->busy++;
-	}
+error:
+	kfree(tcm);
+	return NULL;
 }
diff --git a/drivers/gpu/drm/omapdrm/tcm.h b/drivers/gpu/drm/omapdrm/tcm.h
index a8d5ce47686f..ef7df7d6fc84 100644
--- a/drivers/gpu/drm/omapdrm/tcm.h
+++ b/drivers/gpu/drm/omapdrm/tcm.h
@@ -61,18 +61,17 @@ struct tcm {
 
 	unsigned int y_offset;	/* offset to use for y coordinates */
 
-	/* 'pvt' structure shall contain any tcm details (attr) along with
-	linked list of allocated areas and mutex for mutually exclusive access
-	to the list.  It may also contain copies of width and height to notice
-	any changes to the publicly available width and height fields. */
-	void *pvt;
+	spinlock_t lock;
+	unsigned long *bitmap;
+	size_t map_size;
 
 	/* function table */
-	s32 (*reserve_2d)(struct tcm *tcm, u16 height, u16 width, u8 align,
+	s32 (*reserve_2d)(struct tcm *tcm, u16 height, u16 width, u16 align,
+			  int16_t offset, uint16_t slot_bytes,
 			  struct tcm_area *area);
 	s32 (*reserve_1d)(struct tcm *tcm, u32 slots, struct tcm_area *area);
-	s32 (*free)      (struct tcm *tcm, struct tcm_area *area);
-	void (*deinit)   (struct tcm *tcm);
+	s32 (*free)(struct tcm *tcm, struct tcm_area *area);
+	void (*deinit)(struct tcm *tcm);
 };
 
 /*=============================================================================
@@ -91,7 +90,7 @@ struct tcm {
  *
  */
 
-struct tcm *sita_init(u16 width, u16 height, struct tcm_pt *attr);
+struct tcm *sita_init(u16 width, u16 height);
 
 
 /**
@@ -120,6 +119,9 @@ static inline void tcm_deinit(struct tcm *tcm)
  *			all values may be supported by the container manager,
  *			but it must support 0 (1), 32 and 64.
  *			0 value is equivalent to 1.
+ * @param offset	Offset requirement, in bytes.  This is the offset
+ *			from a 4KiB aligned virtual address.
+ * @param slot_bytes	Width of slot in bytes
  * @param area		Pointer to where the reserved area should be stored.
  *
  * @return 0 on success.  Non-0 error code on failure.  Also,
@@ -129,7 +131,8 @@ static inline void tcm_deinit(struct tcm *tcm)
  *	    allocation.
  */
 static inline s32 tcm_reserve_2d(struct tcm *tcm, u16 width, u16 height,
-				 u16 align, struct tcm_area *area)
+				u16 align, int16_t offset, uint16_t slot_bytes,
+				struct tcm_area *area)
 {
 	/* perform rudimentary error checking */
 	s32 res = tcm  == NULL ? -ENODEV :
@@ -140,7 +143,8 @@ static inline s32 tcm_reserve_2d(struct tcm *tcm, u16 width, u16 height,
 
 	if (!res) {
 		area->is2d = true;
-		res = tcm->reserve_2d(tcm, height, width, align, area);
+		res = tcm->reserve_2d(tcm, height, width, align, offset,
+					slot_bytes, area);
 		area->tcm = res ? NULL : tcm;
 	}
 
diff --git a/drivers/gpu/drm/panel/Kconfig b/drivers/gpu/drm/panel/Kconfig
index 7d4704b1292b..1500ab99f548 100644
--- a/drivers/gpu/drm/panel/Kconfig
+++ b/drivers/gpu/drm/panel/Kconfig
@@ -31,6 +31,16 @@ config DRM_PANEL_LG_LG4573
 	  Say Y here if you want to enable support for LG4573 RGB panel.
 	  To compile this driver as a module, choose M here.
 
+config DRM_PANEL_PANASONIC_VVX10F034N00
+	tristate "Panasonic VVX10F034N00 1920x1200 video mode panel"
+	depends on OF
+	depends on DRM_MIPI_DSI
+	depends on BACKLIGHT_CLASS_DEVICE
+	help
+	  Say Y here if you want to enable support for Panasonic VVX10F034N00
+	  WUXGA (1920x1200) Novatek NT1397-based DSI panel as found in some
+	  Xperia Z2 tablets
+
 config DRM_PANEL_SAMSUNG_S6E8AA0
 	tristate "Samsung S6E8AA0 DSI video mode panel"
 	depends on OF
@@ -51,4 +61,13 @@ config DRM_PANEL_SHARP_LQ101R1SX01
 	  To compile this driver as a module, choose M here: the module
 	  will be called panel-sharp-lq101r1sx01.
 
+config DRM_PANEL_SHARP_LS043T1LE01
+	tristate "Sharp LS043T1LE01 qHD video mode panel"
+	depends on OF
+	depends on DRM_MIPI_DSI
+	depends on BACKLIGHT_CLASS_DEVICE
+	help
+	  Say Y here if you want to enable support for Sharp LS043T1LE01 qHD
+	  (540x960) DSI panel as found on the Qualcomm APQ8074 Dragonboard
+
 endmenu
diff --git a/drivers/gpu/drm/panel/Makefile b/drivers/gpu/drm/panel/Makefile
index d0f016dd7ddb..f277eed933d6 100644
--- a/drivers/gpu/drm/panel/Makefile
+++ b/drivers/gpu/drm/panel/Makefile
@@ -1,5 +1,7 @@
 obj-$(CONFIG_DRM_PANEL_SIMPLE) += panel-simple.o
 obj-$(CONFIG_DRM_PANEL_LG_LG4573) += panel-lg-lg4573.o
+obj-$(CONFIG_DRM_PANEL_PANASONIC_VVX10F034N00) += panel-panasonic-vvx10f034n00.o
 obj-$(CONFIG_DRM_PANEL_SAMSUNG_LD9040) += panel-samsung-ld9040.o
 obj-$(CONFIG_DRM_PANEL_SAMSUNG_S6E8AA0) += panel-samsung-s6e8aa0.o
 obj-$(CONFIG_DRM_PANEL_SHARP_LQ101R1SX01) += panel-sharp-lq101r1sx01.o
+obj-$(CONFIG_DRM_PANEL_SHARP_LS043T1LE01) += panel-sharp-ls043t1le01.o
diff --git a/drivers/gpu/drm/panel/panel-panasonic-vvx10f034n00.c b/drivers/gpu/drm/panel/panel-panasonic-vvx10f034n00.c
new file mode 100644
index 000000000000..7f915f706fa6
--- /dev/null
+++ b/drivers/gpu/drm/panel/panel-panasonic-vvx10f034n00.c
@@ -0,0 +1,334 @@
+/*
+ * Copyright (C) 2015 Red Hat
+ * Copyright (C) 2015 Sony Mobile Communications Inc.
+ * Author: Werner Johansson <werner.johansson@sonymobile.com>
+ *
+ * Based on AUO panel driver by Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/backlight.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_mipi_dsi.h>
+#include <drm/drm_panel.h>
+
+#include <video/mipi_display.h>
+
+/*
+ * When power is turned off to this panel a minimum off time of 500ms has to be
+ * observed before powering back on as there's no external reset pin. Keep
+ * track of earliest wakeup time and delay subsequent prepare call accordingly
+ */
+#define MIN_POFF_MS (500)
+
+struct wuxga_nt_panel {
+	struct drm_panel base;
+	struct mipi_dsi_device *dsi;
+
+	struct backlight_device *backlight;
+	struct regulator *supply;
+
+	bool prepared;
+	bool enabled;
+
+	ktime_t earliest_wake;
+
+	const struct drm_display_mode *mode;
+};
+
+static inline struct wuxga_nt_panel *to_wuxga_nt_panel(struct drm_panel *panel)
+{
+	return container_of(panel, struct wuxga_nt_panel, base);
+}
+
+static int wuxga_nt_panel_on(struct wuxga_nt_panel *wuxga_nt)
+{
+	struct mipi_dsi_device *dsi = wuxga_nt->dsi;
+	int ret;
+
+	ret = mipi_dsi_turn_on_peripheral(dsi);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int wuxga_nt_panel_disable(struct drm_panel *panel)
+{
+	struct wuxga_nt_panel *wuxga_nt = to_wuxga_nt_panel(panel);
+
+	if (!wuxga_nt->enabled)
+		return 0;
+
+	mipi_dsi_shutdown_peripheral(wuxga_nt->dsi);
+
+	if (wuxga_nt->backlight) {
+		wuxga_nt->backlight->props.power = FB_BLANK_POWERDOWN;
+		wuxga_nt->backlight->props.state |= BL_CORE_FBBLANK;
+		backlight_update_status(wuxga_nt->backlight);
+	}
+
+	wuxga_nt->enabled = false;
+
+	return 0;
+}
+
+static int wuxga_nt_panel_unprepare(struct drm_panel *panel)
+{
+	struct wuxga_nt_panel *wuxga_nt = to_wuxga_nt_panel(panel);
+
+	if (!wuxga_nt->prepared)
+		return 0;
+
+	regulator_disable(wuxga_nt->supply);
+	wuxga_nt->earliest_wake = ktime_add_ms(ktime_get_real(), MIN_POFF_MS);
+	wuxga_nt->prepared = false;
+
+	return 0;
+}
+
+static int wuxga_nt_panel_prepare(struct drm_panel *panel)
+{
+	struct wuxga_nt_panel *wuxga_nt = to_wuxga_nt_panel(panel);
+	int ret;
+	s64 enablewait;
+
+	if (wuxga_nt->prepared)
+		return 0;
+
+	/*
+	 * If the user re-enabled the panel before the required off-time then
+	 * we need to wait the remaining period before re-enabling regulator
+	 */
+	enablewait = ktime_ms_delta(wuxga_nt->earliest_wake, ktime_get_real());
+
+	/* Sanity check, this should never happen */
+	if (enablewait > MIN_POFF_MS)
+		enablewait = MIN_POFF_MS;
+
+	if (enablewait > 0)
+		msleep(enablewait);
+
+	ret = regulator_enable(wuxga_nt->supply);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * A minimum delay of 250ms is required after power-up until commands
+	 * can be sent
+	 */
+	msleep(250);
+
+	ret = wuxga_nt_panel_on(wuxga_nt);
+	if (ret < 0) {
+		dev_err(panel->dev, "failed to set panel on: %d\n", ret);
+		goto poweroff;
+	}
+
+	wuxga_nt->prepared = true;
+
+	return 0;
+
+poweroff:
+	regulator_disable(wuxga_nt->supply);
+
+	return ret;
+}
+
+static int wuxga_nt_panel_enable(struct drm_panel *panel)
+{
+	struct wuxga_nt_panel *wuxga_nt = to_wuxga_nt_panel(panel);
+
+	if (wuxga_nt->enabled)
+		return 0;
+
+	if (wuxga_nt->backlight) {
+		wuxga_nt->backlight->props.power = FB_BLANK_UNBLANK;
+		wuxga_nt->backlight->props.state &= ~BL_CORE_FBBLANK;
+		backlight_update_status(wuxga_nt->backlight);
+	}
+
+	wuxga_nt->enabled = true;
+
+	return 0;
+}
+
+static const struct drm_display_mode default_mode = {
+	.clock = 164402,
+	.hdisplay = 1920,
+	.hsync_start = 1920 + 152,
+	.hsync_end = 1920 + 152 + 52,
+	.htotal = 1920 + 152 + 52 + 20,
+	.vdisplay = 1200,
+	.vsync_start = 1200 + 24,
+	.vsync_end = 1200 + 24 + 6,
+	.vtotal = 1200 + 24 + 6 + 48,
+	.vrefresh = 60,
+};
+
+static int wuxga_nt_panel_get_modes(struct drm_panel *panel)
+{
+	struct drm_display_mode *mode;
+
+	mode = drm_mode_duplicate(panel->drm, &default_mode);
+	if (!mode) {
+		dev_err(panel->drm->dev, "failed to add mode %ux%ux@%u\n",
+				default_mode.hdisplay, default_mode.vdisplay,
+				default_mode.vrefresh);
+		return -ENOMEM;
+	}
+
+	drm_mode_set_name(mode);
+
+	drm_mode_probed_add(panel->connector, mode);
+
+	panel->connector->display_info.width_mm = 217;
+	panel->connector->display_info.height_mm = 136;
+
+	return 1;
+}
+
+static const struct drm_panel_funcs wuxga_nt_panel_funcs = {
+	.disable = wuxga_nt_panel_disable,
+	.unprepare = wuxga_nt_panel_unprepare,
+	.prepare = wuxga_nt_panel_prepare,
+	.enable = wuxga_nt_panel_enable,
+	.get_modes = wuxga_nt_panel_get_modes,
+};
+
+static const struct of_device_id wuxga_nt_of_match[] = {
+	{ .compatible = "panasonic,vvx10f034n00", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, wuxga_nt_of_match);
+
+static int wuxga_nt_panel_add(struct wuxga_nt_panel *wuxga_nt)
+{
+	struct device *dev = &wuxga_nt->dsi->dev;
+	struct device_node *np;
+	int ret;
+
+	wuxga_nt->mode = &default_mode;
+
+	wuxga_nt->supply = devm_regulator_get(dev, "power");
+	if (IS_ERR(wuxga_nt->supply))
+		return PTR_ERR(wuxga_nt->supply);
+
+	np = of_parse_phandle(dev->of_node, "backlight", 0);
+	if (np) {
+		wuxga_nt->backlight = of_find_backlight_by_node(np);
+		of_node_put(np);
+
+		if (!wuxga_nt->backlight)
+			return -EPROBE_DEFER;
+	}
+
+	drm_panel_init(&wuxga_nt->base);
+	wuxga_nt->base.funcs = &wuxga_nt_panel_funcs;
+	wuxga_nt->base.dev = &wuxga_nt->dsi->dev;
+
+	ret = drm_panel_add(&wuxga_nt->base);
+	if (ret < 0)
+		goto put_backlight;
+
+	return 0;
+
+put_backlight:
+	if (wuxga_nt->backlight)
+		put_device(&wuxga_nt->backlight->dev);
+
+	return ret;
+}
+
+static void wuxga_nt_panel_del(struct wuxga_nt_panel *wuxga_nt)
+{
+	if (wuxga_nt->base.dev)
+		drm_panel_remove(&wuxga_nt->base);
+
+	if (wuxga_nt->backlight)
+		put_device(&wuxga_nt->backlight->dev);
+}
+
+static int wuxga_nt_panel_probe(struct mipi_dsi_device *dsi)
+{
+	struct wuxga_nt_panel *wuxga_nt;
+	int ret;
+
+	dsi->lanes = 4;
+	dsi->format = MIPI_DSI_FMT_RGB888;
+	dsi->mode_flags = MIPI_DSI_MODE_VIDEO |
+			MIPI_DSI_MODE_VIDEO_HSE |
+			MIPI_DSI_CLOCK_NON_CONTINUOUS |
+			MIPI_DSI_MODE_LPM;
+
+	wuxga_nt = devm_kzalloc(&dsi->dev, sizeof(*wuxga_nt), GFP_KERNEL);
+	if (!wuxga_nt)
+		return -ENOMEM;
+
+	mipi_dsi_set_drvdata(dsi, wuxga_nt);
+
+	wuxga_nt->dsi = dsi;
+
+	ret = wuxga_nt_panel_add(wuxga_nt);
+	if (ret < 0)
+		return ret;
+
+	return mipi_dsi_attach(dsi);
+}
+
+static int wuxga_nt_panel_remove(struct mipi_dsi_device *dsi)
+{
+	struct wuxga_nt_panel *wuxga_nt = mipi_dsi_get_drvdata(dsi);
+	int ret;
+
+	ret = wuxga_nt_panel_disable(&wuxga_nt->base);
+	if (ret < 0)
+		dev_err(&dsi->dev, "failed to disable panel: %d\n", ret);
+
+	ret = mipi_dsi_detach(dsi);
+	if (ret < 0)
+		dev_err(&dsi->dev, "failed to detach from DSI host: %d\n", ret);
+
+	drm_panel_detach(&wuxga_nt->base);
+	wuxga_nt_panel_del(wuxga_nt);
+
+	return 0;
+}
+
+static void wuxga_nt_panel_shutdown(struct mipi_dsi_device *dsi)
+{
+	struct wuxga_nt_panel *wuxga_nt = mipi_dsi_get_drvdata(dsi);
+
+	wuxga_nt_panel_disable(&wuxga_nt->base);
+}
+
+static struct mipi_dsi_driver wuxga_nt_panel_driver = {
+	.driver = {
+		.name = "panel-panasonic-vvx10f034n00",
+		.of_match_table = wuxga_nt_of_match,
+	},
+	.probe = wuxga_nt_panel_probe,
+	.remove = wuxga_nt_panel_remove,
+	.shutdown = wuxga_nt_panel_shutdown,
+};
+module_mipi_dsi_driver(wuxga_nt_panel_driver);
+
+MODULE_AUTHOR("Werner Johansson <werner.johansson@sonymobile.com>");
+MODULE_DESCRIPTION("Panasonic VVX10F034N00 Novatek NT1397-based WUXGA (1920x1200) video mode panel driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/panel/panel-sharp-ls043t1le01.c b/drivers/gpu/drm/panel/panel-sharp-ls043t1le01.c
new file mode 100644
index 000000000000..3aeb0bda4947
--- /dev/null
+++ b/drivers/gpu/drm/panel/panel-sharp-ls043t1le01.c
@@ -0,0 +1,387 @@
+/*
+ * Copyright (C) 2015 Red Hat
+ * Copyright (C) 2015 Sony Mobile Communications Inc.
+ * Author: Werner Johansson <werner.johansson@sonymobile.com>
+ *
+ * Based on AUO panel driver by Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/backlight.h>
+#include <linux/gpio/consumer.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+
+#include <drm/drmP.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_mipi_dsi.h>
+#include <drm/drm_panel.h>
+
+#include <video/mipi_display.h>
+
+struct sharp_nt_panel {
+	struct drm_panel base;
+	struct mipi_dsi_device *dsi;
+
+	struct backlight_device *backlight;
+	struct regulator *supply;
+	struct gpio_desc *reset_gpio;
+
+	bool prepared;
+	bool enabled;
+
+	const struct drm_display_mode *mode;
+};
+
+static inline struct sharp_nt_panel *to_sharp_nt_panel(struct drm_panel *panel)
+{
+	return container_of(panel, struct sharp_nt_panel, base);
+}
+
+static int sharp_nt_panel_init(struct sharp_nt_panel *sharp_nt)
+{
+	struct mipi_dsi_device *dsi = sharp_nt->dsi;
+	int ret;
+
+	dsi->mode_flags |= MIPI_DSI_MODE_LPM;
+
+	ret = mipi_dsi_dcs_exit_sleep_mode(dsi);
+	if (ret < 0)
+		return ret;
+
+	msleep(120);
+
+	/* Novatek two-lane operation */
+	ret = mipi_dsi_dcs_write(dsi, 0xae, (u8[]){ 0x03 }, 1);
+	if (ret < 0)
+		return ret;
+
+	/* Set both MCU and RGB I/F to 24bpp */
+	ret = mipi_dsi_dcs_set_pixel_format(dsi, MIPI_DCS_PIXEL_FMT_24BIT |
+					(MIPI_DCS_PIXEL_FMT_24BIT << 4));
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int sharp_nt_panel_on(struct sharp_nt_panel *sharp_nt)
+{
+	struct mipi_dsi_device *dsi = sharp_nt->dsi;
+	int ret;
+
+	dsi->mode_flags |= MIPI_DSI_MODE_LPM;
+
+	ret = mipi_dsi_dcs_set_display_on(dsi);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int sharp_nt_panel_off(struct sharp_nt_panel *sharp_nt)
+{
+	struct mipi_dsi_device *dsi = sharp_nt->dsi;
+	int ret;
+
+	dsi->mode_flags &= ~MIPI_DSI_MODE_LPM;
+
+	ret = mipi_dsi_dcs_set_display_off(dsi);
+	if (ret < 0)
+		return ret;
+
+	ret = mipi_dsi_dcs_enter_sleep_mode(dsi);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+
+static int sharp_nt_panel_disable(struct drm_panel *panel)
+{
+	struct sharp_nt_panel *sharp_nt = to_sharp_nt_panel(panel);
+
+	if (!sharp_nt->enabled)
+		return 0;
+
+	if (sharp_nt->backlight) {
+		sharp_nt->backlight->props.power = FB_BLANK_POWERDOWN;
+		backlight_update_status(sharp_nt->backlight);
+	}
+
+	sharp_nt->enabled = false;
+
+	return 0;
+}
+
+static int sharp_nt_panel_unprepare(struct drm_panel *panel)
+{
+	struct sharp_nt_panel *sharp_nt = to_sharp_nt_panel(panel);
+	int ret;
+
+	if (!sharp_nt->prepared)
+		return 0;
+
+	ret = sharp_nt_panel_off(sharp_nt);
+	if (ret < 0) {
+		dev_err(panel->dev, "failed to set panel off: %d\n", ret);
+		return ret;
+	}
+
+	regulator_disable(sharp_nt->supply);
+	if (sharp_nt->reset_gpio)
+		gpiod_set_value(sharp_nt->reset_gpio, 0);
+
+	sharp_nt->prepared = false;
+
+	return 0;
+}
+
+static int sharp_nt_panel_prepare(struct drm_panel *panel)
+{
+	struct sharp_nt_panel *sharp_nt = to_sharp_nt_panel(panel);
+	int ret;
+
+	if (sharp_nt->prepared)
+		return 0;
+
+	ret = regulator_enable(sharp_nt->supply);
+	if (ret < 0)
+		return ret;
+
+	msleep(20);
+
+	if (sharp_nt->reset_gpio) {
+		gpiod_set_value(sharp_nt->reset_gpio, 1);
+		msleep(1);
+		gpiod_set_value(sharp_nt->reset_gpio, 0);
+		msleep(1);
+		gpiod_set_value(sharp_nt->reset_gpio, 1);
+		msleep(10);
+	}
+
+	ret = sharp_nt_panel_init(sharp_nt);
+	if (ret < 0) {
+		dev_err(panel->dev, "failed to init panel: %d\n", ret);
+		goto poweroff;
+	}
+
+	ret = sharp_nt_panel_on(sharp_nt);
+	if (ret < 0) {
+		dev_err(panel->dev, "failed to set panel on: %d\n", ret);
+		goto poweroff;
+	}
+
+	sharp_nt->prepared = true;
+
+	return 0;
+
+poweroff:
+	regulator_disable(sharp_nt->supply);
+	if (sharp_nt->reset_gpio)
+		gpiod_set_value(sharp_nt->reset_gpio, 0);
+	return ret;
+}
+
+static int sharp_nt_panel_enable(struct drm_panel *panel)
+{
+	struct sharp_nt_panel *sharp_nt = to_sharp_nt_panel(panel);
+
+	if (sharp_nt->enabled)
+		return 0;
+
+	if (sharp_nt->backlight) {
+		sharp_nt->backlight->props.power = FB_BLANK_UNBLANK;
+		backlight_update_status(sharp_nt->backlight);
+	}
+
+	sharp_nt->enabled = true;
+
+	return 0;
+}
+
+static const struct drm_display_mode default_mode = {
+	.clock = 41118,
+	.hdisplay = 540,
+	.hsync_start = 540 + 48,
+	.hsync_end = 540 + 48 + 80,
+	.htotal = 540 + 48 + 80 + 32,
+	.vdisplay = 960,
+	.vsync_start = 960 + 3,
+	.vsync_end = 960 + 3 + 15,
+	.vtotal = 960 + 3 + 15 + 1,
+	.vrefresh = 60,
+};
+
+static int sharp_nt_panel_get_modes(struct drm_panel *panel)
+{
+	struct drm_display_mode *mode;
+
+	mode = drm_mode_duplicate(panel->drm, &default_mode);
+	if (!mode) {
+		dev_err(panel->drm->dev, "failed to add mode %ux%ux@%u\n",
+				default_mode.hdisplay, default_mode.vdisplay,
+				default_mode.vrefresh);
+		return -ENOMEM;
+	}
+
+	drm_mode_set_name(mode);
+
+	drm_mode_probed_add(panel->connector, mode);
+
+	panel->connector->display_info.width_mm = 54;
+	panel->connector->display_info.height_mm = 95;
+
+	return 1;
+}
+
+static const struct drm_panel_funcs sharp_nt_panel_funcs = {
+	.disable = sharp_nt_panel_disable,
+	.unprepare = sharp_nt_panel_unprepare,
+	.prepare = sharp_nt_panel_prepare,
+	.enable = sharp_nt_panel_enable,
+	.get_modes = sharp_nt_panel_get_modes,
+};
+
+static int sharp_nt_panel_add(struct sharp_nt_panel *sharp_nt)
+{
+	struct device *dev = &sharp_nt->dsi->dev;
+	struct device_node *np;
+	int ret;
+
+	sharp_nt->mode = &default_mode;
+
+	sharp_nt->supply = devm_regulator_get(dev, "avdd");
+	if (IS_ERR(sharp_nt->supply))
+		return PTR_ERR(sharp_nt->supply);
+
+	sharp_nt->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
+	if (IS_ERR(sharp_nt->reset_gpio)) {
+		dev_err(dev, "cannot get reset-gpios %ld\n",
+			PTR_ERR(sharp_nt->reset_gpio));
+		sharp_nt->reset_gpio = NULL;
+	} else {
+		gpiod_set_value(sharp_nt->reset_gpio, 0);
+	}
+
+	np = of_parse_phandle(dev->of_node, "backlight", 0);
+	if (np) {
+		sharp_nt->backlight = of_find_backlight_by_node(np);
+		of_node_put(np);
+
+		if (!sharp_nt->backlight)
+			return -EPROBE_DEFER;
+	}
+
+	drm_panel_init(&sharp_nt->base);
+	sharp_nt->base.funcs = &sharp_nt_panel_funcs;
+	sharp_nt->base.dev = &sharp_nt->dsi->dev;
+
+	ret = drm_panel_add(&sharp_nt->base);
+	if (ret < 0)
+		goto put_backlight;
+
+	return 0;
+
+put_backlight:
+	if (sharp_nt->backlight)
+		put_device(&sharp_nt->backlight->dev);
+
+	return ret;
+}
+
+static void sharp_nt_panel_del(struct sharp_nt_panel *sharp_nt)
+{
+	if (sharp_nt->base.dev)
+		drm_panel_remove(&sharp_nt->base);
+
+	if (sharp_nt->backlight)
+		put_device(&sharp_nt->backlight->dev);
+}
+
+static int sharp_nt_panel_probe(struct mipi_dsi_device *dsi)
+{
+	struct sharp_nt_panel *sharp_nt;
+	int ret;
+
+	dsi->lanes = 2;
+	dsi->format = MIPI_DSI_FMT_RGB888;
+	dsi->mode_flags = MIPI_DSI_MODE_VIDEO |
+			MIPI_DSI_MODE_VIDEO_HSE |
+			MIPI_DSI_CLOCK_NON_CONTINUOUS |
+			MIPI_DSI_MODE_EOT_PACKET;
+
+	sharp_nt = devm_kzalloc(&dsi->dev, sizeof(*sharp_nt), GFP_KERNEL);
+	if (!sharp_nt)
+		return -ENOMEM;
+
+	mipi_dsi_set_drvdata(dsi, sharp_nt);
+
+	sharp_nt->dsi = dsi;
+
+	ret = sharp_nt_panel_add(sharp_nt);
+	if (ret < 0)
+		return ret;
+
+	return mipi_dsi_attach(dsi);
+}
+
+static int sharp_nt_panel_remove(struct mipi_dsi_device *dsi)
+{
+	struct sharp_nt_panel *sharp_nt = mipi_dsi_get_drvdata(dsi);
+	int ret;
+
+	ret = sharp_nt_panel_disable(&sharp_nt->base);
+	if (ret < 0)
+		dev_err(&dsi->dev, "failed to disable panel: %d\n", ret);
+
+	ret = mipi_dsi_detach(dsi);
+	if (ret < 0)
+		dev_err(&dsi->dev, "failed to detach from DSI host: %d\n", ret);
+
+	drm_panel_detach(&sharp_nt->base);
+	sharp_nt_panel_del(sharp_nt);
+
+	return 0;
+}
+
+static void sharp_nt_panel_shutdown(struct mipi_dsi_device *dsi)
+{
+	struct sharp_nt_panel *sharp_nt = mipi_dsi_get_drvdata(dsi);
+
+	sharp_nt_panel_disable(&sharp_nt->base);
+}
+
+static const struct of_device_id sharp_nt_of_match[] = {
+	{ .compatible = "sharp,ls043t1le01-qhd", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, sharp_nt_of_match);
+
+static struct mipi_dsi_driver sharp_nt_panel_driver = {
+	.driver = {
+		.name = "panel-sharp-ls043t1le01-qhd",
+		.of_match_table = sharp_nt_of_match,
+	},
+	.probe = sharp_nt_panel_probe,
+	.remove = sharp_nt_panel_remove,
+	.shutdown = sharp_nt_panel_shutdown,
+};
+module_mipi_dsi_driver(sharp_nt_panel_driver);
+
+MODULE_AUTHOR("Werner Johansson <werner.johansson@sonymobile.com>");
+MODULE_DESCRIPTION("Sharp LS043T1LE01 NT35565-based qHD (540x960) video mode panel driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/panel/panel-simple.c b/drivers/gpu/drm/panel/panel-simple.c
index f97b73ec4713..f88a631c43ab 100644
--- a/drivers/gpu/drm/panel/panel-simple.c
+++ b/drivers/gpu/drm/panel/panel-simple.c
@@ -44,6 +44,10 @@ struct panel_desc {
 
 	unsigned int bpc;
 
+	/**
+	 * @width: width (in millimeters) of the panel's active display area
+	 * @height: height (in millimeters) of the panel's active display area
+	 */
 	struct {
 		unsigned int width;
 		unsigned int height;
@@ -832,6 +836,34 @@ static const struct panel_desc innolux_g121i1_l01 = {
 	},
 };
 
+static const struct drm_display_mode innolux_g121x1_l03_mode = {
+	.clock = 65000,
+	.hdisplay = 1024,
+	.hsync_start = 1024 + 0,
+	.hsync_end = 1024 + 1,
+	.htotal = 1024 + 0 + 1 + 320,
+	.vdisplay = 768,
+	.vsync_start = 768 + 38,
+	.vsync_end = 768 + 38 + 1,
+	.vtotal = 768 + 38 + 1 + 0,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc innolux_g121x1_l03 = {
+	.modes = &innolux_g121x1_l03_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 246,
+		.height = 185,
+	},
+	.delay = {
+		.enable = 200,
+		.unprepare = 200,
+		.disable = 400,
+	},
+};
+
 static const struct drm_display_mode innolux_n116bge_mode = {
 	.clock = 76420,
 	.hdisplay = 1366,
@@ -902,6 +934,30 @@ static const struct panel_desc innolux_zj070na_01p = {
 	},
 };
 
+static const struct display_timing kyo_tcg121xglp_timing = {
+	.pixelclock = { 52000000, 65000000, 71000000 },
+	.hactive = { 1024, 1024, 1024 },
+	.hfront_porch = { 2, 2, 2 },
+	.hback_porch = { 2, 2, 2 },
+	.hsync_len = { 86, 124, 244 },
+	.vactive = { 768, 768, 768 },
+	.vfront_porch = { 2, 2, 2 },
+	.vback_porch = { 2, 2, 2 },
+	.vsync_len = { 6, 34, 73 },
+	.flags = DISPLAY_FLAGS_DE_HIGH,
+};
+
+static const struct panel_desc kyo_tcg121xglp = {
+	.timings = &kyo_tcg121xglp_timing,
+	.num_timings = 1,
+	.bpc = 8,
+	.size = {
+		.width = 246,
+		.height = 184,
+	},
+	.bus_format = MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
+};
+
 static const struct drm_display_mode lg_lb070wv8_mode = {
 	.clock = 33246,
 	.hdisplay = 800,
@@ -1027,6 +1083,30 @@ static const struct panel_desc ortustech_com43h4m85ulc = {
 	.bus_format = MEDIA_BUS_FMT_RGB888_1X24,
 };
 
+static const struct drm_display_mode qd43003c0_40_mode = {
+	.clock = 9000,
+	.hdisplay = 480,
+	.hsync_start = 480 + 8,
+	.hsync_end = 480 + 8 + 4,
+	.htotal = 480 + 8 + 4 + 39,
+	.vdisplay = 272,
+	.vsync_start = 272 + 4,
+	.vsync_end = 272 + 4 + 10,
+	.vtotal = 272 + 4 + 10 + 2,
+	.vrefresh = 60,
+};
+
+static const struct panel_desc qd43003c0_40 = {
+	.modes = &qd43003c0_40_mode,
+	.num_modes = 1,
+	.bpc = 8,
+	.size = {
+		.width = 95,
+		.height = 53,
+	},
+	.bus_format = MEDIA_BUS_FMT_RGB888_1X24,
+};
+
 static const struct drm_display_mode samsung_ltn101nt05_mode = {
 	.clock = 54030,
 	.hdisplay = 1024,
@@ -1158,6 +1238,9 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible ="innolux,g121i1-l01",
 		.data = &innolux_g121i1_l01
 	}, {
+		.compatible = "innolux,g121x1-l03",
+		.data = &innolux_g121x1_l03,
+	}, {
 		.compatible = "innolux,n116bge",
 		.data = &innolux_n116bge,
 	}, {
@@ -1167,6 +1250,9 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible = "innolux,zj070na-01p",
 		.data = &innolux_zj070na_01p,
 	}, {
+		.compatible = "kyo,tcg121xglp",
+		.data = &kyo_tcg121xglp,
+	}, {
 		.compatible = "lg,lb070wv8",
 		.data = &lg_lb070wv8,
 	}, {
@@ -1182,6 +1268,9 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible = "ortustech,com43h4m85ulc",
 		.data = &ortustech_com43h4m85ulc,
 	}, {
+		.compatible = "qiaodian,qd43003c0-40",
+		.data = &qd43003c0_40,
+	}, {
 		.compatible = "samsung,ltn101nt05",
 		.data = &samsung_ltn101nt05,
 	}, {
@@ -1263,6 +1352,36 @@ static const struct panel_desc_dsi auo_b080uan01 = {
 	.lanes = 4,
 };
 
+static const struct drm_display_mode boe_tv080wum_nl0_mode = {
+	.clock = 160000,
+	.hdisplay = 1200,
+	.hsync_start = 1200 + 120,
+	.hsync_end = 1200 + 120 + 20,
+	.htotal = 1200 + 120 + 20 + 21,
+	.vdisplay = 1920,
+	.vsync_start = 1920 + 21,
+	.vsync_end = 1920 + 21 + 3,
+	.vtotal = 1920 + 21 + 3 + 18,
+	.vrefresh = 60,
+	.flags = DRM_MODE_FLAG_NVSYNC | DRM_MODE_FLAG_NHSYNC,
+};
+
+static const struct panel_desc_dsi boe_tv080wum_nl0 = {
+	.desc = {
+		.modes = &boe_tv080wum_nl0_mode,
+		.num_modes = 1,
+		.size = {
+			.width = 107,
+			.height = 172,
+		},
+	},
+	.flags = MIPI_DSI_MODE_VIDEO |
+		 MIPI_DSI_MODE_VIDEO_BURST |
+		 MIPI_DSI_MODE_VIDEO_SYNC_PULSE,
+	.format = MIPI_DSI_FMT_RGB888,
+	.lanes = 4,
+};
+
 static const struct drm_display_mode lg_ld070wx3_sl01_mode = {
 	.clock = 71000,
 	.hdisplay = 800,
@@ -1348,11 +1467,15 @@ static const struct panel_desc_dsi panasonic_vvx10f004b00 = {
 	.lanes = 4,
 };
 
+
 static const struct of_device_id dsi_of_match[] = {
 	{
 		.compatible = "auo,b080uan01",
 		.data = &auo_b080uan01
 	}, {
+		.compatible = "boe,tv080wum-nl0",
+		.data = &boe_tv080wum_nl0
+	}, {
 		.compatible = "lg,ld070wx3-sl01",
 		.data = &lg_ld070wx3_sl01
 	}, {
diff --git a/drivers/gpu/drm/qxl/qxl_display.c b/drivers/gpu/drm/qxl/qxl_display.c
index 183aea1abebc..86276519b2ef 100644
--- a/drivers/gpu/drm/qxl/qxl_display.c
+++ b/drivers/gpu/drm/qxl/qxl_display.c
@@ -521,7 +521,7 @@ static const struct drm_framebuffer_funcs qxl_fb_funcs = {
 int
 qxl_framebuffer_init(struct drm_device *dev,
 		     struct qxl_framebuffer *qfb,
-		     struct drm_mode_fb_cmd2 *mode_cmd,
+		     const struct drm_mode_fb_cmd2 *mode_cmd,
 		     struct drm_gem_object *obj)
 {
 	int ret;
@@ -876,16 +876,6 @@ static const struct drm_connector_helper_funcs qxl_connector_helper_funcs = {
 	.best_encoder = qxl_best_encoder,
 };
 
-static void qxl_conn_save(struct drm_connector *connector)
-{
-	DRM_DEBUG("\n");
-}
-
-static void qxl_conn_restore(struct drm_connector *connector)
-{
-	DRM_DEBUG("\n");
-}
-
 static enum drm_connector_status qxl_conn_detect(
 			struct drm_connector *connector,
 			bool force)
@@ -932,10 +922,8 @@ static void qxl_conn_destroy(struct drm_connector *connector)
 
 static const struct drm_connector_funcs qxl_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
-	.save = qxl_conn_save,
-	.restore = qxl_conn_restore,
 	.detect = qxl_conn_detect,
-	.fill_modes = drm_helper_probe_single_connector_modes_nomerge,
+	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = qxl_conn_set_property,
 	.destroy = qxl_conn_destroy,
 };
@@ -980,7 +968,7 @@ static int qdev_output_init(struct drm_device *dev, int num_output)
 			   &qxl_connector_funcs, DRM_MODE_CONNECTOR_VIRTUAL);
 
 	drm_encoder_init(dev, &qxl_output->enc, &qxl_enc_funcs,
-			 DRM_MODE_ENCODER_VIRTUAL);
+			 DRM_MODE_ENCODER_VIRTUAL, NULL);
 
 	/* we get HPD via client monitors config */
 	connector->polled = DRM_CONNECTOR_POLL_HPD;
@@ -1003,7 +991,7 @@ static int qdev_output_init(struct drm_device *dev, int num_output)
 static struct drm_framebuffer *
 qxl_user_framebuffer_create(struct drm_device *dev,
 			    struct drm_file *file_priv,
-			    struct drm_mode_fb_cmd2 *mode_cmd)
+			    const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct qxl_framebuffer *qxl_fb;
diff --git a/drivers/gpu/drm/qxl/qxl_drv.h b/drivers/gpu/drm/qxl/qxl_drv.h
index 01a86948eb8c..6e6b9b1519b8 100644
--- a/drivers/gpu/drm/qxl/qxl_drv.h
+++ b/drivers/gpu/drm/qxl/qxl_drv.h
@@ -390,7 +390,7 @@ void qxl_fbdev_set_suspend(struct qxl_device *qdev, int state);
 int
 qxl_framebuffer_init(struct drm_device *dev,
 		     struct qxl_framebuffer *rfb,
-		     struct drm_mode_fb_cmd2 *mode_cmd,
+		     const struct drm_mode_fb_cmd2 *mode_cmd,
 		     struct drm_gem_object *obj);
 void qxl_display_read_client_monitors_config(struct qxl_device *qdev);
 void qxl_send_monitors_config(struct qxl_device *qdev);
diff --git a/drivers/gpu/drm/qxl/qxl_fb.c b/drivers/gpu/drm/qxl/qxl_fb.c
index c4a552637c93..7136e521e6db 100644
--- a/drivers/gpu/drm/qxl/qxl_fb.c
+++ b/drivers/gpu/drm/qxl/qxl_fb.c
@@ -40,7 +40,6 @@
 struct qxl_fbdev {
 	struct drm_fb_helper helper;
 	struct qxl_framebuffer	qfb;
-	struct list_head	fbdev_list;
 	struct qxl_device	*qdev;
 
 	spinlock_t delayed_ops_lock;
@@ -283,7 +282,7 @@ int qxl_get_handle_for_primary_fb(struct qxl_device *qdev,
 }
 
 static int qxlfb_create_pinned_object(struct qxl_fbdev *qfbdev,
-				      struct drm_mode_fb_cmd2 *mode_cmd,
+				      const struct drm_mode_fb_cmd2 *mode_cmd,
 				      struct drm_gem_object **gobj_p)
 {
 	struct qxl_device *qdev = qfbdev->qdev;
diff --git a/drivers/gpu/drm/qxl/qxl_ioctl.c b/drivers/gpu/drm/qxl/qxl_ioctl.c
index 2ae8577497ca..7c2e78201ead 100644
--- a/drivers/gpu/drm/qxl/qxl_ioctl.c
+++ b/drivers/gpu/drm/qxl/qxl_ioctl.c
@@ -168,7 +168,8 @@ static int qxl_process_single_command(struct qxl_device *qdev,
 		       cmd->command_size))
 		return -EFAULT;
 
-	reloc_info = kmalloc(sizeof(struct qxl_reloc_info) * cmd->relocs_num, GFP_KERNEL);
+	reloc_info = kmalloc_array(cmd->relocs_num,
+				   sizeof(struct qxl_reloc_info), GFP_KERNEL);
 	if (!reloc_info)
 		return -ENOMEM;
 
diff --git a/drivers/gpu/drm/qxl/qxl_object.c b/drivers/gpu/drm/qxl/qxl_object.c
index b28370e014c6..5e1d7899dd72 100644
--- a/drivers/gpu/drm/qxl/qxl_object.c
+++ b/drivers/gpu/drm/qxl/qxl_object.c
@@ -32,7 +32,7 @@ static void qxl_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 	struct qxl_bo *bo;
 	struct qxl_device *qdev;
 
-	bo = container_of(tbo, struct qxl_bo, tbo);
+	bo = to_qxl_bo(tbo);
 	qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
 
 	qxl_surface_evict(qdev, bo, false);
diff --git a/drivers/gpu/drm/qxl/qxl_prime.c b/drivers/gpu/drm/qxl/qxl_prime.c
index 3d031b50a8fd..9f029dda1f07 100644
--- a/drivers/gpu/drm/qxl/qxl_prime.c
+++ b/drivers/gpu/drm/qxl/qxl_prime.c
@@ -68,5 +68,5 @@ int qxl_gem_prime_mmap(struct drm_gem_object *obj,
 		       struct vm_area_struct *area)
 {
 	WARN_ONCE(1, "not implemented");
-	return ENOSYS;
+	return -ENOSYS;
 }
diff --git a/drivers/gpu/drm/qxl/qxl_ttm.c b/drivers/gpu/drm/qxl/qxl_ttm.c
index 0cbc4c987164..953412766416 100644
--- a/drivers/gpu/drm/qxl/qxl_ttm.c
+++ b/drivers/gpu/drm/qxl/qxl_ttm.c
@@ -201,7 +201,7 @@ static void qxl_evict_flags(struct ttm_buffer_object *bo,
 		placement->num_busy_placement = 1;
 		return;
 	}
-	qbo = container_of(bo, struct qxl_bo, tbo);
+	qbo = to_qxl_bo(bo);
 	qxl_ttm_placement_from_domain(qbo, QXL_GEM_DOMAIN_CPU, false);
 	*placement = qbo->placement;
 }
@@ -365,7 +365,7 @@ static void qxl_bo_move_notify(struct ttm_buffer_object *bo,
 
 	if (!qxl_ttm_bo_is_qxl_bo(bo))
 		return;
-	qbo = container_of(bo, struct qxl_bo, tbo);
+	qbo = to_qxl_bo(bo);
 	qdev = qbo->gem_base.dev->dev_private;
 
 	if (bo->mem.mem_type == TTM_PL_PRIV0 && qbo->surface_id)
diff --git a/drivers/gpu/drm/radeon/Kconfig b/drivers/gpu/drm/radeon/Kconfig
index 421ae130809b..9909f5c68d76 100644
--- a/drivers/gpu/drm/radeon/Kconfig
+++ b/drivers/gpu/drm/radeon/Kconfig
@@ -5,12 +5,3 @@ config DRM_RADEON_USERPTR
 	help
 	  This option selects CONFIG_MMU_NOTIFIER if it isn't already
 	  selected to enabled full userptr support.
-
-config DRM_RADEON_UMS
-	bool "Enable userspace modesetting on radeon (DEPRECATED)"
-	depends on DRM_RADEON
-	help
-	  Choose this option if you still need userspace modesetting.
-
-	  Userspace modesetting is deprecated for quite some time now, so
-	  enable this only if you have ancient versions of the DDX drivers.
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index dea53e36a2ef..08bd17d3925c 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -58,10 +58,6 @@ $(obj)/evergreen_cs.o: $(obj)/evergreen_reg_safe.h $(obj)/cayman_reg_safe.h
 
 radeon-y := radeon_drv.o
 
-# add UMS driver
-radeon-$(CONFIG_DRM_RADEON_UMS)+= radeon_cp.o radeon_state.o radeon_mem.o \
-	radeon_irq.o r300_cmdbuf.o r600_cp.o r600_blit.o drm_buffer.o
-
 # add KMS driver
 radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	radeon_atombios.o radeon_agp.o atombios_crtc.o radeon_combios.o \
diff --git a/drivers/gpu/drm/radeon/atombios_crtc.c b/drivers/gpu/drm/radeon/atombios_crtc.c
index dac78ad24b31..801dd60ac192 100644
--- a/drivers/gpu/drm/radeon/atombios_crtc.c
+++ b/drivers/gpu/drm/radeon/atombios_crtc.c
@@ -25,6 +25,7 @@
  */
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
 #include <drm/radeon_drm.h>
 #include <drm/drm_fixed.h>
 #include "radeon.h"
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index bd73b4069069..6af832545bc5 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -302,77 +302,43 @@ static int convert_bpc_to_bpp(int bpc)
 		return bpc * 3;
 }
 
-/* get the max pix clock supported by the link rate and lane num */
-static int dp_get_max_dp_pix_clock(int link_rate,
-				   int lane_num,
-				   int bpp)
-{
-	return (link_rate * lane_num * 8) / bpp;
-}
-
 /***** radeon specific DP functions *****/
 
-int radeon_dp_get_max_link_rate(struct drm_connector *connector,
-				const u8 dpcd[DP_DPCD_SIZE])
-{
-	int max_link_rate;
-
-	if (radeon_connector_is_dp12_capable(connector))
-		max_link_rate = min(drm_dp_max_link_rate(dpcd), 540000);
-	else
-		max_link_rate = min(drm_dp_max_link_rate(dpcd), 270000);
-
-	return max_link_rate;
-}
-
-/* First get the min lane# when low rate is used according to pixel clock
- * (prefer low rate), second check max lane# supported by DP panel,
- * if the max lane# < low rate lane# then use max lane# instead.
- */
-static int radeon_dp_get_dp_lane_number(struct drm_connector *connector,
-					const u8 dpcd[DP_DPCD_SIZE],
-					int pix_clock)
+int radeon_dp_get_dp_link_config(struct drm_connector *connector,
+				 const u8 dpcd[DP_DPCD_SIZE],
+				 unsigned pix_clock,
+				 unsigned *dp_lanes, unsigned *dp_rate)
 {
 	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
-	int max_link_rate = radeon_dp_get_max_link_rate(connector, dpcd);
-	int max_lane_num = drm_dp_max_lane_count(dpcd);
-	int lane_num;
-	int max_dp_pix_clock;
-
-	for (lane_num = 1; lane_num < max_lane_num; lane_num <<= 1) {
-		max_dp_pix_clock = dp_get_max_dp_pix_clock(max_link_rate, lane_num, bpp);
-		if (pix_clock <= max_dp_pix_clock)
-			break;
-	}
-
-	return lane_num;
-}
-
-static int radeon_dp_get_dp_link_clock(struct drm_connector *connector,
-				       const u8 dpcd[DP_DPCD_SIZE],
-				       int pix_clock)
-{
-	int bpp = convert_bpc_to_bpp(radeon_get_monitor_bpc(connector));
-	int lane_num, max_pix_clock;
+	static const unsigned link_rates[3] = { 162000, 270000, 540000 };
+	unsigned max_link_rate = drm_dp_max_link_rate(dpcd);
+	unsigned max_lane_num = drm_dp_max_lane_count(dpcd);
+	unsigned lane_num, i, max_pix_clock;
 
 	if (radeon_connector_encoder_get_dp_bridge_encoder_id(connector) ==
-	    ENCODER_OBJECT_ID_NUTMEG)
-		return 270000;
-
-	lane_num = radeon_dp_get_dp_lane_number(connector, dpcd, pix_clock);
-	max_pix_clock = dp_get_max_dp_pix_clock(162000, lane_num, bpp);
-	if (pix_clock <= max_pix_clock)
-		return 162000;
-	max_pix_clock = dp_get_max_dp_pix_clock(270000, lane_num, bpp);
-	if (pix_clock <= max_pix_clock)
-		return 270000;
-	if (radeon_connector_is_dp12_capable(connector)) {
-		max_pix_clock = dp_get_max_dp_pix_clock(540000, lane_num, bpp);
-		if (pix_clock <= max_pix_clock)
-			return 540000;
+	    ENCODER_OBJECT_ID_NUTMEG) {
+		for (lane_num = 1; lane_num <= max_lane_num; lane_num <<= 1) {
+			max_pix_clock = (lane_num * 270000 * 8) / bpp;
+			if (max_pix_clock >= pix_clock) {
+				*dp_lanes = lane_num;
+				*dp_rate = 270000;
+				return 0;
+			}
+		}
+	} else {
+		for (lane_num = 1; lane_num <= max_lane_num; lane_num <<= 1) {
+			for (i = 0; i < ARRAY_SIZE(link_rates) && link_rates[i] <= max_link_rate; i++) {
+				max_pix_clock = (lane_num * link_rates[i] * 8) / bpp;
+				if (max_pix_clock >= pix_clock) {
+					*dp_lanes = lane_num;
+					*dp_rate = link_rates[i];
+					return 0;
+				}
+			}
+		}
 	}
 
-	return radeon_dp_get_max_link_rate(connector, dpcd);
+	return -EINVAL;
 }
 
 static u8 radeon_dp_encoder_service(struct radeon_device *rdev,
@@ -491,6 +457,7 @@ void radeon_dp_set_link_config(struct drm_connector *connector,
 {
 	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 	struct radeon_connector_atom_dig *dig_connector;
+	int ret;
 
 	if (!radeon_connector->con_priv)
 		return;
@@ -498,10 +465,14 @@ void radeon_dp_set_link_config(struct drm_connector *connector,
 
 	if ((dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_DISPLAYPORT) ||
 	    (dig_connector->dp_sink_type == CONNECTOR_OBJECT_ID_eDP)) {
-		dig_connector->dp_clock =
-			radeon_dp_get_dp_link_clock(connector, dig_connector->dpcd, mode->clock);
-		dig_connector->dp_lane_count =
-			radeon_dp_get_dp_lane_number(connector, dig_connector->dpcd, mode->clock);
+		ret = radeon_dp_get_dp_link_config(connector, dig_connector->dpcd,
+						   mode->clock,
+						   &dig_connector->dp_lane_count,
+						   &dig_connector->dp_clock);
+		if (ret) {
+			dig_connector->dp_clock = 0;
+			dig_connector->dp_lane_count = 0;
+		}
 	}
 }
 
@@ -510,7 +481,8 @@ int radeon_dp_mode_valid_helper(struct drm_connector *connector,
 {
 	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
 	struct radeon_connector_atom_dig *dig_connector;
-	int dp_clock;
+	unsigned dp_clock, dp_lanes;
+	int ret;
 
 	if ((mode->clock > 340000) &&
 	    (!radeon_connector_is_dp12_capable(connector)))
@@ -520,8 +492,12 @@ int radeon_dp_mode_valid_helper(struct drm_connector *connector,
 		return MODE_CLOCK_HIGH;
 	dig_connector = radeon_connector->con_priv;
 
-	dp_clock =
-		radeon_dp_get_dp_link_clock(connector, dig_connector->dpcd, mode->clock);
+	ret = radeon_dp_get_dp_link_config(connector, dig_connector->dpcd,
+					   mode->clock,
+					   &dp_lanes,
+					   &dp_clock);
+	if (ret)
+		return MODE_CLOCK_HIGH;
 
 	if ((dp_clock == 540000) &&
 	    (!radeon_connector_is_dp12_capable(connector)))
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index bb292143997e..01b20e14a247 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -2767,23 +2767,27 @@ radeon_add_atom_encoder(struct drm_device *dev,
 	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
 		if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
 			radeon_encoder->rmx_type = RMX_FULL;
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_LVDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_LVDS, NULL);
 			radeon_encoder->enc_priv = radeon_atombios_get_lvds_info(radeon_encoder);
 		} else {
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_TMDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_TMDS, NULL);
 			radeon_encoder->enc_priv = radeon_atombios_set_dig_info(radeon_encoder);
 		}
 		drm_encoder_helper_add(encoder, &radeon_atom_dig_helper_funcs);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DAC1:
-		drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_DAC);
+		drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+				 DRM_MODE_ENCODER_DAC, NULL);
 		radeon_encoder->enc_priv = radeon_atombios_set_dac_info(radeon_encoder);
 		drm_encoder_helper_add(encoder, &radeon_atom_dac_helper_funcs);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DAC2:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC1:
 	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DAC2:
-		drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_TVDAC);
+		drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+				 DRM_MODE_ENCODER_TVDAC, NULL);
 		radeon_encoder->enc_priv = radeon_atombios_set_dac_info(radeon_encoder);
 		drm_encoder_helper_add(encoder, &radeon_atom_dac_helper_funcs);
 		break;
@@ -2797,13 +2801,16 @@ radeon_add_atom_encoder(struct drm_device *dev,
 	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY3:
 		if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
 			radeon_encoder->rmx_type = RMX_FULL;
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_LVDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_LVDS, NULL);
 			radeon_encoder->enc_priv = radeon_atombios_get_lvds_info(radeon_encoder);
 		} else if (radeon_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT)) {
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_DAC);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_DAC, NULL);
 			radeon_encoder->enc_priv = radeon_atombios_set_dig_info(radeon_encoder);
 		} else {
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_TMDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_TMDS, NULL);
 			radeon_encoder->enc_priv = radeon_atombios_set_dig_info(radeon_encoder);
 		}
 		drm_encoder_helper_add(encoder, &radeon_atom_dig_helper_funcs);
@@ -2820,11 +2827,14 @@ radeon_add_atom_encoder(struct drm_device *dev,
 		/* these are handled by the primary encoders */
 		radeon_encoder->is_ext_encoder = true;
 		if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT))
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_LVDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_LVDS, NULL);
 		else if (radeon_encoder->devices & (ATOM_DEVICE_CRT_SUPPORT))
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_DAC);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_DAC, NULL);
 		else
-			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs, DRM_MODE_ENCODER_TMDS);
+			drm_encoder_init(dev, encoder, &radeon_atom_enc_funcs,
+					 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &radeon_atom_ext_helper_funcs);
 		break;
 	}
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index f81fb2641097..4c30d8c65558 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -4132,10 +4132,10 @@ struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
  * @rdev: radeon_device pointer
  * @ib: radeon indirect buffer object
  *
- * Emits an DE (drawing engine) or CE (constant engine) IB
+ * Emits a DE (drawing engine) or CE (constant engine) IB
  * on the gfx ring.  IBs are usually generated by userspace
  * acceleration drivers and submitted to the kernel for
- * sheduling on the ring.  This function schedules the IB
+ * scheduling on the ring.  This function schedules the IB
  * on the gfx ring for execution by the GPU.
  */
 void cik_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
diff --git a/drivers/gpu/drm/radeon/dce6_afmt.c b/drivers/gpu/drm/radeon/dce6_afmt.c
index 752072771388..367a916f364e 100644
--- a/drivers/gpu/drm/radeon/dce6_afmt.c
+++ b/drivers/gpu/drm/radeon/dce6_afmt.c
@@ -301,6 +301,14 @@ void dce6_dp_audio_set_dto(struct radeon_device *rdev,
 	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
 	 */
 	if (ASIC_IS_DCE8(rdev)) {
+		unsigned int div = (RREG32(DENTIST_DISPCLK_CNTL) &
+			DENTIST_DPREFCLK_WDIVIDER_MASK) >>
+			DENTIST_DPREFCLK_WDIVIDER_SHIFT;
+		div = radeon_audio_decode_dfs_div(div);
+
+		if (div)
+			clock = clock * 100 / div;
+
 		WREG32(DCE8_DCCG_AUDIO_DTO1_PHASE, 24000);
 		WREG32(DCE8_DCCG_AUDIO_DTO1_MODULE, clock);
 	} else {
diff --git a/drivers/gpu/drm/radeon/drm_buffer.c b/drivers/gpu/drm/radeon/drm_buffer.c
deleted file mode 100644
index f4e0f3a3d7b1..000000000000
--- a/drivers/gpu/drm/radeon/drm_buffer.c
+++ /dev/null
@@ -1,177 +0,0 @@
-/**************************************************************************
- *
- * Copyright 2010 Pauli Nieminen.
- * All Rights Reserved.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the
- * "Software"), to deal in the Software without restriction, including
- * without limitation the rights to use, copy, modify, merge, publish,
- * distribute, sub license, and/or sell copies of the Software, and to
- * permit persons to whom the Software is furnished to do so, subject to
- * the following conditions:
- *
- * The above copyright notice and this permission notice (including the
- * next paragraph) shall be included in all copies or substantial portions
- * of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
- * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
- * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
- * USE OR OTHER DEALINGS IN THE SOFTWARE.
- *
- *
- **************************************************************************/
-/*
- * Multipart buffer for coping data which is larger than the page size.
- *
- * Authors:
- * Pauli Nieminen <suokkos-at-gmail-dot-com>
- */
-
-#include <linux/export.h>
-#include "drm_buffer.h"
-
-/**
- * Allocate the drm buffer object.
- *
- *   buf: Pointer to a pointer where the object is stored.
- *   size: The number of bytes to allocate.
- */
-int drm_buffer_alloc(struct drm_buffer **buf, int size)
-{
-	int nr_pages = size / PAGE_SIZE + 1;
-	int idx;
-
-	/* Allocating pointer table to end of structure makes drm_buffer
-	 * variable sized */
-	*buf = kzalloc(sizeof(struct drm_buffer) + nr_pages*sizeof(char *),
-			GFP_KERNEL);
-
-	if (*buf == NULL) {
-		DRM_ERROR("Failed to allocate drm buffer object to hold"
-				" %d bytes in %d pages.\n",
-				size, nr_pages);
-		return -ENOMEM;
-	}
-
-	(*buf)->size = size;
-
-	for (idx = 0; idx < nr_pages; ++idx) {
-
-		(*buf)->data[idx] =
-			kmalloc(min(PAGE_SIZE, size - idx * PAGE_SIZE),
-				GFP_KERNEL);
-
-
-		if ((*buf)->data[idx] == NULL) {
-			DRM_ERROR("Failed to allocate %dth page for drm"
-					" buffer with %d bytes and %d pages.\n",
-					idx + 1, size, nr_pages);
-			goto error_out;
-		}
-
-	}
-
-	return 0;
-
-error_out:
-
-	for (; idx >= 0; --idx)
-		kfree((*buf)->data[idx]);
-
-	kfree(*buf);
-	return -ENOMEM;
-}
-
-/**
- * Copy the user data to the begin of the buffer and reset the processing
- * iterator.
- *
- *   user_data: A pointer the data that is copied to the buffer.
- *   size: The Number of bytes to copy.
- */
-int drm_buffer_copy_from_user(struct drm_buffer *buf,
-			      void __user *user_data, int size)
-{
-	int nr_pages = size / PAGE_SIZE + 1;
-	int idx;
-
-	if (size > buf->size) {
-		DRM_ERROR("Requesting to copy %d bytes to a drm buffer with"
-				" %d bytes space\n",
-				size, buf->size);
-		return -EFAULT;
-	}
-
-	for (idx = 0; idx < nr_pages; ++idx) {
-
-		if (copy_from_user(buf->data[idx],
-			user_data + idx * PAGE_SIZE,
-			min(PAGE_SIZE, size - idx * PAGE_SIZE))) {
-			DRM_ERROR("Failed to copy user data (%p) to drm buffer"
-					" (%p) %dth page.\n",
-					user_data, buf, idx);
-			return -EFAULT;
-
-		}
-	}
-	buf->iterator = 0;
-	return 0;
-}
-
-/**
- * Free the drm buffer object
- */
-void drm_buffer_free(struct drm_buffer *buf)
-{
-
-	if (buf != NULL) {
-
-		int nr_pages = buf->size / PAGE_SIZE + 1;
-		int idx;
-		for (idx = 0; idx < nr_pages; ++idx)
-			kfree(buf->data[idx]);
-
-		kfree(buf);
-	}
-}
-
-/**
- * Read an object from buffer that may be split to multiple parts. If object
- * is not split function just returns the pointer to object in buffer. But in
- * case of split object data is copied to given stack object that is suplied
- * by caller.
- *
- * The processing location of the buffer is also advanced to the next byte
- * after the object.
- *
- *   objsize: The size of the objet in bytes.
- *   stack_obj: A pointer to a memory location where object can be copied.
- */
-void *drm_buffer_read_object(struct drm_buffer *buf,
-		int objsize, void *stack_obj)
-{
-	int idx = drm_buffer_index(buf);
-	int page = drm_buffer_page(buf);
-	void *obj = NULL;
-
-	if (idx + objsize <= PAGE_SIZE) {
-		obj = &buf->data[page][idx];
-	} else {
-		/* The object is split which forces copy to temporary object.*/
-		int beginsz = PAGE_SIZE - idx;
-		memcpy(stack_obj, &buf->data[page][idx], beginsz);
-
-		memcpy(stack_obj + beginsz, &buf->data[page + 1][0],
-				objsize - beginsz);
-
-		obj = stack_obj;
-	}
-
-	drm_buffer_advance(buf, objsize);
-	return obj;
-}
diff --git a/drivers/gpu/drm/radeon/drm_buffer.h b/drivers/gpu/drm/radeon/drm_buffer.h
deleted file mode 100644
index c80d3a340b94..000000000000
--- a/drivers/gpu/drm/radeon/drm_buffer.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/**************************************************************************
- *
- * Copyright 2010 Pauli Nieminen.
- * All Rights Reserved.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the
- * "Software"), to deal in the Software without restriction, including
- * without limitation the rights to use, copy, modify, merge, publish,
- * distribute, sub license, and/or sell copies of the Software, and to
- * permit persons to whom the Software is furnished to do so, subject to
- * the following conditions:
- *
- * The above copyright notice and this permission notice (including the
- * next paragraph) shall be included in all copies or substantial portions
- * of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
- * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
- * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
- * USE OR OTHER DEALINGS IN THE SOFTWARE.
- *
- *
- **************************************************************************/
-/*
- * Multipart buffer for coping data which is larger than the page size.
- *
- * Authors:
- * Pauli Nieminen <suokkos-at-gmail-dot-com>
- */
-
-#ifndef _DRM_BUFFER_H_
-#define _DRM_BUFFER_H_
-
-#include <drm/drmP.h>
-
-struct drm_buffer {
-	int iterator;
-	int size;
-	char *data[];
-};
-
-
-/**
- * Return the index of page that buffer is currently pointing at.
- */
-static inline int drm_buffer_page(struct drm_buffer *buf)
-{
-	return buf->iterator / PAGE_SIZE;
-}
-/**
- * Return the index of the current byte in the page
- */
-static inline int drm_buffer_index(struct drm_buffer *buf)
-{
-	return buf->iterator & (PAGE_SIZE - 1);
-}
-/**
- * Return number of bytes that is left to process
- */
-static inline int drm_buffer_unprocessed(struct drm_buffer *buf)
-{
-	return buf->size - buf->iterator;
-}
-
-/**
- * Advance the buffer iterator number of bytes that is given.
- */
-static inline void drm_buffer_advance(struct drm_buffer *buf, int bytes)
-{
-	buf->iterator += bytes;
-}
-
-/**
- * Allocate the drm buffer object.
- *
- *   buf: A pointer to a pointer where the object is stored.
- *   size: The number of bytes to allocate.
- */
-extern int drm_buffer_alloc(struct drm_buffer **buf, int size);
-
-/**
- * Copy the user data to the begin of the buffer and reset the processing
- * iterator.
- *
- *   user_data: A pointer the data that is copied to the buffer.
- *   size: The Number of bytes to copy.
- */
-extern int drm_buffer_copy_from_user(struct drm_buffer *buf,
-		void __user *user_data, int size);
-
-/**
- * Free the drm buffer object
- */
-extern void drm_buffer_free(struct drm_buffer *buf);
-
-/**
- * Read an object from buffer that may be split to multiple parts. If object
- * is not split function just returns the pointer to object in buffer. But in
- * case of split object data is copied to given stack object that is suplied
- * by caller.
- *
- * The processing location of the buffer is also advanced to the next byte
- * after the object.
- *
- *   objsize: The size of the objet in bytes.
- *   stack_obj: A pointer to a memory location where object can be copied.
- */
-extern void *drm_buffer_read_object(struct drm_buffer *buf,
-		int objsize, void *stack_obj);
-
-/**
- * Returns the pointer to the dword which is offset number of elements from the
- * current processing location.
- *
- * Caller must make sure that dword is not split in the buffer. This
- * requirement is easily met if all the sizes of objects in buffer are
- * multiples of dword and PAGE_SIZE is multiple dword.
- *
- * Call to this function doesn't change the processing location.
- *
- *   offset: The index of the dword relative to the internat iterator.
- */
-static inline void *drm_buffer_pointer_to_dword(struct drm_buffer *buffer,
-		int offset)
-{
-	int iter = buffer->iterator + offset * 4;
-	return &buffer->data[iter / PAGE_SIZE][iter & (PAGE_SIZE - 1)];
-}
-/**
- * Returns the pointer to the dword which is offset number of elements from
- * the current processing location.
- *
- * Call to this function doesn't change the processing location.
- *
- *   offset: The index of the byte relative to the internat iterator.
- */
-static inline void *drm_buffer_pointer_to_byte(struct drm_buffer *buffer,
-		int offset)
-{
-	int iter = buffer->iterator + offset;
-	return &buffer->data[iter / PAGE_SIZE][iter & (PAGE_SIZE - 1)];
-}
-
-#endif
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
index 9953356fe263..3cf04a2f44bb 100644
--- a/drivers/gpu/drm/radeon/evergreen_hdmi.c
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -289,6 +289,16 @@ void dce4_dp_audio_set_dto(struct radeon_device *rdev,
 	 * number (coefficient of two integer numbers.  DCCG_AUDIO_DTOx_PHASE
 	 * is the numerator, DCCG_AUDIO_DTOx_MODULE is the denominator
 	 */
+	if (ASIC_IS_DCE41(rdev)) {
+		unsigned int div = (RREG32(DCE41_DENTIST_DISPCLK_CNTL) &
+			DENTIST_DPREFCLK_WDIVIDER_MASK) >>
+			DENTIST_DPREFCLK_WDIVIDER_SHIFT;
+		div = radeon_audio_decode_dfs_div(div);
+
+		if (div)
+			clock = 100 * clock / div;
+	}
+
 	WREG32(DCCG_AUDIO_DTO1_PHASE, 24000);
 	WREG32(DCCG_AUDIO_DTO1_MODULE, clock);
 }
diff --git a/drivers/gpu/drm/radeon/evergreend.h b/drivers/gpu/drm/radeon/evergreend.h
index 4aa5f755572b..13b6029d65cc 100644
--- a/drivers/gpu/drm/radeon/evergreend.h
+++ b/drivers/gpu/drm/radeon/evergreend.h
@@ -511,6 +511,11 @@
 #define DCCG_AUDIO_DTO1_CNTL              0x05cc
 #       define DCCG_AUDIO_DTO1_USE_512FBR_DTO (1 << 3)
 
+#define DCE41_DENTIST_DISPCLK_CNTL			0x049c
+#       define DENTIST_DPREFCLK_WDIVIDER(x)		(((x) & 0x7f) << 24)
+#       define DENTIST_DPREFCLK_WDIVIDER_MASK		(0x7f << 24)
+#       define DENTIST_DPREFCLK_WDIVIDER_SHIFT		24
+
 /* DCE 4.0 AFMT */
 #define HDMI_CONTROL                         0x7030
 #       define HDMI_KEEPOUT_MODE             (1 << 0)
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 9e7e2bf03b81..5eae0a88dd3e 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -3150,7 +3150,8 @@ void r100_bandwidth_update(struct radeon_device *rdev)
 {
 	fixed20_12 trcd_ff, trp_ff, tras_ff, trbs_ff, tcas_ff;
 	fixed20_12 sclk_ff, mclk_ff, sclk_eff_ff, sclk_delay_ff;
-	fixed20_12 peak_disp_bw, mem_bw, pix_clk, pix_clk2, temp_ff, crit_point_ff;
+	fixed20_12 peak_disp_bw, mem_bw, pix_clk, pix_clk2, temp_ff;
+	fixed20_12 crit_point_ff = {0};
 	uint32_t temp, data, mem_trcd, mem_trp, mem_tras;
 	fixed20_12 memtcas_ff[8] = {
 		dfixed_init(1),
@@ -3204,7 +3205,7 @@ void r100_bandwidth_update(struct radeon_device *rdev)
 	fixed20_12 min_mem_eff;
 	fixed20_12 mc_latency_sclk, mc_latency_mclk, k1;
 	fixed20_12 cur_latency_mclk, cur_latency_sclk;
-	fixed20_12 disp_latency, disp_latency_overhead, disp_drain_rate,
+	fixed20_12 disp_latency, disp_latency_overhead, disp_drain_rate = {0},
 		disp_drain_rate2, read_return_rate;
 	fixed20_12 time_disp1_drop_priority;
 	int c;
diff --git a/drivers/gpu/drm/radeon/r300_cmdbuf.c b/drivers/gpu/drm/radeon/r300_cmdbuf.c
deleted file mode 100644
index 9418e388b045..000000000000
--- a/drivers/gpu/drm/radeon/r300_cmdbuf.c
+++ /dev/null
@@ -1,1186 +0,0 @@
-/* r300_cmdbuf.c -- Command buffer emission for R300 -*- linux-c -*-
- *
- * Copyright (C) The Weather Channel, Inc.  2002.
- * Copyright (C) 2004 Nicolai Haehnle.
- * All Rights Reserved.
- *
- * The Weather Channel (TM) funded Tungsten Graphics to develop the
- * initial release of the Radeon 8500 driver under the XFree86 license.
- * This notice must be preserved.
- *
- * 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
- * PRECISION INSIGHT AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Nicolai Haehnle <prefect_@gmx.net>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-#include "r300_reg.h"
-#include "drm_buffer.h"
-
-#include <asm/unaligned.h>
-
-#define R300_SIMULTANEOUS_CLIPRECTS		4
-
-/* Values for R300_RE_CLIPRECT_CNTL depending on the number of cliprects
- */
-static const int r300_cliprect_cntl[4] = {
-	0xAAAA,
-	0xEEEE,
-	0xFEFE,
-	0xFFFE
-};
-
-/**
- * Emit up to R300_SIMULTANEOUS_CLIPRECTS cliprects from the given command
- * buffer, starting with index n.
- */
-static int r300_emit_cliprects(drm_radeon_private_t *dev_priv,
-			       drm_radeon_kcmd_buffer_t *cmdbuf, int n)
-{
-	struct drm_clip_rect box;
-	int nr;
-	int i;
-	RING_LOCALS;
-
-	nr = cmdbuf->nbox - n;
-	if (nr > R300_SIMULTANEOUS_CLIPRECTS)
-		nr = R300_SIMULTANEOUS_CLIPRECTS;
-
-	DRM_DEBUG("%i cliprects\n", nr);
-
-	if (nr) {
-		BEGIN_RING(6 + nr * 2);
-		OUT_RING(CP_PACKET0(R300_RE_CLIPRECT_TL_0, nr * 2 - 1));
-
-		for (i = 0; i < nr; ++i) {
-			if (copy_from_user
-			    (&box, &cmdbuf->boxes[n + i], sizeof(box))) {
-				DRM_ERROR("copy cliprect faulted\n");
-				return -EFAULT;
-			}
-
-			box.x2--; /* Hardware expects inclusive bottom-right corner */
-			box.y2--;
-
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV515) {
-				box.x1 = (box.x1) &
-					R300_CLIPRECT_MASK;
-				box.y1 = (box.y1) &
-					R300_CLIPRECT_MASK;
-				box.x2 = (box.x2) &
-					R300_CLIPRECT_MASK;
-				box.y2 = (box.y2) &
-					R300_CLIPRECT_MASK;
-			} else {
-				box.x1 = (box.x1 + R300_CLIPRECT_OFFSET) &
-					R300_CLIPRECT_MASK;
-				box.y1 = (box.y1 + R300_CLIPRECT_OFFSET) &
-					R300_CLIPRECT_MASK;
-				box.x2 = (box.x2 + R300_CLIPRECT_OFFSET) &
-					R300_CLIPRECT_MASK;
-				box.y2 = (box.y2 + R300_CLIPRECT_OFFSET) &
-					R300_CLIPRECT_MASK;
-			}
-
-			OUT_RING((box.x1 << R300_CLIPRECT_X_SHIFT) |
-				 (box.y1 << R300_CLIPRECT_Y_SHIFT));
-			OUT_RING((box.x2 << R300_CLIPRECT_X_SHIFT) |
-				 (box.y2 << R300_CLIPRECT_Y_SHIFT));
-
-		}
-
-		OUT_RING_REG(R300_RE_CLIPRECT_CNTL, r300_cliprect_cntl[nr - 1]);
-
-		/* TODO/SECURITY: Force scissors to a safe value, otherwise the
-		 * client might be able to trample over memory.
-		 * The impact should be very limited, but I'd rather be safe than
-		 * sorry.
-		 */
-		OUT_RING(CP_PACKET0(R300_RE_SCISSORS_TL, 1));
-		OUT_RING(0);
-		OUT_RING(R300_SCISSORS_X_MASK | R300_SCISSORS_Y_MASK);
-		ADVANCE_RING();
-	} else {
-		/* Why we allow zero cliprect rendering:
-		 * There are some commands in a command buffer that must be submitted
-		 * even when there are no cliprects, e.g. DMA buffer discard
-		 * or state setting (though state setting could be avoided by
-		 * simulating a loss of context).
-		 *
-		 * Now since the cmdbuf interface is so chaotic right now (and is
-		 * bound to remain that way for a bit until things settle down),
-		 * it is basically impossible to filter out the commands that are
-		 * necessary and those that aren't.
-		 *
-		 * So I choose the safe way and don't do any filtering at all;
-		 * instead, I simply set up the engine so that all rendering
-		 * can't produce any fragments.
-		 */
-		BEGIN_RING(2);
-		OUT_RING_REG(R300_RE_CLIPRECT_CNTL, 0);
-		ADVANCE_RING();
-	}
-
-	/* flus cache and wait idle clean after cliprect change */
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	OUT_RING(R300_RB3D_DC_FLUSH);
-	ADVANCE_RING();
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(RADEON_WAIT_3D_IDLECLEAN);
-	ADVANCE_RING();
-	/* set flush flag */
-	dev_priv->track_flush |= RADEON_FLUSH_EMITED;
-
-	return 0;
-}
-
-static u8 r300_reg_flags[0x10000 >> 2];
-
-void r300_init_reg_flags(struct drm_device *dev)
-{
-	int i;
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	memset(r300_reg_flags, 0, 0x10000 >> 2);
-#define ADD_RANGE_MARK(reg, count,mark) \
-		for(i=((reg)>>2);i<((reg)>>2)+(count);i++)\
-			r300_reg_flags[i]|=(mark);
-
-#define MARK_SAFE		1
-#define MARK_CHECK_OFFSET	2
-
-#define ADD_RANGE(reg, count)	ADD_RANGE_MARK(reg, count, MARK_SAFE)
-
-	/* these match cmducs() command in r300_driver/r300/r300_cmdbuf.c */
-	ADD_RANGE(R300_SE_VPORT_XSCALE, 6);
-	ADD_RANGE(R300_VAP_CNTL, 1);
-	ADD_RANGE(R300_SE_VTE_CNTL, 2);
-	ADD_RANGE(0x2134, 2);
-	ADD_RANGE(R300_VAP_CNTL_STATUS, 1);
-	ADD_RANGE(R300_VAP_INPUT_CNTL_0, 2);
-	ADD_RANGE(0x21DC, 1);
-	ADD_RANGE(R300_VAP_UNKNOWN_221C, 1);
-	ADD_RANGE(R300_VAP_CLIP_X_0, 4);
-	ADD_RANGE(R300_VAP_PVS_STATE_FLUSH_REG, 1);
-	ADD_RANGE(R300_VAP_UNKNOWN_2288, 1);
-	ADD_RANGE(R300_VAP_OUTPUT_VTX_FMT_0, 2);
-	ADD_RANGE(R300_VAP_PVS_CNTL_1, 3);
-	ADD_RANGE(R300_GB_ENABLE, 1);
-	ADD_RANGE(R300_GB_MSPOS0, 5);
-	ADD_RANGE(R300_TX_INVALTAGS, 1);
-	ADD_RANGE(R300_TX_ENABLE, 1);
-	ADD_RANGE(0x4200, 4);
-	ADD_RANGE(0x4214, 1);
-	ADD_RANGE(R300_RE_POINTSIZE, 1);
-	ADD_RANGE(0x4230, 3);
-	ADD_RANGE(R300_RE_LINE_CNT, 1);
-	ADD_RANGE(R300_RE_UNK4238, 1);
-	ADD_RANGE(0x4260, 3);
-	ADD_RANGE(R300_RE_SHADE, 4);
-	ADD_RANGE(R300_RE_POLYGON_MODE, 5);
-	ADD_RANGE(R300_RE_ZBIAS_CNTL, 1);
-	ADD_RANGE(R300_RE_ZBIAS_T_FACTOR, 4);
-	ADD_RANGE(R300_RE_OCCLUSION_CNTL, 1);
-	ADD_RANGE(R300_RE_CULL_CNTL, 1);
-	ADD_RANGE(0x42C0, 2);
-	ADD_RANGE(R300_RS_CNTL_0, 2);
-
-	ADD_RANGE(R300_SU_REG_DEST, 1);
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV530)
-		ADD_RANGE(RV530_FG_ZBREG_DEST, 1);
-
-	ADD_RANGE(R300_SC_HYPERZ, 2);
-	ADD_RANGE(0x43E8, 1);
-
-	ADD_RANGE(0x46A4, 5);
-
-	ADD_RANGE(R300_RE_FOG_STATE, 1);
-	ADD_RANGE(R300_FOG_COLOR_R, 3);
-	ADD_RANGE(R300_PP_ALPHA_TEST, 2);
-	ADD_RANGE(0x4BD8, 1);
-	ADD_RANGE(R300_PFS_PARAM_0_X, 64);
-	ADD_RANGE(0x4E00, 1);
-	ADD_RANGE(R300_RB3D_CBLEND, 2);
-	ADD_RANGE(R300_RB3D_COLORMASK, 1);
-	ADD_RANGE(R300_RB3D_BLEND_COLOR, 3);
-	ADD_RANGE_MARK(R300_RB3D_COLOROFFSET0, 1, MARK_CHECK_OFFSET);	/* check offset */
-	ADD_RANGE(R300_RB3D_COLORPITCH0, 1);
-	ADD_RANGE(0x4E50, 9);
-	ADD_RANGE(0x4E88, 1);
-	ADD_RANGE(0x4EA0, 2);
-	ADD_RANGE(R300_ZB_CNTL, 3);
-	ADD_RANGE(R300_ZB_FORMAT, 4);
-	ADD_RANGE_MARK(R300_ZB_DEPTHOFFSET, 1, MARK_CHECK_OFFSET);	/* check offset */
-	ADD_RANGE(R300_ZB_DEPTHPITCH, 1);
-	ADD_RANGE(R300_ZB_DEPTHCLEARVALUE, 1);
-	ADD_RANGE(R300_ZB_ZMASK_OFFSET, 13);
-	ADD_RANGE(R300_ZB_ZPASS_DATA, 2); /* ZB_ZPASS_DATA, ZB_ZPASS_ADDR */
-
-	ADD_RANGE(R300_TX_FILTER_0, 16);
-	ADD_RANGE(R300_TX_FILTER1_0, 16);
-	ADD_RANGE(R300_TX_SIZE_0, 16);
-	ADD_RANGE(R300_TX_FORMAT_0, 16);
-	ADD_RANGE(R300_TX_PITCH_0, 16);
-	/* Texture offset is dangerous and needs more checking */
-	ADD_RANGE_MARK(R300_TX_OFFSET_0, 16, MARK_CHECK_OFFSET);
-	ADD_RANGE(R300_TX_CHROMA_KEY_0, 16);
-	ADD_RANGE(R300_TX_BORDER_COLOR_0, 16);
-
-	/* Sporadic registers used as primitives are emitted */
-	ADD_RANGE(R300_ZB_ZCACHE_CTLSTAT, 1);
-	ADD_RANGE(R300_RB3D_DSTCACHE_CTLSTAT, 1);
-	ADD_RANGE(R300_VAP_INPUT_ROUTE_0_0, 8);
-	ADD_RANGE(R300_VAP_INPUT_ROUTE_1_0, 8);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV515) {
-		ADD_RANGE(R500_VAP_INDEX_OFFSET, 1);
-		ADD_RANGE(R500_US_CONFIG, 2);
-		ADD_RANGE(R500_US_CODE_ADDR, 3);
-		ADD_RANGE(R500_US_FC_CTRL, 1);
-		ADD_RANGE(R500_RS_IP_0, 16);
-		ADD_RANGE(R500_RS_INST_0, 16);
-		ADD_RANGE(R500_RB3D_COLOR_CLEAR_VALUE_AR, 2);
-		ADD_RANGE(R500_RB3D_CONSTANT_COLOR_AR, 2);
-		ADD_RANGE(R500_ZB_FIFO_SIZE, 2);
-	} else {
-		ADD_RANGE(R300_PFS_CNTL_0, 3);
-		ADD_RANGE(R300_PFS_NODE_0, 4);
-		ADD_RANGE(R300_PFS_TEXI_0, 64);
-		ADD_RANGE(R300_PFS_INSTR0_0, 64);
-		ADD_RANGE(R300_PFS_INSTR1_0, 64);
-		ADD_RANGE(R300_PFS_INSTR2_0, 64);
-		ADD_RANGE(R300_PFS_INSTR3_0, 64);
-		ADD_RANGE(R300_RS_INTERP_0, 8);
-		ADD_RANGE(R300_RS_ROUTE_0, 8);
-
-	}
-}
-
-static __inline__ int r300_check_range(unsigned reg, int count)
-{
-	int i;
-	if (reg & ~0xffff)
-		return -1;
-	for (i = (reg >> 2); i < (reg >> 2) + count; i++)
-		if (r300_reg_flags[i] != MARK_SAFE)
-			return 1;
-	return 0;
-}
-
-static __inline__ int r300_emit_carefully_checked_packet0(drm_radeon_private_t *
-							  dev_priv,
-							  drm_radeon_kcmd_buffer_t
-							  * cmdbuf,
-							  drm_r300_cmd_header_t
-							  header)
-{
-	int reg;
-	int sz;
-	int i;
-	u32 *value;
-	RING_LOCALS;
-
-	sz = header.packet0.count;
-	reg = (header.packet0.reghi << 8) | header.packet0.reglo;
-
-	if ((sz > 64) || (sz < 0)) {
-		DRM_ERROR("Cannot emit more than 64 values at a time (reg=%04x sz=%d)\n",
-			 reg, sz);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < sz; i++) {
-		switch (r300_reg_flags[(reg >> 2) + i]) {
-		case MARK_SAFE:
-			break;
-		case MARK_CHECK_OFFSET:
-			value = drm_buffer_pointer_to_dword(cmdbuf->buffer, i);
-			if (!radeon_check_offset(dev_priv, *value)) {
-				DRM_ERROR("Offset failed range check (reg=%04x sz=%d)\n",
-					 reg, sz);
-				return -EINVAL;
-			}
-			break;
-		default:
-			DRM_ERROR("Register %04x failed check as flag=%02x\n",
-				reg + i * 4, r300_reg_flags[(reg >> 2) + i]);
-			return -EINVAL;
-		}
-	}
-
-	BEGIN_RING(1 + sz);
-	OUT_RING(CP_PACKET0(reg, sz - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-/**
- * Emits a packet0 setting arbitrary registers.
- * Called by r300_do_cp_cmdbuf.
- *
- * Note that checks are performed on contents and addresses of the registers
- */
-static __inline__ int r300_emit_packet0(drm_radeon_private_t *dev_priv,
-					drm_radeon_kcmd_buffer_t *cmdbuf,
-					drm_r300_cmd_header_t header)
-{
-	int reg;
-	int sz;
-	RING_LOCALS;
-
-	sz = header.packet0.count;
-	reg = (header.packet0.reghi << 8) | header.packet0.reglo;
-
-	if (!sz)
-		return 0;
-
-	if (sz * 4 > drm_buffer_unprocessed(cmdbuf->buffer))
-		return -EINVAL;
-
-	if (reg + sz * 4 >= 0x10000) {
-		DRM_ERROR("No such registers in hardware reg=%04x sz=%d\n", reg,
-			  sz);
-		return -EINVAL;
-	}
-
-	if (r300_check_range(reg, sz)) {
-		/* go and check everything */
-		return r300_emit_carefully_checked_packet0(dev_priv, cmdbuf,
-							   header);
-	}
-	/* the rest of the data is safe to emit, whatever the values the user passed */
-
-	BEGIN_RING(1 + sz);
-	OUT_RING(CP_PACKET0(reg, sz - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-/**
- * Uploads user-supplied vertex program instructions or parameters onto
- * the graphics card.
- * Called by r300_do_cp_cmdbuf.
- */
-static __inline__ int r300_emit_vpu(drm_radeon_private_t *dev_priv,
-				    drm_radeon_kcmd_buffer_t *cmdbuf,
-				    drm_r300_cmd_header_t header)
-{
-	int sz;
-	int addr;
-	RING_LOCALS;
-
-	sz = header.vpu.count;
-	addr = (header.vpu.adrhi << 8) | header.vpu.adrlo;
-
-	if (!sz)
-		return 0;
-	if (sz * 16 > drm_buffer_unprocessed(cmdbuf->buffer))
-		return -EINVAL;
-
-	/* VAP is very sensitive so we purge cache before we program it
-	 * and we also flush its state before & after */
-	BEGIN_RING(6);
-	OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	OUT_RING(R300_RB3D_DC_FLUSH);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(RADEON_WAIT_3D_IDLECLEAN);
-	OUT_RING(CP_PACKET0(R300_VAP_PVS_STATE_FLUSH_REG, 0));
-	OUT_RING(0);
-	ADVANCE_RING();
-	/* set flush flag */
-	dev_priv->track_flush |= RADEON_FLUSH_EMITED;
-
-	BEGIN_RING(3 + sz * 4);
-	OUT_RING_REG(R300_VAP_PVS_UPLOAD_ADDRESS, addr);
-	OUT_RING(CP_PACKET0_TABLE(R300_VAP_PVS_UPLOAD_DATA, sz * 4 - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz * 4);
-	ADVANCE_RING();
-
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_VAP_PVS_STATE_FLUSH_REG, 0));
-	OUT_RING(0);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-/**
- * Emit a clear packet from userspace.
- * Called by r300_emit_packet3.
- */
-static __inline__ int r300_emit_clear(drm_radeon_private_t *dev_priv,
-				      drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	RING_LOCALS;
-
-	if (8 * 4 > drm_buffer_unprocessed(cmdbuf->buffer))
-		return -EINVAL;
-
-	BEGIN_RING(10);
-	OUT_RING(CP_PACKET3(R200_3D_DRAW_IMMD_2, 8));
-	OUT_RING(R300_PRIM_TYPE_POINT | R300_PRIM_WALK_RING |
-		 (1 << R300_PRIM_NUM_VERTICES_SHIFT));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, 8);
-	ADVANCE_RING();
-
-	BEGIN_RING(4);
-	OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	OUT_RING(R300_RB3D_DC_FLUSH);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(RADEON_WAIT_3D_IDLECLEAN);
-	ADVANCE_RING();
-	/* set flush flag */
-	dev_priv->track_flush |= RADEON_FLUSH_EMITED;
-
-	return 0;
-}
-
-static __inline__ int r300_emit_3d_load_vbpntr(drm_radeon_private_t *dev_priv,
-					       drm_radeon_kcmd_buffer_t *cmdbuf,
-					       u32 header)
-{
-	int count, i, k;
-#define MAX_ARRAY_PACKET  64
-	u32 *data;
-	u32 narrays;
-	RING_LOCALS;
-
-	count = (header & RADEON_CP_PACKET_COUNT_MASK) >> 16;
-
-	if ((count + 1) > MAX_ARRAY_PACKET) {
-		DRM_ERROR("Too large payload in 3D_LOAD_VBPNTR (count=%d)\n",
-			  count);
-		return -EINVAL;
-	}
-	/* carefully check packet contents */
-
-	/* We have already read the header so advance the buffer. */
-	drm_buffer_advance(cmdbuf->buffer, 4);
-
-	narrays = *(u32 *)drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-	k = 0;
-	i = 1;
-	while ((k < narrays) && (i < (count + 1))) {
-		i++;		/* skip attribute field */
-		data = drm_buffer_pointer_to_dword(cmdbuf->buffer, i);
-		if (!radeon_check_offset(dev_priv, *data)) {
-			DRM_ERROR
-			    ("Offset failed range check (k=%d i=%d) while processing 3D_LOAD_VBPNTR packet.\n",
-			     k, i);
-			return -EINVAL;
-		}
-		k++;
-		i++;
-		if (k == narrays)
-			break;
-		/* have one more to process, they come in pairs */
-		data = drm_buffer_pointer_to_dword(cmdbuf->buffer, i);
-		if (!radeon_check_offset(dev_priv, *data)) {
-			DRM_ERROR
-			    ("Offset failed range check (k=%d i=%d) while processing 3D_LOAD_VBPNTR packet.\n",
-			     k, i);
-			return -EINVAL;
-		}
-		k++;
-		i++;
-	}
-	/* do the counts match what we expect ? */
-	if ((k != narrays) || (i != (count + 1))) {
-		DRM_ERROR
-		    ("Malformed 3D_LOAD_VBPNTR packet (k=%d i=%d narrays=%d count+1=%d).\n",
-		     k, i, narrays, count + 1);
-		return -EINVAL;
-	}
-
-	/* all clear, output packet */
-
-	BEGIN_RING(count + 2);
-	OUT_RING(header);
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, count + 1);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static __inline__ int r300_emit_bitblt_multi(drm_radeon_private_t *dev_priv,
-					     drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	u32 *cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-	int count, ret;
-	RING_LOCALS;
-
-
-	count = (*cmd & RADEON_CP_PACKET_COUNT_MASK) >> 16;
-
-	if (*cmd & 0x8000) {
-		u32 offset;
-		u32 *cmd1 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-		if (*cmd1 & (RADEON_GMC_SRC_PITCH_OFFSET_CNTL
-			      | RADEON_GMC_DST_PITCH_OFFSET_CNTL)) {
-
-			u32 *cmd2 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 2);
-			offset = *cmd2 << 10;
-			ret = !radeon_check_offset(dev_priv, offset);
-			if (ret) {
-				DRM_ERROR("Invalid bitblt first offset is %08X\n", offset);
-				return -EINVAL;
-			}
-		}
-
-		if ((*cmd1 & RADEON_GMC_SRC_PITCH_OFFSET_CNTL) &&
-		    (*cmd1 & RADEON_GMC_DST_PITCH_OFFSET_CNTL)) {
-			u32 *cmd3 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 3);
-			offset = *cmd3 << 10;
-			ret = !radeon_check_offset(dev_priv, offset);
-			if (ret) {
-				DRM_ERROR("Invalid bitblt second offset is %08X\n", offset);
-				return -EINVAL;
-			}
-
-		}
-	}
-
-	BEGIN_RING(count+2);
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, count + 2);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static __inline__ int r300_emit_draw_indx_2(drm_radeon_private_t *dev_priv,
-					    drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	u32 *cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-	u32 *cmd1 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-	int count;
-	int expected_count;
-	RING_LOCALS;
-
-	count = (*cmd & RADEON_CP_PACKET_COUNT_MASK) >> 16;
-
-	expected_count = *cmd1 >> 16;
-	if (!(*cmd1 & R300_VAP_VF_CNTL__INDEX_SIZE_32bit))
-		expected_count = (expected_count+1)/2;
-
-	if (count && count != expected_count) {
-		DRM_ERROR("3D_DRAW_INDX_2: packet size %i, expected %i\n",
-			count, expected_count);
-		return -EINVAL;
-	}
-
-	BEGIN_RING(count+2);
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, count + 2);
-	ADVANCE_RING();
-
-	if (!count) {
-		drm_r300_cmd_header_t stack_header, *header;
-		u32 *cmd1, *cmd2, *cmd3;
-
-		if (drm_buffer_unprocessed(cmdbuf->buffer)
-				< 4*4 + sizeof(stack_header)) {
-			DRM_ERROR("3D_DRAW_INDX_2: expect subsequent INDX_BUFFER, but stream is too short.\n");
-			return -EINVAL;
-		}
-
-		header = drm_buffer_read_object(cmdbuf->buffer,
-				sizeof(stack_header), &stack_header);
-
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-		cmd1 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-		cmd2 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 2);
-		cmd3 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 3);
-
-		if (header->header.cmd_type != R300_CMD_PACKET3 ||
-		    header->packet3.packet != R300_CMD_PACKET3_RAW ||
-		    *cmd != CP_PACKET3(RADEON_CP_INDX_BUFFER, 2)) {
-			DRM_ERROR("3D_DRAW_INDX_2: expect subsequent INDX_BUFFER.\n");
-			return -EINVAL;
-		}
-
-		if ((*cmd1 & 0x8000ffff) != 0x80000810) {
-			DRM_ERROR("Invalid indx_buffer reg address %08X\n",
-					*cmd1);
-			return -EINVAL;
-		}
-		if (!radeon_check_offset(dev_priv, *cmd2)) {
-			DRM_ERROR("Invalid indx_buffer offset is %08X\n",
-					*cmd2);
-			return -EINVAL;
-		}
-		if (*cmd3 != expected_count) {
-			DRM_ERROR("INDX_BUFFER: buffer size %i, expected %i\n",
-				*cmd3, expected_count);
-			return -EINVAL;
-		}
-
-		BEGIN_RING(4);
-		OUT_RING_DRM_BUFFER(cmdbuf->buffer, 4);
-		ADVANCE_RING();
-	}
-
-	return 0;
-}
-
-static __inline__ int r300_emit_raw_packet3(drm_radeon_private_t *dev_priv,
-					    drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	u32 *header;
-	int count;
-	RING_LOCALS;
-
-	if (4 > drm_buffer_unprocessed(cmdbuf->buffer))
-		return -EINVAL;
-
-	/* Fixme !! This simply emits a packet without much checking.
-	   We need to be smarter. */
-
-	/* obtain first word - actual packet3 header */
-	header = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-
-	/* Is it packet 3 ? */
-	if ((*header >> 30) != 0x3) {
-		DRM_ERROR("Not a packet3 header (0x%08x)\n", *header);
-		return -EINVAL;
-	}
-
-	count = (*header >> 16) & 0x3fff;
-
-	/* Check again now that we know how much data to expect */
-	if ((count + 2) * 4 > drm_buffer_unprocessed(cmdbuf->buffer)) {
-		DRM_ERROR
-		    ("Expected packet3 of length %d but have only %d bytes left\n",
-		     (count + 2) * 4, drm_buffer_unprocessed(cmdbuf->buffer));
-		return -EINVAL;
-	}
-
-	/* Is it a packet type we know about ? */
-	switch (*header & 0xff00) {
-	case RADEON_3D_LOAD_VBPNTR:	/* load vertex array pointers */
-		return r300_emit_3d_load_vbpntr(dev_priv, cmdbuf, *header);
-
-	case RADEON_CNTL_BITBLT_MULTI:
-		return r300_emit_bitblt_multi(dev_priv, cmdbuf);
-
-	case RADEON_CP_INDX_BUFFER:
-		DRM_ERROR("packet3 INDX_BUFFER without preceding 3D_DRAW_INDX_2 is illegal.\n");
-		return -EINVAL;
-	case RADEON_CP_3D_DRAW_IMMD_2:
-		/* triggers drawing using in-packet vertex data */
-	case RADEON_CP_3D_DRAW_VBUF_2:
-		/* triggers drawing of vertex buffers setup elsewhere */
-		dev_priv->track_flush &= ~(RADEON_FLUSH_EMITED |
-					   RADEON_PURGE_EMITED);
-		break;
-	case RADEON_CP_3D_DRAW_INDX_2:
-		/* triggers drawing using indices to vertex buffer */
-		/* whenever we send vertex we clear flush & purge */
-		dev_priv->track_flush &= ~(RADEON_FLUSH_EMITED |
-					   RADEON_PURGE_EMITED);
-		return r300_emit_draw_indx_2(dev_priv, cmdbuf);
-	case RADEON_WAIT_FOR_IDLE:
-	case RADEON_CP_NOP:
-		/* these packets are safe */
-		break;
-	default:
-		DRM_ERROR("Unknown packet3 header (0x%08x)\n", *header);
-		return -EINVAL;
-	}
-
-	BEGIN_RING(count + 2);
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, count + 2);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-/**
- * Emit a rendering packet3 from userspace.
- * Called by r300_do_cp_cmdbuf.
- */
-static __inline__ int r300_emit_packet3(drm_radeon_private_t *dev_priv,
-					drm_radeon_kcmd_buffer_t *cmdbuf,
-					drm_r300_cmd_header_t header)
-{
-	int n;
-	int ret;
-	int orig_iter = cmdbuf->buffer->iterator;
-
-	/* This is a do-while-loop so that we run the interior at least once,
-	 * even if cmdbuf->nbox is 0. Compare r300_emit_cliprects for rationale.
-	 */
-	n = 0;
-	do {
-		if (cmdbuf->nbox > R300_SIMULTANEOUS_CLIPRECTS) {
-			ret = r300_emit_cliprects(dev_priv, cmdbuf, n);
-			if (ret)
-				return ret;
-
-			cmdbuf->buffer->iterator = orig_iter;
-		}
-
-		switch (header.packet3.packet) {
-		case R300_CMD_PACKET3_CLEAR:
-			DRM_DEBUG("R300_CMD_PACKET3_CLEAR\n");
-			ret = r300_emit_clear(dev_priv, cmdbuf);
-			if (ret) {
-				DRM_ERROR("r300_emit_clear failed\n");
-				return ret;
-			}
-			break;
-
-		case R300_CMD_PACKET3_RAW:
-			DRM_DEBUG("R300_CMD_PACKET3_RAW\n");
-			ret = r300_emit_raw_packet3(dev_priv, cmdbuf);
-			if (ret) {
-				DRM_ERROR("r300_emit_raw_packet3 failed\n");
-				return ret;
-			}
-			break;
-
-		default:
-			DRM_ERROR("bad packet3 type %i at byte %d\n",
-				  header.packet3.packet,
-				  cmdbuf->buffer->iterator - (int)sizeof(header));
-			return -EINVAL;
-		}
-
-		n += R300_SIMULTANEOUS_CLIPRECTS;
-	} while (n < cmdbuf->nbox);
-
-	return 0;
-}
-
-/* Some of the R300 chips seem to be extremely touchy about the two registers
- * that are configured in r300_pacify.
- * Among the worst offenders seems to be the R300 ND (0x4E44): When userspace
- * sends a command buffer that contains only state setting commands and a
- * vertex program/parameter upload sequence, this will eventually lead to a
- * lockup, unless the sequence is bracketed by calls to r300_pacify.
- * So we should take great care to *always* call r300_pacify before
- * *anything* 3D related, and again afterwards. This is what the
- * call bracket in r300_do_cp_cmdbuf is for.
- */
-
-/**
- * Emit the sequence to pacify R300.
- */
-static void r300_pacify(drm_radeon_private_t *dev_priv)
-{
-	uint32_t cache_z, cache_3d, cache_2d;
-	RING_LOCALS;
-
-	cache_z = R300_ZC_FLUSH;
-	cache_2d = R300_RB2D_DC_FLUSH;
-	cache_3d = R300_RB3D_DC_FLUSH;
-	if (!(dev_priv->track_flush & RADEON_PURGE_EMITED)) {
-		/* we can purge, primitive where draw since last purge */
-		cache_z |= R300_ZC_FREE;
-		cache_2d |= R300_RB2D_DC_FREE;
-		cache_3d |= R300_RB3D_DC_FREE;
-	}
-
-	/* flush & purge zbuffer */
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_ZB_ZCACHE_CTLSTAT, 0));
-	OUT_RING(cache_z);
-	ADVANCE_RING();
-	/* flush & purge 3d */
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-	OUT_RING(cache_3d);
-	ADVANCE_RING();
-	/* flush & purge texture */
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_TX_INVALTAGS, 0));
-	OUT_RING(0);
-	ADVANCE_RING();
-	/* FIXME: is this one really needed ? */
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(R300_RB3D_AARESOLVE_CTL, 0));
-	OUT_RING(0);
-	ADVANCE_RING();
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(RADEON_WAIT_3D_IDLECLEAN);
-	ADVANCE_RING();
-	/* flush & purge 2d through E2 as RB2D will trigger lockup */
-	BEGIN_RING(4);
-	OUT_RING(CP_PACKET0(R300_DSTCACHE_CTLSTAT, 0));
-	OUT_RING(cache_2d);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(RADEON_WAIT_2D_IDLECLEAN |
-		 RADEON_WAIT_HOST_IDLECLEAN);
-	ADVANCE_RING();
-	/* set flush & purge flags */
-	dev_priv->track_flush |= RADEON_FLUSH_EMITED | RADEON_PURGE_EMITED;
-}
-
-/**
- * Called by r300_do_cp_cmdbuf to update the internal buffer age and state.
- * The actual age emit is done by r300_do_cp_cmdbuf, which is why you must
- * be careful about how this function is called.
- */
-static void r300_discard_buffer(struct drm_device *dev, struct drm_master *master, struct drm_buf *buf)
-{
-	drm_radeon_buf_priv_t *buf_priv = buf->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-
-	buf_priv->age = ++master_priv->sarea_priv->last_dispatch;
-	buf->pending = 1;
-	buf->used = 0;
-}
-
-static void r300_cmd_wait(drm_radeon_private_t * dev_priv,
-			  drm_r300_cmd_header_t header)
-{
-	u32 wait_until;
-	RING_LOCALS;
-
-	if (!header.wait.flags)
-		return;
-
-	wait_until = 0;
-
-	switch(header.wait.flags) {
-	case R300_WAIT_2D:
-		wait_until = RADEON_WAIT_2D_IDLE;
-		break;
-	case R300_WAIT_3D:
-		wait_until = RADEON_WAIT_3D_IDLE;
-		break;
-	case R300_NEW_WAIT_2D_3D:
-		wait_until = RADEON_WAIT_2D_IDLE|RADEON_WAIT_3D_IDLE;
-		break;
-	case R300_NEW_WAIT_2D_2D_CLEAN:
-		wait_until = RADEON_WAIT_2D_IDLE|RADEON_WAIT_2D_IDLECLEAN;
-		break;
-	case R300_NEW_WAIT_3D_3D_CLEAN:
-		wait_until = RADEON_WAIT_3D_IDLE|RADEON_WAIT_3D_IDLECLEAN;
-		break;
-	case R300_NEW_WAIT_2D_2D_CLEAN_3D_3D_CLEAN:
-		wait_until = RADEON_WAIT_2D_IDLE|RADEON_WAIT_2D_IDLECLEAN;
-		wait_until |= RADEON_WAIT_3D_IDLE|RADEON_WAIT_3D_IDLECLEAN;
-		break;
-	default:
-		return;
-	}
-
-	BEGIN_RING(2);
-	OUT_RING(CP_PACKET0(RADEON_WAIT_UNTIL, 0));
-	OUT_RING(wait_until);
-	ADVANCE_RING();
-}
-
-static int r300_scratch(drm_radeon_private_t *dev_priv,
-			drm_radeon_kcmd_buffer_t *cmdbuf,
-			drm_r300_cmd_header_t header)
-{
-	u32 *ref_age_base;
-	u32 i, *buf_idx, h_pending;
-	u64 *ptr_addr;
-	u64 stack_ptr_addr;
-	RING_LOCALS;
-
-	if (drm_buffer_unprocessed(cmdbuf->buffer) <
-	    (sizeof(u64) + header.scratch.n_bufs * sizeof(*buf_idx))) {
-		return -EINVAL;
-	}
-
-	if (header.scratch.reg >= 5) {
-		return -EINVAL;
-	}
-
-	dev_priv->scratch_ages[header.scratch.reg]++;
-
-	ptr_addr = drm_buffer_read_object(cmdbuf->buffer,
-			sizeof(stack_ptr_addr), &stack_ptr_addr);
-	ref_age_base = (u32 *)(unsigned long)get_unaligned(ptr_addr);
-
-	for (i=0; i < header.scratch.n_bufs; i++) {
-		buf_idx = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-		*buf_idx *= 2; /* 8 bytes per buf */
-
-		if (copy_to_user(ref_age_base + *buf_idx,
-				&dev_priv->scratch_ages[header.scratch.reg],
-				sizeof(u32)))
-			return -EINVAL;
-
-		if (copy_from_user(&h_pending,
-				ref_age_base + *buf_idx + 1,
-				sizeof(u32)))
-			return -EINVAL;
-
-		if (h_pending == 0)
-			return -EINVAL;
-
-		h_pending--;
-
-		if (copy_to_user(ref_age_base + *buf_idx + 1,
-					&h_pending,
-					sizeof(u32)))
-			return -EINVAL;
-
-		drm_buffer_advance(cmdbuf->buffer, sizeof(*buf_idx));
-	}
-
-	BEGIN_RING(2);
-	OUT_RING( CP_PACKET0( RADEON_SCRATCH_REG0 + header.scratch.reg * 4, 0 ) );
-	OUT_RING( dev_priv->scratch_ages[header.scratch.reg] );
-	ADVANCE_RING();
-
-	return 0;
-}
-
-/**
- * Uploads user-supplied vertex program instructions or parameters onto
- * the graphics card.
- * Called by r300_do_cp_cmdbuf.
- */
-static inline int r300_emit_r500fp(drm_radeon_private_t *dev_priv,
-				       drm_radeon_kcmd_buffer_t *cmdbuf,
-				       drm_r300_cmd_header_t header)
-{
-	int sz;
-	int addr;
-	int type;
-	int isclamp;
-	int stride;
-	RING_LOCALS;
-
-	sz = header.r500fp.count;
-	/* address is 9 bits 0 - 8, bit 1 of flags is part of address */
-	addr = ((header.r500fp.adrhi_flags & 1) << 8) | header.r500fp.adrlo;
-
-	type = !!(header.r500fp.adrhi_flags & R500FP_CONSTANT_TYPE);
-	isclamp = !!(header.r500fp.adrhi_flags & R500FP_CONSTANT_CLAMP);
-
-	addr |= (type << 16);
-	addr |= (isclamp << 17);
-
-	stride = type ? 4 : 6;
-
-	DRM_DEBUG("r500fp %d %d type: %d\n", sz, addr, type);
-	if (!sz)
-		return 0;
-	if (sz * stride * 4 > drm_buffer_unprocessed(cmdbuf->buffer))
-		return -EINVAL;
-
-	BEGIN_RING(3 + sz * stride);
-	OUT_RING_REG(R500_GA_US_VECTOR_INDEX, addr);
-	OUT_RING(CP_PACKET0_TABLE(R500_GA_US_VECTOR_DATA, sz * stride - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz * stride);
-
-	ADVANCE_RING();
-
-	return 0;
-}
-
-
-/**
- * Parses and validates a user-supplied command buffer and emits appropriate
- * commands on the DMA ring buffer.
- * Called by the ioctl handler function radeon_cp_cmdbuf.
- */
-int r300_do_cp_cmdbuf(struct drm_device *dev,
-		      struct drm_file *file_priv,
-		      drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf = NULL;
-	int emit_dispatch_age = 0;
-	int ret = 0;
-
-	DRM_DEBUG("\n");
-
-	/* pacify */
-	r300_pacify(dev_priv);
-
-	if (cmdbuf->nbox <= R300_SIMULTANEOUS_CLIPRECTS) {
-		ret = r300_emit_cliprects(dev_priv, cmdbuf, 0);
-		if (ret)
-			goto cleanup;
-	}
-
-	while (drm_buffer_unprocessed(cmdbuf->buffer)
-			>= sizeof(drm_r300_cmd_header_t)) {
-		int idx;
-		drm_r300_cmd_header_t *header, stack_header;
-
-		header = drm_buffer_read_object(cmdbuf->buffer,
-				sizeof(stack_header), &stack_header);
-
-		switch (header->header.cmd_type) {
-		case R300_CMD_PACKET0:
-			DRM_DEBUG("R300_CMD_PACKET0\n");
-			ret = r300_emit_packet0(dev_priv, cmdbuf, *header);
-			if (ret) {
-				DRM_ERROR("r300_emit_packet0 failed\n");
-				goto cleanup;
-			}
-			break;
-
-		case R300_CMD_VPU:
-			DRM_DEBUG("R300_CMD_VPU\n");
-			ret = r300_emit_vpu(dev_priv, cmdbuf, *header);
-			if (ret) {
-				DRM_ERROR("r300_emit_vpu failed\n");
-				goto cleanup;
-			}
-			break;
-
-		case R300_CMD_PACKET3:
-			DRM_DEBUG("R300_CMD_PACKET3\n");
-			ret = r300_emit_packet3(dev_priv, cmdbuf, *header);
-			if (ret) {
-				DRM_ERROR("r300_emit_packet3 failed\n");
-				goto cleanup;
-			}
-			break;
-
-		case R300_CMD_END3D:
-			DRM_DEBUG("R300_CMD_END3D\n");
-			/* TODO:
-			   Ideally userspace driver should not need to issue this call,
-			   i.e. the drm driver should issue it automatically and prevent
-			   lockups.
-
-			   In practice, we do not understand why this call is needed and what
-			   it does (except for some vague guesses that it has to do with cache
-			   coherence) and so the user space driver does it.
-
-			   Once we are sure which uses prevent lockups the code could be moved
-			   into the kernel and the userspace driver will not
-			   need to use this command.
-
-			   Note that issuing this command does not hurt anything
-			   except, possibly, performance */
-			r300_pacify(dev_priv);
-			break;
-
-		case R300_CMD_CP_DELAY:
-			/* simple enough, we can do it here */
-			DRM_DEBUG("R300_CMD_CP_DELAY\n");
-			{
-				int i;
-				RING_LOCALS;
-
-				BEGIN_RING(header->delay.count);
-				for (i = 0; i < header->delay.count; i++)
-					OUT_RING(RADEON_CP_PACKET2);
-				ADVANCE_RING();
-			}
-			break;
-
-		case R300_CMD_DMA_DISCARD:
-			DRM_DEBUG("RADEON_CMD_DMA_DISCARD\n");
-			idx = header->dma.buf_idx;
-			if (idx < 0 || idx >= dma->buf_count) {
-				DRM_ERROR("buffer index %d (of %d max)\n",
-					  idx, dma->buf_count - 1);
-				ret = -EINVAL;
-				goto cleanup;
-			}
-
-			buf = dma->buflist[idx];
-			if (buf->file_priv != file_priv || buf->pending) {
-				DRM_ERROR("bad buffer %p %p %d\n",
-					  buf->file_priv, file_priv,
-					  buf->pending);
-				ret = -EINVAL;
-				goto cleanup;
-			}
-
-			emit_dispatch_age = 1;
-			r300_discard_buffer(dev, file_priv->master, buf);
-			break;
-
-		case R300_CMD_WAIT:
-			DRM_DEBUG("R300_CMD_WAIT\n");
-			r300_cmd_wait(dev_priv, *header);
-			break;
-
-		case R300_CMD_SCRATCH:
-			DRM_DEBUG("R300_CMD_SCRATCH\n");
-			ret = r300_scratch(dev_priv, cmdbuf, *header);
-			if (ret) {
-				DRM_ERROR("r300_scratch failed\n");
-				goto cleanup;
-			}
-			break;
-
-		case R300_CMD_R500FP:
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) < CHIP_RV515) {
-				DRM_ERROR("Calling r500 command on r300 card\n");
-				ret = -EINVAL;
-				goto cleanup;
-			}
-			DRM_DEBUG("R300_CMD_R500FP\n");
-			ret = r300_emit_r500fp(dev_priv, cmdbuf, *header);
-			if (ret) {
-				DRM_ERROR("r300_emit_r500fp failed\n");
-				goto cleanup;
-			}
-			break;
-		default:
-			DRM_ERROR("bad cmd_type %i at byte %d\n",
-				  header->header.cmd_type,
-				  cmdbuf->buffer->iterator - (int)sizeof(*header));
-			ret = -EINVAL;
-			goto cleanup;
-		}
-	}
-
-	DRM_DEBUG("END\n");
-
-      cleanup:
-	r300_pacify(dev_priv);
-
-	/* We emit the vertex buffer age here, outside the pacifier "brackets"
-	 * for two reasons:
-	 *  (1) This may coalesce multiple age emissions into a single one and
-	 *  (2) more importantly, some chips lock up hard when scratch registers
-	 *      are written inside the pacifier bracket.
-	 */
-	if (emit_dispatch_age) {
-		RING_LOCALS;
-
-		/* Emit the vertex buffer age */
-		BEGIN_RING(2);
-		RADEON_DISPATCH_AGE(master_priv->sarea_priv->last_dispatch);
-		ADVANCE_RING();
-	}
-
-	COMMIT_RING();
-
-	return ret;
-}
diff --git a/drivers/gpu/drm/radeon/r600_blit.c b/drivers/gpu/drm/radeon/r600_blit.c
deleted file mode 100644
index daf7572be976..000000000000
--- a/drivers/gpu/drm/radeon/r600_blit.c
+++ /dev/null
@@ -1,874 +0,0 @@
-/*
- * Copyright 2009 Advanced Micro Devices, Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice (including the next
- * paragraph) shall be included in all copies or substantial portions of the
- * Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *     Alex Deucher <alexander.deucher@amd.com>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-
-#include "r600_blit_shaders.h"
-
-/* 23 bits of float fractional data */
-#define I2F_FRAC_BITS  23
-#define I2F_MASK ((1 << I2F_FRAC_BITS) - 1)
-
-/*
- * Converts unsigned integer into 32-bit IEEE floating point representation.
- * Will be exact from 0 to 2^24.  Above that, we round towards zero
- * as the fractional bits will not fit in a float.  (It would be better to
- * round towards even as the fpu does, but that is slower.)
- */
-static __pure uint32_t int2float(uint32_t x)
-{
-	uint32_t msb, exponent, fraction;
-
-	/* Zero is special */
-	if (!x) return 0;
-
-	/* Get location of the most significant bit */
-	msb = __fls(x);
-
-	/*
-	 * Use a rotate instead of a shift because that works both leftwards
-	 * and rightwards due to the mod(32) behaviour.  This means we don't
-	 * need to check to see if we are above 2^24 or not.
-	 */
-	fraction = ror32(x, (msb - I2F_FRAC_BITS) & 0x1f) & I2F_MASK;
-	exponent = (127 + msb) << I2F_FRAC_BITS;
-
-	return fraction + exponent;
-}
-
-#define DI_PT_RECTLIST        0x11
-#define DI_INDEX_SIZE_16_BIT  0x0
-#define DI_SRC_SEL_AUTO_INDEX 0x2
-
-#define FMT_8                 0x1
-#define FMT_5_6_5             0x8
-#define FMT_8_8_8_8           0x1a
-#define COLOR_8               0x1
-#define COLOR_5_6_5           0x8
-#define COLOR_8_8_8_8         0x1a
-
-static void
-set_render_target(drm_radeon_private_t *dev_priv, int format, int w, int h, u64 gpu_addr)
-{
-	u32 cb_color_info;
-	int pitch, slice;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	h = ALIGN(h, 8);
-	if (h < 8)
-		h = 8;
-
-	cb_color_info = ((format << 2) | (1 << 27));
-	pitch = (w / 8) - 1;
-	slice = ((w * h) / 64) - 1;
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_R600) &&
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) < CHIP_RV770)) {
-		BEGIN_RING(21 + 2);
-		OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-		OUT_RING((R600_CB_COLOR0_BASE - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-		OUT_RING(gpu_addr >> 8);
-		OUT_RING(CP_PACKET3(R600_IT_SURFACE_BASE_UPDATE, 0));
-		OUT_RING(2 << 0);
-	} else {
-		BEGIN_RING(21);
-		OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-		OUT_RING((R600_CB_COLOR0_BASE - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-		OUT_RING(gpu_addr >> 8);
-	}
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_SIZE - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING((pitch << 0) | (slice << 10));
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_VIEW - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_INFO - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(cb_color_info);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_TILE - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_FRAG - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_CB_COLOR0_MASK - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-
-	ADVANCE_RING();
-}
-
-static void
-cp_set_surface_sync(drm_radeon_private_t *dev_priv,
-		    u32 sync_type, u32 size, u64 mc_addr)
-{
-	u32 cp_coher_size;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	if (size == 0xffffffff)
-		cp_coher_size = 0xffffffff;
-	else
-		cp_coher_size = ((size + 255) >> 8);
-
-	BEGIN_RING(5);
-	OUT_RING(CP_PACKET3(R600_IT_SURFACE_SYNC, 3));
-	OUT_RING(sync_type);
-	OUT_RING(cp_coher_size);
-	OUT_RING((mc_addr >> 8));
-	OUT_RING(10); /* poll interval */
-	ADVANCE_RING();
-}
-
-static void
-set_shaders(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	u64 gpu_addr;
-	int i;
-	u32 *vs, *ps;
-	uint32_t sq_pgm_resources;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	/* load shaders */
-	vs = (u32 *) ((char *)dev->agp_buffer_map->handle + dev_priv->blit_vb->offset);
-	ps = (u32 *) ((char *)dev->agp_buffer_map->handle + dev_priv->blit_vb->offset + 256);
-
-	for (i = 0; i < r6xx_vs_size; i++)
-		vs[i] = cpu_to_le32(r6xx_vs[i]);
-	for (i = 0; i < r6xx_ps_size; i++)
-		ps[i] = cpu_to_le32(r6xx_ps[i]);
-
-	dev_priv->blit_vb->used = 512;
-
-	gpu_addr = dev_priv->gart_buffers_offset + dev_priv->blit_vb->offset;
-
-	/* setup shader regs */
-	sq_pgm_resources = (1 << 0);
-
-	BEGIN_RING(9 + 12);
-	/* VS */
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_START_VS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(gpu_addr >> 8);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_RESOURCES_VS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(sq_pgm_resources);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_CF_OFFSET_VS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-
-	/* PS */
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_START_PS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING((gpu_addr + 256) >> 8);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_RESOURCES_PS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(sq_pgm_resources | (1 << 28));
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_EXPORTS_PS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(2);
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 1));
-	OUT_RING((R600_SQ_PGM_CF_OFFSET_PS - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING(0);
-	ADVANCE_RING();
-
-	cp_set_surface_sync(dev_priv,
-			    R600_SH_ACTION_ENA, 512, gpu_addr);
-}
-
-static void
-set_vtx_resource(drm_radeon_private_t *dev_priv, u64 gpu_addr)
-{
-	uint32_t sq_vtx_constant_word2;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	sq_vtx_constant_word2 = (((gpu_addr >> 32) & 0xff) | (16 << 8));
-#ifdef __BIG_ENDIAN
-	sq_vtx_constant_word2 |= (2 << 30);
-#endif
-
-	BEGIN_RING(9);
-	OUT_RING(CP_PACKET3(R600_IT_SET_RESOURCE, 7));
-	OUT_RING(0x460);
-	OUT_RING(gpu_addr & 0xffffffff);
-	OUT_RING(48 - 1);
-	OUT_RING(sq_vtx_constant_word2);
-	OUT_RING(1 << 0);
-	OUT_RING(0);
-	OUT_RING(0);
-	OUT_RING(R600_SQ_TEX_VTX_VALID_BUFFER << 30);
-	ADVANCE_RING();
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV710))
-		cp_set_surface_sync(dev_priv,
-				    R600_TC_ACTION_ENA, 48, gpu_addr);
-	else
-		cp_set_surface_sync(dev_priv,
-				    R600_VC_ACTION_ENA, 48, gpu_addr);
-}
-
-static void
-set_tex_resource(drm_radeon_private_t *dev_priv,
-		 int format, int w, int h, int pitch, u64 gpu_addr)
-{
-	uint32_t sq_tex_resource_word0, sq_tex_resource_word1, sq_tex_resource_word4;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	if (h < 1)
-		h = 1;
-
-	sq_tex_resource_word0 = (1 << 0);
-	sq_tex_resource_word0 |= ((((pitch >> 3) - 1) << 8) |
-				  ((w - 1) << 19));
-
-	sq_tex_resource_word1 = (format << 26);
-	sq_tex_resource_word1 |= ((h - 1) << 0);
-
-	sq_tex_resource_word4 = ((1 << 14) |
-				 (0 << 16) |
-				 (1 << 19) |
-				 (2 << 22) |
-				 (3 << 25));
-
-	BEGIN_RING(9);
-	OUT_RING(CP_PACKET3(R600_IT_SET_RESOURCE, 7));
-	OUT_RING(0);
-	OUT_RING(sq_tex_resource_word0);
-	OUT_RING(sq_tex_resource_word1);
-	OUT_RING(gpu_addr >> 8);
-	OUT_RING(gpu_addr >> 8);
-	OUT_RING(sq_tex_resource_word4);
-	OUT_RING(0);
-	OUT_RING(R600_SQ_TEX_VTX_VALID_TEXTURE << 30);
-	ADVANCE_RING();
-
-}
-
-static void
-set_scissors(drm_radeon_private_t *dev_priv, int x1, int y1, int x2, int y2)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(12);
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 2));
-	OUT_RING((R600_PA_SC_SCREEN_SCISSOR_TL - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING((x1 << 0) | (y1 << 16));
-	OUT_RING((x2 << 0) | (y2 << 16));
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 2));
-	OUT_RING((R600_PA_SC_GENERIC_SCISSOR_TL - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING((x1 << 0) | (y1 << 16) | (1 << 31));
-	OUT_RING((x2 << 0) | (y2 << 16));
-
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONTEXT_REG, 2));
-	OUT_RING((R600_PA_SC_WINDOW_SCISSOR_TL - R600_SET_CONTEXT_REG_OFFSET) >> 2);
-	OUT_RING((x1 << 0) | (y1 << 16) | (1 << 31));
-	OUT_RING((x2 << 0) | (y2 << 16));
-	ADVANCE_RING();
-}
-
-static void
-draw_auto(drm_radeon_private_t *dev_priv)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(10);
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));
-	OUT_RING((R600_VGT_PRIMITIVE_TYPE - R600_SET_CONFIG_REG_OFFSET) >> 2);
-	OUT_RING(DI_PT_RECTLIST);
-
-	OUT_RING(CP_PACKET3(R600_IT_INDEX_TYPE, 0));
-#ifdef __BIG_ENDIAN
-	OUT_RING((2 << 2) | DI_INDEX_SIZE_16_BIT);
-#else
-	OUT_RING(DI_INDEX_SIZE_16_BIT);
-#endif
-
-	OUT_RING(CP_PACKET3(R600_IT_NUM_INSTANCES, 0));
-	OUT_RING(1);
-
-	OUT_RING(CP_PACKET3(R600_IT_DRAW_INDEX_AUTO, 1));
-	OUT_RING(3);
-	OUT_RING(DI_SRC_SEL_AUTO_INDEX);
-
-	ADVANCE_RING();
-	COMMIT_RING();
-}
-
-static void
-set_default_state(drm_radeon_private_t *dev_priv)
-{
-	int i;
-	u32 sq_config, sq_gpr_resource_mgmt_1, sq_gpr_resource_mgmt_2;
-	u32 sq_thread_resource_mgmt, sq_stack_resource_mgmt_1, sq_stack_resource_mgmt_2;
-	int num_ps_gprs, num_vs_gprs, num_temp_gprs, num_gs_gprs, num_es_gprs;
-	int num_ps_threads, num_vs_threads, num_gs_threads, num_es_threads;
-	int num_ps_stack_entries, num_vs_stack_entries, num_gs_stack_entries, num_es_stack_entries;
-	RING_LOCALS;
-
-	switch ((dev_priv->flags & RADEON_FAMILY_MASK)) {
-	case CHIP_R600:
-		num_ps_gprs = 192;
-		num_vs_gprs = 56;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 136;
-		num_vs_threads = 48;
-		num_gs_threads = 4;
-		num_es_threads = 4;
-		num_ps_stack_entries = 128;
-		num_vs_stack_entries = 128;
-		num_gs_stack_entries = 0;
-		num_es_stack_entries = 0;
-		break;
-	case CHIP_RV630:
-	case CHIP_RV635:
-		num_ps_gprs = 84;
-		num_vs_gprs = 36;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 144;
-		num_vs_threads = 40;
-		num_gs_threads = 4;
-		num_es_threads = 4;
-		num_ps_stack_entries = 40;
-		num_vs_stack_entries = 40;
-		num_gs_stack_entries = 32;
-		num_es_stack_entries = 16;
-		break;
-	case CHIP_RV610:
-	case CHIP_RV620:
-	case CHIP_RS780:
-	case CHIP_RS880:
-	default:
-		num_ps_gprs = 84;
-		num_vs_gprs = 36;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 136;
-		num_vs_threads = 48;
-		num_gs_threads = 4;
-		num_es_threads = 4;
-		num_ps_stack_entries = 40;
-		num_vs_stack_entries = 40;
-		num_gs_stack_entries = 32;
-		num_es_stack_entries = 16;
-		break;
-	case CHIP_RV670:
-		num_ps_gprs = 144;
-		num_vs_gprs = 40;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 136;
-		num_vs_threads = 48;
-		num_gs_threads = 4;
-		num_es_threads = 4;
-		num_ps_stack_entries = 40;
-		num_vs_stack_entries = 40;
-		num_gs_stack_entries = 32;
-		num_es_stack_entries = 16;
-		break;
-	case CHIP_RV770:
-		num_ps_gprs = 192;
-		num_vs_gprs = 56;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 188;
-		num_vs_threads = 60;
-		num_gs_threads = 0;
-		num_es_threads = 0;
-		num_ps_stack_entries = 256;
-		num_vs_stack_entries = 256;
-		num_gs_stack_entries = 0;
-		num_es_stack_entries = 0;
-		break;
-	case CHIP_RV730:
-	case CHIP_RV740:
-		num_ps_gprs = 84;
-		num_vs_gprs = 36;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 188;
-		num_vs_threads = 60;
-		num_gs_threads = 0;
-		num_es_threads = 0;
-		num_ps_stack_entries = 128;
-		num_vs_stack_entries = 128;
-		num_gs_stack_entries = 0;
-		num_es_stack_entries = 0;
-		break;
-	case CHIP_RV710:
-		num_ps_gprs = 192;
-		num_vs_gprs = 56;
-		num_temp_gprs = 4;
-		num_gs_gprs = 0;
-		num_es_gprs = 0;
-		num_ps_threads = 144;
-		num_vs_threads = 48;
-		num_gs_threads = 0;
-		num_es_threads = 0;
-		num_ps_stack_entries = 128;
-		num_vs_stack_entries = 128;
-		num_gs_stack_entries = 0;
-		num_es_stack_entries = 0;
-		break;
-	}
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV710))
-		sq_config = 0;
-	else
-		sq_config = R600_VC_ENABLE;
-
-	sq_config |= (R600_DX9_CONSTS |
-		      R600_ALU_INST_PREFER_VECTOR |
-		      R600_PS_PRIO(0) |
-		      R600_VS_PRIO(1) |
-		      R600_GS_PRIO(2) |
-		      R600_ES_PRIO(3));
-
-	sq_gpr_resource_mgmt_1 = (R600_NUM_PS_GPRS(num_ps_gprs) |
-				  R600_NUM_VS_GPRS(num_vs_gprs) |
-				  R600_NUM_CLAUSE_TEMP_GPRS(num_temp_gprs));
-	sq_gpr_resource_mgmt_2 = (R600_NUM_GS_GPRS(num_gs_gprs) |
-				  R600_NUM_ES_GPRS(num_es_gprs));
-	sq_thread_resource_mgmt = (R600_NUM_PS_THREADS(num_ps_threads) |
-				   R600_NUM_VS_THREADS(num_vs_threads) |
-				   R600_NUM_GS_THREADS(num_gs_threads) |
-				   R600_NUM_ES_THREADS(num_es_threads));
-	sq_stack_resource_mgmt_1 = (R600_NUM_PS_STACK_ENTRIES(num_ps_stack_entries) |
-				    R600_NUM_VS_STACK_ENTRIES(num_vs_stack_entries));
-	sq_stack_resource_mgmt_2 = (R600_NUM_GS_STACK_ENTRIES(num_gs_stack_entries) |
-				    R600_NUM_ES_STACK_ENTRIES(num_es_stack_entries));
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770) {
-		BEGIN_RING(r7xx_default_size + 10);
-		for (i = 0; i < r7xx_default_size; i++)
-			OUT_RING(r7xx_default_state[i]);
-	} else {
-		BEGIN_RING(r6xx_default_size + 10);
-		for (i = 0; i < r6xx_default_size; i++)
-			OUT_RING(r6xx_default_state[i]);
-	}
-	OUT_RING(CP_PACKET3(R600_IT_EVENT_WRITE, 0));
-	OUT_RING(R600_CACHE_FLUSH_AND_INV_EVENT);
-	/* SQ config */
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 6));
-	OUT_RING((R600_SQ_CONFIG - R600_SET_CONFIG_REG_OFFSET) >> 2);
-	OUT_RING(sq_config);
-	OUT_RING(sq_gpr_resource_mgmt_1);
-	OUT_RING(sq_gpr_resource_mgmt_2);
-	OUT_RING(sq_thread_resource_mgmt);
-	OUT_RING(sq_stack_resource_mgmt_1);
-	OUT_RING(sq_stack_resource_mgmt_2);
-	ADVANCE_RING();
-}
-
-static int r600_nomm_get_vb(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	dev_priv->blit_vb = radeon_freelist_get(dev);
-	if (!dev_priv->blit_vb) {
-		DRM_ERROR("Unable to allocate vertex buffer for blit\n");
-		return -EAGAIN;
-	}
-	return 0;
-}
-
-static void r600_nomm_put_vb(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	dev_priv->blit_vb->used = 0;
-	radeon_cp_discard_buffer(dev, dev_priv->blit_vb->file_priv->master, dev_priv->blit_vb);
-}
-
-static void *r600_nomm_get_vb_ptr(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	return (((char *)dev->agp_buffer_map->handle +
-		 dev_priv->blit_vb->offset + dev_priv->blit_vb->used));
-}
-
-int
-r600_prepare_blit_copy(struct drm_device *dev, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int ret;
-	DRM_DEBUG("\n");
-
-	ret = r600_nomm_get_vb(dev);
-	if (ret)
-		return ret;
-
-	dev_priv->blit_vb->file_priv = file_priv;
-
-	set_default_state(dev_priv);
-	set_shaders(dev);
-
-	return 0;
-}
-
-
-void
-r600_done_blit_copy(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(5);
-	OUT_RING(CP_PACKET3(R600_IT_EVENT_WRITE, 0));
-	OUT_RING(R600_CACHE_FLUSH_AND_INV_EVENT);
-	/* wait for 3D idle clean */
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));
-	OUT_RING((R600_WAIT_UNTIL - R600_SET_CONFIG_REG_OFFSET) >> 2);
-	OUT_RING(RADEON_WAIT_3D_IDLE | RADEON_WAIT_3D_IDLECLEAN);
-
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	r600_nomm_put_vb(dev);
-}
-
-void
-r600_blit_copy(struct drm_device *dev,
-	       uint64_t src_gpu_addr, uint64_t dst_gpu_addr,
-	       int size_bytes)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int max_bytes;
-	u64 vb_addr;
-	u32 *vb;
-
-	vb = r600_nomm_get_vb_ptr(dev);
-
-	if ((size_bytes & 3) || (src_gpu_addr & 3) || (dst_gpu_addr & 3)) {
-		max_bytes = 8192;
-
-		while (size_bytes) {
-			int cur_size = size_bytes;
-			int src_x = src_gpu_addr & 255;
-			int dst_x = dst_gpu_addr & 255;
-			int h = 1;
-			src_gpu_addr = src_gpu_addr & ~255;
-			dst_gpu_addr = dst_gpu_addr & ~255;
-
-			if (!src_x && !dst_x) {
-				h = (cur_size / max_bytes);
-				if (h > 8192)
-					h = 8192;
-				if (h == 0)
-					h = 1;
-				else
-					cur_size = max_bytes;
-			} else {
-				if (cur_size > max_bytes)
-					cur_size = max_bytes;
-				if (cur_size > (max_bytes - dst_x))
-					cur_size = (max_bytes - dst_x);
-				if (cur_size > (max_bytes - src_x))
-					cur_size = (max_bytes - src_x);
-			}
-
-			if ((dev_priv->blit_vb->used + 48) > dev_priv->blit_vb->total) {
-
-				r600_nomm_put_vb(dev);
-				r600_nomm_get_vb(dev);
-				if (!dev_priv->blit_vb)
-					return;
-				set_shaders(dev);
-				vb = r600_nomm_get_vb_ptr(dev);
-			}
-
-			vb[0] = int2float(dst_x);
-			vb[1] = 0;
-			vb[2] = int2float(src_x);
-			vb[3] = 0;
-
-			vb[4] = int2float(dst_x);
-			vb[5] = int2float(h);
-			vb[6] = int2float(src_x);
-			vb[7] = int2float(h);
-
-			vb[8] = int2float(dst_x + cur_size);
-			vb[9] = int2float(h);
-			vb[10] = int2float(src_x + cur_size);
-			vb[11] = int2float(h);
-
-			/* src */
-			set_tex_resource(dev_priv, FMT_8,
-					 src_x + cur_size, h, src_x + cur_size,
-					 src_gpu_addr);
-
-			cp_set_surface_sync(dev_priv,
-					    R600_TC_ACTION_ENA, (src_x + cur_size * h), src_gpu_addr);
-
-			/* dst */
-			set_render_target(dev_priv, COLOR_8,
-					  dst_x + cur_size, h,
-					  dst_gpu_addr);
-
-			/* scissors */
-			set_scissors(dev_priv, dst_x, 0, dst_x + cur_size, h);
-
-			/* Vertex buffer setup */
-			vb_addr = dev_priv->gart_buffers_offset +
-				dev_priv->blit_vb->offset +
-				dev_priv->blit_vb->used;
-			set_vtx_resource(dev_priv, vb_addr);
-
-			/* draw */
-			draw_auto(dev_priv);
-
-			cp_set_surface_sync(dev_priv,
-					    R600_CB_ACTION_ENA | R600_CB0_DEST_BASE_ENA,
-					    cur_size * h, dst_gpu_addr);
-
-			vb += 12;
-			dev_priv->blit_vb->used += 12 * 4;
-
-			src_gpu_addr += cur_size * h;
-			dst_gpu_addr += cur_size * h;
-			size_bytes -= cur_size * h;
-		}
-	} else {
-		max_bytes = 8192 * 4;
-
-		while (size_bytes) {
-			int cur_size = size_bytes;
-			int src_x = (src_gpu_addr & 255);
-			int dst_x = (dst_gpu_addr & 255);
-			int h = 1;
-			src_gpu_addr = src_gpu_addr & ~255;
-			dst_gpu_addr = dst_gpu_addr & ~255;
-
-			if (!src_x && !dst_x) {
-				h = (cur_size / max_bytes);
-				if (h > 8192)
-					h = 8192;
-				if (h == 0)
-					h = 1;
-				else
-					cur_size = max_bytes;
-			} else {
-				if (cur_size > max_bytes)
-					cur_size = max_bytes;
-				if (cur_size > (max_bytes - dst_x))
-					cur_size = (max_bytes - dst_x);
-				if (cur_size > (max_bytes - src_x))
-					cur_size = (max_bytes - src_x);
-			}
-
-			if ((dev_priv->blit_vb->used + 48) > dev_priv->blit_vb->total) {
-				r600_nomm_put_vb(dev);
-				r600_nomm_get_vb(dev);
-				if (!dev_priv->blit_vb)
-					return;
-
-				set_shaders(dev);
-				vb = r600_nomm_get_vb_ptr(dev);
-			}
-
-			vb[0] = int2float(dst_x / 4);
-			vb[1] = 0;
-			vb[2] = int2float(src_x / 4);
-			vb[3] = 0;
-
-			vb[4] = int2float(dst_x / 4);
-			vb[5] = int2float(h);
-			vb[6] = int2float(src_x / 4);
-			vb[7] = int2float(h);
-
-			vb[8] = int2float((dst_x + cur_size) / 4);
-			vb[9] = int2float(h);
-			vb[10] = int2float((src_x + cur_size) / 4);
-			vb[11] = int2float(h);
-
-			/* src */
-			set_tex_resource(dev_priv, FMT_8_8_8_8,
-					 (src_x + cur_size) / 4,
-					 h, (src_x + cur_size) / 4,
-					 src_gpu_addr);
-
-			cp_set_surface_sync(dev_priv,
-					    R600_TC_ACTION_ENA, (src_x + cur_size * h), src_gpu_addr);
-
-			/* dst */
-			set_render_target(dev_priv, COLOR_8_8_8_8,
-					  (dst_x + cur_size) / 4, h,
-					  dst_gpu_addr);
-
-			/* scissors */
-			set_scissors(dev_priv, (dst_x / 4), 0, (dst_x + cur_size / 4), h);
-
-			/* Vertex buffer setup */
-			vb_addr = dev_priv->gart_buffers_offset +
-				dev_priv->blit_vb->offset +
-				dev_priv->blit_vb->used;
-			set_vtx_resource(dev_priv, vb_addr);
-
-			/* draw */
-			draw_auto(dev_priv);
-
-			cp_set_surface_sync(dev_priv,
-					    R600_CB_ACTION_ENA | R600_CB0_DEST_BASE_ENA,
-					    cur_size * h, dst_gpu_addr);
-
-			vb += 12;
-			dev_priv->blit_vb->used += 12 * 4;
-
-			src_gpu_addr += cur_size * h;
-			dst_gpu_addr += cur_size * h;
-			size_bytes -= cur_size * h;
-		}
-	}
-}
-
-void
-r600_blit_swap(struct drm_device *dev,
-	       uint64_t src_gpu_addr, uint64_t dst_gpu_addr,
-	       int sx, int sy, int dx, int dy,
-	       int w, int h, int src_pitch, int dst_pitch, int cpp)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int cb_format, tex_format;
-	int sx2, sy2, dx2, dy2;
-	u64 vb_addr;
-	u32 *vb;
-
-	if ((dev_priv->blit_vb->used + 48) > dev_priv->blit_vb->total) {
-
-		r600_nomm_put_vb(dev);
-		r600_nomm_get_vb(dev);
-		if (!dev_priv->blit_vb)
-			return;
-
-		set_shaders(dev);
-	}
-	vb = r600_nomm_get_vb_ptr(dev);
-
-	sx2 = sx + w;
-	sy2 = sy + h;
-	dx2 = dx + w;
-	dy2 = dy + h;
-
-	vb[0] = int2float(dx);
-	vb[1] = int2float(dy);
-	vb[2] = int2float(sx);
-	vb[3] = int2float(sy);
-
-	vb[4] = int2float(dx);
-	vb[5] = int2float(dy2);
-	vb[6] = int2float(sx);
-	vb[7] = int2float(sy2);
-
-	vb[8] = int2float(dx2);
-	vb[9] = int2float(dy2);
-	vb[10] = int2float(sx2);
-	vb[11] = int2float(sy2);
-
-	switch(cpp) {
-	case 4:
-		cb_format = COLOR_8_8_8_8;
-		tex_format = FMT_8_8_8_8;
-		break;
-	case 2:
-		cb_format = COLOR_5_6_5;
-		tex_format = FMT_5_6_5;
-		break;
-	default:
-		cb_format = COLOR_8;
-		tex_format = FMT_8;
-		break;
-	}
-
-	/* src */
-	set_tex_resource(dev_priv, tex_format,
-			 src_pitch / cpp,
-			 sy2, src_pitch / cpp,
-			 src_gpu_addr);
-
-	cp_set_surface_sync(dev_priv,
-			    R600_TC_ACTION_ENA, src_pitch * sy2, src_gpu_addr);
-
-	/* dst */
-	set_render_target(dev_priv, cb_format,
-			  dst_pitch / cpp, dy2,
-			  dst_gpu_addr);
-
-	/* scissors */
-	set_scissors(dev_priv, dx, dy, dx2, dy2);
-
-	/* Vertex buffer setup */
-	vb_addr = dev_priv->gart_buffers_offset +
-		dev_priv->blit_vb->offset +
-		dev_priv->blit_vb->used;
-	set_vtx_resource(dev_priv, vb_addr);
-
-	/* draw */
-	draw_auto(dev_priv);
-
-	cp_set_surface_sync(dev_priv,
-			    R600_CB_ACTION_ENA | R600_CB0_DEST_BASE_ENA,
-			    dst_pitch * dy2, dst_gpu_addr);
-
-	dev_priv->blit_vb->used += 12 * 4;
-}
diff --git a/drivers/gpu/drm/radeon/r600_cp.c b/drivers/gpu/drm/radeon/r600_cp.c
deleted file mode 100644
index e231eeafef23..000000000000
--- a/drivers/gpu/drm/radeon/r600_cp.c
+++ /dev/null
@@ -1,2660 +0,0 @@
-/*
- * Copyright 2008-2009 Advanced Micro Devices, Inc.
- * Copyright 2008 Red Hat Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice (including the next
- * paragraph) shall be included in all copies or substantial portions of the
- * Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *     Dave Airlie <airlied@redhat.com>
- *     Alex Deucher <alexander.deucher@amd.com>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <linux/module.h>
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-
-#define PFP_UCODE_SIZE 576
-#define PM4_UCODE_SIZE 1792
-#define R700_PFP_UCODE_SIZE 848
-#define R700_PM4_UCODE_SIZE 1360
-
-/* Firmware Names */
-MODULE_FIRMWARE("radeon/R600_pfp.bin");
-MODULE_FIRMWARE("radeon/R600_me.bin");
-MODULE_FIRMWARE("radeon/RV610_pfp.bin");
-MODULE_FIRMWARE("radeon/RV610_me.bin");
-MODULE_FIRMWARE("radeon/RV630_pfp.bin");
-MODULE_FIRMWARE("radeon/RV630_me.bin");
-MODULE_FIRMWARE("radeon/RV620_pfp.bin");
-MODULE_FIRMWARE("radeon/RV620_me.bin");
-MODULE_FIRMWARE("radeon/RV635_pfp.bin");
-MODULE_FIRMWARE("radeon/RV635_me.bin");
-MODULE_FIRMWARE("radeon/RV670_pfp.bin");
-MODULE_FIRMWARE("radeon/RV670_me.bin");
-MODULE_FIRMWARE("radeon/RS780_pfp.bin");
-MODULE_FIRMWARE("radeon/RS780_me.bin");
-MODULE_FIRMWARE("radeon/RV770_pfp.bin");
-MODULE_FIRMWARE("radeon/RV770_me.bin");
-MODULE_FIRMWARE("radeon/RV730_pfp.bin");
-MODULE_FIRMWARE("radeon/RV730_me.bin");
-MODULE_FIRMWARE("radeon/RV710_pfp.bin");
-MODULE_FIRMWARE("radeon/RV710_me.bin");
-
-
-int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
-			unsigned family, u32 *ib, int *l);
-void r600_cs_legacy_init(void);
-
-
-# define ATI_PCIGART_PAGE_SIZE		4096	/**< PCI GART page size */
-# define ATI_PCIGART_PAGE_MASK		(~(ATI_PCIGART_PAGE_SIZE-1))
-
-#define R600_PTE_VALID     (1 << 0)
-#define R600_PTE_SYSTEM    (1 << 1)
-#define R600_PTE_SNOOPED   (1 << 2)
-#define R600_PTE_READABLE  (1 << 5)
-#define R600_PTE_WRITEABLE (1 << 6)
-
-/* MAX values used for gfx init */
-#define R6XX_MAX_SH_GPRS           256
-#define R6XX_MAX_TEMP_GPRS         16
-#define R6XX_MAX_SH_THREADS        256
-#define R6XX_MAX_SH_STACK_ENTRIES  4096
-#define R6XX_MAX_BACKENDS          8
-#define R6XX_MAX_BACKENDS_MASK     0xff
-#define R6XX_MAX_SIMDS             8
-#define R6XX_MAX_SIMDS_MASK        0xff
-#define R6XX_MAX_PIPES             8
-#define R6XX_MAX_PIPES_MASK        0xff
-
-#define R7XX_MAX_SH_GPRS           256
-#define R7XX_MAX_TEMP_GPRS         16
-#define R7XX_MAX_SH_THREADS        256
-#define R7XX_MAX_SH_STACK_ENTRIES  4096
-#define R7XX_MAX_BACKENDS          8
-#define R7XX_MAX_BACKENDS_MASK     0xff
-#define R7XX_MAX_SIMDS             16
-#define R7XX_MAX_SIMDS_MASK        0xffff
-#define R7XX_MAX_PIPES             8
-#define R7XX_MAX_PIPES_MASK        0xff
-
-static int r600_do_wait_for_fifo(drm_radeon_private_t *dev_priv, int entries)
-{
-	int i;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	for (i = 0; i < dev_priv->usec_timeout; i++) {
-		int slots;
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)
-			slots = (RADEON_READ(R600_GRBM_STATUS)
-				 & R700_CMDFIFO_AVAIL_MASK);
-		else
-			slots = (RADEON_READ(R600_GRBM_STATUS)
-				 & R600_CMDFIFO_AVAIL_MASK);
-		if (slots >= entries)
-			return 0;
-		DRM_UDELAY(1);
-	}
-	DRM_INFO("wait for fifo failed status : 0x%08X 0x%08X\n",
-		 RADEON_READ(R600_GRBM_STATUS),
-		 RADEON_READ(R600_GRBM_STATUS2));
-
-	return -EBUSY;
-}
-
-static int r600_do_wait_for_idle(drm_radeon_private_t *dev_priv)
-{
-	int i, ret;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)
-		ret = r600_do_wait_for_fifo(dev_priv, 8);
-	else
-		ret = r600_do_wait_for_fifo(dev_priv, 16);
-	if (ret)
-		return ret;
-	for (i = 0; i < dev_priv->usec_timeout; i++) {
-		if (!(RADEON_READ(R600_GRBM_STATUS) & R600_GUI_ACTIVE))
-			return 0;
-		DRM_UDELAY(1);
-	}
-	DRM_INFO("wait idle failed status : 0x%08X 0x%08X\n",
-		 RADEON_READ(R600_GRBM_STATUS),
-		 RADEON_READ(R600_GRBM_STATUS2));
-
-	return -EBUSY;
-}
-
-void r600_page_table_cleanup(struct drm_device *dev, struct drm_ati_pcigart_info *gart_info)
-{
-	struct drm_sg_mem *entry = dev->sg;
-	int max_pages;
-	int pages;
-	int i;
-
-	if (!entry)
-		return;
-
-	if (gart_info->bus_addr) {
-		max_pages = (gart_info->table_size / sizeof(u64));
-		pages = (entry->pages <= max_pages)
-		  ? entry->pages : max_pages;
-
-		for (i = 0; i < pages; i++) {
-			if (!entry->busaddr[i])
-				break;
-			pci_unmap_page(dev->pdev, entry->busaddr[i],
-				       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
-		}
-		if (gart_info->gart_table_location == DRM_ATI_GART_MAIN)
-			gart_info->bus_addr = 0;
-	}
-}
-
-/* R600 has page table setup */
-int r600_page_table_init(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_ati_pcigart_info *gart_info = &dev_priv->gart_info;
-	struct drm_local_map *map = &gart_info->mapping;
-	struct drm_sg_mem *entry = dev->sg;
-	int ret = 0;
-	int i, j;
-	int pages;
-	u64 page_base;
-	dma_addr_t entry_addr;
-	int max_ati_pages, max_real_pages, gart_idx;
-
-	/* okay page table is available - lets rock */
-	max_ati_pages = (gart_info->table_size / sizeof(u64));
-	max_real_pages = max_ati_pages / (PAGE_SIZE / ATI_PCIGART_PAGE_SIZE);
-
-	pages = (entry->pages <= max_real_pages) ?
-		entry->pages : max_real_pages;
-
-	memset_io((void __iomem *)map->handle, 0, max_ati_pages * sizeof(u64));
-
-	gart_idx = 0;
-	for (i = 0; i < pages; i++) {
-		entry->busaddr[i] = pci_map_page(dev->pdev,
-						 entry->pagelist[i], 0,
-						 PAGE_SIZE,
-						 PCI_DMA_BIDIRECTIONAL);
-		if (pci_dma_mapping_error(dev->pdev, entry->busaddr[i])) {
-			DRM_ERROR("unable to map PCIGART pages!\n");
-			r600_page_table_cleanup(dev, gart_info);
-			goto done;
-		}
-		entry_addr = entry->busaddr[i];
-		for (j = 0; j < (PAGE_SIZE / ATI_PCIGART_PAGE_SIZE); j++) {
-			page_base = (u64) entry_addr & ATI_PCIGART_PAGE_MASK;
-			page_base |= R600_PTE_VALID | R600_PTE_SYSTEM | R600_PTE_SNOOPED;
-			page_base |= R600_PTE_READABLE | R600_PTE_WRITEABLE;
-
-			DRM_WRITE64(map, gart_idx * sizeof(u64), page_base);
-
-			gart_idx++;
-
-			if ((i % 128) == 0)
-				DRM_DEBUG("page entry %d: 0x%016llx\n",
-				    i, (unsigned long long)page_base);
-			entry_addr += ATI_PCIGART_PAGE_SIZE;
-		}
-	}
-	ret = 1;
-done:
-	return ret;
-}
-
-static void r600_vm_flush_gart_range(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	u32 resp, countdown = 1000;
-	RADEON_WRITE(R600_VM_CONTEXT0_INVALIDATION_LOW_ADDR, dev_priv->gart_vm_start >> 12);
-	RADEON_WRITE(R600_VM_CONTEXT0_INVALIDATION_HIGH_ADDR, (dev_priv->gart_vm_start + dev_priv->gart_size - 1) >> 12);
-	RADEON_WRITE(R600_VM_CONTEXT0_REQUEST_RESPONSE, 2);
-
-	do {
-		resp = RADEON_READ(R600_VM_CONTEXT0_REQUEST_RESPONSE);
-		countdown--;
-		DRM_UDELAY(1);
-	} while (((resp & 0xf0) == 0) && countdown);
-}
-
-static void r600_vm_init(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	/* initialise the VM to use the page table we constructed up there */
-	u32 vm_c0, i;
-	u32 mc_rd_a;
-	u32 vm_l2_cntl, vm_l2_cntl3;
-	/* okay set up the PCIE aperture type thingo */
-	RADEON_WRITE(R600_MC_VM_SYSTEM_APERTURE_LOW_ADDR, dev_priv->gart_vm_start >> 12);
-	RADEON_WRITE(R600_MC_VM_SYSTEM_APERTURE_HIGH_ADDR, (dev_priv->gart_vm_start + dev_priv->gart_size - 1) >> 12);
-	RADEON_WRITE(R600_MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR, 0);
-
-	/* setup MC RD a */
-	mc_rd_a = R600_MCD_L1_TLB | R600_MCD_L1_FRAG_PROC | R600_MCD_SYSTEM_ACCESS_MODE_IN_SYS |
-		R600_MCD_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU | R600_MCD_EFFECTIVE_L1_TLB_SIZE(5) |
-		R600_MCD_EFFECTIVE_L1_QUEUE_SIZE(5) | R600_MCD_WAIT_L2_QUERY;
-
-	RADEON_WRITE(R600_MCD_RD_A_CNTL, mc_rd_a);
-	RADEON_WRITE(R600_MCD_RD_B_CNTL, mc_rd_a);
-
-	RADEON_WRITE(R600_MCD_WR_A_CNTL, mc_rd_a);
-	RADEON_WRITE(R600_MCD_WR_B_CNTL, mc_rd_a);
-
-	RADEON_WRITE(R600_MCD_RD_GFX_CNTL, mc_rd_a);
-	RADEON_WRITE(R600_MCD_WR_GFX_CNTL, mc_rd_a);
-
-	RADEON_WRITE(R600_MCD_RD_SYS_CNTL, mc_rd_a);
-	RADEON_WRITE(R600_MCD_WR_SYS_CNTL, mc_rd_a);
-
-	RADEON_WRITE(R600_MCD_RD_HDP_CNTL, mc_rd_a | R600_MCD_L1_STRICT_ORDERING);
-	RADEON_WRITE(R600_MCD_WR_HDP_CNTL, mc_rd_a /*| R600_MCD_L1_STRICT_ORDERING*/);
-
-	RADEON_WRITE(R600_MCD_RD_PDMA_CNTL, mc_rd_a);
-	RADEON_WRITE(R600_MCD_WR_PDMA_CNTL, mc_rd_a);
-
-	RADEON_WRITE(R600_MCD_RD_SEM_CNTL, mc_rd_a | R600_MCD_SEMAPHORE_MODE);
-	RADEON_WRITE(R600_MCD_WR_SEM_CNTL, mc_rd_a);
-
-	vm_l2_cntl = R600_VM_L2_CACHE_EN | R600_VM_L2_FRAG_PROC | R600_VM_ENABLE_PTE_CACHE_LRU_W;
-	vm_l2_cntl |= R600_VM_L2_CNTL_QUEUE_SIZE(7);
-	RADEON_WRITE(R600_VM_L2_CNTL, vm_l2_cntl);
-
-	RADEON_WRITE(R600_VM_L2_CNTL2, 0);
-	vm_l2_cntl3 = (R600_VM_L2_CNTL3_BANK_SELECT_0(0) |
-		       R600_VM_L2_CNTL3_BANK_SELECT_1(1) |
-		       R600_VM_L2_CNTL3_CACHE_UPDATE_MODE(2));
-	RADEON_WRITE(R600_VM_L2_CNTL3, vm_l2_cntl3);
-
-	vm_c0 = R600_VM_ENABLE_CONTEXT | R600_VM_PAGE_TABLE_DEPTH_FLAT;
-
-	RADEON_WRITE(R600_VM_CONTEXT0_CNTL, vm_c0);
-
-	vm_c0 &= ~R600_VM_ENABLE_CONTEXT;
-
-	/* disable all other contexts */
-	for (i = 1; i < 8; i++)
-		RADEON_WRITE(R600_VM_CONTEXT0_CNTL + (i * 4), vm_c0);
-
-	RADEON_WRITE(R600_VM_CONTEXT0_PAGE_TABLE_BASE_ADDR, dev_priv->gart_info.bus_addr >> 12);
-	RADEON_WRITE(R600_VM_CONTEXT0_PAGE_TABLE_START_ADDR, dev_priv->gart_vm_start >> 12);
-	RADEON_WRITE(R600_VM_CONTEXT0_PAGE_TABLE_END_ADDR, (dev_priv->gart_vm_start + dev_priv->gart_size - 1) >> 12);
-
-	r600_vm_flush_gart_range(dev);
-}
-
-static int r600_cp_init_microcode(drm_radeon_private_t *dev_priv)
-{
-	struct platform_device *pdev;
-	const char *chip_name;
-	size_t pfp_req_size, me_req_size;
-	char fw_name[30];
-	int err;
-
-	pdev = platform_device_register_simple("r600_cp", 0, NULL, 0);
-	err = IS_ERR(pdev);
-	if (err) {
-		printk(KERN_ERR "r600_cp: Failed to register firmware\n");
-		return -EINVAL;
-	}
-
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_R600:  chip_name = "R600";  break;
-	case CHIP_RV610: chip_name = "RV610"; break;
-	case CHIP_RV630: chip_name = "RV630"; break;
-	case CHIP_RV620: chip_name = "RV620"; break;
-	case CHIP_RV635: chip_name = "RV635"; break;
-	case CHIP_RV670: chip_name = "RV670"; break;
-	case CHIP_RS780:
-	case CHIP_RS880: chip_name = "RS780"; break;
-	case CHIP_RV770: chip_name = "RV770"; break;
-	case CHIP_RV730:
-	case CHIP_RV740: chip_name = "RV730"; break;
-	case CHIP_RV710: chip_name = "RV710"; break;
-	default:         BUG();
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770) {
-		pfp_req_size = R700_PFP_UCODE_SIZE * 4;
-		me_req_size = R700_PM4_UCODE_SIZE * 4;
-	} else {
-		pfp_req_size = PFP_UCODE_SIZE * 4;
-		me_req_size = PM4_UCODE_SIZE * 12;
-	}
-
-	DRM_INFO("Loading %s CP Microcode\n", chip_name);
-
-	snprintf(fw_name, sizeof(fw_name), "radeon/%s_pfp.bin", chip_name);
-	err = request_firmware(&dev_priv->pfp_fw, fw_name, &pdev->dev);
-	if (err)
-		goto out;
-	if (dev_priv->pfp_fw->size != pfp_req_size) {
-		printk(KERN_ERR
-		       "r600_cp: Bogus length %zu in firmware \"%s\"\n",
-		       dev_priv->pfp_fw->size, fw_name);
-		err = -EINVAL;
-		goto out;
-	}
-
-	snprintf(fw_name, sizeof(fw_name), "radeon/%s_me.bin", chip_name);
-	err = request_firmware(&dev_priv->me_fw, fw_name, &pdev->dev);
-	if (err)
-		goto out;
-	if (dev_priv->me_fw->size != me_req_size) {
-		printk(KERN_ERR
-		       "r600_cp: Bogus length %zu in firmware \"%s\"\n",
-		       dev_priv->me_fw->size, fw_name);
-		err = -EINVAL;
-	}
-out:
-	platform_device_unregister(pdev);
-
-	if (err) {
-		if (err != -EINVAL)
-			printk(KERN_ERR
-			       "r600_cp: Failed to load firmware \"%s\"\n",
-			       fw_name);
-		release_firmware(dev_priv->pfp_fw);
-		dev_priv->pfp_fw = NULL;
-		release_firmware(dev_priv->me_fw);
-		dev_priv->me_fw = NULL;
-	}
-	return err;
-}
-
-static void r600_cp_load_microcode(drm_radeon_private_t *dev_priv)
-{
-	const __be32 *fw_data;
-	int i;
-
-	if (!dev_priv->me_fw || !dev_priv->pfp_fw)
-		return;
-
-	r600_do_cp_stop(dev_priv);
-
-	RADEON_WRITE(R600_CP_RB_CNTL,
-#ifdef __BIG_ENDIAN
-		     R600_BUF_SWAP_32BIT |
-#endif
-		     R600_RB_NO_UPDATE |
-		     R600_RB_BLKSZ(15) |
-		     R600_RB_BUFSZ(3));
-
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, R600_SOFT_RESET_CP);
-	RADEON_READ(R600_GRBM_SOFT_RESET);
-	mdelay(15);
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, 0);
-
-	fw_data = (const __be32 *)dev_priv->me_fw->data;
-	RADEON_WRITE(R600_CP_ME_RAM_WADDR, 0);
-	for (i = 0; i < PM4_UCODE_SIZE * 3; i++)
-		RADEON_WRITE(R600_CP_ME_RAM_DATA,
-			     be32_to_cpup(fw_data++));
-
-	fw_data = (const __be32 *)dev_priv->pfp_fw->data;
-	RADEON_WRITE(R600_CP_PFP_UCODE_ADDR, 0);
-	for (i = 0; i < PFP_UCODE_SIZE; i++)
-		RADEON_WRITE(R600_CP_PFP_UCODE_DATA,
-			     be32_to_cpup(fw_data++));
-
-	RADEON_WRITE(R600_CP_PFP_UCODE_ADDR, 0);
-	RADEON_WRITE(R600_CP_ME_RAM_WADDR, 0);
-	RADEON_WRITE(R600_CP_ME_RAM_RADDR, 0);
-
-}
-
-static void r700_vm_init(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	/* initialise the VM to use the page table we constructed up there */
-	u32 vm_c0, i;
-	u32 mc_vm_md_l1;
-	u32 vm_l2_cntl, vm_l2_cntl3;
-	/* okay set up the PCIE aperture type thingo */
-	RADEON_WRITE(R700_MC_VM_SYSTEM_APERTURE_LOW_ADDR, dev_priv->gart_vm_start >> 12);
-	RADEON_WRITE(R700_MC_VM_SYSTEM_APERTURE_HIGH_ADDR, (dev_priv->gart_vm_start + dev_priv->gart_size - 1) >> 12);
-	RADEON_WRITE(R700_MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR, 0);
-
-	mc_vm_md_l1 = R700_ENABLE_L1_TLB |
-	    R700_ENABLE_L1_FRAGMENT_PROCESSING |
-	    R700_SYSTEM_ACCESS_MODE_IN_SYS |
-	    R700_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU |
-	    R700_EFFECTIVE_L1_TLB_SIZE(5) |
-	    R700_EFFECTIVE_L1_QUEUE_SIZE(5);
-
-	RADEON_WRITE(R700_MC_VM_MD_L1_TLB0_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MD_L1_TLB1_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MD_L1_TLB2_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MB_L1_TLB0_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MB_L1_TLB1_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MB_L1_TLB2_CNTL, mc_vm_md_l1);
-	RADEON_WRITE(R700_MC_VM_MB_L1_TLB3_CNTL, mc_vm_md_l1);
-
-	vm_l2_cntl = R600_VM_L2_CACHE_EN | R600_VM_L2_FRAG_PROC | R600_VM_ENABLE_PTE_CACHE_LRU_W;
-	vm_l2_cntl |= R700_VM_L2_CNTL_QUEUE_SIZE(7);
-	RADEON_WRITE(R600_VM_L2_CNTL, vm_l2_cntl);
-
-	RADEON_WRITE(R600_VM_L2_CNTL2, 0);
-	vm_l2_cntl3 = R700_VM_L2_CNTL3_BANK_SELECT(0) | R700_VM_L2_CNTL3_CACHE_UPDATE_MODE(2);
-	RADEON_WRITE(R600_VM_L2_CNTL3, vm_l2_cntl3);
-
-	vm_c0 = R600_VM_ENABLE_CONTEXT | R600_VM_PAGE_TABLE_DEPTH_FLAT;
-
-	RADEON_WRITE(R600_VM_CONTEXT0_CNTL, vm_c0);
-
-	vm_c0 &= ~R600_VM_ENABLE_CONTEXT;
-
-	/* disable all other contexts */
-	for (i = 1; i < 8; i++)
-		RADEON_WRITE(R600_VM_CONTEXT0_CNTL + (i * 4), vm_c0);
-
-	RADEON_WRITE(R700_VM_CONTEXT0_PAGE_TABLE_BASE_ADDR, dev_priv->gart_info.bus_addr >> 12);
-	RADEON_WRITE(R700_VM_CONTEXT0_PAGE_TABLE_START_ADDR, dev_priv->gart_vm_start >> 12);
-	RADEON_WRITE(R700_VM_CONTEXT0_PAGE_TABLE_END_ADDR, (dev_priv->gart_vm_start + dev_priv->gart_size - 1) >> 12);
-
-	r600_vm_flush_gart_range(dev);
-}
-
-static void r700_cp_load_microcode(drm_radeon_private_t *dev_priv)
-{
-	const __be32 *fw_data;
-	int i;
-
-	if (!dev_priv->me_fw || !dev_priv->pfp_fw)
-		return;
-
-	r600_do_cp_stop(dev_priv);
-
-	RADEON_WRITE(R600_CP_RB_CNTL,
-#ifdef __BIG_ENDIAN
-		     R600_BUF_SWAP_32BIT |
-#endif
-		     R600_RB_NO_UPDATE |
-		     R600_RB_BLKSZ(15) |
-		     R600_RB_BUFSZ(3));
-
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, R600_SOFT_RESET_CP);
-	RADEON_READ(R600_GRBM_SOFT_RESET);
-	mdelay(15);
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, 0);
-
-	fw_data = (const __be32 *)dev_priv->pfp_fw->data;
-	RADEON_WRITE(R600_CP_PFP_UCODE_ADDR, 0);
-	for (i = 0; i < R700_PFP_UCODE_SIZE; i++)
-		RADEON_WRITE(R600_CP_PFP_UCODE_DATA, be32_to_cpup(fw_data++));
-	RADEON_WRITE(R600_CP_PFP_UCODE_ADDR, 0);
-
-	fw_data = (const __be32 *)dev_priv->me_fw->data;
-	RADEON_WRITE(R600_CP_ME_RAM_WADDR, 0);
-	for (i = 0; i < R700_PM4_UCODE_SIZE; i++)
-		RADEON_WRITE(R600_CP_ME_RAM_DATA, be32_to_cpup(fw_data++));
-	RADEON_WRITE(R600_CP_ME_RAM_WADDR, 0);
-
-	RADEON_WRITE(R600_CP_PFP_UCODE_ADDR, 0);
-	RADEON_WRITE(R600_CP_ME_RAM_WADDR, 0);
-	RADEON_WRITE(R600_CP_ME_RAM_RADDR, 0);
-
-}
-
-static void r600_test_writeback(drm_radeon_private_t *dev_priv)
-{
-	u32 tmp;
-
-	/* Start with assuming that writeback doesn't work */
-	dev_priv->writeback_works = 0;
-
-	/* Writeback doesn't seem to work everywhere, test it here and possibly
-	 * enable it if it appears to work
-	 */
-	radeon_write_ring_rptr(dev_priv, R600_SCRATCHOFF(1), 0);
-
-	RADEON_WRITE(R600_SCRATCH_REG1, 0xdeadbeef);
-
-	for (tmp = 0; tmp < dev_priv->usec_timeout; tmp++) {
-		u32 val;
-
-		val = radeon_read_ring_rptr(dev_priv, R600_SCRATCHOFF(1));
-		if (val == 0xdeadbeef)
-			break;
-		DRM_UDELAY(1);
-	}
-
-	if (tmp < dev_priv->usec_timeout) {
-		dev_priv->writeback_works = 1;
-		DRM_INFO("writeback test succeeded in %d usecs\n", tmp);
-	} else {
-		dev_priv->writeback_works = 0;
-		DRM_INFO("writeback test failed\n");
-	}
-	if (radeon_no_wb == 1) {
-		dev_priv->writeback_works = 0;
-		DRM_INFO("writeback forced off\n");
-	}
-
-	if (!dev_priv->writeback_works) {
-		/* Disable writeback to avoid unnecessary bus master transfer */
-		RADEON_WRITE(R600_CP_RB_CNTL,
-#ifdef __BIG_ENDIAN
-			     R600_BUF_SWAP_32BIT |
-#endif
-			     RADEON_READ(R600_CP_RB_CNTL) |
-			     R600_RB_NO_UPDATE);
-		RADEON_WRITE(R600_SCRATCH_UMSK, 0);
-	}
-}
-
-int r600_do_engine_reset(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	u32 cp_ptr, cp_me_cntl, cp_rb_cntl;
-
-	DRM_INFO("Resetting GPU\n");
-
-	cp_ptr = RADEON_READ(R600_CP_RB_WPTR);
-	cp_me_cntl = RADEON_READ(R600_CP_ME_CNTL);
-	RADEON_WRITE(R600_CP_ME_CNTL, R600_CP_ME_HALT);
-
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, 0x7fff);
-	RADEON_READ(R600_GRBM_SOFT_RESET);
-	DRM_UDELAY(50);
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, 0);
-	RADEON_READ(R600_GRBM_SOFT_RESET);
-
-	RADEON_WRITE(R600_CP_RB_WPTR_DELAY, 0);
-	cp_rb_cntl = RADEON_READ(R600_CP_RB_CNTL);
-	RADEON_WRITE(R600_CP_RB_CNTL,
-#ifdef __BIG_ENDIAN
-		     R600_BUF_SWAP_32BIT |
-#endif
-		     R600_RB_RPTR_WR_ENA);
-
-	RADEON_WRITE(R600_CP_RB_RPTR_WR, cp_ptr);
-	RADEON_WRITE(R600_CP_RB_WPTR, cp_ptr);
-	RADEON_WRITE(R600_CP_RB_CNTL, cp_rb_cntl);
-	RADEON_WRITE(R600_CP_ME_CNTL, cp_me_cntl);
-
-	/* Reset the CP ring */
-	r600_do_cp_reset(dev_priv);
-
-	/* The CP is no longer running after an engine reset */
-	dev_priv->cp_running = 0;
-
-	/* Reset any pending vertex, indirect buffers */
-	radeon_freelist_reset(dev);
-
-	return 0;
-
-}
-
-static u32 r600_get_tile_pipe_to_backend_map(u32 num_tile_pipes,
-					     u32 num_backends,
-					     u32 backend_disable_mask)
-{
-	u32 backend_map = 0;
-	u32 enabled_backends_mask;
-	u32 enabled_backends_count;
-	u32 cur_pipe;
-	u32 swizzle_pipe[R6XX_MAX_PIPES];
-	u32 cur_backend;
-	u32 i;
-
-	if (num_tile_pipes > R6XX_MAX_PIPES)
-		num_tile_pipes = R6XX_MAX_PIPES;
-	if (num_tile_pipes < 1)
-		num_tile_pipes = 1;
-	if (num_backends > R6XX_MAX_BACKENDS)
-		num_backends = R6XX_MAX_BACKENDS;
-	if (num_backends < 1)
-		num_backends = 1;
-
-	enabled_backends_mask = 0;
-	enabled_backends_count = 0;
-	for (i = 0; i < R6XX_MAX_BACKENDS; ++i) {
-		if (((backend_disable_mask >> i) & 1) == 0) {
-			enabled_backends_mask |= (1 << i);
-			++enabled_backends_count;
-		}
-		if (enabled_backends_count == num_backends)
-			break;
-	}
-
-	if (enabled_backends_count == 0) {
-		enabled_backends_mask = 1;
-		enabled_backends_count = 1;
-	}
-
-	if (enabled_backends_count != num_backends)
-		num_backends = enabled_backends_count;
-
-	memset((uint8_t *)&swizzle_pipe[0], 0, sizeof(u32) * R6XX_MAX_PIPES);
-	switch (num_tile_pipes) {
-	case 1:
-		swizzle_pipe[0] = 0;
-		break;
-	case 2:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 1;
-		break;
-	case 3:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 1;
-		swizzle_pipe[2] = 2;
-		break;
-	case 4:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 1;
-		swizzle_pipe[2] = 2;
-		swizzle_pipe[3] = 3;
-		break;
-	case 5:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 1;
-		swizzle_pipe[2] = 2;
-		swizzle_pipe[3] = 3;
-		swizzle_pipe[4] = 4;
-		break;
-	case 6:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 2;
-		swizzle_pipe[2] = 4;
-		swizzle_pipe[3] = 5;
-		swizzle_pipe[4] = 1;
-		swizzle_pipe[5] = 3;
-		break;
-	case 7:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 2;
-		swizzle_pipe[2] = 4;
-		swizzle_pipe[3] = 6;
-		swizzle_pipe[4] = 1;
-		swizzle_pipe[5] = 3;
-		swizzle_pipe[6] = 5;
-		break;
-	case 8:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 2;
-		swizzle_pipe[2] = 4;
-		swizzle_pipe[3] = 6;
-		swizzle_pipe[4] = 1;
-		swizzle_pipe[5] = 3;
-		swizzle_pipe[6] = 5;
-		swizzle_pipe[7] = 7;
-		break;
-	}
-
-	cur_backend = 0;
-	for (cur_pipe = 0; cur_pipe < num_tile_pipes; ++cur_pipe) {
-		while (((1 << cur_backend) & enabled_backends_mask) == 0)
-			cur_backend = (cur_backend + 1) % R6XX_MAX_BACKENDS;
-
-		backend_map |= (u32)(((cur_backend & 3) << (swizzle_pipe[cur_pipe] * 2)));
-
-		cur_backend = (cur_backend + 1) % R6XX_MAX_BACKENDS;
-	}
-
-	return backend_map;
-}
-
-static int r600_count_pipe_bits(uint32_t val)
-{
-	return hweight32(val);
-}
-
-static void r600_gfx_init(struct drm_device *dev,
-			  drm_radeon_private_t *dev_priv)
-{
-	int i, j, num_qd_pipes;
-	u32 sx_debug_1;
-	u32 tc_cntl;
-	u32 arb_pop;
-	u32 num_gs_verts_per_thread;
-	u32 vgt_gs_per_es;
-	u32 gs_prim_buffer_depth = 0;
-	u32 sq_ms_fifo_sizes;
-	u32 sq_config;
-	u32 sq_gpr_resource_mgmt_1 = 0;
-	u32 sq_gpr_resource_mgmt_2 = 0;
-	u32 sq_thread_resource_mgmt = 0;
-	u32 sq_stack_resource_mgmt_1 = 0;
-	u32 sq_stack_resource_mgmt_2 = 0;
-	u32 hdp_host_path_cntl;
-	u32 backend_map;
-	u32 gb_tiling_config = 0;
-	u32 cc_rb_backend_disable;
-	u32 cc_gc_shader_pipe_config;
-	u32 ramcfg;
-
-	/* setup chip specs */
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_R600:
-		dev_priv->r600_max_pipes = 4;
-		dev_priv->r600_max_tile_pipes = 8;
-		dev_priv->r600_max_simds = 4;
-		dev_priv->r600_max_backends = 4;
-		dev_priv->r600_max_gprs = 256;
-		dev_priv->r600_max_threads = 192;
-		dev_priv->r600_max_stack_entries = 256;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 16;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 128;
-		dev_priv->r600_sq_num_cf_insts = 2;
-		break;
-	case CHIP_RV630:
-	case CHIP_RV635:
-		dev_priv->r600_max_pipes = 2;
-		dev_priv->r600_max_tile_pipes = 2;
-		dev_priv->r600_max_simds = 3;
-		dev_priv->r600_max_backends = 1;
-		dev_priv->r600_max_gprs = 128;
-		dev_priv->r600_max_threads = 192;
-		dev_priv->r600_max_stack_entries = 128;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 4;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 128;
-		dev_priv->r600_sq_num_cf_insts = 2;
-		break;
-	case CHIP_RV610:
-	case CHIP_RS780:
-	case CHIP_RS880:
-	case CHIP_RV620:
-		dev_priv->r600_max_pipes = 1;
-		dev_priv->r600_max_tile_pipes = 1;
-		dev_priv->r600_max_simds = 2;
-		dev_priv->r600_max_backends = 1;
-		dev_priv->r600_max_gprs = 128;
-		dev_priv->r600_max_threads = 192;
-		dev_priv->r600_max_stack_entries = 128;
-		dev_priv->r600_max_hw_contexts = 4;
-		dev_priv->r600_max_gs_threads = 4;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 128;
-		dev_priv->r600_sq_num_cf_insts = 1;
-		break;
-	case CHIP_RV670:
-		dev_priv->r600_max_pipes = 4;
-		dev_priv->r600_max_tile_pipes = 4;
-		dev_priv->r600_max_simds = 4;
-		dev_priv->r600_max_backends = 4;
-		dev_priv->r600_max_gprs = 192;
-		dev_priv->r600_max_threads = 192;
-		dev_priv->r600_max_stack_entries = 256;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 16;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 128;
-		dev_priv->r600_sq_num_cf_insts = 2;
-		break;
-	default:
-		break;
-	}
-
-	/* Initialize HDP */
-	j = 0;
-	for (i = 0; i < 32; i++) {
-		RADEON_WRITE((0x2c14 + j), 0x00000000);
-		RADEON_WRITE((0x2c18 + j), 0x00000000);
-		RADEON_WRITE((0x2c1c + j), 0x00000000);
-		RADEON_WRITE((0x2c20 + j), 0x00000000);
-		RADEON_WRITE((0x2c24 + j), 0x00000000);
-		j += 0x18;
-	}
-
-	RADEON_WRITE(R600_GRBM_CNTL, R600_GRBM_READ_TIMEOUT(0xff));
-
-	/* setup tiling, simd, pipe config */
-	ramcfg = RADEON_READ(R600_RAMCFG);
-
-	switch (dev_priv->r600_max_tile_pipes) {
-	case 1:
-		gb_tiling_config |= R600_PIPE_TILING(0);
-		break;
-	case 2:
-		gb_tiling_config |= R600_PIPE_TILING(1);
-		break;
-	case 4:
-		gb_tiling_config |= R600_PIPE_TILING(2);
-		break;
-	case 8:
-		gb_tiling_config |= R600_PIPE_TILING(3);
-		break;
-	default:
-		break;
-	}
-
-	gb_tiling_config |= R600_BANK_TILING((ramcfg >> R600_NOOFBANK_SHIFT) & R600_NOOFBANK_MASK);
-
-	gb_tiling_config |= R600_GROUP_SIZE(0);
-
-	if (((ramcfg >> R600_NOOFROWS_SHIFT) & R600_NOOFROWS_MASK) > 3) {
-		gb_tiling_config |= R600_ROW_TILING(3);
-		gb_tiling_config |= R600_SAMPLE_SPLIT(3);
-	} else {
-		gb_tiling_config |=
-			R600_ROW_TILING(((ramcfg >> R600_NOOFROWS_SHIFT) & R600_NOOFROWS_MASK));
-		gb_tiling_config |=
-			R600_SAMPLE_SPLIT(((ramcfg >> R600_NOOFROWS_SHIFT) & R600_NOOFROWS_MASK));
-	}
-
-	gb_tiling_config |= R600_BANK_SWAPS(1);
-
-	cc_rb_backend_disable = RADEON_READ(R600_CC_RB_BACKEND_DISABLE) & 0x00ff0000;
-	cc_rb_backend_disable |=
-		R600_BACKEND_DISABLE((R6XX_MAX_BACKENDS_MASK << dev_priv->r600_max_backends) & R6XX_MAX_BACKENDS_MASK);
-
-	cc_gc_shader_pipe_config = RADEON_READ(R600_CC_GC_SHADER_PIPE_CONFIG) & 0xffffff00;
-	cc_gc_shader_pipe_config |=
-		R600_INACTIVE_QD_PIPES((R6XX_MAX_PIPES_MASK << dev_priv->r600_max_pipes) & R6XX_MAX_PIPES_MASK);
-	cc_gc_shader_pipe_config |=
-		R600_INACTIVE_SIMDS((R6XX_MAX_SIMDS_MASK << dev_priv->r600_max_simds) & R6XX_MAX_SIMDS_MASK);
-
-	backend_map = r600_get_tile_pipe_to_backend_map(dev_priv->r600_max_tile_pipes,
-							(R6XX_MAX_BACKENDS -
-							 r600_count_pipe_bits((cc_rb_backend_disable &
-									       R6XX_MAX_BACKENDS_MASK) >> 16)),
-							(cc_rb_backend_disable >> 16));
-	gb_tiling_config |= R600_BACKEND_MAP(backend_map);
-
-	RADEON_WRITE(R600_GB_TILING_CONFIG,      gb_tiling_config);
-	RADEON_WRITE(R600_DCP_TILING_CONFIG,    (gb_tiling_config & 0xffff));
-	RADEON_WRITE(R600_HDP_TILING_CONFIG,    (gb_tiling_config & 0xffff));
-	if (gb_tiling_config & 0xc0) {
-		dev_priv->r600_group_size = 512;
-	} else {
-		dev_priv->r600_group_size = 256;
-	}
-	dev_priv->r600_npipes = 1 << ((gb_tiling_config >> 1) & 0x7);
-	if (gb_tiling_config & 0x30) {
-		dev_priv->r600_nbanks = 8;
-	} else {
-		dev_priv->r600_nbanks = 4;
-	}
-
-	RADEON_WRITE(R600_CC_RB_BACKEND_DISABLE,      cc_rb_backend_disable);
-	RADEON_WRITE(R600_CC_GC_SHADER_PIPE_CONFIG,   cc_gc_shader_pipe_config);
-	RADEON_WRITE(R600_GC_USER_SHADER_PIPE_CONFIG, cc_gc_shader_pipe_config);
-
-	num_qd_pipes =
-		R6XX_MAX_PIPES - r600_count_pipe_bits((cc_gc_shader_pipe_config & R600_INACTIVE_QD_PIPES_MASK) >> 8);
-	RADEON_WRITE(R600_VGT_OUT_DEALLOC_CNTL, (num_qd_pipes * 4) & R600_DEALLOC_DIST_MASK);
-	RADEON_WRITE(R600_VGT_VERTEX_REUSE_BLOCK_CNTL, ((num_qd_pipes * 4) - 2) & R600_VTX_REUSE_DEPTH_MASK);
-
-	/* set HW defaults for 3D engine */
-	RADEON_WRITE(R600_CP_QUEUE_THRESHOLDS, (R600_ROQ_IB1_START(0x16) |
-						R600_ROQ_IB2_START(0x2b)));
-
-	RADEON_WRITE(R600_CP_MEQ_THRESHOLDS, (R600_MEQ_END(0x40) |
-					      R600_ROQ_END(0x40)));
-
-	RADEON_WRITE(R600_TA_CNTL_AUX, (R600_DISABLE_CUBE_ANISO |
-					R600_SYNC_GRADIENT |
-					R600_SYNC_WALKER |
-					R600_SYNC_ALIGNER));
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV670)
-		RADEON_WRITE(R600_ARB_GDEC_RD_CNTL, 0x00000021);
-
-	sx_debug_1 = RADEON_READ(R600_SX_DEBUG_1);
-	sx_debug_1 |= R600_SMX_EVENT_RELEASE;
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_R600))
-		sx_debug_1 |= R600_ENABLE_NEW_SMX_ADDRESS;
-	RADEON_WRITE(R600_SX_DEBUG_1, sx_debug_1);
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R600) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV630) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880))
-		RADEON_WRITE(R600_DB_DEBUG, R600_PREZ_MUST_WAIT_FOR_POSTZ_DONE);
-	else
-		RADEON_WRITE(R600_DB_DEBUG, 0);
-
-	RADEON_WRITE(R600_DB_WATERMARKS, (R600_DEPTH_FREE(4) |
-					  R600_DEPTH_FLUSH(16) |
-					  R600_DEPTH_PENDING_FREE(4) |
-					  R600_DEPTH_CACHELINE_FREE(16)));
-	RADEON_WRITE(R600_PA_SC_MULTI_CHIP_CNTL, 0);
-	RADEON_WRITE(R600_VGT_NUM_INSTANCES, 0);
-
-	RADEON_WRITE(R600_SPI_CONFIG_CNTL, R600_GPR_WRITE_PRIORITY(0));
-	RADEON_WRITE(R600_SPI_CONFIG_CNTL_1, R600_VTX_DONE_DELAY(0));
-
-	sq_ms_fifo_sizes = RADEON_READ(R600_SQ_MS_FIFO_SIZES);
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880)) {
-		sq_ms_fifo_sizes = (R600_CACHE_FIFO_SIZE(0xa) |
-				    R600_FETCH_FIFO_HIWATER(0xa) |
-				    R600_DONE_FIFO_HIWATER(0xe0) |
-				    R600_ALU_UPDATE_FIFO_HIWATER(0x8));
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R600) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV630)) {
-		sq_ms_fifo_sizes &= ~R600_DONE_FIFO_HIWATER(0xff);
-		sq_ms_fifo_sizes |= R600_DONE_FIFO_HIWATER(0x4);
-	}
-	RADEON_WRITE(R600_SQ_MS_FIFO_SIZES, sq_ms_fifo_sizes);
-
-	/* SQ_CONFIG, SQ_GPR_RESOURCE_MGMT, SQ_THREAD_RESOURCE_MGMT, SQ_STACK_RESOURCE_MGMT
-	 * should be adjusted as needed by the 2D/3D drivers.  This just sets default values
-	 */
-	sq_config = RADEON_READ(R600_SQ_CONFIG);
-	sq_config &= ~(R600_PS_PRIO(3) |
-		       R600_VS_PRIO(3) |
-		       R600_GS_PRIO(3) |
-		       R600_ES_PRIO(3));
-	sq_config |= (R600_DX9_CONSTS |
-		      R600_VC_ENABLE |
-		      R600_PS_PRIO(0) |
-		      R600_VS_PRIO(1) |
-		      R600_GS_PRIO(2) |
-		      R600_ES_PRIO(3));
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R600) {
-		sq_gpr_resource_mgmt_1 = (R600_NUM_PS_GPRS(124) |
-					  R600_NUM_VS_GPRS(124) |
-					  R600_NUM_CLAUSE_TEMP_GPRS(4));
-		sq_gpr_resource_mgmt_2 = (R600_NUM_GS_GPRS(0) |
-					  R600_NUM_ES_GPRS(0));
-		sq_thread_resource_mgmt = (R600_NUM_PS_THREADS(136) |
-					   R600_NUM_VS_THREADS(48) |
-					   R600_NUM_GS_THREADS(4) |
-					   R600_NUM_ES_THREADS(4));
-		sq_stack_resource_mgmt_1 = (R600_NUM_PS_STACK_ENTRIES(128) |
-					    R600_NUM_VS_STACK_ENTRIES(128));
-		sq_stack_resource_mgmt_2 = (R600_NUM_GS_STACK_ENTRIES(0) |
-					    R600_NUM_ES_STACK_ENTRIES(0));
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880)) {
-		/* no vertex cache */
-		sq_config &= ~R600_VC_ENABLE;
-
-		sq_gpr_resource_mgmt_1 = (R600_NUM_PS_GPRS(44) |
-					  R600_NUM_VS_GPRS(44) |
-					  R600_NUM_CLAUSE_TEMP_GPRS(2));
-		sq_gpr_resource_mgmt_2 = (R600_NUM_GS_GPRS(17) |
-					  R600_NUM_ES_GPRS(17));
-		sq_thread_resource_mgmt = (R600_NUM_PS_THREADS(79) |
-					   R600_NUM_VS_THREADS(78) |
-					   R600_NUM_GS_THREADS(4) |
-					   R600_NUM_ES_THREADS(31));
-		sq_stack_resource_mgmt_1 = (R600_NUM_PS_STACK_ENTRIES(40) |
-					    R600_NUM_VS_STACK_ENTRIES(40));
-		sq_stack_resource_mgmt_2 = (R600_NUM_GS_STACK_ENTRIES(32) |
-					    R600_NUM_ES_STACK_ENTRIES(16));
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV630) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV635)) {
-		sq_gpr_resource_mgmt_1 = (R600_NUM_PS_GPRS(44) |
-					  R600_NUM_VS_GPRS(44) |
-					  R600_NUM_CLAUSE_TEMP_GPRS(2));
-		sq_gpr_resource_mgmt_2 = (R600_NUM_GS_GPRS(18) |
-					  R600_NUM_ES_GPRS(18));
-		sq_thread_resource_mgmt = (R600_NUM_PS_THREADS(79) |
-					   R600_NUM_VS_THREADS(78) |
-					   R600_NUM_GS_THREADS(4) |
-					   R600_NUM_ES_THREADS(31));
-		sq_stack_resource_mgmt_1 = (R600_NUM_PS_STACK_ENTRIES(40) |
-					    R600_NUM_VS_STACK_ENTRIES(40));
-		sq_stack_resource_mgmt_2 = (R600_NUM_GS_STACK_ENTRIES(32) |
-					    R600_NUM_ES_STACK_ENTRIES(16));
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV670) {
-		sq_gpr_resource_mgmt_1 = (R600_NUM_PS_GPRS(44) |
-					  R600_NUM_VS_GPRS(44) |
-					  R600_NUM_CLAUSE_TEMP_GPRS(2));
-		sq_gpr_resource_mgmt_2 = (R600_NUM_GS_GPRS(17) |
-					  R600_NUM_ES_GPRS(17));
-		sq_thread_resource_mgmt = (R600_NUM_PS_THREADS(79) |
-					   R600_NUM_VS_THREADS(78) |
-					   R600_NUM_GS_THREADS(4) |
-					   R600_NUM_ES_THREADS(31));
-		sq_stack_resource_mgmt_1 = (R600_NUM_PS_STACK_ENTRIES(64) |
-					    R600_NUM_VS_STACK_ENTRIES(64));
-		sq_stack_resource_mgmt_2 = (R600_NUM_GS_STACK_ENTRIES(64) |
-					    R600_NUM_ES_STACK_ENTRIES(64));
-	}
-
-	RADEON_WRITE(R600_SQ_CONFIG, sq_config);
-	RADEON_WRITE(R600_SQ_GPR_RESOURCE_MGMT_1,  sq_gpr_resource_mgmt_1);
-	RADEON_WRITE(R600_SQ_GPR_RESOURCE_MGMT_2,  sq_gpr_resource_mgmt_2);
-	RADEON_WRITE(R600_SQ_THREAD_RESOURCE_MGMT, sq_thread_resource_mgmt);
-	RADEON_WRITE(R600_SQ_STACK_RESOURCE_MGMT_1, sq_stack_resource_mgmt_1);
-	RADEON_WRITE(R600_SQ_STACK_RESOURCE_MGMT_2, sq_stack_resource_mgmt_2);
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV610) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV620) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS780) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS880))
-		RADEON_WRITE(R600_VGT_CACHE_INVALIDATION, R600_CACHE_INVALIDATION(R600_TC_ONLY));
-	else
-		RADEON_WRITE(R600_VGT_CACHE_INVALIDATION, R600_CACHE_INVALIDATION(R600_VC_AND_TC));
-
-	RADEON_WRITE(R600_PA_SC_AA_SAMPLE_LOCS_2S, (R600_S0_X(0xc) |
-						    R600_S0_Y(0x4) |
-						    R600_S1_X(0x4) |
-						    R600_S1_Y(0xc)));
-	RADEON_WRITE(R600_PA_SC_AA_SAMPLE_LOCS_4S, (R600_S0_X(0xe) |
-						    R600_S0_Y(0xe) |
-						    R600_S1_X(0x2) |
-						    R600_S1_Y(0x2) |
-						    R600_S2_X(0xa) |
-						    R600_S2_Y(0x6) |
-						    R600_S3_X(0x6) |
-						    R600_S3_Y(0xa)));
-	RADEON_WRITE(R600_PA_SC_AA_SAMPLE_LOCS_8S_WD0, (R600_S0_X(0xe) |
-							R600_S0_Y(0xb) |
-							R600_S1_X(0x4) |
-							R600_S1_Y(0xc) |
-							R600_S2_X(0x1) |
-							R600_S2_Y(0x6) |
-							R600_S3_X(0xa) |
-							R600_S3_Y(0xe)));
-	RADEON_WRITE(R600_PA_SC_AA_SAMPLE_LOCS_8S_WD1, (R600_S4_X(0x6) |
-							R600_S4_Y(0x1) |
-							R600_S5_X(0x0) |
-							R600_S5_Y(0x0) |
-							R600_S6_X(0xb) |
-							R600_S6_Y(0x4) |
-							R600_S7_X(0x7) |
-							R600_S7_Y(0x8)));
-
-
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_R600:
-	case CHIP_RV630:
-	case CHIP_RV635:
-		gs_prim_buffer_depth = 0;
-		break;
-	case CHIP_RV610:
-	case CHIP_RS780:
-	case CHIP_RS880:
-	case CHIP_RV620:
-		gs_prim_buffer_depth = 32;
-		break;
-	case CHIP_RV670:
-		gs_prim_buffer_depth = 128;
-		break;
-	default:
-		break;
-	}
-
-	num_gs_verts_per_thread = dev_priv->r600_max_pipes * 16;
-	vgt_gs_per_es = gs_prim_buffer_depth + num_gs_verts_per_thread;
-	/* Max value for this is 256 */
-	if (vgt_gs_per_es > 256)
-		vgt_gs_per_es = 256;
-
-	RADEON_WRITE(R600_VGT_ES_PER_GS, 128);
-	RADEON_WRITE(R600_VGT_GS_PER_ES, vgt_gs_per_es);
-	RADEON_WRITE(R600_VGT_GS_PER_VS, 2);
-	RADEON_WRITE(R600_VGT_GS_VERTEX_REUSE, 16);
-
-	/* more default values. 2D/3D driver should adjust as needed */
-	RADEON_WRITE(R600_PA_SC_LINE_STIPPLE_STATE, 0);
-	RADEON_WRITE(R600_VGT_STRMOUT_EN, 0);
-	RADEON_WRITE(R600_SX_MISC, 0);
-	RADEON_WRITE(R600_PA_SC_MODE_CNTL, 0);
-	RADEON_WRITE(R600_PA_SC_AA_CONFIG, 0);
-	RADEON_WRITE(R600_PA_SC_LINE_STIPPLE, 0);
-	RADEON_WRITE(R600_SPI_INPUT_Z, 0);
-	RADEON_WRITE(R600_SPI_PS_IN_CONTROL_0, R600_NUM_INTERP(2));
-	RADEON_WRITE(R600_CB_COLOR7_FRAG, 0);
-
-	/* clear render buffer base addresses */
-	RADEON_WRITE(R600_CB_COLOR0_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR1_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR2_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR3_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR4_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR5_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR6_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR7_BASE, 0);
-
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV610:
-	case CHIP_RS780:
-	case CHIP_RS880:
-	case CHIP_RV620:
-		tc_cntl = R600_TC_L2_SIZE(8);
-		break;
-	case CHIP_RV630:
-	case CHIP_RV635:
-		tc_cntl = R600_TC_L2_SIZE(4);
-		break;
-	case CHIP_R600:
-		tc_cntl = R600_TC_L2_SIZE(0) | R600_L2_DISABLE_LATE_HIT;
-		break;
-	default:
-		tc_cntl = R600_TC_L2_SIZE(0);
-		break;
-	}
-
-	RADEON_WRITE(R600_TC_CNTL, tc_cntl);
-
-	hdp_host_path_cntl = RADEON_READ(R600_HDP_HOST_PATH_CNTL);
-	RADEON_WRITE(R600_HDP_HOST_PATH_CNTL, hdp_host_path_cntl);
-
-	arb_pop = RADEON_READ(R600_ARB_POP);
-	arb_pop |= R600_ENABLE_TC128;
-	RADEON_WRITE(R600_ARB_POP, arb_pop);
-
-	RADEON_WRITE(R600_PA_SC_MULTI_CHIP_CNTL, 0);
-	RADEON_WRITE(R600_PA_CL_ENHANCE, (R600_CLIP_VTX_REORDER_ENA |
-					  R600_NUM_CLIP_SEQ(3)));
-	RADEON_WRITE(R600_PA_SC_ENHANCE, R600_FORCE_EOV_MAX_CLK_CNT(4095));
-
-}
-
-static u32 r700_get_tile_pipe_to_backend_map(drm_radeon_private_t *dev_priv,
-					     u32 num_tile_pipes,
-					     u32 num_backends,
-					     u32 backend_disable_mask)
-{
-	u32 backend_map = 0;
-	u32 enabled_backends_mask;
-	u32 enabled_backends_count;
-	u32 cur_pipe;
-	u32 swizzle_pipe[R7XX_MAX_PIPES];
-	u32 cur_backend;
-	u32 i;
-	bool force_no_swizzle;
-
-	if (num_tile_pipes > R7XX_MAX_PIPES)
-		num_tile_pipes = R7XX_MAX_PIPES;
-	if (num_tile_pipes < 1)
-		num_tile_pipes = 1;
-	if (num_backends > R7XX_MAX_BACKENDS)
-		num_backends = R7XX_MAX_BACKENDS;
-	if (num_backends < 1)
-		num_backends = 1;
-
-	enabled_backends_mask = 0;
-	enabled_backends_count = 0;
-	for (i = 0; i < R7XX_MAX_BACKENDS; ++i) {
-		if (((backend_disable_mask >> i) & 1) == 0) {
-			enabled_backends_mask |= (1 << i);
-			++enabled_backends_count;
-		}
-		if (enabled_backends_count == num_backends)
-			break;
-	}
-
-	if (enabled_backends_count == 0) {
-		enabled_backends_mask = 1;
-		enabled_backends_count = 1;
-	}
-
-	if (enabled_backends_count != num_backends)
-		num_backends = enabled_backends_count;
-
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV770:
-	case CHIP_RV730:
-		force_no_swizzle = false;
-		break;
-	case CHIP_RV710:
-	case CHIP_RV740:
-	default:
-		force_no_swizzle = true;
-		break;
-	}
-
-	memset((uint8_t *)&swizzle_pipe[0], 0, sizeof(u32) * R7XX_MAX_PIPES);
-	switch (num_tile_pipes) {
-	case 1:
-		swizzle_pipe[0] = 0;
-		break;
-	case 2:
-		swizzle_pipe[0] = 0;
-		swizzle_pipe[1] = 1;
-		break;
-	case 3:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 1;
-		}
-		break;
-	case 4:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-			swizzle_pipe[3] = 3;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 3;
-			swizzle_pipe[3] = 1;
-		}
-		break;
-	case 5:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-			swizzle_pipe[3] = 3;
-			swizzle_pipe[4] = 4;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 4;
-			swizzle_pipe[3] = 1;
-			swizzle_pipe[4] = 3;
-		}
-		break;
-	case 6:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-			swizzle_pipe[3] = 3;
-			swizzle_pipe[4] = 4;
-			swizzle_pipe[5] = 5;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 4;
-			swizzle_pipe[3] = 5;
-			swizzle_pipe[4] = 3;
-			swizzle_pipe[5] = 1;
-		}
-		break;
-	case 7:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-			swizzle_pipe[3] = 3;
-			swizzle_pipe[4] = 4;
-			swizzle_pipe[5] = 5;
-			swizzle_pipe[6] = 6;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 4;
-			swizzle_pipe[3] = 6;
-			swizzle_pipe[4] = 3;
-			swizzle_pipe[5] = 1;
-			swizzle_pipe[6] = 5;
-		}
-		break;
-	case 8:
-		if (force_no_swizzle) {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 1;
-			swizzle_pipe[2] = 2;
-			swizzle_pipe[3] = 3;
-			swizzle_pipe[4] = 4;
-			swizzle_pipe[5] = 5;
-			swizzle_pipe[6] = 6;
-			swizzle_pipe[7] = 7;
-		} else {
-			swizzle_pipe[0] = 0;
-			swizzle_pipe[1] = 2;
-			swizzle_pipe[2] = 4;
-			swizzle_pipe[3] = 6;
-			swizzle_pipe[4] = 3;
-			swizzle_pipe[5] = 1;
-			swizzle_pipe[6] = 7;
-			swizzle_pipe[7] = 5;
-		}
-		break;
-	}
-
-	cur_backend = 0;
-	for (cur_pipe = 0; cur_pipe < num_tile_pipes; ++cur_pipe) {
-		while (((1 << cur_backend) & enabled_backends_mask) == 0)
-			cur_backend = (cur_backend + 1) % R7XX_MAX_BACKENDS;
-
-		backend_map |= (u32)(((cur_backend & 3) << (swizzle_pipe[cur_pipe] * 2)));
-
-		cur_backend = (cur_backend + 1) % R7XX_MAX_BACKENDS;
-	}
-
-	return backend_map;
-}
-
-static void r700_gfx_init(struct drm_device *dev,
-			  drm_radeon_private_t *dev_priv)
-{
-	int i, j, num_qd_pipes;
-	u32 ta_aux_cntl;
-	u32 sx_debug_1;
-	u32 smx_dc_ctl0;
-	u32 db_debug3;
-	u32 num_gs_verts_per_thread;
-	u32 vgt_gs_per_es;
-	u32 gs_prim_buffer_depth = 0;
-	u32 sq_ms_fifo_sizes;
-	u32 sq_config;
-	u32 sq_thread_resource_mgmt;
-	u32 hdp_host_path_cntl;
-	u32 sq_dyn_gpr_size_simd_ab_0;
-	u32 backend_map;
-	u32 gb_tiling_config = 0;
-	u32 cc_rb_backend_disable;
-	u32 cc_gc_shader_pipe_config;
-	u32 mc_arb_ramcfg;
-	u32 db_debug4;
-
-	/* setup chip specs */
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV770:
-		dev_priv->r600_max_pipes = 4;
-		dev_priv->r600_max_tile_pipes = 8;
-		dev_priv->r600_max_simds = 10;
-		dev_priv->r600_max_backends = 4;
-		dev_priv->r600_max_gprs = 256;
-		dev_priv->r600_max_threads = 248;
-		dev_priv->r600_max_stack_entries = 512;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 16 * 2;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 112;
-		dev_priv->r600_sq_num_cf_insts = 2;
-
-		dev_priv->r700_sx_num_of_sets = 7;
-		dev_priv->r700_sc_prim_fifo_size = 0xF9;
-		dev_priv->r700_sc_hiz_tile_fifo_size = 0x30;
-		dev_priv->r700_sc_earlyz_tile_fifo_fize = 0x130;
-		break;
-	case CHIP_RV730:
-		dev_priv->r600_max_pipes = 2;
-		dev_priv->r600_max_tile_pipes = 4;
-		dev_priv->r600_max_simds = 8;
-		dev_priv->r600_max_backends = 2;
-		dev_priv->r600_max_gprs = 128;
-		dev_priv->r600_max_threads = 248;
-		dev_priv->r600_max_stack_entries = 256;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 16 * 2;
-		dev_priv->r600_sx_max_export_size = 256;
-		dev_priv->r600_sx_max_export_pos_size = 32;
-		dev_priv->r600_sx_max_export_smx_size = 224;
-		dev_priv->r600_sq_num_cf_insts = 2;
-
-		dev_priv->r700_sx_num_of_sets = 7;
-		dev_priv->r700_sc_prim_fifo_size = 0xf9;
-		dev_priv->r700_sc_hiz_tile_fifo_size = 0x30;
-		dev_priv->r700_sc_earlyz_tile_fifo_fize = 0x130;
-		if (dev_priv->r600_sx_max_export_pos_size > 16) {
-			dev_priv->r600_sx_max_export_pos_size -= 16;
-			dev_priv->r600_sx_max_export_smx_size += 16;
-		}
-		break;
-	case CHIP_RV710:
-		dev_priv->r600_max_pipes = 2;
-		dev_priv->r600_max_tile_pipes = 2;
-		dev_priv->r600_max_simds = 2;
-		dev_priv->r600_max_backends = 1;
-		dev_priv->r600_max_gprs = 256;
-		dev_priv->r600_max_threads = 192;
-		dev_priv->r600_max_stack_entries = 256;
-		dev_priv->r600_max_hw_contexts = 4;
-		dev_priv->r600_max_gs_threads = 8 * 2;
-		dev_priv->r600_sx_max_export_size = 128;
-		dev_priv->r600_sx_max_export_pos_size = 16;
-		dev_priv->r600_sx_max_export_smx_size = 112;
-		dev_priv->r600_sq_num_cf_insts = 1;
-
-		dev_priv->r700_sx_num_of_sets = 7;
-		dev_priv->r700_sc_prim_fifo_size = 0x40;
-		dev_priv->r700_sc_hiz_tile_fifo_size = 0x30;
-		dev_priv->r700_sc_earlyz_tile_fifo_fize = 0x130;
-		break;
-	case CHIP_RV740:
-		dev_priv->r600_max_pipes = 4;
-		dev_priv->r600_max_tile_pipes = 4;
-		dev_priv->r600_max_simds = 8;
-		dev_priv->r600_max_backends = 4;
-		dev_priv->r600_max_gprs = 256;
-		dev_priv->r600_max_threads = 248;
-		dev_priv->r600_max_stack_entries = 512;
-		dev_priv->r600_max_hw_contexts = 8;
-		dev_priv->r600_max_gs_threads = 16 * 2;
-		dev_priv->r600_sx_max_export_size = 256;
-		dev_priv->r600_sx_max_export_pos_size = 32;
-		dev_priv->r600_sx_max_export_smx_size = 224;
-		dev_priv->r600_sq_num_cf_insts = 2;
-
-		dev_priv->r700_sx_num_of_sets = 7;
-		dev_priv->r700_sc_prim_fifo_size = 0x100;
-		dev_priv->r700_sc_hiz_tile_fifo_size = 0x30;
-		dev_priv->r700_sc_earlyz_tile_fifo_fize = 0x130;
-
-		if (dev_priv->r600_sx_max_export_pos_size > 16) {
-			dev_priv->r600_sx_max_export_pos_size -= 16;
-			dev_priv->r600_sx_max_export_smx_size += 16;
-		}
-		break;
-	default:
-		break;
-	}
-
-	/* Initialize HDP */
-	j = 0;
-	for (i = 0; i < 32; i++) {
-		RADEON_WRITE((0x2c14 + j), 0x00000000);
-		RADEON_WRITE((0x2c18 + j), 0x00000000);
-		RADEON_WRITE((0x2c1c + j), 0x00000000);
-		RADEON_WRITE((0x2c20 + j), 0x00000000);
-		RADEON_WRITE((0x2c24 + j), 0x00000000);
-		j += 0x18;
-	}
-
-	RADEON_WRITE(R600_GRBM_CNTL, R600_GRBM_READ_TIMEOUT(0xff));
-
-	/* setup tiling, simd, pipe config */
-	mc_arb_ramcfg = RADEON_READ(R700_MC_ARB_RAMCFG);
-
-	switch (dev_priv->r600_max_tile_pipes) {
-	case 1:
-		gb_tiling_config |= R600_PIPE_TILING(0);
-		break;
-	case 2:
-		gb_tiling_config |= R600_PIPE_TILING(1);
-		break;
-	case 4:
-		gb_tiling_config |= R600_PIPE_TILING(2);
-		break;
-	case 8:
-		gb_tiling_config |= R600_PIPE_TILING(3);
-		break;
-	default:
-		break;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV770)
-		gb_tiling_config |= R600_BANK_TILING(1);
-	else
-		gb_tiling_config |= R600_BANK_TILING((mc_arb_ramcfg >> R700_NOOFBANK_SHIFT) & R700_NOOFBANK_MASK);
-
-	gb_tiling_config |= R600_GROUP_SIZE(0);
-
-	if (((mc_arb_ramcfg >> R700_NOOFROWS_SHIFT) & R700_NOOFROWS_MASK) > 3) {
-		gb_tiling_config |= R600_ROW_TILING(3);
-		gb_tiling_config |= R600_SAMPLE_SPLIT(3);
-	} else {
-		gb_tiling_config |=
-			R600_ROW_TILING(((mc_arb_ramcfg >> R700_NOOFROWS_SHIFT) & R700_NOOFROWS_MASK));
-		gb_tiling_config |=
-			R600_SAMPLE_SPLIT(((mc_arb_ramcfg >> R700_NOOFROWS_SHIFT) & R700_NOOFROWS_MASK));
-	}
-
-	gb_tiling_config |= R600_BANK_SWAPS(1);
-
-	cc_rb_backend_disable = RADEON_READ(R600_CC_RB_BACKEND_DISABLE) & 0x00ff0000;
-	cc_rb_backend_disable |=
-		R600_BACKEND_DISABLE((R7XX_MAX_BACKENDS_MASK << dev_priv->r600_max_backends) & R7XX_MAX_BACKENDS_MASK);
-
-	cc_gc_shader_pipe_config = RADEON_READ(R600_CC_GC_SHADER_PIPE_CONFIG) & 0xffffff00;
-	cc_gc_shader_pipe_config |=
-		R600_INACTIVE_QD_PIPES((R7XX_MAX_PIPES_MASK << dev_priv->r600_max_pipes) & R7XX_MAX_PIPES_MASK);
-	cc_gc_shader_pipe_config |=
-		R600_INACTIVE_SIMDS((R7XX_MAX_SIMDS_MASK << dev_priv->r600_max_simds) & R7XX_MAX_SIMDS_MASK);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV740)
-		backend_map = 0x28;
-	else
-		backend_map = r700_get_tile_pipe_to_backend_map(dev_priv,
-								dev_priv->r600_max_tile_pipes,
-								(R7XX_MAX_BACKENDS -
-								 r600_count_pipe_bits((cc_rb_backend_disable &
-										       R7XX_MAX_BACKENDS_MASK) >> 16)),
-								(cc_rb_backend_disable >> 16));
-	gb_tiling_config |= R600_BACKEND_MAP(backend_map);
-
-	RADEON_WRITE(R600_GB_TILING_CONFIG,      gb_tiling_config);
-	RADEON_WRITE(R600_DCP_TILING_CONFIG,    (gb_tiling_config & 0xffff));
-	RADEON_WRITE(R600_HDP_TILING_CONFIG,    (gb_tiling_config & 0xffff));
-	if (gb_tiling_config & 0xc0) {
-		dev_priv->r600_group_size = 512;
-	} else {
-		dev_priv->r600_group_size = 256;
-	}
-	dev_priv->r600_npipes = 1 << ((gb_tiling_config >> 1) & 0x7);
-	if (gb_tiling_config & 0x30) {
-		dev_priv->r600_nbanks = 8;
-	} else {
-		dev_priv->r600_nbanks = 4;
-	}
-
-	RADEON_WRITE(R600_CC_RB_BACKEND_DISABLE,      cc_rb_backend_disable);
-	RADEON_WRITE(R600_CC_GC_SHADER_PIPE_CONFIG,   cc_gc_shader_pipe_config);
-	RADEON_WRITE(R600_GC_USER_SHADER_PIPE_CONFIG, cc_gc_shader_pipe_config);
-
-	RADEON_WRITE(R700_CC_SYS_RB_BACKEND_DISABLE, cc_rb_backend_disable);
-	RADEON_WRITE(R700_CGTS_SYS_TCC_DISABLE, 0);
-	RADEON_WRITE(R700_CGTS_TCC_DISABLE, 0);
-	RADEON_WRITE(R700_CGTS_USER_SYS_TCC_DISABLE, 0);
-	RADEON_WRITE(R700_CGTS_USER_TCC_DISABLE, 0);
-
-	num_qd_pipes =
-		R7XX_MAX_PIPES - r600_count_pipe_bits((cc_gc_shader_pipe_config & R600_INACTIVE_QD_PIPES_MASK) >> 8);
-	RADEON_WRITE(R600_VGT_OUT_DEALLOC_CNTL, (num_qd_pipes * 4) & R600_DEALLOC_DIST_MASK);
-	RADEON_WRITE(R600_VGT_VERTEX_REUSE_BLOCK_CNTL, ((num_qd_pipes * 4) - 2) & R600_VTX_REUSE_DEPTH_MASK);
-
-	/* set HW defaults for 3D engine */
-	RADEON_WRITE(R600_CP_QUEUE_THRESHOLDS, (R600_ROQ_IB1_START(0x16) |
-						R600_ROQ_IB2_START(0x2b)));
-
-	RADEON_WRITE(R600_CP_MEQ_THRESHOLDS, R700_STQ_SPLIT(0x30));
-
-	ta_aux_cntl = RADEON_READ(R600_TA_CNTL_AUX);
-	RADEON_WRITE(R600_TA_CNTL_AUX, ta_aux_cntl | R600_DISABLE_CUBE_ANISO);
-
-	sx_debug_1 = RADEON_READ(R700_SX_DEBUG_1);
-	sx_debug_1 |= R700_ENABLE_NEW_SMX_ADDRESS;
-	RADEON_WRITE(R700_SX_DEBUG_1, sx_debug_1);
-
-	smx_dc_ctl0 = RADEON_READ(R600_SMX_DC_CTL0);
-	smx_dc_ctl0 &= ~R700_CACHE_DEPTH(0x1ff);
-	smx_dc_ctl0 |= R700_CACHE_DEPTH((dev_priv->r700_sx_num_of_sets * 64) - 1);
-	RADEON_WRITE(R600_SMX_DC_CTL0, smx_dc_ctl0);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) != CHIP_RV740)
-		RADEON_WRITE(R700_SMX_EVENT_CTL, (R700_ES_FLUSH_CTL(4) |
-						  R700_GS_FLUSH_CTL(4) |
-						  R700_ACK_FLUSH_CTL(3) |
-						  R700_SYNC_FLUSH_CTL));
-
-	db_debug3 = RADEON_READ(R700_DB_DEBUG3);
-	db_debug3 &= ~R700_DB_CLK_OFF_DELAY(0x1f);
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV770:
-	case CHIP_RV740:
-		db_debug3 |= R700_DB_CLK_OFF_DELAY(0x1f);
-		break;
-	case CHIP_RV710:
-	case CHIP_RV730:
-	default:
-		db_debug3 |= R700_DB_CLK_OFF_DELAY(2);
-		break;
-	}
-	RADEON_WRITE(R700_DB_DEBUG3, db_debug3);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) != CHIP_RV770) {
-		db_debug4 = RADEON_READ(RV700_DB_DEBUG4);
-		db_debug4 |= RV700_DISABLE_TILE_COVERED_FOR_PS_ITER;
-		RADEON_WRITE(RV700_DB_DEBUG4, db_debug4);
-	}
-
-	RADEON_WRITE(R600_SX_EXPORT_BUFFER_SIZES, (R600_COLOR_BUFFER_SIZE((dev_priv->r600_sx_max_export_size / 4) - 1) |
-						   R600_POSITION_BUFFER_SIZE((dev_priv->r600_sx_max_export_pos_size / 4) - 1) |
-						   R600_SMX_BUFFER_SIZE((dev_priv->r600_sx_max_export_smx_size / 4) - 1)));
-
-	RADEON_WRITE(R700_PA_SC_FIFO_SIZE_R7XX, (R700_SC_PRIM_FIFO_SIZE(dev_priv->r700_sc_prim_fifo_size) |
-						 R700_SC_HIZ_TILE_FIFO_SIZE(dev_priv->r700_sc_hiz_tile_fifo_size) |
-						 R700_SC_EARLYZ_TILE_FIFO_SIZE(dev_priv->r700_sc_earlyz_tile_fifo_fize)));
-
-	RADEON_WRITE(R600_PA_SC_MULTI_CHIP_CNTL, 0);
-
-	RADEON_WRITE(R600_VGT_NUM_INSTANCES, 1);
-
-	RADEON_WRITE(R600_SPI_CONFIG_CNTL, R600_GPR_WRITE_PRIORITY(0));
-
-	RADEON_WRITE(R600_SPI_CONFIG_CNTL_1, R600_VTX_DONE_DELAY(4));
-
-	RADEON_WRITE(R600_CP_PERFMON_CNTL, 0);
-
-	sq_ms_fifo_sizes = (R600_CACHE_FIFO_SIZE(16 * dev_priv->r600_sq_num_cf_insts) |
-			    R600_DONE_FIFO_HIWATER(0xe0) |
-			    R600_ALU_UPDATE_FIFO_HIWATER(0x8));
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV770:
-	case CHIP_RV730:
-	case CHIP_RV710:
-		sq_ms_fifo_sizes |= R600_FETCH_FIFO_HIWATER(0x1);
-		break;
-	case CHIP_RV740:
-	default:
-		sq_ms_fifo_sizes |= R600_FETCH_FIFO_HIWATER(0x4);
-		break;
-	}
-	RADEON_WRITE(R600_SQ_MS_FIFO_SIZES, sq_ms_fifo_sizes);
-
-	/* SQ_CONFIG, SQ_GPR_RESOURCE_MGMT, SQ_THREAD_RESOURCE_MGMT, SQ_STACK_RESOURCE_MGMT
-	 * should be adjusted as needed by the 2D/3D drivers.  This just sets default values
-	 */
-	sq_config = RADEON_READ(R600_SQ_CONFIG);
-	sq_config &= ~(R600_PS_PRIO(3) |
-		       R600_VS_PRIO(3) |
-		       R600_GS_PRIO(3) |
-		       R600_ES_PRIO(3));
-	sq_config |= (R600_DX9_CONSTS |
-		      R600_VC_ENABLE |
-		      R600_EXPORT_SRC_C |
-		      R600_PS_PRIO(0) |
-		      R600_VS_PRIO(1) |
-		      R600_GS_PRIO(2) |
-		      R600_ES_PRIO(3));
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV710)
-		/* no vertex cache */
-		sq_config &= ~R600_VC_ENABLE;
-
-	RADEON_WRITE(R600_SQ_CONFIG, sq_config);
-
-	RADEON_WRITE(R600_SQ_GPR_RESOURCE_MGMT_1,  (R600_NUM_PS_GPRS((dev_priv->r600_max_gprs * 24)/64) |
-						    R600_NUM_VS_GPRS((dev_priv->r600_max_gprs * 24)/64) |
-						    R600_NUM_CLAUSE_TEMP_GPRS(((dev_priv->r600_max_gprs * 24)/64)/2)));
-
-	RADEON_WRITE(R600_SQ_GPR_RESOURCE_MGMT_2,  (R600_NUM_GS_GPRS((dev_priv->r600_max_gprs * 7)/64) |
-						    R600_NUM_ES_GPRS((dev_priv->r600_max_gprs * 7)/64)));
-
-	sq_thread_resource_mgmt = (R600_NUM_PS_THREADS((dev_priv->r600_max_threads * 4)/8) |
-				   R600_NUM_VS_THREADS((dev_priv->r600_max_threads * 2)/8) |
-				   R600_NUM_ES_THREADS((dev_priv->r600_max_threads * 1)/8));
-	if (((dev_priv->r600_max_threads * 1) / 8) > dev_priv->r600_max_gs_threads)
-		sq_thread_resource_mgmt |= R600_NUM_GS_THREADS(dev_priv->r600_max_gs_threads);
-	else
-		sq_thread_resource_mgmt |= R600_NUM_GS_THREADS((dev_priv->r600_max_gs_threads * 1)/8);
-	RADEON_WRITE(R600_SQ_THREAD_RESOURCE_MGMT, sq_thread_resource_mgmt);
-
-	RADEON_WRITE(R600_SQ_STACK_RESOURCE_MGMT_1, (R600_NUM_PS_STACK_ENTRIES((dev_priv->r600_max_stack_entries * 1)/4) |
-						     R600_NUM_VS_STACK_ENTRIES((dev_priv->r600_max_stack_entries * 1)/4)));
-
-	RADEON_WRITE(R600_SQ_STACK_RESOURCE_MGMT_2, (R600_NUM_GS_STACK_ENTRIES((dev_priv->r600_max_stack_entries * 1)/4) |
-						     R600_NUM_ES_STACK_ENTRIES((dev_priv->r600_max_stack_entries * 1)/4)));
-
-	sq_dyn_gpr_size_simd_ab_0 = (R700_SIMDA_RING0((dev_priv->r600_max_gprs * 38)/64) |
-				     R700_SIMDA_RING1((dev_priv->r600_max_gprs * 38)/64) |
-				     R700_SIMDB_RING0((dev_priv->r600_max_gprs * 38)/64) |
-				     R700_SIMDB_RING1((dev_priv->r600_max_gprs * 38)/64));
-
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_0, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_1, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_2, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_3, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_4, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_5, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_6, sq_dyn_gpr_size_simd_ab_0);
-	RADEON_WRITE(R700_SQ_DYN_GPR_SIZE_SIMD_AB_7, sq_dyn_gpr_size_simd_ab_0);
-
-	RADEON_WRITE(R700_PA_SC_FORCE_EOV_MAX_CNTS, (R700_FORCE_EOV_MAX_CLK_CNT(4095) |
-						     R700_FORCE_EOV_MAX_REZ_CNT(255)));
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV710)
-		RADEON_WRITE(R600_VGT_CACHE_INVALIDATION, (R600_CACHE_INVALIDATION(R600_TC_ONLY) |
-							   R700_AUTO_INVLD_EN(R700_ES_AND_GS_AUTO)));
-	else
-		RADEON_WRITE(R600_VGT_CACHE_INVALIDATION, (R600_CACHE_INVALIDATION(R600_VC_AND_TC) |
-							   R700_AUTO_INVLD_EN(R700_ES_AND_GS_AUTO)));
-
-	switch (dev_priv->flags & RADEON_FAMILY_MASK) {
-	case CHIP_RV770:
-	case CHIP_RV730:
-	case CHIP_RV740:
-		gs_prim_buffer_depth = 384;
-		break;
-	case CHIP_RV710:
-		gs_prim_buffer_depth = 128;
-		break;
-	default:
-		break;
-	}
-
-	num_gs_verts_per_thread = dev_priv->r600_max_pipes * 16;
-	vgt_gs_per_es = gs_prim_buffer_depth + num_gs_verts_per_thread;
-	/* Max value for this is 256 */
-	if (vgt_gs_per_es > 256)
-		vgt_gs_per_es = 256;
-
-	RADEON_WRITE(R600_VGT_ES_PER_GS, 128);
-	RADEON_WRITE(R600_VGT_GS_PER_ES, vgt_gs_per_es);
-	RADEON_WRITE(R600_VGT_GS_PER_VS, 2);
-
-	/* more default values. 2D/3D driver should adjust as needed */
-	RADEON_WRITE(R600_VGT_GS_VERTEX_REUSE, 16);
-	RADEON_WRITE(R600_PA_SC_LINE_STIPPLE_STATE, 0);
-	RADEON_WRITE(R600_VGT_STRMOUT_EN, 0);
-	RADEON_WRITE(R600_SX_MISC, 0);
-	RADEON_WRITE(R600_PA_SC_MODE_CNTL, 0);
-	RADEON_WRITE(R700_PA_SC_EDGERULE, 0xaaaaaaaa);
-	RADEON_WRITE(R600_PA_SC_AA_CONFIG, 0);
-	RADEON_WRITE(R600_PA_SC_CLIPRECT_RULE, 0xffff);
-	RADEON_WRITE(R600_PA_SC_LINE_STIPPLE, 0);
-	RADEON_WRITE(R600_SPI_INPUT_Z, 0);
-	RADEON_WRITE(R600_SPI_PS_IN_CONTROL_0, R600_NUM_INTERP(2));
-	RADEON_WRITE(R600_CB_COLOR7_FRAG, 0);
-
-	/* clear render buffer base addresses */
-	RADEON_WRITE(R600_CB_COLOR0_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR1_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR2_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR3_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR4_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR5_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR6_BASE, 0);
-	RADEON_WRITE(R600_CB_COLOR7_BASE, 0);
-
-	RADEON_WRITE(R700_TCP_CNTL, 0);
-
-	hdp_host_path_cntl = RADEON_READ(R600_HDP_HOST_PATH_CNTL);
-	RADEON_WRITE(R600_HDP_HOST_PATH_CNTL, hdp_host_path_cntl);
-
-	RADEON_WRITE(R600_PA_SC_MULTI_CHIP_CNTL, 0);
-
-	RADEON_WRITE(R600_PA_CL_ENHANCE, (R600_CLIP_VTX_REORDER_ENA |
-					  R600_NUM_CLIP_SEQ(3)));
-
-}
-
-static void r600_cp_init_ring_buffer(struct drm_device *dev,
-				       drm_radeon_private_t *dev_priv,
-				       struct drm_file *file_priv)
-{
-	struct drm_radeon_master_private *master_priv;
-	u32 ring_start;
-	u64 rptr_addr;
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770))
-		r700_gfx_init(dev, dev_priv);
-	else
-		r600_gfx_init(dev, dev_priv);
-
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, R600_SOFT_RESET_CP);
-	RADEON_READ(R600_GRBM_SOFT_RESET);
-	mdelay(15);
-	RADEON_WRITE(R600_GRBM_SOFT_RESET, 0);
-
-
-	/* Set ring buffer size */
-#ifdef __BIG_ENDIAN
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     R600_BUF_SWAP_32BIT |
-		     R600_RB_NO_UPDATE |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#else
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     RADEON_RB_NO_UPDATE |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#endif
-
-	RADEON_WRITE(R600_CP_SEM_WAIT_TIMER, 0x0);
-
-	/* Set the write pointer delay */
-	RADEON_WRITE(R600_CP_RB_WPTR_DELAY, 0);
-
-#ifdef __BIG_ENDIAN
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     R600_BUF_SWAP_32BIT |
-		     R600_RB_NO_UPDATE |
-		     R600_RB_RPTR_WR_ENA |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#else
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     R600_RB_NO_UPDATE |
-		     R600_RB_RPTR_WR_ENA |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#endif
-
-	/* Initialize the ring buffer's read and write pointers */
-	RADEON_WRITE(R600_CP_RB_RPTR_WR, 0);
-	RADEON_WRITE(R600_CP_RB_WPTR, 0);
-	SET_RING_HEAD(dev_priv, 0);
-	dev_priv->ring.tail = 0;
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		rptr_addr = dev_priv->ring_rptr->offset
-			- dev->agp->base +
-			dev_priv->gart_vm_start;
-	} else
-#endif
-	{
-		rptr_addr = dev_priv->ring_rptr->offset
-			- ((unsigned long) dev->sg->virtual)
-			+ dev_priv->gart_vm_start;
-	}
-	RADEON_WRITE(R600_CP_RB_RPTR_ADDR, (rptr_addr & 0xfffffffc));
-	RADEON_WRITE(R600_CP_RB_RPTR_ADDR_HI, upper_32_bits(rptr_addr));
-
-#ifdef __BIG_ENDIAN
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     RADEON_BUF_SWAP_32BIT |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#else
-	RADEON_WRITE(R600_CP_RB_CNTL,
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#endif
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		/* XXX */
-		radeon_write_agp_base(dev_priv, dev->agp->base);
-
-		/* XXX */
-		radeon_write_agp_location(dev_priv,
-			     (((dev_priv->gart_vm_start - 1 +
-				dev_priv->gart_size) & 0xffff0000) |
-			      (dev_priv->gart_vm_start >> 16)));
-
-		ring_start = (dev_priv->cp_ring->offset
-			      - dev->agp->base
-			      + dev_priv->gart_vm_start);
-	} else
-#endif
-		ring_start = (dev_priv->cp_ring->offset
-			      - (unsigned long)dev->sg->virtual
-			      + dev_priv->gart_vm_start);
-
-	RADEON_WRITE(R600_CP_RB_BASE, ring_start >> 8);
-
-	RADEON_WRITE(R600_CP_ME_CNTL, 0xff);
-
-	RADEON_WRITE(R600_CP_DEBUG, (1 << 27) | (1 << 28));
-
-	/* Initialize the scratch register pointer.  This will cause
-	 * the scratch register values to be written out to memory
-	 * whenever they are updated.
-	 *
-	 * We simply put this behind the ring read pointer, this works
-	 * with PCI GART as well as (whatever kind of) AGP GART
-	 */
-	{
-		u64 scratch_addr;
-
-		scratch_addr = RADEON_READ(R600_CP_RB_RPTR_ADDR) & 0xFFFFFFFC;
-		scratch_addr |= ((u64)RADEON_READ(R600_CP_RB_RPTR_ADDR_HI)) << 32;
-		scratch_addr += R600_SCRATCH_REG_OFFSET;
-		scratch_addr >>= 8;
-		scratch_addr &= 0xffffffff;
-
-		RADEON_WRITE(R600_SCRATCH_ADDR, (uint32_t)scratch_addr);
-	}
-
-	RADEON_WRITE(R600_SCRATCH_UMSK, 0x7);
-
-	/* Turn on bus mastering */
-	radeon_enable_bm(dev_priv);
-
-	radeon_write_ring_rptr(dev_priv, R600_SCRATCHOFF(0), 0);
-	RADEON_WRITE(R600_LAST_FRAME_REG, 0);
-
-	radeon_write_ring_rptr(dev_priv, R600_SCRATCHOFF(1), 0);
-	RADEON_WRITE(R600_LAST_DISPATCH_REG, 0);
-
-	radeon_write_ring_rptr(dev_priv, R600_SCRATCHOFF(2), 0);
-	RADEON_WRITE(R600_LAST_CLEAR_REG, 0);
-
-	/* reset sarea copies of these */
-	master_priv = file_priv->master->driver_priv;
-	if (master_priv->sarea_priv) {
-		master_priv->sarea_priv->last_frame = 0;
-		master_priv->sarea_priv->last_dispatch = 0;
-		master_priv->sarea_priv->last_clear = 0;
-	}
-
-	r600_do_wait_for_idle(dev_priv);
-
-}
-
-int r600_do_cleanup_cp(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	/* Make sure interrupts are disabled here because the uninstall ioctl
-	 * may not have been called from userspace and after dev_private
-	 * is freed, it's too late.
-	 */
-	if (dev->irq_enabled)
-		drm_irq_uninstall(dev);
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		if (dev_priv->cp_ring != NULL) {
-			drm_legacy_ioremapfree(dev_priv->cp_ring, dev);
-			dev_priv->cp_ring = NULL;
-		}
-		if (dev_priv->ring_rptr != NULL) {
-			drm_legacy_ioremapfree(dev_priv->ring_rptr, dev);
-			dev_priv->ring_rptr = NULL;
-		}
-		if (dev->agp_buffer_map != NULL) {
-			drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
-			dev->agp_buffer_map = NULL;
-		}
-	} else
-#endif
-	{
-
-		if (dev_priv->gart_info.bus_addr)
-			r600_page_table_cleanup(dev, &dev_priv->gart_info);
-
-		if (dev_priv->gart_info.gart_table_location == DRM_ATI_GART_FB) {
-			drm_legacy_ioremapfree(&dev_priv->gart_info.mapping, dev);
-			dev_priv->gart_info.addr = NULL;
-		}
-	}
-	/* only clear to the start of flags */
-	memset(dev_priv, 0, offsetof(drm_radeon_private_t, flags));
-
-	return 0;
-}
-
-int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
-		    struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-
-	DRM_DEBUG("\n");
-
-	mutex_init(&dev_priv->cs_mutex);
-	r600_cs_legacy_init();
-	/* if we require new memory map but we don't have it fail */
-	if ((dev_priv->flags & RADEON_NEW_MEMMAP) && !dev_priv->new_memmap) {
-		DRM_ERROR("Cannot initialise DRM on this card\nThis card requires a new X.org DDX for 3D\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	if (init->is_pci && (dev_priv->flags & RADEON_IS_AGP)) {
-		DRM_DEBUG("Forcing AGP card to PCI mode\n");
-		dev_priv->flags &= ~RADEON_IS_AGP;
-		/* The writeback test succeeds, but when writeback is enabled,
-		 * the ring buffer read ptr update fails after first 128 bytes.
-		 */
-		radeon_no_wb = 1;
-	} else if (!(dev_priv->flags & (RADEON_IS_AGP | RADEON_IS_PCI | RADEON_IS_PCIE))
-		 && !init->is_pci) {
-		DRM_DEBUG("Restoring AGP flag\n");
-		dev_priv->flags |= RADEON_IS_AGP;
-	}
-
-	dev_priv->usec_timeout = init->usec_timeout;
-	if (dev_priv->usec_timeout < 1 ||
-	    dev_priv->usec_timeout > RADEON_MAX_USEC_TIMEOUT) {
-		DRM_DEBUG("TIMEOUT problem!\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	/* Enable vblank on CRTC1 for older X servers
-	 */
-	dev_priv->vblank_crtc = DRM_RADEON_VBLANK_CRTC1;
-	dev_priv->do_boxes = 0;
-	dev_priv->cp_mode = init->cp_mode;
-
-	/* We don't support anything other than bus-mastering ring mode,
-	 * but the ring can be in either AGP or PCI space for the ring
-	 * read pointer.
-	 */
-	if ((init->cp_mode != RADEON_CSQ_PRIBM_INDDIS) &&
-	    (init->cp_mode != RADEON_CSQ_PRIBM_INDBM)) {
-		DRM_DEBUG("BAD cp_mode (%x)!\n", init->cp_mode);
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	switch (init->fb_bpp) {
-	case 16:
-		dev_priv->color_fmt = RADEON_COLOR_FORMAT_RGB565;
-		break;
-	case 32:
-	default:
-		dev_priv->color_fmt = RADEON_COLOR_FORMAT_ARGB8888;
-		break;
-	}
-	dev_priv->front_offset = init->front_offset;
-	dev_priv->front_pitch = init->front_pitch;
-	dev_priv->back_offset = init->back_offset;
-	dev_priv->back_pitch = init->back_pitch;
-
-	dev_priv->ring_offset = init->ring_offset;
-	dev_priv->ring_rptr_offset = init->ring_rptr_offset;
-	dev_priv->buffers_offset = init->buffers_offset;
-	dev_priv->gart_textures_offset = init->gart_textures_offset;
-
-	master_priv->sarea = drm_legacy_getsarea(dev);
-	if (!master_priv->sarea) {
-		DRM_ERROR("could not find sarea!\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	dev_priv->cp_ring = drm_legacy_findmap(dev, init->ring_offset);
-	if (!dev_priv->cp_ring) {
-		DRM_ERROR("could not find cp ring region!\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-	dev_priv->ring_rptr = drm_legacy_findmap(dev, init->ring_rptr_offset);
-	if (!dev_priv->ring_rptr) {
-		DRM_ERROR("could not find ring read pointer!\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
-	if (!dev->agp_buffer_map) {
-		DRM_ERROR("could not find dma buffer region!\n");
-		r600_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	if (init->gart_textures_offset) {
-		dev_priv->gart_textures =
-		    drm_legacy_findmap(dev, init->gart_textures_offset);
-		if (!dev_priv->gart_textures) {
-			DRM_ERROR("could not find GART texture region!\n");
-			r600_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-	}
-
-#if IS_ENABLED(CONFIG_AGP)
-	/* XXX */
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		drm_legacy_ioremap_wc(dev_priv->cp_ring, dev);
-		drm_legacy_ioremap_wc(dev_priv->ring_rptr, dev);
-		drm_legacy_ioremap_wc(dev->agp_buffer_map, dev);
-		if (!dev_priv->cp_ring->handle ||
-		    !dev_priv->ring_rptr->handle ||
-		    !dev->agp_buffer_map->handle) {
-			DRM_ERROR("could not find ioremap agp regions!\n");
-			r600_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-	} else
-#endif
-	{
-		dev_priv->cp_ring->handle = (void *)(unsigned long)dev_priv->cp_ring->offset;
-		dev_priv->ring_rptr->handle =
-			(void *)(unsigned long)dev_priv->ring_rptr->offset;
-		dev->agp_buffer_map->handle =
-			(void *)(unsigned long)dev->agp_buffer_map->offset;
-
-		DRM_DEBUG("dev_priv->cp_ring->handle %p\n",
-			  dev_priv->cp_ring->handle);
-		DRM_DEBUG("dev_priv->ring_rptr->handle %p\n",
-			  dev_priv->ring_rptr->handle);
-		DRM_DEBUG("dev->agp_buffer_map->handle %p\n",
-			  dev->agp_buffer_map->handle);
-	}
-
-	dev_priv->fb_location = (radeon_read_fb_location(dev_priv) & 0xffff) << 24;
-	dev_priv->fb_size =
-		(((radeon_read_fb_location(dev_priv) & 0xffff0000u) << 8) + 0x1000000)
-		- dev_priv->fb_location;
-
-	dev_priv->front_pitch_offset = (((dev_priv->front_pitch / 64) << 22) |
-					((dev_priv->front_offset
-					  + dev_priv->fb_location) >> 10));
-
-	dev_priv->back_pitch_offset = (((dev_priv->back_pitch / 64) << 22) |
-				       ((dev_priv->back_offset
-					 + dev_priv->fb_location) >> 10));
-
-	dev_priv->depth_pitch_offset = (((dev_priv->depth_pitch / 64) << 22) |
-					((dev_priv->depth_offset
-					  + dev_priv->fb_location) >> 10));
-
-	dev_priv->gart_size = init->gart_size;
-
-	/* New let's set the memory map ... */
-	if (dev_priv->new_memmap) {
-		u32 base = 0;
-
-		DRM_INFO("Setting GART location based on new memory map\n");
-
-		/* If using AGP, try to locate the AGP aperture at the same
-		 * location in the card and on the bus, though we have to
-		 * align it down.
-		 */
-#if IS_ENABLED(CONFIG_AGP)
-		/* XXX */
-		if (dev_priv->flags & RADEON_IS_AGP) {
-			base = dev->agp->base;
-			/* Check if valid */
-			if ((base + dev_priv->gart_size - 1) >= dev_priv->fb_location &&
-			    base < (dev_priv->fb_location + dev_priv->fb_size - 1)) {
-				DRM_INFO("Can't use AGP base @0x%08lx, won't fit\n",
-					 dev->agp->base);
-				base = 0;
-			}
-		}
-#endif
-		/* If not or if AGP is at 0 (Macs), try to put it elsewhere */
-		if (base == 0) {
-			base = dev_priv->fb_location + dev_priv->fb_size;
-			if (base < dev_priv->fb_location ||
-			    ((base + dev_priv->gart_size) & 0xfffffffful) < base)
-				base = dev_priv->fb_location
-					- dev_priv->gart_size;
-		}
-		dev_priv->gart_vm_start = base & 0xffc00000u;
-		if (dev_priv->gart_vm_start != base)
-			DRM_INFO("GART aligned down from 0x%08x to 0x%08x\n",
-				 base, dev_priv->gart_vm_start);
-	}
-
-#if IS_ENABLED(CONFIG_AGP)
-	/* XXX */
-	if (dev_priv->flags & RADEON_IS_AGP)
-		dev_priv->gart_buffers_offset = (dev->agp_buffer_map->offset
-						 - dev->agp->base
-						 + dev_priv->gart_vm_start);
-	else
-#endif
-		dev_priv->gart_buffers_offset = (dev->agp_buffer_map->offset
-						 - (unsigned long)dev->sg->virtual
-						 + dev_priv->gart_vm_start);
-
-	DRM_DEBUG("fb 0x%08x size %d\n",
-		  (unsigned int) dev_priv->fb_location,
-		  (unsigned int) dev_priv->fb_size);
-	DRM_DEBUG("dev_priv->gart_size %d\n", dev_priv->gart_size);
-	DRM_DEBUG("dev_priv->gart_vm_start 0x%08x\n",
-		  (unsigned int) dev_priv->gart_vm_start);
-	DRM_DEBUG("dev_priv->gart_buffers_offset 0x%08lx\n",
-		  dev_priv->gart_buffers_offset);
-
-	dev_priv->ring.start = (u32 *) dev_priv->cp_ring->handle;
-	dev_priv->ring.end = ((u32 *) dev_priv->cp_ring->handle
-			      + init->ring_size / sizeof(u32));
-	dev_priv->ring.size = init->ring_size;
-	dev_priv->ring.size_l2qw = order_base_2(init->ring_size / 8);
-
-	dev_priv->ring.rptr_update = /* init->rptr_update */ 4096;
-	dev_priv->ring.rptr_update_l2qw = order_base_2(/* init->rptr_update */ 4096 / 8);
-
-	dev_priv->ring.fetch_size = /* init->fetch_size */ 32;
-	dev_priv->ring.fetch_size_l2ow = order_base_2(/* init->fetch_size */ 32 / 16);
-
-	dev_priv->ring.tail_mask = (dev_priv->ring.size / sizeof(u32)) - 1;
-
-	dev_priv->ring.high_mark = RADEON_RING_HIGH_MARK;
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		/* XXX turn off pcie gart */
-	} else
-#endif
-	{
-		dev_priv->gart_info.table_mask = DMA_BIT_MASK(32);
-		/* if we have an offset set from userspace */
-		if (!dev_priv->pcigart_offset_set) {
-			DRM_ERROR("Need gart offset from userspace\n");
-			r600_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-
-		DRM_DEBUG("Using gart offset 0x%08lx\n", dev_priv->pcigart_offset);
-
-		dev_priv->gart_info.bus_addr =
-			dev_priv->pcigart_offset + dev_priv->fb_location;
-		dev_priv->gart_info.mapping.offset =
-			dev_priv->pcigart_offset + dev_priv->fb_aper_offset;
-		dev_priv->gart_info.mapping.size =
-			dev_priv->gart_info.table_size;
-
-		drm_legacy_ioremap_wc(&dev_priv->gart_info.mapping, dev);
-		if (!dev_priv->gart_info.mapping.handle) {
-			DRM_ERROR("ioremap failed.\n");
-			r600_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-
-		dev_priv->gart_info.addr =
-			dev_priv->gart_info.mapping.handle;
-
-		DRM_DEBUG("Setting phys_pci_gart to %p %08lX\n",
-			  dev_priv->gart_info.addr,
-			  dev_priv->pcigart_offset);
-
-		if (!r600_page_table_init(dev)) {
-			DRM_ERROR("Failed to init GART table\n");
-			r600_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-
-		if (((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770))
-			r700_vm_init(dev);
-		else
-			r600_vm_init(dev);
-	}
-
-	if (!dev_priv->me_fw || !dev_priv->pfp_fw) {
-		int err = r600_cp_init_microcode(dev_priv);
-		if (err) {
-			DRM_ERROR("Failed to load firmware!\n");
-			r600_do_cleanup_cp(dev);
-			return err;
-		}
-	}
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770))
-		r700_cp_load_microcode(dev_priv);
-	else
-		r600_cp_load_microcode(dev_priv);
-
-	r600_cp_init_ring_buffer(dev, dev_priv, file_priv);
-
-	dev_priv->last_buf = 0;
-
-	r600_do_engine_reset(dev);
-	r600_test_writeback(dev_priv);
-
-	return 0;
-}
-
-int r600_do_resume_cp(struct drm_device *dev, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	DRM_DEBUG("\n");
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)) {
-		r700_vm_init(dev);
-		r700_cp_load_microcode(dev_priv);
-	} else {
-		r600_vm_init(dev);
-		r600_cp_load_microcode(dev_priv);
-	}
-	r600_cp_init_ring_buffer(dev, dev_priv, file_priv);
-	r600_do_engine_reset(dev);
-
-	return 0;
-}
-
-/* Wait for the CP to go idle.
- */
-int r600_do_cp_idle(drm_radeon_private_t *dev_priv)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(5);
-	OUT_RING(CP_PACKET3(R600_IT_EVENT_WRITE, 0));
-	OUT_RING(R600_CACHE_FLUSH_AND_INV_EVENT);
-	/* wait for 3D idle clean */
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));
-	OUT_RING((R600_WAIT_UNTIL - R600_SET_CONFIG_REG_OFFSET) >> 2);
-	OUT_RING(RADEON_WAIT_3D_IDLE | RADEON_WAIT_3D_IDLECLEAN);
-
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	return r600_do_wait_for_idle(dev_priv);
-}
-
-/* Start the Command Processor.
- */
-void r600_do_cp_start(drm_radeon_private_t *dev_priv)
-{
-	u32 cp_me;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(7);
-	OUT_RING(CP_PACKET3(R600_IT_ME_INITIALIZE, 5));
-	OUT_RING(0x00000001);
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) < CHIP_RV770))
-		OUT_RING(0x00000003);
-	else
-		OUT_RING(0x00000000);
-	OUT_RING((dev_priv->r600_max_hw_contexts - 1));
-	OUT_RING(R600_ME_INITIALIZE_DEVICE_ID(1));
-	OUT_RING(0x00000000);
-	OUT_RING(0x00000000);
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	/* set the mux and reset the halt bit */
-	cp_me = 0xff;
-	RADEON_WRITE(R600_CP_ME_CNTL, cp_me);
-
-	dev_priv->cp_running = 1;
-
-}
-
-void r600_do_cp_reset(drm_radeon_private_t *dev_priv)
-{
-	u32 cur_read_ptr;
-	DRM_DEBUG("\n");
-
-	cur_read_ptr = RADEON_READ(R600_CP_RB_RPTR);
-	RADEON_WRITE(R600_CP_RB_WPTR, cur_read_ptr);
-	SET_RING_HEAD(dev_priv, cur_read_ptr);
-	dev_priv->ring.tail = cur_read_ptr;
-}
-
-void r600_do_cp_stop(drm_radeon_private_t *dev_priv)
-{
-	uint32_t cp_me;
-
-	DRM_DEBUG("\n");
-
-	cp_me = 0xff | R600_CP_ME_HALT;
-
-	RADEON_WRITE(R600_CP_ME_CNTL, cp_me);
-
-	dev_priv->cp_running = 0;
-}
-
-int r600_cp_dispatch_indirect(struct drm_device *dev,
-			      struct drm_buf *buf, int start, int end)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	if (start != end) {
-		unsigned long offset = (dev_priv->gart_buffers_offset
-					+ buf->offset + start);
-		int dwords = (end - start + 3) / sizeof(u32);
-
-		DRM_DEBUG("dwords:%d\n", dwords);
-		DRM_DEBUG("offset 0x%lx\n", offset);
-
-
-		/* Indirect buffer data must be a multiple of 16 dwords.
-		 * pad the data with a Type-2 CP packet.
-		 */
-		while (dwords & 0xf) {
-			u32 *data = (u32 *)
-			    ((char *)dev->agp_buffer_map->handle
-			     + buf->offset + start);
-			data[dwords++] = RADEON_CP_PACKET2;
-		}
-
-		/* Fire off the indirect buffer */
-		BEGIN_RING(4);
-		OUT_RING(CP_PACKET3(R600_IT_INDIRECT_BUFFER, 2));
-		OUT_RING((offset & 0xfffffffc));
-		OUT_RING((upper_32_bits(offset) & 0xff));
-		OUT_RING(dwords);
-		ADVANCE_RING();
-	}
-
-	return 0;
-}
-
-void r600_cp_dispatch_swap(struct drm_device *dev, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_master *master = file_priv->master;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	int nbox = sarea_priv->nbox;
-	struct drm_clip_rect *pbox = sarea_priv->boxes;
-	int i, cpp, src_pitch, dst_pitch;
-	uint64_t src, dst;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	if (dev_priv->color_fmt == RADEON_COLOR_FORMAT_ARGB8888)
-		cpp = 4;
-	else
-		cpp = 2;
-
-	if (sarea_priv->pfCurrentPage == 0) {
-		src_pitch = dev_priv->back_pitch;
-		dst_pitch = dev_priv->front_pitch;
-		src = dev_priv->back_offset + dev_priv->fb_location;
-		dst = dev_priv->front_offset + dev_priv->fb_location;
-	} else {
-		src_pitch = dev_priv->front_pitch;
-		dst_pitch = dev_priv->back_pitch;
-		src = dev_priv->front_offset + dev_priv->fb_location;
-		dst = dev_priv->back_offset + dev_priv->fb_location;
-	}
-
-	if (r600_prepare_blit_copy(dev, file_priv)) {
-		DRM_ERROR("unable to allocate vertex buffer for swap buffer\n");
-		return;
-	}
-	for (i = 0; i < nbox; i++) {
-		int x = pbox[i].x1;
-		int y = pbox[i].y1;
-		int w = pbox[i].x2 - x;
-		int h = pbox[i].y2 - y;
-
-		DRM_DEBUG("%d,%d-%d,%d\n", x, y, w, h);
-
-		r600_blit_swap(dev,
-			       src, dst,
-			       x, y, x, y, w, h,
-			       src_pitch, dst_pitch, cpp);
-	}
-	r600_done_blit_copy(dev);
-
-	/* Increment the frame counter.  The client-side 3D driver must
-	 * throttle the framerate by waiting for this value before
-	 * performing the swapbuffer ioctl.
-	 */
-	sarea_priv->last_frame++;
-
-	BEGIN_RING(3);
-	R600_FRAME_AGE(sarea_priv->last_frame);
-	ADVANCE_RING();
-}
-
-int r600_cp_dispatch_texture(struct drm_device *dev,
-			     struct drm_file *file_priv,
-			     drm_radeon_texture_t *tex,
-			     drm_radeon_tex_image_t *image)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_buf *buf;
-	u32 *buffer;
-	const u8 __user *data;
-	unsigned int size, pass_size;
-	u64 src_offset, dst_offset;
-
-	if (!radeon_check_offset(dev_priv, tex->offset)) {
-		DRM_ERROR("Invalid destination offset\n");
-		return -EINVAL;
-	}
-
-	/* this might fail for zero-sized uploads - are those illegal? */
-	if (!radeon_check_offset(dev_priv, tex->offset + tex->height * tex->pitch - 1)) {
-		DRM_ERROR("Invalid final destination offset\n");
-		return -EINVAL;
-	}
-
-	size = tex->height * tex->pitch;
-
-	if (size == 0)
-		return 0;
-
-	dst_offset = tex->offset;
-
-	if (r600_prepare_blit_copy(dev, file_priv)) {
-		DRM_ERROR("unable to allocate vertex buffer for swap buffer\n");
-		return -EAGAIN;
-	}
-	do {
-		data = (const u8 __user *)image->data;
-		pass_size = size;
-
-		buf = radeon_freelist_get(dev);
-		if (!buf) {
-			DRM_DEBUG("EAGAIN\n");
-			if (copy_to_user(tex->image, image, sizeof(*image)))
-				return -EFAULT;
-			return -EAGAIN;
-		}
-
-		if (pass_size > buf->total)
-			pass_size = buf->total;
-
-		/* Dispatch the indirect buffer.
-		 */
-		buffer =
-		    (u32 *) ((char *)dev->agp_buffer_map->handle + buf->offset);
-
-		if (copy_from_user(buffer, data, pass_size)) {
-			DRM_ERROR("EFAULT on pad, %d bytes\n", pass_size);
-			return -EFAULT;
-		}
-
-		buf->file_priv = file_priv;
-		buf->used = pass_size;
-		src_offset = dev_priv->gart_buffers_offset + buf->offset;
-
-		r600_blit_copy(dev, src_offset, dst_offset, pass_size);
-
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-
-		/* Update the input parameters for next time */
-		image->data = (const u8 __user *)image->data + pass_size;
-		dst_offset += pass_size;
-		size -= pass_size;
-	} while (size > 0);
-	r600_done_blit_copy(dev);
-
-	return 0;
-}
-
-/*
- * Legacy cs ioctl
- */
-static u32 radeon_cs_id_get(struct drm_radeon_private *radeon)
-{
-	/* FIXME: check if wrap affect last reported wrap & sequence */
-	radeon->cs_id_scnt = (radeon->cs_id_scnt + 1) & 0x00FFFFFF;
-	if (!radeon->cs_id_scnt) {
-		/* increment wrap counter */
-		radeon->cs_id_wcnt += 0x01000000;
-		/* valid sequence counter start at 1 */
-		radeon->cs_id_scnt = 1;
-	}
-	return (radeon->cs_id_scnt | radeon->cs_id_wcnt);
-}
-
-static void r600_cs_id_emit(drm_radeon_private_t *dev_priv, u32 *id)
-{
-	RING_LOCALS;
-
-	*id = radeon_cs_id_get(dev_priv);
-
-	/* SCRATCH 2 */
-	BEGIN_RING(3);
-	R600_CLEAR_AGE(*id);
-	ADVANCE_RING();
-	COMMIT_RING();
-}
-
-static int r600_ib_get(struct drm_device *dev,
-			struct drm_file *fpriv,
-			struct drm_buf **buffer)
-{
-	struct drm_buf *buf;
-
-	*buffer = NULL;
-	buf = radeon_freelist_get(dev);
-	if (!buf) {
-		return -EBUSY;
-	}
-	buf->file_priv = fpriv;
-	*buffer = buf;
-	return 0;
-}
-
-static void r600_ib_free(struct drm_device *dev, struct drm_buf *buf,
-			struct drm_file *fpriv, int l, int r)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if (buf) {
-		if (!r)
-			r600_cp_dispatch_indirect(dev, buf, 0, l * 4);
-		radeon_cp_discard_buffer(dev, fpriv->master, buf);
-		COMMIT_RING();
-	}
-}
-
-int r600_cs_legacy_ioctl(struct drm_device *dev, void *data, struct drm_file *fpriv)
-{
-	struct drm_radeon_private *dev_priv = dev->dev_private;
-	struct drm_radeon_cs *cs = data;
-	struct drm_buf *buf;
-	unsigned family;
-	int l, r = 0;
-	u32 *ib, cs_id = 0;
-
-	if (dev_priv == NULL) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-	family = dev_priv->flags & RADEON_FAMILY_MASK;
-	if (family < CHIP_R600) {
-		DRM_ERROR("cs ioctl valid only for R6XX & R7XX in legacy mode\n");
-		return -EINVAL;
-	}
-	mutex_lock(&dev_priv->cs_mutex);
-	/* get ib */
-	r = r600_ib_get(dev, fpriv, &buf);
-	if (r) {
-		DRM_ERROR("ib_get failed\n");
-		goto out;
-	}
-	ib = dev->agp_buffer_map->handle + buf->offset;
-	/* now parse command stream */
-	r = r600_cs_legacy(dev, data,  fpriv, family, ib, &l);
-	if (r) {
-		goto out;
-	}
-
-out:
-	r600_ib_free(dev, buf, fpriv, l, r);
-	/* emit cs id sequence */
-	r600_cs_id_emit(dev_priv, &cs_id);
-	cs->cs_id = cs_id;
-	mutex_unlock(&dev_priv->cs_mutex);
-	return r;
-}
-
-void r600_cs_legacy_get_tiling_conf(struct drm_device *dev, u32 *npipes, u32 *nbanks, u32 *group_size)
-{
-	struct drm_radeon_private *dev_priv = dev->dev_private;
-
-	*npipes = dev_priv->r600_npipes;
-	*nbanks = dev_priv->r600_nbanks;
-	*group_size = dev_priv->r600_group_size;
-}
diff --git a/drivers/gpu/drm/radeon/r600_cs.c b/drivers/gpu/drm/radeon/r600_cs.c
index acc1f99c84d9..2f36fa1576e0 100644
--- a/drivers/gpu/drm/radeon/r600_cs.c
+++ b/drivers/gpu/drm/radeon/r600_cs.c
@@ -2328,101 +2328,6 @@ int r600_cs_parse(struct radeon_cs_parser *p)
 	return 0;
 }
 
-#ifdef CONFIG_DRM_RADEON_UMS
-
-/**
- * cs_parser_fini() - clean parser states
- * @parser:	parser structure holding parsing context.
- * @error:	error number
- *
- * If error is set than unvalidate buffer, otherwise just free memory
- * used by parsing context.
- **/
-static void r600_cs_parser_fini(struct radeon_cs_parser *parser, int error)
-{
-	unsigned i;
-
-	kfree(parser->relocs);
-	for (i = 0; i < parser->nchunks; i++)
-		drm_free_large(parser->chunks[i].kdata);
-	kfree(parser->chunks);
-	kfree(parser->chunks_array);
-}
-
-static int r600_cs_parser_relocs_legacy(struct radeon_cs_parser *p)
-{
-	if (p->chunk_relocs == NULL) {
-		return 0;
-	}
-	p->relocs = kzalloc(sizeof(struct radeon_bo_list), GFP_KERNEL);
-	if (p->relocs == NULL) {
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-int r600_cs_legacy(struct drm_device *dev, void *data, struct drm_file *filp,
-			unsigned family, u32 *ib, int *l)
-{
-	struct radeon_cs_parser parser;
-	struct radeon_cs_chunk *ib_chunk;
-	struct r600_cs_track *track;
-	int r;
-
-	/* initialize tracker */
-	track = kzalloc(sizeof(*track), GFP_KERNEL);
-	if (track == NULL)
-		return -ENOMEM;
-	r600_cs_track_init(track);
-	r600_cs_legacy_get_tiling_conf(dev, &track->npipes, &track->nbanks, &track->group_size);
-	/* initialize parser */
-	memset(&parser, 0, sizeof(struct radeon_cs_parser));
-	parser.filp = filp;
-	parser.dev = &dev->pdev->dev;
-	parser.rdev = NULL;
-	parser.family = family;
-	parser.track = track;
-	parser.ib.ptr = ib;
-	r = radeon_cs_parser_init(&parser, data);
-	if (r) {
-		DRM_ERROR("Failed to initialize parser !\n");
-		r600_cs_parser_fini(&parser, r);
-		return r;
-	}
-	r = r600_cs_parser_relocs_legacy(&parser);
-	if (r) {
-		DRM_ERROR("Failed to parse relocation !\n");
-		r600_cs_parser_fini(&parser, r);
-		return r;
-	}
-	/* Copy the packet into the IB, the parser will read from the
-	 * input memory (cached) and write to the IB (which can be
-	 * uncached). */
-	ib_chunk = parser.chunk_ib;
-	parser.ib.length_dw = ib_chunk->length_dw;
-	*l = parser.ib.length_dw;
-	if (copy_from_user(ib, ib_chunk->user_ptr, ib_chunk->length_dw * 4)) {
-		r = -EFAULT;
-		r600_cs_parser_fini(&parser, r);
-		return r;
-	}
-	r = r600_cs_parse(&parser);
-	if (r) {
-		DRM_ERROR("Invalid command stream !\n");
-		r600_cs_parser_fini(&parser, r);
-		return r;
-	}
-	r600_cs_parser_fini(&parser, r);
-	return r;
-}
-
-void r600_cs_legacy_init(void)
-{
-	r600_nomm = 1;
-}
-
-#endif
-
 /*
  *  DMA
  */
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 87db64983ea8..78a51b3eda10 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -268,6 +268,7 @@ struct radeon_clock {
 	uint32_t current_dispclk;
 	uint32_t dp_extclk;
 	uint32_t max_pixel_clock;
+	uint32_t vco_freq;
 };
 
 /*
@@ -1889,7 +1890,7 @@ struct radeon_asic {
 		void (*pad_ib)(struct radeon_ib *ib);
 	} vm;
 	/* ring specific callbacks */
-	struct radeon_asic_ring *ring[RADEON_NUM_RINGS];
+	const struct radeon_asic_ring *ring[RADEON_NUM_RINGS];
 	/* irqs */
 	struct {
 		int (*set)(struct radeon_device *rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index 1d4d4520a0ac..7d5a36dd5094 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -179,7 +179,7 @@ void radeon_agp_disable(struct radeon_device *rdev)
  * ASIC
  */
 
-static struct radeon_asic_ring r100_gfx_ring = {
+static const struct radeon_asic_ring r100_gfx_ring = {
 	.ib_execute = &r100_ring_ib_execute,
 	.emit_fence = &r100_fence_ring_emit,
 	.emit_semaphore = &r100_semaphore_ring_emit,
@@ -329,7 +329,7 @@ static struct radeon_asic r200_asic = {
 	},
 };
 
-static struct radeon_asic_ring r300_gfx_ring = {
+static const struct radeon_asic_ring r300_gfx_ring = {
 	.ib_execute = &r100_ring_ib_execute,
 	.emit_fence = &r300_fence_ring_emit,
 	.emit_semaphore = &r100_semaphore_ring_emit,
@@ -343,7 +343,7 @@ static struct radeon_asic_ring r300_gfx_ring = {
 	.set_wptr = &r100_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring rv515_gfx_ring = {
+static const struct radeon_asic_ring rv515_gfx_ring = {
 	.ib_execute = &r100_ring_ib_execute,
 	.emit_fence = &r300_fence_ring_emit,
 	.emit_semaphore = &r100_semaphore_ring_emit,
@@ -901,7 +901,7 @@ static struct radeon_asic r520_asic = {
 	},
 };
 
-static struct radeon_asic_ring r600_gfx_ring = {
+static const struct radeon_asic_ring r600_gfx_ring = {
 	.ib_execute = &r600_ring_ib_execute,
 	.emit_fence = &r600_fence_ring_emit,
 	.emit_semaphore = &r600_semaphore_ring_emit,
@@ -914,7 +914,7 @@ static struct radeon_asic_ring r600_gfx_ring = {
 	.set_wptr = &r600_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring r600_dma_ring = {
+static const struct radeon_asic_ring r600_dma_ring = {
 	.ib_execute = &r600_dma_ring_ib_execute,
 	.emit_fence = &r600_dma_fence_ring_emit,
 	.emit_semaphore = &r600_dma_semaphore_ring_emit,
@@ -999,7 +999,7 @@ static struct radeon_asic r600_asic = {
 	},
 };
 
-static struct radeon_asic_ring rv6xx_uvd_ring = {
+static const struct radeon_asic_ring rv6xx_uvd_ring = {
 	.ib_execute = &uvd_v1_0_ib_execute,
 	.emit_fence = &uvd_v1_0_fence_emit,
 	.emit_semaphore = &uvd_v1_0_semaphore_emit,
@@ -1198,7 +1198,7 @@ static struct radeon_asic rs780_asic = {
 	},
 };
 
-static struct radeon_asic_ring rv770_uvd_ring = {
+static const struct radeon_asic_ring rv770_uvd_ring = {
 	.ib_execute = &uvd_v1_0_ib_execute,
 	.emit_fence = &uvd_v2_2_fence_emit,
 	.emit_semaphore = &uvd_v2_2_semaphore_emit,
@@ -1305,7 +1305,7 @@ static struct radeon_asic rv770_asic = {
 	},
 };
 
-static struct radeon_asic_ring evergreen_gfx_ring = {
+static const struct radeon_asic_ring evergreen_gfx_ring = {
 	.ib_execute = &evergreen_ring_ib_execute,
 	.emit_fence = &r600_fence_ring_emit,
 	.emit_semaphore = &r600_semaphore_ring_emit,
@@ -1318,7 +1318,7 @@ static struct radeon_asic_ring evergreen_gfx_ring = {
 	.set_wptr = &r600_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring evergreen_dma_ring = {
+static const struct radeon_asic_ring evergreen_dma_ring = {
 	.ib_execute = &evergreen_dma_ring_ib_execute,
 	.emit_fence = &evergreen_dma_fence_ring_emit,
 	.emit_semaphore = &r600_dma_semaphore_ring_emit,
@@ -1612,7 +1612,7 @@ static struct radeon_asic btc_asic = {
 	},
 };
 
-static struct radeon_asic_ring cayman_gfx_ring = {
+static const struct radeon_asic_ring cayman_gfx_ring = {
 	.ib_execute = &cayman_ring_ib_execute,
 	.ib_parse = &evergreen_ib_parse,
 	.emit_fence = &cayman_fence_ring_emit,
@@ -1627,7 +1627,7 @@ static struct radeon_asic_ring cayman_gfx_ring = {
 	.set_wptr = &cayman_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring cayman_dma_ring = {
+static const struct radeon_asic_ring cayman_dma_ring = {
 	.ib_execute = &cayman_dma_ring_ib_execute,
 	.ib_parse = &evergreen_dma_ib_parse,
 	.emit_fence = &evergreen_dma_fence_ring_emit,
@@ -1642,7 +1642,7 @@ static struct radeon_asic_ring cayman_dma_ring = {
 	.set_wptr = &cayman_dma_set_wptr
 };
 
-static struct radeon_asic_ring cayman_uvd_ring = {
+static const struct radeon_asic_ring cayman_uvd_ring = {
 	.ib_execute = &uvd_v1_0_ib_execute,
 	.emit_fence = &uvd_v2_2_fence_emit,
 	.emit_semaphore = &uvd_v3_1_semaphore_emit,
@@ -1760,7 +1760,7 @@ static struct radeon_asic cayman_asic = {
 	},
 };
 
-static struct radeon_asic_ring trinity_vce_ring = {
+static const struct radeon_asic_ring trinity_vce_ring = {
 	.ib_execute = &radeon_vce_ib_execute,
 	.emit_fence = &radeon_vce_fence_emit,
 	.emit_semaphore = &radeon_vce_semaphore_emit,
@@ -1881,7 +1881,7 @@ static struct radeon_asic trinity_asic = {
 	},
 };
 
-static struct radeon_asic_ring si_gfx_ring = {
+static const struct radeon_asic_ring si_gfx_ring = {
 	.ib_execute = &si_ring_ib_execute,
 	.ib_parse = &si_ib_parse,
 	.emit_fence = &si_fence_ring_emit,
@@ -1896,7 +1896,7 @@ static struct radeon_asic_ring si_gfx_ring = {
 	.set_wptr = &cayman_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring si_dma_ring = {
+static const struct radeon_asic_ring si_dma_ring = {
 	.ib_execute = &cayman_dma_ring_ib_execute,
 	.ib_parse = &evergreen_dma_ib_parse,
 	.emit_fence = &evergreen_dma_fence_ring_emit,
@@ -2023,7 +2023,7 @@ static struct radeon_asic si_asic = {
 	},
 };
 
-static struct radeon_asic_ring ci_gfx_ring = {
+static const struct radeon_asic_ring ci_gfx_ring = {
 	.ib_execute = &cik_ring_ib_execute,
 	.ib_parse = &cik_ib_parse,
 	.emit_fence = &cik_fence_gfx_ring_emit,
@@ -2038,7 +2038,7 @@ static struct radeon_asic_ring ci_gfx_ring = {
 	.set_wptr = &cik_gfx_set_wptr,
 };
 
-static struct radeon_asic_ring ci_cp_ring = {
+static const struct radeon_asic_ring ci_cp_ring = {
 	.ib_execute = &cik_ring_ib_execute,
 	.ib_parse = &cik_ib_parse,
 	.emit_fence = &cik_fence_compute_ring_emit,
@@ -2053,7 +2053,7 @@ static struct radeon_asic_ring ci_cp_ring = {
 	.set_wptr = &cik_compute_set_wptr,
 };
 
-static struct radeon_asic_ring ci_dma_ring = {
+static const struct radeon_asic_ring ci_dma_ring = {
 	.ib_execute = &cik_sdma_ring_ib_execute,
 	.ib_parse = &cik_ib_parse,
 	.emit_fence = &cik_sdma_fence_ring_emit,
@@ -2068,7 +2068,7 @@ static struct radeon_asic_ring ci_dma_ring = {
 	.set_wptr = &cik_sdma_set_wptr,
 };
 
-static struct radeon_asic_ring ci_vce_ring = {
+static const struct radeon_asic_ring ci_vce_ring = {
 	.ib_execute = &radeon_vce_ib_execute,
 	.emit_fence = &radeon_vce_fence_emit,
 	.emit_semaphore = &radeon_vce_semaphore_emit,
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index 8f285244c839..de9a2ffcf5f7 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -437,7 +437,9 @@ static bool radeon_atom_apply_quirks(struct drm_device *dev,
 	}
 
 	/* Fujitsu D3003-S2 board lists DVI-I as DVI-D and VGA */
-	if (((dev->pdev->device == 0x9802) || (dev->pdev->device == 0x9806)) &&
+	if (((dev->pdev->device == 0x9802) ||
+	     (dev->pdev->device == 0x9805) ||
+	     (dev->pdev->device == 0x9806)) &&
 	    (dev->pdev->subsystem_vendor == 0x1734) &&
 	    (dev->pdev->subsystem_device == 0x11bd)) {
 		if (*connector_type == DRM_MODE_CONNECTOR_VGA) {
@@ -448,14 +450,6 @@ static bool radeon_atom_apply_quirks(struct drm_device *dev,
 		}
 	}
 
-	/* Fujitsu D3003-S2 board lists DVI-I as DVI-I and VGA */
-	if ((dev->pdev->device == 0x9805) &&
-	    (dev->pdev->subsystem_vendor == 0x1734) &&
-	    (dev->pdev->subsystem_device == 0x11bd)) {
-		if (*connector_type == DRM_MODE_CONNECTOR_VGA)
-			return false;
-	}
-
 	return true;
 }
 
@@ -1112,6 +1106,31 @@ union firmware_info {
 	ATOM_FIRMWARE_INFO_V2_2 info_22;
 };
 
+union igp_info {
+	struct _ATOM_INTEGRATED_SYSTEM_INFO info;
+	struct _ATOM_INTEGRATED_SYSTEM_INFO_V2 info_2;
+	struct _ATOM_INTEGRATED_SYSTEM_INFO_V6 info_6;
+	struct _ATOM_INTEGRATED_SYSTEM_INFO_V1_7 info_7;
+	struct _ATOM_INTEGRATED_SYSTEM_INFO_V1_8 info_8;
+};
+
+static void radeon_atombios_get_dentist_vco_freq(struct radeon_device *rdev)
+{
+	struct radeon_mode_info *mode_info = &rdev->mode_info;
+	int index = GetIndexIntoMasterTable(DATA, IntegratedSystemInfo);
+	union igp_info *igp_info;
+	u8 frev, crev;
+	u16 data_offset;
+
+	if (atom_parse_data_header(mode_info->atom_context, index, NULL,
+			&frev, &crev, &data_offset)) {
+		igp_info = (union igp_info *)(mode_info->atom_context->bios +
+			data_offset);
+		rdev->clock.vco_freq =
+			le32_to_cpu(igp_info->info_6.ulDentistVCOFreq);
+	}
+}
+
 bool radeon_atom_get_clock_info(struct drm_device *dev)
 {
 	struct radeon_device *rdev = dev->dev_private;
@@ -1263,20 +1282,25 @@ bool radeon_atom_get_clock_info(struct drm_device *dev)
 		rdev->mode_info.firmware_flags =
 			le16_to_cpu(firmware_info->info.usFirmwareCapability.susAccess);
 
+		if (ASIC_IS_DCE8(rdev))
+			rdev->clock.vco_freq =
+				le32_to_cpu(firmware_info->info_22.ulGPUPLL_OutputFreq);
+		else if (ASIC_IS_DCE5(rdev))
+			rdev->clock.vco_freq = rdev->clock.current_dispclk;
+		else if (ASIC_IS_DCE41(rdev))
+			radeon_atombios_get_dentist_vco_freq(rdev);
+		else
+			rdev->clock.vco_freq = rdev->clock.current_dispclk;
+
+		if (rdev->clock.vco_freq == 0)
+			rdev->clock.vco_freq = 360000;	/* 3.6 GHz */
+
 		return true;
 	}
 
 	return false;
 }
 
-union igp_info {
-	struct _ATOM_INTEGRATED_SYSTEM_INFO info;
-	struct _ATOM_INTEGRATED_SYSTEM_INFO_V2 info_2;
-	struct _ATOM_INTEGRATED_SYSTEM_INFO_V6 info_6;
-	struct _ATOM_INTEGRATED_SYSTEM_INFO_V1_7 info_7;
-	struct _ATOM_INTEGRATED_SYSTEM_INFO_V1_8 info_8;
-};
-
 bool radeon_atombios_sideport_present(struct radeon_device *rdev)
 {
 	struct radeon_mode_info *mode_info = &rdev->mode_info;
diff --git a/drivers/gpu/drm/radeon/radeon_audio.c b/drivers/gpu/drm/radeon/radeon_audio.c
index 2c02e99b5f95..b214663b370d 100644
--- a/drivers/gpu/drm/radeon/radeon_audio.c
+++ b/drivers/gpu/drm/radeon/radeon_audio.c
@@ -739,9 +739,6 @@ static void radeon_audio_dp_mode_set(struct drm_encoder *encoder,
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 	struct drm_connector *connector = radeon_get_connector_for_encoder(encoder);
-	struct radeon_connector *radeon_connector = to_radeon_connector(connector);
-	struct radeon_connector_atom_dig *dig_connector =
-		radeon_connector->con_priv;
 
 	if (!dig || !dig->afmt)
 		return;
@@ -753,10 +750,7 @@ static void radeon_audio_dp_mode_set(struct drm_encoder *encoder,
 		radeon_audio_write_speaker_allocation(encoder);
 		radeon_audio_write_sad_regs(encoder);
 		radeon_audio_write_latency_fields(encoder, mode);
-		if (rdev->clock.dp_extclk || ASIC_IS_DCE5(rdev))
-			radeon_audio_set_dto(encoder, rdev->clock.default_dispclk * 10);
-		else
-			radeon_audio_set_dto(encoder, dig_connector->dp_clock);
+		radeon_audio_set_dto(encoder, rdev->clock.vco_freq * 10);
 		radeon_audio_set_audio_packet(encoder);
 		radeon_audio_select_pin(encoder);
 
@@ -781,3 +775,15 @@ void radeon_audio_dpms(struct drm_encoder *encoder, int mode)
 	if (radeon_encoder->audio && radeon_encoder->audio->dpms)
 		radeon_encoder->audio->dpms(encoder, mode == DRM_MODE_DPMS_ON);
 }
+
+unsigned int radeon_audio_decode_dfs_div(unsigned int div)
+{
+	if (div >= 8 && div < 64)
+		return (div - 8) * 25 + 200;
+	else if (div >= 64 && div < 96)
+		return (div - 64) * 50 + 1600;
+	else if (div >= 96 && div < 128)
+		return (div - 96) * 100 + 3200;
+	else
+		return 0;
+}
diff --git a/drivers/gpu/drm/radeon/radeon_audio.h b/drivers/gpu/drm/radeon/radeon_audio.h
index 059cc3012062..5c70cceaa4a6 100644
--- a/drivers/gpu/drm/radeon/radeon_audio.h
+++ b/drivers/gpu/drm/radeon/radeon_audio.h
@@ -79,5 +79,6 @@ void radeon_audio_fini(struct radeon_device *rdev);
 void radeon_audio_mode_set(struct drm_encoder *encoder,
 	struct drm_display_mode *mode);
 void radeon_audio_dpms(struct drm_encoder *encoder, int mode);
+unsigned int radeon_audio_decode_dfs_div(unsigned int div);
 
 #endif
diff --git a/drivers/gpu/drm/radeon/radeon_cp.c b/drivers/gpu/drm/radeon/radeon_cp.c
deleted file mode 100644
index 500287eff55d..000000000000
--- a/drivers/gpu/drm/radeon/radeon_cp.c
+++ /dev/null
@@ -1,2243 +0,0 @@
-/* radeon_cp.c -- CP support for Radeon -*- linux-c -*- */
-/*
- * Copyright 2000 Precision Insight, Inc., Cedar Park, Texas.
- * Copyright 2000 VA Linux Systems, Inc., Fremont, California.
- * Copyright 2007 Advanced Micro Devices, Inc.
- * All Rights Reserved.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, 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
- * PRECISION INSIGHT AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Kevin E. Martin <martin@valinux.com>
- *    Gareth Hughes <gareth@valinux.com>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <linux/module.h>
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-#include "r300_reg.h"
-
-#define RADEON_FIFO_DEBUG	0
-
-/* Firmware Names */
-#define FIRMWARE_R100		"radeon/R100_cp.bin"
-#define FIRMWARE_R200		"radeon/R200_cp.bin"
-#define FIRMWARE_R300		"radeon/R300_cp.bin"
-#define FIRMWARE_R420		"radeon/R420_cp.bin"
-#define FIRMWARE_RS690		"radeon/RS690_cp.bin"
-#define FIRMWARE_RS600		"radeon/RS600_cp.bin"
-#define FIRMWARE_R520		"radeon/R520_cp.bin"
-
-MODULE_FIRMWARE(FIRMWARE_R100);
-MODULE_FIRMWARE(FIRMWARE_R200);
-MODULE_FIRMWARE(FIRMWARE_R300);
-MODULE_FIRMWARE(FIRMWARE_R420);
-MODULE_FIRMWARE(FIRMWARE_RS690);
-MODULE_FIRMWARE(FIRMWARE_RS600);
-MODULE_FIRMWARE(FIRMWARE_R520);
-
-static int radeon_do_cleanup_cp(struct drm_device * dev);
-static void radeon_do_cp_start(drm_radeon_private_t * dev_priv);
-
-u32 radeon_read_ring_rptr(drm_radeon_private_t *dev_priv, u32 off)
-{
-	u32 val;
-
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		val = DRM_READ32(dev_priv->ring_rptr, off);
-	} else {
-		val = *(((volatile u32 *)
-			 dev_priv->ring_rptr->handle) +
-			(off / sizeof(u32)));
-		val = le32_to_cpu(val);
-	}
-	return val;
-}
-
-u32 radeon_get_ring_head(drm_radeon_private_t *dev_priv)
-{
-	if (dev_priv->writeback_works)
-		return radeon_read_ring_rptr(dev_priv, 0);
-	else {
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			return RADEON_READ(R600_CP_RB_RPTR);
-		else
-			return RADEON_READ(RADEON_CP_RB_RPTR);
-	}
-}
-
-void radeon_write_ring_rptr(drm_radeon_private_t *dev_priv, u32 off, u32 val)
-{
-	if (dev_priv->flags & RADEON_IS_AGP)
-		DRM_WRITE32(dev_priv->ring_rptr, off, val);
-	else
-		*(((volatile u32 *) dev_priv->ring_rptr->handle) +
-		  (off / sizeof(u32))) = cpu_to_le32(val);
-}
-
-void radeon_set_ring_head(drm_radeon_private_t *dev_priv, u32 val)
-{
-	radeon_write_ring_rptr(dev_priv, 0, val);
-}
-
-u32 radeon_get_scratch(drm_radeon_private_t *dev_priv, int index)
-{
-	if (dev_priv->writeback_works) {
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			return radeon_read_ring_rptr(dev_priv,
-						     R600_SCRATCHOFF(index));
-		else
-			return radeon_read_ring_rptr(dev_priv,
-						     RADEON_SCRATCHOFF(index));
-	} else {
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			return RADEON_READ(R600_SCRATCH_REG0 + 4*index);
-		else
-			return RADEON_READ(RADEON_SCRATCH_REG0 + 4*index);
-	}
-}
-
-static u32 R500_READ_MCIND(drm_radeon_private_t *dev_priv, int addr)
-{
-	u32 ret;
-	RADEON_WRITE(R520_MC_IND_INDEX, 0x7f0000 | (addr & 0xff));
-	ret = RADEON_READ(R520_MC_IND_DATA);
-	RADEON_WRITE(R520_MC_IND_INDEX, 0);
-	return ret;
-}
-
-static u32 RS480_READ_MCIND(drm_radeon_private_t *dev_priv, int addr)
-{
-	u32 ret;
-	RADEON_WRITE(RS480_NB_MC_INDEX, addr & 0xff);
-	ret = RADEON_READ(RS480_NB_MC_DATA);
-	RADEON_WRITE(RS480_NB_MC_INDEX, 0xff);
-	return ret;
-}
-
-static u32 RS690_READ_MCIND(drm_radeon_private_t *dev_priv, int addr)
-{
-	u32 ret;
-	RADEON_WRITE(RS690_MC_INDEX, (addr & RS690_MC_INDEX_MASK));
-	ret = RADEON_READ(RS690_MC_DATA);
-	RADEON_WRITE(RS690_MC_INDEX, RS690_MC_INDEX_MASK);
-	return ret;
-}
-
-static u32 RS600_READ_MCIND(drm_radeon_private_t *dev_priv, int addr)
-{
-	u32 ret;
-	RADEON_WRITE(RS600_MC_INDEX, ((addr & RS600_MC_ADDR_MASK) |
-				      RS600_MC_IND_CITF_ARB0));
-	ret = RADEON_READ(RS600_MC_DATA);
-	return ret;
-}
-
-static u32 IGP_READ_MCIND(drm_radeon_private_t *dev_priv, int addr)
-{
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))
-		return RS690_READ_MCIND(dev_priv, addr);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-		return RS600_READ_MCIND(dev_priv, addr);
-	else
-		return RS480_READ_MCIND(dev_priv, addr);
-}
-
-u32 radeon_read_fb_location(drm_radeon_private_t *dev_priv)
-{
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)
-		return RADEON_READ(R700_MC_VM_FB_LOCATION);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return RADEON_READ(R600_MC_VM_FB_LOCATION);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV515)
-		return R500_READ_MCIND(dev_priv, RV515_MC_FB_LOCATION);
-	else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		 ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))
-		return RS690_READ_MCIND(dev_priv, RS690_MC_FB_LOCATION);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-		return RS600_READ_MCIND(dev_priv, RS600_MC_FB_LOCATION);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_RV515)
-		return R500_READ_MCIND(dev_priv, R520_MC_FB_LOCATION);
-	else
-		return RADEON_READ(RADEON_MC_FB_LOCATION);
-}
-
-static void radeon_write_fb_location(drm_radeon_private_t *dev_priv, u32 fb_loc)
-{
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)
-		RADEON_WRITE(R700_MC_VM_FB_LOCATION, fb_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		RADEON_WRITE(R600_MC_VM_FB_LOCATION, fb_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV515)
-		R500_WRITE_MCIND(RV515_MC_FB_LOCATION, fb_loc);
-	else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		 ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))
-		RS690_WRITE_MCIND(RS690_MC_FB_LOCATION, fb_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-		RS600_WRITE_MCIND(RS600_MC_FB_LOCATION, fb_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_RV515)
-		R500_WRITE_MCIND(R520_MC_FB_LOCATION, fb_loc);
-	else
-		RADEON_WRITE(RADEON_MC_FB_LOCATION, fb_loc);
-}
-
-void radeon_write_agp_location(drm_radeon_private_t *dev_priv, u32 agp_loc)
-{
-	/*R6xx/R7xx: AGP_TOP and BOT are actually 18 bits each */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770) {
-		RADEON_WRITE(R700_MC_VM_AGP_BOT, agp_loc & 0xffff); /* FIX ME */
-		RADEON_WRITE(R700_MC_VM_AGP_TOP, (agp_loc >> 16) & 0xffff);
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) {
-		RADEON_WRITE(R600_MC_VM_AGP_BOT, agp_loc & 0xffff); /* FIX ME */
-		RADEON_WRITE(R600_MC_VM_AGP_TOP, (agp_loc >> 16) & 0xffff);
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV515)
-		R500_WRITE_MCIND(RV515_MC_AGP_LOCATION, agp_loc);
-	else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		 ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))
-		RS690_WRITE_MCIND(RS690_MC_AGP_LOCATION, agp_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-		RS600_WRITE_MCIND(RS600_MC_AGP_LOCATION, agp_loc);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_RV515)
-		R500_WRITE_MCIND(R520_MC_AGP_LOCATION, agp_loc);
-	else
-		RADEON_WRITE(RADEON_MC_AGP_LOCATION, agp_loc);
-}
-
-void radeon_write_agp_base(drm_radeon_private_t *dev_priv, u64 agp_base)
-{
-	u32 agp_base_hi = upper_32_bits(agp_base);
-	u32 agp_base_lo = agp_base & 0xffffffff;
-	u32 r6xx_agp_base = (agp_base >> 22) & 0x3ffff;
-
-	/* R6xx/R7xx must be aligned to a 4MB boundary */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV770)
-		RADEON_WRITE(R700_MC_VM_AGP_BASE, r6xx_agp_base);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		RADEON_WRITE(R600_MC_VM_AGP_BASE, r6xx_agp_base);
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV515) {
-		R500_WRITE_MCIND(RV515_MC_AGP_BASE, agp_base_lo);
-		R500_WRITE_MCIND(RV515_MC_AGP_BASE_2, agp_base_hi);
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		 ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740)) {
-		RS690_WRITE_MCIND(RS690_MC_AGP_BASE, agp_base_lo);
-		RS690_WRITE_MCIND(RS690_MC_AGP_BASE_2, agp_base_hi);
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600) {
-		RS600_WRITE_MCIND(RS600_AGP_BASE, agp_base_lo);
-		RS600_WRITE_MCIND(RS600_AGP_BASE_2, agp_base_hi);
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) > CHIP_RV515) {
-		R500_WRITE_MCIND(R520_MC_AGP_BASE, agp_base_lo);
-		R500_WRITE_MCIND(R520_MC_AGP_BASE_2, agp_base_hi);
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS400) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS480)) {
-		RADEON_WRITE(RADEON_AGP_BASE, agp_base_lo);
-		RADEON_WRITE(RS480_AGP_BASE_2, agp_base_hi);
-	} else {
-		RADEON_WRITE(RADEON_AGP_BASE, agp_base_lo);
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R200)
-			RADEON_WRITE(RADEON_AGP_BASE_2, agp_base_hi);
-	}
-}
-
-void radeon_enable_bm(struct drm_radeon_private *dev_priv)
-{
-	u32 tmp;
-	/* Turn on bus mastering */
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740)) {
-		/* rs600/rs690/rs740 */
-		tmp = RADEON_READ(RADEON_BUS_CNTL) & ~RS600_BUS_MASTER_DIS;
-		RADEON_WRITE(RADEON_BUS_CNTL, tmp);
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV350) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R420) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS400) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS480)) {
-		/* r1xx, r2xx, r300, r(v)350, r420/r481, rs400/rs480 */
-		tmp = RADEON_READ(RADEON_BUS_CNTL) & ~RADEON_BUS_MASTER_DIS;
-		RADEON_WRITE(RADEON_BUS_CNTL, tmp);
-	} /* PCIE cards appears to not need this */
-}
-
-static int RADEON_READ_PLL(struct drm_device * dev, int addr)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	RADEON_WRITE8(RADEON_CLOCK_CNTL_INDEX, addr & 0x1f);
-	return RADEON_READ(RADEON_CLOCK_CNTL_DATA);
-}
-
-static u32 RADEON_READ_PCIE(drm_radeon_private_t *dev_priv, int addr)
-{
-	RADEON_WRITE8(RADEON_PCIE_INDEX, addr & 0xff);
-	return RADEON_READ(RADEON_PCIE_DATA);
-}
-
-#if RADEON_FIFO_DEBUG
-static void radeon_status(drm_radeon_private_t * dev_priv)
-{
-	printk("%s:\n", __func__);
-	printk("RBBM_STATUS = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_RBBM_STATUS));
-	printk("CP_RB_RTPR = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_CP_RB_RPTR));
-	printk("CP_RB_WTPR = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_CP_RB_WPTR));
-	printk("AIC_CNTL = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_AIC_CNTL));
-	printk("AIC_STAT = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_AIC_STAT));
-	printk("AIC_PT_BASE = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_AIC_PT_BASE));
-	printk("TLB_ADDR = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_AIC_TLB_ADDR));
-	printk("TLB_DATA = 0x%08x\n",
-	       (unsigned int)RADEON_READ(RADEON_AIC_TLB_DATA));
-}
-#endif
-
-/* ================================================================
- * Engine, FIFO control
- */
-
-static int radeon_do_pixcache_flush(drm_radeon_private_t * dev_priv)
-{
-	u32 tmp;
-	int i;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV280) {
-		tmp = RADEON_READ(RADEON_RB3D_DSTCACHE_CTLSTAT);
-		tmp |= RADEON_RB3D_DC_FLUSH_ALL;
-		RADEON_WRITE(RADEON_RB3D_DSTCACHE_CTLSTAT, tmp);
-
-		for (i = 0; i < dev_priv->usec_timeout; i++) {
-			if (!(RADEON_READ(RADEON_RB3D_DSTCACHE_CTLSTAT)
-			      & RADEON_RB3D_DC_BUSY)) {
-				return 0;
-			}
-			DRM_UDELAY(1);
-		}
-	} else {
-		/* don't flush or purge cache here or lockup */
-		return 0;
-	}
-
-#if RADEON_FIFO_DEBUG
-	DRM_ERROR("failed!\n");
-	radeon_status(dev_priv);
-#endif
-	return -EBUSY;
-}
-
-static int radeon_do_wait_for_fifo(drm_radeon_private_t * dev_priv, int entries)
-{
-	int i;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	for (i = 0; i < dev_priv->usec_timeout; i++) {
-		int slots = (RADEON_READ(RADEON_RBBM_STATUS)
-			     & RADEON_RBBM_FIFOCNT_MASK);
-		if (slots >= entries)
-			return 0;
-		DRM_UDELAY(1);
-	}
-	DRM_DEBUG("wait for fifo failed status : 0x%08X 0x%08X\n",
-		 RADEON_READ(RADEON_RBBM_STATUS),
-		 RADEON_READ(R300_VAP_CNTL_STATUS));
-
-#if RADEON_FIFO_DEBUG
-	DRM_ERROR("failed!\n");
-	radeon_status(dev_priv);
-#endif
-	return -EBUSY;
-}
-
-static int radeon_do_wait_for_idle(drm_radeon_private_t * dev_priv)
-{
-	int i, ret;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	ret = radeon_do_wait_for_fifo(dev_priv, 64);
-	if (ret)
-		return ret;
-
-	for (i = 0; i < dev_priv->usec_timeout; i++) {
-		if (!(RADEON_READ(RADEON_RBBM_STATUS)
-		      & RADEON_RBBM_ACTIVE)) {
-			radeon_do_pixcache_flush(dev_priv);
-			return 0;
-		}
-		DRM_UDELAY(1);
-	}
-	DRM_DEBUG("wait idle failed status : 0x%08X 0x%08X\n",
-		 RADEON_READ(RADEON_RBBM_STATUS),
-		 RADEON_READ(R300_VAP_CNTL_STATUS));
-
-#if RADEON_FIFO_DEBUG
-	DRM_ERROR("failed!\n");
-	radeon_status(dev_priv);
-#endif
-	return -EBUSY;
-}
-
-static void radeon_init_pipes(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	uint32_t gb_tile_config, gb_pipe_sel = 0;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV530) {
-		uint32_t z_pipe_sel = RADEON_READ(RV530_GB_PIPE_SELECT2);
-		if ((z_pipe_sel & 3) == 3)
-			dev_priv->num_z_pipes = 2;
-		else
-			dev_priv->num_z_pipes = 1;
-	} else
-		dev_priv->num_z_pipes = 1;
-
-	/* RS4xx/RS6xx/R4xx/R5xx */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R420) {
-		gb_pipe_sel = RADEON_READ(R400_GB_PIPE_SELECT);
-		dev_priv->num_gb_pipes = ((gb_pipe_sel >> 12) & 0x3) + 1;
-		/* SE cards have 1 pipe */
-		if ((dev->pdev->device == 0x5e4c) ||
-		    (dev->pdev->device == 0x5e4f))
-			dev_priv->num_gb_pipes = 1;
-	} else {
-		/* R3xx */
-		if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R300 &&
-		     dev->pdev->device != 0x4144) ||
-		    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R350 &&
-		     dev->pdev->device != 0x4148)) {
-			dev_priv->num_gb_pipes = 2;
-		} else {
-			/* RV3xx/R300 AD/R350 AH */
-			dev_priv->num_gb_pipes = 1;
-		}
-	}
-	DRM_INFO("Num pipes: %d\n", dev_priv->num_gb_pipes);
-
-	gb_tile_config = (R300_ENABLE_TILING | R300_TILE_SIZE_16 /*| R300_SUBPIXEL_1_16*/);
-
-	switch (dev_priv->num_gb_pipes) {
-	case 2: gb_tile_config |= R300_PIPE_COUNT_R300; break;
-	case 3: gb_tile_config |= R300_PIPE_COUNT_R420_3P; break;
-	case 4: gb_tile_config |= R300_PIPE_COUNT_R420; break;
-	default:
-	case 1: gb_tile_config |= R300_PIPE_COUNT_RV350; break;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RV515) {
-		RADEON_WRITE_PLL(R500_DYN_SCLK_PWMEM_PIPE, (1 | ((gb_pipe_sel >> 8) & 0xf) << 4));
-		RADEON_WRITE(R300_SU_REG_DEST, ((1 << dev_priv->num_gb_pipes) - 1));
-	}
-	RADEON_WRITE(R300_GB_TILE_CONFIG, gb_tile_config);
-	radeon_do_wait_for_idle(dev_priv);
-	RADEON_WRITE(R300_DST_PIPE_CONFIG, RADEON_READ(R300_DST_PIPE_CONFIG) | R300_PIPE_AUTO_CONFIG);
-	RADEON_WRITE(R300_RB2D_DSTCACHE_MODE, (RADEON_READ(R300_RB2D_DSTCACHE_MODE) |
-					       R300_DC_AUTOFLUSH_ENABLE |
-					       R300_DC_DC_DISABLE_IGNORE_PE));
-
-
-}
-
-/* ================================================================
- * CP control, initialization
- */
-
-/* Load the microcode for the CP */
-static int radeon_cp_init_microcode(drm_radeon_private_t *dev_priv)
-{
-	struct platform_device *pdev;
-	const char *fw_name = NULL;
-	int err;
-
-	DRM_DEBUG("\n");
-
-	pdev = platform_device_register_simple("radeon_cp", 0, NULL, 0);
-	err = IS_ERR(pdev);
-	if (err) {
-		printk(KERN_ERR "radeon_cp: Failed to register firmware\n");
-		return -EINVAL;
-	}
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R100) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV100) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV200) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS100) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS200)) {
-		DRM_INFO("Loading R100 Microcode\n");
-		fw_name = FIRMWARE_R100;
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R200) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV250) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV280) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS300)) {
-		DRM_INFO("Loading R200 Microcode\n");
-		fw_name = FIRMWARE_R200;
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R300) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R350) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV350) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV380) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS400) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS480)) {
-		DRM_INFO("Loading R300 Microcode\n");
-		fw_name = FIRMWARE_R300;
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R420) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R423) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV410)) {
-		DRM_INFO("Loading R400 Microcode\n");
-		fw_name = FIRMWARE_R420;
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740)) {
-		DRM_INFO("Loading RS690/RS740 Microcode\n");
-		fw_name = FIRMWARE_RS690;
-	} else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600) {
-		DRM_INFO("Loading RS600 Microcode\n");
-		fw_name = FIRMWARE_RS600;
-	} else if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV515) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R520) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV530) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R580) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV560) ||
-		   ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RV570)) {
-		DRM_INFO("Loading R500 Microcode\n");
-		fw_name = FIRMWARE_R520;
-	}
-
-	err = request_firmware(&dev_priv->me_fw, fw_name, &pdev->dev);
-	platform_device_unregister(pdev);
-	if (err) {
-		printk(KERN_ERR "radeon_cp: Failed to load firmware \"%s\"\n",
-		       fw_name);
-	} else if (dev_priv->me_fw->size % 8) {
-		printk(KERN_ERR
-		       "radeon_cp: Bogus length %zu in firmware \"%s\"\n",
-		       dev_priv->me_fw->size, fw_name);
-		err = -EINVAL;
-		release_firmware(dev_priv->me_fw);
-		dev_priv->me_fw = NULL;
-	}
-	return err;
-}
-
-static void radeon_cp_load_microcode(drm_radeon_private_t *dev_priv)
-{
-	const __be32 *fw_data;
-	int i, size;
-
-	radeon_do_wait_for_idle(dev_priv);
-
-	if (dev_priv->me_fw) {
-		size = dev_priv->me_fw->size / 4;
-		fw_data = (const __be32 *)&dev_priv->me_fw->data[0];
-		RADEON_WRITE(RADEON_CP_ME_RAM_ADDR, 0);
-		for (i = 0; i < size; i += 2) {
-			RADEON_WRITE(RADEON_CP_ME_RAM_DATAH,
-				     be32_to_cpup(&fw_data[i]));
-			RADEON_WRITE(RADEON_CP_ME_RAM_DATAL,
-				     be32_to_cpup(&fw_data[i + 1]));
-		}
-	}
-}
-
-/* Flush any pending commands to the CP.  This should only be used just
- * prior to a wait for idle, as it informs the engine that the command
- * stream is ending.
- */
-static void radeon_do_cp_flush(drm_radeon_private_t * dev_priv)
-{
-	DRM_DEBUG("\n");
-#if 0
-	u32 tmp;
-
-	tmp = RADEON_READ(RADEON_CP_RB_WPTR) | (1 << 31);
-	RADEON_WRITE(RADEON_CP_RB_WPTR, tmp);
-#endif
-}
-
-/* Wait for the CP to go idle.
- */
-int radeon_do_cp_idle(drm_radeon_private_t * dev_priv)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(6);
-
-	RADEON_PURGE_CACHE();
-	RADEON_PURGE_ZCACHE();
-	RADEON_WAIT_UNTIL_IDLE();
-
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	return radeon_do_wait_for_idle(dev_priv);
-}
-
-/* Start the Command Processor.
- */
-static void radeon_do_cp_start(drm_radeon_private_t * dev_priv)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	radeon_do_wait_for_idle(dev_priv);
-
-	RADEON_WRITE(RADEON_CP_CSQ_CNTL, dev_priv->cp_mode);
-
-	dev_priv->cp_running = 1;
-
-	/* on r420, any DMA from CP to system memory while 2D is active
-	 * can cause a hang.  workaround is to queue a CP RESYNC token
-	 */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R420) {
-		BEGIN_RING(3);
-		OUT_RING(CP_PACKET0(R300_CP_RESYNC_ADDR, 1));
-		OUT_RING(5); /* scratch reg 5 */
-		OUT_RING(0xdeadbeef);
-		ADVANCE_RING();
-		COMMIT_RING();
-	}
-
-	BEGIN_RING(8);
-	/* isync can only be written through cp on r5xx write it here */
-	OUT_RING(CP_PACKET0(RADEON_ISYNC_CNTL, 0));
-	OUT_RING(RADEON_ISYNC_ANY2D_IDLE3D |
-		 RADEON_ISYNC_ANY3D_IDLE2D |
-		 RADEON_ISYNC_WAIT_IDLEGUI |
-		 RADEON_ISYNC_CPSCRATCH_IDLEGUI);
-	RADEON_PURGE_CACHE();
-	RADEON_PURGE_ZCACHE();
-	RADEON_WAIT_UNTIL_IDLE();
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	dev_priv->track_flush |= RADEON_FLUSH_EMITED | RADEON_PURGE_EMITED;
-}
-
-/* Reset the Command Processor.  This will not flush any pending
- * commands, so you must wait for the CP command stream to complete
- * before calling this routine.
- */
-static void radeon_do_cp_reset(drm_radeon_private_t * dev_priv)
-{
-	u32 cur_read_ptr;
-	DRM_DEBUG("\n");
-
-	cur_read_ptr = RADEON_READ(RADEON_CP_RB_RPTR);
-	RADEON_WRITE(RADEON_CP_RB_WPTR, cur_read_ptr);
-	SET_RING_HEAD(dev_priv, cur_read_ptr);
-	dev_priv->ring.tail = cur_read_ptr;
-}
-
-/* Stop the Command Processor.  This will not flush any pending
- * commands, so you must flush the command stream and wait for the CP
- * to go idle before calling this routine.
- */
-static void radeon_do_cp_stop(drm_radeon_private_t * dev_priv)
-{
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	/* finish the pending CP_RESYNC token */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_R420) {
-		BEGIN_RING(2);
-		OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
-		OUT_RING(R300_RB3D_DC_FINISH);
-		ADVANCE_RING();
-		COMMIT_RING();
-		radeon_do_wait_for_idle(dev_priv);
-	}
-
-	RADEON_WRITE(RADEON_CP_CSQ_CNTL, RADEON_CSQ_PRIDIS_INDDIS);
-
-	dev_priv->cp_running = 0;
-}
-
-/* Reset the engine.  This will stop the CP if it is running.
- */
-static int radeon_do_engine_reset(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	u32 clock_cntl_index = 0, mclk_cntl = 0, rbbm_soft_reset;
-	DRM_DEBUG("\n");
-
-	radeon_do_pixcache_flush(dev_priv);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV410) {
-		/* may need something similar for newer chips */
-		clock_cntl_index = RADEON_READ(RADEON_CLOCK_CNTL_INDEX);
-		mclk_cntl = RADEON_READ_PLL(dev, RADEON_MCLK_CNTL);
-
-		RADEON_WRITE_PLL(RADEON_MCLK_CNTL, (mclk_cntl |
-						    RADEON_FORCEON_MCLKA |
-						    RADEON_FORCEON_MCLKB |
-						    RADEON_FORCEON_YCLKA |
-						    RADEON_FORCEON_YCLKB |
-						    RADEON_FORCEON_MC |
-						    RADEON_FORCEON_AIC));
-	}
-
-	rbbm_soft_reset = RADEON_READ(RADEON_RBBM_SOFT_RESET);
-
-	RADEON_WRITE(RADEON_RBBM_SOFT_RESET, (rbbm_soft_reset |
-					      RADEON_SOFT_RESET_CP |
-					      RADEON_SOFT_RESET_HI |
-					      RADEON_SOFT_RESET_SE |
-					      RADEON_SOFT_RESET_RE |
-					      RADEON_SOFT_RESET_PP |
-					      RADEON_SOFT_RESET_E2 |
-					      RADEON_SOFT_RESET_RB));
-	RADEON_READ(RADEON_RBBM_SOFT_RESET);
-	RADEON_WRITE(RADEON_RBBM_SOFT_RESET, (rbbm_soft_reset &
-					      ~(RADEON_SOFT_RESET_CP |
-						RADEON_SOFT_RESET_HI |
-						RADEON_SOFT_RESET_SE |
-						RADEON_SOFT_RESET_RE |
-						RADEON_SOFT_RESET_PP |
-						RADEON_SOFT_RESET_E2 |
-						RADEON_SOFT_RESET_RB)));
-	RADEON_READ(RADEON_RBBM_SOFT_RESET);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV410) {
-		RADEON_WRITE_PLL(RADEON_MCLK_CNTL, mclk_cntl);
-		RADEON_WRITE(RADEON_CLOCK_CNTL_INDEX, clock_cntl_index);
-		RADEON_WRITE(RADEON_RBBM_SOFT_RESET, rbbm_soft_reset);
-	}
-
-	/* setup the raster pipes */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R300)
-	    radeon_init_pipes(dev);
-
-	/* Reset the CP ring */
-	radeon_do_cp_reset(dev_priv);
-
-	/* The CP is no longer running after an engine reset */
-	dev_priv->cp_running = 0;
-
-	/* Reset any pending vertex, indirect buffers */
-	radeon_freelist_reset(dev);
-
-	return 0;
-}
-
-static void radeon_cp_init_ring_buffer(struct drm_device * dev,
-				       drm_radeon_private_t *dev_priv,
-				       struct drm_file *file_priv)
-{
-	struct drm_radeon_master_private *master_priv;
-	u32 ring_start, cur_read_ptr;
-
-	/* Initialize the memory controller. With new memory map, the fb location
-	 * is not changed, it should have been properly initialized already. Part
-	 * of the problem is that the code below is bogus, assuming the GART is
-	 * always appended to the fb which is not necessarily the case
-	 */
-	if (!dev_priv->new_memmap)
-		radeon_write_fb_location(dev_priv,
-			     ((dev_priv->gart_vm_start - 1) & 0xffff0000)
-			     | (dev_priv->fb_location >> 16));
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		radeon_write_agp_base(dev_priv, dev->agp->base);
-
-		radeon_write_agp_location(dev_priv,
-			     (((dev_priv->gart_vm_start - 1 +
-				dev_priv->gart_size) & 0xffff0000) |
-			      (dev_priv->gart_vm_start >> 16)));
-
-		ring_start = (dev_priv->cp_ring->offset
-			      - dev->agp->base
-			      + dev_priv->gart_vm_start);
-	} else
-#endif
-		ring_start = (dev_priv->cp_ring->offset
-			      - (unsigned long)dev->sg->virtual
-			      + dev_priv->gart_vm_start);
-
-	RADEON_WRITE(RADEON_CP_RB_BASE, ring_start);
-
-	/* Set the write pointer delay */
-	RADEON_WRITE(RADEON_CP_RB_WPTR_DELAY, 0);
-
-	/* Initialize the ring buffer's read and write pointers */
-	cur_read_ptr = RADEON_READ(RADEON_CP_RB_RPTR);
-	RADEON_WRITE(RADEON_CP_RB_WPTR, cur_read_ptr);
-	SET_RING_HEAD(dev_priv, cur_read_ptr);
-	dev_priv->ring.tail = cur_read_ptr;
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		RADEON_WRITE(RADEON_CP_RB_RPTR_ADDR,
-			     dev_priv->ring_rptr->offset
-			     - dev->agp->base + dev_priv->gart_vm_start);
-	} else
-#endif
-	{
-		RADEON_WRITE(RADEON_CP_RB_RPTR_ADDR,
-			     dev_priv->ring_rptr->offset
-			     - ((unsigned long) dev->sg->virtual)
-			     + dev_priv->gart_vm_start);
-	}
-
-	/* Set ring buffer size */
-#ifdef __BIG_ENDIAN
-	RADEON_WRITE(RADEON_CP_RB_CNTL,
-		     RADEON_BUF_SWAP_32BIT |
-		     (dev_priv->ring.fetch_size_l2ow << 18) |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#else
-	RADEON_WRITE(RADEON_CP_RB_CNTL,
-		     (dev_priv->ring.fetch_size_l2ow << 18) |
-		     (dev_priv->ring.rptr_update_l2qw << 8) |
-		     dev_priv->ring.size_l2qw);
-#endif
-
-
-	/* Initialize the scratch register pointer.  This will cause
-	 * the scratch register values to be written out to memory
-	 * whenever they are updated.
-	 *
-	 * We simply put this behind the ring read pointer, this works
-	 * with PCI GART as well as (whatever kind of) AGP GART
-	 */
-	RADEON_WRITE(RADEON_SCRATCH_ADDR, RADEON_READ(RADEON_CP_RB_RPTR_ADDR)
-		     + RADEON_SCRATCH_REG_OFFSET);
-
-	RADEON_WRITE(RADEON_SCRATCH_UMSK, 0x7);
-
-	radeon_enable_bm(dev_priv);
-
-	radeon_write_ring_rptr(dev_priv, RADEON_SCRATCHOFF(0), 0);
-	RADEON_WRITE(RADEON_LAST_FRAME_REG, 0);
-
-	radeon_write_ring_rptr(dev_priv, RADEON_SCRATCHOFF(1), 0);
-	RADEON_WRITE(RADEON_LAST_DISPATCH_REG, 0);
-
-	radeon_write_ring_rptr(dev_priv, RADEON_SCRATCHOFF(2), 0);
-	RADEON_WRITE(RADEON_LAST_CLEAR_REG, 0);
-
-	/* reset sarea copies of these */
-	master_priv = file_priv->master->driver_priv;
-	if (master_priv->sarea_priv) {
-		master_priv->sarea_priv->last_frame = 0;
-		master_priv->sarea_priv->last_dispatch = 0;
-		master_priv->sarea_priv->last_clear = 0;
-	}
-
-	radeon_do_wait_for_idle(dev_priv);
-
-	/* Sync everything up */
-	RADEON_WRITE(RADEON_ISYNC_CNTL,
-		     (RADEON_ISYNC_ANY2D_IDLE3D |
-		      RADEON_ISYNC_ANY3D_IDLE2D |
-		      RADEON_ISYNC_WAIT_IDLEGUI |
-		      RADEON_ISYNC_CPSCRATCH_IDLEGUI));
-
-}
-
-static void radeon_test_writeback(drm_radeon_private_t * dev_priv)
-{
-	u32 tmp;
-
-	/* Start with assuming that writeback doesn't work */
-	dev_priv->writeback_works = 0;
-
-	/* Writeback doesn't seem to work everywhere, test it here and possibly
-	 * enable it if it appears to work
-	 */
-	radeon_write_ring_rptr(dev_priv, RADEON_SCRATCHOFF(1), 0);
-
-	RADEON_WRITE(RADEON_SCRATCH_REG1, 0xdeadbeef);
-
-	for (tmp = 0; tmp < dev_priv->usec_timeout; tmp++) {
-		u32 val;
-
-		val = radeon_read_ring_rptr(dev_priv, RADEON_SCRATCHOFF(1));
-		if (val == 0xdeadbeef)
-			break;
-		DRM_UDELAY(1);
-	}
-
-	if (tmp < dev_priv->usec_timeout) {
-		dev_priv->writeback_works = 1;
-		DRM_INFO("writeback test succeeded in %d usecs\n", tmp);
-	} else {
-		dev_priv->writeback_works = 0;
-		DRM_INFO("writeback test failed\n");
-	}
-	if (radeon_no_wb == 1) {
-		dev_priv->writeback_works = 0;
-		DRM_INFO("writeback forced off\n");
-	}
-
-	if (!dev_priv->writeback_works) {
-		/* Disable writeback to avoid unnecessary bus master transfer */
-		RADEON_WRITE(RADEON_CP_RB_CNTL, RADEON_READ(RADEON_CP_RB_CNTL) |
-			     RADEON_RB_NO_UPDATE);
-		RADEON_WRITE(RADEON_SCRATCH_UMSK, 0);
-	}
-}
-
-/* Enable or disable IGP GART on the chip */
-static void radeon_set_igpgart(drm_radeon_private_t * dev_priv, int on)
-{
-	u32 temp;
-
-	if (on) {
-		DRM_DEBUG("programming igp gart %08X %08lX %08X\n",
-			  dev_priv->gart_vm_start,
-			  (long)dev_priv->gart_info.bus_addr,
-			  dev_priv->gart_size);
-
-		temp = IGP_READ_MCIND(dev_priv, RS480_MC_MISC_CNTL);
-		if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-		    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))
-			IGP_WRITE_MCIND(RS480_MC_MISC_CNTL, (RS480_GART_INDEX_REG_EN |
-							     RS690_BLOCK_GFX_D3_EN));
-		else
-			IGP_WRITE_MCIND(RS480_MC_MISC_CNTL, RS480_GART_INDEX_REG_EN);
-
-		IGP_WRITE_MCIND(RS480_AGP_ADDRESS_SPACE_SIZE, (RS480_GART_EN |
-							       RS480_VA_SIZE_32MB));
-
-		temp = IGP_READ_MCIND(dev_priv, RS480_GART_FEATURE_ID);
-		IGP_WRITE_MCIND(RS480_GART_FEATURE_ID, (RS480_HANG_EN |
-							RS480_TLB_ENABLE |
-							RS480_GTW_LAC_EN |
-							RS480_1LEVEL_GART));
-
-		temp = dev_priv->gart_info.bus_addr & 0xfffff000;
-		temp |= (upper_32_bits(dev_priv->gart_info.bus_addr) & 0xff) << 4;
-		IGP_WRITE_MCIND(RS480_GART_BASE, temp);
-
-		temp = IGP_READ_MCIND(dev_priv, RS480_AGP_MODE_CNTL);
-		IGP_WRITE_MCIND(RS480_AGP_MODE_CNTL, ((1 << RS480_REQ_TYPE_SNOOP_SHIFT) |
-						      RS480_REQ_TYPE_SNOOP_DIS));
-
-		radeon_write_agp_base(dev_priv, dev_priv->gart_vm_start);
-
-		dev_priv->gart_size = 32*1024*1024;
-		temp = (((dev_priv->gart_vm_start - 1 + dev_priv->gart_size) &
-			 0xffff0000) | (dev_priv->gart_vm_start >> 16));
-
-		radeon_write_agp_location(dev_priv, temp);
-
-		temp = IGP_READ_MCIND(dev_priv, RS480_AGP_ADDRESS_SPACE_SIZE);
-		IGP_WRITE_MCIND(RS480_AGP_ADDRESS_SPACE_SIZE, (RS480_GART_EN |
-							       RS480_VA_SIZE_32MB));
-
-		do {
-			temp = IGP_READ_MCIND(dev_priv, RS480_GART_CACHE_CNTRL);
-			if ((temp & RS480_GART_CACHE_INVALIDATE) == 0)
-				break;
-			DRM_UDELAY(1);
-		} while (1);
-
-		IGP_WRITE_MCIND(RS480_GART_CACHE_CNTRL,
-				RS480_GART_CACHE_INVALIDATE);
-
-		do {
-			temp = IGP_READ_MCIND(dev_priv, RS480_GART_CACHE_CNTRL);
-			if ((temp & RS480_GART_CACHE_INVALIDATE) == 0)
-				break;
-			DRM_UDELAY(1);
-		} while (1);
-
-		IGP_WRITE_MCIND(RS480_GART_CACHE_CNTRL, 0);
-	} else {
-		IGP_WRITE_MCIND(RS480_AGP_ADDRESS_SPACE_SIZE, 0);
-	}
-}
-
-/* Enable or disable IGP GART on the chip */
-static void rs600_set_igpgart(drm_radeon_private_t *dev_priv, int on)
-{
-	u32 temp;
-	int i;
-
-	if (on) {
-		DRM_DEBUG("programming igp gart %08X %08lX %08X\n",
-			 dev_priv->gart_vm_start,
-			 (long)dev_priv->gart_info.bus_addr,
-			 dev_priv->gart_size);
-
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, (RS600_EFFECTIVE_L2_CACHE_SIZE(6) |
-						    RS600_EFFECTIVE_L2_QUEUE_SIZE(6)));
-
-		for (i = 0; i < 19; i++)
-			IGP_WRITE_MCIND(RS600_MC_PT0_CLIENT0_CNTL + i,
-					(RS600_ENABLE_TRANSLATION_MODE_OVERRIDE |
-					 RS600_SYSTEM_ACCESS_MODE_IN_SYS |
-					 RS600_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASSTHROUGH |
-					 RS600_EFFECTIVE_L1_CACHE_SIZE(3) |
-					 RS600_ENABLE_FRAGMENT_PROCESSING |
-					 RS600_EFFECTIVE_L1_QUEUE_SIZE(3)));
-
-		IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_CNTL, (RS600_ENABLE_PAGE_TABLE |
-							     RS600_PAGE_TABLE_TYPE_FLAT));
-
-		/* disable all other contexts */
-		for (i = 1; i < 8; i++)
-			IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_CNTL + i, 0);
-
-		/* setup the page table aperture */
-		IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_FLAT_BASE_ADDR,
-				dev_priv->gart_info.bus_addr);
-		IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_FLAT_START_ADDR,
-				dev_priv->gart_vm_start);
-		IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_FLAT_END_ADDR,
-				(dev_priv->gart_vm_start + dev_priv->gart_size - 1));
-		IGP_WRITE_MCIND(RS600_MC_PT0_CONTEXT0_DEFAULT_READ_ADDR, 0);
-
-		/* setup the system aperture */
-		IGP_WRITE_MCIND(RS600_MC_PT0_SYSTEM_APERTURE_LOW_ADDR,
-				dev_priv->gart_vm_start);
-		IGP_WRITE_MCIND(RS600_MC_PT0_SYSTEM_APERTURE_HIGH_ADDR,
-				(dev_priv->gart_vm_start + dev_priv->gart_size - 1));
-
-		/* enable page tables */
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_PT0_CNTL);
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, (temp | RS600_ENABLE_PT));
-
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_CNTL1);
-		IGP_WRITE_MCIND(RS600_MC_CNTL1, (temp | RS600_ENABLE_PAGE_TABLES));
-
-		/* invalidate the cache */
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_PT0_CNTL);
-
-		temp &= ~(RS600_INVALIDATE_ALL_L1_TLBS | RS600_INVALIDATE_L2_CACHE);
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, temp);
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_PT0_CNTL);
-
-		temp |= RS600_INVALIDATE_ALL_L1_TLBS | RS600_INVALIDATE_L2_CACHE;
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, temp);
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_PT0_CNTL);
-
-		temp &= ~(RS600_INVALIDATE_ALL_L1_TLBS | RS600_INVALIDATE_L2_CACHE);
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, temp);
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_PT0_CNTL);
-
-	} else {
-		IGP_WRITE_MCIND(RS600_MC_PT0_CNTL, 0);
-		temp = IGP_READ_MCIND(dev_priv, RS600_MC_CNTL1);
-		temp &= ~RS600_ENABLE_PAGE_TABLES;
-		IGP_WRITE_MCIND(RS600_MC_CNTL1, temp);
-	}
-}
-
-static void radeon_set_pciegart(drm_radeon_private_t * dev_priv, int on)
-{
-	u32 tmp = RADEON_READ_PCIE(dev_priv, RADEON_PCIE_TX_GART_CNTL);
-	if (on) {
-
-		DRM_DEBUG("programming pcie %08X %08lX %08X\n",
-			  dev_priv->gart_vm_start,
-			  (long)dev_priv->gart_info.bus_addr,
-			  dev_priv->gart_size);
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_DISCARD_RD_ADDR_LO,
-				  dev_priv->gart_vm_start);
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_GART_BASE,
-				  dev_priv->gart_info.bus_addr);
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_GART_START_LO,
-				  dev_priv->gart_vm_start);
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_GART_END_LO,
-				  dev_priv->gart_vm_start +
-				  dev_priv->gart_size - 1);
-
-		radeon_write_agp_location(dev_priv, 0xffffffc0); /* ?? */
-
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_GART_CNTL,
-				  RADEON_PCIE_TX_GART_EN);
-	} else {
-		RADEON_WRITE_PCIE(RADEON_PCIE_TX_GART_CNTL,
-				  tmp & ~RADEON_PCIE_TX_GART_EN);
-	}
-}
-
-/* Enable or disable PCI GART on the chip */
-static void radeon_set_pcigart(drm_radeon_private_t * dev_priv, int on)
-{
-	u32 tmp;
-
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740) ||
-	    (dev_priv->flags & RADEON_IS_IGPGART)) {
-		radeon_set_igpgart(dev_priv, on);
-		return;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600) {
-		rs600_set_igpgart(dev_priv, on);
-		return;
-	}
-
-	if (dev_priv->flags & RADEON_IS_PCIE) {
-		radeon_set_pciegart(dev_priv, on);
-		return;
-	}
-
-	tmp = RADEON_READ(RADEON_AIC_CNTL);
-
-	if (on) {
-		RADEON_WRITE(RADEON_AIC_CNTL,
-			     tmp | RADEON_PCIGART_TRANSLATE_EN);
-
-		/* set PCI GART page-table base address
-		 */
-		RADEON_WRITE(RADEON_AIC_PT_BASE, dev_priv->gart_info.bus_addr);
-
-		/* set address range for PCI address translate
-		 */
-		RADEON_WRITE(RADEON_AIC_LO_ADDR, dev_priv->gart_vm_start);
-		RADEON_WRITE(RADEON_AIC_HI_ADDR, dev_priv->gart_vm_start
-			     + dev_priv->gart_size - 1);
-
-		/* Turn off AGP aperture -- is this required for PCI GART?
-		 */
-		radeon_write_agp_location(dev_priv, 0xffffffc0);
-		RADEON_WRITE(RADEON_AGP_COMMAND, 0);	/* clear AGP_COMMAND */
-	} else {
-		RADEON_WRITE(RADEON_AIC_CNTL,
-			     tmp & ~RADEON_PCIGART_TRANSLATE_EN);
-	}
-}
-
-static int radeon_setup_pcigart_surface(drm_radeon_private_t *dev_priv)
-{
-	struct drm_ati_pcigart_info *gart_info = &dev_priv->gart_info;
-	struct radeon_virt_surface *vp;
-	int i;
-
-	for (i = 0; i < RADEON_MAX_SURFACES * 2; i++) {
-		if (!dev_priv->virt_surfaces[i].file_priv ||
-		    dev_priv->virt_surfaces[i].file_priv == PCIGART_FILE_PRIV)
-			break;
-	}
-	if (i >= 2 * RADEON_MAX_SURFACES)
-		return -ENOMEM;
-	vp = &dev_priv->virt_surfaces[i];
-
-	for (i = 0; i < RADEON_MAX_SURFACES; i++) {
-		struct radeon_surface *sp = &dev_priv->surfaces[i];
-		if (sp->refcount)
-			continue;
-
-		vp->surface_index = i;
-		vp->lower = gart_info->bus_addr;
-		vp->upper = vp->lower + gart_info->table_size;
-		vp->flags = 0;
-		vp->file_priv = PCIGART_FILE_PRIV;
-
-		sp->refcount = 1;
-		sp->lower = vp->lower;
-		sp->upper = vp->upper;
-		sp->flags = 0;
-
-		RADEON_WRITE(RADEON_SURFACE0_INFO + 16 * i, sp->flags);
-		RADEON_WRITE(RADEON_SURFACE0_LOWER_BOUND + 16 * i, sp->lower);
-		RADEON_WRITE(RADEON_SURFACE0_UPPER_BOUND + 16 * i, sp->upper);
-		return 0;
-	}
-
-	return -ENOMEM;
-}
-
-static int radeon_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
-			     struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-
-	DRM_DEBUG("\n");
-
-	/* if we require new memory map but we don't have it fail */
-	if ((dev_priv->flags & RADEON_NEW_MEMMAP) && !dev_priv->new_memmap) {
-		DRM_ERROR("Cannot initialise DRM on this card\nThis card requires a new X.org DDX for 3D\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	if (init->is_pci && (dev_priv->flags & RADEON_IS_AGP)) {
-		DRM_DEBUG("Forcing AGP card to PCI mode\n");
-		dev_priv->flags &= ~RADEON_IS_AGP;
-	} else if (!(dev_priv->flags & (RADEON_IS_AGP | RADEON_IS_PCI | RADEON_IS_PCIE))
-		   && !init->is_pci) {
-		DRM_DEBUG("Restoring AGP flag\n");
-		dev_priv->flags |= RADEON_IS_AGP;
-	}
-
-	if ((!(dev_priv->flags & RADEON_IS_AGP)) && !dev->sg) {
-		DRM_ERROR("PCI GART memory not allocated!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	dev_priv->usec_timeout = init->usec_timeout;
-	if (dev_priv->usec_timeout < 1 ||
-	    dev_priv->usec_timeout > RADEON_MAX_USEC_TIMEOUT) {
-		DRM_DEBUG("TIMEOUT problem!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	/* Enable vblank on CRTC1 for older X servers
-	 */
-	dev_priv->vblank_crtc = DRM_RADEON_VBLANK_CRTC1;
-
-	switch(init->func) {
-	case RADEON_INIT_R200_CP:
-		dev_priv->microcode_version = UCODE_R200;
-		break;
-	case RADEON_INIT_R300_CP:
-		dev_priv->microcode_version = UCODE_R300;
-		break;
-	default:
-		dev_priv->microcode_version = UCODE_R100;
-	}
-
-	dev_priv->do_boxes = 0;
-	dev_priv->cp_mode = init->cp_mode;
-
-	/* We don't support anything other than bus-mastering ring mode,
-	 * but the ring can be in either AGP or PCI space for the ring
-	 * read pointer.
-	 */
-	if ((init->cp_mode != RADEON_CSQ_PRIBM_INDDIS) &&
-	    (init->cp_mode != RADEON_CSQ_PRIBM_INDBM)) {
-		DRM_DEBUG("BAD cp_mode (%x)!\n", init->cp_mode);
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	switch (init->fb_bpp) {
-	case 16:
-		dev_priv->color_fmt = RADEON_COLOR_FORMAT_RGB565;
-		break;
-	case 32:
-	default:
-		dev_priv->color_fmt = RADEON_COLOR_FORMAT_ARGB8888;
-		break;
-	}
-	dev_priv->front_offset = init->front_offset;
-	dev_priv->front_pitch = init->front_pitch;
-	dev_priv->back_offset = init->back_offset;
-	dev_priv->back_pitch = init->back_pitch;
-
-	switch (init->depth_bpp) {
-	case 16:
-		dev_priv->depth_fmt = RADEON_DEPTH_FORMAT_16BIT_INT_Z;
-		break;
-	case 32:
-	default:
-		dev_priv->depth_fmt = RADEON_DEPTH_FORMAT_24BIT_INT_Z;
-		break;
-	}
-	dev_priv->depth_offset = init->depth_offset;
-	dev_priv->depth_pitch = init->depth_pitch;
-
-	/* Hardware state for depth clears.  Remove this if/when we no
-	 * longer clear the depth buffer with a 3D rectangle.  Hard-code
-	 * all values to prevent unwanted 3D state from slipping through
-	 * and screwing with the clear operation.
-	 */
-	dev_priv->depth_clear.rb3d_cntl = (RADEON_PLANE_MASK_ENABLE |
-					   (dev_priv->color_fmt << 10) |
-					   (dev_priv->microcode_version ==
-					    UCODE_R100 ? RADEON_ZBLOCK16 : 0));
-
-	dev_priv->depth_clear.rb3d_zstencilcntl =
-	    (dev_priv->depth_fmt |
-	     RADEON_Z_TEST_ALWAYS |
-	     RADEON_STENCIL_TEST_ALWAYS |
-	     RADEON_STENCIL_S_FAIL_REPLACE |
-	     RADEON_STENCIL_ZPASS_REPLACE |
-	     RADEON_STENCIL_ZFAIL_REPLACE | RADEON_Z_WRITE_ENABLE);
-
-	dev_priv->depth_clear.se_cntl = (RADEON_FFACE_CULL_CW |
-					 RADEON_BFACE_SOLID |
-					 RADEON_FFACE_SOLID |
-					 RADEON_FLAT_SHADE_VTX_LAST |
-					 RADEON_DIFFUSE_SHADE_FLAT |
-					 RADEON_ALPHA_SHADE_FLAT |
-					 RADEON_SPECULAR_SHADE_FLAT |
-					 RADEON_FOG_SHADE_FLAT |
-					 RADEON_VTX_PIX_CENTER_OGL |
-					 RADEON_ROUND_MODE_TRUNC |
-					 RADEON_ROUND_PREC_8TH_PIX);
-
-
-	dev_priv->ring_offset = init->ring_offset;
-	dev_priv->ring_rptr_offset = init->ring_rptr_offset;
-	dev_priv->buffers_offset = init->buffers_offset;
-	dev_priv->gart_textures_offset = init->gart_textures_offset;
-
-	master_priv->sarea = drm_legacy_getsarea(dev);
-	if (!master_priv->sarea) {
-		DRM_ERROR("could not find sarea!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	dev_priv->cp_ring = drm_legacy_findmap(dev, init->ring_offset);
-	if (!dev_priv->cp_ring) {
-		DRM_ERROR("could not find cp ring region!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-	dev_priv->ring_rptr = drm_legacy_findmap(dev, init->ring_rptr_offset);
-	if (!dev_priv->ring_rptr) {
-		DRM_ERROR("could not find ring read pointer!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
-	if (!dev->agp_buffer_map) {
-		DRM_ERROR("could not find dma buffer region!\n");
-		radeon_do_cleanup_cp(dev);
-		return -EINVAL;
-	}
-
-	if (init->gart_textures_offset) {
-		dev_priv->gart_textures =
-		    drm_legacy_findmap(dev, init->gart_textures_offset);
-		if (!dev_priv->gart_textures) {
-			DRM_ERROR("could not find GART texture region!\n");
-			radeon_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-	}
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		drm_legacy_ioremap_wc(dev_priv->cp_ring, dev);
-		drm_legacy_ioremap_wc(dev_priv->ring_rptr, dev);
-		drm_legacy_ioremap_wc(dev->agp_buffer_map, dev);
-		if (!dev_priv->cp_ring->handle ||
-		    !dev_priv->ring_rptr->handle ||
-		    !dev->agp_buffer_map->handle) {
-			DRM_ERROR("could not find ioremap agp regions!\n");
-			radeon_do_cleanup_cp(dev);
-			return -EINVAL;
-		}
-	} else
-#endif
-	{
-		dev_priv->cp_ring->handle =
-			(void *)(unsigned long)dev_priv->cp_ring->offset;
-		dev_priv->ring_rptr->handle =
-			(void *)(unsigned long)dev_priv->ring_rptr->offset;
-		dev->agp_buffer_map->handle =
-			(void *)(unsigned long)dev->agp_buffer_map->offset;
-
-		DRM_DEBUG("dev_priv->cp_ring->handle %p\n",
-			  dev_priv->cp_ring->handle);
-		DRM_DEBUG("dev_priv->ring_rptr->handle %p\n",
-			  dev_priv->ring_rptr->handle);
-		DRM_DEBUG("dev->agp_buffer_map->handle %p\n",
-			  dev->agp_buffer_map->handle);
-	}
-
-	dev_priv->fb_location = (radeon_read_fb_location(dev_priv) & 0xffff) << 16;
-	dev_priv->fb_size =
-		((radeon_read_fb_location(dev_priv) & 0xffff0000u) + 0x10000)
-		- dev_priv->fb_location;
-
-	dev_priv->front_pitch_offset = (((dev_priv->front_pitch / 64) << 22) |
-					((dev_priv->front_offset
-					  + dev_priv->fb_location) >> 10));
-
-	dev_priv->back_pitch_offset = (((dev_priv->back_pitch / 64) << 22) |
-				       ((dev_priv->back_offset
-					 + dev_priv->fb_location) >> 10));
-
-	dev_priv->depth_pitch_offset = (((dev_priv->depth_pitch / 64) << 22) |
-					((dev_priv->depth_offset
-					  + dev_priv->fb_location) >> 10));
-
-	dev_priv->gart_size = init->gart_size;
-
-	/* New let's set the memory map ... */
-	if (dev_priv->new_memmap) {
-		u32 base = 0;
-
-		DRM_INFO("Setting GART location based on new memory map\n");
-
-		/* If using AGP, try to locate the AGP aperture at the same
-		 * location in the card and on the bus, though we have to
-		 * align it down.
-		 */
-#if IS_ENABLED(CONFIG_AGP)
-		if (dev_priv->flags & RADEON_IS_AGP) {
-			base = dev->agp->base;
-			/* Check if valid */
-			if ((base + dev_priv->gart_size - 1) >= dev_priv->fb_location &&
-			    base < (dev_priv->fb_location + dev_priv->fb_size - 1)) {
-				DRM_INFO("Can't use AGP base @0x%08lx, won't fit\n",
-					 dev->agp->base);
-				base = 0;
-			}
-		}
-#endif
-		/* If not or if AGP is at 0 (Macs), try to put it elsewhere */
-		if (base == 0) {
-			base = dev_priv->fb_location + dev_priv->fb_size;
-			if (base < dev_priv->fb_location ||
-			    ((base + dev_priv->gart_size) & 0xfffffffful) < base)
-				base = dev_priv->fb_location
-					- dev_priv->gart_size;
-		}
-		dev_priv->gart_vm_start = base & 0xffc00000u;
-		if (dev_priv->gart_vm_start != base)
-			DRM_INFO("GART aligned down from 0x%08x to 0x%08x\n",
-				 base, dev_priv->gart_vm_start);
-	} else {
-		DRM_INFO("Setting GART location based on old memory map\n");
-		dev_priv->gart_vm_start = dev_priv->fb_location +
-			RADEON_READ(RADEON_CONFIG_APER_SIZE);
-	}
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP)
-		dev_priv->gart_buffers_offset = (dev->agp_buffer_map->offset
-						 - dev->agp->base
-						 + dev_priv->gart_vm_start);
-	else
-#endif
-		dev_priv->gart_buffers_offset = (dev->agp_buffer_map->offset
-					- (unsigned long)dev->sg->virtual
-					+ dev_priv->gart_vm_start);
-
-	DRM_DEBUG("dev_priv->gart_size %d\n", dev_priv->gart_size);
-	DRM_DEBUG("dev_priv->gart_vm_start 0x%x\n", dev_priv->gart_vm_start);
-	DRM_DEBUG("dev_priv->gart_buffers_offset 0x%lx\n",
-		  dev_priv->gart_buffers_offset);
-
-	dev_priv->ring.start = (u32 *) dev_priv->cp_ring->handle;
-	dev_priv->ring.end = ((u32 *) dev_priv->cp_ring->handle
-			      + init->ring_size / sizeof(u32));
-	dev_priv->ring.size = init->ring_size;
-	dev_priv->ring.size_l2qw = order_base_2(init->ring_size / 8);
-
-	dev_priv->ring.rptr_update = /* init->rptr_update */ 4096;
-	dev_priv->ring.rptr_update_l2qw = order_base_2( /* init->rptr_update */ 4096 / 8);
-
-	dev_priv->ring.fetch_size = /* init->fetch_size */ 32;
-	dev_priv->ring.fetch_size_l2ow = order_base_2( /* init->fetch_size */ 32 / 16);
-	dev_priv->ring.tail_mask = (dev_priv->ring.size / sizeof(u32)) - 1;
-
-	dev_priv->ring.high_mark = RADEON_RING_HIGH_MARK;
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		/* Turn off PCI GART */
-		radeon_set_pcigart(dev_priv, 0);
-	} else
-#endif
-	{
-		u32 sctrl;
-		int ret;
-
-		dev_priv->gart_info.table_mask = DMA_BIT_MASK(32);
-		/* if we have an offset set from userspace */
-		if (dev_priv->pcigart_offset_set) {
-			dev_priv->gart_info.bus_addr =
-				(resource_size_t)dev_priv->pcigart_offset + dev_priv->fb_location;
-			dev_priv->gart_info.mapping.offset =
-			    dev_priv->pcigart_offset + dev_priv->fb_aper_offset;
-			dev_priv->gart_info.mapping.size =
-			    dev_priv->gart_info.table_size;
-
-			drm_legacy_ioremap_wc(&dev_priv->gart_info.mapping, dev);
-			dev_priv->gart_info.addr =
-			    dev_priv->gart_info.mapping.handle;
-
-			if (dev_priv->flags & RADEON_IS_PCIE)
-				dev_priv->gart_info.gart_reg_if = DRM_ATI_GART_PCIE;
-			else
-				dev_priv->gart_info.gart_reg_if = DRM_ATI_GART_PCI;
-			dev_priv->gart_info.gart_table_location =
-			    DRM_ATI_GART_FB;
-
-			DRM_DEBUG("Setting phys_pci_gart to %p %08lX\n",
-				  dev_priv->gart_info.addr,
-				  dev_priv->pcigart_offset);
-		} else {
-			if (dev_priv->flags & RADEON_IS_IGPGART)
-				dev_priv->gart_info.gart_reg_if = DRM_ATI_GART_IGP;
-			else
-				dev_priv->gart_info.gart_reg_if = DRM_ATI_GART_PCI;
-			dev_priv->gart_info.gart_table_location =
-			    DRM_ATI_GART_MAIN;
-			dev_priv->gart_info.addr = NULL;
-			dev_priv->gart_info.bus_addr = 0;
-			if (dev_priv->flags & RADEON_IS_PCIE) {
-				DRM_ERROR
-				    ("Cannot use PCI Express without GART in FB memory\n");
-				radeon_do_cleanup_cp(dev);
-				return -EINVAL;
-			}
-		}
-
-		sctrl = RADEON_READ(RADEON_SURFACE_CNTL);
-		RADEON_WRITE(RADEON_SURFACE_CNTL, 0);
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-			ret = r600_page_table_init(dev);
-		else
-			ret = drm_ati_pcigart_init(dev, &dev_priv->gart_info);
-		RADEON_WRITE(RADEON_SURFACE_CNTL, sctrl);
-
-		if (!ret) {
-			DRM_ERROR("failed to init PCI GART!\n");
-			radeon_do_cleanup_cp(dev);
-			return -ENOMEM;
-		}
-
-		ret = radeon_setup_pcigart_surface(dev_priv);
-		if (ret) {
-			DRM_ERROR("failed to setup GART surface!\n");
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-				r600_page_table_cleanup(dev, &dev_priv->gart_info);
-			else
-				drm_ati_pcigart_cleanup(dev, &dev_priv->gart_info);
-			radeon_do_cleanup_cp(dev);
-			return ret;
-		}
-
-		/* Turn on PCI GART */
-		radeon_set_pcigart(dev_priv, 1);
-	}
-
-	if (!dev_priv->me_fw) {
-		int err = radeon_cp_init_microcode(dev_priv);
-		if (err) {
-			DRM_ERROR("Failed to load firmware!\n");
-			radeon_do_cleanup_cp(dev);
-			return err;
-		}
-	}
-	radeon_cp_load_microcode(dev_priv);
-	radeon_cp_init_ring_buffer(dev, dev_priv, file_priv);
-
-	dev_priv->last_buf = 0;
-
-	radeon_do_engine_reset(dev);
-	radeon_test_writeback(dev_priv);
-
-	return 0;
-}
-
-static int radeon_do_cleanup_cp(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	/* Make sure interrupts are disabled here because the uninstall ioctl
-	 * may not have been called from userspace and after dev_private
-	 * is freed, it's too late.
-	 */
-	if (dev->irq_enabled)
-		drm_irq_uninstall(dev);
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		if (dev_priv->cp_ring != NULL) {
-			drm_legacy_ioremapfree(dev_priv->cp_ring, dev);
-			dev_priv->cp_ring = NULL;
-		}
-		if (dev_priv->ring_rptr != NULL) {
-			drm_legacy_ioremapfree(dev_priv->ring_rptr, dev);
-			dev_priv->ring_rptr = NULL;
-		}
-		if (dev->agp_buffer_map != NULL) {
-			drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
-			dev->agp_buffer_map = NULL;
-		}
-	} else
-#endif
-	{
-
-		if (dev_priv->gart_info.bus_addr) {
-			/* Turn off PCI GART */
-			radeon_set_pcigart(dev_priv, 0);
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)
-				r600_page_table_cleanup(dev, &dev_priv->gart_info);
-			else {
-				if (!drm_ati_pcigart_cleanup(dev, &dev_priv->gart_info))
-					DRM_ERROR("failed to cleanup PCI GART!\n");
-			}
-		}
-
-		if (dev_priv->gart_info.gart_table_location == DRM_ATI_GART_FB)
-		{
-			drm_legacy_ioremapfree(&dev_priv->gart_info.mapping, dev);
-			dev_priv->gart_info.addr = NULL;
-		}
-	}
-	/* only clear to the start of flags */
-	memset(dev_priv, 0, offsetof(drm_radeon_private_t, flags));
-
-	return 0;
-}
-
-/* This code will reinit the Radeon CP hardware after a resume from disc.
- * AFAIK, it would be very difficult to pickle the state at suspend time, so
- * here we make sure that all Radeon hardware initialisation is re-done without
- * affecting running applications.
- *
- * Charl P. Botha <http://cpbotha.net>
- */
-static int radeon_do_resume_cp(struct drm_device *dev, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if (!dev_priv) {
-		DRM_ERROR("Called with no initialization\n");
-		return -EINVAL;
-	}
-
-	DRM_DEBUG("Starting radeon_do_resume_cp()\n");
-
-#if IS_ENABLED(CONFIG_AGP)
-	if (dev_priv->flags & RADEON_IS_AGP) {
-		/* Turn off PCI GART */
-		radeon_set_pcigart(dev_priv, 0);
-	} else
-#endif
-	{
-		/* Turn on PCI GART */
-		radeon_set_pcigart(dev_priv, 1);
-	}
-
-	radeon_cp_load_microcode(dev_priv);
-	radeon_cp_init_ring_buffer(dev, dev_priv, file_priv);
-
-	dev_priv->have_z_offset = 0;
-	radeon_do_engine_reset(dev);
-	radeon_irq_set_state(dev, RADEON_SW_INT_ENABLE, 1);
-
-	DRM_DEBUG("radeon_do_resume_cp() complete\n");
-
-	return 0;
-}
-
-int radeon_cp_init(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_init_t *init = data;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (init->func == RADEON_INIT_R300_CP)
-		r300_init_reg_flags(dev);
-
-	switch (init->func) {
-	case RADEON_INIT_CP:
-	case RADEON_INIT_R200_CP:
-	case RADEON_INIT_R300_CP:
-		return radeon_do_init_cp(dev, init, file_priv);
-	case RADEON_INIT_R600_CP:
-		return r600_do_init_cp(dev, init, file_priv);
-	case RADEON_CLEANUP_CP:
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			return r600_do_cleanup_cp(dev);
-		else
-			return radeon_do_cleanup_cp(dev);
-	}
-
-	return -EINVAL;
-}
-
-int radeon_cp_start(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (dev_priv->cp_running) {
-		DRM_DEBUG("while CP running\n");
-		return 0;
-	}
-	if (dev_priv->cp_mode == RADEON_CSQ_PRIDIS_INDDIS) {
-		DRM_DEBUG("called with bogus CP mode (%d)\n",
-			  dev_priv->cp_mode);
-		return 0;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_do_cp_start(dev_priv);
-	else
-		radeon_do_cp_start(dev_priv);
-
-	return 0;
-}
-
-/* Stop the CP.  The engine must have been idled before calling this
- * routine.
- */
-int radeon_cp_stop(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_cp_stop_t *stop = data;
-	int ret;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (!dev_priv->cp_running)
-		return 0;
-
-	/* Flush any pending CP commands.  This ensures any outstanding
-	 * commands are exectuted by the engine before we turn it off.
-	 */
-	if (stop->flush) {
-		radeon_do_cp_flush(dev_priv);
-	}
-
-	/* If we fail to make the engine go idle, we return an error
-	 * code so that the DRM ioctl wrapper can try again.
-	 */
-	if (stop->idle) {
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			ret = r600_do_cp_idle(dev_priv);
-		else
-			ret = radeon_do_cp_idle(dev_priv);
-		if (ret)
-			return ret;
-	}
-
-	/* Finally, we can turn off the CP.  If the engine isn't idle,
-	 * we will get some dropped triangles as they won't be fully
-	 * rendered before the CP is shut down.
-	 */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_do_cp_stop(dev_priv);
-	else
-		radeon_do_cp_stop(dev_priv);
-
-	/* Reset the engine */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_do_engine_reset(dev);
-	else
-		radeon_do_engine_reset(dev);
-
-	return 0;
-}
-
-void radeon_do_release(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int i, ret;
-
-	if (dev_priv) {
-		if (dev_priv->cp_running) {
-			/* Stop the cp */
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) {
-				while ((ret = r600_do_cp_idle(dev_priv)) != 0) {
-					DRM_DEBUG("radeon_do_cp_idle %d\n", ret);
-#ifdef __linux__
-					schedule();
-#else
-					tsleep(&ret, PZERO, "rdnrel", 1);
-#endif
-				}
-			} else {
-				while ((ret = radeon_do_cp_idle(dev_priv)) != 0) {
-					DRM_DEBUG("radeon_do_cp_idle %d\n", ret);
-#ifdef __linux__
-					schedule();
-#else
-					tsleep(&ret, PZERO, "rdnrel", 1);
-#endif
-				}
-			}
-			if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) {
-				r600_do_cp_stop(dev_priv);
-				r600_do_engine_reset(dev);
-			} else {
-				radeon_do_cp_stop(dev_priv);
-				radeon_do_engine_reset(dev);
-			}
-		}
-
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) < CHIP_R600) {
-			/* Disable *all* interrupts */
-			if (dev_priv->mmio)	/* remove this after permanent addmaps */
-				RADEON_WRITE(RADEON_GEN_INT_CNTL, 0);
-
-			if (dev_priv->mmio) {	/* remove all surfaces */
-				for (i = 0; i < RADEON_MAX_SURFACES; i++) {
-					RADEON_WRITE(RADEON_SURFACE0_INFO + 16 * i, 0);
-					RADEON_WRITE(RADEON_SURFACE0_LOWER_BOUND +
-						     16 * i, 0);
-					RADEON_WRITE(RADEON_SURFACE0_UPPER_BOUND +
-						     16 * i, 0);
-				}
-			}
-		}
-
-		/* Free memory heap structures */
-		radeon_mem_takedown(&(dev_priv->gart_heap));
-		radeon_mem_takedown(&(dev_priv->fb_heap));
-
-		/* deallocate kernel resources */
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			r600_do_cleanup_cp(dev);
-		else
-			radeon_do_cleanup_cp(dev);
-		release_firmware(dev_priv->me_fw);
-		dev_priv->me_fw = NULL;
-		release_firmware(dev_priv->pfp_fw);
-		dev_priv->pfp_fw = NULL;
-	}
-}
-
-/* Just reset the CP ring.  Called as part of an X Server engine reset.
- */
-int radeon_cp_reset(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (!dev_priv) {
-		DRM_DEBUG("called before init done\n");
-		return -EINVAL;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_do_cp_reset(dev_priv);
-	else
-		radeon_do_cp_reset(dev_priv);
-
-	/* The CP is no longer running after an engine reset */
-	dev_priv->cp_running = 0;
-
-	return 0;
-}
-
-int radeon_cp_idle(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return r600_do_cp_idle(dev_priv);
-	else
-		return radeon_do_cp_idle(dev_priv);
-}
-
-/* Added by Charl P. Botha to call radeon_do_resume_cp().
- */
-int radeon_cp_resume(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return r600_do_resume_cp(dev, file_priv);
-	else
-		return radeon_do_resume_cp(dev, file_priv);
-}
-
-int radeon_engine_reset(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return r600_do_engine_reset(dev);
-	else
-		return radeon_do_engine_reset(dev);
-}
-
-/* ================================================================
- * Fullscreen mode
- */
-
-/* KW: Deprecated to say the least:
- */
-int radeon_fullscreen(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	return 0;
-}
-
-/* ================================================================
- * Freelist management
- */
-
-/* Original comment: FIXME: ROTATE_BUFS is a hack to cycle through
- *   bufs until freelist code is used.  Note this hides a problem with
- *   the scratch register * (used to keep track of last buffer
- *   completed) being written to before * the last buffer has actually
- *   completed rendering.
- *
- * KW:  It's also a good way to find free buffers quickly.
- *
- * KW: Ideally this loop wouldn't exist, and freelist_get wouldn't
- * sleep.  However, bugs in older versions of radeon_accel.c mean that
- * we essentially have to do this, else old clients will break.
- *
- * However, it does leave open a potential deadlock where all the
- * buffers are held by other clients, which can't release them because
- * they can't get the lock.
- */
-
-struct drm_buf *radeon_freelist_get(struct drm_device * dev)
-{
-	struct drm_device_dma *dma = dev->dma;
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_buf_priv_t *buf_priv;
-	struct drm_buf *buf;
-	int i, t;
-	int start;
-
-	if (++dev_priv->last_buf >= dma->buf_count)
-		dev_priv->last_buf = 0;
-
-	start = dev_priv->last_buf;
-
-	for (t = 0; t < dev_priv->usec_timeout; t++) {
-		u32 done_age = GET_SCRATCH(dev_priv, 1);
-		DRM_DEBUG("done_age = %d\n", done_age);
-		for (i = 0; i < dma->buf_count; i++) {
-			buf = dma->buflist[start];
-			buf_priv = buf->dev_private;
-			if (buf->file_priv == NULL || (buf->pending &&
-						       buf_priv->age <=
-						       done_age)) {
-				dev_priv->stats.requested_bufs++;
-				buf->pending = 0;
-				return buf;
-			}
-			if (++start >= dma->buf_count)
-				start = 0;
-		}
-
-		if (t) {
-			DRM_UDELAY(1);
-			dev_priv->stats.freelist_loops++;
-		}
-	}
-
-	return NULL;
-}
-
-void radeon_freelist_reset(struct drm_device * dev)
-{
-	struct drm_device_dma *dma = dev->dma;
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int i;
-
-	dev_priv->last_buf = 0;
-	for (i = 0; i < dma->buf_count; i++) {
-		struct drm_buf *buf = dma->buflist[i];
-		drm_radeon_buf_priv_t *buf_priv = buf->dev_private;
-		buf_priv->age = 0;
-	}
-}
-
-/* ================================================================
- * CP command submission
- */
-
-int radeon_wait_ring(drm_radeon_private_t * dev_priv, int n)
-{
-	drm_radeon_ring_buffer_t *ring = &dev_priv->ring;
-	int i;
-	u32 last_head = GET_RING_HEAD(dev_priv);
-
-	for (i = 0; i < dev_priv->usec_timeout; i++) {
-		u32 head = GET_RING_HEAD(dev_priv);
-
-		ring->space = (head - ring->tail) * sizeof(u32);
-		if (ring->space <= 0)
-			ring->space += ring->size;
-		if (ring->space > n)
-			return 0;
-
-		dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-		if (head != last_head)
-			i = 0;
-		last_head = head;
-
-		DRM_UDELAY(1);
-	}
-
-	/* FIXME: This return value is ignored in the BEGIN_RING macro! */
-#if RADEON_FIFO_DEBUG
-	radeon_status(dev_priv);
-	DRM_ERROR("failed!\n");
-#endif
-	return -EBUSY;
-}
-
-static int radeon_cp_get_buffers(struct drm_device *dev,
-				 struct drm_file *file_priv,
-				 struct drm_dma * d)
-{
-	int i;
-	struct drm_buf *buf;
-
-	for (i = d->granted_count; i < d->request_count; i++) {
-		buf = radeon_freelist_get(dev);
-		if (!buf)
-			return -EBUSY;	/* NOTE: broken client */
-
-		buf->file_priv = file_priv;
-
-		if (copy_to_user(&d->request_indices[i], &buf->idx,
-				     sizeof(buf->idx)))
-			return -EFAULT;
-		if (copy_to_user(&d->request_sizes[i], &buf->total,
-				     sizeof(buf->total)))
-			return -EFAULT;
-
-		d->granted_count++;
-	}
-	return 0;
-}
-
-int radeon_cp_buffers(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	struct drm_device_dma *dma = dev->dma;
-	int ret = 0;
-	struct drm_dma *d = data;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	/* Please don't send us buffers.
-	 */
-	if (d->send_count != 0) {
-		DRM_ERROR("Process %d trying to send %d buffers via drmDMA\n",
-			  DRM_CURRENTPID, d->send_count);
-		return -EINVAL;
-	}
-
-	/* We'll send you buffers.
-	 */
-	if (d->request_count < 0 || d->request_count > dma->buf_count) {
-		DRM_ERROR("Process %d trying to get %d buffers (of %d max)\n",
-			  DRM_CURRENTPID, d->request_count, dma->buf_count);
-		return -EINVAL;
-	}
-
-	d->granted_count = 0;
-
-	if (d->request_count) {
-		ret = radeon_cp_get_buffers(dev, file_priv, d);
-	}
-
-	return ret;
-}
-
-int radeon_driver_load(struct drm_device *dev, unsigned long flags)
-{
-	drm_radeon_private_t *dev_priv;
-	int ret = 0;
-
-	dev_priv = kzalloc(sizeof(drm_radeon_private_t), GFP_KERNEL);
-	if (dev_priv == NULL)
-		return -ENOMEM;
-
-	dev->dev_private = (void *)dev_priv;
-	dev_priv->flags = flags;
-
-	switch (flags & RADEON_FAMILY_MASK) {
-	case CHIP_R100:
-	case CHIP_RV200:
-	case CHIP_R200:
-	case CHIP_R300:
-	case CHIP_R350:
-	case CHIP_R420:
-	case CHIP_R423:
-	case CHIP_RV410:
-	case CHIP_RV515:
-	case CHIP_R520:
-	case CHIP_RV570:
-	case CHIP_R580:
-		dev_priv->flags |= RADEON_HAS_HIERZ;
-		break;
-	default:
-		/* all other chips have no hierarchical z buffer */
-		break;
-	}
-
-	pci_set_master(dev->pdev);
-
-	if (drm_pci_device_is_agp(dev))
-		dev_priv->flags |= RADEON_IS_AGP;
-	else if (pci_is_pcie(dev->pdev))
-		dev_priv->flags |= RADEON_IS_PCIE;
-	else
-		dev_priv->flags |= RADEON_IS_PCI;
-
-	ret = drm_legacy_addmap(dev, pci_resource_start(dev->pdev, 2),
-				pci_resource_len(dev->pdev, 2), _DRM_REGISTERS,
-				_DRM_READ_ONLY | _DRM_DRIVER, &dev_priv->mmio);
-	if (ret != 0)
-		return ret;
-
-	ret = drm_vblank_init(dev, 2);
-	if (ret) {
-		radeon_driver_unload(dev);
-		return ret;
-	}
-
-	DRM_DEBUG("%s card detected\n",
-		  ((dev_priv->flags & RADEON_IS_AGP) ? "AGP" : (((dev_priv->flags & RADEON_IS_PCIE) ? "PCIE" : "PCI"))));
-	return ret;
-}
-
-int radeon_master_create(struct drm_device *dev, struct drm_master *master)
-{
-	struct drm_radeon_master_private *master_priv;
-	unsigned long sareapage;
-	int ret;
-
-	master_priv = kzalloc(sizeof(*master_priv), GFP_KERNEL);
-	if (!master_priv)
-		return -ENOMEM;
-
-	/* prebuild the SAREA */
-	sareapage = max_t(unsigned long, SAREA_MAX, PAGE_SIZE);
-	ret = drm_legacy_addmap(dev, 0, sareapage, _DRM_SHM, _DRM_CONTAINS_LOCK,
-				&master_priv->sarea);
-	if (ret) {
-		DRM_ERROR("SAREA setup failed\n");
-		kfree(master_priv);
-		return ret;
-	}
-	master_priv->sarea_priv = master_priv->sarea->handle + sizeof(struct drm_sarea);
-	master_priv->sarea_priv->pfCurrentPage = 0;
-
-	master->driver_priv = master_priv;
-	return 0;
-}
-
-void radeon_master_destroy(struct drm_device *dev, struct drm_master *master)
-{
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-
-	if (!master_priv)
-		return;
-
-	if (master_priv->sarea_priv &&
-	    master_priv->sarea_priv->pfCurrentPage != 0)
-		radeon_cp_dispatch_flip(dev, master);
-
-	master_priv->sarea_priv = NULL;
-	if (master_priv->sarea)
-		drm_legacy_rmmap_locked(dev, master_priv->sarea);
-
-	kfree(master_priv);
-
-	master->driver_priv = NULL;
-}
-
-/* Create mappings for registers and framebuffer so userland doesn't necessarily
- * have to find them.
- */
-int radeon_driver_firstopen(struct drm_device *dev)
-{
-	int ret;
-	drm_local_map_t *map;
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	dev_priv->gart_info.table_size = RADEON_PCIGART_TABLE_SIZE;
-
-	dev_priv->fb_aper_offset = pci_resource_start(dev->pdev, 0);
-	ret = drm_legacy_addmap(dev, dev_priv->fb_aper_offset,
-				pci_resource_len(dev->pdev, 0),
-				_DRM_FRAME_BUFFER, _DRM_WRITE_COMBINING, &map);
-	if (ret != 0)
-		return ret;
-
-	return 0;
-}
-
-int radeon_driver_unload(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	DRM_DEBUG("\n");
-
-	drm_legacy_rmmap(dev, dev_priv->mmio);
-
-	kfree(dev_priv);
-
-	dev->dev_private = NULL;
-	return 0;
-}
-
-void radeon_commit_ring(drm_radeon_private_t *dev_priv)
-{
-	int i;
-	u32 *ring;
-	int tail_aligned;
-
-	/* check if the ring is padded out to 16-dword alignment */
-
-	tail_aligned = dev_priv->ring.tail & (RADEON_RING_ALIGN-1);
-	if (tail_aligned) {
-		int num_p2 = RADEON_RING_ALIGN - tail_aligned;
-
-		ring = dev_priv->ring.start;
-		/* pad with some CP_PACKET2 */
-		for (i = 0; i < num_p2; i++)
-			ring[dev_priv->ring.tail + i] = CP_PACKET2();
-
-		dev_priv->ring.tail += i;
-
-		dev_priv->ring.space -= num_p2 * sizeof(u32);
-	}
-
-	dev_priv->ring.tail &= dev_priv->ring.tail_mask;
-
-	mb();
-	GET_RING_HEAD( dev_priv );
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) {
-		RADEON_WRITE(R600_CP_RB_WPTR, dev_priv->ring.tail);
-		/* read from PCI bus to ensure correct posting */
-		RADEON_READ(R600_CP_RB_RPTR);
-	} else {
-		RADEON_WRITE(RADEON_CP_RB_WPTR, dev_priv->ring.tail);
-		/* read from PCI bus to ensure correct posting */
-		RADEON_READ(RADEON_CP_RB_RPTR);
-	}
-}
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index c566993a2ec3..4197ca1bb1e4 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -1150,7 +1150,7 @@ static void radeon_check_arguments(struct radeon_device *rdev)
 	}
 
 	if (radeon_vm_size < 1) {
-		dev_warn(rdev->dev, "VM size (%d) to small, min is 1GB\n",
+		dev_warn(rdev->dev, "VM size (%d) too small, min is 1GB\n",
 			 radeon_vm_size);
 		radeon_vm_size = 4;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 1eca0acac016..2d9196a447fd 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -403,7 +403,8 @@ static void radeon_flip_work_func(struct work_struct *__work)
 	struct drm_crtc *crtc = &radeon_crtc->base;
 	unsigned long flags;
 	int r;
-	int vpos, hpos, stat, min_udelay;
+	int vpos, hpos, stat, min_udelay = 0;
+	unsigned repcnt = 4;
 	struct drm_vblank_crtc *vblank = &crtc->dev->vblank[work->crtc_id];
 
         down_read(&rdev->exclusive_lock);
@@ -454,7 +455,7 @@ static void radeon_flip_work_func(struct work_struct *__work)
 	 * In practice this won't execute very often unless on very fast
 	 * machines because the time window for this to happen is very small.
 	 */
-	for (;;) {
+	while (radeon_crtc->enabled && --repcnt) {
 		/* GET_DISTANCE_TO_VBLANKSTART returns distance to real vblank
 		 * start in hpos, and to the "fudged earlier" vblank start in
 		 * vpos.
@@ -470,12 +471,24 @@ static void radeon_flip_work_func(struct work_struct *__work)
 			break;
 
 		/* Sleep at least until estimated real start of hw vblank */
-		spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
 		min_udelay = (-hpos + 1) * max(vblank->linedur_ns / 1000, 5);
+		if (min_udelay > vblank->framedur_ns / 2000) {
+			/* Don't wait ridiculously long - something is wrong */
+			repcnt = 0;
+			break;
+		}
+		spin_unlock_irqrestore(&crtc->dev->event_lock, flags);
 		usleep_range(min_udelay, 2 * min_udelay);
 		spin_lock_irqsave(&crtc->dev->event_lock, flags);
 	};
 
+	if (!repcnt)
+		DRM_DEBUG_DRIVER("Delay problem on crtc %d: min_udelay %d, "
+				 "framedur %d, linedur %d, stat %d, vpos %d, "
+				 "hpos %d\n", work->crtc_id, min_udelay,
+				 vblank->framedur_ns / 1000,
+				 vblank->linedur_ns / 1000, stat, vpos, hpos);
+
 	/* do the flip (mmio) */
 	radeon_page_flip(rdev, radeon_crtc->crtc_id, work->base);
 
@@ -1331,7 +1344,7 @@ static const struct drm_framebuffer_funcs radeon_fb_funcs = {
 int
 radeon_framebuffer_init(struct drm_device *dev,
 			struct radeon_framebuffer *rfb,
-			struct drm_mode_fb_cmd2 *mode_cmd,
+			const struct drm_mode_fb_cmd2 *mode_cmd,
 			struct drm_gem_object *obj)
 {
 	int ret;
@@ -1348,7 +1361,7 @@ radeon_framebuffer_init(struct drm_device *dev,
 static struct drm_framebuffer *
 radeon_user_framebuffer_create(struct drm_device *dev,
 			       struct drm_file *file_priv,
-			       struct drm_mode_fb_cmd2 *mode_cmd)
+			       const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct radeon_framebuffer *radeon_fb;
@@ -1670,8 +1683,10 @@ int radeon_modeset_init(struct radeon_device *rdev)
 	/* setup afmt */
 	radeon_afmt_init(rdev);
 
-	radeon_fbdev_init(rdev);
-	drm_kms_helper_poll_init(rdev->ddev);
+	if (!list_empty(&rdev->ddev->mode_config.connector_list)) {
+		radeon_fbdev_init(rdev);
+		drm_kms_helper_poll_init(rdev->ddev);
+	}
 
 	/* do pm late init */
 	ret = radeon_pm_late_init(rdev);
diff --git a/drivers/gpu/drm/radeon/radeon_dp_mst.c b/drivers/gpu/drm/radeon/radeon_dp_mst.c
index 744f5c49c664..df7a1719c841 100644
--- a/drivers/gpu/drm/radeon/radeon_dp_mst.c
+++ b/drivers/gpu/drm/radeon/radeon_dp_mst.c
@@ -329,7 +329,7 @@ static void radeon_dp_mst_hotplug(struct drm_dp_mst_topology_mgr *mgr)
 	drm_kms_helper_hotplug_event(dev);
 }
 
-struct drm_dp_mst_topology_cbs mst_cbs = {
+const struct drm_dp_mst_topology_cbs mst_cbs = {
 	.add_connector = radeon_dp_add_mst_connector,
 	.register_connector = radeon_dp_register_mst_connector,
 	.destroy_connector = radeon_dp_destroy_mst_connector,
@@ -525,11 +525,17 @@ static bool radeon_mst_mode_fixup(struct drm_encoder *encoder,
 	drm_mode_set_crtcinfo(adjusted_mode, 0);
 	{
 	  struct radeon_connector_atom_dig *dig_connector;
+	  int ret;
 
 	  dig_connector = mst_enc->connector->con_priv;
-	  dig_connector->dp_lane_count = drm_dp_max_lane_count(dig_connector->dpcd);
-	  dig_connector->dp_clock = radeon_dp_get_max_link_rate(&mst_enc->connector->base,
-								dig_connector->dpcd);
+	  ret = radeon_dp_get_dp_link_config(&mst_enc->connector->base,
+					     dig_connector->dpcd, adjusted_mode->clock,
+					     &dig_connector->dp_lane_count,
+					     &dig_connector->dp_clock);
+	  if (ret) {
+		  dig_connector->dp_lane_count = 0;
+		  dig_connector->dp_clock = 0;
+	  }
 	  DRM_DEBUG_KMS("dig clock %p %d %d\n", dig_connector,
 			dig_connector->dp_lane_count, dig_connector->dp_clock);
 	}
@@ -641,7 +647,7 @@ radeon_dp_create_fake_mst_encoder(struct radeon_connector *connector)
 	}
 
 	drm_encoder_init(dev, &radeon_encoder->base, &radeon_dp_mst_enc_funcs,
-			 DRM_MODE_ENCODER_DPMST);
+			 DRM_MODE_ENCODER_DPMST, NULL);
 	drm_encoder_helper_add(encoder, &radeon_mst_helper_funcs);
 
 	mst_enc = radeon_encoder->enc_priv;
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 5b6a6f5b3619..e266ffc520d2 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -291,88 +291,6 @@ static struct pci_device_id pciidlist[] = {
 
 MODULE_DEVICE_TABLE(pci, pciidlist);
 
-#ifdef CONFIG_DRM_RADEON_UMS
-
-static int radeon_suspend(struct drm_device *dev, pm_message_t state)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return 0;
-
-	/* Disable *all* interrupts */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600)
-		RADEON_WRITE(R500_DxMODE_INT_MASK, 0);
-	RADEON_WRITE(RADEON_GEN_INT_CNTL, 0);
-	return 0;
-}
-
-static int radeon_resume(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return 0;
-
-	/* Restore interrupt registers */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600)
-		RADEON_WRITE(R500_DxMODE_INT_MASK, dev_priv->r500_disp_irq_reg);
-	RADEON_WRITE(RADEON_GEN_INT_CNTL, dev_priv->irq_enable_reg);
-	return 0;
-}
-
-
-static const struct file_operations radeon_driver_old_fops = {
-	.owner = THIS_MODULE,
-	.open = drm_open,
-	.release = drm_release,
-	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_legacy_mmap,
-	.poll = drm_poll,
-	.read = drm_read,
-#ifdef CONFIG_COMPAT
-	.compat_ioctl = radeon_compat_ioctl,
-#endif
-	.llseek = noop_llseek,
-};
-
-static struct drm_driver driver_old = {
-	.driver_features =
-	    DRIVER_USE_AGP | DRIVER_PCI_DMA | DRIVER_SG |
-	    DRIVER_HAVE_IRQ | DRIVER_HAVE_DMA | DRIVER_IRQ_SHARED,
-	.dev_priv_size = sizeof(drm_radeon_buf_priv_t),
-	.load = radeon_driver_load,
-	.firstopen = radeon_driver_firstopen,
-	.open = radeon_driver_open,
-	.preclose = radeon_driver_preclose,
-	.postclose = radeon_driver_postclose,
-	.lastclose = radeon_driver_lastclose,
-	.set_busid = drm_pci_set_busid,
-	.unload = radeon_driver_unload,
-	.suspend = radeon_suspend,
-	.resume = radeon_resume,
-	.get_vblank_counter = radeon_get_vblank_counter,
-	.enable_vblank = radeon_enable_vblank,
-	.disable_vblank = radeon_disable_vblank,
-	.master_create = radeon_master_create,
-	.master_destroy = radeon_master_destroy,
-	.irq_preinstall = radeon_driver_irq_preinstall,
-	.irq_postinstall = radeon_driver_irq_postinstall,
-	.irq_uninstall = radeon_driver_irq_uninstall,
-	.irq_handler = radeon_driver_irq_handler,
-	.ioctls = radeon_ioctls,
-	.dma_ioctl = radeon_cp_buffers,
-	.fops = &radeon_driver_old_fops,
-	.name = DRIVER_NAME,
-	.desc = DRIVER_DESC,
-	.date = DRIVER_DATE,
-	.major = DRIVER_MAJOR,
-	.minor = DRIVER_MINOR,
-	.patchlevel = DRIVER_PATCHLEVEL,
-};
-
-#endif
-
 static struct drm_driver kms_driver;
 
 static int radeon_kick_out_firmware_fb(struct pci_dev *pdev)
@@ -619,13 +537,6 @@ static struct drm_driver kms_driver = {
 static struct drm_driver *driver;
 static struct pci_driver *pdriver;
 
-#ifdef CONFIG_DRM_RADEON_UMS
-static struct pci_driver radeon_pci_driver = {
-	.name = DRIVER_NAME,
-	.id_table = pciidlist,
-};
-#endif
-
 static struct pci_driver radeon_kms_pci_driver = {
 	.name = DRIVER_NAME,
 	.id_table = pciidlist,
@@ -655,16 +566,8 @@ static int __init radeon_init(void)
 		radeon_register_atpx_handler();
 
 	} else {
-#ifdef CONFIG_DRM_RADEON_UMS
-		DRM_INFO("radeon userspace modesetting enabled.\n");
-		driver = &driver_old;
-		pdriver = &radeon_pci_driver;
-		driver->driver_features &= ~DRIVER_MODESET;
-		driver->num_ioctls = radeon_max_ioctl;
-#else
 		DRM_ERROR("No UMS support in radeon module!\n");
 		return -EINVAL;
-#endif
 	}
 
 	radeon_kfd_init();
diff --git a/drivers/gpu/drm/radeon/radeon_drv.h b/drivers/gpu/drm/radeon/radeon_drv.h
index 0caafc7a6e17..afef2d9fccd8 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.h
+++ b/drivers/gpu/drm/radeon/radeon_drv.h
@@ -119,2052 +119,4 @@
 long radeon_drm_ioctl(struct file *filp,
 		      unsigned int cmd, unsigned long arg);
 
-/* The rest of the file is DEPRECATED! */
-#ifdef CONFIG_DRM_RADEON_UMS
-
-enum radeon_cp_microcode_version {
-	UCODE_R100,
-	UCODE_R200,
-	UCODE_R300,
-};
-
-typedef struct drm_radeon_freelist {
-	unsigned int age;
-	struct drm_buf *buf;
-	struct drm_radeon_freelist *next;
-	struct drm_radeon_freelist *prev;
-} drm_radeon_freelist_t;
-
-typedef struct drm_radeon_ring_buffer {
-	u32 *start;
-	u32 *end;
-	int size;
-	int size_l2qw;
-
-	int rptr_update; /* Double Words */
-	int rptr_update_l2qw; /* log2 Quad Words */
-
-	int fetch_size; /* Double Words */
-	int fetch_size_l2ow; /* log2 Oct Words */
-
-	u32 tail;
-	u32 tail_mask;
-	int space;
-
-	int high_mark;
-} drm_radeon_ring_buffer_t;
-
-typedef struct drm_radeon_depth_clear_t {
-	u32 rb3d_cntl;
-	u32 rb3d_zstencilcntl;
-	u32 se_cntl;
-} drm_radeon_depth_clear_t;
-
-struct drm_radeon_driver_file_fields {
-	int64_t radeon_fb_delta;
-};
-
-struct mem_block {
-	struct mem_block *next;
-	struct mem_block *prev;
-	int start;
-	int size;
-	struct drm_file *file_priv; /* NULL: free, -1: heap, other: real files */
-};
-
-struct radeon_surface {
-	int refcount;
-	u32 lower;
-	u32 upper;
-	u32 flags;
-};
-
-struct radeon_virt_surface {
-	int surface_index;
-	u32 lower;
-	u32 upper;
-	u32 flags;
-	struct drm_file *file_priv;
-#define PCIGART_FILE_PRIV	((void *) -1L)
-};
-
-#define RADEON_FLUSH_EMITED	(1 << 0)
-#define RADEON_PURGE_EMITED	(1 << 1)
-
-struct drm_radeon_master_private {
-	drm_local_map_t *sarea;
-	drm_radeon_sarea_t *sarea_priv;
-};
-
-typedef struct drm_radeon_private {
-	drm_radeon_ring_buffer_t ring;
-
-	u32 fb_location;
-	u32 fb_size;
-	int new_memmap;
-
-	int gart_size;
-	u32 gart_vm_start;
-	unsigned long gart_buffers_offset;
-
-	int cp_mode;
-	int cp_running;
-
-	drm_radeon_freelist_t *head;
-	drm_radeon_freelist_t *tail;
-	int last_buf;
-	int writeback_works;
-
-	int usec_timeout;
-
-	int microcode_version;
-
-	struct {
-		u32 boxes;
-		int freelist_timeouts;
-		int freelist_loops;
-		int requested_bufs;
-		int last_frame_reads;
-		int last_clear_reads;
-		int clears;
-		int texture_uploads;
-	} stats;
-
-	int do_boxes;
-	int page_flipping;
-
-	u32 color_fmt;
-	unsigned int front_offset;
-	unsigned int front_pitch;
-	unsigned int back_offset;
-	unsigned int back_pitch;
-
-	u32 depth_fmt;
-	unsigned int depth_offset;
-	unsigned int depth_pitch;
-
-	u32 front_pitch_offset;
-	u32 back_pitch_offset;
-	u32 depth_pitch_offset;
-
-	drm_radeon_depth_clear_t depth_clear;
-
-	unsigned long ring_offset;
-	unsigned long ring_rptr_offset;
-	unsigned long buffers_offset;
-	unsigned long gart_textures_offset;
-
-	drm_local_map_t *sarea;
-	drm_local_map_t *cp_ring;
-	drm_local_map_t *ring_rptr;
-	drm_local_map_t *gart_textures;
-
-	struct mem_block *gart_heap;
-	struct mem_block *fb_heap;
-
-	/* SW interrupt */
-	wait_queue_head_t swi_queue;
-	atomic_t swi_emitted;
-	int vblank_crtc;
-	uint32_t irq_enable_reg;
-	uint32_t r500_disp_irq_reg;
-
-	struct radeon_surface surfaces[RADEON_MAX_SURFACES];
-	struct radeon_virt_surface virt_surfaces[2 * RADEON_MAX_SURFACES];
-
-	unsigned long pcigart_offset;
-	unsigned int pcigart_offset_set;
-	struct drm_ati_pcigart_info gart_info;
-
-	u32 scratch_ages[5];
-
-	int have_z_offset;
-
-	/* starting from here on, data is preserved across an open */
-	uint32_t flags;		/* see radeon_chip_flags */
-	resource_size_t fb_aper_offset;
-
-	int num_gb_pipes;
-	int num_z_pipes;
-	int track_flush;
-	drm_local_map_t *mmio;
-
-	/* r6xx/r7xx pipe/shader config */
-	int r600_max_pipes;
-	int r600_max_tile_pipes;
-	int r600_max_simds;
-	int r600_max_backends;
-	int r600_max_gprs;
-	int r600_max_threads;
-	int r600_max_stack_entries;
-	int r600_max_hw_contexts;
-	int r600_max_gs_threads;
-	int r600_sx_max_export_size;
-	int r600_sx_max_export_pos_size;
-	int r600_sx_max_export_smx_size;
-	int r600_sq_num_cf_insts;
-	int r700_sx_num_of_sets;
-	int r700_sc_prim_fifo_size;
-	int r700_sc_hiz_tile_fifo_size;
-	int r700_sc_earlyz_tile_fifo_fize;
-	int r600_group_size;
-	int r600_npipes;
-	int r600_nbanks;
-
-	struct mutex cs_mutex;
-	u32 cs_id_scnt;
-	u32 cs_id_wcnt;
-	/* r6xx/r7xx drm blit vertex buffer */
-	struct drm_buf *blit_vb;
-
-	/* firmware */
-	const struct firmware *me_fw, *pfp_fw;
-} drm_radeon_private_t;
-
-typedef struct drm_radeon_buf_priv {
-	u32 age;
-} drm_radeon_buf_priv_t;
-
-struct drm_buffer;
-
-typedef struct drm_radeon_kcmd_buffer {
-	int bufsz;
-	struct drm_buffer *buffer;
-	int nbox;
-	struct drm_clip_rect __user *boxes;
-} drm_radeon_kcmd_buffer_t;
-
-extern int radeon_no_wb;
-extern struct drm_ioctl_desc radeon_ioctls[];
-extern int radeon_max_ioctl;
-
-extern u32 radeon_get_ring_head(drm_radeon_private_t *dev_priv);
-extern void radeon_set_ring_head(drm_radeon_private_t *dev_priv, u32 val);
-
-#define GET_RING_HEAD(dev_priv)	radeon_get_ring_head(dev_priv)
-#define SET_RING_HEAD(dev_priv, val) radeon_set_ring_head(dev_priv, val)
-
-/* Check whether the given hardware address is inside the framebuffer or the
- * GART area.
- */
-static __inline__ int radeon_check_offset(drm_radeon_private_t *dev_priv,
-					  u64 off)
-{
-	u32 fb_start = dev_priv->fb_location;
-	u32 fb_end = fb_start + dev_priv->fb_size - 1;
-	u32 gart_start = dev_priv->gart_vm_start;
-	u32 gart_end = gart_start + dev_priv->gart_size - 1;
-
-	return ((off >= fb_start && off <= fb_end) ||
-		(off >= gart_start && off <= gart_end));
-}
-
-/* radeon_state.c */
-extern void radeon_cp_discard_buffer(struct drm_device *dev, struct drm_master *master, struct drm_buf *buf);
-
-				/* radeon_cp.c */
-extern int radeon_cp_init(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_start(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_stop(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_reset(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_idle(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_resume(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_engine_reset(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_fullscreen(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_cp_buffers(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern u32 radeon_read_fb_location(drm_radeon_private_t *dev_priv);
-extern void radeon_write_agp_location(drm_radeon_private_t *dev_priv, u32 agp_loc);
-extern void radeon_write_agp_base(drm_radeon_private_t *dev_priv, u64 agp_base);
-
-extern void radeon_freelist_reset(struct drm_device * dev);
-extern struct drm_buf *radeon_freelist_get(struct drm_device * dev);
-
-extern int radeon_wait_ring(drm_radeon_private_t * dev_priv, int n);
-
-extern int radeon_do_cp_idle(drm_radeon_private_t * dev_priv);
-
-extern int radeon_driver_preinit(struct drm_device *dev, unsigned long flags);
-extern int radeon_presetup(struct drm_device *dev);
-extern int radeon_driver_postcleanup(struct drm_device *dev);
-
-extern int radeon_mem_alloc(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_mem_free(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_mem_init_heap(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern void radeon_mem_takedown(struct mem_block **heap);
-extern void radeon_mem_release(struct drm_file *file_priv,
-			       struct mem_block *heap);
-
-extern void radeon_enable_bm(struct drm_radeon_private *dev_priv);
-extern u32 radeon_read_ring_rptr(drm_radeon_private_t *dev_priv, u32 off);
-extern void radeon_write_ring_rptr(drm_radeon_private_t *dev_priv, u32 off, u32 val);
-
-				/* radeon_irq.c */
-extern void radeon_irq_set_state(struct drm_device *dev, u32 mask, int state);
-extern int radeon_irq_emit(struct drm_device *dev, void *data, struct drm_file *file_priv);
-extern int radeon_irq_wait(struct drm_device *dev, void *data, struct drm_file *file_priv);
-
-extern void radeon_do_release(struct drm_device * dev);
-extern u32 radeon_get_vblank_counter(struct drm_device *dev, unsigned int pipe);
-extern int radeon_enable_vblank(struct drm_device *dev, unsigned int pipe);
-extern void radeon_disable_vblank(struct drm_device *dev, unsigned int pipe);
-extern irqreturn_t radeon_driver_irq_handler(int irq, void *arg);
-extern void radeon_driver_irq_preinstall(struct drm_device * dev);
-extern int radeon_driver_irq_postinstall(struct drm_device *dev);
-extern void radeon_driver_irq_uninstall(struct drm_device * dev);
-extern void radeon_enable_interrupt(struct drm_device *dev);
-extern int radeon_vblank_crtc_get(struct drm_device *dev);
-extern int radeon_vblank_crtc_set(struct drm_device *dev, int64_t value);
-
-extern int radeon_driver_load(struct drm_device *dev, unsigned long flags);
-extern int radeon_driver_unload(struct drm_device *dev);
-extern int radeon_driver_firstopen(struct drm_device *dev);
-extern void radeon_driver_preclose(struct drm_device *dev,
-				   struct drm_file *file_priv);
-extern void radeon_driver_postclose(struct drm_device *dev,
-				    struct drm_file *file_priv);
-extern void radeon_driver_lastclose(struct drm_device * dev);
-extern int radeon_driver_open(struct drm_device *dev,
-			      struct drm_file *file_priv);
-extern long radeon_compat_ioctl(struct file *filp, unsigned int cmd,
-				unsigned long arg);
-
-extern int radeon_master_create(struct drm_device *dev, struct drm_master *master);
-extern void radeon_master_destroy(struct drm_device *dev, struct drm_master *master);
-extern void radeon_cp_dispatch_flip(struct drm_device *dev, struct drm_master *master);
-/* r300_cmdbuf.c */
-extern void r300_init_reg_flags(struct drm_device *dev);
-
-extern int r300_do_cp_cmdbuf(struct drm_device *dev,
-			     struct drm_file *file_priv,
-			     drm_radeon_kcmd_buffer_t *cmdbuf);
-
-/* r600_cp.c */
-extern int r600_do_engine_reset(struct drm_device *dev);
-extern int r600_do_cleanup_cp(struct drm_device *dev);
-extern int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
-			   struct drm_file *file_priv);
-extern int r600_do_resume_cp(struct drm_device *dev, struct drm_file *file_priv);
-extern int r600_do_cp_idle(drm_radeon_private_t *dev_priv);
-extern void r600_do_cp_start(drm_radeon_private_t *dev_priv);
-extern void r600_do_cp_reset(drm_radeon_private_t *dev_priv);
-extern void r600_do_cp_stop(drm_radeon_private_t *dev_priv);
-extern int r600_cp_dispatch_indirect(struct drm_device *dev,
-				     struct drm_buf *buf, int start, int end);
-extern int r600_page_table_init(struct drm_device *dev);
-extern void r600_page_table_cleanup(struct drm_device *dev, struct drm_ati_pcigart_info *gart_info);
-extern int r600_cs_legacy_ioctl(struct drm_device *dev, void *data, struct drm_file *fpriv);
-extern void r600_cp_dispatch_swap(struct drm_device *dev, struct drm_file *file_priv);
-extern int r600_cp_dispatch_texture(struct drm_device *dev,
-				    struct drm_file *file_priv,
-				    drm_radeon_texture_t *tex,
-				    drm_radeon_tex_image_t *image);
-/* r600_blit.c */
-extern int r600_prepare_blit_copy(struct drm_device *dev, struct drm_file *file_priv);
-extern void r600_done_blit_copy(struct drm_device *dev);
-extern void r600_blit_copy(struct drm_device *dev,
-			   uint64_t src_gpu_addr, uint64_t dst_gpu_addr,
-			   int size_bytes);
-extern void r600_blit_swap(struct drm_device *dev,
-			   uint64_t src_gpu_addr, uint64_t dst_gpu_addr,
-			   int sx, int sy, int dx, int dy,
-			   int w, int h, int src_pitch, int dst_pitch, int cpp);
-
-/* Flags for stats.boxes
- */
-#define RADEON_BOX_DMA_IDLE      0x1
-#define RADEON_BOX_RING_FULL     0x2
-#define RADEON_BOX_FLIP          0x4
-#define RADEON_BOX_WAIT_IDLE     0x8
-#define RADEON_BOX_TEXTURE_LOAD  0x10
-
-/* Register definitions, register access macros and drmAddMap constants
- * for Radeon kernel driver.
- */
-#define RADEON_MM_INDEX		        0x0000
-#define RADEON_MM_DATA		        0x0004
-
-#define RADEON_AGP_COMMAND		0x0f60
-#define RADEON_AGP_COMMAND_PCI_CONFIG   0x0060	/* offset in PCI config */
-#	define RADEON_AGP_ENABLE	(1<<8)
-#define RADEON_AUX_SCISSOR_CNTL		0x26f0
-#	define RADEON_EXCLUSIVE_SCISSOR_0	(1 << 24)
-#	define RADEON_EXCLUSIVE_SCISSOR_1	(1 << 25)
-#	define RADEON_EXCLUSIVE_SCISSOR_2	(1 << 26)
-#	define RADEON_SCISSOR_0_ENABLE		(1 << 28)
-#	define RADEON_SCISSOR_1_ENABLE		(1 << 29)
-#	define RADEON_SCISSOR_2_ENABLE		(1 << 30)
-
-/*
- * PCIE radeons (rv370/rv380, rv410, r423/r430/r480, r5xx)
- * don't have an explicit bus mastering disable bit.  It's handled
- * by the PCI D-states.  PMI_BM_DIS disables D-state bus master
- * handling, not bus mastering itself.
- */
-#define RADEON_BUS_CNTL			0x0030
-/* r1xx, r2xx, r300, r(v)350, r420/r481, rs400/rs480 */
-#	define RADEON_BUS_MASTER_DIS		(1 << 6)
-/* rs600/rs690/rs740 */
-#	define RS600_BUS_MASTER_DIS		(1 << 14)
-#	define RS600_MSI_REARM		        (1 << 20)
-/* see RS400_MSI_REARM in AIC_CNTL for rs480 */
-
-#define RADEON_BUS_CNTL1		0x0034
-#	define RADEON_PMI_BM_DIS		(1 << 2)
-#	define RADEON_PMI_INT_DIS		(1 << 3)
-
-#define RV370_BUS_CNTL			0x004c
-#	define RV370_PMI_BM_DIS		        (1 << 5)
-#	define RV370_PMI_INT_DIS		(1 << 6)
-
-#define RADEON_MSI_REARM_EN		0x0160
-/* rv370/rv380, rv410, r423/r430/r480, r5xx */
-#	define RV370_MSI_REARM_EN		(1 << 0)
-
-#define RADEON_CLOCK_CNTL_DATA		0x000c
-#	define RADEON_PLL_WR_EN			(1 << 7)
-#define RADEON_CLOCK_CNTL_INDEX		0x0008
-#define RADEON_CONFIG_APER_SIZE		0x0108
-#define RADEON_CONFIG_MEMSIZE		0x00f8
-#define RADEON_CRTC_OFFSET		0x0224
-#define RADEON_CRTC_OFFSET_CNTL		0x0228
-#	define RADEON_CRTC_TILE_EN		(1 << 15)
-#	define RADEON_CRTC_OFFSET_FLIP_CNTL	(1 << 16)
-#define RADEON_CRTC2_OFFSET		0x0324
-#define RADEON_CRTC2_OFFSET_CNTL	0x0328
-
-#define RADEON_PCIE_INDEX               0x0030
-#define RADEON_PCIE_DATA                0x0034
-#define RADEON_PCIE_TX_GART_CNTL	0x10
-#	define RADEON_PCIE_TX_GART_EN		(1 << 0)
-#	define RADEON_PCIE_TX_GART_UNMAPPED_ACCESS_PASS_THRU (0 << 1)
-#	define RADEON_PCIE_TX_GART_UNMAPPED_ACCESS_CLAMP_LO  (1 << 1)
-#	define RADEON_PCIE_TX_GART_UNMAPPED_ACCESS_DISCARD   (3 << 1)
-#	define RADEON_PCIE_TX_GART_MODE_32_128_CACHE	(0 << 3)
-#	define RADEON_PCIE_TX_GART_MODE_8_4_128_CACHE	(1 << 3)
-#	define RADEON_PCIE_TX_GART_CHK_RW_VALID_EN      (1 << 5)
-#	define RADEON_PCIE_TX_GART_INVALIDATE_TLB	(1 << 8)
-#define RADEON_PCIE_TX_DISCARD_RD_ADDR_LO 0x11
-#define RADEON_PCIE_TX_DISCARD_RD_ADDR_HI 0x12
-#define RADEON_PCIE_TX_GART_BASE	0x13
-#define RADEON_PCIE_TX_GART_START_LO	0x14
-#define RADEON_PCIE_TX_GART_START_HI	0x15
-#define RADEON_PCIE_TX_GART_END_LO	0x16
-#define RADEON_PCIE_TX_GART_END_HI	0x17
-
-#define RS480_NB_MC_INDEX               0x168
-#	define RS480_NB_MC_IND_WR_EN	(1 << 8)
-#define RS480_NB_MC_DATA                0x16c
-
-#define RS690_MC_INDEX                  0x78
-#   define RS690_MC_INDEX_MASK          0x1ff
-#   define RS690_MC_INDEX_WR_EN         (1 << 9)
-#   define RS690_MC_INDEX_WR_ACK        0x7f
-#define RS690_MC_DATA                   0x7c
-
-/* MC indirect registers */
-#define RS480_MC_MISC_CNTL              0x18
-#	define RS480_DISABLE_GTW	(1 << 1)
-/* switch between MCIND GART and MM GART registers. 0 = mmgart, 1 = mcind gart */
-#	define RS480_GART_INDEX_REG_EN	(1 << 12)
-#	define RS690_BLOCK_GFX_D3_EN	(1 << 14)
-#define RS480_K8_FB_LOCATION            0x1e
-#define RS480_GART_FEATURE_ID           0x2b
-#	define RS480_HANG_EN	        (1 << 11)
-#	define RS480_TLB_ENABLE	        (1 << 18)
-#	define RS480_P2P_ENABLE	        (1 << 19)
-#	define RS480_GTW_LAC_EN	        (1 << 25)
-#	define RS480_2LEVEL_GART	(0 << 30)
-#	define RS480_1LEVEL_GART	(1 << 30)
-#	define RS480_PDC_EN	        (1 << 31)
-#define RS480_GART_BASE                 0x2c
-#define RS480_GART_CACHE_CNTRL          0x2e
-#	define RS480_GART_CACHE_INVALIDATE (1 << 0) /* wait for it to clear */
-#define RS480_AGP_ADDRESS_SPACE_SIZE    0x38
-#	define RS480_GART_EN	        (1 << 0)
-#	define RS480_VA_SIZE_32MB	(0 << 1)
-#	define RS480_VA_SIZE_64MB	(1 << 1)
-#	define RS480_VA_SIZE_128MB	(2 << 1)
-#	define RS480_VA_SIZE_256MB	(3 << 1)
-#	define RS480_VA_SIZE_512MB	(4 << 1)
-#	define RS480_VA_SIZE_1GB	(5 << 1)
-#	define RS480_VA_SIZE_2GB	(6 << 1)
-#define RS480_AGP_MODE_CNTL             0x39
-#	define RS480_POST_GART_Q_SIZE	(1 << 18)
-#	define RS480_NONGART_SNOOP	(1 << 19)
-#	define RS480_AGP_RD_BUF_SIZE	(1 << 20)
-#	define RS480_REQ_TYPE_SNOOP_SHIFT 22
-#	define RS480_REQ_TYPE_SNOOP_MASK  0x3
-#	define RS480_REQ_TYPE_SNOOP_DIS	(1 << 24)
-#define RS480_MC_MISC_UMA_CNTL          0x5f
-#define RS480_MC_MCLK_CNTL              0x7a
-#define RS480_MC_UMA_DUALCH_CNTL        0x86
-
-#define RS690_MC_FB_LOCATION            0x100
-#define RS690_MC_AGP_LOCATION           0x101
-#define RS690_MC_AGP_BASE               0x102
-#define RS690_MC_AGP_BASE_2             0x103
-
-#define RS600_MC_INDEX                          0x70
-#       define RS600_MC_ADDR_MASK               0xffff
-#       define RS600_MC_IND_SEQ_RBS_0           (1 << 16)
-#       define RS600_MC_IND_SEQ_RBS_1           (1 << 17)
-#       define RS600_MC_IND_SEQ_RBS_2           (1 << 18)
-#       define RS600_MC_IND_SEQ_RBS_3           (1 << 19)
-#       define RS600_MC_IND_AIC_RBS             (1 << 20)
-#       define RS600_MC_IND_CITF_ARB0           (1 << 21)
-#       define RS600_MC_IND_CITF_ARB1           (1 << 22)
-#       define RS600_MC_IND_WR_EN               (1 << 23)
-#define RS600_MC_DATA                           0x74
-
-#define RS600_MC_STATUS                         0x0
-#       define RS600_MC_IDLE                    (1 << 1)
-#define RS600_MC_FB_LOCATION                    0x4
-#define RS600_MC_AGP_LOCATION                   0x5
-#define RS600_AGP_BASE                          0x6
-#define RS600_AGP_BASE_2                        0x7
-#define RS600_MC_CNTL1                          0x9
-#       define RS600_ENABLE_PAGE_TABLES         (1 << 26)
-#define RS600_MC_PT0_CNTL                       0x100
-#       define RS600_ENABLE_PT                  (1 << 0)
-#       define RS600_EFFECTIVE_L2_CACHE_SIZE(x) ((x) << 15)
-#       define RS600_EFFECTIVE_L2_QUEUE_SIZE(x) ((x) << 21)
-#       define RS600_INVALIDATE_ALL_L1_TLBS     (1 << 28)
-#       define RS600_INVALIDATE_L2_CACHE        (1 << 29)
-#define RS600_MC_PT0_CONTEXT0_CNTL              0x102
-#       define RS600_ENABLE_PAGE_TABLE          (1 << 0)
-#       define RS600_PAGE_TABLE_TYPE_FLAT       (0 << 1)
-#define RS600_MC_PT0_SYSTEM_APERTURE_LOW_ADDR   0x112
-#define RS600_MC_PT0_SYSTEM_APERTURE_HIGH_ADDR  0x114
-#define RS600_MC_PT0_CONTEXT0_DEFAULT_READ_ADDR 0x11c
-#define RS600_MC_PT0_CONTEXT0_FLAT_BASE_ADDR    0x12c
-#define RS600_MC_PT0_CONTEXT0_FLAT_START_ADDR   0x13c
-#define RS600_MC_PT0_CONTEXT0_FLAT_END_ADDR     0x14c
-#define RS600_MC_PT0_CLIENT0_CNTL               0x16c
-#       define RS600_ENABLE_TRANSLATION_MODE_OVERRIDE       (1 << 0)
-#       define RS600_TRANSLATION_MODE_OVERRIDE              (1 << 1)
-#       define RS600_SYSTEM_ACCESS_MODE_MASK                (3 << 8)
-#       define RS600_SYSTEM_ACCESS_MODE_PA_ONLY             (0 << 8)
-#       define RS600_SYSTEM_ACCESS_MODE_USE_SYS_MAP         (1 << 8)
-#       define RS600_SYSTEM_ACCESS_MODE_IN_SYS              (2 << 8)
-#       define RS600_SYSTEM_ACCESS_MODE_NOT_IN_SYS          (3 << 8)
-#       define RS600_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASSTHROUGH        (0 << 10)
-#       define RS600_SYSTEM_APERTURE_UNMAPPED_ACCESS_DEFAULT_PAGE       (1 << 10)
-#       define RS600_EFFECTIVE_L1_CACHE_SIZE(x) ((x) << 11)
-#       define RS600_ENABLE_FRAGMENT_PROCESSING (1 << 14)
-#       define RS600_EFFECTIVE_L1_QUEUE_SIZE(x) ((x) << 15)
-#       define RS600_INVALIDATE_L1_TLB          (1 << 20)
-
-#define R520_MC_IND_INDEX 0x70
-#define R520_MC_IND_WR_EN (1 << 24)
-#define R520_MC_IND_DATA  0x74
-
-#define RV515_MC_FB_LOCATION 0x01
-#define RV515_MC_AGP_LOCATION 0x02
-#define RV515_MC_AGP_BASE     0x03
-#define RV515_MC_AGP_BASE_2   0x04
-
-#define R520_MC_FB_LOCATION 0x04
-#define R520_MC_AGP_LOCATION 0x05
-#define R520_MC_AGP_BASE     0x06
-#define R520_MC_AGP_BASE_2   0x07
-
-#define RADEON_MPP_TB_CONFIG		0x01c0
-#define RADEON_MEM_CNTL			0x0140
-#define RADEON_MEM_SDRAM_MODE_REG	0x0158
-#define RADEON_AGP_BASE_2		0x015c /* r200+ only */
-#define RS480_AGP_BASE_2		0x0164
-#define RADEON_AGP_BASE			0x0170
-
-/* pipe config regs */
-#define R400_GB_PIPE_SELECT             0x402c
-#define RV530_GB_PIPE_SELECT2           0x4124
-#define R500_DYN_SCLK_PWMEM_PIPE        0x000d /* PLL */
-#define R300_GB_TILE_CONFIG             0x4018
-#       define R300_ENABLE_TILING       (1 << 0)
-#       define R300_PIPE_COUNT_RV350    (0 << 1)
-#       define R300_PIPE_COUNT_R300     (3 << 1)
-#       define R300_PIPE_COUNT_R420_3P  (6 << 1)
-#       define R300_PIPE_COUNT_R420     (7 << 1)
-#       define R300_TILE_SIZE_8         (0 << 4)
-#       define R300_TILE_SIZE_16        (1 << 4)
-#       define R300_TILE_SIZE_32        (2 << 4)
-#       define R300_SUBPIXEL_1_12       (0 << 16)
-#       define R300_SUBPIXEL_1_16       (1 << 16)
-#define R300_DST_PIPE_CONFIG            0x170c
-#       define R300_PIPE_AUTO_CONFIG    (1 << 31)
-#define R300_RB2D_DSTCACHE_MODE         0x3428
-#       define R300_DC_AUTOFLUSH_ENABLE (1 << 8)
-#       define R300_DC_DC_DISABLE_IGNORE_PE (1 << 17)
-
-#define RADEON_RB3D_COLOROFFSET		0x1c40
-#define RADEON_RB3D_COLORPITCH		0x1c48
-
-#define	RADEON_SRC_X_Y			0x1590
-
-#define RADEON_DP_GUI_MASTER_CNTL	0x146c
-#	define RADEON_GMC_SRC_PITCH_OFFSET_CNTL	(1 << 0)
-#	define RADEON_GMC_DST_PITCH_OFFSET_CNTL	(1 << 1)
-#	define RADEON_GMC_BRUSH_SOLID_COLOR	(13 << 4)
-#	define RADEON_GMC_BRUSH_NONE		(15 << 4)
-#	define RADEON_GMC_DST_16BPP		(4 << 8)
-#	define RADEON_GMC_DST_24BPP		(5 << 8)
-#	define RADEON_GMC_DST_32BPP		(6 << 8)
-#	define RADEON_GMC_DST_DATATYPE_SHIFT	8
-#	define RADEON_GMC_SRC_DATATYPE_COLOR	(3 << 12)
-#	define RADEON_DP_SRC_SOURCE_MEMORY	(2 << 24)
-#	define RADEON_DP_SRC_SOURCE_HOST_DATA	(3 << 24)
-#	define RADEON_GMC_CLR_CMP_CNTL_DIS	(1 << 28)
-#	define RADEON_GMC_WR_MSK_DIS		(1 << 30)
-#	define RADEON_ROP3_S			0x00cc0000
-#	define RADEON_ROP3_P			0x00f00000
-#define RADEON_DP_WRITE_MASK		0x16cc
-#define RADEON_SRC_PITCH_OFFSET		0x1428
-#define RADEON_DST_PITCH_OFFSET		0x142c
-#define RADEON_DST_PITCH_OFFSET_C	0x1c80
-#	define RADEON_DST_TILE_LINEAR		(0 << 30)
-#	define RADEON_DST_TILE_MACRO		(1 << 30)
-#	define RADEON_DST_TILE_MICRO		(2 << 30)
-#	define RADEON_DST_TILE_BOTH		(3 << 30)
-
-#define RADEON_SCRATCH_REG0		0x15e0
-#define RADEON_SCRATCH_REG1		0x15e4
-#define RADEON_SCRATCH_REG2		0x15e8
-#define RADEON_SCRATCH_REG3		0x15ec
-#define RADEON_SCRATCH_REG4		0x15f0
-#define RADEON_SCRATCH_REG5		0x15f4
-#define RADEON_SCRATCH_UMSK		0x0770
-#define RADEON_SCRATCH_ADDR		0x0774
-
-#define RADEON_SCRATCHOFF( x )		(RADEON_SCRATCH_REG_OFFSET + 4*(x))
-
-extern u32 radeon_get_scratch(drm_radeon_private_t *dev_priv, int index);
-
-#define GET_SCRATCH(dev_priv, x) radeon_get_scratch(dev_priv, x)
-
-#define R600_SCRATCH_REG0		0x8500
-#define R600_SCRATCH_REG1		0x8504
-#define R600_SCRATCH_REG2		0x8508
-#define R600_SCRATCH_REG3		0x850c
-#define R600_SCRATCH_REG4		0x8510
-#define R600_SCRATCH_REG5		0x8514
-#define R600_SCRATCH_REG6		0x8518
-#define R600_SCRATCH_REG7		0x851c
-#define R600_SCRATCH_UMSK		0x8540
-#define R600_SCRATCH_ADDR		0x8544
-
-#define R600_SCRATCHOFF(x)		(R600_SCRATCH_REG_OFFSET + 4*(x))
-
-#define RADEON_GEN_INT_CNTL		0x0040
-#	define RADEON_CRTC_VBLANK_MASK		(1 << 0)
-#	define RADEON_CRTC2_VBLANK_MASK		(1 << 9)
-#	define RADEON_GUI_IDLE_INT_ENABLE	(1 << 19)
-#	define RADEON_SW_INT_ENABLE		(1 << 25)
-
-#define RADEON_GEN_INT_STATUS		0x0044
-#	define RADEON_CRTC_VBLANK_STAT		(1 << 0)
-#	define RADEON_CRTC_VBLANK_STAT_ACK	(1 << 0)
-#	define RADEON_CRTC2_VBLANK_STAT		(1 << 9)
-#	define RADEON_CRTC2_VBLANK_STAT_ACK	(1 << 9)
-#	define RADEON_GUI_IDLE_INT_TEST_ACK     (1 << 19)
-#	define RADEON_SW_INT_TEST		(1 << 25)
-#	define RADEON_SW_INT_TEST_ACK		(1 << 25)
-#	define RADEON_SW_INT_FIRE		(1 << 26)
-#       define R500_DISPLAY_INT_STATUS          (1 << 0)
-
-#define RADEON_HOST_PATH_CNTL		0x0130
-#	define RADEON_HDP_SOFT_RESET		(1 << 26)
-#	define RADEON_HDP_WC_TIMEOUT_MASK	(7 << 28)
-#	define RADEON_HDP_WC_TIMEOUT_28BCLK	(7 << 28)
-
-#define RADEON_ISYNC_CNTL		0x1724
-#	define RADEON_ISYNC_ANY2D_IDLE3D	(1 << 0)
-#	define RADEON_ISYNC_ANY3D_IDLE2D	(1 << 1)
-#	define RADEON_ISYNC_TRIG2D_IDLE3D	(1 << 2)
-#	define RADEON_ISYNC_TRIG3D_IDLE2D	(1 << 3)
-#	define RADEON_ISYNC_WAIT_IDLEGUI	(1 << 4)
-#	define RADEON_ISYNC_CPSCRATCH_IDLEGUI	(1 << 5)
-
-#define RADEON_RBBM_GUICNTL		0x172c
-#	define RADEON_HOST_DATA_SWAP_NONE	(0 << 0)
-#	define RADEON_HOST_DATA_SWAP_16BIT	(1 << 0)
-#	define RADEON_HOST_DATA_SWAP_32BIT	(2 << 0)
-#	define RADEON_HOST_DATA_SWAP_HDW	(3 << 0)
-
-#define RADEON_MC_AGP_LOCATION		0x014c
-#define RADEON_MC_FB_LOCATION		0x0148
-#define RADEON_MCLK_CNTL		0x0012
-#	define RADEON_FORCEON_MCLKA		(1 << 16)
-#	define RADEON_FORCEON_MCLKB		(1 << 17)
-#	define RADEON_FORCEON_YCLKA		(1 << 18)
-#	define RADEON_FORCEON_YCLKB		(1 << 19)
-#	define RADEON_FORCEON_MC		(1 << 20)
-#	define RADEON_FORCEON_AIC		(1 << 21)
-
-#define RADEON_PP_BORDER_COLOR_0	0x1d40
-#define RADEON_PP_BORDER_COLOR_1	0x1d44
-#define RADEON_PP_BORDER_COLOR_2	0x1d48
-#define RADEON_PP_CNTL			0x1c38
-#	define RADEON_SCISSOR_ENABLE		(1 <<  1)
-#define RADEON_PP_LUM_MATRIX		0x1d00
-#define RADEON_PP_MISC			0x1c14
-#define RADEON_PP_ROT_MATRIX_0		0x1d58
-#define RADEON_PP_TXFILTER_0		0x1c54
-#define RADEON_PP_TXOFFSET_0		0x1c5c
-#define RADEON_PP_TXFILTER_1		0x1c6c
-#define RADEON_PP_TXFILTER_2		0x1c84
-
-#define R300_RB2D_DSTCACHE_CTLSTAT	0x342c /* use R300_DSTCACHE_CTLSTAT */
-#define R300_DSTCACHE_CTLSTAT		0x1714
-#	define R300_RB2D_DC_FLUSH		(3 << 0)
-#	define R300_RB2D_DC_FREE		(3 << 2)
-#	define R300_RB2D_DC_FLUSH_ALL		0xf
-#	define R300_RB2D_DC_BUSY		(1 << 31)
-#define RADEON_RB3D_CNTL		0x1c3c
-#	define RADEON_ALPHA_BLEND_ENABLE	(1 << 0)
-#	define RADEON_PLANE_MASK_ENABLE		(1 << 1)
-#	define RADEON_DITHER_ENABLE		(1 << 2)
-#	define RADEON_ROUND_ENABLE		(1 << 3)
-#	define RADEON_SCALE_DITHER_ENABLE	(1 << 4)
-#	define RADEON_DITHER_INIT		(1 << 5)
-#	define RADEON_ROP_ENABLE		(1 << 6)
-#	define RADEON_STENCIL_ENABLE		(1 << 7)
-#	define RADEON_Z_ENABLE			(1 << 8)
-#	define RADEON_ZBLOCK16			(1 << 15)
-#define RADEON_RB3D_DEPTHOFFSET		0x1c24
-#define RADEON_RB3D_DEPTHCLEARVALUE	0x3230
-#define RADEON_RB3D_DEPTHPITCH		0x1c28
-#define RADEON_RB3D_PLANEMASK		0x1d84
-#define RADEON_RB3D_STENCILREFMASK	0x1d7c
-#define RADEON_RB3D_ZCACHE_MODE		0x3250
-#define RADEON_RB3D_ZCACHE_CTLSTAT	0x3254
-#	define RADEON_RB3D_ZC_FLUSH		(1 << 0)
-#	define RADEON_RB3D_ZC_FREE		(1 << 2)
-#	define RADEON_RB3D_ZC_FLUSH_ALL		0x5
-#	define RADEON_RB3D_ZC_BUSY		(1 << 31)
-#define R300_ZB_ZCACHE_CTLSTAT                  0x4f18
-#	define R300_ZC_FLUSH		        (1 << 0)
-#	define R300_ZC_FREE		        (1 << 1)
-#	define R300_ZC_BUSY		        (1 << 31)
-#define RADEON_RB3D_DSTCACHE_CTLSTAT	0x325c
-#	define RADEON_RB3D_DC_FLUSH		(3 << 0)
-#	define RADEON_RB3D_DC_FREE		(3 << 2)
-#	define RADEON_RB3D_DC_FLUSH_ALL		0xf
-#	define RADEON_RB3D_DC_BUSY		(1 << 31)
-#define R300_RB3D_DSTCACHE_CTLSTAT              0x4e4c
-#	define R300_RB3D_DC_FLUSH		(2 << 0)
-#	define R300_RB3D_DC_FREE		(2 << 2)
-#	define R300_RB3D_DC_FINISH		(1 << 4)
-#define RADEON_RB3D_ZSTENCILCNTL	0x1c2c
-#	define RADEON_Z_TEST_MASK		(7 << 4)
-#	define RADEON_Z_TEST_ALWAYS		(7 << 4)
-#	define RADEON_Z_HIERARCHY_ENABLE	(1 << 8)
-#	define RADEON_STENCIL_TEST_ALWAYS	(7 << 12)
-#	define RADEON_STENCIL_S_FAIL_REPLACE	(2 << 16)
-#	define RADEON_STENCIL_ZPASS_REPLACE	(2 << 20)
-#	define RADEON_STENCIL_ZFAIL_REPLACE	(2 << 24)
-#	define RADEON_Z_COMPRESSION_ENABLE	(1 << 28)
-#	define RADEON_FORCE_Z_DIRTY		(1 << 29)
-#	define RADEON_Z_WRITE_ENABLE		(1 << 30)
-#	define RADEON_Z_DECOMPRESSION_ENABLE	(1 << 31)
-#define RADEON_RBBM_SOFT_RESET		0x00f0
-#	define RADEON_SOFT_RESET_CP		(1 <<  0)
-#	define RADEON_SOFT_RESET_HI		(1 <<  1)
-#	define RADEON_SOFT_RESET_SE		(1 <<  2)
-#	define RADEON_SOFT_RESET_RE		(1 <<  3)
-#	define RADEON_SOFT_RESET_PP		(1 <<  4)
-#	define RADEON_SOFT_RESET_E2		(1 <<  5)
-#	define RADEON_SOFT_RESET_RB		(1 <<  6)
-#	define RADEON_SOFT_RESET_HDP		(1 <<  7)
-/*
- *   6:0  Available slots in the FIFO
- *   8    Host Interface active
- *   9    CP request active
- *   10   FIFO request active
- *   11   Host Interface retry active
- *   12   CP retry active
- *   13   FIFO retry active
- *   14   FIFO pipeline busy
- *   15   Event engine busy
- *   16   CP command stream busy
- *   17   2D engine busy
- *   18   2D portion of render backend busy
- *   20   3D setup engine busy
- *   26   GA engine busy
- *   27   CBA 2D engine busy
- *   31   2D engine busy or 3D engine busy or FIFO not empty or CP busy or
- *           command stream queue not empty or Ring Buffer not empty
- */
-#define RADEON_RBBM_STATUS		0x0e40
-/* Same as the previous RADEON_RBBM_STATUS; this is a mirror of that register.  */
-/* #define RADEON_RBBM_STATUS		0x1740 */
-/* bits 6:0 are dword slots available in the cmd fifo */
-#	define RADEON_RBBM_FIFOCNT_MASK		0x007f
-#	define RADEON_HIRQ_ON_RBB	(1 <<  8)
-#	define RADEON_CPRQ_ON_RBB	(1 <<  9)
-#	define RADEON_CFRQ_ON_RBB	(1 << 10)
-#	define RADEON_HIRQ_IN_RTBUF	(1 << 11)
-#	define RADEON_CPRQ_IN_RTBUF	(1 << 12)
-#	define RADEON_CFRQ_IN_RTBUF	(1 << 13)
-#	define RADEON_PIPE_BUSY		(1 << 14)
-#	define RADEON_ENG_EV_BUSY	(1 << 15)
-#	define RADEON_CP_CMDSTRM_BUSY	(1 << 16)
-#	define RADEON_E2_BUSY		(1 << 17)
-#	define RADEON_RB2D_BUSY		(1 << 18)
-#	define RADEON_RB3D_BUSY		(1 << 19) /* not used on r300 */
-#	define RADEON_VAP_BUSY		(1 << 20)
-#	define RADEON_RE_BUSY		(1 << 21) /* not used on r300 */
-#	define RADEON_TAM_BUSY		(1 << 22) /* not used on r300 */
-#	define RADEON_TDM_BUSY		(1 << 23) /* not used on r300 */
-#	define RADEON_PB_BUSY		(1 << 24) /* not used on r300 */
-#	define RADEON_TIM_BUSY		(1 << 25) /* not used on r300 */
-#	define RADEON_GA_BUSY		(1 << 26)
-#	define RADEON_CBA2D_BUSY	(1 << 27)
-#	define RADEON_RBBM_ACTIVE	(1 << 31)
-#define RADEON_RE_LINE_PATTERN		0x1cd0
-#define RADEON_RE_MISC			0x26c4
-#define RADEON_RE_TOP_LEFT		0x26c0
-#define RADEON_RE_WIDTH_HEIGHT		0x1c44
-#define RADEON_RE_STIPPLE_ADDR		0x1cc8
-#define RADEON_RE_STIPPLE_DATA		0x1ccc
-
-#define RADEON_SCISSOR_TL_0		0x1cd8
-#define RADEON_SCISSOR_BR_0		0x1cdc
-#define RADEON_SCISSOR_TL_1		0x1ce0
-#define RADEON_SCISSOR_BR_1		0x1ce4
-#define RADEON_SCISSOR_TL_2		0x1ce8
-#define RADEON_SCISSOR_BR_2		0x1cec
-#define RADEON_SE_COORD_FMT		0x1c50
-#define RADEON_SE_CNTL			0x1c4c
-#	define RADEON_FFACE_CULL_CW		(0 << 0)
-#	define RADEON_BFACE_SOLID		(3 << 1)
-#	define RADEON_FFACE_SOLID		(3 << 3)
-#	define RADEON_FLAT_SHADE_VTX_LAST	(3 << 6)
-#	define RADEON_DIFFUSE_SHADE_FLAT	(1 << 8)
-#	define RADEON_DIFFUSE_SHADE_GOURAUD	(2 << 8)
-#	define RADEON_ALPHA_SHADE_FLAT		(1 << 10)
-#	define RADEON_ALPHA_SHADE_GOURAUD	(2 << 10)
-#	define RADEON_SPECULAR_SHADE_FLAT	(1 << 12)
-#	define RADEON_SPECULAR_SHADE_GOURAUD	(2 << 12)
-#	define RADEON_FOG_SHADE_FLAT		(1 << 14)
-#	define RADEON_FOG_SHADE_GOURAUD		(2 << 14)
-#	define RADEON_VPORT_XY_XFORM_ENABLE	(1 << 24)
-#	define RADEON_VPORT_Z_XFORM_ENABLE	(1 << 25)
-#	define RADEON_VTX_PIX_CENTER_OGL	(1 << 27)
-#	define RADEON_ROUND_MODE_TRUNC		(0 << 28)
-#	define RADEON_ROUND_PREC_8TH_PIX	(1 << 30)
-#define RADEON_SE_CNTL_STATUS		0x2140
-#define RADEON_SE_LINE_WIDTH		0x1db8
-#define RADEON_SE_VPORT_XSCALE		0x1d98
-#define RADEON_SE_ZBIAS_FACTOR		0x1db0
-#define RADEON_SE_TCL_MATERIAL_EMMISSIVE_RED 0x2210
-#define RADEON_SE_TCL_OUTPUT_VTX_FMT         0x2254
-#define RADEON_SE_TCL_VECTOR_INDX_REG        0x2200
-#       define RADEON_VEC_INDX_OCTWORD_STRIDE_SHIFT  16
-#       define RADEON_VEC_INDX_DWORD_COUNT_SHIFT     28
-#define RADEON_SE_TCL_VECTOR_DATA_REG       0x2204
-#define RADEON_SE_TCL_SCALAR_INDX_REG       0x2208
-#       define RADEON_SCAL_INDX_DWORD_STRIDE_SHIFT  16
-#define RADEON_SE_TCL_SCALAR_DATA_REG       0x220C
-#define RADEON_SURFACE_ACCESS_FLAGS	0x0bf8
-#define RADEON_SURFACE_ACCESS_CLR	0x0bfc
-#define RADEON_SURFACE_CNTL		0x0b00
-#	define RADEON_SURF_TRANSLATION_DIS	(1 << 8)
-#	define RADEON_NONSURF_AP0_SWP_MASK	(3 << 20)
-#	define RADEON_NONSURF_AP0_SWP_LITTLE	(0 << 20)
-#	define RADEON_NONSURF_AP0_SWP_BIG16	(1 << 20)
-#	define RADEON_NONSURF_AP0_SWP_BIG32	(2 << 20)
-#	define RADEON_NONSURF_AP1_SWP_MASK	(3 << 22)
-#	define RADEON_NONSURF_AP1_SWP_LITTLE	(0 << 22)
-#	define RADEON_NONSURF_AP1_SWP_BIG16	(1 << 22)
-#	define RADEON_NONSURF_AP1_SWP_BIG32	(2 << 22)
-#define RADEON_SURFACE0_INFO		0x0b0c
-#	define RADEON_SURF_PITCHSEL_MASK	(0x1ff << 0)
-#	define RADEON_SURF_TILE_MODE_MASK	(3 << 16)
-#	define RADEON_SURF_TILE_MODE_MACRO	(0 << 16)
-#	define RADEON_SURF_TILE_MODE_MICRO	(1 << 16)
-#	define RADEON_SURF_TILE_MODE_32BIT_Z	(2 << 16)
-#	define RADEON_SURF_TILE_MODE_16BIT_Z	(3 << 16)
-#define RADEON_SURFACE0_LOWER_BOUND	0x0b04
-#define RADEON_SURFACE0_UPPER_BOUND	0x0b08
-#	define RADEON_SURF_ADDRESS_FIXED_MASK	(0x3ff << 0)
-#define RADEON_SURFACE1_INFO		0x0b1c
-#define RADEON_SURFACE1_LOWER_BOUND	0x0b14
-#define RADEON_SURFACE1_UPPER_BOUND	0x0b18
-#define RADEON_SURFACE2_INFO		0x0b2c
-#define RADEON_SURFACE2_LOWER_BOUND	0x0b24
-#define RADEON_SURFACE2_UPPER_BOUND	0x0b28
-#define RADEON_SURFACE3_INFO		0x0b3c
-#define RADEON_SURFACE3_LOWER_BOUND	0x0b34
-#define RADEON_SURFACE3_UPPER_BOUND	0x0b38
-#define RADEON_SURFACE4_INFO		0x0b4c
-#define RADEON_SURFACE4_LOWER_BOUND	0x0b44
-#define RADEON_SURFACE4_UPPER_BOUND	0x0b48
-#define RADEON_SURFACE5_INFO		0x0b5c
-#define RADEON_SURFACE5_LOWER_BOUND	0x0b54
-#define RADEON_SURFACE5_UPPER_BOUND	0x0b58
-#define RADEON_SURFACE6_INFO		0x0b6c
-#define RADEON_SURFACE6_LOWER_BOUND	0x0b64
-#define RADEON_SURFACE6_UPPER_BOUND	0x0b68
-#define RADEON_SURFACE7_INFO		0x0b7c
-#define RADEON_SURFACE7_LOWER_BOUND	0x0b74
-#define RADEON_SURFACE7_UPPER_BOUND	0x0b78
-#define RADEON_SW_SEMAPHORE		0x013c
-
-#define RADEON_WAIT_UNTIL		0x1720
-#	define RADEON_WAIT_CRTC_PFLIP		(1 << 0)
-#	define RADEON_WAIT_2D_IDLE		(1 << 14)
-#	define RADEON_WAIT_3D_IDLE		(1 << 15)
-#	define RADEON_WAIT_2D_IDLECLEAN		(1 << 16)
-#	define RADEON_WAIT_3D_IDLECLEAN		(1 << 17)
-#	define RADEON_WAIT_HOST_IDLECLEAN	(1 << 18)
-
-#define RADEON_RB3D_ZMASKOFFSET		0x3234
-#define RADEON_RB3D_ZSTENCILCNTL	0x1c2c
-#	define RADEON_DEPTH_FORMAT_16BIT_INT_Z	(0 << 0)
-#	define RADEON_DEPTH_FORMAT_24BIT_INT_Z	(2 << 0)
-
-/* CP registers */
-#define RADEON_CP_ME_RAM_ADDR		0x07d4
-#define RADEON_CP_ME_RAM_RADDR		0x07d8
-#define RADEON_CP_ME_RAM_DATAH		0x07dc
-#define RADEON_CP_ME_RAM_DATAL		0x07e0
-
-#define RADEON_CP_RB_BASE		0x0700
-#define RADEON_CP_RB_CNTL		0x0704
-#	define RADEON_BUF_SWAP_32BIT		(2 << 16)
-#	define RADEON_RB_NO_UPDATE		(1 << 27)
-#	define RADEON_RB_RPTR_WR_ENA		(1 << 31)
-#define RADEON_CP_RB_RPTR_ADDR		0x070c
-#define RADEON_CP_RB_RPTR		0x0710
-#define RADEON_CP_RB_WPTR		0x0714
-
-#define RADEON_CP_RB_WPTR_DELAY		0x0718
-#	define RADEON_PRE_WRITE_TIMER_SHIFT	0
-#	define RADEON_PRE_WRITE_LIMIT_SHIFT	23
-
-#define RADEON_CP_IB_BASE		0x0738
-
-#define RADEON_CP_CSQ_CNTL		0x0740
-#	define RADEON_CSQ_CNT_PRIMARY_MASK	(0xff << 0)
-#	define RADEON_CSQ_PRIDIS_INDDIS		(0 << 28)
-#	define RADEON_CSQ_PRIPIO_INDDIS		(1 << 28)
-#	define RADEON_CSQ_PRIBM_INDDIS		(2 << 28)
-#	define RADEON_CSQ_PRIPIO_INDBM		(3 << 28)
-#	define RADEON_CSQ_PRIBM_INDBM		(4 << 28)
-#	define RADEON_CSQ_PRIPIO_INDPIO		(15 << 28)
-
-#define R300_CP_RESYNC_ADDR		0x0778
-#define R300_CP_RESYNC_DATA		0x077c
-
-#define RADEON_AIC_CNTL			0x01d0
-#	define RADEON_PCIGART_TRANSLATE_EN	(1 << 0)
-#	define RS400_MSI_REARM	                (1 << 3)
-#define RADEON_AIC_STAT			0x01d4
-#define RADEON_AIC_PT_BASE		0x01d8
-#define RADEON_AIC_LO_ADDR		0x01dc
-#define RADEON_AIC_HI_ADDR		0x01e0
-#define RADEON_AIC_TLB_ADDR		0x01e4
-#define RADEON_AIC_TLB_DATA		0x01e8
-
-/* CP command packets */
-#define RADEON_CP_PACKET0		0x00000000
-#	define RADEON_ONE_REG_WR		(1 << 15)
-#define RADEON_CP_PACKET1		0x40000000
-#define RADEON_CP_PACKET2		0x80000000
-#define RADEON_CP_PACKET3		0xC0000000
-#       define RADEON_CP_NOP                    0x00001000
-#       define RADEON_CP_NEXT_CHAR              0x00001900
-#       define RADEON_CP_PLY_NEXTSCAN           0x00001D00
-#       define RADEON_CP_SET_SCISSORS           0x00001E00
-	     /* GEN_INDX_PRIM is unsupported starting with R300 */
-#	define RADEON_3D_RNDR_GEN_INDX_PRIM	0x00002300
-#	define RADEON_WAIT_FOR_IDLE		0x00002600
-#	define RADEON_3D_DRAW_VBUF		0x00002800
-#	define RADEON_3D_DRAW_IMMD		0x00002900
-#	define RADEON_3D_DRAW_INDX		0x00002A00
-#       define RADEON_CP_LOAD_PALETTE           0x00002C00
-#	define RADEON_3D_LOAD_VBPNTR		0x00002F00
-#	define RADEON_MPEG_IDCT_MACROBLOCK	0x00003000
-#	define RADEON_MPEG_IDCT_MACROBLOCK_REV	0x00003100
-#	define RADEON_3D_CLEAR_ZMASK		0x00003200
-#	define RADEON_CP_INDX_BUFFER		0x00003300
-#       define RADEON_CP_3D_DRAW_VBUF_2         0x00003400
-#       define RADEON_CP_3D_DRAW_IMMD_2         0x00003500
-#       define RADEON_CP_3D_DRAW_INDX_2         0x00003600
-#	define RADEON_3D_CLEAR_HIZ		0x00003700
-#       define RADEON_CP_3D_CLEAR_CMASK         0x00003802
-#	define RADEON_CNTL_HOSTDATA_BLT		0x00009400
-#	define RADEON_CNTL_PAINT_MULTI		0x00009A00
-#	define RADEON_CNTL_BITBLT_MULTI		0x00009B00
-#	define RADEON_CNTL_SET_SCISSORS		0xC0001E00
-
-#       define R600_IT_INDIRECT_BUFFER_END      0x00001700
-#       define R600_IT_SET_PREDICATION          0x00002000
-#       define R600_IT_REG_RMW                  0x00002100
-#       define R600_IT_COND_EXEC                0x00002200
-#       define R600_IT_PRED_EXEC                0x00002300
-#       define R600_IT_START_3D_CMDBUF          0x00002400
-#       define R600_IT_DRAW_INDEX_2             0x00002700
-#       define R600_IT_CONTEXT_CONTROL          0x00002800
-#       define R600_IT_DRAW_INDEX_IMMD_BE       0x00002900
-#       define R600_IT_INDEX_TYPE               0x00002A00
-#       define R600_IT_DRAW_INDEX               0x00002B00
-#       define R600_IT_DRAW_INDEX_AUTO          0x00002D00
-#       define R600_IT_DRAW_INDEX_IMMD          0x00002E00
-#       define R600_IT_NUM_INSTANCES            0x00002F00
-#       define R600_IT_STRMOUT_BUFFER_UPDATE    0x00003400
-#       define R600_IT_INDIRECT_BUFFER_MP       0x00003800
-#       define R600_IT_MEM_SEMAPHORE            0x00003900
-#       define R600_IT_MPEG_INDEX               0x00003A00
-#       define R600_IT_WAIT_REG_MEM             0x00003C00
-#       define R600_IT_MEM_WRITE                0x00003D00
-#       define R600_IT_INDIRECT_BUFFER          0x00003200
-#       define R600_IT_SURFACE_SYNC             0x00004300
-#              define R600_CB0_DEST_BASE_ENA    (1 << 6)
-#              define R600_TC_ACTION_ENA        (1 << 23)
-#              define R600_VC_ACTION_ENA        (1 << 24)
-#              define R600_CB_ACTION_ENA        (1 << 25)
-#              define R600_DB_ACTION_ENA        (1 << 26)
-#              define R600_SH_ACTION_ENA        (1 << 27)
-#              define R600_SMX_ACTION_ENA       (1 << 28)
-#       define R600_IT_ME_INITIALIZE            0x00004400
-#	       define R600_ME_INITIALIZE_DEVICE_ID(x) ((x) << 16)
-#       define R600_IT_COND_WRITE               0x00004500
-#       define R600_IT_EVENT_WRITE              0x00004600
-#       define R600_IT_EVENT_WRITE_EOP          0x00004700
-#       define R600_IT_ONE_REG_WRITE            0x00005700
-#       define R600_IT_SET_CONFIG_REG           0x00006800
-#              define R600_SET_CONFIG_REG_OFFSET 0x00008000
-#              define R600_SET_CONFIG_REG_END   0x0000ac00
-#       define R600_IT_SET_CONTEXT_REG          0x00006900
-#              define R600_SET_CONTEXT_REG_OFFSET 0x00028000
-#              define R600_SET_CONTEXT_REG_END  0x00029000
-#       define R600_IT_SET_ALU_CONST            0x00006A00
-#              define R600_SET_ALU_CONST_OFFSET 0x00030000
-#              define R600_SET_ALU_CONST_END    0x00032000
-#       define R600_IT_SET_BOOL_CONST           0x00006B00
-#              define R600_SET_BOOL_CONST_OFFSET 0x0003e380
-#              define R600_SET_BOOL_CONST_END   0x00040000
-#       define R600_IT_SET_LOOP_CONST           0x00006C00
-#              define R600_SET_LOOP_CONST_OFFSET 0x0003e200
-#              define R600_SET_LOOP_CONST_END   0x0003e380
-#       define R600_IT_SET_RESOURCE             0x00006D00
-#              define R600_SET_RESOURCE_OFFSET  0x00038000
-#              define R600_SET_RESOURCE_END     0x0003c000
-#              define R600_SQ_TEX_VTX_INVALID_TEXTURE  0x0
-#              define R600_SQ_TEX_VTX_INVALID_BUFFER   0x1
-#              define R600_SQ_TEX_VTX_VALID_TEXTURE    0x2
-#              define R600_SQ_TEX_VTX_VALID_BUFFER     0x3
-#       define R600_IT_SET_SAMPLER              0x00006E00
-#              define R600_SET_SAMPLER_OFFSET   0x0003c000
-#              define R600_SET_SAMPLER_END      0x0003cff0
-#       define R600_IT_SET_CTL_CONST            0x00006F00
-#              define R600_SET_CTL_CONST_OFFSET 0x0003cff0
-#              define R600_SET_CTL_CONST_END    0x0003e200
-#       define R600_IT_SURFACE_BASE_UPDATE      0x00007300
-
-#define RADEON_CP_PACKET_MASK		0xC0000000
-#define RADEON_CP_PACKET_COUNT_MASK	0x3fff0000
-#define RADEON_CP_PACKET0_REG_MASK	0x000007ff
-#define RADEON_CP_PACKET1_REG0_MASK	0x000007ff
-#define RADEON_CP_PACKET1_REG1_MASK	0x003ff800
-
-#define RADEON_VTX_Z_PRESENT			(1 << 31)
-#define RADEON_VTX_PKCOLOR_PRESENT		(1 << 3)
-
-#define RADEON_PRIM_TYPE_NONE			(0 << 0)
-#define RADEON_PRIM_TYPE_POINT			(1 << 0)
-#define RADEON_PRIM_TYPE_LINE			(2 << 0)
-#define RADEON_PRIM_TYPE_LINE_STRIP		(3 << 0)
-#define RADEON_PRIM_TYPE_TRI_LIST		(4 << 0)
-#define RADEON_PRIM_TYPE_TRI_FAN		(5 << 0)
-#define RADEON_PRIM_TYPE_TRI_STRIP		(6 << 0)
-#define RADEON_PRIM_TYPE_TRI_TYPE2		(7 << 0)
-#define RADEON_PRIM_TYPE_RECT_LIST		(8 << 0)
-#define RADEON_PRIM_TYPE_3VRT_POINT_LIST	(9 << 0)
-#define RADEON_PRIM_TYPE_3VRT_LINE_LIST		(10 << 0)
-#define RADEON_PRIM_TYPE_MASK                   0xf
-#define RADEON_PRIM_WALK_IND			(1 << 4)
-#define RADEON_PRIM_WALK_LIST			(2 << 4)
-#define RADEON_PRIM_WALK_RING			(3 << 4)
-#define RADEON_COLOR_ORDER_BGRA			(0 << 6)
-#define RADEON_COLOR_ORDER_RGBA			(1 << 6)
-#define RADEON_MAOS_ENABLE			(1 << 7)
-#define RADEON_VTX_FMT_R128_MODE		(0 << 8)
-#define RADEON_VTX_FMT_RADEON_MODE		(1 << 8)
-#define RADEON_NUM_VERTICES_SHIFT		16
-
-#define RADEON_COLOR_FORMAT_CI8		2
-#define RADEON_COLOR_FORMAT_ARGB1555	3
-#define RADEON_COLOR_FORMAT_RGB565	4
-#define RADEON_COLOR_FORMAT_ARGB8888	6
-#define RADEON_COLOR_FORMAT_RGB332	7
-#define RADEON_COLOR_FORMAT_RGB8	9
-#define RADEON_COLOR_FORMAT_ARGB4444	15
-
-#define RADEON_TXFORMAT_I8		0
-#define RADEON_TXFORMAT_AI88		1
-#define RADEON_TXFORMAT_RGB332		2
-#define RADEON_TXFORMAT_ARGB1555	3
-#define RADEON_TXFORMAT_RGB565		4
-#define RADEON_TXFORMAT_ARGB4444	5
-#define RADEON_TXFORMAT_ARGB8888	6
-#define RADEON_TXFORMAT_RGBA8888	7
-#define RADEON_TXFORMAT_Y8		8
-#define RADEON_TXFORMAT_VYUY422         10
-#define RADEON_TXFORMAT_YVYU422         11
-#define RADEON_TXFORMAT_DXT1            12
-#define RADEON_TXFORMAT_DXT23           14
-#define RADEON_TXFORMAT_DXT45           15
-
-#define R200_PP_TXCBLEND_0                0x2f00
-#define R200_PP_TXCBLEND_1                0x2f10
-#define R200_PP_TXCBLEND_2                0x2f20
-#define R200_PP_TXCBLEND_3                0x2f30
-#define R200_PP_TXCBLEND_4                0x2f40
-#define R200_PP_TXCBLEND_5                0x2f50
-#define R200_PP_TXCBLEND_6                0x2f60
-#define R200_PP_TXCBLEND_7                0x2f70
-#define R200_SE_TCL_LIGHT_MODEL_CTL_0     0x2268
-#define R200_PP_TFACTOR_0                 0x2ee0
-#define R200_SE_VTX_FMT_0                 0x2088
-#define R200_SE_VAP_CNTL                  0x2080
-#define R200_SE_TCL_MATRIX_SEL_0          0x2230
-#define R200_SE_TCL_TEX_PROC_CTL_2        0x22a8
-#define R200_SE_TCL_UCP_VERT_BLEND_CTL    0x22c0
-#define R200_PP_TXFILTER_5                0x2ca0
-#define R200_PP_TXFILTER_4                0x2c80
-#define R200_PP_TXFILTER_3                0x2c60
-#define R200_PP_TXFILTER_2                0x2c40
-#define R200_PP_TXFILTER_1                0x2c20
-#define R200_PP_TXFILTER_0                0x2c00
-#define R200_PP_TXOFFSET_5                0x2d78
-#define R200_PP_TXOFFSET_4                0x2d60
-#define R200_PP_TXOFFSET_3                0x2d48
-#define R200_PP_TXOFFSET_2                0x2d30
-#define R200_PP_TXOFFSET_1                0x2d18
-#define R200_PP_TXOFFSET_0                0x2d00
-
-#define R200_PP_CUBIC_FACES_0             0x2c18
-#define R200_PP_CUBIC_FACES_1             0x2c38
-#define R200_PP_CUBIC_FACES_2             0x2c58
-#define R200_PP_CUBIC_FACES_3             0x2c78
-#define R200_PP_CUBIC_FACES_4             0x2c98
-#define R200_PP_CUBIC_FACES_5             0x2cb8
-#define R200_PP_CUBIC_OFFSET_F1_0         0x2d04
-#define R200_PP_CUBIC_OFFSET_F2_0         0x2d08
-#define R200_PP_CUBIC_OFFSET_F3_0         0x2d0c
-#define R200_PP_CUBIC_OFFSET_F4_0         0x2d10
-#define R200_PP_CUBIC_OFFSET_F5_0         0x2d14
-#define R200_PP_CUBIC_OFFSET_F1_1         0x2d1c
-#define R200_PP_CUBIC_OFFSET_F2_1         0x2d20
-#define R200_PP_CUBIC_OFFSET_F3_1         0x2d24
-#define R200_PP_CUBIC_OFFSET_F4_1         0x2d28
-#define R200_PP_CUBIC_OFFSET_F5_1         0x2d2c
-#define R200_PP_CUBIC_OFFSET_F1_2         0x2d34
-#define R200_PP_CUBIC_OFFSET_F2_2         0x2d38
-#define R200_PP_CUBIC_OFFSET_F3_2         0x2d3c
-#define R200_PP_CUBIC_OFFSET_F4_2         0x2d40
-#define R200_PP_CUBIC_OFFSET_F5_2         0x2d44
-#define R200_PP_CUBIC_OFFSET_F1_3         0x2d4c
-#define R200_PP_CUBIC_OFFSET_F2_3         0x2d50
-#define R200_PP_CUBIC_OFFSET_F3_3         0x2d54
-#define R200_PP_CUBIC_OFFSET_F4_3         0x2d58
-#define R200_PP_CUBIC_OFFSET_F5_3         0x2d5c
-#define R200_PP_CUBIC_OFFSET_F1_4         0x2d64
-#define R200_PP_CUBIC_OFFSET_F2_4         0x2d68
-#define R200_PP_CUBIC_OFFSET_F3_4         0x2d6c
-#define R200_PP_CUBIC_OFFSET_F4_4         0x2d70
-#define R200_PP_CUBIC_OFFSET_F5_4         0x2d74
-#define R200_PP_CUBIC_OFFSET_F1_5         0x2d7c
-#define R200_PP_CUBIC_OFFSET_F2_5         0x2d80
-#define R200_PP_CUBIC_OFFSET_F3_5         0x2d84
-#define R200_PP_CUBIC_OFFSET_F4_5         0x2d88
-#define R200_PP_CUBIC_OFFSET_F5_5         0x2d8c
-
-#define R200_RE_AUX_SCISSOR_CNTL          0x26f0
-#define R200_SE_VTE_CNTL                  0x20b0
-#define R200_SE_TCL_OUTPUT_VTX_COMP_SEL   0x2250
-#define R200_PP_TAM_DEBUG3                0x2d9c
-#define R200_PP_CNTL_X                    0x2cc4
-#define R200_SE_VAP_CNTL_STATUS           0x2140
-#define R200_RE_SCISSOR_TL_0              0x1cd8
-#define R200_RE_SCISSOR_TL_1              0x1ce0
-#define R200_RE_SCISSOR_TL_2              0x1ce8
-#define R200_RB3D_DEPTHXY_OFFSET          0x1d60
-#define R200_RE_AUX_SCISSOR_CNTL          0x26f0
-#define R200_SE_VTX_STATE_CNTL            0x2180
-#define R200_RE_POINTSIZE                 0x2648
-#define R200_SE_TCL_INPUT_VTX_VECTOR_ADDR_0 0x2254
-
-#define RADEON_PP_TEX_SIZE_0                0x1d04	/* NPOT */
-#define RADEON_PP_TEX_SIZE_1                0x1d0c
-#define RADEON_PP_TEX_SIZE_2                0x1d14
-
-#define RADEON_PP_CUBIC_FACES_0             0x1d24
-#define RADEON_PP_CUBIC_FACES_1             0x1d28
-#define RADEON_PP_CUBIC_FACES_2             0x1d2c
-#define RADEON_PP_CUBIC_OFFSET_T0_0         0x1dd0	/* bits [31:5] */
-#define RADEON_PP_CUBIC_OFFSET_T1_0         0x1e00
-#define RADEON_PP_CUBIC_OFFSET_T2_0         0x1e14
-
-#define RADEON_SE_TCL_STATE_FLUSH           0x2284
-
-#define SE_VAP_CNTL__TCL_ENA_MASK                          0x00000001
-#define SE_VAP_CNTL__FORCE_W_TO_ONE_MASK                   0x00010000
-#define SE_VAP_CNTL__VF_MAX_VTX_NUM__SHIFT                 0x00000012
-#define SE_VTE_CNTL__VTX_XY_FMT_MASK                       0x00000100
-#define SE_VTE_CNTL__VTX_Z_FMT_MASK                        0x00000200
-#define SE_VTX_FMT_0__VTX_Z0_PRESENT_MASK                  0x00000001
-#define SE_VTX_FMT_0__VTX_W0_PRESENT_MASK                  0x00000002
-#define SE_VTX_FMT_0__VTX_COLOR_0_FMT__SHIFT               0x0000000b
-#define R200_3D_DRAW_IMMD_2      0xC0003500
-#define R200_SE_VTX_FMT_1                 0x208c
-#define R200_RE_CNTL                      0x1c50
-
-#define R200_RB3D_BLENDCOLOR              0x3218
-
-#define R200_SE_TCL_POINT_SPRITE_CNTL     0x22c4
-
-#define R200_PP_TRI_PERF 0x2cf8
-
-#define R200_PP_AFS_0                     0x2f80
-#define R200_PP_AFS_1                     0x2f00	/* same as txcblend_0 */
-
-#define R200_VAP_PVS_CNTL_1               0x22D0
-
-#define RADEON_CRTC_CRNT_FRAME 0x0214
-#define RADEON_CRTC2_CRNT_FRAME 0x0314
-
-#define R500_D1CRTC_STATUS 0x609c
-#define R500_D2CRTC_STATUS 0x689c
-#define R500_CRTC_V_BLANK (1<<0)
-
-#define R500_D1CRTC_FRAME_COUNT 0x60a4
-#define R500_D2CRTC_FRAME_COUNT 0x68a4
-
-#define R500_D1MODE_V_COUNTER 0x6530
-#define R500_D2MODE_V_COUNTER 0x6d30
-
-#define R500_D1MODE_VBLANK_STATUS 0x6534
-#define R500_D2MODE_VBLANK_STATUS 0x6d34
-#define R500_VBLANK_OCCURED (1<<0)
-#define R500_VBLANK_ACK     (1<<4)
-#define R500_VBLANK_STAT    (1<<12)
-#define R500_VBLANK_INT     (1<<16)
-
-#define R500_DxMODE_INT_MASK 0x6540
-#define R500_D1MODE_INT_MASK (1<<0)
-#define R500_D2MODE_INT_MASK (1<<8)
-
-#define R500_DISP_INTERRUPT_STATUS 0x7edc
-#define R500_D1_VBLANK_INTERRUPT (1 << 4)
-#define R500_D2_VBLANK_INTERRUPT (1 << 5)
-
-/* R6xx/R7xx registers */
-#define R600_MC_VM_FB_LOCATION                                 0x2180
-#define R600_MC_VM_AGP_TOP                                     0x2184
-#define R600_MC_VM_AGP_BOT                                     0x2188
-#define R600_MC_VM_AGP_BASE                                    0x218c
-#define R600_MC_VM_SYSTEM_APERTURE_LOW_ADDR                    0x2190
-#define R600_MC_VM_SYSTEM_APERTURE_HIGH_ADDR                   0x2194
-#define R600_MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR                0x2198
-
-#define R700_MC_VM_FB_LOCATION                                 0x2024
-#define R700_MC_VM_AGP_TOP                                     0x2028
-#define R700_MC_VM_AGP_BOT                                     0x202c
-#define R700_MC_VM_AGP_BASE                                    0x2030
-#define R700_MC_VM_SYSTEM_APERTURE_LOW_ADDR                    0x2034
-#define R700_MC_VM_SYSTEM_APERTURE_HIGH_ADDR                   0x2038
-#define R700_MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR                0x203c
-
-#define R600_MCD_RD_A_CNTL                                     0x219c
-#define R600_MCD_RD_B_CNTL                                     0x21a0
-
-#define R600_MCD_WR_A_CNTL                                     0x21a4
-#define R600_MCD_WR_B_CNTL                                     0x21a8
-
-#define R600_MCD_RD_SYS_CNTL                                   0x2200
-#define R600_MCD_WR_SYS_CNTL                                   0x2214
-
-#define R600_MCD_RD_GFX_CNTL                                   0x21fc
-#define R600_MCD_RD_HDP_CNTL                                   0x2204
-#define R600_MCD_RD_PDMA_CNTL                                  0x2208
-#define R600_MCD_RD_SEM_CNTL                                   0x220c
-#define R600_MCD_WR_GFX_CNTL                                   0x2210
-#define R600_MCD_WR_HDP_CNTL                                   0x2218
-#define R600_MCD_WR_PDMA_CNTL                                  0x221c
-#define R600_MCD_WR_SEM_CNTL                                   0x2220
-
-#       define R600_MCD_L1_TLB                                 (1 << 0)
-#       define R600_MCD_L1_FRAG_PROC                           (1 << 1)
-#       define R600_MCD_L1_STRICT_ORDERING                     (1 << 2)
-
-#       define R600_MCD_SYSTEM_ACCESS_MODE_MASK                (3 << 6)
-#       define R600_MCD_SYSTEM_ACCESS_MODE_PA_ONLY             (0 << 6)
-#       define R600_MCD_SYSTEM_ACCESS_MODE_USE_SYS_MAP         (1 << 6)
-#       define R600_MCD_SYSTEM_ACCESS_MODE_IN_SYS              (2 << 6)
-#       define R600_MCD_SYSTEM_ACCESS_MODE_NOT_IN_SYS          (3 << 6)
-
-#       define R600_MCD_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU    (0 << 8)
-#       define R600_MCD_SYSTEM_APERTURE_UNMAPPED_ACCESS_DEFAULT_PAGE (1 << 8)
-
-#       define R600_MCD_SEMAPHORE_MODE                         (1 << 10)
-#       define R600_MCD_WAIT_L2_QUERY                          (1 << 11)
-#       define R600_MCD_EFFECTIVE_L1_TLB_SIZE(x)               ((x) << 12)
-#       define R600_MCD_EFFECTIVE_L1_QUEUE_SIZE(x)             ((x) << 15)
-
-#define R700_MC_VM_MD_L1_TLB0_CNTL                             0x2654
-#define R700_MC_VM_MD_L1_TLB1_CNTL                             0x2658
-#define R700_MC_VM_MD_L1_TLB2_CNTL                             0x265c
-
-#define R700_MC_VM_MB_L1_TLB0_CNTL                             0x2234
-#define R700_MC_VM_MB_L1_TLB1_CNTL                             0x2238
-#define R700_MC_VM_MB_L1_TLB2_CNTL                             0x223c
-#define R700_MC_VM_MB_L1_TLB3_CNTL                             0x2240
-
-#       define R700_ENABLE_L1_TLB                              (1 << 0)
-#       define R700_ENABLE_L1_FRAGMENT_PROCESSING              (1 << 1)
-#       define R700_SYSTEM_ACCESS_MODE_IN_SYS                  (2 << 3)
-#       define R700_SYSTEM_APERTURE_UNMAPPED_ACCESS_PASS_THRU  (0 << 5)
-#       define R700_EFFECTIVE_L1_TLB_SIZE(x)                   ((x) << 15)
-#       define R700_EFFECTIVE_L1_QUEUE_SIZE(x)                 ((x) << 18)
-
-#define R700_MC_ARB_RAMCFG                                     0x2760
-#       define R700_NOOFBANK_SHIFT                             0
-#       define R700_NOOFBANK_MASK                              0x3
-#       define R700_NOOFRANK_SHIFT                             2
-#       define R700_NOOFRANK_MASK                              0x1
-#       define R700_NOOFROWS_SHIFT                             3
-#       define R700_NOOFROWS_MASK                              0x7
-#       define R700_NOOFCOLS_SHIFT                             6
-#       define R700_NOOFCOLS_MASK                              0x3
-#       define R700_CHANSIZE_SHIFT                             8
-#       define R700_CHANSIZE_MASK                              0x1
-#       define R700_BURSTLENGTH_SHIFT                          9
-#       define R700_BURSTLENGTH_MASK                           0x1
-#define R600_RAMCFG                                            0x2408
-#       define R600_NOOFBANK_SHIFT                             0
-#       define R600_NOOFBANK_MASK                              0x1
-#       define R600_NOOFRANK_SHIFT                             1
-#       define R600_NOOFRANK_MASK                              0x1
-#       define R600_NOOFROWS_SHIFT                             2
-#       define R600_NOOFROWS_MASK                              0x7
-#       define R600_NOOFCOLS_SHIFT                             5
-#       define R600_NOOFCOLS_MASK                              0x3
-#       define R600_CHANSIZE_SHIFT                             7
-#       define R600_CHANSIZE_MASK                              0x1
-#       define R600_BURSTLENGTH_SHIFT                          8
-#       define R600_BURSTLENGTH_MASK                           0x1
-
-#define R600_VM_L2_CNTL                                        0x1400
-#       define R600_VM_L2_CACHE_EN                             (1 << 0)
-#       define R600_VM_L2_FRAG_PROC                            (1 << 1)
-#       define R600_VM_ENABLE_PTE_CACHE_LRU_W                  (1 << 9)
-#       define R600_VM_L2_CNTL_QUEUE_SIZE(x)                   ((x) << 13)
-#       define R700_VM_L2_CNTL_QUEUE_SIZE(x)                   ((x) << 14)
-
-#define R600_VM_L2_CNTL2                                       0x1404
-#       define R600_VM_L2_CNTL2_INVALIDATE_ALL_L1_TLBS         (1 << 0)
-#       define R600_VM_L2_CNTL2_INVALIDATE_L2_CACHE            (1 << 1)
-#define R600_VM_L2_CNTL3                                       0x1408
-#       define R600_VM_L2_CNTL3_BANK_SELECT_0(x)               ((x) << 0)
-#       define R600_VM_L2_CNTL3_BANK_SELECT_1(x)               ((x) << 5)
-#       define R600_VM_L2_CNTL3_CACHE_UPDATE_MODE(x)           ((x) << 10)
-#       define R700_VM_L2_CNTL3_BANK_SELECT(x)                 ((x) << 0)
-#       define R700_VM_L2_CNTL3_CACHE_UPDATE_MODE(x)           ((x) << 6)
-
-#define R600_VM_L2_STATUS                                      0x140c
-
-#define R600_VM_CONTEXT0_CNTL                                  0x1410
-#       define R600_VM_ENABLE_CONTEXT                          (1 << 0)
-#       define R600_VM_PAGE_TABLE_DEPTH_FLAT                   (0 << 1)
-
-#define R600_VM_CONTEXT0_CNTL2                                 0x1430
-#define R600_VM_CONTEXT0_REQUEST_RESPONSE                      0x1470
-#define R600_VM_CONTEXT0_INVALIDATION_LOW_ADDR                 0x1490
-#define R600_VM_CONTEXT0_INVALIDATION_HIGH_ADDR                0x14b0
-#define R600_VM_CONTEXT0_PAGE_TABLE_BASE_ADDR                  0x1574
-#define R600_VM_CONTEXT0_PAGE_TABLE_START_ADDR                 0x1594
-#define R600_VM_CONTEXT0_PAGE_TABLE_END_ADDR                   0x15b4
-
-#define R700_VM_CONTEXT0_PAGE_TABLE_BASE_ADDR                  0x153c
-#define R700_VM_CONTEXT0_PAGE_TABLE_START_ADDR                 0x155c
-#define R700_VM_CONTEXT0_PAGE_TABLE_END_ADDR                   0x157c
-
-#define R600_HDP_HOST_PATH_CNTL                                0x2c00
-
-#define R600_GRBM_CNTL                                         0x8000
-#       define R600_GRBM_READ_TIMEOUT(x)                       ((x) << 0)
-
-#define R600_GRBM_STATUS                                       0x8010
-#       define R600_CMDFIFO_AVAIL_MASK                         0x1f
-#       define R700_CMDFIFO_AVAIL_MASK                         0xf
-#       define R600_GUI_ACTIVE                                 (1 << 31)
-#define R600_GRBM_STATUS2                                      0x8014
-#define R600_GRBM_SOFT_RESET                                   0x8020
-#       define R600_SOFT_RESET_CP                              (1 << 0)
-#define R600_WAIT_UNTIL		                               0x8040
-
-#define R600_CP_SEM_WAIT_TIMER                                 0x85bc
-#define R600_CP_ME_CNTL                                        0x86d8
-#       define R600_CP_ME_HALT                                 (1 << 28)
-#define R600_CP_QUEUE_THRESHOLDS                               0x8760
-#       define R600_ROQ_IB1_START(x)                           ((x) << 0)
-#       define R600_ROQ_IB2_START(x)                           ((x) << 8)
-#define R600_CP_MEQ_THRESHOLDS                                 0x8764
-#       define R700_STQ_SPLIT(x)                               ((x) << 0)
-#       define R600_MEQ_END(x)                                 ((x) << 16)
-#       define R600_ROQ_END(x)                                 ((x) << 24)
-#define R600_CP_PERFMON_CNTL                                   0x87fc
-#define R600_CP_RB_BASE                                        0xc100
-#define R600_CP_RB_CNTL                                        0xc104
-#       define R600_RB_BUFSZ(x)                                ((x) << 0)
-#       define R600_RB_BLKSZ(x)                                ((x) << 8)
-#	define R600_BUF_SWAP_32BIT		               (2 << 16)
-#       define R600_RB_NO_UPDATE                               (1 << 27)
-#       define R600_RB_RPTR_WR_ENA                             (1 << 31)
-#define R600_CP_RB_RPTR_WR                                     0xc108
-#define R600_CP_RB_RPTR_ADDR                                   0xc10c
-#define R600_CP_RB_RPTR_ADDR_HI                                0xc110
-#define R600_CP_RB_WPTR                                        0xc114
-#define R600_CP_RB_WPTR_ADDR                                   0xc118
-#define R600_CP_RB_WPTR_ADDR_HI                                0xc11c
-#define R600_CP_RB_RPTR                                        0x8700
-#define R600_CP_RB_WPTR_DELAY                                  0x8704
-#define R600_CP_PFP_UCODE_ADDR                                 0xc150
-#define R600_CP_PFP_UCODE_DATA                                 0xc154
-#define R600_CP_ME_RAM_RADDR                                   0xc158
-#define R600_CP_ME_RAM_WADDR                                   0xc15c
-#define R600_CP_ME_RAM_DATA                                    0xc160
-#define R600_CP_DEBUG                                          0xc1fc
-
-#define R600_PA_CL_ENHANCE                                     0x8a14
-#       define R600_CLIP_VTX_REORDER_ENA                       (1 << 0)
-#       define R600_NUM_CLIP_SEQ(x)                            ((x) << 1)
-#define R600_PA_SC_LINE_STIPPLE_STATE                          0x8b10
-#define R600_PA_SC_MULTI_CHIP_CNTL                             0x8b20
-#define R700_PA_SC_FORCE_EOV_MAX_CNTS                          0x8b24
-#       define R700_FORCE_EOV_MAX_CLK_CNT(x)                   ((x) << 0)
-#       define R700_FORCE_EOV_MAX_REZ_CNT(x)                   ((x) << 16)
-#define R600_PA_SC_AA_SAMPLE_LOCS_2S                           0x8b40
-#define R600_PA_SC_AA_SAMPLE_LOCS_4S                           0x8b44
-#define R600_PA_SC_AA_SAMPLE_LOCS_8S_WD0                       0x8b48
-#define R600_PA_SC_AA_SAMPLE_LOCS_8S_WD1                       0x8b4c
-#       define R600_S0_X(x)                                    ((x) << 0)
-#       define R600_S0_Y(x)                                    ((x) << 4)
-#       define R600_S1_X(x)                                    ((x) << 8)
-#       define R600_S1_Y(x)                                    ((x) << 12)
-#       define R600_S2_X(x)                                    ((x) << 16)
-#       define R600_S2_Y(x)                                    ((x) << 20)
-#       define R600_S3_X(x)                                    ((x) << 24)
-#       define R600_S3_Y(x)                                    ((x) << 28)
-#       define R600_S4_X(x)                                    ((x) << 0)
-#       define R600_S4_Y(x)                                    ((x) << 4)
-#       define R600_S5_X(x)                                    ((x) << 8)
-#       define R600_S5_Y(x)                                    ((x) << 12)
-#       define R600_S6_X(x)                                    ((x) << 16)
-#       define R600_S6_Y(x)                                    ((x) << 20)
-#       define R600_S7_X(x)                                    ((x) << 24)
-#       define R600_S7_Y(x)                                    ((x) << 28)
-#define R600_PA_SC_FIFO_SIZE                                   0x8bd0
-#       define R600_SC_PRIM_FIFO_SIZE(x)                       ((x) << 0)
-#       define R600_SC_HIZ_TILE_FIFO_SIZE(x)                   ((x) << 8)
-#       define R600_SC_EARLYZ_TILE_FIFO_SIZE(x)                ((x) << 16)
-#define R700_PA_SC_FIFO_SIZE_R7XX                              0x8bcc
-#       define R700_SC_PRIM_FIFO_SIZE(x)                       ((x) << 0)
-#       define R700_SC_HIZ_TILE_FIFO_SIZE(x)                   ((x) << 12)
-#       define R700_SC_EARLYZ_TILE_FIFO_SIZE(x)                ((x) << 20)
-#define R600_PA_SC_ENHANCE                                     0x8bf0
-#       define R600_FORCE_EOV_MAX_CLK_CNT(x)                   ((x) << 0)
-#       define R600_FORCE_EOV_MAX_TILE_CNT(x)                  ((x) << 12)
-#define R600_PA_SC_CLIPRECT_RULE                               0x2820c
-#define R700_PA_SC_EDGERULE                                    0x28230
-#define R600_PA_SC_LINE_STIPPLE                                0x28a0c
-#define R600_PA_SC_MODE_CNTL                                   0x28a4c
-#define R600_PA_SC_AA_CONFIG                                   0x28c04
-
-#define R600_SX_EXPORT_BUFFER_SIZES                            0x900c
-#       define R600_COLOR_BUFFER_SIZE(x)                       ((x) << 0)
-#       define R600_POSITION_BUFFER_SIZE(x)                    ((x) << 8)
-#       define R600_SMX_BUFFER_SIZE(x)                         ((x) << 16)
-#define R600_SX_DEBUG_1                                        0x9054
-#       define R600_SMX_EVENT_RELEASE                          (1 << 0)
-#       define R600_ENABLE_NEW_SMX_ADDRESS                     (1 << 16)
-#define R700_SX_DEBUG_1                                        0x9058
-#       define R700_ENABLE_NEW_SMX_ADDRESS                     (1 << 16)
-#define R600_SX_MISC                                           0x28350
-
-#define R600_DB_DEBUG                                          0x9830
-#       define R600_PREZ_MUST_WAIT_FOR_POSTZ_DONE              (1 << 31)
-#define R600_DB_WATERMARKS                                     0x9838
-#       define R600_DEPTH_FREE(x)                              ((x) << 0)
-#       define R600_DEPTH_FLUSH(x)                             ((x) << 5)
-#       define R600_DEPTH_PENDING_FREE(x)                      ((x) << 15)
-#       define R600_DEPTH_CACHELINE_FREE(x)                    ((x) << 20)
-#define R700_DB_DEBUG3                                         0x98b0
-#       define R700_DB_CLK_OFF_DELAY(x)                        ((x) << 11)
-#define RV700_DB_DEBUG4                                        0x9b8c
-#       define RV700_DISABLE_TILE_COVERED_FOR_PS_ITER          (1 << 6)
-
-#define R600_VGT_CACHE_INVALIDATION                            0x88c4
-#       define R600_CACHE_INVALIDATION(x)                      ((x) << 0)
-#       define R600_VC_ONLY                                    0
-#       define R600_TC_ONLY                                    1
-#       define R600_VC_AND_TC                                  2
-#       define R700_AUTO_INVLD_EN(x)                           ((x) << 6)
-#       define R700_NO_AUTO                                    0
-#       define R700_ES_AUTO                                    1
-#       define R700_GS_AUTO                                    2
-#       define R700_ES_AND_GS_AUTO                             3
-#define R600_VGT_GS_PER_ES                                     0x88c8
-#define R600_VGT_ES_PER_GS                                     0x88cc
-#define R600_VGT_GS_PER_VS                                     0x88e8
-#define R600_VGT_GS_VERTEX_REUSE                               0x88d4
-#define R600_VGT_NUM_INSTANCES                                 0x8974
-#define R600_VGT_STRMOUT_EN                                    0x28ab0
-#define R600_VGT_EVENT_INITIATOR                               0x28a90
-#       define R600_CACHE_FLUSH_AND_INV_EVENT                  (0x16 << 0)
-#define R600_VGT_VERTEX_REUSE_BLOCK_CNTL                       0x28c58
-#       define R600_VTX_REUSE_DEPTH_MASK                       0xff
-#define R600_VGT_OUT_DEALLOC_CNTL                              0x28c5c
-#       define R600_DEALLOC_DIST_MASK                          0x7f
-
-#define R600_CB_COLOR0_BASE                                    0x28040
-#define R600_CB_COLOR1_BASE                                    0x28044
-#define R600_CB_COLOR2_BASE                                    0x28048
-#define R600_CB_COLOR3_BASE                                    0x2804c
-#define R600_CB_COLOR4_BASE                                    0x28050
-#define R600_CB_COLOR5_BASE                                    0x28054
-#define R600_CB_COLOR6_BASE                                    0x28058
-#define R600_CB_COLOR7_BASE                                    0x2805c
-#define R600_CB_COLOR7_FRAG                                    0x280fc
-
-#define R600_CB_COLOR0_SIZE                                    0x28060
-#define R600_CB_COLOR0_VIEW                                    0x28080
-#define R600_CB_COLOR0_INFO                                    0x280a0
-#define R600_CB_COLOR0_TILE                                    0x280c0
-#define R600_CB_COLOR0_FRAG                                    0x280e0
-#define R600_CB_COLOR0_MASK                                    0x28100
-
-#define AVIVO_D1MODE_VLINE_START_END                           0x6538
-#define AVIVO_D2MODE_VLINE_START_END                           0x6d38
-#define R600_CP_COHER_BASE                                     0x85f8
-#define R600_DB_DEPTH_BASE                                     0x2800c
-#define R600_SQ_PGM_START_FS                                   0x28894
-#define R600_SQ_PGM_START_ES                                   0x28880
-#define R600_SQ_PGM_START_VS                                   0x28858
-#define R600_SQ_PGM_RESOURCES_VS                               0x28868
-#define R600_SQ_PGM_CF_OFFSET_VS                               0x288d0
-#define R600_SQ_PGM_START_GS                                   0x2886c
-#define R600_SQ_PGM_START_PS                                   0x28840
-#define R600_SQ_PGM_RESOURCES_PS                               0x28850
-#define R600_SQ_PGM_EXPORTS_PS                                 0x28854
-#define R600_SQ_PGM_CF_OFFSET_PS                               0x288cc
-#define R600_VGT_DMA_BASE                                      0x287e8
-#define R600_VGT_DMA_BASE_HI                                   0x287e4
-#define R600_VGT_STRMOUT_BASE_OFFSET_0                         0x28b10
-#define R600_VGT_STRMOUT_BASE_OFFSET_1                         0x28b14
-#define R600_VGT_STRMOUT_BASE_OFFSET_2                         0x28b18
-#define R600_VGT_STRMOUT_BASE_OFFSET_3                         0x28b1c
-#define R600_VGT_STRMOUT_BASE_OFFSET_HI_0                      0x28b44
-#define R600_VGT_STRMOUT_BASE_OFFSET_HI_1                      0x28b48
-#define R600_VGT_STRMOUT_BASE_OFFSET_HI_2                      0x28b4c
-#define R600_VGT_STRMOUT_BASE_OFFSET_HI_3                      0x28b50
-#define R600_VGT_STRMOUT_BUFFER_BASE_0                         0x28ad8
-#define R600_VGT_STRMOUT_BUFFER_BASE_1                         0x28ae8
-#define R600_VGT_STRMOUT_BUFFER_BASE_2                         0x28af8
-#define R600_VGT_STRMOUT_BUFFER_BASE_3                         0x28b08
-#define R600_VGT_STRMOUT_BUFFER_OFFSET_0                       0x28adc
-#define R600_VGT_STRMOUT_BUFFER_OFFSET_1                       0x28aec
-#define R600_VGT_STRMOUT_BUFFER_OFFSET_2                       0x28afc
-#define R600_VGT_STRMOUT_BUFFER_OFFSET_3                       0x28b0c
-
-#define R600_VGT_PRIMITIVE_TYPE                                0x8958
-
-#define R600_PA_SC_SCREEN_SCISSOR_TL                           0x28030
-#define R600_PA_SC_GENERIC_SCISSOR_TL                          0x28240
-#define R600_PA_SC_WINDOW_SCISSOR_TL                           0x28204
-
-#define R600_TC_CNTL                                           0x9608
-#       define R600_TC_L2_SIZE(x)                              ((x) << 5)
-#       define R600_L2_DISABLE_LATE_HIT                        (1 << 9)
-
-#define R600_ARB_POP                                           0x2418
-#       define R600_ENABLE_TC128                               (1 << 30)
-#define R600_ARB_GDEC_RD_CNTL                                  0x246c
-
-#define R600_TA_CNTL_AUX                                       0x9508
-#       define R600_DISABLE_CUBE_WRAP                          (1 << 0)
-#       define R600_DISABLE_CUBE_ANISO                         (1 << 1)
-#       define R700_GETLOD_SELECT(x)                           ((x) << 2)
-#       define R600_SYNC_GRADIENT                              (1 << 24)
-#       define R600_SYNC_WALKER                                (1 << 25)
-#       define R600_SYNC_ALIGNER                               (1 << 26)
-#       define R600_BILINEAR_PRECISION_6_BIT                   (0 << 31)
-#       define R600_BILINEAR_PRECISION_8_BIT                   (1 << 31)
-
-#define R700_TCP_CNTL                                          0x9610
-
-#define R600_SMX_DC_CTL0                                       0xa020
-#       define R700_USE_HASH_FUNCTION                          (1 << 0)
-#       define R700_CACHE_DEPTH(x)                             ((x) << 1)
-#       define R700_FLUSH_ALL_ON_EVENT                         (1 << 10)
-#       define R700_STALL_ON_EVENT                             (1 << 11)
-#define R700_SMX_EVENT_CTL                                     0xa02c
-#       define R700_ES_FLUSH_CTL(x)                            ((x) << 0)
-#       define R700_GS_FLUSH_CTL(x)                            ((x) << 3)
-#       define R700_ACK_FLUSH_CTL(x)                           ((x) << 6)
-#       define R700_SYNC_FLUSH_CTL                             (1 << 8)
-
-#define R600_SQ_CONFIG                                         0x8c00
-#       define R600_VC_ENABLE                                  (1 << 0)
-#       define R600_EXPORT_SRC_C                               (1 << 1)
-#       define R600_DX9_CONSTS                                 (1 << 2)
-#       define R600_ALU_INST_PREFER_VECTOR                     (1 << 3)
-#       define R600_DX10_CLAMP                                 (1 << 4)
-#       define R600_CLAUSE_SEQ_PRIO(x)                         ((x) << 8)
-#       define R600_PS_PRIO(x)                                 ((x) << 24)
-#       define R600_VS_PRIO(x)                                 ((x) << 26)
-#       define R600_GS_PRIO(x)                                 ((x) << 28)
-#       define R600_ES_PRIO(x)                                 ((x) << 30)
-#define R600_SQ_GPR_RESOURCE_MGMT_1                            0x8c04
-#       define R600_NUM_PS_GPRS(x)                             ((x) << 0)
-#       define R600_NUM_VS_GPRS(x)                             ((x) << 16)
-#       define R700_DYN_GPR_ENABLE                             (1 << 27)
-#       define R600_NUM_CLAUSE_TEMP_GPRS(x)                    ((x) << 28)
-#define R600_SQ_GPR_RESOURCE_MGMT_2                            0x8c08
-#       define R600_NUM_GS_GPRS(x)                             ((x) << 0)
-#       define R600_NUM_ES_GPRS(x)                             ((x) << 16)
-#define R600_SQ_THREAD_RESOURCE_MGMT                           0x8c0c
-#       define R600_NUM_PS_THREADS(x)                          ((x) << 0)
-#       define R600_NUM_VS_THREADS(x)                          ((x) << 8)
-#       define R600_NUM_GS_THREADS(x)                          ((x) << 16)
-#       define R600_NUM_ES_THREADS(x)                          ((x) << 24)
-#define R600_SQ_STACK_RESOURCE_MGMT_1                          0x8c10
-#       define R600_NUM_PS_STACK_ENTRIES(x)                    ((x) << 0)
-#       define R600_NUM_VS_STACK_ENTRIES(x)                    ((x) << 16)
-#define R600_SQ_STACK_RESOURCE_MGMT_2                          0x8c14
-#       define R600_NUM_GS_STACK_ENTRIES(x)                    ((x) << 0)
-#       define R600_NUM_ES_STACK_ENTRIES(x)                    ((x) << 16)
-#define R600_SQ_MS_FIFO_SIZES                                  0x8cf0
-#       define R600_CACHE_FIFO_SIZE(x)                         ((x) << 0)
-#       define R600_FETCH_FIFO_HIWATER(x)                      ((x) << 8)
-#       define R600_DONE_FIFO_HIWATER(x)                       ((x) << 16)
-#       define R600_ALU_UPDATE_FIFO_HIWATER(x)                 ((x) << 24)
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_0                         0x8db0
-#       define R700_SIMDA_RING0(x)                             ((x) << 0)
-#       define R700_SIMDA_RING1(x)                             ((x) << 8)
-#       define R700_SIMDB_RING0(x)                             ((x) << 16)
-#       define R700_SIMDB_RING1(x)                             ((x) << 24)
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_1                         0x8db4
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_2                         0x8db8
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_3                         0x8dbc
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_4                         0x8dc0
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_5                         0x8dc4
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_6                         0x8dc8
-#define R700_SQ_DYN_GPR_SIZE_SIMD_AB_7                         0x8dcc
-
-#define R600_SPI_PS_IN_CONTROL_0                               0x286cc
-#       define R600_NUM_INTERP(x)                              ((x) << 0)
-#       define R600_POSITION_ENA                               (1 << 8)
-#       define R600_POSITION_CENTROID                          (1 << 9)
-#       define R600_POSITION_ADDR(x)                           ((x) << 10)
-#       define R600_PARAM_GEN(x)                               ((x) << 15)
-#       define R600_PARAM_GEN_ADDR(x)                          ((x) << 19)
-#       define R600_BARYC_SAMPLE_CNTL(x)                       ((x) << 26)
-#       define R600_PERSP_GRADIENT_ENA                         (1 << 28)
-#       define R600_LINEAR_GRADIENT_ENA                        (1 << 29)
-#       define R600_POSITION_SAMPLE                            (1 << 30)
-#       define R600_BARYC_AT_SAMPLE_ENA                        (1 << 31)
-#define R600_SPI_PS_IN_CONTROL_1                               0x286d0
-#       define R600_GEN_INDEX_PIX                              (1 << 0)
-#       define R600_GEN_INDEX_PIX_ADDR(x)                      ((x) << 1)
-#       define R600_FRONT_FACE_ENA                             (1 << 8)
-#       define R600_FRONT_FACE_CHAN(x)                         ((x) << 9)
-#       define R600_FRONT_FACE_ALL_BITS                        (1 << 11)
-#       define R600_FRONT_FACE_ADDR(x)                         ((x) << 12)
-#       define R600_FOG_ADDR(x)                                ((x) << 17)
-#       define R600_FIXED_PT_POSITION_ENA                      (1 << 24)
-#       define R600_FIXED_PT_POSITION_ADDR(x)                  ((x) << 25)
-#       define R700_POSITION_ULC                               (1 << 30)
-#define R600_SPI_INPUT_Z                                       0x286d8
-
-#define R600_SPI_CONFIG_CNTL                                   0x9100
-#       define R600_GPR_WRITE_PRIORITY(x)                      ((x) << 0)
-#       define R600_DISABLE_INTERP_1                           (1 << 5)
-#define R600_SPI_CONFIG_CNTL_1                                 0x913c
-#       define R600_VTX_DONE_DELAY(x)                          ((x) << 0)
-#       define R600_INTERP_ONE_PRIM_PER_ROW                    (1 << 4)
-
-#define R600_GB_TILING_CONFIG                                  0x98f0
-#       define R600_PIPE_TILING(x)                             ((x) << 1)
-#       define R600_BANK_TILING(x)                             ((x) << 4)
-#       define R600_GROUP_SIZE(x)                              ((x) << 6)
-#       define R600_ROW_TILING(x)                              ((x) << 8)
-#       define R600_BANK_SWAPS(x)                              ((x) << 11)
-#       define R600_SAMPLE_SPLIT(x)                            ((x) << 14)
-#       define R600_BACKEND_MAP(x)                             ((x) << 16)
-#define R600_DCP_TILING_CONFIG                                 0x6ca0
-#define R600_HDP_TILING_CONFIG                                 0x2f3c
-
-#define R600_CC_RB_BACKEND_DISABLE                             0x98f4
-#define R700_CC_SYS_RB_BACKEND_DISABLE                         0x3f88
-#       define R600_BACKEND_DISABLE(x)                         ((x) << 16)
-
-#define R600_CC_GC_SHADER_PIPE_CONFIG                          0x8950
-#define R600_GC_USER_SHADER_PIPE_CONFIG                        0x8954
-#       define R600_INACTIVE_QD_PIPES(x)                       ((x) << 8)
-#       define R600_INACTIVE_QD_PIPES_MASK                     (0xff << 8)
-#       define R600_INACTIVE_SIMDS(x)                          ((x) << 16)
-#       define R600_INACTIVE_SIMDS_MASK                        (0xff << 16)
-
-#define R700_CGTS_SYS_TCC_DISABLE                              0x3f90
-#define R700_CGTS_USER_SYS_TCC_DISABLE                         0x3f94
-#define R700_CGTS_TCC_DISABLE                                  0x9148
-#define R700_CGTS_USER_TCC_DISABLE                             0x914c
-
-/* Constants */
-#define RADEON_MAX_USEC_TIMEOUT		100000	/* 100 ms */
-
-#define RADEON_LAST_FRAME_REG		RADEON_SCRATCH_REG0
-#define RADEON_LAST_DISPATCH_REG	RADEON_SCRATCH_REG1
-#define RADEON_LAST_CLEAR_REG		RADEON_SCRATCH_REG2
-#define RADEON_LAST_SWI_REG		RADEON_SCRATCH_REG3
-#define RADEON_LAST_DISPATCH		1
-
-#define R600_LAST_FRAME_REG		R600_SCRATCH_REG0
-#define R600_LAST_DISPATCH_REG	        R600_SCRATCH_REG1
-#define R600_LAST_CLEAR_REG		R600_SCRATCH_REG2
-#define R600_LAST_SWI_REG		R600_SCRATCH_REG3
-
-#define RADEON_MAX_VB_AGE		0x7fffffff
-#define RADEON_MAX_VB_VERTS		(0xffff)
-
-#define RADEON_RING_HIGH_MARK		128
-
-#define RADEON_PCIGART_TABLE_SIZE      (32*1024)
-
-#define RADEON_READ(reg)	DRM_READ32(  dev_priv->mmio, (reg) )
-#define RADEON_WRITE(reg, val)                                          \
-do {									\
-	if (reg < 0x10000) {				                \
-		DRM_WRITE32(dev_priv->mmio, (reg), (val));		\
-	} else {                                                        \
-		DRM_WRITE32(dev_priv->mmio, RADEON_MM_INDEX, (reg));	\
-		DRM_WRITE32(dev_priv->mmio, RADEON_MM_DATA, (val));	\
-	}                                                               \
-} while (0)
-#define RADEON_READ8(reg)	DRM_READ8(  dev_priv->mmio, (reg) )
-#define RADEON_WRITE8(reg,val)	DRM_WRITE8( dev_priv->mmio, (reg), (val) )
-
-#define RADEON_WRITE_PLL(addr, val)					\
-do {									\
-	RADEON_WRITE8(RADEON_CLOCK_CNTL_INDEX,				\
-		       ((addr) & 0x1f) | RADEON_PLL_WR_EN );		\
-	RADEON_WRITE(RADEON_CLOCK_CNTL_DATA, (val));			\
-} while (0)
-
-#define RADEON_WRITE_PCIE(addr, val)					\
-do {									\
-	RADEON_WRITE8(RADEON_PCIE_INDEX,				\
-			((addr) & 0xff));				\
-	RADEON_WRITE(RADEON_PCIE_DATA, (val));			\
-} while (0)
-
-#define R500_WRITE_MCIND(addr, val)					\
-do {								\
-	RADEON_WRITE(R520_MC_IND_INDEX, 0xff0000 | ((addr) & 0xff));	\
-	RADEON_WRITE(R520_MC_IND_DATA, (val));			\
-	RADEON_WRITE(R520_MC_IND_INDEX, 0);	\
-} while (0)
-
-#define RS480_WRITE_MCIND(addr, val)				\
-do {									\
-	RADEON_WRITE(RS480_NB_MC_INDEX,				\
-			((addr) & 0xff) | RS480_NB_MC_IND_WR_EN);	\
-	RADEON_WRITE(RS480_NB_MC_DATA, (val));			\
-	RADEON_WRITE(RS480_NB_MC_INDEX, 0xff);			\
-} while (0)
-
-#define RS690_WRITE_MCIND(addr, val)					\
-do {								\
-	RADEON_WRITE(RS690_MC_INDEX, RS690_MC_INDEX_WR_EN | ((addr) & RS690_MC_INDEX_MASK));	\
-	RADEON_WRITE(RS690_MC_DATA, val);			\
-	RADEON_WRITE(RS690_MC_INDEX, RS690_MC_INDEX_WR_ACK);	\
-} while (0)
-
-#define RS600_WRITE_MCIND(addr, val)				\
-do {							        \
-	RADEON_WRITE(RS600_MC_INDEX, RS600_MC_IND_WR_EN | RS600_MC_IND_CITF_ARB0 | ((addr) & RS600_MC_ADDR_MASK)); \
-	RADEON_WRITE(RS600_MC_DATA, val);                       \
-} while (0)
-
-#define IGP_WRITE_MCIND(addr, val)				\
-do {									\
-	if (((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS690) ||   \
-	    ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS740))      \
-		RS690_WRITE_MCIND(addr, val);				\
-	else if ((dev_priv->flags & RADEON_FAMILY_MASK) == CHIP_RS600)  \
-		RS600_WRITE_MCIND(addr, val);				\
-	else								\
-		RS480_WRITE_MCIND(addr, val);				\
-} while (0)
-
-#define CP_PACKET0( reg, n )						\
-	(RADEON_CP_PACKET0 | ((n) << 16) | ((reg) >> 2))
-#define CP_PACKET0_TABLE( reg, n )					\
-	(RADEON_CP_PACKET0 | RADEON_ONE_REG_WR | ((n) << 16) | ((reg) >> 2))
-#define CP_PACKET1( reg0, reg1 )					\
-	(RADEON_CP_PACKET1 | (((reg1) >> 2) << 15) | ((reg0) >> 2))
-#define CP_PACKET2()							\
-	(RADEON_CP_PACKET2)
-#define CP_PACKET3( pkt, n )						\
-	(RADEON_CP_PACKET3 | (pkt) | ((n) << 16))
-
-/* ================================================================
- * Engine control helper macros
- */
-
-#define RADEON_WAIT_UNTIL_2D_IDLE() do {				\
-	OUT_RING( CP_PACKET0( RADEON_WAIT_UNTIL, 0 ) );			\
-	OUT_RING( (RADEON_WAIT_2D_IDLECLEAN |				\
-		   RADEON_WAIT_HOST_IDLECLEAN) );			\
-} while (0)
-
-#define RADEON_WAIT_UNTIL_3D_IDLE() do {				\
-	OUT_RING( CP_PACKET0( RADEON_WAIT_UNTIL, 0 ) );			\
-	OUT_RING( (RADEON_WAIT_3D_IDLECLEAN |				\
-		   RADEON_WAIT_HOST_IDLECLEAN) );			\
-} while (0)
-
-#define RADEON_WAIT_UNTIL_IDLE() do {					\
-	OUT_RING( CP_PACKET0( RADEON_WAIT_UNTIL, 0 ) );			\
-	OUT_RING( (RADEON_WAIT_2D_IDLECLEAN |				\
-		   RADEON_WAIT_3D_IDLECLEAN |				\
-		   RADEON_WAIT_HOST_IDLECLEAN) );			\
-} while (0)
-
-#define RADEON_WAIT_UNTIL_PAGE_FLIPPED() do {				\
-	OUT_RING( CP_PACKET0( RADEON_WAIT_UNTIL, 0 ) );			\
-	OUT_RING( RADEON_WAIT_CRTC_PFLIP );				\
-} while (0)
-
-#define RADEON_FLUSH_CACHE() do {					\
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV280) {	\
-		OUT_RING(CP_PACKET0(RADEON_RB3D_DSTCACHE_CTLSTAT, 0));	\
-		OUT_RING(RADEON_RB3D_DC_FLUSH);				\
-	} else {                                                        \
-		OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));	\
-		OUT_RING(R300_RB3D_DC_FLUSH);				\
-	}                                                               \
-} while (0)
-
-#define RADEON_PURGE_CACHE() do {					\
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV280) {	\
-		OUT_RING(CP_PACKET0(RADEON_RB3D_DSTCACHE_CTLSTAT, 0));	\
-		OUT_RING(RADEON_RB3D_DC_FLUSH | RADEON_RB3D_DC_FREE);	\
-	} else {                                                        \
-		OUT_RING(CP_PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));	\
-		OUT_RING(R300_RB3D_DC_FLUSH | R300_RB3D_DC_FREE);	\
-	}                                                               \
-} while (0)
-
-#define RADEON_FLUSH_ZCACHE() do {					\
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV280) {	\
-		OUT_RING(CP_PACKET0(RADEON_RB3D_ZCACHE_CTLSTAT, 0));	\
-		OUT_RING(RADEON_RB3D_ZC_FLUSH);				\
-	} else {                                                        \
-		OUT_RING(CP_PACKET0(R300_ZB_ZCACHE_CTLSTAT, 0));	\
-		OUT_RING(R300_ZC_FLUSH);				\
-	}                                                               \
-} while (0)
-
-#define RADEON_PURGE_ZCACHE() do {					\
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) <= CHIP_RV280) {	\
-		OUT_RING(CP_PACKET0(RADEON_RB3D_ZCACHE_CTLSTAT, 0));	\
-		OUT_RING(RADEON_RB3D_ZC_FLUSH | RADEON_RB3D_ZC_FREE);			\
-	} else {                                                        \
-		OUT_RING(CP_PACKET0(R300_ZB_ZCACHE_CTLSTAT, 0));	\
-		OUT_RING(R300_ZC_FLUSH | R300_ZC_FREE);				\
-	}                                                               \
-} while (0)
-
-/* ================================================================
- * Misc helper macros
- */
-
-/* Perfbox functionality only.
- */
-#define RING_SPACE_TEST_WITH_RETURN( dev_priv )				\
-do {									\
-	if (!(dev_priv->stats.boxes & RADEON_BOX_DMA_IDLE)) {		\
-		u32 head = GET_RING_HEAD( dev_priv );			\
-		if (head == dev_priv->ring.tail)			\
-			dev_priv->stats.boxes |= RADEON_BOX_DMA_IDLE;	\
-	}								\
-} while (0)
-
-#define VB_AGE_TEST_WITH_RETURN( dev_priv )				\
-do {								\
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;	\
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;	\
-	if ( sarea_priv->last_dispatch >= RADEON_MAX_VB_AGE ) {		\
-		int __ret;						\
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) \
-			__ret = r600_do_cp_idle(dev_priv);		\
-		else							\
-			__ret = radeon_do_cp_idle(dev_priv);		\
-		if ( __ret ) return __ret;				\
-		sarea_priv->last_dispatch = 0;				\
-		radeon_freelist_reset( dev );				\
-	}								\
-} while (0)
-
-#define RADEON_DISPATCH_AGE( age ) do {					\
-	OUT_RING( CP_PACKET0( RADEON_LAST_DISPATCH_REG, 0 ) );		\
-	OUT_RING( age );						\
-} while (0)
-
-#define RADEON_FRAME_AGE( age ) do {					\
-	OUT_RING( CP_PACKET0( RADEON_LAST_FRAME_REG, 0 ) );		\
-	OUT_RING( age );						\
-} while (0)
-
-#define RADEON_CLEAR_AGE( age ) do {					\
-	OUT_RING( CP_PACKET0( RADEON_LAST_CLEAR_REG, 0 ) );		\
-	OUT_RING( age );						\
-} while (0)
-
-#define R600_DISPATCH_AGE(age) do {					\
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));		\
-	OUT_RING((R600_LAST_DISPATCH_REG - R600_SET_CONFIG_REG_OFFSET) >> 2);  \
-	OUT_RING(age);							\
-} while (0)
-
-#define R600_FRAME_AGE(age) do {					\
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));		\
-	OUT_RING((R600_LAST_FRAME_REG - R600_SET_CONFIG_REG_OFFSET) >> 2);  \
-	OUT_RING(age);							\
-} while (0)
-
-#define R600_CLEAR_AGE(age) do {					\
-	OUT_RING(CP_PACKET3(R600_IT_SET_CONFIG_REG, 1));		\
-	OUT_RING((R600_LAST_CLEAR_REG - R600_SET_CONFIG_REG_OFFSET) >> 2);  \
-	OUT_RING(age);							\
-} while (0)
-
-/* ================================================================
- * Ring control
- */
-
-#define RADEON_VERBOSE	0
-
-#define RING_LOCALS	int write, _nr, _align_nr; unsigned int mask; u32 *ring;
-
-#define RADEON_RING_ALIGN 16
-
-#define BEGIN_RING( n ) do {						\
-	if ( RADEON_VERBOSE ) {						\
-		DRM_INFO( "BEGIN_RING( %d )\n", (n));			\
-	}								\
-	_align_nr = RADEON_RING_ALIGN - ((dev_priv->ring.tail + n) & (RADEON_RING_ALIGN-1));	\
-	_align_nr += n;							\
-	if (dev_priv->ring.space <= (_align_nr * sizeof(u32))) {	\
-                COMMIT_RING();						\
-		radeon_wait_ring( dev_priv, _align_nr * sizeof(u32));	\
-	}								\
-	_nr = n; dev_priv->ring.space -= (n) * sizeof(u32);		\
-	ring = dev_priv->ring.start;					\
-	write = dev_priv->ring.tail;					\
-	mask = dev_priv->ring.tail_mask;				\
-} while (0)
-
-#define ADVANCE_RING() do {						\
-	if ( RADEON_VERBOSE ) {						\
-		DRM_INFO( "ADVANCE_RING() wr=0x%06x tail=0x%06x\n",	\
-			  write, dev_priv->ring.tail );			\
-	}								\
-	if (((dev_priv->ring.tail + _nr) & mask) != write) {		\
-		DRM_ERROR(						\
-			"ADVANCE_RING(): mismatch: nr: %x write: %x line: %d\n",	\
-			((dev_priv->ring.tail + _nr) & mask),		\
-			write, __LINE__);				\
-	} else								\
-		dev_priv->ring.tail = write;				\
-} while (0)
-
-extern void radeon_commit_ring(drm_radeon_private_t *dev_priv);
-
-#define COMMIT_RING() do {						\
-		radeon_commit_ring(dev_priv);				\
-	} while(0)
-
-#define OUT_RING( x ) do {						\
-	if ( RADEON_VERBOSE ) {						\
-		DRM_INFO( "   OUT_RING( 0x%08x ) at 0x%x\n",		\
-			   (unsigned int)(x), write );			\
-	}								\
-	ring[write++] = (x);						\
-	write &= mask;							\
-} while (0)
-
-#define OUT_RING_REG( reg, val ) do {					\
-	OUT_RING( CP_PACKET0( reg, 0 ) );				\
-	OUT_RING( val );						\
-} while (0)
-
-#define OUT_RING_TABLE( tab, sz ) do {					\
-	int _size = (sz);					\
-	int *_tab = (int *)(tab);				\
-								\
-	if (write + _size > mask) {				\
-		int _i = (mask+1) - write;			\
-		_size -= _i;					\
-		while (_i > 0 ) {				\
-			*(int *)(ring + write) = *_tab++;	\
-			write++;				\
-			_i--;					\
-		}						\
-		write = 0;					\
-		_tab += _i;					\
-	}							\
-	while (_size > 0) {					\
-		*(ring + write) = *_tab++;			\
-		write++;					\
-		_size--;					\
-	}							\
-	write &= mask;						\
-} while (0)
-
-/**
- * Copy given number of dwords from drm buffer to the ring buffer.
- */
-#define OUT_RING_DRM_BUFFER(buf, sz) do {				\
-	int _size = (sz) * 4;						\
-	struct drm_buffer *_buf = (buf);				\
-	int _part_size;							\
-	while (_size > 0) {						\
-		_part_size = _size;					\
-									\
-		if (write + _part_size/4 > mask)			\
-			_part_size = ((mask + 1) - write)*4;		\
-									\
-		if (drm_buffer_index(_buf) + _part_size > PAGE_SIZE)	\
-			_part_size = PAGE_SIZE - drm_buffer_index(_buf);\
-									\
-									\
-									\
-		memcpy(ring + write, &_buf->data[drm_buffer_page(_buf)]	\
-			[drm_buffer_index(_buf)], _part_size);		\
-									\
-		_size -= _part_size;					\
-		write = (write + _part_size/4) & mask;			\
-		drm_buffer_advance(_buf, _part_size);			\
-	}								\
-} while (0)
-
-
-#endif				/* CONFIG_DRM_RADEON_UMS */
-
 #endif				/* __RADEON_DRV_H__ */
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index 26da2f4d7b4f..d2e628eea53d 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -44,7 +44,6 @@
 struct radeon_fbdev {
 	struct drm_fb_helper helper;
 	struct radeon_framebuffer rfb;
-	struct list_head fbdev_list;
 	struct radeon_device *rdev;
 };
 
@@ -283,7 +282,7 @@ out_unref:
 
 	}
 	if (fb && ret) {
-		drm_gem_object_unreference(gobj);
+		drm_gem_object_unreference_unlocked(gobj);
 		drm_framebuffer_unregister_private(fb);
 		drm_framebuffer_cleanup(fb);
 		kfree(fb);
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index df09ca7c4889..05815c47b246 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -130,7 +130,7 @@ int radeon_fence_emit(struct radeon_device *rdev,
 		      struct radeon_fence **fence,
 		      int ring)
 {
-	u64 seq = ++rdev->fence_drv[ring].sync_seq[ring];
+	u64 seq;
 
 	/* we are protected by the ring emission mutex */
 	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
@@ -138,7 +138,7 @@ int radeon_fence_emit(struct radeon_device *rdev,
 		return -ENOMEM;
 	}
 	(*fence)->rdev = rdev;
-	(*fence)->seq = seq;
+	(*fence)->seq = seq = ++rdev->fence_drv[ring].sync_seq[ring];
 	(*fence)->ring = ring;
 	(*fence)->is_vm_update = false;
 	fence_init(&(*fence)->base, &radeon_fence_ops,
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index 3dcc5733ff69..e26c963f2e93 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -663,6 +663,7 @@ int radeon_gem_va_ioctl(struct drm_device *dev, void *data,
 	bo_va = radeon_vm_bo_find(&fpriv->vm, rbo);
 	if (!bo_va) {
 		args->operation = RADEON_VA_RESULT_ERROR;
+		radeon_bo_unreserve(rbo);
 		drm_gem_object_unreference_unlocked(gobj);
 		return -ENOENT;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_irq.c b/drivers/gpu/drm/radeon/radeon_irq.c
deleted file mode 100644
index 688afb62f7c4..000000000000
--- a/drivers/gpu/drm/radeon/radeon_irq.c
+++ /dev/null
@@ -1,402 +0,0 @@
-/* radeon_irq.c -- IRQ handling for radeon -*- linux-c -*- */
-/*
- * Copyright (C) The Weather Channel, Inc.  2002.  All Rights Reserved.
- *
- * The Weather Channel (TM) funded Tungsten Graphics to develop the
- * initial release of the Radeon 8500 driver under the XFree86 license.
- * This notice must be preserved.
- *
- * 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
- * PRECISION INSIGHT AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Keith Whitwell <keith@tungstengraphics.com>
- *    Michel D�zer <michel@daenzer.net>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-
-void radeon_irq_set_state(struct drm_device *dev, u32 mask, int state)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if (state)
-		dev_priv->irq_enable_reg |= mask;
-	else
-		dev_priv->irq_enable_reg &= ~mask;
-
-	if (dev->irq_enabled)
-		RADEON_WRITE(RADEON_GEN_INT_CNTL, dev_priv->irq_enable_reg);
-}
-
-static void r500_vbl_irq_set_state(struct drm_device *dev, u32 mask, int state)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if (state)
-		dev_priv->r500_disp_irq_reg |= mask;
-	else
-		dev_priv->r500_disp_irq_reg &= ~mask;
-
-	if (dev->irq_enabled)
-		RADEON_WRITE(R500_DxMODE_INT_MASK, dev_priv->r500_disp_irq_reg);
-}
-
-int radeon_enable_vblank(struct drm_device *dev, unsigned int pipe)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600) {
-		switch (pipe) {
-		case 0:
-			r500_vbl_irq_set_state(dev, R500_D1MODE_INT_MASK, 1);
-			break;
-		case 1:
-			r500_vbl_irq_set_state(dev, R500_D2MODE_INT_MASK, 1);
-			break;
-		default:
-			DRM_ERROR("tried to enable vblank on non-existent crtc %u\n",
-				  pipe);
-			return -EINVAL;
-		}
-	} else {
-		switch (pipe) {
-		case 0:
-			radeon_irq_set_state(dev, RADEON_CRTC_VBLANK_MASK, 1);
-			break;
-		case 1:
-			radeon_irq_set_state(dev, RADEON_CRTC2_VBLANK_MASK, 1);
-			break;
-		default:
-			DRM_ERROR("tried to enable vblank on non-existent crtc %u\n",
-				  pipe);
-			return -EINVAL;
-		}
-	}
-
-	return 0;
-}
-
-void radeon_disable_vblank(struct drm_device *dev, unsigned int pipe)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600) {
-		switch (pipe) {
-		case 0:
-			r500_vbl_irq_set_state(dev, R500_D1MODE_INT_MASK, 0);
-			break;
-		case 1:
-			r500_vbl_irq_set_state(dev, R500_D2MODE_INT_MASK, 0);
-			break;
-		default:
-			DRM_ERROR("tried to enable vblank on non-existent crtc %u\n",
-				  pipe);
-			break;
-		}
-	} else {
-		switch (pipe) {
-		case 0:
-			radeon_irq_set_state(dev, RADEON_CRTC_VBLANK_MASK, 0);
-			break;
-		case 1:
-			radeon_irq_set_state(dev, RADEON_CRTC2_VBLANK_MASK, 0);
-			break;
-		default:
-			DRM_ERROR("tried to enable vblank on non-existent crtc %u\n",
-				  pipe);
-			break;
-		}
-	}
-}
-
-static u32 radeon_acknowledge_irqs(drm_radeon_private_t *dev_priv, u32 *r500_disp_int)
-{
-	u32 irqs = RADEON_READ(RADEON_GEN_INT_STATUS);
-	u32 irq_mask = RADEON_SW_INT_TEST;
-
-	*r500_disp_int = 0;
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600) {
-		/* vbl interrupts in a different place */
-
-		if (irqs & R500_DISPLAY_INT_STATUS) {
-			/* if a display interrupt */
-			u32 disp_irq;
-
-			disp_irq = RADEON_READ(R500_DISP_INTERRUPT_STATUS);
-
-			*r500_disp_int = disp_irq;
-			if (disp_irq & R500_D1_VBLANK_INTERRUPT)
-				RADEON_WRITE(R500_D1MODE_VBLANK_STATUS, R500_VBLANK_ACK);
-			if (disp_irq & R500_D2_VBLANK_INTERRUPT)
-				RADEON_WRITE(R500_D2MODE_VBLANK_STATUS, R500_VBLANK_ACK);
-		}
-		irq_mask |= R500_DISPLAY_INT_STATUS;
-	} else
-		irq_mask |= RADEON_CRTC_VBLANK_STAT | RADEON_CRTC2_VBLANK_STAT;
-
-	irqs &=	irq_mask;
-
-	if (irqs)
-		RADEON_WRITE(RADEON_GEN_INT_STATUS, irqs);
-
-	return irqs;
-}
-
-/* Interrupts - Used for device synchronization and flushing in the
- * following circumstances:
- *
- * - Exclusive FB access with hw idle:
- *    - Wait for GUI Idle (?) interrupt, then do normal flush.
- *
- * - Frame throttling, NV_fence:
- *    - Drop marker irq's into command stream ahead of time.
- *    - Wait on irq's with lock *not held*
- *    - Check each for termination condition
- *
- * - Internally in cp_getbuffer, etc:
- *    - as above, but wait with lock held???
- *
- * NOTE: These functions are misleadingly named -- the irq's aren't
- * tied to dma at all, this is just a hangover from dri prehistory.
- */
-
-irqreturn_t radeon_driver_irq_handler(int irq, void *arg)
-{
-	struct drm_device *dev = (struct drm_device *) arg;
-	drm_radeon_private_t *dev_priv =
-	    (drm_radeon_private_t *) dev->dev_private;
-	u32 stat;
-	u32 r500_disp_int;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return IRQ_NONE;
-
-	/* Only consider the bits we're interested in - others could be used
-	 * outside the DRM
-	 */
-	stat = radeon_acknowledge_irqs(dev_priv, &r500_disp_int);
-	if (!stat)
-		return IRQ_NONE;
-
-	stat &= dev_priv->irq_enable_reg;
-
-	/* SW interrupt */
-	if (stat & RADEON_SW_INT_TEST)
-		wake_up(&dev_priv->swi_queue);
-
-	/* VBLANK interrupt */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600) {
-		if (r500_disp_int & R500_D1_VBLANK_INTERRUPT)
-			drm_handle_vblank(dev, 0);
-		if (r500_disp_int & R500_D2_VBLANK_INTERRUPT)
-			drm_handle_vblank(dev, 1);
-	} else {
-		if (stat & RADEON_CRTC_VBLANK_STAT)
-			drm_handle_vblank(dev, 0);
-		if (stat & RADEON_CRTC2_VBLANK_STAT)
-			drm_handle_vblank(dev, 1);
-	}
-	return IRQ_HANDLED;
-}
-
-static int radeon_emit_irq(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	unsigned int ret;
-	RING_LOCALS;
-
-	atomic_inc(&dev_priv->swi_emitted);
-	ret = atomic_read(&dev_priv->swi_emitted);
-
-	BEGIN_RING(4);
-	OUT_RING_REG(RADEON_LAST_SWI_REG, ret);
-	OUT_RING_REG(RADEON_GEN_INT_STATUS, RADEON_SW_INT_FIRE);
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	return ret;
-}
-
-static int radeon_wait_irq(struct drm_device * dev, int swi_nr)
-{
-	drm_radeon_private_t *dev_priv =
-	    (drm_radeon_private_t *) dev->dev_private;
-	int ret = 0;
-
-	if (RADEON_READ(RADEON_LAST_SWI_REG) >= swi_nr)
-		return 0;
-
-	dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-
-	DRM_WAIT_ON(ret, dev_priv->swi_queue, 3 * HZ,
-		    RADEON_READ(RADEON_LAST_SWI_REG) >= swi_nr);
-
-	return ret;
-}
-
-u32 radeon_get_vblank_counter(struct drm_device *dev, unsigned int pipe)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	if (pipe > 1) {
-		DRM_ERROR("Invalid crtc %u\n", pipe);
-		return -EINVAL;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600) {
-		if (pipe == 0)
-			return RADEON_READ(R500_D1CRTC_FRAME_COUNT);
-		else
-			return RADEON_READ(R500_D2CRTC_FRAME_COUNT);
-	} else {
-		if (pipe == 0)
-			return RADEON_READ(RADEON_CRTC_CRNT_FRAME);
-		else
-			return RADEON_READ(RADEON_CRTC2_CRNT_FRAME);
-	}
-}
-
-/* Needs the lock as it touches the ring.
- */
-int radeon_irq_emit(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_irq_emit_t *emit = data;
-	int result;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return -EINVAL;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	result = radeon_emit_irq(dev);
-
-	if (copy_to_user(emit->irq_seq, &result, sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-/* Doesn't need the hardware lock.
- */
-int radeon_irq_wait(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_irq_wait_t *irqwait = data;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return -EINVAL;
-
-	return radeon_wait_irq(dev, irqwait->irq_seq);
-}
-
-/* drm_dma.h hooks
-*/
-void radeon_driver_irq_preinstall(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv =
-	    (drm_radeon_private_t *) dev->dev_private;
-	u32 dummy;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return;
-
-	/* Disable *all* interrupts */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600)
-		RADEON_WRITE(R500_DxMODE_INT_MASK, 0);
-	RADEON_WRITE(RADEON_GEN_INT_CNTL, 0);
-
-	/* Clear bits if they're already high */
-	radeon_acknowledge_irqs(dev_priv, &dummy);
-}
-
-int radeon_driver_irq_postinstall(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv =
-	    (drm_radeon_private_t *) dev->dev_private;
-
-	atomic_set(&dev_priv->swi_emitted, 0);
-	init_waitqueue_head(&dev_priv->swi_queue);
-
-	dev->max_vblank_count = 0x001fffff;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return 0;
-
-	radeon_irq_set_state(dev, RADEON_SW_INT_ENABLE, 1);
-
-	return 0;
-}
-
-void radeon_driver_irq_uninstall(struct drm_device * dev)
-{
-	drm_radeon_private_t *dev_priv =
-	    (drm_radeon_private_t *) dev->dev_private;
-	if (!dev_priv)
-		return;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		return;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_RS600)
-		RADEON_WRITE(R500_DxMODE_INT_MASK, 0);
-	/* Disable *all* interrupts */
-	RADEON_WRITE(RADEON_GEN_INT_CNTL, 0);
-}
-
-
-int radeon_vblank_crtc_get(struct drm_device *dev)
-{
-	drm_radeon_private_t *dev_priv = (drm_radeon_private_t *) dev->dev_private;
-
-	return dev_priv->vblank_crtc;
-}
-
-int radeon_vblank_crtc_set(struct drm_device *dev, int64_t value)
-{
-	drm_radeon_private_t *dev_priv = (drm_radeon_private_t *) dev->dev_private;
-	if (value & ~(DRM_RADEON_VBLANK_CRTC1 | DRM_RADEON_VBLANK_CRTC2)) {
-		DRM_ERROR("called with invalid crtc 0x%x\n", (unsigned int)value);
-		return -EINVAL;
-	}
-	dev_priv->vblank_crtc = (unsigned int)value;
-	return 0;
-}
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index d290a8a09036..414953c46a38 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -748,19 +748,19 @@ void radeon_driver_preclose_kms(struct drm_device *dev,
  * radeon_get_vblank_counter_kms - get frame count
  *
  * @dev: drm dev pointer
- * @crtc: crtc to get the frame count from
+ * @pipe: crtc to get the frame count from
  *
  * Gets the frame count on the requested crtc (all asics).
  * Returns frame count on success, -EINVAL on failure.
  */
-u32 radeon_get_vblank_counter_kms(struct drm_device *dev, int crtc)
+u32 radeon_get_vblank_counter_kms(struct drm_device *dev, unsigned int pipe)
 {
 	int vpos, hpos, stat;
 	u32 count;
 	struct radeon_device *rdev = dev->dev_private;
 
-	if (crtc < 0 || crtc >= rdev->num_crtc) {
-		DRM_ERROR("Invalid crtc %d\n", crtc);
+	if (pipe >= rdev->num_crtc) {
+		DRM_ERROR("Invalid crtc %u\n", pipe);
 		return -EINVAL;
 	}
 
@@ -772,29 +772,29 @@ u32 radeon_get_vblank_counter_kms(struct drm_device *dev, int crtc)
 	 * and start of vsync, so vpos >= 0 means to bump the hw frame counter
 	 * result by 1 to give the proper appearance to caller.
 	 */
-	if (rdev->mode_info.crtcs[crtc]) {
+	if (rdev->mode_info.crtcs[pipe]) {
 		/* Repeat readout if needed to provide stable result if
 		 * we cross start of vsync during the queries.
 		 */
 		do {
-			count = radeon_get_vblank_counter(rdev, crtc);
+			count = radeon_get_vblank_counter(rdev, pipe);
 			/* Ask radeon_get_crtc_scanoutpos to return vpos as
 			 * distance to start of vblank, instead of regular
 			 * vertical scanout pos.
 			 */
 			stat = radeon_get_crtc_scanoutpos(
-				dev, crtc, GET_DISTANCE_TO_VBLANKSTART,
+				dev, pipe, GET_DISTANCE_TO_VBLANKSTART,
 				&vpos, &hpos, NULL, NULL,
-				&rdev->mode_info.crtcs[crtc]->base.hwmode);
-		} while (count != radeon_get_vblank_counter(rdev, crtc));
+				&rdev->mode_info.crtcs[pipe]->base.hwmode);
+		} while (count != radeon_get_vblank_counter(rdev, pipe));
 
 		if (((stat & (DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_ACCURATE)) !=
 		    (DRM_SCANOUTPOS_VALID | DRM_SCANOUTPOS_ACCURATE))) {
 			DRM_DEBUG_VBL("Query failed! stat %d\n", stat);
 		}
 		else {
-			DRM_DEBUG_VBL("crtc %d: dist from vblank start %d\n",
-				      crtc, vpos);
+			DRM_DEBUG_VBL("crtc %u: dist from vblank start %d\n",
+				      pipe, vpos);
 
 			/* Bump counter if we are at >= leading edge of vblank,
 			 * but before vsync where vpos would turn negative and
@@ -806,7 +806,7 @@ u32 radeon_get_vblank_counter_kms(struct drm_device *dev, int crtc)
 	}
 	else {
 	    /* Fallback to use value as is. */
-	    count = radeon_get_vblank_counter(rdev, crtc);
+	    count = radeon_get_vblank_counter(rdev, pipe);
 	    DRM_DEBUG_VBL("NULL mode info! Returned count may be wrong.\n");
 	}
 
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index 678b4386540d..32b338ff436b 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -25,6 +25,7 @@
  */
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
+#include <drm/drm_fb_helper.h>
 #include <drm/radeon_drm.h>
 #include <drm/drm_fixed.h>
 #include "radeon.h"
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
index 30de43366eae..88dc973fb209 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_encoders.c
@@ -1772,7 +1772,8 @@ radeon_add_legacy_encoder(struct drm_device *dev, uint32_t encoder_enum, uint32_
 	switch (radeon_encoder->encoder_id) {
 	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
 		encoder->possible_crtcs = 0x1;
-		drm_encoder_init(dev, encoder, &radeon_legacy_lvds_enc_funcs, DRM_MODE_ENCODER_LVDS);
+		drm_encoder_init(dev, encoder, &radeon_legacy_lvds_enc_funcs,
+				 DRM_MODE_ENCODER_LVDS, NULL);
 		drm_encoder_helper_add(encoder, &radeon_legacy_lvds_helper_funcs);
 		if (rdev->is_atom_bios)
 			radeon_encoder->enc_priv = radeon_atombios_get_lvds_info(radeon_encoder);
@@ -1781,12 +1782,14 @@ radeon_add_legacy_encoder(struct drm_device *dev, uint32_t encoder_enum, uint32_
 		radeon_encoder->rmx_type = RMX_FULL;
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
-		drm_encoder_init(dev, encoder, &radeon_legacy_tmds_int_enc_funcs, DRM_MODE_ENCODER_TMDS);
+		drm_encoder_init(dev, encoder, &radeon_legacy_tmds_int_enc_funcs,
+				 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &radeon_legacy_tmds_int_helper_funcs);
 		radeon_encoder->enc_priv = radeon_legacy_get_tmds_info(radeon_encoder);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DAC1:
-		drm_encoder_init(dev, encoder, &radeon_legacy_primary_dac_enc_funcs, DRM_MODE_ENCODER_DAC);
+		drm_encoder_init(dev, encoder, &radeon_legacy_primary_dac_enc_funcs,
+				 DRM_MODE_ENCODER_DAC, NULL);
 		drm_encoder_helper_add(encoder, &radeon_legacy_primary_dac_helper_funcs);
 		if (rdev->is_atom_bios)
 			radeon_encoder->enc_priv = radeon_atombios_get_primary_dac_info(radeon_encoder);
@@ -1794,7 +1797,8 @@ radeon_add_legacy_encoder(struct drm_device *dev, uint32_t encoder_enum, uint32_
 			radeon_encoder->enc_priv = radeon_combios_get_primary_dac_info(radeon_encoder);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DAC2:
-		drm_encoder_init(dev, encoder, &radeon_legacy_tv_dac_enc_funcs, DRM_MODE_ENCODER_TVDAC);
+		drm_encoder_init(dev, encoder, &radeon_legacy_tv_dac_enc_funcs,
+				 DRM_MODE_ENCODER_TVDAC, NULL);
 		drm_encoder_helper_add(encoder, &radeon_legacy_tv_dac_helper_funcs);
 		if (rdev->is_atom_bios)
 			radeon_encoder->enc_priv = radeon_atombios_get_tv_dac_info(radeon_encoder);
@@ -1802,7 +1806,8 @@ radeon_add_legacy_encoder(struct drm_device *dev, uint32_t encoder_enum, uint32_
 			radeon_encoder->enc_priv = radeon_combios_get_tv_dac_info(radeon_encoder);
 		break;
 	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
-		drm_encoder_init(dev, encoder, &radeon_legacy_tmds_ext_enc_funcs, DRM_MODE_ENCODER_TMDS);
+		drm_encoder_init(dev, encoder, &radeon_legacy_tmds_ext_enc_funcs,
+				 DRM_MODE_ENCODER_TMDS, NULL);
 		drm_encoder_helper_add(encoder, &radeon_legacy_tmds_ext_helper_funcs);
 		if (!rdev->is_atom_bios)
 			radeon_encoder->enc_priv = radeon_legacy_get_ext_tmds_info(radeon_encoder);
diff --git a/drivers/gpu/drm/radeon/radeon_mem.c b/drivers/gpu/drm/radeon/radeon_mem.c
deleted file mode 100644
index 146d253f1131..000000000000
--- a/drivers/gpu/drm/radeon/radeon_mem.c
+++ /dev/null
@@ -1,302 +0,0 @@
-/* radeon_mem.c -- Simple GART/fb memory manager for radeon -*- linux-c -*- */
-/*
- * Copyright (C) The Weather Channel, Inc.  2002.  All Rights Reserved.
- *
- * The Weather Channel (TM) funded Tungsten Graphics to develop the
- * initial release of the Radeon 8500 driver under the XFree86 license.
- * This notice must be preserved.
- *
- * 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
- * PRECISION INSIGHT AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Keith Whitwell <keith@tungstengraphics.com>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-
-/* Very simple allocator for GART memory, working on a static range
- * already mapped into each client's address space.
- */
-
-static struct mem_block *split_block(struct mem_block *p, int start, int size,
-				     struct drm_file *file_priv)
-{
-	/* Maybe cut off the start of an existing block */
-	if (start > p->start) {
-		struct mem_block *newblock = kmalloc(sizeof(*newblock),
-						     GFP_KERNEL);
-		if (!newblock)
-			goto out;
-		newblock->start = start;
-		newblock->size = p->size - (start - p->start);
-		newblock->file_priv = NULL;
-		newblock->next = p->next;
-		newblock->prev = p;
-		p->next->prev = newblock;
-		p->next = newblock;
-		p->size -= newblock->size;
-		p = newblock;
-	}
-
-	/* Maybe cut off the end of an existing block */
-	if (size < p->size) {
-		struct mem_block *newblock = kmalloc(sizeof(*newblock),
-						     GFP_KERNEL);
-		if (!newblock)
-			goto out;
-		newblock->start = start + size;
-		newblock->size = p->size - size;
-		newblock->file_priv = NULL;
-		newblock->next = p->next;
-		newblock->prev = p;
-		p->next->prev = newblock;
-		p->next = newblock;
-		p->size = size;
-	}
-
-      out:
-	/* Our block is in the middle */
-	p->file_priv = file_priv;
-	return p;
-}
-
-static struct mem_block *alloc_block(struct mem_block *heap, int size,
-				     int align2, struct drm_file *file_priv)
-{
-	struct mem_block *p;
-	int mask = (1 << align2) - 1;
-
-	list_for_each(p, heap) {
-		int start = (p->start + mask) & ~mask;
-		if (p->file_priv == NULL && start + size <= p->start + p->size)
-			return split_block(p, start, size, file_priv);
-	}
-
-	return NULL;
-}
-
-static struct mem_block *find_block(struct mem_block *heap, int start)
-{
-	struct mem_block *p;
-
-	list_for_each(p, heap)
-	    if (p->start == start)
-		return p;
-
-	return NULL;
-}
-
-static void free_block(struct mem_block *p)
-{
-	p->file_priv = NULL;
-
-	/* Assumes a single contiguous range.  Needs a special file_priv in
-	 * 'heap' to stop it being subsumed.
-	 */
-	if (p->next->file_priv == NULL) {
-		struct mem_block *q = p->next;
-		p->size += q->size;
-		p->next = q->next;
-		p->next->prev = p;
-		kfree(q);
-	}
-
-	if (p->prev->file_priv == NULL) {
-		struct mem_block *q = p->prev;
-		q->size += p->size;
-		q->next = p->next;
-		q->next->prev = q;
-		kfree(p);
-	}
-}
-
-/* Initialize.  How to check for an uninitialized heap?
- */
-static int init_heap(struct mem_block **heap, int start, int size)
-{
-	struct mem_block *blocks = kmalloc(sizeof(*blocks), GFP_KERNEL);
-
-	if (!blocks)
-		return -ENOMEM;
-
-	*heap = kzalloc(sizeof(**heap), GFP_KERNEL);
-	if (!*heap) {
-		kfree(blocks);
-		return -ENOMEM;
-	}
-
-	blocks->start = start;
-	blocks->size = size;
-	blocks->file_priv = NULL;
-	blocks->next = blocks->prev = *heap;
-
-	(*heap)->file_priv = (struct drm_file *) - 1;
-	(*heap)->next = (*heap)->prev = blocks;
-	return 0;
-}
-
-/* Free all blocks associated with the releasing file.
- */
-void radeon_mem_release(struct drm_file *file_priv, struct mem_block *heap)
-{
-	struct mem_block *p;
-
-	if (!heap || !heap->next)
-		return;
-
-	list_for_each(p, heap) {
-		if (p->file_priv == file_priv)
-			p->file_priv = NULL;
-	}
-
-	/* Assumes a single contiguous range.  Needs a special file_priv in
-	 * 'heap' to stop it being subsumed.
-	 */
-	list_for_each(p, heap) {
-		while (p->file_priv == NULL && p->next->file_priv == NULL) {
-			struct mem_block *q = p->next;
-			p->size += q->size;
-			p->next = q->next;
-			p->next->prev = p;
-			kfree(q);
-		}
-	}
-}
-
-/* Shutdown.
- */
-void radeon_mem_takedown(struct mem_block **heap)
-{
-	struct mem_block *p;
-
-	if (!*heap)
-		return;
-
-	for (p = (*heap)->next; p != *heap;) {
-		struct mem_block *q = p;
-		p = p->next;
-		kfree(q);
-	}
-
-	kfree(*heap);
-	*heap = NULL;
-}
-
-/* IOCTL HANDLERS */
-
-static struct mem_block **get_heap(drm_radeon_private_t * dev_priv, int region)
-{
-	switch (region) {
-	case RADEON_MEM_REGION_GART:
-		return &dev_priv->gart_heap;
-	case RADEON_MEM_REGION_FB:
-		return &dev_priv->fb_heap;
-	default:
-		return NULL;
-	}
-}
-
-int radeon_mem_alloc(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_mem_alloc_t *alloc = data;
-	struct mem_block *block, **heap;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	heap = get_heap(dev_priv, alloc->region);
-	if (!heap || !*heap)
-		return -EFAULT;
-
-	/* Make things easier on ourselves: all allocations at least
-	 * 4k aligned.
-	 */
-	if (alloc->alignment < 12)
-		alloc->alignment = 12;
-
-	block = alloc_block(*heap, alloc->size, alloc->alignment, file_priv);
-
-	if (!block)
-		return -ENOMEM;
-
-	if (copy_to_user(alloc->region_offset, &block->start,
-			     sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-int radeon_mem_free(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_mem_free_t *memfree = data;
-	struct mem_block *block, **heap;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	heap = get_heap(dev_priv, memfree->region);
-	if (!heap || !*heap)
-		return -EFAULT;
-
-	block = find_block(*heap, memfree->region_offset);
-	if (!block)
-		return -EFAULT;
-
-	if (block->file_priv != file_priv)
-		return -EPERM;
-
-	free_block(block);
-	return 0;
-}
-
-int radeon_mem_init_heap(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_mem_init_heap_t *initheap = data;
-	struct mem_block **heap;
-
-	if (!dev_priv) {
-		DRM_ERROR("called with no initialization\n");
-		return -EINVAL;
-	}
-
-	heap = get_heap(dev_priv, initheap->region);
-	if (!heap)
-		return -EFAULT;
-
-	if (*heap) {
-		DRM_ERROR("heap already initialized?");
-		return -EFAULT;
-	}
-
-	return init_heap(heap, initheap->start, initheap->size);
-}
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index bba112628b47..bb75201a24ba 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -757,8 +757,10 @@ extern u8 radeon_dp_getsinktype(struct radeon_connector *radeon_connector);
 extern bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector);
 extern int radeon_dp_get_panel_mode(struct drm_encoder *encoder,
 				    struct drm_connector *connector);
-int radeon_dp_get_max_link_rate(struct drm_connector *connector,
-				const u8 *dpcd);
+extern int radeon_dp_get_dp_link_config(struct drm_connector *connector,
+					const u8 *dpcd,
+					unsigned pix_clock,
+					unsigned *dp_lanes, unsigned *dp_rate);
 extern void radeon_dp_set_rx_power_state(struct drm_connector *connector,
 					 u8 power_state);
 extern void radeon_dp_aux_init(struct radeon_connector *radeon_connector);
@@ -934,7 +936,7 @@ extern void radeon_crtc_fb_gamma_get(struct drm_crtc *crtc, u16 *red, u16 *green
 				     u16 *blue, int regno);
 int radeon_framebuffer_init(struct drm_device *dev,
 			     struct radeon_framebuffer *rfb,
-			     struct drm_mode_fb_cmd2 *mode_cmd,
+			     const struct drm_mode_fb_cmd2 *mode_cmd,
 			     struct drm_gem_object *obj);
 
 int radeonfb_remove(struct drm_device *dev, struct drm_framebuffer *fb);
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 84d45633d28c..fb6ad143873f 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -33,6 +33,7 @@
 #include <linux/slab.h>
 #include <drm/drmP.h>
 #include <drm/radeon_drm.h>
+#include <drm/drm_cache.h>
 #include "radeon.h"
 #include "radeon_trace.h"
 
@@ -245,6 +246,12 @@ int radeon_bo_create(struct radeon_device *rdev,
 		DRM_INFO_ONCE("Please enable CONFIG_MTRR and CONFIG_X86_PAT for "
 			      "better performance thanks to write-combining\n");
 	bo->flags &= ~(RADEON_GEM_GTT_WC | RADEON_GEM_GTT_UC);
+#else
+	/* For architectures that don't support WC memory,
+	 * mask out the WC flag from the BO
+	 */
+	if (!drm_arch_can_wc_memory())
+		bo->flags &= ~RADEON_GEM_GTT_WC;
 #endif
 
 	radeon_ttm_placement_from_domain(bo, domain);
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 59abebd6b5dc..7a98823bacd1 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -276,8 +276,12 @@ static void radeon_pm_set_clocks(struct radeon_device *rdev)
 	if (rdev->irq.installed) {
 		for (i = 0; i < rdev->num_crtc; i++) {
 			if (rdev->pm.active_crtcs & (1 << i)) {
-				rdev->pm.req_vblank |= (1 << i);
-				drm_vblank_get(rdev->ddev, i);
+				/* This can fail if a modeset is in progress */
+				if (drm_vblank_get(rdev->ddev, i) == 0)
+					rdev->pm.req_vblank |= (1 << i);
+				else
+					DRM_DEBUG_DRIVER("crtc %d no vblank, can glitch\n",
+							 i);
 			}
 		}
 	}
@@ -713,7 +717,7 @@ static struct attribute *hwmon_attributes[] = {
 static umode_t hwmon_attributes_visible(struct kobject *kobj,
 					struct attribute *attr, int index)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct radeon_device *rdev = dev_get_drvdata(dev);
 	umode_t effective_mode = attr->mode;
 
@@ -1078,10 +1082,6 @@ force:
 	/* update displays */
 	radeon_dpm_display_configuration_changed(rdev);
 
-	rdev->pm.dpm.current_active_crtcs = rdev->pm.dpm.new_active_crtcs;
-	rdev->pm.dpm.current_active_crtc_count = rdev->pm.dpm.new_active_crtc_count;
-	rdev->pm.dpm.single_display = single_display;
-
 	/* wait for the rings to drain */
 	for (i = 0; i < RADEON_NUM_RINGS; i++) {
 		struct radeon_ring *ring = &rdev->ring[i];
@@ -1097,6 +1097,10 @@ force:
 
 	radeon_dpm_post_set_power_state(rdev);
 
+	rdev->pm.dpm.current_active_crtcs = rdev->pm.dpm.new_active_crtcs;
+	rdev->pm.dpm.current_active_crtc_count = rdev->pm.dpm.new_active_crtc_count;
+	rdev->pm.dpm.single_display = single_display;
+
 	if (rdev->asic->dpm.force_performance_level) {
 		if (rdev->pm.dpm.thermal_active) {
 			enum radeon_dpm_forced_level level = rdev->pm.dpm.forced_level;
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index c507896aca45..197b157b73d0 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -349,8 +349,13 @@ int radeon_sa_bo_new(struct radeon_device *rdev,
 			/* see if we can skip over some allocations */
 		} while (radeon_sa_bo_next_hole(sa_manager, fences, tries));
 
+		for (i = 0; i < RADEON_NUM_RINGS; ++i)
+			radeon_fence_ref(fences[i]);
+
 		spin_unlock(&sa_manager->wq.lock);
 		r = radeon_fence_wait_any(rdev, fences, false);
+		for (i = 0; i < RADEON_NUM_RINGS; ++i)
+			radeon_fence_unref(&fences[i]);
 		spin_lock(&sa_manager->wq.lock);
 		/* if we have nothing to wait for block */
 		if (r == -ENOENT) {
diff --git a/drivers/gpu/drm/radeon/radeon_state.c b/drivers/gpu/drm/radeon/radeon_state.c
deleted file mode 100644
index 15aee723db77..000000000000
--- a/drivers/gpu/drm/radeon/radeon_state.c
+++ /dev/null
@@ -1,3261 +0,0 @@
-/* radeon_state.c -- State support for Radeon -*- linux-c -*- */
-/*
- * Copyright 2000 VA Linux Systems, Inc., Fremont, California.
- * 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
- * PRECISION INSIGHT AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
- * DEALINGS IN THE SOFTWARE.
- *
- * Authors:
- *    Gareth Hughes <gareth@valinux.com>
- *    Kevin E. Martin <martin@valinux.com>
- *
- * ------------------------ This file is DEPRECATED! -------------------------
- */
-
-#include <drm/drmP.h>
-#include <drm/radeon_drm.h>
-#include "radeon_drv.h"
-#include "drm_buffer.h"
-
-/* ================================================================
- * Helper functions for client state checking and fixup
- */
-
-static __inline__ int radeon_check_and_fixup_offset(drm_radeon_private_t *
-						    dev_priv,
-						    struct drm_file * file_priv,
-						    u32 *offset)
-{
-	u64 off = *offset;
-	u32 fb_end = dev_priv->fb_location + dev_priv->fb_size - 1;
-	struct drm_radeon_driver_file_fields *radeon_priv;
-
-	/* Hrm ... the story of the offset ... So this function converts
-	 * the various ideas of what userland clients might have for an
-	 * offset in the card address space into an offset into the card
-	 * address space :) So with a sane client, it should just keep
-	 * the value intact and just do some boundary checking. However,
-	 * not all clients are sane. Some older clients pass us 0 based
-	 * offsets relative to the start of the framebuffer and some may
-	 * assume the AGP aperture it appended to the framebuffer, so we
-	 * try to detect those cases and fix them up.
-	 *
-	 * Note: It might be a good idea here to make sure the offset lands
-	 * in some "allowed" area to protect things like the PCIE GART...
-	 */
-
-	/* First, the best case, the offset already lands in either the
-	 * framebuffer or the GART mapped space
-	 */
-	if (radeon_check_offset(dev_priv, off))
-		return 0;
-
-	/* Ok, that didn't happen... now check if we have a zero based
-	 * offset that fits in the framebuffer + gart space, apply the
-	 * magic offset we get from SETPARAM or calculated from fb_location
-	 */
-	if (off < (dev_priv->fb_size + dev_priv->gart_size)) {
-		radeon_priv = file_priv->driver_priv;
-		off += radeon_priv->radeon_fb_delta;
-	}
-
-	/* Finally, assume we aimed at a GART offset if beyond the fb */
-	if (off > fb_end)
-		off = off - fb_end - 1 + dev_priv->gart_vm_start;
-
-	/* Now recheck and fail if out of bounds */
-	if (radeon_check_offset(dev_priv, off)) {
-		DRM_DEBUG("offset fixed up to 0x%x\n", (unsigned int)off);
-		*offset = off;
-		return 0;
-	}
-	return -EINVAL;
-}
-
-static __inline__ int radeon_check_and_fixup_packets(drm_radeon_private_t *
-						     dev_priv,
-						     struct drm_file *file_priv,
-						     int id, struct drm_buffer *buf)
-{
-	u32 *data;
-	switch (id) {
-
-	case RADEON_EMIT_PP_MISC:
-		data = drm_buffer_pointer_to_dword(buf,
-			(RADEON_RB3D_DEPTHOFFSET - RADEON_PP_MISC) / 4);
-
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, data)) {
-			DRM_ERROR("Invalid depth buffer offset\n");
-			return -EINVAL;
-		}
-		dev_priv->have_z_offset = 1;
-		break;
-
-	case RADEON_EMIT_PP_CNTL:
-		data = drm_buffer_pointer_to_dword(buf,
-			(RADEON_RB3D_COLOROFFSET - RADEON_PP_CNTL) / 4);
-
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, data)) {
-			DRM_ERROR("Invalid colour buffer offset\n");
-			return -EINVAL;
-		}
-		break;
-
-	case R200_EMIT_PP_TXOFFSET_0:
-	case R200_EMIT_PP_TXOFFSET_1:
-	case R200_EMIT_PP_TXOFFSET_2:
-	case R200_EMIT_PP_TXOFFSET_3:
-	case R200_EMIT_PP_TXOFFSET_4:
-	case R200_EMIT_PP_TXOFFSET_5:
-		data = drm_buffer_pointer_to_dword(buf, 0);
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, data)) {
-			DRM_ERROR("Invalid R200 texture offset\n");
-			return -EINVAL;
-		}
-		break;
-
-	case RADEON_EMIT_PP_TXFILTER_0:
-	case RADEON_EMIT_PP_TXFILTER_1:
-	case RADEON_EMIT_PP_TXFILTER_2:
-		data = drm_buffer_pointer_to_dword(buf,
-			(RADEON_PP_TXOFFSET_0 - RADEON_PP_TXFILTER_0) / 4);
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, data)) {
-			DRM_ERROR("Invalid R100 texture offset\n");
-			return -EINVAL;
-		}
-		break;
-
-	case R200_EMIT_PP_CUBIC_OFFSETS_0:
-	case R200_EMIT_PP_CUBIC_OFFSETS_1:
-	case R200_EMIT_PP_CUBIC_OFFSETS_2:
-	case R200_EMIT_PP_CUBIC_OFFSETS_3:
-	case R200_EMIT_PP_CUBIC_OFFSETS_4:
-	case R200_EMIT_PP_CUBIC_OFFSETS_5:{
-			int i;
-			for (i = 0; i < 5; i++) {
-				data = drm_buffer_pointer_to_dword(buf, i);
-				if (radeon_check_and_fixup_offset(dev_priv,
-								  file_priv,
-								  data)) {
-					DRM_ERROR
-					    ("Invalid R200 cubic texture offset\n");
-					return -EINVAL;
-				}
-			}
-			break;
-		}
-
-	case RADEON_EMIT_PP_CUBIC_OFFSETS_T0:
-	case RADEON_EMIT_PP_CUBIC_OFFSETS_T1:
-	case RADEON_EMIT_PP_CUBIC_OFFSETS_T2:{
-			int i;
-			for (i = 0; i < 5; i++) {
-				data = drm_buffer_pointer_to_dword(buf, i);
-				if (radeon_check_and_fixup_offset(dev_priv,
-								  file_priv,
-								  data)) {
-					DRM_ERROR
-					    ("Invalid R100 cubic texture offset\n");
-					return -EINVAL;
-				}
-			}
-		}
-		break;
-
-	case R200_EMIT_VAP_CTL:{
-			RING_LOCALS;
-			BEGIN_RING(2);
-			OUT_RING_REG(RADEON_SE_TCL_STATE_FLUSH, 0);
-			ADVANCE_RING();
-		}
-		break;
-
-	case RADEON_EMIT_RB3D_COLORPITCH:
-	case RADEON_EMIT_RE_LINE_PATTERN:
-	case RADEON_EMIT_SE_LINE_WIDTH:
-	case RADEON_EMIT_PP_LUM_MATRIX:
-	case RADEON_EMIT_PP_ROT_MATRIX_0:
-	case RADEON_EMIT_RB3D_STENCILREFMASK:
-	case RADEON_EMIT_SE_VPORT_XSCALE:
-	case RADEON_EMIT_SE_CNTL:
-	case RADEON_EMIT_SE_CNTL_STATUS:
-	case RADEON_EMIT_RE_MISC:
-	case RADEON_EMIT_PP_BORDER_COLOR_0:
-	case RADEON_EMIT_PP_BORDER_COLOR_1:
-	case RADEON_EMIT_PP_BORDER_COLOR_2:
-	case RADEON_EMIT_SE_ZBIAS_FACTOR:
-	case RADEON_EMIT_SE_TCL_OUTPUT_VTX_FMT:
-	case RADEON_EMIT_SE_TCL_MATERIAL_EMMISSIVE_RED:
-	case R200_EMIT_PP_TXCBLEND_0:
-	case R200_EMIT_PP_TXCBLEND_1:
-	case R200_EMIT_PP_TXCBLEND_2:
-	case R200_EMIT_PP_TXCBLEND_3:
-	case R200_EMIT_PP_TXCBLEND_4:
-	case R200_EMIT_PP_TXCBLEND_5:
-	case R200_EMIT_PP_TXCBLEND_6:
-	case R200_EMIT_PP_TXCBLEND_7:
-	case R200_EMIT_TCL_LIGHT_MODEL_CTL_0:
-	case R200_EMIT_TFACTOR_0:
-	case R200_EMIT_VTX_FMT_0:
-	case R200_EMIT_MATRIX_SELECT_0:
-	case R200_EMIT_TEX_PROC_CTL_2:
-	case R200_EMIT_TCL_UCP_VERT_BLEND_CTL:
-	case R200_EMIT_PP_TXFILTER_0:
-	case R200_EMIT_PP_TXFILTER_1:
-	case R200_EMIT_PP_TXFILTER_2:
-	case R200_EMIT_PP_TXFILTER_3:
-	case R200_EMIT_PP_TXFILTER_4:
-	case R200_EMIT_PP_TXFILTER_5:
-	case R200_EMIT_VTE_CNTL:
-	case R200_EMIT_OUTPUT_VTX_COMP_SEL:
-	case R200_EMIT_PP_TAM_DEBUG3:
-	case R200_EMIT_PP_CNTL_X:
-	case R200_EMIT_RB3D_DEPTHXY_OFFSET:
-	case R200_EMIT_RE_AUX_SCISSOR_CNTL:
-	case R200_EMIT_RE_SCISSOR_TL_0:
-	case R200_EMIT_RE_SCISSOR_TL_1:
-	case R200_EMIT_RE_SCISSOR_TL_2:
-	case R200_EMIT_SE_VAP_CNTL_STATUS:
-	case R200_EMIT_SE_VTX_STATE_CNTL:
-	case R200_EMIT_RE_POINTSIZE:
-	case R200_EMIT_TCL_INPUT_VTX_VECTOR_ADDR_0:
-	case R200_EMIT_PP_CUBIC_FACES_0:
-	case R200_EMIT_PP_CUBIC_FACES_1:
-	case R200_EMIT_PP_CUBIC_FACES_2:
-	case R200_EMIT_PP_CUBIC_FACES_3:
-	case R200_EMIT_PP_CUBIC_FACES_4:
-	case R200_EMIT_PP_CUBIC_FACES_5:
-	case RADEON_EMIT_PP_TEX_SIZE_0:
-	case RADEON_EMIT_PP_TEX_SIZE_1:
-	case RADEON_EMIT_PP_TEX_SIZE_2:
-	case R200_EMIT_RB3D_BLENDCOLOR:
-	case R200_EMIT_TCL_POINT_SPRITE_CNTL:
-	case RADEON_EMIT_PP_CUBIC_FACES_0:
-	case RADEON_EMIT_PP_CUBIC_FACES_1:
-	case RADEON_EMIT_PP_CUBIC_FACES_2:
-	case R200_EMIT_PP_TRI_PERF_CNTL:
-	case R200_EMIT_PP_AFS_0:
-	case R200_EMIT_PP_AFS_1:
-	case R200_EMIT_ATF_TFACTOR:
-	case R200_EMIT_PP_TXCTLALL_0:
-	case R200_EMIT_PP_TXCTLALL_1:
-	case R200_EMIT_PP_TXCTLALL_2:
-	case R200_EMIT_PP_TXCTLALL_3:
-	case R200_EMIT_PP_TXCTLALL_4:
-	case R200_EMIT_PP_TXCTLALL_5:
-	case R200_EMIT_VAP_PVS_CNTL:
-		/* These packets don't contain memory offsets */
-		break;
-
-	default:
-		DRM_ERROR("Unknown state packet ID %d\n", id);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int radeon_check_and_fixup_packet3(drm_radeon_private_t *
-					  dev_priv,
-					  struct drm_file *file_priv,
-					  drm_radeon_kcmd_buffer_t *
-					  cmdbuf,
-					  unsigned int *cmdsz)
-{
-	u32 *cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 0);
-	u32 offset, narrays;
-	int count, i, k;
-
-	count = ((*cmd & RADEON_CP_PACKET_COUNT_MASK) >> 16);
-	*cmdsz = 2 + count;
-
-	if ((*cmd & 0xc0000000) != RADEON_CP_PACKET3) {
-		DRM_ERROR("Not a type 3 packet\n");
-		return -EINVAL;
-	}
-
-	if (4 * *cmdsz > drm_buffer_unprocessed(cmdbuf->buffer)) {
-		DRM_ERROR("Packet size larger than size of data provided\n");
-		return -EINVAL;
-	}
-
-	switch (*cmd & 0xff00) {
-	/* XXX Are there old drivers needing other packets? */
-
-	case RADEON_3D_DRAW_IMMD:
-	case RADEON_3D_DRAW_VBUF:
-	case RADEON_3D_DRAW_INDX:
-	case RADEON_WAIT_FOR_IDLE:
-	case RADEON_CP_NOP:
-	case RADEON_3D_CLEAR_ZMASK:
-/*	case RADEON_CP_NEXT_CHAR:
-	case RADEON_CP_PLY_NEXTSCAN:
-	case RADEON_CP_SET_SCISSORS: */ /* probably safe but will never need them? */
-		/* these packets are safe */
-		break;
-
-	case RADEON_CP_3D_DRAW_IMMD_2:
-	case RADEON_CP_3D_DRAW_VBUF_2:
-	case RADEON_CP_3D_DRAW_INDX_2:
-	case RADEON_3D_CLEAR_HIZ:
-		/* safe but r200 only */
-		if (dev_priv->microcode_version != UCODE_R200) {
-			DRM_ERROR("Invalid 3d packet for r100-class chip\n");
-			return -EINVAL;
-		}
-		break;
-
-	case RADEON_3D_LOAD_VBPNTR:
-
-		if (count > 18) { /* 12 arrays max */
-			DRM_ERROR("Too large payload in 3D_LOAD_VBPNTR (count=%d)\n",
-				  count);
-			return -EINVAL;
-		}
-
-		/* carefully check packet contents */
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-
-		narrays = *cmd & ~0xc000;
-		k = 0;
-		i = 2;
-		while ((k < narrays) && (i < (count + 2))) {
-			i++;		/* skip attribute field */
-			cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, i);
-			if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-							  cmd)) {
-				DRM_ERROR
-				    ("Invalid offset (k=%d i=%d) in 3D_LOAD_VBPNTR packet.\n",
-				     k, i);
-				return -EINVAL;
-			}
-			k++;
-			i++;
-			if (k == narrays)
-				break;
-			/* have one more to process, they come in pairs */
-			cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, i);
-
-			if (radeon_check_and_fixup_offset(dev_priv,
-							  file_priv, cmd))
-			{
-				DRM_ERROR
-				    ("Invalid offset (k=%d i=%d) in 3D_LOAD_VBPNTR packet.\n",
-				     k, i);
-				return -EINVAL;
-			}
-			k++;
-			i++;
-		}
-		/* do the counts match what we expect ? */
-		if ((k != narrays) || (i != (count + 2))) {
-			DRM_ERROR
-			    ("Malformed 3D_LOAD_VBPNTR packet (k=%d i=%d narrays=%d count+1=%d).\n",
-			      k, i, narrays, count + 1);
-			return -EINVAL;
-		}
-		break;
-
-	case RADEON_3D_RNDR_GEN_INDX_PRIM:
-		if (dev_priv->microcode_version != UCODE_R100) {
-			DRM_ERROR("Invalid 3d packet for r200-class chip\n");
-			return -EINVAL;
-		}
-
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, cmd)) {
-				DRM_ERROR("Invalid rndr_gen_indx offset\n");
-				return -EINVAL;
-		}
-		break;
-
-	case RADEON_CP_INDX_BUFFER:
-		if (dev_priv->microcode_version != UCODE_R200) {
-			DRM_ERROR("Invalid 3d packet for r100-class chip\n");
-			return -EINVAL;
-		}
-
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-		if ((*cmd & 0x8000ffff) != 0x80000810) {
-			DRM_ERROR("Invalid indx_buffer reg address %08X\n", *cmd);
-			return -EINVAL;
-		}
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 2);
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv, cmd)) {
-			DRM_ERROR("Invalid indx_buffer offset is %08X\n", *cmd);
-			return -EINVAL;
-		}
-		break;
-
-	case RADEON_CNTL_HOSTDATA_BLT:
-	case RADEON_CNTL_PAINT_MULTI:
-	case RADEON_CNTL_BITBLT_MULTI:
-		/* MSB of opcode: next DWORD GUI_CNTL */
-		cmd = drm_buffer_pointer_to_dword(cmdbuf->buffer, 1);
-		if (*cmd & (RADEON_GMC_SRC_PITCH_OFFSET_CNTL
-			      | RADEON_GMC_DST_PITCH_OFFSET_CNTL)) {
-			u32 *cmd2 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 2);
-			offset = *cmd2 << 10;
-			if (radeon_check_and_fixup_offset
-			    (dev_priv, file_priv, &offset)) {
-				DRM_ERROR("Invalid first packet offset\n");
-				return -EINVAL;
-			}
-			*cmd2 = (*cmd2 & 0xffc00000) | offset >> 10;
-		}
-
-		if ((*cmd & RADEON_GMC_SRC_PITCH_OFFSET_CNTL) &&
-		    (*cmd & RADEON_GMC_DST_PITCH_OFFSET_CNTL)) {
-			u32 *cmd3 = drm_buffer_pointer_to_dword(cmdbuf->buffer, 3);
-			offset = *cmd3 << 10;
-			if (radeon_check_and_fixup_offset
-			    (dev_priv, file_priv, &offset)) {
-				DRM_ERROR("Invalid second packet offset\n");
-				return -EINVAL;
-			}
-			*cmd3 = (*cmd3 & 0xffc00000) | offset >> 10;
-		}
-		break;
-
-	default:
-		DRM_ERROR("Invalid packet type %x\n", *cmd & 0xff00);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-/* ================================================================
- * CP hardware state programming functions
- */
-
-static void radeon_emit_clip_rect(drm_radeon_private_t * dev_priv,
-				  struct drm_clip_rect * box)
-{
-	RING_LOCALS;
-
-	DRM_DEBUG("   box:  x1=%d y1=%d  x2=%d y2=%d\n",
-		  box->x1, box->y1, box->x2, box->y2);
-
-	BEGIN_RING(4);
-	OUT_RING(CP_PACKET0(RADEON_RE_TOP_LEFT, 0));
-	OUT_RING((box->y1 << 16) | box->x1);
-	OUT_RING(CP_PACKET0(RADEON_RE_WIDTH_HEIGHT, 0));
-	OUT_RING(((box->y2 - 1) << 16) | (box->x2 - 1));
-	ADVANCE_RING();
-}
-
-/* Emit 1.1 state
- */
-static int radeon_emit_state(drm_radeon_private_t * dev_priv,
-			     struct drm_file *file_priv,
-			     drm_radeon_context_regs_t * ctx,
-			     drm_radeon_texture_regs_t * tex,
-			     unsigned int dirty)
-{
-	RING_LOCALS;
-	DRM_DEBUG("dirty=0x%08x\n", dirty);
-
-	if (dirty & RADEON_UPLOAD_CONTEXT) {
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-						  &ctx->rb3d_depthoffset)) {
-			DRM_ERROR("Invalid depth buffer offset\n");
-			return -EINVAL;
-		}
-
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-						  &ctx->rb3d_coloroffset)) {
-			DRM_ERROR("Invalid depth buffer offset\n");
-			return -EINVAL;
-		}
-
-		BEGIN_RING(14);
-		OUT_RING(CP_PACKET0(RADEON_PP_MISC, 6));
-		OUT_RING(ctx->pp_misc);
-		OUT_RING(ctx->pp_fog_color);
-		OUT_RING(ctx->re_solid_color);
-		OUT_RING(ctx->rb3d_blendcntl);
-		OUT_RING(ctx->rb3d_depthoffset);
-		OUT_RING(ctx->rb3d_depthpitch);
-		OUT_RING(ctx->rb3d_zstencilcntl);
-		OUT_RING(CP_PACKET0(RADEON_PP_CNTL, 2));
-		OUT_RING(ctx->pp_cntl);
-		OUT_RING(ctx->rb3d_cntl);
-		OUT_RING(ctx->rb3d_coloroffset);
-		OUT_RING(CP_PACKET0(RADEON_RB3D_COLORPITCH, 0));
-		OUT_RING(ctx->rb3d_colorpitch);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_VERTFMT) {
-		BEGIN_RING(2);
-		OUT_RING(CP_PACKET0(RADEON_SE_COORD_FMT, 0));
-		OUT_RING(ctx->se_coord_fmt);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_LINE) {
-		BEGIN_RING(5);
-		OUT_RING(CP_PACKET0(RADEON_RE_LINE_PATTERN, 1));
-		OUT_RING(ctx->re_line_pattern);
-		OUT_RING(ctx->re_line_state);
-		OUT_RING(CP_PACKET0(RADEON_SE_LINE_WIDTH, 0));
-		OUT_RING(ctx->se_line_width);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_BUMPMAP) {
-		BEGIN_RING(5);
-		OUT_RING(CP_PACKET0(RADEON_PP_LUM_MATRIX, 0));
-		OUT_RING(ctx->pp_lum_matrix);
-		OUT_RING(CP_PACKET0(RADEON_PP_ROT_MATRIX_0, 1));
-		OUT_RING(ctx->pp_rot_matrix_0);
-		OUT_RING(ctx->pp_rot_matrix_1);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_MASKS) {
-		BEGIN_RING(4);
-		OUT_RING(CP_PACKET0(RADEON_RB3D_STENCILREFMASK, 2));
-		OUT_RING(ctx->rb3d_stencilrefmask);
-		OUT_RING(ctx->rb3d_ropcntl);
-		OUT_RING(ctx->rb3d_planemask);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_VIEWPORT) {
-		BEGIN_RING(7);
-		OUT_RING(CP_PACKET0(RADEON_SE_VPORT_XSCALE, 5));
-		OUT_RING(ctx->se_vport_xscale);
-		OUT_RING(ctx->se_vport_xoffset);
-		OUT_RING(ctx->se_vport_yscale);
-		OUT_RING(ctx->se_vport_yoffset);
-		OUT_RING(ctx->se_vport_zscale);
-		OUT_RING(ctx->se_vport_zoffset);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_SETUP) {
-		BEGIN_RING(4);
-		OUT_RING(CP_PACKET0(RADEON_SE_CNTL, 0));
-		OUT_RING(ctx->se_cntl);
-		OUT_RING(CP_PACKET0(RADEON_SE_CNTL_STATUS, 0));
-		OUT_RING(ctx->se_cntl_status);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_MISC) {
-		BEGIN_RING(2);
-		OUT_RING(CP_PACKET0(RADEON_RE_MISC, 0));
-		OUT_RING(ctx->re_misc);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_TEX0) {
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-						  &tex[0].pp_txoffset)) {
-			DRM_ERROR("Invalid texture offset for unit 0\n");
-			return -EINVAL;
-		}
-
-		BEGIN_RING(9);
-		OUT_RING(CP_PACKET0(RADEON_PP_TXFILTER_0, 5));
-		OUT_RING(tex[0].pp_txfilter);
-		OUT_RING(tex[0].pp_txformat);
-		OUT_RING(tex[0].pp_txoffset);
-		OUT_RING(tex[0].pp_txcblend);
-		OUT_RING(tex[0].pp_txablend);
-		OUT_RING(tex[0].pp_tfactor);
-		OUT_RING(CP_PACKET0(RADEON_PP_BORDER_COLOR_0, 0));
-		OUT_RING(tex[0].pp_border_color);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_TEX1) {
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-						  &tex[1].pp_txoffset)) {
-			DRM_ERROR("Invalid texture offset for unit 1\n");
-			return -EINVAL;
-		}
-
-		BEGIN_RING(9);
-		OUT_RING(CP_PACKET0(RADEON_PP_TXFILTER_1, 5));
-		OUT_RING(tex[1].pp_txfilter);
-		OUT_RING(tex[1].pp_txformat);
-		OUT_RING(tex[1].pp_txoffset);
-		OUT_RING(tex[1].pp_txcblend);
-		OUT_RING(tex[1].pp_txablend);
-		OUT_RING(tex[1].pp_tfactor);
-		OUT_RING(CP_PACKET0(RADEON_PP_BORDER_COLOR_1, 0));
-		OUT_RING(tex[1].pp_border_color);
-		ADVANCE_RING();
-	}
-
-	if (dirty & RADEON_UPLOAD_TEX2) {
-		if (radeon_check_and_fixup_offset(dev_priv, file_priv,
-						  &tex[2].pp_txoffset)) {
-			DRM_ERROR("Invalid texture offset for unit 2\n");
-			return -EINVAL;
-		}
-
-		BEGIN_RING(9);
-		OUT_RING(CP_PACKET0(RADEON_PP_TXFILTER_2, 5));
-		OUT_RING(tex[2].pp_txfilter);
-		OUT_RING(tex[2].pp_txformat);
-		OUT_RING(tex[2].pp_txoffset);
-		OUT_RING(tex[2].pp_txcblend);
-		OUT_RING(tex[2].pp_txablend);
-		OUT_RING(tex[2].pp_tfactor);
-		OUT_RING(CP_PACKET0(RADEON_PP_BORDER_COLOR_2, 0));
-		OUT_RING(tex[2].pp_border_color);
-		ADVANCE_RING();
-	}
-
-	return 0;
-}
-
-/* Emit 1.2 state
- */
-static int radeon_emit_state2(drm_radeon_private_t * dev_priv,
-			      struct drm_file *file_priv,
-			      drm_radeon_state_t * state)
-{
-	RING_LOCALS;
-
-	if (state->dirty & RADEON_UPLOAD_ZBIAS) {
-		BEGIN_RING(3);
-		OUT_RING(CP_PACKET0(RADEON_SE_ZBIAS_FACTOR, 1));
-		OUT_RING(state->context2.se_zbias_factor);
-		OUT_RING(state->context2.se_zbias_constant);
-		ADVANCE_RING();
-	}
-
-	return radeon_emit_state(dev_priv, file_priv, &state->context,
-				 state->tex, state->dirty);
-}
-
-/* New (1.3) state mechanism.  3 commands (packet, scalar, vector) in
- * 1.3 cmdbuffers allow all previous state to be updated as well as
- * the tcl scalar and vector areas.
- */
-static struct {
-	int start;
-	int len;
-	const char *name;
-} packet[RADEON_MAX_STATE_PACKETS] = {
-	{RADEON_PP_MISC, 7, "RADEON_PP_MISC"},
-	{RADEON_PP_CNTL, 3, "RADEON_PP_CNTL"},
-	{RADEON_RB3D_COLORPITCH, 1, "RADEON_RB3D_COLORPITCH"},
-	{RADEON_RE_LINE_PATTERN, 2, "RADEON_RE_LINE_PATTERN"},
-	{RADEON_SE_LINE_WIDTH, 1, "RADEON_SE_LINE_WIDTH"},
-	{RADEON_PP_LUM_MATRIX, 1, "RADEON_PP_LUM_MATRIX"},
-	{RADEON_PP_ROT_MATRIX_0, 2, "RADEON_PP_ROT_MATRIX_0"},
-	{RADEON_RB3D_STENCILREFMASK, 3, "RADEON_RB3D_STENCILREFMASK"},
-	{RADEON_SE_VPORT_XSCALE, 6, "RADEON_SE_VPORT_XSCALE"},
-	{RADEON_SE_CNTL, 2, "RADEON_SE_CNTL"},
-	{RADEON_SE_CNTL_STATUS, 1, "RADEON_SE_CNTL_STATUS"},
-	{RADEON_RE_MISC, 1, "RADEON_RE_MISC"},
-	{RADEON_PP_TXFILTER_0, 6, "RADEON_PP_TXFILTER_0"},
-	{RADEON_PP_BORDER_COLOR_0, 1, "RADEON_PP_BORDER_COLOR_0"},
-	{RADEON_PP_TXFILTER_1, 6, "RADEON_PP_TXFILTER_1"},
-	{RADEON_PP_BORDER_COLOR_1, 1, "RADEON_PP_BORDER_COLOR_1"},
-	{RADEON_PP_TXFILTER_2, 6, "RADEON_PP_TXFILTER_2"},
-	{RADEON_PP_BORDER_COLOR_2, 1, "RADEON_PP_BORDER_COLOR_2"},
-	{RADEON_SE_ZBIAS_FACTOR, 2, "RADEON_SE_ZBIAS_FACTOR"},
-	{RADEON_SE_TCL_OUTPUT_VTX_FMT, 11, "RADEON_SE_TCL_OUTPUT_VTX_FMT"},
-	{RADEON_SE_TCL_MATERIAL_EMMISSIVE_RED, 17,
-		    "RADEON_SE_TCL_MATERIAL_EMMISSIVE_RED"},
-	{R200_PP_TXCBLEND_0, 4, "R200_PP_TXCBLEND_0"},
-	{R200_PP_TXCBLEND_1, 4, "R200_PP_TXCBLEND_1"},
-	{R200_PP_TXCBLEND_2, 4, "R200_PP_TXCBLEND_2"},
-	{R200_PP_TXCBLEND_3, 4, "R200_PP_TXCBLEND_3"},
-	{R200_PP_TXCBLEND_4, 4, "R200_PP_TXCBLEND_4"},
-	{R200_PP_TXCBLEND_5, 4, "R200_PP_TXCBLEND_5"},
-	{R200_PP_TXCBLEND_6, 4, "R200_PP_TXCBLEND_6"},
-	{R200_PP_TXCBLEND_7, 4, "R200_PP_TXCBLEND_7"},
-	{R200_SE_TCL_LIGHT_MODEL_CTL_0, 6, "R200_SE_TCL_LIGHT_MODEL_CTL_0"},
-	{R200_PP_TFACTOR_0, 6, "R200_PP_TFACTOR_0"},
-	{R200_SE_VTX_FMT_0, 4, "R200_SE_VTX_FMT_0"},
-	{R200_SE_VAP_CNTL, 1, "R200_SE_VAP_CNTL"},
-	{R200_SE_TCL_MATRIX_SEL_0, 5, "R200_SE_TCL_MATRIX_SEL_0"},
-	{R200_SE_TCL_TEX_PROC_CTL_2, 5, "R200_SE_TCL_TEX_PROC_CTL_2"},
-	{R200_SE_TCL_UCP_VERT_BLEND_CTL, 1, "R200_SE_TCL_UCP_VERT_BLEND_CTL"},
-	{R200_PP_TXFILTER_0, 6, "R200_PP_TXFILTER_0"},
-	{R200_PP_TXFILTER_1, 6, "R200_PP_TXFILTER_1"},
-	{R200_PP_TXFILTER_2, 6, "R200_PP_TXFILTER_2"},
-	{R200_PP_TXFILTER_3, 6, "R200_PP_TXFILTER_3"},
-	{R200_PP_TXFILTER_4, 6, "R200_PP_TXFILTER_4"},
-	{R200_PP_TXFILTER_5, 6, "R200_PP_TXFILTER_5"},
-	{R200_PP_TXOFFSET_0, 1, "R200_PP_TXOFFSET_0"},
-	{R200_PP_TXOFFSET_1, 1, "R200_PP_TXOFFSET_1"},
-	{R200_PP_TXOFFSET_2, 1, "R200_PP_TXOFFSET_2"},
-	{R200_PP_TXOFFSET_3, 1, "R200_PP_TXOFFSET_3"},
-	{R200_PP_TXOFFSET_4, 1, "R200_PP_TXOFFSET_4"},
-	{R200_PP_TXOFFSET_5, 1, "R200_PP_TXOFFSET_5"},
-	{R200_SE_VTE_CNTL, 1, "R200_SE_VTE_CNTL"},
-	{R200_SE_TCL_OUTPUT_VTX_COMP_SEL, 1,
-	 "R200_SE_TCL_OUTPUT_VTX_COMP_SEL"},
-	{R200_PP_TAM_DEBUG3, 1, "R200_PP_TAM_DEBUG3"},
-	{R200_PP_CNTL_X, 1, "R200_PP_CNTL_X"},
-	{R200_RB3D_DEPTHXY_OFFSET, 1, "R200_RB3D_DEPTHXY_OFFSET"},
-	{R200_RE_AUX_SCISSOR_CNTL, 1, "R200_RE_AUX_SCISSOR_CNTL"},
-	{R200_RE_SCISSOR_TL_0, 2, "R200_RE_SCISSOR_TL_0"},
-	{R200_RE_SCISSOR_TL_1, 2, "R200_RE_SCISSOR_TL_1"},
-	{R200_RE_SCISSOR_TL_2, 2, "R200_RE_SCISSOR_TL_2"},
-	{R200_SE_VAP_CNTL_STATUS, 1, "R200_SE_VAP_CNTL_STATUS"},
-	{R200_SE_VTX_STATE_CNTL, 1, "R200_SE_VTX_STATE_CNTL"},
-	{R200_RE_POINTSIZE, 1, "R200_RE_POINTSIZE"},
-	{R200_SE_TCL_INPUT_VTX_VECTOR_ADDR_0, 4,
-		    "R200_SE_TCL_INPUT_VTX_VECTOR_ADDR_0"},
-	{R200_PP_CUBIC_FACES_0, 1, "R200_PP_CUBIC_FACES_0"},	/* 61 */
-	{R200_PP_CUBIC_OFFSET_F1_0, 5, "R200_PP_CUBIC_OFFSET_F1_0"}, /* 62 */
-	{R200_PP_CUBIC_FACES_1, 1, "R200_PP_CUBIC_FACES_1"},
-	{R200_PP_CUBIC_OFFSET_F1_1, 5, "R200_PP_CUBIC_OFFSET_F1_1"},
-	{R200_PP_CUBIC_FACES_2, 1, "R200_PP_CUBIC_FACES_2"},
-	{R200_PP_CUBIC_OFFSET_F1_2, 5, "R200_PP_CUBIC_OFFSET_F1_2"},
-	{R200_PP_CUBIC_FACES_3, 1, "R200_PP_CUBIC_FACES_3"},
-	{R200_PP_CUBIC_OFFSET_F1_3, 5, "R200_PP_CUBIC_OFFSET_F1_3"},
-	{R200_PP_CUBIC_FACES_4, 1, "R200_PP_CUBIC_FACES_4"},
-	{R200_PP_CUBIC_OFFSET_F1_4, 5, "R200_PP_CUBIC_OFFSET_F1_4"},
-	{R200_PP_CUBIC_FACES_5, 1, "R200_PP_CUBIC_FACES_5"},
-	{R200_PP_CUBIC_OFFSET_F1_5, 5, "R200_PP_CUBIC_OFFSET_F1_5"},
-	{RADEON_PP_TEX_SIZE_0, 2, "RADEON_PP_TEX_SIZE_0"},
-	{RADEON_PP_TEX_SIZE_1, 2, "RADEON_PP_TEX_SIZE_1"},
-	{RADEON_PP_TEX_SIZE_2, 2, "RADEON_PP_TEX_SIZE_2"},
-	{R200_RB3D_BLENDCOLOR, 3, "R200_RB3D_BLENDCOLOR"},
-	{R200_SE_TCL_POINT_SPRITE_CNTL, 1, "R200_SE_TCL_POINT_SPRITE_CNTL"},
-	{RADEON_PP_CUBIC_FACES_0, 1, "RADEON_PP_CUBIC_FACES_0"},
-	{RADEON_PP_CUBIC_OFFSET_T0_0, 5, "RADEON_PP_CUBIC_OFFSET_T0_0"},
-	{RADEON_PP_CUBIC_FACES_1, 1, "RADEON_PP_CUBIC_FACES_1"},
-	{RADEON_PP_CUBIC_OFFSET_T1_0, 5, "RADEON_PP_CUBIC_OFFSET_T1_0"},
-	{RADEON_PP_CUBIC_FACES_2, 1, "RADEON_PP_CUBIC_FACES_2"},
-	{RADEON_PP_CUBIC_OFFSET_T2_0, 5, "RADEON_PP_CUBIC_OFFSET_T2_0"},
-	{R200_PP_TRI_PERF, 2, "R200_PP_TRI_PERF"},
-	{R200_PP_AFS_0, 32, "R200_PP_AFS_0"},     /* 85 */
-	{R200_PP_AFS_1, 32, "R200_PP_AFS_1"},
-	{R200_PP_TFACTOR_0, 8, "R200_ATF_TFACTOR"},
-	{R200_PP_TXFILTER_0, 8, "R200_PP_TXCTLALL_0"},
-	{R200_PP_TXFILTER_1, 8, "R200_PP_TXCTLALL_1"},
-	{R200_PP_TXFILTER_2, 8, "R200_PP_TXCTLALL_2"},
-	{R200_PP_TXFILTER_3, 8, "R200_PP_TXCTLALL_3"},
-	{R200_PP_TXFILTER_4, 8, "R200_PP_TXCTLALL_4"},
-	{R200_PP_TXFILTER_5, 8, "R200_PP_TXCTLALL_5"},
-	{R200_VAP_PVS_CNTL_1, 2, "R200_VAP_PVS_CNTL"},
-};
-
-/* ================================================================
- * Performance monitoring functions
- */
-
-static void radeon_clear_box(drm_radeon_private_t * dev_priv,
-			     struct drm_radeon_master_private *master_priv,
-			     int x, int y, int w, int h, int r, int g, int b)
-{
-	u32 color;
-	RING_LOCALS;
-
-	x += master_priv->sarea_priv->boxes[0].x1;
-	y += master_priv->sarea_priv->boxes[0].y1;
-
-	switch (dev_priv->color_fmt) {
-	case RADEON_COLOR_FORMAT_RGB565:
-		color = (((r & 0xf8) << 8) |
-			 ((g & 0xfc) << 3) | ((b & 0xf8) >> 3));
-		break;
-	case RADEON_COLOR_FORMAT_ARGB8888:
-	default:
-		color = (((0xff) << 24) | (r << 16) | (g << 8) | b);
-		break;
-	}
-
-	BEGIN_RING(4);
-	RADEON_WAIT_UNTIL_3D_IDLE();
-	OUT_RING(CP_PACKET0(RADEON_DP_WRITE_MASK, 0));
-	OUT_RING(0xffffffff);
-	ADVANCE_RING();
-
-	BEGIN_RING(6);
-
-	OUT_RING(CP_PACKET3(RADEON_CNTL_PAINT_MULTI, 4));
-	OUT_RING(RADEON_GMC_DST_PITCH_OFFSET_CNTL |
-		 RADEON_GMC_BRUSH_SOLID_COLOR |
-		 (dev_priv->color_fmt << 8) |
-		 RADEON_GMC_SRC_DATATYPE_COLOR |
-		 RADEON_ROP3_P | RADEON_GMC_CLR_CMP_CNTL_DIS);
-
-	if (master_priv->sarea_priv->pfCurrentPage == 1) {
-		OUT_RING(dev_priv->front_pitch_offset);
-	} else {
-		OUT_RING(dev_priv->back_pitch_offset);
-	}
-
-	OUT_RING(color);
-
-	OUT_RING((x << 16) | y);
-	OUT_RING((w << 16) | h);
-
-	ADVANCE_RING();
-}
-
-static void radeon_cp_performance_boxes(drm_radeon_private_t *dev_priv, struct drm_radeon_master_private *master_priv)
-{
-	/* Collapse various things into a wait flag -- trying to
-	 * guess if userspase slept -- better just to have them tell us.
-	 */
-	if (dev_priv->stats.last_frame_reads > 1 ||
-	    dev_priv->stats.last_clear_reads > dev_priv->stats.clears) {
-		dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-	}
-
-	if (dev_priv->stats.freelist_loops) {
-		dev_priv->stats.boxes |= RADEON_BOX_WAIT_IDLE;
-	}
-
-	/* Purple box for page flipping
-	 */
-	if (dev_priv->stats.boxes & RADEON_BOX_FLIP)
-		radeon_clear_box(dev_priv, master_priv, 4, 4, 8, 8, 255, 0, 255);
-
-	/* Red box if we have to wait for idle at any point
-	 */
-	if (dev_priv->stats.boxes & RADEON_BOX_WAIT_IDLE)
-		radeon_clear_box(dev_priv, master_priv, 16, 4, 8, 8, 255, 0, 0);
-
-	/* Blue box: lost context?
-	 */
-
-	/* Yellow box for texture swaps
-	 */
-	if (dev_priv->stats.boxes & RADEON_BOX_TEXTURE_LOAD)
-		radeon_clear_box(dev_priv, master_priv, 40, 4, 8, 8, 255, 255, 0);
-
-	/* Green box if hardware never idles (as far as we can tell)
-	 */
-	if (!(dev_priv->stats.boxes & RADEON_BOX_DMA_IDLE))
-		radeon_clear_box(dev_priv, master_priv, 64, 4, 8, 8, 0, 255, 0);
-
-	/* Draw bars indicating number of buffers allocated
-	 * (not a great measure, easily confused)
-	 */
-	if (dev_priv->stats.requested_bufs) {
-		if (dev_priv->stats.requested_bufs > 100)
-			dev_priv->stats.requested_bufs = 100;
-
-		radeon_clear_box(dev_priv, master_priv, 4, 16,
-				 dev_priv->stats.requested_bufs, 4,
-				 196, 128, 128);
-	}
-
-	memset(&dev_priv->stats, 0, sizeof(dev_priv->stats));
-
-}
-
-/* ================================================================
- * CP command dispatch functions
- */
-
-static void radeon_cp_dispatch_clear(struct drm_device * dev,
-				     struct drm_master *master,
-				     drm_radeon_clear_t * clear,
-				     drm_radeon_clear_rect_t * depth_boxes)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	drm_radeon_depth_clear_t *depth_clear = &dev_priv->depth_clear;
-	int nbox = sarea_priv->nbox;
-	struct drm_clip_rect *pbox = sarea_priv->boxes;
-	unsigned int flags = clear->flags;
-	u32 rb3d_cntl = 0, rb3d_stencilrefmask = 0;
-	int i;
-	RING_LOCALS;
-	DRM_DEBUG("flags = 0x%x\n", flags);
-
-	dev_priv->stats.clears++;
-
-	if (sarea_priv->pfCurrentPage == 1) {
-		unsigned int tmp = flags;
-
-		flags &= ~(RADEON_FRONT | RADEON_BACK);
-		if (tmp & RADEON_FRONT)
-			flags |= RADEON_BACK;
-		if (tmp & RADEON_BACK)
-			flags |= RADEON_FRONT;
-	}
-	if (flags & (RADEON_DEPTH|RADEON_STENCIL)) {
-		if (!dev_priv->have_z_offset) {
-			printk_once(KERN_ERR "radeon: illegal depth clear request. Buggy mesa detected - please update.\n");
-			flags &= ~(RADEON_DEPTH | RADEON_STENCIL);
-		}
-	}
-
-	if (flags & (RADEON_FRONT | RADEON_BACK)) {
-
-		BEGIN_RING(4);
-
-		/* Ensure the 3D stream is idle before doing a
-		 * 2D fill to clear the front or back buffer.
-		 */
-		RADEON_WAIT_UNTIL_3D_IDLE();
-
-		OUT_RING(CP_PACKET0(RADEON_DP_WRITE_MASK, 0));
-		OUT_RING(clear->color_mask);
-
-		ADVANCE_RING();
-
-		/* Make sure we restore the 3D state next time.
-		 */
-		sarea_priv->ctx_owner = 0;
-
-		for (i = 0; i < nbox; i++) {
-			int x = pbox[i].x1;
-			int y = pbox[i].y1;
-			int w = pbox[i].x2 - x;
-			int h = pbox[i].y2 - y;
-
-			DRM_DEBUG("%d,%d-%d,%d flags 0x%x\n",
-				  x, y, w, h, flags);
-
-			if (flags & RADEON_FRONT) {
-				BEGIN_RING(6);
-
-				OUT_RING(CP_PACKET3
-					 (RADEON_CNTL_PAINT_MULTI, 4));
-				OUT_RING(RADEON_GMC_DST_PITCH_OFFSET_CNTL |
-					 RADEON_GMC_BRUSH_SOLID_COLOR |
-					 (dev_priv->
-					  color_fmt << 8) |
-					 RADEON_GMC_SRC_DATATYPE_COLOR |
-					 RADEON_ROP3_P |
-					 RADEON_GMC_CLR_CMP_CNTL_DIS);
-
-				OUT_RING(dev_priv->front_pitch_offset);
-				OUT_RING(clear->clear_color);
-
-				OUT_RING((x << 16) | y);
-				OUT_RING((w << 16) | h);
-
-				ADVANCE_RING();
-			}
-
-			if (flags & RADEON_BACK) {
-				BEGIN_RING(6);
-
-				OUT_RING(CP_PACKET3
-					 (RADEON_CNTL_PAINT_MULTI, 4));
-				OUT_RING(RADEON_GMC_DST_PITCH_OFFSET_CNTL |
-					 RADEON_GMC_BRUSH_SOLID_COLOR |
-					 (dev_priv->
-					  color_fmt << 8) |
-					 RADEON_GMC_SRC_DATATYPE_COLOR |
-					 RADEON_ROP3_P |
-					 RADEON_GMC_CLR_CMP_CNTL_DIS);
-
-				OUT_RING(dev_priv->back_pitch_offset);
-				OUT_RING(clear->clear_color);
-
-				OUT_RING((x << 16) | y);
-				OUT_RING((w << 16) | h);
-
-				ADVANCE_RING();
-			}
-		}
-	}
-
-	/* hyper z clear */
-	/* no docs available, based on reverse engineering by Stephane Marchesin */
-	if ((flags & (RADEON_DEPTH | RADEON_STENCIL))
-	    && (flags & RADEON_CLEAR_FASTZ)) {
-
-		int i;
-		int depthpixperline =
-		    dev_priv->depth_fmt ==
-		    RADEON_DEPTH_FORMAT_16BIT_INT_Z ? (dev_priv->depth_pitch /
-						       2) : (dev_priv->
-							     depth_pitch / 4);
-
-		u32 clearmask;
-
-		u32 tempRB3D_DEPTHCLEARVALUE = clear->clear_depth |
-		    ((clear->depth_mask & 0xff) << 24);
-
-		/* Make sure we restore the 3D state next time.
-		 * we haven't touched any "normal" state - still need this?
-		 */
-		sarea_priv->ctx_owner = 0;
-
-		if ((dev_priv->flags & RADEON_HAS_HIERZ)
-		    && (flags & RADEON_USE_HIERZ)) {
-			/* FIXME : reverse engineer that for Rx00 cards */
-			/* FIXME : the mask supposedly contains low-res z values. So can't set
-			   just to the max (0xff? or actually 0x3fff?), need to take z clear
-			   value into account? */
-			/* pattern seems to work for r100, though get slight
-			   rendering errors with glxgears. If hierz is not enabled for r100,
-			   only 4 bits which indicate clear (15,16,31,32, all zero) matter, the
-			   other ones are ignored, and the same clear mask can be used. That's
-			   very different behaviour than R200 which needs different clear mask
-			   and different number of tiles to clear if hierz is enabled or not !?!
-			 */
-			clearmask = (0xff << 22) | (0xff << 6) | 0x003f003f;
-		} else {
-			/* clear mask : chooses the clearing pattern.
-			   rv250: could be used to clear only parts of macrotiles
-			   (but that would get really complicated...)?
-			   bit 0 and 1 (either or both of them ?!?!) are used to
-			   not clear tile (or maybe one of the bits indicates if the tile is
-			   compressed or not), bit 2 and 3 to not clear tile 1,...,.
-			   Pattern is as follows:
-			   | 0,1 | 4,5 | 8,9 |12,13|16,17|20,21|24,25|28,29|
-			   bits -------------------------------------------------
-			   | 2,3 | 6,7 |10,11|14,15|18,19|22,23|26,27|30,31|
-			   rv100: clearmask covers 2x8 4x1 tiles, but one clear still
-			   covers 256 pixels ?!?
-			 */
-			clearmask = 0x0;
-		}
-
-		BEGIN_RING(8);
-		RADEON_WAIT_UNTIL_2D_IDLE();
-		OUT_RING_REG(RADEON_RB3D_DEPTHCLEARVALUE,
-			     tempRB3D_DEPTHCLEARVALUE);
-		/* what offset is this exactly ? */
-		OUT_RING_REG(RADEON_RB3D_ZMASKOFFSET, 0);
-		/* need ctlstat, otherwise get some strange black flickering */
-		OUT_RING_REG(RADEON_RB3D_ZCACHE_CTLSTAT,
-			     RADEON_RB3D_ZC_FLUSH_ALL);
-		ADVANCE_RING();
-
-		for (i = 0; i < nbox; i++) {
-			int tileoffset, nrtilesx, nrtilesy, j;
-			/* it looks like r200 needs rv-style clears, at least if hierz is not enabled? */
-			if ((dev_priv->flags & RADEON_HAS_HIERZ)
-			    && !(dev_priv->microcode_version == UCODE_R200)) {
-				/* FIXME : figure this out for r200 (when hierz is enabled). Or
-				   maybe r200 actually doesn't need to put the low-res z value into
-				   the tile cache like r100, but just needs to clear the hi-level z-buffer?
-				   Works for R100, both with hierz and without.
-				   R100 seems to operate on 2x1 8x8 tiles, but...
-				   odd: offset/nrtiles need to be 64 pix (4 block) aligned? Potentially
-				   problematic with resolutions which are not 64 pix aligned? */
-				tileoffset =
-				    ((pbox[i].y1 >> 3) * depthpixperline +
-				     pbox[i].x1) >> 6;
-				nrtilesx =
-				    ((pbox[i].x2 & ~63) -
-				     (pbox[i].x1 & ~63)) >> 4;
-				nrtilesy =
-				    (pbox[i].y2 >> 3) - (pbox[i].y1 >> 3);
-				for (j = 0; j <= nrtilesy; j++) {
-					BEGIN_RING(4);
-					OUT_RING(CP_PACKET3
-						 (RADEON_3D_CLEAR_ZMASK, 2));
-					/* first tile */
-					OUT_RING(tileoffset * 8);
-					/* the number of tiles to clear */
-					OUT_RING(nrtilesx + 4);
-					/* clear mask : chooses the clearing pattern. */
-					OUT_RING(clearmask);
-					ADVANCE_RING();
-					tileoffset += depthpixperline >> 6;
-				}
-			} else if (dev_priv->microcode_version == UCODE_R200) {
-				/* works for rv250. */
-				/* find first macro tile (8x2 4x4 z-pixels on rv250) */
-				tileoffset =
-				    ((pbox[i].y1 >> 3) * depthpixperline +
-				     pbox[i].x1) >> 5;
-				nrtilesx =
-				    (pbox[i].x2 >> 5) - (pbox[i].x1 >> 5);
-				nrtilesy =
-				    (pbox[i].y2 >> 3) - (pbox[i].y1 >> 3);
-				for (j = 0; j <= nrtilesy; j++) {
-					BEGIN_RING(4);
-					OUT_RING(CP_PACKET3
-						 (RADEON_3D_CLEAR_ZMASK, 2));
-					/* first tile */
-					/* judging by the first tile offset needed, could possibly
-					   directly address/clear 4x4 tiles instead of 8x2 * 4x4
-					   macro tiles, though would still need clear mask for
-					   right/bottom if truly 4x4 granularity is desired ? */
-					OUT_RING(tileoffset * 16);
-					/* the number of tiles to clear */
-					OUT_RING(nrtilesx + 1);
-					/* clear mask : chooses the clearing pattern. */
-					OUT_RING(clearmask);
-					ADVANCE_RING();
-					tileoffset += depthpixperline >> 5;
-				}
-			} else {	/* rv 100 */
-				/* rv100 might not need 64 pix alignment, who knows */
-				/* offsets are, hmm, weird */
-				tileoffset =
-				    ((pbox[i].y1 >> 4) * depthpixperline +
-				     pbox[i].x1) >> 6;
-				nrtilesx =
-				    ((pbox[i].x2 & ~63) -
-				     (pbox[i].x1 & ~63)) >> 4;
-				nrtilesy =
-				    (pbox[i].y2 >> 4) - (pbox[i].y1 >> 4);
-				for (j = 0; j <= nrtilesy; j++) {
-					BEGIN_RING(4);
-					OUT_RING(CP_PACKET3
-						 (RADEON_3D_CLEAR_ZMASK, 2));
-					OUT_RING(tileoffset * 128);
-					/* the number of tiles to clear */
-					OUT_RING(nrtilesx + 4);
-					/* clear mask : chooses the clearing pattern. */
-					OUT_RING(clearmask);
-					ADVANCE_RING();
-					tileoffset += depthpixperline >> 6;
-				}
-			}
-		}
-
-		/* TODO don't always clear all hi-level z tiles */
-		if ((dev_priv->flags & RADEON_HAS_HIERZ)
-		    && (dev_priv->microcode_version == UCODE_R200)
-		    && (flags & RADEON_USE_HIERZ))
-			/* r100 and cards without hierarchical z-buffer have no high-level z-buffer */
-			/* FIXME : the mask supposedly contains low-res z values. So can't set
-			   just to the max (0xff? or actually 0x3fff?), need to take z clear
-			   value into account? */
-		{
-			BEGIN_RING(4);
-			OUT_RING(CP_PACKET3(RADEON_3D_CLEAR_HIZ, 2));
-			OUT_RING(0x0);	/* First tile */
-			OUT_RING(0x3cc0);
-			OUT_RING((0xff << 22) | (0xff << 6) | 0x003f003f);
-			ADVANCE_RING();
-		}
-	}
-
-	/* We have to clear the depth and/or stencil buffers by
-	 * rendering a quad into just those buffers.  Thus, we have to
-	 * make sure the 3D engine is configured correctly.
-	 */
-	else if ((dev_priv->microcode_version == UCODE_R200) &&
-		(flags & (RADEON_DEPTH | RADEON_STENCIL))) {
-
-		int tempPP_CNTL;
-		int tempRE_CNTL;
-		int tempRB3D_CNTL;
-		int tempRB3D_ZSTENCILCNTL;
-		int tempRB3D_STENCILREFMASK;
-		int tempRB3D_PLANEMASK;
-		int tempSE_CNTL;
-		int tempSE_VTE_CNTL;
-		int tempSE_VTX_FMT_0;
-		int tempSE_VTX_FMT_1;
-		int tempSE_VAP_CNTL;
-		int tempRE_AUX_SCISSOR_CNTL;
-
-		tempPP_CNTL = 0;
-		tempRE_CNTL = 0;
-
-		tempRB3D_CNTL = depth_clear->rb3d_cntl;
-
-		tempRB3D_ZSTENCILCNTL = depth_clear->rb3d_zstencilcntl;
-		tempRB3D_STENCILREFMASK = 0x0;
-
-		tempSE_CNTL = depth_clear->se_cntl;
-
-		/* Disable TCL */
-
-		tempSE_VAP_CNTL = (	/* SE_VAP_CNTL__FORCE_W_TO_ONE_MASK |  */
-					  (0x9 <<
-					   SE_VAP_CNTL__VF_MAX_VTX_NUM__SHIFT));
-
-		tempRB3D_PLANEMASK = 0x0;
-
-		tempRE_AUX_SCISSOR_CNTL = 0x0;
-
-		tempSE_VTE_CNTL =
-		    SE_VTE_CNTL__VTX_XY_FMT_MASK | SE_VTE_CNTL__VTX_Z_FMT_MASK;
-
-		/* Vertex format (X, Y, Z, W) */
-		tempSE_VTX_FMT_0 =
-		    SE_VTX_FMT_0__VTX_Z0_PRESENT_MASK |
-		    SE_VTX_FMT_0__VTX_W0_PRESENT_MASK;
-		tempSE_VTX_FMT_1 = 0x0;
-
-		/*
-		 * Depth buffer specific enables
-		 */
-		if (flags & RADEON_DEPTH) {
-			/* Enable depth buffer */
-			tempRB3D_CNTL |= RADEON_Z_ENABLE;
-		} else {
-			/* Disable depth buffer */
-			tempRB3D_CNTL &= ~RADEON_Z_ENABLE;
-		}
-
-		/*
-		 * Stencil buffer specific enables
-		 */
-		if (flags & RADEON_STENCIL) {
-			tempRB3D_CNTL |= RADEON_STENCIL_ENABLE;
-			tempRB3D_STENCILREFMASK = clear->depth_mask;
-		} else {
-			tempRB3D_CNTL &= ~RADEON_STENCIL_ENABLE;
-			tempRB3D_STENCILREFMASK = 0x00000000;
-		}
-
-		if (flags & RADEON_USE_COMP_ZBUF) {
-			tempRB3D_ZSTENCILCNTL |= RADEON_Z_COMPRESSION_ENABLE |
-			    RADEON_Z_DECOMPRESSION_ENABLE;
-		}
-		if (flags & RADEON_USE_HIERZ) {
-			tempRB3D_ZSTENCILCNTL |= RADEON_Z_HIERARCHY_ENABLE;
-		}
-
-		BEGIN_RING(26);
-		RADEON_WAIT_UNTIL_2D_IDLE();
-
-		OUT_RING_REG(RADEON_PP_CNTL, tempPP_CNTL);
-		OUT_RING_REG(R200_RE_CNTL, tempRE_CNTL);
-		OUT_RING_REG(RADEON_RB3D_CNTL, tempRB3D_CNTL);
-		OUT_RING_REG(RADEON_RB3D_ZSTENCILCNTL, tempRB3D_ZSTENCILCNTL);
-		OUT_RING_REG(RADEON_RB3D_STENCILREFMASK,
-			     tempRB3D_STENCILREFMASK);
-		OUT_RING_REG(RADEON_RB3D_PLANEMASK, tempRB3D_PLANEMASK);
-		OUT_RING_REG(RADEON_SE_CNTL, tempSE_CNTL);
-		OUT_RING_REG(R200_SE_VTE_CNTL, tempSE_VTE_CNTL);
-		OUT_RING_REG(R200_SE_VTX_FMT_0, tempSE_VTX_FMT_0);
-		OUT_RING_REG(R200_SE_VTX_FMT_1, tempSE_VTX_FMT_1);
-		OUT_RING_REG(R200_SE_VAP_CNTL, tempSE_VAP_CNTL);
-		OUT_RING_REG(R200_RE_AUX_SCISSOR_CNTL, tempRE_AUX_SCISSOR_CNTL);
-		ADVANCE_RING();
-
-		/* Make sure we restore the 3D state next time.
-		 */
-		sarea_priv->ctx_owner = 0;
-
-		for (i = 0; i < nbox; i++) {
-
-			/* Funny that this should be required --
-			 *  sets top-left?
-			 */
-			radeon_emit_clip_rect(dev_priv, &sarea_priv->boxes[i]);
-
-			BEGIN_RING(14);
-			OUT_RING(CP_PACKET3(R200_3D_DRAW_IMMD_2, 12));
-			OUT_RING((RADEON_PRIM_TYPE_RECT_LIST |
-				  RADEON_PRIM_WALK_RING |
-				  (3 << RADEON_NUM_VERTICES_SHIFT)));
-			OUT_RING(depth_boxes[i].ui[CLEAR_X1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x3f800000);
-			OUT_RING(depth_boxes[i].ui[CLEAR_X1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x3f800000);
-			OUT_RING(depth_boxes[i].ui[CLEAR_X2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x3f800000);
-			ADVANCE_RING();
-		}
-	} else if ((flags & (RADEON_DEPTH | RADEON_STENCIL))) {
-
-		int tempRB3D_ZSTENCILCNTL = depth_clear->rb3d_zstencilcntl;
-
-		rb3d_cntl = depth_clear->rb3d_cntl;
-
-		if (flags & RADEON_DEPTH) {
-			rb3d_cntl |= RADEON_Z_ENABLE;
-		} else {
-			rb3d_cntl &= ~RADEON_Z_ENABLE;
-		}
-
-		if (flags & RADEON_STENCIL) {
-			rb3d_cntl |= RADEON_STENCIL_ENABLE;
-			rb3d_stencilrefmask = clear->depth_mask;	/* misnamed field */
-		} else {
-			rb3d_cntl &= ~RADEON_STENCIL_ENABLE;
-			rb3d_stencilrefmask = 0x00000000;
-		}
-
-		if (flags & RADEON_USE_COMP_ZBUF) {
-			tempRB3D_ZSTENCILCNTL |= RADEON_Z_COMPRESSION_ENABLE |
-			    RADEON_Z_DECOMPRESSION_ENABLE;
-		}
-		if (flags & RADEON_USE_HIERZ) {
-			tempRB3D_ZSTENCILCNTL |= RADEON_Z_HIERARCHY_ENABLE;
-		}
-
-		BEGIN_RING(13);
-		RADEON_WAIT_UNTIL_2D_IDLE();
-
-		OUT_RING(CP_PACKET0(RADEON_PP_CNTL, 1));
-		OUT_RING(0x00000000);
-		OUT_RING(rb3d_cntl);
-
-		OUT_RING_REG(RADEON_RB3D_ZSTENCILCNTL, tempRB3D_ZSTENCILCNTL);
-		OUT_RING_REG(RADEON_RB3D_STENCILREFMASK, rb3d_stencilrefmask);
-		OUT_RING_REG(RADEON_RB3D_PLANEMASK, 0x00000000);
-		OUT_RING_REG(RADEON_SE_CNTL, depth_clear->se_cntl);
-		ADVANCE_RING();
-
-		/* Make sure we restore the 3D state next time.
-		 */
-		sarea_priv->ctx_owner = 0;
-
-		for (i = 0; i < nbox; i++) {
-
-			/* Funny that this should be required --
-			 *  sets top-left?
-			 */
-			radeon_emit_clip_rect(dev_priv, &sarea_priv->boxes[i]);
-
-			BEGIN_RING(15);
-
-			OUT_RING(CP_PACKET3(RADEON_3D_DRAW_IMMD, 13));
-			OUT_RING(RADEON_VTX_Z_PRESENT |
-				 RADEON_VTX_PKCOLOR_PRESENT);
-			OUT_RING((RADEON_PRIM_TYPE_RECT_LIST |
-				  RADEON_PRIM_WALK_RING |
-				  RADEON_MAOS_ENABLE |
-				  RADEON_VTX_FMT_RADEON_MODE |
-				  (3 << RADEON_NUM_VERTICES_SHIFT)));
-
-			OUT_RING(depth_boxes[i].ui[CLEAR_X1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x0);
-
-			OUT_RING(depth_boxes[i].ui[CLEAR_X1]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x0);
-
-			OUT_RING(depth_boxes[i].ui[CLEAR_X2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_Y2]);
-			OUT_RING(depth_boxes[i].ui[CLEAR_DEPTH]);
-			OUT_RING(0x0);
-
-			ADVANCE_RING();
-		}
-	}
-
-	/* Increment the clear counter.  The client-side 3D driver must
-	 * wait on this value before performing the clear ioctl.  We
-	 * need this because the card's so damned fast...
-	 */
-	sarea_priv->last_clear++;
-
-	BEGIN_RING(4);
-
-	RADEON_CLEAR_AGE(sarea_priv->last_clear);
-	RADEON_WAIT_UNTIL_IDLE();
-
-	ADVANCE_RING();
-}
-
-static void radeon_cp_dispatch_swap(struct drm_device *dev, struct drm_master *master)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	int nbox = sarea_priv->nbox;
-	struct drm_clip_rect *pbox = sarea_priv->boxes;
-	int i;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	/* Do some trivial performance monitoring...
-	 */
-	if (dev_priv->do_boxes)
-		radeon_cp_performance_boxes(dev_priv, master_priv);
-
-	/* Wait for the 3D stream to idle before dispatching the bitblt.
-	 * This will prevent data corruption between the two streams.
-	 */
-	BEGIN_RING(2);
-
-	RADEON_WAIT_UNTIL_3D_IDLE();
-
-	ADVANCE_RING();
-
-	for (i = 0; i < nbox; i++) {
-		int x = pbox[i].x1;
-		int y = pbox[i].y1;
-		int w = pbox[i].x2 - x;
-		int h = pbox[i].y2 - y;
-
-		DRM_DEBUG("%d,%d-%d,%d\n", x, y, w, h);
-
-		BEGIN_RING(9);
-
-		OUT_RING(CP_PACKET0(RADEON_DP_GUI_MASTER_CNTL, 0));
-		OUT_RING(RADEON_GMC_SRC_PITCH_OFFSET_CNTL |
-			 RADEON_GMC_DST_PITCH_OFFSET_CNTL |
-			 RADEON_GMC_BRUSH_NONE |
-			 (dev_priv->color_fmt << 8) |
-			 RADEON_GMC_SRC_DATATYPE_COLOR |
-			 RADEON_ROP3_S |
-			 RADEON_DP_SRC_SOURCE_MEMORY |
-			 RADEON_GMC_CLR_CMP_CNTL_DIS | RADEON_GMC_WR_MSK_DIS);
-
-		/* Make this work even if front & back are flipped:
-		 */
-		OUT_RING(CP_PACKET0(RADEON_SRC_PITCH_OFFSET, 1));
-		if (sarea_priv->pfCurrentPage == 0) {
-			OUT_RING(dev_priv->back_pitch_offset);
-			OUT_RING(dev_priv->front_pitch_offset);
-		} else {
-			OUT_RING(dev_priv->front_pitch_offset);
-			OUT_RING(dev_priv->back_pitch_offset);
-		}
-
-		OUT_RING(CP_PACKET0(RADEON_SRC_X_Y, 2));
-		OUT_RING((x << 16) | y);
-		OUT_RING((x << 16) | y);
-		OUT_RING((w << 16) | h);
-
-		ADVANCE_RING();
-	}
-
-	/* Increment the frame counter.  The client-side 3D driver must
-	 * throttle the framerate by waiting for this value before
-	 * performing the swapbuffer ioctl.
-	 */
-	sarea_priv->last_frame++;
-
-	BEGIN_RING(4);
-
-	RADEON_FRAME_AGE(sarea_priv->last_frame);
-	RADEON_WAIT_UNTIL_2D_IDLE();
-
-	ADVANCE_RING();
-}
-
-void radeon_cp_dispatch_flip(struct drm_device *dev, struct drm_master *master)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	struct drm_sarea *sarea = (struct drm_sarea *)master_priv->sarea->handle;
-	int offset = (master_priv->sarea_priv->pfCurrentPage == 1)
-	    ? dev_priv->front_offset : dev_priv->back_offset;
-	RING_LOCALS;
-	DRM_DEBUG("pfCurrentPage=%d\n",
-		  master_priv->sarea_priv->pfCurrentPage);
-
-	/* Do some trivial performance monitoring...
-	 */
-	if (dev_priv->do_boxes) {
-		dev_priv->stats.boxes |= RADEON_BOX_FLIP;
-		radeon_cp_performance_boxes(dev_priv, master_priv);
-	}
-
-	/* Update the frame offsets for both CRTCs
-	 */
-	BEGIN_RING(6);
-
-	RADEON_WAIT_UNTIL_3D_IDLE();
-	OUT_RING_REG(RADEON_CRTC_OFFSET,
-		     ((sarea->frame.y * dev_priv->front_pitch +
-		       sarea->frame.x * (dev_priv->color_fmt - 2)) & ~7)
-		     + offset);
-	OUT_RING_REG(RADEON_CRTC2_OFFSET, master_priv->sarea_priv->crtc2_base
-		     + offset);
-
-	ADVANCE_RING();
-
-	/* Increment the frame counter.  The client-side 3D driver must
-	 * throttle the framerate by waiting for this value before
-	 * performing the swapbuffer ioctl.
-	 */
-	master_priv->sarea_priv->last_frame++;
-	master_priv->sarea_priv->pfCurrentPage =
-		1 - master_priv->sarea_priv->pfCurrentPage;
-
-	BEGIN_RING(2);
-
-	RADEON_FRAME_AGE(master_priv->sarea_priv->last_frame);
-
-	ADVANCE_RING();
-}
-
-static int bad_prim_vertex_nr(int primitive, int nr)
-{
-	switch (primitive & RADEON_PRIM_TYPE_MASK) {
-	case RADEON_PRIM_TYPE_NONE:
-	case RADEON_PRIM_TYPE_POINT:
-		return nr < 1;
-	case RADEON_PRIM_TYPE_LINE:
-		return (nr & 1) || nr == 0;
-	case RADEON_PRIM_TYPE_LINE_STRIP:
-		return nr < 2;
-	case RADEON_PRIM_TYPE_TRI_LIST:
-	case RADEON_PRIM_TYPE_3VRT_POINT_LIST:
-	case RADEON_PRIM_TYPE_3VRT_LINE_LIST:
-	case RADEON_PRIM_TYPE_RECT_LIST:
-		return nr % 3 || nr == 0;
-	case RADEON_PRIM_TYPE_TRI_FAN:
-	case RADEON_PRIM_TYPE_TRI_STRIP:
-		return nr < 3;
-	default:
-		return 1;
-	}
-}
-
-typedef struct {
-	unsigned int start;
-	unsigned int finish;
-	unsigned int prim;
-	unsigned int numverts;
-	unsigned int offset;
-	unsigned int vc_format;
-} drm_radeon_tcl_prim_t;
-
-static void radeon_cp_dispatch_vertex(struct drm_device * dev,
-				      struct drm_file *file_priv,
-				      struct drm_buf * buf,
-				      drm_radeon_tcl_prim_t * prim)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	int offset = dev_priv->gart_buffers_offset + buf->offset + prim->start;
-	int numverts = (int)prim->numverts;
-	int nbox = sarea_priv->nbox;
-	int i = 0;
-	RING_LOCALS;
-
-	DRM_DEBUG("hwprim 0x%x vfmt 0x%x %d..%d %d verts\n",
-		  prim->prim,
-		  prim->vc_format, prim->start, prim->finish, prim->numverts);
-
-	if (bad_prim_vertex_nr(prim->prim, prim->numverts)) {
-		DRM_ERROR("bad prim %x numverts %d\n",
-			  prim->prim, prim->numverts);
-		return;
-	}
-
-	do {
-		/* Emit the next cliprect */
-		if (i < nbox) {
-			radeon_emit_clip_rect(dev_priv, &sarea_priv->boxes[i]);
-		}
-
-		/* Emit the vertex buffer rendering commands */
-		BEGIN_RING(5);
-
-		OUT_RING(CP_PACKET3(RADEON_3D_RNDR_GEN_INDX_PRIM, 3));
-		OUT_RING(offset);
-		OUT_RING(numverts);
-		OUT_RING(prim->vc_format);
-		OUT_RING(prim->prim | RADEON_PRIM_WALK_LIST |
-			 RADEON_COLOR_ORDER_RGBA |
-			 RADEON_VTX_FMT_RADEON_MODE |
-			 (numverts << RADEON_NUM_VERTICES_SHIFT));
-
-		ADVANCE_RING();
-
-		i++;
-	} while (i < nbox);
-}
-
-void radeon_cp_discard_buffer(struct drm_device *dev, struct drm_master *master, struct drm_buf *buf)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	drm_radeon_buf_priv_t *buf_priv = buf->dev_private;
-	RING_LOCALS;
-
-	buf_priv->age = ++master_priv->sarea_priv->last_dispatch;
-
-	/* Emit the vertex buffer age */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600) {
-		BEGIN_RING(3);
-		R600_DISPATCH_AGE(buf_priv->age);
-		ADVANCE_RING();
-	} else {
-		BEGIN_RING(2);
-		RADEON_DISPATCH_AGE(buf_priv->age);
-		ADVANCE_RING();
-	}
-
-	buf->pending = 1;
-	buf->used = 0;
-}
-
-static void radeon_cp_dispatch_indirect(struct drm_device * dev,
-					struct drm_buf * buf, int start, int end)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-	DRM_DEBUG("buf=%d s=0x%x e=0x%x\n", buf->idx, start, end);
-
-	if (start != end) {
-		int offset = (dev_priv->gart_buffers_offset
-			      + buf->offset + start);
-		int dwords = (end - start + 3) / sizeof(u32);
-
-		/* Indirect buffer data must be an even number of
-		 * dwords, so if we've been given an odd number we must
-		 * pad the data with a Type-2 CP packet.
-		 */
-		if (dwords & 1) {
-			u32 *data = (u32 *)
-			    ((char *)dev->agp_buffer_map->handle
-			     + buf->offset + start);
-			data[dwords++] = RADEON_CP_PACKET2;
-		}
-
-		/* Fire off the indirect buffer */
-		BEGIN_RING(3);
-
-		OUT_RING(CP_PACKET0(RADEON_CP_IB_BASE, 1));
-		OUT_RING(offset);
-		OUT_RING(dwords);
-
-		ADVANCE_RING();
-	}
-}
-
-static void radeon_cp_dispatch_indices(struct drm_device *dev,
-				       struct drm_master *master,
-				       struct drm_buf * elt_buf,
-				       drm_radeon_tcl_prim_t * prim)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	int offset = dev_priv->gart_buffers_offset + prim->offset;
-	u32 *data;
-	int dwords;
-	int i = 0;
-	int start = prim->start + RADEON_INDEX_PRIM_OFFSET;
-	int count = (prim->finish - start) / sizeof(u16);
-	int nbox = sarea_priv->nbox;
-
-	DRM_DEBUG("hwprim 0x%x vfmt 0x%x %d..%d offset: %x nr %d\n",
-		  prim->prim,
-		  prim->vc_format,
-		  prim->start, prim->finish, prim->offset, prim->numverts);
-
-	if (bad_prim_vertex_nr(prim->prim, count)) {
-		DRM_ERROR("bad prim %x count %d\n", prim->prim, count);
-		return;
-	}
-
-	if (start >= prim->finish || (prim->start & 0x7)) {
-		DRM_ERROR("buffer prim %d\n", prim->prim);
-		return;
-	}
-
-	dwords = (prim->finish - prim->start + 3) / sizeof(u32);
-
-	data = (u32 *) ((char *)dev->agp_buffer_map->handle +
-			elt_buf->offset + prim->start);
-
-	data[0] = CP_PACKET3(RADEON_3D_RNDR_GEN_INDX_PRIM, dwords - 2);
-	data[1] = offset;
-	data[2] = prim->numverts;
-	data[3] = prim->vc_format;
-	data[4] = (prim->prim |
-		   RADEON_PRIM_WALK_IND |
-		   RADEON_COLOR_ORDER_RGBA |
-		   RADEON_VTX_FMT_RADEON_MODE |
-		   (count << RADEON_NUM_VERTICES_SHIFT));
-
-	do {
-		if (i < nbox)
-			radeon_emit_clip_rect(dev_priv, &sarea_priv->boxes[i]);
-
-		radeon_cp_dispatch_indirect(dev, elt_buf,
-					    prim->start, prim->finish);
-
-		i++;
-	} while (i < nbox);
-
-}
-
-#define RADEON_MAX_TEXTURE_SIZE RADEON_BUFFER_SIZE
-
-static int radeon_cp_dispatch_texture(struct drm_device * dev,
-				      struct drm_file *file_priv,
-				      drm_radeon_texture_t * tex,
-				      drm_radeon_tex_image_t * image)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_buf *buf;
-	u32 format;
-	u32 *buffer;
-	const u8 __user *data;
-	unsigned int size, dwords, tex_width, blit_width, spitch;
-	u32 height;
-	int i;
-	u32 texpitch, microtile;
-	u32 offset, byte_offset;
-	RING_LOCALS;
-
-	if (radeon_check_and_fixup_offset(dev_priv, file_priv, &tex->offset)) {
-		DRM_ERROR("Invalid destination offset\n");
-		return -EINVAL;
-	}
-
-	dev_priv->stats.boxes |= RADEON_BOX_TEXTURE_LOAD;
-
-	/* Flush the pixel cache.  This ensures no pixel data gets mixed
-	 * up with the texture data from the host data blit, otherwise
-	 * part of the texture image may be corrupted.
-	 */
-	BEGIN_RING(4);
-	RADEON_FLUSH_CACHE();
-	RADEON_WAIT_UNTIL_IDLE();
-	ADVANCE_RING();
-
-	/* The compiler won't optimize away a division by a variable,
-	 * even if the only legal values are powers of two.  Thus, we'll
-	 * use a shift instead.
-	 */
-	switch (tex->format) {
-	case RADEON_TXFORMAT_ARGB8888:
-	case RADEON_TXFORMAT_RGBA8888:
-		format = RADEON_COLOR_FORMAT_ARGB8888;
-		tex_width = tex->width * 4;
-		blit_width = image->width * 4;
-		break;
-	case RADEON_TXFORMAT_AI88:
-	case RADEON_TXFORMAT_ARGB1555:
-	case RADEON_TXFORMAT_RGB565:
-	case RADEON_TXFORMAT_ARGB4444:
-	case RADEON_TXFORMAT_VYUY422:
-	case RADEON_TXFORMAT_YVYU422:
-		format = RADEON_COLOR_FORMAT_RGB565;
-		tex_width = tex->width * 2;
-		blit_width = image->width * 2;
-		break;
-	case RADEON_TXFORMAT_I8:
-	case RADEON_TXFORMAT_RGB332:
-		format = RADEON_COLOR_FORMAT_CI8;
-		tex_width = tex->width * 1;
-		blit_width = image->width * 1;
-		break;
-	default:
-		DRM_ERROR("invalid texture format %d\n", tex->format);
-		return -EINVAL;
-	}
-	spitch = blit_width >> 6;
-	if (spitch == 0 && image->height > 1)
-		return -EINVAL;
-
-	texpitch = tex->pitch;
-	if ((texpitch << 22) & RADEON_DST_TILE_MICRO) {
-		microtile = 1;
-		if (tex_width < 64) {
-			texpitch &= ~(RADEON_DST_TILE_MICRO >> 22);
-			/* we got tiled coordinates, untile them */
-			image->x *= 2;
-		}
-	} else
-		microtile = 0;
-
-	/* this might fail for zero-sized uploads - are those illegal? */
-	if (!radeon_check_offset(dev_priv, tex->offset + image->height *
-				blit_width - 1)) {
-		DRM_ERROR("Invalid final destination offset\n");
-		return -EINVAL;
-	}
-
-	DRM_DEBUG("tex=%dx%d blit=%d\n", tex_width, tex->height, blit_width);
-
-	do {
-		DRM_DEBUG("tex: ofs=0x%x p=%d f=%d x=%hd y=%hd w=%hd h=%hd\n",
-			  tex->offset >> 10, tex->pitch, tex->format,
-			  image->x, image->y, image->width, image->height);
-
-		/* Make a copy of some parameters in case we have to
-		 * update them for a multi-pass texture blit.
-		 */
-		height = image->height;
-		data = (const u8 __user *)image->data;
-
-		size = height * blit_width;
-
-		if (size > RADEON_MAX_TEXTURE_SIZE) {
-			height = RADEON_MAX_TEXTURE_SIZE / blit_width;
-			size = height * blit_width;
-		} else if (size < 4 && size > 0) {
-			size = 4;
-		} else if (size == 0) {
-			return 0;
-		}
-
-		buf = radeon_freelist_get(dev);
-		if (0 && !buf) {
-			radeon_do_cp_idle(dev_priv);
-			buf = radeon_freelist_get(dev);
-		}
-		if (!buf) {
-			DRM_DEBUG("EAGAIN\n");
-			if (copy_to_user(tex->image, image, sizeof(*image)))
-				return -EFAULT;
-			return -EAGAIN;
-		}
-
-		/* Dispatch the indirect buffer.
-		 */
-		buffer =
-		    (u32 *) ((char *)dev->agp_buffer_map->handle + buf->offset);
-		dwords = size / 4;
-
-#define RADEON_COPY_MT(_buf, _data, _width) \
-	do { \
-		if (copy_from_user(_buf, _data, (_width))) {\
-			DRM_ERROR("EFAULT on pad, %d bytes\n", (_width)); \
-			return -EFAULT; \
-		} \
-	} while(0)
-
-		if (microtile) {
-			/* texture micro tiling in use, minimum texture width is thus 16 bytes.
-			   however, we cannot use blitter directly for texture width < 64 bytes,
-			   since minimum tex pitch is 64 bytes and we need this to match
-			   the texture width, otherwise the blitter will tile it wrong.
-			   Thus, tiling manually in this case. Additionally, need to special
-			   case tex height = 1, since our actual image will have height 2
-			   and we need to ensure we don't read beyond the texture size
-			   from user space. */
-			if (tex->height == 1) {
-				if (tex_width >= 64 || tex_width <= 16) {
-					RADEON_COPY_MT(buffer, data,
-						(int)(tex_width * sizeof(u32)));
-				} else if (tex_width == 32) {
-					RADEON_COPY_MT(buffer, data, 16);
-					RADEON_COPY_MT(buffer + 8,
-						       data + 16, 16);
-				}
-			} else if (tex_width >= 64 || tex_width == 16) {
-				RADEON_COPY_MT(buffer, data,
-					       (int)(dwords * sizeof(u32)));
-			} else if (tex_width < 16) {
-				for (i = 0; i < tex->height; i++) {
-					RADEON_COPY_MT(buffer, data, tex_width);
-					buffer += 4;
-					data += tex_width;
-				}
-			} else if (tex_width == 32) {
-				/* TODO: make sure this works when not fitting in one buffer
-				   (i.e. 32bytes x 2048...) */
-				for (i = 0; i < tex->height; i += 2) {
-					RADEON_COPY_MT(buffer, data, 16);
-					data += 16;
-					RADEON_COPY_MT(buffer + 8, data, 16);
-					data += 16;
-					RADEON_COPY_MT(buffer + 4, data, 16);
-					data += 16;
-					RADEON_COPY_MT(buffer + 12, data, 16);
-					data += 16;
-					buffer += 16;
-				}
-			}
-		} else {
-			if (tex_width >= 32) {
-				/* Texture image width is larger than the minimum, so we
-				 * can upload it directly.
-				 */
-				RADEON_COPY_MT(buffer, data,
-					       (int)(dwords * sizeof(u32)));
-			} else {
-				/* Texture image width is less than the minimum, so we
-				 * need to pad out each image scanline to the minimum
-				 * width.
-				 */
-				for (i = 0; i < tex->height; i++) {
-					RADEON_COPY_MT(buffer, data, tex_width);
-					buffer += 8;
-					data += tex_width;
-				}
-			}
-		}
-
-#undef RADEON_COPY_MT
-		byte_offset = (image->y & ~2047) * blit_width;
-		buf->file_priv = file_priv;
-		buf->used = size;
-		offset = dev_priv->gart_buffers_offset + buf->offset;
-		BEGIN_RING(9);
-		OUT_RING(CP_PACKET3(RADEON_CNTL_BITBLT_MULTI, 5));
-		OUT_RING(RADEON_GMC_SRC_PITCH_OFFSET_CNTL |
-			 RADEON_GMC_DST_PITCH_OFFSET_CNTL |
-			 RADEON_GMC_BRUSH_NONE |
-			 (format << 8) |
-			 RADEON_GMC_SRC_DATATYPE_COLOR |
-			 RADEON_ROP3_S |
-			 RADEON_DP_SRC_SOURCE_MEMORY |
-			 RADEON_GMC_CLR_CMP_CNTL_DIS | RADEON_GMC_WR_MSK_DIS);
-		OUT_RING((spitch << 22) | (offset >> 10));
-		OUT_RING((texpitch << 22) | ((tex->offset >> 10) + (byte_offset >> 10)));
-		OUT_RING(0);
-		OUT_RING((image->x << 16) | (image->y % 2048));
-		OUT_RING((image->width << 16) | height);
-		RADEON_WAIT_UNTIL_2D_IDLE();
-		ADVANCE_RING();
-		COMMIT_RING();
-
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-
-		/* Update the input parameters for next time */
-		image->y += height;
-		image->height -= height;
-		image->data = (const u8 __user *)image->data + size;
-	} while (image->height > 0);
-
-	/* Flush the pixel cache after the blit completes.  This ensures
-	 * the texture data is written out to memory before rendering
-	 * continues.
-	 */
-	BEGIN_RING(4);
-	RADEON_FLUSH_CACHE();
-	RADEON_WAIT_UNTIL_2D_IDLE();
-	ADVANCE_RING();
-	COMMIT_RING();
-
-	return 0;
-}
-
-static void radeon_cp_dispatch_stipple(struct drm_device * dev, u32 * stipple)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	int i;
-	RING_LOCALS;
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(35);
-
-	OUT_RING(CP_PACKET0(RADEON_RE_STIPPLE_ADDR, 0));
-	OUT_RING(0x00000000);
-
-	OUT_RING(CP_PACKET0_TABLE(RADEON_RE_STIPPLE_DATA, 31));
-	for (i = 0; i < 32; i++) {
-		OUT_RING(stipple[i]);
-	}
-
-	ADVANCE_RING();
-}
-
-static void radeon_apply_surface_regs(int surf_index,
-				      drm_radeon_private_t *dev_priv)
-{
-	if (!dev_priv->mmio)
-		return;
-
-	radeon_do_cp_idle(dev_priv);
-
-	RADEON_WRITE(RADEON_SURFACE0_INFO + 16 * surf_index,
-		     dev_priv->surfaces[surf_index].flags);
-	RADEON_WRITE(RADEON_SURFACE0_LOWER_BOUND + 16 * surf_index,
-		     dev_priv->surfaces[surf_index].lower);
-	RADEON_WRITE(RADEON_SURFACE0_UPPER_BOUND + 16 * surf_index,
-		     dev_priv->surfaces[surf_index].upper);
-}
-
-/* Allocates a virtual surface
- * doesn't always allocate a real surface, will stretch an existing
- * surface when possible.
- *
- * Note that refcount can be at most 2, since during a free refcount=3
- * might mean we have to allocate a new surface which might not always
- * be available.
- * For example : we allocate three contiguous surfaces ABC. If B is
- * freed, we suddenly need two surfaces to store A and C, which might
- * not always be available.
- */
-static int alloc_surface(drm_radeon_surface_alloc_t *new,
-			 drm_radeon_private_t *dev_priv,
-			 struct drm_file *file_priv)
-{
-	struct radeon_virt_surface *s;
-	int i;
-	int virt_surface_index;
-	uint32_t new_upper, new_lower;
-
-	new_lower = new->address;
-	new_upper = new_lower + new->size - 1;
-
-	/* sanity check */
-	if ((new_lower >= new_upper) || (new->flags == 0) || (new->size == 0) ||
-	    ((new_upper & RADEON_SURF_ADDRESS_FIXED_MASK) !=
-	     RADEON_SURF_ADDRESS_FIXED_MASK)
-	    || ((new_lower & RADEON_SURF_ADDRESS_FIXED_MASK) != 0))
-		return -1;
-
-	/* make sure there is no overlap with existing surfaces */
-	for (i = 0; i < RADEON_MAX_SURFACES; i++) {
-		if ((dev_priv->surfaces[i].refcount != 0) &&
-		    (((new_lower >= dev_priv->surfaces[i].lower) &&
-		      (new_lower < dev_priv->surfaces[i].upper)) ||
-		     ((new_lower < dev_priv->surfaces[i].lower) &&
-		      (new_upper > dev_priv->surfaces[i].lower)))) {
-			return -1;
-		}
-	}
-
-	/* find a virtual surface */
-	for (i = 0; i < 2 * RADEON_MAX_SURFACES; i++)
-		if (dev_priv->virt_surfaces[i].file_priv == NULL)
-			break;
-	if (i == 2 * RADEON_MAX_SURFACES) {
-		return -1;
-	}
-	virt_surface_index = i;
-
-	/* try to reuse an existing surface */
-	for (i = 0; i < RADEON_MAX_SURFACES; i++) {
-		/* extend before */
-		if ((dev_priv->surfaces[i].refcount == 1) &&
-		    (new->flags == dev_priv->surfaces[i].flags) &&
-		    (new_upper + 1 == dev_priv->surfaces[i].lower)) {
-			s = &(dev_priv->virt_surfaces[virt_surface_index]);
-			s->surface_index = i;
-			s->lower = new_lower;
-			s->upper = new_upper;
-			s->flags = new->flags;
-			s->file_priv = file_priv;
-			dev_priv->surfaces[i].refcount++;
-			dev_priv->surfaces[i].lower = s->lower;
-			radeon_apply_surface_regs(s->surface_index, dev_priv);
-			return virt_surface_index;
-		}
-
-		/* extend after */
-		if ((dev_priv->surfaces[i].refcount == 1) &&
-		    (new->flags == dev_priv->surfaces[i].flags) &&
-		    (new_lower == dev_priv->surfaces[i].upper + 1)) {
-			s = &(dev_priv->virt_surfaces[virt_surface_index]);
-			s->surface_index = i;
-			s->lower = new_lower;
-			s->upper = new_upper;
-			s->flags = new->flags;
-			s->file_priv = file_priv;
-			dev_priv->surfaces[i].refcount++;
-			dev_priv->surfaces[i].upper = s->upper;
-			radeon_apply_surface_regs(s->surface_index, dev_priv);
-			return virt_surface_index;
-		}
-	}
-
-	/* okay, we need a new one */
-	for (i = 0; i < RADEON_MAX_SURFACES; i++) {
-		if (dev_priv->surfaces[i].refcount == 0) {
-			s = &(dev_priv->virt_surfaces[virt_surface_index]);
-			s->surface_index = i;
-			s->lower = new_lower;
-			s->upper = new_upper;
-			s->flags = new->flags;
-			s->file_priv = file_priv;
-			dev_priv->surfaces[i].refcount = 1;
-			dev_priv->surfaces[i].lower = s->lower;
-			dev_priv->surfaces[i].upper = s->upper;
-			dev_priv->surfaces[i].flags = s->flags;
-			radeon_apply_surface_regs(s->surface_index, dev_priv);
-			return virt_surface_index;
-		}
-	}
-
-	/* we didn't find anything */
-	return -1;
-}
-
-static int free_surface(struct drm_file *file_priv,
-			drm_radeon_private_t * dev_priv,
-			int lower)
-{
-	struct radeon_virt_surface *s;
-	int i;
-	/* find the virtual surface */
-	for (i = 0; i < 2 * RADEON_MAX_SURFACES; i++) {
-		s = &(dev_priv->virt_surfaces[i]);
-		if (s->file_priv) {
-			if ((lower == s->lower) && (file_priv == s->file_priv))
-			{
-				if (dev_priv->surfaces[s->surface_index].
-				    lower == s->lower)
-					dev_priv->surfaces[s->surface_index].
-					    lower = s->upper;
-
-				if (dev_priv->surfaces[s->surface_index].
-				    upper == s->upper)
-					dev_priv->surfaces[s->surface_index].
-					    upper = s->lower;
-
-				dev_priv->surfaces[s->surface_index].refcount--;
-				if (dev_priv->surfaces[s->surface_index].
-				    refcount == 0)
-					dev_priv->surfaces[s->surface_index].
-					    flags = 0;
-				s->file_priv = NULL;
-				radeon_apply_surface_regs(s->surface_index,
-							  dev_priv);
-				return 0;
-			}
-		}
-	}
-	return 1;
-}
-
-static void radeon_surfaces_release(struct drm_file *file_priv,
-				    drm_radeon_private_t * dev_priv)
-{
-	int i;
-	for (i = 0; i < 2 * RADEON_MAX_SURFACES; i++) {
-		if (dev_priv->virt_surfaces[i].file_priv == file_priv)
-			free_surface(file_priv, dev_priv,
-				     dev_priv->virt_surfaces[i].lower);
-	}
-}
-
-/* ================================================================
- * IOCTL functions
- */
-static int radeon_surface_alloc(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_surface_alloc_t *alloc = data;
-
-	if (alloc_surface(alloc, dev_priv, file_priv) == -1)
-		return -EINVAL;
-	else
-		return 0;
-}
-
-static int radeon_surface_free(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_surface_free_t *memfree = data;
-
-	if (free_surface(file_priv, dev_priv, memfree->address))
-		return -EINVAL;
-	else
-		return 0;
-}
-
-static int radeon_cp_clear(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-	drm_radeon_clear_t *clear = data;
-	drm_radeon_clear_rect_t depth_boxes[RADEON_NR_SAREA_CLIPRECTS];
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-
-	if (sarea_priv->nbox > RADEON_NR_SAREA_CLIPRECTS)
-		sarea_priv->nbox = RADEON_NR_SAREA_CLIPRECTS;
-
-	if (copy_from_user(&depth_boxes, clear->depth_boxes,
-			       sarea_priv->nbox * sizeof(depth_boxes[0])))
-		return -EFAULT;
-
-	radeon_cp_dispatch_clear(dev, file_priv->master, clear, depth_boxes);
-
-	COMMIT_RING();
-	return 0;
-}
-
-/* Not sure why this isn't set all the time:
- */
-static int radeon_do_init_pageflip(struct drm_device *dev, struct drm_master *master)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = master->driver_priv;
-	RING_LOCALS;
-
-	DRM_DEBUG("\n");
-
-	BEGIN_RING(6);
-	RADEON_WAIT_UNTIL_3D_IDLE();
-	OUT_RING(CP_PACKET0(RADEON_CRTC_OFFSET_CNTL, 0));
-	OUT_RING(RADEON_READ(RADEON_CRTC_OFFSET_CNTL) |
-		 RADEON_CRTC_OFFSET_FLIP_CNTL);
-	OUT_RING(CP_PACKET0(RADEON_CRTC2_OFFSET_CNTL, 0));
-	OUT_RING(RADEON_READ(RADEON_CRTC2_OFFSET_CNTL) |
-		 RADEON_CRTC_OFFSET_FLIP_CNTL);
-	ADVANCE_RING();
-
-	dev_priv->page_flipping = 1;
-
-	if (master_priv->sarea_priv->pfCurrentPage != 1)
-		master_priv->sarea_priv->pfCurrentPage = 0;
-
-	return 0;
-}
-
-/* Swapping and flipping are different operations, need different ioctls.
- * They can & should be intermixed to support multiple 3d windows.
- */
-static int radeon_cp_flip(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-
-	if (!dev_priv->page_flipping)
-		radeon_do_init_pageflip(dev, file_priv->master);
-
-	radeon_cp_dispatch_flip(dev, file_priv->master);
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_swap(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv = master_priv->sarea_priv;
-
-	DRM_DEBUG("\n");
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-
-	if (sarea_priv->nbox > RADEON_NR_SAREA_CLIPRECTS)
-		sarea_priv->nbox = RADEON_NR_SAREA_CLIPRECTS;
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_cp_dispatch_swap(dev, file_priv);
-	else
-		radeon_cp_dispatch_swap(dev, file_priv->master);
-	sarea_priv->ctx_owner = 0;
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_vertex(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf;
-	drm_radeon_vertex_t *vertex = data;
-	drm_radeon_tcl_prim_t prim;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	sarea_priv = master_priv->sarea_priv;
-
-	DRM_DEBUG("pid=%d index=%d count=%d discard=%d\n",
-		  DRM_CURRENTPID, vertex->idx, vertex->count, vertex->discard);
-
-	if (vertex->idx < 0 || vertex->idx >= dma->buf_count) {
-		DRM_ERROR("buffer index %d (of %d max)\n",
-			  vertex->idx, dma->buf_count - 1);
-		return -EINVAL;
-	}
-	if (vertex->prim < 0 || vertex->prim > RADEON_PRIM_TYPE_3VRT_LINE_LIST) {
-		DRM_ERROR("buffer prim %d\n", vertex->prim);
-		return -EINVAL;
-	}
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	buf = dma->buflist[vertex->idx];
-
-	if (buf->file_priv != file_priv) {
-		DRM_ERROR("process %d using buffer owned by %p\n",
-			  DRM_CURRENTPID, buf->file_priv);
-		return -EINVAL;
-	}
-	if (buf->pending) {
-		DRM_ERROR("sending pending buffer %d\n", vertex->idx);
-		return -EINVAL;
-	}
-
-	/* Build up a prim_t record:
-	 */
-	if (vertex->count) {
-		buf->used = vertex->count;	/* not used? */
-
-		if (sarea_priv->dirty & ~RADEON_UPLOAD_CLIPRECTS) {
-			if (radeon_emit_state(dev_priv, file_priv,
-					      &sarea_priv->context_state,
-					      sarea_priv->tex_state,
-					      sarea_priv->dirty)) {
-				DRM_ERROR("radeon_emit_state failed\n");
-				return -EINVAL;
-			}
-
-			sarea_priv->dirty &= ~(RADEON_UPLOAD_TEX0IMAGES |
-					       RADEON_UPLOAD_TEX1IMAGES |
-					       RADEON_UPLOAD_TEX2IMAGES |
-					       RADEON_REQUIRE_QUIESCENCE);
-		}
-
-		prim.start = 0;
-		prim.finish = vertex->count;	/* unused */
-		prim.prim = vertex->prim;
-		prim.numverts = vertex->count;
-		prim.vc_format = sarea_priv->vc_format;
-
-		radeon_cp_dispatch_vertex(dev, file_priv, buf, &prim);
-	}
-
-	if (vertex->discard) {
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-	}
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_indices(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf;
-	drm_radeon_indices_t *elts = data;
-	drm_radeon_tcl_prim_t prim;
-	int count;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	sarea_priv = master_priv->sarea_priv;
-
-	DRM_DEBUG("pid=%d index=%d start=%d end=%d discard=%d\n",
-		  DRM_CURRENTPID, elts->idx, elts->start, elts->end,
-		  elts->discard);
-
-	if (elts->idx < 0 || elts->idx >= dma->buf_count) {
-		DRM_ERROR("buffer index %d (of %d max)\n",
-			  elts->idx, dma->buf_count - 1);
-		return -EINVAL;
-	}
-	if (elts->prim < 0 || elts->prim > RADEON_PRIM_TYPE_3VRT_LINE_LIST) {
-		DRM_ERROR("buffer prim %d\n", elts->prim);
-		return -EINVAL;
-	}
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	buf = dma->buflist[elts->idx];
-
-	if (buf->file_priv != file_priv) {
-		DRM_ERROR("process %d using buffer owned by %p\n",
-			  DRM_CURRENTPID, buf->file_priv);
-		return -EINVAL;
-	}
-	if (buf->pending) {
-		DRM_ERROR("sending pending buffer %d\n", elts->idx);
-		return -EINVAL;
-	}
-
-	count = (elts->end - elts->start) / sizeof(u16);
-	elts->start -= RADEON_INDEX_PRIM_OFFSET;
-
-	if (elts->start & 0x7) {
-		DRM_ERROR("misaligned buffer 0x%x\n", elts->start);
-		return -EINVAL;
-	}
-	if (elts->start < buf->used) {
-		DRM_ERROR("no header 0x%x - 0x%x\n", elts->start, buf->used);
-		return -EINVAL;
-	}
-
-	buf->used = elts->end;
-
-	if (sarea_priv->dirty & ~RADEON_UPLOAD_CLIPRECTS) {
-		if (radeon_emit_state(dev_priv, file_priv,
-				      &sarea_priv->context_state,
-				      sarea_priv->tex_state,
-				      sarea_priv->dirty)) {
-			DRM_ERROR("radeon_emit_state failed\n");
-			return -EINVAL;
-		}
-
-		sarea_priv->dirty &= ~(RADEON_UPLOAD_TEX0IMAGES |
-				       RADEON_UPLOAD_TEX1IMAGES |
-				       RADEON_UPLOAD_TEX2IMAGES |
-				       RADEON_REQUIRE_QUIESCENCE);
-	}
-
-	/* Build up a prim_t record:
-	 */
-	prim.start = elts->start;
-	prim.finish = elts->end;
-	prim.prim = elts->prim;
-	prim.offset = 0;	/* offset from start of dma buffers */
-	prim.numverts = RADEON_MAX_VB_VERTS;	/* duh */
-	prim.vc_format = sarea_priv->vc_format;
-
-	radeon_cp_dispatch_indices(dev, file_priv->master, buf, &prim);
-	if (elts->discard) {
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-	}
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_texture(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_texture_t *tex = data;
-	drm_radeon_tex_image_t image;
-	int ret;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (tex->image == NULL) {
-		DRM_ERROR("null texture image!\n");
-		return -EINVAL;
-	}
-
-	if (copy_from_user(&image,
-			       (drm_radeon_tex_image_t __user *) tex->image,
-			       sizeof(image)))
-		return -EFAULT;
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		ret = r600_cp_dispatch_texture(dev, file_priv, tex, &image);
-	else
-		ret = radeon_cp_dispatch_texture(dev, file_priv, tex, &image);
-
-	return ret;
-}
-
-static int radeon_cp_stipple(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_stipple_t *stipple = data;
-	u32 mask[32];
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	if (copy_from_user(&mask, stipple->mask, 32 * sizeof(u32)))
-		return -EFAULT;
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-
-	radeon_cp_dispatch_stipple(dev, mask);
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_indirect(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf;
-	drm_radeon_indirect_t *indirect = data;
-	RING_LOCALS;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	DRM_DEBUG("idx=%d s=%d e=%d d=%d\n",
-		  indirect->idx, indirect->start, indirect->end,
-		  indirect->discard);
-
-	if (indirect->idx < 0 || indirect->idx >= dma->buf_count) {
-		DRM_ERROR("buffer index %d (of %d max)\n",
-			  indirect->idx, dma->buf_count - 1);
-		return -EINVAL;
-	}
-
-	buf = dma->buflist[indirect->idx];
-
-	if (buf->file_priv != file_priv) {
-		DRM_ERROR("process %d using buffer owned by %p\n",
-			  DRM_CURRENTPID, buf->file_priv);
-		return -EINVAL;
-	}
-	if (buf->pending) {
-		DRM_ERROR("sending pending buffer %d\n", indirect->idx);
-		return -EINVAL;
-	}
-
-	if (indirect->start < buf->used) {
-		DRM_ERROR("reusing indirect: start=0x%x actual=0x%x\n",
-			  indirect->start, buf->used);
-		return -EINVAL;
-	}
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	buf->used = indirect->end;
-
-	/* Dispatch the indirect buffer full of commands from the
-	 * X server.  This is insecure and is thus only available to
-	 * privileged clients.
-	 */
-	if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-		r600_cp_dispatch_indirect(dev, buf, indirect->start, indirect->end);
-	else {
-		/* Wait for the 3D stream to idle before the indirect buffer
-		 * containing 2D acceleration commands is processed.
-		 */
-		BEGIN_RING(2);
-		RADEON_WAIT_UNTIL_3D_IDLE();
-		ADVANCE_RING();
-		radeon_cp_dispatch_indirect(dev, buf, indirect->start, indirect->end);
-	}
-
-	if (indirect->discard) {
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-	}
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_cp_vertex2(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_sarea_t *sarea_priv;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf;
-	drm_radeon_vertex2_t *vertex = data;
-	int i;
-	unsigned char laststate;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	sarea_priv = master_priv->sarea_priv;
-
-	DRM_DEBUG("pid=%d index=%d discard=%d\n",
-		  DRM_CURRENTPID, vertex->idx, vertex->discard);
-
-	if (vertex->idx < 0 || vertex->idx >= dma->buf_count) {
-		DRM_ERROR("buffer index %d (of %d max)\n",
-			  vertex->idx, dma->buf_count - 1);
-		return -EINVAL;
-	}
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	buf = dma->buflist[vertex->idx];
-
-	if (buf->file_priv != file_priv) {
-		DRM_ERROR("process %d using buffer owned by %p\n",
-			  DRM_CURRENTPID, buf->file_priv);
-		return -EINVAL;
-	}
-
-	if (buf->pending) {
-		DRM_ERROR("sending pending buffer %d\n", vertex->idx);
-		return -EINVAL;
-	}
-
-	if (sarea_priv->nbox > RADEON_NR_SAREA_CLIPRECTS)
-		return -EINVAL;
-
-	for (laststate = 0xff, i = 0; i < vertex->nr_prims; i++) {
-		drm_radeon_prim_t prim;
-		drm_radeon_tcl_prim_t tclprim;
-
-		if (copy_from_user(&prim, &vertex->prim[i], sizeof(prim)))
-			return -EFAULT;
-
-		if (prim.stateidx != laststate) {
-			drm_radeon_state_t state;
-
-			if (copy_from_user(&state,
-					       &vertex->state[prim.stateidx],
-					       sizeof(state)))
-				return -EFAULT;
-
-			if (radeon_emit_state2(dev_priv, file_priv, &state)) {
-				DRM_ERROR("radeon_emit_state2 failed\n");
-				return -EINVAL;
-			}
-
-			laststate = prim.stateidx;
-		}
-
-		tclprim.start = prim.start;
-		tclprim.finish = prim.finish;
-		tclprim.prim = prim.prim;
-		tclprim.vc_format = prim.vc_format;
-
-		if (prim.prim & RADEON_PRIM_WALK_IND) {
-			tclprim.offset = prim.numverts * 64;
-			tclprim.numverts = RADEON_MAX_VB_VERTS;	/* duh */
-
-			radeon_cp_dispatch_indices(dev, file_priv->master, buf, &tclprim);
-		} else {
-			tclprim.numverts = prim.numverts;
-			tclprim.offset = 0;	/* not used */
-
-			radeon_cp_dispatch_vertex(dev, file_priv, buf, &tclprim);
-		}
-
-		if (sarea_priv->nbox == 1)
-			sarea_priv->nbox = 0;
-	}
-
-	if (vertex->discard) {
-		radeon_cp_discard_buffer(dev, file_priv->master, buf);
-	}
-
-	COMMIT_RING();
-	return 0;
-}
-
-static int radeon_emit_packets(drm_radeon_private_t * dev_priv,
-			       struct drm_file *file_priv,
-			       drm_radeon_cmd_header_t header,
-			       drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	int id = (int)header.packet.packet_id;
-	int sz, reg;
-	RING_LOCALS;
-
-	if (id >= RADEON_MAX_STATE_PACKETS)
-		return -EINVAL;
-
-	sz = packet[id].len;
-	reg = packet[id].start;
-
-	if (sz * sizeof(u32) > drm_buffer_unprocessed(cmdbuf->buffer)) {
-		DRM_ERROR("Packet size provided larger than data provided\n");
-		return -EINVAL;
-	}
-
-	if (radeon_check_and_fixup_packets(dev_priv, file_priv, id,
-				cmdbuf->buffer)) {
-		DRM_ERROR("Packet verification failed\n");
-		return -EINVAL;
-	}
-
-	BEGIN_RING(sz + 1);
-	OUT_RING(CP_PACKET0(reg, (sz - 1)));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static __inline__ int radeon_emit_scalars(drm_radeon_private_t *dev_priv,
-					  drm_radeon_cmd_header_t header,
-					  drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	int sz = header.scalars.count;
-	int start = header.scalars.offset;
-	int stride = header.scalars.stride;
-	RING_LOCALS;
-
-	BEGIN_RING(3 + sz);
-	OUT_RING(CP_PACKET0(RADEON_SE_TCL_SCALAR_INDX_REG, 0));
-	OUT_RING(start | (stride << RADEON_SCAL_INDX_DWORD_STRIDE_SHIFT));
-	OUT_RING(CP_PACKET0_TABLE(RADEON_SE_TCL_SCALAR_DATA_REG, sz - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-	return 0;
-}
-
-/* God this is ugly
- */
-static __inline__ int radeon_emit_scalars2(drm_radeon_private_t *dev_priv,
-					   drm_radeon_cmd_header_t header,
-					   drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	int sz = header.scalars.count;
-	int start = ((unsigned int)header.scalars.offset) + 0x100;
-	int stride = header.scalars.stride;
-	RING_LOCALS;
-
-	BEGIN_RING(3 + sz);
-	OUT_RING(CP_PACKET0(RADEON_SE_TCL_SCALAR_INDX_REG, 0));
-	OUT_RING(start | (stride << RADEON_SCAL_INDX_DWORD_STRIDE_SHIFT));
-	OUT_RING(CP_PACKET0_TABLE(RADEON_SE_TCL_SCALAR_DATA_REG, sz - 1));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-	return 0;
-}
-
-static __inline__ int radeon_emit_vectors(drm_radeon_private_t *dev_priv,
-					  drm_radeon_cmd_header_t header,
-					  drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	int sz = header.vectors.count;
-	int start = header.vectors.offset;
-	int stride = header.vectors.stride;
-	RING_LOCALS;
-
-	BEGIN_RING(5 + sz);
-	OUT_RING_REG(RADEON_SE_TCL_STATE_FLUSH, 0);
-	OUT_RING(CP_PACKET0(RADEON_SE_TCL_VECTOR_INDX_REG, 0));
-	OUT_RING(start | (stride << RADEON_VEC_INDX_OCTWORD_STRIDE_SHIFT));
-	OUT_RING(CP_PACKET0_TABLE(RADEON_SE_TCL_VECTOR_DATA_REG, (sz - 1)));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static __inline__ int radeon_emit_veclinear(drm_radeon_private_t *dev_priv,
-					  drm_radeon_cmd_header_t header,
-					  drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	int sz = header.veclinear.count * 4;
-	int start = header.veclinear.addr_lo | (header.veclinear.addr_hi << 8);
-	RING_LOCALS;
-
-        if (!sz)
-                return 0;
-	if (sz * 4 > drm_buffer_unprocessed(cmdbuf->buffer))
-                return -EINVAL;
-
-	BEGIN_RING(5 + sz);
-	OUT_RING_REG(RADEON_SE_TCL_STATE_FLUSH, 0);
-	OUT_RING(CP_PACKET0(RADEON_SE_TCL_VECTOR_INDX_REG, 0));
-	OUT_RING(start | (1 << RADEON_VEC_INDX_OCTWORD_STRIDE_SHIFT));
-	OUT_RING(CP_PACKET0_TABLE(RADEON_SE_TCL_VECTOR_DATA_REG, (sz - 1)));
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, sz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static int radeon_emit_packet3(struct drm_device * dev,
-			       struct drm_file *file_priv,
-			       drm_radeon_kcmd_buffer_t *cmdbuf)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	unsigned int cmdsz;
-	int ret;
-	RING_LOCALS;
-
-	DRM_DEBUG("\n");
-
-	if ((ret = radeon_check_and_fixup_packet3(dev_priv, file_priv,
-						  cmdbuf, &cmdsz))) {
-		DRM_ERROR("Packet verification failed\n");
-		return ret;
-	}
-
-	BEGIN_RING(cmdsz);
-	OUT_RING_DRM_BUFFER(cmdbuf->buffer, cmdsz);
-	ADVANCE_RING();
-
-	return 0;
-}
-
-static int radeon_emit_packet3_cliprect(struct drm_device *dev,
-					struct drm_file *file_priv,
-					drm_radeon_kcmd_buffer_t *cmdbuf,
-					int orig_nbox)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_clip_rect box;
-	unsigned int cmdsz;
-	int ret;
-	struct drm_clip_rect __user *boxes = cmdbuf->boxes;
-	int i = 0;
-	RING_LOCALS;
-
-	DRM_DEBUG("\n");
-
-	if ((ret = radeon_check_and_fixup_packet3(dev_priv, file_priv,
-						  cmdbuf, &cmdsz))) {
-		DRM_ERROR("Packet verification failed\n");
-		return ret;
-	}
-
-	if (!orig_nbox)
-		goto out;
-
-	do {
-		if (i < cmdbuf->nbox) {
-			if (copy_from_user(&box, &boxes[i], sizeof(box)))
-				return -EFAULT;
-			/* FIXME The second and subsequent times round
-			 * this loop, send a WAIT_UNTIL_3D_IDLE before
-			 * calling emit_clip_rect(). This fixes a
-			 * lockup on fast machines when sending
-			 * several cliprects with a cmdbuf, as when
-			 * waving a 2D window over a 3D
-			 * window. Something in the commands from user
-			 * space seems to hang the card when they're
-			 * sent several times in a row. That would be
-			 * the correct place to fix it but this works
-			 * around it until I can figure that out - Tim
-			 * Smith */
-			if (i) {
-				BEGIN_RING(2);
-				RADEON_WAIT_UNTIL_3D_IDLE();
-				ADVANCE_RING();
-			}
-			radeon_emit_clip_rect(dev_priv, &box);
-		}
-
-		BEGIN_RING(cmdsz);
-		OUT_RING_DRM_BUFFER(cmdbuf->buffer, cmdsz);
-		ADVANCE_RING();
-
-	} while (++i < cmdbuf->nbox);
-	if (cmdbuf->nbox == 1)
-		cmdbuf->nbox = 0;
-
-	return 0;
-      out:
-	drm_buffer_advance(cmdbuf->buffer, cmdsz * 4);
-	return 0;
-}
-
-static int radeon_emit_wait(struct drm_device * dev, int flags)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	RING_LOCALS;
-
-	DRM_DEBUG("%x\n", flags);
-	switch (flags) {
-	case RADEON_WAIT_2D:
-		BEGIN_RING(2);
-		RADEON_WAIT_UNTIL_2D_IDLE();
-		ADVANCE_RING();
-		break;
-	case RADEON_WAIT_3D:
-		BEGIN_RING(2);
-		RADEON_WAIT_UNTIL_3D_IDLE();
-		ADVANCE_RING();
-		break;
-	case RADEON_WAIT_2D | RADEON_WAIT_3D:
-		BEGIN_RING(2);
-		RADEON_WAIT_UNTIL_IDLE();
-		ADVANCE_RING();
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int radeon_cp_cmdbuf(struct drm_device *dev, void *data,
-		struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_device_dma *dma = dev->dma;
-	struct drm_buf *buf = NULL;
-	drm_radeon_cmd_header_t stack_header;
-	int idx;
-	drm_radeon_kcmd_buffer_t *cmdbuf = data;
-	int orig_nbox;
-
-	LOCK_TEST_WITH_RETURN(dev, file_priv);
-
-	RING_SPACE_TEST_WITH_RETURN(dev_priv);
-	VB_AGE_TEST_WITH_RETURN(dev_priv);
-
-	if (cmdbuf->bufsz > 64 * 1024 || cmdbuf->bufsz < 0) {
-		return -EINVAL;
-	}
-
-	/* Allocate an in-kernel area and copy in the cmdbuf.  Do this to avoid
-	 * races between checking values and using those values in other code,
-	 * and simply to avoid a lot of function calls to copy in data.
-	 */
-	if (cmdbuf->bufsz != 0) {
-		int rv;
-		void __user *buffer = cmdbuf->buffer;
-		rv = drm_buffer_alloc(&cmdbuf->buffer, cmdbuf->bufsz);
-		if (rv)
-			return rv;
-		rv = drm_buffer_copy_from_user(cmdbuf->buffer, buffer,
-						cmdbuf->bufsz);
-		if (rv) {
-			drm_buffer_free(cmdbuf->buffer);
-			return rv;
-		}
-	} else
-		goto done;
-
-	orig_nbox = cmdbuf->nbox;
-
-	if (dev_priv->microcode_version == UCODE_R300) {
-		int temp;
-		temp = r300_do_cp_cmdbuf(dev, file_priv, cmdbuf);
-
-		drm_buffer_free(cmdbuf->buffer);
-
-		return temp;
-	}
-
-	/* microcode_version != r300 */
-	while (drm_buffer_unprocessed(cmdbuf->buffer) >= sizeof(stack_header)) {
-
-		drm_radeon_cmd_header_t *header;
-		header = drm_buffer_read_object(cmdbuf->buffer,
-				sizeof(stack_header), &stack_header);
-
-		switch (header->header.cmd_type) {
-		case RADEON_CMD_PACKET:
-			DRM_DEBUG("RADEON_CMD_PACKET\n");
-			if (radeon_emit_packets
-			    (dev_priv, file_priv, *header, cmdbuf)) {
-				DRM_ERROR("radeon_emit_packets failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_SCALARS:
-			DRM_DEBUG("RADEON_CMD_SCALARS\n");
-			if (radeon_emit_scalars(dev_priv, *header, cmdbuf)) {
-				DRM_ERROR("radeon_emit_scalars failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_VECTORS:
-			DRM_DEBUG("RADEON_CMD_VECTORS\n");
-			if (radeon_emit_vectors(dev_priv, *header, cmdbuf)) {
-				DRM_ERROR("radeon_emit_vectors failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_DMA_DISCARD:
-			DRM_DEBUG("RADEON_CMD_DMA_DISCARD\n");
-			idx = header->dma.buf_idx;
-			if (idx < 0 || idx >= dma->buf_count) {
-				DRM_ERROR("buffer index %d (of %d max)\n",
-					  idx, dma->buf_count - 1);
-				goto err;
-			}
-
-			buf = dma->buflist[idx];
-			if (buf->file_priv != file_priv || buf->pending) {
-				DRM_ERROR("bad buffer %p %p %d\n",
-					  buf->file_priv, file_priv,
-					  buf->pending);
-				goto err;
-			}
-
-			radeon_cp_discard_buffer(dev, file_priv->master, buf);
-			break;
-
-		case RADEON_CMD_PACKET3:
-			DRM_DEBUG("RADEON_CMD_PACKET3\n");
-			if (radeon_emit_packet3(dev, file_priv, cmdbuf)) {
-				DRM_ERROR("radeon_emit_packet3 failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_PACKET3_CLIP:
-			DRM_DEBUG("RADEON_CMD_PACKET3_CLIP\n");
-			if (radeon_emit_packet3_cliprect
-			    (dev, file_priv, cmdbuf, orig_nbox)) {
-				DRM_ERROR("radeon_emit_packet3_clip failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_SCALARS2:
-			DRM_DEBUG("RADEON_CMD_SCALARS2\n");
-			if (radeon_emit_scalars2(dev_priv, *header, cmdbuf)) {
-				DRM_ERROR("radeon_emit_scalars2 failed\n");
-				goto err;
-			}
-			break;
-
-		case RADEON_CMD_WAIT:
-			DRM_DEBUG("RADEON_CMD_WAIT\n");
-			if (radeon_emit_wait(dev, header->wait.flags)) {
-				DRM_ERROR("radeon_emit_wait failed\n");
-				goto err;
-			}
-			break;
-		case RADEON_CMD_VECLINEAR:
-			DRM_DEBUG("RADEON_CMD_VECLINEAR\n");
-			if (radeon_emit_veclinear(dev_priv, *header, cmdbuf)) {
-				DRM_ERROR("radeon_emit_veclinear failed\n");
-				goto err;
-			}
-			break;
-
-		default:
-			DRM_ERROR("bad cmd_type %d at byte %d\n",
-				  header->header.cmd_type,
-				  cmdbuf->buffer->iterator);
-			goto err;
-		}
-	}
-
-	drm_buffer_free(cmdbuf->buffer);
-
-      done:
-	DRM_DEBUG("DONE\n");
-	COMMIT_RING();
-	return 0;
-
-      err:
-	drm_buffer_free(cmdbuf->buffer);
-	return -EINVAL;
-}
-
-static int radeon_cp_getparam(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	drm_radeon_getparam_t *param = data;
-	int value;
-
-	DRM_DEBUG("pid=%d\n", DRM_CURRENTPID);
-
-	switch (param->param) {
-	case RADEON_PARAM_GART_BUFFER_OFFSET:
-		value = dev_priv->gart_buffers_offset;
-		break;
-	case RADEON_PARAM_LAST_FRAME:
-		dev_priv->stats.last_frame_reads++;
-		value = GET_SCRATCH(dev_priv, 0);
-		break;
-	case RADEON_PARAM_LAST_DISPATCH:
-		value = GET_SCRATCH(dev_priv, 1);
-		break;
-	case RADEON_PARAM_LAST_CLEAR:
-		dev_priv->stats.last_clear_reads++;
-		value = GET_SCRATCH(dev_priv, 2);
-		break;
-	case RADEON_PARAM_IRQ_NR:
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			value = 0;
-		else
-			value = dev->pdev->irq;
-		break;
-	case RADEON_PARAM_GART_BASE:
-		value = dev_priv->gart_vm_start;
-		break;
-	case RADEON_PARAM_REGISTER_HANDLE:
-		value = dev_priv->mmio->offset;
-		break;
-	case RADEON_PARAM_STATUS_HANDLE:
-		value = dev_priv->ring_rptr_offset;
-		break;
-#if BITS_PER_LONG == 32
-		/*
-		 * This ioctl() doesn't work on 64-bit platforms because hw_lock is a
-		 * pointer which can't fit into an int-sized variable.  According to
-		 * Michel Dänzer, the ioctl() is only used on embedded platforms, so
-		 * not supporting it shouldn't be a problem.  If the same functionality
-		 * is needed on 64-bit platforms, a new ioctl() would have to be added,
-		 * so backwards-compatibility for the embedded platforms can be
-		 * maintained.  --davidm 4-Feb-2004.
-		 */
-	case RADEON_PARAM_SAREA_HANDLE:
-		/* The lock is the first dword in the sarea. */
-		/* no users of this parameter */
-		break;
-#endif
-	case RADEON_PARAM_GART_TEX_HANDLE:
-		value = dev_priv->gart_textures_offset;
-		break;
-	case RADEON_PARAM_SCRATCH_OFFSET:
-		if (!dev_priv->writeback_works)
-			return -EINVAL;
-		if ((dev_priv->flags & RADEON_FAMILY_MASK) >= CHIP_R600)
-			value = R600_SCRATCH_REG_OFFSET;
-		else
-			value = RADEON_SCRATCH_REG_OFFSET;
-		break;
-	case RADEON_PARAM_CARD_TYPE:
-		if (dev_priv->flags & RADEON_IS_PCIE)
-			value = RADEON_CARD_PCIE;
-		else if (dev_priv->flags & RADEON_IS_AGP)
-			value = RADEON_CARD_AGP;
-		else
-			value = RADEON_CARD_PCI;
-		break;
-	case RADEON_PARAM_VBLANK_CRTC:
-		value = radeon_vblank_crtc_get(dev);
-		break;
-	case RADEON_PARAM_FB_LOCATION:
-		value = radeon_read_fb_location(dev_priv);
-		break;
-	case RADEON_PARAM_NUM_GB_PIPES:
-		value = dev_priv->num_gb_pipes;
-		break;
-	case RADEON_PARAM_NUM_Z_PIPES:
-		value = dev_priv->num_z_pipes;
-		break;
-	default:
-		DRM_DEBUG("Invalid parameter %d\n", param->param);
-		return -EINVAL;
-	}
-
-	if (copy_to_user(param->value, &value, sizeof(int))) {
-		DRM_ERROR("copy_to_user\n");
-		return -EFAULT;
-	}
-
-	return 0;
-}
-
-static int radeon_cp_setparam(struct drm_device *dev, void *data, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_master_private *master_priv = file_priv->master->driver_priv;
-	drm_radeon_setparam_t *sp = data;
-	struct drm_radeon_driver_file_fields *radeon_priv;
-
-	switch (sp->param) {
-	case RADEON_SETPARAM_FB_LOCATION:
-		radeon_priv = file_priv->driver_priv;
-		radeon_priv->radeon_fb_delta = dev_priv->fb_location -
-		    sp->value;
-		break;
-	case RADEON_SETPARAM_SWITCH_TILING:
-		if (sp->value == 0) {
-			DRM_DEBUG("color tiling disabled\n");
-			dev_priv->front_pitch_offset &= ~RADEON_DST_TILE_MACRO;
-			dev_priv->back_pitch_offset &= ~RADEON_DST_TILE_MACRO;
-			if (master_priv->sarea_priv)
-				master_priv->sarea_priv->tiling_enabled = 0;
-		} else if (sp->value == 1) {
-			DRM_DEBUG("color tiling enabled\n");
-			dev_priv->front_pitch_offset |= RADEON_DST_TILE_MACRO;
-			dev_priv->back_pitch_offset |= RADEON_DST_TILE_MACRO;
-			if (master_priv->sarea_priv)
-				master_priv->sarea_priv->tiling_enabled = 1;
-		}
-		break;
-	case RADEON_SETPARAM_PCIGART_LOCATION:
-		dev_priv->pcigart_offset = sp->value;
-		dev_priv->pcigart_offset_set = 1;
-		break;
-	case RADEON_SETPARAM_NEW_MEMMAP:
-		dev_priv->new_memmap = sp->value;
-		break;
-	case RADEON_SETPARAM_PCIGART_TABLE_SIZE:
-		dev_priv->gart_info.table_size = sp->value;
-		if (dev_priv->gart_info.table_size < RADEON_PCIGART_TABLE_SIZE)
-			dev_priv->gart_info.table_size = RADEON_PCIGART_TABLE_SIZE;
-		break;
-	case RADEON_SETPARAM_VBLANK_CRTC:
-		return radeon_vblank_crtc_set(dev, sp->value);
-		break;
-	default:
-		DRM_DEBUG("Invalid parameter %d\n", sp->param);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-/* When a client dies:
- *    - Check for and clean up flipped page state
- *    - Free any alloced GART memory.
- *    - Free any alloced radeon surfaces.
- *
- * DRM infrastructure takes care of reclaiming dma buffers.
- */
-void radeon_driver_preclose(struct drm_device *dev, struct drm_file *file_priv)
-{
-	if (dev->dev_private) {
-		drm_radeon_private_t *dev_priv = dev->dev_private;
-		dev_priv->page_flipping = 0;
-		radeon_mem_release(file_priv, dev_priv->gart_heap);
-		radeon_mem_release(file_priv, dev_priv->fb_heap);
-		radeon_surfaces_release(file_priv, dev_priv);
-	}
-}
-
-void radeon_driver_lastclose(struct drm_device *dev)
-{
-	radeon_surfaces_release(PCIGART_FILE_PRIV, dev->dev_private);
-	radeon_do_release(dev);
-}
-
-int radeon_driver_open(struct drm_device *dev, struct drm_file *file_priv)
-{
-	drm_radeon_private_t *dev_priv = dev->dev_private;
-	struct drm_radeon_driver_file_fields *radeon_priv;
-
-	DRM_DEBUG("\n");
-	radeon_priv = kmalloc(sizeof(*radeon_priv), GFP_KERNEL);
-
-	if (!radeon_priv)
-		return -ENOMEM;
-
-	file_priv->driver_priv = radeon_priv;
-
-	if (dev_priv)
-		radeon_priv->radeon_fb_delta = dev_priv->fb_location;
-	else
-		radeon_priv->radeon_fb_delta = 0;
-	return 0;
-}
-
-void radeon_driver_postclose(struct drm_device *dev, struct drm_file *file_priv)
-{
-	struct drm_radeon_driver_file_fields *radeon_priv =
-	    file_priv->driver_priv;
-
-	kfree(radeon_priv);
-}
-
-struct drm_ioctl_desc radeon_ioctls[] = {
-	DRM_IOCTL_DEF_DRV(RADEON_CP_INIT, radeon_cp_init, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_CP_START, radeon_cp_start, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_CP_STOP, radeon_cp_stop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_CP_RESET, radeon_cp_reset, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_CP_IDLE, radeon_cp_idle, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_CP_RESUME, radeon_cp_resume, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_RESET, radeon_engine_reset, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_FULLSCREEN, radeon_fullscreen, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_SWAP, radeon_cp_swap, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_CLEAR, radeon_cp_clear, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_VERTEX, radeon_cp_vertex, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_INDICES, radeon_cp_indices, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_TEXTURE, radeon_cp_texture, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_STIPPLE, radeon_cp_stipple, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_INDIRECT, radeon_cp_indirect, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_VERTEX2, radeon_cp_vertex2, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_CMDBUF, radeon_cp_cmdbuf, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_GETPARAM, radeon_cp_getparam, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_FLIP, radeon_cp_flip, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_ALLOC, radeon_mem_alloc, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_FREE, radeon_mem_free, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_INIT_HEAP, radeon_mem_init_heap, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF_DRV(RADEON_IRQ_EMIT, radeon_irq_emit, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_IRQ_WAIT, radeon_irq_wait, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_SETPARAM, radeon_cp_setparam, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_SURF_ALLOC, radeon_surface_alloc, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_SURF_FREE, radeon_surface_free, DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(RADEON_CS, r600_cs_legacy_ioctl, DRM_AUTH)
-};
-
-int radeon_max_ioctl = ARRAY_SIZE(radeon_ioctls);
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index e34307459e50..e06ac546a90f 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -758,7 +758,7 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 						       0, PAGE_SIZE,
 						       PCI_DMA_BIDIRECTIONAL);
 		if (pci_dma_mapping_error(rdev->pdev, gtt->ttm.dma_address[i])) {
-			while (--i) {
+			while (i--) {
 				pci_unmap_page(rdev->pdev, gtt->ttm.dma_address[i],
 					       PAGE_SIZE, PCI_DMA_BIDIRECTIONAL);
 				gtt->ttm.dma_address[i] = 0;
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index 48d97c040f49..3979632b9225 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -455,15 +455,15 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 
 	if (soffset) {
 		/* make sure object fit at this offset */
-		eoffset = soffset + size;
+		eoffset = soffset + size - 1;
 		if (soffset >= eoffset) {
 			r = -EINVAL;
 			goto error_unreserve;
 		}
 
 		last_pfn = eoffset / RADEON_GPU_PAGE_SIZE;
-		if (last_pfn > rdev->vm_manager.max_pfn) {
-			dev_err(rdev->dev, "va above limit (0x%08X > 0x%08X)\n",
+		if (last_pfn >= rdev->vm_manager.max_pfn) {
+			dev_err(rdev->dev, "va above limit (0x%08X >= 0x%08X)\n",
 				last_pfn, rdev->vm_manager.max_pfn);
 			r = -EINVAL;
 			goto error_unreserve;
@@ -478,7 +478,7 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 	eoffset /= RADEON_GPU_PAGE_SIZE;
 	if (soffset || eoffset) {
 		struct interval_tree_node *it;
-		it = interval_tree_iter_first(&vm->va, soffset, eoffset - 1);
+		it = interval_tree_iter_first(&vm->va, soffset, eoffset);
 		if (it && it != &bo_va->it) {
 			struct radeon_bo_va *tmp;
 			tmp = container_of(it, struct radeon_bo_va, it);
@@ -518,7 +518,7 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 	if (soffset || eoffset) {
 		spin_lock(&vm->status_lock);
 		bo_va->it.start = soffset;
-		bo_va->it.last = eoffset - 1;
+		bo_va->it.last = eoffset;
 		list_add(&bo_va->vm_status, &vm->cleared);
 		spin_unlock(&vm->status_lock);
 		interval_tree_insert(&bo_va->it, &vm->va);
@@ -888,7 +888,7 @@ static void radeon_vm_fence_pts(struct radeon_vm *vm,
 	unsigned i;
 
 	start >>= radeon_vm_block_size;
-	end >>= radeon_vm_block_size;
+	end = (end - 1) >> radeon_vm_block_size;
 
 	for (i = start; i <= end; ++i)
 		radeon_bo_fence(vm->page_tables[i].bo, fence, true);
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index 4c4a7218a3bd..d1a7b58dd291 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -915,6 +915,11 @@
 #define DCCG_AUDIO_DTO1_PHASE                           0x05c0
 #define DCCG_AUDIO_DTO1_MODULE                          0x05c4
 
+#define DENTIST_DISPCLK_CNTL				0x0490
+#	define DENTIST_DPREFCLK_WDIVIDER(x)		(((x) & 0x7f) << 24)
+#	define DENTIST_DPREFCLK_WDIVIDER_MASK		(0x7f << 24)
+#	define DENTIST_DPREFCLK_WDIVIDER_SHIFT		24
+
 #define AFMT_AUDIO_SRC_CONTROL                          0x713c
 #define		AFMT_AUDIO_SRC_SELECT(x)		(((x) & 7) << 0)
 /* AFMT_AUDIO_SRC_SELECT
diff --git a/drivers/gpu/drm/radeon/vce_v1_0.c b/drivers/gpu/drm/radeon/vce_v1_0.c
index 07a0d378e122..a01efe39a820 100644
--- a/drivers/gpu/drm/radeon/vce_v1_0.c
+++ b/drivers/gpu/drm/radeon/vce_v1_0.c
@@ -178,12 +178,12 @@ int vce_v1_0_load_fw(struct radeon_device *rdev, uint32_t *data)
 		return -EINVAL;
 	}
 
-	for (i = 0; i < sign->num; ++i) {
-		if (sign->val[i].chip_id == chip_id)
+	for (i = 0; i < le32_to_cpu(sign->num); ++i) {
+		if (le32_to_cpu(sign->val[i].chip_id) == chip_id)
 			break;
 	}
 
-	if (i == sign->num)
+	if (i == le32_to_cpu(sign->num))
 		return -EINVAL;
 
 	data += (256 - 64) / 4;
@@ -191,18 +191,18 @@ int vce_v1_0_load_fw(struct radeon_device *rdev, uint32_t *data)
 	data[1] = sign->val[i].nonce[1];
 	data[2] = sign->val[i].nonce[2];
 	data[3] = sign->val[i].nonce[3];
-	data[4] = sign->len + 64;
+	data[4] = cpu_to_le32(le32_to_cpu(sign->len) + 64);
 
 	memset(&data[5], 0, 44);
 	memcpy(&data[16], &sign[1], rdev->vce_fw->size - sizeof(*sign));
 
-	data += data[4] / 4;
+	data += le32_to_cpu(data[4]) / 4;
 	data[0] = sign->val[i].sigval[0];
 	data[1] = sign->val[i].sigval[1];
 	data[2] = sign->val[i].sigval[2];
 	data[3] = sign->val[i].sigval[3];
 
-	rdev->vce.keyselect = sign->val[i].keyselect;
+	rdev->vce.keyselect = le32_to_cpu(sign->val[i].keyselect);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/rcar-du/Kconfig b/drivers/gpu/drm/rcar-du/Kconfig
index d4e0a39568f6..96dcd4a78951 100644
--- a/drivers/gpu/drm/rcar-du/Kconfig
+++ b/drivers/gpu/drm/rcar-du/Kconfig
@@ -1,6 +1,6 @@
 config DRM_RCAR_DU
 	tristate "DRM Support for R-Car Display Unit"
-	depends on DRM && ARM && HAVE_DMA_ATTRS && OF
+	depends on DRM && ARM && OF
 	depends on ARCH_SHMOBILE || COMPILE_TEST
 	select DRM_KMS_HELPER
 	select DRM_KMS_CMA_HELPER
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
index 48cb19949ca3..88a4b706be16 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
@@ -613,7 +613,7 @@ int rcar_du_crtc_create(struct rcar_du_group *rgrp, unsigned int index)
 
 	ret = drm_crtc_init_with_planes(rcdu->ddev, crtc,
 					&rgrp->planes[index % 2].plane,
-					NULL, &crtc_funcs);
+					NULL, &crtc_funcs, NULL);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
index d0ae1e8009c6..c08700757feb 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
@@ -173,7 +173,7 @@ int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 			goto done;
 	} else {
 		ret = drm_encoder_init(rcdu->ddev, encoder, &encoder_funcs,
-				       encoder_type);
+				       encoder_type, NULL);
 		if (ret < 0)
 			goto done;
 
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c
index 96f2eb43713c..a37b6e2fe51a 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmicon.c
@@ -28,7 +28,7 @@ static int rcar_du_hdmi_connector_get_modes(struct drm_connector *connector)
 {
 	struct rcar_du_connector *con = to_rcar_connector(connector);
 	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (sfuncs->get_modes == NULL)
 		return 0;
@@ -41,7 +41,7 @@ static int rcar_du_hdmi_connector_mode_valid(struct drm_connector *connector,
 {
 	struct rcar_du_connector *con = to_rcar_connector(connector);
 	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (sfuncs->mode_valid == NULL)
 		return MODE_OK;
@@ -66,7 +66,7 @@ rcar_du_hdmi_connector_detect(struct drm_connector *connector, bool force)
 {
 	struct rcar_du_connector *con = to_rcar_connector(connector);
 	struct drm_encoder *encoder = rcar_encoder_to_drm_encoder(con->encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (sfuncs->detect == NULL)
 		return connector_status_unknown;
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c
index 81da8419282b..2567efcbee36 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_hdmienc.c
@@ -35,7 +35,7 @@ struct rcar_du_hdmienc {
 static void rcar_du_hdmienc_disable(struct drm_encoder *encoder)
 {
 	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (sfuncs->dpms)
 		sfuncs->dpms(encoder, DRM_MODE_DPMS_OFF);
@@ -50,7 +50,7 @@ static void rcar_du_hdmienc_disable(struct drm_encoder *encoder)
 static void rcar_du_hdmienc_enable(struct drm_encoder *encoder)
 {
 	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (hdmienc->renc->lvds)
 		rcar_du_lvdsenc_enable(hdmienc->renc->lvds, encoder->crtc,
@@ -67,7 +67,7 @@ static int rcar_du_hdmienc_atomic_check(struct drm_encoder *encoder,
 					struct drm_connector_state *conn_state)
 {
 	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 	struct drm_display_mode *adjusted_mode = &crtc_state->adjusted_mode;
 	const struct drm_display_mode *mode = &crtc_state->mode;
 
@@ -89,7 +89,7 @@ static void rcar_du_hdmienc_mode_set(struct drm_encoder *encoder,
 				     struct drm_display_mode *adjusted_mode)
 {
 	struct rcar_du_hdmienc *hdmienc = to_rcar_hdmienc(encoder);
-	struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
+	const struct drm_encoder_slave_funcs *sfuncs = to_slave_funcs(encoder);
 
 	if (sfuncs->mode_set)
 		sfuncs->mode_set(encoder, mode, adjusted_mode);
@@ -151,7 +151,7 @@ int rcar_du_hdmienc_init(struct rcar_du_device *rcdu,
 		goto error;
 
 	ret = drm_encoder_init(rcdu->ddev, encoder, &encoder_funcs,
-			       DRM_MODE_ENCODER_TMDS);
+			       DRM_MODE_ENCODER_TMDS, NULL);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_kms.c b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
index ca12e8ca5552..43bce69d8560 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_kms.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
@@ -136,7 +136,7 @@ int rcar_du_dumb_create(struct drm_file *file, struct drm_device *dev,
 
 static struct drm_framebuffer *
 rcar_du_fb_create(struct drm_device *dev, struct drm_file *file_priv,
-		  struct drm_mode_fb_cmd2 *mode_cmd)
+		  const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct rcar_du_device *rcdu = dev->dev_private;
 	const struct rcar_du_format_info *format;
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_plane.c b/drivers/gpu/drm/rcar-du/rcar_du_plane.c
index ffa583712cd9..c3ed9522c0e1 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_plane.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_plane.c
@@ -410,7 +410,8 @@ int rcar_du_planes_init(struct rcar_du_group *rgrp)
 
 		ret = drm_universal_plane_init(rcdu->ddev, &plane->plane, crtcs,
 					       &rcar_du_plane_funcs, formats,
-					       ARRAY_SIZE(formats), type);
+					       ARRAY_SIZE(formats), type,
+					       NULL);
 		if (ret < 0)
 			return ret;
 
diff --git a/drivers/gpu/drm/rockchip/Kconfig b/drivers/gpu/drm/rockchip/Kconfig
index 35215f6867d3..85739859dffc 100644
--- a/drivers/gpu/drm/rockchip/Kconfig
+++ b/drivers/gpu/drm/rockchip/Kconfig
@@ -25,3 +25,13 @@ config ROCKCHIP_DW_HDMI
 	  for the Synopsys DesignWare HDMI driver. If you want to
 	  enable HDMI on RK3288 based SoC, you should selet this
 	  option.
+
+config ROCKCHIP_DW_MIPI_DSI
+	tristate "Rockchip specific extensions for Synopsys DW MIPI DSI"
+	depends on DRM_ROCKCHIP
+	select DRM_MIPI_DSI
+	help
+	 This selects support for Rockchip SoC specific extensions
+	 for the Synopsys DesignWare HDMI driver. If you want to
+	 enable MIPI DSI on RK3288 based SoC, you should selet this
+	 option.
diff --git a/drivers/gpu/drm/rockchip/Makefile b/drivers/gpu/drm/rockchip/Makefile
index f3d8a19c641f..f6a809afceec 100644
--- a/drivers/gpu/drm/rockchip/Makefile
+++ b/drivers/gpu/drm/rockchip/Makefile
@@ -2,9 +2,11 @@
 # Makefile for the drm device driver.  This driver provides support for the
 # Direct Rendering Infrastructure (DRI) in XFree86 4.1.0 and higher.
 
-rockchipdrm-y := rockchip_drm_drv.o rockchip_drm_fb.o rockchip_drm_fbdev.o \
-		rockchip_drm_gem.o
+rockchipdrm-y := rockchip_drm_drv.o rockchip_drm_fb.o \
+		rockchip_drm_gem.o rockchip_drm_vop.o
+rockchipdrm-$(CONFIG_DRM_FBDEV_EMULATION) += rockchip_drm_fbdev.o
 
 obj-$(CONFIG_ROCKCHIP_DW_HDMI) += dw_hdmi-rockchip.o
+obj-$(CONFIG_ROCKCHIP_DW_MIPI_DSI) += dw-mipi-dsi.o
 
-obj-$(CONFIG_DRM_ROCKCHIP) += rockchipdrm.o rockchip_drm_vop.o
+obj-$(CONFIG_DRM_ROCKCHIP) += rockchipdrm.o rockchip_vop_reg.o
diff --git a/drivers/gpu/drm/rockchip/dw-mipi-dsi.c b/drivers/gpu/drm/rockchip/dw-mipi-dsi.c
new file mode 100644
index 000000000000..f8f8f29fb7c3
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/dw-mipi-dsi.c
@@ -0,0 +1,1195 @@
+/*
+ * Copyright (c) 2016, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#include <linux/clk.h>
+#include <linux/component.h>
+#include <linux/iopoll.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
+#include <drm/drm_atomic_helper.h>
+#include <drm/drm_crtc.h>
+#include <drm/drm_crtc_helper.h>
+#include <drm/drm_mipi_dsi.h>
+#include <drm/drm_of.h>
+#include <drm/drm_panel.h>
+#include <drm/drmP.h>
+#include <video/mipi_display.h>
+
+#include "rockchip_drm_drv.h"
+#include "rockchip_drm_vop.h"
+
+#define DRIVER_NAME    "dw-mipi-dsi"
+
+#define GRF_SOC_CON6                    0x025c
+#define DSI0_SEL_VOP_LIT                (1 << 6)
+#define DSI1_SEL_VOP_LIT                (1 << 9)
+
+#define DSI_VERSION			0x00
+#define DSI_PWR_UP			0x04
+#define RESET				0
+#define POWERUP				BIT(0)
+
+#define DSI_CLKMGR_CFG			0x08
+#define TO_CLK_DIVIDSION(div)		(((div) & 0xff) << 8)
+#define TX_ESC_CLK_DIVIDSION(div)	(((div) & 0xff) << 0)
+
+#define DSI_DPI_VCID			0x0c
+#define DPI_VID(vid)			(((vid) & 0x3) << 0)
+
+#define DSI_DPI_COLOR_CODING		0x10
+#define EN18_LOOSELY			BIT(8)
+#define DPI_COLOR_CODING_16BIT_1	0x0
+#define DPI_COLOR_CODING_16BIT_2	0x1
+#define DPI_COLOR_CODING_16BIT_3	0x2
+#define DPI_COLOR_CODING_18BIT_1	0x3
+#define DPI_COLOR_CODING_18BIT_2	0x4
+#define DPI_COLOR_CODING_24BIT		0x5
+
+#define DSI_DPI_CFG_POL			0x14
+#define COLORM_ACTIVE_LOW		BIT(4)
+#define SHUTD_ACTIVE_LOW		BIT(3)
+#define HSYNC_ACTIVE_LOW		BIT(2)
+#define VSYNC_ACTIVE_LOW		BIT(1)
+#define DATAEN_ACTIVE_LOW		BIT(0)
+
+#define DSI_DPI_LP_CMD_TIM		0x18
+#define OUTVACT_LPCMD_TIME(p)		(((p) & 0xff) << 16)
+#define INVACT_LPCMD_TIME(p)		((p) & 0xff)
+
+#define DSI_DBI_CFG			0x20
+#define DSI_DBI_CMDSIZE			0x28
+
+#define DSI_PCKHDL_CFG			0x2c
+#define EN_CRC_RX			BIT(4)
+#define EN_ECC_RX			BIT(3)
+#define EN_BTA				BIT(2)
+#define EN_EOTP_RX			BIT(1)
+#define EN_EOTP_TX			BIT(0)
+
+#define DSI_MODE_CFG			0x34
+#define ENABLE_VIDEO_MODE		0
+#define ENABLE_CMD_MODE			BIT(0)
+
+#define DSI_VID_MODE_CFG		0x38
+#define FRAME_BTA_ACK			BIT(14)
+#define ENABLE_LOW_POWER		(0x3f << 8)
+#define ENABLE_LOW_POWER_MASK		(0x3f << 8)
+#define VID_MODE_TYPE_BURST_SYNC_PULSES		0x2
+#define VID_MODE_TYPE_MASK			0x3
+
+#define DSI_VID_PKT_SIZE		0x3c
+#define VID_PKT_SIZE(p)			(((p) & 0x3fff) << 0)
+#define VID_PKT_MAX_SIZE		0x3fff
+
+#define DSI_VID_HSA_TIME		0x48
+#define DSI_VID_HBP_TIME		0x4c
+#define DSI_VID_HLINE_TIME		0x50
+#define DSI_VID_VSA_LINES		0x54
+#define DSI_VID_VBP_LINES		0x58
+#define DSI_VID_VFP_LINES		0x5c
+#define DSI_VID_VACTIVE_LINES		0x60
+#define DSI_CMD_MODE_CFG		0x68
+#define MAX_RD_PKT_SIZE_LP		BIT(24)
+#define DCS_LW_TX_LP			BIT(19)
+#define DCS_SR_0P_TX_LP			BIT(18)
+#define DCS_SW_1P_TX_LP			BIT(17)
+#define DCS_SW_0P_TX_LP			BIT(16)
+#define GEN_LW_TX_LP			BIT(14)
+#define GEN_SR_2P_TX_LP			BIT(13)
+#define GEN_SR_1P_TX_LP			BIT(12)
+#define GEN_SR_0P_TX_LP			BIT(11)
+#define GEN_SW_2P_TX_LP			BIT(10)
+#define GEN_SW_1P_TX_LP			BIT(9)
+#define GEN_SW_0P_TX_LP			BIT(8)
+#define EN_ACK_RQST			BIT(1)
+#define EN_TEAR_FX			BIT(0)
+
+#define CMD_MODE_ALL_LP			(MAX_RD_PKT_SIZE_LP | \
+					 DCS_LW_TX_LP | \
+					 DCS_SR_0P_TX_LP | \
+					 DCS_SW_1P_TX_LP | \
+					 DCS_SW_0P_TX_LP | \
+					 GEN_LW_TX_LP | \
+					 GEN_SR_2P_TX_LP | \
+					 GEN_SR_1P_TX_LP | \
+					 GEN_SR_0P_TX_LP | \
+					 GEN_SW_2P_TX_LP | \
+					 GEN_SW_1P_TX_LP | \
+					 GEN_SW_0P_TX_LP)
+
+#define DSI_GEN_HDR			0x6c
+#define GEN_HDATA(data)			(((data) & 0xffff) << 8)
+#define GEN_HDATA_MASK			(0xffff << 8)
+#define GEN_HTYPE(type)			(((type) & 0xff) << 0)
+#define GEN_HTYPE_MASK			0xff
+
+#define DSI_GEN_PLD_DATA		0x70
+
+#define DSI_CMD_PKT_STATUS		0x74
+#define GEN_CMD_EMPTY			BIT(0)
+#define GEN_CMD_FULL			BIT(1)
+#define GEN_PLD_W_EMPTY			BIT(2)
+#define GEN_PLD_W_FULL			BIT(3)
+#define GEN_PLD_R_EMPTY			BIT(4)
+#define GEN_PLD_R_FULL			BIT(5)
+#define GEN_RD_CMD_BUSY			BIT(6)
+
+#define DSI_TO_CNT_CFG			0x78
+#define HSTX_TO_CNT(p)			(((p) & 0xffff) << 16)
+#define LPRX_TO_CNT(p)			((p) & 0xffff)
+
+#define DSI_BTA_TO_CNT			0x8c
+
+#define DSI_LPCLK_CTRL			0x94
+#define AUTO_CLKLANE_CTRL		BIT(1)
+#define PHY_TXREQUESTCLKHS		BIT(0)
+
+#define DSI_PHY_TMR_LPCLK_CFG		0x98
+#define PHY_CLKHS2LP_TIME(lbcc)		(((lbcc) & 0x3ff) << 16)
+#define PHY_CLKLP2HS_TIME(lbcc)		((lbcc) & 0x3ff)
+
+#define DSI_PHY_TMR_CFG			0x9c
+#define PHY_HS2LP_TIME(lbcc)		(((lbcc) & 0xff) << 24)
+#define PHY_LP2HS_TIME(lbcc)		(((lbcc) & 0xff) << 16)
+#define MAX_RD_TIME(lbcc)		((lbcc) & 0x7fff)
+
+#define DSI_PHY_RSTZ			0xa0
+#define PHY_DISFORCEPLL			0
+#define PHY_ENFORCEPLL			BIT(3)
+#define PHY_DISABLECLK			0
+#define PHY_ENABLECLK			BIT(2)
+#define PHY_RSTZ			0
+#define PHY_UNRSTZ			BIT(1)
+#define PHY_SHUTDOWNZ			0
+#define PHY_UNSHUTDOWNZ			BIT(0)
+
+#define DSI_PHY_IF_CFG			0xa4
+#define N_LANES(n)			((((n) - 1) & 0x3) << 0)
+#define PHY_STOP_WAIT_TIME(cycle)	(((cycle) & 0xff) << 8)
+
+#define DSI_PHY_STATUS			0xb0
+#define LOCK				BIT(0)
+#define STOP_STATE_CLK_LANE		BIT(2)
+
+#define DSI_PHY_TST_CTRL0		0xb4
+#define PHY_TESTCLK			BIT(1)
+#define PHY_UNTESTCLK			0
+#define PHY_TESTCLR			BIT(0)
+#define PHY_UNTESTCLR			0
+
+#define DSI_PHY_TST_CTRL1		0xb8
+#define PHY_TESTEN			BIT(16)
+#define PHY_UNTESTEN			0
+#define PHY_TESTDOUT(n)			(((n) & 0xff) << 8)
+#define PHY_TESTDIN(n)			(((n) & 0xff) << 0)
+
+#define DSI_INT_ST0			0xbc
+#define DSI_INT_ST1			0xc0
+#define DSI_INT_MSK0			0xc4
+#define DSI_INT_MSK1			0xc8
+
+#define PHY_STATUS_TIMEOUT_US		10000
+#define CMD_PKT_STATUS_TIMEOUT_US	20000
+
+#define BYPASS_VCO_RANGE	BIT(7)
+#define VCO_RANGE_CON_SEL(val)	(((val) & 0x7) << 3)
+#define VCO_IN_CAP_CON_DEFAULT	(0x0 << 1)
+#define VCO_IN_CAP_CON_LOW	(0x1 << 1)
+#define VCO_IN_CAP_CON_HIGH	(0x2 << 1)
+#define REF_BIAS_CUR_SEL	BIT(0)
+
+#define CP_CURRENT_3MA		BIT(3)
+#define CP_PROGRAM_EN		BIT(7)
+#define LPF_PROGRAM_EN		BIT(6)
+#define LPF_RESISTORS_20_KOHM	0
+
+#define HSFREQRANGE_SEL(val)	(((val) & 0x3f) << 1)
+
+#define INPUT_DIVIDER(val)	((val - 1) & 0x7f)
+#define LOW_PROGRAM_EN		0
+#define HIGH_PROGRAM_EN		BIT(7)
+#define LOOP_DIV_LOW_SEL(val)	((val - 1) & 0x1f)
+#define LOOP_DIV_HIGH_SEL(val)	(((val - 1) >> 5) & 0x1f)
+#define PLL_LOOP_DIV_EN		BIT(5)
+#define PLL_INPUT_DIV_EN	BIT(4)
+
+#define POWER_CONTROL		BIT(6)
+#define INTERNAL_REG_CURRENT	BIT(3)
+#define BIAS_BLOCK_ON		BIT(2)
+#define BANDGAP_ON		BIT(0)
+
+#define TER_RESISTOR_HIGH	BIT(7)
+#define	TER_RESISTOR_LOW	0
+#define LEVEL_SHIFTERS_ON	BIT(6)
+#define TER_CAL_DONE		BIT(5)
+#define SETRD_MAX		(0x7 << 2)
+#define POWER_MANAGE		BIT(1)
+#define TER_RESISTORS_ON	BIT(0)
+
+#define BIASEXTR_SEL(val)	((val) & 0x7)
+#define BANDGAP_SEL(val)	((val) & 0x7)
+#define TLP_PROGRAM_EN		BIT(7)
+#define THS_PRE_PROGRAM_EN	BIT(7)
+#define THS_ZERO_PROGRAM_EN	BIT(6)
+
+enum {
+	BANDGAP_97_07,
+	BANDGAP_98_05,
+	BANDGAP_99_02,
+	BANDGAP_100_00,
+	BANDGAP_93_17,
+	BANDGAP_94_15,
+	BANDGAP_95_12,
+	BANDGAP_96_10,
+};
+
+enum {
+	BIASEXTR_87_1,
+	BIASEXTR_91_5,
+	BIASEXTR_95_9,
+	BIASEXTR_100,
+	BIASEXTR_105_94,
+	BIASEXTR_111_88,
+	BIASEXTR_118_8,
+	BIASEXTR_127_7,
+};
+
+struct dw_mipi_dsi_plat_data {
+	unsigned int max_data_lanes;
+	enum drm_mode_status (*mode_valid)(struct drm_connector *connector,
+					   struct drm_display_mode *mode);
+};
+
+struct dw_mipi_dsi {
+	struct drm_encoder encoder;
+	struct drm_connector connector;
+	struct mipi_dsi_host dsi_host;
+	struct drm_panel *panel;
+	struct device *dev;
+	struct regmap *grf_regmap;
+	void __iomem *base;
+
+	struct clk *pllref_clk;
+	struct clk *pclk;
+
+	unsigned int lane_mbps; /* per lane */
+	u32 channel;
+	u32 lanes;
+	u32 format;
+	u16 input_div;
+	u16 feedback_div;
+	struct drm_display_mode *mode;
+
+	const struct dw_mipi_dsi_plat_data *pdata;
+};
+
+enum dw_mipi_dsi_mode {
+	DW_MIPI_DSI_CMD_MODE,
+	DW_MIPI_DSI_VID_MODE,
+};
+
+struct dphy_pll_testdin_map {
+	unsigned int max_mbps;
+	u8 testdin;
+};
+
+/* The table is based on 27MHz DPHY pll reference clock. */
+static const struct dphy_pll_testdin_map dptdin_map[] = {
+	{  90, 0x00}, { 100, 0x10}, { 110, 0x20}, { 130, 0x01},
+	{ 140, 0x11}, { 150, 0x21}, { 170, 0x02}, { 180, 0x12},
+	{ 200, 0x22}, { 220, 0x03}, { 240, 0x13}, { 250, 0x23},
+	{ 270, 0x04}, { 300, 0x14}, { 330, 0x05}, { 360, 0x15},
+	{ 400, 0x25}, { 450, 0x06}, { 500, 0x16}, { 550, 0x07},
+	{ 600, 0x17}, { 650, 0x08}, { 700, 0x18}, { 750, 0x09},
+	{ 800, 0x19}, { 850, 0x29}, { 900, 0x39}, { 950, 0x0a},
+	{1000, 0x1a}, {1050, 0x2a}, {1100, 0x3a}, {1150, 0x0b},
+	{1200, 0x1b}, {1250, 0x2b}, {1300, 0x3b}, {1350, 0x0c},
+	{1400, 0x1c}, {1450, 0x2c}, {1500, 0x3c}
+};
+
+static int max_mbps_to_testdin(unsigned int max_mbps)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dptdin_map); i++)
+		if (dptdin_map[i].max_mbps > max_mbps)
+			return dptdin_map[i].testdin;
+
+	return -EINVAL;
+}
+
+/*
+ * The controller should generate 2 frames before
+ * preparing the peripheral.
+ */
+static void dw_mipi_dsi_wait_for_two_frames(struct dw_mipi_dsi *dsi)
+{
+	int refresh, two_frames;
+
+	refresh = drm_mode_vrefresh(dsi->mode);
+	two_frames = DIV_ROUND_UP(MSEC_PER_SEC, refresh) * 2;
+	msleep(two_frames);
+}
+
+static inline struct dw_mipi_dsi *host_to_dsi(struct mipi_dsi_host *host)
+{
+	return container_of(host, struct dw_mipi_dsi, dsi_host);
+}
+
+static inline struct dw_mipi_dsi *con_to_dsi(struct drm_connector *con)
+{
+	return container_of(con, struct dw_mipi_dsi, connector);
+}
+
+static inline struct dw_mipi_dsi *encoder_to_dsi(struct drm_encoder *encoder)
+{
+	return container_of(encoder, struct dw_mipi_dsi, encoder);
+}
+static inline void dsi_write(struct dw_mipi_dsi *dsi, u32 reg, u32 val)
+{
+	writel(val, dsi->base + reg);
+}
+
+static inline u32 dsi_read(struct dw_mipi_dsi *dsi, u32 reg)
+{
+	return readl(dsi->base + reg);
+}
+
+static void dw_mipi_dsi_phy_write(struct dw_mipi_dsi *dsi, u8 test_code,
+				 u8 test_data)
+{
+	/*
+	 * With the falling edge on TESTCLK, the TESTDIN[7:0] signal content
+	 * is latched internally as the current test code. Test data is
+	 * programmed internally by rising edge on TESTCLK.
+	 */
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_TESTCLK | PHY_UNTESTCLR);
+
+	dsi_write(dsi, DSI_PHY_TST_CTRL1, PHY_TESTEN | PHY_TESTDOUT(0) |
+					  PHY_TESTDIN(test_code));
+
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_UNTESTCLK | PHY_UNTESTCLR);
+
+	dsi_write(dsi, DSI_PHY_TST_CTRL1, PHY_UNTESTEN | PHY_TESTDOUT(0) |
+					  PHY_TESTDIN(test_data));
+
+	dsi_write(dsi, DSI_PHY_TST_CTRL0, PHY_TESTCLK | PHY_UNTESTCLR);
+}
+
+static int dw_mipi_dsi_phy_init(struct dw_mipi_dsi *dsi)
+{
+	int ret, testdin, vco, val;
+
+	vco = (dsi->lane_mbps < 200) ? 0 : (dsi->lane_mbps + 100) / 200;
+
+	testdin = max_mbps_to_testdin(dsi->lane_mbps);
+	if (testdin < 0) {
+		dev_err(dsi->dev,
+			"failed to get testdin for %dmbps lane clock\n",
+			dsi->lane_mbps);
+		return testdin;
+	}
+
+	dsi_write(dsi, DSI_PWR_UP, POWERUP);
+
+	dw_mipi_dsi_phy_write(dsi, 0x10, BYPASS_VCO_RANGE |
+					 VCO_RANGE_CON_SEL(vco) |
+					 VCO_IN_CAP_CON_LOW |
+					 REF_BIAS_CUR_SEL);
+
+	dw_mipi_dsi_phy_write(dsi, 0x11, CP_CURRENT_3MA);
+	dw_mipi_dsi_phy_write(dsi, 0x12, CP_PROGRAM_EN | LPF_PROGRAM_EN |
+					 LPF_RESISTORS_20_KOHM);
+
+	dw_mipi_dsi_phy_write(dsi, 0x44, HSFREQRANGE_SEL(testdin));
+
+	dw_mipi_dsi_phy_write(dsi, 0x19, PLL_LOOP_DIV_EN | PLL_INPUT_DIV_EN);
+	dw_mipi_dsi_phy_write(dsi, 0x17, INPUT_DIVIDER(dsi->input_div));
+	dw_mipi_dsi_phy_write(dsi, 0x18, LOOP_DIV_LOW_SEL(dsi->feedback_div) |
+					 LOW_PROGRAM_EN);
+	dw_mipi_dsi_phy_write(dsi, 0x18, LOOP_DIV_HIGH_SEL(dsi->feedback_div) |
+					 HIGH_PROGRAM_EN);
+
+	dw_mipi_dsi_phy_write(dsi, 0x20, POWER_CONTROL | INTERNAL_REG_CURRENT |
+					 BIAS_BLOCK_ON | BANDGAP_ON);
+
+	dw_mipi_dsi_phy_write(dsi, 0x21, TER_RESISTOR_LOW | TER_CAL_DONE |
+					 SETRD_MAX | TER_RESISTORS_ON);
+	dw_mipi_dsi_phy_write(dsi, 0x21, TER_RESISTOR_HIGH | LEVEL_SHIFTERS_ON |
+					 SETRD_MAX | POWER_MANAGE |
+					 TER_RESISTORS_ON);
+
+	dw_mipi_dsi_phy_write(dsi, 0x22, LOW_PROGRAM_EN |
+					 BIASEXTR_SEL(BIASEXTR_127_7));
+	dw_mipi_dsi_phy_write(dsi, 0x22, HIGH_PROGRAM_EN |
+					 BANDGAP_SEL(BANDGAP_96_10));
+
+	dw_mipi_dsi_phy_write(dsi, 0x70, TLP_PROGRAM_EN | 0xf);
+	dw_mipi_dsi_phy_write(dsi, 0x71, THS_PRE_PROGRAM_EN | 0x55);
+	dw_mipi_dsi_phy_write(dsi, 0x72, THS_ZERO_PROGRAM_EN | 0xa);
+
+	dsi_write(dsi, DSI_PHY_RSTZ, PHY_ENFORCEPLL | PHY_ENABLECLK |
+				     PHY_UNRSTZ | PHY_UNSHUTDOWNZ);
+
+
+	ret = readx_poll_timeout(readl, dsi->base + DSI_PHY_STATUS,
+				 val, val & LOCK, 1000, PHY_STATUS_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(dsi->dev, "failed to wait for phy lock state\n");
+		return ret;
+	}
+
+	ret = readx_poll_timeout(readl, dsi->base + DSI_PHY_STATUS,
+				 val, val & STOP_STATE_CLK_LANE, 1000,
+				 PHY_STATUS_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(dsi->dev,
+			"failed to wait for phy clk lane stop state\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static int dw_mipi_dsi_get_lane_bps(struct dw_mipi_dsi *dsi)
+{
+	unsigned int i, pre;
+	unsigned long mpclk, pllref, tmp;
+	unsigned int m = 1, n = 1, target_mbps = 1000;
+	unsigned int max_mbps = dptdin_map[ARRAY_SIZE(dptdin_map) - 1].max_mbps;
+	int bpp;
+
+	bpp = mipi_dsi_pixel_format_to_bpp(dsi->format);
+	if (bpp < 0) {
+		dev_err(dsi->dev, "failed to get bpp for pixel format %d\n",
+			dsi->format);
+		return bpp;
+	}
+
+	mpclk = DIV_ROUND_UP(dsi->mode->clock, MSEC_PER_SEC);
+	if (mpclk) {
+		/* take 1 / 0.9, since mbps must big than bandwidth of RGB */
+		tmp = mpclk * (bpp / dsi->lanes) * 10 / 9;
+		if (tmp < max_mbps)
+			target_mbps = tmp;
+		else
+			dev_err(dsi->dev, "DPHY clock frequency is out of range\n");
+	}
+
+	pllref = DIV_ROUND_UP(clk_get_rate(dsi->pllref_clk), USEC_PER_SEC);
+	tmp = pllref;
+
+	for (i = 1; i < 6; i++) {
+		pre = pllref / i;
+		if ((tmp > (target_mbps % pre)) && (target_mbps / pre < 512)) {
+			tmp = target_mbps % pre;
+			n = i;
+			m = target_mbps / pre;
+		}
+		if (tmp == 0)
+			break;
+	}
+
+	dsi->lane_mbps = pllref / n * m;
+	dsi->input_div = n;
+	dsi->feedback_div = m;
+
+	return 0;
+}
+
+static int dw_mipi_dsi_host_attach(struct mipi_dsi_host *host,
+				   struct mipi_dsi_device *device)
+{
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+
+	if (device->lanes > dsi->pdata->max_data_lanes) {
+		dev_err(dsi->dev, "the number of data lanes(%u) is too many\n",
+				device->lanes);
+		return -EINVAL;
+	}
+
+	if (!(device->mode_flags & MIPI_DSI_MODE_VIDEO_BURST) ||
+	    !(device->mode_flags & MIPI_DSI_MODE_VIDEO_SYNC_PULSE)) {
+		dev_err(dsi->dev, "device mode is unsupported\n");
+		return -EINVAL;
+	}
+
+	dsi->lanes = device->lanes;
+	dsi->channel = device->channel;
+	dsi->format = device->format;
+	dsi->panel = of_drm_find_panel(device->dev.of_node);
+	if (dsi->panel)
+		return drm_panel_attach(dsi->panel, &dsi->connector);
+
+	return -EINVAL;
+}
+
+static int dw_mipi_dsi_host_detach(struct mipi_dsi_host *host,
+				   struct mipi_dsi_device *device)
+{
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+
+	drm_panel_detach(dsi->panel);
+
+	return 0;
+}
+
+static int dw_mipi_dsi_gen_pkt_hdr_write(struct dw_mipi_dsi *dsi, u32 val)
+{
+	int ret;
+
+	ret = readx_poll_timeout(readl, dsi->base + DSI_CMD_PKT_STATUS,
+				 val, !(val & GEN_CMD_FULL), 1000,
+				 CMD_PKT_STATUS_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(dsi->dev, "failed to get available command FIFO\n");
+		return ret;
+	}
+
+	dsi_write(dsi, DSI_GEN_HDR, val);
+
+	ret = readx_poll_timeout(readl, dsi->base + DSI_CMD_PKT_STATUS,
+				 val, val & (GEN_CMD_EMPTY | GEN_PLD_W_EMPTY),
+				 1000, CMD_PKT_STATUS_TIMEOUT_US);
+	if (ret < 0) {
+		dev_err(dsi->dev, "failed to write command FIFO\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int dw_mipi_dsi_dcs_short_write(struct dw_mipi_dsi *dsi,
+				       const struct mipi_dsi_msg *msg)
+{
+	const u16 *tx_buf = msg->tx_buf;
+	u32 val = GEN_HDATA(*tx_buf) | GEN_HTYPE(msg->type);
+
+	if (msg->tx_len > 2) {
+		dev_err(dsi->dev, "too long tx buf length %zu for short write\n",
+			msg->tx_len);
+		return -EINVAL;
+	}
+
+	return dw_mipi_dsi_gen_pkt_hdr_write(dsi, val);
+}
+
+static int dw_mipi_dsi_dcs_long_write(struct dw_mipi_dsi *dsi,
+				      const struct mipi_dsi_msg *msg)
+{
+	const u32 *tx_buf = msg->tx_buf;
+	int len = msg->tx_len, pld_data_bytes = sizeof(*tx_buf), ret;
+	u32 val = GEN_HDATA(msg->tx_len) | GEN_HTYPE(msg->type);
+	u32 remainder = 0;
+
+	if (msg->tx_len < 3) {
+		dev_err(dsi->dev, "wrong tx buf length %zu for long write\n",
+			msg->tx_len);
+		return -EINVAL;
+	}
+
+	while (DIV_ROUND_UP(len, pld_data_bytes)) {
+		if (len < pld_data_bytes) {
+			memcpy(&remainder, tx_buf, len);
+			dsi_write(dsi, DSI_GEN_PLD_DATA, remainder);
+			len = 0;
+		} else {
+			dsi_write(dsi, DSI_GEN_PLD_DATA, *tx_buf);
+			tx_buf++;
+			len -= pld_data_bytes;
+		}
+
+		ret = readx_poll_timeout(readl, dsi->base + DSI_CMD_PKT_STATUS,
+					 val, !(val & GEN_PLD_W_FULL), 1000,
+					 CMD_PKT_STATUS_TIMEOUT_US);
+		if (ret < 0) {
+			dev_err(dsi->dev,
+				"failed to get available write payload FIFO\n");
+			return ret;
+		}
+	}
+
+	return dw_mipi_dsi_gen_pkt_hdr_write(dsi, val);
+}
+
+static ssize_t dw_mipi_dsi_host_transfer(struct mipi_dsi_host *host,
+					 const struct mipi_dsi_msg *msg)
+{
+	struct dw_mipi_dsi *dsi = host_to_dsi(host);
+	int ret;
+
+	switch (msg->type) {
+	case MIPI_DSI_DCS_SHORT_WRITE:
+	case MIPI_DSI_DCS_SHORT_WRITE_PARAM:
+	case MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE:
+		ret = dw_mipi_dsi_dcs_short_write(dsi, msg);
+		break;
+	case MIPI_DSI_DCS_LONG_WRITE:
+		ret = dw_mipi_dsi_dcs_long_write(dsi, msg);
+		break;
+	default:
+		dev_err(dsi->dev, "unsupported message type\n");
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct mipi_dsi_host_ops dw_mipi_dsi_host_ops = {
+	.attach = dw_mipi_dsi_host_attach,
+	.detach = dw_mipi_dsi_host_detach,
+	.transfer = dw_mipi_dsi_host_transfer,
+};
+
+static void dw_mipi_dsi_video_mode_config(struct dw_mipi_dsi *dsi)
+{
+	u32 val;
+
+	val = VID_MODE_TYPE_BURST_SYNC_PULSES | ENABLE_LOW_POWER;
+
+	dsi_write(dsi, DSI_VID_MODE_CFG, val);
+}
+
+static void dw_mipi_dsi_set_mode(struct dw_mipi_dsi *dsi,
+				 enum dw_mipi_dsi_mode mode)
+{
+	if (mode == DW_MIPI_DSI_CMD_MODE) {
+		dsi_write(dsi, DSI_PWR_UP, RESET);
+		dsi_write(dsi, DSI_MODE_CFG, ENABLE_CMD_MODE);
+		dsi_write(dsi, DSI_PWR_UP, POWERUP);
+	} else {
+		dsi_write(dsi, DSI_PWR_UP, RESET);
+		dsi_write(dsi, DSI_MODE_CFG, ENABLE_VIDEO_MODE);
+		dw_mipi_dsi_video_mode_config(dsi);
+		dsi_write(dsi, DSI_PWR_UP, POWERUP);
+	}
+}
+
+static void dw_mipi_dsi_disable(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PWR_UP, RESET);
+	dsi_write(dsi, DSI_PHY_RSTZ, PHY_RSTZ);
+}
+
+static void dw_mipi_dsi_init(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PWR_UP, RESET);
+	dsi_write(dsi, DSI_PHY_RSTZ, PHY_DISFORCEPLL | PHY_DISABLECLK
+		  | PHY_RSTZ | PHY_SHUTDOWNZ);
+	dsi_write(dsi, DSI_CLKMGR_CFG, TO_CLK_DIVIDSION(10) |
+		  TX_ESC_CLK_DIVIDSION(7));
+	dsi_write(dsi, DSI_LPCLK_CTRL, PHY_TXREQUESTCLKHS);
+}
+
+static void dw_mipi_dsi_dpi_config(struct dw_mipi_dsi *dsi,
+				   struct drm_display_mode *mode)
+{
+	u32 val = 0, color = 0;
+
+	switch (dsi->format) {
+	case MIPI_DSI_FMT_RGB888:
+		color = DPI_COLOR_CODING_24BIT;
+		break;
+	case MIPI_DSI_FMT_RGB666:
+		color = DPI_COLOR_CODING_18BIT_2 | EN18_LOOSELY;
+		break;
+	case MIPI_DSI_FMT_RGB666_PACKED:
+		color = DPI_COLOR_CODING_18BIT_1;
+		break;
+	case MIPI_DSI_FMT_RGB565:
+		color = DPI_COLOR_CODING_16BIT_1;
+		break;
+	}
+
+	if (!(mode->flags & DRM_MODE_FLAG_PVSYNC))
+		val |= VSYNC_ACTIVE_LOW;
+	if (!(mode->flags & DRM_MODE_FLAG_PHSYNC))
+		val |= HSYNC_ACTIVE_LOW;
+
+	dsi_write(dsi, DSI_DPI_VCID, DPI_VID(dsi->channel));
+	dsi_write(dsi, DSI_DPI_COLOR_CODING, color);
+	dsi_write(dsi, DSI_DPI_CFG_POL, val);
+	dsi_write(dsi, DSI_DPI_LP_CMD_TIM, OUTVACT_LPCMD_TIME(4)
+		  | INVACT_LPCMD_TIME(4));
+}
+
+static void dw_mipi_dsi_packet_handler_config(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PCKHDL_CFG, EN_CRC_RX | EN_ECC_RX | EN_BTA);
+}
+
+static void dw_mipi_dsi_video_packet_config(struct dw_mipi_dsi *dsi,
+					    struct drm_display_mode *mode)
+{
+	dsi_write(dsi, DSI_VID_PKT_SIZE, VID_PKT_SIZE(mode->hdisplay));
+}
+
+static void dw_mipi_dsi_command_mode_config(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_TO_CNT_CFG, HSTX_TO_CNT(1000) | LPRX_TO_CNT(1000));
+	dsi_write(dsi, DSI_BTA_TO_CNT, 0xd00);
+	dsi_write(dsi, DSI_CMD_MODE_CFG, CMD_MODE_ALL_LP);
+	dsi_write(dsi, DSI_MODE_CFG, ENABLE_CMD_MODE);
+}
+
+/* Get lane byte clock cycles. */
+static u32 dw_mipi_dsi_get_hcomponent_lbcc(struct dw_mipi_dsi *dsi,
+					   u32 hcomponent)
+{
+	u32 frac, lbcc;
+
+	lbcc = hcomponent * dsi->lane_mbps * MSEC_PER_SEC / 8;
+
+	frac = lbcc % dsi->mode->clock;
+	lbcc = lbcc / dsi->mode->clock;
+	if (frac)
+		lbcc++;
+
+	return lbcc;
+}
+
+static void dw_mipi_dsi_line_timer_config(struct dw_mipi_dsi *dsi)
+{
+	u32 htotal, hsa, hbp, lbcc;
+	struct drm_display_mode *mode = dsi->mode;
+
+	htotal = mode->htotal;
+	hsa = mode->hsync_end - mode->hsync_start;
+	hbp = mode->htotal - mode->hsync_end;
+
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, htotal);
+	dsi_write(dsi, DSI_VID_HLINE_TIME, lbcc);
+
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, hsa);
+	dsi_write(dsi, DSI_VID_HSA_TIME, lbcc);
+
+	lbcc = dw_mipi_dsi_get_hcomponent_lbcc(dsi, hbp);
+	dsi_write(dsi, DSI_VID_HBP_TIME, lbcc);
+}
+
+static void dw_mipi_dsi_vertical_timing_config(struct dw_mipi_dsi *dsi)
+{
+	u32 vactive, vsa, vfp, vbp;
+	struct drm_display_mode *mode = dsi->mode;
+
+	vactive = mode->vdisplay;
+	vsa = mode->vsync_end - mode->vsync_start;
+	vfp = mode->vsync_start - mode->vdisplay;
+	vbp = mode->vtotal - mode->vsync_end;
+
+	dsi_write(dsi, DSI_VID_VACTIVE_LINES, vactive);
+	dsi_write(dsi, DSI_VID_VSA_LINES, vsa);
+	dsi_write(dsi, DSI_VID_VFP_LINES, vfp);
+	dsi_write(dsi, DSI_VID_VBP_LINES, vbp);
+}
+
+static void dw_mipi_dsi_dphy_timing_config(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PHY_TMR_CFG, PHY_HS2LP_TIME(0x40)
+		  | PHY_LP2HS_TIME(0x40) | MAX_RD_TIME(10000));
+
+	dsi_write(dsi, DSI_PHY_TMR_LPCLK_CFG, PHY_CLKHS2LP_TIME(0x40)
+		  | PHY_CLKLP2HS_TIME(0x40));
+}
+
+static void dw_mipi_dsi_dphy_interface_config(struct dw_mipi_dsi *dsi)
+{
+	dsi_write(dsi, DSI_PHY_IF_CFG, PHY_STOP_WAIT_TIME(0x20) |
+		  N_LANES(dsi->lanes));
+}
+
+static void dw_mipi_dsi_clear_err(struct dw_mipi_dsi *dsi)
+{
+	dsi_read(dsi, DSI_INT_ST0);
+	dsi_read(dsi, DSI_INT_ST1);
+	dsi_write(dsi, DSI_INT_MSK0, 0);
+	dsi_write(dsi, DSI_INT_MSK1, 0);
+}
+
+static void dw_mipi_dsi_encoder_mode_set(struct drm_encoder *encoder,
+					struct drm_display_mode *mode,
+					struct drm_display_mode *adjusted_mode)
+{
+	struct dw_mipi_dsi *dsi = encoder_to_dsi(encoder);
+	int ret;
+
+	dsi->mode = adjusted_mode;
+
+	ret = dw_mipi_dsi_get_lane_bps(dsi);
+	if (ret < 0)
+		return;
+
+	if (clk_prepare_enable(dsi->pclk)) {
+		dev_err(dsi->dev, "%s: Failed to enable pclk\n", __func__);
+		return;
+	}
+
+	dw_mipi_dsi_init(dsi);
+	dw_mipi_dsi_dpi_config(dsi, mode);
+	dw_mipi_dsi_packet_handler_config(dsi);
+	dw_mipi_dsi_video_mode_config(dsi);
+	dw_mipi_dsi_video_packet_config(dsi, mode);
+	dw_mipi_dsi_command_mode_config(dsi);
+	dw_mipi_dsi_line_timer_config(dsi);
+	dw_mipi_dsi_vertical_timing_config(dsi);
+	dw_mipi_dsi_dphy_timing_config(dsi);
+	dw_mipi_dsi_dphy_interface_config(dsi);
+	dw_mipi_dsi_clear_err(dsi);
+	if (drm_panel_prepare(dsi->panel))
+		dev_err(dsi->dev, "failed to prepare panel\n");
+
+	clk_disable_unprepare(dsi->pclk);
+}
+
+static void dw_mipi_dsi_encoder_disable(struct drm_encoder *encoder)
+{
+	struct dw_mipi_dsi *dsi = encoder_to_dsi(encoder);
+
+	drm_panel_disable(dsi->panel);
+
+	if (clk_prepare_enable(dsi->pclk)) {
+		dev_err(dsi->dev, "%s: Failed to enable pclk\n", __func__);
+		return;
+	}
+
+	dw_mipi_dsi_set_mode(dsi, DW_MIPI_DSI_CMD_MODE);
+	drm_panel_unprepare(dsi->panel);
+	dw_mipi_dsi_set_mode(dsi, DW_MIPI_DSI_VID_MODE);
+
+	/*
+	 * This is necessary to make sure the peripheral will be driven
+	 * normally when the display is enabled again later.
+	 */
+	msleep(120);
+
+	dw_mipi_dsi_set_mode(dsi, DW_MIPI_DSI_CMD_MODE);
+	dw_mipi_dsi_disable(dsi);
+	clk_disable_unprepare(dsi->pclk);
+}
+
+static bool dw_mipi_dsi_encoder_mode_fixup(struct drm_encoder *encoder,
+					const struct drm_display_mode *mode,
+					struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void dw_mipi_dsi_encoder_commit(struct drm_encoder *encoder)
+{
+	struct dw_mipi_dsi *dsi = encoder_to_dsi(encoder);
+	int mux  = rockchip_drm_encoder_get_mux_id(dsi->dev->of_node, encoder);
+	u32 interface_pix_fmt;
+	u32 val;
+
+	if (clk_prepare_enable(dsi->pclk)) {
+		dev_err(dsi->dev, "%s: Failed to enable pclk\n", __func__);
+		return;
+	}
+
+	dw_mipi_dsi_phy_init(dsi);
+	dw_mipi_dsi_wait_for_two_frames(dsi);
+
+	dw_mipi_dsi_set_mode(dsi, DW_MIPI_DSI_VID_MODE);
+	drm_panel_enable(dsi->panel);
+
+	clk_disable_unprepare(dsi->pclk);
+
+	switch (dsi->format) {
+	case MIPI_DSI_FMT_RGB888:
+		interface_pix_fmt = ROCKCHIP_OUT_MODE_P888;
+		break;
+	case MIPI_DSI_FMT_RGB666:
+		interface_pix_fmt = ROCKCHIP_OUT_MODE_P666;
+		break;
+	case MIPI_DSI_FMT_RGB565:
+		interface_pix_fmt = ROCKCHIP_OUT_MODE_P565;
+		break;
+	default:
+		WARN_ON(1);
+		return;
+	}
+
+	rockchip_drm_crtc_mode_config(encoder->crtc, DRM_MODE_CONNECTOR_DSI,
+				      interface_pix_fmt);
+
+	if (mux)
+		val = DSI0_SEL_VOP_LIT | (DSI0_SEL_VOP_LIT << 16);
+	else
+		val = DSI0_SEL_VOP_LIT << 16;
+
+	regmap_write(dsi->grf_regmap, GRF_SOC_CON6, val);
+	dev_dbg(dsi->dev, "vop %s output to dsi0\n", (mux) ? "LIT" : "BIG");
+}
+
+static struct drm_encoder_helper_funcs
+dw_mipi_dsi_encoder_helper_funcs = {
+	.mode_fixup = dw_mipi_dsi_encoder_mode_fixup,
+	.commit = dw_mipi_dsi_encoder_commit,
+	.mode_set = dw_mipi_dsi_encoder_mode_set,
+	.disable = dw_mipi_dsi_encoder_disable,
+};
+
+static struct drm_encoder_funcs dw_mipi_dsi_encoder_funcs = {
+	.destroy = drm_encoder_cleanup,
+};
+
+static int dw_mipi_dsi_connector_get_modes(struct drm_connector *connector)
+{
+	struct dw_mipi_dsi *dsi = con_to_dsi(connector);
+
+	return drm_panel_get_modes(dsi->panel);
+}
+
+static enum drm_mode_status dw_mipi_dsi_mode_valid(
+					struct drm_connector *connector,
+					struct drm_display_mode *mode)
+{
+	struct dw_mipi_dsi *dsi = con_to_dsi(connector);
+
+	enum drm_mode_status mode_status = MODE_OK;
+
+	if (dsi->pdata->mode_valid)
+		mode_status = dsi->pdata->mode_valid(connector, mode);
+
+	return mode_status;
+}
+
+static struct drm_encoder *dw_mipi_dsi_connector_best_encoder(
+					struct drm_connector *connector)
+{
+	struct dw_mipi_dsi *dsi = con_to_dsi(connector);
+
+	return &dsi->encoder;
+}
+
+static struct drm_connector_helper_funcs dw_mipi_dsi_connector_helper_funcs = {
+	.get_modes = dw_mipi_dsi_connector_get_modes,
+	.mode_valid = dw_mipi_dsi_mode_valid,
+	.best_encoder = dw_mipi_dsi_connector_best_encoder,
+};
+
+static enum drm_connector_status
+dw_mipi_dsi_detect(struct drm_connector *connector, bool force)
+{
+	return connector_status_connected;
+}
+
+static void dw_mipi_dsi_drm_connector_destroy(struct drm_connector *connector)
+{
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
+}
+
+static struct drm_connector_funcs dw_mipi_dsi_atomic_connector_funcs = {
+	.dpms = drm_atomic_helper_connector_dpms,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.detect = dw_mipi_dsi_detect,
+	.destroy = dw_mipi_dsi_drm_connector_destroy,
+	.reset = drm_atomic_helper_connector_reset,
+	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_connector_destroy_state,
+};
+
+static int dw_mipi_dsi_register(struct drm_device *drm,
+				      struct dw_mipi_dsi *dsi)
+{
+	struct drm_encoder *encoder = &dsi->encoder;
+	struct drm_connector *connector = &dsi->connector;
+	struct device *dev = dsi->dev;
+	int ret;
+
+	encoder->possible_crtcs = drm_of_find_possible_crtcs(drm,
+							     dev->of_node);
+	/*
+	 * If we failed to find the CRTC(s) which this encoder is
+	 * supposed to be connected to, it's because the CRTC has
+	 * not been registered yet.  Defer probing, and hope that
+	 * the required CRTC is added later.
+	 */
+	if (encoder->possible_crtcs == 0)
+		return -EPROBE_DEFER;
+
+	drm_encoder_helper_add(&dsi->encoder,
+			       &dw_mipi_dsi_encoder_helper_funcs);
+	ret = drm_encoder_init(drm, &dsi->encoder, &dw_mipi_dsi_encoder_funcs,
+			 DRM_MODE_ENCODER_DSI, NULL);
+	if (ret) {
+		dev_err(dev, "Failed to initialize encoder with drm\n");
+		return ret;
+	}
+
+	drm_connector_helper_add(connector,
+			&dw_mipi_dsi_connector_helper_funcs);
+
+	drm_connector_init(drm, &dsi->connector,
+			   &dw_mipi_dsi_atomic_connector_funcs,
+			   DRM_MODE_CONNECTOR_DSI);
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	return 0;
+}
+
+static int rockchip_mipi_parse_dt(struct dw_mipi_dsi *dsi)
+{
+	struct device_node *np = dsi->dev->of_node;
+
+	dsi->grf_regmap = syscon_regmap_lookup_by_phandle(np, "rockchip,grf");
+	if (IS_ERR(dsi->grf_regmap)) {
+		dev_err(dsi->dev, "Unable to get rockchip,grf\n");
+		return PTR_ERR(dsi->grf_regmap);
+	}
+
+	return 0;
+}
+
+static enum drm_mode_status rk3288_mipi_dsi_mode_valid(
+					struct drm_connector *connector,
+					struct drm_display_mode *mode)
+{
+	/*
+	 * The VID_PKT_SIZE field in the DSI_VID_PKT_CFG
+	 * register is 11-bit.
+	 */
+	if (mode->hdisplay > 0x7ff)
+		return MODE_BAD_HVALUE;
+
+	/*
+	 * The V_ACTIVE_LINES field in the DSI_VTIMING_CFG
+	 * register is 11-bit.
+	 */
+	if (mode->vdisplay > 0x7ff)
+		return MODE_BAD_VVALUE;
+
+	return MODE_OK;
+}
+
+static struct dw_mipi_dsi_plat_data rk3288_mipi_dsi_drv_data = {
+	.max_data_lanes = 4,
+	.mode_valid = rk3288_mipi_dsi_mode_valid,
+};
+
+static const struct of_device_id dw_mipi_dsi_dt_ids[] = {
+	{
+	 .compatible = "rockchip,rk3288-mipi-dsi",
+	 .data = &rk3288_mipi_dsi_drv_data,
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, dw_mipi_dsi_dt_ids);
+
+static int dw_mipi_dsi_bind(struct device *dev, struct device *master,
+			     void *data)
+{
+	const struct of_device_id *of_id =
+			of_match_device(dw_mipi_dsi_dt_ids, dev);
+	const struct dw_mipi_dsi_plat_data *pdata = of_id->data;
+	struct platform_device *pdev = to_platform_device(dev);
+	struct drm_device *drm = data;
+	struct dw_mipi_dsi *dsi;
+	struct resource *res;
+	int ret;
+
+	dsi = devm_kzalloc(dev, sizeof(*dsi), GFP_KERNEL);
+	if (!dsi)
+		return -ENOMEM;
+
+	dsi->dev = dev;
+	dsi->pdata = pdata;
+
+	ret = rockchip_mipi_parse_dt(dsi);
+	if (ret)
+		return ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	dsi->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(dsi->base))
+		return PTR_ERR(dsi->base);
+
+	dsi->pllref_clk = devm_clk_get(dev, "ref");
+	if (IS_ERR(dsi->pllref_clk)) {
+		ret = PTR_ERR(dsi->pllref_clk);
+		dev_err(dev, "Unable to get pll reference clock: %d\n", ret);
+		return ret;
+	}
+
+	dsi->pclk = devm_clk_get(dev, "pclk");
+	if (IS_ERR(dsi->pclk)) {
+		ret = PTR_ERR(dsi->pclk);
+		dev_err(dev, "Unable to get pclk: %d\n", ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(dsi->pllref_clk);
+	if (ret) {
+		dev_err(dev, "%s: Failed to enable pllref_clk\n", __func__);
+		return ret;
+	}
+
+	ret = dw_mipi_dsi_register(drm, dsi);
+	if (ret) {
+		dev_err(dev, "Failed to register mipi_dsi: %d\n", ret);
+		goto err_pllref;
+	}
+
+	dev_set_drvdata(dev, dsi);
+
+	dsi->dsi_host.ops = &dw_mipi_dsi_host_ops;
+	dsi->dsi_host.dev = dev;
+	return mipi_dsi_host_register(&dsi->dsi_host);
+
+err_pllref:
+	clk_disable_unprepare(dsi->pllref_clk);
+	return ret;
+}
+
+static void dw_mipi_dsi_unbind(struct device *dev, struct device *master,
+	void *data)
+{
+	struct dw_mipi_dsi *dsi = dev_get_drvdata(dev);
+
+	mipi_dsi_host_unregister(&dsi->dsi_host);
+	clk_disable_unprepare(dsi->pllref_clk);
+}
+
+static const struct component_ops dw_mipi_dsi_ops = {
+	.bind	= dw_mipi_dsi_bind,
+	.unbind	= dw_mipi_dsi_unbind,
+};
+
+static int dw_mipi_dsi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dw_mipi_dsi_ops);
+}
+
+static int dw_mipi_dsi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dw_mipi_dsi_ops);
+	return 0;
+}
+
+static struct platform_driver dw_mipi_dsi_driver = {
+	.probe		= dw_mipi_dsi_probe,
+	.remove		= dw_mipi_dsi_remove,
+	.driver		= {
+		.of_match_table = dw_mipi_dsi_dt_ids,
+		.name	= DRIVER_NAME,
+	},
+};
+module_platform_driver(dw_mipi_dsi_driver);
+
+MODULE_DESCRIPTION("ROCKCHIP MIPI DSI host controller driver");
+MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DRIVER_NAME);
diff --git a/drivers/gpu/drm/rockchip/dw_hdmi-rockchip.c b/drivers/gpu/drm/rockchip/dw_hdmi-rockchip.c
index 80d6fc8a5cee..c65ce8cb30d3 100644
--- a/drivers/gpu/drm/rockchip/dw_hdmi-rockchip.c
+++ b/drivers/gpu/drm/rockchip/dw_hdmi-rockchip.c
@@ -173,7 +173,7 @@ dw_hdmi_rockchip_mode_valid(struct drm_connector *connector,
 	return (valid) ? MODE_OK : MODE_BAD;
 }
 
-static struct drm_encoder_funcs dw_hdmi_rockchip_encoder_funcs = {
+static const struct drm_encoder_funcs dw_hdmi_rockchip_encoder_funcs = {
 	.destroy = drm_encoder_cleanup,
 };
 
@@ -195,12 +195,15 @@ static void dw_hdmi_rockchip_encoder_mode_set(struct drm_encoder *encoder,
 {
 }
 
-static void dw_hdmi_rockchip_encoder_commit(struct drm_encoder *encoder)
+static void dw_hdmi_rockchip_encoder_enable(struct drm_encoder *encoder)
 {
 	struct rockchip_hdmi *hdmi = to_rockchip_hdmi(encoder);
 	u32 val;
 	int mux;
 
+	rockchip_drm_crtc_mode_config(encoder->crtc, DRM_MODE_CONNECTOR_HDMIA,
+				      ROCKCHIP_OUT_MODE_AAAA);
+
 	mux = rockchip_drm_encoder_get_mux_id(hdmi->dev->of_node, encoder);
 	if (mux)
 		val = HDMI_SEL_VOP_LIT | (HDMI_SEL_VOP_LIT << 16);
@@ -212,17 +215,10 @@ static void dw_hdmi_rockchip_encoder_commit(struct drm_encoder *encoder)
 		(mux) ? "LIT" : "BIG");
 }
 
-static void dw_hdmi_rockchip_encoder_prepare(struct drm_encoder *encoder)
-{
-	rockchip_drm_crtc_mode_config(encoder->crtc, DRM_MODE_CONNECTOR_HDMIA,
-				      ROCKCHIP_OUT_MODE_AAAA);
-}
-
-static struct drm_encoder_helper_funcs dw_hdmi_rockchip_encoder_helper_funcs = {
+static const struct drm_encoder_helper_funcs dw_hdmi_rockchip_encoder_helper_funcs = {
 	.mode_fixup = dw_hdmi_rockchip_encoder_mode_fixup,
 	.mode_set   = dw_hdmi_rockchip_encoder_mode_set,
-	.prepare    = dw_hdmi_rockchip_encoder_prepare,
-	.commit     = dw_hdmi_rockchip_encoder_commit,
+	.enable     = dw_hdmi_rockchip_encoder_enable,
 	.disable    = dw_hdmi_rockchip_encoder_disable,
 };
 
@@ -295,7 +291,7 @@ static int dw_hdmi_rockchip_bind(struct device *dev, struct device *master,
 
 	drm_encoder_helper_add(encoder, &dw_hdmi_rockchip_encoder_helper_funcs);
 	drm_encoder_init(drm, encoder, &dw_hdmi_rockchip_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 
 	return dw_hdmi_bind(dev, master, data, encoder, iores, irq, plat_data);
 }
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.c b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
index f22e1e1ee64a..a0d51ccb6ea4 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.c
@@ -55,20 +55,18 @@ int rockchip_drm_dma_attach_device(struct drm_device *drm_dev,
 
 	return arm_iommu_attach_device(dev, mapping);
 }
-EXPORT_SYMBOL_GPL(rockchip_drm_dma_attach_device);
 
 void rockchip_drm_dma_detach_device(struct drm_device *drm_dev,
 				    struct device *dev)
 {
 	arm_iommu_detach_device(dev);
 }
-EXPORT_SYMBOL_GPL(rockchip_drm_dma_detach_device);
 
-int rockchip_register_crtc_funcs(struct drm_device *dev,
-				 const struct rockchip_crtc_funcs *crtc_funcs,
-				 int pipe)
+int rockchip_register_crtc_funcs(struct drm_crtc *crtc,
+				 const struct rockchip_crtc_funcs *crtc_funcs)
 {
-	struct rockchip_drm_private *priv = dev->dev_private;
+	int pipe = drm_crtc_index(crtc);
+	struct rockchip_drm_private *priv = crtc->dev->dev_private;
 
 	if (pipe > ROCKCHIP_MAX_CRTC)
 		return -EINVAL;
@@ -77,18 +75,17 @@ int rockchip_register_crtc_funcs(struct drm_device *dev,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(rockchip_register_crtc_funcs);
 
-void rockchip_unregister_crtc_funcs(struct drm_device *dev, int pipe)
+void rockchip_unregister_crtc_funcs(struct drm_crtc *crtc)
 {
-	struct rockchip_drm_private *priv = dev->dev_private;
+	int pipe = drm_crtc_index(crtc);
+	struct rockchip_drm_private *priv = crtc->dev->dev_private;
 
 	if (pipe > ROCKCHIP_MAX_CRTC)
 		return;
 
 	priv->crtc_funcs[pipe] = NULL;
 }
-EXPORT_SYMBOL_GPL(rockchip_unregister_crtc_funcs);
 
 static struct drm_crtc *rockchip_crtc_from_pipe(struct drm_device *drm,
 						int pipe)
@@ -139,6 +136,9 @@ static int rockchip_drm_load(struct drm_device *drm_dev, unsigned long flags)
 	if (!private)
 		return -ENOMEM;
 
+	mutex_init(&private->commit.lock);
+	INIT_WORK(&private->commit.work, rockchip_drm_atomic_work);
+
 	drm_dev->dev_private = private;
 
 	drm_mode_config_init(drm_dev);
@@ -212,6 +212,8 @@ static int rockchip_drm_load(struct drm_device *drm_dev, unsigned long flags)
 	 */
 	drm_dev->vblank_disable_allowed = true;
 
+	drm_mode_config_reset(drm_dev);
+
 	ret = rockchip_drm_fbdev_init(drm_dev);
 	if (ret)
 		goto err_vblank_cleanup;
@@ -275,7 +277,8 @@ const struct vm_operations_struct rockchip_drm_vm_ops = {
 };
 
 static struct drm_driver rockchip_drm_driver = {
-	.driver_features	= DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
+	.driver_features	= DRIVER_MODESET | DRIVER_GEM |
+				  DRIVER_PRIME | DRIVER_ATOMIC,
 	.load			= rockchip_drm_load,
 	.unload			= rockchip_drm_unload,
 	.lastclose		= rockchip_drm_lastclose,
@@ -450,10 +453,6 @@ static int rockchip_drm_bind(struct device *dev)
 	if (!drm)
 		return -ENOMEM;
 
-	ret = drm_dev_set_unique(drm, "%s", dev_name(dev));
-	if (ret)
-		goto err_free;
-
 	ret = drm_dev_register(drm, 0);
 	if (ret)
 		goto err_free;
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_drv.h b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
index dc4e5f03ac79..bb8b076f1dbb 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_drv.h
@@ -18,6 +18,7 @@
 #define _ROCKCHIP_DRM_DRV_H
 
 #include <drm/drm_fb_helper.h>
+#include <drm/drm_atomic_helper.h>
 #include <drm/drm_gem.h>
 
 #include <linux/module.h>
@@ -38,6 +39,14 @@ struct drm_connector;
 struct rockchip_crtc_funcs {
 	int (*enable_vblank)(struct drm_crtc *crtc);
 	void (*disable_vblank)(struct drm_crtc *crtc);
+	void (*wait_for_update)(struct drm_crtc *crtc);
+};
+
+struct rockchip_atomic_commit {
+	struct work_struct	work;
+	struct drm_atomic_state *state;
+	struct drm_device *dev;
+	struct mutex lock;
 };
 
 /*
@@ -50,12 +59,14 @@ struct rockchip_drm_private {
 	struct drm_fb_helper fbdev_helper;
 	struct drm_gem_object *fbdev_bo;
 	const struct rockchip_crtc_funcs *crtc_funcs[ROCKCHIP_MAX_CRTC];
+
+	struct rockchip_atomic_commit commit;
 };
 
-int rockchip_register_crtc_funcs(struct drm_device *dev,
-				 const struct rockchip_crtc_funcs *crtc_funcs,
-				 int pipe);
-void rockchip_unregister_crtc_funcs(struct drm_device *dev, int pipe);
+void rockchip_drm_atomic_work(struct work_struct *work);
+int rockchip_register_crtc_funcs(struct drm_crtc *crtc,
+				 const struct rockchip_crtc_funcs *crtc_funcs);
+void rockchip_unregister_crtc_funcs(struct drm_crtc *crtc);
 int rockchip_drm_encoder_get_mux_id(struct device_node *node,
 				    struct drm_encoder *encoder);
 int rockchip_drm_crtc_mode_config(struct drm_crtc *crtc, int connector_type,
@@ -64,5 +75,4 @@ int rockchip_drm_dma_attach_device(struct drm_device *drm_dev,
 				   struct device *dev);
 void rockchip_drm_dma_detach_device(struct drm_device *drm_dev,
 				    struct device *dev);
-
 #endif /* _ROCKCHIP_DRM_DRV_H_ */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fb.c b/drivers/gpu/drm/rockchip/rockchip_drm_fb.c
index 002645bb5bbf..3b8f652698f8 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_fb.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fb.c
@@ -15,6 +15,7 @@
 #include <linux/kernel.h>
 #include <drm/drm.h>
 #include <drm/drmP.h>
+#include <drm/drm_atomic.h>
 #include <drm/drm_fb_helper.h>
 #include <drm/drm_crtc_helper.h>
 
@@ -38,7 +39,6 @@ struct drm_gem_object *rockchip_fb_get_gem_obj(struct drm_framebuffer *fb,
 
 	return rk_fb->obj[plane];
 }
-EXPORT_SYMBOL_GPL(rockchip_fb_get_gem_obj);
 
 static void rockchip_drm_fb_destroy(struct drm_framebuffer *fb)
 {
@@ -66,13 +66,13 @@ static int rockchip_drm_fb_create_handle(struct drm_framebuffer *fb,
 				     rockchip_fb->obj[0], handle);
 }
 
-static struct drm_framebuffer_funcs rockchip_drm_fb_funcs = {
+static const struct drm_framebuffer_funcs rockchip_drm_fb_funcs = {
 	.destroy	= rockchip_drm_fb_destroy,
 	.create_handle	= rockchip_drm_fb_create_handle,
 };
 
 static struct rockchip_drm_fb *
-rockchip_fb_alloc(struct drm_device *dev, struct drm_mode_fb_cmd2 *mode_cmd,
+rockchip_fb_alloc(struct drm_device *dev, const struct drm_mode_fb_cmd2 *mode_cmd,
 		  struct drm_gem_object **obj, unsigned int num_planes)
 {
 	struct rockchip_drm_fb *rockchip_fb;
@@ -102,7 +102,7 @@ rockchip_fb_alloc(struct drm_device *dev, struct drm_mode_fb_cmd2 *mode_cmd,
 
 static struct drm_framebuffer *
 rockchip_user_fb_create(struct drm_device *dev, struct drm_file *file_priv,
-			struct drm_mode_fb_cmd2 *mode_cmd)
+			const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct rockchip_drm_fb *rockchip_fb;
 	struct drm_gem_object *objs[ROCKCHIP_MAX_FB_BUFFER];
@@ -166,14 +166,155 @@ static void rockchip_drm_output_poll_changed(struct drm_device *dev)
 		drm_fb_helper_hotplug_event(fb_helper);
 }
 
+static void rockchip_crtc_wait_for_update(struct drm_crtc *crtc)
+{
+	struct rockchip_drm_private *priv = crtc->dev->dev_private;
+	int pipe = drm_crtc_index(crtc);
+	const struct rockchip_crtc_funcs *crtc_funcs = priv->crtc_funcs[pipe];
+
+	if (crtc_funcs && crtc_funcs->wait_for_update)
+		crtc_funcs->wait_for_update(crtc);
+}
+
+/*
+ * We can't use drm_atomic_helper_wait_for_vblanks() because rk3288 and rk3066
+ * have hardware counters for neither vblanks nor scanlines, which results in
+ * a race where:
+ *				| <-- HW vsync irq and reg take effect
+ *	       plane_commit --> |
+ *	get_vblank and wait --> |
+ *				| <-- handle_vblank, vblank->count + 1
+ *		 cleanup_fb --> |
+ *		iommu crash --> |
+ *				| <-- HW vsync irq and reg take effect
+ *
+ * This function is equivalent but uses rockchip_crtc_wait_for_update() instead
+ * of waiting for vblank_count to change.
+ */
+static void
+rockchip_atomic_wait_for_complete(struct drm_device *dev, struct drm_atomic_state *old_state)
+{
+	struct drm_crtc_state *old_crtc_state;
+	struct drm_crtc *crtc;
+	int i, ret;
+
+	for_each_crtc_in_state(old_state, crtc, old_crtc_state, i) {
+		/* No one cares about the old state, so abuse it for tracking
+		 * and store whether we hold a vblank reference (and should do a
+		 * vblank wait) in the ->enable boolean.
+		 */
+		old_crtc_state->enable = false;
+
+		if (!crtc->state->active)
+			continue;
+
+		if (!drm_atomic_helper_framebuffer_changed(dev,
+				old_state, crtc))
+			continue;
+
+		ret = drm_crtc_vblank_get(crtc);
+		if (ret != 0)
+			continue;
+
+		old_crtc_state->enable = true;
+	}
+
+	for_each_crtc_in_state(old_state, crtc, old_crtc_state, i) {
+		if (!old_crtc_state->enable)
+			continue;
+
+		rockchip_crtc_wait_for_update(crtc);
+		drm_crtc_vblank_put(crtc);
+	}
+}
+
+static void
+rockchip_atomic_commit_complete(struct rockchip_atomic_commit *commit)
+{
+	struct drm_atomic_state *state = commit->state;
+	struct drm_device *dev = commit->dev;
+
+	/*
+	 * TODO: do fence wait here.
+	 */
+
+	/*
+	 * Rockchip crtc support runtime PM, can't update display planes
+	 * when crtc is disabled.
+	 *
+	 * drm_atomic_helper_commit comments detail that:
+	 *     For drivers supporting runtime PM the recommended sequence is
+	 *
+	 *     drm_atomic_helper_commit_modeset_disables(dev, state);
+	 *
+	 *     drm_atomic_helper_commit_modeset_enables(dev, state);
+	 *
+	 *     drm_atomic_helper_commit_planes(dev, state, true);
+	 *
+	 * See the kerneldoc entries for these three functions for more details.
+	 */
+	drm_atomic_helper_commit_modeset_disables(dev, state);
+
+	drm_atomic_helper_commit_modeset_enables(dev, state);
+
+	drm_atomic_helper_commit_planes(dev, state, true);
+
+	rockchip_atomic_wait_for_complete(dev, state);
+
+	drm_atomic_helper_cleanup_planes(dev, state);
+
+	drm_atomic_state_free(state);
+}
+
+void rockchip_drm_atomic_work(struct work_struct *work)
+{
+	struct rockchip_atomic_commit *commit = container_of(work,
+					struct rockchip_atomic_commit, work);
+
+	rockchip_atomic_commit_complete(commit);
+}
+
+int rockchip_drm_atomic_commit(struct drm_device *dev,
+			       struct drm_atomic_state *state,
+			       bool async)
+{
+	struct rockchip_drm_private *private = dev->dev_private;
+	struct rockchip_atomic_commit *commit = &private->commit;
+	int ret;
+
+	ret = drm_atomic_helper_prepare_planes(dev, state);
+	if (ret)
+		return ret;
+
+	/* serialize outstanding asynchronous commits */
+	mutex_lock(&commit->lock);
+	flush_work(&commit->work);
+
+	drm_atomic_helper_swap_state(dev, state);
+
+	commit->dev = dev;
+	commit->state = state;
+
+	if (async)
+		schedule_work(&commit->work);
+	else
+		rockchip_atomic_commit_complete(commit);
+
+	mutex_unlock(&commit->lock);
+
+	return 0;
+}
+
 static const struct drm_mode_config_funcs rockchip_drm_mode_config_funcs = {
 	.fb_create = rockchip_user_fb_create,
 	.output_poll_changed = rockchip_drm_output_poll_changed,
+	.atomic_check = drm_atomic_helper_check,
+	.atomic_commit = rockchip_drm_atomic_commit,
 };
 
 struct drm_framebuffer *
 rockchip_drm_framebuffer_init(struct drm_device *dev,
-			      struct drm_mode_fb_cmd2 *mode_cmd,
+			      const struct drm_mode_fb_cmd2 *mode_cmd,
 			      struct drm_gem_object *obj)
 {
 	struct rockchip_drm_fb *rockchip_fb;
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fb.h b/drivers/gpu/drm/rockchip/rockchip_drm_fb.h
index 09574d48226f..2fe47f1ee98f 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_fb.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fb.h
@@ -17,7 +17,7 @@
 
 struct drm_framebuffer *
 rockchip_drm_framebuffer_init(struct drm_device *dev,
-			      struct drm_mode_fb_cmd2 *mode_cmd,
+			      const struct drm_mode_fb_cmd2 *mode_cmd,
 			      struct drm_gem_object *obj);
 void rockchip_drm_framebuffer_fini(struct drm_framebuffer *fb);
 
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h
index 50432e9b5b37..73718c5f5bbf 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_fbdev.h
@@ -15,7 +15,18 @@
 #ifndef _ROCKCHIP_DRM_FBDEV_H
 #define _ROCKCHIP_DRM_FBDEV_H
 
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 int rockchip_drm_fbdev_init(struct drm_device *dev);
 void rockchip_drm_fbdev_fini(struct drm_device *dev);
+#else
+static inline int rockchip_drm_fbdev_init(struct drm_device *dev)
+{
+	return 0;
+}
+
+static inline void rockchip_drm_fbdev_fini(struct drm_device *dev)
+{
+}
+#endif
 
 #endif /* _ROCKCHIP_DRM_FBDEV_H */
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_gem.c b/drivers/gpu/drm/rockchip/rockchip_drm_gem.c
index d908321b94ce..18e07338c6e5 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_gem.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_gem.c
@@ -234,13 +234,8 @@ int rockchip_gem_dumb_create(struct drm_file *file_priv,
 	/*
 	 * align to 64 bytes since Mali requires it.
 	 */
-	min_pitch = ALIGN(min_pitch, 64);
-
-	if (args->pitch < min_pitch)
-		args->pitch = min_pitch;
-
-	if (args->size < args->pitch * args->height)
-		args->size = args->pitch * args->height;
+	args->pitch = ALIGN(min_pitch, 64);
+	args->size = args->pitch * args->height;
 
 	rk_obj = rockchip_gem_create_with_handle(file_priv, dev, args->size,
 						 &args->handle);
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
index 03c47eeadc81..fd370548d7d7 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.c
@@ -14,6 +14,7 @@
 
 #include <drm/drm.h>
 #include <drm/drmP.h>
+#include <drm/drm_atomic.h>
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_plane_helper.h>
@@ -35,11 +36,6 @@
 #include "rockchip_drm_fb.h"
 #include "rockchip_drm_vop.h"
 
-#define VOP_REG(off, _mask, s) \
-		{.offset = off, \
-		 .mask = _mask, \
-		 .shift = s,}
-
 #define __REG_SET_RELAXED(x, off, mask, shift, v) \
 		vop_mask_write_relaxed(x, off, (mask) << shift, (v) << shift)
 #define __REG_SET_NORMAL(x, off, mask, shift, v) \
@@ -47,14 +43,37 @@
 
 #define REG_SET(x, base, reg, v, mode) \
 		__REG_SET_##mode(x, base + reg.offset, reg.mask, reg.shift, v)
+#define REG_SET_MASK(x, base, reg, mask, v, mode) \
+		__REG_SET_##mode(x, base + reg.offset, mask, reg.shift, v)
 
 #define VOP_WIN_SET(x, win, name, v) \
 		REG_SET(x, win->base, win->phy->name, v, RELAXED)
 #define VOP_SCL_SET(x, win, name, v) \
 		REG_SET(x, win->base, win->phy->scl->name, v, RELAXED)
+#define VOP_SCL_SET_EXT(x, win, name, v) \
+		REG_SET(x, win->base, win->phy->scl->ext->name, v, RELAXED)
 #define VOP_CTRL_SET(x, name, v) \
 		REG_SET(x, 0, (x)->data->ctrl->name, v, NORMAL)
 
+#define VOP_INTR_GET(vop, name) \
+		vop_read_reg(vop, 0, &vop->data->ctrl->name)
+
+#define VOP_INTR_SET(vop, name, mask, v) \
+		REG_SET_MASK(vop, 0, vop->data->intr->name, mask, v, NORMAL)
+#define VOP_INTR_SET_TYPE(vop, name, type, v) \
+	do { \
+		int i, reg = 0, mask = 0; \
+		for (i = 0; i < vop->data->intr->nintrs; i++) { \
+			if (vop->data->intr->intrs[i] & type) { \
+				reg |= (v) << i; \
+				mask |= 1 << i; \
+			} \
+		} \
+		VOP_INTR_SET(vop, name, mask, reg); \
+	} while (0)
+#define VOP_INTR_GET_TYPE(vop, name, type) \
+		vop_get_intr_type(vop, &vop->data->intr->name, type)
+
 #define VOP_WIN_GET(x, win, name) \
 		vop_read_reg(x, win->base, &win->phy->name)
 
@@ -63,12 +82,15 @@
 
 #define to_vop(x) container_of(x, struct vop, crtc)
 #define to_vop_win(x) container_of(x, struct vop_win, base)
+#define to_vop_plane_state(x) container_of(x, struct vop_plane_state, base)
 
-struct vop_win_state {
-	struct list_head head;
-	struct drm_framebuffer *fb;
+struct vop_plane_state {
+	struct drm_plane_state base;
+	int format;
+	struct drm_rect src;
+	struct drm_rect dest;
 	dma_addr_t yrgb_mst;
-	struct drm_pending_vblank_event *event;
+	bool enable;
 };
 
 struct vop_win {
@@ -76,8 +98,7 @@ struct vop_win {
 	const struct vop_win_data *data;
 	struct vop *vop;
 
-	struct list_head pending;
-	struct vop_win_state *active;
+	struct vop_plane_state state;
 };
 
 struct vop {
@@ -86,13 +107,12 @@ struct vop {
 	struct drm_device *drm_dev;
 	bool is_enabled;
 
-	int connector_type;
-	int connector_out_mode;
-
 	/* mutex vsync_ work */
 	struct mutex vsync_mutex;
 	bool vsync_work_pending;
 	struct completion dsp_hold_completion;
+	struct completion wait_update_complete;
+	struct drm_pending_vblank_event *event;
 
 	const struct vop_data *data;
 
@@ -119,263 +139,9 @@ struct vop {
 	/* vop dclk reset */
 	struct reset_control *dclk_rst;
 
-	int pipe;
-
 	struct vop_win win[];
 };
 
-enum vop_data_format {
-	VOP_FMT_ARGB8888 = 0,
-	VOP_FMT_RGB888,
-	VOP_FMT_RGB565,
-	VOP_FMT_YUV420SP = 4,
-	VOP_FMT_YUV422SP,
-	VOP_FMT_YUV444SP,
-};
-
-struct vop_reg_data {
-	uint32_t offset;
-	uint32_t value;
-};
-
-struct vop_reg {
-	uint32_t offset;
-	uint32_t shift;
-	uint32_t mask;
-};
-
-struct vop_ctrl {
-	struct vop_reg standby;
-	struct vop_reg data_blank;
-	struct vop_reg gate_en;
-	struct vop_reg mmu_en;
-	struct vop_reg rgb_en;
-	struct vop_reg edp_en;
-	struct vop_reg hdmi_en;
-	struct vop_reg mipi_en;
-	struct vop_reg out_mode;
-	struct vop_reg dither_down;
-	struct vop_reg dither_up;
-	struct vop_reg pin_pol;
-
-	struct vop_reg htotal_pw;
-	struct vop_reg hact_st_end;
-	struct vop_reg vtotal_pw;
-	struct vop_reg vact_st_end;
-	struct vop_reg hpost_st_end;
-	struct vop_reg vpost_st_end;
-};
-
-struct vop_scl_regs {
-	struct vop_reg cbcr_vsd_mode;
-	struct vop_reg cbcr_vsu_mode;
-	struct vop_reg cbcr_hsd_mode;
-	struct vop_reg cbcr_ver_scl_mode;
-	struct vop_reg cbcr_hor_scl_mode;
-	struct vop_reg yrgb_vsd_mode;
-	struct vop_reg yrgb_vsu_mode;
-	struct vop_reg yrgb_hsd_mode;
-	struct vop_reg yrgb_ver_scl_mode;
-	struct vop_reg yrgb_hor_scl_mode;
-	struct vop_reg line_load_mode;
-	struct vop_reg cbcr_axi_gather_num;
-	struct vop_reg yrgb_axi_gather_num;
-	struct vop_reg vsd_cbcr_gt2;
-	struct vop_reg vsd_cbcr_gt4;
-	struct vop_reg vsd_yrgb_gt2;
-	struct vop_reg vsd_yrgb_gt4;
-	struct vop_reg bic_coe_sel;
-	struct vop_reg cbcr_axi_gather_en;
-	struct vop_reg yrgb_axi_gather_en;
-
-	struct vop_reg lb_mode;
-	struct vop_reg scale_yrgb_x;
-	struct vop_reg scale_yrgb_y;
-	struct vop_reg scale_cbcr_x;
-	struct vop_reg scale_cbcr_y;
-};
-
-struct vop_win_phy {
-	const struct vop_scl_regs *scl;
-	const uint32_t *data_formats;
-	uint32_t nformats;
-
-	struct vop_reg enable;
-	struct vop_reg format;
-	struct vop_reg rb_swap;
-	struct vop_reg act_info;
-	struct vop_reg dsp_info;
-	struct vop_reg dsp_st;
-	struct vop_reg yrgb_mst;
-	struct vop_reg uv_mst;
-	struct vop_reg yrgb_vir;
-	struct vop_reg uv_vir;
-
-	struct vop_reg dst_alpha_ctl;
-	struct vop_reg src_alpha_ctl;
-};
-
-struct vop_win_data {
-	uint32_t base;
-	const struct vop_win_phy *phy;
-	enum drm_plane_type type;
-};
-
-struct vop_data {
-	const struct vop_reg_data *init_table;
-	unsigned int table_size;
-	const struct vop_ctrl *ctrl;
-	const struct vop_win_data *win;
-	unsigned int win_size;
-};
-
-static const uint32_t formats_01[] = {
-	DRM_FORMAT_XRGB8888,
-	DRM_FORMAT_ARGB8888,
-	DRM_FORMAT_XBGR8888,
-	DRM_FORMAT_ABGR8888,
-	DRM_FORMAT_RGB888,
-	DRM_FORMAT_BGR888,
-	DRM_FORMAT_RGB565,
-	DRM_FORMAT_BGR565,
-	DRM_FORMAT_NV12,
-	DRM_FORMAT_NV16,
-	DRM_FORMAT_NV24,
-};
-
-static const uint32_t formats_234[] = {
-	DRM_FORMAT_XRGB8888,
-	DRM_FORMAT_ARGB8888,
-	DRM_FORMAT_XBGR8888,
-	DRM_FORMAT_ABGR8888,
-	DRM_FORMAT_RGB888,
-	DRM_FORMAT_BGR888,
-	DRM_FORMAT_RGB565,
-	DRM_FORMAT_BGR565,
-};
-
-static const struct vop_scl_regs win_full_scl = {
-	.cbcr_vsd_mode = VOP_REG(WIN0_CTRL1, 0x1, 31),
-	.cbcr_vsu_mode = VOP_REG(WIN0_CTRL1, 0x1, 30),
-	.cbcr_hsd_mode = VOP_REG(WIN0_CTRL1, 0x3, 28),
-	.cbcr_ver_scl_mode = VOP_REG(WIN0_CTRL1, 0x3, 26),
-	.cbcr_hor_scl_mode = VOP_REG(WIN0_CTRL1, 0x3, 24),
-	.yrgb_vsd_mode = VOP_REG(WIN0_CTRL1, 0x1, 23),
-	.yrgb_vsu_mode = VOP_REG(WIN0_CTRL1, 0x1, 22),
-	.yrgb_hsd_mode = VOP_REG(WIN0_CTRL1, 0x3, 20),
-	.yrgb_ver_scl_mode = VOP_REG(WIN0_CTRL1, 0x3, 18),
-	.yrgb_hor_scl_mode = VOP_REG(WIN0_CTRL1, 0x3, 16),
-	.line_load_mode = VOP_REG(WIN0_CTRL1, 0x1, 15),
-	.cbcr_axi_gather_num = VOP_REG(WIN0_CTRL1, 0x7, 12),
-	.yrgb_axi_gather_num = VOP_REG(WIN0_CTRL1, 0xf, 8),
-	.vsd_cbcr_gt2 = VOP_REG(WIN0_CTRL1, 0x1, 7),
-	.vsd_cbcr_gt4 = VOP_REG(WIN0_CTRL1, 0x1, 6),
-	.vsd_yrgb_gt2 = VOP_REG(WIN0_CTRL1, 0x1, 5),
-	.vsd_yrgb_gt4 = VOP_REG(WIN0_CTRL1, 0x1, 4),
-	.bic_coe_sel = VOP_REG(WIN0_CTRL1, 0x3, 2),
-	.cbcr_axi_gather_en = VOP_REG(WIN0_CTRL1, 0x1, 1),
-	.yrgb_axi_gather_en = VOP_REG(WIN0_CTRL1, 0x1, 0),
-	.lb_mode = VOP_REG(WIN0_CTRL0, 0x7, 5),
-	.scale_yrgb_x = VOP_REG(WIN0_SCL_FACTOR_YRGB, 0xffff, 0x0),
-	.scale_yrgb_y = VOP_REG(WIN0_SCL_FACTOR_YRGB, 0xffff, 16),
-	.scale_cbcr_x = VOP_REG(WIN0_SCL_FACTOR_CBR, 0xffff, 0x0),
-	.scale_cbcr_y = VOP_REG(WIN0_SCL_FACTOR_CBR, 0xffff, 16),
-};
-
-static const struct vop_win_phy win01_data = {
-	.scl = &win_full_scl,
-	.data_formats = formats_01,
-	.nformats = ARRAY_SIZE(formats_01),
-	.enable = VOP_REG(WIN0_CTRL0, 0x1, 0),
-	.format = VOP_REG(WIN0_CTRL0, 0x7, 1),
-	.rb_swap = VOP_REG(WIN0_CTRL0, 0x1, 12),
-	.act_info = VOP_REG(WIN0_ACT_INFO, 0x1fff1fff, 0),
-	.dsp_info = VOP_REG(WIN0_DSP_INFO, 0x0fff0fff, 0),
-	.dsp_st = VOP_REG(WIN0_DSP_ST, 0x1fff1fff, 0),
-	.yrgb_mst = VOP_REG(WIN0_YRGB_MST, 0xffffffff, 0),
-	.uv_mst = VOP_REG(WIN0_CBR_MST, 0xffffffff, 0),
-	.yrgb_vir = VOP_REG(WIN0_VIR, 0x3fff, 0),
-	.uv_vir = VOP_REG(WIN0_VIR, 0x3fff, 16),
-	.src_alpha_ctl = VOP_REG(WIN0_SRC_ALPHA_CTRL, 0xff, 0),
-	.dst_alpha_ctl = VOP_REG(WIN0_DST_ALPHA_CTRL, 0xff, 0),
-};
-
-static const struct vop_win_phy win23_data = {
-	.data_formats = formats_234,
-	.nformats = ARRAY_SIZE(formats_234),
-	.enable = VOP_REG(WIN2_CTRL0, 0x1, 0),
-	.format = VOP_REG(WIN2_CTRL0, 0x7, 1),
-	.rb_swap = VOP_REG(WIN2_CTRL0, 0x1, 12),
-	.dsp_info = VOP_REG(WIN2_DSP_INFO0, 0x0fff0fff, 0),
-	.dsp_st = VOP_REG(WIN2_DSP_ST0, 0x1fff1fff, 0),
-	.yrgb_mst = VOP_REG(WIN2_MST0, 0xffffffff, 0),
-	.yrgb_vir = VOP_REG(WIN2_VIR0_1, 0x1fff, 0),
-	.src_alpha_ctl = VOP_REG(WIN2_SRC_ALPHA_CTRL, 0xff, 0),
-	.dst_alpha_ctl = VOP_REG(WIN2_DST_ALPHA_CTRL, 0xff, 0),
-};
-
-static const struct vop_ctrl ctrl_data = {
-	.standby = VOP_REG(SYS_CTRL, 0x1, 22),
-	.gate_en = VOP_REG(SYS_CTRL, 0x1, 23),
-	.mmu_en = VOP_REG(SYS_CTRL, 0x1, 20),
-	.rgb_en = VOP_REG(SYS_CTRL, 0x1, 12),
-	.hdmi_en = VOP_REG(SYS_CTRL, 0x1, 13),
-	.edp_en = VOP_REG(SYS_CTRL, 0x1, 14),
-	.mipi_en = VOP_REG(SYS_CTRL, 0x1, 15),
-	.dither_down = VOP_REG(DSP_CTRL1, 0xf, 1),
-	.dither_up = VOP_REG(DSP_CTRL1, 0x1, 6),
-	.data_blank = VOP_REG(DSP_CTRL0, 0x1, 19),
-	.out_mode = VOP_REG(DSP_CTRL0, 0xf, 0),
-	.pin_pol = VOP_REG(DSP_CTRL0, 0xf, 4),
-	.htotal_pw = VOP_REG(DSP_HTOTAL_HS_END, 0x1fff1fff, 0),
-	.hact_st_end = VOP_REG(DSP_HACT_ST_END, 0x1fff1fff, 0),
-	.vtotal_pw = VOP_REG(DSP_VTOTAL_VS_END, 0x1fff1fff, 0),
-	.vact_st_end = VOP_REG(DSP_VACT_ST_END, 0x1fff1fff, 0),
-	.hpost_st_end = VOP_REG(POST_DSP_HACT_INFO, 0x1fff1fff, 0),
-	.vpost_st_end = VOP_REG(POST_DSP_VACT_INFO, 0x1fff1fff, 0),
-};
-
-static const struct vop_reg_data vop_init_reg_table[] = {
-	{SYS_CTRL, 0x00c00000},
-	{DSP_CTRL0, 0x00000000},
-	{WIN0_CTRL0, 0x00000080},
-	{WIN1_CTRL0, 0x00000080},
-	/* TODO: Win2/3 support multiple area function, but we haven't found
-	 * a suitable way to use it yet, so let's just use them as other windows
-	 * with only area 0 enabled.
-	 */
-	{WIN2_CTRL0, 0x00000010},
-	{WIN3_CTRL0, 0x00000010},
-};
-
-/*
- * Note: rk3288 has a dedicated 'cursor' window, however, that window requires
- * special support to get alpha blending working.  For now, just use overlay
- * window 3 for the drm cursor.
- *
- */
-static const struct vop_win_data rk3288_vop_win_data[] = {
-	{ .base = 0x00, .phy = &win01_data, .type = DRM_PLANE_TYPE_PRIMARY },
-	{ .base = 0x40, .phy = &win01_data, .type = DRM_PLANE_TYPE_OVERLAY },
-	{ .base = 0x00, .phy = &win23_data, .type = DRM_PLANE_TYPE_OVERLAY },
-	{ .base = 0x50, .phy = &win23_data, .type = DRM_PLANE_TYPE_CURSOR },
-};
-
-static const struct vop_data rk3288_vop = {
-	.init_table = vop_init_reg_table,
-	.table_size = ARRAY_SIZE(vop_init_reg_table),
-	.ctrl = &ctrl_data,
-	.win = rk3288_vop_win_data,
-	.win_size = ARRAY_SIZE(rk3288_vop_win_data),
-};
-
-static const struct of_device_id vop_driver_dt_match[] = {
-	{ .compatible = "rockchip,rk3288-vop",
-	  .data = &rk3288_vop },
-	{},
-};
-MODULE_DEVICE_TABLE(of, vop_driver_dt_match);
-
 static inline void vop_writel(struct vop *vop, uint32_t offset, uint32_t v)
 {
 	writel(v, vop->regs + offset);
@@ -393,11 +159,6 @@ static inline uint32_t vop_read_reg(struct vop *vop, uint32_t base,
 	return (vop_readl(vop, base + reg->offset) >> reg->shift) & reg->mask;
 }
 
-static inline void vop_cfg_done(struct vop *vop)
-{
-	writel(0x01, vop->regs + REG_CFG_DONE);
-}
-
 static inline void vop_mask_write(struct vop *vop, uint32_t offset,
 				  uint32_t mask, uint32_t v)
 {
@@ -422,6 +183,25 @@ static inline void vop_mask_write_relaxed(struct vop *vop, uint32_t offset,
 	}
 }
 
+static inline uint32_t vop_get_intr_type(struct vop *vop,
+					 const struct vop_reg *reg, int type)
+{
+	uint32_t i, ret = 0;
+	uint32_t regs = vop_read_reg(vop, 0, reg);
+
+	for (i = 0; i < vop->data->intr->nintrs; i++) {
+		if ((type & vop->data->intr->intrs[i]) && (regs & 1 << i))
+			ret |= vop->data->intr->intrs[i];
+	}
+
+	return ret;
+}
+
+static inline void vop_cfg_done(struct vop *vop)
+{
+	VOP_CTRL_SET(vop, cfg_done, 1);
+}
+
 static bool has_rb_swapped(uint32_t format)
 {
 	switch (format) {
@@ -537,6 +317,20 @@ static void scl_vop_cal_scl_fac(struct vop *vop, const struct vop_win_data *win,
 		return;
 	}
 
+	if (!win->phy->scl->ext) {
+		VOP_SCL_SET(vop, win, scale_yrgb_x,
+			    scl_cal_scale2(src_w, dst_w));
+		VOP_SCL_SET(vop, win, scale_yrgb_y,
+			    scl_cal_scale2(src_h, dst_h));
+		if (is_yuv) {
+			VOP_SCL_SET(vop, win, scale_cbcr_x,
+				    scl_cal_scale2(src_w, dst_w));
+			VOP_SCL_SET(vop, win, scale_cbcr_y,
+				    scl_cal_scale2(src_h, dst_h));
+		}
+		return;
+	}
+
 	yrgb_hor_scl_mode = scl_get_scl_mode(src_w, dst_w);
 	yrgb_ver_scl_mode = scl_get_scl_mode(src_h, dst_h);
 
@@ -554,7 +348,7 @@ static void scl_vop_cal_scl_fac(struct vop *vop, const struct vop_win_data *win,
 			lb_mode = scl_vop_cal_lb_mode(src_w, false);
 	}
 
-	VOP_SCL_SET(vop, win, lb_mode, lb_mode);
+	VOP_SCL_SET_EXT(vop, win, lb_mode, lb_mode);
 	if (lb_mode == LB_RGB_3840X2) {
 		if (yrgb_ver_scl_mode != SCALE_NONE) {
 			DRM_ERROR("ERROR : not allow yrgb ver scale\n");
@@ -578,14 +372,14 @@ static void scl_vop_cal_scl_fac(struct vop *vop, const struct vop_win_data *win,
 				false, vsu_mode, &vskiplines);
 	VOP_SCL_SET(vop, win, scale_yrgb_y, val);
 
-	VOP_SCL_SET(vop, win, vsd_yrgb_gt4, vskiplines == 4);
-	VOP_SCL_SET(vop, win, vsd_yrgb_gt2, vskiplines == 2);
+	VOP_SCL_SET_EXT(vop, win, vsd_yrgb_gt4, vskiplines == 4);
+	VOP_SCL_SET_EXT(vop, win, vsd_yrgb_gt2, vskiplines == 2);
 
-	VOP_SCL_SET(vop, win, yrgb_hor_scl_mode, yrgb_hor_scl_mode);
-	VOP_SCL_SET(vop, win, yrgb_ver_scl_mode, yrgb_ver_scl_mode);
-	VOP_SCL_SET(vop, win, yrgb_hsd_mode, SCALE_DOWN_BIL);
-	VOP_SCL_SET(vop, win, yrgb_vsd_mode, SCALE_DOWN_BIL);
-	VOP_SCL_SET(vop, win, yrgb_vsu_mode, vsu_mode);
+	VOP_SCL_SET_EXT(vop, win, yrgb_hor_scl_mode, yrgb_hor_scl_mode);
+	VOP_SCL_SET_EXT(vop, win, yrgb_ver_scl_mode, yrgb_ver_scl_mode);
+	VOP_SCL_SET_EXT(vop, win, yrgb_hsd_mode, SCALE_DOWN_BIL);
+	VOP_SCL_SET_EXT(vop, win, yrgb_vsd_mode, SCALE_DOWN_BIL);
+	VOP_SCL_SET_EXT(vop, win, yrgb_vsu_mode, vsu_mode);
 	if (is_yuv) {
 		val = scl_vop_cal_scale(cbcr_hor_scl_mode, cbcr_src_w,
 					dst_w, true, 0, NULL);
@@ -594,13 +388,13 @@ static void scl_vop_cal_scl_fac(struct vop *vop, const struct vop_win_data *win,
 					dst_h, false, vsu_mode, &vskiplines);
 		VOP_SCL_SET(vop, win, scale_cbcr_y, val);
 
-		VOP_SCL_SET(vop, win, vsd_cbcr_gt4, vskiplines == 4);
-		VOP_SCL_SET(vop, win, vsd_cbcr_gt2, vskiplines == 2);
-		VOP_SCL_SET(vop, win, cbcr_hor_scl_mode, cbcr_hor_scl_mode);
-		VOP_SCL_SET(vop, win, cbcr_ver_scl_mode, cbcr_ver_scl_mode);
-		VOP_SCL_SET(vop, win, cbcr_hsd_mode, SCALE_DOWN_BIL);
-		VOP_SCL_SET(vop, win, cbcr_vsd_mode, SCALE_DOWN_BIL);
-		VOP_SCL_SET(vop, win, cbcr_vsu_mode, vsu_mode);
+		VOP_SCL_SET_EXT(vop, win, vsd_cbcr_gt4, vskiplines == 4);
+		VOP_SCL_SET_EXT(vop, win, vsd_cbcr_gt2, vskiplines == 2);
+		VOP_SCL_SET_EXT(vop, win, cbcr_hor_scl_mode, cbcr_hor_scl_mode);
+		VOP_SCL_SET_EXT(vop, win, cbcr_ver_scl_mode, cbcr_ver_scl_mode);
+		VOP_SCL_SET_EXT(vop, win, cbcr_hsd_mode, SCALE_DOWN_BIL);
+		VOP_SCL_SET_EXT(vop, win, cbcr_vsd_mode, SCALE_DOWN_BIL);
+		VOP_SCL_SET_EXT(vop, win, cbcr_vsu_mode, vsu_mode);
 	}
 }
 
@@ -613,8 +407,7 @@ static void vop_dsp_hold_valid_irq_enable(struct vop *vop)
 
 	spin_lock_irqsave(&vop->irq_lock, flags);
 
-	vop_mask_write(vop, INTR_CTRL0, DSP_HOLD_VALID_INTR_MASK,
-		       DSP_HOLD_VALID_INTR_EN(1));
+	VOP_INTR_SET_TYPE(vop, enable, DSP_HOLD_VALID_INTR, 1);
 
 	spin_unlock_irqrestore(&vop->irq_lock, flags);
 }
@@ -628,8 +421,7 @@ static void vop_dsp_hold_valid_irq_disable(struct vop *vop)
 
 	spin_lock_irqsave(&vop->irq_lock, flags);
 
-	vop_mask_write(vop, INTR_CTRL0, DSP_HOLD_VALID_INTR_MASK,
-		       DSP_HOLD_VALID_INTR_EN(0));
+	VOP_INTR_SET_TYPE(vop, enable, DSP_HOLD_VALID_INTR, 0);
 
 	spin_unlock_irqrestore(&vop->irq_lock, flags);
 }
@@ -692,7 +484,7 @@ static void vop_enable(struct drm_crtc *crtc)
 
 	enable_irq(vop->irq);
 
-	drm_vblank_on(vop->drm_dev, vop->pipe);
+	drm_crtc_vblank_on(crtc);
 
 	return;
 
@@ -704,14 +496,14 @@ err_disable_hclk:
 	clk_disable(vop->hclk);
 }
 
-static void vop_disable(struct drm_crtc *crtc)
+static void vop_crtc_disable(struct drm_crtc *crtc)
 {
 	struct vop *vop = to_vop(crtc);
 
 	if (!vop->is_enabled)
 		return;
 
-	drm_vblank_off(crtc->dev, vop->pipe);
+	drm_crtc_vblank_off(crtc);
 
 	/*
 	 * Vop standby will take effect at end of current frame,
@@ -748,224 +540,188 @@ static void vop_disable(struct drm_crtc *crtc)
 	pm_runtime_put(vop->dev);
 }
 
-/*
- * Caller must hold vsync_mutex.
- */
-static struct drm_framebuffer *vop_win_last_pending_fb(struct vop_win *vop_win)
-{
-	struct vop_win_state *last;
-	struct vop_win_state *active = vop_win->active;
-
-	if (list_empty(&vop_win->pending))
-		return active ? active->fb : NULL;
-
-	last = list_last_entry(&vop_win->pending, struct vop_win_state, head);
-	return last ? last->fb : NULL;
-}
-
-/*
- * Caller must hold vsync_mutex.
- */
-static int vop_win_queue_fb(struct vop_win *vop_win,
-			    struct drm_framebuffer *fb, dma_addr_t yrgb_mst,
-			    struct drm_pending_vblank_event *event)
+static void vop_plane_destroy(struct drm_plane *plane)
 {
-	struct vop_win_state *state;
-
-	state = kzalloc(sizeof(*state), GFP_KERNEL);
-	if (!state)
-		return -ENOMEM;
-
-	state->fb = fb;
-	state->yrgb_mst = yrgb_mst;
-	state->event = event;
-
-	list_add_tail(&state->head, &vop_win->pending);
-
-	return 0;
+	drm_plane_cleanup(plane);
 }
 
-static int vop_update_plane_event(struct drm_plane *plane,
-				  struct drm_crtc *crtc,
-				  struct drm_framebuffer *fb, int crtc_x,
-				  int crtc_y, unsigned int crtc_w,
-				  unsigned int crtc_h, uint32_t src_x,
-				  uint32_t src_y, uint32_t src_w,
-				  uint32_t src_h,
-				  struct drm_pending_vblank_event *event)
+static int vop_plane_atomic_check(struct drm_plane *plane,
+			   struct drm_plane_state *state)
 {
+	struct drm_crtc *crtc = state->crtc;
+	struct drm_framebuffer *fb = state->fb;
 	struct vop_win *vop_win = to_vop_win(plane);
+	struct vop_plane_state *vop_plane_state = to_vop_plane_state(state);
 	const struct vop_win_data *win = vop_win->data;
-	struct vop *vop = to_vop(crtc);
-	struct drm_gem_object *obj;
-	struct rockchip_gem_object *rk_obj;
-	struct drm_gem_object *uv_obj;
-	struct rockchip_gem_object *rk_uv_obj;
-	unsigned long offset;
-	unsigned int actual_w;
-	unsigned int actual_h;
-	unsigned int dsp_stx;
-	unsigned int dsp_sty;
-	unsigned int y_vir_stride;
-	unsigned int uv_vir_stride = 0;
-	dma_addr_t yrgb_mst;
-	dma_addr_t uv_mst = 0;
-	enum vop_data_format format;
-	uint32_t val;
-	bool is_alpha;
-	bool rb_swap;
-	bool is_yuv;
 	bool visible;
 	int ret;
-	struct drm_rect dest = {
-		.x1 = crtc_x,
-		.y1 = crtc_y,
-		.x2 = crtc_x + crtc_w,
-		.y2 = crtc_y + crtc_h,
-	};
-	struct drm_rect src = {
-		/* 16.16 fixed point */
-		.x1 = src_x,
-		.y1 = src_y,
-		.x2 = src_x + src_w,
-		.y2 = src_y + src_h,
-	};
-	const struct drm_rect clip = {
-		.x2 = crtc->mode.hdisplay,
-		.y2 = crtc->mode.vdisplay,
-	};
-	bool can_position = plane->type != DRM_PLANE_TYPE_PRIMARY;
+	struct drm_rect *dest = &vop_plane_state->dest;
+	struct drm_rect *src = &vop_plane_state->src;
+	struct drm_rect clip;
 	int min_scale = win->phy->scl ? FRAC_16_16(1, 8) :
 					DRM_PLANE_HELPER_NO_SCALING;
 	int max_scale = win->phy->scl ? FRAC_16_16(8, 1) :
 					DRM_PLANE_HELPER_NO_SCALING;
 
-	ret = drm_plane_helper_check_update(plane, crtc, fb,
-					    &src, &dest, &clip,
+	crtc = crtc ? crtc : plane->state->crtc;
+	/*
+	 * Both crtc or plane->state->crtc can be null.
+	 */
+	if (!crtc || !fb)
+		goto out_disable;
+	src->x1 = state->src_x;
+	src->y1 = state->src_y;
+	src->x2 = state->src_x + state->src_w;
+	src->y2 = state->src_y + state->src_h;
+	dest->x1 = state->crtc_x;
+	dest->y1 = state->crtc_y;
+	dest->x2 = state->crtc_x + state->crtc_w;
+	dest->y2 = state->crtc_y + state->crtc_h;
+
+	clip.x1 = 0;
+	clip.y1 = 0;
+	clip.x2 = crtc->mode.hdisplay;
+	clip.y2 = crtc->mode.vdisplay;
+
+	ret = drm_plane_helper_check_update(plane, crtc, state->fb,
+					    src, dest, &clip,
 					    min_scale,
 					    max_scale,
-					    can_position, false, &visible);
+					    true, true, &visible);
 	if (ret)
 		return ret;
 
 	if (!visible)
-		return 0;
-
-	is_alpha = is_alpha_support(fb->pixel_format);
-	rb_swap = has_rb_swapped(fb->pixel_format);
-	is_yuv = is_yuv_support(fb->pixel_format);
+		goto out_disable;
 
-	format = vop_convert_format(fb->pixel_format);
-	if (format < 0)
-		return format;
+	vop_plane_state->format = vop_convert_format(fb->pixel_format);
+	if (vop_plane_state->format < 0)
+		return vop_plane_state->format;
 
-	obj = rockchip_fb_get_gem_obj(fb, 0);
-	if (!obj) {
-		DRM_ERROR("fail to get rockchip gem object from framebuffer\n");
+	/*
+	 * Src.x1 can be odd when do clip, but yuv plane start point
+	 * need align with 2 pixel.
+	 */
+	if (is_yuv_support(fb->pixel_format) && ((src->x1 >> 16) % 2))
 		return -EINVAL;
-	}
 
-	rk_obj = to_rockchip_obj(obj);
+	vop_plane_state->enable = true;
 
-	if (is_yuv) {
-		/*
-		 * Src.x1 can be odd when do clip, but yuv plane start point
-		 * need align with 2 pixel.
-		 */
-		val = (src.x1 >> 16) % 2;
-		src.x1 += val << 16;
-		src.x2 += val << 16;
-	}
+	return 0;
 
-	actual_w = (src.x2 - src.x1) >> 16;
-	actual_h = (src.y2 - src.y1) >> 16;
+out_disable:
+	vop_plane_state->enable = false;
+	return 0;
+}
 
-	dsp_stx = dest.x1 + crtc->mode.htotal - crtc->mode.hsync_start;
-	dsp_sty = dest.y1 + crtc->mode.vtotal - crtc->mode.vsync_start;
+static void vop_plane_atomic_disable(struct drm_plane *plane,
+				     struct drm_plane_state *old_state)
+{
+	struct vop_plane_state *vop_plane_state = to_vop_plane_state(old_state);
+	struct vop_win *vop_win = to_vop_win(plane);
+	const struct vop_win_data *win = vop_win->data;
+	struct vop *vop = to_vop(old_state->crtc);
 
-	offset = (src.x1 >> 16) * drm_format_plane_cpp(fb->pixel_format, 0);
-	offset += (src.y1 >> 16) * fb->pitches[0];
+	if (!old_state->crtc)
+		return;
 
-	yrgb_mst = rk_obj->dma_addr + offset + fb->offsets[0];
-	y_vir_stride = fb->pitches[0] >> 2;
+	spin_lock(&vop->reg_lock);
 
-	if (is_yuv) {
-		int hsub = drm_format_horz_chroma_subsampling(fb->pixel_format);
-		int vsub = drm_format_vert_chroma_subsampling(fb->pixel_format);
-		int bpp = drm_format_plane_cpp(fb->pixel_format, 1);
+	VOP_WIN_SET(vop, win, enable, 0);
 
-		uv_obj = rockchip_fb_get_gem_obj(fb, 1);
-		if (!uv_obj) {
-			DRM_ERROR("fail to get uv object from framebuffer\n");
-			return -EINVAL;
-		}
-		rk_uv_obj = to_rockchip_obj(uv_obj);
-		uv_vir_stride = fb->pitches[1] >> 2;
+	spin_unlock(&vop->reg_lock);
 
-		offset = (src.x1 >> 16) * bpp / hsub;
-		offset += (src.y1 >> 16) * fb->pitches[1] / vsub;
+	vop_plane_state->enable = false;
+}
 
-		uv_mst = rk_uv_obj->dma_addr + offset + fb->offsets[1];
-	}
+static void vop_plane_atomic_update(struct drm_plane *plane,
+		struct drm_plane_state *old_state)
+{
+	struct drm_plane_state *state = plane->state;
+	struct drm_crtc *crtc = state->crtc;
+	struct vop_win *vop_win = to_vop_win(plane);
+	struct vop_plane_state *vop_plane_state = to_vop_plane_state(state);
+	const struct vop_win_data *win = vop_win->data;
+	struct vop *vop = to_vop(state->crtc);
+	struct drm_framebuffer *fb = state->fb;
+	unsigned int actual_w, actual_h;
+	unsigned int dsp_stx, dsp_sty;
+	uint32_t act_info, dsp_info, dsp_st;
+	struct drm_rect *src = &vop_plane_state->src;
+	struct drm_rect *dest = &vop_plane_state->dest;
+	struct drm_gem_object *obj, *uv_obj;
+	struct rockchip_gem_object *rk_obj, *rk_uv_obj;
+	unsigned long offset;
+	dma_addr_t dma_addr;
+	uint32_t val;
+	bool rb_swap;
 
 	/*
-	 * If this plane update changes the plane's framebuffer, (or more
-	 * precisely, if this update has a different framebuffer than the last
-	 * update), enqueue it so we can track when it completes.
-	 *
-	 * Only when we discover that this update has completed, can we
-	 * unreference any previous framebuffers.
+	 * can't update plane when vop is disabled.
 	 */
-	mutex_lock(&vop->vsync_mutex);
-	if (fb != vop_win_last_pending_fb(vop_win)) {
-		ret = drm_vblank_get(plane->dev, vop->pipe);
-		if (ret) {
-			DRM_ERROR("failed to get vblank, %d\n", ret);
-			mutex_unlock(&vop->vsync_mutex);
-			return ret;
-		}
-
-		drm_framebuffer_reference(fb);
+	if (!crtc)
+		return;
 
-		ret = vop_win_queue_fb(vop_win, fb, yrgb_mst, event);
-		if (ret) {
-			drm_vblank_put(plane->dev, vop->pipe);
-			mutex_unlock(&vop->vsync_mutex);
-			return ret;
-		}
+	if (WARN_ON(!vop->is_enabled))
+		return;
 
-		vop->vsync_work_pending = true;
+	if (!vop_plane_state->enable) {
+		vop_plane_atomic_disable(plane, old_state);
+		return;
 	}
-	mutex_unlock(&vop->vsync_mutex);
+
+	obj = rockchip_fb_get_gem_obj(fb, 0);
+	rk_obj = to_rockchip_obj(obj);
+
+	actual_w = drm_rect_width(src) >> 16;
+	actual_h = drm_rect_height(src) >> 16;
+	act_info = (actual_h - 1) << 16 | ((actual_w - 1) & 0xffff);
+
+	dsp_info = (drm_rect_height(dest) - 1) << 16;
+	dsp_info |= (drm_rect_width(dest) - 1) & 0xffff;
+
+	dsp_stx = dest->x1 + crtc->mode.htotal - crtc->mode.hsync_start;
+	dsp_sty = dest->y1 + crtc->mode.vtotal - crtc->mode.vsync_start;
+	dsp_st = dsp_sty << 16 | (dsp_stx & 0xffff);
+
+	offset = (src->x1 >> 16) * drm_format_plane_cpp(fb->pixel_format, 0);
+	offset += (src->y1 >> 16) * fb->pitches[0];
+	vop_plane_state->yrgb_mst = rk_obj->dma_addr + offset + fb->offsets[0];
 
 	spin_lock(&vop->reg_lock);
 
-	VOP_WIN_SET(vop, win, format, format);
-	VOP_WIN_SET(vop, win, yrgb_vir, y_vir_stride);
-	VOP_WIN_SET(vop, win, yrgb_mst, yrgb_mst);
-	if (is_yuv) {
-		VOP_WIN_SET(vop, win, uv_vir, uv_vir_stride);
-		VOP_WIN_SET(vop, win, uv_mst, uv_mst);
+	VOP_WIN_SET(vop, win, format, vop_plane_state->format);
+	VOP_WIN_SET(vop, win, yrgb_vir, fb->pitches[0] >> 2);
+	VOP_WIN_SET(vop, win, yrgb_mst, vop_plane_state->yrgb_mst);
+	if (is_yuv_support(fb->pixel_format)) {
+		int hsub = drm_format_horz_chroma_subsampling(fb->pixel_format);
+		int vsub = drm_format_vert_chroma_subsampling(fb->pixel_format);
+		int bpp = drm_format_plane_cpp(fb->pixel_format, 1);
+
+		uv_obj = rockchip_fb_get_gem_obj(fb, 1);
+		rk_uv_obj = to_rockchip_obj(uv_obj);
+
+		offset = (src->x1 >> 16) * bpp / hsub;
+		offset += (src->y1 >> 16) * fb->pitches[1] / vsub;
+
+		dma_addr = rk_uv_obj->dma_addr + offset + fb->offsets[1];
+		VOP_WIN_SET(vop, win, uv_vir, fb->pitches[1] >> 2);
+		VOP_WIN_SET(vop, win, uv_mst, dma_addr);
 	}
 
 	if (win->phy->scl)
 		scl_vop_cal_scl_fac(vop, win, actual_w, actual_h,
-				    dest.x2 - dest.x1, dest.y2 - dest.y1,
+				    drm_rect_width(dest), drm_rect_height(dest),
 				    fb->pixel_format);
 
-	val = (actual_h - 1) << 16;
-	val |= (actual_w - 1) & 0xffff;
-	VOP_WIN_SET(vop, win, act_info, val);
+	VOP_WIN_SET(vop, win, act_info, act_info);
+	VOP_WIN_SET(vop, win, dsp_info, dsp_info);
+	VOP_WIN_SET(vop, win, dsp_st, dsp_st);
 
-	val = (dest.y2 - dest.y1 - 1) << 16;
-	val |= (dest.x2 - dest.x1 - 1) & 0xffff;
-	VOP_WIN_SET(vop, win, dsp_info, val);
-	val = dsp_sty << 16;
-	val |= dsp_stx & 0xffff;
-	VOP_WIN_SET(vop, win, dsp_st, val);
+	rb_swap = has_rb_swapped(fb->pixel_format);
 	VOP_WIN_SET(vop, win, rb_swap, rb_swap);
 
-	if (is_alpha) {
+	if (is_alpha_support(fb->pixel_format)) {
 		VOP_WIN_SET(vop, win, dst_alpha_ctl,
 			    DST_FACTOR_M0(ALPHA_SRC_INVERSE));
 		val = SRC_ALPHA_EN(1) | SRC_COLOR_M0(ALPHA_SRC_PRE_MUL) |
@@ -979,86 +735,70 @@ static int vop_update_plane_event(struct drm_plane *plane,
 	}
 
 	VOP_WIN_SET(vop, win, enable, 1);
-
-	vop_cfg_done(vop);
 	spin_unlock(&vop->reg_lock);
-
-	return 0;
 }
 
-static int vop_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
-			    struct drm_framebuffer *fb, int crtc_x, int crtc_y,
-			    unsigned int crtc_w, unsigned int crtc_h,
-			    uint32_t src_x, uint32_t src_y, uint32_t src_w,
-			    uint32_t src_h)
-{
-	return vop_update_plane_event(plane, crtc, fb, crtc_x, crtc_y, crtc_w,
-				      crtc_h, src_x, src_y, src_w, src_h,
-				      NULL);
-}
+static const struct drm_plane_helper_funcs plane_helper_funcs = {
+	.atomic_check = vop_plane_atomic_check,
+	.atomic_update = vop_plane_atomic_update,
+	.atomic_disable = vop_plane_atomic_disable,
+};
 
-static int vop_update_primary_plane(struct drm_crtc *crtc,
-				    struct drm_pending_vblank_event *event)
+void vop_atomic_plane_reset(struct drm_plane *plane)
 {
-	unsigned int crtc_w, crtc_h;
+	struct vop_plane_state *vop_plane_state =
+					to_vop_plane_state(plane->state);
 
-	crtc_w = crtc->primary->fb->width - crtc->x;
-	crtc_h = crtc->primary->fb->height - crtc->y;
+	if (plane->state && plane->state->fb)
+		drm_framebuffer_unreference(plane->state->fb);
 
-	return vop_update_plane_event(crtc->primary, crtc, crtc->primary->fb,
-				      0, 0, crtc_w, crtc_h, crtc->x << 16,
-				      crtc->y << 16, crtc_w << 16,
-				      crtc_h << 16, event);
+	kfree(vop_plane_state);
+	vop_plane_state = kzalloc(sizeof(*vop_plane_state), GFP_KERNEL);
+	if (!vop_plane_state)
+		return;
+
+	plane->state = &vop_plane_state->base;
+	plane->state->plane = plane;
 }
 
-static int vop_disable_plane(struct drm_plane *plane)
+struct drm_plane_state *
+vop_atomic_plane_duplicate_state(struct drm_plane *plane)
 {
-	struct vop_win *vop_win = to_vop_win(plane);
-	const struct vop_win_data *win = vop_win->data;
-	struct vop *vop;
-	int ret;
+	struct vop_plane_state *old_vop_plane_state;
+	struct vop_plane_state *vop_plane_state;
 
-	if (!plane->crtc)
-		return 0;
+	if (WARN_ON(!plane->state))
+		return NULL;
 
-	vop = to_vop(plane->crtc);
+	old_vop_plane_state = to_vop_plane_state(plane->state);
+	vop_plane_state = kmemdup(old_vop_plane_state,
+				  sizeof(*vop_plane_state), GFP_KERNEL);
+	if (!vop_plane_state)
+		return NULL;
 
-	ret = drm_vblank_get(plane->dev, vop->pipe);
-	if (ret) {
-		DRM_ERROR("failed to get vblank, %d\n", ret);
-		return ret;
-	}
+	__drm_atomic_helper_plane_duplicate_state(plane,
+						  &vop_plane_state->base);
 
-	mutex_lock(&vop->vsync_mutex);
-
-	ret = vop_win_queue_fb(vop_win, NULL, 0, NULL);
-	if (ret) {
-		drm_vblank_put(plane->dev, vop->pipe);
-		mutex_unlock(&vop->vsync_mutex);
-		return ret;
-	}
-
-	vop->vsync_work_pending = true;
-	mutex_unlock(&vop->vsync_mutex);
-
-	spin_lock(&vop->reg_lock);
-	VOP_WIN_SET(vop, win, enable, 0);
-	vop_cfg_done(vop);
-	spin_unlock(&vop->reg_lock);
-
-	return 0;
+	return &vop_plane_state->base;
 }
 
-static void vop_plane_destroy(struct drm_plane *plane)
+static void vop_atomic_plane_destroy_state(struct drm_plane *plane,
+					   struct drm_plane_state *state)
 {
-	vop_disable_plane(plane);
-	drm_plane_cleanup(plane);
+	struct vop_plane_state *vop_state = to_vop_plane_state(state);
+
+	__drm_atomic_helper_plane_destroy_state(plane, state);
+
+	kfree(vop_state);
 }
 
 static const struct drm_plane_funcs vop_plane_funcs = {
-	.update_plane = vop_update_plane,
-	.disable_plane = vop_disable_plane,
+	.update_plane	= drm_atomic_helper_update_plane,
+	.disable_plane	= drm_atomic_helper_disable_plane,
 	.destroy = vop_plane_destroy,
+	.reset = vop_atomic_plane_reset,
+	.atomic_duplicate_state = vop_atomic_plane_duplicate_state,
+	.atomic_destroy_state = vop_atomic_plane_destroy_state,
 };
 
 int rockchip_drm_crtc_mode_config(struct drm_crtc *crtc,
@@ -1067,8 +807,27 @@ int rockchip_drm_crtc_mode_config(struct drm_crtc *crtc,
 {
 	struct vop *vop = to_vop(crtc);
 
-	vop->connector_type = connector_type;
-	vop->connector_out_mode = out_mode;
+	if (WARN_ON(!vop->is_enabled))
+		return -EINVAL;
+
+	switch (connector_type) {
+	case DRM_MODE_CONNECTOR_LVDS:
+		VOP_CTRL_SET(vop, rgb_en, 1);
+		break;
+	case DRM_MODE_CONNECTOR_eDP:
+		VOP_CTRL_SET(vop, edp_en, 1);
+		break;
+	case DRM_MODE_CONNECTOR_HDMIA:
+		VOP_CTRL_SET(vop, hdmi_en, 1);
+		break;
+	case DRM_MODE_CONNECTOR_DSI:
+		VOP_CTRL_SET(vop, mipi_en, 1);
+		break;
+	default:
+		DRM_ERROR("unsupport connector_type[%d]\n", connector_type);
+		return -EINVAL;
+	};
+	VOP_CTRL_SET(vop, out_mode, out_mode);
 
 	return 0;
 }
@@ -1079,12 +838,12 @@ static int vop_crtc_enable_vblank(struct drm_crtc *crtc)
 	struct vop *vop = to_vop(crtc);
 	unsigned long flags;
 
-	if (!vop->is_enabled)
+	if (WARN_ON(!vop->is_enabled))
 		return -EPERM;
 
 	spin_lock_irqsave(&vop->irq_lock, flags);
 
-	vop_mask_write(vop, INTR_CTRL0, FS_INTR_MASK, FS_INTR_EN(1));
+	VOP_INTR_SET_TYPE(vop, enable, FS_INTR, 1);
 
 	spin_unlock_irqrestore(&vop->irq_lock, flags);
 
@@ -1096,76 +855,49 @@ static void vop_crtc_disable_vblank(struct drm_crtc *crtc)
 	struct vop *vop = to_vop(crtc);
 	unsigned long flags;
 
-	if (!vop->is_enabled)
+	if (WARN_ON(!vop->is_enabled))
 		return;
 
 	spin_lock_irqsave(&vop->irq_lock, flags);
-	vop_mask_write(vop, INTR_CTRL0, FS_INTR_MASK, FS_INTR_EN(0));
+
+	VOP_INTR_SET_TYPE(vop, enable, FS_INTR, 0);
+
 	spin_unlock_irqrestore(&vop->irq_lock, flags);
 }
 
-static const struct rockchip_crtc_funcs private_crtc_funcs = {
-	.enable_vblank = vop_crtc_enable_vblank,
-	.disable_vblank = vop_crtc_disable_vblank,
-};
-
-static void vop_crtc_dpms(struct drm_crtc *crtc, int mode)
+static void vop_crtc_wait_for_update(struct drm_crtc *crtc)
 {
-	DRM_DEBUG_KMS("crtc[%d] mode[%d]\n", crtc->base.id, mode);
+	struct vop *vop = to_vop(crtc);
 
-	switch (mode) {
-	case DRM_MODE_DPMS_ON:
-		vop_enable(crtc);
-		break;
-	case DRM_MODE_DPMS_STANDBY:
-	case DRM_MODE_DPMS_SUSPEND:
-	case DRM_MODE_DPMS_OFF:
-		vop_disable(crtc);
-		break;
-	default:
-		DRM_DEBUG_KMS("unspecified mode %d\n", mode);
-		break;
-	}
+	reinit_completion(&vop->wait_update_complete);
+	WARN_ON(!wait_for_completion_timeout(&vop->wait_update_complete, 100));
 }
 
-static void vop_crtc_prepare(struct drm_crtc *crtc)
-{
-	vop_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
-}
+static const struct rockchip_crtc_funcs private_crtc_funcs = {
+	.enable_vblank = vop_crtc_enable_vblank,
+	.disable_vblank = vop_crtc_disable_vblank,
+	.wait_for_update = vop_crtc_wait_for_update,
+};
 
 static bool vop_crtc_mode_fixup(struct drm_crtc *crtc,
 				const struct drm_display_mode *mode,
 				struct drm_display_mode *adjusted_mode)
 {
+	struct vop *vop = to_vop(crtc);
+
 	if (adjusted_mode->htotal == 0 || adjusted_mode->vtotal == 0)
 		return false;
 
-	return true;
-}
-
-static int vop_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
-				  struct drm_framebuffer *old_fb)
-{
-	int ret;
-
-	crtc->x = x;
-	crtc->y = y;
+	adjusted_mode->clock =
+		clk_round_rate(vop->dclk, mode->clock * 1000) / 1000;
 
-	ret = vop_update_primary_plane(crtc, NULL);
-	if (ret < 0) {
-		DRM_ERROR("fail to update plane\n");
-		return ret;
-	}
-
-	return 0;
+	return true;
 }
 
-static int vop_crtc_mode_set(struct drm_crtc *crtc,
-			     struct drm_display_mode *mode,
-			     struct drm_display_mode *adjusted_mode,
-			     int x, int y, struct drm_framebuffer *fb)
+static void vop_crtc_enable(struct drm_crtc *crtc)
 {
 	struct vop *vop = to_vop(crtc);
+	struct drm_display_mode *adjusted_mode = &crtc->state->adjusted_mode;
 	u16 hsync_len = adjusted_mode->hsync_end - adjusted_mode->hsync_start;
 	u16 hdisplay = adjusted_mode->hdisplay;
 	u16 htotal = adjusted_mode->htotal;
@@ -1176,32 +908,44 @@ static int vop_crtc_mode_set(struct drm_crtc *crtc,
 	u16 vsync_len = adjusted_mode->vsync_end - adjusted_mode->vsync_start;
 	u16 vact_st = adjusted_mode->vtotal - adjusted_mode->vsync_start;
 	u16 vact_end = vact_st + vdisplay;
-	int ret, ret_clk;
 	uint32_t val;
 
+	vop_enable(crtc);
 	/*
-	 * disable dclk to stop frame scan, so that we can safe config mode and
-	 * enable iommu.
+	 * If dclk rate is zero, mean that scanout is stop,
+	 * we don't need wait any more.
 	 */
-	clk_disable(vop->dclk);
+	if (clk_get_rate(vop->dclk)) {
+		/*
+		 * Rk3288 vop timing register is immediately, when configure
+		 * display timing on display time, may cause tearing.
+		 *
+		 * Vop standby will take effect at end of current frame,
+		 * if dsp hold valid irq happen, it means standby complete.
+		 *
+		 * mode set:
+		 *    standby and wait complete --> |----
+		 *                                  | display time
+		 *                                  |----
+		 *                                  |---> dsp hold irq
+		 *     configure display timing --> |
+		 *         standby exit             |
+		 *                                  | new frame start.
+		 */
 
-	switch (vop->connector_type) {
-	case DRM_MODE_CONNECTOR_LVDS:
-		VOP_CTRL_SET(vop, rgb_en, 1);
-		break;
-	case DRM_MODE_CONNECTOR_eDP:
-		VOP_CTRL_SET(vop, edp_en, 1);
-		break;
-	case DRM_MODE_CONNECTOR_HDMIA:
-		VOP_CTRL_SET(vop, hdmi_en, 1);
-		break;
-	default:
-		DRM_ERROR("unsupport connector_type[%d]\n",
-			  vop->connector_type);
-		ret = -EINVAL;
-		goto out;
-	};
-	VOP_CTRL_SET(vop, out_mode, vop->connector_out_mode);
+		reinit_completion(&vop->dsp_hold_completion);
+		vop_dsp_hold_valid_irq_enable(vop);
+
+		spin_lock(&vop->reg_lock);
+
+		VOP_CTRL_SET(vop, standby, 1);
+
+		spin_unlock(&vop->reg_lock);
+
+		wait_for_completion(&vop->dsp_hold_completion);
+
+		vop_dsp_hold_valid_irq_disable(vop);
+	}
 
 	val = 0x8;
 	val |= (adjusted_mode->flags & DRM_MODE_FLAG_NHSYNC) ? 0 : 1;
@@ -1220,211 +964,119 @@ static int vop_crtc_mode_set(struct drm_crtc *crtc,
 	VOP_CTRL_SET(vop, vact_st_end, val);
 	VOP_CTRL_SET(vop, vpost_st_end, val);
 
-	ret = vop_crtc_mode_set_base(crtc, x, y, fb);
-	if (ret)
-		goto out;
-
-	/*
-	 * reset dclk, take all mode config affect, so the clk would run in
-	 * correct frame.
-	 */
-	reset_control_assert(vop->dclk_rst);
-	usleep_range(10, 20);
-	reset_control_deassert(vop->dclk_rst);
-
 	clk_set_rate(vop->dclk, adjusted_mode->clock * 1000);
-out:
-	ret_clk = clk_enable(vop->dclk);
-	if (ret_clk < 0) {
-		dev_err(vop->dev, "failed to enable dclk - %d\n", ret_clk);
-		return ret_clk;
-	}
 
-	return ret;
-}
-
-static void vop_crtc_commit(struct drm_crtc *crtc)
-{
+	VOP_CTRL_SET(vop, standby, 0);
 }
 
-static const struct drm_crtc_helper_funcs vop_crtc_helper_funcs = {
-	.dpms = vop_crtc_dpms,
-	.prepare = vop_crtc_prepare,
-	.mode_fixup = vop_crtc_mode_fixup,
-	.mode_set = vop_crtc_mode_set,
-	.mode_set_base = vop_crtc_mode_set_base,
-	.commit = vop_crtc_commit,
-};
-
-static int vop_crtc_page_flip(struct drm_crtc *crtc,
-			      struct drm_framebuffer *fb,
-			      struct drm_pending_vblank_event *event,
-			      uint32_t page_flip_flags)
+static void vop_crtc_atomic_flush(struct drm_crtc *crtc,
+				  struct drm_crtc_state *old_crtc_state)
 {
 	struct vop *vop = to_vop(crtc);
-	struct drm_framebuffer *old_fb = crtc->primary->fb;
-	int ret;
 
-	/* when the page flip is requested, crtc should be on */
-	if (!vop->is_enabled) {
-		DRM_DEBUG("page flip request rejected because crtc is off.\n");
-		return 0;
-	}
+	if (WARN_ON(!vop->is_enabled))
+		return;
 
-	crtc->primary->fb = fb;
+	spin_lock(&vop->reg_lock);
 
-	ret = vop_update_primary_plane(crtc, event);
-	if (ret)
-		crtc->primary->fb = old_fb;
+	vop_cfg_done(vop);
 
-	return ret;
+	spin_unlock(&vop->reg_lock);
 }
 
-static void vop_win_state_complete(struct vop_win *vop_win,
-				   struct vop_win_state *state)
+static void vop_crtc_atomic_begin(struct drm_crtc *crtc,
+				  struct drm_crtc_state *old_crtc_state)
 {
-	struct vop *vop = vop_win->vop;
-	struct drm_crtc *crtc = &vop->crtc;
-	struct drm_device *drm = crtc->dev;
-	unsigned long flags;
+	struct vop *vop = to_vop(crtc);
 
-	if (state->event) {
-		spin_lock_irqsave(&drm->event_lock, flags);
-		drm_crtc_send_vblank_event(crtc, state->event);
-		spin_unlock_irqrestore(&drm->event_lock, flags);
-	}
+	if (crtc->state->event) {
+		WARN_ON(drm_crtc_vblank_get(crtc) != 0);
 
-	list_del(&state->head);
-	drm_vblank_put(crtc->dev, vop->pipe);
+		vop->event = crtc->state->event;
+		crtc->state->event = NULL;
+	}
 }
 
+static const struct drm_crtc_helper_funcs vop_crtc_helper_funcs = {
+	.enable = vop_crtc_enable,
+	.disable = vop_crtc_disable,
+	.mode_fixup = vop_crtc_mode_fixup,
+	.atomic_flush = vop_crtc_atomic_flush,
+	.atomic_begin = vop_crtc_atomic_begin,
+};
+
 static void vop_crtc_destroy(struct drm_crtc *crtc)
 {
 	drm_crtc_cleanup(crtc);
 }
 
 static const struct drm_crtc_funcs vop_crtc_funcs = {
-	.set_config = drm_crtc_helper_set_config,
-	.page_flip = vop_crtc_page_flip,
+	.set_config = drm_atomic_helper_set_config,
+	.page_flip = drm_atomic_helper_page_flip,
 	.destroy = vop_crtc_destroy,
+	.reset = drm_atomic_helper_crtc_reset,
+	.atomic_duplicate_state = drm_atomic_helper_crtc_duplicate_state,
+	.atomic_destroy_state = drm_atomic_helper_crtc_destroy_state,
 };
 
-static bool vop_win_state_is_active(struct vop_win *vop_win,
-				    struct vop_win_state *state)
+static bool vop_win_pending_is_complete(struct vop_win *vop_win)
 {
-	bool active = false;
-
-	if (state->fb) {
-		dma_addr_t yrgb_mst;
-
-		/* check yrgb_mst to tell if pending_fb is now front */
-		yrgb_mst = VOP_WIN_GET_YRGBADDR(vop_win->vop, vop_win->data);
-
-		active = (yrgb_mst == state->yrgb_mst);
-	} else {
-		bool enabled;
-
-		/* if enable bit is clear, plane is now disabled */
-		enabled = VOP_WIN_GET(vop_win->vop, vop_win->data, enable);
-
-		active = (enabled == 0);
-	}
+	struct drm_plane *plane = &vop_win->base;
+	struct vop_plane_state *state = to_vop_plane_state(plane->state);
+	dma_addr_t yrgb_mst;
 
-	return active;
-}
+	if (!state->enable)
+		return VOP_WIN_GET(vop_win->vop, vop_win->data, enable) == 0;
 
-static void vop_win_state_destroy(struct vop_win_state *state)
-{
-	struct drm_framebuffer *fb = state->fb;
+	yrgb_mst = VOP_WIN_GET_YRGBADDR(vop_win->vop, vop_win->data);
 
-	if (fb)
-		drm_framebuffer_unreference(fb);
-
-	kfree(state);
+	return yrgb_mst == state->yrgb_mst;
 }
 
-static void vop_win_update_state(struct vop_win *vop_win)
+static void vop_handle_vblank(struct vop *vop)
 {
-	struct vop_win_state *state, *n, *new_active = NULL;
-
-	/* Check if any pending states are now active */
-	list_for_each_entry(state, &vop_win->pending, head)
-		if (vop_win_state_is_active(vop_win, state)) {
-			new_active = state;
-			break;
-		}
-
-	if (!new_active)
-		return;
+	struct drm_device *drm = vop->drm_dev;
+	struct drm_crtc *crtc = &vop->crtc;
+	unsigned long flags;
+	int i;
 
-	/*
-	 * Destroy any 'skipped' pending states - states that were queued
-	 * before the newly active state.
-	 */
-	list_for_each_entry_safe(state, n, &vop_win->pending, head) {
-		if (state == new_active)
-			break;
-		vop_win_state_complete(vop_win, state);
-		vop_win_state_destroy(state);
+	for (i = 0; i < vop->data->win_size; i++) {
+		if (!vop_win_pending_is_complete(&vop->win[i]))
+			return;
 	}
 
-	vop_win_state_complete(vop_win, new_active);
-
-	if (vop_win->active)
-		vop_win_state_destroy(vop_win->active);
-	vop_win->active = new_active;
-}
-
-static bool vop_win_has_pending_state(struct vop_win *vop_win)
-{
-	return !list_empty(&vop_win->pending);
-}
-
-static irqreturn_t vop_isr_thread(int irq, void *data)
-{
-	struct vop *vop = data;
-	const struct vop_data *vop_data = vop->data;
-	unsigned int i;
-
-	mutex_lock(&vop->vsync_mutex);
-
-	if (!vop->vsync_work_pending)
-		goto done;
+	if (vop->event) {
+		spin_lock_irqsave(&drm->event_lock, flags);
 
-	vop->vsync_work_pending = false;
+		drm_crtc_send_vblank_event(crtc, vop->event);
+		drm_crtc_vblank_put(crtc);
+		vop->event = NULL;
 
-	for (i = 0; i < vop_data->win_size; i++) {
-		struct vop_win *vop_win = &vop->win[i];
-
-		vop_win_update_state(vop_win);
-		if (vop_win_has_pending_state(vop_win))
-			vop->vsync_work_pending = true;
+		spin_unlock_irqrestore(&drm->event_lock, flags);
 	}
-
-done:
-	mutex_unlock(&vop->vsync_mutex);
-
-	return IRQ_HANDLED;
+	if (!completion_done(&vop->wait_update_complete))
+		complete(&vop->wait_update_complete);
 }
 
 static irqreturn_t vop_isr(int irq, void *data)
 {
 	struct vop *vop = data;
-	uint32_t intr0_reg, active_irqs;
+	struct drm_crtc *crtc = &vop->crtc;
+	uint32_t active_irqs;
 	unsigned long flags;
 	int ret = IRQ_NONE;
 
 	/*
-	 * INTR_CTRL0 register has interrupt status, enable and clear bits, we
+	 * interrupt register has interrupt status, enable and clear bits, we
 	 * must hold irq_lock to avoid a race with enable/disable_vblank().
 	*/
 	spin_lock_irqsave(&vop->irq_lock, flags);
-	intr0_reg = vop_readl(vop, INTR_CTRL0);
-	active_irqs = intr0_reg & INTR_MASK;
+
+	active_irqs = VOP_INTR_GET_TYPE(vop, status, INTR_MASK);
 	/* Clear all active interrupt sources */
 	if (active_irqs)
-		vop_writel(vop, INTR_CTRL0,
-			   intr0_reg | (active_irqs << INTR_CLR_SHIFT));
+		VOP_INTR_SET_TYPE(vop, clear, active_irqs, 1);
+
 	spin_unlock_irqrestore(&vop->irq_lock, flags);
 
 	/* This is expected for vop iommu irqs, since the irq is shared */
@@ -1438,9 +1090,10 @@ static irqreturn_t vop_isr(int irq, void *data)
 	}
 
 	if (active_irqs & FS_INTR) {
-		drm_handle_vblank(vop->drm_dev, vop->pipe);
+		drm_crtc_handle_vblank(crtc);
+		vop_handle_vblank(vop);
 		active_irqs &= ~FS_INTR;
-		ret = (vop->vsync_work_pending) ? IRQ_WAKE_THREAD : IRQ_HANDLED;
+		ret = IRQ_HANDLED;
 	}
 
 	/* Unhandled irqs are spurious. */
@@ -1478,13 +1131,14 @@ static int vop_create_crtc(struct vop *vop)
 					       0, &vop_plane_funcs,
 					       win_data->phy->data_formats,
 					       win_data->phy->nformats,
-					       win_data->type);
+					       win_data->type, NULL);
 		if (ret) {
 			DRM_ERROR("failed to initialize plane\n");
 			goto err_cleanup_planes;
 		}
 
 		plane = &vop_win->base;
+		drm_plane_helper_add(plane, &plane_helper_funcs);
 		if (plane->type == DRM_PLANE_TYPE_PRIMARY)
 			primary = plane;
 		else if (plane->type == DRM_PLANE_TYPE_CURSOR)
@@ -1492,7 +1146,7 @@ static int vop_create_crtc(struct vop *vop)
 	}
 
 	ret = drm_crtc_init_with_planes(drm_dev, crtc, primary, cursor,
-					&vop_crtc_funcs);
+					&vop_crtc_funcs, NULL);
 	if (ret)
 		return ret;
 
@@ -1515,11 +1169,12 @@ static int vop_create_crtc(struct vop *vop)
 					       &vop_plane_funcs,
 					       win_data->phy->data_formats,
 					       win_data->phy->nformats,
-					       win_data->type);
+					       win_data->type, NULL);
 		if (ret) {
 			DRM_ERROR("failed to initialize overlay plane\n");
 			goto err_cleanup_crtc;
 		}
+		drm_plane_helper_add(&vop_win->base, &plane_helper_funcs);
 	}
 
 	port = of_get_child_by_name(dev->of_node, "port");
@@ -1530,9 +1185,9 @@ static int vop_create_crtc(struct vop *vop)
 	}
 
 	init_completion(&vop->dsp_hold_completion);
+	init_completion(&vop->wait_update_complete);
 	crtc->port = port;
-	vop->pipe = drm_crtc_index(crtc);
-	rockchip_register_crtc_funcs(drm_dev, &private_crtc_funcs, vop->pipe);
+	rockchip_register_crtc_funcs(crtc, &private_crtc_funcs);
 
 	return 0;
 
@@ -1548,7 +1203,7 @@ static void vop_destroy_crtc(struct vop *vop)
 {
 	struct drm_crtc *crtc = &vop->crtc;
 
-	rockchip_unregister_crtc_funcs(vop->drm_dev, vop->pipe);
+	rockchip_unregister_crtc_funcs(crtc);
 	of_node_put(crtc->port);
 	drm_crtc_cleanup(crtc);
 }
@@ -1664,14 +1319,12 @@ static void vop_win_init(struct vop *vop)
 
 		vop_win->data = win_data;
 		vop_win->vop = vop;
-		INIT_LIST_HEAD(&vop_win->pending);
 	}
 }
 
 static int vop_bind(struct device *dev, struct device *master, void *data)
 {
 	struct platform_device *pdev = to_platform_device(dev);
-	const struct of_device_id *of_id;
 	const struct vop_data *vop_data;
 	struct drm_device *drm_dev = data;
 	struct vop *vop;
@@ -1679,8 +1332,7 @@ static int vop_bind(struct device *dev, struct device *master, void *data)
 	size_t alloc_size;
 	int ret, irq;
 
-	of_id = of_match_device(vop_driver_dt_match, dev);
-	vop_data = of_id->data;
+	vop_data = of_device_get_match_data(dev);
 	if (!vop_data)
 		return -ENODEV;
 
@@ -1725,8 +1377,8 @@ static int vop_bind(struct device *dev, struct device *master, void *data)
 
 	mutex_init(&vop->vsync_mutex);
 
-	ret = devm_request_threaded_irq(dev, vop->irq, vop_isr, vop_isr_thread,
-					IRQF_SHARED, dev_name(dev), vop);
+	ret = devm_request_irq(dev, vop->irq, vop_isr,
+			       IRQF_SHARED, dev_name(dev), vop);
 	if (ret)
 		return ret;
 
@@ -1749,42 +1401,8 @@ static void vop_unbind(struct device *dev, struct device *master, void *data)
 	vop_destroy_crtc(vop);
 }
 
-static const struct component_ops vop_component_ops = {
+const struct component_ops vop_component_ops = {
 	.bind = vop_bind,
 	.unbind = vop_unbind,
 };
-
-static int vop_probe(struct platform_device *pdev)
-{
-	struct device *dev = &pdev->dev;
-
-	if (!dev->of_node) {
-		dev_err(dev, "can't find vop devices\n");
-		return -ENODEV;
-	}
-
-	return component_add(dev, &vop_component_ops);
-}
-
-static int vop_remove(struct platform_device *pdev)
-{
-	component_del(&pdev->dev, &vop_component_ops);
-
-	return 0;
-}
-
-struct platform_driver vop_platform_driver = {
-	.probe = vop_probe,
-	.remove = vop_remove,
-	.driver = {
-		.name = "rockchip-vop",
-		.owner = THIS_MODULE,
-		.of_match_table = of_match_ptr(vop_driver_dt_match),
-	},
-};
-
-module_platform_driver(vop_platform_driver);
-
-MODULE_AUTHOR("Mark Yao <mark.yao@rock-chips.com>");
-MODULE_DESCRIPTION("ROCKCHIP VOP Driver");
-MODULE_LICENSE("GPL v2");
+EXPORT_SYMBOL_GPL(vop_component_ops);
diff --git a/drivers/gpu/drm/rockchip/rockchip_drm_vop.h b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
index a2d4ddb896fa..071ff0be7a95 100644
--- a/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
+++ b/drivers/gpu/drm/rockchip/rockchip_drm_vop.h
@@ -15,111 +15,125 @@
 #ifndef _ROCKCHIP_DRM_VOP_H
 #define _ROCKCHIP_DRM_VOP_H
 
-/* register definition */
-#define REG_CFG_DONE			0x0000
-#define VERSION_INFO			0x0004
-#define SYS_CTRL			0x0008
-#define SYS_CTRL1			0x000c
-#define DSP_CTRL0			0x0010
-#define DSP_CTRL1			0x0014
-#define DSP_BG				0x0018
-#define MCU_CTRL			0x001c
-#define INTR_CTRL0			0x0020
-#define INTR_CTRL1			0x0024
-#define WIN0_CTRL0			0x0030
-#define WIN0_CTRL1			0x0034
-#define WIN0_COLOR_KEY			0x0038
-#define WIN0_VIR			0x003c
-#define WIN0_YRGB_MST			0x0040
-#define WIN0_CBR_MST			0x0044
-#define WIN0_ACT_INFO			0x0048
-#define WIN0_DSP_INFO			0x004c
-#define WIN0_DSP_ST			0x0050
-#define WIN0_SCL_FACTOR_YRGB		0x0054
-#define WIN0_SCL_FACTOR_CBR		0x0058
-#define WIN0_SCL_OFFSET			0x005c
-#define WIN0_SRC_ALPHA_CTRL		0x0060
-#define WIN0_DST_ALPHA_CTRL		0x0064
-#define WIN0_FADING_CTRL		0x0068
-/* win1 register */
-#define WIN1_CTRL0			0x0070
-#define WIN1_CTRL1			0x0074
-#define WIN1_COLOR_KEY			0x0078
-#define WIN1_VIR			0x007c
-#define WIN1_YRGB_MST			0x0080
-#define WIN1_CBR_MST			0x0084
-#define WIN1_ACT_INFO			0x0088
-#define WIN1_DSP_INFO			0x008c
-#define WIN1_DSP_ST			0x0090
-#define WIN1_SCL_FACTOR_YRGB		0x0094
-#define WIN1_SCL_FACTOR_CBR		0x0098
-#define WIN1_SCL_OFFSET			0x009c
-#define WIN1_SRC_ALPHA_CTRL		0x00a0
-#define WIN1_DST_ALPHA_CTRL		0x00a4
-#define WIN1_FADING_CTRL		0x00a8
-/* win2 register */
-#define WIN2_CTRL0			0x00b0
-#define WIN2_CTRL1			0x00b4
-#define WIN2_VIR0_1			0x00b8
-#define WIN2_VIR2_3			0x00bc
-#define WIN2_MST0			0x00c0
-#define WIN2_DSP_INFO0			0x00c4
-#define WIN2_DSP_ST0			0x00c8
-#define WIN2_COLOR_KEY			0x00cc
-#define WIN2_MST1			0x00d0
-#define WIN2_DSP_INFO1			0x00d4
-#define WIN2_DSP_ST1			0x00d8
-#define WIN2_SRC_ALPHA_CTRL		0x00dc
-#define WIN2_MST2			0x00e0
-#define WIN2_DSP_INFO2			0x00e4
-#define WIN2_DSP_ST2			0x00e8
-#define WIN2_DST_ALPHA_CTRL		0x00ec
-#define WIN2_MST3			0x00f0
-#define WIN2_DSP_INFO3			0x00f4
-#define WIN2_DSP_ST3			0x00f8
-#define WIN2_FADING_CTRL		0x00fc
-/* win3 register */
-#define WIN3_CTRL0			0x0100
-#define WIN3_CTRL1			0x0104
-#define WIN3_VIR0_1			0x0108
-#define WIN3_VIR2_3			0x010c
-#define WIN3_MST0			0x0110
-#define WIN3_DSP_INFO0			0x0114
-#define WIN3_DSP_ST0			0x0118
-#define WIN3_COLOR_KEY			0x011c
-#define WIN3_MST1			0x0120
-#define WIN3_DSP_INFO1			0x0124
-#define WIN3_DSP_ST1			0x0128
-#define WIN3_SRC_ALPHA_CTRL		0x012c
-#define WIN3_MST2			0x0130
-#define WIN3_DSP_INFO2			0x0134
-#define WIN3_DSP_ST2			0x0138
-#define WIN3_DST_ALPHA_CTRL		0x013c
-#define WIN3_MST3			0x0140
-#define WIN3_DSP_INFO3			0x0144
-#define WIN3_DSP_ST3			0x0148
-#define WIN3_FADING_CTRL		0x014c
-/* hwc register */
-#define HWC_CTRL0			0x0150
-#define HWC_CTRL1			0x0154
-#define HWC_MST				0x0158
-#define HWC_DSP_ST			0x015c
-#define HWC_SRC_ALPHA_CTRL		0x0160
-#define HWC_DST_ALPHA_CTRL		0x0164
-#define HWC_FADING_CTRL			0x0168
-/* post process register */
-#define POST_DSP_HACT_INFO		0x0170
-#define POST_DSP_VACT_INFO		0x0174
-#define POST_SCL_FACTOR_YRGB		0x0178
-#define POST_SCL_CTRL			0x0180
-#define POST_DSP_VACT_INFO_F1		0x0184
-#define DSP_HTOTAL_HS_END		0x0188
-#define DSP_HACT_ST_END			0x018c
-#define DSP_VTOTAL_VS_END		0x0190
-#define DSP_VACT_ST_END			0x0194
-#define DSP_VS_ST_END_F1		0x0198
-#define DSP_VACT_ST_END_F1		0x019c
-/* register definition end */
+enum vop_data_format {
+	VOP_FMT_ARGB8888 = 0,
+	VOP_FMT_RGB888,
+	VOP_FMT_RGB565,
+	VOP_FMT_YUV420SP = 4,
+	VOP_FMT_YUV422SP,
+	VOP_FMT_YUV444SP,
+};
+
+struct vop_reg_data {
+	uint32_t offset;
+	uint32_t value;
+};
+
+struct vop_reg {
+	uint32_t offset;
+	uint32_t shift;
+	uint32_t mask;
+};
+
+struct vop_ctrl {
+	struct vop_reg standby;
+	struct vop_reg data_blank;
+	struct vop_reg gate_en;
+	struct vop_reg mmu_en;
+	struct vop_reg rgb_en;
+	struct vop_reg edp_en;
+	struct vop_reg hdmi_en;
+	struct vop_reg mipi_en;
+	struct vop_reg out_mode;
+	struct vop_reg dither_down;
+	struct vop_reg dither_up;
+	struct vop_reg pin_pol;
+
+	struct vop_reg htotal_pw;
+	struct vop_reg hact_st_end;
+	struct vop_reg vtotal_pw;
+	struct vop_reg vact_st_end;
+	struct vop_reg hpost_st_end;
+	struct vop_reg vpost_st_end;
+
+	struct vop_reg cfg_done;
+};
+
+struct vop_intr {
+	const int *intrs;
+	uint32_t nintrs;
+	struct vop_reg enable;
+	struct vop_reg clear;
+	struct vop_reg status;
+};
+
+struct vop_scl_extension {
+	struct vop_reg cbcr_vsd_mode;
+	struct vop_reg cbcr_vsu_mode;
+	struct vop_reg cbcr_hsd_mode;
+	struct vop_reg cbcr_ver_scl_mode;
+	struct vop_reg cbcr_hor_scl_mode;
+	struct vop_reg yrgb_vsd_mode;
+	struct vop_reg yrgb_vsu_mode;
+	struct vop_reg yrgb_hsd_mode;
+	struct vop_reg yrgb_ver_scl_mode;
+	struct vop_reg yrgb_hor_scl_mode;
+	struct vop_reg line_load_mode;
+	struct vop_reg cbcr_axi_gather_num;
+	struct vop_reg yrgb_axi_gather_num;
+	struct vop_reg vsd_cbcr_gt2;
+	struct vop_reg vsd_cbcr_gt4;
+	struct vop_reg vsd_yrgb_gt2;
+	struct vop_reg vsd_yrgb_gt4;
+	struct vop_reg bic_coe_sel;
+	struct vop_reg cbcr_axi_gather_en;
+	struct vop_reg yrgb_axi_gather_en;
+	struct vop_reg lb_mode;
+};
+
+struct vop_scl_regs {
+	const struct vop_scl_extension *ext;
+
+	struct vop_reg scale_yrgb_x;
+	struct vop_reg scale_yrgb_y;
+	struct vop_reg scale_cbcr_x;
+	struct vop_reg scale_cbcr_y;
+};
+
+struct vop_win_phy {
+	const struct vop_scl_regs *scl;
+	const uint32_t *data_formats;
+	uint32_t nformats;
+
+	struct vop_reg enable;
+	struct vop_reg format;
+	struct vop_reg rb_swap;
+	struct vop_reg act_info;
+	struct vop_reg dsp_info;
+	struct vop_reg dsp_st;
+	struct vop_reg yrgb_mst;
+	struct vop_reg uv_mst;
+	struct vop_reg yrgb_vir;
+	struct vop_reg uv_vir;
+
+	struct vop_reg dst_alpha_ctl;
+	struct vop_reg src_alpha_ctl;
+};
+
+struct vop_win_data {
+	uint32_t base;
+	const struct vop_win_phy *phy;
+	enum drm_plane_type type;
+};
+
+struct vop_data {
+	const struct vop_reg_data *init_table;
+	unsigned int table_size;
+	const struct vop_ctrl *ctrl;
+	const struct vop_intr *intr;
+	const struct vop_win_data *win;
+	unsigned int win_size;
+};
 
 /* interrupt define */
 #define DSP_HOLD_VALID_INTR		(1 << 0)
@@ -233,6 +247,11 @@ static inline uint16_t scl_cal_scale(int src, int dst, int shift)
 	return ((src * 2 - 3) << (shift - 1)) / (dst - 1);
 }
 
+static inline uint16_t scl_cal_scale2(int src, int dst)
+{
+	return ((src - 1) << 12) / (dst - 1);
+}
+
 #define GET_SCL_FT_BILI_DN(src, dst)	scl_cal_scale(src, dst, 12)
 #define GET_SCL_FT_BILI_UP(src, dst)	scl_cal_scale(src, dst, 16)
 #define GET_SCL_FT_BIC(src, dst)	scl_cal_scale(src, dst, 16)
@@ -286,4 +305,5 @@ static inline int scl_vop_cal_lb_mode(int width, bool is_yuv)
 	return lb_mode;
 }
 
+extern const struct component_ops vop_component_ops;
 #endif /* _ROCKCHIP_DRM_VOP_H */
diff --git a/drivers/gpu/drm/rockchip/rockchip_vop_reg.c b/drivers/gpu/drm/rockchip/rockchip_vop_reg.c
new file mode 100644
index 000000000000..3166b46a5893
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_vop_reg.c
@@ -0,0 +1,316 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <drm/drmP.h>
+
+#include <linux/kernel.h>
+#include <linux/component.h>
+
+#include "rockchip_drm_vop.h"
+#include "rockchip_vop_reg.h"
+
+#define VOP_REG(off, _mask, s) \
+		{.offset = off, \
+		 .mask = _mask, \
+		 .shift = s,}
+
+static const uint32_t formats_win_full[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_ABGR8888,
+	DRM_FORMAT_RGB888,
+	DRM_FORMAT_BGR888,
+	DRM_FORMAT_RGB565,
+	DRM_FORMAT_BGR565,
+	DRM_FORMAT_NV12,
+	DRM_FORMAT_NV16,
+	DRM_FORMAT_NV24,
+};
+
+static const uint32_t formats_win_lite[] = {
+	DRM_FORMAT_XRGB8888,
+	DRM_FORMAT_ARGB8888,
+	DRM_FORMAT_XBGR8888,
+	DRM_FORMAT_ABGR8888,
+	DRM_FORMAT_RGB888,
+	DRM_FORMAT_BGR888,
+	DRM_FORMAT_RGB565,
+	DRM_FORMAT_BGR565,
+};
+
+static const struct vop_scl_extension rk3288_win_full_scl_ext = {
+	.cbcr_vsd_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 31),
+	.cbcr_vsu_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 30),
+	.cbcr_hsd_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 28),
+	.cbcr_ver_scl_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 26),
+	.cbcr_hor_scl_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 24),
+	.yrgb_vsd_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 23),
+	.yrgb_vsu_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 22),
+	.yrgb_hsd_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 20),
+	.yrgb_ver_scl_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 18),
+	.yrgb_hor_scl_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 16),
+	.line_load_mode = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 15),
+	.cbcr_axi_gather_num = VOP_REG(RK3288_WIN0_CTRL1, 0x7, 12),
+	.yrgb_axi_gather_num = VOP_REG(RK3288_WIN0_CTRL1, 0xf, 8),
+	.vsd_cbcr_gt2 = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 7),
+	.vsd_cbcr_gt4 = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 6),
+	.vsd_yrgb_gt2 = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 5),
+	.vsd_yrgb_gt4 = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 4),
+	.bic_coe_sel = VOP_REG(RK3288_WIN0_CTRL1, 0x3, 2),
+	.cbcr_axi_gather_en = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 1),
+	.yrgb_axi_gather_en = VOP_REG(RK3288_WIN0_CTRL1, 0x1, 0),
+	.lb_mode = VOP_REG(RK3288_WIN0_CTRL0, 0x7, 5),
+};
+
+static const struct vop_scl_regs rk3288_win_full_scl = {
+	.ext = &rk3288_win_full_scl_ext,
+	.scale_yrgb_x = VOP_REG(RK3288_WIN0_SCL_FACTOR_YRGB, 0xffff, 0x0),
+	.scale_yrgb_y = VOP_REG(RK3288_WIN0_SCL_FACTOR_YRGB, 0xffff, 16),
+	.scale_cbcr_x = VOP_REG(RK3288_WIN0_SCL_FACTOR_CBR, 0xffff, 0x0),
+	.scale_cbcr_y = VOP_REG(RK3288_WIN0_SCL_FACTOR_CBR, 0xffff, 16),
+};
+
+static const struct vop_win_phy rk3288_win01_data = {
+	.scl = &rk3288_win_full_scl,
+	.data_formats = formats_win_full,
+	.nformats = ARRAY_SIZE(formats_win_full),
+	.enable = VOP_REG(RK3288_WIN0_CTRL0, 0x1, 0),
+	.format = VOP_REG(RK3288_WIN0_CTRL0, 0x7, 1),
+	.rb_swap = VOP_REG(RK3288_WIN0_CTRL0, 0x1, 12),
+	.act_info = VOP_REG(RK3288_WIN0_ACT_INFO, 0x1fff1fff, 0),
+	.dsp_info = VOP_REG(RK3288_WIN0_DSP_INFO, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(RK3288_WIN0_DSP_ST, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(RK3288_WIN0_YRGB_MST, 0xffffffff, 0),
+	.uv_mst = VOP_REG(RK3288_WIN0_CBR_MST, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(RK3288_WIN0_VIR, 0x3fff, 0),
+	.uv_vir = VOP_REG(RK3288_WIN0_VIR, 0x3fff, 16),
+	.src_alpha_ctl = VOP_REG(RK3288_WIN0_SRC_ALPHA_CTRL, 0xff, 0),
+	.dst_alpha_ctl = VOP_REG(RK3288_WIN0_DST_ALPHA_CTRL, 0xff, 0),
+};
+
+static const struct vop_win_phy rk3288_win23_data = {
+	.data_formats = formats_win_lite,
+	.nformats = ARRAY_SIZE(formats_win_lite),
+	.enable = VOP_REG(RK3288_WIN2_CTRL0, 0x1, 0),
+	.format = VOP_REG(RK3288_WIN2_CTRL0, 0x7, 1),
+	.rb_swap = VOP_REG(RK3288_WIN2_CTRL0, 0x1, 12),
+	.dsp_info = VOP_REG(RK3288_WIN2_DSP_INFO0, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(RK3288_WIN2_DSP_ST0, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(RK3288_WIN2_MST0, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(RK3288_WIN2_VIR0_1, 0x1fff, 0),
+	.src_alpha_ctl = VOP_REG(RK3288_WIN2_SRC_ALPHA_CTRL, 0xff, 0),
+	.dst_alpha_ctl = VOP_REG(RK3288_WIN2_DST_ALPHA_CTRL, 0xff, 0),
+};
+
+static const struct vop_ctrl rk3288_ctrl_data = {
+	.standby = VOP_REG(RK3288_SYS_CTRL, 0x1, 22),
+	.gate_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 23),
+	.mmu_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 20),
+	.rgb_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 12),
+	.hdmi_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 13),
+	.edp_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 14),
+	.mipi_en = VOP_REG(RK3288_SYS_CTRL, 0x1, 15),
+	.dither_down = VOP_REG(RK3288_DSP_CTRL1, 0xf, 1),
+	.dither_up = VOP_REG(RK3288_DSP_CTRL1, 0x1, 6),
+	.data_blank = VOP_REG(RK3288_DSP_CTRL0, 0x1, 19),
+	.out_mode = VOP_REG(RK3288_DSP_CTRL0, 0xf, 0),
+	.pin_pol = VOP_REG(RK3288_DSP_CTRL0, 0xf, 4),
+	.htotal_pw = VOP_REG(RK3288_DSP_HTOTAL_HS_END, 0x1fff1fff, 0),
+	.hact_st_end = VOP_REG(RK3288_DSP_HACT_ST_END, 0x1fff1fff, 0),
+	.vtotal_pw = VOP_REG(RK3288_DSP_VTOTAL_VS_END, 0x1fff1fff, 0),
+	.vact_st_end = VOP_REG(RK3288_DSP_VACT_ST_END, 0x1fff1fff, 0),
+	.hpost_st_end = VOP_REG(RK3288_POST_DSP_HACT_INFO, 0x1fff1fff, 0),
+	.vpost_st_end = VOP_REG(RK3288_POST_DSP_VACT_INFO, 0x1fff1fff, 0),
+	.cfg_done = VOP_REG(RK3288_REG_CFG_DONE, 0x1, 0),
+};
+
+static const struct vop_reg_data rk3288_init_reg_table[] = {
+	{RK3288_SYS_CTRL, 0x00c00000},
+	{RK3288_DSP_CTRL0, 0x00000000},
+	{RK3288_WIN0_CTRL0, 0x00000080},
+	{RK3288_WIN1_CTRL0, 0x00000080},
+	/* TODO: Win2/3 support multiple area function, but we haven't found
+	 * a suitable way to use it yet, so let's just use them as other windows
+	 * with only area 0 enabled.
+	 */
+	{RK3288_WIN2_CTRL0, 0x00000010},
+	{RK3288_WIN3_CTRL0, 0x00000010},
+};
+
+/*
+ * Note: rk3288 has a dedicated 'cursor' window, however, that window requires
+ * special support to get alpha blending working.  For now, just use overlay
+ * window 3 for the drm cursor.
+ *
+ */
+static const struct vop_win_data rk3288_vop_win_data[] = {
+	{ .base = 0x00, .phy = &rk3288_win01_data,
+	  .type = DRM_PLANE_TYPE_PRIMARY },
+	{ .base = 0x40, .phy = &rk3288_win01_data,
+	  .type = DRM_PLANE_TYPE_OVERLAY },
+	{ .base = 0x00, .phy = &rk3288_win23_data,
+	  .type = DRM_PLANE_TYPE_OVERLAY },
+	{ .base = 0x50, .phy = &rk3288_win23_data,
+	  .type = DRM_PLANE_TYPE_CURSOR },
+};
+
+static const int rk3288_vop_intrs[] = {
+	DSP_HOLD_VALID_INTR,
+	FS_INTR,
+	LINE_FLAG_INTR,
+	BUS_ERROR_INTR,
+};
+
+static const struct vop_intr rk3288_vop_intr = {
+	.intrs = rk3288_vop_intrs,
+	.nintrs = ARRAY_SIZE(rk3288_vop_intrs),
+	.status = VOP_REG(RK3288_INTR_CTRL0, 0xf, 0),
+	.enable = VOP_REG(RK3288_INTR_CTRL0, 0xf, 4),
+	.clear = VOP_REG(RK3288_INTR_CTRL0, 0xf, 8),
+};
+
+static const struct vop_data rk3288_vop = {
+	.init_table = rk3288_init_reg_table,
+	.table_size = ARRAY_SIZE(rk3288_init_reg_table),
+	.intr = &rk3288_vop_intr,
+	.ctrl = &rk3288_ctrl_data,
+	.win = rk3288_vop_win_data,
+	.win_size = ARRAY_SIZE(rk3288_vop_win_data),
+};
+
+static const struct vop_scl_regs rk3066_win_scl = {
+	.scale_yrgb_x = VOP_REG(RK3036_WIN0_SCL_FACTOR_YRGB, 0xffff, 0x0),
+	.scale_yrgb_y = VOP_REG(RK3036_WIN0_SCL_FACTOR_YRGB, 0xffff, 16),
+	.scale_cbcr_x = VOP_REG(RK3036_WIN0_SCL_FACTOR_CBR, 0xffff, 0x0),
+	.scale_cbcr_y = VOP_REG(RK3036_WIN0_SCL_FACTOR_CBR, 0xffff, 16),
+};
+
+static const struct vop_win_phy rk3036_win0_data = {
+	.scl = &rk3066_win_scl,
+	.data_formats = formats_win_full,
+	.nformats = ARRAY_SIZE(formats_win_full),
+	.enable = VOP_REG(RK3036_SYS_CTRL, 0x1, 0),
+	.format = VOP_REG(RK3036_SYS_CTRL, 0x7, 3),
+	.rb_swap = VOP_REG(RK3036_SYS_CTRL, 0x1, 15),
+	.act_info = VOP_REG(RK3036_WIN0_ACT_INFO, 0x1fff1fff, 0),
+	.dsp_info = VOP_REG(RK3036_WIN0_DSP_INFO, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(RK3036_WIN0_DSP_ST, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(RK3036_WIN0_YRGB_MST, 0xffffffff, 0),
+	.uv_mst = VOP_REG(RK3036_WIN0_CBR_MST, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(RK3036_WIN0_VIR, 0xffff, 0),
+};
+
+static const struct vop_win_phy rk3036_win1_data = {
+	.data_formats = formats_win_lite,
+	.nformats = ARRAY_SIZE(formats_win_lite),
+	.enable = VOP_REG(RK3036_SYS_CTRL, 0x1, 1),
+	.format = VOP_REG(RK3036_SYS_CTRL, 0x7, 6),
+	.rb_swap = VOP_REG(RK3036_SYS_CTRL, 0x1, 19),
+	.act_info = VOP_REG(RK3036_WIN1_ACT_INFO, 0x1fff1fff, 0),
+	.dsp_info = VOP_REG(RK3036_WIN1_DSP_INFO, 0x0fff0fff, 0),
+	.dsp_st = VOP_REG(RK3036_WIN1_DSP_ST, 0x1fff1fff, 0),
+	.yrgb_mst = VOP_REG(RK3036_WIN1_MST, 0xffffffff, 0),
+	.yrgb_vir = VOP_REG(RK3036_WIN1_VIR, 0xffff, 0),
+};
+
+static const struct vop_win_data rk3036_vop_win_data[] = {
+	{ .base = 0x00, .phy = &rk3036_win0_data,
+	  .type = DRM_PLANE_TYPE_PRIMARY },
+	{ .base = 0x00, .phy = &rk3036_win1_data,
+	  .type = DRM_PLANE_TYPE_CURSOR },
+};
+
+static const int rk3036_vop_intrs[] = {
+	DSP_HOLD_VALID_INTR,
+	FS_INTR,
+	LINE_FLAG_INTR,
+	BUS_ERROR_INTR,
+};
+
+static const struct vop_intr rk3036_intr = {
+	.intrs = rk3036_vop_intrs,
+	.nintrs = ARRAY_SIZE(rk3036_vop_intrs),
+	.status = VOP_REG(RK3036_INT_STATUS, 0xf, 0),
+	.enable = VOP_REG(RK3036_INT_STATUS, 0xf, 4),
+	.clear = VOP_REG(RK3036_INT_STATUS, 0xf, 8),
+};
+
+static const struct vop_ctrl rk3036_ctrl_data = {
+	.standby = VOP_REG(RK3036_SYS_CTRL, 0x1, 30),
+	.out_mode = VOP_REG(RK3036_DSP_CTRL0, 0xf, 0),
+	.pin_pol = VOP_REG(RK3036_DSP_CTRL0, 0xf, 4),
+	.htotal_pw = VOP_REG(RK3036_DSP_HTOTAL_HS_END, 0x1fff1fff, 0),
+	.hact_st_end = VOP_REG(RK3036_DSP_HACT_ST_END, 0x1fff1fff, 0),
+	.vtotal_pw = VOP_REG(RK3036_DSP_VTOTAL_VS_END, 0x1fff1fff, 0),
+	.vact_st_end = VOP_REG(RK3036_DSP_VACT_ST_END, 0x1fff1fff, 0),
+	.cfg_done = VOP_REG(RK3036_REG_CFG_DONE, 0x1, 0),
+};
+
+static const struct vop_reg_data rk3036_vop_init_reg_table[] = {
+	{RK3036_DSP_CTRL1, 0x00000000},
+};
+
+static const struct vop_data rk3036_vop = {
+	.init_table = rk3036_vop_init_reg_table,
+	.table_size = ARRAY_SIZE(rk3036_vop_init_reg_table),
+	.ctrl = &rk3036_ctrl_data,
+	.intr = &rk3036_intr,
+	.win = rk3036_vop_win_data,
+	.win_size = ARRAY_SIZE(rk3036_vop_win_data),
+};
+
+static const struct of_device_id vop_driver_dt_match[] = {
+	{ .compatible = "rockchip,rk3288-vop",
+	  .data = &rk3288_vop },
+	{ .compatible = "rockchip,rk3036-vop",
+	  .data = &rk3036_vop },
+	{},
+};
+MODULE_DEVICE_TABLE(of, vop_driver_dt_match);
+
+static int vop_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+
+	if (!dev->of_node) {
+		dev_err(dev, "can't find vop devices\n");
+		return -ENODEV;
+	}
+
+	return component_add(dev, &vop_component_ops);
+}
+
+static int vop_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vop_component_ops);
+
+	return 0;
+}
+
+struct platform_driver vop_platform_driver = {
+	.probe = vop_probe,
+	.remove = vop_remove,
+	.driver = {
+		.name = "rockchip-vop",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(vop_driver_dt_match),
+	},
+};
+
+module_platform_driver(vop_platform_driver);
+
+MODULE_AUTHOR("Mark Yao <mark.yao@rock-chips.com>");
+MODULE_DESCRIPTION("ROCKCHIP VOP Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/gpu/drm/rockchip/rockchip_vop_reg.h b/drivers/gpu/drm/rockchip/rockchip_vop_reg.h
new file mode 100644
index 000000000000..d4b46cba2f26
--- /dev/null
+++ b/drivers/gpu/drm/rockchip/rockchip_vop_reg.h
@@ -0,0 +1,169 @@
+/*
+ * Copyright (C) Fuzhou Rockchip Electronics Co.Ltd
+ * Author:Mark Yao <mark.yao@rock-chips.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef _ROCKCHIP_VOP_REG_H
+#define _ROCKCHIP_VOP_REG_H
+
+/* rk3288 register definition */
+#define RK3288_REG_CFG_DONE			0x0000
+#define RK3288_VERSION_INFO			0x0004
+#define RK3288_SYS_CTRL				0x0008
+#define RK3288_SYS_CTRL1			0x000c
+#define RK3288_DSP_CTRL0			0x0010
+#define RK3288_DSP_CTRL1			0x0014
+#define RK3288_DSP_BG				0x0018
+#define RK3288_MCU_CTRL				0x001c
+#define RK3288_INTR_CTRL0			0x0020
+#define RK3288_INTR_CTRL1			0x0024
+#define RK3288_WIN0_CTRL0			0x0030
+#define RK3288_WIN0_CTRL1			0x0034
+#define RK3288_WIN0_COLOR_KEY			0x0038
+#define RK3288_WIN0_VIR				0x003c
+#define RK3288_WIN0_YRGB_MST			0x0040
+#define RK3288_WIN0_CBR_MST			0x0044
+#define RK3288_WIN0_ACT_INFO			0x0048
+#define RK3288_WIN0_DSP_INFO			0x004c
+#define RK3288_WIN0_DSP_ST			0x0050
+#define RK3288_WIN0_SCL_FACTOR_YRGB		0x0054
+#define RK3288_WIN0_SCL_FACTOR_CBR		0x0058
+#define RK3288_WIN0_SCL_OFFSET			0x005c
+#define RK3288_WIN0_SRC_ALPHA_CTRL		0x0060
+#define RK3288_WIN0_DST_ALPHA_CTRL		0x0064
+#define RK3288_WIN0_FADING_CTRL			0x0068
+
+/* win1 register */
+#define RK3288_WIN1_CTRL0			0x0070
+#define RK3288_WIN1_CTRL1			0x0074
+#define RK3288_WIN1_COLOR_KEY			0x0078
+#define RK3288_WIN1_VIR				0x007c
+#define RK3288_WIN1_YRGB_MST			0x0080
+#define RK3288_WIN1_CBR_MST			0x0084
+#define RK3288_WIN1_ACT_INFO			0x0088
+#define RK3288_WIN1_DSP_INFO			0x008c
+#define RK3288_WIN1_DSP_ST			0x0090
+#define RK3288_WIN1_SCL_FACTOR_YRGB		0x0094
+#define RK3288_WIN1_SCL_FACTOR_CBR		0x0098
+#define RK3288_WIN1_SCL_OFFSET			0x009c
+#define RK3288_WIN1_SRC_ALPHA_CTRL		0x00a0
+#define RK3288_WIN1_DST_ALPHA_CTRL		0x00a4
+#define RK3288_WIN1_FADING_CTRL			0x00a8
+/* win2 register */
+#define RK3288_WIN2_CTRL0			0x00b0
+#define RK3288_WIN2_CTRL1			0x00b4
+#define RK3288_WIN2_VIR0_1			0x00b8
+#define RK3288_WIN2_VIR2_3			0x00bc
+#define RK3288_WIN2_MST0			0x00c0
+#define RK3288_WIN2_DSP_INFO0			0x00c4
+#define RK3288_WIN2_DSP_ST0			0x00c8
+#define RK3288_WIN2_COLOR_KEY			0x00cc
+#define RK3288_WIN2_MST1			0x00d0
+#define RK3288_WIN2_DSP_INFO1			0x00d4
+#define RK3288_WIN2_DSP_ST1			0x00d8
+#define RK3288_WIN2_SRC_ALPHA_CTRL		0x00dc
+#define RK3288_WIN2_MST2			0x00e0
+#define RK3288_WIN2_DSP_INFO2			0x00e4
+#define RK3288_WIN2_DSP_ST2			0x00e8
+#define RK3288_WIN2_DST_ALPHA_CTRL		0x00ec
+#define RK3288_WIN2_MST3			0x00f0
+#define RK3288_WIN2_DSP_INFO3			0x00f4
+#define RK3288_WIN2_DSP_ST3			0x00f8
+#define RK3288_WIN2_FADING_CTRL			0x00fc
+/* win3 register */
+#define RK3288_WIN3_CTRL0			0x0100
+#define RK3288_WIN3_CTRL1			0x0104
+#define RK3288_WIN3_VIR0_1			0x0108
+#define RK3288_WIN3_VIR2_3			0x010c
+#define RK3288_WIN3_MST0			0x0110
+#define RK3288_WIN3_DSP_INFO0			0x0114
+#define RK3288_WIN3_DSP_ST0			0x0118
+#define RK3288_WIN3_COLOR_KEY			0x011c
+#define RK3288_WIN3_MST1			0x0120
+#define RK3288_WIN3_DSP_INFO1			0x0124
+#define RK3288_WIN3_DSP_ST1			0x0128
+#define RK3288_WIN3_SRC_ALPHA_CTRL		0x012c
+#define RK3288_WIN3_MST2			0x0130
+#define RK3288_WIN3_DSP_INFO2			0x0134
+#define RK3288_WIN3_DSP_ST2			0x0138
+#define RK3288_WIN3_DST_ALPHA_CTRL		0x013c
+#define RK3288_WIN3_MST3			0x0140
+#define RK3288_WIN3_DSP_INFO3			0x0144
+#define RK3288_WIN3_DSP_ST3			0x0148
+#define RK3288_WIN3_FADING_CTRL			0x014c
+/* hwc register */
+#define RK3288_HWC_CTRL0			0x0150
+#define RK3288_HWC_CTRL1			0x0154
+#define RK3288_HWC_MST				0x0158
+#define RK3288_HWC_DSP_ST			0x015c
+#define RK3288_HWC_SRC_ALPHA_CTRL		0x0160
+#define RK3288_HWC_DST_ALPHA_CTRL		0x0164
+#define RK3288_HWC_FADING_CTRL			0x0168
+/* post process register */
+#define RK3288_POST_DSP_HACT_INFO		0x0170
+#define RK3288_POST_DSP_VACT_INFO		0x0174
+#define RK3288_POST_SCL_FACTOR_YRGB		0x0178
+#define RK3288_POST_SCL_CTRL			0x0180
+#define RK3288_POST_DSP_VACT_INFO_F1		0x0184
+#define RK3288_DSP_HTOTAL_HS_END		0x0188
+#define RK3288_DSP_HACT_ST_END			0x018c
+#define RK3288_DSP_VTOTAL_VS_END		0x0190
+#define RK3288_DSP_VACT_ST_END			0x0194
+#define RK3288_DSP_VS_ST_END_F1			0x0198
+#define RK3288_DSP_VACT_ST_END_F1		0x019c
+/* register definition end */
+
+/* rk3036 register definition */
+#define RK3036_SYS_CTRL			0x00
+#define RK3036_DSP_CTRL0		0x04
+#define RK3036_DSP_CTRL1		0x08
+#define RK3036_INT_STATUS		0x10
+#define RK3036_ALPHA_CTRL		0x14
+#define RK3036_WIN0_COLOR_KEY		0x18
+#define RK3036_WIN1_COLOR_KEY		0x1c
+#define RK3036_WIN0_YRGB_MST		0x20
+#define RK3036_WIN0_CBR_MST		0x24
+#define RK3036_WIN1_VIR			0x28
+#define RK3036_AXI_BUS_CTRL		0x2c
+#define RK3036_WIN0_VIR			0x30
+#define RK3036_WIN0_ACT_INFO		0x34
+#define RK3036_WIN0_DSP_INFO		0x38
+#define RK3036_WIN0_DSP_ST		0x3c
+#define RK3036_WIN0_SCL_FACTOR_YRGB	0x40
+#define RK3036_WIN0_SCL_FACTOR_CBR	0x44
+#define RK3036_WIN0_SCL_OFFSET		0x48
+#define RK3036_HWC_MST			0x58
+#define RK3036_HWC_DSP_ST		0x5c
+#define RK3036_DSP_HTOTAL_HS_END	0x6c
+#define RK3036_DSP_HACT_ST_END		0x70
+#define RK3036_DSP_VTOTAL_VS_END	0x74
+#define RK3036_DSP_VACT_ST_END		0x78
+#define RK3036_DSP_VS_ST_END_F1		0x7c
+#define RK3036_DSP_VACT_ST_END_F1	0x80
+#define RK3036_GATHER_TRANSFER		0x84
+#define RK3036_VERSION_INFO		0x94
+#define RK3036_REG_CFG_DONE		0x90
+#define RK3036_WIN1_MST			0xa0
+#define RK3036_WIN1_ACT_INFO		0xb4
+#define RK3036_WIN1_DSP_INFO		0xb8
+#define RK3036_WIN1_DSP_ST		0xbc
+#define RK3036_WIN1_SCL_FACTOR_YRGB	0xc0
+#define RK3036_WIN1_SCL_OFFSET		0xc8
+#define RK3036_BCSH_CTRL		0xd0
+#define RK3036_BCSH_COLOR_BAR		0xd4
+#define RK3036_BCSH_BCS			0xd8
+#define RK3036_BCSH_H			0xdc
+#define RK3036_WIN1_LUT_ADDR		0x400
+#define RK3036_HWC_LUT_ADDR		0x800
+/* rk3036 register definition end */
+
+#endif /* _ROCKCHIP_VOP_REG_H */
diff --git a/drivers/gpu/drm/shmobile/Kconfig b/drivers/gpu/drm/shmobile/Kconfig
index b9202aa6f8ab..8d17d00ddb4b 100644
--- a/drivers/gpu/drm/shmobile/Kconfig
+++ b/drivers/gpu/drm/shmobile/Kconfig
@@ -1,6 +1,6 @@
 config DRM_SHMOBILE
 	tristate "DRM Support for SH Mobile"
-	depends on DRM && ARM && HAVE_DMA_ATTRS
+	depends on DRM && ARM
 	depends on ARCH_SHMOBILE || COMPILE_TEST
 	depends on FB_SH_MOBILE_MERAM || !FB_SH_MOBILE_MERAM
 	select BACKLIGHT_CLASS_DEVICE
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
index e9272b0a8592..db0763794edc 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
@@ -613,7 +613,7 @@ int shmob_drm_encoder_create(struct shmob_drm_device *sdev)
 	encoder->possible_crtcs = 1;
 
 	ret = drm_encoder_init(sdev->ddev, encoder, &encoder_funcs,
-			       DRM_MODE_ENCODER_LVDS);
+			       DRM_MODE_ENCODER_LVDS, NULL);
 	if (ret < 0)
 		return ret;
 
@@ -739,8 +739,6 @@ int shmob_drm_connector_create(struct shmob_drm_device *sdev,
 	if (ret < 0)
 		goto err_backlight;
 
-	connector->encoder = encoder;
-
 	drm_helper_connector_dpms(connector, DRM_MODE_DPMS_OFF);
 	drm_object_property_set_value(&connector->base,
 		sdev->ddev->mode_config.dpms_property, DRM_MODE_DPMS_OFF);
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_kms.c b/drivers/gpu/drm/shmobile/shmob_drm_kms.c
index aaf98ace4a90..388a0fc13564 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_kms.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_kms.c
@@ -104,7 +104,7 @@ const struct shmob_drm_format_info *shmob_drm_format_info(u32 fourcc)
 
 static struct drm_framebuffer *
 shmob_drm_fb_create(struct drm_device *dev, struct drm_file *file_priv,
-		    struct drm_mode_fb_cmd2 *mode_cmd)
+		    const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	const struct shmob_drm_format_info *format;
 
diff --git a/drivers/gpu/drm/sti/Kconfig b/drivers/gpu/drm/sti/Kconfig
index 10c1b1926e6f..5ad43a1bb260 100644
--- a/drivers/gpu/drm/sti/Kconfig
+++ b/drivers/gpu/drm/sti/Kconfig
@@ -1,6 +1,6 @@
 config DRM_STI
 	tristate "DRM Support for STMicroelectronics SoC stiH41x Series"
-	depends on DRM && (SOC_STIH415 || SOC_STIH416 || ARCH_MULTIPLATFORM) && HAVE_DMA_ATTRS
+	depends on DRM && (SOC_STIH415 || SOC_STIH416 || ARCH_MULTIPLATFORM)
 	select RESET_CONTROLLER
 	select DRM_KMS_HELPER
 	select DRM_GEM_CMA_HELPER
diff --git a/drivers/gpu/drm/sti/sti_crtc.c b/drivers/gpu/drm/sti/sti_crtc.c
index 3ae09dcd4fd8..de11c7cfb02f 100644
--- a/drivers/gpu/drm/sti/sti_crtc.c
+++ b/drivers/gpu/drm/sti/sti_crtc.c
@@ -367,7 +367,7 @@ int sti_crtc_init(struct drm_device *drm_dev, struct sti_mixer *mixer,
 	int res;
 
 	res = drm_crtc_init_with_planes(drm_dev, crtc, primary, cursor,
-					&sti_crtc_funcs);
+					&sti_crtc_funcs, NULL);
 	if (res) {
 		DRM_ERROR("Can't initialze CRTC\n");
 		return -EINVAL;
diff --git a/drivers/gpu/drm/sti/sti_cursor.c b/drivers/gpu/drm/sti/sti_cursor.c
index dd1032195051..807863106b8d 100644
--- a/drivers/gpu/drm/sti/sti_cursor.c
+++ b/drivers/gpu/drm/sti/sti_cursor.c
@@ -272,7 +272,7 @@ struct drm_plane *sti_cursor_create(struct drm_device *drm_dev,
 				       &sti_plane_helpers_funcs,
 				       cursor_supported_formats,
 				       ARRAY_SIZE(cursor_supported_formats),
-				       DRM_PLANE_TYPE_CURSOR);
+				       DRM_PLANE_TYPE_CURSOR, NULL);
 	if (res) {
 		DRM_ERROR("Failed to initialize universal plane\n");
 		goto err_plane;
diff --git a/drivers/gpu/drm/sti/sti_drv.c b/drivers/gpu/drm/sti/sti_drv.c
index 1469987949d8..506b5626f3ed 100644
--- a/drivers/gpu/drm/sti/sti_drv.c
+++ b/drivers/gpu/drm/sti/sti_drv.c
@@ -160,6 +160,7 @@ static int sti_load(struct drm_device *dev, unsigned long flags)
 
 	drm_mode_config_reset(dev);
 
+	drm_helper_disable_unused_functions(dev);
 	drm_fbdev_cma_init(dev, 32,
 			   dev->mode_config.num_crtc,
 			   dev->mode_config.num_connector);
diff --git a/drivers/gpu/drm/sti/sti_gdp.c b/drivers/gpu/drm/sti/sti_gdp.c
index c85dc7d6b005..f9a1d92c9d95 100644
--- a/drivers/gpu/drm/sti/sti_gdp.c
+++ b/drivers/gpu/drm/sti/sti_gdp.c
@@ -630,7 +630,7 @@ struct drm_plane *sti_gdp_create(struct drm_device *drm_dev,
 				       &sti_plane_helpers_funcs,
 				       gdp_supported_formats,
 				       ARRAY_SIZE(gdp_supported_formats),
-				       type);
+				       type, NULL);
 	if (res) {
 		DRM_ERROR("Failed to initialize universal plane\n");
 		goto err;
diff --git a/drivers/gpu/drm/sti/sti_hda.c b/drivers/gpu/drm/sti/sti_hda.c
index d735daccd458..49cce833f2c8 100644
--- a/drivers/gpu/drm/sti/sti_hda.c
+++ b/drivers/gpu/drm/sti/sti_hda.c
@@ -543,8 +543,6 @@ static int sti_hda_connector_get_modes(struct drm_connector *connector)
 		count++;
 	}
 
-	drm_mode_sort(&connector->modes);
-
 	return count;
 }
 
diff --git a/drivers/gpu/drm/sti/sti_hqvdp.c b/drivers/gpu/drm/sti/sti_hqvdp.c
index ea0690bc77d5..43861b52261d 100644
--- a/drivers/gpu/drm/sti/sti_hqvdp.c
+++ b/drivers/gpu/drm/sti/sti_hqvdp.c
@@ -973,7 +973,7 @@ static struct drm_plane *sti_hqvdp_create(struct drm_device *drm_dev,
 				       &sti_plane_helpers_funcs,
 				       hqvdp_supported_formats,
 				       ARRAY_SIZE(hqvdp_supported_formats),
-				       DRM_PLANE_TYPE_OVERLAY);
+				       DRM_PLANE_TYPE_OVERLAY, NULL);
 	if (res) {
 		DRM_ERROR("Failed to initialize universal plane\n");
 		return NULL;
diff --git a/drivers/gpu/drm/sti/sti_tvout.c b/drivers/gpu/drm/sti/sti_tvout.c
index c8a4c5dae2b6..f2afcf5438b8 100644
--- a/drivers/gpu/drm/sti/sti_tvout.c
+++ b/drivers/gpu/drm/sti/sti_tvout.c
@@ -512,7 +512,8 @@ sti_tvout_create_dvo_encoder(struct drm_device *dev,
 	drm_encoder->possible_clones = 1 << 0;
 
 	drm_encoder_init(dev, drm_encoder,
-			 &sti_tvout_encoder_funcs, DRM_MODE_ENCODER_LVDS);
+			 &sti_tvout_encoder_funcs, DRM_MODE_ENCODER_LVDS,
+			 NULL);
 
 	drm_encoder_helper_add(drm_encoder, &sti_dvo_encoder_helper_funcs);
 
@@ -564,7 +565,7 @@ static struct drm_encoder *sti_tvout_create_hda_encoder(struct drm_device *dev,
 	drm_encoder->possible_clones = 1 << 0;
 
 	drm_encoder_init(dev, drm_encoder,
-			&sti_tvout_encoder_funcs, DRM_MODE_ENCODER_DAC);
+			&sti_tvout_encoder_funcs, DRM_MODE_ENCODER_DAC, NULL);
 
 	drm_encoder_helper_add(drm_encoder, &sti_hda_encoder_helper_funcs);
 
@@ -613,7 +614,7 @@ static struct drm_encoder *sti_tvout_create_hdmi_encoder(struct drm_device *dev,
 	drm_encoder->possible_clones = 1 << 1;
 
 	drm_encoder_init(dev, drm_encoder,
-			&sti_tvout_encoder_funcs, DRM_MODE_ENCODER_TMDS);
+			&sti_tvout_encoder_funcs, DRM_MODE_ENCODER_TMDS, NULL);
 
 	drm_encoder_helper_add(drm_encoder, &sti_hdmi_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/tegra/Kconfig b/drivers/gpu/drm/tegra/Kconfig
index 74d9d621453d..63ebb154b9b5 100644
--- a/drivers/gpu/drm/tegra/Kconfig
+++ b/drivers/gpu/drm/tegra/Kconfig
@@ -16,18 +16,6 @@ config DRM_TEGRA
 
 if DRM_TEGRA
 
-config DRM_TEGRA_FBDEV
-	bool "Enable legacy fbdev support"
-	select DRM_KMS_FB_HELPER
-	select FB_SYS_FILLRECT
-	select FB_SYS_COPYAREA
-	select FB_SYS_IMAGEBLIT
-	default y
-	help
-	  Choose this option if you have a need for the legacy fbdev support.
-	  Note that this support also provides the Linux console on top of
-	  the Tegra modesetting driver.
-
 config DRM_TEGRA_DEBUG
 	bool "NVIDIA Tegra DRM debug support"
 	help
diff --git a/drivers/gpu/drm/tegra/dc.c b/drivers/gpu/drm/tegra/dc.c
index e9f24a85a103..dde6f208c347 100644
--- a/drivers/gpu/drm/tegra/dc.c
+++ b/drivers/gpu/drm/tegra/dc.c
@@ -660,7 +660,8 @@ static struct drm_plane *tegra_dc_primary_plane_create(struct drm_device *drm,
 
 	err = drm_universal_plane_init(drm, &plane->base, possible_crtcs,
 				       &tegra_primary_plane_funcs, formats,
-				       num_formats, DRM_PLANE_TYPE_PRIMARY);
+				       num_formats, DRM_PLANE_TYPE_PRIMARY,
+				       NULL);
 	if (err < 0) {
 		kfree(plane);
 		return ERR_PTR(err);
@@ -827,7 +828,8 @@ static struct drm_plane *tegra_dc_cursor_plane_create(struct drm_device *drm,
 
 	err = drm_universal_plane_init(drm, &plane->base, 1 << dc->pipe,
 				       &tegra_cursor_plane_funcs, formats,
-				       num_formats, DRM_PLANE_TYPE_CURSOR);
+				       num_formats, DRM_PLANE_TYPE_CURSOR,
+				       NULL);
 	if (err < 0) {
 		kfree(plane);
 		return ERR_PTR(err);
@@ -890,7 +892,8 @@ static struct drm_plane *tegra_dc_overlay_plane_create(struct drm_device *drm,
 
 	err = drm_universal_plane_init(drm, &plane->base, 1 << dc->pipe,
 				       &tegra_overlay_plane_funcs, formats,
-				       num_formats, DRM_PLANE_TYPE_OVERLAY);
+				       num_formats, DRM_PLANE_TYPE_OVERLAY,
+				       NULL);
 	if (err < 0) {
 		kfree(plane);
 		return ERR_PTR(err);
@@ -1732,7 +1735,7 @@ static int tegra_dc_init(struct host1x_client *client)
 	}
 
 	err = drm_crtc_init_with_planes(drm, &dc->base, primary, cursor,
-					&tegra_crtc_funcs);
+					&tegra_crtc_funcs, NULL);
 	if (err < 0)
 		goto cleanup;
 
@@ -1952,8 +1955,10 @@ static int tegra_dc_parse_dt(struct tegra_dc *dc)
 		 * cases where only a single display controller is used.
 		 */
 		for_each_matching_node(np, tegra_dc_of_match) {
-			if (np == dc->dev->of_node)
+			if (np == dc->dev->of_node) {
+				of_node_put(np);
 				break;
+			}
 
 			value++;
 		}
diff --git a/drivers/gpu/drm/tegra/dpaux.c b/drivers/gpu/drm/tegra/dpaux.c
index 6aecb6647313..b24a0f14821a 100644
--- a/drivers/gpu/drm/tegra/dpaux.c
+++ b/drivers/gpu/drm/tegra/dpaux.c
@@ -436,7 +436,7 @@ struct platform_driver tegra_dpaux_driver = {
 	.remove = tegra_dpaux_remove,
 };
 
-struct tegra_dpaux *tegra_dpaux_find_by_of_node(struct device_node *np)
+struct drm_dp_aux *drm_dp_aux_find_by_of_node(struct device_node *np)
 {
 	struct tegra_dpaux *dpaux;
 
@@ -445,7 +445,7 @@ struct tegra_dpaux *tegra_dpaux_find_by_of_node(struct device_node *np)
 	list_for_each_entry(dpaux, &dpaux_list, list)
 		if (np == dpaux->dev->of_node) {
 			mutex_unlock(&dpaux_lock);
-			return dpaux;
+			return &dpaux->aux;
 		}
 
 	mutex_unlock(&dpaux_lock);
@@ -453,8 +453,9 @@ struct tegra_dpaux *tegra_dpaux_find_by_of_node(struct device_node *np)
 	return NULL;
 }
 
-int tegra_dpaux_attach(struct tegra_dpaux *dpaux, struct tegra_output *output)
+int drm_dp_aux_attach(struct drm_dp_aux *aux, struct tegra_output *output)
 {
+	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	unsigned long timeout;
 	int err;
 
@@ -470,7 +471,7 @@ int tegra_dpaux_attach(struct tegra_dpaux *dpaux, struct tegra_output *output)
 	while (time_before(jiffies, timeout)) {
 		enum drm_connector_status status;
 
-		status = tegra_dpaux_detect(dpaux);
+		status = drm_dp_aux_detect(aux);
 		if (status == connector_status_connected) {
 			enable_irq(dpaux->irq);
 			return 0;
@@ -482,8 +483,9 @@ int tegra_dpaux_attach(struct tegra_dpaux *dpaux, struct tegra_output *output)
 	return -ETIMEDOUT;
 }
 
-int tegra_dpaux_detach(struct tegra_dpaux *dpaux)
+int drm_dp_aux_detach(struct drm_dp_aux *aux)
 {
+	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	unsigned long timeout;
 	int err;
 
@@ -498,7 +500,7 @@ int tegra_dpaux_detach(struct tegra_dpaux *dpaux)
 	while (time_before(jiffies, timeout)) {
 		enum drm_connector_status status;
 
-		status = tegra_dpaux_detect(dpaux);
+		status = drm_dp_aux_detect(aux);
 		if (status == connector_status_disconnected) {
 			dpaux->output = NULL;
 			return 0;
@@ -510,8 +512,9 @@ int tegra_dpaux_detach(struct tegra_dpaux *dpaux)
 	return -ETIMEDOUT;
 }
 
-enum drm_connector_status tegra_dpaux_detect(struct tegra_dpaux *dpaux)
+enum drm_connector_status drm_dp_aux_detect(struct drm_dp_aux *aux)
 {
+	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	u32 value;
 
 	value = tegra_dpaux_readl(dpaux, DPAUX_DP_AUXSTAT);
@@ -522,8 +525,9 @@ enum drm_connector_status tegra_dpaux_detect(struct tegra_dpaux *dpaux)
 	return connector_status_disconnected;
 }
 
-int tegra_dpaux_enable(struct tegra_dpaux *dpaux)
+int drm_dp_aux_enable(struct drm_dp_aux *aux)
 {
+	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	u32 value;
 
 	value = DPAUX_HYBRID_PADCTL_AUX_CMH(2) |
@@ -540,8 +544,9 @@ int tegra_dpaux_enable(struct tegra_dpaux *dpaux)
 	return 0;
 }
 
-int tegra_dpaux_disable(struct tegra_dpaux *dpaux)
+int drm_dp_aux_disable(struct drm_dp_aux *aux)
 {
+	struct tegra_dpaux *dpaux = to_dpaux(aux);
 	u32 value;
 
 	value = tegra_dpaux_readl(dpaux, DPAUX_HYBRID_SPARE);
@@ -551,11 +556,11 @@ int tegra_dpaux_disable(struct tegra_dpaux *dpaux)
 	return 0;
 }
 
-int tegra_dpaux_prepare(struct tegra_dpaux *dpaux, u8 encoding)
+int drm_dp_aux_prepare(struct drm_dp_aux *aux, u8 encoding)
 {
 	int err;
 
-	err = drm_dp_dpcd_writeb(&dpaux->aux, DP_MAIN_LINK_CHANNEL_CODING_SET,
+	err = drm_dp_dpcd_writeb(aux, DP_MAIN_LINK_CHANNEL_CODING_SET,
 				 encoding);
 	if (err < 0)
 		return err;
@@ -563,15 +568,15 @@ int tegra_dpaux_prepare(struct tegra_dpaux *dpaux, u8 encoding)
 	return 0;
 }
 
-int tegra_dpaux_train(struct tegra_dpaux *dpaux, struct drm_dp_link *link,
-		      u8 pattern)
+int drm_dp_aux_train(struct drm_dp_aux *aux, struct drm_dp_link *link,
+		     u8 pattern)
 {
 	u8 tp = pattern & DP_TRAINING_PATTERN_MASK;
 	u8 status[DP_LINK_STATUS_SIZE], values[4];
 	unsigned int i;
 	int err;
 
-	err = drm_dp_dpcd_writeb(&dpaux->aux, DP_TRAINING_PATTERN_SET, pattern);
+	err = drm_dp_dpcd_writeb(aux, DP_TRAINING_PATTERN_SET, pattern);
 	if (err < 0)
 		return err;
 
@@ -584,14 +589,14 @@ int tegra_dpaux_train(struct tegra_dpaux *dpaux, struct drm_dp_link *link,
 			    DP_TRAIN_MAX_SWING_REACHED |
 			    DP_TRAIN_VOLTAGE_SWING_LEVEL_0;
 
-	err = drm_dp_dpcd_write(&dpaux->aux, DP_TRAINING_LANE0_SET, values,
+	err = drm_dp_dpcd_write(aux, DP_TRAINING_LANE0_SET, values,
 				link->num_lanes);
 	if (err < 0)
 		return err;
 
 	usleep_range(500, 1000);
 
-	err = drm_dp_dpcd_read_link_status(&dpaux->aux, status);
+	err = drm_dp_dpcd_read_link_status(aux, status);
 	if (err < 0)
 		return err;
 
@@ -609,11 +614,11 @@ int tegra_dpaux_train(struct tegra_dpaux *dpaux, struct drm_dp_link *link,
 		break;
 
 	default:
-		dev_err(dpaux->dev, "unsupported training pattern %u\n", tp);
+		dev_err(aux->dev, "unsupported training pattern %u\n", tp);
 		return -EINVAL;
 	}
 
-	err = drm_dp_dpcd_writeb(&dpaux->aux, DP_EDP_CONFIGURATION_SET, 0);
+	err = drm_dp_dpcd_writeb(aux, DP_EDP_CONFIGURATION_SET, 0);
 	if (err < 0)
 		return err;
 
diff --git a/drivers/gpu/drm/tegra/drm.c b/drivers/gpu/drm/tegra/drm.c
index 159ef515cab1..c5c856a0879d 100644
--- a/drivers/gpu/drm/tegra/drm.c
+++ b/drivers/gpu/drm/tegra/drm.c
@@ -106,7 +106,7 @@ static int tegra_atomic_commit(struct drm_device *drm,
 
 static const struct drm_mode_config_funcs tegra_drm_mode_funcs = {
 	.fb_create = tegra_fb_create,
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	.output_poll_changed = tegra_fb_output_poll_changed,
 #endif
 	.atomic_check = drm_atomic_helper_check,
@@ -137,8 +137,8 @@ static int tegra_drm_load(struct drm_device *drm, unsigned long flags)
 		start = geometry->aperture_start;
 		end = geometry->aperture_end;
 
-		DRM_DEBUG("IOMMU context initialized (aperture: %#llx-%#llx)\n",
-			  start, end);
+		DRM_DEBUG_DRIVER("IOMMU aperture initialized (%#llx-%#llx)\n",
+				 start, end);
 		drm_mm_init(&tegra->mm, start, end - start + 1);
 	}
 
@@ -260,7 +260,7 @@ static void tegra_drm_context_free(struct tegra_drm_context *context)
 
 static void tegra_drm_lastclose(struct drm_device *drm)
 {
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_drm *tegra = drm->dev_private;
 
 	tegra_fbdev_restore_mode(tegra->fbdev);
@@ -277,9 +277,7 @@ host1x_bo_lookup(struct drm_device *drm, struct drm_file *file, u32 handle)
 	if (!gem)
 		return NULL;
 
-	mutex_lock(&drm->struct_mutex);
-	drm_gem_object_unreference(gem);
-	mutex_unlock(&drm->struct_mutex);
+	drm_gem_object_unreference_unlocked(gem);
 
 	bo = to_tegra_bo(gem);
 	return &bo->base;
@@ -473,7 +471,7 @@ static int tegra_gem_mmap(struct drm_device *drm, void *data,
 
 	args->offset = drm_vma_node_offset_addr(&bo->gem.vma_node);
 
-	drm_gem_object_unreference(gem);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return 0;
 }
@@ -683,7 +681,7 @@ static int tegra_gem_set_tiling(struct drm_device *drm, void *data,
 	bo->tiling.mode = mode;
 	bo->tiling.value = value;
 
-	drm_gem_object_unreference(gem);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return 0;
 }
@@ -723,7 +721,7 @@ static int tegra_gem_get_tiling(struct drm_device *drm, void *data,
 		break;
 	}
 
-	drm_gem_object_unreference(gem);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return err;
 }
@@ -748,7 +746,7 @@ static int tegra_gem_set_flags(struct drm_device *drm, void *data,
 	if (args->flags & DRM_TEGRA_GEM_BOTTOM_UP)
 		bo->flags |= TEGRA_BO_BOTTOM_UP;
 
-	drm_gem_object_unreference(gem);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return 0;
 }
@@ -770,7 +768,7 @@ static int tegra_gem_get_flags(struct drm_device *drm, void *data,
 	if (bo->flags & TEGRA_BO_BOTTOM_UP)
 		args->flags |= DRM_TEGRA_GEM_BOTTOM_UP;
 
-	drm_gem_object_unreference(gem);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return 0;
 }
@@ -921,7 +919,8 @@ static void tegra_debugfs_cleanup(struct drm_minor *minor)
 #endif
 
 static struct drm_driver tegra_drm_driver = {
-	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
+	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME |
+			   DRIVER_ATOMIC,
 	.load = tegra_drm_load,
 	.unload = tegra_drm_unload,
 	.open = tegra_drm_open,
@@ -991,7 +990,6 @@ static int host1x_drm_probe(struct host1x_device *dev)
 	if (!drm)
 		return -ENOMEM;
 
-	drm_dev_set_unique(drm, dev_name(&dev->dev));
 	dev_set_drvdata(&dev->dev, drm);
 
 	err = drm_dev_register(drm, 0);
@@ -1023,8 +1021,17 @@ static int host1x_drm_remove(struct host1x_device *dev)
 static int host1x_drm_suspend(struct device *dev)
 {
 	struct drm_device *drm = dev_get_drvdata(dev);
+	struct tegra_drm *tegra = drm->dev_private;
 
 	drm_kms_helper_poll_disable(drm);
+	tegra_drm_fb_suspend(drm);
+
+	tegra->state = drm_atomic_helper_suspend(drm);
+	if (IS_ERR(tegra->state)) {
+		tegra_drm_fb_resume(drm);
+		drm_kms_helper_poll_enable(drm);
+		return PTR_ERR(tegra->state);
+	}
 
 	return 0;
 }
@@ -1032,7 +1039,10 @@ static int host1x_drm_suspend(struct device *dev)
 static int host1x_drm_resume(struct device *dev)
 {
 	struct drm_device *drm = dev_get_drvdata(dev);
+	struct tegra_drm *tegra = drm->dev_private;
 
+	drm_atomic_helper_resume(drm, tegra->state);
+	tegra_drm_fb_resume(drm);
 	drm_kms_helper_poll_enable(drm);
 
 	return 0;
@@ -1076,6 +1086,16 @@ static struct host1x_driver host1x_drm_driver = {
 	.subdevs = host1x_drm_subdevs,
 };
 
+static struct platform_driver * const drivers[] = {
+	&tegra_dc_driver,
+	&tegra_hdmi_driver,
+	&tegra_dsi_driver,
+	&tegra_dpaux_driver,
+	&tegra_sor_driver,
+	&tegra_gr2d_driver,
+	&tegra_gr3d_driver,
+};
+
 static int __init host1x_drm_init(void)
 {
 	int err;
@@ -1084,48 +1104,12 @@ static int __init host1x_drm_init(void)
 	if (err < 0)
 		return err;
 
-	err = platform_driver_register(&tegra_dc_driver);
+	err = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 	if (err < 0)
 		goto unregister_host1x;
 
-	err = platform_driver_register(&tegra_dsi_driver);
-	if (err < 0)
-		goto unregister_dc;
-
-	err = platform_driver_register(&tegra_sor_driver);
-	if (err < 0)
-		goto unregister_dsi;
-
-	err = platform_driver_register(&tegra_hdmi_driver);
-	if (err < 0)
-		goto unregister_sor;
-
-	err = platform_driver_register(&tegra_dpaux_driver);
-	if (err < 0)
-		goto unregister_hdmi;
-
-	err = platform_driver_register(&tegra_gr2d_driver);
-	if (err < 0)
-		goto unregister_dpaux;
-
-	err = platform_driver_register(&tegra_gr3d_driver);
-	if (err < 0)
-		goto unregister_gr2d;
-
 	return 0;
 
-unregister_gr2d:
-	platform_driver_unregister(&tegra_gr2d_driver);
-unregister_dpaux:
-	platform_driver_unregister(&tegra_dpaux_driver);
-unregister_hdmi:
-	platform_driver_unregister(&tegra_hdmi_driver);
-unregister_sor:
-	platform_driver_unregister(&tegra_sor_driver);
-unregister_dsi:
-	platform_driver_unregister(&tegra_dsi_driver);
-unregister_dc:
-	platform_driver_unregister(&tegra_dc_driver);
 unregister_host1x:
 	host1x_driver_unregister(&host1x_drm_driver);
 	return err;
@@ -1134,13 +1118,7 @@ module_init(host1x_drm_init);
 
 static void __exit host1x_drm_exit(void)
 {
-	platform_driver_unregister(&tegra_gr3d_driver);
-	platform_driver_unregister(&tegra_gr2d_driver);
-	platform_driver_unregister(&tegra_dpaux_driver);
-	platform_driver_unregister(&tegra_hdmi_driver);
-	platform_driver_unregister(&tegra_sor_driver);
-	platform_driver_unregister(&tegra_dsi_driver);
-	platform_driver_unregister(&tegra_dc_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 	host1x_driver_unregister(&host1x_drm_driver);
 }
 module_exit(host1x_drm_exit);
diff --git a/drivers/gpu/drm/tegra/drm.h b/drivers/gpu/drm/tegra/drm.h
index ec49275ffb24..c088f2f67eda 100644
--- a/drivers/gpu/drm/tegra/drm.h
+++ b/drivers/gpu/drm/tegra/drm.h
@@ -30,7 +30,7 @@ struct tegra_fb {
 	unsigned int num_planes;
 };
 
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 struct tegra_fbdev {
 	struct drm_fb_helper base;
 	struct tegra_fb *fb;
@@ -46,7 +46,7 @@ struct tegra_drm {
 	struct mutex clients_lock;
 	struct list_head clients;
 
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_fbdev *fbdev;
 #endif
 
@@ -57,6 +57,8 @@ struct tegra_drm {
 		struct work_struct work;
 		struct mutex lock;
 	} commit;
+
+	struct drm_atomic_state *state;
 };
 
 struct tegra_drm_client;
@@ -247,18 +249,17 @@ void tegra_output_connector_destroy(struct drm_connector *connector);
 void tegra_output_encoder_destroy(struct drm_encoder *encoder);
 
 /* from dpaux.c */
-struct tegra_dpaux;
 struct drm_dp_link;
 
-struct tegra_dpaux *tegra_dpaux_find_by_of_node(struct device_node *np);
-enum drm_connector_status tegra_dpaux_detect(struct tegra_dpaux *dpaux);
-int tegra_dpaux_attach(struct tegra_dpaux *dpaux, struct tegra_output *output);
-int tegra_dpaux_detach(struct tegra_dpaux *dpaux);
-int tegra_dpaux_enable(struct tegra_dpaux *dpaux);
-int tegra_dpaux_disable(struct tegra_dpaux *dpaux);
-int tegra_dpaux_prepare(struct tegra_dpaux *dpaux, u8 encoding);
-int tegra_dpaux_train(struct tegra_dpaux *dpaux, struct drm_dp_link *link,
-		      u8 pattern);
+struct drm_dp_aux *drm_dp_aux_find_by_of_node(struct device_node *np);
+enum drm_connector_status drm_dp_aux_detect(struct drm_dp_aux *aux);
+int drm_dp_aux_attach(struct drm_dp_aux *aux, struct tegra_output *output);
+int drm_dp_aux_detach(struct drm_dp_aux *aux);
+int drm_dp_aux_enable(struct drm_dp_aux *aux);
+int drm_dp_aux_disable(struct drm_dp_aux *aux);
+int drm_dp_aux_prepare(struct drm_dp_aux *aux, u8 encoding);
+int drm_dp_aux_train(struct drm_dp_aux *aux, struct drm_dp_link *link,
+		     u8 pattern);
 
 /* from fb.c */
 struct tegra_bo *tegra_fb_get_plane(struct drm_framebuffer *framebuffer,
@@ -268,21 +269,23 @@ int tegra_fb_get_tiling(struct drm_framebuffer *framebuffer,
 			struct tegra_bo_tiling *tiling);
 struct drm_framebuffer *tegra_fb_create(struct drm_device *drm,
 					struct drm_file *file,
-					struct drm_mode_fb_cmd2 *cmd);
+					const struct drm_mode_fb_cmd2 *cmd);
 int tegra_drm_fb_prepare(struct drm_device *drm);
 void tegra_drm_fb_free(struct drm_device *drm);
 int tegra_drm_fb_init(struct drm_device *drm);
 void tegra_drm_fb_exit(struct drm_device *drm);
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+void tegra_drm_fb_suspend(struct drm_device *drm);
+void tegra_drm_fb_resume(struct drm_device *drm);
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 void tegra_fbdev_restore_mode(struct tegra_fbdev *fbdev);
 void tegra_fb_output_poll_changed(struct drm_device *drm);
 #endif
 
 extern struct platform_driver tegra_dc_driver;
-extern struct platform_driver tegra_dsi_driver;
-extern struct platform_driver tegra_sor_driver;
 extern struct platform_driver tegra_hdmi_driver;
+extern struct platform_driver tegra_dsi_driver;
 extern struct platform_driver tegra_dpaux_driver;
+extern struct platform_driver tegra_sor_driver;
 extern struct platform_driver tegra_gr2d_driver;
 extern struct platform_driver tegra_gr3d_driver;
 
diff --git a/drivers/gpu/drm/tegra/dsi.c b/drivers/gpu/drm/tegra/dsi.c
index f0a138ef68ce..44e102799195 100644
--- a/drivers/gpu/drm/tegra/dsi.c
+++ b/drivers/gpu/drm/tegra/dsi.c
@@ -745,14 +745,13 @@ static void tegra_dsi_soft_reset(struct tegra_dsi *dsi)
 
 static void tegra_dsi_connector_reset(struct drm_connector *connector)
 {
-	struct tegra_dsi_state *state;
-
-	kfree(connector->state);
-	connector->state = NULL;
+	struct tegra_dsi_state *state =
+		kzalloc(sizeof(*state), GFP_KERNEL);
 
-	state = kzalloc(sizeof(*state), GFP_KERNEL);
-	if (state)
-		connector->state = &state->base;
+	if (state) {
+		kfree(connector->state);
+		__drm_atomic_helper_connector_reset(connector, &state->base);
+	}
 }
 
 static struct drm_connector_state *
@@ -1023,7 +1022,7 @@ static int tegra_dsi_init(struct host1x_client *client)
 
 		drm_encoder_init(drm, &dsi->output.encoder,
 				 &tegra_dsi_encoder_funcs,
-				 DRM_MODE_ENCODER_DSI);
+				 DRM_MODE_ENCODER_DSI, NULL);
 		drm_encoder_helper_add(&dsi->output.encoder,
 				       &tegra_dsi_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/tegra/fb.c b/drivers/gpu/drm/tegra/fb.c
index 1004075fd088..ca84de9ccb51 100644
--- a/drivers/gpu/drm/tegra/fb.c
+++ b/drivers/gpu/drm/tegra/fb.c
@@ -10,6 +10,8 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/console.h>
+
 #include "drm.h"
 #include "gem.h"
 
@@ -18,7 +20,7 @@ static inline struct tegra_fb *to_tegra_fb(struct drm_framebuffer *fb)
 	return container_of(fb, struct tegra_fb, base);
 }
 
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 static inline struct tegra_fbdev *to_tegra_fbdev(struct drm_fb_helper *helper)
 {
 	return container_of(helper, struct tegra_fbdev, base);
@@ -86,13 +88,13 @@ static int tegra_fb_create_handle(struct drm_framebuffer *framebuffer,
 	return drm_gem_handle_create(file, &fb->planes[0]->gem, handle);
 }
 
-static struct drm_framebuffer_funcs tegra_fb_funcs = {
+static const struct drm_framebuffer_funcs tegra_fb_funcs = {
 	.destroy = tegra_fb_destroy,
 	.create_handle = tegra_fb_create_handle,
 };
 
 static struct tegra_fb *tegra_fb_alloc(struct drm_device *drm,
-				       struct drm_mode_fb_cmd2 *mode_cmd,
+				       const struct drm_mode_fb_cmd2 *mode_cmd,
 				       struct tegra_bo **planes,
 				       unsigned int num_planes)
 {
@@ -131,7 +133,7 @@ static struct tegra_fb *tegra_fb_alloc(struct drm_device *drm,
 
 struct drm_framebuffer *tegra_fb_create(struct drm_device *drm,
 					struct drm_file *file,
-					struct drm_mode_fb_cmd2 *cmd)
+					const struct drm_mode_fb_cmd2 *cmd)
 {
 	unsigned int hsub, vsub, i;
 	struct tegra_bo *planes[4];
@@ -181,7 +183,7 @@ unreference:
 	return ERR_PTR(err);
 }
 
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 static struct fb_ops tegra_fb_ops = {
 	.owner = THIS_MODULE,
 	.fb_fillrect = drm_fb_helper_sys_fillrect,
@@ -370,7 +372,7 @@ void tegra_fb_output_poll_changed(struct drm_device *drm)
 
 int tegra_drm_fb_prepare(struct drm_device *drm)
 {
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_drm *tegra = drm->dev_private;
 
 	tegra->fbdev = tegra_fbdev_create(drm);
@@ -383,7 +385,7 @@ int tegra_drm_fb_prepare(struct drm_device *drm)
 
 void tegra_drm_fb_free(struct drm_device *drm)
 {
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_drm *tegra = drm->dev_private;
 
 	tegra_fbdev_free(tegra->fbdev);
@@ -392,7 +394,7 @@ void tegra_drm_fb_free(struct drm_device *drm)
 
 int tegra_drm_fb_init(struct drm_device *drm)
 {
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_drm *tegra = drm->dev_private;
 	int err;
 
@@ -407,9 +409,31 @@ int tegra_drm_fb_init(struct drm_device *drm)
 
 void tegra_drm_fb_exit(struct drm_device *drm)
 {
-#ifdef CONFIG_DRM_TEGRA_FBDEV
+#ifdef CONFIG_DRM_FBDEV_EMULATION
 	struct tegra_drm *tegra = drm->dev_private;
 
 	tegra_fbdev_exit(tegra->fbdev);
 #endif
 }
+
+void tegra_drm_fb_suspend(struct drm_device *drm)
+{
+#ifdef CONFIG_DRM_FBDEV_EMULATION
+	struct tegra_drm *tegra = drm->dev_private;
+
+	console_lock();
+	drm_fb_helper_set_suspend(&tegra->fbdev->base, 1);
+	console_unlock();
+#endif
+}
+
+void tegra_drm_fb_resume(struct drm_device *drm)
+{
+#ifdef CONFIG_DRM_FBDEV_EMULATION
+	struct tegra_drm *tegra = drm->dev_private;
+
+	console_lock();
+	drm_fb_helper_set_suspend(&tegra->fbdev->base, 0);
+	console_unlock();
+#endif
+}
diff --git a/drivers/gpu/drm/tegra/gem.c b/drivers/gpu/drm/tegra/gem.c
index 01e16e146bfe..33add93b4ed9 100644
--- a/drivers/gpu/drm/tegra/gem.c
+++ b/drivers/gpu/drm/tegra/gem.c
@@ -28,11 +28,8 @@ static inline struct tegra_bo *host1x_to_tegra_bo(struct host1x_bo *bo)
 static void tegra_bo_put(struct host1x_bo *bo)
 {
 	struct tegra_bo *obj = host1x_to_tegra_bo(bo);
-	struct drm_device *drm = obj->gem.dev;
 
-	mutex_lock(&drm->struct_mutex);
-	drm_gem_object_unreference(&obj->gem);
-	mutex_unlock(&drm->struct_mutex);
+	drm_gem_object_unreference_unlocked(&obj->gem);
 }
 
 static dma_addr_t tegra_bo_pin(struct host1x_bo *bo, struct sg_table **sgt)
@@ -72,11 +69,8 @@ static void tegra_bo_kunmap(struct host1x_bo *bo, unsigned int page,
 static struct host1x_bo *tegra_bo_get(struct host1x_bo *bo)
 {
 	struct tegra_bo *obj = host1x_to_tegra_bo(bo);
-	struct drm_device *drm = obj->gem.dev;
 
-	mutex_lock(&drm->struct_mutex);
 	drm_gem_object_reference(&obj->gem);
-	mutex_unlock(&drm->struct_mutex);
 
 	return bo;
 }
@@ -408,12 +402,9 @@ int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
 	struct drm_gem_object *gem;
 	struct tegra_bo *bo;
 
-	mutex_lock(&drm->struct_mutex);
-
 	gem = drm_gem_object_lookup(drm, file, handle);
 	if (!gem) {
 		dev_err(drm->dev, "failed to lookup GEM object\n");
-		mutex_unlock(&drm->struct_mutex);
 		return -EINVAL;
 	}
 
@@ -421,9 +412,7 @@ int tegra_bo_dumb_map_offset(struct drm_file *file, struct drm_device *drm,
 
 	*offset = drm_vma_node_offset_addr(&bo->gem.vma_node);
 
-	drm_gem_object_unreference(gem);
-
-	mutex_unlock(&drm->struct_mutex);
+	drm_gem_object_unreference_unlocked(gem);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/tegra/hdmi.c b/drivers/gpu/drm/tegra/hdmi.c
index 52b32cbd9de6..b7ef4929e347 100644
--- a/drivers/gpu/drm/tegra/hdmi.c
+++ b/drivers/gpu/drm/tegra/hdmi.c
@@ -1320,7 +1320,7 @@ static int tegra_hdmi_init(struct host1x_client *client)
 	hdmi->output.connector.dpms = DRM_MODE_DPMS_OFF;
 
 	drm_encoder_init(drm, &hdmi->output.encoder, &tegra_hdmi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 	drm_encoder_helper_add(&hdmi->output.encoder,
 			       &tegra_hdmi_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/tegra/rgb.c b/drivers/gpu/drm/tegra/rgb.c
index bc9735b4ad60..e246334e0252 100644
--- a/drivers/gpu/drm/tegra/rgb.c
+++ b/drivers/gpu/drm/tegra/rgb.c
@@ -287,7 +287,7 @@ int tegra_dc_rgb_init(struct drm_device *drm, struct tegra_dc *dc)
 	output->connector.dpms = DRM_MODE_DPMS_OFF;
 
 	drm_encoder_init(drm, &output->encoder, &tegra_rgb_encoder_funcs,
-			 DRM_MODE_ENCODER_LVDS);
+			 DRM_MODE_ENCODER_LVDS, NULL);
 	drm_encoder_helper_add(&output->encoder,
 			       &tegra_rgb_encoder_helper_funcs);
 
diff --git a/drivers/gpu/drm/tegra/sor.c b/drivers/gpu/drm/tegra/sor.c
index 3eff7cf75d25..757c6e8603af 100644
--- a/drivers/gpu/drm/tegra/sor.c
+++ b/drivers/gpu/drm/tegra/sor.c
@@ -173,7 +173,7 @@ struct tegra_sor {
 	struct clk *clk_dp;
 	struct clk *clk;
 
-	struct tegra_dpaux *dpaux;
+	struct drm_dp_aux *aux;
 
 	struct drm_info_list *debugfs_files;
 	struct drm_minor *minor;
@@ -273,7 +273,7 @@ static int tegra_sor_dp_train_fast(struct tegra_sor *sor,
 		   SOR_DP_PADCTL_CM_TXD_1 | SOR_DP_PADCTL_CM_TXD_0);
 	tegra_sor_writel(sor, value, SOR_DP_PADCTL0);
 
-	err = tegra_dpaux_prepare(sor->dpaux, DP_SET_ANSI_8B10B);
+	err = drm_dp_aux_prepare(sor->aux, DP_SET_ANSI_8B10B);
 	if (err < 0)
 		return err;
 
@@ -288,7 +288,7 @@ static int tegra_sor_dp_train_fast(struct tegra_sor *sor,
 
 	pattern = DP_TRAINING_PATTERN_1;
 
-	err = tegra_dpaux_train(sor->dpaux, link, pattern);
+	err = drm_dp_aux_train(sor->aux, link, pattern);
 	if (err < 0)
 		return err;
 
@@ -309,7 +309,7 @@ static int tegra_sor_dp_train_fast(struct tegra_sor *sor,
 
 	pattern = DP_LINK_SCRAMBLING_DISABLE | DP_TRAINING_PATTERN_2;
 
-	err = tegra_dpaux_train(sor->dpaux, link, pattern);
+	err = drm_dp_aux_train(sor->aux, link, pattern);
 	if (err < 0)
 		return err;
 
@@ -324,7 +324,7 @@ static int tegra_sor_dp_train_fast(struct tegra_sor *sor,
 
 	pattern = DP_TRAINING_PATTERN_DISABLE;
 
-	err = tegra_dpaux_train(sor->dpaux, link, pattern);
+	err = drm_dp_aux_train(sor->aux, link, pattern);
 	if (err < 0)
 		return err;
 
@@ -1044,8 +1044,8 @@ tegra_sor_connector_detect(struct drm_connector *connector, bool force)
 	struct tegra_output *output = connector_to_output(connector);
 	struct tegra_sor *sor = to_sor(output);
 
-	if (sor->dpaux)
-		return tegra_dpaux_detect(sor->dpaux);
+	if (sor->aux)
+		return drm_dp_aux_detect(sor->aux);
 
 	return tegra_output_connector_detect(connector, force);
 }
@@ -1066,13 +1066,13 @@ static int tegra_sor_connector_get_modes(struct drm_connector *connector)
 	struct tegra_sor *sor = to_sor(output);
 	int err;
 
-	if (sor->dpaux)
-		tegra_dpaux_enable(sor->dpaux);
+	if (sor->aux)
+		drm_dp_aux_enable(sor->aux);
 
 	err = tegra_output_connector_get_modes(connector);
 
-	if (sor->dpaux)
-		tegra_dpaux_disable(sor->dpaux);
+	if (sor->aux)
+		drm_dp_aux_disable(sor->aux);
 
 	return err;
 }
@@ -1128,8 +1128,8 @@ static void tegra_sor_edp_disable(struct drm_encoder *encoder)
 	if (err < 0)
 		dev_err(sor->dev, "failed to power down SOR: %d\n", err);
 
-	if (sor->dpaux) {
-		err = tegra_dpaux_disable(sor->dpaux);
+	if (sor->aux) {
+		err = drm_dp_aux_disable(sor->aux);
 		if (err < 0)
 			dev_err(sor->dev, "failed to disable DP: %d\n", err);
 	}
@@ -1196,7 +1196,7 @@ static void tegra_sor_edp_enable(struct drm_encoder *encoder)
 	struct tegra_sor *sor = to_sor(output);
 	struct tegra_sor_config config;
 	struct drm_dp_link link;
-	struct drm_dp_aux *aux;
+	u8 rate, lanes;
 	int err = 0;
 	u32 value;
 
@@ -1209,20 +1209,14 @@ static void tegra_sor_edp_enable(struct drm_encoder *encoder)
 	if (output->panel)
 		drm_panel_prepare(output->panel);
 
-	/* FIXME: properly convert to struct drm_dp_aux */
-	aux = (struct drm_dp_aux *)sor->dpaux;
-
-	if (sor->dpaux) {
-		err = tegra_dpaux_enable(sor->dpaux);
-		if (err < 0)
-			dev_err(sor->dev, "failed to enable DP: %d\n", err);
+	err = drm_dp_aux_enable(sor->aux);
+	if (err < 0)
+		dev_err(sor->dev, "failed to enable DP: %d\n", err);
 
-		err = drm_dp_link_probe(aux, &link);
-		if (err < 0) {
-			dev_err(sor->dev, "failed to probe eDP link: %d\n",
-				err);
-			return;
-		}
+	err = drm_dp_link_probe(sor->aux, &link);
+	if (err < 0) {
+		dev_err(sor->dev, "failed to probe eDP link: %d\n", err);
+		return;
 	}
 
 	err = clk_set_parent(sor->clk, sor->clk_safe);
@@ -1434,60 +1428,51 @@ static void tegra_sor_edp_enable(struct drm_encoder *encoder)
 	value |= SOR_DP_PADCTL_PAD_CAL_PD;
 	tegra_sor_writel(sor, value, SOR_DP_PADCTL0);
 
-	if (sor->dpaux) {
-		u8 rate, lanes;
-
-		err = drm_dp_link_probe(aux, &link);
-		if (err < 0)
-			dev_err(sor->dev, "failed to probe eDP link: %d\n",
-				err);
+	err = drm_dp_link_probe(sor->aux, &link);
+	if (err < 0)
+		dev_err(sor->dev, "failed to probe eDP link: %d\n", err);
 
-		err = drm_dp_link_power_up(aux, &link);
-		if (err < 0)
-			dev_err(sor->dev, "failed to power up eDP link: %d\n",
-				err);
+	err = drm_dp_link_power_up(sor->aux, &link);
+	if (err < 0)
+		dev_err(sor->dev, "failed to power up eDP link: %d\n", err);
 
-		err = drm_dp_link_configure(aux, &link);
-		if (err < 0)
-			dev_err(sor->dev, "failed to configure eDP link: %d\n",
-				err);
+	err = drm_dp_link_configure(sor->aux, &link);
+	if (err < 0)
+		dev_err(sor->dev, "failed to configure eDP link: %d\n", err);
 
-		rate = drm_dp_link_rate_to_bw_code(link.rate);
-		lanes = link.num_lanes;
+	rate = drm_dp_link_rate_to_bw_code(link.rate);
+	lanes = link.num_lanes;
 
-		value = tegra_sor_readl(sor, SOR_CLK_CNTRL);
-		value &= ~SOR_CLK_CNTRL_DP_LINK_SPEED_MASK;
-		value |= SOR_CLK_CNTRL_DP_LINK_SPEED(rate);
-		tegra_sor_writel(sor, value, SOR_CLK_CNTRL);
+	value = tegra_sor_readl(sor, SOR_CLK_CNTRL);
+	value &= ~SOR_CLK_CNTRL_DP_LINK_SPEED_MASK;
+	value |= SOR_CLK_CNTRL_DP_LINK_SPEED(rate);
+	tegra_sor_writel(sor, value, SOR_CLK_CNTRL);
 
-		value = tegra_sor_readl(sor, SOR_DP_LINKCTL0);
-		value &= ~SOR_DP_LINKCTL_LANE_COUNT_MASK;
-		value |= SOR_DP_LINKCTL_LANE_COUNT(lanes);
+	value = tegra_sor_readl(sor, SOR_DP_LINKCTL0);
+	value &= ~SOR_DP_LINKCTL_LANE_COUNT_MASK;
+	value |= SOR_DP_LINKCTL_LANE_COUNT(lanes);
 
-		if (link.capabilities & DP_LINK_CAP_ENHANCED_FRAMING)
-			value |= SOR_DP_LINKCTL_ENHANCED_FRAME;
+	if (link.capabilities & DP_LINK_CAP_ENHANCED_FRAMING)
+		value |= SOR_DP_LINKCTL_ENHANCED_FRAME;
 
-		tegra_sor_writel(sor, value, SOR_DP_LINKCTL0);
+	tegra_sor_writel(sor, value, SOR_DP_LINKCTL0);
 
-		/* disable training pattern generator */
+	/* disable training pattern generator */
 
-		for (i = 0; i < link.num_lanes; i++) {
-			unsigned long lane = SOR_DP_TPG_CHANNEL_CODING |
-					     SOR_DP_TPG_SCRAMBLER_GALIOS |
-					     SOR_DP_TPG_PATTERN_NONE;
-			value = (value << 8) | lane;
-		}
+	for (i = 0; i < link.num_lanes; i++) {
+		unsigned long lane = SOR_DP_TPG_CHANNEL_CODING |
+				     SOR_DP_TPG_SCRAMBLER_GALIOS |
+				     SOR_DP_TPG_PATTERN_NONE;
+		value = (value << 8) | lane;
+	}
 
-		tegra_sor_writel(sor, value, SOR_DP_TPG);
+	tegra_sor_writel(sor, value, SOR_DP_TPG);
 
-		err = tegra_sor_dp_train_fast(sor, &link);
-		if (err < 0) {
-			dev_err(sor->dev, "DP fast link training failed: %d\n",
-				err);
-		}
+	err = tegra_sor_dp_train_fast(sor, &link);
+	if (err < 0)
+		dev_err(sor->dev, "DP fast link training failed: %d\n", err);
 
-		dev_dbg(sor->dev, "fast link training succeeded\n");
-	}
+	dev_dbg(sor->dev, "fast link training succeeded\n");
 
 	err = tegra_sor_power_up(sor, 250);
 	if (err < 0)
@@ -1961,9 +1946,9 @@ static void tegra_sor_hdmi_enable(struct drm_encoder *encoder)
 
 	/* production settings */
 	settings = tegra_sor_hdmi_find_settings(sor, mode->clock * 1000);
-	if (IS_ERR(settings)) {
-		dev_err(sor->dev, "no settings for pixel clock %d Hz: %ld\n",
-			mode->clock * 1000, PTR_ERR(settings));
+	if (!settings) {
+		dev_err(sor->dev, "no settings for pixel clock %d Hz\n",
+			mode->clock * 1000);
 		return;
 	}
 
@@ -2148,7 +2133,7 @@ static int tegra_sor_init(struct host1x_client *client)
 	int encoder = DRM_MODE_ENCODER_NONE;
 	int err;
 
-	if (!sor->dpaux) {
+	if (!sor->aux) {
 		if (sor->soc->supports_hdmi) {
 			connector = DRM_MODE_CONNECTOR_HDMIA;
 			encoder = DRM_MODE_ENCODER_TMDS;
@@ -2178,7 +2163,7 @@ static int tegra_sor_init(struct host1x_client *client)
 	sor->output.connector.dpms = DRM_MODE_DPMS_OFF;
 
 	drm_encoder_init(drm, &sor->output.encoder, &tegra_sor_encoder_funcs,
-			 encoder);
+			 encoder, NULL);
 	drm_encoder_helper_add(&sor->output.encoder, helpers);
 
 	drm_mode_connector_attach_encoder(&sor->output.connector,
@@ -2199,8 +2184,8 @@ static int tegra_sor_init(struct host1x_client *client)
 			dev_err(sor->dev, "debugfs setup failed: %d\n", err);
 	}
 
-	if (sor->dpaux) {
-		err = tegra_dpaux_attach(sor->dpaux, &sor->output);
+	if (sor->aux) {
+		err = drm_dp_aux_attach(sor->aux, &sor->output);
 		if (err < 0) {
 			dev_err(sor->dev, "failed to attach DP: %d\n", err);
 			return err;
@@ -2249,8 +2234,8 @@ static int tegra_sor_exit(struct host1x_client *client)
 
 	tegra_output_exit(&sor->output);
 
-	if (sor->dpaux) {
-		err = tegra_dpaux_detach(sor->dpaux);
+	if (sor->aux) {
+		err = drm_dp_aux_detach(sor->aux);
 		if (err < 0) {
 			dev_err(sor->dev, "failed to detach DP: %d\n", err);
 			return err;
@@ -2399,14 +2384,14 @@ static int tegra_sor_probe(struct platform_device *pdev)
 
 	np = of_parse_phandle(pdev->dev.of_node, "nvidia,dpaux", 0);
 	if (np) {
-		sor->dpaux = tegra_dpaux_find_by_of_node(np);
+		sor->aux = drm_dp_aux_find_by_of_node(np);
 		of_node_put(np);
 
-		if (!sor->dpaux)
+		if (!sor->aux)
 			return -EPROBE_DEFER;
 	}
 
-	if (!sor->dpaux) {
+	if (!sor->aux) {
 		if (sor->soc->supports_hdmi) {
 			sor->ops = &tegra_sor_hdmi_ops;
 		} else if (sor->soc->supports_lvds) {
diff --git a/drivers/gpu/drm/tilcdc/Kconfig b/drivers/gpu/drm/tilcdc/Kconfig
index 78beafb0742c..f60a1ec84fa4 100644
--- a/drivers/gpu/drm/tilcdc/Kconfig
+++ b/drivers/gpu/drm/tilcdc/Kconfig
@@ -1,6 +1,6 @@
 config DRM_TILCDC
 	tristate "DRM Support for TI LCDC Display Controller"
-	depends on DRM && OF && ARM && HAVE_DMA_ATTRS
+	depends on DRM && OF && ARM
 	select DRM_KMS_HELPER
 	select DRM_KMS_FB_HELPER
 	select DRM_KMS_CMA_HELPER
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_drv.c b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
index 876cad58b1f9..d7f5b897c6c5 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_drv.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
@@ -46,7 +46,7 @@ void tilcdc_module_cleanup(struct tilcdc_module *mod)
 static struct of_device_id tilcdc_of_match[];
 
 static struct drm_framebuffer *tilcdc_fb_create(struct drm_device *dev,
-		struct drm_file *file_priv, struct drm_mode_fb_cmd2 *mode_cmd)
+		struct drm_file *file_priv, const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	return drm_fb_cma_create(dev, file_priv, mode_cmd);
 }
@@ -294,6 +294,7 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 			break;
 	}
 
+	drm_helper_disable_unused_functions(dev);
 	priv->fbdev = drm_fbdev_cma_init(dev, bpp,
 			dev->mode_config.num_crtc,
 			dev->mode_config.num_connector);
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_panel.c b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
index 0af8bed7ce1e..4dda6e2f464b 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_panel.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
@@ -128,7 +128,7 @@ static struct drm_encoder *panel_encoder_create(struct drm_device *dev,
 	encoder->possible_crtcs = 1;
 
 	ret = drm_encoder_init(dev, encoder, &panel_encoder_funcs,
-			DRM_MODE_ENCODER_LVDS);
+			DRM_MODE_ENCODER_LVDS, NULL);
 	if (ret < 0)
 		goto fail;
 
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c b/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
index 354c47ca6374..5052a8af7ecb 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_tfp410.c
@@ -138,7 +138,7 @@ static struct drm_encoder *tfp410_encoder_create(struct drm_device *dev,
 	encoder->possible_crtcs = 1;
 
 	ret = drm_encoder_init(dev, encoder, &tfp410_encoder_funcs,
-			DRM_MODE_ENCODER_TMDS);
+			DRM_MODE_ENCODER_TMDS, NULL);
 	if (ret < 0)
 		goto fail;
 
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 745e996d2dbc..4cbf26555093 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -176,7 +176,7 @@ void ttm_bo_add_to_lru(struct ttm_buffer_object *bo)
 		list_add_tail(&bo->lru, &man->lru);
 		kref_get(&bo->list_kref);
 
-		if (bo->ttm != NULL) {
+		if (bo->ttm && !(bo->ttm->page_flags & TTM_PAGE_FLAG_SG)) {
 			list_add_tail(&bo->swap, &bo->glob->swap_lru);
 			kref_get(&bo->list_kref);
 		}
@@ -228,6 +228,27 @@ void ttm_bo_del_sub_from_lru(struct ttm_buffer_object *bo)
 }
 EXPORT_SYMBOL(ttm_bo_del_sub_from_lru);
 
+void ttm_bo_move_to_lru_tail(struct ttm_buffer_object *bo)
+{
+	struct ttm_bo_device *bdev = bo->bdev;
+	struct ttm_mem_type_manager *man;
+
+	lockdep_assert_held(&bo->resv->lock.base);
+
+	if (bo->mem.placement & TTM_PL_FLAG_NO_EVICT) {
+		list_del_init(&bo->swap);
+		list_del_init(&bo->lru);
+
+	} else {
+		if (bo->ttm && !(bo->ttm->page_flags & TTM_PAGE_FLAG_SG))
+			list_move_tail(&bo->swap, &bo->glob->swap_lru);
+
+		man = &bdev->man[bo->mem.mem_type];
+		list_move_tail(&bo->lru, &man->lru);
+	}
+}
+EXPORT_SYMBOL(ttm_bo_move_to_lru_tail);
+
 /*
  * Call bo->mutex locked.
  */
@@ -1170,9 +1191,15 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	if (likely(!ret))
 		ret = ttm_bo_validate(bo, placement, interruptible, false);
 
-	if (!resv)
+	if (!resv) {
 		ttm_bo_unreserve(bo);
 
+	} else if (!(bo->mem.placement & TTM_PL_FLAG_NO_EVICT)) {
+		spin_lock(&bo->glob->lru_lock);
+		ttm_bo_add_to_lru(bo);
+		spin_unlock(&bo->glob->lru_lock);
+	}
+
 	if (unlikely(ret))
 		ttm_bo_unref(&bo);
 
diff --git a/drivers/gpu/drm/ttm/ttm_bo_vm.c b/drivers/gpu/drm/ttm/ttm_bo_vm.c
index 8fb7213277cc..06d26dc438b2 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_vm.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_vm.c
@@ -35,6 +35,7 @@
 #include <ttm/ttm_placement.h>
 #include <drm/drm_vma_manager.h>
 #include <linux/mm.h>
+#include <linux/pfn_t.h>
 #include <linux/rbtree.h>
 #include <linux/module.h>
 #include <linux/uaccess.h>
@@ -229,7 +230,8 @@ static int ttm_bo_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 		}
 
 		if (vma->vm_flags & VM_MIXEDMAP)
-			ret = vm_insert_mixed(&cvma, address, pfn);
+			ret = vm_insert_mixed(&cvma, address,
+					__pfn_to_pfn_t(pfn, PFN_DEV));
 		else
 			ret = vm_insert_pfn(&cvma, address, pfn);
 
diff --git a/drivers/gpu/drm/udl/udl_connector.c b/drivers/gpu/drm/udl/udl_connector.c
index 0110d95522f3..4709b54c204c 100644
--- a/drivers/gpu/drm/udl/udl_connector.c
+++ b/drivers/gpu/drm/udl/udl_connector.c
@@ -122,13 +122,13 @@ static void udl_connector_destroy(struct drm_connector *connector)
 	kfree(connector);
 }
 
-static struct drm_connector_helper_funcs udl_connector_helper_funcs = {
+static const struct drm_connector_helper_funcs udl_connector_helper_funcs = {
 	.get_modes = udl_get_modes,
 	.mode_valid = udl_mode_valid,
 	.best_encoder = udl_best_single_encoder,
 };
 
-static struct drm_connector_funcs udl_connector_funcs = {
+static const struct drm_connector_funcs udl_connector_funcs = {
 	.dpms = drm_helper_connector_dpms,
 	.detect = udl_detect,
 	.fill_modes = drm_helper_probe_single_connector_modes,
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index 80adbac82bde..4a064efcea58 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -108,7 +108,7 @@ void udl_fbdev_unplug(struct drm_device *dev);
 struct drm_framebuffer *
 udl_fb_user_fb_create(struct drm_device *dev,
 		      struct drm_file *file,
-		      struct drm_mode_fb_cmd2 *mode_cmd);
+		      const struct drm_mode_fb_cmd2 *mode_cmd);
 
 int udl_render_hline(struct drm_device *dev, int bpp, struct urb **urb_ptr,
 		     const char *front, char **urb_buf_ptr,
diff --git a/drivers/gpu/drm/udl/udl_encoder.c b/drivers/gpu/drm/udl/udl_encoder.c
index 4052c4656498..a181a647fcf9 100644
--- a/drivers/gpu/drm/udl/udl_encoder.c
+++ b/drivers/gpu/drm/udl/udl_encoder.c
@@ -73,7 +73,8 @@ struct drm_encoder *udl_encoder_init(struct drm_device *dev)
 	if (!encoder)
 		return NULL;
 
-	drm_encoder_init(dev, encoder, &udl_enc_funcs, DRM_MODE_ENCODER_TMDS);
+	drm_encoder_init(dev, encoder, &udl_enc_funcs, DRM_MODE_ENCODER_TMDS,
+			 NULL);
 	drm_encoder_helper_add(encoder, &udl_helper_funcs);
 	encoder->possible_crtcs = 1;
 	return encoder;
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index 62c7b1dafaa4..200419d4d43c 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -33,7 +33,6 @@ module_param(fb_defio, int, S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP);
 struct udl_fbdev {
 	struct drm_fb_helper helper;
 	struct udl_framebuffer ufb;
-	struct list_head fbdev_list;
 	int fb_count;
 };
 
@@ -456,7 +455,7 @@ static const struct drm_framebuffer_funcs udlfb_funcs = {
 static int
 udl_framebuffer_init(struct drm_device *dev,
 		     struct udl_framebuffer *ufb,
-		     struct drm_mode_fb_cmd2 *mode_cmd,
+		     const struct drm_mode_fb_cmd2 *mode_cmd,
 		     struct udl_gem_object *obj)
 {
 	int ret;
@@ -624,7 +623,7 @@ void udl_fbdev_unplug(struct drm_device *dev)
 struct drm_framebuffer *
 udl_fb_user_fb_create(struct drm_device *dev,
 		   struct drm_file *file,
-		   struct drm_mode_fb_cmd2 *mode_cmd)
+		   const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj;
 	struct udl_framebuffer *ufb;
diff --git a/drivers/gpu/drm/udl/udl_modeset.c b/drivers/gpu/drm/udl/udl_modeset.c
index 677190a65e82..160ef2a08b89 100644
--- a/drivers/gpu/drm/udl/udl_modeset.c
+++ b/drivers/gpu/drm/udl/udl_modeset.c
@@ -400,7 +400,7 @@ static void udl_crtc_commit(struct drm_crtc *crtc)
 	udl_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
 }
 
-static struct drm_crtc_helper_funcs udl_helper_funcs = {
+static const struct drm_crtc_helper_funcs udl_helper_funcs = {
 	.dpms = udl_crtc_dpms,
 	.mode_fixup = udl_crtc_mode_fixup,
 	.mode_set = udl_crtc_mode_set,
diff --git a/drivers/gpu/drm/vc4/Kconfig b/drivers/gpu/drm/vc4/Kconfig
index 2d7d115ddf3f..584810474e5b 100644
--- a/drivers/gpu/drm/vc4/Kconfig
+++ b/drivers/gpu/drm/vc4/Kconfig
@@ -1,7 +1,7 @@
 config DRM_VC4
 	tristate "Broadcom VC4 Graphics"
 	depends on ARCH_BCM2835 || COMPILE_TEST
-	depends on DRM && HAVE_DMA_ATTRS
+	depends on DRM
 	select DRM_KMS_HELPER
 	select DRM_KMS_CMA_HELPER
 	select DRM_GEM_CMA_HELPER
diff --git a/drivers/gpu/drm/vc4/Makefile b/drivers/gpu/drm/vc4/Makefile
index 32b4f9cd8f52..4c6a99f0398c 100644
--- a/drivers/gpu/drm/vc4/Makefile
+++ b/drivers/gpu/drm/vc4/Makefile
@@ -8,10 +8,19 @@ vc4-y := \
 	vc4_crtc.o \
 	vc4_drv.o \
 	vc4_kms.o \
+	vc4_gem.o \
 	vc4_hdmi.o \
 	vc4_hvs.o \
-	vc4_plane.o
+	vc4_irq.o \
+	vc4_plane.o \
+	vc4_render_cl.o \
+	vc4_trace_points.o \
+	vc4_v3d.o \
+	vc4_validate.o \
+	vc4_validate_shaders.o
 
 vc4-$(CONFIG_DEBUG_FS) += vc4_debugfs.o
 
 obj-$(CONFIG_DRM_VC4)  += vc4.o
+
+CFLAGS_vc4_trace_points.o := -I$(src)
diff --git a/drivers/gpu/drm/vc4/vc4_bo.c b/drivers/gpu/drm/vc4/vc4_bo.c
index ab9f5108ae1a..22278bcfc60e 100644
--- a/drivers/gpu/drm/vc4/vc4_bo.c
+++ b/drivers/gpu/drm/vc4/vc4_bo.c
@@ -12,19 +12,236 @@
  * access to system memory with no MMU in between.  To support it, we
  * use the GEM CMA helper functions to allocate contiguous ranges of
  * physical memory for our BOs.
+ *
+ * Since the CMA allocator is very slow, we keep a cache of recently
+ * freed BOs around so that the kernel's allocation of objects for 3D
+ * rendering can return quickly.
  */
 
 #include "vc4_drv.h"
+#include "uapi/drm/vc4_drm.h"
+
+static void vc4_bo_stats_dump(struct vc4_dev *vc4)
+{
+	DRM_INFO("num bos allocated: %d\n",
+		 vc4->bo_stats.num_allocated);
+	DRM_INFO("size bos allocated: %dkb\n",
+		 vc4->bo_stats.size_allocated / 1024);
+	DRM_INFO("num bos used: %d\n",
+		 vc4->bo_stats.num_allocated - vc4->bo_stats.num_cached);
+	DRM_INFO("size bos used: %dkb\n",
+		 (vc4->bo_stats.size_allocated -
+		  vc4->bo_stats.size_cached) / 1024);
+	DRM_INFO("num bos cached: %d\n",
+		 vc4->bo_stats.num_cached);
+	DRM_INFO("size bos cached: %dkb\n",
+		 vc4->bo_stats.size_cached / 1024);
+}
+
+#ifdef CONFIG_DEBUG_FS
+int vc4_bo_stats_debugfs(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct vc4_bo_stats stats;
+
+	/* Take a snapshot of the current stats with the lock held. */
+	mutex_lock(&vc4->bo_lock);
+	stats = vc4->bo_stats;
+	mutex_unlock(&vc4->bo_lock);
+
+	seq_printf(m, "num bos allocated: %d\n",
+		   stats.num_allocated);
+	seq_printf(m, "size bos allocated: %dkb\n",
+		   stats.size_allocated / 1024);
+	seq_printf(m, "num bos used: %d\n",
+		   stats.num_allocated - stats.num_cached);
+	seq_printf(m, "size bos used: %dkb\n",
+		   (stats.size_allocated - stats.size_cached) / 1024);
+	seq_printf(m, "num bos cached: %d\n",
+		   stats.num_cached);
+	seq_printf(m, "size bos cached: %dkb\n",
+		   stats.size_cached / 1024);
+
+	return 0;
+}
+#endif
+
+static uint32_t bo_page_index(size_t size)
+{
+	return (size / PAGE_SIZE) - 1;
+}
+
+/* Must be called with bo_lock held. */
+static void vc4_bo_destroy(struct vc4_bo *bo)
+{
+	struct drm_gem_object *obj = &bo->base.base;
+	struct vc4_dev *vc4 = to_vc4_dev(obj->dev);
+
+	if (bo->validated_shader) {
+		kfree(bo->validated_shader->texture_samples);
+		kfree(bo->validated_shader);
+		bo->validated_shader = NULL;
+	}
+
+	vc4->bo_stats.num_allocated--;
+	vc4->bo_stats.size_allocated -= obj->size;
+	drm_gem_cma_free_object(obj);
+}
+
+/* Must be called with bo_lock held. */
+static void vc4_bo_remove_from_cache(struct vc4_bo *bo)
+{
+	struct drm_gem_object *obj = &bo->base.base;
+	struct vc4_dev *vc4 = to_vc4_dev(obj->dev);
+
+	vc4->bo_stats.num_cached--;
+	vc4->bo_stats.size_cached -= obj->size;
+
+	list_del(&bo->unref_head);
+	list_del(&bo->size_head);
+}
+
+static struct list_head *vc4_get_cache_list_for_size(struct drm_device *dev,
+						     size_t size)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint32_t page_index = bo_page_index(size);
+
+	if (vc4->bo_cache.size_list_size <= page_index) {
+		uint32_t new_size = max(vc4->bo_cache.size_list_size * 2,
+					page_index + 1);
+		struct list_head *new_list;
+		uint32_t i;
+
+		new_list = kmalloc_array(new_size, sizeof(struct list_head),
+					 GFP_KERNEL);
+		if (!new_list)
+			return NULL;
+
+		/* Rebase the old cached BO lists to their new list
+		 * head locations.
+		 */
+		for (i = 0; i < vc4->bo_cache.size_list_size; i++) {
+			struct list_head *old_list =
+				&vc4->bo_cache.size_list[i];
+
+			if (list_empty(old_list))
+				INIT_LIST_HEAD(&new_list[i]);
+			else
+				list_replace(old_list, &new_list[i]);
+		}
+		/* And initialize the brand new BO list heads. */
+		for (i = vc4->bo_cache.size_list_size; i < new_size; i++)
+			INIT_LIST_HEAD(&new_list[i]);
+
+		kfree(vc4->bo_cache.size_list);
+		vc4->bo_cache.size_list = new_list;
+		vc4->bo_cache.size_list_size = new_size;
+	}
+
+	return &vc4->bo_cache.size_list[page_index];
+}
+
+void vc4_bo_cache_purge(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	mutex_lock(&vc4->bo_lock);
+	while (!list_empty(&vc4->bo_cache.time_list)) {
+		struct vc4_bo *bo = list_last_entry(&vc4->bo_cache.time_list,
+						    struct vc4_bo, unref_head);
+		vc4_bo_remove_from_cache(bo);
+		vc4_bo_destroy(bo);
+	}
+	mutex_unlock(&vc4->bo_lock);
+}
+
+static struct vc4_bo *vc4_bo_get_from_cache(struct drm_device *dev,
+					    uint32_t size)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint32_t page_index = bo_page_index(size);
+	struct vc4_bo *bo = NULL;
+
+	size = roundup(size, PAGE_SIZE);
 
-struct vc4_bo *vc4_bo_create(struct drm_device *dev, size_t size)
+	mutex_lock(&vc4->bo_lock);
+	if (page_index >= vc4->bo_cache.size_list_size)
+		goto out;
+
+	if (list_empty(&vc4->bo_cache.size_list[page_index]))
+		goto out;
+
+	bo = list_first_entry(&vc4->bo_cache.size_list[page_index],
+			      struct vc4_bo, size_head);
+	vc4_bo_remove_from_cache(bo);
+	kref_init(&bo->base.base.refcount);
+
+out:
+	mutex_unlock(&vc4->bo_lock);
+	return bo;
+}
+
+/**
+ * vc4_gem_create_object - Implementation of driver->gem_create_object.
+ *
+ * This lets the CMA helpers allocate object structs for us, and keep
+ * our BO stats correct.
+ */
+struct drm_gem_object *vc4_create_object(struct drm_device *dev, size_t size)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct vc4_bo *bo;
+
+	bo = kzalloc(sizeof(*bo), GFP_KERNEL);
+	if (!bo)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&vc4->bo_lock);
+	vc4->bo_stats.num_allocated++;
+	vc4->bo_stats.size_allocated += size;
+	mutex_unlock(&vc4->bo_lock);
+
+	return &bo->base.base;
+}
+
+struct vc4_bo *vc4_bo_create(struct drm_device *dev, size_t unaligned_size,
+			     bool from_cache)
 {
+	size_t size = roundup(unaligned_size, PAGE_SIZE);
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
 	struct drm_gem_cma_object *cma_obj;
 
+	if (size == 0)
+		return ERR_PTR(-EINVAL);
+
+	/* First, try to get a vc4_bo from the kernel BO cache. */
+	if (from_cache) {
+		struct vc4_bo *bo = vc4_bo_get_from_cache(dev, size);
+
+		if (bo)
+			return bo;
+	}
+
 	cma_obj = drm_gem_cma_create(dev, size);
-	if (IS_ERR(cma_obj))
-		return NULL;
-	else
-		return to_vc4_bo(&cma_obj->base);
+	if (IS_ERR(cma_obj)) {
+		/*
+		 * If we've run out of CMA memory, kill the cache of
+		 * CMA allocations we've got laying around and try again.
+		 */
+		vc4_bo_cache_purge(dev);
+
+		cma_obj = drm_gem_cma_create(dev, size);
+		if (IS_ERR(cma_obj)) {
+			DRM_ERROR("Failed to allocate from CMA:\n");
+			vc4_bo_stats_dump(vc4);
+			return ERR_PTR(-ENOMEM);
+		}
+	}
+
+	return to_vc4_bo(&cma_obj->base);
 }
 
 int vc4_dumb_create(struct drm_file *file_priv,
@@ -41,12 +258,300 @@ int vc4_dumb_create(struct drm_file *file_priv,
 	if (args->size < args->pitch * args->height)
 		args->size = args->pitch * args->height;
 
-	bo = vc4_bo_create(dev, roundup(args->size, PAGE_SIZE));
-	if (!bo)
-		return -ENOMEM;
+	bo = vc4_bo_create(dev, args->size, false);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
 
 	ret = drm_gem_handle_create(file_priv, &bo->base.base, &args->handle);
 	drm_gem_object_unreference_unlocked(&bo->base.base);
 
 	return ret;
 }
+
+/* Must be called with bo_lock held. */
+static void vc4_bo_cache_free_old(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	unsigned long expire_time = jiffies - msecs_to_jiffies(1000);
+
+	while (!list_empty(&vc4->bo_cache.time_list)) {
+		struct vc4_bo *bo = list_last_entry(&vc4->bo_cache.time_list,
+						    struct vc4_bo, unref_head);
+		if (time_before(expire_time, bo->free_time)) {
+			mod_timer(&vc4->bo_cache.time_timer,
+				  round_jiffies_up(jiffies +
+						   msecs_to_jiffies(1000)));
+			return;
+		}
+
+		vc4_bo_remove_from_cache(bo);
+		vc4_bo_destroy(bo);
+	}
+}
+
+/* Called on the last userspace/kernel unreference of the BO.  Returns
+ * it to the BO cache if possible, otherwise frees it.
+ *
+ * Note that this is called with the struct_mutex held.
+ */
+void vc4_free_object(struct drm_gem_object *gem_bo)
+{
+	struct drm_device *dev = gem_bo->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct vc4_bo *bo = to_vc4_bo(gem_bo);
+	struct list_head *cache_list;
+
+	mutex_lock(&vc4->bo_lock);
+	/* If the object references someone else's memory, we can't cache it.
+	 */
+	if (gem_bo->import_attach) {
+		vc4_bo_destroy(bo);
+		goto out;
+	}
+
+	/* Don't cache if it was publicly named. */
+	if (gem_bo->name) {
+		vc4_bo_destroy(bo);
+		goto out;
+	}
+
+	cache_list = vc4_get_cache_list_for_size(dev, gem_bo->size);
+	if (!cache_list) {
+		vc4_bo_destroy(bo);
+		goto out;
+	}
+
+	if (bo->validated_shader) {
+		kfree(bo->validated_shader->texture_samples);
+		kfree(bo->validated_shader);
+		bo->validated_shader = NULL;
+	}
+
+	bo->free_time = jiffies;
+	list_add(&bo->size_head, cache_list);
+	list_add(&bo->unref_head, &vc4->bo_cache.time_list);
+
+	vc4->bo_stats.num_cached++;
+	vc4->bo_stats.size_cached += gem_bo->size;
+
+	vc4_bo_cache_free_old(dev);
+
+out:
+	mutex_unlock(&vc4->bo_lock);
+}
+
+static void vc4_bo_cache_time_work(struct work_struct *work)
+{
+	struct vc4_dev *vc4 =
+		container_of(work, struct vc4_dev, bo_cache.time_work);
+	struct drm_device *dev = vc4->dev;
+
+	mutex_lock(&vc4->bo_lock);
+	vc4_bo_cache_free_old(dev);
+	mutex_unlock(&vc4->bo_lock);
+}
+
+static void vc4_bo_cache_time_timer(unsigned long data)
+{
+	struct drm_device *dev = (struct drm_device *)data;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	schedule_work(&vc4->bo_cache.time_work);
+}
+
+struct dma_buf *
+vc4_prime_export(struct drm_device *dev, struct drm_gem_object *obj, int flags)
+{
+	struct vc4_bo *bo = to_vc4_bo(obj);
+
+	if (bo->validated_shader) {
+		DRM_ERROR("Attempting to export shader BO\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	return drm_gem_prime_export(dev, obj, flags);
+}
+
+int vc4_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	struct drm_gem_object *gem_obj;
+	struct vc4_bo *bo;
+	int ret;
+
+	ret = drm_gem_mmap(filp, vma);
+	if (ret)
+		return ret;
+
+	gem_obj = vma->vm_private_data;
+	bo = to_vc4_bo(gem_obj);
+
+	if (bo->validated_shader && (vma->vm_flags & VM_WRITE)) {
+		DRM_ERROR("mmaping of shader BOs for writing not allowed.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Clear the VM_PFNMAP flag that was set by drm_gem_mmap(), and set the
+	 * vm_pgoff (used as a fake buffer offset by DRM) to 0 as we want to map
+	 * the whole buffer.
+	 */
+	vma->vm_flags &= ~VM_PFNMAP;
+	vma->vm_pgoff = 0;
+
+	ret = dma_mmap_writecombine(bo->base.base.dev->dev, vma,
+				    bo->base.vaddr, bo->base.paddr,
+				    vma->vm_end - vma->vm_start);
+	if (ret)
+		drm_gem_vm_close(vma);
+
+	return ret;
+}
+
+int vc4_prime_mmap(struct drm_gem_object *obj, struct vm_area_struct *vma)
+{
+	struct vc4_bo *bo = to_vc4_bo(obj);
+
+	if (bo->validated_shader && (vma->vm_flags & VM_WRITE)) {
+		DRM_ERROR("mmaping of shader BOs for writing not allowed.\n");
+		return -EINVAL;
+	}
+
+	return drm_gem_cma_prime_mmap(obj, vma);
+}
+
+void *vc4_prime_vmap(struct drm_gem_object *obj)
+{
+	struct vc4_bo *bo = to_vc4_bo(obj);
+
+	if (bo->validated_shader) {
+		DRM_ERROR("mmaping of shader BOs not allowed.\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	return drm_gem_cma_prime_vmap(obj);
+}
+
+int vc4_create_bo_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
+{
+	struct drm_vc4_create_bo *args = data;
+	struct vc4_bo *bo = NULL;
+	int ret;
+
+	/*
+	 * We can't allocate from the BO cache, because the BOs don't
+	 * get zeroed, and that might leak data between users.
+	 */
+	bo = vc4_bo_create(dev, args->size, false);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
+
+	ret = drm_gem_handle_create(file_priv, &bo->base.base, &args->handle);
+	drm_gem_object_unreference_unlocked(&bo->base.base);
+
+	return ret;
+}
+
+int vc4_mmap_bo_ioctl(struct drm_device *dev, void *data,
+		      struct drm_file *file_priv)
+{
+	struct drm_vc4_mmap_bo *args = data;
+	struct drm_gem_object *gem_obj;
+
+	gem_obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!gem_obj) {
+		DRM_ERROR("Failed to look up GEM BO %d\n", args->handle);
+		return -EINVAL;
+	}
+
+	/* The mmap offset was set up at BO allocation time. */
+	args->offset = drm_vma_node_offset_addr(&gem_obj->vma_node);
+
+	drm_gem_object_unreference_unlocked(gem_obj);
+	return 0;
+}
+
+int
+vc4_create_shader_bo_ioctl(struct drm_device *dev, void *data,
+			   struct drm_file *file_priv)
+{
+	struct drm_vc4_create_shader_bo *args = data;
+	struct vc4_bo *bo = NULL;
+	int ret;
+
+	if (args->size == 0)
+		return -EINVAL;
+
+	if (args->size % sizeof(u64) != 0)
+		return -EINVAL;
+
+	if (args->flags != 0) {
+		DRM_INFO("Unknown flags set: 0x%08x\n", args->flags);
+		return -EINVAL;
+	}
+
+	if (args->pad != 0) {
+		DRM_INFO("Pad set: 0x%08x\n", args->pad);
+		return -EINVAL;
+	}
+
+	bo = vc4_bo_create(dev, args->size, true);
+	if (IS_ERR(bo))
+		return PTR_ERR(bo);
+
+	ret = copy_from_user(bo->base.vaddr,
+			     (void __user *)(uintptr_t)args->data,
+			     args->size);
+	if (ret != 0)
+		goto fail;
+	/* Clear the rest of the memory from allocating from the BO
+	 * cache.
+	 */
+	memset(bo->base.vaddr + args->size, 0,
+	       bo->base.base.size - args->size);
+
+	bo->validated_shader = vc4_validate_shader(&bo->base);
+	if (!bo->validated_shader) {
+		ret = -EINVAL;
+		goto fail;
+	}
+
+	/* We have to create the handle after validation, to avoid
+	 * races for users to do doing things like mmap the shader BO.
+	 */
+	ret = drm_gem_handle_create(file_priv, &bo->base.base, &args->handle);
+
+ fail:
+	drm_gem_object_unreference_unlocked(&bo->base.base);
+
+	return ret;
+}
+
+void vc4_bo_cache_init(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	mutex_init(&vc4->bo_lock);
+
+	INIT_LIST_HEAD(&vc4->bo_cache.time_list);
+
+	INIT_WORK(&vc4->bo_cache.time_work, vc4_bo_cache_time_work);
+	setup_timer(&vc4->bo_cache.time_timer,
+		    vc4_bo_cache_time_timer,
+		    (unsigned long)dev);
+}
+
+void vc4_bo_cache_destroy(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	del_timer(&vc4->bo_cache.time_timer);
+	cancel_work_sync(&vc4->bo_cache.time_work);
+
+	vc4_bo_cache_purge(dev);
+
+	if (vc4->bo_stats.num_allocated) {
+		DRM_ERROR("Destroying BO cache while BOs still allocated:\n");
+		vc4_bo_stats_dump(vc4);
+	}
+}
diff --git a/drivers/gpu/drm/vc4/vc4_crtc.c b/drivers/gpu/drm/vc4/vc4_crtc.c
index 265064c62d49..018145e0b87d 100644
--- a/drivers/gpu/drm/vc4/vc4_crtc.c
+++ b/drivers/gpu/drm/vc4/vc4_crtc.c
@@ -35,6 +35,7 @@
 #include "drm_atomic_helper.h"
 #include "drm_crtc_helper.h"
 #include "linux/clk.h"
+#include "drm_fb_cma_helper.h"
 #include "linux/component.h"
 #include "linux/of_device.h"
 #include "vc4_drv.h"
@@ -327,7 +328,7 @@ static int vc4_crtc_atomic_check(struct drm_crtc *crtc,
 	/* The pixelvalve can only feed one encoder (and encoders are
 	 * 1:1 with connectors.)
 	 */
-	if (drm_atomic_connectors_for_crtc(state->state, crtc) > 1)
+	if (hweight32(state->connector_mask) > 1)
 		return -EINVAL;
 
 	drm_atomic_crtc_state_for_each_plane(plane, state) {
@@ -476,10 +477,106 @@ static irqreturn_t vc4_crtc_irq_handler(int irq, void *data)
 	return ret;
 }
 
+struct vc4_async_flip_state {
+	struct drm_crtc *crtc;
+	struct drm_framebuffer *fb;
+	struct drm_pending_vblank_event *event;
+
+	struct vc4_seqno_cb cb;
+};
+
+/* Called when the V3D execution for the BO being flipped to is done, so that
+ * we can actually update the plane's address to point to it.
+ */
+static void
+vc4_async_page_flip_complete(struct vc4_seqno_cb *cb)
+{
+	struct vc4_async_flip_state *flip_state =
+		container_of(cb, struct vc4_async_flip_state, cb);
+	struct drm_crtc *crtc = flip_state->crtc;
+	struct drm_device *dev = crtc->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct drm_plane *plane = crtc->primary;
+
+	vc4_plane_async_set_fb(plane, flip_state->fb);
+	if (flip_state->event) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&dev->event_lock, flags);
+		drm_crtc_send_vblank_event(crtc, flip_state->event);
+		spin_unlock_irqrestore(&dev->event_lock, flags);
+	}
+
+	drm_framebuffer_unreference(flip_state->fb);
+	kfree(flip_state);
+
+	up(&vc4->async_modeset);
+}
+
+/* Implements async (non-vblank-synced) page flips.
+ *
+ * The page flip ioctl needs to return immediately, so we grab the
+ * modeset semaphore on the pipe, and queue the address update for
+ * when V3D is done with the BO being flipped to.
+ */
+static int vc4_async_page_flip(struct drm_crtc *crtc,
+			       struct drm_framebuffer *fb,
+			       struct drm_pending_vblank_event *event,
+			       uint32_t flags)
+{
+	struct drm_device *dev = crtc->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct drm_plane *plane = crtc->primary;
+	int ret = 0;
+	struct vc4_async_flip_state *flip_state;
+	struct drm_gem_cma_object *cma_bo = drm_fb_cma_get_gem_obj(fb, 0);
+	struct vc4_bo *bo = to_vc4_bo(&cma_bo->base);
+
+	flip_state = kzalloc(sizeof(*flip_state), GFP_KERNEL);
+	if (!flip_state)
+		return -ENOMEM;
+
+	drm_framebuffer_reference(fb);
+	flip_state->fb = fb;
+	flip_state->crtc = crtc;
+	flip_state->event = event;
+
+	/* Make sure all other async modesetes have landed. */
+	ret = down_interruptible(&vc4->async_modeset);
+	if (ret) {
+		kfree(flip_state);
+		return ret;
+	}
+
+	/* Immediately update the plane's legacy fb pointer, so that later
+	 * modeset prep sees the state that will be present when the semaphore
+	 * is released.
+	 */
+	drm_atomic_set_fb_for_plane(plane->state, fb);
+	plane->fb = fb;
+
+	vc4_queue_seqno_cb(dev, &flip_state->cb, bo->seqno,
+			   vc4_async_page_flip_complete);
+
+	/* Driver takes ownership of state on successful async commit. */
+	return 0;
+}
+
+static int vc4_page_flip(struct drm_crtc *crtc,
+			 struct drm_framebuffer *fb,
+			 struct drm_pending_vblank_event *event,
+			 uint32_t flags)
+{
+	if (flags & DRM_MODE_PAGE_FLIP_ASYNC)
+		return vc4_async_page_flip(crtc, fb, event, flags);
+	else
+		return drm_atomic_helper_page_flip(crtc, fb, event, flags);
+}
+
 static const struct drm_crtc_funcs vc4_crtc_funcs = {
 	.set_config = drm_atomic_helper_set_config,
 	.destroy = vc4_crtc_destroy,
-	.page_flip = drm_atomic_helper_page_flip,
+	.page_flip = vc4_page_flip,
 	.set_property = NULL,
 	.cursor_set = NULL, /* handled by drm_mode_cursor_universal */
 	.cursor_move = NULL, /* handled by drm_mode_cursor_universal */
@@ -606,7 +703,7 @@ static int vc4_crtc_bind(struct device *dev, struct device *master, void *data)
 	}
 
 	drm_crtc_init_with_planes(drm, crtc, primary_plane, cursor_plane,
-				  &vc4_crtc_funcs);
+				  &vc4_crtc_funcs, NULL);
 	drm_crtc_helper_add(crtc, &vc4_crtc_helper_funcs);
 	primary_plane->crtc = crtc;
 	cursor_plane->crtc = crtc;
diff --git a/drivers/gpu/drm/vc4/vc4_debugfs.c b/drivers/gpu/drm/vc4/vc4_debugfs.c
index 4297b0a5b74e..d76ad10b07fd 100644
--- a/drivers/gpu/drm/vc4/vc4_debugfs.c
+++ b/drivers/gpu/drm/vc4/vc4_debugfs.c
@@ -16,11 +16,14 @@
 #include "vc4_regs.h"
 
 static const struct drm_info_list vc4_debugfs_list[] = {
+	{"bo_stats", vc4_bo_stats_debugfs, 0},
 	{"hdmi_regs", vc4_hdmi_debugfs_regs, 0},
 	{"hvs_regs", vc4_hvs_debugfs_regs, 0},
 	{"crtc0_regs", vc4_crtc_debugfs_regs, 0, (void *)(uintptr_t)0},
 	{"crtc1_regs", vc4_crtc_debugfs_regs, 0, (void *)(uintptr_t)1},
 	{"crtc2_regs", vc4_crtc_debugfs_regs, 0, (void *)(uintptr_t)2},
+	{"v3d_ident", vc4_v3d_debugfs_ident, 0},
+	{"v3d_regs", vc4_v3d_debugfs_regs, 0},
 };
 
 #define VC4_DEBUGFS_ENTRIES ARRAY_SIZE(vc4_debugfs_list)
diff --git a/drivers/gpu/drm/vc4/vc4_drv.c b/drivers/gpu/drm/vc4/vc4_drv.c
index d5db9e0f3b73..f1655fff8425 100644
--- a/drivers/gpu/drm/vc4/vc4_drv.c
+++ b/drivers/gpu/drm/vc4/vc4_drv.c
@@ -16,6 +16,7 @@
 #include <linux/platform_device.h>
 #include "drm_fb_cma_helper.h"
 
+#include "uapi/drm/vc4_drm.h"
 #include "vc4_drv.h"
 #include "vc4_regs.h"
 
@@ -63,7 +64,7 @@ static const struct file_operations vc4_drm_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_gem_cma_mmap,
+	.mmap = vc4_mmap,
 	.poll = drm_poll,
 	.read = drm_read,
 #ifdef CONFIG_COMPAT
@@ -73,16 +74,30 @@ static const struct file_operations vc4_drm_fops = {
 };
 
 static const struct drm_ioctl_desc vc4_drm_ioctls[] = {
+	DRM_IOCTL_DEF_DRV(VC4_SUBMIT_CL, vc4_submit_cl_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_WAIT_SEQNO, vc4_wait_seqno_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_WAIT_BO, vc4_wait_bo_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_CREATE_BO, vc4_create_bo_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_MMAP_BO, vc4_mmap_bo_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_CREATE_SHADER_BO, vc4_create_shader_bo_ioctl, 0),
+	DRM_IOCTL_DEF_DRV(VC4_GET_HANG_STATE, vc4_get_hang_state_ioctl,
+			  DRM_ROOT_ONLY),
 };
 
 static struct drm_driver vc4_drm_driver = {
 	.driver_features = (DRIVER_MODESET |
 			    DRIVER_ATOMIC |
 			    DRIVER_GEM |
+			    DRIVER_HAVE_IRQ |
 			    DRIVER_PRIME),
 	.lastclose = vc4_lastclose,
 	.preclose = vc4_drm_preclose,
 
+	.irq_handler = vc4_irq,
+	.irq_preinstall = vc4_irq_preinstall,
+	.irq_postinstall = vc4_irq_postinstall,
+	.irq_uninstall = vc4_irq_uninstall,
+
 	.enable_vblank = vc4_enable_vblank,
 	.disable_vblank = vc4_disable_vblank,
 	.get_vblank_counter = drm_vblank_count,
@@ -92,18 +107,19 @@ static struct drm_driver vc4_drm_driver = {
 	.debugfs_cleanup = vc4_debugfs_cleanup,
 #endif
 
-	.gem_free_object = drm_gem_cma_free_object,
+	.gem_create_object = vc4_create_object,
+	.gem_free_object = vc4_free_object,
 	.gem_vm_ops = &drm_gem_cma_vm_ops,
 
 	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
 	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
 	.gem_prime_import = drm_gem_prime_import,
-	.gem_prime_export = drm_gem_prime_export,
+	.gem_prime_export = vc4_prime_export,
 	.gem_prime_get_sg_table	= drm_gem_cma_prime_get_sg_table,
 	.gem_prime_import_sg_table = drm_gem_cma_prime_import_sg_table,
-	.gem_prime_vmap = drm_gem_cma_prime_vmap,
+	.gem_prime_vmap = vc4_prime_vmap,
 	.gem_prime_vunmap = drm_gem_cma_prime_vunmap,
-	.gem_prime_mmap = drm_gem_cma_prime_mmap,
+	.gem_prime_mmap = vc4_prime_mmap,
 
 	.dumb_create = vc4_dumb_create,
 	.dumb_map_offset = drm_gem_cma_dumb_map_offset,
@@ -168,15 +184,17 @@ static int vc4_drm_bind(struct device *dev)
 	vc4->dev = drm;
 	drm->dev_private = vc4;
 
-	drm_dev_set_unique(drm, dev_name(dev));
+	vc4_bo_cache_init(drm);
 
 	drm_mode_config_init(drm);
 	if (ret)
 		goto unref;
 
+	vc4_gem_init(drm);
+
 	ret = component_bind_all(dev, drm);
 	if (ret)
-		goto unref;
+		goto gem_destroy;
 
 	ret = drm_dev_register(drm, 0);
 	if (ret < 0)
@@ -200,8 +218,11 @@ unregister:
 	drm_dev_unregister(drm);
 unbind_all:
 	component_unbind_all(dev, drm);
+gem_destroy:
+	vc4_gem_destroy(drm);
 unref:
 	drm_dev_unref(drm);
+	vc4_bo_cache_destroy(drm);
 	return ret;
 }
 
@@ -228,6 +249,7 @@ static struct platform_driver *const component_drivers[] = {
 	&vc4_hdmi_driver,
 	&vc4_crtc_driver,
 	&vc4_hvs_driver,
+	&vc4_v3d_driver,
 };
 
 static int vc4_platform_drm_probe(struct platform_device *pdev)
diff --git a/drivers/gpu/drm/vc4/vc4_drv.h b/drivers/gpu/drm/vc4/vc4_drv.h
index fd8319fa682e..51a63330d4f8 100644
--- a/drivers/gpu/drm/vc4/vc4_drv.h
+++ b/drivers/gpu/drm/vc4/vc4_drv.h
@@ -15,8 +15,93 @@ struct vc4_dev {
 	struct vc4_hdmi *hdmi;
 	struct vc4_hvs *hvs;
 	struct vc4_crtc *crtc[3];
+	struct vc4_v3d *v3d;
 
 	struct drm_fbdev_cma *fbdev;
+
+	struct vc4_hang_state *hang_state;
+
+	/* The kernel-space BO cache.  Tracks buffers that have been
+	 * unreferenced by all other users (refcounts of 0!) but not
+	 * yet freed, so we can do cheap allocations.
+	 */
+	struct vc4_bo_cache {
+		/* Array of list heads for entries in the BO cache,
+		 * based on number of pages, so we can do O(1) lookups
+		 * in the cache when allocating.
+		 */
+		struct list_head *size_list;
+		uint32_t size_list_size;
+
+		/* List of all BOs in the cache, ordered by age, so we
+		 * can do O(1) lookups when trying to free old
+		 * buffers.
+		 */
+		struct list_head time_list;
+		struct work_struct time_work;
+		struct timer_list time_timer;
+	} bo_cache;
+
+	struct vc4_bo_stats {
+		u32 num_allocated;
+		u32 size_allocated;
+		u32 num_cached;
+		u32 size_cached;
+	} bo_stats;
+
+	/* Protects bo_cache and the BO stats. */
+	struct mutex bo_lock;
+
+	/* Sequence number for the last job queued in job_list.
+	 * Starts at 0 (no jobs emitted).
+	 */
+	uint64_t emit_seqno;
+
+	/* Sequence number for the last completed job on the GPU.
+	 * Starts at 0 (no jobs completed).
+	 */
+	uint64_t finished_seqno;
+
+	/* List of all struct vc4_exec_info for jobs to be executed.
+	 * The first job in the list is the one currently programmed
+	 * into ct0ca/ct1ca for execution.
+	 */
+	struct list_head job_list;
+	/* List of the finished vc4_exec_infos waiting to be freed by
+	 * job_done_work.
+	 */
+	struct list_head job_done_list;
+	/* Spinlock used to synchronize the job_list and seqno
+	 * accesses between the IRQ handler and GEM ioctls.
+	 */
+	spinlock_t job_lock;
+	wait_queue_head_t job_wait_queue;
+	struct work_struct job_done_work;
+
+	/* List of struct vc4_seqno_cb for callbacks to be made from a
+	 * workqueue when the given seqno is passed.
+	 */
+	struct list_head seqno_cb_list;
+
+	/* The binner overflow memory that's currently set up in
+	 * BPOA/BPOS registers.  When overflow occurs and a new one is
+	 * allocated, the previous one will be moved to
+	 * vc4->current_exec's free list.
+	 */
+	struct vc4_bo *overflow_mem;
+	struct work_struct overflow_mem_work;
+
+	int power_refcount;
+
+	/* Mutex controlling the power refcount. */
+	struct mutex power_lock;
+
+	struct {
+		struct timer_list timer;
+		struct work_struct reset_work;
+	} hangcheck;
+
+	struct semaphore async_modeset;
 };
 
 static inline struct vc4_dev *
@@ -27,6 +112,25 @@ 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. */
+	uint64_t seqno;
+
+	/* List entry for the BO's position in either
+	 * vc4_exec_info->unref_list or vc4_dev->bo_cache.time_list
+	 */
+	struct list_head unref_head;
+
+	/* Time in jiffies when the BO was put in vc4->bo_cache. */
+	unsigned long free_time;
+
+	/* List entry for the BO's position in vc4_dev->bo_cache.size_list */
+	struct list_head size_head;
+
+	/* Struct for shader validation state, if created by
+	 * DRM_IOCTL_VC4_CREATE_SHADER_BO.
+	 */
+	struct vc4_validated_shader_info *validated_shader;
 };
 
 static inline struct vc4_bo *
@@ -35,6 +139,18 @@ to_vc4_bo(struct drm_gem_object *bo)
 	return (struct vc4_bo *)bo;
 }
 
+struct vc4_seqno_cb {
+	struct work_struct work;
+	uint64_t seqno;
+	void (*func)(struct vc4_seqno_cb *cb);
+};
+
+struct vc4_v3d {
+	struct vc4_dev *vc4;
+	struct platform_device *pdev;
+	void __iomem *regs;
+};
+
 struct vc4_hvs {
 	struct platform_device *pdev;
 	void __iomem *regs;
@@ -72,9 +188,147 @@ to_vc4_encoder(struct drm_encoder *encoder)
 	return container_of(encoder, struct vc4_encoder, base);
 }
 
+#define V3D_READ(offset) readl(vc4->v3d->regs + offset)
+#define V3D_WRITE(offset, val) writel(val, vc4->v3d->regs + offset)
 #define HVS_READ(offset) readl(vc4->hvs->regs + offset)
 #define HVS_WRITE(offset, val) writel(val, vc4->hvs->regs + offset)
 
+struct vc4_exec_info {
+	/* Sequence number for this bin/render job. */
+	uint64_t seqno;
+
+	/* Last current addresses the hardware was processing when the
+	 * hangcheck timer checked on us.
+	 */
+	uint32_t last_ct0ca, last_ct1ca;
+
+	/* Kernel-space copy of the ioctl arguments */
+	struct drm_vc4_submit_cl *args;
+
+	/* This is the array of BOs that were looked up at the start of exec.
+	 * Command validation will use indices into this array.
+	 */
+	struct drm_gem_cma_object **bo;
+	uint32_t bo_count;
+
+	/* Pointers for our position in vc4->job_list */
+	struct list_head head;
+
+	/* List of other BOs used in the job that need to be released
+	 * once the job is complete.
+	 */
+	struct list_head unref_list;
+
+	/* Current unvalidated indices into @bo loaded by the non-hardware
+	 * VC4_PACKET_GEM_HANDLES.
+	 */
+	uint32_t bo_index[2];
+
+	/* This is the BO where we store the validated command lists, shader
+	 * records, and uniforms.
+	 */
+	struct drm_gem_cma_object *exec_bo;
+
+	/**
+	 * This tracks the per-shader-record state (packet 64) that
+	 * determines the length of the shader record and the offset
+	 * it's expected to be found at.  It gets read in from the
+	 * command lists.
+	 */
+	struct vc4_shader_state {
+		uint32_t addr;
+		/* Maximum vertex index referenced by any primitive using this
+		 * shader state.
+		 */
+		uint32_t max_index;
+	} *shader_state;
+
+	/** How many shader states the user declared they were using. */
+	uint32_t shader_state_size;
+	/** How many shader state records the validator has seen. */
+	uint32_t shader_state_count;
+
+	bool found_tile_binning_mode_config_packet;
+	bool found_start_tile_binning_packet;
+	bool found_increment_semaphore_packet;
+	bool found_flush;
+	uint8_t bin_tiles_x, bin_tiles_y;
+	struct drm_gem_cma_object *tile_bo;
+	uint32_t tile_alloc_offset;
+
+	/**
+	 * Computed addresses pointing into exec_bo where we start the
+	 * bin thread (ct0) and render thread (ct1).
+	 */
+	uint32_t ct0ca, ct0ea;
+	uint32_t ct1ca, ct1ea;
+
+	/* Pointer to the unvalidated bin CL (if present). */
+	void *bin_u;
+
+	/* Pointers to the shader recs.  These paddr gets incremented as CL
+	 * packets are relocated in validate_gl_shader_state, and the vaddrs
+	 * (u and v) get incremented and size decremented as the shader recs
+	 * themselves are validated.
+	 */
+	void *shader_rec_u;
+	void *shader_rec_v;
+	uint32_t shader_rec_p;
+	uint32_t shader_rec_size;
+
+	/* Pointers to the uniform data.  These pointers are incremented, and
+	 * size decremented, as each batch of uniforms is uploaded.
+	 */
+	void *uniforms_u;
+	void *uniforms_v;
+	uint32_t uniforms_p;
+	uint32_t uniforms_size;
+};
+
+static inline struct vc4_exec_info *
+vc4_first_job(struct vc4_dev *vc4)
+{
+	if (list_empty(&vc4->job_list))
+		return NULL;
+	return list_first_entry(&vc4->job_list, struct vc4_exec_info, head);
+}
+
+/**
+ * struct vc4_texture_sample_info - saves the offsets into the UBO for texture
+ * setup parameters.
+ *
+ * This will be used at draw time to relocate the reference to the texture
+ * contents in p0, and validate that the offset combined with
+ * width/height/stride/etc. from p1 and p2/p3 doesn't sample outside the BO.
+ * Note that the hardware treats unprovided config parameters as 0, so not all
+ * of them need to be set up for every texure sample, and we'll store ~0 as
+ * the offset to mark the unused ones.
+ *
+ * See the VC4 3D architecture guide page 41 ("Texture and Memory Lookup Unit
+ * Setup") for definitions of the texture parameters.
+ */
+struct vc4_texture_sample_info {
+	bool is_direct;
+	uint32_t p_offset[4];
+};
+
+/**
+ * struct vc4_validated_shader_info - information about validated shaders that
+ * needs to be used from command list validation.
+ *
+ * For a given shader, each time a shader state record references it, we need
+ * to verify that the shader doesn't read more uniforms than the shader state
+ * record's uniform BO pointer can provide, and we need to apply relocations
+ * and validate the shader state record's uniforms that define the texture
+ * samples.
+ */
+struct vc4_validated_shader_info {
+	uint32_t uniforms_size;
+	uint32_t uniforms_src_size;
+	uint32_t num_texture_samples;
+	struct vc4_texture_sample_info *texture_samples;
+};
+
 /**
  * _wait_for - magic (register) wait macro
  *
@@ -104,13 +358,29 @@ to_vc4_encoder(struct drm_encoder *encoder)
 #define wait_for(COND, MS) _wait_for(COND, MS, 1)
 
 /* vc4_bo.c */
+struct drm_gem_object *vc4_create_object(struct drm_device *dev, size_t size);
 void vc4_free_object(struct drm_gem_object *gem_obj);
-struct vc4_bo *vc4_bo_create(struct drm_device *dev, size_t size);
+struct vc4_bo *vc4_bo_create(struct drm_device *dev, size_t size,
+			     bool from_cache);
 int vc4_dumb_create(struct drm_file *file_priv,
 		    struct drm_device *dev,
 		    struct drm_mode_create_dumb *args);
 struct dma_buf *vc4_prime_export(struct drm_device *dev,
 				 struct drm_gem_object *obj, int flags);
+int vc4_create_bo_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int vc4_create_shader_bo_ioctl(struct drm_device *dev, void *data,
+			       struct drm_file *file_priv);
+int vc4_mmap_bo_ioctl(struct drm_device *dev, void *data,
+		      struct drm_file *file_priv);
+int vc4_get_hang_state_ioctl(struct drm_device *dev, void *data,
+			     struct drm_file *file_priv);
+int vc4_mmap(struct file *filp, struct vm_area_struct *vma);
+int vc4_prime_mmap(struct drm_gem_object *obj, struct vm_area_struct *vma);
+void *vc4_prime_vmap(struct drm_gem_object *obj);
+void vc4_bo_cache_init(struct drm_device *dev);
+void vc4_bo_cache_destroy(struct drm_device *dev);
+int vc4_bo_stats_debugfs(struct seq_file *m, void *arg);
 
 /* vc4_crtc.c */
 extern struct platform_driver vc4_crtc_driver;
@@ -126,10 +396,34 @@ void vc4_debugfs_cleanup(struct drm_minor *minor);
 /* vc4_drv.c */
 void __iomem *vc4_ioremap_regs(struct platform_device *dev, int index);
 
+/* vc4_gem.c */
+void vc4_gem_init(struct drm_device *dev);
+void vc4_gem_destroy(struct drm_device *dev);
+int vc4_submit_cl_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int vc4_wait_seqno_ioctl(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv);
+int vc4_wait_bo_ioctl(struct drm_device *dev, void *data,
+		      struct drm_file *file_priv);
+void vc4_submit_next_job(struct drm_device *dev);
+int vc4_wait_for_seqno(struct drm_device *dev, uint64_t seqno,
+		       uint64_t timeout_ns, bool interruptible);
+void vc4_job_handle_completed(struct vc4_dev *vc4);
+int vc4_queue_seqno_cb(struct drm_device *dev,
+		       struct vc4_seqno_cb *cb, uint64_t seqno,
+		       void (*func)(struct vc4_seqno_cb *cb));
+
 /* vc4_hdmi.c */
 extern struct platform_driver vc4_hdmi_driver;
 int vc4_hdmi_debugfs_regs(struct seq_file *m, void *unused);
 
+/* vc4_irq.c */
+irqreturn_t vc4_irq(int irq, void *arg);
+void vc4_irq_preinstall(struct drm_device *dev);
+int vc4_irq_postinstall(struct drm_device *dev);
+void vc4_irq_uninstall(struct drm_device *dev);
+void vc4_irq_reset(struct drm_device *dev);
+
 /* vc4_hvs.c */
 extern struct platform_driver vc4_hvs_driver;
 void vc4_hvs_dump_state(struct drm_device *dev);
@@ -143,3 +437,34 @@ struct drm_plane *vc4_plane_init(struct drm_device *dev,
 				 enum drm_plane_type type);
 u32 vc4_plane_write_dlist(struct drm_plane *plane, u32 __iomem *dlist);
 u32 vc4_plane_dlist_size(struct drm_plane_state *state);
+void vc4_plane_async_set_fb(struct drm_plane *plane,
+			    struct drm_framebuffer *fb);
+
+/* vc4_v3d.c */
+extern struct platform_driver vc4_v3d_driver;
+int vc4_v3d_debugfs_ident(struct seq_file *m, void *unused);
+int vc4_v3d_debugfs_regs(struct seq_file *m, void *unused);
+
+/* vc4_validate.c */
+int
+vc4_validate_bin_cl(struct drm_device *dev,
+		    void *validated,
+		    void *unvalidated,
+		    struct vc4_exec_info *exec);
+
+int
+vc4_validate_shader_recs(struct drm_device *dev, struct vc4_exec_info *exec);
+
+struct drm_gem_cma_object *vc4_use_bo(struct vc4_exec_info *exec,
+				      uint32_t hindex);
+
+int vc4_get_rcl(struct drm_device *dev, struct vc4_exec_info *exec);
+
+bool vc4_check_tex_size(struct vc4_exec_info *exec,
+			struct drm_gem_cma_object *fbo,
+			uint32_t offset, uint8_t tiling_format,
+			uint32_t width, uint32_t height, uint8_t cpp);
+
+/* vc4_validate_shader.c */
+struct vc4_validated_shader_info *
+vc4_validate_shader(struct drm_gem_cma_object *shader_obj);
diff --git a/drivers/gpu/drm/vc4/vc4_gem.c b/drivers/gpu/drm/vc4/vc4_gem.c
new file mode 100644
index 000000000000..202aa1544acc
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_gem.c
@@ -0,0 +1,899 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * 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/module.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/device.h>
+#include <linux/io.h>
+
+#include "uapi/drm/vc4_drm.h"
+#include "vc4_drv.h"
+#include "vc4_regs.h"
+#include "vc4_trace.h"
+
+static void
+vc4_queue_hangcheck(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	mod_timer(&vc4->hangcheck.timer,
+		  round_jiffies_up(jiffies + msecs_to_jiffies(100)));
+}
+
+struct vc4_hang_state {
+	struct drm_vc4_get_hang_state user_state;
+
+	u32 bo_count;
+	struct drm_gem_object **bo;
+};
+
+static void
+vc4_free_hang_state(struct drm_device *dev, struct vc4_hang_state *state)
+{
+	unsigned int i;
+
+	mutex_lock(&dev->struct_mutex);
+	for (i = 0; i < state->user_state.bo_count; i++)
+		drm_gem_object_unreference(state->bo[i]);
+	mutex_unlock(&dev->struct_mutex);
+
+	kfree(state);
+}
+
+int
+vc4_get_hang_state_ioctl(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv)
+{
+	struct drm_vc4_get_hang_state *get_state = data;
+	struct drm_vc4_get_hang_state_bo *bo_state;
+	struct vc4_hang_state *kernel_state;
+	struct drm_vc4_get_hang_state *state;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	unsigned long irqflags;
+	u32 i;
+	int ret = 0;
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	kernel_state = vc4->hang_state;
+	if (!kernel_state) {
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		return -ENOENT;
+	}
+	state = &kernel_state->user_state;
+
+	/* If the user's array isn't big enough, just return the
+	 * required array size.
+	 */
+	if (get_state->bo_count < state->bo_count) {
+		get_state->bo_count = state->bo_count;
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		return 0;
+	}
+
+	vc4->hang_state = NULL;
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+
+	/* Save the user's BO pointer, so we don't stomp it with the memcpy. */
+	state->bo = get_state->bo;
+	memcpy(get_state, state, sizeof(*state));
+
+	bo_state = kcalloc(state->bo_count, sizeof(*bo_state), GFP_KERNEL);
+	if (!bo_state) {
+		ret = -ENOMEM;
+		goto err_free;
+	}
+
+	for (i = 0; i < state->bo_count; i++) {
+		struct vc4_bo *vc4_bo = to_vc4_bo(kernel_state->bo[i]);
+		u32 handle;
+
+		ret = drm_gem_handle_create(file_priv, kernel_state->bo[i],
+					    &handle);
+
+		if (ret) {
+			state->bo_count = i - 1;
+			goto err;
+		}
+		bo_state[i].handle = handle;
+		bo_state[i].paddr = vc4_bo->base.paddr;
+		bo_state[i].size = vc4_bo->base.base.size;
+	}
+
+	if (copy_to_user((void __user *)(uintptr_t)get_state->bo,
+			 bo_state,
+			 state->bo_count * sizeof(*bo_state)))
+		ret = -EFAULT;
+
+	kfree(bo_state);
+
+err_free:
+
+	vc4_free_hang_state(dev, kernel_state);
+
+err:
+	return ret;
+}
+
+static void
+vc4_save_hang_state(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct drm_vc4_get_hang_state *state;
+	struct vc4_hang_state *kernel_state;
+	struct vc4_exec_info *exec;
+	struct vc4_bo *bo;
+	unsigned long irqflags;
+	unsigned int i, unref_list_count;
+
+	kernel_state = kcalloc(1, sizeof(*kernel_state), GFP_KERNEL);
+	if (!kernel_state)
+		return;
+
+	state = &kernel_state->user_state;
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	exec = vc4_first_job(vc4);
+	if (!exec) {
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		return;
+	}
+
+	unref_list_count = 0;
+	list_for_each_entry(bo, &exec->unref_list, unref_head)
+		unref_list_count++;
+
+	state->bo_count = exec->bo_count + unref_list_count;
+	kernel_state->bo = kcalloc(state->bo_count, sizeof(*kernel_state->bo),
+				   GFP_ATOMIC);
+	if (!kernel_state->bo) {
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		return;
+	}
+
+	for (i = 0; i < exec->bo_count; i++) {
+		drm_gem_object_reference(&exec->bo[i]->base);
+		kernel_state->bo[i] = &exec->bo[i]->base;
+	}
+
+	list_for_each_entry(bo, &exec->unref_list, unref_head) {
+		drm_gem_object_reference(&bo->base.base);
+		kernel_state->bo[i] = &bo->base.base;
+		i++;
+	}
+
+	state->start_bin = exec->ct0ca;
+	state->start_render = exec->ct1ca;
+
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+
+	state->ct0ca = V3D_READ(V3D_CTNCA(0));
+	state->ct0ea = V3D_READ(V3D_CTNEA(0));
+
+	state->ct1ca = V3D_READ(V3D_CTNCA(1));
+	state->ct1ea = V3D_READ(V3D_CTNEA(1));
+
+	state->ct0cs = V3D_READ(V3D_CTNCS(0));
+	state->ct1cs = V3D_READ(V3D_CTNCS(1));
+
+	state->ct0ra0 = V3D_READ(V3D_CT00RA0);
+	state->ct1ra0 = V3D_READ(V3D_CT01RA0);
+
+	state->bpca = V3D_READ(V3D_BPCA);
+	state->bpcs = V3D_READ(V3D_BPCS);
+	state->bpoa = V3D_READ(V3D_BPOA);
+	state->bpos = V3D_READ(V3D_BPOS);
+
+	state->vpmbase = V3D_READ(V3D_VPMBASE);
+
+	state->dbge = V3D_READ(V3D_DBGE);
+	state->fdbgo = V3D_READ(V3D_FDBGO);
+	state->fdbgb = V3D_READ(V3D_FDBGB);
+	state->fdbgr = V3D_READ(V3D_FDBGR);
+	state->fdbgs = V3D_READ(V3D_FDBGS);
+	state->errstat = V3D_READ(V3D_ERRSTAT);
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	if (vc4->hang_state) {
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		vc4_free_hang_state(dev, kernel_state);
+	} else {
+		vc4->hang_state = kernel_state;
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+	}
+}
+
+static void
+vc4_reset(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	DRM_INFO("Resetting GPU.\n");
+
+	mutex_lock(&vc4->power_lock);
+	if (vc4->power_refcount) {
+		/* Power the device off and back on the by dropping the
+		 * reference on runtime PM.
+		 */
+		pm_runtime_put_sync_suspend(&vc4->v3d->pdev->dev);
+		pm_runtime_get_sync(&vc4->v3d->pdev->dev);
+	}
+	mutex_unlock(&vc4->power_lock);
+
+	vc4_irq_reset(dev);
+
+	/* Rearm the hangcheck -- another job might have been waiting
+	 * for our hung one to get kicked off, and vc4_irq_reset()
+	 * would have started it.
+	 */
+	vc4_queue_hangcheck(dev);
+}
+
+static void
+vc4_reset_work(struct work_struct *work)
+{
+	struct vc4_dev *vc4 =
+		container_of(work, struct vc4_dev, hangcheck.reset_work);
+
+	vc4_save_hang_state(vc4->dev);
+
+	vc4_reset(vc4->dev);
+}
+
+static void
+vc4_hangcheck_elapsed(unsigned long data)
+{
+	struct drm_device *dev = (struct drm_device *)data;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint32_t ct0ca, ct1ca;
+	unsigned long irqflags;
+	struct vc4_exec_info *exec;
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	exec = vc4_first_job(vc4);
+
+	/* If idle, we can stop watching for hangs. */
+	if (!exec) {
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		return;
+	}
+
+	ct0ca = V3D_READ(V3D_CTNCA(0));
+	ct1ca = V3D_READ(V3D_CTNCA(1));
+
+	/* If we've made any progress in execution, rearm the timer
+	 * and wait.
+	 */
+	if (ct0ca != exec->last_ct0ca || ct1ca != exec->last_ct1ca) {
+		exec->last_ct0ca = ct0ca;
+		exec->last_ct1ca = ct1ca;
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		vc4_queue_hangcheck(dev);
+		return;
+	}
+
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+
+	/* We've gone too long with no progress, reset.  This has to
+	 * be done from a work struct, since resetting can sleep and
+	 * this timer hook isn't allowed to.
+	 */
+	schedule_work(&vc4->hangcheck.reset_work);
+}
+
+static void
+submit_cl(struct drm_device *dev, uint32_t thread, uint32_t start, uint32_t end)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Set the current and end address of the control list.
+	 * Writing the end register is what starts the job.
+	 */
+	V3D_WRITE(V3D_CTNCA(thread), start);
+	V3D_WRITE(V3D_CTNEA(thread), end);
+}
+
+int
+vc4_wait_for_seqno(struct drm_device *dev, uint64_t seqno, uint64_t timeout_ns,
+		   bool interruptible)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	int ret = 0;
+	unsigned long timeout_expire;
+	DEFINE_WAIT(wait);
+
+	if (vc4->finished_seqno >= seqno)
+		return 0;
+
+	if (timeout_ns == 0)
+		return -ETIME;
+
+	timeout_expire = jiffies + nsecs_to_jiffies(timeout_ns);
+
+	trace_vc4_wait_for_seqno_begin(dev, seqno, timeout_ns);
+	for (;;) {
+		prepare_to_wait(&vc4->job_wait_queue, &wait,
+				interruptible ? TASK_INTERRUPTIBLE :
+				TASK_UNINTERRUPTIBLE);
+
+		if (interruptible && signal_pending(current)) {
+			ret = -ERESTARTSYS;
+			break;
+		}
+
+		if (vc4->finished_seqno >= seqno)
+			break;
+
+		if (timeout_ns != ~0ull) {
+			if (time_after_eq(jiffies, timeout_expire)) {
+				ret = -ETIME;
+				break;
+			}
+			schedule_timeout(timeout_expire - jiffies);
+		} else {
+			schedule();
+		}
+	}
+
+	finish_wait(&vc4->job_wait_queue, &wait);
+	trace_vc4_wait_for_seqno_end(dev, seqno);
+
+	return ret;
+}
+
+static void
+vc4_flush_caches(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Flush the GPU L2 caches.  These caches sit on top of system
+	 * L3 (the 128kb or so shared with the CPU), and are
+	 * non-allocating in the L3.
+	 */
+	V3D_WRITE(V3D_L2CACTL,
+		  V3D_L2CACTL_L2CCLR);
+
+	V3D_WRITE(V3D_SLCACTL,
+		  VC4_SET_FIELD(0xf, V3D_SLCACTL_T1CC) |
+		  VC4_SET_FIELD(0xf, V3D_SLCACTL_T0CC) |
+		  VC4_SET_FIELD(0xf, V3D_SLCACTL_UCC) |
+		  VC4_SET_FIELD(0xf, V3D_SLCACTL_ICC));
+}
+
+/* Sets the registers for the next job to be actually be executed in
+ * the hardware.
+ *
+ * The job_lock should be held during this.
+ */
+void
+vc4_submit_next_job(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct vc4_exec_info *exec = vc4_first_job(vc4);
+
+	if (!exec)
+		return;
+
+	vc4_flush_caches(dev);
+
+	/* Disable the binner's pre-loaded overflow memory address */
+	V3D_WRITE(V3D_BPOA, 0);
+	V3D_WRITE(V3D_BPOS, 0);
+
+	if (exec->ct0ca != exec->ct0ea)
+		submit_cl(dev, 0, exec->ct0ca, exec->ct0ea);
+	submit_cl(dev, 1, exec->ct1ca, exec->ct1ea);
+}
+
+static void
+vc4_update_bo_seqnos(struct vc4_exec_info *exec, uint64_t seqno)
+{
+	struct vc4_bo *bo;
+	unsigned i;
+
+	for (i = 0; i < exec->bo_count; i++) {
+		bo = to_vc4_bo(&exec->bo[i]->base);
+		bo->seqno = seqno;
+	}
+
+	list_for_each_entry(bo, &exec->unref_list, unref_head) {
+		bo->seqno = seqno;
+	}
+}
+
+/* Queues a struct vc4_exec_info for execution.  If no job is
+ * currently executing, then submits it.
+ *
+ * Unlike most GPUs, our hardware only handles one command list at a
+ * time.  To queue multiple jobs at once, we'd need to edit the
+ * previous command list to have a jump to the new one at the end, and
+ * then bump the end address.  That's a change for a later date,
+ * though.
+ */
+static void
+vc4_queue_submit(struct drm_device *dev, struct vc4_exec_info *exec)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint64_t seqno;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+
+	seqno = ++vc4->emit_seqno;
+	exec->seqno = seqno;
+	vc4_update_bo_seqnos(exec, seqno);
+
+	list_add_tail(&exec->head, &vc4->job_list);
+
+	/* If no job was executing, kick ours off.  Otherwise, it'll
+	 * get started when the previous job's frame done interrupt
+	 * occurs.
+	 */
+	if (vc4_first_job(vc4) == exec) {
+		vc4_submit_next_job(dev);
+		vc4_queue_hangcheck(dev);
+	}
+
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+}
+
+/**
+ * Looks up a bunch of GEM handles for BOs and stores the array for
+ * use in the command validator that actually writes relocated
+ * addresses pointing to them.
+ */
+static int
+vc4_cl_lookup_bos(struct drm_device *dev,
+		  struct drm_file *file_priv,
+		  struct vc4_exec_info *exec)
+{
+	struct drm_vc4_submit_cl *args = exec->args;
+	uint32_t *handles;
+	int ret = 0;
+	int i;
+
+	exec->bo_count = args->bo_handle_count;
+
+	if (!exec->bo_count) {
+		/* See comment on bo_index for why we have to check
+		 * this.
+		 */
+		DRM_ERROR("Rendering requires BOs to validate\n");
+		return -EINVAL;
+	}
+
+	exec->bo = kcalloc(exec->bo_count, sizeof(struct drm_gem_cma_object *),
+			   GFP_KERNEL);
+	if (!exec->bo) {
+		DRM_ERROR("Failed to allocate validated BO pointers\n");
+		return -ENOMEM;
+	}
+
+	handles = drm_malloc_ab(exec->bo_count, sizeof(uint32_t));
+	if (!handles) {
+		DRM_ERROR("Failed to allocate incoming GEM handles\n");
+		goto fail;
+	}
+
+	ret = copy_from_user(handles,
+			     (void __user *)(uintptr_t)args->bo_handles,
+			     exec->bo_count * sizeof(uint32_t));
+	if (ret) {
+		DRM_ERROR("Failed to copy in GEM handles\n");
+		goto fail;
+	}
+
+	spin_lock(&file_priv->table_lock);
+	for (i = 0; i < exec->bo_count; i++) {
+		struct drm_gem_object *bo = idr_find(&file_priv->object_idr,
+						     handles[i]);
+		if (!bo) {
+			DRM_ERROR("Failed to look up GEM BO %d: %d\n",
+				  i, handles[i]);
+			ret = -EINVAL;
+			spin_unlock(&file_priv->table_lock);
+			goto fail;
+		}
+		drm_gem_object_reference(bo);
+		exec->bo[i] = (struct drm_gem_cma_object *)bo;
+	}
+	spin_unlock(&file_priv->table_lock);
+
+fail:
+	kfree(handles);
+	return 0;
+}
+
+static int
+vc4_get_bcl(struct drm_device *dev, struct vc4_exec_info *exec)
+{
+	struct drm_vc4_submit_cl *args = exec->args;
+	void *temp = NULL;
+	void *bin;
+	int ret = 0;
+	uint32_t bin_offset = 0;
+	uint32_t shader_rec_offset = roundup(bin_offset + args->bin_cl_size,
+					     16);
+	uint32_t uniforms_offset = shader_rec_offset + args->shader_rec_size;
+	uint32_t exec_size = uniforms_offset + args->uniforms_size;
+	uint32_t temp_size = exec_size + (sizeof(struct vc4_shader_state) *
+					  args->shader_rec_count);
+	struct vc4_bo *bo;
+
+	if (uniforms_offset < shader_rec_offset ||
+	    exec_size < uniforms_offset ||
+	    args->shader_rec_count >= (UINT_MAX /
+					  sizeof(struct vc4_shader_state)) ||
+	    temp_size < exec_size) {
+		DRM_ERROR("overflow in exec arguments\n");
+		goto fail;
+	}
+
+	/* Allocate space where we'll store the copied in user command lists
+	 * and shader records.
+	 *
+	 * We don't just copy directly into the BOs because we need to
+	 * read the contents back for validation, and I think the
+	 * bo->vaddr is uncached access.
+	 */
+	temp = kmalloc(temp_size, GFP_KERNEL);
+	if (!temp) {
+		DRM_ERROR("Failed to allocate storage for copying "
+			  "in bin/render CLs.\n");
+		ret = -ENOMEM;
+		goto fail;
+	}
+	bin = temp + bin_offset;
+	exec->shader_rec_u = temp + shader_rec_offset;
+	exec->uniforms_u = temp + uniforms_offset;
+	exec->shader_state = temp + exec_size;
+	exec->shader_state_size = args->shader_rec_count;
+
+	if (copy_from_user(bin,
+			   (void __user *)(uintptr_t)args->bin_cl,
+			   args->bin_cl_size)) {
+		ret = -EFAULT;
+		goto fail;
+	}
+
+	if (copy_from_user(exec->shader_rec_u,
+			   (void __user *)(uintptr_t)args->shader_rec,
+			   args->shader_rec_size)) {
+		ret = -EFAULT;
+		goto fail;
+	}
+
+	if (copy_from_user(exec->uniforms_u,
+			   (void __user *)(uintptr_t)args->uniforms,
+			   args->uniforms_size)) {
+		ret = -EFAULT;
+		goto fail;
+	}
+
+	bo = vc4_bo_create(dev, exec_size, true);
+	if (IS_ERR(bo)) {
+		DRM_ERROR("Couldn't allocate BO for binning\n");
+		ret = PTR_ERR(bo);
+		goto fail;
+	}
+	exec->exec_bo = &bo->base;
+
+	list_add_tail(&to_vc4_bo(&exec->exec_bo->base)->unref_head,
+		      &exec->unref_list);
+
+	exec->ct0ca = exec->exec_bo->paddr + bin_offset;
+
+	exec->bin_u = bin;
+
+	exec->shader_rec_v = exec->exec_bo->vaddr + shader_rec_offset;
+	exec->shader_rec_p = exec->exec_bo->paddr + shader_rec_offset;
+	exec->shader_rec_size = args->shader_rec_size;
+
+	exec->uniforms_v = exec->exec_bo->vaddr + uniforms_offset;
+	exec->uniforms_p = exec->exec_bo->paddr + uniforms_offset;
+	exec->uniforms_size = args->uniforms_size;
+
+	ret = vc4_validate_bin_cl(dev,
+				  exec->exec_bo->vaddr + bin_offset,
+				  bin,
+				  exec);
+	if (ret)
+		goto fail;
+
+	ret = vc4_validate_shader_recs(dev, exec);
+
+fail:
+	kfree(temp);
+	return ret;
+}
+
+static void
+vc4_complete_exec(struct drm_device *dev, struct vc4_exec_info *exec)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	unsigned i;
+
+	/* Need the struct lock for drm_gem_object_unreference(). */
+	mutex_lock(&dev->struct_mutex);
+	if (exec->bo) {
+		for (i = 0; i < exec->bo_count; i++)
+			drm_gem_object_unreference(&exec->bo[i]->base);
+		kfree(exec->bo);
+	}
+
+	while (!list_empty(&exec->unref_list)) {
+		struct vc4_bo *bo = list_first_entry(&exec->unref_list,
+						     struct vc4_bo, unref_head);
+		list_del(&bo->unref_head);
+		drm_gem_object_unreference(&bo->base.base);
+	}
+	mutex_unlock(&dev->struct_mutex);
+
+	mutex_lock(&vc4->power_lock);
+	if (--vc4->power_refcount == 0)
+		pm_runtime_put(&vc4->v3d->pdev->dev);
+	mutex_unlock(&vc4->power_lock);
+
+	kfree(exec);
+}
+
+void
+vc4_job_handle_completed(struct vc4_dev *vc4)
+{
+	unsigned long irqflags;
+	struct vc4_seqno_cb *cb, *cb_temp;
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	while (!list_empty(&vc4->job_done_list)) {
+		struct vc4_exec_info *exec =
+			list_first_entry(&vc4->job_done_list,
+					 struct vc4_exec_info, head);
+		list_del(&exec->head);
+
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+		vc4_complete_exec(vc4->dev, exec);
+		spin_lock_irqsave(&vc4->job_lock, irqflags);
+	}
+
+	list_for_each_entry_safe(cb, cb_temp, &vc4->seqno_cb_list, work.entry) {
+		if (cb->seqno <= vc4->finished_seqno) {
+			list_del_init(&cb->work.entry);
+			schedule_work(&cb->work);
+		}
+	}
+
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+}
+
+static void vc4_seqno_cb_work(struct work_struct *work)
+{
+	struct vc4_seqno_cb *cb = container_of(work, struct vc4_seqno_cb, work);
+
+	cb->func(cb);
+}
+
+int vc4_queue_seqno_cb(struct drm_device *dev,
+		       struct vc4_seqno_cb *cb, uint64_t seqno,
+		       void (*func)(struct vc4_seqno_cb *cb))
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	int ret = 0;
+	unsigned long irqflags;
+
+	cb->func = func;
+	INIT_WORK(&cb->work, vc4_seqno_cb_work);
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	if (seqno > vc4->finished_seqno) {
+		cb->seqno = seqno;
+		list_add_tail(&cb->work.entry, &vc4->seqno_cb_list);
+	} else {
+		schedule_work(&cb->work);
+	}
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+
+	return ret;
+}
+
+/* Scheduled when any job has been completed, this walks the list of
+ * jobs that had completed and unrefs their BOs and frees their exec
+ * structs.
+ */
+static void
+vc4_job_done_work(struct work_struct *work)
+{
+	struct vc4_dev *vc4 =
+		container_of(work, struct vc4_dev, job_done_work);
+
+	vc4_job_handle_completed(vc4);
+}
+
+static int
+vc4_wait_for_seqno_ioctl_helper(struct drm_device *dev,
+				uint64_t seqno,
+				uint64_t *timeout_ns)
+{
+	unsigned long start = jiffies;
+	int ret = vc4_wait_for_seqno(dev, seqno, *timeout_ns, true);
+
+	if ((ret == -EINTR || ret == -ERESTARTSYS) && *timeout_ns != ~0ull) {
+		uint64_t delta = jiffies_to_nsecs(jiffies - start);
+
+		if (*timeout_ns >= delta)
+			*timeout_ns -= delta;
+	}
+
+	return ret;
+}
+
+int
+vc4_wait_seqno_ioctl(struct drm_device *dev, void *data,
+		     struct drm_file *file_priv)
+{
+	struct drm_vc4_wait_seqno *args = data;
+
+	return vc4_wait_for_seqno_ioctl_helper(dev, args->seqno,
+					       &args->timeout_ns);
+}
+
+int
+vc4_wait_bo_ioctl(struct drm_device *dev, void *data,
+		  struct drm_file *file_priv)
+{
+	int ret;
+	struct drm_vc4_wait_bo *args = data;
+	struct drm_gem_object *gem_obj;
+	struct vc4_bo *bo;
+
+	if (args->pad != 0)
+		return -EINVAL;
+
+	gem_obj = drm_gem_object_lookup(dev, file_priv, args->handle);
+	if (!gem_obj) {
+		DRM_ERROR("Failed to look up GEM BO %d\n", args->handle);
+		return -EINVAL;
+	}
+	bo = to_vc4_bo(gem_obj);
+
+	ret = vc4_wait_for_seqno_ioctl_helper(dev, bo->seqno,
+					      &args->timeout_ns);
+
+	drm_gem_object_unreference_unlocked(gem_obj);
+	return ret;
+}
+
+/**
+ * Submits a command list to the VC4.
+ *
+ * This is what is called batchbuffer emitting on other hardware.
+ */
+int
+vc4_submit_cl_ioctl(struct drm_device *dev, void *data,
+		    struct drm_file *file_priv)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct drm_vc4_submit_cl *args = data;
+	struct vc4_exec_info *exec;
+	int ret = 0;
+
+	if ((args->flags & ~VC4_SUBMIT_CL_USE_CLEAR_COLOR) != 0) {
+		DRM_ERROR("Unknown flags: 0x%02x\n", args->flags);
+		return -EINVAL;
+	}
+
+	exec = kcalloc(1, sizeof(*exec), GFP_KERNEL);
+	if (!exec) {
+		DRM_ERROR("malloc failure on exec struct\n");
+		return -ENOMEM;
+	}
+
+	mutex_lock(&vc4->power_lock);
+	if (vc4->power_refcount++ == 0)
+		ret = pm_runtime_get_sync(&vc4->v3d->pdev->dev);
+	mutex_unlock(&vc4->power_lock);
+	if (ret < 0) {
+		kfree(exec);
+		return ret;
+	}
+
+	exec->args = args;
+	INIT_LIST_HEAD(&exec->unref_list);
+
+	ret = vc4_cl_lookup_bos(dev, file_priv, exec);
+	if (ret)
+		goto fail;
+
+	if (exec->args->bin_cl_size != 0) {
+		ret = vc4_get_bcl(dev, exec);
+		if (ret)
+			goto fail;
+	} else {
+		exec->ct0ca = 0;
+		exec->ct0ea = 0;
+	}
+
+	ret = vc4_get_rcl(dev, exec);
+	if (ret)
+		goto fail;
+
+	/* Clear this out of the struct we'll be putting in the queue,
+	 * since it's part of our stack.
+	 */
+	exec->args = NULL;
+
+	vc4_queue_submit(dev, exec);
+
+	/* Return the seqno for our job. */
+	args->seqno = vc4->emit_seqno;
+
+	return 0;
+
+fail:
+	vc4_complete_exec(vc4->dev, exec);
+
+	return ret;
+}
+
+void
+vc4_gem_init(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	INIT_LIST_HEAD(&vc4->job_list);
+	INIT_LIST_HEAD(&vc4->job_done_list);
+	INIT_LIST_HEAD(&vc4->seqno_cb_list);
+	spin_lock_init(&vc4->job_lock);
+
+	INIT_WORK(&vc4->hangcheck.reset_work, vc4_reset_work);
+	setup_timer(&vc4->hangcheck.timer,
+		    vc4_hangcheck_elapsed,
+		    (unsigned long)dev);
+
+	INIT_WORK(&vc4->job_done_work, vc4_job_done_work);
+
+	mutex_init(&vc4->power_lock);
+}
+
+void
+vc4_gem_destroy(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Waiting for exec to finish would need to be done before
+	 * unregistering V3D.
+	 */
+	WARN_ON(vc4->emit_seqno != vc4->finished_seqno);
+
+	/* V3D should already have disabled its interrupt and cleared
+	 * the overflow allocation registers.  Now free the object.
+	 */
+	if (vc4->overflow_mem) {
+		drm_gem_object_unreference_unlocked(&vc4->overflow_mem->base.base);
+		vc4->overflow_mem = NULL;
+	}
+
+	vc4_bo_cache_destroy(dev);
+
+	if (vc4->hang_state)
+		vc4_free_hang_state(dev, vc4->hang_state);
+}
diff --git a/drivers/gpu/drm/vc4/vc4_hdmi.c b/drivers/gpu/drm/vc4/vc4_hdmi.c
index da9a36d6e1d1..c69c0460196b 100644
--- a/drivers/gpu/drm/vc4/vc4_hdmi.c
+++ b/drivers/gpu/drm/vc4/vc4_hdmi.c
@@ -519,7 +519,7 @@ static int vc4_hdmi_bind(struct device *dev, struct device *master, void *data)
 	WARN_ON_ONCE((HD_READ(VC4_HD_M_CTL) & VC4_HD_M_ENABLE) == 0);
 
 	drm_encoder_init(drm, hdmi->encoder, &vc4_hdmi_encoder_funcs,
-			 DRM_MODE_ENCODER_TMDS);
+			 DRM_MODE_ENCODER_TMDS, NULL);
 	drm_encoder_helper_add(hdmi->encoder, &vc4_hdmi_encoder_helper_funcs);
 
 	hdmi->connector = vc4_hdmi_connector_init(drm, hdmi->encoder);
diff --git a/drivers/gpu/drm/vc4/vc4_irq.c b/drivers/gpu/drm/vc4/vc4_irq.c
new file mode 100644
index 000000000000..78a21357fb2d
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_irq.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+/** DOC: Interrupt management for the V3D engine.
+ *
+ * We have an interrupt status register (V3D_INTCTL) which reports
+ * interrupts, and where writing 1 bits clears those interrupts.
+ * There are also a pair of interrupt registers
+ * (V3D_INTENA/V3D_INTDIS) where writing a 1 to their bits enables or
+ * disables that specific interrupt, and 0s written are ignored
+ * (reading either one returns the set of enabled interrupts).
+ *
+ * When we take a render frame interrupt, we need to wake the
+ * processes waiting for some frame to be done, and get the next frame
+ * submitted ASAP (so the hardware doesn't sit idle when there's work
+ * to do).
+ *
+ * When we take the binner out of memory interrupt, we need to
+ * allocate some new memory and pass it to the binner so that the
+ * current job can make progress.
+ */
+
+#include "vc4_drv.h"
+#include "vc4_regs.h"
+
+#define V3D_DRIVER_IRQS (V3D_INT_OUTOMEM | \
+			 V3D_INT_FRDONE)
+
+DECLARE_WAIT_QUEUE_HEAD(render_wait);
+
+static void
+vc4_overflow_mem_work(struct work_struct *work)
+{
+	struct vc4_dev *vc4 =
+		container_of(work, struct vc4_dev, overflow_mem_work);
+	struct drm_device *dev = vc4->dev;
+	struct vc4_bo *bo;
+
+	bo = vc4_bo_create(dev, 256 * 1024, true);
+	if (IS_ERR(bo)) {
+		DRM_ERROR("Couldn't allocate binner overflow mem\n");
+		return;
+	}
+
+	/* If there's a job executing currently, then our previous
+	 * overflow allocation is getting used in that job and we need
+	 * to queue it to be released when the job is done.  But if no
+	 * job is executing at all, then we can free the old overflow
+	 * object direcctly.
+	 *
+	 * No lock necessary for this pointer since we're the only
+	 * ones that update the pointer, and our workqueue won't
+	 * reenter.
+	 */
+	if (vc4->overflow_mem) {
+		struct vc4_exec_info *current_exec;
+		unsigned long irqflags;
+
+		spin_lock_irqsave(&vc4->job_lock, irqflags);
+		current_exec = vc4_first_job(vc4);
+		if (current_exec) {
+			vc4->overflow_mem->seqno = vc4->finished_seqno + 1;
+			list_add_tail(&vc4->overflow_mem->unref_head,
+				      &current_exec->unref_list);
+			vc4->overflow_mem = NULL;
+		}
+		spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+	}
+
+	if (vc4->overflow_mem)
+		drm_gem_object_unreference_unlocked(&vc4->overflow_mem->base.base);
+	vc4->overflow_mem = bo;
+
+	V3D_WRITE(V3D_BPOA, bo->base.paddr);
+	V3D_WRITE(V3D_BPOS, bo->base.base.size);
+	V3D_WRITE(V3D_INTCTL, V3D_INT_OUTOMEM);
+	V3D_WRITE(V3D_INTENA, V3D_INT_OUTOMEM);
+}
+
+static void
+vc4_irq_finish_job(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	struct vc4_exec_info *exec = vc4_first_job(vc4);
+
+	if (!exec)
+		return;
+
+	vc4->finished_seqno++;
+	list_move_tail(&exec->head, &vc4->job_done_list);
+	vc4_submit_next_job(dev);
+
+	wake_up_all(&vc4->job_wait_queue);
+	schedule_work(&vc4->job_done_work);
+}
+
+irqreturn_t
+vc4_irq(int irq, void *arg)
+{
+	struct drm_device *dev = arg;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint32_t intctl;
+	irqreturn_t status = IRQ_NONE;
+
+	barrier();
+	intctl = V3D_READ(V3D_INTCTL);
+
+	/* Acknowledge the interrupts we're handling here. The render
+	 * frame done interrupt will be cleared, while OUTOMEM will
+	 * stay high until the underlying cause is cleared.
+	 */
+	V3D_WRITE(V3D_INTCTL, intctl);
+
+	if (intctl & V3D_INT_OUTOMEM) {
+		/* Disable OUTOMEM until the work is done. */
+		V3D_WRITE(V3D_INTDIS, V3D_INT_OUTOMEM);
+		schedule_work(&vc4->overflow_mem_work);
+		status = IRQ_HANDLED;
+	}
+
+	if (intctl & V3D_INT_FRDONE) {
+		spin_lock(&vc4->job_lock);
+		vc4_irq_finish_job(dev);
+		spin_unlock(&vc4->job_lock);
+		status = IRQ_HANDLED;
+	}
+
+	return status;
+}
+
+void
+vc4_irq_preinstall(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	init_waitqueue_head(&vc4->job_wait_queue);
+	INIT_WORK(&vc4->overflow_mem_work, vc4_overflow_mem_work);
+
+	/* Clear any pending interrupts someone might have left around
+	 * for us.
+	 */
+	V3D_WRITE(V3D_INTCTL, V3D_DRIVER_IRQS);
+}
+
+int
+vc4_irq_postinstall(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Enable both the render done and out of memory interrupts. */
+	V3D_WRITE(V3D_INTENA, V3D_DRIVER_IRQS);
+
+	return 0;
+}
+
+void
+vc4_irq_uninstall(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Disable sending interrupts for our driver's IRQs. */
+	V3D_WRITE(V3D_INTDIS, V3D_DRIVER_IRQS);
+
+	/* Clear any pending interrupts we might have left. */
+	V3D_WRITE(V3D_INTCTL, V3D_DRIVER_IRQS);
+
+	cancel_work_sync(&vc4->overflow_mem_work);
+}
+
+/** Reinitializes interrupt registers when a GPU reset is performed. */
+void vc4_irq_reset(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	unsigned long irqflags;
+
+	/* Acknowledge any stale IRQs. */
+	V3D_WRITE(V3D_INTCTL, V3D_DRIVER_IRQS);
+
+	/*
+	 * Turn all our interrupts on.  Binner out of memory is the
+	 * only one we expect to trigger at this point, since we've
+	 * just come from poweron and haven't supplied any overflow
+	 * memory yet.
+	 */
+	V3D_WRITE(V3D_INTENA, V3D_DRIVER_IRQS);
+
+	spin_lock_irqsave(&vc4->job_lock, irqflags);
+	vc4_irq_finish_job(dev);
+	spin_unlock_irqrestore(&vc4->job_lock, irqflags);
+}
diff --git a/drivers/gpu/drm/vc4/vc4_kms.c b/drivers/gpu/drm/vc4/vc4_kms.c
index 2e5597d10cc6..f95f2df5f8d1 100644
--- a/drivers/gpu/drm/vc4/vc4_kms.c
+++ b/drivers/gpu/drm/vc4/vc4_kms.c
@@ -15,6 +15,7 @@
  */
 
 #include "drm_crtc.h"
+#include "drm_atomic.h"
 #include "drm_atomic_helper.h"
 #include "drm_crtc_helper.h"
 #include "drm_plane_helper.h"
@@ -29,10 +30,152 @@ static void vc4_output_poll_changed(struct drm_device *dev)
 		drm_fbdev_cma_hotplug_event(vc4->fbdev);
 }
 
+struct vc4_commit {
+	struct drm_device *dev;
+	struct drm_atomic_state *state;
+	struct vc4_seqno_cb cb;
+};
+
+static void
+vc4_atomic_complete_commit(struct vc4_commit *c)
+{
+	struct drm_atomic_state *state = c->state;
+	struct drm_device *dev = state->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	drm_atomic_helper_commit_modeset_disables(dev, state);
+
+	drm_atomic_helper_commit_planes(dev, state, false);
+
+	drm_atomic_helper_commit_modeset_enables(dev, state);
+
+	drm_atomic_helper_wait_for_vblanks(dev, state);
+
+	drm_atomic_helper_cleanup_planes(dev, state);
+
+	drm_atomic_state_free(state);
+
+	up(&vc4->async_modeset);
+
+	kfree(c);
+}
+
+static void
+vc4_atomic_complete_commit_seqno_cb(struct vc4_seqno_cb *cb)
+{
+	struct vc4_commit *c = container_of(cb, struct vc4_commit, cb);
+
+	vc4_atomic_complete_commit(c);
+}
+
+static struct vc4_commit *commit_init(struct drm_atomic_state *state)
+{
+	struct vc4_commit *c = kzalloc(sizeof(*c), GFP_KERNEL);
+
+	if (!c)
+		return NULL;
+	c->dev = state->dev;
+	c->state = state;
+
+	return c;
+}
+
+/**
+ * vc4_atomic_commit - commit validated state object
+ * @dev: DRM device
+ * @state: the driver state object
+ * @async: asynchronous commit
+ *
+ * This function commits a with drm_atomic_helper_check() pre-validated state
+ * object. This can still fail when e.g. the framebuffer reservation fails. For
+ * now this doesn't implement asynchronous commits.
+ *
+ * RETURNS
+ * Zero for success or -errno.
+ */
+static int vc4_atomic_commit(struct drm_device *dev,
+			     struct drm_atomic_state *state,
+			     bool async)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	int ret;
+	int i;
+	uint64_t wait_seqno = 0;
+	struct vc4_commit *c;
+
+	c = commit_init(state);
+	if (!c)
+		return -ENOMEM;
+
+	/* Make sure that any outstanding modesets have finished. */
+	ret = down_interruptible(&vc4->async_modeset);
+	if (ret) {
+		kfree(c);
+		return ret;
+	}
+
+	ret = drm_atomic_helper_prepare_planes(dev, state);
+	if (ret) {
+		kfree(c);
+		up(&vc4->async_modeset);
+		return ret;
+	}
+
+	for (i = 0; i < dev->mode_config.num_total_plane; i++) {
+		struct drm_plane *plane = state->planes[i];
+		struct drm_plane_state *new_state = state->plane_states[i];
+
+		if (!plane)
+			continue;
+
+		if ((plane->state->fb != new_state->fb) && new_state->fb) {
+			struct drm_gem_cma_object *cma_bo =
+				drm_fb_cma_get_gem_obj(new_state->fb, 0);
+			struct vc4_bo *bo = to_vc4_bo(&cma_bo->base);
+
+			wait_seqno = max(bo->seqno, wait_seqno);
+		}
+	}
+
+	/*
+	 * This is the point of no return - everything below never fails except
+	 * when the hw goes bonghits. Which means we can commit the new state on
+	 * the software side now.
+	 */
+
+	drm_atomic_helper_swap_state(dev, state);
+
+	/*
+	 * Everything below can be run asynchronously without the need to grab
+	 * any modeset locks at all under one condition: It must be guaranteed
+	 * that the asynchronous work has either been cancelled (if the driver
+	 * supports it, which at least requires that the framebuffers get
+	 * cleaned up with drm_atomic_helper_cleanup_planes()) or completed
+	 * before the new state gets committed on the software side with
+	 * drm_atomic_helper_swap_state().
+	 *
+	 * This scheme allows new atomic state updates to be prepared and
+	 * checked in parallel to the asynchronous completion of the previous
+	 * update. Which is important since compositors need to figure out the
+	 * composition of the next frame right after having submitted the
+	 * current layout.
+	 */
+
+	if (async) {
+		vc4_queue_seqno_cb(dev, &c->cb, wait_seqno,
+				   vc4_atomic_complete_commit_seqno_cb);
+	} else {
+		vc4_wait_for_seqno(dev, wait_seqno, ~0ull, false);
+		vc4_atomic_complete_commit(c);
+	}
+
+	return 0;
+}
+
 static const struct drm_mode_config_funcs vc4_mode_funcs = {
 	.output_poll_changed = vc4_output_poll_changed,
 	.atomic_check = drm_atomic_helper_check,
-	.atomic_commit = drm_atomic_helper_commit,
+	.atomic_commit = vc4_atomic_commit,
 	.fb_create = drm_fb_cma_create,
 };
 
@@ -41,6 +184,8 @@ int vc4_kms_load(struct drm_device *dev)
 	struct vc4_dev *vc4 = to_vc4_dev(dev);
 	int ret;
 
+	sema_init(&vc4->async_modeset, 1);
+
 	ret = drm_vblank_init(dev, dev->mode_config.num_crtc);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to initialize vblank\n");
@@ -51,6 +196,8 @@ int vc4_kms_load(struct drm_device *dev)
 	dev->mode_config.max_height = 2048;
 	dev->mode_config.funcs = &vc4_mode_funcs;
 	dev->mode_config.preferred_depth = 24;
+	dev->mode_config.async_page_flip = true;
+
 	dev->vblank_disable_allowed = true;
 
 	drm_mode_config_reset(dev);
diff --git a/drivers/gpu/drm/vc4/vc4_packet.h b/drivers/gpu/drm/vc4/vc4_packet.h
new file mode 100644
index 000000000000..0f31cc06500f
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_packet.h
@@ -0,0 +1,399 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * 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 VC4_PACKET_H
+#define VC4_PACKET_H
+
+#include "vc4_regs.h" /* for VC4_MASK, VC4_GET_FIELD, VC4_SET_FIELD */
+
+enum vc4_packet {
+	VC4_PACKET_HALT = 0,
+	VC4_PACKET_NOP = 1,
+
+	VC4_PACKET_FLUSH = 4,
+	VC4_PACKET_FLUSH_ALL = 5,
+	VC4_PACKET_START_TILE_BINNING = 6,
+	VC4_PACKET_INCREMENT_SEMAPHORE = 7,
+	VC4_PACKET_WAIT_ON_SEMAPHORE = 8,
+
+	VC4_PACKET_BRANCH = 16,
+	VC4_PACKET_BRANCH_TO_SUB_LIST = 17,
+
+	VC4_PACKET_STORE_MS_TILE_BUFFER = 24,
+	VC4_PACKET_STORE_MS_TILE_BUFFER_AND_EOF = 25,
+	VC4_PACKET_STORE_FULL_RES_TILE_BUFFER = 26,
+	VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER = 27,
+	VC4_PACKET_STORE_TILE_BUFFER_GENERAL = 28,
+	VC4_PACKET_LOAD_TILE_BUFFER_GENERAL = 29,
+
+	VC4_PACKET_GL_INDEXED_PRIMITIVE = 32,
+	VC4_PACKET_GL_ARRAY_PRIMITIVE = 33,
+
+	VC4_PACKET_COMPRESSED_PRIMITIVE = 48,
+	VC4_PACKET_CLIPPED_COMPRESSED_PRIMITIVE = 49,
+
+	VC4_PACKET_PRIMITIVE_LIST_FORMAT = 56,
+
+	VC4_PACKET_GL_SHADER_STATE = 64,
+	VC4_PACKET_NV_SHADER_STATE = 65,
+	VC4_PACKET_VG_SHADER_STATE = 66,
+
+	VC4_PACKET_CONFIGURATION_BITS = 96,
+	VC4_PACKET_FLAT_SHADE_FLAGS = 97,
+	VC4_PACKET_POINT_SIZE = 98,
+	VC4_PACKET_LINE_WIDTH = 99,
+	VC4_PACKET_RHT_X_BOUNDARY = 100,
+	VC4_PACKET_DEPTH_OFFSET = 101,
+	VC4_PACKET_CLIP_WINDOW = 102,
+	VC4_PACKET_VIEWPORT_OFFSET = 103,
+	VC4_PACKET_Z_CLIPPING = 104,
+	VC4_PACKET_CLIPPER_XY_SCALING = 105,
+	VC4_PACKET_CLIPPER_Z_SCALING = 106,
+
+	VC4_PACKET_TILE_BINNING_MODE_CONFIG = 112,
+	VC4_PACKET_TILE_RENDERING_MODE_CONFIG = 113,
+	VC4_PACKET_CLEAR_COLORS = 114,
+	VC4_PACKET_TILE_COORDINATES = 115,
+
+	/* Not an actual hardware packet -- this is what we use to put
+	 * references to GEM bos in the command stream, since we need the u32
+	 * int the actual address packet in order to store the offset from the
+	 * start of the BO.
+	 */
+	VC4_PACKET_GEM_HANDLES = 254,
+} __attribute__ ((__packed__));
+
+#define VC4_PACKET_HALT_SIZE						1
+#define VC4_PACKET_NOP_SIZE						1
+#define VC4_PACKET_FLUSH_SIZE						1
+#define VC4_PACKET_FLUSH_ALL_SIZE					1
+#define VC4_PACKET_START_TILE_BINNING_SIZE				1
+#define VC4_PACKET_INCREMENT_SEMAPHORE_SIZE				1
+#define VC4_PACKET_WAIT_ON_SEMAPHORE_SIZE				1
+#define VC4_PACKET_BRANCH_SIZE						5
+#define VC4_PACKET_BRANCH_TO_SUB_LIST_SIZE				5
+#define VC4_PACKET_STORE_MS_TILE_BUFFER_SIZE				1
+#define VC4_PACKET_STORE_MS_TILE_BUFFER_AND_EOF_SIZE			1
+#define VC4_PACKET_STORE_FULL_RES_TILE_BUFFER_SIZE			5
+#define VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER_SIZE			5
+#define VC4_PACKET_STORE_TILE_BUFFER_GENERAL_SIZE			7
+#define VC4_PACKET_LOAD_TILE_BUFFER_GENERAL_SIZE			7
+#define VC4_PACKET_GL_INDEXED_PRIMITIVE_SIZE				14
+#define VC4_PACKET_GL_ARRAY_PRIMITIVE_SIZE				10
+#define VC4_PACKET_COMPRESSED_PRIMITIVE_SIZE				1
+#define VC4_PACKET_CLIPPED_COMPRESSED_PRIMITIVE_SIZE			1
+#define VC4_PACKET_PRIMITIVE_LIST_FORMAT_SIZE				2
+#define VC4_PACKET_GL_SHADER_STATE_SIZE					5
+#define VC4_PACKET_NV_SHADER_STATE_SIZE					5
+#define VC4_PACKET_VG_SHADER_STATE_SIZE					5
+#define VC4_PACKET_CONFIGURATION_BITS_SIZE				4
+#define VC4_PACKET_FLAT_SHADE_FLAGS_SIZE				5
+#define VC4_PACKET_POINT_SIZE_SIZE					5
+#define VC4_PACKET_LINE_WIDTH_SIZE					5
+#define VC4_PACKET_RHT_X_BOUNDARY_SIZE					3
+#define VC4_PACKET_DEPTH_OFFSET_SIZE					5
+#define VC4_PACKET_CLIP_WINDOW_SIZE					9
+#define VC4_PACKET_VIEWPORT_OFFSET_SIZE					5
+#define VC4_PACKET_Z_CLIPPING_SIZE					9
+#define VC4_PACKET_CLIPPER_XY_SCALING_SIZE				9
+#define VC4_PACKET_CLIPPER_Z_SCALING_SIZE				9
+#define VC4_PACKET_TILE_BINNING_MODE_CONFIG_SIZE			16
+#define VC4_PACKET_TILE_RENDERING_MODE_CONFIG_SIZE			11
+#define VC4_PACKET_CLEAR_COLORS_SIZE					14
+#define VC4_PACKET_TILE_COORDINATES_SIZE				3
+#define VC4_PACKET_GEM_HANDLES_SIZE					9
+
+/* Number of multisamples supported. */
+#define VC4_MAX_SAMPLES							4
+/* Size of a full resolution color or Z tile buffer load/store. */
+#define VC4_TILE_BUFFER_SIZE			(64 * 64 * 4)
+
+/** @{
+ * Bits used by packets like VC4_PACKET_STORE_TILE_BUFFER_GENERAL and
+ * VC4_PACKET_TILE_RENDERING_MODE_CONFIG.
+*/
+#define VC4_TILING_FORMAT_LINEAR    0
+#define VC4_TILING_FORMAT_T         1
+#define VC4_TILING_FORMAT_LT        2
+/** @} */
+
+/** @{
+ *
+ * low bits of VC4_PACKET_STORE_FULL_RES_TILE_BUFFER and
+ * VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER.
+ */
+#define VC4_LOADSTORE_FULL_RES_EOF                     BIT(3)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_CLEAR_ALL       BIT(2)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_ZS              BIT(1)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_COLOR           BIT(0)
+
+/** @{
+ *
+ * low bits of VC4_PACKET_STORE_FULL_RES_TILE_BUFFER and
+ * VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER.
+ */
+#define VC4_LOADSTORE_FULL_RES_EOF                     BIT(3)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_CLEAR_ALL       BIT(2)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_ZS              BIT(1)
+#define VC4_LOADSTORE_FULL_RES_DISABLE_COLOR           BIT(0)
+
+/** @{
+ *
+ * byte 2 of VC4_PACKET_STORE_TILE_BUFFER_GENERAL and
+ * VC4_PACKET_LOAD_TILE_BUFFER_GENERAL (low bits of the address)
+ */
+
+#define VC4_LOADSTORE_TILE_BUFFER_EOF                  BIT(3)
+#define VC4_LOADSTORE_TILE_BUFFER_DISABLE_FULL_VG_MASK BIT(2)
+#define VC4_LOADSTORE_TILE_BUFFER_DISABLE_FULL_ZS      BIT(1)
+#define VC4_LOADSTORE_TILE_BUFFER_DISABLE_FULL_COLOR   BIT(0)
+
+/** @} */
+
+/** @{
+ *
+ * byte 0-1 of VC4_PACKET_STORE_TILE_BUFFER_GENERAL and
+ * VC4_PACKET_LOAD_TILE_BUFFER_GENERAL
+ */
+#define VC4_STORE_TILE_BUFFER_DISABLE_VG_MASK_CLEAR BIT(15)
+#define VC4_STORE_TILE_BUFFER_DISABLE_ZS_CLEAR     BIT(14)
+#define VC4_STORE_TILE_BUFFER_DISABLE_COLOR_CLEAR  BIT(13)
+#define VC4_STORE_TILE_BUFFER_DISABLE_SWAP         BIT(12)
+
+#define VC4_LOADSTORE_TILE_BUFFER_FORMAT_MASK      VC4_MASK(9, 8)
+#define VC4_LOADSTORE_TILE_BUFFER_FORMAT_SHIFT     8
+#define VC4_LOADSTORE_TILE_BUFFER_RGBA8888         0
+#define VC4_LOADSTORE_TILE_BUFFER_BGR565_DITHER    1
+#define VC4_LOADSTORE_TILE_BUFFER_BGR565           2
+/** @} */
+
+/** @{
+ *
+ * byte 0 of VC4_PACKET_STORE_TILE_BUFFER_GENERAL and
+ * VC4_PACKET_LOAD_TILE_BUFFER_GENERAL
+ */
+#define VC4_STORE_TILE_BUFFER_MODE_MASK            VC4_MASK(7, 6)
+#define VC4_STORE_TILE_BUFFER_MODE_SHIFT           6
+#define VC4_STORE_TILE_BUFFER_MODE_SAMPLE0         (0 << 6)
+#define VC4_STORE_TILE_BUFFER_MODE_DECIMATE_X4     (1 << 6)
+#define VC4_STORE_TILE_BUFFER_MODE_DECIMATE_X16    (2 << 6)
+
+/** The values of the field are VC4_TILING_FORMAT_* */
+#define VC4_LOADSTORE_TILE_BUFFER_TILING_MASK      VC4_MASK(5, 4)
+#define VC4_LOADSTORE_TILE_BUFFER_TILING_SHIFT     4
+
+#define VC4_LOADSTORE_TILE_BUFFER_BUFFER_MASK      VC4_MASK(2, 0)
+#define VC4_LOADSTORE_TILE_BUFFER_BUFFER_SHIFT     0
+#define VC4_LOADSTORE_TILE_BUFFER_NONE             0
+#define VC4_LOADSTORE_TILE_BUFFER_COLOR            1
+#define VC4_LOADSTORE_TILE_BUFFER_ZS               2
+#define VC4_LOADSTORE_TILE_BUFFER_Z                3
+#define VC4_LOADSTORE_TILE_BUFFER_VG_MASK          4
+#define VC4_LOADSTORE_TILE_BUFFER_FULL             5
+/** @} */
+
+#define VC4_INDEX_BUFFER_U8                        (0 << 4)
+#define VC4_INDEX_BUFFER_U16                       (1 << 4)
+
+/* This flag is only present in NV shader state. */
+#define VC4_SHADER_FLAG_SHADED_CLIP_COORDS         BIT(3)
+#define VC4_SHADER_FLAG_ENABLE_CLIPPING            BIT(2)
+#define VC4_SHADER_FLAG_VS_POINT_SIZE              BIT(1)
+#define VC4_SHADER_FLAG_FS_SINGLE_THREAD           BIT(0)
+
+/** @{ byte 2 of config bits. */
+#define VC4_CONFIG_BITS_EARLY_Z_UPDATE             BIT(1)
+#define VC4_CONFIG_BITS_EARLY_Z                    BIT(0)
+/** @} */
+
+/** @{ byte 1 of config bits. */
+#define VC4_CONFIG_BITS_Z_UPDATE                   BIT(7)
+/** same values in this 3-bit field as PIPE_FUNC_* */
+#define VC4_CONFIG_BITS_DEPTH_FUNC_SHIFT           4
+#define VC4_CONFIG_BITS_COVERAGE_READ_LEAVE        BIT(3)
+
+#define VC4_CONFIG_BITS_COVERAGE_UPDATE_NONZERO    (0 << 1)
+#define VC4_CONFIG_BITS_COVERAGE_UPDATE_ODD        (1 << 1)
+#define VC4_CONFIG_BITS_COVERAGE_UPDATE_OR         (2 << 1)
+#define VC4_CONFIG_BITS_COVERAGE_UPDATE_ZERO       (3 << 1)
+
+#define VC4_CONFIG_BITS_COVERAGE_PIPE_SELECT       BIT(0)
+/** @} */
+
+/** @{ byte 0 of config bits. */
+#define VC4_CONFIG_BITS_RASTERIZER_OVERSAMPLE_NONE (0 << 6)
+#define VC4_CONFIG_BITS_RASTERIZER_OVERSAMPLE_4X   (1 << 6)
+#define VC4_CONFIG_BITS_RASTERIZER_OVERSAMPLE_16X  (2 << 6)
+
+#define VC4_CONFIG_BITS_AA_POINTS_AND_LINES        BIT(4)
+#define VC4_CONFIG_BITS_ENABLE_DEPTH_OFFSET        BIT(3)
+#define VC4_CONFIG_BITS_CW_PRIMITIVES              BIT(2)
+#define VC4_CONFIG_BITS_ENABLE_PRIM_BACK           BIT(1)
+#define VC4_CONFIG_BITS_ENABLE_PRIM_FRONT          BIT(0)
+/** @} */
+
+/** @{ bits in the last u8 of VC4_PACKET_TILE_BINNING_MODE_CONFIG */
+#define VC4_BIN_CONFIG_DB_NON_MS                   BIT(7)
+
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_MASK       VC4_MASK(6, 5)
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_SHIFT      5
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_32         0
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_64         1
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_128        2
+#define VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_256        3
+
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_MASK  VC4_MASK(4, 3)
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_SHIFT 3
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_32    0
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_64    1
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_128   2
+#define VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_256   3
+
+#define VC4_BIN_CONFIG_AUTO_INIT_TSDA              BIT(2)
+#define VC4_BIN_CONFIG_TILE_BUFFER_64BIT           BIT(1)
+#define VC4_BIN_CONFIG_MS_MODE_4X                  BIT(0)
+/** @} */
+
+/** @{ bits in the last u16 of VC4_PACKET_TILE_RENDERING_MODE_CONFIG */
+#define VC4_RENDER_CONFIG_DB_NON_MS                BIT(12)
+#define VC4_RENDER_CONFIG_EARLY_Z_COVERAGE_DISABLE BIT(11)
+#define VC4_RENDER_CONFIG_EARLY_Z_DIRECTION_G      BIT(10)
+#define VC4_RENDER_CONFIG_COVERAGE_MODE            BIT(9)
+#define VC4_RENDER_CONFIG_ENABLE_VG_MASK           BIT(8)
+
+/** The values of the field are VC4_TILING_FORMAT_* */
+#define VC4_RENDER_CONFIG_MEMORY_FORMAT_MASK       VC4_MASK(7, 6)
+#define VC4_RENDER_CONFIG_MEMORY_FORMAT_SHIFT      6
+
+#define VC4_RENDER_CONFIG_DECIMATE_MODE_1X         (0 << 4)
+#define VC4_RENDER_CONFIG_DECIMATE_MODE_4X         (1 << 4)
+#define VC4_RENDER_CONFIG_DECIMATE_MODE_16X        (2 << 4)
+
+#define VC4_RENDER_CONFIG_FORMAT_MASK              VC4_MASK(3, 2)
+#define VC4_RENDER_CONFIG_FORMAT_SHIFT             2
+#define VC4_RENDER_CONFIG_FORMAT_BGR565_DITHERED   0
+#define VC4_RENDER_CONFIG_FORMAT_RGBA8888          1
+#define VC4_RENDER_CONFIG_FORMAT_BGR565            2
+
+#define VC4_RENDER_CONFIG_TILE_BUFFER_64BIT        BIT(1)
+#define VC4_RENDER_CONFIG_MS_MODE_4X               BIT(0)
+
+#define VC4_PRIMITIVE_LIST_FORMAT_16_INDEX         (1 << 4)
+#define VC4_PRIMITIVE_LIST_FORMAT_32_XY            (3 << 4)
+#define VC4_PRIMITIVE_LIST_FORMAT_TYPE_POINTS      (0 << 0)
+#define VC4_PRIMITIVE_LIST_FORMAT_TYPE_LINES       (1 << 0)
+#define VC4_PRIMITIVE_LIST_FORMAT_TYPE_TRIANGLES   (2 << 0)
+#define VC4_PRIMITIVE_LIST_FORMAT_TYPE_RHT         (3 << 0)
+
+enum vc4_texture_data_type {
+	VC4_TEXTURE_TYPE_RGBA8888 = 0,
+	VC4_TEXTURE_TYPE_RGBX8888 = 1,
+	VC4_TEXTURE_TYPE_RGBA4444 = 2,
+	VC4_TEXTURE_TYPE_RGBA5551 = 3,
+	VC4_TEXTURE_TYPE_RGB565 = 4,
+	VC4_TEXTURE_TYPE_LUMINANCE = 5,
+	VC4_TEXTURE_TYPE_ALPHA = 6,
+	VC4_TEXTURE_TYPE_LUMALPHA = 7,
+	VC4_TEXTURE_TYPE_ETC1 = 8,
+	VC4_TEXTURE_TYPE_S16F = 9,
+	VC4_TEXTURE_TYPE_S8 = 10,
+	VC4_TEXTURE_TYPE_S16 = 11,
+	VC4_TEXTURE_TYPE_BW1 = 12,
+	VC4_TEXTURE_TYPE_A4 = 13,
+	VC4_TEXTURE_TYPE_A1 = 14,
+	VC4_TEXTURE_TYPE_RGBA64 = 15,
+	VC4_TEXTURE_TYPE_RGBA32R = 16,
+	VC4_TEXTURE_TYPE_YUV422R = 17,
+};
+
+#define VC4_TEX_P0_OFFSET_MASK                     VC4_MASK(31, 12)
+#define VC4_TEX_P0_OFFSET_SHIFT                    12
+#define VC4_TEX_P0_CSWIZ_MASK                      VC4_MASK(11, 10)
+#define VC4_TEX_P0_CSWIZ_SHIFT                     10
+#define VC4_TEX_P0_CMMODE_MASK                     VC4_MASK(9, 9)
+#define VC4_TEX_P0_CMMODE_SHIFT                    9
+#define VC4_TEX_P0_FLIPY_MASK                      VC4_MASK(8, 8)
+#define VC4_TEX_P0_FLIPY_SHIFT                     8
+#define VC4_TEX_P0_TYPE_MASK                       VC4_MASK(7, 4)
+#define VC4_TEX_P0_TYPE_SHIFT                      4
+#define VC4_TEX_P0_MIPLVLS_MASK                    VC4_MASK(3, 0)
+#define VC4_TEX_P0_MIPLVLS_SHIFT                   0
+
+#define VC4_TEX_P1_TYPE4_MASK                      VC4_MASK(31, 31)
+#define VC4_TEX_P1_TYPE4_SHIFT                     31
+#define VC4_TEX_P1_HEIGHT_MASK                     VC4_MASK(30, 20)
+#define VC4_TEX_P1_HEIGHT_SHIFT                    20
+#define VC4_TEX_P1_ETCFLIP_MASK                    VC4_MASK(19, 19)
+#define VC4_TEX_P1_ETCFLIP_SHIFT                   19
+#define VC4_TEX_P1_WIDTH_MASK                      VC4_MASK(18, 8)
+#define VC4_TEX_P1_WIDTH_SHIFT                     8
+
+#define VC4_TEX_P1_MAGFILT_MASK                    VC4_MASK(7, 7)
+#define VC4_TEX_P1_MAGFILT_SHIFT                   7
+# define VC4_TEX_P1_MAGFILT_LINEAR                 0
+# define VC4_TEX_P1_MAGFILT_NEAREST                1
+
+#define VC4_TEX_P1_MINFILT_MASK                    VC4_MASK(6, 4)
+#define VC4_TEX_P1_MINFILT_SHIFT                   4
+# define VC4_TEX_P1_MINFILT_LINEAR                 0
+# define VC4_TEX_P1_MINFILT_NEAREST                1
+# define VC4_TEX_P1_MINFILT_NEAR_MIP_NEAR          2
+# define VC4_TEX_P1_MINFILT_NEAR_MIP_LIN           3
+# define VC4_TEX_P1_MINFILT_LIN_MIP_NEAR           4
+# define VC4_TEX_P1_MINFILT_LIN_MIP_LIN            5
+
+#define VC4_TEX_P1_WRAP_T_MASK                     VC4_MASK(3, 2)
+#define VC4_TEX_P1_WRAP_T_SHIFT                    2
+#define VC4_TEX_P1_WRAP_S_MASK                     VC4_MASK(1, 0)
+#define VC4_TEX_P1_WRAP_S_SHIFT                    0
+# define VC4_TEX_P1_WRAP_REPEAT                    0
+# define VC4_TEX_P1_WRAP_CLAMP                     1
+# define VC4_TEX_P1_WRAP_MIRROR                    2
+# define VC4_TEX_P1_WRAP_BORDER                    3
+
+#define VC4_TEX_P2_PTYPE_MASK                      VC4_MASK(31, 30)
+#define VC4_TEX_P2_PTYPE_SHIFT                     30
+# define VC4_TEX_P2_PTYPE_IGNORED                  0
+# define VC4_TEX_P2_PTYPE_CUBE_MAP_STRIDE          1
+# define VC4_TEX_P2_PTYPE_CHILD_IMAGE_DIMENSIONS   2
+# define VC4_TEX_P2_PTYPE_CHILD_IMAGE_OFFSETS      3
+
+/* VC4_TEX_P2_PTYPE_CUBE_MAP_STRIDE bits */
+#define VC4_TEX_P2_CMST_MASK                       VC4_MASK(29, 12)
+#define VC4_TEX_P2_CMST_SHIFT                      12
+#define VC4_TEX_P2_BSLOD_MASK                      VC4_MASK(0, 0)
+#define VC4_TEX_P2_BSLOD_SHIFT                     0
+
+/* VC4_TEX_P2_PTYPE_CHILD_IMAGE_DIMENSIONS */
+#define VC4_TEX_P2_CHEIGHT_MASK                    VC4_MASK(22, 12)
+#define VC4_TEX_P2_CHEIGHT_SHIFT                   12
+#define VC4_TEX_P2_CWIDTH_MASK                     VC4_MASK(10, 0)
+#define VC4_TEX_P2_CWIDTH_SHIFT                    0
+
+/* VC4_TEX_P2_PTYPE_CHILD_IMAGE_OFFSETS */
+#define VC4_TEX_P2_CYOFF_MASK                      VC4_MASK(22, 12)
+#define VC4_TEX_P2_CYOFF_SHIFT                     12
+#define VC4_TEX_P2_CXOFF_MASK                      VC4_MASK(10, 0)
+#define VC4_TEX_P2_CXOFF_SHIFT                     0
+
+#endif /* VC4_PACKET_H */
diff --git a/drivers/gpu/drm/vc4/vc4_plane.c b/drivers/gpu/drm/vc4/vc4_plane.c
index 887f3caad0be..0addbad15832 100644
--- a/drivers/gpu/drm/vc4/vc4_plane.c
+++ b/drivers/gpu/drm/vc4/vc4_plane.c
@@ -29,6 +29,14 @@ struct vc4_plane_state {
 	u32 *dlist;
 	u32 dlist_size; /* Number of dwords in allocated for the display list */
 	u32 dlist_count; /* Number of used dwords in the display list. */
+
+	/* Offset in the dlist to pointer word 0. */
+	u32 pw0_offset;
+
+	/* Offset where the plane's dlist was last stored in the
+	   hardware at vc4_crtc_atomic_flush() time.
+	*/
+	u32 *hw_dlist;
 };
 
 static inline struct vc4_plane_state *
@@ -207,6 +215,8 @@ static int vc4_plane_mode_set(struct drm_plane *plane,
 	/* Position Word 3: Context.  Written by the HVS. */
 	vc4_dlist_write(vc4_state, 0xc0c0c0c0);
 
+	vc4_state->pw0_offset = vc4_state->dlist_count;
+
 	/* Pointer Word 0: RGB / Y Pointer */
 	vc4_dlist_write(vc4_state, bo->paddr + offset);
 
@@ -258,6 +268,8 @@ u32 vc4_plane_write_dlist(struct drm_plane *plane, u32 __iomem *dlist)
 	struct vc4_plane_state *vc4_state = to_vc4_plane_state(plane->state);
 	int i;
 
+	vc4_state->hw_dlist = dlist;
+
 	/* Can't memcpy_toio() because it needs to be 32-bit writes. */
 	for (i = 0; i < vc4_state->dlist_count; i++)
 		writel(vc4_state->dlist[i], &dlist[i]);
@@ -272,6 +284,34 @@ u32 vc4_plane_dlist_size(struct drm_plane_state *state)
 	return vc4_state->dlist_count;
 }
 
+/* Updates the plane to immediately (well, once the FIFO needs
+ * refilling) scan out from at a new framebuffer.
+ */
+void vc4_plane_async_set_fb(struct drm_plane *plane, struct drm_framebuffer *fb)
+{
+	struct vc4_plane_state *vc4_state = to_vc4_plane_state(plane->state);
+	struct drm_gem_cma_object *bo = drm_fb_cma_get_gem_obj(fb, 0);
+	uint32_t addr;
+
+	/* We're skipping the address adjustment for negative origin,
+	 * because this is only called on the primary plane.
+	 */
+	WARN_ON_ONCE(plane->state->crtc_x < 0 || plane->state->crtc_y < 0);
+	addr = bo->paddr + fb->offsets[0];
+
+	/* Write the new address into the hardware immediately.  The
+	 * scanout will start from this address as soon as the FIFO
+	 * needs to refill with pixels.
+	 */
+	writel(addr, &vc4_state->hw_dlist[vc4_state->pw0_offset]);
+
+	/* Also update the CPU-side dlist copy, so that any later
+	 * atomic updates that don't do a new modeset on our plane
+	 * also use our updated address.
+	 */
+	vc4_state->dlist[vc4_state->pw0_offset] = addr;
+}
+
 static const struct drm_plane_helper_funcs vc4_plane_helper_funcs = {
 	.prepare_fb = NULL,
 	.cleanup_fb = NULL,
@@ -317,7 +357,7 @@ struct drm_plane *vc4_plane_init(struct drm_device *dev,
 	ret = drm_universal_plane_init(dev, plane, 0xff,
 				       &vc4_plane_funcs,
 				       formats, ARRAY_SIZE(formats),
-				       type);
+				       type, NULL);
 
 	drm_plane_helper_add(plane, &vc4_plane_helper_funcs);
 
diff --git a/drivers/gpu/drm/vc4/vc4_qpu_defines.h b/drivers/gpu/drm/vc4/vc4_qpu_defines.h
new file mode 100644
index 000000000000..d5c2f3c85ebb
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_qpu_defines.h
@@ -0,0 +1,264 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * 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 VC4_QPU_DEFINES_H
+#define VC4_QPU_DEFINES_H
+
+enum qpu_op_add {
+	QPU_A_NOP,
+	QPU_A_FADD,
+	QPU_A_FSUB,
+	QPU_A_FMIN,
+	QPU_A_FMAX,
+	QPU_A_FMINABS,
+	QPU_A_FMAXABS,
+	QPU_A_FTOI,
+	QPU_A_ITOF,
+	QPU_A_ADD = 12,
+	QPU_A_SUB,
+	QPU_A_SHR,
+	QPU_A_ASR,
+	QPU_A_ROR,
+	QPU_A_SHL,
+	QPU_A_MIN,
+	QPU_A_MAX,
+	QPU_A_AND,
+	QPU_A_OR,
+	QPU_A_XOR,
+	QPU_A_NOT,
+	QPU_A_CLZ,
+	QPU_A_V8ADDS = 30,
+	QPU_A_V8SUBS = 31,
+};
+
+enum qpu_op_mul {
+	QPU_M_NOP,
+	QPU_M_FMUL,
+	QPU_M_MUL24,
+	QPU_M_V8MULD,
+	QPU_M_V8MIN,
+	QPU_M_V8MAX,
+	QPU_M_V8ADDS,
+	QPU_M_V8SUBS,
+};
+
+enum qpu_raddr {
+	QPU_R_FRAG_PAYLOAD_ZW = 15, /* W for A file, Z for B file */
+	/* 0-31 are the plain regfile a or b fields */
+	QPU_R_UNIF = 32,
+	QPU_R_VARY = 35,
+	QPU_R_ELEM_QPU = 38,
+	QPU_R_NOP,
+	QPU_R_XY_PIXEL_COORD = 41,
+	QPU_R_MS_REV_FLAGS = 41,
+	QPU_R_VPM = 48,
+	QPU_R_VPM_LD_BUSY,
+	QPU_R_VPM_LD_WAIT,
+	QPU_R_MUTEX_ACQUIRE,
+};
+
+enum qpu_waddr {
+	/* 0-31 are the plain regfile a or b fields */
+	QPU_W_ACC0 = 32, /* aka r0 */
+	QPU_W_ACC1,
+	QPU_W_ACC2,
+	QPU_W_ACC3,
+	QPU_W_TMU_NOSWAP,
+	QPU_W_ACC5,
+	QPU_W_HOST_INT,
+	QPU_W_NOP,
+	QPU_W_UNIFORMS_ADDRESS,
+	QPU_W_QUAD_XY, /* X for regfile a, Y for regfile b */
+	QPU_W_MS_FLAGS = 42,
+	QPU_W_REV_FLAG = 42,
+	QPU_W_TLB_STENCIL_SETUP = 43,
+	QPU_W_TLB_Z,
+	QPU_W_TLB_COLOR_MS,
+	QPU_W_TLB_COLOR_ALL,
+	QPU_W_TLB_ALPHA_MASK,
+	QPU_W_VPM,
+	QPU_W_VPMVCD_SETUP, /* LD for regfile a, ST for regfile b */
+	QPU_W_VPM_ADDR, /* LD for regfile a, ST for regfile b */
+	QPU_W_MUTEX_RELEASE,
+	QPU_W_SFU_RECIP,
+	QPU_W_SFU_RECIPSQRT,
+	QPU_W_SFU_EXP,
+	QPU_W_SFU_LOG,
+	QPU_W_TMU0_S,
+	QPU_W_TMU0_T,
+	QPU_W_TMU0_R,
+	QPU_W_TMU0_B,
+	QPU_W_TMU1_S,
+	QPU_W_TMU1_T,
+	QPU_W_TMU1_R,
+	QPU_W_TMU1_B,
+};
+
+enum qpu_sig_bits {
+	QPU_SIG_SW_BREAKPOINT,
+	QPU_SIG_NONE,
+	QPU_SIG_THREAD_SWITCH,
+	QPU_SIG_PROG_END,
+	QPU_SIG_WAIT_FOR_SCOREBOARD,
+	QPU_SIG_SCOREBOARD_UNLOCK,
+	QPU_SIG_LAST_THREAD_SWITCH,
+	QPU_SIG_COVERAGE_LOAD,
+	QPU_SIG_COLOR_LOAD,
+	QPU_SIG_COLOR_LOAD_END,
+	QPU_SIG_LOAD_TMU0,
+	QPU_SIG_LOAD_TMU1,
+	QPU_SIG_ALPHA_MASK_LOAD,
+	QPU_SIG_SMALL_IMM,
+	QPU_SIG_LOAD_IMM,
+	QPU_SIG_BRANCH
+};
+
+enum qpu_mux {
+	/* hardware mux values */
+	QPU_MUX_R0,
+	QPU_MUX_R1,
+	QPU_MUX_R2,
+	QPU_MUX_R3,
+	QPU_MUX_R4,
+	QPU_MUX_R5,
+	QPU_MUX_A,
+	QPU_MUX_B,
+
+	/* non-hardware mux values */
+	QPU_MUX_IMM,
+};
+
+enum qpu_cond {
+	QPU_COND_NEVER,
+	QPU_COND_ALWAYS,
+	QPU_COND_ZS,
+	QPU_COND_ZC,
+	QPU_COND_NS,
+	QPU_COND_NC,
+	QPU_COND_CS,
+	QPU_COND_CC,
+};
+
+enum qpu_pack_mul {
+	QPU_PACK_MUL_NOP,
+	/* replicated to each 8 bits of the 32-bit dst. */
+	QPU_PACK_MUL_8888 = 3,
+	QPU_PACK_MUL_8A,
+	QPU_PACK_MUL_8B,
+	QPU_PACK_MUL_8C,
+	QPU_PACK_MUL_8D,
+};
+
+enum qpu_pack_a {
+	QPU_PACK_A_NOP,
+	/* convert to 16 bit float if float input, or to int16. */
+	QPU_PACK_A_16A,
+	QPU_PACK_A_16B,
+	/* replicated to each 8 bits of the 32-bit dst. */
+	QPU_PACK_A_8888,
+	/* Convert to 8-bit unsigned int. */
+	QPU_PACK_A_8A,
+	QPU_PACK_A_8B,
+	QPU_PACK_A_8C,
+	QPU_PACK_A_8D,
+
+	/* Saturating variants of the previous instructions. */
+	QPU_PACK_A_32_SAT, /* int-only */
+	QPU_PACK_A_16A_SAT, /* int or float */
+	QPU_PACK_A_16B_SAT,
+	QPU_PACK_A_8888_SAT,
+	QPU_PACK_A_8A_SAT,
+	QPU_PACK_A_8B_SAT,
+	QPU_PACK_A_8C_SAT,
+	QPU_PACK_A_8D_SAT,
+};
+
+enum qpu_unpack_r4 {
+	QPU_UNPACK_R4_NOP,
+	QPU_UNPACK_R4_F16A_TO_F32,
+	QPU_UNPACK_R4_F16B_TO_F32,
+	QPU_UNPACK_R4_8D_REP,
+	QPU_UNPACK_R4_8A,
+	QPU_UNPACK_R4_8B,
+	QPU_UNPACK_R4_8C,
+	QPU_UNPACK_R4_8D,
+};
+
+#define QPU_MASK(high, low) \
+	((((uint64_t)1 << ((high) - (low) + 1)) - 1) << (low))
+
+#define QPU_GET_FIELD(word, field) \
+	((uint32_t)(((word)  & field ## _MASK) >> field ## _SHIFT))
+
+#define QPU_SIG_SHIFT                   60
+#define QPU_SIG_MASK                    QPU_MASK(63, 60)
+
+#define QPU_UNPACK_SHIFT                57
+#define QPU_UNPACK_MASK                 QPU_MASK(59, 57)
+
+/**
+ * If set, the pack field means PACK_MUL or R4 packing, instead of normal
+ * regfile a packing.
+ */
+#define QPU_PM                          ((uint64_t)1 << 56)
+
+#define QPU_PACK_SHIFT                  52
+#define QPU_PACK_MASK                   QPU_MASK(55, 52)
+
+#define QPU_COND_ADD_SHIFT              49
+#define QPU_COND_ADD_MASK               QPU_MASK(51, 49)
+#define QPU_COND_MUL_SHIFT              46
+#define QPU_COND_MUL_MASK               QPU_MASK(48, 46)
+
+#define QPU_SF                          ((uint64_t)1 << 45)
+
+#define QPU_WADDR_ADD_SHIFT             38
+#define QPU_WADDR_ADD_MASK              QPU_MASK(43, 38)
+#define QPU_WADDR_MUL_SHIFT             32
+#define QPU_WADDR_MUL_MASK              QPU_MASK(37, 32)
+
+#define QPU_OP_MUL_SHIFT                29
+#define QPU_OP_MUL_MASK                 QPU_MASK(31, 29)
+
+#define QPU_RADDR_A_SHIFT               18
+#define QPU_RADDR_A_MASK                QPU_MASK(23, 18)
+#define QPU_RADDR_B_SHIFT               12
+#define QPU_RADDR_B_MASK                QPU_MASK(17, 12)
+#define QPU_SMALL_IMM_SHIFT             12
+#define QPU_SMALL_IMM_MASK              QPU_MASK(17, 12)
+
+#define QPU_ADD_A_SHIFT                 9
+#define QPU_ADD_A_MASK                  QPU_MASK(11, 9)
+#define QPU_ADD_B_SHIFT                 6
+#define QPU_ADD_B_MASK                  QPU_MASK(8, 6)
+#define QPU_MUL_A_SHIFT                 3
+#define QPU_MUL_A_MASK                  QPU_MASK(5, 3)
+#define QPU_MUL_B_SHIFT                 0
+#define QPU_MUL_B_MASK                  QPU_MASK(2, 0)
+
+#define QPU_WS                          ((uint64_t)1 << 44)
+
+#define QPU_OP_ADD_SHIFT                24
+#define QPU_OP_ADD_MASK                 QPU_MASK(28, 24)
+
+#endif /* VC4_QPU_DEFINES_H */
diff --git a/drivers/gpu/drm/vc4/vc4_regs.h b/drivers/gpu/drm/vc4/vc4_regs.h
index 9e4e904c668e..4e52a0a88551 100644
--- a/drivers/gpu/drm/vc4/vc4_regs.h
+++ b/drivers/gpu/drm/vc4/vc4_regs.h
@@ -154,7 +154,7 @@
 #define V3D_PCTRS14  0x006f4
 #define V3D_PCTR15   0x006f8
 #define V3D_PCTRS15  0x006fc
-#define V3D_BGE      0x00f00
+#define V3D_DBGE     0x00f00
 #define V3D_FDBGO    0x00f04
 #define V3D_FDBGB    0x00f08
 #define V3D_FDBGR    0x00f0c
diff --git a/drivers/gpu/drm/vc4/vc4_render_cl.c b/drivers/gpu/drm/vc4/vc4_render_cl.c
new file mode 100644
index 000000000000..0f12418725e5
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_render_cl.c
@@ -0,0 +1,634 @@
+/*
+ * Copyright © 2014-2015 Broadcom
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+/**
+ * DOC: Render command list generation
+ *
+ * In the VC4 driver, render command list generation is performed by the
+ * kernel instead of userspace.  We do this because validating a
+ * user-submitted command list is hard to get right and has high CPU overhead,
+ * while the number of valid configurations for render command lists is
+ * actually fairly low.
+ */
+
+#include "uapi/drm/vc4_drm.h"
+#include "vc4_drv.h"
+#include "vc4_packet.h"
+
+struct vc4_rcl_setup {
+	struct drm_gem_cma_object *color_read;
+	struct drm_gem_cma_object *color_write;
+	struct drm_gem_cma_object *zs_read;
+	struct drm_gem_cma_object *zs_write;
+	struct drm_gem_cma_object *msaa_color_write;
+	struct drm_gem_cma_object *msaa_zs_write;
+
+	struct drm_gem_cma_object *rcl;
+	u32 next_offset;
+};
+
+static inline void rcl_u8(struct vc4_rcl_setup *setup, u8 val)
+{
+	*(u8 *)(setup->rcl->vaddr + setup->next_offset) = val;
+	setup->next_offset += 1;
+}
+
+static inline void rcl_u16(struct vc4_rcl_setup *setup, u16 val)
+{
+	*(u16 *)(setup->rcl->vaddr + setup->next_offset) = val;
+	setup->next_offset += 2;
+}
+
+static inline void rcl_u32(struct vc4_rcl_setup *setup, u32 val)
+{
+	*(u32 *)(setup->rcl->vaddr + setup->next_offset) = val;
+	setup->next_offset += 4;
+}
+
+/*
+ * Emits a no-op STORE_TILE_BUFFER_GENERAL.
+ *
+ * If we emit a PACKET_TILE_COORDINATES, it must be followed by a store of
+ * some sort before another load is triggered.
+ */
+static void vc4_store_before_load(struct vc4_rcl_setup *setup)
+{
+	rcl_u8(setup, VC4_PACKET_STORE_TILE_BUFFER_GENERAL);
+	rcl_u16(setup,
+		VC4_SET_FIELD(VC4_LOADSTORE_TILE_BUFFER_NONE,
+			      VC4_LOADSTORE_TILE_BUFFER_BUFFER) |
+		VC4_STORE_TILE_BUFFER_DISABLE_COLOR_CLEAR |
+		VC4_STORE_TILE_BUFFER_DISABLE_ZS_CLEAR |
+		VC4_STORE_TILE_BUFFER_DISABLE_VG_MASK_CLEAR);
+	rcl_u32(setup, 0); /* no address, since we're in None mode */
+}
+
+/*
+ * Calculates the physical address of the start of a tile in a RCL surface.
+ *
+ * Unlike the other load/store packets,
+ * VC4_PACKET_LOAD/STORE_FULL_RES_TILE_BUFFER don't look at the tile
+ * coordinates packet, and instead just store to the address given.
+ */
+static uint32_t vc4_full_res_offset(struct vc4_exec_info *exec,
+				    struct drm_gem_cma_object *bo,
+				    struct drm_vc4_submit_rcl_surface *surf,
+				    uint8_t x, uint8_t y)
+{
+	return bo->paddr + surf->offset + VC4_TILE_BUFFER_SIZE *
+		(DIV_ROUND_UP(exec->args->width, 32) * y + x);
+}
+
+/*
+ * Emits a PACKET_TILE_COORDINATES if one isn't already pending.
+ *
+ * The tile coordinates packet triggers a pending load if there is one, are
+ * used for clipping during rendering, and determine where loads/stores happen
+ * relative to their base address.
+ */
+static void vc4_tile_coordinates(struct vc4_rcl_setup *setup,
+				 uint32_t x, uint32_t y)
+{
+	rcl_u8(setup, VC4_PACKET_TILE_COORDINATES);
+	rcl_u8(setup, x);
+	rcl_u8(setup, y);
+}
+
+static void emit_tile(struct vc4_exec_info *exec,
+		      struct vc4_rcl_setup *setup,
+		      uint8_t x, uint8_t y, bool first, bool last)
+{
+	struct drm_vc4_submit_cl *args = exec->args;
+	bool has_bin = args->bin_cl_size != 0;
+
+	/* Note that the load doesn't actually occur until the
+	 * tile coords packet is processed, and only one load
+	 * may be outstanding at a time.
+	 */
+	if (setup->color_read) {
+		if (args->color_read.flags &
+		    VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
+			rcl_u8(setup, VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER);
+			rcl_u32(setup,
+				vc4_full_res_offset(exec, setup->color_read,
+						    &args->color_read, x, y) |
+				VC4_LOADSTORE_FULL_RES_DISABLE_ZS);
+		} else {
+			rcl_u8(setup, VC4_PACKET_LOAD_TILE_BUFFER_GENERAL);
+			rcl_u16(setup, args->color_read.bits);
+			rcl_u32(setup, setup->color_read->paddr +
+				args->color_read.offset);
+		}
+	}
+
+	if (setup->zs_read) {
+		if (args->zs_read.flags &
+		    VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
+			rcl_u8(setup, VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER);
+			rcl_u32(setup,
+				vc4_full_res_offset(exec, setup->zs_read,
+						    &args->zs_read, x, y) |
+				VC4_LOADSTORE_FULL_RES_DISABLE_COLOR);
+		} else {
+			if (setup->color_read) {
+				/* Exec previous load. */
+				vc4_tile_coordinates(setup, x, y);
+				vc4_store_before_load(setup);
+			}
+
+			rcl_u8(setup, VC4_PACKET_LOAD_TILE_BUFFER_GENERAL);
+			rcl_u16(setup, args->zs_read.bits);
+			rcl_u32(setup, setup->zs_read->paddr +
+				args->zs_read.offset);
+		}
+	}
+
+	/* Clipping depends on tile coordinates having been
+	 * emitted, so we always need one here.
+	 */
+	vc4_tile_coordinates(setup, x, y);
+
+	/* Wait for the binner before jumping to the first
+	 * tile's lists.
+	 */
+	if (first && has_bin)
+		rcl_u8(setup, VC4_PACKET_WAIT_ON_SEMAPHORE);
+
+	if (has_bin) {
+		rcl_u8(setup, VC4_PACKET_BRANCH_TO_SUB_LIST);
+		rcl_u32(setup, (exec->tile_bo->paddr +
+				exec->tile_alloc_offset +
+				(y * exec->bin_tiles_x + x) * 32));
+	}
+
+	if (setup->msaa_color_write) {
+		bool last_tile_write = (!setup->msaa_zs_write &&
+					!setup->zs_write &&
+					!setup->color_write);
+		uint32_t bits = VC4_LOADSTORE_FULL_RES_DISABLE_ZS;
+
+		if (!last_tile_write)
+			bits |= VC4_LOADSTORE_FULL_RES_DISABLE_CLEAR_ALL;
+		else if (last)
+			bits |= VC4_LOADSTORE_FULL_RES_EOF;
+		rcl_u8(setup, VC4_PACKET_STORE_FULL_RES_TILE_BUFFER);
+		rcl_u32(setup,
+			vc4_full_res_offset(exec, setup->msaa_color_write,
+					    &args->msaa_color_write, x, y) |
+			bits);
+	}
+
+	if (setup->msaa_zs_write) {
+		bool last_tile_write = (!setup->zs_write &&
+					!setup->color_write);
+		uint32_t bits = VC4_LOADSTORE_FULL_RES_DISABLE_COLOR;
+
+		if (setup->msaa_color_write)
+			vc4_tile_coordinates(setup, x, y);
+		if (!last_tile_write)
+			bits |= VC4_LOADSTORE_FULL_RES_DISABLE_CLEAR_ALL;
+		else if (last)
+			bits |= VC4_LOADSTORE_FULL_RES_EOF;
+		rcl_u8(setup, VC4_PACKET_STORE_FULL_RES_TILE_BUFFER);
+		rcl_u32(setup,
+			vc4_full_res_offset(exec, setup->msaa_zs_write,
+					    &args->msaa_zs_write, x, y) |
+			bits);
+	}
+
+	if (setup->zs_write) {
+		bool last_tile_write = !setup->color_write;
+
+		if (setup->msaa_color_write || setup->msaa_zs_write)
+			vc4_tile_coordinates(setup, x, y);
+
+		rcl_u8(setup, VC4_PACKET_STORE_TILE_BUFFER_GENERAL);
+		rcl_u16(setup, args->zs_write.bits |
+			(last_tile_write ?
+			 0 : VC4_STORE_TILE_BUFFER_DISABLE_COLOR_CLEAR));
+		rcl_u32(setup,
+			(setup->zs_write->paddr + args->zs_write.offset) |
+			((last && last_tile_write) ?
+			 VC4_LOADSTORE_TILE_BUFFER_EOF : 0));
+	}
+
+	if (setup->color_write) {
+		if (setup->msaa_color_write || setup->msaa_zs_write ||
+		    setup->zs_write) {
+			vc4_tile_coordinates(setup, x, y);
+		}
+
+		if (last)
+			rcl_u8(setup, VC4_PACKET_STORE_MS_TILE_BUFFER_AND_EOF);
+		else
+			rcl_u8(setup, VC4_PACKET_STORE_MS_TILE_BUFFER);
+	}
+}
+
+static int vc4_create_rcl_bo(struct drm_device *dev, struct vc4_exec_info *exec,
+			     struct vc4_rcl_setup *setup)
+{
+	struct drm_vc4_submit_cl *args = exec->args;
+	bool has_bin = args->bin_cl_size != 0;
+	uint8_t min_x_tile = args->min_x_tile;
+	uint8_t min_y_tile = args->min_y_tile;
+	uint8_t max_x_tile = args->max_x_tile;
+	uint8_t max_y_tile = args->max_y_tile;
+	uint8_t xtiles = max_x_tile - min_x_tile + 1;
+	uint8_t ytiles = max_y_tile - min_y_tile + 1;
+	uint8_t x, y;
+	uint32_t size, loop_body_size;
+
+	size = VC4_PACKET_TILE_RENDERING_MODE_CONFIG_SIZE;
+	loop_body_size = VC4_PACKET_TILE_COORDINATES_SIZE;
+
+	if (args->flags & VC4_SUBMIT_CL_USE_CLEAR_COLOR) {
+		size += VC4_PACKET_CLEAR_COLORS_SIZE +
+			VC4_PACKET_TILE_COORDINATES_SIZE +
+			VC4_PACKET_STORE_TILE_BUFFER_GENERAL_SIZE;
+	}
+
+	if (setup->color_read) {
+		if (args->color_read.flags &
+		    VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
+			loop_body_size += VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER_SIZE;
+		} else {
+			loop_body_size += VC4_PACKET_LOAD_TILE_BUFFER_GENERAL_SIZE;
+		}
+	}
+	if (setup->zs_read) {
+		if (args->zs_read.flags &
+		    VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
+			loop_body_size += VC4_PACKET_LOAD_FULL_RES_TILE_BUFFER_SIZE;
+		} else {
+			if (setup->color_read &&
+			    !(args->color_read.flags &
+			      VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES)) {
+				loop_body_size += VC4_PACKET_TILE_COORDINATES_SIZE;
+				loop_body_size += VC4_PACKET_STORE_TILE_BUFFER_GENERAL_SIZE;
+			}
+			loop_body_size += VC4_PACKET_LOAD_TILE_BUFFER_GENERAL_SIZE;
+		}
+	}
+
+	if (has_bin) {
+		size += VC4_PACKET_WAIT_ON_SEMAPHORE_SIZE;
+		loop_body_size += VC4_PACKET_BRANCH_TO_SUB_LIST_SIZE;
+	}
+
+	if (setup->msaa_color_write)
+		loop_body_size += VC4_PACKET_STORE_FULL_RES_TILE_BUFFER_SIZE;
+	if (setup->msaa_zs_write)
+		loop_body_size += VC4_PACKET_STORE_FULL_RES_TILE_BUFFER_SIZE;
+
+	if (setup->zs_write)
+		loop_body_size += VC4_PACKET_STORE_TILE_BUFFER_GENERAL_SIZE;
+	if (setup->color_write)
+		loop_body_size += VC4_PACKET_STORE_MS_TILE_BUFFER_SIZE;
+
+	/* We need a VC4_PACKET_TILE_COORDINATES in between each store. */
+	loop_body_size += VC4_PACKET_TILE_COORDINATES_SIZE *
+		((setup->msaa_color_write != NULL) +
+		 (setup->msaa_zs_write != NULL) +
+		 (setup->color_write != NULL) +
+		 (setup->zs_write != NULL) - 1);
+
+	size += xtiles * ytiles * loop_body_size;
+
+	setup->rcl = &vc4_bo_create(dev, size, true)->base;
+	if (IS_ERR(setup->rcl))
+		return PTR_ERR(setup->rcl);
+	list_add_tail(&to_vc4_bo(&setup->rcl->base)->unref_head,
+		      &exec->unref_list);
+
+	/* The tile buffer gets cleared when the previous tile is stored.  If
+	 * the clear values changed between frames, then the tile buffer has
+	 * stale clear values in it, so we have to do a store in None mode (no
+	 * writes) so that we trigger the tile buffer clear.
+	 */
+	if (args->flags & VC4_SUBMIT_CL_USE_CLEAR_COLOR) {
+		rcl_u8(setup, VC4_PACKET_CLEAR_COLORS);
+		rcl_u32(setup, args->clear_color[0]);
+		rcl_u32(setup, args->clear_color[1]);
+		rcl_u32(setup, args->clear_z);
+		rcl_u8(setup, args->clear_s);
+
+		vc4_tile_coordinates(setup, 0, 0);
+
+		rcl_u8(setup, VC4_PACKET_STORE_TILE_BUFFER_GENERAL);
+		rcl_u16(setup, VC4_LOADSTORE_TILE_BUFFER_NONE);
+		rcl_u32(setup, 0); /* no address, since we're in None mode */
+	}
+
+	rcl_u8(setup, VC4_PACKET_TILE_RENDERING_MODE_CONFIG);
+	rcl_u32(setup,
+		(setup->color_write ? (setup->color_write->paddr +
+				       args->color_write.offset) :
+		 0));
+	rcl_u16(setup, args->width);
+	rcl_u16(setup, args->height);
+	rcl_u16(setup, args->color_write.bits);
+
+	for (y = min_y_tile; y <= max_y_tile; y++) {
+		for (x = min_x_tile; x <= max_x_tile; x++) {
+			bool first = (x == min_x_tile && y == min_y_tile);
+			bool last = (x == max_x_tile && y == max_y_tile);
+
+			emit_tile(exec, setup, x, y, first, last);
+		}
+	}
+
+	BUG_ON(setup->next_offset != size);
+	exec->ct1ca = setup->rcl->paddr;
+	exec->ct1ea = setup->rcl->paddr + setup->next_offset;
+
+	return 0;
+}
+
+static int vc4_full_res_bounds_check(struct vc4_exec_info *exec,
+				     struct drm_gem_cma_object *obj,
+				     struct drm_vc4_submit_rcl_surface *surf)
+{
+	struct drm_vc4_submit_cl *args = exec->args;
+	u32 render_tiles_stride = DIV_ROUND_UP(exec->args->width, 32);
+
+	if (surf->offset > obj->base.size) {
+		DRM_ERROR("surface offset %d > BO size %zd\n",
+			  surf->offset, obj->base.size);
+		return -EINVAL;
+	}
+
+	if ((obj->base.size - surf->offset) / VC4_TILE_BUFFER_SIZE <
+	    render_tiles_stride * args->max_y_tile + args->max_x_tile) {
+		DRM_ERROR("MSAA tile %d, %d out of bounds "
+			  "(bo size %zd, offset %d).\n",
+			  args->max_x_tile, args->max_y_tile,
+			  obj->base.size,
+			  surf->offset);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int vc4_rcl_msaa_surface_setup(struct vc4_exec_info *exec,
+				      struct drm_gem_cma_object **obj,
+				      struct drm_vc4_submit_rcl_surface *surf)
+{
+	if (surf->flags != 0 || surf->bits != 0) {
+		DRM_ERROR("MSAA surface had nonzero flags/bits\n");
+		return -EINVAL;
+	}
+
+	if (surf->hindex == ~0)
+		return 0;
+
+	*obj = vc4_use_bo(exec, surf->hindex);
+	if (!*obj)
+		return -EINVAL;
+
+	if (surf->offset & 0xf) {
+		DRM_ERROR("MSAA write must be 16b aligned.\n");
+		return -EINVAL;
+	}
+
+	return vc4_full_res_bounds_check(exec, *obj, surf);
+}
+
+static int vc4_rcl_surface_setup(struct vc4_exec_info *exec,
+				 struct drm_gem_cma_object **obj,
+				 struct drm_vc4_submit_rcl_surface *surf)
+{
+	uint8_t tiling = VC4_GET_FIELD(surf->bits,
+				       VC4_LOADSTORE_TILE_BUFFER_TILING);
+	uint8_t buffer = VC4_GET_FIELD(surf->bits,
+				       VC4_LOADSTORE_TILE_BUFFER_BUFFER);
+	uint8_t format = VC4_GET_FIELD(surf->bits,
+				       VC4_LOADSTORE_TILE_BUFFER_FORMAT);
+	int cpp;
+	int ret;
+
+	if (surf->flags & ~VC4_SUBMIT_RCL_SURFACE_READ_IS_FULL_RES) {
+		DRM_ERROR("Extra flags set\n");
+		return -EINVAL;
+	}
+
+	if (surf->hindex == ~0)
+		return 0;
+
+	*obj = vc4_use_bo(exec, surf->hindex);
+	if (!*obj)
+		return -EINVAL;
+
+	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");
+			return -EINVAL;
+		}
+
+		if (surf->bits != 0) {
+			DRM_ERROR("load/store general bits set with "
+				  "full res load/store.\n");
+			return -EINVAL;
+		}
+
+		ret = vc4_full_res_bounds_check(exec, *obj, surf);
+		if (!ret)
+			return ret;
+
+		return 0;
+	}
+
+	if (surf->bits & ~(VC4_LOADSTORE_TILE_BUFFER_TILING_MASK |
+			   VC4_LOADSTORE_TILE_BUFFER_BUFFER_MASK |
+			   VC4_LOADSTORE_TILE_BUFFER_FORMAT_MASK)) {
+		DRM_ERROR("Unknown bits in load/store: 0x%04x\n",
+			  surf->bits);
+		return -EINVAL;
+	}
+
+	if (tiling > VC4_TILING_FORMAT_LT) {
+		DRM_ERROR("Bad tiling format\n");
+		return -EINVAL;
+	}
+
+	if (buffer == VC4_LOADSTORE_TILE_BUFFER_ZS) {
+		if (format != 0) {
+			DRM_ERROR("No color format should be set for ZS\n");
+			return -EINVAL;
+		}
+		cpp = 4;
+	} else if (buffer == VC4_LOADSTORE_TILE_BUFFER_COLOR) {
+		switch (format) {
+		case VC4_LOADSTORE_TILE_BUFFER_BGR565:
+		case VC4_LOADSTORE_TILE_BUFFER_BGR565_DITHER:
+			cpp = 2;
+			break;
+		case VC4_LOADSTORE_TILE_BUFFER_RGBA8888:
+			cpp = 4;
+			break;
+		default:
+			DRM_ERROR("Bad tile buffer format\n");
+			return -EINVAL;
+		}
+	} else {
+		DRM_ERROR("Bad load/store buffer %d.\n", buffer);
+		return -EINVAL;
+	}
+
+	if (surf->offset & 0xf) {
+		DRM_ERROR("load/store buffer must be 16b aligned.\n");
+		return -EINVAL;
+	}
+
+	if (!vc4_check_tex_size(exec, *obj, surf->offset, tiling,
+				exec->args->width, exec->args->height, cpp)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+vc4_rcl_render_config_surface_setup(struct vc4_exec_info *exec,
+				    struct vc4_rcl_setup *setup,
+				    struct drm_gem_cma_object **obj,
+				    struct drm_vc4_submit_rcl_surface *surf)
+{
+	uint8_t tiling = VC4_GET_FIELD(surf->bits,
+				       VC4_RENDER_CONFIG_MEMORY_FORMAT);
+	uint8_t format = VC4_GET_FIELD(surf->bits,
+				       VC4_RENDER_CONFIG_FORMAT);
+	int cpp;
+
+	if (surf->flags != 0) {
+		DRM_ERROR("No flags supported on render config.\n");
+		return -EINVAL;
+	}
+
+	if (surf->bits & ~(VC4_RENDER_CONFIG_MEMORY_FORMAT_MASK |
+			   VC4_RENDER_CONFIG_FORMAT_MASK |
+			   VC4_RENDER_CONFIG_MS_MODE_4X |
+			   VC4_RENDER_CONFIG_DECIMATE_MODE_4X)) {
+		DRM_ERROR("Unknown bits in render config: 0x%04x\n",
+			  surf->bits);
+		return -EINVAL;
+	}
+
+	if (surf->hindex == ~0)
+		return 0;
+
+	*obj = vc4_use_bo(exec, surf->hindex);
+	if (!*obj)
+		return -EINVAL;
+
+	if (tiling > VC4_TILING_FORMAT_LT) {
+		DRM_ERROR("Bad tiling format\n");
+		return -EINVAL;
+	}
+
+	switch (format) {
+	case VC4_RENDER_CONFIG_FORMAT_BGR565_DITHERED:
+	case VC4_RENDER_CONFIG_FORMAT_BGR565:
+		cpp = 2;
+		break;
+	case VC4_RENDER_CONFIG_FORMAT_RGBA8888:
+		cpp = 4;
+		break;
+	default:
+		DRM_ERROR("Bad tile buffer format\n");
+		return -EINVAL;
+	}
+
+	if (!vc4_check_tex_size(exec, *obj, surf->offset, tiling,
+				exec->args->width, exec->args->height, cpp)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vc4_get_rcl(struct drm_device *dev, struct vc4_exec_info *exec)
+{
+	struct vc4_rcl_setup setup = {0};
+	struct drm_vc4_submit_cl *args = exec->args;
+	bool has_bin = args->bin_cl_size != 0;
+	int ret;
+
+	if (args->min_x_tile > args->max_x_tile ||
+	    args->min_y_tile > args->max_y_tile) {
+		DRM_ERROR("Bad render tile set (%d,%d)-(%d,%d)\n",
+			  args->min_x_tile, args->min_y_tile,
+			  args->max_x_tile, args->max_y_tile);
+		return -EINVAL;
+	}
+
+	if (has_bin &&
+	    (args->max_x_tile > exec->bin_tiles_x ||
+	     args->max_y_tile > exec->bin_tiles_y)) {
+		DRM_ERROR("Render tiles (%d,%d) outside of bin config "
+			  "(%d,%d)\n",
+			  args->max_x_tile, args->max_y_tile,
+			  exec->bin_tiles_x, exec->bin_tiles_y);
+		return -EINVAL;
+	}
+
+	ret = vc4_rcl_render_config_surface_setup(exec, &setup,
+						  &setup.color_write,
+						  &args->color_write);
+	if (ret)
+		return ret;
+
+	ret = vc4_rcl_surface_setup(exec, &setup.color_read, &args->color_read);
+	if (ret)
+		return ret;
+
+	ret = vc4_rcl_surface_setup(exec, &setup.zs_read, &args->zs_read);
+	if (ret)
+		return ret;
+
+	ret = vc4_rcl_surface_setup(exec, &setup.zs_write, &args->zs_write);
+	if (ret)
+		return ret;
+
+	ret = vc4_rcl_msaa_surface_setup(exec, &setup.msaa_color_write,
+					 &args->msaa_color_write);
+	if (ret)
+		return ret;
+
+	ret = vc4_rcl_msaa_surface_setup(exec, &setup.msaa_zs_write,
+					 &args->msaa_zs_write);
+	if (ret)
+		return ret;
+
+	/* We shouldn't even have the job submitted to us if there's no
+	 * surface to write out.
+	 */
+	if (!setup.color_write && !setup.zs_write &&
+	    !setup.msaa_color_write && !setup.msaa_zs_write) {
+		DRM_ERROR("RCL requires color or Z/S write\n");
+		return -EINVAL;
+	}
+
+	return vc4_create_rcl_bo(dev, exec, &setup);
+}
diff --git a/drivers/gpu/drm/vc4/vc4_trace.h b/drivers/gpu/drm/vc4/vc4_trace.h
new file mode 100644
index 000000000000..ad7b1ea720c2
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_trace.h
@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2015 Broadcom
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#if !defined(_VC4_TRACE_H_) || defined(TRACE_HEADER_MULTI_READ)
+#define _VC4_TRACE_H_
+
+#include <linux/stringify.h>
+#include <linux/types.h>
+#include <linux/tracepoint.h>
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM vc4
+#define TRACE_INCLUDE_FILE vc4_trace
+
+TRACE_EVENT(vc4_wait_for_seqno_begin,
+	    TP_PROTO(struct drm_device *dev, uint64_t seqno, uint64_t timeout),
+	    TP_ARGS(dev, seqno, timeout),
+
+	    TP_STRUCT__entry(
+			     __field(u32, dev)
+			     __field(u64, seqno)
+			     __field(u64, timeout)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->dev = dev->primary->index;
+			   __entry->seqno = seqno;
+			   __entry->timeout = timeout;
+			   ),
+
+	    TP_printk("dev=%u, seqno=%llu, timeout=%llu",
+		      __entry->dev, __entry->seqno, __entry->timeout)
+);
+
+TRACE_EVENT(vc4_wait_for_seqno_end,
+	    TP_PROTO(struct drm_device *dev, uint64_t seqno),
+	    TP_ARGS(dev, seqno),
+
+	    TP_STRUCT__entry(
+			     __field(u32, dev)
+			     __field(u64, seqno)
+			     ),
+
+	    TP_fast_assign(
+			   __entry->dev = dev->primary->index;
+			   __entry->seqno = seqno;
+			   ),
+
+	    TP_printk("dev=%u, seqno=%llu",
+		      __entry->dev, __entry->seqno)
+);
+
+#endif /* _VC4_TRACE_H_ */
+
+/* This part must be outside protection */
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#include <trace/define_trace.h>
diff --git a/drivers/gpu/drm/vc4/vc4_trace_points.c b/drivers/gpu/drm/vc4/vc4_trace_points.c
new file mode 100644
index 000000000000..e6278f25716b
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_trace_points.c
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2015 Broadcom
+ *
+ * This program is free software; you can 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 "vc4_drv.h"
+
+#ifndef __CHECKER__
+#define CREATE_TRACE_POINTS
+#include "vc4_trace.h"
+#endif
diff --git a/drivers/gpu/drm/vc4/vc4_v3d.c b/drivers/gpu/drm/vc4/vc4_v3d.c
new file mode 100644
index 000000000000..31de5d17bc85
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_v3d.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2014 The Linux Foundation. All rights reserved.
+ * Copyright (C) 2013 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "linux/component.h"
+#include "linux/pm_runtime.h"
+#include "vc4_drv.h"
+#include "vc4_regs.h"
+
+#ifdef CONFIG_DEBUG_FS
+#define REGDEF(reg) { reg, #reg }
+static const struct {
+	uint32_t reg;
+	const char *name;
+} vc4_reg_defs[] = {
+	REGDEF(V3D_IDENT0),
+	REGDEF(V3D_IDENT1),
+	REGDEF(V3D_IDENT2),
+	REGDEF(V3D_SCRATCH),
+	REGDEF(V3D_L2CACTL),
+	REGDEF(V3D_SLCACTL),
+	REGDEF(V3D_INTCTL),
+	REGDEF(V3D_INTENA),
+	REGDEF(V3D_INTDIS),
+	REGDEF(V3D_CT0CS),
+	REGDEF(V3D_CT1CS),
+	REGDEF(V3D_CT0EA),
+	REGDEF(V3D_CT1EA),
+	REGDEF(V3D_CT0CA),
+	REGDEF(V3D_CT1CA),
+	REGDEF(V3D_CT00RA0),
+	REGDEF(V3D_CT01RA0),
+	REGDEF(V3D_CT0LC),
+	REGDEF(V3D_CT1LC),
+	REGDEF(V3D_CT0PC),
+	REGDEF(V3D_CT1PC),
+	REGDEF(V3D_PCS),
+	REGDEF(V3D_BFC),
+	REGDEF(V3D_RFC),
+	REGDEF(V3D_BPCA),
+	REGDEF(V3D_BPCS),
+	REGDEF(V3D_BPOA),
+	REGDEF(V3D_BPOS),
+	REGDEF(V3D_BXCF),
+	REGDEF(V3D_SQRSV0),
+	REGDEF(V3D_SQRSV1),
+	REGDEF(V3D_SQCNTL),
+	REGDEF(V3D_SRQPC),
+	REGDEF(V3D_SRQUA),
+	REGDEF(V3D_SRQUL),
+	REGDEF(V3D_SRQCS),
+	REGDEF(V3D_VPACNTL),
+	REGDEF(V3D_VPMBASE),
+	REGDEF(V3D_PCTRC),
+	REGDEF(V3D_PCTRE),
+	REGDEF(V3D_PCTR0),
+	REGDEF(V3D_PCTRS0),
+	REGDEF(V3D_PCTR1),
+	REGDEF(V3D_PCTRS1),
+	REGDEF(V3D_PCTR2),
+	REGDEF(V3D_PCTRS2),
+	REGDEF(V3D_PCTR3),
+	REGDEF(V3D_PCTRS3),
+	REGDEF(V3D_PCTR4),
+	REGDEF(V3D_PCTRS4),
+	REGDEF(V3D_PCTR5),
+	REGDEF(V3D_PCTRS5),
+	REGDEF(V3D_PCTR6),
+	REGDEF(V3D_PCTRS6),
+	REGDEF(V3D_PCTR7),
+	REGDEF(V3D_PCTRS7),
+	REGDEF(V3D_PCTR8),
+	REGDEF(V3D_PCTRS8),
+	REGDEF(V3D_PCTR9),
+	REGDEF(V3D_PCTRS9),
+	REGDEF(V3D_PCTR10),
+	REGDEF(V3D_PCTRS10),
+	REGDEF(V3D_PCTR11),
+	REGDEF(V3D_PCTRS11),
+	REGDEF(V3D_PCTR12),
+	REGDEF(V3D_PCTRS12),
+	REGDEF(V3D_PCTR13),
+	REGDEF(V3D_PCTRS13),
+	REGDEF(V3D_PCTR14),
+	REGDEF(V3D_PCTRS14),
+	REGDEF(V3D_PCTR15),
+	REGDEF(V3D_PCTRS15),
+	REGDEF(V3D_DBGE),
+	REGDEF(V3D_FDBGO),
+	REGDEF(V3D_FDBGB),
+	REGDEF(V3D_FDBGR),
+	REGDEF(V3D_FDBGS),
+	REGDEF(V3D_ERRSTAT),
+};
+
+int vc4_v3d_debugfs_regs(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(vc4_reg_defs); i++) {
+		seq_printf(m, "%s (0x%04x): 0x%08x\n",
+			   vc4_reg_defs[i].name, vc4_reg_defs[i].reg,
+			   V3D_READ(vc4_reg_defs[i].reg));
+	}
+
+	return 0;
+}
+
+int vc4_v3d_debugfs_ident(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+	uint32_t ident1 = V3D_READ(V3D_IDENT1);
+	uint32_t nslc = VC4_GET_FIELD(ident1, V3D_IDENT1_NSLC);
+	uint32_t tups = VC4_GET_FIELD(ident1, V3D_IDENT1_TUPS);
+	uint32_t qups = VC4_GET_FIELD(ident1, V3D_IDENT1_QUPS);
+
+	seq_printf(m, "Revision:   %d\n",
+		   VC4_GET_FIELD(ident1, V3D_IDENT1_REV));
+	seq_printf(m, "Slices:     %d\n", nslc);
+	seq_printf(m, "TMUs:       %d\n", nslc * tups);
+	seq_printf(m, "QPUs:       %d\n", nslc * qups);
+	seq_printf(m, "Semaphores: %d\n",
+		   VC4_GET_FIELD(ident1, V3D_IDENT1_NSEM));
+
+	return 0;
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static void vc4_v3d_init_hw(struct drm_device *dev)
+{
+	struct vc4_dev *vc4 = to_vc4_dev(dev);
+
+	/* Take all the memory that would have been reserved for user
+	 * QPU programs, since we don't have an interface for running
+	 * them, anyway.
+	 */
+	V3D_WRITE(V3D_VPMBASE, 0);
+}
+
+#ifdef CONFIG_PM
+static int vc4_v3d_runtime_suspend(struct device *dev)
+{
+	struct vc4_v3d *v3d = dev_get_drvdata(dev);
+	struct vc4_dev *vc4 = v3d->vc4;
+
+	vc4_irq_uninstall(vc4->dev);
+
+	return 0;
+}
+
+static int vc4_v3d_runtime_resume(struct device *dev)
+{
+	struct vc4_v3d *v3d = dev_get_drvdata(dev);
+	struct vc4_dev *vc4 = v3d->vc4;
+
+	vc4_v3d_init_hw(vc4->dev);
+	vc4_irq_postinstall(vc4->dev);
+
+	return 0;
+}
+#endif
+
+static int vc4_v3d_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct drm_device *drm = dev_get_drvdata(master);
+	struct vc4_dev *vc4 = to_vc4_dev(drm);
+	struct vc4_v3d *v3d = NULL;
+	int ret;
+
+	v3d = devm_kzalloc(&pdev->dev, sizeof(*v3d), GFP_KERNEL);
+	if (!v3d)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, v3d);
+
+	v3d->pdev = pdev;
+
+	v3d->regs = vc4_ioremap_regs(pdev, 0);
+	if (IS_ERR(v3d->regs))
+		return PTR_ERR(v3d->regs);
+
+	vc4->v3d = v3d;
+	v3d->vc4 = vc4;
+
+	if (V3D_READ(V3D_IDENT0) != V3D_EXPECTED_IDENT0) {
+		DRM_ERROR("V3D_IDENT0 read 0x%08x instead of 0x%08x\n",
+			  V3D_READ(V3D_IDENT0), V3D_EXPECTED_IDENT0);
+		return -EINVAL;
+	}
+
+	/* Reset the binner overflow address/size at setup, to be sure
+	 * we don't reuse an old one.
+	 */
+	V3D_WRITE(V3D_BPOA, 0);
+	V3D_WRITE(V3D_BPOS, 0);
+
+	vc4_v3d_init_hw(drm);
+
+	ret = drm_irq_install(drm, platform_get_irq(pdev, 0));
+	if (ret) {
+		DRM_ERROR("Failed to install IRQ handler\n");
+		return ret;
+	}
+
+	pm_runtime_enable(dev);
+
+	return 0;
+}
+
+static void vc4_v3d_unbind(struct device *dev, struct device *master,
+			   void *data)
+{
+	struct drm_device *drm = dev_get_drvdata(master);
+	struct vc4_dev *vc4 = to_vc4_dev(drm);
+
+	pm_runtime_disable(dev);
+
+	drm_irq_uninstall(drm);
+
+	/* Disable the binner's overflow memory address, so the next
+	 * driver probe (if any) doesn't try to reuse our old
+	 * allocation.
+	 */
+	V3D_WRITE(V3D_BPOA, 0);
+	V3D_WRITE(V3D_BPOS, 0);
+
+	vc4->v3d = NULL;
+}
+
+static const struct dev_pm_ops vc4_v3d_pm_ops = {
+	SET_RUNTIME_PM_OPS(vc4_v3d_runtime_suspend, vc4_v3d_runtime_resume, NULL)
+};
+
+static const struct component_ops vc4_v3d_ops = {
+	.bind   = vc4_v3d_bind,
+	.unbind = vc4_v3d_unbind,
+};
+
+static int vc4_v3d_dev_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &vc4_v3d_ops);
+}
+
+static int vc4_v3d_dev_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &vc4_v3d_ops);
+	return 0;
+}
+
+static const struct of_device_id vc4_v3d_dt_match[] = {
+	{ .compatible = "brcm,vc4-v3d" },
+	{}
+};
+
+struct platform_driver vc4_v3d_driver = {
+	.probe = vc4_v3d_dev_probe,
+	.remove = vc4_v3d_dev_remove,
+	.driver = {
+		.name = "vc4_v3d",
+		.of_match_table = vc4_v3d_dt_match,
+		.pm = &vc4_v3d_pm_ops,
+	},
+};
diff --git a/drivers/gpu/drm/vc4/vc4_validate.c b/drivers/gpu/drm/vc4/vc4_validate.c
new file mode 100644
index 000000000000..24c2c746e8f3
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_validate.c
@@ -0,0 +1,900 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * 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.
+ */
+
+/**
+ * Command list validator for VC4.
+ *
+ * The VC4 has no IOMMU between it and system memory.  So, a user with
+ * access to execute command lists could escalate privilege by
+ * overwriting system memory (drawing to it as a framebuffer) or
+ * reading system memory it shouldn't (reading it as a texture, or
+ * uniform data, or vertex data).
+ *
+ * This validates command lists to ensure that all accesses are within
+ * the bounds of the GEM objects referenced.  It explicitly whitelists
+ * packets, and looks at the offsets in any address fields to make
+ * sure they're constrained within the BOs they reference.
+ *
+ * Note that because of the validation that's happening anyway, this
+ * is where GEM relocation processing happens.
+ */
+
+#include "uapi/drm/vc4_drm.h"
+#include "vc4_drv.h"
+#include "vc4_packet.h"
+
+#define VALIDATE_ARGS \
+	struct vc4_exec_info *exec,			\
+	void *validated,				\
+	void *untrusted
+
+/** Return the width in pixels of a 64-byte microtile. */
+static uint32_t
+utile_width(int cpp)
+{
+	switch (cpp) {
+	case 1:
+	case 2:
+		return 8;
+	case 4:
+		return 4;
+	case 8:
+		return 2;
+	default:
+		DRM_ERROR("unknown cpp: %d\n", cpp);
+		return 1;
+	}
+}
+
+/** Return the height in pixels of a 64-byte microtile. */
+static uint32_t
+utile_height(int cpp)
+{
+	switch (cpp) {
+	case 1:
+		return 8;
+	case 2:
+	case 4:
+	case 8:
+		return 4;
+	default:
+		DRM_ERROR("unknown cpp: %d\n", cpp);
+		return 1;
+	}
+}
+
+/**
+ * The texture unit decides what tiling format a particular miplevel is using
+ * this function, so we lay out our miptrees accordingly.
+ */
+static bool
+size_is_lt(uint32_t width, uint32_t height, int cpp)
+{
+	return (width <= 4 * utile_width(cpp) ||
+		height <= 4 * utile_height(cpp));
+}
+
+struct drm_gem_cma_object *
+vc4_use_bo(struct vc4_exec_info *exec, uint32_t hindex)
+{
+	struct drm_gem_cma_object *obj;
+	struct vc4_bo *bo;
+
+	if (hindex >= exec->bo_count) {
+		DRM_ERROR("BO index %d greater than BO count %d\n",
+			  hindex, exec->bo_count);
+		return NULL;
+	}
+	obj = exec->bo[hindex];
+	bo = to_vc4_bo(&obj->base);
+
+	if (bo->validated_shader) {
+		DRM_ERROR("Trying to use shader BO as something other than "
+			  "a shader\n");
+		return NULL;
+	}
+
+	return obj;
+}
+
+static struct drm_gem_cma_object *
+vc4_use_handle(struct vc4_exec_info *exec, uint32_t gem_handles_packet_index)
+{
+	return vc4_use_bo(exec, exec->bo_index[gem_handles_packet_index]);
+}
+
+static bool
+validate_bin_pos(struct vc4_exec_info *exec, void *untrusted, uint32_t pos)
+{
+	/* Note that the untrusted pointer passed to these functions is
+	 * incremented past the packet byte.
+	 */
+	return (untrusted - 1 == exec->bin_u + pos);
+}
+
+static uint32_t
+gl_shader_rec_size(uint32_t pointer_bits)
+{
+	uint32_t attribute_count = pointer_bits & 7;
+	bool extended = pointer_bits & 8;
+
+	if (attribute_count == 0)
+		attribute_count = 8;
+
+	if (extended)
+		return 100 + attribute_count * 4;
+	else
+		return 36 + attribute_count * 8;
+}
+
+bool
+vc4_check_tex_size(struct vc4_exec_info *exec, struct drm_gem_cma_object *fbo,
+		   uint32_t offset, uint8_t tiling_format,
+		   uint32_t width, uint32_t height, uint8_t cpp)
+{
+	uint32_t aligned_width, aligned_height, stride, size;
+	uint32_t utile_w = utile_width(cpp);
+	uint32_t utile_h = utile_height(cpp);
+
+	/* The shaded vertex format stores signed 12.4 fixed point
+	 * (-2048,2047) offsets from the viewport center, so we should
+	 * never have a render target larger than 4096.  The texture
+	 * unit can only sample from 2048x2048, so it's even more
+	 * restricted.  This lets us avoid worrying about overflow in
+	 * our math.
+	 */
+	if (width > 4096 || height > 4096) {
+		DRM_ERROR("Surface dimesions (%d,%d) too large", width, height);
+		return false;
+	}
+
+	switch (tiling_format) {
+	case VC4_TILING_FORMAT_LINEAR:
+		aligned_width = round_up(width, utile_w);
+		aligned_height = height;
+		break;
+	case VC4_TILING_FORMAT_T:
+		aligned_width = round_up(width, utile_w * 8);
+		aligned_height = round_up(height, utile_h * 8);
+		break;
+	case VC4_TILING_FORMAT_LT:
+		aligned_width = round_up(width, utile_w);
+		aligned_height = round_up(height, utile_h);
+		break;
+	default:
+		DRM_ERROR("buffer tiling %d unsupported\n", tiling_format);
+		return false;
+	}
+
+	stride = aligned_width * cpp;
+	size = stride * aligned_height;
+
+	if (size + offset < size ||
+	    size + offset > fbo->base.size) {
+		DRM_ERROR("Overflow in %dx%d (%dx%d) fbo size (%d + %d > %zd)\n",
+			  width, height,
+			  aligned_width, aligned_height,
+			  size, offset, fbo->base.size);
+		return false;
+	}
+
+	return true;
+}
+
+static int
+validate_flush(VALIDATE_ARGS)
+{
+	if (!validate_bin_pos(exec, untrusted, exec->args->bin_cl_size - 1)) {
+		DRM_ERROR("Bin CL must end with VC4_PACKET_FLUSH\n");
+		return -EINVAL;
+	}
+	exec->found_flush = true;
+
+	return 0;
+}
+
+static int
+validate_start_tile_binning(VALIDATE_ARGS)
+{
+	if (exec->found_start_tile_binning_packet) {
+		DRM_ERROR("Duplicate VC4_PACKET_START_TILE_BINNING\n");
+		return -EINVAL;
+	}
+	exec->found_start_tile_binning_packet = true;
+
+	if (!exec->found_tile_binning_mode_config_packet) {
+		DRM_ERROR("missing VC4_PACKET_TILE_BINNING_MODE_CONFIG\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int
+validate_increment_semaphore(VALIDATE_ARGS)
+{
+	if (!validate_bin_pos(exec, untrusted, exec->args->bin_cl_size - 2)) {
+		DRM_ERROR("Bin CL must end with "
+			  "VC4_PACKET_INCREMENT_SEMAPHORE\n");
+		return -EINVAL;
+	}
+	exec->found_increment_semaphore_packet = true;
+
+	return 0;
+}
+
+static int
+validate_indexed_prim_list(VALIDATE_ARGS)
+{
+	struct drm_gem_cma_object *ib;
+	uint32_t length = *(uint32_t *)(untrusted + 1);
+	uint32_t offset = *(uint32_t *)(untrusted + 5);
+	uint32_t max_index = *(uint32_t *)(untrusted + 9);
+	uint32_t index_size = (*(uint8_t *)(untrusted + 0) >> 4) ? 2 : 1;
+	struct vc4_shader_state *shader_state;
+
+	/* Check overflow condition */
+	if (exec->shader_state_count == 0) {
+		DRM_ERROR("shader state must precede primitives\n");
+		return -EINVAL;
+	}
+	shader_state = &exec->shader_state[exec->shader_state_count - 1];
+
+	if (max_index > shader_state->max_index)
+		shader_state->max_index = max_index;
+
+	ib = vc4_use_handle(exec, 0);
+	if (!ib)
+		return -EINVAL;
+
+	if (offset > ib->base.size ||
+	    (ib->base.size - offset) / index_size < length) {
+		DRM_ERROR("IB access overflow (%d + %d*%d > %zd)\n",
+			  offset, length, index_size, ib->base.size);
+		return -EINVAL;
+	}
+
+	*(uint32_t *)(validated + 5) = ib->paddr + offset;
+
+	return 0;
+}
+
+static int
+validate_gl_array_primitive(VALIDATE_ARGS)
+{
+	uint32_t length = *(uint32_t *)(untrusted + 1);
+	uint32_t base_index = *(uint32_t *)(untrusted + 5);
+	uint32_t max_index;
+	struct vc4_shader_state *shader_state;
+
+	/* Check overflow condition */
+	if (exec->shader_state_count == 0) {
+		DRM_ERROR("shader state must precede primitives\n");
+		return -EINVAL;
+	}
+	shader_state = &exec->shader_state[exec->shader_state_count - 1];
+
+	if (length + base_index < length) {
+		DRM_ERROR("primitive vertex count overflow\n");
+		return -EINVAL;
+	}
+	max_index = length + base_index - 1;
+
+	if (max_index > shader_state->max_index)
+		shader_state->max_index = max_index;
+
+	return 0;
+}
+
+static int
+validate_gl_shader_state(VALIDATE_ARGS)
+{
+	uint32_t i = exec->shader_state_count++;
+
+	if (i >= exec->shader_state_size) {
+		DRM_ERROR("More requests for shader states than declared\n");
+		return -EINVAL;
+	}
+
+	exec->shader_state[i].addr = *(uint32_t *)untrusted;
+	exec->shader_state[i].max_index = 0;
+
+	if (exec->shader_state[i].addr & ~0xf) {
+		DRM_ERROR("high bits set in GL shader rec reference\n");
+		return -EINVAL;
+	}
+
+	*(uint32_t *)validated = (exec->shader_rec_p +
+				  exec->shader_state[i].addr);
+
+	exec->shader_rec_p +=
+		roundup(gl_shader_rec_size(exec->shader_state[i].addr), 16);
+
+	return 0;
+}
+
+static int
+validate_tile_binning_config(VALIDATE_ARGS)
+{
+	struct drm_device *dev = exec->exec_bo->base.dev;
+	struct vc4_bo *tile_bo;
+	uint8_t flags;
+	uint32_t tile_state_size, tile_alloc_size;
+	uint32_t tile_count;
+
+	if (exec->found_tile_binning_mode_config_packet) {
+		DRM_ERROR("Duplicate VC4_PACKET_TILE_BINNING_MODE_CONFIG\n");
+		return -EINVAL;
+	}
+	exec->found_tile_binning_mode_config_packet = true;
+
+	exec->bin_tiles_x = *(uint8_t *)(untrusted + 12);
+	exec->bin_tiles_y = *(uint8_t *)(untrusted + 13);
+	tile_count = exec->bin_tiles_x * exec->bin_tiles_y;
+	flags = *(uint8_t *)(untrusted + 14);
+
+	if (exec->bin_tiles_x == 0 ||
+	    exec->bin_tiles_y == 0) {
+		DRM_ERROR("Tile binning config of %dx%d too small\n",
+			  exec->bin_tiles_x, exec->bin_tiles_y);
+		return -EINVAL;
+	}
+
+	if (flags & (VC4_BIN_CONFIG_DB_NON_MS |
+		     VC4_BIN_CONFIG_TILE_BUFFER_64BIT)) {
+		DRM_ERROR("unsupported binning config flags 0x%02x\n", flags);
+		return -EINVAL;
+	}
+
+	/* The tile state data array is 48 bytes per tile, and we put it at
+	 * the start of a BO containing both it and the tile alloc.
+	 */
+	tile_state_size = 48 * tile_count;
+
+	/* Since the tile alloc array will follow us, align. */
+	exec->tile_alloc_offset = roundup(tile_state_size, 4096);
+
+	*(uint8_t *)(validated + 14) =
+		((flags & ~(VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_MASK |
+			    VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_MASK)) |
+		 VC4_BIN_CONFIG_AUTO_INIT_TSDA |
+		 VC4_SET_FIELD(VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE_32,
+			       VC4_BIN_CONFIG_ALLOC_INIT_BLOCK_SIZE) |
+		 VC4_SET_FIELD(VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE_128,
+			       VC4_BIN_CONFIG_ALLOC_BLOCK_SIZE));
+
+	/* Initial block size. */
+	tile_alloc_size = 32 * tile_count;
+
+	/*
+	 * The initial allocation gets rounded to the next 256 bytes before
+	 * the hardware starts fulfilling further allocations.
+	 */
+	tile_alloc_size = roundup(tile_alloc_size, 256);
+
+	/* Add space for the extra allocations.  This is what gets used first,
+	 * before overflow memory.  It must have at least 4096 bytes, but we
+	 * want to avoid overflow memory usage if possible.
+	 */
+	tile_alloc_size += 1024 * 1024;
+
+	tile_bo = vc4_bo_create(dev, exec->tile_alloc_offset + tile_alloc_size,
+				true);
+	exec->tile_bo = &tile_bo->base;
+	if (IS_ERR(exec->tile_bo))
+		return PTR_ERR(exec->tile_bo);
+	list_add_tail(&tile_bo->unref_head, &exec->unref_list);
+
+	/* tile alloc address. */
+	*(uint32_t *)(validated + 0) = (exec->tile_bo->paddr +
+					exec->tile_alloc_offset);
+	/* tile alloc size. */
+	*(uint32_t *)(validated + 4) = tile_alloc_size;
+	/* tile state address. */
+	*(uint32_t *)(validated + 8) = exec->tile_bo->paddr;
+
+	return 0;
+}
+
+static int
+validate_gem_handles(VALIDATE_ARGS)
+{
+	memcpy(exec->bo_index, untrusted, sizeof(exec->bo_index));
+	return 0;
+}
+
+#define VC4_DEFINE_PACKET(packet, func) \
+	[packet] = { packet ## _SIZE, #packet, func }
+
+static const struct cmd_info {
+	uint16_t len;
+	const char *name;
+	int (*func)(struct vc4_exec_info *exec, void *validated,
+		    void *untrusted);
+} cmd_info[] = {
+	VC4_DEFINE_PACKET(VC4_PACKET_HALT, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_NOP, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_FLUSH, validate_flush),
+	VC4_DEFINE_PACKET(VC4_PACKET_FLUSH_ALL, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_START_TILE_BINNING,
+			  validate_start_tile_binning),
+	VC4_DEFINE_PACKET(VC4_PACKET_INCREMENT_SEMAPHORE,
+			  validate_increment_semaphore),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_GL_INDEXED_PRIMITIVE,
+			  validate_indexed_prim_list),
+	VC4_DEFINE_PACKET(VC4_PACKET_GL_ARRAY_PRIMITIVE,
+			  validate_gl_array_primitive),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_PRIMITIVE_LIST_FORMAT, NULL),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_GL_SHADER_STATE, validate_gl_shader_state),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_CONFIGURATION_BITS, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_FLAT_SHADE_FLAGS, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_POINT_SIZE, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_LINE_WIDTH, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_RHT_X_BOUNDARY, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_DEPTH_OFFSET, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_CLIP_WINDOW, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_VIEWPORT_OFFSET, NULL),
+	VC4_DEFINE_PACKET(VC4_PACKET_CLIPPER_XY_SCALING, NULL),
+	/* Note: The docs say this was also 105, but it was 106 in the
+	 * initial userland code drop.
+	 */
+	VC4_DEFINE_PACKET(VC4_PACKET_CLIPPER_Z_SCALING, NULL),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_TILE_BINNING_MODE_CONFIG,
+			  validate_tile_binning_config),
+
+	VC4_DEFINE_PACKET(VC4_PACKET_GEM_HANDLES, validate_gem_handles),
+};
+
+int
+vc4_validate_bin_cl(struct drm_device *dev,
+		    void *validated,
+		    void *unvalidated,
+		    struct vc4_exec_info *exec)
+{
+	uint32_t len = exec->args->bin_cl_size;
+	uint32_t dst_offset = 0;
+	uint32_t src_offset = 0;
+
+	while (src_offset < len) {
+		void *dst_pkt = validated + dst_offset;
+		void *src_pkt = unvalidated + src_offset;
+		u8 cmd = *(uint8_t *)src_pkt;
+		const struct cmd_info *info;
+
+		if (cmd >= ARRAY_SIZE(cmd_info)) {
+			DRM_ERROR("0x%08x: packet %d out of bounds\n",
+				  src_offset, cmd);
+			return -EINVAL;
+		}
+
+		info = &cmd_info[cmd];
+		if (!info->name) {
+			DRM_ERROR("0x%08x: packet %d invalid\n",
+				  src_offset, cmd);
+			return -EINVAL;
+		}
+
+		if (src_offset + info->len > len) {
+			DRM_ERROR("0x%08x: packet %d (%s) length 0x%08x "
+				  "exceeds bounds (0x%08x)\n",
+				  src_offset, cmd, info->name, info->len,
+				  src_offset + len);
+			return -EINVAL;
+		}
+
+		if (cmd != VC4_PACKET_GEM_HANDLES)
+			memcpy(dst_pkt, src_pkt, info->len);
+
+		if (info->func && info->func(exec,
+					     dst_pkt + 1,
+					     src_pkt + 1)) {
+			DRM_ERROR("0x%08x: packet %d (%s) failed to validate\n",
+				  src_offset, cmd, info->name);
+			return -EINVAL;
+		}
+
+		src_offset += info->len;
+		/* GEM handle loading doesn't produce HW packets. */
+		if (cmd != VC4_PACKET_GEM_HANDLES)
+			dst_offset += info->len;
+
+		/* When the CL hits halt, it'll stop reading anything else. */
+		if (cmd == VC4_PACKET_HALT)
+			break;
+	}
+
+	exec->ct0ea = exec->ct0ca + dst_offset;
+
+	if (!exec->found_start_tile_binning_packet) {
+		DRM_ERROR("Bin CL missing VC4_PACKET_START_TILE_BINNING\n");
+		return -EINVAL;
+	}
+
+	/* The bin CL must be ended with INCREMENT_SEMAPHORE and FLUSH.  The
+	 * semaphore is used to trigger the render CL to start up, and the
+	 * FLUSH is what caps the bin lists with
+	 * VC4_PACKET_RETURN_FROM_SUB_LIST (so they jump back to the main
+	 * render CL when they get called to) and actually triggers the queued
+	 * semaphore increment.
+	 */
+	if (!exec->found_increment_semaphore_packet || !exec->found_flush) {
+		DRM_ERROR("Bin CL missing VC4_PACKET_INCREMENT_SEMAPHORE + "
+			  "VC4_PACKET_FLUSH\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static bool
+reloc_tex(struct vc4_exec_info *exec,
+	  void *uniform_data_u,
+	  struct vc4_texture_sample_info *sample,
+	  uint32_t texture_handle_index)
+
+{
+	struct drm_gem_cma_object *tex;
+	uint32_t p0 = *(uint32_t *)(uniform_data_u + sample->p_offset[0]);
+	uint32_t p1 = *(uint32_t *)(uniform_data_u + sample->p_offset[1]);
+	uint32_t p2 = (sample->p_offset[2] != ~0 ?
+		       *(uint32_t *)(uniform_data_u + sample->p_offset[2]) : 0);
+	uint32_t p3 = (sample->p_offset[3] != ~0 ?
+		       *(uint32_t *)(uniform_data_u + sample->p_offset[3]) : 0);
+	uint32_t *validated_p0 = exec->uniforms_v + sample->p_offset[0];
+	uint32_t offset = p0 & VC4_TEX_P0_OFFSET_MASK;
+	uint32_t miplevels = VC4_GET_FIELD(p0, VC4_TEX_P0_MIPLVLS);
+	uint32_t width = VC4_GET_FIELD(p1, VC4_TEX_P1_WIDTH);
+	uint32_t height = VC4_GET_FIELD(p1, VC4_TEX_P1_HEIGHT);
+	uint32_t cpp, tiling_format, utile_w, utile_h;
+	uint32_t i;
+	uint32_t cube_map_stride = 0;
+	enum vc4_texture_data_type type;
+
+	tex = vc4_use_bo(exec, texture_handle_index);
+	if (!tex)
+		return false;
+
+	if (sample->is_direct) {
+		uint32_t remaining_size = tex->base.size - p0;
+
+		if (p0 > tex->base.size - 4) {
+			DRM_ERROR("UBO offset greater than UBO size\n");
+			goto fail;
+		}
+		if (p1 > remaining_size - 4) {
+			DRM_ERROR("UBO clamp would allow reads "
+				  "outside of UBO\n");
+			goto fail;
+		}
+		*validated_p0 = tex->paddr + p0;
+		return true;
+	}
+
+	if (width == 0)
+		width = 2048;
+	if (height == 0)
+		height = 2048;
+
+	if (p0 & VC4_TEX_P0_CMMODE_MASK) {
+		if (VC4_GET_FIELD(p2, VC4_TEX_P2_PTYPE) ==
+		    VC4_TEX_P2_PTYPE_CUBE_MAP_STRIDE)
+			cube_map_stride = p2 & VC4_TEX_P2_CMST_MASK;
+		if (VC4_GET_FIELD(p3, VC4_TEX_P2_PTYPE) ==
+		    VC4_TEX_P2_PTYPE_CUBE_MAP_STRIDE) {
+			if (cube_map_stride) {
+				DRM_ERROR("Cube map stride set twice\n");
+				goto fail;
+			}
+
+			cube_map_stride = p3 & VC4_TEX_P2_CMST_MASK;
+		}
+		if (!cube_map_stride) {
+			DRM_ERROR("Cube map stride not set\n");
+			goto fail;
+		}
+	}
+
+	type = (VC4_GET_FIELD(p0, VC4_TEX_P0_TYPE) |
+		(VC4_GET_FIELD(p1, VC4_TEX_P1_TYPE4) << 4));
+
+	switch (type) {
+	case VC4_TEXTURE_TYPE_RGBA8888:
+	case VC4_TEXTURE_TYPE_RGBX8888:
+	case VC4_TEXTURE_TYPE_RGBA32R:
+		cpp = 4;
+		break;
+	case VC4_TEXTURE_TYPE_RGBA4444:
+	case VC4_TEXTURE_TYPE_RGBA5551:
+	case VC4_TEXTURE_TYPE_RGB565:
+	case VC4_TEXTURE_TYPE_LUMALPHA:
+	case VC4_TEXTURE_TYPE_S16F:
+	case VC4_TEXTURE_TYPE_S16:
+		cpp = 2;
+		break;
+	case VC4_TEXTURE_TYPE_LUMINANCE:
+	case VC4_TEXTURE_TYPE_ALPHA:
+	case VC4_TEXTURE_TYPE_S8:
+		cpp = 1;
+		break;
+	case VC4_TEXTURE_TYPE_ETC1:
+	case VC4_TEXTURE_TYPE_BW1:
+	case VC4_TEXTURE_TYPE_A4:
+	case VC4_TEXTURE_TYPE_A1:
+	case VC4_TEXTURE_TYPE_RGBA64:
+	case VC4_TEXTURE_TYPE_YUV422R:
+	default:
+		DRM_ERROR("Texture format %d unsupported\n", type);
+		goto fail;
+	}
+	utile_w = utile_width(cpp);
+	utile_h = utile_height(cpp);
+
+	if (type == VC4_TEXTURE_TYPE_RGBA32R) {
+		tiling_format = VC4_TILING_FORMAT_LINEAR;
+	} else {
+		if (size_is_lt(width, height, cpp))
+			tiling_format = VC4_TILING_FORMAT_LT;
+		else
+			tiling_format = VC4_TILING_FORMAT_T;
+	}
+
+	if (!vc4_check_tex_size(exec, tex, offset + cube_map_stride * 5,
+				tiling_format, width, height, cpp)) {
+		goto fail;
+	}
+
+	/* The mipmap levels are stored before the base of the texture.  Make
+	 * sure there is actually space in the BO.
+	 */
+	for (i = 1; i <= miplevels; i++) {
+		uint32_t level_width = max(width >> i, 1u);
+		uint32_t level_height = max(height >> i, 1u);
+		uint32_t aligned_width, aligned_height;
+		uint32_t level_size;
+
+		/* Once the levels get small enough, they drop from T to LT. */
+		if (tiling_format == VC4_TILING_FORMAT_T &&
+		    size_is_lt(level_width, level_height, cpp)) {
+			tiling_format = VC4_TILING_FORMAT_LT;
+		}
+
+		switch (tiling_format) {
+		case VC4_TILING_FORMAT_T:
+			aligned_width = round_up(level_width, utile_w * 8);
+			aligned_height = round_up(level_height, utile_h * 8);
+			break;
+		case VC4_TILING_FORMAT_LT:
+			aligned_width = round_up(level_width, utile_w);
+			aligned_height = round_up(level_height, utile_h);
+			break;
+		default:
+			aligned_width = round_up(level_width, utile_w);
+			aligned_height = level_height;
+			break;
+		}
+
+		level_size = aligned_width * cpp * aligned_height;
+
+		if (offset < level_size) {
+			DRM_ERROR("Level %d (%dx%d -> %dx%d) size %db "
+				  "overflowed buffer bounds (offset %d)\n",
+				  i, level_width, level_height,
+				  aligned_width, aligned_height,
+				  level_size, offset);
+			goto fail;
+		}
+
+		offset -= level_size;
+	}
+
+	*validated_p0 = tex->paddr + p0;
+
+	return true;
+ fail:
+	DRM_INFO("Texture p0 at %d: 0x%08x\n", sample->p_offset[0], p0);
+	DRM_INFO("Texture p1 at %d: 0x%08x\n", sample->p_offset[1], p1);
+	DRM_INFO("Texture p2 at %d: 0x%08x\n", sample->p_offset[2], p2);
+	DRM_INFO("Texture p3 at %d: 0x%08x\n", sample->p_offset[3], p3);
+	return false;
+}
+
+static int
+validate_gl_shader_rec(struct drm_device *dev,
+		       struct vc4_exec_info *exec,
+		       struct vc4_shader_state *state)
+{
+	uint32_t *src_handles;
+	void *pkt_u, *pkt_v;
+	static const uint32_t shader_reloc_offsets[] = {
+		4, /* fs */
+		16, /* vs */
+		28, /* cs */
+	};
+	uint32_t shader_reloc_count = ARRAY_SIZE(shader_reloc_offsets);
+	struct drm_gem_cma_object *bo[shader_reloc_count + 8];
+	uint32_t nr_attributes, nr_relocs, packet_size;
+	int i;
+
+	nr_attributes = state->addr & 0x7;
+	if (nr_attributes == 0)
+		nr_attributes = 8;
+	packet_size = gl_shader_rec_size(state->addr);
+
+	nr_relocs = ARRAY_SIZE(shader_reloc_offsets) + nr_attributes;
+	if (nr_relocs * 4 > exec->shader_rec_size) {
+		DRM_ERROR("overflowed shader recs reading %d handles "
+			  "from %d bytes left\n",
+			  nr_relocs, exec->shader_rec_size);
+		return -EINVAL;
+	}
+	src_handles = exec->shader_rec_u;
+	exec->shader_rec_u += nr_relocs * 4;
+	exec->shader_rec_size -= nr_relocs * 4;
+
+	if (packet_size > exec->shader_rec_size) {
+		DRM_ERROR("overflowed shader recs copying %db packet "
+			  "from %d bytes left\n",
+			  packet_size, exec->shader_rec_size);
+		return -EINVAL;
+	}
+	pkt_u = exec->shader_rec_u;
+	pkt_v = exec->shader_rec_v;
+	memcpy(pkt_v, pkt_u, packet_size);
+	exec->shader_rec_u += packet_size;
+	/* Shader recs have to be aligned to 16 bytes (due to the attribute
+	 * flags being in the low bytes), so round the next validated shader
+	 * rec address up.  This should be safe, since we've got so many
+	 * relocations in a shader rec packet.
+	 */
+	BUG_ON(roundup(packet_size, 16) - packet_size > nr_relocs * 4);
+	exec->shader_rec_v += roundup(packet_size, 16);
+	exec->shader_rec_size -= packet_size;
+
+	if (!(*(uint16_t *)pkt_u & VC4_SHADER_FLAG_FS_SINGLE_THREAD)) {
+		DRM_ERROR("Multi-threaded fragment shaders not supported.\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < shader_reloc_count; i++) {
+		if (src_handles[i] > exec->bo_count) {
+			DRM_ERROR("Shader handle %d too big\n", src_handles[i]);
+			return -EINVAL;
+		}
+
+		bo[i] = exec->bo[src_handles[i]];
+		if (!bo[i])
+			return -EINVAL;
+	}
+	for (i = shader_reloc_count; i < nr_relocs; i++) {
+		bo[i] = vc4_use_bo(exec, src_handles[i]);
+		if (!bo[i])
+			return -EINVAL;
+	}
+
+	for (i = 0; i < shader_reloc_count; i++) {
+		struct vc4_validated_shader_info *validated_shader;
+		uint32_t o = shader_reloc_offsets[i];
+		uint32_t src_offset = *(uint32_t *)(pkt_u + o);
+		uint32_t *texture_handles_u;
+		void *uniform_data_u;
+		uint32_t tex;
+
+		*(uint32_t *)(pkt_v + o) = bo[i]->paddr + src_offset;
+
+		if (src_offset != 0) {
+			DRM_ERROR("Shaders must be at offset 0 of "
+				  "the BO.\n");
+			return -EINVAL;
+		}
+
+		validated_shader = to_vc4_bo(&bo[i]->base)->validated_shader;
+		if (!validated_shader)
+			return -EINVAL;
+
+		if (validated_shader->uniforms_src_size >
+		    exec->uniforms_size) {
+			DRM_ERROR("Uniforms src buffer overflow\n");
+			return -EINVAL;
+		}
+
+		texture_handles_u = exec->uniforms_u;
+		uniform_data_u = (texture_handles_u +
+				  validated_shader->num_texture_samples);
+
+		memcpy(exec->uniforms_v, uniform_data_u,
+		       validated_shader->uniforms_size);
+
+		for (tex = 0;
+		     tex < validated_shader->num_texture_samples;
+		     tex++) {
+			if (!reloc_tex(exec,
+				       uniform_data_u,
+				       &validated_shader->texture_samples[tex],
+				       texture_handles_u[tex])) {
+				return -EINVAL;
+			}
+		}
+
+		*(uint32_t *)(pkt_v + o + 4) = exec->uniforms_p;
+
+		exec->uniforms_u += validated_shader->uniforms_src_size;
+		exec->uniforms_v += validated_shader->uniforms_size;
+		exec->uniforms_p += validated_shader->uniforms_size;
+	}
+
+	for (i = 0; i < nr_attributes; i++) {
+		struct drm_gem_cma_object *vbo =
+			bo[ARRAY_SIZE(shader_reloc_offsets) + i];
+		uint32_t o = 36 + i * 8;
+		uint32_t offset = *(uint32_t *)(pkt_u + o + 0);
+		uint32_t attr_size = *(uint8_t *)(pkt_u + o + 4) + 1;
+		uint32_t stride = *(uint8_t *)(pkt_u + o + 5);
+		uint32_t max_index;
+
+		if (state->addr & 0x8)
+			stride |= (*(uint32_t *)(pkt_u + 100 + i * 4)) & ~0xff;
+
+		if (vbo->base.size < offset ||
+		    vbo->base.size - offset < attr_size) {
+			DRM_ERROR("BO offset overflow (%d + %d > %zu)\n",
+				  offset, attr_size, vbo->base.size);
+			return -EINVAL;
+		}
+
+		if (stride != 0) {
+			max_index = ((vbo->base.size - offset - attr_size) /
+				     stride);
+			if (state->max_index > max_index) {
+				DRM_ERROR("primitives use index %d out of "
+					  "supplied %d\n",
+					  state->max_index, max_index);
+				return -EINVAL;
+			}
+		}
+
+		*(uint32_t *)(pkt_v + o) = vbo->paddr + offset;
+	}
+
+	return 0;
+}
+
+int
+vc4_validate_shader_recs(struct drm_device *dev,
+			 struct vc4_exec_info *exec)
+{
+	uint32_t i;
+	int ret = 0;
+
+	for (i = 0; i < exec->shader_state_count; i++) {
+		ret = validate_gl_shader_rec(dev, exec, &exec->shader_state[i]);
+		if (ret)
+			return ret;
+	}
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/vc4/vc4_validate_shaders.c b/drivers/gpu/drm/vc4/vc4_validate_shaders.c
new file mode 100644
index 000000000000..f67124b4c534
--- /dev/null
+++ b/drivers/gpu/drm/vc4/vc4_validate_shaders.c
@@ -0,0 +1,513 @@
+/*
+ * Copyright © 2014 Broadcom
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+/**
+ * DOC: Shader validator for VC4.
+ *
+ * The VC4 has no IOMMU between it and system memory, so a user with
+ * access to execute shaders could escalate privilege by overwriting
+ * system memory (using the VPM write address register in the
+ * general-purpose DMA mode) or reading system memory it shouldn't
+ * (reading it as a texture, or uniform data, or vertex data).
+ *
+ * This walks over a shader BO, ensuring that its accesses are
+ * appropriately bounded, and recording how many texture accesses are
+ * made and where so that we can do relocations for them in the
+ * uniform stream.
+ */
+
+#include "vc4_drv.h"
+#include "vc4_qpu_defines.h"
+
+struct vc4_shader_validation_state {
+	struct vc4_texture_sample_info tmu_setup[2];
+	int tmu_write_count[2];
+
+	/* For registers that were last written to by a MIN instruction with
+	 * one argument being a uniform, the address of the uniform.
+	 * Otherwise, ~0.
+	 *
+	 * This is used for the validation of direct address memory reads.
+	 */
+	uint32_t live_min_clamp_offsets[32 + 32 + 4];
+	bool live_max_clamp_regs[32 + 32 + 4];
+};
+
+static uint32_t
+waddr_to_live_reg_index(uint32_t waddr, bool is_b)
+{
+	if (waddr < 32) {
+		if (is_b)
+			return 32 + waddr;
+		else
+			return waddr;
+	} else if (waddr <= QPU_W_ACC3) {
+		return 64 + waddr - QPU_W_ACC0;
+	} else {
+		return ~0;
+	}
+}
+
+static uint32_t
+raddr_add_a_to_live_reg_index(uint64_t inst)
+{
+	uint32_t sig = QPU_GET_FIELD(inst, QPU_SIG);
+	uint32_t add_a = QPU_GET_FIELD(inst, QPU_ADD_A);
+	uint32_t raddr_a = QPU_GET_FIELD(inst, QPU_RADDR_A);
+	uint32_t raddr_b = QPU_GET_FIELD(inst, QPU_RADDR_B);
+
+	if (add_a == QPU_MUX_A)
+		return raddr_a;
+	else if (add_a == QPU_MUX_B && sig != QPU_SIG_SMALL_IMM)
+		return 32 + raddr_b;
+	else if (add_a <= QPU_MUX_R3)
+		return 64 + add_a;
+	else
+		return ~0;
+}
+
+static bool
+is_tmu_submit(uint32_t waddr)
+{
+	return (waddr == QPU_W_TMU0_S ||
+		waddr == QPU_W_TMU1_S);
+}
+
+static bool
+is_tmu_write(uint32_t waddr)
+{
+	return (waddr >= QPU_W_TMU0_S &&
+		waddr <= QPU_W_TMU1_B);
+}
+
+static bool
+record_texture_sample(struct vc4_validated_shader_info *validated_shader,
+		      struct vc4_shader_validation_state *validation_state,
+		      int tmu)
+{
+	uint32_t s = validated_shader->num_texture_samples;
+	int i;
+	struct vc4_texture_sample_info *temp_samples;
+
+	temp_samples = krealloc(validated_shader->texture_samples,
+				(s + 1) * sizeof(*temp_samples),
+				GFP_KERNEL);
+	if (!temp_samples)
+		return false;
+
+	memcpy(&temp_samples[s],
+	       &validation_state->tmu_setup[tmu],
+	       sizeof(*temp_samples));
+
+	validated_shader->num_texture_samples = s + 1;
+	validated_shader->texture_samples = temp_samples;
+
+	for (i = 0; i < 4; i++)
+		validation_state->tmu_setup[tmu].p_offset[i] = ~0;
+
+	return true;
+}
+
+static bool
+check_tmu_write(uint64_t inst,
+		struct vc4_validated_shader_info *validated_shader,
+		struct vc4_shader_validation_state *validation_state,
+		bool is_mul)
+{
+	uint32_t waddr = (is_mul ?
+			  QPU_GET_FIELD(inst, QPU_WADDR_MUL) :
+			  QPU_GET_FIELD(inst, QPU_WADDR_ADD));
+	uint32_t raddr_a = QPU_GET_FIELD(inst, QPU_RADDR_A);
+	uint32_t raddr_b = QPU_GET_FIELD(inst, QPU_RADDR_B);
+	int tmu = waddr > QPU_W_TMU0_B;
+	bool submit = is_tmu_submit(waddr);
+	bool is_direct = submit && validation_state->tmu_write_count[tmu] == 0;
+	uint32_t sig = QPU_GET_FIELD(inst, QPU_SIG);
+
+	if (is_direct) {
+		uint32_t add_b = QPU_GET_FIELD(inst, QPU_ADD_B);
+		uint32_t clamp_reg, clamp_offset;
+
+		if (sig == QPU_SIG_SMALL_IMM) {
+			DRM_ERROR("direct TMU read used small immediate\n");
+			return false;
+		}
+
+		/* Make sure that this texture load is an add of the base
+		 * address of the UBO to a clamped offset within the UBO.
+		 */
+		if (is_mul ||
+		    QPU_GET_FIELD(inst, QPU_OP_ADD) != QPU_A_ADD) {
+			DRM_ERROR("direct TMU load wasn't an add\n");
+			return false;
+		}
+
+		/* We assert that the the clamped address is the first
+		 * argument, and the UBO base address is the second argument.
+		 * This is arbitrary, but simpler than supporting flipping the
+		 * two either way.
+		 */
+		clamp_reg = raddr_add_a_to_live_reg_index(inst);
+		if (clamp_reg == ~0) {
+			DRM_ERROR("direct TMU load wasn't clamped\n");
+			return false;
+		}
+
+		clamp_offset = validation_state->live_min_clamp_offsets[clamp_reg];
+		if (clamp_offset == ~0) {
+			DRM_ERROR("direct TMU load wasn't clamped\n");
+			return false;
+		}
+
+		/* Store the clamp value's offset in p1 (see reloc_tex() in
+		 * vc4_validate.c).
+		 */
+		validation_state->tmu_setup[tmu].p_offset[1] =
+			clamp_offset;
+
+		if (!(add_b == QPU_MUX_A && raddr_a == QPU_R_UNIF) &&
+		    !(add_b == QPU_MUX_B && raddr_b == QPU_R_UNIF)) {
+			DRM_ERROR("direct TMU load didn't add to a uniform\n");
+			return false;
+		}
+
+		validation_state->tmu_setup[tmu].is_direct = true;
+	} else {
+		if (raddr_a == QPU_R_UNIF || (sig != QPU_SIG_SMALL_IMM &&
+					      raddr_b == QPU_R_UNIF)) {
+			DRM_ERROR("uniform read in the same instruction as "
+				  "texture setup.\n");
+			return false;
+		}
+	}
+
+	if (validation_state->tmu_write_count[tmu] >= 4) {
+		DRM_ERROR("TMU%d got too many parameters before dispatch\n",
+			  tmu);
+		return false;
+	}
+	validation_state->tmu_setup[tmu].p_offset[validation_state->tmu_write_count[tmu]] =
+		validated_shader->uniforms_size;
+	validation_state->tmu_write_count[tmu]++;
+	/* Since direct uses a RADDR uniform reference, it will get counted in
+	 * check_instruction_reads()
+	 */
+	if (!is_direct)
+		validated_shader->uniforms_size += 4;
+
+	if (submit) {
+		if (!record_texture_sample(validated_shader,
+					   validation_state, tmu)) {
+			return false;
+		}
+
+		validation_state->tmu_write_count[tmu] = 0;
+	}
+
+	return true;
+}
+
+static bool
+check_reg_write(uint64_t inst,
+		struct vc4_validated_shader_info *validated_shader,
+		struct vc4_shader_validation_state *validation_state,
+		bool is_mul)
+{
+	uint32_t waddr = (is_mul ?
+			  QPU_GET_FIELD(inst, QPU_WADDR_MUL) :
+			  QPU_GET_FIELD(inst, QPU_WADDR_ADD));
+
+	switch (waddr) {
+	case QPU_W_UNIFORMS_ADDRESS:
+		/* XXX: We'll probably need to support this for reladdr, but
+		 * it's definitely a security-related one.
+		 */
+		DRM_ERROR("uniforms address load unsupported\n");
+		return false;
+
+	case QPU_W_TLB_COLOR_MS:
+	case QPU_W_TLB_COLOR_ALL:
+	case QPU_W_TLB_Z:
+		/* These only interact with the tile buffer, not main memory,
+		 * so they're safe.
+		 */
+		return true;
+
+	case QPU_W_TMU0_S:
+	case QPU_W_TMU0_T:
+	case QPU_W_TMU0_R:
+	case QPU_W_TMU0_B:
+	case QPU_W_TMU1_S:
+	case QPU_W_TMU1_T:
+	case QPU_W_TMU1_R:
+	case QPU_W_TMU1_B:
+		return check_tmu_write(inst, validated_shader, validation_state,
+				       is_mul);
+
+	case QPU_W_HOST_INT:
+	case QPU_W_TMU_NOSWAP:
+	case QPU_W_TLB_ALPHA_MASK:
+	case QPU_W_MUTEX_RELEASE:
+		/* XXX: I haven't thought about these, so don't support them
+		 * for now.
+		 */
+		DRM_ERROR("Unsupported waddr %d\n", waddr);
+		return false;
+
+	case QPU_W_VPM_ADDR:
+		DRM_ERROR("General VPM DMA unsupported\n");
+		return false;
+
+	case QPU_W_VPM:
+	case QPU_W_VPMVCD_SETUP:
+		/* We allow VPM setup in general, even including VPM DMA
+		 * configuration setup, because the (unsafe) DMA can only be
+		 * triggered by QPU_W_VPM_ADDR writes.
+		 */
+		return true;
+
+	case QPU_W_TLB_STENCIL_SETUP:
+		return true;
+	}
+
+	return true;
+}
+
+static void
+track_live_clamps(uint64_t inst,
+		  struct vc4_validated_shader_info *validated_shader,
+		  struct vc4_shader_validation_state *validation_state)
+{
+	uint32_t op_add = QPU_GET_FIELD(inst, QPU_OP_ADD);
+	uint32_t waddr_add = QPU_GET_FIELD(inst, QPU_WADDR_ADD);
+	uint32_t waddr_mul = QPU_GET_FIELD(inst, QPU_WADDR_MUL);
+	uint32_t cond_add = QPU_GET_FIELD(inst, QPU_COND_ADD);
+	uint32_t add_a = QPU_GET_FIELD(inst, QPU_ADD_A);
+	uint32_t add_b = QPU_GET_FIELD(inst, QPU_ADD_B);
+	uint32_t raddr_a = QPU_GET_FIELD(inst, QPU_RADDR_A);
+	uint32_t raddr_b = QPU_GET_FIELD(inst, QPU_RADDR_B);
+	uint32_t sig = QPU_GET_FIELD(inst, QPU_SIG);
+	bool ws = inst & QPU_WS;
+	uint32_t lri_add_a, lri_add, lri_mul;
+	bool add_a_is_min_0;
+
+	/* Check whether OP_ADD's A argumennt comes from a live MAX(x, 0),
+	 * before we clear previous live state.
+	 */
+	lri_add_a = raddr_add_a_to_live_reg_index(inst);
+	add_a_is_min_0 = (lri_add_a != ~0 &&
+			  validation_state->live_max_clamp_regs[lri_add_a]);
+
+	/* Clear live state for registers written by our instruction. */
+	lri_add = waddr_to_live_reg_index(waddr_add, ws);
+	lri_mul = waddr_to_live_reg_index(waddr_mul, !ws);
+	if (lri_mul != ~0) {
+		validation_state->live_max_clamp_regs[lri_mul] = false;
+		validation_state->live_min_clamp_offsets[lri_mul] = ~0;
+	}
+	if (lri_add != ~0) {
+		validation_state->live_max_clamp_regs[lri_add] = false;
+		validation_state->live_min_clamp_offsets[lri_add] = ~0;
+	} else {
+		/* Nothing further to do for live tracking, since only ADDs
+		 * generate new live clamp registers.
+		 */
+		return;
+	}
+
+	/* Now, handle remaining live clamp tracking for the ADD operation. */
+
+	if (cond_add != QPU_COND_ALWAYS)
+		return;
+
+	if (op_add == QPU_A_MAX) {
+		/* Track live clamps of a value to a minimum of 0 (in either
+		 * arg).
+		 */
+		if (sig != QPU_SIG_SMALL_IMM || raddr_b != 0 ||
+		    (add_a != QPU_MUX_B && add_b != QPU_MUX_B)) {
+			return;
+		}
+
+		validation_state->live_max_clamp_regs[lri_add] = true;
+	} else if (op_add == QPU_A_MIN) {
+		/* Track live clamps of a value clamped to a minimum of 0 and
+		 * a maximum of some uniform's offset.
+		 */
+		if (!add_a_is_min_0)
+			return;
+
+		if (!(add_b == QPU_MUX_A && raddr_a == QPU_R_UNIF) &&
+		    !(add_b == QPU_MUX_B && raddr_b == QPU_R_UNIF &&
+		      sig != QPU_SIG_SMALL_IMM)) {
+			return;
+		}
+
+		validation_state->live_min_clamp_offsets[lri_add] =
+			validated_shader->uniforms_size;
+	}
+}
+
+static bool
+check_instruction_writes(uint64_t inst,
+			 struct vc4_validated_shader_info *validated_shader,
+			 struct vc4_shader_validation_state *validation_state)
+{
+	uint32_t waddr_add = QPU_GET_FIELD(inst, QPU_WADDR_ADD);
+	uint32_t waddr_mul = QPU_GET_FIELD(inst, QPU_WADDR_MUL);
+	bool ok;
+
+	if (is_tmu_write(waddr_add) && is_tmu_write(waddr_mul)) {
+		DRM_ERROR("ADD and MUL both set up textures\n");
+		return false;
+	}
+
+	ok = (check_reg_write(inst, validated_shader, validation_state,
+			      false) &&
+	      check_reg_write(inst, validated_shader, validation_state,
+			      true));
+
+	track_live_clamps(inst, validated_shader, validation_state);
+
+	return ok;
+}
+
+static bool
+check_instruction_reads(uint64_t inst,
+			struct vc4_validated_shader_info *validated_shader)
+{
+	uint32_t raddr_a = QPU_GET_FIELD(inst, QPU_RADDR_A);
+	uint32_t raddr_b = QPU_GET_FIELD(inst, QPU_RADDR_B);
+	uint32_t sig = QPU_GET_FIELD(inst, QPU_SIG);
+
+	if (raddr_a == QPU_R_UNIF ||
+	    (raddr_b == QPU_R_UNIF && sig != QPU_SIG_SMALL_IMM)) {
+		/* This can't overflow the uint32_t, because we're reading 8
+		 * bytes of instruction to increment by 4 here, so we'd
+		 * already be OOM.
+		 */
+		validated_shader->uniforms_size += 4;
+	}
+
+	return true;
+}
+
+struct vc4_validated_shader_info *
+vc4_validate_shader(struct drm_gem_cma_object *shader_obj)
+{
+	bool found_shader_end = false;
+	int shader_end_ip = 0;
+	uint32_t ip, max_ip;
+	uint64_t *shader;
+	struct vc4_validated_shader_info *validated_shader;
+	struct vc4_shader_validation_state validation_state;
+	int i;
+
+	memset(&validation_state, 0, sizeof(validation_state));
+
+	for (i = 0; i < 8; i++)
+		validation_state.tmu_setup[i / 4].p_offset[i % 4] = ~0;
+	for (i = 0; i < ARRAY_SIZE(validation_state.live_min_clamp_offsets); i++)
+		validation_state.live_min_clamp_offsets[i] = ~0;
+
+	shader = shader_obj->vaddr;
+	max_ip = shader_obj->base.size / sizeof(uint64_t);
+
+	validated_shader = kcalloc(1, sizeof(*validated_shader), GFP_KERNEL);
+	if (!validated_shader)
+		return NULL;
+
+	for (ip = 0; ip < max_ip; ip++) {
+		uint64_t inst = shader[ip];
+		uint32_t sig = QPU_GET_FIELD(inst, QPU_SIG);
+
+		switch (sig) {
+		case QPU_SIG_NONE:
+		case QPU_SIG_WAIT_FOR_SCOREBOARD:
+		case QPU_SIG_SCOREBOARD_UNLOCK:
+		case QPU_SIG_COLOR_LOAD:
+		case QPU_SIG_LOAD_TMU0:
+		case QPU_SIG_LOAD_TMU1:
+		case QPU_SIG_PROG_END:
+		case QPU_SIG_SMALL_IMM:
+			if (!check_instruction_writes(inst, validated_shader,
+						      &validation_state)) {
+				DRM_ERROR("Bad write at ip %d\n", ip);
+				goto fail;
+			}
+
+			if (!check_instruction_reads(inst, validated_shader))
+				goto fail;
+
+			if (sig == QPU_SIG_PROG_END) {
+				found_shader_end = true;
+				shader_end_ip = ip;
+			}
+
+			break;
+
+		case QPU_SIG_LOAD_IMM:
+			if (!check_instruction_writes(inst, validated_shader,
+						      &validation_state)) {
+				DRM_ERROR("Bad LOAD_IMM write at ip %d\n", ip);
+				goto fail;
+			}
+			break;
+
+		default:
+			DRM_ERROR("Unsupported QPU signal %d at "
+				  "instruction %d\n", sig, ip);
+			goto fail;
+		}
+
+		/* There are two delay slots after program end is signaled
+		 * that are still executed, then we're finished.
+		 */
+		if (found_shader_end && ip == shader_end_ip + 2)
+			break;
+	}
+
+	if (ip == max_ip) {
+		DRM_ERROR("shader failed to terminate before "
+			  "shader BO end at %zd\n",
+			  shader_obj->base.size);
+		goto fail;
+	}
+
+	/* Again, no chance of integer overflow here because the worst case
+	 * scenario is 8 bytes of uniforms plus handles per 8-byte
+	 * instruction.
+	 */
+	validated_shader->uniforms_src_size =
+		(validated_shader->uniforms_size +
+		 4 * validated_shader->num_texture_samples);
+
+	return validated_shader;
+
+fail:
+	if (validated_shader) {
+		kfree(validated_shader->texture_samples);
+		kfree(validated_shader);
+	}
+	return NULL;
+}
diff --git a/drivers/gpu/drm/virtio/virtgpu_display.c b/drivers/gpu/drm/virtio/virtgpu_display.c
index 578fe0a9324c..a165f03eaa79 100644
--- a/drivers/gpu/drm/virtio/virtgpu_display.c
+++ b/drivers/gpu/drm/virtio/virtgpu_display.c
@@ -215,7 +215,7 @@ static const struct drm_framebuffer_funcs virtio_gpu_fb_funcs = {
 int
 virtio_gpu_framebuffer_init(struct drm_device *dev,
 			    struct virtio_gpu_framebuffer *vgfb,
-			    struct drm_mode_fb_cmd2 *mode_cmd,
+			    const struct drm_mode_fb_cmd2 *mode_cmd,
 			    struct drm_gem_object *obj)
 {
 	int ret;
@@ -374,16 +374,6 @@ static const struct drm_connector_helper_funcs virtio_gpu_conn_helper_funcs = {
 	.best_encoder = virtio_gpu_best_encoder,
 };
 
-static void virtio_gpu_conn_save(struct drm_connector *connector)
-{
-	DRM_DEBUG("\n");
-}
-
-static void virtio_gpu_conn_restore(struct drm_connector *connector)
-{
-	DRM_DEBUG("\n");
-}
-
 static enum drm_connector_status virtio_gpu_conn_detect(
 			struct drm_connector *connector,
 			bool force)
@@ -409,10 +399,8 @@ static void virtio_gpu_conn_destroy(struct drm_connector *connector)
 
 static const struct drm_connector_funcs virtio_gpu_connector_funcs = {
 	.dpms = drm_atomic_helper_connector_dpms,
-	.save = virtio_gpu_conn_save,
-	.restore = virtio_gpu_conn_restore,
 	.detect = virtio_gpu_conn_detect,
-	.fill_modes = drm_helper_probe_single_connector_modes_nomerge,
+	.fill_modes = drm_helper_probe_single_connector_modes,
 	.destroy = virtio_gpu_conn_destroy,
 	.reset = drm_atomic_helper_connector_reset,
 	.atomic_duplicate_state = drm_atomic_helper_connector_duplicate_state,
@@ -443,7 +431,7 @@ static int vgdev_output_init(struct virtio_gpu_device *vgdev, int index)
 	if (IS_ERR(plane))
 		return PTR_ERR(plane);
 	drm_crtc_init_with_planes(dev, crtc, plane, NULL,
-				  &virtio_gpu_crtc_funcs);
+				  &virtio_gpu_crtc_funcs, NULL);
 	drm_mode_crtc_set_gamma_size(crtc, 256);
 	drm_crtc_helper_add(crtc, &virtio_gpu_crtc_helper_funcs);
 	plane->crtc = crtc;
@@ -453,7 +441,7 @@ static int vgdev_output_init(struct virtio_gpu_device *vgdev, int index)
 	drm_connector_helper_add(connector, &virtio_gpu_conn_helper_funcs);
 
 	drm_encoder_init(dev, encoder, &virtio_gpu_enc_funcs,
-			 DRM_MODE_ENCODER_VIRTUAL);
+			 DRM_MODE_ENCODER_VIRTUAL, NULL);
 	drm_encoder_helper_add(encoder, &virtio_gpu_enc_helper_funcs);
 	encoder->possible_crtcs = 1 << index;
 
@@ -465,7 +453,7 @@ static int vgdev_output_init(struct virtio_gpu_device *vgdev, int index)
 static struct drm_framebuffer *
 virtio_gpu_user_framebuffer_create(struct drm_device *dev,
 				   struct drm_file *file_priv,
-				   struct drm_mode_fb_cmd2 *mode_cmd)
+				   const struct drm_mode_fb_cmd2 *mode_cmd)
 {
 	struct drm_gem_object *obj = NULL;
 	struct virtio_gpu_framebuffer *virtio_gpu_fb;
diff --git a/drivers/gpu/drm/virtio/virtgpu_drv.h b/drivers/gpu/drm/virtio/virtgpu_drv.h
index 79f0abe69b64..8f486f4c7023 100644
--- a/drivers/gpu/drm/virtio/virtgpu_drv.h
+++ b/drivers/gpu/drm/virtio/virtgpu_drv.h
@@ -328,7 +328,7 @@ void virtio_gpu_dequeue_fence_func(struct work_struct *work);
 /* virtio_gpu_display.c */
 int virtio_gpu_framebuffer_init(struct drm_device *dev,
 				struct virtio_gpu_framebuffer *vgfb,
-				struct drm_mode_fb_cmd2 *mode_cmd,
+				const struct drm_mode_fb_cmd2 *mode_cmd,
 				struct drm_gem_object *obj);
 int virtio_gpu_modeset_init(struct virtio_gpu_device *vgdev);
 void virtio_gpu_modeset_fini(struct virtio_gpu_device *vgdev);
diff --git a/drivers/gpu/drm/virtio/virtgpu_fb.c b/drivers/gpu/drm/virtio/virtgpu_fb.c
index 6a81e084593b..2242a80866a9 100644
--- a/drivers/gpu/drm/virtio/virtgpu_fb.c
+++ b/drivers/gpu/drm/virtio/virtgpu_fb.c
@@ -32,7 +32,6 @@
 struct virtio_gpu_fbdev {
 	struct drm_fb_helper           helper;
 	struct virtio_gpu_framebuffer  vgfb;
-	struct list_head	       fbdev_list;
 	struct virtio_gpu_device       *vgdev;
 	struct delayed_work            work;
 };
diff --git a/drivers/gpu/drm/virtio/virtgpu_kms.c b/drivers/gpu/drm/virtio/virtgpu_kms.c
index 06496a128162..4150873d432e 100644
--- a/drivers/gpu/drm/virtio/virtgpu_kms.c
+++ b/drivers/gpu/drm/virtio/virtgpu_kms.c
@@ -130,7 +130,7 @@ int virtio_gpu_driver_load(struct drm_device *dev, unsigned long flags)
 	static vq_callback_t *callbacks[] = {
 		virtio_gpu_ctrl_ack, virtio_gpu_cursor_ack
 	};
-	static const char *names[] = { "control", "cursor" };
+	static const char * const names[] = { "control", "cursor" };
 
 	struct virtio_gpu_device *vgdev;
 	/* this will expand later */
diff --git a/drivers/gpu/drm/virtio/virtgpu_plane.c b/drivers/gpu/drm/virtio/virtgpu_plane.c
index 4a74129c5708..572fb351feab 100644
--- a/drivers/gpu/drm/virtio/virtgpu_plane.c
+++ b/drivers/gpu/drm/virtio/virtgpu_plane.c
@@ -107,7 +107,7 @@ struct drm_plane *virtio_gpu_plane_init(struct virtio_gpu_device *vgdev,
 				       &virtio_gpu_plane_funcs,
 				       virtio_gpu_formats,
 				       ARRAY_SIZE(virtio_gpu_formats),
-				       DRM_PLANE_TYPE_PRIMARY);
+				       DRM_PLANE_TYPE_PRIMARY, NULL);
 	if (ret)
 		goto err_plane_init;
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf.c
index 6377e8151000..67cebb23c940 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_cmdbuf.c
@@ -247,7 +247,7 @@ static void __vmw_cmdbuf_header_free(struct vmw_cmdbuf_header *header)
 {
 	struct vmw_cmdbuf_man *man = header->man;
 
-	BUG_ON(!spin_is_locked(&man->lock));
+	lockdep_assert_held_once(&man->lock);
 
 	if (header->inline_space) {
 		vmw_cmdbuf_header_inline_free(header);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index c49812b80dd0..24fb348a44e1 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -25,6 +25,7 @@
  *
  **************************************************************************/
 #include <linux/module.h>
+#include <linux/console.h>
 
 #include <drm/drmP.h>
 #include "vmwgfx_drv.h"
@@ -1538,6 +1539,12 @@ static int vmw_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 static int __init vmwgfx_init(void)
 {
 	int ret;
+
+#ifdef CONFIG_VGA_CONSOLE
+	if (vgacon_text_force())
+		return -EINVAL;
+#endif
+
 	ret = drm_pci_init(&driver, &vmw_pci_driver);
 	if (ret)
 		DRM_ERROR("Failed initializing DRM.\n");
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
index 9b4bb9e74d73..b221a8c40282 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_kms.c
@@ -508,7 +508,7 @@ int vmw_kms_readback(struct vmw_private *dev_priv,
 }
 
 
-static struct drm_framebuffer_funcs vmw_framebuffer_surface_funcs = {
+static const struct drm_framebuffer_funcs vmw_framebuffer_surface_funcs = {
 	.destroy = vmw_framebuffer_surface_destroy,
 	.dirty = vmw_framebuffer_surface_dirty,
 };
@@ -685,7 +685,7 @@ static int vmw_framebuffer_dmabuf_dirty(struct drm_framebuffer *framebuffer,
 	return ret;
 }
 
-static struct drm_framebuffer_funcs vmw_framebuffer_dmabuf_funcs = {
+static const struct drm_framebuffer_funcs vmw_framebuffer_dmabuf_funcs = {
 	.destroy = vmw_framebuffer_dmabuf_destroy,
 	.dirty = vmw_framebuffer_dmabuf_dirty,
 };
@@ -763,21 +763,25 @@ static int vmw_create_dmabuf_proxy(struct drm_device *dev,
 	uint32_t format;
 	struct drm_vmw_size content_base_size;
 	struct vmw_resource *res;
+	unsigned int bytes_pp;
 	int ret;
 
 	switch (mode_cmd->depth) {
 	case 32:
 	case 24:
 		format = SVGA3D_X8R8G8B8;
+		bytes_pp = 4;
 		break;
 
 	case 16:
 	case 15:
 		format = SVGA3D_R5G6B5;
+		bytes_pp = 2;
 		break;
 
 	case 8:
 		format = SVGA3D_P8;
+		bytes_pp = 1;
 		break;
 
 	default:
@@ -785,7 +789,7 @@ static int vmw_create_dmabuf_proxy(struct drm_device *dev,
 		return -EINVAL;
 	}
 
-	content_base_size.width  = mode_cmd->width;
+	content_base_size.width  = mode_cmd->pitch / bytes_pp;
 	content_base_size.height = mode_cmd->height;
 	content_base_size.depth  = 1;
 
@@ -968,7 +972,7 @@ vmw_kms_new_framebuffer(struct vmw_private *dev_priv,
 
 static struct drm_framebuffer *vmw_kms_fb_create(struct drm_device *dev,
 						 struct drm_file *file_priv,
-						 struct drm_mode_fb_cmd2 *mode_cmd2)
+						 const struct drm_mode_fb_cmd2 *mode_cmd2)
 {
 	struct vmw_private *dev_priv = vmw_priv(dev);
 	struct ttm_object_file *tfile = vmw_fpriv(file_priv)->tfile;
@@ -1369,14 +1373,6 @@ static int vmw_du_update_layout(struct vmw_private *dev_priv, unsigned num,
 	return 0;
 }
 
-void vmw_du_crtc_save(struct drm_crtc *crtc)
-{
-}
-
-void vmw_du_crtc_restore(struct drm_crtc *crtc)
-{
-}
-
 void vmw_du_crtc_gamma_set(struct drm_crtc *crtc,
 			   u16 *r, u16 *g, u16 *b,
 			   uint32_t start, uint32_t size)
@@ -1398,14 +1394,6 @@ int vmw_du_connector_dpms(struct drm_connector *connector, int mode)
 	return 0;
 }
 
-void vmw_du_connector_save(struct drm_connector *connector)
-{
-}
-
-void vmw_du_connector_restore(struct drm_connector *connector)
-{
-}
-
 enum drm_connector_status
 vmw_du_connector_detect(struct drm_connector *connector, bool force)
 {
@@ -1592,7 +1580,7 @@ int vmw_du_connector_fill_modes(struct drm_connector *connector,
 		drm_mode_probed_add(connector, mode);
 	}
 
-	drm_mode_connector_list_update(connector, true);
+	drm_mode_connector_list_update(connector);
 	/* Move the prefered mode first, help apps pick the right mode. */
 	drm_mode_sort(&connector->modes);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
index 52caecb4502e..b6fa44fe8929 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_ldu.c
@@ -294,9 +294,7 @@ static int vmw_ldu_crtc_set_config(struct drm_mode_set *set)
 	return vmw_ldu_commit_list(dev_priv);
 }
 
-static struct drm_crtc_funcs vmw_legacy_crtc_funcs = {
-	.save = vmw_du_crtc_save,
-	.restore = vmw_du_crtc_restore,
+static const struct drm_crtc_funcs vmw_legacy_crtc_funcs = {
 	.cursor_set2 = vmw_du_crtc_cursor_set2,
 	.cursor_move = vmw_du_crtc_cursor_move,
 	.gamma_set = vmw_du_crtc_gamma_set,
@@ -314,7 +312,7 @@ static void vmw_ldu_encoder_destroy(struct drm_encoder *encoder)
 	vmw_ldu_destroy(vmw_encoder_to_ldu(encoder));
 }
 
-static struct drm_encoder_funcs vmw_legacy_encoder_funcs = {
+static const struct drm_encoder_funcs vmw_legacy_encoder_funcs = {
 	.destroy = vmw_ldu_encoder_destroy,
 };
 
@@ -327,10 +325,8 @@ static void vmw_ldu_connector_destroy(struct drm_connector *connector)
 	vmw_ldu_destroy(vmw_connector_to_ldu(connector));
 }
 
-static struct drm_connector_funcs vmw_legacy_connector_funcs = {
+static const struct drm_connector_funcs vmw_legacy_connector_funcs = {
 	.dpms = vmw_du_connector_dpms,
-	.save = vmw_du_connector_save,
-	.restore = vmw_du_connector_restore,
 	.detect = vmw_du_connector_detect,
 	.fill_modes = vmw_du_connector_fill_modes,
 	.set_property = vmw_du_connector_set_property,
@@ -367,7 +363,7 @@ static int vmw_ldu_init(struct vmw_private *dev_priv, unsigned unit)
 	connector->status = vmw_du_connector_detect(connector, true);
 
 	drm_encoder_init(dev, encoder, &vmw_legacy_encoder_funcs,
-			 DRM_MODE_ENCODER_VIRTUAL);
+			 DRM_MODE_ENCODER_VIRTUAL, NULL);
 	drm_mode_connector_attach_encoder(connector, encoder);
 	encoder->possible_crtcs = (1 << unit);
 	encoder->possible_clones = 0;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c b/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
index 13926ff192e3..c5a1a08b0449 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_scrn.c
@@ -530,9 +530,7 @@ out_no_fence:
 	return ret;
 }
 
-static struct drm_crtc_funcs vmw_screen_object_crtc_funcs = {
-	.save = vmw_du_crtc_save,
-	.restore = vmw_du_crtc_restore,
+static const struct drm_crtc_funcs vmw_screen_object_crtc_funcs = {
 	.cursor_set2 = vmw_du_crtc_cursor_set2,
 	.cursor_move = vmw_du_crtc_cursor_move,
 	.gamma_set = vmw_du_crtc_gamma_set,
@@ -550,7 +548,7 @@ static void vmw_sou_encoder_destroy(struct drm_encoder *encoder)
 	vmw_sou_destroy(vmw_encoder_to_sou(encoder));
 }
 
-static struct drm_encoder_funcs vmw_screen_object_encoder_funcs = {
+static const struct drm_encoder_funcs vmw_screen_object_encoder_funcs = {
 	.destroy = vmw_sou_encoder_destroy,
 };
 
@@ -563,10 +561,8 @@ static void vmw_sou_connector_destroy(struct drm_connector *connector)
 	vmw_sou_destroy(vmw_connector_to_sou(connector));
 }
 
-static struct drm_connector_funcs vmw_sou_connector_funcs = {
+static const struct drm_connector_funcs vmw_sou_connector_funcs = {
 	.dpms = vmw_du_connector_dpms,
-	.save = vmw_du_connector_save,
-	.restore = vmw_du_connector_restore,
 	.detect = vmw_du_connector_detect,
 	.fill_modes = vmw_du_connector_fill_modes,
 	.set_property = vmw_du_connector_set_property,
@@ -603,7 +599,7 @@ static int vmw_sou_init(struct vmw_private *dev_priv, unsigned unit)
 	connector->status = vmw_du_connector_detect(connector, true);
 
 	drm_encoder_init(dev, encoder, &vmw_screen_object_encoder_funcs,
-			 DRM_MODE_ENCODER_VIRTUAL);
+			 DRM_MODE_ENCODER_VIRTUAL, NULL);
 	drm_mode_connector_attach_encoder(connector, encoder);
 	encoder->possible_crtcs = (1 << unit);
 	encoder->possible_clones = 0;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_stdu.c b/drivers/gpu/drm/vmwgfx/vmwgfx_stdu.c
index f823fc3efed7..4ef5ffd7189d 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_stdu.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_stdu.c
@@ -1040,9 +1040,7 @@ out_finish:
 /*
  *  Screen Target CRTC dispatch table
  */
-static struct drm_crtc_funcs vmw_stdu_crtc_funcs = {
-	.save = vmw_du_crtc_save,
-	.restore = vmw_du_crtc_restore,
+static const struct drm_crtc_funcs vmw_stdu_crtc_funcs = {
 	.cursor_set2 = vmw_du_crtc_cursor_set2,
 	.cursor_move = vmw_du_crtc_cursor_move,
 	.gamma_set = vmw_du_crtc_gamma_set,
@@ -1072,7 +1070,7 @@ static void vmw_stdu_encoder_destroy(struct drm_encoder *encoder)
 	vmw_stdu_destroy(vmw_encoder_to_stdu(encoder));
 }
 
-static struct drm_encoder_funcs vmw_stdu_encoder_funcs = {
+static const struct drm_encoder_funcs vmw_stdu_encoder_funcs = {
 	.destroy = vmw_stdu_encoder_destroy,
 };
 
@@ -1099,10 +1097,8 @@ static void vmw_stdu_connector_destroy(struct drm_connector *connector)
 
 
 
-static struct drm_connector_funcs vmw_stdu_connector_funcs = {
+static const struct drm_connector_funcs vmw_stdu_connector_funcs = {
 	.dpms = vmw_du_connector_dpms,
-	.save = vmw_du_connector_save,
-	.restore = vmw_du_connector_restore,
 	.detect = vmw_du_connector_detect,
 	.fill_modes = vmw_du_connector_fill_modes,
 	.set_property = vmw_du_connector_set_property,
@@ -1149,7 +1145,7 @@ static int vmw_stdu_init(struct vmw_private *dev_priv, unsigned unit)
 	connector->status = vmw_du_connector_detect(connector, false);
 
 	drm_encoder_init(dev, encoder, &vmw_stdu_encoder_funcs,
-			 DRM_MODE_ENCODER_VIRTUAL);
+			 DRM_MODE_ENCODER_VIRTUAL, NULL);
 	drm_mode_connector_attach_encoder(connector, encoder);
 	encoder->possible_crtcs = (1 << unit);
 	encoder->possible_clones = 0;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
index 7d620e82e000..c2a721a8cef9 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_surface.c
@@ -771,7 +771,7 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 	}
 	srf->offsets = kmalloc(srf->num_sizes * sizeof(*srf->offsets),
 			       GFP_KERNEL);
-	if (unlikely(srf->sizes == NULL)) {
+	if (unlikely(srf->offsets == NULL)) {
 		ret = -ENOMEM;
 		goto out_no_offsets;
 	}
@@ -815,11 +815,8 @@ int vmw_surface_define_ioctl(struct drm_device *dev, void *data,
 	    srf->sizes[0].height == 64 &&
 	    srf->format == SVGA3D_A8R8G8B8) {
 
-		srf->snooper.image = kmalloc(64 * 64 * 4, GFP_KERNEL);
-		/* clear the image */
-		if (srf->snooper.image) {
-			memset(srf->snooper.image, 0x00, 64 * 64 * 4);
-		} else {
+		srf->snooper.image = kzalloc(64 * 64 * 4, GFP_KERNEL);
+		if (!srf->snooper.image) {
 			DRM_ERROR("Failed to allocate cursor_image\n");
 			ret = -ENOMEM;
 			goto out_no_copy;
diff --git a/drivers/gpu/host1x/Makefile b/drivers/gpu/host1x/Makefile
index c1189f004441..a1d9974cfcb5 100644
--- a/drivers/gpu/host1x/Makefile
+++ b/drivers/gpu/host1x/Makefile
@@ -10,6 +10,7 @@ host1x-y = \
 	mipi.o \
 	hw/host1x01.o \
 	hw/host1x02.o \
-	hw/host1x04.o
+	hw/host1x04.o \
+	hw/host1x05.o
 
 obj-$(CONFIG_TEGRA_HOST1X) += host1x.o
diff --git a/drivers/gpu/host1x/bus.c b/drivers/gpu/host1x/bus.c
index 4a99c6416e6a..dd2dbb9746ce 100644
--- a/drivers/gpu/host1x/bus.c
+++ b/drivers/gpu/host1x/bus.c
@@ -18,6 +18,7 @@
 #include <linux/host1x.h>
 #include <linux/of.h>
 #include <linux/slab.h>
+#include <linux/of_device.h>
 
 #include "bus.h"
 #include "dev.h"
@@ -394,6 +395,7 @@ static int host1x_device_add(struct host1x *host1x,
 	device->dev.coherent_dma_mask = host1x->dev->coherent_dma_mask;
 	device->dev.dma_mask = &device->dev.coherent_dma_mask;
 	dev_set_name(&device->dev, "%s", driver->driver.name);
+	of_dma_configure(&device->dev, host1x->dev->of_node);
 	device->dev.release = host1x_device_release;
 	device->dev.bus = &host1x_bus_type;
 	device->dev.parent = host1x->dev;
@@ -538,6 +540,8 @@ EXPORT_SYMBOL(host1x_driver_register_full);
 
 void host1x_driver_unregister(struct host1x_driver *driver)
 {
+	driver_unregister(&driver->driver);
+
 	mutex_lock(&drivers_lock);
 	list_del_init(&driver->list);
 	mutex_unlock(&drivers_lock);
diff --git a/drivers/gpu/host1x/dev.c b/drivers/gpu/host1x/dev.c
index 53d3d1d45b48..ff348690df94 100644
--- a/drivers/gpu/host1x/dev.c
+++ b/drivers/gpu/host1x/dev.c
@@ -23,6 +23,7 @@
 #include <linux/of_device.h>
 #include <linux/clk.h>
 #include <linux/io.h>
+#include <linux/dma-mapping.h>
 
 #define CREATE_TRACE_POINTS
 #include <trace/events/host1x.h>
@@ -35,6 +36,7 @@
 #include "hw/host1x01.h"
 #include "hw/host1x02.h"
 #include "hw/host1x04.h"
+#include "hw/host1x05.h"
 
 void host1x_sync_writel(struct host1x *host1x, u32 v, u32 r)
 {
@@ -67,6 +69,7 @@ static const struct host1x_info host1x01_info = {
 	.nb_bases	= 8,
 	.init		= host1x01_init,
 	.sync_offset	= 0x3000,
+	.dma_mask	= DMA_BIT_MASK(32),
 };
 
 static const struct host1x_info host1x02_info = {
@@ -76,6 +79,7 @@ static const struct host1x_info host1x02_info = {
 	.nb_bases = 12,
 	.init = host1x02_init,
 	.sync_offset = 0x3000,
+	.dma_mask = DMA_BIT_MASK(32),
 };
 
 static const struct host1x_info host1x04_info = {
@@ -85,9 +89,21 @@ static const struct host1x_info host1x04_info = {
 	.nb_bases = 64,
 	.init = host1x04_init,
 	.sync_offset = 0x2100,
+	.dma_mask = DMA_BIT_MASK(34),
+};
+
+static const struct host1x_info host1x05_info = {
+	.nb_channels = 14,
+	.nb_pts = 192,
+	.nb_mlocks = 16,
+	.nb_bases = 64,
+	.init = host1x05_init,
+	.sync_offset = 0x2100,
+	.dma_mask = DMA_BIT_MASK(34),
 };
 
 static struct of_device_id host1x_of_match[] = {
+	{ .compatible = "nvidia,tegra210-host1x", .data = &host1x05_info, },
 	{ .compatible = "nvidia,tegra124-host1x", .data = &host1x04_info, },
 	{ .compatible = "nvidia,tegra114-host1x", .data = &host1x02_info, },
 	{ .compatible = "nvidia,tegra30-host1x", .data = &host1x01_info, },
@@ -137,6 +153,8 @@ static int host1x_probe(struct platform_device *pdev)
 	if (IS_ERR(host->regs))
 		return PTR_ERR(host->regs);
 
+	dma_set_mask_and_coherent(host->dev, host->info->dma_mask);
+
 	if (host->info->init) {
 		err = host->info->init(host);
 		if (err)
@@ -212,6 +230,11 @@ static struct platform_driver tegra_host1x_driver = {
 	.remove = host1x_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&tegra_host1x_driver,
+	&tegra_mipi_driver,
+};
+
 static int __init tegra_host1x_init(void)
 {
 	int err;
@@ -220,28 +243,17 @@ static int __init tegra_host1x_init(void)
 	if (err < 0)
 		return err;
 
-	err = platform_driver_register(&tegra_host1x_driver);
-	if (err < 0)
-		goto unregister_bus;
-
-	err = platform_driver_register(&tegra_mipi_driver);
+	err = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 	if (err < 0)
-		goto unregister_host1x;
+		bus_unregister(&host1x_bus_type);
 
-	return 0;
-
-unregister_host1x:
-	platform_driver_unregister(&tegra_host1x_driver);
-unregister_bus:
-	bus_unregister(&host1x_bus_type);
 	return err;
 }
 module_init(tegra_host1x_init);
 
 static void __exit tegra_host1x_exit(void)
 {
-	platform_driver_unregister(&tegra_mipi_driver);
-	platform_driver_unregister(&tegra_host1x_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 	bus_unregister(&host1x_bus_type);
 }
 module_exit(tegra_host1x_exit);
diff --git a/drivers/gpu/host1x/dev.h b/drivers/gpu/host1x/dev.h
index 0b6e8e9629c5..dace124994bb 100644
--- a/drivers/gpu/host1x/dev.h
+++ b/drivers/gpu/host1x/dev.h
@@ -96,6 +96,7 @@ struct host1x_info {
 	int	nb_mlocks;		/* host1x: number of mlocks */
 	int	(*init)(struct host1x *); /* initialize per SoC ops */
 	int	sync_offset;
+	u64	dma_mask;		/* mask of addressable memory */
 };
 
 struct host1x {
diff --git a/drivers/gpu/host1x/hw/host1x05.c b/drivers/gpu/host1x/hw/host1x05.c
new file mode 100644
index 000000000000..047097ce3bad
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x05.c
@@ -0,0 +1,42 @@
+/*
+ * Host1x init for Tegra210 SoCs
+ *
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* include hw specification */
+#include "host1x05.h"
+#include "host1x05_hardware.h"
+
+/* include code */
+#include "cdma_hw.c"
+#include "channel_hw.c"
+#include "debug_hw.c"
+#include "intr_hw.c"
+#include "syncpt_hw.c"
+
+#include "../dev.h"
+
+int host1x05_init(struct host1x *host)
+{
+	host->channel_op = &host1x_channel_ops;
+	host->cdma_op = &host1x_cdma_ops;
+	host->cdma_pb_op = &host1x_pushbuffer_ops;
+	host->syncpt_op = &host1x_syncpt_ops;
+	host->intr_op = &host1x_intr_ops;
+	host->debug_op = &host1x_debug_ops;
+
+	return 0;
+}
diff --git a/drivers/gpu/host1x/hw/host1x05.h b/drivers/gpu/host1x/hw/host1x05.h
new file mode 100644
index 000000000000..a306d9c05cd5
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x05.h
@@ -0,0 +1,26 @@
+/*
+ * Host1x init for Tegra210 SoCs
+ *
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef HOST1X_HOST1X05_H
+#define HOST1X_HOST1X05_H
+
+struct host1x;
+
+int host1x05_init(struct host1x *host);
+
+#endif
diff --git a/drivers/gpu/host1x/hw/host1x05_hardware.h b/drivers/gpu/host1x/hw/host1x05_hardware.h
new file mode 100644
index 000000000000..2937ebb6be11
--- /dev/null
+++ b/drivers/gpu/host1x/hw/host1x05_hardware.h
@@ -0,0 +1,142 @@
+/*
+ * Tegra host1x Register Offsets for Tegra210
+ *
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __HOST1X_HOST1X05_HARDWARE_H
+#define __HOST1X_HOST1X05_HARDWARE_H
+
+#include <linux/types.h>
+#include <linux/bitops.h>
+
+#include "hw_host1x05_channel.h"
+#include "hw_host1x05_sync.h"
+#include "hw_host1x05_uclass.h"
+
+static inline u32 host1x_class_host_wait_syncpt(
+	unsigned indx, unsigned threshold)
+{
+	return host1x_uclass_wait_syncpt_indx_f(indx)
+		| host1x_uclass_wait_syncpt_thresh_f(threshold);
+}
+
+static inline u32 host1x_class_host_load_syncpt_base(
+	unsigned indx, unsigned threshold)
+{
+	return host1x_uclass_load_syncpt_base_base_indx_f(indx)
+		| host1x_uclass_load_syncpt_base_value_f(threshold);
+}
+
+static inline u32 host1x_class_host_wait_syncpt_base(
+	unsigned indx, unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_wait_syncpt_base_indx_f(indx)
+		| host1x_uclass_wait_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_wait_syncpt_base_offset_f(offset);
+}
+
+static inline u32 host1x_class_host_incr_syncpt_base(
+	unsigned base_indx, unsigned offset)
+{
+	return host1x_uclass_incr_syncpt_base_base_indx_f(base_indx)
+		| host1x_uclass_incr_syncpt_base_offset_f(offset);
+}
+
+static inline u32 host1x_class_host_incr_syncpt(
+	unsigned cond, unsigned indx)
+{
+	return host1x_uclass_incr_syncpt_cond_f(cond)
+		| host1x_uclass_incr_syncpt_indx_f(indx);
+}
+
+static inline u32 host1x_class_host_indoff_reg_write(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indbe_f(0xf)
+		| host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset);
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+static inline u32 host1x_class_host_indoff_reg_read(
+	unsigned mod_id, unsigned offset, bool auto_inc)
+{
+	u32 v = host1x_uclass_indoff_indmodid_f(mod_id)
+		| host1x_uclass_indoff_indroffset_f(offset)
+		| host1x_uclass_indoff_rwn_read_v();
+	if (auto_inc)
+		v |= host1x_uclass_indoff_autoinc_f(1);
+	return v;
+}
+
+/* cdma opcodes */
+static inline u32 host1x_opcode_setclass(
+	unsigned class_id, unsigned offset, unsigned mask)
+{
+	return (0 << 28) | (offset << 16) | (class_id << 6) | mask;
+}
+
+static inline u32 host1x_opcode_incr(unsigned offset, unsigned count)
+{
+	return (1 << 28) | (offset << 16) | count;
+}
+
+static inline u32 host1x_opcode_nonincr(unsigned offset, unsigned count)
+{
+	return (2 << 28) | (offset << 16) | count;
+}
+
+static inline u32 host1x_opcode_mask(unsigned offset, unsigned mask)
+{
+	return (3 << 28) | (offset << 16) | mask;
+}
+
+static inline u32 host1x_opcode_imm(unsigned offset, unsigned value)
+{
+	return (4 << 28) | (offset << 16) | value;
+}
+
+static inline u32 host1x_opcode_imm_incr_syncpt(unsigned cond, unsigned indx)
+{
+	return host1x_opcode_imm(host1x_uclass_incr_syncpt_r(),
+		host1x_class_host_incr_syncpt(cond, indx));
+}
+
+static inline u32 host1x_opcode_restart(unsigned address)
+{
+	return (5 << 28) | (address >> 4);
+}
+
+static inline u32 host1x_opcode_gather(unsigned count)
+{
+	return (6 << 28) | count;
+}
+
+static inline u32 host1x_opcode_gather_nonincr(unsigned offset,	unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | count;
+}
+
+static inline u32 host1x_opcode_gather_incr(unsigned offset, unsigned count)
+{
+	return (6 << 28) | (offset << 16) | BIT(15) | BIT(14) | count;
+}
+
+#define HOST1X_OPCODE_NOP host1x_opcode_nonincr(0, 0)
+
+#endif
diff --git a/drivers/gpu/host1x/hw/hw_host1x05_channel.h b/drivers/gpu/host1x/hw/hw_host1x05_channel.h
new file mode 100644
index 000000000000..fce6e2c1ff4c
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x05_channel.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef HOST1X_HW_HOST1X05_CHANNEL_H
+#define HOST1X_HW_HOST1X05_CHANNEL_H
+
+static inline u32 host1x_channel_fifostat_r(void)
+{
+	return 0x0;
+}
+#define HOST1X_CHANNEL_FIFOSTAT \
+	host1x_channel_fifostat_r()
+static inline u32 host1x_channel_fifostat_cfempty_v(u32 r)
+{
+	return (r >> 11) & 0x1;
+}
+#define HOST1X_CHANNEL_FIFOSTAT_CFEMPTY_V(r) \
+	host1x_channel_fifostat_cfempty_v(r)
+static inline u32 host1x_channel_dmastart_r(void)
+{
+	return 0x14;
+}
+#define HOST1X_CHANNEL_DMASTART \
+	host1x_channel_dmastart_r()
+static inline u32 host1x_channel_dmaput_r(void)
+{
+	return 0x18;
+}
+#define HOST1X_CHANNEL_DMAPUT \
+	host1x_channel_dmaput_r()
+static inline u32 host1x_channel_dmaget_r(void)
+{
+	return 0x1c;
+}
+#define HOST1X_CHANNEL_DMAGET \
+	host1x_channel_dmaget_r()
+static inline u32 host1x_channel_dmaend_r(void)
+{
+	return 0x20;
+}
+#define HOST1X_CHANNEL_DMAEND \
+	host1x_channel_dmaend_r()
+static inline u32 host1x_channel_dmactrl_r(void)
+{
+	return 0x24;
+}
+#define HOST1X_CHANNEL_DMACTRL \
+	host1x_channel_dmactrl_r()
+static inline u32 host1x_channel_dmactrl_dmastop(void)
+{
+	return 1 << 0;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMASTOP \
+	host1x_channel_dmactrl_dmastop()
+static inline u32 host1x_channel_dmactrl_dmastop_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMASTOP_V(r) \
+	host1x_channel_dmactrl_dmastop_v(r)
+static inline u32 host1x_channel_dmactrl_dmagetrst(void)
+{
+	return 1 << 1;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMAGETRST \
+	host1x_channel_dmactrl_dmagetrst()
+static inline u32 host1x_channel_dmactrl_dmainitget(void)
+{
+	return 1 << 2;
+}
+#define HOST1X_CHANNEL_DMACTRL_DMAINITGET \
+	host1x_channel_dmactrl_dmainitget()
+
+#endif
diff --git a/drivers/gpu/host1x/hw/hw_host1x05_sync.h b/drivers/gpu/host1x/hw/hw_host1x05_sync.h
new file mode 100644
index 000000000000..ca10eee5045c
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x05_sync.h
@@ -0,0 +1,243 @@
+/*
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef HOST1X_HW_HOST1X05_SYNC_H
+#define HOST1X_HW_HOST1X05_SYNC_H
+
+#define REGISTER_STRIDE	4
+
+static inline u32 host1x_sync_syncpt_r(unsigned int id)
+{
+	return 0xf80 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT(id) \
+	host1x_sync_syncpt_r(id)
+static inline u32 host1x_sync_syncpt_thresh_cpu0_int_status_r(unsigned int id)
+{
+	return 0xe80 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_CPU0_INT_STATUS(id) \
+	host1x_sync_syncpt_thresh_cpu0_int_status_r(id)
+static inline u32 host1x_sync_syncpt_thresh_int_disable_r(unsigned int id)
+{
+	return 0xf00 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_INT_DISABLE(id) \
+	host1x_sync_syncpt_thresh_int_disable_r(id)
+static inline u32 host1x_sync_syncpt_thresh_int_enable_cpu0_r(unsigned int id)
+{
+	return 0xf20 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_THRESH_INT_ENABLE_CPU0(id) \
+	host1x_sync_syncpt_thresh_int_enable_cpu0_r(id)
+static inline u32 host1x_sync_cf_setup_r(unsigned int channel)
+{
+	return 0xc00 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CF_SETUP(channel) \
+	host1x_sync_cf_setup_r(channel)
+static inline u32 host1x_sync_cf_setup_base_v(u32 r)
+{
+	return (r >> 0) & 0x3ff;
+}
+#define HOST1X_SYNC_CF_SETUP_BASE_V(r) \
+	host1x_sync_cf_setup_base_v(r)
+static inline u32 host1x_sync_cf_setup_limit_v(u32 r)
+{
+	return (r >> 16) & 0x3ff;
+}
+#define HOST1X_SYNC_CF_SETUP_LIMIT_V(r) \
+	host1x_sync_cf_setup_limit_v(r)
+static inline u32 host1x_sync_cmdproc_stop_r(void)
+{
+	return 0xac;
+}
+#define HOST1X_SYNC_CMDPROC_STOP \
+	host1x_sync_cmdproc_stop_r()
+static inline u32 host1x_sync_ch_teardown_r(void)
+{
+	return 0xb0;
+}
+#define HOST1X_SYNC_CH_TEARDOWN \
+	host1x_sync_ch_teardown_r()
+static inline u32 host1x_sync_usec_clk_r(void)
+{
+	return 0x1a4;
+}
+#define HOST1X_SYNC_USEC_CLK \
+	host1x_sync_usec_clk_r()
+static inline u32 host1x_sync_ctxsw_timeout_cfg_r(void)
+{
+	return 0x1a8;
+}
+#define HOST1X_SYNC_CTXSW_TIMEOUT_CFG \
+	host1x_sync_ctxsw_timeout_cfg_r()
+static inline u32 host1x_sync_ip_busy_timeout_r(void)
+{
+	return 0x1bc;
+}
+#define HOST1X_SYNC_IP_BUSY_TIMEOUT \
+	host1x_sync_ip_busy_timeout_r()
+static inline u32 host1x_sync_mlock_owner_r(unsigned int id)
+{
+	return 0x340 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_MLOCK_OWNER(id) \
+	host1x_sync_mlock_owner_r(id)
+static inline u32 host1x_sync_mlock_owner_chid_v(u32 r)
+{
+	return (r >> 8) & 0xf;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CHID_V(v) \
+	host1x_sync_mlock_owner_chid_v(v)
+static inline u32 host1x_sync_mlock_owner_cpu_owns_v(u32 r)
+{
+	return (r >> 1) & 0x1;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CPU_OWNS_V(r) \
+	host1x_sync_mlock_owner_cpu_owns_v(r)
+static inline u32 host1x_sync_mlock_owner_ch_owns_v(u32 r)
+{
+	return (r >> 0) & 0x1;
+}
+#define HOST1X_SYNC_MLOCK_OWNER_CH_OWNS_V(r) \
+	host1x_sync_mlock_owner_ch_owns_v(r)
+static inline u32 host1x_sync_syncpt_int_thresh_r(unsigned int id)
+{
+	return 0x1380 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_INT_THRESH(id) \
+	host1x_sync_syncpt_int_thresh_r(id)
+static inline u32 host1x_sync_syncpt_base_r(unsigned int id)
+{
+	return 0x600 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_BASE(id) \
+	host1x_sync_syncpt_base_r(id)
+static inline u32 host1x_sync_syncpt_cpu_incr_r(unsigned int id)
+{
+	return 0xf60 + id * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_SYNCPT_CPU_INCR(id) \
+	host1x_sync_syncpt_cpu_incr_r(id)
+static inline u32 host1x_sync_cbread_r(unsigned int channel)
+{
+	return 0xc80 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CBREAD(channel) \
+	host1x_sync_cbread_r(channel)
+static inline u32 host1x_sync_cfpeek_ctrl_r(void)
+{
+	return 0x74c;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL \
+	host1x_sync_cfpeek_ctrl_r()
+static inline u32 host1x_sync_cfpeek_ctrl_addr_f(u32 v)
+{
+	return (v & 0x3ff) << 0;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_ADDR_F(v) \
+	host1x_sync_cfpeek_ctrl_addr_f(v)
+static inline u32 host1x_sync_cfpeek_ctrl_channr_f(u32 v)
+{
+	return (v & 0xf) << 16;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_CHANNR_F(v) \
+	host1x_sync_cfpeek_ctrl_channr_f(v)
+static inline u32 host1x_sync_cfpeek_ctrl_ena_f(u32 v)
+{
+	return (v & 0x1) << 31;
+}
+#define HOST1X_SYNC_CFPEEK_CTRL_ENA_F(v) \
+	host1x_sync_cfpeek_ctrl_ena_f(v)
+static inline u32 host1x_sync_cfpeek_read_r(void)
+{
+	return 0x750;
+}
+#define HOST1X_SYNC_CFPEEK_READ \
+	host1x_sync_cfpeek_read_r()
+static inline u32 host1x_sync_cfpeek_ptrs_r(void)
+{
+	return 0x754;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS \
+	host1x_sync_cfpeek_ptrs_r()
+static inline u32 host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(u32 r)
+{
+	return (r >> 0) & 0x3ff;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS_CF_RD_PTR_V(r) \
+	host1x_sync_cfpeek_ptrs_cf_rd_ptr_v(r)
+static inline u32 host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(u32 r)
+{
+	return (r >> 16) & 0x3ff;
+}
+#define HOST1X_SYNC_CFPEEK_PTRS_CF_WR_PTR_V(r) \
+	host1x_sync_cfpeek_ptrs_cf_wr_ptr_v(r)
+static inline u32 host1x_sync_cbstat_r(unsigned int channel)
+{
+	return 0xcc0 + channel * REGISTER_STRIDE;
+}
+#define HOST1X_SYNC_CBSTAT(channel) \
+	host1x_sync_cbstat_r(channel)
+static inline u32 host1x_sync_cbstat_cboffset_v(u32 r)
+{
+	return (r >> 0) & 0xffff;
+}
+#define HOST1X_SYNC_CBSTAT_CBOFFSET_V(r) \
+	host1x_sync_cbstat_cboffset_v(r)
+static inline u32 host1x_sync_cbstat_cbclass_v(u32 r)
+{
+	return (r >> 16) & 0x3ff;
+}
+#define HOST1X_SYNC_CBSTAT_CBCLASS_V(r) \
+	host1x_sync_cbstat_cbclass_v(r)
+
+#endif
diff --git a/drivers/gpu/host1x/hw/hw_host1x05_uclass.h b/drivers/gpu/host1x/hw/hw_host1x05_uclass.h
new file mode 100644
index 000000000000..0c411da6bc41
--- /dev/null
+++ b/drivers/gpu/host1x/hw/hw_host1x05_uclass.h
@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2015 NVIDIA Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+ /*
+  * Function naming determines intended use:
+  *
+  *     <x>_r(void) : Returns the offset for register <x>.
+  *
+  *     <x>_w(void) : Returns the word offset for word (4 byte) element <x>.
+  *
+  *     <x>_<y>_s(void) : Returns size of field <y> of register <x> in bits.
+  *
+  *     <x>_<y>_f(u32 v) : Returns a value based on 'v' which has been shifted
+  *         and masked to place it at field <y> of register <x>.  This value
+  *         can be |'d with others to produce a full register value for
+  *         register <x>.
+  *
+  *     <x>_<y>_m(void) : Returns a mask for field <y> of register <x>.  This
+  *         value can be ~'d and then &'d to clear the value of field <y> for
+  *         register <x>.
+  *
+  *     <x>_<y>_<z>_f(void) : Returns the constant value <z> after being shifted
+  *         to place it at field <y> of register <x>.  This value can be |'d
+  *         with others to produce a full register value for <x>.
+  *
+  *     <x>_<y>_v(u32 r) : Returns the value of field <y> from a full register
+  *         <x> value 'r' after being shifted to place its LSB at bit 0.
+  *         This value is suitable for direct comparison with other unshifted
+  *         values appropriate for use in field <y> of register <x>.
+  *
+  *     <x>_<y>_<z>_v(void) : Returns the constant value for <z> defined for
+  *         field <y> of register <x>.  This value is suitable for direct
+  *         comparison with unshifted values appropriate for use in field <y>
+  *         of register <x>.
+  */
+
+#ifndef HOST1X_HW_HOST1X05_UCLASS_H
+#define HOST1X_HW_HOST1X05_UCLASS_H
+
+static inline u32 host1x_uclass_incr_syncpt_r(void)
+{
+	return 0x0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT \
+	host1x_uclass_incr_syncpt_r()
+static inline u32 host1x_uclass_incr_syncpt_cond_f(u32 v)
+{
+	return (v & 0xff) << 8;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_COND_F(v) \
+	host1x_uclass_incr_syncpt_cond_f(v)
+static inline u32 host1x_uclass_incr_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_INDX_F(v) \
+	host1x_uclass_incr_syncpt_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_r(void)
+{
+	return 0x8;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT \
+	host1x_uclass_wait_syncpt_r()
+static inline u32 host1x_uclass_wait_syncpt_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_INDX_F(v) \
+	host1x_uclass_wait_syncpt_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_thresh_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_THRESH_F(v) \
+	host1x_uclass_wait_syncpt_thresh_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_r(void)
+{
+	return 0x9;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE \
+	host1x_uclass_wait_syncpt_base_r()
+static inline u32 host1x_uclass_wait_syncpt_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_INDX_F(v) \
+	host1x_uclass_wait_syncpt_base_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 16;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_wait_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_wait_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffff) << 0;
+}
+#define HOST1X_UCLASS_WAIT_SYNCPT_BASE_OFFSET_F(v) \
+	host1x_uclass_wait_syncpt_base_offset_f(v)
+static inline u32 host1x_uclass_load_syncpt_base_r(void)
+{
+	return 0xb;
+}
+#define HOST1X_UCLASS_LOAD_SYNCPT_BASE \
+	host1x_uclass_load_syncpt_base_r()
+static inline u32 host1x_uclass_load_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_LOAD_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_load_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_load_syncpt_base_value_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_LOAD_SYNCPT_BASE_VALUE_F(v) \
+	host1x_uclass_load_syncpt_base_value_f(v)
+static inline u32 host1x_uclass_incr_syncpt_base_base_indx_f(u32 v)
+{
+	return (v & 0xff) << 24;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_BASE_BASE_INDX_F(v) \
+	host1x_uclass_incr_syncpt_base_base_indx_f(v)
+static inline u32 host1x_uclass_incr_syncpt_base_offset_f(u32 v)
+{
+	return (v & 0xffffff) << 0;
+}
+#define HOST1X_UCLASS_INCR_SYNCPT_BASE_OFFSET_F(v) \
+	host1x_uclass_incr_syncpt_base_offset_f(v)
+static inline u32 host1x_uclass_indoff_r(void)
+{
+	return 0x2d;
+}
+#define HOST1X_UCLASS_INDOFF \
+	host1x_uclass_indoff_r()
+static inline u32 host1x_uclass_indoff_indbe_f(u32 v)
+{
+	return (v & 0xf) << 28;
+}
+#define HOST1X_UCLASS_INDOFF_INDBE_F(v) \
+	host1x_uclass_indoff_indbe_f(v)
+static inline u32 host1x_uclass_indoff_autoinc_f(u32 v)
+{
+	return (v & 0x1) << 27;
+}
+#define HOST1X_UCLASS_INDOFF_AUTOINC_F(v) \
+	host1x_uclass_indoff_autoinc_f(v)
+static inline u32 host1x_uclass_indoff_indmodid_f(u32 v)
+{
+	return (v & 0xff) << 18;
+}
+#define HOST1X_UCLASS_INDOFF_INDMODID_F(v) \
+	host1x_uclass_indoff_indmodid_f(v)
+static inline u32 host1x_uclass_indoff_indroffset_f(u32 v)
+{
+	return (v & 0xffff) << 2;
+}
+#define HOST1X_UCLASS_INDOFF_INDROFFSET_F(v) \
+	host1x_uclass_indoff_indroffset_f(v)
+static inline u32 host1x_uclass_indoff_rwn_read_v(void)
+{
+	return 1;
+}
+#define HOST1X_UCLASS_INDOFF_INDROFFSET_F(v) \
+	host1x_uclass_indoff_indroffset_f(v)
+
+#endif
diff --git a/drivers/gpu/ipu-v3/ipu-common.c b/drivers/gpu/ipu-v3/ipu-common.c
index f2e13eb8339f..e00db3f510dd 100644
--- a/drivers/gpu/ipu-v3/ipu-common.c
+++ b/drivers/gpu/ipu-v3/ipu-common.c
@@ -1050,6 +1050,17 @@ static int ipu_add_client_devices(struct ipu_soc *ipu, unsigned long ipu_base)
 	for (i = 0; i < ARRAY_SIZE(client_reg); i++) {
 		const struct ipu_platform_reg *reg = &client_reg[i];
 		struct platform_device *pdev;
+		struct device_node *of_node;
+
+		/* Associate subdevice with the corresponding port node */
+		of_node = of_graph_get_port_by_id(dev->of_node, i);
+		if (!of_node) {
+			dev_info(dev,
+				 "no port@%d node in %s, not using %s%d\n",
+				 i, dev->of_node->full_name,
+				 (i / 2) ? "DI" : "CSI", i % 2);
+			continue;
+		}
 
 		pdev = platform_device_alloc(reg->name, id++);
 		if (!pdev) {
@@ -1057,17 +1068,9 @@ static int ipu_add_client_devices(struct ipu_soc *ipu, unsigned long ipu_base)
 			goto err_register;
 		}
 
+		pdev->dev.of_node = of_node;
 		pdev->dev.parent = dev;
 
-		/* Associate subdevice with the corresponding port node */
-		pdev->dev.of_node = of_graph_get_port_by_id(dev->of_node, i);
-		if (!pdev->dev.of_node) {
-			dev_err(dev, "missing port@%d node in %s\n", i,
-				dev->of_node->full_name);
-			ret = -ENODEV;
-			goto err_register;
-		}
-
 		ret = platform_device_add_data(pdev, &reg->pdata,
 					       sizeof(reg->pdata));
 		if (!ret)
@@ -1289,10 +1292,6 @@ static int ipu_probe(struct platform_device *pdev)
 	ipu->irq_sync = irq_sync;
 	ipu->irq_err = irq_err;
 
-	ret = ipu_irq_init(ipu);
-	if (ret)
-		goto out_failed_irq;
-
 	ret = device_reset(&pdev->dev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to reset: %d\n", ret);
@@ -1302,6 +1301,10 @@ static int ipu_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_failed_reset;
 
+	ret = ipu_irq_init(ipu);
+	if (ret)
+		goto out_failed_irq;
+
 	/* Set MCU_T to divide MCU access window into 2 */
 	ipu_cm_write(ipu, 0x00400000L | (IPU_MCU_T_DEFAULT << 18),
 			IPU_DISP_GEN);
@@ -1324,9 +1327,9 @@ static int ipu_probe(struct platform_device *pdev)
 failed_add_clients:
 	ipu_submodules_exit(ipu);
 failed_submodules_init:
-out_failed_reset:
 	ipu_irq_exit(ipu);
 out_failed_irq:
+out_failed_reset:
 	clk_disable_unprepare(ipu->clk);
 	return ret;
 }
diff --git a/drivers/gpu/ipu-v3/ipu-cpmem.c b/drivers/gpu/ipu-v3/ipu-cpmem.c
index 63eb16bf2cf0..883a314cd83a 100644
--- a/drivers/gpu/ipu-v3/ipu-cpmem.c
+++ b/drivers/gpu/ipu-v3/ipu-cpmem.c
@@ -161,7 +161,7 @@ static u32 ipu_ch_param_read_field(struct ipuv3_channel *ch, u32 wbs)
  * The DRM pixel formats and IPU internal representation are ordered the other
  * way around, with the first named component ordered at the most significant
  * bits. Further, V4L2 formats are not well defined:
- *     http://linuxtv.org/downloads/v4l-dvb-apis/packed-rgb.html
+ *     https://linuxtv.org/downloads/v4l-dvb-apis/packed-rgb.html
  * We choose the interpretation which matches GStreamer behavior.
  */
 static int v4l2_pix_fmt_to_drm_fourcc(u32 pixelformat)
diff --git a/drivers/gpu/vga/vga_switcheroo.c b/drivers/gpu/vga/vga_switcheroo.c
index 41edd5a3f100..665ab9fd0e01 100644
--- a/drivers/gpu/vga/vga_switcheroo.c
+++ b/drivers/gpu/vga/vga_switcheroo.c
@@ -36,6 +36,7 @@
 #include <linux/fs.h>
 #include <linux/module.h>
 #include <linux/pci.h>
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 #include <linux/seq_file.h>
 #include <linux/uaccess.h>
@@ -63,7 +64,7 @@
  * for the inactive GPU.) Also, muxes are often used to cut power to the
  * discrete GPU while it is not used.
  *
- * DRM drivers register GPUs with vga_switcheroo, these are heretoforth called
+ * DRM drivers register GPUs with vga_switcheroo, these are henceforth called
  * clients. The mux is called the handler. Muxless machines also register a
  * handler to control the power state of the discrete GPU, its ->switchto
  * callback is a no-op for obvious reasons. The discrete GPU is often equipped
@@ -918,17 +919,17 @@ int vga_switcheroo_init_domain_pm_ops(struct device *dev,
 		domain->ops.runtime_suspend = vga_switcheroo_runtime_suspend;
 		domain->ops.runtime_resume = vga_switcheroo_runtime_resume;
 
-		dev->pm_domain = domain;
+		dev_pm_domain_set(dev, domain);
 		return 0;
 	}
-	dev->pm_domain = NULL;
+	dev_pm_domain_set(dev, NULL);
 	return -EINVAL;
 }
 EXPORT_SYMBOL(vga_switcheroo_init_domain_pm_ops);
 
 void vga_switcheroo_fini_domain_pm_ops(struct device *dev)
 {
-	dev->pm_domain = NULL;
+	dev_pm_domain_set(dev, NULL);
 }
 EXPORT_SYMBOL(vga_switcheroo_fini_domain_pm_ops);
 
@@ -989,10 +990,10 @@ vga_switcheroo_init_domain_pm_optimus_hdmi_audio(struct device *dev,
 		domain->ops.runtime_resume =
 			vga_switcheroo_runtime_resume_hdmi_audio;
 
-		dev->pm_domain = domain;
+		dev_pm_domain_set(dev, domain);
 		return 0;
 	}
-	dev->pm_domain = NULL;
+	dev_pm_domain_set(dev, NULL);
 	return -EINVAL;
 }
 EXPORT_SYMBOL(vga_switcheroo_init_domain_pm_optimus_hdmi_audio);
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index 9abcaa53bd25..f17cb0431833 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -1163,12 +1163,8 @@ done:
 
 static unsigned int vga_arb_fpoll(struct file *file, poll_table *wait)
 {
-	struct vga_arb_private *priv = file->private_data;
-
 	pr_debug("%s\n", __func__);
 
-	if (priv == NULL)
-		return -ENODEV;
 	poll_wait(file, &vga_wait_queue, wait);
 	return POLLIN;
 }
@@ -1209,9 +1205,6 @@ static int vga_arb_release(struct inode *inode, struct file *file)
 
 	pr_debug("%s\n", __func__);
 
-	if (priv == NULL)
-		return -ENODEV;
-
 	spin_lock_irqsave(&vga_user_lock, flags);
 	list_del(&priv->list);
 	for (i = 0; i < MAX_USER_CARDS; i++) {
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index c6f7a694f67a..7e89288b1537 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -625,7 +625,7 @@ static void hid_close_report(struct hid_device *device)
 
 static void hid_device_release(struct device *dev)
 {
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 
 	hid_close_report(hid);
 	kfree(hid->dev_rdesc);
@@ -1571,8 +1571,8 @@ read_report_descriptor(struct file *filp, struct kobject *kobj,
 		struct bin_attribute *attr,
 		char *buf, loff_t off, size_t count)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct device *dev = kobj_to_dev(kobj);
+	struct hid_device *hdev = to_hid_device(dev);
 
 	if (off >= hdev->rsize)
 		return 0;
@@ -1589,7 +1589,7 @@ static ssize_t
 show_country(struct device *dev, struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 
 	return sprintf(buf, "%02x\n", hdev->country & 0xff);
 }
@@ -1691,11 +1691,6 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 		hid_warn(hdev,
 			 "can't create sysfs country code attribute err: %d\n", ret);
 
-	ret = device_create_bin_file(&hdev->dev, &dev_bin_attr_report_desc);
-	if (ret)
-		hid_warn(hdev,
-			 "can't create sysfs report descriptor attribute err: %d\n", ret);
-
 	hid_info(hdev, "%s: %s HID v%x.%02x %s [%s] on %s\n",
 		 buf, bus, hdev->version >> 8, hdev->version & 0xff,
 		 type, hdev->name, hdev->phys);
@@ -1707,7 +1702,6 @@ EXPORT_SYMBOL_GPL(hid_connect);
 void hid_disconnect(struct hid_device *hdev)
 {
 	device_remove_file(&hdev->dev, &dev_attr_country);
-	device_remove_bin_file(&hdev->dev, &dev_bin_attr_report_desc);
 	if (hdev->claimed & HID_CLAIMED_INPUT)
 		hidinput_disconnect(hdev);
 	if (hdev->claimed & HID_CLAIMED_HIDDEV)
@@ -1902,6 +1896,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_RUMBLEPAD2_2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G29_WHEEL) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G920_WHEEL) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_F3D) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_WINGMAN_FFG ) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_FORCE3D_PRO) },
@@ -2076,7 +2071,7 @@ struct hid_dynid {
 static ssize_t store_new_id(struct device_driver *drv, const char *buf,
 		size_t count)
 {
-	struct hid_driver *hdrv = container_of(drv, struct hid_driver, driver);
+	struct hid_driver *hdrv = to_hid_driver(drv);
 	struct hid_dynid *dynid;
 	__u32 bus, vendor, product;
 	unsigned long driver_data = 0;
@@ -2138,17 +2133,16 @@ static const struct hid_device_id *hid_match_device(struct hid_device *hdev,
 
 static int hid_bus_match(struct device *dev, struct device_driver *drv)
 {
-	struct hid_driver *hdrv = container_of(drv, struct hid_driver, driver);
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_driver *hdrv = to_hid_driver(drv);
+	struct hid_device *hdev = to_hid_device(dev);
 
 	return hid_match_device(hdev, hdrv) != NULL;
 }
 
 static int hid_device_probe(struct device *dev)
 {
-	struct hid_driver *hdrv = container_of(dev->driver,
-			struct hid_driver, driver);
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_driver *hdrv = to_hid_driver(dev->driver);
+	struct hid_device *hdev = to_hid_device(dev);
 	const struct hid_device_id *id;
 	int ret = 0;
 
@@ -2190,7 +2184,7 @@ unlock_driver_lock:
 
 static int hid_device_remove(struct device *dev)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct hid_driver *hdrv;
 	int ret = 0;
 
@@ -2223,12 +2217,9 @@ static ssize_t modalias_show(struct device *dev, struct device_attribute *a,
 			     char *buf)
 {
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
-	int len;
-
-	len = snprintf(buf, PAGE_SIZE, "hid:b%04Xg%04Xv%08Xp%08X\n",
-		       hdev->bus, hdev->group, hdev->vendor, hdev->product);
 
-	return (len >= PAGE_SIZE) ? (PAGE_SIZE - 1) : len;
+	return scnprintf(buf, PAGE_SIZE, "hid:b%04Xg%04Xv%08Xp%08X\n",
+			 hdev->bus, hdev->group, hdev->vendor, hdev->product);
 }
 static DEVICE_ATTR_RO(modalias);
 
@@ -2236,11 +2227,19 @@ static struct attribute *hid_dev_attrs[] = {
 	&dev_attr_modalias.attr,
 	NULL,
 };
-ATTRIBUTE_GROUPS(hid_dev);
+static struct bin_attribute *hid_dev_bin_attrs[] = {
+	&dev_bin_attr_report_desc,
+	NULL
+};
+static const struct attribute_group hid_dev_group = {
+	.attrs = hid_dev_attrs,
+	.bin_attrs = hid_dev_bin_attrs,
+};
+__ATTRIBUTE_GROUPS(hid_dev);
 
 static int hid_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);	
 
 	if (add_uevent_var(env, "HID_ID=%04X:%08X:%08X",
 			hdev->bus, hdev->vendor, hdev->product))
@@ -2408,6 +2407,7 @@ static const struct hid_device_id hid_ignore_list[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICKIT1) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICKIT2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICK16F1454) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_MICROCHIP, USB_DEVICE_ID_PICK16F1454_V2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NATIONAL_SEMICONDUCTOR, USB_DEVICE_ID_N_S_HARMONY) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ONTRAK, USB_DEVICE_ID_ONTRAK_ADU100) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ONTRAK, USB_DEVICE_ID_ONTRAK_ADU100 + 20) },
@@ -2660,6 +2660,7 @@ struct hid_device *hid_allocate_device(void)
 	device_initialize(&hdev->dev);
 	hdev->dev.release = hid_device_release;
 	hdev->dev.bus = &hid_bus_type;
+	device_enable_async_suspend(&hdev->dev);
 
 	hid_close_report(hdev);
 
diff --git a/drivers/hid/hid-corsair.c b/drivers/hid/hid-corsair.c
index bcefb9ebb026..58551964ce86 100644
--- a/drivers/hid/hid-corsair.c
+++ b/drivers/hid/hid-corsair.c
@@ -655,18 +655,7 @@ static struct hid_driver corsair_driver = {
 	.input_mapping = corsair_input_mapping,
 };
 
-static int __init corsair_init(void)
-{
-	return hid_register_driver(&corsair_driver);
-}
-
-static void corsair_exit(void)
-{
-	hid_unregister_driver(&corsair_driver);
-}
-
-module_init(corsair_init);
-module_exit(corsair_exit);
+module_hid_driver(corsair_driver);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Clement Vuchener");
diff --git a/drivers/hid/hid-cp2112.c b/drivers/hid/hid-cp2112.c
index 7afc3fcc122c..086d8a507157 100644
--- a/drivers/hid/hid-cp2112.c
+++ b/drivers/hid/hid-cp2112.c
@@ -24,7 +24,7 @@
  *   http://www.silabs.com/Support%20Documents/TechnicalDocs/AN495.pdf
  */
 
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/hid.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
@@ -169,8 +169,7 @@ MODULE_PARM_DESC(gpio_push_pull, "GPIO push-pull configuration bitmask");
 
 static int cp2112_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct cp2112_device *dev = container_of(chip, struct cp2112_device,
-						 gc);
+	struct cp2112_device *dev = gpiochip_get_data(chip);
 	struct hid_device *hdev = dev->hdev;
 	u8 buf[5];
 	int ret;
@@ -198,8 +197,7 @@ static int cp2112_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static void cp2112_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct cp2112_device *dev = container_of(chip, struct cp2112_device,
-						 gc);
+	struct cp2112_device *dev = gpiochip_get_data(chip);
 	struct hid_device *hdev = dev->hdev;
 	u8 buf[3];
 	int ret;
@@ -216,8 +214,7 @@ static void cp2112_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int cp2112_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct cp2112_device *dev = container_of(chip, struct cp2112_device,
-						 gc);
+	struct cp2112_device *dev = gpiochip_get_data(chip);
 	struct hid_device *hdev = dev->hdev;
 	u8 buf[2];
 	int ret;
@@ -235,8 +232,7 @@ static int cp2112_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int cp2112_gpio_direction_output(struct gpio_chip *chip,
 					unsigned offset, int value)
 {
-	struct cp2112_device *dev = container_of(chip, struct cp2112_device,
-						 gc);
+	struct cp2112_device *dev = gpiochip_get_data(chip);
 	struct hid_device *hdev = dev->hdev;
 	u8 buf[5];
 	int ret;
@@ -807,7 +803,7 @@ static ssize_t name##_store(struct device *kdev, \
 			    struct device_attribute *attr, const char *buf, \
 			    size_t count) \
 { \
-	struct hid_device *hdev = container_of(kdev, struct hid_device, dev); \
+	struct hid_device *hdev = to_hid_device(kdev); \
 	struct cp2112_usb_config_report cfg; \
 	int ret = cp2112_get_usb_config(hdev, &cfg); \
 	if (ret) \
@@ -822,7 +818,7 @@ static ssize_t name##_store(struct device *kdev, \
 static ssize_t name##_show(struct device *kdev, \
 			   struct device_attribute *attr, char *buf) \
 { \
-	struct hid_device *hdev = container_of(kdev, struct hid_device, dev); \
+	struct hid_device *hdev = to_hid_device(kdev); \
 	struct cp2112_usb_config_report cfg; \
 	int ret = cp2112_get_usb_config(hdev, &cfg); \
 	if (ret) \
@@ -887,7 +883,7 @@ static ssize_t pstr_store(struct device *kdev,
 			  struct device_attribute *kattr, const char *buf,
 			  size_t count)
 {
-	struct hid_device *hdev = container_of(kdev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(kdev);
 	struct cp2112_pstring_attribute *attr =
 		container_of(kattr, struct cp2112_pstring_attribute, attr);
 	struct cp2112_string_report report;
@@ -918,7 +914,7 @@ static ssize_t pstr_store(struct device *kdev,
 static ssize_t pstr_show(struct device *kdev,
 			 struct device_attribute *kattr, char *buf)
 {
-	struct hid_device *hdev = container_of(kdev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(kdev);
 	struct cp2112_pstring_attribute *attr =
 		container_of(kattr, struct cp2112_pstring_attribute, attr);
 	struct cp2112_string_report report;
@@ -1104,9 +1100,9 @@ static int cp2112_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	dev->gc.base			= -1;
 	dev->gc.ngpio			= 8;
 	dev->gc.can_sleep		= 1;
-	dev->gc.dev			= &hdev->dev;
+	dev->gc.parent			= &hdev->dev;
 
-	ret = gpiochip_add(&dev->gc);
+	ret = gpiochip_add_data(&dev->gc, dev);
 	if (ret < 0) {
 		hid_err(hdev, "error registering gpio chip\n");
 		goto err_free_i2c;
diff --git a/drivers/hid/hid-debug.c b/drivers/hid/hid-debug.c
index 2886b645ced7..acfb522a432a 100644
--- a/drivers/hid/hid-debug.c
+++ b/drivers/hid/hid-debug.c
@@ -659,13 +659,13 @@ EXPORT_SYMBOL_GPL(hid_dump_device);
 /* enqueue string to 'events' ring buffer */
 void hid_debug_event(struct hid_device *hdev, char *buf)
 {
-	int i;
+	unsigned i;
 	struct hid_debug_list *list;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hdev->debug_list_lock, flags);
 	list_for_each_entry(list, &hdev->debug_list, node) {
-		for (i = 0; i < strlen(buf); i++)
+		for (i = 0; buf[i]; i++)
 			list->hid_debug_buf[(list->tail + i) % HID_DEBUG_BUFSIZE] =
 				buf[i];
 		list->tail = (list->tail + i) % HID_DEBUG_BUFSIZE;
diff --git a/drivers/hid/hid-gt683r.c b/drivers/hid/hid-gt683r.c
index 0d6f135e266c..a298fbd8db6b 100644
--- a/drivers/hid/hid-gt683r.c
+++ b/drivers/hid/hid-gt683r.c
@@ -70,7 +70,7 @@ static void gt683r_brightness_set(struct led_classdev *led_cdev,
 {
 	int i;
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct gt683r_led *led = hid_get_drvdata(hdev);
 
 	for (i = 0; i < GT683R_LED_COUNT; i++) {
@@ -89,8 +89,7 @@ static ssize_t mode_show(struct device *dev,
 				char *buf)
 {
 	u8 sysfs_mode;
-	struct hid_device *hdev = container_of(dev->parent,
-					struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev->parent);
 	struct gt683r_led *led = hid_get_drvdata(hdev);
 
 	if (led->mode == GT683R_LED_NORMAL)
@@ -108,8 +107,7 @@ static ssize_t mode_store(struct device *dev,
 				const char *buf, size_t count)
 {
 	u8 sysfs_mode;
-	struct hid_device *hdev = container_of(dev->parent,
-					struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev->parent);
 	struct gt683r_led *led = hid_get_drvdata(hdev);
 
 
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 8b78a7f1f779..b6ff6e78ac54 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -510,6 +510,7 @@
 #define USB_VENDOR_ID_ITE               0x048d
 #define USB_DEVICE_ID_ITE_LENOVO_YOGA   0x8386
 #define USB_DEVICE_ID_ITE_LENOVO_YOGA2  0x8350
+#define USB_DEVICE_ID_ITE_LENOVO_YOGA900	0x8396
 
 #define USB_VENDOR_ID_JABRA		0x0b0e
 #define USB_DEVICE_ID_JABRA_SPEAK_410	0x0412
@@ -615,6 +616,7 @@
 #define USB_DEVICE_ID_LOGITECH_RUMBLEPAD2	0xc218
 #define USB_DEVICE_ID_LOGITECH_RUMBLEPAD2_2	0xc219
 #define USB_DEVICE_ID_LOGITECH_G29_WHEEL	0xc24f
+#define USB_DEVICE_ID_LOGITECH_G920_WHEEL	0xc262
 #define USB_DEVICE_ID_LOGITECH_WINGMAN_F3D	0xc283
 #define USB_DEVICE_ID_LOGITECH_FORCE3D_PRO	0xc286
 #define USB_DEVICE_ID_LOGITECH_FLIGHT_SYSTEM_G940	0xc287
@@ -664,6 +666,7 @@
 #define USB_DEVICE_ID_PICOLCD		0xc002
 #define USB_DEVICE_ID_PICOLCD_BOOTLOADER	0xf002
 #define USB_DEVICE_ID_PICK16F1454	0x0042
+#define USB_DEVICE_ID_PICK16F1454_V2	0xf2f7
 
 #define USB_VENDOR_ID_MICROSOFT		0x045e
 #define USB_DEVICE_ID_SIDEWINDER_GV	0x003b
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 2ba6bf69b7d0..bcfaf32d9e5e 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -303,6 +303,7 @@ static enum power_supply_property hidinput_battery_props[] = {
 
 #define HID_BATTERY_QUIRK_PERCENT	(1 << 0) /* always reports percent */
 #define HID_BATTERY_QUIRK_FEATURE	(1 << 1) /* ask for feature report */
+#define HID_BATTERY_QUIRK_IGNORE	(1 << 2) /* completely ignore the battery */
 
 static const struct hid_device_id hid_battery_quirks[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
@@ -320,6 +321,9 @@ static const struct hid_device_id hid_battery_quirks[] = {
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_APPLE,
 		USB_DEVICE_ID_APPLE_ALU_WIRELESS_ANSI),
 	  HID_BATTERY_QUIRK_PERCENT | HID_BATTERY_QUIRK_FEATURE },
+	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ELECOM,
+		USB_DEVICE_ID_ELECOM_BM084),
+	  HID_BATTERY_QUIRK_IGNORE },
 	{}
 };
 
@@ -408,6 +412,14 @@ static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type,
 	if (dev->battery != NULL)
 		goto out;	/* already initialized? */
 
+	quirks = find_battery_quirk(dev);
+
+	hid_dbg(dev, "device %x:%x:%x %d quirks %d\n",
+		dev->bus, dev->vendor, dev->product, dev->version, quirks);
+
+	if (quirks & HID_BATTERY_QUIRK_IGNORE)
+		goto out;
+
 	psy_desc = kzalloc(sizeof(*psy_desc), GFP_KERNEL);
 	if (psy_desc == NULL)
 		goto out;
@@ -424,11 +436,6 @@ static bool hidinput_setup_battery(struct hid_device *dev, unsigned report_type,
 	psy_desc->use_for_apm = 0;
 	psy_desc->get_property = hidinput_get_battery_property;
 
-	quirks = find_battery_quirk(dev);
-
-	hid_dbg(dev, "device %x:%x:%x %d quirks %d\n",
-		dev->bus, dev->vendor, dev->product, dev->version, quirks);
-
 	min = field->logical_minimum;
 	max = field->logical_maximum;
 
@@ -960,6 +967,10 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		goto ignore;
 
 	case HID_UP_LOGIVENDOR:
+		/* intentional fallback */
+	case HID_UP_LOGIVENDOR2:
+		/* intentional fallback */
+	case HID_UP_LOGIVENDOR3:
 		goto ignore;
 
 	case HID_UP_PID:
diff --git a/drivers/hid/hid-lenovo.c b/drivers/hid/hid-lenovo.c
index 8979f1fd5208..0125e356bd8d 100644
--- a/drivers/hid/hid-lenovo.c
+++ b/drivers/hid/hid-lenovo.c
@@ -220,7 +220,7 @@ static ssize_t attr_fn_lock_show_cptkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_cptkbd *cptkbd_data = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", cptkbd_data->fn_lock);
@@ -231,7 +231,7 @@ static ssize_t attr_fn_lock_store_cptkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_cptkbd *cptkbd_data = hid_get_drvdata(hdev);
 	int value;
 
@@ -250,7 +250,7 @@ static ssize_t attr_sensitivity_show_cptkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_cptkbd *cptkbd_data = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n",
@@ -262,7 +262,7 @@ static ssize_t attr_sensitivity_store_cptkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_cptkbd *cptkbd_data = hid_get_drvdata(hdev);
 	int value;
 
@@ -387,7 +387,7 @@ static ssize_t attr_press_to_select_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->press_to_select);
@@ -398,7 +398,7 @@ static ssize_t attr_press_to_select_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -417,7 +417,7 @@ static ssize_t attr_dragging_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->dragging);
@@ -428,7 +428,7 @@ static ssize_t attr_dragging_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -447,7 +447,7 @@ static ssize_t attr_release_to_select_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->release_to_select);
@@ -458,7 +458,7 @@ static ssize_t attr_release_to_select_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -477,7 +477,7 @@ static ssize_t attr_select_right_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n", data_pointer->select_right);
@@ -488,7 +488,7 @@ static ssize_t attr_select_right_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -507,7 +507,7 @@ static ssize_t attr_sensitivity_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n",
@@ -519,7 +519,7 @@ static ssize_t attr_sensitivity_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -536,7 +536,7 @@ static ssize_t attr_press_speed_show_tpkbd(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%u\n",
@@ -548,7 +548,7 @@ static ssize_t attr_press_speed_store_tpkbd(struct device *dev,
 		const char *buf,
 		size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int value;
 
@@ -609,7 +609,7 @@ static enum led_brightness lenovo_led_brightness_get_tpkbd(
 			struct led_classdev *led_cdev)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	int led_nr = 0;
 
@@ -625,7 +625,7 @@ static void lenovo_led_brightness_set_tpkbd(struct led_classdev *led_cdev,
 			enum led_brightness value)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct lenovo_drvdata_tpkbd *data_pointer = hid_get_drvdata(hdev);
 	struct hid_report *report;
 	int led_nr = 0;
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index fbddcb37ae98..af3a8ec8a746 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -33,8 +33,6 @@
 #include "hid-lg4ff.h"
 #include "hid-ids.h"
 
-#define to_hid_device(pdev) container_of(pdev, struct hid_device, dev)
-
 #define LG4FF_MMODE_IS_MULTIMODE 0
 #define LG4FF_MMODE_SWITCHED 1
 #define LG4FF_MMODE_NOT_MULTIMODE 2
@@ -1020,7 +1018,7 @@ static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness value)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct lg_drv_data *drv_data = hid_get_drvdata(hid);
 	struct lg4ff_device_entry *entry;
 	int i, state = 0;
@@ -1055,7 +1053,7 @@ static void lg4ff_led_set_brightness(struct led_classdev *led_cdev,
 static enum led_brightness lg4ff_led_get_brightness(struct led_classdev *led_cdev)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct lg_drv_data *drv_data = hid_get_drvdata(hid);
 	struct lg4ff_device_entry *entry;
 	int i, value = 0;
diff --git a/drivers/hid/hid-logitech-hidpp.c b/drivers/hid/hid-logitech-hidpp.c
index 5fd97860aec4..bd2ab476c65e 100644
--- a/drivers/hid/hid-logitech-hidpp.c
+++ b/drivers/hid/hid-logitech-hidpp.c
@@ -40,18 +40,22 @@ MODULE_PARM_DESC(disable_tap_to_click,
 
 #define REPORT_ID_HIDPP_SHORT			0x10
 #define REPORT_ID_HIDPP_LONG			0x11
+#define REPORT_ID_HIDPP_VERY_LONG		0x12
 
 #define HIDPP_REPORT_SHORT_LENGTH		7
 #define HIDPP_REPORT_LONG_LENGTH		20
+#define HIDPP_REPORT_VERY_LONG_LENGTH		64
 
 #define HIDPP_QUIRK_CLASS_WTP			BIT(0)
 #define HIDPP_QUIRK_CLASS_M560			BIT(1)
 #define HIDPP_QUIRK_CLASS_K400			BIT(2)
+#define HIDPP_QUIRK_CLASS_G920			BIT(3)
 
 /* bits 2..20 are reserved for classes */
 #define HIDPP_QUIRK_CONNECT_EVENTS		BIT(21)
 #define HIDPP_QUIRK_WTP_PHYSICAL_BUTTONS	BIT(22)
 #define HIDPP_QUIRK_NO_HIDINPUT			BIT(23)
+#define HIDPP_QUIRK_FORCE_OUTPUT_REPORTS	BIT(24)
 
 #define HIDPP_QUIRK_DELAYED_INIT		(HIDPP_QUIRK_NO_HIDINPUT | \
 						 HIDPP_QUIRK_CONNECT_EVENTS)
@@ -81,13 +85,13 @@ MODULE_PARM_DESC(disable_tap_to_click,
 struct fap {
 	u8 feature_index;
 	u8 funcindex_clientid;
-	u8 params[HIDPP_REPORT_LONG_LENGTH - 4U];
+	u8 params[HIDPP_REPORT_VERY_LONG_LENGTH - 4U];
 };
 
 struct rap {
 	u8 sub_id;
 	u8 reg_address;
-	u8 params[HIDPP_REPORT_LONG_LENGTH - 4U];
+	u8 params[HIDPP_REPORT_VERY_LONG_LENGTH - 4U];
 };
 
 struct hidpp_report {
@@ -144,8 +148,11 @@ static void hidpp_connect_event(struct hidpp_device *hidpp_dev);
 static int __hidpp_send_report(struct hid_device *hdev,
 				struct hidpp_report *hidpp_report)
 {
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
 	int fields_count, ret;
 
+	hidpp = hid_get_drvdata(hdev);
+
 	switch (hidpp_report->report_id) {
 	case REPORT_ID_HIDPP_SHORT:
 		fields_count = HIDPP_REPORT_SHORT_LENGTH;
@@ -153,6 +160,9 @@ static int __hidpp_send_report(struct hid_device *hdev,
 	case REPORT_ID_HIDPP_LONG:
 		fields_count = HIDPP_REPORT_LONG_LENGTH;
 		break;
+	case REPORT_ID_HIDPP_VERY_LONG:
+		fields_count = HIDPP_REPORT_VERY_LONG_LENGTH;
+		break;
 	default:
 		return -ENODEV;
 	}
@@ -163,9 +173,13 @@ static int __hidpp_send_report(struct hid_device *hdev,
 	 */
 	hidpp_report->device_index = 0xff;
 
-	ret = hid_hw_raw_request(hdev, hidpp_report->report_id,
-		(u8 *)hidpp_report, fields_count, HID_OUTPUT_REPORT,
-		HID_REQ_SET_REPORT);
+	if (hidpp->quirks & HIDPP_QUIRK_FORCE_OUTPUT_REPORTS) {
+		ret = hid_hw_output_report(hdev, (u8 *)hidpp_report, fields_count);
+	} else {
+		ret = hid_hw_raw_request(hdev, hidpp_report->report_id,
+			(u8 *)hidpp_report, fields_count, HID_OUTPUT_REPORT,
+			HID_REQ_SET_REPORT);
+	}
 
 	return ret == fields_count ? 0 : -1;
 }
@@ -217,8 +231,9 @@ static int hidpp_send_message_sync(struct hidpp_device *hidpp,
 		goto exit;
 	}
 
-	if (response->report_id == REPORT_ID_HIDPP_LONG &&
-	    response->fap.feature_index == HIDPP20_ERROR) {
+	if ((response->report_id == REPORT_ID_HIDPP_LONG ||
+			response->report_id == REPORT_ID_HIDPP_VERY_LONG) &&
+			response->fap.feature_index == HIDPP20_ERROR) {
 		ret = response->fap.params[1];
 		dbg_hid("%s:got hidpp 2.0 error %02X\n", __func__, ret);
 		goto exit;
@@ -243,7 +258,11 @@ static int hidpp_send_fap_command_sync(struct hidpp_device *hidpp,
 	message = kzalloc(sizeof(struct hidpp_report), GFP_KERNEL);
 	if (!message)
 		return -ENOMEM;
-	message->report_id = REPORT_ID_HIDPP_LONG;
+
+	if (param_count > (HIDPP_REPORT_LONG_LENGTH - 4))
+		message->report_id = REPORT_ID_HIDPP_VERY_LONG;
+	else
+		message->report_id = REPORT_ID_HIDPP_LONG;
 	message->fap.feature_index = feat_index;
 	message->fap.funcindex_clientid = funcindex_clientid;
 	memcpy(&message->fap.params, params, param_count);
@@ -258,13 +277,23 @@ static int hidpp_send_rap_command_sync(struct hidpp_device *hidpp_dev,
 	struct hidpp_report *response)
 {
 	struct hidpp_report *message;
-	int ret;
+	int ret, max_count;
 
-	if ((report_id != REPORT_ID_HIDPP_SHORT) &&
-	    (report_id != REPORT_ID_HIDPP_LONG))
+	switch (report_id) {
+	case REPORT_ID_HIDPP_SHORT:
+		max_count = HIDPP_REPORT_SHORT_LENGTH - 4;
+		break;
+	case REPORT_ID_HIDPP_LONG:
+		max_count = HIDPP_REPORT_LONG_LENGTH - 4;
+		break;
+	case REPORT_ID_HIDPP_VERY_LONG:
+		max_count = HIDPP_REPORT_VERY_LONG_LENGTH - 4;
+		break;
+	default:
 		return -EINVAL;
+	}
 
-	if (param_count > sizeof(message->rap.params))
+	if (param_count > max_count)
 		return -EINVAL;
 
 	message = kzalloc(sizeof(struct hidpp_report), GFP_KERNEL);
@@ -508,10 +537,19 @@ static int hidpp_devicenametype_get_device_name(struct hidpp_device *hidpp,
 	if (ret)
 		return ret;
 
-	if (response.report_id == REPORT_ID_HIDPP_LONG)
+	switch (response.report_id) {
+	case REPORT_ID_HIDPP_VERY_LONG:
+		count = HIDPP_REPORT_VERY_LONG_LENGTH - 4;
+		break;
+	case REPORT_ID_HIDPP_LONG:
 		count = HIDPP_REPORT_LONG_LENGTH - 4;
-	else
+		break;
+	case REPORT_ID_HIDPP_SHORT:
 		count = HIDPP_REPORT_SHORT_LENGTH - 4;
+		break;
+	default:
+		return -EPROTO;
+	}
 
 	if (len_buf < count)
 		count = len_buf;
@@ -1257,6 +1295,131 @@ static int k400_connect(struct hid_device *hdev, bool connected)
 	return k400_disable_tap_to_click(hidpp);
 }
 
+/* ------------------------------------------------------------------------- */
+/* Logitech G920 Driving Force Racing Wheel for Xbox One                     */
+/* ------------------------------------------------------------------------- */
+
+#define HIDPP_PAGE_G920_FORCE_FEEDBACK			0x8123
+
+/* Using session ID = 1 */
+#define CMD_G920_FORCE_GET_APERTURE			0x51
+#define CMD_G920_FORCE_SET_APERTURE			0x61
+
+struct g920_private_data {
+	u8 force_feature;
+	u16 range;
+};
+
+static ssize_t g920_range_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct hidpp_device *hidpp = hid_get_drvdata(hid);
+	struct g920_private_data *pdata;
+
+	pdata = hidpp->private_data;
+	if (!pdata) {
+		hid_err(hid, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", pdata->range);
+}
+
+static ssize_t g920_range_store(struct device *dev, struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct hid_device *hid = to_hid_device(dev);
+	struct hidpp_device *hidpp = hid_get_drvdata(hid);
+	struct g920_private_data *pdata;
+	struct hidpp_report response;
+	u8 params[2];
+	int ret;
+	u16 range = simple_strtoul(buf, NULL, 10);
+
+	pdata = hidpp->private_data;
+	if (!pdata) {
+		hid_err(hid, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	if (range < 180)
+		range = 180;
+	else if (range > 900)
+		range = 900;
+
+	params[0] = range >> 8;
+	params[1] = range & 0x00FF;
+
+	ret = hidpp_send_fap_command_sync(hidpp, pdata->force_feature,
+		CMD_G920_FORCE_SET_APERTURE, params, 2, &response);
+	if (ret)
+		return ret;
+
+	pdata->range = range;
+	return count;
+}
+
+static DEVICE_ATTR(range, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP | S_IROTH, g920_range_show, g920_range_store);
+
+static int g920_allocate(struct hid_device *hdev)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+	struct g920_private_data *pdata;
+
+	pdata = devm_kzalloc(&hdev->dev, sizeof(struct g920_private_data),
+			GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+
+	hidpp->private_data = pdata;
+
+	return 0;
+}
+
+static int g920_get_config(struct hidpp_device *hidpp)
+{
+	struct g920_private_data *pdata = hidpp->private_data;
+	struct hidpp_report response;
+	u8 feature_type;
+	u8 feature_index;
+	int ret;
+
+	pdata = hidpp->private_data;
+	if (!pdata) {
+		hid_err(hidpp->hid_dev, "Private driver data not found!\n");
+		return -EINVAL;
+	}
+
+	/* Find feature and store for later use */
+	ret = hidpp_root_get_feature(hidpp, HIDPP_PAGE_G920_FORCE_FEEDBACK,
+		&feature_index, &feature_type);
+	if (ret)
+		return ret;
+
+	pdata->force_feature = feature_index;
+
+	/* Read current Range */
+	ret = hidpp_send_fap_command_sync(hidpp, feature_index,
+		CMD_G920_FORCE_GET_APERTURE, NULL, 0, &response);
+	if (ret > 0) {
+		hid_err(hidpp->hid_dev, "%s: received protocol error 0x%02x\n",
+			__func__, ret);
+		return -EPROTO;
+	}
+	if (ret)
+		return ret;
+
+	pdata->range = get_unaligned_be16(&response.fap.params[0]);
+
+	/* Create sysfs interface */
+	ret = device_create_file(&(hidpp->hid_dev->dev), &dev_attr_range);
+	if (ret)
+		hid_warn(hidpp->hid_dev, "Unable to create sysfs interface for \"range\", errno %d\n", ret);
+
+	return 0;
+}
+
 /* -------------------------------------------------------------------------- */
 /* Generic HID++ devices                                                      */
 /* -------------------------------------------------------------------------- */
@@ -1276,6 +1439,25 @@ static int hidpp_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 	return 0;
 }
 
+static int hidpp_input_mapped(struct hid_device *hdev, struct hid_input *hi,
+		struct hid_field *field, struct hid_usage *usage,
+		unsigned long **bit, int *max)
+{
+	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
+
+	/* Ensure that Logitech G920 is not given a default fuzz/flat value */
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_G920) {
+		if (usage->type == EV_ABS && (usage->code == ABS_X ||
+				usage->code == ABS_Y || usage->code == ABS_Z ||
+				usage->code == ABS_RZ)) {
+			field->application = HID_GD_MULTIAXIS;
+		}
+	}
+
+	return 0;
+}
+
+
 static void hidpp_populate_input(struct hidpp_device *hidpp,
 		struct input_dev *input, bool origin_is_hid_core)
 {
@@ -1347,6 +1529,14 @@ static int hidpp_raw_event(struct hid_device *hdev, struct hid_report *report,
 
 	/* Generic HID++ processing. */
 	switch (data[0]) {
+	case REPORT_ID_HIDPP_VERY_LONG:
+		if (size != HIDPP_REPORT_VERY_LONG_LENGTH) {
+			hid_err(hdev, "received hid++ report of bad size (%d)",
+				size);
+			return 1;
+		}
+		ret = hidpp_raw_hidpp_event(hidpp, data, size);
+		break;
 	case REPORT_ID_HIDPP_LONG:
 		if (size != HIDPP_REPORT_LONG_LENGTH) {
 			hid_err(hdev, "received hid++ report of bad size (%d)",
@@ -1393,10 +1583,12 @@ static void hidpp_overwrite_name(struct hid_device *hdev, bool use_unifying)
 	else
 		name = hidpp_get_device_name(hidpp);
 
-	if (!name)
+	if (!name) {
 		hid_err(hdev, "unable to retrieve the name of the device");
-	else
+	} else {
+		dbg_hid("HID++: Got name: %s\n", name);
 		snprintf(hdev->name, sizeof(hdev->name), "%s", name);
+	}
 
 	kfree(name);
 }
@@ -1547,6 +1739,10 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		ret = k400_allocate(hdev);
 		if (ret)
 			goto allocate_fail;
+	} else if (hidpp->quirks & HIDPP_QUIRK_CLASS_G920) {
+		ret = g920_allocate(hdev);
+		if (ret)
+			goto allocate_fail;
 	}
 
 	INIT_WORK(&hidpp->work, delayed_work_cb);
@@ -1559,6 +1755,25 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		goto hid_parse_fail;
 	}
 
+	if (hidpp->quirks & HIDPP_QUIRK_NO_HIDINPUT)
+		connect_mask &= ~HID_CONNECT_HIDINPUT;
+
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_G920) {
+		ret = hid_hw_start(hdev, connect_mask);
+		if (ret) {
+			hid_err(hdev, "hw start failed\n");
+			goto hid_hw_start_fail;
+		}
+		ret = hid_hw_open(hdev);
+		if (ret < 0) {
+			dev_err(&hdev->dev, "%s:hid_hw_open returned error:%d\n",
+				__func__, ret);
+			hid_hw_stop(hdev);
+			goto hid_hw_start_fail;
+		}
+	}
+
+
 	/* Allow incoming packets */
 	hid_device_io_start(hdev);
 
@@ -1567,8 +1782,7 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		if (!connected) {
 			ret = -ENODEV;
 			hid_err(hdev, "Device not connected");
-			hid_device_io_stop(hdev);
-			goto hid_parse_fail;
+			goto hid_hw_open_failed;
 		}
 
 		hid_info(hdev, "HID++ %u.%u device connected.\n",
@@ -1581,19 +1795,22 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	if (connected && (hidpp->quirks & HIDPP_QUIRK_CLASS_WTP)) {
 		ret = wtp_get_config(hidpp);
 		if (ret)
-			goto hid_parse_fail;
+			goto hid_hw_open_failed;
+	} else if (connected && (hidpp->quirks & HIDPP_QUIRK_CLASS_G920)) {
+		ret = g920_get_config(hidpp);
+		if (ret)
+			goto hid_hw_open_failed;
 	}
 
 	/* Block incoming packets */
 	hid_device_io_stop(hdev);
 
-	if (hidpp->quirks & HIDPP_QUIRK_NO_HIDINPUT)
-		connect_mask &= ~HID_CONNECT_HIDINPUT;
-
-	ret = hid_hw_start(hdev, connect_mask);
-	if (ret) {
-		hid_err(hdev, "%s:hid_hw_start returned error\n", __func__);
-		goto hid_hw_start_fail;
+	if (!(hidpp->quirks & HIDPP_QUIRK_CLASS_G920)) {
+		ret = hid_hw_start(hdev, connect_mask);
+		if (ret) {
+			hid_err(hdev, "%s:hid_hw_start returned error\n", __func__);
+			goto hid_hw_start_fail;
+		}
 	}
 
 	if (hidpp->quirks & HIDPP_QUIRK_CONNECT_EVENTS) {
@@ -1605,6 +1822,13 @@ static int hidpp_probe(struct hid_device *hdev, const struct hid_device_id *id)
 
 	return ret;
 
+hid_hw_open_failed:
+	hid_device_io_stop(hdev);
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_G920) {
+		device_remove_file(&hdev->dev, &dev_attr_range);
+		hid_hw_close(hdev);
+		hid_hw_stop(hdev);
+	}
 hid_hw_start_fail:
 hid_parse_fail:
 	cancel_work_sync(&hidpp->work);
@@ -1618,9 +1842,13 @@ static void hidpp_remove(struct hid_device *hdev)
 {
 	struct hidpp_device *hidpp = hid_get_drvdata(hdev);
 
+	if (hidpp->quirks & HIDPP_QUIRK_CLASS_G920) {
+		device_remove_file(&hdev->dev, &dev_attr_range);
+		hid_hw_close(hdev);
+	}
+	hid_hw_stop(hdev);
 	cancel_work_sync(&hidpp->work);
 	mutex_destroy(&hidpp->send_mutex);
-	hid_hw_stop(hdev);
 }
 
 static const struct hid_device_id hidpp_devices[] = {
@@ -1648,6 +1876,9 @@ static const struct hid_device_id hidpp_devices[] = {
 
 	{ HID_DEVICE(BUS_USB, HID_GROUP_LOGITECH_DJ_DEVICE,
 		USB_VENDOR_ID_LOGITECH, HID_ANY_ID)},
+
+	{ HID_USB_DEVICE(USB_VENDOR_ID_LOGITECH, USB_DEVICE_ID_LOGITECH_G920_WHEEL),
+		.driver_data = HIDPP_QUIRK_CLASS_G920 | HIDPP_QUIRK_FORCE_OUTPUT_REPORTS},
 	{}
 };
 
@@ -1661,6 +1892,7 @@ static struct hid_driver hidpp_driver = {
 	.raw_event = hidpp_raw_event,
 	.input_configured = hidpp_input_configured,
 	.input_mapping = hidpp_input_mapping,
+	.input_mapped = hidpp_input_mapped,
 };
 
 module_hid_driver(hidpp_driver);
diff --git a/drivers/hid/hid-multitouch.c b/drivers/hid/hid-multitouch.c
index 3d664d01305e..296d4991560e 100644
--- a/drivers/hid/hid-multitouch.c
+++ b/drivers/hid/hid-multitouch.c
@@ -272,7 +272,7 @@ static ssize_t mt_show_quirks(struct device *dev,
 			   struct device_attribute *attr,
 			   char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct mt_device *td = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%u\n", td->mtclass.quirks);
@@ -282,7 +282,7 @@ static ssize_t mt_set_quirks(struct device *dev,
 			  struct device_attribute *attr,
 			  const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct mt_device *td = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -357,8 +357,19 @@ static void mt_feature_mapping(struct hid_device *hdev,
 			break;
 		}
 
-		td->inputmode = field->report->id;
-		td->inputmode_index = usage->usage_index;
+		if (td->inputmode < 0) {
+			td->inputmode = field->report->id;
+			td->inputmode_index = usage->usage_index;
+		} else {
+			/*
+			 * Some elan panels wrongly declare 2 input mode
+			 * features, and silently ignore when we set the
+			 * value in the second field. Skip the second feature
+			 * and hope for the best.
+			 */
+			dev_info(&hdev->dev,
+				 "Ignoring the extra HID_DG_INPUTMODE\n");
+		}
 
 		break;
 	case HID_DG_CONTACTMAX:
@@ -486,6 +497,11 @@ static int mt_touch_input_mapping(struct hid_device *hdev, struct hid_input *hi,
 			mt_store_field(usage, td, hi);
 			return 1;
 		case HID_DG_CONFIDENCE:
+			if (cls->name == MT_CLS_WIN_8 &&
+				field->application == HID_DG_TOUCHPAD) {
+				cls->quirks &= ~MT_QUIRK_ALWAYS_VALID;
+				cls->quirks |= MT_QUIRK_VALID_IS_CONFIDENCE;
+			}
 			mt_store_field(usage, td, hi);
 			return 1;
 		case HID_DG_TIPSWITCH:
diff --git a/drivers/hid/hid-ntrig.c b/drivers/hid/hid-ntrig.c
index 756d1ef9bd99..1b0084d4af2e 100644
--- a/drivers/hid/hid-ntrig.c
+++ b/drivers/hid/hid-ntrig.c
@@ -173,7 +173,7 @@ static ssize_t show_phys_width(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->sensor_physical_width);
@@ -185,7 +185,7 @@ static ssize_t show_phys_height(struct device *dev,
 				struct device_attribute *attr,
 				char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->sensor_physical_height);
@@ -197,7 +197,7 @@ static ssize_t show_log_width(struct device *dev,
 			      struct device_attribute *attr,
 			      char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->sensor_logical_width);
@@ -209,7 +209,7 @@ static ssize_t show_log_height(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->sensor_logical_height);
@@ -221,7 +221,7 @@ static ssize_t show_min_width(struct device *dev,
 			      struct device_attribute *attr,
 			      char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->min_width *
@@ -233,7 +233,7 @@ static ssize_t set_min_width(struct device *dev,
 			     struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -256,7 +256,7 @@ static ssize_t show_min_height(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->min_height *
@@ -268,7 +268,7 @@ static ssize_t set_min_height(struct device *dev,
 			      struct device_attribute *attr,
 			      const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -292,7 +292,7 @@ static ssize_t show_activate_slack(struct device *dev,
 				   struct device_attribute *attr,
 				   char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->activate_slack);
@@ -302,7 +302,7 @@ static ssize_t set_activate_slack(struct device *dev,
 				  struct device_attribute *attr,
 				  const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -325,7 +325,7 @@ static ssize_t show_activation_width(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->activation_width *
@@ -337,7 +337,7 @@ static ssize_t set_activation_width(struct device *dev,
 				    struct device_attribute *attr,
 				    const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -361,7 +361,7 @@ static ssize_t show_activation_height(struct device *dev,
 				      struct device_attribute *attr,
 				      char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", nd->activation_height *
@@ -373,7 +373,7 @@ static ssize_t set_activation_height(struct device *dev,
 				     struct device_attribute *attr,
 				     const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
@@ -397,7 +397,7 @@ static ssize_t show_deactivate_slack(struct device *dev,
 				     struct device_attribute *attr,
 				     char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	return sprintf(buf, "%d\n", -nd->deactivate_slack);
@@ -407,7 +407,7 @@ static ssize_t set_deactivate_slack(struct device *dev,
 				    struct device_attribute *attr,
 				    const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct ntrig_data *nd = hid_get_drvdata(hdev);
 
 	unsigned long val;
diff --git a/drivers/hid/hid-picolcd_leds.c b/drivers/hid/hid-picolcd_leds.c
index e994f9c29012..a802b4f49c7b 100644
--- a/drivers/hid/hid-picolcd_leds.c
+++ b/drivers/hid/hid-picolcd_leds.c
@@ -66,7 +66,7 @@ static void picolcd_led_set_brightness(struct led_classdev *led_cdev,
 	int i, state = 0;
 
 	dev  = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
+	hdev = to_hid_device(dev);
 	data = hid_get_drvdata(hdev);
 	if (!data)
 		return;
@@ -93,7 +93,7 @@ static enum led_brightness picolcd_led_get_brightness(struct led_classdev *led_c
 	int i, value = 0;
 
 	dev  = led_cdev->dev->parent;
-	hdev = container_of(dev, struct hid_device, dev);
+	hdev = to_hid_device(dev);
 	data = hid_get_drvdata(hdev);
 	for (i = 0; i < 8; i++)
 		if (led_cdev == data->led[i]) {
diff --git a/drivers/hid/hid-prodikeys.c b/drivers/hid/hid-prodikeys.c
index 3a207c0ac0e3..f095bf8a3aa9 100644
--- a/drivers/hid/hid-prodikeys.c
+++ b/drivers/hid/hid-prodikeys.c
@@ -103,7 +103,7 @@ MODULE_PARM_DESC(enable, "Enable for the PC-MIDI virtual audio driver");
 static ssize_t show_channel(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read channel=%u\n", pk->pm->midi_channel);
@@ -116,7 +116,7 @@ static ssize_t show_channel(struct device *dev,
 static ssize_t store_channel(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	unsigned channel = 0;
@@ -140,7 +140,7 @@ static struct device_attribute *sysfs_device_attr_channel = {
 static ssize_t show_sustain(struct device *dev,
  struct device_attribute *attr, char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read sustain=%u\n", pk->pm->midi_sustain);
@@ -153,7 +153,7 @@ static ssize_t show_sustain(struct device *dev,
 static ssize_t store_sustain(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	unsigned sustain = 0;
@@ -179,7 +179,7 @@ static struct device_attribute *sysfs_device_attr_sustain = {
 static ssize_t show_octave(struct device *dev,
 	struct device_attribute *attr, char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	dbg_hid("pcmidi sysfs read octave=%d\n", pk->pm->midi_octave);
@@ -192,7 +192,7 @@ static ssize_t show_octave(struct device *dev,
 static ssize_t store_octave(struct device *dev,
 	struct device_attribute *attr, const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct pk_device *pk = hid_get_drvdata(hdev);
 
 	int octave = 0;
diff --git a/drivers/hid/hid-roccat-arvo.c b/drivers/hid/hid-roccat-arvo.c
index 1948208fe038..329c5d1270f9 100644
--- a/drivers/hid/hid-roccat-arvo.c
+++ b/drivers/hid/hid-roccat-arvo.c
@@ -191,8 +191,7 @@ static ssize_t arvo_sysfs_write(struct file *fp,
 		struct kobject *kobj, void const *buf,
 		loff_t off, size_t count, size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct arvo_device *arvo = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -211,8 +210,7 @@ static ssize_t arvo_sysfs_read(struct file *fp,
 		struct kobject *kobj, void *buf, loff_t off,
 		size_t count, size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct arvo_device *arvo = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
diff --git a/drivers/hid/hid-roccat-common.c b/drivers/hid/hid-roccat-common.c
index 02e28e9f4ea7..8155ac5fede2 100644
--- a/drivers/hid/hid-roccat-common.c
+++ b/drivers/hid/hid-roccat-common.c
@@ -134,8 +134,7 @@ ssize_t roccat_common2_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct roccat_common2_device *roccat_dev = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -158,8 +157,7 @@ ssize_t roccat_common2_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct roccat_common2_device *roccat_dev = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
diff --git a/drivers/hid/hid-roccat-isku.c b/drivers/hid/hid-roccat-isku.c
index bc62ed91e451..02db537f8f3e 100644
--- a/drivers/hid/hid-roccat-isku.c
+++ b/drivers/hid/hid-roccat-isku.c
@@ -121,8 +121,7 @@ static ssize_t isku_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct isku_device *isku = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -144,8 +143,7 @@ static ssize_t isku_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct isku_device *isku = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
diff --git a/drivers/hid/hid-roccat-kone.c b/drivers/hid/hid-roccat-kone.c
index c29265055ac1..bf4675a27396 100644
--- a/drivers/hid/hid-roccat-kone.c
+++ b/drivers/hid/hid-roccat-kone.c
@@ -269,8 +269,7 @@ static int kone_get_firmware_version(struct usb_device *usb_dev, int *result)
 static ssize_t kone_sysfs_read_settings(struct file *fp, struct kobject *kobj,
 		struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count) {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kone_device *kone = hid_get_drvdata(dev_get_drvdata(dev));
 
 	if (off >= sizeof(struct kone_settings))
@@ -294,8 +293,7 @@ static ssize_t kone_sysfs_read_settings(struct file *fp, struct kobject *kobj,
 static ssize_t kone_sysfs_write_settings(struct file *fp, struct kobject *kobj,
 		struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count) {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kone_device *kone = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval = 0, difference, old_profile;
@@ -332,8 +330,7 @@ static BIN_ATTR(settings, 0660, kone_sysfs_read_settings,
 static ssize_t kone_sysfs_read_profilex(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr,
 		char *buf, loff_t off, size_t count) {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kone_device *kone = hid_get_drvdata(dev_get_drvdata(dev));
 
 	if (off >= sizeof(struct kone_profile))
@@ -353,8 +350,7 @@ static ssize_t kone_sysfs_read_profilex(struct file *fp,
 static ssize_t kone_sysfs_write_profilex(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr,
 		char *buf, loff_t off, size_t count) {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kone_device *kone = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	struct kone_profile *profile;
diff --git a/drivers/hid/hid-roccat-koneplus.c b/drivers/hid/hid-roccat-koneplus.c
index 5e99fcdc71b9..09e8fc72aa1d 100644
--- a/drivers/hid/hid-roccat-koneplus.c
+++ b/drivers/hid/hid-roccat-koneplus.c
@@ -87,8 +87,7 @@ static ssize_t koneplus_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct koneplus_device *koneplus = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -113,8 +112,7 @@ static ssize_t koneplus_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct koneplus_device *koneplus = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -193,8 +191,7 @@ static ssize_t koneplus_sysfs_read_profilex_settings(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
@@ -212,8 +209,7 @@ static ssize_t koneplus_sysfs_read_profilex_buttons(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
diff --git a/drivers/hid/hid-roccat-kovaplus.c b/drivers/hid/hid-roccat-kovaplus.c
index 966047711fbf..43617fb28b87 100644
--- a/drivers/hid/hid-roccat-kovaplus.c
+++ b/drivers/hid/hid-roccat-kovaplus.c
@@ -128,8 +128,7 @@ static ssize_t kovaplus_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -154,8 +153,7 @@ static ssize_t kovaplus_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct kovaplus_device *kovaplus = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -221,8 +219,7 @@ static ssize_t kovaplus_sysfs_read_profilex_settings(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
@@ -240,8 +237,7 @@ static ssize_t kovaplus_sysfs_read_profilex_buttons(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
diff --git a/drivers/hid/hid-roccat-lua.c b/drivers/hid/hid-roccat-lua.c
index 65e2e76bf2fe..ac1a7313e259 100644
--- a/drivers/hid/hid-roccat-lua.c
+++ b/drivers/hid/hid-roccat-lua.c
@@ -30,7 +30,7 @@ static ssize_t lua_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct lua_device *lua = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -52,7 +52,7 @@ static ssize_t lua_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct lua_device *lua = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
diff --git a/drivers/hid/hid-roccat-pyra.c b/drivers/hid/hid-roccat-pyra.c
index 47d7e74231e5..b30aa7b82bf8 100644
--- a/drivers/hid/hid-roccat-pyra.c
+++ b/drivers/hid/hid-roccat-pyra.c
@@ -90,8 +90,7 @@ static ssize_t pyra_sysfs_read(struct file *fp, struct kobject *kobj,
 		char *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct pyra_device *pyra = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -116,8 +115,7 @@ static ssize_t pyra_sysfs_write(struct file *fp, struct kobject *kobj,
 		void const *buf, loff_t off, size_t count,
 		size_t real_size, uint command)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct pyra_device *pyra = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval;
@@ -191,8 +189,7 @@ static ssize_t pyra_sysfs_read_profilex_settings(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
@@ -210,8 +207,7 @@ static ssize_t pyra_sysfs_read_profilex_buttons(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	ssize_t retval;
 
@@ -248,8 +244,7 @@ static ssize_t pyra_sysfs_write_settings(struct file *fp,
 		struct kobject *kobj, struct bin_attribute *attr, char *buf,
 		loff_t off, size_t count)
 {
-	struct device *dev =
-			container_of(kobj, struct device, kobj)->parent->parent;
+	struct device *dev = kobj_to_dev(kobj)->parent->parent;
 	struct pyra_device *pyra = hid_get_drvdata(dev_get_drvdata(dev));
 	struct usb_device *usb_dev = interface_to_usbdev(to_usb_interface(dev));
 	int retval = 0;
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index 92870cdb52d9..3d5ba5b51af3 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -218,7 +218,8 @@ int sensor_hub_set_feature(struct hid_sensor_hub_device *hsdev, u32 report_id,
 		goto done_proc;
 	}
 
-	remaining_bytes = do_div(buffer_size, sizeof(__s32));
+	remaining_bytes = buffer_size % sizeof(__s32);
+	buffer_size = buffer_size / sizeof(__s32);
 	if (buffer_size) {
 		for (i = 0; i < buffer_size; ++i) {
 			hid_set_field(report->field[field_index], i,
@@ -794,6 +795,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_YOGA2),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ 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, HID_ANY_ID,
 		     HID_ANY_ID) },
 	{ }
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index 774cd2210566..9b8db0e0ef1c 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -1028,6 +1028,7 @@ struct sony_sc {
 	struct led_classdev *leds[MAX_LEDS];
 	unsigned long quirks;
 	struct work_struct state_worker;
+	void(*send_output_report)(struct sony_sc*);
 	struct power_supply *battery;
 	struct power_supply_desc battery_desc;
 	int device_id;
@@ -1044,6 +1045,7 @@ struct sony_sc {
 	__u8 battery_charging;
 	__u8 battery_capacity;
 	__u8 led_state[MAX_LEDS];
+	__u8 resume_led_state[MAX_LEDS];
 	__u8 led_delay_on[MAX_LEDS];
 	__u8 led_delay_off[MAX_LEDS];
 	__u8 led_count;
@@ -1137,11 +1139,11 @@ static __u8 *sony_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	 * the gyroscope values to corresponding axes so we need a
 	 * modified one.
 	 */
-	if ((sc->quirks & DUALSHOCK4_CONTROLLER_USB) && *rsize == 467) {
+	if (sc->quirks & DUALSHOCK4_CONTROLLER_USB) {
 		hid_info(hdev, "Using modified Dualshock 4 report descriptor with gyroscope axes\n");
 		rdesc = dualshock4_usb_rdesc;
 		*rsize = sizeof(dualshock4_usb_rdesc);
-	} else if ((sc->quirks & DUALSHOCK4_CONTROLLER_BT) && *rsize == 357) {
+	} else if (sc->quirks & DUALSHOCK4_CONTROLLER_BT) {
 		hid_info(hdev, "Using modified Dualshock 4 Bluetooth report descriptor\n");
 		rdesc = dualshock4_bt_rdesc;
 		*rsize = sizeof(dualshock4_bt_rdesc);
@@ -1549,7 +1551,7 @@ static void sony_led_set_brightness(struct led_classdev *led,
 				    enum led_brightness value)
 {
 	struct device *dev = led->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct sony_sc *drv_data;
 
 	int n;
@@ -1591,7 +1593,7 @@ static void sony_led_set_brightness(struct led_classdev *led,
 static enum led_brightness sony_led_get_brightness(struct led_classdev *led)
 {
 	struct device *dev = led->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct sony_sc *drv_data;
 
 	int n;
@@ -1614,7 +1616,7 @@ static int sony_led_blink_set(struct led_classdev *led, unsigned long *delay_on,
 				unsigned long *delay_off)
 {
 	struct device *dev = led->dev->parent;
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct sony_sc *drv_data = hid_get_drvdata(hdev);
 	int n;
 	__u8 new_on, new_off;
@@ -1789,7 +1791,7 @@ error_leds:
 	return ret;
 }
 
-static void sixaxis_state_worker(struct work_struct *work)
+static void sixaxis_send_output_report(struct sony_sc *sc)
 {
 	static const union sixaxis_output_report_01 default_report = {
 		.buf = {
@@ -1803,7 +1805,6 @@ static void sixaxis_state_worker(struct work_struct *work)
 			0x00, 0x00, 0x00, 0x00, 0x00
 		}
 	};
-	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
 	struct sixaxis_output_report *report =
 		(struct sixaxis_output_report *)sc->output_report_dmabuf;
 	int n;
@@ -1846,9 +1847,8 @@ static void sixaxis_state_worker(struct work_struct *work)
 			HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
-static void dualshock4_state_worker(struct work_struct *work)
+static void dualshock4_send_output_report(struct sony_sc *sc)
 {
-	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
 	struct hid_device *hdev = sc->hdev;
 	__u8 *buf = sc->output_report_dmabuf;
 	int offset;
@@ -1893,9 +1893,8 @@ static void dualshock4_state_worker(struct work_struct *work)
 				HID_OUTPUT_REPORT, HID_REQ_SET_REPORT);
 }
 
-static void motion_state_worker(struct work_struct *work)
+static void motion_send_output_report(struct sony_sc *sc)
 {
-	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
 	struct hid_device *hdev = sc->hdev;
 	struct motion_output_report_02 *report =
 		(struct motion_output_report_02 *)sc->output_report_dmabuf;
@@ -1914,6 +1913,18 @@ static void motion_state_worker(struct work_struct *work)
 	hid_hw_output_report(hdev, (__u8 *)report, MOTION_REPORT_0x02_SIZE);
 }
 
+static inline void sony_send_output_report(struct sony_sc *sc)
+{
+	if (sc->send_output_report)
+		sc->send_output_report(sc);
+}
+
+static void sony_state_worker(struct work_struct *work)
+{
+	struct sony_sc *sc = container_of(work, struct sony_sc, state_worker);
+	sc->send_output_report(sc);
+}
+
 static int sony_allocate_output_report(struct sony_sc *sc)
 {
 	if ((sc->quirks & SIXAXIS_CONTROLLER) ||
@@ -2241,11 +2252,13 @@ static void sony_release_device_id(struct sony_sc *sc)
 	}
 }
 
-static inline void sony_init_work(struct sony_sc *sc,
-					void (*worker)(struct work_struct *))
+static inline void sony_init_output_report(struct sony_sc *sc,
+				void(*send_output_report)(struct sony_sc*))
 {
+	sc->send_output_report = send_output_report;
+
 	if (!sc->worker_initialized)
-		INIT_WORK(&sc->state_worker, worker);
+		INIT_WORK(&sc->state_worker, sony_state_worker);
 
 	sc->worker_initialized = 1;
 }
@@ -2319,7 +2332,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		hdev->quirks |= HID_QUIRK_NO_OUTPUT_REPORTS_ON_INTR_EP;
 		hdev->quirks |= HID_QUIRK_SKIP_OUTPUT_REPORT_ID;
 		ret = sixaxis_set_operational_usb(hdev);
-		sony_init_work(sc, sixaxis_state_worker);
+		sony_init_output_report(sc, sixaxis_send_output_report);
 	} else if ((sc->quirks & SIXAXIS_CONTROLLER_BT) ||
 			(sc->quirks & NAVIGATION_CONTROLLER_BT)) {
 		/*
@@ -2328,7 +2341,7 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		 */
 		hdev->quirks |= HID_QUIRK_NO_OUTPUT_REPORTS_ON_INTR_EP;
 		ret = sixaxis_set_operational_bt(hdev);
-		sony_init_work(sc, sixaxis_state_worker);
+		sony_init_output_report(sc, sixaxis_send_output_report);
 	} else if (sc->quirks & DUALSHOCK4_CONTROLLER) {
 		if (sc->quirks & DUALSHOCK4_CONTROLLER_BT) {
 			/*
@@ -2343,9 +2356,9 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 			}
 		}
 
-		sony_init_work(sc, dualshock4_state_worker);
+		sony_init_output_report(sc, dualshock4_send_output_report);
 	} else if (sc->quirks & MOTION_CONTROLLER) {
-		sony_init_work(sc, motion_state_worker);
+		sony_init_output_report(sc, motion_send_output_report);
 	} else {
 		ret = 0;
 	}
@@ -2421,6 +2434,56 @@ static void sony_remove(struct hid_device *hdev)
 	hid_hw_stop(hdev);
 }
 
+#ifdef CONFIG_PM
+
+static int sony_suspend(struct hid_device *hdev, pm_message_t message)
+{
+	/*
+	 * On suspend save the current LED state,
+	 * stop running force-feedback and blank the LEDS.
+         */
+	if (SONY_LED_SUPPORT || SONY_FF_SUPPORT) {
+		struct sony_sc *sc = hid_get_drvdata(hdev);
+
+#ifdef CONFIG_SONY_FF
+		sc->left = sc->right = 0;
+#endif
+
+		memcpy(sc->resume_led_state, sc->led_state,
+			sizeof(sc->resume_led_state));
+		memset(sc->led_state, 0, sizeof(sc->led_state));
+
+		sony_send_output_report(sc);
+	}
+
+	return 0;
+}
+
+static int sony_resume(struct hid_device *hdev)
+{
+	/* Restore the state of controller LEDs on resume */
+	if (SONY_LED_SUPPORT) {
+		struct sony_sc *sc = hid_get_drvdata(hdev);
+
+		memcpy(sc->led_state, sc->resume_led_state,
+			sizeof(sc->led_state));
+
+		/*
+		 * The Sixaxis and navigation controllers on USB need to be
+		 * reinitialized on resume or they won't behave properly.
+		 */
+		if ((sc->quirks & SIXAXIS_CONTROLLER_USB) ||
+			(sc->quirks & NAVIGATION_CONTROLLER_USB))
+			sixaxis_set_operational_usb(sc->hdev);
+
+		sony_set_leds(sc);
+	}
+
+	return 0;
+}
+
+#endif
+
 static const struct hid_device_id sony_devices[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_SONY, USB_DEVICE_ID_SONY_PS3_CONTROLLER),
 		.driver_data = SIXAXIS_CONTROLLER_USB },
@@ -2470,7 +2533,13 @@ static struct hid_driver sony_driver = {
 	.probe            = sony_probe,
 	.remove           = sony_remove,
 	.report_fixup     = sony_report_fixup,
-	.raw_event        = sony_raw_event
+	.raw_event        = sony_raw_event,
+
+#ifdef CONFIG_PM
+	.suspend          = sony_suspend,
+	.resume	          = sony_resume,
+	.reset_resume     = sony_resume,
+#endif
 };
 
 static int __init sony_init(void)
diff --git a/drivers/hid/hid-steelseries.c b/drivers/hid/hid-steelseries.c
index 3edd4ac36494..ec18768b124a 100644
--- a/drivers/hid/hid-steelseries.c
+++ b/drivers/hid/hid-steelseries.c
@@ -141,7 +141,7 @@ static void steelseries_srws1_led_all_set_brightness(struct led_classdev *led_cd
 			enum led_brightness value)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct steelseries_srws1_data *drv_data = hid_get_drvdata(hid);
 
 	if (!drv_data) {
@@ -160,7 +160,7 @@ static void steelseries_srws1_led_all_set_brightness(struct led_classdev *led_cd
 static enum led_brightness steelseries_srws1_led_all_get_brightness(struct led_classdev *led_cdev)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct steelseries_srws1_data *drv_data;
 
 	drv_data = hid_get_drvdata(hid);
@@ -177,7 +177,7 @@ static void steelseries_srws1_led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness value)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct steelseries_srws1_data *drv_data = hid_get_drvdata(hid);
 	int i, state = 0;
 
@@ -205,7 +205,7 @@ static void steelseries_srws1_led_set_brightness(struct led_classdev *led_cdev,
 static enum led_brightness steelseries_srws1_led_get_brightness(struct led_classdev *led_cdev)
 {
 	struct device *dev = led_cdev->dev->parent;
-	struct hid_device *hid = container_of(dev, struct hid_device, dev);
+	struct hid_device *hid = to_hid_device(dev);
 	struct steelseries_srws1_data *drv_data;
 	int i, value = 0;
 
diff --git a/drivers/hid/hid-wiimote-modules.c b/drivers/hid/hid-wiimote-modules.c
index 05e23c417d50..4390eee2ce84 100644
--- a/drivers/hid/hid-wiimote-modules.c
+++ b/drivers/hid/hid-wiimote-modules.c
@@ -296,14 +296,12 @@ static const struct wiimod_ops wiimod_battery = {
 
 static enum led_brightness wiimod_led_get(struct led_classdev *led_dev)
 {
-	struct wiimote_data *wdata;
 	struct device *dev = led_dev->dev->parent;
+	struct wiimote_data *wdata = dev_to_wii(dev);
 	int i;
 	unsigned long flags;
 	bool value = false;
 
-	wdata = hid_get_drvdata(container_of(dev, struct hid_device, dev));
-
 	for (i = 0; i < 4; ++i) {
 		if (wdata->leds[i] == led_dev) {
 			spin_lock_irqsave(&wdata->state.lock, flags);
@@ -319,14 +317,12 @@ static enum led_brightness wiimod_led_get(struct led_classdev *led_dev)
 static void wiimod_led_set(struct led_classdev *led_dev,
 			   enum led_brightness value)
 {
-	struct wiimote_data *wdata;
 	struct device *dev = led_dev->dev->parent;
+	struct wiimote_data *wdata = dev_to_wii(dev);
 	int i;
 	unsigned long flags;
 	__u8 state, flag;
 
-	wdata = hid_get_drvdata(container_of(dev, struct hid_device, dev));
-
 	for (i = 0; i < 4; ++i) {
 		if (wdata->leds[i] == led_dev) {
 			flag = WIIPROTO_FLAG_LED(i + 1);
diff --git a/drivers/hid/hid-wiimote.h b/drivers/hid/hid-wiimote.h
index 875694d43e4d..510ca77fe14e 100644
--- a/drivers/hid/hid-wiimote.h
+++ b/drivers/hid/hid-wiimote.h
@@ -256,8 +256,7 @@ enum wiiproto_reqs {
 	WIIPROTO_REQ_MAX
 };
 
-#define dev_to_wii(pdev) hid_get_drvdata(container_of(pdev, struct hid_device, \
-									dev))
+#define dev_to_wii(pdev) hid_get_drvdata(to_hid_device(pdev))
 
 void __wiimote_schedule(struct wiimote_data *wdata);
 
diff --git a/drivers/hid/i2c-hid/i2c-hid.c b/drivers/hid/i2c-hid/i2c-hid.c
index 10bd8e6e4c9c..b9216938a718 100644
--- a/drivers/hid/i2c-hid/i2c-hid.c
+++ b/drivers/hid/i2c-hid/i2c-hid.c
@@ -151,6 +151,7 @@ struct i2c_hid {
 	struct i2c_hid_platform_data pdata;
 
 	bool			irq_wake_enabled;
+	struct mutex		reset_lock;
 };
 
 static int __i2c_hid_command(struct i2c_client *client,
@@ -356,9 +357,16 @@ static int i2c_hid_hwreset(struct i2c_client *client)
 
 	i2c_hid_dbg(ihid, "%s\n", __func__);
 
+	/*
+	 * This prevents sending feature reports while the device is
+	 * being reset. Otherwise we may lose the reset complete
+	 * interrupt.
+	 */
+	mutex_lock(&ihid->reset_lock);
+
 	ret = i2c_hid_set_power(client, I2C_HID_PWR_ON);
 	if (ret)
-		return ret;
+		goto out_unlock;
 
 	i2c_hid_dbg(ihid, "resetting...\n");
 
@@ -366,10 +374,11 @@ static int i2c_hid_hwreset(struct i2c_client *client)
 	if (ret) {
 		dev_err(&client->dev, "failed to reset device.\n");
 		i2c_hid_set_power(client, I2C_HID_PWR_SLEEP);
-		return ret;
 	}
 
-	return 0;
+out_unlock:
+	mutex_unlock(&ihid->reset_lock);
+	return ret;
 }
 
 static void i2c_hid_get_input(struct i2c_hid *ihid)
@@ -587,12 +596,15 @@ static int i2c_hid_output_raw_report(struct hid_device *hid, __u8 *buf,
 		size_t count, unsigned char report_type, bool use_data)
 {
 	struct i2c_client *client = hid->driver_data;
+	struct i2c_hid *ihid = i2c_get_clientdata(client);
 	int report_id = buf[0];
 	int ret;
 
 	if (report_type == HID_INPUT_REPORT)
 		return -EINVAL;
 
+	mutex_lock(&ihid->reset_lock);
+
 	if (report_id) {
 		buf++;
 		count--;
@@ -605,6 +617,8 @@ static int i2c_hid_output_raw_report(struct hid_device *hid, __u8 *buf,
 	if (report_id && ret >= 0)
 		ret++; /* add report_id to the number of transfered bytes */
 
+	mutex_unlock(&ihid->reset_lock);
+
 	return ret;
 }
 
@@ -990,6 +1004,7 @@ static int i2c_hid_probe(struct i2c_client *client,
 	ihid->wHIDDescRegister = cpu_to_le16(hidRegister);
 
 	init_waitqueue_head(&ihid->wait);
+	mutex_init(&ihid->reset_lock);
 
 	/* we need to allocate the command buffer without knowing the maximum
 	 * size of the reports. Let's use HID_MIN_BUFFER_SIZE, then we do the
@@ -1184,7 +1199,6 @@ MODULE_DEVICE_TABLE(i2c, i2c_hid_id_table);
 static struct i2c_driver i2c_hid_driver = {
 	.driver = {
 		.name	= "i2c_hid",
-		.owner	= THIS_MODULE,
 		.pm	= &i2c_hid_pm,
 		.acpi_match_table = ACPI_PTR(i2c_hid_acpi_match),
 		.of_match_table = of_match_ptr(i2c_hid_of_match),
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 36712e9f56c2..ad71160b9ea4 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -274,10 +274,10 @@ static void hid_irq_in(struct urb *urb)
 
 	switch (urb->status) {
 	case 0:			/* success */
-		usbhid_mark_busy(usbhid);
 		usbhid->retry_delay = 0;
 		if ((hid->quirks & HID_QUIRK_ALWAYS_POLL) && !hid->open)
 			break;
+		usbhid_mark_busy(usbhid);
 		if (!test_bit(HID_RESUME_RUNNING, &usbhid->iofl)) {
 			hid_input_report(urb->context, HID_INPUT_REPORT,
 					 urb->transfer_buffer,
@@ -477,8 +477,6 @@ static void hid_ctrl(struct urb *urb)
 	struct usbhid_device *usbhid = hid->driver_data;
 	int unplug = 0, status = urb->status;
 
-	spin_lock(&usbhid->lock);
-
 	switch (status) {
 	case 0:			/* success */
 		if (usbhid->ctrl[usbhid->ctrltail].dir == USB_DIR_IN)
@@ -498,6 +496,8 @@ static void hid_ctrl(struct urb *urb)
 		hid_warn(urb->dev, "ctrl urb status %d received\n", status);
 	}
 
+	spin_lock(&usbhid->lock);
+
 	if (unplug) {
 		usbhid->ctrltail = usbhid->ctrlhead;
 	} else {
diff --git a/drivers/hid/usbhid/usbhid.h b/drivers/hid/usbhid/usbhid.h
index 807922b49aa4..fa47d666cfcf 100644
--- a/drivers/hid/usbhid/usbhid.h
+++ b/drivers/hid/usbhid/usbhid.h
@@ -96,7 +96,7 @@ struct usbhid_device {
 };
 
 #define	hid_to_usb_dev(hid_dev) \
-	container_of(hid_dev->dev.parent->parent, struct usb_device, dev)
+	to_usb_device(hid_dev->dev.parent->parent)
 
 #endif
 
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index e06af5b9f59e..5cb21dd91094 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -686,7 +686,7 @@ out:
 static ssize_t wacom_led_select_store(struct device *dev, int set_id,
 				      const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	unsigned int id;
 	int err;
@@ -714,7 +714,7 @@ static ssize_t wacom_led##SET_ID##_select_store(struct device *dev,	\
 static ssize_t wacom_led##SET_ID##_select_show(struct device *dev,	\
 	struct device_attribute *attr, char *buf)			\
 {									\
-	struct hid_device *hdev = container_of(dev, struct hid_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]);			\
@@ -750,7 +750,7 @@ static ssize_t wacom_luminance_store(struct wacom *wacom, u8 *dest,
 static ssize_t wacom_##name##_luminance_store(struct device *dev,	\
 	struct device_attribute *attr, const char *buf, size_t count)	\
 {									\
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);\
+	struct hid_device *hdev = to_hid_device(dev);\
 	struct wacom *wacom = hid_get_drvdata(hdev);			\
 									\
 	return wacom_luminance_store(wacom, &wacom->led.field,		\
@@ -773,7 +773,7 @@ DEVICE_LUMINANCE_ATTR(buttons, img_lum);
 static ssize_t wacom_button_image_store(struct device *dev, int button_id,
 					const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	int err;
 	unsigned len;
@@ -1097,7 +1097,7 @@ static ssize_t wacom_show_speed(struct device *dev,
 				struct device_attribute
 				*attr, char *buf)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 
 	return snprintf(buf, PAGE_SIZE, "%i\n", wacom->wacom_wac.bt_high_speed);
@@ -1107,7 +1107,7 @@ static ssize_t wacom_store_speed(struct device *dev,
 				struct device_attribute *attr,
 				const char *buf, size_t count)
 {
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	u8 new_speed;
 
@@ -1130,8 +1130,8 @@ static ssize_t wacom_show_remote_mode(struct kobject *kobj,
 				      struct kobj_attribute *kattr,
 				      char *buf, int index)
 {
-	struct device *dev = container_of(kobj->parent, struct device, kobj);
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct device *dev = kobj_to_dev(kobj->parent);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	u8 mode;
 
@@ -1241,8 +1241,8 @@ static ssize_t wacom_store_unpair_remote(struct kobject *kobj,
 					 const char *buf, size_t count)
 {
 	unsigned char selector = 0;
-	struct device *dev = container_of(kobj->parent, struct device, kobj);
-	struct hid_device *hdev = container_of(dev, struct hid_device, dev);
+	struct device *dev = kobj_to_dev(kobj->parent);
+	struct hid_device *hdev = to_hid_device(dev);
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	int err;
 
@@ -1353,8 +1353,7 @@ static void wacom_clean_inputs(struct wacom *wacom)
 		else
 			input_free_device(wacom->wacom_wac.pad_input);
 	}
-	if (wacom->remote_dir)
-		kobject_put(wacom->remote_dir);
+	kobject_put(wacom->remote_dir);
 	wacom->wacom_wac.pen_input = NULL;
 	wacom->wacom_wac.touch_input = NULL;
 	wacom->wacom_wac.pad_input = NULL;
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index 01a4f05c1642..99ef77fcfb80 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -34,6 +34,9 @@
  */
 #define WACOM_CONTACT_AREA_SCALE 2607
 
+static void wacom_report_numbered_buttons(struct input_dev *input_dev,
+				int button_count, int mask);
+
 /*
  * Percent of battery capacity for Graphire.
  * 8th value means AC online and show 100% capacity.
@@ -436,16 +439,142 @@ exit:
 static void wacom_intuos_schedule_prox_event(struct wacom_wac *wacom_wac)
 {
 	struct wacom *wacom = container_of(wacom_wac, struct wacom, wacom_wac);
+	struct wacom_features *features = &wacom_wac->features;
 	struct hid_report *r;
 	struct hid_report_enum *re;
 
 	re = &(wacom->hdev->report_enum[HID_FEATURE_REPORT]);
-	r = re->report_id_hash[WACOM_REPORT_INTUOSREAD];
+	if (features->type == INTUOSHT2)
+		r = re->report_id_hash[WACOM_REPORT_INTUOSHT2_ID];
+	else
+		r = re->report_id_hash[WACOM_REPORT_INTUOS_ID1];
 	if (r) {
 		hid_hw_request(wacom->hdev, r, HID_REQ_GET_REPORT);
 	}
 }
 
+static int wacom_intuos_pad(struct wacom_wac *wacom)
+{
+	struct wacom_features *features = &wacom->features;
+	unsigned char *data = wacom->data;
+	struct input_dev *input = wacom->pad_input;
+	int i;
+	int buttons = 0, nbuttons = features->numbered_buttons;
+	int keys = 0, nkeys = 0;
+	int ring1 = 0, ring2 = 0;
+	int strip1 = 0, strip2 = 0;
+	bool prox = false;
+
+	/* pad packets. Works as a second tool and is always in prox */
+	if (!(data[0] == WACOM_REPORT_INTUOSPAD || data[0] == WACOM_REPORT_INTUOS5PAD ||
+	      data[0] == WACOM_REPORT_CINTIQPAD))
+		return 0;
+
+	if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
+		buttons = (data[3] << 1) | (data[2] & 0x01);
+		ring1 = data[1];
+	} else if (features->type == DTK) {
+		buttons = data[6];
+	} else if (features->type == WACOM_13HD) {
+		buttons = (data[4] << 1) | (data[3] & 0x01);
+	} else if (features->type == WACOM_24HD) {
+		buttons = (data[8] << 8) | data[6];
+		ring1 = data[1];
+		ring2 = data[2];
+
+		/*
+		 * Three "buttons" are available on the 24HD which are
+		 * physically implemented as a touchstrip. Each button
+		 * is approximately 3 bits wide with a 2 bit spacing.
+		 * The raw touchstrip bits are stored at:
+		 *    ((data[3] & 0x1f) << 8) | data[4])
+		 */
+		nkeys = 3;
+		keys = ((data[3] & 0x1C) ? 1<<2 : 0) |
+		       ((data[4] & 0xE0) ? 1<<1 : 0) |
+		       ((data[4] & 0x07) ? 1<<0 : 0);
+	} else if (features->type == WACOM_27QHD) {
+		nkeys = 3;
+		keys = data[2] & 0x07;
+
+		input_report_abs(input, ABS_X, be16_to_cpup((__be16 *)&data[4]));
+		input_report_abs(input, ABS_Y, be16_to_cpup((__be16 *)&data[6]));
+		input_report_abs(input, ABS_Z, be16_to_cpup((__be16 *)&data[8]));
+	} else if (features->type == CINTIQ_HYBRID) {
+		/*
+		 * Do not send hardware buttons under Android. They
+		 * are already sent to the system through GPIO (and
+		 * have different meaning).
+		 *
+		 * d-pad right  -> data[4] & 0x10
+		 * d-pad up     -> data[4] & 0x20
+		 * d-pad left   -> data[4] & 0x40
+		 * d-pad down   -> data[4] & 0x80
+		 * d-pad center -> data[3] & 0x01
+		 */
+		buttons = (data[4] << 1) | (data[3] & 0x01);
+	} else if (features->type == CINTIQ_COMPANION_2) {
+		/* d-pad right  -> data[4] & 0x10
+		 * d-pad up     -> data[4] & 0x20
+		 * d-pad left   -> data[4] & 0x40
+		 * d-pad down   -> data[4] & 0x80
+		 * d-pad center -> data[3] & 0x01
+		 */
+		buttons = ((data[2] >> 4) << 7) |
+		          ((data[1] & 0x04) << 6) |
+		          ((data[2] & 0x0F) << 2) |
+		          (data[1] & 0x03);
+	} else if (features->type >= INTUOS5S && features->type <= INTUOSPL) {
+		/*
+		 * ExpressKeys on Intuos5/Intuos Pro have a capacitive sensor in
+		 * addition to the mechanical switch. Switch data is
+		 * stored in data[4], capacitive data in data[5].
+		 *
+		 * Touch ring mode switch (data[3]) has no capacitive sensor
+		 */
+		buttons = (data[4] << 1) | (data[3] & 0x01);
+		ring1 = data[2];
+	} else {
+		if (features->type == WACOM_21UX2 || features->type == WACOM_22HD) {
+			buttons = (data[8] << 10) | ((data[7] & 0x01) << 9) |
+			          (data[6] << 1) | (data[5] & 0x01);
+
+			if (features->type == WACOM_22HD) {
+				nkeys = 3;
+				keys = data[9] & 0x07;
+			}
+		} else {
+			buttons = ((data[6] & 0x10) << 10) |
+			          ((data[5] & 0x10) << 9)  |
+			          ((data[6] & 0x0F) << 4)  |
+			          (data[5] & 0x0F);
+		}
+		strip1 = ((data[1] & 0x1f) << 8) | data[2];
+		strip2 = ((data[3] & 0x1f) << 8) | data[4];
+	}
+
+	prox = (buttons & ~(~0 << nbuttons)) | (keys & ~(~0 << nkeys)) |
+	       (ring1 & 0x80) | (ring2 & 0x80) | strip1 | strip2;
+
+	wacom_report_numbered_buttons(input, nbuttons, buttons);
+
+	for (i = 0; i < nkeys; i++)
+		input_report_key(input, KEY_PROG1 + i, keys & (1 << i));
+
+	input_report_abs(input, ABS_RX, strip1);
+	input_report_abs(input, ABS_RY, strip2);
+
+	input_report_abs(input, ABS_WHEEL,    (ring1 & 0x80) ? (ring1 & 0x7f) : 0);
+	input_report_abs(input, ABS_THROTTLE, (ring2 & 0x80) ? (ring2 & 0x7f) : 0);
+
+	input_report_key(input, wacom->tool[1], prox ? 1 : 0);
+	input_report_abs(input, ABS_MISC, prox ? PAD_DEVICE_ID : 0);
+
+	input_event(input, EV_MSC, MSC_SERIAL, 0xffffffff);
+
+	return 1;
+}
+
 static int wacom_intuos_inout(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
@@ -755,19 +884,40 @@ static int wacom_remote_status_irq(struct wacom_wac *wacom_wac, size_t len)
 	return 0;
 }
 
-static void wacom_intuos_general(struct wacom_wac *wacom)
+static int wacom_intuos_general(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->pen_input;
-	unsigned int t;
+	int idx = (features->type == INTUOS) ? (data[1] & 0x01) : 0;
+	unsigned char type = (data[1] >> 1) & 0x0F;
+	unsigned int x, y, distance, t;
 
-	/* general pen packet */
-	if ((data[1] & 0xb8) == 0xa0) {
-		t = (data[6] << 2) | ((data[7] >> 6) & 3);
-		if (features->pressure_max == 2047) {
-			t = (t << 1) | (data[1] & 1);
-		}
+	if (data[0] != WACOM_REPORT_PENABLED && data[0] != WACOM_REPORT_CINTIQ &&
+		data[0] != WACOM_REPORT_INTUOS_PEN)
+		return 0;
+
+	x = (be16_to_cpup((__be16 *)&data[2]) << 1) | ((data[9] >> 1) & 1);
+	y = (be16_to_cpup((__be16 *)&data[4]) << 1) | (data[9] & 1);
+	distance = data[9] >> 2;
+	if (features->type < INTUOS3S) {
+		x >>= 1;
+		y >>= 1;
+		distance >>= 1;
+	}
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_abs(input, ABS_DISTANCE, distance);
+
+	switch (type) {
+	case 0x00:
+	case 0x01:
+	case 0x02:
+	case 0x03:
+		/* general pen packet */
+		t = (data[6] << 3) | ((data[7] & 0xC0) >> 5) | (data[1] & 1);
+		if (features->pressure_max < 2047)
+			t >>= 1;
 		input_report_abs(input, ABS_PRESSURE, t);
 		if (features->type != INTUOSHT2) {
 		    input_report_abs(input, ABS_TILT_X,
@@ -777,29 +927,112 @@ static void wacom_intuos_general(struct wacom_wac *wacom)
 		input_report_key(input, BTN_STYLUS, data[1] & 2);
 		input_report_key(input, BTN_STYLUS2, data[1] & 4);
 		input_report_key(input, BTN_TOUCH, t > 10);
-	}
+		break;
 
-	/* airbrush second packet */
-	if ((data[1] & 0xbc) == 0xb4) {
+	case 0x0a:
+		/* airbrush second packet */
 		input_report_abs(input, ABS_WHEEL,
 				(data[6] << 2) | ((data[7] >> 6) & 3));
 		input_report_abs(input, ABS_TILT_X,
 				 (((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
 		input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
+		break;
+
+	case 0x05:
+		/* Rotation packet */
+		if (features->type >= INTUOS3S) {
+			/* I3 marker pen rotation */
+			t = (data[6] << 3) | ((data[7] >> 5) & 7);
+			t = (data[7] & 0x20) ? ((t > 900) ? ((t-1) / 2 - 1350) :
+				((t-1) / 2 + 450)) : (450 - t / 2) ;
+			input_report_abs(input, ABS_Z, t);
+		} else {
+			/* 4D mouse 2nd packet */
+			t = (data[6] << 3) | ((data[7] >> 5) & 7);
+			input_report_abs(input, ABS_RZ, (data[7] & 0x20) ?
+				((t - 1) / 2) : -t / 2);
+		}
+		break;
+
+	case 0x04:
+		/* 4D mouse 1st packet */
+		input_report_key(input, BTN_LEFT,   data[8] & 0x01);
+		input_report_key(input, BTN_MIDDLE, data[8] & 0x02);
+		input_report_key(input, BTN_RIGHT,  data[8] & 0x04);
+
+		input_report_key(input, BTN_SIDE,   data[8] & 0x20);
+		input_report_key(input, BTN_EXTRA,  data[8] & 0x10);
+		t = (data[6] << 2) | ((data[7] >> 6) & 3);
+		input_report_abs(input, ABS_THROTTLE, (data[8] & 0x08) ? -t : t);
+		break;
+
+	case 0x06:
+		/* I4 mouse */
+		input_report_key(input, BTN_LEFT,   data[6] & 0x01);
+		input_report_key(input, BTN_MIDDLE, data[6] & 0x02);
+		input_report_key(input, BTN_RIGHT,  data[6] & 0x04);
+		input_report_rel(input, REL_WHEEL, ((data[7] & 0x80) >> 7)
+				 - ((data[7] & 0x40) >> 6));
+		input_report_key(input, BTN_SIDE,   data[6] & 0x08);
+		input_report_key(input, BTN_EXTRA,  data[6] & 0x10);
+
+		input_report_abs(input, ABS_TILT_X,
+			(((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
+		input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
+		break;
+
+	case 0x08:
+		if (wacom->tool[idx] == BTN_TOOL_MOUSE) {
+			/* 2D mouse packet */
+			input_report_key(input, BTN_LEFT,   data[8] & 0x04);
+			input_report_key(input, BTN_MIDDLE, data[8] & 0x08);
+			input_report_key(input, BTN_RIGHT,  data[8] & 0x10);
+			input_report_rel(input, REL_WHEEL, (data[8] & 0x01)
+					 - ((data[8] & 0x02) >> 1));
+
+			/* I3 2D mouse side buttons */
+			if (features->type >= INTUOS3S && features->type <= INTUOS3L) {
+				input_report_key(input, BTN_SIDE,   data[8] & 0x40);
+				input_report_key(input, BTN_EXTRA,  data[8] & 0x20);
+			}
+		}
+		else if (wacom->tool[idx] == BTN_TOOL_LENS) {
+			/* Lens cursor packets */
+			input_report_key(input, BTN_LEFT,   data[8] & 0x01);
+			input_report_key(input, BTN_MIDDLE, data[8] & 0x02);
+			input_report_key(input, BTN_RIGHT,  data[8] & 0x04);
+			input_report_key(input, BTN_SIDE,   data[8] & 0x10);
+			input_report_key(input, BTN_EXTRA,  data[8] & 0x08);
+		}
+		break;
+
+	case 0x07:
+	case 0x09:
+	case 0x0b:
+	case 0x0c:
+	case 0x0d:
+	case 0x0e:
+	case 0x0f:
+		/* unhandled */
+		break;
 	}
+
+	input_report_abs(input, ABS_MISC, wacom->id[idx]); /* report tool id */
+	input_report_key(input, wacom->tool[idx], 1);
+	input_event(input, EV_MSC, MSC_SERIAL, wacom->serial[idx]);
+	wacom->reporting_data = true;
+	return 2;
 }
 
 static int wacom_intuos_irq(struct wacom_wac *wacom)
 {
-	struct wacom_features *features = &wacom->features;
 	unsigned char *data = wacom->data;
 	struct input_dev *input = wacom->pen_input;
-	unsigned int t;
-	int idx = 0, result;
+	int result;
 
 	if (data[0] != WACOM_REPORT_PENABLED &&
-	    data[0] != WACOM_REPORT_INTUOSREAD &&
-	    data[0] != WACOM_REPORT_INTUOSWRITE &&
+	    data[0] != WACOM_REPORT_INTUOS_ID1 &&
+	    data[0] != WACOM_REPORT_INTUOS_ID2 &&
 	    data[0] != WACOM_REPORT_INTUOSPAD &&
 	    data[0] != WACOM_REPORT_INTUOS_PEN &&
 	    data[0] != WACOM_REPORT_CINTIQ &&
@@ -810,339 +1043,22 @@ static int wacom_intuos_irq(struct wacom_wac *wacom)
                 return 0;
 	}
 
-	/* tool number */
-	if (features->type == INTUOS)
-		idx = data[1] & 0x01;
-
-	/* pad packets. Works as a second tool and is always in prox */
-	if (data[0] == WACOM_REPORT_INTUOSPAD || data[0] == WACOM_REPORT_INTUOS5PAD ||
-	    data[0] == WACOM_REPORT_CINTIQPAD) {
-		input = wacom->pad_input;
-		if (features->type >= INTUOS4S && features->type <= INTUOS4L) {
-			input_report_key(input, BTN_0, (data[2] & 0x01));
-			input_report_key(input, BTN_1, (data[3] & 0x01));
-			input_report_key(input, BTN_2, (data[3] & 0x02));
-			input_report_key(input, BTN_3, (data[3] & 0x04));
-			input_report_key(input, BTN_4, (data[3] & 0x08));
-			input_report_key(input, BTN_5, (data[3] & 0x10));
-			input_report_key(input, BTN_6, (data[3] & 0x20));
-			if (data[1] & 0x80) {
-				input_report_abs(input, ABS_WHEEL, (data[1] & 0x7f));
-			} else {
-				/* Out of proximity, clear wheel value. */
-				input_report_abs(input, ABS_WHEEL, 0);
-			}
-			if (features->type != INTUOS4S) {
-				input_report_key(input, BTN_7, (data[3] & 0x40));
-				input_report_key(input, BTN_8, (data[3] & 0x80));
-			}
-			if (data[1] | (data[2] & 0x01) | data[3]) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else if (features->type == DTK) {
-			input_report_key(input, BTN_0, (data[6] & 0x01));
-			input_report_key(input, BTN_1, (data[6] & 0x02));
-			input_report_key(input, BTN_2, (data[6] & 0x04));
-			input_report_key(input, BTN_3, (data[6] & 0x08));
-			input_report_key(input, BTN_4, (data[6] & 0x10));
-			input_report_key(input, BTN_5, (data[6] & 0x20));
-			if (data[6] & 0x3f) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else if (features->type == WACOM_13HD) {
-			input_report_key(input, BTN_0, (data[3] & 0x01));
-			input_report_key(input, BTN_1, (data[4] & 0x01));
-			input_report_key(input, BTN_2, (data[4] & 0x02));
-			input_report_key(input, BTN_3, (data[4] & 0x04));
-			input_report_key(input, BTN_4, (data[4] & 0x08));
-			input_report_key(input, BTN_5, (data[4] & 0x10));
-			input_report_key(input, BTN_6, (data[4] & 0x20));
-			input_report_key(input, BTN_7, (data[4] & 0x40));
-			input_report_key(input, BTN_8, (data[4] & 0x80));
-			if ((data[3] & 0x01) | data[4]) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else if (features->type == WACOM_24HD) {
-			input_report_key(input, BTN_0, (data[6] & 0x01));
-			input_report_key(input, BTN_1, (data[6] & 0x02));
-			input_report_key(input, BTN_2, (data[6] & 0x04));
-			input_report_key(input, BTN_3, (data[6] & 0x08));
-			input_report_key(input, BTN_4, (data[6] & 0x10));
-			input_report_key(input, BTN_5, (data[6] & 0x20));
-			input_report_key(input, BTN_6, (data[6] & 0x40));
-			input_report_key(input, BTN_7, (data[6] & 0x80));
-			input_report_key(input, BTN_8, (data[8] & 0x01));
-			input_report_key(input, BTN_9, (data[8] & 0x02));
-			input_report_key(input, BTN_A, (data[8] & 0x04));
-			input_report_key(input, BTN_B, (data[8] & 0x08));
-			input_report_key(input, BTN_C, (data[8] & 0x10));
-			input_report_key(input, BTN_X, (data[8] & 0x20));
-			input_report_key(input, BTN_Y, (data[8] & 0x40));
-			input_report_key(input, BTN_Z, (data[8] & 0x80));
-
-			/*
-			 * Three "buttons" are available on the 24HD which are
-			 * physically implemented as a touchstrip. Each button
-			 * is approximately 3 bits wide with a 2 bit spacing.
-			 * The raw touchstrip bits are stored at:
-			 *    ((data[3] & 0x1f) << 8) | data[4])
-			 */
-			input_report_key(input, KEY_PROG1, data[4] & 0x07);
-			input_report_key(input, KEY_PROG2, data[4] & 0xE0);
-			input_report_key(input, KEY_PROG3, data[3] & 0x1C);
-
-			if (data[1] & 0x80) {
-				input_report_abs(input, ABS_WHEEL, (data[1] & 0x7f));
-			} else {
-				/* Out of proximity, clear wheel value. */
-				input_report_abs(input, ABS_WHEEL, 0);
-			}
-
-			if (data[2] & 0x80) {
-				input_report_abs(input, ABS_THROTTLE, (data[2] & 0x7f));
-			} else {
-				/* Out of proximity, clear second wheel value. */
-				input_report_abs(input, ABS_THROTTLE, 0);
-			}
-
-			if (data[1] | data[2] | (data[3] & 0x1f) | data[4] | data[6] | data[8]) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else if (features->type == WACOM_27QHD) {
-			input_report_key(input, KEY_PROG1, data[2] & 0x01);
-			input_report_key(input, KEY_PROG2, data[2] & 0x02);
-			input_report_key(input, KEY_PROG3, data[2] & 0x04);
-
-			input_report_abs(input, ABS_X, be16_to_cpup((__be16 *)&data[4]));
-			input_report_abs(input, ABS_Y, be16_to_cpup((__be16 *)&data[6]));
-			input_report_abs(input, ABS_Z, be16_to_cpup((__be16 *)&data[8]));
-			if ((data[2] & 0x07) | data[4] | data[5] | data[6] | data[7] | data[8] | data[9]) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else if (features->type == CINTIQ_HYBRID) {
-			/*
-			 * Do not send hardware buttons under Android. They
-			 * are already sent to the system through GPIO (and
-			 * have different meaning).
-			 */
-			input_report_key(input, BTN_1, (data[4] & 0x01));
-			input_report_key(input, BTN_2, (data[4] & 0x02));
-			input_report_key(input, BTN_3, (data[4] & 0x04));
-			input_report_key(input, BTN_4, (data[4] & 0x08));
-
-			input_report_key(input, BTN_5, (data[4] & 0x10));  /* Right  */
-			input_report_key(input, BTN_6, (data[4] & 0x20));  /* Up     */
-			input_report_key(input, BTN_7, (data[4] & 0x40));  /* Left   */
-			input_report_key(input, BTN_8, (data[4] & 0x80));  /* Down   */
-			input_report_key(input, BTN_0, (data[3] & 0x01));  /* Center */
-
-			if (data[4] | (data[3] & 0x01)) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-
-		} else if (features->type == CINTIQ_COMPANION_2) {
-			input_report_key(input, BTN_1, (data[1] & 0x02));
-			input_report_key(input, BTN_2, (data[2] & 0x01));
-			input_report_key(input, BTN_3, (data[2] & 0x02));
-			input_report_key(input, BTN_4, (data[2] & 0x04));
-			input_report_key(input, BTN_5, (data[2] & 0x08));
-			input_report_key(input, BTN_6, (data[1] & 0x04));
-
-			input_report_key(input, BTN_7, (data[2] & 0x10));  /* Right  */
-			input_report_key(input, BTN_8, (data[2] & 0x20));  /* Up	 */
-			input_report_key(input, BTN_9, (data[2] & 0x40));  /* Left   */
-			input_report_key(input, BTN_A, (data[2] & 0x80));  /* Down   */
-			input_report_key(input, BTN_0, (data[1] & 0x01));  /* Center */
-
-			if (data[2] | (data[1] & 0x07)) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-
-		} else if (features->type >= INTUOS5S && features->type <= INTUOSPL) {
-			int i;
-
-			/* Touch ring mode switch has no capacitive sensor */
-			input_report_key(input, BTN_0, (data[3] & 0x01));
-
-			/*
-			 * ExpressKeys on Intuos5/Intuos Pro have a capacitive sensor in
-			 * addition to the mechanical switch. Switch data is
-			 * stored in data[4], capacitive data in data[5].
-			 */
-			for (i = 0; i < 8; i++)
-				input_report_key(input, BTN_1 + i, data[4] & (1 << i));
-
-			if (data[2] & 0x80) {
-				input_report_abs(input, ABS_WHEEL, (data[2] & 0x7f));
-			} else {
-				/* Out of proximity, clear wheel value. */
-				input_report_abs(input, ABS_WHEEL, 0);
-			}
-
-			if (data[2] | (data[3] & 0x01) | data[4] | data[5]) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		} else {
-			if (features->type == WACOM_21UX2 || features->type == WACOM_22HD) {
-				input_report_key(input, BTN_0, (data[5] & 0x01));
-				input_report_key(input, BTN_1, (data[6] & 0x01));
-				input_report_key(input, BTN_2, (data[6] & 0x02));
-				input_report_key(input, BTN_3, (data[6] & 0x04));
-				input_report_key(input, BTN_4, (data[6] & 0x08));
-				input_report_key(input, BTN_5, (data[6] & 0x10));
-				input_report_key(input, BTN_6, (data[6] & 0x20));
-				input_report_key(input, BTN_7, (data[6] & 0x40));
-				input_report_key(input, BTN_8, (data[6] & 0x80));
-				input_report_key(input, BTN_9, (data[7] & 0x01));
-				input_report_key(input, BTN_A, (data[8] & 0x01));
-				input_report_key(input, BTN_B, (data[8] & 0x02));
-				input_report_key(input, BTN_C, (data[8] & 0x04));
-				input_report_key(input, BTN_X, (data[8] & 0x08));
-				input_report_key(input, BTN_Y, (data[8] & 0x10));
-				input_report_key(input, BTN_Z, (data[8] & 0x20));
-				input_report_key(input, BTN_BASE, (data[8] & 0x40));
-				input_report_key(input, BTN_BASE2, (data[8] & 0x80));
-
-				if (features->type == WACOM_22HD) {
-					input_report_key(input, KEY_PROG1, data[9] & 0x01);
-					input_report_key(input, KEY_PROG2, data[9] & 0x02);
-					input_report_key(input, KEY_PROG3, data[9] & 0x04);
-				}
-			} else {
-				input_report_key(input, BTN_0, (data[5] & 0x01));
-				input_report_key(input, BTN_1, (data[5] & 0x02));
-				input_report_key(input, BTN_2, (data[5] & 0x04));
-				input_report_key(input, BTN_3, (data[5] & 0x08));
-				input_report_key(input, BTN_4, (data[6] & 0x01));
-				input_report_key(input, BTN_5, (data[6] & 0x02));
-				input_report_key(input, BTN_6, (data[6] & 0x04));
-				input_report_key(input, BTN_7, (data[6] & 0x08));
-				input_report_key(input, BTN_8, (data[5] & 0x10));
-				input_report_key(input, BTN_9, (data[6] & 0x10));
-			}
-			input_report_abs(input, ABS_RX, ((data[1] & 0x1f) << 8) | data[2]);
-			input_report_abs(input, ABS_RY, ((data[3] & 0x1f) << 8) | data[4]);
-
-			if ((data[5] & 0x1f) | data[6] | (data[1] & 0x1f) |
-				data[2] | (data[3] & 0x1f) | data[4] | data[8] |
-				(data[7] & 0x01)) {
-				input_report_abs(input, ABS_MISC, PAD_DEVICE_ID);
-			} else {
-				input_report_abs(input, ABS_MISC, 0);
-			}
-		}
-                return 1;
-	}
+	/* process pad events */
+	result = wacom_intuos_pad(wacom);
+	if (result)
+		return result;
 
 	/* process in/out prox events */
 	result = wacom_intuos_inout(wacom);
 	if (result)
-                return result - 1;
-
-	if (features->type >= INTUOS3S) {
-		input_report_abs(input, ABS_X, (data[2] << 9) | (data[3] << 1) | ((data[9] >> 1) & 1));
-		input_report_abs(input, ABS_Y, (data[4] << 9) | (data[5] << 1) | (data[9] & 1));
-		input_report_abs(input, ABS_DISTANCE, ((data[9] >> 2) & 0x3f));
-	} else {
-		input_report_abs(input, ABS_X, be16_to_cpup((__be16 *)&data[2]));
-		input_report_abs(input, ABS_Y, be16_to_cpup((__be16 *)&data[4]));
-		input_report_abs(input, ABS_DISTANCE, ((data[9] >> 3) & 0x1f));
-	}
+		return result - 1;
 
 	/* process general packets */
-	wacom_intuos_general(wacom);
-
-	/* 4D mouse, 2D mouse, marker pen rotation, tilt mouse, or Lens cursor packets */
-	if ((data[1] & 0xbc) == 0xa8 || (data[1] & 0xbe) == 0xb0 || (data[1] & 0xbc) == 0xac) {
-
-		if (data[1] & 0x02) {
-			/* Rotation packet */
-			if (features->type >= INTUOS3S) {
-				/* I3 marker pen rotation */
-				t = (data[6] << 3) | ((data[7] >> 5) & 7);
-				t = (data[7] & 0x20) ? ((t > 900) ? ((t-1) / 2 - 1350) :
-					((t-1) / 2 + 450)) : (450 - t / 2) ;
-				input_report_abs(input, ABS_Z, t);
-			} else {
-				/* 4D mouse rotation packet */
-				t = (data[6] << 3) | ((data[7] >> 5) & 7);
-				input_report_abs(input, ABS_RZ, (data[7] & 0x20) ?
-					((t - 1) / 2) : -t / 2);
-			}
-
-		} else if (!(data[1] & 0x10) && features->type < INTUOS3S) {
-			/* 4D mouse packet */
-			input_report_key(input, BTN_LEFT,   data[8] & 0x01);
-			input_report_key(input, BTN_MIDDLE, data[8] & 0x02);
-			input_report_key(input, BTN_RIGHT,  data[8] & 0x04);
-
-			input_report_key(input, BTN_SIDE,   data[8] & 0x20);
-			input_report_key(input, BTN_EXTRA,  data[8] & 0x10);
-			t = (data[6] << 2) | ((data[7] >> 6) & 3);
-			input_report_abs(input, ABS_THROTTLE, (data[8] & 0x08) ? -t : t);
-
-		} else if (wacom->tool[idx] == BTN_TOOL_MOUSE) {
-			/* I4 mouse */
-			if (features->type >= INTUOS4S && features->type <= INTUOSPL) {
-				input_report_key(input, BTN_LEFT,   data[6] & 0x01);
-				input_report_key(input, BTN_MIDDLE, data[6] & 0x02);
-				input_report_key(input, BTN_RIGHT,  data[6] & 0x04);
-				input_report_rel(input, REL_WHEEL, ((data[7] & 0x80) >> 7)
-						 - ((data[7] & 0x40) >> 6));
-				input_report_key(input, BTN_SIDE,   data[6] & 0x08);
-				input_report_key(input, BTN_EXTRA,  data[6] & 0x10);
-
-				input_report_abs(input, ABS_TILT_X,
-					(((data[7] << 1) & 0x7e) | (data[8] >> 7)) - 64);
-				input_report_abs(input, ABS_TILT_Y, (data[8] & 0x7f) - 64);
-			} else {
-				/* 2D mouse packet */
-				input_report_key(input, BTN_LEFT,   data[8] & 0x04);
-				input_report_key(input, BTN_MIDDLE, data[8] & 0x08);
-				input_report_key(input, BTN_RIGHT,  data[8] & 0x10);
-				input_report_rel(input, REL_WHEEL, (data[8] & 0x01)
-						 - ((data[8] & 0x02) >> 1));
-
-				/* I3 2D mouse side buttons */
-				if (features->type >= INTUOS3S && features->type <= INTUOS3L) {
-					input_report_key(input, BTN_SIDE,   data[8] & 0x40);
-					input_report_key(input, BTN_EXTRA,  data[8] & 0x20);
-				}
-			}
-		} else if ((features->type < INTUOS3S || features->type == INTUOS3L ||
-				features->type == INTUOS4L || features->type == INTUOS5L ||
-				features->type == INTUOSPL) &&
-			   wacom->tool[idx] == BTN_TOOL_LENS) {
-			/* Lens cursor packets */
-			input_report_key(input, BTN_LEFT,   data[8] & 0x01);
-			input_report_key(input, BTN_MIDDLE, data[8] & 0x02);
-			input_report_key(input, BTN_RIGHT,  data[8] & 0x04);
-			input_report_key(input, BTN_SIDE,   data[8] & 0x10);
-			input_report_key(input, BTN_EXTRA,  data[8] & 0x08);
-		}
-	}
+	result = wacom_intuos_general(wacom);
+	if (result)
+		return result - 1;
 
-	input_report_abs(input, ABS_MISC, wacom->id[idx]); /* report tool id */
-	input_report_key(input, wacom->tool[idx], 1);
-	input_event(input, EV_MSC, MSC_SERIAL, wacom->serial[idx]);
-	wacom->reporting_data = true;
-	return 1;
+	return 0;
 }
 
 static int int_dist(int x1, int y1, int x2, int y2)
@@ -2509,7 +2425,7 @@ void wacom_setup_device_quirks(struct wacom *wacom)
 		features->quirks |= WACOM_QUIRK_BATTERY;
 
 	/* quirk for bamboo touch with 2 low res touches */
-	if (features->type == BAMBOO_PT &&
+	if ((features->type == BAMBOO_PT || features->type == BAMBOO_TOUCH) &&
 	    features->pktlen == WACOM_PKGLEN_BBTOUCH) {
 		features->x_max <<= 5;
 		features->y_max <<= 5;
@@ -2806,6 +2722,19 @@ static void wacom_setup_numbered_buttons(struct input_dev *input_dev,
 		__set_bit(BTN_BASE + (i-16), input_dev->keybit);
 }
 
+static void wacom_report_numbered_buttons(struct input_dev *input_dev,
+				int button_count, int mask)
+{
+	int 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++)
+		input_report_key(input_dev, BTN_A + (i-10), mask & (1 << i));
+	for (i = 16; i < button_count && i < 18; i++)
+		input_report_key(input_dev, BTN_BASE + (i-16), mask & (1 << i));
+}
+
 int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 				   struct wacom_wac *wacom_wac)
 {
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 877c24a5df94..25baa7f29599 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -47,8 +47,8 @@
 /* wacom data packet report IDs */
 #define WACOM_REPORT_PENABLED		2
 #define WACOM_REPORT_PENABLED_BT	3
-#define WACOM_REPORT_INTUOSREAD		5
-#define WACOM_REPORT_INTUOSWRITE	6
+#define WACOM_REPORT_INTUOS_ID1		5
+#define WACOM_REPORT_INTUOS_ID2		6
 #define WACOM_REPORT_INTUOSPAD		12
 #define WACOM_REPORT_INTUOS5PAD		3
 #define WACOM_REPORT_DTUSPAD		21
@@ -70,6 +70,7 @@
 #define WACOM_REPORT_DEVICE_LIST	16
 #define WACOM_REPORT_INTUOS_PEN		16
 #define WACOM_REPORT_REMOTE		17
+#define WACOM_REPORT_INTUOSHT2_ID	8
 
 /* device quirks */
 #define WACOM_QUIRK_BBTOUCH_LOWRES	0x0001
diff --git a/drivers/hsi/controllers/omap_ssi.c b/drivers/hsi/controllers/omap_ssi.c
index f6d3100b7a32..27b91f14ba7a 100644
--- a/drivers/hsi/controllers/omap_ssi.c
+++ b/drivers/hsi/controllers/omap_ssi.c
@@ -323,11 +323,10 @@ static int __init ssi_add_controller(struct hsi_controller *ssi,
 		return -ENOMEM;
 	}
 
-	ssi->id = ida_simple_get(&platform_omap_ssi_ida, 0, 0, GFP_KERNEL);
-	if (ssi->id < 0) {
-		err = ssi->id;
+	err = ida_simple_get(&platform_omap_ssi_ida, 0, 0, GFP_KERNEL);
+	if (err < 0)
 		goto out_err;
-	}
+	ssi->id = err;
 
 	ssi->owner = THIS_MODULE;
 	ssi->device.parent = &pd->dev;
diff --git a/drivers/hsi/controllers/omap_ssi_port.c b/drivers/hsi/controllers/omap_ssi_port.c
index 02e66032ae73..e80a66e20998 100644
--- a/drivers/hsi/controllers/omap_ssi_port.c
+++ b/drivers/hsi/controllers/omap_ssi_port.c
@@ -1147,13 +1147,13 @@ static int __init ssi_port_probe(struct platform_device *pd)
 		goto error;
 	}
 
-	cawake_gpio = of_get_named_gpio(np, "ti,ssi-cawake-gpio", 0);
-	if (cawake_gpio < 0) {
+	err = of_get_named_gpio(np, "ti,ssi-cawake-gpio", 0);
+	if (err < 0) {
 		dev_err(&pd->dev, "DT data is missing cawake gpio (err=%d)\n",
-			cawake_gpio);
-		err = -ENODEV;
+			err);
 		goto error;
 	}
+	cawake_gpio = err;
 
 	err = devm_gpio_request_one(&port->device, cawake_gpio, GPIOF_DIR_IN,
 		"cawake");
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index c4dcab048cb8..1161d68a1863 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -28,6 +28,7 @@
 #include <linux/module.h>
 #include <linux/hyperv.h>
 #include <linux/uio.h>
+#include <linux/interrupt.h>
 
 #include "hyperv_vmbus.h"
 
@@ -496,8 +497,33 @@ 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;
 
+	/*
+	 * process_chn_event(), running in the tasklet, can race
+	 * with vmbus_close_internal() in the case of SMP guest, e.g., when
+	 * the former is accessing channel->inbound.ring_buffer, the latter
+	 * could be freeing the ring_buffer pages.
+	 *
+	 * 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);
+
+	/*
+	 * In case a device driver's probe() fails (e.g.,
+	 * util_probe() -> vmbus_open() returns -ENOMEM) and the device is
+	 * rescinded later (e.g., we dynamically disble an Integrated Service
+	 * in Hyper-V Manager), the driver's remove() invokes vmbus_close():
+	 * here we should skip most of the below cleanup work.
+	 */
+	if (channel->state != CHANNEL_OPENED_STATE) {
+		ret = -EINVAL;
+		goto out;
+	}
+
 	channel->state = CHANNEL_OPEN_STATE;
 	channel->sc_creation_callback = NULL;
 	/* Stop callback and cancel the timer asap */
@@ -525,7 +551,7 @@ static int vmbus_close_internal(struct vmbus_channel *channel)
 		 * If we failed to post the close msg,
 		 * it is perhaps better to leak memory.
 		 */
-		return ret;
+		goto out;
 	}
 
 	/* Tear down the gpadl for the channel's ring buffer */
@@ -538,7 +564,7 @@ static int vmbus_close_internal(struct vmbus_channel *channel)
 			 * If we failed to teardown gpadl,
 			 * it is perhaps better to leak memory.
 			 */
-			return ret;
+			goto out;
 		}
 	}
 
@@ -549,12 +575,9 @@ static int vmbus_close_internal(struct vmbus_channel *channel)
 	free_pages((unsigned long)channel->ringbuffer_pages,
 		get_order(channel->ringbuffer_pagecount * PAGE_SIZE));
 
-	/*
-	 * If the channel has been rescinded; process device removal.
-	 */
-	if (channel->rescind)
-		hv_process_channel_removal(channel,
-					   channel->offermsg.child_relid);
+out:
+	tasklet_enable(tasklet);
+
 	return ret;
 }
 
@@ -630,10 +653,19 @@ int vmbus_sendpacket_ctl(struct vmbus_channel *channel, void *buffer,
 	 *    on the ring. We will not signal if more data is
 	 *    to be placed.
 	 *
+	 * Based on the channel signal state, we will decide
+	 * which signaling policy will be applied.
+	 *
 	 * If we cannot write to the ring-buffer; signal the host
 	 * even if we may not have written anything. This is a rare
 	 * 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);
 
@@ -733,10 +765,19 @@ int vmbus_sendpacket_pagebuffer_ctl(struct vmbus_channel *channel,
 	 *    on the ring. We will not signal if more data is
 	 *    to be placed.
 	 *
+	 * Based on the channel signal state, we will decide
+	 * which signaling policy will be applied.
+	 *
 	 * If we cannot write to the ring-buffer; signal the host
 	 * even if we may not have written anything. This is a rare
 	 * 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);
 
@@ -881,46 +922,29 @@ EXPORT_SYMBOL_GPL(vmbus_sendpacket_multipagebuffer);
  *
  * Mainly used by Hyper-V drivers.
  */
-int vmbus_recvpacket(struct vmbus_channel *channel, void *buffer,
-			u32 bufferlen, u32 *buffer_actual_len, u64 *requestid)
+static inline int
+__vmbus_recvpacket(struct vmbus_channel *channel, void *buffer,
+		   u32 bufferlen, u32 *buffer_actual_len, u64 *requestid,
+		   bool raw)
 {
-	struct vmpacket_descriptor desc;
-	u32 packetlen;
-	u32 userlen;
 	int ret;
 	bool signal = false;
 
-	*buffer_actual_len = 0;
-	*requestid = 0;
-
-
-	ret = hv_ringbuffer_peek(&channel->inbound, &desc,
-			     sizeof(struct vmpacket_descriptor));
-	if (ret != 0)
-		return 0;
-
-	packetlen = desc.len8 << 3;
-	userlen = packetlen - (desc.offset8 << 3);
-
-	*buffer_actual_len = userlen;
-
-	if (userlen > bufferlen) {
-
-		pr_err("Buffer too small - got %d needs %d\n",
-			   bufferlen, userlen);
-		return -ETOOSMALL;
-	}
-
-	*requestid = desc.trans_id;
-
-	/* Copy over the packet to the user buffer */
-	ret = hv_ringbuffer_read(&channel->inbound, buffer, userlen,
-			     (desc.offset8 << 3), &signal);
+	ret = hv_ringbuffer_read(&channel->inbound, buffer, bufferlen,
+				 buffer_actual_len, requestid, &signal, raw);
 
 	if (signal)
 		vmbus_setevent(channel);
 
-	return 0;
+	return ret;
+}
+
+int vmbus_recvpacket(struct vmbus_channel *channel, void *buffer,
+		     u32 bufferlen, u32 *buffer_actual_len,
+		     u64 *requestid)
+{
+	return __vmbus_recvpacket(channel, buffer, bufferlen,
+				  buffer_actual_len, requestid, false);
 }
 EXPORT_SYMBOL(vmbus_recvpacket);
 
@@ -931,37 +955,7 @@ int vmbus_recvpacket_raw(struct vmbus_channel *channel, void *buffer,
 			      u32 bufferlen, u32 *buffer_actual_len,
 			      u64 *requestid)
 {
-	struct vmpacket_descriptor desc;
-	u32 packetlen;
-	int ret;
-	bool signal = false;
-
-	*buffer_actual_len = 0;
-	*requestid = 0;
-
-
-	ret = hv_ringbuffer_peek(&channel->inbound, &desc,
-			     sizeof(struct vmpacket_descriptor));
-	if (ret != 0)
-		return 0;
-
-
-	packetlen = desc.len8 << 3;
-
-	*buffer_actual_len = packetlen;
-
-	if (packetlen > bufferlen)
-		return -ENOBUFS;
-
-	*requestid = desc.trans_id;
-
-	/* Copy over the entire packet to the user buffer */
-	ret = hv_ringbuffer_read(&channel->inbound, buffer, packetlen, 0,
-				 &signal);
-
-	if (signal)
-		vmbus_setevent(channel);
-
-	return ret;
+	return __vmbus_recvpacket(channel, buffer, bufferlen,
+				  buffer_actual_len, requestid, true);
 }
 EXPORT_SYMBOL_GPL(vmbus_recvpacket_raw);
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index 652afd11a9ef..1c1ad47042c5 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -177,19 +177,24 @@ static void percpu_channel_deq(void *arg)
 }
 
 
-void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
+static void vmbus_release_relid(u32 relid)
 {
 	struct vmbus_channel_relid_released msg;
-	unsigned long flags;
-	struct vmbus_channel *primary_channel;
 
 	memset(&msg, 0, sizeof(struct vmbus_channel_relid_released));
 	msg.child_relid = relid;
 	msg.header.msgtype = CHANNELMSG_RELID_RELEASED;
 	vmbus_post_msg(&msg, sizeof(struct vmbus_channel_relid_released));
+}
 
-	if (channel == NULL)
-		return;
+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);
 
 	if (channel->target_cpu != get_cpu()) {
 		put_cpu();
@@ -201,9 +206,9 @@ void hv_process_channel_removal(struct vmbus_channel *channel, u32 relid)
 	}
 
 	if (channel->primary_channel == NULL) {
-		spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
+		mutex_lock(&vmbus_connection.channel_mutex);
 		list_del(&channel->listentry);
-		spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
+		mutex_unlock(&vmbus_connection.channel_mutex);
 
 		primary_channel = channel;
 	} else {
@@ -230,9 +235,7 @@ void vmbus_free_channels(void)
 
 	list_for_each_entry_safe(channel, tmp, &vmbus_connection.chn_list,
 		listentry) {
-		/* if we don't set rescind to true, vmbus_close_internal()
-		 * won't invoke hv_process_channel_removal().
-		 */
+		/* hv_process_channel_removal() needs this */
 		channel->rescind = true;
 
 		vmbus_device_unregister(channel->device_obj);
@@ -250,7 +253,7 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	unsigned long flags;
 
 	/* Make sure this is a new offer */
-	spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
+	mutex_lock(&vmbus_connection.channel_mutex);
 
 	list_for_each_entry(channel, &vmbus_connection.chn_list, listentry) {
 		if (!uuid_le_cmp(channel->offermsg.offer.if_type,
@@ -266,7 +269,7 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 		list_add_tail(&newchannel->listentry,
 			      &vmbus_connection.chn_list);
 
-	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
+	mutex_unlock(&vmbus_connection.channel_mutex);
 
 	if (!fnew) {
 		/*
@@ -336,9 +339,11 @@ static void vmbus_process_offer(struct vmbus_channel *newchannel)
 	return;
 
 err_deq_chan:
-	spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
+	vmbus_release_relid(newchannel->offermsg.child_relid);
+
+	mutex_lock(&vmbus_connection.channel_mutex);
 	list_del(&newchannel->listentry);
-	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
+	mutex_unlock(&vmbus_connection.channel_mutex);
 
 	if (newchannel->target_cpu != get_cpu()) {
 		put_cpu();
@@ -356,8 +361,10 @@ err_free_chan:
 enum {
 	IDE = 0,
 	SCSI,
+	FC,
 	NIC,
 	ND_NIC,
+	PCIE,
 	MAX_PERF_CHN,
 };
 
@@ -371,10 +378,14 @@ static const struct hv_vmbus_device_id hp_devs[] = {
 	{ HV_IDE_GUID, },
 	/* Storage - SCSI */
 	{ HV_SCSI_GUID, },
+	/* Storage - FC */
+	{ HV_SYNTHFC_GUID, },
 	/* Network */
 	{ HV_NIC_GUID, },
 	/* NetworkDirect Guest RDMA */
 	{ HV_ND_GUID, },
+	/* PCI Express Pass Through */
+	{ HV_PCIE_GUID, },
 };
 
 
@@ -405,8 +416,7 @@ static void init_vp_index(struct vmbus_channel *channel, const uuid_le *type_gui
 	struct cpumask *alloced_mask;
 
 	for (i = IDE; i < MAX_PERF_CHN; i++) {
-		if (!memcmp(type_guid->b, hp_devs[i].guid,
-				 sizeof(uuid_le))) {
+		if (!uuid_le_cmp(*type_guid, hp_devs[i].guid)) {
 			perf_chn = true;
 			break;
 		}
@@ -585,7 +595,11 @@ static void vmbus_onoffer_rescind(struct vmbus_channel_message_header *hdr)
 	channel = relid2channel(rescind->child_relid);
 
 	if (channel == NULL) {
-		hv_process_channel_removal(NULL, rescind->child_relid);
+		/*
+		 * This is very impossible, because in
+		 * vmbus_process_offer(), we have already invoked
+		 * vmbus_release_relid() on error.
+		 */
 		return;
 	}
 
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index 4fc2e8836e60..3dc5a9c7fad6 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -83,10 +83,13 @@ static int vmbus_negotiate_version(struct vmbus_channel_msginfo *msginfo,
 	msg->interrupt_page = virt_to_phys(vmbus_connection.int_page);
 	msg->monitor_page1 = virt_to_phys(vmbus_connection.monitor_pages[0]);
 	msg->monitor_page2 = virt_to_phys(vmbus_connection.monitor_pages[1]);
-	if (version >= VERSION_WIN8_1) {
-		msg->target_vcpu = hv_context.vp_index[get_cpu()];
-		put_cpu();
-	}
+	/*
+	 * We want all channel messages to be delivered on CPU 0.
+	 * This has been the behavior pre-win8. This is not
+	 * perf issue and having all channel messages delivered on CPU 0
+	 * would be ok.
+	 */
+	msg->target_vcpu = 0;
 
 	/*
 	 * Add to list before we send the request since we may
@@ -146,7 +149,7 @@ int vmbus_connect(void)
 	spin_lock_init(&vmbus_connection.channelmsg_lock);
 
 	INIT_LIST_HEAD(&vmbus_connection.chn_list);
-	spin_lock_init(&vmbus_connection.channel_lock);
+	mutex_init(&vmbus_connection.channel_mutex);
 
 	/*
 	 * Setup the vmbus event connection for channel interrupt
@@ -282,11 +285,10 @@ struct vmbus_channel *relid2channel(u32 relid)
 {
 	struct vmbus_channel *channel;
 	struct vmbus_channel *found_channel  = NULL;
-	unsigned long flags;
 	struct list_head *cur, *tmp;
 	struct vmbus_channel *cur_sc;
 
-	spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
+	mutex_lock(&vmbus_connection.channel_mutex);
 	list_for_each_entry(channel, &vmbus_connection.chn_list, listentry) {
 		if (channel->offermsg.child_relid == relid) {
 			found_channel = channel;
@@ -305,7 +307,7 @@ struct vmbus_channel *relid2channel(u32 relid)
 			}
 		}
 	}
-	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
+	mutex_unlock(&vmbus_connection.channel_mutex);
 
 	return found_channel;
 }
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index 6341be8739ae..11bca51ef5ff 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -89,9 +89,9 @@ static int query_hypervisor_info(void)
 }
 
 /*
- * do_hypercall- Invoke the specified hypercall
+ * hv_do_hypercall- Invoke the specified hypercall
  */
-static u64 do_hypercall(u64 control, void *input, void *output)
+u64 hv_do_hypercall(u64 control, void *input, void *output)
 {
 	u64 input_address = (input) ? virt_to_phys(input) : 0;
 	u64 output_address = (output) ? virt_to_phys(output) : 0;
@@ -132,6 +132,7 @@ static u64 do_hypercall(u64 control, void *input, void *output)
 	return hv_status_lo | ((u64)hv_status_hi << 32);
 #endif /* !x86_64 */
 }
+EXPORT_SYMBOL_GPL(hv_do_hypercall);
 
 #ifdef CONFIG_X86_64
 static cycle_t read_hv_clock_tsc(struct clocksource *arg)
@@ -139,7 +140,7 @@ static cycle_t read_hv_clock_tsc(struct clocksource *arg)
 	cycle_t current_tick;
 	struct ms_hyperv_tsc_page *tsc_pg = hv_context.tsc_page;
 
-	if (tsc_pg->tsc_sequence != -1) {
+	if (tsc_pg->tsc_sequence != 0) {
 		/*
 		 * Use the tsc page to compute the value.
 		 */
@@ -161,7 +162,7 @@ static cycle_t read_hv_clock_tsc(struct clocksource *arg)
 			if (tsc_pg->tsc_sequence == sequence)
 				return current_tick;
 
-			if (tsc_pg->tsc_sequence != -1)
+			if (tsc_pg->tsc_sequence != 0)
 				continue;
 			/*
 			 * Fallback using MSR method.
@@ -192,9 +193,7 @@ int hv_init(void)
 {
 	int max_leaf;
 	union hv_x64_msr_hypercall_contents hypercall_msr;
-	union hv_x64_msr_hypercall_contents tsc_msr;
 	void *virtaddr = NULL;
-	void *va_tsc = NULL;
 
 	memset(hv_context.synic_event_page, 0, sizeof(void *) * NR_CPUS);
 	memset(hv_context.synic_message_page, 0,
@@ -240,6 +239,9 @@ int hv_init(void)
 
 #ifdef CONFIG_X86_64
 	if (ms_hyperv.features & HV_X64_MSR_REFERENCE_TSC_AVAILABLE) {
+		union hv_x64_msr_hypercall_contents tsc_msr;
+		void *va_tsc;
+
 		va_tsc = __vmalloc(PAGE_SIZE, GFP_KERNEL, PAGE_KERNEL);
 		if (!va_tsc)
 			goto cleanup;
@@ -315,7 +317,7 @@ int hv_post_message(union hv_connection_id connection_id,
 {
 
 	struct hv_input_post_message *aligned_msg;
-	u16 status;
+	u64 status;
 
 	if (payload_size > HV_MESSAGE_PAYLOAD_BYTE_COUNT)
 		return -EMSGSIZE;
@@ -329,11 +331,10 @@ int hv_post_message(union hv_connection_id connection_id,
 	aligned_msg->payload_size = payload_size;
 	memcpy((void *)aligned_msg->payload, payload, payload_size);
 
-	status = do_hypercall(HVCALL_POST_MESSAGE, aligned_msg, NULL)
-		& 0xFFFF;
+	status = hv_do_hypercall(HVCALL_POST_MESSAGE, aligned_msg, NULL);
 
 	put_cpu();
-	return status;
+	return status & 0xFFFF;
 }
 
 
@@ -343,13 +344,13 @@ int hv_post_message(union hv_connection_id connection_id,
  *
  * This involves a hypercall.
  */
-u16 hv_signal_event(void *con_id)
+int hv_signal_event(void *con_id)
 {
-	u16 status;
+	u64 status;
 
-	status = (do_hypercall(HVCALL_SIGNAL_EVENT, con_id, NULL) & 0xFFFF);
+	status = hv_do_hypercall(HVCALL_SIGNAL_EVENT, con_id, NULL);
 
-	return status;
+	return status & 0xFFFF;
 }
 
 static int hv_ce_set_next_event(unsigned long delta,
diff --git a/drivers/hv/hv_fcopy.c b/drivers/hv/hv_fcopy.c
index db4b887b889d..c37a71e13de0 100644
--- a/drivers/hv/hv_fcopy.c
+++ b/drivers/hv/hv_fcopy.c
@@ -51,7 +51,6 @@ static struct {
 	struct hv_fcopy_hdr  *fcopy_msg; /* current message */
 	struct vmbus_channel *recv_channel; /* chn we got the request */
 	u64 recv_req_id; /* request ID. */
-	void *fcopy_context; /* for the channel callback */
 } fcopy_transaction;
 
 static void fcopy_respond_to_host(int error);
@@ -67,6 +66,13 @@ static struct hvutil_transport *hvt;
  */
 static int dm_reg_value;
 
+static void fcopy_poll_wrapper(void *channel)
+{
+	/* Transaction is finished, reset the state here to avoid races. */
+	fcopy_transaction.state = HVUTIL_READY;
+	hv_fcopy_onchannelcallback(channel);
+}
+
 static void fcopy_timeout_func(struct work_struct *dummy)
 {
 	/*
@@ -74,13 +80,7 @@ static void fcopy_timeout_func(struct work_struct *dummy)
 	 * process the pending transaction.
 	 */
 	fcopy_respond_to_host(HV_E_FAIL);
-
-	/* Transaction is finished, reset the state. */
-	if (fcopy_transaction.state > HVUTIL_READY)
-		fcopy_transaction.state = HVUTIL_READY;
-
-	hv_poll_channel(fcopy_transaction.fcopy_context,
-			hv_fcopy_onchannelcallback);
+	hv_poll_channel(fcopy_transaction.recv_channel, fcopy_poll_wrapper);
 }
 
 static int fcopy_handle_handshake(u32 version)
@@ -108,9 +108,7 @@ static int fcopy_handle_handshake(u32 version)
 		return -EINVAL;
 	}
 	pr_debug("FCP: userspace daemon ver. %d registered\n", version);
-	fcopy_transaction.state = HVUTIL_READY;
-	hv_poll_channel(fcopy_transaction.fcopy_context,
-			hv_fcopy_onchannelcallback);
+	hv_poll_channel(fcopy_transaction.recv_channel, fcopy_poll_wrapper);
 	return 0;
 }
 
@@ -227,15 +225,8 @@ void hv_fcopy_onchannelcallback(void *context)
 	int util_fw_version;
 	int fcopy_srv_version;
 
-	if (fcopy_transaction.state > HVUTIL_READY) {
-		/*
-		 * We will defer processing this callback once
-		 * the current transaction is complete.
-		 */
-		fcopy_transaction.fcopy_context = context;
+	if (fcopy_transaction.state > HVUTIL_READY)
 		return;
-	}
-	fcopy_transaction.fcopy_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
 			 &requestid);
@@ -275,7 +266,8 @@ void hv_fcopy_onchannelcallback(void *context)
 		 * Send the information to the user-level daemon.
 		 */
 		schedule_work(&fcopy_send_work);
-		schedule_delayed_work(&fcopy_timeout_work, 5*HZ);
+		schedule_delayed_work(&fcopy_timeout_work,
+				      HV_UTIL_TIMEOUT * HZ);
 		return;
 	}
 	icmsghdr->icflags = ICMSGHDRFLAG_TRANSACTION | ICMSGHDRFLAG_RESPONSE;
@@ -304,9 +296,8 @@ static int fcopy_on_msg(void *msg, int len)
 	if (cancel_delayed_work_sync(&fcopy_timeout_work)) {
 		fcopy_transaction.state = HVUTIL_USERSPACE_RECV;
 		fcopy_respond_to_host(*val);
-		fcopy_transaction.state = HVUTIL_READY;
-		hv_poll_channel(fcopy_transaction.fcopy_context,
-				hv_fcopy_onchannelcallback);
+		hv_poll_channel(fcopy_transaction.recv_channel,
+				fcopy_poll_wrapper);
 	}
 
 	return 0;
diff --git a/drivers/hv/hv_kvp.c b/drivers/hv/hv_kvp.c
index 74c38a9f34a6..d4ab81bcd515 100644
--- a/drivers/hv/hv_kvp.c
+++ b/drivers/hv/hv_kvp.c
@@ -66,7 +66,6 @@ static struct {
 	struct hv_kvp_msg  *kvp_msg; /* current message */
 	struct vmbus_channel *recv_channel; /* chn we got the request */
 	u64 recv_req_id; /* request ID. */
-	void *kvp_context; /* for the channel callback */
 } kvp_transaction;
 
 /*
@@ -94,6 +93,13 @@ static struct hvutil_transport *hvt;
  */
 #define HV_DRV_VERSION           "3.1"
 
+static void kvp_poll_wrapper(void *channel)
+{
+	/* Transaction is finished, reset the state here to avoid races. */
+	kvp_transaction.state = HVUTIL_READY;
+	hv_kvp_onchannelcallback(channel);
+}
+
 static void
 kvp_register(int reg_value)
 {
@@ -121,12 +127,7 @@ static void kvp_timeout_func(struct work_struct *dummy)
 	 */
 	kvp_respond_to_host(NULL, HV_E_FAIL);
 
-	/* Transaction is finished, reset the state. */
-	if (kvp_transaction.state > HVUTIL_READY)
-		kvp_transaction.state = HVUTIL_READY;
-
-	hv_poll_channel(kvp_transaction.kvp_context,
-			hv_kvp_onchannelcallback);
+	hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
 }
 
 static int kvp_handle_handshake(struct hv_kvp_msg *msg)
@@ -153,7 +154,7 @@ static int kvp_handle_handshake(struct hv_kvp_msg *msg)
 	pr_debug("KVP: userspace daemon ver. %d registered\n",
 		 KVP_OP_REGISTER);
 	kvp_register(dm_reg_value);
-	kvp_transaction.state = HVUTIL_READY;
+	hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
 
 	return 0;
 }
@@ -218,9 +219,7 @@ static int kvp_on_msg(void *msg, int len)
 	 */
 	if (cancel_delayed_work_sync(&kvp_timeout_work)) {
 		kvp_respond_to_host(message, error);
-		kvp_transaction.state = HVUTIL_READY;
-		hv_poll_channel(kvp_transaction.kvp_context,
-				hv_kvp_onchannelcallback);
+		hv_poll_channel(kvp_transaction.recv_channel, kvp_poll_wrapper);
 	}
 
 	return 0;
@@ -596,15 +595,8 @@ void hv_kvp_onchannelcallback(void *context)
 	int util_fw_version;
 	int kvp_srv_version;
 
-	if (kvp_transaction.state > HVUTIL_READY) {
-		/*
-		 * We will defer processing this callback once
-		 * the current transaction is complete.
-		 */
-		kvp_transaction.kvp_context = context;
+	if (kvp_transaction.state > HVUTIL_READY)
 		return;
-	}
-	kvp_transaction.kvp_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 4, &recvlen,
 			 &requestid);
@@ -668,7 +660,8 @@ void hv_kvp_onchannelcallback(void *context)
 			 * user-mode not responding.
 			 */
 			schedule_work(&kvp_sendkey_work);
-			schedule_delayed_work(&kvp_timeout_work, 5*HZ);
+			schedule_delayed_work(&kvp_timeout_work,
+					      HV_UTIL_TIMEOUT * HZ);
 
 			return;
 
diff --git a/drivers/hv/hv_snapshot.c b/drivers/hv/hv_snapshot.c
index 815405f2e777..67def4a831c8 100644
--- a/drivers/hv/hv_snapshot.c
+++ b/drivers/hv/hv_snapshot.c
@@ -53,7 +53,6 @@ static struct {
 	struct vmbus_channel *recv_channel; /* chn we got the request */
 	u64 recv_req_id; /* request ID. */
 	struct hv_vss_msg  *msg; /* current message */
-	void *vss_context; /* for the channel callback */
 } vss_transaction;
 
 
@@ -74,6 +73,13 @@ static void vss_timeout_func(struct work_struct *dummy);
 static DECLARE_DELAYED_WORK(vss_timeout_work, vss_timeout_func);
 static DECLARE_WORK(vss_send_op_work, vss_send_op);
 
+static void vss_poll_wrapper(void *channel)
+{
+	/* Transaction is finished, reset the state here to avoid races. */
+	vss_transaction.state = HVUTIL_READY;
+	hv_vss_onchannelcallback(channel);
+}
+
 /*
  * Callback when data is received from user mode.
  */
@@ -86,12 +92,7 @@ static void vss_timeout_func(struct work_struct *dummy)
 	pr_warn("VSS: timeout waiting for daemon to reply\n");
 	vss_respond_to_host(HV_E_FAIL);
 
-	/* Transaction is finished, reset the state. */
-	if (vss_transaction.state > HVUTIL_READY)
-		vss_transaction.state = HVUTIL_READY;
-
-	hv_poll_channel(vss_transaction.vss_context,
-			hv_vss_onchannelcallback);
+	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
 }
 
 static int vss_handle_handshake(struct hv_vss_msg *vss_msg)
@@ -112,7 +113,7 @@ static int vss_handle_handshake(struct hv_vss_msg *vss_msg)
 	default:
 		return -EINVAL;
 	}
-	vss_transaction.state = HVUTIL_READY;
+	hv_poll_channel(vss_transaction.recv_channel, vss_poll_wrapper);
 	pr_debug("VSS: userspace daemon ver. %d registered\n", dm_reg_value);
 	return 0;
 }
@@ -138,9 +139,8 @@ static int vss_on_msg(void *msg, int len)
 		if (cancel_delayed_work_sync(&vss_timeout_work)) {
 			vss_respond_to_host(vss_msg->error);
 			/* Transaction is finished, reset the state. */
-			vss_transaction.state = HVUTIL_READY;
-			hv_poll_channel(vss_transaction.vss_context,
-					hv_vss_onchannelcallback);
+			hv_poll_channel(vss_transaction.recv_channel,
+					vss_poll_wrapper);
 		}
 	} else {
 		/* This is a spurious call! */
@@ -238,15 +238,8 @@ void hv_vss_onchannelcallback(void *context)
 	struct icmsg_hdr *icmsghdrp;
 	struct icmsg_negotiate *negop = NULL;
 
-	if (vss_transaction.state > HVUTIL_READY) {
-		/*
-		 * We will defer processing this callback once
-		 * the current transaction is complete.
-		 */
-		vss_transaction.vss_context = context;
+	if (vss_transaction.state > HVUTIL_READY)
 		return;
-	}
-	vss_transaction.vss_context = NULL;
 
 	vmbus_recvpacket(channel, recv_buffer, PAGE_SIZE * 2, &recvlen,
 			 &requestid);
@@ -338,6 +331,11 @@ static void vss_on_reset(void)
 int
 hv_vss_init(struct hv_util_service *srv)
 {
+	if (vmbus_proto_version < VERSION_WIN8_1) {
+		pr_warn("Integration service 'Backup (volume snapshot)'"
+			" not supported on this host version.\n");
+		return -ENOTSUPP;
+	}
 	recv_buffer = srv->recv_buffer;
 
 	/*
diff --git a/drivers/hv/hv_utils_transport.c b/drivers/hv/hv_utils_transport.c
index 6a9d80a5332d..4f42c0e20c20 100644
--- a/drivers/hv/hv_utils_transport.c
+++ b/drivers/hv/hv_utils_transport.c
@@ -27,11 +27,9 @@ static struct list_head hvt_list = LIST_HEAD_INIT(hvt_list);
 
 static void hvt_reset(struct hvutil_transport *hvt)
 {
-	mutex_lock(&hvt->outmsg_lock);
 	kfree(hvt->outmsg);
 	hvt->outmsg = NULL;
 	hvt->outmsg_len = 0;
-	mutex_unlock(&hvt->outmsg_lock);
 	if (hvt->on_reset)
 		hvt->on_reset();
 }
@@ -44,10 +42,17 @@ static ssize_t hvt_op_read(struct file *file, char __user *buf,
 
 	hvt = container_of(file->f_op, struct hvutil_transport, fops);
 
-	if (wait_event_interruptible(hvt->outmsg_q, hvt->outmsg_len > 0))
+	if (wait_event_interruptible(hvt->outmsg_q, hvt->outmsg_len > 0 ||
+				     hvt->mode != HVUTIL_TRANSPORT_CHARDEV))
 		return -EINTR;
 
-	mutex_lock(&hvt->outmsg_lock);
+	mutex_lock(&hvt->lock);
+
+	if (hvt->mode == HVUTIL_TRANSPORT_DESTROY) {
+		ret = -EBADF;
+		goto out_unlock;
+	}
+
 	if (!hvt->outmsg) {
 		ret = -EAGAIN;
 		goto out_unlock;
@@ -68,7 +73,7 @@ static ssize_t hvt_op_read(struct file *file, char __user *buf,
 	hvt->outmsg_len = 0;
 
 out_unlock:
-	mutex_unlock(&hvt->outmsg_lock);
+	mutex_unlock(&hvt->lock);
 	return ret;
 }
 
@@ -77,19 +82,22 @@ static ssize_t hvt_op_write(struct file *file, const char __user *buf,
 {
 	struct hvutil_transport *hvt;
 	u8 *inmsg;
+	int ret;
 
 	hvt = container_of(file->f_op, struct hvutil_transport, fops);
 
-	inmsg = kzalloc(count, GFP_KERNEL);
-	if (copy_from_user(inmsg, buf, count)) {
-		kfree(inmsg);
-		return -EFAULT;
-	}
-	if (hvt->on_msg(inmsg, count))
-		return -EFAULT;
+	inmsg = memdup_user(buf, count);
+	if (IS_ERR(inmsg))
+		return PTR_ERR(inmsg);
+
+	if (hvt->mode == HVUTIL_TRANSPORT_DESTROY)
+		ret = -EBADF;
+	else
+		ret = hvt->on_msg(inmsg, count);
+
 	kfree(inmsg);
 
-	return count;
+	return ret ? ret : count;
 }
 
 static unsigned int hvt_op_poll(struct file *file, poll_table *wait)
@@ -99,6 +107,10 @@ static unsigned int hvt_op_poll(struct file *file, poll_table *wait)
 	hvt = container_of(file->f_op, struct hvutil_transport, fops);
 
 	poll_wait(file, &hvt->outmsg_q, wait);
+
+	if (hvt->mode == HVUTIL_TRANSPORT_DESTROY)
+		return POLLERR | POLLHUP;
+
 	if (hvt->outmsg_len > 0)
 		return POLLIN | POLLRDNORM;
 
@@ -108,40 +120,68 @@ static unsigned int hvt_op_poll(struct file *file, poll_table *wait)
 static int hvt_op_open(struct inode *inode, struct file *file)
 {
 	struct hvutil_transport *hvt;
+	int ret = 0;
+	bool issue_reset = false;
 
 	hvt = container_of(file->f_op, struct hvutil_transport, fops);
 
-	/*
-	 * Switching to CHARDEV mode. We switch bach to INIT when device
-	 * gets released.
-	 */
-	if (hvt->mode == HVUTIL_TRANSPORT_INIT)
+	mutex_lock(&hvt->lock);
+
+	if (hvt->mode == HVUTIL_TRANSPORT_DESTROY) {
+		ret = -EBADF;
+	} else if (hvt->mode == HVUTIL_TRANSPORT_INIT) {
+		/*
+		 * Switching to CHARDEV mode. We switch bach to INIT when
+		 * device gets released.
+		 */
 		hvt->mode = HVUTIL_TRANSPORT_CHARDEV;
+	}
 	else if (hvt->mode == HVUTIL_TRANSPORT_NETLINK) {
 		/*
 		 * We're switching from netlink communication to using char
 		 * device. Issue the reset first.
 		 */
-		hvt_reset(hvt);
+		issue_reset = true;
 		hvt->mode = HVUTIL_TRANSPORT_CHARDEV;
-	} else
-		return -EBUSY;
+	} else {
+		ret = -EBUSY;
+	}
 
-	return 0;
+	if (issue_reset)
+		hvt_reset(hvt);
+
+	mutex_unlock(&hvt->lock);
+
+	return ret;
+}
+
+static void hvt_transport_free(struct hvutil_transport *hvt)
+{
+	misc_deregister(&hvt->mdev);
+	kfree(hvt->outmsg);
+	kfree(hvt);
 }
 
 static int hvt_op_release(struct inode *inode, struct file *file)
 {
 	struct hvutil_transport *hvt;
+	int mode_old;
 
 	hvt = container_of(file->f_op, struct hvutil_transport, fops);
 
-	hvt->mode = HVUTIL_TRANSPORT_INIT;
+	mutex_lock(&hvt->lock);
+	mode_old = hvt->mode;
+	if (hvt->mode != HVUTIL_TRANSPORT_DESTROY)
+		hvt->mode = HVUTIL_TRANSPORT_INIT;
 	/*
 	 * Cleanup message buffers to avoid spurious messages when the daemon
 	 * connects back.
 	 */
 	hvt_reset(hvt);
+	mutex_unlock(&hvt->lock);
+
+	if (mode_old == HVUTIL_TRANSPORT_DESTROY)
+		hvt_transport_free(hvt);
 
 	return 0;
 }
@@ -168,6 +208,7 @@ static void hvt_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 	 * Switching to NETLINK mode. Switching to CHARDEV happens when someone
 	 * opens the device.
 	 */
+	mutex_lock(&hvt->lock);
 	if (hvt->mode == HVUTIL_TRANSPORT_INIT)
 		hvt->mode = HVUTIL_TRANSPORT_NETLINK;
 
@@ -175,6 +216,7 @@ static void hvt_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 		hvt_found->on_msg(msg->data, msg->len);
 	else
 		pr_warn("hvt_cn_callback: unexpected netlink message!\n");
+	mutex_unlock(&hvt->lock);
 }
 
 int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
@@ -182,7 +224,8 @@ int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
 	struct cn_msg *cn_msg;
 	int ret = 0;
 
-	if (hvt->mode == HVUTIL_TRANSPORT_INIT) {
+	if (hvt->mode == HVUTIL_TRANSPORT_INIT ||
+	    hvt->mode == HVUTIL_TRANSPORT_DESTROY) {
 		return -EINVAL;
 	} else if (hvt->mode == HVUTIL_TRANSPORT_NETLINK) {
 		cn_msg = kzalloc(sizeof(*cn_msg) + len, GFP_ATOMIC);
@@ -197,18 +240,26 @@ int hvutil_transport_send(struct hvutil_transport *hvt, void *msg, int len)
 		return ret;
 	}
 	/* HVUTIL_TRANSPORT_CHARDEV */
-	mutex_lock(&hvt->outmsg_lock);
+	mutex_lock(&hvt->lock);
+	if (hvt->mode != HVUTIL_TRANSPORT_CHARDEV) {
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
 	if (hvt->outmsg) {
 		/* Previous message wasn't received */
 		ret = -EFAULT;
 		goto out_unlock;
 	}
 	hvt->outmsg = kzalloc(len, GFP_KERNEL);
-	memcpy(hvt->outmsg, msg, len);
-	hvt->outmsg_len = len;
-	wake_up_interruptible(&hvt->outmsg_q);
+	if (hvt->outmsg) {
+		memcpy(hvt->outmsg, msg, len);
+		hvt->outmsg_len = len;
+		wake_up_interruptible(&hvt->outmsg_q);
+	} else
+		ret = -ENOMEM;
 out_unlock:
-	mutex_unlock(&hvt->outmsg_lock);
+	mutex_unlock(&hvt->lock);
 	return ret;
 }
 
@@ -239,7 +290,7 @@ struct hvutil_transport *hvutil_transport_init(const char *name,
 	hvt->mdev.fops = &hvt->fops;
 
 	init_waitqueue_head(&hvt->outmsg_q);
-	mutex_init(&hvt->outmsg_lock);
+	mutex_init(&hvt->lock);
 
 	spin_lock(&hvt_list_lock);
 	list_add(&hvt->list, &hvt_list);
@@ -265,12 +316,25 @@ err_free_hvt:
 
 void hvutil_transport_destroy(struct hvutil_transport *hvt)
 {
+	int mode_old;
+
+	mutex_lock(&hvt->lock);
+	mode_old = hvt->mode;
+	hvt->mode = HVUTIL_TRANSPORT_DESTROY;
+	wake_up_interruptible(&hvt->outmsg_q);
+	mutex_unlock(&hvt->lock);
+
+	/*
+	 * In case we were in 'chardev' mode we still have an open fd so we
+	 * have to defer freeing the device. Netlink interface can be freed
+	 * now.
+	 */
 	spin_lock(&hvt_list_lock);
 	list_del(&hvt->list);
 	spin_unlock(&hvt_list_lock);
 	if (hvt->cn_id.idx > 0 && hvt->cn_id.val > 0)
 		cn_del_callback(&hvt->cn_id);
-	misc_deregister(&hvt->mdev);
-	kfree(hvt->outmsg);
-	kfree(hvt);
+
+	if (mode_old != HVUTIL_TRANSPORT_CHARDEV)
+		hvt_transport_free(hvt);
 }
diff --git a/drivers/hv/hv_utils_transport.h b/drivers/hv/hv_utils_transport.h
index 314c76ce1b07..06254a165a18 100644
--- a/drivers/hv/hv_utils_transport.h
+++ b/drivers/hv/hv_utils_transport.h
@@ -25,6 +25,7 @@ enum hvutil_transport_mode {
 	HVUTIL_TRANSPORT_INIT = 0,
 	HVUTIL_TRANSPORT_NETLINK,
 	HVUTIL_TRANSPORT_CHARDEV,
+	HVUTIL_TRANSPORT_DESTROY,
 };
 
 struct hvutil_transport {
@@ -38,7 +39,7 @@ struct hvutil_transport {
 	u8 *outmsg;                         /* message to the userspace */
 	int outmsg_len;                     /* its length */
 	wait_queue_head_t outmsg_q;         /* poll/read wait queue */
-	struct mutex outmsg_lock;           /* protects outmsg */
+	struct mutex lock;                  /* protects struct members */
 };
 
 struct hvutil_transport *hvutil_transport_init(const char *name,
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 3782636562a1..4ebc796b4f33 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -31,6 +31,11 @@
 #include <linux/hyperv.h>
 
 /*
+ * Timeout for services such as KVP and fcopy.
+ */
+#define HV_UTIL_TIMEOUT 30
+
+/*
  * The below CPUID leaves are present if VersionAndFeatures.HypervisorPresent
  * is set by CPUID(HVCPUID_VERSION_FEATURES).
  */
@@ -63,10 +68,6 @@ enum hv_cpuid_function {
 /* Define version of the synthetic interrupt controller. */
 #define HV_SYNIC_VERSION		(1)
 
-/* Define synthetic interrupt controller message constants. */
-#define HV_MESSAGE_SIZE			(256)
-#define HV_MESSAGE_PAYLOAD_BYTE_COUNT	(240)
-#define HV_MESSAGE_PAYLOAD_QWORD_COUNT	(30)
 #define HV_ANY_VP			(0xFFFFFFFF)
 
 /* Define synthetic interrupt controller flag constants. */
@@ -74,48 +75,9 @@ enum hv_cpuid_function {
 #define HV_EVENT_FLAGS_BYTE_COUNT	(256)
 #define HV_EVENT_FLAGS_DWORD_COUNT	(256 / sizeof(u32))
 
-/* Define hypervisor message types. */
-enum hv_message_type {
-	HVMSG_NONE			= 0x00000000,
-
-	/* Memory access messages. */
-	HVMSG_UNMAPPED_GPA		= 0x80000000,
-	HVMSG_GPA_INTERCEPT		= 0x80000001,
-
-	/* Timer notification messages. */
-	HVMSG_TIMER_EXPIRED			= 0x80000010,
-
-	/* Error messages. */
-	HVMSG_INVALID_VP_REGISTER_VALUE	= 0x80000020,
-	HVMSG_UNRECOVERABLE_EXCEPTION	= 0x80000021,
-	HVMSG_UNSUPPORTED_FEATURE		= 0x80000022,
-
-	/* Trace buffer complete messages. */
-	HVMSG_EVENTLOG_BUFFERCOMPLETE	= 0x80000040,
-
-	/* Platform-specific processor intercept messages. */
-	HVMSG_X64_IOPORT_INTERCEPT		= 0x80010000,
-	HVMSG_X64_MSR_INTERCEPT		= 0x80010001,
-	HVMSG_X64_CPUID_INTERCEPT		= 0x80010002,
-	HVMSG_X64_EXCEPTION_INTERCEPT	= 0x80010003,
-	HVMSG_X64_APIC_EOI			= 0x80010004,
-	HVMSG_X64_LEGACY_FP_ERROR		= 0x80010005
-};
-
-#define HV_SYNIC_STIMER_COUNT		(4)
-
 /* Define invalid partition identifier. */
 #define HV_PARTITION_ID_INVALID		((u64)0x0)
 
-/* Define port identifier type. */
-union hv_port_id {
-	u32 asu32;
-	struct {
-		u32 id:24;
-		u32 reserved:8;
-	} u ;
-};
-
 /* Define port type. */
 enum hv_port_type {
 	HVPORT_MSG	= 1,
@@ -163,27 +125,6 @@ struct hv_connection_info {
 	};
 };
 
-/* Define synthetic interrupt controller message flags. */
-union hv_message_flags {
-	u8 asu8;
-	struct {
-		u8 msg_pending:1;
-		u8 reserved:7;
-	};
-};
-
-/* Define synthetic interrupt controller message header. */
-struct hv_message_header {
-	enum hv_message_type message_type;
-	u8 payload_size;
-	union hv_message_flags message_flags;
-	u8 reserved[2];
-	union {
-		u64 sender;
-		union hv_port_id port;
-	};
-};
-
 /*
  * Timer configuration register.
  */
@@ -200,31 +141,9 @@ union hv_timer_config {
 	};
 };
 
-
-/* Define timer message payload structure. */
-struct hv_timer_message_payload {
-	u32 timer_index;
-	u32 reserved;
-	u64 expiration_time;	/* When the timer expired */
-	u64 delivery_time;	/* When the message was delivered */
-};
-
-/* Define synthetic interrupt controller message format. */
-struct hv_message {
-	struct hv_message_header header;
-	union {
-		u64 payload[HV_MESSAGE_PAYLOAD_QWORD_COUNT];
-	} u ;
-};
-
 /* Define the number of message buffers associated with each port. */
 #define HV_PORT_MESSAGE_BUFFER_COUNT	(16)
 
-/* Define the synthetic interrupt message page layout. */
-struct hv_message_page {
-	struct hv_message sint_message[HV_SYNIC_SINT_COUNT];
-};
-
 /* Define the synthetic interrupt controller event flags format. */
 union hv_synic_event_flags {
 	u8 flags8[HV_EVENT_FLAGS_BYTE_COUNT];
@@ -347,7 +266,7 @@ enum hv_call_code {
 struct hv_input_post_message {
 	union hv_connection_id connectionid;
 	u32 reserved;
-	enum hv_message_type message_type;
+	u32 message_type;
 	u32 payload_size;
 	u64 payload[HV_MESSAGE_PAYLOAD_QWORD_COUNT];
 };
@@ -582,7 +501,7 @@ extern int hv_post_message(union hv_connection_id connection_id,
 			 enum hv_message_type message_type,
 			 void *payload, size_t payload_size);
 
-extern u16 hv_signal_event(void *con_id);
+extern int hv_signal_event(void *con_id);
 
 extern int hv_synic_alloc(void);
 
@@ -614,14 +533,9 @@ int hv_ringbuffer_write(struct hv_ring_buffer_info *ring_info,
 		    struct kvec *kv_list,
 		    u32 kv_count, bool *signal);
 
-int hv_ringbuffer_peek(struct hv_ring_buffer_info *ring_info, void *buffer,
-		   u32 buflen);
-
-int hv_ringbuffer_read(struct hv_ring_buffer_info *ring_info,
-		   void *buffer,
-		   u32 buflen,
-		   u32 offset, bool *signal);
-
+int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info,
+		       void *buffer, u32 buflen, u32 *buffer_actual_len,
+		       u64 *requestid, bool *signal, bool raw);
 
 void hv_ringbuffer_get_debuginfo(struct hv_ring_buffer_info *ring_info,
 			    struct hv_ring_buffer_debug_info *debug_info);
@@ -678,7 +592,7 @@ struct vmbus_connection {
 
 	/* List of channels */
 	struct list_head chn_list;
-	spinlock_t channel_lock;
+	struct mutex channel_mutex;
 
 	struct workqueue_struct *work_queue;
 };
@@ -759,11 +673,7 @@ static inline void hv_poll_channel(struct vmbus_channel *channel,
 	if (!channel)
 		return;
 
-	if (channel->target_cpu != smp_processor_id())
-		smp_call_function_single(channel->target_cpu,
-					 cb, channel, true);
-	else
-		cb(channel);
+	smp_call_function_single(channel->target_cpu, cb, channel, true);
 }
 
 enum hvutil_device_state {
diff --git a/drivers/hv/ring_buffer.c b/drivers/hv/ring_buffer.c
index 70a1a9a22f87..b53702ce692f 100644
--- a/drivers/hv/ring_buffer.c
+++ b/drivers/hv/ring_buffer.c
@@ -112,9 +112,7 @@ static bool hv_need_to_signal_on_read(u32 prev_write_sz,
 	u32 read_loc = rbi->ring_buffer->read_index;
 	u32 pending_sz = rbi->ring_buffer->pending_send_sz;
 
-	/*
-	 * If the other end is not blocked on write don't bother.
-	 */
+	/* If the other end is not blocked on write don't bother. */
 	if (pending_sz == 0)
 		return false;
 
@@ -128,12 +126,7 @@ static bool hv_need_to_signal_on_read(u32 prev_write_sz,
 	return false;
 }
 
-/*
- * hv_get_next_write_location()
- *
- * Get the next write location for the specified ring buffer
- *
- */
+/* Get the next write location for the specified ring buffer. */
 static inline u32
 hv_get_next_write_location(struct hv_ring_buffer_info *ring_info)
 {
@@ -142,12 +135,7 @@ hv_get_next_write_location(struct hv_ring_buffer_info *ring_info)
 	return next;
 }
 
-/*
- * hv_set_next_write_location()
- *
- * Set the next write location for the specified ring buffer
- *
- */
+/* Set the next write location for the specified ring buffer. */
 static inline void
 hv_set_next_write_location(struct hv_ring_buffer_info *ring_info,
 		     u32 next_write_location)
@@ -155,11 +143,7 @@ hv_set_next_write_location(struct hv_ring_buffer_info *ring_info,
 	ring_info->ring_buffer->write_index = next_write_location;
 }
 
-/*
- * hv_get_next_read_location()
- *
- * Get the next read location for the specified ring buffer
- */
+/* Get the next read location for the specified ring buffer. */
 static inline u32
 hv_get_next_read_location(struct hv_ring_buffer_info *ring_info)
 {
@@ -169,10 +153,8 @@ hv_get_next_read_location(struct hv_ring_buffer_info *ring_info)
 }
 
 /*
- * hv_get_next_readlocation_withoffset()
- *
  * Get the next read location + offset for the specified ring buffer.
- * This allows the caller to skip
+ * This allows the caller to skip.
  */
 static inline u32
 hv_get_next_readlocation_withoffset(struct hv_ring_buffer_info *ring_info,
@@ -186,13 +168,7 @@ hv_get_next_readlocation_withoffset(struct hv_ring_buffer_info *ring_info,
 	return next;
 }
 
-/*
- *
- * hv_set_next_read_location()
- *
- * Set the next read location for the specified ring buffer
- *
- */
+/* Set the next read location for the specified ring buffer. */
 static inline void
 hv_set_next_read_location(struct hv_ring_buffer_info *ring_info,
 		    u32 next_read_location)
@@ -201,12 +177,7 @@ hv_set_next_read_location(struct hv_ring_buffer_info *ring_info,
 }
 
 
-/*
- *
- * hv_get_ring_buffer()
- *
- * Get the start of the ring buffer
- */
+/* Get the start of the ring buffer. */
 static inline void *
 hv_get_ring_buffer(struct hv_ring_buffer_info *ring_info)
 {
@@ -214,25 +185,14 @@ hv_get_ring_buffer(struct hv_ring_buffer_info *ring_info)
 }
 
 
-/*
- *
- * hv_get_ring_buffersize()
- *
- * Get the size of the ring buffer
- */
+/* Get the size of the ring buffer. */
 static inline u32
 hv_get_ring_buffersize(struct hv_ring_buffer_info *ring_info)
 {
 	return ring_info->ring_datasize;
 }
 
-/*
- *
- * hv_get_ring_bufferindices()
- *
- * Get the read and write indices as u64 of the specified ring buffer
- *
- */
+/* Get the read and write indices as u64 of the specified ring buffer. */
 static inline u64
 hv_get_ring_bufferindices(struct hv_ring_buffer_info *ring_info)
 {
@@ -240,12 +200,8 @@ hv_get_ring_bufferindices(struct hv_ring_buffer_info *ring_info)
 }
 
 /*
- *
- * hv_copyfrom_ringbuffer()
- *
  * Helper routine to copy to source from ring buffer.
  * Assume there is enough room. Handles wrap-around in src case only!!
- *
  */
 static u32 hv_copyfrom_ringbuffer(
 	struct hv_ring_buffer_info	*ring_info,
@@ -277,12 +233,8 @@ static u32 hv_copyfrom_ringbuffer(
 
 
 /*
- *
- * hv_copyto_ringbuffer()
- *
  * Helper routine to copy from source to ring buffer.
  * Assume there is enough room. Handles wrap-around in dest case only!!
- *
  */
 static u32 hv_copyto_ringbuffer(
 	struct hv_ring_buffer_info	*ring_info,
@@ -308,13 +260,7 @@ static u32 hv_copyto_ringbuffer(
 	return start_write_offset;
 }
 
-/*
- *
- * hv_ringbuffer_get_debuginfo()
- *
- * Get various debug metrics for the specified ring buffer
- *
- */
+/* Get various debug metrics for the specified ring buffer. */
 void hv_ringbuffer_get_debuginfo(struct hv_ring_buffer_info *ring_info,
 			    struct hv_ring_buffer_debug_info *debug_info)
 {
@@ -337,13 +283,7 @@ void hv_ringbuffer_get_debuginfo(struct hv_ring_buffer_info *ring_info,
 	}
 }
 
-/*
- *
- * hv_ringbuffer_init()
- *
- *Initialize the ring buffer
- *
- */
+/* Initialize the ring buffer. */
 int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
 		   void *buffer, u32 buflen)
 {
@@ -356,9 +296,7 @@ int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
 	ring_info->ring_buffer->read_index =
 		ring_info->ring_buffer->write_index = 0;
 
-	/*
-	 * Set the feature bit for enabling flow control.
-	 */
+	/* Set the feature bit for enabling flow control. */
 	ring_info->ring_buffer->feature_bits.value = 1;
 
 	ring_info->ring_size = buflen;
@@ -369,24 +307,12 @@ int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
 	return 0;
 }
 
-/*
- *
- * hv_ringbuffer_cleanup()
- *
- * Cleanup the ring buffer
- *
- */
+/* Cleanup the ring buffer. */
 void hv_ringbuffer_cleanup(struct hv_ring_buffer_info *ring_info)
 {
 }
 
-/*
- *
- * hv_ringbuffer_write()
- *
- * Write to the 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)
 {
@@ -411,10 +337,11 @@ int hv_ringbuffer_write(struct hv_ring_buffer_info *outring_info,
 				&bytes_avail_toread,
 				&bytes_avail_towrite);
 
-
-	/* If there is only room for the packet, assume it is full. */
-	/* Otherwise, the next time around, we think the ring buffer */
-	/* is empty since the read index == write index */
+	/*
+	 * If there is only room for the packet, assume it is full.
+	 * Otherwise, the next time around, we think the ring buffer
+	 * is empty since the read index == write index.
+	 */
 	if (bytes_avail_towrite <= totalbytes_towrite) {
 		spin_unlock_irqrestore(&outring_info->ring_lock, flags);
 		return -EAGAIN;
@@ -453,80 +380,59 @@ int hv_ringbuffer_write(struct hv_ring_buffer_info *outring_info,
 	return 0;
 }
 
-
-/*
- *
- * hv_ringbuffer_peek()
- *
- * Read without advancing the read index
- *
- */
-int hv_ringbuffer_peek(struct hv_ring_buffer_info *Inring_info,
-		   void *Buffer, u32 buflen)
-{
-	u32 bytes_avail_towrite;
-	u32 bytes_avail_toread;
-	u32 next_read_location = 0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&Inring_info->ring_lock, flags);
-
-	hv_get_ringbuffer_availbytes(Inring_info,
-				&bytes_avail_toread,
-				&bytes_avail_towrite);
-
-	/* Make sure there is something to read */
-	if (bytes_avail_toread < buflen) {
-
-		spin_unlock_irqrestore(&Inring_info->ring_lock, flags);
-
-		return -EAGAIN;
-	}
-
-	/* Convert to byte offset */
-	next_read_location = hv_get_next_read_location(Inring_info);
-
-	next_read_location = hv_copyfrom_ringbuffer(Inring_info,
-						Buffer,
-						buflen,
-						next_read_location);
-
-	spin_unlock_irqrestore(&Inring_info->ring_lock, flags);
-
-	return 0;
-}
-
-
-/*
- *
- * hv_ringbuffer_read()
- *
- * Read and advance the read index
- *
- */
-int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info, void *buffer,
-		   u32 buflen, u32 offset, bool *signal)
+int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info,
+		       void *buffer, u32 buflen, u32 *buffer_actual_len,
+		       u64 *requestid, bool *signal, bool raw)
 {
 	u32 bytes_avail_towrite;
 	u32 bytes_avail_toread;
 	u32 next_read_location = 0;
 	u64 prev_indices = 0;
 	unsigned long flags;
+	struct vmpacket_descriptor desc;
+	u32 offset;
+	u32 packetlen;
+	int ret = 0;
 
 	if (buflen <= 0)
 		return -EINVAL;
 
 	spin_lock_irqsave(&inring_info->ring_lock, flags);
 
+	*buffer_actual_len = 0;
+	*requestid = 0;
+
 	hv_get_ringbuffer_availbytes(inring_info,
 				&bytes_avail_toread,
 				&bytes_avail_towrite);
 
 	/* Make sure there is something to read */
-	if (bytes_avail_toread < buflen) {
-		spin_unlock_irqrestore(&inring_info->ring_lock, flags);
+	if (bytes_avail_toread < sizeof(desc)) {
+		/*
+		 * No error is set when there is even no header, drivers are
+		 * supposed to analyze buffer_actual_len.
+		 */
+		goto out_unlock;
+	}
 
-		return -EAGAIN;
+	next_read_location = hv_get_next_read_location(inring_info);
+	next_read_location = hv_copyfrom_ringbuffer(inring_info, &desc,
+						    sizeof(desc),
+						    next_read_location);
+
+	offset = raw ? 0 : (desc.offset8 << 3);
+	packetlen = (desc.len8 << 3) - offset;
+	*buffer_actual_len = packetlen;
+	*requestid = desc.trans_id;
+
+	if (bytes_avail_toread < packetlen + offset) {
+		ret = -EAGAIN;
+		goto out_unlock;
+	}
+
+	if (packetlen > buflen) {
+		ret = -ENOBUFS;
+		goto out_unlock;
 	}
 
 	next_read_location =
@@ -534,7 +440,7 @@ int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info, void *buffer,
 
 	next_read_location = hv_copyfrom_ringbuffer(inring_info,
 						buffer,
-						buflen,
+						packetlen,
 						next_read_location);
 
 	next_read_location = hv_copyfrom_ringbuffer(inring_info,
@@ -542,17 +448,19 @@ int hv_ringbuffer_read(struct hv_ring_buffer_info *inring_info, void *buffer,
 						sizeof(u64),
 						next_read_location);
 
-	/* Make sure all reads are done before we update the read index since */
-	/* the writer may start writing to the read area once the read index */
-	/*is updated */
+	/*
+	 * Make sure all reads are done before we update the read index since
+	 * the writer may start writing to the read area once the read index
+	 * is updated.
+	 */
 	mb();
 
 	/* Update the read index */
 	hv_set_next_read_location(inring_info, next_read_location);
 
-	spin_unlock_irqrestore(&inring_info->ring_lock, flags);
-
 	*signal = hv_need_to_signal_on_read(bytes_avail_towrite, inring_info);
 
-	return 0;
+out_unlock:
+	spin_unlock_irqrestore(&inring_info->ring_lock, flags);
+	return ret;
 }
diff --git a/drivers/hv/vmbus_drv.c b/drivers/hv/vmbus_drv.c
index f19b6f7a467a..328e4c3808e0 100644
--- a/drivers/hv/vmbus_drv.c
+++ b/drivers/hv/vmbus_drv.c
@@ -47,7 +47,6 @@ static struct acpi_device  *hv_acpi_dev;
 
 static struct tasklet_struct msg_dpc;
 static struct completion probe_event;
-static int irq;
 
 
 static void hyperv_report_panic(struct pt_regs *regs)
@@ -531,9 +530,9 @@ static int vmbus_uevent(struct device *device, struct kobj_uevent_env *env)
 
 static const uuid_le null_guid;
 
-static inline bool is_null_guid(const __u8 *guid)
+static inline bool is_null_guid(const uuid_le *guid)
 {
-	if (memcmp(guid, &null_guid, sizeof(uuid_le)))
+	if (uuid_le_cmp(*guid, null_guid))
 		return false;
 	return true;
 }
@@ -544,10 +543,10 @@ static inline bool is_null_guid(const __u8 *guid)
  */
 static const struct hv_vmbus_device_id *hv_vmbus_get_id(
 					const struct hv_vmbus_device_id *id,
-					const __u8 *guid)
+					const uuid_le *guid)
 {
-	for (; !is_null_guid(id->guid); id++)
-		if (!memcmp(&id->guid, guid, sizeof(uuid_le)))
+	for (; !is_null_guid(&id->guid); id++)
+		if (!uuid_le_cmp(id->guid, *guid))
 			return id;
 
 	return NULL;
@@ -563,7 +562,7 @@ static int vmbus_match(struct device *device, struct device_driver *driver)
 	struct hv_driver *drv = drv_to_hv_drv(driver);
 	struct hv_device *hv_dev = device_to_hv_device(device);
 
-	if (hv_vmbus_get_id(drv->id_table, hv_dev->dev_type.b))
+	if (hv_vmbus_get_id(drv->id_table, &hv_dev->dev_type))
 		return 1;
 
 	return 0;
@@ -580,7 +579,7 @@ static int vmbus_probe(struct device *child_device)
 	struct hv_device *dev = device_to_hv_device(child_device);
 	const struct hv_vmbus_device_id *dev_id;
 
-	dev_id = hv_vmbus_get_id(drv->id_table, dev->dev_type.b);
+	dev_id = hv_vmbus_get_id(drv->id_table, &dev->dev_type);
 	if (drv->probe) {
 		ret = drv->probe(dev, dev_id);
 		if (ret != 0)
@@ -602,23 +601,11 @@ static int vmbus_remove(struct device *child_device)
 {
 	struct hv_driver *drv;
 	struct hv_device *dev = device_to_hv_device(child_device);
-	u32 relid = dev->channel->offermsg.child_relid;
 
 	if (child_device->driver) {
 		drv = drv_to_hv_drv(child_device->driver);
 		if (drv->remove)
 			drv->remove(dev);
-		else {
-			hv_process_channel_removal(dev->channel, relid);
-			pr_err("remove not set for driver %s\n",
-				dev_name(child_device));
-		}
-	} else {
-		/*
-		 * We don't have a driver for this device; deal with the
-		 * rescind message by removing the channel.
-		 */
-		hv_process_channel_removal(dev->channel, relid);
 	}
 
 	return 0;
@@ -653,7 +640,10 @@ static void vmbus_shutdown(struct device *child_device)
 static void vmbus_device_release(struct device *device)
 {
 	struct hv_device *hv_dev = device_to_hv_device(device);
+	struct vmbus_channel *channel = hv_dev->channel;
 
+	hv_process_channel_removal(channel,
+				   channel->offermsg.child_relid);
 	kfree(hv_dev);
 
 }
@@ -835,10 +825,9 @@ static void vmbus_isr(void)
  * Here, we
  *	- initialize the vmbus driver context
  *	- invoke the vmbus hv main init routine
- *	- get the irq resource
  *	- retrieve the channel offers
  */
-static int vmbus_bus_init(int irq)
+static int vmbus_bus_init(void)
 {
 	int ret;
 
@@ -867,7 +856,7 @@ static int vmbus_bus_init(int irq)
 	on_each_cpu(hv_synic_init, NULL, 1);
 	ret = vmbus_connect();
 	if (ret)
-		goto err_alloc;
+		goto err_connect;
 
 	if (vmbus_proto_version > VERSION_WIN7)
 		cpu_hotplug_disable();
@@ -885,6 +874,8 @@ static int vmbus_bus_init(int irq)
 
 	return 0;
 
+err_connect:
+	on_each_cpu(hv_synic_cleanup, NULL, 1);
 err_alloc:
 	hv_synic_free();
 	hv_remove_vmbus_irq();
@@ -1031,9 +1022,6 @@ static acpi_status vmbus_walk_resources(struct acpi_resource *res, void *ctx)
 	struct resource **prev_res = NULL;
 
 	switch (res->type) {
-	case ACPI_RESOURCE_TYPE_IRQ:
-		irq = res->data.irq.interrupts[0];
-		return AE_OK;
 
 	/*
 	 * "Address" descriptors are for bus windows. Ignore
@@ -1075,12 +1063,28 @@ static acpi_status vmbus_walk_resources(struct acpi_resource *res, void *ctx)
 	new_res->start = start;
 	new_res->end = end;
 
+	/*
+	 * Stick ranges from higher in address space at the front of the list.
+	 * If two ranges are adjacent, merge them.
+	 */
 	do {
 		if (!*old_res) {
 			*old_res = new_res;
 			break;
 		}
 
+		if (((*old_res)->end + 1) == new_res->start) {
+			(*old_res)->end = new_res->end;
+			kfree(new_res);
+			break;
+		}
+
+		if ((*old_res)->start == new_res->end + 1) {
+			(*old_res)->start = new_res->start;
+			kfree(new_res);
+			break;
+		}
+
 		if ((*old_res)->end < new_res->start) {
 			new_res->sibling = *old_res;
 			if (prev_res)
@@ -1191,6 +1195,23 @@ int vmbus_allocate_mmio(struct resource **new, struct hv_device *device_obj,
 }
 EXPORT_SYMBOL_GPL(vmbus_allocate_mmio);
 
+/**
+ * vmbus_cpu_number_to_vp_number() - Map CPU to VP.
+ * @cpu_number: CPU number in Linux terms
+ *
+ * This function returns the mapping between the Linux processor
+ * number and the hypervisor's virtual processor number, useful
+ * in making hypercalls and such that talk about specific
+ * processors.
+ *
+ * Return: Virtual processor number in Hyper-V terms
+ */
+int vmbus_cpu_number_to_vp_number(int cpu_number)
+{
+	return hv_context.vp_index[cpu_number];
+}
+EXPORT_SYMBOL_GPL(vmbus_cpu_number_to_vp_number);
+
 static int vmbus_acpi_add(struct acpi_device *device)
 {
 	acpi_status result;
@@ -1275,7 +1296,7 @@ static int __init hv_acpi_init(void)
 	init_completion(&probe_event);
 
 	/*
-	 * Get irq resources first.
+	 * Get ACPI resources first.
 	 */
 	ret = acpi_bus_register_driver(&vmbus_acpi_driver);
 
@@ -1288,12 +1309,7 @@ static int __init hv_acpi_init(void)
 		goto cleanup;
 	}
 
-	if (irq <= 0) {
-		ret = -ENODEV;
-		goto cleanup;
-	}
-
-	ret = vmbus_bus_init(irq);
+	ret = vmbus_bus_init();
 	if (ret)
 		goto cleanup;
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index 80a73bfc1a65..60fb80bd353d 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -859,16 +859,6 @@ config SENSORS_MAX31790
 	  This driver can also be built as a module.  If so, the module
 	  will be called max31790.
 
-config SENSORS_HTU21
-	tristate "Measurement Specialties HTU21D humidity/temperature sensors"
-	depends on I2C
-	help
-	  If you say yes here you get support for the Measurement Specialties
-	  HTU21D humidity and temperature sensors.
-
-	  This driver can also be built as a module.  If so, the module
-	  will be called htu21.
-
 config SENSORS_MCP3021
 	tristate "Microchip MCP3021 and compatibles"
 	depends on I2C
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 12a32398fdcc..30c94df31465 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -68,7 +68,6 @@ obj-$(CONFIG_SENSORS_GL518SM)	+= gl518sm.o
 obj-$(CONFIG_SENSORS_GL520SM)	+= gl520sm.o
 obj-$(CONFIG_SENSORS_GPIO_FAN)	+= gpio-fan.o
 obj-$(CONFIG_SENSORS_HIH6130)	+= hih6130.o
-obj-$(CONFIG_SENSORS_HTU21)	+= htu21.o
 obj-$(CONFIG_SENSORS_ULTRA45)	+= ultra45_env.o
 obj-$(CONFIG_SENSORS_I5500)	+= i5500_temp.o
 obj-$(CONFIG_SENSORS_I5K_AMB)	+= i5k_amb.o
diff --git a/drivers/hwmon/ads1015.c b/drivers/hwmon/ads1015.c
index f155b8380481..2b3105c8aed3 100644
--- a/drivers/hwmon/ads1015.c
+++ b/drivers/hwmon/ads1015.c
@@ -126,7 +126,7 @@ static int ads1015_reg_to_mv(struct i2c_client *client, unsigned int channel,
 	struct ads1015_data *data = i2c_get_clientdata(client);
 	unsigned int pga = data->channel_data[channel].pga;
 	int fullscale = fullscale_table[pga];
-	const unsigned mask = data->id == ads1115 ? 0x7fff : 0x7ff0;
+	const int mask = data->id == ads1115 ? 0x7fff : 0x7ff0;
 
 	return DIV_ROUND_CLOSEST(reg * fullscale, mask);
 }
diff --git a/drivers/hwmon/dell-smm-hwmon.c b/drivers/hwmon/dell-smm-hwmon.c
index c8487894b312..c43318d3416e 100644
--- a/drivers/hwmon/dell-smm-hwmon.c
+++ b/drivers/hwmon/dell-smm-hwmon.c
@@ -932,6 +932,17 @@ MODULE_DEVICE_TABLE(dmi, i8k_dmi_table);
 static struct dmi_system_id i8k_blacklist_dmi_table[] __initdata = {
 	{
 		/*
+		 * CPU fan speed going up and down on Dell Studio XPS 8000
+		 * for unknown reasons.
+		 */
+		.ident = "Dell Studio XPS 8000",
+		.matches = {
+			DMI_EXACT_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_EXACT_MATCH(DMI_PRODUCT_NAME, "Studio XPS 8000"),
+		},
+	},
+	{
+		/*
 		 * CPU fan speed going up and down on Dell Studio XPS 8100
 		 * for unknown reasons.
 		 */
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index 5f7067d7b625..4f695d8fcafa 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -47,6 +47,8 @@ MODULE_LICENSE("GPL");
 
 #define MSR_F15H_CU_MAX_PWR_ACCUMULATOR	0xc001007b
 
+#define PCI_DEVICE_ID_AMD_15H_M70H_NB_F4 0x15b4
+
 struct fam15h_power_data {
 	struct pci_dev *pdev;
 	unsigned int tdp_to_watts;
@@ -88,7 +90,15 @@ static ssize_t show_power(struct device *dev,
 	pci_bus_read_config_dword(f4->bus, PCI_DEVFN(PCI_SLOT(f4->devfn), 5),
 				  REG_TDP_LIMIT3, &val);
 
-	tdp_limit = val >> 16;
+	/*
+	 * On Carrizo and later platforms, ApmTdpLimit bit field
+	 * is extended to 16:31 from 16:28.
+	 */
+	if (boot_cpu_data.x86 == 0x15 && boot_cpu_data.x86_model >= 0x60)
+		tdp_limit = val >> 16;
+	else
+		tdp_limit = (val >> 16) & 0x1fff;
+
 	curr_pwr_watts = ((u64)(tdp_limit +
 				data->base_tdp)) << running_avg_range;
 	curr_pwr_watts -= running_avg_capture;
@@ -124,7 +134,7 @@ static int fam15h_power_init_attrs(struct pci_dev *pdev,
 
 	if (c->x86 == 0x15 &&
 	    (c->x86_model <= 0xf ||
-	     (c->x86_model >= 0x60 && c->x86_model <= 0x6f)))
+	     (c->x86_model >= 0x60 && c->x86_model <= 0x7f)))
 		n += 1;
 
 	fam15h_power_attrs = devm_kcalloc(&pdev->dev, n,
@@ -138,7 +148,7 @@ static int fam15h_power_init_attrs(struct pci_dev *pdev,
 	fam15h_power_attrs[n++] = &dev_attr_power1_crit.attr;
 	if (c->x86 == 0x15 &&
 	    (c->x86_model <= 0xf ||
-	     (c->x86_model >= 0x60 && c->x86_model <= 0x6f)))
+	     (c->x86_model >= 0x60 && c->x86_model <= 0x7f)))
 		fam15h_power_attrs[n++] = &dev_attr_power1_input.attr;
 
 	data->group.attrs = fam15h_power_attrs;
@@ -296,6 +306,7 @@ static const struct pci_device_id fam15h_power_id_table[] = {
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_NB_F4) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M30H_NB_F4) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M60H_NB_F4) },
+	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M70H_NB_F4) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_NB_F4) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_M30H_NB_F4) },
 	{}
diff --git a/drivers/hwmon/gpio-fan.c b/drivers/hwmon/gpio-fan.c
index 82de3deeb18a..685568b1236d 100644
--- a/drivers/hwmon/gpio-fan.c
+++ b/drivers/hwmon/gpio-fan.c
@@ -406,16 +406,11 @@ static int gpio_fan_get_cur_state(struct thermal_cooling_device *cdev,
 				  unsigned long *state)
 {
 	struct gpio_fan_data *fan_data = cdev->devdata;
-	int r;
 
 	if (!fan_data)
 		return -EINVAL;
 
-	r = get_fan_speed_index(fan_data);
-	if (r < 0)
-		return r;
-
-	*state = r;
+	*state = fan_data->speed_index;
 	return 0;
 }
 
diff --git a/drivers/hwmon/htu21.c b/drivers/hwmon/htu21.c
deleted file mode 100644
index 4c3bbb72f82a..000000000000
--- a/drivers/hwmon/htu21.c
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Measurement Specialties HTU21D humidity and temperature sensor driver
- *
- * Copyright (C) 2013 William Markezana <william.markezana@meas-spec.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/slab.h>
-#include <linux/i2c.h>
-#include <linux/hwmon.h>
-#include <linux/hwmon-sysfs.h>
-#include <linux/err.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/jiffies.h>
-
-/* HTU21 Commands */
-#define HTU21_T_MEASUREMENT_HM	0xE3
-#define HTU21_RH_MEASUREMENT_HM	0xE5
-
-struct htu21 {
-	struct i2c_client *client;
-	struct mutex lock;
-	bool valid;
-	unsigned long last_update;
-	int temperature;
-	int humidity;
-};
-
-static inline int htu21_temp_ticks_to_millicelsius(int ticks)
-{
-	ticks &= ~0x0003; /* clear status bits */
-	/*
-	 * Formula T = -46.85 + 175.72 * ST / 2^16 from datasheet p14,
-	 * optimized for integer fixed point (3 digits) arithmetic
-	 */
-	return ((21965 * ticks) >> 13) - 46850;
-}
-
-static inline int htu21_rh_ticks_to_per_cent_mille(int ticks)
-{
-	ticks &= ~0x0003; /* clear status bits */
-	/*
-	 * Formula RH = -6 + 125 * SRH / 2^16 from datasheet p14,
-	 * optimized for integer fixed point (3 digits) arithmetic
-	 */
-	return ((15625 * ticks) >> 13) - 6000;
-}
-
-static int htu21_update_measurements(struct device *dev)
-{
-	struct htu21 *htu21 = dev_get_drvdata(dev);
-	struct i2c_client *client = htu21->client;
-	int ret = 0;
-
-	mutex_lock(&htu21->lock);
-
-	if (time_after(jiffies, htu21->last_update + HZ / 2) ||
-	    !htu21->valid) {
-		ret = i2c_smbus_read_word_swapped(client,
-						  HTU21_T_MEASUREMENT_HM);
-		if (ret < 0)
-			goto out;
-		htu21->temperature = htu21_temp_ticks_to_millicelsius(ret);
-		ret = i2c_smbus_read_word_swapped(client,
-						  HTU21_RH_MEASUREMENT_HM);
-		if (ret < 0)
-			goto out;
-		htu21->humidity = htu21_rh_ticks_to_per_cent_mille(ret);
-		htu21->last_update = jiffies;
-		htu21->valid = true;
-	}
-out:
-	mutex_unlock(&htu21->lock);
-
-	return ret >= 0 ? 0 : ret;
-}
-
-static ssize_t htu21_show_temperature(struct device *dev,
-				      struct device_attribute *attr, char *buf)
-{
-	struct htu21 *htu21 = dev_get_drvdata(dev);
-	int ret;
-
-	ret = htu21_update_measurements(dev);
-	if (ret < 0)
-		return ret;
-	return sprintf(buf, "%d\n", htu21->temperature);
-}
-
-static ssize_t htu21_show_humidity(struct device *dev,
-				   struct device_attribute *attr, char *buf)
-{
-	struct htu21 *htu21 = dev_get_drvdata(dev);
-	int ret;
-
-	ret = htu21_update_measurements(dev);
-	if (ret < 0)
-		return ret;
-	return sprintf(buf, "%d\n", htu21->humidity);
-}
-
-static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO,
-			  htu21_show_temperature, NULL, 0);
-static SENSOR_DEVICE_ATTR(humidity1_input, S_IRUGO,
-			  htu21_show_humidity, NULL, 0);
-
-static struct attribute *htu21_attrs[] = {
-	&sensor_dev_attr_temp1_input.dev_attr.attr,
-	&sensor_dev_attr_humidity1_input.dev_attr.attr,
-	NULL
-};
-
-ATTRIBUTE_GROUPS(htu21);
-
-static int htu21_probe(struct i2c_client *client,
-		       const struct i2c_device_id *id)
-{
-	struct device *dev = &client->dev;
-	struct htu21 *htu21;
-	struct device *hwmon_dev;
-
-	if (!i2c_check_functionality(client->adapter,
-				     I2C_FUNC_SMBUS_READ_WORD_DATA)) {
-		dev_err(&client->dev,
-			"adapter does not support SMBus word transactions\n");
-		return -ENODEV;
-	}
-
-	htu21 = devm_kzalloc(dev, sizeof(*htu21), GFP_KERNEL);
-	if (!htu21)
-		return -ENOMEM;
-
-	htu21->client = client;
-	mutex_init(&htu21->lock);
-
-	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
-							   htu21,
-							   htu21_groups);
-	return PTR_ERR_OR_ZERO(hwmon_dev);
-}
-
-static const struct i2c_device_id htu21_id[] = {
-	{ "htu21", 0 },
-	{ }
-};
-MODULE_DEVICE_TABLE(i2c, htu21_id);
-
-static struct i2c_driver htu21_driver = {
-	.class = I2C_CLASS_HWMON,
-	.driver = {
-		.name	= "htu21",
-	},
-	.probe       = htu21_probe,
-	.id_table    = htu21_id,
-};
-
-module_i2c_driver(htu21_driver);
-
-MODULE_AUTHOR("William Markezana <william.markezana@meas-spec.com>");
-MODULE_DESCRIPTION("MEAS HTU21D humidity and temperature sensor driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/ibmaem.c b/drivers/hwmon/ibmaem.c
index 7a8a6fbf11ff..1f643782ce04 100644
--- a/drivers/hwmon/ibmaem.c
+++ b/drivers/hwmon/ibmaem.c
@@ -920,8 +920,8 @@ static ssize_t aem_set_power_period(struct device *dev,
 
 /* Discover sensors on an AEM device */
 static int aem_register_sensors(struct aem_data *data,
-				struct aem_ro_sensor_template *ro,
-				struct aem_rw_sensor_template *rw)
+				const struct aem_ro_sensor_template *ro,
+				const struct aem_rw_sensor_template *rw)
 {
 	struct device *dev = &data->pdev->dev;
 	struct sensor_device_attribute *sensors = data->sensors;
@@ -1020,19 +1020,19 @@ static void aem_remove_sensors(struct aem_data *data)
 /* Sensor probe functions */
 
 /* Description of AEM1 sensors */
-static struct aem_ro_sensor_template aem1_ro_sensors[] = {
+static const struct aem_ro_sensor_template aem1_ro_sensors[] = {
 {"energy1_input",  aem_show_energy, 0},
 {"power1_average", aem_show_power,  0},
 {NULL,		   NULL,	    0},
 };
 
-static struct aem_rw_sensor_template aem1_rw_sensors[] = {
+static const struct aem_rw_sensor_template aem1_rw_sensors[] = {
 {"power1_average_interval", aem_show_power_period, aem_set_power_period, 0},
 {NULL,			    NULL,                  NULL,                 0},
 };
 
 /* Description of AEM2 sensors */
-static struct aem_ro_sensor_template aem2_ro_sensors[] = {
+static const struct aem_ro_sensor_template aem2_ro_sensors[] = {
 {"energy1_input",	  aem_show_energy,	0},
 {"energy2_input",	  aem_show_energy,	1},
 {"power1_average",	  aem_show_power,	0},
@@ -1050,7 +1050,7 @@ static struct aem_ro_sensor_template aem2_ro_sensors[] = {
 {NULL,                    NULL,                 0},
 };
 
-static struct aem_rw_sensor_template aem2_rw_sensors[] = {
+static const struct aem_rw_sensor_template aem2_rw_sensors[] = {
 {"power1_average_interval", aem_show_power_period, aem_set_power_period, 0},
 {"power2_average_interval", aem_show_power_period, aem_set_power_period, 1},
 {NULL,			    NULL,                  NULL,                 0},
diff --git a/drivers/hwmon/nct6683.c b/drivers/hwmon/nct6683.c
index 37f01702d081..559c596b24f9 100644
--- a/drivers/hwmon/nct6683.c
+++ b/drivers/hwmon/nct6683.c
@@ -29,7 +29,7 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/acpi.h>
-#include <linux/dmi.h>
+#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/init.h>
 #include <linux/io.h>
@@ -45,7 +45,7 @@ enum kinds { nct6683 };
 
 static bool force;
 module_param(force, bool, 0);
-MODULE_PARM_DESC(force, "Set to one to enable detection on non-Intel boards");
+MODULE_PARM_DESC(force, "Set to one to enable support for unknown vendors");
 
 static const char * const nct6683_device_names[] = {
 	"nct6683",
@@ -141,6 +141,7 @@ superio_exit(int ioreg)
 #define NCT6683_REG_MON(x)		(0x100 + (x) * 2)
 #define NCT6683_REG_FAN_RPM(x)		(0x140 + (x) * 2)
 #define NCT6683_REG_PWM(x)		(0x160 + (x))
+#define NCT6683_REG_PWM_WRITE(x)	(0xa28 + (x))
 
 #define NCT6683_REG_MON_STS(x)		(0x174 + (x))
 #define NCT6683_REG_IDLE(x)		(0x178 + (x))
@@ -165,8 +166,13 @@ superio_exit(int ioreg)
 
 #define NCT6683_REG_FAN_MIN(x)		(0x3b8 + (x) * 2)	/* 16 bit */
 
+#define NCT6683_REG_FAN_CFG_CTRL	0xa01
+#define NCT6683_FAN_CFG_REQ		0x80
+#define NCT6683_FAN_CFG_DONE		0x40
+
 #define NCT6683_REG_CUSTOMER_ID		0x602
 #define NCT6683_CUSTOMER_ID_INTEL	0x805
+#define NCT6683_CUSTOMER_ID_MITAC	0xa0e
 
 #define NCT6683_REG_BUILD_YEAR		0x604
 #define NCT6683_REG_BUILD_MONTH		0x605
@@ -394,7 +400,8 @@ struct sensor_template_group {
 };
 
 static struct attribute_group *
-nct6683_create_attr_group(struct device *dev, struct sensor_template_group *tg,
+nct6683_create_attr_group(struct device *dev,
+			  const struct sensor_template_group *tg,
 			  int repeat)
 {
 	struct sensor_device_attribute_2 *a2;
@@ -559,6 +566,7 @@ static int get_temp_reg(struct nct6683_data *data, int nr, int index)
 			break;
 		}
 		break;
+	case NCT6683_CUSTOMER_ID_MITAC:
 	default:
 		switch (nr) {
 		default:
@@ -703,7 +711,7 @@ static struct sensor_device_template *nct6683_attributes_in_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6683_in_template_group = {
+static const struct sensor_template_group nct6683_in_template_group = {
 	.templates = nct6683_attributes_in_template,
 	.is_visible = nct6683_in_is_visible,
 };
@@ -774,7 +782,7 @@ static struct sensor_device_template *nct6683_attributes_fan_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6683_fan_template_group = {
+static const struct sensor_template_group nct6683_fan_template_group = {
 	.templates = nct6683_attributes_fan_template,
 	.is_visible = nct6683_fan_is_visible,
 	.base = 1,
@@ -902,7 +910,7 @@ static struct sensor_device_template *nct6683_attributes_temp_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6683_temp_template_group = {
+static const struct sensor_template_group nct6683_temp_template_group = {
 	.templates = nct6683_attributes_temp_template,
 	.is_visible = nct6683_temp_is_visible,
 	.base = 1,
@@ -918,7 +926,29 @@ show_pwm(struct device *dev, struct device_attribute *attr, char *buf)
 	return sprintf(buf, "%d\n", data->pwm[index]);
 }
 
-SENSOR_TEMPLATE(pwm, "pwm%d", S_IRUGO, show_pwm, NULL, 0);
+static ssize_t
+store_pwm(struct device *dev, struct device_attribute *attr, const char *buf,
+	  size_t count)
+{
+	struct sensor_device_attribute_2 *sattr = to_sensor_dev_attr_2(attr);
+	struct nct6683_data *data = dev_get_drvdata(dev);
+	int index = sattr->index;
+	unsigned long val;
+
+	if (kstrtoul(buf, 10, &val) || val > 255)
+		return -EINVAL;
+
+	mutex_lock(&data->update_lock);
+	nct6683_write(data, NCT6683_REG_FAN_CFG_CTRL, NCT6683_FAN_CFG_REQ);
+	usleep_range(1000, 2000);
+	nct6683_write(data, NCT6683_REG_PWM_WRITE(index), val);
+	nct6683_write(data, NCT6683_REG_FAN_CFG_CTRL, NCT6683_FAN_CFG_DONE);
+	mutex_unlock(&data->update_lock);
+
+	return count;
+}
+
+SENSOR_TEMPLATE(pwm, "pwm%d", S_IRUGO, show_pwm, store_pwm, 0);
 
 static umode_t nct6683_pwm_is_visible(struct kobject *kobj,
 				      struct attribute *attr, int index)
@@ -930,6 +960,10 @@ static umode_t nct6683_pwm_is_visible(struct kobject *kobj,
 	if (!(data->have_pwm & (1 << pwm)))
 		return 0;
 
+	/* Only update pwm values for Mitac boards */
+	if (data->customer_id == NCT6683_CUSTOMER_ID_MITAC)
+		return attr->mode | S_IWUSR;
+
 	return attr->mode;
 }
 
@@ -938,7 +972,7 @@ static struct sensor_device_template *nct6683_attributes_pwm_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6683_pwm_template_group = {
+static const struct sensor_template_group nct6683_pwm_template_group = {
 	.templates = nct6683_attributes_pwm_template,
 	.is_visible = nct6683_pwm_is_visible,
 	.base = 1,
@@ -1170,6 +1204,7 @@ static int nct6683_probe(struct platform_device *pdev)
 	struct device *hwmon_dev;
 	struct resource *res;
 	int groups = 0;
+	char build[16];
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(dev, res->start, IOREGION_LENGTH, DRVNAME))
@@ -1187,6 +1222,17 @@ static int nct6683_probe(struct platform_device *pdev)
 
 	data->customer_id = nct6683_read16(data, NCT6683_REG_CUSTOMER_ID);
 
+	/* By default only instantiate driver if the customer ID is known */
+	switch (data->customer_id) {
+	case NCT6683_CUSTOMER_ID_INTEL:
+		break;
+	case NCT6683_CUSTOMER_ID_MITAC:
+		break;
+	default:
+		if (!force)
+			return -ENODEV;
+	}
+
 	nct6683_init_device(data);
 	nct6683_setup_fans(data);
 	nct6683_setup_sensors(data);
@@ -1230,13 +1276,22 @@ static int nct6683_probe(struct platform_device *pdev)
 	}
 	data->groups[groups++] = &nct6683_group_other;
 
-	dev_info(dev, "%s EC firmware version %d.%d build %02x/%02x/%02x\n",
+	if (data->customer_id == NCT6683_CUSTOMER_ID_INTEL)
+		scnprintf(build, sizeof(build), "%02x/%02x/%02x",
+			  nct6683_read(data, NCT6683_REG_BUILD_MONTH),
+			  nct6683_read(data, NCT6683_REG_BUILD_DAY),
+			  nct6683_read(data, NCT6683_REG_BUILD_YEAR));
+	else
+		scnprintf(build, sizeof(build), "%02d/%02d/%02d",
+			  nct6683_read(data, NCT6683_REG_BUILD_MONTH),
+			  nct6683_read(data, NCT6683_REG_BUILD_DAY),
+			  nct6683_read(data, NCT6683_REG_BUILD_YEAR));
+
+	dev_info(dev, "%s EC firmware version %d.%d build %s\n",
 		 nct6683_chip_names[data->kind],
 		 nct6683_read(data, NCT6683_REG_VERSION_HI),
 		 nct6683_read(data, NCT6683_REG_VERSION_LO),
-		 nct6683_read(data, NCT6683_REG_BUILD_MONTH),
-		 nct6683_read(data, NCT6683_REG_BUILD_DAY),
-		 nct6683_read(data, NCT6683_REG_BUILD_YEAR));
+		 build);
 
 	hwmon_dev = devm_hwmon_device_register_with_groups(dev,
 			nct6683_device_names[data->kind], data, data->groups);
@@ -1292,20 +1347,10 @@ static struct platform_driver nct6683_driver = {
 
 static int __init nct6683_find(int sioaddr, struct nct6683_sio_data *sio_data)
 {
-	const char *board_vendor;
 	int addr;
 	u16 val;
 	int err;
 
-	/*
-	 * Only run on Intel boards unless the 'force' module parameter is set
-	 */
-	if (!force) {
-		board_vendor = dmi_get_system_info(DMI_BOARD_VENDOR);
-		if (!board_vendor || strcmp(board_vendor, "Intel Corporation"))
-			return -ENODEV;
-	}
-
 	err = superio_enter(sioaddr);
 	if (err)
 		return err;
diff --git a/drivers/hwmon/nct6775.c b/drivers/hwmon/nct6775.c
index d7ebdf8651f5..d087a8e00cf5 100644
--- a/drivers/hwmon/nct6775.c
+++ b/drivers/hwmon/nct6775.c
@@ -1045,7 +1045,8 @@ struct sensor_template_group {
 };
 
 static struct attribute_group *
-nct6775_create_attr_group(struct device *dev, struct sensor_template_group *tg,
+nct6775_create_attr_group(struct device *dev,
+			  const struct sensor_template_group *tg,
 			  int repeat)
 {
 	struct attribute_group *group;
@@ -1827,7 +1828,7 @@ static struct sensor_device_template *nct6775_attributes_in_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6775_in_template_group = {
+static const struct sensor_template_group nct6775_in_template_group = {
 	.templates = nct6775_attributes_in_template,
 	.is_visible = nct6775_in_is_visible,
 };
@@ -2046,7 +2047,7 @@ static struct sensor_device_template *nct6775_attributes_fan_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6775_fan_template_group = {
+static const struct sensor_template_group nct6775_fan_template_group = {
 	.templates = nct6775_attributes_fan_template,
 	.is_visible = nct6775_fan_is_visible,
 	.base = 1,
@@ -2255,7 +2256,7 @@ static struct sensor_device_template *nct6775_attributes_temp_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6775_temp_template_group = {
+static const struct sensor_template_group nct6775_temp_template_group = {
 	.templates = nct6775_attributes_temp_template,
 	.is_visible = nct6775_temp_is_visible,
 	.base = 1,
@@ -3117,7 +3118,7 @@ static struct sensor_device_template *nct6775_attributes_pwm_template[] = {
 	NULL
 };
 
-static struct sensor_template_group nct6775_pwm_template_group = {
+static const struct sensor_template_group nct6775_pwm_template_group = {
 	.templates = nct6775_attributes_pwm_template,
 	.is_visible = nct6775_pwm_is_visible,
 	.base = 1,
diff --git a/drivers/hwmon/pmbus/Kconfig b/drivers/hwmon/pmbus/Kconfig
index df6ebb2b8f0f..7e5cc3d025ef 100644
--- a/drivers/hwmon/pmbus/Kconfig
+++ b/drivers/hwmon/pmbus/Kconfig
@@ -65,6 +65,16 @@ config SENSORS_LTC2978_REGULATOR
 	  If you say yes here you get regulator support for Linear
 	  Technology LTC2974, LTC2977, LTC2978, LTC3880, LTC3883, and LTM4676.
 
+config SENSORS_LTC3815
+	tristate "Linear Technologies LTC3815"
+	default n
+	help
+	  If you say yes here you get hardware monitoring support for Linear
+	  Technology LTC3815.
+
+	  This driver can also be built as a module. If so, the module will
+	  be called ltc3815.
+
 config SENSORS_MAX16064
 	tristate "Maxim MAX16064"
 	default n
diff --git a/drivers/hwmon/pmbus/Makefile b/drivers/hwmon/pmbus/Makefile
index bce046d37f02..562132054aaf 100644
--- a/drivers/hwmon/pmbus/Makefile
+++ b/drivers/hwmon/pmbus/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_SENSORS_PMBUS)	+= pmbus.o
 obj-$(CONFIG_SENSORS_ADM1275)	+= adm1275.o
 obj-$(CONFIG_SENSORS_LM25066)	+= lm25066.o
 obj-$(CONFIG_SENSORS_LTC2978)	+= ltc2978.o
+obj-$(CONFIG_SENSORS_LTC3815)	+= ltc3815.o
 obj-$(CONFIG_SENSORS_MAX16064)	+= max16064.o
 obj-$(CONFIG_SENSORS_MAX20751)	+= max20751.o
 obj-$(CONFIG_SENSORS_MAX34440)	+= max34440.o
diff --git a/drivers/hwmon/pmbus/ltc3815.c b/drivers/hwmon/pmbus/ltc3815.c
new file mode 100644
index 000000000000..bb32e6276622
--- /dev/null
+++ b/drivers/hwmon/pmbus/ltc3815.c
@@ -0,0 +1,215 @@
+/*
+ * Hardware monitoring driver for LTC3815
+ *
+ * Copyright (c) 2015 Linear Technology
+ * Copyright (c) 2015 Guenter Roeck
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include "pmbus.h"
+
+#define LTC3815_MFR_IOUT_PEAK	0xd7
+#define LTC3815_MFR_VOUT_PEAK	0xdd
+#define LTC3815_MFR_VIN_PEAK	0xde
+#define LTC3815_MFR_TEMP_PEAK	0xdf
+#define LTC3815_MFR_IIN_PEAK	0xe1
+#define LTC3815_MFR_SPECIAL_ID	0xe7
+
+#define LTC3815_ID		0x8000
+#define LTC3815_ID_MASK		0xff00
+
+static int ltc3815_read_byte_data(struct i2c_client *client, int page, int reg)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VOUT_MODE:
+		/*
+		 * The chip returns 0x3e, suggesting VID mode with manufacturer
+		 * specific VID codes. Since the output voltage is reported
+		 * with a LSB of 0.5mV, override and report direct mode with
+		 * appropriate coefficients.
+		 */
+		ret = 0x40;
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static int ltc3815_write_byte(struct i2c_client *client, int page, u8 reg)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_CLEAR_FAULTS:
+		/*
+		 * LTC3815 does not support the CLEAR_FAULTS command.
+		 * Emulate it by clearing the status register.
+		 */
+		ret = pmbus_read_word_data(client, 0, PMBUS_STATUS_WORD);
+		if (ret > 0) {
+			pmbus_write_word_data(client, 0, PMBUS_STATUS_WORD,
+					      ret);
+			ret = 0;
+		}
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static int ltc3815_read_word_data(struct i2c_client *client, int page, int reg)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VIRT_READ_VIN_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3815_MFR_VIN_PEAK);
+		break;
+	case PMBUS_VIRT_READ_VOUT_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3815_MFR_VOUT_PEAK);
+		break;
+	case PMBUS_VIRT_READ_TEMP_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3815_MFR_TEMP_PEAK);
+		break;
+	case PMBUS_VIRT_READ_IOUT_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3815_MFR_IOUT_PEAK);
+		break;
+	case PMBUS_VIRT_READ_IIN_MAX:
+		ret = pmbus_read_word_data(client, page, LTC3815_MFR_IIN_PEAK);
+		break;
+	case PMBUS_VIRT_RESET_VOUT_HISTORY:
+	case PMBUS_VIRT_RESET_VIN_HISTORY:
+	case PMBUS_VIRT_RESET_TEMP_HISTORY:
+	case PMBUS_VIRT_RESET_IOUT_HISTORY:
+	case PMBUS_VIRT_RESET_IIN_HISTORY:
+		ret = 0;
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static int ltc3815_write_word_data(struct i2c_client *client, int page,
+				   int reg, u16 word)
+{
+	int ret;
+
+	switch (reg) {
+	case PMBUS_VIRT_RESET_IIN_HISTORY:
+		ret = pmbus_write_word_data(client, page,
+					    LTC3815_MFR_IIN_PEAK, 0);
+		break;
+	case PMBUS_VIRT_RESET_IOUT_HISTORY:
+		ret = pmbus_write_word_data(client, page,
+					    LTC3815_MFR_IOUT_PEAK, 0);
+		break;
+	case PMBUS_VIRT_RESET_VOUT_HISTORY:
+		ret = pmbus_write_word_data(client, page,
+					    LTC3815_MFR_VOUT_PEAK, 0);
+		break;
+	case PMBUS_VIRT_RESET_VIN_HISTORY:
+		ret = pmbus_write_word_data(client, page,
+					    LTC3815_MFR_VIN_PEAK, 0);
+		break;
+	case PMBUS_VIRT_RESET_TEMP_HISTORY:
+		ret = pmbus_write_word_data(client, page,
+					    LTC3815_MFR_TEMP_PEAK, 0);
+		break;
+	default:
+		ret = -ENODATA;
+		break;
+	}
+	return ret;
+}
+
+static const struct i2c_device_id ltc3815_id[] = {
+	{"ltc3815", 0},
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ltc3815_id);
+
+static struct pmbus_driver_info ltc3815_info = {
+	.pages = 1,
+	.format[PSC_VOLTAGE_IN] = direct,
+	.format[PSC_VOLTAGE_OUT] = direct,
+	.format[PSC_CURRENT_IN] = direct,
+	.format[PSC_CURRENT_OUT] = direct,
+	.format[PSC_TEMPERATURE] = direct,
+	.m[PSC_VOLTAGE_IN] = 250,
+	.b[PSC_VOLTAGE_IN] = 0,
+	.R[PSC_VOLTAGE_IN] = 0,
+	.m[PSC_VOLTAGE_OUT] = 2,
+	.b[PSC_VOLTAGE_OUT] = 0,
+	.R[PSC_VOLTAGE_OUT] = 3,
+	.m[PSC_CURRENT_IN] = 1,
+	.b[PSC_CURRENT_IN] = 0,
+	.R[PSC_CURRENT_IN] = 2,
+	.m[PSC_CURRENT_OUT] = 1,
+	.b[PSC_CURRENT_OUT] = 0,
+	.R[PSC_CURRENT_OUT] = 2,
+	.m[PSC_TEMPERATURE] = 1,
+	.b[PSC_TEMPERATURE] = 0,
+	.R[PSC_TEMPERATURE] = 0,
+	.func[0] = PMBUS_HAVE_VIN | PMBUS_HAVE_IIN | PMBUS_HAVE_VOUT |
+		PMBUS_HAVE_IOUT | PMBUS_HAVE_TEMP,
+	.read_byte_data = ltc3815_read_byte_data,
+	.read_word_data = ltc3815_read_word_data,
+	.write_byte = ltc3815_write_byte,
+	.write_word_data = ltc3815_write_word_data,
+};
+
+static int ltc3815_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	int chip_id;
+
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_READ_WORD_DATA))
+		return -ENODEV;
+
+	chip_id = i2c_smbus_read_word_data(client, LTC3815_MFR_SPECIAL_ID);
+	if (chip_id < 0)
+		return chip_id;
+	if ((chip_id & LTC3815_ID_MASK) != LTC3815_ID)
+		return -ENODEV;
+
+	return pmbus_do_probe(client, id, &ltc3815_info);
+}
+
+static struct i2c_driver ltc3815_driver = {
+	.driver = {
+		   .name = "ltc3815",
+		   },
+	.probe = ltc3815_probe,
+	.remove = pmbus_do_remove,
+	.id_table = ltc3815_id,
+};
+
+module_i2c_driver(ltc3815_driver);
+
+MODULE_AUTHOR("Guenter Roeck");
+MODULE_DESCRIPTION("PMBus driver for LTC3815");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hwmon/sch56xx-common.c b/drivers/hwmon/sch56xx-common.c
index 738681983284..68c350c704fb 100644
--- a/drivers/hwmon/sch56xx-common.c
+++ b/drivers/hwmon/sch56xx-common.c
@@ -30,7 +30,6 @@
 #include <linux/watchdog.h>
 #include <linux/miscdevice.h>
 #include <linux/uaccess.h>
-#include <linux/kref.h>
 #include <linux/slab.h>
 #include "sch56xx-common.h"
 
@@ -67,7 +66,6 @@ MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
 struct sch56xx_watchdog_data {
 	u16 addr;
 	struct mutex *io_lock;
-	struct kref kref;
 	struct watchdog_info wdinfo;
 	struct watchdog_device wddev;
 	u8 watchdog_preset;
@@ -258,15 +256,6 @@ EXPORT_SYMBOL(sch56xx_read_virtual_reg12);
  * Watchdog routines
  */
 
-/* Release our data struct when we're unregistered *and*
-   all references to our watchdog device are released */
-static void watchdog_release_resources(struct kref *r)
-{
-	struct sch56xx_watchdog_data *data =
-		container_of(r, struct sch56xx_watchdog_data, kref);
-	kfree(data);
-}
-
 static int watchdog_set_timeout(struct watchdog_device *wddev,
 				unsigned int timeout)
 {
@@ -395,28 +384,12 @@ static int watchdog_stop(struct watchdog_device *wddev)
 	return 0;
 }
 
-static void watchdog_ref(struct watchdog_device *wddev)
-{
-	struct sch56xx_watchdog_data *data = watchdog_get_drvdata(wddev);
-
-	kref_get(&data->kref);
-}
-
-static void watchdog_unref(struct watchdog_device *wddev)
-{
-	struct sch56xx_watchdog_data *data = watchdog_get_drvdata(wddev);
-
-	kref_put(&data->kref, watchdog_release_resources);
-}
-
 static const struct watchdog_ops watchdog_ops = {
 	.owner		= THIS_MODULE,
 	.start		= watchdog_start,
 	.stop		= watchdog_stop,
 	.ping		= watchdog_trigger,
 	.set_timeout	= watchdog_set_timeout,
-	.ref		= watchdog_ref,
-	.unref		= watchdog_unref,
 };
 
 struct sch56xx_watchdog_data *sch56xx_watchdog_register(struct device *parent,
@@ -448,7 +421,6 @@ struct sch56xx_watchdog_data *sch56xx_watchdog_register(struct device *parent,
 
 	data->addr = addr;
 	data->io_lock = io_lock;
-	kref_init(&data->kref);
 
 	strlcpy(data->wdinfo.identity, "sch56xx watchdog",
 		sizeof(data->wdinfo.identity));
@@ -494,8 +466,7 @@ EXPORT_SYMBOL(sch56xx_watchdog_register);
 void sch56xx_watchdog_unregister(struct sch56xx_watchdog_data *data)
 {
 	watchdog_unregister_device(&data->wddev);
-	kref_put(&data->kref, watchdog_release_resources);
-	/* Don't touch data after this it may have been free-ed! */
+	kfree(data);
 }
 EXPORT_SYMBOL(sch56xx_watchdog_unregister);
 
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index 52f708bcf77f..d50c701b19d6 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -313,6 +313,10 @@ int of_hwspin_lock_get_id(struct device_node *np, int index)
 		hwlock = radix_tree_deref_slot(slot);
 		if (unlikely(!hwlock))
 			continue;
+		if (radix_tree_is_indirect_ptr(hwlock)) {
+			slot = radix_tree_iter_retry(&iter);
+			continue;
+		}
 
 		if (hwlock->bank->dev->of_node == args.np) {
 			ret = 0;
diff --git a/drivers/hwtracing/coresight/Kconfig b/drivers/hwtracing/coresight/Kconfig
index 6c8921140f02..c85935f3525a 100644
--- a/drivers/hwtracing/coresight/Kconfig
+++ b/drivers/hwtracing/coresight/Kconfig
@@ -8,7 +8,7 @@ menuconfig CORESIGHT
 	  This framework provides a kernel interface for the CoreSight debug
 	  and trace drivers to register themselves with. It's intended to build
 	  a topological view of the CoreSight components based on a DT
-	  specification and configure the right serie of components when a
+	  specification and configure the right series of components when a
 	  trace source gets enabled.
 
 if CORESIGHT
diff --git a/drivers/hwtracing/coresight/coresight.c b/drivers/hwtracing/coresight/coresight.c
index e25492137d8b..93738dfbf631 100644
--- a/drivers/hwtracing/coresight/coresight.c
+++ b/drivers/hwtracing/coresight/coresight.c
@@ -548,7 +548,7 @@ static int coresight_name_match(struct device *dev, void *data)
 	to_match = data;
 	i_csdev = to_coresight_device(dev);
 
-	if (!strcmp(to_match, dev_name(&i_csdev->dev)))
+	if (to_match && !strcmp(to_match, dev_name(&i_csdev->dev)))
 		return 1;
 
 	return 0;
diff --git a/drivers/i2c/algos/i2c-algo-bit.c b/drivers/i2c/algos/i2c-algo-bit.c
index 899bede81b31..9d233bbde5e1 100644
--- a/drivers/i2c/algos/i2c-algo-bit.c
+++ b/drivers/i2c/algos/i2c-algo-bit.c
@@ -617,6 +617,10 @@ const struct i2c_algorithm i2c_bit_algo = {
 };
 EXPORT_SYMBOL(i2c_bit_algo);
 
+const struct i2c_adapter_quirks i2c_bit_quirk_no_clk_stretch = {
+	.flags = I2C_AQ_NO_CLK_STRETCH,
+};
+
 /*
  * registering functions to load algorithms at runtime
  */
@@ -635,6 +639,8 @@ static int __i2c_bit_add_bus(struct i2c_adapter *adap,
 	/* register new adapter to i2c module... */
 	adap->algo = &i2c_bit_algo;
 	adap->retries = 3;
+	if (bit_adap->getscl == NULL)
+		adap->quirks = &i2c_bit_quirk_no_clk_stretch;
 
 	ret = add_adapter(adap);
 	if (ret < 0)
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 7b0aa82ea38b..0299dfa746a3 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -516,7 +516,7 @@ config I2C_EFM32
 
 config I2C_EG20T
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) I2C"
-	depends on PCI && (X86_32 || COMPILE_TEST)
+	depends on PCI && (X86_32 || MIPS || COMPILE_TEST)
 	help
 	  This driver is for PCH(Platform controller Hub) I2C of EG20T which
 	  is an IOH(Input/Output Hub) for x86 embedded processor.
@@ -532,6 +532,7 @@ config I2C_EG20T
 config I2C_EMEV2
 	tristate "EMMA Mobile series I2C adapter"
 	depends on HAVE_CLK
+	select I2C_SLAVE
 	help
 	  If you say yes to this option, support will be included for the
 	  I2C interface on the Renesas Electronics EM/EV family of processors.
@@ -963,11 +964,11 @@ config I2C_XILINX
 	  will be called xilinx_i2c.
 
 config I2C_XLR
-	tristate "XLR I2C support"
-	depends on CPU_XLR
+	tristate "Netlogic XLR and Sigma Designs I2C support"
+	depends on CPU_XLR || ARCH_TANGOX
 	help
 	  This driver enables support for the on-chip I2C interface of
-	  the Netlogic XLR/XLS MIPS processors.
+	  the Netlogic XLR/XLS MIPS processors and Sigma Designs SOCs.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-xlr.
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 10835d1f559b..921d32bfcda8 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -64,6 +64,8 @@
 #define	AT91_TWI_IADR		0x000c	/* Internal Address Register */
 
 #define	AT91_TWI_CWGR		0x0010	/* Clock Waveform Generator Reg */
+#define	AT91_TWI_CWGR_HOLD_MAX	0x1f
+#define	AT91_TWI_CWGR_HOLD(x)	(((x) & AT91_TWI_CWGR_HOLD_MAX) << 24)
 
 #define	AT91_TWI_SR		0x0020	/* Status Register */
 #define	AT91_TWI_TXCOMP		BIT(0)	/* Transmission Complete */
@@ -110,6 +112,7 @@ struct at91_twi_pdata {
 	unsigned clk_offset;
 	bool has_unre_flag;
 	bool has_alt_cmd;
+	bool has_hold_field;
 	struct at_dma_slave dma_slave;
 };
 
@@ -187,10 +190,11 @@ static void at91_init_twi_bus(struct at91_twi_dev *dev)
  */
 static void at91_calc_twi_clock(struct at91_twi_dev *dev, int twi_clk)
 {
-	int ckdiv, cdiv, div;
+	int ckdiv, cdiv, div, hold = 0;
 	struct at91_twi_pdata *pdata = dev->pdata;
 	int offset = pdata->clk_offset;
 	int max_ckdiv = pdata->clk_max_div;
+	u32 twd_hold_time_ns = 0;
 
 	div = max(0, (int)DIV_ROUND_UP(clk_get_rate(dev->clk),
 				       2 * twi_clk) - offset);
@@ -204,8 +208,33 @@ static void at91_calc_twi_clock(struct at91_twi_dev *dev, int twi_clk)
 		cdiv = 255;
 	}
 
-	dev->twi_cwgr_reg = (ckdiv << 16) | (cdiv << 8) | cdiv;
-	dev_dbg(dev->dev, "cdiv %d ckdiv %d\n", cdiv, ckdiv);
+	if (pdata->has_hold_field) {
+		of_property_read_u32(dev->dev->of_node, "i2c-sda-hold-time-ns",
+				     &twd_hold_time_ns);
+
+		/*
+		 * hold time = HOLD + 3 x T_peripheral_clock
+		 * Use clk rate in kHz to prevent overflows when computing
+		 * hold.
+		 */
+		hold = DIV_ROUND_UP(twd_hold_time_ns
+				    * (clk_get_rate(dev->clk) / 1000), 1000000);
+		hold -= 3;
+		if (hold < 0)
+			hold = 0;
+		if (hold > AT91_TWI_CWGR_HOLD_MAX) {
+			dev_warn(dev->dev,
+				 "HOLD field set to its maximum value (%d instead of %d)\n",
+				 AT91_TWI_CWGR_HOLD_MAX, hold);
+			hold = AT91_TWI_CWGR_HOLD_MAX;
+		}
+	}
+
+	dev->twi_cwgr_reg = (ckdiv << 16) | (cdiv << 8) | cdiv
+			    | AT91_TWI_CWGR_HOLD(hold);
+
+	dev_dbg(dev->dev, "cdiv %d ckdiv %d hold %d (%d ns)\n",
+		cdiv, ckdiv, hold, twd_hold_time_ns);
 }
 
 static void at91_twi_dma_cleanup(struct at91_twi_dev *dev)
@@ -797,6 +826,7 @@ static struct at91_twi_pdata at91rm9200_config = {
 	.clk_offset = 3,
 	.has_unre_flag = true,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
 };
 
 static struct at91_twi_pdata at91sam9261_config = {
@@ -804,6 +834,7 @@ static struct at91_twi_pdata at91sam9261_config = {
 	.clk_offset = 4,
 	.has_unre_flag = false,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
 };
 
 static struct at91_twi_pdata at91sam9260_config = {
@@ -811,6 +842,7 @@ static struct at91_twi_pdata at91sam9260_config = {
 	.clk_offset = 4,
 	.has_unre_flag = false,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
 };
 
 static struct at91_twi_pdata at91sam9g20_config = {
@@ -818,6 +850,7 @@ static struct at91_twi_pdata at91sam9g20_config = {
 	.clk_offset = 4,
 	.has_unre_flag = false,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
 };
 
 static struct at91_twi_pdata at91sam9g10_config = {
@@ -825,6 +858,7 @@ static struct at91_twi_pdata at91sam9g10_config = {
 	.clk_offset = 4,
 	.has_unre_flag = false,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
 };
 
 static const struct platform_device_id at91_twi_devtypes[] = {
@@ -854,6 +888,15 @@ static struct at91_twi_pdata at91sam9x5_config = {
 	.clk_offset = 4,
 	.has_unre_flag = false,
 	.has_alt_cmd = false,
+	.has_hold_field = false,
+};
+
+static struct at91_twi_pdata sama5d4_config = {
+	.clk_max_div = 7,
+	.clk_offset = 4,
+	.has_unre_flag = false,
+	.has_alt_cmd = false,
+	.has_hold_field = true,
 };
 
 static struct at91_twi_pdata sama5d2_config = {
@@ -861,6 +904,7 @@ static struct at91_twi_pdata sama5d2_config = {
 	.clk_offset = 4,
 	.has_unre_flag = true,
 	.has_alt_cmd = true,
+	.has_hold_field = true,
 };
 
 static const struct of_device_id atmel_twi_dt_ids[] = {
@@ -883,6 +927,9 @@ static const struct of_device_id atmel_twi_dt_ids[] = {
 		.compatible = "atmel,at91sam9x5-i2c",
 		.data = &at91sam9x5_config,
 	}, {
+		.compatible = "atmel,sama5d4-i2c",
+		.data = &sama5d4_config,
+	}, {
 		.compatible = "atmel,sama5d2-i2c",
 		.data = &sama5d2_config,
 	}, {
diff --git a/drivers/i2c/busses/i2c-bcm2835.c b/drivers/i2c/busses/i2c-bcm2835.c
index 3032b89ac60b..818b051d25e6 100644
--- a/drivers/i2c/busses/i2c-bcm2835.c
+++ b/drivers/i2c/busses/i2c-bcm2835.c
@@ -222,6 +222,15 @@ static const struct i2c_algorithm bcm2835_i2c_algo = {
 	.functionality	= bcm2835_i2c_func,
 };
 
+/*
+ * This HW was reported to have problems with clock stretching:
+ * http://www.advamation.com/knowhow/raspberrypi/rpi-i2c-bug.html
+ * https://www.raspberrypi.org/forums/viewtopic.php?p=146272
+ */
+static const struct i2c_adapter_quirks bcm2835_i2c_quirks = {
+	.flags = I2C_AQ_NO_CLK_STRETCH,
+};
+
 static int bcm2835_i2c_probe(struct platform_device *pdev)
 {
 	struct bcm2835_i2c_dev *i2c_dev;
@@ -293,6 +302,7 @@ static int bcm2835_i2c_probe(struct platform_device *pdev)
 	adap->algo = &bcm2835_i2c_algo;
 	adap->dev.parent = &pdev->dev;
 	adap->dev.of_node = pdev->dev.of_node;
+	adap->quirks = &bcm2835_i2c_quirks;
 
 	bcm2835_i2c_writel(i2c_dev, BCM2835_I2C_C, 0);
 
diff --git a/drivers/i2c/busses/i2c-brcmstb.c b/drivers/i2c/busses/i2c-brcmstb.c
index 8e9637eea512..4a45408dd820 100644
--- a/drivers/i2c/busses/i2c-brcmstb.c
+++ b/drivers/i2c/busses/i2c-brcmstb.c
@@ -25,13 +25,16 @@
 #include <linux/version.h>
 
 #define N_DATA_REGS					8
-#define N_DATA_BYTES					(N_DATA_REGS * 4)
 
-/* BSC count register field definitions */
-#define BSC_CNT_REG1_MASK				0x0000003f
-#define BSC_CNT_REG1_SHIFT				0
-#define BSC_CNT_REG2_MASK				0x00000fc0
-#define BSC_CNT_REG2_SHIFT				6
+/*
+ * PER_I2C/BSC count register mask depends on 1 byte/4 byte data register
+ * size. Cable modem and DSL SoCs with Peripheral i2c cores use 1 byte per
+ * data register whereas STB SoCs use 4 byte per data register transfer,
+ * account for this difference in total count per transaction and mask to
+ * use.
+ */
+#define BSC_CNT_REG1_MASK(nb)	(nb == 1 ? GENMASK(3, 0) : GENMASK(5, 0))
+#define BSC_CNT_REG1_SHIFT	0
 
 /* BSC CTL register field definitions */
 #define BSC_CTL_REG_DTF_MASK				0x00000003
@@ -41,7 +44,7 @@
 #define BSC_CTL_REG_INT_EN_SHIFT			6
 #define BSC_CTL_REG_DIV_CLK_MASK			0x00000080
 
-/* BSC_IIC_ENABLE r/w enable and interrupt field defintions */
+/* BSC_IIC_ENABLE r/w enable and interrupt field definitions */
 #define BSC_IIC_EN_RESTART_MASK				0x00000040
 #define BSC_IIC_EN_NOSTART_MASK				0x00000020
 #define BSC_IIC_EN_NOSTOP_MASK				0x00000010
@@ -169,6 +172,7 @@ struct brcmstb_i2c_dev {
 	struct completion done;
 	bool is_suspended;
 	u32 clk_freq_hz;
+	int data_regsz;
 };
 
 /* register accessors for both be and le cpu arch */
@@ -186,6 +190,16 @@ struct brcmstb_i2c_dev {
 #define bsc_writel(_dev, _val, _reg)					\
 	__bsc_writel(_val, _dev->base + offsetof(struct bsc_regs, _reg))
 
+static inline int brcmstb_i2c_get_xfersz(struct brcmstb_i2c_dev *dev)
+{
+	return (N_DATA_REGS * dev->data_regsz);
+}
+
+static inline int brcmstb_i2c_get_data_regsz(struct brcmstb_i2c_dev *dev)
+{
+	return dev->data_regsz;
+}
+
 static void brcmstb_i2c_enable_disable_irq(struct brcmstb_i2c_dev *dev,
 					   bool int_en)
 {
@@ -323,14 +337,16 @@ static int brcmstb_i2c_xfer_bsc_data(struct brcmstb_i2c_dev *dev,
 				     u8 *buf, unsigned int len,
 				     struct i2c_msg *pmsg)
 {
-	int cnt, byte, rc;
+	int cnt, byte, i, rc;
 	enum bsc_xfer_cmd cmd;
 	u32 ctl_reg;
 	struct bsc_regs *pi2creg = dev->bsc_regmap;
 	int no_ack = pmsg->flags & I2C_M_IGNORE_NAK;
+	int data_regsz = brcmstb_i2c_get_data_regsz(dev);
+	int xfersz = brcmstb_i2c_get_xfersz(dev);
 
 	/* see if the transaction needs to check NACK conditions */
-	if (no_ack || len <= N_DATA_BYTES) {
+	if (no_ack || len <= xfersz) {
 		cmd = (pmsg->flags & I2C_M_RD) ? CMD_RD_NOACK
 			: CMD_WR_NOACK;
 		pi2creg->ctlhi_reg |= BSC_CTLHI_REG_IGNORE_ACK_MASK;
@@ -348,20 +364,22 @@ static int brcmstb_i2c_xfer_bsc_data(struct brcmstb_i2c_dev *dev,
 		pi2creg->ctl_reg = ctl_reg | DTF_RD_MASK;
 
 	/* set the read/write length */
-	bsc_writel(dev, BSC_CNT_REG1_MASK & (len << BSC_CNT_REG1_SHIFT),
-		   cnt_reg);
+	bsc_writel(dev, BSC_CNT_REG1_MASK(data_regsz) &
+		   (len << BSC_CNT_REG1_SHIFT), cnt_reg);
 
 	/* Write data into data_in register */
+
 	if (cmd == CMD_WR || cmd == CMD_WR_NOACK) {
-		for (cnt = 0; cnt < len; cnt += 4) {
+		for (cnt = 0, i = 0; cnt < len; cnt += data_regsz, i++) {
 			u32 word = 0;
 
-			for (byte = 0; byte < 4; byte++) {
-				word >>= 8;
+			for (byte = 0; byte < data_regsz; byte++) {
+				word >>= BITS_PER_BYTE;
 				if ((cnt + byte) < len)
-					word |= buf[cnt + byte] << 24;
+					word |= buf[cnt + byte] <<
+					(BITS_PER_BYTE * (data_regsz - 1));
 			}
-			bsc_writel(dev, word, data_in[cnt >> 2]);
+			bsc_writel(dev, word, data_in[i]);
 		}
 	}
 
@@ -373,14 +391,15 @@ static int brcmstb_i2c_xfer_bsc_data(struct brcmstb_i2c_dev *dev,
 		return rc;
 	}
 
+	/* Read data from data_out register */
 	if (cmd == CMD_RD || cmd == CMD_RD_NOACK) {
-		for (cnt = 0; cnt < len; cnt += 4) {
-			u32 data = bsc_readl(dev, data_out[cnt >> 2]);
+		for (cnt = 0, i = 0; cnt < len; cnt += data_regsz, i++) {
+			u32 data = bsc_readl(dev, data_out[i]);
 
-			for (byte = 0; byte < 4 &&
+			for (byte = 0; byte < data_regsz &&
 				     (byte + cnt) < len; byte++) {
 				buf[cnt + byte] = data & 0xff;
-				data >>= 8;
+				data >>= BITS_PER_BYTE;
 			}
 		}
 	}
@@ -448,6 +467,7 @@ static int brcmstb_i2c_xfer(struct i2c_adapter *adapter,
 	int bytes_to_xfer;
 	u8 *tmp_buf;
 	int len = 0;
+	int xfersz = brcmstb_i2c_get_xfersz(dev);
 
 	if (dev->is_suspended)
 		return -EBUSY;
@@ -482,9 +502,9 @@ static int brcmstb_i2c_xfer(struct i2c_adapter *adapter,
 
 		/* Perform data transfer */
 		while (len) {
-			bytes_to_xfer = min(len, N_DATA_BYTES);
+			bytes_to_xfer = min(len, xfersz);
 
-			if (len <= N_DATA_BYTES && i == (num - 1))
+			if (len <= xfersz && i == (num - 1))
 				brcmstb_set_i2c_start_stop(dev,
 							   ~(COND_START_STOP));
 
@@ -542,8 +562,12 @@ static void brcmstb_i2c_set_bus_speed(struct brcmstb_i2c_dev *dev)
 
 static void brcmstb_i2c_set_bsc_reg_defaults(struct brcmstb_i2c_dev *dev)
 {
-	/* 4 byte data register */
-	dev->bsc_regmap->ctlhi_reg = BSC_CTLHI_REG_DATAREG_SIZE_MASK;
+	if (brcmstb_i2c_get_data_regsz(dev) == sizeof(u32))
+		/* set 4 byte data in/out xfers  */
+		dev->bsc_regmap->ctlhi_reg = BSC_CTLHI_REG_DATAREG_SIZE_MASK;
+	else
+		dev->bsc_regmap->ctlhi_reg &= ~BSC_CTLHI_REG_DATAREG_SIZE_MASK;
+
 	bsc_writel(dev, dev->bsc_regmap->ctlhi_reg, ctlhi_reg);
 	/* set bus speed */
 	brcmstb_i2c_set_bus_speed(dev);
@@ -562,8 +586,7 @@ static int brcmstb_i2c_probe(struct platform_device *pdev)
 	if (!dev)
 		return -ENOMEM;
 
-	dev->bsc_regmap = devm_kzalloc(&pdev->dev, sizeof(struct bsc_regs *),
-				       GFP_KERNEL);
+	dev->bsc_regmap = devm_kzalloc(&pdev->dev, sizeof(*dev->bsc_regmap), GFP_KERNEL);
 	if (!dev->bsc_regmap)
 		return -ENOMEM;
 
@@ -608,6 +631,13 @@ static int brcmstb_i2c_probe(struct platform_device *pdev)
 		dev->clk_freq_hz = bsc_clk[0].hz;
 	}
 
+	/* set the data in/out register size for compatible SoCs */
+	if (of_device_is_compatible(dev->device->of_node,
+				    "brcmstb,brcmper-i2c"))
+		dev->data_regsz = sizeof(u8);
+	else
+		dev->data_regsz = sizeof(u32);
+
 	brcmstb_i2c_set_bsc_reg_defaults(dev);
 
 	/* Add the i2c adapter */
@@ -674,6 +704,7 @@ static SIMPLE_DEV_PM_OPS(brcmstb_i2c_pm, brcmstb_i2c_suspend,
 
 static const struct of_device_id brcmstb_i2c_of_match[] = {
 	{.compatible = "brcm,brcmstb-i2c"},
+	{.compatible = "brcm,brcmper-i2c"},
 	{},
 };
 MODULE_DEVICE_TABLE(of, brcmstb_i2c_of_match);
diff --git a/drivers/i2c/busses/i2c-cadence.c b/drivers/i2c/busses/i2c-cadence.c
index 84deed6571bd..6b08d1607b7a 100644
--- a/drivers/i2c/busses/i2c-cadence.c
+++ b/drivers/i2c/busses/i2c-cadence.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/of.h>
+#include <linux/pm_runtime.h>
 
 /* Register offsets for the I2C device. */
 #define CDNS_I2C_CR_OFFSET		0x00 /* Control Register, RW */
@@ -96,6 +97,8 @@
 					 CDNS_I2C_IXR_COMP)
 
 #define CDNS_I2C_TIMEOUT		msecs_to_jiffies(1000)
+/* timeout for pm runtime autosuspend */
+#define CNDS_I2C_PM_TIMEOUT		1000	/* ms */
 
 #define CDNS_I2C_FIFO_DEPTH		16
 /* FIFO depth at which the DATA interrupt occurs */
@@ -128,7 +131,6 @@
  * @xfer_done:		Transfer complete status
  * @p_send_buf:		Pointer to transmit buffer
  * @p_recv_buf:		Pointer to receive buffer
- * @suspended:		Flag holding the device's PM status
  * @send_count:		Number of bytes still expected to send
  * @recv_count:		Number of bytes still expected to receive
  * @curr_recv_count:	Number of bytes to be received in current transfer
@@ -141,6 +143,7 @@
  * @quirks:		flag for broken hold bit usage in r1p10
  */
 struct cdns_i2c {
+	struct device		*dev;
 	void __iomem *membase;
 	struct i2c_adapter adap;
 	struct i2c_msg *p_msg;
@@ -148,7 +151,6 @@ struct cdns_i2c {
 	struct completion xfer_done;
 	unsigned char *p_send_buf;
 	unsigned char *p_recv_buf;
-	u8 suspended;
 	unsigned int send_count;
 	unsigned int recv_count;
 	unsigned int curr_recv_count;
@@ -569,9 +571,14 @@ static int cdns_i2c_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 	struct cdns_i2c *id = adap->algo_data;
 	bool hold_quirk;
 
+	ret = pm_runtime_get_sync(id->dev);
+	if (ret < 0)
+		return ret;
 	/* Check if the bus is free */
-	if (cdns_i2c_readreg(CDNS_I2C_SR_OFFSET) & CDNS_I2C_SR_BA)
-		return -EAGAIN;
+	if (cdns_i2c_readreg(CDNS_I2C_SR_OFFSET) & CDNS_I2C_SR_BA) {
+		ret = -EAGAIN;
+		goto out;
+	}
 
 	hold_quirk = !!(id->quirks & CDNS_I2C_BROKEN_HOLD_BIT);
 	/*
@@ -590,7 +597,8 @@ static int cdns_i2c_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 			if (msgs[count].flags & I2C_M_RD) {
 				dev_warn(adap->dev.parent,
 					 "Can't do repeated start after a receive message\n");
-				return -EOPNOTSUPP;
+				ret = -EOPNOTSUPP;
+				goto out;
 			}
 		}
 		id->bus_hold_flag = 1;
@@ -608,20 +616,26 @@ static int cdns_i2c_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 
 		ret = cdns_i2c_process_msg(id, msgs, adap);
 		if (ret)
-			return ret;
+			goto out;
 
 		/* Report the other error interrupts to application */
 		if (id->err_status) {
 			cdns_i2c_master_reset(adap);
 
-			if (id->err_status & CDNS_I2C_IXR_NACK)
-				return -ENXIO;
-
-			return -EIO;
+			if (id->err_status & CDNS_I2C_IXR_NACK) {
+				ret = -ENXIO;
+				goto out;
+			}
+			ret = -EIO;
+			goto out;
 		}
 	}
 
-	return num;
+	ret = num;
+out:
+	pm_runtime_mark_last_busy(id->dev);
+	pm_runtime_put_autosuspend(id->dev);
+	return ret;
 }
 
 /**
@@ -760,7 +774,7 @@ static int cdns_i2c_clk_notifier_cb(struct notifier_block *nb, unsigned long
 	struct clk_notifier_data *ndata = data;
 	struct cdns_i2c *id = to_cdns_i2c(nb);
 
-	if (id->suspended)
+	if (pm_runtime_suspended(id->dev))
 		return NOTIFY_OK;
 
 	switch (event) {
@@ -808,14 +822,12 @@ static int cdns_i2c_clk_notifier_cb(struct notifier_block *nb, unsigned long
  *
  * Return: 0 always
  */
-static int __maybe_unused cdns_i2c_suspend(struct device *_dev)
+static int __maybe_unused cdns_i2c_runtime_suspend(struct device *dev)
 {
-	struct platform_device *pdev = container_of(_dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cdns_i2c *xi2c = platform_get_drvdata(pdev);
 
 	clk_disable(xi2c->clk);
-	xi2c->suspended = 1;
 
 	return 0;
 }
@@ -828,26 +840,25 @@ static int __maybe_unused cdns_i2c_suspend(struct device *_dev)
  *
  * Return: 0 on success and error value on error
  */
-static int __maybe_unused cdns_i2c_resume(struct device *_dev)
+static int __maybe_unused cdns_i2c_runtime_resume(struct device *dev)
 {
-	struct platform_device *pdev = container_of(_dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cdns_i2c *xi2c = platform_get_drvdata(pdev);
 	int ret;
 
 	ret = clk_enable(xi2c->clk);
 	if (ret) {
-		dev_err(_dev, "Cannot enable clock.\n");
+		dev_err(dev, "Cannot enable clock.\n");
 		return ret;
 	}
 
-	xi2c->suspended = 0;
-
 	return 0;
 }
 
-static SIMPLE_DEV_PM_OPS(cdns_i2c_dev_pm_ops, cdns_i2c_suspend,
-			 cdns_i2c_resume);
+static const struct dev_pm_ops cdns_i2c_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(cdns_i2c_runtime_suspend,
+			   cdns_i2c_runtime_resume, NULL)
+};
 
 static const struct cdns_platform_data r1p10_i2c_def = {
 	.quirks = CDNS_I2C_BROKEN_HOLD_BIT,
@@ -881,6 +892,7 @@ static int cdns_i2c_probe(struct platform_device *pdev)
 	if (!id)
 		return -ENOMEM;
 
+	id->dev = &pdev->dev;
 	platform_set_drvdata(pdev, id);
 
 	match = of_match_node(cdns_i2c_of_match, pdev->dev.of_node);
@@ -913,10 +925,14 @@ static int cdns_i2c_probe(struct platform_device *pdev)
 		return PTR_ERR(id->clk);
 	}
 	ret = clk_prepare_enable(id->clk);
-	if (ret) {
+	if (ret)
 		dev_err(&pdev->dev, "Unable to enable clock.\n");
-		return ret;
-	}
+
+	pm_runtime_enable(id->dev);
+	pm_runtime_set_autosuspend_delay(id->dev, CNDS_I2C_PM_TIMEOUT);
+	pm_runtime_use_autosuspend(id->dev);
+	pm_runtime_set_active(id->dev);
+
 	id->clk_rate_change_nb.notifier_call = cdns_i2c_clk_notifier_cb;
 	if (clk_notifier_register(id->clk, &id->clk_rate_change_nb))
 		dev_warn(&pdev->dev, "Unable to register clock notifier.\n");
@@ -966,6 +982,8 @@ static int cdns_i2c_probe(struct platform_device *pdev)
 
 err_clk_dis:
 	clk_disable_unprepare(id->clk);
+	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	return ret;
 }
 
@@ -984,6 +1002,7 @@ static int cdns_i2c_remove(struct platform_device *pdev)
 	i2c_del_adapter(&id->adap);
 	clk_notifier_unregister(id->clk, &id->clk_rate_change_nb);
 	clk_disable_unprepare(id->clk);
+	pm_runtime_disable(&pdev->dev);
 
 	return 0;
 }
diff --git a/drivers/i2c/busses/i2c-designware-baytrail.c b/drivers/i2c/busses/i2c-designware-baytrail.c
index 7d7ae97476e2..e38c2bbba940 100644
--- a/drivers/i2c/busses/i2c-designware-baytrail.c
+++ b/drivers/i2c/busses/i2c-designware-baytrail.c
@@ -34,8 +34,7 @@ static int get_sem(struct device *dev, u32 *sem)
 	u32 data;
 	int ret;
 
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ, PUNIT_SEMAPHORE,
-				&data);
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, PUNIT_SEMAPHORE, &data);
 	if (ret) {
 		dev_err(dev, "iosf failed to read punit semaphore\n");
 		return ret;
@@ -50,21 +49,19 @@ static void reset_semaphore(struct device *dev)
 {
 	u32 data;
 
-	if (iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-				PUNIT_SEMAPHORE, &data)) {
+	if (iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, PUNIT_SEMAPHORE, &data)) {
 		dev_err(dev, "iosf failed to reset punit semaphore during read\n");
 		return;
 	}
 
 	data &= ~PUNIT_SEMAPHORE_BIT;
-	if (iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				PUNIT_SEMAPHORE, data))
+	if (iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, PUNIT_SEMAPHORE, data))
 		dev_err(dev, "iosf failed to reset punit semaphore during write\n");
 }
 
 static int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 {
-	u32 sem;
+	u32 sem = PUNIT_SEMAPHORE_ACQUIRE;
 	int ret;
 	unsigned long start, end;
 
@@ -77,8 +74,7 @@ static int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 		return 0;
 
 	/* host driver writes to side band semaphore register */
-	ret = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
-				PUNIT_SEMAPHORE, PUNIT_SEMAPHORE_ACQUIRE);
+	ret = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, PUNIT_SEMAPHORE, sem);
 	if (ret) {
 		dev_err(dev->dev, "iosf punit semaphore request failed\n");
 		return ret;
@@ -102,8 +98,7 @@ static int baytrail_i2c_acquire(struct dw_i2c_dev *dev)
 	dev_err(dev->dev, "punit semaphore timed out, resetting\n");
 	reset_semaphore(dev->dev);
 
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-				PUNIT_SEMAPHORE, &sem);
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, PUNIT_SEMAPHORE, &sem);
 	if (ret)
 		dev_err(dev->dev, "iosf failed to read punit semaphore\n");
 	else
diff --git a/drivers/i2c/busses/i2c-designware-core.c b/drivers/i2c/busses/i2c-designware-core.c
index de7fbbb374cd..10fbd6d841e0 100644
--- a/drivers/i2c/busses/i2c-designware-core.c
+++ b/drivers/i2c/busses/i2c-designware-core.c
@@ -271,6 +271,17 @@ static void __i2c_dw_enable(struct dw_i2c_dev *dev, bool enable)
 		 enable ? "en" : "dis");
 }
 
+static unsigned long i2c_dw_clk_rate(struct dw_i2c_dev *dev)
+{
+	/*
+	 * Clock is not necessary if we got LCNT/HCNT values directly from
+	 * the platform code.
+	 */
+	if (WARN_ON_ONCE(!dev->get_clk_rate_khz))
+		return 0;
+	return dev->get_clk_rate_khz(dev);
+}
+
 /**
  * i2c_dw_init() - initialize the designware i2c master hardware
  * @dev: device private data
@@ -281,7 +292,6 @@ static void __i2c_dw_enable(struct dw_i2c_dev *dev, bool enable)
  */
 int i2c_dw_init(struct dw_i2c_dev *dev)
 {
-	u32 input_clock_khz;
 	u32 hcnt, lcnt;
 	u32 reg;
 	u32 sda_falling_time, scl_falling_time;
@@ -295,8 +305,6 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 		}
 	}
 
-	input_clock_khz = dev->get_clk_rate_khz(dev);
-
 	reg = dw_readl(dev, DW_IC_COMP_TYPE);
 	if (reg == ___constant_swab32(DW_IC_COMP_TYPE_VALUE)) {
 		/* Configure register endianess access */
@@ -325,12 +333,12 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 		hcnt = dev->ss_hcnt;
 		lcnt = dev->ss_lcnt;
 	} else {
-		hcnt = i2c_dw_scl_hcnt(input_clock_khz,
+		hcnt = i2c_dw_scl_hcnt(i2c_dw_clk_rate(dev),
 					4000,	/* tHD;STA = tHIGH = 4.0 us */
 					sda_falling_time,
 					0,	/* 0: DW default, 1: Ideal */
 					0);	/* No offset */
-		lcnt = i2c_dw_scl_lcnt(input_clock_khz,
+		lcnt = i2c_dw_scl_lcnt(i2c_dw_clk_rate(dev),
 					4700,	/* tLOW = 4.7 us */
 					scl_falling_time,
 					0);	/* No offset */
@@ -344,12 +352,12 @@ int i2c_dw_init(struct dw_i2c_dev *dev)
 		hcnt = dev->fs_hcnt;
 		lcnt = dev->fs_lcnt;
 	} else {
-		hcnt = i2c_dw_scl_hcnt(input_clock_khz,
+		hcnt = i2c_dw_scl_hcnt(i2c_dw_clk_rate(dev),
 					600,	/* tHD;STA = tHIGH = 0.6 us */
 					sda_falling_time,
 					0,	/* 0: DW default, 1: Ideal */
 					0);	/* No offset */
-		lcnt = i2c_dw_scl_lcnt(input_clock_khz,
+		lcnt = i2c_dw_scl_lcnt(i2c_dw_clk_rate(dev),
 					1300,	/* tLOW = 1.3 us */
 					scl_falling_time,
 					0);	/* No offset */
@@ -860,12 +868,14 @@ int i2c_dw_probe(struct dw_i2c_dev *dev)
 
 	snprintf(adap->name, sizeof(adap->name),
 		 "Synopsys DesignWare I2C adapter");
+	adap->retries = 3;
 	adap->algo = &i2c_dw_algo;
 	adap->dev.parent = dev->dev;
 	i2c_set_adapdata(adap, dev);
 
 	i2c_dw_disable_int(dev);
-	r = devm_request_irq(dev->dev, dev->irq, i2c_dw_isr, IRQF_SHARED,
+	r = devm_request_irq(dev->dev, dev->irq, i2c_dw_isr,
+			     IRQF_SHARED | IRQF_COND_SUSPEND,
 			     dev_name(dev->dev), dev);
 	if (r) {
 		dev_err(dev->dev, "failure requesting irq %i: %d\n",
diff --git a/drivers/i2c/busses/i2c-designware-pcidrv.c b/drivers/i2c/busses/i2c-designware-pcidrv.c
index 1543d35d228d..7368be000c96 100644
--- a/drivers/i2c/busses/i2c-designware-pcidrv.c
+++ b/drivers/i2c/busses/i2c-designware-pcidrv.c
@@ -162,7 +162,7 @@ static struct dw_pci_controller dw_pci_controllers[] = {
 #ifdef CONFIG_PM
 static int i2c_dw_pci_suspend(struct device *dev)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 
 	i2c_dw_disable(pci_get_drvdata(pdev));
 	return 0;
@@ -170,7 +170,7 @@ static int i2c_dw_pci_suspend(struct device *dev)
 
 static int i2c_dw_pci_resume(struct device *dev)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 
 	return i2c_dw_init(pci_get_drvdata(pdev));
 }
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index 6b00061c3746..438f1b4964c0 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -36,6 +36,7 @@
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
+#include <linux/property.h>
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/acpi.h>
@@ -122,6 +123,8 @@ static const struct acpi_device_id dw_i2c_acpi_match[] = {
 	{ "80860F41", 0 },
 	{ "808622C1", 0 },
 	{ "AMD0010", ACCESS_INTR_MASK },
+	{ "AMDI0510", 0 },
+	{ "APMC0D0F", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, dw_i2c_acpi_match);
@@ -132,12 +135,24 @@ static inline int dw_i2c_acpi_configure(struct platform_device *pdev)
 }
 #endif
 
+static int i2c_dw_plat_prepare_clk(struct dw_i2c_dev *i_dev, bool prepare)
+{
+	if (IS_ERR(i_dev->clk))
+		return PTR_ERR(i_dev->clk);
+
+	if (prepare)
+		return clk_prepare_enable(i_dev->clk);
+
+	clk_disable_unprepare(i_dev->clk);
+	return 0;
+}
+
 static int dw_i2c_plat_probe(struct platform_device *pdev)
 {
+	struct dw_i2c_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct dw_i2c_dev *dev;
 	struct i2c_adapter *adap;
 	struct resource *mem;
-	struct dw_i2c_platform_data *pdata;
 	int irq, r;
 	u32 clk_freq, ht = 0;
 
@@ -161,33 +176,28 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 	/* fast mode by default because of legacy reasons */
 	clk_freq = 400000;
 
-	if (has_acpi_companion(&pdev->dev)) {
-		dw_i2c_acpi_configure(pdev);
-	} else if (pdev->dev.of_node) {
-		of_property_read_u32(pdev->dev.of_node,
-					"i2c-sda-hold-time-ns", &ht);
-
-		of_property_read_u32(pdev->dev.of_node,
-				     "i2c-sda-falling-time-ns",
-				     &dev->sda_falling_time);
-		of_property_read_u32(pdev->dev.of_node,
-				     "i2c-scl-falling-time-ns",
-				     &dev->scl_falling_time);
-
-		of_property_read_u32(pdev->dev.of_node, "clock-frequency",
-				     &clk_freq);
-
-		/* Only standard mode at 100kHz and fast mode at 400kHz
-		 * are supported.
-		 */
-		if (clk_freq != 100000 && clk_freq != 400000) {
-			dev_err(&pdev->dev, "Only 100kHz and 400kHz supported");
-			return -EINVAL;
-		}
+	if (pdata) {
+		clk_freq = pdata->i2c_scl_freq;
 	} else {
-		pdata = dev_get_platdata(&pdev->dev);
-		if (pdata)
-			clk_freq = pdata->i2c_scl_freq;
+		device_property_read_u32(&pdev->dev, "i2c-sda-hold-time-ns",
+					 &ht);
+		device_property_read_u32(&pdev->dev, "i2c-sda-falling-time-ns",
+					 &dev->sda_falling_time);
+		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);
+	}
+
+	if (has_acpi_companion(&pdev->dev))
+		dw_i2c_acpi_configure(pdev);
+
+	/*
+	 * Only standard mode at 100kHz and fast mode at 400kHz are supported.
+	 */
+	if (clk_freq != 100000 && clk_freq != 400000) {
+		dev_err(&pdev->dev, "Only 100kHz and 400kHz supported");
+		return -EINVAL;
 	}
 
 	r = i2c_dw_eval_lock_support(dev);
@@ -209,16 +219,13 @@ static int dw_i2c_plat_probe(struct platform_device *pdev)
 			DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_FAST;
 
 	dev->clk = devm_clk_get(&pdev->dev, NULL);
-	dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
-	if (IS_ERR(dev->clk))
-		return PTR_ERR(dev->clk);
-	clk_prepare_enable(dev->clk);
+	if (!i2c_dw_plat_prepare_clk(dev, true)) {
+		dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
 
-	if (!dev->sda_hold_time && ht) {
-		u32 ic_clk = dev->get_clk_rate_khz(dev);
-
-		dev->sda_hold_time = div_u64((u64)ic_clk * ht + 500000,
-					     1000000);
+		if (!dev->sda_hold_time && ht)
+			dev->sda_hold_time = div_u64(
+				(u64)dev->get_clk_rate_khz(dev) * ht + 500000,
+				1000000);
 	}
 
 	if (!dev->tx_fifo_depth) {
@@ -300,7 +307,7 @@ static int dw_i2c_plat_suspend(struct device *dev)
 	struct dw_i2c_dev *i_dev = platform_get_drvdata(pdev);
 
 	i2c_dw_disable(i_dev);
-	clk_disable_unprepare(i_dev->clk);
+	i2c_dw_plat_prepare_clk(i_dev, false);
 
 	return 0;
 }
@@ -310,7 +317,7 @@ static int dw_i2c_plat_resume(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dw_i2c_dev *i_dev = platform_get_drvdata(pdev);
 
-	clk_prepare_enable(i_dev->clk);
+	i2c_dw_plat_prepare_clk(i_dev, true);
 
 	if (!i_dev->pm_runtime_disabled)
 		i2c_dw_init(i_dev);
diff --git a/drivers/i2c/busses/i2c-eg20t.c b/drivers/i2c/busses/i2c-eg20t.c
index 76e699f9ed97..137125b5eae7 100644
--- a/drivers/i2c/busses/i2c-eg20t.c
+++ b/drivers/i2c/busses/i2c-eg20t.c
@@ -795,6 +795,7 @@ static int pch_i2c_probe(struct pci_dev *pdev,
 		/* base_addr + offset; */
 		adap_info->pch_data[i].pch_base_address = base_addr + 0x100 * i;
 
+		pch_adap->dev.of_node = pdev->dev.of_node;
 		pch_adap->dev.parent = &pdev->dev;
 
 		pch_i2c_init(&adap_info->pch_data[i]);
diff --git a/drivers/i2c/busses/i2c-emev2.c b/drivers/i2c/busses/i2c-emev2.c
index 192ef6b50c79..96bb4e749012 100644
--- a/drivers/i2c/busses/i2c-emev2.c
+++ b/drivers/i2c/busses/i2c-emev2.c
@@ -71,6 +71,7 @@ struct em_i2c_device {
 	struct i2c_adapter adap;
 	struct completion msg_done;
 	struct clk *sclk;
+	struct i2c_client *slave;
 };
 
 static inline void em_clear_set_bit(struct em_i2c_device *priv, u8 clear, u8 set, u8 reg)
@@ -226,22 +227,131 @@ static int em_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 	return num;
 }
 
+static bool em_i2c_slave_irq(struct em_i2c_device *priv)
+{
+	u8 status, value;
+	enum i2c_slave_event event;
+	int ret;
+
+	if (!priv->slave)
+		return false;
+
+	status = readb(priv->base + I2C_OFS_IICSE0);
+
+	/* Extension code, do not participate */
+	if (status & I2C_BIT_EXC0) {
+		em_clear_set_bit(priv, 0, I2C_BIT_LREL0, I2C_OFS_IICC0);
+		return true;
+	}
+
+	/* Stop detected, we don't know if it's for slave or master */
+	if (status & I2C_BIT_SPD0) {
+		/* Notify slave device */
+		i2c_slave_event(priv->slave, I2C_SLAVE_STOP, &value);
+		/* Pretend we did not handle the interrupt */
+		return false;
+	}
+
+	/* Only handle interrupts addressed to us */
+	if (!(status & I2C_BIT_COI0))
+		return false;
+
+	/* Enable stop interrupts */
+	em_clear_set_bit(priv, 0, I2C_BIT_SPIE0, I2C_OFS_IICC0);
+
+	/* Transmission or Reception */
+	if (status & I2C_BIT_TRC0) {
+		if (status & I2C_BIT_ACKD0) {
+			/* 9 bit interrupt mode */
+			em_clear_set_bit(priv, 0, I2C_BIT_WTIM0, I2C_OFS_IICC0);
+
+			/* Send data */
+			event = status & I2C_BIT_STD0 ?
+				I2C_SLAVE_READ_REQUESTED :
+				I2C_SLAVE_READ_PROCESSED;
+			i2c_slave_event(priv->slave, event, &value);
+			writeb(value, priv->base + I2C_OFS_IIC0);
+		} else {
+			/* NACK, stop transmitting */
+			em_clear_set_bit(priv, 0, I2C_BIT_LREL0, I2C_OFS_IICC0);
+		}
+	} else {
+		/* 8 bit interrupt mode */
+		em_clear_set_bit(priv, I2C_BIT_WTIM0, I2C_BIT_ACKE0,
+				I2C_OFS_IICC0);
+		em_clear_set_bit(priv, I2C_BIT_WTIM0, I2C_BIT_WREL0,
+				I2C_OFS_IICC0);
+
+		if (status & I2C_BIT_STD0) {
+			i2c_slave_event(priv->slave, I2C_SLAVE_WRITE_REQUESTED,
+					&value);
+		} else {
+			/* Recv data */
+			value = readb(priv->base + I2C_OFS_IIC0);
+			ret = i2c_slave_event(priv->slave,
+					I2C_SLAVE_WRITE_RECEIVED, &value);
+			if (ret < 0)
+				em_clear_set_bit(priv, I2C_BIT_ACKE0, 0,
+						I2C_OFS_IICC0);
+		}
+	}
+
+	return true;
+}
+
 static irqreturn_t em_i2c_irq_handler(int this_irq, void *dev_id)
 {
 	struct em_i2c_device *priv = dev_id;
 
+	if (em_i2c_slave_irq(priv))
+		return IRQ_HANDLED;
+
 	complete(&priv->msg_done);
+
 	return IRQ_HANDLED;
 }
 
 static u32 em_i2c_func(struct i2c_adapter *adap)
 {
-	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
+	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL | I2C_FUNC_SLAVE;
+}
+
+static int em_i2c_reg_slave(struct i2c_client *slave)
+{
+	struct em_i2c_device *priv = i2c_get_adapdata(slave->adapter);
+
+	if (priv->slave)
+		return -EBUSY;
+
+	if (slave->flags & I2C_CLIENT_TEN)
+		return -EAFNOSUPPORT;
+
+	priv->slave = slave;
+
+	/* Set slave address */
+	writeb(slave->addr << 1, priv->base + I2C_OFS_SVA0);
+
+	return 0;
+}
+
+static int em_i2c_unreg_slave(struct i2c_client *slave)
+{
+	struct em_i2c_device *priv = i2c_get_adapdata(slave->adapter);
+
+	WARN_ON(!priv->slave);
+
+	writeb(0, priv->base + I2C_OFS_SVA0);
+
+	priv->slave = NULL;
+
+	return 0;
 }
 
 static struct i2c_algorithm em_i2c_algo = {
 	.master_xfer = em_i2c_xfer,
 	.functionality = em_i2c_func,
+	.reg_slave      = em_i2c_reg_slave,
+	.unreg_slave    = em_i2c_unreg_slave,
 };
 
 static int em_i2c_probe(struct platform_device *pdev)
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index f62d69799a9c..27fa0cb09538 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -1271,6 +1271,8 @@ static int i801_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	switch (dev->device) {
 	case PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS:
 	case PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_SMBUS:
+	case PCI_DEVICE_ID_INTEL_LEWISBURG_SMBUS:
+	case PCI_DEVICE_ID_INTEL_LEWISBURG_SSKU_SMBUS:
 	case PCI_DEVICE_ID_INTEL_DNV_SMBUS:
 		priv->features |= FEATURE_I2C_BLOCK_READ;
 		priv->features |= FEATURE_IRQ;
diff --git a/drivers/i2c/busses/i2c-ibm_iic.c b/drivers/i2c/busses/i2c-ibm_iic.c
index ab492301581a..b6c080334297 100644
--- a/drivers/i2c/busses/i2c-ibm_iic.c
+++ b/drivers/i2c/busses/i2c-ibm_iic.c
@@ -99,7 +99,7 @@ static void dump_iic_regs(const char* header, struct ibm_iic_private* dev)
 #endif
 
 /* Bus timings (in ns) for bit-banging */
-static struct i2c_timings {
+static struct ibm_iic_timings {
 	unsigned int hd_sta;
 	unsigned int su_sto;
 	unsigned int low;
@@ -241,7 +241,7 @@ static int iic_dc_wait(volatile struct iic_regs __iomem *iic, u8 mask)
 static int iic_smbus_quick(struct ibm_iic_private* dev, const struct i2c_msg* p)
 {
 	volatile struct iic_regs __iomem *iic = dev->vaddr;
-	const struct i2c_timings* t = &timings[dev->fast_mode ? 1 : 0];
+	const struct ibm_iic_timings *t = &timings[dev->fast_mode ? 1 : 0];
 	u8 mask, v, sda;
 	int i, res;
 
diff --git a/drivers/i2c/busses/i2c-img-scb.c b/drivers/i2c/busses/i2c-img-scb.c
index 3795fe130ef2..379ef9c31664 100644
--- a/drivers/i2c/busses/i2c-img-scb.c
+++ b/drivers/i2c/busses/i2c-img-scb.c
@@ -151,10 +151,11 @@
 #define INT_FIFO_EMPTYING		BIT(12)
 #define INT_TRANSACTION_DONE		BIT(15)
 #define INT_SLAVE_EVENT			BIT(16)
+#define INT_MASTER_HALTED		BIT(17)
 #define INT_TIMING			BIT(18)
+#define INT_STOP_DETECTED		BIT(19)
 
 #define INT_FIFO_FULL_FILLING	(INT_FIFO_FULL  | INT_FIFO_FILLING)
-#define INT_FIFO_EMPTY_EMPTYING	(INT_FIFO_EMPTY | INT_FIFO_EMPTYING)
 
 /* Level interrupts need clearing after handling instead of before */
 #define INT_LEVEL			0x01e00
@@ -177,7 +178,8 @@
 					 INT_FIFO_FULL        | \
 					 INT_FIFO_FILLING     | \
 					 INT_FIFO_EMPTY       | \
-					 INT_FIFO_EMPTYING)
+					 INT_MASTER_HALTED    | \
+					 INT_STOP_DETECTED)
 
 #define INT_ENABLE_MASK_WAITSTOP	(INT_SLAVE_EVENT      | \
 					 INT_ADDR_ACK_ERR     | \
@@ -511,7 +513,17 @@ static void img_i2c_soft_reset(struct img_i2c *i2c)
 		       SCB_CONTROL_CLK_ENABLE | SCB_CONTROL_SOFT_RESET);
 }
 
-/* enable or release transaction halt for control of repeated starts */
+/*
+ * Enable or release transaction halt for control of repeated starts.
+ * In version 3.3 of the IP when transaction halt is set, an interrupt
+ * will be generated after each byte of a transfer instead of after
+ * every transfer but before the stop bit.
+ * Due to this behaviour we have to be careful that every time we
+ * release the transaction halt we have to re-enable it straight away
+ * so that we only process a single byte, not doing so will result in
+ * all remaining bytes been processed and a stop bit being issued,
+ * which will prevent us having a repeated start.
+ */
 static void img_i2c_transaction_halt(struct img_i2c *i2c, bool t_halt)
 {
 	u32 val;
@@ -580,7 +592,6 @@ static void img_i2c_read(struct img_i2c *i2c)
 	img_i2c_writel(i2c, SCB_READ_ADDR_REG, i2c->msg.addr);
 	img_i2c_writel(i2c, SCB_READ_COUNT_REG, i2c->msg.len);
 
-	img_i2c_transaction_halt(i2c, false);
 	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
 }
 
@@ -594,7 +605,6 @@ static void img_i2c_write(struct img_i2c *i2c)
 	img_i2c_writel(i2c, SCB_WRITE_ADDR_REG, i2c->msg.addr);
 	img_i2c_writel(i2c, SCB_WRITE_COUNT_REG, i2c->msg.len);
 
-	img_i2c_transaction_halt(i2c, false);
 	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
 	img_i2c_write_fifo(i2c);
 
@@ -750,7 +760,9 @@ static unsigned int img_i2c_atomic(struct img_i2c *i2c,
 			next_cmd = CMD_RET_ACK;
 		break;
 	case CMD_RET_ACK:
-		if (i2c->line_status & LINESTAT_ACK_DET) {
+		if (i2c->line_status & LINESTAT_ACK_DET ||
+		    (i2c->line_status & LINESTAT_NACK_DET &&
+		    i2c->msg.flags & I2C_M_IGNORE_NAK)) {
 			if (i2c->msg.len == 0) {
 				next_cmd = CMD_GEN_STOP;
 			} else if (i2c->msg.flags & I2C_M_RD) {
@@ -858,34 +870,42 @@ static unsigned int img_i2c_auto(struct img_i2c *i2c,
 
 	/* Enable transaction halt on start bit */
 	if (!i2c->last_msg && line_status & LINESTAT_START_BIT_DET) {
-		img_i2c_transaction_halt(i2c, true);
+		img_i2c_transaction_halt(i2c, !i2c->last_msg);
 		/* we're no longer interested in the slave event */
 		i2c->int_enable &= ~INT_SLAVE_EVENT;
 	}
 
 	mod_timer(&i2c->check_timer, jiffies + msecs_to_jiffies(1));
 
+	if (int_status & INT_STOP_DETECTED) {
+		/* Drain remaining data in FIFO and complete transaction */
+		if (i2c->msg.flags & I2C_M_RD)
+			img_i2c_read_fifo(i2c);
+		return ISR_COMPLETE(0);
+	}
+
 	if (i2c->msg.flags & I2C_M_RD) {
-		if (int_status & INT_FIFO_FULL_FILLING) {
+		if (int_status & (INT_FIFO_FULL_FILLING | INT_MASTER_HALTED)) {
 			img_i2c_read_fifo(i2c);
 			if (i2c->msg.len == 0)
 				return ISR_WAITSTOP;
 		}
 	} else {
-		if (int_status & INT_FIFO_EMPTY_EMPTYING) {
-			/*
-			 * The write fifo empty indicates that we're in the
-			 * last byte so it's safe to start a new write
-			 * transaction without losing any bytes from the
-			 * previous one.
-			 * see 2.3.7 Repeated Start Transactions.
-			 */
+		if (int_status & (INT_FIFO_EMPTY | INT_MASTER_HALTED)) {
 			if ((int_status & INT_FIFO_EMPTY) &&
 			    i2c->msg.len == 0)
 				return ISR_WAITSTOP;
 			img_i2c_write_fifo(i2c);
 		}
 	}
+	if (int_status & INT_MASTER_HALTED) {
+		/*
+		 * Release and then enable transaction halt, to
+		 * allow only a single byte to proceed.
+		 */
+		img_i2c_transaction_halt(i2c, false);
+		img_i2c_transaction_halt(i2c, !i2c->last_msg);
+	}
 
 	return 0;
 }
@@ -1017,20 +1037,23 @@ static int img_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 		return -EIO;
 
 	for (i = 0; i < num; i++) {
-		if (likely(msgs[i].len))
-			continue;
 		/*
 		 * 0 byte reads are not possible because the slave could try
 		 * and pull the data line low, preventing a stop bit.
 		 */
-		if (unlikely(msgs[i].flags & I2C_M_RD))
+		if (!msgs[i].len && msgs[i].flags & I2C_M_RD)
 			return -EIO;
 		/*
 		 * 0 byte writes are possible and used for probing, but we
 		 * cannot do them in automatic mode, so use atomic mode
 		 * instead.
+		 *
+		 * Also, the I2C_M_IGNORE_NAK mode can only be implemented
+		 * in atomic mode.
 		 */
-		atomic = true;
+		if (!msgs[i].len ||
+		    (msgs[i].flags & I2C_M_IGNORE_NAK))
+			atomic = true;
 	}
 
 	ret = clk_prepare_enable(i2c->scb_clk);
@@ -1069,12 +1092,31 @@ static int img_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs,
 		img_i2c_writel(i2c, SCB_INT_CLEAR_REG, ~0);
 		img_i2c_writel(i2c, SCB_CLEAR_REG, ~0);
 
-		if (atomic)
+		if (atomic) {
 			img_i2c_atomic_start(i2c);
-		else if (msg->flags & I2C_M_RD)
-			img_i2c_read(i2c);
-		else
-			img_i2c_write(i2c);
+		} else {
+			/*
+			 * Enable transaction halt if not the last message in
+			 * the queue so that we can control repeated starts.
+			 */
+			img_i2c_transaction_halt(i2c, !i2c->last_msg);
+
+			if (msg->flags & I2C_M_RD)
+				img_i2c_read(i2c);
+			else
+				img_i2c_write(i2c);
+
+			/*
+			 * Release and then enable transaction halt, to
+			 * allow only a single byte to proceed.
+			 * This doesn't have an effect on the initial transfer
+			 * but will allow the following transfers to start
+			 * processing if the previous transfer was marked as
+			 * complete while the i2c block was halted.
+			 */
+			img_i2c_transaction_halt(i2c, false);
+			img_i2c_transaction_halt(i2c, !i2c->last_msg);
+		}
 		spin_unlock_irqrestore(&i2c->lock, flags);
 
 		time_left = wait_for_completion_timeout(&i2c->msg_complete,
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index d4d853680ae4..a2b132cef717 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -29,9 +29,6 @@
  *
  */
 
-/** Includes *******************************************************************
-*******************************************************************************/
-
 #include <linux/clk.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
@@ -53,12 +50,10 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_data/i2c-imx.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
 
-/** Defines ********************************************************************
-*******************************************************************************/
-
 /* This will be the driver name the kernel reports */
 #define DRIVER_NAME "imx-i2c"
 
@@ -120,8 +115,7 @@
 #define I2CR_IEN_OPCODE_0	0x0
 #define I2CR_IEN_OPCODE_1	I2CR_IEN
 
-/** Variables ******************************************************************
-*******************************************************************************/
+#define I2C_PM_TIMEOUT		10 /* ms */
 
 /*
  * sorted list of clock divider, register value pairs
@@ -346,7 +340,7 @@ fail_tx:
 	dma_release_channel(dma->chan_tx);
 fail_al:
 	devm_kfree(dev, dma);
-	dev_info(dev, "can't use DMA\n");
+	dev_info(dev, "can't use DMA, using PIO instead.\n");
 }
 
 static void i2c_imx_dma_callback(void *arg)
@@ -393,6 +387,7 @@ static int i2c_imx_dma_xfer(struct imx_i2c_struct *i2c_imx,
 	return 0;
 
 err_submit:
+	dmaengine_terminate_all(dma->chan_using);
 err_desc:
 	dma_unmap_single(chan_dev, dma->dma_buf,
 			dma->dma_len, dma->dma_data_dir);
@@ -416,9 +411,6 @@ static void i2c_imx_dma_free(struct imx_i2c_struct *i2c_imx)
 	dma->chan_using = NULL;
 }
 
-/** Functions for IMX I2C adapter driver ***************************************
-*******************************************************************************/
-
 static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx, int for_busy)
 {
 	unsigned long orig_jiffies = jiffies;
@@ -527,9 +519,6 @@ static int i2c_imx_start(struct imx_i2c_struct *i2c_imx)
 
 	i2c_imx_set_clk(i2c_imx);
 
-	result = clk_prepare_enable(i2c_imx->clk);
-	if (result)
-		return result;
 	imx_i2c_write_reg(i2c_imx->ifdr, i2c_imx, IMX_I2C_IFDR);
 	/* Enable I2C controller */
 	imx_i2c_write_reg(i2c_imx->hwdata->i2sr_clr_opcode, i2c_imx, IMX_I2C_I2SR);
@@ -582,7 +571,6 @@ static void i2c_imx_stop(struct imx_i2c_struct *i2c_imx)
 	/* Disable I2C controller */
 	temp = i2c_imx->hwdata->i2cr_ien_opcode ^ I2CR_IEN,
 	imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2CR);
-	clk_disable_unprepare(i2c_imx->clk);
 }
 
 static irqreturn_t i2c_imx_isr(int irq, void *dev_id)
@@ -901,6 +889,10 @@ static int i2c_imx_xfer(struct i2c_adapter *adapter,
 
 	dev_dbg(&i2c_imx->adapter.dev, "<%s>\n", __func__);
 
+	result = pm_runtime_get_sync(i2c_imx->adapter.dev.parent);
+	if (result < 0)
+		goto out;
+
 	/* Start I2C transfer */
 	result = i2c_imx_start(i2c_imx);
 	if (result) {
@@ -964,6 +956,10 @@ fail0:
 	/* Stop I2C transfer */
 	i2c_imx_stop(i2c_imx);
 
+	pm_runtime_mark_last_busy(i2c_imx->adapter.dev.parent);
+	pm_runtime_put_autosuspend(i2c_imx->adapter.dev.parent);
+
+out:
 	dev_dbg(&i2c_imx->adapter.dev, "<%s> exit with: %s: %d\n", __func__,
 		(result < 0) ? "error" : "success msg",
 			(result < 0) ? result : num);
@@ -997,10 +993,8 @@ static void i2c_imx_init_recovery_info(struct imx_i2c_struct *i2c_imx,
 			PINCTRL_STATE_DEFAULT);
 	i2c_imx->pinctrl_pins_gpio = pinctrl_lookup_state(i2c_imx->pinctrl,
 			"gpio");
-	rinfo->sda_gpio = of_get_named_gpio_flags(pdev->dev.of_node,
-			"sda-gpios", 0, NULL);
-	rinfo->scl_gpio = of_get_named_gpio_flags(pdev->dev.of_node,
-			"scl-gpios", 0, NULL);
+	rinfo->sda_gpio = of_get_named_gpio(pdev->dev.of_node, "sda-gpios", 0);
+	rinfo->scl_gpio = of_get_named_gpio(pdev->dev.of_node, "scl-gpios", 0);
 
 	if (!gpio_is_valid(rinfo->sda_gpio) ||
 	    !gpio_is_valid(rinfo->scl_gpio) ||
@@ -1083,7 +1077,7 @@ static int i2c_imx_probe(struct platform_device *pdev)
 
 	ret = clk_prepare_enable(i2c_imx->clk);
 	if (ret) {
-		dev_err(&pdev->dev, "can't enable I2C clock\n");
+		dev_err(&pdev->dev, "can't enable I2C clock, ret=%d\n", ret);
 		return ret;
 	}
 
@@ -1107,6 +1101,18 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	/* Set up adapter data */
 	i2c_set_adapdata(&i2c_imx->adapter, i2c_imx);
 
+	/* Set up platform driver data */
+	platform_set_drvdata(pdev, i2c_imx);
+
+	pm_runtime_set_autosuspend_delay(&pdev->dev, I2C_PM_TIMEOUT);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
+		goto rpm_disable;
+
 	/* Set up clock divider */
 	i2c_imx->bitrate = IMX_I2C_BIT_RATE;
 	ret = of_property_read_u32(pdev->dev.of_node,
@@ -1125,12 +1131,11 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	ret = i2c_add_numbered_adapter(&i2c_imx->adapter);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "registration failed\n");
-		goto clk_disable;
+		goto rpm_disable;
 	}
 
-	/* Set up platform driver data */
-	platform_set_drvdata(pdev, i2c_imx);
-	clk_disable_unprepare(i2c_imx->clk);
+	pm_runtime_mark_last_busy(&pdev->dev);
+	pm_runtime_put_autosuspend(&pdev->dev);
 
 	dev_dbg(&i2c_imx->adapter.dev, "claimed irq %d\n", irq);
 	dev_dbg(&i2c_imx->adapter.dev, "device resources: %pR\n", res);
@@ -1143,6 +1148,12 @@ static int i2c_imx_probe(struct platform_device *pdev)
 
 	return 0;   /* Return OK */
 
+rpm_disable:
+	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+
 clk_disable:
 	clk_disable_unprepare(i2c_imx->clk);
 	return ret;
@@ -1151,6 +1162,11 @@ clk_disable:
 static int i2c_imx_remove(struct platform_device *pdev)
 {
 	struct imx_i2c_struct *i2c_imx = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0)
+		return ret;
 
 	/* remove adapter */
 	dev_dbg(&i2c_imx->adapter.dev, "adapter removed\n");
@@ -1165,17 +1181,54 @@ static int i2c_imx_remove(struct platform_device *pdev)
 	imx_i2c_write_reg(0, i2c_imx, IMX_I2C_I2CR);
 	imx_i2c_write_reg(0, i2c_imx, IMX_I2C_I2SR);
 
+	clk_disable_unprepare(i2c_imx->clk);
+
+	pm_runtime_put_noidle(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int i2c_imx_runtime_suspend(struct device *dev)
+{
+	struct imx_i2c_struct *i2c_imx  = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(i2c_imx->clk);
+
 	return 0;
 }
 
+static int i2c_imx_runtime_resume(struct device *dev)
+{
+	struct imx_i2c_struct *i2c_imx  = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_prepare_enable(i2c_imx->clk);
+	if (ret)
+		dev_err(dev, "can't enable I2C clock, ret=%d\n", ret);
+
+	return ret;
+}
+
+static const struct dev_pm_ops i2c_imx_pm_ops = {
+	SET_RUNTIME_PM_OPS(i2c_imx_runtime_suspend,
+			   i2c_imx_runtime_resume, NULL)
+};
+#define I2C_IMX_PM_OPS (&i2c_imx_pm_ops)
+#else
+#define I2C_IMX_PM_OPS NULL
+#endif /* CONFIG_PM */
+
 static struct platform_driver i2c_imx_driver = {
 	.probe = i2c_imx_probe,
 	.remove = i2c_imx_remove,
-	.driver	= {
-		.name	= DRIVER_NAME,
+	.driver = {
+		.name = DRIVER_NAME,
+		.pm = I2C_IMX_PM_OPS,
 		.of_match_table = i2c_imx_dt_ids,
 	},
-	.id_table	= imx_i2c_devtype,
+	.id_table = imx_i2c_devtype,
 };
 
 static int __init i2c_adap_imx_init(void)
diff --git a/drivers/i2c/busses/i2c-mt65xx.c b/drivers/i2c/busses/i2c-mt65xx.c
index 9b867169142f..aec8e6ce38a4 100644
--- a/drivers/i2c/busses/i2c-mt65xx.c
+++ b/drivers/i2c/busses/i2c-mt65xx.c
@@ -132,6 +132,7 @@ struct mtk_i2c_compatible {
 	unsigned char pmic_i2c: 1;
 	unsigned char dcm: 1;
 	unsigned char auto_restart: 1;
+	unsigned char aux_len_reg: 1;
 };
 
 struct mtk_i2c {
@@ -153,6 +154,8 @@ struct mtk_i2c {
 	enum mtk_trans_op op;
 	u16 timing_reg;
 	u16 high_speed_reg;
+	unsigned char auto_restart;
+	bool ignore_restart_irq;
 	const struct mtk_i2c_compatible *dev_comp;
 };
 
@@ -178,6 +181,7 @@ static const struct mtk_i2c_compatible mt6577_compat = {
 	.pmic_i2c = 0,
 	.dcm = 1,
 	.auto_restart = 0,
+	.aux_len_reg = 0,
 };
 
 static const struct mtk_i2c_compatible mt6589_compat = {
@@ -185,6 +189,7 @@ static const struct mtk_i2c_compatible mt6589_compat = {
 	.pmic_i2c = 1,
 	.dcm = 0,
 	.auto_restart = 0,
+	.aux_len_reg = 0,
 };
 
 static const struct mtk_i2c_compatible mt8173_compat = {
@@ -192,6 +197,7 @@ static const struct mtk_i2c_compatible mt8173_compat = {
 	.pmic_i2c = 0,
 	.dcm = 1,
 	.auto_restart = 1,
+	.aux_len_reg = 1,
 };
 
 static const struct of_device_id mtk_i2c_of_match[] = {
@@ -373,7 +379,7 @@ static int mtk_i2c_do_transfer(struct mtk_i2c *i2c, struct i2c_msg *msgs,
 
 	i2c->irq_stat = 0;
 
-	if (i2c->dev_comp->auto_restart)
+	if (i2c->auto_restart)
 		restart_flag = I2C_RS_TRANSFER;
 
 	reinit_completion(&i2c->msg_complete);
@@ -411,8 +417,14 @@ static int mtk_i2c_do_transfer(struct mtk_i2c *i2c, struct i2c_msg *msgs,
 
 	/* Set transfer and transaction len */
 	if (i2c->op == I2C_MASTER_WRRD) {
-		writew(msgs->len | ((msgs + 1)->len) << 8,
-		       i2c->base + OFFSET_TRANSFER_LEN);
+		if (i2c->dev_comp->aux_len_reg) {
+			writew(msgs->len, i2c->base + OFFSET_TRANSFER_LEN);
+			writew((msgs + 1)->len, i2c->base +
+			       OFFSET_TRANSFER_LEN_AUX);
+		} else {
+			writew(msgs->len | ((msgs + 1)->len) << 8,
+			       i2c->base + OFFSET_TRANSFER_LEN);
+		}
 		writew(I2C_WRRD_TRANAC_VALUE, i2c->base + OFFSET_TRANSAC_LEN);
 	} else {
 		writew(msgs->len, i2c->base + OFFSET_TRANSFER_LEN);
@@ -461,7 +473,7 @@ static int mtk_i2c_do_transfer(struct mtk_i2c *i2c, struct i2c_msg *msgs,
 
 	writel(I2C_DMA_START_EN, i2c->pdmabase + OFFSET_EN);
 
-	if (!i2c->dev_comp->auto_restart) {
+	if (!i2c->auto_restart) {
 		start_reg = I2C_TRANSAC_START;
 	} else {
 		start_reg = I2C_TRANSAC_START | I2C_RS_MUL_TRIG;
@@ -518,6 +530,24 @@ static int mtk_i2c_transfer(struct i2c_adapter *adap,
 	if (ret)
 		return ret;
 
+	i2c->auto_restart = i2c->dev_comp->auto_restart;
+
+	/* checking if we can skip restart and optimize using WRRD mode */
+	if (i2c->auto_restart && num == 2) {
+		if (!(msgs[0].flags & I2C_M_RD) && (msgs[1].flags & I2C_M_RD) &&
+		    msgs[0].addr == msgs[1].addr) {
+			i2c->auto_restart = 0;
+		}
+	}
+
+	if (i2c->auto_restart && num >= 2 && i2c->speed_hz > MAX_FS_MODE_SPEED)
+		/* ignore the first restart irq after the master code,
+		 * otherwise the first transfer will be discarded.
+		 */
+		i2c->ignore_restart_irq = true;
+	else
+		i2c->ignore_restart_irq = false;
+
 	while (left_num--) {
 		if (!msgs->buf) {
 			dev_dbg(i2c->dev, "data buffer is NULL.\n");
@@ -530,7 +560,7 @@ static int mtk_i2c_transfer(struct i2c_adapter *adap,
 		else
 			i2c->op = I2C_MASTER_WR;
 
-		if (!i2c->dev_comp->auto_restart) {
+		if (!i2c->auto_restart) {
 			if (num > 1) {
 				/* combined two messages into one transaction */
 				i2c->op = I2C_MASTER_WRRD;
@@ -559,7 +589,7 @@ static irqreturn_t mtk_i2c_irq(int irqno, void *dev_id)
 	u16 restart_flag = 0;
 	u16 intr_stat;
 
-	if (i2c->dev_comp->auto_restart)
+	if (i2c->auto_restart)
 		restart_flag = I2C_RS_TRANSFER;
 
 	intr_stat = readw(i2c->base + OFFSET_INTR_STAT);
@@ -571,8 +601,16 @@ static irqreturn_t mtk_i2c_irq(int irqno, void *dev_id)
 	 * i2c->irq_stat need keep the two interrupt value.
 	 */
 	i2c->irq_stat |= intr_stat;
-	if (i2c->irq_stat & (I2C_TRANSAC_COMP | restart_flag))
-		complete(&i2c->msg_complete);
+
+	if (i2c->ignore_restart_irq && (i2c->irq_stat & restart_flag)) {
+		i2c->ignore_restart_irq = false;
+		i2c->irq_stat = 0;
+		writew(I2C_RS_MUL_CNFG | I2C_RS_MUL_TRIG | I2C_TRANSAC_START,
+		       i2c->base + OFFSET_START);
+	} else {
+		if (i2c->irq_stat & (I2C_TRANSAC_COMP | restart_flag))
+			complete(&i2c->msg_complete);
+	}
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 08d26ba61ed3..13c45296ce5b 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -1450,7 +1450,8 @@ omap_i2c_probe(struct platform_device *pdev)
 
 err_unuse_clocks:
 	omap_i2c_write_reg(omap, OMAP_I2C_CON_REG, 0);
-	pm_runtime_put(omap->dev);
+	pm_runtime_dont_use_autosuspend(omap->dev);
+	pm_runtime_put_sync(omap->dev);
 	pm_runtime_disable(&pdev->dev);
 err_free_mem:
 
@@ -1468,6 +1469,7 @@ static int omap_i2c_remove(struct platform_device *pdev)
 		return ret;
 
 	omap_i2c_write_reg(omap, OMAP_I2C_CON_REG, 0);
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 	return 0;
diff --git a/drivers/i2c/busses/i2c-piix4.c b/drivers/i2c/busses/i2c-piix4.c
index 630bce68bf38..93f2895383ee 100644
--- a/drivers/i2c/busses/i2c-piix4.c
+++ b/drivers/i2c/busses/i2c-piix4.c
@@ -23,6 +23,9 @@
 
    Note: we assume there can only be one device, with one or more
    SMBus interfaces.
+   The device can register multiple i2c_adapters (up to PIIX4_MAX_ADAPTERS).
+   For devices supporting multiple ports the i2c_adapter should provide
+   an i2c_algorithm to access them.
 */
 
 #include <linux/module.h>
@@ -37,6 +40,7 @@
 #include <linux/dmi.h>
 #include <linux/acpi.h>
 #include <linux/io.h>
+#include <linux/mutex.h>
 
 
 /* PIIX4 SMBus address offsets */
@@ -75,6 +79,16 @@
 #define PIIX4_WORD_DATA		0x0C
 #define PIIX4_BLOCK_DATA	0x14
 
+/* Multi-port constants */
+#define PIIX4_MAX_ADAPTERS 4
+
+/* SB800 constants */
+#define SB800_PIIX4_SMB_IDX		0xcd6
+
+/* SB800 port is selected by bits 2:1 of the smb_en register (0x2c) */
+#define SB800_PIIX4_PORT_IDX		0x2c
+#define SB800_PIIX4_PORT_IDX_MASK	0x06
+
 /* insmod parameters */
 
 /* If force is set to anything different from 0, we forcibly enable the
@@ -122,8 +136,19 @@ static const struct dmi_system_id piix4_dmi_ibm[] = {
 	{ },
 };
 
+/* SB800 globals */
+static DEFINE_MUTEX(piix4_mutex_sb800);
+static const char *piix4_main_port_names_sb800[PIIX4_MAX_ADAPTERS] = {
+	" port 0", " port 2", " port 3", " port 4"
+};
+static const char *piix4_aux_port_name_sb800 = " port 1";
+
 struct i2c_piix4_adapdata {
 	unsigned short smba;
+
+	/* SB800 */
+	bool sb800_main;
+	unsigned short port;
 };
 
 static int piix4_setup(struct pci_dev *PIIX4_dev,
@@ -229,7 +254,6 @@ static int piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 			     const struct pci_device_id *id, u8 aux)
 {
 	unsigned short piix4_smba;
-	unsigned short smba_idx = 0xcd6;
 	u8 smba_en_lo, smba_en_hi, smb_en, smb_en_status;
 	u8 i2ccfg, i2ccfg_offset = 0x10;
 
@@ -251,16 +275,12 @@ static int piix4_setup_sb800(struct pci_dev *PIIX4_dev,
 	else
 		smb_en = (aux) ? 0x28 : 0x2c;
 
-	if (!request_region(smba_idx, 2, "smba_idx")) {
-		dev_err(&PIIX4_dev->dev, "SMBus base address index region "
-			"0x%x already in use!\n", smba_idx);
-		return -EBUSY;
-	}
-	outb_p(smb_en, smba_idx);
-	smba_en_lo = inb_p(smba_idx + 1);
-	outb_p(smb_en + 1, smba_idx);
-	smba_en_hi = inb_p(smba_idx + 1);
-	release_region(smba_idx, 2);
+	mutex_lock(&piix4_mutex_sb800);
+	outb_p(smb_en, SB800_PIIX4_SMB_IDX);
+	smba_en_lo = inb_p(SB800_PIIX4_SMB_IDX + 1);
+	outb_p(smb_en + 1, SB800_PIIX4_SMB_IDX);
+	smba_en_hi = inb_p(SB800_PIIX4_SMB_IDX + 1);
+	mutex_unlock(&piix4_mutex_sb800);
 
 	if (!smb_en) {
 		smb_en_status = smba_en_lo & 0x10;
@@ -483,7 +503,7 @@ static s32 piix4_access(struct i2c_adapter * adap, u16 addr,
 			if (len == 0 || len > I2C_SMBUS_BLOCK_MAX)
 				return -EINVAL;
 			outb_p(len, SMBHSTDAT0);
-			i = inb_p(SMBHSTCNT);	/* Reset SMBBLKDAT */
+			inb_p(SMBHSTCNT);	/* Reset SMBBLKDAT */
 			for (i = 1; i <= len; i++)
 				outb_p(data->block[i], SMBBLKDAT);
 		}
@@ -516,7 +536,7 @@ static s32 piix4_access(struct i2c_adapter * adap, u16 addr,
 		data->block[0] = inb_p(SMBHSTDAT0);
 		if (data->block[0] == 0 || data->block[0] > I2C_SMBUS_BLOCK_MAX)
 			return -EPROTO;
-		i = inb_p(SMBHSTCNT);	/* Reset SMBBLKDAT */
+		inb_p(SMBHSTCNT);	/* Reset SMBBLKDAT */
 		for (i = 1; i <= data->block[0]; i++)
 			data->block[i] = inb_p(SMBBLKDAT);
 		break;
@@ -524,6 +544,43 @@ static s32 piix4_access(struct i2c_adapter * adap, u16 addr,
 	return 0;
 }
 
+/*
+ * Handles access to multiple SMBus ports on the SB800.
+ * The port is selected by bits 2:1 of the smb_en register (0x2c).
+ * Returns negative errno on error.
+ *
+ * Note: The selected port must be returned to the initial selection to avoid
+ * problems on certain systems.
+ */
+static s32 piix4_access_sb800(struct i2c_adapter *adap, u16 addr,
+		 unsigned short flags, char read_write,
+		 u8 command, int size, union i2c_smbus_data *data)
+{
+	struct i2c_piix4_adapdata *adapdata = i2c_get_adapdata(adap);
+	u8 smba_en_lo;
+	u8 port;
+	int retval;
+
+	mutex_lock(&piix4_mutex_sb800);
+
+	outb_p(SB800_PIIX4_PORT_IDX, SB800_PIIX4_SMB_IDX);
+	smba_en_lo = inb_p(SB800_PIIX4_SMB_IDX + 1);
+
+	port = adapdata->port;
+	if ((smba_en_lo & SB800_PIIX4_PORT_IDX_MASK) != (port << 1))
+		outb_p((smba_en_lo & ~SB800_PIIX4_PORT_IDX_MASK) | (port << 1),
+		       SB800_PIIX4_SMB_IDX + 1);
+
+	retval = piix4_access(adap, addr, flags, read_write,
+			      command, size, data);
+
+	outb_p(smba_en_lo, SB800_PIIX4_SMB_IDX + 1);
+
+	mutex_unlock(&piix4_mutex_sb800);
+
+	return retval;
+}
+
 static u32 piix4_func(struct i2c_adapter *adapter)
 {
 	return I2C_FUNC_SMBUS_QUICK | I2C_FUNC_SMBUS_BYTE |
@@ -536,6 +593,11 @@ static const struct i2c_algorithm smbus_algorithm = {
 	.functionality	= piix4_func,
 };
 
+static const struct i2c_algorithm piix4_smbus_algorithm_sb800 = {
+	.smbus_xfer	= piix4_access_sb800,
+	.functionality	= piix4_func,
+};
+
 static const struct pci_device_id piix4_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82371AB_3) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_82443MX_3) },
@@ -561,11 +623,12 @@ static const struct pci_device_id piix4_ids[] = {
 
 MODULE_DEVICE_TABLE (pci, piix4_ids);
 
-static struct i2c_adapter *piix4_main_adapter;
+static struct i2c_adapter *piix4_main_adapters[PIIX4_MAX_ADAPTERS];
 static struct i2c_adapter *piix4_aux_adapter;
 
 static int piix4_add_adapter(struct pci_dev *dev, unsigned short smba,
-			     struct i2c_adapter **padap)
+			     bool sb800_main, unsigned short port,
+			     const char *name, struct i2c_adapter **padap)
 {
 	struct i2c_adapter *adap;
 	struct i2c_piix4_adapdata *adapdata;
@@ -579,7 +642,8 @@ static int piix4_add_adapter(struct pci_dev *dev, unsigned short smba,
 
 	adap->owner = THIS_MODULE;
 	adap->class = I2C_CLASS_HWMON | I2C_CLASS_SPD;
-	adap->algo = &smbus_algorithm;
+	adap->algo = sb800_main ? &piix4_smbus_algorithm_sb800
+				: &smbus_algorithm;
 
 	adapdata = kzalloc(sizeof(*adapdata), GFP_KERNEL);
 	if (adapdata == NULL) {
@@ -589,12 +653,14 @@ static int piix4_add_adapter(struct pci_dev *dev, unsigned short smba,
 	}
 
 	adapdata->smba = smba;
+	adapdata->sb800_main = sb800_main;
+	adapdata->port = port;
 
 	/* set up the sysfs linkage to our parent device */
 	adap->dev.parent = &dev->dev;
 
 	snprintf(adap->name, sizeof(adap->name),
-		"SMBus PIIX4 adapter at %04x", smba);
+		"SMBus PIIX4 adapter%s at %04x", name, smba);
 
 	i2c_set_adapdata(adap, adapdata);
 
@@ -611,27 +677,83 @@ static int piix4_add_adapter(struct pci_dev *dev, unsigned short smba,
 	return 0;
 }
 
+static int piix4_add_adapters_sb800(struct pci_dev *dev, unsigned short smba)
+{
+	struct i2c_piix4_adapdata *adapdata;
+	int port;
+	int retval;
+
+	for (port = 0; port < PIIX4_MAX_ADAPTERS; port++) {
+		retval = piix4_add_adapter(dev, smba, true, port,
+					   piix4_main_port_names_sb800[port],
+					   &piix4_main_adapters[port]);
+		if (retval < 0)
+			goto error;
+	}
+
+	return retval;
+
+error:
+	dev_err(&dev->dev,
+		"Error setting up SB800 adapters. Unregistering!\n");
+	while (--port >= 0) {
+		adapdata = i2c_get_adapdata(piix4_main_adapters[port]);
+		if (adapdata->smba) {
+			i2c_del_adapter(piix4_main_adapters[port]);
+			kfree(adapdata);
+			kfree(piix4_main_adapters[port]);
+			piix4_main_adapters[port] = NULL;
+		}
+	}
+
+	return retval;
+}
+
 static int piix4_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
 	int retval;
+	bool is_sb800 = false;
 
 	if ((dev->vendor == PCI_VENDOR_ID_ATI &&
 	     dev->device == PCI_DEVICE_ID_ATI_SBX00_SMBUS &&
 	     dev->revision >= 0x40) ||
-	    dev->vendor == PCI_VENDOR_ID_AMD)
+	    dev->vendor == PCI_VENDOR_ID_AMD) {
+		is_sb800 = true;
+
+		if (!request_region(SB800_PIIX4_SMB_IDX, 2, "smba_idx")) {
+			dev_err(&dev->dev,
+			"SMBus base address index region 0x%x already in use!\n",
+			SB800_PIIX4_SMB_IDX);
+			return -EBUSY;
+		}
+
 		/* base address location etc changed in SB800 */
 		retval = piix4_setup_sb800(dev, id, 0);
-	else
-		retval = piix4_setup(dev, id);
+		if (retval < 0) {
+			release_region(SB800_PIIX4_SMB_IDX, 2);
+			return retval;
+		}
 
-	/* If no main SMBus found, give up */
-	if (retval < 0)
-		return retval;
+		/*
+		 * Try to register multiplexed main SMBus adapter,
+		 * give up if we can't
+		 */
+		retval = piix4_add_adapters_sb800(dev, retval);
+		if (retval < 0) {
+			release_region(SB800_PIIX4_SMB_IDX, 2);
+			return retval;
+		}
+	} else {
+		retval = piix4_setup(dev, id);
+		if (retval < 0)
+			return retval;
 
-	/* Try to register main SMBus adapter, give up if we can't */
-	retval = piix4_add_adapter(dev, retval, &piix4_main_adapter);
-	if (retval < 0)
-		return retval;
+		/* Try to register main SMBus adapter, give up if we can't */
+		retval = piix4_add_adapter(dev, retval, false, 0, "",
+					   &piix4_main_adapters[0]);
+		if (retval < 0)
+			return retval;
+	}
 
 	/* Check for auxiliary SMBus on some AMD chipsets */
 	retval = -ENODEV;
@@ -654,7 +776,9 @@ static int piix4_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	if (retval > 0) {
 		/* Try to add the aux adapter if it exists,
 		 * piix4_add_adapter will clean up if this fails */
-		piix4_add_adapter(dev, retval, &piix4_aux_adapter);
+		piix4_add_adapter(dev, retval, false, 0,
+				  is_sb800 ? piix4_aux_port_name_sb800 : "",
+				  &piix4_aux_adapter);
 	}
 
 	return 0;
@@ -666,7 +790,11 @@ static void piix4_adap_remove(struct i2c_adapter *adap)
 
 	if (adapdata->smba) {
 		i2c_del_adapter(adap);
-		release_region(adapdata->smba, SMBIOSIZE);
+		if (adapdata->port == 0) {
+			release_region(adapdata->smba, SMBIOSIZE);
+			if (adapdata->sb800_main)
+				release_region(SB800_PIIX4_SMB_IDX, 2);
+		}
 		kfree(adapdata);
 		kfree(adap);
 	}
@@ -674,9 +802,13 @@ static void piix4_adap_remove(struct i2c_adapter *adap)
 
 static void piix4_remove(struct pci_dev *dev)
 {
-	if (piix4_main_adapter) {
-		piix4_adap_remove(piix4_main_adapter);
-		piix4_main_adapter = NULL;
+	int port = PIIX4_MAX_ADAPTERS;
+
+	while (--port >= 0) {
+		if (piix4_main_adapters[port]) {
+			piix4_adap_remove(piix4_main_adapters[port]);
+			piix4_main_adapters[port] = NULL;
+		}
 	}
 
 	if (piix4_aux_adapter) {
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index 599c0d7bd906..1abeadc8ab79 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -1,7 +1,8 @@
 /*
  * Driver for the Renesas RCar I2C unit
  *
- * Copyright (C) 2014 Wolfram Sang <wsa@sang-engineering.com>
+ * Copyright (C) 2014-15 Wolfram Sang <wsa@sang-engineering.com>
+ * Copyright (C) 2011-2015 Renesas Electronics Corporation
  *
  * Copyright (C) 2012-14 Renesas Solutions Corp.
  * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
@@ -9,9 +10,6 @@
  * This file is based on the drivers/i2c/busses/i2c-sh7760.c
  * (c) 2005-2008 MSC Vertriebsges.m.b.H, Manuel Lauss <mlau@msc-ge.com>
  *
- * This file used out-of-tree driver i2c-rcar.c
- * Copyright (C) 2011-2012 Renesas Electronics Corporation
- *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; version 2 of the License.
@@ -33,7 +31,6 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
 
 /* register offsets */
 #define ICSCR	0x00	/* slave ctrl */
@@ -84,6 +81,7 @@
 
 #define RCAR_BUS_PHASE_START	(MDBS | MIE | ESG)
 #define RCAR_BUS_PHASE_DATA	(MDBS | MIE)
+#define RCAR_BUS_MASK_DATA	(~(ESG | FSB) & 0xFF)
 #define RCAR_BUS_PHASE_STOP	(MDBS | MIE | FSB)
 
 #define RCAR_IRQ_SEND	(MNR | MAL | MST | MAT | MDE)
@@ -94,10 +92,13 @@
 #define RCAR_IRQ_ACK_RECV	(~(MAT | MDR) & 0xFF)
 
 #define ID_LAST_MSG	(1 << 0)
-#define ID_IOERROR	(1 << 1)
+#define ID_FIRST_MSG	(1 << 1)
 #define ID_DONE		(1 << 2)
 #define ID_ARBLOST	(1 << 3)
 #define ID_NACK		(1 << 4)
+/* persistent flags */
+#define ID_P_PM_BLOCKED	(1 << 31)
+#define ID_P_MASK	ID_P_PM_BLOCKED
 
 enum rcar_i2c_type {
 	I2C_RCAR_GEN1,
@@ -108,10 +109,10 @@ enum rcar_i2c_type {
 struct rcar_i2c_priv {
 	void __iomem *io;
 	struct i2c_adapter adap;
-	struct i2c_msg	*msg;
+	struct i2c_msg *msg;
+	int msgs_left;
 	struct clk *clk;
 
-	spinlock_t lock;
 	wait_queue_head_t wait;
 
 	int pos;
@@ -124,9 +125,6 @@ struct rcar_i2c_priv {
 #define rcar_i2c_priv_to_dev(p)		((p)->adap.dev.parent)
 #define rcar_i2c_is_recv(p)		((p)->msg->flags & I2C_M_RD)
 
-#define rcar_i2c_flags_set(p, f)	((p)->flags |= (f))
-#define rcar_i2c_flags_has(p, f)	((p)->flags & (f))
-
 #define LOOP_TIMEOUT	1024
 
 
@@ -144,9 +142,10 @@ static void rcar_i2c_init(struct rcar_i2c_priv *priv)
 {
 	/* reset master mode */
 	rcar_i2c_write(priv, ICMIER, 0);
-	rcar_i2c_write(priv, ICMCR, 0);
+	rcar_i2c_write(priv, ICMCR, MDBS);
 	rcar_i2c_write(priv, ICMSR, 0);
-	rcar_i2c_write(priv, ICMAR, 0);
+	/* start clock */
+	rcar_i2c_write(priv, ICCCR, priv->icccr);
 }
 
 static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
@@ -163,15 +162,17 @@ static int rcar_i2c_bus_barrier(struct rcar_i2c_priv *priv)
 	return -EBUSY;
 }
 
-static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
-				    u32 bus_speed,
-				    struct device *dev)
+static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv, struct i2c_timings *t)
 {
-	u32 scgd, cdf;
-	u32 round, ick;
-	u32 scl;
-	u32 cdf_width;
+	u32 scgd, cdf, round, ick, sum, scl, cdf_width;
 	unsigned long rate;
+	struct device *dev = rcar_i2c_priv_to_dev(priv);
+
+	/* Fall back to previously used values if not supplied */
+	t->bus_freq_hz = t->bus_freq_hz ?: 100000;
+	t->scl_fall_ns = t->scl_fall_ns ?: 35;
+	t->scl_rise_ns = t->scl_rise_ns ?: 200;
+	t->scl_int_delay_ns = t->scl_int_delay_ns ?: 50;
 
 	switch (priv->devtype) {
 	case I2C_RCAR_GEN1:
@@ -195,9 +196,9 @@ static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 	 * SCL	= ick / (20 + SCGD * 8 + F[(ticf + tr + intd) * ick])
 	 *
 	 * ick  : I2C internal clock < 20 MHz
-	 * ticf : I2C SCL falling time  =  35 ns here
-	 * tr   : I2C SCL rising  time  = 200 ns here
-	 * intd : LSI internal delay    =  50 ns here
+	 * ticf : I2C SCL falling time
+	 * tr   : I2C SCL rising  time
+	 * intd : LSI internal delay
 	 * clkp : peripheral_clk
 	 * F[]  : integer up-valuation
 	 */
@@ -213,12 +214,12 @@ static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 	 * it is impossible to calculate large scale
 	 * number on u32. separate it
 	 *
-	 * F[(ticf + tr + intd) * ick]
-	 *  = F[(35 + 200 + 50)ns * ick]
-	 *  = F[285 * ick / 1000000000]
-	 *  = F[(ick / 1000000) * 285 / 1000]
+	 * F[(ticf + tr + intd) * ick] with sum = (ticf + tr + intd)
+	 *  = F[sum * ick / 1000000000]
+	 *  = F[(ick / 1000000) * sum / 1000]
 	 */
-	round = (ick + 500000) / 1000000 * 285;
+	sum = t->scl_fall_ns + t->scl_rise_ns + t->scl_int_delay_ns;
+	round = (ick + 500000) / 1000000 * sum;
 	round = (round + 500) / 1000;
 
 	/*
@@ -235,7 +236,7 @@ static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 	 */
 	for (scgd = 0; scgd < 0x40; scgd++) {
 		scl = ick / (20 + (scgd * 8) + round);
-		if (scl <= bus_speed)
+		if (scl <= t->bus_freq_hz)
 			goto scgd_find;
 	}
 	dev_err(dev, "it is impossible to calculate best SCL\n");
@@ -243,11 +244,9 @@ static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 
 scgd_find:
 	dev_dbg(dev, "clk %d/%d(%lu), round %u, CDF:0x%x, SCGD: 0x%x\n",
-		scl, bus_speed, clk_get_rate(priv->clk), round, cdf, scgd);
+		scl, t->bus_freq_hz, clk_get_rate(priv->clk), round, cdf, scgd);
 
-	/*
-	 * keep icccr value
-	 */
+	/* keep icccr value */
 	priv->icccr = scgd << cdf_width | cdf;
 
 	return 0;
@@ -257,33 +256,44 @@ static void rcar_i2c_prepare_msg(struct rcar_i2c_priv *priv)
 {
 	int read = !!rcar_i2c_is_recv(priv);
 
+	priv->pos = 0;
+	if (priv->msgs_left == 1)
+		priv->flags |= ID_LAST_MSG;
+
 	rcar_i2c_write(priv, ICMAR, (priv->msg->addr << 1) | read);
-	rcar_i2c_write(priv, ICMSR, 0);
-	rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_START);
+	/*
+	 * We don't have a testcase but the HW engineers say that the write order
+	 * of ICMSR and ICMCR depends on whether we issue START or REP_START. Since
+	 * it didn't cause a drawback for me, let's rather be safe than sorry.
+	 */
+	if (priv->flags & ID_FIRST_MSG) {
+		rcar_i2c_write(priv, ICMSR, 0);
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_START);
+	} else {
+		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_START);
+		rcar_i2c_write(priv, ICMSR, 0);
+	}
 	rcar_i2c_write(priv, ICMIER, read ? RCAR_IRQ_RECV : RCAR_IRQ_SEND);
 }
 
+static void rcar_i2c_next_msg(struct rcar_i2c_priv *priv)
+{
+	priv->msg++;
+	priv->msgs_left--;
+	priv->flags &= ID_P_MASK;
+	rcar_i2c_prepare_msg(priv);
+}
+
 /*
  *		interrupt functions
  */
-static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
+static void rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
 {
 	struct i2c_msg *msg = priv->msg;
 
-	/*
-	 * FIXME
-	 * sometimes, unknown interrupt happened.
-	 * Do nothing
-	 */
+	/* FIXME: sometimes, unknown interrupt happened. Do nothing */
 	if (!(msr & MDE))
-		return 0;
-
-	/*
-	 * If address transfer phase finished,
-	 * goto data phase.
-	 */
-	if (msr & MAT)
-		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_DATA);
+		return;
 
 	if (priv->pos < msg->len) {
 		/*
@@ -305,67 +315,50 @@ static int rcar_i2c_irq_send(struct rcar_i2c_priv *priv, u32 msr)
 		 * [ICRXTX] -> [SHIFT] -> [I2C bus]
 		 */
 
-		if (priv->flags & ID_LAST_MSG)
+		if (priv->flags & ID_LAST_MSG) {
 			/*
 			 * If current msg is the _LAST_ msg,
 			 * prepare stop condition here.
 			 * ID_DONE will be set on STOP irq.
 			 */
 			rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
-		else
-			/*
-			 * If current msg is _NOT_ last msg,
-			 * it doesn't call stop phase.
-			 * thus, there is no STOP irq.
-			 * return ID_DONE here.
-			 */
-			return ID_DONE;
+		} else {
+			rcar_i2c_next_msg(priv);
+			return;
+		}
 	}
 
 	rcar_i2c_write(priv, ICMSR, RCAR_IRQ_ACK_SEND);
-
-	return 0;
 }
 
-static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
+static void rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
 {
 	struct i2c_msg *msg = priv->msg;
 
-	/*
-	 * FIXME
-	 * sometimes, unknown interrupt happened.
-	 * Do nothing
-	 */
+	/* FIXME: sometimes, unknown interrupt happened. Do nothing */
 	if (!(msr & MDR))
-		return 0;
+		return;
 
 	if (msr & MAT) {
-		/*
-		 * Address transfer phase finished,
-		 * but, there is no data at this point.
-		 * Do nothing.
-		 */
+		/* Address transfer phase finished, but no data at this point. */
 	} else if (priv->pos < msg->len) {
-		/*
-		 * get received data
-		 */
+		/* get received data */
 		msg->buf[priv->pos] = rcar_i2c_read(priv, ICRXTX);
 		priv->pos++;
 	}
 
 	/*
-	 * If next received data is the _LAST_,
-	 * go to STOP phase,
-	 * otherwise, go to DATA phase.
+	 * If next received data is the _LAST_, go to STOP phase. Might be
+	 * overwritten by REP START when setting up a new msg. Not elegant
+	 * but the only stable sequence for REP START I have found so far.
 	 */
 	if (priv->pos + 1 >= msg->len)
 		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
-	else
-		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_DATA);
-
-	rcar_i2c_write(priv, ICMSR, RCAR_IRQ_ACK_RECV);
 
-	return 0;
+	if (priv->pos == msg->len && !(priv->flags & ID_LAST_MSG))
+		rcar_i2c_next_msg(priv);
+	else
+		rcar_i2c_write(priv, ICMSR, RCAR_IRQ_ACK_RECV);
 }
 
 static bool rcar_i2c_slave_irq(struct rcar_i2c_priv *priv)
@@ -426,62 +419,57 @@ static bool rcar_i2c_slave_irq(struct rcar_i2c_priv *priv)
 static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 {
 	struct rcar_i2c_priv *priv = ptr;
-	irqreturn_t result = IRQ_HANDLED;
-	u32 msr;
-
-	/*-------------- spin lock -----------------*/
-	spin_lock(&priv->lock);
+	u32 msr, val;
 
-	if (rcar_i2c_slave_irq(priv))
-		goto exit;
+	/* Clear START or STOP as soon as we can */
+	val = rcar_i2c_read(priv, ICMCR);
+	rcar_i2c_write(priv, ICMCR, val & RCAR_BUS_MASK_DATA);
 
 	msr = rcar_i2c_read(priv, ICMSR);
 
 	/* Only handle interrupts that are currently enabled */
 	msr &= rcar_i2c_read(priv, ICMIER);
 	if (!msr) {
-		result = IRQ_NONE;
-		goto exit;
+		if (rcar_i2c_slave_irq(priv))
+			return IRQ_HANDLED;
+
+		return IRQ_NONE;
 	}
 
 	/* Arbitration lost */
 	if (msr & MAL) {
-		rcar_i2c_flags_set(priv, (ID_DONE | ID_ARBLOST));
+		priv->flags |= ID_DONE | ID_ARBLOST;
 		goto out;
 	}
 
 	/* Nack */
 	if (msr & MNR) {
-		/* go to stop phase */
-		rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_STOP);
+		/* HW automatically sends STOP after received NACK */
 		rcar_i2c_write(priv, ICMIER, RCAR_IRQ_STOP);
-		rcar_i2c_flags_set(priv, ID_NACK);
+		priv->flags |= ID_NACK;
 		goto out;
 	}
 
 	/* Stop */
 	if (msr & MST) {
-		rcar_i2c_flags_set(priv, ID_DONE);
+		priv->msgs_left--; /* The last message also made it */
+		priv->flags |= ID_DONE;
 		goto out;
 	}
 
 	if (rcar_i2c_is_recv(priv))
-		rcar_i2c_flags_set(priv, rcar_i2c_irq_recv(priv, msr));
+		rcar_i2c_irq_recv(priv, msr);
 	else
-		rcar_i2c_flags_set(priv, rcar_i2c_irq_send(priv, msr));
+		rcar_i2c_irq_send(priv, msr);
 
 out:
-	if (rcar_i2c_flags_has(priv, ID_DONE)) {
+	if (priv->flags & ID_DONE) {
 		rcar_i2c_write(priv, ICMIER, 0);
 		rcar_i2c_write(priv, ICMSR, 0);
 		wake_up(&priv->wait);
 	}
 
-exit:
-	spin_unlock(&priv->lock);
-	/*-------------- spin unlock -----------------*/
-
-	return result;
+	return IRQ_HANDLED;
 }
 
 static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
@@ -490,22 +478,11 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 {
 	struct rcar_i2c_priv *priv = i2c_get_adapdata(adap);
 	struct device *dev = rcar_i2c_priv_to_dev(priv);
-	unsigned long flags;
 	int i, ret;
-	long timeout;
+	long time_left;
 
 	pm_runtime_get_sync(dev);
 
-	/*-------------- spin lock -----------------*/
-	spin_lock_irqsave(&priv->lock, flags);
-
-	rcar_i2c_init(priv);
-	/* start clock */
-	rcar_i2c_write(priv, ICCCR, priv->icccr);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-	/*-------------- spin unlock -----------------*/
-
 	ret = rcar_i2c_bus_barrier(priv);
 	if (ret < 0)
 		goto out;
@@ -514,48 +491,27 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 		/* This HW can't send STOP after address phase */
 		if (msgs[i].len == 0) {
 			ret = -EOPNOTSUPP;
-			break;
-		}
-
-		/*-------------- spin lock -----------------*/
-		spin_lock_irqsave(&priv->lock, flags);
-
-		/* init each data */
-		priv->msg	= &msgs[i];
-		priv->pos	= 0;
-		priv->flags	= 0;
-		if (i == num - 1)
-			rcar_i2c_flags_set(priv, ID_LAST_MSG);
-
-		rcar_i2c_prepare_msg(priv);
-
-		spin_unlock_irqrestore(&priv->lock, flags);
-		/*-------------- spin unlock -----------------*/
-
-		timeout = wait_event_timeout(priv->wait,
-					     rcar_i2c_flags_has(priv, ID_DONE),
-					     adap->timeout);
-		if (!timeout) {
-			ret = -ETIMEDOUT;
-			break;
-		}
-
-		if (rcar_i2c_flags_has(priv, ID_NACK)) {
-			ret = -ENXIO;
-			break;
-		}
-
-		if (rcar_i2c_flags_has(priv, ID_ARBLOST)) {
-			ret = -EAGAIN;
-			break;
-		}
-
-		if (rcar_i2c_flags_has(priv, ID_IOERROR)) {
-			ret = -EIO;
-			break;
+			goto out;
 		}
+	}
 
-		ret = i + 1; /* The number of transfer */
+	/* init first message */
+	priv->msg = msgs;
+	priv->msgs_left = num;
+	priv->flags = (priv->flags & ID_P_MASK) | ID_FIRST_MSG;
+	rcar_i2c_prepare_msg(priv);
+
+	time_left = wait_event_timeout(priv->wait, priv->flags & ID_DONE,
+				     num * adap->timeout);
+	if (!time_left) {
+		rcar_i2c_init(priv);
+		ret = -ETIMEDOUT;
+	} else if (priv->flags & ID_NACK) {
+		ret = -ENXIO;
+	} else if (priv->flags & ID_ARBLOST) {
+		ret = -EAGAIN;
+	} else {
+		ret = num - priv->msgs_left; /* The number of transfer */
 	}
 out:
 	pm_runtime_put(dev);
@@ -637,7 +593,7 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	struct i2c_adapter *adap;
 	struct resource *res;
 	struct device *dev = &pdev->dev;
-	u32 bus_speed;
+	struct i2c_timings i2c_t;
 	int irq, ret;
 
 	priv = devm_kzalloc(dev, sizeof(struct rcar_i2c_priv), GFP_KERNEL);
@@ -650,23 +606,13 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 		return PTR_ERR(priv->clk);
 	}
 
-	bus_speed = 100000; /* default 100 kHz */
-	of_property_read_u32(dev->of_node, "clock-frequency", &bus_speed);
-
-	priv->devtype = (enum rcar_i2c_type)of_match_device(rcar_i2c_dt_ids, dev)->data;
-
-	ret = rcar_i2c_clock_calculate(priv, bus_speed, dev);
-	if (ret < 0)
-		return ret;
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->io = devm_ioremap_resource(dev, res);
 	if (IS_ERR(priv->io))
 		return PTR_ERR(priv->io);
 
-	irq = platform_get_irq(pdev, 0);
+	priv->devtype = (enum rcar_i2c_type)of_match_device(rcar_i2c_dt_ids, dev)->data;
 	init_waitqueue_head(&priv->wait);
-	spin_lock_init(&priv->lock);
 
 	adap = &priv->adap;
 	adap->nr = pdev->id;
@@ -678,26 +624,47 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 	i2c_set_adapdata(adap, priv);
 	strlcpy(adap->name, pdev->name, sizeof(adap->name));
 
-	ret = devm_request_irq(dev, irq, rcar_i2c_irq, 0,
-			       dev_name(dev), priv);
+	i2c_parse_fw_timings(dev, &i2c_t, false);
+
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+	ret = rcar_i2c_clock_calculate(priv, &i2c_t);
+	if (ret < 0)
+		goto out_pm_put;
+
+	rcar_i2c_init(priv);
+
+	/* Don't suspend when multi-master to keep arbitration working */
+	if (of_property_read_bool(dev->of_node, "multi-master"))
+		priv->flags |= ID_P_PM_BLOCKED;
+	else
+		pm_runtime_put(dev);
+
+
+	irq = platform_get_irq(pdev, 0);
+	ret = devm_request_irq(dev, irq, rcar_i2c_irq, 0, dev_name(dev), priv);
 	if (ret < 0) {
 		dev_err(dev, "cannot get irq %d\n", irq);
-		return ret;
+		goto out_pm_disable;
 	}
 
-	pm_runtime_enable(dev);
 	platform_set_drvdata(pdev, priv);
 
 	ret = i2c_add_numbered_adapter(adap);
 	if (ret < 0) {
 		dev_err(dev, "reg adap failed: %d\n", ret);
-		pm_runtime_disable(dev);
-		return ret;
+		goto out_pm_disable;
 	}
 
 	dev_info(dev, "probed\n");
 
 	return 0;
+
+ out_pm_put:
+	pm_runtime_put(dev);
+ out_pm_disable:
+	pm_runtime_disable(dev);
+	return ret;
 }
 
 static int rcar_i2c_remove(struct platform_device *pdev)
@@ -706,6 +673,8 @@ static int rcar_i2c_remove(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 
 	i2c_del_adapter(&priv->adap);
+	if (priv->flags & ID_P_PM_BLOCKED)
+		pm_runtime_put(dev);
 	pm_runtime_disable(dev);
 
 	return 0;
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index 5df819610d52..362a6de54833 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -784,7 +784,6 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 	int retry;
 	int ret;
 
-	pm_runtime_get_sync(&adap->dev);
 	ret = clk_enable(i2c->clk);
 	if (ret)
 		return ret;
@@ -795,7 +794,6 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 
 		if (ret != -EAGAIN) {
 			clk_disable(i2c->clk);
-			pm_runtime_put(&adap->dev);
 			return ret;
 		}
 
@@ -805,7 +803,6 @@ static int s3c24xx_i2c_xfer(struct i2c_adapter *adap,
 	}
 
 	clk_disable(i2c->clk);
-	pm_runtime_put(&adap->dev);
 	return -EREMOTEIO;
 }
 
@@ -1256,8 +1253,6 @@ static int s3c24xx_i2c_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	pm_runtime_enable(&i2c->adap.dev);
-
 	dev_info(&pdev->dev, "%s: S3C I2C adapter\n", dev_name(&i2c->adap.dev));
 	return 0;
 }
@@ -1273,7 +1268,6 @@ static int s3c24xx_i2c_remove(struct platform_device *pdev)
 
 	clk_unprepare(i2c->clk);
 
-	pm_runtime_disable(&i2c->adap.dev);
 	pm_runtime_disable(&pdev->dev);
 
 	s3c24xx_i2c_deregister_cpufreq(i2c);
diff --git a/drivers/i2c/busses/i2c-st.c b/drivers/i2c/busses/i2c-st.c
index 25020ec777c9..6ee77159ac57 100644
--- a/drivers/i2c/busses/i2c-st.c
+++ b/drivers/i2c/busses/i2c-st.c
@@ -708,8 +708,7 @@ static int st_i2c_xfer(struct i2c_adapter *i2c_adap,
 #ifdef CONFIG_PM_SLEEP
 static int st_i2c_suspend(struct device *dev)
 {
-	struct platform_device *pdev =
-		container_of(dev, struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct st_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 
 	if (i2c_dev->busy)
diff --git a/drivers/i2c/busses/i2c-taos-evm.c b/drivers/i2c/busses/i2c-taos-evm.c
index 4c7fc2d47014..210ca82f8aa0 100644
--- a/drivers/i2c/busses/i2c-taos-evm.c
+++ b/drivers/i2c/busses/i2c-taos-evm.c
@@ -130,7 +130,13 @@ static int taos_smbus_xfer(struct i2c_adapter *adapter, u16 addr,
 			return 0;
 	} else {
 		if (p[0] == 'x') {
-			data->byte = simple_strtol(p + 1, NULL, 16);
+			/*
+			 * Voluntarily dropping error code of kstrtou8 since all
+			 * error code that it could return are invalid according
+			 * to Documentation/i2c/fault-codes.
+			 */
+			if (kstrtou8(p + 1, 16, &data->byte))
+				return -EPROTO;
 			return 0;
 		}
 	}
diff --git a/drivers/i2c/busses/i2c-uniphier-f.c b/drivers/i2c/busses/i2c-uniphier-f.c
index e8d03bcfe3e0..213ba55e17c3 100644
--- a/drivers/i2c/busses/i2c-uniphier-f.c
+++ b/drivers/i2c/busses/i2c-uniphier-f.c
@@ -466,6 +466,11 @@ static int uniphier_fi2c_clk_init(struct device *dev,
 	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;
 
@@ -481,6 +486,10 @@ static int uniphier_fi2c_clk_init(struct device *dev,
 		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_fi2c_reset(priv);
 
@@ -531,7 +540,7 @@ static int uniphier_fi2c_probe(struct platform_device *pdev)
 
 	ret = uniphier_fi2c_clk_init(dev, priv);
 	if (ret)
-		return ret;
+		goto err;
 
 	ret = devm_request_irq(dev, irq, uniphier_fi2c_interrupt, 0,
 			       pdev->name, priv);
diff --git a/drivers/i2c/busses/i2c-uniphier.c b/drivers/i2c/busses/i2c-uniphier.c
index e3c3861c3325..89eaa8a7e1e0 100644
--- a/drivers/i2c/busses/i2c-uniphier.c
+++ b/drivers/i2c/busses/i2c-uniphier.c
@@ -327,6 +327,11 @@ static int uniphier_i2c_clk_init(struct device *dev,
 	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;
 
@@ -342,6 +347,10 @@ static int uniphier_i2c_clk_init(struct device *dev,
 		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);
 
@@ -388,7 +397,7 @@ static int uniphier_i2c_probe(struct platform_device *pdev)
 
 	ret = uniphier_i2c_clk_init(dev, priv);
 	if (ret)
-		return ret;
+		goto err;
 
 	ret = devm_request_irq(dev, irq, uniphier_i2c_interrupt, 0, pdev->name,
 			       priv);
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index 0b20449e48cf..6efd20095d5d 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -70,7 +70,7 @@ struct xiic_i2c {
 	wait_queue_head_t	wait;
 	struct i2c_adapter	adap;
 	struct i2c_msg		*tx_msg;
-	spinlock_t		lock;
+	struct mutex		lock;
 	unsigned int		tx_pos;
 	unsigned int		nmsgs;
 	enum xilinx_i2c_state	state;
@@ -369,7 +369,7 @@ static irqreturn_t xiic_process(int irq, void *dev_id)
 	 * To find which interrupts are pending; AND interrupts pending with
 	 * interrupts masked.
 	 */
-	spin_lock(&i2c->lock);
+	mutex_lock(&i2c->lock);
 	isr = xiic_getreg32(i2c, XIIC_IISR_OFFSET);
 	ier = xiic_getreg32(i2c, XIIC_IIER_OFFSET);
 	pend = isr & ier;
@@ -497,7 +497,7 @@ out:
 	dev_dbg(i2c->adap.dev.parent, "%s clr: 0x%x\n", __func__, clr);
 
 	xiic_setreg32(i2c, XIIC_IISR_OFFSET, clr);
-	spin_unlock(&i2c->lock);
+	mutex_unlock(&i2c->lock);
 	return IRQ_HANDLED;
 }
 
@@ -662,10 +662,10 @@ static void __xiic_start_xfer(struct xiic_i2c *i2c)
 
 static void xiic_start_xfer(struct xiic_i2c *i2c)
 {
-	spin_lock(&i2c->lock);
+	mutex_lock(&i2c->lock);
 	xiic_reinit(i2c);
 	__xiic_start_xfer(i2c);
-	spin_unlock(&i2c->lock);
+	mutex_unlock(&i2c->lock);
 }
 
 static int xiic_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
@@ -745,7 +745,7 @@ static int xiic_i2c_probe(struct platform_device *pdev)
 	i2c->adap.dev.parent = &pdev->dev;
 	i2c->adap.dev.of_node = pdev->dev.of_node;
 
-	spin_lock_init(&i2c->lock);
+	mutex_init(&i2c->lock);
 	init_waitqueue_head(&i2c->wait);
 
 	ret = devm_request_threaded_irq(&pdev->dev, irq, xiic_isr,
diff --git a/drivers/i2c/busses/i2c-xlr.c b/drivers/i2c/busses/i2c-xlr.c
index 8b36bcfd952d..613c3a4f2c51 100644
--- a/drivers/i2c/busses/i2c-xlr.c
+++ b/drivers/i2c/busses/i2c-xlr.c
@@ -17,6 +17,10 @@
 #include <linux/i2c.h>
 #include <linux/io.h>
 #include <linux/platform_device.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
+#include <linux/interrupt.h>
+#include <linux/wait.h>
 
 /* XLR I2C REGISTERS */
 #define XLR_I2C_CFG		0x00
@@ -30,6 +34,10 @@
 #define XLR_I2C_BYTECNT		0x08
 #define XLR_I2C_HDSTATIM	0x09
 
+/* Sigma Designs additional registers */
+#define XLR_I2C_INT_EN		0x09
+#define XLR_I2C_INT_STAT	0x0a
+
 /* XLR I2C REGISTERS FLAGS */
 #define XLR_I2C_BUS_BUSY	0x01
 #define XLR_I2C_SDOEMPTY	0x02
@@ -63,11 +71,98 @@ static inline u32 xlr_i2c_rdreg(u32 __iomem *base, unsigned int reg)
 	return __raw_readl(base + reg);
 }
 
+#define XLR_I2C_FLAG_IRQ	1
+
+struct xlr_i2c_config {
+	u32 flags;		/* optional feature support */
+	u32 status_busy;	/* value of STATUS[0] when busy */
+	u32 cfg_extra;		/* extra CFG bits to set */
+};
+
 struct xlr_i2c_private {
 	struct i2c_adapter adap;
 	u32 __iomem *iobase;
+	int irq;
+	int pos;
+	struct i2c_msg *msg;
+	const struct xlr_i2c_config *cfg;
+	wait_queue_head_t wait;
+	struct clk *clk;
 };
 
+static int xlr_i2c_busy(struct xlr_i2c_private *priv, u32 status)
+{
+	return (status & XLR_I2C_BUS_BUSY) == priv->cfg->status_busy;
+}
+
+static int xlr_i2c_idle(struct xlr_i2c_private *priv)
+{
+	return !xlr_i2c_busy(priv, xlr_i2c_rdreg(priv->iobase, XLR_I2C_STATUS));
+}
+
+static int xlr_i2c_wait(struct xlr_i2c_private *priv, unsigned long timeout)
+{
+	int status;
+	int t;
+
+	t = wait_event_timeout(priv->wait, xlr_i2c_idle(priv),
+				msecs_to_jiffies(timeout));
+	if (!t)
+		return -ETIMEDOUT;
+
+	status = xlr_i2c_rdreg(priv->iobase, XLR_I2C_STATUS);
+
+	return status & XLR_I2C_ACK_ERR ? -EIO : 0;
+}
+
+static void xlr_i2c_tx_irq(struct xlr_i2c_private *priv, u32 status)
+{
+	struct i2c_msg *msg = priv->msg;
+
+	if (status & XLR_I2C_SDOEMPTY)
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_DATAOUT,
+				msg->buf[priv->pos++]);
+}
+
+static void xlr_i2c_rx_irq(struct xlr_i2c_private *priv, u32 status)
+{
+	struct i2c_msg *msg = priv->msg;
+
+	if (status & XLR_I2C_RXRDY)
+		msg->buf[priv->pos++] =
+			xlr_i2c_rdreg(priv->iobase, XLR_I2C_DATAIN);
+}
+
+static irqreturn_t xlr_i2c_irq(int irq, void *dev_id)
+{
+	struct xlr_i2c_private *priv = dev_id;
+	struct i2c_msg *msg = priv->msg;
+	u32 int_stat, status;
+
+	int_stat = xlr_i2c_rdreg(priv->iobase, XLR_I2C_INT_STAT);
+	if (!int_stat)
+		return IRQ_NONE;
+
+	xlr_i2c_wreg(priv->iobase, XLR_I2C_INT_STAT, int_stat);
+
+	if (!msg)
+		return IRQ_HANDLED;
+
+	status = xlr_i2c_rdreg(priv->iobase, XLR_I2C_STATUS);
+
+	if (priv->pos < msg->len) {
+		if (msg->flags & I2C_M_RD)
+			xlr_i2c_rx_irq(priv, status);
+		else
+			xlr_i2c_tx_irq(priv, status);
+	}
+
+	if (!xlr_i2c_busy(priv, status))
+		wake_up(&priv->wait);
+
+	return IRQ_HANDLED;
+}
+
 static int xlr_i2c_tx(struct xlr_i2c_private *priv,  u16 len,
 	u8 *buf, u16 addr)
 {
@@ -75,37 +170,48 @@ static int xlr_i2c_tx(struct xlr_i2c_private *priv,  u16 len,
 	unsigned long timeout, stoptime, checktime;
 	u32 i2c_status;
 	int pos, timedout;
-	u8 offset, byte;
+	u8 offset;
+	u32 xfer;
+
+	if (!len)
+		return -EOPNOTSUPP;
 
 	offset = buf[0];
 	xlr_i2c_wreg(priv->iobase, XLR_I2C_ADDR, offset);
 	xlr_i2c_wreg(priv->iobase, XLR_I2C_DEVADDR, addr);
-	xlr_i2c_wreg(priv->iobase, XLR_I2C_CFG, XLR_I2C_CFG_ADDR);
-	xlr_i2c_wreg(priv->iobase, XLR_I2C_BYTECNT, len - 1);
+	xlr_i2c_wreg(priv->iobase, XLR_I2C_CFG,
+			XLR_I2C_CFG_ADDR | priv->cfg->cfg_extra);
 
 	timeout = msecs_to_jiffies(XLR_I2C_TIMEOUT);
 	stoptime = jiffies + timeout;
 	timedout = 0;
-	pos = 1;
-retry:
+
 	if (len == 1) {
-		xlr_i2c_wreg(priv->iobase, XLR_I2C_STARTXFR,
-				XLR_I2C_STARTXFR_ND);
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_BYTECNT, len - 1);
+		xfer = XLR_I2C_STARTXFR_ND;
+		pos = 1;
 	} else {
-		xlr_i2c_wreg(priv->iobase, XLR_I2C_DATAOUT, buf[pos]);
-		xlr_i2c_wreg(priv->iobase, XLR_I2C_STARTXFR,
-				XLR_I2C_STARTXFR_WR);
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_BYTECNT, len - 2);
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_DATAOUT, buf[1]);
+		xfer = XLR_I2C_STARTXFR_WR;
+		pos = 2;
 	}
 
+	priv->pos = pos;
+
+retry:
+	/* retry can only happen on the first byte */
+	xlr_i2c_wreg(priv->iobase, XLR_I2C_STARTXFR, xfer);
+
+	if (priv->irq > 0)
+		return xlr_i2c_wait(priv, XLR_I2C_TIMEOUT * len);
+
 	while (!timedout) {
 		checktime = jiffies;
 		i2c_status = xlr_i2c_rdreg(priv->iobase, XLR_I2C_STATUS);
 
-		if (i2c_status & XLR_I2C_SDOEMPTY) {
-			pos++;
-			/* need to do a empty dataout after the last byte */
-			byte = (pos < len) ? buf[pos] : 0;
-			xlr_i2c_wreg(priv->iobase, XLR_I2C_DATAOUT, byte);
+		if ((i2c_status & XLR_I2C_SDOEMPTY) && pos < len) {
+			xlr_i2c_wreg(priv->iobase, XLR_I2C_DATAOUT, buf[pos++]);
 
 			/* reset timeout on successful xmit */
 			stoptime = jiffies + timeout;
@@ -121,7 +227,7 @@ retry:
 		if (i2c_status & XLR_I2C_ACK_ERR)
 			return -EIO;
 
-		if ((i2c_status & XLR_I2C_BUS_BUSY) == 0 && pos >= len)
+		if (!xlr_i2c_busy(priv, i2c_status) && pos >= len)
 			return 0;
 	}
 	dev_err(&adap->dev, "I2C transmit timeout\n");
@@ -134,12 +240,17 @@ static int xlr_i2c_rx(struct xlr_i2c_private *priv, u16 len, u8 *buf, u16 addr)
 	u32 i2c_status;
 	unsigned long timeout, stoptime, checktime;
 	int nbytes, timedout;
-	u8 byte;
 
-	xlr_i2c_wreg(priv->iobase, XLR_I2C_CFG, XLR_I2C_CFG_NOADDR);
-	xlr_i2c_wreg(priv->iobase, XLR_I2C_BYTECNT, len);
+	if (!len)
+		return -EOPNOTSUPP;
+
+	xlr_i2c_wreg(priv->iobase, XLR_I2C_CFG,
+			XLR_I2C_CFG_NOADDR | priv->cfg->cfg_extra);
+	xlr_i2c_wreg(priv->iobase, XLR_I2C_BYTECNT, len - 1);
 	xlr_i2c_wreg(priv->iobase, XLR_I2C_DEVADDR, addr);
 
+	priv->pos = 0;
+
 	timeout = msecs_to_jiffies(XLR_I2C_TIMEOUT);
 	stoptime = jiffies + timeout;
 	timedout = 0;
@@ -147,18 +258,18 @@ static int xlr_i2c_rx(struct xlr_i2c_private *priv, u16 len, u8 *buf, u16 addr)
 retry:
 	xlr_i2c_wreg(priv->iobase, XLR_I2C_STARTXFR, XLR_I2C_STARTXFR_RD);
 
+	if (priv->irq > 0)
+		return xlr_i2c_wait(priv, XLR_I2C_TIMEOUT * len);
+
 	while (!timedout) {
 		checktime = jiffies;
 		i2c_status = xlr_i2c_rdreg(priv->iobase, XLR_I2C_STATUS);
 		if (i2c_status & XLR_I2C_RXRDY) {
-			if (nbytes > len)
+			if (nbytes >= len)
 				return -EIO;	/* should not happen */
 
-			/* we need to do a dummy datain when nbytes == len */
-			byte = xlr_i2c_rdreg(priv->iobase, XLR_I2C_DATAIN);
-			if (nbytes < len)
-				buf[nbytes] = byte;
-			nbytes++;
+			buf[nbytes++] =
+				xlr_i2c_rdreg(priv->iobase, XLR_I2C_DATAIN);
 
 			/* reset timeout on successful read */
 			stoptime = jiffies + timeout;
@@ -174,7 +285,7 @@ retry:
 		if (i2c_status & XLR_I2C_ACK_ERR)
 			return -EIO;
 
-		if ((i2c_status & XLR_I2C_BUS_BUSY) == 0)
+		if (!xlr_i2c_busy(priv, i2c_status))
 			return 0;
 	}
 
@@ -190,8 +301,17 @@ static int xlr_i2c_xfer(struct i2c_adapter *adap,
 	int ret = 0;
 	struct xlr_i2c_private *priv = i2c_get_adapdata(adap);
 
+	ret = clk_enable(priv->clk);
+	if (ret)
+		return ret;
+
+	if (priv->irq)
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_INT_EN, 0xf);
+
+
 	for (i = 0; ret == 0 && i < num; i++) {
 		msg = &msgs[i];
+		priv->msg = msg;
 		if (msg->flags & I2C_M_RD)
 			ret = xlr_i2c_rx(priv, msg->len, &msg->buf[0],
 					msg->addr);
@@ -200,13 +320,19 @@ static int xlr_i2c_xfer(struct i2c_adapter *adap,
 					msg->addr);
 	}
 
+	if (priv->irq)
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_INT_EN, 0);
+
+	clk_disable(priv->clk);
+	priv->msg = NULL;
+
 	return (ret != 0) ? ret : num;
 }
 
 static u32 xlr_func(struct i2c_adapter *adap)
 {
 	/* Emulate SMBUS over I2C */
-	return I2C_FUNC_SMBUS_EMUL | I2C_FUNC_I2C;
+	return (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK) | I2C_FUNC_I2C;
 }
 
 static struct i2c_algorithm xlr_i2c_algo = {
@@ -214,22 +340,89 @@ static struct i2c_algorithm xlr_i2c_algo = {
 	.functionality	= xlr_func,
 };
 
+static const struct xlr_i2c_config xlr_i2c_config_default = {
+	.status_busy	= XLR_I2C_BUS_BUSY,
+	.cfg_extra	= 0,
+};
+
+static const struct xlr_i2c_config xlr_i2c_config_tangox = {
+	.flags		= XLR_I2C_FLAG_IRQ,
+	.status_busy	= 0,
+	.cfg_extra	= 1 << 8,
+};
+
+static const struct of_device_id xlr_i2c_dt_ids[] = {
+	{
+		.compatible	= "sigma,smp8642-i2c",
+		.data		= &xlr_i2c_config_tangox,
+	},
+	{ }
+};
+
 static int xlr_i2c_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *match;
 	struct xlr_i2c_private  *priv;
 	struct resource *res;
+	struct clk *clk;
+	unsigned long clk_rate;
+	unsigned long clk_div;
+	u32 busfreq;
+	int irq;
 	int ret;
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
+	match = of_match_device(xlr_i2c_dt_ids, &pdev->dev);
+	if (match)
+		priv->cfg = match->data;
+	else
+		priv->cfg = &xlr_i2c_config_default;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->iobase = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(priv->iobase))
 		return PTR_ERR(priv->iobase);
 
+	irq = platform_get_irq(pdev, 0);
+
+	if (irq > 0 && (priv->cfg->flags & XLR_I2C_FLAG_IRQ)) {
+		priv->irq = irq;
+
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_INT_EN, 0);
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_INT_STAT, 0xf);
+
+		ret = devm_request_irq(&pdev->dev, priv->irq, xlr_i2c_irq,
+					IRQF_SHARED, dev_name(&pdev->dev),
+					priv);
+		if (ret)
+			return ret;
+
+		init_waitqueue_head(&priv->wait);
+	}
+
+	if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				 &busfreq))
+		busfreq = 100000;
+
+	clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(clk)) {
+		ret = clk_prepare_enable(clk);
+		if (ret)
+			return ret;
+
+		clk_rate = clk_get_rate(clk);
+		clk_div = DIV_ROUND_UP(clk_rate, 2 * busfreq);
+		xlr_i2c_wreg(priv->iobase, XLR_I2C_CLKDIV, clk_div);
+
+		clk_disable(clk);
+		priv->clk = clk;
+	}
+
 	priv->adap.dev.parent = &pdev->dev;
+	priv->adap.dev.of_node	= pdev->dev.of_node;
 	priv->adap.owner	= THIS_MODULE;
 	priv->adap.algo_data	= priv;
 	priv->adap.algo		= &xlr_i2c_algo;
@@ -255,6 +448,8 @@ static int xlr_i2c_remove(struct platform_device *pdev)
 
 	priv = platform_get_drvdata(pdev);
 	i2c_del_adapter(&priv->adap);
+	clk_unprepare(priv->clk);
+
 	return 0;
 }
 
@@ -263,6 +458,7 @@ static struct platform_driver xlr_i2c_driver = {
 	.remove = xlr_i2c_remove,
 	.driver = {
 		.name   = "xlr-i2cbus",
+		.of_match_table	= xlr_i2c_dt_ids,
 	},
 };
 
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index ba8eb087f224..ffe715d346d8 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -53,6 +53,7 @@
 #include <linux/jump_label.h>
 #include <asm/uaccess.h>
 #include <linux/err.h>
+#include <linux/property.h>
 
 #include "i2c-core.h"
 
@@ -1563,6 +1564,7 @@ static int i2c_register_adapter(struct i2c_adapter *adap)
 	dev_dbg(&adap->dev, "adapter [%s] registered\n", adap->name);
 
 	pm_runtime_no_callbacks(&adap->dev);
+	pm_runtime_enable(&adap->dev);
 
 #ifdef CONFIG_I2C_COMPAT
 	res = class_compat_create_link(i2c_adapter_compat_class, &adap->dev,
@@ -1817,6 +1819,8 @@ void i2c_del_adapter(struct i2c_adapter *adap)
 	/* device name is gone after device_unregister */
 	dev_dbg(&adap->dev, "adapter [%s] unregistered\n", adap->name);
 
+	pm_runtime_disable(&adap->dev);
+
 	/* wait until all references to the device are gone
 	 *
 	 * FIXME: This is old code and should ideally be replaced by an
@@ -1839,6 +1843,58 @@ void i2c_del_adapter(struct i2c_adapter *adap)
 }
 EXPORT_SYMBOL(i2c_del_adapter);
 
+/**
+ * i2c_parse_fw_timings - get I2C related timing parameters from firmware
+ * @dev: The device to scan for I2C timing properties
+ * @t: the i2c_timings struct to be filled with values
+ * @use_defaults: bool to use sane defaults derived from the I2C specification
+ *		  when properties are not found, otherwise use 0
+ *
+ * Scan the device for the generic I2C properties describing timing parameters
+ * for the signal and fill the given struct with the results. If a property was
+ * not found and use_defaults was true, then maximum timings are assumed which
+ * are derived from the I2C specification. If use_defaults is not used, the
+ * results will be 0, so drivers can apply their own defaults later. The latter
+ * is mainly intended for avoiding regressions of existing drivers which want
+ * to switch to this function. New drivers almost always should use the defaults.
+ */
+
+void i2c_parse_fw_timings(struct device *dev, struct i2c_timings *t, bool use_defaults)
+{
+	int ret;
+
+	memset(t, 0, sizeof(*t));
+
+	ret = device_property_read_u32(dev, "clock-frequency", &t->bus_freq_hz);
+	if (ret && use_defaults)
+		t->bus_freq_hz = 100000;
+
+	ret = device_property_read_u32(dev, "i2c-scl-rising-time-ns", &t->scl_rise_ns);
+	if (ret && use_defaults) {
+		if (t->bus_freq_hz <= 100000)
+			t->scl_rise_ns = 1000;
+		else if (t->bus_freq_hz <= 400000)
+			t->scl_rise_ns = 300;
+		else
+			t->scl_rise_ns = 120;
+	}
+
+	ret = device_property_read_u32(dev, "i2c-scl-falling-time-ns", &t->scl_fall_ns);
+	if (ret && use_defaults) {
+		if (t->bus_freq_hz <= 400000)
+			t->scl_fall_ns = 300;
+		else
+			t->scl_fall_ns = 120;
+	}
+
+	device_property_read_u32(dev, "i2c-scl-internal-delay-ns", &t->scl_int_delay_ns);
+
+	ret = device_property_read_u32(dev, "i2c-sda-falling-time-ns", &t->sda_fall_ns);
+	if (ret && use_defaults)
+		t->sda_fall_ns = t->scl_fall_ns;
+}
+EXPORT_SYMBOL_GPL(i2c_parse_fw_timings);
+
 /* ------------------------------------------------------------------------- */
 
 int i2c_for_each_dev(void *data, int (*fn)(struct device *, void *))
diff --git a/drivers/ide/ide-scan-pci.c b/drivers/ide/ide-scan-pci.c
index c3da53e7bb2b..86aa88aeb3a6 100644
--- a/drivers/ide/ide-scan-pci.c
+++ b/drivers/ide/ide-scan-pci.c
@@ -107,5 +107,4 @@ static int __init ide_scan_pcibus(void)
 
 	return 0;
 }
-
-module_init(ide_scan_pcibus);
+device_initcall(ide_scan_pcibus);
diff --git a/drivers/ide/ide.c b/drivers/ide/ide.c
index f086ef387475..d127ace6aa57 100644
--- a/drivers/ide/ide.c
+++ b/drivers/ide/ide.c
@@ -178,17 +178,17 @@ MODULE_PARM_DESC(pci_clock, "PCI bus clock frequency (in MHz)");
 
 static int ide_set_dev_param_mask(const char *s, const struct kernel_param *kp)
 {
-	int a, b, i, j = 1;
+	unsigned int a, b, i, j = 1;
 	unsigned int *dev_param_mask = (unsigned int *)kp->arg;
 
 	/* controller . device (0 or 1) [ : 1 (set) | 0 (clear) ] */
-	if (sscanf(s, "%d.%d:%d", &a, &b, &j) != 3 &&
-	    sscanf(s, "%d.%d", &a, &b) != 2)
+	if (sscanf(s, "%u.%u:%u", &a, &b, &j) != 3 &&
+	    sscanf(s, "%u.%u", &a, &b) != 2)
 		return -EINVAL;
 
 	i = a * MAX_DRIVES + b;
 
-	if (i >= MAX_HWIFS * MAX_DRIVES || j < 0 || j > 1)
+	if (i >= MAX_HWIFS * MAX_DRIVES || j > 1)
 		return -EINVAL;
 
 	if (j)
@@ -246,17 +246,17 @@ static struct chs_geom ide_disks_chs[MAX_HWIFS * MAX_DRIVES];
 
 static int ide_set_disk_chs(const char *str, struct kernel_param *kp)
 {
-	int a, b, c = 0, h = 0, s = 0, i, j = 1;
+	unsigned int a, b, c = 0, h = 0, s = 0, i, j = 1;
 
 	/* controller . device (0 or 1) : Cylinders , Heads , Sectors */
 	/* controller . device (0 or 1) : 1 (use CHS) | 0 (ignore CHS) */
-	if (sscanf(str, "%d.%d:%d,%d,%d", &a, &b, &c, &h, &s) != 5 &&
-	    sscanf(str, "%d.%d:%d", &a, &b, &j) != 3)
+	if (sscanf(str, "%u.%u:%u,%u,%u", &a, &b, &c, &h, &s) != 5 &&
+	    sscanf(str, "%u.%u:%u", &a, &b, &j) != 3)
 		return -EINVAL;
 
 	i = a * MAX_DRIVES + b;
 
-	if (i >= MAX_HWIFS * MAX_DRIVES || j < 0 || j > 1)
+	if (i >= MAX_HWIFS * MAX_DRIVES || j > 1)
 		return -EINVAL;
 
 	if (c > INT_MAX || h > 255 || s > 255)
diff --git a/drivers/ide/it821x.c b/drivers/ide/it821x.c
index f01ba4606be0..04029d18a696 100644
--- a/drivers/ide/it821x.c
+++ b/drivers/ide/it821x.c
@@ -508,7 +508,7 @@ static void it821x_quirkproc(ide_drive_t *drive)
 
 }
 
-static struct ide_dma_ops it821x_pass_through_dma_ops = {
+static const struct ide_dma_ops it821x_pass_through_dma_ops = {
 	.dma_host_set		= ide_dma_host_set,
 	.dma_setup		= ide_dma_setup,
 	.dma_start		= it821x_dma_start,
diff --git a/drivers/ide/trm290.c b/drivers/ide/trm290.c
index 0069f6ce74cf..d550b379b0f1 100644
--- a/drivers/ide/trm290.c
+++ b/drivers/ide/trm290.c
@@ -314,7 +314,7 @@ static const struct ide_tp_ops trm290_tp_ops = {
 	.output_data		= ide_output_data,
 };
 
-static struct ide_dma_ops trm290_dma_ops = {
+static const struct ide_dma_ops trm290_dma_ops = {
 	.dma_host_set		= trm290_dma_host_set,
 	.dma_setup 		= trm290_dma_setup,
 	.dma_start 		= trm290_dma_start,
diff --git a/drivers/iio/Kconfig b/drivers/iio/Kconfig
index 66792e707d74..505e921f0b19 100644
--- a/drivers/iio/Kconfig
+++ b/drivers/iio/Kconfig
@@ -22,6 +22,14 @@ if IIO_BUFFER
 	source "drivers/iio/buffer/Kconfig"
 endif # IIO_BUFFER
 
+config IIO_CONFIGFS
+	tristate "Enable IIO configuration via configfs"
+	select CONFIGFS_FS
+	help
+	  This allows configuring various IIO bits through configfs
+	  (e.g. software triggers). For more info see
+	  Documentation/iio/iio_configfs.txt.
+
 config IIO_TRIGGER
 	bool "Enable triggered sampling support"
 	help
@@ -38,6 +46,14 @@ config IIO_CONSUMERS_PER_TRIGGER
 	This value controls the maximum number of consumers that a
 	given trigger may handle. Default is 2.
 
+config IIO_SW_TRIGGER
+	tristate "Enable software triggers support"
+	select IIO_CONFIGFS
+	help
+	 Provides IIO core support for software triggers. A software
+	 trigger can be created via configfs or directly by a driver
+	 using the API provided.
+
 config IIO_TRIGGERED_EVENT
 	tristate
 	select IIO_TRIGGER
@@ -50,8 +66,10 @@ source "drivers/iio/amplifiers/Kconfig"
 source "drivers/iio/chemical/Kconfig"
 source "drivers/iio/common/Kconfig"
 source "drivers/iio/dac/Kconfig"
+source "drivers/iio/dummy/Kconfig"
 source "drivers/iio/frequency/Kconfig"
 source "drivers/iio/gyro/Kconfig"
+source "drivers/iio/health/Kconfig"
 source "drivers/iio/humidity/Kconfig"
 source "drivers/iio/imu/Kconfig"
 source "drivers/iio/light/Kconfig"
diff --git a/drivers/iio/Makefile b/drivers/iio/Makefile
index aeca7269fe44..20f649073462 100644
--- a/drivers/iio/Makefile
+++ b/drivers/iio/Makefile
@@ -7,6 +7,8 @@ industrialio-y := industrialio-core.o industrialio-event.o inkern.o
 industrialio-$(CONFIG_IIO_BUFFER) += industrialio-buffer.o
 industrialio-$(CONFIG_IIO_TRIGGER) += industrialio-trigger.o
 
+obj-$(CONFIG_IIO_CONFIGFS) += industrialio-configfs.o
+obj-$(CONFIG_IIO_SW_TRIGGER) += industrialio-sw-trigger.o
 obj-$(CONFIG_IIO_TRIGGERED_EVENT) += industrialio-triggered-event.o
 
 obj-y += accel/
@@ -16,8 +18,10 @@ obj-y += buffer/
 obj-y += chemical/
 obj-y += common/
 obj-y += dac/
+obj-y += dummy/
 obj-y += gyro/
 obj-y += frequency/
+obj-y += health/
 obj-y += humidity/
 obj-y += imu/
 obj-y += light/
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 969428dd6329..833ea9dd4464 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -64,7 +64,7 @@ config IIO_ST_ACCEL_3AXIS
 	help
 	  Say yes here to build support for STMicroelectronics accelerometers:
 	  LSM303DLH, LSM303DLHC, LIS3DH, LSM330D, LSM330DL, LSM330DLC,
-	  LIS331DLH, LSM303DL, LSM303DLM, LSM330.
+	  LIS331DLH, LSM303DL, LSM303DLM, LSM330, LIS2DH12.
 
 	  This driver can also be built as a module. If so, these modules
 	  will be created:
@@ -107,6 +107,35 @@ config KXCJK1013
 	  To compile this driver as a module, choose M here: the module will
 	  be called kxcjk-1013.
 
+config MMA7455
+	tristate
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+
+config MMA7455_I2C
+	tristate "Freescale MMA7455L/MMA7456L Accelerometer I2C Driver"
+	depends on I2C
+	select MMA7455
+	select REGMAP_I2C
+	help
+	  Say yes here to build support for the Freescale MMA7455L and
+	  MMA7456L 3-axis accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mma7455_i2c.
+
+config MMA7455_SPI
+	tristate "Freescale MMA7455L/MMA7456L Accelerometer SPI Driver"
+	depends on SPI_MASTER
+	select MMA7455
+	select REGMAP_SPI
+	help
+	  Say yes here to build support for the Freescale MMA7455L and
+	  MMA7456L 3-axis accelerometer.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mma7455_spi.
+
 config MMA8452
 	tristate "Freescale MMA8452Q and similar Accelerometers Driver"
 	depends on I2C
@@ -158,6 +187,17 @@ config MXC4005
 	  To compile this driver as a module, choose M. The module will be
 	  called mxc4005.
 
+config MXC6255
+	tristate "Memsic MXC6255 Orientation Sensing Accelerometer Driver"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  Say yes here to build support for the Memsic MXC6255 Orientation
+	  Sensing Accelerometer Driver.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called mxc6255.
+
 config STK8312
 	tristate "Sensortek STK8312 3-Axis Accelerometer Driver"
 	depends on I2C
@@ -173,6 +213,7 @@ config STK8312
 config STK8BA50
 	tristate "Sensortek STK8BA50 3-Axis Accelerometer Driver"
 	depends on I2C
+	depends on IIO_TRIGGER
 	help
 	  Say yes here to get support for the Sensortek STK8BA50 3-axis
 	  accelerometer.
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 7925f166e6e9..71b6794de885 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -10,6 +10,11 @@ obj-$(CONFIG_BMC150_ACCEL_SPI) += bmc150-accel-spi.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_MMA7455)		+= mma7455_core.o
+obj-$(CONFIG_MMA7455_I2C)	+= mma7455_i2c.o
+obj-$(CONFIG_MMA7455_SPI)	+= mma7455_spi.o
+
 obj-$(CONFIG_MMA8452)	+= mma8452.o
 
 obj-$(CONFIG_MMA9551_CORE)	+= mma9551_core.o
@@ -17,6 +22,7 @@ obj-$(CONFIG_MMA9551)		+= mma9551.o
 obj-$(CONFIG_MMA9553)		+= mma9553.o
 
 obj-$(CONFIG_MXC4005)		+= mxc4005.o
+obj-$(CONFIG_MXC6255)		+= mxc6255.o
 
 obj-$(CONFIG_STK8312)		+= stk8312.o
 obj-$(CONFIG_STK8BA50)		+= stk8ba50.o
diff --git a/drivers/iio/accel/bmc150-accel-core.c b/drivers/iio/accel/bmc150-accel-core.c
index 2d33f1e821db..c73331f7782b 100644
--- a/drivers/iio/accel/bmc150-accel-core.c
+++ b/drivers/iio/accel/bmc150-accel-core.c
@@ -1623,24 +1623,22 @@ int bmc150_accel_core_probe(struct device *dev, struct regmap *regmap, int irq,
 		}
 	}
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(dev, "Unable to register iio device\n");
-		goto err_trigger_unregister;
-	}
-
 	ret = pm_runtime_set_active(dev);
 	if (ret)
-		goto err_iio_unregister;
+		goto err_trigger_unregister;
 
 	pm_runtime_enable(dev);
 	pm_runtime_set_autosuspend_delay(dev, BMC150_AUTO_SUSPEND_DELAY_MS);
 	pm_runtime_use_autosuspend(dev);
 
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(dev, "Unable to register iio device\n");
+		goto err_trigger_unregister;
+	}
+
 	return 0;
 
-err_iio_unregister:
-	iio_device_unregister(indio_dev);
 err_trigger_unregister:
 	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
 err_buffer_cleanup:
@@ -1655,12 +1653,12 @@ int bmc150_accel_core_remove(struct device *dev)
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct bmc150_accel_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(data->dev);
 	pm_runtime_set_suspended(data->dev);
 	pm_runtime_put_noidle(data->dev);
 
-	iio_device_unregister(indio_dev);
-
 	bmc150_accel_unregister_triggers(data, BMC150_ACCEL_TRIGGERS - 1);
 
 	iio_triggered_buffer_cleanup(indio_dev);
diff --git a/drivers/iio/accel/kxcjk-1013.c b/drivers/iio/accel/kxcjk-1013.c
index 18c1b06684c1..edec1d099e91 100644
--- a/drivers/iio/accel/kxcjk-1013.c
+++ b/drivers/iio/accel/kxcjk-1013.c
@@ -1264,25 +1264,23 @@ static int kxcjk1013_probe(struct i2c_client *client,
 		goto err_trigger_unregister;
 	}
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(&client->dev, "unable to register iio device\n");
-		goto err_buffer_cleanup;
-	}
-
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret)
-		goto err_iio_unregister;
+		goto err_buffer_cleanup;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev,
 					 KXCJK1013_SLEEP_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto err_buffer_cleanup;
+	}
+
 	return 0;
 
-err_iio_unregister:
-	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
 	if (data->dready_trig)
 		iio_triggered_buffer_cleanup(indio_dev);
@@ -1302,12 +1300,12 @@ static int kxcjk1013_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(indio_dev);
-
 	if (data->dready_trig) {
 		iio_triggered_buffer_cleanup(indio_dev);
 		iio_trigger_unregister(data->dready_trig);
diff --git a/drivers/iio/accel/mma7455.h b/drivers/iio/accel/mma7455.h
new file mode 100644
index 000000000000..2b1152c53d4f
--- /dev/null
+++ b/drivers/iio/accel/mma7455.h
@@ -0,0 +1,19 @@
+/*
+ * IIO accel driver for Freescale MMA7455L 3-axis 10-bit accelerometer
+ * Copyright 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __MMA7455_H
+#define __MMA7455_H
+
+extern const struct regmap_config mma7455_core_regmap;
+
+int mma7455_core_probe(struct device *dev, struct regmap *regmap,
+		       const char *name);
+int mma7455_core_remove(struct device *dev);
+
+#endif
diff --git a/drivers/iio/accel/mma7455_core.c b/drivers/iio/accel/mma7455_core.c
new file mode 100644
index 000000000000..c633cc2c0789
--- /dev/null
+++ b/drivers/iio/accel/mma7455_core.c
@@ -0,0 +1,311 @@
+/*
+ * IIO accel core driver for Freescale MMA7455L 3-axis 10-bit accelerometer
+ * Copyright 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * UNSUPPORTED hardware features:
+ *  - 8-bit mode with different scales
+ *  - INT1/INT2 interrupts
+ *  - Offset calibration
+ *  - Events
+ */
+
+#include <linux/delay.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 <linux/module.h>
+#include <linux/regmap.h>
+
+#include "mma7455.h"
+
+#define MMA7455_REG_XOUTL		0x00
+#define MMA7455_REG_XOUTH		0x01
+#define MMA7455_REG_YOUTL		0x02
+#define MMA7455_REG_YOUTH		0x03
+#define MMA7455_REG_ZOUTL		0x04
+#define MMA7455_REG_ZOUTH		0x05
+#define MMA7455_REG_STATUS		0x09
+#define  MMA7455_STATUS_DRDY		BIT(0)
+#define MMA7455_REG_WHOAMI		0x0f
+#define  MMA7455_WHOAMI_ID		0x55
+#define MMA7455_REG_MCTL		0x16
+#define  MMA7455_MCTL_MODE_STANDBY	0x00
+#define  MMA7455_MCTL_MODE_MEASURE	0x01
+#define MMA7455_REG_CTL1		0x18
+#define  MMA7455_CTL1_DFBW_MASK		BIT(7)
+#define  MMA7455_CTL1_DFBW_125HZ	BIT(7)
+#define  MMA7455_CTL1_DFBW_62_5HZ	0
+#define MMA7455_REG_TW			0x1e
+
+/*
+ * When MMA7455 is used in 10-bit it has a fullscale of -8g
+ * corresponding to raw value -512. The userspace interface
+ * uses m/s^2 and we declare micro units.
+ * So scale factor is given by:
+ *       g * 8 * 1e6 / 512 = 153228.90625, with g = 9.80665
+ */
+#define MMA7455_10BIT_SCALE	153229
+
+struct mma7455_data {
+	struct regmap *regmap;
+	struct device *dev;
+};
+
+static int mma7455_drdy(struct mma7455_data *mma7455)
+{
+	unsigned int reg;
+	int tries = 3;
+	int ret;
+
+	while (tries-- > 0) {
+		ret = regmap_read(mma7455->regmap, MMA7455_REG_STATUS, &reg);
+		if (ret)
+			return ret;
+
+		if (reg & MMA7455_STATUS_DRDY)
+			return 0;
+
+		msleep(20);
+	}
+
+	dev_warn(mma7455->dev, "data not ready\n");
+
+	return -EIO;
+}
+
+static irqreturn_t mma7455_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct mma7455_data *mma7455 = iio_priv(indio_dev);
+	u8 buf[16]; /* 3 x 16-bit channels + padding + ts */
+	int ret;
+
+	ret = mma7455_drdy(mma7455);
+	if (ret)
+		goto done;
+
+	ret = regmap_bulk_read(mma7455->regmap, MMA7455_REG_XOUTL, buf,
+			       sizeof(__le16) * 3);
+	if (ret)
+		goto done;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buf, iio_get_time_ns());
+
+done:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int mma7455_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mma7455_data *mma7455 = iio_priv(indio_dev);
+	unsigned int reg;
+	__le16 data;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		if (iio_buffer_enabled(indio_dev))
+			return -EBUSY;
+
+		ret = mma7455_drdy(mma7455);
+		if (ret)
+			return ret;
+
+		ret = regmap_bulk_read(mma7455->regmap, chan->address, &data,
+				       sizeof(data));
+		if (ret)
+			return ret;
+
+		*val = sign_extend32(le16_to_cpu(data), 9);
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = MMA7455_10BIT_SCALE;
+
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = regmap_read(mma7455->regmap, MMA7455_REG_CTL1, &reg);
+		if (ret)
+			return ret;
+
+		if (reg & MMA7455_CTL1_DFBW_MASK)
+			*val = 250;
+		else
+			*val = 125;
+
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static int mma7455_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct mma7455_data *mma7455 = iio_priv(indio_dev);
+	int i;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		if (val == 250 && val2 == 0)
+			i = MMA7455_CTL1_DFBW_125HZ;
+		else if (val == 125 && val2 == 0)
+			i = MMA7455_CTL1_DFBW_62_5HZ;
+		else
+			return -EINVAL;
+
+		return regmap_update_bits(mma7455->regmap, MMA7455_REG_CTL1,
+					  MMA7455_CTL1_DFBW_MASK, i);
+
+	case IIO_CHAN_INFO_SCALE:
+		/* In 10-bit mode there is only one scale available */
+		if (val == 0 && val2 == MMA7455_10BIT_SCALE)
+			return 0;
+		break;
+	}
+
+	return -EINVAL;
+}
+
+static IIO_CONST_ATTR(sampling_frequency_available, "125 250");
+
+static struct attribute *mma7455_attributes[] = {
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group mma7455_group = {
+	.attrs = mma7455_attributes,
+};
+
+static const struct iio_info mma7455_info = {
+	.attrs = &mma7455_group,
+	.read_raw = mma7455_read_raw,
+	.write_raw = mma7455_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define MMA7455_CHANNEL(axis, idx) { \
+	.type = IIO_ACCEL, \
+	.modified = 1, \
+	.address = MMA7455_REG_##axis##OUTL,\
+	.channel2 = IIO_MOD_##axis, \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+				    BIT(IIO_CHAN_INFO_SCALE), \
+	.scan_index = idx, \
+	.scan_type = { \
+		.sign = 's', \
+		.realbits = 10, \
+		.storagebits = 16, \
+		.endianness = IIO_LE, \
+	}, \
+}
+
+static const struct iio_chan_spec mma7455_channels[] = {
+	MMA7455_CHANNEL(X, 0),
+	MMA7455_CHANNEL(Y, 1),
+	MMA7455_CHANNEL(Z, 2),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const unsigned long mma7455_scan_masks[] = {0x7, 0};
+
+const struct regmap_config mma7455_core_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = MMA7455_REG_TW,
+};
+EXPORT_SYMBOL_GPL(mma7455_core_regmap);
+
+int mma7455_core_probe(struct device *dev, struct regmap *regmap,
+		       const char *name)
+{
+	struct mma7455_data *mma7455;
+	struct iio_dev *indio_dev;
+	unsigned int reg;
+	int ret;
+
+	ret = regmap_read(regmap, MMA7455_REG_WHOAMI, &reg);
+	if (ret) {
+		dev_err(dev, "unable to read reg\n");
+		return ret;
+	}
+
+	if (reg != MMA7455_WHOAMI_ID) {
+		dev_err(dev, "device id mismatch\n");
+		return -ENODEV;
+	}
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*mma7455));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	dev_set_drvdata(dev, indio_dev);
+	mma7455 = iio_priv(indio_dev);
+	mma7455->regmap = regmap;
+	mma7455->dev = dev;
+
+	indio_dev->info = &mma7455_info;
+	indio_dev->name = name;
+	indio_dev->dev.parent = dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = mma7455_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mma7455_channels);
+	indio_dev->available_scan_masks = mma7455_scan_masks;
+
+	regmap_write(mma7455->regmap, MMA7455_REG_MCTL,
+		     MMA7455_MCTL_MODE_MEASURE);
+
+	ret = iio_triggered_buffer_setup(indio_dev, NULL,
+					 mma7455_trigger_handler, NULL);
+	if (ret) {
+		dev_err(dev, "unable to setup triggered buffer\n");
+		return ret;
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		dev_err(dev, "unable to register device\n");
+		iio_triggered_buffer_cleanup(indio_dev);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mma7455_core_probe);
+
+int mma7455_core_remove(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct mma7455_data *mma7455 = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	iio_triggered_buffer_cleanup(indio_dev);
+
+	regmap_write(mma7455->regmap, MMA7455_REG_MCTL,
+		     MMA7455_MCTL_MODE_STANDBY);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mma7455_core_remove);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("Freescale MMA7455L core accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/mma7455_i2c.c b/drivers/iio/accel/mma7455_i2c.c
new file mode 100644
index 000000000000..3cab5fb4a3c4
--- /dev/null
+++ b/drivers/iio/accel/mma7455_i2c.c
@@ -0,0 +1,56 @@
+/*
+ * IIO accel I2C driver for Freescale MMA7455L 3-axis 10-bit accelerometer
+ * Copyright 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+#include "mma7455.h"
+
+static int mma7455_i2c_probe(struct i2c_client *i2c,
+			     const struct i2c_device_id *id)
+{
+	struct regmap *regmap;
+	const char *name = NULL;
+
+	regmap = devm_regmap_init_i2c(i2c, &mma7455_core_regmap);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	if (id)
+		name = id->name;
+
+	return mma7455_core_probe(&i2c->dev, regmap, name);
+}
+
+static int mma7455_i2c_remove(struct i2c_client *i2c)
+{
+	return mma7455_core_remove(&i2c->dev);
+}
+
+static const struct i2c_device_id mma7455_i2c_ids[] = {
+	{ "mma7455", 0 },
+	{ "mma7456", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mma7455_i2c_ids);
+
+static struct i2c_driver mma7455_i2c_driver = {
+	.probe = mma7455_i2c_probe,
+	.remove = mma7455_i2c_remove,
+	.id_table = mma7455_i2c_ids,
+	.driver = {
+		.name	= "mma7455-i2c",
+	},
+};
+module_i2c_driver(mma7455_i2c_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("Freescale MMA7455L I2C accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/mma7455_spi.c b/drivers/iio/accel/mma7455_spi.c
new file mode 100644
index 000000000000..79df8f27cf99
--- /dev/null
+++ b/drivers/iio/accel/mma7455_spi.c
@@ -0,0 +1,52 @@
+/*
+ * IIO accel SPI driver for Freescale MMA7455L 3-axis 10-bit accelerometer
+ * Copyright 2015 Joachim Eastwood <manabian@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/regmap.h>
+#include <linux/spi/spi.h>
+
+#include "mma7455.h"
+
+static int mma7455_spi_probe(struct spi_device *spi)
+{
+	const struct spi_device_id *id = spi_get_device_id(spi);
+	struct regmap *regmap;
+
+	regmap = devm_regmap_init_spi(spi, &mma7455_core_regmap);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return mma7455_core_probe(&spi->dev, regmap, id->name);
+}
+
+static int mma7455_spi_remove(struct spi_device *spi)
+{
+	return mma7455_core_remove(&spi->dev);
+}
+
+static const struct spi_device_id mma7455_spi_ids[] = {
+	{ "mma7455", 0 },
+	{ "mma7456", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, mma7455_spi_ids);
+
+static struct spi_driver mma7455_spi_driver = {
+	.probe = mma7455_spi_probe,
+	.remove = mma7455_spi_remove,
+	.id_table = mma7455_spi_ids,
+	.driver = {
+		.name = "mma7455-spi",
+	},
+};
+module_spi_driver(mma7455_spi_driver);
+
+MODULE_AUTHOR("Joachim Eastwood <manabian@gmail.com>");
+MODULE_DESCRIPTION("Freescale MMA7455L SPI accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/mma8452.c b/drivers/iio/accel/mma8452.c
index 1eccc2dcf14c..ccc632a7cf01 100644
--- a/drivers/iio/accel/mma8452.c
+++ b/drivers/iio/accel/mma8452.c
@@ -29,6 +29,7 @@
 #include <linux/iio/events.h>
 #include <linux/delay.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 
 #define MMA8452_STATUS				0x00
 #define  MMA8452_STATUS_DRDY			(BIT(2) | BIT(1) | BIT(0))
@@ -57,7 +58,6 @@
 #define MMA8452_FF_MT_COUNT			0x18
 #define MMA8452_TRANSIENT_CFG			0x1d
 #define  MMA8452_TRANSIENT_CFG_HPF_BYP		BIT(0)
-#define  MMA8452_TRANSIENT_CFG_CHAN(chan)	BIT(chan + 1)
 #define  MMA8452_TRANSIENT_CFG_ELE		BIT(4)
 #define MMA8452_TRANSIENT_SRC			0x1e
 #define  MMA8452_TRANSIENT_SRC_XTRANSE		BIT(1)
@@ -143,6 +143,13 @@ struct mma_chip_info {
 	u8 ev_count;
 };
 
+enum {
+	idx_x,
+	idx_y,
+	idx_z,
+	idx_ts,
+};
+
 static int mma8452_drdy(struct mma8452_data *data)
 {
 	int tries = 150;
@@ -816,31 +823,31 @@ static struct attribute_group mma8452_event_attribute_group = {
 }
 
 static const struct iio_chan_spec mma8452_channels[] = {
-	MMA8452_CHANNEL(X, 0, 12),
-	MMA8452_CHANNEL(Y, 1, 12),
-	MMA8452_CHANNEL(Z, 2, 12),
-	IIO_CHAN_SOFT_TIMESTAMP(3),
+	MMA8452_CHANNEL(X, idx_x, 12),
+	MMA8452_CHANNEL(Y, idx_y, 12),
+	MMA8452_CHANNEL(Z, idx_z, 12),
+	IIO_CHAN_SOFT_TIMESTAMP(idx_ts),
 };
 
 static const struct iio_chan_spec mma8453_channels[] = {
-	MMA8452_CHANNEL(X, 0, 10),
-	MMA8452_CHANNEL(Y, 1, 10),
-	MMA8452_CHANNEL(Z, 2, 10),
-	IIO_CHAN_SOFT_TIMESTAMP(3),
+	MMA8452_CHANNEL(X, idx_x, 10),
+	MMA8452_CHANNEL(Y, idx_y, 10),
+	MMA8452_CHANNEL(Z, idx_z, 10),
+	IIO_CHAN_SOFT_TIMESTAMP(idx_ts),
 };
 
 static const struct iio_chan_spec mma8652_channels[] = {
-	MMA8652_CHANNEL(X, 0, 12),
-	MMA8652_CHANNEL(Y, 1, 12),
-	MMA8652_CHANNEL(Z, 2, 12),
-	IIO_CHAN_SOFT_TIMESTAMP(3),
+	MMA8652_CHANNEL(X, idx_x, 12),
+	MMA8652_CHANNEL(Y, idx_y, 12),
+	MMA8652_CHANNEL(Z, idx_z, 12),
+	IIO_CHAN_SOFT_TIMESTAMP(idx_ts),
 };
 
 static const struct iio_chan_spec mma8653_channels[] = {
-	MMA8652_CHANNEL(X, 0, 10),
-	MMA8652_CHANNEL(Y, 1, 10),
-	MMA8652_CHANNEL(Z, 2, 10),
-	IIO_CHAN_SOFT_TIMESTAMP(3),
+	MMA8652_CHANNEL(X, idx_x, 10),
+	MMA8652_CHANNEL(Y, idx_y, 10),
+	MMA8652_CHANNEL(Z, idx_z, 10),
+	IIO_CHAN_SOFT_TIMESTAMP(idx_ts),
 };
 
 enum {
@@ -1130,13 +1137,21 @@ static int mma8452_probe(struct i2c_client *client,
 					   MMA8452_INT_FF_MT;
 		int enabled_interrupts = MMA8452_INT_TRANS |
 					 MMA8452_INT_FF_MT;
+		int irq2;
 
-		/* Assume wired to INT1 pin */
-		ret = i2c_smbus_write_byte_data(client,
-						MMA8452_CTRL_REG5,
-						supported_interrupts);
-		if (ret < 0)
-			return ret;
+		irq2 = of_irq_get_byname(client->dev.of_node, "INT2");
+
+		if (irq2 == client->irq) {
+			dev_dbg(&client->dev, "using interrupt line INT2\n");
+		} else {
+			ret = i2c_smbus_write_byte_data(client,
+							MMA8452_CTRL_REG5,
+							supported_interrupts);
+			if (ret < 0)
+				return ret;
+
+			dev_dbg(&client->dev, "using interrupt line INT1\n");
+		}
 
 		ret = i2c_smbus_write_byte_data(client,
 						MMA8452_CTRL_REG4,
diff --git a/drivers/iio/accel/mma9551.c b/drivers/iio/accel/mma9551.c
index 7db7cc0bf362..d899a4d4307f 100644
--- a/drivers/iio/accel/mma9551.c
+++ b/drivers/iio/accel/mma9551.c
@@ -495,25 +495,23 @@ static int mma9551_probe(struct i2c_client *client,
 	if (ret < 0)
 		goto out_poweroff;
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(&client->dev, "unable to register iio device\n");
-		goto out_poweroff;
-	}
-
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret < 0)
-		goto out_iio_unregister;
+		goto out_poweroff;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev,
 					 MMA9551_AUTO_SUSPEND_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto out_poweroff;
+	}
+
 	return 0;
 
-out_iio_unregister:
-	iio_device_unregister(indio_dev);
 out_poweroff:
 	mma9551_set_device_state(client, false);
 
@@ -525,11 +523,12 @@ static int mma9551_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct mma9551_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(indio_dev);
 	mutex_lock(&data->mutex);
 	mma9551_set_device_state(data->client, false);
 	mutex_unlock(&data->mutex);
diff --git a/drivers/iio/accel/mma9553.c b/drivers/iio/accel/mma9553.c
index 9408ef3add58..fa7d36217c4b 100644
--- a/drivers/iio/accel/mma9553.c
+++ b/drivers/iio/accel/mma9553.c
@@ -1133,27 +1133,24 @@ static int mma9553_probe(struct i2c_client *client,
 		}
 	}
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(&client->dev, "unable to register iio device\n");
-		goto out_poweroff;
-	}
-
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret < 0)
-		goto out_iio_unregister;
+		goto out_poweroff;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev,
 					 MMA9551_AUTO_SUSPEND_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
-	dev_dbg(&indio_dev->dev, "Registered device %s\n", name);
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto out_poweroff;
+	}
 
+	dev_dbg(&indio_dev->dev, "Registered device %s\n", name);
 	return 0;
 
-out_iio_unregister:
-	iio_device_unregister(indio_dev);
 out_poweroff:
 	mma9551_set_device_state(client, false);
 	return ret;
@@ -1164,11 +1161,12 @@ static int mma9553_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct mma9553_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(indio_dev);
 	mutex_lock(&data->mutex);
 	mma9551_set_device_state(data->client, false);
 	mutex_unlock(&data->mutex);
diff --git a/drivers/iio/accel/mxc6255.c b/drivers/iio/accel/mxc6255.c
new file mode 100644
index 000000000000..97ccde722e7b
--- /dev/null
+++ b/drivers/iio/accel/mxc6255.c
@@ -0,0 +1,198 @@
+/*
+ * MXC6255 - MEMSIC orientation sensing accelerometer
+ *
+ * Copyright (c) 2015, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for MXC6255 (7-bit I2C slave address 0x15).
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/iio/iio.h>
+#include <linux/delay.h>
+#include <linux/acpi.h>
+#include <linux/regmap.h>
+#include <linux/iio/sysfs.h>
+
+#define MXC6255_DRV_NAME		"mxc6255"
+#define MXC6255_REGMAP_NAME		"mxc6255_regmap"
+
+#define MXC6255_REG_XOUT		0x00
+#define MXC6255_REG_YOUT		0x01
+#define MXC6255_REG_CHIP_ID		0x08
+
+#define MXC6255_CHIP_ID			0x05
+
+/*
+ * MXC6255 has only one measurement range: +/- 2G.
+ * The acceleration output is an 8-bit value.
+ *
+ * Scale is calculated as follows:
+ * (2 + 2) * 9.80665 / (2^8 - 1) = 0.153829
+ *
+ * Scale value for +/- 2G measurement range
+ */
+#define MXC6255_SCALE			153829
+
+enum mxc6255_axis {
+	AXIS_X,
+	AXIS_Y,
+};
+
+struct mxc6255_data {
+	struct i2c_client *client;
+	struct regmap *regmap;
+};
+
+static int mxc6255_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long mask)
+{
+	struct mxc6255_data *data = iio_priv(indio_dev);
+	unsigned int reg;
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = regmap_read(data->regmap, chan->address, &reg);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error reading reg %lu\n", chan->address);
+			return ret;
+		}
+
+		*val = sign_extend32(reg, 7);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = MXC6255_SCALE;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info mxc6255_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= mxc6255_read_raw,
+};
+
+#define MXC6255_CHANNEL(_axis, reg) {				\
+	.type = IIO_ACCEL,					\
+	.modified = 1,						\
+	.channel2 = IIO_MOD_##_axis,				\
+	.address = reg,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+}
+
+static const struct iio_chan_spec mxc6255_channels[] = {
+	MXC6255_CHANNEL(X, MXC6255_REG_XOUT),
+	MXC6255_CHANNEL(Y, MXC6255_REG_YOUT),
+};
+
+static bool mxc6255_is_readable_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MXC6255_REG_XOUT:
+	case MXC6255_REG_YOUT:
+	case MXC6255_REG_CHIP_ID:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config mxc6255_regmap_config = {
+	.name = MXC6255_REGMAP_NAME,
+
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.readable_reg = mxc6255_is_readable_reg,
+};
+
+static int mxc6255_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct mxc6255_data *data;
+	struct iio_dev *indio_dev;
+	struct regmap *regmap;
+	unsigned int chip_id;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	regmap = devm_regmap_init_i2c(client, &mxc6255_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "Error initializing regmap\n");
+		return PTR_ERR(regmap);
+	}
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+	data->regmap = regmap;
+
+	indio_dev->name = MXC6255_DRV_NAME;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->channels = mxc6255_channels;
+	indio_dev->num_channels = ARRAY_SIZE(mxc6255_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &mxc6255_info;
+
+	ret = regmap_read(data->regmap, MXC6255_REG_CHIP_ID, &chip_id);
+	if (ret < 0) {
+		dev_err(&client->dev, "Error reading chip id %d\n", ret);
+		return ret;
+	}
+
+	if (chip_id != MXC6255_CHIP_ID) {
+		dev_err(&client->dev, "Invalid chip id %x\n", chip_id);
+		return -ENODEV;
+	}
+
+	dev_dbg(&client->dev, "Chip id %x\n", chip_id);
+
+	ret = devm_iio_device_register(&client->dev, indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "Could not register IIO device\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static const struct acpi_device_id mxc6255_acpi_match[] = {
+	{"MXC6255",	0},
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, mxc6255_acpi_match);
+
+static const struct i2c_device_id mxc6255_id[] = {
+	{"mxc6255",	0},
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, mxc6255_id);
+
+static struct i2c_driver mxc6255_driver = {
+	.driver = {
+		.name = MXC6255_DRV_NAME,
+		.acpi_match_table = ACPI_PTR(mxc6255_acpi_match),
+	},
+	.probe		= mxc6255_probe,
+	.id_table	= mxc6255_id,
+};
+
+module_i2c_driver(mxc6255_driver);
+
+MODULE_AUTHOR("Teodora Baluta <teodora.baluta@intel.com>");
+MODULE_DESCRIPTION("MEMSIC MXC6255 orientation sensing accelerometer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h
index 468f21fa2950..5d4a1897b293 100644
--- a/drivers/iio/accel/st_accel.h
+++ b/drivers/iio/accel/st_accel.h
@@ -27,6 +27,7 @@
 #define LSM303DLM_ACCEL_DEV_NAME	"lsm303dlm_accel"
 #define LSM330_ACCEL_DEV_NAME		"lsm330_accel"
 #define LSM303AGR_ACCEL_DEV_NAME	"lsm303agr_accel"
+#define LIS2DH12_ACCEL_DEV_NAME		"lis2dh12_accel"
 
 /**
 * struct st_sensors_platform_data - default accel platform data
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 197a08b4e2f3..70f042797f15 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -232,6 +232,7 @@ static const struct st_sensor_settings st_accel_sensors_settings[] = {
 			[3] = LSM330DL_ACCEL_DEV_NAME,
 			[4] = LSM330DLC_ACCEL_DEV_NAME,
 			[5] = LSM303AGR_ACCEL_DEV_NAME,
+			[6] = LIS2DH12_ACCEL_DEV_NAME,
 		},
 		.ch = (struct iio_chan_spec *)st_accel_12bit_channels,
 		.odr = {
diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c
index 8b9cc84fd44f..294a32f89367 100644
--- a/drivers/iio/accel/st_accel_i2c.c
+++ b/drivers/iio/accel/st_accel_i2c.c
@@ -72,6 +72,10 @@ static const struct of_device_id st_accel_of_match[] = {
 		.compatible = "st,lsm303agr-accel",
 		.data = LSM303AGR_ACCEL_DEV_NAME,
 	},
+	{
+		.compatible = "st,lis2dh12-accel",
+		.data = LIS2DH12_ACCEL_DEV_NAME,
+	},
 	{},
 };
 MODULE_DEVICE_TABLE(of, st_accel_of_match);
@@ -121,6 +125,7 @@ static const struct i2c_device_id st_accel_id_table[] = {
 	{ LSM303DLM_ACCEL_DEV_NAME },
 	{ LSM330_ACCEL_DEV_NAME },
 	{ LSM303AGR_ACCEL_DEV_NAME },
+	{ LIS2DH12_ACCEL_DEV_NAME },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, st_accel_id_table);
diff --git a/drivers/iio/accel/st_accel_spi.c b/drivers/iio/accel/st_accel_spi.c
index f71b0d391272..fcd5847a3fd3 100644
--- a/drivers/iio/accel/st_accel_spi.c
+++ b/drivers/iio/accel/st_accel_spi.c
@@ -58,6 +58,7 @@ static const struct spi_device_id st_accel_id_table[] = {
 	{ LSM303DLM_ACCEL_DEV_NAME },
 	{ LSM330_ACCEL_DEV_NAME },
 	{ LSM303AGR_ACCEL_DEV_NAME },
+	{ LIS2DH12_ACCEL_DEV_NAME },
 	{},
 };
 MODULE_DEVICE_TABLE(spi, st_accel_id_table);
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 7868c744fd4b..283ded7747a9 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -175,6 +175,7 @@ config DA9150_GPADC
 config EXYNOS_ADC
 	tristate "Exynos ADC driver support"
 	depends on ARCH_EXYNOS || ARCH_S3C24XX || ARCH_S3C64XX || (OF && COMPILE_TEST)
+	depends on HAS_IOMEM
 	help
 	  Core support for the ADC block found in the Samsung EXYNOS series
 	  of SoCs for drivers such as the touchscreen and hwmon to use to share
@@ -194,6 +195,26 @@ config HI8435
 	  This driver can also be built as a module. If so, the module will be
 	  called hi8435.
 
+config INA2XX_ADC
+	tristate "Texas Instruments INA2xx Power Monitors IIO driver"
+	depends on I2C && !SENSORS_INA2XX
+	select REGMAP_I2C
+	select IIO_BUFFER
+	select IIO_KFIFO_BUF
+	help
+	  Say yes here to build support for TI INA2xx family of Power Monitors.
+	  This driver is mutually exclusive with the HWMON version.
+
+config IMX7D_ADC
+	tristate "IMX7D ADC driver"
+	depends on ARCH_MXC || COMPILE_TEST
+	depends on HAS_IOMEM
+	help
+	  Say yes here to build support for IMX7D ADC.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called imx7d_adc.
+
 config LP8788_ADC
 	tristate "LP8788 ADC driver"
 	depends on MFD_LP8788
@@ -275,6 +296,14 @@ config NAU7802
 	  To compile this driver as a module, choose M here: the
 	  module will be called nau7802.
 
+config PALMAS_GPADC
+	tristate "TI Palmas General Purpose ADC"
+	depends on MFD_PALMAS
+	help
+	  Palmas series pmic chip by Texas Instruments (twl6035/6037)
+	  is used in smartphones and tablets and supports a 16 channel
+	  general purpose ADC.
+
 config QCOM_SPMI_IADC
 	tristate "Qualcomm SPMI PMIC current ADC"
 	depends on SPMI
@@ -324,15 +353,25 @@ config TI_ADC081C
 	  called ti-adc081c.
 
 config TI_ADC128S052
-	tristate "Texas Instruments ADC128S052/ADC122S021"
+	tristate "Texas Instruments ADC128S052/ADC122S021/ADC124S021"
 	depends on SPI
 	help
-	  If you say yes here you get support for Texas Instruments ADC128S052
-	  and ADC122S021 chips.
+	  If you say yes here you get support for Texas Instruments ADC128S052,
+	  ADC122S021 and ADC124S021 chips.
 
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc128s052.
 
+config TI_ADS8688
+	tristate "Texas Instruments ADS8688"
+	depends on SPI && OF
+	help
+	  If you say yes here you get support for Texas Instruments ADS8684 and
+	  and ADS8688 ADC chips
+
+	  This driver can also be built as a module. If so, the module will be
+	  called ti-ads8688.
+
 config TI_AM335X_ADC
 	tristate "TI's AM335X ADC driver"
 	depends on MFD_TI_AM335X_TSCADC
@@ -372,6 +411,7 @@ config TWL6030_GPADC
 config VF610_ADC
 	tristate "Freescale vf610 ADC driver"
 	depends on OF
+	depends on HAS_IOMEM
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 99b37a963a1e..6435780e9b71 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -20,6 +20,8 @@ obj-$(CONFIG_CC10001_ADC) += cc10001_adc.o
 obj-$(CONFIG_DA9150_GPADC) += da9150-gpadc.o
 obj-$(CONFIG_EXYNOS_ADC) += exynos_adc.o
 obj-$(CONFIG_HI8435) += hi8435.o
+obj-$(CONFIG_IMX7D_ADC) += imx7d_adc.o
+obj-$(CONFIG_INA2XX_ADC) += ina2xx-adc.o
 obj-$(CONFIG_LP8788_ADC) += lp8788_adc.o
 obj-$(CONFIG_MAX1027) += max1027.o
 obj-$(CONFIG_MAX1363) += max1363.o
@@ -27,11 +29,13 @@ obj-$(CONFIG_MCP320X) += mcp320x.o
 obj-$(CONFIG_MCP3422) += mcp3422.o
 obj-$(CONFIG_MEN_Z188_ADC) += men_z188_adc.o
 obj-$(CONFIG_NAU7802) += nau7802.o
+obj-$(CONFIG_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_TI_ADC081C) += ti-adc081c.o
 obj-$(CONFIG_TI_ADC128S052) += ti-adc128s052.o
+obj-$(CONFIG_TI_ADS8688) += ti-ads8688.o
 obj-$(CONFIG_TI_AM335X_ADC) += ti_am335x_adc.o
 obj-$(CONFIG_TWL4030_MADC) += twl4030-madc.o
 obj-$(CONFIG_TWL6030_GPADC) += twl6030-gpadc.o
diff --git a/drivers/iio/adc/ad7793.c b/drivers/iio/adc/ad7793.c
index 4d960d3b93c0..7b07bb651671 100644
--- a/drivers/iio/adc/ad7793.c
+++ b/drivers/iio/adc/ad7793.c
@@ -478,10 +478,9 @@ static int ad7793_read_raw(struct iio_dev *indio_dev,
 				*val2 = st->
 					scale_avail[(st->conf >> 8) & 0x7][1];
 				return IIO_VAL_INT_PLUS_NANO;
-			} else {
-				/* 1170mV / 2^23 * 6 */
-				scale_uv = (1170ULL * 1000000000ULL * 6ULL);
 			}
+			/* 1170mV / 2^23 * 6 */
+			scale_uv = (1170ULL * 1000000000ULL * 6ULL);
 			break;
 		case IIO_TEMP:
 				/* 1170mV / 0.81 mV/C / 2^23 */
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 7b40925dd4ff..f284cd6a93d6 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -742,7 +742,7 @@ static int at91_adc_of_get_resolution(struct at91_adc_state *st,
 		return count;
 	}
 
-	resolutions = kmalloc(count * sizeof(*resolutions), GFP_KERNEL);
+	resolutions = kmalloc_array(count, sizeof(*resolutions), GFP_KERNEL);
 	if (!resolutions)
 		return -ENOMEM;
 
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index 3a2dbb3b4926..c15756d7bf7f 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -35,6 +35,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/of_platform.h>
 #include <linux/err.h>
+#include <linux/input.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/machine.h>
@@ -42,12 +43,18 @@
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
 
+#include <linux/platform_data/touchscreen-s3c2410.h>
+
 /* S3C/EXYNOS4412/5250 ADC_V1 registers definitions */
 #define ADC_V1_CON(x)		((x) + 0x00)
+#define ADC_V1_TSC(x)		((x) + 0x04)
 #define ADC_V1_DLY(x)		((x) + 0x08)
 #define ADC_V1_DATX(x)		((x) + 0x0C)
+#define ADC_V1_DATY(x)		((x) + 0x10)
+#define ADC_V1_UPDN(x)		((x) + 0x14)
 #define ADC_V1_INTCLR(x)	((x) + 0x18)
 #define ADC_V1_MUX(x)		((x) + 0x1c)
+#define ADC_V1_CLRINTPNDNUP(x)	((x) + 0x20)
 
 /* S3C2410 ADC registers definitions */
 #define ADC_S3C2410_MUX(x)	((x) + 0x18)
@@ -71,6 +78,30 @@
 #define ADC_S3C2410_DATX_MASK	0x3FF
 #define ADC_S3C2416_CON_RES_SEL	(1u << 3)
 
+/* touch screen always uses channel 0 */
+#define ADC_S3C2410_MUX_TS	0
+
+/* ADCTSC Register Bits */
+#define ADC_S3C2443_TSC_UD_SEN		(1u << 8)
+#define ADC_S3C2410_TSC_YM_SEN		(1u << 7)
+#define ADC_S3C2410_TSC_YP_SEN		(1u << 6)
+#define ADC_S3C2410_TSC_XM_SEN		(1u << 5)
+#define ADC_S3C2410_TSC_XP_SEN		(1u << 4)
+#define ADC_S3C2410_TSC_PULL_UP_DISABLE	(1u << 3)
+#define ADC_S3C2410_TSC_AUTO_PST	(1u << 2)
+#define ADC_S3C2410_TSC_XY_PST(x)	(((x) & 0x3) << 0)
+
+#define ADC_TSC_WAIT4INT (ADC_S3C2410_TSC_YM_SEN | \
+			 ADC_S3C2410_TSC_YP_SEN | \
+			 ADC_S3C2410_TSC_XP_SEN | \
+			 ADC_S3C2410_TSC_XY_PST(3))
+
+#define ADC_TSC_AUTOPST	(ADC_S3C2410_TSC_YM_SEN | \
+			 ADC_S3C2410_TSC_YP_SEN | \
+			 ADC_S3C2410_TSC_XP_SEN | \
+			 ADC_S3C2410_TSC_AUTO_PST | \
+			 ADC_S3C2410_TSC_XY_PST(0))
+
 /* Bit definitions for ADC_V2 */
 #define ADC_V2_CON1_SOFT_RESET	(1u << 2)
 
@@ -88,7 +119,9 @@
 /* Bit definitions common for ADC_V1 and ADC_V2 */
 #define ADC_CON_EN_START	(1u << 0)
 #define ADC_CON_EN_START_MASK	(0x3 << 0)
+#define ADC_DATX_PRESSED	(1u << 15)
 #define ADC_DATX_MASK		0xFFF
+#define ADC_DATY_MASK		0xFFF
 
 #define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(100))
 
@@ -98,17 +131,24 @@
 struct exynos_adc {
 	struct exynos_adc_data	*data;
 	struct device		*dev;
+	struct input_dev	*input;
 	void __iomem		*regs;
 	struct regmap		*pmu_map;
 	struct clk		*clk;
 	struct clk		*sclk;
 	unsigned int		irq;
+	unsigned int		tsirq;
+	unsigned int		delay;
 	struct regulator	*vdd;
 
 	struct completion	completion;
 
 	u32			value;
 	unsigned int            version;
+
+	bool			read_ts;
+	u32			ts_x;
+	u32			ts_y;
 };
 
 struct exynos_adc_data {
@@ -197,6 +237,9 @@ static void exynos_adc_v1_init_hw(struct exynos_adc *info)
 	/* Enable 12-bit ADC resolution */
 	con1 |= ADC_V1_CON_RES;
 	writel(con1, ADC_V1_CON(info->regs));
+
+	/* set touchscreen delay */
+	writel(info->delay, ADC_V1_DLY(info->regs));
 }
 
 static void exynos_adc_v1_exit_hw(struct exynos_adc *info)
@@ -480,8 +523,8 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 	if (info->data->start_conv)
 		info->data->start_conv(info, chan->address);
 
-	timeout = wait_for_completion_timeout
-			(&info->completion, EXYNOS_ADC_TIMEOUT);
+	timeout = wait_for_completion_timeout(&info->completion,
+					      EXYNOS_ADC_TIMEOUT);
 	if (timeout == 0) {
 		dev_warn(&indio_dev->dev, "Conversion timed out! Resetting\n");
 		if (info->data->init_hw)
@@ -498,13 +541,55 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
+static int exynos_read_s3c64xx_ts(struct iio_dev *indio_dev, int *x, int *y)
+{
+	struct exynos_adc *info = iio_priv(indio_dev);
+	unsigned long timeout;
+	int ret;
+
+	mutex_lock(&indio_dev->mlock);
+	info->read_ts = true;
+
+	reinit_completion(&info->completion);
+
+	writel(ADC_S3C2410_TSC_PULL_UP_DISABLE | ADC_TSC_AUTOPST,
+	       ADC_V1_TSC(info->regs));
+
+	/* Select the ts channel to be used and Trigger conversion */
+	info->data->start_conv(info, ADC_S3C2410_MUX_TS);
+
+	timeout = wait_for_completion_timeout(&info->completion,
+					      EXYNOS_ADC_TIMEOUT);
+	if (timeout == 0) {
+		dev_warn(&indio_dev->dev, "Conversion timed out! Resetting\n");
+		if (info->data->init_hw)
+			info->data->init_hw(info);
+		ret = -ETIMEDOUT;
+	} else {
+		*x = info->ts_x;
+		*y = info->ts_y;
+		ret = 0;
+	}
+
+	info->read_ts = false;
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
 static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
 {
 	struct exynos_adc *info = (struct exynos_adc *)dev_id;
 	u32 mask = info->data->mask;
 
 	/* Read value */
-	info->value = readl(ADC_V1_DATX(info->regs)) & mask;
+	if (info->read_ts) {
+		info->ts_x = readl(ADC_V1_DATX(info->regs));
+		info->ts_y = readl(ADC_V1_DATY(info->regs));
+		writel(ADC_TSC_WAIT4INT | ADC_S3C2443_TSC_UD_SEN, ADC_V1_TSC(info->regs));
+	} else {
+		info->value = readl(ADC_V1_DATX(info->regs)) & mask;
+	}
 
 	/* clear irq */
 	if (info->data->clear_irq)
@@ -515,6 +600,46 @@ static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
+/*
+ * Here we (ab)use a threaded interrupt handler to stay running
+ * for as long as the touchscreen remains pressed, we report
+ * a new event with the latest data and then sleep until the
+ * next timer tick. This mirrors the behavior of the old
+ * driver, with much less code.
+ */
+static irqreturn_t exynos_ts_isr(int irq, void *dev_id)
+{
+	struct exynos_adc *info = dev_id;
+	struct iio_dev *dev = dev_get_drvdata(info->dev);
+	u32 x, y;
+	bool pressed;
+	int ret;
+
+	while (info->input->users) {
+		ret = exynos_read_s3c64xx_ts(dev, &x, &y);
+		if (ret == -ETIMEDOUT)
+			break;
+
+		pressed = x & y & ADC_DATX_PRESSED;
+		if (!pressed) {
+			input_report_key(info->input, BTN_TOUCH, 0);
+			input_sync(info->input);
+			break;
+		}
+
+		input_report_abs(info->input, ABS_X, x & ADC_DATX_MASK);
+		input_report_abs(info->input, ABS_Y, y & ADC_DATY_MASK);
+		input_report_key(info->input, BTN_TOUCH, 1);
+		input_sync(info->input);
+
+		msleep(1);
+	};
+
+	writel(0, ADC_V1_CLRINTPNDNUP(info->regs));
+
+	return IRQ_HANDLED;
+}
+
 static int exynos_adc_reg_access(struct iio_dev *indio_dev,
 			      unsigned reg, unsigned writeval,
 			      unsigned *readval)
@@ -566,18 +691,72 @@ static int exynos_adc_remove_devices(struct device *dev, void *c)
 	return 0;
 }
 
+static int exynos_adc_ts_open(struct input_dev *dev)
+{
+	struct exynos_adc *info = input_get_drvdata(dev);
+
+	enable_irq(info->tsirq);
+
+	return 0;
+}
+
+static void exynos_adc_ts_close(struct input_dev *dev)
+{
+	struct exynos_adc *info = input_get_drvdata(dev);
+
+	disable_irq(info->tsirq);
+}
+
+static int exynos_adc_ts_init(struct exynos_adc *info)
+{
+	int ret;
+
+	if (info->tsirq <= 0)
+		return -ENODEV;
+
+	info->input = input_allocate_device();
+	if (!info->input)
+		return -ENOMEM;
+
+	info->input->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
+	info->input->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
+
+	input_set_abs_params(info->input, ABS_X, 0, 0x3FF, 0, 0);
+	input_set_abs_params(info->input, ABS_Y, 0, 0x3FF, 0, 0);
+
+	info->input->name = "S3C24xx TouchScreen";
+	info->input->id.bustype = BUS_HOST;
+	info->input->open = exynos_adc_ts_open;
+	info->input->close = exynos_adc_ts_close;
+
+	input_set_drvdata(info->input, info);
+
+	ret = input_register_device(info->input);
+	if (ret) {
+		input_free_device(info->input);
+		return ret;
+	}
+
+	disable_irq(info->tsirq);
+	ret = request_threaded_irq(info->tsirq, NULL, exynos_ts_isr,
+				   IRQF_ONESHOT, "touchscreen", info);
+	if (ret)
+		input_unregister_device(info->input);
+
+	return ret;
+}
+
 static int exynos_adc_probe(struct platform_device *pdev)
 {
 	struct exynos_adc *info = NULL;
 	struct device_node *np = pdev->dev.of_node;
+	struct s3c2410_ts_mach_info *pdata = dev_get_platdata(&pdev->dev);
 	struct iio_dev *indio_dev = NULL;
 	struct resource	*mem;
+	bool has_ts = false;
 	int ret = -ENODEV;
 	int irq;
 
-	if (!np)
-		return ret;
-
 	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(struct exynos_adc));
 	if (!indio_dev) {
 		dev_err(&pdev->dev, "failed allocating iio device\n");
@@ -613,8 +792,14 @@ static int exynos_adc_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "no irq resource?\n");
 		return irq;
 	}
-
 	info->irq = irq;
+
+	irq = platform_get_irq(pdev, 1);
+	if (irq == -EPROBE_DEFER)
+		return irq;
+
+	info->tsirq = irq;
+
 	info->dev = &pdev->dev;
 
 	init_completion(&info->completion);
@@ -680,6 +865,22 @@ static int exynos_adc_probe(struct platform_device *pdev)
 	if (info->data->init_hw)
 		info->data->init_hw(info);
 
+	/* leave out any TS related code if unreachable */
+	if (IS_REACHABLE(CONFIG_INPUT)) {
+		has_ts = of_property_read_bool(pdev->dev.of_node,
+					       "has-touchscreen") || pdata;
+	}
+
+	if (pdata)
+		info->delay = pdata->delay;
+	else
+		info->delay = 10000;
+
+	if (has_ts)
+		ret = exynos_adc_ts_init(info);
+	if (ret)
+		goto err_iio;
+
 	ret = of_platform_populate(np, exynos_adc_match, NULL, &indio_dev->dev);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed adding child nodes\n");
@@ -691,6 +892,11 @@ static int exynos_adc_probe(struct platform_device *pdev)
 err_of_populate:
 	device_for_each_child(&indio_dev->dev, NULL,
 				exynos_adc_remove_devices);
+	if (has_ts) {
+		input_unregister_device(info->input);
+		free_irq(info->tsirq, info);
+	}
+err_iio:
 	iio_device_unregister(indio_dev);
 err_irq:
 	free_irq(info->irq, info);
@@ -710,6 +916,10 @@ static int exynos_adc_remove(struct platform_device *pdev)
 	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
 	struct exynos_adc *info = iio_priv(indio_dev);
 
+	if (IS_REACHABLE(CONFIG_INPUT)) {
+		free_irq(info->tsirq, info);
+		input_unregister_device(info->input);
+	}
 	device_for_each_child(&indio_dev->dev, NULL,
 				exynos_adc_remove_devices);
 	iio_device_unregister(indio_dev);
diff --git a/drivers/iio/adc/imx7d_adc.c b/drivers/iio/adc/imx7d_adc.c
new file mode 100644
index 000000000000..e2241ee94783
--- /dev/null
+++ b/drivers/iio/adc/imx7d_adc.c
@@ -0,0 +1,609 @@
+/*
+ * Freescale i.MX7D ADC driver
+ *
+ * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/driver.h>
+#include <linux/iio/sysfs.h>
+
+/* ADC register */
+#define IMX7D_REG_ADC_CH_A_CFG1			0x00
+#define IMX7D_REG_ADC_CH_A_CFG2			0x10
+#define IMX7D_REG_ADC_CH_B_CFG1			0x20
+#define IMX7D_REG_ADC_CH_B_CFG2			0x30
+#define IMX7D_REG_ADC_CH_C_CFG1			0x40
+#define IMX7D_REG_ADC_CH_C_CFG2			0x50
+#define IMX7D_REG_ADC_CH_D_CFG1			0x60
+#define IMX7D_REG_ADC_CH_D_CFG2			0x70
+#define IMX7D_REG_ADC_CH_SW_CFG			0x80
+#define IMX7D_REG_ADC_TIMER_UNIT		0x90
+#define IMX7D_REG_ADC_DMA_FIFO			0xa0
+#define IMX7D_REG_ADC_FIFO_STATUS		0xb0
+#define IMX7D_REG_ADC_INT_SIG_EN		0xc0
+#define IMX7D_REG_ADC_INT_EN			0xd0
+#define IMX7D_REG_ADC_INT_STATUS		0xe0
+#define IMX7D_REG_ADC_CHA_B_CNV_RSLT		0xf0
+#define IMX7D_REG_ADC_CHC_D_CNV_RSLT		0x100
+#define IMX7D_REG_ADC_CH_SW_CNV_RSLT		0x110
+#define IMX7D_REG_ADC_DMA_FIFO_DAT		0x120
+#define IMX7D_REG_ADC_ADC_CFG			0x130
+
+#define IMX7D_REG_ADC_CHANNEL_CFG2_BASE		0x10
+#define IMX7D_EACH_CHANNEL_REG_OFFSET		0x20
+
+#define IMX7D_REG_ADC_CH_CFG1_CHANNEL_EN			(0x1 << 31)
+#define IMX7D_REG_ADC_CH_CFG1_CHANNEL_SINGLE			BIT(30)
+#define IMX7D_REG_ADC_CH_CFG1_CHANNEL_AVG_EN			BIT(29)
+#define IMX7D_REG_ADC_CH_CFG1_CHANNEL_SEL(x)			((x) << 24)
+
+#define IMX7D_REG_ADC_CH_CFG2_AVG_NUM_4				(0x0 << 12)
+#define IMX7D_REG_ADC_CH_CFG2_AVG_NUM_8				(0x1 << 12)
+#define IMX7D_REG_ADC_CH_CFG2_AVG_NUM_16			(0x2 << 12)
+#define IMX7D_REG_ADC_CH_CFG2_AVG_NUM_32			(0x3 << 12)
+
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_4			(0x0 << 29)
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_8			(0x1 << 29)
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_16			(0x2 << 29)
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_32			(0x3 << 29)
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_64			(0x4 << 29)
+#define IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_128			(0x5 << 29)
+
+#define IMX7D_REG_ADC_ADC_CFG_ADC_CLK_DOWN			BIT(31)
+#define IMX7D_REG_ADC_ADC_CFG_ADC_POWER_DOWN			BIT(1)
+#define IMX7D_REG_ADC_ADC_CFG_ADC_EN				BIT(0)
+
+#define IMX7D_REG_ADC_INT_CHA_COV_INT_EN			BIT(8)
+#define IMX7D_REG_ADC_INT_CHB_COV_INT_EN			BIT(9)
+#define IMX7D_REG_ADC_INT_CHC_COV_INT_EN			BIT(10)
+#define IMX7D_REG_ADC_INT_CHD_COV_INT_EN			BIT(11)
+#define IMX7D_REG_ADC_INT_CHANNEL_INT_EN \
+	(IMX7D_REG_ADC_INT_CHA_COV_INT_EN | \
+	 IMX7D_REG_ADC_INT_CHB_COV_INT_EN | \
+	 IMX7D_REG_ADC_INT_CHC_COV_INT_EN | \
+	 IMX7D_REG_ADC_INT_CHD_COV_INT_EN)
+#define IMX7D_REG_ADC_INT_STATUS_CHANNEL_INT_STATUS		0xf00
+#define IMX7D_REG_ADC_INT_STATUS_CHANNEL_CONV_TIME_OUT		0xf0000
+
+#define IMX7D_ADC_TIMEOUT		msecs_to_jiffies(100)
+
+enum imx7d_adc_clk_pre_div {
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_4,
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_8,
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_16,
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_32,
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_64,
+	IMX7D_ADC_ANALOG_CLK_PRE_DIV_128,
+};
+
+enum imx7d_adc_average_num {
+	IMX7D_ADC_AVERAGE_NUM_4,
+	IMX7D_ADC_AVERAGE_NUM_8,
+	IMX7D_ADC_AVERAGE_NUM_16,
+	IMX7D_ADC_AVERAGE_NUM_32,
+};
+
+struct imx7d_adc_feature {
+	enum imx7d_adc_clk_pre_div clk_pre_div;
+	enum imx7d_adc_average_num avg_num;
+
+	u32 core_time_unit;	/* impact the sample rate */
+
+	bool average_en;
+};
+
+struct imx7d_adc {
+	struct device *dev;
+	void __iomem *regs;
+	struct clk *clk;
+
+	u32 vref_uv;
+	u32 value;
+	u32 channel;
+	u32 pre_div_num;
+
+	struct regulator *vref;
+	struct imx7d_adc_feature adc_feature;
+
+	struct completion completion;
+};
+
+struct imx7d_adc_analogue_core_clk {
+	u32 pre_div;
+	u32 reg_config;
+};
+
+#define IMX7D_ADC_ANALOGUE_CLK_CONFIG(_pre_div, _reg_conf) {	\
+	.pre_div = (_pre_div),					\
+	.reg_config = (_reg_conf),				\
+}
+
+static const struct imx7d_adc_analogue_core_clk imx7d_adc_analogue_clk[] = {
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(4, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_4),
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(8, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_8),
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(16, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_16),
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(32, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_32),
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(64, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_64),
+	IMX7D_ADC_ANALOGUE_CLK_CONFIG(128, IMX7D_REG_ADC_TIMER_UNIT_PRE_DIV_128),
+};
+
+#define IMX7D_ADC_CHAN(_idx) {					\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = (_idx),					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |	\
+				BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+}
+
+static const struct iio_chan_spec imx7d_adc_iio_channels[] = {
+	IMX7D_ADC_CHAN(0),
+	IMX7D_ADC_CHAN(1),
+	IMX7D_ADC_CHAN(2),
+	IMX7D_ADC_CHAN(3),
+	IMX7D_ADC_CHAN(4),
+	IMX7D_ADC_CHAN(5),
+	IMX7D_ADC_CHAN(6),
+	IMX7D_ADC_CHAN(7),
+	IMX7D_ADC_CHAN(8),
+	IMX7D_ADC_CHAN(9),
+	IMX7D_ADC_CHAN(10),
+	IMX7D_ADC_CHAN(11),
+	IMX7D_ADC_CHAN(12),
+	IMX7D_ADC_CHAN(13),
+	IMX7D_ADC_CHAN(14),
+	IMX7D_ADC_CHAN(15),
+};
+
+static const u32 imx7d_adc_average_num[] = {
+	IMX7D_REG_ADC_CH_CFG2_AVG_NUM_4,
+	IMX7D_REG_ADC_CH_CFG2_AVG_NUM_8,
+	IMX7D_REG_ADC_CH_CFG2_AVG_NUM_16,
+	IMX7D_REG_ADC_CH_CFG2_AVG_NUM_32,
+};
+
+static void imx7d_adc_feature_config(struct imx7d_adc *info)
+{
+	info->adc_feature.clk_pre_div = IMX7D_ADC_ANALOG_CLK_PRE_DIV_4;
+	info->adc_feature.avg_num = IMX7D_ADC_AVERAGE_NUM_32;
+	info->adc_feature.core_time_unit = 1;
+	info->adc_feature.average_en = true;
+}
+
+static void imx7d_adc_sample_rate_set(struct imx7d_adc *info)
+{
+	struct imx7d_adc_feature *adc_feature = &info->adc_feature;
+	struct imx7d_adc_analogue_core_clk adc_analogure_clk;
+	u32 i;
+	u32 tmp_cfg1;
+	u32 sample_rate = 0;
+
+	/*
+	 * Before sample set, disable channel A,B,C,D. Here we
+	 * clear the bit 31 of register REG_ADC_CH_A\B\C\D_CFG1.
+	 */
+	for (i = 0; i < 4; i++) {
+		tmp_cfg1 =
+			readl(info->regs + i * IMX7D_EACH_CHANNEL_REG_OFFSET);
+		tmp_cfg1 &= ~IMX7D_REG_ADC_CH_CFG1_CHANNEL_EN;
+		writel(tmp_cfg1,
+		       info->regs + i * IMX7D_EACH_CHANNEL_REG_OFFSET);
+	}
+
+	adc_analogure_clk = imx7d_adc_analogue_clk[adc_feature->clk_pre_div];
+	sample_rate |= adc_analogure_clk.reg_config;
+	info->pre_div_num = adc_analogure_clk.pre_div;
+
+	sample_rate |= adc_feature->core_time_unit;
+	writel(sample_rate, info->regs + IMX7D_REG_ADC_TIMER_UNIT);
+}
+
+static void imx7d_adc_hw_init(struct imx7d_adc *info)
+{
+	u32 cfg;
+
+	/* power up and enable adc analogue core */
+	cfg = readl(info->regs + IMX7D_REG_ADC_ADC_CFG);
+	cfg &= ~(IMX7D_REG_ADC_ADC_CFG_ADC_CLK_DOWN |
+		 IMX7D_REG_ADC_ADC_CFG_ADC_POWER_DOWN);
+	cfg |= IMX7D_REG_ADC_ADC_CFG_ADC_EN;
+	writel(cfg, info->regs + IMX7D_REG_ADC_ADC_CFG);
+
+	/* enable channel A,B,C,D interrupt */
+	writel(IMX7D_REG_ADC_INT_CHANNEL_INT_EN,
+	       info->regs + IMX7D_REG_ADC_INT_SIG_EN);
+	writel(IMX7D_REG_ADC_INT_CHANNEL_INT_EN,
+	       info->regs + IMX7D_REG_ADC_INT_EN);
+
+	imx7d_adc_sample_rate_set(info);
+}
+
+static void imx7d_adc_channel_set(struct imx7d_adc *info)
+{
+	u32 cfg1 = 0;
+	u32 cfg2;
+	u32 channel;
+
+	channel = info->channel;
+
+	/* the channel choose single conversion, and enable average mode */
+	cfg1 |= (IMX7D_REG_ADC_CH_CFG1_CHANNEL_EN |
+		 IMX7D_REG_ADC_CH_CFG1_CHANNEL_SINGLE);
+	if (info->adc_feature.average_en)
+		cfg1 |= IMX7D_REG_ADC_CH_CFG1_CHANNEL_AVG_EN;
+
+	/*
+	 * physical channel 0 chose logical channel A
+	 * physical channel 1 chose logical channel B
+	 * physical channel 2 chose logical channel C
+	 * physical channel 3 chose logical channel D
+	 */
+	cfg1 |= IMX7D_REG_ADC_CH_CFG1_CHANNEL_SEL(channel);
+
+	/*
+	 * read register REG_ADC_CH_A\B\C\D_CFG2, according to the
+	 * channel chosen
+	 */
+	cfg2 = readl(info->regs + IMX7D_EACH_CHANNEL_REG_OFFSET * channel +
+		     IMX7D_REG_ADC_CHANNEL_CFG2_BASE);
+
+	cfg2 |= imx7d_adc_average_num[info->adc_feature.avg_num];
+
+	/*
+	 * write the register REG_ADC_CH_A\B\C\D_CFG2, according to
+	 * the channel chosen
+	 */
+	writel(cfg2, info->regs + IMX7D_EACH_CHANNEL_REG_OFFSET * channel +
+	       IMX7D_REG_ADC_CHANNEL_CFG2_BASE);
+	writel(cfg1, info->regs + IMX7D_EACH_CHANNEL_REG_OFFSET * channel);
+}
+
+static u32 imx7d_adc_get_sample_rate(struct imx7d_adc *info)
+{
+	/* input clock is always 24MHz */
+	u32 input_clk = 24000000;
+	u32 analogue_core_clk;
+	u32 core_time_unit = info->adc_feature.core_time_unit;
+	u32 tmp;
+
+	analogue_core_clk = input_clk / info->pre_div_num;
+	tmp = (core_time_unit + 1) * 6;
+
+	return analogue_core_clk / tmp;
+}
+
+static int imx7d_adc_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val,
+			int *val2,
+			long mask)
+{
+	struct imx7d_adc *info = iio_priv(indio_dev);
+
+	u32 channel;
+	long ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+		reinit_completion(&info->completion);
+
+		channel = chan->channel & 0x03;
+		info->channel = channel;
+		imx7d_adc_channel_set(info);
+
+		ret = wait_for_completion_interruptible_timeout
+				(&info->completion, IMX7D_ADC_TIMEOUT);
+		if (ret == 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return -ETIMEDOUT;
+		}
+		if (ret < 0) {
+			mutex_unlock(&indio_dev->mlock);
+			return ret;
+		}
+
+		*val = info->value;
+		mutex_unlock(&indio_dev->mlock);
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		info->vref_uv = regulator_get_voltage(info->vref);
+		*val = info->vref_uv / 1000;
+		*val2 = 12;
+		return IIO_VAL_FRACTIONAL_LOG2;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = imx7d_adc_get_sample_rate(info);
+		return IIO_VAL_INT;
+
+	default:
+		return -EINVAL;
+	}
+}
+
+static int imx7d_adc_read_data(struct imx7d_adc *info)
+{
+	u32 channel;
+	u32 value;
+
+	channel = info->channel & 0x03;
+
+	/*
+	 * channel A and B conversion result share one register,
+	 * bit[27~16] is the channel B conversion result,
+	 * bit[11~0] is the channel A conversion result.
+	 * channel C and D is the same.
+	 */
+	if (channel < 2)
+		value = readl(info->regs + IMX7D_REG_ADC_CHA_B_CNV_RSLT);
+	else
+		value = readl(info->regs + IMX7D_REG_ADC_CHC_D_CNV_RSLT);
+	if (channel & 0x1)	/* channel B or D */
+		value = (value >> 16) & 0xFFF;
+	else			/* channel A or C */
+		value &= 0xFFF;
+
+	return value;
+}
+
+static irqreturn_t imx7d_adc_isr(int irq, void *dev_id)
+{
+	struct imx7d_adc *info = (struct imx7d_adc *)dev_id;
+	int status;
+
+	status = readl(info->regs + IMX7D_REG_ADC_INT_STATUS);
+	if (status & IMX7D_REG_ADC_INT_STATUS_CHANNEL_INT_STATUS) {
+		info->value = imx7d_adc_read_data(info);
+		complete(&info->completion);
+
+		/*
+		 * The register IMX7D_REG_ADC_INT_STATUS can't clear
+		 * itself after read operation, need software to write
+		 * 0 to the related bit. Here we clear the channel A/B/C/D
+		 * conversion finished flag.
+		 */
+		status &= ~IMX7D_REG_ADC_INT_STATUS_CHANNEL_INT_STATUS;
+		writel(status, info->regs + IMX7D_REG_ADC_INT_STATUS);
+	}
+
+	/*
+	 * If the channel A/B/C/D conversion timeout, report it and clear these
+	 * timeout flags.
+	 */
+	if (status & IMX7D_REG_ADC_INT_STATUS_CHANNEL_CONV_TIME_OUT) {
+		pr_err("%s: ADC got conversion time out interrupt: 0x%08x\n",
+			dev_name(info->dev), status);
+		status &= ~IMX7D_REG_ADC_INT_STATUS_CHANNEL_CONV_TIME_OUT;
+		writel(status, info->regs + IMX7D_REG_ADC_INT_STATUS);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int imx7d_adc_reg_access(struct iio_dev *indio_dev,
+			unsigned reg, unsigned writeval,
+			unsigned *readval)
+{
+	struct imx7d_adc *info = iio_priv(indio_dev);
+
+	if (!readval || reg % 4 || reg > IMX7D_REG_ADC_ADC_CFG)
+		return -EINVAL;
+
+	*readval = readl(info->regs + reg);
+
+	return 0;
+}
+
+static const struct iio_info imx7d_adc_iio_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = &imx7d_adc_read_raw,
+	.debugfs_reg_access = &imx7d_adc_reg_access,
+};
+
+static const struct of_device_id imx7d_adc_match[] = {
+	{ .compatible = "fsl,imx7d-adc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, imx7d_adc_match);
+
+static void imx7d_adc_power_down(struct imx7d_adc *info)
+{
+	u32 adc_cfg;
+
+	adc_cfg = readl(info->regs + IMX7D_REG_ADC_ADC_CFG);
+	adc_cfg |= IMX7D_REG_ADC_ADC_CFG_ADC_CLK_DOWN |
+		   IMX7D_REG_ADC_ADC_CFG_ADC_POWER_DOWN;
+	adc_cfg &= ~IMX7D_REG_ADC_ADC_CFG_ADC_EN;
+	writel(adc_cfg, info->regs + IMX7D_REG_ADC_ADC_CFG);
+}
+
+static int imx7d_adc_probe(struct platform_device *pdev)
+{
+	struct imx7d_adc *info;
+	struct iio_dev *indio_dev;
+	struct resource *mem;
+	int irq;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*info));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "Failed allocating iio device\n");
+		return -ENOMEM;
+	}
+
+	info = iio_priv(indio_dev);
+	info->dev = &pdev->dev;
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	info->regs = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(info->regs)) {
+		ret = PTR_ERR(info->regs);
+		dev_err(&pdev->dev,
+			"Failed to remap adc memory, err = %d\n", ret);
+		return ret;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "No irq resource?\n");
+		return irq;
+	}
+
+	info->clk = devm_clk_get(&pdev->dev, "adc");
+	if (IS_ERR(info->clk)) {
+		ret = PTR_ERR(info->clk);
+		dev_err(&pdev->dev, "Failed getting clock, err = %d\n", ret);
+		return ret;
+	}
+
+	info->vref = devm_regulator_get(&pdev->dev, "vref");
+	if (IS_ERR(info->vref)) {
+		ret = PTR_ERR(info->vref);
+		dev_err(&pdev->dev,
+			"Failed getting reference voltage, err = %d\n", ret);
+		return ret;
+	}
+
+	ret = regulator_enable(info->vref);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Can't enable adc reference top voltage, err = %d\n",
+			ret);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	init_completion(&info->completion);
+
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &imx7d_adc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = imx7d_adc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(imx7d_adc_iio_channels);
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Could not prepare or enable the clock.\n");
+		goto error_adc_clk_enable;
+	}
+
+	ret = devm_request_irq(info->dev, irq,
+				imx7d_adc_isr, 0,
+				dev_name(&pdev->dev), info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed requesting irq, irq = %d\n", irq);
+		goto error_iio_device_register;
+	}
+
+	imx7d_adc_feature_config(info);
+	imx7d_adc_hw_init(info);
+
+	ret = iio_device_register(indio_dev);
+	if (ret) {
+		imx7d_adc_power_down(info);
+		dev_err(&pdev->dev, "Couldn't register the device.\n");
+		goto error_iio_device_register;
+	}
+
+	return 0;
+
+error_iio_device_register:
+	clk_disable_unprepare(info->clk);
+error_adc_clk_enable:
+	regulator_disable(info->vref);
+
+	return ret;
+}
+
+static int imx7d_adc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct imx7d_adc *info = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	imx7d_adc_power_down(info);
+
+	clk_disable_unprepare(info->clk);
+	regulator_disable(info->vref);
+
+	return 0;
+}
+
+static int __maybe_unused imx7d_adc_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct imx7d_adc *info = iio_priv(indio_dev);
+
+	imx7d_adc_power_down(info);
+
+	clk_disable_unprepare(info->clk);
+	regulator_disable(info->vref);
+
+	return 0;
+}
+
+static int __maybe_unused imx7d_adc_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct imx7d_adc *info = iio_priv(indio_dev);
+	int ret;
+
+	ret = regulator_enable(info->vref);
+	if (ret) {
+		dev_err(info->dev,
+			"Can't enable adc reference top voltage, err = %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret) {
+		dev_err(info->dev,
+			"Could not prepare or enable clock.\n");
+		regulator_disable(info->vref);
+		return ret;
+	}
+
+	imx7d_adc_hw_init(info);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(imx7d_adc_pm_ops, imx7d_adc_suspend, imx7d_adc_resume);
+
+static struct platform_driver imx7d_adc_driver = {
+	.probe		= imx7d_adc_probe,
+	.remove		= imx7d_adc_remove,
+	.driver		= {
+		.name	= "imx7d_adc",
+		.of_match_table = imx7d_adc_match,
+		.pm	= &imx7d_adc_pm_ops,
+	},
+};
+
+module_platform_driver(imx7d_adc_driver);
+
+MODULE_AUTHOR("Haibo Chen <haibo.chen@freescale.com>");
+MODULE_DESCRIPTION("Freeacale IMX7D ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ina2xx-adc.c b/drivers/iio/adc/ina2xx-adc.c
new file mode 100644
index 000000000000..d803e5018a42
--- /dev/null
+++ b/drivers/iio/adc/ina2xx-adc.c
@@ -0,0 +1,745 @@
+/*
+ * INA2XX Current and Power Monitors
+ *
+ * Copyright 2015 Baylibre SAS.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Based on linux/drivers/iio/adc/ad7291.c
+ * Copyright 2010-2011 Analog Devices Inc.
+ *
+ * Based on linux/drivers/hwmon/ina2xx.c
+ * Copyright 2012 Lothar Felten <l-felten@ti.com>
+ *
+ * Licensed under the GPL-2 or later.
+ *
+ * IIO driver for INA219-220-226-230-231
+ *
+ * Configurable 7-bit I2C slave address from 0x40 to 0x4F
+ */
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/delay.h>
+#include <linux/iio/kfifo_buf.h>
+#include <linux/iio/sysfs.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/platform_data/ina2xx.h>
+
+#include <linux/util_macros.h>
+
+/* INA2XX registers definition */
+#define INA2XX_CONFIG                   0x00
+#define INA2XX_SHUNT_VOLTAGE            0x01	/* readonly */
+#define INA2XX_BUS_VOLTAGE              0x02	/* readonly */
+#define INA2XX_POWER                    0x03	/* readonly */
+#define INA2XX_CURRENT                  0x04	/* readonly */
+#define INA2XX_CALIBRATION              0x05
+
+#define INA226_ALERT_MASK		0x06
+#define INA266_CVRF			BIT(3)
+
+#define INA2XX_MAX_REGISTERS            8
+
+/* settings - depend on use case */
+#define INA219_CONFIG_DEFAULT           0x399F	/* PGA=8 */
+#define INA226_CONFIG_DEFAULT           0x4327
+#define INA226_DEFAULT_AVG              4
+#define INA226_DEFAULT_IT		1110
+
+#define INA2XX_RSHUNT_DEFAULT           10000
+
+/*
+ * bit mask for reading the averaging setting in the configuration register
+ * FIXME: use regmap_fields.
+ */
+#define INA2XX_MODE_MASK	GENMASK(3, 0)
+
+#define INA226_AVG_MASK		GENMASK(11, 9)
+#define INA226_SHIFT_AVG(val)	((val) << 9)
+
+/* Integration time for VBus */
+#define INA226_ITB_MASK		GENMASK(8, 6)
+#define INA226_SHIFT_ITB(val)	((val) << 6)
+
+/* Integration time for VShunt */
+#define INA226_ITS_MASK		GENMASK(5, 3)
+#define INA226_SHIFT_ITS(val)	((val) << 3)
+
+/* Cosmetic macro giving the sampling period for a full P=UxI cycle */
+#define SAMPLING_PERIOD(c)	((c->int_time_vbus + c->int_time_vshunt) \
+				 * c->avg)
+
+static bool ina2xx_is_writeable_reg(struct device *dev, unsigned int reg)
+{
+	return (reg == INA2XX_CONFIG) || (reg > INA2XX_CURRENT);
+}
+
+static bool ina2xx_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	return (reg != INA2XX_CONFIG);
+}
+
+static inline bool is_signed_reg(unsigned int reg)
+{
+	return (reg == INA2XX_SHUNT_VOLTAGE) || (reg == INA2XX_CURRENT);
+}
+
+static const struct regmap_config ina2xx_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 16,
+	.max_register = INA2XX_MAX_REGISTERS,
+	.writeable_reg = ina2xx_is_writeable_reg,
+	.volatile_reg = ina2xx_is_volatile_reg,
+};
+
+enum ina2xx_ids { ina219, ina226 };
+
+struct ina2xx_config {
+	u16 config_default;
+	int calibration_factor;
+	int shunt_div;
+	int bus_voltage_shift;
+	int bus_voltage_lsb;	/* uV */
+	int power_lsb;		/* uW */
+};
+
+struct ina2xx_chip_info {
+	struct regmap *regmap;
+	struct task_struct *task;
+	const struct ina2xx_config *config;
+	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;
+};
+
+static const struct ina2xx_config ina2xx_config[] = {
+	[ina219] = {
+		    .config_default = INA219_CONFIG_DEFAULT,
+		    .calibration_factor = 40960000,
+		    .shunt_div = 100,
+		    .bus_voltage_shift = 3,
+		    .bus_voltage_lsb = 4000,
+		    .power_lsb = 20000,
+		    },
+	[ina226] = {
+		    .config_default = INA226_CONFIG_DEFAULT,
+		    .calibration_factor = 5120000,
+		    .shunt_div = 400,
+		    .bus_voltage_shift = 0,
+		    .bus_voltage_lsb = 1250,
+		    .power_lsb = 25000,
+		    },
+};
+
+static int ina2xx_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	int ret;
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	unsigned int regval;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = regmap_read(chip->regmap, chan->address, &regval);
+		if (ret < 0)
+			return ret;
+
+		if (is_signed_reg(chan->address))
+			*val = (s16) regval;
+		else
+			*val  = regval;
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
+		*val = chip->avg;
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_INT_TIME:
+		*val = 0;
+		if (chan->address == INA2XX_SHUNT_VOLTAGE)
+			*val2 = chip->int_time_vshunt;
+		else
+			*val2 = chip->int_time_vbus;
+
+		return IIO_VAL_INT_PLUS_MICRO;
+
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		/*
+		 * Sample freq is read only, it is a consequence of
+		 * 1/AVG*(CT_bus+CT_shunt).
+		 */
+		*val = DIV_ROUND_CLOSEST(1000000, SAMPLING_PERIOD(chip));
+
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->address) {
+		case INA2XX_SHUNT_VOLTAGE:
+			/* processed (mV) = raw*1000/shunt_div */
+			*val2 = chip->config->shunt_div;
+			*val = 1000;
+			return IIO_VAL_FRACTIONAL;
+
+		case INA2XX_BUS_VOLTAGE:
+			/* processed (mV) = raw*lsb (uV) / (1000 << shift) */
+			*val = chip->config->bus_voltage_lsb;
+			*val2 = 1000 << chip->config->bus_voltage_shift;
+			return IIO_VAL_FRACTIONAL;
+
+		case INA2XX_POWER:
+			/* processed (mW) = raw*lsb (uW) / 1000 */
+			*val = chip->config->power_lsb;
+			*val2 = 1000;
+			return IIO_VAL_FRACTIONAL;
+
+		case INA2XX_CURRENT:
+			/* processed (mA) = raw (mA) */
+			*val = 1;
+			return IIO_VAL_INT;
+		}
+	}
+
+	return -EINVAL;
+}
+
+/*
+ * Available averaging rates for ina226. The indices correspond with
+ * the bit values expected by the chip (according to the ina226 datasheet,
+ * table 3 AVG bit settings, found at
+ * http://www.ti.com/lit/ds/symlink/ina226.pdf.
+ */
+static const int ina226_avg_tab[] = { 1, 4, 16, 64, 128, 256, 512, 1024 };
+
+static int ina226_set_average(struct ina2xx_chip_info *chip, unsigned int val,
+			      unsigned int *config)
+{
+	int bits;
+
+	if (val > 1024 || val < 1)
+		return -EINVAL;
+
+	bits = find_closest(val, ina226_avg_tab,
+			    ARRAY_SIZE(ina226_avg_tab));
+
+	chip->avg = ina226_avg_tab[bits];
+
+	*config &= ~INA226_AVG_MASK;
+	*config |= INA226_SHIFT_AVG(bits) & INA226_AVG_MASK;
+
+	return 0;
+}
+
+/* Conversion times in uS */
+static const int ina226_conv_time_tab[] = { 140, 204, 332, 588, 1100,
+					    2116, 4156, 8244 };
+
+static int ina226_set_int_time_vbus(struct ina2xx_chip_info *chip,
+				    unsigned int val_us, unsigned int *config)
+{
+	int bits;
+
+	if (val_us > 8244 || val_us < 140)
+		return -EINVAL;
+
+	bits = find_closest(val_us, ina226_conv_time_tab,
+			ARRAY_SIZE(ina226_conv_time_tab));
+
+	chip->int_time_vbus = ina226_conv_time_tab[bits];
+
+	*config &= ~INA226_ITB_MASK;
+	*config |= INA226_SHIFT_ITB(bits) & INA226_ITB_MASK;
+
+	return 0;
+}
+
+static int ina226_set_int_time_vshunt(struct ina2xx_chip_info *chip,
+				      unsigned int val_us, unsigned int *config)
+{
+	int bits;
+
+	if (val_us > 8244 || val_us < 140)
+		return -EINVAL;
+
+	bits = find_closest(val_us, ina226_conv_time_tab,
+			ARRAY_SIZE(ina226_conv_time_tab));
+
+	chip->int_time_vshunt = ina226_conv_time_tab[bits];
+
+	*config &= ~INA226_ITS_MASK;
+	*config |= INA226_SHIFT_ITS(bits) & INA226_ITS_MASK;
+
+	return 0;
+}
+
+static int ina2xx_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int val, int val2, long mask)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	int ret;
+	unsigned int config, tmp;
+
+	if (iio_buffer_enabled(indio_dev))
+		return -EBUSY;
+
+	mutex_lock(&chip->state_lock);
+
+	ret = regmap_read(chip->regmap, INA2XX_CONFIG, &config);
+	if (ret < 0)
+		goto _err;
+
+	tmp = config;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
+		ret = ina226_set_average(chip, val, &tmp);
+		break;
+
+	case IIO_CHAN_INFO_INT_TIME:
+		if (chan->address == INA2XX_SHUNT_VOLTAGE)
+			ret = ina226_set_int_time_vshunt(chip, val2, &tmp);
+		else
+			ret = ina226_set_int_time_vbus(chip, val2, &tmp);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	if (!ret && (tmp != config))
+		ret = regmap_write(chip->regmap, INA2XX_CONFIG, tmp);
+_err:
+	mutex_unlock(&chip->state_lock);
+
+	return ret;
+}
+
+
+static ssize_t ina2xx_allow_async_readout_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct ina2xx_chip_info *chip = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "%d\n", chip->allow_async_readout);
+}
+
+static ssize_t ina2xx_allow_async_readout_store(struct device *dev,
+				struct device_attribute *attr,
+				const char *buf, size_t len)
+{
+	struct ina2xx_chip_info *chip = iio_priv(dev_to_iio_dev(dev));
+	bool val;
+	int ret;
+
+	ret = strtobool((const char *) buf, &val);
+	if (ret)
+		return ret;
+
+	chip->allow_async_readout = val;
+
+	return len;
+}
+
+static int set_shunt_resistor(struct ina2xx_chip_info *chip, unsigned int val)
+{
+	if (val <= 0 || val > chip->config->calibration_factor)
+		return -EINVAL;
+
+	chip->shunt_resistor = val;
+	return 0;
+}
+
+static ssize_t ina2xx_shunt_resistor_show(struct device *dev,
+					  struct device_attribute *attr,
+					  char *buf)
+{
+	struct ina2xx_chip_info *chip = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "%d\n", chip->shunt_resistor);
+}
+
+static ssize_t ina2xx_shunt_resistor_store(struct device *dev,
+					   struct device_attribute *attr,
+					   const char *buf, size_t len)
+{
+	struct ina2xx_chip_info *chip = iio_priv(dev_to_iio_dev(dev));
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul((const char *) buf, 10, &val);
+	if (ret)
+		return ret;
+
+	ret = set_shunt_resistor(chip, val);
+	if (ret)
+		return ret;
+
+	return len;
+}
+
+#define INA2XX_CHAN(_type, _index, _address) { \
+	.type = (_type), \
+	.address = (_address), \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) \
+	| BIT(IIO_CHAN_INFO_SCALE), \
+	.info_mask_shared_by_dir = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+				   BIT(IIO_CHAN_INFO_OVERSAMPLING_RATIO), \
+	.scan_index = (_index), \
+	.scan_type = { \
+		.sign = 'u', \
+		.realbits = 16, \
+		.storagebits = 16, \
+		.endianness = IIO_CPU, \
+	} \
+}
+
+/*
+ * Sampling Freq is a consequence of the integration times of
+ * the Voltage channels.
+ */
+#define INA2XX_CHAN_VOLTAGE(_index, _address) { \
+	.type = IIO_VOLTAGE, \
+	.address = (_address), \
+	.indexed = 1, \
+	.channel = (_index), \
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			      BIT(IIO_CHAN_INFO_SCALE) | \
+			      BIT(IIO_CHAN_INFO_INT_TIME), \
+	.scan_index = (_index), \
+	.scan_type = { \
+		.sign = 'u', \
+		.realbits = 16, \
+		.storagebits = 16, \
+		.endianness = IIO_LE, \
+	} \
+}
+
+static const struct iio_chan_spec ina2xx_channels[] = {
+	INA2XX_CHAN_VOLTAGE(0, INA2XX_SHUNT_VOLTAGE),
+	INA2XX_CHAN_VOLTAGE(1, INA2XX_BUS_VOLTAGE),
+	INA2XX_CHAN(IIO_POWER, 2, INA2XX_POWER),
+	INA2XX_CHAN(IIO_CURRENT, 3, INA2XX_CURRENT),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static int ina2xx_work_buffer(struct iio_dev *indio_dev)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	unsigned short data[8];
+	int bit, ret, i = 0;
+	unsigned long buffer_us, elapsed_us;
+	s64 time_a, time_b;
+	unsigned int alert;
+
+	time_a = iio_get_time_ns();
+
+	/*
+	 * Because the timer thread and the chip conversion clock
+	 * are asynchronous, the period difference will eventually
+	 * result in reading V[k-1] again, or skip V[k] at time Tk.
+	 * In order to resync the timer with the conversion process
+	 * we check the ConVersionReadyFlag.
+	 * On hardware that supports using the ALERT pin to toggle a
+	 * GPIO a triggered buffer could be used instead.
+	 * For now, we pay for that extra read of the ALERT register
+	 */
+	if (!chip->allow_async_readout)
+		do {
+			ret = regmap_read(chip->regmap, INA226_ALERT_MASK,
+					  &alert);
+			if (ret < 0)
+				return ret;
+
+			alert &= INA266_CVRF;
+			trace_printk("Conversion ready: %d\n", !!alert);
+
+		} while (!alert);
+
+	/*
+	 * Single register reads: bulk_read will not work with ina226
+	 * as there is no auto-increment of the address register for
+	 * data length longer than 16bits.
+	 */
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		unsigned int val;
+
+		ret = regmap_read(chip->regmap,
+				  INA2XX_SHUNT_VOLTAGE + bit, &val);
+		if (ret < 0)
+			return ret;
+
+		data[i++] = val;
+	}
+
+	time_b = iio_get_time_ns();
+
+	iio_push_to_buffers_with_timestamp(indio_dev,
+					   (unsigned int *)data, time_a);
+
+	buffer_us = (unsigned long)(time_b - time_a) / 1000;
+	elapsed_us = (unsigned long)(time_a - chip->prev_ns) / 1000;
+
+	trace_printk("uS: elapsed: %lu, buf: %lu\n", elapsed_us, buffer_us);
+
+	chip->prev_ns = time_a;
+
+	return buffer_us;
+};
+
+static int ina2xx_capture_thread(void *data)
+{
+	struct iio_dev *indio_dev = (struct iio_dev *)data;
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	unsigned int sampling_us = SAMPLING_PERIOD(chip);
+	int buffer_us;
+
+	/*
+	 * Poll a bit faster than the chip internal Fs, in case
+	 * we wish to sync with the conversion ready flag.
+	 */
+	if (!chip->allow_async_readout)
+		sampling_us -= 200;
+
+	do {
+		buffer_us = ina2xx_work_buffer(indio_dev);
+		if (buffer_us < 0)
+			return buffer_us;
+
+		if (sampling_us > buffer_us)
+			udelay(sampling_us - buffer_us);
+
+	} while (!kthread_should_stop());
+
+	return 0;
+}
+
+static int ina2xx_buffer_enable(struct iio_dev *indio_dev)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+	unsigned int sampling_us = SAMPLING_PERIOD(chip);
+
+	trace_printk("Enabling buffer w/ scan_mask %02x, freq = %d, avg =%u\n",
+		     (unsigned int)(*indio_dev->active_scan_mask),
+		     1000000/sampling_us, chip->avg);
+
+	trace_printk("Expected work period: %u us\n", sampling_us);
+	trace_printk("Async readout mode: %d\n", chip->allow_async_readout);
+
+	chip->prev_ns = iio_get_time_ns();
+
+	chip->task = kthread_run(ina2xx_capture_thread, (void *)indio_dev,
+				 "%s:%d-%uus", indio_dev->name, indio_dev->id,
+				 sampling_us);
+
+	return PTR_ERR_OR_ZERO(chip->task);
+}
+
+static int ina2xx_buffer_disable(struct iio_dev *indio_dev)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+
+	if (chip->task) {
+		kthread_stop(chip->task);
+		chip->task = NULL;
+	}
+
+	return 0;
+}
+
+static const struct iio_buffer_setup_ops ina2xx_setup_ops = {
+	.postenable = &ina2xx_buffer_enable,
+	.predisable = &ina2xx_buffer_disable,
+};
+
+static int ina2xx_debug_reg(struct iio_dev *indio_dev,
+			    unsigned reg, unsigned writeval, unsigned *readval)
+{
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+
+	if (!readval)
+		return regmap_write(chip->regmap, reg, writeval);
+
+	return regmap_read(chip->regmap, reg, readval);
+}
+
+/* Possible integration times for vshunt and vbus */
+static IIO_CONST_ATTR_INT_TIME_AVAIL \
+ ("0.000140 0.000204 0.000332 0.000588 0.001100 0.002116 0.004156 0.008244");
+
+static IIO_DEVICE_ATTR(in_allow_async_readout, S_IRUGO | S_IWUSR,
+		       ina2xx_allow_async_readout_show,
+		       ina2xx_allow_async_readout_store, 0);
+
+static IIO_DEVICE_ATTR(in_shunt_resistor, S_IRUGO | S_IWUSR,
+		       ina2xx_shunt_resistor_show,
+		       ina2xx_shunt_resistor_store, 0);
+
+static struct attribute *ina2xx_attributes[] = {
+	&iio_dev_attr_in_allow_async_readout.dev_attr.attr,
+	&iio_const_attr_integration_time_available.dev_attr.attr,
+	&iio_dev_attr_in_shunt_resistor.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ina2xx_attribute_group = {
+	.attrs = ina2xx_attributes,
+};
+
+static const struct iio_info ina2xx_info = {
+	.debugfs_reg_access = &ina2xx_debug_reg,
+	.read_raw = &ina2xx_read_raw,
+	.write_raw = &ina2xx_write_raw,
+	.attrs = &ina2xx_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+/* Initialize the configuration and calibration registers. */
+static int ina2xx_init(struct ina2xx_chip_info *chip, unsigned int config)
+{
+	u16 regval;
+	int ret = regmap_write(chip->regmap, INA2XX_CONFIG, config);
+
+	if (ret < 0)
+		return ret;
+	/*
+	 * Set current LSB to 1mA, shunt is in uOhms
+	 * (equation 13 in datasheet). We hardcode a Current_LSB
+	 * of 1.0 x10-6. The only remaining parameter is RShunt.
+	 * There is no need to expose the CALIBRATION register
+	 * to the user for now.
+	 */
+	regval = DIV_ROUND_CLOSEST(chip->config->calibration_factor,
+			    chip->shunt_resistor);
+
+	return regmap_write(chip->regmap, INA2XX_CALIBRATION, regval);
+}
+
+static int ina2xx_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct ina2xx_chip_info *chip;
+	struct iio_dev *indio_dev;
+	struct iio_buffer *buffer;
+	int ret;
+	unsigned int val;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*chip));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	chip = iio_priv(indio_dev);
+
+	chip->config = &ina2xx_config[id->driver_data];
+
+	if (of_property_read_u32(client->dev.of_node,
+				 "shunt-resistor", &val) < 0) {
+		struct ina2xx_platform_data *pdata =
+		    dev_get_platdata(&client->dev);
+
+		if (pdata)
+			val = pdata->shunt_uohms;
+		else
+			val = INA2XX_RSHUNT_DEFAULT;
+	}
+
+	ret = set_shunt_resistor(chip, val);
+	if (ret)
+		return ret;
+
+	mutex_init(&chip->state_lock);
+
+	/* This is only used for device removal purposes. */
+	i2c_set_clientdata(client, indio_dev);
+
+	indio_dev->name = id->name;
+	indio_dev->channels = ina2xx_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ina2xx_channels);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &ina2xx_info;
+	indio_dev->modes = INDIO_DIRECT_MODE | INDIO_BUFFER_SOFTWARE;
+
+	chip->regmap = devm_regmap_init_i2c(client, &ina2xx_regmap_config);
+	if (IS_ERR(chip->regmap)) {
+		dev_err(&client->dev, "failed to allocate register map\n");
+		return PTR_ERR(chip->regmap);
+	}
+
+	/* Patch the current config register with default. */
+	val = chip->config->config_default;
+
+	if (id->driver_data == ina226) {
+		ina226_set_average(chip, INA226_DEFAULT_AVG, &val);
+		ina226_set_int_time_vbus(chip, INA226_DEFAULT_IT, &val);
+		ina226_set_int_time_vshunt(chip, INA226_DEFAULT_IT, &val);
+	}
+
+	ret = ina2xx_init(chip, val);
+	if (ret < 0) {
+		dev_err(&client->dev, "error configuring the device: %d\n",
+			ret);
+		return -ENODEV;
+	}
+
+	buffer = devm_iio_kfifo_allocate(&indio_dev->dev);
+	if (!buffer)
+		return -ENOMEM;
+
+	indio_dev->setup_ops = &ina2xx_setup_ops;
+
+	iio_device_attach_buffer(indio_dev, buffer);
+
+	return iio_device_register(indio_dev);
+}
+
+
+static int ina2xx_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct ina2xx_chip_info *chip = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	/* Powerdown */
+	return regmap_update_bits(chip->regmap, INA2XX_CONFIG,
+				  INA2XX_MODE_MASK, 0);
+}
+
+
+static const struct i2c_device_id ina2xx_id[] = {
+	{"ina219", ina219},
+	{"ina220", ina219},
+	{"ina226", ina226},
+	{"ina230", ina226},
+	{"ina231", ina226},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, ina2xx_id);
+
+static struct i2c_driver ina2xx_driver = {
+	.driver = {
+		   .name = KBUILD_MODNAME,
+	},
+	.probe = ina2xx_probe,
+	.remove = ina2xx_remove,
+	.id_table = ina2xx_id,
+};
+
+module_i2c_driver(ina2xx_driver);
+
+MODULE_AUTHOR("Marc Titinger <marc.titinger@baylibre.com>");
+MODULE_DESCRIPTION("Texas Instruments INA2XX ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/mcp320x.c b/drivers/iio/adc/mcp320x.c
index 8569c8e1f4b2..d1c05f6eed18 100644
--- a/drivers/iio/adc/mcp320x.c
+++ b/drivers/iio/adc/mcp320x.c
@@ -354,6 +354,7 @@ static int mcp320x_remove(struct spi_device *spi)
 
 #if defined(CONFIG_OF)
 static const struct of_device_id mcp320x_dt_ids[] = {
+	/* NOTE: The use of compatibles with no vendor prefix is deprecated. */
 	{
 		.compatible = "mcp3001",
 		.data = &mcp320x_chip_infos[mcp3001],
@@ -382,6 +383,33 @@ static const struct of_device_id mcp320x_dt_ids[] = {
 		.compatible = "mcp3301",
 		.data = &mcp320x_chip_infos[mcp3301],
 	}, {
+		.compatible = "microchip,mcp3001",
+		.data = &mcp320x_chip_infos[mcp3001],
+	}, {
+		.compatible = "microchip,mcp3002",
+		.data = &mcp320x_chip_infos[mcp3002],
+	}, {
+		.compatible = "microchip,mcp3004",
+		.data = &mcp320x_chip_infos[mcp3004],
+	}, {
+		.compatible = "microchip,mcp3008",
+		.data = &mcp320x_chip_infos[mcp3008],
+	}, {
+		.compatible = "microchip,mcp3201",
+		.data = &mcp320x_chip_infos[mcp3201],
+	}, {
+		.compatible = "microchip,mcp3202",
+		.data = &mcp320x_chip_infos[mcp3202],
+	}, {
+		.compatible = "microchip,mcp3204",
+		.data = &mcp320x_chip_infos[mcp3204],
+	}, {
+		.compatible = "microchip,mcp3208",
+		.data = &mcp320x_chip_infos[mcp3208],
+	}, {
+		.compatible = "microchip,mcp3301",
+		.data = &mcp320x_chip_infos[mcp3301],
+	}, {
 	}
 };
 MODULE_DEVICE_TABLE(of, mcp320x_dt_ids);
diff --git a/drivers/iio/adc/mcp3422.c b/drivers/iio/adc/mcp3422.c
index 3555122008b4..6eca7aea8a37 100644
--- a/drivers/iio/adc/mcp3422.c
+++ b/drivers/iio/adc/mcp3422.c
@@ -305,6 +305,10 @@ static const struct attribute_group mcp3422_attribute_group = {
 	.attrs = mcp3422_attributes,
 };
 
+static const struct iio_chan_spec mcp3421_channels[] = {
+	MCP3422_CHAN(0),
+};
+
 static const struct iio_chan_spec mcp3422_channels[] = {
 	MCP3422_CHAN(0),
 	MCP3422_CHAN(1),
@@ -352,6 +356,10 @@ static int mcp3422_probe(struct i2c_client *client,
 	indio_dev->info = &mcp3422_info;
 
 	switch (adc->id) {
+	case 1:
+		indio_dev->channels = mcp3421_channels;
+		indio_dev->num_channels = ARRAY_SIZE(mcp3421_channels);
+		break;
 	case 2:
 	case 3:
 	case 6:
@@ -383,6 +391,7 @@ static int mcp3422_probe(struct i2c_client *client,
 }
 
 static const struct i2c_device_id mcp3422_id[] = {
+	{ "mcp3421", 1 },
 	{ "mcp3422", 2 },
 	{ "mcp3423", 3 },
 	{ "mcp3424", 4 },
diff --git a/drivers/iio/adc/palmas_gpadc.c b/drivers/iio/adc/palmas_gpadc.c
new file mode 100644
index 000000000000..f42eb8a7d21f
--- /dev/null
+++ b/drivers/iio/adc/palmas_gpadc.c
@@ -0,0 +1,859 @@
+/*
+ * palmas-adc.c -- TI PALMAS GPADC.
+ *
+ * Copyright (c) 2013, NVIDIA Corporation. All rights reserved.
+ *
+ * Author: Pradeep Goudagunta <pgoudagunta@nvidia.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/pm.h>
+#include <linux/mfd/palmas.h>
+#include <linux/completion.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/machine.h>
+#include <linux/iio/driver.h>
+
+#define MOD_NAME "palmas-gpadc"
+#define PALMAS_ADC_CONVERSION_TIMEOUT	(msecs_to_jiffies(5000))
+#define PALMAS_TO_BE_CALCULATED 0
+#define PALMAS_GPADC_TRIMINVALID	-1
+
+struct palmas_gpadc_info {
+/* calibration codes and regs */
+	int x1;	/* lower ideal code */
+	int x2;	/* higher ideal code */
+	int v1;	/* expected lower volt reading */
+	int v2;	/* expected higher volt reading */
+	u8 trim1_reg;	/* register number for lower trim */
+	u8 trim2_reg;	/* register number for upper trim */
+	int gain;	/* calculated from above (after reading trim regs) */
+	int offset;	/* calculated from above (after reading trim regs) */
+	int gain_error;	/* calculated from above (after reading trim regs) */
+	bool is_uncalibrated;	/* if channel has calibration data */
+};
+
+#define PALMAS_ADC_INFO(_chan, _x1, _x2, _v1, _v2, _t1, _t2, _is_uncalibrated) \
+	[PALMAS_ADC_CH_##_chan] = { \
+		.x1 = _x1, \
+		.x2 = _x2, \
+		.v1 = _v1, \
+		.v2 = _v2, \
+		.gain = PALMAS_TO_BE_CALCULATED, \
+		.offset = PALMAS_TO_BE_CALCULATED, \
+		.gain_error = PALMAS_TO_BE_CALCULATED, \
+		.trim1_reg = PALMAS_GPADC_TRIM##_t1, \
+		.trim2_reg = PALMAS_GPADC_TRIM##_t2,  \
+		.is_uncalibrated = _is_uncalibrated \
+	}
+
+static struct palmas_gpadc_info palmas_gpadc_info[] = {
+	PALMAS_ADC_INFO(IN0, 2064, 3112, 630, 950, 1, 2, false),
+	PALMAS_ADC_INFO(IN1, 2064, 3112, 630, 950, 1, 2, false),
+	PALMAS_ADC_INFO(IN2, 2064, 3112, 1260, 1900, 3, 4, false),
+	PALMAS_ADC_INFO(IN3, 2064, 3112, 630, 950, 1, 2, false),
+	PALMAS_ADC_INFO(IN4, 2064, 3112, 630, 950, 1, 2, false),
+	PALMAS_ADC_INFO(IN5, 2064, 3112, 630, 950, 1, 2, false),
+	PALMAS_ADC_INFO(IN6, 2064, 3112, 2520, 3800, 5, 6, false),
+	PALMAS_ADC_INFO(IN7, 2064, 3112, 2520, 3800, 7, 8, false),
+	PALMAS_ADC_INFO(IN8, 2064, 3112, 3150, 4750, 9, 10, false),
+	PALMAS_ADC_INFO(IN9, 2064, 3112, 5670, 8550, 11, 12, false),
+	PALMAS_ADC_INFO(IN10, 2064, 3112, 3465, 5225, 13, 14, false),
+	PALMAS_ADC_INFO(IN11, 0, 0, 0, 0, INVALID, INVALID, true),
+	PALMAS_ADC_INFO(IN12, 0, 0, 0, 0, INVALID, INVALID, true),
+	PALMAS_ADC_INFO(IN13, 0, 0, 0, 0, INVALID, INVALID, true),
+	PALMAS_ADC_INFO(IN14, 2064, 3112, 3645, 5225, 15, 16, false),
+	PALMAS_ADC_INFO(IN15, 0, 0, 0, 0, INVALID, INVALID, true),
+};
+
+/**
+ * struct palmas_gpadc - the palmas_gpadc structure
+ * @ch0_current:	channel 0 current source setting
+ *			0: 0 uA
+ *			1: 5 uA
+ *			2: 15 uA
+ *			3: 20 uA
+ * @ch3_current:	channel 0 current source setting
+ *			0: 0 uA
+ *			1: 10 uA
+ *			2: 400 uA
+ *			3: 800 uA
+ * @extended_delay:	enable the gpadc extended delay mode
+ * @auto_conversion_period:	define the auto_conversion_period
+ *
+ * This is the palmas_gpadc structure to store run-time information
+ * and pointers for this driver instance.
+ */
+
+struct palmas_gpadc {
+	struct device			*dev;
+	struct palmas			*palmas;
+	u8				ch0_current;
+	u8				ch3_current;
+	bool				extended_delay;
+	int				irq;
+	int				irq_auto_0;
+	int				irq_auto_1;
+	struct palmas_gpadc_info	*adc_info;
+	struct completion		conv_completion;
+	struct palmas_adc_wakeup_property wakeup1_data;
+	struct palmas_adc_wakeup_property wakeup2_data;
+	bool				wakeup1_enable;
+	bool				wakeup2_enable;
+	int				auto_conversion_period;
+};
+
+/*
+ * GPADC lock issue in AUTO mode.
+ * Impact: In AUTO mode, GPADC conversion can be locked after disabling AUTO
+ *	   mode feature.
+ * Details:
+ *	When the AUTO mode is the only conversion mode enabled, if the AUTO
+ *	mode feature is disabled with bit GPADC_AUTO_CTRL.  AUTO_CONV1_EN = 0
+ *	or bit GPADC_AUTO_CTRL.  AUTO_CONV0_EN = 0 during a conversion, the
+ *	conversion mechanism can be seen as locked meaning that all following
+ *	conversion will give 0 as a result.  Bit GPADC_STATUS.GPADC_AVAILABLE
+ *	will stay at 0 meaning that GPADC is busy.  An RT conversion can unlock
+ *	the GPADC.
+ *
+ * Workaround(s):
+ *	To avoid the lock mechanism, the workaround to follow before any stop
+ *	conversion request is:
+ *	Force the GPADC state machine to be ON by using the GPADC_CTRL1.
+ *		GPADC_FORCE bit = 1
+ *	Shutdown the GPADC AUTO conversion using
+ *		GPADC_AUTO_CTRL.SHUTDOWN_CONV[01] = 0.
+ *	After 100us, force the GPADC state machine to be OFF by using the
+ *		GPADC_CTRL1.  GPADC_FORCE bit = 0
+ */
+
+static int palmas_disable_auto_conversion(struct palmas_gpadc *adc)
+{
+	int ret;
+
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_CTRL1,
+			PALMAS_GPADC_CTRL1_GPADC_FORCE,
+			PALMAS_GPADC_CTRL1_GPADC_FORCE);
+	if (ret < 0) {
+		dev_err(adc->dev, "GPADC_CTRL1 update failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_AUTO_CTRL,
+			PALMAS_GPADC_AUTO_CTRL_SHUTDOWN_CONV1 |
+			PALMAS_GPADC_AUTO_CTRL_SHUTDOWN_CONV0,
+			0);
+	if (ret < 0) {
+		dev_err(adc->dev, "AUTO_CTRL update failed: %d\n", ret);
+		return ret;
+	}
+
+	udelay(100);
+
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_CTRL1,
+			PALMAS_GPADC_CTRL1_GPADC_FORCE, 0);
+	if (ret < 0)
+		dev_err(adc->dev, "GPADC_CTRL1 update failed: %d\n", ret);
+
+	return ret;
+}
+
+static irqreturn_t palmas_gpadc_irq(int irq, void *data)
+{
+	struct palmas_gpadc *adc = data;
+
+	complete(&adc->conv_completion);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t palmas_gpadc_irq_auto(int irq, void *data)
+{
+	struct palmas_gpadc *adc = data;
+
+	dev_dbg(adc->dev, "Threshold interrupt %d occurs\n", irq);
+	palmas_disable_auto_conversion(adc);
+
+	return IRQ_HANDLED;
+}
+
+static int palmas_gpadc_start_mask_interrupt(struct palmas_gpadc *adc,
+						bool mask)
+{
+	int ret;
+
+	if (!mask)
+		ret = palmas_update_bits(adc->palmas, PALMAS_INTERRUPT_BASE,
+					PALMAS_INT3_MASK,
+					PALMAS_INT3_MASK_GPADC_EOC_SW, 0);
+	else
+		ret = palmas_update_bits(adc->palmas, PALMAS_INTERRUPT_BASE,
+					PALMAS_INT3_MASK,
+					PALMAS_INT3_MASK_GPADC_EOC_SW,
+					PALMAS_INT3_MASK_GPADC_EOC_SW);
+	if (ret < 0)
+		dev_err(adc->dev, "GPADC INT MASK update failed: %d\n", ret);
+
+	return ret;
+}
+
+static int palmas_gpadc_enable(struct palmas_gpadc *adc, int adc_chan,
+			       int enable)
+{
+	unsigned int mask, val;
+	int ret;
+
+	if (enable) {
+		val = (adc->extended_delay
+			<< PALMAS_GPADC_RT_CTRL_EXTEND_DELAY_SHIFT);
+		ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+					PALMAS_GPADC_RT_CTRL,
+					PALMAS_GPADC_RT_CTRL_EXTEND_DELAY, val);
+		if (ret < 0) {
+			dev_err(adc->dev, "RT_CTRL update failed: %d\n", ret);
+			return ret;
+		}
+
+		mask = (PALMAS_GPADC_CTRL1_CURRENT_SRC_CH0_MASK |
+			PALMAS_GPADC_CTRL1_CURRENT_SRC_CH3_MASK |
+			PALMAS_GPADC_CTRL1_GPADC_FORCE);
+		val = (adc->ch0_current
+			<< PALMAS_GPADC_CTRL1_CURRENT_SRC_CH0_SHIFT);
+		val |= (adc->ch3_current
+			<< PALMAS_GPADC_CTRL1_CURRENT_SRC_CH3_SHIFT);
+		val |= PALMAS_GPADC_CTRL1_GPADC_FORCE;
+		ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_CTRL1, mask, val);
+		if (ret < 0) {
+			dev_err(adc->dev,
+				"Failed to update current setting: %d\n", ret);
+			return ret;
+		}
+
+		mask = (PALMAS_GPADC_SW_SELECT_SW_CONV0_SEL_MASK |
+			PALMAS_GPADC_SW_SELECT_SW_CONV_EN);
+		val = (adc_chan | PALMAS_GPADC_SW_SELECT_SW_CONV_EN);
+		ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_SW_SELECT, mask, val);
+		if (ret < 0) {
+			dev_err(adc->dev, "SW_SELECT update failed: %d\n", ret);
+			return ret;
+		}
+	} else {
+		ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_SW_SELECT, 0);
+		if (ret < 0)
+			dev_err(adc->dev, "SW_SELECT write failed: %d\n", ret);
+
+		ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_CTRL1,
+				PALMAS_GPADC_CTRL1_GPADC_FORCE, 0);
+		if (ret < 0) {
+			dev_err(adc->dev, "CTRL1 update failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+static int palmas_gpadc_read_prepare(struct palmas_gpadc *adc, int adc_chan)
+{
+	int ret;
+
+	ret = palmas_gpadc_enable(adc, adc_chan, true);
+	if (ret < 0)
+		return ret;
+
+	return palmas_gpadc_start_mask_interrupt(adc, 0);
+}
+
+static void palmas_gpadc_read_done(struct palmas_gpadc *adc, int adc_chan)
+{
+	palmas_gpadc_start_mask_interrupt(adc, 1);
+	palmas_gpadc_enable(adc, adc_chan, false);
+}
+
+static int palmas_gpadc_calibrate(struct palmas_gpadc *adc, int adc_chan)
+{
+	int k;
+	int d1;
+	int d2;
+	int ret;
+	int gain;
+	int x1 =  adc->adc_info[adc_chan].x1;
+	int x2 =  adc->adc_info[adc_chan].x2;
+	int v1 = adc->adc_info[adc_chan].v1;
+	int v2 = adc->adc_info[adc_chan].v2;
+
+	ret = palmas_read(adc->palmas, PALMAS_TRIM_GPADC_BASE,
+				adc->adc_info[adc_chan].trim1_reg, &d1);
+	if (ret < 0) {
+		dev_err(adc->dev, "TRIM read failed: %d\n", ret);
+		goto scrub;
+	}
+
+	ret = palmas_read(adc->palmas, PALMAS_TRIM_GPADC_BASE,
+				adc->adc_info[adc_chan].trim2_reg, &d2);
+	if (ret < 0) {
+		dev_err(adc->dev, "TRIM read failed: %d\n", ret);
+		goto scrub;
+	}
+
+	/* gain error calculation */
+	k = (1000 + (1000 * (d2 - d1)) / (x2 - x1));
+
+	/* gain calculation */
+	gain = ((v2 - v1) * 1000) / (x2 - x1);
+
+	adc->adc_info[adc_chan].gain_error = k;
+	adc->adc_info[adc_chan].gain = gain;
+	/* offset Calculation */
+	adc->adc_info[adc_chan].offset = (d1 * 1000) - ((k - 1000) * x1);
+
+scrub:
+	return ret;
+}
+
+static int palmas_gpadc_start_conversion(struct palmas_gpadc *adc, int adc_chan)
+{
+	unsigned int val;
+	int ret;
+
+	init_completion(&adc->conv_completion);
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_SW_SELECT,
+				PALMAS_GPADC_SW_SELECT_SW_START_CONV0,
+				PALMAS_GPADC_SW_SELECT_SW_START_CONV0);
+	if (ret < 0) {
+		dev_err(adc->dev, "SELECT_SW_START write failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = wait_for_completion_timeout(&adc->conv_completion,
+				PALMAS_ADC_CONVERSION_TIMEOUT);
+	if (ret == 0) {
+		dev_err(adc->dev, "conversion not completed\n");
+		return -ETIMEDOUT;
+	}
+
+	ret = palmas_bulk_read(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_SW_CONV0_LSB, &val, 2);
+	if (ret < 0) {
+		dev_err(adc->dev, "SW_CONV0_LSB read failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = val & 0xFFF;
+
+	return ret;
+}
+
+static int palmas_gpadc_get_calibrated_code(struct palmas_gpadc *adc,
+						int adc_chan, int val)
+{
+	if (!adc->adc_info[adc_chan].is_uncalibrated)
+		val  = (val*1000 - adc->adc_info[adc_chan].offset) /
+					adc->adc_info[adc_chan].gain_error;
+
+	if (val < 0) {
+		dev_err(adc->dev, "Mismatch with calibration\n");
+		return 0;
+	}
+
+	val = (val * adc->adc_info[adc_chan].gain) / 1000;
+
+	return val;
+}
+
+static int palmas_gpadc_read_raw(struct iio_dev *indio_dev,
+	struct iio_chan_spec const *chan, int *val, int *val2, long mask)
+{
+	struct  palmas_gpadc *adc = iio_priv(indio_dev);
+	int adc_chan = chan->channel;
+	int ret = 0;
+
+	if (adc_chan > PALMAS_ADC_CH_MAX)
+		return -EINVAL;
+
+	mutex_lock(&indio_dev->mlock);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_PROCESSED:
+		ret = palmas_gpadc_read_prepare(adc, adc_chan);
+		if (ret < 0)
+			goto out;
+
+		ret = palmas_gpadc_start_conversion(adc, adc_chan);
+		if (ret < 0) {
+			dev_err(adc->dev,
+			"ADC start conversion failed\n");
+			goto out;
+		}
+
+		if (mask == IIO_CHAN_INFO_PROCESSED)
+			ret = palmas_gpadc_get_calibrated_code(
+							adc, adc_chan, ret);
+
+		*val = ret;
+
+		ret = IIO_VAL_INT;
+		goto out;
+	}
+
+	mutex_unlock(&indio_dev->mlock);
+	return ret;
+
+out:
+	palmas_gpadc_read_done(adc, adc_chan);
+	mutex_unlock(&indio_dev->mlock);
+
+	return ret;
+}
+
+static const struct iio_info palmas_gpadc_iio_info = {
+	.read_raw = palmas_gpadc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define PALMAS_ADC_CHAN_IIO(chan, _type, chan_info)	\
+{							\
+	.datasheet_name = PALMAS_DATASHEET_NAME(chan),	\
+	.type = _type,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |	\
+			BIT(chan_info),			\
+	.indexed = 1,					\
+	.channel = PALMAS_ADC_CH_##chan,		\
+}
+
+static const struct iio_chan_spec palmas_gpadc_iio_channel[] = {
+	PALMAS_ADC_CHAN_IIO(IN0, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN1, IIO_TEMP, IIO_CHAN_INFO_RAW),
+	PALMAS_ADC_CHAN_IIO(IN2, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN3, IIO_TEMP, IIO_CHAN_INFO_RAW),
+	PALMAS_ADC_CHAN_IIO(IN4, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN5, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN6, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN7, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN8, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN9, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN10, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN11, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN12, IIO_TEMP, IIO_CHAN_INFO_RAW),
+	PALMAS_ADC_CHAN_IIO(IN13, IIO_TEMP, IIO_CHAN_INFO_RAW),
+	PALMAS_ADC_CHAN_IIO(IN14, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+	PALMAS_ADC_CHAN_IIO(IN15, IIO_VOLTAGE, IIO_CHAN_INFO_PROCESSED),
+};
+
+static int palmas_gpadc_get_adc_dt_data(struct platform_device *pdev,
+	struct palmas_gpadc_platform_data **gpadc_pdata)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct palmas_gpadc_platform_data *gp_data;
+	int ret;
+	u32 pval;
+
+	gp_data = devm_kzalloc(&pdev->dev, sizeof(*gp_data), GFP_KERNEL);
+	if (!gp_data)
+		return -ENOMEM;
+
+	ret = of_property_read_u32(np, "ti,channel0-current-microamp", &pval);
+	if (!ret)
+		gp_data->ch0_current = pval;
+
+	ret = of_property_read_u32(np, "ti,channel3-current-microamp", &pval);
+	if (!ret)
+		gp_data->ch3_current = pval;
+
+	gp_data->extended_delay = of_property_read_bool(np,
+					"ti,enable-extended-delay");
+
+	*gpadc_pdata = gp_data;
+
+	return 0;
+}
+
+static int palmas_gpadc_probe(struct platform_device *pdev)
+{
+	struct palmas_gpadc *adc;
+	struct palmas_platform_data *pdata;
+	struct palmas_gpadc_platform_data *gpadc_pdata = NULL;
+	struct iio_dev *indio_dev;
+	int ret, i;
+
+	pdata = dev_get_platdata(pdev->dev.parent);
+
+	if (pdata && pdata->gpadc_pdata)
+		gpadc_pdata = pdata->gpadc_pdata;
+
+	if (!gpadc_pdata && pdev->dev.of_node) {
+		ret = palmas_gpadc_get_adc_dt_data(pdev, &gpadc_pdata);
+		if (ret < 0)
+			return ret;
+	}
+	if (!gpadc_pdata)
+		return -EINVAL;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*adc));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "iio_device_alloc failed\n");
+		return -ENOMEM;
+	}
+
+	adc = iio_priv(indio_dev);
+	adc->dev = &pdev->dev;
+	adc->palmas = dev_get_drvdata(pdev->dev.parent);
+	adc->adc_info = palmas_gpadc_info;
+	init_completion(&adc->conv_completion);
+	dev_set_drvdata(&pdev->dev, indio_dev);
+
+	adc->auto_conversion_period = gpadc_pdata->auto_conversion_period_ms;
+	adc->irq = palmas_irq_get_virq(adc->palmas, PALMAS_GPADC_EOC_SW_IRQ);
+	if (adc->irq < 0) {
+		dev_err(adc->dev,
+			"get virq failed: %d\n", adc->irq);
+		ret = adc->irq;
+		goto out;
+	}
+	ret = request_threaded_irq(adc->irq, NULL,
+		palmas_gpadc_irq,
+		IRQF_ONESHOT | IRQF_EARLY_RESUME, dev_name(adc->dev),
+		adc);
+	if (ret < 0) {
+		dev_err(adc->dev,
+			"request irq %d failed: %d\n", adc->irq, ret);
+		goto out;
+	}
+
+	if (gpadc_pdata->adc_wakeup1_data) {
+		memcpy(&adc->wakeup1_data, gpadc_pdata->adc_wakeup1_data,
+			sizeof(adc->wakeup1_data));
+		adc->wakeup1_enable = true;
+		adc->irq_auto_0 =  platform_get_irq(pdev, 1);
+		ret = request_threaded_irq(adc->irq_auto_0, NULL,
+				palmas_gpadc_irq_auto,
+				IRQF_ONESHOT | IRQF_EARLY_RESUME,
+				"palmas-adc-auto-0", adc);
+		if (ret < 0) {
+			dev_err(adc->dev, "request auto0 irq %d failed: %d\n",
+				adc->irq_auto_0, ret);
+			goto out_irq_free;
+		}
+	}
+
+	if (gpadc_pdata->adc_wakeup2_data) {
+		memcpy(&adc->wakeup2_data, gpadc_pdata->adc_wakeup2_data,
+				sizeof(adc->wakeup2_data));
+		adc->wakeup2_enable = true;
+		adc->irq_auto_1 =  platform_get_irq(pdev, 2);
+		ret = request_threaded_irq(adc->irq_auto_1, NULL,
+				palmas_gpadc_irq_auto,
+				IRQF_ONESHOT | IRQF_EARLY_RESUME,
+				"palmas-adc-auto-1", adc);
+		if (ret < 0) {
+			dev_err(adc->dev, "request auto1 irq %d failed: %d\n",
+				adc->irq_auto_1, ret);
+			goto out_irq_auto0_free;
+		}
+	}
+
+	/* set the current source 0 (value 0/5/15/20 uA => 0..3) */
+	if (gpadc_pdata->ch0_current <= 1)
+		adc->ch0_current = PALMAS_ADC_CH0_CURRENT_SRC_0;
+	else if (gpadc_pdata->ch0_current <= 5)
+		adc->ch0_current = PALMAS_ADC_CH0_CURRENT_SRC_5;
+	else if (gpadc_pdata->ch0_current <= 15)
+		adc->ch0_current = PALMAS_ADC_CH0_CURRENT_SRC_15;
+	else
+		adc->ch0_current = PALMAS_ADC_CH0_CURRENT_SRC_20;
+
+	/* set the current source 3 (value 0/10/400/800 uA => 0..3) */
+	if (gpadc_pdata->ch3_current <= 1)
+		adc->ch3_current = PALMAS_ADC_CH3_CURRENT_SRC_0;
+	else if (gpadc_pdata->ch3_current <= 10)
+		adc->ch3_current = PALMAS_ADC_CH3_CURRENT_SRC_10;
+	else if (gpadc_pdata->ch3_current <= 400)
+		adc->ch3_current = PALMAS_ADC_CH3_CURRENT_SRC_400;
+	else
+		adc->ch3_current = PALMAS_ADC_CH3_CURRENT_SRC_800;
+
+	adc->extended_delay = gpadc_pdata->extended_delay;
+
+	indio_dev->name = MOD_NAME;
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->info = &palmas_gpadc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = palmas_gpadc_iio_channel;
+	indio_dev->num_channels = ARRAY_SIZE(palmas_gpadc_iio_channel);
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(adc->dev, "iio_device_register() failed: %d\n", ret);
+		goto out_irq_auto1_free;
+	}
+
+	device_set_wakeup_capable(&pdev->dev, 1);
+	for (i = 0; i < PALMAS_ADC_CH_MAX; i++) {
+		if (!(adc->adc_info[i].is_uncalibrated))
+			palmas_gpadc_calibrate(adc, i);
+	}
+
+	if (adc->wakeup1_enable || adc->wakeup2_enable)
+		device_wakeup_enable(&pdev->dev);
+
+	return 0;
+
+out_irq_auto1_free:
+	if (gpadc_pdata->adc_wakeup2_data)
+		free_irq(adc->irq_auto_1, adc);
+out_irq_auto0_free:
+	if (gpadc_pdata->adc_wakeup1_data)
+		free_irq(adc->irq_auto_0, adc);
+out_irq_free:
+	free_irq(adc->irq, adc);
+out:
+	return ret;
+}
+
+static int palmas_gpadc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(&pdev->dev);
+	struct palmas_gpadc *adc = iio_priv(indio_dev);
+
+	if (adc->wakeup1_enable || adc->wakeup2_enable)
+		device_wakeup_disable(&pdev->dev);
+	iio_device_unregister(indio_dev);
+	free_irq(adc->irq, adc);
+	if (adc->wakeup1_enable)
+		free_irq(adc->irq_auto_0, adc);
+	if (adc->wakeup2_enable)
+		free_irq(adc->irq_auto_1, adc);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int palmas_adc_wakeup_configure(struct palmas_gpadc *adc)
+{
+	int adc_period, conv;
+	int i;
+	int ch0 = 0, ch1 = 0;
+	int thres;
+	int ret;
+
+	adc_period = adc->auto_conversion_period;
+	for (i = 0; i < 16; ++i) {
+		if (((1000 * (1 << i)) / 32) < adc_period)
+			continue;
+	}
+	if (i > 0)
+		i--;
+	adc_period = i;
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_AUTO_CTRL,
+			PALMAS_GPADC_AUTO_CTRL_COUNTER_CONV_MASK,
+			adc_period);
+	if (ret < 0) {
+		dev_err(adc->dev, "AUTO_CTRL write failed: %d\n", ret);
+		return ret;
+	}
+
+	conv = 0;
+	if (adc->wakeup1_enable) {
+		int polarity;
+
+		ch0 = adc->wakeup1_data.adc_channel_number;
+		conv |= PALMAS_GPADC_AUTO_CTRL_AUTO_CONV0_EN;
+		if (adc->wakeup1_data.adc_high_threshold > 0) {
+			thres = adc->wakeup1_data.adc_high_threshold;
+			polarity = 0;
+		} else {
+			thres = adc->wakeup1_data.adc_low_threshold;
+			polarity = PALMAS_GPADC_THRES_CONV0_MSB_THRES_CONV0_POL;
+		}
+
+		ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_THRES_CONV0_LSB, thres & 0xFF);
+		if (ret < 0) {
+			dev_err(adc->dev,
+				"THRES_CONV0_LSB write failed: %d\n", ret);
+			return ret;
+		}
+
+		ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_THRES_CONV0_MSB,
+				((thres >> 8) & 0xF) | polarity);
+		if (ret < 0) {
+			dev_err(adc->dev,
+				"THRES_CONV0_MSB write failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	if (adc->wakeup2_enable) {
+		int polarity;
+
+		ch1 = adc->wakeup2_data.adc_channel_number;
+		conv |= PALMAS_GPADC_AUTO_CTRL_AUTO_CONV1_EN;
+		if (adc->wakeup2_data.adc_high_threshold > 0) {
+			thres = adc->wakeup2_data.adc_high_threshold;
+			polarity = 0;
+		} else {
+			thres = adc->wakeup2_data.adc_low_threshold;
+			polarity = PALMAS_GPADC_THRES_CONV1_MSB_THRES_CONV1_POL;
+		}
+
+		ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_THRES_CONV1_LSB, thres & 0xFF);
+		if (ret < 0) {
+			dev_err(adc->dev,
+				"THRES_CONV1_LSB write failed: %d\n", ret);
+			return ret;
+		}
+
+		ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+				PALMAS_GPADC_THRES_CONV1_MSB,
+				((thres >> 8) & 0xF) | polarity);
+		if (ret < 0) {
+			dev_err(adc->dev,
+				"THRES_CONV1_MSB write failed: %d\n", ret);
+			return ret;
+		}
+	}
+
+	ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_AUTO_SELECT, (ch1 << 4) | ch0);
+	if (ret < 0) {
+		dev_err(adc->dev, "AUTO_SELECT write failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = palmas_update_bits(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_AUTO_CTRL,
+			PALMAS_GPADC_AUTO_CTRL_AUTO_CONV1_EN |
+			PALMAS_GPADC_AUTO_CTRL_AUTO_CONV0_EN, conv);
+	if (ret < 0)
+		dev_err(adc->dev, "AUTO_CTRL write failed: %d\n", ret);
+
+	return ret;
+}
+
+static int palmas_adc_wakeup_reset(struct palmas_gpadc *adc)
+{
+	int ret;
+
+	ret = palmas_write(adc->palmas, PALMAS_GPADC_BASE,
+			PALMAS_GPADC_AUTO_SELECT, 0);
+	if (ret < 0) {
+		dev_err(adc->dev, "AUTO_SELECT write failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = palmas_disable_auto_conversion(adc);
+	if (ret < 0)
+		dev_err(adc->dev, "Disable auto conversion failed: %d\n", ret);
+
+	return ret;
+}
+
+static int palmas_gpadc_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct palmas_gpadc *adc = iio_priv(indio_dev);
+	int wakeup = adc->wakeup1_enable || adc->wakeup2_enable;
+	int ret;
+
+	if (!device_may_wakeup(dev) || !wakeup)
+		return 0;
+
+	ret = palmas_adc_wakeup_configure(adc);
+	if (ret < 0)
+		return ret;
+
+	if (adc->wakeup1_enable)
+		enable_irq_wake(adc->irq_auto_0);
+
+	if (adc->wakeup2_enable)
+		enable_irq_wake(adc->irq_auto_1);
+
+	return 0;
+}
+
+static int palmas_gpadc_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct palmas_gpadc *adc = iio_priv(indio_dev);
+	int wakeup = adc->wakeup1_enable || adc->wakeup2_enable;
+	int ret;
+
+	if (!device_may_wakeup(dev) || !wakeup)
+		return 0;
+
+	ret = palmas_adc_wakeup_reset(adc);
+	if (ret < 0)
+		return ret;
+
+	if (adc->wakeup1_enable)
+		disable_irq_wake(adc->irq_auto_0);
+
+	if (adc->wakeup2_enable)
+		disable_irq_wake(adc->irq_auto_1);
+
+	return 0;
+};
+#endif
+
+static const struct dev_pm_ops palmas_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(palmas_gpadc_suspend,
+				palmas_gpadc_resume)
+};
+
+static const struct of_device_id of_palmas_gpadc_match_tbl[] = {
+	{ .compatible = "ti,palmas-gpadc", },
+	{ /* end */ }
+};
+MODULE_DEVICE_TABLE(of, of_palmas_gpadc_match_tbl);
+
+static struct platform_driver palmas_gpadc_driver = {
+	.probe = palmas_gpadc_probe,
+	.remove = palmas_gpadc_remove,
+	.driver = {
+		.name = MOD_NAME,
+		.pm = &palmas_pm_ops,
+		.of_match_table = of_palmas_gpadc_match_tbl,
+	},
+};
+
+static int __init palmas_gpadc_init(void)
+{
+	return platform_driver_register(&palmas_gpadc_driver);
+}
+module_init(palmas_gpadc_init);
+
+static void __exit palmas_gpadc_exit(void)
+{
+	platform_driver_unregister(&palmas_gpadc_driver);
+}
+module_exit(palmas_gpadc_exit);
+
+MODULE_DESCRIPTION("palmas GPADC driver");
+MODULE_AUTHOR("Pradeep Goudagunta<pgoudagunta@nvidia.com>");
+MODULE_ALIAS("platform:palmas-gpadc");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti-adc128s052.c b/drivers/iio/adc/ti-adc128s052.c
index ff6f7f63c8d9..bc58867d6e8d 100644
--- a/drivers/iio/adc/ti-adc128s052.c
+++ b/drivers/iio/adc/ti-adc128s052.c
@@ -1,10 +1,11 @@
 /*
  * Copyright (C) 2014 Angelo Compagnucci <angelo.compagnucci@gmail.com>
  *
- * Driver for Texas Instruments' ADC128S052 and ADC122S021 ADC chip.
+ * Driver for Texas Instruments' ADC128S052, ADC122S021 and ADC124S021 ADC chip.
  * Datasheets can be found here:
  * http://www.ti.com/lit/ds/symlink/adc128s052.pdf
  * http://www.ti.com/lit/ds/symlink/adc122s021.pdf
+ * http://www.ti.com/lit/ds/symlink/adc124s021.pdf
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -114,9 +115,17 @@ static const struct iio_chan_spec adc122s021_channels[] = {
 	ADC128_VOLTAGE_CHANNEL(1),
 };
 
+static const struct iio_chan_spec adc124s021_channels[] = {
+	ADC128_VOLTAGE_CHANNEL(0),
+	ADC128_VOLTAGE_CHANNEL(1),
+	ADC128_VOLTAGE_CHANNEL(2),
+	ADC128_VOLTAGE_CHANNEL(3),
+};
+
 static const struct adc128_configuration adc128_config[] = {
 	{ adc128s052_channels, ARRAY_SIZE(adc128s052_channels) },
 	{ adc122s021_channels, ARRAY_SIZE(adc122s021_channels) },
+	{ adc124s021_channels, ARRAY_SIZE(adc124s021_channels) },
 };
 
 static const struct iio_info adc128_info = {
@@ -177,6 +186,7 @@ static int adc128_remove(struct spi_device *spi)
 static const struct of_device_id adc128_of_match[] = {
 	{ .compatible = "ti,adc128s052", },
 	{ .compatible = "ti,adc122s021", },
+	{ .compatible = "ti,adc124s021", },
 	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, adc128_of_match);
@@ -184,6 +194,7 @@ MODULE_DEVICE_TABLE(of, adc128_of_match);
 static const struct spi_device_id adc128_id[] = {
 	{ "adc128s052", 0},	/* index into adc128_config */
 	{ "adc122s021",	1},
+	{ "adc124s021", 2},
 	{ }
 };
 MODULE_DEVICE_TABLE(spi, adc128_id);
diff --git a/drivers/iio/adc/ti-ads8688.c b/drivers/iio/adc/ti-ads8688.c
new file mode 100644
index 000000000000..03e907028cb6
--- /dev/null
+++ b/drivers/iio/adc/ti-ads8688.c
@@ -0,0 +1,486 @@
+/*
+ * Copyright (C) 2015 Prevas A/S
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define ADS8688_CMD_REG(x)		(x << 8)
+#define ADS8688_CMD_REG_NOOP		0x00
+#define ADS8688_CMD_REG_RST		0x85
+#define ADS8688_CMD_REG_MAN_CH(chan)	(0xC0 | (4 * chan))
+#define ADS8688_CMD_DONT_CARE_BITS	16
+
+#define ADS8688_PROG_REG(x)		(x << 9)
+#define ADS8688_PROG_REG_RANGE_CH(chan)	(0x05 + chan)
+#define ADS8688_PROG_WR_BIT		BIT(8)
+#define ADS8688_PROG_DONT_CARE_BITS	8
+
+#define ADS8688_REG_PLUSMINUS25VREF	0
+#define ADS8688_REG_PLUSMINUS125VREF	1
+#define ADS8688_REG_PLUSMINUS0625VREF	2
+#define ADS8688_REG_PLUS25VREF		5
+#define ADS8688_REG_PLUS125VREF		6
+
+#define ADS8688_VREF_MV			4096
+#define ADS8688_REALBITS		16
+
+/*
+ * enum ads8688_range - ADS8688 reference voltage range
+ * @ADS8688_PLUSMINUS25VREF: Device is configured for input range ±2.5 * VREF
+ * @ADS8688_PLUSMINUS125VREF: Device is configured for input range ±1.25 * VREF
+ * @ADS8688_PLUSMINUS0625VREF: Device is configured for input range ±0.625 * VREF
+ * @ADS8688_PLUS25VREF: Device is configured for input range 0 - 2.5 * VREF
+ * @ADS8688_PLUS125VREF: Device is configured for input range 0 - 1.25 * VREF
+ */
+enum ads8688_range {
+	ADS8688_PLUSMINUS25VREF,
+	ADS8688_PLUSMINUS125VREF,
+	ADS8688_PLUSMINUS0625VREF,
+	ADS8688_PLUS25VREF,
+	ADS8688_PLUS125VREF,
+};
+
+struct ads8688_chip_info {
+	const struct iio_chan_spec *channels;
+	unsigned int num_channels;
+};
+
+struct ads8688_state {
+	struct mutex			lock;
+	const struct ads8688_chip_info	*chip_info;
+	struct spi_device		*spi;
+	struct regulator		*reg;
+	unsigned int			vref_mv;
+	enum ads8688_range		range[8];
+	union {
+		__be32 d32;
+		u8 d8[4];
+	} data[2] ____cacheline_aligned;
+};
+
+enum ads8688_id {
+	ID_ADS8684,
+	ID_ADS8688,
+};
+
+struct ads8688_ranges {
+	enum ads8688_range range;
+	unsigned int scale;
+	int offset;
+	u8 reg;
+};
+
+static const struct ads8688_ranges ads8688_range_def[5] = {
+	{
+		.range = ADS8688_PLUSMINUS25VREF,
+		.scale = 76295,
+		.offset = -(1 << (ADS8688_REALBITS - 1)),
+		.reg = ADS8688_REG_PLUSMINUS25VREF,
+	}, {
+		.range = ADS8688_PLUSMINUS125VREF,
+		.scale = 38148,
+		.offset = -(1 << (ADS8688_REALBITS - 1)),
+		.reg = ADS8688_REG_PLUSMINUS125VREF,
+	}, {
+		.range = ADS8688_PLUSMINUS0625VREF,
+		.scale = 19074,
+		.offset = -(1 << (ADS8688_REALBITS - 1)),
+		.reg = ADS8688_REG_PLUSMINUS0625VREF,
+	}, {
+		.range = ADS8688_PLUS25VREF,
+		.scale = 38148,
+		.offset = 0,
+		.reg = ADS8688_REG_PLUS25VREF,
+	}, {
+		.range = ADS8688_PLUS125VREF,
+		.scale = 19074,
+		.offset = 0,
+		.reg = ADS8688_REG_PLUS125VREF,
+	}
+};
+
+static ssize_t ads8688_show_scales(struct device *dev,
+				   struct device_attribute *attr, char *buf)
+{
+	struct ads8688_state *st = iio_priv(dev_to_iio_dev(dev));
+
+	return sprintf(buf, "0.%09u 0.%09u 0.%09u\n",
+		       ads8688_range_def[0].scale * st->vref_mv,
+		       ads8688_range_def[1].scale * st->vref_mv,
+		       ads8688_range_def[2].scale * st->vref_mv);
+}
+
+static ssize_t ads8688_show_offsets(struct device *dev,
+				    struct device_attribute *attr, char *buf)
+{
+	return sprintf(buf, "%d %d\n", ads8688_range_def[0].offset,
+		       ads8688_range_def[3].offset);
+}
+
+static IIO_DEVICE_ATTR(in_voltage_scale_available, S_IRUGO,
+		       ads8688_show_scales, NULL, 0);
+static IIO_DEVICE_ATTR(in_voltage_offset_available, S_IRUGO,
+		       ads8688_show_offsets, NULL, 0);
+
+static struct attribute *ads8688_attributes[] = {
+	&iio_dev_attr_in_voltage_scale_available.dev_attr.attr,
+	&iio_dev_attr_in_voltage_offset_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group ads8688_attribute_group = {
+	.attrs = ads8688_attributes,
+};
+
+#define ADS8688_CHAN(index)					\
+{								\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = index,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW)		\
+			      | BIT(IIO_CHAN_INFO_SCALE)	\
+			      | BIT(IIO_CHAN_INFO_OFFSET),	\
+}
+
+static const struct iio_chan_spec ads8684_channels[] = {
+	ADS8688_CHAN(0),
+	ADS8688_CHAN(1),
+	ADS8688_CHAN(2),
+	ADS8688_CHAN(3),
+};
+
+static const struct iio_chan_spec ads8688_channels[] = {
+	ADS8688_CHAN(0),
+	ADS8688_CHAN(1),
+	ADS8688_CHAN(2),
+	ADS8688_CHAN(3),
+	ADS8688_CHAN(4),
+	ADS8688_CHAN(5),
+	ADS8688_CHAN(6),
+	ADS8688_CHAN(7),
+};
+
+static int ads8688_prog_write(struct iio_dev *indio_dev, unsigned int addr,
+			      unsigned int val)
+{
+	struct ads8688_state *st = iio_priv(indio_dev);
+	u32 tmp;
+
+	tmp = ADS8688_PROG_REG(addr) | ADS8688_PROG_WR_BIT | val;
+	tmp <<= ADS8688_PROG_DONT_CARE_BITS;
+	st->data[0].d32 = cpu_to_be32(tmp);
+
+	return spi_write(st->spi, &st->data[0].d8[1], 3);
+}
+
+static int ads8688_reset(struct iio_dev *indio_dev)
+{
+	struct ads8688_state *st = iio_priv(indio_dev);
+	u32 tmp;
+
+	tmp = ADS8688_CMD_REG(ADS8688_CMD_REG_RST);
+	tmp <<= ADS8688_CMD_DONT_CARE_BITS;
+	st->data[0].d32 = cpu_to_be32(tmp);
+
+	return spi_write(st->spi, &st->data[0].d8[0], 4);
+}
+
+static int ads8688_read(struct iio_dev *indio_dev, unsigned int chan)
+{
+	struct ads8688_state *st = iio_priv(indio_dev);
+	int ret;
+	u32 tmp;
+	struct spi_transfer t[] = {
+		{
+			.tx_buf = &st->data[0].d8[0],
+			.len = 4,
+			.cs_change = 1,
+		}, {
+			.tx_buf = &st->data[1].d8[0],
+			.rx_buf = &st->data[1].d8[0],
+			.len = 4,
+		},
+	};
+
+	tmp = ADS8688_CMD_REG(ADS8688_CMD_REG_MAN_CH(chan));
+	tmp <<= ADS8688_CMD_DONT_CARE_BITS;
+	st->data[0].d32 = cpu_to_be32(tmp);
+
+	tmp = ADS8688_CMD_REG(ADS8688_CMD_REG_NOOP);
+	tmp <<= ADS8688_CMD_DONT_CARE_BITS;
+	st->data[1].d32 = cpu_to_be32(tmp);
+
+	ret = spi_sync_transfer(st->spi, t, ARRAY_SIZE(t));
+	if (ret < 0)
+		return ret;
+
+	return be32_to_cpu(st->data[1].d32) & 0xffff;
+}
+
+static int ads8688_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int *val, int *val2, long m)
+{
+	int ret, offset;
+	unsigned long scale_mv;
+
+	struct ads8688_state *st = iio_priv(indio_dev);
+
+	mutex_lock(&st->lock);
+	switch (m) {
+	case IIO_CHAN_INFO_RAW:
+		ret = ads8688_read(indio_dev, chan->channel);
+		mutex_unlock(&st->lock);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		scale_mv = st->vref_mv;
+		scale_mv *= ads8688_range_def[st->range[chan->channel]].scale;
+		*val = 0;
+		*val2 = scale_mv;
+		mutex_unlock(&st->lock);
+		return IIO_VAL_INT_PLUS_NANO;
+	case IIO_CHAN_INFO_OFFSET:
+		offset = ads8688_range_def[st->range[chan->channel]].offset;
+		*val = offset;
+		mutex_unlock(&st->lock);
+		return IIO_VAL_INT;
+	}
+	mutex_unlock(&st->lock);
+
+	return -EINVAL;
+}
+
+static int ads8688_write_reg_range(struct iio_dev *indio_dev,
+				   struct iio_chan_spec const *chan,
+				   enum ads8688_range range)
+{
+	unsigned int tmp;
+	int ret;
+
+	tmp = ADS8688_PROG_REG_RANGE_CH(chan->channel);
+	ret = ads8688_prog_write(indio_dev, tmp, range);
+
+	return ret;
+}
+
+static int ads8688_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	struct ads8688_state *st = iio_priv(indio_dev);
+	unsigned int scale = 0;
+	int ret = -EINVAL, i, offset = 0;
+
+	mutex_lock(&st->lock);
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		/* If the offset is 0 the ±2.5 * VREF mode is not available */
+		offset = ads8688_range_def[st->range[chan->channel]].offset;
+		if (offset == 0 && val2 == ads8688_range_def[0].scale * st->vref_mv) {
+			mutex_unlock(&st->lock);
+			return -EINVAL;
+		}
+
+		/* Lookup new mode */
+		for (i = 0; i < ARRAY_SIZE(ads8688_range_def); i++)
+			if (val2 == ads8688_range_def[i].scale * st->vref_mv &&
+			    offset == ads8688_range_def[i].offset) {
+				ret = ads8688_write_reg_range(indio_dev, chan,
+					ads8688_range_def[i].reg);
+				break;
+			}
+		break;
+	case IIO_CHAN_INFO_OFFSET:
+		/*
+		 * There are only two available offsets:
+		 * 0 and -(1 << (ADS8688_REALBITS - 1))
+		 */
+		if (!(ads8688_range_def[0].offset == val ||
+		    ads8688_range_def[3].offset == val)) {
+			mutex_unlock(&st->lock);
+			return -EINVAL;
+		}
+
+		/*
+		 * If the device are in ±2.5 * VREF mode, it's not allowed to
+		 * switch to a mode where the offset is 0
+		 */
+		if (val == 0 &&
+		    st->range[chan->channel] == ADS8688_PLUSMINUS25VREF) {
+			mutex_unlock(&st->lock);
+			return -EINVAL;
+		}
+
+		scale = ads8688_range_def[st->range[chan->channel]].scale;
+
+		/* Lookup new mode */
+		for (i = 0; i < ARRAY_SIZE(ads8688_range_def); i++)
+			if (val == ads8688_range_def[i].offset &&
+			    scale == ads8688_range_def[i].scale) {
+				ret = ads8688_write_reg_range(indio_dev, chan,
+					ads8688_range_def[i].reg);
+				break;
+			}
+		break;
+	}
+
+	if (!ret)
+		st->range[chan->channel] = ads8688_range_def[i].range;
+
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
+static int ads8688_write_raw_get_fmt(struct iio_dev *indio_dev,
+				     struct iio_chan_spec const *chan,
+				     long mask)
+{
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		return IIO_VAL_INT_PLUS_NANO;
+	case IIO_CHAN_INFO_OFFSET:
+		return IIO_VAL_INT;
+	}
+
+	return -EINVAL;
+}
+
+static const struct iio_info ads8688_info = {
+	.read_raw = &ads8688_read_raw,
+	.write_raw = &ads8688_write_raw,
+	.write_raw_get_fmt = &ads8688_write_raw_get_fmt,
+	.attrs = &ads8688_attribute_group,
+	.driver_module = THIS_MODULE,
+};
+
+static const struct ads8688_chip_info ads8688_chip_info_tbl[] = {
+	[ID_ADS8684] = {
+		.channels = ads8684_channels,
+		.num_channels = ARRAY_SIZE(ads8684_channels),
+	},
+	[ID_ADS8688] = {
+		.channels = ads8688_channels,
+		.num_channels = ARRAY_SIZE(ads8688_channels),
+	},
+};
+
+static int ads8688_probe(struct spi_device *spi)
+{
+	struct ads8688_state *st;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+
+	st->reg = devm_regulator_get_optional(&spi->dev, "vref");
+	if (!IS_ERR(st->reg)) {
+		ret = regulator_enable(st->reg);
+		if (ret)
+			return ret;
+
+		ret = regulator_get_voltage(st->reg);
+		if (ret < 0)
+			goto error_out;
+
+		st->vref_mv = ret / 1000;
+	} else {
+		/* Use internal reference */
+		st->vref_mv = ADS8688_VREF_MV;
+	}
+
+	st->chip_info =	&ads8688_chip_info_tbl[spi_get_device_id(spi)->driver_data];
+
+	spi->mode = SPI_MODE_1;
+
+	spi_set_drvdata(spi, indio_dev);
+
+	st->spi = spi;
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = st->chip_info->channels;
+	indio_dev->num_channels = st->chip_info->num_channels;
+	indio_dev->info = &ads8688_info;
+
+	ads8688_reset(indio_dev);
+
+	mutex_init(&st->lock);
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto error_out;
+
+	return 0;
+
+error_out:
+	if (!IS_ERR_OR_NULL(st->reg))
+		regulator_disable(st->reg);
+
+	return ret;
+}
+
+static int ads8688_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct ads8688_state *st = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (!IS_ERR_OR_NULL(st->reg))
+		regulator_disable(st->reg);
+
+	return 0;
+}
+
+static const struct spi_device_id ads8688_id[] = {
+	{"ads8684", ID_ADS8684},
+	{"ads8688", ID_ADS8688},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, ads8688_id);
+
+static const struct of_device_id ads8688_of_match[] = {
+	{ .compatible = "ti,ads8684" },
+	{ .compatible = "ti,ads8688" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ads8688_of_match);
+
+static struct spi_driver ads8688_driver = {
+	.driver = {
+		.name	= "ads8688",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= ads8688_probe,
+	.remove		= ads8688_remove,
+	.id_table	= ads8688_id,
+};
+module_spi_driver(ads8688_driver);
+
+MODULE_AUTHOR("Sean Nyekjaer <sean.nyekjaer@prevas.dk>");
+MODULE_DESCRIPTION("Texas Instruments ADS8688 driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index 942320e32753..c1e05532d437 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -289,7 +289,7 @@ static int tiadc_iio_buffered_hardware_setup(struct iio_dev *indio_dev,
 		goto error_kfifo_free;
 
 	indio_dev->setup_ops = setup_ops;
-	indio_dev->modes |= INDIO_BUFFER_HARDWARE;
+	indio_dev->modes |= INDIO_BUFFER_SOFTWARE;
 
 	return 0;
 
diff --git a/drivers/iio/adc/xilinx-xadc-core.c b/drivers/iio/adc/xilinx-xadc-core.c
index 02e636a1c49a..0a6beb3d99cb 100644
--- a/drivers/iio/adc/xilinx-xadc-core.c
+++ b/drivers/iio/adc/xilinx-xadc-core.c
@@ -803,7 +803,7 @@ err:
 	return ret;
 }
 
-static struct iio_buffer_setup_ops xadc_buffer_ops = {
+static const struct iio_buffer_setup_ops xadc_buffer_ops = {
 	.preenable = &xadc_preenable,
 	.postenable = &iio_triggered_buffer_postenable,
 	.predisable = &iio_triggered_buffer_predisable,
diff --git a/drivers/iio/buffer/Kconfig b/drivers/iio/buffer/Kconfig
index 0a7b2fd3699b..4ffd3db7817f 100644
--- a/drivers/iio/buffer/Kconfig
+++ b/drivers/iio/buffer/Kconfig
@@ -9,6 +9,26 @@ config IIO_BUFFER_CB
 	  Should be selected by any drivers that do in-kernel push
 	  usage.  That is, those where the data is pushed to the consumer.
 
+config IIO_BUFFER_DMA
+	tristate
+	help
+	  Provides the generic IIO DMA buffer infrastructure that can be used by
+	  drivers for devices with DMA support to implement the IIO buffer.
+
+	  Should be selected by drivers that want to use the generic DMA buffer
+	  infrastructure.
+
+config IIO_BUFFER_DMAENGINE
+	tristate
+	select IIO_BUFFER_DMA
+	help
+	  Provides a bonding of the generic IIO DMA buffer infrastructure with the
+	  DMAengine framework. This can be used by converter drivers with a DMA port
+	  connected to an external DMA controller which is supported by the
+	  DMAengine framework.
+
+	  Should be selected by drivers that want to use this functionality.
+
 config IIO_KFIFO_BUF
 	tristate "Industrial I/O buffering based on kfifo"
 	help
diff --git a/drivers/iio/buffer/Makefile b/drivers/iio/buffer/Makefile
index 4d193b9a9123..85beaae831ae 100644
--- a/drivers/iio/buffer/Makefile
+++ b/drivers/iio/buffer/Makefile
@@ -4,5 +4,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_IIO_BUFFER_CB) += industrialio-buffer-cb.o
+obj-$(CONFIG_IIO_BUFFER_DMA) += industrialio-buffer-dma.o
+obj-$(CONFIG_IIO_BUFFER_DMAENGINE) += industrialio-buffer-dmaengine.o
 obj-$(CONFIG_IIO_TRIGGERED_BUFFER) += industrialio-triggered-buffer.o
 obj-$(CONFIG_IIO_KFIFO_BUF) += kfifo_buf.o
diff --git a/drivers/iio/buffer/industrialio-buffer-dma.c b/drivers/iio/buffer/industrialio-buffer-dma.c
new file mode 100644
index 000000000000..212cbedc7abb
--- /dev/null
+++ b/drivers/iio/buffer/industrialio-buffer-dma.c
@@ -0,0 +1,683 @@
+/*
+ * Copyright 2013-2015 Analog Devices Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2.
+ */
+
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/workqueue.h>
+#include <linux/mutex.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/buffer-dma.h>
+#include <linux/dma-mapping.h>
+#include <linux/sizes.h>
+
+/*
+ * For DMA buffers the storage is sub-divided into so called blocks. Each block
+ * has its own memory buffer. The size of the block is the granularity at which
+ * memory is exchanged between the hardware and the application. Increasing the
+ * basic unit of data exchange from one sample to one block decreases the
+ * management overhead that is associated with each sample. E.g. if we say the
+ * management overhead for one exchange is x and the unit of exchange is one
+ * sample the overhead will be x for each sample. Whereas when using a block
+ * which contains n samples the overhead per sample is reduced to x/n. This
+ * allows to achieve much higher samplerates than what can be sustained with
+ * the one sample approach.
+ *
+ * Blocks are exchanged between the DMA controller and the application via the
+ * means of two queues. The incoming queue and the outgoing queue. Blocks on the
+ * incoming queue are waiting for the DMA controller to pick them up and fill
+ * them with data. Block on the outgoing queue have been filled with data and
+ * are waiting for the application to dequeue them and read the data.
+ *
+ * A block can be in one of the following states:
+ *  * Owned by the application. In this state the application can read data from
+ *    the block.
+ *  * On the incoming list: Blocks on the incoming list are queued up to be
+ *    processed by the DMA controller.
+ *  * Owned by the DMA controller: The DMA controller is processing the block
+ *    and filling it with data.
+ *  * On the outgoing list: Blocks on the outgoing list have been successfully
+ *    processed by the DMA controller and contain data. They can be dequeued by
+ *    the application.
+ *  * Dead: A block that is dead has been marked as to be freed. It might still
+ *    be owned by either the application or the DMA controller at the moment.
+ *    But once they are done processing it instead of going to either the
+ *    incoming or outgoing queue the block will be freed.
+ *
+ * In addition to this blocks are reference counted and the memory associated
+ * with both the block structure as well as the storage memory for the block
+ * will be freed when the last reference to the block is dropped. This means a
+ * block must not be accessed without holding a reference.
+ *
+ * The iio_dma_buffer implementation provides a generic infrastructure for
+ * managing the blocks.
+ *
+ * A driver for a specific piece of hardware that has DMA capabilities need to
+ * implement the submit() callback from the iio_dma_buffer_ops structure. This
+ * callback is supposed to initiate the DMA transfer copying data from the
+ * converter to the memory region of the block. Once the DMA transfer has been
+ * completed the driver must call iio_dma_buffer_block_done() for the completed
+ * block.
+ *
+ * Prior to this it must set the bytes_used field of the block contains
+ * the actual number of bytes in the buffer. Typically this will be equal to the
+ * size of the block, but if the DMA hardware has certain alignment requirements
+ * for the transfer length it might choose to use less than the full size. In
+ * either case it is expected that bytes_used is a multiple of the bytes per
+ * datum, i.e. the block must not contain partial samples.
+ *
+ * The driver must call iio_dma_buffer_block_done() for each block it has
+ * received through its submit_block() callback, even if it does not actually
+ * perform a DMA transfer for the block, e.g. because the buffer was disabled
+ * before the block transfer was started. In this case it should set bytes_used
+ * to 0.
+ *
+ * In addition it is recommended that a driver implements the abort() callback.
+ * It will be called when the buffer is disabled and can be used to cancel
+ * pending and stop active transfers.
+ *
+ * The specific driver implementation should use the default callback
+ * implementations provided by this module for the iio_buffer_access_funcs
+ * struct. It may overload some callbacks with custom variants if the hardware
+ * has special requirements that are not handled by the generic functions. If a
+ * driver chooses to overload a callback it has to ensure that the generic
+ * callback is called from within the custom callback.
+ */
+
+static void iio_buffer_block_release(struct kref *kref)
+{
+	struct iio_dma_buffer_block *block = container_of(kref,
+		struct iio_dma_buffer_block, kref);
+
+	WARN_ON(block->state != IIO_BLOCK_STATE_DEAD);
+
+	dma_free_coherent(block->queue->dev, PAGE_ALIGN(block->size),
+					block->vaddr, block->phys_addr);
+
+	iio_buffer_put(&block->queue->buffer);
+	kfree(block);
+}
+
+static void iio_buffer_block_get(struct iio_dma_buffer_block *block)
+{
+	kref_get(&block->kref);
+}
+
+static void iio_buffer_block_put(struct iio_dma_buffer_block *block)
+{
+	kref_put(&block->kref, iio_buffer_block_release);
+}
+
+/*
+ * dma_free_coherent can sleep, hence we need to take some special care to be
+ * able to drop a reference from an atomic context.
+ */
+static LIST_HEAD(iio_dma_buffer_dead_blocks);
+static DEFINE_SPINLOCK(iio_dma_buffer_dead_blocks_lock);
+
+static void iio_dma_buffer_cleanup_worker(struct work_struct *work)
+{
+	struct iio_dma_buffer_block *block, *_block;
+	LIST_HEAD(block_list);
+
+	spin_lock_irq(&iio_dma_buffer_dead_blocks_lock);
+	list_splice_tail_init(&iio_dma_buffer_dead_blocks, &block_list);
+	spin_unlock_irq(&iio_dma_buffer_dead_blocks_lock);
+
+	list_for_each_entry_safe(block, _block, &block_list, head)
+		iio_buffer_block_release(&block->kref);
+}
+static DECLARE_WORK(iio_dma_buffer_cleanup_work, iio_dma_buffer_cleanup_worker);
+
+static void iio_buffer_block_release_atomic(struct kref *kref)
+{
+	struct iio_dma_buffer_block *block;
+	unsigned long flags;
+
+	block = container_of(kref, struct iio_dma_buffer_block, kref);
+
+	spin_lock_irqsave(&iio_dma_buffer_dead_blocks_lock, flags);
+	list_add_tail(&block->head, &iio_dma_buffer_dead_blocks);
+	spin_unlock_irqrestore(&iio_dma_buffer_dead_blocks_lock, flags);
+
+	schedule_work(&iio_dma_buffer_cleanup_work);
+}
+
+/*
+ * Version of iio_buffer_block_put() that can be called from atomic context
+ */
+static void iio_buffer_block_put_atomic(struct iio_dma_buffer_block *block)
+{
+	kref_put(&block->kref, iio_buffer_block_release_atomic);
+}
+
+static struct iio_dma_buffer_queue *iio_buffer_to_queue(struct iio_buffer *buf)
+{
+	return container_of(buf, struct iio_dma_buffer_queue, buffer);
+}
+
+static struct iio_dma_buffer_block *iio_dma_buffer_alloc_block(
+	struct iio_dma_buffer_queue *queue, size_t size)
+{
+	struct iio_dma_buffer_block *block;
+
+	block = kzalloc(sizeof(*block), GFP_KERNEL);
+	if (!block)
+		return NULL;
+
+	block->vaddr = dma_alloc_coherent(queue->dev, PAGE_ALIGN(size),
+		&block->phys_addr, GFP_KERNEL);
+	if (!block->vaddr) {
+		kfree(block);
+		return NULL;
+	}
+
+	block->size = size;
+	block->state = IIO_BLOCK_STATE_DEQUEUED;
+	block->queue = queue;
+	INIT_LIST_HEAD(&block->head);
+	kref_init(&block->kref);
+
+	iio_buffer_get(&queue->buffer);
+
+	return block;
+}
+
+static void _iio_dma_buffer_block_done(struct iio_dma_buffer_block *block)
+{
+	struct iio_dma_buffer_queue *queue = block->queue;
+
+	/*
+	 * The buffer has already been freed by the application, just drop the
+	 * reference.
+	 */
+	if (block->state != IIO_BLOCK_STATE_DEAD) {
+		block->state = IIO_BLOCK_STATE_DONE;
+		list_add_tail(&block->head, &queue->outgoing);
+	}
+}
+
+/**
+ * iio_dma_buffer_block_done() - Indicate that a block has been completed
+ * @block: The completed block
+ *
+ * Should be called when the DMA controller has finished handling the block to
+ * pass back ownership of the block to the queue.
+ */
+void iio_dma_buffer_block_done(struct iio_dma_buffer_block *block)
+{
+	struct iio_dma_buffer_queue *queue = block->queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&queue->list_lock, flags);
+	_iio_dma_buffer_block_done(block);
+	spin_unlock_irqrestore(&queue->list_lock, flags);
+
+	iio_buffer_block_put_atomic(block);
+	wake_up_interruptible_poll(&queue->buffer.pollq, POLLIN | POLLRDNORM);
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_block_done);
+
+/**
+ * iio_dma_buffer_block_list_abort() - Indicate that a list block has been
+ *   aborted
+ * @queue: Queue for which to complete blocks.
+ * @list: List of aborted blocks. All blocks in this list must be from @queue.
+ *
+ * Typically called from the abort() callback after the DMA controller has been
+ * stopped. This will set bytes_used to 0 for each block in the list and then
+ * hand the blocks back to the queue.
+ */
+void iio_dma_buffer_block_list_abort(struct iio_dma_buffer_queue *queue,
+	struct list_head *list)
+{
+	struct iio_dma_buffer_block *block, *_block;
+	unsigned long flags;
+
+	spin_lock_irqsave(&queue->list_lock, flags);
+	list_for_each_entry_safe(block, _block, list, head) {
+		list_del(&block->head);
+		block->bytes_used = 0;
+		_iio_dma_buffer_block_done(block);
+		iio_buffer_block_put_atomic(block);
+	}
+	spin_unlock_irqrestore(&queue->list_lock, flags);
+
+	wake_up_interruptible_poll(&queue->buffer.pollq, POLLIN | POLLRDNORM);
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_block_list_abort);
+
+static bool iio_dma_block_reusable(struct iio_dma_buffer_block *block)
+{
+	/*
+	 * If the core owns the block it can be re-used. This should be the
+	 * default case when enabling the buffer, unless the DMA controller does
+	 * not support abort and has not given back the block yet.
+	 */
+	switch (block->state) {
+	case IIO_BLOCK_STATE_DEQUEUED:
+	case IIO_BLOCK_STATE_QUEUED:
+	case IIO_BLOCK_STATE_DONE:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/**
+ * iio_dma_buffer_request_update() - DMA buffer request_update callback
+ * @buffer: The buffer which to request an update
+ *
+ * Should be used as the iio_dma_buffer_request_update() callback for
+ * iio_buffer_access_ops struct for DMA buffers.
+ */
+int iio_dma_buffer_request_update(struct iio_buffer *buffer)
+{
+	struct iio_dma_buffer_queue *queue = iio_buffer_to_queue(buffer);
+	struct iio_dma_buffer_block *block;
+	bool try_reuse = false;
+	size_t size;
+	int ret = 0;
+	int i;
+
+	/*
+	 * Split the buffer into two even parts. This is used as a double
+	 * buffering scheme with usually one block at a time being used by the
+	 * DMA and the other one by the application.
+	 */
+	size = DIV_ROUND_UP(queue->buffer.bytes_per_datum *
+		queue->buffer.length, 2);
+
+	mutex_lock(&queue->lock);
+
+	/* Allocations are page aligned */
+	if (PAGE_ALIGN(queue->fileio.block_size) == PAGE_ALIGN(size))
+		try_reuse = true;
+
+	queue->fileio.block_size = size;
+	queue->fileio.active_block = NULL;
+
+	spin_lock_irq(&queue->list_lock);
+	for (i = 0; i < 2; i++) {
+		block = queue->fileio.blocks[i];
+
+		/* If we can't re-use it free it */
+		if (block && (!iio_dma_block_reusable(block) || !try_reuse))
+			block->state = IIO_BLOCK_STATE_DEAD;
+	}
+
+	/*
+	 * At this point all blocks are either owned by the core or marked as
+	 * dead. This means we can reset the lists without having to fear
+	 * corrution.
+	 */
+	INIT_LIST_HEAD(&queue->outgoing);
+	spin_unlock_irq(&queue->list_lock);
+
+	INIT_LIST_HEAD(&queue->incoming);
+
+	for (i = 0; i < 2; i++) {
+		if (queue->fileio.blocks[i]) {
+			block = queue->fileio.blocks[i];
+			if (block->state == IIO_BLOCK_STATE_DEAD) {
+				/* Could not reuse it */
+				iio_buffer_block_put(block);
+				block = NULL;
+			} else {
+				block->size = size;
+			}
+		} else {
+			block = NULL;
+		}
+
+		if (!block) {
+			block = iio_dma_buffer_alloc_block(queue, size);
+			if (!block) {
+				ret = -ENOMEM;
+				goto out_unlock;
+			}
+			queue->fileio.blocks[i] = block;
+		}
+
+		block->state = IIO_BLOCK_STATE_QUEUED;
+		list_add_tail(&block->head, &queue->incoming);
+	}
+
+out_unlock:
+	mutex_unlock(&queue->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_request_update);
+
+static void iio_dma_buffer_submit_block(struct iio_dma_buffer_queue *queue,
+	struct iio_dma_buffer_block *block)
+{
+	int ret;
+
+	/*
+	 * If the hardware has already been removed we put the block into
+	 * limbo. It will neither be on the incoming nor outgoing list, nor will
+	 * it ever complete. It will just wait to be freed eventually.
+	 */
+	if (!queue->ops)
+		return;
+
+	block->state = IIO_BLOCK_STATE_ACTIVE;
+	iio_buffer_block_get(block);
+	ret = queue->ops->submit(queue, block);
+	if (ret) {
+		/*
+		 * This is a bit of a problem and there is not much we can do
+		 * other then wait for the buffer to be disabled and re-enabled
+		 * and try again. But it should not really happen unless we run
+		 * out of memory or something similar.
+		 *
+		 * TODO: Implement support in the IIO core to allow buffers to
+		 * notify consumers that something went wrong and the buffer
+		 * should be disabled.
+		 */
+		iio_buffer_block_put(block);
+	}
+}
+
+/**
+ * iio_dma_buffer_enable() - Enable DMA buffer
+ * @buffer: IIO buffer to enable
+ * @indio_dev: IIO device the buffer is attached to
+ *
+ * Needs to be called when the device that the buffer is attached to starts
+ * sampling. Typically should be the iio_buffer_access_ops enable callback.
+ *
+ * This will allocate the DMA buffers and start the DMA transfers.
+ */
+int iio_dma_buffer_enable(struct iio_buffer *buffer,
+	struct iio_dev *indio_dev)
+{
+	struct iio_dma_buffer_queue *queue = iio_buffer_to_queue(buffer);
+	struct iio_dma_buffer_block *block, *_block;
+
+	mutex_lock(&queue->lock);
+	queue->active = true;
+	list_for_each_entry_safe(block, _block, &queue->incoming, head) {
+		list_del(&block->head);
+		iio_dma_buffer_submit_block(queue, block);
+	}
+	mutex_unlock(&queue->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_enable);
+
+/**
+ * iio_dma_buffer_disable() - Disable DMA buffer
+ * @buffer: IIO DMA buffer to disable
+ * @indio_dev: IIO device the buffer is attached to
+ *
+ * Needs to be called when the device that the buffer is attached to stops
+ * sampling. Typically should be the iio_buffer_access_ops disable callback.
+ */
+int iio_dma_buffer_disable(struct iio_buffer *buffer,
+	struct iio_dev *indio_dev)
+{
+	struct iio_dma_buffer_queue *queue = iio_buffer_to_queue(buffer);
+
+	mutex_lock(&queue->lock);
+	queue->active = false;
+
+	if (queue->ops && queue->ops->abort)
+		queue->ops->abort(queue);
+	mutex_unlock(&queue->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_disable);
+
+static void iio_dma_buffer_enqueue(struct iio_dma_buffer_queue *queue,
+	struct iio_dma_buffer_block *block)
+{
+	if (block->state == IIO_BLOCK_STATE_DEAD) {
+		iio_buffer_block_put(block);
+	} else if (queue->active) {
+		iio_dma_buffer_submit_block(queue, block);
+	} else {
+		block->state = IIO_BLOCK_STATE_QUEUED;
+		list_add_tail(&block->head, &queue->incoming);
+	}
+}
+
+static struct iio_dma_buffer_block *iio_dma_buffer_dequeue(
+	struct iio_dma_buffer_queue *queue)
+{
+	struct iio_dma_buffer_block *block;
+
+	spin_lock_irq(&queue->list_lock);
+	block = list_first_entry_or_null(&queue->outgoing, struct
+		iio_dma_buffer_block, head);
+	if (block != NULL) {
+		list_del(&block->head);
+		block->state = IIO_BLOCK_STATE_DEQUEUED;
+	}
+	spin_unlock_irq(&queue->list_lock);
+
+	return block;
+}
+
+/**
+ * iio_dma_buffer_read() - DMA buffer read callback
+ * @buffer: Buffer to read form
+ * @n: Number of bytes to read
+ * @user_buffer: Userspace buffer to copy the data to
+ *
+ * Should be used as the read_first_n callback for iio_buffer_access_ops
+ * struct for DMA buffers.
+ */
+int iio_dma_buffer_read(struct iio_buffer *buffer, size_t n,
+	char __user *user_buffer)
+{
+	struct iio_dma_buffer_queue *queue = iio_buffer_to_queue(buffer);
+	struct iio_dma_buffer_block *block;
+	int ret;
+
+	if (n < buffer->bytes_per_datum)
+		return -EINVAL;
+
+	mutex_lock(&queue->lock);
+
+	if (!queue->fileio.active_block) {
+		block = iio_dma_buffer_dequeue(queue);
+		if (block == NULL) {
+			ret = 0;
+			goto out_unlock;
+		}
+		queue->fileio.pos = 0;
+		queue->fileio.active_block = block;
+	} else {
+		block = queue->fileio.active_block;
+	}
+
+	n = rounddown(n, buffer->bytes_per_datum);
+	if (n > block->bytes_used - queue->fileio.pos)
+		n = block->bytes_used - queue->fileio.pos;
+
+	if (copy_to_user(user_buffer, block->vaddr + queue->fileio.pos, n)) {
+		ret = -EFAULT;
+		goto out_unlock;
+	}
+
+	queue->fileio.pos += n;
+
+	if (queue->fileio.pos == block->bytes_used) {
+		queue->fileio.active_block = NULL;
+		iio_dma_buffer_enqueue(queue, block);
+	}
+
+	ret = n;
+
+out_unlock:
+	mutex_unlock(&queue->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_read);
+
+/**
+ * iio_dma_buffer_data_available() - DMA buffer data_available callback
+ * @buf: Buffer to check for data availability
+ *
+ * Should be used as the data_available callback for iio_buffer_access_ops
+ * struct for DMA buffers.
+ */
+size_t iio_dma_buffer_data_available(struct iio_buffer *buf)
+{
+	struct iio_dma_buffer_queue *queue = iio_buffer_to_queue(buf);
+	struct iio_dma_buffer_block *block;
+	size_t data_available = 0;
+
+	/*
+	 * For counting the available bytes we'll use the size of the block not
+	 * the number of actual bytes available in the block. Otherwise it is
+	 * possible that we end up with a value that is lower than the watermark
+	 * but won't increase since all blocks are in use.
+	 */
+
+	mutex_lock(&queue->lock);
+	if (queue->fileio.active_block)
+		data_available += queue->fileio.active_block->size;
+
+	spin_lock_irq(&queue->list_lock);
+	list_for_each_entry(block, &queue->outgoing, head)
+		data_available += block->size;
+	spin_unlock_irq(&queue->list_lock);
+	mutex_unlock(&queue->lock);
+
+	return data_available;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_data_available);
+
+/**
+ * iio_dma_buffer_set_bytes_per_datum() - DMA buffer set_bytes_per_datum callback
+ * @buffer: Buffer to set the bytes-per-datum for
+ * @bpd: The new bytes-per-datum value
+ *
+ * Should be used as the set_bytes_per_datum callback for iio_buffer_access_ops
+ * struct for DMA buffers.
+ */
+int iio_dma_buffer_set_bytes_per_datum(struct iio_buffer *buffer, size_t bpd)
+{
+	buffer->bytes_per_datum = bpd;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_set_bytes_per_datum);
+
+/**
+ * iio_dma_buffer_set_length - DMA buffer set_length callback
+ * @buffer: Buffer to set the length for
+ * @length: The new buffer length
+ *
+ * Should be used as the set_length callback for iio_buffer_access_ops
+ * struct for DMA buffers.
+ */
+int iio_dma_buffer_set_length(struct iio_buffer *buffer, int length)
+{
+	/* Avoid an invalid state */
+	if (length < 2)
+		length = 2;
+	buffer->length = length;
+	buffer->watermark = length / 2;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_set_length);
+
+/**
+ * iio_dma_buffer_init() - Initialize DMA buffer queue
+ * @queue: Buffer to initialize
+ * @dev: DMA device
+ * @ops: DMA buffer queue callback operations
+ *
+ * The DMA device will be used by the queue to do DMA memory allocations. So it
+ * should refer to the device that will perform the DMA to ensure that
+ * allocations are done from a memory region that can be accessed by the device.
+ */
+int iio_dma_buffer_init(struct iio_dma_buffer_queue *queue,
+	struct device *dev, const struct iio_dma_buffer_ops *ops)
+{
+	iio_buffer_init(&queue->buffer);
+	queue->buffer.length = PAGE_SIZE;
+	queue->buffer.watermark = queue->buffer.length / 2;
+	queue->dev = dev;
+	queue->ops = ops;
+
+	INIT_LIST_HEAD(&queue->incoming);
+	INIT_LIST_HEAD(&queue->outgoing);
+
+	mutex_init(&queue->lock);
+	spin_lock_init(&queue->list_lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_init);
+
+/**
+ * iio_dma_buffer_exit() - Cleanup DMA buffer queue
+ * @queue: Buffer to cleanup
+ *
+ * After this function has completed it is safe to free any resources that are
+ * associated with the buffer and are accessed inside the callback operations.
+ */
+void iio_dma_buffer_exit(struct iio_dma_buffer_queue *queue)
+{
+	unsigned int i;
+
+	mutex_lock(&queue->lock);
+
+	spin_lock_irq(&queue->list_lock);
+	for (i = 0; i < ARRAY_SIZE(queue->fileio.blocks); i++) {
+		if (!queue->fileio.blocks[i])
+			continue;
+		queue->fileio.blocks[i]->state = IIO_BLOCK_STATE_DEAD;
+	}
+	INIT_LIST_HEAD(&queue->outgoing);
+	spin_unlock_irq(&queue->list_lock);
+
+	INIT_LIST_HEAD(&queue->incoming);
+
+	for (i = 0; i < ARRAY_SIZE(queue->fileio.blocks); i++) {
+		if (!queue->fileio.blocks[i])
+			continue;
+		iio_buffer_block_put(queue->fileio.blocks[i]);
+		queue->fileio.blocks[i] = NULL;
+	}
+	queue->fileio.active_block = NULL;
+	queue->ops = NULL;
+
+	mutex_unlock(&queue->lock);
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_exit);
+
+/**
+ * iio_dma_buffer_release() - Release final buffer resources
+ * @queue: Buffer to release
+ *
+ * Frees resources that can't yet be freed in iio_dma_buffer_exit(). Should be
+ * called in the buffers release callback implementation right before freeing
+ * the memory associated with the buffer.
+ */
+void iio_dma_buffer_release(struct iio_dma_buffer_queue *queue)
+{
+	mutex_destroy(&queue->lock);
+}
+EXPORT_SYMBOL_GPL(iio_dma_buffer_release);
+
+MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
+MODULE_DESCRIPTION("DMA buffer for the IIO framework");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/buffer/industrialio-buffer-dmaengine.c b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
new file mode 100644
index 000000000000..ebdb838d3a1c
--- /dev/null
+++ b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
@@ -0,0 +1,213 @@
+/*
+ * Copyright 2014-2015 Analog Devices Inc.
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
+ *
+ * Licensed under the GPL-2 or later.
+ */
+
+#include <linux/slab.h>
+#include <linux/kernel.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/buffer-dma.h>
+#include <linux/iio/buffer-dmaengine.h>
+
+/*
+ * The IIO DMAengine buffer combines the generic IIO DMA buffer infrastructure
+ * with the DMAengine framework. The generic IIO DMA buffer infrastructure is
+ * used to manage the buffer memory and implement the IIO buffer operations
+ * while the DMAengine framework is used to perform the DMA transfers. Combined
+ * this results in a device independent fully functional DMA buffer
+ * implementation that can be used by device drivers for peripherals which are
+ * connected to a DMA controller which has a DMAengine driver implementation.
+ */
+
+struct dmaengine_buffer {
+	struct iio_dma_buffer_queue queue;
+
+	struct dma_chan *chan;
+	struct list_head active;
+
+	size_t align;
+	size_t max_size;
+};
+
+static struct dmaengine_buffer *iio_buffer_to_dmaengine_buffer(
+		struct iio_buffer *buffer)
+{
+	return container_of(buffer, struct dmaengine_buffer, queue.buffer);
+}
+
+static void iio_dmaengine_buffer_block_done(void *data)
+{
+	struct iio_dma_buffer_block *block = data;
+	unsigned long flags;
+
+	spin_lock_irqsave(&block->queue->list_lock, flags);
+	list_del(&block->head);
+	spin_unlock_irqrestore(&block->queue->list_lock, flags);
+	iio_dma_buffer_block_done(block);
+}
+
+static int iio_dmaengine_buffer_submit_block(struct iio_dma_buffer_queue *queue,
+	struct iio_dma_buffer_block *block)
+{
+	struct dmaengine_buffer *dmaengine_buffer =
+		iio_buffer_to_dmaengine_buffer(&queue->buffer);
+	struct dma_async_tx_descriptor *desc;
+	dma_cookie_t cookie;
+
+	block->bytes_used = min(block->size, dmaengine_buffer->max_size);
+	block->bytes_used = rounddown(block->bytes_used,
+			dmaengine_buffer->align);
+
+	desc = dmaengine_prep_slave_single(dmaengine_buffer->chan,
+		block->phys_addr, block->bytes_used, DMA_DEV_TO_MEM,
+		DMA_PREP_INTERRUPT);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->callback = iio_dmaengine_buffer_block_done;
+	desc->callback_param = block;
+
+	cookie = dmaengine_submit(desc);
+	if (dma_submit_error(cookie))
+		return dma_submit_error(cookie);
+
+	spin_lock_irq(&dmaengine_buffer->queue.list_lock);
+	list_add_tail(&block->head, &dmaengine_buffer->active);
+	spin_unlock_irq(&dmaengine_buffer->queue.list_lock);
+
+	dma_async_issue_pending(dmaengine_buffer->chan);
+
+	return 0;
+}
+
+static void iio_dmaengine_buffer_abort(struct iio_dma_buffer_queue *queue)
+{
+	struct dmaengine_buffer *dmaengine_buffer =
+		iio_buffer_to_dmaengine_buffer(&queue->buffer);
+
+	dmaengine_terminate_all(dmaengine_buffer->chan);
+	/* FIXME: There is a slight chance of a race condition here.
+	 * dmaengine_terminate_all() does not guarantee that all transfer
+	 * callbacks have finished running. Need to introduce a
+	 * dmaengine_terminate_all_sync().
+	 */
+	iio_dma_buffer_block_list_abort(queue, &dmaengine_buffer->active);
+}
+
+static void iio_dmaengine_buffer_release(struct iio_buffer *buf)
+{
+	struct dmaengine_buffer *dmaengine_buffer =
+		iio_buffer_to_dmaengine_buffer(buf);
+
+	iio_dma_buffer_release(&dmaengine_buffer->queue);
+	kfree(dmaengine_buffer);
+}
+
+static const struct iio_buffer_access_funcs iio_dmaengine_buffer_ops = {
+	.read_first_n = iio_dma_buffer_read,
+	.set_bytes_per_datum = iio_dma_buffer_set_bytes_per_datum,
+	.set_length = iio_dma_buffer_set_length,
+	.request_update = iio_dma_buffer_request_update,
+	.enable = iio_dma_buffer_enable,
+	.disable = iio_dma_buffer_disable,
+	.data_available = iio_dma_buffer_data_available,
+	.release = iio_dmaengine_buffer_release,
+
+	.modes = INDIO_BUFFER_HARDWARE,
+	.flags = INDIO_BUFFER_FLAG_FIXED_WATERMARK,
+};
+
+static const struct iio_dma_buffer_ops iio_dmaengine_default_ops = {
+	.submit = iio_dmaengine_buffer_submit_block,
+	.abort = iio_dmaengine_buffer_abort,
+};
+
+/**
+ * iio_dmaengine_buffer_alloc() - Allocate new buffer which uses DMAengine
+ * @dev: Parent device for the buffer
+ * @channel: DMA channel name, typically "rx".
+ *
+ * This allocates a new IIO buffer which internally uses the DMAengine framework
+ * to perform its transfers. The parent device will be used to request the DMA
+ * channel.
+ *
+ * Once done using the buffer iio_dmaengine_buffer_free() should be used to
+ * release it.
+ */
+struct iio_buffer *iio_dmaengine_buffer_alloc(struct device *dev,
+	const char *channel)
+{
+	struct dmaengine_buffer *dmaengine_buffer;
+	unsigned int width, src_width, dest_width;
+	struct dma_slave_caps caps;
+	struct dma_chan *chan;
+	int ret;
+
+	dmaengine_buffer = kzalloc(sizeof(*dmaengine_buffer), GFP_KERNEL);
+	if (!dmaengine_buffer)
+		return ERR_PTR(-ENOMEM);
+
+	chan = dma_request_slave_channel_reason(dev, channel);
+	if (IS_ERR(chan)) {
+		ret = PTR_ERR(chan);
+		goto err_free;
+	}
+
+	ret = dma_get_slave_caps(chan, &caps);
+	if (ret < 0)
+		goto err_free;
+
+	/* Needs to be aligned to the maximum of the minimums */
+	if (caps.src_addr_widths)
+		src_width = __ffs(caps.src_addr_widths);
+	else
+		src_width = 1;
+	if (caps.dst_addr_widths)
+		dest_width = __ffs(caps.dst_addr_widths);
+	else
+		dest_width = 1;
+	width = max(src_width, dest_width);
+
+	INIT_LIST_HEAD(&dmaengine_buffer->active);
+	dmaengine_buffer->chan = chan;
+	dmaengine_buffer->align = width;
+	dmaengine_buffer->max_size = dma_get_max_seg_size(chan->device->dev);
+
+	iio_dma_buffer_init(&dmaengine_buffer->queue, chan->device->dev,
+		&iio_dmaengine_default_ops);
+
+	dmaengine_buffer->queue.buffer.access = &iio_dmaengine_buffer_ops;
+
+	return &dmaengine_buffer->queue.buffer;
+
+err_free:
+	kfree(dmaengine_buffer);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL(iio_dmaengine_buffer_alloc);
+
+/**
+ * iio_dmaengine_buffer_free() - Free dmaengine buffer
+ * @buffer: Buffer to free
+ *
+ * Frees a buffer previously allocated with iio_dmaengine_buffer_alloc().
+ */
+void iio_dmaengine_buffer_free(struct iio_buffer *buffer)
+{
+	struct dmaengine_buffer *dmaengine_buffer =
+		iio_buffer_to_dmaengine_buffer(buffer);
+
+	iio_dma_buffer_exit(&dmaengine_buffer->queue);
+	dma_release_channel(dmaengine_buffer->chan);
+
+	iio_buffer_put(buffer);
+}
+EXPORT_SYMBOL_GPL(iio_dmaengine_buffer_free);
diff --git a/drivers/iio/chemical/Kconfig b/drivers/iio/chemical/Kconfig
index 3061b7299f0f..f16de61be46d 100644
--- a/drivers/iio/chemical/Kconfig
+++ b/drivers/iio/chemical/Kconfig
@@ -4,6 +4,14 @@
 
 menu "Chemical Sensors"
 
+config IAQCORE
+	tristate "AMS iAQ-Core VOC sensors"
+	depends on I2C
+	help
+	  Say Y here to build I2C interface support for the AMS
+	  iAQ-Core Continuous/Pulsed VOC (Volatile Organic Compounds)
+	  sensors
+
 config VZ89X
 	tristate "SGX Sensortech MiCS VZ89X VOC sensor"
 	depends on I2C
diff --git a/drivers/iio/chemical/Makefile b/drivers/iio/chemical/Makefile
index 7292f2ded587..167861fadfab 100644
--- a/drivers/iio/chemical/Makefile
+++ b/drivers/iio/chemical/Makefile
@@ -3,4 +3,5 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+obj-$(CONFIG_IAQCORE)		+= ams-iaq-core.o
 obj-$(CONFIG_VZ89X)		+= vz89x.o
diff --git a/drivers/iio/chemical/ams-iaq-core.c b/drivers/iio/chemical/ams-iaq-core.c
new file mode 100644
index 000000000000..41a8e6f2e31d
--- /dev/null
+++ b/drivers/iio/chemical/ams-iaq-core.c
@@ -0,0 +1,200 @@
+/*
+ * ams-iaq-core.c - Support for AMS iAQ-Core VOC sensors
+ *
+ * Copyright (C) 2015 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/mutex.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+
+#define AMS_IAQCORE_DATA_SIZE		9
+
+#define AMS_IAQCORE_VOC_CO2_IDX		0
+#define AMS_IAQCORE_VOC_RESISTANCE_IDX	1
+#define AMS_IAQCORE_VOC_TVOC_IDX	2
+
+struct ams_iaqcore_reading {
+	__be16 co2_ppm;
+	u8 status;
+	__be32 resistance;
+	__be16 voc_ppb;
+} __attribute__((__packed__));
+
+struct ams_iaqcore_data {
+	struct i2c_client *client;
+	struct mutex lock;
+	unsigned long last_update;
+
+	struct ams_iaqcore_reading buffer;
+};
+
+static const struct iio_chan_spec ams_iaqcore_channels[] = {
+	{
+		.type = IIO_CONCENTRATION,
+		.channel2 = IIO_MOD_CO2,
+		.modified = 1,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+		.address = AMS_IAQCORE_VOC_CO2_IDX,
+	},
+	{
+		.type = IIO_RESISTANCE,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+		.address = AMS_IAQCORE_VOC_RESISTANCE_IDX,
+	},
+	{
+		.type = IIO_CONCENTRATION,
+		.channel2 = IIO_MOD_VOC,
+		.modified = 1,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+		.address = AMS_IAQCORE_VOC_TVOC_IDX,
+	},
+};
+
+static int ams_iaqcore_read_measurement(struct ams_iaqcore_data *data)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = client->flags | I2C_M_RD,
+		.len = AMS_IAQCORE_DATA_SIZE,
+		.buf = (char *) &data->buffer,
+	};
+
+	ret = i2c_transfer(client->adapter, &msg, 1);
+
+	return (ret == AMS_IAQCORE_DATA_SIZE) ? 0 : ret;
+}
+
+static int ams_iaqcore_get_measurement(struct ams_iaqcore_data *data)
+{
+	int ret;
+
+	/* sensor can only be polled once a second max per datasheet */
+	if (!time_after(jiffies, data->last_update + HZ))
+		return 0;
+
+	ret = ams_iaqcore_read_measurement(data);
+	if (ret < 0)
+		return ret;
+
+	data->last_update = jiffies;
+
+	return 0;
+}
+
+static int ams_iaqcore_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan, int *val,
+				int *val2, long mask)
+{
+	struct ams_iaqcore_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (mask != IIO_CHAN_INFO_PROCESSED)
+		return -EINVAL;
+
+	mutex_lock(&data->lock);
+	ret = ams_iaqcore_get_measurement(data);
+
+	if (ret)
+		goto err_out;
+
+	switch (chan->address) {
+	case AMS_IAQCORE_VOC_CO2_IDX:
+		*val = 0;
+		*val2 = be16_to_cpu(data->buffer.co2_ppm);
+		ret = IIO_VAL_INT_PLUS_MICRO;
+		break;
+	case AMS_IAQCORE_VOC_RESISTANCE_IDX:
+		*val = be32_to_cpu(data->buffer.resistance);
+		ret = IIO_VAL_INT;
+		break;
+	case AMS_IAQCORE_VOC_TVOC_IDX:
+		*val = 0;
+		*val2 = be16_to_cpu(data->buffer.voc_ppb);
+		ret = IIO_VAL_INT_PLUS_NANO;
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+err_out:
+	mutex_unlock(&data->lock);
+
+	return ret;
+}
+
+static const struct iio_info ams_iaqcore_info = {
+	.read_raw	= ams_iaqcore_read_raw,
+	.driver_module	= THIS_MODULE,
+};
+
+static int ams_iaqcore_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	struct iio_dev *indio_dev;
+	struct ams_iaqcore_data *data;
+
+	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;
+
+	/* so initial reading will complete */
+	data->last_update = jiffies - HZ;
+	mutex_init(&data->lock);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &ams_iaqcore_info,
+	indio_dev->name = dev_name(&client->dev);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	indio_dev->channels = ams_iaqcore_channels;
+	indio_dev->num_channels = ARRAY_SIZE(ams_iaqcore_channels);
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct i2c_device_id ams_iaqcore_id[] = {
+	{ "ams-iaq-core", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ams_iaqcore_id);
+
+static const struct of_device_id ams_iaqcore_dt_ids[] = {
+	{ .compatible = "ams,iaq-core" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ams_iaqcore_dt_ids);
+
+static struct i2c_driver ams_iaqcore_driver = {
+	.driver = {
+		.name	= "ams-iaq-core",
+		.of_match_table = of_match_ptr(ams_iaqcore_dt_ids),
+	},
+	.probe = ams_iaqcore_probe,
+	.id_table = ams_iaqcore_id,
+};
+module_i2c_driver(ams_iaqcore_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("AMS iAQ-Core VOC sensors");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/chemical/vz89x.c b/drivers/iio/chemical/vz89x.c
index 11e59a5a5112..b8b804923230 100644
--- a/drivers/iio/chemical/vz89x.c
+++ b/drivers/iio/chemical/vz89x.c
@@ -34,8 +34,9 @@
 struct vz89x_data {
 	struct i2c_client *client;
 	struct mutex lock;
-	unsigned long last_update;
+	int (*xfer)(struct vz89x_data *data, u8 cmd);
 
+	unsigned long last_update;
 	u8 buffer[VZ89X_REG_MEASUREMENT_SIZE];
 };
 
@@ -100,27 +101,60 @@ static int vz89x_measurement_is_valid(struct vz89x_data *data)
 	return !!(data->buffer[VZ89X_REG_MEASUREMENT_SIZE - 1] > 0);
 }
 
-static int vz89x_get_measurement(struct vz89x_data *data)
+static int vz89x_i2c_xfer(struct vz89x_data *data, u8 cmd)
 {
+	struct i2c_client *client = data->client;
+	struct i2c_msg msg[2];
 	int ret;
-	int i;
+	u8 buf[3] = { cmd, 0, 0};
 
-	/* sensor can only be polled once a second max per datasheet */
-	if (!time_after(jiffies, data->last_update + HZ))
-		return 0;
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags;
+	msg[0].len = 3;
+	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].buf = (char *) &data->buffer;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
 
-	ret = i2c_smbus_write_word_data(data->client,
-					VZ89X_REG_MEASUREMENT, 0);
+	return (ret == 2) ? 0 : ret;
+}
+
+static int vz89x_smbus_xfer(struct vz89x_data *data, u8 cmd)
+{
+	struct i2c_client *client = data->client;
+	int ret;
+	int i;
+
+	ret = i2c_smbus_write_word_data(client, cmd, 0);
 	if (ret < 0)
 		return ret;
 
 	for (i = 0; i < VZ89X_REG_MEASUREMENT_SIZE; i++) {
-		ret = i2c_smbus_read_byte(data->client);
+		ret = i2c_smbus_read_byte(client);
 		if (ret < 0)
 			return ret;
 		data->buffer[i] = ret;
 	}
 
+	return 0;
+}
+
+static int vz89x_get_measurement(struct vz89x_data *data)
+{
+	int ret;
+
+	/* sensor can only be polled once a second max per datasheet */
+	if (!time_after(jiffies, data->last_update + HZ))
+		return 0;
+
+	ret = data->xfer(data, VZ89X_REG_MEASUREMENT);
+	if (ret < 0)
+		return ret;
+
 	ret = vz89x_measurement_is_valid(data);
 	if (ret)
 		return -EAGAIN;
@@ -204,15 +238,19 @@ static int vz89x_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	struct vz89x_data *data;
 
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_WORD_DATA |
-				     I2C_FUNC_SMBUS_BYTE))
-		return -ENODEV;
-
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
 		return -ENOMEM;
-
 	data = iio_priv(indio_dev);
+
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C))
+		data->xfer = vz89x_i2c_xfer;
+	else if (i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_BYTE))
+		data->xfer = vz89x_smbus_xfer;
+	else
+		return -ENOTSUPP;
+
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
 	data->last_update = jiffies - HZ;
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 25258e2c1a82..8447c31e27f2 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -18,9 +18,6 @@
 #include <asm/unaligned.h>
 #include <linux/iio/common/st_sensors.h>
 
-
-#define ST_SENSORS_WAI_ADDRESS		0x0f
-
 static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
 {
 	return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
diff --git a/drivers/iio/dac/mcp4725.c b/drivers/iio/dac/mcp4725.c
index 43d14588448d..b4dde8315210 100644
--- a/drivers/iio/dac/mcp4725.c
+++ b/drivers/iio/dac/mcp4725.c
@@ -300,6 +300,7 @@ static int mcp4725_probe(struct i2c_client *client,
 	data->client = client;
 
 	indio_dev->dev.parent = &client->dev;
+	indio_dev->name = id->name;
 	indio_dev->info = &mcp4725_info;
 	indio_dev->channels = &mcp4725_channel;
 	indio_dev->num_channels = 1;
diff --git a/drivers/iio/dummy/Kconfig b/drivers/iio/dummy/Kconfig
new file mode 100644
index 000000000000..71805ced1aae
--- /dev/null
+++ b/drivers/iio/dummy/Kconfig
@@ -0,0 +1,36 @@
+#
+# Industrial I/O subsystem Dummy Driver configuration
+#
+menu "IIO dummy driver"
+	depends on IIO
+
+config IIO_DUMMY_EVGEN
+	select IRQ_WORK
+	tristate
+
+config IIO_SIMPLE_DUMMY
+       tristate "An example driver with no hardware requirements"
+       help
+	 Driver intended mainly as documentation for how to write
+	 a driver. May also be useful for testing userspace code
+	 without hardware.
+
+if IIO_SIMPLE_DUMMY
+
+config IIO_SIMPLE_DUMMY_EVENTS
+       bool "Event generation support"
+       select IIO_DUMMY_EVGEN
+       help
+         Add some dummy events to the simple dummy driver.
+
+config IIO_SIMPLE_DUMMY_BUFFER
+	bool "Buffered capture support"
+	select IIO_BUFFER
+	select IIO_TRIGGER
+	select IIO_KFIFO_BUF
+	help
+	  Add buffered data capture to the simple dummy driver.
+
+endif # IIO_SIMPLE_DUMMY
+
+endmenu
diff --git a/drivers/iio/dummy/Makefile b/drivers/iio/dummy/Makefile
new file mode 100644
index 000000000000..0765e93d7804
--- /dev/null
+++ b/drivers/iio/dummy/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the IIO Dummy Driver
+#
+
+obj-$(CONFIG_IIO_SIMPLE_DUMMY) += iio_dummy.o
+iio_dummy-y := iio_simple_dummy.o
+iio_dummy-$(CONFIG_IIO_SIMPLE_DUMMY_EVENTS) += iio_simple_dummy_events.o
+iio_dummy-$(CONFIG_IIO_SIMPLE_DUMMY_BUFFER) += iio_simple_dummy_buffer.o
+
+obj-$(CONFIG_IIO_DUMMY_EVGEN) += iio_dummy_evgen.o
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/iio/dummy/iio_dummy_evgen.c
index 9e83f348df51..9e83f348df51 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/iio/dummy/iio_dummy_evgen.c
diff --git a/drivers/staging/iio/iio_dummy_evgen.h b/drivers/iio/dummy/iio_dummy_evgen.h
index d044b946e74a..d044b946e74a 100644
--- a/drivers/staging/iio/iio_dummy_evgen.h
+++ b/drivers/iio/dummy/iio_dummy_evgen.h
diff --git a/drivers/staging/iio/iio_simple_dummy.c b/drivers/iio/dummy/iio_simple_dummy.c
index 43fe4ba7d0dc..43fe4ba7d0dc 100644
--- a/drivers/staging/iio/iio_simple_dummy.c
+++ b/drivers/iio/dummy/iio_simple_dummy.c
diff --git a/drivers/staging/iio/iio_simple_dummy.h b/drivers/iio/dummy/iio_simple_dummy.h
index b9069a180672..b9069a180672 100644
--- a/drivers/staging/iio/iio_simple_dummy.h
+++ b/drivers/iio/dummy/iio_simple_dummy.h
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/iio/dummy/iio_simple_dummy_buffer.c
index cf44a6f79431..cf44a6f79431 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/iio/dummy/iio_simple_dummy_buffer.c
diff --git a/drivers/staging/iio/iio_simple_dummy_events.c b/drivers/iio/dummy/iio_simple_dummy_events.c
index 6eb600ff7056..6eb600ff7056 100644
--- a/drivers/staging/iio/iio_simple_dummy_events.c
+++ b/drivers/iio/dummy/iio_simple_dummy_events.c
diff --git a/drivers/iio/gyro/adis16136.c b/drivers/iio/gyro/adis16136.c
index f8d1c2210066..b04faf93e1bc 100644
--- a/drivers/iio/gyro/adis16136.c
+++ b/drivers/iio/gyro/adis16136.c
@@ -435,7 +435,9 @@ static int adis16136_initial_setup(struct iio_dev *indio_dev)
 	if (ret)
 		return ret;
 
-	sscanf(indio_dev->name, "adis%u\n", &device_id);
+	ret = sscanf(indio_dev->name, "adis%u\n", &device_id);
+	if (ret != 1)
+		return -EINVAL;
 
 	if (prod_id != device_id)
 		dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.",
diff --git a/drivers/iio/gyro/bmg160_core.c b/drivers/iio/gyro/bmg160_core.c
index 02ff789852a0..bbce3b09ac45 100644
--- a/drivers/iio/gyro/bmg160_core.c
+++ b/drivers/iio/gyro/bmg160_core.c
@@ -1077,25 +1077,23 @@ int bmg160_core_probe(struct device *dev, struct regmap *regmap, int irq,
 		goto err_trigger_unregister;
 	}
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(dev, "unable to register iio device\n");
-		goto err_buffer_cleanup;
-	}
-
 	ret = pm_runtime_set_active(dev);
 	if (ret)
-		goto err_iio_unregister;
+		goto err_buffer_cleanup;
 
 	pm_runtime_enable(dev);
 	pm_runtime_set_autosuspend_delay(dev,
 					 BMG160_AUTO_SUSPEND_DELAY_MS);
 	pm_runtime_use_autosuspend(dev);
 
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(dev, "unable to register iio device\n");
+		goto err_buffer_cleanup;
+	}
+
 	return 0;
 
-err_iio_unregister:
-	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
@@ -1113,11 +1111,12 @@ void bmg160_core_remove(struct device *dev)
 	struct iio_dev *indio_dev = dev_get_drvdata(dev);
 	struct bmg160_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(dev);
 	pm_runtime_set_suspended(dev);
 	pm_runtime_put_noidle(dev);
 
-	iio_device_unregister(indio_dev);
 	iio_triggered_buffer_cleanup(indio_dev);
 
 	if (data->dready_trig) {
diff --git a/drivers/iio/health/Kconfig b/drivers/iio/health/Kconfig
new file mode 100644
index 000000000000..a647679da805
--- /dev/null
+++ b/drivers/iio/health/Kconfig
@@ -0,0 +1,21 @@
+#
+# Health sensors
+#
+# When adding new entries keep the list in alphabetical order
+
+menu "Health sensors"
+
+config MAX30100
+	tristate "MAX30100 heart rate and pulse oximeter sensor"
+	depends on I2C
+	select REGMAP_I2C
+	select IIO_BUFFER
+	select IIO_KFIFO_BUF
+	help
+	  Say Y here to build I2C interface support for the Maxim
+	  MAX30100 heart rate, and pulse oximeter sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called max30100.
+
+endmenu
diff --git a/drivers/iio/health/Makefile b/drivers/iio/health/Makefile
new file mode 100644
index 000000000000..7c475d7faad8
--- /dev/null
+++ b/drivers/iio/health/Makefile
@@ -0,0 +1,7 @@
+#
+# Makefile for IIO Health sensors
+#
+
+# When adding new entries keep the list in alphabetical order
+
+obj-$(CONFIG_MAX30100)		+= max30100.o
diff --git a/drivers/iio/health/max30100.c b/drivers/iio/health/max30100.c
new file mode 100644
index 000000000000..9d1c81f91dd7
--- /dev/null
+++ b/drivers/iio/health/max30100.c
@@ -0,0 +1,453 @@
+/*
+ * max30100.c - Support for MAX30100 heart rate and pulse oximeter sensor
+ *
+ * Copyright (C) 2015 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.
+ *
+ * TODO: allow LED current and pulse length controls via device tree properties
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/irq.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/regmap.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/kfifo_buf.h>
+
+#define MAX30100_REGMAP_NAME	"max30100_regmap"
+#define MAX30100_DRV_NAME	"max30100"
+
+#define MAX30100_REG_INT_STATUS			0x00
+#define MAX30100_REG_INT_STATUS_PWR_RDY		BIT(0)
+#define MAX30100_REG_INT_STATUS_SPO2_RDY	BIT(4)
+#define MAX30100_REG_INT_STATUS_HR_RDY		BIT(5)
+#define MAX30100_REG_INT_STATUS_FIFO_RDY	BIT(7)
+
+#define MAX30100_REG_INT_ENABLE			0x01
+#define MAX30100_REG_INT_ENABLE_SPO2_EN		BIT(0)
+#define MAX30100_REG_INT_ENABLE_HR_EN		BIT(1)
+#define MAX30100_REG_INT_ENABLE_FIFO_EN		BIT(3)
+#define MAX30100_REG_INT_ENABLE_MASK		0xf0
+#define MAX30100_REG_INT_ENABLE_MASK_SHIFT	4
+
+#define MAX30100_REG_FIFO_WR_PTR		0x02
+#define MAX30100_REG_FIFO_OVR_CTR		0x03
+#define MAX30100_REG_FIFO_RD_PTR		0x04
+#define MAX30100_REG_FIFO_DATA			0x05
+#define MAX30100_REG_FIFO_DATA_ENTRY_COUNT	16
+#define MAX30100_REG_FIFO_DATA_ENTRY_LEN	4
+
+#define MAX30100_REG_MODE_CONFIG		0x06
+#define MAX30100_REG_MODE_CONFIG_MODE_SPO2_EN	BIT(0)
+#define MAX30100_REG_MODE_CONFIG_MODE_HR_EN	BIT(1)
+#define MAX30100_REG_MODE_CONFIG_MODE_MASK	0x03
+#define MAX30100_REG_MODE_CONFIG_TEMP_EN	BIT(3)
+#define MAX30100_REG_MODE_CONFIG_PWR		BIT(7)
+
+#define MAX30100_REG_SPO2_CONFIG		0x07
+#define MAX30100_REG_SPO2_CONFIG_100HZ		BIT(2)
+#define MAX30100_REG_SPO2_CONFIG_HI_RES_EN	BIT(6)
+#define MAX30100_REG_SPO2_CONFIG_1600US		0x3
+
+#define MAX30100_REG_LED_CONFIG			0x09
+#define MAX30100_REG_LED_CONFIG_RED_LED_SHIFT	4
+
+#define MAX30100_REG_LED_CONFIG_24MA		0x07
+#define MAX30100_REG_LED_CONFIG_50MA		0x0f
+
+#define MAX30100_REG_TEMP_INTEGER		0x16
+#define MAX30100_REG_TEMP_FRACTION		0x17
+
+struct max30100_data {
+	struct i2c_client *client;
+	struct iio_dev *indio_dev;
+	struct mutex lock;
+	struct regmap *regmap;
+
+	__be16 buffer[2]; /* 2 16-bit channels */
+};
+
+static bool max30100_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case MAX30100_REG_INT_STATUS:
+	case MAX30100_REG_MODE_CONFIG:
+	case MAX30100_REG_FIFO_WR_PTR:
+	case MAX30100_REG_FIFO_OVR_CTR:
+	case MAX30100_REG_FIFO_RD_PTR:
+	case MAX30100_REG_FIFO_DATA:
+	case MAX30100_REG_TEMP_INTEGER:
+	case MAX30100_REG_TEMP_FRACTION:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config max30100_regmap_config = {
+	.name = MAX30100_REGMAP_NAME,
+
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = MAX30100_REG_TEMP_FRACTION,
+	.cache_type = REGCACHE_FLAT,
+
+	.volatile_reg = max30100_is_volatile_reg,
+};
+
+static const unsigned long max30100_scan_masks[] = {0x3, 0};
+
+static const struct iio_chan_spec max30100_channels[] = {
+	{
+		.type = IIO_INTENSITY,
+		.channel2 = IIO_MOD_LIGHT_IR,
+		.modified = 1,
+
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 16,
+			.storagebits = 16,
+			.endianness = IIO_BE,
+		},
+	},
+	{
+		.type = IIO_INTENSITY,
+		.channel2 = IIO_MOD_LIGHT_RED,
+		.modified = 1,
+
+		.scan_index = 1,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 16,
+			.storagebits = 16,
+			.endianness = IIO_BE,
+		},
+	},
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = -1,
+	},
+};
+
+static int max30100_set_powermode(struct max30100_data *data, bool state)
+{
+	return regmap_update_bits(data->regmap, MAX30100_REG_MODE_CONFIG,
+				  MAX30100_REG_MODE_CONFIG_PWR,
+				  state ? 0 : MAX30100_REG_MODE_CONFIG_PWR);
+}
+
+static int max30100_clear_fifo(struct max30100_data *data)
+{
+	int ret;
+
+	ret = regmap_write(data->regmap, MAX30100_REG_FIFO_WR_PTR, 0);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(data->regmap, MAX30100_REG_FIFO_OVR_CTR, 0);
+	if (ret)
+		return ret;
+
+	return regmap_write(data->regmap, MAX30100_REG_FIFO_RD_PTR, 0);
+}
+
+static int max30100_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct max30100_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = max30100_set_powermode(data, true);
+	if (ret)
+		return ret;
+
+	return max30100_clear_fifo(data);
+}
+
+static int max30100_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct max30100_data *data = iio_priv(indio_dev);
+
+	return max30100_set_powermode(data, false);
+}
+
+static const struct iio_buffer_setup_ops max30100_buffer_setup_ops = {
+	.postenable = max30100_buffer_postenable,
+	.predisable = max30100_buffer_predisable,
+};
+
+static inline int max30100_fifo_count(struct max30100_data *data)
+{
+	unsigned int val;
+	int ret;
+
+	ret = regmap_read(data->regmap, MAX30100_REG_INT_STATUS, &val);
+	if (ret)
+		return ret;
+
+	/* FIFO is almost full */
+	if (val & MAX30100_REG_INT_STATUS_FIFO_RDY)
+		return MAX30100_REG_FIFO_DATA_ENTRY_COUNT - 1;
+
+	return 0;
+}
+
+static int max30100_read_measurement(struct max30100_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_read_i2c_block_data(data->client,
+					    MAX30100_REG_FIFO_DATA,
+					    MAX30100_REG_FIFO_DATA_ENTRY_LEN,
+					    (u8 *) &data->buffer);
+
+	return (ret == MAX30100_REG_FIFO_DATA_ENTRY_LEN) ? 0 : ret;
+}
+
+static irqreturn_t max30100_interrupt_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct max30100_data *data = iio_priv(indio_dev);
+	int ret, cnt = 0;
+
+	mutex_lock(&data->lock);
+
+	while (cnt-- || (cnt = max30100_fifo_count(data) > 0)) {
+		ret = max30100_read_measurement(data);
+		if (ret)
+			break;
+
+		iio_push_to_buffers(data->indio_dev, data->buffer);
+	}
+
+	mutex_unlock(&data->lock);
+
+	return IRQ_HANDLED;
+}
+
+static int max30100_chip_init(struct max30100_data *data)
+{
+	int ret;
+
+	/* RED IR LED = 24mA, IR LED = 50mA */
+	ret = regmap_write(data->regmap, MAX30100_REG_LED_CONFIG,
+				(MAX30100_REG_LED_CONFIG_24MA <<
+				 MAX30100_REG_LED_CONFIG_RED_LED_SHIFT) |
+				 MAX30100_REG_LED_CONFIG_50MA);
+	if (ret)
+		return ret;
+
+	/* enable hi-res SPO2 readings at 100Hz */
+	ret = regmap_write(data->regmap, MAX30100_REG_SPO2_CONFIG,
+				 MAX30100_REG_SPO2_CONFIG_HI_RES_EN |
+				 MAX30100_REG_SPO2_CONFIG_100HZ);
+	if (ret)
+		return ret;
+
+	/* enable SPO2 mode */
+	ret = regmap_update_bits(data->regmap, MAX30100_REG_MODE_CONFIG,
+				 MAX30100_REG_MODE_CONFIG_MODE_MASK,
+				 MAX30100_REG_MODE_CONFIG_MODE_HR_EN |
+				 MAX30100_REG_MODE_CONFIG_MODE_SPO2_EN);
+	if (ret)
+		return ret;
+
+	/* enable FIFO interrupt */
+	return regmap_update_bits(data->regmap, MAX30100_REG_INT_ENABLE,
+				 MAX30100_REG_INT_ENABLE_MASK,
+				 MAX30100_REG_INT_ENABLE_FIFO_EN
+				 << MAX30100_REG_INT_ENABLE_MASK_SHIFT);
+}
+
+static int max30100_read_temp(struct max30100_data *data, int *val)
+{
+	int ret;
+	unsigned int reg;
+
+	ret = regmap_read(data->regmap, MAX30100_REG_TEMP_INTEGER, &reg);
+	if (ret < 0)
+		return ret;
+	*val = reg << 4;
+
+	ret = regmap_read(data->regmap, MAX30100_REG_TEMP_FRACTION, &reg);
+	if (ret < 0)
+		return ret;
+
+	*val |= reg & 0xf;
+	*val = sign_extend32(*val, 11);
+
+	return 0;
+}
+
+static int max30100_get_temp(struct max30100_data *data, int *val)
+{
+	int ret;
+
+	/* start acquisition */
+	ret = regmap_update_bits(data->regmap, MAX30100_REG_MODE_CONFIG,
+				 MAX30100_REG_MODE_CONFIG_TEMP_EN,
+				 MAX30100_REG_MODE_CONFIG_TEMP_EN);
+	if (ret)
+		return ret;
+
+	usleep_range(35000, 50000);
+
+	return max30100_read_temp(data, val);
+}
+
+static int max30100_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int *val, int *val2, long mask)
+{
+	struct max30100_data *data = iio_priv(indio_dev);
+	int ret = -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * Temperature reading can only be acquired while engine
+		 * is running
+		 */
+		mutex_lock(&indio_dev->mlock);
+
+		if (!iio_buffer_enabled(indio_dev))
+			ret = -EAGAIN;
+		else {
+			ret = max30100_get_temp(data, val);
+			if (!ret)
+				ret = IIO_VAL_INT;
+
+		}
+
+		mutex_unlock(&indio_dev->mlock);
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 1;  /* 0.0625 */
+		*val2 = 16;
+		ret = IIO_VAL_FRACTIONAL;
+		break;
+	}
+
+	return ret;
+}
+
+static const struct iio_info max30100_info = {
+	.driver_module = THIS_MODULE,
+	.read_raw = max30100_read_raw,
+};
+
+static int max30100_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct max30100_data *data;
+	struct iio_buffer *buffer;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	buffer = devm_iio_kfifo_allocate(&client->dev);
+	if (!buffer)
+		return -ENOMEM;
+
+	iio_device_attach_buffer(indio_dev, buffer);
+
+	indio_dev->name = MAX30100_DRV_NAME;
+	indio_dev->channels = max30100_channels;
+	indio_dev->info = &max30100_info;
+	indio_dev->num_channels = ARRAY_SIZE(max30100_channels);
+	indio_dev->available_scan_masks = max30100_scan_masks;
+	indio_dev->modes = (INDIO_BUFFER_SOFTWARE | INDIO_DIRECT_MODE);
+	indio_dev->setup_ops = &max30100_buffer_setup_ops;
+
+	data = iio_priv(indio_dev);
+	data->indio_dev = indio_dev;
+	data->client = client;
+
+	mutex_init(&data->lock);
+	i2c_set_clientdata(client, indio_dev);
+
+	data->regmap = devm_regmap_init_i2c(client, &max30100_regmap_config);
+	if (IS_ERR(data->regmap)) {
+		dev_err(&client->dev, "regmap initialization failed.\n");
+		return PTR_ERR(data->regmap);
+	}
+	max30100_set_powermode(data, false);
+
+	ret = max30100_chip_init(data);
+	if (ret)
+		return ret;
+
+	if (client->irq <= 0) {
+		dev_err(&client->dev, "no valid irq defined\n");
+		return -EINVAL;
+	}
+	ret = devm_request_threaded_irq(&client->dev, client->irq,
+					NULL, max30100_interrupt_handler,
+					IRQF_TRIGGER_FALLING | IRQF_ONESHOT,
+					"max30100_irq", indio_dev);
+	if (ret) {
+		dev_err(&client->dev, "request irq (%d) failed\n", client->irq);
+		return ret;
+	}
+
+	return iio_device_register(indio_dev);
+}
+
+static int max30100_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct max30100_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	max30100_set_powermode(data, false);
+
+	return 0;
+}
+
+static const struct i2c_device_id max30100_id[] = {
+	{ "max30100", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, max30100_id);
+
+static const struct of_device_id max30100_dt_ids[] = {
+	{ .compatible = "maxim,max30100" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, max30100_dt_ids);
+
+static struct i2c_driver max30100_driver = {
+	.driver = {
+		.name	= MAX30100_DRV_NAME,
+		.of_match_table	= of_match_ptr(max30100_dt_ids),
+	},
+	.probe		= max30100_probe,
+	.remove		= max30100_remove,
+	.id_table	= max30100_id,
+};
+module_i2c_driver(max30100_driver);
+
+MODULE_AUTHOR("Matt Ranostay <mranostay@gmail.com>");
+MODULE_DESCRIPTION("MAX30100 heart rate and pulse oximeter sensor");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/humidity/dht11.c b/drivers/iio/humidity/dht11.c
index 1165b1c4f9d6..cfc5a051ab9f 100644
--- a/drivers/iio/humidity/dht11.c
+++ b/drivers/iio/humidity/dht11.c
@@ -117,7 +117,7 @@ static int dht11_decode(struct dht11 *dht11, int offset, int timeres)
 	if (((hum_int + hum_dec + temp_int + temp_dec) & 0xff) != checksum)
 		return -EIO;
 
-	dht11->timestamp = ktime_get_real_ns();
+	dht11->timestamp = ktime_get_boot_ns();
 	if (hum_int < 20) {  /* DHT22 */
 		dht11->temperature = (((temp_int & 0x7f) << 8) + temp_dec) *
 					((temp_int & 0x80) ? -100 : 100);
@@ -145,7 +145,7 @@ static irqreturn_t dht11_handle_irq(int irq, void *data)
 
 	/* TODO: Consider making the handler safe for IRQ sharing */
 	if (dht11->num_edges < DHT11_EDGES_PER_READ && dht11->num_edges >= 0) {
-		dht11->edges[dht11->num_edges].ts = ktime_get_real_ns();
+		dht11->edges[dht11->num_edges].ts = ktime_get_boot_ns();
 		dht11->edges[dht11->num_edges++].value =
 						gpio_get_value(dht11->gpio);
 
@@ -164,7 +164,7 @@ static int dht11_read_raw(struct iio_dev *iio_dev,
 	int ret, timeres;
 
 	mutex_lock(&dht11->lock);
-	if (dht11->timestamp + DHT11_DATA_VALID_TIME < ktime_get_real_ns()) {
+	if (dht11->timestamp + DHT11_DATA_VALID_TIME < ktime_get_boot_ns()) {
 		timeres = ktime_get_resolution_ns();
 		if (DHT11_DATA_BIT_HIGH < 2 * timeres) {
 			dev_err(dht11->dev, "timeresolution %dns too low\n",
@@ -279,7 +279,7 @@ static int dht11_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	dht11->timestamp = ktime_get_real_ns() - DHT11_DATA_VALID_TIME - 1;
+	dht11->timestamp = ktime_get_boot_ns() - DHT11_DATA_VALID_TIME - 1;
 	dht11->num_edges = -1;
 
 	platform_set_drvdata(pdev, iio);
diff --git a/drivers/iio/imu/adis16400_core.c b/drivers/iio/imu/adis16400_core.c
index 0618f831ecd4..fb7c0dbed51c 100644
--- a/drivers/iio/imu/adis16400_core.c
+++ b/drivers/iio/imu/adis16400_core.c
@@ -288,7 +288,11 @@ static int adis16400_initial_setup(struct iio_dev *indio_dev)
 		if (ret)
 			goto err_ret;
 
-		sscanf(indio_dev->name, "adis%u\n", &device_id);
+		ret = sscanf(indio_dev->name, "adis%u\n", &device_id);
+		if (ret != 1) {
+			ret = -EINVAL;
+			goto err_ret;
+		}
 
 		if (prod_id != device_id)
 			dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.",
diff --git a/drivers/iio/imu/adis16480.c b/drivers/iio/imu/adis16480.c
index 2485b88ee1b6..8cf84d3488b2 100644
--- a/drivers/iio/imu/adis16480.c
+++ b/drivers/iio/imu/adis16480.c
@@ -765,7 +765,9 @@ static int adis16480_initial_setup(struct iio_dev *indio_dev)
 	if (ret)
 		return ret;
 
-	sscanf(indio_dev->name, "adis%u\n", &device_id);
+	ret = sscanf(indio_dev->name, "adis%u\n", &device_id);
+	if (ret != 1)
+		return -EINVAL;
 
 	if (prod_id != device_id)
 		dev_warn(&indio_dev->dev, "Device ID(%u) and product ID(%u) do not match.",
diff --git a/drivers/iio/imu/adis_buffer.c b/drivers/iio/imu/adis_buffer.c
index cb32b593f1c5..36607d52fee0 100644
--- a/drivers/iio/imu/adis_buffer.c
+++ b/drivers/iio/imu/adis_buffer.c
@@ -43,7 +43,7 @@ int adis_update_scan_mode(struct iio_dev *indio_dev,
 		return -ENOMEM;
 
 	rx = adis->buffer;
-	tx = rx + indio_dev->scan_bytes;
+	tx = rx + scan_count;
 
 	spi_message_init(&adis->msg);
 
diff --git a/drivers/iio/imu/inv_mpu6050/Kconfig b/drivers/iio/imu/inv_mpu6050/Kconfig
index 48fbc0bc7e2a..8f8d1370ed8b 100644
--- a/drivers/iio/imu/inv_mpu6050/Kconfig
+++ b/drivers/iio/imu/inv_mpu6050/Kconfig
@@ -5,9 +5,9 @@
 config INV_MPU6050_IIO
 	tristate "Invensense MPU6050 devices"
 	depends on I2C && SYSFS
+	depends on I2C_MUX
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
-	select I2C_MUX
 	help
 	  This driver supports the Invensense MPU6050 devices.
 	  This driver can also support MPU6500 in MPU6050 compatibility mode
diff --git a/drivers/iio/imu/kmx61.c b/drivers/iio/imu/kmx61.c
index dbf5e9936635..e5306b4e020e 100644
--- a/drivers/iio/imu/kmx61.c
+++ b/drivers/iio/imu/kmx61.c
@@ -1390,6 +1390,14 @@ static int kmx61_probe(struct i2c_client *client,
 		}
 	}
 
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret < 0)
+		goto err_buffer_cleanup_mag;
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev, KMX61_SLEEP_DELAY_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
 	ret = iio_device_register(data->acc_indio_dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "Failed to register acc iio device\n");
@@ -1402,18 +1410,8 @@ static int kmx61_probe(struct i2c_client *client,
 		goto err_iio_unregister_acc;
 	}
 
-	ret = pm_runtime_set_active(&client->dev);
-	if (ret < 0)
-		goto err_iio_unregister_mag;
-
-	pm_runtime_enable(&client->dev);
-	pm_runtime_set_autosuspend_delay(&client->dev, KMX61_SLEEP_DELAY_MS);
-	pm_runtime_use_autosuspend(&client->dev);
-
 	return 0;
 
-err_iio_unregister_mag:
-	iio_device_unregister(data->mag_indio_dev);
 err_iio_unregister_acc:
 	iio_device_unregister(data->acc_indio_dev);
 err_buffer_cleanup_mag:
@@ -1437,13 +1435,13 @@ static int kmx61_remove(struct i2c_client *client)
 {
 	struct kmx61_data *data = i2c_get_clientdata(client);
 
+	iio_device_unregister(data->acc_indio_dev);
+	iio_device_unregister(data->mag_indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(data->acc_indio_dev);
-	iio_device_unregister(data->mag_indio_dev);
-
 	if (client->irq > 0) {
 		iio_triggered_buffer_cleanup(data->acc_indio_dev);
 		iio_triggered_buffer_cleanup(data->mag_indio_dev);
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 0f6f63b20263..139ae916225f 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -193,7 +193,8 @@ void iio_buffer_init(struct iio_buffer *buffer)
 	INIT_LIST_HEAD(&buffer->buffer_list);
 	init_waitqueue_head(&buffer->pollq);
 	kref_init(&buffer->ref);
-	buffer->watermark = 1;
+	if (!buffer->watermark)
+		buffer->watermark = 1;
 }
 EXPORT_SYMBOL(iio_buffer_init);
 
@@ -567,6 +568,22 @@ static void iio_buffer_deactivate_all(struct iio_dev *indio_dev)
 		iio_buffer_deactivate(buffer);
 }
 
+static int iio_buffer_enable(struct iio_buffer *buffer,
+	struct iio_dev *indio_dev)
+{
+	if (!buffer->access->enable)
+		return 0;
+	return buffer->access->enable(buffer, indio_dev);
+}
+
+static int iio_buffer_disable(struct iio_buffer *buffer,
+	struct iio_dev *indio_dev)
+{
+	if (!buffer->access->disable)
+		return 0;
+	return buffer->access->disable(buffer, indio_dev);
+}
+
 static void iio_buffer_update_bytes_per_datum(struct iio_dev *indio_dev,
 	struct iio_buffer *buffer)
 {
@@ -610,6 +627,7 @@ static void iio_free_scan_mask(struct iio_dev *indio_dev,
 
 struct iio_device_config {
 	unsigned int mode;
+	unsigned int watermark;
 	const unsigned long *scan_mask;
 	unsigned int scan_bytes;
 	bool scan_timestamp;
@@ -642,10 +660,14 @@ static int iio_verify_update(struct iio_dev *indio_dev,
 		if (buffer == remove_buffer)
 			continue;
 		modes &= buffer->access->modes;
+		config->watermark = min(config->watermark, buffer->watermark);
 	}
 
-	if (insert_buffer)
+	if (insert_buffer) {
 		modes &= insert_buffer->access->modes;
+		config->watermark = min(config->watermark,
+			insert_buffer->watermark);
+	}
 
 	/* Definitely possible for devices to support both of these. */
 	if ((modes & INDIO_BUFFER_TRIGGERED) && indio_dev->trig) {
@@ -713,6 +735,7 @@ static int iio_verify_update(struct iio_dev *indio_dev,
 static int iio_enable_buffers(struct iio_dev *indio_dev,
 	struct iio_device_config *config)
 {
+	struct iio_buffer *buffer;
 	int ret;
 
 	indio_dev->active_scan_mask = config->scan_mask;
@@ -743,6 +766,16 @@ static int iio_enable_buffers(struct iio_dev *indio_dev,
 		}
 	}
 
+	if (indio_dev->info->hwfifo_set_watermark)
+		indio_dev->info->hwfifo_set_watermark(indio_dev,
+			config->watermark);
+
+	list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
+		ret = iio_buffer_enable(buffer, indio_dev);
+		if (ret)
+			goto err_disable_buffers;
+	}
+
 	indio_dev->currentmode = config->mode;
 
 	if (indio_dev->setup_ops->postenable) {
@@ -750,12 +783,16 @@ static int iio_enable_buffers(struct iio_dev *indio_dev,
 		if (ret) {
 			dev_dbg(&indio_dev->dev,
 			       "Buffer not started: postenable failed (%d)\n", ret);
-			goto err_run_postdisable;
+			goto err_disable_buffers;
 		}
 	}
 
 	return 0;
 
+err_disable_buffers:
+	list_for_each_entry_continue_reverse(buffer, &indio_dev->buffer_list,
+					     buffer_list)
+		iio_buffer_disable(buffer, indio_dev);
 err_run_postdisable:
 	indio_dev->currentmode = INDIO_DIRECT_MODE;
 	if (indio_dev->setup_ops->postdisable)
@@ -768,6 +805,7 @@ err_undo_config:
 
 static int iio_disable_buffers(struct iio_dev *indio_dev)
 {
+	struct iio_buffer *buffer;
 	int ret = 0;
 	int ret2;
 
@@ -788,6 +826,12 @@ static int iio_disable_buffers(struct iio_dev *indio_dev)
 			ret = ret2;
 	}
 
+	list_for_each_entry(buffer, &indio_dev->buffer_list, buffer_list) {
+		ret2 = iio_buffer_disable(buffer, indio_dev);
+		if (ret2 && !ret)
+			ret = ret2;
+	}
+
 	indio_dev->currentmode = INDIO_DIRECT_MODE;
 
 	if (indio_dev->setup_ops->postdisable) {
@@ -974,9 +1018,6 @@ static ssize_t iio_buffer_store_watermark(struct device *dev,
 	}
 
 	buffer->watermark = val;
-
-	if (indio_dev->info->hwfifo_set_watermark)
-		indio_dev->info->hwfifo_set_watermark(indio_dev, val);
 out:
 	mutex_unlock(&indio_dev->mlock);
 
@@ -991,6 +1032,8 @@ static DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,
 		   iio_buffer_show_enable, iio_buffer_store_enable);
 static DEVICE_ATTR(watermark, S_IRUGO | S_IWUSR,
 		   iio_buffer_show_watermark, iio_buffer_store_watermark);
+static struct device_attribute dev_attr_watermark_ro = __ATTR(watermark,
+	S_IRUGO, iio_buffer_show_watermark, NULL);
 
 static struct attribute *iio_buffer_attrs[] = {
 	&dev_attr_length.attr,
@@ -1033,6 +1076,9 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 	if (!buffer->access->set_length)
 		attr[0] = &dev_attr_length_ro.attr;
 
+	if (buffer->access->flags & INDIO_BUFFER_FLAG_FIXED_WATERMARK)
+		attr[2] = &dev_attr_watermark_ro.attr;
+
 	if (buffer->attrs)
 		memcpy(&attr[ARRAY_SIZE(iio_buffer_attrs)], buffer->attrs,
 		       sizeof(struct attribute *) * attrcount);
diff --git a/drivers/iio/industrialio-configfs.c b/drivers/iio/industrialio-configfs.c
new file mode 100644
index 000000000000..45ce2bc47180
--- /dev/null
+++ b/drivers/iio/industrialio-configfs.c
@@ -0,0 +1,51 @@
+/*
+ * Industrial I/O configfs bits
+ *
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/configfs.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/slab.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/configfs.h>
+
+static struct config_item_type iio_root_group_type = {
+	.ct_owner       = THIS_MODULE,
+};
+
+struct configfs_subsystem iio_configfs_subsys = {
+	.su_group = {
+		.cg_item = {
+			.ci_namebuf = "iio",
+			.ci_type = &iio_root_group_type,
+		},
+	},
+	.su_mutex = __MUTEX_INITIALIZER(iio_configfs_subsys.su_mutex),
+};
+EXPORT_SYMBOL(iio_configfs_subsys);
+
+static int __init iio_configfs_init(void)
+{
+	config_group_init(&iio_configfs_subsys.su_group);
+
+	return configfs_register_subsystem(&iio_configfs_subsys);
+}
+module_init(iio_configfs_init);
+
+static void __exit iio_configfs_exit(void)
+{
+	configfs_unregister_subsystem(&iio_configfs_subsys);
+}
+module_exit(iio_configfs_exit);
+
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("Industrial I/O configfs support");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 159ede61f793..af7cc1e65656 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -433,16 +433,15 @@ ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals)
 		scale_db = true;
 	case IIO_VAL_INT_PLUS_MICRO:
 		if (vals[1] < 0)
-			return sprintf(buf, "-%ld.%06u%s\n", abs(vals[0]),
-					-vals[1],
-				scale_db ? " dB" : "");
+			return sprintf(buf, "-%d.%06u%s\n", abs(vals[0]),
+				       -vals[1], scale_db ? " dB" : "");
 		else
 			return sprintf(buf, "%d.%06u%s\n", vals[0], vals[1],
 				scale_db ? " dB" : "");
 	case IIO_VAL_INT_PLUS_NANO:
 		if (vals[1] < 0)
-			return sprintf(buf, "-%ld.%09u\n", abs(vals[0]),
-					-vals[1]);
+			return sprintf(buf, "-%d.%09u\n", abs(vals[0]),
+				       -vals[1]);
 		else
 			return sprintf(buf, "%d.%09u\n", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL:
@@ -470,6 +469,7 @@ ssize_t iio_format_value(char *buf, unsigned int type, int size, int *vals)
 		return 0;
 	}
 }
+EXPORT_SYMBOL_GPL(iio_format_value);
 
 static ssize_t iio_read_channel_info(struct device *dev,
 				     struct device_attribute *attr,
@@ -512,6 +512,12 @@ int iio_str_to_fixpoint(const char *str, int fract_mult,
 	int i = 0, f = 0;
 	bool integer_part = true, negative = false;
 
+	if (fract_mult == 0) {
+		*fract = 0;
+
+		return kstrtoint(str, 0, integer);
+	}
+
 	if (str[0] == '-') {
 		negative = true;
 		str++;
@@ -571,6 +577,9 @@ static ssize_t iio_write_channel_info(struct device *dev,
 	if (indio_dev->info->write_raw_get_fmt)
 		switch (indio_dev->info->write_raw_get_fmt(indio_dev,
 			this_attr->c, this_attr->address)) {
+		case IIO_VAL_INT:
+			fract_mult = 0;
+			break;
 		case IIO_VAL_INT_PLUS_MICRO:
 			fract_mult = 100000;
 			break;
diff --git a/drivers/iio/industrialio-sw-trigger.c b/drivers/iio/industrialio-sw-trigger.c
new file mode 100644
index 000000000000..8d24fb159cc9
--- /dev/null
+++ b/drivers/iio/industrialio-sw-trigger.c
@@ -0,0 +1,182 @@
+/*
+ * The Industrial I/O core, software trigger functions
+ *
+ * Copyright (c) 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kmod.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+
+#include <linux/iio/sw_trigger.h>
+#include <linux/iio/configfs.h>
+#include <linux/configfs.h>
+
+static struct config_group *iio_triggers_group;
+static struct config_item_type iio_trigger_type_group_type;
+
+static struct config_item_type iio_triggers_group_type = {
+	.ct_owner = THIS_MODULE,
+};
+
+static LIST_HEAD(iio_trigger_types_list);
+static DEFINE_MUTEX(iio_trigger_types_lock);
+
+static
+struct iio_sw_trigger_type *__iio_find_sw_trigger_type(const char *name,
+						       unsigned len)
+{
+	struct iio_sw_trigger_type *t = NULL, *iter;
+
+	list_for_each_entry(iter, &iio_trigger_types_list, list)
+		if (!strcmp(iter->name, name)) {
+			t = iter;
+			break;
+		}
+
+	return t;
+}
+
+int iio_register_sw_trigger_type(struct iio_sw_trigger_type *t)
+{
+	struct iio_sw_trigger_type *iter;
+	int ret = 0;
+
+	mutex_lock(&iio_trigger_types_lock);
+	iter = __iio_find_sw_trigger_type(t->name, strlen(t->name));
+	if (iter)
+		ret = -EBUSY;
+	else
+		list_add_tail(&t->list, &iio_trigger_types_list);
+	mutex_unlock(&iio_trigger_types_lock);
+
+	if (ret)
+		return ret;
+
+	t->group = configfs_register_default_group(iio_triggers_group, t->name,
+						&iio_trigger_type_group_type);
+	if (IS_ERR(t->group))
+		ret = PTR_ERR(t->group);
+
+	return ret;
+}
+EXPORT_SYMBOL(iio_register_sw_trigger_type);
+
+void iio_unregister_sw_trigger_type(struct iio_sw_trigger_type *t)
+{
+	struct iio_sw_trigger_type *iter;
+
+	mutex_lock(&iio_trigger_types_lock);
+	iter = __iio_find_sw_trigger_type(t->name, strlen(t->name));
+	if (iter)
+		list_del(&t->list);
+	mutex_unlock(&iio_trigger_types_lock);
+
+	configfs_unregister_default_group(t->group);
+}
+EXPORT_SYMBOL(iio_unregister_sw_trigger_type);
+
+static
+struct iio_sw_trigger_type *iio_get_sw_trigger_type(const char *name)
+{
+	struct iio_sw_trigger_type *t;
+
+	mutex_lock(&iio_trigger_types_lock);
+	t = __iio_find_sw_trigger_type(name, strlen(name));
+	if (t && !try_module_get(t->owner))
+		t = NULL;
+	mutex_unlock(&iio_trigger_types_lock);
+
+	return t;
+}
+
+struct iio_sw_trigger *iio_sw_trigger_create(const char *type, const char *name)
+{
+	struct iio_sw_trigger *t;
+	struct iio_sw_trigger_type *tt;
+
+	tt = iio_get_sw_trigger_type(type);
+	if (!tt) {
+		pr_err("Invalid trigger type: %s\n", type);
+		return ERR_PTR(-EINVAL);
+	}
+	t = tt->ops->probe(name);
+	if (IS_ERR(t))
+		goto out_module_put;
+
+	t->trigger_type = tt;
+
+	return t;
+out_module_put:
+	module_put(tt->owner);
+	return t;
+}
+EXPORT_SYMBOL(iio_sw_trigger_create);
+
+void iio_sw_trigger_destroy(struct iio_sw_trigger *t)
+{
+	struct iio_sw_trigger_type *tt = t->trigger_type;
+
+	tt->ops->remove(t);
+	module_put(tt->owner);
+}
+EXPORT_SYMBOL(iio_sw_trigger_destroy);
+
+static struct config_group *trigger_make_group(struct config_group *group,
+					       const char *name)
+{
+	struct iio_sw_trigger *t;
+
+	t = iio_sw_trigger_create(group->cg_item.ci_name, name);
+	if (IS_ERR(t))
+		return ERR_CAST(t);
+
+	config_item_set_name(&t->group.cg_item, "%s", name);
+
+	return &t->group;
+}
+
+static void trigger_drop_group(struct config_group *group,
+			       struct config_item *item)
+{
+	struct iio_sw_trigger *t = to_iio_sw_trigger(item);
+
+	iio_sw_trigger_destroy(t);
+	config_item_put(item);
+}
+
+static struct configfs_group_operations trigger_ops = {
+	.make_group	= &trigger_make_group,
+	.drop_item	= &trigger_drop_group,
+};
+
+static struct config_item_type iio_trigger_type_group_type = {
+	.ct_group_ops = &trigger_ops,
+	.ct_owner       = THIS_MODULE,
+};
+
+static int __init iio_sw_trigger_init(void)
+{
+	iio_triggers_group =
+		configfs_register_default_group(&iio_configfs_subsys.su_group,
+						"triggers",
+						&iio_triggers_group_type);
+	return PTR_ERR_OR_ZERO(iio_triggers_group);
+}
+module_init(iio_sw_trigger_init);
+
+static void __exit iio_sw_trigger_exit(void)
+{
+	configfs_unregister_default_group(iio_triggers_group);
+}
+module_exit(iio_sw_trigger_exit);
+
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("Industrial I/O software triggers support");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index c8bad3cf891d..734a0042de0c 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -61,12 +61,10 @@ EXPORT_SYMBOL_GPL(iio_map_array_register);
 int iio_map_array_unregister(struct iio_dev *indio_dev)
 {
 	int ret = -ENODEV;
-	struct iio_map_internal *mapi;
-	struct list_head *pos, *tmp;
+	struct iio_map_internal *mapi, *next;
 
 	mutex_lock(&iio_map_list_lock);
-	list_for_each_safe(pos, tmp, &iio_map_list) {
-		mapi = list_entry(pos, struct iio_map_internal, l);
+	list_for_each_entry_safe(mapi, next, &iio_map_list, l) {
 		if (indio_dev == mapi->indio_dev) {
 			list_del(&mapi->l);
 			kfree(mapi);
@@ -351,6 +349,8 @@ EXPORT_SYMBOL_GPL(iio_channel_get);
 
 void iio_channel_release(struct iio_channel *channel)
 {
+	if (!channel)
+		return;
 	iio_device_put(channel->indio_dev);
 	kfree(channel);
 }
diff --git a/drivers/iio/light/acpi-als.c b/drivers/iio/light/acpi-als.c
index 60537ec0c923..53201d99a16c 100644
--- a/drivers/iio/light/acpi-als.c
+++ b/drivers/iio/light/acpi-als.c
@@ -54,7 +54,9 @@ static const struct iio_chan_spec acpi_als_channels[] = {
 			.realbits	= 32,
 			.storagebits	= 32,
 		},
-		.info_mask_separate	= BIT(IIO_CHAN_INFO_RAW),
+		/* _RAW is here for backward ABI compatibility */
+		.info_mask_separate	= BIT(IIO_CHAN_INFO_RAW) |
+					  BIT(IIO_CHAN_INFO_PROCESSED),
 	},
 };
 
@@ -152,7 +154,7 @@ static int acpi_als_read_raw(struct iio_dev *indio_dev,
 	s32 temp_val;
 	int ret;
 
-	if (mask != IIO_CHAN_INFO_RAW)
+	if ((mask != IIO_CHAN_INFO_PROCESSED) && (mask != IIO_CHAN_INFO_RAW))
 		return -EINVAL;
 
 	/* we support only illumination (_ALI) so far. */
diff --git a/drivers/iio/light/lm3533-als.c b/drivers/iio/light/lm3533-als.c
index 076bc46fad03..e56937c40a18 100644
--- a/drivers/iio/light/lm3533-als.c
+++ b/drivers/iio/light/lm3533-als.c
@@ -743,8 +743,10 @@ static int lm3533_als_set_resistor(struct lm3533_als *als, u8 val)
 {
 	int ret;
 
-	if (val < LM3533_ALS_RESISTOR_MIN || val > LM3533_ALS_RESISTOR_MAX)
+	if (val < LM3533_ALS_RESISTOR_MIN || val > LM3533_ALS_RESISTOR_MAX) {
+		dev_err(&als->pdev->dev, "invalid resistor value\n");
 		return -EINVAL;
+	};
 
 	ret = lm3533_write(als->lm3533, LM3533_REG_ALS_RESISTOR_SELECT, val);
 	if (ret) {
diff --git a/drivers/iio/light/ltr501.c b/drivers/iio/light/ltr501.c
index 809a961b9a7f..6bf89d8f3741 100644
--- a/drivers/iio/light/ltr501.c
+++ b/drivers/iio/light/ltr501.c
@@ -180,7 +180,7 @@ static const struct ltr501_samp_table ltr501_ps_samp_table[] = {
 			{500000, 2000000}
 };
 
-static unsigned int ltr501_match_samp_freq(const struct ltr501_samp_table *tab,
+static int ltr501_match_samp_freq(const struct ltr501_samp_table *tab,
 					   int len, int val, int val2)
 {
 	int i, freq;
diff --git a/drivers/iio/light/pa12203001.c b/drivers/iio/light/pa12203001.c
index 45f7bde02bbf..76a9e12b46bc 100644
--- a/drivers/iio/light/pa12203001.c
+++ b/drivers/iio/light/pa12203001.c
@@ -381,17 +381,23 @@ static int pa12203001_probe(struct i2c_client *client,
 		return ret;
 
 	ret = pm_runtime_set_active(&client->dev);
-	if (ret < 0) {
-		pa12203001_power_chip(indio_dev, PA12203001_CHIP_DISABLE);
-		return ret;
-	}
+	if (ret < 0)
+		goto out_err;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev,
 					 PA12203001_SLEEP_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
-	return iio_device_register(indio_dev);
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	pa12203001_power_chip(indio_dev, PA12203001_CHIP_DISABLE);
+	return ret;
 }
 
 static int pa12203001_remove(struct i2c_client *client)
diff --git a/drivers/iio/light/rpr0521.c b/drivers/iio/light/rpr0521.c
index 4b75bb0998b3..7de0f397194b 100644
--- a/drivers/iio/light/rpr0521.c
+++ b/drivers/iio/light/rpr0521.c
@@ -507,34 +507,28 @@ static int rpr0521_probe(struct i2c_client *client,
 		dev_err(&client->dev, "rpr0521 chip init failed\n");
 		return ret;
 	}
-	ret = iio_device_register(indio_dev);
-	if (ret < 0)
-		return ret;
 
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret < 0)
-		goto err_iio_unregister;
+		return ret;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev, RPR0521_SLEEP_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
-	return 0;
-
-err_iio_unregister:
-	iio_device_unregister(indio_dev);
-	return ret;
+	return iio_device_register(indio_dev);
 }
 
 static int rpr0521_remove(struct i2c_client *client)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(indio_dev);
 	rpr0521_poweroff(iio_priv(indio_dev));
 
 	return 0;
diff --git a/drivers/iio/light/us5182d.c b/drivers/iio/light/us5182d.c
index 49dab3cb3e23..45bc2f742f46 100644
--- a/drivers/iio/light/us5182d.c
+++ b/drivers/iio/light/us5182d.c
@@ -20,14 +20,21 @@
 #include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/i2c.h>
+#include <linux/iio/events.h>
 #include <linux/iio/iio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/iio/sysfs.h>
 #include <linux/mutex.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
 
 #define US5182D_REG_CFG0				0x00
 #define US5182D_CFG0_ONESHOT_EN				BIT(6)
 #define US5182D_CFG0_SHUTDOWN_EN			BIT(7)
 #define US5182D_CFG0_WORD_ENABLE			BIT(0)
+#define US5182D_CFG0_PROX				BIT(3)
+#define US5182D_CFG0_PX_IRQ				BIT(2)
 
 #define US5182D_REG_CFG1				0x01
 #define US5182D_CFG1_ALS_RES16				BIT(4)
@@ -39,6 +46,7 @@
 
 #define US5182D_REG_CFG3				0x03
 #define US5182D_CFG3_LED_CURRENT100			(BIT(4) | BIT(5))
+#define US5182D_CFG3_INT_SOURCE_PX			BIT(3)
 
 #define US5182D_REG_CFG4				0x10
 
@@ -53,6 +61,13 @@
 #define US5182D_REG_AUTO_LDARK_GAIN		0x29
 #define US5182D_REG_AUTO_HDARK_GAIN		0x2a
 
+/* Thresholds for events: px low (0x08-l, 0x09-h), px high (0x0a-l 0x0b-h) */
+#define US5182D_REG_PXL_TH			0x08
+#define US5182D_REG_PXH_TH			0x0a
+
+#define US5182D_REG_PXL_TH_DEFAULT		1000
+#define US5182D_REG_PXH_TH_DEFAULT		30000
+
 #define US5182D_OPMODE_ALS			0x01
 #define US5182D_OPMODE_PX			0x02
 #define US5182D_OPMODE_SHIFT			4
@@ -81,6 +96,9 @@
 #define US5182D_READ_BYTE			1
 #define US5182D_READ_WORD			2
 #define US5182D_OPSTORE_SLEEP_TIME		20 /* ms */
+#define US5182D_SLEEP_MS			3000 /* ms */
+#define US5182D_PXH_TH_DISABLE			0xffff
+#define US5182D_PXL_TH_DISABLE			0x0000
 
 /* Available ranges: [12354, 7065, 3998, 2202, 1285, 498, 256, 138] lux */
 static const int us5182d_scales[] = {188500, 107800, 61000, 33600, 19600, 7600,
@@ -99,6 +117,11 @@ enum mode {
 	US5182D_PX_ONLY
 };
 
+enum pmode {
+	US5182D_CONTINUOUS,
+	US5182D_ONESHOT
+};
+
 struct us5182d_data {
 	struct i2c_client *client;
 	struct mutex lock;
@@ -111,7 +134,19 @@ struct us5182d_data {
 	u8 upper_dark_gain;
 	u16 *us5182d_dark_ths;
 
+	u16 px_low_th;
+	u16 px_high_th;
+
+	int rising_en;
+	int falling_en;
+
 	u8 opmode;
+	u8 power_mode;
+
+	bool als_enabled;
+	bool px_enabled;
+
+	bool default_continuous;
 };
 
 static IIO_CONST_ATTR(in_illuminance_scale_available,
@@ -130,16 +165,30 @@ static const struct {
 	u8 reg;
 	u8 val;
 } us5182d_regvals[] = {
-	{US5182D_REG_CFG0, (US5182D_CFG0_SHUTDOWN_EN |
-			    US5182D_CFG0_WORD_ENABLE)},
+	{US5182D_REG_CFG0, US5182D_CFG0_WORD_ENABLE},
 	{US5182D_REG_CFG1, US5182D_CFG1_ALS_RES16},
 	{US5182D_REG_CFG2, (US5182D_CFG2_PX_RES16 |
 			    US5182D_CFG2_PXGAIN_DEFAULT)},
-	{US5182D_REG_CFG3, US5182D_CFG3_LED_CURRENT100},
-	{US5182D_REG_MODE_STORE, US5182D_STORE_MODE},
+	{US5182D_REG_CFG3, US5182D_CFG3_LED_CURRENT100 |
+			   US5182D_CFG3_INT_SOURCE_PX},
 	{US5182D_REG_CFG4, 0x00},
 };
 
+static const struct iio_event_spec us5182d_events[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				BIT(IIO_EV_INFO_ENABLE),
+	},
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				BIT(IIO_EV_INFO_ENABLE),
+	},
+};
+
 static const struct iio_chan_spec us5182d_channels[] = {
 	{
 		.type = IIO_LIGHT,
@@ -149,40 +198,39 @@ static const struct iio_chan_spec us5182d_channels[] = {
 	{
 		.type = IIO_PROXIMITY,
 		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.event_spec = us5182d_events,
+		.num_event_specs = ARRAY_SIZE(us5182d_events),
 	}
 };
 
-static int us5182d_get_als(struct us5182d_data *data)
+static int us5182d_oneshot_en(struct us5182d_data *data)
 {
 	int ret;
-	unsigned long result;
 
-	ret = i2c_smbus_read_word_data(data->client,
-				       US5182D_REG_ADL);
+	ret = i2c_smbus_read_byte_data(data->client, US5182D_REG_CFG0);
 	if (ret < 0)
 		return ret;
 
-	result = ret * data->ga / US5182D_GA_RESOLUTION;
-	if (result > 0xffff)
-		result = 0xffff;
+	/*
+	 * In oneshot mode the chip will power itself down after taking the
+	 * required measurement.
+	 */
+	ret = ret | US5182D_CFG0_ONESHOT_EN;
 
-	return result;
+	return i2c_smbus_write_byte_data(data->client, US5182D_REG_CFG0, ret);
 }
 
 static int us5182d_set_opmode(struct us5182d_data *data, u8 mode)
 {
 	int ret;
 
+	if (mode == data->opmode)
+		return 0;
+
 	ret = i2c_smbus_read_byte_data(data->client, US5182D_REG_CFG0);
 	if (ret < 0)
 		return ret;
 
-	/*
-	 * In oneshot mode the chip will power itself down after taking the
-	 * required measurement.
-	 */
-	ret = ret | US5182D_CFG0_ONESHOT_EN;
-
 	/* update mode */
 	ret = ret & ~US5182D_OPMODE_MASK;
 	ret = ret | (mode << US5182D_OPMODE_SHIFT);
@@ -196,9 +244,6 @@ static int us5182d_set_opmode(struct us5182d_data *data, u8 mode)
 	if (ret < 0)
 		return ret;
 
-	if (mode == data->opmode)
-		return 0;
-
 	ret = i2c_smbus_write_byte_data(data->client, US5182D_REG_MODE_STORE,
 					US5182D_STORE_MODE);
 	if (ret < 0)
@@ -210,6 +255,177 @@ static int us5182d_set_opmode(struct us5182d_data *data, u8 mode)
 	return 0;
 }
 
+static int us5182d_als_enable(struct us5182d_data *data)
+{
+	int ret;
+	u8 mode;
+
+	if (data->power_mode == US5182D_ONESHOT) {
+		ret = us5182d_set_opmode(data, US5182D_ALS_ONLY);
+		if (ret < 0)
+			return ret;
+		data->px_enabled = false;
+	}
+
+	if (data->als_enabled)
+		return 0;
+
+	mode = data->px_enabled ? US5182D_ALS_PX : US5182D_ALS_ONLY;
+
+	ret = us5182d_set_opmode(data, mode);
+	if (ret < 0)
+		return ret;
+
+	data->als_enabled = true;
+
+	return 0;
+}
+
+static int us5182d_px_enable(struct us5182d_data *data)
+{
+	int ret;
+	u8 mode;
+
+	if (data->power_mode == US5182D_ONESHOT) {
+		ret = us5182d_set_opmode(data, US5182D_PX_ONLY);
+		if (ret < 0)
+			return ret;
+		data->als_enabled = false;
+	}
+
+	if (data->px_enabled)
+		return 0;
+
+	mode = data->als_enabled ? US5182D_ALS_PX : US5182D_PX_ONLY;
+
+	ret = us5182d_set_opmode(data, mode);
+	if (ret < 0)
+		return ret;
+
+	data->px_enabled = true;
+
+	return 0;
+}
+
+static int us5182d_get_als(struct us5182d_data *data)
+{
+	int ret;
+	unsigned long result;
+
+	ret = us5182d_als_enable(data);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_word_data(data->client,
+				       US5182D_REG_ADL);
+	if (ret < 0)
+		return ret;
+
+	result = ret * data->ga / US5182D_GA_RESOLUTION;
+	if (result > 0xffff)
+		result = 0xffff;
+
+	return result;
+}
+
+static int us5182d_get_px(struct us5182d_data *data)
+{
+	int ret;
+
+	ret = us5182d_px_enable(data);
+	if (ret < 0)
+		return ret;
+
+	return i2c_smbus_read_word_data(data->client,
+					US5182D_REG_PDL);
+}
+
+static int us5182d_shutdown_en(struct us5182d_data *data, u8 state)
+{
+	int ret;
+
+	if (data->power_mode == US5182D_ONESHOT)
+		return 0;
+
+	ret = i2c_smbus_read_byte_data(data->client, US5182D_REG_CFG0);
+	if (ret < 0)
+		return ret;
+
+	ret = ret & ~US5182D_CFG0_SHUTDOWN_EN;
+	ret = ret | state;
+
+	ret = i2c_smbus_write_byte_data(data->client, US5182D_REG_CFG0, ret);
+	if (ret < 0)
+		return ret;
+
+	if (state & US5182D_CFG0_SHUTDOWN_EN) {
+		data->als_enabled = false;
+		data->px_enabled = false;
+	}
+
+	return ret;
+}
+
+
+static int us5182d_set_power_state(struct us5182d_data *data, bool on)
+{
+	int ret;
+
+	if (data->power_mode == US5182D_ONESHOT)
+		return 0;
+
+	if (on) {
+		ret = pm_runtime_get_sync(&data->client->dev);
+		if (ret < 0)
+			pm_runtime_put_noidle(&data->client->dev);
+	} else {
+		pm_runtime_mark_last_busy(&data->client->dev);
+		ret = pm_runtime_put_autosuspend(&data->client->dev);
+	}
+
+	return ret;
+}
+
+static int us5182d_read_value(struct us5182d_data *data,
+			      struct iio_chan_spec const *chan)
+{
+	int ret, value;
+
+	mutex_lock(&data->lock);
+
+	if (data->power_mode == US5182D_ONESHOT) {
+		ret = us5182d_oneshot_en(data);
+		if (ret < 0)
+			goto out_err;
+	}
+
+	ret = us5182d_set_power_state(data, true);
+	if (ret < 0)
+		goto out_err;
+
+	if (chan->type == IIO_LIGHT)
+		ret = us5182d_get_als(data);
+	else
+		ret = us5182d_get_px(data);
+	if (ret < 0)
+		goto out_poweroff;
+
+	value = ret;
+
+	ret = us5182d_set_power_state(data, false);
+	if (ret < 0)
+		goto out_err;
+
+	mutex_unlock(&data->lock);
+	return value;
+
+out_poweroff:
+	us5182d_set_power_state(data, false);
+out_err:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
 static int us5182d_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *chan, int *val,
 			    int *val2, long mask)
@@ -219,53 +435,21 @@ static int us5182d_read_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		switch (chan->type) {
-		case IIO_LIGHT:
-			mutex_lock(&data->lock);
-			ret = us5182d_set_opmode(data, US5182D_OPMODE_ALS);
-			if (ret < 0)
-				goto out_err;
-
-			ret = us5182d_get_als(data);
-			if (ret < 0)
-				goto out_err;
-			mutex_unlock(&data->lock);
-			*val = ret;
-			return IIO_VAL_INT;
-		case IIO_PROXIMITY:
-			mutex_lock(&data->lock);
-			ret = us5182d_set_opmode(data, US5182D_OPMODE_PX);
-			if (ret < 0)
-				goto out_err;
-
-			ret = i2c_smbus_read_word_data(data->client,
-						       US5182D_REG_PDL);
-			if (ret < 0)
-				goto out_err;
-			mutex_unlock(&data->lock);
-			*val = ret;
-			return  IIO_VAL_INT;
-		default:
-			return -EINVAL;
-		}
-
+		ret = us5182d_read_value(data, chan);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
 		ret = i2c_smbus_read_byte_data(data->client, US5182D_REG_CFG1);
 		if (ret < 0)
 			return ret;
-
 		*val = 0;
 		*val2 = us5182d_scales[ret & US5182D_AGAIN_MASK];
-
 		return IIO_VAL_INT_PLUS_MICRO;
 	default:
 		return -EINVAL;
 	}
-
-	return -EINVAL;
-out_err:
-	mutex_unlock(&data->lock);
-	return ret;
 }
 
 /**
@@ -343,11 +527,201 @@ static int us5182d_write_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
+static int us5182d_setup_prox(struct iio_dev *indio_dev,
+			      enum iio_event_direction dir, u16 val)
+{
+	struct us5182d_data *data = iio_priv(indio_dev);
+
+	if (dir == IIO_EV_DIR_FALLING)
+		return i2c_smbus_write_word_data(data->client,
+						 US5182D_REG_PXL_TH, val);
+	else if (dir == IIO_EV_DIR_RISING)
+		return i2c_smbus_write_word_data(data->client,
+						 US5182D_REG_PXH_TH, val);
+
+	return 0;
+}
+
+static int us5182d_read_thresh(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, enum iio_event_type type,
+	enum iio_event_direction dir, enum iio_event_info info, int *val,
+	int *val2)
+{
+	struct us5182d_data *data = iio_priv(indio_dev);
+
+	switch (dir) {
+	case IIO_EV_DIR_RISING:
+		mutex_lock(&data->lock);
+		*val = data->px_high_th;
+		mutex_unlock(&data->lock);
+		break;
+	case IIO_EV_DIR_FALLING:
+		mutex_lock(&data->lock);
+		*val = data->px_low_th;
+		mutex_unlock(&data->lock);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int us5182d_write_thresh(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, enum iio_event_type type,
+	enum iio_event_direction dir, enum iio_event_info info, int val,
+	int val2)
+{
+	struct us5182d_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (val < 0 || val > USHRT_MAX || val2 != 0)
+		return -EINVAL;
+
+	switch (dir) {
+	case IIO_EV_DIR_RISING:
+		mutex_lock(&data->lock);
+		if (data->rising_en) {
+			ret = us5182d_setup_prox(indio_dev, dir, val);
+			if (ret < 0)
+				goto err;
+		}
+		data->px_high_th = val;
+		mutex_unlock(&data->lock);
+		break;
+	case IIO_EV_DIR_FALLING:
+		mutex_lock(&data->lock);
+		if (data->falling_en) {
+			ret = us5182d_setup_prox(indio_dev, dir, val);
+			if (ret < 0)
+				goto err;
+		}
+		data->px_low_th = val;
+		mutex_unlock(&data->lock);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+err:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
+static int us5182d_read_event_config(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, enum iio_event_type type,
+	enum iio_event_direction dir)
+{
+	struct us5182d_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (dir) {
+	case IIO_EV_DIR_RISING:
+		mutex_lock(&data->lock);
+		ret = data->rising_en;
+		mutex_unlock(&data->lock);
+		break;
+	case IIO_EV_DIR_FALLING:
+		mutex_lock(&data->lock);
+		ret = data->falling_en;
+		mutex_unlock(&data->lock);
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int us5182d_write_event_config(struct iio_dev *indio_dev,
+	const struct iio_chan_spec *chan, enum iio_event_type type,
+	enum iio_event_direction dir, int state)
+{
+	struct us5182d_data *data = iio_priv(indio_dev);
+	int ret;
+	u16 new_th;
+
+	mutex_lock(&data->lock);
+
+	switch (dir) {
+	case IIO_EV_DIR_RISING:
+		if (data->rising_en == state) {
+			mutex_unlock(&data->lock);
+			return 0;
+		}
+		new_th = US5182D_PXH_TH_DISABLE;
+		if (state) {
+			data->power_mode = US5182D_CONTINUOUS;
+			ret = us5182d_set_power_state(data, true);
+			if (ret < 0)
+				goto err;
+			ret = us5182d_px_enable(data);
+			if (ret < 0)
+				goto err_poweroff;
+			new_th = data->px_high_th;
+		}
+		ret = us5182d_setup_prox(indio_dev, dir, new_th);
+		if (ret < 0)
+			goto err_poweroff;
+		data->rising_en = state;
+		break;
+	case IIO_EV_DIR_FALLING:
+		if (data->falling_en == state) {
+			mutex_unlock(&data->lock);
+			return 0;
+		}
+		new_th =  US5182D_PXL_TH_DISABLE;
+		if (state) {
+			data->power_mode = US5182D_CONTINUOUS;
+			ret = us5182d_set_power_state(data, true);
+			if (ret < 0)
+				goto err;
+			ret = us5182d_px_enable(data);
+			if (ret < 0)
+				goto err_poweroff;
+			new_th = data->px_low_th;
+		}
+		ret = us5182d_setup_prox(indio_dev, dir, new_th);
+		if (ret < 0)
+			goto err_poweroff;
+		data->falling_en = state;
+		break;
+	default:
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (!state) {
+		ret = us5182d_set_power_state(data, false);
+		if (ret < 0)
+			goto err;
+	}
+
+	if (!data->falling_en && !data->rising_en && !data->default_continuous)
+		data->power_mode = US5182D_ONESHOT;
+
+	mutex_unlock(&data->lock);
+	return 0;
+
+err_poweroff:
+	if (state)
+		us5182d_set_power_state(data, false);
+err:
+	mutex_unlock(&data->lock);
+	return ret;
+}
+
 static const struct iio_info us5182d_info = {
 	.driver_module	= THIS_MODULE,
 	.read_raw = us5182d_read_raw,
 	.write_raw = us5182d_write_raw,
 	.attrs = &us5182d_attr_group,
+	.read_event_value = &us5182d_read_thresh,
+	.write_event_value = &us5182d_write_thresh,
+	.read_event_config = &us5182d_read_event_config,
+	.write_event_config = &us5182d_write_event_config,
 };
 
 static int us5182d_reset(struct iio_dev *indio_dev)
@@ -368,6 +742,10 @@ static int us5182d_init(struct iio_dev *indio_dev)
 		return ret;
 
 	data->opmode = 0;
+	data->power_mode = US5182D_CONTINUOUS;
+	data->px_low_th = US5182D_REG_PXL_TH_DEFAULT;
+	data->px_high_th = US5182D_REG_PXH_TH_DEFAULT;
+
 	for (i = 0; i < ARRAY_SIZE(us5182d_regvals); i++) {
 		ret = i2c_smbus_write_byte_data(data->client,
 						us5182d_regvals[i].reg,
@@ -376,7 +754,17 @@ static int us5182d_init(struct iio_dev *indio_dev)
 			return ret;
 	}
 
-	return 0;
+	data->als_enabled = true;
+	data->px_enabled = true;
+
+	if (!data->default_continuous) {
+		ret = us5182d_shutdown_en(data, US5182D_CFG0_SHUTDOWN_EN);
+		if (ret < 0)
+			return ret;
+		data->power_mode = US5182D_ONESHOT;
+	}
+
+	return ret;
 }
 
 static void us5182d_get_platform_data(struct iio_dev *indio_dev)
@@ -399,6 +787,8 @@ static void us5182d_get_platform_data(struct iio_dev *indio_dev)
 				    "upisemi,lower-dark-gain",
 				    &data->lower_dark_gain))
 		data->lower_dark_gain = US5182D_REG_AUTO_LDARK_GAIN_DEFAULT;
+	data->default_continuous = device_property_read_bool(&data->client->dev,
+							     "upisemi,continuous");
 }
 
 static int  us5182d_dark_gain_config(struct iio_dev *indio_dev)
@@ -426,6 +816,33 @@ static int  us5182d_dark_gain_config(struct iio_dev *indio_dev)
 					 US5182D_REG_DARK_AUTO_EN_DEFAULT);
 }
 
+static irqreturn_t us5182d_irq_thread_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct us5182d_data *data = iio_priv(indio_dev);
+	enum iio_event_direction dir;
+	int ret;
+	u64 ev;
+
+	ret = i2c_smbus_read_byte_data(data->client, US5182D_REG_CFG0);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "i2c transfer error in irq\n");
+		return IRQ_HANDLED;
+	}
+
+	dir = ret & US5182D_CFG0_PROX ? IIO_EV_DIR_RISING : IIO_EV_DIR_FALLING;
+	ev = IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY, 1, IIO_EV_TYPE_THRESH, dir);
+
+	iio_push_event(indio_dev, ev, iio_get_time_ns());
+
+	ret = i2c_smbus_write_byte_data(data->client, US5182D_REG_CFG0,
+					ret & ~US5182D_CFG0_PX_IRQ);
+	if (ret < 0)
+		dev_err(&data->client->dev, "i2c transfer error in irq\n");
+
+	return IRQ_HANDLED;
+}
+
 static int us5182d_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
@@ -457,6 +874,16 @@ static int us5182d_probe(struct i2c_client *client,
 		return (ret < 0) ? ret : -ENODEV;
 	}
 
+	if (client->irq > 0) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq, NULL,
+						us5182d_irq_thread_handler,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						"us5182d-irq", indio_dev);
+		if (ret < 0)
+			return ret;
+	} else
+		dev_warn(&client->dev, "no valid irq found\n");
+
 	us5182d_get_platform_data(indio_dev);
 	ret = us5182d_init(indio_dev);
 	if (ret < 0)
@@ -464,18 +891,73 @@ static int us5182d_probe(struct i2c_client *client,
 
 	ret = us5182d_dark_gain_config(indio_dev);
 	if (ret < 0)
-		return ret;
+		goto out_err;
+
+	if (data->default_continuous) {
+		pm_runtime_set_active(&client->dev);
+		if (ret < 0)
+			goto out_err;
+	}
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev,
+					 US5182D_SLEEP_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0)
+		goto out_err;
+
+	return 0;
+
+out_err:
+	us5182d_shutdown_en(data, US5182D_CFG0_SHUTDOWN_EN);
+	return ret;
 
-	return iio_device_register(indio_dev);
 }
 
 static int us5182d_remove(struct i2c_client *client)
 {
+	struct us5182d_data *data = iio_priv(i2c_get_clientdata(client));
+
 	iio_device_unregister(i2c_get_clientdata(client));
-	return i2c_smbus_write_byte_data(client, US5182D_REG_CFG0,
-					 US5182D_CFG0_SHUTDOWN_EN);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
+	return us5182d_shutdown_en(data, US5182D_CFG0_SHUTDOWN_EN);
+}
+
+#if defined(CONFIG_PM_SLEEP) || defined(CONFIG_PM)
+static int us5182d_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct us5182d_data *data = iio_priv(indio_dev);
+
+	if (data->power_mode == US5182D_CONTINUOUS)
+		return us5182d_shutdown_en(data, US5182D_CFG0_SHUTDOWN_EN);
+
+	return 0;
 }
 
+static int us5182d_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct us5182d_data *data = iio_priv(indio_dev);
+
+	if (data->power_mode == US5182D_CONTINUOUS)
+		return us5182d_shutdown_en(data,
+					   ~US5182D_CFG0_SHUTDOWN_EN & 0xff);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops us5182d_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(us5182d_suspend, us5182d_resume)
+	SET_RUNTIME_PM_OPS(us5182d_suspend, us5182d_resume, NULL)
+};
+
 static const struct acpi_device_id us5182d_acpi_match[] = {
 	{ "USD5182", 0},
 	{}
@@ -493,6 +975,7 @@ MODULE_DEVICE_TABLE(i2c, us5182d_id);
 static struct i2c_driver us5182d_driver = {
 	.driver = {
 		.name = US5182D_DRV_NAME,
+		.pm = &us5182d_pm_ops,
 		.acpi_match_table = ACPI_PTR(us5182d_acpi_match),
 	},
 	.probe = us5182d_probe,
diff --git a/drivers/iio/magnetometer/bmc150_magn.c b/drivers/iio/magnetometer/bmc150_magn.c
index 1615b23d7b2a..ffcb75ea64fb 100644
--- a/drivers/iio/magnetometer/bmc150_magn.c
+++ b/drivers/iio/magnetometer/bmc150_magn.c
@@ -928,27 +928,24 @@ static int bmc150_magn_probe(struct i2c_client *client,
 		goto err_free_irq;
 	}
 
-	ret = iio_device_register(indio_dev);
-	if (ret < 0) {
-		dev_err(&client->dev, "unable to register iio device\n");
-		goto err_buffer_cleanup;
-	}
-
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret)
-		goto err_iio_unregister;
+		goto err_buffer_cleanup;
 
 	pm_runtime_enable(&client->dev);
 	pm_runtime_set_autosuspend_delay(&client->dev,
 					 BMC150_MAGN_AUTO_SUSPEND_DELAY_MS);
 	pm_runtime_use_autosuspend(&client->dev);
 
-	dev_dbg(&indio_dev->dev, "Registered device %s\n", name);
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto err_buffer_cleanup;
+	}
 
+	dev_dbg(&indio_dev->dev, "Registered device %s\n", name);
 	return 0;
 
-err_iio_unregister:
-	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
 err_free_irq:
@@ -967,11 +964,12 @@ static int bmc150_magn_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct bmc150_magn_data *data = iio_priv(indio_dev);
 
+	iio_device_unregister(indio_dev);
+
 	pm_runtime_disable(&client->dev);
 	pm_runtime_set_suspended(&client->dev);
 	pm_runtime_put_noidle(&client->dev);
 
-	iio_device_unregister(indio_dev);
 	iio_triggered_buffer_cleanup(indio_dev);
 
 	if (client->irq > 0)
diff --git a/drivers/iio/pressure/mpl115.c b/drivers/iio/pressure/mpl115.c
index f5ecd6e19f5d..a0d7deeac62f 100644
--- a/drivers/iio/pressure/mpl115.c
+++ b/drivers/iio/pressure/mpl115.c
@@ -117,7 +117,7 @@ static int mpl115_read_raw(struct iio_dev *indio_dev,
 		*val = ret >> 6;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_OFFSET:
-		*val = 605;
+		*val = -605;
 		*val2 = 750000;
 		return IIO_VAL_INT_PLUS_MICRO;
 	case IIO_CHAN_INFO_SCALE:
diff --git a/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
index e544fcfd5ced..db35e04a0637 100644
--- a/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
+++ b/drivers/iio/proximity/pulsedlight-lidar-lite-v2.c
@@ -13,7 +13,7 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  * GNU General Public License for more details.
  *
- * TODO: runtime pm, interrupt mode, and signal strength reporting
+ * TODO: interrupt mode, and signal strength reporting
  */
 
 #include <linux/err.h>
@@ -21,6 +21,7 @@
 #include <linux/i2c.h>
 #include <linux/delay.h>
 #include <linux/module.h>
+#include <linux/pm_runtime.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
@@ -35,8 +36,11 @@
 #define LIDAR_REG_STATUS_INVALID	BIT(3)
 #define LIDAR_REG_STATUS_READY		BIT(0)
 
-#define LIDAR_REG_DATA_HBYTE	0x0f
-#define LIDAR_REG_DATA_LBYTE	0x10
+#define LIDAR_REG_DATA_HBYTE		0x0f
+#define LIDAR_REG_DATA_LBYTE		0x10
+#define LIDAR_REG_DATA_WORD_READ	BIT(7)
+
+#define LIDAR_REG_PWR_CONTROL	0x65
 
 #define LIDAR_DRV_NAME "lidar"
 
@@ -44,6 +48,9 @@ struct lidar_data {
 	struct iio_dev *indio_dev;
 	struct i2c_client *client;
 
+	int (*xfer)(struct lidar_data *data, u8 reg, u8 *val, int len);
+	int i2c_enabled;
+
 	u16 buffer[8]; /* 2 byte distance + 8 byte timestamp */
 };
 
@@ -62,7 +69,28 @@ static const struct iio_chan_spec lidar_channels[] = {
 	IIO_CHAN_SOFT_TIMESTAMP(1),
 };
 
-static int lidar_read_byte(struct lidar_data *data, int reg)
+static int lidar_i2c_xfer(struct lidar_data *data, u8 reg, u8 *val, int len)
+{
+	struct i2c_client *client = data->client;
+	struct i2c_msg msg[2];
+	int ret;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = client->flags | I2C_M_STOP;
+	msg[0].len = 1;
+	msg[0].buf  = (char *) &reg;
+
+	msg[1].addr = client->addr;
+	msg[1].flags = client->flags | I2C_M_RD;
+	msg[1].len = len;
+	msg[1].buf = (char *) val;
+
+	ret = i2c_transfer(client->adapter, msg, 2);
+
+	return (ret == 2) ? 0 : -EIO;
+}
+
+static int lidar_smbus_xfer(struct lidar_data *data, u8 reg, u8 *val, int len)
 {
 	struct i2c_client *client = data->client;
 	int ret;
@@ -72,17 +100,35 @@ static int lidar_read_byte(struct lidar_data *data, int reg)
 	 * so in turn i2c_smbus_read_byte_data cannot be used
 	 */
 
-	ret = i2c_smbus_write_byte(client, reg);
-	if (ret < 0) {
-		dev_err(&client->dev, "cannot write addr value");
-		return ret;
+	while (len--) {
+		ret = i2c_smbus_write_byte(client, reg++);
+		if (ret < 0) {
+			dev_err(&client->dev, "cannot write addr value");
+			return ret;
+		}
+
+		ret = i2c_smbus_read_byte(client);
+		if (ret < 0) {
+			dev_err(&client->dev, "cannot read data value");
+			return ret;
+		}
+
+		*(val++) = ret;
 	}
 
-	ret = i2c_smbus_read_byte(client);
+	return 0;
+}
+
+static int lidar_read_byte(struct lidar_data *data, u8 reg)
+{
+	int ret;
+	u8 val;
+
+	ret = data->xfer(data, reg, &val, 1);
 	if (ret < 0)
-		dev_err(&client->dev, "cannot read data value");
+		return ret;
 
-	return ret;
+	return val;
 }
 
 static inline int lidar_write_control(struct lidar_data *data, int val)
@@ -90,24 +136,22 @@ static inline int lidar_write_control(struct lidar_data *data, int val)
 	return i2c_smbus_write_byte_data(data->client, LIDAR_REG_CONTROL, val);
 }
 
-static int lidar_read_measurement(struct lidar_data *data, u16 *reg)
+static inline int lidar_write_power(struct lidar_data *data, int val)
 {
-	int ret;
-	int val;
-
-	ret = lidar_read_byte(data, LIDAR_REG_DATA_HBYTE);
-	if (ret < 0)
-		return ret;
-	val = ret << 8;
+	return i2c_smbus_write_byte_data(data->client,
+					 LIDAR_REG_PWR_CONTROL, val);
+}
 
-	ret = lidar_read_byte(data, LIDAR_REG_DATA_LBYTE);
-	if (ret < 0)
-		return ret;
+static int lidar_read_measurement(struct lidar_data *data, u16 *reg)
+{
+	int ret = data->xfer(data, LIDAR_REG_DATA_HBYTE |
+			(data->i2c_enabled ? LIDAR_REG_DATA_WORD_READ : 0),
+			(u8 *) reg, 2);
 
-	val |= ret;
-	*reg = val;
+	if (!ret)
+		*reg = be16_to_cpu(*reg);
 
-	return 0;
+	return ret;
 }
 
 static int lidar_get_measurement(struct lidar_data *data, u16 *reg)
@@ -116,6 +160,8 @@ static int lidar_get_measurement(struct lidar_data *data, u16 *reg)
 	int tries = 10;
 	int ret;
 
+	pm_runtime_get_sync(&client->dev);
+
 	/* start sample */
 	ret = lidar_write_control(data, LIDAR_REG_CONTROL_ACQUIRE);
 	if (ret < 0) {
@@ -144,6 +190,8 @@ static int lidar_get_measurement(struct lidar_data *data, u16 *reg)
 		}
 		ret = -EIO;
 	}
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_put_autosuspend(&client->dev);
 
 	return ret;
 }
@@ -221,6 +269,16 @@ static int lidar_probe(struct i2c_client *client,
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
 	if (!indio_dev)
 		return -ENOMEM;
+	data = iio_priv(indio_dev);
+
+	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		data->xfer = lidar_i2c_xfer;
+		data->i2c_enabled = 1;
+	} else if (i2c_check_functionality(client->adapter,
+				I2C_FUNC_SMBUS_WORD_DATA | I2C_FUNC_SMBUS_BYTE))
+		data->xfer = lidar_smbus_xfer;
+	else
+		return -ENOTSUPP;
 
 	indio_dev->info = &lidar_info;
 	indio_dev->name = LIDAR_DRV_NAME;
@@ -228,7 +286,6 @@ static int lidar_probe(struct i2c_client *client,
 	indio_dev->num_channels = ARRAY_SIZE(lidar_channels);
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 
 	data->client = client;
@@ -243,6 +300,17 @@ static int lidar_probe(struct i2c_client *client,
 	if (ret)
 		goto error_unreg_buffer;
 
+	pm_runtime_set_autosuspend_delay(&client->dev, 1000);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret)
+		goto error_unreg_buffer;
+	pm_runtime_enable(&client->dev);
+
+	pm_runtime_mark_last_busy(&client->dev);
+	pm_runtime_idle(&client->dev);
+
 	return 0;
 
 error_unreg_buffer:
@@ -258,6 +326,9 @@ static int lidar_remove(struct i2c_client *client)
 	iio_device_unregister(indio_dev);
 	iio_triggered_buffer_cleanup(indio_dev);
 
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+
 	return 0;
 }
 
@@ -273,10 +344,38 @@ static const struct of_device_id lidar_dt_ids[] = {
 };
 MODULE_DEVICE_TABLE(of, lidar_dt_ids);
 
+#ifdef CONFIG_PM
+static int lidar_pm_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct lidar_data *data = iio_priv(indio_dev);
+
+	return lidar_write_power(data, 0x0f);
+}
+
+static int lidar_pm_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct lidar_data *data = iio_priv(indio_dev);
+	int ret = lidar_write_power(data, 0);
+
+	/* regulator and FPGA needs settling time */
+	usleep_range(15000, 20000);
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops lidar_pm_ops = {
+	SET_RUNTIME_PM_OPS(lidar_pm_runtime_suspend,
+			   lidar_pm_runtime_resume, NULL)
+};
+
 static struct i2c_driver lidar_driver = {
 	.driver = {
 		.name	= LIDAR_DRV_NAME,
 		.of_match_table	= of_match_ptr(lidar_dt_ids),
+		.pm	= &lidar_pm_ops,
 	},
 	.probe		= lidar_probe,
 	.remove		= lidar_remove,
diff --git a/drivers/iio/trigger/Kconfig b/drivers/iio/trigger/Kconfig
index 79996123a71b..519e6772f6f5 100644
--- a/drivers/iio/trigger/Kconfig
+++ b/drivers/iio/trigger/Kconfig
@@ -5,6 +5,16 @@
 
 menu "Triggers - standalone"
 
+config IIO_HRTIMER_TRIGGER
+	tristate "High resolution timer trigger"
+	depends on IIO_SW_TRIGGER
+	help
+	  Provides a frequency based IIO trigger using high resolution
+	  timers as interrupt source.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called iio-trig-hrtimer.
+
 config IIO_INTERRUPT_TRIGGER
 	tristate "Generic interrupt trigger"
 	help
diff --git a/drivers/iio/trigger/Makefile b/drivers/iio/trigger/Makefile
index 0694daecaf22..fe06eb564367 100644
--- a/drivers/iio/trigger/Makefile
+++ b/drivers/iio/trigger/Makefile
@@ -3,5 +3,7 @@
 #
 
 # When adding new entries keep the list in alphabetical order
+
+obj-$(CONFIG_IIO_HRTIMER_TRIGGER) += iio-trig-hrtimer.o
 obj-$(CONFIG_IIO_INTERRUPT_TRIGGER) += iio-trig-interrupt.o
 obj-$(CONFIG_IIO_SYSFS_TRIGGER) += iio-trig-sysfs.o
diff --git a/drivers/iio/trigger/iio-trig-hrtimer.c b/drivers/iio/trigger/iio-trig-hrtimer.c
new file mode 100644
index 000000000000..5e6d451febeb
--- /dev/null
+++ b/drivers/iio/trigger/iio-trig-hrtimer.c
@@ -0,0 +1,193 @@
+/**
+ * The industrial I/O periodic hrtimer trigger driver
+ *
+ * Copyright (C) Intuitive Aerial AB
+ * Written by Marten Svanfeldt, marten@intuitiveaerial.com
+ * Copyright (C) 2012, Analog Device Inc.
+ *	Author: Lars-Peter Clausen <lars@metafoo.de>
+ * Copyright (C) 2015, Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ */
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/hrtimer.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/sw_trigger.h>
+
+/* default sampling frequency - 100Hz */
+#define HRTIMER_DEFAULT_SAMPLING_FREQUENCY 100
+
+struct iio_hrtimer_info {
+	struct iio_sw_trigger swt;
+	struct hrtimer timer;
+	unsigned long sampling_frequency;
+	ktime_t period;
+};
+
+static struct config_item_type iio_hrtimer_type = {
+	.ct_owner = THIS_MODULE,
+};
+
+static
+ssize_t iio_hrtimer_show_sampling_frequency(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct iio_trigger *trig = to_iio_trigger(dev);
+	struct iio_hrtimer_info *info = iio_trigger_get_drvdata(trig);
+
+	return snprintf(buf, PAGE_SIZE, "%lu\n", info->sampling_frequency);
+}
+
+static
+ssize_t iio_hrtimer_store_sampling_frequency(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf, size_t len)
+{
+	struct iio_trigger *trig = to_iio_trigger(dev);
+	struct iio_hrtimer_info *info = iio_trigger_get_drvdata(trig);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (!val || val > NSEC_PER_SEC)
+		return -EINVAL;
+
+	info->sampling_frequency = val;
+	info->period = ktime_set(0, NSEC_PER_SEC / val);
+
+	return len;
+}
+
+static DEVICE_ATTR(sampling_frequency, S_IRUGO | S_IWUSR,
+		   iio_hrtimer_show_sampling_frequency,
+		   iio_hrtimer_store_sampling_frequency);
+
+static struct attribute *iio_hrtimer_attrs[] = {
+	&dev_attr_sampling_frequency.attr,
+	NULL
+};
+
+static const struct attribute_group iio_hrtimer_attr_group = {
+	.attrs = iio_hrtimer_attrs,
+};
+
+static const struct attribute_group *iio_hrtimer_attr_groups[] = {
+	&iio_hrtimer_attr_group,
+	NULL
+};
+
+static enum hrtimer_restart iio_hrtimer_trig_handler(struct hrtimer *timer)
+{
+	struct iio_hrtimer_info *info;
+
+	info = container_of(timer, struct iio_hrtimer_info, timer);
+
+	hrtimer_forward_now(timer, info->period);
+	iio_trigger_poll(info->swt.trigger);
+
+	return HRTIMER_RESTART;
+}
+
+static int iio_trig_hrtimer_set_state(struct iio_trigger *trig, bool state)
+{
+	struct iio_hrtimer_info *trig_info;
+
+	trig_info = iio_trigger_get_drvdata(trig);
+
+	if (state)
+		hrtimer_start(&trig_info->timer, trig_info->period,
+			      HRTIMER_MODE_REL);
+	else
+		hrtimer_cancel(&trig_info->timer);
+
+	return 0;
+}
+
+static const struct iio_trigger_ops iio_hrtimer_trigger_ops = {
+	.owner = THIS_MODULE,
+	.set_trigger_state = iio_trig_hrtimer_set_state,
+};
+
+static struct iio_sw_trigger *iio_trig_hrtimer_probe(const char *name)
+{
+	struct iio_hrtimer_info *trig_info;
+	int ret;
+
+	trig_info = kzalloc(sizeof(*trig_info), GFP_KERNEL);
+	if (!trig_info)
+		return ERR_PTR(-ENOMEM);
+
+	trig_info->swt.trigger = iio_trigger_alloc("%s", name);
+	if (!trig_info->swt.trigger) {
+		ret = -ENOMEM;
+		goto err_free_trig_info;
+	}
+
+	iio_trigger_set_drvdata(trig_info->swt.trigger, trig_info);
+	trig_info->swt.trigger->ops = &iio_hrtimer_trigger_ops;
+	trig_info->swt.trigger->dev.groups = iio_hrtimer_attr_groups;
+
+	hrtimer_init(&trig_info->timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	trig_info->timer.function = iio_hrtimer_trig_handler;
+
+	trig_info->sampling_frequency = HRTIMER_DEFAULT_SAMPLING_FREQUENCY;
+	trig_info->period = ktime_set(0, NSEC_PER_SEC /
+				      trig_info->sampling_frequency);
+
+	ret = iio_trigger_register(trig_info->swt.trigger);
+	if (ret)
+		goto err_free_trigger;
+
+	iio_swt_group_init_type_name(&trig_info->swt, name, &iio_hrtimer_type);
+	return &trig_info->swt;
+err_free_trigger:
+	iio_trigger_free(trig_info->swt.trigger);
+err_free_trig_info:
+	kfree(trig_info);
+
+	return ERR_PTR(ret);
+}
+
+static int iio_trig_hrtimer_remove(struct iio_sw_trigger *swt)
+{
+	struct iio_hrtimer_info *trig_info;
+
+	trig_info = iio_trigger_get_drvdata(swt->trigger);
+
+	iio_trigger_unregister(swt->trigger);
+
+	/* cancel the timer after unreg to make sure no one rearms it */
+	hrtimer_cancel(&trig_info->timer);
+	iio_trigger_free(swt->trigger);
+	kfree(trig_info);
+
+	return 0;
+}
+
+static const struct iio_sw_trigger_ops iio_trig_hrtimer_ops = {
+	.probe		= iio_trig_hrtimer_probe,
+	.remove		= iio_trig_hrtimer_remove,
+};
+
+static struct iio_sw_trigger_type iio_trig_hrtimer = {
+	.name = "hrtimer",
+	.owner = THIS_MODULE,
+	.ops = &iio_trig_hrtimer_ops,
+};
+
+module_iio_sw_trigger_driver(iio_trig_hrtimer);
+
+MODULE_AUTHOR("Marten Svanfeldt <marten@intuitiveaerial.com>");
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("Periodic hrtimer trigger for the IIO subsystem");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/infiniband/Kconfig b/drivers/infiniband/Kconfig
index aa26f3c3416b..8a8440c0eed1 100644
--- a/drivers/infiniband/Kconfig
+++ b/drivers/infiniband/Kconfig
@@ -5,6 +5,7 @@ menuconfig INFINIBAND
 	depends on NET
 	depends on INET
 	depends on m || IPV6 != m
+	select IRQ_POLL
 	---help---
 	  Core support for InfiniBand (IB).  Make sure to also select
 	  any protocols you wish to use as well as drivers for your
@@ -54,6 +55,15 @@ config INFINIBAND_ADDR_TRANS
 	depends on INFINIBAND
 	default y
 
+config INFINIBAND_ADDR_TRANS_CONFIGFS
+	bool
+	depends on INFINIBAND_ADDR_TRANS && CONFIGFS_FS && !(INFINIBAND=y && CONFIGFS_FS=m)
+	default y
+	---help---
+	  ConfigFS support for RDMA communication manager (CM).
+	  This allows the user to config the default GID type that the CM
+	  uses for each device, when initiaing new connections.
+
 source "drivers/infiniband/hw/mthca/Kconfig"
 source "drivers/infiniband/hw/qib/Kconfig"
 source "drivers/infiniband/hw/cxgb3/Kconfig"
diff --git a/drivers/infiniband/core/Makefile b/drivers/infiniband/core/Makefile
index d43a8994ac5c..f818538a7f4e 100644
--- a/drivers/infiniband/core/Makefile
+++ b/drivers/infiniband/core/Makefile
@@ -8,7 +8,7 @@ obj-$(CONFIG_INFINIBAND_USER_MAD) +=	ib_umad.o
 obj-$(CONFIG_INFINIBAND_USER_ACCESS) +=	ib_uverbs.o ib_ucm.o \
 					$(user_access-y)
 
-ib_core-y :=			packer.o ud_header.o verbs.o sysfs.o \
+ib_core-y :=			packer.o ud_header.o verbs.o cq.o sysfs.o \
 				device.o fmr_pool.o cache.o netlink.o \
 				roce_gid_mgmt.o
 ib_core-$(CONFIG_INFINIBAND_USER_MEM) += umem.o
@@ -24,6 +24,8 @@ iw_cm-y :=			iwcm.o iwpm_util.o iwpm_msg.o
 
 rdma_cm-y :=			cma.o
 
+rdma_cm-$(CONFIG_INFINIBAND_ADDR_TRANS_CONFIGFS) += cma_configfs.o
+
 rdma_ucm-y :=			ucma.o
 
 ib_addr-y :=			addr.o
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index 34b1adad07aa..337353d86cfa 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -121,7 +121,8 @@ int rdma_copy_addr(struct rdma_dev_addr *dev_addr, struct net_device *dev,
 }
 EXPORT_SYMBOL(rdma_copy_addr);
 
-int rdma_translate_ip(struct sockaddr *addr, struct rdma_dev_addr *dev_addr,
+int rdma_translate_ip(const struct sockaddr *addr,
+		      struct rdma_dev_addr *dev_addr,
 		      u16 *vlan_id)
 {
 	struct net_device *dev;
@@ -139,7 +140,7 @@ int rdma_translate_ip(struct sockaddr *addr, struct rdma_dev_addr *dev_addr,
 	switch (addr->sa_family) {
 	case AF_INET:
 		dev = ip_dev_find(dev_addr->net,
-			((struct sockaddr_in *) addr)->sin_addr.s_addr);
+			((const struct sockaddr_in *)addr)->sin_addr.s_addr);
 
 		if (!dev)
 			return ret;
@@ -154,7 +155,7 @@ int rdma_translate_ip(struct sockaddr *addr, struct rdma_dev_addr *dev_addr,
 		rcu_read_lock();
 		for_each_netdev_rcu(dev_addr->net, dev) {
 			if (ipv6_chk_addr(dev_addr->net,
-					  &((struct sockaddr_in6 *) addr)->sin6_addr,
+					  &((const struct sockaddr_in6 *)addr)->sin6_addr,
 					  dev, 1)) {
 				ret = rdma_copy_addr(dev_addr, dev, NULL);
 				if (vlan_id)
@@ -198,7 +199,8 @@ static void queue_req(struct addr_req *req)
 	mutex_unlock(&lock);
 }
 
-static int dst_fetch_ha(struct dst_entry *dst, struct rdma_dev_addr *dev_addr, void *daddr)
+static int dst_fetch_ha(struct dst_entry *dst, struct rdma_dev_addr *dev_addr,
+			const void *daddr)
 {
 	struct neighbour *n;
 	int ret;
@@ -222,8 +224,9 @@ static int dst_fetch_ha(struct dst_entry *dst, struct rdma_dev_addr *dev_addr, v
 }
 
 static int addr4_resolve(struct sockaddr_in *src_in,
-			 struct sockaddr_in *dst_in,
-			 struct rdma_dev_addr *addr)
+			 const struct sockaddr_in *dst_in,
+			 struct rdma_dev_addr *addr,
+			 struct rtable **prt)
 {
 	__be32 src_ip = src_in->sin_addr.s_addr;
 	__be32 dst_ip = dst_in->sin_addr.s_addr;
@@ -243,33 +246,29 @@ static int addr4_resolve(struct sockaddr_in *src_in,
 	src_in->sin_family = AF_INET;
 	src_in->sin_addr.s_addr = fl4.saddr;
 
-	if (rt->dst.dev->flags & IFF_LOOPBACK) {
-		ret = rdma_translate_ip((struct sockaddr *)dst_in, addr, NULL);
-		if (!ret)
-			memcpy(addr->dst_dev_addr, addr->src_dev_addr, MAX_ADDR_LEN);
-		goto put;
-	}
+	/* If there's a gateway, we're definitely in RoCE v2 (as RoCE v1 isn't
+	 * routable) and we could set the network type accordingly.
+	 */
+	if (rt->rt_uses_gateway)
+		addr->network = RDMA_NETWORK_IPV4;
 
-	/* If the device does ARP internally, return 'done' */
-	if (rt->dst.dev->flags & IFF_NOARP) {
-		ret = rdma_copy_addr(addr, rt->dst.dev, NULL);
-		goto put;
-	}
+	addr->hoplimit = ip4_dst_hoplimit(&rt->dst);
 
-	ret = dst_fetch_ha(&rt->dst, addr, &fl4.daddr);
-put:
-	ip_rt_put(rt);
+	*prt = rt;
+	return 0;
 out:
 	return ret;
 }
 
 #if IS_ENABLED(CONFIG_IPV6)
 static int addr6_resolve(struct sockaddr_in6 *src_in,
-			 struct sockaddr_in6 *dst_in,
-			 struct rdma_dev_addr *addr)
+			 const struct sockaddr_in6 *dst_in,
+			 struct rdma_dev_addr *addr,
+			 struct dst_entry **pdst)
 {
 	struct flowi6 fl6;
 	struct dst_entry *dst;
+	struct rt6_info *rt;
 	int ret;
 
 	memset(&fl6, 0, sizeof fl6);
@@ -281,6 +280,7 @@ static int addr6_resolve(struct sockaddr_in6 *src_in,
 	if ((ret = dst->error))
 		goto put;
 
+	rt = (struct rt6_info *)dst;
 	if (ipv6_addr_any(&fl6.saddr)) {
 		ret = ipv6_dev_get_saddr(addr->net, ip6_dst_idev(dst)->dev,
 					 &fl6.daddr, 0, &fl6.saddr);
@@ -291,43 +291,111 @@ static int addr6_resolve(struct sockaddr_in6 *src_in,
 		src_in->sin6_addr = fl6.saddr;
 	}
 
-	if (dst->dev->flags & IFF_LOOPBACK) {
-		ret = rdma_translate_ip((struct sockaddr *)dst_in, addr, NULL);
-		if (!ret)
-			memcpy(addr->dst_dev_addr, addr->src_dev_addr, MAX_ADDR_LEN);
-		goto put;
-	}
+	/* If there's a gateway, we're definitely in RoCE v2 (as RoCE v1 isn't
+	 * routable) and we could set the network type accordingly.
+	 */
+	if (rt->rt6i_flags & RTF_GATEWAY)
+		addr->network = RDMA_NETWORK_IPV6;
 
-	/* If the device does ARP internally, return 'done' */
-	if (dst->dev->flags & IFF_NOARP) {
-		ret = rdma_copy_addr(addr, dst->dev, NULL);
-		goto put;
-	}
+	addr->hoplimit = ip6_dst_hoplimit(dst);
 
-	ret = dst_fetch_ha(dst, addr, &fl6.daddr);
+	*pdst = dst;
+	return 0;
 put:
 	dst_release(dst);
 	return ret;
 }
 #else
 static int addr6_resolve(struct sockaddr_in6 *src_in,
-			 struct sockaddr_in6 *dst_in,
-			 struct rdma_dev_addr *addr)
+			 const struct sockaddr_in6 *dst_in,
+			 struct rdma_dev_addr *addr,
+			 struct dst_entry **pdst)
 {
 	return -EADDRNOTAVAIL;
 }
 #endif
 
+static int addr_resolve_neigh(struct dst_entry *dst,
+			      const struct sockaddr *dst_in,
+			      struct rdma_dev_addr *addr)
+{
+	if (dst->dev->flags & IFF_LOOPBACK) {
+		int ret;
+
+		ret = rdma_translate_ip(dst_in, addr, NULL);
+		if (!ret)
+			memcpy(addr->dst_dev_addr, addr->src_dev_addr,
+			       MAX_ADDR_LEN);
+
+		return ret;
+	}
+
+	/* If the device doesn't do ARP internally */
+	if (!(dst->dev->flags & IFF_NOARP)) {
+		const struct sockaddr_in *dst_in4 =
+			(const struct sockaddr_in *)dst_in;
+		const struct sockaddr_in6 *dst_in6 =
+			(const struct sockaddr_in6 *)dst_in;
+
+		return dst_fetch_ha(dst, addr,
+				    dst_in->sa_family == AF_INET ?
+				    (const void *)&dst_in4->sin_addr.s_addr :
+				    (const void *)&dst_in6->sin6_addr);
+	}
+
+	return rdma_copy_addr(addr, dst->dev, NULL);
+}
+
 static int addr_resolve(struct sockaddr *src_in,
-			struct sockaddr *dst_in,
-			struct rdma_dev_addr *addr)
+			const struct sockaddr *dst_in,
+			struct rdma_dev_addr *addr,
+			bool resolve_neigh)
 {
+	struct net_device *ndev;
+	struct dst_entry *dst;
+	int ret;
+
 	if (src_in->sa_family == AF_INET) {
-		return addr4_resolve((struct sockaddr_in *) src_in,
-			(struct sockaddr_in *) dst_in, addr);
-	} else
-		return addr6_resolve((struct sockaddr_in6 *) src_in,
-			(struct sockaddr_in6 *) dst_in, addr);
+		struct rtable *rt = NULL;
+		const struct sockaddr_in *dst_in4 =
+			(const struct sockaddr_in *)dst_in;
+
+		ret = addr4_resolve((struct sockaddr_in *)src_in,
+				    dst_in4, addr, &rt);
+		if (ret)
+			return ret;
+
+		if (resolve_neigh)
+			ret = addr_resolve_neigh(&rt->dst, dst_in, addr);
+
+		ndev = rt->dst.dev;
+		dev_hold(ndev);
+
+		ip_rt_put(rt);
+	} else {
+		const struct sockaddr_in6 *dst_in6 =
+			(const struct sockaddr_in6 *)dst_in;
+
+		ret = addr6_resolve((struct sockaddr_in6 *)src_in,
+				    dst_in6, addr,
+				    &dst);
+		if (ret)
+			return ret;
+
+		if (resolve_neigh)
+			ret = addr_resolve_neigh(dst, dst_in, addr);
+
+		ndev = dst->dev;
+		dev_hold(ndev);
+
+		dst_release(dst);
+	}
+
+	addr->bound_dev_if = ndev->ifindex;
+	addr->net = dev_net(ndev);
+	dev_put(ndev);
+
+	return ret;
 }
 
 static void process_req(struct work_struct *work)
@@ -343,7 +411,8 @@ static void process_req(struct work_struct *work)
 		if (req->status == -ENODATA) {
 			src_in = (struct sockaddr *) &req->src_addr;
 			dst_in = (struct sockaddr *) &req->dst_addr;
-			req->status = addr_resolve(src_in, dst_in, req->addr);
+			req->status = addr_resolve(src_in, dst_in, req->addr,
+						   true);
 			if (req->status && time_after_eq(jiffies, req->timeout))
 				req->status = -ETIMEDOUT;
 			else if (req->status == -ENODATA)
@@ -403,7 +472,7 @@ int rdma_resolve_ip(struct rdma_addr_client *client,
 	req->client = client;
 	atomic_inc(&client->refcount);
 
-	req->status = addr_resolve(src_in, dst_in, addr);
+	req->status = addr_resolve(src_in, dst_in, addr, true);
 	switch (req->status) {
 	case 0:
 		req->timeout = jiffies;
@@ -425,6 +494,26 @@ err:
 }
 EXPORT_SYMBOL(rdma_resolve_ip);
 
+int rdma_resolve_ip_route(struct sockaddr *src_addr,
+			  const struct sockaddr *dst_addr,
+			  struct rdma_dev_addr *addr)
+{
+	struct sockaddr_storage ssrc_addr = {};
+	struct sockaddr *src_in = (struct sockaddr *)&ssrc_addr;
+
+	if (src_addr) {
+		if (src_addr->sa_family != dst_addr->sa_family)
+			return -EINVAL;
+
+		memcpy(src_in, src_addr, rdma_addr_size(src_addr));
+	} else {
+		src_in->sa_family = dst_addr->sa_family;
+	}
+
+	return addr_resolve(src_in, dst_addr, addr, false);
+}
+EXPORT_SYMBOL(rdma_resolve_ip_route);
+
 void rdma_addr_cancel(struct rdma_dev_addr *addr)
 {
 	struct addr_req *req, *temp_req;
@@ -456,8 +545,10 @@ static void resolve_cb(int status, struct sockaddr *src_addr,
 	complete(&((struct resolve_cb_context *)context)->comp);
 }
 
-int rdma_addr_find_dmac_by_grh(const union ib_gid *sgid, const union ib_gid *dgid,
-			       u8 *dmac, u16 *vlan_id, int if_index)
+int rdma_addr_find_l2_eth_by_grh(const union ib_gid *sgid,
+				 const union ib_gid *dgid,
+				 u8 *dmac, u16 *vlan_id, int *if_index,
+				 int *hoplimit)
 {
 	int ret = 0;
 	struct rdma_dev_addr dev_addr;
@@ -475,7 +566,8 @@ int rdma_addr_find_dmac_by_grh(const union ib_gid *sgid, const union ib_gid *dgi
 	rdma_gid2ip(&dgid_addr._sockaddr, dgid);
 
 	memset(&dev_addr, 0, sizeof(dev_addr));
-	dev_addr.bound_dev_if = if_index;
+	if (if_index)
+		dev_addr.bound_dev_if = *if_index;
 	dev_addr.net = &init_net;
 
 	ctx.addr = &dev_addr;
@@ -491,12 +583,16 @@ int rdma_addr_find_dmac_by_grh(const union ib_gid *sgid, const union ib_gid *dgi
 	dev = dev_get_by_index(&init_net, dev_addr.bound_dev_if);
 	if (!dev)
 		return -ENODEV;
+	if (if_index)
+		*if_index = dev_addr.bound_dev_if;
 	if (vlan_id)
 		*vlan_id = rdma_vlan_dev_vlan_id(dev);
+	if (hoplimit)
+		*hoplimit = dev_addr.hoplimit;
 	dev_put(dev);
 	return ret;
 }
-EXPORT_SYMBOL(rdma_addr_find_dmac_by_grh);
+EXPORT_SYMBOL(rdma_addr_find_l2_eth_by_grh);
 
 int rdma_addr_find_smac_by_sgid(union ib_gid *sgid, u8 *smac, u16 *vlan_id)
 {
diff --git a/drivers/infiniband/core/cache.c b/drivers/infiniband/core/cache.c
index 89bebeada38b..53343ffbff7a 100644
--- a/drivers/infiniband/core/cache.c
+++ b/drivers/infiniband/core/cache.c
@@ -64,6 +64,7 @@ enum gid_attr_find_mask {
 	GID_ATTR_FIND_MASK_GID          = 1UL << 0,
 	GID_ATTR_FIND_MASK_NETDEV	= 1UL << 1,
 	GID_ATTR_FIND_MASK_DEFAULT	= 1UL << 2,
+	GID_ATTR_FIND_MASK_GID_TYPE	= 1UL << 3,
 };
 
 enum gid_table_entry_props {
@@ -81,10 +82,6 @@ enum gid_table_write_action {
 };
 
 struct ib_gid_table_entry {
-	/* This lock protects an entry from being
-	 * read and written simultaneously.
-	 */
-	rwlock_t	    lock;
 	unsigned long	    props;
 	union ib_gid        gid;
 	struct ib_gid_attr  attr;
@@ -109,28 +106,86 @@ struct ib_gid_table {
 	 * are locked by this lock.
 	 **/
 	struct mutex         lock;
+	/* This lock protects the table entries from being
+	 * read and written simultaneously.
+	 */
+	rwlock_t	     rwlock;
 	struct ib_gid_table_entry *data_vec;
 };
 
+static void dispatch_gid_change_event(struct ib_device *ib_dev, u8 port)
+{
+	if (rdma_cap_roce_gid_table(ib_dev, port)) {
+		struct ib_event event;
+
+		event.device		= ib_dev;
+		event.element.port_num	= port;
+		event.event		= IB_EVENT_GID_CHANGE;
+
+		ib_dispatch_event(&event);
+	}
+}
+
+static const char * const gid_type_str[] = {
+	[IB_GID_TYPE_IB]	= "IB/RoCE v1",
+	[IB_GID_TYPE_ROCE_UDP_ENCAP]	= "RoCE v2",
+};
+
+const char *ib_cache_gid_type_str(enum ib_gid_type gid_type)
+{
+	if (gid_type < ARRAY_SIZE(gid_type_str) && gid_type_str[gid_type])
+		return gid_type_str[gid_type];
+
+	return "Invalid GID type";
+}
+EXPORT_SYMBOL(ib_cache_gid_type_str);
+
+int ib_cache_gid_parse_type_str(const char *buf)
+{
+	unsigned int i;
+	size_t len;
+	int err = -EINVAL;
+
+	len = strlen(buf);
+	if (len == 0)
+		return -EINVAL;
+
+	if (buf[len - 1] == '\n')
+		len--;
+
+	for (i = 0; i < ARRAY_SIZE(gid_type_str); ++i)
+		if (gid_type_str[i] && !strncmp(buf, gid_type_str[i], len) &&
+		    len == strlen(gid_type_str[i])) {
+			err = i;
+			break;
+		}
+
+	return err;
+}
+EXPORT_SYMBOL(ib_cache_gid_parse_type_str);
+
+/* This function expects that rwlock will be write locked in all
+ * scenarios and that lock will be locked in sleep-able (RoCE)
+ * scenarios.
+ */
 static int write_gid(struct ib_device *ib_dev, u8 port,
 		     struct ib_gid_table *table, int ix,
 		     const union ib_gid *gid,
 		     const struct ib_gid_attr *attr,
 		     enum gid_table_write_action action,
 		     bool  default_gid)
+	__releases(&table->rwlock) __acquires(&table->rwlock)
 {
 	int ret = 0;
 	struct net_device *old_net_dev;
-	unsigned long flags;
 
 	/* in rdma_cap_roce_gid_table, this funciton should be protected by a
 	 * sleep-able lock.
 	 */
-	write_lock_irqsave(&table->data_vec[ix].lock, flags);
 
 	if (rdma_cap_roce_gid_table(ib_dev, port)) {
 		table->data_vec[ix].props |= GID_TABLE_ENTRY_INVALID;
-		write_unlock_irqrestore(&table->data_vec[ix].lock, flags);
+		write_unlock_irq(&table->rwlock);
 		/* GID_TABLE_WRITE_ACTION_MODIFY currently isn't supported by
 		 * RoCE providers and thus only updates the cache.
 		 */
@@ -140,7 +195,7 @@ static int write_gid(struct ib_device *ib_dev, u8 port,
 		else if (action == GID_TABLE_WRITE_ACTION_DEL)
 			ret = ib_dev->del_gid(ib_dev, port, ix,
 					      &table->data_vec[ix].context);
-		write_lock_irqsave(&table->data_vec[ix].lock, flags);
+		write_lock_irq(&table->rwlock);
 	}
 
 	old_net_dev = table->data_vec[ix].attr.ndev;
@@ -162,17 +217,6 @@ static int write_gid(struct ib_device *ib_dev, u8 port,
 
 	table->data_vec[ix].props &= ~GID_TABLE_ENTRY_INVALID;
 
-	write_unlock_irqrestore(&table->data_vec[ix].lock, flags);
-
-	if (!ret && rdma_cap_roce_gid_table(ib_dev, port)) {
-		struct ib_event event;
-
-		event.device		= ib_dev;
-		event.element.port_num	= port;
-		event.event		= IB_EVENT_GID_CHANGE;
-
-		ib_dispatch_event(&event);
-	}
 	return ret;
 }
 
@@ -201,41 +245,58 @@ static int del_gid(struct ib_device *ib_dev, u8 port,
 			 GID_TABLE_WRITE_ACTION_DEL, default_gid);
 }
 
+/* rwlock should be read locked */
 static int find_gid(struct ib_gid_table *table, const union ib_gid *gid,
 		    const struct ib_gid_attr *val, bool default_gid,
-		    unsigned long mask)
+		    unsigned long mask, int *pempty)
 {
-	int i;
+	int i = 0;
+	int found = -1;
+	int empty = pempty ? -1 : 0;
 
-	for (i = 0; i < table->sz; i++) {
-		unsigned long flags;
-		struct ib_gid_attr *attr = &table->data_vec[i].attr;
+	while (i < table->sz && (found < 0 || empty < 0)) {
+		struct ib_gid_table_entry *data = &table->data_vec[i];
+		struct ib_gid_attr *attr = &data->attr;
+		int curr_index = i;
 
-		read_lock_irqsave(&table->data_vec[i].lock, flags);
+		i++;
 
-		if (table->data_vec[i].props & GID_TABLE_ENTRY_INVALID)
-			goto next;
+		if (data->props & GID_TABLE_ENTRY_INVALID)
+			continue;
+
+		if (empty < 0)
+			if (!memcmp(&data->gid, &zgid, sizeof(*gid)) &&
+			    !memcmp(attr, &zattr, sizeof(*attr)) &&
+			    !data->props)
+				empty = curr_index;
+
+		if (found >= 0)
+			continue;
+
+		if (mask & GID_ATTR_FIND_MASK_GID_TYPE &&
+		    attr->gid_type != val->gid_type)
+			continue;
 
 		if (mask & GID_ATTR_FIND_MASK_GID &&
-		    memcmp(gid, &table->data_vec[i].gid, sizeof(*gid)))
-			goto next;
+		    memcmp(gid, &data->gid, sizeof(*gid)))
+			continue;
 
 		if (mask & GID_ATTR_FIND_MASK_NETDEV &&
 		    attr->ndev != val->ndev)
-			goto next;
+			continue;
 
 		if (mask & GID_ATTR_FIND_MASK_DEFAULT &&
-		    !!(table->data_vec[i].props & GID_TABLE_ENTRY_DEFAULT) !=
+		    !!(data->props & GID_TABLE_ENTRY_DEFAULT) !=
 		    default_gid)
-			goto next;
+			continue;
 
-		read_unlock_irqrestore(&table->data_vec[i].lock, flags);
-		return i;
-next:
-		read_unlock_irqrestore(&table->data_vec[i].lock, flags);
+		found = curr_index;
 	}
 
-	return -1;
+	if (pempty)
+		*pempty = empty;
+
+	return found;
 }
 
 static void make_default_gid(struct  net_device *dev, union ib_gid *gid)
@@ -252,6 +313,7 @@ int ib_cache_gid_add(struct ib_device *ib_dev, u8 port,
 	int ix;
 	int ret = 0;
 	struct net_device *idev;
+	int empty;
 
 	table = ports_table[port - rdma_start_port(ib_dev)];
 
@@ -275,22 +337,25 @@ int ib_cache_gid_add(struct ib_device *ib_dev, u8 port,
 	}
 
 	mutex_lock(&table->lock);
+	write_lock_irq(&table->rwlock);
 
 	ix = find_gid(table, gid, attr, false, GID_ATTR_FIND_MASK_GID |
-		      GID_ATTR_FIND_MASK_NETDEV);
+		      GID_ATTR_FIND_MASK_GID_TYPE |
+		      GID_ATTR_FIND_MASK_NETDEV, &empty);
 	if (ix >= 0)
 		goto out_unlock;
 
-	ix = find_gid(table, &zgid, NULL, false, GID_ATTR_FIND_MASK_GID |
-		      GID_ATTR_FIND_MASK_DEFAULT);
-	if (ix < 0) {
+	if (empty < 0) {
 		ret = -ENOSPC;
 		goto out_unlock;
 	}
 
-	add_gid(ib_dev, port, table, ix, gid, attr, false);
+	ret = add_gid(ib_dev, port, table, empty, gid, attr, false);
+	if (!ret)
+		dispatch_gid_change_event(ib_dev, port);
 
 out_unlock:
+	write_unlock_irq(&table->rwlock);
 	mutex_unlock(&table->lock);
 	return ret;
 }
@@ -305,17 +370,22 @@ int ib_cache_gid_del(struct ib_device *ib_dev, u8 port,
 	table = ports_table[port - rdma_start_port(ib_dev)];
 
 	mutex_lock(&table->lock);
+	write_lock_irq(&table->rwlock);
 
 	ix = find_gid(table, gid, attr, false,
 		      GID_ATTR_FIND_MASK_GID	  |
+		      GID_ATTR_FIND_MASK_GID_TYPE |
 		      GID_ATTR_FIND_MASK_NETDEV	  |
-		      GID_ATTR_FIND_MASK_DEFAULT);
+		      GID_ATTR_FIND_MASK_DEFAULT,
+		      NULL);
 	if (ix < 0)
 		goto out_unlock;
 
-	del_gid(ib_dev, port, table, ix, false);
+	if (!del_gid(ib_dev, port, table, ix, false))
+		dispatch_gid_change_event(ib_dev, port);
 
 out_unlock:
+	write_unlock_irq(&table->rwlock);
 	mutex_unlock(&table->lock);
 	return 0;
 }
@@ -326,16 +396,24 @@ int ib_cache_gid_del_all_netdev_gids(struct ib_device *ib_dev, u8 port,
 	struct ib_gid_table **ports_table = ib_dev->cache.gid_cache;
 	struct ib_gid_table *table;
 	int ix;
+	bool deleted = false;
 
 	table  = ports_table[port - rdma_start_port(ib_dev)];
 
 	mutex_lock(&table->lock);
+	write_lock_irq(&table->rwlock);
 
 	for (ix = 0; ix < table->sz; ix++)
 		if (table->data_vec[ix].attr.ndev == ndev)
-			del_gid(ib_dev, port, table, ix, false);
+			if (!del_gid(ib_dev, port, table, ix, false))
+				deleted = true;
 
+	write_unlock_irq(&table->rwlock);
 	mutex_unlock(&table->lock);
+
+	if (deleted)
+		dispatch_gid_change_event(ib_dev, port);
+
 	return 0;
 }
 
@@ -344,18 +422,14 @@ static int __ib_cache_gid_get(struct ib_device *ib_dev, u8 port, int index,
 {
 	struct ib_gid_table **ports_table = ib_dev->cache.gid_cache;
 	struct ib_gid_table *table;
-	unsigned long flags;
 
 	table = ports_table[port - rdma_start_port(ib_dev)];
 
 	if (index < 0 || index >= table->sz)
 		return -EINVAL;
 
-	read_lock_irqsave(&table->data_vec[index].lock, flags);
-	if (table->data_vec[index].props & GID_TABLE_ENTRY_INVALID) {
-		read_unlock_irqrestore(&table->data_vec[index].lock, flags);
+	if (table->data_vec[index].props & GID_TABLE_ENTRY_INVALID)
 		return -EAGAIN;
-	}
 
 	memcpy(gid, &table->data_vec[index].gid, sizeof(*gid));
 	if (attr) {
@@ -364,7 +438,6 @@ static int __ib_cache_gid_get(struct ib_device *ib_dev, u8 port, int index,
 			dev_hold(attr->ndev);
 	}
 
-	read_unlock_irqrestore(&table->data_vec[index].lock, flags);
 	return 0;
 }
 
@@ -378,17 +451,21 @@ static int _ib_cache_gid_table_find(struct ib_device *ib_dev,
 	struct ib_gid_table *table;
 	u8 p;
 	int local_index;
+	unsigned long flags;
 
 	for (p = 0; p < ib_dev->phys_port_cnt; p++) {
 		table = ports_table[p];
-		local_index = find_gid(table, gid, val, false, mask);
+		read_lock_irqsave(&table->rwlock, flags);
+		local_index = find_gid(table, gid, val, false, mask, NULL);
 		if (local_index >= 0) {
 			if (index)
 				*index = local_index;
 			if (port)
 				*port = p + rdma_start_port(ib_dev);
+			read_unlock_irqrestore(&table->rwlock, flags);
 			return 0;
 		}
+		read_unlock_irqrestore(&table->rwlock, flags);
 	}
 
 	return -ENOENT;
@@ -396,11 +473,13 @@ static int _ib_cache_gid_table_find(struct ib_device *ib_dev,
 
 static int ib_cache_gid_find(struct ib_device *ib_dev,
 			     const union ib_gid *gid,
+			     enum ib_gid_type gid_type,
 			     struct net_device *ndev, u8 *port,
 			     u16 *index)
 {
-	unsigned long mask = GID_ATTR_FIND_MASK_GID;
-	struct ib_gid_attr gid_attr_val = {.ndev = ndev};
+	unsigned long mask = GID_ATTR_FIND_MASK_GID |
+			     GID_ATTR_FIND_MASK_GID_TYPE;
+	struct ib_gid_attr gid_attr_val = {.ndev = ndev, .gid_type = gid_type};
 
 	if (ndev)
 		mask |= GID_ATTR_FIND_MASK_NETDEV;
@@ -411,14 +490,17 @@ static int ib_cache_gid_find(struct ib_device *ib_dev,
 
 int ib_find_cached_gid_by_port(struct ib_device *ib_dev,
 			       const union ib_gid *gid,
+			       enum ib_gid_type gid_type,
 			       u8 port, struct net_device *ndev,
 			       u16 *index)
 {
 	int local_index;
 	struct ib_gid_table **ports_table = ib_dev->cache.gid_cache;
 	struct ib_gid_table *table;
-	unsigned long mask = GID_ATTR_FIND_MASK_GID;
-	struct ib_gid_attr val = {.ndev = ndev};
+	unsigned long mask = GID_ATTR_FIND_MASK_GID |
+			     GID_ATTR_FIND_MASK_GID_TYPE;
+	struct ib_gid_attr val = {.ndev = ndev, .gid_type = gid_type};
+	unsigned long flags;
 
 	if (port < rdma_start_port(ib_dev) ||
 	    port > rdma_end_port(ib_dev))
@@ -429,13 +511,16 @@ int ib_find_cached_gid_by_port(struct ib_device *ib_dev,
 	if (ndev)
 		mask |= GID_ATTR_FIND_MASK_NETDEV;
 
-	local_index = find_gid(table, gid, &val, false, mask);
+	read_lock_irqsave(&table->rwlock, flags);
+	local_index = find_gid(table, gid, &val, false, mask, NULL);
 	if (local_index >= 0) {
 		if (index)
 			*index = local_index;
+		read_unlock_irqrestore(&table->rwlock, flags);
 		return 0;
 	}
 
+	read_unlock_irqrestore(&table->rwlock, flags);
 	return -ENOENT;
 }
 EXPORT_SYMBOL(ib_find_cached_gid_by_port);
@@ -472,6 +557,7 @@ static int ib_cache_gid_find_by_filter(struct ib_device *ib_dev,
 	struct ib_gid_table **ports_table = ib_dev->cache.gid_cache;
 	struct ib_gid_table *table;
 	unsigned int i;
+	unsigned long flags;
 	bool found = false;
 
 	if (!ports_table)
@@ -484,11 +570,10 @@ static int ib_cache_gid_find_by_filter(struct ib_device *ib_dev,
 
 	table = ports_table[port - rdma_start_port(ib_dev)];
 
+	read_lock_irqsave(&table->rwlock, flags);
 	for (i = 0; i < table->sz; i++) {
 		struct ib_gid_attr attr;
-		unsigned long flags;
 
-		read_lock_irqsave(&table->data_vec[i].lock, flags);
 		if (table->data_vec[i].props & GID_TABLE_ENTRY_INVALID)
 			goto next;
 
@@ -501,11 +586,10 @@ static int ib_cache_gid_find_by_filter(struct ib_device *ib_dev,
 			found = true;
 
 next:
-		read_unlock_irqrestore(&table->data_vec[i].lock, flags);
-
 		if (found)
 			break;
 	}
+	read_unlock_irqrestore(&table->rwlock, flags);
 
 	if (!found)
 		return -ENOENT;
@@ -517,9 +601,9 @@ next:
 
 static struct ib_gid_table *alloc_gid_table(int sz)
 {
-	unsigned int i;
 	struct ib_gid_table *table =
 		kzalloc(sizeof(struct ib_gid_table), GFP_KERNEL);
+
 	if (!table)
 		return NULL;
 
@@ -530,9 +614,7 @@ static struct ib_gid_table *alloc_gid_table(int sz)
 	mutex_init(&table->lock);
 
 	table->sz = sz;
-
-	for (i = 0; i < sz; i++)
-		rwlock_init(&table->data_vec[i].lock);
+	rwlock_init(&table->rwlock);
 
 	return table;
 
@@ -553,30 +635,37 @@ static void cleanup_gid_table_port(struct ib_device *ib_dev, u8 port,
 				   struct ib_gid_table *table)
 {
 	int i;
+	bool deleted = false;
 
 	if (!table)
 		return;
 
+	write_lock_irq(&table->rwlock);
 	for (i = 0; i < table->sz; ++i) {
 		if (memcmp(&table->data_vec[i].gid, &zgid,
 			   sizeof(table->data_vec[i].gid)))
-			del_gid(ib_dev, port, table, i,
-				table->data_vec[i].props &
-				GID_ATTR_FIND_MASK_DEFAULT);
+			if (!del_gid(ib_dev, port, table, i,
+				     table->data_vec[i].props &
+				     GID_ATTR_FIND_MASK_DEFAULT))
+				deleted = true;
 	}
+	write_unlock_irq(&table->rwlock);
+
+	if (deleted)
+		dispatch_gid_change_event(ib_dev, port);
 }
 
 void ib_cache_gid_set_default_gid(struct ib_device *ib_dev, u8 port,
 				  struct net_device *ndev,
+				  unsigned long gid_type_mask,
 				  enum ib_cache_gid_default_mode mode)
 {
 	struct ib_gid_table **ports_table = ib_dev->cache.gid_cache;
 	union ib_gid gid;
 	struct ib_gid_attr gid_attr;
+	struct ib_gid_attr zattr_type = zattr;
 	struct ib_gid_table *table;
-	int ix;
-	union ib_gid current_gid;
-	struct ib_gid_attr current_gid_attr = {};
+	unsigned int gid_type;
 
 	table  = ports_table[port - rdma_start_port(ib_dev)];
 
@@ -584,46 +673,82 @@ void ib_cache_gid_set_default_gid(struct ib_device *ib_dev, u8 port,
 	memset(&gid_attr, 0, sizeof(gid_attr));
 	gid_attr.ndev = ndev;
 
-	mutex_lock(&table->lock);
-	ix = find_gid(table, NULL, NULL, true, GID_ATTR_FIND_MASK_DEFAULT);
-
-	/* Coudn't find default GID location */
-	WARN_ON(ix < 0);
-
-	if (!__ib_cache_gid_get(ib_dev, port, ix,
-				&current_gid, &current_gid_attr) &&
-	    mode == IB_CACHE_GID_DEFAULT_MODE_SET &&
-	    !memcmp(&gid, &current_gid, sizeof(gid)) &&
-	    !memcmp(&gid_attr, &current_gid_attr, sizeof(gid_attr)))
-		goto unlock;
-
-	if ((memcmp(&current_gid, &zgid, sizeof(current_gid)) ||
-	     memcmp(&current_gid_attr, &zattr,
-		    sizeof(current_gid_attr))) &&
-	    del_gid(ib_dev, port, table, ix, true)) {
-		pr_warn("ib_cache_gid: can't delete index %d for default gid %pI6\n",
-			ix, gid.raw);
-		goto unlock;
-	}
+	for (gid_type = 0; gid_type < IB_GID_TYPE_SIZE; ++gid_type) {
+		int ix;
+		union ib_gid current_gid;
+		struct ib_gid_attr current_gid_attr = {};
+
+		if (1UL << gid_type & ~gid_type_mask)
+			continue;
+
+		gid_attr.gid_type = gid_type;
+
+		mutex_lock(&table->lock);
+		write_lock_irq(&table->rwlock);
+		ix = find_gid(table, NULL, &gid_attr, true,
+			      GID_ATTR_FIND_MASK_GID_TYPE |
+			      GID_ATTR_FIND_MASK_DEFAULT,
+			      NULL);
+
+		/* Coudn't find default GID location */
+		WARN_ON(ix < 0);
+
+		zattr_type.gid_type = gid_type;
+
+		if (!__ib_cache_gid_get(ib_dev, port, ix,
+					&current_gid, &current_gid_attr) &&
+		    mode == IB_CACHE_GID_DEFAULT_MODE_SET &&
+		    !memcmp(&gid, &current_gid, sizeof(gid)) &&
+		    !memcmp(&gid_attr, &current_gid_attr, sizeof(gid_attr)))
+			goto release;
+
+		if (memcmp(&current_gid, &zgid, sizeof(current_gid)) ||
+		    memcmp(&current_gid_attr, &zattr_type,
+			   sizeof(current_gid_attr))) {
+			if (del_gid(ib_dev, port, table, ix, true)) {
+				pr_warn("ib_cache_gid: can't delete index %d for default gid %pI6\n",
+					ix, gid.raw);
+				goto release;
+			} else {
+				dispatch_gid_change_event(ib_dev, port);
+			}
+		}
 
-	if (mode == IB_CACHE_GID_DEFAULT_MODE_SET)
-		if (add_gid(ib_dev, port, table, ix, &gid, &gid_attr, true))
-			pr_warn("ib_cache_gid: unable to add default gid %pI6\n",
-				gid.raw);
+		if (mode == IB_CACHE_GID_DEFAULT_MODE_SET) {
+			if (add_gid(ib_dev, port, table, ix, &gid, &gid_attr, true))
+				pr_warn("ib_cache_gid: unable to add default gid %pI6\n",
+					gid.raw);
+			else
+				dispatch_gid_change_event(ib_dev, port);
+		}
 
-unlock:
-	if (current_gid_attr.ndev)
-		dev_put(current_gid_attr.ndev);
-	mutex_unlock(&table->lock);
+release:
+		if (current_gid_attr.ndev)
+			dev_put(current_gid_attr.ndev);
+		write_unlock_irq(&table->rwlock);
+		mutex_unlock(&table->lock);
+	}
 }
 
 static int gid_table_reserve_default(struct ib_device *ib_dev, u8 port,
 				     struct ib_gid_table *table)
 {
-	if (rdma_protocol_roce(ib_dev, port)) {
-		struct ib_gid_table_entry *entry = &table->data_vec[0];
+	unsigned int i;
+	unsigned long roce_gid_type_mask;
+	unsigned int num_default_gids;
+	unsigned int current_gid = 0;
+
+	roce_gid_type_mask = roce_gid_type_mask_support(ib_dev, port);
+	num_default_gids = hweight_long(roce_gid_type_mask);
+	for (i = 0; i < num_default_gids && i < table->sz; i++) {
+		struct ib_gid_table_entry *entry =
+			&table->data_vec[i];
 
 		entry->props |= GID_TABLE_ENTRY_DEFAULT;
+		current_gid = find_next_bit(&roce_gid_type_mask,
+					    BITS_PER_LONG,
+					    current_gid);
+		entry->attr.gid_type = current_gid++;
 	}
 
 	return 0;
@@ -728,20 +853,30 @@ int ib_get_cached_gid(struct ib_device *device,
 		      union ib_gid     *gid,
 		      struct ib_gid_attr *gid_attr)
 {
+	int res;
+	unsigned long flags;
+	struct ib_gid_table **ports_table = device->cache.gid_cache;
+	struct ib_gid_table *table = ports_table[port_num - rdma_start_port(device)];
+
 	if (port_num < rdma_start_port(device) || port_num > rdma_end_port(device))
 		return -EINVAL;
 
-	return __ib_cache_gid_get(device, port_num, index, gid, gid_attr);
+	read_lock_irqsave(&table->rwlock, flags);
+	res = __ib_cache_gid_get(device, port_num, index, gid, gid_attr);
+	read_unlock_irqrestore(&table->rwlock, flags);
+
+	return res;
 }
 EXPORT_SYMBOL(ib_get_cached_gid);
 
 int ib_find_cached_gid(struct ib_device *device,
 		       const union ib_gid *gid,
+		       enum ib_gid_type gid_type,
 		       struct net_device *ndev,
 		       u8               *port_num,
 		       u16              *index)
 {
-	return ib_cache_gid_find(device, gid, ndev, port_num, index);
+	return ib_cache_gid_find(device, gid, gid_type, ndev, port_num, index);
 }
 EXPORT_SYMBOL(ib_find_cached_gid);
 
@@ -956,10 +1091,12 @@ static void ib_cache_update(struct ib_device *device,
 
 	device->cache.pkey_cache[port - rdma_start_port(device)] = pkey_cache;
 	if (!use_roce_gid_table) {
+		write_lock(&table->rwlock);
 		for (i = 0; i < gid_cache->table_len; i++) {
 			modify_gid(device, port, table, i, gid_cache->table + i,
 				   &zattr, false);
 		}
+		write_unlock(&table->rwlock);
 	}
 
 	device->cache.lmc_cache[port - rdma_start_port(device)] = tprops->lmc;
diff --git a/drivers/infiniband/core/cm.c b/drivers/infiniband/core/cm.c
index 0a26dd6d9b19..1d92e091e22e 100644
--- a/drivers/infiniband/core/cm.c
+++ b/drivers/infiniband/core/cm.c
@@ -364,7 +364,7 @@ static int cm_init_av_by_path(struct ib_sa_path_rec *path, struct cm_av *av)
 	read_lock_irqsave(&cm.device_lock, flags);
 	list_for_each_entry(cm_dev, &cm.device_list, list) {
 		if (!ib_find_cached_gid(cm_dev->ib_device, &path->sgid,
-					ndev, &p, NULL)) {
+					path->gid_type, ndev, &p, NULL)) {
 			port = cm_dev->port[p-1];
 			break;
 		}
@@ -782,11 +782,11 @@ static void cm_enter_timewait(struct cm_id_private *cm_id_priv)
 	wait_time = cm_convert_to_ms(cm_id_priv->av.timeout);
 
 	/* Check if the device started its remove_one */
-	spin_lock_irq(&cm.lock);
+	spin_lock_irqsave(&cm.lock, flags);
 	if (!cm_dev->going_down)
 		queue_delayed_work(cm.wq, &cm_id_priv->timewait_info->work.work,
 				   msecs_to_jiffies(wait_time));
-	spin_unlock_irq(&cm.lock);
+	spin_unlock_irqrestore(&cm.lock, flags);
 
 	cm_id_priv->timewait_info = NULL;
 }
@@ -1600,6 +1600,8 @@ static int cm_req_handler(struct cm_work *work)
 	struct ib_cm_id *cm_id;
 	struct cm_id_private *cm_id_priv, *listen_cm_id_priv;
 	struct cm_req_msg *req_msg;
+	union ib_gid gid;
+	struct ib_gid_attr gid_attr;
 	int ret;
 
 	req_msg = (struct cm_req_msg *)work->mad_recv_wc->recv_buf.mad;
@@ -1639,11 +1641,31 @@ static int cm_req_handler(struct cm_work *work)
 	cm_format_paths_from_req(req_msg, &work->path[0], &work->path[1]);
 
 	memcpy(work->path[0].dmac, cm_id_priv->av.ah_attr.dmac, ETH_ALEN);
-	ret = cm_init_av_by_path(&work->path[0], &cm_id_priv->av);
+	work->path[0].hop_limit = cm_id_priv->av.ah_attr.grh.hop_limit;
+	ret = ib_get_cached_gid(work->port->cm_dev->ib_device,
+				work->port->port_num,
+				cm_id_priv->av.ah_attr.grh.sgid_index,
+				&gid, &gid_attr);
+	if (!ret) {
+		if (gid_attr.ndev) {
+			work->path[0].ifindex = gid_attr.ndev->ifindex;
+			work->path[0].net = dev_net(gid_attr.ndev);
+			dev_put(gid_attr.ndev);
+		}
+		work->path[0].gid_type = gid_attr.gid_type;
+		ret = cm_init_av_by_path(&work->path[0], &cm_id_priv->av);
+	}
 	if (ret) {
-		ib_get_cached_gid(work->port->cm_dev->ib_device,
-				  work->port->port_num, 0, &work->path[0].sgid,
-				  NULL);
+		int err = ib_get_cached_gid(work->port->cm_dev->ib_device,
+					    work->port->port_num, 0,
+					    &work->path[0].sgid,
+					    &gid_attr);
+		if (!err && gid_attr.ndev) {
+			work->path[0].ifindex = gid_attr.ndev->ifindex;
+			work->path[0].net = dev_net(gid_attr.ndev);
+			dev_put(gid_attr.ndev);
+		}
+		work->path[0].gid_type = gid_attr.gid_type;
 		ib_send_cm_rej(cm_id, IB_CM_REJ_INVALID_GID,
 			       &work->path[0].sgid, sizeof work->path[0].sgid,
 			       NULL, 0);
@@ -3482,6 +3504,7 @@ int ib_cm_notify(struct ib_cm_id *cm_id, enum ib_event_type event)
 EXPORT_SYMBOL(ib_cm_notify);
 
 static void cm_recv_handler(struct ib_mad_agent *mad_agent,
+			    struct ib_mad_send_buf *send_buf,
 			    struct ib_mad_recv_wc *mad_recv_wc)
 {
 	struct cm_port *port = mad_agent->context;
@@ -3731,16 +3754,6 @@ int ib_cm_init_qp_attr(struct ib_cm_id *cm_id,
 }
 EXPORT_SYMBOL(ib_cm_init_qp_attr);
 
-static void cm_get_ack_delay(struct cm_device *cm_dev)
-{
-	struct ib_device_attr attr;
-
-	if (ib_query_device(cm_dev->ib_device, &attr))
-		cm_dev->ack_delay = 0; /* acks will rely on packet life time */
-	else
-		cm_dev->ack_delay = attr.local_ca_ack_delay;
-}
-
 static ssize_t cm_show_counter(struct kobject *obj, struct attribute *attr,
 			       char *buf)
 {
@@ -3852,7 +3865,7 @@ static void cm_add_one(struct ib_device *ib_device)
 		return;
 
 	cm_dev->ib_device = ib_device;
-	cm_get_ack_delay(cm_dev);
+	cm_dev->ack_delay = ib_device->attrs.local_ca_ack_delay;
 	cm_dev->going_down = 0;
 	cm_dev->device = device_create(&cm_class, &ib_device->dev,
 				       MKDEV(0, 0), NULL,
diff --git a/drivers/infiniband/core/cma.c b/drivers/infiniband/core/cma.c
index 2d762a2ecd81..9729639df407 100644
--- a/drivers/infiniband/core/cma.c
+++ b/drivers/infiniband/core/cma.c
@@ -38,6 +38,7 @@
 #include <linux/in6.h>
 #include <linux/mutex.h>
 #include <linux/random.h>
+#include <linux/igmp.h>
 #include <linux/idr.h>
 #include <linux/inetdevice.h>
 #include <linux/slab.h>
@@ -60,6 +61,8 @@
 #include <rdma/ib_sa.h>
 #include <rdma/iw_cm.h>
 
+#include "core_priv.h"
+
 MODULE_AUTHOR("Sean Hefty");
 MODULE_DESCRIPTION("Generic RDMA CM Agent");
 MODULE_LICENSE("Dual BSD/GPL");
@@ -150,6 +153,7 @@ struct cma_device {
 	struct completion	comp;
 	atomic_t		refcount;
 	struct list_head	id_list;
+	enum ib_gid_type	*default_gid_type;
 };
 
 struct rdma_bind_list {
@@ -185,6 +189,67 @@ enum {
 	CMA_OPTION_AFONLY,
 };
 
+void cma_ref_dev(struct cma_device *cma_dev)
+{
+	atomic_inc(&cma_dev->refcount);
+}
+
+struct cma_device *cma_enum_devices_by_ibdev(cma_device_filter	filter,
+					     void		*cookie)
+{
+	struct cma_device *cma_dev;
+	struct cma_device *found_cma_dev = NULL;
+
+	mutex_lock(&lock);
+
+	list_for_each_entry(cma_dev, &dev_list, list)
+		if (filter(cma_dev->device, cookie)) {
+			found_cma_dev = cma_dev;
+			break;
+		}
+
+	if (found_cma_dev)
+		cma_ref_dev(found_cma_dev);
+	mutex_unlock(&lock);
+	return found_cma_dev;
+}
+
+int cma_get_default_gid_type(struct cma_device *cma_dev,
+			     unsigned int port)
+{
+	if (port < rdma_start_port(cma_dev->device) ||
+	    port > rdma_end_port(cma_dev->device))
+		return -EINVAL;
+
+	return cma_dev->default_gid_type[port - rdma_start_port(cma_dev->device)];
+}
+
+int cma_set_default_gid_type(struct cma_device *cma_dev,
+			     unsigned int port,
+			     enum ib_gid_type default_gid_type)
+{
+	unsigned long supported_gids;
+
+	if (port < rdma_start_port(cma_dev->device) ||
+	    port > rdma_end_port(cma_dev->device))
+		return -EINVAL;
+
+	supported_gids = roce_gid_type_mask_support(cma_dev->device, port);
+
+	if (!(supported_gids & 1 << default_gid_type))
+		return -EINVAL;
+
+	cma_dev->default_gid_type[port - rdma_start_port(cma_dev->device)] =
+		default_gid_type;
+
+	return 0;
+}
+
+struct ib_device *cma_get_ib_dev(struct cma_device *cma_dev)
+{
+	return cma_dev->device;
+}
+
 /*
  * Device removal can occur at anytime, so we need extra handling to
  * serialize notifying the user of device removal with other callbacks.
@@ -228,6 +293,7 @@ struct rdma_id_private {
 	u8			tos;
 	u8			reuseaddr;
 	u8			afonly;
+	enum ib_gid_type	gid_type;
 };
 
 struct cma_multicast {
@@ -239,6 +305,7 @@ struct cma_multicast {
 	void			*context;
 	struct sockaddr_storage	addr;
 	struct kref		mcref;
+	bool			igmp_joined;
 };
 
 struct cma_work {
@@ -335,18 +402,48 @@ static inline void cma_set_ip_ver(struct cma_hdr *hdr, u8 ip_ver)
 	hdr->ip_version = (ip_ver << 4) | (hdr->ip_version & 0xF);
 }
 
-static void cma_attach_to_dev(struct rdma_id_private *id_priv,
-			      struct cma_device *cma_dev)
+static int cma_igmp_send(struct net_device *ndev, union ib_gid *mgid, bool join)
 {
-	atomic_inc(&cma_dev->refcount);
+	struct in_device *in_dev = NULL;
+
+	if (ndev) {
+		rtnl_lock();
+		in_dev = __in_dev_get_rtnl(ndev);
+		if (in_dev) {
+			if (join)
+				ip_mc_inc_group(in_dev,
+						*(__be32 *)(mgid->raw + 12));
+			else
+				ip_mc_dec_group(in_dev,
+						*(__be32 *)(mgid->raw + 12));
+		}
+		rtnl_unlock();
+	}
+	return (in_dev) ? 0 : -ENODEV;
+}
+
+static void _cma_attach_to_dev(struct rdma_id_private *id_priv,
+			       struct cma_device *cma_dev)
+{
+	cma_ref_dev(cma_dev);
 	id_priv->cma_dev = cma_dev;
+	id_priv->gid_type = 0;
 	id_priv->id.device = cma_dev->device;
 	id_priv->id.route.addr.dev_addr.transport =
 		rdma_node_get_transport(cma_dev->device->node_type);
 	list_add_tail(&id_priv->list, &cma_dev->id_list);
 }
 
-static inline void cma_deref_dev(struct cma_device *cma_dev)
+static void cma_attach_to_dev(struct rdma_id_private *id_priv,
+			      struct cma_device *cma_dev)
+{
+	_cma_attach_to_dev(id_priv, cma_dev);
+	id_priv->gid_type =
+		cma_dev->default_gid_type[id_priv->id.port_num -
+					  rdma_start_port(cma_dev->device)];
+}
+
+void cma_deref_dev(struct cma_device *cma_dev)
 {
 	if (atomic_dec_and_test(&cma_dev->refcount))
 		complete(&cma_dev->comp);
@@ -441,6 +538,7 @@ static int cma_translate_addr(struct sockaddr *addr, struct rdma_dev_addr *dev_a
 }
 
 static inline int cma_validate_port(struct ib_device *device, u8 port,
+				    enum ib_gid_type gid_type,
 				      union ib_gid *gid, int dev_type,
 				      int bound_if_index)
 {
@@ -453,10 +551,25 @@ static inline int cma_validate_port(struct ib_device *device, u8 port,
 	if ((dev_type != ARPHRD_INFINIBAND) && rdma_protocol_ib(device, port))
 		return ret;
 
-	if (dev_type == ARPHRD_ETHER)
+	if (dev_type == ARPHRD_ETHER && rdma_protocol_roce(device, port)) {
 		ndev = dev_get_by_index(&init_net, bound_if_index);
+		if (ndev && ndev->flags & IFF_LOOPBACK) {
+			pr_info("detected loopback device\n");
+			dev_put(ndev);
 
-	ret = ib_find_cached_gid_by_port(device, gid, port, ndev, NULL);
+			if (!device->get_netdev)
+				return -EOPNOTSUPP;
+
+			ndev = device->get_netdev(device, port);
+			if (!ndev)
+				return -ENODEV;
+		}
+	} else {
+		gid_type = IB_GID_TYPE_IB;
+	}
+
+	ret = ib_find_cached_gid_by_port(device, gid, gid_type, port,
+					 ndev, NULL);
 
 	if (ndev)
 		dev_put(ndev);
@@ -490,7 +603,10 @@ static int cma_acquire_dev(struct rdma_id_private *id_priv,
 		gidp = rdma_protocol_roce(cma_dev->device, port) ?
 		       &iboe_gid : &gid;
 
-		ret = cma_validate_port(cma_dev->device, port, gidp,
+		ret = cma_validate_port(cma_dev->device, port,
+					rdma_protocol_ib(cma_dev->device, port) ?
+					IB_GID_TYPE_IB :
+					listen_id_priv->gid_type, gidp,
 					dev_addr->dev_type,
 					dev_addr->bound_dev_if);
 		if (!ret) {
@@ -509,8 +625,11 @@ static int cma_acquire_dev(struct rdma_id_private *id_priv,
 			gidp = rdma_protocol_roce(cma_dev->device, port) ?
 			       &iboe_gid : &gid;
 
-			ret = cma_validate_port(cma_dev->device, port, gidp,
-						dev_addr->dev_type,
+			ret = cma_validate_port(cma_dev->device, port,
+						rdma_protocol_ib(cma_dev->device, port) ?
+						IB_GID_TYPE_IB :
+						cma_dev->default_gid_type[port - 1],
+						gidp, dev_addr->dev_type,
 						dev_addr->bound_dev_if);
 			if (!ret) {
 				id_priv->id.port_num = port;
@@ -1437,8 +1556,24 @@ static void cma_leave_mc_groups(struct rdma_id_private *id_priv)
 				      id_priv->id.port_num)) {
 			ib_sa_free_multicast(mc->multicast.ib);
 			kfree(mc);
-		} else
+		} else {
+			if (mc->igmp_joined) {
+				struct rdma_dev_addr *dev_addr =
+					&id_priv->id.route.addr.dev_addr;
+				struct net_device *ndev = NULL;
+
+				if (dev_addr->bound_dev_if)
+					ndev = dev_get_by_index(&init_net,
+								dev_addr->bound_dev_if);
+				if (ndev) {
+					cma_igmp_send(ndev,
+						      &mc->multicast.ib->rec.mgid,
+						      false);
+					dev_put(ndev);
+				}
+			}
 			kref_put(&mc->mcref, release_mc);
+		}
 	}
 }
 
@@ -1896,7 +2031,6 @@ static int iw_conn_req_handler(struct iw_cm_id *cm_id,
 	struct rdma_id_private *listen_id, *conn_id;
 	struct rdma_cm_event event;
 	int ret;
-	struct ib_device_attr attr;
 	struct sockaddr *laddr = (struct sockaddr *)&iw_event->local_addr;
 	struct sockaddr *raddr = (struct sockaddr *)&iw_event->remote_addr;
 
@@ -1938,13 +2072,6 @@ static int iw_conn_req_handler(struct iw_cm_id *cm_id,
 	memcpy(cma_src_addr(conn_id), laddr, rdma_addr_size(laddr));
 	memcpy(cma_dst_addr(conn_id), raddr, rdma_addr_size(raddr));
 
-	ret = ib_query_device(conn_id->id.device, &attr);
-	if (ret) {
-		mutex_unlock(&conn_id->handler_mutex);
-		rdma_destroy_id(new_cm_id);
-		goto out;
-	}
-
 	memset(&event, 0, sizeof event);
 	event.event = RDMA_CM_EVENT_CONNECT_REQUEST;
 	event.param.conn.private_data = iw_event->private_data;
@@ -2051,7 +2178,7 @@ static void cma_listen_on_dev(struct rdma_id_private *id_priv,
 	memcpy(cma_src_addr(dev_id_priv), cma_src_addr(id_priv),
 	       rdma_addr_size(cma_src_addr(id_priv)));
 
-	cma_attach_to_dev(dev_id_priv, cma_dev);
+	_cma_attach_to_dev(dev_id_priv, cma_dev);
 	list_add_tail(&dev_id_priv->listen_list, &id_priv->listen_list);
 	atomic_inc(&id_priv->refcount);
 	dev_id_priv->internal_id = 1;
@@ -2321,8 +2448,23 @@ static int cma_resolve_iboe_route(struct rdma_id_private *id_priv)
 
 	if (addr->dev_addr.bound_dev_if) {
 		ndev = dev_get_by_index(&init_net, addr->dev_addr.bound_dev_if);
+		if (!ndev)
+			return -ENODEV;
+
+		if (ndev->flags & IFF_LOOPBACK) {
+			dev_put(ndev);
+			if (!id_priv->id.device->get_netdev)
+				return -EOPNOTSUPP;
+
+			ndev = id_priv->id.device->get_netdev(id_priv->id.device,
+							      id_priv->id.port_num);
+			if (!ndev)
+				return -ENODEV;
+		}
+
 		route->path_rec->net = &init_net;
-		route->path_rec->ifindex = addr->dev_addr.bound_dev_if;
+		route->path_rec->ifindex = ndev->ifindex;
+		route->path_rec->gid_type = id_priv->gid_type;
 	}
 	if (!ndev) {
 		ret = -ENODEV;
@@ -2336,7 +2478,14 @@ static int cma_resolve_iboe_route(struct rdma_id_private *id_priv)
 	rdma_ip2gid((struct sockaddr *)&id_priv->id.route.addr.dst_addr,
 		    &route->path_rec->dgid);
 
-	route->path_rec->hop_limit = 1;
+	/* Use the hint from IP Stack to select GID Type */
+	if (route->path_rec->gid_type < ib_network_to_gid_type(addr->dev_addr.network))
+		route->path_rec->gid_type = ib_network_to_gid_type(addr->dev_addr.network);
+	if (((struct sockaddr *)&id_priv->id.route.addr.dst_addr)->sa_family != AF_IB)
+		/* TODO: get the hoplimit from the inet/inet6 device */
+		route->path_rec->hop_limit = addr->dev_addr.hoplimit;
+	else
+		route->path_rec->hop_limit = 1;
 	route->path_rec->reversible = 1;
 	route->path_rec->pkey = cpu_to_be16(0xffff);
 	route->path_rec->mtu_selector = IB_SA_EQ;
@@ -3534,12 +3683,23 @@ static int cma_ib_mc_handler(int status, struct ib_sa_multicast *multicast)
 	event.status = status;
 	event.param.ud.private_data = mc->context;
 	if (!status) {
+		struct rdma_dev_addr *dev_addr =
+			&id_priv->id.route.addr.dev_addr;
+		struct net_device *ndev =
+			dev_get_by_index(&init_net, dev_addr->bound_dev_if);
+		enum ib_gid_type gid_type =
+			id_priv->cma_dev->default_gid_type[id_priv->id.port_num -
+			rdma_start_port(id_priv->cma_dev->device)];
+
 		event.event = RDMA_CM_EVENT_MULTICAST_JOIN;
 		ib_init_ah_from_mcmember(id_priv->id.device,
 					 id_priv->id.port_num, &multicast->rec,
+					 ndev, gid_type,
 					 &event.param.ud.ah_attr);
 		event.param.ud.qp_num = 0xFFFFFF;
 		event.param.ud.qkey = be32_to_cpu(multicast->rec.qkey);
+		if (ndev)
+			dev_put(ndev);
 	} else
 		event.event = RDMA_CM_EVENT_MULTICAST_ERROR;
 
@@ -3672,9 +3832,10 @@ static int cma_iboe_join_multicast(struct rdma_id_private *id_priv,
 {
 	struct iboe_mcast_work *work;
 	struct rdma_dev_addr *dev_addr = &id_priv->id.route.addr.dev_addr;
-	int err;
+	int err = 0;
 	struct sockaddr *addr = (struct sockaddr *)&mc->addr;
 	struct net_device *ndev = NULL;
+	enum ib_gid_type gid_type;
 
 	if (cma_zero_addr((struct sockaddr *)&mc->addr))
 		return -EINVAL;
@@ -3704,9 +3865,25 @@ static int cma_iboe_join_multicast(struct rdma_id_private *id_priv,
 	mc->multicast.ib->rec.rate = iboe_get_rate(ndev);
 	mc->multicast.ib->rec.hop_limit = 1;
 	mc->multicast.ib->rec.mtu = iboe_get_mtu(ndev->mtu);
+
+	gid_type = id_priv->cma_dev->default_gid_type[id_priv->id.port_num -
+		   rdma_start_port(id_priv->cma_dev->device)];
+	if (addr->sa_family == AF_INET) {
+		if (gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP)
+			err = cma_igmp_send(ndev, &mc->multicast.ib->rec.mgid,
+					    true);
+		if (!err) {
+			mc->igmp_joined = true;
+			mc->multicast.ib->rec.hop_limit = IPV6_DEFAULT_HOPLIMIT;
+		}
+	} else {
+		if (gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP)
+			err = -ENOTSUPP;
+	}
 	dev_put(ndev);
-	if (!mc->multicast.ib->rec.mtu) {
-		err = -EINVAL;
+	if (err || !mc->multicast.ib->rec.mtu) {
+		if (!err)
+			err = -EINVAL;
 		goto out2;
 	}
 	rdma_ip2gid((struct sockaddr *)&id_priv->id.route.addr.src_addr,
@@ -3745,7 +3922,7 @@ int rdma_join_multicast(struct rdma_cm_id *id, struct sockaddr *addr,
 	memcpy(&mc->addr, addr, rdma_addr_size(addr));
 	mc->context = context;
 	mc->id_priv = id_priv;
-
+	mc->igmp_joined = false;
 	spin_lock(&id_priv->lock);
 	list_add(&mc->list, &id_priv->mc_list);
 	spin_unlock(&id_priv->lock);
@@ -3790,9 +3967,25 @@ void rdma_leave_multicast(struct rdma_cm_id *id, struct sockaddr *addr)
 			if (rdma_cap_ib_mcast(id->device, id->port_num)) {
 				ib_sa_free_multicast(mc->multicast.ib);
 				kfree(mc);
-			} else if (rdma_protocol_roce(id->device, id->port_num))
+			} else if (rdma_protocol_roce(id->device, id->port_num)) {
+				if (mc->igmp_joined) {
+					struct rdma_dev_addr *dev_addr =
+						&id->route.addr.dev_addr;
+					struct net_device *ndev = NULL;
+
+					if (dev_addr->bound_dev_if)
+						ndev = dev_get_by_index(&init_net,
+									dev_addr->bound_dev_if);
+					if (ndev) {
+						cma_igmp_send(ndev,
+							      &mc->multicast.ib->rec.mgid,
+							      false);
+						dev_put(ndev);
+					}
+					mc->igmp_joined = false;
+				}
 				kref_put(&mc->mcref, release_mc);
-
+			}
 			return;
 		}
 	}
@@ -3861,12 +4054,27 @@ static void cma_add_one(struct ib_device *device)
 {
 	struct cma_device *cma_dev;
 	struct rdma_id_private *id_priv;
+	unsigned int i;
+	unsigned long supported_gids = 0;
 
 	cma_dev = kmalloc(sizeof *cma_dev, GFP_KERNEL);
 	if (!cma_dev)
 		return;
 
 	cma_dev->device = device;
+	cma_dev->default_gid_type = kcalloc(device->phys_port_cnt,
+					    sizeof(*cma_dev->default_gid_type),
+					    GFP_KERNEL);
+	if (!cma_dev->default_gid_type) {
+		kfree(cma_dev);
+		return;
+	}
+	for (i = rdma_start_port(device); i <= rdma_end_port(device); i++) {
+		supported_gids = roce_gid_type_mask_support(device, i);
+		WARN_ON(!supported_gids);
+		cma_dev->default_gid_type[i - rdma_start_port(device)] =
+			find_first_bit(&supported_gids, BITS_PER_LONG);
+	}
 
 	init_completion(&cma_dev->comp);
 	atomic_set(&cma_dev->refcount, 1);
@@ -3946,6 +4154,7 @@ static void cma_remove_one(struct ib_device *device, void *client_data)
 	mutex_unlock(&lock);
 
 	cma_process_remove(cma_dev);
+	kfree(cma_dev->default_gid_type);
 	kfree(cma_dev);
 }
 
@@ -4079,6 +4288,7 @@ static int __init cma_init(void)
 
 	if (ibnl_add_client(RDMA_NL_RDMA_CM, RDMA_NL_RDMA_CM_NUM_OPS, cma_cb_table))
 		printk(KERN_WARNING "RDMA CMA: failed to add netlink callback\n");
+	cma_configfs_init();
 
 	return 0;
 
@@ -4093,6 +4303,7 @@ err_wq:
 
 static void __exit cma_cleanup(void)
 {
+	cma_configfs_exit();
 	ibnl_remove_client(RDMA_NL_RDMA_CM);
 	ib_unregister_client(&cma_client);
 	unregister_netdevice_notifier(&cma_nb);
diff --git a/drivers/infiniband/core/cma_configfs.c b/drivers/infiniband/core/cma_configfs.c
new file mode 100644
index 000000000000..18b112aa577e
--- /dev/null
+++ b/drivers/infiniband/core/cma_configfs.c
@@ -0,0 +1,321 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies 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/configfs.h>
+#include <rdma/ib_verbs.h>
+#include "core_priv.h"
+
+struct cma_device;
+
+struct cma_dev_group;
+
+struct cma_dev_port_group {
+	unsigned int		port_num;
+	struct cma_dev_group	*cma_dev_group;
+	struct config_group	group;
+};
+
+struct cma_dev_group {
+	char				name[IB_DEVICE_NAME_MAX];
+	struct config_group		device_group;
+	struct config_group		ports_group;
+	struct config_group		*default_dev_group[2];
+	struct config_group		**default_ports_group;
+	struct cma_dev_port_group	*ports;
+};
+
+static struct cma_dev_port_group *to_dev_port_group(struct config_item *item)
+{
+	struct config_group *group;
+
+	if (!item)
+		return NULL;
+
+	group = container_of(item, struct config_group, cg_item);
+	return container_of(group, struct cma_dev_port_group, group);
+}
+
+static bool filter_by_name(struct ib_device *ib_dev, void *cookie)
+{
+	return !strcmp(ib_dev->name, cookie);
+}
+
+static int cma_configfs_params_get(struct config_item *item,
+				   struct cma_device **pcma_dev,
+				   struct cma_dev_port_group **pgroup)
+{
+	struct cma_dev_port_group *group = to_dev_port_group(item);
+	struct cma_device *cma_dev;
+
+	if (!group)
+		return -ENODEV;
+
+	cma_dev = cma_enum_devices_by_ibdev(filter_by_name,
+					    group->cma_dev_group->name);
+	if (!cma_dev)
+		return -ENODEV;
+
+	*pcma_dev = cma_dev;
+	*pgroup = group;
+
+	return 0;
+}
+
+static void cma_configfs_params_put(struct cma_device *cma_dev)
+{
+	cma_deref_dev(cma_dev);
+}
+
+static ssize_t default_roce_mode_show(struct config_item *item,
+				      char *buf)
+{
+	struct cma_device *cma_dev;
+	struct cma_dev_port_group *group;
+	int gid_type;
+	ssize_t ret;
+
+	ret = cma_configfs_params_get(item, &cma_dev, &group);
+	if (ret)
+		return ret;
+
+	gid_type = cma_get_default_gid_type(cma_dev, group->port_num);
+	cma_configfs_params_put(cma_dev);
+
+	if (gid_type < 0)
+		return gid_type;
+
+	return sprintf(buf, "%s\n", ib_cache_gid_type_str(gid_type));
+}
+
+static ssize_t default_roce_mode_store(struct config_item *item,
+				       const char *buf, size_t count)
+{
+	struct cma_device *cma_dev;
+	struct cma_dev_port_group *group;
+	int gid_type = ib_cache_gid_parse_type_str(buf);
+	ssize_t ret;
+
+	if (gid_type < 0)
+		return -EINVAL;
+
+	ret = cma_configfs_params_get(item, &cma_dev, &group);
+	if (ret)
+		return ret;
+
+	ret = cma_set_default_gid_type(cma_dev, group->port_num, gid_type);
+
+	cma_configfs_params_put(cma_dev);
+
+	return !ret ? strnlen(buf, count) : ret;
+}
+
+CONFIGFS_ATTR(, default_roce_mode);
+
+static struct configfs_attribute *cma_configfs_attributes[] = {
+	&attr_default_roce_mode,
+	NULL,
+};
+
+static struct config_item_type cma_port_group_type = {
+	.ct_attrs	= cma_configfs_attributes,
+	.ct_owner	= THIS_MODULE
+};
+
+static int make_cma_ports(struct cma_dev_group *cma_dev_group,
+			  struct cma_device *cma_dev)
+{
+	struct ib_device *ibdev;
+	unsigned int i;
+	unsigned int ports_num;
+	struct cma_dev_port_group *ports;
+	struct config_group **ports_group;
+	int err;
+
+	ibdev = cma_get_ib_dev(cma_dev);
+
+	if (!ibdev)
+		return -ENODEV;
+
+	ports_num = ibdev->phys_port_cnt;
+	ports = kcalloc(ports_num, sizeof(*cma_dev_group->ports),
+			GFP_KERNEL);
+	ports_group = kcalloc(ports_num + 1, sizeof(*ports_group), GFP_KERNEL);
+
+	if (!ports || !ports_group) {
+		err = -ENOMEM;
+		goto free;
+	}
+
+	for (i = 0; i < ports_num; i++) {
+		char port_str[10];
+
+		ports[i].port_num = i + 1;
+		snprintf(port_str, sizeof(port_str), "%u", i + 1);
+		ports[i].cma_dev_group = cma_dev_group;
+		config_group_init_type_name(&ports[i].group,
+					    port_str,
+					    &cma_port_group_type);
+		ports_group[i] = &ports[i].group;
+	}
+	ports_group[i] = NULL;
+	cma_dev_group->default_ports_group = ports_group;
+	cma_dev_group->ports = ports;
+
+	return 0;
+free:
+	kfree(ports);
+	kfree(ports_group);
+	cma_dev_group->ports = NULL;
+	cma_dev_group->default_ports_group = NULL;
+	return err;
+}
+
+static void release_cma_dev(struct config_item  *item)
+{
+	struct config_group *group = container_of(item, struct config_group,
+						  cg_item);
+	struct cma_dev_group *cma_dev_group = container_of(group,
+							   struct cma_dev_group,
+							   device_group);
+
+	kfree(cma_dev_group);
+};
+
+static void release_cma_ports_group(struct config_item  *item)
+{
+	struct config_group *group = container_of(item, struct config_group,
+						  cg_item);
+	struct cma_dev_group *cma_dev_group = container_of(group,
+							   struct cma_dev_group,
+							   ports_group);
+
+	kfree(cma_dev_group->ports);
+	kfree(cma_dev_group->default_ports_group);
+	cma_dev_group->ports = NULL;
+	cma_dev_group->default_ports_group = NULL;
+};
+
+static struct configfs_item_operations cma_ports_item_ops = {
+	.release = release_cma_ports_group
+};
+
+static struct config_item_type cma_ports_group_type = {
+	.ct_item_ops	= &cma_ports_item_ops,
+	.ct_owner	= THIS_MODULE
+};
+
+static struct configfs_item_operations cma_device_item_ops = {
+	.release = release_cma_dev
+};
+
+static struct config_item_type cma_device_group_type = {
+	.ct_item_ops	= &cma_device_item_ops,
+	.ct_owner	= THIS_MODULE
+};
+
+static struct config_group *make_cma_dev(struct config_group *group,
+					 const char *name)
+{
+	int err = -ENODEV;
+	struct cma_device *cma_dev = cma_enum_devices_by_ibdev(filter_by_name,
+							       (void *)name);
+	struct cma_dev_group *cma_dev_group = NULL;
+
+	if (!cma_dev)
+		goto fail;
+
+	cma_dev_group = kzalloc(sizeof(*cma_dev_group), GFP_KERNEL);
+
+	if (!cma_dev_group) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	strncpy(cma_dev_group->name, name, sizeof(cma_dev_group->name));
+
+	err = make_cma_ports(cma_dev_group, cma_dev);
+	if (err)
+		goto fail;
+
+	cma_dev_group->ports_group.default_groups =
+		cma_dev_group->default_ports_group;
+	config_group_init_type_name(&cma_dev_group->ports_group, "ports",
+				    &cma_ports_group_type);
+
+	cma_dev_group->device_group.default_groups
+		= cma_dev_group->default_dev_group;
+	cma_dev_group->default_dev_group[0] = &cma_dev_group->ports_group;
+	cma_dev_group->default_dev_group[1] = NULL;
+
+	config_group_init_type_name(&cma_dev_group->device_group, name,
+				    &cma_device_group_type);
+
+	cma_deref_dev(cma_dev);
+	return &cma_dev_group->device_group;
+
+fail:
+	if (cma_dev)
+		cma_deref_dev(cma_dev);
+	kfree(cma_dev_group);
+	return ERR_PTR(err);
+}
+
+static struct configfs_group_operations cma_subsys_group_ops = {
+	.make_group	= make_cma_dev,
+};
+
+static struct config_item_type cma_subsys_type = {
+	.ct_group_ops	= &cma_subsys_group_ops,
+	.ct_owner	= THIS_MODULE,
+};
+
+static struct configfs_subsystem cma_subsys = {
+	.su_group	= {
+		.cg_item	= {
+			.ci_namebuf	= "rdma_cm",
+			.ci_type	= &cma_subsys_type,
+		},
+	},
+};
+
+int __init cma_configfs_init(void)
+{
+	config_group_init(&cma_subsys.su_group);
+	mutex_init(&cma_subsys.su_mutex);
+	return configfs_register_subsystem(&cma_subsys);
+}
+
+void __exit cma_configfs_exit(void)
+{
+	configfs_unregister_subsystem(&cma_subsys);
+}
diff --git a/drivers/infiniband/core/core_priv.h b/drivers/infiniband/core/core_priv.h
index 5cf6eb716f00..eab32215756b 100644
--- a/drivers/infiniband/core/core_priv.h
+++ b/drivers/infiniband/core/core_priv.h
@@ -38,6 +38,32 @@
 
 #include <rdma/ib_verbs.h>
 
+#if IS_ENABLED(CONFIG_INFINIBAND_ADDR_TRANS_CONFIGFS)
+int cma_configfs_init(void);
+void cma_configfs_exit(void);
+#else
+static inline int cma_configfs_init(void)
+{
+	return 0;
+}
+
+static inline void cma_configfs_exit(void)
+{
+}
+#endif
+struct cma_device;
+void cma_ref_dev(struct cma_device *cma_dev);
+void cma_deref_dev(struct cma_device *cma_dev);
+typedef bool (*cma_device_filter)(struct ib_device *, void *);
+struct cma_device *cma_enum_devices_by_ibdev(cma_device_filter	filter,
+					     void		*cookie);
+int cma_get_default_gid_type(struct cma_device *cma_dev,
+			     unsigned int port);
+int cma_set_default_gid_type(struct cma_device *cma_dev,
+			     unsigned int port,
+			     enum ib_gid_type default_gid_type);
+struct ib_device *cma_get_ib_dev(struct cma_device *cma_dev);
+
 int  ib_device_register_sysfs(struct ib_device *device,
 			      int (*port_callback)(struct ib_device *,
 						   u8, struct kobject *));
@@ -70,8 +96,13 @@ enum ib_cache_gid_default_mode {
 	IB_CACHE_GID_DEFAULT_MODE_DELETE
 };
 
+int ib_cache_gid_parse_type_str(const char *buf);
+
+const char *ib_cache_gid_type_str(enum ib_gid_type gid_type);
+
 void ib_cache_gid_set_default_gid(struct ib_device *ib_dev, u8 port,
 				  struct net_device *ndev,
+				  unsigned long gid_type_mask,
 				  enum ib_cache_gid_default_mode mode);
 
 int ib_cache_gid_add(struct ib_device *ib_dev, u8 port,
@@ -87,9 +118,23 @@ int roce_gid_mgmt_init(void);
 void roce_gid_mgmt_cleanup(void);
 
 int roce_rescan_device(struct ib_device *ib_dev);
+unsigned long roce_gid_type_mask_support(struct ib_device *ib_dev, u8 port);
 
 int ib_cache_setup_one(struct ib_device *device);
 void ib_cache_cleanup_one(struct ib_device *device);
 void ib_cache_release_one(struct ib_device *device);
 
+static inline bool rdma_is_upper_dev_rcu(struct net_device *dev,
+					 struct net_device *upper)
+{
+	struct net_device *_upper = NULL;
+	struct list_head *iter;
+
+	netdev_for_each_all_upper_dev_rcu(dev, _upper, iter)
+		if (_upper == upper)
+			break;
+
+	return _upper == upper;
+}
+
 #endif /* _CORE_PRIV_H */
diff --git a/drivers/infiniband/core/cq.c b/drivers/infiniband/core/cq.c
new file mode 100644
index 000000000000..a754fc727de5
--- /dev/null
+++ b/drivers/infiniband/core/cq.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2015 HGST, a Western Digital Company.
+ *
+ * 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/err.h>
+#include <linux/slab.h>
+#include <rdma/ib_verbs.h>
+
+/* # of WCs to poll for with a single call to ib_poll_cq */
+#define IB_POLL_BATCH			16
+
+/* # of WCs to iterate over before yielding */
+#define IB_POLL_BUDGET_IRQ		256
+#define IB_POLL_BUDGET_WORKQUEUE	65536
+
+#define IB_POLL_FLAGS \
+	(IB_CQ_NEXT_COMP | IB_CQ_REPORT_MISSED_EVENTS)
+
+static int __ib_process_cq(struct ib_cq *cq, int budget)
+{
+	int i, n, completed = 0;
+
+	while ((n = ib_poll_cq(cq, IB_POLL_BATCH, cq->wc)) > 0) {
+		for (i = 0; i < n; i++) {
+			struct ib_wc *wc = &cq->wc[i];
+
+			if (wc->wr_cqe)
+				wc->wr_cqe->done(cq, wc);
+			else
+				WARN_ON_ONCE(wc->status == IB_WC_SUCCESS);
+		}
+
+		completed += n;
+
+		if (n != IB_POLL_BATCH ||
+		    (budget != -1 && completed >= budget))
+			break;
+	}
+
+	return completed;
+}
+
+/**
+ * ib_process_direct_cq - process a CQ in caller context
+ * @cq:		CQ to process
+ * @budget:	number of CQEs to poll for
+ *
+ * This function is used to process all outstanding CQ entries on a
+ * %IB_POLL_DIRECT CQ.  It does not offload CQ processing to a different
+ * context and does not ask for completion interrupts from the HCA.
+ *
+ * Note: for compatibility reasons -1 can be passed in %budget for unlimited
+ * polling.  Do not use this feature in new code, it will be removed soon.
+ */
+int ib_process_cq_direct(struct ib_cq *cq, int budget)
+{
+	WARN_ON_ONCE(cq->poll_ctx != IB_POLL_DIRECT);
+
+	return __ib_process_cq(cq, budget);
+}
+EXPORT_SYMBOL(ib_process_cq_direct);
+
+static void ib_cq_completion_direct(struct ib_cq *cq, void *private)
+{
+	WARN_ONCE(1, "got unsolicited completion for CQ 0x%p\n", cq);
+}
+
+static int ib_poll_handler(struct irq_poll *iop, int budget)
+{
+	struct ib_cq *cq = container_of(iop, struct ib_cq, iop);
+	int completed;
+
+	completed = __ib_process_cq(cq, budget);
+	if (completed < budget) {
+		irq_poll_complete(&cq->iop);
+		if (ib_req_notify_cq(cq, IB_POLL_FLAGS) > 0)
+			irq_poll_sched(&cq->iop);
+	}
+
+	return completed;
+}
+
+static void ib_cq_completion_softirq(struct ib_cq *cq, void *private)
+{
+	irq_poll_sched(&cq->iop);
+}
+
+static void ib_cq_poll_work(struct work_struct *work)
+{
+	struct ib_cq *cq = container_of(work, struct ib_cq, work);
+	int completed;
+
+	completed = __ib_process_cq(cq, IB_POLL_BUDGET_WORKQUEUE);
+	if (completed >= IB_POLL_BUDGET_WORKQUEUE ||
+	    ib_req_notify_cq(cq, IB_POLL_FLAGS) > 0)
+		queue_work(ib_comp_wq, &cq->work);
+}
+
+static void ib_cq_completion_workqueue(struct ib_cq *cq, void *private)
+{
+	queue_work(ib_comp_wq, &cq->work);
+}
+
+/**
+ * ib_alloc_cq - allocate a completion queue
+ * @dev:		device to allocate the CQ for
+ * @private:		driver private data, accessible from cq->cq_context
+ * @nr_cqe:		number of CQEs to allocate
+ * @comp_vector:	HCA completion vectors for this CQ
+ * @poll_ctx:		context to poll the CQ from.
+ *
+ * This is the proper interface to allocate a CQ for in-kernel users. A
+ * CQ allocated with this interface will automatically be polled from the
+ * specified context.  The ULP needs must use wr->wr_cqe instead of wr->wr_id
+ * to use this CQ abstraction.
+ */
+struct ib_cq *ib_alloc_cq(struct ib_device *dev, void *private,
+		int nr_cqe, int comp_vector, enum ib_poll_context poll_ctx)
+{
+	struct ib_cq_init_attr cq_attr = {
+		.cqe		= nr_cqe,
+		.comp_vector	= comp_vector,
+	};
+	struct ib_cq *cq;
+	int ret = -ENOMEM;
+
+	cq = dev->create_cq(dev, &cq_attr, NULL, NULL);
+	if (IS_ERR(cq))
+		return cq;
+
+	cq->device = dev;
+	cq->uobject = NULL;
+	cq->event_handler = NULL;
+	cq->cq_context = private;
+	cq->poll_ctx = poll_ctx;
+	atomic_set(&cq->usecnt, 0);
+
+	cq->wc = kmalloc_array(IB_POLL_BATCH, sizeof(*cq->wc), GFP_KERNEL);
+	if (!cq->wc)
+		goto out_destroy_cq;
+
+	switch (cq->poll_ctx) {
+	case IB_POLL_DIRECT:
+		cq->comp_handler = ib_cq_completion_direct;
+		break;
+	case IB_POLL_SOFTIRQ:
+		cq->comp_handler = ib_cq_completion_softirq;
+
+		irq_poll_init(&cq->iop, IB_POLL_BUDGET_IRQ, ib_poll_handler);
+		ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
+		break;
+	case IB_POLL_WORKQUEUE:
+		cq->comp_handler = ib_cq_completion_workqueue;
+		INIT_WORK(&cq->work, ib_cq_poll_work);
+		ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
+		break;
+	default:
+		ret = -EINVAL;
+		goto out_free_wc;
+	}
+
+	return cq;
+
+out_free_wc:
+	kfree(cq->wc);
+out_destroy_cq:
+	cq->device->destroy_cq(cq);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL(ib_alloc_cq);
+
+/**
+ * ib_free_cq - free a completion queue
+ * @cq:		completion queue to free.
+ */
+void ib_free_cq(struct ib_cq *cq)
+{
+	int ret;
+
+	if (WARN_ON_ONCE(atomic_read(&cq->usecnt)))
+		return;
+
+	switch (cq->poll_ctx) {
+	case IB_POLL_DIRECT:
+		break;
+	case IB_POLL_SOFTIRQ:
+		irq_poll_disable(&cq->iop);
+		break;
+	case IB_POLL_WORKQUEUE:
+		flush_work(&cq->work);
+		break;
+	default:
+		WARN_ON_ONCE(1);
+	}
+
+	kfree(cq->wc);
+	ret = cq->device->destroy_cq(cq);
+	WARN_ON_ONCE(ret);
+}
+EXPORT_SYMBOL(ib_free_cq);
diff --git a/drivers/infiniband/core/device.c b/drivers/infiniband/core/device.c
index 179e8134d57f..94b80a51ab68 100644
--- a/drivers/infiniband/core/device.c
+++ b/drivers/infiniband/core/device.c
@@ -58,6 +58,7 @@ struct ib_client_data {
 	bool		  going_down;
 };
 
+struct workqueue_struct *ib_comp_wq;
 struct workqueue_struct *ib_wq;
 EXPORT_SYMBOL_GPL(ib_wq);
 
@@ -325,6 +326,7 @@ int ib_register_device(struct ib_device *device,
 {
 	int ret;
 	struct ib_client *client;
+	struct ib_udata uhw = {.outlen = 0, .inlen = 0};
 
 	mutex_lock(&device_mutex);
 
@@ -352,6 +354,14 @@ int ib_register_device(struct ib_device *device,
 		goto out;
 	}
 
+	memset(&device->attrs, 0, sizeof(device->attrs));
+	ret = device->query_device(device, &device->attrs, &uhw);
+	if (ret) {
+		printk(KERN_WARNING "Couldn't query the device attributes\n");
+		ib_cache_cleanup_one(device);
+		goto out;
+	}
+
 	ret = ib_device_register_sysfs(device, port_callback);
 	if (ret) {
 		printk(KERN_WARNING "Couldn't register device %s with driver model\n",
@@ -628,25 +638,6 @@ void ib_dispatch_event(struct ib_event *event)
 EXPORT_SYMBOL(ib_dispatch_event);
 
 /**
- * ib_query_device - Query IB device attributes
- * @device:Device to query
- * @device_attr:Device attributes
- *
- * ib_query_device() returns the attributes of a device through the
- * @device_attr pointer.
- */
-int ib_query_device(struct ib_device *device,
-		    struct ib_device_attr *device_attr)
-{
-	struct ib_udata uhw = {.outlen = 0, .inlen = 0};
-
-	memset(device_attr, 0, sizeof(*device_attr));
-
-	return device->query_device(device, device_attr, &uhw);
-}
-EXPORT_SYMBOL(ib_query_device);
-
-/**
  * ib_query_port - Query IB port attributes
  * @device:Device to query
  * @port_num:Port number to query
@@ -825,26 +816,31 @@ EXPORT_SYMBOL(ib_modify_port);
  *   a specified GID value occurs.
  * @device: The device to query.
  * @gid: The GID value to search for.
+ * @gid_type: Type of GID.
  * @ndev: The ndev related to the GID to search for.
  * @port_num: The port number of the device where the GID value was found.
  * @index: The index into the GID table where the GID was found.  This
  *   parameter may be NULL.
  */
 int ib_find_gid(struct ib_device *device, union ib_gid *gid,
-		struct net_device *ndev, u8 *port_num, u16 *index)
+		enum ib_gid_type gid_type, struct net_device *ndev,
+		u8 *port_num, u16 *index)
 {
 	union ib_gid tmp_gid;
 	int ret, port, i;
 
 	for (port = rdma_start_port(device); port <= rdma_end_port(device); ++port) {
 		if (rdma_cap_roce_gid_table(device, port)) {
-			if (!ib_find_cached_gid_by_port(device, gid, port,
+			if (!ib_find_cached_gid_by_port(device, gid, gid_type, port,
 							ndev, index)) {
 				*port_num = port;
 				return 0;
 			}
 		}
 
+		if (gid_type != IB_GID_TYPE_IB)
+			continue;
+
 		for (i = 0; i < device->port_immutable[port].gid_tbl_len; ++i) {
 			ret = ib_query_gid(device, port, i, &tmp_gid, NULL);
 			if (ret)
@@ -954,10 +950,18 @@ static int __init ib_core_init(void)
 	if (!ib_wq)
 		return -ENOMEM;
 
+	ib_comp_wq = alloc_workqueue("ib-comp-wq",
+			WQ_UNBOUND | WQ_HIGHPRI | WQ_MEM_RECLAIM,
+			WQ_UNBOUND_MAX_ACTIVE);
+	if (!ib_comp_wq) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
 	ret = class_register(&ib_class);
 	if (ret) {
 		printk(KERN_WARNING "Couldn't create InfiniBand device class\n");
-		goto err;
+		goto err_comp;
 	}
 
 	ret = ibnl_init();
@@ -972,7 +976,8 @@ static int __init ib_core_init(void)
 
 err_sysfs:
 	class_unregister(&ib_class);
-
+err_comp:
+	destroy_workqueue(ib_comp_wq);
 err:
 	destroy_workqueue(ib_wq);
 	return ret;
@@ -983,6 +988,7 @@ static void __exit ib_core_cleanup(void)
 	ib_cache_cleanup();
 	ibnl_cleanup();
 	class_unregister(&ib_class);
+	destroy_workqueue(ib_comp_wq);
 	/* Make sure that any pending umem accounting work is done. */
 	destroy_workqueue(ib_wq);
 }
diff --git a/drivers/infiniband/core/fmr_pool.c b/drivers/infiniband/core/fmr_pool.c
index 9f5ad7cc33c8..6ac3683c144b 100644
--- a/drivers/infiniband/core/fmr_pool.c
+++ b/drivers/infiniband/core/fmr_pool.c
@@ -212,7 +212,6 @@ struct ib_fmr_pool *ib_create_fmr_pool(struct ib_pd             *pd,
 {
 	struct ib_device   *device;
 	struct ib_fmr_pool *pool;
-	struct ib_device_attr *attr;
 	int i;
 	int ret;
 	int max_remaps;
@@ -228,25 +227,10 @@ struct ib_fmr_pool *ib_create_fmr_pool(struct ib_pd             *pd,
 		return ERR_PTR(-ENOSYS);
 	}
 
-	attr = kmalloc(sizeof *attr, GFP_KERNEL);
-	if (!attr) {
-		printk(KERN_WARNING PFX "couldn't allocate device attr struct\n");
-		return ERR_PTR(-ENOMEM);
-	}
-
-	ret = ib_query_device(device, attr);
-	if (ret) {
-		printk(KERN_WARNING PFX "couldn't query device: %d\n", ret);
-		kfree(attr);
-		return ERR_PTR(ret);
-	}
-
-	if (!attr->max_map_per_fmr)
+	if (!device->attrs.max_map_per_fmr)
 		max_remaps = IB_FMR_MAX_REMAPS;
 	else
-		max_remaps = attr->max_map_per_fmr;
-
-	kfree(attr);
+		max_remaps = device->attrs.max_map_per_fmr;
 
 	pool = kmalloc(sizeof *pool, GFP_KERNEL);
 	if (!pool) {
diff --git a/drivers/infiniband/core/mad.c b/drivers/infiniband/core/mad.c
index 2281de122038..9fa5bf33f5a3 100644
--- a/drivers/infiniband/core/mad.c
+++ b/drivers/infiniband/core/mad.c
@@ -84,6 +84,9 @@ static int add_nonoui_reg_req(struct ib_mad_reg_req *mad_reg_req,
 			      u8 mgmt_class);
 static int add_oui_reg_req(struct ib_mad_reg_req *mad_reg_req,
 			   struct ib_mad_agent_private *agent_priv);
+static bool ib_mad_send_error(struct ib_mad_port_private *port_priv,
+			      struct ib_wc *wc);
+static void ib_mad_send_done(struct ib_cq *cq, struct ib_wc *wc);
 
 /*
  * Returns a ib_mad_port_private structure or NULL for a device/port
@@ -681,7 +684,7 @@ static void snoop_recv(struct ib_mad_qp_info *qp_info,
 
 		atomic_inc(&mad_snoop_priv->refcount);
 		spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
-		mad_snoop_priv->agent.recv_handler(&mad_snoop_priv->agent,
+		mad_snoop_priv->agent.recv_handler(&mad_snoop_priv->agent, NULL,
 						   mad_recv_wc);
 		deref_snoop_agent(mad_snoop_priv);
 		spin_lock_irqsave(&qp_info->snoop_lock, flags);
@@ -689,12 +692,11 @@ static void snoop_recv(struct ib_mad_qp_info *qp_info,
 	spin_unlock_irqrestore(&qp_info->snoop_lock, flags);
 }
 
-static void build_smp_wc(struct ib_qp *qp,
-			 u64 wr_id, u16 slid, u16 pkey_index, u8 port_num,
-			 struct ib_wc *wc)
+static void build_smp_wc(struct ib_qp *qp, struct ib_cqe *cqe, u16 slid,
+		u16 pkey_index, u8 port_num, struct ib_wc *wc)
 {
 	memset(wc, 0, sizeof *wc);
-	wc->wr_id = wr_id;
+	wc->wr_cqe = cqe;
 	wc->status = IB_WC_SUCCESS;
 	wc->opcode = IB_WC_RECV;
 	wc->pkey_index = pkey_index;
@@ -832,7 +834,7 @@ static int handle_outgoing_dr_smp(struct ib_mad_agent_private *mad_agent_priv,
 	}
 
 	build_smp_wc(mad_agent_priv->agent.qp,
-		     send_wr->wr.wr_id, drslid,
+		     send_wr->wr.wr_cqe, drslid,
 		     send_wr->pkey_index,
 		     send_wr->port_num, &mad_wc);
 
@@ -1039,7 +1041,9 @@ struct ib_mad_send_buf * ib_create_send_mad(struct ib_mad_agent *mad_agent,
 
 	mad_send_wr->sg_list[1].lkey = mad_agent->qp->pd->local_dma_lkey;
 
-	mad_send_wr->send_wr.wr.wr_id = (unsigned long) mad_send_wr;
+	mad_send_wr->mad_list.cqe.done = ib_mad_send_done;
+
+	mad_send_wr->send_wr.wr.wr_cqe = &mad_send_wr->mad_list.cqe;
 	mad_send_wr->send_wr.wr.sg_list = mad_send_wr->sg_list;
 	mad_send_wr->send_wr.wr.num_sge = 2;
 	mad_send_wr->send_wr.wr.opcode = IB_WR_SEND;
@@ -1151,8 +1155,9 @@ int ib_send_mad(struct ib_mad_send_wr_private *mad_send_wr)
 
 	/* Set WR ID to find mad_send_wr upon completion */
 	qp_info = mad_send_wr->mad_agent_priv->qp_info;
-	mad_send_wr->send_wr.wr.wr_id = (unsigned long)&mad_send_wr->mad_list;
 	mad_send_wr->mad_list.mad_queue = &qp_info->send_queue;
+	mad_send_wr->mad_list.cqe.done = ib_mad_send_done;
+	mad_send_wr->send_wr.wr.wr_cqe = &mad_send_wr->mad_list.cqe;
 
 	mad_agent = mad_send_wr->send_buf.mad_agent;
 	sge = mad_send_wr->sg_list;
@@ -1982,9 +1987,9 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 				/* user rmpp is in effect
 				 * and this is an active RMPP MAD
 				 */
-				mad_recv_wc->wc->wr_id = 0;
-				mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent,
-								   mad_recv_wc);
+				mad_agent_priv->agent.recv_handler(
+						&mad_agent_priv->agent, NULL,
+						mad_recv_wc);
 				atomic_dec(&mad_agent_priv->refcount);
 			} else {
 				/* not user rmpp, revert to normal behavior and
@@ -1998,9 +2003,10 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 			spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 
 			/* Defined behavior is to complete response before request */
-			mad_recv_wc->wc->wr_id = (unsigned long) &mad_send_wr->send_buf;
-			mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent,
-							   mad_recv_wc);
+			mad_agent_priv->agent.recv_handler(
+					&mad_agent_priv->agent,
+					&mad_send_wr->send_buf,
+					mad_recv_wc);
 			atomic_dec(&mad_agent_priv->refcount);
 
 			mad_send_wc.status = IB_WC_SUCCESS;
@@ -2009,7 +2015,7 @@ static void ib_mad_complete_recv(struct ib_mad_agent_private *mad_agent_priv,
 			ib_mad_complete_send_wr(mad_send_wr, &mad_send_wc);
 		}
 	} else {
-		mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent,
+		mad_agent_priv->agent.recv_handler(&mad_agent_priv->agent, NULL,
 						   mad_recv_wc);
 		deref_mad_agent(mad_agent_priv);
 	}
@@ -2172,13 +2178,14 @@ handle_smi(struct ib_mad_port_private *port_priv,
 	return handle_ib_smi(port_priv, qp_info, wc, port_num, recv, response);
 }
 
-static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
-				     struct ib_wc *wc)
+static void ib_mad_recv_done(struct ib_cq *cq, struct ib_wc *wc)
 {
+	struct ib_mad_port_private *port_priv = cq->cq_context;
+	struct ib_mad_list_head *mad_list =
+		container_of(wc->wr_cqe, struct ib_mad_list_head, cqe);
 	struct ib_mad_qp_info *qp_info;
 	struct ib_mad_private_header *mad_priv_hdr;
 	struct ib_mad_private *recv, *response = NULL;
-	struct ib_mad_list_head *mad_list;
 	struct ib_mad_agent_private *mad_agent;
 	int port_num;
 	int ret = IB_MAD_RESULT_SUCCESS;
@@ -2186,7 +2193,17 @@ static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
 	u16 resp_mad_pkey_index = 0;
 	bool opa;
 
-	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
+	if (list_empty_careful(&port_priv->port_list))
+		return;
+
+	if (wc->status != IB_WC_SUCCESS) {
+		/*
+		 * Receive errors indicate that the QP has entered the error
+		 * state - error handling/shutdown code will cleanup
+		 */
+		return;
+	}
+
 	qp_info = mad_list->mad_queue->qp_info;
 	dequeue_mad(mad_list);
 
@@ -2227,7 +2244,7 @@ static void ib_mad_recv_done_handler(struct ib_mad_port_private *port_priv,
 	response = alloc_mad_private(mad_size, GFP_KERNEL);
 	if (!response) {
 		dev_err(&port_priv->device->dev,
-			"ib_mad_recv_done_handler no memory for response buffer\n");
+			"%s: no memory for response buffer\n", __func__);
 		goto out;
 	}
 
@@ -2413,11 +2430,12 @@ done:
 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 }
 
-static void ib_mad_send_done_handler(struct ib_mad_port_private *port_priv,
-				     struct ib_wc *wc)
+static void ib_mad_send_done(struct ib_cq *cq, struct ib_wc *wc)
 {
+	struct ib_mad_port_private *port_priv = cq->cq_context;
+	struct ib_mad_list_head *mad_list =
+		container_of(wc->wr_cqe, struct ib_mad_list_head, cqe);
 	struct ib_mad_send_wr_private	*mad_send_wr, *queued_send_wr;
-	struct ib_mad_list_head		*mad_list;
 	struct ib_mad_qp_info		*qp_info;
 	struct ib_mad_queue		*send_queue;
 	struct ib_send_wr		*bad_send_wr;
@@ -2425,7 +2443,14 @@ static void ib_mad_send_done_handler(struct ib_mad_port_private *port_priv,
 	unsigned long flags;
 	int ret;
 
-	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
+	if (list_empty_careful(&port_priv->port_list))
+		return;
+
+	if (wc->status != IB_WC_SUCCESS) {
+		if (!ib_mad_send_error(port_priv, wc))
+			return;
+	}
+
 	mad_send_wr = container_of(mad_list, struct ib_mad_send_wr_private,
 				   mad_list);
 	send_queue = mad_list->mad_queue;
@@ -2490,24 +2515,15 @@ static void mark_sends_for_retry(struct ib_mad_qp_info *qp_info)
 	spin_unlock_irqrestore(&qp_info->send_queue.lock, flags);
 }
 
-static void mad_error_handler(struct ib_mad_port_private *port_priv,
-			      struct ib_wc *wc)
+static bool ib_mad_send_error(struct ib_mad_port_private *port_priv,
+		struct ib_wc *wc)
 {
-	struct ib_mad_list_head *mad_list;
-	struct ib_mad_qp_info *qp_info;
+	struct ib_mad_list_head *mad_list =
+		container_of(wc->wr_cqe, struct ib_mad_list_head, cqe);
+	struct ib_mad_qp_info *qp_info = mad_list->mad_queue->qp_info;
 	struct ib_mad_send_wr_private *mad_send_wr;
 	int ret;
 
-	/* Determine if failure was a send or receive */
-	mad_list = (struct ib_mad_list_head *)(unsigned long)wc->wr_id;
-	qp_info = mad_list->mad_queue->qp_info;
-	if (mad_list->mad_queue == &qp_info->recv_queue)
-		/*
-		 * Receive errors indicate that the QP has entered the error
-		 * state - error handling/shutdown code will cleanup
-		 */
-		return;
-
 	/*
 	 * Send errors will transition the QP to SQE - move
 	 * QP to RTS and repost flushed work requests
@@ -2522,10 +2538,9 @@ static void mad_error_handler(struct ib_mad_port_private *port_priv,
 			mad_send_wr->retry = 0;
 			ret = ib_post_send(qp_info->qp, &mad_send_wr->send_wr.wr,
 					&bad_send_wr);
-			if (ret)
-				ib_mad_send_done_handler(port_priv, wc);
-		} else
-			ib_mad_send_done_handler(port_priv, wc);
+			if (!ret)
+				return false;
+		}
 	} else {
 		struct ib_qp_attr *attr;
 
@@ -2539,42 +2554,14 @@ static void mad_error_handler(struct ib_mad_port_private *port_priv,
 			kfree(attr);
 			if (ret)
 				dev_err(&port_priv->device->dev,
-					"mad_error_handler - ib_modify_qp to RTS : %d\n",
-					ret);
+					"%s - ib_modify_qp to RTS: %d\n",
+					__func__, ret);
 			else
 				mark_sends_for_retry(qp_info);
 		}
-		ib_mad_send_done_handler(port_priv, wc);
 	}
-}
 
-/*
- * IB MAD completion callback
- */
-static void ib_mad_completion_handler(struct work_struct *work)
-{
-	struct ib_mad_port_private *port_priv;
-	struct ib_wc wc;
-
-	port_priv = container_of(work, struct ib_mad_port_private, work);
-	ib_req_notify_cq(port_priv->cq, IB_CQ_NEXT_COMP);
-
-	while (ib_poll_cq(port_priv->cq, 1, &wc) == 1) {
-		if (wc.status == IB_WC_SUCCESS) {
-			switch (wc.opcode) {
-			case IB_WC_SEND:
-				ib_mad_send_done_handler(port_priv, &wc);
-				break;
-			case IB_WC_RECV:
-				ib_mad_recv_done_handler(port_priv, &wc);
-				break;
-			default:
-				BUG_ON(1);
-				break;
-			}
-		} else
-			mad_error_handler(port_priv, &wc);
-	}
+	return true;
 }
 
 static void cancel_mads(struct ib_mad_agent_private *mad_agent_priv)
@@ -2716,7 +2703,7 @@ static void local_completions(struct work_struct *work)
 			 * before request
 			 */
 			build_smp_wc(recv_mad_agent->agent.qp,
-				     (unsigned long) local->mad_send_wr,
+				     local->mad_send_wr->send_wr.wr.wr_cqe,
 				     be16_to_cpu(IB_LID_PERMISSIVE),
 				     local->mad_send_wr->send_wr.pkey_index,
 				     recv_mad_agent->agent.port_num, &wc);
@@ -2744,6 +2731,7 @@ static void local_completions(struct work_struct *work)
 					   IB_MAD_SNOOP_RECVS);
 			recv_mad_agent->agent.recv_handler(
 						&recv_mad_agent->agent,
+						&local->mad_send_wr->send_buf,
 						&local->mad_priv->header.recv_wc);
 			spin_lock_irqsave(&recv_mad_agent->lock, flags);
 			atomic_dec(&recv_mad_agent->refcount);
@@ -2855,17 +2843,6 @@ static void timeout_sends(struct work_struct *work)
 	spin_unlock_irqrestore(&mad_agent_priv->lock, flags);
 }
 
-static void ib_mad_thread_completion_handler(struct ib_cq *cq, void *arg)
-{
-	struct ib_mad_port_private *port_priv = cq->cq_context;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
-	if (!list_empty(&port_priv->port_list))
-		queue_work(port_priv->wq, &port_priv->work);
-	spin_unlock_irqrestore(&ib_mad_port_list_lock, flags);
-}
-
 /*
  * Allocate receive MADs and post receive WRs for them
  */
@@ -2913,8 +2890,9 @@ static int ib_mad_post_receive_mads(struct ib_mad_qp_info *qp_info,
 			break;
 		}
 		mad_priv->header.mapping = sg_list.addr;
-		recv_wr.wr_id = (unsigned long)&mad_priv->header.mad_list;
 		mad_priv->header.mad_list.mad_queue = recv_queue;
+		mad_priv->header.mad_list.cqe.done = ib_mad_recv_done;
+		recv_wr.wr_cqe = &mad_priv->header.mad_list.cqe;
 
 		/* Post receive WR */
 		spin_lock_irqsave(&recv_queue->lock, flags);
@@ -3151,7 +3129,6 @@ static int ib_mad_port_open(struct ib_device *device,
 	unsigned long flags;
 	char name[sizeof "ib_mad123"];
 	int has_smi;
-	struct ib_cq_init_attr cq_attr = {};
 
 	if (WARN_ON(rdma_max_mad_size(device, port_num) < IB_MGMT_MAD_SIZE))
 		return -EFAULT;
@@ -3179,10 +3156,8 @@ static int ib_mad_port_open(struct ib_device *device,
 	if (has_smi)
 		cq_size *= 2;
 
-	cq_attr.cqe = cq_size;
-	port_priv->cq = ib_create_cq(port_priv->device,
-				     ib_mad_thread_completion_handler,
-				     NULL, port_priv, &cq_attr);
+	port_priv->cq = ib_alloc_cq(port_priv->device, port_priv, cq_size, 0,
+			IB_POLL_WORKQUEUE);
 	if (IS_ERR(port_priv->cq)) {
 		dev_err(&device->dev, "Couldn't create ib_mad CQ\n");
 		ret = PTR_ERR(port_priv->cq);
@@ -3211,7 +3186,6 @@ static int ib_mad_port_open(struct ib_device *device,
 		ret = -ENOMEM;
 		goto error8;
 	}
-	INIT_WORK(&port_priv->work, ib_mad_completion_handler);
 
 	spin_lock_irqsave(&ib_mad_port_list_lock, flags);
 	list_add_tail(&port_priv->port_list, &ib_mad_port_list);
@@ -3238,7 +3212,7 @@ error7:
 error6:
 	ib_dealloc_pd(port_priv->pd);
 error4:
-	ib_destroy_cq(port_priv->cq);
+	ib_free_cq(port_priv->cq);
 	cleanup_recv_queue(&port_priv->qp_info[1]);
 	cleanup_recv_queue(&port_priv->qp_info[0]);
 error3:
@@ -3271,7 +3245,7 @@ static int ib_mad_port_close(struct ib_device *device, int port_num)
 	destroy_mad_qp(&port_priv->qp_info[1]);
 	destroy_mad_qp(&port_priv->qp_info[0]);
 	ib_dealloc_pd(port_priv->pd);
-	ib_destroy_cq(port_priv->cq);
+	ib_free_cq(port_priv->cq);
 	cleanup_recv_queue(&port_priv->qp_info[1]);
 	cleanup_recv_queue(&port_priv->qp_info[0]);
 	/* XXX: Handle deallocation of MAD registration tables */
diff --git a/drivers/infiniband/core/mad_priv.h b/drivers/infiniband/core/mad_priv.h
index 990698a6ab4b..28669f6419e1 100644
--- a/drivers/infiniband/core/mad_priv.h
+++ b/drivers/infiniband/core/mad_priv.h
@@ -64,6 +64,7 @@
 
 struct ib_mad_list_head {
 	struct list_head list;
+	struct ib_cqe cqe;
 	struct ib_mad_queue *mad_queue;
 };
 
@@ -204,7 +205,6 @@ struct ib_mad_port_private {
 	struct ib_mad_mgmt_version_table version[MAX_MGMT_VERSION];
 	struct list_head agent_list;
 	struct workqueue_struct *wq;
-	struct work_struct work;
 	struct ib_mad_qp_info qp_info[IB_MAD_QPS_CORE];
 };
 
diff --git a/drivers/infiniband/core/multicast.c b/drivers/infiniband/core/multicast.c
index bb6685fb08c6..250937cb9a1a 100644
--- a/drivers/infiniband/core/multicast.c
+++ b/drivers/infiniband/core/multicast.c
@@ -723,14 +723,27 @@ EXPORT_SYMBOL(ib_sa_get_mcmember_rec);
 
 int ib_init_ah_from_mcmember(struct ib_device *device, u8 port_num,
 			     struct ib_sa_mcmember_rec *rec,
+			     struct net_device *ndev,
+			     enum ib_gid_type gid_type,
 			     struct ib_ah_attr *ah_attr)
 {
 	int ret;
 	u16 gid_index;
 	u8 p;
 
-	ret = ib_find_cached_gid(device, &rec->port_gid,
-				 NULL, &p, &gid_index);
+	if (rdma_protocol_roce(device, port_num)) {
+		ret = ib_find_cached_gid_by_port(device, &rec->port_gid,
+						 gid_type, port_num,
+						 ndev,
+						 &gid_index);
+	} else if (rdma_protocol_ib(device, port_num)) {
+		ret = ib_find_cached_gid(device, &rec->port_gid,
+					 IB_GID_TYPE_IB, NULL, &p,
+					 &gid_index);
+	} else {
+		ret = -EINVAL;
+	}
+
 	if (ret)
 		return ret;
 
diff --git a/drivers/infiniband/core/roce_gid_mgmt.c b/drivers/infiniband/core/roce_gid_mgmt.c
index 178f98482e13..06556c34606d 100644
--- a/drivers/infiniband/core/roce_gid_mgmt.c
+++ b/drivers/infiniband/core/roce_gid_mgmt.c
@@ -67,17 +67,53 @@ struct netdev_event_work {
 	struct netdev_event_work_cmd	cmds[ROCE_NETDEV_CALLBACK_SZ];
 };
 
+static const struct {
+	bool (*is_supported)(const struct ib_device *device, u8 port_num);
+	enum ib_gid_type gid_type;
+} PORT_CAP_TO_GID_TYPE[] = {
+	{rdma_protocol_roce_eth_encap, IB_GID_TYPE_ROCE},
+	{rdma_protocol_roce_udp_encap, IB_GID_TYPE_ROCE_UDP_ENCAP},
+};
+
+#define CAP_TO_GID_TABLE_SIZE	ARRAY_SIZE(PORT_CAP_TO_GID_TYPE)
+
+unsigned long roce_gid_type_mask_support(struct ib_device *ib_dev, u8 port)
+{
+	int i;
+	unsigned int ret_flags = 0;
+
+	if (!rdma_protocol_roce(ib_dev, port))
+		return 1UL << IB_GID_TYPE_IB;
+
+	for (i = 0; i < CAP_TO_GID_TABLE_SIZE; i++)
+		if (PORT_CAP_TO_GID_TYPE[i].is_supported(ib_dev, port))
+			ret_flags |= 1UL << PORT_CAP_TO_GID_TYPE[i].gid_type;
+
+	return ret_flags;
+}
+EXPORT_SYMBOL(roce_gid_type_mask_support);
+
 static void update_gid(enum gid_op_type gid_op, struct ib_device *ib_dev,
 		       u8 port, union ib_gid *gid,
 		       struct ib_gid_attr *gid_attr)
 {
-	switch (gid_op) {
-	case GID_ADD:
-		ib_cache_gid_add(ib_dev, port, gid, gid_attr);
-		break;
-	case GID_DEL:
-		ib_cache_gid_del(ib_dev, port, gid, gid_attr);
-		break;
+	int i;
+	unsigned long gid_type_mask = roce_gid_type_mask_support(ib_dev, port);
+
+	for (i = 0; i < IB_GID_TYPE_SIZE; i++) {
+		if ((1UL << i) & gid_type_mask) {
+			gid_attr->gid_type = i;
+			switch (gid_op) {
+			case GID_ADD:
+				ib_cache_gid_add(ib_dev, port,
+						 gid, gid_attr);
+				break;
+			case GID_DEL:
+				ib_cache_gid_del(ib_dev, port,
+						 gid, gid_attr);
+				break;
+			}
+		}
 	}
 }
 
@@ -103,18 +139,6 @@ static enum bonding_slave_state is_eth_active_slave_of_bonding_rcu(struct net_de
 	return BONDING_SLAVE_STATE_NA;
 }
 
-static bool is_upper_dev_rcu(struct net_device *dev, struct net_device *upper)
-{
-	struct net_device *_upper = NULL;
-	struct list_head *iter;
-
-	netdev_for_each_all_upper_dev_rcu(dev, _upper, iter)
-		if (_upper == upper)
-			break;
-
-	return _upper == upper;
-}
-
 #define REQUIRED_BOND_STATES		(BONDING_SLAVE_STATE_ACTIVE |	\
 					 BONDING_SLAVE_STATE_NA)
 static int is_eth_port_of_netdev(struct ib_device *ib_dev, u8 port,
@@ -132,7 +156,7 @@ static int is_eth_port_of_netdev(struct ib_device *ib_dev, u8 port,
 	if (!real_dev)
 		real_dev = event_ndev;
 
-	res = ((is_upper_dev_rcu(rdma_ndev, event_ndev) &&
+	res = ((rdma_is_upper_dev_rcu(rdma_ndev, event_ndev) &&
 	       (is_eth_active_slave_of_bonding_rcu(rdma_ndev, real_dev) &
 		REQUIRED_BOND_STATES)) ||
 	       real_dev == rdma_ndev);
@@ -178,7 +202,7 @@ static int upper_device_filter(struct ib_device *ib_dev, u8 port,
 		return 1;
 
 	rcu_read_lock();
-	res = is_upper_dev_rcu(rdma_ndev, event_ndev);
+	res = rdma_is_upper_dev_rcu(rdma_ndev, event_ndev);
 	rcu_read_unlock();
 
 	return res;
@@ -203,10 +227,12 @@ static void enum_netdev_default_gids(struct ib_device *ib_dev,
 				     u8 port, struct net_device *event_ndev,
 				     struct net_device *rdma_ndev)
 {
+	unsigned long gid_type_mask;
+
 	rcu_read_lock();
 	if (!rdma_ndev ||
 	    ((rdma_ndev != event_ndev &&
-	      !is_upper_dev_rcu(rdma_ndev, event_ndev)) ||
+	      !rdma_is_upper_dev_rcu(rdma_ndev, event_ndev)) ||
 	     is_eth_active_slave_of_bonding_rcu(rdma_ndev,
 						netdev_master_upper_dev_get_rcu(rdma_ndev)) ==
 	     BONDING_SLAVE_STATE_INACTIVE)) {
@@ -215,7 +241,9 @@ static void enum_netdev_default_gids(struct ib_device *ib_dev,
 	}
 	rcu_read_unlock();
 
-	ib_cache_gid_set_default_gid(ib_dev, port, rdma_ndev,
+	gid_type_mask = roce_gid_type_mask_support(ib_dev, port);
+
+	ib_cache_gid_set_default_gid(ib_dev, port, rdma_ndev, gid_type_mask,
 				     IB_CACHE_GID_DEFAULT_MODE_SET);
 }
 
@@ -234,12 +262,17 @@ static void bond_delete_netdev_default_gids(struct ib_device *ib_dev,
 
 	rcu_read_lock();
 
-	if (is_upper_dev_rcu(rdma_ndev, event_ndev) &&
+	if (rdma_is_upper_dev_rcu(rdma_ndev, event_ndev) &&
 	    is_eth_active_slave_of_bonding_rcu(rdma_ndev, real_dev) ==
 	    BONDING_SLAVE_STATE_INACTIVE) {
+		unsigned long gid_type_mask;
+
 		rcu_read_unlock();
 
+		gid_type_mask = roce_gid_type_mask_support(ib_dev, port);
+
 		ib_cache_gid_set_default_gid(ib_dev, port, rdma_ndev,
+					     gid_type_mask,
 					     IB_CACHE_GID_DEFAULT_MODE_DELETE);
 	} else {
 		rcu_read_unlock();
diff --git a/drivers/infiniband/core/sa_query.c b/drivers/infiniband/core/sa_query.c
index a95a32ba596e..1e37f3515d98 100644
--- a/drivers/infiniband/core/sa_query.c
+++ b/drivers/infiniband/core/sa_query.c
@@ -49,7 +49,9 @@
 #include <net/netlink.h>
 #include <uapi/rdma/ib_user_sa.h>
 #include <rdma/ib_marshall.h>
+#include <rdma/ib_addr.h>
 #include "sa.h"
+#include "core_priv.h"
 
 MODULE_AUTHOR("Roland Dreier");
 MODULE_DESCRIPTION("InfiniBand subnet administration query support");
@@ -715,7 +717,9 @@ static int ib_nl_handle_set_timeout(struct sk_buff *skb,
 	struct nlattr *tb[LS_NLA_TYPE_MAX];
 	int ret;
 
-	if (!netlink_capable(skb, CAP_NET_ADMIN))
+	if (!(nlh->nlmsg_flags & NLM_F_REQUEST) ||
+	    !(NETLINK_CB(skb).sk) ||
+	    !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	ret = nla_parse(tb, LS_NLA_TYPE_MAX - 1, nlmsg_data(nlh),
@@ -789,7 +793,9 @@ static int ib_nl_handle_resolve_resp(struct sk_buff *skb,
 	int found = 0;
 	int ret;
 
-	if (!netlink_capable(skb, CAP_NET_ADMIN))
+	if ((nlh->nlmsg_flags & NLM_F_REQUEST) ||
+	    !(NETLINK_CB(skb).sk) ||
+	    !netlink_capable(skb, CAP_NET_ADMIN))
 		return -EPERM;
 
 	spin_lock_irqsave(&ib_nl_request_lock, flags);
@@ -996,7 +1002,8 @@ int ib_init_ah_from_path(struct ib_device *device, u8 port_num,
 {
 	int ret;
 	u16 gid_index;
-	int force_grh;
+	int use_roce;
+	struct net_device *ndev = NULL;
 
 	memset(ah_attr, 0, sizeof *ah_attr);
 	ah_attr->dlid = be16_to_cpu(rec->dlid);
@@ -1006,16 +1013,71 @@ int ib_init_ah_from_path(struct ib_device *device, u8 port_num,
 	ah_attr->port_num = port_num;
 	ah_attr->static_rate = rec->rate;
 
-	force_grh = rdma_cap_eth_ah(device, port_num);
+	use_roce = rdma_cap_eth_ah(device, port_num);
+
+	if (use_roce) {
+		struct net_device *idev;
+		struct net_device *resolved_dev;
+		struct rdma_dev_addr dev_addr = {.bound_dev_if = rec->ifindex,
+						 .net = rec->net ? rec->net :
+							 &init_net};
+		union {
+			struct sockaddr     _sockaddr;
+			struct sockaddr_in  _sockaddr_in;
+			struct sockaddr_in6 _sockaddr_in6;
+		} sgid_addr, dgid_addr;
+
+		if (!device->get_netdev)
+			return -EOPNOTSUPP;
+
+		rdma_gid2ip(&sgid_addr._sockaddr, &rec->sgid);
+		rdma_gid2ip(&dgid_addr._sockaddr, &rec->dgid);
+
+		/* validate the route */
+		ret = rdma_resolve_ip_route(&sgid_addr._sockaddr,
+					    &dgid_addr._sockaddr, &dev_addr);
+		if (ret)
+			return ret;
 
-	if (rec->hop_limit > 1 || force_grh) {
-		struct net_device *ndev = ib_get_ndev_from_path(rec);
+		if ((dev_addr.network == RDMA_NETWORK_IPV4 ||
+		     dev_addr.network == RDMA_NETWORK_IPV6) &&
+		    rec->gid_type != IB_GID_TYPE_ROCE_UDP_ENCAP)
+			return -EINVAL;
+
+		idev = device->get_netdev(device, port_num);
+		if (!idev)
+			return -ENODEV;
+
+		resolved_dev = dev_get_by_index(dev_addr.net,
+						dev_addr.bound_dev_if);
+		if (resolved_dev->flags & IFF_LOOPBACK) {
+			dev_put(resolved_dev);
+			resolved_dev = idev;
+			dev_hold(resolved_dev);
+		}
+		ndev = ib_get_ndev_from_path(rec);
+		rcu_read_lock();
+		if ((ndev && ndev != resolved_dev) ||
+		    (resolved_dev != idev &&
+		     !rdma_is_upper_dev_rcu(idev, resolved_dev)))
+			ret = -EHOSTUNREACH;
+		rcu_read_unlock();
+		dev_put(idev);
+		dev_put(resolved_dev);
+		if (ret) {
+			if (ndev)
+				dev_put(ndev);
+			return ret;
+		}
+	}
 
+	if (rec->hop_limit > 0 || use_roce) {
 		ah_attr->ah_flags = IB_AH_GRH;
 		ah_attr->grh.dgid = rec->dgid;
 
-		ret = ib_find_cached_gid(device, &rec->sgid, ndev, &port_num,
-					 &gid_index);
+		ret = ib_find_cached_gid_by_port(device, &rec->sgid,
+						 rec->gid_type, port_num, ndev,
+						 &gid_index);
 		if (ret) {
 			if (ndev)
 				dev_put(ndev);
@@ -1029,9 +1091,10 @@ int ib_init_ah_from_path(struct ib_device *device, u8 port_num,
 		if (ndev)
 			dev_put(ndev);
 	}
-	if (force_grh) {
+
+	if (use_roce)
 		memcpy(ah_attr->dmac, rec->dmac, ETH_ALEN);
-	}
+
 	return 0;
 }
 EXPORT_SYMBOL(ib_init_ah_from_path);
@@ -1157,6 +1220,7 @@ static void ib_sa_path_rec_callback(struct ib_sa_query *sa_query,
 			  mad->data, &rec);
 		rec.net = NULL;
 		rec.ifindex = 0;
+		rec.gid_type = IB_GID_TYPE_IB;
 		memset(rec.dmac, 0, ETH_ALEN);
 		query->callback(status, &rec, query->context);
 	} else
@@ -1609,14 +1673,15 @@ static void send_handler(struct ib_mad_agent *agent,
 }
 
 static void recv_handler(struct ib_mad_agent *mad_agent,
+			 struct ib_mad_send_buf *send_buf,
 			 struct ib_mad_recv_wc *mad_recv_wc)
 {
 	struct ib_sa_query *query;
-	struct ib_mad_send_buf *mad_buf;
 
-	mad_buf = (void *) (unsigned long) mad_recv_wc->wc->wr_id;
-	query = mad_buf->context[0];
+	if (!send_buf)
+		return;
 
+	query = send_buf->context[0];
 	if (query->callback) {
 		if (mad_recv_wc->wc->status == IB_WC_SUCCESS)
 			query->callback(query,
diff --git a/drivers/infiniband/core/sysfs.c b/drivers/infiniband/core/sysfs.c
index b1f37d4095fa..14606afbfaa8 100644
--- a/drivers/infiniband/core/sysfs.c
+++ b/drivers/infiniband/core/sysfs.c
@@ -37,15 +37,27 @@
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/string.h>
+#include <linux/netdevice.h>
 
 #include <rdma/ib_mad.h>
+#include <rdma/ib_pma.h>
 
+struct ib_port;
+
+struct gid_attr_group {
+	struct ib_port		*port;
+	struct kobject		kobj;
+	struct attribute_group	ndev;
+	struct attribute_group	type;
+};
 struct ib_port {
 	struct kobject         kobj;
 	struct ib_device      *ibdev;
+	struct gid_attr_group *gid_attr_group;
 	struct attribute_group gid_group;
 	struct attribute_group pkey_group;
 	u8                     port_num;
+	struct attribute_group *pma_table;
 };
 
 struct port_attribute {
@@ -65,6 +77,7 @@ struct port_table_attribute {
 	struct port_attribute	attr;
 	char			name[8];
 	int			index;
+	__be16			attr_id;
 };
 
 static ssize_t port_attr_show(struct kobject *kobj,
@@ -84,6 +97,24 @@ static const struct sysfs_ops port_sysfs_ops = {
 	.show = port_attr_show
 };
 
+static ssize_t gid_attr_show(struct kobject *kobj,
+			     struct attribute *attr, char *buf)
+{
+	struct port_attribute *port_attr =
+		container_of(attr, struct port_attribute, attr);
+	struct ib_port *p = container_of(kobj, struct gid_attr_group,
+					 kobj)->port;
+
+	if (!port_attr->show)
+		return -EIO;
+
+	return port_attr->show(p, port_attr, buf);
+}
+
+static const struct sysfs_ops gid_attr_sysfs_ops = {
+	.show = gid_attr_show
+};
+
 static ssize_t state_show(struct ib_port *p, struct port_attribute *unused,
 			  char *buf)
 {
@@ -281,6 +312,44 @@ static struct attribute *port_default_attrs[] = {
 	NULL
 };
 
+static size_t print_ndev(struct ib_gid_attr *gid_attr, char *buf)
+{
+	if (!gid_attr->ndev)
+		return -EINVAL;
+
+	return sprintf(buf, "%s\n", gid_attr->ndev->name);
+}
+
+static size_t print_gid_type(struct ib_gid_attr *gid_attr, char *buf)
+{
+	return sprintf(buf, "%s\n", ib_cache_gid_type_str(gid_attr->gid_type));
+}
+
+static ssize_t _show_port_gid_attr(struct ib_port *p,
+				   struct port_attribute *attr,
+				   char *buf,
+				   size_t (*print)(struct ib_gid_attr *gid_attr,
+						   char *buf))
+{
+	struct port_table_attribute *tab_attr =
+		container_of(attr, struct port_table_attribute, attr);
+	union ib_gid gid;
+	struct ib_gid_attr gid_attr = {};
+	ssize_t ret;
+
+	ret = ib_query_gid(p->ibdev, p->port_num, tab_attr->index, &gid,
+			   &gid_attr);
+	if (ret)
+		goto err;
+
+	ret = print(&gid_attr, buf);
+
+err:
+	if (gid_attr.ndev)
+		dev_put(gid_attr.ndev);
+	return ret;
+}
+
 static ssize_t show_port_gid(struct ib_port *p, struct port_attribute *attr,
 			     char *buf)
 {
@@ -296,6 +365,19 @@ static ssize_t show_port_gid(struct ib_port *p, struct port_attribute *attr,
 	return sprintf(buf, "%pI6\n", gid.raw);
 }
 
+static ssize_t show_port_gid_attr_ndev(struct ib_port *p,
+				       struct port_attribute *attr, char *buf)
+{
+	return _show_port_gid_attr(p, attr, buf, print_ndev);
+}
+
+static ssize_t show_port_gid_attr_gid_type(struct ib_port *p,
+					   struct port_attribute *attr,
+					   char *buf)
+{
+	return _show_port_gid_attr(p, attr, buf, print_gid_type);
+}
+
 static ssize_t show_port_pkey(struct ib_port *p, struct port_attribute *attr,
 			      char *buf)
 {
@@ -314,24 +396,32 @@ static ssize_t show_port_pkey(struct ib_port *p, struct port_attribute *attr,
 #define PORT_PMA_ATTR(_name, _counter, _width, _offset)			\
 struct port_table_attribute port_pma_attr_##_name = {			\
 	.attr  = __ATTR(_name, S_IRUGO, show_pma_counter, NULL),	\
-	.index = (_offset) | ((_width) << 16) | ((_counter) << 24)	\
+	.index = (_offset) | ((_width) << 16) | ((_counter) << 24),	\
+	.attr_id = IB_PMA_PORT_COUNTERS ,				\
 }
 
-static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
-				char *buf)
+#define PORT_PMA_ATTR_EXT(_name, _width, _offset)			\
+struct port_table_attribute port_pma_attr_ext_##_name = {		\
+	.attr  = __ATTR(_name, S_IRUGO, show_pma_counter, NULL),	\
+	.index = (_offset) | ((_width) << 16),				\
+	.attr_id = IB_PMA_PORT_COUNTERS_EXT ,				\
+}
+
+/*
+ * Get a Perfmgmt MAD block of data.
+ * Returns error code or the number of bytes retrieved.
+ */
+static int get_perf_mad(struct ib_device *dev, int port_num, __be16 attr,
+		void *data, int offset, size_t size)
 {
-	struct port_table_attribute *tab_attr =
-		container_of(attr, struct port_table_attribute, attr);
-	int offset = tab_attr->index & 0xffff;
-	int width  = (tab_attr->index >> 16) & 0xff;
-	struct ib_mad *in_mad  = NULL;
-	struct ib_mad *out_mad = NULL;
+	struct ib_mad *in_mad;
+	struct ib_mad *out_mad;
 	size_t mad_size = sizeof(*out_mad);
 	u16 out_mad_pkey_index = 0;
 	ssize_t ret;
 
-	if (!p->ibdev->process_mad)
-		return sprintf(buf, "N/A (no PMA)\n");
+	if (!dev->process_mad)
+		return -ENOSYS;
 
 	in_mad  = kzalloc(sizeof *in_mad, GFP_KERNEL);
 	out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
@@ -344,12 +434,13 @@ static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
 	in_mad->mad_hdr.mgmt_class    = IB_MGMT_CLASS_PERF_MGMT;
 	in_mad->mad_hdr.class_version = 1;
 	in_mad->mad_hdr.method        = IB_MGMT_METHOD_GET;
-	in_mad->mad_hdr.attr_id       = cpu_to_be16(0x12); /* PortCounters */
+	in_mad->mad_hdr.attr_id       = attr;
 
-	in_mad->data[41] = p->port_num;	/* PortSelect field */
+	if (attr != IB_PMA_CLASS_PORT_INFO)
+		in_mad->data[41] = port_num;	/* PortSelect field */
 
-	if ((p->ibdev->process_mad(p->ibdev, IB_MAD_IGNORE_MKEY,
-		 p->port_num, NULL, NULL,
+	if ((dev->process_mad(dev, IB_MAD_IGNORE_MKEY,
+		 port_num, NULL, NULL,
 		 (const struct ib_mad_hdr *)in_mad, mad_size,
 		 (struct ib_mad_hdr *)out_mad, &mad_size,
 		 &out_mad_pkey_index) &
@@ -358,31 +449,54 @@ static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
 		ret = -EINVAL;
 		goto out;
 	}
+	memcpy(data, out_mad->data + offset, size);
+	ret = size;
+out:
+	kfree(in_mad);
+	kfree(out_mad);
+	return ret;
+}
+
+static ssize_t show_pma_counter(struct ib_port *p, struct port_attribute *attr,
+				char *buf)
+{
+	struct port_table_attribute *tab_attr =
+		container_of(attr, struct port_table_attribute, attr);
+	int offset = tab_attr->index & 0xffff;
+	int width  = (tab_attr->index >> 16) & 0xff;
+	ssize_t ret;
+	u8 data[8];
+
+	ret = get_perf_mad(p->ibdev, p->port_num, tab_attr->attr_id, &data,
+			40 + offset / 8, sizeof(data));
+	if (ret < 0)
+		return sprintf(buf, "N/A (no PMA)\n");
 
 	switch (width) {
 	case 4:
-		ret = sprintf(buf, "%u\n", (out_mad->data[40 + offset / 8] >>
+		ret = sprintf(buf, "%u\n", (*data >>
 					    (4 - (offset % 8))) & 0xf);
 		break;
 	case 8:
-		ret = sprintf(buf, "%u\n", out_mad->data[40 + offset / 8]);
+		ret = sprintf(buf, "%u\n", *data);
 		break;
 	case 16:
 		ret = sprintf(buf, "%u\n",
-			      be16_to_cpup((__be16 *)(out_mad->data + 40 + offset / 8)));
+			      be16_to_cpup((__be16 *)data));
 		break;
 	case 32:
 		ret = sprintf(buf, "%u\n",
-			      be32_to_cpup((__be32 *)(out_mad->data + 40 + offset / 8)));
+			      be32_to_cpup((__be32 *)data));
+		break;
+	case 64:
+		ret = sprintf(buf, "%llu\n",
+				be64_to_cpup((__be64 *)data));
 		break;
+
 	default:
 		ret = 0;
 	}
 
-out:
-	kfree(in_mad);
-	kfree(out_mad);
-
 	return ret;
 }
 
@@ -403,6 +517,18 @@ static PORT_PMA_ATTR(port_rcv_data		    , 13, 32, 224);
 static PORT_PMA_ATTR(port_xmit_packets		    , 14, 32, 256);
 static PORT_PMA_ATTR(port_rcv_packets		    , 15, 32, 288);
 
+/*
+ * Counters added by extended set
+ */
+static PORT_PMA_ATTR_EXT(port_xmit_data		    , 64,  64);
+static PORT_PMA_ATTR_EXT(port_rcv_data		    , 64, 128);
+static PORT_PMA_ATTR_EXT(port_xmit_packets	    , 64, 192);
+static PORT_PMA_ATTR_EXT(port_rcv_packets	    , 64, 256);
+static PORT_PMA_ATTR_EXT(unicast_xmit_packets	    , 64, 320);
+static PORT_PMA_ATTR_EXT(unicast_rcv_packets	    , 64, 384);
+static PORT_PMA_ATTR_EXT(multicast_xmit_packets	    , 64, 448);
+static PORT_PMA_ATTR_EXT(multicast_rcv_packets	    , 64, 512);
+
 static struct attribute *pma_attrs[] = {
 	&port_pma_attr_symbol_error.attr.attr,
 	&port_pma_attr_link_error_recovery.attr.attr,
@@ -423,11 +549,65 @@ static struct attribute *pma_attrs[] = {
 	NULL
 };
 
+static struct attribute *pma_attrs_ext[] = {
+	&port_pma_attr_symbol_error.attr.attr,
+	&port_pma_attr_link_error_recovery.attr.attr,
+	&port_pma_attr_link_downed.attr.attr,
+	&port_pma_attr_port_rcv_errors.attr.attr,
+	&port_pma_attr_port_rcv_remote_physical_errors.attr.attr,
+	&port_pma_attr_port_rcv_switch_relay_errors.attr.attr,
+	&port_pma_attr_port_xmit_discards.attr.attr,
+	&port_pma_attr_port_xmit_constraint_errors.attr.attr,
+	&port_pma_attr_port_rcv_constraint_errors.attr.attr,
+	&port_pma_attr_local_link_integrity_errors.attr.attr,
+	&port_pma_attr_excessive_buffer_overrun_errors.attr.attr,
+	&port_pma_attr_VL15_dropped.attr.attr,
+	&port_pma_attr_ext_port_xmit_data.attr.attr,
+	&port_pma_attr_ext_port_rcv_data.attr.attr,
+	&port_pma_attr_ext_port_xmit_packets.attr.attr,
+	&port_pma_attr_ext_port_rcv_packets.attr.attr,
+	&port_pma_attr_ext_unicast_rcv_packets.attr.attr,
+	&port_pma_attr_ext_unicast_xmit_packets.attr.attr,
+	&port_pma_attr_ext_multicast_rcv_packets.attr.attr,
+	&port_pma_attr_ext_multicast_xmit_packets.attr.attr,
+	NULL
+};
+
+static struct attribute *pma_attrs_noietf[] = {
+	&port_pma_attr_symbol_error.attr.attr,
+	&port_pma_attr_link_error_recovery.attr.attr,
+	&port_pma_attr_link_downed.attr.attr,
+	&port_pma_attr_port_rcv_errors.attr.attr,
+	&port_pma_attr_port_rcv_remote_physical_errors.attr.attr,
+	&port_pma_attr_port_rcv_switch_relay_errors.attr.attr,
+	&port_pma_attr_port_xmit_discards.attr.attr,
+	&port_pma_attr_port_xmit_constraint_errors.attr.attr,
+	&port_pma_attr_port_rcv_constraint_errors.attr.attr,
+	&port_pma_attr_local_link_integrity_errors.attr.attr,
+	&port_pma_attr_excessive_buffer_overrun_errors.attr.attr,
+	&port_pma_attr_VL15_dropped.attr.attr,
+	&port_pma_attr_ext_port_xmit_data.attr.attr,
+	&port_pma_attr_ext_port_rcv_data.attr.attr,
+	&port_pma_attr_ext_port_xmit_packets.attr.attr,
+	&port_pma_attr_ext_port_rcv_packets.attr.attr,
+	NULL
+};
+
 static struct attribute_group pma_group = {
 	.name  = "counters",
 	.attrs  = pma_attrs
 };
 
+static struct attribute_group pma_group_ext = {
+	.name  = "counters",
+	.attrs  = pma_attrs_ext
+};
+
+static struct attribute_group pma_group_noietf = {
+	.name  = "counters",
+	.attrs  = pma_attrs_noietf
+};
+
 static void ib_port_release(struct kobject *kobj)
 {
 	struct ib_port *p = container_of(kobj, struct ib_port, kobj);
@@ -451,12 +631,41 @@ static void ib_port_release(struct kobject *kobj)
 	kfree(p);
 }
 
+static void ib_port_gid_attr_release(struct kobject *kobj)
+{
+	struct gid_attr_group *g = container_of(kobj, struct gid_attr_group,
+						kobj);
+	struct attribute *a;
+	int i;
+
+	if (g->ndev.attrs) {
+		for (i = 0; (a = g->ndev.attrs[i]); ++i)
+			kfree(a);
+
+		kfree(g->ndev.attrs);
+	}
+
+	if (g->type.attrs) {
+		for (i = 0; (a = g->type.attrs[i]); ++i)
+			kfree(a);
+
+		kfree(g->type.attrs);
+	}
+
+	kfree(g);
+}
+
 static struct kobj_type port_type = {
 	.release       = ib_port_release,
 	.sysfs_ops     = &port_sysfs_ops,
 	.default_attrs = port_default_attrs
 };
 
+static struct kobj_type gid_attr_type = {
+	.sysfs_ops      = &gid_attr_sysfs_ops,
+	.release        = ib_port_gid_attr_release
+};
+
 static struct attribute **
 alloc_group_attrs(ssize_t (*show)(struct ib_port *,
 				  struct port_attribute *, char *buf),
@@ -500,6 +709,30 @@ err:
 	return NULL;
 }
 
+/*
+ * Figure out which counter table to use depending on
+ * the device capabilities.
+ */
+static struct attribute_group *get_counter_table(struct ib_device *dev,
+						 int port_num)
+{
+	struct ib_class_port_info cpi;
+
+	if (get_perf_mad(dev, port_num, IB_PMA_CLASS_PORT_INFO,
+				&cpi, 40, sizeof(cpi)) >= 0) {
+		if (cpi.capability_mask & IB_PMA_CLASS_CAP_EXT_WIDTH)
+			/* We have extended counters */
+			return &pma_group_ext;
+
+		if (cpi.capability_mask & IB_PMA_CLASS_CAP_EXT_WIDTH_NOIETF)
+			/* But not the IETF ones */
+			return &pma_group_noietf;
+	}
+
+	/* Fall back to normal counters */
+	return &pma_group;
+}
+
 static int add_port(struct ib_device *device, int port_num,
 		    int (*port_callback)(struct ib_device *,
 					 u8, struct kobject *))
@@ -528,9 +761,24 @@ static int add_port(struct ib_device *device, int port_num,
 		return ret;
 	}
 
-	ret = sysfs_create_group(&p->kobj, &pma_group);
-	if (ret)
+	p->gid_attr_group = kzalloc(sizeof(*p->gid_attr_group), GFP_KERNEL);
+	if (!p->gid_attr_group) {
+		ret = -ENOMEM;
 		goto err_put;
+	}
+
+	p->gid_attr_group->port = p;
+	ret = kobject_init_and_add(&p->gid_attr_group->kobj, &gid_attr_type,
+				   &p->kobj, "gid_attrs");
+	if (ret) {
+		kfree(p->gid_attr_group);
+		goto err_put;
+	}
+
+	p->pma_table = get_counter_table(device, port_num);
+	ret = sysfs_create_group(&p->kobj, p->pma_table);
+	if (ret)
+		goto err_put_gid_attrs;
 
 	p->gid_group.name  = "gids";
 	p->gid_group.attrs = alloc_group_attrs(show_port_gid, attr.gid_tbl_len);
@@ -543,12 +791,38 @@ static int add_port(struct ib_device *device, int port_num,
 	if (ret)
 		goto err_free_gid;
 
+	p->gid_attr_group->ndev.name = "ndevs";
+	p->gid_attr_group->ndev.attrs = alloc_group_attrs(show_port_gid_attr_ndev,
+							  attr.gid_tbl_len);
+	if (!p->gid_attr_group->ndev.attrs) {
+		ret = -ENOMEM;
+		goto err_remove_gid;
+	}
+
+	ret = sysfs_create_group(&p->gid_attr_group->kobj,
+				 &p->gid_attr_group->ndev);
+	if (ret)
+		goto err_free_gid_ndev;
+
+	p->gid_attr_group->type.name = "types";
+	p->gid_attr_group->type.attrs = alloc_group_attrs(show_port_gid_attr_gid_type,
+							  attr.gid_tbl_len);
+	if (!p->gid_attr_group->type.attrs) {
+		ret = -ENOMEM;
+		goto err_remove_gid_ndev;
+	}
+
+	ret = sysfs_create_group(&p->gid_attr_group->kobj,
+				 &p->gid_attr_group->type);
+	if (ret)
+		goto err_free_gid_type;
+
 	p->pkey_group.name  = "pkeys";
 	p->pkey_group.attrs = alloc_group_attrs(show_port_pkey,
 						attr.pkey_tbl_len);
 	if (!p->pkey_group.attrs) {
 		ret = -ENOMEM;
-		goto err_remove_gid;
+		goto err_remove_gid_type;
 	}
 
 	ret = sysfs_create_group(&p->kobj, &p->pkey_group);
@@ -576,6 +850,28 @@ err_free_pkey:
 	kfree(p->pkey_group.attrs);
 	p->pkey_group.attrs = NULL;
 
+err_remove_gid_type:
+	sysfs_remove_group(&p->gid_attr_group->kobj,
+			   &p->gid_attr_group->type);
+
+err_free_gid_type:
+	for (i = 0; i < attr.gid_tbl_len; ++i)
+		kfree(p->gid_attr_group->type.attrs[i]);
+
+	kfree(p->gid_attr_group->type.attrs);
+	p->gid_attr_group->type.attrs = NULL;
+
+err_remove_gid_ndev:
+	sysfs_remove_group(&p->gid_attr_group->kobj,
+			   &p->gid_attr_group->ndev);
+
+err_free_gid_ndev:
+	for (i = 0; i < attr.gid_tbl_len; ++i)
+		kfree(p->gid_attr_group->ndev.attrs[i]);
+
+	kfree(p->gid_attr_group->ndev.attrs);
+	p->gid_attr_group->ndev.attrs = NULL;
+
 err_remove_gid:
 	sysfs_remove_group(&p->kobj, &p->gid_group);
 
@@ -587,7 +883,10 @@ err_free_gid:
 	p->gid_group.attrs = NULL;
 
 err_remove_pma:
-	sysfs_remove_group(&p->kobj, &pma_group);
+	sysfs_remove_group(&p->kobj, p->pma_table);
+
+err_put_gid_attrs:
+	kobject_put(&p->gid_attr_group->kobj);
 
 err_put:
 	kobject_put(&p->kobj);
@@ -614,18 +913,12 @@ static ssize_t show_sys_image_guid(struct device *device,
 				   struct device_attribute *dev_attr, char *buf)
 {
 	struct ib_device *dev = container_of(device, struct ib_device, dev);
-	struct ib_device_attr attr;
-	ssize_t ret;
-
-	ret = ib_query_device(dev, &attr);
-	if (ret)
-		return ret;
 
 	return sprintf(buf, "%04x:%04x:%04x:%04x\n",
-		       be16_to_cpu(((__be16 *) &attr.sys_image_guid)[0]),
-		       be16_to_cpu(((__be16 *) &attr.sys_image_guid)[1]),
-		       be16_to_cpu(((__be16 *) &attr.sys_image_guid)[2]),
-		       be16_to_cpu(((__be16 *) &attr.sys_image_guid)[3]));
+		       be16_to_cpu(((__be16 *) &dev->attrs.sys_image_guid)[0]),
+		       be16_to_cpu(((__be16 *) &dev->attrs.sys_image_guid)[1]),
+		       be16_to_cpu(((__be16 *) &dev->attrs.sys_image_guid)[2]),
+		       be16_to_cpu(((__be16 *) &dev->attrs.sys_image_guid)[3]));
 }
 
 static ssize_t show_node_guid(struct device *device,
@@ -800,9 +1093,14 @@ static void free_port_list_attributes(struct ib_device *device)
 	list_for_each_entry_safe(p, t, &device->port_list, entry) {
 		struct ib_port *port = container_of(p, struct ib_port, kobj);
 		list_del(&p->entry);
-		sysfs_remove_group(p, &pma_group);
+		sysfs_remove_group(p, port->pma_table);
 		sysfs_remove_group(p, &port->pkey_group);
 		sysfs_remove_group(p, &port->gid_group);
+		sysfs_remove_group(&port->gid_attr_group->kobj,
+				   &port->gid_attr_group->ndev);
+		sysfs_remove_group(&port->gid_attr_group->kobj,
+				   &port->gid_attr_group->type);
+		kobject_put(&port->gid_attr_group->kobj);
 		kobject_put(p);
 	}
 
diff --git a/drivers/infiniband/core/ud_header.c b/drivers/infiniband/core/ud_header.c
index 72feee620ebf..2116132568e7 100644
--- a/drivers/infiniband/core/ud_header.c
+++ b/drivers/infiniband/core/ud_header.c
@@ -35,6 +35,7 @@
 #include <linux/string.h>
 #include <linux/export.h>
 #include <linux/if_ether.h>
+#include <linux/ip.h>
 
 #include <rdma/ib_pack.h>
 
@@ -116,6 +117,72 @@ static const struct ib_field vlan_table[]  = {
 	  .size_bits    = 16 }
 };
 
+static const struct ib_field ip4_table[]  = {
+	{ STRUCT_FIELD(ip4, ver),
+	  .offset_words = 0,
+	  .offset_bits  = 0,
+	  .size_bits    = 4 },
+	{ STRUCT_FIELD(ip4, hdr_len),
+	  .offset_words = 0,
+	  .offset_bits  = 4,
+	  .size_bits    = 4 },
+	{ STRUCT_FIELD(ip4, tos),
+	  .offset_words = 0,
+	  .offset_bits  = 8,
+	  .size_bits    = 8 },
+	{ STRUCT_FIELD(ip4, tot_len),
+	  .offset_words = 0,
+	  .offset_bits  = 16,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(ip4, id),
+	  .offset_words = 1,
+	  .offset_bits  = 0,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(ip4, frag_off),
+	  .offset_words = 1,
+	  .offset_bits  = 16,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(ip4, ttl),
+	  .offset_words = 2,
+	  .offset_bits  = 0,
+	  .size_bits    = 8 },
+	{ STRUCT_FIELD(ip4, protocol),
+	  .offset_words = 2,
+	  .offset_bits  = 8,
+	  .size_bits    = 8 },
+	{ STRUCT_FIELD(ip4, check),
+	  .offset_words = 2,
+	  .offset_bits  = 16,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(ip4, saddr),
+	  .offset_words = 3,
+	  .offset_bits  = 0,
+	  .size_bits    = 32 },
+	{ STRUCT_FIELD(ip4, daddr),
+	  .offset_words = 4,
+	  .offset_bits  = 0,
+	  .size_bits    = 32 }
+};
+
+static const struct ib_field udp_table[]  = {
+	{ STRUCT_FIELD(udp, sport),
+	  .offset_words = 0,
+	  .offset_bits  = 0,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(udp, dport),
+	  .offset_words = 0,
+	  .offset_bits  = 16,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(udp, length),
+	  .offset_words = 1,
+	  .offset_bits  = 0,
+	  .size_bits    = 16 },
+	{ STRUCT_FIELD(udp, csum),
+	  .offset_words = 1,
+	  .offset_bits  = 16,
+	  .size_bits    = 16 }
+};
+
 static const struct ib_field grh_table[]  = {
 	{ STRUCT_FIELD(grh, ip_version),
 	  .offset_words = 0,
@@ -213,26 +280,59 @@ static const struct ib_field deth_table[] = {
 	  .size_bits    = 24 }
 };
 
+__sum16 ib_ud_ip4_csum(struct ib_ud_header *header)
+{
+	struct iphdr iph;
+
+	iph.ihl		= 5;
+	iph.version	= 4;
+	iph.tos		= header->ip4.tos;
+	iph.tot_len	= header->ip4.tot_len;
+	iph.id		= header->ip4.id;
+	iph.frag_off	= header->ip4.frag_off;
+	iph.ttl		= header->ip4.ttl;
+	iph.protocol	= header->ip4.protocol;
+	iph.check	= 0;
+	iph.saddr	= header->ip4.saddr;
+	iph.daddr	= header->ip4.daddr;
+
+	return ip_fast_csum((u8 *)&iph, iph.ihl);
+}
+EXPORT_SYMBOL(ib_ud_ip4_csum);
+
 /**
  * ib_ud_header_init - Initialize UD header structure
  * @payload_bytes:Length of packet payload
  * @lrh_present: specify if LRH is present
  * @eth_present: specify if Eth header is present
  * @vlan_present: packet is tagged vlan
- * @grh_present:GRH flag (if non-zero, GRH will be included)
+ * @grh_present: GRH flag (if non-zero, GRH will be included)
+ * @ip_version: if non-zero, IP header, V4 or V6, will be included
+ * @udp_present :if non-zero, UDP header will be included
  * @immediate_present: specify if immediate data is present
  * @header:Structure to initialize
  */
-void ib_ud_header_init(int     		    payload_bytes,
-		       int		    lrh_present,
-		       int		    eth_present,
-		       int		    vlan_present,
-		       int    		    grh_present,
-		       int		    immediate_present,
-		       struct ib_ud_header *header)
+int ib_ud_header_init(int     payload_bytes,
+		      int    lrh_present,
+		      int    eth_present,
+		      int    vlan_present,
+		      int    grh_present,
+		      int    ip_version,
+		      int    udp_present,
+		      int    immediate_present,
+		      struct ib_ud_header *header)
 {
+	size_t udp_bytes = udp_present ? IB_UDP_BYTES : 0;
+
+	grh_present = grh_present && !ip_version;
 	memset(header, 0, sizeof *header);
 
+	/*
+	 * UDP header without IP header doesn't make sense
+	 */
+	if (udp_present && ip_version != 4 && ip_version != 6)
+		return -EINVAL;
+
 	if (lrh_present) {
 		u16 packet_length;
 
@@ -252,16 +352,37 @@ void ib_ud_header_init(int     		    payload_bytes,
 	if (vlan_present)
 		header->eth.type = cpu_to_be16(ETH_P_8021Q);
 
-	if (grh_present) {
+	if (ip_version == 6 || grh_present) {
 		header->grh.ip_version      = 6;
 		header->grh.payload_length  =
-			cpu_to_be16((IB_BTH_BYTES     +
+			cpu_to_be16((udp_bytes        +
+				     IB_BTH_BYTES     +
 				     IB_DETH_BYTES    +
 				     payload_bytes    +
 				     4                + /* ICRC     */
 				     3) & ~3);          /* round up */
-		header->grh.next_header     = 0x1b;
+		header->grh.next_header     = udp_present ? IPPROTO_UDP : 0x1b;
+	}
+
+	if (ip_version == 4) {
+		header->ip4.ver = 4; /* version 4 */
+		header->ip4.hdr_len = 5; /* 5 words */
+		header->ip4.tot_len =
+			cpu_to_be16(IB_IP4_BYTES   +
+				     udp_bytes     +
+				     IB_BTH_BYTES  +
+				     IB_DETH_BYTES +
+				     payload_bytes +
+				     4);     /* ICRC     */
+		header->ip4.protocol = IPPROTO_UDP;
 	}
+	if (udp_present && ip_version)
+		header->udp.length =
+			cpu_to_be16(IB_UDP_BYTES   +
+				     IB_BTH_BYTES  +
+				     IB_DETH_BYTES +
+				     payload_bytes +
+				     4);     /* ICRC     */
 
 	if (immediate_present)
 		header->bth.opcode           = IB_OPCODE_UD_SEND_ONLY_WITH_IMMEDIATE;
@@ -273,8 +394,11 @@ void ib_ud_header_init(int     		    payload_bytes,
 	header->lrh_present = lrh_present;
 	header->eth_present = eth_present;
 	header->vlan_present = vlan_present;
-	header->grh_present = grh_present;
+	header->grh_present = grh_present || (ip_version == 6);
+	header->ipv4_present = ip_version == 4;
+	header->udp_present = udp_present;
 	header->immediate_present = immediate_present;
+	return 0;
 }
 EXPORT_SYMBOL(ib_ud_header_init);
 
@@ -311,6 +435,16 @@ int ib_ud_header_pack(struct ib_ud_header *header,
 			&header->grh, buf + len);
 		len += IB_GRH_BYTES;
 	}
+	if (header->ipv4_present) {
+		ib_pack(ip4_table, ARRAY_SIZE(ip4_table),
+			&header->ip4, buf + len);
+		len += IB_IP4_BYTES;
+	}
+	if (header->udp_present) {
+		ib_pack(udp_table, ARRAY_SIZE(udp_table),
+			&header->udp, buf + len);
+		len += IB_UDP_BYTES;
+	}
 
 	ib_pack(bth_table, ARRAY_SIZE(bth_table),
 		&header->bth, buf + len);
diff --git a/drivers/infiniband/core/umem_odp.c b/drivers/infiniband/core/umem_odp.c
index 40becdb3196e..e69bf266049d 100644
--- a/drivers/infiniband/core/umem_odp.c
+++ b/drivers/infiniband/core/umem_odp.c
@@ -232,7 +232,7 @@ static void ib_umem_notifier_invalidate_range_end(struct mmu_notifier *mn,
 	ib_ucontext_notifier_end_account(context);
 }
 
-static struct mmu_notifier_ops ib_umem_notifiers = {
+static const struct mmu_notifier_ops ib_umem_notifiers = {
 	.release                    = ib_umem_notifier_release,
 	.invalidate_page            = ib_umem_notifier_invalidate_page,
 	.invalidate_range_start     = ib_umem_notifier_invalidate_range_start,
diff --git a/drivers/infiniband/core/user_mad.c b/drivers/infiniband/core/user_mad.c
index 57f281f8d686..415a3185cde7 100644
--- a/drivers/infiniband/core/user_mad.c
+++ b/drivers/infiniband/core/user_mad.c
@@ -210,6 +210,7 @@ static void send_handler(struct ib_mad_agent *agent,
 }
 
 static void recv_handler(struct ib_mad_agent *agent,
+			 struct ib_mad_send_buf *send_buf,
 			 struct ib_mad_recv_wc *mad_recv_wc)
 {
 	struct ib_umad_file *file = agent->context;
diff --git a/drivers/infiniband/core/uverbs.h b/drivers/infiniband/core/uverbs.h
index 94bbd8c155fc..612ccfd39bf9 100644
--- a/drivers/infiniband/core/uverbs.h
+++ b/drivers/infiniband/core/uverbs.h
@@ -204,6 +204,8 @@ void ib_uverbs_event_handler(struct ib_event_handler *handler,
 			     struct ib_event *event);
 void ib_uverbs_dealloc_xrcd(struct ib_uverbs_device *dev, struct ib_xrcd *xrcd);
 
+int uverbs_dealloc_mw(struct ib_mw *mw);
+
 struct ib_uverbs_flow_spec {
 	union {
 		union {
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index 1c02deab068f..6c6fbff19752 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -291,9 +291,6 @@ ssize_t ib_uverbs_get_context(struct ib_uverbs_file *file,
 	struct ib_uverbs_get_context      cmd;
 	struct ib_uverbs_get_context_resp resp;
 	struct ib_udata                   udata;
-#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
-	struct ib_device_attr		  dev_attr;
-#endif
 	struct ib_ucontext		 *ucontext;
 	struct file			 *filp;
 	int ret;
@@ -342,10 +339,7 @@ ssize_t ib_uverbs_get_context(struct ib_uverbs_file *file,
 	ucontext->odp_mrs_count = 0;
 	INIT_LIST_HEAD(&ucontext->no_private_counters);
 
-	ret = ib_query_device(ib_dev, &dev_attr);
-	if (ret)
-		goto err_free;
-	if (!(dev_attr.device_cap_flags & IB_DEVICE_ON_DEMAND_PAGING))
+	if (!(ib_dev->attrs.device_cap_flags & IB_DEVICE_ON_DEMAND_PAGING))
 		ucontext->invalidate_range = NULL;
 
 #endif
@@ -447,8 +441,6 @@ ssize_t ib_uverbs_query_device(struct ib_uverbs_file *file,
 {
 	struct ib_uverbs_query_device      cmd;
 	struct ib_uverbs_query_device_resp resp;
-	struct ib_device_attr              attr;
-	int                                ret;
 
 	if (out_len < sizeof resp)
 		return -ENOSPC;
@@ -456,12 +448,8 @@ ssize_t ib_uverbs_query_device(struct ib_uverbs_file *file,
 	if (copy_from_user(&cmd, buf, sizeof cmd))
 		return -EFAULT;
 
-	ret = ib_query_device(ib_dev, &attr);
-	if (ret)
-		return ret;
-
 	memset(&resp, 0, sizeof resp);
-	copy_query_dev_fields(file, ib_dev, &resp, &attr);
+	copy_query_dev_fields(file, ib_dev, &resp, &ib_dev->attrs);
 
 	if (copy_to_user((void __user *) (unsigned long) cmd.response,
 			 &resp, sizeof resp))
@@ -986,11 +974,8 @@ ssize_t ib_uverbs_reg_mr(struct ib_uverbs_file *file,
 	}
 
 	if (cmd.access_flags & IB_ACCESS_ON_DEMAND) {
-		struct ib_device_attr attr;
-
-		ret = ib_query_device(pd->device, &attr);
-		if (ret || !(attr.device_cap_flags &
-				IB_DEVICE_ON_DEMAND_PAGING)) {
+		if (!(pd->device->attrs.device_cap_flags &
+		      IB_DEVICE_ON_DEMAND_PAGING)) {
 			pr_debug("ODP support not available\n");
 			ret = -EINVAL;
 			goto err_put;
@@ -1008,7 +993,6 @@ ssize_t ib_uverbs_reg_mr(struct ib_uverbs_file *file,
 	mr->pd      = pd;
 	mr->uobject = uobj;
 	atomic_inc(&pd->usecnt);
-	atomic_set(&mr->usecnt, 0);
 
 	uobj->object = mr;
 	ret = idr_add_uobj(&ib_uverbs_mr_idr, uobj);
@@ -1106,11 +1090,6 @@ ssize_t ib_uverbs_rereg_mr(struct ib_uverbs_file *file,
 		}
 	}
 
-	if (atomic_read(&mr->usecnt)) {
-		ret = -EBUSY;
-		goto put_uobj_pd;
-	}
-
 	old_pd = mr->pd;
 	ret = mr->device->rereg_user_mr(mr, cmd.flags, cmd.start,
 					cmd.length, cmd.hca_va,
@@ -1258,7 +1237,7 @@ err_copy:
 	idr_remove_uobj(&ib_uverbs_mw_idr, uobj);
 
 err_unalloc:
-	ib_dealloc_mw(mw);
+	uverbs_dealloc_mw(mw);
 
 err_put:
 	put_pd_read(pd);
@@ -1287,7 +1266,7 @@ ssize_t ib_uverbs_dealloc_mw(struct ib_uverbs_file *file,
 
 	mw = uobj->object;
 
-	ret = ib_dealloc_mw(mw);
+	ret = uverbs_dealloc_mw(mw);
 	if (!ret)
 		uobj->live = 0;
 
@@ -1845,7 +1824,10 @@ static int create_qp(struct ib_uverbs_file *file,
 		      sizeof(cmd->create_flags))
 		attr.create_flags = cmd->create_flags;
 
-	if (attr.create_flags & ~IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK) {
+	if (attr.create_flags & ~(IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK |
+				IB_QP_CREATE_CROSS_CHANNEL |
+				IB_QP_CREATE_MANAGED_SEND |
+				IB_QP_CREATE_MANAGED_RECV)) {
 		ret = -EINVAL;
 		goto err_put;
 	}
@@ -1988,7 +1970,8 @@ ssize_t ib_uverbs_create_qp(struct ib_uverbs_file *file,
 		   resp_size);
 	INIT_UDATA(&uhw, buf + sizeof(cmd),
 		   (unsigned long)cmd.response + resp_size,
-		   in_len - sizeof(cmd), out_len - resp_size);
+		   in_len - sizeof(cmd) - sizeof(struct ib_uverbs_cmd_hdr),
+		   out_len - resp_size);
 
 	memset(&cmd_ex, 0, sizeof(cmd_ex));
 	cmd_ex.user_handle = cmd.user_handle;
@@ -3431,7 +3414,8 @@ ssize_t ib_uverbs_create_srq(struct ib_uverbs_file *file,
 
 	INIT_UDATA(&udata, buf + sizeof cmd,
 		   (unsigned long) cmd.response + sizeof resp,
-		   in_len - sizeof cmd, out_len - sizeof resp);
+		   in_len - sizeof cmd - sizeof(struct ib_uverbs_cmd_hdr),
+		   out_len - sizeof resp);
 
 	ret = __uverbs_create_xsrq(file, ib_dev, &xcmd, &udata);
 	if (ret)
@@ -3457,7 +3441,8 @@ ssize_t ib_uverbs_create_xsrq(struct ib_uverbs_file *file,
 
 	INIT_UDATA(&udata, buf + sizeof cmd,
 		   (unsigned long) cmd.response + sizeof resp,
-		   in_len - sizeof cmd, out_len - sizeof resp);
+		   in_len - sizeof cmd - sizeof(struct ib_uverbs_cmd_hdr),
+		   out_len - sizeof resp);
 
 	ret = __uverbs_create_xsrq(file, ib_dev, &cmd, &udata);
 	if (ret)
diff --git a/drivers/infiniband/core/uverbs_main.c b/drivers/infiniband/core/uverbs_main.c
index e3ef28861be6..39680aed99dd 100644
--- a/drivers/infiniband/core/uverbs_main.c
+++ b/drivers/infiniband/core/uverbs_main.c
@@ -133,6 +133,17 @@ static int (*uverbs_ex_cmd_table[])(struct ib_uverbs_file *file,
 static void ib_uverbs_add_one(struct ib_device *device);
 static void ib_uverbs_remove_one(struct ib_device *device, void *client_data);
 
+int uverbs_dealloc_mw(struct ib_mw *mw)
+{
+	struct ib_pd *pd = mw->pd;
+	int ret;
+
+	ret = mw->device->dealloc_mw(mw);
+	if (!ret)
+		atomic_dec(&pd->usecnt);
+	return ret;
+}
+
 static void ib_uverbs_release_dev(struct kobject *kobj)
 {
 	struct ib_uverbs_device *dev =
@@ -224,7 +235,7 @@ static int ib_uverbs_cleanup_ucontext(struct ib_uverbs_file *file,
 		struct ib_mw *mw = uobj->object;
 
 		idr_remove_uobj(&ib_uverbs_mw_idr, uobj);
-		ib_dealloc_mw(mw);
+		uverbs_dealloc_mw(mw);
 		kfree(uobj);
 	}
 
diff --git a/drivers/infiniband/core/uverbs_marshall.c b/drivers/infiniband/core/uverbs_marshall.c
index 7d2f14c9bbef..af020f80d50f 100644
--- a/drivers/infiniband/core/uverbs_marshall.c
+++ b/drivers/infiniband/core/uverbs_marshall.c
@@ -144,5 +144,6 @@ void ib_copy_path_rec_from_user(struct ib_sa_path_rec *dst,
 	memset(dst->dmac, 0, sizeof(dst->dmac));
 	dst->net = NULL;
 	dst->ifindex = 0;
+	dst->gid_type = IB_GID_TYPE_IB;
 }
 EXPORT_SYMBOL(ib_copy_path_rec_from_user);
diff --git a/drivers/infiniband/core/verbs.c b/drivers/infiniband/core/verbs.c
index 545906dec26d..5af6d024e053 100644
--- a/drivers/infiniband/core/verbs.c
+++ b/drivers/infiniband/core/verbs.c
@@ -229,12 +229,6 @@ EXPORT_SYMBOL(rdma_port_get_link_layer);
 struct ib_pd *ib_alloc_pd(struct ib_device *device)
 {
 	struct ib_pd *pd;
-	struct ib_device_attr devattr;
-	int rc;
-
-	rc = ib_query_device(device, &devattr);
-	if (rc)
-		return ERR_PTR(rc);
 
 	pd = device->alloc_pd(device, NULL, NULL);
 	if (IS_ERR(pd))
@@ -245,7 +239,7 @@ struct ib_pd *ib_alloc_pd(struct ib_device *device)
 	pd->local_mr = NULL;
 	atomic_set(&pd->usecnt, 0);
 
-	if (devattr.device_cap_flags & IB_DEVICE_LOCAL_DMA_LKEY)
+	if (device->attrs.device_cap_flags & IB_DEVICE_LOCAL_DMA_LKEY)
 		pd->local_dma_lkey = device->local_dma_lkey;
 	else {
 		struct ib_mr *mr;
@@ -311,8 +305,61 @@ struct ib_ah *ib_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr)
 }
 EXPORT_SYMBOL(ib_create_ah);
 
+static int ib_get_header_version(const union rdma_network_hdr *hdr)
+{
+	const struct iphdr *ip4h = (struct iphdr *)&hdr->roce4grh;
+	struct iphdr ip4h_checked;
+	const struct ipv6hdr *ip6h = (struct ipv6hdr *)&hdr->ibgrh;
+
+	/* If it's IPv6, the version must be 6, otherwise, the first
+	 * 20 bytes (before the IPv4 header) are garbled.
+	 */
+	if (ip6h->version != 6)
+		return (ip4h->version == 4) ? 4 : 0;
+	/* version may be 6 or 4 because the first 20 bytes could be garbled */
+
+	/* RoCE v2 requires no options, thus header length
+	 * must be 5 words
+	 */
+	if (ip4h->ihl != 5)
+		return 6;
+
+	/* Verify checksum.
+	 * We can't write on scattered buffers so we need to copy to
+	 * temp buffer.
+	 */
+	memcpy(&ip4h_checked, ip4h, sizeof(ip4h_checked));
+	ip4h_checked.check = 0;
+	ip4h_checked.check = ip_fast_csum((u8 *)&ip4h_checked, 5);
+	/* if IPv4 header checksum is OK, believe it */
+	if (ip4h->check == ip4h_checked.check)
+		return 4;
+	return 6;
+}
+
+static enum rdma_network_type ib_get_net_type_by_grh(struct ib_device *device,
+						     u8 port_num,
+						     const struct ib_grh *grh)
+{
+	int grh_version;
+
+	if (rdma_protocol_ib(device, port_num))
+		return RDMA_NETWORK_IB;
+
+	grh_version = ib_get_header_version((union rdma_network_hdr *)grh);
+
+	if (grh_version == 4)
+		return RDMA_NETWORK_IPV4;
+
+	if (grh->next_hdr == IPPROTO_UDP)
+		return RDMA_NETWORK_IPV6;
+
+	return RDMA_NETWORK_ROCE_V1;
+}
+
 struct find_gid_index_context {
 	u16 vlan_id;
+	enum ib_gid_type gid_type;
 };
 
 static bool find_gid_index(const union ib_gid *gid,
@@ -322,6 +369,9 @@ static bool find_gid_index(const union ib_gid *gid,
 	struct find_gid_index_context *ctx =
 		(struct find_gid_index_context *)context;
 
+	if (ctx->gid_type != gid_attr->gid_type)
+		return false;
+
 	if ((!!(ctx->vlan_id != 0xffff) == !is_vlan_dev(gid_attr->ndev)) ||
 	    (is_vlan_dev(gid_attr->ndev) &&
 	     vlan_dev_vlan_id(gid_attr->ndev) != ctx->vlan_id))
@@ -332,14 +382,49 @@ static bool find_gid_index(const union ib_gid *gid,
 
 static int get_sgid_index_from_eth(struct ib_device *device, u8 port_num,
 				   u16 vlan_id, const union ib_gid *sgid,
+				   enum ib_gid_type gid_type,
 				   u16 *gid_index)
 {
-	struct find_gid_index_context context = {.vlan_id = vlan_id};
+	struct find_gid_index_context context = {.vlan_id = vlan_id,
+						 .gid_type = gid_type};
 
 	return ib_find_gid_by_filter(device, sgid, port_num, find_gid_index,
 				     &context, gid_index);
 }
 
+static int get_gids_from_rdma_hdr(union rdma_network_hdr *hdr,
+				  enum rdma_network_type net_type,
+				  union ib_gid *sgid, union ib_gid *dgid)
+{
+	struct sockaddr_in  src_in;
+	struct sockaddr_in  dst_in;
+	__be32 src_saddr, dst_saddr;
+
+	if (!sgid || !dgid)
+		return -EINVAL;
+
+	if (net_type == RDMA_NETWORK_IPV4) {
+		memcpy(&src_in.sin_addr.s_addr,
+		       &hdr->roce4grh.saddr, 4);
+		memcpy(&dst_in.sin_addr.s_addr,
+		       &hdr->roce4grh.daddr, 4);
+		src_saddr = src_in.sin_addr.s_addr;
+		dst_saddr = dst_in.sin_addr.s_addr;
+		ipv6_addr_set_v4mapped(src_saddr,
+				       (struct in6_addr *)sgid);
+		ipv6_addr_set_v4mapped(dst_saddr,
+				       (struct in6_addr *)dgid);
+		return 0;
+	} else if (net_type == RDMA_NETWORK_IPV6 ||
+		   net_type == RDMA_NETWORK_IB) {
+		*dgid = hdr->ibgrh.dgid;
+		*sgid = hdr->ibgrh.sgid;
+		return 0;
+	} else {
+		return -EINVAL;
+	}
+}
+
 int ib_init_ah_from_wc(struct ib_device *device, u8 port_num,
 		       const struct ib_wc *wc, const struct ib_grh *grh,
 		       struct ib_ah_attr *ah_attr)
@@ -347,33 +432,72 @@ int ib_init_ah_from_wc(struct ib_device *device, u8 port_num,
 	u32 flow_class;
 	u16 gid_index;
 	int ret;
+	enum rdma_network_type net_type = RDMA_NETWORK_IB;
+	enum ib_gid_type gid_type = IB_GID_TYPE_IB;
+	int hoplimit = 0xff;
+	union ib_gid dgid;
+	union ib_gid sgid;
 
 	memset(ah_attr, 0, sizeof *ah_attr);
 	if (rdma_cap_eth_ah(device, port_num)) {
+		if (wc->wc_flags & IB_WC_WITH_NETWORK_HDR_TYPE)
+			net_type = wc->network_hdr_type;
+		else
+			net_type = ib_get_net_type_by_grh(device, port_num, grh);
+		gid_type = ib_network_to_gid_type(net_type);
+	}
+	ret = get_gids_from_rdma_hdr((union rdma_network_hdr *)grh, net_type,
+				     &sgid, &dgid);
+	if (ret)
+		return ret;
+
+	if (rdma_protocol_roce(device, port_num)) {
+		int if_index = 0;
 		u16 vlan_id = wc->wc_flags & IB_WC_WITH_VLAN ?
 				wc->vlan_id : 0xffff;
+		struct net_device *idev;
+		struct net_device *resolved_dev;
 
 		if (!(wc->wc_flags & IB_WC_GRH))
 			return -EPROTOTYPE;
 
-		if (!(wc->wc_flags & IB_WC_WITH_SMAC) ||
-		    !(wc->wc_flags & IB_WC_WITH_VLAN)) {
-			ret = rdma_addr_find_dmac_by_grh(&grh->dgid, &grh->sgid,
-							 ah_attr->dmac,
-							 wc->wc_flags & IB_WC_WITH_VLAN ?
-							 NULL : &vlan_id,
-							 0);
-			if (ret)
-				return ret;
+		if (!device->get_netdev)
+			return -EOPNOTSUPP;
+
+		idev = device->get_netdev(device, port_num);
+		if (!idev)
+			return -ENODEV;
+
+		ret = rdma_addr_find_l2_eth_by_grh(&dgid, &sgid,
+						   ah_attr->dmac,
+						   wc->wc_flags & IB_WC_WITH_VLAN ?
+						   NULL : &vlan_id,
+						   &if_index, &hoplimit);
+		if (ret) {
+			dev_put(idev);
+			return ret;
 		}
 
-		ret = get_sgid_index_from_eth(device, port_num, vlan_id,
-					      &grh->dgid, &gid_index);
+		resolved_dev = dev_get_by_index(&init_net, if_index);
+		if (resolved_dev->flags & IFF_LOOPBACK) {
+			dev_put(resolved_dev);
+			resolved_dev = idev;
+			dev_hold(resolved_dev);
+		}
+		rcu_read_lock();
+		if (resolved_dev != idev && !rdma_is_upper_dev_rcu(idev,
+								   resolved_dev))
+			ret = -EHOSTUNREACH;
+		rcu_read_unlock();
+		dev_put(idev);
+		dev_put(resolved_dev);
 		if (ret)
 			return ret;
 
-		if (wc->wc_flags & IB_WC_WITH_SMAC)
-			memcpy(ah_attr->dmac, wc->smac, ETH_ALEN);
+		ret = get_sgid_index_from_eth(device, port_num, vlan_id,
+					      &dgid, gid_type, &gid_index);
+		if (ret)
+			return ret;
 	}
 
 	ah_attr->dlid = wc->slid;
@@ -383,10 +507,11 @@ int ib_init_ah_from_wc(struct ib_device *device, u8 port_num,
 
 	if (wc->wc_flags & IB_WC_GRH) {
 		ah_attr->ah_flags = IB_AH_GRH;
-		ah_attr->grh.dgid = grh->sgid;
+		ah_attr->grh.dgid = sgid;
 
 		if (!rdma_cap_eth_ah(device, port_num)) {
-			ret = ib_find_cached_gid_by_port(device, &grh->dgid,
+			ret = ib_find_cached_gid_by_port(device, &dgid,
+							 IB_GID_TYPE_IB,
 							 port_num, NULL,
 							 &gid_index);
 			if (ret)
@@ -396,7 +521,7 @@ int ib_init_ah_from_wc(struct ib_device *device, u8 port_num,
 		ah_attr->grh.sgid_index = (u8) gid_index;
 		flow_class = be32_to_cpu(grh->version_tclass_flow);
 		ah_attr->grh.flow_label = flow_class & 0xFFFFF;
-		ah_attr->grh.hop_limit = 0xFF;
+		ah_attr->grh.hop_limit = hoplimit;
 		ah_attr->grh.traffic_class = (flow_class >> 20) & 0xFF;
 	}
 	return 0;
@@ -1014,6 +1139,7 @@ int ib_resolve_eth_dmac(struct ib_qp *qp,
 			union ib_gid		sgid;
 			struct ib_gid_attr	sgid_attr;
 			int			ifindex;
+			int			hop_limit;
 
 			ret = ib_query_gid(qp->device,
 					   qp_attr->ah_attr.port_num,
@@ -1028,12 +1154,14 @@ int ib_resolve_eth_dmac(struct ib_qp *qp,
 
 			ifindex = sgid_attr.ndev->ifindex;
 
-			ret = rdma_addr_find_dmac_by_grh(&sgid,
-							 &qp_attr->ah_attr.grh.dgid,
-							 qp_attr->ah_attr.dmac,
-							 NULL, ifindex);
+			ret = rdma_addr_find_l2_eth_by_grh(&sgid,
+							   &qp_attr->ah_attr.grh.dgid,
+							   qp_attr->ah_attr.dmac,
+							   NULL, &ifindex, &hop_limit);
 
 			dev_put(sgid_attr.ndev);
+
+			qp_attr->ah_attr.grh.hop_limit = hop_limit;
 		}
 	}
 out:
@@ -1215,29 +1343,17 @@ struct ib_mr *ib_get_dma_mr(struct ib_pd *pd, int mr_access_flags)
 		mr->pd      = pd;
 		mr->uobject = NULL;
 		atomic_inc(&pd->usecnt);
-		atomic_set(&mr->usecnt, 0);
 	}
 
 	return mr;
 }
 EXPORT_SYMBOL(ib_get_dma_mr);
 
-int ib_query_mr(struct ib_mr *mr, struct ib_mr_attr *mr_attr)
-{
-	return mr->device->query_mr ?
-		mr->device->query_mr(mr, mr_attr) : -ENOSYS;
-}
-EXPORT_SYMBOL(ib_query_mr);
-
 int ib_dereg_mr(struct ib_mr *mr)
 {
-	struct ib_pd *pd;
+	struct ib_pd *pd = mr->pd;
 	int ret;
 
-	if (atomic_read(&mr->usecnt))
-		return -EBUSY;
-
-	pd = mr->pd;
 	ret = mr->device->dereg_mr(mr);
 	if (!ret)
 		atomic_dec(&pd->usecnt);
@@ -1273,49 +1389,12 @@ struct ib_mr *ib_alloc_mr(struct ib_pd *pd,
 		mr->pd      = pd;
 		mr->uobject = NULL;
 		atomic_inc(&pd->usecnt);
-		atomic_set(&mr->usecnt, 0);
 	}
 
 	return mr;
 }
 EXPORT_SYMBOL(ib_alloc_mr);
 
-/* Memory windows */
-
-struct ib_mw *ib_alloc_mw(struct ib_pd *pd, enum ib_mw_type type)
-{
-	struct ib_mw *mw;
-
-	if (!pd->device->alloc_mw)
-		return ERR_PTR(-ENOSYS);
-
-	mw = pd->device->alloc_mw(pd, type);
-	if (!IS_ERR(mw)) {
-		mw->device  = pd->device;
-		mw->pd      = pd;
-		mw->uobject = NULL;
-		mw->type    = type;
-		atomic_inc(&pd->usecnt);
-	}
-
-	return mw;
-}
-EXPORT_SYMBOL(ib_alloc_mw);
-
-int ib_dealloc_mw(struct ib_mw *mw)
-{
-	struct ib_pd *pd;
-	int ret;
-
-	pd = mw->pd;
-	ret = mw->device->dealloc_mw(mw);
-	if (!ret)
-		atomic_dec(&pd->usecnt);
-
-	return ret;
-}
-EXPORT_SYMBOL(ib_dealloc_mw);
-
 /* "Fast" memory regions */
 
 struct ib_fmr *ib_alloc_fmr(struct ib_pd *pd,
@@ -1530,7 +1609,7 @@ int ib_sg_to_pages(struct ib_mr *mr,
 		   int (*set_page)(struct ib_mr *, u64))
 {
 	struct scatterlist *sg;
-	u64 last_end_dma_addr = 0, last_page_addr = 0;
+	u64 last_end_dma_addr = 0;
 	unsigned int last_page_off = 0;
 	u64 page_mask = ~((u64)mr->page_size - 1);
 	int i, ret;
@@ -1572,7 +1651,6 @@ next_page:
 
 		mr->length += dma_len;
 		last_end_dma_addr = end_dma_addr;
-		last_page_addr = end_dma_addr & page_mask;
 		last_page_off = end_dma_addr & ~page_mask;
 	}
 
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cm.c b/drivers/infiniband/hw/cxgb3/iwch_cm.c
index cb78b1e9bcd9..f504ba73e5dc 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cm.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cm.c
@@ -149,7 +149,7 @@ static int iwch_l2t_send(struct t3cdev *tdev, struct sk_buff *skb, struct l2t_en
 	error = l2t_send(tdev, skb, l2e);
 	if (error < 0)
 		kfree_skb(skb);
-	return error;
+	return error < 0 ? error : 0;
 }
 
 int iwch_cxgb3_ofld_send(struct t3cdev *tdev, struct sk_buff *skb)
@@ -165,7 +165,7 @@ int iwch_cxgb3_ofld_send(struct t3cdev *tdev, struct sk_buff *skb)
 	error = cxgb3_ofld_send(tdev, skb);
 	if (error < 0)
 		kfree_skb(skb);
-	return error;
+	return error < 0 ? error : 0;
 }
 
 static void release_tid(struct t3cdev *tdev, u32 hwtid, struct sk_buff *skb)
diff --git a/drivers/infiniband/hw/cxgb3/iwch_cq.c b/drivers/infiniband/hw/cxgb3/iwch_cq.c
index cfe404925a39..97fbfd2c298e 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_cq.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_cq.c
@@ -115,10 +115,6 @@ static int iwch_poll_cq_one(struct iwch_dev *rhp, struct iwch_cq *chp,
 		case T3_SEND_WITH_SE_INV:
 			wc->opcode = IB_WC_SEND;
 			break;
-		case T3_BIND_MW:
-			wc->opcode = IB_WC_BIND_MW;
-			break;
-
 		case T3_LOCAL_INV:
 			wc->opcode = IB_WC_LOCAL_INV;
 			break;
diff --git a/drivers/infiniband/hw/cxgb3/iwch_mem.c b/drivers/infiniband/hw/cxgb3/iwch_mem.c
index 5c36ee2809ac..1d04c872c9d5 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_mem.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_mem.c
@@ -75,37 +75,6 @@ int iwch_register_mem(struct iwch_dev *rhp, struct iwch_pd *php,
 	return ret;
 }
 
-int iwch_reregister_mem(struct iwch_dev *rhp, struct iwch_pd *php,
-					struct iwch_mr *mhp,
-					int shift,
-					int npages)
-{
-	u32 stag;
-	int ret;
-
-	/* We could support this... */
-	if (npages > mhp->attr.pbl_size)
-		return -ENOMEM;
-
-	stag = mhp->attr.stag;
-	if (cxio_reregister_phys_mem(&rhp->rdev,
-				   &stag, mhp->attr.pdid,
-				   mhp->attr.perms,
-				   mhp->attr.zbva,
-				   mhp->attr.va_fbo,
-				   mhp->attr.len,
-				   shift - 12,
-				   mhp->attr.pbl_size, mhp->attr.pbl_addr))
-		return -ENOMEM;
-
-	ret = iwch_finish_mem_reg(mhp, stag);
-	if (ret)
-		cxio_dereg_mem(&rhp->rdev, mhp->attr.stag, mhp->attr.pbl_size,
-		       mhp->attr.pbl_addr);
-
-	return ret;
-}
-
 int iwch_alloc_pbl(struct iwch_mr *mhp, int npages)
 {
 	mhp->attr.pbl_addr = cxio_hal_pblpool_alloc(&mhp->rhp->rdev,
@@ -130,74 +99,3 @@ int iwch_write_pbl(struct iwch_mr *mhp, __be64 *pages, int npages, int offset)
 	return cxio_write_pbl(&mhp->rhp->rdev, pages,
 			      mhp->attr.pbl_addr + (offset << 3), npages);
 }
-
-int build_phys_page_list(struct ib_phys_buf *buffer_list,
-					int num_phys_buf,
-					u64 *iova_start,
-					u64 *total_size,
-					int *npages,
-					int *shift,
-					__be64 **page_list)
-{
-	u64 mask;
-	int i, j, n;
-
-	mask = 0;
-	*total_size = 0;
-	for (i = 0; i < num_phys_buf; ++i) {
-		if (i != 0 && buffer_list[i].addr & ~PAGE_MASK)
-			return -EINVAL;
-		if (i != 0 && i != num_phys_buf - 1 &&
-		    (buffer_list[i].size & ~PAGE_MASK))
-			return -EINVAL;
-		*total_size += buffer_list[i].size;
-		if (i > 0)
-			mask |= buffer_list[i].addr;
-		else
-			mask |= buffer_list[i].addr & PAGE_MASK;
-		if (i != num_phys_buf - 1)
-			mask |= buffer_list[i].addr + buffer_list[i].size;
-		else
-			mask |= (buffer_list[i].addr + buffer_list[i].size +
-				PAGE_SIZE - 1) & PAGE_MASK;
-	}
-
-	if (*total_size > 0xFFFFFFFFULL)
-		return -ENOMEM;
-
-	/* Find largest page shift we can use to cover buffers */
-	for (*shift = PAGE_SHIFT; *shift < 27; ++(*shift))
-		if ((1ULL << *shift) & mask)
-			break;
-
-	buffer_list[0].size += buffer_list[0].addr & ((1ULL << *shift) - 1);
-	buffer_list[0].addr &= ~0ull << *shift;
-
-	*npages = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		*npages += (buffer_list[i].size +
-			(1ULL << *shift) - 1) >> *shift;
-
-	if (!*npages)
-		return -EINVAL;
-
-	*page_list = kmalloc(sizeof(u64) * *npages, GFP_KERNEL);
-	if (!*page_list)
-		return -ENOMEM;
-
-	n = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		for (j = 0;
-		     j < (buffer_list[i].size + (1ULL << *shift) - 1) >> *shift;
-		     ++j)
-			(*page_list)[n++] = cpu_to_be64(buffer_list[i].addr +
-			    ((u64) j << *shift));
-
-	PDBG("%s va 0x%llx mask 0x%llx shift %d len %lld pbl_size %d\n",
-	     __func__, (unsigned long long) *iova_start,
-	     (unsigned long long) mask, *shift, (unsigned long long) *total_size,
-	     *npages);
-
-	return 0;
-
-}
diff --git a/drivers/infiniband/hw/cxgb3/iwch_provider.c b/drivers/infiniband/hw/cxgb3/iwch_provider.c
index c34725ca0bb4..2734820d291b 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_provider.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_provider.c
@@ -458,9 +458,6 @@ static int iwch_dereg_mr(struct ib_mr *ib_mr)
 	u32 mmid;
 
 	PDBG("%s ib_mr %p\n", __func__, ib_mr);
-	/* There can be no memory windows */
-	if (atomic_read(&ib_mr->usecnt))
-		return -EINVAL;
 
 	mhp = to_iwch_mr(ib_mr);
 	kfree(mhp->pages);
@@ -479,24 +476,25 @@ static int iwch_dereg_mr(struct ib_mr *ib_mr)
 	return 0;
 }
 
-static struct ib_mr *iwch_register_phys_mem(struct ib_pd *pd,
-					struct ib_phys_buf *buffer_list,
-					int num_phys_buf,
-					int acc,
-					u64 *iova_start)
+static struct ib_mr *iwch_get_dma_mr(struct ib_pd *pd, int acc)
 {
-	__be64 *page_list;
-	int shift;
-	u64 total_size;
-	int npages;
-	struct iwch_dev *rhp;
-	struct iwch_pd *php;
+	const u64 total_size = 0xffffffff;
+	const u64 mask = (total_size + PAGE_SIZE - 1) & PAGE_MASK;
+	struct iwch_pd *php = to_iwch_pd(pd);
+	struct iwch_dev *rhp = php->rhp;
 	struct iwch_mr *mhp;
-	int ret;
+	__be64 *page_list;
+	int shift = 26, npages, ret, i;
 
 	PDBG("%s ib_pd %p\n", __func__, pd);
-	php = to_iwch_pd(pd);
-	rhp = php->rhp;
+
+	/*
+	 * T3 only supports 32 bits of size.
+	 */
+	if (sizeof(phys_addr_t) > 4) {
+		pr_warn_once(MOD "Cannot support dma_mrs on this platform.\n");
+		return ERR_PTR(-ENOTSUPP);
+	}
 
 	mhp = kzalloc(sizeof(*mhp), GFP_KERNEL);
 	if (!mhp)
@@ -504,22 +502,23 @@ static struct ib_mr *iwch_register_phys_mem(struct ib_pd *pd,
 
 	mhp->rhp = rhp;
 
-	/* First check that we have enough alignment */
-	if ((*iova_start & ~PAGE_MASK) != (buffer_list[0].addr & ~PAGE_MASK)) {
+	npages = (total_size + (1ULL << shift) - 1) >> shift;
+	if (!npages) {
 		ret = -EINVAL;
 		goto err;
 	}
 
-	if (num_phys_buf > 1 &&
-	    ((buffer_list[0].addr + buffer_list[0].size) & ~PAGE_MASK)) {
-		ret = -EINVAL;
+	page_list = kmalloc_array(npages, sizeof(u64), GFP_KERNEL);
+	if (!page_list) {
+		ret = -ENOMEM;
 		goto err;
 	}
 
-	ret = build_phys_page_list(buffer_list, num_phys_buf, iova_start,
-				   &total_size, &npages, &shift, &page_list);
-	if (ret)
-		goto err;
+	for (i = 0; i < npages; i++)
+		page_list[i] = cpu_to_be64((u64)i << shift);
+
+	PDBG("%s mask 0x%llx shift %d len %lld pbl_size %d\n",
+		__func__, mask, shift, total_size, npages);
 
 	ret = iwch_alloc_pbl(mhp, npages);
 	if (ret) {
@@ -536,7 +535,7 @@ static struct ib_mr *iwch_register_phys_mem(struct ib_pd *pd,
 	mhp->attr.zbva = 0;
 
 	mhp->attr.perms = iwch_ib_to_tpt_access(acc);
-	mhp->attr.va_fbo = *iova_start;
+	mhp->attr.va_fbo = 0;
 	mhp->attr.page_size = shift - 12;
 
 	mhp->attr.len = (u32) total_size;
@@ -553,76 +552,8 @@ err_pbl:
 err:
 	kfree(mhp);
 	return ERR_PTR(ret);
-
-}
-
-static int iwch_reregister_phys_mem(struct ib_mr *mr,
-				     int mr_rereg_mask,
-				     struct ib_pd *pd,
-	                             struct ib_phys_buf *buffer_list,
-	                             int num_phys_buf,
-	                             int acc, u64 * iova_start)
-{
-
-	struct iwch_mr mh, *mhp;
-	struct iwch_pd *php;
-	struct iwch_dev *rhp;
-	__be64 *page_list = NULL;
-	int shift = 0;
-	u64 total_size;
-	int npages = 0;
-	int ret;
-
-	PDBG("%s ib_mr %p ib_pd %p\n", __func__, mr, pd);
-
-	/* There can be no memory windows */
-	if (atomic_read(&mr->usecnt))
-		return -EINVAL;
-
-	mhp = to_iwch_mr(mr);
-	rhp = mhp->rhp;
-	php = to_iwch_pd(mr->pd);
-
-	/* make sure we are on the same adapter */
-	if (rhp != php->rhp)
-		return -EINVAL;
-
-	memcpy(&mh, mhp, sizeof *mhp);
-
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		php = to_iwch_pd(pd);
-	if (mr_rereg_mask & IB_MR_REREG_ACCESS)
-		mh.attr.perms = iwch_ib_to_tpt_access(acc);
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) {
-		ret = build_phys_page_list(buffer_list, num_phys_buf,
-					   iova_start,
-					   &total_size, &npages,
-					   &shift, &page_list);
-		if (ret)
-			return ret;
-	}
-
-	ret = iwch_reregister_mem(rhp, php, &mh, shift, npages);
-	kfree(page_list);
-	if (ret) {
-		return ret;
-	}
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		mhp->attr.pdid = php->pdid;
-	if (mr_rereg_mask & IB_MR_REREG_ACCESS)
-		mhp->attr.perms = iwch_ib_to_tpt_access(acc);
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) {
-		mhp->attr.zbva = 0;
-		mhp->attr.va_fbo = *iova_start;
-		mhp->attr.page_size = shift - 12;
-		mhp->attr.len = (u32) total_size;
-		mhp->attr.pbl_size = npages;
-	}
-
-	return 0;
 }
 
-
 static struct ib_mr *iwch_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				      u64 virt, int acc, struct ib_udata *udata)
 {
@@ -726,28 +657,6 @@ err:
 	return ERR_PTR(err);
 }
 
-static struct ib_mr *iwch_get_dma_mr(struct ib_pd *pd, int acc)
-{
-	struct ib_phys_buf bl;
-	u64 kva;
-	struct ib_mr *ibmr;
-
-	PDBG("%s ib_pd %p\n", __func__, pd);
-
-	/*
-	 * T3 only supports 32 bits of size.
-	 */
-	if (sizeof(phys_addr_t) > 4) {
-		pr_warn_once(MOD "Cannot support dma_mrs on this platform.\n");
-		return ERR_PTR(-ENOTSUPP);
-	}
-	bl.size = 0xffffffff;
-	bl.addr = 0;
-	kva = 0;
-	ibmr = iwch_register_phys_mem(pd, &bl, 1, acc, &kva);
-	return ibmr;
-}
-
 static struct ib_mw *iwch_alloc_mw(struct ib_pd *pd, enum ib_mw_type type)
 {
 	struct iwch_dev *rhp;
@@ -1452,12 +1361,9 @@ int iwch_register_device(struct iwch_dev *dev)
 	dev->ibdev.resize_cq = iwch_resize_cq;
 	dev->ibdev.poll_cq = iwch_poll_cq;
 	dev->ibdev.get_dma_mr = iwch_get_dma_mr;
-	dev->ibdev.reg_phys_mr = iwch_register_phys_mem;
-	dev->ibdev.rereg_phys_mr = iwch_reregister_phys_mem;
 	dev->ibdev.reg_user_mr = iwch_reg_user_mr;
 	dev->ibdev.dereg_mr = iwch_dereg_mr;
 	dev->ibdev.alloc_mw = iwch_alloc_mw;
-	dev->ibdev.bind_mw = iwch_bind_mw;
 	dev->ibdev.dealloc_mw = iwch_dealloc_mw;
 	dev->ibdev.alloc_mr = iwch_alloc_mr;
 	dev->ibdev.map_mr_sg = iwch_map_mr_sg;
diff --git a/drivers/infiniband/hw/cxgb3/iwch_provider.h b/drivers/infiniband/hw/cxgb3/iwch_provider.h
index 2ac85b86a680..252c464a09f6 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_provider.h
+++ b/drivers/infiniband/hw/cxgb3/iwch_provider.h
@@ -330,9 +330,6 @@ int iwch_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		      struct ib_send_wr **bad_wr);
 int iwch_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		      struct ib_recv_wr **bad_wr);
-int iwch_bind_mw(struct ib_qp *qp,
-			     struct ib_mw *mw,
-			     struct ib_mw_bind *mw_bind);
 int iwch_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *wc);
 int iwch_post_terminate(struct iwch_qp *qhp, struct respQ_msg_t *rsp_msg);
 int iwch_post_zb_read(struct iwch_ep *ep);
@@ -341,21 +338,9 @@ void iwch_unregister_device(struct iwch_dev *dev);
 void stop_read_rep_timer(struct iwch_qp *qhp);
 int iwch_register_mem(struct iwch_dev *rhp, struct iwch_pd *php,
 		      struct iwch_mr *mhp, int shift);
-int iwch_reregister_mem(struct iwch_dev *rhp, struct iwch_pd *php,
-					struct iwch_mr *mhp,
-					int shift,
-					int npages);
 int iwch_alloc_pbl(struct iwch_mr *mhp, int npages);
 void iwch_free_pbl(struct iwch_mr *mhp);
 int iwch_write_pbl(struct iwch_mr *mhp, __be64 *pages, int npages, int offset);
-int build_phys_page_list(struct ib_phys_buf *buffer_list,
-					int num_phys_buf,
-					u64 *iova_start,
-					u64 *total_size,
-					int *npages,
-					int *shift,
-					__be64 **page_list);
-
 
 #define IWCH_NODE_DESC "cxgb3 Chelsio Communications"
 
diff --git a/drivers/infiniband/hw/cxgb3/iwch_qp.c b/drivers/infiniband/hw/cxgb3/iwch_qp.c
index d0548fc6395e..d939980a708f 100644
--- a/drivers/infiniband/hw/cxgb3/iwch_qp.c
+++ b/drivers/infiniband/hw/cxgb3/iwch_qp.c
@@ -526,88 +526,6 @@ out:
 	return err;
 }
 
-int iwch_bind_mw(struct ib_qp *qp,
-			     struct ib_mw *mw,
-			     struct ib_mw_bind *mw_bind)
-{
-	struct iwch_dev *rhp;
-	struct iwch_mw *mhp;
-	struct iwch_qp *qhp;
-	union t3_wr *wqe;
-	u32 pbl_addr;
-	u8 page_size;
-	u32 num_wrs;
-	unsigned long flag;
-	struct ib_sge sgl;
-	int err=0;
-	enum t3_wr_flags t3_wr_flags;
-	u32 idx;
-	struct t3_swsq *sqp;
-
-	qhp = to_iwch_qp(qp);
-	mhp = to_iwch_mw(mw);
-	rhp = qhp->rhp;
-
-	spin_lock_irqsave(&qhp->lock, flag);
-	if (qhp->attr.state > IWCH_QP_STATE_RTS) {
-		spin_unlock_irqrestore(&qhp->lock, flag);
-		return -EINVAL;
-	}
-	num_wrs = Q_FREECNT(qhp->wq.sq_rptr, qhp->wq.sq_wptr,
-			    qhp->wq.sq_size_log2);
-	if (num_wrs == 0) {
-		spin_unlock_irqrestore(&qhp->lock, flag);
-		return -ENOMEM;
-	}
-	idx = Q_PTR2IDX(qhp->wq.wptr, qhp->wq.size_log2);
-	PDBG("%s: idx 0x%0x, mw 0x%p, mw_bind 0x%p\n", __func__, idx,
-	     mw, mw_bind);
-	wqe = (union t3_wr *) (qhp->wq.queue + idx);
-
-	t3_wr_flags = 0;
-	if (mw_bind->send_flags & IB_SEND_SIGNALED)
-		t3_wr_flags = T3_COMPLETION_FLAG;
-
-	sgl.addr = mw_bind->bind_info.addr;
-	sgl.lkey = mw_bind->bind_info.mr->lkey;
-	sgl.length = mw_bind->bind_info.length;
-	wqe->bind.reserved = 0;
-	wqe->bind.type = TPT_VATO;
-
-	/* TBD: check perms */
-	wqe->bind.perms = iwch_ib_to_tpt_bind_access(
-		mw_bind->bind_info.mw_access_flags);
-	wqe->bind.mr_stag = cpu_to_be32(mw_bind->bind_info.mr->lkey);
-	wqe->bind.mw_stag = cpu_to_be32(mw->rkey);
-	wqe->bind.mw_len = cpu_to_be32(mw_bind->bind_info.length);
-	wqe->bind.mw_va = cpu_to_be64(mw_bind->bind_info.addr);
-	err = iwch_sgl2pbl_map(rhp, &sgl, 1, &pbl_addr, &page_size);
-	if (err) {
-		spin_unlock_irqrestore(&qhp->lock, flag);
-		return err;
-	}
-	wqe->send.wrid.id0.hi = qhp->wq.sq_wptr;
-	sqp = qhp->wq.sq + Q_PTR2IDX(qhp->wq.sq_wptr, qhp->wq.sq_size_log2);
-	sqp->wr_id = mw_bind->wr_id;
-	sqp->opcode = T3_BIND_MW;
-	sqp->sq_wptr = qhp->wq.sq_wptr;
-	sqp->complete = 0;
-	sqp->signaled = (mw_bind->send_flags & IB_SEND_SIGNALED);
-	wqe->bind.mr_pbl_addr = cpu_to_be32(pbl_addr);
-	wqe->bind.mr_pagesz = page_size;
-	build_fw_riwrh((void *)wqe, T3_WR_BIND, t3_wr_flags,
-		       Q_GENBIT(qhp->wq.wptr, qhp->wq.size_log2), 0,
-		       sizeof(struct t3_bind_mw_wr) >> 3, T3_SOPEOP);
-	++(qhp->wq.wptr);
-	++(qhp->wq.sq_wptr);
-	spin_unlock_irqrestore(&qhp->lock, flag);
-
-	if (cxio_wq_db_enabled(&qhp->wq))
-		ring_doorbell(qhp->wq.doorbell, qhp->wq.qpid);
-
-	return err;
-}
-
 static inline void build_term_codes(struct respQ_msg_t *rsp_msg,
 				    u8 *layer_type, u8 *ecode)
 {
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index c9cffced00ca..cd2ff5f9518a 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -449,7 +449,7 @@ static void act_open_req_arp_failure(void *handle, struct sk_buff *skb)
 {
 	struct c4iw_ep *ep = handle;
 
-	printk(KERN_ERR MOD "ARP failure duing connect\n");
+	printk(KERN_ERR MOD "ARP failure during connect\n");
 	kfree_skb(skb);
 	connect_reply_upcall(ep, -EHOSTUNREACH);
 	state_set(&ep->com, DEAD);
@@ -3271,6 +3271,12 @@ static int create_server6(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 	struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)
 				    &ep->com.mapped_local_addr;
 
+	if (ipv6_addr_type(&sin6->sin6_addr) != IPV6_ADDR_ANY) {
+		err = cxgb4_clip_get(ep->com.dev->rdev.lldi.ports[0],
+				     (const u32 *)&sin6->sin6_addr.s6_addr, 1);
+		if (err)
+			return err;
+	}
 	c4iw_init_wr_wait(&ep->com.wr_wait);
 	err = cxgb4_create_server6(ep->com.dev->rdev.lldi.ports[0],
 				   ep->stid, &sin6->sin6_addr,
@@ -3282,13 +3288,13 @@ static int create_server6(struct c4iw_dev *dev, struct c4iw_listen_ep *ep)
 					  0, 0, __func__);
 	else if (err > 0)
 		err = net_xmit_errno(err);
-	if (err)
+	if (err) {
+		cxgb4_clip_release(ep->com.dev->rdev.lldi.ports[0],
+				   (const u32 *)&sin6->sin6_addr.s6_addr, 1);
 		pr_err("cxgb4_create_server6/filter failed err %d stid %d laddr %pI6 lport %d\n",
 		       err, ep->stid,
 		       sin6->sin6_addr.s6_addr, ntohs(sin6->sin6_port));
-	else
-		cxgb4_clip_get(ep->com.dev->rdev.lldi.ports[0],
-			       (const u32 *)&sin6->sin6_addr.s6_addr, 1);
+	}
 	return err;
 }
 
diff --git a/drivers/infiniband/hw/cxgb4/cq.c b/drivers/infiniband/hw/cxgb4/cq.c
index de9cd6901752..cf21df4a8bf5 100644
--- a/drivers/infiniband/hw/cxgb4/cq.c
+++ b/drivers/infiniband/hw/cxgb4/cq.c
@@ -744,9 +744,6 @@ static int c4iw_poll_cq_one(struct c4iw_cq *chp, struct ib_wc *wc)
 		case FW_RI_SEND_WITH_SE:
 			wc->opcode = IB_WC_SEND;
 			break;
-		case FW_RI_BIND_MW:
-			wc->opcode = IB_WC_BIND_MW;
-			break;
 
 		case FW_RI_LOCAL_INV:
 			wc->opcode = IB_WC_LOCAL_INV;
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index 58fce1742b8d..8024ea4417b8 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -315,14 +315,12 @@ static int qp_release(struct inode *inode, struct file *file)
 static int qp_open(struct inode *inode, struct file *file)
 {
 	struct c4iw_debugfs_data *qpd;
-	int ret = 0;
 	int count = 1;
 
 	qpd = kmalloc(sizeof *qpd, GFP_KERNEL);
-	if (!qpd) {
-		ret = -ENOMEM;
-		goto out;
-	}
+	if (!qpd)
+		return -ENOMEM;
+
 	qpd->devp = inode->i_private;
 	qpd->pos = 0;
 
@@ -333,8 +331,8 @@ static int qp_open(struct inode *inode, struct file *file)
 	qpd->bufsize = count * 128;
 	qpd->buf = vmalloc(qpd->bufsize);
 	if (!qpd->buf) {
-		ret = -ENOMEM;
-		goto err1;
+		kfree(qpd);
+		return -ENOMEM;
 	}
 
 	spin_lock_irq(&qpd->devp->lock);
@@ -343,11 +341,7 @@ static int qp_open(struct inode *inode, struct file *file)
 
 	qpd->buf[qpd->pos++] = 0;
 	file->private_data = qpd;
-	goto out;
-err1:
-	kfree(qpd);
-out:
-	return ret;
+	return 0;
 }
 
 static const struct file_operations qp_debugfs_fops = {
@@ -781,8 +775,7 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 		pr_err(MOD "%s: unsupported udb/ucq densities %u/%u\n",
 		       pci_name(rdev->lldi.pdev), rdev->lldi.udb_density,
 		       rdev->lldi.ucq_density);
-		err = -EINVAL;
-		goto err1;
+		return -EINVAL;
 	}
 	if (rdev->lldi.vr->qp.start != rdev->lldi.vr->cq.start ||
 	    rdev->lldi.vr->qp.size != rdev->lldi.vr->cq.size) {
@@ -791,8 +784,7 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 		       pci_name(rdev->lldi.pdev), rdev->lldi.vr->qp.start,
 		       rdev->lldi.vr->qp.size, rdev->lldi.vr->cq.size,
 		       rdev->lldi.vr->cq.size);
-		err = -EINVAL;
-		goto err1;
+		return -EINVAL;
 	}
 
 	rdev->qpmask = rdev->lldi.udb_density - 1;
@@ -816,10 +808,8 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 	     rdev->lldi.db_reg, rdev->lldi.gts_reg,
 	     rdev->qpmask, rdev->cqmask);
 
-	if (c4iw_num_stags(rdev) == 0) {
-		err = -EINVAL;
-		goto err1;
-	}
+	if (c4iw_num_stags(rdev) == 0)
+		return -EINVAL;
 
 	rdev->stats.pd.total = T4_MAX_NUM_PD;
 	rdev->stats.stag.total = rdev->lldi.vr->stag.size;
@@ -831,29 +821,31 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 	err = c4iw_init_resource(rdev, c4iw_num_stags(rdev), T4_MAX_NUM_PD);
 	if (err) {
 		printk(KERN_ERR MOD "error %d initializing resources\n", err);
-		goto err1;
+		return err;
 	}
 	err = c4iw_pblpool_create(rdev);
 	if (err) {
 		printk(KERN_ERR MOD "error %d initializing pbl pool\n", err);
-		goto err2;
+		goto destroy_resource;
 	}
 	err = c4iw_rqtpool_create(rdev);
 	if (err) {
 		printk(KERN_ERR MOD "error %d initializing rqt pool\n", err);
-		goto err3;
+		goto destroy_pblpool;
 	}
 	err = c4iw_ocqp_pool_create(rdev);
 	if (err) {
 		printk(KERN_ERR MOD "error %d initializing ocqp pool\n", err);
-		goto err4;
+		goto destroy_rqtpool;
 	}
 	rdev->status_page = (struct t4_dev_status_page *)
 			    __get_free_page(GFP_KERNEL);
-	if (!rdev->status_page) {
-		pr_err(MOD "error allocating status page\n");
-		goto err4;
-	}
+	if (!rdev->status_page)
+		goto destroy_ocqp_pool;
+	rdev->status_page->qp_start = rdev->lldi.vr->qp.start;
+	rdev->status_page->qp_size = rdev->lldi.vr->qp.size;
+	rdev->status_page->cq_start = rdev->lldi.vr->cq.start;
+	rdev->status_page->cq_size = rdev->lldi.vr->cq.size;
 
 	if (c4iw_wr_log) {
 		rdev->wr_log = kzalloc((1 << c4iw_wr_log_size_order) *
@@ -869,13 +861,14 @@ static int c4iw_rdev_open(struct c4iw_rdev *rdev)
 	rdev->status_page->db_off = 0;
 
 	return 0;
-err4:
+destroy_ocqp_pool:
+	c4iw_ocqp_pool_destroy(rdev);
+destroy_rqtpool:
 	c4iw_rqtpool_destroy(rdev);
-err3:
+destroy_pblpool:
 	c4iw_pblpool_destroy(rdev);
-err2:
+destroy_resource:
 	c4iw_destroy_resource(&rdev->resource);
-err1:
 	return err;
 }
 
diff --git a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
index 00e55faa086a..fb2de75a0392 100644
--- a/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
+++ b/drivers/infiniband/hw/cxgb4/iw_cxgb4.h
@@ -947,8 +947,6 @@ int c4iw_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		      struct ib_send_wr **bad_wr);
 int c4iw_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		      struct ib_recv_wr **bad_wr);
-int c4iw_bind_mw(struct ib_qp *qp, struct ib_mw *mw,
-		 struct ib_mw_bind *mw_bind);
 int c4iw_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param);
 int c4iw_create_listen(struct iw_cm_id *cm_id, int backlog);
 int c4iw_destroy_listen(struct iw_cm_id *cm_id);
@@ -968,17 +966,6 @@ struct ib_mr *c4iw_reg_user_mr(struct ib_pd *pd, u64 start,
 					   u64 length, u64 virt, int acc,
 					   struct ib_udata *udata);
 struct ib_mr *c4iw_get_dma_mr(struct ib_pd *pd, int acc);
-struct ib_mr *c4iw_register_phys_mem(struct ib_pd *pd,
-					struct ib_phys_buf *buffer_list,
-					int num_phys_buf,
-					int acc,
-					u64 *iova_start);
-int c4iw_reregister_phys_mem(struct ib_mr *mr,
-				     int mr_rereg_mask,
-				     struct ib_pd *pd,
-				     struct ib_phys_buf *buffer_list,
-				     int num_phys_buf,
-				     int acc, u64 *iova_start);
 int c4iw_dereg_mr(struct ib_mr *ib_mr);
 int c4iw_destroy_cq(struct ib_cq *ib_cq);
 struct ib_cq *c4iw_create_cq(struct ib_device *ibdev,
diff --git a/drivers/infiniband/hw/cxgb4/mem.c b/drivers/infiniband/hw/cxgb4/mem.c
index e1629ab58db7..7849890c4781 100644
--- a/drivers/infiniband/hw/cxgb4/mem.c
+++ b/drivers/infiniband/hw/cxgb4/mem.c
@@ -392,32 +392,6 @@ static int register_mem(struct c4iw_dev *rhp, struct c4iw_pd *php,
 	return ret;
 }
 
-static int reregister_mem(struct c4iw_dev *rhp, struct c4iw_pd *php,
-			  struct c4iw_mr *mhp, int shift, int npages)
-{
-	u32 stag;
-	int ret;
-
-	if (npages > mhp->attr.pbl_size)
-		return -ENOMEM;
-
-	stag = mhp->attr.stag;
-	ret = write_tpt_entry(&rhp->rdev, 0, &stag, 1, mhp->attr.pdid,
-			      FW_RI_STAG_NSMR, mhp->attr.perms,
-			      mhp->attr.mw_bind_enable, mhp->attr.zbva,
-			      mhp->attr.va_fbo, mhp->attr.len, shift - 12,
-			      mhp->attr.pbl_size, mhp->attr.pbl_addr);
-	if (ret)
-		return ret;
-
-	ret = finish_mem_reg(mhp, stag);
-	if (ret)
-		dereg_mem(&rhp->rdev, mhp->attr.stag, mhp->attr.pbl_size,
-		       mhp->attr.pbl_addr);
-
-	return ret;
-}
-
 static int alloc_pbl(struct c4iw_mr *mhp, int npages)
 {
 	mhp->attr.pbl_addr = c4iw_pblpool_alloc(&mhp->rhp->rdev,
@@ -431,228 +405,6 @@ static int alloc_pbl(struct c4iw_mr *mhp, int npages)
 	return 0;
 }
 
-static int build_phys_page_list(struct ib_phys_buf *buffer_list,
-				int num_phys_buf, u64 *iova_start,
-				u64 *total_size, int *npages,
-				int *shift, __be64 **page_list)
-{
-	u64 mask;
-	int i, j, n;
-
-	mask = 0;
-	*total_size = 0;
-	for (i = 0; i < num_phys_buf; ++i) {
-		if (i != 0 && buffer_list[i].addr & ~PAGE_MASK)
-			return -EINVAL;
-		if (i != 0 && i != num_phys_buf - 1 &&
-		    (buffer_list[i].size & ~PAGE_MASK))
-			return -EINVAL;
-		*total_size += buffer_list[i].size;
-		if (i > 0)
-			mask |= buffer_list[i].addr;
-		else
-			mask |= buffer_list[i].addr & PAGE_MASK;
-		if (i != num_phys_buf - 1)
-			mask |= buffer_list[i].addr + buffer_list[i].size;
-		else
-			mask |= (buffer_list[i].addr + buffer_list[i].size +
-				PAGE_SIZE - 1) & PAGE_MASK;
-	}
-
-	if (*total_size > 0xFFFFFFFFULL)
-		return -ENOMEM;
-
-	/* Find largest page shift we can use to cover buffers */
-	for (*shift = PAGE_SHIFT; *shift < 27; ++(*shift))
-		if ((1ULL << *shift) & mask)
-			break;
-
-	buffer_list[0].size += buffer_list[0].addr & ((1ULL << *shift) - 1);
-	buffer_list[0].addr &= ~0ull << *shift;
-
-	*npages = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		*npages += (buffer_list[i].size +
-			(1ULL << *shift) - 1) >> *shift;
-
-	if (!*npages)
-		return -EINVAL;
-
-	*page_list = kmalloc(sizeof(u64) * *npages, GFP_KERNEL);
-	if (!*page_list)
-		return -ENOMEM;
-
-	n = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		for (j = 0;
-		     j < (buffer_list[i].size + (1ULL << *shift) - 1) >> *shift;
-		     ++j)
-			(*page_list)[n++] = cpu_to_be64(buffer_list[i].addr +
-			    ((u64) j << *shift));
-
-	PDBG("%s va 0x%llx mask 0x%llx shift %d len %lld pbl_size %d\n",
-	     __func__, (unsigned long long)*iova_start,
-	     (unsigned long long)mask, *shift, (unsigned long long)*total_size,
-	     *npages);
-
-	return 0;
-
-}
-
-int c4iw_reregister_phys_mem(struct ib_mr *mr, int mr_rereg_mask,
-			     struct ib_pd *pd, struct ib_phys_buf *buffer_list,
-			     int num_phys_buf, int acc, u64 *iova_start)
-{
-
-	struct c4iw_mr mh, *mhp;
-	struct c4iw_pd *php;
-	struct c4iw_dev *rhp;
-	__be64 *page_list = NULL;
-	int shift = 0;
-	u64 total_size;
-	int npages;
-	int ret;
-
-	PDBG("%s ib_mr %p ib_pd %p\n", __func__, mr, pd);
-
-	/* There can be no memory windows */
-	if (atomic_read(&mr->usecnt))
-		return -EINVAL;
-
-	mhp = to_c4iw_mr(mr);
-	rhp = mhp->rhp;
-	php = to_c4iw_pd(mr->pd);
-
-	/* make sure we are on the same adapter */
-	if (rhp != php->rhp)
-		return -EINVAL;
-
-	memcpy(&mh, mhp, sizeof *mhp);
-
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		php = to_c4iw_pd(pd);
-	if (mr_rereg_mask & IB_MR_REREG_ACCESS) {
-		mh.attr.perms = c4iw_ib_to_tpt_access(acc);
-		mh.attr.mw_bind_enable = (acc & IB_ACCESS_MW_BIND) ==
-					 IB_ACCESS_MW_BIND;
-	}
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) {
-		ret = build_phys_page_list(buffer_list, num_phys_buf,
-						iova_start,
-						&total_size, &npages,
-						&shift, &page_list);
-		if (ret)
-			return ret;
-	}
-
-	if (mr_exceeds_hw_limits(rhp, total_size)) {
-		kfree(page_list);
-		return -EINVAL;
-	}
-
-	ret = reregister_mem(rhp, php, &mh, shift, npages);
-	kfree(page_list);
-	if (ret)
-		return ret;
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		mhp->attr.pdid = php->pdid;
-	if (mr_rereg_mask & IB_MR_REREG_ACCESS)
-		mhp->attr.perms = c4iw_ib_to_tpt_access(acc);
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) {
-		mhp->attr.zbva = 0;
-		mhp->attr.va_fbo = *iova_start;
-		mhp->attr.page_size = shift - 12;
-		mhp->attr.len = (u32) total_size;
-		mhp->attr.pbl_size = npages;
-	}
-
-	return 0;
-}
-
-struct ib_mr *c4iw_register_phys_mem(struct ib_pd *pd,
-				     struct ib_phys_buf *buffer_list,
-				     int num_phys_buf, int acc, u64 *iova_start)
-{
-	__be64 *page_list;
-	int shift;
-	u64 total_size;
-	int npages;
-	struct c4iw_dev *rhp;
-	struct c4iw_pd *php;
-	struct c4iw_mr *mhp;
-	int ret;
-
-	PDBG("%s ib_pd %p\n", __func__, pd);
-	php = to_c4iw_pd(pd);
-	rhp = php->rhp;
-
-	mhp = kzalloc(sizeof(*mhp), GFP_KERNEL);
-	if (!mhp)
-		return ERR_PTR(-ENOMEM);
-
-	mhp->rhp = rhp;
-
-	/* First check that we have enough alignment */
-	if ((*iova_start & ~PAGE_MASK) != (buffer_list[0].addr & ~PAGE_MASK)) {
-		ret = -EINVAL;
-		goto err;
-	}
-
-	if (num_phys_buf > 1 &&
-	    ((buffer_list[0].addr + buffer_list[0].size) & ~PAGE_MASK)) {
-		ret = -EINVAL;
-		goto err;
-	}
-
-	ret = build_phys_page_list(buffer_list, num_phys_buf, iova_start,
-					&total_size, &npages, &shift,
-					&page_list);
-	if (ret)
-		goto err;
-
-	if (mr_exceeds_hw_limits(rhp, total_size)) {
-		kfree(page_list);
-		ret = -EINVAL;
-		goto err;
-	}
-
-	ret = alloc_pbl(mhp, npages);
-	if (ret) {
-		kfree(page_list);
-		goto err;
-	}
-
-	ret = write_pbl(&mhp->rhp->rdev, page_list, mhp->attr.pbl_addr,
-			     npages);
-	kfree(page_list);
-	if (ret)
-		goto err_pbl;
-
-	mhp->attr.pdid = php->pdid;
-	mhp->attr.zbva = 0;
-
-	mhp->attr.perms = c4iw_ib_to_tpt_access(acc);
-	mhp->attr.va_fbo = *iova_start;
-	mhp->attr.page_size = shift - 12;
-
-	mhp->attr.len = (u32) total_size;
-	mhp->attr.pbl_size = npages;
-	ret = register_mem(rhp, php, mhp, shift);
-	if (ret)
-		goto err_pbl;
-
-	return &mhp->ibmr;
-
-err_pbl:
-	c4iw_pblpool_free(&mhp->rhp->rdev, mhp->attr.pbl_addr,
-			      mhp->attr.pbl_size << 3);
-
-err:
-	kfree(mhp);
-	return ERR_PTR(ret);
-
-}
-
 struct ib_mr *c4iw_get_dma_mr(struct ib_pd *pd, int acc)
 {
 	struct c4iw_dev *rhp;
@@ -952,9 +704,6 @@ int c4iw_dereg_mr(struct ib_mr *ib_mr)
 	u32 mmid;
 
 	PDBG("%s ib_mr %p\n", __func__, ib_mr);
-	/* There can be no memory windows */
-	if (atomic_read(&ib_mr->usecnt))
-		return -EINVAL;
 
 	mhp = to_c4iw_mr(ib_mr);
 	rhp = mhp->rhp;
diff --git a/drivers/infiniband/hw/cxgb4/provider.c b/drivers/infiniband/hw/cxgb4/provider.c
index 0a7d99818b17..ec04272fbdc2 100644
--- a/drivers/infiniband/hw/cxgb4/provider.c
+++ b/drivers/infiniband/hw/cxgb4/provider.c
@@ -549,12 +549,9 @@ int c4iw_register_device(struct c4iw_dev *dev)
 	dev->ibdev.resize_cq = c4iw_resize_cq;
 	dev->ibdev.poll_cq = c4iw_poll_cq;
 	dev->ibdev.get_dma_mr = c4iw_get_dma_mr;
-	dev->ibdev.reg_phys_mr = c4iw_register_phys_mem;
-	dev->ibdev.rereg_phys_mr = c4iw_reregister_phys_mem;
 	dev->ibdev.reg_user_mr = c4iw_reg_user_mr;
 	dev->ibdev.dereg_mr = c4iw_dereg_mr;
 	dev->ibdev.alloc_mw = c4iw_alloc_mw;
-	dev->ibdev.bind_mw = c4iw_bind_mw;
 	dev->ibdev.dealloc_mw = c4iw_dealloc_mw;
 	dev->ibdev.alloc_mr = c4iw_alloc_mr;
 	dev->ibdev.map_mr_sg = c4iw_map_mr_sg;
diff --git a/drivers/infiniband/hw/cxgb4/qp.c b/drivers/infiniband/hw/cxgb4/qp.c
index aa515afee724..e99345eb875a 100644
--- a/drivers/infiniband/hw/cxgb4/qp.c
+++ b/drivers/infiniband/hw/cxgb4/qp.c
@@ -933,11 +933,6 @@ int c4iw_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 	return err;
 }
 
-int c4iw_bind_mw(struct ib_qp *qp, struct ib_mw *mw, struct ib_mw_bind *mw_bind)
-{
-	return -ENOSYS;
-}
-
 static inline void build_term_codes(struct t4_cqe *err_cqe, u8 *layer_type,
 				    u8 *ecode)
 {
diff --git a/drivers/infiniband/hw/cxgb4/t4.h b/drivers/infiniband/hw/cxgb4/t4.h
index 1092a2d1f607..6126bbe36095 100644
--- a/drivers/infiniband/hw/cxgb4/t4.h
+++ b/drivers/infiniband/hw/cxgb4/t4.h
@@ -699,4 +699,11 @@ static inline void t4_set_cq_in_error(struct t4_cq *cq)
 
 struct t4_dev_status_page {
 	u8 db_off;
+	u8 pad1;
+	u16 pad2;
+	u32 pad3;
+	u64 qp_start;
+	u64 qp_size;
+	u64 cq_start;
+	u64 cq_size;
 };
diff --git a/drivers/infiniband/hw/cxgb4/user.h b/drivers/infiniband/hw/cxgb4/user.h
index cbd0ce170728..295f422b9a3a 100644
--- a/drivers/infiniband/hw/cxgb4/user.h
+++ b/drivers/infiniband/hw/cxgb4/user.h
@@ -32,7 +32,7 @@
 #ifndef __C4IW_USER_H__
 #define __C4IW_USER_H__
 
-#define C4IW_UVERBS_ABI_VERSION	2
+#define C4IW_UVERBS_ABI_VERSION	3
 
 /*
  * Make sure that all structs defined in this file remain laid out so
diff --git a/drivers/infiniband/hw/mlx4/ah.c b/drivers/infiniband/hw/mlx4/ah.c
index 86af71351d9a..105246fba2e7 100644
--- a/drivers/infiniband/hw/mlx4/ah.c
+++ b/drivers/infiniband/hw/mlx4/ah.c
@@ -92,7 +92,7 @@ static struct ib_ah *create_iboe_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr
 				ah_attr->grh.sgid_index, &sgid, &gid_attr);
 	if (ret)
 		return ERR_PTR(ret);
-	memset(ah->av.eth.s_mac, 0, ETH_ALEN);
+	eth_zero_addr(ah->av.eth.s_mac);
 	if (gid_attr.ndev) {
 		if (is_vlan_dev(gid_attr.ndev))
 			vlan_tag = vlan_dev_vlan_id(gid_attr.ndev);
@@ -104,6 +104,7 @@ static struct ib_ah *create_iboe_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr
 	ah->av.eth.port_pd = cpu_to_be32(to_mpd(pd)->pdn | (ah_attr->port_num << 24));
 	ah->av.eth.gid_index = mlx4_ib_gid_index_to_real_index(ibdev, ah_attr->port_num, ah_attr->grh.sgid_index);
 	ah->av.eth.vlan = cpu_to_be16(vlan_tag);
+	ah->av.eth.hop_limit = ah_attr->grh.hop_limit;
 	if (ah_attr->static_rate) {
 		ah->av.eth.stat_rate = ah_attr->static_rate + MLX4_STAT_RATE_OFFSET;
 		while (ah->av.eth.stat_rate > IB_RATE_2_5_GBPS + MLX4_STAT_RATE_OFFSET &&
diff --git a/drivers/infiniband/hw/mlx4/cq.c b/drivers/infiniband/hw/mlx4/cq.c
index b88fc8f5ab18..9f8b516eb2b0 100644
--- a/drivers/infiniband/hw/mlx4/cq.c
+++ b/drivers/infiniband/hw/mlx4/cq.c
@@ -811,9 +811,6 @@ repoll:
 			wc->opcode    = IB_WC_MASKED_FETCH_ADD;
 			wc->byte_len  = 8;
 			break;
-		case MLX4_OPCODE_BIND_MW:
-			wc->opcode    = IB_WC_BIND_MW;
-			break;
 		case MLX4_OPCODE_LSO:
 			wc->opcode    = IB_WC_LSO;
 			break;
diff --git a/drivers/infiniband/hw/mlx4/mad.c b/drivers/infiniband/hw/mlx4/mad.c
index 870e56b6b25f..d68f506c1922 100644
--- a/drivers/infiniband/hw/mlx4/mad.c
+++ b/drivers/infiniband/hw/mlx4/mad.c
@@ -40,6 +40,7 @@
 #include <linux/gfp.h>
 #include <rdma/ib_pma.h>
 
+#include <linux/mlx4/driver.h>
 #include "mlx4_ib.h"
 
 enum {
@@ -606,8 +607,8 @@ static int mlx4_ib_demux_mad(struct ib_device *ibdev, u8 port,
 			struct ib_mad *mad)
 {
 	struct mlx4_ib_dev *dev = to_mdev(ibdev);
-	int err;
-	int slave;
+	int err, other_port;
+	int slave = -1;
 	u8 *slave_id;
 	int is_eth = 0;
 
@@ -625,7 +626,17 @@ static int mlx4_ib_demux_mad(struct ib_device *ibdev, u8 port,
 			mlx4_ib_warn(ibdev, "RoCE mgmt class is not CM\n");
 			return -EINVAL;
 		}
-		if (mlx4_get_slave_from_roce_gid(dev->dev, port, grh->dgid.raw, &slave)) {
+		err = mlx4_get_slave_from_roce_gid(dev->dev, port, grh->dgid.raw, &slave);
+		if (err && mlx4_is_mf_bonded(dev->dev)) {
+			other_port = (port == 1) ? 2 : 1;
+			err = mlx4_get_slave_from_roce_gid(dev->dev, other_port, grh->dgid.raw, &slave);
+			if (!err) {
+				port = other_port;
+				pr_debug("resolved slave %d from gid %pI6 wire port %d other %d\n",
+					 slave, grh->dgid.raw, port, other_port);
+			}
+		}
+		if (err) {
 			mlx4_ib_warn(ibdev, "failed matching grh\n");
 			return -ENOENT;
 		}
@@ -806,17 +817,48 @@ static int ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
 }
 
-static void edit_counter(struct mlx4_counter *cnt,
-					struct ib_pma_portcounters *pma_cnt)
+static void edit_counter(struct mlx4_counter *cnt, void *counters,
+			 __be16 attr_id)
+{
+	switch (attr_id) {
+	case IB_PMA_PORT_COUNTERS:
+	{
+		struct ib_pma_portcounters *pma_cnt =
+			(struct ib_pma_portcounters *)counters;
+
+		ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_data,
+				     (be64_to_cpu(cnt->tx_bytes) >> 2));
+		ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_data,
+				     (be64_to_cpu(cnt->rx_bytes) >> 2));
+		ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_packets,
+				     be64_to_cpu(cnt->tx_frames));
+		ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_packets,
+				     be64_to_cpu(cnt->rx_frames));
+		break;
+	}
+	case IB_PMA_PORT_COUNTERS_EXT:
+	{
+		struct ib_pma_portcounters_ext *pma_cnt_ext =
+			(struct ib_pma_portcounters_ext *)counters;
+
+		pma_cnt_ext->port_xmit_data =
+			cpu_to_be64(be64_to_cpu(cnt->tx_bytes) >> 2);
+		pma_cnt_ext->port_rcv_data =
+			cpu_to_be64(be64_to_cpu(cnt->rx_bytes) >> 2);
+		pma_cnt_ext->port_xmit_packets = cnt->tx_frames;
+		pma_cnt_ext->port_rcv_packets = cnt->rx_frames;
+		break;
+	}
+	}
+}
+
+static int iboe_process_mad_port_info(void *out_mad)
 {
-	ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_data,
-			     (be64_to_cpu(cnt->tx_bytes) >> 2));
-	ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_data,
-			     (be64_to_cpu(cnt->rx_bytes) >> 2));
-	ASSIGN_32BIT_COUNTER(pma_cnt->port_xmit_packets,
-			     be64_to_cpu(cnt->tx_frames));
-	ASSIGN_32BIT_COUNTER(pma_cnt->port_rcv_packets,
-			     be64_to_cpu(cnt->rx_frames));
+	struct ib_class_port_info cpi = {};
+
+	cpi.capability_mask = IB_PMA_CLASS_CAP_EXT_WIDTH;
+	memcpy(out_mad, &cpi, sizeof(cpi));
+	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
 }
 
 static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
@@ -831,6 +873,9 @@ static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 	if (in_mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_PERF_MGMT)
 		return -EINVAL;
 
+	if (in_mad->mad_hdr.attr_id == IB_PMA_CLASS_PORT_INFO)
+		return iboe_process_mad_port_info((void *)(out_mad->data + 40));
+
 	memset(&counter_stats, 0, sizeof(counter_stats));
 	mutex_lock(&dev->counters_table[port_num - 1].mutex);
 	list_for_each_entry(tmp_counter,
@@ -852,7 +897,8 @@ static int iboe_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 		switch (counter_stats.counter_mode & 0xf) {
 		case 0:
 			edit_counter(&counter_stats,
-				     (void *)(out_mad->data + 40));
+				     (void *)(out_mad->data + 40),
+				     in_mad->mad_hdr.attr_id);
 			err = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
 			break;
 		default:
@@ -883,8 +929,10 @@ int mlx4_ib_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
 	 */
 	if (link == IB_LINK_LAYER_INFINIBAND) {
 		if (mlx4_is_slave(dev->dev) &&
-		    in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_PERF_MGMT &&
-		    in_mad->mad_hdr.attr_id == IB_PMA_PORT_COUNTERS)
+		    (in_mad->mad_hdr.mgmt_class == IB_MGMT_CLASS_PERF_MGMT &&
+		     (in_mad->mad_hdr.attr_id == IB_PMA_PORT_COUNTERS ||
+		      in_mad->mad_hdr.attr_id == IB_PMA_PORT_COUNTERS_EXT ||
+		      in_mad->mad_hdr.attr_id == IB_PMA_CLASS_PORT_INFO)))
 			return iboe_process_mad(ibdev, mad_flags, port_num, in_wc,
 						in_grh, in_mad, out_mad);
 
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index 97d6878f9938..1c7ab6cabbb8 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -154,9 +154,9 @@ static struct net_device *mlx4_ib_get_netdev(struct ib_device *device, u8 port_n
 	return dev;
 }
 
-static int mlx4_ib_update_gids(struct gid_entry *gids,
-			       struct mlx4_ib_dev *ibdev,
-			       u8 port_num)
+static int mlx4_ib_update_gids_v1(struct gid_entry *gids,
+				  struct mlx4_ib_dev *ibdev,
+				  u8 port_num)
 {
 	struct mlx4_cmd_mailbox *mailbox;
 	int err;
@@ -187,6 +187,63 @@ static int mlx4_ib_update_gids(struct gid_entry *gids,
 	return err;
 }
 
+static int mlx4_ib_update_gids_v1_v2(struct gid_entry *gids,
+				     struct mlx4_ib_dev *ibdev,
+				     u8 port_num)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	int err;
+	struct mlx4_dev *dev = ibdev->dev;
+	int i;
+	struct {
+		union ib_gid	gid;
+		__be32		rsrvd1[2];
+		__be16		rsrvd2;
+		u8		type;
+		u8		version;
+		__be32		rsrvd3;
+	} *gid_tbl;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return -ENOMEM;
+
+	gid_tbl = mailbox->buf;
+	for (i = 0; i < MLX4_MAX_PORT_GIDS; ++i) {
+		memcpy(&gid_tbl[i].gid, &gids[i].gid, sizeof(union ib_gid));
+		if (gids[i].gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP) {
+			gid_tbl[i].version = 2;
+			if (!ipv6_addr_v4mapped((struct in6_addr *)&gids[i].gid))
+				gid_tbl[i].type = 1;
+			else
+				memset(&gid_tbl[i].gid, 0, 12);
+		}
+	}
+
+	err = mlx4_cmd(dev, mailbox->dma,
+		       MLX4_SET_PORT_ROCE_ADDR << 8 | port_num,
+		       1, MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+		       MLX4_CMD_WRAPPED);
+	if (mlx4_is_bonded(dev))
+		err += mlx4_cmd(dev, mailbox->dma,
+				MLX4_SET_PORT_ROCE_ADDR << 8 | 2,
+				1, MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
+				MLX4_CMD_WRAPPED);
+
+	mlx4_free_cmd_mailbox(dev, mailbox);
+	return err;
+}
+
+static int mlx4_ib_update_gids(struct gid_entry *gids,
+			       struct mlx4_ib_dev *ibdev,
+			       u8 port_num)
+{
+	if (ibdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2)
+		return mlx4_ib_update_gids_v1_v2(gids, ibdev, port_num);
+
+	return mlx4_ib_update_gids_v1(gids, ibdev, port_num);
+}
+
 static int mlx4_ib_add_gid(struct ib_device *device,
 			   u8 port_num,
 			   unsigned int index,
@@ -215,7 +272,8 @@ static int mlx4_ib_add_gid(struct ib_device *device,
 	port_gid_table = &iboe->gids[port_num - 1];
 	spin_lock_bh(&iboe->lock);
 	for (i = 0; i < MLX4_MAX_PORT_GIDS; ++i) {
-		if (!memcmp(&port_gid_table->gids[i].gid, gid, sizeof(*gid))) {
+		if (!memcmp(&port_gid_table->gids[i].gid, gid, sizeof(*gid)) &&
+		    (port_gid_table->gids[i].gid_type == attr->gid_type))  {
 			found = i;
 			break;
 		}
@@ -233,6 +291,7 @@ static int mlx4_ib_add_gid(struct ib_device *device,
 			} else {
 				*context = port_gid_table->gids[free].ctx;
 				memcpy(&port_gid_table->gids[free].gid, gid, sizeof(*gid));
+				port_gid_table->gids[free].gid_type = attr->gid_type;
 				port_gid_table->gids[free].ctx->real_index = free;
 				port_gid_table->gids[free].ctx->refcount = 1;
 				hw_update = 1;
@@ -248,8 +307,10 @@ static int mlx4_ib_add_gid(struct ib_device *device,
 		if (!gids) {
 			ret = -ENOMEM;
 		} else {
-			for (i = 0; i < MLX4_MAX_PORT_GIDS; i++)
+			for (i = 0; i < MLX4_MAX_PORT_GIDS; i++) {
 				memcpy(&gids[i].gid, &port_gid_table->gids[i].gid, sizeof(union ib_gid));
+				gids[i].gid_type = port_gid_table->gids[i].gid_type;
+			}
 		}
 	}
 	spin_unlock_bh(&iboe->lock);
@@ -325,6 +386,7 @@ int mlx4_ib_gid_index_to_real_index(struct mlx4_ib_dev *ibdev,
 	int i;
 	int ret;
 	unsigned long flags;
+	struct ib_gid_attr attr;
 
 	if (port_num > MLX4_MAX_PORTS)
 		return -EINVAL;
@@ -335,10 +397,13 @@ int mlx4_ib_gid_index_to_real_index(struct mlx4_ib_dev *ibdev,
 	if (!rdma_cap_roce_gid_table(&ibdev->ib_dev, port_num))
 		return index;
 
-	ret = ib_get_cached_gid(&ibdev->ib_dev, port_num, index, &gid, NULL);
+	ret = ib_get_cached_gid(&ibdev->ib_dev, port_num, index, &gid, &attr);
 	if (ret)
 		return ret;
 
+	if (attr.ndev)
+		dev_put(attr.ndev);
+
 	if (!memcmp(&gid, &zgid, sizeof(gid)))
 		return -EINVAL;
 
@@ -346,7 +411,8 @@ int mlx4_ib_gid_index_to_real_index(struct mlx4_ib_dev *ibdev,
 	port_gid_table = &iboe->gids[port_num - 1];
 
 	for (i = 0; i < MLX4_MAX_PORT_GIDS; ++i)
-		if (!memcmp(&port_gid_table->gids[i].gid, &gid, sizeof(gid))) {
+		if (!memcmp(&port_gid_table->gids[i].gid, &gid, sizeof(gid)) &&
+		    attr.gid_type == port_gid_table->gids[i].gid_type) {
 			ctx = port_gid_table->gids[i].ctx;
 			break;
 		}
@@ -2119,6 +2185,7 @@ static int mlx4_port_immutable(struct ib_device *ibdev, u8 port_num,
 			       struct ib_port_immutable *immutable)
 {
 	struct ib_port_attr attr;
+	struct mlx4_ib_dev *mdev = to_mdev(ibdev);
 	int err;
 
 	err = mlx4_ib_query_port(ibdev, port_num, &attr);
@@ -2128,10 +2195,15 @@ static int mlx4_port_immutable(struct ib_device *ibdev, u8 port_num,
 	immutable->pkey_tbl_len = attr.pkey_tbl_len;
 	immutable->gid_tbl_len = attr.gid_tbl_len;
 
-	if (mlx4_ib_port_link_layer(ibdev, port_num) == IB_LINK_LAYER_INFINIBAND)
+	if (mlx4_ib_port_link_layer(ibdev, port_num) == IB_LINK_LAYER_INFINIBAND) {
 		immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
-	else
-		immutable->core_cap_flags = RDMA_CORE_PORT_IBA_ROCE;
+	} else {
+		if (mdev->dev->caps.flags & MLX4_DEV_CAP_FLAG_IBOE)
+			immutable->core_cap_flags = RDMA_CORE_PORT_IBA_ROCE;
+		if (mdev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2)
+			immutable->core_cap_flags = RDMA_CORE_PORT_IBA_ROCE |
+				RDMA_CORE_PORT_IBA_ROCE_UDP_ENCAP;
+	}
 
 	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
 
@@ -2283,7 +2355,6 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_MEM_WINDOW ||
 	    dev->caps.bmme_flags & MLX4_BMME_FLAG_TYPE_2_WIN) {
 		ibdev->ib_dev.alloc_mw = mlx4_ib_alloc_mw;
-		ibdev->ib_dev.bind_mw = mlx4_ib_bind_mw;
 		ibdev->ib_dev.dealloc_mw = mlx4_ib_dealloc_mw;
 
 		ibdev->ib_dev.uverbs_cmd_mask |=
@@ -2423,7 +2494,8 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 	if (mlx4_ib_init_sriov(ibdev))
 		goto err_mad;
 
-	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_IBOE) {
+	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_IBOE ||
+	    dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2) {
 		if (!iboe->nb.notifier_call) {
 			iboe->nb.notifier_call = mlx4_ib_netdev_event;
 			err = register_netdevice_notifier(&iboe->nb);
@@ -2432,6 +2504,12 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 				goto err_notif;
 			}
 		}
+		if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2) {
+			err = mlx4_config_roce_v2_port(dev, ROCE_V2_UDP_DPORT);
+			if (err) {
+				goto err_notif;
+			}
+		}
 	}
 
 	for (j = 0; j < ARRAY_SIZE(mlx4_class_attributes); ++j) {
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index 1caa11edac03..52ce7b000044 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -177,11 +177,18 @@ struct mlx4_ib_wq {
 	unsigned		tail;
 };
 
+enum {
+	MLX4_IB_QP_CREATE_ROCE_V2_GSI = IB_QP_CREATE_RESERVED_START
+};
+
 enum mlx4_ib_qp_flags {
 	MLX4_IB_QP_LSO = IB_QP_CREATE_IPOIB_UD_LSO,
 	MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK = IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK,
 	MLX4_IB_QP_NETIF = IB_QP_CREATE_NETIF_QP,
 	MLX4_IB_QP_CREATE_USE_GFP_NOIO = IB_QP_CREATE_USE_GFP_NOIO,
+
+	/* Mellanox specific flags start from IB_QP_CREATE_RESERVED_START */
+	MLX4_IB_ROCE_V2_GSI_QP = MLX4_IB_QP_CREATE_ROCE_V2_GSI,
 	MLX4_IB_SRIOV_TUNNEL_QP = 1 << 30,
 	MLX4_IB_SRIOV_SQP = 1 << 31,
 };
@@ -478,6 +485,7 @@ struct gid_cache_context {
 
 struct gid_entry {
 	union ib_gid	gid;
+	enum ib_gid_type gid_type;
 	struct gid_cache_context *ctx;
 };
 
@@ -704,8 +712,6 @@ struct ib_mr *mlx4_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				  struct ib_udata *udata);
 int mlx4_ib_dereg_mr(struct ib_mr *mr);
 struct ib_mw *mlx4_ib_alloc_mw(struct ib_pd *pd, enum ib_mw_type type);
-int mlx4_ib_bind_mw(struct ib_qp *qp, struct ib_mw *mw,
-		    struct ib_mw_bind *mw_bind);
 int mlx4_ib_dealloc_mw(struct ib_mw *mw);
 struct ib_mr *mlx4_ib_alloc_mr(struct ib_pd *pd,
 			       enum ib_mr_type mr_type,
diff --git a/drivers/infiniband/hw/mlx4/mr.c b/drivers/infiniband/hw/mlx4/mr.c
index 4d1e1c632603..242b94ec105b 100644
--- a/drivers/infiniband/hw/mlx4/mr.c
+++ b/drivers/infiniband/hw/mlx4/mr.c
@@ -366,28 +366,6 @@ err_free:
 	return ERR_PTR(err);
 }
 
-int mlx4_ib_bind_mw(struct ib_qp *qp, struct ib_mw *mw,
-		    struct ib_mw_bind *mw_bind)
-{
-	struct ib_bind_mw_wr  wr;
-	struct ib_send_wr *bad_wr;
-	int ret;
-
-	memset(&wr, 0, sizeof(wr));
-	wr.wr.opcode		= IB_WR_BIND_MW;
-	wr.wr.wr_id		= mw_bind->wr_id;
-	wr.wr.send_flags	= mw_bind->send_flags;
-	wr.mw			= mw;
-	wr.bind_info		= mw_bind->bind_info;
-	wr.rkey			= ib_inc_rkey(mw->rkey);
-
-	ret = mlx4_ib_post_send(qp, &wr.wr, &bad_wr);
-	if (!ret)
-		mw->rkey = wr.rkey;
-
-	return ret;
-}
-
 int mlx4_ib_dealloc_mw(struct ib_mw *ibmw)
 {
 	struct mlx4_ib_mw *mw = to_mmw(ibmw);
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 13eaaf45288f..fd97534762b8 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -32,6 +32,8 @@
  */
 
 #include <linux/log2.h>
+#include <linux/etherdevice.h>
+#include <net/ip.h>
 #include <linux/slab.h>
 #include <linux/netdevice.h>
 #include <linux/vmalloc.h>
@@ -85,6 +87,7 @@ struct mlx4_ib_sqp {
 	u32			send_psn;
 	struct ib_ud_header	ud_header;
 	u8			header_buf[MLX4_IB_UD_HEADER_SIZE];
+	struct ib_qp		*roce_v2_gsi;
 };
 
 enum {
@@ -115,7 +118,6 @@ static const __be32 mlx4_ib_opcode[] = {
 	[IB_WR_REG_MR]				= cpu_to_be32(MLX4_OPCODE_FMR),
 	[IB_WR_MASKED_ATOMIC_CMP_AND_SWP]	= cpu_to_be32(MLX4_OPCODE_MASKED_ATOMIC_CS),
 	[IB_WR_MASKED_ATOMIC_FETCH_AND_ADD]	= cpu_to_be32(MLX4_OPCODE_MASKED_ATOMIC_FA),
-	[IB_WR_BIND_MW]				= cpu_to_be32(MLX4_OPCODE_BIND_MW),
 };
 
 static struct mlx4_ib_sqp *to_msqp(struct mlx4_ib_qp *mqp)
@@ -154,7 +156,10 @@ static int is_sqp(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp)
 			}
 		}
 	}
-	return proxy_sqp;
+	if (proxy_sqp)
+		return 1;
+
+	return !!(qp->flags & MLX4_IB_ROCE_V2_GSI_QP);
 }
 
 /* used for INIT/CLOSE port logic */
@@ -796,11 +801,13 @@ static int create_qp_common(struct mlx4_ib_dev *dev, struct ib_pd *pd,
 		if (err)
 			goto err_mtt;
 
-		qp->sq.wrid = kmalloc(qp->sq.wqe_cnt * sizeof(u64), gfp);
+		qp->sq.wrid = kmalloc_array(qp->sq.wqe_cnt, sizeof(u64),
+					gfp | __GFP_NOWARN);
 		if (!qp->sq.wrid)
 			qp->sq.wrid = __vmalloc(qp->sq.wqe_cnt * sizeof(u64),
 						gfp, PAGE_KERNEL);
-		qp->rq.wrid = kmalloc(qp->rq.wqe_cnt * sizeof(u64), gfp);
+		qp->rq.wrid = kmalloc_array(qp->rq.wqe_cnt, sizeof(u64),
+					gfp | __GFP_NOWARN);
 		if (!qp->rq.wrid)
 			qp->rq.wrid = __vmalloc(qp->rq.wqe_cnt * sizeof(u64),
 						gfp, PAGE_KERNEL);
@@ -1099,9 +1106,9 @@ static u32 get_sqp_num(struct mlx4_ib_dev *dev, struct ib_qp_init_attr *attr)
 		return dev->dev->caps.qp1_proxy[attr->port_num - 1];
 }
 
-struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
-				struct ib_qp_init_attr *init_attr,
-				struct ib_udata *udata)
+static struct ib_qp *_mlx4_ib_create_qp(struct ib_pd *pd,
+					struct ib_qp_init_attr *init_attr,
+					struct ib_udata *udata)
 {
 	struct mlx4_ib_qp *qp = NULL;
 	int err;
@@ -1120,6 +1127,7 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 					MLX4_IB_SRIOV_TUNNEL_QP |
 					MLX4_IB_SRIOV_SQP |
 					MLX4_IB_QP_NETIF |
+					MLX4_IB_QP_CREATE_ROCE_V2_GSI |
 					MLX4_IB_QP_CREATE_USE_GFP_NOIO))
 		return ERR_PTR(-EINVAL);
 
@@ -1128,15 +1136,21 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 			return ERR_PTR(-EINVAL);
 	}
 
-	if (init_attr->create_flags &&
-	    ((udata && init_attr->create_flags & ~(sup_u_create_flags)) ||
-	     ((init_attr->create_flags & ~(MLX4_IB_SRIOV_SQP |
-					   MLX4_IB_QP_CREATE_USE_GFP_NOIO |
-					   MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK)) &&
-	      init_attr->qp_type != IB_QPT_UD) ||
-	     ((init_attr->create_flags & MLX4_IB_SRIOV_SQP) &&
-	      init_attr->qp_type > IB_QPT_GSI)))
-		return ERR_PTR(-EINVAL);
+	if (init_attr->create_flags) {
+		if (udata && init_attr->create_flags & ~(sup_u_create_flags))
+			return ERR_PTR(-EINVAL);
+
+		if ((init_attr->create_flags & ~(MLX4_IB_SRIOV_SQP |
+						 MLX4_IB_QP_CREATE_USE_GFP_NOIO |
+						 MLX4_IB_QP_CREATE_ROCE_V2_GSI  |
+						 MLX4_IB_QP_BLOCK_MULTICAST_LOOPBACK) &&
+		     init_attr->qp_type != IB_QPT_UD) ||
+		    (init_attr->create_flags & MLX4_IB_SRIOV_SQP &&
+		     init_attr->qp_type > IB_QPT_GSI) ||
+		    (init_attr->create_flags & MLX4_IB_QP_CREATE_ROCE_V2_GSI &&
+		     init_attr->qp_type != IB_QPT_GSI))
+			return ERR_PTR(-EINVAL);
+	}
 
 	switch (init_attr->qp_type) {
 	case IB_QPT_XRC_TGT:
@@ -1173,19 +1187,29 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	case IB_QPT_SMI:
 	case IB_QPT_GSI:
 	{
+		int sqpn;
+
 		/* Userspace is not allowed to create special QPs: */
 		if (udata)
 			return ERR_PTR(-EINVAL);
+		if (init_attr->create_flags & MLX4_IB_QP_CREATE_ROCE_V2_GSI) {
+			int res = mlx4_qp_reserve_range(to_mdev(pd->device)->dev, 1, 1, &sqpn, 0);
+
+			if (res)
+				return ERR_PTR(res);
+		} else {
+			sqpn = get_sqp_num(to_mdev(pd->device), init_attr);
+		}
 
 		err = create_qp_common(to_mdev(pd->device), pd, init_attr, udata,
-				       get_sqp_num(to_mdev(pd->device), init_attr),
+				       sqpn,
 				       &qp, gfp);
 		if (err)
 			return ERR_PTR(err);
 
 		qp->port	= init_attr->port_num;
-		qp->ibqp.qp_num = init_attr->qp_type == IB_QPT_SMI ? 0 : 1;
-
+		qp->ibqp.qp_num = init_attr->qp_type == IB_QPT_SMI ? 0 :
+			init_attr->create_flags & MLX4_IB_QP_CREATE_ROCE_V2_GSI ? sqpn : 1;
 		break;
 	}
 	default:
@@ -1196,7 +1220,41 @@ struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
 	return &qp->ibqp;
 }
 
-int mlx4_ib_destroy_qp(struct ib_qp *qp)
+struct ib_qp *mlx4_ib_create_qp(struct ib_pd *pd,
+				struct ib_qp_init_attr *init_attr,
+				struct ib_udata *udata) {
+	struct ib_device *device = pd ? pd->device : init_attr->xrcd->device;
+	struct ib_qp *ibqp;
+	struct mlx4_ib_dev *dev = to_mdev(device);
+
+	ibqp = _mlx4_ib_create_qp(pd, init_attr, udata);
+
+	if (!IS_ERR(ibqp) &&
+	    (init_attr->qp_type == IB_QPT_GSI) &&
+	    !(init_attr->create_flags & MLX4_IB_QP_CREATE_ROCE_V2_GSI)) {
+		struct mlx4_ib_sqp *sqp = to_msqp((to_mqp(ibqp)));
+		int is_eth = rdma_cap_eth_ah(&dev->ib_dev, init_attr->port_num);
+
+		if (is_eth &&
+		    dev->dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2) {
+			init_attr->create_flags |= MLX4_IB_QP_CREATE_ROCE_V2_GSI;
+			sqp->roce_v2_gsi = ib_create_qp(pd, init_attr);
+
+			if (IS_ERR(sqp->roce_v2_gsi)) {
+				pr_err("Failed to create GSI QP for RoCEv2 (%ld)\n", PTR_ERR(sqp->roce_v2_gsi));
+				sqp->roce_v2_gsi = NULL;
+			} else {
+				sqp = to_msqp(to_mqp(sqp->roce_v2_gsi));
+				sqp->qp.flags |= MLX4_IB_ROCE_V2_GSI_QP;
+			}
+
+			init_attr->create_flags &= ~MLX4_IB_QP_CREATE_ROCE_V2_GSI;
+		}
+	}
+	return ibqp;
+}
+
+static int _mlx4_ib_destroy_qp(struct ib_qp *qp)
 {
 	struct mlx4_ib_dev *dev = to_mdev(qp->device);
 	struct mlx4_ib_qp *mqp = to_mqp(qp);
@@ -1225,6 +1283,20 @@ int mlx4_ib_destroy_qp(struct ib_qp *qp)
 	return 0;
 }
 
+int mlx4_ib_destroy_qp(struct ib_qp *qp)
+{
+	struct mlx4_ib_qp *mqp = to_mqp(qp);
+
+	if (mqp->mlx4_ib_qp_type == MLX4_IB_QPT_GSI) {
+		struct mlx4_ib_sqp *sqp = to_msqp(mqp);
+
+		if (sqp->roce_v2_gsi)
+			ib_destroy_qp(sqp->roce_v2_gsi);
+	}
+
+	return _mlx4_ib_destroy_qp(qp);
+}
+
 static int to_mlx4_st(struct mlx4_ib_dev *dev, enum mlx4_ib_qp_type type)
 {
 	switch (type) {
@@ -1507,6 +1579,24 @@ static int create_qp_lb_counter(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp)
 	return 0;
 }
 
+enum {
+	MLX4_QPC_ROCE_MODE_1 = 0,
+	MLX4_QPC_ROCE_MODE_2 = 2,
+	MLX4_QPC_ROCE_MODE_UNDEFINED = 0xff
+};
+
+static u8 gid_type_to_qpc(enum ib_gid_type gid_type)
+{
+	switch (gid_type) {
+	case IB_GID_TYPE_ROCE:
+		return MLX4_QPC_ROCE_MODE_1;
+	case IB_GID_TYPE_ROCE_UDP_ENCAP:
+		return MLX4_QPC_ROCE_MODE_2;
+	default:
+		return MLX4_QPC_ROCE_MODE_UNDEFINED;
+	}
+}
+
 static int __mlx4_ib_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)
@@ -1591,9 +1681,12 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	}
 
 	if (qp->ibqp.uobject)
-		context->usr_page = cpu_to_be32(to_mucontext(ibqp->uobject->context)->uar.index);
+		context->usr_page = cpu_to_be32(
+			mlx4_to_hw_uar_index(dev->dev,
+					     to_mucontext(ibqp->uobject->context)->uar.index));
 	else
-		context->usr_page = cpu_to_be32(dev->priv_uar.index);
+		context->usr_page = cpu_to_be32(
+			mlx4_to_hw_uar_index(dev->dev, dev->priv_uar.index));
 
 	if (attr_mask & IB_QP_DEST_QPN)
 		context->remote_qpn = cpu_to_be32(attr->dest_qp_num);
@@ -1633,6 +1726,14 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 			mlx4_ib_steer_qp_reg(dev, qp, 1);
 			steer_qp = 1;
 		}
+
+		if (ibqp->qp_type == IB_QPT_GSI) {
+			enum ib_gid_type gid_type = qp->flags & MLX4_IB_ROCE_V2_GSI_QP ?
+				IB_GID_TYPE_ROCE_UDP_ENCAP : IB_GID_TYPE_ROCE;
+			u8 qpc_roce_mode = gid_type_to_qpc(gid_type);
+
+			context->rlkey_roce_mode |= (qpc_roce_mode << 6);
+		}
 	}
 
 	if (attr_mask & IB_QP_PKEY_INDEX) {
@@ -1650,9 +1751,10 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 		u16 vlan = 0xffff;
 		u8 smac[ETH_ALEN];
 		int status = 0;
+		int is_eth = rdma_cap_eth_ah(&dev->ib_dev, port_num) &&
+			attr->ah_attr.ah_flags & IB_AH_GRH;
 
-		if (rdma_cap_eth_ah(&dev->ib_dev, port_num) &&
-		    attr->ah_attr.ah_flags & IB_AH_GRH) {
+		if (is_eth) {
 			int index = attr->ah_attr.grh.sgid_index;
 
 			status = ib_get_cached_gid(ibqp->device, port_num,
@@ -1674,6 +1776,18 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 
 		optpar |= (MLX4_QP_OPTPAR_PRIMARY_ADDR_PATH |
 			   MLX4_QP_OPTPAR_SCHED_QUEUE);
+
+		if (is_eth &&
+		    (cur_state == IB_QPS_INIT && new_state == IB_QPS_RTR)) {
+			u8 qpc_roce_mode = gid_type_to_qpc(gid_attr.gid_type);
+
+			if (qpc_roce_mode == MLX4_QPC_ROCE_MODE_UNDEFINED) {
+				err = -EINVAL;
+				goto out;
+			}
+			context->rlkey_roce_mode |= (qpc_roce_mode << 6);
+		}
+
 	}
 
 	if (attr_mask & IB_QP_TIMEOUT) {
@@ -1845,7 +1959,7 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 		sqd_event = 0;
 
 	if (!ibqp->uobject && cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT)
-		context->rlkey |= (1 << 4);
+		context->rlkey_roce_mode |= (1 << 4);
 
 	/*
 	 * Before passing a kernel QP to the HW, make sure that the
@@ -2022,8 +2136,8 @@ out:
 	return err;
 }
 
-int mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-		      int attr_mask, struct ib_udata *udata)
+static int _mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+			      int attr_mask, struct ib_udata *udata)
 {
 	struct mlx4_ib_dev *dev = to_mdev(ibqp->device);
 	struct mlx4_ib_qp *qp = to_mqp(ibqp);
@@ -2126,6 +2240,27 @@ out:
 	return err;
 }
 
+int mlx4_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
+		      int attr_mask, struct ib_udata *udata)
+{
+	struct mlx4_ib_qp *mqp = to_mqp(ibqp);
+	int ret;
+
+	ret = _mlx4_ib_modify_qp(ibqp, attr, attr_mask, udata);
+
+	if (mqp->mlx4_ib_qp_type == MLX4_IB_QPT_GSI) {
+		struct mlx4_ib_sqp *sqp = to_msqp(mqp);
+		int err = 0;
+
+		if (sqp->roce_v2_gsi)
+			err = ib_modify_qp(sqp->roce_v2_gsi, attr, attr_mask);
+		if (err)
+			pr_err("Failed to modify GSI QP for RoCEv2 (%d)\n",
+			       err);
+	}
+	return ret;
+}
+
 static int vf_get_qp0_qkey(struct mlx4_dev *dev, int qpn, u32 *qkey)
 {
 	int i;
@@ -2168,7 +2303,7 @@ static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 	if (sqp->qp.mlx4_ib_qp_type == MLX4_IB_QPT_PROXY_SMI_OWNER)
 		send_size += sizeof (struct mlx4_ib_tunnel_header);
 
-	ib_ud_header_init(send_size, 1, 0, 0, 0, 0, &sqp->ud_header);
+	ib_ud_header_init(send_size, 1, 0, 0, 0, 0, 0, 0, &sqp->ud_header);
 
 	if (sqp->qp.mlx4_ib_qp_type == MLX4_IB_QPT_PROXY_SMI_OWNER) {
 		sqp->ud_header.lrh.service_level =
@@ -2252,16 +2387,7 @@ static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 	return 0;
 }
 
-static void mlx4_u64_to_smac(u8 *dst_mac, u64 src_mac)
-{
-	int i;
-
-	for (i = ETH_ALEN; i; i--) {
-		dst_mac[i - 1] = src_mac & 0xff;
-		src_mac >>= 8;
-	}
-}
-
+#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)
 {
@@ -2281,6 +2407,8 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 	bool is_eth;
 	bool is_vlan = false;
 	bool is_grh;
+	bool is_udp = false;
+	int ip_version = 0;
 
 	send_size = 0;
 	for (i = 0; i < wr->wr.num_sge; ++i)
@@ -2289,6 +2417,8 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 	is_eth = rdma_port_get_link_layer(sqp->qp.ibqp.device, sqp->qp.port) == IB_LINK_LAYER_ETHERNET;
 	is_grh = mlx4_ib_ah_grh_present(ah);
 	if (is_eth) {
+		struct ib_gid_attr gid_attr;
+
 		if (mlx4_is_mfunc(to_mdev(ib_dev)->dev)) {
 			/* When multi-function is enabled, the ib_core gid
 			 * indexes don't necessarily match the hw ones, so
@@ -2302,19 +2432,35 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 			err = ib_get_cached_gid(ib_dev,
 						be32_to_cpu(ah->av.ib.port_pd) >> 24,
 						ah->av.ib.gid_index, &sgid,
-						NULL);
-			if (!err && !memcmp(&sgid, &zgid, sizeof(sgid)))
-				err = -ENOENT;
-			if (err)
+						&gid_attr);
+			if (!err) {
+				if (gid_attr.ndev)
+					dev_put(gid_attr.ndev);
+				if (!memcmp(&sgid, &zgid, sizeof(sgid)))
+					err = -ENOENT;
+			}
+			if (!err) {
+				is_udp = gid_attr.gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP;
+				if (is_udp) {
+					if (ipv6_addr_v4mapped((struct in6_addr *)&sgid))
+						ip_version = 4;
+					else
+						ip_version = 6;
+					is_grh = false;
+				}
+			} else {
 				return err;
+			}
 		}
-
 		if (ah->av.eth.vlan != cpu_to_be16(0xffff)) {
 			vlan = be16_to_cpu(ah->av.eth.vlan) & 0x0fff;
 			is_vlan = 1;
 		}
 	}
-	ib_ud_header_init(send_size, !is_eth, is_eth, is_vlan, is_grh, 0, &sqp->ud_header);
+	err = ib_ud_header_init(send_size, !is_eth, is_eth, is_vlan, is_grh,
+			  ip_version, is_udp, 0, &sqp->ud_header);
+	if (err)
+		return err;
 
 	if (!is_eth) {
 		sqp->ud_header.lrh.service_level =
@@ -2323,7 +2469,7 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 		sqp->ud_header.lrh.source_lid = cpu_to_be16(ah->av.ib.g_slid & 0x7f);
 	}
 
-	if (is_grh) {
+	if (is_grh || (ip_version == 6)) {
 		sqp->ud_header.grh.traffic_class =
 			(be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 20) & 0xff;
 		sqp->ud_header.grh.flow_label    =
@@ -2352,6 +2498,25 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 		       ah->av.ib.dgid, 16);
 	}
 
+	if (ip_version == 4) {
+		sqp->ud_header.ip4.tos =
+			(be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 20) & 0xff;
+		sqp->ud_header.ip4.id = 0;
+		sqp->ud_header.ip4.frag_off = htons(IP_DF);
+		sqp->ud_header.ip4.ttl = ah->av.eth.hop_limit;
+
+		memcpy(&sqp->ud_header.ip4.saddr,
+		       sgid.raw + 12, 4);
+		memcpy(&sqp->ud_header.ip4.daddr, ah->av.ib.dgid + 12, 4);
+		sqp->ud_header.ip4.check = ib_ud_ip4_csum(&sqp->ud_header);
+	}
+
+	if (is_udp) {
+		sqp->ud_header.udp.dport = htons(ROCE_V2_UDP_DPORT);
+		sqp->ud_header.udp.sport = htons(MLX4_ROCEV2_QP1_SPORT);
+		sqp->ud_header.udp.csum = 0;
+	}
+
 	mlx->flags &= cpu_to_be32(MLX4_WQE_CTRL_CQ_UPDATE);
 
 	if (!is_eth) {
@@ -2380,34 +2545,27 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_ud_wr *wr,
 
 	if (is_eth) {
 		struct in6_addr in6;
-
+		u16 ether_type;
 		u16 pcp = (be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 29) << 13;
 
+		ether_type = (!is_udp) ? MLX4_IB_IBOE_ETHERTYPE :
+			(ip_version == 4 ? ETH_P_IP : ETH_P_IPV6);
+
 		mlx->sched_prio = cpu_to_be16(pcp);
 
+		ether_addr_copy(sqp->ud_header.eth.smac_h, ah->av.eth.s_mac);
 		memcpy(sqp->ud_header.eth.dmac_h, ah->av.eth.mac, 6);
-		/* FIXME: cache smac value? */
 		memcpy(&ctrl->srcrb_flags16[0], ah->av.eth.mac, 2);
 		memcpy(&ctrl->imm, ah->av.eth.mac + 2, 4);
 		memcpy(&in6, sgid.raw, sizeof(in6));
 
-		if (!mlx4_is_mfunc(to_mdev(ib_dev)->dev)) {
-			u64 mac = atomic64_read(&to_mdev(ib_dev)->iboe.mac[sqp->qp.port - 1]);
-			u8 smac[ETH_ALEN];
-
-			mlx4_u64_to_smac(smac, mac);
-			memcpy(sqp->ud_header.eth.smac_h, smac, ETH_ALEN);
-		} else {
-			/* use the src mac of the tunnel */
-			memcpy(sqp->ud_header.eth.smac_h, ah->av.eth.s_mac, ETH_ALEN);
-		}
 
 		if (!memcmp(sqp->ud_header.eth.smac_h, sqp->ud_header.eth.dmac_h, 6))
 			mlx->flags |= cpu_to_be32(MLX4_WQE_CTRL_FORCE_LOOPBACK);
 		if (!is_vlan) {
-			sqp->ud_header.eth.type = cpu_to_be16(MLX4_IB_IBOE_ETHERTYPE);
+			sqp->ud_header.eth.type = cpu_to_be16(ether_type);
 		} else {
-			sqp->ud_header.vlan.type = cpu_to_be16(MLX4_IB_IBOE_ETHERTYPE);
+			sqp->ud_header.vlan.type = cpu_to_be16(ether_type);
 			sqp->ud_header.vlan.tag = cpu_to_be16(vlan | pcp);
 		}
 	} else {
@@ -2528,25 +2686,6 @@ static void set_reg_seg(struct mlx4_wqe_fmr_seg *fseg,
 	fseg->reserved[1]	= 0;
 }
 
-static void set_bind_seg(struct mlx4_wqe_bind_seg *bseg,
-		struct ib_bind_mw_wr *wr)
-{
-	bseg->flags1 =
-		convert_access(wr->bind_info.mw_access_flags) &
-		cpu_to_be32(MLX4_WQE_FMR_AND_BIND_PERM_REMOTE_READ  |
-			    MLX4_WQE_FMR_AND_BIND_PERM_REMOTE_WRITE |
-			    MLX4_WQE_FMR_AND_BIND_PERM_ATOMIC);
-	bseg->flags2 = 0;
-	if (wr->mw->type == IB_MW_TYPE_2)
-		bseg->flags2 |= cpu_to_be32(MLX4_WQE_BIND_TYPE_2);
-	if (wr->bind_info.mw_access_flags & IB_ZERO_BASED)
-		bseg->flags2 |= cpu_to_be32(MLX4_WQE_BIND_ZERO_BASED);
-	bseg->new_rkey = cpu_to_be32(wr->rkey);
-	bseg->lkey = cpu_to_be32(wr->bind_info.mr->lkey);
-	bseg->addr = cpu_to_be64(wr->bind_info.addr);
-	bseg->length = cpu_to_be64(wr->bind_info.length);
-}
-
 static void set_local_inv_seg(struct mlx4_wqe_local_inval_seg *iseg, u32 rkey)
 {
 	memset(iseg, 0, sizeof(*iseg));
@@ -2766,6 +2905,29 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 	int i;
 	struct mlx4_ib_dev *mdev = to_mdev(ibqp->device);
 
+	if (qp->mlx4_ib_qp_type == MLX4_IB_QPT_GSI) {
+		struct mlx4_ib_sqp *sqp = to_msqp(qp);
+
+		if (sqp->roce_v2_gsi) {
+			struct mlx4_ib_ah *ah = to_mah(ud_wr(wr)->ah);
+			struct ib_gid_attr gid_attr;
+			union ib_gid gid;
+
+			if (!ib_get_cached_gid(ibqp->device,
+					       be32_to_cpu(ah->av.ib.port_pd) >> 24,
+					       ah->av.ib.gid_index, &gid,
+					       &gid_attr)) {
+				if (gid_attr.ndev)
+					dev_put(gid_attr.ndev);
+				qp = (gid_attr.gid_type == IB_GID_TYPE_ROCE_UDP_ENCAP) ?
+					to_mqp(sqp->roce_v2_gsi) : qp;
+			} else {
+				pr_err("Failed to get gid at index %d. RoCEv2 will not work properly\n",
+				       ah->av.ib.gid_index);
+			}
+		}
+	}
+
 	spin_lock_irqsave(&qp->sq.lock, flags);
 	if (mdev->dev->persist->state & MLX4_DEVICE_STATE_INTERNAL_ERROR) {
 		err = -EIO;
@@ -2867,13 +3029,6 @@ int mlx4_ib_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 				size += sizeof(struct mlx4_wqe_fmr_seg) / 16;
 				break;
 
-			case IB_WR_BIND_MW:
-				ctrl->srcrb_flags |=
-					cpu_to_be32(MLX4_WQE_CTRL_STRONG_ORDER);
-				set_bind_seg(wqe, bind_mw_wr(wr));
-				wqe  += sizeof(struct mlx4_wqe_bind_seg);
-				size += sizeof(struct mlx4_wqe_bind_seg) / 16;
-				break;
 			default:
 				/* No extra segments required for sends */
 				break;
diff --git a/drivers/infiniband/hw/mlx4/srq.c b/drivers/infiniband/hw/mlx4/srq.c
index c394376ebe06..0597f3eef5d0 100644
--- a/drivers/infiniband/hw/mlx4/srq.c
+++ b/drivers/infiniband/hw/mlx4/srq.c
@@ -171,7 +171,8 @@ struct ib_srq *mlx4_ib_create_srq(struct ib_pd *pd,
 		if (err)
 			goto err_mtt;
 
-		srq->wrid = kmalloc(srq->msrq.max * sizeof (u64), GFP_KERNEL);
+		srq->wrid = kmalloc_array(srq->msrq.max, sizeof(u64),
+					GFP_KERNEL | __GFP_NOWARN);
 		if (!srq->wrid) {
 			srq->wrid = __vmalloc(srq->msrq.max * sizeof(u64),
 					      GFP_KERNEL, PAGE_KERNEL);
diff --git a/drivers/infiniband/hw/mlx5/ah.c b/drivers/infiniband/hw/mlx5/ah.c
index 66080580e24d..745efa4cfc71 100644
--- a/drivers/infiniband/hw/mlx5/ah.c
+++ b/drivers/infiniband/hw/mlx5/ah.c
@@ -32,8 +32,10 @@
 
 #include "mlx5_ib.h"
 
-struct ib_ah *create_ib_ah(struct ib_ah_attr *ah_attr,
-			   struct mlx5_ib_ah *ah)
+static struct ib_ah *create_ib_ah(struct mlx5_ib_dev *dev,
+				  struct mlx5_ib_ah *ah,
+				  struct ib_ah_attr *ah_attr,
+				  enum rdma_link_layer ll)
 {
 	if (ah_attr->ah_flags & IB_AH_GRH) {
 		memcpy(ah->av.rgid, &ah_attr->grh.dgid, 16);
@@ -44,9 +46,20 @@ struct ib_ah *create_ib_ah(struct ib_ah_attr *ah_attr,
 		ah->av.tclass = ah_attr->grh.traffic_class;
 	}
 
-	ah->av.rlid = cpu_to_be16(ah_attr->dlid);
-	ah->av.fl_mlid = ah_attr->src_path_bits & 0x7f;
-	ah->av.stat_rate_sl = (ah_attr->static_rate << 4) | (ah_attr->sl & 0xf);
+	ah->av.stat_rate_sl = (ah_attr->static_rate << 4);
+
+	if (ll == IB_LINK_LAYER_ETHERNET) {
+		memcpy(ah->av.rmac, ah_attr->dmac, sizeof(ah_attr->dmac));
+		ah->av.udp_sport =
+			mlx5_get_roce_udp_sport(dev,
+						ah_attr->port_num,
+						ah_attr->grh.sgid_index);
+		ah->av.stat_rate_sl |= (ah_attr->sl & 0x7) << 1;
+	} else {
+		ah->av.rlid = cpu_to_be16(ah_attr->dlid);
+		ah->av.fl_mlid = ah_attr->src_path_bits & 0x7f;
+		ah->av.stat_rate_sl |= (ah_attr->sl & 0xf);
+	}
 
 	return &ah->ibah;
 }
@@ -54,12 +67,19 @@ struct ib_ah *create_ib_ah(struct ib_ah_attr *ah_attr,
 struct ib_ah *mlx5_ib_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr)
 {
 	struct mlx5_ib_ah *ah;
+	struct mlx5_ib_dev *dev = to_mdev(pd->device);
+	enum rdma_link_layer ll;
+
+	ll = pd->device->get_link_layer(pd->device, ah_attr->port_num);
+
+	if (ll == IB_LINK_LAYER_ETHERNET && !(ah_attr->ah_flags & IB_AH_GRH))
+		return ERR_PTR(-EINVAL);
 
 	ah = kzalloc(sizeof(*ah), GFP_ATOMIC);
 	if (!ah)
 		return ERR_PTR(-ENOMEM);
 
-	return create_ib_ah(ah_attr, ah); /* never fails */
+	return create_ib_ah(dev, ah, ah_attr, ll); /* never fails */
 }
 
 int mlx5_ib_query_ah(struct ib_ah *ibah, struct ib_ah_attr *ah_attr)
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index 3dfd287256d6..fd1de31e0611 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -154,9 +154,6 @@ static void handle_good_req(struct ib_wc *wc, struct mlx5_cqe64 *cqe,
 		wc->opcode    = IB_WC_MASKED_FETCH_ADD;
 		wc->byte_len  = 8;
 		break;
-	case MLX5_OPCODE_BIND_MW:
-		wc->opcode    = IB_WC_BIND_MW;
-		break;
 	case MLX5_OPCODE_UMR:
 		wc->opcode = get_umr_comp(wq, idx);
 		break;
@@ -171,6 +168,7 @@ enum {
 static void handle_responder(struct ib_wc *wc, struct mlx5_cqe64 *cqe,
 			     struct mlx5_ib_qp *qp)
 {
+	enum rdma_link_layer ll = rdma_port_get_link_layer(qp->ibqp.device, 1);
 	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.device);
 	struct mlx5_ib_srq *srq;
 	struct mlx5_ib_wq *wq;
@@ -236,6 +234,22 @@ static void handle_responder(struct ib_wc *wc, struct mlx5_cqe64 *cqe,
 	} else {
 		wc->pkey_index = 0;
 	}
+
+	if (ll != IB_LINK_LAYER_ETHERNET)
+		return;
+
+	switch (wc->sl & 0x3) {
+	case MLX5_CQE_ROCE_L3_HEADER_TYPE_GRH:
+		wc->network_hdr_type = RDMA_NETWORK_IB;
+		break;
+	case MLX5_CQE_ROCE_L3_HEADER_TYPE_IPV6:
+		wc->network_hdr_type = RDMA_NETWORK_IPV6;
+		break;
+	case MLX5_CQE_ROCE_L3_HEADER_TYPE_IPV4:
+		wc->network_hdr_type = RDMA_NETWORK_IPV4;
+		break;
+	}
+	wc->wc_flags |= IB_WC_WITH_NETWORK_HDR_TYPE;
 }
 
 static void dump_cqe(struct mlx5_ib_dev *dev, struct mlx5_err_cqe *cqe)
@@ -756,16 +770,16 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 	int uninitialized_var(index);
 	int uninitialized_var(inlen);
 	int cqe_size;
-	int irqn;
+	unsigned int irqn;
 	int eqn;
 	int err;
 
-	if (attr->flags)
-		return ERR_PTR(-EINVAL);
-
 	if (entries < 0)
 		return ERR_PTR(-EINVAL);
 
+	if (check_cq_create_flags(attr->flags))
+		return ERR_PTR(-EOPNOTSUPP);
+
 	entries = roundup_pow_of_two(entries + 1);
 	if (entries > (1 << MLX5_CAP_GEN(dev->mdev, log_max_cq_sz)))
 		return ERR_PTR(-EINVAL);
@@ -779,6 +793,7 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 	spin_lock_init(&cq->lock);
 	cq->resize_buf = NULL;
 	cq->resize_umem = NULL;
+	cq->create_flags = attr->flags;
 
 	if (context) {
 		err = create_cq_user(dev, udata, context, cq, entries,
@@ -796,6 +811,10 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 
 	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)
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index 7e97cb55a6bf..03c418ccbc98 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -40,9 +40,14 @@
 #include <linux/io-mapping.h>
 #include <linux/sched.h>
 #include <rdma/ib_user_verbs.h>
+#include <rdma/ib_addr.h>
+#include <rdma/ib_cache.h>
 #include <linux/mlx5/vport.h>
 #include <rdma/ib_smi.h>
 #include <rdma/ib_umem.h>
+#include <linux/in.h>
+#include <linux/etherdevice.h>
+#include <linux/mlx5/fs.h>
 #include "user.h"
 #include "mlx5_ib.h"
 
@@ -63,12 +68,14 @@ static char mlx5_version[] =
 	DRIVER_NAME ": Mellanox Connect-IB Infiniband driver v"
 	DRIVER_VERSION " (" DRIVER_RELDATE ")\n";
 
+enum {
+	MLX5_ATOMIC_SIZE_QP_8BYTES = 1 << 3,
+};
+
 static enum rdma_link_layer
-mlx5_ib_port_link_layer(struct ib_device *device)
+mlx5_port_type_cap_to_rdma_ll(int port_type_cap)
 {
-	struct mlx5_ib_dev *dev = to_mdev(device);
-
-	switch (MLX5_CAP_GEN(dev->mdev, port_type)) {
+	switch (port_type_cap) {
 	case MLX5_CAP_PORT_TYPE_IB:
 		return IB_LINK_LAYER_INFINIBAND;
 	case MLX5_CAP_PORT_TYPE_ETH:
@@ -78,6 +85,202 @@ mlx5_ib_port_link_layer(struct ib_device *device)
 	}
 }
 
+static enum rdma_link_layer
+mlx5_ib_port_link_layer(struct ib_device *device, u8 port_num)
+{
+	struct mlx5_ib_dev *dev = to_mdev(device);
+	int port_type_cap = MLX5_CAP_GEN(dev->mdev, port_type);
+
+	return mlx5_port_type_cap_to_rdma_ll(port_type_cap);
+}
+
+static int mlx5_netdev_event(struct notifier_block *this,
+			     unsigned long event, void *ptr)
+{
+	struct net_device *ndev = netdev_notifier_info_to_dev(ptr);
+	struct mlx5_ib_dev *ibdev = container_of(this, struct mlx5_ib_dev,
+						 roce.nb);
+
+	if ((event != NETDEV_UNREGISTER) && (event != NETDEV_REGISTER))
+		return NOTIFY_DONE;
+
+	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);
+
+	return NOTIFY_DONE;
+}
+
+static struct net_device *mlx5_ib_get_netdev(struct ib_device *device,
+					     u8 port_num)
+{
+	struct mlx5_ib_dev *ibdev = to_mdev(device);
+	struct net_device *ndev;
+
+	/* Ensure ndev does not disappear before we invoke dev_hold()
+	 */
+	read_lock(&ibdev->roce.netdev_lock);
+	ndev = ibdev->roce.netdev;
+	if (ndev)
+		dev_hold(ndev);
+	read_unlock(&ibdev->roce.netdev_lock);
+
+	return ndev;
+}
+
+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;
+	enum ib_mtu ndev_ib_mtu;
+	u16 qkey_viol_cntr;
+
+	memset(props, 0, sizeof(*props));
+
+	props->port_cap_flags  |= IB_PORT_CM_SUP;
+	props->port_cap_flags  |= IB_PORT_IP_BASED_GIDS;
+
+	props->gid_tbl_len      = MLX5_CAP_ROCE(dev->mdev,
+						roce_address_table_size);
+	props->max_mtu          = IB_MTU_4096;
+	props->max_msg_sz       = 1 << MLX5_CAP_GEN(dev->mdev, log_max_msg);
+	props->pkey_tbl_len     = 1;
+	props->state            = IB_PORT_DOWN;
+	props->phys_state       = 3;
+
+	mlx5_query_nic_vport_qkey_viol_cntr(dev->mdev, &qkey_viol_cntr);
+	props->qkey_viol_cntr = qkey_viol_cntr;
+
+	ndev = mlx5_ib_get_netdev(device, port_num);
+	if (!ndev)
+		return 0;
+
+	if (netif_running(ndev) && netif_carrier_ok(ndev)) {
+		props->state      = IB_PORT_ACTIVE;
+		props->phys_state = 5;
+	}
+
+	ndev_ib_mtu = iboe_get_mtu(ndev->mtu);
+
+	dev_put(ndev);
+
+	props->active_mtu	= min(props->max_mtu, ndev_ib_mtu);
+
+	props->active_width	= IB_WIDTH_4X;  /* TODO */
+	props->active_speed	= IB_SPEED_QDR; /* TODO */
+
+	return 0;
+}
+
+static void ib_gid_to_mlx5_roce_addr(const union ib_gid *gid,
+				     const struct ib_gid_attr *attr,
+				     void *mlx5_addr)
+{
+#define MLX5_SET_RA(p, f, v) MLX5_SET(roce_addr_layout, p, f, v)
+	char *mlx5_addr_l3_addr	= MLX5_ADDR_OF(roce_addr_layout, mlx5_addr,
+					       source_l3_address);
+	void *mlx5_addr_mac	= MLX5_ADDR_OF(roce_addr_layout, mlx5_addr,
+					       source_mac_47_32);
+
+	if (!gid)
+		return;
+
+	ether_addr_copy(mlx5_addr_mac, attr->ndev->dev_addr);
+
+	if (is_vlan_dev(attr->ndev)) {
+		MLX5_SET_RA(mlx5_addr, vlan_valid, 1);
+		MLX5_SET_RA(mlx5_addr, vlan_id, vlan_dev_vlan_id(attr->ndev));
+	}
+
+	switch (attr->gid_type) {
+	case IB_GID_TYPE_IB:
+		MLX5_SET_RA(mlx5_addr, roce_version, MLX5_ROCE_VERSION_1);
+		break;
+	case IB_GID_TYPE_ROCE_UDP_ENCAP:
+		MLX5_SET_RA(mlx5_addr, roce_version, MLX5_ROCE_VERSION_2);
+		break;
+
+	default:
+		WARN_ON(true);
+	}
+
+	if (attr->gid_type != IB_GID_TYPE_IB) {
+		if (ipv6_addr_v4mapped((void *)gid))
+			MLX5_SET_RA(mlx5_addr, roce_l3_type,
+				    MLX5_ROCE_L3_TYPE_IPV4);
+		else
+			MLX5_SET_RA(mlx5_addr, roce_l3_type,
+				    MLX5_ROCE_L3_TYPE_IPV6);
+	}
+
+	if ((attr->gid_type == IB_GID_TYPE_IB) ||
+	    !ipv6_addr_v4mapped((void *)gid))
+		memcpy(mlx5_addr_l3_addr, gid, sizeof(*gid));
+	else
+		memcpy(&mlx5_addr_l3_addr[12], &gid->raw[12], 4);
+}
+
+static int set_roce_addr(struct ib_device *device, u8 port_num,
+			 unsigned int index,
+			 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)];
+	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));
+}
+
+static int mlx5_ib_add_gid(struct ib_device *device, u8 port_num,
+			   unsigned int index, const union ib_gid *gid,
+			   const struct ib_gid_attr *attr,
+			   __always_unused void **context)
+{
+	return set_roce_addr(device, port_num, index, gid, attr);
+}
+
+static int mlx5_ib_del_gid(struct ib_device *device, u8 port_num,
+			   unsigned int index, __always_unused void **context)
+{
+	return set_roce_addr(device, port_num, index, NULL, NULL);
+}
+
+__be16 mlx5_get_roce_udp_sport(struct mlx5_ib_dev *dev, u8 port_num,
+			       int index)
+{
+	struct ib_gid_attr attr;
+	union ib_gid gid;
+
+	if (ib_get_cached_gid(&dev->ib_dev, port_num, index, &gid, &attr))
+		return 0;
+
+	if (!attr.ndev)
+		return 0;
+
+	dev_put(attr.ndev);
+
+	if (attr.gid_type != IB_GID_TYPE_ROCE_UDP_ENCAP)
+		return 0;
+
+	return cpu_to_be16(MLX5_CAP_ROCE(dev->mdev, r_roce_min_src_udp_port));
+}
+
 static int mlx5_use_mad_ifc(struct mlx5_ib_dev *dev)
 {
 	return !dev->mdev->issi;
@@ -94,13 +297,35 @@ static int mlx5_get_vport_access_method(struct ib_device *ibdev)
 	if (mlx5_use_mad_ifc(to_mdev(ibdev)))
 		return MLX5_VPORT_ACCESS_METHOD_MAD;
 
-	if (mlx5_ib_port_link_layer(ibdev) ==
+	if (mlx5_ib_port_link_layer(ibdev, 1) ==
 	    IB_LINK_LAYER_ETHERNET)
 		return MLX5_VPORT_ACCESS_METHOD_NIC;
 
 	return MLX5_VPORT_ACCESS_METHOD_HCA;
 }
 
+static void get_atomic_caps(struct mlx5_ib_dev *dev,
+			    struct ib_device_attr *props)
+{
+	u8 tmp;
+	u8 atomic_operations = MLX5_CAP_ATOMIC(dev->mdev, atomic_operations);
+	u8 atomic_size_qp = MLX5_CAP_ATOMIC(dev->mdev, atomic_size_qp);
+	u8 atomic_req_8B_endianness_mode =
+		MLX5_CAP_ATOMIC(dev->mdev, atomic_req_8B_endianess_mode);
+
+	/* Check if HW supports 8 bytes standard atomic operations and capable
+	 * of host endianness respond
+	 */
+	tmp = MLX5_ATOMIC_OPS_CMP_SWAP | MLX5_ATOMIC_OPS_FETCH_ADD;
+	if (((atomic_operations & tmp) == tmp) &&
+	    (atomic_size_qp & MLX5_ATOMIC_SIZE_QP_8BYTES) &&
+	    (atomic_req_8B_endianness_mode)) {
+		props->atomic_cap = IB_ATOMIC_HCA;
+	} else {
+		props->atomic_cap = IB_ATOMIC_NONE;
+	}
+}
+
 static int mlx5_query_system_image_guid(struct ib_device *ibdev,
 					__be64 *sys_image_guid)
 {
@@ -116,13 +341,21 @@ static int mlx5_query_system_image_guid(struct ib_device *ibdev,
 
 	case MLX5_VPORT_ACCESS_METHOD_HCA:
 		err = mlx5_query_hca_vport_system_image_guid(mdev, &tmp);
-		if (!err)
-			*sys_image_guid = cpu_to_be64(tmp);
-		return err;
+		break;
+
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		err = mlx5_query_nic_vport_system_image_guid(mdev, &tmp);
+		break;
 
 	default:
 		return -EINVAL;
 	}
+
+	if (!err)
+		*sys_image_guid = cpu_to_be64(tmp);
+
+	return err;
+
 }
 
 static int mlx5_query_max_pkeys(struct ib_device *ibdev,
@@ -176,13 +409,20 @@ static int mlx5_query_node_guid(struct mlx5_ib_dev *dev,
 
 	case MLX5_VPORT_ACCESS_METHOD_HCA:
 		err = mlx5_query_hca_vport_node_guid(dev->mdev, &tmp);
-		if (!err)
-			*node_guid = cpu_to_be64(tmp);
-		return err;
+		break;
+
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		err = mlx5_query_nic_vport_node_guid(dev->mdev, &tmp);
+		break;
 
 	default:
 		return -EINVAL;
 	}
+
+	if (!err)
+		*node_guid = cpu_to_be64(tmp);
+
+	return err;
 }
 
 struct mlx5_reg_node_desc {
@@ -260,6 +500,10 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	if (MLX5_CAP_GEN(mdev, block_lb_mc))
 		props->device_cap_flags |= IB_DEVICE_BLOCK_MULTICAST_LOOPBACK;
 
+	if (MLX5_CAP_GEN(dev->mdev, eth_net_offloads) &&
+	    (MLX5_CAP_ETH(dev->mdev, csum_cap)))
+			props->device_cap_flags |= IB_DEVICE_RAW_IP_CSUM;
+
 	props->vendor_part_id	   = mdev->pdev->device;
 	props->hw_ver		   = mdev->pdev->revision;
 
@@ -275,7 +519,7 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	props->max_sge = min(max_rq_sg, max_sq_sg);
 	props->max_sge_rd = props->max_sge;
 	props->max_cq		   = 1 << MLX5_CAP_GEN(mdev, log_max_cq);
-	props->max_cqe = (1 << MLX5_CAP_GEN(mdev, log_max_eq_sz)) - 1;
+	props->max_cqe = (1 << MLX5_CAP_GEN(mdev, log_max_cq_sz)) - 1;
 	props->max_mr		   = 1 << MLX5_CAP_GEN(mdev, log_max_mkey);
 	props->max_pd		   = 1 << MLX5_CAP_GEN(mdev, log_max_pd);
 	props->max_qp_rd_atom	   = 1 << MLX5_CAP_GEN(mdev, log_max_ra_req_qp);
@@ -286,13 +530,15 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	props->max_res_rd_atom	   = props->max_qp_rd_atom * props->max_qp;
 	props->max_srq_sge	   = max_rq_sg - 1;
 	props->max_fast_reg_page_list_len = (unsigned int)-1;
-	props->atomic_cap	   = IB_ATOMIC_NONE;
+	get_atomic_caps(dev, props);
 	props->masked_atomic_cap   = IB_ATOMIC_NONE;
 	props->max_mcast_grp	   = 1 << MLX5_CAP_GEN(mdev, log_max_mcg);
 	props->max_mcast_qp_attach = MLX5_CAP_GEN(mdev, max_qp_mcg);
 	props->max_total_mcast_qp_attach = props->max_mcast_qp_attach *
 					   props->max_mcast_grp;
 	props->max_map_per_fmr = INT_MAX; /* no limit in ConnectIB */
+	props->hca_core_clock = MLX5_CAP_GEN(mdev, device_frequency_khz);
+	props->timestamp_mask = 0x7FFFFFFFFFFFFFFFULL;
 
 #ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
 	if (MLX5_CAP_GEN(mdev, pg))
@@ -300,6 +546,9 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	props->odp_caps = dev->odp_caps;
 #endif
 
+	if (MLX5_CAP_GEN(mdev, cd))
+		props->device_cap_flags |= IB_DEVICE_CROSS_CHANNEL;
+
 	return 0;
 }
 
@@ -480,6 +729,9 @@ int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 	case MLX5_VPORT_ACCESS_METHOD_HCA:
 		return mlx5_query_hca_port(ibdev, port, props);
 
+	case MLX5_VPORT_ACCESS_METHOD_NIC:
+		return mlx5_query_port_roce(ibdev, port, props);
+
 	default:
 		return -EINVAL;
 	}
@@ -580,8 +832,8 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 						  struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
-	struct mlx5_ib_alloc_ucontext_req_v2 req;
-	struct mlx5_ib_alloc_ucontext_resp resp;
+	struct mlx5_ib_alloc_ucontext_req_v2 req = {};
+	struct mlx5_ib_alloc_ucontext_resp resp = {};
 	struct mlx5_ib_ucontext *context;
 	struct mlx5_uuar_info *uuari;
 	struct mlx5_uar *uars;
@@ -592,24 +844,28 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	int err;
 	int i;
 	size_t reqlen;
+	size_t min_req_v2 = offsetof(struct mlx5_ib_alloc_ucontext_req_v2,
+				     max_cqe_version);
 
 	if (!dev->ib_active)
 		return ERR_PTR(-EAGAIN);
 
-	memset(&req, 0, sizeof(req));
+	if (udata->inlen < sizeof(struct ib_uverbs_cmd_hdr))
+		return ERR_PTR(-EINVAL);
+
 	reqlen = udata->inlen - sizeof(struct ib_uverbs_cmd_hdr);
 	if (reqlen == sizeof(struct mlx5_ib_alloc_ucontext_req))
 		ver = 0;
-	else if (reqlen == sizeof(struct mlx5_ib_alloc_ucontext_req_v2))
+	else if (reqlen >= min_req_v2)
 		ver = 2;
 	else
 		return ERR_PTR(-EINVAL);
 
-	err = ib_copy_from_udata(&req, udata, reqlen);
+	err = ib_copy_from_udata(&req, udata, min(reqlen, sizeof(req)));
 	if (err)
 		return ERR_PTR(err);
 
-	if (req.flags || req.reserved)
+	if (req.flags)
 		return ERR_PTR(-EINVAL);
 
 	if (req.total_num_uuars > MLX5_MAX_UUARS)
@@ -618,6 +874,14 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	if (req.total_num_uuars == 0)
 		return ERR_PTR(-EINVAL);
 
+	if (req.comp_mask || req.reserved0 || req.reserved1 || req.reserved2)
+		return ERR_PTR(-EOPNOTSUPP);
+
+	if (reqlen > sizeof(req) &&
+	    !ib_is_udata_cleared(udata, sizeof(req),
+				 reqlen - sizeof(req)))
+		return ERR_PTR(-EOPNOTSUPP);
+
 	req.total_num_uuars = ALIGN(req.total_num_uuars,
 				    MLX5_NON_FP_BF_REGS_PER_PAGE);
 	if (req.num_low_latency_uuars > req.total_num_uuars - 1)
@@ -633,6 +897,11 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	resp.max_send_wqebb = 1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz);
 	resp.max_recv_wr = 1 << MLX5_CAP_GEN(dev->mdev, log_max_qp_sz);
 	resp.max_srq_recv_wr = 1 << MLX5_CAP_GEN(dev->mdev, log_max_srq_sz);
+	resp.cqe_version = min_t(__u8,
+				 (__u8)MLX5_CAP_GEN(dev->mdev, cqe_version),
+				 req.max_cqe_version);
+	resp.response_length = min(offsetof(typeof(resp), response_length) +
+				   sizeof(resp.response_length), udata->outlen);
 
 	context = kzalloc(sizeof(*context), GFP_KERNEL);
 	if (!context)
@@ -678,22 +947,49 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	context->ibucontext.invalidate_range = &mlx5_ib_invalidate_range;
 #endif
 
+	if (MLX5_CAP_GEN(dev->mdev, log_max_transport_domain)) {
+		err = mlx5_core_alloc_transport_domain(dev->mdev,
+						       &context->tdn);
+		if (err)
+			goto out_uars;
+	}
+
 	INIT_LIST_HEAD(&context->db_page_list);
 	mutex_init(&context->db_page_mutex);
 
 	resp.tot_uuars = req.total_num_uuars;
 	resp.num_ports = MLX5_CAP_GEN(dev->mdev, num_ports);
-	err = ib_copy_to_udata(udata, &resp,
-			       sizeof(resp) - sizeof(resp.reserved));
+
+	if (field_avail(typeof(resp), cqe_version, udata->outlen))
+		resp.response_length += sizeof(resp.cqe_version);
+
+	if (field_avail(typeof(resp), hca_core_clock_offset, udata->outlen)) {
+		resp.comp_mask |=
+			MLX5_IB_ALLOC_UCONTEXT_RESP_MASK_CORE_CLOCK_OFFSET;
+		resp.hca_core_clock_offset =
+			offsetof(struct mlx5_init_seg, internal_timer_h) %
+			PAGE_SIZE;
+		resp.response_length += sizeof(resp.hca_core_clock_offset) +
+					sizeof(resp.reserved2) +
+					sizeof(resp.reserved3);
+	}
+
+	err = ib_copy_to_udata(udata, &resp, resp.response_length);
 	if (err)
-		goto out_uars;
+		goto out_td;
 
 	uuari->ver = ver;
 	uuari->num_low_latency_uuars = req.num_low_latency_uuars;
 	uuari->uars = uars;
 	uuari->num_uars = num_uars;
+	context->cqe_version = resp.cqe_version;
+
 	return &context->ibucontext;
 
+out_td:
+	if (MLX5_CAP_GEN(dev->mdev, log_max_transport_domain))
+		mlx5_core_dealloc_transport_domain(dev->mdev, context->tdn);
+
 out_uars:
 	for (i--; i >= 0; i--)
 		mlx5_cmd_free_uar(dev->mdev, uars[i].index);
@@ -718,6 +1014,9 @@ static int mlx5_ib_dealloc_ucontext(struct ib_ucontext *ibcontext)
 	struct mlx5_uuar_info *uuari = &context->uuari;
 	int i;
 
+	if (MLX5_CAP_GEN(dev->mdev, log_max_transport_domain))
+		mlx5_core_dealloc_transport_domain(dev->mdev, context->tdn);
+
 	for (i = 0; i < uuari->num_uars; i++) {
 		if (mlx5_cmd_free_uar(dev->mdev, uuari->uars[i].index))
 			mlx5_ib_warn(dev, "failed to free UAR 0x%x\n", uuari->uars[i].index);
@@ -787,6 +1086,30 @@ static int mlx5_ib_mmap(struct ib_ucontext *ibcontext, struct vm_area_struct *vm
 	case MLX5_IB_MMAP_GET_CONTIGUOUS_PAGES:
 		return -ENOSYS;
 
+	case MLX5_IB_MMAP_CORE_CLOCK:
+		if (vma->vm_end - vma->vm_start != PAGE_SIZE)
+			return -EINVAL;
+
+		if (vma->vm_flags & (VM_WRITE | VM_EXEC))
+			return -EPERM;
+
+		/* Don't expose to user-space information it shouldn't have */
+		if (PAGE_SIZE > 4096)
+			return -EOPNOTSUPP;
+
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+		pfn = (dev->mdev->iseg_base +
+		       offsetof(struct mlx5_init_seg, internal_timer_h)) >>
+			PAGE_SHIFT;
+		if (io_remap_pfn_range(vma, vma->vm_start, pfn,
+				       PAGE_SIZE, vma->vm_page_prot))
+			return -EAGAIN;
+
+		mlx5_ib_dbg(dev, "mapped internal timer at 0x%lx, PA 0x%llx\n",
+			    vma->vm_start,
+			    (unsigned long long)pfn << PAGE_SHIFT);
+		break;
+
 	default:
 		return -EINVAL;
 	}
@@ -835,6 +1158,457 @@ static int mlx5_ib_dealloc_pd(struct ib_pd *pd)
 	return 0;
 }
 
+static bool outer_header_zero(u32 *match_criteria)
+{
+	int size = MLX5_ST_SZ_BYTES(fte_match_param);
+	char *outer_headers_c = MLX5_ADDR_OF(fte_match_param, match_criteria,
+					     outer_headers);
+
+	return outer_headers_c[0] == 0 && !memcmp(outer_headers_c,
+						  outer_headers_c + 1,
+						  size - 1);
+}
+
+static int parse_flow_attr(u32 *match_c, u32 *match_v,
+			   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);
+	switch (ib_spec->type) {
+	case IB_FLOW_SPEC_ETH:
+		if (ib_spec->size != sizeof(ib_spec->eth))
+			return -EINVAL;
+
+		ether_addr_copy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_c,
+					     dmac_47_16),
+				ib_spec->eth.mask.dst_mac);
+		ether_addr_copy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_v,
+					     dmac_47_16),
+				ib_spec->eth.val.dst_mac);
+
+		if (ib_spec->eth.mask.vlan_tag) {
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+				 vlan_tag, 1);
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v,
+				 vlan_tag, 1);
+
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+				 first_vid, ntohs(ib_spec->eth.mask.vlan_tag));
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v,
+				 first_vid, ntohs(ib_spec->eth.val.vlan_tag));
+
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+				 first_cfi,
+				 ntohs(ib_spec->eth.mask.vlan_tag) >> 12);
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v,
+				 first_cfi,
+				 ntohs(ib_spec->eth.val.vlan_tag) >> 12);
+
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+				 first_prio,
+				 ntohs(ib_spec->eth.mask.vlan_tag) >> 13);
+			MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v,
+				 first_prio,
+				 ntohs(ib_spec->eth.val.vlan_tag) >> 13);
+		}
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+			 ethertype, ntohs(ib_spec->eth.mask.ether_type));
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_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;
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c,
+			 ethertype, 0xffff);
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v,
+			 ethertype, ETH_P_IP);
+
+		memcpy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_c,
+				    src_ipv4_src_ipv6.ipv4_layout.ipv4),
+		       &ib_spec->ipv4.mask.src_ip,
+		       sizeof(ib_spec->ipv4.mask.src_ip));
+		memcpy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_v,
+				    src_ipv4_src_ipv6.ipv4_layout.ipv4),
+		       &ib_spec->ipv4.val.src_ip,
+		       sizeof(ib_spec->ipv4.val.src_ip));
+		memcpy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_c,
+				    dst_ipv4_dst_ipv6.ipv4_layout.ipv4),
+		       &ib_spec->ipv4.mask.dst_ip,
+		       sizeof(ib_spec->ipv4.mask.dst_ip));
+		memcpy(MLX5_ADDR_OF(fte_match_set_lyr_2_4, outer_headers_v,
+				    dst_ipv4_dst_ipv6.ipv4_layout.ipv4),
+		       &ib_spec->ipv4.val.dst_ip,
+		       sizeof(ib_spec->ipv4.val.dst_ip));
+		break;
+	case IB_FLOW_SPEC_TCP:
+		if (ib_spec->size != sizeof(ib_spec->tcp_udp))
+			return -EINVAL;
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, ip_protocol,
+			 0xff);
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, ip_protocol,
+			 IPPROTO_TCP);
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, tcp_sport,
+			 ntohs(ib_spec->tcp_udp.mask.src_port));
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, tcp_sport,
+			 ntohs(ib_spec->tcp_udp.val.src_port));
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, tcp_dport,
+			 ntohs(ib_spec->tcp_udp.mask.dst_port));
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, tcp_dport,
+			 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;
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, ip_protocol,
+			 0xff);
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, ip_protocol,
+			 IPPROTO_UDP);
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, udp_sport,
+			 ntohs(ib_spec->tcp_udp.mask.src_port));
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, udp_sport,
+			 ntohs(ib_spec->tcp_udp.val.src_port));
+
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_c, udp_dport,
+			 ntohs(ib_spec->tcp_udp.mask.dst_port));
+		MLX5_SET(fte_match_set_lyr_2_4, outer_headers_v, udp_dport,
+			 ntohs(ib_spec->tcp_udp.val.dst_port));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* If a flow could catch both multicast and unicast packets,
+ * it won't fall into the multicast flow steering table and this rule
+ * could steal other multicast packets.
+ */
+static bool flow_is_multicast_only(struct ib_flow_attr *ib_attr)
+{
+	struct ib_flow_spec_eth *eth_spec;
+
+	if (ib_attr->type != IB_FLOW_ATTR_NORMAL ||
+	    ib_attr->size < sizeof(struct ib_flow_attr) +
+	    sizeof(struct ib_flow_spec_eth) ||
+	    ib_attr->num_of_specs < 1)
+		return false;
+
+	eth_spec = (struct ib_flow_spec_eth *)(ib_attr + 1);
+	if (eth_spec->type != IB_FLOW_SPEC_ETH ||
+	    eth_spec->size != sizeof(*eth_spec))
+		return false;
+
+	return is_multicast_ether_addr(eth_spec->mask.dst_mac) &&
+	       is_multicast_ether_addr(eth_spec->val.dst_mac);
+}
+
+static bool is_valid_attr(struct ib_flow_attr *flow_attr)
+{
+	union ib_flow_spec *ib_spec = (union ib_flow_spec *)(flow_attr + 1);
+	bool has_ipv4_spec = false;
+	bool eth_type_ipv4 = true;
+	unsigned int spec_index;
+
+	/* Validate that ethertype is correct */
+	for (spec_index = 0; spec_index < flow_attr->num_of_specs; spec_index++) {
+		if (ib_spec->type == IB_FLOW_SPEC_ETH &&
+		    ib_spec->eth.mask.ether_type) {
+			if (!((ib_spec->eth.mask.ether_type == htons(0xffff)) &&
+			      ib_spec->eth.val.ether_type == htons(ETH_P_IP)))
+				eth_type_ipv4 = false;
+		} else if (ib_spec->type == IB_FLOW_SPEC_IPV4) {
+			has_ipv4_spec = true;
+		}
+		ib_spec = (void *)ib_spec + ib_spec->size;
+	}
+	return !has_ipv4_spec || eth_type_ipv4;
+}
+
+static void put_flow_table(struct mlx5_ib_dev *dev,
+			   struct mlx5_ib_flow_prio *prio, bool ft_added)
+{
+	prio->refcount -= !!ft_added;
+	if (!prio->refcount) {
+		mlx5_destroy_flow_table(prio->flow_table);
+		prio->flow_table = NULL;
+	}
+}
+
+static int mlx5_ib_destroy_flow(struct ib_flow *flow_id)
+{
+	struct mlx5_ib_dev *dev = to_mdev(flow_id->qp->device);
+	struct mlx5_ib_flow_handler *handler = container_of(flow_id,
+							  struct mlx5_ib_flow_handler,
+							  ibflow);
+	struct mlx5_ib_flow_handler *iter, *tmp;
+
+	mutex_lock(&dev->flow_db.lock);
+
+	list_for_each_entry_safe(iter, tmp, &handler->list, list) {
+		mlx5_del_flow_rule(iter->rule);
+		list_del(&iter->list);
+		kfree(iter);
+	}
+
+	mlx5_del_flow_rule(handler->rule);
+	put_flow_table(dev, &dev->flow_db.prios[handler->prio], true);
+	mutex_unlock(&dev->flow_db.lock);
+
+	kfree(handler);
+
+	return 0;
+}
+
+#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 mlx5_flow_namespace *ns = NULL;
+	struct mlx5_ib_flow_prio *prio;
+	struct mlx5_flow_table *ft;
+	int num_entries;
+	int num_groups;
+	int priority;
+	int err = 0;
+
+	if (flow_attr->type == IB_FLOW_ATTR_NORMAL) {
+		if (flow_is_multicast_only(flow_attr))
+			priority = MLX5_IB_FLOW_MCAST_PRIO;
+		else
+			priority = flow_attr->priority;
+		ns = mlx5_get_flow_namespace(dev->mdev,
+					     MLX5_FLOW_NAMESPACE_BYPASS);
+		num_entries = MLX5_FS_MAX_ENTRIES;
+		num_groups = MLX5_FS_MAX_TYPES;
+		prio = &dev->flow_db.prios[priority];
+	} else if (flow_attr->type == IB_FLOW_ATTR_ALL_DEFAULT ||
+		   flow_attr->type == IB_FLOW_ATTR_MC_DEFAULT) {
+		ns = mlx5_get_flow_namespace(dev->mdev,
+					     MLX5_FLOW_NAMESPACE_LEFTOVERS);
+		build_leftovers_ft_param(&priority,
+					 &num_entries,
+					 &num_groups);
+		prio = &dev->flow_db.prios[MLX5_IB_FLOW_LEFTOVERS_PRIO];
+	}
+
+	if (!ns)
+		return ERR_PTR(-ENOTSUPP);
+
+	ft = prio->flow_table;
+	if (!ft) {
+		ft = mlx5_create_auto_grouped_flow_table(ns, priority,
+							 num_entries,
+							 num_groups);
+
+		if (!IS_ERR(ft)) {
+			prio->refcount = 0;
+			prio->flow_table = ft;
+		} else {
+			err = PTR_ERR(ft);
+		}
+	}
+
+	return err ? ERR_PTR(err) : prio;
+}
+
+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,
+						     struct mlx5_flow_destination *dst)
+{
+	struct mlx5_flow_table	*ft = ft_prio->flow_table;
+	struct mlx5_ib_flow_handler *handler;
+	void *ib_flow = flow_attr + 1;
+	u8 match_criteria_enable = 0;
+	unsigned int spec_index;
+	u32 *match_c;
+	u32 *match_v;
+	int err = 0;
+
+	if (!is_valid_attr(flow_attr))
+		return ERR_PTR(-EINVAL);
+
+	match_c = kzalloc(MLX5_ST_SZ_BYTES(fte_match_param), GFP_KERNEL);
+	match_v = kzalloc(MLX5_ST_SZ_BYTES(fte_match_param), GFP_KERNEL);
+	handler = kzalloc(sizeof(*handler), GFP_KERNEL);
+	if (!handler || !match_c || !match_v) {
+		err = -ENOMEM;
+		goto free;
+	}
+
+	INIT_LIST_HEAD(&handler->list);
+
+	for (spec_index = 0; spec_index < flow_attr->num_of_specs; spec_index++) {
+		err = parse_flow_attr(match_c, match_v, ib_flow);
+		if (err < 0)
+			goto free;
+
+		ib_flow += ((union ib_flow_spec *)ib_flow)->size;
+	}
+
+	/* Outer header support only */
+	match_criteria_enable = (!outer_header_zero(match_c)) << 0;
+	handler->rule = mlx5_add_flow_rule(ft, match_criteria_enable,
+					   match_c, match_v,
+					   MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					   MLX5_FS_DEFAULT_FLOW_TAG,
+					   dst);
+
+	if (IS_ERR(handler->rule)) {
+		err = PTR_ERR(handler->rule);
+		goto free;
+	}
+
+	handler->prio = ft_prio - dev->flow_db.prios;
+
+	ft_prio->flow_table = ft;
+free:
+	if (err)
+		kfree(handler);
+	kfree(match_c);
+	kfree(match_v);
+	return err ? ERR_PTR(err) : handler;
+}
+
+enum {
+	LEFTOVERS_MC,
+	LEFTOVERS_UC,
+};
+
+static struct mlx5_ib_flow_handler *create_leftovers_rule(struct mlx5_ib_dev *dev,
+							  struct mlx5_ib_flow_prio *ft_prio,
+							  struct ib_flow_attr *flow_attr,
+							  struct mlx5_flow_destination *dst)
+{
+	struct mlx5_ib_flow_handler *handler_ucast = NULL;
+	struct mlx5_ib_flow_handler *handler = NULL;
+
+	static struct {
+		struct ib_flow_attr	flow_attr;
+		struct ib_flow_spec_eth eth_flow;
+	} leftovers_specs[] = {
+		[LEFTOVERS_MC] = {
+			.flow_attr = {
+				.num_of_specs = 1,
+				.size = sizeof(leftovers_specs[0])
+			},
+			.eth_flow = {
+				.type = IB_FLOW_SPEC_ETH,
+				.size = sizeof(struct ib_flow_spec_eth),
+				.mask = {.dst_mac = {0x1} },
+				.val =  {.dst_mac = {0x1} }
+			}
+		},
+		[LEFTOVERS_UC] = {
+			.flow_attr = {
+				.num_of_specs = 1,
+				.size = sizeof(leftovers_specs[0])
+			},
+			.eth_flow = {
+				.type = IB_FLOW_SPEC_ETH,
+				.size = sizeof(struct ib_flow_spec_eth),
+				.mask = {.dst_mac = {0x1} },
+				.val = {.dst_mac = {} }
+			}
+		}
+	};
+
+	handler = create_flow_rule(dev, ft_prio,
+				   &leftovers_specs[LEFTOVERS_MC].flow_attr,
+				   dst);
+	if (!IS_ERR(handler) &&
+	    flow_attr->type == IB_FLOW_ATTR_ALL_DEFAULT) {
+		handler_ucast = create_flow_rule(dev, ft_prio,
+						 &leftovers_specs[LEFTOVERS_UC].flow_attr,
+						 dst);
+		if (IS_ERR(handler_ucast)) {
+			kfree(handler);
+			handler = handler_ucast;
+		} else {
+			list_add(&handler_ucast->list, &handler->list);
+		}
+	}
+
+	return handler;
+}
+
+static struct ib_flow *mlx5_ib_create_flow(struct ib_qp *qp,
+					   struct ib_flow_attr *flow_attr,
+					   int domain)
+{
+	struct mlx5_ib_dev *dev = to_mdev(qp->device);
+	struct mlx5_ib_flow_handler *handler = NULL;
+	struct mlx5_flow_destination *dst = NULL;
+	struct mlx5_ib_flow_prio *ft_prio;
+	int err;
+
+	if (flow_attr->priority > MLX5_IB_FLOW_LAST_PRIO)
+		return ERR_PTR(-ENOSPC);
+
+	if (domain != IB_FLOW_DOMAIN_USER ||
+	    flow_attr->port > MLX5_CAP_GEN(dev->mdev, num_ports) ||
+	    flow_attr->flags)
+		return ERR_PTR(-EINVAL);
+
+	dst = kzalloc(sizeof(*dst), GFP_KERNEL);
+	if (!dst)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&dev->flow_db.lock);
+
+	ft_prio = get_flow_table(dev, flow_attr);
+	if (IS_ERR(ft_prio)) {
+		err = PTR_ERR(ft_prio);
+		goto unlock;
+	}
+
+	dst->type = MLX5_FLOW_DESTINATION_TYPE_TIR;
+	dst->tir_num = to_mqp(qp)->raw_packet_qp.rq.tirn;
+
+	if (flow_attr->type == IB_FLOW_ATTR_NORMAL) {
+		handler = create_flow_rule(dev, ft_prio, flow_attr,
+					   dst);
+	} else if (flow_attr->type == IB_FLOW_ATTR_ALL_DEFAULT ||
+		   flow_attr->type == IB_FLOW_ATTR_MC_DEFAULT) {
+		handler = create_leftovers_rule(dev, ft_prio, flow_attr,
+						dst);
+	} else {
+		err = -EINVAL;
+		goto destroy_ft;
+	}
+
+	if (IS_ERR(handler)) {
+		err = PTR_ERR(handler);
+		handler = NULL;
+		goto destroy_ft;
+	}
+
+	ft_prio->refcount++;
+	mutex_unlock(&dev->flow_db.lock);
+	kfree(dst);
+
+	return &handler->ibflow;
+
+destroy_ft:
+	put_flow_table(dev, ft_prio, false);
+unlock:
+	mutex_unlock(&dev->flow_db.lock);
+	kfree(dst);
+	kfree(handler);
+	return ERR_PTR(err);
+}
+
 static int mlx5_ib_mcg_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibqp->device);
@@ -1304,6 +2078,32 @@ static void destroy_dev_resources(struct mlx5_ib_resources *devr)
 	mlx5_ib_dealloc_pd(devr->p0);
 }
 
+static u32 get_core_cap_flags(struct ib_device *ibdev)
+{
+	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	enum rdma_link_layer ll = mlx5_ib_port_link_layer(ibdev, 1);
+	u8 l3_type_cap = MLX5_CAP_ROCE(dev->mdev, l3_type);
+	u8 roce_version_cap = MLX5_CAP_ROCE(dev->mdev, roce_version);
+	u32 ret = 0;
+
+	if (ll == IB_LINK_LAYER_INFINIBAND)
+		return RDMA_CORE_PORT_IBA_IB;
+
+	if (!(l3_type_cap & MLX5_ROCE_L3_TYPE_IPV4_CAP))
+		return 0;
+
+	if (!(l3_type_cap & MLX5_ROCE_L3_TYPE_IPV6_CAP))
+		return 0;
+
+	if (roce_version_cap & MLX5_ROCE_VERSION_1_CAP)
+		ret |= RDMA_CORE_PORT_IBA_ROCE;
+
+	if (roce_version_cap & MLX5_ROCE_VERSION_2_CAP)
+		ret |= RDMA_CORE_PORT_IBA_ROCE_UDP_ENCAP;
+
+	return ret;
+}
+
 static int mlx5_port_immutable(struct ib_device *ibdev, u8 port_num,
 			       struct ib_port_immutable *immutable)
 {
@@ -1316,20 +2116,50 @@ static int mlx5_port_immutable(struct ib_device *ibdev, u8 port_num,
 
 	immutable->pkey_tbl_len = attr.pkey_tbl_len;
 	immutable->gid_tbl_len = attr.gid_tbl_len;
-	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
+	immutable->core_cap_flags = get_core_cap_flags(ibdev);
 	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
 
 	return 0;
 }
 
+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)
+		return err;
+
+	err = mlx5_nic_vport_enable_roce(dev->mdev);
+	if (err)
+		goto err_unregister_netdevice_notifier;
+
+	return 0;
+
+err_unregister_netdevice_notifier:
+	unregister_netdevice_notifier(&dev->roce.nb);
+	return err;
+}
+
+static void mlx5_disable_roce(struct mlx5_ib_dev *dev)
+{
+	mlx5_nic_vport_disable_roce(dev->mdev);
+	unregister_netdevice_notifier(&dev->roce.nb);
+}
+
 static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 {
 	struct mlx5_ib_dev *dev;
+	enum rdma_link_layer ll;
+	int port_type_cap;
 	int err;
 	int i;
 
-	/* don't create IB instance over Eth ports, no RoCE yet! */
-	if (MLX5_CAP_GEN(mdev, port_type) == MLX5_CAP_PORT_TYPE_ETH)
+	port_type_cap = MLX5_CAP_GEN(mdev, port_type);
+	ll = mlx5_port_type_cap_to_rdma_ll(port_type_cap);
+
+	if ((ll == IB_LINK_LAYER_ETHERNET) && !MLX5_CAP_GEN(mdev, roce))
 		return NULL;
 
 	printk_once(KERN_INFO "%s", mlx5_version);
@@ -1340,6 +2170,7 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 
 	dev->mdev = mdev;
 
+	rwlock_init(&dev->roce.netdev_lock);
 	err = get_port_caps(dev);
 	if (err)
 		goto err_dealloc;
@@ -1385,11 +2216,18 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 		(1ull << IB_USER_VERBS_CMD_CREATE_XSRQ)		|
 		(1ull << IB_USER_VERBS_CMD_OPEN_QP);
 	dev->ib_dev.uverbs_ex_cmd_mask =
-		(1ull << IB_USER_VERBS_EX_CMD_QUERY_DEVICE);
+		(1ull << IB_USER_VERBS_EX_CMD_QUERY_DEVICE)	|
+		(1ull << IB_USER_VERBS_EX_CMD_CREATE_CQ)	|
+		(1ull << IB_USER_VERBS_EX_CMD_CREATE_QP);
 
 	dev->ib_dev.query_device	= mlx5_ib_query_device;
 	dev->ib_dev.query_port		= mlx5_ib_query_port;
+	dev->ib_dev.get_link_layer	= mlx5_ib_port_link_layer;
+	if (ll == IB_LINK_LAYER_ETHERNET)
+		dev->ib_dev.get_netdev	= mlx5_ib_get_netdev;
 	dev->ib_dev.query_gid		= mlx5_ib_query_gid;
+	dev->ib_dev.add_gid		= mlx5_ib_add_gid;
+	dev->ib_dev.del_gid		= mlx5_ib_del_gid;
 	dev->ib_dev.query_pkey		= mlx5_ib_query_pkey;
 	dev->ib_dev.modify_device	= mlx5_ib_modify_device;
 	dev->ib_dev.modify_port		= mlx5_ib_modify_port;
@@ -1439,15 +2277,30 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 			(1ull << IB_USER_VERBS_CMD_CLOSE_XRCD);
 	}
 
+	if (mlx5_ib_port_link_layer(&dev->ib_dev, 1) ==
+	    IB_LINK_LAYER_ETHERNET) {
+		dev->ib_dev.create_flow	= mlx5_ib_create_flow;
+		dev->ib_dev.destroy_flow = mlx5_ib_destroy_flow;
+		dev->ib_dev.uverbs_ex_cmd_mask |=
+			(1ull << IB_USER_VERBS_EX_CMD_CREATE_FLOW) |
+			(1ull << IB_USER_VERBS_EX_CMD_DESTROY_FLOW);
+	}
 	err = init_node_data(dev);
 	if (err)
 		goto err_dealloc;
 
+	mutex_init(&dev->flow_db.lock);
 	mutex_init(&dev->cap_mask_mutex);
 
+	if (ll == IB_LINK_LAYER_ETHERNET) {
+		err = mlx5_enable_roce(dev);
+		if (err)
+			goto err_dealloc;
+	}
+
 	err = create_dev_resources(&dev->devr);
 	if (err)
-		goto err_dealloc;
+		goto err_disable_roce;
 
 	err = mlx5_ib_odp_init_one(dev);
 	if (err)
@@ -1484,6 +2337,10 @@ err_odp:
 err_rsrc:
 	destroy_dev_resources(&dev->devr);
 
+err_disable_roce:
+	if (ll == IB_LINK_LAYER_ETHERNET)
+		mlx5_disable_roce(dev);
+
 err_dealloc:
 	ib_dealloc_device((struct ib_device *)dev);
 
@@ -1493,11 +2350,14 @@ err_dealloc:
 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);
 
 	ib_unregister_device(&dev->ib_dev);
 	destroy_umrc_res(dev);
 	mlx5_ib_odp_remove_one(dev);
 	destroy_dev_resources(&dev->devr);
+	if (ll == IB_LINK_LAYER_ETHERNET)
+		mlx5_disable_roce(dev);
 	ib_dealloc_device(&dev->ib_dev);
 }
 
diff --git a/drivers/infiniband/hw/mlx5/mlx5_ib.h b/drivers/infiniband/hw/mlx5/mlx5_ib.h
index 633347260b79..d2b9737baa36 100644
--- a/drivers/infiniband/hw/mlx5/mlx5_ib.h
+++ b/drivers/infiniband/hw/mlx5/mlx5_ib.h
@@ -42,6 +42,7 @@
 #include <linux/mlx5/qp.h>
 #include <linux/mlx5/srq.h>
 #include <linux/types.h>
+#include <linux/mlx5/transobj.h>
 
 #define mlx5_ib_dbg(dev, format, arg...)				\
 pr_debug("%s:%s:%d:(pid %d): " format, (dev)->ib_dev.name, __func__,	\
@@ -55,6 +56,11 @@ pr_err("%s:%s:%d:(pid %d): " format, (dev)->ib_dev.name, __func__,	\
 pr_warn("%s:%s:%d:(pid %d): " format, (dev)->ib_dev.name, __func__,	\
 	__LINE__, current->pid, ##arg)
 
+#define field_avail(type, fld, sz) (offsetof(type, fld) +		\
+				    sizeof(((type *)0)->fld) <= (sz))
+#define MLX5_IB_DEFAULT_UIDX 0xffffff
+#define MLX5_USER_ASSIGNED_UIDX_MASK __mlx5_mask(qpc, user_index)
+
 enum {
 	MLX5_IB_MMAP_CMD_SHIFT	= 8,
 	MLX5_IB_MMAP_CMD_MASK	= 0xff,
@@ -62,7 +68,9 @@ enum {
 
 enum mlx5_ib_mmap_cmd {
 	MLX5_IB_MMAP_REGULAR_PAGE		= 0,
-	MLX5_IB_MMAP_GET_CONTIGUOUS_PAGES	= 1, /* always last */
+	MLX5_IB_MMAP_GET_CONTIGUOUS_PAGES	= 1,
+	/* 5 is chosen in order to be compatible with old versions of libmlx5 */
+	MLX5_IB_MMAP_CORE_CLOCK			= 5,
 };
 
 enum {
@@ -85,6 +93,15 @@ enum mlx5_ib_mad_ifc_flags {
 	MLX5_MAD_IFC_NET_VIEW		= 4,
 };
 
+enum {
+	MLX5_CROSS_CHANNEL_UUAR         = 0,
+};
+
+enum {
+	MLX5_CQE_VERSION_V0,
+	MLX5_CQE_VERSION_V1,
+};
+
 struct mlx5_ib_ucontext {
 	struct ib_ucontext	ibucontext;
 	struct list_head	db_page_list;
@@ -93,6 +110,9 @@ struct mlx5_ib_ucontext {
 	 */
 	struct mutex		db_page_mutex;
 	struct mlx5_uuar_info	uuari;
+	u8			cqe_version;
+	/* Transport Domain number */
+	u32			tdn;
 };
 
 static inline struct mlx5_ib_ucontext *to_mucontext(struct ib_ucontext *ibucontext)
@@ -105,6 +125,36 @@ struct mlx5_ib_pd {
 	u32			pdn;
 };
 
+#define MLX5_IB_FLOW_MCAST_PRIO		(MLX5_BY_PASS_NUM_PRIOS - 1)
+#define MLX5_IB_FLOW_LAST_PRIO		(MLX5_IB_FLOW_MCAST_PRIO - 1)
+#if (MLX5_IB_FLOW_LAST_PRIO <= 0)
+#error "Invalid number of bypass priorities"
+#endif
+#define MLX5_IB_FLOW_LEFTOVERS_PRIO	(MLX5_IB_FLOW_MCAST_PRIO + 1)
+
+#define MLX5_IB_NUM_FLOW_FT		(MLX5_IB_FLOW_LEFTOVERS_PRIO + 1)
+struct mlx5_ib_flow_prio {
+	struct mlx5_flow_table		*flow_table;
+	unsigned int			refcount;
+};
+
+struct mlx5_ib_flow_handler {
+	struct list_head		list;
+	struct ib_flow			ibflow;
+	unsigned int			prio;
+	struct mlx5_flow_rule	*rule;
+};
+
+struct mlx5_ib_flow_db {
+	struct mlx5_ib_flow_prio	prios[MLX5_IB_NUM_FLOW_FT];
+	/* Protect flow steering bypass flow tables
+	 * when add/del flow rules.
+	 * only single add/removal of flow steering rule could be done
+	 * simultaneously.
+	 */
+	struct mutex			lock;
+};
+
 /* Use macros here so that don't have to duplicate
  * enum ib_send_flags and enum ib_qp_type for low-level driver
  */
@@ -171,35 +221,70 @@ struct mlx5_ib_pfault {
 	struct mlx5_pagefault	mpfault;
 };
 
+struct mlx5_ib_ubuffer {
+	struct ib_umem	       *umem;
+	int			buf_size;
+	u64			buf_addr;
+};
+
+struct mlx5_ib_qp_base {
+	struct mlx5_ib_qp	*container_mibqp;
+	struct mlx5_core_qp	mqp;
+	struct mlx5_ib_ubuffer	ubuffer;
+};
+
+struct mlx5_ib_qp_trans {
+	struct mlx5_ib_qp_base	base;
+	u16			xrcdn;
+	u8			alt_port;
+	u8			atomic_rd_en;
+	u8			resp_depth;
+};
+
+struct mlx5_ib_rq {
+	struct mlx5_ib_qp_base base;
+	struct mlx5_ib_wq	*rq;
+	struct mlx5_ib_ubuffer	ubuffer;
+	struct mlx5_db		*doorbell;
+	u32			tirn;
+	u8			state;
+};
+
+struct mlx5_ib_sq {
+	struct mlx5_ib_qp_base base;
+	struct mlx5_ib_wq	*sq;
+	struct mlx5_ib_ubuffer  ubuffer;
+	struct mlx5_db		*doorbell;
+	u32			tisn;
+	u8			state;
+};
+
+struct mlx5_ib_raw_packet_qp {
+	struct mlx5_ib_sq sq;
+	struct mlx5_ib_rq rq;
+};
+
 struct mlx5_ib_qp {
 	struct ib_qp		ibqp;
-	struct mlx5_core_qp	mqp;
+	union {
+		struct mlx5_ib_qp_trans trans_qp;
+		struct mlx5_ib_raw_packet_qp raw_packet_qp;
+	};
 	struct mlx5_buf		buf;
 
 	struct mlx5_db		db;
 	struct mlx5_ib_wq	rq;
 
-	u32			doorbell_qpn;
 	u8			sq_signal_bits;
 	u8			fm_cache;
-	int			sq_max_wqes_per_wr;
-	int			sq_spare_wqes;
 	struct mlx5_ib_wq	sq;
 
-	struct ib_umem	       *umem;
-	int			buf_size;
-
 	/* serialize qp state modifications
 	 */
 	struct mutex		mutex;
-	u16			xrcdn;
 	u32			flags;
 	u8			port;
-	u8			alt_port;
-	u8			atomic_rd_en;
-	u8			resp_depth;
 	u8			state;
-	int			mlx_type;
 	int			wq_sig;
 	int			scat_cqe;
 	int			max_inline_data;
@@ -242,6 +327,9 @@ struct mlx5_ib_cq_buf {
 enum mlx5_ib_qp_flags {
 	MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK     = 1 << 0,
 	MLX5_IB_QP_SIGNATURE_HANDLING           = 1 << 1,
+	MLX5_IB_QP_CROSS_CHANNEL		= 1 << 2,
+	MLX5_IB_QP_MANAGED_SEND			= 1 << 3,
+	MLX5_IB_QP_MANAGED_RECV			= 1 << 4,
 };
 
 struct mlx5_umr_wr {
@@ -284,6 +372,7 @@ struct mlx5_ib_cq {
 	struct mlx5_ib_cq_buf  *resize_buf;
 	struct ib_umem	       *resize_umem;
 	int			cqe_size;
+	u32			create_flags;
 };
 
 struct mlx5_ib_srq {
@@ -407,9 +496,19 @@ struct mlx5_ib_resources {
 	struct ib_srq	*s1;
 };
 
+struct mlx5_roce {
+	/* Protect mlx5_ib_get_netdev from invoking dev_hold() with a NULL
+	 * netdev pointer
+	 */
+	rwlock_t		netdev_lock;
+	struct net_device	*netdev;
+	struct notifier_block	nb;
+};
+
 struct mlx5_ib_dev {
 	struct ib_device		ib_dev;
 	struct mlx5_core_dev		*mdev;
+	struct mlx5_roce		roce;
 	MLX5_DECLARE_DOORBELL_LOCK(uar_lock);
 	int				num_ports;
 	/* serialize update of capability mask
@@ -431,6 +530,7 @@ struct mlx5_ib_dev {
 	 */
 	struct srcu_struct      mr_srcu;
 #endif
+	struct mlx5_ib_flow_db	flow_db;
 };
 
 static inline struct mlx5_ib_cq *to_mibcq(struct mlx5_core_cq *mcq)
@@ -455,7 +555,7 @@ static inline struct mlx5_ib_cq *to_mcq(struct ib_cq *ibcq)
 
 static inline struct mlx5_ib_qp *to_mibqp(struct mlx5_core_qp *mqp)
 {
-	return container_of(mqp, struct mlx5_ib_qp, mqp);
+	return container_of(mqp, struct mlx5_ib_qp_base, mqp)->container_mibqp;
 }
 
 static inline struct mlx5_ib_mr *to_mibmr(struct mlx5_core_mr *mmr)
@@ -507,8 +607,6 @@ void mlx5_ib_free_srq_wqe(struct mlx5_ib_srq *srq, int wqe_index);
 int mlx5_MAD_IFC(struct mlx5_ib_dev *dev, int ignore_mkey, int ignore_bkey,
 		 u8 port, const struct ib_wc *in_wc, const struct ib_grh *in_grh,
 		 const void *in_mad, void *response_mad);
-struct ib_ah *create_ib_ah(struct ib_ah_attr *ah_attr,
-			   struct mlx5_ib_ah *ah);
 struct ib_ah *mlx5_ib_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr);
 int mlx5_ib_query_ah(struct ib_ah *ibah, struct ib_ah_attr *ah_attr);
 int mlx5_ib_destroy_ah(struct ib_ah *ah);
@@ -535,7 +633,8 @@ int mlx5_ib_post_recv(struct ib_qp *ibqp, struct ib_recv_wr *wr,
 		      struct ib_recv_wr **bad_wr);
 void *mlx5_get_send_wqe(struct mlx5_ib_qp *qp, int n);
 int mlx5_ib_read_user_wqe(struct mlx5_ib_qp *qp, int send, int wqe_index,
-			  void *buffer, u32 length);
+			  void *buffer, u32 length,
+			  struct mlx5_ib_qp_base *base);
 struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev,
 				const struct ib_cq_init_attr *attr,
 				struct ib_ucontext *context,
@@ -637,6 +736,9 @@ static inline void mlx5_ib_qp_enable_pagefaults(struct mlx5_ib_qp *qp)  {}
 
 #endif /* CONFIG_INFINIBAND_ON_DEMAND_PAGING */
 
+__be16 mlx5_get_roce_udp_sport(struct mlx5_ib_dev *dev, u8 port_num,
+			       int index);
+
 static inline void init_query_mad(struct ib_smp *mad)
 {
 	mad->base_version  = 1;
@@ -662,4 +764,28 @@ static inline int is_qp1(enum ib_qp_type qp_type)
 #define MLX5_MAX_UMR_SHIFT 16
 #define MLX5_MAX_UMR_PAGES (1 << MLX5_MAX_UMR_SHIFT)
 
+static inline u32 check_cq_create_flags(u32 flags)
+{
+	/*
+	 * It returns non-zero value for unsupported CQ
+	 * create flags, otherwise it returns zero.
+	 */
+	return (flags & ~(IB_CQ_FLAGS_IGNORE_OVERRUN |
+			  IB_CQ_FLAGS_TIMESTAMP_COMPLETION));
+}
+
+static inline int verify_assign_uidx(u8 cqe_version, u32 cmd_uidx,
+				     u32 *user_index)
+{
+	if (cqe_version) {
+		if ((cmd_uidx == MLX5_IB_DEFAULT_UIDX) ||
+		    (cmd_uidx & ~MLX5_USER_ASSIGNED_UIDX_MASK))
+			return -EINVAL;
+		*user_index = cmd_uidx;
+	} else {
+		*user_index = MLX5_IB_DEFAULT_UIDX;
+	}
+
+	return 0;
+}
 #endif /* MLX5_IB_H */
diff --git a/drivers/infiniband/hw/mlx5/odp.c b/drivers/infiniband/hw/mlx5/odp.c
index aa8391e75385..b8d76361a48d 100644
--- a/drivers/infiniband/hw/mlx5/odp.c
+++ b/drivers/infiniband/hw/mlx5/odp.c
@@ -153,14 +153,16 @@ static struct mlx5_ib_mr *mlx5_ib_odp_find_mr_lkey(struct mlx5_ib_dev *dev,
 
 static void mlx5_ib_page_fault_resume(struct mlx5_ib_qp *qp,
 				      struct mlx5_ib_pfault *pfault,
-				      int error) {
+				      int error)
+{
 	struct mlx5_ib_dev *dev = to_mdev(qp->ibqp.pd->device);
-	int ret = mlx5_core_page_fault_resume(dev->mdev, qp->mqp.qpn,
+	u32 qpn = qp->trans_qp.base.mqp.qpn;
+	int ret = mlx5_core_page_fault_resume(dev->mdev,
+					      qpn,
 					      pfault->mpfault.flags,
 					      error);
 	if (ret)
-		pr_err("Failed to resolve the page fault on QP 0x%x\n",
-		       qp->mqp.qpn);
+		pr_err("Failed to resolve the page fault on QP 0x%x\n", qpn);
 }
 
 /*
@@ -391,6 +393,7 @@ static int mlx5_ib_mr_initiator_pfault_handler(
 #if defined(DEBUG)
 	u32 ctrl_wqe_index, ctrl_qpn;
 #endif
+	u32 qpn = qp->trans_qp.base.mqp.qpn;
 
 	ds = be32_to_cpu(ctrl->qpn_ds) & MLX5_WQE_CTRL_DS_MASK;
 	if (ds * MLX5_WQE_DS_UNITS > wqe_length) {
@@ -401,7 +404,7 @@ static int mlx5_ib_mr_initiator_pfault_handler(
 
 	if (ds == 0) {
 		mlx5_ib_err(dev, "Got WQE with zero DS. wqe_index=%x, qpn=%x\n",
-			    wqe_index, qp->mqp.qpn);
+			    wqe_index, qpn);
 		return -EFAULT;
 	}
 
@@ -411,16 +414,16 @@ static int mlx5_ib_mr_initiator_pfault_handler(
 			MLX5_WQE_CTRL_WQE_INDEX_SHIFT;
 	if (wqe_index != ctrl_wqe_index) {
 		mlx5_ib_err(dev, "Got WQE with invalid wqe_index. wqe_index=0x%x, qpn=0x%x ctrl->wqe_index=0x%x\n",
-			    wqe_index, qp->mqp.qpn,
+			    wqe_index, qpn,
 			    ctrl_wqe_index);
 		return -EFAULT;
 	}
 
 	ctrl_qpn = (be32_to_cpu(ctrl->qpn_ds) & MLX5_WQE_CTRL_QPN_MASK) >>
 		MLX5_WQE_CTRL_QPN_SHIFT;
-	if (qp->mqp.qpn != ctrl_qpn) {
+	if (qpn != ctrl_qpn) {
 		mlx5_ib_err(dev, "Got WQE with incorrect QP number. wqe_index=0x%x, qpn=0x%x ctrl->qpn=0x%x\n",
-			    wqe_index, qp->mqp.qpn,
+			    wqe_index, qpn,
 			    ctrl_qpn);
 		return -EFAULT;
 	}
@@ -537,6 +540,7 @@ static void mlx5_ib_mr_wqe_pfault_handler(struct mlx5_ib_qp *qp,
 	int resume_with_error = 0;
 	u16 wqe_index = pfault->mpfault.wqe.wqe_index;
 	int requestor = pfault->mpfault.flags & MLX5_PFAULT_REQUESTOR;
+	u32 qpn = qp->trans_qp.base.mqp.qpn;
 
 	buffer = (char *)__get_free_page(GFP_KERNEL);
 	if (!buffer) {
@@ -546,10 +550,10 @@ static void mlx5_ib_mr_wqe_pfault_handler(struct mlx5_ib_qp *qp,
 	}
 
 	ret = mlx5_ib_read_user_wqe(qp, requestor, wqe_index, buffer,
-				    PAGE_SIZE);
+				    PAGE_SIZE, &qp->trans_qp.base);
 	if (ret < 0) {
 		mlx5_ib_err(dev, "Failed reading a WQE following page fault, error=%x, wqe_index=%x, qpn=%x\n",
-			    -ret, wqe_index, qp->mqp.qpn);
+			    -ret, wqe_index, qpn);
 		resume_with_error = 1;
 		goto resolve_page_fault;
 	}
@@ -586,7 +590,8 @@ static void mlx5_ib_mr_wqe_pfault_handler(struct mlx5_ib_qp *qp,
 resolve_page_fault:
 	mlx5_ib_page_fault_resume(qp, pfault, resume_with_error);
 	mlx5_ib_dbg(dev, "PAGE FAULT completed. QP 0x%x resume_with_error=%d, flags: 0x%x\n",
-		    qp->mqp.qpn, resume_with_error, pfault->mpfault.flags);
+		    qpn, resume_with_error,
+		    pfault->mpfault.flags);
 
 	free_page((unsigned long)buffer);
 }
@@ -753,7 +758,7 @@ void mlx5_ib_odp_create_qp(struct mlx5_ib_qp *qp)
 	qp->disable_page_faults = 1;
 	spin_lock_init(&qp->disable_page_faults_lock);
 
-	qp->mqp.pfault_handler	= mlx5_ib_pfault_handler;
+	qp->trans_qp.base.mqp.pfault_handler = mlx5_ib_pfault_handler;
 
 	for (i = 0; i < MLX5_IB_PAGEFAULT_CONTEXTS; ++i)
 		INIT_WORK(&qp->pagefaults[i].work, mlx5_ib_qp_pfault_action);
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index 307bdbca8938..34cb8e87c7b8 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -32,6 +32,8 @@
 
 #include <linux/module.h>
 #include <rdma/ib_umem.h>
+#include <rdma/ib_cache.h>
+#include <rdma/ib_user_verbs.h>
 #include "mlx5_ib.h"
 #include "user.h"
 
@@ -114,14 +116,15 @@ void *mlx5_get_send_wqe(struct mlx5_ib_qp *qp, int n)
  * Return: the number of bytes copied, or an error code.
  */
 int mlx5_ib_read_user_wqe(struct mlx5_ib_qp *qp, int send, int wqe_index,
-			  void *buffer, u32 length)
+			  void *buffer, u32 length,
+			  struct mlx5_ib_qp_base *base)
 {
 	struct ib_device *ibdev = qp->ibqp.device;
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
 	struct mlx5_ib_wq *wq = send ? &qp->sq : &qp->rq;
 	size_t offset;
 	size_t wq_end;
-	struct ib_umem *umem = qp->umem;
+	struct ib_umem *umem = base->ubuffer.umem;
 	u32 first_copy_length;
 	int wqe_length;
 	int ret;
@@ -172,8 +175,10 @@ static void mlx5_ib_qp_event(struct mlx5_core_qp *qp, int type)
 	struct ib_qp *ibqp = &to_mibqp(qp)->ibqp;
 	struct ib_event event;
 
-	if (type == MLX5_EVENT_TYPE_PATH_MIG)
-		to_mibqp(qp)->port = to_mibqp(qp)->alt_port;
+	if (type == MLX5_EVENT_TYPE_PATH_MIG) {
+		/* This event is only valid for trans_qps */
+		to_mibqp(qp)->port = to_mibqp(qp)->trans_qp.alt_port;
+	}
 
 	if (ibqp->event_handler) {
 		event.device     = ibqp->device;
@@ -265,8 +270,10 @@ static int sq_overhead(enum ib_qp_type qp_type)
 		/* fall through */
 	case IB_QPT_RC:
 		size += sizeof(struct mlx5_wqe_ctrl_seg) +
-			sizeof(struct mlx5_wqe_atomic_seg) +
-			sizeof(struct mlx5_wqe_raddr_seg);
+			max(sizeof(struct mlx5_wqe_atomic_seg) +
+			    sizeof(struct mlx5_wqe_raddr_seg),
+			    sizeof(struct mlx5_wqe_umr_ctrl_seg) +
+			    sizeof(struct mlx5_mkey_seg));
 		break;
 
 	case IB_QPT_XRC_TGT:
@@ -274,9 +281,9 @@ static int sq_overhead(enum ib_qp_type qp_type)
 
 	case IB_QPT_UC:
 		size += sizeof(struct mlx5_wqe_ctrl_seg) +
-			sizeof(struct mlx5_wqe_raddr_seg) +
-			sizeof(struct mlx5_wqe_umr_ctrl_seg) +
-			sizeof(struct mlx5_mkey_seg);
+			max(sizeof(struct mlx5_wqe_raddr_seg),
+			    sizeof(struct mlx5_wqe_umr_ctrl_seg) +
+			    sizeof(struct mlx5_mkey_seg));
 		break;
 
 	case IB_QPT_UD:
@@ -366,7 +373,9 @@ static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 
 static int set_user_buf_size(struct mlx5_ib_dev *dev,
 			    struct mlx5_ib_qp *qp,
-			    struct mlx5_ib_create_qp *ucmd)
+			    struct mlx5_ib_create_qp *ucmd,
+			    struct mlx5_ib_qp_base *base,
+			    struct ib_qp_init_attr *attr)
 {
 	int desc_sz = 1 << qp->sq.wqe_shift;
 
@@ -391,8 +400,13 @@ static int set_user_buf_size(struct mlx5_ib_dev *dev,
 		return -EINVAL;
 	}
 
-	qp->buf_size = (qp->rq.wqe_cnt << qp->rq.wqe_shift) +
-		(qp->sq.wqe_cnt << 6);
+	if (attr->qp_type == IB_QPT_RAW_PACKET) {
+		base->ubuffer.buf_size = qp->rq.wqe_cnt << qp->rq.wqe_shift;
+		qp->raw_packet_qp.sq.ubuffer.buf_size = qp->sq.wqe_cnt << 6;
+	} else {
+		base->ubuffer.buf_size = (qp->rq.wqe_cnt << qp->rq.wqe_shift) +
+					 (qp->sq.wqe_cnt << 6);
+	}
 
 	return 0;
 }
@@ -578,8 +592,8 @@ static int to_mlx5_st(enum ib_qp_type type)
 	case IB_QPT_SMI:		return MLX5_QP_ST_QP0;
 	case IB_QPT_GSI:		return MLX5_QP_ST_QP1;
 	case IB_QPT_RAW_IPV6:		return MLX5_QP_ST_RAW_IPV6;
-	case IB_QPT_RAW_ETHERTYPE:	return MLX5_QP_ST_RAW_ETHERTYPE;
 	case IB_QPT_RAW_PACKET:
+	case IB_QPT_RAW_ETHERTYPE:	return MLX5_QP_ST_RAW_ETHERTYPE;
 	case IB_QPT_MAX:
 	default:		return -EINVAL;
 	}
@@ -590,13 +604,51 @@ static int uuarn_to_uar_index(struct mlx5_uuar_info *uuari, int uuarn)
 	return uuari->uars[uuarn / MLX5_BF_REGS_PER_PAGE].index;
 }
 
+static int mlx5_ib_umem_get(struct mlx5_ib_dev *dev,
+			    struct ib_pd *pd,
+			    unsigned long addr, size_t size,
+			    struct ib_umem **umem,
+			    int *npages, int *page_shift, int *ncont,
+			    u32 *offset)
+{
+	int err;
+
+	*umem = ib_umem_get(pd->uobject->context, addr, size, 0, 0);
+	if (IS_ERR(*umem)) {
+		mlx5_ib_dbg(dev, "umem_get failed\n");
+		return PTR_ERR(*umem);
+	}
+
+	mlx5_ib_cont_pages(*umem, addr, npages, page_shift, ncont, NULL);
+
+	err = mlx5_ib_get_buf_offset(addr, *page_shift, offset);
+	if (err) {
+		mlx5_ib_warn(dev, "bad offset\n");
+		goto err_umem;
+	}
+
+	mlx5_ib_dbg(dev, "addr 0x%lx, size %zu, npages %d, page_shift %d, ncont %d, offset %d\n",
+		    addr, size, *npages, *page_shift, *ncont, *offset);
+
+	return 0;
+
+err_umem:
+	ib_umem_release(*umem);
+	*umem = NULL;
+
+	return err;
+}
+
 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,
-			  struct mlx5_ib_create_qp_resp *resp, int *inlen)
+			  struct mlx5_ib_create_qp_resp *resp, int *inlen,
+			  struct mlx5_ib_qp_base *base)
 {
 	struct mlx5_ib_ucontext *context;
 	struct mlx5_ib_create_qp ucmd;
+	struct mlx5_ib_ubuffer *ubuffer = &base->ubuffer;
 	int page_shift = 0;
 	int uar_index;
 	int npages;
@@ -615,18 +667,23 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	/*
 	 * TBD: should come from the verbs when we have the API
 	 */
-	uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_HIGH);
-	if (uuarn < 0) {
-		mlx5_ib_dbg(dev, "failed to allocate low latency UUAR\n");
-		mlx5_ib_dbg(dev, "reverting to medium latency\n");
-		uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_MEDIUM);
+	if (qp->flags & MLX5_IB_QP_CROSS_CHANNEL)
+		/* In CROSS_CHANNEL CQ and QP must use the same UAR */
+		uuarn = MLX5_CROSS_CHANNEL_UUAR;
+	else {
+		uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_HIGH);
 		if (uuarn < 0) {
-			mlx5_ib_dbg(dev, "failed to allocate medium latency UUAR\n");
-			mlx5_ib_dbg(dev, "reverting to high latency\n");
-			uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_LOW);
+			mlx5_ib_dbg(dev, "failed to allocate low latency UUAR\n");
+			mlx5_ib_dbg(dev, "reverting to medium latency\n");
+			uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_MEDIUM);
 			if (uuarn < 0) {
-				mlx5_ib_warn(dev, "uuar allocation failed\n");
-				return uuarn;
+				mlx5_ib_dbg(dev, "failed to allocate medium latency UUAR\n");
+				mlx5_ib_dbg(dev, "reverting to high latency\n");
+				uuarn = alloc_uuar(&context->uuari, MLX5_IB_LATENCY_CLASS_LOW);
+				if (uuarn < 0) {
+					mlx5_ib_warn(dev, "uuar allocation failed\n");
+					return uuarn;
+				}
 			}
 		}
 	}
@@ -638,32 +695,20 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	qp->sq.wqe_shift = ilog2(MLX5_SEND_WQE_BB);
 	qp->sq.offset = qp->rq.wqe_cnt << qp->rq.wqe_shift;
 
-	err = set_user_buf_size(dev, qp, &ucmd);
+	err = set_user_buf_size(dev, qp, &ucmd, base, attr);
 	if (err)
 		goto err_uuar;
 
-	if (ucmd.buf_addr && qp->buf_size) {
-		qp->umem = ib_umem_get(pd->uobject->context, ucmd.buf_addr,
-				       qp->buf_size, 0, 0);
-		if (IS_ERR(qp->umem)) {
-			mlx5_ib_dbg(dev, "umem_get failed\n");
-			err = PTR_ERR(qp->umem);
+	if (ucmd.buf_addr && ubuffer->buf_size) {
+		ubuffer->buf_addr = ucmd.buf_addr;
+		err = mlx5_ib_umem_get(dev, pd, ubuffer->buf_addr,
+				       ubuffer->buf_size,
+				       &ubuffer->umem, &npages, &page_shift,
+				       &ncont, &offset);
+		if (err)
 			goto err_uuar;
-		}
 	} else {
-		qp->umem = NULL;
-	}
-
-	if (qp->umem) {
-		mlx5_ib_cont_pages(qp->umem, ucmd.buf_addr, &npages, &page_shift,
-				   &ncont, NULL);
-		err = mlx5_ib_get_buf_offset(ucmd.buf_addr, page_shift, &offset);
-		if (err) {
-			mlx5_ib_warn(dev, "bad offset\n");
-			goto err_umem;
-		}
-		mlx5_ib_dbg(dev, "addr 0x%llx, size %d, npages %d, page_shift %d, ncont %d, offset %d\n",
-			    ucmd.buf_addr, qp->buf_size, npages, page_shift, ncont, offset);
+		ubuffer->umem = NULL;
 	}
 
 	*inlen = sizeof(**in) + sizeof(*(*in)->pas) * ncont;
@@ -672,8 +717,9 @@ static int create_user_qp(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		err = -ENOMEM;
 		goto err_umem;
 	}
-	if (qp->umem)
-		mlx5_ib_populate_pas(dev, qp->umem, page_shift, (*in)->pas, 0);
+	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);
@@ -704,29 +750,31 @@ err_free:
 	kvfree(*in);
 
 err_umem:
-	if (qp->umem)
-		ib_umem_release(qp->umem);
+	if (ubuffer->umem)
+		ib_umem_release(ubuffer->umem);
 
 err_uuar:
 	free_uuar(&context->uuari, uuarn);
 	return err;
 }
 
-static void destroy_qp_user(struct ib_pd *pd, struct mlx5_ib_qp *qp)
+static void destroy_qp_user(struct ib_pd *pd, struct mlx5_ib_qp *qp,
+			    struct mlx5_ib_qp_base *base)
 {
 	struct mlx5_ib_ucontext *context;
 
 	context = to_mucontext(pd->uobject->context);
 	mlx5_ib_db_unmap_user(context, &qp->db);
-	if (qp->umem)
-		ib_umem_release(qp->umem);
+	if (base->ubuffer.umem)
+		ib_umem_release(base->ubuffer.umem);
 	free_uuar(&context->uuari, qp->uuarn);
 }
 
 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)
+			    struct mlx5_create_qp_mbox_in **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;
@@ -758,9 +806,9 @@ static int create_kernel_qp(struct mlx5_ib_dev *dev,
 
 	qp->rq.offset = 0;
 	qp->sq.offset = qp->rq.wqe_cnt << qp->rq.wqe_shift;
-	qp->buf_size = err + (qp->rq.wqe_cnt << qp->rq.wqe_shift);
+	base->ubuffer.buf_size = err + (qp->rq.wqe_cnt << qp->rq.wqe_shift);
 
-	err = mlx5_buf_alloc(dev->mdev, qp->buf_size, &qp->buf);
+	err = mlx5_buf_alloc(dev->mdev, base->ubuffer.buf_size, &qp->buf);
 	if (err) {
 		mlx5_ib_dbg(dev, "err %d\n", err);
 		goto err_uuar;
@@ -853,19 +901,304 @@ static int is_connected(enum ib_qp_type qp_type)
 	return 0;
 }
 
+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)];
+	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);
+}
+
+static void destroy_raw_packet_qp_tis(struct mlx5_ib_dev *dev,
+				      struct mlx5_ib_sq *sq)
+{
+	mlx5_core_destroy_tis(dev->mdev, sq->tisn);
+}
+
+static int create_raw_packet_qp_sq(struct mlx5_ib_dev *dev,
+				   struct mlx5_ib_sq *sq, void *qpin,
+				   struct ib_pd *pd)
+{
+	struct mlx5_ib_ubuffer *ubuffer = &sq->ubuffer;
+	__be64 *pas;
+	void *in;
+	void *sqc;
+	void *qpc = MLX5_ADDR_OF(create_qp_in, qpin, qpc);
+	void *wq;
+	int inlen;
+	int err;
+	int page_shift = 0;
+	int npages;
+	int ncont = 0;
+	u32 offset = 0;
+
+	err = mlx5_ib_umem_get(dev, pd, ubuffer->buf_addr, ubuffer->buf_size,
+			       &sq->ubuffer.umem, &npages, &page_shift,
+			       &ncont, &offset);
+	if (err)
+		return err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_sq_in) + sizeof(u64) * ncont;
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		err = -ENOMEM;
+		goto err_umem;
+	}
+
+	sqc = MLX5_ADDR_OF(create_sq_in, in, ctx);
+	MLX5_SET(sqc, sqc, flush_in_error_en, 1);
+	MLX5_SET(sqc, sqc, state, MLX5_SQC_STATE_RST);
+	MLX5_SET(sqc, sqc, user_index, MLX5_GET(qpc, qpc, user_index));
+	MLX5_SET(sqc, sqc, cqn, MLX5_GET(qpc, qpc, cqn_snd));
+	MLX5_SET(sqc, sqc, tis_lst_sz, 1);
+	MLX5_SET(sqc, sqc, tis_num_0, sq->tisn);
+
+	wq = MLX5_ADDR_OF(sqc, sqc, wq);
+	MLX5_SET(wq, wq, wq_type, MLX5_WQ_TYPE_CYCLIC);
+	MLX5_SET(wq, wq, pd, MLX5_GET(qpc, qpc, pd));
+	MLX5_SET(wq, wq, uar_page, MLX5_GET(qpc, qpc, uar_page));
+	MLX5_SET64(wq, wq, dbr_addr, MLX5_GET64(qpc, qpc, dbr_addr));
+	MLX5_SET(wq, wq, log_wq_stride, ilog2(MLX5_SEND_WQE_BB));
+	MLX5_SET(wq, wq, log_wq_sz, MLX5_GET(qpc, qpc, log_sq_size));
+	MLX5_SET(wq, wq, log_wq_pg_sz,  page_shift - MLX5_ADAPTER_PAGE_SHIFT);
+	MLX5_SET(wq, wq, page_offset, offset);
+
+	pas = (__be64 *)MLX5_ADDR_OF(wq, wq, pas);
+	mlx5_ib_populate_pas(dev, sq->ubuffer.umem, page_shift, pas, 0);
+
+	err = mlx5_core_create_sq_tracked(dev->mdev, in, inlen, &sq->base.mqp);
+
+	kvfree(in);
+
+	if (err)
+		goto err_umem;
+
+	return 0;
+
+err_umem:
+	ib_umem_release(sq->ubuffer.umem);
+	sq->ubuffer.umem = NULL;
+
+	return err;
+}
+
+static void destroy_raw_packet_qp_sq(struct mlx5_ib_dev *dev,
+				     struct mlx5_ib_sq *sq)
+{
+	mlx5_core_destroy_sq_tracked(dev->mdev, &sq->base.mqp);
+	ib_umem_release(sq->ubuffer.umem);
+}
+
+static int get_rq_pas_size(void *qpc)
+{
+	u32 log_page_size = MLX5_GET(qpc, qpc, log_page_size) + 12;
+	u32 log_rq_stride = MLX5_GET(qpc, qpc, log_rq_stride);
+	u32 log_rq_size   = MLX5_GET(qpc, qpc, log_rq_size);
+	u32 page_offset   = MLX5_GET(qpc, qpc, page_offset);
+	u32 po_quanta	  = 1 << (log_page_size - 6);
+	u32 rq_sz	  = 1 << (log_rq_size + 4 + log_rq_stride);
+	u32 page_size	  = 1 << log_page_size;
+	u32 rq_sz_po      = rq_sz + (page_offset * po_quanta);
+	u32 rq_num_pas	  = (rq_sz_po + page_size - 1) / page_size;
+
+	return rq_num_pas * sizeof(u64);
+}
+
+static int create_raw_packet_qp_rq(struct mlx5_ib_dev *dev,
+				   struct mlx5_ib_rq *rq, void *qpin)
+{
+	__be64 *pas;
+	__be64 *qp_pas;
+	void *in;
+	void *rqc;
+	void *wq;
+	void *qpc = MLX5_ADDR_OF(create_qp_in, qpin, qpc);
+	int inlen;
+	int err;
+	u32 rq_pas_size = get_rq_pas_size(qpc);
+
+	inlen = MLX5_ST_SZ_BYTES(create_rq_in) + rq_pas_size;
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	rqc = MLX5_ADDR_OF(create_rq_in, in, ctx);
+	MLX5_SET(rqc, rqc, vsd, 1);
+	MLX5_SET(rqc, rqc, mem_rq_type, MLX5_RQC_MEM_RQ_TYPE_MEMORY_RQ_INLINE);
+	MLX5_SET(rqc, rqc, state, MLX5_RQC_STATE_RST);
+	MLX5_SET(rqc, rqc, flush_in_error_en, 1);
+	MLX5_SET(rqc, rqc, user_index, MLX5_GET(qpc, qpc, user_index));
+	MLX5_SET(rqc, rqc, cqn, MLX5_GET(qpc, qpc, cqn_rcv));
+
+	wq = MLX5_ADDR_OF(rqc, rqc, wq);
+	MLX5_SET(wq, wq, wq_type, MLX5_WQ_TYPE_CYCLIC);
+	MLX5_SET(wq, wq, end_padding_mode,
+		 MLX5_GET(qpc, qpc, end_padding_mode));
+	MLX5_SET(wq, wq, page_offset, MLX5_GET(qpc, qpc, page_offset));
+	MLX5_SET(wq, wq, pd, MLX5_GET(qpc, qpc, pd));
+	MLX5_SET64(wq, wq, dbr_addr, MLX5_GET64(qpc, qpc, dbr_addr));
+	MLX5_SET(wq, wq, log_wq_stride, MLX5_GET(qpc, qpc, log_rq_stride) + 4);
+	MLX5_SET(wq, wq, log_wq_pg_sz, MLX5_GET(qpc, qpc, log_page_size));
+	MLX5_SET(wq, wq, log_wq_sz, MLX5_GET(qpc, qpc, log_rq_size));
+
+	pas = (__be64 *)MLX5_ADDR_OF(wq, wq, pas);
+	qp_pas = (__be64 *)MLX5_ADDR_OF(create_qp_in, qpin, pas);
+	memcpy(pas, qp_pas, rq_pas_size);
+
+	err = mlx5_core_create_rq_tracked(dev->mdev, in, inlen, &rq->base.mqp);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void destroy_raw_packet_qp_rq(struct mlx5_ib_dev *dev,
+				     struct mlx5_ib_rq *rq)
+{
+	mlx5_core_destroy_rq_tracked(dev->mdev, &rq->base.mqp);
+}
+
+static int create_raw_packet_qp_tir(struct mlx5_ib_dev *dev,
+				    struct mlx5_ib_rq *rq, u32 tdn)
+{
+	u32 *in;
+	void *tirc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(create_tir_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	tirc = MLX5_ADDR_OF(create_tir_in, in, ctx);
+	MLX5_SET(tirc, tirc, disp_type, MLX5_TIRC_DISP_TYPE_DIRECT);
+	MLX5_SET(tirc, tirc, inline_rqn, rq->base.mqp.qpn);
+	MLX5_SET(tirc, tirc, transport_domain, tdn);
+
+	err = mlx5_core_create_tir(dev->mdev, in, inlen, &rq->tirn);
+
+	kvfree(in);
+
+	return err;
+}
+
+static void destroy_raw_packet_qp_tir(struct mlx5_ib_dev *dev,
+				      struct mlx5_ib_rq *rq)
+{
+	mlx5_core_destroy_tir(dev->mdev, rq->tirn);
+}
+
+static int create_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
+				struct mlx5_create_qp_mbox_in *in,
+				struct ib_pd *pd)
+{
+	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
+	struct mlx5_ib_sq *sq = &raw_packet_qp->sq;
+	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
+	struct ib_uobject *uobj = pd->uobject;
+	struct ib_ucontext *ucontext = uobj->context;
+	struct mlx5_ib_ucontext *mucontext = to_mucontext(ucontext);
+	int err;
+	u32 tdn = mucontext->tdn;
+
+	if (qp->sq.wqe_cnt) {
+		err = create_raw_packet_qp_tis(dev, sq, tdn);
+		if (err)
+			return err;
+
+		err = create_raw_packet_qp_sq(dev, sq, in, pd);
+		if (err)
+			goto err_destroy_tis;
+
+		sq->base.container_mibqp = qp;
+	}
+
+	if (qp->rq.wqe_cnt) {
+		err = create_raw_packet_qp_rq(dev, rq, in);
+		if (err)
+			goto err_destroy_sq;
+
+		rq->base.container_mibqp = qp;
+
+		err = create_raw_packet_qp_tir(dev, rq, tdn);
+		if (err)
+			goto err_destroy_rq;
+	}
+
+	qp->trans_qp.base.mqp.qpn = qp->sq.wqe_cnt ? sq->base.mqp.qpn :
+						     rq->base.mqp.qpn;
+
+	return 0;
+
+err_destroy_rq:
+	destroy_raw_packet_qp_rq(dev, rq);
+err_destroy_sq:
+	if (!qp->sq.wqe_cnt)
+		return err;
+	destroy_raw_packet_qp_sq(dev, sq);
+err_destroy_tis:
+	destroy_raw_packet_qp_tis(dev, sq);
+
+	return err;
+}
+
+static void destroy_raw_packet_qp(struct mlx5_ib_dev *dev,
+				  struct mlx5_ib_qp *qp)
+{
+	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
+	struct mlx5_ib_sq *sq = &raw_packet_qp->sq;
+	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
+
+	if (qp->rq.wqe_cnt) {
+		destroy_raw_packet_qp_tir(dev, rq);
+		destroy_raw_packet_qp_rq(dev, rq);
+	}
+
+	if (qp->sq.wqe_cnt) {
+		destroy_raw_packet_qp_sq(dev, sq);
+		destroy_raw_packet_qp_tis(dev, sq);
+	}
+}
+
+static void raw_packet_qp_copy_info(struct mlx5_ib_qp *qp,
+				    struct mlx5_ib_raw_packet_qp *raw_packet_qp)
+{
+	struct mlx5_ib_sq *sq = &raw_packet_qp->sq;
+	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
+
+	sq->sq = &qp->sq;
+	rq->rq = &qp->rq;
+	sq->doorbell = &qp->db;
+	rq->doorbell = &qp->db;
+}
+
 static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 			    struct ib_qp_init_attr *init_attr,
 			    struct ib_udata *udata, struct mlx5_ib_qp *qp)
 {
 	struct mlx5_ib_resources *devr = &dev->devr;
 	struct mlx5_core_dev *mdev = dev->mdev;
+	struct mlx5_ib_qp_base *base;
 	struct mlx5_ib_create_qp_resp resp;
 	struct mlx5_create_qp_mbox_in *in;
 	struct mlx5_ib_create_qp ucmd;
 	int inlen = sizeof(*in);
 	int err;
+	u32 uidx = MLX5_IB_DEFAULT_UIDX;
+	void *qpc;
+
+	base = init_attr->qp_type == IB_QPT_RAW_PACKET ?
+	       &qp->raw_packet_qp.rq.base :
+	       &qp->trans_qp.base;
 
-	mlx5_ib_odp_create_qp(qp);
+	if (init_attr->qp_type != IB_QPT_RAW_PACKET)
+		mlx5_ib_odp_create_qp(qp);
 
 	mutex_init(&qp->mutex);
 	spin_lock_init(&qp->sq.lock);
@@ -880,6 +1213,21 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 		}
 	}
 
+	if (init_attr->create_flags &
+			(IB_QP_CREATE_CROSS_CHANNEL |
+			 IB_QP_CREATE_MANAGED_SEND |
+			 IB_QP_CREATE_MANAGED_RECV)) {
+		if (!MLX5_CAP_GEN(mdev, cd)) {
+			mlx5_ib_dbg(dev, "cross-channel isn't supported\n");
+			return -EINVAL;
+		}
+		if (init_attr->create_flags & IB_QP_CREATE_CROSS_CHANNEL)
+			qp->flags |= MLX5_IB_QP_CROSS_CHANNEL;
+		if (init_attr->create_flags & IB_QP_CREATE_MANAGED_SEND)
+			qp->flags |= MLX5_IB_QP_MANAGED_SEND;
+		if (init_attr->create_flags & IB_QP_CREATE_MANAGED_RECV)
+			qp->flags |= MLX5_IB_QP_MANAGED_RECV;
+	}
 	if (init_attr->sq_sig_type == IB_SIGNAL_ALL_WR)
 		qp->sq_signal_bits = MLX5_WQE_CTRL_CQ_UPDATE;
 
@@ -889,6 +1237,11 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 			return -EFAULT;
 		}
 
+		err = get_qp_user_index(to_mucontext(pd->uobject->context),
+					&ucmd, udata->inlen, &uidx);
+		if (err)
+			return err;
+
 		qp->wq_sig = !!(ucmd.flags & MLX5_QP_FLAG_SIGNATURE);
 		qp->scat_cqe = !!(ucmd.flags & MLX5_QP_FLAG_SCATTER_CQE);
 	} else {
@@ -918,11 +1271,13 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 					    ucmd.sq_wqe_count, max_wqes);
 				return -EINVAL;
 			}
-			err = create_user_qp(dev, pd, qp, udata, &in, &resp, &inlen);
+			err = create_user_qp(dev, pd, qp, udata, init_attr, &in,
+					     &resp, &inlen, base);
 			if (err)
 				mlx5_ib_dbg(dev, "err %d\n", err);
 		} else {
-			err = create_kernel_qp(dev, init_attr, qp, &in, &inlen);
+			err = create_kernel_qp(dev, init_attr, qp, &in, &inlen,
+					       base);
 			if (err)
 				mlx5_ib_dbg(dev, "err %d\n", err);
 		}
@@ -954,6 +1309,13 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	if (qp->flags & MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK)
 		in->ctx.flags_pd |= cpu_to_be32(MLX5_QP_BLOCK_MCAST);
 
+	if (qp->flags & MLX5_IB_QP_CROSS_CHANNEL)
+		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_MASTER);
+	if (qp->flags & MLX5_IB_QP_MANAGED_SEND)
+		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_SLAVE_SEND);
+	if (qp->flags & MLX5_IB_QP_MANAGED_RECV)
+		in->ctx.params2 |= cpu_to_be32(MLX5_QP_BIT_CC_SLAVE_RECV);
+
 	if (qp->scat_cqe && is_connected(init_attr->qp_type)) {
 		int rcqe_sz;
 		int scqe_sz;
@@ -1018,26 +1380,35 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 
 	in->ctx.db_rec_addr = cpu_to_be64(qp->db.dma);
 
-	err = mlx5_core_create_qp(dev->mdev, &qp->mqp, in, inlen);
+	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 */
+		MLX5_SET(qpc, qpc, user_index, uidx);
+	}
+
+	if (init_attr->qp_type == IB_QPT_RAW_PACKET) {
+		qp->raw_packet_qp.sq.ubuffer.buf_addr = ucmd.sq_buf_addr;
+		raw_packet_qp_copy_info(qp, &qp->raw_packet_qp);
+		err = create_raw_packet_qp(dev, qp, in, pd);
+	} else {
+		err = mlx5_core_create_qp(dev->mdev, &base->mqp, in, inlen);
+	}
+
 	if (err) {
 		mlx5_ib_dbg(dev, "create qp failed\n");
 		goto err_create;
 	}
 
 	kvfree(in);
-	/* Hardware wants QPN written in big-endian order (after
-	 * shifting) for send doorbell.  Precompute this value to save
-	 * a little bit when posting sends.
-	 */
-	qp->doorbell_qpn = swab32(qp->mqp.qpn << 8);
 
-	qp->mqp.event = mlx5_ib_qp_event;
+	base->container_mibqp = qp;
+	base->mqp.event = mlx5_ib_qp_event;
 
 	return 0;
 
 err_create:
 	if (qp->create_type == MLX5_QP_USER)
-		destroy_qp_user(pd, qp);
+		destroy_qp_user(pd, qp, base);
 	else if (qp->create_type == MLX5_QP_KERNEL)
 		destroy_qp_kernel(dev, qp);
 
@@ -1129,11 +1500,11 @@ static void get_cqs(struct mlx5_ib_qp *qp,
 	case IB_QPT_UD:
 	case IB_QPT_RAW_IPV6:
 	case IB_QPT_RAW_ETHERTYPE:
+	case IB_QPT_RAW_PACKET:
 		*send_cq = to_mcq(qp->ibqp.send_cq);
 		*recv_cq = to_mcq(qp->ibqp.recv_cq);
 		break;
 
-	case IB_QPT_RAW_PACKET:
 	case IB_QPT_MAX:
 	default:
 		*send_cq = NULL;
@@ -1142,45 +1513,66 @@ static void get_cqs(struct mlx5_ib_qp *qp,
 	}
 }
 
+static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
+				u16 operation);
+
 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;
 	int err;
 
+	base = qp->ibqp.qp_type == IB_QPT_RAW_PACKET ?
+	       &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) {
-		mlx5_ib_qp_disable_pagefaults(qp);
-		if (mlx5_core_qp_modify(dev->mdev, to_mlx5_state(qp->state),
-					MLX5_QP_STATE_RST, in, 0, &qp->mqp))
-			mlx5_ib_warn(dev, "mlx5_ib: modify QP %06x to RESET failed\n",
-				     qp->mqp.qpn);
+		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);
+		} else {
+			err = modify_raw_packet_qp(dev, qp,
+						   MLX5_CMD_OP_2RST_QP);
+		}
+		if (err)
+			mlx5_ib_warn(dev, "mlx5_ib: modify QP 0x%06x to RESET failed\n",
+				     base->mqp.qpn);
 	}
 
 	get_cqs(qp, &send_cq, &recv_cq);
 
 	if (qp->create_type == MLX5_QP_KERNEL) {
 		mlx5_ib_lock_cqs(send_cq, recv_cq);
-		__mlx5_ib_cq_clean(recv_cq, qp->mqp.qpn,
+		__mlx5_ib_cq_clean(recv_cq, base->mqp.qpn,
 				   qp->ibqp.srq ? to_msrq(qp->ibqp.srq) : NULL);
 		if (send_cq != recv_cq)
-			__mlx5_ib_cq_clean(send_cq, qp->mqp.qpn, NULL);
+			__mlx5_ib_cq_clean(send_cq, base->mqp.qpn,
+					   NULL);
 		mlx5_ib_unlock_cqs(send_cq, recv_cq);
 	}
 
-	err = mlx5_core_destroy_qp(dev->mdev, &qp->mqp);
-	if (err)
-		mlx5_ib_warn(dev, "failed to destroy QP 0x%x\n", qp->mqp.qpn);
-	kfree(in);
+	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET) {
+		destroy_raw_packet_qp(dev, qp);
+	} else {
+		err = mlx5_core_destroy_qp(dev->mdev, &base->mqp);
+		if (err)
+			mlx5_ib_warn(dev, "failed to destroy QP 0x%x\n",
+				     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)
-		destroy_qp_user(&get_pd(qp)->ibpd, qp);
+		destroy_qp_user(&get_pd(qp)->ibpd, qp, base);
 }
 
 static const char *ib_qp_type_str(enum ib_qp_type type)
@@ -1225,6 +1617,16 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 
 	if (pd) {
 		dev = to_mdev(pd->device);
+
+		if (init_attr->qp_type == IB_QPT_RAW_PACKET) {
+			if (!pd->uobject) {
+				mlx5_ib_dbg(dev, "Raw Packet QP is not supported for kernel consumers\n");
+				return ERR_PTR(-EINVAL);
+			} else if (!to_mucontext(pd->uobject->context)->cqe_version) {
+				mlx5_ib_dbg(dev, "Raw Packet QP is only supported for CQE version > 0\n");
+				return ERR_PTR(-EINVAL);
+			}
+		}
 	} else {
 		/* being cautious here */
 		if (init_attr->qp_type != IB_QPT_XRC_TGT &&
@@ -1250,6 +1652,7 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 		}
 
 		/* fall through */
+	case IB_QPT_RAW_PACKET:
 	case IB_QPT_RC:
 	case IB_QPT_UC:
 	case IB_QPT_UD:
@@ -1272,19 +1675,19 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 		else if (is_qp1(init_attr->qp_type))
 			qp->ibqp.qp_num = 1;
 		else
-			qp->ibqp.qp_num = qp->mqp.qpn;
+			qp->ibqp.qp_num = qp->trans_qp.base.mqp.qpn;
 
 		mlx5_ib_dbg(dev, "ib qpnum 0x%x, mlx qpn 0x%x, rcqn 0x%x, scqn 0x%x\n",
-			    qp->ibqp.qp_num, qp->mqp.qpn, to_mcq(init_attr->recv_cq)->mcq.cqn,
+			    qp->ibqp.qp_num, qp->trans_qp.base.mqp.qpn,
+			    to_mcq(init_attr->recv_cq)->mcq.cqn,
 			    to_mcq(init_attr->send_cq)->mcq.cqn);
 
-		qp->xrcdn = xrcdn;
+		qp->trans_qp.xrcdn = xrcdn;
 
 		break;
 
 	case IB_QPT_RAW_IPV6:
 	case IB_QPT_RAW_ETHERTYPE:
-	case IB_QPT_RAW_PACKET:
 	case IB_QPT_MAX:
 	default:
 		mlx5_ib_dbg(dev, "unsupported qp type %d\n",
@@ -1318,12 +1721,12 @@ static __be32 to_mlx5_access_flags(struct mlx5_ib_qp *qp, const struct ib_qp_att
 	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)
 		dest_rd_atomic = attr->max_dest_rd_atomic;
 	else
-		dest_rd_atomic = qp->resp_depth;
+		dest_rd_atomic = qp->trans_qp.resp_depth;
 
 	if (attr_mask & IB_QP_ACCESS_FLAGS)
 		access_flags = attr->qp_access_flags;
 	else
-		access_flags = qp->atomic_rd_en;
+		access_flags = qp->trans_qp.atomic_rd_en;
 
 	if (!dest_rd_atomic)
 		access_flags &= IB_ACCESS_REMOTE_WRITE;
@@ -1360,21 +1763,42 @@ static int ib_rate_to_mlx5(struct mlx5_ib_dev *dev, u8 rate)
 	return rate + MLX5_STAT_RATE_OFFSET;
 }
 
-static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
+static int modify_raw_packet_eth_prio(struct mlx5_core_dev *dev,
+				      struct mlx5_ib_sq *sq, u8 sl)
+{
+	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.prio, 1);
+
+	tisc = MLX5_ADDR_OF(modify_tis_in, in, ctx);
+	MLX5_SET(tisc, tisc, prio, ((sl & 0x7) << 1));
+
+	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,
 			 u32 path_flags, const struct ib_qp_attr *attr)
 {
+	enum rdma_link_layer ll = rdma_port_get_link_layer(&dev->ib_dev, port);
 	int err;
 
-	path->fl = (path_flags & MLX5_PATH_FLAG_FL) ? 0x80 : 0;
-	path->free_ar = (path_flags & MLX5_PATH_FLAG_FREE_AR) ? 0x80 : 0;
-
 	if (attr_mask & IB_QP_PKEY_INDEX)
 		path->pkey_index = attr->pkey_index;
 
-	path->grh_mlid	= ah->src_path_bits & 0x7f;
-	path->rlid	= cpu_to_be16(ah->dlid);
-
 	if (ah->ah_flags & IB_AH_GRH) {
 		if (ah->grh.sgid_index >=
 		    dev->mdev->port_caps[port - 1].gid_table_len) {
@@ -1383,7 +1807,27 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 			       dev->mdev->port_caps[port - 1].gid_table_len);
 			return -EINVAL;
 		}
-		path->grh_mlid |= 1 << 7;
+	}
+
+	if (ll == IB_LINK_LAYER_ETHERNET) {
+		if (!(ah->ah_flags & IB_AH_GRH))
+			return -EINVAL;
+		memcpy(path->rmac, ah->dmac, sizeof(ah->dmac));
+		path->udp_sport = mlx5_get_roce_udp_sport(dev, port,
+							  ah->grh.sgid_index);
+		path->dci_cfi_prio_sl = (ah->sl & 0x7) << 4;
+	} else {
+		path->fl = (path_flags & MLX5_PATH_FLAG_FL) ? 0x80 : 0;
+		path->free_ar = (path_flags & MLX5_PATH_FLAG_FREE_AR) ? 0x80 :
+									0;
+		path->rlid = cpu_to_be16(ah->dlid);
+		path->grh_mlid = ah->src_path_bits & 0x7f;
+		if (ah->ah_flags & IB_AH_GRH)
+			path->grh_mlid	|= 1 << 7;
+		path->dci_cfi_prio_sl = ah->sl & 0xf;
+	}
+
+	if (ah->ah_flags & IB_AH_GRH) {
 		path->mgid_index = ah->grh.sgid_index;
 		path->hop_limit  = ah->grh.hop_limit;
 		path->tclass_flowlabel =
@@ -1401,7 +1845,10 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 	if (attr_mask & IB_QP_TIMEOUT)
 		path->ackto_lt = attr->timeout << 3;
 
-	path->sl = ah->sl & 0xf;
+	if ((qp->ibqp.qp_type == IB_QPT_RAW_PACKET) && qp->sq.wqe_cnt)
+		return modify_raw_packet_eth_prio(dev->mdev,
+						  &qp->raw_packet_qp.sq,
+						  ah->sl & 0xf);
 
 	return 0;
 }
@@ -1549,12 +1996,154 @@ 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)
+{
+	void *in;
+	void *rqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(modify_rq_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(modify_rq_in, in, rq_state, rq->state);
+
+	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 (err)
+		goto out;
+
+	rq->state = new_state;
+
+out:
+	kvfree(in);
+	return err;
+}
+
+static int modify_raw_packet_qp_sq(struct mlx5_core_dev *dev,
+				   struct mlx5_ib_sq *sq, int new_state)
+{
+	void *in;
+	void *sqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(modify_sq_in);
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(modify_sq_in, in, sq_state, sq->state);
+
+	sqc = MLX5_ADDR_OF(modify_sq_in, in, ctx);
+	MLX5_SET(sqc, sqc, state, new_state);
+
+	err = mlx5_core_modify_sq(dev, sq->base.mqp.qpn, in, inlen);
+	if (err)
+		goto out;
+
+	sq->state = new_state;
+
+out:
+	kvfree(in);
+	return err;
+}
+
+static int modify_raw_packet_qp(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
+				u16 operation)
+{
+	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
+	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
+	struct mlx5_ib_sq *sq = &raw_packet_qp->sq;
+	int rq_state;
+	int sq_state;
+	int err;
+
+	switch (operation) {
+	case MLX5_CMD_OP_RST2INIT_QP:
+		rq_state = MLX5_RQC_STATE_RDY;
+		sq_state = MLX5_SQC_STATE_RDY;
+		break;
+	case MLX5_CMD_OP_2ERR_QP:
+		rq_state = MLX5_RQC_STATE_ERR;
+		sq_state = MLX5_SQC_STATE_ERR;
+		break;
+	case MLX5_CMD_OP_2RST_QP:
+		rq_state = MLX5_RQC_STATE_RST;
+		sq_state = MLX5_SQC_STATE_RST;
+		break;
+	case MLX5_CMD_OP_INIT2INIT_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;
+	default:
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
+	if (qp->rq.wqe_cnt) {
+		err =  modify_raw_packet_qp_rq(dev->mdev, rq, rq_state);
+		if (err)
+			return err;
+	}
+
+	if (qp->sq.wqe_cnt)
+		return modify_raw_packet_qp_sq(dev->mdev, sq, sq_state);
+
+	return 0;
+}
+
 static int __mlx5_ib_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)
 {
+	static const u16 optab[MLX5_QP_NUM_STATE][MLX5_QP_NUM_STATE] = {
+		[MLX5_QP_STATE_RST] = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+			[MLX5_QP_STATE_INIT]	= MLX5_CMD_OP_RST2INIT_QP,
+		},
+		[MLX5_QP_STATE_INIT]  = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+			[MLX5_QP_STATE_INIT]	= MLX5_CMD_OP_INIT2INIT_QP,
+			[MLX5_QP_STATE_RTR]	= MLX5_CMD_OP_INIT2RTR_QP,
+		},
+		[MLX5_QP_STATE_RTR]   = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_RTR2RTS_QP,
+		},
+		[MLX5_QP_STATE_RTS]   = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_RTS2RTS_QP,
+		},
+		[MLX5_QP_STATE_SQD] = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+		},
+		[MLX5_QP_STATE_SQER] = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_SQERR2RTS_QP,
+		},
+		[MLX5_QP_STATE_ERR] = {
+			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
+			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
+		}
+	};
+
 	struct mlx5_ib_dev *dev = to_mdev(ibqp->device);
 	struct mlx5_ib_qp *qp = to_mqp(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;
@@ -1564,6 +2153,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	int sqd_event;
 	int mlx5_st;
 	int err;
+	u16 op;
 
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
 	if (!in)
@@ -1623,7 +2213,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 		context->pri_path.port = attr->port_num;
 
 	if (attr_mask & IB_QP_AV) {
-		err = mlx5_set_path(dev, &attr->ah_attr, &context->pri_path,
+		err = mlx5_set_path(dev, qp, &attr->ah_attr, &context->pri_path,
 				    attr_mask & IB_QP_PORT ? attr->port_num : qp->port,
 				    attr_mask, 0, attr);
 		if (err)
@@ -1634,7 +2224,8 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 		context->pri_path.ackto_lt |= attr->timeout << 3;
 
 	if (attr_mask & IB_QP_ALT_PATH) {
-		err = mlx5_set_path(dev, &attr->alt_ah_attr, &context->alt_path,
+		err = mlx5_set_path(dev, qp, &attr->alt_ah_attr,
+				    &context->alt_path,
 				    attr->alt_port_num, attr_mask, 0, attr);
 		if (err)
 			goto out;
@@ -1706,41 +2297,51 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	 * again to RTS, and may cause the driver and the device to get out of
 	 * sync. */
 	if (cur_state != IB_QPS_RESET && cur_state != IB_QPS_ERR &&
-	    (new_state == IB_QPS_RESET || new_state == IB_QPS_ERR))
+	    (new_state == IB_QPS_RESET || new_state == IB_QPS_ERR) &&
+	    (qp->ibqp.qp_type != IB_QPT_RAW_PACKET))
 		mlx5_ib_qp_disable_pagefaults(qp);
 
+	if (mlx5_cur >= MLX5_QP_NUM_STATE || mlx5_new >= MLX5_QP_NUM_STATE ||
+	    !optab[mlx5_cur][mlx5_new])
+		goto out;
+
+	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);
-	err = mlx5_core_qp_modify(dev->mdev, to_mlx5_state(cur_state),
-				  to_mlx5_state(new_state), in, sqd_event,
-				  &qp->mqp);
+
+	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,
+					  &base->mqp);
 	if (err)
 		goto out;
 
-	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT)
+	if (cur_state == IB_QPS_RESET && new_state == IB_QPS_INIT &&
+	    (qp->ibqp.qp_type != IB_QPT_RAW_PACKET))
 		mlx5_ib_qp_enable_pagefaults(qp);
 
 	qp->state = new_state;
 
 	if (attr_mask & IB_QP_ACCESS_FLAGS)
-		qp->atomic_rd_en = attr->qp_access_flags;
+		qp->trans_qp.atomic_rd_en = attr->qp_access_flags;
 	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)
-		qp->resp_depth = attr->max_dest_rd_atomic;
+		qp->trans_qp.resp_depth = attr->max_dest_rd_atomic;
 	if (attr_mask & IB_QP_PORT)
 		qp->port = attr->port_num;
 	if (attr_mask & IB_QP_ALT_PATH)
-		qp->alt_port = attr->alt_port_num;
+		qp->trans_qp.alt_port = attr->alt_port_num;
 
 	/*
 	 * If we moved a kernel QP to RESET, clean up all old CQ
 	 * entries and reinitialize the QP.
 	 */
 	if (new_state == IB_QPS_RESET && !ibqp->uobject) {
-		mlx5_ib_cq_clean(recv_cq, qp->mqp.qpn,
+		mlx5_ib_cq_clean(recv_cq, base->mqp.qpn,
 				 ibqp->srq ? to_msrq(ibqp->srq) : NULL);
 		if (send_cq != recv_cq)
-			mlx5_ib_cq_clean(send_cq, qp->mqp.qpn, NULL);
+			mlx5_ib_cq_clean(send_cq, base->mqp.qpn, NULL);
 
 		qp->rq.head = 0;
 		qp->rq.tail = 0;
@@ -1765,15 +2366,21 @@ int mlx5_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	enum ib_qp_state cur_state, new_state;
 	int err = -EINVAL;
 	int port;
+	enum rdma_link_layer ll = IB_LINK_LAYER_UNSPECIFIED;
 
 	mutex_lock(&qp->mutex);
 
 	cur_state = attr_mask & IB_QP_CUR_STATE ? attr->cur_qp_state : qp->state;
 	new_state = attr_mask & IB_QP_STATE ? attr->qp_state : cur_state;
 
+	if (!(cur_state == new_state && cur_state == IB_QPS_RESET)) {
+		port = attr_mask & IB_QP_PORT ? attr->port_num : qp->port;
+		ll = dev->ib_dev.get_link_layer(&dev->ib_dev, port);
+	}
+
 	if (ibqp->qp_type != MLX5_IB_QPT_REG_UMR &&
 	    !ib_modify_qp_is_ok(cur_state, new_state, ibqp->qp_type, attr_mask,
-				IB_LINK_LAYER_UNSPECIFIED))
+				ll))
 		goto out;
 
 	if ((attr_mask & IB_QP_PORT) &&
@@ -2570,7 +3177,7 @@ static void finish_wqe(struct mlx5_ib_qp *qp,
 
 	ctrl->opmod_idx_opcode = cpu_to_be32(((u32)(qp->sq.cur_post) << 8) |
 					     mlx5_opcode | ((u32)opmod << 24));
-	ctrl->qpn_ds = cpu_to_be32(size | (qp->mqp.qpn << 8));
+	ctrl->qpn_ds = cpu_to_be32(size | (qp->trans_qp.base.mqp.qpn << 8));
 	ctrl->fm_ce_se |= fence;
 	qp->fm_cache = next_fence;
 	if (unlikely(qp->wq_sig))
@@ -3003,7 +3610,7 @@ static void to_ib_ah_attr(struct mlx5_ib_dev *ibdev, struct ib_ah_attr *ib_ah_at
 	    ib_ah_attr->port_num > MLX5_CAP_GEN(dev, num_ports))
 		return;
 
-	ib_ah_attr->sl = path->sl & 0xf;
+	ib_ah_attr->sl = path->dci_cfi_prio_sl & 0xf;
 
 	ib_ah_attr->dlid	  = be16_to_cpu(path->rlid);
 	ib_ah_attr->src_path_bits = path->grh_mlid & 0x7f;
@@ -3021,39 +3628,153 @@ static void to_ib_ah_attr(struct mlx5_ib_dev *ibdev, struct ib_ah_attr *ib_ah_at
 	}
 }
 
-int mlx5_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr, int qp_attr_mask,
-		     struct ib_qp_init_attr *qp_init_attr)
+static int query_raw_packet_qp_sq_state(struct mlx5_ib_dev *dev,
+					struct mlx5_ib_sq *sq,
+					u8 *sq_state)
+{
+	void *out;
+	void *sqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(query_sq_out);
+	out = mlx5_vzalloc(inlen);
+	if (!out)
+		return -ENOMEM;
+
+	err = mlx5_core_query_sq(dev->mdev, sq->base.mqp.qpn, out);
+	if (err)
+		goto out;
+
+	sqc = MLX5_ADDR_OF(query_sq_out, out, sq_context);
+	*sq_state = MLX5_GET(sqc, sqc, state);
+	sq->state = *sq_state;
+
+out:
+	kvfree(out);
+	return err;
+}
+
+static int query_raw_packet_qp_rq_state(struct mlx5_ib_dev *dev,
+					struct mlx5_ib_rq *rq,
+					u8 *rq_state)
+{
+	void *out;
+	void *rqc;
+	int inlen;
+	int err;
+
+	inlen = MLX5_ST_SZ_BYTES(query_rq_out);
+	out = mlx5_vzalloc(inlen);
+	if (!out)
+		return -ENOMEM;
+
+	err = mlx5_core_query_rq(dev->mdev, rq->base.mqp.qpn, out);
+	if (err)
+		goto out;
+
+	rqc = MLX5_ADDR_OF(query_rq_out, out, rq_context);
+	*rq_state = MLX5_GET(rqc, rqc, state);
+	rq->state = *rq_state;
+
+out:
+	kvfree(out);
+	return err;
+}
+
+static int sqrq_state_to_qp_state(u8 sq_state, u8 rq_state,
+				  struct mlx5_ib_qp *qp, u8 *qp_state)
+{
+	static const u8 sqrq_trans[MLX5_RQ_NUM_STATE][MLX5_SQ_NUM_STATE] = {
+		[MLX5_RQC_STATE_RST] = {
+			[MLX5_SQC_STATE_RST]	= IB_QPS_RESET,
+			[MLX5_SQC_STATE_RDY]	= MLX5_QP_STATE_BAD,
+			[MLX5_SQC_STATE_ERR]	= MLX5_QP_STATE_BAD,
+			[MLX5_SQ_STATE_NA]	= IB_QPS_RESET,
+		},
+		[MLX5_RQC_STATE_RDY] = {
+			[MLX5_SQC_STATE_RST]	= MLX5_QP_STATE_BAD,
+			[MLX5_SQC_STATE_RDY]	= MLX5_QP_STATE,
+			[MLX5_SQC_STATE_ERR]	= IB_QPS_SQE,
+			[MLX5_SQ_STATE_NA]	= MLX5_QP_STATE,
+		},
+		[MLX5_RQC_STATE_ERR] = {
+			[MLX5_SQC_STATE_RST]    = MLX5_QP_STATE_BAD,
+			[MLX5_SQC_STATE_RDY]	= MLX5_QP_STATE_BAD,
+			[MLX5_SQC_STATE_ERR]	= IB_QPS_ERR,
+			[MLX5_SQ_STATE_NA]	= IB_QPS_ERR,
+		},
+		[MLX5_RQ_STATE_NA] = {
+			[MLX5_SQC_STATE_RST]    = IB_QPS_RESET,
+			[MLX5_SQC_STATE_RDY]	= MLX5_QP_STATE,
+			[MLX5_SQC_STATE_ERR]	= MLX5_QP_STATE,
+			[MLX5_SQ_STATE_NA]	= MLX5_QP_STATE_BAD,
+		},
+	};
+
+	*qp_state = sqrq_trans[rq_state][sq_state];
+
+	if (*qp_state == MLX5_QP_STATE_BAD) {
+		WARN(1, "Buggy Raw Packet QP state, SQ 0x%x state: 0x%x, RQ 0x%x state: 0x%x",
+		     qp->raw_packet_qp.sq.base.mqp.qpn, sq_state,
+		     qp->raw_packet_qp.rq.base.mqp.qpn, rq_state);
+		return -EINVAL;
+	}
+
+	if (*qp_state == MLX5_QP_STATE)
+		*qp_state = qp->state;
+
+	return 0;
+}
+
+static int query_raw_packet_qp_state(struct mlx5_ib_dev *dev,
+				     struct mlx5_ib_qp *qp,
+				     u8 *raw_packet_qp_state)
+{
+	struct mlx5_ib_raw_packet_qp *raw_packet_qp = &qp->raw_packet_qp;
+	struct mlx5_ib_sq *sq = &raw_packet_qp->sq;
+	struct mlx5_ib_rq *rq = &raw_packet_qp->rq;
+	int err;
+	u8 sq_state = MLX5_SQ_STATE_NA;
+	u8 rq_state = MLX5_RQ_STATE_NA;
+
+	if (qp->sq.wqe_cnt) {
+		err = query_raw_packet_qp_sq_state(dev, sq, &sq_state);
+		if (err)
+			return err;
+	}
+
+	if (qp->rq.wqe_cnt) {
+		err = query_raw_packet_qp_rq_state(dev, rq, &rq_state);
+		if (err)
+			return err;
+	}
+
+	return sqrq_state_to_qp_state(sq_state, rq_state, qp,
+				      raw_packet_qp_state);
+}
+
+static int query_qp_attr(struct mlx5_ib_dev *dev, struct mlx5_ib_qp *qp,
+			 struct ib_qp_attr *qp_attr)
 {
-	struct mlx5_ib_dev *dev = to_mdev(ibqp->device);
-	struct mlx5_ib_qp *qp = to_mqp(ibqp);
 	struct mlx5_query_qp_mbox_out *outb;
 	struct mlx5_qp_context *context;
 	int mlx5_state;
 	int err = 0;
 
-#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
-	/*
-	 * Wait for any outstanding page faults, in case the user frees memory
-	 * based upon this query's result.
-	 */
-	flush_workqueue(mlx5_ib_page_fault_wq);
-#endif
-
-	mutex_lock(&qp->mutex);
 	outb = kzalloc(sizeof(*outb), GFP_KERNEL);
-	if (!outb) {
-		err = -ENOMEM;
-		goto out;
-	}
+	if (!outb)
+		return -ENOMEM;
+
 	context = &outb->ctx;
-	err = mlx5_core_qp_query(dev->mdev, &qp->mqp, outb, sizeof(*outb));
+	err = mlx5_core_qp_query(dev->mdev, &qp->trans_qp.base.mqp, outb,
+				 sizeof(*outb));
 	if (err)
-		goto out_free;
+		goto out;
 
 	mlx5_state = be32_to_cpu(context->flags) >> 28;
 
 	qp->state		     = to_ib_qp_state(mlx5_state);
-	qp_attr->qp_state	     = qp->state;
 	qp_attr->path_mtu	     = context->mtu_msgmax >> 5;
 	qp_attr->path_mig_state	     =
 		to_ib_mig_state((be32_to_cpu(context->flags) >> 11) & 0x3);
@@ -3087,6 +3808,43 @@ int mlx5_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr, int qp_attr
 	qp_attr->retry_cnt	    = (be32_to_cpu(context->params1) >> 16) & 0x7;
 	qp_attr->rnr_retry	    = (be32_to_cpu(context->params1) >> 13) & 0x7;
 	qp_attr->alt_timeout	    = context->alt_path.ackto_lt >> 3;
+
+out:
+	kfree(outb);
+	return err;
+}
+
+int mlx5_ib_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 mlx5_ib_dev *dev = to_mdev(ibqp->device);
+	struct mlx5_ib_qp *qp = to_mqp(ibqp);
+	int err = 0;
+	u8 raw_packet_qp_state;
+
+#ifdef CONFIG_INFINIBAND_ON_DEMAND_PAGING
+	/*
+	 * Wait for any outstanding page faults, in case the user frees memory
+	 * based upon this query's result.
+	 */
+	flush_workqueue(mlx5_ib_page_fault_wq);
+#endif
+
+	mutex_lock(&qp->mutex);
+
+	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET) {
+		err = query_raw_packet_qp_state(dev, qp, &raw_packet_qp_state);
+		if (err)
+			goto out;
+		qp->state = raw_packet_qp_state;
+		qp_attr->port_num = 1;
+	} else {
+		err = query_qp_attr(dev, qp, qp_attr);
+		if (err)
+			goto out;
+	}
+
+	qp_attr->qp_state	     = qp->state;
 	qp_attr->cur_qp_state	     = qp_attr->qp_state;
 	qp_attr->cap.max_recv_wr     = qp->rq.wqe_cnt;
 	qp_attr->cap.max_recv_sge    = qp->rq.max_gs;
@@ -3110,12 +3868,16 @@ int mlx5_ib_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *qp_attr, int qp_attr
 	if (qp->flags & MLX5_IB_QP_BLOCK_MULTICAST_LOOPBACK)
 		qp_init_attr->create_flags |= IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK;
 
+	if (qp->flags & MLX5_IB_QP_CROSS_CHANNEL)
+		qp_init_attr->create_flags |= IB_QP_CREATE_CROSS_CHANNEL;
+	if (qp->flags & MLX5_IB_QP_MANAGED_SEND)
+		qp_init_attr->create_flags |= IB_QP_CREATE_MANAGED_SEND;
+	if (qp->flags & MLX5_IB_QP_MANAGED_RECV)
+		qp_init_attr->create_flags |= IB_QP_CREATE_MANAGED_RECV;
+
 	qp_init_attr->sq_sig_type = qp->sq_signal_bits & MLX5_WQE_CTRL_CQ_UPDATE ?
 		IB_SIGNAL_ALL_WR : IB_SIGNAL_REQ_WR;
 
-out_free:
-	kfree(outb);
-
 out:
 	mutex_unlock(&qp->mutex);
 	return err;
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index e008505e96e9..3b2ddd64a371 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -75,31 +75,42 @@ static void mlx5_ib_srq_event(struct mlx5_core_srq *srq, enum mlx5_event type)
 
 static int create_srq_user(struct ib_pd *pd, struct mlx5_ib_srq *srq,
 			   struct mlx5_create_srq_mbox_in **in,
-			   struct ib_udata *udata, int buf_size, int *inlen)
+			   struct ib_udata *udata, int buf_size, int *inlen,
+			   int is_xrc)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
-	struct mlx5_ib_create_srq ucmd;
+	struct mlx5_ib_create_srq ucmd = {};
 	size_t ucmdlen;
+	void *xsrqc;
 	int err;
 	int npages;
 	int page_shift;
 	int ncont;
 	u32 offset;
+	u32 uidx = MLX5_IB_DEFAULT_UIDX;
 
-	ucmdlen =
-		(udata->inlen - sizeof(struct ib_uverbs_cmd_hdr) <
-		 sizeof(ucmd)) ? (sizeof(ucmd) -
-				  sizeof(ucmd.reserved)) : sizeof(ucmd);
+	ucmdlen = min(udata->inlen, sizeof(ucmd));
 
 	if (ib_copy_from_udata(&ucmd, udata, ucmdlen)) {
 		mlx5_ib_dbg(dev, "failed copy udata\n");
 		return -EFAULT;
 	}
 
-	if (ucmdlen == sizeof(ucmd) &&
-	    ucmd.reserved != 0)
+	if (ucmd.reserved0 || ucmd.reserved1)
 		return -EINVAL;
 
+	if (udata->inlen > sizeof(ucmd) &&
+	    !ib_is_udata_cleared(udata, sizeof(ucmd),
+				 udata->inlen - sizeof(ucmd)))
+		return -EINVAL;
+
+	if (is_xrc) {
+		err = get_srq_user_index(to_mucontext(pd->uobject->context),
+					 &ucmd, udata->inlen, &uidx);
+		if (err)
+			return err;
+	}
+
 	srq->wq_sig = !!(ucmd.flags & MLX5_SRQ_FLAG_SIGNATURE);
 
 	srq->umem = ib_umem_get(pd->uobject->context, ucmd.buf_addr, buf_size,
@@ -138,6 +149,13 @@ static int create_srq_user(struct ib_pd *pd, struct mlx5_ib_srq *srq,
 	(*in)->ctx.log_pg_sz = page_shift - MLX5_ADAPTER_PAGE_SHIFT;
 	(*in)->ctx.pgoff_cqn = cpu_to_be32(offset << 26);
 
+	if ((MLX5_CAP_GEN(dev->mdev, cqe_version) == MLX5_CQE_VERSION_V1) &&
+	     is_xrc){
+		xsrqc = MLX5_ADDR_OF(create_xrc_srq_in, *in,
+				     xrc_srq_context_entry);
+		MLX5_SET(xrc_srqc, xsrqc, user_index, uidx);
+	}
+
 	return 0;
 
 err_in:
@@ -151,13 +169,14 @@ err_umem:
 
 static int create_srq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_srq *srq,
 			     struct mlx5_create_srq_mbox_in **in, int buf_size,
-			     int *inlen)
+			     int *inlen, int is_xrc)
 {
 	int err;
 	int i;
 	struct mlx5_wqe_srq_next_seg *next;
 	int page_shift;
 	int npages;
+	void *xsrqc;
 
 	err = mlx5_db_alloc(dev->mdev, &srq->db);
 	if (err) {
@@ -204,6 +223,14 @@ static int create_srq_kernel(struct mlx5_ib_dev *dev, struct mlx5_ib_srq *srq,
 
 	(*in)->ctx.log_pg_sz = page_shift - MLX5_ADAPTER_PAGE_SHIFT;
 
+	if ((MLX5_CAP_GEN(dev->mdev, cqe_version) == MLX5_CQE_VERSION_V1) &&
+	     is_xrc){
+		xsrqc = MLX5_ADDR_OF(create_xrc_srq_in, *in,
+				     xrc_srq_context_entry);
+		/* 0xffffff means we ask to work with cqe version 0 */
+		MLX5_SET(xrc_srqc, xsrqc, user_index, MLX5_IB_DEFAULT_UIDX);
+	}
+
 	return 0;
 
 err_in:
@@ -275,10 +302,14 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 		    desc_size, init_attr->attr.max_wr, srq->msrq.max, srq->msrq.max_gs,
 		    srq->msrq.max_avail_gather);
 
+	is_xrc = (init_attr->srq_type == IB_SRQT_XRC);
+
 	if (pd->uobject)
-		err = create_srq_user(pd, srq, &in, udata, buf_size, &inlen);
+		err = create_srq_user(pd, srq, &in, udata, buf_size, &inlen,
+				      is_xrc);
 	else
-		err = create_srq_kernel(dev, srq, &in, buf_size, &inlen);
+		err = create_srq_kernel(dev, srq, &in, buf_size, &inlen,
+					is_xrc);
 
 	if (err) {
 		mlx5_ib_warn(dev, "create srq %s failed, err %d\n",
@@ -286,7 +317,6 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 		goto err_srq;
 	}
 
-	is_xrc = (init_attr->srq_type == IB_SRQT_XRC);
 	in->ctx.state_log_sz = ilog2(srq->msrq.max);
 	flgs = ((srq->msrq.wqe_shift - 4) | (is_xrc << 5) | (srq->wq_sig << 7)) << 24;
 	xrcdn = 0;
diff --git a/drivers/infiniband/hw/mlx5/user.h b/drivers/infiniband/hw/mlx5/user.h
index 76fb7b927d37..b94a55404a59 100644
--- a/drivers/infiniband/hw/mlx5/user.h
+++ b/drivers/infiniband/hw/mlx5/user.h
@@ -35,6 +35,8 @@
 
 #include <linux/types.h>
 
+#include "mlx5_ib.h"
+
 enum {
 	MLX5_QP_FLAG_SIGNATURE		= 1 << 0,
 	MLX5_QP_FLAG_SCATTER_CQE	= 1 << 1,
@@ -66,7 +68,15 @@ struct mlx5_ib_alloc_ucontext_req_v2 {
 	__u32	total_num_uuars;
 	__u32	num_low_latency_uuars;
 	__u32	flags;
-	__u32	reserved;
+	__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,
 };
 
 struct mlx5_ib_alloc_ucontext_resp {
@@ -80,7 +90,13 @@ struct mlx5_ib_alloc_ucontext_resp {
 	__u32	max_recv_wr;
 	__u32	max_srq_recv_wr;
 	__u16	num_ports;
-	__u16	reserved;
+	__u16	reserved1;
+	__u32	comp_mask;
+	__u32	response_length;
+	__u8	cqe_version;
+	__u8	reserved2;
+	__u16	reserved3;
+	__u64	hca_core_clock_offset;
 };
 
 struct mlx5_ib_alloc_pd_resp {
@@ -110,7 +126,9 @@ struct mlx5_ib_create_srq {
 	__u64	buf_addr;
 	__u64	db_addr;
 	__u32	flags;
-	__u32	reserved; /* explicit padding (optional on i386) */
+	__u32	reserved0; /* explicit padding (optional on i386) */
+	__u32	uidx;
+	__u32	reserved1;
 };
 
 struct mlx5_ib_create_srq_resp {
@@ -125,9 +143,48 @@ struct mlx5_ib_create_qp {
 	__u32	rq_wqe_count;
 	__u32	rq_wqe_shift;
 	__u32	flags;
+	__u32	uidx;
+	__u32	reserved0;
+	__u64	sq_buf_addr;
 };
 
 struct mlx5_ib_create_qp_resp {
 	__u32	uuar_index;
 };
+
+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_cq.c b/drivers/infiniband/hw/mthca/mthca_cq.c
index 40ba83338155..a6531ffe29a6 100644
--- a/drivers/infiniband/hw/mthca/mthca_cq.c
+++ b/drivers/infiniband/hw/mthca/mthca_cq.c
@@ -608,9 +608,6 @@ static inline int mthca_poll_one(struct mthca_dev *dev,
 			entry->opcode    = IB_WC_FETCH_ADD;
 			entry->byte_len  = MTHCA_ATOMIC_BYTE_LEN;
 			break;
-		case MTHCA_OPCODE_BIND_MW:
-			entry->opcode    = IB_WC_BIND_MW;
-			break;
 		default:
 			entry->opcode    = MTHCA_OPCODE_INVALID;
 			break;
diff --git a/drivers/infiniband/hw/mthca/mthca_provider.c b/drivers/infiniband/hw/mthca/mthca_provider.c
index dc2d48c59e62..9866c35cc977 100644
--- a/drivers/infiniband/hw/mthca/mthca_provider.c
+++ b/drivers/infiniband/hw/mthca/mthca_provider.c
@@ -898,89 +898,6 @@ static struct ib_mr *mthca_get_dma_mr(struct ib_pd *pd, int acc)
 	return &mr->ibmr;
 }
 
-static struct ib_mr *mthca_reg_phys_mr(struct ib_pd       *pd,
-				       struct ib_phys_buf *buffer_list,
-				       int                 num_phys_buf,
-				       int                 acc,
-				       u64                *iova_start)
-{
-	struct mthca_mr *mr;
-	u64 *page_list;
-	u64 total_size;
-	unsigned long mask;
-	int shift;
-	int npages;
-	int err;
-	int i, j, n;
-
-	mask = buffer_list[0].addr ^ *iova_start;
-	total_size = 0;
-	for (i = 0; i < num_phys_buf; ++i) {
-		if (i != 0)
-			mask |= buffer_list[i].addr;
-		if (i != num_phys_buf - 1)
-			mask |= buffer_list[i].addr + buffer_list[i].size;
-
-		total_size += buffer_list[i].size;
-	}
-
-	if (mask & ~PAGE_MASK)
-		return ERR_PTR(-EINVAL);
-
-	shift = __ffs(mask | 1 << 31);
-
-	buffer_list[0].size += buffer_list[0].addr & ((1ULL << shift) - 1);
-	buffer_list[0].addr &= ~0ull << shift;
-
-	mr = kmalloc(sizeof *mr, GFP_KERNEL);
-	if (!mr)
-		return ERR_PTR(-ENOMEM);
-
-	npages = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		npages += (buffer_list[i].size + (1ULL << shift) - 1) >> shift;
-
-	if (!npages)
-		return &mr->ibmr;
-
-	page_list = kmalloc(npages * sizeof *page_list, GFP_KERNEL);
-	if (!page_list) {
-		kfree(mr);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	n = 0;
-	for (i = 0; i < num_phys_buf; ++i)
-		for (j = 0;
-		     j < (buffer_list[i].size + (1ULL << shift) - 1) >> shift;
-		     ++j)
-			page_list[n++] = buffer_list[i].addr + ((u64) j << shift);
-
-	mthca_dbg(to_mdev(pd->device), "Registering memory at %llx (iova %llx) "
-		  "in PD %x; shift %d, npages %d.\n",
-		  (unsigned long long) buffer_list[0].addr,
-		  (unsigned long long) *iova_start,
-		  to_mpd(pd)->pd_num,
-		  shift, npages);
-
-	err = mthca_mr_alloc_phys(to_mdev(pd->device),
-				  to_mpd(pd)->pd_num,
-				  page_list, shift, npages,
-				  *iova_start, total_size,
-				  convert_access(acc), mr);
-
-	if (err) {
-		kfree(page_list);
-		kfree(mr);
-		return ERR_PTR(err);
-	}
-
-	kfree(page_list);
-	mr->umem = NULL;
-
-	return &mr->ibmr;
-}
-
 static struct ib_mr *mthca_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 				       u64 virt, int acc, struct ib_udata *udata)
 {
@@ -1346,7 +1263,6 @@ int mthca_register_device(struct mthca_dev *dev)
 	dev->ib_dev.destroy_cq           = mthca_destroy_cq;
 	dev->ib_dev.poll_cq              = mthca_poll_cq;
 	dev->ib_dev.get_dma_mr           = mthca_get_dma_mr;
-	dev->ib_dev.reg_phys_mr          = mthca_reg_phys_mr;
 	dev->ib_dev.reg_user_mr          = mthca_reg_user_mr;
 	dev->ib_dev.dereg_mr             = mthca_dereg_mr;
 	dev->ib_dev.get_port_immutable   = mthca_port_immutable;
diff --git a/drivers/infiniband/hw/mthca/mthca_qp.c b/drivers/infiniband/hw/mthca/mthca_qp.c
index 35fe506e2cfa..96e5fb91fb48 100644
--- a/drivers/infiniband/hw/mthca/mthca_qp.c
+++ b/drivers/infiniband/hw/mthca/mthca_qp.c
@@ -1485,7 +1485,7 @@ static int build_mlx_header(struct mthca_dev *dev, struct mthca_sqp *sqp,
 	u16 pkey;
 
 	ib_ud_header_init(256, /* assume a MAD */ 1, 0, 0,
-			  mthca_ah_grh_present(to_mah(wr->ah)), 0,
+			  mthca_ah_grh_present(to_mah(wr->ah)), 0, 0, 0,
 			  &sqp->ud_header);
 
 	err = mthca_read_ah(dev, to_mah(wr->ah), &sqp->ud_header);
diff --git a/drivers/infiniband/hw/nes/nes_cm.c b/drivers/infiniband/hw/nes/nes_cm.c
index 8a3ad170d790..cb9f0f27308d 100644
--- a/drivers/infiniband/hw/nes/nes_cm.c
+++ b/drivers/infiniband/hw/nes/nes_cm.c
@@ -134,7 +134,7 @@ static void record_ird_ord(struct nes_cm_node *, u16, u16);
 /* External CM API Interface */
 /* instance of function pointers for client API */
 /* set address of this instance to cm_core->cm_ops at cm_core alloc */
-static struct nes_cm_ops nes_cm_api = {
+static const struct nes_cm_ops nes_cm_api = {
 	mini_cm_accelerated,
 	mini_cm_listen,
 	mini_cm_del_listen,
@@ -3232,7 +3232,6 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 	int passive_state;
 	struct nes_ib_device *nesibdev;
 	struct ib_mr *ibmr = NULL;
-	struct ib_phys_buf ibphysbuf;
 	struct nes_pd *nespd;
 	u64 tagged_offset;
 	u8 mpa_frame_offset = 0;
@@ -3316,21 +3315,19 @@ int nes_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *conn_param)
 		u64temp = (unsigned long)nesqp;
 		nesibdev = nesvnic->nesibdev;
 		nespd = nesqp->nespd;
-		ibphysbuf.addr = nesqp->ietf_frame_pbase + mpa_frame_offset;
-		ibphysbuf.size = buff_len;
 		tagged_offset = (u64)(unsigned long)*start_buff;
-		ibmr = nesibdev->ibdev.reg_phys_mr((struct ib_pd *)nespd,
-						   &ibphysbuf, 1,
-						   IB_ACCESS_LOCAL_WRITE,
-						   &tagged_offset);
-		if (!ibmr) {
+		ibmr = nes_reg_phys_mr(&nespd->ibpd,
+				nesqp->ietf_frame_pbase + mpa_frame_offset,
+				buff_len, IB_ACCESS_LOCAL_WRITE,
+				&tagged_offset);
+		if (IS_ERR(ibmr)) {
 			nes_debug(NES_DBG_CM, "Unable to register memory region"
 				  "for lSMM for cm_node = %p \n",
 				  cm_node);
 			pci_free_consistent(nesdev->pcidev,
 					    nesqp->private_data_len + nesqp->ietf_frame_size,
 					    nesqp->ietf_frame, nesqp->ietf_frame_pbase);
-			return -ENOMEM;
+			return PTR_ERR(ibmr);
 		}
 
 		ibmr->pd = &nespd->ibpd;
diff --git a/drivers/infiniband/hw/nes/nes_cm.h b/drivers/infiniband/hw/nes/nes_cm.h
index 32a6420c2940..147c2c884227 100644
--- a/drivers/infiniband/hw/nes/nes_cm.h
+++ b/drivers/infiniband/hw/nes/nes_cm.h
@@ -423,7 +423,7 @@ struct nes_cm_core {
 
 	struct timer_list       tcp_timer;
 
-	struct nes_cm_ops       *api;
+	const struct nes_cm_ops *api;
 
 	int (*post_event)(struct nes_cm_event *event);
 	atomic_t                events_posted;
diff --git a/drivers/infiniband/hw/nes/nes_utils.c b/drivers/infiniband/hw/nes/nes_utils.c
index 2042c0f29759..6d3a169c049b 100644
--- a/drivers/infiniband/hw/nes/nes_utils.c
+++ b/drivers/infiniband/hw/nes/nes_utils.c
@@ -727,7 +727,7 @@ int nes_arp_table(struct nes_device *nesdev, u32 ip_addr, u8 *mac_addr, u32 acti
 	if (action == NES_ARP_DELETE) {
 		nes_debug(NES_DBG_NETDEV, "DELETE, arp_index=%d\n", arp_index);
 		nesadapter->arp_table[arp_index].ip_addr = 0;
-		memset(nesadapter->arp_table[arp_index].mac_addr, 0x00, ETH_ALEN);
+		eth_zero_addr(nesadapter->arp_table[arp_index].mac_addr);
 		nes_free_resource(nesadapter, nesadapter->allocated_arps, arp_index);
 		return arp_index;
 	}
diff --git a/drivers/infiniband/hw/nes/nes_verbs.c b/drivers/infiniband/hw/nes/nes_verbs.c
index 137880a19ebe..8c4daf7f22ec 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.c
+++ b/drivers/infiniband/hw/nes/nes_verbs.c
@@ -206,80 +206,6 @@ static int nes_dealloc_mw(struct ib_mw *ibmw)
 }
 
 
-/**
- * nes_bind_mw
- */
-static int nes_bind_mw(struct ib_qp *ibqp, struct ib_mw *ibmw,
-		struct ib_mw_bind *ibmw_bind)
-{
-	u64 u64temp;
-	struct nes_vnic *nesvnic = to_nesvnic(ibqp->device);
-	struct nes_device *nesdev = nesvnic->nesdev;
-	/* struct nes_mr *nesmr = to_nesmw(ibmw); */
-	struct nes_qp *nesqp = to_nesqp(ibqp);
-	struct nes_hw_qp_wqe *wqe;
-	unsigned long flags = 0;
-	u32 head;
-	u32 wqe_misc = 0;
-	u32 qsize;
-
-	if (nesqp->ibqp_state > IB_QPS_RTS)
-		return -EINVAL;
-
-	spin_lock_irqsave(&nesqp->lock, flags);
-
-	head = nesqp->hwqp.sq_head;
-	qsize = nesqp->hwqp.sq_tail;
-
-	/* Check for SQ overflow */
-	if (((head + (2 * qsize) - nesqp->hwqp.sq_tail) % qsize) == (qsize - 1)) {
-		spin_unlock_irqrestore(&nesqp->lock, flags);
-		return -ENOMEM;
-	}
-
-	wqe = &nesqp->hwqp.sq_vbase[head];
-	/* nes_debug(NES_DBG_MR, "processing sq wqe at %p, head = %u.\n", wqe, head); */
-	nes_fill_init_qp_wqe(wqe, nesqp, head);
-	u64temp = ibmw_bind->wr_id;
-	set_wqe_64bit_value(wqe->wqe_words, NES_IWARP_SQ_WQE_COMP_SCRATCH_LOW_IDX, u64temp);
-	wqe_misc = NES_IWARP_SQ_OP_BIND;
-
-	wqe_misc |= NES_IWARP_SQ_WQE_LOCAL_FENCE;
-
-	if (ibmw_bind->send_flags & IB_SEND_SIGNALED)
-		wqe_misc |= NES_IWARP_SQ_WQE_SIGNALED_COMPL;
-
-	if (ibmw_bind->bind_info.mw_access_flags & IB_ACCESS_REMOTE_WRITE)
-		wqe_misc |= NES_CQP_STAG_RIGHTS_REMOTE_WRITE;
-	if (ibmw_bind->bind_info.mw_access_flags & IB_ACCESS_REMOTE_READ)
-		wqe_misc |= NES_CQP_STAG_RIGHTS_REMOTE_READ;
-
-	set_wqe_32bit_value(wqe->wqe_words, NES_IWARP_SQ_WQE_MISC_IDX, wqe_misc);
-	set_wqe_32bit_value(wqe->wqe_words, NES_IWARP_SQ_BIND_WQE_MR_IDX,
-			    ibmw_bind->bind_info.mr->lkey);
-	set_wqe_32bit_value(wqe->wqe_words, NES_IWARP_SQ_BIND_WQE_MW_IDX, ibmw->rkey);
-	set_wqe_32bit_value(wqe->wqe_words, NES_IWARP_SQ_BIND_WQE_LENGTH_LOW_IDX,
-			ibmw_bind->bind_info.length);
-	wqe->wqe_words[NES_IWARP_SQ_BIND_WQE_LENGTH_HIGH_IDX] = 0;
-	u64temp = (u64)ibmw_bind->bind_info.addr;
-	set_wqe_64bit_value(wqe->wqe_words, NES_IWARP_SQ_BIND_WQE_VA_FBO_LOW_IDX, u64temp);
-
-	head++;
-	if (head >= qsize)
-		head = 0;
-
-	nesqp->hwqp.sq_head = head;
-	barrier();
-
-	nes_write32(nesdev->regs+NES_WQE_ALLOC,
-			(1 << 24) | 0x00800000 | nesqp->hwqp.qp_id);
-
-	spin_unlock_irqrestore(&nesqp->lock, flags);
-
-	return 0;
-}
-
-
 /*
  * nes_alloc_fast_mr
  */
@@ -2074,9 +2000,8 @@ static int nes_reg_mr(struct nes_device *nesdev, struct nes_pd *nespd,
 /**
  * nes_reg_phys_mr
  */
-static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
-		struct ib_phys_buf *buffer_list, int num_phys_buf, int acc,
-		u64 * iova_start)
+struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd, u64 addr, u64 size,
+		int acc, u64 *iova_start)
 {
 	u64 region_length;
 	struct nes_pd *nespd = to_nespd(ib_pd);
@@ -2088,13 +2013,10 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
 	struct nes_vpbl vpbl;
 	struct nes_root_vpbl root_vpbl;
 	u32 stag;
-	u32 i;
 	unsigned long mask;
 	u32 stag_index = 0;
 	u32 next_stag_index = 0;
 	u32 driver_key = 0;
-	u32 root_pbl_index = 0;
-	u32 cur_pbl_index = 0;
 	int err = 0;
 	int ret = 0;
 	u16 pbl_count = 0;
@@ -2113,11 +2035,8 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
 
 	next_stag_index >>= 8;
 	next_stag_index %= nesadapter->max_mr;
-	if (num_phys_buf > (1024*512)) {
-		return ERR_PTR(-E2BIG);
-	}
 
-	if ((buffer_list[0].addr ^ *iova_start) & ~PAGE_MASK)
+	if ((addr ^ *iova_start) & ~PAGE_MASK)
 		return ERR_PTR(-EINVAL);
 
 	err = nes_alloc_resource(nesadapter, nesadapter->allocated_mrs, nesadapter->max_mr,
@@ -2132,84 +2051,33 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
 		return ERR_PTR(-ENOMEM);
 	}
 
-	for (i = 0; i < num_phys_buf; i++) {
+	/* Allocate a 4K buffer for the PBL */
+	vpbl.pbl_vbase = pci_alloc_consistent(nesdev->pcidev, 4096,
+			&vpbl.pbl_pbase);
+	nes_debug(NES_DBG_MR, "Allocating leaf PBL, va = %p, pa = 0x%016lX\n",
+			vpbl.pbl_vbase, (unsigned long)vpbl.pbl_pbase);
+	if (!vpbl.pbl_vbase) {
+		nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
+		ibmr = ERR_PTR(-ENOMEM);
+		kfree(nesmr);
+		goto reg_phys_err;
+	}
 
-		if ((i & 0x01FF) == 0) {
-			if (root_pbl_index == 1) {
-				/* Allocate the root PBL */
-				root_vpbl.pbl_vbase = pci_alloc_consistent(nesdev->pcidev, 8192,
-						&root_vpbl.pbl_pbase);
-				nes_debug(NES_DBG_MR, "Allocating root PBL, va = %p, pa = 0x%08X\n",
-						root_vpbl.pbl_vbase, (unsigned int)root_vpbl.pbl_pbase);
-				if (!root_vpbl.pbl_vbase) {
-					pci_free_consistent(nesdev->pcidev, 4096, vpbl.pbl_vbase,
-							vpbl.pbl_pbase);
-					nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
-					kfree(nesmr);
-					return ERR_PTR(-ENOMEM);
-				}
-				root_vpbl.leaf_vpbl = kzalloc(sizeof(*root_vpbl.leaf_vpbl)*1024, GFP_KERNEL);
-				if (!root_vpbl.leaf_vpbl) {
-					pci_free_consistent(nesdev->pcidev, 8192, root_vpbl.pbl_vbase,
-							root_vpbl.pbl_pbase);
-					pci_free_consistent(nesdev->pcidev, 4096, vpbl.pbl_vbase,
-							vpbl.pbl_pbase);
-					nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
-					kfree(nesmr);
-					return ERR_PTR(-ENOMEM);
-				}
-				root_vpbl.pbl_vbase[0].pa_low = cpu_to_le32((u32)vpbl.pbl_pbase);
-				root_vpbl.pbl_vbase[0].pa_high =
-						cpu_to_le32((u32)((((u64)vpbl.pbl_pbase) >> 32)));
-				root_vpbl.leaf_vpbl[0] = vpbl;
-			}
-			/* Allocate a 4K buffer for the PBL */
-			vpbl.pbl_vbase = pci_alloc_consistent(nesdev->pcidev, 4096,
-					&vpbl.pbl_pbase);
-			nes_debug(NES_DBG_MR, "Allocating leaf PBL, va = %p, pa = 0x%016lX\n",
-					vpbl.pbl_vbase, (unsigned long)vpbl.pbl_pbase);
-			if (!vpbl.pbl_vbase) {
-				nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
-				ibmr = ERR_PTR(-ENOMEM);
-				kfree(nesmr);
-				goto reg_phys_err;
-			}
-			/* Fill in the root table */
-			if (1 <= root_pbl_index) {
-				root_vpbl.pbl_vbase[root_pbl_index].pa_low =
-						cpu_to_le32((u32)vpbl.pbl_pbase);
-				root_vpbl.pbl_vbase[root_pbl_index].pa_high =
-						cpu_to_le32((u32)((((u64)vpbl.pbl_pbase) >> 32)));
-				root_vpbl.leaf_vpbl[root_pbl_index] = vpbl;
-			}
-			root_pbl_index++;
-			cur_pbl_index = 0;
-		}
 
-		mask = !buffer_list[i].size;
-		if (i != 0)
-			mask |= buffer_list[i].addr;
-		if (i != num_phys_buf - 1)
-			mask |= buffer_list[i].addr + buffer_list[i].size;
-
-		if (mask & ~PAGE_MASK) {
-			nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
-			nes_debug(NES_DBG_MR, "Invalid buffer addr or size\n");
-			ibmr = ERR_PTR(-EINVAL);
-			kfree(nesmr);
-			goto reg_phys_err;
-		}
+	mask = !size;
 
-		region_length += buffer_list[i].size;
-		if ((i != 0) && (single_page)) {
-			if ((buffer_list[i-1].addr+PAGE_SIZE) != buffer_list[i].addr)
-				single_page = 0;
-		}
-		vpbl.pbl_vbase[cur_pbl_index].pa_low = cpu_to_le32((u32)buffer_list[i].addr & PAGE_MASK);
-		vpbl.pbl_vbase[cur_pbl_index++].pa_high =
-				cpu_to_le32((u32)((((u64)buffer_list[i].addr) >> 32)));
+	if (mask & ~PAGE_MASK) {
+		nes_free_resource(nesadapter, nesadapter->allocated_mrs, stag_index);
+		nes_debug(NES_DBG_MR, "Invalid buffer addr or size\n");
+		ibmr = ERR_PTR(-EINVAL);
+		kfree(nesmr);
+		goto reg_phys_err;
 	}
 
+	region_length += size;
+	vpbl.pbl_vbase[0].pa_low = cpu_to_le32((u32)addr & PAGE_MASK);
+	vpbl.pbl_vbase[0].pa_high = cpu_to_le32((u32)((((u64)addr) >> 32)));
+
 	stag = stag_index << 8;
 	stag |= driver_key;
 	stag += (u32)stag_key;
@@ -2219,17 +2087,15 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
 			stag, (unsigned long)*iova_start, (unsigned long)region_length, stag_index);
 
 	/* Make the leaf PBL the root if only one PBL */
-	if (root_pbl_index == 1) {
-		root_vpbl.pbl_pbase = vpbl.pbl_pbase;
-	}
+	root_vpbl.pbl_pbase = vpbl.pbl_pbase;
 
 	if (single_page) {
 		pbl_count = 0;
 	} else {
-		pbl_count = root_pbl_index;
+		pbl_count = 1;
 	}
 	ret = nes_reg_mr(nesdev, nespd, stag, region_length, &root_vpbl,
-			buffer_list[0].addr, pbl_count, (u16)cur_pbl_index, acc, iova_start,
+			addr, pbl_count, 1, acc, iova_start,
 			&nesmr->pbls_used, &nesmr->pbl_4k);
 
 	if (ret == 0) {
@@ -2242,21 +2108,9 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
 		ibmr = ERR_PTR(-ENOMEM);
 	}
 
-	reg_phys_err:
-	/* free the resources */
-	if (root_pbl_index == 1) {
-		/* single PBL case */
-		pci_free_consistent(nesdev->pcidev, 4096, vpbl.pbl_vbase, vpbl.pbl_pbase);
-	} else {
-		for (i=0; i<root_pbl_index; i++) {
-			pci_free_consistent(nesdev->pcidev, 4096, root_vpbl.leaf_vpbl[i].pbl_vbase,
-					root_vpbl.leaf_vpbl[i].pbl_pbase);
-		}
-		kfree(root_vpbl.leaf_vpbl);
-		pci_free_consistent(nesdev->pcidev, 8192, root_vpbl.pbl_vbase,
-				root_vpbl.pbl_pbase);
-	}
-
+reg_phys_err:
+	/* single PBL case */
+	pci_free_consistent(nesdev->pcidev, 4096, vpbl.pbl_vbase, vpbl.pbl_pbase);
 	return ibmr;
 }
 
@@ -2266,17 +2120,13 @@ static struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
  */
 static struct ib_mr *nes_get_dma_mr(struct ib_pd *pd, int acc)
 {
-	struct ib_phys_buf bl;
 	u64 kva = 0;
 
 	nes_debug(NES_DBG_MR, "\n");
 
-	bl.size = (u64)0xffffffffffULL;
-	bl.addr = 0;
-	return nes_reg_phys_mr(pd, &bl, 1, acc, &kva);
+	return nes_reg_phys_mr(pd, 0, 0xffffffffffULL, acc, &kva);
 }
 
-
 /**
  * nes_reg_user_mr
  */
@@ -3888,12 +3738,10 @@ struct nes_ib_device *nes_init_ofa_device(struct net_device *netdev)
 	nesibdev->ibdev.destroy_cq = nes_destroy_cq;
 	nesibdev->ibdev.poll_cq = nes_poll_cq;
 	nesibdev->ibdev.get_dma_mr = nes_get_dma_mr;
-	nesibdev->ibdev.reg_phys_mr = nes_reg_phys_mr;
 	nesibdev->ibdev.reg_user_mr = nes_reg_user_mr;
 	nesibdev->ibdev.dereg_mr = nes_dereg_mr;
 	nesibdev->ibdev.alloc_mw = nes_alloc_mw;
 	nesibdev->ibdev.dealloc_mw = nes_dealloc_mw;
-	nesibdev->ibdev.bind_mw = nes_bind_mw;
 
 	nesibdev->ibdev.alloc_mr = nes_alloc_mr;
 	nesibdev->ibdev.map_mr_sg = nes_map_mr_sg;
diff --git a/drivers/infiniband/hw/nes/nes_verbs.h b/drivers/infiniband/hw/nes/nes_verbs.h
index a204b677af22..70290883d067 100644
--- a/drivers/infiniband/hw/nes/nes_verbs.h
+++ b/drivers/infiniband/hw/nes/nes_verbs.h
@@ -190,4 +190,8 @@ struct nes_qp {
 	u8                    pau_state;
 	__u64                 nesuqp_addr;
 };
+
+struct ib_mr *nes_reg_phys_mr(struct ib_pd *ib_pd,
+		u64 addr, u64 size, int acc, u64 *iova_start);
+
 #endif			/* NES_VERBS_H */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma.h b/drivers/infiniband/hw/ocrdma/ocrdma.h
index 040bb8b5cb15..12503f15fbd6 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma.h
@@ -323,9 +323,6 @@ struct ocrdma_cq {
 			 */
 	u32 max_hw_cqe;
 	bool phase_change;
-	bool deferred_arm, deferred_sol;
-	bool first_arm;
-
 	spinlock_t cq_lock ____cacheline_aligned; /* provide synchronization
 						   * to cq polling
 						   */
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
index 9820074be59d..3790771f2baa 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
@@ -152,9 +152,10 @@ struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
 	if ((pd->uctx) &&
 	    (!rdma_is_multicast_addr((struct in6_addr *)attr->grh.dgid.raw)) &&
 	    (!rdma_link_local_addr((struct in6_addr *)attr->grh.dgid.raw))) {
-		status = rdma_addr_find_dmac_by_grh(&sgid, &attr->grh.dgid,
-						    attr->dmac, &vlan_tag,
-						    sgid_attr.ndev->ifindex);
+		status = rdma_addr_find_l2_eth_by_grh(&sgid, &attr->grh.dgid,
+						      attr->dmac, &vlan_tag,
+						      &sgid_attr.ndev->ifindex,
+						      NULL);
 		if (status) {
 			pr_err("%s(): Failed to resolve dmac from gid." 
 				"status = %d\n", __func__, status);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
index 3afb40b85159..f38743018cb4 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_main.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -175,7 +175,6 @@ static int ocrdma_register_device(struct ocrdma_dev *dev)
 	dev->ibdev.req_notify_cq = ocrdma_arm_cq;
 
 	dev->ibdev.get_dma_mr = ocrdma_get_dma_mr;
-	dev->ibdev.reg_phys_mr = ocrdma_reg_kernel_mr;
 	dev->ibdev.dereg_mr = ocrdma_dereg_mr;
 	dev->ibdev.reg_user_mr = ocrdma_reg_user_mr;
 
@@ -229,6 +228,11 @@ static int ocrdma_alloc_resources(struct ocrdma_dev *dev)
 
 	ocrdma_alloc_pd_pool(dev);
 
+	if (!ocrdma_alloc_stats_resources(dev)) {
+		pr_err("%s: stats resource allocation failed\n", __func__);
+		goto alloc_err;
+	}
+
 	spin_lock_init(&dev->av_tbl.lock);
 	spin_lock_init(&dev->flush_q_lock);
 	return 0;
@@ -239,6 +243,7 @@ alloc_err:
 
 static void ocrdma_free_resources(struct ocrdma_dev *dev)
 {
+	ocrdma_release_stats_resources(dev);
 	kfree(dev->stag_arr);
 	kfree(dev->qp_tbl);
 	kfree(dev->cq_tbl);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_stats.c b/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
index 86c303a620c1..255f774080a4 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_stats.c
@@ -64,10 +64,11 @@ static int ocrdma_add_stat(char *start, char *pcur,
 	return cpy_len;
 }
 
-static bool ocrdma_alloc_stats_mem(struct ocrdma_dev *dev)
+bool ocrdma_alloc_stats_resources(struct ocrdma_dev *dev)
 {
 	struct stats_mem *mem = &dev->stats_mem;
 
+	mutex_init(&dev->stats_lock);
 	/* Alloc mbox command mem*/
 	mem->size = max_t(u32, sizeof(struct ocrdma_rdma_stats_req),
 			sizeof(struct ocrdma_rdma_stats_resp));
@@ -91,13 +92,14 @@ static bool ocrdma_alloc_stats_mem(struct ocrdma_dev *dev)
 	return true;
 }
 
-static void ocrdma_release_stats_mem(struct ocrdma_dev *dev)
+void ocrdma_release_stats_resources(struct ocrdma_dev *dev)
 {
 	struct stats_mem *mem = &dev->stats_mem;
 
 	if (mem->va)
 		dma_free_coherent(&dev->nic_info.pdev->dev, mem->size,
 				  mem->va, mem->pa);
+	mem->va = NULL;
 	kfree(mem->debugfs_mem);
 }
 
@@ -838,15 +840,9 @@ void ocrdma_add_port_stats(struct ocrdma_dev *dev)
 				&dev->reset_stats, &ocrdma_dbg_ops))
 		goto err;
 
-	/* Now create dma_mem for stats mbx command */
-	if (!ocrdma_alloc_stats_mem(dev))
-		goto err;
-
-	mutex_init(&dev->stats_lock);
 
 	return;
 err:
-	ocrdma_release_stats_mem(dev);
 	debugfs_remove_recursive(dev->dir);
 	dev->dir = NULL;
 }
@@ -855,9 +851,7 @@ void ocrdma_rem_port_stats(struct ocrdma_dev *dev)
 {
 	if (!dev->dir)
 		return;
-	debugfs_remove(dev->dir);
-	mutex_destroy(&dev->stats_lock);
-	ocrdma_release_stats_mem(dev);
+	debugfs_remove_recursive(dev->dir);
 }
 
 void ocrdma_init_debugfs(void)
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_stats.h b/drivers/infiniband/hw/ocrdma/ocrdma_stats.h
index c9e58d04c7b8..bba1fec4f11f 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_stats.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_stats.h
@@ -65,6 +65,8 @@ enum OCRDMA_STATS_TYPE {
 
 void ocrdma_rem_debugfs(void);
 void ocrdma_init_debugfs(void);
+bool ocrdma_alloc_stats_resources(struct ocrdma_dev *dev);
+void ocrdma_release_stats_resources(struct ocrdma_dev *dev);
 void ocrdma_rem_port_stats(struct ocrdma_dev *dev);
 void ocrdma_add_port_stats(struct ocrdma_dev *dev);
 int ocrdma_pma_counters(struct ocrdma_dev *dev,
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index 76e96f97b3f6..12420e4ecf3d 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -125,8 +125,8 @@ int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr,
 					IB_DEVICE_SYS_IMAGE_GUID |
 					IB_DEVICE_LOCAL_DMA_LKEY |
 					IB_DEVICE_MEM_MGT_EXTENSIONS;
-	attr->max_sge = min(dev->attr.max_send_sge, dev->attr.max_srq_sge);
-	attr->max_sge_rd = 0;
+	attr->max_sge = dev->attr.max_send_sge;
+	attr->max_sge_rd = attr->max_sge;
 	attr->max_cq = dev->attr.max_cq;
 	attr->max_cqe = dev->attr.max_cqe;
 	attr->max_mr = dev->attr.max_mr;
@@ -1094,7 +1094,6 @@ struct ib_cq *ocrdma_create_cq(struct ib_device *ibdev,
 	spin_lock_init(&cq->comp_handler_lock);
 	INIT_LIST_HEAD(&cq->sq_head);
 	INIT_LIST_HEAD(&cq->rq_head);
-	cq->first_arm = true;
 
 	if (ib_ctx) {
 		uctx = get_ocrdma_ucontext(ib_ctx);
@@ -2726,8 +2725,7 @@ static int ocrdma_update_ud_rcqe(struct ib_wc *ibwc, struct ocrdma_cqe *cqe)
 		OCRDMA_CQE_UD_STATUS_MASK) >> OCRDMA_CQE_UD_STATUS_SHIFT;
 	ibwc->src_qp = le32_to_cpu(cqe->flags_status_srcqpn) &
 						OCRDMA_CQE_SRCQP_MASK;
-	ibwc->pkey_index = le32_to_cpu(cqe->ud.rxlen_pkey) &
-						OCRDMA_CQE_PKEY_MASK;
+	ibwc->pkey_index = 0;
 	ibwc->wc_flags = IB_WC_GRH;
 	ibwc->byte_len = (le32_to_cpu(cqe->ud.rxlen_pkey) >>
 					OCRDMA_CQE_UD_XFER_LEN_SHIFT);
@@ -2911,12 +2909,9 @@ expand_cqe:
 	}
 stop_cqe:
 	cq->getp = cur_getp;
-	if (cq->deferred_arm || polled_hw_cqes) {
-		ocrdma_ring_cq_db(dev, cq->id, cq->deferred_arm,
-				  cq->deferred_sol, polled_hw_cqes);
-		cq->deferred_arm = false;
-		cq->deferred_sol = false;
-	}
+
+	if (polled_hw_cqes)
+		ocrdma_ring_cq_db(dev, cq->id, false, false, polled_hw_cqes);
 
 	return i;
 }
@@ -3000,13 +2995,7 @@ int ocrdma_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags cq_flags)
 	if (cq_flags & IB_CQ_SOLICITED)
 		sol_needed = true;
 
-	if (cq->first_arm) {
-		ocrdma_ring_cq_db(dev, cq_id, arm_needed, sol_needed, 0);
-		cq->first_arm = false;
-	}
-
-	cq->deferred_arm = true;
-	cq->deferred_sol = sol_needed;
+	ocrdma_ring_cq_db(dev, cq_id, arm_needed, sol_needed, 0);
 	spin_unlock_irqrestore(&cq->cq_lock, flags);
 
 	return 0;
@@ -3066,169 +3055,6 @@ pl_err:
 	return ERR_PTR(-ENOMEM);
 }
 
-#define MAX_KERNEL_PBE_SIZE 65536
-static inline int count_kernel_pbes(struct ib_phys_buf *buf_list,
-				    int buf_cnt, u32 *pbe_size)
-{
-	u64 total_size = 0;
-	u64 buf_size = 0;
-	int i;
-	*pbe_size = roundup(buf_list[0].size, PAGE_SIZE);
-	*pbe_size = roundup_pow_of_two(*pbe_size);
-
-	/* find the smallest PBE size that we can have */
-	for (i = 0; i < buf_cnt; i++) {
-		/* first addr may not be page aligned, so ignore checking */
-		if ((i != 0) && ((buf_list[i].addr & ~PAGE_MASK) ||
-				 (buf_list[i].size & ~PAGE_MASK))) {
-			return 0;
-		}
-
-		/* if configured PBE size is greater then the chosen one,
-		 * reduce the PBE size.
-		 */
-		buf_size = roundup(buf_list[i].size, PAGE_SIZE);
-		/* pbe_size has to be even multiple of 4K 1,2,4,8...*/
-		buf_size = roundup_pow_of_two(buf_size);
-		if (*pbe_size > buf_size)
-			*pbe_size = buf_size;
-
-		total_size += buf_size;
-	}
-	*pbe_size = *pbe_size > MAX_KERNEL_PBE_SIZE ?
-	    (MAX_KERNEL_PBE_SIZE) : (*pbe_size);
-
-	/* num_pbes = total_size / (*pbe_size);  this is implemented below. */
-
-	return total_size >> ilog2(*pbe_size);
-}
-
-static void build_kernel_pbes(struct ib_phys_buf *buf_list, int ib_buf_cnt,
-			      u32 pbe_size, struct ocrdma_pbl *pbl_tbl,
-			      struct ocrdma_hw_mr *hwmr)
-{
-	int i;
-	int idx;
-	int pbes_per_buf = 0;
-	u64 buf_addr = 0;
-	int num_pbes;
-	struct ocrdma_pbe *pbe;
-	int total_num_pbes = 0;
-
-	if (!hwmr->num_pbes)
-		return;
-
-	pbe = (struct ocrdma_pbe *)pbl_tbl->va;
-	num_pbes = 0;
-
-	/* go through the OS phy regions & fill hw pbe entries into pbls. */
-	for (i = 0; i < ib_buf_cnt; i++) {
-		buf_addr = buf_list[i].addr;
-		pbes_per_buf =
-		    roundup_pow_of_two(roundup(buf_list[i].size, PAGE_SIZE)) /
-		    pbe_size;
-		hwmr->len += buf_list[i].size;
-		/* number of pbes can be more for one OS buf, when
-		 * buffers are of different sizes.
-		 * split the ib_buf to one or more pbes.
-		 */
-		for (idx = 0; idx < pbes_per_buf; idx++) {
-			/* we program always page aligned addresses,
-			 * first unaligned address is taken care by fbo.
-			 */
-			if (i == 0) {
-				/* for non zero fbo, assign the
-				 * start of the page.
-				 */
-				pbe->pa_lo =
-				    cpu_to_le32((u32) (buf_addr & PAGE_MASK));
-				pbe->pa_hi =
-				    cpu_to_le32((u32) upper_32_bits(buf_addr));
-			} else {
-				pbe->pa_lo =
-				    cpu_to_le32((u32) (buf_addr & 0xffffffff));
-				pbe->pa_hi =
-				    cpu_to_le32((u32) upper_32_bits(buf_addr));
-			}
-			buf_addr += pbe_size;
-			num_pbes += 1;
-			total_num_pbes += 1;
-			pbe++;
-
-			if (total_num_pbes == hwmr->num_pbes)
-				goto mr_tbl_done;
-			/* if the pbl is full storing the pbes,
-			 * move to next pbl.
-			 */
-			if (num_pbes == (hwmr->pbl_size/sizeof(u64))) {
-				pbl_tbl++;
-				pbe = (struct ocrdma_pbe *)pbl_tbl->va;
-				num_pbes = 0;
-			}
-		}
-	}
-mr_tbl_done:
-	return;
-}
-
-struct ib_mr *ocrdma_reg_kernel_mr(struct ib_pd *ibpd,
-				   struct ib_phys_buf *buf_list,
-				   int buf_cnt, int acc, u64 *iova_start)
-{
-	int status = -ENOMEM;
-	struct ocrdma_mr *mr;
-	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
-	struct ocrdma_dev *dev = get_ocrdma_dev(ibpd->device);
-	u32 num_pbes;
-	u32 pbe_size = 0;
-
-	if ((acc & IB_ACCESS_REMOTE_WRITE) && !(acc & IB_ACCESS_LOCAL_WRITE))
-		return ERR_PTR(-EINVAL);
-
-	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
-	if (!mr)
-		return ERR_PTR(status);
-
-	num_pbes = count_kernel_pbes(buf_list, buf_cnt, &pbe_size);
-	if (num_pbes == 0) {
-		status = -EINVAL;
-		goto pbl_err;
-	}
-	status = ocrdma_get_pbl_info(dev, mr, num_pbes);
-	if (status)
-		goto pbl_err;
-
-	mr->hwmr.pbe_size = pbe_size;
-	mr->hwmr.fbo = *iova_start - (buf_list[0].addr & PAGE_MASK);
-	mr->hwmr.va = *iova_start;
-	mr->hwmr.local_rd = 1;
-	mr->hwmr.remote_wr = (acc & IB_ACCESS_REMOTE_WRITE) ? 1 : 0;
-	mr->hwmr.remote_rd = (acc & IB_ACCESS_REMOTE_READ) ? 1 : 0;
-	mr->hwmr.local_wr = (acc & IB_ACCESS_LOCAL_WRITE) ? 1 : 0;
-	mr->hwmr.remote_atomic = (acc & IB_ACCESS_REMOTE_ATOMIC) ? 1 : 0;
-	mr->hwmr.mw_bind = (acc & IB_ACCESS_MW_BIND) ? 1 : 0;
-
-	status = ocrdma_build_pbl_tbl(dev, &mr->hwmr);
-	if (status)
-		goto pbl_err;
-	build_kernel_pbes(buf_list, buf_cnt, pbe_size, mr->hwmr.pbl_table,
-			  &mr->hwmr);
-	status = ocrdma_reg_mr(dev, &mr->hwmr, pd->id, acc);
-	if (status)
-		goto mbx_err;
-
-	mr->ibmr.lkey = mr->hwmr.lkey;
-	if (mr->hwmr.remote_wr || mr->hwmr.remote_rd)
-		mr->ibmr.rkey = mr->hwmr.lkey;
-	return &mr->ibmr;
-
-mbx_err:
-	ocrdma_free_mr_pbl_tbl(dev, &mr->hwmr);
-pbl_err:
-	kfree(mr);
-	return ERR_PTR(status);
-}
-
 static int ocrdma_set_page(struct ib_mr *ibmr, u64 addr)
 {
 	struct ocrdma_mr *mr = get_ocrdma_mr(ibmr);
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
index a2f3b4dc20b0..8b517fd36779 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.h
@@ -117,9 +117,6 @@ int ocrdma_post_srq_recv(struct ib_srq *, struct ib_recv_wr *,
 
 int ocrdma_dereg_mr(struct ib_mr *);
 struct ib_mr *ocrdma_get_dma_mr(struct ib_pd *, int acc);
-struct ib_mr *ocrdma_reg_kernel_mr(struct ib_pd *,
-				   struct ib_phys_buf *buffer_list,
-				   int num_phys_buf, int acc, u64 *iova_start);
 struct ib_mr *ocrdma_reg_user_mr(struct ib_pd *, u64 start, u64 length,
 				 u64 virt, int acc, struct ib_udata *);
 struct ib_mr *ocrdma_alloc_mr(struct ib_pd *pd,
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index 13ef22bd9459..fcdf37913a26 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -89,14 +89,14 @@ static int create_file(const char *name, umode_t mode,
 {
 	int error;
 
-	mutex_lock(&d_inode(parent)->i_mutex);
+	inode_lock(d_inode(parent));
 	*dentry = lookup_one_len(name, parent, strlen(name));
 	if (!IS_ERR(*dentry))
 		error = qibfs_mknod(d_inode(parent), *dentry,
 				    mode, fops, data);
 	else
 		error = PTR_ERR(*dentry);
-	mutex_unlock(&d_inode(parent)->i_mutex);
+	inode_unlock(d_inode(parent));
 
 	return error;
 }
@@ -481,7 +481,7 @@ static int remove_device_files(struct super_block *sb,
 	int ret, i;
 
 	root = dget(sb->s_root);
-	mutex_lock(&d_inode(root)->i_mutex);
+	inode_lock(d_inode(root));
 	snprintf(unit, sizeof(unit), "%u", dd->unit);
 	dir = lookup_one_len(unit, root, strlen(unit));
 
@@ -491,7 +491,7 @@ static int remove_device_files(struct super_block *sb,
 		goto bail;
 	}
 
-	mutex_lock(&d_inode(dir)->i_mutex);
+	inode_lock(d_inode(dir));
 	remove_file(dir, "counters");
 	remove_file(dir, "counter_names");
 	remove_file(dir, "portcounter_names");
@@ -506,13 +506,13 @@ static int remove_device_files(struct super_block *sb,
 		}
 	}
 	remove_file(dir, "flash");
-	mutex_unlock(&d_inode(dir)->i_mutex);
+	inode_unlock(d_inode(dir));
 	ret = simple_rmdir(d_inode(root), dir);
 	d_delete(dir);
 	dput(dir);
 
 bail:
-	mutex_unlock(&d_inode(root)->i_mutex);
+	inode_unlock(d_inode(root));
 	dput(root);
 	return ret;
 }
diff --git a/drivers/infiniband/hw/qib/qib_mr.c b/drivers/infiniband/hw/qib/qib_mr.c
index 294f5c706be9..5f53304e8a9b 100644
--- a/drivers/infiniband/hw/qib/qib_mr.c
+++ b/drivers/infiniband/hw/qib/qib_mr.c
@@ -150,10 +150,7 @@ static struct qib_mr *alloc_mr(int count, struct ib_pd *pd)
 	rval = init_qib_mregion(&mr->mr, pd, count);
 	if (rval)
 		goto bail;
-	/*
-	 * ib_reg_phys_mr() will initialize mr->ibmr except for
-	 * lkey and rkey.
-	 */
+
 	rval = qib_alloc_lkey(&mr->mr, 0);
 	if (rval)
 		goto bail_mregion;
@@ -171,52 +168,6 @@ bail:
 }
 
 /**
- * qib_reg_phys_mr - register a physical memory region
- * @pd: protection domain for this memory region
- * @buffer_list: pointer to the list of physical buffers to register
- * @num_phys_buf: the number of physical buffers to register
- * @iova_start: the starting address passed over IB which maps to this MR
- *
- * Returns the memory region on success, otherwise returns an errno.
- */
-struct ib_mr *qib_reg_phys_mr(struct ib_pd *pd,
-			      struct ib_phys_buf *buffer_list,
-			      int num_phys_buf, int acc, u64 *iova_start)
-{
-	struct qib_mr *mr;
-	int n, m, i;
-	struct ib_mr *ret;
-
-	mr = alloc_mr(num_phys_buf, pd);
-	if (IS_ERR(mr)) {
-		ret = (struct ib_mr *)mr;
-		goto bail;
-	}
-
-	mr->mr.user_base = *iova_start;
-	mr->mr.iova = *iova_start;
-	mr->mr.access_flags = acc;
-
-	m = 0;
-	n = 0;
-	for (i = 0; i < num_phys_buf; i++) {
-		mr->mr.map[m]->segs[n].vaddr = (void *) buffer_list[i].addr;
-		mr->mr.map[m]->segs[n].length = buffer_list[i].size;
-		mr->mr.length += buffer_list[i].size;
-		n++;
-		if (n == QIB_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-
-	ret = &mr->ibmr;
-
-bail:
-	return ret;
-}
-
-/**
  * qib_reg_user_mr - register a userspace memory region
  * @pd: protection domain for this memory region
  * @start: starting userspace address
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index 40f85bb3e0d3..3eff35c2d453 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -100,9 +100,10 @@ static u32 credit_table[31] = {
 	32768                   /* 1E */
 };
 
-static void get_map_page(struct qib_qpn_table *qpt, struct qpn_map *map)
+static void get_map_page(struct qib_qpn_table *qpt, struct qpn_map *map,
+			 gfp_t gfp)
 {
-	unsigned long page = get_zeroed_page(GFP_KERNEL);
+	unsigned long page = get_zeroed_page(gfp);
 
 	/*
 	 * Free the page if someone raced with us installing it.
@@ -121,7 +122,7 @@ static void get_map_page(struct qib_qpn_table *qpt, struct qpn_map *map)
  * zero/one for QP type IB_QPT_SMI/IB_QPT_GSI.
  */
 static int alloc_qpn(struct qib_devdata *dd, struct qib_qpn_table *qpt,
-		     enum ib_qp_type type, u8 port)
+		     enum ib_qp_type type, u8 port, gfp_t gfp)
 {
 	u32 i, offset, max_scan, qpn;
 	struct qpn_map *map;
@@ -151,7 +152,7 @@ static int alloc_qpn(struct qib_devdata *dd, struct qib_qpn_table *qpt,
 	max_scan = qpt->nmaps - !offset;
 	for (i = 0;;) {
 		if (unlikely(!map->page)) {
-			get_map_page(qpt, map);
+			get_map_page(qpt, map, gfp);
 			if (unlikely(!map->page))
 				break;
 		}
@@ -983,13 +984,21 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 	size_t sz;
 	size_t sg_list_sz;
 	struct ib_qp *ret;
+	gfp_t gfp;
+
 
 	if (init_attr->cap.max_send_sge > ib_qib_max_sges ||
 	    init_attr->cap.max_send_wr > ib_qib_max_qp_wrs ||
-	    init_attr->create_flags) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
+	    init_attr->create_flags & ~(IB_QP_CREATE_USE_GFP_NOIO))
+		return ERR_PTR(-EINVAL);
+
+	/* GFP_NOIO is applicable in RC QPs only */
+	if (init_attr->create_flags & IB_QP_CREATE_USE_GFP_NOIO &&
+	    init_attr->qp_type != IB_QPT_RC)
+		return ERR_PTR(-EINVAL);
+
+	gfp = init_attr->create_flags & IB_QP_CREATE_USE_GFP_NOIO ?
+			GFP_NOIO : GFP_KERNEL;
 
 	/* Check receive queue parameters if no SRQ is specified. */
 	if (!init_attr->srq) {
@@ -1021,7 +1030,8 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 		sz = sizeof(struct qib_sge) *
 			init_attr->cap.max_send_sge +
 			sizeof(struct qib_swqe);
-		swq = vmalloc((init_attr->cap.max_send_wr + 1) * sz);
+		swq = __vmalloc((init_attr->cap.max_send_wr + 1) * sz,
+				gfp, PAGE_KERNEL);
 		if (swq == NULL) {
 			ret = ERR_PTR(-ENOMEM);
 			goto bail;
@@ -1037,13 +1047,13 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 		} else if (init_attr->cap.max_recv_sge > 1)
 			sg_list_sz = sizeof(*qp->r_sg_list) *
 				(init_attr->cap.max_recv_sge - 1);
-		qp = kzalloc(sz + sg_list_sz, GFP_KERNEL);
+		qp = kzalloc(sz + sg_list_sz, gfp);
 		if (!qp) {
 			ret = ERR_PTR(-ENOMEM);
 			goto bail_swq;
 		}
 		RCU_INIT_POINTER(qp->next, NULL);
-		qp->s_hdr = kzalloc(sizeof(*qp->s_hdr), GFP_KERNEL);
+		qp->s_hdr = kzalloc(sizeof(*qp->s_hdr), gfp);
 		if (!qp->s_hdr) {
 			ret = ERR_PTR(-ENOMEM);
 			goto bail_qp;
@@ -1058,8 +1068,16 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 			qp->r_rq.max_sge = init_attr->cap.max_recv_sge;
 			sz = (sizeof(struct ib_sge) * qp->r_rq.max_sge) +
 				sizeof(struct qib_rwqe);
-			qp->r_rq.wq = vmalloc_user(sizeof(struct qib_rwq) +
-						   qp->r_rq.size * sz);
+			if (gfp != GFP_NOIO)
+				qp->r_rq.wq = vmalloc_user(
+						sizeof(struct qib_rwq) +
+						qp->r_rq.size * sz);
+			else
+				qp->r_rq.wq = __vmalloc(
+						sizeof(struct qib_rwq) +
+						qp->r_rq.size * sz,
+						gfp, PAGE_KERNEL);
+
 			if (!qp->r_rq.wq) {
 				ret = ERR_PTR(-ENOMEM);
 				goto bail_qp;
@@ -1090,7 +1108,7 @@ struct ib_qp *qib_create_qp(struct ib_pd *ibpd,
 		dev = to_idev(ibpd->device);
 		dd = dd_from_dev(dev);
 		err = alloc_qpn(dd, &dev->qpn_table, init_attr->qp_type,
-				init_attr->port_num);
+				init_attr->port_num, gfp);
 		if (err < 0) {
 			ret = ERR_PTR(err);
 			vfree(qp->r_rq.wq);
diff --git a/drivers/infiniband/hw/qib/qib_verbs.c b/drivers/infiniband/hw/qib/qib_verbs.c
index de6cb6fcda8d..baf1e42b6896 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.c
+++ b/drivers/infiniband/hw/qib/qib_verbs.c
@@ -346,6 +346,7 @@ static int qib_post_one_send(struct qib_qp *qp, struct ib_send_wr *wr,
 	unsigned long flags;
 	struct qib_lkey_table *rkt;
 	struct qib_pd *pd;
+	int avoid_schedule = 0;
 
 	spin_lock_irqsave(&qp->s_lock, flags);
 
@@ -438,11 +439,15 @@ static int qib_post_one_send(struct qib_qp *qp, struct ib_send_wr *wr,
 	    qp->ibqp.qp_type == IB_QPT_RC) {
 		if (wqe->length > 0x80000000U)
 			goto bail_inval_free;
+		if (wqe->length <= qp->pmtu)
+			avoid_schedule = 1;
 	} else if (wqe->length > (dd_from_ibdev(qp->ibqp.device)->pport +
-				  qp->port_num - 1)->ibmtu)
+				  qp->port_num - 1)->ibmtu) {
 		goto bail_inval_free;
-	else
+	} else {
 		atomic_inc(&to_iah(ud_wr(wr)->ah)->refcount);
+		avoid_schedule = 1;
+	}
 	wqe->ssn = qp->s_ssn++;
 	qp->s_head = next;
 
@@ -458,7 +463,7 @@ bail_inval_free:
 bail_inval:
 	ret = -EINVAL;
 bail:
-	if (!ret && !wr->next &&
+	if (!ret && !wr->next && !avoid_schedule &&
 	 !qib_sdma_empty(
 	   dd_from_ibdev(qp->ibqp.device)->pport + qp->port_num - 1)) {
 		qib_schedule_send(qp);
@@ -2256,7 +2261,6 @@ int qib_register_ib_device(struct qib_devdata *dd)
 	ibdev->poll_cq = qib_poll_cq;
 	ibdev->req_notify_cq = qib_req_notify_cq;
 	ibdev->get_dma_mr = qib_get_dma_mr;
-	ibdev->reg_phys_mr = qib_reg_phys_mr;
 	ibdev->reg_user_mr = qib_reg_user_mr;
 	ibdev->dereg_mr = qib_dereg_mr;
 	ibdev->alloc_mr = qib_alloc_mr;
diff --git a/drivers/infiniband/hw/qib/qib_verbs.h b/drivers/infiniband/hw/qib/qib_verbs.h
index bc803f33d5f6..6c5e77753d85 100644
--- a/drivers/infiniband/hw/qib/qib_verbs.h
+++ b/drivers/infiniband/hw/qib/qib_verbs.h
@@ -1032,10 +1032,6 @@ int qib_resize_cq(struct ib_cq *ibcq, int cqe, struct ib_udata *udata);
 
 struct ib_mr *qib_get_dma_mr(struct ib_pd *pd, int acc);
 
-struct ib_mr *qib_reg_phys_mr(struct ib_pd *pd,
-			      struct ib_phys_buf *buffer_list,
-			      int num_phys_buf, int acc, u64 *iova_start);
-
 struct ib_mr *qib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 			      u64 virt_addr, int mr_access_flags,
 			      struct ib_udata *udata);
diff --git a/drivers/infiniband/hw/qib/qib_verbs_mcast.c b/drivers/infiniband/hw/qib/qib_verbs_mcast.c
index f8ea069a3eaf..b2fb5286dbd9 100644
--- a/drivers/infiniband/hw/qib/qib_verbs_mcast.c
+++ b/drivers/infiniband/hw/qib/qib_verbs_mcast.c
@@ -286,15 +286,13 @@ int qib_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 	struct qib_ibdev *dev = to_idev(ibqp->device);
 	struct qib_ibport *ibp = to_iport(ibqp->device, qp->port_num);
 	struct qib_mcast *mcast = NULL;
-	struct qib_mcast_qp *p, *tmp;
+	struct qib_mcast_qp *p, *tmp, *delp = NULL;
 	struct rb_node *n;
 	int last = 0;
 	int ret;
 
-	if (ibqp->qp_num <= 1 || qp->state == IB_QPS_RESET) {
-		ret = -EINVAL;
-		goto bail;
-	}
+	if (ibqp->qp_num <= 1 || qp->state == IB_QPS_RESET)
+		return -EINVAL;
 
 	spin_lock_irq(&ibp->lock);
 
@@ -303,8 +301,7 @@ int qib_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 	while (1) {
 		if (n == NULL) {
 			spin_unlock_irq(&ibp->lock);
-			ret = -EINVAL;
-			goto bail;
+			return -EINVAL;
 		}
 
 		mcast = rb_entry(n, struct qib_mcast, rb_node);
@@ -328,6 +325,7 @@ int qib_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 		 */
 		list_del_rcu(&p->list);
 		mcast->n_attached--;
+		delp = p;
 
 		/* If this was the last attached QP, remove the GID too. */
 		if (list_empty(&mcast->qp_list)) {
@@ -338,15 +336,16 @@ int qib_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 	}
 
 	spin_unlock_irq(&ibp->lock);
+	/* QP not attached */
+	if (!delp)
+		return -EINVAL;
+	/*
+	 * Wait for any list walkers to finish before freeing the
+	 * list element.
+	 */
+	wait_event(mcast->wait, atomic_read(&mcast->refcount) <= 1);
+	qib_mcast_qp_free(delp);
 
-	if (p) {
-		/*
-		 * Wait for any list walkers to finish before freeing the
-		 * list element.
-		 */
-		wait_event(mcast->wait, atomic_read(&mcast->refcount) <= 1);
-		qib_mcast_qp_free(p);
-	}
 	if (last) {
 		atomic_dec(&mcast->refcount);
 		wait_event(mcast->wait, !atomic_read(&mcast->refcount));
@@ -355,11 +354,7 @@ int qib_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 		dev->n_mcast_grps_allocated--;
 		spin_unlock_irq(&dev->n_mcast_grps_lock);
 	}
-
-	ret = 0;
-
-bail:
-	return ret;
+	return 0;
 }
 
 int qib_mcast_tree_empty(struct qib_ibport *ibp)
diff --git a/drivers/infiniband/hw/usnic/usnic_debugfs.c b/drivers/infiniband/hw/usnic/usnic_debugfs.c
index 5e55b8bc6fe4..92dc66cc2d50 100644
--- a/drivers/infiniband/hw/usnic/usnic_debugfs.c
+++ b/drivers/infiniband/hw/usnic/usnic_debugfs.c
@@ -157,8 +157,9 @@ void usnic_debugfs_flow_add(struct usnic_ib_qp_grp_flow *qp_flow)
 							qp_flow,
 							&flowinfo_ops);
 	if (IS_ERR_OR_NULL(qp_flow->dbgfs_dentry)) {
-		usnic_err("Failed to create dbg fs entry for flow %u\n",
-				qp_flow->flow->flow_id);
+		usnic_err("Failed to create dbg fs entry for flow %u with error %ld\n",
+				qp_flow->flow->flow_id,
+				PTR_ERR(qp_flow->dbgfs_dentry));
 	}
 }
 
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c b/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
index fcea3a24d3eb..5f44b66ccb86 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
@@ -521,7 +521,7 @@ int usnic_ib_qp_grp_modify(struct usnic_ib_qp_grp *qp_grp,
 
 	if (!status) {
 		qp_grp->state = new_state;
-		usnic_info("Transistioned %u from %s to %s",
+		usnic_info("Transitioned %u from %s to %s",
 		qp_grp->grp_id,
 		usnic_ib_qp_grp_state_to_string(old_state),
 		usnic_ib_qp_grp_state_to_string(new_state));
@@ -575,7 +575,7 @@ alloc_res_chunk_list(struct usnic_vnic *vnic,
 	return res_chunk_list;
 
 out_free_res:
-	for (i--; i > 0; i--)
+	for (i--; i >= 0; i--)
 		usnic_vnic_put_resources(res_chunk_list[i]);
 	kfree(res_chunk_list);
 	return ERR_PTR(err);
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
index f8e3211689a3..6cdb4d23f78f 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_verbs.c
@@ -51,7 +51,7 @@
 
 static void usnic_ib_fw_string_to_u64(char *fw_ver_str, u64 *fw_ver)
 {
-	*fw_ver = (u64) *fw_ver_str;
+	*fw_ver = *((u64 *)fw_ver_str);
 }
 
 static int usnic_ib_fill_create_qp_resp(struct usnic_ib_qp_grp *qp_grp,
@@ -571,20 +571,20 @@ int usnic_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 
 	qp_grp = to_uqp_grp(ibqp);
 
-	/* TODO: Future Support All States */
 	mutex_lock(&qp_grp->vf->pf->usdev_lock);
-	if ((attr_mask & IB_QP_STATE) && attr->qp_state == IB_QPS_INIT) {
-		status = usnic_ib_qp_grp_modify(qp_grp, IB_QPS_INIT, NULL);
-	} else if ((attr_mask & IB_QP_STATE) && attr->qp_state == IB_QPS_RTR) {
-		status = usnic_ib_qp_grp_modify(qp_grp, IB_QPS_RTR, NULL);
-	} else if ((attr_mask & IB_QP_STATE) && attr->qp_state == IB_QPS_RTS) {
-		status = usnic_ib_qp_grp_modify(qp_grp, IB_QPS_RTS, NULL);
+	if ((attr_mask & IB_QP_PORT) && attr->port_num != 1) {
+		/* usnic devices only have one port */
+		status = -EINVAL;
+		goto out_unlock;
+	}
+	if (attr_mask & IB_QP_STATE) {
+		status = usnic_ib_qp_grp_modify(qp_grp, attr->qp_state, NULL);
 	} else {
-		usnic_err("Unexpected combination mask: %u state: %u\n",
-				attr_mask & IB_QP_STATE, attr->qp_state);
+		usnic_err("Unhandled request, attr_mask=0x%x\n", attr_mask);
 		status = -EINVAL;
 	}
 
+out_unlock:
 	mutex_unlock(&qp_grp->vf->pf->usdev_lock);
 	return status;
 }
@@ -625,8 +625,8 @@ struct ib_mr *usnic_ib_reg_mr(struct ib_pd *pd, u64 start, u64 length,
 			virt_addr, length);
 
 	mr = kzalloc(sizeof(*mr), GFP_KERNEL);
-	if (IS_ERR_OR_NULL(mr))
-		return ERR_PTR(mr ? PTR_ERR(mr) : -ENOMEM);
+	if (!mr)
+		return ERR_PTR(-ENOMEM);
 
 	mr->umem = usnic_uiom_reg_get(to_upd(pd)->umem_pd, start, length,
 					access_flags, 0);
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_verbs.h b/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
index 414eaa566bd9..0d9d2e6a14d5 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
+++ b/drivers/infiniband/hw/usnic/usnic_ib_verbs.h
@@ -43,8 +43,6 @@ int usnic_ib_query_device(struct ib_device *ibdev,
 			  struct ib_udata *uhw);
 int usnic_ib_query_port(struct ib_device *ibdev, u8 port,
 				struct ib_port_attr *props);
-enum rdma_protocol_type
-usnic_ib_query_protocol(struct ib_device *device, u8 port_num);
 int usnic_ib_query_qp(struct ib_qp *qp, struct ib_qp_attr *qp_attr,
 				int qp_attr_mask,
 				struct ib_qp_init_attr *qp_init_attr);
diff --git a/drivers/infiniband/hw/usnic/usnic_vnic.c b/drivers/infiniband/hw/usnic/usnic_vnic.c
index 66de93fb8ea9..887510718690 100644
--- a/drivers/infiniband/hw/usnic/usnic_vnic.c
+++ b/drivers/infiniband/hw/usnic/usnic_vnic.c
@@ -237,7 +237,7 @@ usnic_vnic_get_resources(struct usnic_vnic *vnic, enum usnic_vnic_res_type type,
 	struct usnic_vnic_res *res;
 	int i;
 
-	if (usnic_vnic_res_free_cnt(vnic, type) < cnt || cnt < 1 || !owner)
+	if (usnic_vnic_res_free_cnt(vnic, type) < cnt || cnt < 0 || !owner)
 		return ERR_PTR(-EINVAL);
 
 	ret = kzalloc(sizeof(*ret), GFP_ATOMIC);
@@ -247,26 +247,28 @@ usnic_vnic_get_resources(struct usnic_vnic *vnic, enum usnic_vnic_res_type type,
 		return ERR_PTR(-ENOMEM);
 	}
 
-	ret->res = kzalloc(sizeof(*(ret->res))*cnt, GFP_ATOMIC);
-	if (!ret->res) {
-		usnic_err("Failed to allocate resources for %s. Out of memory\n",
-				usnic_vnic_pci_name(vnic));
-		kfree(ret);
-		return ERR_PTR(-ENOMEM);
-	}
+	if (cnt > 0) {
+		ret->res = kcalloc(cnt, sizeof(*(ret->res)), GFP_ATOMIC);
+		if (!ret->res) {
+			usnic_err("Failed to allocate resources for %s. Out of memory\n",
+					usnic_vnic_pci_name(vnic));
+			kfree(ret);
+			return ERR_PTR(-ENOMEM);
+		}
 
-	spin_lock(&vnic->res_lock);
-	src = &vnic->chunks[type];
-	for (i = 0; i < src->cnt && ret->cnt < cnt; i++) {
-		res = src->res[i];
-		if (!res->owner) {
-			src->free_cnt--;
-			res->owner = owner;
-			ret->res[ret->cnt++] = res;
+		spin_lock(&vnic->res_lock);
+		src = &vnic->chunks[type];
+		for (i = 0; i < src->cnt && ret->cnt < cnt; i++) {
+			res = src->res[i];
+			if (!res->owner) {
+				src->free_cnt--;
+				res->owner = owner;
+				ret->res[ret->cnt++] = res;
+			}
 		}
-	}
 
-	spin_unlock(&vnic->res_lock);
+		spin_unlock(&vnic->res_lock);
+	}
 	ret->type = type;
 	ret->vnic = vnic;
 	WARN_ON(ret->cnt != cnt);
@@ -281,14 +283,16 @@ void usnic_vnic_put_resources(struct usnic_vnic_res_chunk *chunk)
 	int i;
 	struct usnic_vnic *vnic = chunk->vnic;
 
-	spin_lock(&vnic->res_lock);
-	while ((i = --chunk->cnt) >= 0) {
-		res = chunk->res[i];
-		chunk->res[i] = NULL;
-		res->owner = NULL;
-		vnic->chunks[res->type].free_cnt++;
+	if (chunk->cnt > 0) {
+		spin_lock(&vnic->res_lock);
+		while ((i = --chunk->cnt) >= 0) {
+			res = chunk->res[i];
+			chunk->res[i] = NULL;
+			res->owner = NULL;
+			vnic->chunks[res->type].free_cnt++;
+		}
+		spin_unlock(&vnic->res_lock);
 	}
-	spin_unlock(&vnic->res_lock);
 
 	kfree(chunk->res);
 	kfree(chunk);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index 3ede10309754..a6f3eab0f350 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -495,7 +495,6 @@ void ipoib_dev_cleanup(struct net_device *dev);
 void ipoib_mcast_join_task(struct work_struct *work);
 void ipoib_mcast_carrier_on_task(struct work_struct *work);
 void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb);
-void ipoib_mcast_free(struct ipoib_mcast *mc);
 
 void ipoib_mcast_restart_task(struct work_struct *work);
 int ipoib_mcast_start_thread(struct net_device *dev);
@@ -549,8 +548,9 @@ void ipoib_path_iter_read(struct ipoib_path_iter *iter,
 
 int ipoib_mcast_attach(struct net_device *dev, u16 mlid,
 		       union ib_gid *mgid, int set_qkey);
-int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast);
-struct ipoib_mcast *__ipoib_mcast_find(struct net_device *dev, void *mgid);
+void ipoib_mcast_remove_list(struct list_head *remove_list);
+void ipoib_check_and_add_mcast_sendonly(struct ipoib_dev_priv *priv, u8 *mgid,
+				struct list_head *remove_list);
 
 int ipoib_init_qp(struct net_device *dev);
 int ipoib_transport_dev_init(struct net_device *dev, struct ib_device *ca);
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_cm.c b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
index 3ae9726efb98..917e46ea3bf6 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_cm.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_cm.c
@@ -70,7 +70,6 @@ static struct ib_qp_attr ipoib_cm_err_attr = {
 #define IPOIB_CM_RX_DRAIN_WRID 0xffffffff
 
 static struct ib_send_wr ipoib_cm_rx_drain_wr = {
-	.wr_id = IPOIB_CM_RX_DRAIN_WRID,
 	.opcode = IB_WR_SEND,
 };
 
@@ -223,6 +222,7 @@ static void ipoib_cm_start_rx_drain(struct ipoib_dev_priv *priv)
 	 * error" WC will be immediately generated for each WR we post.
 	 */
 	p = list_entry(priv->cm.rx_flush_list.next, typeof(*p), list);
+	ipoib_cm_rx_drain_wr.wr_id = IPOIB_CM_RX_DRAIN_WRID;
 	if (ib_post_send(p->qp, &ipoib_cm_rx_drain_wr, &bad_wr))
 		ipoib_warn(priv, "failed to post drain wr\n");
 
@@ -1522,8 +1522,7 @@ static void ipoib_cm_create_srq(struct net_device *dev, int max_sge)
 int ipoib_cm_dev_init(struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
-	int i, ret;
-	struct ib_device_attr attr;
+	int max_srq_sge, i;
 
 	INIT_LIST_HEAD(&priv->cm.passive_ids);
 	INIT_LIST_HEAD(&priv->cm.reap_list);
@@ -1540,19 +1539,13 @@ int ipoib_cm_dev_init(struct net_device *dev)
 
 	skb_queue_head_init(&priv->cm.skb_queue);
 
-	ret = ib_query_device(priv->ca, &attr);
-	if (ret) {
-		printk(KERN_WARNING "ib_query_device() failed with %d\n", ret);
-		return ret;
-	}
-
-	ipoib_dbg(priv, "max_srq_sge=%d\n", attr.max_srq_sge);
+	ipoib_dbg(priv, "max_srq_sge=%d\n", priv->ca->attrs.max_srq_sge);
 
-	attr.max_srq_sge = min_t(int, IPOIB_CM_RX_SG, attr.max_srq_sge);
-	ipoib_cm_create_srq(dev, attr.max_srq_sge);
+	max_srq_sge = min_t(int, IPOIB_CM_RX_SG, priv->ca->attrs.max_srq_sge);
+	ipoib_cm_create_srq(dev, max_srq_sge);
 	if (ipoib_cm_has_srq(dev)) {
-		priv->cm.max_cm_mtu = attr.max_srq_sge * PAGE_SIZE - 0x10;
-		priv->cm.num_frags  = attr.max_srq_sge;
+		priv->cm.max_cm_mtu = max_srq_sge * PAGE_SIZE - 0x10;
+		priv->cm.num_frags  = max_srq_sge;
 		ipoib_dbg(priv, "max_cm_mtu = 0x%x, num_frags=%d\n",
 			  priv->cm.max_cm_mtu, priv->cm.num_frags);
 	} else {
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c b/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
index 078cadd6c797..a53fa5fc0dec 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ethtool.c
@@ -40,15 +40,11 @@ static void ipoib_get_drvinfo(struct net_device *netdev,
 			      struct ethtool_drvinfo *drvinfo)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(netdev);
-	struct ib_device_attr *attr;
-
-	attr = kmalloc(sizeof(*attr), GFP_KERNEL);
-	if (attr && !ib_query_device(priv->ca, attr))
-		snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
-			 "%d.%d.%d", (int)(attr->fw_ver >> 32),
-			 (int)(attr->fw_ver >> 16) & 0xffff,
-			 (int)attr->fw_ver & 0xffff);
-	kfree(attr);
+
+	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+		 "%d.%d.%d", (int)(priv->ca->attrs.fw_ver >> 32),
+		 (int)(priv->ca->attrs.fw_ver >> 16) & 0xffff,
+		 (int)priv->ca->attrs.fw_ver & 0xffff);
 
 	strlcpy(drvinfo->bus_info, dev_name(priv->ca->dma_device),
 		sizeof(drvinfo->bus_info));
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_ib.c b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
index 5ea0c14070d1..fa9c42ff1fb0 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_ib.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_ib.c
@@ -245,8 +245,6 @@ static void ipoib_ib_handle_rx_wc(struct net_device *dev, struct ib_wc *wc)
 	skb_reset_mac_header(skb);
 	skb_pull(skb, IPOIB_ENCAP_LEN);
 
-	skb->truesize = SKB_TRUESIZE(skb->len);
-
 	++dev->stats.rx_packets;
 	dev->stats.rx_bytes += skb->len;
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 7d3281866ffc..25509bbd4a05 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -1150,8 +1150,6 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 	unsigned long flags;
 	int i;
 	LIST_HEAD(remove_list);
-	struct ipoib_mcast *mcast, *tmcast;
-	struct net_device *dev = priv->dev;
 
 	if (test_bit(IPOIB_STOP_NEIGH_GC, &priv->flags))
 		return;
@@ -1179,18 +1177,8 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 							  lockdep_is_held(&priv->lock))) != NULL) {
 			/* was the neigh idle for two GC periods */
 			if (time_after(neigh_obsolete, neigh->alive)) {
-				u8 *mgid = neigh->daddr + 4;
 
-				/* Is this multicast ? */
-				if (*mgid == 0xff) {
-					mcast = __ipoib_mcast_find(dev, mgid);
-
-					if (mcast && test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags)) {
-						list_del(&mcast->list);
-						rb_erase(&mcast->rb_node, &priv->multicast_tree);
-						list_add_tail(&mcast->list, &remove_list);
-					}
-				}
+				ipoib_check_and_add_mcast_sendonly(priv, neigh->daddr + 4, &remove_list);
 
 				rcu_assign_pointer(*np,
 						   rcu_dereference_protected(neigh->hnext,
@@ -1207,10 +1195,7 @@ static void __ipoib_reap_neigh(struct ipoib_dev_priv *priv)
 
 out_unlock:
 	spin_unlock_irqrestore(&priv->lock, flags);
-	list_for_each_entry_safe(mcast, tmcast, &remove_list, list) {
-		ipoib_mcast_leave(dev, mcast);
-		ipoib_mcast_free(mcast);
-	}
+	ipoib_mcast_remove_list(&remove_list);
 }
 
 static void ipoib_reap_neigh(struct work_struct *work)
@@ -1777,26 +1762,7 @@ int ipoib_add_pkey_attr(struct net_device *dev)
 
 int ipoib_set_dev_features(struct ipoib_dev_priv *priv, struct ib_device *hca)
 {
-	struct ib_device_attr *device_attr;
-	int result = -ENOMEM;
-
-	device_attr = kmalloc(sizeof *device_attr, GFP_KERNEL);
-	if (!device_attr) {
-		printk(KERN_WARNING "%s: allocation of %zu bytes failed\n",
-		       hca->name, sizeof *device_attr);
-		return result;
-	}
-
-	result = ib_query_device(hca, device_attr);
-	if (result) {
-		printk(KERN_WARNING "%s: ib_query_device failed (ret = %d)\n",
-		       hca->name, result);
-		kfree(device_attr);
-		return result;
-	}
-	priv->hca_caps = device_attr->device_cap_flags;
-
-	kfree(device_attr);
+	priv->hca_caps = hca->attrs.device_cap_flags;
 
 	if (priv->hca_caps & IB_DEVICE_UD_IP_CSUM) {
 		priv->dev->hw_features = NETIF_F_SG |
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index f357ca67a41c..25889311b1e9 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -106,7 +106,7 @@ static void __ipoib_mcast_schedule_join_thread(struct ipoib_dev_priv *priv,
 		queue_delayed_work(priv->wq, &priv->mcast_task, 0);
 }
 
-void ipoib_mcast_free(struct ipoib_mcast *mcast)
+static void ipoib_mcast_free(struct ipoib_mcast *mcast)
 {
 	struct net_device *dev = mcast->dev;
 	int tx_dropped = 0;
@@ -153,7 +153,7 @@ static struct ipoib_mcast *ipoib_mcast_alloc(struct net_device *dev,
 	return mcast;
 }
 
-struct ipoib_mcast *__ipoib_mcast_find(struct net_device *dev, void *mgid)
+static struct ipoib_mcast *__ipoib_mcast_find(struct net_device *dev, void *mgid)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct rb_node *n = priv->multicast_tree.rb_node;
@@ -456,7 +456,10 @@ out_locked:
 	return status;
 }
 
-static void ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast)
+/*
+ * Caller must hold 'priv->lock'
+ */
+static int ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ib_sa_multicast *multicast;
@@ -466,6 +469,10 @@ static void ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast)
 	ib_sa_comp_mask comp_mask;
 	int ret = 0;
 
+	if (!priv->broadcast ||
+	    !test_bit(IPOIB_FLAG_OPER_UP, &priv->flags))
+		return -EINVAL;
+
 	ipoib_dbg_mcast(priv, "joining MGID %pI6\n", mcast->mcmember.mgid.raw);
 
 	rec.mgid     = mcast->mcmember.mgid;
@@ -525,20 +532,23 @@ static void ipoib_mcast_join(struct net_device *dev, struct ipoib_mcast *mcast)
 			rec.join_state = 4;
 #endif
 	}
+	spin_unlock_irq(&priv->lock);
 
 	multicast = ib_sa_join_multicast(&ipoib_sa_client, priv->ca, priv->port,
 					 &rec, comp_mask, GFP_KERNEL,
 					 ipoib_mcast_join_complete, mcast);
+	spin_lock_irq(&priv->lock);
 	if (IS_ERR(multicast)) {
 		ret = PTR_ERR(multicast);
 		ipoib_warn(priv, "ib_sa_join_multicast failed, status %d\n", ret);
-		spin_lock_irq(&priv->lock);
 		/* Requeue this join task with a backoff delay */
 		__ipoib_mcast_schedule_join_thread(priv, mcast, 1);
 		clear_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
 		spin_unlock_irq(&priv->lock);
 		complete(&mcast->done);
+		spin_lock_irq(&priv->lock);
 	}
+	return 0;
 }
 
 void ipoib_mcast_join_task(struct work_struct *work)
@@ -620,9 +630,10 @@ void ipoib_mcast_join_task(struct work_struct *work)
 				/* Found the next unjoined group */
 				init_completion(&mcast->done);
 				set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
-				spin_unlock_irq(&priv->lock);
-				ipoib_mcast_join(dev, mcast);
-				spin_lock_irq(&priv->lock);
+				if (ipoib_mcast_join(dev, mcast)) {
+					spin_unlock_irq(&priv->lock);
+					return;
+				}
 			} else if (!delay_until ||
 				 time_before(mcast->delay_until, delay_until))
 				delay_until = mcast->delay_until;
@@ -641,10 +652,9 @@ out:
 	if (mcast) {
 		init_completion(&mcast->done);
 		set_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags);
+		ipoib_mcast_join(dev, mcast);
 	}
 	spin_unlock_irq(&priv->lock);
-	if (mcast)
-		ipoib_mcast_join(dev, mcast);
 }
 
 int ipoib_mcast_start_thread(struct net_device *dev)
@@ -677,7 +687,7 @@ int ipoib_mcast_stop_thread(struct net_device *dev)
 	return 0;
 }
 
-int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast)
+static int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	int ret = 0;
@@ -704,6 +714,35 @@ int ipoib_mcast_leave(struct net_device *dev, struct ipoib_mcast *mcast)
 	return 0;
 }
 
+/*
+ * Check if the multicast group is sendonly. If so remove it from the maps
+ * and add to the remove list
+ */
+void ipoib_check_and_add_mcast_sendonly(struct ipoib_dev_priv *priv, u8 *mgid,
+				struct list_head *remove_list)
+{
+	/* Is this multicast ? */
+	if (*mgid == 0xff) {
+		struct ipoib_mcast *mcast = __ipoib_mcast_find(priv->dev, mgid);
+
+		if (mcast && test_bit(IPOIB_MCAST_FLAG_SENDONLY, &mcast->flags)) {
+			list_del(&mcast->list);
+			rb_erase(&mcast->rb_node, &priv->multicast_tree);
+			list_add_tail(&mcast->list, remove_list);
+		}
+	}
+}
+
+void ipoib_mcast_remove_list(struct list_head *remove_list)
+{
+	struct ipoib_mcast *mcast, *tmcast;
+
+	list_for_each_entry_safe(mcast, tmcast, remove_list, list) {
+		ipoib_mcast_leave(mcast->dev, mcast);
+		ipoib_mcast_free(mcast);
+	}
+}
+
 void ipoib_mcast_send(struct net_device *dev, u8 *daddr, struct sk_buff *skb)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
@@ -810,10 +849,7 @@ void ipoib_mcast_dev_flush(struct net_device *dev)
 		if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
 			wait_for_completion(&mcast->done);
 
-	list_for_each_entry_safe(mcast, tmcast, &remove_list, list) {
-		ipoib_mcast_leave(dev, mcast);
-		ipoib_mcast_free(mcast);
-	}
+	ipoib_mcast_remove_list(&remove_list);
 }
 
 static int ipoib_mcast_addr_is_valid(const u8 *addr, const u8 *broadcast)
@@ -939,10 +975,7 @@ void ipoib_mcast_restart_task(struct work_struct *work)
 		if (test_bit(IPOIB_MCAST_FLAG_BUSY, &mcast->flags))
 			wait_for_completion(&mcast->done);
 
-	list_for_each_entry_safe(mcast, tmcast, &remove_list, list) {
-		ipoib_mcast_leave(mcast->dev, mcast);
-		ipoib_mcast_free(mcast);
-	}
+	ipoib_mcast_remove_list(&remove_list);
 
 	/*
 	 * Double check that we are still up
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 9080161e01af..c827c93f46c5 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -644,7 +644,7 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 
 		ib_conn = &iser_conn->ib_conn;
 		if (ib_conn->pi_support) {
-			u32 sig_caps = ib_conn->device->dev_attr.sig_prot_cap;
+			u32 sig_caps = ib_conn->device->ib_device->attrs.sig_prot_cap;
 
 			scsi_host_set_prot(shost, iser_dif_prot_caps(sig_caps));
 			scsi_host_set_guard(shost, SHOST_DIX_GUARD_IP |
@@ -656,7 +656,7 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 		 * max fastreg page list length.
 		 */
 		shost->sg_tablesize = min_t(unsigned short, shost->sg_tablesize,
-			ib_conn->device->dev_attr.max_fast_reg_page_list_len);
+			ib_conn->device->ib_device->attrs.max_fast_reg_page_list_len);
 		shost->max_sectors = min_t(unsigned int,
 			1024, (shost->sg_tablesize * PAGE_SIZE) >> 9);
 
@@ -1059,7 +1059,8 @@ static int __init iser_init(void)
 	release_wq = alloc_workqueue("release workqueue", 0, 0);
 	if (!release_wq) {
 		iser_err("failed to allocate release workqueue\n");
-		return -ENOMEM;
+		err = -ENOMEM;
+		goto err_alloc_wq;
 	}
 
 	iscsi_iser_scsi_transport = iscsi_register_transport(
@@ -1067,12 +1068,14 @@ static int __init iser_init(void)
 	if (!iscsi_iser_scsi_transport) {
 		iser_err("iscsi_register_transport failed\n");
 		err = -EINVAL;
-		goto register_transport_failure;
+		goto err_reg;
 	}
 
 	return 0;
 
-register_transport_failure:
+err_reg:
+	destroy_workqueue(release_wq);
+err_alloc_wq:
 	kmem_cache_destroy(ig.desc_cache);
 
 	return err;
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index 8a5998e6a407..95f0a64e076b 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -48,6 +48,7 @@
 #include <scsi/scsi_transport_iscsi.h>
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
+#include <scsi/iser.h>
 
 #include <linux/interrupt.h>
 #include <linux/wait.h>
@@ -151,46 +152,10 @@
 					 - ISER_MAX_RX_MISC_PDUS) /	\
 					 (1 + ISER_INFLIGHT_DATAOUTS))
 
-#define ISER_WC_BATCH_COUNT   16
 #define ISER_SIGNAL_CMD_COUNT 32
 
-#define ISER_VER			0x10
-#define ISER_WSV			0x08
-#define ISER_RSV			0x04
-
-#define ISER_FASTREG_LI_WRID		0xffffffffffffffffULL
-#define ISER_BEACON_WRID		0xfffffffffffffffeULL
-
-/**
- * struct iser_hdr - iSER header
- *
- * @flags:        flags support (zbva, remote_inv)
- * @rsvd:         reserved
- * @write_stag:   write rkey
- * @write_va:     write virtual address
- * @reaf_stag:    read rkey
- * @read_va:      read virtual address
- */
-struct iser_hdr {
-	u8      flags;
-	u8      rsvd[3];
-	__be32  write_stag;
-	__be64  write_va;
-	__be32  read_stag;
-	__be64  read_va;
-} __attribute__((packed));
-
-
-#define ISER_ZBVA_NOT_SUPPORTED		0x80
-#define ISER_SEND_W_INV_NOT_SUPPORTED	0x40
-
-struct iser_cm_hdr {
-	u8      flags;
-	u8      rsvd[3];
-} __packed;
-
 /* Constant PDU lengths calculations */
-#define ISER_HEADERS_LEN  (sizeof(struct iser_hdr) + sizeof(struct iscsi_hdr))
+#define ISER_HEADERS_LEN	(sizeof(struct iser_ctrl) + sizeof(struct iscsi_hdr))
 
 #define ISER_RECV_DATA_SEG_LEN	128
 #define ISER_RX_PAYLOAD_SIZE	(ISER_HEADERS_LEN + ISER_RECV_DATA_SEG_LEN)
@@ -269,7 +234,7 @@ enum iser_desc_type {
 #define ISER_MAX_WRS 7
 
 /**
- * struct iser_tx_desc - iSER TX descriptor (for send wr_id)
+ * struct iser_tx_desc - iSER TX descriptor
  *
  * @iser_header:   iser header
  * @iscsi_header:  iscsi header
@@ -287,12 +252,13 @@ enum iser_desc_type {
  * @sig_attrs:     Signature attributes
  */
 struct iser_tx_desc {
-	struct iser_hdr              iser_header;
+	struct iser_ctrl             iser_header;
 	struct iscsi_hdr             iscsi_header;
 	enum   iser_desc_type        type;
 	u64		             dma_addr;
 	struct ib_sge		     tx_sg[2];
 	int                          num_sge;
+	struct ib_cqe		     cqe;
 	bool			     mapped;
 	u8                           wr_idx;
 	union iser_wr {
@@ -306,9 +272,10 @@ struct iser_tx_desc {
 };
 
 #define ISER_RX_PAD_SIZE	(256 - (ISER_RX_PAYLOAD_SIZE + \
-					sizeof(u64) + sizeof(struct ib_sge)))
+				 sizeof(u64) + sizeof(struct ib_sge) + \
+				 sizeof(struct ib_cqe)))
 /**
- * struct iser_rx_desc - iSER RX descriptor (for recv wr_id)
+ * struct iser_rx_desc - iSER RX descriptor
  *
  * @iser_header:   iser header
  * @iscsi_header:  iscsi header
@@ -318,12 +285,32 @@ struct iser_tx_desc {
  * @pad:           for sense data TODO: Modify to maximum sense length supported
  */
 struct iser_rx_desc {
-	struct iser_hdr              iser_header;
+	struct iser_ctrl             iser_header;
 	struct iscsi_hdr             iscsi_header;
 	char		             data[ISER_RECV_DATA_SEG_LEN];
 	u64		             dma_addr;
 	struct ib_sge		     rx_sg;
+	struct ib_cqe		     cqe;
 	char		             pad[ISER_RX_PAD_SIZE];
+} __packed;
+
+/**
+ * struct iser_login_desc - iSER login descriptor
+ *
+ * @req:           pointer to login request buffer
+ * @resp:          pointer to login response buffer
+ * @req_dma:       DMA address of login request buffer
+ * @rsp_dma:      DMA address of login response buffer
+ * @sge:           IB sge for login post recv
+ * @cqe:           completion handler
+ */
+struct iser_login_desc {
+	void                         *req;
+	void                         *rsp;
+	u64                          req_dma;
+	u64                          rsp_dma;
+	struct ib_sge                sge;
+	struct ib_cqe		     cqe;
 } __attribute__((packed));
 
 struct iser_conn;
@@ -333,18 +320,12 @@ struct iscsi_iser_task;
 /**
  * struct iser_comp - iSER completion context
  *
- * @device:     pointer to device handle
  * @cq:         completion queue
- * @wcs:        work completion array
- * @tasklet:    Tasklet handle
  * @active_qps: Number of active QPs attached
  *              to completion context
  */
 struct iser_comp {
-	struct iser_device      *device;
 	struct ib_cq		*cq;
-	struct ib_wc		 wcs[ISER_WC_BATCH_COUNT];
-	struct tasklet_struct	 tasklet;
 	int                      active_qps;
 };
 
@@ -380,7 +361,6 @@ struct iser_reg_ops {
  *
  * @ib_device:     RDMA device
  * @pd:            Protection Domain for this device
- * @dev_attr:      Device attributes container
  * @mr:            Global DMA memory region
  * @event_handler: IB events handle routine
  * @ig_list:	   entry in devices list
@@ -389,18 +369,19 @@ struct iser_reg_ops {
  *                 cpus and device max completion vectors
  * @comps:         Dinamically allocated array of completion handlers
  * @reg_ops:       Registration ops
+ * @remote_inv_sup: Remote invalidate is supported on this device
  */
 struct iser_device {
 	struct ib_device             *ib_device;
 	struct ib_pd	             *pd;
-	struct ib_device_attr	     dev_attr;
 	struct ib_mr	             *mr;
 	struct ib_event_handler      event_handler;
 	struct list_head             ig_list;
 	int                          refcount;
 	int			     comps_used;
 	struct iser_comp	     *comps;
-	struct iser_reg_ops          *reg_ops;
+	const struct iser_reg_ops    *reg_ops;
+	bool                         remote_inv_sup;
 };
 
 #define ISER_CHECK_GUARD	0xc0
@@ -475,10 +456,11 @@ struct iser_fr_pool {
  * @rx_wr:               receive work request for batch posts
  * @device:              reference to iser device
  * @comp:                iser completion context
- * @pi_support:          Indicate device T10-PI support
- * @beacon:              beacon send wr to signal all flush errors were drained
- * @flush_comp:          completes when all connection completions consumed
  * @fr_pool:             connection fast registration poool
+ * @pi_support:          Indicate device T10-PI support
+ * @last:                last send wr to signal all flush errors were drained
+ * @last_cqe:            cqe handler for last wr
+ * @last_comp:           completes when all connection completions consumed
  */
 struct ib_conn {
 	struct rdma_cm_id           *cma_id;
@@ -488,10 +470,12 @@ struct ib_conn {
 	struct ib_recv_wr	     rx_wr[ISER_MIN_POSTED_RX];
 	struct iser_device          *device;
 	struct iser_comp	    *comp;
-	bool			     pi_support;
-	struct ib_send_wr	     beacon;
-	struct completion	     flush_comp;
 	struct iser_fr_pool          fr_pool;
+	bool			     pi_support;
+	struct ib_send_wr	     last;
+	struct ib_cqe		     last_cqe;
+	struct ib_cqe		     reg_cqe;
+	struct completion	     last_comp;
 };
 
 /**
@@ -514,11 +498,7 @@ struct ib_conn {
  * @up_completion:    connection establishment completed
  *                    (state is ISER_CONN_UP)
  * @conn_list:        entry in ig conn list
- * @login_buf:        login data buffer (stores login parameters)
- * @login_req_buf:    login request buffer
- * @login_req_dma:    login request buffer dma address
- * @login_resp_buf:   login response buffer
- * @login_resp_dma:   login response buffer dma address
+ * @login_desc:       login descriptor
  * @rx_desc_head:     head of rx_descs cyclic buffer
  * @rx_descs:         rx buffers array (cyclic buffer)
  * @num_rx_descs:     number of rx descriptors
@@ -541,15 +521,13 @@ struct iser_conn {
 	struct completion	     ib_completion;
 	struct completion	     up_completion;
 	struct list_head	     conn_list;
-
-	char  			     *login_buf;
-	char			     *login_req_buf, *login_resp_buf;
-	u64			     login_req_dma, login_resp_dma;
+	struct iser_login_desc       login_desc;
 	unsigned int 		     rx_desc_head;
 	struct iser_rx_desc	     *rx_descs;
 	u32                          num_rx_descs;
 	unsigned short               scsi_sg_tablesize;
 	unsigned int                 scsi_max_sectors;
+	bool			     snd_w_inv;
 };
 
 /**
@@ -579,9 +557,8 @@ struct iscsi_iser_task {
 
 struct iser_page_vec {
 	u64 *pages;
-	int length;
-	int offset;
-	int data_size;
+	int npages;
+	struct ib_mr fake_mr;
 };
 
 /**
@@ -633,12 +610,14 @@ int iser_conn_terminate(struct iser_conn *iser_conn);
 
 void iser_release_work(struct work_struct *work);
 
-void iser_rcv_completion(struct iser_rx_desc *desc,
-			 unsigned long dto_xfer_len,
-			 struct ib_conn *ib_conn);
-
-void iser_snd_completion(struct iser_tx_desc *desc,
-			 struct ib_conn *ib_conn);
+void iser_err_comp(struct ib_wc *wc, const char *type);
+void iser_login_rsp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_task_rsp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_cmd_comp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_ctrl_comp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_dataout_comp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_reg_comp(struct ib_cq *cq, struct ib_wc *wc);
+void iser_last_comp(struct ib_cq *cq, struct ib_wc *wc);
 
 void iser_task_rdma_init(struct iscsi_iser_task *task);
 
@@ -651,7 +630,8 @@ void iser_finalize_rdma_unaligned_sg(struct iscsi_iser_task *iser_task,
 				     enum iser_data_dir cmd_dir);
 
 int iser_reg_rdma_mem(struct iscsi_iser_task *task,
-		      enum iser_data_dir dir);
+		      enum iser_data_dir dir,
+		      bool all_imm);
 void iser_unreg_rdma_mem(struct iscsi_iser_task *task,
 			 enum iser_data_dir dir);
 
@@ -719,4 +699,28 @@ iser_tx_next_wr(struct iser_tx_desc *tx_desc)
 	return cur_wr;
 }
 
+static inline struct iser_conn *
+to_iser_conn(struct ib_conn *ib_conn)
+{
+	return container_of(ib_conn, struct iser_conn, ib_conn);
+}
+
+static inline struct iser_rx_desc *
+iser_rx(struct ib_cqe *cqe)
+{
+	return container_of(cqe, struct iser_rx_desc, cqe);
+}
+
+static inline struct iser_tx_desc *
+iser_tx(struct ib_cqe *cqe)
+{
+	return container_of(cqe, struct iser_tx_desc, cqe);
+}
+
+static inline struct iser_login_desc *
+iser_login(struct ib_cqe *cqe)
+{
+	return container_of(cqe, struct iser_login_desc, cqe);
+}
+
 #endif
diff --git a/drivers/infiniband/ulp/iser/iser_initiator.c b/drivers/infiniband/ulp/iser/iser_initiator.c
index ffd00c420729..ed54b388e7ad 100644
--- a/drivers/infiniband/ulp/iser/iser_initiator.c
+++ b/drivers/infiniband/ulp/iser/iser_initiator.c
@@ -51,7 +51,7 @@ static int iser_prepare_read_cmd(struct iscsi_task *task)
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	struct iser_mem_reg *mem_reg;
 	int err;
-	struct iser_hdr *hdr = &iser_task->desc.iser_header;
+	struct iser_ctrl *hdr = &iser_task->desc.iser_header;
 	struct iser_data_buf *buf_in = &iser_task->data[ISER_DIR_IN];
 
 	err = iser_dma_map_task_data(iser_task,
@@ -72,7 +72,7 @@ static int iser_prepare_read_cmd(struct iscsi_task *task)
 			return err;
 	}
 
-	err = iser_reg_rdma_mem(iser_task, ISER_DIR_IN);
+	err = iser_reg_rdma_mem(iser_task, ISER_DIR_IN, false);
 	if (err) {
 		iser_err("Failed to set up Data-IN RDMA\n");
 		return err;
@@ -104,7 +104,7 @@ iser_prepare_write_cmd(struct iscsi_task *task,
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	struct iser_mem_reg *mem_reg;
 	int err;
-	struct iser_hdr *hdr = &iser_task->desc.iser_header;
+	struct iser_ctrl *hdr = &iser_task->desc.iser_header;
 	struct iser_data_buf *buf_out = &iser_task->data[ISER_DIR_OUT];
 	struct ib_sge *tx_dsg = &iser_task->desc.tx_sg[1];
 
@@ -126,7 +126,8 @@ iser_prepare_write_cmd(struct iscsi_task *task,
 			return err;
 	}
 
-	err = iser_reg_rdma_mem(iser_task, ISER_DIR_OUT);
+	err = iser_reg_rdma_mem(iser_task, ISER_DIR_OUT,
+				buf_out->data_len == imm_sz);
 	if (err != 0) {
 		iser_err("Failed to register write cmd RDMA mem\n");
 		return err;
@@ -166,7 +167,7 @@ static void iser_create_send_desc(struct iser_conn	*iser_conn,
 	ib_dma_sync_single_for_cpu(device->ib_device,
 		tx_desc->dma_addr, ISER_HEADERS_LEN, DMA_TO_DEVICE);
 
-	memset(&tx_desc->iser_header, 0, sizeof(struct iser_hdr));
+	memset(&tx_desc->iser_header, 0, sizeof(struct iser_ctrl));
 	tx_desc->iser_header.flags = ISER_VER;
 	tx_desc->num_sge = 1;
 }
@@ -174,73 +175,63 @@ static void iser_create_send_desc(struct iser_conn	*iser_conn,
 static void iser_free_login_buf(struct iser_conn *iser_conn)
 {
 	struct iser_device *device = iser_conn->ib_conn.device;
+	struct iser_login_desc *desc = &iser_conn->login_desc;
 
-	if (!iser_conn->login_buf)
+	if (!desc->req)
 		return;
 
-	if (iser_conn->login_req_dma)
-		ib_dma_unmap_single(device->ib_device,
-				    iser_conn->login_req_dma,
-				    ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_TO_DEVICE);
+	ib_dma_unmap_single(device->ib_device, desc->req_dma,
+			    ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_TO_DEVICE);
 
-	if (iser_conn->login_resp_dma)
-		ib_dma_unmap_single(device->ib_device,
-				    iser_conn->login_resp_dma,
-				    ISER_RX_LOGIN_SIZE, DMA_FROM_DEVICE);
+	ib_dma_unmap_single(device->ib_device, desc->rsp_dma,
+			    ISER_RX_LOGIN_SIZE, DMA_FROM_DEVICE);
 
-	kfree(iser_conn->login_buf);
+	kfree(desc->req);
+	kfree(desc->rsp);
 
 	/* make sure we never redo any unmapping */
-	iser_conn->login_req_dma = 0;
-	iser_conn->login_resp_dma = 0;
-	iser_conn->login_buf = NULL;
+	desc->req = NULL;
+	desc->rsp = NULL;
 }
 
 static int iser_alloc_login_buf(struct iser_conn *iser_conn)
 {
 	struct iser_device *device = iser_conn->ib_conn.device;
-	int			req_err, resp_err;
-
-	BUG_ON(device == NULL);
-
-	iser_conn->login_buf = kmalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
-				     ISER_RX_LOGIN_SIZE, GFP_KERNEL);
-	if (!iser_conn->login_buf)
-		goto out_err;
-
-	iser_conn->login_req_buf  = iser_conn->login_buf;
-	iser_conn->login_resp_buf = iser_conn->login_buf +
-						ISCSI_DEF_MAX_RECV_SEG_LEN;
-
-	iser_conn->login_req_dma = ib_dma_map_single(device->ib_device,
-						     iser_conn->login_req_buf,
-						     ISCSI_DEF_MAX_RECV_SEG_LEN,
-						     DMA_TO_DEVICE);
-
-	iser_conn->login_resp_dma = ib_dma_map_single(device->ib_device,
-						      iser_conn->login_resp_buf,
-						      ISER_RX_LOGIN_SIZE,
-						      DMA_FROM_DEVICE);
-
-	req_err  = ib_dma_mapping_error(device->ib_device,
-					iser_conn->login_req_dma);
-	resp_err = ib_dma_mapping_error(device->ib_device,
-					iser_conn->login_resp_dma);
-
-	if (req_err || resp_err) {
-		if (req_err)
-			iser_conn->login_req_dma = 0;
-		if (resp_err)
-			iser_conn->login_resp_dma = 0;
-		goto free_login_buf;
-	}
+	struct iser_login_desc *desc = &iser_conn->login_desc;
+
+	desc->req = kmalloc(ISCSI_DEF_MAX_RECV_SEG_LEN, GFP_KERNEL);
+	if (!desc->req)
+		return -ENOMEM;
+
+	desc->req_dma = ib_dma_map_single(device->ib_device, desc->req,
+					  ISCSI_DEF_MAX_RECV_SEG_LEN,
+					  DMA_TO_DEVICE);
+	if (ib_dma_mapping_error(device->ib_device,
+				desc->req_dma))
+		goto free_req;
+
+	desc->rsp = kmalloc(ISER_RX_LOGIN_SIZE, GFP_KERNEL);
+	if (!desc->rsp)
+		goto unmap_req;
+
+	desc->rsp_dma = ib_dma_map_single(device->ib_device, desc->rsp,
+					   ISER_RX_LOGIN_SIZE,
+					   DMA_FROM_DEVICE);
+	if (ib_dma_mapping_error(device->ib_device,
+				desc->rsp_dma))
+		goto free_rsp;
+
 	return 0;
 
-free_login_buf:
-	iser_free_login_buf(iser_conn);
+free_rsp:
+	kfree(desc->rsp);
+unmap_req:
+	ib_dma_unmap_single(device->ib_device, desc->req_dma,
+			    ISCSI_DEF_MAX_RECV_SEG_LEN,
+			    DMA_TO_DEVICE);
+free_req:
+	kfree(desc->req);
 
-out_err:
-	iser_err("unable to alloc or map login buf\n");
 	return -ENOMEM;
 }
 
@@ -280,11 +271,11 @@ int iser_alloc_rx_descriptors(struct iser_conn *iser_conn,
 			goto rx_desc_dma_map_failed;
 
 		rx_desc->dma_addr = dma_addr;
-
+		rx_desc->cqe.done = iser_task_rsp;
 		rx_sg = &rx_desc->rx_sg;
-		rx_sg->addr   = rx_desc->dma_addr;
+		rx_sg->addr = rx_desc->dma_addr;
 		rx_sg->length = ISER_RX_PAYLOAD_SIZE;
-		rx_sg->lkey   = device->pd->local_dma_lkey;
+		rx_sg->lkey = device->pd->local_dma_lkey;
 	}
 
 	iser_conn->rx_desc_head = 0;
@@ -383,6 +374,7 @@ int iser_send_command(struct iscsi_conn *conn,
 
 	/* build the tx desc regd header and add it to the tx desc dto */
 	tx_desc->type = ISCSI_TX_SCSI_COMMAND;
+	tx_desc->cqe.done = iser_cmd_comp;
 	iser_create_send_desc(iser_conn, tx_desc);
 
 	if (hdr->flags & ISCSI_FLAG_CMD_READ) {
@@ -464,6 +456,7 @@ int iser_send_data_out(struct iscsi_conn *conn,
 	}
 
 	tx_desc->type = ISCSI_TX_DATAOUT;
+	tx_desc->cqe.done = iser_dataout_comp;
 	tx_desc->iser_header.flags = ISER_VER;
 	memcpy(&tx_desc->iscsi_header, hdr, sizeof(struct iscsi_hdr));
 
@@ -513,6 +506,7 @@ int iser_send_control(struct iscsi_conn *conn,
 
 	/* build the tx desc regd header and add it to the tx desc dto */
 	mdesc->type = ISCSI_TX_CONTROL;
+	mdesc->cqe.done = iser_ctrl_comp;
 	iser_create_send_desc(iser_conn, mdesc);
 
 	device = iser_conn->ib_conn.device;
@@ -520,25 +514,25 @@ int iser_send_control(struct iscsi_conn *conn,
 	data_seg_len = ntoh24(task->hdr->dlength);
 
 	if (data_seg_len > 0) {
+		struct iser_login_desc *desc = &iser_conn->login_desc;
 		struct ib_sge *tx_dsg = &mdesc->tx_sg[1];
+
 		if (task != conn->login_task) {
 			iser_err("data present on non login task!!!\n");
 			goto send_control_error;
 		}
 
-		ib_dma_sync_single_for_cpu(device->ib_device,
-			iser_conn->login_req_dma, task->data_count,
-			DMA_TO_DEVICE);
+		ib_dma_sync_single_for_cpu(device->ib_device, desc->req_dma,
+					   task->data_count, DMA_TO_DEVICE);
 
-		memcpy(iser_conn->login_req_buf, task->data, task->data_count);
+		memcpy(desc->req, task->data, task->data_count);
 
-		ib_dma_sync_single_for_device(device->ib_device,
-			iser_conn->login_req_dma, task->data_count,
-			DMA_TO_DEVICE);
+		ib_dma_sync_single_for_device(device->ib_device, desc->req_dma,
+					      task->data_count, DMA_TO_DEVICE);
 
-		tx_dsg->addr    = iser_conn->login_req_dma;
-		tx_dsg->length  = task->data_count;
-		tx_dsg->lkey    = device->pd->local_dma_lkey;
+		tx_dsg->addr = desc->req_dma;
+		tx_dsg->length = task->data_count;
+		tx_dsg->lkey = device->pd->local_dma_lkey;
 		mdesc->num_sge = 2;
 	}
 
@@ -562,41 +556,126 @@ send_control_error:
 	return err;
 }
 
-/**
- * iser_rcv_dto_completion - recv DTO completion
- */
-void iser_rcv_completion(struct iser_rx_desc *rx_desc,
-			 unsigned long rx_xfer_len,
-			 struct ib_conn *ib_conn)
+void iser_login_rsp(struct ib_cq *cq, struct ib_wc *wc)
 {
-	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
-						   ib_conn);
+	struct ib_conn *ib_conn = wc->qp->qp_context;
+	struct iser_conn *iser_conn = to_iser_conn(ib_conn);
+	struct iser_login_desc *desc = iser_login(wc->wr_cqe);
 	struct iscsi_hdr *hdr;
-	u64 rx_dma;
-	int rx_buflen, outstanding, count, err;
+	char *data;
+	int length;
 
-	/* differentiate between login to all other PDUs */
-	if ((char *)rx_desc == iser_conn->login_resp_buf) {
-		rx_dma = iser_conn->login_resp_dma;
-		rx_buflen = ISER_RX_LOGIN_SIZE;
-	} else {
-		rx_dma = rx_desc->dma_addr;
-		rx_buflen = ISER_RX_PAYLOAD_SIZE;
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		iser_err_comp(wc, "login_rsp");
+		return;
+	}
+
+	ib_dma_sync_single_for_cpu(ib_conn->device->ib_device,
+				   desc->rsp_dma, ISER_RX_LOGIN_SIZE,
+				   DMA_FROM_DEVICE);
+
+	hdr = desc->rsp + sizeof(struct iser_ctrl);
+	data = desc->rsp + ISER_HEADERS_LEN;
+	length = wc->byte_len - ISER_HEADERS_LEN;
+
+	iser_dbg("op 0x%x itt 0x%x dlen %d\n", hdr->opcode,
+		 hdr->itt, length);
+
+	iscsi_iser_recv(iser_conn->iscsi_conn, hdr, data, length);
+
+	ib_dma_sync_single_for_device(ib_conn->device->ib_device,
+				      desc->rsp_dma, ISER_RX_LOGIN_SIZE,
+				      DMA_FROM_DEVICE);
+
+	ib_conn->post_recv_buf_count--;
+}
+
+static inline void
+iser_inv_desc(struct iser_fr_desc *desc, u32 rkey)
+{
+	if (likely(rkey == desc->rsc.mr->rkey))
+		desc->rsc.mr_valid = 0;
+	else if (likely(rkey == desc->pi_ctx->sig_mr->rkey))
+		desc->pi_ctx->sig_mr_valid = 0;
+}
+
+static int
+iser_check_remote_inv(struct iser_conn *iser_conn,
+		      struct ib_wc *wc,
+		      struct iscsi_hdr *hdr)
+{
+	if (wc->wc_flags & IB_WC_WITH_INVALIDATE) {
+		struct iscsi_task *task;
+		u32 rkey = wc->ex.invalidate_rkey;
+
+		iser_dbg("conn %p: remote invalidation for rkey %#x\n",
+			 iser_conn, rkey);
+
+		if (unlikely(!iser_conn->snd_w_inv)) {
+			iser_err("conn %p: unexepected remote invalidation, "
+				 "terminating connection\n", iser_conn);
+			return -EPROTO;
+		}
+
+		task = iscsi_itt_to_ctask(iser_conn->iscsi_conn, hdr->itt);
+		if (likely(task)) {
+			struct iscsi_iser_task *iser_task = task->dd_data;
+			struct iser_fr_desc *desc;
+
+			if (iser_task->dir[ISER_DIR_IN]) {
+				desc = iser_task->rdma_reg[ISER_DIR_IN].mem_h;
+				iser_inv_desc(desc, rkey);
+			}
+
+			if (iser_task->dir[ISER_DIR_OUT]) {
+				desc = iser_task->rdma_reg[ISER_DIR_OUT].mem_h;
+				iser_inv_desc(desc, rkey);
+			}
+		} else {
+			iser_err("failed to get task for itt=%d\n", hdr->itt);
+			return -EINVAL;
+		}
 	}
 
-	ib_dma_sync_single_for_cpu(ib_conn->device->ib_device, rx_dma,
-				   rx_buflen, DMA_FROM_DEVICE);
+	return 0;
+}
 
-	hdr = &rx_desc->iscsi_header;
+
+void iser_task_rsp(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct ib_conn *ib_conn = wc->qp->qp_context;
+	struct iser_conn *iser_conn = to_iser_conn(ib_conn);
+	struct iser_rx_desc *desc = iser_rx(wc->wr_cqe);
+	struct iscsi_hdr *hdr;
+	int length;
+	int outstanding, count, err;
+
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		iser_err_comp(wc, "task_rsp");
+		return;
+	}
+
+	ib_dma_sync_single_for_cpu(ib_conn->device->ib_device,
+				   desc->dma_addr, ISER_RX_PAYLOAD_SIZE,
+				   DMA_FROM_DEVICE);
+
+	hdr = &desc->iscsi_header;
+	length = wc->byte_len - ISER_HEADERS_LEN;
 
 	iser_dbg("op 0x%x itt 0x%x dlen %d\n", hdr->opcode,
-			hdr->itt, (int)(rx_xfer_len - ISER_HEADERS_LEN));
+		 hdr->itt, length);
+
+	if (iser_check_remote_inv(iser_conn, wc, hdr)) {
+		iscsi_conn_failure(iser_conn->iscsi_conn,
+				   ISCSI_ERR_CONN_FAILED);
+		return;
+	}
 
-	iscsi_iser_recv(iser_conn->iscsi_conn, hdr, rx_desc->data,
-			rx_xfer_len - ISER_HEADERS_LEN);
+	iscsi_iser_recv(iser_conn->iscsi_conn, hdr, desc->data, length);
 
-	ib_dma_sync_single_for_device(ib_conn->device->ib_device, rx_dma,
-				      rx_buflen, DMA_FROM_DEVICE);
+	ib_dma_sync_single_for_device(ib_conn->device->ib_device,
+				      desc->dma_addr, ISER_RX_PAYLOAD_SIZE,
+				      DMA_FROM_DEVICE);
 
 	/* decrementing conn->post_recv_buf_count only --after-- freeing the   *
 	 * task eliminates the need to worry on tasks which are completed in   *
@@ -604,9 +683,6 @@ void iser_rcv_completion(struct iser_rx_desc *rx_desc,
 	 * for the posted rx bufs refcount to become zero handles everything   */
 	ib_conn->post_recv_buf_count--;
 
-	if (rx_dma == iser_conn->login_resp_dma)
-		return;
-
 	outstanding = ib_conn->post_recv_buf_count;
 	if (outstanding + iser_conn->min_posted_rx <= iser_conn->qp_max_recv_dtos) {
 		count = min(iser_conn->qp_max_recv_dtos - outstanding,
@@ -617,26 +693,47 @@ void iser_rcv_completion(struct iser_rx_desc *rx_desc,
 	}
 }
 
-void iser_snd_completion(struct iser_tx_desc *tx_desc,
-			struct ib_conn *ib_conn)
+void iser_cmd_comp(struct ib_cq *cq, struct ib_wc *wc)
 {
+	if (unlikely(wc->status != IB_WC_SUCCESS))
+		iser_err_comp(wc, "command");
+}
+
+void iser_ctrl_comp(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct iser_tx_desc *desc = iser_tx(wc->wr_cqe);
 	struct iscsi_task *task;
-	struct iser_device *device = ib_conn->device;
 
-	if (tx_desc->type == ISCSI_TX_DATAOUT) {
-		ib_dma_unmap_single(device->ib_device, tx_desc->dma_addr,
-					ISER_HEADERS_LEN, DMA_TO_DEVICE);
-		kmem_cache_free(ig.desc_cache, tx_desc);
-		tx_desc = NULL;
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		iser_err_comp(wc, "control");
+		return;
 	}
 
-	if (tx_desc && tx_desc->type == ISCSI_TX_CONTROL) {
-		/* this arithmetic is legal by libiscsi dd_data allocation */
-		task = (void *) ((long)(void *)tx_desc -
-				  sizeof(struct iscsi_task));
-		if (task->hdr->itt == RESERVED_ITT)
-			iscsi_put_task(task);
-	}
+	/* this arithmetic is legal by libiscsi dd_data allocation */
+	task = (void *)desc - sizeof(struct iscsi_task);
+	if (task->hdr->itt == RESERVED_ITT)
+		iscsi_put_task(task);
+}
+
+void iser_dataout_comp(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct iser_tx_desc *desc = iser_tx(wc->wr_cqe);
+	struct ib_conn *ib_conn = wc->qp->qp_context;
+	struct iser_device *device = ib_conn->device;
+
+	if (unlikely(wc->status != IB_WC_SUCCESS))
+		iser_err_comp(wc, "dataout");
+
+	ib_dma_unmap_single(device->ib_device, desc->dma_addr,
+			    ISER_HEADERS_LEN, DMA_TO_DEVICE);
+	kmem_cache_free(ig.desc_cache, desc);
+}
+
+void iser_last_comp(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct ib_conn *ib_conn = wc->qp->qp_context;
+
+	complete(&ib_conn->last_comp);
 }
 
 void iser_task_rdma_init(struct iscsi_iser_task *iser_task)
diff --git a/drivers/infiniband/ulp/iser/iser_memory.c b/drivers/infiniband/ulp/iser/iser_memory.c
index ea765fb9664d..9a391cc5b9b3 100644
--- a/drivers/infiniband/ulp/iser/iser_memory.c
+++ b/drivers/infiniband/ulp/iser/iser_memory.c
@@ -49,7 +49,7 @@ int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 		     struct iser_reg_resources *rsc,
 		     struct iser_mem_reg *mem_reg);
 
-static struct iser_reg_ops fastreg_ops = {
+static const struct iser_reg_ops fastreg_ops = {
 	.alloc_reg_res	= iser_alloc_fastreg_pool,
 	.free_reg_res	= iser_free_fastreg_pool,
 	.reg_mem	= iser_fast_reg_mr,
@@ -58,7 +58,7 @@ static struct iser_reg_ops fastreg_ops = {
 	.reg_desc_put	= iser_reg_desc_put_fr,
 };
 
-static struct iser_reg_ops fmr_ops = {
+static const struct iser_reg_ops fmr_ops = {
 	.alloc_reg_res	= iser_alloc_fmr_pool,
 	.free_reg_res	= iser_free_fmr_pool,
 	.reg_mem	= iser_fast_reg_fmr,
@@ -67,19 +67,24 @@ static struct iser_reg_ops fmr_ops = {
 	.reg_desc_put	= iser_reg_desc_put_fmr,
 };
 
+void iser_reg_comp(struct ib_cq *cq, struct ib_wc *wc)
+{
+	iser_err_comp(wc, "memreg");
+}
+
 int iser_assign_reg_ops(struct iser_device *device)
 {
-	struct ib_device_attr *dev_attr = &device->dev_attr;
+	struct ib_device *ib_dev = device->ib_device;
 
 	/* Assign function handles  - based on FMR support */
-	if (device->ib_device->alloc_fmr && device->ib_device->dealloc_fmr &&
-	    device->ib_device->map_phys_fmr && device->ib_device->unmap_fmr) {
+	if (ib_dev->alloc_fmr && ib_dev->dealloc_fmr &&
+	    ib_dev->map_phys_fmr && ib_dev->unmap_fmr) {
 		iser_info("FMR supported, using FMR for registration\n");
 		device->reg_ops = &fmr_ops;
-	} else
-	if (dev_attr->device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS) {
+	} else if (ib_dev->attrs.device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS) {
 		iser_info("FastReg supported, using FastReg for registration\n");
 		device->reg_ops = &fastreg_ops;
+		device->remote_inv_sup = iser_always_reg;
 	} else {
 		iser_err("IB device does not support FMRs nor FastRegs, can't register memory\n");
 		return -1;
@@ -131,67 +136,6 @@ iser_reg_desc_put_fmr(struct ib_conn *ib_conn,
 {
 }
 
-#define IS_4K_ALIGNED(addr)	((((unsigned long)addr) & ~MASK_4K) == 0)
-
-/**
- * iser_sg_to_page_vec - Translates scatterlist entries to physical addresses
- * and returns the length of resulting physical address array (may be less than
- * the original due to possible compaction).
- *
- * we build a "page vec" under the assumption that the SG meets the RDMA
- * alignment requirements. Other then the first and last SG elements, all
- * the "internal" elements can be compacted into a list whose elements are
- * dma addresses of physical pages. The code supports also the weird case
- * where --few fragments of the same page-- are present in the SG as
- * consecutive elements. Also, it handles one entry SG.
- */
-
-static int iser_sg_to_page_vec(struct iser_data_buf *data,
-			       struct ib_device *ibdev, u64 *pages,
-			       int *offset, int *data_size)
-{
-	struct scatterlist *sg, *sgl = data->sg;
-	u64 start_addr, end_addr, page, chunk_start = 0;
-	unsigned long total_sz = 0;
-	unsigned int dma_len;
-	int i, new_chunk, cur_page, last_ent = data->dma_nents - 1;
-
-	/* compute the offset of first element */
-	*offset = (u64) sgl[0].offset & ~MASK_4K;
-
-	new_chunk = 1;
-	cur_page  = 0;
-	for_each_sg(sgl, sg, data->dma_nents, i) {
-		start_addr = ib_sg_dma_address(ibdev, sg);
-		if (new_chunk)
-			chunk_start = start_addr;
-		dma_len = ib_sg_dma_len(ibdev, sg);
-		end_addr = start_addr + dma_len;
-		total_sz += dma_len;
-
-		/* collect page fragments until aligned or end of SG list */
-		if (!IS_4K_ALIGNED(end_addr) && i < last_ent) {
-			new_chunk = 0;
-			continue;
-		}
-		new_chunk = 1;
-
-		/* address of the first page in the contiguous chunk;
-		   masking relevant for the very first SG entry,
-		   which might be unaligned */
-		page = chunk_start & MASK_4K;
-		do {
-			pages[cur_page++] = page;
-			page += SIZE_4K;
-		} while (page < end_addr);
-	}
-
-	*data_size = total_sz;
-	iser_dbg("page_vec->data_size:%d cur_page %d\n",
-		 *data_size, cur_page);
-	return cur_page;
-}
-
 static void iser_data_buf_dump(struct iser_data_buf *data,
 			       struct ib_device *ibdev)
 {
@@ -210,10 +154,10 @@ static void iser_dump_page_vec(struct iser_page_vec *page_vec)
 {
 	int i;
 
-	iser_err("page vec length %d data size %d\n",
-		 page_vec->length, page_vec->data_size);
-	for (i = 0; i < page_vec->length; i++)
-		iser_err("%d %lx\n",i,(unsigned long)page_vec->pages[i]);
+	iser_err("page vec npages %d data length %d\n",
+		 page_vec->npages, page_vec->fake_mr.length);
+	for (i = 0; i < page_vec->npages; i++)
+		iser_err("vec[%d]: %llx\n", i, page_vec->pages[i]);
 }
 
 int iser_dma_map_task_data(struct iscsi_iser_task *iser_task,
@@ -251,7 +195,11 @@ iser_reg_dma(struct iser_device *device, struct iser_data_buf *mem,
 	struct scatterlist *sg = mem->sg;
 
 	reg->sge.lkey = device->pd->local_dma_lkey;
-	reg->rkey = device->mr->rkey;
+	/*
+	 * FIXME: rework the registration code path to differentiate
+	 * rkey/lkey use cases
+	 */
+	reg->rkey = device->mr ? device->mr->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]);
 
@@ -262,11 +210,16 @@ iser_reg_dma(struct iser_device *device, struct iser_data_buf *mem,
 	return 0;
 }
 
-/**
- * iser_reg_page_vec - Register physical memory
- *
- * returns: 0 on success, errno code on failure
- */
+static int iser_set_page(struct ib_mr *mr, u64 addr)
+{
+	struct iser_page_vec *page_vec =
+		container_of(mr, struct iser_page_vec, fake_mr);
+
+	page_vec->pages[page_vec->npages++] = addr;
+
+	return 0;
+}
+
 static
 int iser_fast_reg_fmr(struct iscsi_iser_task *iser_task,
 		      struct iser_data_buf *mem,
@@ -280,22 +233,19 @@ int iser_fast_reg_fmr(struct iscsi_iser_task *iser_task,
 	struct ib_pool_fmr *fmr;
 	int ret, plen;
 
-	plen = iser_sg_to_page_vec(mem, device->ib_device,
-				   page_vec->pages,
-				   &page_vec->offset,
-				   &page_vec->data_size);
-	page_vec->length = plen;
-	if (plen * SIZE_4K < page_vec->data_size) {
+	page_vec->npages = 0;
+	page_vec->fake_mr.page_size = SIZE_4K;
+	plen = ib_sg_to_pages(&page_vec->fake_mr, mem->sg,
+			      mem->size, iser_set_page);
+	if (unlikely(plen < mem->size)) {
 		iser_err("page vec too short to hold this SG\n");
 		iser_data_buf_dump(mem, device->ib_device);
 		iser_dump_page_vec(page_vec);
 		return -EINVAL;
 	}
 
-	fmr  = ib_fmr_pool_map_phys(fmr_pool,
-				    page_vec->pages,
-				    page_vec->length,
-				    page_vec->pages[0]);
+	fmr  = ib_fmr_pool_map_phys(fmr_pool, page_vec->pages,
+				    page_vec->npages, page_vec->pages[0]);
 	if (IS_ERR(fmr)) {
 		ret = PTR_ERR(fmr);
 		iser_err("ib_fmr_pool_map_phys failed: %d\n", ret);
@@ -304,8 +254,8 @@ int iser_fast_reg_fmr(struct iscsi_iser_task *iser_task,
 
 	reg->sge.lkey = fmr->fmr->lkey;
 	reg->rkey = fmr->fmr->rkey;
-	reg->sge.addr = page_vec->pages[0] + page_vec->offset;
-	reg->sge.length = page_vec->data_size;
+	reg->sge.addr = page_vec->fake_mr.iova;
+	reg->sge.length = page_vec->fake_mr.length;
 	reg->mem_h = fmr;
 
 	iser_dbg("fmr reg: lkey=0x%x, rkey=0x%x, addr=0x%llx,"
@@ -413,19 +363,16 @@ iser_set_prot_checks(struct scsi_cmnd *sc, u8 *mask)
 		*mask |= ISER_CHECK_GUARD;
 }
 
-static void
-iser_inv_rkey(struct ib_send_wr *inv_wr, struct ib_mr *mr)
+static inline void
+iser_inv_rkey(struct ib_send_wr *inv_wr,
+	      struct ib_mr *mr,
+	      struct ib_cqe *cqe)
 {
-	u32 rkey;
-
 	inv_wr->opcode = IB_WR_LOCAL_INV;
-	inv_wr->wr_id = ISER_FASTREG_LI_WRID;
+	inv_wr->wr_cqe = cqe;
 	inv_wr->ex.invalidate_rkey = mr->rkey;
 	inv_wr->send_flags = 0;
 	inv_wr->num_sge = 0;
-
-	rkey = ib_inc_rkey(mr->rkey);
-	ib_update_fast_reg_key(mr, rkey);
 }
 
 static int
@@ -437,7 +384,9 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 {
 	struct iser_tx_desc *tx_desc = &iser_task->desc;
 	struct ib_sig_attrs *sig_attrs = &tx_desc->sig_attrs;
+	struct ib_cqe *cqe = &iser_task->iser_conn->ib_conn.reg_cqe;
 	struct ib_sig_handover_wr *wr;
+	struct ib_mr *mr = pi_ctx->sig_mr;
 	int ret;
 
 	memset(sig_attrs, 0, sizeof(*sig_attrs));
@@ -447,17 +396,19 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 
 	iser_set_prot_checks(iser_task->sc, &sig_attrs->check_mask);
 
-	if (!pi_ctx->sig_mr_valid)
-		iser_inv_rkey(iser_tx_next_wr(tx_desc), pi_ctx->sig_mr);
+	if (pi_ctx->sig_mr_valid)
+		iser_inv_rkey(iser_tx_next_wr(tx_desc), mr, cqe);
+
+	ib_update_fast_reg_key(mr, ib_inc_rkey(mr->rkey));
 
 	wr = sig_handover_wr(iser_tx_next_wr(tx_desc));
 	wr->wr.opcode = IB_WR_REG_SIG_MR;
-	wr->wr.wr_id = ISER_FASTREG_LI_WRID;
+	wr->wr.wr_cqe = cqe;
 	wr->wr.sg_list = &data_reg->sge;
 	wr->wr.num_sge = 1;
 	wr->wr.send_flags = 0;
 	wr->sig_attrs = sig_attrs;
-	wr->sig_mr = pi_ctx->sig_mr;
+	wr->sig_mr = mr;
 	if (scsi_prot_sg_count(iser_task->sc))
 		wr->prot = &prot_reg->sge;
 	else
@@ -465,10 +416,10 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 	wr->access_flags = IB_ACCESS_LOCAL_WRITE |
 			   IB_ACCESS_REMOTE_READ |
 			   IB_ACCESS_REMOTE_WRITE;
-	pi_ctx->sig_mr_valid = 0;
+	pi_ctx->sig_mr_valid = 1;
 
-	sig_reg->sge.lkey = pi_ctx->sig_mr->lkey;
-	sig_reg->rkey = pi_ctx->sig_mr->rkey;
+	sig_reg->sge.lkey = mr->lkey;
+	sig_reg->rkey = mr->rkey;
 	sig_reg->sge.addr = 0;
 	sig_reg->sge.length = scsi_transfer_length(iser_task->sc);
 
@@ -485,12 +436,15 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 			    struct iser_mem_reg *reg)
 {
 	struct iser_tx_desc *tx_desc = &iser_task->desc;
+	struct ib_cqe *cqe = &iser_task->iser_conn->ib_conn.reg_cqe;
 	struct ib_mr *mr = rsc->mr;
 	struct ib_reg_wr *wr;
 	int n;
 
-	if (!rsc->mr_valid)
-		iser_inv_rkey(iser_tx_next_wr(tx_desc), mr);
+	if (rsc->mr_valid)
+		iser_inv_rkey(iser_tx_next_wr(tx_desc), mr, cqe);
+
+	ib_update_fast_reg_key(mr, ib_inc_rkey(mr->rkey));
 
 	n = ib_map_mr_sg(mr, mem->sg, mem->size, SIZE_4K);
 	if (unlikely(n != mem->size)) {
@@ -501,7 +455,7 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 
 	wr = reg_wr(iser_tx_next_wr(tx_desc));
 	wr->wr.opcode = IB_WR_REG_MR;
-	wr->wr.wr_id = ISER_FASTREG_LI_WRID;
+	wr->wr.wr_cqe = cqe;
 	wr->wr.send_flags = 0;
 	wr->wr.num_sge = 0;
 	wr->mr = mr;
@@ -510,7 +464,7 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 		     IB_ACCESS_REMOTE_WRITE |
 		     IB_ACCESS_REMOTE_READ;
 
-	rsc->mr_valid = 0;
+	rsc->mr_valid = 1;
 
 	reg->sge.lkey = mr->lkey;
 	reg->rkey = mr->rkey;
@@ -554,7 +508,8 @@ iser_reg_data_sg(struct iscsi_iser_task *task,
 }
 
 int iser_reg_rdma_mem(struct iscsi_iser_task *task,
-		      enum iser_data_dir dir)
+		      enum iser_data_dir dir,
+		      bool all_imm)
 {
 	struct ib_conn *ib_conn = &task->iser_conn->ib_conn;
 	struct iser_device *device = ib_conn->device;
@@ -565,8 +520,8 @@ int iser_reg_rdma_mem(struct iscsi_iser_task *task,
 	bool use_dma_key;
 	int err;
 
-	use_dma_key = (mem->dma_nents == 1 && !iser_always_reg &&
-		       scsi_get_prot_op(task->sc) == SCSI_PROT_NORMAL);
+	use_dma_key = mem->dma_nents == 1 && (all_imm || !iser_always_reg) &&
+		      scsi_get_prot_op(task->sc) == SCSI_PROT_NORMAL;
 
 	if (!use_dma_key) {
 		desc = device->reg_ops->reg_desc_get(ib_conn);
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 42f4da620f2e..40c0f4978e2f 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -44,17 +44,6 @@
 #define ISER_MAX_CQ_LEN		(ISER_MAX_RX_LEN + ISER_MAX_TX_LEN + \
 				 ISCSI_ISER_MAX_CONN)
 
-static int iser_cq_poll_limit = 512;
-
-static void iser_cq_tasklet_fn(unsigned long data);
-static void iser_cq_callback(struct ib_cq *cq, void *cq_context);
-
-static void iser_cq_event_callback(struct ib_event *cause, void *context)
-{
-	iser_err("cq event %s (%d)\n",
-		 ib_event_msg(cause->event), cause->event);
-}
-
 static void iser_qp_event_callback(struct ib_event *cause, void *context)
 {
 	iser_err("qp event %s (%d)\n",
@@ -78,59 +67,40 @@ static void iser_event_handler(struct ib_event_handler *handler,
  */
 static int iser_create_device_ib_res(struct iser_device *device)
 {
-	struct ib_device_attr *dev_attr = &device->dev_attr;
+	struct ib_device *ib_dev = device->ib_device;
 	int ret, i, max_cqe;
 
-	ret = ib_query_device(device->ib_device, dev_attr);
-	if (ret) {
-		pr_warn("Query device failed for %s\n", device->ib_device->name);
-		return ret;
-	}
-
 	ret = iser_assign_reg_ops(device);
 	if (ret)
 		return ret;
 
 	device->comps_used = min_t(int, num_online_cpus(),
-				 device->ib_device->num_comp_vectors);
+				 ib_dev->num_comp_vectors);
 
 	device->comps = kcalloc(device->comps_used, sizeof(*device->comps),
 				GFP_KERNEL);
 	if (!device->comps)
 		goto comps_err;
 
-	max_cqe = min(ISER_MAX_CQ_LEN, dev_attr->max_cqe);
+	max_cqe = min(ISER_MAX_CQ_LEN, ib_dev->attrs.max_cqe);
 
 	iser_info("using %d CQs, device %s supports %d vectors max_cqe %d\n",
-		  device->comps_used, device->ib_device->name,
-		  device->ib_device->num_comp_vectors, max_cqe);
+		  device->comps_used, ib_dev->name,
+		  ib_dev->num_comp_vectors, max_cqe);
 
-	device->pd = ib_alloc_pd(device->ib_device);
+	device->pd = ib_alloc_pd(ib_dev);
 	if (IS_ERR(device->pd))
 		goto pd_err;
 
 	for (i = 0; i < device->comps_used; i++) {
-		struct ib_cq_init_attr cq_attr = {};
 		struct iser_comp *comp = &device->comps[i];
 
-		comp->device = device;
-		cq_attr.cqe = max_cqe;
-		cq_attr.comp_vector = i;
-		comp->cq = ib_create_cq(device->ib_device,
-					iser_cq_callback,
-					iser_cq_event_callback,
-					(void *)comp,
-					&cq_attr);
+		comp->cq = ib_alloc_cq(ib_dev, comp, max_cqe, i,
+				       IB_POLL_SOFTIRQ);
 		if (IS_ERR(comp->cq)) {
 			comp->cq = NULL;
 			goto cq_err;
 		}
-
-		if (ib_req_notify_cq(comp->cq, IB_CQ_NEXT_COMP))
-			goto cq_err;
-
-		tasklet_init(&comp->tasklet, iser_cq_tasklet_fn,
-			     (unsigned long)comp);
 	}
 
 	if (!iser_always_reg) {
@@ -140,11 +110,11 @@ static int iser_create_device_ib_res(struct iser_device *device)
 
 		device->mr = ib_get_dma_mr(device->pd, access);
 		if (IS_ERR(device->mr))
-			goto dma_mr_err;
+			goto cq_err;
 	}
 
-	INIT_IB_EVENT_HANDLER(&device->event_handler, device->ib_device,
-				iser_event_handler);
+	INIT_IB_EVENT_HANDLER(&device->event_handler, ib_dev,
+			      iser_event_handler);
 	if (ib_register_event_handler(&device->event_handler))
 		goto handler_err;
 
@@ -153,15 +123,12 @@ static int iser_create_device_ib_res(struct iser_device *device)
 handler_err:
 	if (device->mr)
 		ib_dereg_mr(device->mr);
-dma_mr_err:
-	for (i = 0; i < device->comps_used; i++)
-		tasklet_kill(&device->comps[i].tasklet);
 cq_err:
 	for (i = 0; i < device->comps_used; i++) {
 		struct iser_comp *comp = &device->comps[i];
 
 		if (comp->cq)
-			ib_destroy_cq(comp->cq);
+			ib_free_cq(comp->cq);
 	}
 	ib_dealloc_pd(device->pd);
 pd_err:
@@ -182,8 +149,7 @@ static void iser_free_device_ib_res(struct iser_device *device)
 	for (i = 0; i < device->comps_used; i++) {
 		struct iser_comp *comp = &device->comps[i];
 
-		tasklet_kill(&comp->tasklet);
-		ib_destroy_cq(comp->cq);
+		ib_free_cq(comp->cq);
 		comp->cq = NULL;
 	}
 
@@ -299,7 +265,7 @@ iser_alloc_reg_res(struct ib_device *ib_device,
 		iser_err("Failed to allocate ib_fast_reg_mr err=%d\n", ret);
 		return ret;
 	}
-	res->mr_valid = 1;
+	res->mr_valid = 0;
 
 	return 0;
 }
@@ -336,7 +302,7 @@ iser_alloc_pi_ctx(struct ib_device *ib_device,
 		ret = PTR_ERR(pi_ctx->sig_mr);
 		goto sig_mr_failure;
 	}
-	pi_ctx->sig_mr_valid = 1;
+	pi_ctx->sig_mr_valid = 0;
 	desc->pi_ctx->sig_protected = 0;
 
 	return 0;
@@ -461,10 +427,9 @@ void iser_free_fastreg_pool(struct ib_conn *ib_conn)
  */
 static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 {
-	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
-						   ib_conn);
+	struct iser_conn *iser_conn = to_iser_conn(ib_conn);
 	struct iser_device	*device;
-	struct ib_device_attr *dev_attr;
+	struct ib_device	*ib_dev;
 	struct ib_qp_init_attr	init_attr;
 	int			ret = -ENOMEM;
 	int index, min_index = 0;
@@ -472,7 +437,7 @@ static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 	BUG_ON(ib_conn->device == NULL);
 
 	device = ib_conn->device;
-	dev_attr = &device->dev_attr;
+	ib_dev = device->ib_device;
 
 	memset(&init_attr, 0, sizeof init_attr);
 
@@ -503,16 +468,16 @@ static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 		iser_conn->max_cmds =
 			ISER_GET_MAX_XMIT_CMDS(ISER_QP_SIG_MAX_REQ_DTOS);
 	} else {
-		if (dev_attr->max_qp_wr > ISER_QP_MAX_REQ_DTOS) {
+		if (ib_dev->attrs.max_qp_wr > ISER_QP_MAX_REQ_DTOS) {
 			init_attr.cap.max_send_wr  = ISER_QP_MAX_REQ_DTOS + 1;
 			iser_conn->max_cmds =
 				ISER_GET_MAX_XMIT_CMDS(ISER_QP_MAX_REQ_DTOS);
 		} else {
-			init_attr.cap.max_send_wr = dev_attr->max_qp_wr;
+			init_attr.cap.max_send_wr = ib_dev->attrs.max_qp_wr;
 			iser_conn->max_cmds =
-				ISER_GET_MAX_XMIT_CMDS(dev_attr->max_qp_wr);
+				ISER_GET_MAX_XMIT_CMDS(ib_dev->attrs.max_qp_wr);
 			iser_dbg("device %s supports max_send_wr %d\n",
-				 device->ib_device->name, dev_attr->max_qp_wr);
+				 device->ib_device->name, ib_dev->attrs.max_qp_wr);
 		}
 	}
 
@@ -724,13 +689,13 @@ int iser_conn_terminate(struct iser_conn *iser_conn)
 				 iser_conn, err);
 
 		/* post an indication that all flush errors were consumed */
-		err = ib_post_send(ib_conn->qp, &ib_conn->beacon, &bad_wr);
+		err = ib_post_send(ib_conn->qp, &ib_conn->last, &bad_wr);
 		if (err) {
-			iser_err("conn %p failed to post beacon", ib_conn);
+			iser_err("conn %p failed to post last wr", ib_conn);
 			return 1;
 		}
 
-		wait_for_completion(&ib_conn->flush_comp);
+		wait_for_completion(&ib_conn->last_comp);
 	}
 
 	return 1;
@@ -756,7 +721,7 @@ iser_calc_scsi_params(struct iser_conn *iser_conn,
 
 	sg_tablesize = DIV_ROUND_UP(max_sectors * 512, SIZE_4K);
 	sup_sg_tablesize = min_t(unsigned, ISCSI_ISER_MAX_SG_TABLESIZE,
-				 device->dev_attr.max_fast_reg_page_list_len);
+				 device->ib_device->attrs.max_fast_reg_page_list_len);
 
 	if (sg_tablesize > sup_sg_tablesize) {
 		sg_tablesize = sup_sg_tablesize;
@@ -799,7 +764,7 @@ static void iser_addr_handler(struct rdma_cm_id *cma_id)
 
 	/* connection T10-PI support */
 	if (iser_pi_enable) {
-		if (!(device->dev_attr.device_cap_flags &
+		if (!(device->ib_device->attrs.device_cap_flags &
 		      IB_DEVICE_SIGNATURE_HANDOVER)) {
 			iser_warn("T10-PI requested but not supported on %s, "
 				  "continue without T10-PI\n",
@@ -841,16 +806,17 @@ static void iser_route_handler(struct rdma_cm_id *cma_id)
 		goto failure;
 
 	memset(&conn_param, 0, sizeof conn_param);
-	conn_param.responder_resources = device->dev_attr.max_qp_rd_atom;
+	conn_param.responder_resources = device->ib_device->attrs.max_qp_rd_atom;
 	conn_param.initiator_depth     = 1;
 	conn_param.retry_count	       = 7;
 	conn_param.rnr_retry_count     = 6;
 
 	memset(&req_hdr, 0, sizeof(req_hdr));
-	req_hdr.flags = (ISER_ZBVA_NOT_SUPPORTED |
-			ISER_SEND_W_INV_NOT_SUPPORTED);
-	conn_param.private_data		= (void *)&req_hdr;
-	conn_param.private_data_len	= sizeof(struct iser_cm_hdr);
+	req_hdr.flags = ISER_ZBVA_NOT_SUP;
+	if (!device->remote_inv_sup)
+		req_hdr.flags |= ISER_SEND_W_INV_NOT_SUP;
+	conn_param.private_data	= (void *)&req_hdr;
+	conn_param.private_data_len = sizeof(struct iser_cm_hdr);
 
 	ret = rdma_connect(cma_id, &conn_param);
 	if (ret) {
@@ -863,7 +829,8 @@ failure:
 	iser_connect_error(cma_id);
 }
 
-static void iser_connected_handler(struct rdma_cm_id *cma_id)
+static void iser_connected_handler(struct rdma_cm_id *cma_id,
+				   const void *private_data)
 {
 	struct iser_conn *iser_conn;
 	struct ib_qp_attr attr;
@@ -877,6 +844,15 @@ static void iser_connected_handler(struct rdma_cm_id *cma_id)
 	(void)ib_query_qp(cma_id->qp, &attr, ~0, &init_attr);
 	iser_info("remote qpn:%x my qpn:%x\n", attr.dest_qp_num, cma_id->qp->qp_num);
 
+	if (private_data) {
+		u8 flags = *(u8 *)private_data;
+
+		iser_conn->snd_w_inv = !(flags & ISER_SEND_W_INV_NOT_SUP);
+	}
+
+	iser_info("conn %p: negotiated %s invalidation\n",
+		  iser_conn, iser_conn->snd_w_inv ? "remote" : "local");
+
 	iser_conn->state = ISER_CONN_UP;
 	complete(&iser_conn->up_completion);
 }
@@ -928,7 +904,7 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 		iser_route_handler(cma_id);
 		break;
 	case RDMA_CM_EVENT_ESTABLISHED:
-		iser_connected_handler(cma_id);
+		iser_connected_handler(cma_id, event->param.conn.private_data);
 		break;
 	case RDMA_CM_EVENT_ADDR_ERROR:
 	case RDMA_CM_EVENT_ROUTE_ERROR:
@@ -967,14 +943,21 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 
 void iser_conn_init(struct iser_conn *iser_conn)
 {
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
+
 	iser_conn->state = ISER_CONN_INIT;
-	iser_conn->ib_conn.post_recv_buf_count = 0;
-	init_completion(&iser_conn->ib_conn.flush_comp);
 	init_completion(&iser_conn->stop_completion);
 	init_completion(&iser_conn->ib_completion);
 	init_completion(&iser_conn->up_completion);
 	INIT_LIST_HEAD(&iser_conn->conn_list);
 	mutex_init(&iser_conn->state_mutex);
+
+	ib_conn->post_recv_buf_count = 0;
+	ib_conn->reg_cqe.done = iser_reg_comp;
+	ib_conn->last_cqe.done = iser_last_comp;
+	ib_conn->last.wr_cqe = &ib_conn->last_cqe;
+	ib_conn->last.opcode = IB_WR_SEND;
+	init_completion(&ib_conn->last_comp);
 }
 
  /**
@@ -1000,9 +983,6 @@ int iser_connect(struct iser_conn   *iser_conn,
 
 	iser_conn->state = ISER_CONN_PENDING;
 
-	ib_conn->beacon.wr_id = ISER_BEACON_WRID;
-	ib_conn->beacon.opcode = IB_WR_SEND;
-
 	ib_conn->cma_id = rdma_create_id(&init_net, iser_cma_handler,
 					 (void *)iser_conn,
 					 RDMA_PS_TCP, IB_QPT_RC);
@@ -1045,56 +1025,60 @@ connect_failure:
 
 int iser_post_recvl(struct iser_conn *iser_conn)
 {
-	struct ib_recv_wr rx_wr, *rx_wr_failed;
 	struct ib_conn *ib_conn = &iser_conn->ib_conn;
-	struct ib_sge	  sge;
+	struct iser_login_desc *desc = &iser_conn->login_desc;
+	struct ib_recv_wr wr, *wr_failed;
 	int ib_ret;
 
-	sge.addr   = iser_conn->login_resp_dma;
-	sge.length = ISER_RX_LOGIN_SIZE;
-	sge.lkey   = ib_conn->device->pd->local_dma_lkey;
+	desc->sge.addr = desc->rsp_dma;
+	desc->sge.length = ISER_RX_LOGIN_SIZE;
+	desc->sge.lkey = ib_conn->device->pd->local_dma_lkey;
 
-	rx_wr.wr_id   = (uintptr_t)iser_conn->login_resp_buf;
-	rx_wr.sg_list = &sge;
-	rx_wr.num_sge = 1;
-	rx_wr.next    = NULL;
+	desc->cqe.done = iser_login_rsp;
+	wr.wr_cqe = &desc->cqe;
+	wr.sg_list = &desc->sge;
+	wr.num_sge = 1;
+	wr.next = NULL;
 
 	ib_conn->post_recv_buf_count++;
-	ib_ret	= ib_post_recv(ib_conn->qp, &rx_wr, &rx_wr_failed);
+	ib_ret = ib_post_recv(ib_conn->qp, &wr, &wr_failed);
 	if (ib_ret) {
 		iser_err("ib_post_recv failed ret=%d\n", ib_ret);
 		ib_conn->post_recv_buf_count--;
 	}
+
 	return ib_ret;
 }
 
 int iser_post_recvm(struct iser_conn *iser_conn, int count)
 {
-	struct ib_recv_wr *rx_wr, *rx_wr_failed;
-	int i, ib_ret;
 	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	unsigned int my_rx_head = iser_conn->rx_desc_head;
 	struct iser_rx_desc *rx_desc;
+	struct ib_recv_wr *wr, *wr_failed;
+	int i, ib_ret;
 
-	for (rx_wr = ib_conn->rx_wr, i = 0; i < count; i++, rx_wr++) {
-		rx_desc		= &iser_conn->rx_descs[my_rx_head];
-		rx_wr->wr_id	= (uintptr_t)rx_desc;
-		rx_wr->sg_list	= &rx_desc->rx_sg;
-		rx_wr->num_sge	= 1;
-		rx_wr->next	= rx_wr + 1;
+	for (wr = ib_conn->rx_wr, i = 0; i < count; i++, wr++) {
+		rx_desc = &iser_conn->rx_descs[my_rx_head];
+		rx_desc->cqe.done = iser_task_rsp;
+		wr->wr_cqe = &rx_desc->cqe;
+		wr->sg_list = &rx_desc->rx_sg;
+		wr->num_sge = 1;
+		wr->next = wr + 1;
 		my_rx_head = (my_rx_head + 1) & iser_conn->qp_max_recv_dtos_mask;
 	}
 
-	rx_wr--;
-	rx_wr->next = NULL; /* mark end of work requests list */
+	wr--;
+	wr->next = NULL; /* mark end of work requests list */
 
 	ib_conn->post_recv_buf_count += count;
-	ib_ret	= ib_post_recv(ib_conn->qp, ib_conn->rx_wr, &rx_wr_failed);
+	ib_ret = ib_post_recv(ib_conn->qp, ib_conn->rx_wr, &wr_failed);
 	if (ib_ret) {
 		iser_err("ib_post_recv failed ret=%d\n", ib_ret);
 		ib_conn->post_recv_buf_count -= count;
 	} else
 		iser_conn->rx_desc_head = my_rx_head;
+
 	return ib_ret;
 }
 
@@ -1115,7 +1099,7 @@ int iser_post_send(struct ib_conn *ib_conn, struct iser_tx_desc *tx_desc,
 				      DMA_TO_DEVICE);
 
 	wr->next = NULL;
-	wr->wr_id = (uintptr_t)tx_desc;
+	wr->wr_cqe = &tx_desc->cqe;
 	wr->sg_list = tx_desc->tx_sg;
 	wr->num_sge = tx_desc->num_sge;
 	wr->opcode = IB_WR_SEND;
@@ -1129,149 +1113,6 @@ int iser_post_send(struct ib_conn *ib_conn, struct iser_tx_desc *tx_desc,
 	return ib_ret;
 }
 
-/**
- * is_iser_tx_desc - Indicate if the completion wr_id
- *     is a TX descriptor or not.
- * @iser_conn: iser connection
- * @wr_id: completion WR identifier
- *
- * Since we cannot rely on wc opcode in FLUSH errors
- * we must work around it by checking if the wr_id address
- * falls in the iser connection rx_descs buffer. If so
- * it is an RX descriptor, otherwize it is a TX.
- */
-static inline bool
-is_iser_tx_desc(struct iser_conn *iser_conn, void *wr_id)
-{
-	void *start = iser_conn->rx_descs;
-	int len = iser_conn->num_rx_descs * sizeof(*iser_conn->rx_descs);
-
-	if (wr_id >= start && wr_id < start + len)
-		return false;
-
-	return true;
-}
-
-/**
- * iser_handle_comp_error() - Handle error completion
- * @ib_conn:   connection RDMA resources
- * @wc:        work completion
- *
- * Notes: We may handle a FLUSH error completion and in this case
- *        we only cleanup in case TX type was DATAOUT. For non-FLUSH
- *        error completion we should also notify iscsi layer that
- *        connection is failed (in case we passed bind stage).
- */
-static void
-iser_handle_comp_error(struct ib_conn *ib_conn,
-		       struct ib_wc *wc)
-{
-	void *wr_id = (void *)(uintptr_t)wc->wr_id;
-	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
-						   ib_conn);
-
-	if (wc->status != IB_WC_WR_FLUSH_ERR)
-		if (iser_conn->iscsi_conn)
-			iscsi_conn_failure(iser_conn->iscsi_conn,
-					   ISCSI_ERR_CONN_FAILED);
-
-	if (wc->wr_id == ISER_FASTREG_LI_WRID)
-		return;
-
-	if (is_iser_tx_desc(iser_conn, wr_id)) {
-		struct iser_tx_desc *desc = wr_id;
-
-		if (desc->type == ISCSI_TX_DATAOUT)
-			kmem_cache_free(ig.desc_cache, desc);
-	} else {
-		ib_conn->post_recv_buf_count--;
-	}
-}
-
-/**
- * iser_handle_wc - handle a single work completion
- * @wc: work completion
- *
- * Soft-IRQ context, work completion can be either
- * SEND or RECV, and can turn out successful or
- * with error (or flush error).
- */
-static void iser_handle_wc(struct ib_wc *wc)
-{
-	struct ib_conn *ib_conn;
-	struct iser_tx_desc *tx_desc;
-	struct iser_rx_desc *rx_desc;
-
-	ib_conn = wc->qp->qp_context;
-	if (likely(wc->status == IB_WC_SUCCESS)) {
-		if (wc->opcode == IB_WC_RECV) {
-			rx_desc = (struct iser_rx_desc *)(uintptr_t)wc->wr_id;
-			iser_rcv_completion(rx_desc, wc->byte_len,
-					    ib_conn);
-		} else
-		if (wc->opcode == IB_WC_SEND) {
-			tx_desc = (struct iser_tx_desc *)(uintptr_t)wc->wr_id;
-			iser_snd_completion(tx_desc, ib_conn);
-		} else {
-			iser_err("Unknown wc opcode %d\n", wc->opcode);
-		}
-	} else {
-		if (wc->status != IB_WC_WR_FLUSH_ERR)
-			iser_err("%s (%d): wr id %llx vend_err %x\n",
-				 ib_wc_status_msg(wc->status), wc->status,
-				 wc->wr_id, wc->vendor_err);
-		else
-			iser_dbg("%s (%d): wr id %llx\n",
-				 ib_wc_status_msg(wc->status), wc->status,
-				 wc->wr_id);
-
-		if (wc->wr_id == ISER_BEACON_WRID)
-			/* all flush errors were consumed */
-			complete(&ib_conn->flush_comp);
-		else
-			iser_handle_comp_error(ib_conn, wc);
-	}
-}
-
-/**
- * iser_cq_tasklet_fn - iSER completion polling loop
- * @data: iSER completion context
- *
- * Soft-IRQ context, polling connection CQ until
- * either CQ was empty or we exausted polling budget
- */
-static void iser_cq_tasklet_fn(unsigned long data)
-{
-	struct iser_comp *comp = (struct iser_comp *)data;
-	struct ib_cq *cq = comp->cq;
-	struct ib_wc *const wcs = comp->wcs;
-	int i, n, completed = 0;
-
-	while ((n = ib_poll_cq(cq, ARRAY_SIZE(comp->wcs), wcs)) > 0) {
-		for (i = 0; i < n; i++)
-			iser_handle_wc(&wcs[i]);
-
-		completed += n;
-		if (completed >= iser_cq_poll_limit)
-			break;
-	}
-
-	/*
-	 * It is assumed here that arming CQ only once its empty
-	 * would not cause interrupts to be missed.
-	 */
-	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
-
-	iser_dbg("got %d completions\n", completed);
-}
-
-static void iser_cq_callback(struct ib_cq *cq, void *cq_context)
-{
-	struct iser_comp *comp = cq_context;
-
-	tasklet_schedule(&comp->tasklet);
-}
-
 u8 iser_check_task_pi_status(struct iscsi_iser_task *iser_task,
 			     enum iser_data_dir cmd_dir, sector_t *sector)
 {
@@ -1319,3 +1160,21 @@ err:
 	/* Not alot we can do here, return ambiguous guard error */
 	return 0x1;
 }
+
+void iser_err_comp(struct ib_wc *wc, const char *type)
+{
+	if (wc->status != IB_WC_WR_FLUSH_ERR) {
+		struct iser_conn *iser_conn = to_iser_conn(wc->qp->qp_context);
+
+		iser_err("%s failure: %s (%d) vend_err %x\n", type,
+			 ib_wc_status_msg(wc->status), wc->status,
+			 wc->vendor_err);
+
+		if (iser_conn->iscsi_conn)
+			iscsi_conn_failure(iser_conn->iscsi_conn,
+					   ISCSI_ERR_CONN_FAILED);
+	} else {
+		iser_dbg("%s failure: %s (%d)\n", type,
+			 ib_wc_status_msg(wc->status), wc->status);
+	}
+}
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index 8a51c3b5d657..f121e6129339 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -29,7 +29,6 @@
 #include <target/iscsi/iscsi_transport.h>
 #include <linux/semaphore.h>
 
-#include "isert_proto.h"
 #include "ib_isert.h"
 
 #define	ISERT_MAX_CONN		8
@@ -95,22 +94,6 @@ isert_qp_event_callback(struct ib_event *e, void *context)
 	}
 }
 
-static int
-isert_query_device(struct ib_device *ib_dev, struct ib_device_attr *devattr)
-{
-	int ret;
-
-	ret = ib_query_device(ib_dev, devattr);
-	if (ret) {
-		isert_err("ib_query_device() failed: %d\n", ret);
-		return ret;
-	}
-	isert_dbg("devattr->max_sge: %d\n", devattr->max_sge);
-	isert_dbg("devattr->max_sge_rd: %d\n", devattr->max_sge_rd);
-
-	return 0;
-}
-
 static struct isert_comp *
 isert_comp_get(struct isert_conn *isert_conn)
 {
@@ -157,9 +140,9 @@ isert_create_qp(struct isert_conn *isert_conn,
 	attr.recv_cq = comp->cq;
 	attr.cap.max_send_wr = ISERT_QP_MAX_REQ_DTOS;
 	attr.cap.max_recv_wr = ISERT_QP_MAX_RECV_DTOS + 1;
-	attr.cap.max_send_sge = device->dev_attr.max_sge;
-	isert_conn->max_sge = min(device->dev_attr.max_sge,
-				  device->dev_attr.max_sge_rd);
+	attr.cap.max_send_sge = device->ib_device->attrs.max_sge;
+	isert_conn->max_sge = min(device->ib_device->attrs.max_sge,
+				  device->ib_device->attrs.max_sge_rd);
 	attr.cap.max_recv_sge = 1;
 	attr.sq_sig_type = IB_SIGNAL_REQ_WR;
 	attr.qp_type = IB_QPT_RC;
@@ -287,8 +270,7 @@ isert_free_comps(struct isert_device *device)
 }
 
 static int
-isert_alloc_comps(struct isert_device *device,
-		  struct ib_device_attr *attr)
+isert_alloc_comps(struct isert_device *device)
 {
 	int i, max_cqe, ret = 0;
 
@@ -308,7 +290,7 @@ isert_alloc_comps(struct isert_device *device,
 		return -ENOMEM;
 	}
 
-	max_cqe = min(ISER_MAX_CQ_LEN, attr->max_cqe);
+	max_cqe = min(ISER_MAX_CQ_LEN, device->ib_device->attrs.max_cqe);
 
 	for (i = 0; i < device->comps_used; i++) {
 		struct ib_cq_init_attr cq_attr = {};
@@ -344,17 +326,15 @@ out_cq:
 static int
 isert_create_device_ib_res(struct isert_device *device)
 {
-	struct ib_device_attr *dev_attr;
+	struct ib_device *ib_dev = device->ib_device;
 	int ret;
 
-	dev_attr = &device->dev_attr;
-	ret = isert_query_device(device->ib_device, dev_attr);
-	if (ret)
-		return ret;
+	isert_dbg("devattr->max_sge: %d\n", ib_dev->attrs.max_sge);
+	isert_dbg("devattr->max_sge_rd: %d\n", ib_dev->attrs.max_sge_rd);
 
 	/* asign function handlers */
-	if (dev_attr->device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS &&
-	    dev_attr->device_cap_flags & IB_DEVICE_SIGNATURE_HANDOVER) {
+	if (ib_dev->attrs.device_cap_flags & IB_DEVICE_MEM_MGT_EXTENSIONS &&
+	    ib_dev->attrs.device_cap_flags & IB_DEVICE_SIGNATURE_HANDOVER) {
 		device->use_fastreg = 1;
 		device->reg_rdma_mem = isert_reg_rdma;
 		device->unreg_rdma_mem = isert_unreg_rdma;
@@ -364,11 +344,11 @@ isert_create_device_ib_res(struct isert_device *device)
 		device->unreg_rdma_mem = isert_unmap_cmd;
 	}
 
-	ret = isert_alloc_comps(device, dev_attr);
+	ret = isert_alloc_comps(device);
 	if (ret)
-		return ret;
+		goto out;
 
-	device->pd = ib_alloc_pd(device->ib_device);
+	device->pd = ib_alloc_pd(ib_dev);
 	if (IS_ERR(device->pd)) {
 		ret = PTR_ERR(device->pd);
 		isert_err("failed to allocate pd, device %p, ret=%d\n",
@@ -377,13 +357,16 @@ isert_create_device_ib_res(struct isert_device *device)
 	}
 
 	/* Check signature cap */
-	device->pi_capable = dev_attr->device_cap_flags &
+	device->pi_capable = ib_dev->attrs.device_cap_flags &
 			     IB_DEVICE_SIGNATURE_HANDOVER ? true : false;
 
 	return 0;
 
 out_cq:
 	isert_free_comps(device);
+out:
+	if (ret > 0)
+		ret = -EINVAL;
 	return ret;
 }
 
@@ -673,6 +656,32 @@ out_login_buf:
 	return ret;
 }
 
+static void
+isert_set_nego_params(struct isert_conn *isert_conn,
+		      struct rdma_conn_param *param)
+{
+	struct ib_device_attr *attr = &isert_conn->device->ib_device->attrs;
+
+	/* Set max inflight RDMA READ requests */
+	isert_conn->initiator_depth = min_t(u8, param->initiator_depth,
+				attr->max_qp_init_rd_atom);
+	isert_dbg("Using initiator_depth: %u\n", isert_conn->initiator_depth);
+
+	if (param->private_data) {
+		u8 flags = *(u8 *)param->private_data;
+
+		/*
+		 * use remote invalidation if the both initiator
+		 * and the HCA support it
+		 */
+		isert_conn->snd_w_inv = !(flags & ISER_SEND_W_INV_NOT_SUP) &&
+					  (attr->device_cap_flags &
+					   IB_DEVICE_MEM_MGT_EXTENSIONS);
+		if (isert_conn->snd_w_inv)
+			isert_info("Using remote invalidation\n");
+	}
+}
+
 static int
 isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
@@ -711,11 +720,7 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	}
 	isert_conn->device = device;
 
-	/* Set max inflight RDMA READ requests */
-	isert_conn->initiator_depth = min_t(u8,
-				event->param.conn.initiator_depth,
-				device->dev_attr.max_qp_init_rd_atom);
-	isert_dbg("Using initiator_depth: %u\n", isert_conn->initiator_depth);
+	isert_set_nego_params(isert_conn, &event->param.conn);
 
 	ret = isert_conn_setup_qp(isert_conn, cma_id);
 	if (ret)
@@ -1047,8 +1052,8 @@ isert_create_send_desc(struct isert_conn *isert_conn,
 	ib_dma_sync_single_for_cpu(ib_dev, tx_desc->dma_addr,
 				   ISER_HEADERS_LEN, DMA_TO_DEVICE);
 
-	memset(&tx_desc->iser_header, 0, sizeof(struct iser_hdr));
-	tx_desc->iser_header.flags = ISER_VER;
+	memset(&tx_desc->iser_header, 0, sizeof(struct iser_ctrl));
+	tx_desc->iser_header.flags = ISCSI_CTRL;
 
 	tx_desc->num_sge = 1;
 	tx_desc->isert_cmd = isert_cmd;
@@ -1094,7 +1099,14 @@ isert_init_send_wr(struct isert_conn *isert_conn, struct isert_cmd *isert_cmd,
 
 	isert_cmd->rdma_wr.iser_ib_op = ISER_IB_SEND;
 	send_wr->wr_id = (uintptr_t)&isert_cmd->tx_desc;
-	send_wr->opcode = IB_WR_SEND;
+
+	if (isert_conn->snd_w_inv && isert_cmd->inv_rkey) {
+		send_wr->opcode  = IB_WR_SEND_WITH_INV;
+		send_wr->ex.invalidate_rkey = isert_cmd->inv_rkey;
+	} else {
+		send_wr->opcode = IB_WR_SEND;
+	}
+
 	send_wr->sg_list = &tx_desc->tx_sg[0];
 	send_wr->num_sge = isert_cmd->tx_desc.num_sge;
 	send_wr->send_flags = IB_SEND_SIGNALED;
@@ -1483,6 +1495,7 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 		isert_cmd->read_va = read_va;
 		isert_cmd->write_stag = write_stag;
 		isert_cmd->write_va = write_va;
+		isert_cmd->inv_rkey = read_stag ? read_stag : write_stag;
 
 		ret = isert_handle_scsi_cmd(isert_conn, isert_cmd, cmd,
 					rx_desc, (unsigned char *)hdr);
@@ -1540,21 +1553,21 @@ isert_rx_opcode(struct isert_conn *isert_conn, struct iser_rx_desc *rx_desc,
 static void
 isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
 {
-	struct iser_hdr *iser_hdr = &rx_desc->iser_header;
+	struct iser_ctrl *iser_ctrl = &rx_desc->iser_header;
 	uint64_t read_va = 0, write_va = 0;
 	uint32_t read_stag = 0, write_stag = 0;
 
-	switch (iser_hdr->flags & 0xF0) {
+	switch (iser_ctrl->flags & 0xF0) {
 	case ISCSI_CTRL:
-		if (iser_hdr->flags & ISER_RSV) {
-			read_stag = be32_to_cpu(iser_hdr->read_stag);
-			read_va = be64_to_cpu(iser_hdr->read_va);
+		if (iser_ctrl->flags & ISER_RSV) {
+			read_stag = be32_to_cpu(iser_ctrl->read_stag);
+			read_va = be64_to_cpu(iser_ctrl->read_va);
 			isert_dbg("ISER_RSV: read_stag: 0x%x read_va: 0x%llx\n",
 				  read_stag, (unsigned long long)read_va);
 		}
-		if (iser_hdr->flags & ISER_WSV) {
-			write_stag = be32_to_cpu(iser_hdr->write_stag);
-			write_va = be64_to_cpu(iser_hdr->write_va);
+		if (iser_ctrl->flags & ISER_WSV) {
+			write_stag = be32_to_cpu(iser_ctrl->write_stag);
+			write_va = be64_to_cpu(iser_ctrl->write_va);
 			isert_dbg("ISER_WSV: write_stag: 0x%x write_va: 0x%llx\n",
 				  write_stag, (unsigned long long)write_va);
 		}
@@ -1565,7 +1578,7 @@ isert_rx_do_work(struct iser_rx_desc *rx_desc, struct isert_conn *isert_conn)
 		isert_err("iSER Hello message\n");
 		break;
 	default:
-		isert_warn("Unknown iSER hdr flags: 0x%02x\n", iser_hdr->flags);
+		isert_warn("Unknown iSER hdr flags: 0x%02x\n", iser_ctrl->flags);
 		break;
 	}
 
@@ -3092,12 +3105,20 @@ isert_rdma_accept(struct isert_conn *isert_conn)
 	struct rdma_cm_id *cm_id = isert_conn->cm_id;
 	struct rdma_conn_param cp;
 	int ret;
+	struct iser_cm_hdr rsp_hdr;
 
 	memset(&cp, 0, sizeof(struct rdma_conn_param));
 	cp.initiator_depth = isert_conn->initiator_depth;
 	cp.retry_count = 7;
 	cp.rnr_retry_count = 7;
 
+	memset(&rsp_hdr, 0, sizeof(rsp_hdr));
+	rsp_hdr.flags = ISERT_ZBVA_NOT_USED;
+	if (!isert_conn->snd_w_inv)
+		rsp_hdr.flags = rsp_hdr.flags | ISERT_SEND_W_INV_NOT_USED;
+	cp.private_data = (void *)&rsp_hdr;
+	cp.private_data_len = sizeof(rsp_hdr);
+
 	ret = rdma_accept(cm_id, &cp);
 	if (ret) {
 		isert_err("rdma_accept() failed with: %d\n", ret);
diff --git a/drivers/infiniband/ulp/isert/ib_isert.h b/drivers/infiniband/ulp/isert/ib_isert.h
index 3d7fbc47c343..8d50453eef66 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.h
+++ b/drivers/infiniband/ulp/isert/ib_isert.h
@@ -3,6 +3,8 @@
 #include <linux/in6.h>
 #include <rdma/ib_verbs.h>
 #include <rdma/rdma_cm.h>
+#include <scsi/iser.h>
+
 
 #define DRV_NAME	"isert"
 #define PFX		DRV_NAME ": "
@@ -31,6 +33,38 @@
 #define isert_err(fmt, arg...) \
 	pr_err(PFX "%s: " fmt, __func__ , ## arg)
 
+/* Constant PDU lengths calculations */
+#define ISER_HEADERS_LEN	(sizeof(struct iser_ctrl) + \
+				 sizeof(struct iscsi_hdr))
+#define ISER_RECV_DATA_SEG_LEN	8192
+#define ISER_RX_PAYLOAD_SIZE	(ISER_HEADERS_LEN + ISER_RECV_DATA_SEG_LEN)
+#define ISER_RX_LOGIN_SIZE	(ISER_HEADERS_LEN + ISCSI_DEF_MAX_RECV_SEG_LEN)
+
+/* QP settings */
+/* Maximal bounds on received asynchronous PDUs */
+#define ISERT_MAX_TX_MISC_PDUS	4 /* NOOP_IN(2) , ASYNC_EVENT(2)   */
+
+#define ISERT_MAX_RX_MISC_PDUS	6 /*
+				   * NOOP_OUT(2), TEXT(1),
+				   * SCSI_TMFUNC(2), LOGOUT(1)
+				   */
+
+#define ISCSI_DEF_XMIT_CMDS_MAX 128 /* from libiscsi.h, must be power of 2 */
+
+#define ISERT_QP_MAX_RECV_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX)
+
+#define ISERT_MIN_POSTED_RX	(ISCSI_DEF_XMIT_CMDS_MAX >> 2)
+
+#define ISERT_INFLIGHT_DATAOUTS	8
+
+#define ISERT_QP_MAX_REQ_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX *    \
+				(1 + ISERT_INFLIGHT_DATAOUTS) + \
+				ISERT_MAX_TX_MISC_PDUS	+ \
+				ISERT_MAX_RX_MISC_PDUS)
+
+#define ISER_RX_PAD_SIZE	(ISER_RECV_DATA_SEG_LEN + 4096 - \
+		(ISER_RX_PAYLOAD_SIZE + sizeof(u64) + sizeof(struct ib_sge)))
+
 #define ISCSI_ISER_SG_TABLESIZE		256
 #define ISER_FASTREG_LI_WRID		0xffffffffffffffffULL
 #define ISER_BEACON_WRID               0xfffffffffffffffeULL
@@ -56,7 +90,7 @@ enum iser_conn_state {
 };
 
 struct iser_rx_desc {
-	struct iser_hdr iser_header;
+	struct iser_ctrl iser_header;
 	struct iscsi_hdr iscsi_header;
 	char		data[ISER_RECV_DATA_SEG_LEN];
 	u64		dma_addr;
@@ -65,7 +99,7 @@ struct iser_rx_desc {
 } __packed;
 
 struct iser_tx_desc {
-	struct iser_hdr iser_header;
+	struct iser_ctrl iser_header;
 	struct iscsi_hdr iscsi_header;
 	enum isert_desc_type type;
 	u64		dma_addr;
@@ -129,6 +163,7 @@ struct isert_cmd {
 	uint32_t		write_stag;
 	uint64_t		read_va;
 	uint64_t		write_va;
+	uint32_t		inv_rkey;
 	u64			pdu_buf_dma;
 	u32			pdu_buf_len;
 	struct isert_conn	*conn;
@@ -176,6 +211,7 @@ struct isert_conn {
 	struct work_struct	release_work;
 	struct ib_recv_wr       beacon;
 	bool                    logout_posted;
+	bool                    snd_w_inv;
 };
 
 #define ISERT_MAX_CQ 64
@@ -207,7 +243,6 @@ struct isert_device {
 	struct isert_comp	*comps;
 	int                     comps_used;
 	struct list_head	dev_node;
-	struct ib_device_attr	dev_attr;
 	int			(*reg_rdma_mem)(struct iscsi_conn *conn,
 						    struct iscsi_cmd *cmd,
 						    struct isert_rdma_wr *wr);
diff --git a/drivers/infiniband/ulp/isert/isert_proto.h b/drivers/infiniband/ulp/isert/isert_proto.h
deleted file mode 100644
index 4dccd313b777..000000000000
--- a/drivers/infiniband/ulp/isert/isert_proto.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/* From iscsi_iser.h */
-
-struct iser_hdr {
-	u8	flags;
-	u8	rsvd[3];
-	__be32	write_stag; /* write rkey */
-	__be64	write_va;
-	__be32	read_stag;  /* read rkey */
-	__be64	read_va;
-} __packed;
-
-/*Constant PDU lengths calculations */
-#define ISER_HEADERS_LEN  (sizeof(struct iser_hdr) + sizeof(struct iscsi_hdr))
-
-#define ISER_RECV_DATA_SEG_LEN  8192
-#define ISER_RX_PAYLOAD_SIZE    (ISER_HEADERS_LEN + ISER_RECV_DATA_SEG_LEN)
-#define ISER_RX_LOGIN_SIZE      (ISER_HEADERS_LEN + ISCSI_DEF_MAX_RECV_SEG_LEN)
-
-/* QP settings */
-/* Maximal bounds on received asynchronous PDUs */
-#define ISERT_MAX_TX_MISC_PDUS	4 /* NOOP_IN(2) , ASYNC_EVENT(2)   */
-
-#define ISERT_MAX_RX_MISC_PDUS	6 /* NOOP_OUT(2), TEXT(1),         *
-				   * SCSI_TMFUNC(2), LOGOUT(1) */
-
-#define ISCSI_DEF_XMIT_CMDS_MAX 128 /* from libiscsi.h, must be power of 2 */
-
-#define ISERT_QP_MAX_RECV_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX)
-
-#define ISERT_MIN_POSTED_RX	(ISCSI_DEF_XMIT_CMDS_MAX >> 2)
-
-#define ISERT_INFLIGHT_DATAOUTS	8
-
-#define ISERT_QP_MAX_REQ_DTOS	(ISCSI_DEF_XMIT_CMDS_MAX *    \
-				(1 + ISERT_INFLIGHT_DATAOUTS) + \
-				ISERT_MAX_TX_MISC_PDUS	+ \
-				ISERT_MAX_RX_MISC_PDUS)
-
-#define ISER_RX_PAD_SIZE	(ISER_RECV_DATA_SEG_LEN + 4096 - \
-		(ISER_RX_PAYLOAD_SIZE + sizeof(u64) + sizeof(struct ib_sge)))
-
-#define ISER_VER	0x10
-#define ISER_WSV	0x08
-#define ISER_RSV	0x04
-#define ISCSI_CTRL	0x10
-#define ISER_HELLO	0x20
-#define ISER_HELLORPLY	0x30
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 3db9a659719b..03022f6420d7 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -132,8 +132,9 @@ MODULE_PARM_DESC(ch_count,
 
 static void srp_add_one(struct ib_device *device);
 static void srp_remove_one(struct ib_device *device, void *client_data);
-static void srp_recv_completion(struct ib_cq *cq, void *ch_ptr);
-static void srp_send_completion(struct ib_cq *cq, void *ch_ptr);
+static void srp_recv_done(struct ib_cq *cq, struct ib_wc *wc);
+static void srp_handle_qp_err(struct ib_cq *cq, struct ib_wc *wc,
+		const char *opname);
 static int srp_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event);
 
 static struct scsi_transport_template *ib_srp_transport_template;
@@ -445,6 +446,17 @@ static struct srp_fr_pool *srp_alloc_fr_pool(struct srp_target_port *target)
 				  dev->max_pages_per_mr);
 }
 
+static void srp_drain_done(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct srp_rdma_ch *ch = cq->cq_context;
+
+	complete(&ch->done);
+}
+
+static struct ib_cqe srp_drain_cqe = {
+	.done		= srp_drain_done,
+};
+
 /**
  * srp_destroy_qp() - destroy an RDMA queue pair
  * @ch: SRP RDMA channel.
@@ -457,10 +469,11 @@ static struct srp_fr_pool *srp_alloc_fr_pool(struct srp_target_port *target)
 static void srp_destroy_qp(struct srp_rdma_ch *ch)
 {
 	static struct ib_qp_attr attr = { .qp_state = IB_QPS_ERR };
-	static struct ib_recv_wr wr = { .wr_id = SRP_LAST_WR_ID };
+	static struct ib_recv_wr wr = { 0 };
 	struct ib_recv_wr *bad_wr;
 	int ret;
 
+	wr.wr_cqe = &srp_drain_cqe;
 	/* Destroying a QP and reusing ch->done is only safe if not connected */
 	WARN_ON_ONCE(ch->connected);
 
@@ -489,34 +502,27 @@ static int srp_create_ch_ib(struct srp_rdma_ch *ch)
 	struct ib_fmr_pool *fmr_pool = NULL;
 	struct srp_fr_pool *fr_pool = NULL;
 	const int m = dev->use_fast_reg ? 3 : 1;
-	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	init_attr = kzalloc(sizeof *init_attr, GFP_KERNEL);
 	if (!init_attr)
 		return -ENOMEM;
 
-	/* + 1 for SRP_LAST_WR_ID */
-	cq_attr.cqe = target->queue_size + 1;
-	cq_attr.comp_vector = ch->comp_vector;
-	recv_cq = ib_create_cq(dev->dev, srp_recv_completion, NULL, ch,
-			       &cq_attr);
+	/* queue_size + 1 for ib_drain_qp */
+	recv_cq = ib_alloc_cq(dev->dev, ch, target->queue_size + 1,
+				ch->comp_vector, IB_POLL_SOFTIRQ);
 	if (IS_ERR(recv_cq)) {
 		ret = PTR_ERR(recv_cq);
 		goto err;
 	}
 
-	cq_attr.cqe = m * target->queue_size;
-	cq_attr.comp_vector = ch->comp_vector;
-	send_cq = ib_create_cq(dev->dev, srp_send_completion, NULL, ch,
-			       &cq_attr);
+	send_cq = ib_alloc_cq(dev->dev, ch, m * target->queue_size,
+				ch->comp_vector, IB_POLL_DIRECT);
 	if (IS_ERR(send_cq)) {
 		ret = PTR_ERR(send_cq);
 		goto err_recv_cq;
 	}
 
-	ib_req_notify_cq(recv_cq, IB_CQ_NEXT_COMP);
-
 	init_attr->event_handler       = srp_qp_event;
 	init_attr->cap.max_send_wr     = m * target->queue_size;
 	init_attr->cap.max_recv_wr     = target->queue_size + 1;
@@ -558,9 +564,9 @@ static int srp_create_ch_ib(struct srp_rdma_ch *ch)
 	if (ch->qp)
 		srp_destroy_qp(ch);
 	if (ch->recv_cq)
-		ib_destroy_cq(ch->recv_cq);
+		ib_free_cq(ch->recv_cq);
 	if (ch->send_cq)
-		ib_destroy_cq(ch->send_cq);
+		ib_free_cq(ch->send_cq);
 
 	ch->qp = qp;
 	ch->recv_cq = recv_cq;
@@ -580,13 +586,13 @@ static int srp_create_ch_ib(struct srp_rdma_ch *ch)
 	return 0;
 
 err_qp:
-	ib_destroy_qp(qp);
+	srp_destroy_qp(ch);
 
 err_send_cq:
-	ib_destroy_cq(send_cq);
+	ib_free_cq(send_cq);
 
 err_recv_cq:
-	ib_destroy_cq(recv_cq);
+	ib_free_cq(recv_cq);
 
 err:
 	kfree(init_attr);
@@ -622,9 +628,10 @@ static void srp_free_ch_ib(struct srp_target_port *target,
 		if (ch->fmr_pool)
 			ib_destroy_fmr_pool(ch->fmr_pool);
 	}
+
 	srp_destroy_qp(ch);
-	ib_destroy_cq(ch->send_cq);
-	ib_destroy_cq(ch->recv_cq);
+	ib_free_cq(ch->send_cq);
+	ib_free_cq(ch->recv_cq);
 
 	/*
 	 * Avoid that the SCSI error handler tries to use this channel after
@@ -1041,18 +1048,25 @@ out:
 	return ret <= 0 ? ret : -ENODEV;
 }
 
-static int srp_inv_rkey(struct srp_rdma_ch *ch, u32 rkey)
+static void srp_inv_rkey_err_done(struct ib_cq *cq, struct ib_wc *wc)
+{
+	srp_handle_qp_err(cq, wc, "INV RKEY");
+}
+
+static int srp_inv_rkey(struct srp_request *req, struct srp_rdma_ch *ch,
+		u32 rkey)
 {
 	struct ib_send_wr *bad_wr;
 	struct ib_send_wr wr = {
 		.opcode		    = IB_WR_LOCAL_INV,
-		.wr_id		    = LOCAL_INV_WR_ID_MASK,
 		.next		    = NULL,
 		.num_sge	    = 0,
 		.send_flags	    = 0,
 		.ex.invalidate_rkey = rkey,
 	};
 
+	wr.wr_cqe = &req->reg_cqe;
+	req->reg_cqe.done = srp_inv_rkey_err_done;
 	return ib_post_send(ch->qp, &wr, &bad_wr);
 }
 
@@ -1074,7 +1088,7 @@ static void srp_unmap_data(struct scsi_cmnd *scmnd,
 		struct srp_fr_desc **pfr;
 
 		for (i = req->nmdesc, pfr = req->fr_list; i > 0; i--, pfr++) {
-			res = srp_inv_rkey(ch, (*pfr)->mr->rkey);
+			res = srp_inv_rkey(req, ch, (*pfr)->mr->rkey);
 			if (res < 0) {
 				shost_printk(KERN_ERR, target->scsi_host, PFX
 				  "Queueing INV WR for rkey %#x failed (%d)\n",
@@ -1312,7 +1326,13 @@ reset_state:
 	return 0;
 }
 
+static void srp_reg_mr_err_done(struct ib_cq *cq, struct ib_wc *wc)
+{
+	srp_handle_qp_err(cq, wc, "FAST REG");
+}
+
 static int srp_map_finish_fr(struct srp_map_state *state,
+			     struct srp_request *req,
 			     struct srp_rdma_ch *ch, int sg_nents)
 {
 	struct srp_target_port *target = ch->target;
@@ -1349,9 +1369,11 @@ static int srp_map_finish_fr(struct srp_map_state *state,
 	if (unlikely(n < 0))
 		return n;
 
+	req->reg_cqe.done = srp_reg_mr_err_done;
+
 	wr.wr.next = NULL;
 	wr.wr.opcode = IB_WR_REG_MR;
-	wr.wr.wr_id = FAST_REG_WR_ID_MASK;
+	wr.wr.wr_cqe = &req->reg_cqe;
 	wr.wr.num_sge = 0;
 	wr.wr.send_flags = 0;
 	wr.mr = desc->mr;
@@ -1455,7 +1477,7 @@ static int srp_map_sg_fr(struct srp_map_state *state, struct srp_rdma_ch *ch,
 	while (count) {
 		int i, n;
 
-		n = srp_map_finish_fr(state, ch, count);
+		n = srp_map_finish_fr(state, req, ch, count);
 		if (unlikely(n < 0))
 			return n;
 
@@ -1524,7 +1546,7 @@ static int srp_map_idb(struct srp_rdma_ch *ch, struct srp_request *req,
 #ifdef CONFIG_NEED_SG_DMA_LENGTH
 		idb_sg->dma_length = idb_sg->length;	      /* hack^2 */
 #endif
-		ret = srp_map_finish_fr(&state, ch, 1);
+		ret = srp_map_finish_fr(&state, req, ch, 1);
 		if (ret < 0)
 			return ret;
 	} else if (dev->use_fmr) {
@@ -1719,7 +1741,7 @@ static struct srp_iu *__srp_get_tx_iu(struct srp_rdma_ch *ch,
 	s32 rsv = (iu_type == SRP_IU_TSK_MGMT) ? 0 : SRP_TSK_MGMT_SQ_SIZE;
 	struct srp_iu *iu;
 
-	srp_send_completion(ch->send_cq, ch);
+	ib_process_cq_direct(ch->send_cq, -1);
 
 	if (list_empty(&ch->free_tx))
 		return NULL;
@@ -1739,6 +1761,19 @@ static struct srp_iu *__srp_get_tx_iu(struct srp_rdma_ch *ch,
 	return iu;
 }
 
+static void srp_send_done(struct ib_cq *cq, struct ib_wc *wc)
+{
+	struct srp_iu *iu = container_of(wc->wr_cqe, struct srp_iu, cqe);
+	struct srp_rdma_ch *ch = cq->cq_context;
+
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		srp_handle_qp_err(cq, wc, "SEND");
+		return;
+	}
+
+	list_add(&iu->list, &ch->free_tx);
+}
+
 static int srp_post_send(struct srp_rdma_ch *ch, struct srp_iu *iu, int len)
 {
 	struct srp_target_port *target = ch->target;
@@ -1749,8 +1784,10 @@ static int srp_post_send(struct srp_rdma_ch *ch, struct srp_iu *iu, int len)
 	list.length = len;
 	list.lkey   = target->lkey;
 
+	iu->cqe.done = srp_send_done;
+
 	wr.next       = NULL;
-	wr.wr_id      = (uintptr_t) iu;
+	wr.wr_cqe     = &iu->cqe;
 	wr.sg_list    = &list;
 	wr.num_sge    = 1;
 	wr.opcode     = IB_WR_SEND;
@@ -1769,8 +1806,10 @@ static int srp_post_recv(struct srp_rdma_ch *ch, struct srp_iu *iu)
 	list.length = iu->size;
 	list.lkey   = target->lkey;
 
+	iu->cqe.done = srp_recv_done;
+
 	wr.next     = NULL;
-	wr.wr_id    = (uintptr_t) iu;
+	wr.wr_cqe   = &iu->cqe;
 	wr.sg_list  = &list;
 	wr.num_sge  = 1;
 
@@ -1902,14 +1941,20 @@ static void srp_process_aer_req(struct srp_rdma_ch *ch,
 			     "problems processing SRP_AER_REQ\n");
 }
 
-static void srp_handle_recv(struct srp_rdma_ch *ch, struct ib_wc *wc)
+static void srp_recv_done(struct ib_cq *cq, struct ib_wc *wc)
 {
+	struct srp_iu *iu = container_of(wc->wr_cqe, struct srp_iu, cqe);
+	struct srp_rdma_ch *ch = cq->cq_context;
 	struct srp_target_port *target = ch->target;
 	struct ib_device *dev = target->srp_host->srp_dev->dev;
-	struct srp_iu *iu = (struct srp_iu *) (uintptr_t) wc->wr_id;
 	int res;
 	u8 opcode;
 
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		srp_handle_qp_err(cq, wc, "RECV");
+		return;
+	}
+
 	ib_dma_sync_single_for_cpu(dev, iu->dma, ch->max_ti_iu_len,
 				   DMA_FROM_DEVICE);
 
@@ -1972,68 +2017,22 @@ static void srp_tl_err_work(struct work_struct *work)
 		srp_start_tl_fail_timers(target->rport);
 }
 
-static void srp_handle_qp_err(u64 wr_id, enum ib_wc_status wc_status,
-			      bool send_err, struct srp_rdma_ch *ch)
+static void srp_handle_qp_err(struct ib_cq *cq, struct ib_wc *wc,
+		const char *opname)
 {
+	struct srp_rdma_ch *ch = cq->cq_context;
 	struct srp_target_port *target = ch->target;
 
-	if (wr_id == SRP_LAST_WR_ID) {
-		complete(&ch->done);
-		return;
-	}
-
 	if (ch->connected && !target->qp_in_error) {
-		if (wr_id & LOCAL_INV_WR_ID_MASK) {
-			shost_printk(KERN_ERR, target->scsi_host, PFX
-				     "LOCAL_INV failed with status %s (%d)\n",
-				     ib_wc_status_msg(wc_status), wc_status);
-		} else if (wr_id & FAST_REG_WR_ID_MASK) {
-			shost_printk(KERN_ERR, target->scsi_host, PFX
-				     "FAST_REG_MR failed status %s (%d)\n",
-				     ib_wc_status_msg(wc_status), wc_status);
-		} else {
-			shost_printk(KERN_ERR, target->scsi_host,
-				     PFX "failed %s status %s (%d) for iu %p\n",
-				     send_err ? "send" : "receive",
-				     ib_wc_status_msg(wc_status), wc_status,
-				     (void *)(uintptr_t)wr_id);
-		}
+		shost_printk(KERN_ERR, target->scsi_host,
+			     PFX "failed %s status %s (%d) for CQE %p\n",
+			     opname, ib_wc_status_msg(wc->status), wc->status,
+			     wc->wr_cqe);
 		queue_work(system_long_wq, &target->tl_err_work);
 	}
 	target->qp_in_error = true;
 }
 
-static void srp_recv_completion(struct ib_cq *cq, void *ch_ptr)
-{
-	struct srp_rdma_ch *ch = ch_ptr;
-	struct ib_wc wc;
-
-	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
-	while (ib_poll_cq(cq, 1, &wc) > 0) {
-		if (likely(wc.status == IB_WC_SUCCESS)) {
-			srp_handle_recv(ch, &wc);
-		} else {
-			srp_handle_qp_err(wc.wr_id, wc.status, false, ch);
-		}
-	}
-}
-
-static void srp_send_completion(struct ib_cq *cq, void *ch_ptr)
-{
-	struct srp_rdma_ch *ch = ch_ptr;
-	struct ib_wc wc;
-	struct srp_iu *iu;
-
-	while (ib_poll_cq(cq, 1, &wc) > 0) {
-		if (likely(wc.status == IB_WC_SUCCESS)) {
-			iu = (struct srp_iu *) (uintptr_t) wc.wr_id;
-			list_add(&iu->list, &ch->free_tx);
-		} else {
-			srp_handle_qp_err(wc.wr_id, wc.status, true, ch);
-		}
-	}
-}
-
 static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
 {
 	struct srp_target_port *target = host_to_target(shost);
@@ -3439,27 +3438,17 @@ free_host:
 static void srp_add_one(struct ib_device *device)
 {
 	struct srp_device *srp_dev;
-	struct ib_device_attr *dev_attr;
 	struct srp_host *host;
 	int mr_page_shift, p;
 	u64 max_pages_per_mr;
 
-	dev_attr = kmalloc(sizeof *dev_attr, GFP_KERNEL);
-	if (!dev_attr)
-		return;
-
-	if (ib_query_device(device, dev_attr)) {
-		pr_warn("Query device failed for %s\n", device->name);
-		goto free_attr;
-	}
-
 	srp_dev = kmalloc(sizeof *srp_dev, GFP_KERNEL);
 	if (!srp_dev)
-		goto free_attr;
+		return;
 
 	srp_dev->has_fmr = (device->alloc_fmr && device->dealloc_fmr &&
 			    device->map_phys_fmr && device->unmap_fmr);
-	srp_dev->has_fr = (dev_attr->device_cap_flags &
+	srp_dev->has_fr = (device->attrs.device_cap_flags &
 			   IB_DEVICE_MEM_MGT_EXTENSIONS);
 	if (!srp_dev->has_fmr && !srp_dev->has_fr)
 		dev_warn(&device->dev, "neither FMR nor FR is supported\n");
@@ -3473,23 +3462,23 @@ static void srp_add_one(struct ib_device *device)
 	 * minimum of 4096 bytes. We're unlikely to build large sglists
 	 * out of smaller entries.
 	 */
-	mr_page_shift		= max(12, ffs(dev_attr->page_size_cap) - 1);
+	mr_page_shift		= max(12, ffs(device->attrs.page_size_cap) - 1);
 	srp_dev->mr_page_size	= 1 << mr_page_shift;
 	srp_dev->mr_page_mask	= ~((u64) srp_dev->mr_page_size - 1);
-	max_pages_per_mr	= dev_attr->max_mr_size;
+	max_pages_per_mr	= device->attrs.max_mr_size;
 	do_div(max_pages_per_mr, srp_dev->mr_page_size);
 	srp_dev->max_pages_per_mr = min_t(u64, SRP_MAX_PAGES_PER_MR,
 					  max_pages_per_mr);
 	if (srp_dev->use_fast_reg) {
 		srp_dev->max_pages_per_mr =
 			min_t(u32, srp_dev->max_pages_per_mr,
-			      dev_attr->max_fast_reg_page_list_len);
+			      device->attrs.max_fast_reg_page_list_len);
 	}
 	srp_dev->mr_max_size	= srp_dev->mr_page_size *
 				   srp_dev->max_pages_per_mr;
-	pr_debug("%s: mr_page_shift = %d, dev_attr->max_mr_size = %#llx, dev_attr->max_fast_reg_page_list_len = %u, max_pages_per_mr = %d, mr_max_size = %#x\n",
-		 device->name, mr_page_shift, dev_attr->max_mr_size,
-		 dev_attr->max_fast_reg_page_list_len,
+	pr_debug("%s: mr_page_shift = %d, device->max_mr_size = %#llx, device->max_fast_reg_page_list_len = %u, max_pages_per_mr = %d, mr_max_size = %#x\n",
+		 device->name, mr_page_shift, device->attrs.max_mr_size,
+		 device->attrs.max_fast_reg_page_list_len,
 		 srp_dev->max_pages_per_mr, srp_dev->mr_max_size);
 
 	INIT_LIST_HEAD(&srp_dev->dev_list);
@@ -3517,17 +3506,13 @@ static void srp_add_one(struct ib_device *device)
 	}
 
 	ib_set_client_data(device, &srp_client, srp_dev);
-
-	goto free_attr;
+	return;
 
 err_pd:
 	ib_dealloc_pd(srp_dev->pd);
 
 free_dev:
 	kfree(srp_dev);
-
-free_attr:
-	kfree(dev_attr);
 }
 
 static void srp_remove_one(struct ib_device *device, void *client_data)
@@ -3587,8 +3572,6 @@ static int __init srp_init_module(void)
 {
 	int ret;
 
-	BUILD_BUG_ON(FIELD_SIZEOF(struct ib_wc, wr_id) < sizeof(void *));
-
 	if (srp_sg_tablesize) {
 		pr_warn("srp_sg_tablesize is deprecated, please use cmd_sg_entries\n");
 		if (!cmd_sg_entries)
diff --git a/drivers/infiniband/ulp/srp/ib_srp.h b/drivers/infiniband/ulp/srp/ib_srp.h
index f6af531f9f32..9e05ce4a04fd 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.h
+++ b/drivers/infiniband/ulp/srp/ib_srp.h
@@ -66,11 +66,6 @@ enum {
 	SRP_TAG_TSK_MGMT	= 1U << 31,
 
 	SRP_MAX_PAGES_PER_MR	= 512,
-
-	LOCAL_INV_WR_ID_MASK	= 1,
-	FAST_REG_WR_ID_MASK	= 2,
-
-	SRP_LAST_WR_ID		= 0xfffffffcU,
 };
 
 enum srp_target_state {
@@ -128,6 +123,7 @@ struct srp_request {
 	struct srp_direct_buf  *indirect_desc;
 	dma_addr_t		indirect_dma_addr;
 	short			nmdesc;
+	struct ib_cqe		reg_cqe;
 };
 
 /**
@@ -231,6 +227,7 @@ struct srp_iu {
 	void		       *buf;
 	size_t			size;
 	enum dma_data_direction	direction;
+	struct ib_cqe		cqe;
 };
 
 /**
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index 2e2fe818ca9f..0c37fee363b1 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -93,6 +93,8 @@ MODULE_PARM_DESC(srpt_service_guid,
 static struct ib_client srpt_client;
 static void srpt_release_channel(struct srpt_rdma_ch *ch);
 static int srpt_queue_status(struct se_cmd *cmd);
+static void srpt_recv_done(struct ib_cq *cq, struct ib_wc *wc);
+static void srpt_send_done(struct ib_cq *cq, struct ib_wc *wc);
 
 /**
  * opposite_dma_dir() - Swap DMA_TO_DEVICE and DMA_FROM_DEVICE.
@@ -341,10 +343,10 @@ static void srpt_get_ioc(struct srpt_port *sport, u32 slot,
 	memset(iocp, 0, sizeof *iocp);
 	strcpy(iocp->id_string, SRPT_ID_STRING);
 	iocp->guid = cpu_to_be64(srpt_service_guid);
-	iocp->vendor_id = cpu_to_be32(sdev->dev_attr.vendor_id);
-	iocp->device_id = cpu_to_be32(sdev->dev_attr.vendor_part_id);
-	iocp->device_version = cpu_to_be16(sdev->dev_attr.hw_ver);
-	iocp->subsys_vendor_id = cpu_to_be32(sdev->dev_attr.vendor_id);
+	iocp->vendor_id = cpu_to_be32(sdev->device->attrs.vendor_id);
+	iocp->device_id = cpu_to_be32(sdev->device->attrs.vendor_part_id);
+	iocp->device_version = cpu_to_be16(sdev->device->attrs.hw_ver);
+	iocp->subsys_vendor_id = cpu_to_be32(sdev->device->attrs.vendor_id);
 	iocp->subsys_device_id = 0x0;
 	iocp->io_class = cpu_to_be16(SRP_REV16A_IB_IO_CLASS);
 	iocp->io_subclass = cpu_to_be16(SRP_IO_SUBCLASS);
@@ -453,6 +455,7 @@ static void srpt_mad_send_handler(struct ib_mad_agent *mad_agent,
  * srpt_mad_recv_handler() - MAD reception callback function.
  */
 static void srpt_mad_recv_handler(struct ib_mad_agent *mad_agent,
+				  struct ib_mad_send_buf *send_buf,
 				  struct ib_mad_recv_wc *mad_wc)
 {
 	struct srpt_port *sport = (struct srpt_port *)mad_agent->context;
@@ -778,12 +781,12 @@ static int srpt_post_recv(struct srpt_device *sdev,
 	struct ib_recv_wr wr, *bad_wr;
 
 	BUG_ON(!sdev);
-	wr.wr_id = encode_wr_id(SRPT_RECV, ioctx->ioctx.index);
-
 	list.addr = ioctx->ioctx.dma;
 	list.length = srp_max_req_size;
 	list.lkey = sdev->pd->local_dma_lkey;
 
+	ioctx->ioctx.cqe.done = srpt_recv_done;
+	wr.wr_cqe = &ioctx->ioctx.cqe;
 	wr.next = NULL;
 	wr.sg_list = &list;
 	wr.num_sge = 1;
@@ -819,8 +822,9 @@ static int srpt_post_send(struct srpt_rdma_ch *ch,
 	list.length = len;
 	list.lkey = sdev->pd->local_dma_lkey;
 
+	ioctx->ioctx.cqe.done = srpt_send_done;
 	wr.next = NULL;
-	wr.wr_id = encode_wr_id(SRPT_SEND, ioctx->ioctx.index);
+	wr.wr_cqe = &ioctx->ioctx.cqe;
 	wr.sg_list = &list;
 	wr.num_sge = 1;
 	wr.opcode = IB_WR_SEND;
@@ -1052,13 +1056,13 @@ static void srpt_unmap_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 
 	BUG_ON(!ch);
 	BUG_ON(!ioctx);
-	BUG_ON(ioctx->n_rdma && !ioctx->rdma_ius);
+	BUG_ON(ioctx->n_rdma && !ioctx->rdma_wrs);
 
 	while (ioctx->n_rdma)
-		kfree(ioctx->rdma_ius[--ioctx->n_rdma].sge);
+		kfree(ioctx->rdma_wrs[--ioctx->n_rdma].wr.sg_list);
 
-	kfree(ioctx->rdma_ius);
-	ioctx->rdma_ius = NULL;
+	kfree(ioctx->rdma_wrs);
+	ioctx->rdma_wrs = NULL;
 
 	if (ioctx->mapped_sg_count) {
 		sg = ioctx->sg;
@@ -1082,7 +1086,7 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 	struct scatterlist *sg, *sg_orig;
 	int sg_cnt;
 	enum dma_data_direction dir;
-	struct rdma_iu *riu;
+	struct ib_rdma_wr *riu;
 	struct srp_direct_buf *db;
 	dma_addr_t dma_addr;
 	struct ib_sge *sge;
@@ -1109,23 +1113,24 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 
 	ioctx->mapped_sg_count = count;
 
-	if (ioctx->rdma_ius && ioctx->n_rdma_ius)
-		nrdma = ioctx->n_rdma_ius;
+	if (ioctx->rdma_wrs && ioctx->n_rdma_wrs)
+		nrdma = ioctx->n_rdma_wrs;
 	else {
 		nrdma = (count + SRPT_DEF_SG_PER_WQE - 1) / SRPT_DEF_SG_PER_WQE
 			+ ioctx->n_rbuf;
 
-		ioctx->rdma_ius = kzalloc(nrdma * sizeof *riu, GFP_KERNEL);
-		if (!ioctx->rdma_ius)
+		ioctx->rdma_wrs = kcalloc(nrdma, sizeof(*ioctx->rdma_wrs),
+				GFP_KERNEL);
+		if (!ioctx->rdma_wrs)
 			goto free_mem;
 
-		ioctx->n_rdma_ius = nrdma;
+		ioctx->n_rdma_wrs = nrdma;
 	}
 
 	db = ioctx->rbufs;
 	tsize = cmd->data_length;
 	dma_len = ib_sg_dma_len(dev, &sg[0]);
-	riu = ioctx->rdma_ius;
+	riu = ioctx->rdma_wrs;
 
 	/*
 	 * For each remote desc - calculate the #ib_sge.
@@ -1139,9 +1144,9 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 	     j < count && i < ioctx->n_rbuf && tsize > 0; ++i, ++riu, ++db) {
 		rsize = be32_to_cpu(db->len);
 		raddr = be64_to_cpu(db->va);
-		riu->raddr = raddr;
+		riu->remote_addr = raddr;
 		riu->rkey = be32_to_cpu(db->key);
-		riu->sge_cnt = 0;
+		riu->wr.num_sge = 0;
 
 		/* calculate how many sge required for this remote_buf */
 		while (rsize > 0 && tsize > 0) {
@@ -1165,33 +1170,35 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 				rsize = 0;
 			}
 
-			++riu->sge_cnt;
+			++riu->wr.num_sge;
 
-			if (rsize > 0 && riu->sge_cnt == SRPT_DEF_SG_PER_WQE) {
+			if (rsize > 0 &&
+			    riu->wr.num_sge == SRPT_DEF_SG_PER_WQE) {
 				++ioctx->n_rdma;
-				riu->sge =
-				    kmalloc(riu->sge_cnt * sizeof *riu->sge,
-					    GFP_KERNEL);
-				if (!riu->sge)
+				riu->wr.sg_list = kmalloc_array(riu->wr.num_sge,
+						sizeof(*riu->wr.sg_list),
+						GFP_KERNEL);
+				if (!riu->wr.sg_list)
 					goto free_mem;
 
 				++riu;
-				riu->sge_cnt = 0;
-				riu->raddr = raddr;
+				riu->wr.num_sge = 0;
+				riu->remote_addr = raddr;
 				riu->rkey = be32_to_cpu(db->key);
 			}
 		}
 
 		++ioctx->n_rdma;
-		riu->sge = kmalloc(riu->sge_cnt * sizeof *riu->sge,
-				   GFP_KERNEL);
-		if (!riu->sge)
+		riu->wr.sg_list = kmalloc_array(riu->wr.num_sge,
+					sizeof(*riu->wr.sg_list),
+					GFP_KERNEL);
+		if (!riu->wr.sg_list)
 			goto free_mem;
 	}
 
 	db = ioctx->rbufs;
 	tsize = cmd->data_length;
-	riu = ioctx->rdma_ius;
+	riu = ioctx->rdma_wrs;
 	sg = sg_orig;
 	dma_len = ib_sg_dma_len(dev, &sg[0]);
 	dma_addr = ib_sg_dma_address(dev, &sg[0]);
@@ -1200,7 +1207,7 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 	for (i = 0, j = 0;
 	     j < count && i < ioctx->n_rbuf && tsize > 0; ++i, ++riu, ++db) {
 		rsize = be32_to_cpu(db->len);
-		sge = riu->sge;
+		sge = riu->wr.sg_list;
 		k = 0;
 
 		while (rsize > 0 && tsize > 0) {
@@ -1232,9 +1239,9 @@ static int srpt_map_sg_to_ib_sge(struct srpt_rdma_ch *ch,
 			}
 
 			++k;
-			if (k == riu->sge_cnt && rsize > 0 && tsize > 0) {
+			if (k == riu->wr.num_sge && rsize > 0 && tsize > 0) {
 				++riu;
-				sge = riu->sge;
+				sge = riu->wr.sg_list;
 				k = 0;
 			} else if (rsize > 0 && tsize > 0)
 				++sge;
@@ -1277,8 +1284,8 @@ static struct srpt_send_ioctx *srpt_get_send_ioctx(struct srpt_rdma_ch *ch)
 	ioctx->n_rbuf = 0;
 	ioctx->rbufs = NULL;
 	ioctx->n_rdma = 0;
-	ioctx->n_rdma_ius = 0;
-	ioctx->rdma_ius = NULL;
+	ioctx->n_rdma_wrs = 0;
+	ioctx->rdma_wrs = NULL;
 	ioctx->mapped_sg_count = 0;
 	init_completion(&ioctx->tx_done);
 	ioctx->queue_status_only = false;
@@ -1380,118 +1387,44 @@ out:
 }
 
 /**
- * srpt_handle_send_err_comp() - Process an IB_WC_SEND error completion.
- */
-static void srpt_handle_send_err_comp(struct srpt_rdma_ch *ch, u64 wr_id)
-{
-	struct srpt_send_ioctx *ioctx;
-	enum srpt_command_state state;
-	u32 index;
-
-	atomic_inc(&ch->sq_wr_avail);
-
-	index = idx_from_wr_id(wr_id);
-	ioctx = ch->ioctx_ring[index];
-	state = srpt_get_cmd_state(ioctx);
-
-	WARN_ON(state != SRPT_STATE_CMD_RSP_SENT
-		&& state != SRPT_STATE_MGMT_RSP_SENT
-		&& state != SRPT_STATE_NEED_DATA
-		&& state != SRPT_STATE_DONE);
-
-	/* If SRP_RSP sending failed, undo the ch->req_lim change. */
-	if (state == SRPT_STATE_CMD_RSP_SENT
-	    || state == SRPT_STATE_MGMT_RSP_SENT)
-		atomic_dec(&ch->req_lim);
-
-	srpt_abort_cmd(ioctx);
-}
-
-/**
- * srpt_handle_send_comp() - Process an IB send completion notification.
- */
-static void srpt_handle_send_comp(struct srpt_rdma_ch *ch,
-				  struct srpt_send_ioctx *ioctx)
-{
-	enum srpt_command_state state;
-
-	atomic_inc(&ch->sq_wr_avail);
-
-	state = srpt_set_cmd_state(ioctx, SRPT_STATE_DONE);
-
-	if (WARN_ON(state != SRPT_STATE_CMD_RSP_SENT
-		    && state != SRPT_STATE_MGMT_RSP_SENT
-		    && state != SRPT_STATE_DONE))
-		pr_debug("state = %d\n", state);
-
-	if (state != SRPT_STATE_DONE) {
-		srpt_unmap_sg_to_ib_sge(ch, ioctx);
-		transport_generic_free_cmd(&ioctx->cmd, 0);
-	} else {
-		pr_err("IB completion has been received too late for"
-		       " wr_id = %u.\n", ioctx->ioctx.index);
-	}
-}
-
-/**
- * srpt_handle_rdma_comp() - Process an IB RDMA completion notification.
- *
  * XXX: what is now target_execute_cmd used to be asynchronous, and unmapping
  * the data that has been transferred via IB RDMA had to be postponed until the
  * check_stop_free() callback.  None of this is necessary anymore and needs to
  * be cleaned up.
  */
-static void srpt_handle_rdma_comp(struct srpt_rdma_ch *ch,
-				  struct srpt_send_ioctx *ioctx,
-				  enum srpt_opcode opcode)
+static void srpt_rdma_read_done(struct ib_cq *cq, struct ib_wc *wc)
 {
+	struct srpt_rdma_ch *ch = cq->cq_context;
+	struct srpt_send_ioctx *ioctx =
+		container_of(wc->wr_cqe, struct srpt_send_ioctx, rdma_cqe);
+
 	WARN_ON(ioctx->n_rdma <= 0);
 	atomic_add(ioctx->n_rdma, &ch->sq_wr_avail);
 
-	if (opcode == SRPT_RDMA_READ_LAST) {
-		if (srpt_test_and_set_cmd_state(ioctx, SRPT_STATE_NEED_DATA,
-						SRPT_STATE_DATA_IN))
-			target_execute_cmd(&ioctx->cmd);
-		else
-			pr_err("%s[%d]: wrong state = %d\n", __func__,
-			       __LINE__, srpt_get_cmd_state(ioctx));
-	} else if (opcode == SRPT_RDMA_ABORT) {
-		ioctx->rdma_aborted = true;
-	} else {
-		WARN(true, "unexpected opcode %d\n", opcode);
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		pr_info("RDMA_READ for ioctx 0x%p failed with status %d\n",
+			ioctx, wc->status);
+		srpt_abort_cmd(ioctx);
+		return;
 	}
+
+	if (srpt_test_and_set_cmd_state(ioctx, SRPT_STATE_NEED_DATA,
+					SRPT_STATE_DATA_IN))
+		target_execute_cmd(&ioctx->cmd);
+	else
+		pr_err("%s[%d]: wrong state = %d\n", __func__,
+		       __LINE__, srpt_get_cmd_state(ioctx));
 }
 
-/**
- * srpt_handle_rdma_err_comp() - Process an IB RDMA error completion.
- */
-static void srpt_handle_rdma_err_comp(struct srpt_rdma_ch *ch,
-				      struct srpt_send_ioctx *ioctx,
-				      enum srpt_opcode opcode)
+static void srpt_rdma_write_done(struct ib_cq *cq, struct ib_wc *wc)
 {
-	enum srpt_command_state state;
+	struct srpt_send_ioctx *ioctx =
+		container_of(wc->wr_cqe, struct srpt_send_ioctx, rdma_cqe);
 
-	state = srpt_get_cmd_state(ioctx);
-	switch (opcode) {
-	case SRPT_RDMA_READ_LAST:
-		if (ioctx->n_rdma <= 0) {
-			pr_err("Received invalid RDMA read"
-			       " error completion with idx %d\n",
-			       ioctx->ioctx.index);
-			break;
-		}
-		atomic_add(ioctx->n_rdma, &ch->sq_wr_avail);
-		if (state == SRPT_STATE_NEED_DATA)
-			srpt_abort_cmd(ioctx);
-		else
-			pr_err("%s[%d]: wrong state = %d\n",
-			       __func__, __LINE__, state);
-		break;
-	case SRPT_RDMA_WRITE_LAST:
-		break;
-	default:
-		pr_err("%s[%d]: opcode = %u\n", __func__, __LINE__, opcode);
-		break;
+	if (unlikely(wc->status != IB_WC_SUCCESS)) {
+		pr_info("RDMA_WRITE for ioctx 0x%p failed with status %d\n",
+			ioctx, wc->status);
+		srpt_abort_cmd(ioctx);
 	}
 }
 
@@ -1926,32 +1859,26 @@ out:
 	return;
 }
 
-static void srpt_process_rcv_completion(struct ib_cq *cq,
-					struct srpt_rdma_ch *ch,
-					struct ib_wc *wc)
+static void srpt_recv_done(struct ib_cq *cq, struct ib_wc *wc)
 {
-	struct srpt_device *sdev = ch->sport->sdev;
-	struct srpt_recv_ioctx *ioctx;
-	u32 index;
+	struct srpt_rdma_ch *ch = cq->cq_context;
+	struct srpt_recv_ioctx *ioctx =
+		container_of(wc->wr_cqe, struct srpt_recv_ioctx, ioctx.cqe);
 
-	index = idx_from_wr_id(wc->wr_id);
 	if (wc->status == IB_WC_SUCCESS) {
 		int req_lim;
 
 		req_lim = atomic_dec_return(&ch->req_lim);
 		if (unlikely(req_lim < 0))
 			pr_err("req_lim = %d < 0\n", req_lim);
-		ioctx = sdev->ioctx_ring[index];
 		srpt_handle_new_iu(ch, ioctx, NULL);
 	} else {
-		pr_info("receiving failed for idx %u with status %d\n",
-			index, wc->status);
+		pr_info("receiving failed for ioctx %p with status %d\n",
+			ioctx, wc->status);
 	}
 }
 
 /**
- * srpt_process_send_completion() - Process an IB send completion.
- *
  * Note: Although this has not yet been observed during tests, at least in
  * theory it is possible that the srpt_get_send_ioctx() call invoked by
  * srpt_handle_new_iu() fails. This is possible because the req_lim_delta
@@ -1964,109 +1891,52 @@ static void srpt_process_rcv_completion(struct ib_cq *cq,
  * are queued on cmd_wait_list. The code below processes these delayed
  * requests one at a time.
  */
-static void srpt_process_send_completion(struct ib_cq *cq,
-					 struct srpt_rdma_ch *ch,
-					 struct ib_wc *wc)
+static void srpt_send_done(struct ib_cq *cq, struct ib_wc *wc)
 {
-	struct srpt_send_ioctx *send_ioctx;
-	uint32_t index;
-	enum srpt_opcode opcode;
+	struct srpt_rdma_ch *ch = cq->cq_context;
+	struct srpt_send_ioctx *ioctx =
+		container_of(wc->wr_cqe, struct srpt_send_ioctx, ioctx.cqe);
+	enum srpt_command_state state;
 
-	index = idx_from_wr_id(wc->wr_id);
-	opcode = opcode_from_wr_id(wc->wr_id);
-	send_ioctx = ch->ioctx_ring[index];
-	if (wc->status == IB_WC_SUCCESS) {
-		if (opcode == SRPT_SEND)
-			srpt_handle_send_comp(ch, send_ioctx);
-		else {
-			WARN_ON(opcode != SRPT_RDMA_ABORT &&
-				wc->opcode != IB_WC_RDMA_READ);
-			srpt_handle_rdma_comp(ch, send_ioctx, opcode);
-		}
+	state = srpt_set_cmd_state(ioctx, SRPT_STATE_DONE);
+
+	WARN_ON(state != SRPT_STATE_CMD_RSP_SENT &&
+		state != SRPT_STATE_MGMT_RSP_SENT);
+
+	atomic_inc(&ch->sq_wr_avail);
+
+	if (wc->status != IB_WC_SUCCESS) {
+		pr_info("sending response for ioctx 0x%p failed"
+			" with status %d\n", ioctx, wc->status);
+
+		atomic_dec(&ch->req_lim);
+		srpt_abort_cmd(ioctx);
+		goto out;
+	}
+
+	if (state != SRPT_STATE_DONE) {
+		srpt_unmap_sg_to_ib_sge(ch, ioctx);
+		transport_generic_free_cmd(&ioctx->cmd, 0);
 	} else {
-		if (opcode == SRPT_SEND) {
-			pr_info("sending response for idx %u failed"
-				" with status %d\n", index, wc->status);
-			srpt_handle_send_err_comp(ch, wc->wr_id);
-		} else if (opcode != SRPT_RDMA_MID) {
-			pr_info("RDMA t %d for idx %u failed with"
-				" status %d\n", opcode, index, wc->status);
-			srpt_handle_rdma_err_comp(ch, send_ioctx, opcode);
-		}
+		pr_err("IB completion has been received too late for"
+		       " wr_id = %u.\n", ioctx->ioctx.index);
 	}
 
-	while (unlikely(opcode == SRPT_SEND
-			&& !list_empty(&ch->cmd_wait_list)
-			&& srpt_get_ch_state(ch) == CH_LIVE
-			&& (send_ioctx = srpt_get_send_ioctx(ch)) != NULL)) {
+out:
+	while (!list_empty(&ch->cmd_wait_list) &&
+	       srpt_get_ch_state(ch) == CH_LIVE &&
+	       (ioctx = srpt_get_send_ioctx(ch)) != NULL) {
 		struct srpt_recv_ioctx *recv_ioctx;
 
 		recv_ioctx = list_first_entry(&ch->cmd_wait_list,
 					      struct srpt_recv_ioctx,
 					      wait_list);
 		list_del(&recv_ioctx->wait_list);
-		srpt_handle_new_iu(ch, recv_ioctx, send_ioctx);
-	}
-}
-
-static void srpt_process_completion(struct ib_cq *cq, struct srpt_rdma_ch *ch)
-{
-	struct ib_wc *const wc = ch->wc;
-	int i, n;
-
-	WARN_ON(cq != ch->cq);
-
-	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
-	while ((n = ib_poll_cq(cq, ARRAY_SIZE(ch->wc), wc)) > 0) {
-		for (i = 0; i < n; i++) {
-			if (opcode_from_wr_id(wc[i].wr_id) == SRPT_RECV)
-				srpt_process_rcv_completion(cq, ch, &wc[i]);
-			else
-				srpt_process_send_completion(cq, ch, &wc[i]);
-		}
+		srpt_handle_new_iu(ch, recv_ioctx, ioctx);
 	}
 }
 
 /**
- * srpt_completion() - IB completion queue callback function.
- *
- * Notes:
- * - It is guaranteed that a completion handler will never be invoked
- *   concurrently on two different CPUs for the same completion queue. See also
- *   Documentation/infiniband/core_locking.txt and the implementation of
- *   handle_edge_irq() in kernel/irq/chip.c.
- * - When threaded IRQs are enabled, completion handlers are invoked in thread
- *   context instead of interrupt context.
- */
-static void srpt_completion(struct ib_cq *cq, void *ctx)
-{
-	struct srpt_rdma_ch *ch = ctx;
-
-	wake_up_interruptible(&ch->wait_queue);
-}
-
-static int srpt_compl_thread(void *arg)
-{
-	struct srpt_rdma_ch *ch;
-
-	/* Hibernation / freezing of the SRPT kernel thread is not supported. */
-	current->flags |= PF_NOFREEZE;
-
-	ch = arg;
-	BUG_ON(!ch);
-	pr_info("Session %s: kernel thread %s (PID %d) started\n",
-		ch->sess_name, ch->thread->comm, current->pid);
-	while (!kthread_should_stop()) {
-		wait_event_interruptible(ch->wait_queue,
-			(srpt_process_completion(ch->cq, ch),
-			 kthread_should_stop()));
-	}
-	pr_info("Session %s: kernel thread %s (PID %d) stopped\n",
-		ch->sess_name, ch->thread->comm, current->pid);
-	return 0;
-}
-
-/**
  * srpt_create_ch_ib() - Create receive and send completion queues.
  */
 static int srpt_create_ch_ib(struct srpt_rdma_ch *ch)
@@ -2075,7 +1945,6 @@ static int srpt_create_ch_ib(struct srpt_rdma_ch *ch)
 	struct srpt_port *sport = ch->sport;
 	struct srpt_device *sdev = sport->sdev;
 	u32 srp_sq_size = sport->port_attrib.srp_sq_size;
-	struct ib_cq_init_attr cq_attr = {};
 	int ret;
 
 	WARN_ON(ch->rq_size < 1);
@@ -2086,9 +1955,8 @@ static int srpt_create_ch_ib(struct srpt_rdma_ch *ch)
 		goto out;
 
 retry:
-	cq_attr.cqe = ch->rq_size + srp_sq_size;
-	ch->cq = ib_create_cq(sdev->device, srpt_completion, NULL, ch,
-			      &cq_attr);
+	ch->cq = ib_alloc_cq(sdev->device, ch, ch->rq_size + srp_sq_size,
+			0 /* XXX: spread CQs */, IB_POLL_WORKQUEUE);
 	if (IS_ERR(ch->cq)) {
 		ret = PTR_ERR(ch->cq);
 		pr_err("failed to create CQ cqe= %d ret= %d\n",
@@ -2131,18 +1999,6 @@ retry:
 	if (ret)
 		goto err_destroy_qp;
 
-	init_waitqueue_head(&ch->wait_queue);
-
-	pr_debug("creating thread for session %s\n", ch->sess_name);
-
-	ch->thread = kthread_run(srpt_compl_thread, ch, "ib_srpt_compl");
-	if (IS_ERR(ch->thread)) {
-		pr_err("failed to create kernel thread %ld\n",
-		       PTR_ERR(ch->thread));
-		ch->thread = NULL;
-		goto err_destroy_qp;
-	}
-
 out:
 	kfree(qp_init);
 	return ret;
@@ -2150,17 +2006,14 @@ out:
 err_destroy_qp:
 	ib_destroy_qp(ch->qp);
 err_destroy_cq:
-	ib_destroy_cq(ch->cq);
+	ib_free_cq(ch->cq);
 	goto out;
 }
 
 static void srpt_destroy_ch_ib(struct srpt_rdma_ch *ch)
 {
-	if (ch->thread)
-		kthread_stop(ch->thread);
-
 	ib_destroy_qp(ch->qp);
-	ib_destroy_cq(ch->cq);
+	ib_free_cq(ch->cq);
 }
 
 /**
@@ -2370,31 +2223,6 @@ static void srpt_release_channel_work(struct work_struct *w)
 	kfree(ch);
 }
 
-static struct srpt_node_acl *__srpt_lookup_acl(struct srpt_port *sport,
-					       u8 i_port_id[16])
-{
-	struct srpt_node_acl *nacl;
-
-	list_for_each_entry(nacl, &sport->port_acl_list, list)
-		if (memcmp(nacl->i_port_id, i_port_id,
-			   sizeof(nacl->i_port_id)) == 0)
-			return nacl;
-
-	return NULL;
-}
-
-static struct srpt_node_acl *srpt_lookup_acl(struct srpt_port *sport,
-					     u8 i_port_id[16])
-{
-	struct srpt_node_acl *nacl;
-
-	spin_lock_irq(&sport->port_acl_lock);
-	nacl = __srpt_lookup_acl(sport, i_port_id);
-	spin_unlock_irq(&sport->port_acl_lock);
-
-	return nacl;
-}
-
 /**
  * srpt_cm_req_recv() - Process the event IB_CM_REQ_RECEIVED.
  *
@@ -2412,10 +2240,10 @@ static int srpt_cm_req_recv(struct ib_cm_id *cm_id,
 	struct srp_login_rej *rej;
 	struct ib_cm_rep_param *rep_param;
 	struct srpt_rdma_ch *ch, *tmp_ch;
-	struct srpt_node_acl *nacl;
+	struct se_node_acl *se_acl;
 	u32 it_iu_len;
-	int i;
-	int ret = 0;
+	int i, ret = 0;
+	unsigned char *p;
 
 	WARN_ON_ONCE(irqs_disabled());
 
@@ -2565,33 +2393,47 @@ static int srpt_cm_req_recv(struct ib_cm_id *cm_id,
 		       " RTR failed (error code = %d)\n", ret);
 		goto destroy_ib;
 	}
+
 	/*
-	 * Use the initator port identifier as the session name.
+	 * Use the initator port identifier as the session name, when
+	 * checking against se_node_acl->initiatorname[] this can be
+	 * with or without preceeding '0x'.
 	 */
 	snprintf(ch->sess_name, sizeof(ch->sess_name), "0x%016llx%016llx",
 			be64_to_cpu(*(__be64 *)ch->i_port_id),
 			be64_to_cpu(*(__be64 *)(ch->i_port_id + 8)));
 
 	pr_debug("registering session %s\n", ch->sess_name);
+	p = &ch->sess_name[0];
 
-	nacl = srpt_lookup_acl(sport, ch->i_port_id);
-	if (!nacl) {
-		pr_info("Rejected login because no ACL has been"
-			" configured yet for initiator %s.\n", ch->sess_name);
+	ch->sess = transport_init_session(TARGET_PROT_NORMAL);
+	if (IS_ERR(ch->sess)) {
 		rej->reason = cpu_to_be32(
-			      SRP_LOGIN_REJ_CHANNEL_LIMIT_REACHED);
+				SRP_LOGIN_REJ_INSUFFICIENT_RESOURCES);
+		pr_debug("Failed to create session\n");
 		goto destroy_ib;
 	}
 
-	ch->sess = transport_init_session(TARGET_PROT_NORMAL);
-	if (IS_ERR(ch->sess)) {
+try_again:
+	se_acl = core_tpg_get_initiator_node_acl(&sport->port_tpg_1, p);
+	if (!se_acl) {
+		pr_info("Rejected login because no ACL has been"
+			" configured yet for initiator %s.\n", ch->sess_name);
+		/*
+		 * XXX: Hack to retry of ch->i_port_id without leading '0x'
+		 */
+		if (p == &ch->sess_name[0]) {
+			p += 2;
+			goto try_again;
+		}
 		rej->reason = cpu_to_be32(
-			      SRP_LOGIN_REJ_INSUFFICIENT_RESOURCES);
-		pr_debug("Failed to create session\n");
-		goto deregister_session;
+				SRP_LOGIN_REJ_CHANNEL_LIMIT_REACHED);
+		transport_free_session(ch->sess);
+		goto destroy_ib;
 	}
-	ch->sess->se_node_acl = &nacl->nacl;
-	transport_register_session(&sport->port_tpg_1, &nacl->nacl, ch->sess, ch);
+	ch->sess->se_node_acl = se_acl;
+
+	transport_register_session(&sport->port_tpg_1, se_acl, ch->sess, ch);
 
 	pr_debug("Establish connection sess=%p name=%s cm_id=%p\n", ch->sess,
 		 ch->sess_name, ch->cm_id);
@@ -2635,8 +2477,6 @@ static int srpt_cm_req_recv(struct ib_cm_id *cm_id,
 release_channel:
 	srpt_set_ch_state(ch, CH_RELEASING);
 	transport_deregister_session_configfs(ch->sess);
-
-deregister_session:
 	transport_deregister_session(ch->sess);
 	ch->sess = NULL;
 
@@ -2821,12 +2661,8 @@ static int srpt_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event)
 static int srpt_perform_rdmas(struct srpt_rdma_ch *ch,
 			      struct srpt_send_ioctx *ioctx)
 {
-	struct ib_rdma_wr wr;
 	struct ib_send_wr *bad_wr;
-	struct rdma_iu *riu;
-	int i;
-	int ret;
-	int sq_wr_avail;
+	int sq_wr_avail, ret, i;
 	enum dma_data_direction dir;
 	const int n_rdma = ioctx->n_rdma;
 
@@ -2842,59 +2678,32 @@ static int srpt_perform_rdmas(struct srpt_rdma_ch *ch,
 		}
 	}
 
-	ioctx->rdma_aborted = false;
-	ret = 0;
-	riu = ioctx->rdma_ius;
-	memset(&wr, 0, sizeof wr);
-
-	for (i = 0; i < n_rdma; ++i, ++riu) {
-		if (dir == DMA_FROM_DEVICE) {
-			wr.wr.opcode = IB_WR_RDMA_WRITE;
-			wr.wr.wr_id = encode_wr_id(i == n_rdma - 1 ?
-						SRPT_RDMA_WRITE_LAST :
-						SRPT_RDMA_MID,
-						ioctx->ioctx.index);
-		} else {
-			wr.wr.opcode = IB_WR_RDMA_READ;
-			wr.wr.wr_id = encode_wr_id(i == n_rdma - 1 ?
-						SRPT_RDMA_READ_LAST :
-						SRPT_RDMA_MID,
-						ioctx->ioctx.index);
-		}
-		wr.wr.next = NULL;
-		wr.remote_addr = riu->raddr;
-		wr.rkey = riu->rkey;
-		wr.wr.num_sge = riu->sge_cnt;
-		wr.wr.sg_list = riu->sge;
+	for (i = 0; i < n_rdma; i++) {
+		struct ib_send_wr *wr = &ioctx->rdma_wrs[i].wr;
 
-		/* only get completion event for the last rdma write */
-		if (i == (n_rdma - 1) && dir == DMA_TO_DEVICE)
-			wr.wr.send_flags = IB_SEND_SIGNALED;
+		wr->opcode = (dir == DMA_FROM_DEVICE) ?
+				IB_WR_RDMA_WRITE : IB_WR_RDMA_READ;
 
-		ret = ib_post_send(ch->qp, &wr.wr, &bad_wr);
-		if (ret)
-			break;
+		if (i == n_rdma - 1) {
+			/* only get completion event for the last rdma read */
+			if (dir == DMA_TO_DEVICE) {
+				wr->send_flags = IB_SEND_SIGNALED;
+				ioctx->rdma_cqe.done = srpt_rdma_read_done;
+			} else {
+				ioctx->rdma_cqe.done = srpt_rdma_write_done;
+			}
+			wr->wr_cqe = &ioctx->rdma_cqe;
+			wr->next = NULL;
+		} else {
+			wr->wr_cqe = NULL;
+			wr->next = &ioctx->rdma_wrs[i + 1].wr;
+		}
 	}
 
+	ret = ib_post_send(ch->qp, &ioctx->rdma_wrs->wr, &bad_wr);
 	if (ret)
 		pr_err("%s[%d]: ib_post_send() returned %d for %d/%d\n",
 				 __func__, __LINE__, ret, i, n_rdma);
-	if (ret && i > 0) {
-		wr.wr.num_sge = 0;
-		wr.wr.wr_id = encode_wr_id(SRPT_RDMA_ABORT, ioctx->ioctx.index);
-		wr.wr.send_flags = IB_SEND_SIGNALED;
-		while (ch->state == CH_LIVE &&
-			ib_post_send(ch->qp, &wr.wr, &bad_wr) != 0) {
-			pr_info("Trying to abort failed RDMA transfer [%d]\n",
-				ioctx->ioctx.index);
-			msleep(1000);
-		}
-		while (ch->state != CH_RELEASING && !ioctx->rdma_aborted) {
-			pr_info("Waiting until RDMA abort finished [%d]\n",
-				ioctx->ioctx.index);
-			msleep(1000);
-		}
-	}
 out:
 	if (unlikely(dir == DMA_TO_DEVICE && ret < 0))
 		atomic_add(n_rdma, &ch->sq_wr_avail);
@@ -3203,14 +3012,11 @@ static void srpt_add_one(struct ib_device *device)
 	init_waitqueue_head(&sdev->ch_releaseQ);
 	spin_lock_init(&sdev->spinlock);
 
-	if (ib_query_device(device, &sdev->dev_attr))
-		goto free_dev;
-
 	sdev->pd = ib_alloc_pd(device);
 	if (IS_ERR(sdev->pd))
 		goto free_dev;
 
-	sdev->srq_size = min(srpt_srq_size, sdev->dev_attr.max_srq_wr);
+	sdev->srq_size = min(srpt_srq_size, sdev->device->attrs.max_srq_wr);
 
 	srq_attr.event_handler = srpt_srq_event;
 	srq_attr.srq_context = (void *)sdev;
@@ -3224,7 +3030,7 @@ static void srpt_add_one(struct ib_device *device)
 		goto err_pd;
 
 	pr_debug("%s: create SRQ #wr= %d max_allow=%d dev= %s\n",
-		 __func__, sdev->srq_size, sdev->dev_attr.max_srq_wr,
+		 __func__, sdev->srq_size, sdev->device->attrs.max_srq_wr,
 		 device->name);
 
 	if (!srpt_service_guid)
@@ -3273,8 +3079,6 @@ static void srpt_add_one(struct ib_device *device)
 		sport->port_attrib.srp_max_rsp_size = DEFAULT_MAX_RSP_SIZE;
 		sport->port_attrib.srp_sq_size = DEF_SRPT_SQ_SIZE;
 		INIT_WORK(&sport->work, srpt_refresh_port_work);
-		INIT_LIST_HEAD(&sport->port_acl_list);
-		spin_lock_init(&sport->port_acl_lock);
 
 		if (srpt_refresh_port(sport)) {
 			pr_err("MAD registration failed for %s-%d.\n",
@@ -3508,42 +3312,15 @@ out:
  */
 static int srpt_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
 {
-	struct srpt_port *sport =
-		container_of(se_nacl->se_tpg, struct srpt_port, port_tpg_1);
-	struct srpt_node_acl *nacl =
-		container_of(se_nacl, struct srpt_node_acl, nacl);
 	u8 i_port_id[16];
 
 	if (srpt_parse_i_port_id(i_port_id, name) < 0) {
 		pr_err("invalid initiator port ID %s\n", name);
 		return -EINVAL;
 	}
-
-	memcpy(&nacl->i_port_id[0], &i_port_id[0], 16);
-	nacl->sport = sport;
-
-	spin_lock_irq(&sport->port_acl_lock);
-	list_add_tail(&nacl->list, &sport->port_acl_list);
-	spin_unlock_irq(&sport->port_acl_lock);
-
 	return 0;
 }
 
-/*
- * configfs callback function invoked for
- * rmdir /sys/kernel/config/target/$driver/$port/$tpg/acls/$i_port_id
- */
-static void srpt_cleanup_nodeacl(struct se_node_acl *se_nacl)
-{
-	struct srpt_node_acl *nacl =
-		container_of(se_nacl, struct srpt_node_acl, nacl);
-	struct srpt_port *sport = nacl->sport;
-
-	spin_lock_irq(&sport->port_acl_lock);
-	list_del(&nacl->list);
-	spin_unlock_irq(&sport->port_acl_lock);
-}
-
 static ssize_t srpt_tpg_attrib_srp_max_rdma_size_show(struct config_item *item,
 		char *page)
 {
@@ -3820,7 +3597,6 @@ static const struct target_core_fabric_ops srpt_template = {
 	.fabric_make_tpg		= srpt_make_tpg,
 	.fabric_drop_tpg		= srpt_drop_tpg,
 	.fabric_init_nodeacl		= srpt_init_nodeacl,
-	.fabric_cleanup_nodeacl		= srpt_cleanup_nodeacl,
 
 	.tfc_wwn_attrs			= srpt_wwn_attrs,
 	.tfc_tpg_base_attrs		= srpt_tpg_attrs,
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.h b/drivers/infiniband/ulp/srpt/ib_srpt.h
index 5faad8acd789..09037f2b0b51 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.h
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.h
@@ -128,36 +128,6 @@ enum {
 	DEFAULT_MAX_RDMA_SIZE = 65536,
 };
 
-enum srpt_opcode {
-	SRPT_RECV,
-	SRPT_SEND,
-	SRPT_RDMA_MID,
-	SRPT_RDMA_ABORT,
-	SRPT_RDMA_READ_LAST,
-	SRPT_RDMA_WRITE_LAST,
-};
-
-static inline u64 encode_wr_id(u8 opcode, u32 idx)
-{
-	return ((u64)opcode << 32) | idx;
-}
-static inline enum srpt_opcode opcode_from_wr_id(u64 wr_id)
-{
-	return wr_id >> 32;
-}
-static inline u32 idx_from_wr_id(u64 wr_id)
-{
-	return (u32)wr_id;
-}
-
-struct rdma_iu {
-	u64		raddr;
-	u32		rkey;
-	struct ib_sge	*sge;
-	u32		sge_cnt;
-	int		mem_id;
-};
-
 /**
  * enum srpt_command_state - SCSI command state managed by SRPT.
  * @SRPT_STATE_NEW:           New command arrived and is being processed.
@@ -189,6 +159,7 @@ enum srpt_command_state {
  * @index: Index of the I/O context in its ioctx_ring array.
  */
 struct srpt_ioctx {
+	struct ib_cqe		cqe;
 	void			*buf;
 	dma_addr_t		dma;
 	uint32_t		index;
@@ -215,32 +186,30 @@ struct srpt_recv_ioctx {
  * @sg:          Pointer to sg-list associated with this I/O context.
  * @sg_cnt:      SG-list size.
  * @mapped_sg_count: ib_dma_map_sg() return value.
- * @n_rdma_ius:  Number of elements in the rdma_ius array.
- * @rdma_ius:    Array with information about the RDMA mapping.
+ * @n_rdma_wrs:  Number of elements in the rdma_wrs array.
+ * @rdma_wrs:    Array with information about the RDMA mapping.
  * @tag:         Tag of the received SRP information unit.
  * @spinlock:    Protects 'state'.
  * @state:       I/O context state.
- * @rdma_aborted: If initiating a multipart RDMA transfer failed, whether
- * 		 the already initiated transfers have finished.
  * @cmd:         Target core command data structure.
  * @sense_data:  SCSI sense data.
  */
 struct srpt_send_ioctx {
 	struct srpt_ioctx	ioctx;
 	struct srpt_rdma_ch	*ch;
-	struct rdma_iu		*rdma_ius;
+	struct ib_rdma_wr	*rdma_wrs;
+	struct ib_cqe		rdma_cqe;
 	struct srp_direct_buf	*rbufs;
 	struct srp_direct_buf	single_rbuf;
 	struct scatterlist	*sg;
 	struct list_head	free_list;
 	spinlock_t		spinlock;
 	enum srpt_command_state	state;
-	bool			rdma_aborted;
 	struct se_cmd		cmd;
 	struct completion	tx_done;
 	int			sg_cnt;
 	int			mapped_sg_count;
-	u16			n_rdma_ius;
+	u16			n_rdma_wrs;
 	u8			n_rdma;
 	u8			n_rbuf;
 	bool			queue_status_only;
@@ -267,9 +236,6 @@ enum rdma_ch_state {
 
 /**
  * struct srpt_rdma_ch - RDMA channel.
- * @wait_queue:    Allows the kernel thread to wait for more work.
- * @thread:        Kernel thread that processes the IB queues associated with
- *                 the channel.
  * @cm_id:         IB CM ID associated with the channel.
  * @qp:            IB queue pair used for communicating over this channel.
  * @cq:            IB completion queue for this channel.
@@ -288,7 +254,6 @@ enum rdma_ch_state {
  * @free_list:     Head of list with free send I/O contexts.
  * @state:         channel state. See also enum rdma_ch_state.
  * @ioctx_ring:    Send ring.
- * @wc:            IB work completion array for srpt_process_completion().
  * @list:          Node for insertion in the srpt_device.rch_list list.
  * @cmd_wait_list: List of SCSI commands that arrived before the RTU event. This
  *                 list contains struct srpt_ioctx elements and is protected
@@ -299,8 +264,6 @@ enum rdma_ch_state {
  * @release_done:  Enables waiting for srpt_release_channel() completion.
  */
 struct srpt_rdma_ch {
-	wait_queue_head_t	wait_queue;
-	struct task_struct	*thread;
 	struct ib_cm_id		*cm_id;
 	struct ib_qp		*qp;
 	struct ib_cq		*cq;
@@ -317,7 +280,6 @@ struct srpt_rdma_ch {
 	struct list_head	free_list;
 	enum rdma_ch_state	state;
 	struct srpt_send_ioctx	**ioctx_ring;
-	struct ib_wc		wc[16];
 	struct list_head	list;
 	struct list_head	cmd_wait_list;
 	struct se_session	*sess;
@@ -364,11 +326,9 @@ struct srpt_port {
 	u16			sm_lid;
 	u16			lid;
 	union ib_gid		gid;
-	spinlock_t		port_acl_lock;
 	struct work_struct	work;
 	struct se_portal_group	port_tpg_1;
 	struct se_wwn		port_wwn;
-	struct list_head	port_acl_list;
 	struct srpt_port_attrib port_attrib;
 };
 
@@ -379,8 +339,6 @@ struct srpt_port {
  * @mr:            L_Key (local key) with write access to all local memory.
  * @srq:           Per-HCA SRQ (shared receive queue).
  * @cm_id:         Connection identifier.
- * @dev_attr:      Attributes of the InfiniBand device as obtained during the
- *                 ib_client.add() callback.
  * @srq_size:      SRQ size.
  * @ioctx_ring:    Per-HCA SRQ.
  * @rch_list:      Per-device channel list -- see also srpt_rdma_ch.list.
@@ -395,7 +353,6 @@ struct srpt_device {
 	struct ib_pd		*pd;
 	struct ib_srq		*srq;
 	struct ib_cm_id		*cm_id;
-	struct ib_device_attr	dev_attr;
 	int			srq_size;
 	struct srpt_recv_ioctx	**ioctx_ring;
 	struct list_head	rch_list;
@@ -409,15 +366,9 @@ struct srpt_device {
 /**
  * struct srpt_node_acl - Per-initiator ACL data (managed via configfs).
  * @nacl:      Target core node ACL information.
- * @i_port_id: 128-bit SRP initiator port ID.
- * @sport:     port information.
- * @list:      Element of the per-HCA ACL list.
  */
 struct srpt_node_acl {
 	struct se_node_acl	nacl;
-	u8			i_port_id[16];
-	struct srpt_port	*sport;
-	struct list_head	list;
 };
 
 #endif				/* IB_SRPT_H */
diff --git a/drivers/input/joystick/xpad.c b/drivers/input/joystick/xpad.c
index 6727954ab74b..e8a84d12b7ff 100644
--- a/drivers/input/joystick/xpad.c
+++ b/drivers/input/joystick/xpad.c
@@ -1207,7 +1207,6 @@ static void xpad_led_disconnect(struct usb_xpad *xpad)
 #else
 static int xpad_led_probe(struct usb_xpad *xpad) { return 0; }
 static void xpad_led_disconnect(struct usb_xpad *xpad) { }
-static void xpad_identify_controller(struct usb_xpad *xpad) { }
 #endif
 
 static int xpad_start_input(struct usb_xpad *xpad)
diff --git a/drivers/input/keyboard/adp5589-keys.c b/drivers/input/keyboard/adp5589-keys.c
index 4d446d5085aa..c01a1d648f9f 100644
--- a/drivers/input/keyboard/adp5589-keys.c
+++ b/drivers/input/keyboard/adp5589-keys.c
@@ -235,7 +235,7 @@ struct adp5589_kpad {
 	unsigned short gpimapsize;
 	unsigned extend_cfg;
 	bool is_adp5585;
-	bool adp5585_support_row5;
+	bool support_row5;
 #ifdef CONFIG_GPIOLIB
 	unsigned char gpiomap[ADP5589_MAXGPIO];
 	bool export_gpio;
@@ -485,7 +485,7 @@ static int adp5589_build_gpiomap(struct adp5589_kpad *kpad,
 	if (kpad->extend_cfg & C4_EXTEND_CFG)
 		pin_used[kpad->var->c4_extend_cfg] = true;
 
-	if (!kpad->adp5585_support_row5)
+	if (!kpad->support_row5)
 		pin_used[5] = true;
 
 	for (i = 0; i < kpad->var->maxgpio; i++)
@@ -884,12 +884,13 @@ static int adp5589_probe(struct i2c_client *client,
 
 	switch (id->driver_data) {
 	case ADP5585_02:
-		kpad->adp5585_support_row5 = true;
+		kpad->support_row5 = true;
 	case ADP5585_01:
 		kpad->is_adp5585 = true;
 		kpad->var = &const_adp5585;
 		break;
 	case ADP5589:
+		kpad->support_row5 = true;
 		kpad->var = &const_adp5589;
 		break;
 	}
diff --git a/drivers/input/keyboard/cap11xx.c b/drivers/input/keyboard/cap11xx.c
index 378db10001df..4401be225d64 100644
--- a/drivers/input/keyboard/cap11xx.c
+++ b/drivers/input/keyboard/cap11xx.c
@@ -304,8 +304,10 @@ static int cap11xx_init_leds(struct device *dev,
 		led->cdev.brightness = LED_OFF;
 
 		error = of_property_read_u32(child, "reg", &reg);
-		if (error != 0 || reg >= num_leds)
+		if (error != 0 || reg >= num_leds) {
+			of_node_put(child);
 			return -EINVAL;
+		}
 
 		led->reg = reg;
 		led->priv = priv;
@@ -313,8 +315,10 @@ static int cap11xx_init_leds(struct device *dev,
 		INIT_WORK(&led->work, cap11xx_led_work);
 
 		error = devm_led_classdev_register(dev, &led->cdev);
-		if (error)
+		if (error) {
+			of_node_put(child);
 			return error;
+		}
 
 		priv->num_leds++;
 		led++;
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index d6d16fa78281..1f2337abcf2f 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -733,7 +733,7 @@ config INPUT_XEN_KBDDEV_FRONTEND
 	  module will be called xen-kbdfront.
 
 config INPUT_SIRFSOC_ONKEY
-	bool "CSR SiRFSoC power on/off/suspend key support"
+	tristate "CSR SiRFSoC power on/off/suspend key support"
 	depends on ARCH_SIRF && OF
 	default y
 	help
diff --git a/drivers/input/misc/sirfsoc-onkey.c b/drivers/input/misc/sirfsoc-onkey.c
index 9d5b89befe6f..ed7237f19539 100644
--- a/drivers/input/misc/sirfsoc-onkey.c
+++ b/drivers/input/misc/sirfsoc-onkey.c
@@ -101,7 +101,7 @@ static void sirfsoc_pwrc_close(struct input_dev *input)
 static const struct of_device_id sirfsoc_pwrc_of_match[] = {
 	{ .compatible = "sirf,prima2-pwrc" },
 	{},
-}
+};
 MODULE_DEVICE_TABLE(of, sirfsoc_pwrc_of_match);
 
 static int sirfsoc_pwrc_probe(struct platform_device *pdev)
diff --git a/drivers/input/mouse/vmmouse.c b/drivers/input/mouse/vmmouse.c
index e272f06258ce..a3f0f5a47490 100644
--- a/drivers/input/mouse/vmmouse.c
+++ b/drivers/input/mouse/vmmouse.c
@@ -458,8 +458,6 @@ int vmmouse_init(struct psmouse *psmouse)
 	priv->abs_dev = abs_dev;
 	psmouse->private = priv;
 
-	input_set_capability(rel_dev, EV_REL, REL_WHEEL);
-
 	/* Set up and register absolute device */
 	snprintf(priv->phys, sizeof(priv->phys), "%s/input1",
 		 psmouse->ps2dev.serio->phys);
@@ -475,10 +473,6 @@ int vmmouse_init(struct psmouse *psmouse)
 	abs_dev->id.version = psmouse->model;
 	abs_dev->dev.parent = &psmouse->ps2dev.serio->dev;
 
-	error = input_register_device(priv->abs_dev);
-	if (error)
-		goto init_fail;
-
 	/* Set absolute device capabilities */
 	input_set_capability(abs_dev, EV_KEY, BTN_LEFT);
 	input_set_capability(abs_dev, EV_KEY, BTN_RIGHT);
@@ -488,6 +482,13 @@ int vmmouse_init(struct psmouse *psmouse)
 	input_set_abs_params(abs_dev, ABS_X, 0, VMMOUSE_MAX_X, 0, 0);
 	input_set_abs_params(abs_dev, ABS_Y, 0, VMMOUSE_MAX_Y, 0, 0);
 
+	error = input_register_device(priv->abs_dev);
+	if (error)
+		goto init_fail;
+
+	/* Add wheel capability to the relative device */
+	input_set_capability(rel_dev, EV_REL, REL_WHEEL);
+
 	psmouse->protocol_handler = vmmouse_process_byte;
 	psmouse->disconnect = vmmouse_disconnect;
 	psmouse->reconnect = vmmouse_reconnect;
diff --git a/drivers/input/serio/hyperv-keyboard.c b/drivers/input/serio/hyperv-keyboard.c
index e74e5d6e5f9f..c948866edf87 100644
--- a/drivers/input/serio/hyperv-keyboard.c
+++ b/drivers/input/serio/hyperv-keyboard.c
@@ -412,16 +412,6 @@ static int hv_kbd_remove(struct hv_device *hv_dev)
 	return 0;
 }
 
-/*
- * Keyboard GUID
- * {f912ad6d-2b17-48ea-bd65-f927a61c7684}
- */
-#define HV_KBD_GUID \
-	.guid = { \
-			0x6d, 0xad, 0x12, 0xf9, 0x17, 0x2b, 0xea, 0x48, \
-			0xbd, 0x65, 0xf9, 0x27, 0xa6, 0x1c, 0x76, 0x84 \
-	}
-
 static const struct hv_vmbus_device_id id_table[] = {
 	/* Keyboard guid */
 	{ HV_KBD_GUID, },
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index c11556563ef0..68f5f4a0f1e7 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -258,6 +258,13 @@ static const struct dmi_system_id __initconst i8042_dmi_nomux_table[] = {
 		},
 	},
 	{
+		/* Fujitsu Lifebook U745 */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK U745"),
+		},
+	},
+	{
 		/* Fujitsu T70H */
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index 8f828975ab10..1ca7f551e2da 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -134,7 +134,7 @@ static void serio_find_driver(struct serio *serio)
 	int error;
 
 	error = device_attach(&serio->dev);
-	if (error < 0)
+	if (error < 0 && error != -EPROBE_DEFER)
 		dev_warn(&serio->dev,
 			 "device_attach() failed for %s (%s), error: %d\n",
 			 serio->phys, serio->name, error);
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index c7afa57f6287..1f99e7f30e27 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -376,7 +376,7 @@ config TOUCHSCREEN_IPROC
 config TOUCHSCREEN_S3C2410
 	tristate "Samsung S3C2410/generic touchscreen input driver"
 	depends on ARCH_S3C24XX || SAMSUNG_DEV_TS
-	select S3C_ADC
+	depends on S3C_ADC
 	help
 	  Say Y here if you have the s3c2410 touchscreen.
 
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index cd013434473d..69d299d5dd00 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -454,7 +454,7 @@ static int ad7879_gpio_add(struct ad7879 *ts,
 		ts->gc.ngpio = 1;
 		ts->gc.label = "AD7879-GPIO";
 		ts->gc.owner = THIS_MODULE;
-		ts->gc.dev = ts->dev;
+		ts->gc.parent = ts->dev;
 
 		ret = gpiochip_add(&ts->gc);
 		if (ret)
diff --git a/drivers/input/touchscreen/colibri-vf50-ts.c b/drivers/input/touchscreen/colibri-vf50-ts.c
index 5d4903a402cc..69828d015d45 100644
--- a/drivers/input/touchscreen/colibri-vf50-ts.c
+++ b/drivers/input/touchscreen/colibri-vf50-ts.c
@@ -21,6 +21,7 @@
 #include <linux/interrupt.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index 0b0f8c17f3f7..23fbe382da8b 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -822,16 +822,22 @@ static void edt_ft5x06_ts_get_defaults(struct device *dev,
 	int error;
 
 	error = device_property_read_u32(dev, "threshold", &val);
-	if (!error)
-		reg_addr->reg_threshold = val;
+	if (!error) {
+		edt_ft5x06_register_write(tsdata, reg_addr->reg_threshold, val);
+		tsdata->threshold = val;
+	}
 
 	error = device_property_read_u32(dev, "gain", &val);
-	if (!error)
-		reg_addr->reg_gain = val;
+	if (!error) {
+		edt_ft5x06_register_write(tsdata, reg_addr->reg_gain, val);
+		tsdata->gain = val;
+	}
 
 	error = device_property_read_u32(dev, "offset", &val);
-	if (!error)
-		reg_addr->reg_offset = val;
+	if (!error) {
+		edt_ft5x06_register_write(tsdata, reg_addr->reg_offset, val);
+		tsdata->offset = val;
+	}
 }
 
 static void
diff --git a/drivers/input/touchscreen/sur40.c b/drivers/input/touchscreen/sur40.c
index d214f22ed305..b6c4d03de340 100644
--- a/drivers/input/touchscreen/sur40.c
+++ b/drivers/input/touchscreen/sur40.c
@@ -444,7 +444,7 @@ static void sur40_process_video(struct sur40_state *sur40)
 		goto err_poll;
 
 	/* mark as finished */
-	v4l2_get_timestamp(&new_buf->vb.timestamp);
+	new_buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	new_buf->vb.sequence = sur40->sequence++;
 	new_buf->vb.field = V4L2_FIELD_NONE;
 	vb2_buffer_done(&new_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -644,22 +644,21 @@ static void sur40_disconnect(struct usb_interface *interface)
  * minimum number: many DMA engines need a minimum of 2 buffers in the
  * queue and you need to have another available for userspace processing.
  */
-static int sur40_queue_setup(struct vb2_queue *q, const void *parg,
+static int sur40_queue_setup(struct vb2_queue *q,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct sur40_state *sur40 = vb2_get_drv_priv(q);
 
 	if (q->num_buffers + *nbuffers < 3)
 		*nbuffers = 3 - q->num_buffers;
+	alloc_ctxs[0] = sur40->alloc_ctx;
 
-	if (fmt && fmt->fmt.pix.sizeimage < sur40_video_format.sizeimage)
-		return -EINVAL;
+	if (*nplanes)
+		return sizes[0] < sur40_video_format.sizeimage ? -EINVAL : 0;
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : sur40_video_format.sizeimage;
-	alloc_ctxs[0] = sur40->alloc_ctx;
+	sizes[0] = sur40_video_format.sizeimage;
 
 	return 0;
 }
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index b9094e9da537..a1e75cba18e0 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -263,81 +263,6 @@ config EXYNOS_IOMMU_DEBUG
 
 	  Say N unless you need kernel log message for IOMMU debugging.
 
-config SHMOBILE_IPMMU
-	bool
-
-config SHMOBILE_IPMMU_TLB
-	bool
-
-config SHMOBILE_IOMMU
-	bool "IOMMU for Renesas IPMMU/IPMMUI"
-	default n
-	depends on ARM && MMU
-	depends on ARCH_SHMOBILE || COMPILE_TEST
-	select IOMMU_API
-	select ARM_DMA_USE_IOMMU
-	select SHMOBILE_IPMMU
-	select SHMOBILE_IPMMU_TLB
-	help
-	  Support for Renesas IPMMU/IPMMUI. This option enables
-	  remapping of DMA memory accesses from all of the IP blocks
-	  on the ICB.
-
-	  Warning: Drivers (including userspace drivers of UIO
-	  devices) of the IP blocks on the ICB *must* use addresses
-	  allocated from the IPMMU (iova) for DMA with this option
-	  enabled.
-
-	  If unsure, say N.
-
-choice
-	prompt "IPMMU/IPMMUI address space size"
-	default SHMOBILE_IOMMU_ADDRSIZE_2048MB
-	depends on SHMOBILE_IOMMU
-	help
-	  This option sets IPMMU/IPMMUI address space size by
-	  adjusting the 1st level page table size. The page table size
-	  is calculated as follows:
-
-	      page table size = number of page table entries * 4 bytes
-	      number of page table entries = address space size / 1 MiB
-
-	  For example, when the address space size is 2048 MiB, the
-	  1st level page table size is 8192 bytes.
-
-	config SHMOBILE_IOMMU_ADDRSIZE_2048MB
-		bool "2 GiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_1024MB
-		bool "1 GiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_512MB
-		bool "512 MiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_256MB
-		bool "256 MiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_128MB
-		bool "128 MiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_64MB
-		bool "64 MiB"
-
-	config SHMOBILE_IOMMU_ADDRSIZE_32MB
-		bool "32 MiB"
-
-endchoice
-
-config SHMOBILE_IOMMU_L1SIZE
-	int
-	default 8192 if SHMOBILE_IOMMU_ADDRSIZE_2048MB
-	default 4096 if SHMOBILE_IOMMU_ADDRSIZE_1024MB
-	default 2048 if SHMOBILE_IOMMU_ADDRSIZE_512MB
-	default 1024 if SHMOBILE_IOMMU_ADDRSIZE_256MB
-	default 512 if SHMOBILE_IOMMU_ADDRSIZE_128MB
-	default 256 if SHMOBILE_IOMMU_ADDRSIZE_64MB
-	default 128 if SHMOBILE_IOMMU_ADDRSIZE_32MB
-
 config IPMMU_VMSA
 	bool "Renesas VMSA-compatible IPMMU"
 	depends on ARM_LPAE
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index 68faca02225d..42fc0c25cf1a 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -22,7 +22,5 @@ obj-$(CONFIG_ROCKCHIP_IOMMU) += rockchip-iommu.o
 obj-$(CONFIG_TEGRA_IOMMU_GART) += tegra-gart.o
 obj-$(CONFIG_TEGRA_IOMMU_SMMU) += tegra-smmu.o
 obj-$(CONFIG_EXYNOS_IOMMU) += exynos-iommu.o
-obj-$(CONFIG_SHMOBILE_IOMMU) += shmobile-iommu.o
-obj-$(CONFIG_SHMOBILE_IPMMU) += shmobile-ipmmu.o
 obj-$(CONFIG_FSL_PAMU) += fsl_pamu.o fsl_pamu_domain.o
 obj-$(CONFIG_S390_IOMMU) += s390-iommu.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 8b2be1e7714f..374c129219ef 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -35,6 +35,7 @@
 #include <linux/msi.h>
 #include <linux/dma-contiguous.h>
 #include <linux/irqdomain.h>
+#include <linux/percpu.h>
 #include <asm/irq_remapping.h>
 #include <asm/io_apic.h>
 #include <asm/apic.h>
@@ -113,6 +114,46 @@ struct kmem_cache *amd_iommu_irq_cache;
 
 static void update_domain(struct protection_domain *domain);
 static int protection_domain_init(struct protection_domain *domain);
+static void detach_device(struct device *dev);
+
+/*
+ * For dynamic growth the aperture size is split into ranges of 128MB of
+ * DMA address space each. This struct represents one such range.
+ */
+struct aperture_range {
+
+	spinlock_t bitmap_lock;
+
+	/* address allocation bitmap */
+	unsigned long *bitmap;
+	unsigned long offset;
+	unsigned long next_bit;
+
+	/*
+	 * Array of PTE pages for the aperture. In this array we save all the
+	 * leaf pages of the domain page table used for the aperture. This way
+	 * we don't need to walk the page table to find a specific PTE. We can
+	 * just calculate its address in constant time.
+	 */
+	u64 *pte_pages[64];
+};
+
+/*
+ * Data container for a dma_ops specific protection domain
+ */
+struct dma_ops_domain {
+	/* generic protection domain information */
+	struct protection_domain domain;
+
+	/* size of the aperture for the mappings */
+	unsigned long aperture_size;
+
+	/* aperture index we start searching for free addresses */
+	u32 __percpu *next_index;
+
+	/* address space relevant data */
+	struct aperture_range *aperture[APERTURE_MAX_RANGES];
+};
 
 /****************************************************************************
  *
@@ -344,6 +385,9 @@ static void iommu_uninit_device(struct device *dev)
 	if (!dev_data)
 		return;
 
+	if (dev_data->domain)
+		detach_device(dev);
+
 	iommu_device_unlink(amd_iommu_rlookup_table[dev_data->devid]->iommu_dev,
 			    dev);
 
@@ -1167,11 +1211,21 @@ static u64 *alloc_pte(struct protection_domain *domain,
 	end_lvl = PAGE_SIZE_LEVEL(page_size);
 
 	while (level > end_lvl) {
-		if (!IOMMU_PTE_PRESENT(*pte)) {
+		u64 __pte, __npte;
+
+		__pte = *pte;
+
+		if (!IOMMU_PTE_PRESENT(__pte)) {
 			page = (u64 *)get_zeroed_page(gfp);
 			if (!page)
 				return NULL;
-			*pte = PM_LEVEL_PDE(level, virt_to_phys(page));
+
+			__npte = PM_LEVEL_PDE(level, virt_to_phys(page));
+
+			if (cmpxchg64(pte, __pte, __npte)) {
+				free_page((unsigned long)page);
+				continue;
+			}
 		}
 
 		/* No level skipping support yet */
@@ -1376,8 +1430,10 @@ static int alloc_new_range(struct dma_ops_domain *dma_dom,
 			   bool populate, gfp_t gfp)
 {
 	int index = dma_dom->aperture_size >> APERTURE_RANGE_SHIFT;
-	struct amd_iommu *iommu;
 	unsigned long i, old_size, pte_pgsize;
+	struct aperture_range *range;
+	struct amd_iommu *iommu;
+	unsigned long flags;
 
 #ifdef CONFIG_IOMMU_STRESS
 	populate = false;
@@ -1386,15 +1442,17 @@ static int alloc_new_range(struct dma_ops_domain *dma_dom,
 	if (index >= APERTURE_MAX_RANGES)
 		return -ENOMEM;
 
-	dma_dom->aperture[index] = kzalloc(sizeof(struct aperture_range), gfp);
-	if (!dma_dom->aperture[index])
+	range = kzalloc(sizeof(struct aperture_range), gfp);
+	if (!range)
 		return -ENOMEM;
 
-	dma_dom->aperture[index]->bitmap = (void *)get_zeroed_page(gfp);
-	if (!dma_dom->aperture[index]->bitmap)
+	range->bitmap = (void *)get_zeroed_page(gfp);
+	if (!range->bitmap)
 		goto out_free;
 
-	dma_dom->aperture[index]->offset = dma_dom->aperture_size;
+	range->offset = dma_dom->aperture_size;
+
+	spin_lock_init(&range->bitmap_lock);
 
 	if (populate) {
 		unsigned long address = dma_dom->aperture_size;
@@ -1407,14 +1465,20 @@ static int alloc_new_range(struct dma_ops_domain *dma_dom,
 			if (!pte)
 				goto out_free;
 
-			dma_dom->aperture[index]->pte_pages[i] = pte_page;
+			range->pte_pages[i] = pte_page;
 
 			address += APERTURE_RANGE_SIZE / 64;
 		}
 	}
 
-	old_size                = dma_dom->aperture_size;
-	dma_dom->aperture_size += APERTURE_RANGE_SIZE;
+	spin_lock_irqsave(&dma_dom->domain.lock, flags);
+
+	/* First take the bitmap_lock and then publish the range */
+	spin_lock(&range->bitmap_lock);
+
+	old_size                 = dma_dom->aperture_size;
+	dma_dom->aperture[index] = range;
+	dma_dom->aperture_size  += APERTURE_RANGE_SIZE;
 
 	/* Reserve address range used for MSI messages */
 	if (old_size < MSI_ADDR_BASE_LO &&
@@ -1461,62 +1525,123 @@ static int alloc_new_range(struct dma_ops_domain *dma_dom,
 
 	update_domain(&dma_dom->domain);
 
+	spin_unlock(&range->bitmap_lock);
+
+	spin_unlock_irqrestore(&dma_dom->domain.lock, flags);
+
 	return 0;
 
 out_free:
 	update_domain(&dma_dom->domain);
 
-	free_page((unsigned long)dma_dom->aperture[index]->bitmap);
+	free_page((unsigned long)range->bitmap);
 
-	kfree(dma_dom->aperture[index]);
-	dma_dom->aperture[index] = NULL;
+	kfree(range);
 
 	return -ENOMEM;
 }
 
+static dma_addr_t dma_ops_aperture_alloc(struct dma_ops_domain *dom,
+					 struct aperture_range *range,
+					 unsigned long pages,
+					 unsigned long dma_mask,
+					 unsigned long boundary_size,
+					 unsigned long align_mask,
+					 bool trylock)
+{
+	unsigned long offset, limit, flags;
+	dma_addr_t address;
+	bool flush = false;
+
+	offset = range->offset >> PAGE_SHIFT;
+	limit  = iommu_device_max_index(APERTURE_RANGE_PAGES, offset,
+					dma_mask >> PAGE_SHIFT);
+
+	if (trylock) {
+		if (!spin_trylock_irqsave(&range->bitmap_lock, flags))
+			return -1;
+	} else {
+		spin_lock_irqsave(&range->bitmap_lock, flags);
+	}
+
+	address = iommu_area_alloc(range->bitmap, limit, range->next_bit,
+				   pages, offset, boundary_size, align_mask);
+	if (address == -1) {
+		/* Nothing found, retry one time */
+		address = iommu_area_alloc(range->bitmap, limit,
+					   0, pages, offset, boundary_size,
+					   align_mask);
+		flush = true;
+	}
+
+	if (address != -1)
+		range->next_bit = address + pages;
+
+	spin_unlock_irqrestore(&range->bitmap_lock, flags);
+
+	if (flush) {
+		domain_flush_tlb(&dom->domain);
+		domain_flush_complete(&dom->domain);
+	}
+
+	return address;
+}
+
 static unsigned long dma_ops_area_alloc(struct device *dev,
 					struct dma_ops_domain *dom,
 					unsigned int pages,
 					unsigned long align_mask,
-					u64 dma_mask,
-					unsigned long start)
+					u64 dma_mask)
 {
-	unsigned long next_bit = dom->next_address % APERTURE_RANGE_SIZE;
-	int max_index = dom->aperture_size >> APERTURE_RANGE_SHIFT;
-	int i = start >> APERTURE_RANGE_SHIFT;
 	unsigned long boundary_size, mask;
 	unsigned long address = -1;
-	unsigned long limit;
+	bool first = true;
+	u32 start, i;
 
-	next_bit >>= PAGE_SHIFT;
+	preempt_disable();
 
 	mask = dma_get_seg_boundary(dev);
 
+again:
+	start = this_cpu_read(*dom->next_index);
+
+	/* Sanity check - is it really necessary? */
+	if (unlikely(start > APERTURE_MAX_RANGES)) {
+		start = 0;
+		this_cpu_write(*dom->next_index, 0);
+	}
+
 	boundary_size = mask + 1 ? ALIGN(mask + 1, PAGE_SIZE) >> PAGE_SHIFT :
 				   1UL << (BITS_PER_LONG - PAGE_SHIFT);
 
-	for (;i < max_index; ++i) {
-		unsigned long offset = dom->aperture[i]->offset >> PAGE_SHIFT;
+	for (i = 0; i < APERTURE_MAX_RANGES; ++i) {
+		struct aperture_range *range;
+		int index;
 
-		if (dom->aperture[i]->offset >= dma_mask)
-			break;
+		index = (start + i) % APERTURE_MAX_RANGES;
 
-		limit = iommu_device_max_index(APERTURE_RANGE_PAGES, offset,
-					       dma_mask >> PAGE_SHIFT);
+		range = dom->aperture[index];
 
-		address = iommu_area_alloc(dom->aperture[i]->bitmap,
-					   limit, next_bit, pages, 0,
-					    boundary_size, align_mask);
+		if (!range || range->offset >= dma_mask)
+			continue;
+
+		address = dma_ops_aperture_alloc(dom, range, pages,
+						 dma_mask, boundary_size,
+						 align_mask, first);
 		if (address != -1) {
-			address = dom->aperture[i]->offset +
-				  (address << PAGE_SHIFT);
-			dom->next_address = address + (pages << PAGE_SHIFT);
+			address = range->offset + (address << PAGE_SHIFT);
+			this_cpu_write(*dom->next_index, index);
 			break;
 		}
+	}
 
-		next_bit = 0;
+	if (address == -1 && first) {
+		first = false;
+		goto again;
 	}
 
+	preempt_enable();
+
 	return address;
 }
 
@@ -1526,21 +1651,14 @@ static unsigned long dma_ops_alloc_addresses(struct device *dev,
 					     unsigned long align_mask,
 					     u64 dma_mask)
 {
-	unsigned long address;
-
-#ifdef CONFIG_IOMMU_STRESS
-	dom->next_address = 0;
-	dom->need_flush = true;
-#endif
+	unsigned long address = -1;
 
-	address = dma_ops_area_alloc(dev, dom, pages, align_mask,
-				     dma_mask, dom->next_address);
+	while (address == -1) {
+		address = dma_ops_area_alloc(dev, dom, pages,
+					     align_mask, dma_mask);
 
-	if (address == -1) {
-		dom->next_address = 0;
-		address = dma_ops_area_alloc(dev, dom, pages, align_mask,
-					     dma_mask, 0);
-		dom->need_flush = true;
+		if (address == -1 && alloc_new_range(dom, false, GFP_ATOMIC))
+			break;
 	}
 
 	if (unlikely(address == -1))
@@ -1562,6 +1680,7 @@ static void dma_ops_free_addresses(struct dma_ops_domain *dom,
 {
 	unsigned i = address >> APERTURE_RANGE_SHIFT;
 	struct aperture_range *range = dom->aperture[i];
+	unsigned long flags;
 
 	BUG_ON(i >= APERTURE_MAX_RANGES || range == NULL);
 
@@ -1570,12 +1689,18 @@ static void dma_ops_free_addresses(struct dma_ops_domain *dom,
 		return;
 #endif
 
-	if (address >= dom->next_address)
-		dom->need_flush = true;
+	if (amd_iommu_unmap_flush) {
+		domain_flush_tlb(&dom->domain);
+		domain_flush_complete(&dom->domain);
+	}
 
 	address = (address % APERTURE_RANGE_SIZE) >> PAGE_SHIFT;
 
+	spin_lock_irqsave(&range->bitmap_lock, flags);
+	if (address + pages > range->next_bit)
+		range->next_bit = address + pages;
 	bitmap_clear(range->bitmap, address, pages);
+	spin_unlock_irqrestore(&range->bitmap_lock, flags);
 
 }
 
@@ -1755,6 +1880,8 @@ static void dma_ops_domain_free(struct dma_ops_domain *dom)
 	if (!dom)
 		return;
 
+	free_percpu(dom->next_index);
+
 	del_domain_from_list(&dom->domain);
 
 	free_pagetable(&dom->domain);
@@ -1769,6 +1896,23 @@ static void dma_ops_domain_free(struct dma_ops_domain *dom)
 	kfree(dom);
 }
 
+static int dma_ops_domain_alloc_apertures(struct dma_ops_domain *dma_dom,
+					  int max_apertures)
+{
+	int ret, i, apertures;
+
+	apertures = dma_dom->aperture_size >> APERTURE_RANGE_SHIFT;
+	ret       = 0;
+
+	for (i = apertures; i < max_apertures; ++i) {
+		ret = alloc_new_range(dma_dom, false, GFP_KERNEL);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
 /*
  * Allocates a new protection domain usable for the dma_ops functions.
  * It also initializes the page table and the address allocator data
@@ -1777,6 +1921,7 @@ static void dma_ops_domain_free(struct dma_ops_domain *dom)
 static struct dma_ops_domain *dma_ops_domain_alloc(void)
 {
 	struct dma_ops_domain *dma_dom;
+	int cpu;
 
 	dma_dom = kzalloc(sizeof(struct dma_ops_domain), GFP_KERNEL);
 	if (!dma_dom)
@@ -1785,6 +1930,10 @@ static struct dma_ops_domain *dma_ops_domain_alloc(void)
 	if (protection_domain_init(&dma_dom->domain))
 		goto free_dma_dom;
 
+	dma_dom->next_index = alloc_percpu(u32);
+	if (!dma_dom->next_index)
+		goto free_dma_dom;
+
 	dma_dom->domain.mode = PAGE_MODE_2_LEVEL;
 	dma_dom->domain.pt_root = (void *)get_zeroed_page(GFP_KERNEL);
 	dma_dom->domain.flags = PD_DMA_OPS_MASK;
@@ -1792,8 +1941,6 @@ static struct dma_ops_domain *dma_ops_domain_alloc(void)
 	if (!dma_dom->domain.pt_root)
 		goto free_dma_dom;
 
-	dma_dom->need_flush = false;
-
 	add_domain_to_list(&dma_dom->domain);
 
 	if (alloc_new_range(dma_dom, true, GFP_KERNEL))
@@ -1804,8 +1951,9 @@ static struct dma_ops_domain *dma_ops_domain_alloc(void)
 	 * a valid dma-address. So we can use 0 as error value
 	 */
 	dma_dom->aperture[0]->bitmap[0] = 1;
-	dma_dom->next_address = 0;
 
+	for_each_possible_cpu(cpu)
+		*per_cpu_ptr(dma_dom->next_index, cpu) = 0;
 
 	return dma_dom;
 
@@ -1905,7 +2053,7 @@ static void do_attach(struct iommu_dev_data *dev_data,
 	/* Update device table */
 	set_dte_entry(dev_data->devid, domain, ats);
 	if (alias != dev_data->devid)
-		set_dte_entry(dev_data->devid, domain, ats);
+		set_dte_entry(alias, domain, ats);
 
 	device_flush_dte(dev_data);
 }
@@ -2328,7 +2476,7 @@ static dma_addr_t dma_ops_domain_map(struct dma_ops_domain *dom,
 	else if (direction == DMA_BIDIRECTIONAL)
 		__pte |= IOMMU_PTE_IR | IOMMU_PTE_IW;
 
-	WARN_ON(*pte);
+	WARN_ON_ONCE(*pte);
 
 	*pte = __pte;
 
@@ -2357,7 +2505,7 @@ static void dma_ops_domain_unmap(struct dma_ops_domain *dom,
 
 	pte += PM_LEVEL_INDEX(0, address);
 
-	WARN_ON(!*pte);
+	WARN_ON_ONCE(!*pte);
 
 	*pte = 0ULL;
 }
@@ -2393,26 +2541,11 @@ static dma_addr_t __map_single(struct device *dev,
 	if (align)
 		align_mask = (1UL << get_order(size)) - 1;
 
-retry:
 	address = dma_ops_alloc_addresses(dev, dma_dom, pages, align_mask,
 					  dma_mask);
-	if (unlikely(address == DMA_ERROR_CODE)) {
-		/*
-		 * setting next_address here will let the address
-		 * allocator only scan the new allocated range in the
-		 * first run. This is a small optimization.
-		 */
-		dma_dom->next_address = dma_dom->aperture_size;
-
-		if (alloc_new_range(dma_dom, false, GFP_ATOMIC))
-			goto out;
 
-		/*
-		 * aperture was successfully enlarged by 128 MB, try
-		 * allocation again
-		 */
-		goto retry;
-	}
+	if (address == DMA_ERROR_CODE)
+		goto out;
 
 	start = address;
 	for (i = 0; i < pages; ++i) {
@@ -2427,11 +2560,10 @@ retry:
 
 	ADD_STATS_COUNTER(alloced_io_mem, size);
 
-	if (unlikely(dma_dom->need_flush && !amd_iommu_unmap_flush)) {
-		domain_flush_tlb(&dma_dom->domain);
-		dma_dom->need_flush = false;
-	} else if (unlikely(amd_iommu_np_cache))
+	if (unlikely(amd_iommu_np_cache)) {
 		domain_flush_pages(&dma_dom->domain, address, size);
+		domain_flush_complete(&dma_dom->domain);
+	}
 
 out:
 	return address;
@@ -2478,11 +2610,6 @@ static void __unmap_single(struct dma_ops_domain *dma_dom,
 	SUB_STATS_COUNTER(alloced_io_mem, size);
 
 	dma_ops_free_addresses(dma_dom, dma_addr, pages);
-
-	if (amd_iommu_unmap_flush || dma_dom->need_flush) {
-		domain_flush_pages(&dma_dom->domain, flush_addr, size);
-		dma_dom->need_flush = false;
-	}
 }
 
 /*
@@ -2493,11 +2620,9 @@ static dma_addr_t map_page(struct device *dev, struct page *page,
 			   enum dma_data_direction dir,
 			   struct dma_attrs *attrs)
 {
-	unsigned long flags;
+	phys_addr_t paddr = page_to_phys(page) + offset;
 	struct protection_domain *domain;
-	dma_addr_t addr;
 	u64 dma_mask;
-	phys_addr_t paddr = page_to_phys(page) + offset;
 
 	INC_STATS_COUNTER(cnt_map_single);
 
@@ -2509,19 +2634,8 @@ static dma_addr_t map_page(struct device *dev, struct page *page,
 
 	dma_mask = *dev->dma_mask;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
-	addr = __map_single(dev, domain->priv, paddr, size, dir, false,
+	return __map_single(dev, domain->priv, paddr, size, dir, false,
 			    dma_mask);
-	if (addr == DMA_ERROR_CODE)
-		goto out;
-
-	domain_flush_complete(domain);
-
-out:
-	spin_unlock_irqrestore(&domain->lock, flags);
-
-	return addr;
 }
 
 /*
@@ -2530,7 +2644,6 @@ out:
 static void unmap_page(struct device *dev, dma_addr_t dma_addr, size_t size,
 		       enum dma_data_direction dir, struct dma_attrs *attrs)
 {
-	unsigned long flags;
 	struct protection_domain *domain;
 
 	INC_STATS_COUNTER(cnt_unmap_single);
@@ -2539,13 +2652,7 @@ static void unmap_page(struct device *dev, dma_addr_t dma_addr, size_t size,
 	if (IS_ERR(domain))
 		return;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
 	__unmap_single(domain->priv, dma_addr, size, dir);
-
-	domain_flush_complete(domain);
-
-	spin_unlock_irqrestore(&domain->lock, flags);
 }
 
 /*
@@ -2556,7 +2663,6 @@ static int map_sg(struct device *dev, struct scatterlist *sglist,
 		  int nelems, enum dma_data_direction dir,
 		  struct dma_attrs *attrs)
 {
-	unsigned long flags;
 	struct protection_domain *domain;
 	int i;
 	struct scatterlist *s;
@@ -2572,8 +2678,6 @@ static int map_sg(struct device *dev, struct scatterlist *sglist,
 
 	dma_mask = *dev->dma_mask;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
 	for_each_sg(sglist, s, nelems, i) {
 		paddr = sg_phys(s);
 
@@ -2588,12 +2692,8 @@ static int map_sg(struct device *dev, struct scatterlist *sglist,
 			goto unmap;
 	}
 
-	domain_flush_complete(domain);
-
-out:
-	spin_unlock_irqrestore(&domain->lock, flags);
-
 	return mapped_elems;
+
 unmap:
 	for_each_sg(sglist, s, mapped_elems, i) {
 		if (s->dma_address)
@@ -2602,9 +2702,7 @@ unmap:
 		s->dma_address = s->dma_length = 0;
 	}
 
-	mapped_elems = 0;
-
-	goto out;
+	return 0;
 }
 
 /*
@@ -2615,7 +2713,6 @@ static void unmap_sg(struct device *dev, struct scatterlist *sglist,
 		     int nelems, enum dma_data_direction dir,
 		     struct dma_attrs *attrs)
 {
-	unsigned long flags;
 	struct protection_domain *domain;
 	struct scatterlist *s;
 	int i;
@@ -2626,17 +2723,11 @@ static void unmap_sg(struct device *dev, struct scatterlist *sglist,
 	if (IS_ERR(domain))
 		return;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
 	for_each_sg(sglist, s, nelems, i) {
 		__unmap_single(domain->priv, s->dma_address,
 			       s->dma_length, dir);
 		s->dma_address = s->dma_length = 0;
 	}
-
-	domain_flush_complete(domain);
-
-	spin_unlock_irqrestore(&domain->lock, flags);
 }
 
 /*
@@ -2648,7 +2739,6 @@ static void *alloc_coherent(struct device *dev, size_t size,
 {
 	u64 dma_mask = dev->coherent_dma_mask;
 	struct protection_domain *domain;
-	unsigned long flags;
 	struct page *page;
 
 	INC_STATS_COUNTER(cnt_alloc_coherent);
@@ -2680,19 +2770,11 @@ static void *alloc_coherent(struct device *dev, size_t size,
 	if (!dma_mask)
 		dma_mask = *dev->dma_mask;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
 	*dma_addr = __map_single(dev, domain->priv, page_to_phys(page),
 				 size, DMA_BIDIRECTIONAL, true, dma_mask);
 
-	if (*dma_addr == DMA_ERROR_CODE) {
-		spin_unlock_irqrestore(&domain->lock, flags);
+	if (*dma_addr == DMA_ERROR_CODE)
 		goto out_free;
-	}
-
-	domain_flush_complete(domain);
-
-	spin_unlock_irqrestore(&domain->lock, flags);
 
 	return page_address(page);
 
@@ -2712,7 +2794,6 @@ static void free_coherent(struct device *dev, size_t size,
 			  struct dma_attrs *attrs)
 {
 	struct protection_domain *domain;
-	unsigned long flags;
 	struct page *page;
 
 	INC_STATS_COUNTER(cnt_free_coherent);
@@ -2724,14 +2805,8 @@ static void free_coherent(struct device *dev, size_t size,
 	if (IS_ERR(domain))
 		goto free_mem;
 
-	spin_lock_irqsave(&domain->lock, flags);
-
 	__unmap_single(domain->priv, dma_addr, size, DMA_BIDIRECTIONAL);
 
-	domain_flush_complete(domain);
-
-	spin_unlock_irqrestore(&domain->lock, flags);
-
 free_mem:
 	if (!dma_release_from_contiguous(dev, page, size >> PAGE_SHIFT))
 		__free_pages(page, get_order(size));
@@ -2746,14 +2821,43 @@ static int amd_iommu_dma_supported(struct device *dev, u64 mask)
 	return check_device(dev);
 }
 
+static int set_dma_mask(struct device *dev, u64 mask)
+{
+	struct protection_domain *domain;
+	int max_apertures = 1;
+
+	domain = get_domain(dev);
+	if (IS_ERR(domain))
+		return PTR_ERR(domain);
+
+	if (mask == DMA_BIT_MASK(64))
+		max_apertures = 8;
+	else if (mask > DMA_BIT_MASK(32))
+		max_apertures = 4;
+
+	/*
+	 * To prevent lock contention it doesn't make sense to allocate more
+	 * apertures than online cpus
+	 */
+	if (max_apertures > num_online_cpus())
+		max_apertures = num_online_cpus();
+
+	if (dma_ops_domain_alloc_apertures(domain->priv, max_apertures))
+		dev_err(dev, "Can't allocate %d iommu apertures\n",
+			max_apertures);
+
+	return 0;
+}
+
 static struct dma_map_ops amd_iommu_dma_ops = {
-	.alloc = alloc_coherent,
-	.free = free_coherent,
-	.map_page = map_page,
-	.unmap_page = unmap_page,
-	.map_sg = map_sg,
-	.unmap_sg = unmap_sg,
-	.dma_supported = amd_iommu_dma_supported,
+	.alloc		= alloc_coherent,
+	.free		= free_coherent,
+	.map_page	= map_page,
+	.unmap_page	= unmap_page,
+	.map_sg		= map_sg,
+	.unmap_sg	= unmap_sg,
+	.dma_supported	= amd_iommu_dma_supported,
+	.set_dma_mask	= set_dma_mask,
 };
 
 int __init amd_iommu_init_api(void)
@@ -3757,11 +3861,9 @@ static struct irq_domain *get_irq_domain(struct irq_alloc_info *info)
 	case X86_IRQ_ALLOC_TYPE_MSI:
 	case X86_IRQ_ALLOC_TYPE_MSIX:
 		devid = get_device_id(&info->msi_dev->dev);
-		if (devid >= 0) {
-			iommu = amd_iommu_rlookup_table[devid];
-			if (iommu)
-				return iommu->msi_domain;
-		}
+		iommu = amd_iommu_rlookup_table[devid];
+		if (iommu)
+			return iommu->msi_domain;
 		break;
 	default:
 		break;
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 013bdfff2d4d..bf4959f4225b 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -228,6 +228,10 @@ static int amd_iommu_enable_interrupts(void);
 static int __init iommu_go_to_state(enum iommu_init_state state);
 static void init_device_table_dma(void);
 
+static int iommu_pc_get_set_reg_val(struct amd_iommu *iommu,
+				    u8 bank, u8 cntr, u8 fxn,
+				    u64 *value, bool is_write);
+
 static inline void update_last_devid(u16 devid)
 {
 	if (devid > amd_iommu_last_bdf)
@@ -1016,6 +1020,34 @@ static void amd_iommu_erratum_746_workaround(struct amd_iommu *iommu)
 }
 
 /*
+ * Family15h Model 30h-3fh (IOMMU Mishandles ATS Write Permission)
+ * Workaround:
+ *     BIOS should enable ATS write permission check by setting
+ *     L2_DEBUG_3[AtsIgnoreIWDis](D0F2xF4_x47[0]) = 1b
+ */
+static void amd_iommu_ats_write_check_workaround(struct amd_iommu *iommu)
+{
+	u32 value;
+
+	if ((boot_cpu_data.x86 != 0x15) ||
+	    (boot_cpu_data.x86_model < 0x30) ||
+	    (boot_cpu_data.x86_model > 0x3f))
+		return;
+
+	/* Test L2_DEBUG_3[AtsIgnoreIWDis] == 1 */
+	value = iommu_read_l2(iommu, 0x47);
+
+	if (value & BIT(0))
+		return;
+
+	/* Set L2_DEBUG_3[AtsIgnoreIWDis] = 1 */
+	iommu_write_l2(iommu, 0x47, value | BIT(0));
+
+	pr_info("AMD-Vi: Applying ATS write check workaround for IOMMU at %s\n",
+		dev_name(&iommu->dev->dev));
+}
+
+/*
  * This function clues the initialization function for one IOMMU
  * together and also allocates the command buffer and programs the
  * hardware. It does NOT enable the IOMMU. This is done afterwards.
@@ -1142,8 +1174,8 @@ static void init_iommu_perf_ctr(struct amd_iommu *iommu)
 	amd_iommu_pc_present = true;
 
 	/* Check if the performance counters can be written to */
-	if ((0 != amd_iommu_pc_get_set_reg_val(0, 0, 0, 0, &val, true)) ||
-	    (0 != amd_iommu_pc_get_set_reg_val(0, 0, 0, 0, &val2, false)) ||
+	if ((0 != iommu_pc_get_set_reg_val(iommu, 0, 0, 0, &val, true)) ||
+	    (0 != iommu_pc_get_set_reg_val(iommu, 0, 0, 0, &val2, false)) ||
 	    (val != val2)) {
 		pr_err("AMD-Vi: Unable to write to IOMMU perf counter.\n");
 		amd_iommu_pc_present = false;
@@ -1284,6 +1316,7 @@ static int iommu_init_pci(struct amd_iommu *iommu)
 	}
 
 	amd_iommu_erratum_746_workaround(iommu);
+	amd_iommu_ats_write_check_workaround(iommu);
 
 	iommu->iommu_dev = iommu_device_create(&iommu->dev->dev, iommu,
 					       amd_iommu_groups, "ivhd%d",
@@ -2283,22 +2316,15 @@ u8 amd_iommu_pc_get_max_counters(u16 devid)
 }
 EXPORT_SYMBOL(amd_iommu_pc_get_max_counters);
 
-int amd_iommu_pc_get_set_reg_val(u16 devid, u8 bank, u8 cntr, u8 fxn,
+static int iommu_pc_get_set_reg_val(struct amd_iommu *iommu,
+				    u8 bank, u8 cntr, u8 fxn,
 				    u64 *value, bool is_write)
 {
-	struct amd_iommu *iommu;
 	u32 offset;
 	u32 max_offset_lim;
 
-	/* Make sure the IOMMU PC resource is available */
-	if (!amd_iommu_pc_present)
-		return -ENODEV;
-
-	/* Locate the iommu associated with the device ID */
-	iommu = amd_iommu_rlookup_table[devid];
-
 	/* Check for valid iommu and pc register indexing */
-	if (WARN_ON((iommu == NULL) || (fxn > 0x28) || (fxn & 7)))
+	if (WARN_ON((fxn > 0x28) || (fxn & 7)))
 		return -ENODEV;
 
 	offset = (u32)(((0x40|bank) << 12) | (cntr << 8) | fxn);
@@ -2322,3 +2348,16 @@ int amd_iommu_pc_get_set_reg_val(u16 devid, u8 bank, u8 cntr, u8 fxn,
 	return 0;
 }
 EXPORT_SYMBOL(amd_iommu_pc_get_set_reg_val);
+
+int amd_iommu_pc_get_set_reg_val(u16 devid, u8 bank, u8 cntr, u8 fxn,
+				    u64 *value, bool is_write)
+{
+	struct amd_iommu *iommu = amd_iommu_rlookup_table[devid];
+
+	/* Make sure the IOMMU PC resource is available */
+	if (!amd_iommu_pc_present || iommu == NULL)
+		return -ENODEV;
+
+	return iommu_pc_get_set_reg_val(iommu, bank, cntr, fxn,
+					value, is_write);
+}
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index b08cf57bf455..9d32b20a5e9a 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -425,46 +425,6 @@ struct protection_domain {
 };
 
 /*
- * For dynamic growth the aperture size is split into ranges of 128MB of
- * DMA address space each. This struct represents one such range.
- */
-struct aperture_range {
-
-	/* address allocation bitmap */
-	unsigned long *bitmap;
-
-	/*
-	 * Array of PTE pages for the aperture. In this array we save all the
-	 * leaf pages of the domain page table used for the aperture. This way
-	 * we don't need to walk the page table to find a specific PTE. We can
-	 * just calculate its address in constant time.
-	 */
-	u64 *pte_pages[64];
-
-	unsigned long offset;
-};
-
-/*
- * Data container for a dma_ops specific protection domain
- */
-struct dma_ops_domain {
-	/* generic protection domain information */
-	struct protection_domain domain;
-
-	/* size of the aperture for the mappings */
-	unsigned long aperture_size;
-
-	/* address we start to search for free addresses */
-	unsigned long next_address;
-
-	/* address space relevant data */
-	struct aperture_range *aperture[APERTURE_MAX_RANGES];
-
-	/* This will be set to true when TLB needs to be flushed */
-	bool need_flush;
-};
-
-/*
  * Structure where we save information about one hardware AMD IOMMU in the
  * system.
  */
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 7caf2fa237f2..c865737326e1 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -432,7 +432,7 @@ static void mn_release(struct mmu_notifier *mn, struct mm_struct *mm)
 	unbind_pasid(pasid_state);
 }
 
-static struct mmu_notifier_ops iommu_mn = {
+static const struct mmu_notifier_ops iommu_mn = {
 	.release		= mn_release,
 	.clear_flush_young      = mn_clear_flush_young,
 	.invalidate_page        = mn_invalidate_page,
@@ -513,43 +513,39 @@ static bool access_error(struct vm_area_struct *vma, struct fault *fault)
 static void do_fault(struct work_struct *work)
 {
 	struct fault *fault = container_of(work, struct fault, work);
-	struct mm_struct *mm;
 	struct vm_area_struct *vma;
+	int ret = VM_FAULT_ERROR;
+	unsigned int flags = 0;
+	struct mm_struct *mm;
 	u64 address;
-	int ret, write;
-
-	write = !!(fault->flags & PPR_FAULT_WRITE);
 
 	mm = fault->state->mm;
 	address = fault->address;
 
+	if (fault->flags & PPR_FAULT_USER)
+		flags |= FAULT_FLAG_USER;
+	if (fault->flags & PPR_FAULT_WRITE)
+		flags |= FAULT_FLAG_WRITE;
+
 	down_read(&mm->mmap_sem);
 	vma = find_extend_vma(mm, address);
-	if (!vma || address < vma->vm_start) {
+	if (!vma || address < vma->vm_start)
 		/* failed to get a vma in the right range */
-		up_read(&mm->mmap_sem);
-		handle_fault_error(fault);
 		goto out;
-	}
 
 	/* Check if we have the right permissions on the vma */
-	if (access_error(vma, fault)) {
-		up_read(&mm->mmap_sem);
-		handle_fault_error(fault);
+	if (access_error(vma, fault))
 		goto out;
-	}
 
-	ret = handle_mm_fault(mm, vma, address, write);
-	if (ret & VM_FAULT_ERROR) {
-		/* failed to service fault */
-		up_read(&mm->mmap_sem);
-		handle_fault_error(fault);
-		goto out;
-	}
+	ret = handle_mm_fault(mm, vma, address, flags);
 
+out:
 	up_read(&mm->mmap_sem);
 
-out:
+	if (ret & VM_FAULT_ERROR)
+		/* failed to service fault */
+		handle_fault_error(fault);
+
 	finish_pri_tag(fault->dev_state, fault->state, fault->tag);
 
 	put_pasid_state(fault->state);
diff --git a/drivers/iommu/arm-smmu-v3.c b/drivers/iommu/arm-smmu-v3.c
index 4e5118a4cd30..20875341c865 100644
--- a/drivers/iommu/arm-smmu-v3.c
+++ b/drivers/iommu/arm-smmu-v3.c
@@ -40,7 +40,10 @@
 #define IDR0_ST_LVL_SHIFT		27
 #define IDR0_ST_LVL_MASK		0x3
 #define IDR0_ST_LVL_2LVL		(1 << IDR0_ST_LVL_SHIFT)
-#define IDR0_STALL_MODEL		(3 << 24)
+#define IDR0_STALL_MODEL_SHIFT		24
+#define IDR0_STALL_MODEL_MASK		0x3
+#define IDR0_STALL_MODEL_STALL		(0 << IDR0_STALL_MODEL_SHIFT)
+#define IDR0_STALL_MODEL_FORCE		(2 << IDR0_STALL_MODEL_SHIFT)
 #define IDR0_TTENDIAN_SHIFT		21
 #define IDR0_TTENDIAN_MASK		0x3
 #define IDR0_TTENDIAN_LE		(2 << IDR0_TTENDIAN_SHIFT)
@@ -253,6 +256,9 @@
 #define STRTAB_STE_1_STRW_EL2		2UL
 #define STRTAB_STE_1_STRW_SHIFT		30
 
+#define STRTAB_STE_1_SHCFG_INCOMING	1UL
+#define STRTAB_STE_1_SHCFG_SHIFT	44
+
 #define STRTAB_STE_2_S2VMID_SHIFT	0
 #define STRTAB_STE_2_S2VMID_MASK	0xffffUL
 #define STRTAB_STE_2_VTCR_SHIFT		32
@@ -378,7 +384,6 @@
 #define PRIQ_0_SID_MASK			0xffffffffUL
 #define PRIQ_0_SSID_SHIFT		32
 #define PRIQ_0_SSID_MASK		0xfffffUL
-#define PRIQ_0_OF			(1UL << 57)
 #define PRIQ_0_PERM_PRIV		(1UL << 58)
 #define PRIQ_0_PERM_EXEC		(1UL << 59)
 #define PRIQ_0_PERM_READ		(1UL << 60)
@@ -855,15 +860,17 @@ static void arm_smmu_cmdq_skip_err(struct arm_smmu_device *smmu)
 	};
 
 	dev_err(smmu->dev, "CMDQ error (cons 0x%08x): %s\n", cons,
-		cerror_str[idx]);
+		idx < ARRAY_SIZE(cerror_str) ?  cerror_str[idx] : "Unknown");
 
 	switch (idx) {
-	case CMDQ_ERR_CERROR_ILL_IDX:
-		break;
 	case CMDQ_ERR_CERROR_ABT_IDX:
 		dev_err(smmu->dev, "retrying command fetch\n");
 	case CMDQ_ERR_CERROR_NONE_IDX:
 		return;
+	case CMDQ_ERR_CERROR_ILL_IDX:
+		/* Fallthrough */
+	default:
+		break;
 	}
 
 	/*
@@ -1042,6 +1049,8 @@ static void arm_smmu_write_strtab_ent(struct arm_smmu_device *smmu, u32 sid,
 		val |= disable_bypass ? STRTAB_STE_0_CFG_ABORT
 				      : STRTAB_STE_0_CFG_BYPASS;
 		dst[0] = cpu_to_le64(val);
+		dst[1] = cpu_to_le64(STRTAB_STE_1_SHCFG_INCOMING
+			 << STRTAB_STE_1_SHCFG_SHIFT);
 		dst[2] = 0; /* Nuke the VMID */
 		if (ste_live)
 			arm_smmu_sync_ste_for_sid(smmu, sid);
@@ -1056,12 +1065,14 @@ static void arm_smmu_write_strtab_ent(struct arm_smmu_device *smmu, u32 sid,
 			 STRTAB_STE_1_S1C_CACHE_WBRA
 			 << STRTAB_STE_1_S1COR_SHIFT |
 			 STRTAB_STE_1_S1C_SH_ISH << STRTAB_STE_1_S1CSH_SHIFT |
-			 STRTAB_STE_1_S1STALLD |
 #ifdef CONFIG_PCI_ATS
 			 STRTAB_STE_1_EATS_TRANS << STRTAB_STE_1_EATS_SHIFT |
 #endif
 			 STRTAB_STE_1_STRW_NSEL1 << STRTAB_STE_1_STRW_SHIFT);
 
+		if (smmu->features & ARM_SMMU_FEAT_STALLS)
+			dst[1] |= cpu_to_le64(STRTAB_STE_1_S1STALLD);
+
 		val |= (ste->s1_cfg->cdptr_dma & STRTAB_STE_0_S1CTXPTR_MASK
 		        << STRTAB_STE_0_S1CTXPTR_SHIFT) |
 			STRTAB_STE_0_CFG_S1_TRANS;
@@ -1123,8 +1134,8 @@ static int arm_smmu_init_l2_strtab(struct arm_smmu_device *smmu, u32 sid)
 	strtab = &cfg->strtab[(sid >> STRTAB_SPLIT) * STRTAB_L1_DESC_DWORDS];
 
 	desc->span = STRTAB_SPLIT + 1;
-	desc->l2ptr = dma_zalloc_coherent(smmu->dev, size, &desc->l2ptr_dma,
-					  GFP_KERNEL);
+	desc->l2ptr = dmam_alloc_coherent(smmu->dev, size, &desc->l2ptr_dma,
+					  GFP_KERNEL | __GFP_ZERO);
 	if (!desc->l2ptr) {
 		dev_err(smmu->dev,
 			"failed to allocate l2 stream table for SID %u\n",
@@ -1250,50 +1261,50 @@ static int arm_smmu_device_disable(struct arm_smmu_device *smmu);
 
 static irqreturn_t arm_smmu_gerror_handler(int irq, void *dev)
 {
-	u32 gerror, gerrorn;
+	u32 gerror, gerrorn, active;
 	struct arm_smmu_device *smmu = dev;
 
 	gerror = readl_relaxed(smmu->base + ARM_SMMU_GERROR);
 	gerrorn = readl_relaxed(smmu->base + ARM_SMMU_GERRORN);
 
-	gerror ^= gerrorn;
-	if (!(gerror & GERROR_ERR_MASK))
+	active = gerror ^ gerrorn;
+	if (!(active & GERROR_ERR_MASK))
 		return IRQ_NONE; /* No errors pending */
 
 	dev_warn(smmu->dev,
 		 "unexpected global error reported (0x%08x), this could be serious\n",
-		 gerror);
+		 active);
 
-	if (gerror & GERROR_SFM_ERR) {
+	if (active & GERROR_SFM_ERR) {
 		dev_err(smmu->dev, "device has entered Service Failure Mode!\n");
 		arm_smmu_device_disable(smmu);
 	}
 
-	if (gerror & GERROR_MSI_GERROR_ABT_ERR)
+	if (active & GERROR_MSI_GERROR_ABT_ERR)
 		dev_warn(smmu->dev, "GERROR MSI write aborted\n");
 
-	if (gerror & 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 (gerror & 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 (gerror & GERROR_MSI_CMDQ_ABT_ERR) {
+	if (active & GERROR_MSI_CMDQ_ABT_ERR) {
 		dev_warn(smmu->dev, "CMDQ MSI write aborted\n");
 		arm_smmu_cmdq_sync_handler(irq, smmu->dev);
 	}
 
-	if (gerror & GERROR_PRIQ_ABT_ERR)
+	if (active & GERROR_PRIQ_ABT_ERR)
 		dev_err(smmu->dev, "PRIQ write aborted -- events may have been lost\n");
 
-	if (gerror & GERROR_EVTQ_ABT_ERR)
+	if (active & GERROR_EVTQ_ABT_ERR)
 		dev_err(smmu->dev, "EVTQ write aborted -- events may have been lost\n");
 
-	if (gerror & GERROR_CMDQ_ERR)
+	if (active & GERROR_CMDQ_ERR)
 		arm_smmu_cmdq_skip_err(smmu);
 
 	writel(gerror, smmu->base + ARM_SMMU_GERRORN);
@@ -1335,7 +1346,7 @@ static void arm_smmu_tlb_inv_context(void *cookie)
 }
 
 static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
-					  bool leaf, void *cookie)
+					  size_t granule, bool leaf, void *cookie)
 {
 	struct arm_smmu_domain *smmu_domain = cookie;
 	struct arm_smmu_device *smmu = smmu_domain->smmu;
@@ -1354,7 +1365,10 @@ static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
 		cmd.tlbi.vmid	= smmu_domain->s2_cfg.vmid;
 	}
 
-	arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+	do {
+		arm_smmu_cmdq_issue_cmd(smmu, &cmd);
+		cmd.tlbi.addr += granule;
+	} while (size -= granule);
 }
 
 static struct iommu_gather_ops arm_smmu_gather_ops = {
@@ -1429,10 +1443,10 @@ static void arm_smmu_domain_free(struct iommu_domain *domain)
 		struct arm_smmu_s1_cfg *cfg = &smmu_domain->s1_cfg;
 
 		if (cfg->cdptr) {
-			dma_free_coherent(smmu_domain->smmu->dev,
-					  CTXDESC_CD_DWORDS << 3,
-					  cfg->cdptr,
-					  cfg->cdptr_dma);
+			dmam_free_coherent(smmu_domain->smmu->dev,
+					   CTXDESC_CD_DWORDS << 3,
+					   cfg->cdptr,
+					   cfg->cdptr_dma);
 
 			arm_smmu_bitmap_free(smmu->asid_map, cfg->cd.asid);
 		}
@@ -1457,8 +1471,9 @@ static int arm_smmu_domain_finalise_s1(struct arm_smmu_domain *smmu_domain,
 	if (IS_ERR_VALUE(asid))
 		return asid;
 
-	cfg->cdptr = dma_zalloc_coherent(smmu->dev, CTXDESC_CD_DWORDS << 3,
-					 &cfg->cdptr_dma, GFP_KERNEL);
+	cfg->cdptr = dmam_alloc_coherent(smmu->dev, CTXDESC_CD_DWORDS << 3,
+					 &cfg->cdptr_dma,
+					 GFP_KERNEL | __GFP_ZERO);
 	if (!cfg->cdptr) {
 		dev_warn(smmu->dev, "failed to allocate context descriptor\n");
 		ret = -ENOMEM;
@@ -1804,13 +1819,13 @@ static int arm_smmu_add_device(struct device *dev)
 		smmu = arm_smmu_get_for_pci_dev(pdev);
 		if (!smmu) {
 			ret = -ENOENT;
-			goto out_put_group;
+			goto out_remove_dev;
 		}
 
 		smmu_group = kzalloc(sizeof(*smmu_group), GFP_KERNEL);
 		if (!smmu_group) {
 			ret = -ENOMEM;
-			goto out_put_group;
+			goto out_remove_dev;
 		}
 
 		smmu_group->ste.valid	= true;
@@ -1826,20 +1841,20 @@ static int arm_smmu_add_device(struct device *dev)
 	for (i = 0; i < smmu_group->num_sids; ++i) {
 		/* If we already know about this SID, then we're done */
 		if (smmu_group->sids[i] == sid)
-			return 0;
+			goto out_put_group;
 	}
 
 	/* Check the SID is in range of the SMMU and our stream table */
 	if (!arm_smmu_sid_in_range(smmu, sid)) {
 		ret = -ERANGE;
-		goto out_put_group;
+		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)
-			goto out_put_group;
+			goto out_remove_dev;
 	}
 
 	/* Resize the SID array for the group */
@@ -1849,16 +1864,20 @@ static int arm_smmu_add_device(struct device *dev)
 	if (!sids) {
 		smmu_group->num_sids--;
 		ret = -ENOMEM;
-		goto out_put_group;
+		goto out_remove_dev;
 	}
 
 	/* Add the new SID */
 	sids[smmu_group->num_sids - 1] = sid;
 	smmu_group->sids = sids;
-	return 0;
 
 out_put_group:
 	iommu_group_put(group);
+	return 0;
+
+out_remove_dev:
+	iommu_group_remove_device(dev);
+	iommu_group_put(group);
 	return ret;
 }
 
@@ -1937,7 +1956,7 @@ static int arm_smmu_init_one_queue(struct arm_smmu_device *smmu,
 {
 	size_t qsz = ((1 << q->max_n_shift) * dwords) << 3;
 
-	q->base = dma_alloc_coherent(smmu->dev, qsz, &q->base_dma, GFP_KERNEL);
+	q->base = dmam_alloc_coherent(smmu->dev, qsz, &q->base_dma, GFP_KERNEL);
 	if (!q->base) {
 		dev_err(smmu->dev, "failed to allocate queue (0x%zx bytes)\n",
 			qsz);
@@ -1957,23 +1976,6 @@ static int arm_smmu_init_one_queue(struct arm_smmu_device *smmu,
 	return 0;
 }
 
-static void arm_smmu_free_one_queue(struct arm_smmu_device *smmu,
-				    struct arm_smmu_queue *q)
-{
-	size_t qsz = ((1 << q->max_n_shift) * q->ent_dwords) << 3;
-
-	dma_free_coherent(smmu->dev, qsz, q->base, q->base_dma);
-}
-
-static void arm_smmu_free_queues(struct arm_smmu_device *smmu)
-{
-	arm_smmu_free_one_queue(smmu, &smmu->cmdq.q);
-	arm_smmu_free_one_queue(smmu, &smmu->evtq.q);
-
-	if (smmu->features & ARM_SMMU_FEAT_PRI)
-		arm_smmu_free_one_queue(smmu, &smmu->priq.q);
-}
-
 static int arm_smmu_init_queues(struct arm_smmu_device *smmu)
 {
 	int ret;
@@ -1983,49 +1985,20 @@ static int arm_smmu_init_queues(struct arm_smmu_device *smmu)
 	ret = arm_smmu_init_one_queue(smmu, &smmu->cmdq.q, ARM_SMMU_CMDQ_PROD,
 				      ARM_SMMU_CMDQ_CONS, CMDQ_ENT_DWORDS);
 	if (ret)
-		goto out;
+		return ret;
 
 	/* evtq */
 	ret = arm_smmu_init_one_queue(smmu, &smmu->evtq.q, ARM_SMMU_EVTQ_PROD,
 				      ARM_SMMU_EVTQ_CONS, EVTQ_ENT_DWORDS);
 	if (ret)
-		goto out_free_cmdq;
+		return ret;
 
 	/* priq */
 	if (!(smmu->features & ARM_SMMU_FEAT_PRI))
 		return 0;
 
-	ret = arm_smmu_init_one_queue(smmu, &smmu->priq.q, ARM_SMMU_PRIQ_PROD,
-				      ARM_SMMU_PRIQ_CONS, PRIQ_ENT_DWORDS);
-	if (ret)
-		goto out_free_evtq;
-
-	return 0;
-
-out_free_evtq:
-	arm_smmu_free_one_queue(smmu, &smmu->evtq.q);
-out_free_cmdq:
-	arm_smmu_free_one_queue(smmu, &smmu->cmdq.q);
-out:
-	return ret;
-}
-
-static void arm_smmu_free_l2_strtab(struct arm_smmu_device *smmu)
-{
-	int i;
-	size_t size;
-	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
-
-	size = 1 << (STRTAB_SPLIT + ilog2(STRTAB_STE_DWORDS) + 3);
-	for (i = 0; i < cfg->num_l1_ents; ++i) {
-		struct arm_smmu_strtab_l1_desc *desc = &cfg->l1_desc[i];
-
-		if (!desc->l2ptr)
-			continue;
-
-		dma_free_coherent(smmu->dev, size, desc->l2ptr,
-				  desc->l2ptr_dma);
-	}
+	return arm_smmu_init_one_queue(smmu, &smmu->priq.q, ARM_SMMU_PRIQ_PROD,
+				       ARM_SMMU_PRIQ_CONS, PRIQ_ENT_DWORDS);
 }
 
 static int arm_smmu_init_l1_strtab(struct arm_smmu_device *smmu)
@@ -2054,7 +2027,6 @@ static int arm_smmu_init_strtab_2lvl(struct arm_smmu_device *smmu)
 	void *strtab;
 	u64 reg;
 	u32 size, l1size;
-	int ret;
 	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
 
 	/*
@@ -2077,8 +2049,8 @@ static int arm_smmu_init_strtab_2lvl(struct arm_smmu_device *smmu)
 			 size, smmu->sid_bits);
 
 	l1size = cfg->num_l1_ents * (STRTAB_L1_DESC_DWORDS << 3);
-	strtab = dma_zalloc_coherent(smmu->dev, l1size, &cfg->strtab_dma,
-				     GFP_KERNEL);
+	strtab = dmam_alloc_coherent(smmu->dev, l1size, &cfg->strtab_dma,
+				     GFP_KERNEL | __GFP_ZERO);
 	if (!strtab) {
 		dev_err(smmu->dev,
 			"failed to allocate l1 stream table (%u bytes)\n",
@@ -2095,13 +2067,7 @@ static int arm_smmu_init_strtab_2lvl(struct arm_smmu_device *smmu)
 		<< STRTAB_BASE_CFG_SPLIT_SHIFT;
 	cfg->strtab_base_cfg = reg;
 
-	ret = arm_smmu_init_l1_strtab(smmu);
-	if (ret)
-		dma_free_coherent(smmu->dev,
-				  l1size,
-				  strtab,
-				  cfg->strtab_dma);
-	return ret;
+	return arm_smmu_init_l1_strtab(smmu);
 }
 
 static int arm_smmu_init_strtab_linear(struct arm_smmu_device *smmu)
@@ -2112,8 +2078,8 @@ static int arm_smmu_init_strtab_linear(struct arm_smmu_device *smmu)
 	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
 
 	size = (1 << smmu->sid_bits) * (STRTAB_STE_DWORDS << 3);
-	strtab = dma_zalloc_coherent(smmu->dev, size, &cfg->strtab_dma,
-				     GFP_KERNEL);
+	strtab = dmam_alloc_coherent(smmu->dev, size, &cfg->strtab_dma,
+				     GFP_KERNEL | __GFP_ZERO);
 	if (!strtab) {
 		dev_err(smmu->dev,
 			"failed to allocate linear stream table (%u bytes)\n",
@@ -2157,21 +2123,6 @@ static int arm_smmu_init_strtab(struct arm_smmu_device *smmu)
 	return 0;
 }
 
-static void arm_smmu_free_strtab(struct arm_smmu_device *smmu)
-{
-	struct arm_smmu_strtab_cfg *cfg = &smmu->strtab_cfg;
-	u32 size = cfg->num_l1_ents;
-
-	if (smmu->features & ARM_SMMU_FEAT_2_LVL_STRTAB) {
-		arm_smmu_free_l2_strtab(smmu);
-		size *= STRTAB_L1_DESC_DWORDS << 3;
-	} else {
-		size *= STRTAB_STE_DWORDS * 3;
-	}
-
-	dma_free_coherent(smmu->dev, size, cfg->strtab, cfg->strtab_dma);
-}
-
 static int arm_smmu_init_structures(struct arm_smmu_device *smmu)
 {
 	int ret;
@@ -2180,21 +2131,7 @@ static int arm_smmu_init_structures(struct arm_smmu_device *smmu)
 	if (ret)
 		return ret;
 
-	ret = arm_smmu_init_strtab(smmu);
-	if (ret)
-		goto out_free_queues;
-
-	return 0;
-
-out_free_queues:
-	arm_smmu_free_queues(smmu);
-	return ret;
-}
-
-static void arm_smmu_free_structures(struct arm_smmu_device *smmu)
-{
-	arm_smmu_free_strtab(smmu);
-	arm_smmu_free_queues(smmu);
+	return arm_smmu_init_strtab(smmu);
 }
 
 static int arm_smmu_write_reg_sync(struct arm_smmu_device *smmu, u32 val,
@@ -2532,8 +2469,12 @@ static int arm_smmu_device_probe(struct arm_smmu_device *smmu)
 		dev_warn(smmu->dev, "IDR0.COHACC overridden by dma-coherent property (%s)\n",
 			 coherent ? "true" : "false");
 
-	if (reg & IDR0_STALL_MODEL)
+	switch (reg & IDR0_STALL_MODEL_MASK << IDR0_STALL_MODEL_SHIFT) {
+	case IDR0_STALL_MODEL_STALL:
+		/* Fallthrough */
+	case IDR0_STALL_MODEL_FORCE:
 		smmu->features |= ARM_SMMU_FEAT_STALLS;
+	}
 
 	if (reg & IDR0_S1P)
 		smmu->features |= ARM_SMMU_FEAT_TRANS_S1;
@@ -2699,15 +2640,7 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, smmu);
 
 	/* Reset the device */
-	ret = arm_smmu_device_reset(smmu);
-	if (ret)
-		goto out_free_structures;
-
-	return 0;
-
-out_free_structures:
-	arm_smmu_free_structures(smmu);
-	return ret;
+	return arm_smmu_device_reset(smmu);
 }
 
 static int arm_smmu_device_remove(struct platform_device *pdev)
@@ -2715,7 +2648,6 @@ static int arm_smmu_device_remove(struct platform_device *pdev)
 	struct arm_smmu_device *smmu = platform_get_drvdata(pdev);
 
 	arm_smmu_device_disable(smmu);
-	arm_smmu_free_structures(smmu);
 	return 0;
 }
 
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index 47dc7a793f5c..59ee4b8a3236 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -582,7 +582,7 @@ static void arm_smmu_tlb_inv_context(void *cookie)
 }
 
 static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
-					  bool leaf, void *cookie)
+					  size_t granule, bool leaf, void *cookie)
 {
 	struct arm_smmu_domain *smmu_domain = cookie;
 	struct arm_smmu_cfg *cfg = &smmu_domain->cfg;
@@ -597,12 +597,18 @@ static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
 		if (!IS_ENABLED(CONFIG_64BIT) || smmu->version == ARM_SMMU_V1) {
 			iova &= ~12UL;
 			iova |= ARM_SMMU_CB_ASID(cfg);
-			writel_relaxed(iova, reg);
+			do {
+				writel_relaxed(iova, reg);
+				iova += granule;
+			} while (size -= granule);
 #ifdef CONFIG_64BIT
 		} else {
 			iova >>= 12;
 			iova |= (u64)ARM_SMMU_CB_ASID(cfg) << 48;
-			writeq_relaxed(iova, reg);
+			do {
+				writeq_relaxed(iova, reg);
+				iova += granule >> 12;
+			} while (size -= granule);
 #endif
 		}
 #ifdef CONFIG_64BIT
@@ -610,7 +616,11 @@ static void arm_smmu_tlb_inv_range_nosync(unsigned long iova, size_t size,
 		reg = ARM_SMMU_CB_BASE(smmu) + ARM_SMMU_CB(smmu, cfg->cbndx);
 		reg += leaf ? ARM_SMMU_CB_S2_TLBIIPAS2L :
 			      ARM_SMMU_CB_S2_TLBIIPAS2;
-		writeq_relaxed(iova >> 12, reg);
+		iova >>= 12;
+		do {
+			writeq_relaxed(iova, reg);
+			iova += granule >> 12;
+		} while (size -= granule);
 #endif
 	} else {
 		reg = ARM_SMMU_GR0(smmu) + ARM_SMMU_GR0_TLBIVMID;
@@ -945,9 +955,7 @@ static void arm_smmu_destroy_domain_context(struct iommu_domain *domain)
 		free_irq(irq, domain);
 	}
 
-	if (smmu_domain->pgtbl_ops)
-		free_io_pgtable_ops(smmu_domain->pgtbl_ops);
-
+	free_io_pgtable_ops(smmu_domain->pgtbl_ops);
 	__arm_smmu_free_bitmap(smmu->context_map, cfg->cbndx);
 }
 
@@ -1357,6 +1365,7 @@ static int arm_smmu_add_device(struct device *dev)
 	if (IS_ERR(group))
 		return PTR_ERR(group);
 
+	iommu_group_put(group);
 	return 0;
 }
 
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 80e3c176008e..8ffd7568fc91 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -329,7 +329,8 @@ static int dmar_pci_bus_notifier(struct notifier_block *nb,
 	/* Only care about add/remove events for physical functions */
 	if (pdev->is_virtfn)
 		return NOTIFY_DONE;
-	if (action != BUS_NOTIFY_ADD_DEVICE && action != BUS_NOTIFY_DEL_DEVICE)
+	if (action != BUS_NOTIFY_ADD_DEVICE &&
+	    action != BUS_NOTIFY_REMOVED_DEVICE)
 		return NOTIFY_DONE;
 
 	info = dmar_alloc_pci_notify_info(pdev, action);
@@ -339,7 +340,7 @@ static int dmar_pci_bus_notifier(struct notifier_block *nb,
 	down_write(&dmar_global_lock);
 	if (action == BUS_NOTIFY_ADD_DEVICE)
 		dmar_pci_bus_add_dev(info);
-	else if (action == BUS_NOTIFY_DEL_DEVICE)
+	else if (action == BUS_NOTIFY_REMOVED_DEVICE)
 		dmar_pci_bus_del_dev(info);
 	up_write(&dmar_global_lock);
 
@@ -1063,13 +1064,19 @@ static int alloc_iommu(struct dmar_drhd_unit *drhd)
 
 	raw_spin_lock_init(&iommu->register_lock);
 
-	drhd->iommu = iommu;
-
-	if (intel_iommu_enabled)
+	if (intel_iommu_enabled) {
 		iommu->iommu_dev = iommu_device_create(NULL, iommu,
 						       intel_iommu_groups,
 						       "%s", iommu->name);
 
+		if (IS_ERR(iommu->iommu_dev)) {
+			err = PTR_ERR(iommu->iommu_dev);
+			goto err_unmap;
+		}
+	}
+
+	drhd->iommu = iommu;
+
 	return 0;
 
 err_unmap:
@@ -1347,7 +1354,7 @@ void dmar_disable_qi(struct intel_iommu *iommu)
 
 	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
-	sts =  dmar_readq(iommu->reg + DMAR_GSTS_REG);
+	sts =  readl(iommu->reg + DMAR_GSTS_REG);
 	if (!(sts & DMA_GSTS_QIES))
 		goto end;
 
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index ac7387686ddc..a2e1b7f14df2 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -1489,7 +1489,7 @@ static void iommu_disable_dev_iotlb(struct device_domain_info *info)
 {
 	struct pci_dev *pdev;
 
-	if (dev_is_pci(info->dev))
+	if (!dev_is_pci(info->dev))
 		return;
 
 	pdev = to_pci_dev(info->dev);
@@ -4367,7 +4367,7 @@ int dmar_iommu_notify_scope_dev(struct dmar_pci_notify_info *info)
 				rmrru->devices_cnt);
 			if(ret < 0)
 				return ret;
-		} else if (info->event == BUS_NOTIFY_DEL_DEVICE) {
+		} else if (info->event == BUS_NOTIFY_REMOVED_DEVICE) {
 			dmar_remove_dev_scope(info, rmrr->segment,
 				rmrru->devices, rmrru->devices_cnt);
 		}
@@ -4387,7 +4387,7 @@ int dmar_iommu_notify_scope_dev(struct dmar_pci_notify_info *info)
 				break;
 			else if(ret < 0)
 				return ret;
-		} else if (info->event == BUS_NOTIFY_DEL_DEVICE) {
+		} else if (info->event == BUS_NOTIFY_REMOVED_DEVICE) {
 			if (dmar_remove_dev_scope(info, atsr->segment,
 					atsru->devices, atsru->devices_cnt))
 				break;
diff --git a/drivers/iommu/intel-svm.c b/drivers/iommu/intel-svm.c
index 50464833d0b8..d9939fa9b588 100644
--- a/drivers/iommu/intel-svm.c
+++ b/drivers/iommu/intel-svm.c
@@ -249,12 +249,30 @@ static void intel_flush_pasid_dev(struct intel_svm *svm, struct intel_svm_dev *s
 static void intel_mm_release(struct mmu_notifier *mn, struct mm_struct *mm)
 {
 	struct intel_svm *svm = container_of(mn, struct intel_svm, notifier);
+	struct intel_svm_dev *sdev;
 
+	/* This might end up being called from exit_mmap(), *before* the page
+	 * tables are cleared. And __mmu_notifier_release() will delete us from
+	 * the list of notifiers so that our invalidate_range() callback doesn't
+	 * get called when the page tables are cleared. So we need to protect
+	 * against hardware accessing those page tables.
+	 *
+	 * We do it by clearing the entry in the PASID table and then flushing
+	 * the IOTLB and the PASID table caches. This might upset hardware;
+	 * perhaps we'll want to point the PASID to a dummy PGD (like the zero
+	 * page) so that we end up taking a fault that the hardware really
+	 * *has* to handle gracefully without affecting other processes.
+	 */
 	svm->iommu->pasid_table[svm->pasid].val = 0;
+	wmb();
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(sdev, &svm->devs, list) {
+		intel_flush_pasid_dev(svm, sdev, svm->pasid);
+		intel_flush_svm_range_dev(svm, sdev, 0, -1, 0, !svm->mm);
+	}
+	rcu_read_unlock();
 
-	/* There's no need to do any flush because we can't get here if there
-	 * are any devices left anyway. */
-	WARN_ON(!list_empty(&svm->devs));
 }
 
 static const struct mmu_notifier_ops intel_mmuops = {
@@ -379,7 +397,6 @@ int intel_svm_bind_mm(struct device *dev, int *pasid, int flags, struct svm_dev_
 				goto out;
 			}
 			iommu->pasid_table[svm->pasid].val = (u64)__pa(mm->pgd) | 1;
-			mm = NULL;
 		} else
 			iommu->pasid_table[svm->pasid].val = (u64)__pa(init_mm.pgd) | 1 | (1ULL << 11);
 		wmb();
@@ -442,11 +459,11 @@ int intel_svm_unbind_mm(struct device *dev, int pasid)
 				kfree_rcu(sdev, rcu);
 
 				if (list_empty(&svm->devs)) {
-					mmu_notifier_unregister(&svm->notifier, svm->mm);
 
 					idr_remove(&svm->iommu->pasid_idr, svm->pasid);
 					if (svm->mm)
-						mmput(svm->mm);
+						mmu_notifier_unregister(&svm->notifier, svm->mm);
+
 					/* We mandate that no page faults may be outstanding
 					 * for the PASID when intel_svm_unbind_mm() is called.
 					 * If that is not obeyed, subtle errors will happen.
@@ -507,6 +524,10 @@ static irqreturn_t prq_event_thread(int irq, void *d)
 	struct intel_svm *svm = NULL;
 	int head, tail, handled = 0;
 
+	/* Clear PPR bit before reading head/tail registers, to
+	 * ensure that we get a new interrupt if needed. */
+	writel(DMA_PRS_PPR, iommu->reg + DMAR_PRS_REG);
+
 	tail = dmar_readq(iommu->reg + DMAR_PQT_REG) & PRQ_RING_MASK;
 	head = dmar_readq(iommu->reg + DMAR_PQH_REG) & PRQ_RING_MASK;
 	while (head != tail) {
@@ -551,6 +572,9 @@ static irqreturn_t prq_event_thread(int irq, void *d)
 		 * any faults on kernel addresses. */
 		if (!svm->mm)
 			goto bad_req;
+		/* If the mm is already defunct, don't handle faults. */
+		if (!atomic_inc_not_zero(&svm->mm->mm_users))
+			goto bad_req;
 		down_read(&svm->mm->mmap_sem);
 		vma = find_extend_vma(svm->mm, address);
 		if (!vma || address < vma->vm_start)
@@ -567,6 +591,7 @@ static irqreturn_t prq_event_thread(int irq, void *d)
 		result = QI_RESP_SUCCESS;
 	invalid:
 		up_read(&svm->mm->mmap_sem);
+		mmput(svm->mm);
 	bad_req:
 		/* Accounting for major/minor faults? */
 		rcu_read_lock();
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 1fae1881648c..ac596928f6b4 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -629,7 +629,7 @@ static void iommu_disable_irq_remapping(struct intel_iommu *iommu)
 
 	raw_spin_lock_irqsave(&iommu->register_lock, flags);
 
-	sts = dmar_readq(iommu->reg + DMAR_GSTS_REG);
+	sts = readl(iommu->reg + DMAR_GSTS_REG);
 	if (!(sts & DMA_GSTS_IRES))
 		goto end;
 
@@ -753,7 +753,7 @@ static inline void set_irq_posting_cap(void)
 		 * should have X86_FEATURE_CX16 support, this has been confirmed
 		 * with Intel hardware guys.
 		 */
-		if ( cpu_has_cx16 )
+		if (boot_cpu_has(X86_FEATURE_CX16))
 			intel_irq_remap_ops.capability |= 1 << IRQ_POSTING_CAP;
 
 		for_each_iommu(iommu, drhd)
diff --git a/drivers/iommu/io-pgtable-arm.c b/drivers/iommu/io-pgtable-arm.c
index 7df97777662d..381ca5a37a7b 100644
--- a/drivers/iommu/io-pgtable-arm.c
+++ b/drivers/iommu/io-pgtable-arm.c
@@ -25,6 +25,7 @@
 #include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/types.h>
+#include <linux/dma-mapping.h>
 
 #include <asm/barrier.h>
 
@@ -38,9 +39,6 @@
 #define io_pgtable_to_data(x)						\
 	container_of((x), struct arm_lpae_io_pgtable, iop)
 
-#define io_pgtable_ops_to_pgtable(x)					\
-	container_of((x), struct io_pgtable, ops)
-
 #define io_pgtable_ops_to_data(x)					\
 	io_pgtable_to_data(io_pgtable_ops_to_pgtable(x))
 
@@ -58,8 +56,10 @@
 	((((d)->levels - ((l) - ARM_LPAE_START_LVL(d) + 1))		\
 	  * (d)->bits_per_level) + (d)->pg_shift)
 
+#define ARM_LPAE_GRANULE(d)		(1UL << (d)->pg_shift)
+
 #define ARM_LPAE_PAGES_PER_PGD(d)					\
-	DIV_ROUND_UP((d)->pgd_size, 1UL << (d)->pg_shift)
+	DIV_ROUND_UP((d)->pgd_size, ARM_LPAE_GRANULE(d))
 
 /*
  * Calculate the index at level l used to map virtual address a using the
@@ -169,7 +169,7 @@
 /* IOPTE accessors */
 #define iopte_deref(pte,d)					\
 	(__va((pte) & ((1ULL << ARM_LPAE_MAX_ADDR_BITS) - 1)	\
-	& ~((1ULL << (d)->pg_shift) - 1)))
+	& ~(ARM_LPAE_GRANULE(d) - 1ULL)))
 
 #define iopte_type(pte,l)					\
 	(((pte) >> ARM_LPAE_PTE_TYPE_SHIFT) & ARM_LPAE_PTE_TYPE_MASK)
@@ -326,7 +326,7 @@ static int __arm_lpae_map(struct arm_lpae_io_pgtable *data, unsigned long iova,
 	/* Grab a pointer to the next level */
 	pte = *ptep;
 	if (!pte) {
-		cptep = __arm_lpae_alloc_pages(1UL << data->pg_shift,
+		cptep = __arm_lpae_alloc_pages(ARM_LPAE_GRANULE(data),
 					       GFP_ATOMIC, cfg);
 		if (!cptep)
 			return -ENOMEM;
@@ -405,17 +405,18 @@ static void __arm_lpae_free_pgtable(struct arm_lpae_io_pgtable *data, int lvl,
 	arm_lpae_iopte *start, *end;
 	unsigned long table_size;
 
-	/* Only leaf entries at the last level */
-	if (lvl == ARM_LPAE_MAX_LEVELS - 1)
-		return;
-
 	if (lvl == ARM_LPAE_START_LVL(data))
 		table_size = data->pgd_size;
 	else
-		table_size = 1UL << data->pg_shift;
+		table_size = ARM_LPAE_GRANULE(data);
 
 	start = ptep;
-	end = (void *)ptep + table_size;
+
+	/* Only leaf entries at the last level */
+	if (lvl == ARM_LPAE_MAX_LEVELS - 1)
+		end = ptep;
+	else
+		end = (void *)ptep + table_size;
 
 	while (ptep != end) {
 		arm_lpae_iopte pte = *ptep++;
@@ -473,7 +474,7 @@ static int arm_lpae_split_blk_unmap(struct arm_lpae_io_pgtable *data,
 
 	__arm_lpae_set_pte(ptep, table, cfg);
 	iova &= ~(blk_size - 1);
-	cfg->tlb->tlb_add_flush(iova, blk_size, true, data->iop.cookie);
+	cfg->tlb->tlb_add_flush(iova, blk_size, blk_size, true, data->iop.cookie);
 	return size;
 }
 
@@ -486,11 +487,13 @@ static int __arm_lpae_unmap(struct arm_lpae_io_pgtable *data,
 	void *cookie = data->iop.cookie;
 	size_t blk_size = ARM_LPAE_BLOCK_SIZE(lvl, data);
 
+	/* Something went horribly wrong and we ran out of page table */
+	if (WARN_ON(lvl == ARM_LPAE_MAX_LEVELS))
+		return 0;
+
 	ptep += ARM_LPAE_LVL_IDX(iova, lvl, data);
 	pte = *ptep;
-
-	/* Something went horribly wrong and we ran out of page table */
-	if (WARN_ON(!pte || (lvl == ARM_LPAE_MAX_LEVELS)))
+	if (WARN_ON(!pte))
 		return 0;
 
 	/* If the size matches this level, we're in the right place */
@@ -499,12 +502,13 @@ static int __arm_lpae_unmap(struct arm_lpae_io_pgtable *data,
 
 		if (!iopte_leaf(pte, lvl)) {
 			/* Also flush any partial walks */
-			tlb->tlb_add_flush(iova, size, false, cookie);
+			tlb->tlb_add_flush(iova, size, ARM_LPAE_GRANULE(data),
+					   false, cookie);
 			tlb->tlb_sync(cookie);
 			ptep = iopte_deref(pte, data);
 			__arm_lpae_free_pgtable(data, lvl + 1, ptep);
 		} else {
-			tlb->tlb_add_flush(iova, size, true, cookie);
+			tlb->tlb_add_flush(iova, size, size, true, cookie);
 		}
 
 		return size;
@@ -570,7 +574,7 @@ static phys_addr_t arm_lpae_iova_to_phys(struct io_pgtable_ops *ops,
 	return 0;
 
 found_translation:
-	iova &= ((1 << data->pg_shift) - 1);
+	iova &= (ARM_LPAE_GRANULE(data) - 1);
 	return ((phys_addr_t)iopte_to_pfn(pte,data) << data->pg_shift) | iova;
 }
 
@@ -668,7 +672,7 @@ arm_64_lpae_alloc_pgtable_s1(struct io_pgtable_cfg *cfg, void *cookie)
 	      (ARM_LPAE_TCR_RGN_WBWA << ARM_LPAE_TCR_IRGN0_SHIFT) |
 	      (ARM_LPAE_TCR_RGN_WBWA << ARM_LPAE_TCR_ORGN0_SHIFT);
 
-	switch (1 << data->pg_shift) {
+	switch (ARM_LPAE_GRANULE(data)) {
 	case SZ_4K:
 		reg |= ARM_LPAE_TCR_TG0_4K;
 		break;
@@ -769,7 +773,7 @@ arm_64_lpae_alloc_pgtable_s2(struct io_pgtable_cfg *cfg, void *cookie)
 
 	sl = ARM_LPAE_START_LVL(data);
 
-	switch (1 << data->pg_shift) {
+	switch (ARM_LPAE_GRANULE(data)) {
 	case SZ_4K:
 		reg |= ARM_LPAE_TCR_TG0_4K;
 		sl++; /* SL0 format is different for 4K granule size */
@@ -889,8 +893,8 @@ static void dummy_tlb_flush_all(void *cookie)
 	WARN_ON(cookie != cfg_cookie);
 }
 
-static void dummy_tlb_add_flush(unsigned long iova, size_t size, bool leaf,
-				void *cookie)
+static void dummy_tlb_add_flush(unsigned long iova, size_t size,
+				size_t granule, bool leaf, void *cookie)
 {
 	WARN_ON(cookie != cfg_cookie);
 	WARN_ON(!(size & cfg_cookie->pgsize_bitmap));
diff --git a/drivers/iommu/io-pgtable.h b/drivers/iommu/io-pgtable.h
index ac9e2341a633..36673c83de58 100644
--- a/drivers/iommu/io-pgtable.h
+++ b/drivers/iommu/io-pgtable.h
@@ -26,8 +26,8 @@ enum io_pgtable_fmt {
  */
 struct iommu_gather_ops {
 	void (*tlb_flush_all)(void *cookie);
-	void (*tlb_add_flush)(unsigned long iova, size_t size, bool leaf,
-			      void *cookie);
+	void (*tlb_add_flush)(unsigned long iova, size_t size, size_t granule,
+			      bool leaf, void *cookie);
 	void (*tlb_sync)(void *cookie);
 };
 
@@ -131,6 +131,8 @@ struct io_pgtable {
 	struct io_pgtable_ops	ops;
 };
 
+#define io_pgtable_ops_to_pgtable(x) container_of((x), struct io_pgtable, ops)
+
 /**
  * struct io_pgtable_init_fns - Alloc/free a set of page tables for a
  *                              particular format.
diff --git a/drivers/iommu/ipmmu-vmsa.c b/drivers/iommu/ipmmu-vmsa.c
index dfb868e2d129..2fdbac67a77f 100644
--- a/drivers/iommu/ipmmu-vmsa.c
+++ b/drivers/iommu/ipmmu-vmsa.c
@@ -277,8 +277,8 @@ static void ipmmu_tlb_flush_all(void *cookie)
 	ipmmu_tlb_invalidate(domain);
 }
 
-static void ipmmu_tlb_add_flush(unsigned long iova, size_t size, bool leaf,
-				void *cookie)
+static void ipmmu_tlb_add_flush(unsigned long iova, size_t size,
+				size_t granule, bool leaf, void *cookie)
 {
 	/* The hardware doesn't support selective TLB flush. */
 }
diff --git a/drivers/iommu/msm_iommu_dev.c b/drivers/iommu/msm_iommu_dev.c
index b6d01f97e537..4b09e815accf 100644
--- a/drivers/iommu/msm_iommu_dev.c
+++ b/drivers/iommu/msm_iommu_dev.c
@@ -359,30 +359,19 @@ static struct platform_driver msm_iommu_ctx_driver = {
 	.remove		= msm_iommu_ctx_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&msm_iommu_driver,
+	&msm_iommu_ctx_driver,
+};
+
 static int __init msm_iommu_driver_init(void)
 {
-	int ret;
-	ret = platform_driver_register(&msm_iommu_driver);
-	if (ret != 0) {
-		pr_err("Failed to register IOMMU driver\n");
-		goto error;
-	}
-
-	ret = platform_driver_register(&msm_iommu_ctx_driver);
-	if (ret != 0) {
-		platform_driver_unregister(&msm_iommu_driver);
-		pr_err("Failed to register IOMMU context driver\n");
-		goto error;
-	}
-
-error:
-	return ret;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit msm_iommu_driver_exit(void)
 {
-	platform_driver_unregister(&msm_iommu_ctx_driver);
-	platform_driver_unregister(&msm_iommu_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 subsys_initcall(msm_iommu_driver_init);
diff --git a/drivers/iommu/s390-iommu.c b/drivers/iommu/s390-iommu.c
index 471ee36b9c6e..a04d491cf431 100644
--- a/drivers/iommu/s390-iommu.c
+++ b/drivers/iommu/s390-iommu.c
@@ -49,7 +49,7 @@ static bool s390_iommu_capable(enum iommu_cap cap)
 	}
 }
 
-struct iommu_domain *s390_domain_alloc(unsigned domain_type)
+static struct iommu_domain *s390_domain_alloc(unsigned domain_type)
 {
 	struct s390_domain *s390_domain;
 
@@ -73,7 +73,7 @@ struct iommu_domain *s390_domain_alloc(unsigned domain_type)
 	return &s390_domain->domain;
 }
 
-void s390_domain_free(struct iommu_domain *domain)
+static void s390_domain_free(struct iommu_domain *domain)
 {
 	struct s390_domain *s390_domain = to_s390_domain(domain);
 
diff --git a/drivers/iommu/shmobile-iommu.c b/drivers/iommu/shmobile-iommu.c
deleted file mode 100644
index a0287519a1d4..000000000000
--- a/drivers/iommu/shmobile-iommu.c
+++ /dev/null
@@ -1,402 +0,0 @@
-/*
- * IOMMU for IPMMU/IPMMUI
- * Copyright (C) 2012  Hideki EIRAKU
- *
- * This program is free software; 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/dma-mapping.h>
-#include <linux/io.h>
-#include <linux/iommu.h>
-#include <linux/platform_device.h>
-#include <linux/sizes.h>
-#include <linux/slab.h>
-#include <asm/dma-iommu.h>
-#include "shmobile-ipmmu.h"
-
-#define L1_SIZE CONFIG_SHMOBILE_IOMMU_L1SIZE
-#define L1_LEN (L1_SIZE / 4)
-#define L1_ALIGN L1_SIZE
-#define L2_SIZE SZ_1K
-#define L2_LEN (L2_SIZE / 4)
-#define L2_ALIGN L2_SIZE
-
-struct shmobile_iommu_domain_pgtable {
-	uint32_t *pgtable;
-	dma_addr_t handle;
-};
-
-struct shmobile_iommu_archdata {
-	struct list_head attached_list;
-	struct dma_iommu_mapping *iommu_mapping;
-	spinlock_t attach_lock;
-	struct shmobile_iommu_domain *attached;
-	int num_attached_devices;
-	struct shmobile_ipmmu *ipmmu;
-};
-
-struct shmobile_iommu_domain {
-	struct shmobile_iommu_domain_pgtable l1, l2[L1_LEN];
-	spinlock_t map_lock;
-	spinlock_t attached_list_lock;
-	struct list_head attached_list;
-	struct iommu_domain domain;
-};
-
-static struct shmobile_iommu_archdata *ipmmu_archdata;
-static struct kmem_cache *l1cache, *l2cache;
-
-static struct shmobile_iommu_domain *to_sh_domain(struct iommu_domain *dom)
-{
-	return container_of(dom, struct shmobile_iommu_domain, domain);
-}
-
-static int pgtable_alloc(struct shmobile_iommu_domain_pgtable *pgtable,
-			 struct kmem_cache *cache, size_t size)
-{
-	pgtable->pgtable = kmem_cache_zalloc(cache, GFP_ATOMIC);
-	if (!pgtable->pgtable)
-		return -ENOMEM;
-	pgtable->handle = dma_map_single(NULL, pgtable->pgtable, size,
-					 DMA_TO_DEVICE);
-	return 0;
-}
-
-static void pgtable_free(struct shmobile_iommu_domain_pgtable *pgtable,
-			 struct kmem_cache *cache, size_t size)
-{
-	dma_unmap_single(NULL, pgtable->handle, size, DMA_TO_DEVICE);
-	kmem_cache_free(cache, pgtable->pgtable);
-}
-
-static uint32_t pgtable_read(struct shmobile_iommu_domain_pgtable *pgtable,
-			     unsigned int index)
-{
-	return pgtable->pgtable[index];
-}
-
-static void pgtable_write(struct shmobile_iommu_domain_pgtable *pgtable,
-			  unsigned int index, unsigned int count, uint32_t val)
-{
-	unsigned int i;
-
-	for (i = 0; i < count; i++)
-		pgtable->pgtable[index + i] = val;
-	dma_sync_single_for_device(NULL, pgtable->handle + index * sizeof(val),
-				   sizeof(val) * count, DMA_TO_DEVICE);
-}
-
-static struct iommu_domain *shmobile_iommu_domain_alloc(unsigned type)
-{
-	struct shmobile_iommu_domain *sh_domain;
-	int i, ret;
-
-	if (type != IOMMU_DOMAIN_UNMANAGED)
-		return NULL;
-
-	sh_domain = kzalloc(sizeof(*sh_domain), GFP_KERNEL);
-	if (!sh_domain)
-		return NULL;
-	ret = pgtable_alloc(&sh_domain->l1, l1cache, L1_SIZE);
-	if (ret < 0) {
-		kfree(sh_domain);
-		return NULL;
-	}
-	for (i = 0; i < L1_LEN; i++)
-		sh_domain->l2[i].pgtable = NULL;
-	spin_lock_init(&sh_domain->map_lock);
-	spin_lock_init(&sh_domain->attached_list_lock);
-	INIT_LIST_HEAD(&sh_domain->attached_list);
-	return &sh_domain->domain;
-}
-
-static void shmobile_iommu_domain_free(struct iommu_domain *domain)
-{
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-	int i;
-
-	for (i = 0; i < L1_LEN; i++) {
-		if (sh_domain->l2[i].pgtable)
-			pgtable_free(&sh_domain->l2[i], l2cache, L2_SIZE);
-	}
-	pgtable_free(&sh_domain->l1, l1cache, L1_SIZE);
-	kfree(sh_domain);
-}
-
-static int shmobile_iommu_attach_device(struct iommu_domain *domain,
-					struct device *dev)
-{
-	struct shmobile_iommu_archdata *archdata = dev->archdata.iommu;
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-	int ret = -EBUSY;
-
-	if (!archdata)
-		return -ENODEV;
-	spin_lock(&sh_domain->attached_list_lock);
-	spin_lock(&archdata->attach_lock);
-	if (archdata->attached != sh_domain) {
-		if (archdata->attached)
-			goto err;
-		ipmmu_tlb_set(archdata->ipmmu, sh_domain->l1.handle, L1_SIZE,
-			      0);
-		ipmmu_tlb_flush(archdata->ipmmu);
-		archdata->attached = sh_domain;
-		archdata->num_attached_devices = 0;
-		list_add(&archdata->attached_list, &sh_domain->attached_list);
-	}
-	archdata->num_attached_devices++;
-	ret = 0;
-err:
-	spin_unlock(&archdata->attach_lock);
-	spin_unlock(&sh_domain->attached_list_lock);
-	return ret;
-}
-
-static void shmobile_iommu_detach_device(struct iommu_domain *domain,
-					 struct device *dev)
-{
-	struct shmobile_iommu_archdata *archdata = dev->archdata.iommu;
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-
-	if (!archdata)
-		return;
-	spin_lock(&sh_domain->attached_list_lock);
-	spin_lock(&archdata->attach_lock);
-	archdata->num_attached_devices--;
-	if (!archdata->num_attached_devices) {
-		ipmmu_tlb_set(archdata->ipmmu, 0, 0, 0);
-		ipmmu_tlb_flush(archdata->ipmmu);
-		archdata->attached = NULL;
-		list_del(&archdata->attached_list);
-	}
-	spin_unlock(&archdata->attach_lock);
-	spin_unlock(&sh_domain->attached_list_lock);
-}
-
-static void domain_tlb_flush(struct shmobile_iommu_domain *sh_domain)
-{
-	struct shmobile_iommu_archdata *archdata;
-
-	spin_lock(&sh_domain->attached_list_lock);
-	list_for_each_entry(archdata, &sh_domain->attached_list, attached_list)
-		ipmmu_tlb_flush(archdata->ipmmu);
-	spin_unlock(&sh_domain->attached_list_lock);
-}
-
-static int l2alloc(struct shmobile_iommu_domain *sh_domain,
-		   unsigned int l1index)
-{
-	int ret;
-
-	if (!sh_domain->l2[l1index].pgtable) {
-		ret = pgtable_alloc(&sh_domain->l2[l1index], l2cache, L2_SIZE);
-		if (ret < 0)
-			return ret;
-	}
-	pgtable_write(&sh_domain->l1, l1index, 1,
-		      sh_domain->l2[l1index].handle | 0x1);
-	return 0;
-}
-
-static void l2realfree(struct shmobile_iommu_domain_pgtable *l2)
-{
-	if (l2->pgtable)
-		pgtable_free(l2, l2cache, L2_SIZE);
-}
-
-static void l2free(struct shmobile_iommu_domain *sh_domain,
-		   unsigned int l1index,
-		   struct shmobile_iommu_domain_pgtable *l2)
-{
-	pgtable_write(&sh_domain->l1, l1index, 1, 0);
-	if (sh_domain->l2[l1index].pgtable) {
-		*l2 = sh_domain->l2[l1index];
-		sh_domain->l2[l1index].pgtable = NULL;
-	}
-}
-
-static int shmobile_iommu_map(struct iommu_domain *domain, unsigned long iova,
-			      phys_addr_t paddr, size_t size, int prot)
-{
-	struct shmobile_iommu_domain_pgtable l2 = { .pgtable = NULL };
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-	unsigned int l1index, l2index;
-	int ret;
-
-	l1index = iova >> 20;
-	switch (size) {
-	case SZ_4K:
-		l2index = (iova >> 12) & 0xff;
-		spin_lock(&sh_domain->map_lock);
-		ret = l2alloc(sh_domain, l1index);
-		if (!ret)
-			pgtable_write(&sh_domain->l2[l1index], l2index, 1,
-				      paddr | 0xff2);
-		spin_unlock(&sh_domain->map_lock);
-		break;
-	case SZ_64K:
-		l2index = (iova >> 12) & 0xf0;
-		spin_lock(&sh_domain->map_lock);
-		ret = l2alloc(sh_domain, l1index);
-		if (!ret)
-			pgtable_write(&sh_domain->l2[l1index], l2index, 0x10,
-				      paddr | 0xff1);
-		spin_unlock(&sh_domain->map_lock);
-		break;
-	case SZ_1M:
-		spin_lock(&sh_domain->map_lock);
-		l2free(sh_domain, l1index, &l2);
-		pgtable_write(&sh_domain->l1, l1index, 1, paddr | 0xc02);
-		spin_unlock(&sh_domain->map_lock);
-		ret = 0;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-	if (!ret)
-		domain_tlb_flush(sh_domain);
-	l2realfree(&l2);
-	return ret;
-}
-
-static size_t shmobile_iommu_unmap(struct iommu_domain *domain,
-				   unsigned long iova, size_t size)
-{
-	struct shmobile_iommu_domain_pgtable l2 = { .pgtable = NULL };
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-	unsigned int l1index, l2index;
-	uint32_t l2entry = 0;
-	size_t ret = 0;
-
-	l1index = iova >> 20;
-	if (!(iova & 0xfffff) && size >= SZ_1M) {
-		spin_lock(&sh_domain->map_lock);
-		l2free(sh_domain, l1index, &l2);
-		spin_unlock(&sh_domain->map_lock);
-		ret = SZ_1M;
-		goto done;
-	}
-	l2index = (iova >> 12) & 0xff;
-	spin_lock(&sh_domain->map_lock);
-	if (sh_domain->l2[l1index].pgtable)
-		l2entry = pgtable_read(&sh_domain->l2[l1index], l2index);
-	switch (l2entry & 3) {
-	case 1:
-		if (l2index & 0xf)
-			break;
-		pgtable_write(&sh_domain->l2[l1index], l2index, 0x10, 0);
-		ret = SZ_64K;
-		break;
-	case 2:
-		pgtable_write(&sh_domain->l2[l1index], l2index, 1, 0);
-		ret = SZ_4K;
-		break;
-	}
-	spin_unlock(&sh_domain->map_lock);
-done:
-	if (ret)
-		domain_tlb_flush(sh_domain);
-	l2realfree(&l2);
-	return ret;
-}
-
-static phys_addr_t shmobile_iommu_iova_to_phys(struct iommu_domain *domain,
-					       dma_addr_t iova)
-{
-	struct shmobile_iommu_domain *sh_domain = to_sh_domain(domain);
-	uint32_t l1entry = 0, l2entry = 0;
-	unsigned int l1index, l2index;
-
-	l1index = iova >> 20;
-	l2index = (iova >> 12) & 0xff;
-	spin_lock(&sh_domain->map_lock);
-	if (sh_domain->l2[l1index].pgtable)
-		l2entry = pgtable_read(&sh_domain->l2[l1index], l2index);
-	else
-		l1entry = pgtable_read(&sh_domain->l1, l1index);
-	spin_unlock(&sh_domain->map_lock);
-	switch (l2entry & 3) {
-	case 1:
-		return (l2entry & ~0xffff) | (iova & 0xffff);
-	case 2:
-		return (l2entry & ~0xfff) | (iova & 0xfff);
-	default:
-		if ((l1entry & 3) == 2)
-			return (l1entry & ~0xfffff) | (iova & 0xfffff);
-		return 0;
-	}
-}
-
-static int find_dev_name(struct shmobile_ipmmu *ipmmu, const char *dev_name)
-{
-	unsigned int i, n = ipmmu->num_dev_names;
-
-	for (i = 0; i < n; i++) {
-		if (strcmp(ipmmu->dev_names[i], dev_name) == 0)
-			return 1;
-	}
-	return 0;
-}
-
-static int shmobile_iommu_add_device(struct device *dev)
-{
-	struct shmobile_iommu_archdata *archdata = ipmmu_archdata;
-	struct dma_iommu_mapping *mapping;
-
-	if (!find_dev_name(archdata->ipmmu, dev_name(dev)))
-		return 0;
-	mapping = archdata->iommu_mapping;
-	if (!mapping) {
-		mapping = arm_iommu_create_mapping(&platform_bus_type, 0,
-						   L1_LEN << 20);
-		if (IS_ERR(mapping))
-			return PTR_ERR(mapping);
-		archdata->iommu_mapping = mapping;
-	}
-	dev->archdata.iommu = archdata;
-	if (arm_iommu_attach_device(dev, mapping))
-		pr_err("arm_iommu_attach_device failed\n");
-	return 0;
-}
-
-static const struct iommu_ops shmobile_iommu_ops = {
-	.domain_alloc = shmobile_iommu_domain_alloc,
-	.domain_free = shmobile_iommu_domain_free,
-	.attach_dev = shmobile_iommu_attach_device,
-	.detach_dev = shmobile_iommu_detach_device,
-	.map = shmobile_iommu_map,
-	.unmap = shmobile_iommu_unmap,
-	.map_sg = default_iommu_map_sg,
-	.iova_to_phys = shmobile_iommu_iova_to_phys,
-	.add_device = shmobile_iommu_add_device,
-	.pgsize_bitmap = SZ_1M | SZ_64K | SZ_4K,
-};
-
-int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu)
-{
-	static struct shmobile_iommu_archdata *archdata;
-
-	l1cache = kmem_cache_create("shmobile-iommu-pgtable1", L1_SIZE,
-				    L1_ALIGN, SLAB_HWCACHE_ALIGN, NULL);
-	if (!l1cache)
-		return -ENOMEM;
-	l2cache = kmem_cache_create("shmobile-iommu-pgtable2", L2_SIZE,
-				    L2_ALIGN, SLAB_HWCACHE_ALIGN, NULL);
-	if (!l2cache) {
-		kmem_cache_destroy(l1cache);
-		return -ENOMEM;
-	}
-	archdata = kzalloc(sizeof(*archdata), GFP_KERNEL);
-	if (!archdata) {
-		kmem_cache_destroy(l1cache);
-		kmem_cache_destroy(l2cache);
-		return -ENOMEM;
-	}
-	spin_lock_init(&archdata->attach_lock);
-	archdata->ipmmu = ipmmu;
-	ipmmu_archdata = archdata;
-	bus_set_iommu(&platform_bus_type, &shmobile_iommu_ops);
-	return 0;
-}
diff --git a/drivers/iommu/shmobile-ipmmu.c b/drivers/iommu/shmobile-ipmmu.c
deleted file mode 100644
index 951651a9746b..000000000000
--- a/drivers/iommu/shmobile-ipmmu.c
+++ /dev/null
@@ -1,129 +0,0 @@
-/*
- * IPMMU/IPMMUI
- * Copyright (C) 2012  Hideki EIRAKU
- *
- * This program is free software; 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/err.h>
-#include <linux/export.h>
-#include <linux/io.h>
-#include <linux/platform_device.h>
-#include <linux/slab.h>
-#include <linux/platform_data/sh_ipmmu.h>
-#include "shmobile-ipmmu.h"
-
-#define IMCTR1 0x000
-#define IMCTR2 0x004
-#define IMASID 0x010
-#define IMTTBR 0x014
-#define IMTTBCR 0x018
-
-#define IMCTR1_TLBEN (1 << 0)
-#define IMCTR1_FLUSH (1 << 1)
-
-static void ipmmu_reg_write(struct shmobile_ipmmu *ipmmu, unsigned long reg_off,
-			    unsigned long data)
-{
-	iowrite32(data, ipmmu->ipmmu_base + reg_off);
-}
-
-void ipmmu_tlb_flush(struct shmobile_ipmmu *ipmmu)
-{
-	if (!ipmmu)
-		return;
-
-	spin_lock(&ipmmu->flush_lock);
-	if (ipmmu->tlb_enabled)
-		ipmmu_reg_write(ipmmu, IMCTR1, IMCTR1_FLUSH | IMCTR1_TLBEN);
-	else
-		ipmmu_reg_write(ipmmu, IMCTR1, IMCTR1_FLUSH);
-	spin_unlock(&ipmmu->flush_lock);
-}
-
-void ipmmu_tlb_set(struct shmobile_ipmmu *ipmmu, unsigned long phys, int size,
-		   int asid)
-{
-	if (!ipmmu)
-		return;
-
-	spin_lock(&ipmmu->flush_lock);
-	switch (size) {
-	default:
-		ipmmu->tlb_enabled = 0;
-		break;
-	case 0x2000:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 1);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x1000:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 2);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x800:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 3);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x400:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 4);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x200:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 5);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x100:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 6);
-		ipmmu->tlb_enabled = 1;
-		break;
-	case 0x80:
-		ipmmu_reg_write(ipmmu, IMTTBCR, 7);
-		ipmmu->tlb_enabled = 1;
-		break;
-	}
-	ipmmu_reg_write(ipmmu, IMTTBR, phys);
-	ipmmu_reg_write(ipmmu, IMASID, asid);
-	spin_unlock(&ipmmu->flush_lock);
-}
-
-static int ipmmu_probe(struct platform_device *pdev)
-{
-	struct shmobile_ipmmu *ipmmu;
-	struct resource *res;
-	struct shmobile_ipmmu_platform_data *pdata = pdev->dev.platform_data;
-
-	ipmmu = devm_kzalloc(&pdev->dev, sizeof(*ipmmu), GFP_KERNEL);
-	if (!ipmmu) {
-		dev_err(&pdev->dev, "cannot allocate device data\n");
-		return -ENOMEM;
-	}
-	spin_lock_init(&ipmmu->flush_lock);
-	ipmmu->dev = &pdev->dev;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	ipmmu->ipmmu_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(ipmmu->ipmmu_base))
-		return PTR_ERR(ipmmu->ipmmu_base);
-
-	ipmmu->dev_names = pdata->dev_names;
-	ipmmu->num_dev_names = pdata->num_dev_names;
-	platform_set_drvdata(pdev, ipmmu);
-	ipmmu_reg_write(ipmmu, IMCTR1, 0x0); /* disable TLB */
-	ipmmu_reg_write(ipmmu, IMCTR2, 0x0); /* disable PMB */
-	return ipmmu_iommu_init(ipmmu);
-}
-
-static struct platform_driver ipmmu_driver = {
-	.probe = ipmmu_probe,
-	.driver = {
-		.name = "ipmmu",
-	},
-};
-
-static int __init ipmmu_init(void)
-{
-	return platform_driver_register(&ipmmu_driver);
-}
-subsys_initcall(ipmmu_init);
diff --git a/drivers/iommu/shmobile-ipmmu.h b/drivers/iommu/shmobile-ipmmu.h
deleted file mode 100644
index 9524743ca1fb..000000000000
--- a/drivers/iommu/shmobile-ipmmu.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/* shmobile-ipmmu.h
- *
- * Copyright (C) 2012  Hideki EIRAKU
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
- */
-
-#ifndef __SHMOBILE_IPMMU_H__
-#define __SHMOBILE_IPMMU_H__
-
-struct shmobile_ipmmu {
-	struct device *dev;
-	void __iomem *ipmmu_base;
-	int tlb_enabled;
-	spinlock_t flush_lock;
-	const char * const *dev_names;
-	unsigned int num_dev_names;
-};
-
-#ifdef CONFIG_SHMOBILE_IPMMU_TLB
-void ipmmu_tlb_flush(struct shmobile_ipmmu *ipmmu);
-void ipmmu_tlb_set(struct shmobile_ipmmu *ipmmu, unsigned long phys, int size,
-		   int asid);
-int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu);
-#else
-static inline int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu)
-{
-	return -EINVAL;
-}
-#endif
-
-#endif /* __SHMOBILE_IPMMU_H__ */
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index 4d7294e5d982..fb50911b3940 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -8,6 +8,11 @@ config ARM_GIC
 	select IRQ_DOMAIN_HIERARCHY
 	select MULTI_IRQ_HANDLER
 
+config ARM_GIC_MAX_NR
+	int
+	default 2 if ARCH_REALVIEW
+	default 1
+
 config ARM_GIC_V2M
 	bool
 	depends on ARM_GIC
@@ -27,6 +32,14 @@ config ARM_GIC_V3_ITS
 	bool
 	select PCI_MSI_IRQ_DOMAIN
 
+config HISILICON_IRQ_MBIGEN
+	bool "Support mbigen interrupt controller"
+	default n
+	depends on ARM_GIC_V3 && ARM_GIC_V3_ITS && GENERIC_MSI_IRQ_DOMAIN
+	help
+	 Enable the mbigen interrupt controller used on
+	 Hisilicon platform.
+
 config ARM_NVIC
 	bool
 	select IRQ_DOMAIN
@@ -117,6 +130,11 @@ config ORION_IRQCHIP
 	select IRQ_DOMAIN
 	select MULTI_IRQ_HANDLER
 
+config PIC32_EVIC
+	bool
+	select GENERIC_IRQ_CHIP
+	select IRQ_DOMAIN
+
 config RENESAS_INTC_IRQPIN
 	bool
 	select IRQ_DOMAIN
@@ -138,6 +156,13 @@ config TB10X_IRQC
 	select IRQ_DOMAIN
 	select GENERIC_IRQ_CHIP
 
+config TS4800_IRQ
+	tristate "TS-4800 IRQ controller"
+	select IRQ_DOMAIN
+	depends on HAS_IOMEM
+	help
+	  Support for the TS-4800 FPGA IRQ controller
+
 config VERSATILE_FPGA_IRQ
 	bool
 	select IRQ_DOMAIN
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 177f78f6e6d6..18caacb60d58 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -21,9 +21,11 @@ obj-$(CONFIG_ARCH_SUNXI)		+= irq-sun4i.o
 obj-$(CONFIG_ARCH_SUNXI)		+= irq-sunxi-nmi.o
 obj-$(CONFIG_ARCH_SPEAR3XX)		+= spear-shirq.o
 obj-$(CONFIG_ARM_GIC)			+= irq-gic.o irq-gic-common.o
+obj-$(CONFIG_REALVIEW_DT)		+= 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
+obj-$(CONFIG_HISILICON_IRQ_MBIGEN)	+= irq-mbigen.o
 obj-$(CONFIG_ARM_NVIC)			+= irq-nvic.o
 obj-$(CONFIG_ARM_VIC)			+= irq-vic.o
 obj-$(CONFIG_ATMEL_AIC_IRQ)		+= irq-atmel-aic-common.o irq-atmel-aic.o
@@ -39,6 +41,7 @@ obj-$(CONFIG_ARCH_NSPIRE)		+= irq-zevio.o
 obj-$(CONFIG_ARCH_VT8500)		+= irq-vt8500.o
 obj-$(CONFIG_ST_IRQCHIP)		+= irq-st.o
 obj-$(CONFIG_TB10X_IRQC)		+= irq-tb10x.o
+obj-$(CONFIG_TS4800_IRQ)		+= irq-ts4800.o
 obj-$(CONFIG_XTENSA)			+= irq-xtensa-pic.o
 obj-$(CONFIG_XTENSA_MX)			+= irq-xtensa-mx.o
 obj-$(CONFIG_IRQ_CROSSBAR)		+= irq-crossbar.o
@@ -55,3 +58,4 @@ obj-$(CONFIG_RENESAS_H8S_INTC)		+= irq-renesas-h8s.o
 obj-$(CONFIG_ARCH_SA1100)		+= irq-sa11x0.o
 obj-$(CONFIG_INGENIC_IRQ)		+= irq-ingenic.o
 obj-$(CONFIG_IMX_GPCV2)			+= irq-imx-gpcv2.o
+obj-$(CONFIG_PIC32_EVIC)		+= irq-pic32-evic.o
diff --git a/drivers/irqchip/irq-atmel-aic-common.c b/drivers/irqchip/irq-atmel-aic-common.c
index b12a5d58546f..37199b9b2cfa 100644
--- a/drivers/irqchip/irq-atmel-aic-common.c
+++ b/drivers/irqchip/irq-atmel-aic-common.c
@@ -86,7 +86,7 @@ int aic_common_set_priority(int priority, unsigned *val)
 	    priority > AT91_AIC_IRQ_MAX_PRIORITY)
 		return -EINVAL;
 
-	*val &= AT91_AIC_PRIOR;
+	*val &= ~AT91_AIC_PRIOR;
 	*val |= priority;
 
 	return 0;
diff --git a/drivers/irqchip/irq-bcm2836.c b/drivers/irqchip/irq-bcm2836.c
index f68708281fcf..963065a0d774 100644
--- a/drivers/irqchip/irq-bcm2836.c
+++ b/drivers/irqchip/irq-bcm2836.c
@@ -21,6 +21,9 @@
 #include <linux/irqdomain.h>
 #include <asm/exception.h>
 
+#define LOCAL_CONTROL			0x000
+#define LOCAL_PRESCALER			0x008
+
 /*
  * The low 2 bits identify the CPU that the GPU IRQ goes to, and the
  * next 2 bits identify the CPU that the GPU FIQ goes to.
@@ -50,14 +53,16 @@
 /* Same status bits as above, but for FIQ. */
 #define LOCAL_FIQ_PENDING0		0x070
 /*
- * Mailbox0 write-to-set bits.  There are 16 mailboxes, 4 per CPU, and
+ * Mailbox write-to-set bits.  There are 16 mailboxes, 4 per CPU, and
  * these bits are organized by mailbox number and then CPU number.  We
  * use mailbox 0 for IPIs.  The mailbox's interrupt is raised while
  * any bit is set.
  */
 #define LOCAL_MAILBOX0_SET0		0x080
-/* Mailbox0 write-to-clear bits. */
+#define LOCAL_MAILBOX3_SET0		0x08c
+/* Mailbox write-to-clear bits. */
 #define LOCAL_MAILBOX0_CLR0		0x0c0
+#define LOCAL_MAILBOX3_CLR0		0x0cc
 
 #define LOCAL_IRQ_CNTPSIRQ	0
 #define LOCAL_IRQ_CNTPNSIRQ	1
@@ -162,7 +167,7 @@ __exception_irq_entry bcm2836_arm_irqchip_handle_irq(struct pt_regs *regs)
 	u32 stat;
 
 	stat = readl_relaxed(intc.base + LOCAL_IRQ_PENDING0 + 4 * cpu);
-	if (stat & 0x10) {
+	if (stat & BIT(LOCAL_IRQ_MAILBOX0)) {
 #ifdef CONFIG_SMP
 		void __iomem *mailbox0 = (intc.base +
 					  LOCAL_MAILBOX0_CLR0 + 16 * cpu);
@@ -172,7 +177,7 @@ __exception_irq_entry bcm2836_arm_irqchip_handle_irq(struct pt_regs *regs)
 		writel(1 << ipi, mailbox0);
 		handle_IPI(ipi, regs);
 #endif
-	} else {
+	} else if (stat) {
 		u32 hwirq = ffs(stat) - 1;
 
 		handle_IRQ(irq_linear_revmap(intc.domain, hwirq), regs);
@@ -217,6 +222,24 @@ static struct notifier_block bcm2836_arm_irqchip_cpu_notifier = {
 	.notifier_call = bcm2836_arm_irqchip_cpu_notify,
 	.priority = 100,
 };
+
+int __init bcm2836_smp_boot_secondary(unsigned int cpu,
+				      struct task_struct *idle)
+{
+	unsigned long secondary_startup_phys =
+		(unsigned long)virt_to_phys((void *)secondary_startup);
+
+	dsb();
+	writel(secondary_startup_phys,
+	       intc.base + LOCAL_MAILBOX3_SET0 + 16 * cpu);
+
+	return 0;
+}
+
+static const struct smp_operations bcm2836_smp_ops __initconst = {
+	.smp_boot_secondary	= bcm2836_smp_boot_secondary,
+};
+
 #endif
 
 static const struct irq_domain_ops bcm2836_arm_irqchip_intc_ops = {
@@ -234,9 +257,31 @@ bcm2836_arm_irqchip_smp_init(void)
 	register_cpu_notifier(&bcm2836_arm_irqchip_cpu_notifier);
 
 	set_smp_cross_call(bcm2836_arm_irqchip_send_ipi);
+	smp_set_ops(&bcm2836_smp_ops);
 #endif
 }
 
+/*
+ * The LOCAL_IRQ_CNT* timer firings are based off of the external
+ * oscillator with some scaling.  The firmware sets up CNTFRQ to
+ * report 19.2Mhz, but doesn't set up the scaling registers.
+ */
+static void bcm2835_init_local_timer_frequency(void)
+{
+	/*
+	 * Set the timer to source from the 19.2Mhz crystal clock (bit
+	 * 8 unset), and only increment by 1 instead of 2 (bit 9
+	 * unset).
+	 */
+	writel(0, intc.base + LOCAL_CONTROL);
+
+	/*
+	 * Set the timer prescaler to 1:1 (timer freq = input freq *
+	 * 2**31 / prescaler)
+	 */
+	writel(0x80000000, intc.base + LOCAL_PRESCALER);
+}
+
 static int __init bcm2836_arm_irqchip_l1_intc_of_init(struct device_node *node,
 						      struct device_node *parent)
 {
@@ -246,6 +291,8 @@ static int __init bcm2836_arm_irqchip_l1_intc_of_init(struct device_node *node,
 			node->full_name);
 	}
 
+	bcm2835_init_local_timer_frequency();
+
 	intc.domain = irq_domain_add_linear(node, LAST_IRQ + 1,
 					    &bcm2836_arm_irqchip_intc_ops,
 					    NULL);
diff --git a/drivers/irqchip/irq-gic-realview.c b/drivers/irqchip/irq-gic-realview.c
new file mode 100644
index 000000000000..aa46eb280a7f
--- /dev/null
+++ b/drivers/irqchip/irq-gic-realview.c
@@ -0,0 +1,43 @@
+/*
+ * Special GIC quirks for the ARM RealView
+ * Copyright (C) 2015 Linus Walleij
+ */
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
+#include <linux/bitops.h>
+#include <linux/irqchip.h>
+#include <linux/irqchip/arm-gic.h>
+
+#define REALVIEW_SYS_LOCK_OFFSET	0x20
+#define REALVIEW_PB11MP_SYS_PLD_CTRL1	0x74
+#define VERSATILE_LOCK_VAL		0xA05F
+#define PLD_INTMODE_MASK		BIT(22)|BIT(23)|BIT(24)
+#define PLD_INTMODE_LEGACY		0x0
+#define PLD_INTMODE_NEW_DCC		BIT(22)
+#define PLD_INTMODE_NEW_NO_DCC		BIT(23)
+#define PLD_INTMODE_FIQ_ENABLE		BIT(24)
+
+static int __init
+realview_gic_of_init(struct device_node *node, struct device_node *parent)
+{
+	static struct regmap *map;
+
+	/* The PB11MPCore GIC needs to be configured in the syscon */
+	map = syscon_regmap_lookup_by_compatible("arm,realview-pb11mp-syscon");
+	if (!IS_ERR(map)) {
+		/* new irq mode with no DCC */
+		regmap_write(map, REALVIEW_SYS_LOCK_OFFSET,
+			     VERSATILE_LOCK_VAL);
+		regmap_update_bits(map, REALVIEW_PB11MP_SYS_PLD_CTRL1,
+				   PLD_INTMODE_NEW_NO_DCC,
+				   PLD_INTMODE_MASK);
+		regmap_write(map, REALVIEW_SYS_LOCK_OFFSET, 0x0000);
+		pr_info("TC11MP GIC: set up interrupt controller to NEW mode, no DCC\n");
+	} else {
+		pr_err("TC11MP GIC setup: could not find syscon\n");
+		return -ENXIO;
+	}
+	return gic_of_init(node, parent);
+}
+IRQCHIP_DECLARE(armtc11mp_gic, "arm,tc11mp-gic", realview_gic_of_init);
diff --git a/drivers/irqchip/irq-gic-v2m.c b/drivers/irqchip/irq-gic-v2m.c
index 87f8d104acab..c779f83e511d 100644
--- a/drivers/irqchip/irq-gic-v2m.c
+++ b/drivers/irqchip/irq-gic-v2m.c
@@ -15,9 +15,11 @@
 
 #define pr_fmt(fmt) "GICv2m: " fmt
 
+#include <linux/acpi.h>
 #include <linux/irq.h>
 #include <linux/irqdomain.h>
 #include <linux/kernel.h>
+#include <linux/msi.h>
 #include <linux/of_address.h>
 #include <linux/of_pci.h>
 #include <linux/slab.h>
@@ -55,7 +57,7 @@ static DEFINE_SPINLOCK(v2m_lock);
 
 struct v2m_data {
 	struct list_head entry;
-	struct device_node *node;
+	struct fwnode_handle *fwnode;
 	struct resource res;	/* GICv2m resource */
 	void __iomem *base;	/* GICv2m virt address */
 	u32 spi_start;		/* The SPI number that MSIs start */
@@ -138,6 +140,11 @@ static int gicv2m_irq_gic_domain_alloc(struct irq_domain *domain,
 		fwspec.param[0] = 0;
 		fwspec.param[1] = hwirq - 32;
 		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;
 	}
@@ -254,7 +261,9 @@ static void gicv2m_teardown(void)
 		list_del(&v2m->entry);
 		kfree(v2m->bm);
 		iounmap(v2m->base);
-		of_node_put(v2m->node);
+		of_node_put(to_of_node(v2m->fwnode));
+		if (is_fwnode_irqchip(v2m->fwnode))
+			irq_domain_free_fwnode(v2m->fwnode);
 		kfree(v2m);
 	}
 }
@@ -268,7 +277,7 @@ static int gicv2m_allocate_domains(struct irq_domain *parent)
 	if (!v2m)
 		return 0;
 
-	inner_domain = irq_domain_create_tree(of_node_to_fwnode(v2m->node),
+	inner_domain = irq_domain_create_tree(v2m->fwnode,
 					      &gicv2m_domain_ops, v2m);
 	if (!inner_domain) {
 		pr_err("Failed to create GICv2m domain\n");
@@ -277,10 +286,10 @@ static int gicv2m_allocate_domains(struct irq_domain *parent)
 
 	inner_domain->bus_token = DOMAIN_BUS_NEXUS;
 	inner_domain->parent = parent;
-	pci_domain = pci_msi_create_irq_domain(of_node_to_fwnode(v2m->node),
+	pci_domain = pci_msi_create_irq_domain(v2m->fwnode,
 					       &gicv2m_msi_domain_info,
 					       inner_domain);
-	plat_domain = platform_msi_create_irq_domain(of_node_to_fwnode(v2m->node),
+	plat_domain = platform_msi_create_irq_domain(v2m->fwnode,
 						     &gicv2m_pmsi_domain_info,
 						     inner_domain);
 	if (!pci_domain || !plat_domain) {
@@ -296,8 +305,9 @@ static int gicv2m_allocate_domains(struct irq_domain *parent)
 	return 0;
 }
 
-static int __init gicv2m_init_one(struct device_node *node,
-				  struct irq_domain *parent)
+static int __init gicv2m_init_one(struct fwnode_handle *fwnode,
+				  u32 spi_start, u32 nr_spis,
+				  struct resource *res)
 {
 	int ret;
 	struct v2m_data *v2m;
@@ -309,13 +319,9 @@ static int __init gicv2m_init_one(struct device_node *node,
 	}
 
 	INIT_LIST_HEAD(&v2m->entry);
-	v2m->node = node;
+	v2m->fwnode = fwnode;
 
-	ret = of_address_to_resource(node, 0, &v2m->res);
-	if (ret) {
-		pr_err("Failed to allocate v2m resource.\n");
-		goto err_free_v2m;
-	}
+	memcpy(&v2m->res, res, sizeof(struct resource));
 
 	v2m->base = ioremap(v2m->res.start, resource_size(&v2m->res));
 	if (!v2m->base) {
@@ -324,10 +330,9 @@ static int __init gicv2m_init_one(struct device_node *node,
 		goto err_free_v2m;
 	}
 
-	if (!of_property_read_u32(node, "arm,msi-base-spi", &v2m->spi_start) &&
-	    !of_property_read_u32(node, "arm,msi-num-spis", &v2m->nr_spis)) {
-		pr_info("Overriding V2M MSI_TYPER (base:%u, num:%u)\n",
-			v2m->spi_start, v2m->nr_spis);
+	if (spi_start && nr_spis) {
+		v2m->spi_start = spi_start;
+		v2m->nr_spis = nr_spis;
 	} else {
 		u32 typer = readl_relaxed(v2m->base + V2M_MSI_TYPER);
 
@@ -359,10 +364,9 @@ static int __init gicv2m_init_one(struct device_node *node,
 	}
 
 	list_add_tail(&v2m->entry, &v2m_nodes);
-	pr_info("Node %s: range[%#lx:%#lx], SPI[%d:%d]\n", node->name,
-		(unsigned long)v2m->res.start, (unsigned long)v2m->res.end,
-		v2m->spi_start, (v2m->spi_start + v2m->nr_spis));
 
+	pr_info("range%pR, SPI[%d:%d]\n", res,
+		v2m->spi_start, (v2m->spi_start + v2m->nr_spis - 1));
 	return 0;
 
 err_iounmap:
@@ -377,19 +381,36 @@ static struct of_device_id gicv2m_device_id[] = {
 	{},
 };
 
-int __init gicv2m_of_init(struct device_node *node, struct irq_domain *parent)
+static int __init gicv2m_of_init(struct fwnode_handle *parent_handle,
+				 struct irq_domain *parent)
 {
 	int ret = 0;
+	struct device_node *node = to_of_node(parent_handle);
 	struct device_node *child;
 
 	for (child = of_find_matching_node(node, gicv2m_device_id); child;
 	     child = of_find_matching_node(child, gicv2m_device_id)) {
+		u32 spi_start = 0, nr_spis = 0;
+		struct resource res;
+
 		if (!of_find_property(child, "msi-controller", NULL))
 			continue;
 
-		ret = gicv2m_init_one(child, parent);
+		ret = of_address_to_resource(child, 0, &res);
+		if (ret) {
+			pr_err("Failed to allocate v2m resource.\n");
+			break;
+		}
+
+		if (!of_property_read_u32(child, "arm,msi-base-spi",
+					  &spi_start) &&
+		    !of_property_read_u32(child, "arm,msi-num-spis", &nr_spis))
+			pr_info("DT overriding V2M MSI_TYPER (base:%u, num:%u)\n",
+				spi_start, nr_spis);
+
+		ret = gicv2m_init_one(&child->fwnode, spi_start, nr_spis, &res);
 		if (ret) {
-			of_node_put(node);
+			of_node_put(child);
 			break;
 		}
 	}
@@ -400,3 +421,101 @@ int __init gicv2m_of_init(struct device_node *node, struct irq_domain *parent)
 		gicv2m_teardown();
 	return ret;
 }
+
+#ifdef CONFIG_ACPI
+static int acpi_num_msi;
+
+static struct fwnode_handle *gicv2m_get_fwnode(struct device *dev)
+{
+	struct v2m_data *data;
+
+	if (WARN_ON(acpi_num_msi <= 0))
+		return NULL;
+
+	/* We only return the fwnode of the first MSI frame. */
+	data = list_first_entry_or_null(&v2m_nodes, struct v2m_data, entry);
+	if (!data)
+		return NULL;
+
+	return data->fwnode;
+}
+
+static int __init
+acpi_parse_madt_msi(struct acpi_subtable_header *header,
+		    const unsigned long end)
+{
+	int ret;
+	struct resource res;
+	u32 spi_start = 0, nr_spis = 0;
+	struct acpi_madt_generic_msi_frame *m;
+	struct fwnode_handle *fwnode;
+
+	m = (struct acpi_madt_generic_msi_frame *)header;
+	if (BAD_MADT_ENTRY(m, end))
+		return -EINVAL;
+
+	res.start = m->base_address;
+	res.end = m->base_address + SZ_4K - 1;
+	res.flags = IORESOURCE_MEM;
+
+	if (m->flags & ACPI_MADT_OVERRIDE_SPI_VALUES) {
+		spi_start = m->spi_base;
+		nr_spis = m->spi_count;
+
+		pr_info("ACPI overriding V2M MSI_TYPER (base:%u, num:%u)\n",
+			spi_start, nr_spis);
+	}
+
+	fwnode = irq_domain_alloc_fwnode((void *)m->base_address);
+	if (!fwnode) {
+		pr_err("Unable to allocate GICv2m domain token\n");
+		return -EINVAL;
+	}
+
+	ret = gicv2m_init_one(fwnode, spi_start, nr_spis, &res);
+	if (ret)
+		irq_domain_free_fwnode(fwnode);
+
+	return ret;
+}
+
+static int __init gicv2m_acpi_init(struct irq_domain *parent)
+{
+	int ret;
+
+	if (acpi_num_msi > 0)
+		return 0;
+
+	acpi_num_msi = acpi_table_parse_madt(ACPI_MADT_TYPE_GENERIC_MSI_FRAME,
+				      acpi_parse_madt_msi, 0);
+
+	if (acpi_num_msi <= 0)
+		goto err_out;
+
+	ret = gicv2m_allocate_domains(parent);
+	if (ret)
+		goto err_out;
+
+	pci_msi_register_fwnode_provider(&gicv2m_get_fwnode);
+
+	return 0;
+
+err_out:
+	gicv2m_teardown();
+	return -EINVAL;
+}
+#else /* CONFIG_ACPI */
+static int __init gicv2m_acpi_init(struct irq_domain *parent)
+{
+	return -EINVAL;
+}
+#endif /* CONFIG_ACPI */
+
+int __init gicv2m_init(struct fwnode_handle *parent_handle,
+		       struct irq_domain *parent)
+{
+	if (is_of_node(parent_handle))
+		return gicv2m_of_init(parent_handle, parent);
+
+	return gicv2m_acpi_init(parent);
+}
diff --git a/drivers/irqchip/irq-gic-v3-its.c b/drivers/irqchip/irq-gic-v3-its.c
index e23d1d18f9d6..43dfd15c1dd2 100644
--- a/drivers/irqchip/irq-gic-v3-its.c
+++ b/drivers/irqchip/irq-gic-v3-its.c
@@ -66,7 +66,10 @@ struct its_node {
 	unsigned long		phys_base;
 	struct its_cmd_block	*cmd_base;
 	struct its_cmd_block	*cmd_write;
-	void			*tables[GITS_BASER_NR_REGS];
+	struct {
+		void		*base;
+		u32		order;
+	} tables[GITS_BASER_NR_REGS];
 	struct its_collection	*collections;
 	struct list_head	its_device_list;
 	u64			flags;
@@ -75,6 +78,9 @@ struct its_node {
 
 #define ITS_ITT_ALIGN		SZ_256
 
+/* Convert page order to size in bytes */
+#define PAGE_ORDER_TO_SIZE(o)	(PAGE_SIZE << (o))
+
 struct event_lpi_map {
 	unsigned long		*lpi_map;
 	u16			*col_map;
@@ -597,11 +603,6 @@ static void its_unmask_irq(struct irq_data *d)
 	lpi_set_config(d, true);
 }
 
-static void its_eoi_irq(struct irq_data *d)
-{
-	gic_write_eoir(d->hwirq);
-}
-
 static int its_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 			    bool force)
 {
@@ -638,7 +639,7 @@ static struct irq_chip its_irq_chip = {
 	.name			= "ITS",
 	.irq_mask		= its_mask_irq,
 	.irq_unmask		= its_unmask_irq,
-	.irq_eoi		= its_eoi_irq,
+	.irq_eoi		= irq_chip_eoi_parent,
 	.irq_set_affinity	= its_set_affinity,
 	.irq_compose_msi_msg	= its_irq_compose_msi_msg,
 };
@@ -807,9 +808,10 @@ static void its_free_tables(struct its_node *its)
 	int i;
 
 	for (i = 0; i < GITS_BASER_NR_REGS; i++) {
-		if (its->tables[i]) {
-			free_page((unsigned long)its->tables[i]);
-			its->tables[i] = NULL;
+		if (its->tables[i].base) {
+			free_pages((unsigned long)its->tables[i].base,
+				   its->tables[i].order);
+			its->tables[i].base = NULL;
 		}
 	}
 }
@@ -842,7 +844,6 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 		u64 type = GITS_BASER_TYPE(val);
 		u64 entry_size = GITS_BASER_ENTRY_SIZE(val);
 		int order = get_order(psz);
-		int alloc_size;
 		int alloc_pages;
 		u64 tmp;
 		void *base;
@@ -874,8 +875,8 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 			}
 		}
 
-		alloc_size = (1 << order) * PAGE_SIZE;
-		alloc_pages = (alloc_size / psz);
+retry_alloc_baser:
+		alloc_pages = (PAGE_ORDER_TO_SIZE(order) / psz);
 		if (alloc_pages > GITS_BASER_PAGES_MAX) {
 			alloc_pages = GITS_BASER_PAGES_MAX;
 			order = get_order(GITS_BASER_PAGES_MAX * psz);
@@ -889,7 +890,8 @@ static int its_alloc_tables(const char *node_name, struct its_node *its)
 			goto out_free;
 		}
 
-		its->tables[i] = base;
+		its->tables[i].base = base;
+		its->tables[i].order = order;
 
 retry_baser:
 		val = (virt_to_phys(base) 				 |
@@ -927,7 +929,7 @@ retry_baser:
 			shr = tmp & GITS_BASER_SHAREABILITY_MASK;
 			if (!shr) {
 				cache = GITS_BASER_nC;
-				__flush_dcache_area(base, alloc_size);
+				__flush_dcache_area(base, PAGE_ORDER_TO_SIZE(order));
 			}
 			goto retry_baser;
 		}
@@ -938,13 +940,16 @@ retry_baser:
 			 * size and retry. If we reach 4K, then
 			 * something is horribly wrong...
 			 */
+			free_pages((unsigned long)base, order);
+			its->tables[i].base = NULL;
+
 			switch (psz) {
 			case SZ_16K:
 				psz = SZ_4K;
-				goto retry_baser;
+				goto retry_alloc_baser;
 			case SZ_64K:
 				psz = SZ_16K;
-				goto retry_baser;
+				goto retry_alloc_baser;
 			}
 		}
 
@@ -957,7 +962,7 @@ retry_baser:
 		}
 
 		pr_info("ITS: allocated %d %s @%lx (psz %dK, shr %d)\n",
-			(int)(alloc_size / entry_size),
+			(int)(PAGE_ORDER_TO_SIZE(order) / entry_size),
 			its_base_type_string[type],
 			(unsigned long)virt_to_phys(base),
 			psz / SZ_1K, (int)shr >> GITS_BASER_SHAREABILITY_SHIFT);
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index abf2ffaed392..8f9ebf714e2b 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -69,6 +69,7 @@ union gic_base {
 };
 
 struct gic_chip_data {
+	struct irq_chip chip;
 	union gic_base dist_base;
 	union gic_base cpu_base;
 #ifdef CONFIG_CPU_PM
@@ -99,11 +100,7 @@ static u8 gic_cpu_map[NR_GIC_CPU_IF] __read_mostly;
 
 static struct static_key supports_deactivate = STATIC_KEY_INIT_TRUE;
 
-#ifndef MAX_GIC_NR
-#define MAX_GIC_NR	1
-#endif
-
-static struct gic_chip_data gic_data[MAX_GIC_NR] __read_mostly;
+static struct gic_chip_data gic_data[CONFIG_ARM_GIC_MAX_NR] __read_mostly;
 
 #ifdef CONFIG_GIC_NON_BANKED
 static void __iomem *gic_get_percpu_base(union gic_base *base)
@@ -336,7 +333,7 @@ static void __exception_irq_entry gic_handle_irq(struct pt_regs *regs)
 		irqstat = readl_relaxed(cpu_base + GIC_CPU_INTACK);
 		irqnr = irqstat & GICC_IAR_INT_ID_MASK;
 
-		if (likely(irqnr > 15 && irqnr < 1021)) {
+		if (likely(irqnr > 15 && irqnr < 1020)) {
 			if (static_key_true(&supports_deactivate))
 				writel_relaxed(irqstat, cpu_base + GIC_CPU_EOI);
 			handle_domain_irq(gic->domain, irqnr, regs);
@@ -383,14 +380,10 @@ static void gic_handle_cascade_irq(struct irq_desc *desc)
 }
 
 static struct irq_chip gic_chip = {
-	.name			= "GIC",
 	.irq_mask		= gic_mask_irq,
 	.irq_unmask		= gic_unmask_irq,
 	.irq_eoi		= gic_eoi_irq,
 	.irq_set_type		= gic_set_type,
-#ifdef CONFIG_SMP
-	.irq_set_affinity	= gic_set_affinity,
-#endif
 	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
 	.flags			= IRQCHIP_SET_TYPE_MASKED |
@@ -404,9 +397,6 @@ static struct irq_chip gic_eoimode1_chip = {
 	.irq_unmask		= gic_unmask_irq,
 	.irq_eoi		= gic_eoimode1_eoi_irq,
 	.irq_set_type		= gic_set_type,
-#ifdef CONFIG_SMP
-	.irq_set_affinity	= gic_set_affinity,
-#endif
 	.irq_get_irqchip_state	= gic_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gic_irq_set_irqchip_state,
 	.irq_set_vcpu_affinity	= gic_irq_set_vcpu_affinity,
@@ -417,8 +407,7 @@ static struct irq_chip gic_eoimode1_chip = {
 
 void __init gic_cascade_irq(unsigned int gic_nr, unsigned int irq)
 {
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 	irq_set_chained_handler_and_data(irq, gic_handle_cascade_irq,
 					 &gic_data[gic_nr]);
 }
@@ -448,7 +437,7 @@ static void gic_cpu_if_up(struct gic_chip_data *gic)
 	u32 bypass = 0;
 	u32 mode = 0;
 
-	if (static_key_true(&supports_deactivate))
+	if (gic == &gic_data[0] && static_key_true(&supports_deactivate))
 		mode = GIC_CPU_CTRL_EOImodeNS;
 
 	/*
@@ -524,7 +513,7 @@ int gic_cpu_if_down(unsigned int gic_nr)
 	void __iomem *cpu_base;
 	u32 val = 0;
 
-	if (gic_nr >= MAX_GIC_NR)
+	if (gic_nr >= CONFIG_ARM_GIC_MAX_NR)
 		return -EINVAL;
 
 	cpu_base = gic_data_cpu_base(&gic_data[gic_nr]);
@@ -548,8 +537,7 @@ static void gic_dist_save(unsigned int gic_nr)
 	void __iomem *dist_base;
 	int i;
 
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	gic_irqs = gic_data[gic_nr].gic_irqs;
 	dist_base = gic_data_dist_base(&gic_data[gic_nr]);
@@ -587,8 +575,7 @@ static void gic_dist_restore(unsigned int gic_nr)
 	unsigned int i;
 	void __iomem *dist_base;
 
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	gic_irqs = gic_data[gic_nr].gic_irqs;
 	dist_base = gic_data_dist_base(&gic_data[gic_nr]);
@@ -634,8 +621,7 @@ static void gic_cpu_save(unsigned int gic_nr)
 	void __iomem *dist_base;
 	void __iomem *cpu_base;
 
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	dist_base = gic_data_dist_base(&gic_data[gic_nr]);
 	cpu_base = gic_data_cpu_base(&gic_data[gic_nr]);
@@ -664,8 +650,7 @@ static void gic_cpu_restore(unsigned int gic_nr)
 	void __iomem *dist_base;
 	void __iomem *cpu_base;
 
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	dist_base = gic_data_dist_base(&gic_data[gic_nr]);
 	cpu_base = gic_data_cpu_base(&gic_data[gic_nr]);
@@ -703,7 +688,7 @@ static int gic_notifier(struct notifier_block *self, unsigned long cmd,	void *v)
 {
 	int i;
 
-	for (i = 0; i < MAX_GIC_NR; i++) {
+	for (i = 0; i < CONFIG_ARM_GIC_MAX_NR; i++) {
 #ifdef CONFIG_GIC_NON_BANKED
 		/* Skip over unused GICs */
 		if (!gic_data[i].get_base)
@@ -835,8 +820,7 @@ void gic_migrate_target(unsigned int new_cpu_id)
 	int i, ror_val, cpu = smp_processor_id();
 	u32 val, cur_target_mask, active_mask;
 
-	if (gic_nr >= MAX_GIC_NR)
-		BUG();
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	dist_base = gic_data_dist_base(&gic_data[gic_nr]);
 	if (!dist_base)
@@ -925,20 +909,15 @@ void __init gic_init_physaddr(struct device_node *node)
 static int gic_irq_domain_map(struct irq_domain *d, unsigned int irq,
 				irq_hw_number_t hw)
 {
-	struct irq_chip *chip = &gic_chip;
-
-	if (static_key_true(&supports_deactivate)) {
-		if (d->host_data == (void *)&gic_data[0])
-			chip = &gic_eoimode1_chip;
-	}
+	struct gic_chip_data *gic = d->host_data;
 
 	if (hw < 32) {
 		irq_set_percpu_devid(irq);
-		irq_domain_set_info(d, irq, hw, chip, d->host_data,
+		irq_domain_set_info(d, irq, hw, &gic->chip, d->host_data,
 				    handle_percpu_devid_irq, NULL, NULL);
 		irq_set_status_flags(irq, IRQ_NOAUTOEN);
 	} else {
-		irq_domain_set_info(d, irq, hw, chip, d->host_data,
+		irq_domain_set_info(d, irq, hw, &gic->chip, d->host_data,
 				    handle_fasteoi_irq, NULL, NULL);
 		irq_set_probe(irq);
 	}
@@ -972,7 +951,7 @@ static int gic_irq_domain_translate(struct irq_domain *d,
 		return 0;
 	}
 
-	if (fwspec->fwnode->type == FWNODE_IRQCHIP) {
+	if (is_fwnode_irqchip(fwspec->fwnode)) {
 		if(fwspec->param_count != 2)
 			return -EINVAL;
 
@@ -1040,11 +1019,25 @@ static void __init __gic_init_bases(unsigned int gic_nr, int irq_start,
 	struct gic_chip_data *gic;
 	int gic_irqs, irq_base, i;
 
-	BUG_ON(gic_nr >= MAX_GIC_NR);
+	BUG_ON(gic_nr >= CONFIG_ARM_GIC_MAX_NR);
 
 	gic_check_cpu_features();
 
 	gic = &gic_data[gic_nr];
+
+	/* Initialize irq_chip */
+	if (static_key_true(&supports_deactivate) && gic_nr == 0) {
+		gic->chip = gic_eoimode1_chip;
+	} else {
+		gic->chip = gic_chip;
+		gic->chip.name = kasprintf(GFP_KERNEL, "GIC-%d", gic_nr);
+	}
+
+#ifdef CONFIG_SMP
+	if (gic_nr == 0)
+		gic->chip.irq_set_affinity = gic_set_affinity;
+#endif
+
 #ifdef CONFIG_GIC_NON_BANKED
 	if (percpu_offset) { /* Frankein-GIC without banked registers... */
 		unsigned int cpu;
@@ -1196,7 +1189,7 @@ static bool gic_check_eoimode(struct device_node *node, void __iomem **base)
 	return true;
 }
 
-static int __init
+int __init
 gic_of_init(struct device_node *node, struct device_node *parent)
 {
 	void __iomem *cpu_base;
@@ -1234,7 +1227,7 @@ gic_of_init(struct device_node *node, struct device_node *parent)
 	}
 
 	if (IS_ENABLED(CONFIG_ARM_GIC_V2M))
-		gicv2m_of_init(node, gic_data[gic_cnt].domain);
+		gicv2m_init(&node->fwnode, gic_data[gic_cnt].domain);
 
 	gic_cnt++;
 	return 0;
@@ -1359,6 +1352,10 @@ static int __init gic_v2_acpi_init(struct acpi_subtable_header *header,
 	__gic_init_bases(0, -1, dist_base, cpu_base, 0, domain_handle);
 
 	acpi_set_irq_model(ACPI_IRQ_MODEL_GIC, domain_handle);
+
+	if (IS_ENABLED(CONFIG_ARM_GIC_V2M))
+		gicv2m_init(NULL, gic_data[0].domain);
+
 	return 0;
 }
 IRQCHIP_ACPI_DECLARE(gic_v2, ACPI_MADT_TYPE_GENERIC_DISTRIBUTOR,
diff --git a/drivers/irqchip/irq-mbigen.c b/drivers/irqchip/irq-mbigen.c
new file mode 100644
index 000000000000..4dd3eb8a40b3
--- /dev/null
+++ b/drivers/irqchip/irq-mbigen.c
@@ -0,0 +1,297 @@
+/*
+ * Copyright (C) 2015 Hisilicon Limited, All Rights Reserved.
+ * Author: Jun Ma <majun258@huawei.com>
+ * Author: Yun Wu <wuyun.wu@huawei.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irqchip.h>
+#include <linux/module.h>
+#include <linux/msi.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+/* Interrupt numbers per mbigen node supported */
+#define IRQS_PER_MBIGEN_NODE		128
+
+/* 64 irqs (Pin0-pin63) are reserved for each mbigen chip */
+#define RESERVED_IRQ_PER_MBIGEN_CHIP	64
+
+/* The maximum IRQ pin number of mbigen chip(start from 0) */
+#define MAXIMUM_IRQ_PIN_NUM		1407
+
+/**
+ * In mbigen vector register
+ * bit[21:12]:	event id value
+ * bit[11:0]:	device id
+ */
+#define IRQ_EVENT_ID_SHIFT		12
+#define IRQ_EVENT_ID_MASK		0x3ff
+
+/* register range of each mbigen node */
+#define MBIGEN_NODE_OFFSET		0x1000
+
+/* offset of vector register in mbigen node */
+#define REG_MBIGEN_VEC_OFFSET		0x200
+
+/**
+ * offset of clear register in mbigen node
+ * This register is used to clear the status
+ * of interrupt
+ */
+#define REG_MBIGEN_CLEAR_OFFSET		0xa000
+
+/**
+ * offset of interrupt type register
+ * This register is used to configure interrupt
+ * trigger type
+ */
+#define REG_MBIGEN_TYPE_OFFSET		0x0
+
+/**
+ * struct mbigen_device - holds the information of mbigen device.
+ *
+ * @pdev:		pointer to the platform device structure of mbigen chip.
+ * @base:		mapped address of this mbigen chip.
+ */
+struct mbigen_device {
+	struct platform_device	*pdev;
+	void __iomem		*base;
+};
+
+static inline unsigned int get_mbigen_vec_reg(irq_hw_number_t hwirq)
+{
+	unsigned int nid, pin;
+
+	hwirq -= RESERVED_IRQ_PER_MBIGEN_CHIP;
+	nid = hwirq / IRQS_PER_MBIGEN_NODE + 1;
+	pin = hwirq % IRQS_PER_MBIGEN_NODE;
+
+	return pin * 4 + nid * MBIGEN_NODE_OFFSET
+			+ REG_MBIGEN_VEC_OFFSET;
+}
+
+static inline void get_mbigen_type_reg(irq_hw_number_t hwirq,
+					u32 *mask, u32 *addr)
+{
+	unsigned int nid, irq_ofst, ofst;
+
+	hwirq -= RESERVED_IRQ_PER_MBIGEN_CHIP;
+	nid = hwirq / IRQS_PER_MBIGEN_NODE + 1;
+	irq_ofst = hwirq % IRQS_PER_MBIGEN_NODE;
+
+	*mask = 1 << (irq_ofst % 32);
+	ofst = irq_ofst / 32 * 4;
+
+	*addr = ofst + nid * MBIGEN_NODE_OFFSET
+		+ REG_MBIGEN_TYPE_OFFSET;
+}
+
+static inline void get_mbigen_clear_reg(irq_hw_number_t hwirq,
+					u32 *mask, u32 *addr)
+{
+	unsigned int ofst;
+
+	hwirq -= RESERVED_IRQ_PER_MBIGEN_CHIP;
+	ofst = hwirq / 32 * 4;
+
+	*mask = 1 << (hwirq % 32);
+	*addr = ofst + REG_MBIGEN_CLEAR_OFFSET;
+}
+
+static void mbigen_eoi_irq(struct irq_data *data)
+{
+	void __iomem *base = data->chip_data;
+	u32 mask, addr;
+
+	get_mbigen_clear_reg(data->hwirq, &mask, &addr);
+
+	writel_relaxed(mask, base + addr);
+
+	irq_chip_eoi_parent(data);
+}
+
+static int mbigen_set_type(struct irq_data *data, unsigned int type)
+{
+	void __iomem *base = data->chip_data;
+	u32 mask, addr, val;
+
+	if (type != IRQ_TYPE_LEVEL_HIGH && type != IRQ_TYPE_EDGE_RISING)
+		return -EINVAL;
+
+	get_mbigen_type_reg(data->hwirq, &mask, &addr);
+
+	val = readl_relaxed(base + addr);
+
+	if (type == IRQ_TYPE_LEVEL_HIGH)
+		val |= mask;
+	else
+		val &= ~mask;
+
+	writel_relaxed(val, base + addr);
+
+	return 0;
+}
+
+static struct irq_chip mbigen_irq_chip = {
+	.name =			"mbigen-v2",
+	.irq_mask =		irq_chip_mask_parent,
+	.irq_unmask =		irq_chip_unmask_parent,
+	.irq_eoi =		mbigen_eoi_irq,
+	.irq_set_type =		mbigen_set_type,
+	.irq_set_affinity =	irq_chip_set_affinity_parent,
+};
+
+static void mbigen_write_msg(struct msi_desc *desc, struct msi_msg *msg)
+{
+	struct irq_data *d = irq_get_irq_data(desc->irq);
+	void __iomem *base = d->chip_data;
+	u32 val;
+
+	base += get_mbigen_vec_reg(d->hwirq);
+	val = readl_relaxed(base);
+
+	val &= ~(IRQ_EVENT_ID_MASK << IRQ_EVENT_ID_SHIFT);
+	val |= (msg->data << IRQ_EVENT_ID_SHIFT);
+
+	/* The address of doorbell is encoded in mbigen register by default
+	 * So,we don't need to program the doorbell address at here
+	 */
+	writel_relaxed(val, base);
+}
+
+static int mbigen_domain_translate(struct irq_domain *d,
+				    struct irq_fwspec *fwspec,
+				    unsigned long *hwirq,
+				    unsigned int *type)
+{
+	if (is_of_node(fwspec->fwnode)) {
+		if (fwspec->param_count != 2)
+			return -EINVAL;
+
+		if ((fwspec->param[0] > MAXIMUM_IRQ_PIN_NUM) ||
+			(fwspec->param[0] < RESERVED_IRQ_PER_MBIGEN_CHIP))
+			return -EINVAL;
+		else
+			*hwirq = fwspec->param[0];
+
+		/* If there is no valid irq type, just use the default type */
+		if ((fwspec->param[1] == IRQ_TYPE_EDGE_RISING) ||
+			(fwspec->param[1] == IRQ_TYPE_LEVEL_HIGH))
+			*type = fwspec->param[1];
+		else
+			return -EINVAL;
+
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int mbigen_irq_domain_alloc(struct irq_domain *domain,
+					unsigned int virq,
+					unsigned int nr_irqs,
+					void *args)
+{
+	struct irq_fwspec *fwspec = args;
+	irq_hw_number_t hwirq;
+	unsigned int type;
+	struct mbigen_device *mgn_chip;
+	int i, err;
+
+	err = mbigen_domain_translate(domain, fwspec, &hwirq, &type);
+	if (err)
+		return err;
+
+	err = platform_msi_domain_alloc(domain, virq, nr_irqs);
+	if (err)
+		return err;
+
+	mgn_chip = platform_msi_get_host_data(domain);
+
+	for (i = 0; i < nr_irqs; i++)
+		irq_domain_set_hwirq_and_chip(domain, virq + i, hwirq + i,
+				      &mbigen_irq_chip, mgn_chip->base);
+
+	return 0;
+}
+
+static struct irq_domain_ops mbigen_domain_ops = {
+	.translate	= mbigen_domain_translate,
+	.alloc		= mbigen_irq_domain_alloc,
+	.free		= irq_domain_free_irqs_common,
+};
+
+static int mbigen_device_probe(struct platform_device *pdev)
+{
+	struct mbigen_device *mgn_chip;
+	struct resource *res;
+	struct irq_domain *domain;
+	u32 num_pins;
+
+	mgn_chip = devm_kzalloc(&pdev->dev, sizeof(*mgn_chip), GFP_KERNEL);
+	if (!mgn_chip)
+		return -ENOMEM;
+
+	mgn_chip->pdev = pdev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mgn_chip->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mgn_chip->base))
+		return PTR_ERR(mgn_chip->base);
+
+	if (of_property_read_u32(pdev->dev.of_node, "num-pins", &num_pins) < 0) {
+		dev_err(&pdev->dev, "No num-pins property\n");
+		return -EINVAL;
+	}
+
+	domain = platform_msi_create_device_domain(&pdev->dev, num_pins,
+							mbigen_write_msg,
+							&mbigen_domain_ops,
+							mgn_chip);
+
+	if (!domain)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, mgn_chip);
+
+	dev_info(&pdev->dev, "Allocated %d MSIs\n", num_pins);
+
+	return 0;
+}
+
+static const struct of_device_id mbigen_of_match[] = {
+	{ .compatible = "hisilicon,mbigen-v2" },
+	{ /* END */ }
+};
+MODULE_DEVICE_TABLE(of, mbigen_of_match);
+
+static struct platform_driver mbigen_platform_driver = {
+	.driver = {
+		.name		= "Hisilicon MBIGEN-V2",
+		.owner		= THIS_MODULE,
+		.of_match_table	= mbigen_of_match,
+	},
+	.probe			= mbigen_device_probe,
+};
+
+module_platform_driver(mbigen_platform_driver);
+
+MODULE_AUTHOR("Jun Ma <majun258@huawei.com>");
+MODULE_AUTHOR("Yun Wu <wuyun.wu@huawei.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Hisilicon MBI Generator driver");
diff --git a/drivers/irqchip/irq-mxs.c b/drivers/irqchip/irq-mxs.c
index c22e2d40cb30..efe50845939d 100644
--- a/drivers/irqchip/irq-mxs.c
+++ b/drivers/irqchip/irq-mxs.c
@@ -241,6 +241,7 @@ static int __init asm9260_of_init(struct device_node *np,
 		writel(0, icoll_priv.intr + i);
 
 	icoll_add_domain(np, ASM9260_NUM_IRQS);
+	set_handle_irq(icoll_handle_irq);
 
 	return 0;
 }
diff --git a/drivers/irqchip/irq-omap-intc.c b/drivers/irqchip/irq-omap-intc.c
index 8587d0f8d8c0..9d1bcfc33e4c 100644
--- a/drivers/irqchip/irq-omap-intc.c
+++ b/drivers/irqchip/irq-omap-intc.c
@@ -47,6 +47,7 @@
 #define INTC_ILR0		0x0100
 
 #define ACTIVEIRQ_MASK		0x7f	/* omap2/3 active interrupt bits */
+#define SPURIOUSIRQ_MASK	(0x1ffffff << 7)
 #define INTCPS_NR_ILR_REGS	128
 #define INTCPS_NR_MIR_REGS	4
 
@@ -207,7 +208,6 @@ static int __init omap_alloc_gc_of(struct irq_domain *d, void __iomem *base)
 		ct = gc->chip_types;
 
 		ct->type = IRQ_TYPE_LEVEL_MASK;
-		ct->handler = handle_level_irq;
 
 		ct->chip.irq_ack = omap_mask_ack_irq;
 		ct->chip.irq_mask = irq_gc_mask_disable_reg;
@@ -330,11 +330,35 @@ static int __init omap_init_irq(u32 base, struct device_node *node)
 static asmlinkage void __exception_irq_entry
 omap_intc_handle_irq(struct pt_regs *regs)
 {
+	extern unsigned long irq_err_count;
 	u32 irqnr;
 
 	irqnr = intc_readl(INTC_SIR);
+
+	/*
+	 * A spurious IRQ can result if interrupt that triggered the
+	 * sorting is no longer active during the sorting (10 INTC
+	 * functional clock cycles after interrupt assertion). Or a
+	 * change in interrupt mask affected the result during sorting
+	 * time. There is no special handling required except ignoring
+	 * the SIR register value just read and retrying.
+	 * See section 6.2.5 of AM335x TRM Literature Number: SPRUH73K
+	 *
+	 * Many a times, a spurious interrupt situation has been fixed
+	 * by adding a flush for the posted write acking the IRQ in
+	 * the device driver. Typically, this is going be the device
+	 * driver whose interrupt was handled just before the spurious
+	 * IRQ occurred. Pay attention to those device drivers if you
+	 * run into hitting the spurious IRQ condition below.
+	 */
+	if (unlikely((irqnr & SPURIOUSIRQ_MASK) == SPURIOUSIRQ_MASK)) {
+		pr_err_once("%s: spurious irq!\n", __func__);
+		irq_err_count++;
+		omap_ack_irq(NULL);
+		return;
+	}
+
 	irqnr &= ACTIVEIRQ_MASK;
-	WARN_ONCE(!irqnr, "Spurious IRQ ?\n");
 	handle_domain_irq(domain, irqnr, regs);
 }
 
diff --git a/drivers/irqchip/irq-pic32-evic.c b/drivers/irqchip/irq-pic32-evic.c
new file mode 100644
index 000000000000..e7155db01d55
--- /dev/null
+++ b/drivers/irqchip/irq-pic32-evic.c
@@ -0,0 +1,324 @@
+/*
+ * Cristian Birsan <cristian.birsan@microchip.com>
+ * Joshua Henderson <joshua.henderson@microchip.com>
+ * Copyright (C) 2016 Microchip Technology Inc.  All rights reserved.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/irqchip.h>
+#include <linux/irq.h>
+
+#include <asm/irq.h>
+#include <asm/traps.h>
+#include <asm/mach-pic32/pic32.h>
+
+#define REG_INTCON	0x0000
+#define REG_INTSTAT	0x0020
+#define REG_IFS_OFFSET	0x0040
+#define REG_IEC_OFFSET	0x00C0
+#define REG_IPC_OFFSET	0x0140
+#define REG_OFF_OFFSET	0x0540
+
+#define MAJPRI_MASK	0x07
+#define SUBPRI_MASK	0x03
+#define PRIORITY_MASK	0x1F
+
+#define PIC32_INT_PRI(pri, subpri)				\
+	((((pri) & MAJPRI_MASK) << 2) | ((subpri) & SUBPRI_MASK))
+
+struct evic_chip_data {
+	u32 irq_types[NR_IRQS];
+	u32 ext_irqs[8];
+};
+
+static struct irq_domain *evic_irq_domain;
+static void __iomem *evic_base;
+
+asmlinkage void __weak plat_irq_dispatch(void)
+{
+	unsigned int irq, hwirq;
+
+	hwirq = readl(evic_base + REG_INTSTAT) & 0xFF;
+	irq = irq_linear_revmap(evic_irq_domain, hwirq);
+	do_IRQ(irq);
+}
+
+static struct evic_chip_data *irqd_to_priv(struct irq_data *data)
+{
+	return (struct evic_chip_data *)data->domain->host_data;
+}
+
+static int pic32_set_ext_polarity(int bit, u32 type)
+{
+	/*
+	 * External interrupts can be either edge rising or edge falling,
+	 * but not both.
+	 */
+	switch (type) {
+	case IRQ_TYPE_EDGE_RISING:
+		writel(BIT(bit), evic_base + PIC32_SET(REG_INTCON));
+		break;
+	case IRQ_TYPE_EDGE_FALLING:
+		writel(BIT(bit), evic_base + PIC32_CLR(REG_INTCON));
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int pic32_set_type_edge(struct irq_data *data,
+			       unsigned int flow_type)
+{
+	struct evic_chip_data *priv = irqd_to_priv(data);
+	int ret;
+	int i;
+
+	if (!(flow_type & IRQ_TYPE_EDGE_BOTH))
+		return -EBADR;
+
+	/* set polarity for external interrupts only */
+	for (i = 0; i < ARRAY_SIZE(priv->ext_irqs); i++) {
+		if (priv->ext_irqs[i] == data->hwirq) {
+			ret = pic32_set_ext_polarity(i + 1, flow_type);
+			if (ret)
+				return ret;
+		}
+	}
+
+	irqd_set_trigger_type(data, flow_type);
+
+	return IRQ_SET_MASK_OK;
+}
+
+static void pic32_bind_evic_interrupt(int irq, int set)
+{
+	writel(set, evic_base + REG_OFF_OFFSET + irq * 4);
+}
+
+static void pic32_set_irq_priority(int irq, int priority)
+{
+	u32 reg, shift;
+
+	reg = irq / 4;
+	shift = (irq % 4) * 8;
+
+	writel(PRIORITY_MASK << shift,
+		evic_base + PIC32_CLR(REG_IPC_OFFSET + reg * 0x10));
+	writel(priority << shift,
+		evic_base + PIC32_SET(REG_IPC_OFFSET + reg * 0x10));
+}
+
+#define IRQ_REG_MASK(_hwirq, _reg, _mask)		       \
+	do {						       \
+		_reg = _hwirq / 32;			       \
+		_mask = 1 << (_hwirq % 32);		       \
+	} while (0)
+
+static int pic32_irq_domain_map(struct irq_domain *d, unsigned int virq,
+				irq_hw_number_t hw)
+{
+	struct evic_chip_data *priv = d->host_data;
+	struct irq_data *data;
+	int ret;
+	u32 iecclr, ifsclr;
+	u32 reg, mask;
+
+	ret = irq_map_generic_chip(d, virq, hw);
+	if (ret)
+		return ret;
+
+	/*
+	 * Piggyback on xlate function to move to an alternate chip as necessary
+	 * at time of mapping instead of allowing the flow handler/chip to be
+	 * changed later. This requires all interrupts to be configured through
+	 * DT.
+	 */
+	if (priv->irq_types[hw] & IRQ_TYPE_SENSE_MASK) {
+		data = irq_domain_get_irq_data(d, virq);
+		irqd_set_trigger_type(data, priv->irq_types[hw]);
+		irq_setup_alt_chip(data, priv->irq_types[hw]);
+	}
+
+	IRQ_REG_MASK(hw, reg, mask);
+
+	iecclr = PIC32_CLR(REG_IEC_OFFSET + reg * 0x10);
+	ifsclr = PIC32_CLR(REG_IFS_OFFSET + reg * 0x10);
+
+	/* mask and clear flag */
+	writel(mask, evic_base + iecclr);
+	writel(mask, evic_base + ifsclr);
+
+	/* default priority is required */
+	pic32_set_irq_priority(hw, PIC32_INT_PRI(2, 0));
+
+	return ret;
+}
+
+int pic32_irq_domain_xlate(struct irq_domain *d, struct device_node *ctrlr,
+			   const u32 *intspec, unsigned int intsize,
+			   irq_hw_number_t *out_hwirq, unsigned int *out_type)
+{
+	struct evic_chip_data *priv = d->host_data;
+
+	if (WARN_ON(intsize < 2))
+		return -EINVAL;
+
+	if (WARN_ON(intspec[0] >= NR_IRQS))
+		return -EINVAL;
+
+	*out_hwirq = intspec[0];
+	*out_type = intspec[1] & IRQ_TYPE_SENSE_MASK;
+
+	priv->irq_types[intspec[0]] = intspec[1] & IRQ_TYPE_SENSE_MASK;
+
+	return 0;
+}
+
+static const struct irq_domain_ops pic32_irq_domain_ops = {
+	.map	= pic32_irq_domain_map,
+	.xlate	= pic32_irq_domain_xlate,
+};
+
+static void __init pic32_ext_irq_of_init(struct irq_domain *domain)
+{
+	struct device_node *node = irq_domain_get_of_node(domain);
+	struct evic_chip_data *priv = domain->host_data;
+	struct property *prop;
+	const __le32 *p;
+	u32 hwirq;
+	int i = 0;
+	const char *pname = "microchip,external-irqs";
+
+	of_property_for_each_u32(node, pname, prop, p, hwirq) {
+		if (i >= ARRAY_SIZE(priv->ext_irqs)) {
+			pr_warn("More than %d external irq, skip rest\n",
+				ARRAY_SIZE(priv->ext_irqs));
+			break;
+		}
+
+		priv->ext_irqs[i] = hwirq;
+		i++;
+	}
+}
+
+static int __init pic32_of_init(struct device_node *node,
+				struct device_node *parent)
+{
+	struct irq_chip_generic *gc;
+	struct evic_chip_data *priv;
+	unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN;
+	int nchips, ret;
+	int i;
+
+	nchips = DIV_ROUND_UP(NR_IRQS, 32);
+
+	evic_base = of_iomap(node, 0);
+	if (!evic_base)
+		return -ENOMEM;
+
+	priv = kcalloc(nchips, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		ret = -ENOMEM;
+		goto err_iounmap;
+	}
+
+	evic_irq_domain = irq_domain_add_linear(node, nchips * 32,
+						&pic32_irq_domain_ops,
+						priv);
+	if (!evic_irq_domain) {
+		ret = -ENOMEM;
+		goto err_free_priv;
+	}
+
+	/*
+	 * The PIC32 EVIC has a linear list of irqs and the type of each
+	 * irq is determined by the hardware peripheral the EVIC is arbitrating.
+	 * These irq types are defined in the datasheet as "persistent" and
+	 * "non-persistent" which are mapped here to level and edge
+	 * respectively. To manage the different flow handler requirements of
+	 * each irq type, different chip_types are used.
+	 */
+	ret = irq_alloc_domain_generic_chips(evic_irq_domain, 32, 2,
+					     "evic-level", handle_level_irq,
+					     clr, 0, 0);
+	if (ret)
+		goto err_domain_remove;
+
+	board_bind_eic_interrupt = &pic32_bind_evic_interrupt;
+
+	for (i = 0; i < nchips; i++) {
+		u32 ifsclr = PIC32_CLR(REG_IFS_OFFSET + (i * 0x10));
+		u32 iec = REG_IEC_OFFSET + (i * 0x10);
+
+		gc = irq_get_domain_generic_chip(evic_irq_domain, i * 32);
+
+		gc->reg_base = evic_base;
+		gc->unused = 0;
+
+		/*
+		 * Level/persistent interrupts have a special requirement that
+		 * the condition generating the interrupt be cleared before the
+		 * interrupt flag (ifs) can be cleared. chip.irq_eoi is used to
+		 * complete the interrupt with an ack.
+		 */
+		gc->chip_types[0].type			= IRQ_TYPE_LEVEL_MASK;
+		gc->chip_types[0].handler		= handle_fasteoi_irq;
+		gc->chip_types[0].regs.ack		= ifsclr;
+		gc->chip_types[0].regs.mask		= iec;
+		gc->chip_types[0].chip.name		= "evic-level";
+		gc->chip_types[0].chip.irq_eoi		= irq_gc_ack_set_bit;
+		gc->chip_types[0].chip.irq_mask		= irq_gc_mask_clr_bit;
+		gc->chip_types[0].chip.irq_unmask	= irq_gc_mask_set_bit;
+		gc->chip_types[0].chip.flags		= IRQCHIP_SKIP_SET_WAKE;
+
+		/* Edge interrupts */
+		gc->chip_types[1].type			= IRQ_TYPE_EDGE_BOTH;
+		gc->chip_types[1].handler		= handle_edge_irq;
+		gc->chip_types[1].regs.ack		= ifsclr;
+		gc->chip_types[1].regs.mask		= iec;
+		gc->chip_types[1].chip.name		= "evic-edge";
+		gc->chip_types[1].chip.irq_ack		= irq_gc_ack_set_bit;
+		gc->chip_types[1].chip.irq_mask		= irq_gc_mask_clr_bit;
+		gc->chip_types[1].chip.irq_unmask	= irq_gc_mask_set_bit;
+		gc->chip_types[1].chip.irq_set_type	= pic32_set_type_edge;
+		gc->chip_types[1].chip.flags		= IRQCHIP_SKIP_SET_WAKE;
+
+		gc->private = &priv[i];
+	}
+
+	irq_set_default_host(evic_irq_domain);
+
+	/*
+	 * External interrupts have software configurable edge polarity. These
+	 * interrupts are defined in DT allowing polarity to be configured only
+	 * for these interrupts when requested.
+	 */
+	pic32_ext_irq_of_init(evic_irq_domain);
+
+	return 0;
+
+err_domain_remove:
+	irq_domain_remove(evic_irq_domain);
+
+err_free_priv:
+	kfree(priv);
+
+err_iounmap:
+	iounmap(evic_base);
+
+	return ret;
+}
+
+IRQCHIP_DECLARE(pic32_evic, "microchip,pic32mzda-evic", pic32_of_init);
diff --git a/drivers/irqchip/irq-renesas-h8300h.c b/drivers/irqchip/irq-renesas-h8300h.c
index 6fd30d5ee14d..c378768d75b3 100644
--- a/drivers/irqchip/irq-renesas-h8300h.c
+++ b/drivers/irqchip/irq-renesas-h8300h.c
@@ -21,9 +21,9 @@ static const char ipr_bit[] = {
 	10, 10, 10, 10,  9,  9,  9,  9,
 };
 
-static void *intc_baseaddr;
+static void __iomem *intc_baseaddr;
 
-#define IPR ((unsigned long)intc_baseaddr + 6)
+#define IPR (intc_baseaddr + 6)
 
 static void h8300h_disable_irq(struct irq_data *data)
 {
@@ -81,8 +81,8 @@ static int __init h8300h_intc_of_init(struct device_node *intc,
 	BUG_ON(!intc_baseaddr);
 
 	/* All interrupt priority low */
-	ctrl_outb(0x00, IPR + 0);
-	ctrl_outb(0x00, IPR + 1);
+	writeb(0x00, IPR + 0);
+	writeb(0x00, IPR + 1);
 
 	domain = irq_domain_add_linear(intc, NR_IRQS, &irq_ops, NULL);
 	BUG_ON(!domain);
diff --git a/drivers/irqchip/irq-renesas-h8s.c b/drivers/irqchip/irq-renesas-h8s.c
index 8098ead1eb22..af8c6c61c824 100644
--- a/drivers/irqchip/irq-renesas-h8s.c
+++ b/drivers/irqchip/irq-renesas-h8s.c
@@ -40,8 +40,8 @@ static void h8s_disable_irq(struct irq_data *data)
 	addr = IPRA + ((ipr_table[irq - 16] & 0xf0) >> 3);
 	pos = (ipr_table[irq - 16] & 0x0f) * 4;
 	pri = ~(0x000f << pos);
-	pri &= ctrl_inw(addr);
-	ctrl_outw(pri, addr);
+	pri &= readw(addr);
+	writew(pri, addr);
 }
 
 static void h8s_enable_irq(struct irq_data *data)
@@ -54,9 +54,9 @@ static void h8s_enable_irq(struct irq_data *data)
 	addr = IPRA + ((ipr_table[irq - 16] & 0xf0) >> 3);
 	pos = (ipr_table[irq - 16] & 0x0f) * 4;
 	pri = ~(0x000f << pos);
-	pri &= ctrl_inw(addr);
+	pri &= readw(addr);
 	pri |= 1 << pos;
-	ctrl_outw(pri, addr);
+	writew(pri, addr);
 }
 
 struct irq_chip h8s_irq_chip = {
@@ -90,7 +90,7 @@ static int __init h8s_intc_of_init(struct device_node *intc,
 	/* All interrupt priority is 0 (disable) */
 	/* IPRA to IPRK */
 	for (n = 0; n <= 'k' - 'a'; n++)
-		ctrl_outw(0x0000, IPRA + (n * 2));
+		writew(0x0000, IPRA + (n * 2));
 
 	domain = irq_domain_add_linear(intc, NR_IRQS, &irq_ops, NULL);
 	BUG_ON(!domain);
diff --git a/drivers/irqchip/irq-renesas-intc-irqpin.c b/drivers/irqchip/irq-renesas-intc-irqpin.c
index c325806561be..713177d97c7a 100644
--- a/drivers/irqchip/irq-renesas-intc-irqpin.c
+++ b/drivers/irqchip/irq-renesas-intc-irqpin.c
@@ -31,7 +31,6 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
-#include <linux/platform_data/irq-renesas-intc-irqpin.h>
 #include <linux/pm_runtime.h>
 
 #define INTC_IRQPIN_MAX 8 /* maximum 8 interrupts per driver instance */
@@ -75,18 +74,20 @@ struct intc_irqpin_irq {
 struct intc_irqpin_priv {
 	struct intc_irqpin_iomem iomem[INTC_IRQPIN_REG_NR];
 	struct intc_irqpin_irq irq[INTC_IRQPIN_MAX];
-	struct renesas_intc_irqpin_config config;
-	unsigned int number_of_irqs;
+	unsigned int sense_bitfield_width;
 	struct platform_device *pdev;
 	struct irq_chip irq_chip;
 	struct irq_domain *irq_domain;
 	struct clk *clk;
-	bool shared_irqs;
+	unsigned shared_irqs:1;
+	unsigned needs_clk:1;
 	u8 shared_irq_mask;
 };
 
-struct intc_irqpin_irlm_config {
+struct intc_irqpin_config {
 	unsigned int irlm_bit;
+	unsigned needs_irlm:1;
+	unsigned needs_clk:1;
 };
 
 static unsigned long intc_irqpin_read32(void __iomem *iomem)
@@ -171,7 +172,7 @@ static void intc_irqpin_mask_unmask_prio(struct intc_irqpin_priv *p,
 static int intc_irqpin_set_sense(struct intc_irqpin_priv *p, int irq, int value)
 {
 	/* The SENSE register is assumed to be 32-bit. */
-	int bitfield_width = p->config.sense_bitfield_width;
+	int bitfield_width = p->sense_bitfield_width;
 	int shift = 32 - (irq + 1) * bitfield_width;
 
 	dev_dbg(&p->pdev->dev, "sense irq = %d, mode = %d\n", irq, value);
@@ -361,8 +362,15 @@ static const struct irq_domain_ops intc_irqpin_irq_domain_ops = {
 	.xlate  = irq_domain_xlate_twocell,
 };
 
-static const struct intc_irqpin_irlm_config intc_irqpin_irlm_r8a777x = {
+static const struct intc_irqpin_config intc_irqpin_irlm_r8a777x = {
 	.irlm_bit = 23, /* ICR0.IRLM0 */
+	.needs_irlm = 1,
+	.needs_clk = 0,
+};
+
+static const struct intc_irqpin_config intc_irqpin_rmobile = {
+	.needs_irlm = 0,
+	.needs_clk = 1,
 };
 
 static const struct of_device_id intc_irqpin_dt_ids[] = {
@@ -371,14 +379,18 @@ static const struct of_device_id intc_irqpin_dt_ids[] = {
 	  .data = &intc_irqpin_irlm_r8a777x },
 	{ .compatible = "renesas,intc-irqpin-r8a7779",
 	  .data = &intc_irqpin_irlm_r8a777x },
+	{ .compatible = "renesas,intc-irqpin-r8a7740",
+	  .data = &intc_irqpin_rmobile },
+	{ .compatible = "renesas,intc-irqpin-sh73a0",
+	  .data = &intc_irqpin_rmobile },
 	{},
 };
 MODULE_DEVICE_TABLE(of, intc_irqpin_dt_ids);
 
 static int intc_irqpin_probe(struct platform_device *pdev)
 {
+	const struct intc_irqpin_config *config = NULL;
 	struct device *dev = &pdev->dev;
-	struct renesas_intc_irqpin_config *pdata = dev->platform_data;
 	const struct of_device_id *of_id;
 	struct intc_irqpin_priv *p;
 	struct intc_irqpin_iomem *i;
@@ -388,6 +400,8 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	void (*enable_fn)(struct irq_data *d);
 	void (*disable_fn)(struct irq_data *d);
 	const char *name = dev_name(dev);
+	bool control_parent;
+	unsigned int nirqs;
 	int ref_irq;
 	int ret;
 	int k;
@@ -399,23 +413,28 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	}
 
 	/* deal with driver instance configuration */
-	if (pdata) {
-		memcpy(&p->config, pdata, sizeof(*pdata));
-	} else {
-		of_property_read_u32(dev->of_node, "sense-bitfield-width",
-				     &p->config.sense_bitfield_width);
-		p->config.control_parent = of_property_read_bool(dev->of_node,
-								 "control-parent");
-	}
-	if (!p->config.sense_bitfield_width)
-		p->config.sense_bitfield_width = 4; /* default to 4 bits */
+	of_property_read_u32(dev->of_node, "sense-bitfield-width",
+			     &p->sense_bitfield_width);
+	control_parent = of_property_read_bool(dev->of_node, "control-parent");
+	if (!p->sense_bitfield_width)
+		p->sense_bitfield_width = 4; /* default to 4 bits */
 
 	p->pdev = pdev;
 	platform_set_drvdata(pdev, p);
 
+	of_id = of_match_device(intc_irqpin_dt_ids, dev);
+	if (of_id && of_id->data) {
+		config = of_id->data;
+		p->needs_clk = config->needs_clk;
+	}
+
 	p->clk = devm_clk_get(dev, NULL);
 	if (IS_ERR(p->clk)) {
-		dev_warn(dev, "unable to get clock\n");
+		if (p->needs_clk) {
+			dev_err(dev, "unable to get clock\n");
+			ret = PTR_ERR(p->clk);
+			goto err0;
+		}
 		p->clk = NULL;
 	}
 
@@ -443,8 +462,8 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 		p->irq[k].requested_irq = irq->start;
 	}
 
-	p->number_of_irqs = k;
-	if (p->number_of_irqs < 1) {
+	nirqs = k;
+	if (nirqs < 1) {
 		dev_err(dev, "not enough IRQ resources\n");
 		ret = -EINVAL;
 		goto err0;
@@ -485,20 +504,16 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	}
 
 	/* configure "individual IRQ mode" where needed */
-	of_id = of_match_device(intc_irqpin_dt_ids, dev);
-	if (of_id && of_id->data) {
-		const struct intc_irqpin_irlm_config *irlm_config = of_id->data;
-
+	if (config && config->needs_irlm) {
 		if (io[INTC_IRQPIN_REG_IRLM])
 			intc_irqpin_read_modify_write(p, INTC_IRQPIN_REG_IRLM,
-						      irlm_config->irlm_bit,
-						      1, 1);
+						      config->irlm_bit, 1, 1);
 		else
 			dev_warn(dev, "unable to select IRLM mode\n");
 	}
 
 	/* mask all interrupts using priority */
-	for (k = 0; k < p->number_of_irqs; k++)
+	for (k = 0; k < nirqs; k++)
 		intc_irqpin_mask_unmask_prio(p, k, 1);
 
 	/* clear all pending interrupts */
@@ -506,16 +521,16 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 
 	/* scan for shared interrupt lines */
 	ref_irq = p->irq[0].requested_irq;
-	p->shared_irqs = true;
-	for (k = 1; k < p->number_of_irqs; k++) {
+	p->shared_irqs = 1;
+	for (k = 1; k < nirqs; k++) {
 		if (ref_irq != p->irq[k].requested_irq) {
-			p->shared_irqs = false;
+			p->shared_irqs = 0;
 			break;
 		}
 	}
 
 	/* use more severe masking method if requested */
-	if (p->config.control_parent) {
+	if (control_parent) {
 		enable_fn = intc_irqpin_irq_enable_force;
 		disable_fn = intc_irqpin_irq_disable_force;
 	} else if (!p->shared_irqs) {
@@ -534,9 +549,7 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	irq_chip->irq_set_wake = intc_irqpin_irq_set_wake;
 	irq_chip->flags	= IRQCHIP_MASK_ON_SUSPEND;
 
-	p->irq_domain = irq_domain_add_simple(dev->of_node,
-					      p->number_of_irqs,
-					      p->config.irq_base,
+	p->irq_domain = irq_domain_add_simple(dev->of_node, nirqs, 0,
 					      &intc_irqpin_irq_domain_ops, p);
 	if (!p->irq_domain) {
 		ret = -ENXIO;
@@ -555,7 +568,7 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 		}
 	} else {
 		/* request interrupts one by one */
-		for (k = 0; k < p->number_of_irqs; k++) {
+		for (k = 0; k < nirqs; k++) {
 			if (devm_request_irq(dev, p->irq[k].requested_irq,
 					     intc_irqpin_irq_handler, 0, name,
 					     &p->irq[k])) {
@@ -567,17 +580,10 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	}
 
 	/* unmask all interrupts on prio level */
-	for (k = 0; k < p->number_of_irqs; k++)
+	for (k = 0; k < nirqs; k++)
 		intc_irqpin_mask_unmask_prio(p, k, 0);
 
-	dev_info(dev, "driving %d irqs\n", p->number_of_irqs);
-
-	/* warn in case of mismatch if irq base is specified */
-	if (p->config.irq_base) {
-		if (p->config.irq_base != p->irq[0].domain_irq)
-			dev_warn(dev, "irq base mismatch (%d/%d)\n",
-				 p->config.irq_base, p->irq[0].domain_irq);
-	}
+	dev_info(dev, "driving %d irqs\n", nirqs);
 
 	return 0;
 
diff --git a/drivers/irqchip/irq-s3c24xx.c b/drivers/irqchip/irq-s3c24xx.c
index c71914e8f596..5dc5a760c723 100644
--- a/drivers/irqchip/irq-s3c24xx.c
+++ b/drivers/irqchip/irq-s3c24xx.c
@@ -605,7 +605,7 @@ err:
 	return ERR_PTR(ret);
 }
 
-static struct s3c_irq_data init_eint[32] = {
+static struct s3c_irq_data __maybe_unused init_eint[32] = {
 	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
 	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
 	{ .type = S3C_IRQTYPE_NONE, }, /* reserved */
diff --git a/drivers/irqchip/irq-sun4i.c b/drivers/irqchip/irq-sun4i.c
index 0704362f4c82..376b28074e0d 100644
--- a/drivers/irqchip/irq-sun4i.c
+++ b/drivers/irqchip/irq-sun4i.c
@@ -22,7 +22,6 @@
 #include <linux/of_irq.h>
 
 #include <asm/exception.h>
-#include <asm/mach/irq.h>
 
 #define SUN4I_IRQ_VECTOR_REG		0x00
 #define SUN4I_IRQ_PROTECTION_REG	0x08
diff --git a/drivers/irqchip/irq-sunxi-nmi.c b/drivers/irqchip/irq-sunxi-nmi.c
index 4ef178078e5b..0820f67cc9a7 100644
--- a/drivers/irqchip/irq-sunxi-nmi.c
+++ b/drivers/irqchip/irq-sunxi-nmi.c
@@ -50,6 +50,12 @@ static struct sunxi_sc_nmi_reg_offs sun6i_reg_offs = {
 	.enable	= 0x34,
 };
 
+static struct sunxi_sc_nmi_reg_offs sun9i_reg_offs = {
+	.ctrl	= 0x00,
+	.pend	= 0x08,
+	.enable	= 0x04,
+};
+
 static inline void sunxi_sc_nmi_write(struct irq_chip_generic *gc, u32 off,
 				      u32 val)
 {
@@ -207,3 +213,10 @@ static int __init sun7i_sc_nmi_irq_init(struct device_node *node,
 	return sunxi_sc_nmi_irq_init(node, &sun7i_reg_offs);
 }
 IRQCHIP_DECLARE(sun7i_sc_nmi, "allwinner,sun7i-a20-sc-nmi", sun7i_sc_nmi_irq_init);
+
+static int __init sun9i_nmi_irq_init(struct device_node *node,
+				     struct device_node *parent)
+{
+	return sunxi_sc_nmi_irq_init(node, &sun9i_reg_offs);
+}
+IRQCHIP_DECLARE(sun9i_nmi, "allwinner,sun9i-a80-nmi", sun9i_nmi_irq_init);
diff --git a/drivers/irqchip/irq-ts4800.c b/drivers/irqchip/irq-ts4800.c
new file mode 100644
index 000000000000..4192bdcd2734
--- /dev/null
+++ b/drivers/irqchip/irq-ts4800.c
@@ -0,0 +1,163 @@
+/*
+ * Multiplexed-IRQs driver for TS-4800's FPGA
+ *
+ * Copyright (c) 2015 - Savoir-faire Linux
+ *
+ * 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.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+
+#define IRQ_MASK        0x4
+#define IRQ_STATUS      0x8
+
+struct ts4800_irq_data {
+	void __iomem            *base;
+	struct irq_domain       *domain;
+	struct irq_chip         irq_chip;
+};
+
+static void ts4800_irq_mask(struct irq_data *d)
+{
+	struct ts4800_irq_data *data = irq_data_get_irq_chip_data(d);
+	u16 reg = readw(data->base + IRQ_MASK);
+	u16 mask = 1 << d->hwirq;
+
+	writew(reg | mask, data->base + IRQ_MASK);
+}
+
+static void ts4800_irq_unmask(struct irq_data *d)
+{
+	struct ts4800_irq_data *data = irq_data_get_irq_chip_data(d);
+	u16 reg = readw(data->base + IRQ_MASK);
+	u16 mask = 1 << d->hwirq;
+
+	writew(reg & ~mask, data->base + IRQ_MASK);
+}
+
+static int ts4800_irqdomain_map(struct irq_domain *d, unsigned int irq,
+				irq_hw_number_t hwirq)
+{
+	struct ts4800_irq_data *data = d->host_data;
+
+	irq_set_chip_and_handler(irq, &data->irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, data);
+	irq_set_noprobe(irq);
+
+	return 0;
+}
+
+struct irq_domain_ops ts4800_ic_ops = {
+	.map = ts4800_irqdomain_map,
+	.xlate = irq_domain_xlate_onecell,
+};
+
+static void ts4800_ic_chained_handle_irq(struct irq_desc *desc)
+{
+	struct ts4800_irq_data *data = irq_desc_get_handler_data(desc);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	u16 status = readw(data->base + IRQ_STATUS);
+
+	chained_irq_enter(chip, desc);
+
+	if (unlikely(status == 0)) {
+		handle_bad_irq(desc);
+		goto out;
+	}
+
+	do {
+		unsigned int bit = __ffs(status);
+		int irq = irq_find_mapping(data->domain, bit);
+
+		status &= ~(1 << bit);
+		generic_handle_irq(irq);
+	} while (status);
+
+out:
+	chained_irq_exit(chip, desc);
+}
+
+static int ts4800_ic_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct ts4800_irq_data *data;
+	struct irq_chip *irq_chip;
+	struct resource *res;
+	int parent_irq;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(data->base))
+		return PTR_ERR(data->base);
+
+	writew(0xFFFF, data->base + IRQ_MASK);
+
+	parent_irq = irq_of_parse_and_map(node, 0);
+	if (!parent_irq) {
+		dev_err(&pdev->dev, "failed to get parent IRQ\n");
+		return -EINVAL;
+	}
+
+	irq_chip = &data->irq_chip;
+	irq_chip->name = dev_name(&pdev->dev);
+	irq_chip->irq_mask = ts4800_irq_mask;
+	irq_chip->irq_unmask = ts4800_irq_unmask;
+
+	data->domain = irq_domain_add_linear(node, 8, &ts4800_ic_ops, data);
+	if (!data->domain) {
+		dev_err(&pdev->dev, "cannot add IRQ domain\n");
+		return -ENOMEM;
+	}
+
+	irq_set_chained_handler_and_data(parent_irq,
+					 ts4800_ic_chained_handle_irq, data);
+
+	platform_set_drvdata(pdev, data);
+
+	return 0;
+}
+
+static int ts4800_ic_remove(struct platform_device *pdev)
+{
+	struct ts4800_irq_data *data = platform_get_drvdata(pdev);
+
+	irq_domain_remove(data->domain);
+
+	return 0;
+}
+
+static const struct of_device_id ts4800_ic_of_match[] = {
+	{ .compatible = "technologic,ts4800-irqc", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, ts4800_ic_of_match);
+
+static struct platform_driver ts4800_ic_driver = {
+	.probe  = ts4800_ic_probe,
+	.remove = ts4800_ic_remove,
+	.driver = {
+		.name = "ts4800-irqc",
+		.of_match_table = ts4800_ic_of_match,
+	},
+};
+module_platform_driver(ts4800_ic_driver);
+
+MODULE_AUTHOR("Damien Riegel <damien.riegel@savoirfairelinux.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:ts4800_irqc");
diff --git a/drivers/irqchip/irq-versatile-fpga.c b/drivers/irqchip/irq-versatile-fpga.c
index cadf104e3074..598ab3f0e0ac 100644
--- a/drivers/irqchip/irq-versatile-fpga.c
+++ b/drivers/irqchip/irq-versatile-fpga.c
@@ -210,12 +210,7 @@ int __init fpga_irq_of_init(struct device_node *node,
 		parent_irq = -1;
 	}
 
-#ifdef CONFIG_ARCH_VERSATILE
-	fpga_irq_init(base, node->name, IRQ_SIC_START, parent_irq, valid_mask,
-				  node);
-#else
 	fpga_irq_init(base, node->name, 0, parent_irq, valid_mask, node);
-#endif
 
 	writel(clear_mask, base + IRQ_ENABLE_CLEAR);
 	writel(clear_mask, base + FIQ_ENABLE_CLEAR);
diff --git a/drivers/irqchip/irq-zevio.c b/drivers/irqchip/irq-zevio.c
index 4c48fa88a03d..cb9d8ec37507 100644
--- a/drivers/irqchip/irq-zevio.c
+++ b/drivers/irqchip/irq-zevio.c
@@ -43,8 +43,7 @@ static void __iomem *zevio_irq_io;
 static void zevio_irq_ack(struct irq_data *irqd)
 {
 	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(irqd);
-	struct irq_chip_regs *regs =
-		&container_of(irqd->chip, struct irq_chip_type, chip)->regs;
+	struct irq_chip_regs *regs = &irq_data_get_chip_type(irqd)->regs;
 
 	readl(gc->reg_base + regs->ack);
 }
diff --git a/drivers/isdn/Makefile b/drivers/isdn/Makefile
index f1f777570e8e..91c81965e7ca 100644
--- a/drivers/isdn/Makefile
+++ b/drivers/isdn/Makefile
@@ -10,7 +10,6 @@ obj-$(CONFIG_ISDN_DIVERSION)		+= divert/
 obj-$(CONFIG_ISDN_DRV_HISAX)		+= hisax/
 obj-$(CONFIG_ISDN_DRV_ICN)		+= icn/
 obj-$(CONFIG_ISDN_DRV_PCBIT)		+= pcbit/
-obj-$(CONFIG_ISDN_DRV_SC)		+= sc/
 obj-$(CONFIG_ISDN_DRV_LOOP)		+= isdnloop/
 obj-$(CONFIG_ISDN_DRV_ACT2000)		+= act2000/
 obj-$(CONFIG_HYSDN)			+= hysdn/
diff --git a/drivers/isdn/act2000/module.c b/drivers/isdn/act2000/module.c
index c3a1b061838d..68073d0da0e3 100644
--- a/drivers/isdn/act2000/module.c
+++ b/drivers/isdn/act2000/module.c
@@ -37,7 +37,7 @@ MODULE_DESCRIPTION("ISDN4Linux: Driver for IBM Active 2000 ISDN card");
 MODULE_AUTHOR("Fritz Elfert");
 MODULE_LICENSE("GPL");
 MODULE_PARM_DESC(act_bus, "BusType of first card, 1=ISA, 2=MCA, 3=PCMCIA, currently only ISA");
-MODULE_PARM_DESC(membase, "Base port address of first card");
+MODULE_PARM_DESC(act_port, "Base port address of first card");
 MODULE_PARM_DESC(act_irq, "IRQ of first card");
 MODULE_PARM_DESC(act_id, "ID-String of first card");
 module_param(act_bus, int, 0);
diff --git a/drivers/isdn/gigaset/ser-gigaset.c b/drivers/isdn/gigaset/ser-gigaset.c
index 2a506fe0c8a4..d1f8ab915b15 100644
--- a/drivers/isdn/gigaset/ser-gigaset.c
+++ b/drivers/isdn/gigaset/ser-gigaset.c
@@ -373,13 +373,7 @@ static void gigaset_freecshw(struct cardstate *cs)
 
 static void gigaset_device_release(struct device *dev)
 {
-	struct cardstate *cs = dev_get_drvdata(dev);
-
-	if (!cs)
-		return;
-	dev_set_drvdata(dev, NULL);
-	kfree(cs->hw.ser);
-	cs->hw.ser = NULL;
+	kfree(container_of(dev, struct ser_cardstate, dev.dev));
 }
 
 /*
@@ -408,7 +402,6 @@ static int gigaset_initcshw(struct cardstate *cs)
 		cs->hw.ser = NULL;
 		return rc;
 	}
-	dev_set_drvdata(&cs->hw.ser->dev.dev, cs);
 
 	tasklet_init(&cs->write_tasklet,
 		     gigaset_modem_fill, (unsigned long) cs);
diff --git a/drivers/isdn/hardware/mISDN/netjet.c b/drivers/isdn/hardware/mISDN/netjet.c
index 8e2944784e00..afde4edef9ae 100644
--- a/drivers/isdn/hardware/mISDN/netjet.c
+++ b/drivers/isdn/hardware/mISDN/netjet.c
@@ -392,7 +392,7 @@ read_dma(struct tiger_ch *bc, u32 idx, int cnt)
 	}
 	stat = bchannel_get_rxbuf(&bc->bch, cnt);
 	/* only transparent use the count here, HDLC overun is detected later */
-	if (stat == ENOMEM) {
+	if (stat == -ENOMEM) {
 		pr_warning("%s.B%d: No memory for %d bytes\n",
 			   card->name, bc->bch.nr, cnt);
 		return;
diff --git a/drivers/isdn/i4l/Kconfig b/drivers/isdn/i4l/Kconfig
index 9c6650ea848e..f5b714cd7618 100644
--- a/drivers/isdn/i4l/Kconfig
+++ b/drivers/isdn/i4l/Kconfig
@@ -130,8 +130,6 @@ source "drivers/isdn/icn/Kconfig"
 
 source "drivers/isdn/pcbit/Kconfig"
 
-source "drivers/isdn/sc/Kconfig"
-
 source "drivers/isdn/act2000/Kconfig"
 
 endmenu
diff --git a/drivers/isdn/sc/Kconfig b/drivers/isdn/sc/Kconfig
deleted file mode 100644
index 7469863a7925..000000000000
--- a/drivers/isdn/sc/Kconfig
+++ /dev/null
@@ -1,8 +0,0 @@
-config ISDN_DRV_SC
-	tristate "Spellcaster support"
-	depends on ISA
-	help
-	  This enables support for the Spellcaster BRI ISDN boards.  This
-	  driver currently builds only in a modularized version.
-	  To build it, choose M here: the module will be called sc.
-	  See <file:Documentation/isdn/README.sc> for more information.
diff --git a/drivers/isdn/sc/Makefile b/drivers/isdn/sc/Makefile
deleted file mode 100644
index 0f2b7d602ac0..000000000000
--- a/drivers/isdn/sc/Makefile
+++ /dev/null
@@ -1,10 +0,0 @@
-# Makefile for the sc ISDN device driver
-
-# Each configuration option enables a list of files.
-
-obj-$(CONFIG_ISDN_DRV_SC)	+= sc.o
-
-# Multipart objects.
-
-sc-y				:= shmem.o init.o packet.o command.o event.o \
-		   		   ioctl.o interrupt.o message.o timer.o	
diff --git a/drivers/isdn/sc/card.h b/drivers/isdn/sc/card.h
deleted file mode 100644
index 3da69ee43da7..000000000000
--- a/drivers/isdn/sc/card.h
+++ /dev/null
@@ -1,131 +0,0 @@
-/* $Id: card.h,v 1.1.10.1 2001/09/23 22:24:59 kai Exp $
- *
- * Driver parameters for SpellCaster ISA ISDN adapters
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#ifndef CARD_H
-#define CARD_H
-
-/*
- * We need these if they're not already included
- */
-#include <linux/timer.h>
-#include <linux/time.h>
-#include <linux/isdnif.h>
-#include <linux/irqreturn.h>
-#include "message.h"
-#include "scioc.h"
-
-/*
- * Amount of time to wait for a reset to complete
- */
-#define CHECKRESET_TIME		msecs_to_jiffies(4000)
-
-/*
- * Amount of time between line status checks
- */
-#define CHECKSTAT_TIME		msecs_to_jiffies(8000)
-
-/*
- * The maximum amount of time to wait for a message response
- * to arrive. Use exclusively by send_and_receive
- */
-#define SAR_TIMEOUT		msecs_to_jiffies(10000)
-
-/*
- * Macro to determine is a card id is valid
- */
-#define IS_VALID_CARD(x)	((x >= 0) && (x <= cinst))
-
-/*
- * Per channel status and configuration
- */
-typedef struct {
-	int l2_proto;
-	int l3_proto;
-	char dn[50];
-	unsigned long first_sendbuf;	/* Offset of first send buffer */
-	unsigned int num_sendbufs;	/* Number of send buffers */
-	unsigned int free_sendbufs;	/* Number of free sendbufs */
-	unsigned int next_sendbuf;	/* Next sequential buffer */
-	char eazlist[50];		/* Set with SETEAZ */
-	char sillist[50];		/* Set with SETSIL */
-	int eazclear;			/* Don't accept calls if TRUE */
-} bchan;
-
-/*
- * Everything you want to know about the adapter ...
- */
-typedef struct {
-	int model;
-	int driverId;			/* LL Id */
-	char devicename[20];		/* The device name */
-	isdn_if *card;			/* ISDN4Linux structure */
-	bchan *channel;			/* status of the B channels */
-	char nChannels;			/* Number of channels */
-	unsigned int interrupt;		/* Interrupt number */
-	int iobase;			/* I/O Base address */
-	int ioport[MAX_IO_REGS];	/* Index to I/O ports */
-	int shmem_pgport;		/* port for the exp mem page reg. */
-	int shmem_magic;		/* adapter magic number */
-	unsigned int rambase;		/* Shared RAM base address */
-	unsigned int ramsize;		/* Size of shared memory */
-	RspMessage async_msg;		/* Async response message */
-	int want_async_messages;	/* Snoop the Q ? */
-	unsigned char seq_no;		/* Next send seq. number */
-	struct timer_list reset_timer;	/* Check reset timer */
-	struct timer_list stat_timer;	/* Check startproc timer */
-	unsigned char nphystat;		/* Latest PhyStat info */
-	unsigned char phystat;		/* Last PhyStat info */
-	HWConfig_pl hwconfig;		/* Hardware config info */
-	char load_ver[11];		/* CommManage Version string */
-	char proc_ver[11];		/* CommEngine Version */
-	int StartOnReset;		/* Indicates startproc after reset */
-	int EngineUp;			/* Indicates CommEngine Up */
-	int trace_mode;			/* Indicate if tracing is on */
-	spinlock_t lock;		/* local lock */
-} board;
-
-
-extern board *sc_adapter[];
-extern int cinst;
-
-void memcpy_toshmem(int card, void *dest, const void *src, size_t n);
-void memcpy_fromshmem(int card, void *dest, const void *src, size_t n);
-int get_card_from_id(int driver);
-int indicate_status(int card, int event, ulong Channel, char *Data);
-irqreturn_t interrupt_handler(int interrupt, void *cardptr);
-int sndpkt(int devId, int channel, int ack, struct sk_buff *data);
-void rcvpkt(int card, RspMessage *rcvmsg);
-int command(isdn_ctrl *cmd);
-int reset(int card);
-int startproc(int card);
-int send_and_receive(int card, unsigned int procid, unsigned char type,
-		     unsigned char class, unsigned char code,
-		     unsigned char link, unsigned char data_len,
-		     unsigned char *data,  RspMessage *mesgdata, int timeout);
-void flushreadfifo(int card);
-int sendmessage(int card, unsigned int procid, unsigned int type,
-		unsigned int class, unsigned int code, unsigned int link,
-		unsigned int data_len, unsigned int *data);
-int receivemessage(int card, RspMessage *rspmsg);
-int sc_ioctl(int card, scs_ioctl *data);
-int setup_buffers(int card, int c);
-void sc_check_reset(unsigned long data);
-void check_phystat(unsigned long data);
-
-#endif /* CARD_H */
diff --git a/drivers/isdn/sc/command.c b/drivers/isdn/sc/command.c
deleted file mode 100644
index 4a4e66152ce7..000000000000
--- a/drivers/isdn/sc/command.c
+++ /dev/null
@@ -1,363 +0,0 @@
-/* $Id: command.c,v 1.4.10.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include <linux/module.h>
-#include "includes.h"		/* This must be first */
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-#include "scioc.h"
-
-static int dial(int card, unsigned long channel, setup_parm setup);
-static int hangup(int card, unsigned long channel);
-static int answer(int card, unsigned long channel);
-static int clreaz(int card, unsigned long channel);
-static int seteaz(int card, unsigned long channel, char *);
-static int setl2(int card, unsigned long arg);
-static int setl3(int card, unsigned long arg);
-static int acceptb(int card, unsigned long channel);
-
-#ifdef DEBUG
-/*
- * Translate command codes to strings
- */
-static char *commands[] = { "ISDN_CMD_IOCTL",
-			    "ISDN_CMD_DIAL",
-			    "ISDN_CMD_ACCEPTB",
-			    "ISDN_CMD_ACCEPTB",
-			    "ISDN_CMD_HANGUP",
-			    "ISDN_CMD_CLREAZ",
-			    "ISDN_CMD_SETEAZ",
-			    NULL,
-			    NULL,
-			    NULL,
-			    "ISDN_CMD_SETL2",
-			    NULL,
-			    "ISDN_CMD_SETL3",
-			    NULL,
-			    NULL,
-			    NULL,
-			    NULL,
-			    NULL, };
-
-/*
- * Translates ISDN4Linux protocol codes to strings for debug messages
- */
-static char *l3protos[] = { "ISDN_PROTO_L3_TRANS" };
-static char *l2protos[] = { "ISDN_PROTO_L2_X75I",
-			    "ISDN_PROTO_L2_X75UI",
-			    "ISDN_PROTO_L2_X75BUI",
-			    "ISDN_PROTO_L2_HDLC",
-			    "ISDN_PROTO_L2_TRANS" };
-#endif
-
-int get_card_from_id(int driver)
-{
-	int i;
-
-	for (i = 0; i < cinst; i++) {
-		if (sc_adapter[i]->driverId == driver)
-			return i;
-	}
-	return -ENODEV;
-}
-
-/*
- * command
- */
-
-int command(isdn_ctrl *cmd)
-{
-	int card;
-
-	card = get_card_from_id(cmd->driver);
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	/*
-	 * Dispatch the command
-	 */
-	switch (cmd->command) {
-	case ISDN_CMD_IOCTL:
-	{
-		unsigned long	cmdptr;
-		scs_ioctl	ioc;
-
-		memcpy(&cmdptr, cmd->parm.num, sizeof(unsigned long));
-		if (copy_from_user(&ioc, (scs_ioctl __user *)cmdptr,
-				   sizeof(scs_ioctl))) {
-			pr_debug("%s: Failed to verify user space 0x%lx\n",
-				 sc_adapter[card]->devicename, cmdptr);
-			return -EFAULT;
-		}
-		return sc_ioctl(card, &ioc);
-	}
-	case ISDN_CMD_DIAL:
-		return dial(card, cmd->arg, cmd->parm.setup);
-	case ISDN_CMD_HANGUP:
-		return hangup(card, cmd->arg);
-	case ISDN_CMD_ACCEPTD:
-		return answer(card, cmd->arg);
-	case ISDN_CMD_ACCEPTB:
-		return acceptb(card, cmd->arg);
-	case ISDN_CMD_CLREAZ:
-		return clreaz(card, cmd->arg);
-	case ISDN_CMD_SETEAZ:
-		return seteaz(card, cmd->arg, cmd->parm.num);
-	case ISDN_CMD_SETL2:
-		return setl2(card, cmd->arg);
-	case ISDN_CMD_SETL3:
-		return setl3(card, cmd->arg);
-	default:
-		return -EINVAL;
-	}
-	return 0;
-}
-
-/*
- * start the onboard firmware
- */
-int startproc(int card)
-{
-	int status;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	/*
-	 * send start msg
-	 */
-	status = sendmessage(card, CMPID, cmReqType2,
-			     cmReqClass0,
-			     cmReqStartProc,
-			     0, 0, NULL);
-	pr_debug("%s: Sent startProc\n", sc_adapter[card]->devicename);
-
-	return status;
-}
-
-
-/*
- * Dials the number passed in
- */
-static int dial(int card, unsigned long channel, setup_parm setup)
-{
-	int status;
-	char Phone[48];
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	/*extract ISDN number to dial from eaz/msn string*/
-	strcpy(Phone, setup.phone);
-
-	/*send the connection message*/
-	status = sendmessage(card, CEPID, ceReqTypePhy,
-			     ceReqClass1,
-			     ceReqPhyConnect,
-			     (unsigned char)channel + 1,
-			     strlen(Phone),
-			     (unsigned int *)Phone);
-
-	pr_debug("%s: Dialing %s on channel %lu\n",
-		 sc_adapter[card]->devicename, Phone, channel + 1);
-
-	return status;
-}
-
-/*
- * Answer an incoming call
- */
-static int answer(int card, unsigned long channel)
-{
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	if (setup_buffers(card, channel + 1)) {
-		hangup(card, channel + 1);
-		return -ENOBUFS;
-	}
-
-	indicate_status(card, ISDN_STAT_BCONN, channel, NULL);
-	pr_debug("%s: Answered incoming call on channel %lu\n",
-		 sc_adapter[card]->devicename, channel + 1);
-	return 0;
-}
-
-/*
- * Hangup up the call on specified channel
- */
-static int hangup(int card, unsigned long channel)
-{
-	int status;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	status = sendmessage(card, CEPID, ceReqTypePhy,
-			     ceReqClass1,
-			     ceReqPhyDisconnect,
-			     (unsigned char)channel + 1,
-			     0,
-			     NULL);
-	pr_debug("%s: Sent HANGUP message to channel %lu\n",
-		 sc_adapter[card]->devicename, channel + 1);
-	return status;
-}
-
-/*
- * Set the layer 2 protocol (X.25, HDLC, Raw)
- */
-static int setl2(int card, unsigned long arg)
-{
-	int status = 0;
-	int protocol, channel;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-	protocol = arg >> 8;
-	channel = arg & 0xff;
-	sc_adapter[card]->channel[channel].l2_proto = protocol;
-
-	/*
-	 * check that the adapter is also set to the correct protocol
-	 */
-	pr_debug("%s: Sending GetFrameFormat for channel %d\n",
-		 sc_adapter[card]->devicename, channel + 1);
-	status = sendmessage(card, CEPID, ceReqTypeCall,
-			     ceReqClass0,
-			     ceReqCallGetFrameFormat,
-			     (unsigned char)channel + 1,
-			     1,
-			     (unsigned int *)protocol);
-	if (status)
-		return status;
-	return 0;
-}
-
-/*
- * Set the layer 3 protocol
- */
-static int setl3(int card, unsigned long channel)
-{
-	int protocol = channel >> 8;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	sc_adapter[card]->channel[channel].l3_proto = protocol;
-	return 0;
-}
-
-static int acceptb(int card, unsigned long channel)
-{
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	if (setup_buffers(card, channel + 1))
-	{
-		hangup(card, channel + 1);
-		return -ENOBUFS;
-	}
-
-	pr_debug("%s: B-Channel connection accepted on channel %lu\n",
-		 sc_adapter[card]->devicename, channel + 1);
-	indicate_status(card, ISDN_STAT_BCONN, channel, NULL);
-	return 0;
-}
-
-static int clreaz(int card, unsigned long arg)
-{
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	strcpy(sc_adapter[card]->channel[arg].eazlist, "");
-	sc_adapter[card]->channel[arg].eazclear = 1;
-	pr_debug("%s: EAZ List cleared for channel %lu\n",
-		 sc_adapter[card]->devicename, arg + 1);
-	return 0;
-}
-
-static int seteaz(int card, unsigned long arg, char *num)
-{
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	strcpy(sc_adapter[card]->channel[arg].eazlist, num);
-	sc_adapter[card]->channel[arg].eazclear = 0;
-	pr_debug("%s: EAZ list for channel %lu set to: %s\n",
-		 sc_adapter[card]->devicename, arg + 1,
-		 sc_adapter[card]->channel[arg].eazlist);
-	return 0;
-}
-
-int reset(int card)
-{
-	unsigned long flags;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	indicate_status(card, ISDN_STAT_STOP, 0, NULL);
-
-	if (sc_adapter[card]->EngineUp) {
-		del_timer(&sc_adapter[card]->stat_timer);
-	}
-
-	sc_adapter[card]->EngineUp = 0;
-
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-	init_timer(&sc_adapter[card]->reset_timer);
-	sc_adapter[card]->reset_timer.function = sc_check_reset;
-	sc_adapter[card]->reset_timer.data = card;
-	sc_adapter[card]->reset_timer.expires = jiffies + CHECKRESET_TIME;
-	add_timer(&sc_adapter[card]->reset_timer);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-
-	outb(0x1, sc_adapter[card]->ioport[SFT_RESET]);
-
-	pr_debug("%s: Adapter Reset\n", sc_adapter[card]->devicename);
-	return 0;
-}
-
-void flushreadfifo(int card)
-{
-	while (inb(sc_adapter[card]->ioport[FIFO_STATUS]) & RF_HAS_DATA)
-		inb(sc_adapter[card]->ioport[FIFO_READ]);
-}
diff --git a/drivers/isdn/sc/event.c b/drivers/isdn/sc/event.c
deleted file mode 100644
index 833d96c2cf92..000000000000
--- a/drivers/isdn/sc/event.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/* $Id: event.c,v 1.4.8.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-
-#ifdef DEBUG
-static char *events[] = { "ISDN_STAT_STAVAIL",
-			  "ISDN_STAT_ICALL",
-			  "ISDN_STAT_RUN",
-			  "ISDN_STAT_STOP",
-			  "ISDN_STAT_DCONN",
-			  "ISDN_STAT_BCONN",
-			  "ISDN_STAT_DHUP",
-			  "ISDN_STAT_BHUP",
-			  "ISDN_STAT_CINF",
-			  "ISDN_STAT_LOAD",
-			  "ISDN_STAT_UNLOAD",
-			  "ISDN_STAT_BSENT",
-			  "ISDN_STAT_NODCH",
-			  "ISDN_STAT_ADDCH",
-			  "ISDN_STAT_CAUSE" };
-#endif
-
-int indicate_status(int card, int event, ulong Channel, char *Data)
-{
-	isdn_ctrl cmd;
-
-#ifdef DEBUG
-	pr_debug("%s: Indicating event %s on Channel %d\n",
-		 sc_adapter[card]->devicename, events[event - 256], Channel);
-#endif
-	if (Data != NULL) {
-		pr_debug("%s: Event data: %s\n", sc_adapter[card]->devicename,
-			 Data);
-		switch (event) {
-		case ISDN_STAT_BSENT:
-			memcpy(&cmd.parm.length, Data, sizeof(cmd.parm.length));
-			break;
-		case ISDN_STAT_ICALL:
-			memcpy(&cmd.parm.setup, Data, sizeof(cmd.parm.setup));
-			break;
-		default:
-			strlcpy(cmd.parm.num, Data, sizeof(cmd.parm.num));
-		}
-	}
-
-	cmd.command = event;
-	cmd.driver = sc_adapter[card]->driverId;
-	cmd.arg = Channel;
-	return sc_adapter[card]->card->statcallb(&cmd);
-}
diff --git a/drivers/isdn/sc/hardware.h b/drivers/isdn/sc/hardware.h
deleted file mode 100644
index 81fbe78701f0..000000000000
--- a/drivers/isdn/sc/hardware.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * Hardware specific macros, defines and structures
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- */
-
-#ifndef HARDWARE_H
-#define HARDWARE_H
-
-#include <asm/param.h>			/* For HZ */
-
-/*
- * General hardware parameters common to all ISA adapters
- */
-
-#define MAX_CARDS	4		/* The maximum number of cards to
-					   control or probe for. */
-
-#define SIGNATURE	0x87654321	/* Board reset signature */
-#define SIG_OFFSET	0x1004		/* Where to find signature in shared RAM */
-#define TRACE_OFFSET	0x1008		/* Trace enable word offset in shared RAM */
-#define BUFFER_OFFSET	0x1800		/* Beginning of buffers */
-
-/* I/O Port parameters */
-#define IOBASE_MIN	0x180		/* Lowest I/O port address */
-#define IOBASE_MAX	0x3C0		/* Highest I/O port address */
-#define IOBASE_OFFSET	0x20		/* Inter-board I/O port gap used during
-					   probing */
-#define FIFORD_OFFSET	0x0
-#define FIFOWR_OFFSET	0x400
-#define FIFOSTAT_OFFSET	0x1000
-#define RESET_OFFSET	0x2800
-#define PG0_OFFSET	0x3000		/* Offset from I/O Base for Page 0 register */
-#define PG1_OFFSET	0x3400		/* Offset from I/O Base for Page 1 register */
-#define PG2_OFFSET	0x3800		/* Offset from I/O Base for Page 2 register */
-#define PG3_OFFSET	0x3C00		/* Offset from I/O Base for Page 3 register */
-
-#define FIFO_READ	0		/* FIFO Read register */
-#define FIFO_WRITE	1		/* FIFO Write rgister */
-#define LO_ADDR_PTR	2		/* Extended RAM Low Addr Pointer */
-#define HI_ADDR_PTR	3		/* Extended RAM High Addr Pointer */
-#define NOT_USED_1	4
-#define FIFO_STATUS	5		/* FIFO Status Register */
-#define NOT_USED_2	6
-#define MEM_OFFSET	7
-#define SFT_RESET	10		/* Reset Register */
-#define EXP_BASE	11		/* Shared RAM Base address */
-#define EXP_PAGE0	12		/* Shared RAM Page0 register */
-#define EXP_PAGE1	13		/* Shared RAM Page1 register */
-#define EXP_PAGE2	14		/* Shared RAM Page2 register */
-#define EXP_PAGE3	15		/* Shared RAM Page3 register */
-#define IRQ_SELECT	16		/* IRQ selection register */
-#define MAX_IO_REGS	17		/* Total number of I/O ports */
-
-/* FIFO register values */
-#define RF_HAS_DATA	0x01		/* fifo has data */
-#define RF_QUART_FULL	0x02		/* fifo quarter full */
-#define RF_HALF_FULL	0x04		/* fifo half full */
-#define RF_NOT_FULL	0x08		/* fifo not full */
-#define WF_HAS_DATA	0x10		/* fifo has data */
-#define WF_QUART_FULL	0x20		/* fifo quarter full */
-#define WF_HALF_FULL	0x40		/* fifo half full */
-#define WF_NOT_FULL	0x80		/* fifo not full */
-
-/* Shared RAM parameters */
-#define SRAM_MIN	0xC0000         /* Lowest host shared RAM address */
-#define SRAM_MAX	0xEFFFF         /* Highest host shared RAM address */
-#define SRAM_PAGESIZE	0x4000		/* Size of one RAM page (16K) */
-
-/* Shared RAM buffer parameters */
-#define BUFFER_SIZE	0x800		/* The size of a buffer in bytes */
-#define BUFFER_BASE	BUFFER_OFFSET	/* Offset from start of shared RAM
-					   where buffer start */
-#define BUFFERS_MAX	16		/* Maximum number of send/receive
-					   buffers per channel */
-#define HDLC_PROTO	0x01		/* Frame Format for Layer 2 */
-
-#define BRI_BOARD	0
-#define POTS_BOARD	1
-#define PRI_BOARD	2
-
-/*
- * Specific hardware parameters for the DataCommute/BRI
- */
-#define BRI_CHANNELS	2		/* Number of B channels */
-#define BRI_BASEPG_VAL	0x98
-#define BRI_MAGIC	0x60000		/* Magic Number */
-#define BRI_MEMSIZE	0x10000		/* Amount of RAM (64K) */
-#define BRI_PARTNO	"72-029"
-#define BRI_FEATURES	ISDN_FEATURE_L2_HDLC | ISDN_FEATURE_L3_TRANS;
-/*
- * Specific hardware parameters for the DataCommute/PRI
- */
-#define PRI_CHANNELS	23		/* Number of B channels */
-#define PRI_BASEPG_VAL	0x88
-#define PRI_MAGIC	0x20000		/* Magic Number */
-#define PRI_MEMSIZE	0x100000	/* Amount of RAM (1M) */
-#define PRI_PARTNO	"72-030"
-#define PRI_FEATURES	ISDN_FEATURE_L2_HDLC | ISDN_FEATURE_L3_TRANS;
-
-/*
- * Some handy macros
- */
-
-/* Determine if a channel number is valid for the adapter */
-#define IS_VALID_CHANNEL(y, x)	((x > 0) && (x <= sc_adapter[y]->channels))
-
-#endif
diff --git a/drivers/isdn/sc/includes.h b/drivers/isdn/sc/includes.h
deleted file mode 100644
index 4766e5b77378..000000000000
--- a/drivers/isdn/sc/includes.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- */
-
-#include <linux/errno.h>
-#include <asm/io.h>
-#include <linux/delay.h>
-#include <linux/kernel.h>
-#include <linux/slab.h>
-#include <linux/mm.h>
-#include <linux/ioport.h>
-#include <linux/timer.h>
-#include <linux/wait.h>
-#include <linux/isdnif.h>
diff --git a/drivers/isdn/sc/init.c b/drivers/isdn/sc/init.c
deleted file mode 100644
index 3597ef47b28a..000000000000
--- a/drivers/isdn/sc/init.c
+++ /dev/null
@@ -1,549 +0,0 @@
-/*
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include "includes.h"
-#include "hardware.h"
-#include "card.h"
-
-MODULE_DESCRIPTION("ISDN4Linux: Driver for Spellcaster card");
-MODULE_AUTHOR("Spellcaster Telecommunications Inc.");
-MODULE_LICENSE("GPL");
-
-board *sc_adapter[MAX_CARDS];
-int cinst;
-
-static char devname[] = "scX";
-static const char version[] = "2.0b1";
-
-static const char *boardname[] = { "DataCommute/BRI", "DataCommute/PRI", "TeleCommute/BRI" };
-
-/* insmod set parameters */
-static unsigned int io[] = {0, 0, 0, 0};
-static unsigned char irq[] = {0, 0, 0, 0};
-static unsigned long ram[] = {0, 0, 0, 0};
-static bool do_reset;
-
-module_param_array(io, int, NULL, 0);
-module_param_array(irq, byte, NULL, 0);
-module_param_array(ram, long, NULL, 0);
-module_param(do_reset, bool, 0);
-
-static int identify_board(unsigned long, unsigned int);
-
-static int __init sc_init(void)
-{
-	int b = -1;
-	int i, j;
-	int status = -ENODEV;
-
-	unsigned long memsize = 0;
-	unsigned long features = 0;
-	isdn_if *interface;
-	unsigned char channels;
-	unsigned char pgport;
-	unsigned long magic;
-	int model;
-	int last_base = IOBASE_MIN;
-	int probe_exhasted = 0;
-
-#ifdef MODULE
-	pr_info("SpellCaster ISA ISDN Adapter Driver rev. %s Loaded\n", version);
-#else
-	pr_info("SpellCaster ISA ISDN Adapter Driver rev. %s\n", version);
-#endif
-	pr_info("Copyright (C) 1996 SpellCaster Telecommunications Inc.\n");
-
-	while (b++ < MAX_CARDS - 1) {
-		pr_debug("Probing for adapter #%d\n", b);
-		/*
-		 * Initialize reusable variables
-		 */
-		model = -1;
-		magic = 0;
-		channels = 0;
-		pgport = 0;
-
-		/*
-		 * See if we should probe for IO base
-		 */
-		pr_debug("I/O Base for board %d is 0x%x, %s probe\n", b, io[b],
-			 io[b] == 0 ? "will" : "won't");
-		if (io[b]) {
-			/*
-			 * No, I/O Base has been provided
-			 */
-			for (i = 0; i < MAX_IO_REGS - 1; i++) {
-				if (!request_region(io[b] + i * 0x400, 1, "sc test")) {
-					pr_debug("request_region for 0x%x failed\n", io[b] + i * 0x400);
-					io[b] = 0;
-					break;
-				} else
-					release_region(io[b] + i * 0x400, 1);
-			}
-
-			/*
-			 * Confirm the I/O Address with a test
-			 */
-			if (io[b] == 0) {
-				pr_debug("I/O Address invalid.\n");
-				continue;
-			}
-
-			outb(0x18, io[b] + 0x400 * EXP_PAGE0);
-			if (inb(io[b] + 0x400 * EXP_PAGE0) != 0x18) {
-				pr_debug("I/O Base 0x%x fails test\n",
-					 io[b] + 0x400 * EXP_PAGE0);
-				continue;
-			}
-		} else {
-			/*
-			 * Yes, probe for I/O Base
-			 */
-			if (probe_exhasted) {
-				pr_debug("All probe addresses exhausted, skipping\n");
-				continue;
-			}
-			pr_debug("Probing for I/O...\n");
-			for (i = last_base; i <= IOBASE_MAX; i += IOBASE_OFFSET) {
-				int found_io = 1;
-				if (i == IOBASE_MAX) {
-					probe_exhasted = 1; /* No more addresses to probe */
-					pr_debug("End of Probes\n");
-				}
-				last_base = i + IOBASE_OFFSET;
-				pr_debug("  checking 0x%x...", i);
-				for (j = 0; j < MAX_IO_REGS - 1; j++) {
-					if (!request_region(i + j * 0x400, 1, "sc test")) {
-						pr_debug("Failed\n");
-						found_io = 0;
-						break;
-					} else
-						release_region(i + j * 0x400, 1);
-				}
-
-				if (found_io) {
-					io[b] = i;
-					outb(0x18, io[b] + 0x400 * EXP_PAGE0);
-					if (inb(io[b] + 0x400 * EXP_PAGE0) != 0x18) {
-						pr_debug("Failed by test\n");
-						continue;
-					}
-					pr_debug("Passed\n");
-					break;
-				}
-			}
-			if (probe_exhasted) {
-				continue;
-			}
-		}
-
-		/*
-		 * See if we should probe for shared RAM
-		 */
-		if (do_reset) {
-			pr_debug("Doing a SAFE probe reset\n");
-			outb(0xFF, io[b] + RESET_OFFSET);
-			msleep_interruptible(10000);
-		}
-		pr_debug("RAM Base for board %d is 0x%lx, %s probe\n", b,
-			 ram[b], ram[b] == 0 ? "will" : "won't");
-
-		if (ram[b]) {
-			/*
-			 * No, the RAM base has been provided
-			 * Just look for a signature and ID the
-			 * board model
-			 */
-			if (request_region(ram[b], SRAM_PAGESIZE, "sc test")) {
-				pr_debug("request_region for RAM base 0x%lx succeeded\n", ram[b]);
-				model = identify_board(ram[b], io[b]);
-				release_region(ram[b], SRAM_PAGESIZE);
-			}
-		} else {
-			/*
-			 * Yes, probe for free RAM and look for
-			 * a signature and id the board model
-			 */
-			for (i = SRAM_MIN; i < SRAM_MAX; i += SRAM_PAGESIZE) {
-				pr_debug("Checking RAM address 0x%x...\n", i);
-				if (request_region(i, SRAM_PAGESIZE, "sc test")) {
-					pr_debug("  request_region succeeded\n");
-					model = identify_board(i, io[b]);
-					release_region(i, SRAM_PAGESIZE);
-					if (model >= 0) {
-						pr_debug("  Identified a %s\n",
-							 boardname[model]);
-						ram[b] = i;
-						break;
-					}
-					pr_debug("  Unidentified or inaccessible\n");
-					continue;
-				}
-				pr_debug("  request failed\n");
-			}
-		}
-		/*
-		 * See if we found free RAM and the board model
-		 */
-		if (!ram[b] || model < 0) {
-			/*
-			 * Nope, there was no place in RAM for the
-			 * board, or it couldn't be identified
-			 */
-			pr_debug("Failed to find an adapter at 0x%lx\n", ram[b]);
-			continue;
-		}
-
-		/*
-		 * Set the board's magic number, memory size and page register
-		 */
-		switch (model) {
-		case PRI_BOARD:
-			channels = 23;
-			magic = 0x20000;
-			memsize = 0x100000;
-			features = PRI_FEATURES;
-			break;
-
-		case BRI_BOARD:
-		case POTS_BOARD:
-			channels = 2;
-			magic = 0x60000;
-			memsize = 0x10000;
-			features = BRI_FEATURES;
-			break;
-		}
-		switch (ram[b] >> 12 & 0x0F) {
-		case 0x0:
-			pr_debug("RAM Page register set to EXP_PAGE0\n");
-			pgport = EXP_PAGE0;
-			break;
-
-		case 0x4:
-			pr_debug("RAM Page register set to EXP_PAGE1\n");
-			pgport = EXP_PAGE1;
-			break;
-
-		case 0x8:
-			pr_debug("RAM Page register set to EXP_PAGE2\n");
-			pgport = EXP_PAGE2;
-			break;
-
-		case 0xC:
-			pr_debug("RAM Page register set to EXP_PAGE3\n");
-			pgport = EXP_PAGE3;
-			break;
-
-		default:
-			pr_debug("RAM base address doesn't fall on 16K boundary\n");
-			continue;
-		}
-
-		pr_debug("current IRQ: %d  b: %d\n", irq[b], b);
-
-		/*
-		 * Make sure we got an IRQ
-		 */
-		if (!irq[b]) {
-			/*
-			 * No interrupt could be used
-			 */
-			pr_debug("Failed to acquire an IRQ line\n");
-			continue;
-		}
-
-		/*
-		 * Horray! We found a board, Make sure we can register
-		 * it with ISDN4Linux
-		 */
-		interface = kzalloc(sizeof(isdn_if), GFP_KERNEL);
-		if (interface == NULL) {
-			/*
-			 * Oops, can't malloc isdn_if
-			 */
-			continue;
-		}
-
-		interface->owner = THIS_MODULE;
-		interface->hl_hdrlen = 0;
-		interface->channels = channels;
-		interface->maxbufsize = BUFFER_SIZE;
-		interface->features = features;
-		interface->writebuf_skb = sndpkt;
-		interface->writecmd = NULL;
-		interface->command = command;
-		strcpy(interface->id, devname);
-		interface->id[2] = '0' + cinst;
-
-		/*
-		 * Allocate the board structure
-		 */
-		sc_adapter[cinst] = kzalloc(sizeof(board), GFP_KERNEL);
-		if (sc_adapter[cinst] == NULL) {
-			/*
-			 * Oops, can't alloc memory for the board
-			 */
-			kfree(interface);
-			continue;
-		}
-		spin_lock_init(&sc_adapter[cinst]->lock);
-
-		if (!register_isdn(interface)) {
-			/*
-			 * Oops, couldn't register for some reason
-			 */
-			kfree(interface);
-			kfree(sc_adapter[cinst]);
-			continue;
-		}
-
-		sc_adapter[cinst]->card = interface;
-		sc_adapter[cinst]->driverId = interface->channels;
-		strcpy(sc_adapter[cinst]->devicename, interface->id);
-		sc_adapter[cinst]->nChannels = channels;
-		sc_adapter[cinst]->ramsize = memsize;
-		sc_adapter[cinst]->shmem_magic = magic;
-		sc_adapter[cinst]->shmem_pgport = pgport;
-		sc_adapter[cinst]->StartOnReset = 1;
-
-		/*
-		 * Allocate channels status structures
-		 */
-		sc_adapter[cinst]->channel = kzalloc(sizeof(bchan) * channels, GFP_KERNEL);
-		if (sc_adapter[cinst]->channel == NULL) {
-			/*
-			 * Oops, can't alloc memory for the channels
-			 */
-			indicate_status(cinst, ISDN_STAT_UNLOAD, 0, NULL);	/* Fix me */
-			kfree(interface);
-			kfree(sc_adapter[cinst]);
-			continue;
-		}
-
-		/*
-		 * Lock down the hardware resources
-		 */
-		sc_adapter[cinst]->interrupt = irq[b];
-		if (request_irq(sc_adapter[cinst]->interrupt, interrupt_handler,
-				0, interface->id,
-				(void *)(unsigned long) cinst)) {
-			kfree(sc_adapter[cinst]->channel);
-			indicate_status(cinst, ISDN_STAT_UNLOAD, 0, NULL);	/* Fix me */
-			kfree(interface);
-			kfree(sc_adapter[cinst]);
-			continue;
-
-		}
-		sc_adapter[cinst]->iobase = io[b];
-		for (i = 0; i < MAX_IO_REGS - 1; i++) {
-			sc_adapter[cinst]->ioport[i] = io[b] + i * 0x400;
-			request_region(sc_adapter[cinst]->ioport[i], 1,
-				       interface->id);
-			pr_debug("Requesting I/O Port %#x\n",
-				 sc_adapter[cinst]->ioport[i]);
-		}
-		sc_adapter[cinst]->ioport[IRQ_SELECT] = io[b] + 0x2;
-		request_region(sc_adapter[cinst]->ioport[IRQ_SELECT], 1,
-			       interface->id);
-		pr_debug("Requesting I/O Port %#x\n",
-			 sc_adapter[cinst]->ioport[IRQ_SELECT]);
-		sc_adapter[cinst]->rambase = ram[b];
-		request_region(sc_adapter[cinst]->rambase, SRAM_PAGESIZE,
-			       interface->id);
-
-		pr_info("  %s (%d) - %s %d channels IRQ %d, I/O Base 0x%x, RAM Base 0x%lx\n",
-			sc_adapter[cinst]->devicename,
-			sc_adapter[cinst]->driverId,
-			boardname[model], channels, irq[b], io[b], ram[b]);
-
-		/*
-		 * reset the adapter to put things in motion
-		 */
-		reset(cinst);
-
-		cinst++;
-		status = 0;
-	}
-	if (status)
-		pr_info("Failed to find any adapters, driver unloaded\n");
-	return status;
-}
-
-static void __exit sc_exit(void)
-{
-	int i, j;
-
-	for (i = 0; i < cinst; i++) {
-		pr_debug("Cleaning up after adapter %d\n", i);
-		/*
-		 * kill the timers
-		 */
-		del_timer_sync(&(sc_adapter[i]->reset_timer));
-		del_timer_sync(&(sc_adapter[i]->stat_timer));
-
-		/*
-		 * Tell I4L we're toast
-		 */
-		indicate_status(i, ISDN_STAT_STOP, 0, NULL);
-		indicate_status(i, ISDN_STAT_UNLOAD, 0, NULL);
-
-		/*
-		 * Release shared RAM
-		 */
-		release_region(sc_adapter[i]->rambase, SRAM_PAGESIZE);
-
-		/*
-		 * Release the IRQ
-		 */
-		free_irq(sc_adapter[i]->interrupt, NULL);
-
-		/*
-		 * Reset for a clean start
-		 */
-		outb(0xFF, sc_adapter[i]->ioport[SFT_RESET]);
-
-		/*
-		 * Release the I/O Port regions
-		 */
-		for (j = 0; j < MAX_IO_REGS - 1; j++) {
-			release_region(sc_adapter[i]->ioport[j], 1);
-			pr_debug("Releasing I/O Port %#x\n",
-				 sc_adapter[i]->ioport[j]);
-		}
-		release_region(sc_adapter[i]->ioport[IRQ_SELECT], 1);
-		pr_debug("Releasing I/O Port %#x\n",
-			 sc_adapter[i]->ioport[IRQ_SELECT]);
-
-		/*
-		 * Release any memory we alloced
-		 */
-		kfree(sc_adapter[i]->channel);
-		kfree(sc_adapter[i]->card);
-		kfree(sc_adapter[i]);
-	}
-	pr_info("SpellCaster ISA ISDN Adapter Driver Unloaded.\n");
-}
-
-static int identify_board(unsigned long rambase, unsigned int iobase)
-{
-	unsigned int pgport;
-	unsigned long sig;
-	DualPortMemory *dpm;
-	RspMessage rcvmsg;
-	ReqMessage sndmsg;
-	HWConfig_pl hwci;
-	int x;
-
-	pr_debug("Attempting to identify adapter @ 0x%lx io 0x%x\n",
-		 rambase, iobase);
-
-	/*
-	 * Enable the base pointer
-	 */
-	outb(rambase >> 12, iobase + 0x2c00);
-
-	switch (rambase >> 12 & 0x0F) {
-	case 0x0:
-		pgport = iobase + PG0_OFFSET;
-		pr_debug("Page Register offset is 0x%x\n", PG0_OFFSET);
-		break;
-
-	case 0x4:
-		pgport = iobase + PG1_OFFSET;
-		pr_debug("Page Register offset is 0x%x\n", PG1_OFFSET);
-		break;
-
-	case 0x8:
-		pgport = iobase + PG2_OFFSET;
-		pr_debug("Page Register offset is 0x%x\n", PG2_OFFSET);
-		break;
-
-	case 0xC:
-		pgport = iobase + PG3_OFFSET;
-		pr_debug("Page Register offset is 0x%x\n", PG3_OFFSET);
-		break;
-	default:
-		pr_debug("Invalid rambase 0x%lx\n", rambase);
-		return -1;
-	}
-
-	/*
-	 * Try to identify a PRI card
-	 */
-	outb(PRI_BASEPG_VAL, pgport);
-	msleep_interruptible(1000);
-	sig = readl(rambase + SIG_OFFSET);
-	pr_debug("Looking for a signature, got 0x%lx\n", sig);
-	if (sig == SIGNATURE)
-		return PRI_BOARD;
-
-	/*
-	 * Try to identify a PRI card
-	 */
-	outb(BRI_BASEPG_VAL, pgport);
-	msleep_interruptible(1000);
-	sig = readl(rambase + SIG_OFFSET);
-	pr_debug("Looking for a signature, got 0x%lx\n", sig);
-	if (sig == SIGNATURE)
-		return BRI_BOARD;
-
-	return -1;
-
-	/*
-	 * Try to spot a card
-	 */
-	sig = readl(rambase + SIG_OFFSET);
-	pr_debug("Looking for a signature, got 0x%lx\n", sig);
-	if (sig != SIGNATURE)
-		return -1;
-
-	dpm = (DualPortMemory *) rambase;
-
-	memset(&sndmsg, 0, MSG_LEN);
-	sndmsg.msg_byte_cnt = 3;
-	sndmsg.type = cmReqType1;
-	sndmsg.class = cmReqClass0;
-	sndmsg.code = cmReqHWConfig;
-	memcpy_toio(&(dpm->req_queue[dpm->req_head++]), &sndmsg, MSG_LEN);
-	outb(0, iobase + 0x400);
-	pr_debug("Sent HWConfig message\n");
-	/*
-	 * Wait for the response
-	 */
-	x = 0;
-	while ((inb(iobase + FIFOSTAT_OFFSET) & RF_HAS_DATA) && x < 100) {
-		schedule_timeout_interruptible(1);
-		x++;
-	}
-	if (x == 100) {
-		pr_debug("Timeout waiting for response\n");
-		return -1;
-	}
-
-	memcpy_fromio(&rcvmsg, &(dpm->rsp_queue[dpm->rsp_tail]), MSG_LEN);
-	pr_debug("Got HWConfig response, status = 0x%x\n", rcvmsg.rsp_status);
-	memcpy(&hwci, &(rcvmsg.msg_data.HWCresponse), sizeof(HWConfig_pl));
-	pr_debug("Hardware Config: Interface: %s, RAM Size: %ld, Serial: %s\n"
-		 "                 Part: %s, Rev: %s\n",
-		 hwci.st_u_sense ? "S/T" : "U", hwci.ram_size,
-		 hwci.serial_no, hwci.part_no, hwci.rev_no);
-
-	if (!strncmp(PRI_PARTNO, hwci.part_no, 6))
-		return PRI_BOARD;
-	if (!strncmp(BRI_PARTNO, hwci.part_no, 6))
-		return BRI_BOARD;
-
-	return -1;
-}
-
-module_init(sc_init);
-module_exit(sc_exit);
diff --git a/drivers/isdn/sc/interrupt.c b/drivers/isdn/sc/interrupt.c
deleted file mode 100644
index e80cc76bc314..000000000000
--- a/drivers/isdn/sc/interrupt.c
+++ /dev/null
@@ -1,247 +0,0 @@
-/* $Id: interrupt.c,v 1.4.8.3 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-#include <linux/interrupt.h>
-
-/*
- *
- */
-irqreturn_t interrupt_handler(int dummy, void *card_inst)
-{
-
-	RspMessage rcvmsg;
-	int channel;
-	int card = (int)(unsigned long) card_inst;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return IRQ_NONE;
-	}
-
-	pr_debug("%s: Entered Interrupt handler\n",
-		 sc_adapter[card]->devicename);
-
-	/*
-	 * Pull all of the waiting messages off the response queue
-	 */
-	while (!receivemessage(card, &rcvmsg)) {
-		/*
-		 * Push the message to the adapter structure for
-		 * send_and_receive to snoop
-		 */
-		if (sc_adapter[card]->want_async_messages)
-			memcpy(&(sc_adapter[card]->async_msg),
-			       &rcvmsg, sizeof(RspMessage));
-
-		channel = (unsigned int) rcvmsg.phy_link_no;
-
-		/*
-		 * Trap Invalid request messages
-		 */
-		if (IS_CM_MESSAGE(rcvmsg, 0, 0, Invalid)) {
-			pr_debug("%s: Invalid request Message, rsp_status = %d\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.rsp_status);
-			break;
-		}
-
-		/*
-		 * Check for a linkRead message
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Lnk, 1, Read))
-		{
-			pr_debug("%s: Received packet 0x%x bytes long at 0x%lx\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.msg_data.response.msg_len,
-				 rcvmsg.msg_data.response.buff_offset);
-			rcvpkt(card, &rcvmsg);
-			continue;
-
-		}
-
-		/*
-		 * Handle a write acknoledgement
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Lnk, 1, Write)) {
-			pr_debug("%s: Packet Send ACK on channel %d\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.phy_link_no);
-			sc_adapter[card]->channel[rcvmsg.phy_link_no - 1].free_sendbufs++;
-			continue;
-		}
-
-		/*
-		 * Handle a connection message
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Phy, 1, Connect))
-		{
-			unsigned int callid;
-			setup_parm setup;
-			pr_debug("%s: Connect message: line %d: status %d: cause 0x%x\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.phy_link_no,
-				 rcvmsg.rsp_status,
-				 rcvmsg.msg_data.byte_array[2]);
-
-			memcpy(&callid, rcvmsg.msg_data.byte_array, sizeof(int));
-			if (callid >= 0x8000 && callid <= 0xFFFF)
-			{
-				pr_debug("%s: Got Dial-Out Rsp\n",
-					 sc_adapter[card]->devicename);
-				indicate_status(card, ISDN_STAT_DCONN,
-						(unsigned long)rcvmsg.phy_link_no - 1, NULL);
-
-			}
-			else if (callid >= 0x0000 && callid <= 0x7FFF)
-			{
-				int len;
-
-				pr_debug("%s: Got Incoming Call\n",
-					 sc_adapter[card]->devicename);
-				len = strlcpy(setup.phone, &(rcvmsg.msg_data.byte_array[4]),
-					      sizeof(setup.phone));
-				if (len >= sizeof(setup.phone))
-					continue;
-				len = strlcpy(setup.eazmsn,
-					      sc_adapter[card]->channel[rcvmsg.phy_link_no - 1].dn,
-					      sizeof(setup.eazmsn));
-				if (len >= sizeof(setup.eazmsn))
-					continue;
-				setup.si1 = 7;
-				setup.si2 = 0;
-				setup.plan = 0;
-				setup.screen = 0;
-
-				indicate_status(card, ISDN_STAT_ICALL, (unsigned long)rcvmsg.phy_link_no - 1, (char *)&setup);
-				indicate_status(card, ISDN_STAT_DCONN, (unsigned long)rcvmsg.phy_link_no - 1, NULL);
-			}
-			continue;
-		}
-
-		/*
-		 * Handle a disconnection message
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Phy, 1, Disconnect))
-		{
-			pr_debug("%s: disconnect message: line %d: status %d: cause 0x%x\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.phy_link_no,
-				 rcvmsg.rsp_status,
-				 rcvmsg.msg_data.byte_array[2]);
-
-			indicate_status(card, ISDN_STAT_BHUP, (unsigned long)rcvmsg.phy_link_no - 1, NULL);
-			indicate_status(card, ISDN_STAT_DHUP, (unsigned long)rcvmsg.phy_link_no - 1, NULL);
-			continue;
-
-		}
-
-		/*
-		 * Handle a startProc engine up message
-		 */
-		if (IS_CM_MESSAGE(rcvmsg, 5, 0, MiscEngineUp)) {
-			pr_debug("%s: Received EngineUp message\n",
-				 sc_adapter[card]->devicename);
-			sc_adapter[card]->EngineUp = 1;
-			sendmessage(card, CEPID, ceReqTypeCall, ceReqClass0, ceReqCallGetMyNumber, 1, 0, NULL);
-			sendmessage(card, CEPID, ceReqTypeCall, ceReqClass0, ceReqCallGetMyNumber, 2, 0, NULL);
-			init_timer(&sc_adapter[card]->stat_timer);
-			sc_adapter[card]->stat_timer.function = check_phystat;
-			sc_adapter[card]->stat_timer.data = card;
-			sc_adapter[card]->stat_timer.expires = jiffies + CHECKSTAT_TIME;
-			add_timer(&sc_adapter[card]->stat_timer);
-			continue;
-		}
-
-		/*
-		 * Start proc response
-		 */
-		if (IS_CM_MESSAGE(rcvmsg, 2, 0, StartProc)) {
-			pr_debug("%s: StartProc Response Status %d\n",
-				 sc_adapter[card]->devicename,
-				 rcvmsg.rsp_status);
-			continue;
-		}
-
-		/*
-		 * Handle a GetMyNumber Rsp
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Call, 0, GetMyNumber)) {
-			strlcpy(sc_adapter[card]->channel[rcvmsg.phy_link_no - 1].dn,
-				rcvmsg.msg_data.byte_array,
-				sizeof(rcvmsg.msg_data.byte_array));
-			continue;
-		}
-
-		/*
-		 * PhyStatus response
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Phy, 2, Status)) {
-			unsigned int b1stat, b2stat;
-
-			/*
-			 * Covert the message data to the adapter->phystat code
-			 */
-			b1stat = (unsigned int) rcvmsg.msg_data.byte_array[0];
-			b2stat = (unsigned int) rcvmsg.msg_data.byte_array[1];
-
-			sc_adapter[card]->nphystat = (b2stat >> 8) | b1stat; /* endian?? */
-			pr_debug("%s: PhyStat is 0x%2x\n",
-				 sc_adapter[card]->devicename,
-				 sc_adapter[card]->nphystat);
-			continue;
-		}
-
-
-		/*
-		 * Handle a GetFramFormat
-		 */
-		if (IS_CE_MESSAGE(rcvmsg, Call, 0, GetFrameFormat)) {
-			if (rcvmsg.msg_data.byte_array[0] != HDLC_PROTO) {
-				unsigned int proto = HDLC_PROTO;
-				/*
-				 * Set board format to HDLC if it wasn't already
-				 */
-				pr_debug("%s: current frame format: 0x%x, will change to HDLC\n",
-					 sc_adapter[card]->devicename,
-					 rcvmsg.msg_data.byte_array[0]);
-				sendmessage(card, CEPID, ceReqTypeCall,
-					    ceReqClass0,
-					    ceReqCallSetFrameFormat,
-					    (unsigned char)channel + 1,
-					    1, &proto);
-			}
-			continue;
-		}
-
-		/*
-		 * Hmm...
-		 */
-		pr_debug("%s: Received unhandled message (%d,%d,%d) link %d\n",
-			 sc_adapter[card]->devicename,
-			 rcvmsg.type, rcvmsg.class, rcvmsg.code,
-			 rcvmsg.phy_link_no);
-
-	}	/* while */
-
-	pr_debug("%s: Exiting Interrupt Handler\n",
-		 sc_adapter[card]->devicename);
-	return IRQ_HANDLED;
-}
diff --git a/drivers/isdn/sc/ioctl.c b/drivers/isdn/sc/ioctl.c
deleted file mode 100644
index e63983aa1d27..000000000000
--- a/drivers/isdn/sc/ioctl.c
+++ /dev/null
@@ -1,582 +0,0 @@
-/*
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- */
-
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-#include "scioc.h"
-
-static int GetStatus(int card, boardInfo *);
-
-/*
- * Process private IOCTL messages (typically from scctrl)
- */
-int sc_ioctl(int card, scs_ioctl *data)
-{
-	int		status;
-	RspMessage	*rcvmsg;
-	char		*spid;
-	char		*dn;
-	char		switchtype;
-	char		speed;
-
-	rcvmsg = kmalloc(sizeof(RspMessage), GFP_KERNEL);
-	if (!rcvmsg)
-		return -ENOMEM;
-
-	switch (data->command) {
-	case SCIOCRESET:	/* Perform a hard reset of the adapter */
-	{
-		pr_debug("%s: SCIOCRESET: ioctl received\n",
-			 sc_adapter[card]->devicename);
-		sc_adapter[card]->StartOnReset = 0;
-		kfree(rcvmsg);
-		return reset(card);
-	}
-
-	case SCIOCLOAD:
-	{
-		char *srec;
-
-		srec = kmalloc(SCIOC_SRECSIZE, GFP_KERNEL);
-		if (!srec) {
-			kfree(rcvmsg);
-			return -ENOMEM;
-		}
-		pr_debug("%s: SCIOLOAD: ioctl received\n",
-			 sc_adapter[card]->devicename);
-		if (sc_adapter[card]->EngineUp) {
-			pr_debug("%s: SCIOCLOAD: command failed, LoadProc while engine running.\n",
-				 sc_adapter[card]->devicename);
-			kfree(rcvmsg);
-			kfree(srec);
-			return -1;
-		}
-
-		/*
-		 * Get the SRec from user space
-		 */
-		if (copy_from_user(srec, data->dataptr, SCIOC_SRECSIZE)) {
-			kfree(rcvmsg);
-			kfree(srec);
-			return -EFAULT;
-		}
-
-		status = send_and_receive(card, CMPID, cmReqType2, cmReqClass0, cmReqLoadProc,
-					  0, SCIOC_SRECSIZE, srec, rcvmsg, SAR_TIMEOUT);
-		kfree(rcvmsg);
-		kfree(srec);
-
-		if (status) {
-			pr_debug("%s: SCIOCLOAD: command failed, status = %d\n",
-				 sc_adapter[card]->devicename, status);
-			return -1;
-		}
-		else {
-			pr_debug("%s: SCIOCLOAD: command successful\n",
-				 sc_adapter[card]->devicename);
-			return 0;
-		}
-	}
-
-	case SCIOCSTART:
-	{
-		kfree(rcvmsg);
-		pr_debug("%s: SCIOSTART: ioctl received\n",
-			 sc_adapter[card]->devicename);
-		if (sc_adapter[card]->EngineUp) {
-			pr_debug("%s: SCIOCSTART: command failed, engine already running.\n",
-				 sc_adapter[card]->devicename);
-			return -1;
-		}
-
-		sc_adapter[card]->StartOnReset = 1;
-		startproc(card);
-		return 0;
-	}
-
-	case SCIOCSETSWITCH:
-	{
-		pr_debug("%s: SCIOSETSWITCH: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the switch type from user space
-		 */
-		if (copy_from_user(&switchtype, data->dataptr, sizeof(char))) {
-			kfree(rcvmsg);
-			return -EFAULT;
-		}
-
-		pr_debug("%s: SCIOCSETSWITCH: setting switch type to %d\n",
-			 sc_adapter[card]->devicename,
-			 switchtype);
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0, ceReqCallSetSwitchType,
-					  0, sizeof(char), &switchtype, rcvmsg, SAR_TIMEOUT);
-		if (!status && !(rcvmsg->rsp_status)) {
-			pr_debug("%s: SCIOCSETSWITCH: command successful\n",
-				 sc_adapter[card]->devicename);
-			kfree(rcvmsg);
-			return 0;
-		}
-		else {
-			pr_debug("%s: SCIOCSETSWITCH: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			return status;
-		}
-	}
-
-	case SCIOCGETSWITCH:
-	{
-		pr_debug("%s: SCIOGETSWITCH: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the switch type from the board
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0,
-					  ceReqCallGetSwitchType, 0, 0, NULL, rcvmsg, SAR_TIMEOUT);
-		if (!status && !(rcvmsg->rsp_status)) {
-			pr_debug("%s: SCIOCGETSWITCH: command successful\n",
-				 sc_adapter[card]->devicename);
-		}
-		else {
-			pr_debug("%s: SCIOCGETSWITCH: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			return status;
-		}
-
-		switchtype = rcvmsg->msg_data.byte_array[0];
-
-		/*
-		 * Package the switch type and send to user space
-		 */
-		if (copy_to_user(data->dataptr, &switchtype,
-				 sizeof(char))) {
-			kfree(rcvmsg);
-			return -EFAULT;
-		}
-
-		kfree(rcvmsg);
-		return 0;
-	}
-
-	case SCIOCGETSPID:
-	{
-		pr_debug("%s: SCIOGETSPID: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		spid = kzalloc(SCIOC_SPIDSIZE, GFP_KERNEL);
-		if (!spid) {
-			kfree(rcvmsg);
-			return -ENOMEM;
-		}
-		/*
-		 * Get the spid from the board
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0, ceReqCallGetSPID,
-					  data->channel, 0, NULL, rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			pr_debug("%s: SCIOCGETSPID: command successful\n",
-				 sc_adapter[card]->devicename);
-		} else {
-			pr_debug("%s: SCIOCGETSPID: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(spid);
-			kfree(rcvmsg);
-			return status;
-		}
-		strlcpy(spid, rcvmsg->msg_data.byte_array, SCIOC_SPIDSIZE);
-
-		/*
-		 * Package the switch type and send to user space
-		 */
-		if (copy_to_user(data->dataptr, spid, SCIOC_SPIDSIZE)) {
-			kfree(spid);
-			kfree(rcvmsg);
-			return -EFAULT;
-		}
-
-		kfree(spid);
-		kfree(rcvmsg);
-		return 0;
-	}
-
-	case SCIOCSETSPID:
-	{
-		pr_debug("%s: DCBIOSETSPID: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the spid from user space
-		 */
-		spid = memdup_user(data->dataptr, SCIOC_SPIDSIZE);
-		if (IS_ERR(spid)) {
-			kfree(rcvmsg);
-			return PTR_ERR(spid);
-		}
-
-		pr_debug("%s: SCIOCSETSPID: setting channel %d spid to %s\n",
-			 sc_adapter[card]->devicename, data->channel, spid);
-		status = send_and_receive(card, CEPID, ceReqTypeCall,
-					  ceReqClass0, ceReqCallSetSPID, data->channel,
-					  strlen(spid), spid, rcvmsg, SAR_TIMEOUT);
-		if (!status && !(rcvmsg->rsp_status)) {
-			pr_debug("%s: SCIOCSETSPID: command successful\n",
-				 sc_adapter[card]->devicename);
-			kfree(rcvmsg);
-			kfree(spid);
-			return 0;
-		}
-		else {
-			pr_debug("%s: SCIOCSETSPID: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			kfree(spid);
-			return status;
-		}
-	}
-
-	case SCIOCGETDN:
-	{
-		pr_debug("%s: SCIOGETDN: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the dn from the board
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0, ceReqCallGetMyNumber,
-					  data->channel, 0, NULL, rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			pr_debug("%s: SCIOCGETDN: command successful\n",
-				 sc_adapter[card]->devicename);
-		}
-		else {
-			pr_debug("%s: SCIOCGETDN: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			return status;
-		}
-
-		dn = kzalloc(SCIOC_DNSIZE, GFP_KERNEL);
-		if (!dn) {
-			kfree(rcvmsg);
-			return -ENOMEM;
-		}
-		strlcpy(dn, rcvmsg->msg_data.byte_array, SCIOC_DNSIZE);
-		kfree(rcvmsg);
-
-		/*
-		 * Package the dn and send to user space
-		 */
-		if (copy_to_user(data->dataptr, dn, SCIOC_DNSIZE)) {
-			kfree(dn);
-			return -EFAULT;
-		}
-		kfree(dn);
-		return 0;
-	}
-
-	case SCIOCSETDN:
-	{
-		pr_debug("%s: SCIOSETDN: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the spid from user space
-		 */
-		dn = memdup_user(data->dataptr, SCIOC_DNSIZE);
-		if (IS_ERR(dn)) {
-			kfree(rcvmsg);
-			return PTR_ERR(dn);
-		}
-
-		pr_debug("%s: SCIOCSETDN: setting channel %d dn to %s\n",
-			 sc_adapter[card]->devicename, data->channel, dn);
-		status = send_and_receive(card, CEPID, ceReqTypeCall,
-					  ceReqClass0, ceReqCallSetMyNumber, data->channel,
-					  strlen(dn), dn, rcvmsg, SAR_TIMEOUT);
-		if (!status && !(rcvmsg->rsp_status)) {
-			pr_debug("%s: SCIOCSETDN: command successful\n",
-				 sc_adapter[card]->devicename);
-			kfree(rcvmsg);
-			kfree(dn);
-			return 0;
-		}
-		else {
-			pr_debug("%s: SCIOCSETDN: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			kfree(dn);
-			return status;
-		}
-	}
-
-	case SCIOCTRACE:
-
-		pr_debug("%s: SCIOTRACE: ioctl received\n",
-			 sc_adapter[card]->devicename);
-/*		sc_adapter[card]->trace = !sc_adapter[card]->trace;
-		pr_debug("%s: SCIOCTRACE: tracing turned %s\n",
-		sc_adapter[card]->devicename,
-		sc_adapter[card]->trace ? "ON" : "OFF"); */
-		break;
-
-	case SCIOCSTAT:
-	{
-		boardInfo *bi;
-
-		pr_debug("%s: SCIOSTAT: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		bi = kzalloc(sizeof(boardInfo), GFP_KERNEL);
-		if (!bi) {
-			kfree(rcvmsg);
-			return -ENOMEM;
-		}
-
-		kfree(rcvmsg);
-		GetStatus(card, bi);
-
-		if (copy_to_user(data->dataptr, bi, sizeof(boardInfo))) {
-			kfree(bi);
-			return -EFAULT;
-		}
-
-		kfree(bi);
-		return 0;
-	}
-
-	case SCIOCGETSPEED:
-	{
-		pr_debug("%s: SCIOGETSPEED: ioctl received\n",
-			 sc_adapter[card]->devicename);
-
-		/*
-		 * Get the speed from the board
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0,
-					  ceReqCallGetCallType, data->channel, 0, NULL, rcvmsg, SAR_TIMEOUT);
-		if (!status && !(rcvmsg->rsp_status)) {
-			pr_debug("%s: SCIOCGETSPEED: command successful\n",
-				 sc_adapter[card]->devicename);
-		}
-		else {
-			pr_debug("%s: SCIOCGETSPEED: command failed (status = %d)\n",
-				 sc_adapter[card]->devicename, status);
-			kfree(rcvmsg);
-			return status;
-		}
-
-		speed = rcvmsg->msg_data.byte_array[0];
-
-		kfree(rcvmsg);
-
-		/*
-		 * Package the switch type and send to user space
-		 */
-
-		if (copy_to_user(data->dataptr, &speed, sizeof(char)))
-			return -EFAULT;
-
-		return 0;
-	}
-
-	case SCIOCSETSPEED:
-		pr_debug("%s: SCIOCSETSPEED: ioctl received\n",
-			 sc_adapter[card]->devicename);
-		break;
-
-	case SCIOCLOOPTST:
-		pr_debug("%s: SCIOCLOOPTST: ioctl received\n",
-			 sc_adapter[card]->devicename);
-		break;
-
-	default:
-		kfree(rcvmsg);
-		return -1;
-	}
-
-	kfree(rcvmsg);
-	return 0;
-}
-
-static int GetStatus(int card, boardInfo *bi)
-{
-	RspMessage rcvmsg;
-	int i, status;
-
-	/*
-	 * Fill in some of the basic info about the board
-	 */
-	bi->modelid = sc_adapter[card]->model;
-	strcpy(bi->serial_no, sc_adapter[card]->hwconfig.serial_no);
-	strcpy(bi->part_no, sc_adapter[card]->hwconfig.part_no);
-	bi->iobase = sc_adapter[card]->iobase;
-	bi->rambase = sc_adapter[card]->rambase;
-	bi->irq = sc_adapter[card]->interrupt;
-	bi->ramsize = sc_adapter[card]->hwconfig.ram_size;
-	bi->interface = sc_adapter[card]->hwconfig.st_u_sense;
-	strcpy(bi->load_ver, sc_adapter[card]->load_ver);
-	strcpy(bi->proc_ver, sc_adapter[card]->proc_ver);
-
-	/*
-	 * Get the current PhyStats and LnkStats
-	 */
-	status = send_and_receive(card, CEPID, ceReqTypePhy, ceReqClass2,
-				  ceReqPhyStatus, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-	if (!status) {
-		if (sc_adapter[card]->model < PRI_BOARD) {
-			bi->l1_status = rcvmsg.msg_data.byte_array[2];
-			for (i = 0; i < BRI_CHANNELS; i++)
-				bi->status.bristats[i].phy_stat =
-					rcvmsg.msg_data.byte_array[i];
-		}
-		else {
-			bi->l1_status = rcvmsg.msg_data.byte_array[0];
-			bi->l2_status = rcvmsg.msg_data.byte_array[1];
-			for (i = 0; i < PRI_CHANNELS; i++)
-				bi->status.pristats[i].phy_stat =
-					rcvmsg.msg_data.byte_array[i + 2];
-		}
-	}
-
-	/*
-	 * Get the call types for each channel
-	 */
-	for (i = 0; i < sc_adapter[card]->nChannels; i++) {
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0,
-					  ceReqCallGetCallType, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			if (sc_adapter[card]->model == PRI_BOARD) {
-				bi->status.pristats[i].call_type =
-					rcvmsg.msg_data.byte_array[0];
-			}
-			else {
-				bi->status.bristats[i].call_type =
-					rcvmsg.msg_data.byte_array[0];
-			}
-		}
-	}
-
-	/*
-	 * If PRI, get the call states and service states for each channel
-	 */
-	if (sc_adapter[card]->model == PRI_BOARD) {
-		/*
-		 * Get the call states
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeStat, ceReqClass2,
-					  ceReqPhyChCallState, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			for (i = 0; i < PRI_CHANNELS; i++)
-				bi->status.pristats[i].call_state =
-					rcvmsg.msg_data.byte_array[i];
-		}
-
-		/*
-		 * Get the service states
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeStat, ceReqClass2,
-					  ceReqPhyChServState, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			for (i = 0; i < PRI_CHANNELS; i++)
-				bi->status.pristats[i].serv_state =
-					rcvmsg.msg_data.byte_array[i];
-		}
-
-		/*
-		 * Get the link stats for the channels
-		 */
-		for (i = 1; i <= PRI_CHANNELS; i++) {
-			status = send_and_receive(card, CEPID, ceReqTypeLnk, ceReqClass0,
-						  ceReqLnkGetStats, i, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-			if (!status) {
-				bi->status.pristats[i - 1].link_stats.tx_good =
-					(unsigned long)rcvmsg.msg_data.byte_array[0];
-				bi->status.pristats[i - 1].link_stats.tx_bad =
-					(unsigned long)rcvmsg.msg_data.byte_array[4];
-				bi->status.pristats[i - 1].link_stats.rx_good =
-					(unsigned long)rcvmsg.msg_data.byte_array[8];
-				bi->status.pristats[i - 1].link_stats.rx_bad =
-					(unsigned long)rcvmsg.msg_data.byte_array[12];
-			}
-		}
-
-		/*
-		 * Link stats for the D channel
-		 */
-		status = send_and_receive(card, CEPID, ceReqTypeLnk, ceReqClass0,
-					  ceReqLnkGetStats, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status) {
-			bi->dch_stats.tx_good = (unsigned long)rcvmsg.msg_data.byte_array[0];
-			bi->dch_stats.tx_bad = (unsigned long)rcvmsg.msg_data.byte_array[4];
-			bi->dch_stats.rx_good = (unsigned long)rcvmsg.msg_data.byte_array[8];
-			bi->dch_stats.rx_bad = (unsigned long)rcvmsg.msg_data.byte_array[12];
-		}
-
-		return 0;
-	}
-
-	/*
-	 * If BRI or POTS, Get SPID, DN and call types for each channel
-	 */
-
-	/*
-	 * Get the link stats for the channels
-	 */
-	status = send_and_receive(card, CEPID, ceReqTypeLnk, ceReqClass0,
-				  ceReqLnkGetStats, 0, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-	if (!status) {
-		bi->dch_stats.tx_good = (unsigned long)rcvmsg.msg_data.byte_array[0];
-		bi->dch_stats.tx_bad = (unsigned long)rcvmsg.msg_data.byte_array[4];
-		bi->dch_stats.rx_good = (unsigned long)rcvmsg.msg_data.byte_array[8];
-		bi->dch_stats.rx_bad = (unsigned long)rcvmsg.msg_data.byte_array[12];
-		bi->status.bristats[0].link_stats.tx_good =
-			(unsigned long)rcvmsg.msg_data.byte_array[16];
-		bi->status.bristats[0].link_stats.tx_bad =
-			(unsigned long)rcvmsg.msg_data.byte_array[20];
-		bi->status.bristats[0].link_stats.rx_good =
-			(unsigned long)rcvmsg.msg_data.byte_array[24];
-		bi->status.bristats[0].link_stats.rx_bad =
-			(unsigned long)rcvmsg.msg_data.byte_array[28];
-		bi->status.bristats[1].link_stats.tx_good =
-			(unsigned long)rcvmsg.msg_data.byte_array[32];
-		bi->status.bristats[1].link_stats.tx_bad =
-			(unsigned long)rcvmsg.msg_data.byte_array[36];
-		bi->status.bristats[1].link_stats.rx_good =
-			(unsigned long)rcvmsg.msg_data.byte_array[40];
-		bi->status.bristats[1].link_stats.rx_bad =
-			(unsigned long)rcvmsg.msg_data.byte_array[44];
-	}
-
-	/*
-	 * Get the SPIDs
-	 */
-	for (i = 0; i < BRI_CHANNELS; i++) {
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0,
-					  ceReqCallGetSPID, i + 1, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status)
-			strcpy(bi->status.bristats[i].spid, rcvmsg.msg_data.byte_array);
-	}
-
-	/*
-	 * Get the DNs
-	 */
-	for (i = 0; i < BRI_CHANNELS; i++) {
-		status = send_and_receive(card, CEPID, ceReqTypeCall, ceReqClass0,
-					  ceReqCallGetMyNumber, i + 1, 0, NULL, &rcvmsg, SAR_TIMEOUT);
-		if (!status)
-			strcpy(bi->status.bristats[i].dn, rcvmsg.msg_data.byte_array);
-	}
-
-	return 0;
-}
diff --git a/drivers/isdn/sc/message.c b/drivers/isdn/sc/message.c
deleted file mode 100644
index 9679a1902b32..000000000000
--- a/drivers/isdn/sc/message.c
+++ /dev/null
@@ -1,230 +0,0 @@
-/* $Id: message.c,v 1.5.8.2 2001/09/23 22:24:59 kai Exp $
- *
- * functions for sending and receiving control messages
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-#include <linux/sched.h>
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-
-/*
- * receive a message from the board
- */
-int receivemessage(int card, RspMessage *rspmsg)
-{
-	DualPortMemory *dpm;
-	unsigned long flags;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -EINVAL;
-	}
-
-	pr_debug("%s: Entered receivemessage\n",
-		 sc_adapter[card]->devicename);
-
-	/*
-	 * See if there are messages waiting
-	 */
-	if (inb(sc_adapter[card]->ioport[FIFO_STATUS]) & RF_HAS_DATA) {
-		/*
-		 * Map in the DPM to the base page and copy the message
-		 */
-		spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-		outb((sc_adapter[card]->shmem_magic >> 14) | 0x80,
-		     sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport]);
-		dpm = (DualPortMemory *) sc_adapter[card]->rambase;
-		memcpy_fromio(rspmsg, &(dpm->rsp_queue[dpm->rsp_tail]),
-			      MSG_LEN);
-		dpm->rsp_tail = (dpm->rsp_tail + 1) % MAX_MESSAGES;
-		inb(sc_adapter[card]->ioport[FIFO_READ]);
-		spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-		/*
-		 * Tell the board that the message is received
-		 */
-		pr_debug("%s: Received Message seq:%d pid:%d time:%d cmd:%d "
-			 "cnt:%d (type,class,code):(%d,%d,%d) "
-			 "link:%d stat:0x%x\n",
-			 sc_adapter[card]->devicename,
-			 rspmsg->sequence_no,
-			 rspmsg->process_id,
-			 rspmsg->time_stamp,
-			 rspmsg->cmd_sequence_no,
-			 rspmsg->msg_byte_cnt,
-			 rspmsg->type,
-			 rspmsg->class,
-			 rspmsg->code,
-			 rspmsg->phy_link_no,
-			 rspmsg->rsp_status);
-
-		return 0;
-	}
-	return -ENOMSG;
-}
-
-/*
- * send a message to the board
- */
-int sendmessage(int card,
-		unsigned int procid,
-		unsigned int type,
-		unsigned int class,
-		unsigned int code,
-		unsigned int link,
-		unsigned int data_len,
-		unsigned int *data)
-{
-	DualPortMemory *dpm;
-	ReqMessage sndmsg;
-	unsigned long flags;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -EINVAL;
-	}
-
-	/*
-	 * Make sure we only send CEPID messages when the engine is up
-	 * and CMPID messages when it is down
-	 */
-	if (sc_adapter[card]->EngineUp && procid == CMPID) {
-		pr_debug("%s: Attempt to send CM message with engine up\n",
-			 sc_adapter[card]->devicename);
-		return -ESRCH;
-	}
-
-	if (!sc_adapter[card]->EngineUp && procid == CEPID) {
-		pr_debug("%s: Attempt to send CE message with engine down\n",
-			 sc_adapter[card]->devicename);
-		return -ESRCH;
-	}
-
-	memset(&sndmsg, 0, MSG_LEN);
-	sndmsg.msg_byte_cnt = 4;
-	sndmsg.type = type;
-	sndmsg.class = class;
-	sndmsg.code = code;
-	sndmsg.phy_link_no = link;
-
-	if (data_len > 0) {
-		if (data_len > MSG_DATA_LEN)
-			data_len = MSG_DATA_LEN;
-		memcpy(&(sndmsg.msg_data), data, data_len);
-		sndmsg.msg_byte_cnt = data_len + 8;
-	}
-
-	sndmsg.process_id = procid;
-	sndmsg.sequence_no = sc_adapter[card]->seq_no++ % 256;
-
-	/*
-	 * wait for an empty slot in the queue
-	 */
-	while (!(inb(sc_adapter[card]->ioport[FIFO_STATUS]) & WF_NOT_FULL))
-		udelay(1);
-
-	/*
-	 * Disable interrupts and map in shared memory
-	 */
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-	outb((sc_adapter[card]->shmem_magic >> 14) | 0x80,
-	     sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport]);
-	dpm = (DualPortMemory *) sc_adapter[card]->rambase;	/* Fix me */
-	memcpy_toio(&(dpm->req_queue[dpm->req_head]), &sndmsg, MSG_LEN);
-	dpm->req_head = (dpm->req_head + 1) % MAX_MESSAGES;
-	outb(sndmsg.sequence_no, sc_adapter[card]->ioport[FIFO_WRITE]);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-
-	pr_debug("%s: Sent Message seq:%d pid:%d time:%d "
-		 "cnt:%d (type,class,code):(%d,%d,%d) "
-		 "link:%d\n ",
-		 sc_adapter[card]->devicename,
-		 sndmsg.sequence_no,
-		 sndmsg.process_id,
-		 sndmsg.time_stamp,
-		 sndmsg.msg_byte_cnt,
-		 sndmsg.type,
-		 sndmsg.class,
-		 sndmsg.code,
-		 sndmsg.phy_link_no);
-
-	return 0;
-}
-
-int send_and_receive(int card,
-		     unsigned int procid,
-		     unsigned char type,
-		     unsigned char class,
-		     unsigned char code,
-		     unsigned char link,
-		     unsigned char data_len,
-		     unsigned char *data,
-		     RspMessage *mesgdata,
-		     int timeout)
-{
-	int retval;
-	int tries;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return -EINVAL;
-	}
-
-	sc_adapter[card]->want_async_messages = 1;
-	retval = sendmessage(card, procid, type, class, code, link,
-			     data_len, (unsigned int *) data);
-
-	if (retval) {
-		pr_debug("%s: SendMessage failed in SAR\n",
-			 sc_adapter[card]->devicename);
-		sc_adapter[card]->want_async_messages = 0;
-		return -EIO;
-	}
-
-	tries = 0;
-	/* wait for the response */
-	while (tries < timeout) {
-		schedule_timeout_interruptible(1);
-
-		pr_debug("SAR waiting..\n");
-
-		/*
-		 * See if we got our message back
-		 */
-		if ((sc_adapter[card]->async_msg.type == type) &&
-		    (sc_adapter[card]->async_msg.class == class) &&
-		    (sc_adapter[card]->async_msg.code == code) &&
-		    (sc_adapter[card]->async_msg.phy_link_no == link)) {
-
-			/*
-			 * Got it!
-			 */
-			pr_debug("%s: Got ASYNC message\n",
-				 sc_adapter[card]->devicename);
-			memcpy(mesgdata, &(sc_adapter[card]->async_msg),
-			       sizeof(RspMessage));
-			sc_adapter[card]->want_async_messages = 0;
-			return 0;
-		}
-
-		tries++;
-	}
-
-	pr_debug("%s: SAR message timeout\n", sc_adapter[card]->devicename);
-	sc_adapter[card]->want_async_messages = 0;
-	return -ETIME;
-}
diff --git a/drivers/isdn/sc/message.h b/drivers/isdn/sc/message.h
deleted file mode 100644
index 5e6f4a5c15f8..000000000000
--- a/drivers/isdn/sc/message.h
+++ /dev/null
@@ -1,245 +0,0 @@
-/* $Id: message.h,v 1.1.10.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * structures, macros and defines useful for sending
- * messages to the adapter
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-/*
- * Board message macros, defines and structures
- */
-
-#ifndef MESSAGE_H
-#define MESSAGE_H
-
-#define MAX_MESSAGES		32	/* Maximum messages that can be
-					   queued */
-#define MSG_DATA_LEN		48	/* Maximum size of message payload */
-#define MSG_LEN			64	/* Size of a message */
-#define CMPID			0	/* Loader message process ID */
-#define CEPID			64	/* Firmware message process ID */
-
-/*
- * Macro to determine if a message is a loader message
- */
-#define IS_CM_MESSAGE(mesg, tx, cx, dx)		\
-	((mesg.type == cmRspType##tx)		\
-	 && (mesg.class == cmRspClass##cx)	\
-	 && (mesg.code == cmRsp##dx))
-
-/*
- * Macro to determine if a message is a firmware message
- */
-#define IS_CE_MESSAGE(mesg, tx, cx, dx)		\
-	((mesg.type == ceRspType##tx)		\
-	 && (mesg.class == ceRspClass##cx)	\
-	 && (mesg.code == ceRsp##tx##dx))
-
-/*
- * Loader Request and Response Messages
- */
-
-/* message types */
-#define cmReqType1			1
-#define cmReqType2			2
-#define cmRspType0			0
-#define cmRspType1			1
-#define cmRspType2			2
-#define cmRspType5			5
-
-/* message classes */
-#define cmReqClass0			0
-#define cmRspClass0			0
-
-/* message codes */
-#define cmReqHWConfig		1			/* 1,0,1 */
-#define cmReqMsgLpbk		2			/* 1,0,2 */
-#define cmReqVersion		3			/* 1,0,3 */
-#define cmReqLoadProc		1			/* 2,0,1 */
-#define cmReqStartProc		2			/* 2,0,2 */
-#define cmReqReadMem		6			/* 2,0,6 */
-#define cmRspHWConfig		cmReqHWConfig
-#define	cmRspMsgLpbk		cmReqMsgLpbk
-#define cmRspVersion		cmReqVersion
-#define cmRspLoadProc		cmReqLoadProc
-#define cmRspStartProc		cmReqStartProc
-#define	cmRspReadMem		cmReqReadMem
-#define cmRspMiscEngineUp	1			/* 5,0,1 */
-#define cmRspInvalid		0			/* 0,0,0 */
-
-
-/*
- * Firmware Request and Response Messages
- */
-
-/* message types */
-#define ceReqTypePhy		1
-#define ceReqTypeLnk		2
-#define ceReqTypeCall		3
-#define ceReqTypeStat		1
-#define ceRspTypeErr		0
-#define	ceRspTypePhy		ceReqTypePhy
-#define ceRspTypeLnk		ceReqTypeLnk
-#define ceRspTypeCall		ceReqTypeCall
-#define ceRspTypeStat		ceReqTypeStat
-
-/* message classes */
-#define ceReqClass0		0
-#define ceReqClass1		1
-#define ceReqClass2		2
-#define ceReqClass3		3
-#define ceRspClass0		ceReqClass0
-#define ceRspClass1		ceReqClass1
-#define ceRspClass2		ceReqClass2
-#define ceRspClass3		ceReqClass3
-
-/* message codes  (B) = BRI only, (P) = PRI only, (V) = POTS only */
-#define ceReqPhyProcInfo	1			/* 1,0,1 */
-#define ceReqPhyConnect		1			/* 1,1,1 */
-#define ceReqPhyDisconnect	2			/* 1,1,2 */
-#define ceReqPhySetParams	3			/* 1,1,3 (P) */
-#define ceReqPhyGetParams	4			/* 1,1,4 (P) */
-#define ceReqPhyStatus		1			/* 1,2,1 */
-#define ceReqPhyAcfaStatus	2			/* 1,2,2 (P) */
-#define ceReqPhyChCallState	3			/* 1,2,3 (P) */
-#define ceReqPhyChServState	4			/* 1,2,4 (P) */
-#define ceReqPhyRLoopBack	1			/* 1,3,1 */
-#define ceRspPhyProcInfo	ceReqPhyProcInfo
-#define	ceRspPhyConnect		ceReqPhyConnect
-#define ceRspPhyDisconnect	ceReqPhyDisconnect
-#define ceRspPhySetParams	ceReqPhySetParams
-#define ceRspPhyGetParams	ceReqPhyGetParams
-#define ceRspPhyStatus		ceReqPhyStatus
-#define ceRspPhyAcfaStatus	ceReqPhyAcfaStatus
-#define ceRspPhyChCallState	ceReqPhyChCallState
-#define ceRspPhyChServState	ceReqPhyChServState
-#define ceRspPhyRLoopBack	ceReqphyRLoopBack
-#define ceReqLnkSetParam	1			/* 2,0,1 */
-#define ceReqLnkGetParam	2			/* 2,0,2 */
-#define ceReqLnkGetStats	3			/* 2,0,3 */
-#define ceReqLnkWrite		1			/* 2,1,1 */
-#define ceReqLnkRead		2			/* 2,1,2 */
-#define ceReqLnkFlush		3			/* 2,1,3 */
-#define ceReqLnkWrBufTrc	4			/* 2,1,4 */
-#define ceReqLnkRdBufTrc	5			/* 2,1,5 */
-#define ceRspLnkSetParam	ceReqLnkSetParam
-#define ceRspLnkGetParam	ceReqLnkGetParam
-#define ceRspLnkGetStats	ceReqLnkGetStats
-#define ceRspLnkWrite		ceReqLnkWrite
-#define ceRspLnkRead		ceReqLnkRead
-#define ceRspLnkFlush		ceReqLnkFlush
-#define ceRspLnkWrBufTrc	ceReqLnkWrBufTrc
-#define ceRspLnkRdBufTrc	ceReqLnkRdBufTrc
-#define ceReqCallSetSwitchType	1			/* 3,0,1 */
-#define ceReqCallGetSwitchType	2			/* 3,0,2 */
-#define ceReqCallSetFrameFormat	3			/* 3,0,3 */
-#define ceReqCallGetFrameFormat	4			/* 3,0,4 */
-#define ceReqCallSetCallType	5			/* 3,0,5 */
-#define ceReqCallGetCallType	6			/* 3,0,6 */
-#define ceReqCallSetSPID	7			/* 3,0,7 (!P) */
-#define ceReqCallGetSPID	8			/* 3,0,8 (!P) */
-#define ceReqCallSetMyNumber	9			/* 3,0,9 (!P) */
-#define ceReqCallGetMyNumber	10			/* 3,0,10 (!P) */
-#define	ceRspCallSetSwitchType	ceReqCallSetSwitchType
-#define ceRspCallGetSwitchType	ceReqCallSetSwitchType
-#define ceRspCallSetFrameFormat	ceReqCallSetFrameFormat
-#define ceRspCallGetFrameFormat	ceReqCallGetFrameFormat
-#define ceRspCallSetCallType	ceReqCallSetCallType
-#define ceRspCallGetCallType	ceReqCallGetCallType
-#define ceRspCallSetSPID	ceReqCallSetSPID
-#define ceRspCallGetSPID	ceReqCallGetSPID
-#define ceRspCallSetMyNumber	ceReqCallSetMyNumber
-#define ceRspCallGetMyNumber	ceReqCallGetMyNumber
-#define ceRspStatAcfaStatus	2
-#define ceRspStat
-#define ceRspErrError		0			/* 0,0,0 */
-
-/*
- * Call Types
- */
-#define CALLTYPE_64K		0
-#define CALLTYPE_56K		1
-#define CALLTYPE_SPEECH		2
-#define CALLTYPE_31KHZ		3
-
-/*
- * Link Level data contains a pointer to and the length of
- * a buffer in shared RAM. Used by LnkRead and LnkWrite message
- * types. Part of RspMsgStruct and ReqMsgStruct.
- */
-typedef struct {
-	unsigned long buff_offset;
-	unsigned short msg_len;
-} LLData;
-
-
-/*
- * Message payload template for an HWConfig message
- */
-typedef struct {
-	char st_u_sense;
-	char powr_sense;
-	char sply_sense;
-	unsigned char asic_id;
-	long ram_size;
-	char serial_no[13];
-	char part_no[13];
-	char rev_no[2];
-} HWConfig_pl;
-
-/*
- * A Message
- */
-struct message {
-	unsigned char sequence_no;
-	unsigned char process_id;
-	unsigned char time_stamp;
-	unsigned char cmd_sequence_no;	/* Rsp messages only */
-	unsigned char reserved1[3];
-	unsigned char msg_byte_cnt;
-	unsigned char type;
-	unsigned char class;
-	unsigned char code;
-	unsigned char phy_link_no;
-	unsigned char rsp_status;	/* Rsp messages only */
-	unsigned char reseved2[3];
-	union {
-		unsigned char byte_array[MSG_DATA_LEN];
-		LLData response;
-		HWConfig_pl HWCresponse;
-	} msg_data;
-};
-
-typedef struct message ReqMessage;	/* Request message */
-typedef struct message RspMessage;	/* Response message */
-
-/*
- * The first 5010 bytes of shared memory contain the message queues,
- * indexes and other data. This structure is its template
- */
-typedef struct {
-	volatile ReqMessage req_queue[MAX_MESSAGES];
-	volatile RspMessage rsp_queue[MAX_MESSAGES];
-	volatile unsigned char req_head;
-	volatile unsigned char req_tail;
-	volatile unsigned char rsp_head;
-	volatile unsigned char rsp_tail;
-	volatile unsigned long signature;
-	volatile unsigned long trace_enable;
-	volatile unsigned char reserved[4];
-} DualPortMemory;
-
-#endif
diff --git a/drivers/isdn/sc/packet.c b/drivers/isdn/sc/packet.c
deleted file mode 100644
index 2446957085e0..000000000000
--- a/drivers/isdn/sc/packet.c
+++ /dev/null
@@ -1,204 +0,0 @@
-/* $Id: packet.c,v 1.5.8.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-
-int sndpkt(int devId, int channel, int ack, struct sk_buff *data)
-{
-	LLData	ReqLnkWrite;
-	int status;
-	int card;
-	unsigned long len;
-
-	card = get_card_from_id(devId);
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	pr_debug("%s: sndpkt: frst = 0x%lx nxt = %d  f = %d n = %d\n",
-		 sc_adapter[card]->devicename,
-		 sc_adapter[card]->channel[channel].first_sendbuf,
-		 sc_adapter[card]->channel[channel].next_sendbuf,
-		 sc_adapter[card]->channel[channel].free_sendbufs,
-		 sc_adapter[card]->channel[channel].num_sendbufs);
-
-	if (!sc_adapter[card]->channel[channel].free_sendbufs) {
-		pr_debug("%s: out of TX buffers\n",
-			 sc_adapter[card]->devicename);
-		return -EINVAL;
-	}
-
-	if (data->len > BUFFER_SIZE) {
-		pr_debug("%s: data overflows buffer size (data > buffer)\n",
-			 sc_adapter[card]->devicename);
-		return -EINVAL;
-	}
-
-	ReqLnkWrite.buff_offset = sc_adapter[card]->channel[channel].next_sendbuf *
-		BUFFER_SIZE + sc_adapter[card]->channel[channel].first_sendbuf;
-	ReqLnkWrite.msg_len = data->len; /* sk_buff size */
-	pr_debug("%s: writing %d bytes to buffer offset 0x%lx\n",
-		 sc_adapter[card]->devicename,
-		 ReqLnkWrite.msg_len, ReqLnkWrite.buff_offset);
-	memcpy_toshmem(card, (char *)ReqLnkWrite.buff_offset, data->data, ReqLnkWrite.msg_len);
-
-	/*
-	 * sendmessage
-	 */
-	pr_debug("%s: sndpkt size=%d, buf_offset=0x%lx buf_indx=%d\n",
-		 sc_adapter[card]->devicename,
-		 ReqLnkWrite.msg_len, ReqLnkWrite.buff_offset,
-		 sc_adapter[card]->channel[channel].next_sendbuf);
-
-	status = sendmessage(card, CEPID, ceReqTypeLnk, ceReqClass1, ceReqLnkWrite,
-			     channel + 1, sizeof(LLData), (unsigned int *)&ReqLnkWrite);
-	len = data->len;
-	if (status) {
-		pr_debug("%s: failed to send packet, status = %d\n",
-			 sc_adapter[card]->devicename, status);
-		return -1;
-	}
-	else {
-		sc_adapter[card]->channel[channel].free_sendbufs--;
-		sc_adapter[card]->channel[channel].next_sendbuf =
-			++sc_adapter[card]->channel[channel].next_sendbuf ==
-			sc_adapter[card]->channel[channel].num_sendbufs ? 0 :
-			sc_adapter[card]->channel[channel].next_sendbuf;
-		pr_debug("%s: packet sent successfully\n", sc_adapter[card]->devicename);
-		dev_kfree_skb(data);
-		indicate_status(card, ISDN_STAT_BSENT, channel, (char *)&len);
-	}
-	return len;
-}
-
-void rcvpkt(int card, RspMessage *rcvmsg)
-{
-	LLData newll;
-	struct sk_buff *skb;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("invalid param: %d is not a valid card id\n", card);
-		return;
-	}
-
-	switch (rcvmsg->rsp_status) {
-	case 0x01:
-	case 0x02:
-	case 0x70:
-		pr_debug("%s: error status code: 0x%x\n",
-			 sc_adapter[card]->devicename, rcvmsg->rsp_status);
-		return;
-	case 0x00:
-		if (!(skb = dev_alloc_skb(rcvmsg->msg_data.response.msg_len))) {
-			printk(KERN_WARNING "%s: rcvpkt out of memory, dropping packet\n",
-			       sc_adapter[card]->devicename);
-			return;
-		}
-		skb_put(skb, rcvmsg->msg_data.response.msg_len);
-		pr_debug("%s: getting data from offset: 0x%lx\n",
-			 sc_adapter[card]->devicename,
-			 rcvmsg->msg_data.response.buff_offset);
-		memcpy_fromshmem(card,
-				 skb_put(skb, rcvmsg->msg_data.response.msg_len),
-				 (char *)rcvmsg->msg_data.response.buff_offset,
-				 rcvmsg->msg_data.response.msg_len);
-		sc_adapter[card]->card->rcvcallb_skb(sc_adapter[card]->driverId,
-						     rcvmsg->phy_link_no - 1, skb);
-
-	case 0x03:
-		/*
-		 * Recycle the buffer
-		 */
-		pr_debug("%s: buffer size : %d\n",
-			 sc_adapter[card]->devicename, BUFFER_SIZE);
-/*		memset_shmem(card, rcvmsg->msg_data.response.buff_offset, 0, BUFFER_SIZE); */
-		newll.buff_offset = rcvmsg->msg_data.response.buff_offset;
-		newll.msg_len = BUFFER_SIZE;
-		pr_debug("%s: recycled buffer at offset 0x%lx size %d\n",
-			 sc_adapter[card]->devicename,
-			 newll.buff_offset, newll.msg_len);
-		sendmessage(card, CEPID, ceReqTypeLnk, ceReqClass1, ceReqLnkRead,
-			    rcvmsg->phy_link_no, sizeof(LLData), (unsigned int *)&newll);
-	}
-
-}
-
-int setup_buffers(int card, int c)
-{
-	unsigned int nBuffers, i, cBase;
-	unsigned int buffer_size;
-	LLData	RcvBuffOffset;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("invalid param: %d is not a valid card id\n", card);
-		return -ENODEV;
-	}
-
-	/*
-	 * Calculate the buffer offsets (send/recv/send/recv)
-	 */
-	pr_debug("%s: setting up channel buffer space in shared RAM\n",
-		 sc_adapter[card]->devicename);
-	buffer_size = BUFFER_SIZE;
-	nBuffers = ((sc_adapter[card]->ramsize - BUFFER_BASE) / buffer_size) / 2;
-	nBuffers = nBuffers > BUFFERS_MAX ? BUFFERS_MAX : nBuffers;
-	pr_debug("%s: calculating buffer space: %d buffers, %d big\n",
-		 sc_adapter[card]->devicename,
-		 nBuffers, buffer_size);
-	if (nBuffers < 2) {
-		pr_debug("%s: not enough buffer space\n",
-			 sc_adapter[card]->devicename);
-		return -1;
-	}
-	cBase = (nBuffers * buffer_size) * (c - 1);
-	pr_debug("%s: channel buffer offset from shared RAM: 0x%x\n",
-		 sc_adapter[card]->devicename, cBase);
-	sc_adapter[card]->channel[c - 1].first_sendbuf = BUFFER_BASE + cBase;
-	sc_adapter[card]->channel[c - 1].num_sendbufs = nBuffers / 2;
-	sc_adapter[card]->channel[c - 1].free_sendbufs = nBuffers / 2;
-	sc_adapter[card]->channel[c - 1].next_sendbuf = 0;
-	pr_debug("%s: send buffer setup complete: first=0x%lx n=%d f=%d, nxt=%d\n",
-		 sc_adapter[card]->devicename,
-		 sc_adapter[card]->channel[c - 1].first_sendbuf,
-		 sc_adapter[card]->channel[c - 1].num_sendbufs,
-		 sc_adapter[card]->channel[c - 1].free_sendbufs,
-		 sc_adapter[card]->channel[c - 1].next_sendbuf);
-
-	/*
-	 * Prep the receive buffers
-	 */
-	pr_debug("%s: adding %d RecvBuffers:\n",
-		 sc_adapter[card]->devicename, nBuffers / 2);
-	for (i = 0; i < nBuffers / 2; i++) {
-		RcvBuffOffset.buff_offset =
-			((sc_adapter[card]->channel[c - 1].first_sendbuf +
-			  (nBuffers / 2) * buffer_size) + (buffer_size * i));
-		RcvBuffOffset.msg_len = buffer_size;
-		pr_debug("%s: adding RcvBuffer #%d offset=0x%lx sz=%d bufsz:%d\n",
-			 sc_adapter[card]->devicename,
-			 i + 1, RcvBuffOffset.buff_offset,
-			 RcvBuffOffset.msg_len, buffer_size);
-		sendmessage(card, CEPID, ceReqTypeLnk, ceReqClass1, ceReqLnkRead,
-			    c, sizeof(LLData), (unsigned int *)&RcvBuffOffset);
-	}
-	return 0;
-}
diff --git a/drivers/isdn/sc/scioc.h b/drivers/isdn/sc/scioc.h
deleted file mode 100644
index a50e143779e7..000000000000
--- a/drivers/isdn/sc/scioc.h
+++ /dev/null
@@ -1,110 +0,0 @@
-#ifndef __ISDN_SC_SCIOC_H__
-#define __ISDN_SC_SCIOC_H__
-
-/*
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- */
-
-/*
- * IOCTL Command Codes
- */
-#define SCIOCLOAD	0x01	/* Load a firmware record */
-#define SCIOCRESET	0x02	/* Perform hard reset */
-#define SCIOCDEBUG	0x03	/* Set debug level */
-#define SCIOCREV	0x04	/* Get driver revision(s) */
-#define SCIOCSTART	0x05	/* Start the firmware */
-#define SCIOCGETSWITCH	0x06	/* Get switch type */
-#define SCIOCSETSWITCH	0x07	/* Set switch type */
-#define SCIOCGETSPID	0x08	/* Get channel SPID */
-#define SCIOCSETSPID	0x09	/* Set channel SPID */
-#define SCIOCGETDN	0x0A	/* Get channel DN */
-#define SCIOCSETDN	0x0B	/* Set channel DN */
-#define SCIOCTRACE	0x0C	/* Toggle trace mode */
-#define SCIOCSTAT	0x0D	/* Get line status */
-#define SCIOCGETSPEED	0x0E	/* Set channel speed */
-#define SCIOCSETSPEED	0x0F	/* Set channel speed */
-#define SCIOCLOOPTST	0x10	/* Perform loopback test */
-
-typedef struct {
-	int device;
-	int channel;
-	unsigned long command;
-	void __user *dataptr;
-} scs_ioctl;
-
-/* Size of strings */
-#define SCIOC_SPIDSIZE		49
-#define SCIOC_DNSIZE		SCIOC_SPIDSIZE
-#define SCIOC_REVSIZE		SCIOC_SPIDSIZE
-#define SCIOC_SRECSIZE		49
-
-typedef struct {
-	unsigned long tx_good;
-	unsigned long tx_bad;
-	unsigned long rx_good;
-	unsigned long rx_bad;
-} ChLinkStats;
-
-typedef struct {
-	char spid[49];
-	char dn[49];
-	char call_type;
-	char phy_stat;
-	ChLinkStats link_stats;
-} BRIStat;
-
-typedef BRIStat POTStat;
-
-typedef struct {
-	char call_type;
-	char call_state;
-	char serv_state;
-	char phy_stat;
-	ChLinkStats link_stats;
-} PRIStat;
-
-typedef char PRIInfo;
-typedef char BRIInfo;
-typedef char POTInfo;
-
-
-typedef struct {
-	char acfa_nos;
-	char acfa_ais;
-	char acfa_los;
-	char acfa_rra;
-	char acfa_slpp;
-	char acfa_slpn;
-	char acfa_fsrf;
-} ACFAStat;
-
-typedef struct {
-	unsigned char modelid;
-	char serial_no[13];
-	char part_no[13];
-	char load_ver[11];
-	char proc_ver[11];
-	int iobase;
-	long rambase;
-	char irq;
-	long ramsize;
-	char interface;
-	char switch_type;
-	char l1_status;
-	char l2_status;
-	ChLinkStats dch_stats;
-	ACFAStat AcfaStats;
-	union {
-		PRIStat pristats[23];
-		BRIStat bristats[2];
-		POTStat potsstats[2];
-	} status;
-	union {
-		PRIInfo priinfo;
-		BRIInfo briinfo;
-		POTInfo potsinfo;
-	} info;
-} boardInfo;
-
-#endif  /*  __ISDN_SC_SCIOC_H__  */
diff --git a/drivers/isdn/sc/shmem.c b/drivers/isdn/sc/shmem.c
deleted file mode 100644
index d24506ceb6e8..000000000000
--- a/drivers/isdn/sc/shmem.c
+++ /dev/null
@@ -1,138 +0,0 @@
-/* $Id: shmem.c,v 1.2.10.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * Card functions implementing ISDN4Linux functionality
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include "includes.h"		/* This must be first */
-#include "hardware.h"
-#include "card.h"
-
-/*
- *
- */
-void memcpy_toshmem(int card, void *dest, const void *src, size_t n)
-{
-	unsigned long flags;
-	unsigned char ch;
-	unsigned long dest_rem = ((unsigned long) dest) % 0x4000;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return;
-	}
-
-	if (n > SRAM_PAGESIZE)
-		return;
-
-	/*
-	 * determine the page to load from the address
-	 */
-	ch = (unsigned long) dest / SRAM_PAGESIZE;
-	pr_debug("%s: loaded page %d\n", sc_adapter[card]->devicename, ch);
-	/*
-	 * Block interrupts and load the page
-	 */
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-
-	outb(((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80,
-	     sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport]);
-	memcpy_toio((void __iomem *)(sc_adapter[card]->rambase + dest_rem), src, n);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-	pr_debug("%s: set page to %#x\n", sc_adapter[card]->devicename,
-		 ((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80);
-	pr_debug("%s: copying %zu bytes from %#lx to %#lx\n",
-		 sc_adapter[card]->devicename, n,
-		 (unsigned long) src,
-		 sc_adapter[card]->rambase + ((unsigned long) dest % 0x4000));
-}
-
-/*
- * Reverse of above
- */
-void memcpy_fromshmem(int card, void *dest, const void *src, size_t n)
-{
-	unsigned long flags;
-	unsigned char ch;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return;
-	}
-
-	if (n > SRAM_PAGESIZE) {
-		return;
-	}
-
-	/*
-	 * determine the page to load from the address
-	 */
-	ch = (unsigned long) src / SRAM_PAGESIZE;
-	pr_debug("%s: loaded page %d\n", sc_adapter[card]->devicename, ch);
-
-
-	/*
-	 * Block interrupts and load the page
-	 */
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-
-	outb(((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80,
-	     sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport]);
-	memcpy_fromio(dest, (void *)(sc_adapter[card]->rambase +
-				     ((unsigned long) src % 0x4000)), n);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-	pr_debug("%s: set page to %#x\n", sc_adapter[card]->devicename,
-		 ((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80);
-/*	pr_debug("%s: copying %d bytes from %#x to %#x\n",
-	sc_adapter[card]->devicename, n,
-	sc_adapter[card]->rambase + ((unsigned long) src %0x4000), (unsigned long) dest); */
-}
-
-#if 0
-void memset_shmem(int card, void *dest, int c, size_t n)
-{
-	unsigned long flags;
-	unsigned char ch;
-
-	if (!IS_VALID_CARD(card)) {
-		pr_debug("Invalid param: %d is not a valid card id\n", card);
-		return;
-	}
-
-	if (n > SRAM_PAGESIZE) {
-		return;
-	}
-
-	/*
-	 * determine the page to load from the address
-	 */
-	ch = (unsigned long) dest / SRAM_PAGESIZE;
-	pr_debug("%s: loaded page %d\n", sc_adapter[card]->devicename, ch);
-
-	/*
-	 * Block interrupts and load the page
-	 */
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-
-	outb(((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80,
-	     sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport]);
-	memset_io(sc_adapter[card]->rambase +
-		  ((unsigned long) dest % 0x4000), c, n);
-	pr_debug("%s: set page to %#x\n", sc_adapter[card]->devicename,
-		 ((sc_adapter[card]->shmem_magic + ch * SRAM_PAGESIZE) >> 14) | 0x80);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-}
-#endif  /*  0  */
diff --git a/drivers/isdn/sc/timer.c b/drivers/isdn/sc/timer.c
deleted file mode 100644
index 6fbac2230d7e..000000000000
--- a/drivers/isdn/sc/timer.c
+++ /dev/null
@@ -1,122 +0,0 @@
-/* $Id: timer.c,v 1.3.6.1 2001/09/23 22:24:59 kai Exp $
- *
- * Copyright (C) 1996  SpellCaster Telecommunications Inc.
- *
- * This software may be used and distributed according to the terms
- * of the GNU General Public License, incorporated herein by reference.
- *
- * For more information, please contact gpl-info@spellcast.com or write:
- *
- *     SpellCaster Telecommunications Inc.
- *     5621 Finch Avenue East, Unit #3
- *     Scarborough, Ontario  Canada
- *     M1B 2T9
- *     +1 (416) 297-8565
- *     +1 (416) 297-6433 Facsimile
- */
-
-#include "includes.h"
-#include "hardware.h"
-#include "message.h"
-#include "card.h"
-
-
-/*
- * Write the proper values into the I/O ports following a reset
- */
-static void setup_ports(int card)
-{
-
-	outb((sc_adapter[card]->rambase >> 12), sc_adapter[card]->ioport[EXP_BASE]);
-
-	/* And the IRQ */
-	outb((sc_adapter[card]->interrupt | 0x80),
-	     sc_adapter[card]->ioport[IRQ_SELECT]);
-}
-
-/*
- * Timed function to check the status of a previous reset
- * Must be very fast as this function runs in the context of
- * an interrupt handler.
- *
- * Setup the ioports for the board that were cleared by the reset.
- * Then, check to see if the signate has been set. Next, set the
- * signature to a known value and issue a startproc if needed.
- */
-void sc_check_reset(unsigned long data)
-{
-	unsigned long flags;
-	unsigned long sig;
-	int card = (unsigned int) data;
-
-	pr_debug("%s: check_timer timer called\n",
-		 sc_adapter[card]->devicename);
-
-	/* Setup the io ports */
-	setup_ports(card);
-
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-	outb(sc_adapter[card]->ioport[sc_adapter[card]->shmem_pgport],
-	     (sc_adapter[card]->shmem_magic >> 14) | 0x80);
-	sig = (unsigned long) *((unsigned long *)(sc_adapter[card]->rambase + SIG_OFFSET));
-
-	/* check the signature */
-	if (sig == SIGNATURE) {
-		flushreadfifo(card);
-		spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-		/* See if we need to do a startproc */
-		if (sc_adapter[card]->StartOnReset)
-			startproc(card);
-	} else  {
-		pr_debug("%s: No signature yet, waiting another %lu jiffies.\n",
-			 sc_adapter[card]->devicename, CHECKRESET_TIME);
-		mod_timer(&sc_adapter[card]->reset_timer, jiffies + CHECKRESET_TIME);
-		spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-	}
-}
-
-/*
- * Timed function to check the status of a previous reset
- * Must be very fast as this function runs in the context of
- * an interrupt handler.
- *
- * Send check sc_adapter->phystat to see if the channels are up
- * If they are, tell ISDN4Linux that the board is up. If not,
- * tell IADN4Linux that it is up. Always reset the timer to
- * fire again (endless loop).
- */
-void check_phystat(unsigned long data)
-{
-	unsigned long flags;
-	int card = (unsigned int) data;
-
-	pr_debug("%s: Checking status...\n", sc_adapter[card]->devicename);
-	/*
-	 * check the results of the last PhyStat and change only if
-	 * has changed drastically
-	 */
-	if (sc_adapter[card]->nphystat && !sc_adapter[card]->phystat) {   /* All is well */
-		pr_debug("PhyStat transition to RUN\n");
-		pr_info("%s: Switch contacted, transmitter enabled\n",
-			sc_adapter[card]->devicename);
-		indicate_status(card, ISDN_STAT_RUN, 0, NULL);
-	}
-	else if (!sc_adapter[card]->nphystat && sc_adapter[card]->phystat) {   /* All is not well */
-		pr_debug("PhyStat transition to STOP\n");
-		pr_info("%s: Switch connection lost, transmitter disabled\n",
-			sc_adapter[card]->devicename);
-
-		indicate_status(card, ISDN_STAT_STOP, 0, NULL);
-	}
-
-	sc_adapter[card]->phystat = sc_adapter[card]->nphystat;
-
-	/* Reinitialize the timer */
-	spin_lock_irqsave(&sc_adapter[card]->lock, flags);
-	mod_timer(&sc_adapter[card]->stat_timer, jiffies + CHECKSTAT_TIME);
-	spin_unlock_irqrestore(&sc_adapter[card]->lock, flags);
-
-	/* Send a new cePhyStatus message */
-	sendmessage(card, CEPID, ceReqTypePhy, ceReqClass2,
-		    ceReqPhyStatus, 0, 0, NULL);
-}
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index b1ab8bdf8251..7f940c24a16b 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -52,6 +52,7 @@ config LEDS_AAT1290
 config LEDS_BCM6328
 	tristate "LED Support for Broadcom BCM6328"
 	depends on LEDS_CLASS
+	depends on HAS_IOMEM
 	depends on OF
 	help
 	  This option enables support for LEDs connected to the BCM6328
@@ -60,6 +61,7 @@ config LEDS_BCM6328
 config LEDS_BCM6358
 	tristate "LED Support for Broadcom BCM6358"
 	depends on LEDS_CLASS
+	depends on HAS_IOMEM
 	depends on OF
 	help
 	  This option enables support for LEDs connected to the BCM6358
diff --git a/drivers/leds/led-class-flash.c b/drivers/leds/led-class-flash.c
index 3b2573411a37..cf398275a53c 100644
--- a/drivers/leds/led-class-flash.c
+++ b/drivers/leds/led-class-flash.c
@@ -108,7 +108,7 @@ static ssize_t flash_strobe_store(struct device *dev,
 	if (ret)
 		goto unlock;
 
-	if (state < 0 || state > 1) {
+	if (state > 1) {
 		ret = -EINVAL;
 		goto unlock;
 	}
@@ -298,7 +298,7 @@ int led_classdev_flash_register(struct device *parent,
 	led_cdev = &fled_cdev->led_cdev;
 
 	if (led_cdev->flags & LED_DEV_CAP_FLASH) {
-		if (!led_cdev->brightness_set_sync)
+		if (!led_cdev->brightness_set_blocking)
 			return -EINVAL;
 
 		ops = fled_cdev->ops;
@@ -316,10 +316,6 @@ int led_classdev_flash_register(struct device *parent,
 	if (ret < 0)
 		return ret;
 
-	/* Setting a torch brightness needs to have immediate effect */
-	led_cdev->flags &= ~SET_BRIGHTNESS_ASYNC;
-	led_cdev->flags |= SET_BRIGHTNESS_SYNC;
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(led_classdev_flash_register);
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 7385f98dd54b..14139c337312 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -109,7 +109,7 @@ static const struct attribute_group *led_groups[] = {
 void led_classdev_suspend(struct led_classdev *led_cdev)
 {
 	led_cdev->flags |= LED_SUSPENDED;
-	led_cdev->brightness_set(led_cdev, 0);
+	led_set_brightness_nopm(led_cdev, 0);
 }
 EXPORT_SYMBOL_GPL(led_classdev_suspend);
 
@@ -119,7 +119,7 @@ EXPORT_SYMBOL_GPL(led_classdev_suspend);
  */
 void led_classdev_resume(struct led_classdev *led_cdev)
 {
-	led_cdev->brightness_set(led_cdev, led_cdev->brightness);
+	led_set_brightness_nopm(led_cdev, led_cdev->brightness);
 
 	if (led_cdev->flash_resume)
 		led_cdev->flash_resume(led_cdev);
@@ -215,8 +215,6 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 	if (!led_cdev->max_brightness)
 		led_cdev->max_brightness = LED_FULL;
 
-	led_cdev->flags |= SET_BRIGHTNESS_ASYNC;
-
 	led_update_brightness(led_cdev);
 
 	led_init_core(led_cdev);
@@ -247,12 +245,13 @@ void led_classdev_unregister(struct led_classdev *led_cdev)
 	up_write(&led_cdev->trigger_lock);
 #endif
 
-	cancel_work_sync(&led_cdev->set_brightness_work);
-
 	/* Stop blinking */
 	led_stop_software_blink(led_cdev);
+
 	led_set_brightness(led_cdev, LED_OFF);
 
+	flush_work(&led_cdev->set_brightness_work);
+
 	device_unregister(led_cdev->dev);
 
 	down_write(&leds_list_lock);
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index c1c3af089634..19e1e60dfaa3 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -32,7 +32,7 @@ static void led_timer_function(unsigned long data)
 	unsigned long delay;
 
 	if (!led_cdev->blink_delay_on || !led_cdev->blink_delay_off) {
-		led_set_brightness_async(led_cdev, LED_OFF);
+		led_set_brightness_nosleep(led_cdev, LED_OFF);
 		return;
 	}
 
@@ -44,23 +44,23 @@ static void led_timer_function(unsigned long data)
 	brightness = led_get_brightness(led_cdev);
 	if (!brightness) {
 		/* Time to switch the LED on. */
-		if (led_cdev->delayed_set_value) {
-			led_cdev->blink_brightness =
-					led_cdev->delayed_set_value;
-			led_cdev->delayed_set_value = 0;
-		}
 		brightness = led_cdev->blink_brightness;
 		delay = led_cdev->blink_delay_on;
 	} else {
 		/* Store the current brightness value to be able
 		 * to restore it when the delay_off period is over.
+		 * Do it only if there is no pending blink brightness
+		 * change, to avoid overwriting the new value.
 		 */
-		led_cdev->blink_brightness = brightness;
+		if (!(led_cdev->flags & LED_BLINK_BRIGHTNESS_CHANGE))
+			led_cdev->blink_brightness = brightness;
+		else
+			led_cdev->flags &= ~LED_BLINK_BRIGHTNESS_CHANGE;
 		brightness = LED_OFF;
 		delay = led_cdev->blink_delay_off;
 	}
 
-	led_set_brightness_async(led_cdev, brightness);
+	led_set_brightness_nosleep(led_cdev, brightness);
 
 	/* Return in next iteration if led is in one-shot mode and we are in
 	 * the final blink state so that the led is toggled each delay_on +
@@ -83,10 +83,24 @@ static void set_brightness_delayed(struct work_struct *ws)
 {
 	struct led_classdev *led_cdev =
 		container_of(ws, struct led_classdev, set_brightness_work);
+	int ret = 0;
 
-	led_stop_software_blink(led_cdev);
+	if (led_cdev->flags & LED_BLINK_DISABLE) {
+		led_cdev->delayed_set_value = LED_OFF;
+		led_stop_software_blink(led_cdev);
+		led_cdev->flags &= ~LED_BLINK_DISABLE;
+	}
 
-	led_set_brightness_async(led_cdev, led_cdev->delayed_set_value);
+	if (led_cdev->brightness_set)
+		led_cdev->brightness_set(led_cdev, led_cdev->delayed_set_value);
+	else if (led_cdev->brightness_set_blocking)
+		ret = led_cdev->brightness_set_blocking(led_cdev,
+						led_cdev->delayed_set_value);
+	else
+		ret = -ENOTSUPP;
+	if (ret < 0)
+		dev_err(led_cdev->dev,
+			"Setting an LED's brightness failed (%d)\n", ret);
 }
 
 static void led_set_software_blink(struct led_classdev *led_cdev,
@@ -106,13 +120,14 @@ static void led_set_software_blink(struct led_classdev *led_cdev,
 
 	/* never on - just set to off */
 	if (!delay_on) {
-		led_set_brightness_async(led_cdev, LED_OFF);
+		led_set_brightness_nosleep(led_cdev, LED_OFF);
 		return;
 	}
 
 	/* never off - just set to brightness */
 	if (!delay_off) {
-		led_set_brightness_async(led_cdev, led_cdev->blink_brightness);
+		led_set_brightness_nosleep(led_cdev,
+					   led_cdev->blink_brightness);
 		return;
 	}
 
@@ -156,7 +171,7 @@ void led_blink_set(struct led_classdev *led_cdev,
 
 	led_blink_setup(led_cdev, delay_on, delay_off);
 }
-EXPORT_SYMBOL(led_blink_set);
+EXPORT_SYMBOL_GPL(led_blink_set);
 
 void led_blink_set_oneshot(struct led_classdev *led_cdev,
 			   unsigned long *delay_on,
@@ -177,7 +192,7 @@ void led_blink_set_oneshot(struct led_classdev *led_cdev,
 
 	led_blink_setup(led_cdev, delay_on, delay_off);
 }
-EXPORT_SYMBOL(led_blink_set_oneshot);
+EXPORT_SYMBOL_GPL(led_blink_set_oneshot);
 
 void led_stop_software_blink(struct led_classdev *led_cdev)
 {
@@ -190,29 +205,74 @@ EXPORT_SYMBOL_GPL(led_stop_software_blink);
 void led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness brightness)
 {
-	int ret = 0;
-
-	/* delay brightness if soft-blink is active */
+	/*
+	 * In case blinking is on delay brightness setting
+	 * until the next timer tick.
+	 */
 	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
-		led_cdev->delayed_set_value = brightness;
-		if (brightness == LED_OFF)
+		/*
+		 * If we need to disable soft blinking delegate this to the
+		 * work queue task to avoid problems in case we are called
+		 * from hard irq context.
+		 */
+		if (brightness == LED_OFF) {
+			led_cdev->flags |= LED_BLINK_DISABLE;
 			schedule_work(&led_cdev->set_brightness_work);
+		} else {
+			led_cdev->flags |= LED_BLINK_BRIGHTNESS_CHANGE;
+			led_cdev->blink_brightness = brightness;
+		}
 		return;
 	}
 
-	if (led_cdev->flags & SET_BRIGHTNESS_ASYNC) {
-		led_set_brightness_async(led_cdev, brightness);
+	led_set_brightness_nosleep(led_cdev, brightness);
+}
+EXPORT_SYMBOL_GPL(led_set_brightness);
+
+void led_set_brightness_nopm(struct led_classdev *led_cdev,
+			      enum led_brightness value)
+{
+	/* Use brightness_set op if available, it is guaranteed not to sleep */
+	if (led_cdev->brightness_set) {
+		led_cdev->brightness_set(led_cdev, value);
 		return;
-	} else if (led_cdev->flags & SET_BRIGHTNESS_SYNC)
-		ret = led_set_brightness_sync(led_cdev, brightness);
-	else
-		ret = -EINVAL;
+	}
 
-	if (ret < 0)
-		dev_dbg(led_cdev->dev, "Setting LED brightness failed (%d)\n",
-			ret);
+	/* If brightness setting can sleep, delegate it to a work queue task */
+	led_cdev->delayed_set_value = value;
+	schedule_work(&led_cdev->set_brightness_work);
+}
+EXPORT_SYMBOL_GPL(led_set_brightness_nopm);
+
+void led_set_brightness_nosleep(struct led_classdev *led_cdev,
+				enum led_brightness value)
+{
+	led_cdev->brightness = min(value, led_cdev->max_brightness);
+
+	if (led_cdev->flags & LED_SUSPENDED)
+		return;
+
+	led_set_brightness_nopm(led_cdev, led_cdev->brightness);
+}
+EXPORT_SYMBOL_GPL(led_set_brightness_nosleep);
+
+int led_set_brightness_sync(struct led_classdev *led_cdev,
+			    enum led_brightness value)
+{
+	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off)
+		return -EBUSY;
+
+	led_cdev->brightness = min(value, led_cdev->max_brightness);
+
+	if (led_cdev->flags & LED_SUSPENDED)
+		return 0;
+
+	if (led_cdev->brightness_set_blocking)
+		return led_cdev->brightness_set_blocking(led_cdev,
+							 led_cdev->brightness);
+	return -ENOTSUPP;
 }
-EXPORT_SYMBOL(led_set_brightness);
+EXPORT_SYMBOL_GPL(led_set_brightness_sync);
 
 int led_update_brightness(struct led_classdev *led_cdev)
 {
@@ -228,7 +288,7 @@ int led_update_brightness(struct led_classdev *led_cdev)
 
 	return ret;
 }
-EXPORT_SYMBOL(led_update_brightness);
+EXPORT_SYMBOL_GPL(led_update_brightness);
 
 /* Caller must ensure led_cdev->led_access held */
 void led_sysfs_disable(struct led_classdev *led_cdev)
diff --git a/drivers/leds/led-triggers.c b/drivers/leds/led-triggers.c
index e8b1120f486d..e1e933424ac9 100644
--- a/drivers/leds/led-triggers.c
+++ b/drivers/leds/led-triggers.c
@@ -249,6 +249,34 @@ void led_trigger_unregister(struct led_trigger *trig)
 }
 EXPORT_SYMBOL_GPL(led_trigger_unregister);
 
+static void devm_led_trigger_release(struct device *dev, void *res)
+{
+	led_trigger_unregister(*(struct led_trigger **)res);
+}
+
+int devm_led_trigger_register(struct device *dev,
+			      struct led_trigger *trig)
+{
+	struct led_trigger **dr;
+	int rc;
+
+	dr = devres_alloc(devm_led_trigger_release, sizeof(*dr),
+			  GFP_KERNEL);
+	if (!dr)
+		return -ENOMEM;
+
+	*dr = trig;
+
+	rc = led_trigger_register(trig);
+	if (rc)
+		devres_free(dr);
+	else
+		devres_add(dev, dr);
+
+	return rc;
+}
+EXPORT_SYMBOL_GPL(devm_led_trigger_register);
+
 /* Simple LED Tigger Interface */
 
 void led_trigger_event(struct led_trigger *trig,
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index 7870840e7cc9..1ad4d03a0a3c 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -16,7 +16,6 @@
 #include <linux/i2c.h>
 #include <linux/leds.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <linux/mfd/88pm860x.h>
 #include <linux/module.h>
 
@@ -33,7 +32,6 @@
 struct pm860x_led {
 	struct led_classdev cdev;
 	struct i2c_client *i2c;
-	struct work_struct work;
 	struct pm860x_chip *chip;
 	struct mutex lock;
 	char name[MFD_NAME_SIZE];
@@ -69,17 +67,18 @@ static int led_power_set(struct pm860x_chip *chip, int port, int on)
 	return ret;
 }
 
-static void pm860x_led_work(struct work_struct *work)
+static int pm860x_led_set(struct led_classdev *cdev,
+			   enum led_brightness value)
 {
-
-	struct pm860x_led *led;
+	struct pm860x_led *led = container_of(cdev, struct pm860x_led, cdev);
 	struct pm860x_chip *chip;
 	unsigned char buf[3];
 	int ret;
 
-	led = container_of(work, struct pm860x_led, work);
 	chip = led->chip;
 	mutex_lock(&led->lock);
+	led->brightness = value >> 3;
+
 	if ((led->current_brightness == 0) && led->brightness) {
 		led_power_set(chip, led->port, 1);
 		if (led->iset) {
@@ -112,15 +111,8 @@ static void pm860x_led_work(struct work_struct *work)
 	dev_dbg(chip->dev, "Update LED. (reg:%d, brightness:%d)\n",
 		led->reg_control, led->brightness);
 	mutex_unlock(&led->lock);
-}
 
-static void pm860x_led_set(struct led_classdev *cdev,
-			   enum led_brightness value)
-{
-	struct pm860x_led *data = container_of(cdev, struct pm860x_led, cdev);
-
-	data->brightness = value >> 3;
-	schedule_work(&data->work);
+	return 0;
 }
 
 #ifdef CONFIG_OF
@@ -213,9 +205,8 @@ static int pm860x_led_probe(struct platform_device *pdev)
 
 	data->current_brightness = 0;
 	data->cdev.name = data->name;
-	data->cdev.brightness_set = pm860x_led_set;
+	data->cdev.brightness_set_blocking = pm860x_led_set;
 	mutex_init(&data->lock);
-	INIT_WORK(&data->work, pm860x_led_work);
 
 	ret = led_classdev_register(chip->dev, &data->cdev);
 	if (ret < 0) {
diff --git a/drivers/leds/leds-aat1290.c b/drivers/leds/leds-aat1290.c
index ac77d36b630c..def3cf9f7e92 100644
--- a/drivers/leds/leds-aat1290.c
+++ b/drivers/leds/leds-aat1290.c
@@ -20,7 +20,6 @@
 #include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <media/v4l2-flash-led-class.h>
 
 #define AAT1290_MOVIE_MODE_CURRENT_ADDR	17
@@ -82,8 +81,6 @@ struct aat1290_led {
 
 	/* brightness cache */
 	unsigned int torch_brightness;
-	/* assures led-triggers compatibility */
-	struct work_struct work_brightness_set;
 };
 
 static struct aat1290_led *fled_cdev_to_led(
@@ -92,6 +89,12 @@ static struct aat1290_led *fled_cdev_to_led(
 	return container_of(fled_cdev, struct aat1290_led, fled_cdev);
 }
 
+static struct led_classdev_flash *led_cdev_to_fled_cdev(
+				struct led_classdev *led_cdev)
+{
+	return container_of(led_cdev, struct led_classdev_flash, led_cdev);
+}
+
 static void aat1290_as2cwire_write(struct aat1290_led *led, int addr, int value)
 {
 	int i;
@@ -134,9 +137,14 @@ static void aat1290_set_flash_safety_timer(struct aat1290_led *led,
 							flash_tm_reg);
 }
 
-static void aat1290_brightness_set(struct aat1290_led *led,
+/* LED subsystem callbacks */
+
+static int aat1290_led_brightness_set(struct led_classdev *led_cdev,
 					enum led_brightness brightness)
 {
+	struct led_classdev_flash *fled_cdev = led_cdev_to_fled_cdev(led_cdev);
+	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
+
 	mutex_lock(&led->lock);
 
 	if (brightness == 0) {
@@ -158,35 +166,6 @@ static void aat1290_brightness_set(struct aat1290_led *led,
 	}
 
 	mutex_unlock(&led->lock);
-}
-
-/* LED subsystem callbacks */
-
-static void aat1290_brightness_set_work(struct work_struct *work)
-{
-	struct aat1290_led *led =
-		container_of(work, struct aat1290_led, work_brightness_set);
-
-	aat1290_brightness_set(led, led->torch_brightness);
-}
-
-static void aat1290_led_brightness_set(struct led_classdev *led_cdev,
-					enum led_brightness brightness)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
-
-	led->torch_brightness = brightness;
-	schedule_work(&led->work_brightness_set);
-}
-
-static int aat1290_led_brightness_set_sync(struct led_classdev *led_cdev,
-					enum led_brightness brightness)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct aat1290_led *led = fled_cdev_to_led(fled_cdev);
-
-	aat1290_brightness_set(led, brightness);
 
 	return 0;
 }
@@ -296,7 +275,7 @@ static int aat1290_led_parse_dt(struct aat1290_led *led,
 	if (ret < 0) {
 		dev_err(dev,
 			"flash-max-microamp DT property missing\n");
-		return ret;
+		goto err_parse_dt;
 	}
 
 	ret = of_property_read_u32(child_node, "flash-max-timeout-us",
@@ -304,13 +283,14 @@ static int aat1290_led_parse_dt(struct aat1290_led *led,
 	if (ret < 0) {
 		dev_err(dev,
 			"flash-max-timeout-us DT property missing\n");
-		return ret;
+		goto err_parse_dt;
 	}
 
-	of_node_put(child_node);
-
 	*sub_node = child_node;
 
+err_parse_dt:
+	of_node_put(child_node);
+
 	return ret;
 }
 
@@ -509,11 +489,9 @@ static int aat1290_led_probe(struct platform_device *pdev)
 	mutex_init(&led->lock);
 
 	/* Initialize LED Flash class device */
-	led_cdev->brightness_set = aat1290_led_brightness_set;
-	led_cdev->brightness_set_sync = aat1290_led_brightness_set_sync;
+	led_cdev->brightness_set_blocking = aat1290_led_brightness_set;
 	led_cdev->max_brightness = led_cfg.max_brightness;
 	led_cdev->flags |= LED_DEV_CAP_FLASH;
-	INIT_WORK(&led->work_brightness_set, aat1290_brightness_set_work);
 
 	aat1290_init_flash_timeout(led, &led_cfg);
 
@@ -548,7 +526,6 @@ static int aat1290_led_remove(struct platform_device *pdev)
 
 	v4l2_flash_release(led->v4l2_flash);
 	led_classdev_flash_unregister(&led->fled_cdev);
-	cancel_work_sync(&led->work_brightness_set);
 
 	mutex_destroy(&led->lock);
 
diff --git a/drivers/leds/leds-adp5520.c b/drivers/leds/leds-adp5520.c
index 07e66cae32d3..853b2d3bdb17 100644
--- a/drivers/leds/leds-adp5520.c
+++ b/drivers/leds/leds-adp5520.c
@@ -17,34 +17,24 @@
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/leds.h>
-#include <linux/workqueue.h>
 #include <linux/mfd/adp5520.h>
 #include <linux/slab.h>
 
 struct adp5520_led {
 	struct led_classdev	cdev;
-	struct work_struct	work;
 	struct device		*master;
-	enum led_brightness	new_brightness;
 	int			id;
 	int			flags;
 };
 
-static void adp5520_led_work(struct work_struct *work)
-{
-	struct adp5520_led *led = container_of(work, struct adp5520_led, work);
-	adp5520_write(led->master, ADP5520_LED1_CURRENT + led->id - 1,
-			 led->new_brightness >> 2);
-}
-
-static void adp5520_led_set(struct led_classdev *led_cdev,
+static int adp5520_led_set(struct led_classdev *led_cdev,
 			   enum led_brightness value)
 {
 	struct adp5520_led *led;
 
 	led = container_of(led_cdev, struct adp5520_led, cdev);
-	led->new_brightness = value;
-	schedule_work(&led->work);
+	return adp5520_write(led->master, ADP5520_LED1_CURRENT + led->id - 1,
+			 value >> 2);
 }
 
 static int adp5520_led_setup(struct adp5520_led *led)
@@ -135,7 +125,7 @@ static int adp5520_led_probe(struct platform_device *pdev)
 
 		led_dat->cdev.name = cur_led->name;
 		led_dat->cdev.default_trigger = cur_led->default_trigger;
-		led_dat->cdev.brightness_set = adp5520_led_set;
+		led_dat->cdev.brightness_set_blocking = adp5520_led_set;
 		led_dat->cdev.brightness = LED_OFF;
 
 		if (cur_led->flags & ADP5520_FLAG_LED_MASK)
@@ -146,9 +136,6 @@ static int adp5520_led_probe(struct platform_device *pdev)
 		led_dat->id = led_dat->flags & ADP5520_FLAG_LED_MASK;
 
 		led_dat->master = pdev->dev.parent;
-		led_dat->new_brightness = LED_OFF;
-
-		INIT_WORK(&led_dat->work, adp5520_led_work);
 
 		ret = led_classdev_register(led_dat->master, &led_dat->cdev);
 		if (ret) {
@@ -170,10 +157,8 @@ static int adp5520_led_probe(struct platform_device *pdev)
 
 err:
 	if (i > 0) {
-		for (i = i - 1; i >= 0; i--) {
+		for (i = i - 1; i >= 0; i--)
 			led_classdev_unregister(&led[i].cdev);
-			cancel_work_sync(&led[i].work);
-		}
 	}
 
 	return ret;
@@ -192,7 +177,6 @@ static int adp5520_led_remove(struct platform_device *pdev)
 
 	for (i = 0; i < pdata->num_leds; i++) {
 		led_classdev_unregister(&led[i].cdev);
-		cancel_work_sync(&led[i].work);
 	}
 
 	return 0;
diff --git a/drivers/leds/leds-bcm6328.c b/drivers/leds/leds-bcm6328.c
index c7ea5c626331..1548259297c1 100644
--- a/drivers/leds/leds-bcm6328.c
+++ b/drivers/leds/leds-bcm6328.c
@@ -42,16 +42,16 @@
 #define BCM6328_LED_SHIFT_TEST		BIT(30)
 #define BCM6328_LED_TEST		BIT(31)
 #define BCM6328_INIT_MASK		(BCM6328_SERIAL_LED_EN | \
-					 BCM6328_SERIAL_LED_MUX  | \
+					 BCM6328_SERIAL_LED_MUX | \
 					 BCM6328_SERIAL_LED_CLK_NPOL | \
 					 BCM6328_SERIAL_LED_DATA_PPOL | \
 					 BCM6328_SERIAL_LED_SHIFT_DIR)
 
 #define BCM6328_LED_MODE_MASK		3
-#define BCM6328_LED_MODE_OFF		0
+#define BCM6328_LED_MODE_ON		0
 #define BCM6328_LED_MODE_FAST		1
 #define BCM6328_LED_MODE_BLINK		2
-#define BCM6328_LED_MODE_ON		3
+#define BCM6328_LED_MODE_OFF		3
 #define BCM6328_LED_SHIFT(X)		((X) << 1)
 
 /**
@@ -76,12 +76,20 @@ struct bcm6328_led {
 
 static void bcm6328_led_write(void __iomem *reg, unsigned long data)
 {
+#ifdef CONFIG_CPU_BIG_ENDIAN
 	iowrite32be(data, reg);
+#else
+	writel(data, reg);
+#endif
 }
 
 static unsigned long bcm6328_led_read(void __iomem *reg)
 {
+#ifdef CONFIG_CPU_BIG_ENDIAN
 	return ioread32be(reg);
+#else
+	return readl(reg);
+#endif
 }
 
 /**
@@ -126,34 +134,45 @@ static void bcm6328_led_set(struct led_classdev *led_cdev,
 	*(led->blink_leds) &= ~BIT(led->pin);
 	if ((led->active_low && value == LED_OFF) ||
 	    (!led->active_low && value != LED_OFF))
-		bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
-	else
 		bcm6328_led_mode(led, BCM6328_LED_MODE_ON);
+	else
+		bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
 	spin_unlock_irqrestore(led->lock, flags);
 }
 
+static unsigned long bcm6328_blink_delay(unsigned long delay)
+{
+	unsigned long bcm6328_delay;
+
+	bcm6328_delay = delay + BCM6328_LED_INTERVAL_MS / 2;
+	bcm6328_delay = bcm6328_delay / BCM6328_LED_INTERVAL_MS;
+	if (bcm6328_delay == 0)
+		bcm6328_delay = 1;
+
+	return bcm6328_delay;
+}
+
 static int bcm6328_blink_set(struct led_classdev *led_cdev,
 			     unsigned long *delay_on, unsigned long *delay_off)
 {
 	struct bcm6328_led *led =
 		container_of(led_cdev, struct bcm6328_led, cdev);
 	unsigned long delay, flags;
+	int rc;
 
 	if (!*delay_on)
 		*delay_on = BCM6328_LED_DEF_DELAY;
 	if (!*delay_off)
 		*delay_off = BCM6328_LED_DEF_DELAY;
 
-	if (*delay_on != *delay_off) {
+	delay = bcm6328_blink_delay(*delay_on);
+	if (delay != bcm6328_blink_delay(*delay_off)) {
 		dev_dbg(led_cdev->dev,
 			"fallback to soft blinking (delay_on != delay_off)\n");
 		return -EINVAL;
 	}
 
-	delay = *delay_on / BCM6328_LED_INTERVAL_MS;
-	if (delay == 0)
-		delay = 1;
-	else if (delay > BCM6328_LED_INTV_MASK) {
+	if (delay > BCM6328_LED_INTV_MASK) {
 		dev_dbg(led_cdev->dev,
 			"fallback to soft blinking (delay > %ums)\n",
 			BCM6328_LED_INTV_MASK * BCM6328_LED_INTERVAL_MS);
@@ -175,16 +194,15 @@ static int bcm6328_blink_set(struct led_classdev *led_cdev,
 		bcm6328_led_write(led->mem + BCM6328_REG_INIT, val);
 
 		bcm6328_led_mode(led, BCM6328_LED_MODE_BLINK);
-
-		spin_unlock_irqrestore(led->lock, flags);
+		rc = 0;
 	} else {
-		spin_unlock_irqrestore(led->lock, flags);
 		dev_dbg(led_cdev->dev,
 			"fallback to soft blinking (delay already set)\n");
-		return -EINVAL;
+		rc = -EINVAL;
 	}
+	spin_unlock_irqrestore(led->lock, flags);
 
-	return 0;
+	return rc;
 }
 
 static int bcm6328_hwled(struct device *dev, struct device_node *nc, u32 reg,
@@ -264,7 +282,6 @@ static int bcm6328_led(struct device *dev, struct device_node *nc, u32 reg,
 		       unsigned long *blink_leds, unsigned long *blink_delay)
 {
 	struct bcm6328_led *led;
-	unsigned long flags;
 	const char *state;
 	int rc;
 
@@ -286,7 +303,6 @@ static int bcm6328_led(struct device *dev, struct device_node *nc, u32 reg,
 						    "linux,default-trigger",
 						    NULL);
 
-	spin_lock_irqsave(lock, flags);
 	if (!of_property_read_string(nc, "default-state", &state)) {
 		if (!strcmp(state, "on")) {
 			led->cdev.brightness = LED_FULL;
@@ -303,8 +319,8 @@ static int bcm6328_led(struct device *dev, struct device_node *nc, u32 reg,
 			val = bcm6328_led_read(mode) >>
 			      BCM6328_LED_SHIFT(shift % 16);
 			val &= BCM6328_LED_MODE_MASK;
-			if ((led->active_low && val == BCM6328_LED_MODE_ON) ||
-			    (!led->active_low && val == BCM6328_LED_MODE_OFF))
+			if ((led->active_low && val == BCM6328_LED_MODE_OFF) ||
+			    (!led->active_low && val == BCM6328_LED_MODE_ON))
 				led->cdev.brightness = LED_FULL;
 			else
 				led->cdev.brightness = LED_OFF;
@@ -315,12 +331,7 @@ static int bcm6328_led(struct device *dev, struct device_node *nc, u32 reg,
 		led->cdev.brightness = LED_OFF;
 	}
 
-	if ((led->active_low && led->cdev.brightness == LED_FULL) ||
-	    (!led->active_low && led->cdev.brightness == LED_OFF))
-		bcm6328_led_mode(led, BCM6328_LED_MODE_ON);
-	else
-		bcm6328_led_mode(led, BCM6328_LED_MODE_OFF);
-	spin_unlock_irqrestore(lock, flags);
+	bcm6328_led_set(&led->cdev, led->cdev.brightness);
 
 	led->cdev.brightness_set = bcm6328_led_set;
 	led->cdev.blink_set = bcm6328_blink_set;
@@ -341,7 +352,7 @@ static int bcm6328_leds_probe(struct platform_device *pdev)
 	struct device_node *child;
 	struct resource *mem_r;
 	void __iomem *mem;
-	spinlock_t *lock;
+	spinlock_t *lock; /* memory lock */
 	unsigned long val, *blink_leds, *blink_delay;
 
 	mem_r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/leds/leds-bcm6358.c b/drivers/leds/leds-bcm6358.c
index 82b4ee1bc87e..b2cc06618abe 100644
--- a/drivers/leds/leds-bcm6358.c
+++ b/drivers/leds/leds-bcm6358.c
@@ -49,12 +49,20 @@ struct bcm6358_led {
 
 static void bcm6358_led_write(void __iomem *reg, unsigned long data)
 {
+#ifdef CONFIG_CPU_BIG_ENDIAN
 	iowrite32be(data, reg);
+#else
+	writel(data, reg);
+#endif
 }
 
 static unsigned long bcm6358_led_read(void __iomem *reg)
 {
+#ifdef CONFIG_CPU_BIG_ENDIAN
 	return ioread32be(reg);
+#else
+	return readl(reg);
+#endif
 }
 
 static unsigned long bcm6358_led_busy(void __iomem *mem)
@@ -68,12 +76,15 @@ static unsigned long bcm6358_led_busy(void __iomem *mem)
 	return val;
 }
 
-static void bcm6358_led_mode(struct bcm6358_led *led, unsigned long value)
+static void bcm6358_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
 {
-	unsigned long val;
+	struct bcm6358_led *led =
+		container_of(led_cdev, struct bcm6358_led, cdev);
+	unsigned long flags, val;
 
+	spin_lock_irqsave(led->lock, flags);
 	bcm6358_led_busy(led->mem);
-
 	val = bcm6358_led_read(led->mem + BCM6358_REG_MODE);
 	if ((led->active_low && value == LED_OFF) ||
 	    (!led->active_low && value != LED_OFF))
@@ -81,17 +92,6 @@ static void bcm6358_led_mode(struct bcm6358_led *led, unsigned long value)
 	else
 		val &= ~(BIT(led->pin));
 	bcm6358_led_write(led->mem + BCM6358_REG_MODE, val);
-}
-
-static void bcm6358_led_set(struct led_classdev *led_cdev,
-			    enum led_brightness value)
-{
-	struct bcm6358_led *led =
-		container_of(led_cdev, struct bcm6358_led, cdev);
-	unsigned long flags;
-
-	spin_lock_irqsave(led->lock, flags);
-	bcm6358_led_mode(led, value);
 	spin_unlock_irqrestore(led->lock, flags);
 }
 
@@ -99,7 +99,6 @@ static int bcm6358_led(struct device *dev, struct device_node *nc, u32 reg,
 		       void __iomem *mem, spinlock_t *lock)
 {
 	struct bcm6358_led *led;
-	unsigned long flags;
 	const char *state;
 	int rc;
 
@@ -119,15 +118,11 @@ static int bcm6358_led(struct device *dev, struct device_node *nc, u32 reg,
 						    "linux,default-trigger",
 						    NULL);
 
-	spin_lock_irqsave(lock, flags);
 	if (!of_property_read_string(nc, "default-state", &state)) {
 		if (!strcmp(state, "on")) {
 			led->cdev.brightness = LED_FULL;
 		} else if (!strcmp(state, "keep")) {
 			unsigned long val;
-
-			bcm6358_led_busy(led->mem);
-
 			val = bcm6358_led_read(led->mem + BCM6358_REG_MODE);
 			val &= BIT(led->pin);
 			if ((led->active_low && !val) ||
@@ -141,8 +136,8 @@ static int bcm6358_led(struct device *dev, struct device_node *nc, u32 reg,
 	} else {
 		led->cdev.brightness = LED_OFF;
 	}
-	bcm6358_led_mode(led, led->cdev.brightness);
-	spin_unlock_irqrestore(lock, flags);
+
+	bcm6358_led_set(&led->cdev, led->cdev.brightness);
 
 	led->cdev.brightness_set = bcm6358_led_set;
 
diff --git a/drivers/leds/leds-bd2802.c b/drivers/leds/leds-bd2802.c
index 6078c15d3452..6b4de762a760 100644
--- a/drivers/leds/leds-bd2802.c
+++ b/drivers/leds/leds-bd2802.c
@@ -72,7 +72,6 @@ struct bd2802_led {
 	struct bd2802_led_platform_data	*pdata;
 	struct i2c_client		*client;
 	struct rw_semaphore		rwsem;
-	struct work_struct		work;
 
 	struct led_state		led[2];
 
@@ -518,29 +517,22 @@ static struct device_attribute *bd2802_attributes[] = {
 	&bd2802_rgb_current_attr,
 };
 
-static void bd2802_led_work(struct work_struct *work)
-{
-	struct bd2802_led *led = container_of(work, struct bd2802_led, work);
-
-	if (led->state)
-		bd2802_turn_on(led, led->led_id, led->color, led->state);
-	else
-		bd2802_turn_off(led, led->led_id, led->color);
-}
-
 #define BD2802_CONTROL_RGBS(name, id, clr)				\
-static void bd2802_set_##name##_brightness(struct led_classdev *led_cdev,\
+static int bd2802_set_##name##_brightness(struct led_classdev *led_cdev,\
 					enum led_brightness value)	\
 {									\
 	struct bd2802_led *led =					\
 		container_of(led_cdev, struct bd2802_led, cdev_##name);	\
 	led->led_id = id;						\
 	led->color = clr;						\
-	if (value == LED_OFF)						\
+	if (value == LED_OFF) {						\
 		led->state = BD2802_OFF;				\
-	else								\
+		bd2802_turn_off(led, led->led_id, led->color);		\
+	} else {							\
 		led->state = BD2802_ON;					\
-	schedule_work(&led->work);					\
+		bd2802_turn_on(led, led->led_id, led->color, BD2802_ON);\
+	}								\
+	return 0;							\
 }									\
 static int bd2802_set_##name##_blink(struct led_classdev *led_cdev,	\
 		unsigned long *delay_on, unsigned long *delay_off)	\
@@ -552,7 +544,7 @@ static int bd2802_set_##name##_blink(struct led_classdev *led_cdev,	\
 	led->led_id = id;						\
 	led->color = clr;						\
 	led->state = BD2802_BLINK;					\
-	schedule_work(&led->work);					\
+	bd2802_turn_on(led, led->led_id, led->color, BD2802_BLINK);	\
 	return 0;							\
 }
 
@@ -567,11 +559,9 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 {
 	int ret;
 
-	INIT_WORK(&led->work, bd2802_led_work);
-
 	led->cdev_led1r.name = "led1_R";
 	led->cdev_led1r.brightness = LED_OFF;
-	led->cdev_led1r.brightness_set = bd2802_set_led1r_brightness;
+	led->cdev_led1r.brightness_set_blocking = bd2802_set_led1r_brightness;
 	led->cdev_led1r.blink_set = bd2802_set_led1r_blink;
 
 	ret = led_classdev_register(&led->client->dev, &led->cdev_led1r);
@@ -583,7 +573,7 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 
 	led->cdev_led1g.name = "led1_G";
 	led->cdev_led1g.brightness = LED_OFF;
-	led->cdev_led1g.brightness_set = bd2802_set_led1g_brightness;
+	led->cdev_led1g.brightness_set_blocking = bd2802_set_led1g_brightness;
 	led->cdev_led1g.blink_set = bd2802_set_led1g_blink;
 
 	ret = led_classdev_register(&led->client->dev, &led->cdev_led1g);
@@ -595,7 +585,7 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 
 	led->cdev_led1b.name = "led1_B";
 	led->cdev_led1b.brightness = LED_OFF;
-	led->cdev_led1b.brightness_set = bd2802_set_led1b_brightness;
+	led->cdev_led1b.brightness_set_blocking = bd2802_set_led1b_brightness;
 	led->cdev_led1b.blink_set = bd2802_set_led1b_blink;
 
 	ret = led_classdev_register(&led->client->dev, &led->cdev_led1b);
@@ -607,7 +597,7 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 
 	led->cdev_led2r.name = "led2_R";
 	led->cdev_led2r.brightness = LED_OFF;
-	led->cdev_led2r.brightness_set = bd2802_set_led2r_brightness;
+	led->cdev_led2r.brightness_set_blocking = bd2802_set_led2r_brightness;
 	led->cdev_led2r.blink_set = bd2802_set_led2r_blink;
 
 	ret = led_classdev_register(&led->client->dev, &led->cdev_led2r);
@@ -619,7 +609,7 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 
 	led->cdev_led2g.name = "led2_G";
 	led->cdev_led2g.brightness = LED_OFF;
-	led->cdev_led2g.brightness_set = bd2802_set_led2g_brightness;
+	led->cdev_led2g.brightness_set_blocking = bd2802_set_led2g_brightness;
 	led->cdev_led2g.blink_set = bd2802_set_led2g_blink;
 
 	ret = led_classdev_register(&led->client->dev, &led->cdev_led2g);
@@ -631,7 +621,7 @@ static int bd2802_register_led_classdev(struct bd2802_led *led)
 
 	led->cdev_led2b.name = "led2_B";
 	led->cdev_led2b.brightness = LED_OFF;
-	led->cdev_led2b.brightness_set = bd2802_set_led2b_brightness;
+	led->cdev_led2b.brightness_set_blocking = bd2802_set_led2b_brightness;
 	led->cdev_led2b.blink_set = bd2802_set_led2b_blink;
 	led->cdev_led2b.flags |= LED_CORE_SUSPENDRESUME;
 
@@ -661,7 +651,6 @@ failed_unregister_led1_R:
 
 static void bd2802_unregister_led_classdev(struct bd2802_led *led)
 {
-	cancel_work_sync(&led->work);
 	led_classdev_unregister(&led->cdev_led2b);
 	led_classdev_unregister(&led->cdev_led2g);
 	led_classdev_unregister(&led->cdev_led2r);
diff --git a/drivers/leds/leds-blinkm.c b/drivers/leds/leds-blinkm.c
index d0452b099aee..617fe975bf6e 100644
--- a/drivers/leds/leds-blinkm.c
+++ b/drivers/leds/leds-blinkm.c
@@ -39,16 +39,9 @@ struct blinkm_led {
 	struct i2c_client *i2c_client;
 	struct led_classdev led_cdev;
 	int id;
-	atomic_t active;
-};
-
-struct blinkm_work {
-	struct blinkm_led *blinkm_led;
-	struct work_struct work;
 };
 
 #define cdev_to_blmled(c)          container_of(c, struct blinkm_led, led_cdev)
-#define work_to_blmwork(c)         container_of(c, struct blinkm_work, work)
 
 struct blinkm_data {
 	struct i2c_client *i2c_client;
@@ -439,65 +432,30 @@ static int blinkm_transfer_hw(struct i2c_client *client, int cmd)
 	return 0;
 }
 
-static void led_work(struct work_struct *work)
-{
-	int ret;
-	struct blinkm_led *led;
-	struct blinkm_data *data;
-	struct blinkm_work *blm_work = work_to_blmwork(work);
-
-	led = blm_work->blinkm_led;
-	data = i2c_get_clientdata(led->i2c_client);
-	ret = blinkm_transfer_hw(led->i2c_client, BLM_GO_RGB);
-	atomic_dec(&led->active);
-	dev_dbg(&led->i2c_client->dev,
-			"# DONE # next_red = %d, next_green = %d,"
-			" next_blue = %d, active = %d\n",
-			data->next_red, data->next_green,
-			data->next_blue, atomic_read(&led->active));
-	kfree(blm_work);
-}
-
 static int blinkm_led_common_set(struct led_classdev *led_cdev,
 				 enum led_brightness value, int color)
 {
 	/* led_brightness is 0, 127 or 255 - we just use it here as-is */
 	struct blinkm_led *led = cdev_to_blmled(led_cdev);
 	struct blinkm_data *data = i2c_get_clientdata(led->i2c_client);
-	struct blinkm_work *bl_work;
 
 	switch (color) {
 	case RED:
 		/* bail out if there's no change */
 		if (data->next_red == (u8) value)
 			return 0;
-		/* we assume a quite fast sequence here ([off]->on->off)
-		 * think of network led trigger - we cannot blink that fast, so
-		 * in case we already have a off->on->off transition queued up,
-		 * we refuse to queue up more.
-		 * Revisit: fast-changing brightness. */
-		if (atomic_read(&led->active) > 1)
-			return 0;
 		data->next_red = (u8) value;
 		break;
 	case GREEN:
 		/* bail out if there's no change */
 		if (data->next_green == (u8) value)
 			return 0;
-		/* we assume a quite fast sequence here ([off]->on->off)
-		 * Revisit: fast-changing brightness. */
-		if (atomic_read(&led->active) > 1)
-			return 0;
 		data->next_green = (u8) value;
 		break;
 	case BLUE:
 		/* bail out if there's no change */
 		if (data->next_blue == (u8) value)
 			return 0;
-		/* we assume a quite fast sequence here ([off]->on->off)
-		 * Revisit: fast-changing brightness. */
-		if (atomic_read(&led->active) > 1)
-			return 0;
 		data->next_blue = (u8) value;
 		break;
 
@@ -506,42 +464,31 @@ static int blinkm_led_common_set(struct led_classdev *led_cdev,
 		return -EINVAL;
 	}
 
-	bl_work = kzalloc(sizeof(*bl_work), GFP_ATOMIC);
-	if (!bl_work)
-		return -ENOMEM;
-
-	atomic_inc(&led->active);
+	blinkm_transfer_hw(led->i2c_client, BLM_GO_RGB);
 	dev_dbg(&led->i2c_client->dev,
-			"#TO_SCHED# next_red = %d, next_green = %d,"
-			" next_blue = %d, active = %d\n",
+			"# DONE # next_red = %d, next_green = %d,"
+			" next_blue = %d\n",
 			data->next_red, data->next_green,
-			data->next_blue, atomic_read(&led->active));
-
-	/* a fresh work _item_ for each change */
-	bl_work->blinkm_led = led;
-	INIT_WORK(&bl_work->work, led_work);
-	/* queue work in own queue for easy sync on exit*/
-	schedule_work(&bl_work->work);
-
+			data->next_blue);
 	return 0;
 }
 
-static void blinkm_led_red_set(struct led_classdev *led_cdev,
+static int blinkm_led_red_set(struct led_classdev *led_cdev,
 			       enum led_brightness value)
 {
-	blinkm_led_common_set(led_cdev, value, RED);
+	return blinkm_led_common_set(led_cdev, value, RED);
 }
 
-static void blinkm_led_green_set(struct led_classdev *led_cdev,
+static int blinkm_led_green_set(struct led_classdev *led_cdev,
 				 enum led_brightness value)
 {
-	blinkm_led_common_set(led_cdev, value, GREEN);
+	return blinkm_led_common_set(led_cdev, value, GREEN);
 }
 
-static void blinkm_led_blue_set(struct led_classdev *led_cdev,
+static int blinkm_led_blue_set(struct led_classdev *led_cdev,
 				enum led_brightness value)
 {
-	blinkm_led_common_set(led_cdev, value, BLUE);
+	return blinkm_led_common_set(led_cdev, value, BLUE);
 }
 
 static void blinkm_init_hw(struct i2c_client *client)
@@ -669,7 +616,6 @@ static int blinkm_probe(struct i2c_client *client,
 		led[i]->id = i;
 		led[i]->led_cdev.max_brightness = 255;
 		led[i]->led_cdev.flags = LED_CORE_SUSPENDRESUME;
-		atomic_set(&led[i]->active, 0);
 		switch (i) {
 		case RED:
 			snprintf(blinkm_led_name, sizeof(blinkm_led_name),
@@ -677,7 +623,8 @@ static int blinkm_probe(struct i2c_client *client,
 					 client->adapter->nr,
 					 client->addr);
 			led[i]->led_cdev.name = blinkm_led_name;
-			led[i]->led_cdev.brightness_set = blinkm_led_red_set;
+			led[i]->led_cdev.brightness_set_blocking =
+							blinkm_led_red_set;
 			err = led_classdev_register(&client->dev,
 						    &led[i]->led_cdev);
 			if (err < 0) {
@@ -693,7 +640,8 @@ static int blinkm_probe(struct i2c_client *client,
 					 client->adapter->nr,
 					 client->addr);
 			led[i]->led_cdev.name = blinkm_led_name;
-			led[i]->led_cdev.brightness_set = blinkm_led_green_set;
+			led[i]->led_cdev.brightness_set_blocking =
+							blinkm_led_green_set;
 			err = led_classdev_register(&client->dev,
 						    &led[i]->led_cdev);
 			if (err < 0) {
@@ -709,7 +657,8 @@ static int blinkm_probe(struct i2c_client *client,
 					 client->adapter->nr,
 					 client->addr);
 			led[i]->led_cdev.name = blinkm_led_name;
-			led[i]->led_cdev.brightness_set = blinkm_led_blue_set;
+			led[i]->led_cdev.brightness_set_blocking =
+							blinkm_led_blue_set;
 			err = led_classdev_register(&client->dev,
 						    &led[i]->led_cdev);
 			if (err < 0) {
@@ -746,10 +695,8 @@ static int blinkm_remove(struct i2c_client *client)
 	int i;
 
 	/* make sure no workqueue entries are pending */
-	for (i = 0; i < 3; i++) {
-		flush_scheduled_work();
+	for (i = 0; i < 3; i++)
 		led_classdev_unregister(&data->blinkm_leds[i].led_cdev);
-	}
 
 	/* reset rgb */
 	data->next_red = 0x00;
diff --git a/drivers/leds/leds-da903x.c b/drivers/leds/leds-da903x.c
index 952ba96e5b38..4752a2b6ba2b 100644
--- a/drivers/leds/leds-da903x.c
+++ b/drivers/leds/leds-da903x.c
@@ -16,7 +16,6 @@
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/leds.h>
-#include <linux/workqueue.h>
 #include <linux/mfd/da903x.h>
 #include <linux/slab.h>
 
@@ -33,9 +32,7 @@
 
 struct da903x_led {
 	struct led_classdev	cdev;
-	struct work_struct	work;
 	struct device		*master;
-	enum led_brightness	new_brightness;
 	int			id;
 	int			flags;
 };
@@ -43,11 +40,13 @@ struct da903x_led {
 #define DA9030_LED_OFFSET(id)	((id) - DA9030_ID_LED_1)
 #define DA9034_LED_OFFSET(id)	((id) - DA9034_ID_LED_1)
 
-static void da903x_led_work(struct work_struct *work)
+static int da903x_led_set(struct led_classdev *led_cdev,
+			   enum led_brightness value)
 {
-	struct da903x_led *led = container_of(work, struct da903x_led, work);
+	struct da903x_led *led =
+			container_of(led_cdev, struct da903x_led, cdev);
 	uint8_t val;
-	int offset;
+	int offset, ret = -EINVAL;
 
 	switch (led->id) {
 	case DA9030_ID_LED_1:
@@ -57,37 +56,31 @@ static void da903x_led_work(struct work_struct *work)
 	case DA9030_ID_LED_PC:
 		offset = DA9030_LED_OFFSET(led->id);
 		val = led->flags & ~0x87;
-		val |= (led->new_brightness) ? 0x80 : 0; /* EN bit */
-		val |= (0x7 - (led->new_brightness >> 5)) & 0x7; /* PWM<2:0> */
-		da903x_write(led->master, DA9030_LED1_CONTROL + offset, val);
+		val |= value ? 0x80 : 0; /* EN bit */
+		val |= (0x7 - (value >> 5)) & 0x7; /* PWM<2:0> */
+		ret = da903x_write(led->master, DA9030_LED1_CONTROL + offset,
+				   val);
 		break;
 	case DA9030_ID_VIBRA:
 		val = led->flags & ~0x80;
-		val |= (led->new_brightness) ? 0x80 : 0; /* EN bit */
-		da903x_write(led->master, DA9030_MISC_CONTROL_A, val);
+		val |= value ? 0x80 : 0; /* EN bit */
+		ret = da903x_write(led->master, DA9030_MISC_CONTROL_A, val);
 		break;
 	case DA9034_ID_LED_1:
 	case DA9034_ID_LED_2:
 		offset = DA9034_LED_OFFSET(led->id);
-		val = (led->new_brightness * 0x5f / LED_FULL) & 0x7f;
+		val = (value * 0x5f / LED_FULL) & 0x7f;
 		val |= (led->flags & DA9034_LED_RAMP) ? 0x80 : 0;
-		da903x_write(led->master, DA9034_LED1_CONTROL + offset, val);
+		ret = da903x_write(led->master, DA9034_LED1_CONTROL + offset,
+				   val);
 		break;
 	case DA9034_ID_VIBRA:
-		val = led->new_brightness & 0xfe;
-		da903x_write(led->master, DA9034_VIBRA, val);
+		val = value & 0xfe;
+		ret = da903x_write(led->master, DA9034_VIBRA, val);
 		break;
 	}
-}
 
-static void da903x_led_set(struct led_classdev *led_cdev,
-			   enum led_brightness value)
-{
-	struct da903x_led *led;
-
-	led = container_of(led_cdev, struct da903x_led, cdev);
-	led->new_brightness = value;
-	schedule_work(&led->work);
+	return ret;
 }
 
 static int da903x_led_probe(struct platform_device *pdev)
@@ -113,15 +106,12 @@ static int da903x_led_probe(struct platform_device *pdev)
 
 	led->cdev.name = pdata->name;
 	led->cdev.default_trigger = pdata->default_trigger;
-	led->cdev.brightness_set = da903x_led_set;
+	led->cdev.brightness_set_blocking = da903x_led_set;
 	led->cdev.brightness = LED_OFF;
 
 	led->id = id;
 	led->flags = pdata->flags;
 	led->master = pdev->dev.parent;
-	led->new_brightness = LED_OFF;
-
-	INIT_WORK(&led->work, da903x_led_work);
 
 	ret = led_classdev_register(led->master, &led->cdev);
 	if (ret) {
diff --git a/drivers/leds/leds-da9052.c b/drivers/leds/leds-da9052.c
index 28291b6acc8e..f8c7d82c2652 100644
--- a/drivers/leds/leds-da9052.c
+++ b/drivers/leds/leds-da9052.c
@@ -16,7 +16,6 @@
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/leds.h>
-#include <linux/workqueue.h>
 #include <linux/slab.h>
 
 #include <linux/mfd/da9052/reg.h>
@@ -32,11 +31,9 @@
 
 struct da9052_led {
 	struct led_classdev cdev;
-	struct work_struct work;
 	struct da9052 *da9052;
 	unsigned char led_index;
 	unsigned char id;
-	int brightness;
 };
 
 static unsigned char led_reg[] = {
@@ -44,12 +41,13 @@ static unsigned char led_reg[] = {
 	DA9052_LED_CONT_5_REG,
 };
 
-static int da9052_set_led_brightness(struct da9052_led *led)
+static int da9052_set_led_brightness(struct da9052_led *led,
+				     enum led_brightness brightness)
 {
 	u8 val;
 	int error;
 
-	val = (led->brightness & 0x7f) | DA9052_LED_CONT_DIM;
+	val = (brightness & 0x7f) | DA9052_LED_CONT_DIM;
 
 	error = da9052_reg_write(led->da9052, led_reg[led->led_index], val);
 	if (error < 0)
@@ -58,21 +56,13 @@ static int da9052_set_led_brightness(struct da9052_led *led)
 	return error;
 }
 
-static void da9052_led_work(struct work_struct *work)
-{
-	struct da9052_led *led = container_of(work, struct da9052_led, work);
-
-	da9052_set_led_brightness(led);
-}
-
-static void da9052_led_set(struct led_classdev *led_cdev,
+static int da9052_led_set(struct led_classdev *led_cdev,
 			   enum led_brightness value)
 {
-	struct da9052_led *led;
+	struct da9052_led *led =
+			container_of(led_cdev, struct da9052_led, cdev);
 
-	led = container_of(led_cdev, struct da9052_led, cdev);
-	led->brightness = value;
-	schedule_work(&led->work);
+	return da9052_set_led_brightness(led, value);
 }
 
 static int da9052_configure_leds(struct da9052 *da9052)
@@ -133,13 +123,11 @@ static int da9052_led_probe(struct platform_device *pdev)
 
 	for (i = 0; i < pled->num_leds; i++) {
 		led[i].cdev.name = pled->leds[i].name;
-		led[i].cdev.brightness_set = da9052_led_set;
+		led[i].cdev.brightness_set_blocking = da9052_led_set;
 		led[i].cdev.brightness = LED_OFF;
 		led[i].cdev.max_brightness = DA9052_MAX_BRIGHTNESS;
-		led[i].brightness = LED_OFF;
 		led[i].led_index = pled->leds[i].flags;
 		led[i].da9052 = dev_get_drvdata(pdev->dev.parent);
-		INIT_WORK(&led[i].work, da9052_led_work);
 
 		error = led_classdev_register(pdev->dev.parent, &led[i].cdev);
 		if (error) {
@@ -148,7 +136,8 @@ static int da9052_led_probe(struct platform_device *pdev)
 			goto err_register;
 		}
 
-		error = da9052_set_led_brightness(&led[i]);
+		error = da9052_set_led_brightness(&led[i],
+						  led[i].cdev.brightness);
 		if (error) {
 			dev_err(&pdev->dev, "Unable to init led %d\n",
 				led[i].led_index);
@@ -166,10 +155,8 @@ static int da9052_led_probe(struct platform_device *pdev)
 	return 0;
 
 err_register:
-	for (i = i - 1; i >= 0; i--) {
+	for (i = i - 1; i >= 0; i--)
 		led_classdev_unregister(&led[i].cdev);
-		cancel_work_sync(&led[i].work);
-	}
 err:
 	return error;
 }
@@ -187,10 +174,8 @@ static int da9052_led_remove(struct platform_device *pdev)
 	pled = pdata->pled;
 
 	for (i = 0; i < pled->num_leds; i++) {
-		led[i].brightness = 0;
-		da9052_set_led_brightness(&led[i]);
+		da9052_set_led_brightness(&led[i], LED_OFF);
 		led_classdev_unregister(&led[i].cdev);
-		cancel_work_sync(&led[i].work);
 	}
 
 	return 0;
diff --git a/drivers/leds/leds-dac124s085.c b/drivers/leds/leds-dac124s085.c
index 314159610d24..5a5a86d5f1f5 100644
--- a/drivers/leds/leds-dac124s085.c
+++ b/drivers/leds/leds-dac124s085.c
@@ -13,20 +13,15 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
-#include <linux/spinlock.h>
-#include <linux/workqueue.h>
 #include <linux/spi/spi.h>
 
 struct dac124s085_led {
 	struct led_classdev	ldev;
 	struct spi_device	*spi;
 	int			id;
-	int			brightness;
 	char			name[sizeof("dac124s085-3")];
 
 	struct mutex		mutex;
-	struct work_struct	work;
-	spinlock_t		lock;
 };
 
 struct dac124s085 {
@@ -38,29 +33,21 @@ struct dac124s085 {
 #define ALL_WRITE_UPDATE	(2 << 12)
 #define POWER_DOWN_OUTPUT	(3 << 12)
 
-static void dac124s085_led_work(struct work_struct *work)
+static int dac124s085_set_brightness(struct led_classdev *ldev,
+				      enum led_brightness brightness)
 {
-	struct dac124s085_led *led = container_of(work, struct dac124s085_led,
-						  work);
+	struct dac124s085_led *led = container_of(ldev, struct dac124s085_led,
+						  ldev);
 	u16 word;
+	int ret;
 
 	mutex_lock(&led->mutex);
 	word = cpu_to_le16(((led->id) << 14) | REG_WRITE_UPDATE |
-			   (led->brightness & 0xfff));
-	spi_write(led->spi, (const u8 *)&word, sizeof(word));
+			   (brightness & 0xfff));
+	ret = spi_write(led->spi, (const u8 *)&word, sizeof(word));
 	mutex_unlock(&led->mutex);
-}
-
-static void dac124s085_set_brightness(struct led_classdev *ldev,
-				      enum led_brightness brightness)
-{
-	struct dac124s085_led *led = container_of(ldev, struct dac124s085_led,
-						  ldev);
 
-	spin_lock(&led->lock);
-	led->brightness = brightness;
-	schedule_work(&led->work);
-	spin_unlock(&led->lock);
+	return ret;
 }
 
 static int dac124s085_probe(struct spi_device *spi)
@@ -78,16 +65,13 @@ static int dac124s085_probe(struct spi_device *spi)
 	for (i = 0; i < ARRAY_SIZE(dac->leds); i++) {
 		led		= dac->leds + i;
 		led->id		= i;
-		led->brightness	= LED_OFF;
 		led->spi	= spi;
 		snprintf(led->name, sizeof(led->name), "dac124s085-%d", i);
-		spin_lock_init(&led->lock);
-		INIT_WORK(&led->work, dac124s085_led_work);
 		mutex_init(&led->mutex);
 		led->ldev.name = led->name;
 		led->ldev.brightness = LED_OFF;
 		led->ldev.max_brightness = 0xfff;
-		led->ldev.brightness_set = dac124s085_set_brightness;
+		led->ldev.brightness_set_blocking = dac124s085_set_brightness;
 		ret = led_classdev_register(&spi->dev, &led->ldev);
 		if (ret < 0)
 			goto eledcr;
@@ -109,10 +93,8 @@ static int dac124s085_remove(struct spi_device *spi)
 	struct dac124s085	*dac = spi_get_drvdata(spi);
 	int i;
 
-	for (i = 0; i < ARRAY_SIZE(dac->leds); i++) {
+	for (i = 0; i < ARRAY_SIZE(dac->leds); i++)
 		led_classdev_unregister(&dac->leds[i].ldev);
-		cancel_work_sync(&dac->leds[i].work);
-	}
 
 	return 0;
 }
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 5db4515a4fd7..7bc53280dbfd 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -20,32 +20,16 @@
 #include <linux/platform_device.h>
 #include <linux/property.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 
 struct gpio_led_data {
 	struct led_classdev cdev;
 	struct gpio_desc *gpiod;
-	struct work_struct work;
-	u8 new_level;
 	u8 can_sleep;
 	u8 blinking;
 	int (*platform_gpio_blink_set)(struct gpio_desc *desc, int state,
 			unsigned long *delay_on, unsigned long *delay_off);
 };
 
-static void gpio_led_work(struct work_struct *work)
-{
-	struct gpio_led_data *led_dat =
-		container_of(work, struct gpio_led_data, work);
-
-	if (led_dat->blinking) {
-		led_dat->platform_gpio_blink_set(led_dat->gpiod,
-					led_dat->new_level, NULL, NULL);
-		led_dat->blinking = 0;
-	} else
-		gpiod_set_value_cansleep(led_dat->gpiod, led_dat->new_level);
-}
-
 static void gpio_led_set(struct led_classdev *led_cdev,
 	enum led_brightness value)
 {
@@ -58,23 +42,25 @@ static void gpio_led_set(struct led_classdev *led_cdev,
 	else
 		level = 1;
 
-	/* Setting GPIOs with I2C/etc requires a task context, and we don't
-	 * seem to have a reliable way to know if we're already in one; so
-	 * let's just assume the worst.
-	 */
-	if (led_dat->can_sleep) {
-		led_dat->new_level = level;
-		schedule_work(&led_dat->work);
+	if (led_dat->blinking) {
+		led_dat->platform_gpio_blink_set(led_dat->gpiod, level,
+						 NULL, NULL);
+		led_dat->blinking = 0;
 	} else {
-		if (led_dat->blinking) {
-			led_dat->platform_gpio_blink_set(led_dat->gpiod, level,
-							 NULL, NULL);
-			led_dat->blinking = 0;
-		} else
+		if (led_dat->can_sleep)
+			gpiod_set_value_cansleep(led_dat->gpiod, level);
+		else
 			gpiod_set_value(led_dat->gpiod, level);
 	}
 }
 
+static int gpio_led_set_blocking(struct led_classdev *led_cdev,
+	enum led_brightness value)
+{
+	gpio_led_set(led_cdev, value);
+	return 0;
+}
+
 static int gpio_blink_set(struct led_classdev *led_cdev,
 	unsigned long *delay_on, unsigned long *delay_off)
 {
@@ -125,12 +111,15 @@ static int create_gpio_led(const struct gpio_led *template,
 	led_dat->cdev.name = template->name;
 	led_dat->cdev.default_trigger = template->default_trigger;
 	led_dat->can_sleep = gpiod_cansleep(led_dat->gpiod);
+	if (!led_dat->can_sleep)
+		led_dat->cdev.brightness_set = gpio_led_set;
+	else
+		led_dat->cdev.brightness_set_blocking = gpio_led_set_blocking;
 	led_dat->blinking = 0;
 	if (blink_set) {
 		led_dat->platform_gpio_blink_set = blink_set;
 		led_dat->cdev.blink_set = gpio_blink_set;
 	}
-	led_dat->cdev.brightness_set = gpio_led_set;
 	if (template->default_state == LEDS_GPIO_DEFSTATE_KEEP)
 		state = !!gpiod_get_value_cansleep(led_dat->gpiod);
 	else
@@ -143,17 +132,9 @@ static int create_gpio_led(const struct gpio_led *template,
 	if (ret < 0)
 		return ret;
 
-	INIT_WORK(&led_dat->work, gpio_led_work);
-
 	return led_classdev_register(parent, &led_dat->cdev);
 }
 
-static void delete_gpio_led(struct gpio_led_data *led)
-{
-	led_classdev_unregister(&led->cdev);
-	cancel_work_sync(&led->work);
-}
-
 struct gpio_leds_priv {
 	int num_leds;
 	struct gpio_led_data leds[];
@@ -233,7 +214,7 @@ static struct gpio_leds_priv *gpio_leds_create(struct platform_device *pdev)
 
 err:
 	for (count = priv->num_leds - 1; count >= 0; count--)
-		delete_gpio_led(&priv->leds[count]);
+		led_classdev_unregister(&priv->leds[count].cdev);
 	return ERR_PTR(ret);
 }
 
@@ -265,7 +246,8 @@ static int gpio_led_probe(struct platform_device *pdev)
 			if (ret < 0) {
 				/* On failure: unwind the led creations */
 				for (i = i - 1; i >= 0; i--)
-					delete_gpio_led(&priv->leds[i]);
+					led_classdev_unregister(
+							&priv->leds[i].cdev);
 				return ret;
 			}
 		}
@@ -286,7 +268,7 @@ static int gpio_led_remove(struct platform_device *pdev)
 	int i;
 
 	for (i = 0; i < priv->num_leds; i++)
-		delete_gpio_led(&priv->leds[i]);
+		led_classdev_unregister(&priv->leds[i].cdev);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-ipaq-micro.c b/drivers/leds/leds-ipaq-micro.c
index fa262b6b25eb..02f17331379d 100644
--- a/drivers/leds/leds-ipaq-micro.c
+++ b/drivers/leds/leds-ipaq-micro.c
@@ -20,7 +20,7 @@
 #define LED_AUTOSTOP (1 << 5) /* LED ON/OFF auto stop set 0:disable, 1:enable */
 #define LED_ALWAYS   (1 << 6) /* LED Interrupt Mask 0:No mask, 1:mask         */
 
-static void micro_leds_brightness_set(struct led_classdev *led_cdev,
+static int micro_leds_brightness_set(struct led_classdev *led_cdev,
 				      enum led_brightness value)
 {
 	struct ipaq_micro *micro = dev_get_drvdata(led_cdev->dev->parent->parent);
@@ -50,7 +50,7 @@ static void micro_leds_brightness_set(struct led_classdev *led_cdev,
 		msg.tx_data[2] = 1;
 		msg.tx_data[3] = 0; /* Duty cycle 256 */
 	}
-	ipaq_micro_tx_msg_sync(micro, &msg);
+	return ipaq_micro_tx_msg_sync(micro, &msg);
 }
 
 /* Maximum duty cycle in ms 256/10 sec = 25600 ms */
@@ -102,7 +102,7 @@ static int micro_leds_blink_set(struct led_classdev *led_cdev,
 
 static struct led_classdev micro_led = {
 	.name			= "led-ipaq-micro",
-	.brightness_set		= micro_leds_brightness_set,
+	.brightness_set_blocking = micro_leds_brightness_set,
 	.blink_set		= micro_leds_blink_set,
 	.flags			= LED_CORE_SUSPENDRESUME,
 };
diff --git a/drivers/leds/leds-ktd2692.c b/drivers/leds/leds-ktd2692.c
index feca07be85f5..bf23ba191ad0 100644
--- a/drivers/leds/leds-ktd2692.c
+++ b/drivers/leds/leds-ktd2692.c
@@ -18,7 +18,6 @@
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
-#include <linux/workqueue.h>
 
 /* Value related the movie mode */
 #define KTD2692_MOVIE_MODE_CURRENT_LEVELS	16
@@ -82,7 +81,6 @@ struct ktd2692_context {
 	/* secures access to the device */
 	struct mutex lock;
 	struct regulator *regulator;
-	struct work_struct work_brightness_set;
 
 	struct gpio_desc *aux_gpio;
 	struct gpio_desc *ctrl_gpio;
@@ -158,9 +156,12 @@ static void ktd2692_expresswire_write(struct ktd2692_context *led, u8 value)
 	ktd2692_expresswire_end(led);
 }
 
-static void ktd2692_brightness_set(struct ktd2692_context *led,
-				   enum led_brightness brightness)
+static int ktd2692_led_brightness_set(struct led_classdev *led_cdev,
+				       enum led_brightness brightness)
 {
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
+
 	mutex_lock(&led->lock);
 
 	if (brightness == LED_OFF) {
@@ -174,33 +175,6 @@ static void ktd2692_brightness_set(struct ktd2692_context *led,
 
 	ktd2692_expresswire_write(led, led->mode | KTD2692_REG_MODE_BASE);
 	mutex_unlock(&led->lock);
-}
-
-static void ktd2692_brightness_set_work(struct work_struct *work)
-{
-	struct ktd2692_context *led =
-		container_of(work, struct ktd2692_context, work_brightness_set);
-
-	ktd2692_brightness_set(led, led->torch_brightness);
-}
-
-static void ktd2692_led_brightness_set(struct led_classdev *led_cdev,
-				       enum led_brightness brightness)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
-
-	led->torch_brightness = brightness;
-	schedule_work(&led->work_brightness_set);
-}
-
-static int ktd2692_led_brightness_set_sync(struct led_classdev *led_cdev,
-					   enum led_brightness brightness)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct ktd2692_context *led = fled_cdev_to_led(fled_cdev);
-
-	ktd2692_brightness_set(led, brightness);
 
 	return 0;
 }
@@ -332,21 +306,24 @@ static int ktd2692_parse_dt(struct ktd2692_context *led, struct device *dev,
 				   &cfg->movie_max_microamp);
 	if (ret) {
 		dev_err(dev, "failed to parse led-max-microamp\n");
-		return ret;
+		goto err_parse_dt;
 	}
 
 	ret = of_property_read_u32(child_node, "flash-max-microamp",
 				   &cfg->flash_max_microamp);
 	if (ret) {
 		dev_err(dev, "failed to parse flash-max-microamp\n");
-		return ret;
+		goto err_parse_dt;
 	}
 
 	ret = of_property_read_u32(child_node, "flash-max-timeout-us",
 				   &cfg->flash_max_timeout);
-	if (ret)
+	if (ret) {
 		dev_err(dev, "failed to parse flash-max-timeout-us\n");
+		goto err_parse_dt;
+	}
 
+err_parse_dt:
 	of_node_put(child_node);
 	return ret;
 }
@@ -381,12 +358,10 @@ static int ktd2692_probe(struct platform_device *pdev)
 	fled_cdev->ops = &flash_ops;
 
 	led_cdev->max_brightness = led_cfg.max_brightness;
-	led_cdev->brightness_set = ktd2692_led_brightness_set;
-	led_cdev->brightness_set_sync = ktd2692_led_brightness_set_sync;
+	led_cdev->brightness_set_blocking = ktd2692_led_brightness_set;
 	led_cdev->flags |= LED_CORE_SUSPENDRESUME | LED_DEV_CAP_FLASH;
 
 	mutex_init(&led->lock);
-	INIT_WORK(&led->work_brightness_set, ktd2692_brightness_set_work);
 
 	platform_set_drvdata(pdev, led);
 
@@ -408,7 +383,6 @@ static int ktd2692_remove(struct platform_device *pdev)
 	int ret;
 
 	led_classdev_flash_unregister(&led->fled_cdev);
-	cancel_work_sync(&led->work_brightness_set);
 
 	if (led->regulator) {
 		ret = regulator_disable(led->regulator);
diff --git a/drivers/leds/leds-lm3533.c b/drivers/leds/leds-lm3533.c
index 6e2e02035dd7..196dcb5e6004 100644
--- a/drivers/leds/leds-lm3533.c
+++ b/drivers/leds/leds-lm3533.c
@@ -17,7 +17,6 @@
 #include <linux/mutex.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 
 #include <linux/mfd/lm3533.h>
 
@@ -53,9 +52,6 @@ struct lm3533_led {
 
 	struct mutex mutex;
 	unsigned long flags;
-
-	struct work_struct work;
-	u8 new_brightness;
 };
 
 
@@ -123,27 +119,17 @@ out:
 	return ret;
 }
 
-static void lm3533_led_work(struct work_struct *work)
-{
-	struct lm3533_led *led = container_of(work, struct lm3533_led, work);
-
-	dev_dbg(led->cdev.dev, "%s - %u\n", __func__, led->new_brightness);
-
-	if (led->new_brightness == 0)
-		lm3533_led_pattern_enable(led, 0);	/* disable blink */
-
-	lm3533_ctrlbank_set_brightness(&led->cb, led->new_brightness);
-}
-
-static void lm3533_led_set(struct led_classdev *cdev,
+static int lm3533_led_set(struct led_classdev *cdev,
 						enum led_brightness value)
 {
 	struct lm3533_led *led = to_lm3533_led(cdev);
 
 	dev_dbg(led->cdev.dev, "%s - %d\n", __func__, value);
 
-	led->new_brightness = value;
-	schedule_work(&led->work);
+	if (value == 0)
+		lm3533_led_pattern_enable(led, 0);	/* disable blink */
+
+	return lm3533_ctrlbank_set_brightness(&led->cb, value);
 }
 
 static enum led_brightness lm3533_led_get(struct led_classdev *cdev)
@@ -693,7 +679,7 @@ static int lm3533_led_probe(struct platform_device *pdev)
 	led->lm3533 = lm3533;
 	led->cdev.name = pdata->name;
 	led->cdev.default_trigger = pdata->default_trigger;
-	led->cdev.brightness_set = lm3533_led_set;
+	led->cdev.brightness_set_blocking = lm3533_led_set;
 	led->cdev.brightness_get = lm3533_led_get;
 	led->cdev.blink_set = lm3533_led_blink_set;
 	led->cdev.brightness = LED_OFF;
@@ -701,7 +687,6 @@ static int lm3533_led_probe(struct platform_device *pdev)
 	led->id = pdev->id;
 
 	mutex_init(&led->mutex);
-	INIT_WORK(&led->work, lm3533_led_work);
 
 	/* The class framework makes a callback to get brightness during
 	 * registration so use parent device (for error reporting) until
@@ -733,7 +718,6 @@ static int lm3533_led_probe(struct platform_device *pdev)
 
 err_unregister:
 	led_classdev_unregister(&led->cdev);
-	flush_work(&led->work);
 
 	return ret;
 }
@@ -746,7 +730,6 @@ static int lm3533_led_remove(struct platform_device *pdev)
 
 	lm3533_ctrlbank_disable(&led->cb);
 	led_classdev_unregister(&led->cdev);
-	flush_work(&led->work);
 
 	return 0;
 }
@@ -760,7 +743,6 @@ static void lm3533_led_shutdown(struct platform_device *pdev)
 
 	lm3533_ctrlbank_disable(&led->cb);
 	lm3533_led_set(&led->cdev, LED_OFF);		/* disable blink */
-	flush_work(&led->work);
 }
 
 static struct platform_driver lm3533_led_driver = {
diff --git a/drivers/leds/leds-lm355x.c b/drivers/leds/leds-lm355x.c
index 48872997d6b4..6cb94f9a2f3f 100644
--- a/drivers/leds/leds-lm355x.c
+++ b/drivers/leds/leds-lm355x.c
@@ -16,7 +16,6 @@
 #include <linux/platform_device.h>
 #include <linux/fs.h>
 #include <linux/regmap.h>
-#include <linux/workqueue.h>
 #include <linux/platform_data/leds-lm355x.h>
 
 enum lm355x_type {
@@ -59,14 +58,6 @@ struct lm355x_chip_data {
 	struct led_classdev cdev_torch;
 	struct led_classdev cdev_indicator;
 
-	struct work_struct work_flash;
-	struct work_struct work_torch;
-	struct work_struct work_indicator;
-
-	u8 br_flash;
-	u8 br_torch;
-	u8 br_indicator;
-
 	struct lm355x_platform_data *pdata;
 	struct regmap *regmap;
 	struct mutex lock;
@@ -204,7 +195,7 @@ out:
 }
 
 /* chip control */
-static void lm355x_control(struct lm355x_chip_data *chip,
+static int lm355x_control(struct lm355x_chip_data *chip,
 			   u8 brightness, enum lm355x_mode opmode)
 {
 	int ret;
@@ -301,7 +292,7 @@ static void lm355x_control(struct lm355x_chip_data *chip,
 	case MODE_SHDN:
 		break;
 	default:
-		return;
+		return -EINVAL;
 	}
 	/* operation mode control */
 	ret = regmap_update_bits(chip->regmap, preg[REG_OPMODE].regno,
@@ -309,73 +300,55 @@ static void lm355x_control(struct lm355x_chip_data *chip,
 				 opmode << preg[REG_OPMODE].shift);
 	if (ret < 0)
 		goto out;
-	return;
+	return ret;
 out:
 	dev_err(chip->dev, "%s:i2c access fail to register\n", __func__);
-	return;
+	return ret;
 }
 
 /* torch */
-static void lm355x_deferred_torch_brightness_set(struct work_struct *work)
-{
-	struct lm355x_chip_data *chip =
-	    container_of(work, struct lm355x_chip_data, work_torch);
 
-	mutex_lock(&chip->lock);
-	lm355x_control(chip, chip->br_torch, MODE_TORCH);
-	mutex_unlock(&chip->lock);
-}
-
-static void lm355x_torch_brightness_set(struct led_classdev *cdev,
+static int lm355x_torch_brightness_set(struct led_classdev *cdev,
 					enum led_brightness brightness)
 {
 	struct lm355x_chip_data *chip =
 	    container_of(cdev, struct lm355x_chip_data, cdev_torch);
-
-	chip->br_torch = brightness;
-	schedule_work(&chip->work_torch);
-}
-
-/* flash */
-static void lm355x_deferred_strobe_brightness_set(struct work_struct *work)
-{
-	struct lm355x_chip_data *chip =
-	    container_of(work, struct lm355x_chip_data, work_flash);
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lm355x_control(chip, chip->br_flash, MODE_FLASH);
+	ret = lm355x_control(chip, brightness, MODE_TORCH);
 	mutex_unlock(&chip->lock);
+	return ret;
 }
 
-static void lm355x_strobe_brightness_set(struct led_classdev *cdev,
+/* flash */
+
+static int lm355x_strobe_brightness_set(struct led_classdev *cdev,
 					 enum led_brightness brightness)
 {
 	struct lm355x_chip_data *chip =
 	    container_of(cdev, struct lm355x_chip_data, cdev_flash);
-
-	chip->br_flash = brightness;
-	schedule_work(&chip->work_flash);
-}
-
-/* indicator */
-static void lm355x_deferred_indicator_brightness_set(struct work_struct *work)
-{
-	struct lm355x_chip_data *chip =
-	    container_of(work, struct lm355x_chip_data, work_indicator);
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lm355x_control(chip, chip->br_indicator, MODE_INDIC);
+	ret = lm355x_control(chip, brightness, MODE_FLASH);
 	mutex_unlock(&chip->lock);
+	return ret;
 }
 
-static void lm355x_indicator_brightness_set(struct led_classdev *cdev,
+/* indicator */
+
+static int lm355x_indicator_brightness_set(struct led_classdev *cdev,
 					    enum led_brightness brightness)
 {
 	struct lm355x_chip_data *chip =
 	    container_of(cdev, struct lm355x_chip_data, cdev_indicator);
+	int ret;
 
-	chip->br_indicator = brightness;
-	schedule_work(&chip->work_indicator);
+	mutex_lock(&chip->lock);
+	ret = lm355x_control(chip, brightness, MODE_INDIC);
+	mutex_unlock(&chip->lock);
+	return ret;
 }
 
 /* indicator pattern only for lm3556*/
@@ -479,34 +452,31 @@ static int lm355x_probe(struct i2c_client *client,
 		goto err_out;
 
 	/* flash */
-	INIT_WORK(&chip->work_flash, lm355x_deferred_strobe_brightness_set);
 	chip->cdev_flash.name = "flash";
 	chip->cdev_flash.max_brightness = 16;
-	chip->cdev_flash.brightness_set = lm355x_strobe_brightness_set;
+	chip->cdev_flash.brightness_set_blocking = lm355x_strobe_brightness_set;
 	chip->cdev_flash.default_trigger = "flash";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_flash);
 	if (err < 0)
 		goto err_out;
 	/* torch */
-	INIT_WORK(&chip->work_torch, lm355x_deferred_torch_brightness_set);
 	chip->cdev_torch.name = "torch";
 	chip->cdev_torch.max_brightness = 8;
-	chip->cdev_torch.brightness_set = lm355x_torch_brightness_set;
+	chip->cdev_torch.brightness_set_blocking = lm355x_torch_brightness_set;
 	chip->cdev_torch.default_trigger = "torch";
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_torch);
 	if (err < 0)
 		goto err_create_torch_file;
 	/* indicator */
-	INIT_WORK(&chip->work_indicator,
-		  lm355x_deferred_indicator_brightness_set);
 	chip->cdev_indicator.name = "indicator";
 	if (id->driver_data == CHIP_LM3554)
 		chip->cdev_indicator.max_brightness = 4;
 	else
 		chip->cdev_indicator.max_brightness = 8;
-	chip->cdev_indicator.brightness_set = lm355x_indicator_brightness_set;
+	chip->cdev_indicator.brightness_set_blocking =
+					lm355x_indicator_brightness_set;
 	/* indicator pattern control only for LM3556 */
 	if (id->driver_data == CHIP_LM3556)
 		chip->cdev_indicator.groups = lm355x_indicator_groups;
@@ -534,11 +504,8 @@ static int lm355x_remove(struct i2c_client *client)
 
 	regmap_write(chip->regmap, preg[REG_OPMODE].regno, 0);
 	led_classdev_unregister(&chip->cdev_indicator);
-	flush_work(&chip->work_indicator);
 	led_classdev_unregister(&chip->cdev_torch);
-	flush_work(&chip->work_torch);
 	led_classdev_unregister(&chip->cdev_flash);
-	flush_work(&chip->work_flash);
 	dev_info(&client->dev, "%s is removed\n", lm355x_name[chip->type]);
 
 	return 0;
diff --git a/drivers/leds/leds-lm3642.c b/drivers/leds/leds-lm3642.c
index 02ebe342f5af..cada0848db7b 100644
--- a/drivers/leds/leds-lm3642.c
+++ b/drivers/leds/leds-lm3642.c
@@ -15,7 +15,6 @@
 #include <linux/platform_device.h>
 #include <linux/fs.h>
 #include <linux/regmap.h>
-#include <linux/workqueue.h>
 #include <linux/platform_data/leds-lm3642.h>
 
 #define	REG_FILT_TIME			(0x0)
@@ -73,10 +72,6 @@ struct lm3642_chip_data {
 	struct led_classdev cdev_torch;
 	struct led_classdev cdev_indicator;
 
-	struct work_struct work_flash;
-	struct work_struct work_torch;
-	struct work_struct work_indicator;
-
 	u8 br_flash;
 	u8 br_torch;
 	u8 br_indicator;
@@ -209,24 +204,18 @@ out_strtoint:
 
 static DEVICE_ATTR(torch_pin, S_IWUSR, NULL, lm3642_torch_pin_store);
 
-static void lm3642_deferred_torch_brightness_set(struct work_struct *work)
-{
-	struct lm3642_chip_data *chip =
-	    container_of(work, struct lm3642_chip_data, work_torch);
-
-	mutex_lock(&chip->lock);
-	lm3642_control(chip, chip->br_torch, MODES_TORCH);
-	mutex_unlock(&chip->lock);
-}
-
-static void lm3642_torch_brightness_set(struct led_classdev *cdev,
+static int lm3642_torch_brightness_set(struct led_classdev *cdev,
 					enum led_brightness brightness)
 {
 	struct lm3642_chip_data *chip =
 	    container_of(cdev, struct lm3642_chip_data, cdev_torch);
+	int ret;
 
+	mutex_lock(&chip->lock);
 	chip->br_torch = brightness;
-	schedule_work(&chip->work_torch);
+	ret = lm3642_control(chip, chip->br_torch, MODES_TORCH);
+	mutex_unlock(&chip->lock);
+	return ret;
 }
 
 /* flash */
@@ -266,45 +255,33 @@ out_strtoint:
 
 static DEVICE_ATTR(strobe_pin, S_IWUSR, NULL, lm3642_strobe_pin_store);
 
-static void lm3642_deferred_strobe_brightness_set(struct work_struct *work)
-{
-	struct lm3642_chip_data *chip =
-	    container_of(work, struct lm3642_chip_data, work_flash);
-
-	mutex_lock(&chip->lock);
-	lm3642_control(chip, chip->br_flash, MODES_FLASH);
-	mutex_unlock(&chip->lock);
-}
-
-static void lm3642_strobe_brightness_set(struct led_classdev *cdev,
+static int lm3642_strobe_brightness_set(struct led_classdev *cdev,
 					 enum led_brightness brightness)
 {
 	struct lm3642_chip_data *chip =
 	    container_of(cdev, struct lm3642_chip_data, cdev_flash);
-
-	chip->br_flash = brightness;
-	schedule_work(&chip->work_flash);
-}
-
-/* indicator */
-static void lm3642_deferred_indicator_brightness_set(struct work_struct *work)
-{
-	struct lm3642_chip_data *chip =
-	    container_of(work, struct lm3642_chip_data, work_indicator);
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lm3642_control(chip, chip->br_indicator, MODES_INDIC);
+	chip->br_flash = brightness;
+	ret = lm3642_control(chip, chip->br_flash, MODES_FLASH);
 	mutex_unlock(&chip->lock);
+	return ret;
 }
 
-static void lm3642_indicator_brightness_set(struct led_classdev *cdev,
+/* indicator */
+static int lm3642_indicator_brightness_set(struct led_classdev *cdev,
 					    enum led_brightness brightness)
 {
 	struct lm3642_chip_data *chip =
 	    container_of(cdev, struct lm3642_chip_data, cdev_indicator);
+	int ret;
 
+	mutex_lock(&chip->lock);
 	chip->br_indicator = brightness;
-	schedule_work(&chip->work_indicator);
+	ret = lm3642_control(chip, chip->br_indicator, MODES_INDIC);
+	mutex_unlock(&chip->lock);
+	return ret;
 }
 
 static const struct regmap_config lm3642_regmap = {
@@ -371,10 +348,9 @@ static int lm3642_probe(struct i2c_client *client,
 		goto err_out;
 
 	/* flash */
-	INIT_WORK(&chip->work_flash, lm3642_deferred_strobe_brightness_set);
 	chip->cdev_flash.name = "flash";
 	chip->cdev_flash.max_brightness = 16;
-	chip->cdev_flash.brightness_set = lm3642_strobe_brightness_set;
+	chip->cdev_flash.brightness_set_blocking = lm3642_strobe_brightness_set;
 	chip->cdev_flash.default_trigger = "flash";
 	chip->cdev_flash.groups = lm3642_flash_groups,
 	err = led_classdev_register((struct device *)
@@ -385,10 +361,9 @@ static int lm3642_probe(struct i2c_client *client,
 	}
 
 	/* torch */
-	INIT_WORK(&chip->work_torch, lm3642_deferred_torch_brightness_set);
 	chip->cdev_torch.name = "torch";
 	chip->cdev_torch.max_brightness = 8;
-	chip->cdev_torch.brightness_set = lm3642_torch_brightness_set;
+	chip->cdev_torch.brightness_set_blocking = lm3642_torch_brightness_set;
 	chip->cdev_torch.default_trigger = "torch";
 	chip->cdev_torch.groups = lm3642_torch_groups,
 	err = led_classdev_register((struct device *)
@@ -399,11 +374,10 @@ static int lm3642_probe(struct i2c_client *client,
 	}
 
 	/* indicator */
-	INIT_WORK(&chip->work_indicator,
-		  lm3642_deferred_indicator_brightness_set);
 	chip->cdev_indicator.name = "indicator";
 	chip->cdev_indicator.max_brightness = 8;
-	chip->cdev_indicator.brightness_set = lm3642_indicator_brightness_set;
+	chip->cdev_indicator.brightness_set_blocking =
+						lm3642_indicator_brightness_set;
 	err = led_classdev_register((struct device *)
 				    &client->dev, &chip->cdev_indicator);
 	if (err < 0) {
@@ -427,11 +401,8 @@ static int lm3642_remove(struct i2c_client *client)
 	struct lm3642_chip_data *chip = i2c_get_clientdata(client);
 
 	led_classdev_unregister(&chip->cdev_indicator);
-	flush_work(&chip->work_indicator);
 	led_classdev_unregister(&chip->cdev_torch);
-	flush_work(&chip->work_torch);
 	led_classdev_unregister(&chip->cdev_flash);
-	flush_work(&chip->work_flash);
 	regmap_write(chip->regmap, REG_ENABLE, 0);
 	return 0;
 }
diff --git a/drivers/leds/leds-lp3944.c b/drivers/leds/leds-lp3944.c
index 53144fb96167..6c758aea1bbd 100644
--- a/drivers/leds/leds-lp3944.c
+++ b/drivers/leds/leds-lp3944.c
@@ -31,7 +31,6 @@
 #include <linux/slab.h>
 #include <linux/leds.h>
 #include <linux/mutex.h>
-#include <linux/workqueue.h>
 #include <linux/leds-lp3944.h>
 
 /* Read Only Registers */
@@ -68,10 +67,8 @@
 struct lp3944_led_data {
 	u8 id;
 	enum lp3944_type type;
-	enum lp3944_status status;
 	struct led_classdev ldev;
 	struct i2c_client *client;
-	struct work_struct work;
 };
 
 struct lp3944_data {
@@ -275,13 +272,12 @@ static int lp3944_led_set_blink(struct led_classdev *led_cdev,
 	dev_dbg(&led->client->dev, "%s: OK hardware accelerated blink!\n",
 		__func__);
 
-	led->status = LP3944_LED_STATUS_DIM0;
-	schedule_work(&led->work);
+	lp3944_led_set(led, LP3944_LED_STATUS_DIM0);
 
 	return 0;
 }
 
-static void lp3944_led_set_brightness(struct led_classdev *led_cdev,
+static int lp3944_led_set_brightness(struct led_classdev *led_cdev,
 				      enum led_brightness brightness)
 {
 	struct lp3944_led_data *led = ldev_to_led(led_cdev);
@@ -289,16 +285,7 @@ static void lp3944_led_set_brightness(struct led_classdev *led_cdev,
 	dev_dbg(&led->client->dev, "%s: %s, %d\n",
 		__func__, led_cdev->name, brightness);
 
-	led->status = !!brightness;
-	schedule_work(&led->work);
-}
-
-static void lp3944_led_work(struct work_struct *work)
-{
-	struct lp3944_led_data *led;
-
-	led = container_of(work, struct lp3944_led_data, work);
-	lp3944_led_set(led, led->status);
+	return lp3944_led_set(led, !!brightness);
 }
 
 static int lp3944_configure(struct i2c_client *client,
@@ -318,14 +305,13 @@ static int lp3944_configure(struct i2c_client *client,
 		case LP3944_LED_TYPE_LED:
 		case LP3944_LED_TYPE_LED_INVERTED:
 			led->type = pled->type;
-			led->status = pled->status;
 			led->ldev.name = pled->name;
 			led->ldev.max_brightness = 1;
-			led->ldev.brightness_set = lp3944_led_set_brightness;
+			led->ldev.brightness_set_blocking =
+						lp3944_led_set_brightness;
 			led->ldev.blink_set = lp3944_led_set_blink;
 			led->ldev.flags = LED_CORE_SUSPENDRESUME;
 
-			INIT_WORK(&led->work, lp3944_led_work);
 			err = led_classdev_register(&client->dev, &led->ldev);
 			if (err < 0) {
 				dev_err(&client->dev,
@@ -336,14 +322,14 @@ static int lp3944_configure(struct i2c_client *client,
 
 			/* to expose the default value to userspace */
 			led->ldev.brightness =
-					(enum led_brightness) led->status;
+					(enum led_brightness) pled->status;
 
 			/* Set the default led status */
-			err = lp3944_led_set(led, led->status);
+			err = lp3944_led_set(led, pled->status);
 			if (err < 0) {
 				dev_err(&client->dev,
 					"%s couldn't set STATUS %d\n",
-					led->ldev.name, led->status);
+					led->ldev.name, pled->status);
 				goto exit;
 			}
 			break;
@@ -364,7 +350,6 @@ exit:
 			case LP3944_LED_TYPE_LED:
 			case LP3944_LED_TYPE_LED_INVERTED:
 				led_classdev_unregister(&data->leds[i].ldev);
-				cancel_work_sync(&data->leds[i].work);
 				break;
 
 			case LP3944_LED_TYPE_NONE:
@@ -424,7 +409,6 @@ static int lp3944_remove(struct i2c_client *client)
 		case LP3944_LED_TYPE_LED:
 		case LP3944_LED_TYPE_LED_INVERTED:
 			led_classdev_unregister(&data->leds[i].ldev);
-			cancel_work_sync(&data->leds[i].work);
 			break;
 
 		case LP3944_LED_TYPE_NONE:
diff --git a/drivers/leds/leds-lp5521.c b/drivers/leds/leds-lp5521.c
index 63a92542c8cb..549b315ca8fe 100644
--- a/drivers/leds/leds-lp5521.c
+++ b/drivers/leds/leds-lp5521.c
@@ -362,16 +362,17 @@ static int lp5521_run_selftest(struct lp55xx_chip *chip, char *buf)
 	return 0;
 }
 
-static void lp5521_led_brightness_work(struct work_struct *work)
+static int lp5521_led_brightness(struct lp55xx_led *led)
 {
-	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
-					      brightness_work);
 	struct lp55xx_chip *chip = led->chip;
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lp55xx_write(chip, LP5521_REG_LED_PWM_BASE + led->chan_nr,
+	ret = lp55xx_write(chip, LP5521_REG_LED_PWM_BASE + led->chan_nr,
 		led->brightness);
 	mutex_unlock(&chip->lock);
+
+	return ret;
 }
 
 static ssize_t show_engine_mode(struct device *dev,
@@ -501,7 +502,7 @@ static struct lp55xx_device_config lp5521_cfg = {
 	},
 	.max_channel  = LP5521_MAX_LEDS,
 	.post_init_device   = lp5521_post_init_device,
-	.brightness_work_fn = lp5521_led_brightness_work,
+	.brightness_fn      = lp5521_led_brightness,
 	.set_led_current    = lp5521_set_led_current,
 	.firmware_cb        = lp5521_firmware_loaded,
 	.run_engine         = lp5521_run_engine,
diff --git a/drivers/leds/leds-lp5523.c b/drivers/leds/leds-lp5523.c
index 1d0187f42941..c5b30f06218a 100644
--- a/drivers/leds/leds-lp5523.c
+++ b/drivers/leds/leds-lp5523.c
@@ -802,16 +802,16 @@ leave:
 	return ret;
 }
 
-static void lp5523_led_brightness_work(struct work_struct *work)
+static int lp5523_led_brightness(struct lp55xx_led *led)
 {
-	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
-					      brightness_work);
 	struct lp55xx_chip *chip = led->chip;
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lp55xx_write(chip, LP5523_REG_LED_PWM_BASE + led->chan_nr,
+	ret = lp55xx_write(chip, LP5523_REG_LED_PWM_BASE + led->chan_nr,
 		     led->brightness);
 	mutex_unlock(&chip->lock);
+	return ret;
 }
 
 static LP55XX_DEV_ATTR_RW(engine1_mode, show_engine1_mode, store_engine1_mode);
@@ -867,7 +867,7 @@ static struct lp55xx_device_config lp5523_cfg = {
 	},
 	.max_channel  = LP5523_MAX_LEDS,
 	.post_init_device   = lp5523_post_init_device,
-	.brightness_work_fn = lp5523_led_brightness_work,
+	.brightness_fn      = lp5523_led_brightness,
 	.set_led_current    = lp5523_set_led_current,
 	.firmware_cb        = lp5523_firmware_loaded,
 	.run_engine         = lp5523_run_engine,
diff --git a/drivers/leds/leds-lp5562.c b/drivers/leds/leds-lp5562.c
index 0360c59dbdc9..b75333803a63 100644
--- a/drivers/leds/leds-lp5562.c
+++ b/drivers/leds/leds-lp5562.c
@@ -311,10 +311,8 @@ static int lp5562_post_init_device(struct lp55xx_chip *chip)
 	return 0;
 }
 
-static void lp5562_led_brightness_work(struct work_struct *work)
+static int lp5562_led_brightness(struct lp55xx_led *led)
 {
-	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
-					      brightness_work);
 	struct lp55xx_chip *chip = led->chip;
 	u8 addr[] = {
 		LP5562_REG_R_PWM,
@@ -322,10 +320,13 @@ static void lp5562_led_brightness_work(struct work_struct *work)
 		LP5562_REG_B_PWM,
 		LP5562_REG_W_PWM,
 	};
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lp55xx_write(chip, addr[led->chan_nr], led->brightness);
+	ret = lp55xx_write(chip, addr[led->chan_nr], led->brightness);
 	mutex_unlock(&chip->lock);
+
+	return ret;
 }
 
 static void lp5562_write_program_memory(struct lp55xx_chip *chip,
@@ -503,7 +504,7 @@ static struct lp55xx_device_config lp5562_cfg = {
 	},
 	.post_init_device   = lp5562_post_init_device,
 	.set_led_current    = lp5562_set_led_current,
-	.brightness_work_fn = lp5562_led_brightness_work,
+	.brightness_fn      = lp5562_led_brightness,
 	.run_engine         = lp5562_run_engine,
 	.firmware_cb        = lp5562_firmware_loaded,
 	.dev_attr_group     = &lp5562_group,
diff --git a/drivers/leds/leds-lp55xx-common.c b/drivers/leds/leds-lp55xx-common.c
index 59b76833f0d3..5377f22ff994 100644
--- a/drivers/leds/leds-lp55xx-common.c
+++ b/drivers/leds/leds-lp55xx-common.c
@@ -134,13 +134,14 @@ static struct attribute *lp55xx_led_attrs[] = {
 };
 ATTRIBUTE_GROUPS(lp55xx_led);
 
-static void lp55xx_set_brightness(struct led_classdev *cdev,
+static int lp55xx_set_brightness(struct led_classdev *cdev,
 			     enum led_brightness brightness)
 {
 	struct lp55xx_led *led = cdev_to_lp55xx_led(cdev);
+	struct lp55xx_device_config *cfg = led->chip->cfg;
 
 	led->brightness = (u8)brightness;
-	schedule_work(&led->brightness_work);
+	return cfg->brightness_fn(led);
 }
 
 static int lp55xx_init_led(struct lp55xx_led *led,
@@ -172,7 +173,7 @@ static int lp55xx_init_led(struct lp55xx_led *led,
 		return -EINVAL;
 	}
 
-	led->cdev.brightness_set = lp55xx_set_brightness;
+	led->cdev.brightness_set_blocking = lp55xx_set_brightness;
 	led->cdev.groups = lp55xx_led_groups;
 
 	if (pdata->led_config[chan].name) {
@@ -464,7 +465,7 @@ int lp55xx_register_leds(struct lp55xx_led *led, struct lp55xx_chip *chip)
 	int ret;
 	int i;
 
-	if (!cfg->brightness_work_fn) {
+	if (!cfg->brightness_fn) {
 		dev_err(&chip->cl->dev, "empty brightness configuration\n");
 		return -EINVAL;
 	}
@@ -481,8 +482,6 @@ int lp55xx_register_leds(struct lp55xx_led *led, struct lp55xx_chip *chip)
 		if (ret)
 			goto err_init_led;
 
-		INIT_WORK(&each->brightness_work, cfg->brightness_work_fn);
-
 		chip->num_leds++;
 		each->chip = chip;
 
@@ -507,7 +506,6 @@ void lp55xx_unregister_leds(struct lp55xx_led *led, struct lp55xx_chip *chip)
 	for (i = 0; i < chip->num_leds; i++) {
 		each = led + i;
 		led_classdev_unregister(&each->cdev);
-		flush_work(&each->brightness_work);
 	}
 }
 EXPORT_SYMBOL_GPL(lp55xx_unregister_leds);
diff --git a/drivers/leds/leds-lp55xx-common.h b/drivers/leds/leds-lp55xx-common.h
index c7f1e6155001..abf1fb5da37d 100644
--- a/drivers/leds/leds-lp55xx-common.h
+++ b/drivers/leds/leds-lp55xx-common.h
@@ -95,7 +95,7 @@ struct lp55xx_reg {
  * @enable             : Chip specific enable command
  * @max_channel        : Maximum number of channels
  * @post_init_device   : Chip specific initialization code
- * @brightness_work_fn : Brightness work function
+ * @brightness_fn      : Brightness function
  * @set_led_current    : LED current set function
  * @firmware_cb        : Call function when the firmware is loaded
  * @run_engine         : Run internal engine for pattern
@@ -110,7 +110,7 @@ struct lp55xx_device_config {
 	int (*post_init_device) (struct lp55xx_chip *chip);
 
 	/* access brightness register */
-	void (*brightness_work_fn)(struct work_struct *work);
+	int (*brightness_fn)(struct lp55xx_led *led);
 
 	/* current setting function */
 	void (*set_led_current) (struct lp55xx_led *led, u8 led_current);
@@ -164,7 +164,6 @@ struct lp55xx_chip {
  * @cdev            : LED class device
  * @led_current     : Current setting at each led channel
  * @max_current     : Maximun current at each led channel
- * @brightness_work : Workqueue for brightness control
  * @brightness      : Brightness value
  * @chip            : The lp55xx chip data
  */
@@ -173,7 +172,6 @@ struct lp55xx_led {
 	struct led_classdev cdev;
 	u8 led_current;
 	u8 max_current;
-	struct work_struct brightness_work;
 	u8 brightness;
 	struct lp55xx_chip *chip;
 };
diff --git a/drivers/leds/leds-lp8501.c b/drivers/leds/leds-lp8501.c
index 3f54f6f2b821..3f9675bd214a 100644
--- a/drivers/leds/leds-lp8501.c
+++ b/drivers/leds/leds-lp8501.c
@@ -272,16 +272,17 @@ static void lp8501_firmware_loaded(struct lp55xx_chip *chip)
 	lp8501_update_program_memory(chip, fw->data, fw->size);
 }
 
-static void lp8501_led_brightness_work(struct work_struct *work)
+static int lp8501_led_brightness(struct lp55xx_led *led)
 {
-	struct lp55xx_led *led = container_of(work, struct lp55xx_led,
-					      brightness_work);
 	struct lp55xx_chip *chip = led->chip;
+	int ret;
 
 	mutex_lock(&chip->lock);
-	lp55xx_write(chip, LP8501_REG_LED_PWM_BASE + led->chan_nr,
+	ret = lp55xx_write(chip, LP8501_REG_LED_PWM_BASE + led->chan_nr,
 		     led->brightness);
 	mutex_unlock(&chip->lock);
+
+	return ret;
 }
 
 /* Chip specific configurations */
@@ -296,7 +297,7 @@ static struct lp55xx_device_config lp8501_cfg = {
 	},
 	.max_channel  = LP8501_MAX_LEDS,
 	.post_init_device   = lp8501_post_init_device,
-	.brightness_work_fn = lp8501_led_brightness_work,
+	.brightness_fn      = lp8501_led_brightness,
 	.set_led_current    = lp8501_set_led_current,
 	.firmware_cb        = lp8501_firmware_loaded,
 	.run_engine         = lp8501_run_engine,
diff --git a/drivers/leds/leds-lp8788.c b/drivers/leds/leds-lp8788.c
index 3409f03c1fa8..0eee38fc0565 100644
--- a/drivers/leds/leds-lp8788.c
+++ b/drivers/leds/leds-lp8788.c
@@ -26,10 +26,8 @@
 struct lp8788_led {
 	struct lp8788 *lp;
 	struct mutex lock;
-	struct work_struct work;
 	struct led_classdev led_dev;
 	enum lp8788_isink_number isink_num;
-	enum led_brightness brightness;
 	int on;
 };
 
@@ -76,24 +74,29 @@ static int lp8788_led_init_device(struct lp8788_led *led,
 	return lp8788_update_bits(led->lp, addr, mask, val);
 }
 
-static void lp8788_led_enable(struct lp8788_led *led,
+static int lp8788_led_enable(struct lp8788_led *led,
 			enum lp8788_isink_number num, int on)
 {
+	int ret;
+
 	u8 mask = 1 << num;
 	u8 val = on << num;
 
-	if (lp8788_update_bits(led->lp, LP8788_ISINK_CTRL, mask, val))
-		return;
+	ret = lp8788_update_bits(led->lp, LP8788_ISINK_CTRL, mask, val);
+	if (ret == 0)
+		led->on = on;
 
-	led->on = on;
+	return ret;
 }
 
-static void lp8788_led_work(struct work_struct *work)
+static int lp8788_brightness_set(struct led_classdev *led_cdev,
+				enum led_brightness val)
 {
-	struct lp8788_led *led = container_of(work, struct lp8788_led, work);
+	struct lp8788_led *led =
+			container_of(led_cdev, struct lp8788_led, led_dev);
+
 	enum lp8788_isink_number num = led->isink_num;
-	int enable;
-	u8 val = led->brightness;
+	int enable, ret;
 
 	mutex_lock(&led->lock);
 
@@ -101,28 +104,21 @@ static void lp8788_led_work(struct work_struct *work)
 	case LP8788_ISINK_1:
 	case LP8788_ISINK_2:
 	case LP8788_ISINK_3:
-		lp8788_write_byte(led->lp, lp8788_pwm_addr[num], val);
+		ret = lp8788_write_byte(led->lp, lp8788_pwm_addr[num], val);
+		if (ret < 0)
+			goto unlock;
 		break;
 	default:
 		mutex_unlock(&led->lock);
-		return;
+		return -EINVAL;
 	}
 
 	enable = (val > 0) ? 1 : 0;
 	if (enable != led->on)
-		lp8788_led_enable(led, num, enable);
-
+		ret = lp8788_led_enable(led, num, enable);
+unlock:
 	mutex_unlock(&led->lock);
-}
-
-static void lp8788_brightness_set(struct led_classdev *led_cdev,
-				enum led_brightness brt_val)
-{
-	struct lp8788_led *led =
-			container_of(led_cdev, struct lp8788_led, led_dev);
-
-	led->brightness = brt_val;
-	schedule_work(&led->work);
+	return ret;
 }
 
 static int lp8788_led_probe(struct platform_device *pdev)
@@ -139,7 +135,7 @@ static int lp8788_led_probe(struct platform_device *pdev)
 
 	led->lp = lp;
 	led->led_dev.max_brightness = MAX_BRIGHTNESS;
-	led->led_dev.brightness_set = lp8788_brightness_set;
+	led->led_dev.brightness_set_blocking = lp8788_brightness_set;
 
 	led_pdata = lp->pdata ? lp->pdata->led_pdata : NULL;
 
@@ -149,7 +145,6 @@ static int lp8788_led_probe(struct platform_device *pdev)
 		led->led_dev.name = led_pdata->name;
 
 	mutex_init(&led->lock);
-	INIT_WORK(&led->work, lp8788_led_work);
 
 	platform_set_drvdata(pdev, led);
 
@@ -173,7 +168,6 @@ static int lp8788_led_remove(struct platform_device *pdev)
 	struct lp8788_led *led = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&led->led_dev);
-	flush_work(&led->work);
 
 	return 0;
 }
diff --git a/drivers/leds/leds-lp8860.c b/drivers/leds/leds-lp8860.c
index 79f084354e67..3e70775a2d54 100644
--- a/drivers/leds/leds-lp8860.c
+++ b/drivers/leds/leds-lp8860.c
@@ -91,26 +91,22 @@
 /**
  * struct lp8860_led -
  * @lock - Lock for reading/writing the device
- * @work - Work item used to off load the brightness register writes
  * @client - Pointer to the I2C client
  * @led_dev - led class device pointer
  * @regmap - Devices register map
  * @eeprom_regmap - EEPROM register map
  * @enable_gpio - VDDIO/EN gpio to enable communication interface
  * @regulator - LED supply regulator pointer
- * @brightness - Current brightness value requested
  * @label - LED label
 **/
 struct lp8860_led {
 	struct mutex lock;
-	struct work_struct work;
 	struct i2c_client *client;
 	struct led_classdev led_dev;
 	struct regmap *regmap;
 	struct regmap *eeprom_regmap;
 	struct gpio_desc *enable_gpio;
 	struct regulator *regulator;
-	enum led_brightness brightness;
 	const char *label;
 };
 
@@ -212,11 +208,13 @@ out:
 	return ret;
 }
 
-static void lp8860_led_brightness_work(struct work_struct *work)
+static int lp8860_brightness_set(struct led_classdev *led_cdev,
+				enum led_brightness brt_val)
 {
-	struct lp8860_led *led = container_of(work, struct lp8860_led, work);
+	struct lp8860_led *led =
+			container_of(led_cdev, struct lp8860_led, led_dev);
+	int disp_brightness = brt_val * 255;
 	int ret;
-	int disp_brightness = led->brightness * 255;
 
 	mutex_lock(&led->lock);
 
@@ -241,16 +239,7 @@ static void lp8860_led_brightness_work(struct work_struct *work)
 	}
 out:
 	mutex_unlock(&led->lock);
-}
-
-static void lp8860_brightness_set(struct led_classdev *led_cdev,
-				enum led_brightness brt_val)
-{
-	struct lp8860_led *led =
-			container_of(led_cdev, struct lp8860_led, led_dev);
-
-	led->brightness = brt_val;
-	schedule_work(&led->work);
+	return ret;
 }
 
 static int lp8860_init(struct lp8860_led *led)
@@ -406,10 +395,9 @@ static int lp8860_probe(struct i2c_client *client,
 	led->client = client;
 	led->led_dev.name = led->label;
 	led->led_dev.max_brightness = LED_FULL;
-	led->led_dev.brightness_set = lp8860_brightness_set;
+	led->led_dev.brightness_set_blocking = lp8860_brightness_set;
 
 	mutex_init(&led->lock);
-	INIT_WORK(&led->work, lp8860_led_brightness_work);
 
 	i2c_set_clientdata(client, led);
 
@@ -448,7 +436,6 @@ static int lp8860_remove(struct i2c_client *client)
 	int ret;
 
 	led_classdev_unregister(&led->led_dev);
-	cancel_work_sync(&led->work);
 
 	if (led->enable_gpio)
 		gpiod_direction_output(led->enable_gpio, 0);
diff --git a/drivers/leds/leds-lt3593.c b/drivers/leds/leds-lt3593.c
index 9f41124765cc..a7ff510cbdd0 100644
--- a/drivers/leds/leds-lt3593.c
+++ b/drivers/leds/leds-lt3593.c
@@ -19,7 +19,6 @@
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/leds.h>
-#include <linux/workqueue.h>
 #include <linux/delay.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -28,15 +27,14 @@
 struct lt3593_led_data {
 	struct led_classdev cdev;
 	unsigned gpio;
-	struct work_struct work;
-	u8 new_level;
 };
 
-static void lt3593_led_work(struct work_struct *work)
+static int lt3593_led_set(struct led_classdev *led_cdev,
+			   enum led_brightness value)
 {
-	int pulses;
 	struct lt3593_led_data *led_dat =
-		container_of(work, struct lt3593_led_data, work);
+		container_of(led_cdev, struct lt3593_led_data, cdev);
+	int pulses;
 
 	/*
 	 * The LT3593 resets its internal current level register to the maximum
@@ -47,18 +45,18 @@ static void lt3593_led_work(struct work_struct *work)
 	 * applied is to the output driver.
 	 */
 
-	if (led_dat->new_level == 0) {
+	if (value == 0) {
 		gpio_set_value_cansleep(led_dat->gpio, 0);
-		return;
+		return 0;
 	}
 
-	pulses = 32 - (led_dat->new_level * 32) / 255;
+	pulses = 32 - (value * 32) / 255;
 
 	if (pulses == 0) {
 		gpio_set_value_cansleep(led_dat->gpio, 0);
 		mdelay(1);
 		gpio_set_value_cansleep(led_dat->gpio, 1);
-		return;
+		return 0;
 	}
 
 	gpio_set_value_cansleep(led_dat->gpio, 1);
@@ -69,16 +67,8 @@ static void lt3593_led_work(struct work_struct *work)
 		gpio_set_value_cansleep(led_dat->gpio, 1);
 		udelay(1);
 	}
-}
 
-static void lt3593_led_set(struct led_classdev *led_cdev,
-	enum led_brightness value)
-{
-	struct lt3593_led_data *led_dat =
-		container_of(led_cdev, struct lt3593_led_data, cdev);
-
-	led_dat->new_level = value;
-	schedule_work(&led_dat->work);
+	return 0;
 }
 
 static int create_lt3593_led(const struct gpio_led *template,
@@ -97,7 +87,7 @@ static int create_lt3593_led(const struct gpio_led *template,
 	led_dat->cdev.default_trigger = template->default_trigger;
 	led_dat->gpio = template->gpio;
 
-	led_dat->cdev.brightness_set = lt3593_led_set;
+	led_dat->cdev.brightness_set_blocking = lt3593_led_set;
 
 	state = (template->default_state == LEDS_GPIO_DEFSTATE_ON);
 	led_dat->cdev.brightness = state ? LED_FULL : LED_OFF;
@@ -111,8 +101,6 @@ static int create_lt3593_led(const struct gpio_led *template,
 	if (ret < 0)
 		return ret;
 
-	INIT_WORK(&led_dat->work, lt3593_led_work);
-
 	ret = led_classdev_register(parent, &led_dat->cdev);
 	if (ret < 0)
 		return ret;
@@ -129,7 +117,6 @@ static void delete_lt3593_led(struct lt3593_led_data *led)
 		return;
 
 	led_classdev_unregister(&led->cdev);
-	cancel_work_sync(&led->work);
 }
 
 static int lt3593_led_probe(struct platform_device *pdev)
diff --git a/drivers/leds/leds-max77693.c b/drivers/leds/leds-max77693.c
index afbb1409b2e2..1eb58ef6aefe 100644
--- a/drivers/leds/leds-max77693.c
+++ b/drivers/leds/leds-max77693.c
@@ -20,7 +20,6 @@
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <media/v4l2-flash-led-class.h>
 
 #define MODE_OFF		0
@@ -62,8 +61,6 @@ struct max77693_sub_led {
 	int fled_id;
 	/* corresponding LED Flash class device */
 	struct led_classdev_flash fled_cdev;
-	/* assures led-triggers compatibility */
-	struct work_struct work_brightness_set;
 	/* V4L2 Flash device */
 	struct v4l2_flash *v4l2_flash;
 
@@ -463,10 +460,14 @@ static int max77693_setup(struct max77693_led_device *led,
 	return max77693_set_mode_reg(led, MODE_OFF);
 }
 
-static int __max77693_led_brightness_set(struct max77693_led_device *led,
-					int fled_id, enum led_brightness value)
+/* LED subsystem callbacks */
+static int max77693_led_brightness_set(struct led_classdev *led_cdev,
+					enum led_brightness value)
 {
-	int ret;
+	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
+	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
+	struct max77693_led_device *led = sub_led_to_led(sub_led);
+	int fled_id = sub_led->fled_id, ret;
 
 	mutex_lock(&led->lock);
 
@@ -494,43 +495,8 @@ static int __max77693_led_brightness_set(struct max77693_led_device *led,
 			ret);
 unlock:
 	mutex_unlock(&led->lock);
-	return ret;
-}
-
-static void max77693_led_brightness_set_work(
-					struct work_struct *work)
-{
-	struct max77693_sub_led *sub_led =
-			container_of(work, struct max77693_sub_led,
-					work_brightness_set);
-	struct max77693_led_device *led = sub_led_to_led(sub_led);
-
-	__max77693_led_brightness_set(led, sub_led->fled_id,
-				sub_led->torch_brightness);
-}
-
-/* LED subsystem callbacks */
-
-static int max77693_led_brightness_set_sync(
-				struct led_classdev *led_cdev,
-				enum led_brightness value)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
-	struct max77693_led_device *led = sub_led_to_led(sub_led);
-
-	return __max77693_led_brightness_set(led, sub_led->fled_id, value);
-}
 
-static void max77693_led_brightness_set(
-				struct led_classdev *led_cdev,
-				enum led_brightness value)
-{
-	struct led_classdev_flash *fled_cdev = lcdev_to_flcdev(led_cdev);
-	struct max77693_sub_led *sub_led = flcdev_to_sub_led(fled_cdev);
-
-	sub_led->torch_brightness = value;
-	schedule_work(&sub_led->work_brightness_set);
+	return ret;
 }
 
 static int max77693_led_flash_brightness_set(
@@ -682,6 +648,7 @@ static int max77693_led_parse_dt(struct max77693_led_device *led,
 		if (sub_nodes[fled_id]) {
 			dev_err(dev,
 				"Conflicting \"led-sources\" DT properties\n");
+			of_node_put(child_node);
 			return -EINVAL;
 		}
 
@@ -931,16 +898,13 @@ static void max77693_init_fled_cdev(struct max77693_sub_led *sub_led,
 
 	led_cdev->name = led_cfg->label[fled_id];
 
-	led_cdev->brightness_set = max77693_led_brightness_set;
-	led_cdev->brightness_set_sync = max77693_led_brightness_set_sync;
+	led_cdev->brightness_set_blocking = max77693_led_brightness_set;
 	led_cdev->max_brightness = (led->iout_joint ?
 					led_cfg->iout_torch_max[FLED1] +
 					led_cfg->iout_torch_max[FLED2] :
 					led_cfg->iout_torch_max[fled_id]) /
 				   TORCH_IOUT_STEP;
 	led_cdev->flags |= LED_DEV_CAP_FLASH;
-	INIT_WORK(&sub_led->work_brightness_set,
-			max77693_led_brightness_set_work);
 
 	max77693_init_flash_settings(sub_led, led_cfg);
 
@@ -1062,13 +1026,11 @@ static int max77693_led_remove(struct platform_device *pdev)
 	if (led->iout_joint || max77693_fled_used(led, FLED1)) {
 		v4l2_flash_release(sub_leds[FLED1].v4l2_flash);
 		led_classdev_flash_unregister(&sub_leds[FLED1].fled_cdev);
-		cancel_work_sync(&sub_leds[FLED1].work_brightness_set);
 	}
 
 	if (!led->iout_joint && max77693_fled_used(led, FLED2)) {
 		v4l2_flash_release(sub_leds[FLED2].v4l2_flash);
 		led_classdev_flash_unregister(&sub_leds[FLED2].fled_cdev);
-		cancel_work_sync(&sub_leds[FLED2].work_brightness_set);
 	}
 
 	mutex_destroy(&led->lock);
diff --git a/drivers/leds/leds-max8997.c b/drivers/leds/leds-max8997.c
index c592aa5662bb..01b459069358 100644
--- a/drivers/leds/leds-max8997.c
+++ b/drivers/leds/leds-max8997.c
@@ -13,7 +13,6 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <linux/leds.h>
 #include <linux/mfd/max8997.h>
 #include <linux/mfd/max8997-private.h>
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index e2b847fe22a1..a2e4c1792e17 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -20,7 +20,6 @@
 #include <linux/platform_device.h>
 #include <linux/leds.h>
 #include <linux/of.h>
-#include <linux/workqueue.h>
 #include <linux/mfd/mc13xxx.h>
 
 struct mc13xxx_led_devtype {
@@ -32,8 +31,6 @@ struct mc13xxx_led_devtype {
 
 struct mc13xxx_led {
 	struct led_classdev	cdev;
-	struct work_struct	work;
-	enum led_brightness	new_brightness;
 	int			id;
 	struct mc13xxx_leds	*leds;
 };
@@ -55,9 +52,11 @@ static unsigned int mc13xxx_max_brightness(int id)
 	return 0x3f;
 }
 
-static void mc13xxx_led_work(struct work_struct *work)
+static int mc13xxx_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
 {
-	struct mc13xxx_led *led = container_of(work, struct mc13xxx_led, work);
+	struct mc13xxx_led *led =
+		container_of(led_cdev, struct mc13xxx_led, cdev);
 	struct mc13xxx_leds *leds = led->leds;
 	unsigned int reg, bank, off, shift;
 
@@ -105,19 +104,9 @@ static void mc13xxx_led_work(struct work_struct *work)
 		BUG();
 	}
 
-	mc13xxx_reg_rmw(leds->master, leds->devtype->ledctrl_base + reg,
+	return mc13xxx_reg_rmw(leds->master, leds->devtype->ledctrl_base + reg,
 			mc13xxx_max_brightness(led->id) << shift,
-			led->new_brightness << shift);
-}
-
-static void mc13xxx_led_set(struct led_classdev *led_cdev,
-			    enum led_brightness value)
-{
-	struct mc13xxx_led *led =
-		container_of(led_cdev, struct mc13xxx_led, cdev);
-
-	led->new_brightness = value;
-	schedule_work(&led->work);
+			value << shift);
 }
 
 #ifdef CONFIG_OF
@@ -257,11 +246,9 @@ static int __init mc13xxx_led_probe(struct platform_device *pdev)
 		leds->led[i].cdev.name = name;
 		leds->led[i].cdev.default_trigger = trig;
 		leds->led[i].cdev.flags = LED_CORE_SUSPENDRESUME;
-		leds->led[i].cdev.brightness_set = mc13xxx_led_set;
+		leds->led[i].cdev.brightness_set_blocking = mc13xxx_led_set;
 		leds->led[i].cdev.max_brightness = mc13xxx_max_brightness(id);
 
-		INIT_WORK(&leds->led[i].work, mc13xxx_led_work);
-
 		ret = led_classdev_register(dev->parent, &leds->led[i].cdev);
 		if (ret) {
 			dev_err(dev, "Failed to register LED %i\n", id);
@@ -270,10 +257,8 @@ static int __init mc13xxx_led_probe(struct platform_device *pdev)
 	}
 
 	if (ret)
-		while (--i >= 0) {
+		while (--i >= 0)
 			led_classdev_unregister(&leds->led[i].cdev);
-			cancel_work_sync(&leds->led[i].work);
-		}
 
 	return ret;
 }
@@ -283,10 +268,8 @@ static int mc13xxx_led_remove(struct platform_device *pdev)
 	struct mc13xxx_leds *leds = platform_get_drvdata(pdev);
 	int i;
 
-	for (i = 0; i < leds->num_leds; i++) {
+	for (i = 0; i < leds->num_leds; i++)
 		led_classdev_unregister(&leds->led[i].cdev);
-		cancel_work_sync(&leds->led[i].work);
-	}
 
 	return 0;
 }
diff --git a/drivers/leds/leds-ns2.c b/drivers/leds/leds-ns2.c
index a95a61220169..506b75b190e7 100644
--- a/drivers/leds/leds-ns2.c
+++ b/drivers/leds/leds-ns2.c
@@ -45,24 +45,12 @@ struct ns2_led_data {
 	unsigned		cmd;
 	unsigned		slow;
 	bool			can_sleep;
-	int			mode_index;
 	unsigned char		sata; /* True when SATA mode active. */
 	rwlock_t		rw_lock; /* Lock GPIOs. */
-	struct work_struct	work;
 	int			num_modes;
 	struct ns2_led_modval	*modval;
 };
 
-static void ns2_led_work(struct work_struct *work)
-{
-	struct ns2_led_data *led_dat =
-		container_of(work, struct ns2_led_data, work);
-	int i = led_dat->mode_index;
-
-	gpio_set_value_cansleep(led_dat->cmd, led_dat->modval[i].cmd_level);
-	gpio_set_value_cansleep(led_dat->slow, led_dat->modval[i].slow_level);
-}
-
 static int ns2_led_get_mode(struct ns2_led_data *led_dat,
 			    enum ns2_led_modes *mode)
 {
@@ -112,8 +100,8 @@ static void ns2_led_set_mode(struct ns2_led_data *led_dat,
 		goto exit_unlock;
 	}
 
-	led_dat->mode_index = i;
-	schedule_work(&led_dat->work);
+	gpio_set_value_cansleep(led_dat->cmd, led_dat->modval[i].cmd_level);
+	gpio_set_value_cansleep(led_dat->slow, led_dat->modval[i].slow_level);
 
 exit_unlock:
 	write_unlock_irqrestore(&led_dat->rw_lock, flags);
@@ -136,6 +124,13 @@ static void ns2_led_set(struct led_classdev *led_cdev,
 	ns2_led_set_mode(led_dat, mode);
 }
 
+static int ns2_led_set_blocking(struct led_classdev *led_cdev,
+			enum led_brightness value)
+{
+	ns2_led_set(led_cdev, value);
+	return 0;
+}
+
 static ssize_t ns2_led_sata_store(struct device *dev,
 				  struct device_attribute *attr,
 				  const char *buff, size_t count)
@@ -219,13 +214,16 @@ create_ns2_led(struct platform_device *pdev, struct ns2_led_data *led_dat,
 	led_dat->cdev.name = template->name;
 	led_dat->cdev.default_trigger = template->default_trigger;
 	led_dat->cdev.blink_set = NULL;
-	led_dat->cdev.brightness_set = ns2_led_set;
 	led_dat->cdev.flags |= LED_CORE_SUSPENDRESUME;
 	led_dat->cdev.groups = ns2_led_groups;
 	led_dat->cmd = template->cmd;
 	led_dat->slow = template->slow;
 	led_dat->can_sleep = gpio_cansleep(led_dat->cmd) |
 				gpio_cansleep(led_dat->slow);
+	if (led_dat->can_sleep)
+		led_dat->cdev.brightness_set_blocking = ns2_led_set_blocking;
+	else
+		led_dat->cdev.brightness_set = ns2_led_set;
 	led_dat->modval = template->modval;
 	led_dat->num_modes = template->num_modes;
 
@@ -238,8 +236,6 @@ create_ns2_led(struct platform_device *pdev, struct ns2_led_data *led_dat,
 	led_dat->cdev.brightness =
 		(mode == NS_V2_LED_OFF) ? LED_OFF : LED_FULL;
 
-	INIT_WORK(&led_dat->work, ns2_led_work);
-
 	ret = led_classdev_register(&pdev->dev, &led_dat->cdev);
 	if (ret < 0)
 		return ret;
@@ -250,7 +246,6 @@ create_ns2_led(struct platform_device *pdev, struct ns2_led_data *led_dat,
 static void delete_ns2_led(struct ns2_led_data *led_dat)
 {
 	led_classdev_unregister(&led_dat->cdev);
-	cancel_work_sync(&led_dat->work);
 }
 
 #ifdef CONFIG_OF_GPIO
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
index 5a6363d161a2..e3d3b1aaa9e0 100644
--- a/drivers/leds/leds-pca9532.c
+++ b/drivers/leds/leds-pca9532.c
@@ -158,7 +158,7 @@ static void pca9532_setled(struct pca9532_led *led)
 	mutex_unlock(&data->update_lock);
 }
 
-static void pca9532_set_brightness(struct led_classdev *led_cdev,
+static int pca9532_set_brightness(struct led_classdev *led_cdev,
 	enum led_brightness value)
 {
 	int err = 0;
@@ -172,9 +172,12 @@ static void pca9532_set_brightness(struct led_classdev *led_cdev,
 		led->state = PCA9532_PWM0; /* Thecus: hardcode one pwm */
 		err = pca9532_calcpwm(led->client, 0, 0, value);
 		if (err)
-			return; /* XXX: led api doesn't allow error code? */
+			return err;
 	}
-	schedule_work(&led->work);
+	if (led->state == PCA9532_PWM0)
+		pca9532_setpwm(led->client, 0);
+	pca9532_setled(led);
+	return err;
 }
 
 static int pca9532_set_blink(struct led_classdev *led_cdev,
@@ -198,7 +201,10 @@ static int pca9532_set_blink(struct led_classdev *led_cdev,
 	err = pca9532_calcpwm(client, 0, psc, led_cdev->brightness);
 	if (err)
 		return err;
-	schedule_work(&led->work);
+	if (led->state == PCA9532_PWM0)
+		pca9532_setpwm(led->client, 0);
+	pca9532_setled(led);
+
 	return 0;
 }
 
@@ -233,19 +239,10 @@ static void pca9532_input_work(struct work_struct *work)
 	mutex_unlock(&data->update_lock);
 }
 
-static void pca9532_led_work(struct work_struct *work)
-{
-	struct pca9532_led *led;
-	led = container_of(work, struct pca9532_led, work);
-	if (led->state == PCA9532_PWM0)
-		pca9532_setpwm(led->client, 0);
-	pca9532_setled(led);
-}
-
 #ifdef CONFIG_LEDS_PCA9532_GPIO
 static int pca9532_gpio_request_pin(struct gpio_chip *gc, unsigned offset)
 {
-	struct pca9532_data *data = container_of(gc, struct pca9532_data, gpio);
+	struct pca9532_data *data = gpiochip_get_data(gc);
 	struct pca9532_led *led = &data->leds[offset];
 
 	if (led->type == PCA9532_TYPE_GPIO)
@@ -256,7 +253,7 @@ static int pca9532_gpio_request_pin(struct gpio_chip *gc, unsigned offset)
 
 static void pca9532_gpio_set_value(struct gpio_chip *gc, unsigned offset, int val)
 {
-	struct pca9532_data *data = container_of(gc, struct pca9532_data, gpio);
+	struct pca9532_data *data = gpiochip_get_data(gc);
 	struct pca9532_led *led = &data->leds[offset];
 
 	if (val)
@@ -269,7 +266,7 @@ static void pca9532_gpio_set_value(struct gpio_chip *gc, unsigned offset, int va
 
 static int pca9532_gpio_get_value(struct gpio_chip *gc, unsigned offset)
 {
-	struct pca9532_data *data = container_of(gc, struct pca9532_data, gpio);
+	struct pca9532_data *data = gpiochip_get_data(gc);
 	unsigned char reg;
 
 	reg = i2c_smbus_read_byte_data(data->client, PCA9532_REG_INPUT(offset));
@@ -307,7 +304,6 @@ static int pca9532_destroy_devices(struct pca9532_data *data, int n_devs)
 			break;
 		case PCA9532_TYPE_LED:
 			led_classdev_unregister(&data->leds[i].ldev);
-			cancel_work_sync(&data->leds[i].work);
 			break;
 		case PCA9532_TYPE_N2100_BEEP:
 			if (data->idev != NULL) {
@@ -319,7 +315,7 @@ static int pca9532_destroy_devices(struct pca9532_data *data, int n_devs)
 	}
 
 #ifdef CONFIG_LEDS_PCA9532_GPIO
-	if (data->gpio.dev)
+	if (data->gpio.parent)
 		gpiochip_remove(&data->gpio);
 #endif
 
@@ -359,9 +355,9 @@ static int pca9532_configure(struct i2c_client *client,
 			led->name = pled->name;
 			led->ldev.name = led->name;
 			led->ldev.brightness = LED_OFF;
-			led->ldev.brightness_set = pca9532_set_brightness;
+			led->ldev.brightness_set_blocking =
+						pca9532_set_brightness;
 			led->ldev.blink_set = pca9532_set_blink;
-			INIT_WORK(&led->work, pca9532_led_work);
 			err = led_classdev_register(&client->dev, &led->ldev);
 			if (err < 0) {
 				dev_err(&client->dev,
@@ -413,13 +409,13 @@ static int pca9532_configure(struct i2c_client *client,
 		data->gpio.can_sleep = 1;
 		data->gpio.base = pdata->gpio_base;
 		data->gpio.ngpio = data->chip_info->num_leds;
-		data->gpio.dev = &client->dev;
+		data->gpio.parent = &client->dev;
 		data->gpio.owner = THIS_MODULE;
 
-		err = gpiochip_add(&data->gpio);
+		err = gpiochip_add_data(&data->gpio, data);
 		if (err) {
 			/* Use data->gpio.dev as a flag for freeing gpiochip */
-			data->gpio.dev = NULL;
+			data->gpio.parent = NULL;
 			dev_warn(&client->dev, "could not add gpiochip\n");
 		} else {
 			dev_info(&client->dev, "gpios %i...%i\n",
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index b775e1efecd3..840401ae9a4e 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -47,7 +47,6 @@
 #include <linux/leds.h>
 #include <linux/err.h>
 #include <linux/i2c.h>
-#include <linux/workqueue.h>
 #include <linux/slab.h>
 
 /* LED select registers determine the source that drives LED outputs */
@@ -110,8 +109,6 @@ struct pca955x {
 
 struct pca955x_led {
 	struct pca955x	*pca955x;
-	struct work_struct	work;
-	enum led_brightness	brightness;
 	struct led_classdev	led_cdev;
 	int			led_num;	/* 0 .. 15 potentially */
 	char			name[32];
@@ -193,7 +190,8 @@ static u8 pca955x_read_ls(struct i2c_client *client, int n)
 		pca95xx_num_input_regs(pca955x->chipdef->bits) + 4 + n);
 }
 
-static void pca955x_led_work(struct work_struct *work)
+static int pca955x_led_set(struct led_classdev *led_cdev,
+			    enum led_brightness value)
 {
 	struct pca955x_led *pca955x_led;
 	struct pca955x *pca955x;
@@ -201,7 +199,7 @@ static void pca955x_led_work(struct work_struct *work)
 	int chip_ls;	/* which LSx to use (0-3 potentially) */
 	int ls_led;	/* which set of bits within LSx to use (0-3) */
 
-	pca955x_led = container_of(work, struct pca955x_led, work);
+	pca955x_led = container_of(led_cdev, struct pca955x_led, led_cdev);
 	pca955x = pca955x_led->pca955x;
 
 	chip_ls = pca955x_led->led_num / 4;
@@ -211,7 +209,7 @@ static void pca955x_led_work(struct work_struct *work)
 
 	ls = pca955x_read_ls(pca955x->client, chip_ls);
 
-	switch (pca955x_led->brightness) {
+	switch (value) {
 	case LED_FULL:
 		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_LED_ON);
 		break;
@@ -230,7 +228,7 @@ static void pca955x_led_work(struct work_struct *work)
 		 * just turning off for all other values.
 		 */
 		pca955x_write_pwm(pca955x->client, 1,
-				255 - pca955x_led->brightness);
+				255 - value);
 		ls = pca955x_ledsel(ls, ls_led, PCA955X_LS_BLINK1);
 		break;
 	}
@@ -238,21 +236,8 @@ static void pca955x_led_work(struct work_struct *work)
 	pca955x_write_ls(pca955x->client, chip_ls, ls);
 
 	mutex_unlock(&pca955x->lock);
-}
-
-static void pca955x_led_set(struct led_classdev *led_cdev, enum led_brightness value)
-{
-	struct pca955x_led *pca955x;
-
-	pca955x = container_of(led_cdev, struct pca955x_led, led_cdev);
-
-	pca955x->brightness = value;
 
-	/*
-	 * Must use workqueue for the actual I/O since I2C operations
-	 * can sleep.
-	 */
-	schedule_work(&pca955x->work);
+	return 0;
 }
 
 static int pca955x_probe(struct i2c_client *client,
@@ -328,9 +313,7 @@ static int pca955x_probe(struct i2c_client *client,
 		}
 
 		pca955x_led->led_cdev.name = pca955x_led->name;
-		pca955x_led->led_cdev.brightness_set = pca955x_led_set;
-
-		INIT_WORK(&pca955x_led->work, pca955x_led_work);
+		pca955x_led->led_cdev.brightness_set_blocking = pca955x_led_set;
 
 		err = led_classdev_register(&client->dev,
 					&pca955x_led->led_cdev);
@@ -355,10 +338,8 @@ static int pca955x_probe(struct i2c_client *client,
 	return 0;
 
 exit:
-	while (i--) {
+	while (i--)
 		led_classdev_unregister(&pca955x->leds[i].led_cdev);
-		cancel_work_sync(&pca955x->leds[i].work);
-	}
 
 	return err;
 }
@@ -368,10 +349,8 @@ static int pca955x_remove(struct i2c_client *client)
 	struct pca955x *pca955x = i2c_get_clientdata(client);
 	int i;
 
-	for (i = 0; i < pca955x->chipdef->bits; i++) {
+	for (i = 0; i < pca955x->chipdef->bits; i++)
 		led_classdev_unregister(&pca955x->leds[i].led_cdev);
-		cancel_work_sync(&pca955x->leds[i].work);
-	}
 
 	return 0;
 }
diff --git a/drivers/leds/leds-pca963x.c b/drivers/leds/leds-pca963x.c
index 41f269fe0920..407eba11e187 100644
--- a/drivers/leds/leds-pca963x.c
+++ b/drivers/leds/leds-pca963x.c
@@ -32,7 +32,6 @@
 #include <linux/leds.h>
 #include <linux/err.h>
 #include <linux/i2c.h>
-#include <linux/workqueue.h>
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/platform_data/leds-pca963x.h>
@@ -96,11 +95,6 @@ static const struct i2c_device_id pca963x_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, pca963x_id);
 
-enum pca963x_cmd {
-	BRIGHTNESS_SET,
-	BLINK_SET,
-};
-
 struct pca963x_led;
 
 struct pca963x {
@@ -112,47 +106,52 @@ struct pca963x {
 
 struct pca963x_led {
 	struct pca963x *chip;
-	struct work_struct work;
-	enum led_brightness brightness;
 	struct led_classdev led_cdev;
 	int led_num; /* 0 .. 15 potentially */
-	enum pca963x_cmd cmd;
 	char name[32];
 	u8 gdc;
 	u8 gfrq;
 };
 
-static void pca963x_brightness_work(struct pca963x_led *pca963x)
+static int pca963x_brightness(struct pca963x_led *pca963x,
+			       enum led_brightness brightness)
 {
 	u8 ledout_addr = pca963x->chip->chipdef->ledout_base
 		+ (pca963x->led_num / 4);
 	u8 ledout;
 	int shift = 2 * (pca963x->led_num % 4);
 	u8 mask = 0x3 << shift;
+	int ret;
 
 	mutex_lock(&pca963x->chip->mutex);
 	ledout = i2c_smbus_read_byte_data(pca963x->chip->client, ledout_addr);
-	switch (pca963x->brightness) {
+	switch (brightness) {
 	case LED_FULL:
-		i2c_smbus_write_byte_data(pca963x->chip->client, ledout_addr,
+		ret = i2c_smbus_write_byte_data(pca963x->chip->client,
+			ledout_addr,
 			(ledout & ~mask) | (PCA963X_LED_ON << shift));
 		break;
 	case LED_OFF:
-		i2c_smbus_write_byte_data(pca963x->chip->client, ledout_addr,
-			ledout & ~mask);
+		ret = i2c_smbus_write_byte_data(pca963x->chip->client,
+			ledout_addr, ledout & ~mask);
 		break;
 	default:
-		i2c_smbus_write_byte_data(pca963x->chip->client,
+		ret = i2c_smbus_write_byte_data(pca963x->chip->client,
 			PCA963X_PWM_BASE + pca963x->led_num,
-			pca963x->brightness);
-		i2c_smbus_write_byte_data(pca963x->chip->client, ledout_addr,
+			brightness);
+		if (ret < 0)
+			goto unlock;
+		ret = i2c_smbus_write_byte_data(pca963x->chip->client,
+			ledout_addr,
 			(ledout & ~mask) | (PCA963X_LED_PWM << shift));
 		break;
 	}
+unlock:
 	mutex_unlock(&pca963x->chip->mutex);
+	return ret;
 }
 
-static void pca963x_blink_work(struct pca963x_led *pca963x)
+static void pca963x_blink(struct pca963x_led *pca963x)
 {
 	u8 ledout_addr = pca963x->chip->chipdef->ledout_base +
 		(pca963x->led_num / 4);
@@ -180,36 +179,14 @@ static void pca963x_blink_work(struct pca963x_led *pca963x)
 	mutex_unlock(&pca963x->chip->mutex);
 }
 
-static void pca963x_work(struct work_struct *work)
-{
-	struct pca963x_led *pca963x = container_of(work,
-		struct pca963x_led, work);
-
-	switch (pca963x->cmd) {
-	case BRIGHTNESS_SET:
-		pca963x_brightness_work(pca963x);
-		break;
-	case BLINK_SET:
-		pca963x_blink_work(pca963x);
-		break;
-	}
-}
-
-static void pca963x_led_set(struct led_classdev *led_cdev,
+static int pca963x_led_set(struct led_classdev *led_cdev,
 	enum led_brightness value)
 {
 	struct pca963x_led *pca963x;
 
 	pca963x = container_of(led_cdev, struct pca963x_led, led_cdev);
 
-	pca963x->cmd = BRIGHTNESS_SET;
-	pca963x->brightness = value;
-
-	/*
-	 * Must use workqueue for the actual I/O since I2C operations
-	 * can sleep.
-	 */
-	schedule_work(&pca963x->work);
+	return pca963x_brightness(pca963x, value);
 }
 
 static int pca963x_blink_set(struct led_classdev *led_cdev,
@@ -254,15 +231,10 @@ static int pca963x_blink_set(struct led_classdev *led_cdev,
 	 */
 	gfrq = (period * 24 / 1000) - 1;
 
-	pca963x->cmd = BLINK_SET;
 	pca963x->gdc = gdc;
 	pca963x->gfrq = gfrq;
 
-	/*
-	 * Must use workqueue for the actual I/O since I2C operations
-	 * can sleep.
-	 */
-	schedule_work(&pca963x->work);
+	pca963x_blink(pca963x);
 
 	*delay_on = time_on;
 	*delay_off = time_off;
@@ -409,13 +381,11 @@ static int pca963x_probe(struct i2c_client *client,
 				 client->addr, i);
 
 		pca963x[i].led_cdev.name = pca963x[i].name;
-		pca963x[i].led_cdev.brightness_set = pca963x_led_set;
+		pca963x[i].led_cdev.brightness_set_blocking = pca963x_led_set;
 
 		if (pdata && pdata->blink_type == PCA963X_HW_BLINK)
 			pca963x[i].led_cdev.blink_set = pca963x_blink_set;
 
-		INIT_WORK(&pca963x[i].work, pca963x_work);
-
 		err = led_classdev_register(&client->dev, &pca963x[i].led_cdev);
 		if (err < 0)
 			goto exit;
@@ -435,10 +405,8 @@ static int pca963x_probe(struct i2c_client *client,
 	return 0;
 
 exit:
-	while (i--) {
+	while (i--)
 		led_classdev_unregister(&pca963x[i].led_cdev);
-		cancel_work_sync(&pca963x[i].work);
-	}
 
 	return err;
 }
@@ -448,10 +416,8 @@ static int pca963x_remove(struct i2c_client *client)
 	struct pca963x *pca963x = i2c_get_clientdata(client);
 	int i;
 
-	for (i = 0; i < pca963x->chipdef->n_leds; i++) {
+	for (i = 0; i < pca963x->chipdef->n_leds; i++)
 		led_classdev_unregister(&pca963x->leds[i].led_cdev);
-		cancel_work_sync(&pca963x->leds[i].work);
-	}
 
 	return 0;
 }
diff --git a/drivers/leds/leds-powernv.c b/drivers/leds/leds-powernv.c
index 1e75e1fe9b72..dfb8bd390125 100644
--- a/drivers/leds/leds-powernv.c
+++ b/drivers/leds/leds-powernv.c
@@ -77,7 +77,7 @@ static int powernv_get_led_type(const char *led_type_desc)
  * This function is called from work queue task context when ever it gets
  * scheduled. This function can sleep at opal_async_wait_response call.
  */
-static void powernv_led_set(struct powernv_led_data *powernv_led,
+static int powernv_led_set(struct powernv_led_data *powernv_led,
 			    enum led_brightness value)
 {
 	int rc, token;
@@ -99,7 +99,7 @@ static void powernv_led_set(struct powernv_led_data *powernv_led,
 		if (token != -ERESTARTSYS)
 			dev_err(dev, "%s: Couldn't get OPAL async token\n",
 				__func__);
-		return;
+		return token;
 	}
 
 	rc = opal_leds_set_ind(token, powernv_led->loc_code,
@@ -125,6 +125,7 @@ static void powernv_led_set(struct powernv_led_data *powernv_led,
 
 out_token:
 	opal_async_release_token(token);
+	return rc;
 }
 
 /*
@@ -173,20 +174,23 @@ static enum led_brightness powernv_led_get(struct powernv_led_data *powernv_led)
  * LED classdev 'brightness_get' function. This schedules work
  * to update LED state.
  */
-static void powernv_brightness_set(struct led_classdev *led_cdev,
+static int powernv_brightness_set(struct led_classdev *led_cdev,
 				   enum led_brightness value)
 {
 	struct powernv_led_data *powernv_led =
 		container_of(led_cdev, struct powernv_led_data, cdev);
 	struct powernv_led_common *powernv_led_common = powernv_led->common;
+	int rc;
 
 	/* Do not modify LED in unload path */
 	if (powernv_led_common->led_disabled)
-		return;
+		return 0;
 
 	mutex_lock(&powernv_led_common->lock);
-	powernv_led_set(powernv_led, value);
+	rc = powernv_led_set(powernv_led, value);
 	mutex_unlock(&powernv_led_common->lock);
+
+	return rc;
 }
 
 /* LED classdev 'brightness_get' function */
@@ -227,7 +231,7 @@ static int powernv_led_create(struct device *dev,
 		return -ENOMEM;
 	}
 
-	powernv_led->cdev.brightness_set = powernv_brightness_set;
+	powernv_led->cdev.brightness_set_blocking = powernv_brightness_set;
 	powernv_led->cdev.brightness_get = powernv_brightness_get;
 	powernv_led->cdev.brightness = LED_OFF;
 	powernv_led->cdev.max_brightness = LED_FULL;
@@ -256,8 +260,6 @@ static int powernv_led_classdev(struct platform_device *pdev,
 
 	for_each_child_of_node(led_node, np) {
 		p = of_find_property(np, "led-types", NULL);
-		if (!p)
-			continue;
 
 		while ((cur = of_prop_next_string(p, cur)) != NULL) {
 			powernv_led = devm_kzalloc(dev, sizeof(*powernv_led),
diff --git a/drivers/leds/leds-pwm.c b/drivers/leds/leds-pwm.c
index 1d07e3e83d29..4783bacb2e9d 100644
--- a/drivers/leds/leds-pwm.c
+++ b/drivers/leds/leds-pwm.c
@@ -22,12 +22,10 @@
 #include <linux/pwm.h>
 #include <linux/leds_pwm.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 
 struct led_pwm_data {
 	struct led_classdev	cdev;
 	struct pwm_device	*pwm;
-	struct work_struct	work;
 	unsigned int		active_low;
 	unsigned int		period;
 	int			duty;
@@ -51,14 +49,6 @@ static void __led_pwm_set(struct led_pwm_data *led_dat)
 		pwm_enable(led_dat->pwm);
 }
 
-static void led_pwm_work(struct work_struct *work)
-{
-	struct led_pwm_data *led_dat =
-		container_of(work, struct led_pwm_data, work);
-
-	__led_pwm_set(led_dat);
-}
-
 static void led_pwm_set(struct led_classdev *led_cdev,
 	enum led_brightness brightness)
 {
@@ -75,10 +65,14 @@ static void led_pwm_set(struct led_classdev *led_cdev,
 
 	led_dat->duty = duty;
 
-	if (led_dat->can_sleep)
-		schedule_work(&led_dat->work);
-	else
-		__led_pwm_set(led_dat);
+	__led_pwm_set(led_dat);
+}
+
+static int led_pwm_set_blocking(struct led_classdev *led_cdev,
+	enum led_brightness brightness)
+{
+	led_pwm_set(led_cdev, brightness);
+	return 0;
 }
 
 static inline size_t sizeof_pwm_leds_priv(int num_leds)
@@ -89,11 +83,8 @@ static inline size_t sizeof_pwm_leds_priv(int num_leds)
 
 static void led_pwm_cleanup(struct led_pwm_priv *priv)
 {
-	while (priv->num_leds--) {
+	while (priv->num_leds--)
 		led_classdev_unregister(&priv->leds[priv->num_leds].cdev);
-		if (priv->leds[priv->num_leds].can_sleep)
-			cancel_work_sync(&priv->leds[priv->num_leds].work);
-	}
 }
 
 static int led_pwm_add(struct device *dev, struct led_pwm_priv *priv,
@@ -105,7 +96,6 @@ static int led_pwm_add(struct device *dev, struct led_pwm_priv *priv,
 	led_data->active_low = led->active_low;
 	led_data->cdev.name = led->name;
 	led_data->cdev.default_trigger = led->default_trigger;
-	led_data->cdev.brightness_set = led_pwm_set;
 	led_data->cdev.brightness = LED_OFF;
 	led_data->cdev.max_brightness = led->max_brightness;
 	led_data->cdev.flags = LED_CORE_SUSPENDRESUME;
@@ -122,8 +112,10 @@ static int led_pwm_add(struct device *dev, struct led_pwm_priv *priv,
 	}
 
 	led_data->can_sleep = pwm_can_sleep(led_data->pwm);
-	if (led_data->can_sleep)
-		INIT_WORK(&led_data->work, led_pwm_work);
+	if (!led_data->can_sleep)
+		led_data->cdev.brightness_set = led_pwm_set;
+	else
+		led_data->cdev.brightness_set_blocking = led_pwm_set_blocking;
 
 	led_data->period = pwm_get_period(led_data->pwm);
 	if (!led_data->period && (led->pwm_period_ns > 0))
@@ -132,6 +124,7 @@ static int led_pwm_add(struct device *dev, struct led_pwm_priv *priv,
 	ret = led_classdev_register(dev, &led_data->cdev);
 	if (ret == 0) {
 		priv->num_leds++;
+		led_pwm_set(&led_data->cdev, led_data->cdev.brightness);
 	} else {
 		dev_err(dev, "failed to register PWM led for %s: %d\n",
 			led->name, ret);
@@ -236,6 +229,6 @@ static struct platform_driver led_pwm_driver = {
 module_platform_driver(led_pwm_driver);
 
 MODULE_AUTHOR("Luotao Fu <l.fu@pengutronix.de>");
-MODULE_DESCRIPTION("PWM LED driver for PXA");
-MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("generic PWM LED driver");
+MODULE_LICENSE("GPL v2");
 MODULE_ALIAS("platform:leds-pwm");
diff --git a/drivers/leds/leds-regulator.c b/drivers/leds/leds-regulator.c
index ffc21397a675..acf77ca47558 100644
--- a/drivers/leds/leds-regulator.c
+++ b/drivers/leds/leds-regulator.c
@@ -14,7 +14,6 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 #include <linux/leds.h>
 #include <linux/leds-regulator.h>
 #include <linux/platform_device.h>
@@ -25,10 +24,8 @@
 
 struct regulator_led {
 	struct led_classdev cdev;
-	enum led_brightness value;
 	int enabled;
 	struct mutex mutex;
-	struct work_struct work;
 
 	struct regulator *vcc;
 };
@@ -94,22 +91,24 @@ static void regulator_led_disable(struct regulator_led *led)
 	led->enabled = 0;
 }
 
-static void regulator_led_set_value(struct regulator_led *led)
+static int regulator_led_brightness_set(struct led_classdev *led_cdev,
+					 enum led_brightness value)
 {
+	struct regulator_led *led = to_regulator_led(led_cdev);
 	int voltage;
-	int ret;
+	int ret = 0;
 
 	mutex_lock(&led->mutex);
 
-	if (led->value == LED_OFF) {
+	if (value == LED_OFF) {
 		regulator_led_disable(led);
 		goto out;
 	}
 
 	if (led->cdev.max_brightness > 1) {
-		voltage = led_regulator_get_voltage(led->vcc, led->value);
+		voltage = led_regulator_get_voltage(led->vcc, value);
 		dev_dbg(led->cdev.dev, "brightness: %d voltage: %d\n",
-				led->value, voltage);
+				value, voltage);
 
 		ret = regulator_set_voltage(led->vcc, voltage, voltage);
 		if (ret != 0)
@@ -121,23 +120,7 @@ static void regulator_led_set_value(struct regulator_led *led)
 
 out:
 	mutex_unlock(&led->mutex);
-}
-
-static void led_work(struct work_struct *work)
-{
-	struct regulator_led *led;
-
-	led = container_of(work, struct regulator_led, work);
-	regulator_led_set_value(led);
-}
-
-static void regulator_led_brightness_set(struct led_classdev *led_cdev,
-			   enum led_brightness value)
-{
-	struct regulator_led *led = to_regulator_led(led_cdev);
-
-	led->value = value;
-	schedule_work(&led->work);
+	return ret;
 }
 
 static int regulator_led_probe(struct platform_device *pdev)
@@ -169,9 +152,8 @@ static int regulator_led_probe(struct platform_device *pdev)
 				pdata->brightness);
 		return -EINVAL;
 	}
-	led->value = pdata->brightness;
 
-	led->cdev.brightness_set = regulator_led_brightness_set;
+	led->cdev.brightness_set_blocking = regulator_led_brightness_set;
 	led->cdev.name = pdata->name;
 	led->cdev.flags |= LED_CORE_SUSPENDRESUME;
 	led->vcc = vcc;
@@ -181,21 +163,18 @@ static int regulator_led_probe(struct platform_device *pdev)
 		led->enabled = 1;
 
 	mutex_init(&led->mutex);
-	INIT_WORK(&led->work, led_work);
 
 	platform_set_drvdata(pdev, led);
 
 	ret = led_classdev_register(&pdev->dev, &led->cdev);
-	if (ret < 0) {
-		cancel_work_sync(&led->work);
+	if (ret < 0)
 		return ret;
-	}
 
 	/* to expose the default value to userspace */
-	led->cdev.brightness = led->value;
+	led->cdev.brightness = pdata->brightness;
 
 	/* Set the default led status */
-	regulator_led_set_value(led);
+	regulator_led_brightness_set(&led->cdev, led->cdev.brightness);
 
 	return 0;
 }
@@ -205,7 +184,6 @@ static int regulator_led_remove(struct platform_device *pdev)
 	struct regulator_led *led = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&led->cdev);
-	cancel_work_sync(&led->work);
 	regulator_led_disable(led);
 	return 0;
 }
diff --git a/drivers/leds/leds-sunfire.c b/drivers/leds/leds-sunfire.c
index c2553c54f2cf..7c09db8bd4e8 100644
--- a/drivers/leds/leds-sunfire.c
+++ b/drivers/leds/leds-sunfire.c
@@ -234,28 +234,19 @@ static struct platform_driver sunfire_fhc_led_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&sunfire_clockboard_led_driver,
+	&sunfire_fhc_led_driver,
+};
+
 static int __init sunfire_leds_init(void)
 {
-	int err = platform_driver_register(&sunfire_clockboard_led_driver);
-
-	if (err) {
-		pr_err("Could not register clock board LED driver\n");
-		return err;
-	}
-
-	err = platform_driver_register(&sunfire_fhc_led_driver);
-	if (err) {
-		pr_err("Could not register FHC LED driver\n");
-		platform_driver_unregister(&sunfire_clockboard_led_driver);
-	}
-
-	return err;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit sunfire_leds_exit(void)
 {
-	platform_driver_unregister(&sunfire_clockboard_led_driver);
-	platform_driver_unregister(&sunfire_fhc_led_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_init(sunfire_leds_init);
diff --git a/drivers/leds/leds-syscon.c b/drivers/leds/leds-syscon.c
index b88900d721e4..3be40f74f12a 100644
--- a/drivers/leds/leds-syscon.c
+++ b/drivers/leds/leds-syscon.c
@@ -20,7 +20,7 @@
  * MA 02111-1307 USA
  */
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of_device.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
@@ -139,29 +139,17 @@ static int syscon_led_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int syscon_led_remove(struct platform_device *pdev)
-{
-	struct syscon_led *sled = platform_get_drvdata(pdev);
-
-	led_classdev_unregister(&sled->cdev);
-	/* Turn it off */
-	regmap_update_bits(sled->map, sled->offset, sled->mask, 0);
-	return 0;
-}
-
 static const struct of_device_id of_syscon_leds_match[] = {
 	{ .compatible = "register-bit-led", },
 	{},
 };
 
-MODULE_DEVICE_TABLE(of, of_syscon_leds_match);
-
 static struct platform_driver syscon_led_driver = {
 	.probe		= syscon_led_probe,
-	.remove		= syscon_led_remove,
 	.driver		= {
 		.name	= "leds-syscon",
 		.of_match_table = of_syscon_leds_match,
+		.suppress_bind_attrs = true,
 	},
 };
-module_platform_driver(syscon_led_driver);
+builtin_platform_driver(syscon_led_driver);
diff --git a/drivers/leds/leds-tca6507.c b/drivers/leds/leds-tca6507.c
index edbecc4ca2da..c548ea10f0f0 100644
--- a/drivers/leds/leds-tca6507.c
+++ b/drivers/leds/leds-tca6507.c
@@ -603,7 +603,7 @@ static int tca6507_blink_set(struct led_classdev *led_cdev,
 static void tca6507_gpio_set_value(struct gpio_chip *gc,
 				   unsigned offset, int val)
 {
-	struct tca6507_chip *tca = container_of(gc, struct tca6507_chip, gpio);
+	struct tca6507_chip *tca = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&tca->lock, flags);
@@ -651,11 +651,11 @@ static int tca6507_probe_gpios(struct i2c_client *client,
 	tca->gpio.owner = THIS_MODULE;
 	tca->gpio.direction_output = tca6507_gpio_direction_output;
 	tca->gpio.set = tca6507_gpio_set_value;
-	tca->gpio.dev = &client->dev;
+	tca->gpio.parent = &client->dev;
 #ifdef CONFIG_OF_GPIO
 	tca->gpio.of_node = of_node_get(client->dev.of_node);
 #endif
-	err = gpiochip_add(&tca->gpio);
+	err = gpiochip_add_data(&tca->gpio, tca);
 	if (err) {
 		tca->gpio.ngpio = 0;
 		return err;
diff --git a/drivers/leds/leds-tlc591xx.c b/drivers/leds/leds-tlc591xx.c
index b806eca83d27..304531644938 100644
--- a/drivers/leds/leds-tlc591xx.c
+++ b/drivers/leds/leds-tlc591xx.c
@@ -14,7 +14,6 @@
 #include <linux/of_device.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
-#include <linux/workqueue.h>
 
 #define TLC591XX_MAX_LEDS	16
 
@@ -42,13 +41,11 @@
 #define LEDOUT_MASK		0x3
 
 #define ldev_to_led(c)		container_of(c, struct tlc591xx_led, ldev)
-#define work_to_led(work)	container_of(work, struct tlc591xx_led, work)
 
 struct tlc591xx_led {
 	bool active;
 	unsigned int led_no;
 	struct led_classdev ldev;
-	struct work_struct work;
 	struct tlc591xx_priv *priv;
 };
 
@@ -110,12 +107,12 @@ tlc591xx_set_pwm(struct tlc591xx_priv *priv, struct tlc591xx_led *led,
 	return regmap_write(priv->regmap, pwm, brightness);
 }
 
-static void
-tlc591xx_led_work(struct work_struct *work)
+static int
+tlc591xx_brightness_set(struct led_classdev *led_cdev,
+			enum led_brightness brightness)
 {
-	struct tlc591xx_led *led = work_to_led(work);
+	struct tlc591xx_led *led = ldev_to_led(led_cdev);
 	struct tlc591xx_priv *priv = led->priv;
-	enum led_brightness brightness = led->ldev.brightness;
 	int err;
 
 	switch (brightness) {
@@ -131,18 +128,7 @@ tlc591xx_led_work(struct work_struct *work)
 			err = tlc591xx_set_pwm(priv, led, brightness);
 	}
 
-	if (err)
-		dev_err(led->ldev.dev, "Failed setting brightness\n");
-}
-
-static void
-tlc591xx_brightness_set(struct led_classdev *led_cdev,
-			enum led_brightness brightness)
-{
-	struct tlc591xx_led *led = ldev_to_led(led_cdev);
-
-	led->ldev.brightness = brightness;
-	schedule_work(&led->work);
+	return err;
 }
 
 static void
@@ -151,10 +137,8 @@ tlc591xx_destroy_devices(struct tlc591xx_priv *priv, unsigned int j)
 	int i = j;
 
 	while (--i >= 0) {
-		if (priv->leds[i].active) {
+		if (priv->leds[i].active)
 			led_classdev_unregister(&priv->leds[i].ldev);
-			cancel_work_sync(&priv->leds[i].work);
-		}
 	}
 }
 
@@ -175,9 +159,8 @@ tlc591xx_configure(struct device *dev,
 
 		led->priv = priv;
 		led->led_no = i;
-		led->ldev.brightness_set = tlc591xx_brightness_set;
+		led->ldev.brightness_set_blocking = tlc591xx_brightness_set;
 		led->ldev.max_brightness = LED_FULL;
-		INIT_WORK(&led->work, tlc591xx_led_work);
 		err = led_classdev_register(dev, &led->ldev);
 		if (err < 0) {
 			dev_err(dev, "couldn't register LED %s\n",
diff --git a/drivers/leds/leds-wm831x-status.c b/drivers/leds/leds-wm831x-status.c
index 56027ef7c7e8..64a22263e7fc 100644
--- a/drivers/leds/leds-wm831x-status.c
+++ b/drivers/leds/leds-wm831x-status.c
@@ -23,7 +23,6 @@
 struct wm831x_status {
 	struct led_classdev cdev;
 	struct wm831x *wm831x;
-	struct work_struct work;
 	struct mutex mutex;
 
 	spinlock_t value_lock;
@@ -40,10 +39,8 @@ struct wm831x_status {
 #define to_wm831x_status(led_cdev) \
 	container_of(led_cdev, struct wm831x_status, cdev)
 
-static void wm831x_status_work(struct work_struct *work)
+static void wm831x_status_set(struct wm831x_status *led)
 {
-	struct wm831x_status *led = container_of(work, struct wm831x_status,
-						 work);
 	unsigned long flags;
 
 	mutex_lock(&led->mutex);
@@ -70,8 +67,8 @@ static void wm831x_status_work(struct work_struct *work)
 	mutex_unlock(&led->mutex);
 }
 
-static void wm831x_status_set(struct led_classdev *led_cdev,
-			   enum led_brightness value)
+static int wm831x_status_brightness_set(struct led_classdev *led_cdev,
+					 enum led_brightness value)
 {
 	struct wm831x_status *led = to_wm831x_status(led_cdev);
 	unsigned long flags;
@@ -80,8 +77,10 @@ static void wm831x_status_set(struct led_classdev *led_cdev,
 	led->brightness = value;
 	if (value == LED_OFF)
 		led->blink = 0;
-	schedule_work(&led->work);
 	spin_unlock_irqrestore(&led->value_lock, flags);
+	wm831x_status_set(led);
+
+	return 0;
 }
 
 static int wm831x_status_blink_set(struct led_classdev *led_cdev,
@@ -147,11 +146,8 @@ static int wm831x_status_blink_set(struct led_classdev *led_cdev,
 	else
 		led->blink = 0;
 
-	/* Always update; if we fail turn off blinking since we expect
-	 * a software fallback. */
-	schedule_work(&led->work);
-
 	spin_unlock_irqrestore(&led->value_lock, flags);
+	wm831x_status_set(led);
 
 	return ret;
 }
@@ -206,11 +202,9 @@ static ssize_t wm831x_status_src_store(struct device *dev,
 	for (i = 0; i < ARRAY_SIZE(led_src_texts); i++) {
 		if (!strcmp(name, led_src_texts[i])) {
 			mutex_lock(&led->mutex);
-
 			led->src = i;
-			schedule_work(&led->work);
-
 			mutex_unlock(&led->mutex);
+			wm831x_status_set(led);
 		}
 	}
 
@@ -262,7 +256,6 @@ static int wm831x_status_probe(struct platform_device *pdev)
 		pdata.name = dev_name(&pdev->dev);
 
 	mutex_init(&drvdata->mutex);
-	INIT_WORK(&drvdata->work, wm831x_status_work);
 	spin_lock_init(&drvdata->value_lock);
 
 	/* We cache the configuration register and read startup values
@@ -287,7 +280,7 @@ static int wm831x_status_probe(struct platform_device *pdev)
 
 	drvdata->cdev.name = pdata.name;
 	drvdata->cdev.default_trigger = pdata.default_trigger;
-	drvdata->cdev.brightness_set = wm831x_status_set;
+	drvdata->cdev.brightness_set_blocking = wm831x_status_brightness_set;
 	drvdata->cdev.blink_set = wm831x_status_blink_set;
 	drvdata->cdev.groups = wm831x_status_groups;
 
diff --git a/drivers/leds/leds-wm8350.c b/drivers/leds/leds-wm8350.c
index 0d121835673f..e1e4e9d0b8b1 100644
--- a/drivers/leds/leds-wm8350.c
+++ b/drivers/leds/leds-wm8350.c
@@ -89,40 +89,42 @@ static const int isink_cur[] = {
 #define to_wm8350_led(led_cdev) \
 	container_of(led_cdev, struct wm8350_led, cdev)
 
-static void wm8350_led_enable(struct wm8350_led *led)
+static int wm8350_led_enable(struct wm8350_led *led)
 {
-	int ret;
+	int ret = 0;
 
 	if (led->enabled)
-		return;
+		return ret;
 
 	ret = regulator_enable(led->isink);
 	if (ret != 0) {
 		dev_err(led->cdev.dev, "Failed to enable ISINK: %d\n", ret);
-		return;
+		return ret;
 	}
 
 	ret = regulator_enable(led->dcdc);
 	if (ret != 0) {
 		dev_err(led->cdev.dev, "Failed to enable DCDC: %d\n", ret);
 		regulator_disable(led->isink);
-		return;
+		return ret;
 	}
 
 	led->enabled = 1;
+
+	return ret;
 }
 
-static void wm8350_led_disable(struct wm8350_led *led)
+static int wm8350_led_disable(struct wm8350_led *led)
 {
-	int ret;
+	int ret = 0;
 
 	if (!led->enabled)
-		return;
+		return ret;
 
 	ret = regulator_disable(led->dcdc);
 	if (ret != 0) {
 		dev_err(led->cdev.dev, "Failed to disable DCDC: %d\n", ret);
-		return;
+		return ret;
 	}
 
 	ret = regulator_disable(led->isink);
@@ -132,27 +134,29 @@ static void wm8350_led_disable(struct wm8350_led *led)
 		if (ret != 0)
 			dev_err(led->cdev.dev, "Failed to reenable DCDC: %d\n",
 				ret);
-		return;
+		return ret;
 	}
 
 	led->enabled = 0;
+
+	return ret;
 }
 
-static void led_work(struct work_struct *work)
+static int wm8350_led_set(struct led_classdev *led_cdev,
+			   enum led_brightness value)
 {
-	struct wm8350_led *led = container_of(work, struct wm8350_led, work);
+	struct wm8350_led *led = to_wm8350_led(led_cdev);
+	unsigned long flags;
 	int ret;
 	int uA;
-	unsigned long flags;
 
-	mutex_lock(&led->mutex);
+	led->value = value;
 
 	spin_lock_irqsave(&led->value_lock, flags);
 
 	if (led->value == LED_OFF) {
 		spin_unlock_irqrestore(&led->value_lock, flags);
-		wm8350_led_disable(led);
-		goto out;
+		return wm8350_led_disable(led);
 	}
 
 	/* This scales linearly into the index of valid current
@@ -166,36 +170,21 @@ static void led_work(struct work_struct *work)
 
 	ret = regulator_set_current_limit(led->isink, isink_cur[uA],
 					  isink_cur[uA]);
-	if (ret != 0)
+	if (ret != 0) {
 		dev_err(led->cdev.dev, "Failed to set %duA: %d\n",
 			isink_cur[uA], ret);
+		return ret;
+	}
 
-	wm8350_led_enable(led);
-
-out:
-	mutex_unlock(&led->mutex);
-}
-
-static void wm8350_led_set(struct led_classdev *led_cdev,
-			   enum led_brightness value)
-{
-	struct wm8350_led *led = to_wm8350_led(led_cdev);
-	unsigned long flags;
-
-	spin_lock_irqsave(&led->value_lock, flags);
-	led->value = value;
-	schedule_work(&led->work);
-	spin_unlock_irqrestore(&led->value_lock, flags);
+	return wm8350_led_enable(led);
 }
 
 static void wm8350_led_shutdown(struct platform_device *pdev)
 {
 	struct wm8350_led *led = platform_get_drvdata(pdev);
 
-	mutex_lock(&led->mutex);
 	led->value = LED_OFF;
 	wm8350_led_disable(led);
-	mutex_unlock(&led->mutex);
 }
 
 static int wm8350_led_probe(struct platform_device *pdev)
@@ -232,7 +221,7 @@ static int wm8350_led_probe(struct platform_device *pdev)
 	if (led == NULL)
 		return -ENOMEM;
 
-	led->cdev.brightness_set = wm8350_led_set;
+	led->cdev.brightness_set_blocking = wm8350_led_set;
 	led->cdev.default_trigger = pdata->default_trigger;
 	led->cdev.name = pdata->name;
 	led->cdev.flags |= LED_CORE_SUSPENDRESUME;
@@ -251,8 +240,6 @@ static int wm8350_led_probe(struct platform_device *pdev)
 			 pdata->max_uA);
 
 	spin_lock_init(&led->value_lock);
-	mutex_init(&led->mutex);
-	INIT_WORK(&led->work, led_work);
 	led->value = LED_OFF;
 	platform_set_drvdata(pdev, led);
 
@@ -264,7 +251,6 @@ static int wm8350_led_remove(struct platform_device *pdev)
 	struct wm8350_led *led = platform_get_drvdata(pdev);
 
 	led_classdev_unregister(&led->cdev);
-	flush_work(&led->work);
 	wm8350_led_disable(led);
 	return 0;
 }
diff --git a/drivers/leds/leds.h b/drivers/leds/leds.h
index 4238fbc31d35..db3f20da7221 100644
--- a/drivers/leds/leds.h
+++ b/drivers/leds/leds.h
@@ -16,29 +16,6 @@
 #include <linux/rwsem.h>
 #include <linux/leds.h>
 
-static inline void led_set_brightness_async(struct led_classdev *led_cdev,
-					enum led_brightness value)
-{
-	value = min(value, led_cdev->max_brightness);
-	led_cdev->brightness = value;
-
-	if (!(led_cdev->flags & LED_SUSPENDED))
-		led_cdev->brightness_set(led_cdev, value);
-}
-
-static inline int led_set_brightness_sync(struct led_classdev *led_cdev,
-					enum led_brightness value)
-{
-	int ret = 0;
-
-	led_cdev->brightness = min(value, led_cdev->max_brightness);
-
-	if (!(led_cdev->flags & LED_SUSPENDED))
-		ret = led_cdev->brightness_set_sync(led_cdev,
-						led_cdev->brightness);
-	return ret;
-}
-
 static inline int led_get_brightness(struct led_classdev *led_cdev)
 {
 	return led_cdev->brightness;
@@ -46,6 +23,10 @@ static inline int led_get_brightness(struct led_classdev *led_cdev)
 
 void led_init_core(struct led_classdev *led_cdev);
 void led_stop_software_blink(struct led_classdev *led_cdev);
+void led_set_brightness_nopm(struct led_classdev *led_cdev,
+				enum led_brightness value);
+void led_set_brightness_nosleep(struct led_classdev *led_cdev,
+				enum led_brightness value);
 
 extern struct rw_semaphore leds_list_lock;
 extern struct list_head leds_list;
diff --git a/drivers/leds/trigger/ledtrig-backlight.c b/drivers/leds/trigger/ledtrig-backlight.c
index 59eca17d9661..1ca1f1608f76 100644
--- a/drivers/leds/trigger/ledtrig-backlight.c
+++ b/drivers/leds/trigger/ledtrig-backlight.c
@@ -51,9 +51,9 @@ static int fb_notifier_callback(struct notifier_block *p,
 
 	if ((n->old_status == UNBLANK) ^ n->invert) {
 		n->brightness = led->brightness;
-		led_set_brightness_async(led, LED_OFF);
+		led_set_brightness_nosleep(led, LED_OFF);
 	} else {
-		led_set_brightness_async(led, n->brightness);
+		led_set_brightness_nosleep(led, n->brightness);
 	}
 
 	n->old_status = new_status;
@@ -89,9 +89,9 @@ static ssize_t bl_trig_invert_store(struct device *dev,
 
 	/* After inverting, we need to update the LED. */
 	if ((n->old_status == BLANK) ^ n->invert)
-		led_set_brightness_async(led, LED_OFF);
+		led_set_brightness_nosleep(led, LED_OFF);
 	else
-		led_set_brightness_async(led, n->brightness);
+		led_set_brightness_nosleep(led, n->brightness);
 
 	return num;
 }
diff --git a/drivers/leds/trigger/ledtrig-cpu.c b/drivers/leds/trigger/ledtrig-cpu.c
index aec0f02b6b3e..938467fb82be 100644
--- a/drivers/leds/trigger/ledtrig-cpu.c
+++ b/drivers/leds/trigger/ledtrig-cpu.c
@@ -19,7 +19,6 @@
  *
  */
 
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/slab.h>
@@ -140,27 +139,4 @@ static int __init ledtrig_cpu_init(void)
 
 	return 0;
 }
-module_init(ledtrig_cpu_init);
-
-static void __exit ledtrig_cpu_exit(void)
-{
-	int cpu;
-
-	unregister_cpu_notifier(&ledtrig_cpu_nb);
-
-	for_each_possible_cpu(cpu) {
-		struct led_trigger_cpu *trig = &per_cpu(cpu_trig, cpu);
-
-		led_trigger_unregister_simple(trig->_trig);
-		trig->_trig = NULL;
-		memset(trig->name, 0, MAX_NAME_LEN);
-	}
-
-	unregister_syscore_ops(&ledtrig_cpu_syscore_ops);
-}
-module_exit(ledtrig_cpu_exit);
-
-MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
-MODULE_AUTHOR("Bryan Wu <bryan.wu@canonical.com>");
-MODULE_DESCRIPTION("CPU LED trigger");
-MODULE_LICENSE("GPL");
+device_initcall(ledtrig_cpu_init);
diff --git a/drivers/leds/trigger/ledtrig-default-on.c b/drivers/leds/trigger/ledtrig-default-on.c
index 6f38f883aaf1..ff455cb46680 100644
--- a/drivers/leds/trigger/ledtrig-default-on.c
+++ b/drivers/leds/trigger/ledtrig-default-on.c
@@ -19,7 +19,7 @@
 
 static void defon_trig_activate(struct led_classdev *led_cdev)
 {
-	led_set_brightness_async(led_cdev, led_cdev->max_brightness);
+	led_set_brightness_nosleep(led_cdev, led_cdev->max_brightness);
 }
 
 static struct led_trigger defon_led_trigger = {
diff --git a/drivers/leds/trigger/ledtrig-gpio.c b/drivers/leds/trigger/ledtrig-gpio.c
index 4cc7040746c6..51288a45fbcb 100644
--- a/drivers/leds/trigger/ledtrig-gpio.c
+++ b/drivers/leds/trigger/ledtrig-gpio.c
@@ -54,12 +54,12 @@ static void gpio_trig_work(struct work_struct *work)
 
 	if (tmp) {
 		if (gpio_data->desired_brightness)
-			led_set_brightness_async(gpio_data->led,
+			led_set_brightness_nosleep(gpio_data->led,
 					   gpio_data->desired_brightness);
 		else
-			led_set_brightness_async(gpio_data->led, LED_FULL);
+			led_set_brightness_nosleep(gpio_data->led, LED_FULL);
 	} else {
-		led_set_brightness_async(gpio_data->led, LED_OFF);
+		led_set_brightness_nosleep(gpio_data->led, LED_OFF);
 	}
 }
 
diff --git a/drivers/leds/trigger/ledtrig-heartbeat.c b/drivers/leds/trigger/ledtrig-heartbeat.c
index 8622ce651ae2..410c39c62dc7 100644
--- a/drivers/leds/trigger/ledtrig-heartbeat.c
+++ b/drivers/leds/trigger/ledtrig-heartbeat.c
@@ -38,7 +38,7 @@ static void led_heartbeat_function(unsigned long data)
 	unsigned long delay = 0;
 
 	if (unlikely(panic_heartbeats)) {
-		led_set_brightness(led_cdev, LED_OFF);
+		led_set_brightness_nosleep(led_cdev, LED_OFF);
 		return;
 	}
 
@@ -81,7 +81,7 @@ static void led_heartbeat_function(unsigned long data)
 		break;
 	}
 
-	led_set_brightness_async(led_cdev, brightness);
+	led_set_brightness_nosleep(led_cdev, brightness);
 	mod_timer(&heartbeat_data->timer, jiffies + delay);
 }
 
diff --git a/drivers/leds/trigger/ledtrig-ide-disk.c b/drivers/leds/trigger/ledtrig-ide-disk.c
index 2cd7c0cf5924..c02a3ac3cd2b 100644
--- a/drivers/leds/trigger/ledtrig-ide-disk.c
+++ b/drivers/leds/trigger/ledtrig-ide-disk.c
@@ -11,7 +11,6 @@
  *
  */
 
-#include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/leds.h>
@@ -33,15 +32,4 @@ static int __init ledtrig_ide_init(void)
 	led_trigger_register_simple("ide-disk", &ledtrig_ide);
 	return 0;
 }
-
-static void __exit ledtrig_ide_exit(void)
-{
-	led_trigger_unregister_simple(ledtrig_ide);
-}
-
-module_init(ledtrig_ide_init);
-module_exit(ledtrig_ide_exit);
-
-MODULE_AUTHOR("Richard Purdie <rpurdie@openedhand.com>");
-MODULE_DESCRIPTION("LED IDE Disk Activity Trigger");
-MODULE_LICENSE("GPL");
+device_initcall(ledtrig_ide_init);
diff --git a/drivers/leds/trigger/ledtrig-oneshot.c b/drivers/leds/trigger/ledtrig-oneshot.c
index fbd02cdc3ad7..b8ea9f0f1e19 100644
--- a/drivers/leds/trigger/ledtrig-oneshot.c
+++ b/drivers/leds/trigger/ledtrig-oneshot.c
@@ -63,9 +63,9 @@ static ssize_t led_invert_store(struct device *dev,
 	oneshot_data->invert = !!state;
 
 	if (oneshot_data->invert)
-		led_set_brightness_async(led_cdev, LED_FULL);
+		led_set_brightness_nosleep(led_cdev, LED_FULL);
 	else
-		led_set_brightness_async(led_cdev, LED_OFF);
+		led_set_brightness_nosleep(led_cdev, LED_OFF);
 
 	return size;
 }
@@ -201,4 +201,4 @@ module_exit(oneshot_trig_exit);
 
 MODULE_AUTHOR("Fabio Baltieri <fabio.baltieri@gmail.com>");
 MODULE_DESCRIPTION("One-shot LED trigger");
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/leds/trigger/ledtrig-transient.c b/drivers/leds/trigger/ledtrig-transient.c
index 3c34de404d18..7e6011bd3646 100644
--- a/drivers/leds/trigger/ledtrig-transient.c
+++ b/drivers/leds/trigger/ledtrig-transient.c
@@ -41,7 +41,7 @@ static void transient_timer_function(unsigned long data)
 	struct transient_trig_data *transient_data = led_cdev->trigger_data;
 
 	transient_data->activate = 0;
-	led_set_brightness_async(led_cdev, transient_data->restore_state);
+	led_set_brightness_nosleep(led_cdev, transient_data->restore_state);
 }
 
 static ssize_t transient_activate_show(struct device *dev,
@@ -72,7 +72,7 @@ static ssize_t transient_activate_store(struct device *dev,
 	if (state == 0 && transient_data->activate == 1) {
 		del_timer(&transient_data->timer);
 		transient_data->activate = state;
-		led_set_brightness_async(led_cdev,
+		led_set_brightness_nosleep(led_cdev,
 					transient_data->restore_state);
 		return size;
 	}
@@ -81,11 +81,11 @@ static ssize_t transient_activate_store(struct device *dev,
 	if (state == 1 && transient_data->activate == 0 &&
 	    transient_data->duration != 0) {
 		transient_data->activate = state;
-		led_set_brightness_async(led_cdev, transient_data->state);
+		led_set_brightness_nosleep(led_cdev, transient_data->state);
 		transient_data->restore_state =
 		    (transient_data->state == LED_FULL) ? LED_OFF : LED_FULL;
 		mod_timer(&transient_data->timer,
-			  jiffies + transient_data->duration);
+			  jiffies + msecs_to_jiffies(transient_data->duration));
 	}
 
 	/* state == 0 && transient_data->activate == 0
@@ -204,7 +204,7 @@ static void transient_trig_deactivate(struct led_classdev *led_cdev)
 
 	if (led_cdev->activated) {
 		del_timer_sync(&transient_data->timer);
-		led_set_brightness_async(led_cdev,
+		led_set_brightness_nosleep(led_cdev,
 					transient_data->restore_state);
 		device_remove_file(led_cdev->dev, &dev_attr_activate);
 		device_remove_file(led_cdev->dev, &dev_attr_duration);
diff --git a/drivers/lguest/core.c b/drivers/lguest/core.c
index 312ffd3d0017..9e385b38debf 100644
--- a/drivers/lguest/core.c
+++ b/drivers/lguest/core.c
@@ -22,7 +22,8 @@
 
 unsigned long switcher_addr;
 struct page **lg_switcher_pages;
-static struct vm_struct *switcher_vma;
+static struct vm_struct *switcher_text_vma;
+static struct vm_struct *switcher_stacks_vma;
 
 /* This One Big lock protects all inter-guest data structures. */
 DEFINE_MUTEX(lguest_lock);
@@ -83,54 +84,80 @@ static __init int map_switcher(void)
 	}
 
 	/*
+	 * Copy in the compiled-in Switcher code (from x86/switcher_32.S).
+	 * It goes in the first page, which we map in momentarily.
+	 */
+	memcpy(kmap(lg_switcher_pages[0]), start_switcher_text,
+	       end_switcher_text - start_switcher_text);
+	kunmap(lg_switcher_pages[0]);
+
+	/*
 	 * We place the Switcher underneath the fixmap area, which is the
 	 * highest virtual address we can get.  This is important, since we
 	 * tell the Guest it can't access this memory, so we want its ceiling
 	 * as high as possible.
 	 */
-	switcher_addr = FIXADDR_START - (TOTAL_SWITCHER_PAGES+1)*PAGE_SIZE;
+	switcher_addr = FIXADDR_START - TOTAL_SWITCHER_PAGES*PAGE_SIZE;
 
 	/*
-	 * Now we reserve the "virtual memory area" we want.  We might
-	 * not get it in theory, but in practice it's worked so far.
-	 * The end address needs +1 because __get_vm_area allocates an
-	 * extra guard page, so we need space for that.
+	 * Now we reserve the "virtual memory area"s we want.  We might
+	 * not get them in theory, but in practice it's worked so far.
+	 *
+	 * We want the switcher text to be read-only and executable, and
+	 * the stacks to be read-write and non-executable.
 	 */
-	switcher_vma = __get_vm_area(TOTAL_SWITCHER_PAGES * PAGE_SIZE,
-				     VM_ALLOC, switcher_addr, switcher_addr
-				     + (TOTAL_SWITCHER_PAGES+1) * PAGE_SIZE);
-	if (!switcher_vma) {
+	switcher_text_vma = __get_vm_area(PAGE_SIZE, VM_ALLOC|VM_NO_GUARD,
+					  switcher_addr,
+					  switcher_addr + PAGE_SIZE);
+
+	if (!switcher_text_vma) {
 		err = -ENOMEM;
 		printk("lguest: could not map switcher pages high\n");
 		goto free_pages;
 	}
 
+	switcher_stacks_vma = __get_vm_area(SWITCHER_STACK_PAGES * PAGE_SIZE,
+					    VM_ALLOC|VM_NO_GUARD,
+					    switcher_addr + PAGE_SIZE,
+					    switcher_addr + TOTAL_SWITCHER_PAGES * PAGE_SIZE);
+	if (!switcher_stacks_vma) {
+		err = -ENOMEM;
+		printk("lguest: could not map switcher pages high\n");
+		goto free_text_vma;
+	}
+
 	/*
 	 * This code actually sets up the pages we've allocated to appear at
 	 * switcher_addr.  map_vm_area() takes the vma we allocated above, the
-	 * kind of pages we're mapping (kernel pages), and a pointer to our
-	 * array of struct pages.
+	 * kind of pages we're mapping (kernel text pages and kernel writable
+	 * pages respectively), and a pointer to our array of struct pages.
 	 */
-	err = map_vm_area(switcher_vma, PAGE_KERNEL_EXEC, lg_switcher_pages);
+	err = map_vm_area(switcher_text_vma, PAGE_KERNEL_RX, lg_switcher_pages);
+	if (err) {
+		printk("lguest: text map_vm_area failed: %i\n", err);
+		goto free_vmas;
+	}
+
+	err = map_vm_area(switcher_stacks_vma, PAGE_KERNEL,
+			  lg_switcher_pages + SWITCHER_TEXT_PAGES);
 	if (err) {
-		printk("lguest: map_vm_area failed: %i\n", err);
-		goto free_vma;
+		printk("lguest: stacks map_vm_area failed: %i\n", err);
+		goto free_vmas;
 	}
 
 	/*
 	 * Now the Switcher is mapped at the right address, we can't fail!
-	 * Copy in the compiled-in Switcher code (from x86/switcher_32.S).
 	 */
-	memcpy(switcher_vma->addr, start_switcher_text,
-	       end_switcher_text - start_switcher_text);
-
 	printk(KERN_INFO "lguest: mapped switcher at %p\n",
-	       switcher_vma->addr);
+	       switcher_text_vma->addr);
 	/* And we succeeded... */
 	return 0;
 
-free_vma:
-	vunmap(switcher_vma->addr);
+free_vmas:
+	/* Undoes map_vm_area and __get_vm_area */
+	vunmap(switcher_stacks_vma->addr);
+free_text_vma:
+	vunmap(switcher_text_vma->addr);
 free_pages:
 	i = TOTAL_SWITCHER_PAGES;
 free_some_pages:
@@ -148,7 +175,8 @@ static void unmap_switcher(void)
 	unsigned int i;
 
 	/* vunmap() undoes *both* map_vm_area() and __get_vm_area(). */
-	vunmap(switcher_vma->addr);
+	vunmap(switcher_text_vma->addr);
+	vunmap(switcher_stacks_vma->addr);
 	/* Now we just need to free the pages we copied the switcher into */
 	for (i = 0; i < TOTAL_SWITCHER_PAGES; i++)
 		__free_pages(lg_switcher_pages[i], 0);
diff --git a/drivers/lightnvm/Makefile b/drivers/lightnvm/Makefile
index 7e0f42acb737..a7a0a22cf1a5 100644
--- a/drivers/lightnvm/Makefile
+++ b/drivers/lightnvm/Makefile
@@ -2,6 +2,6 @@
 # Makefile for Open-Channel SSDs.
 #
 
-obj-$(CONFIG_NVM)		:= core.o
+obj-$(CONFIG_NVM)		:= core.o sysblk.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 8f41b245cd55..9f6acd5d1d2e 100644
--- a/drivers/lightnvm/core.c
+++ b/drivers/lightnvm/core.c
@@ -27,6 +27,7 @@
 #include <linux/module.h>
 #include <linux/miscdevice.h>
 #include <linux/lightnvm.h>
+#include <linux/sched/sysctl.h>
 #include <uapi/linux/lightnvm.h>
 
 static LIST_HEAD(nvm_targets);
@@ -105,6 +106,9 @@ struct nvmm_type *nvm_init_mgr(struct nvm_dev *dev)
 	lockdep_assert_held(&nvm_lock);
 
 	list_for_each_entry(mt, &nvm_mgrs, list) {
+		if (strncmp(dev->sb.mmtype, mt->name, NVM_MMTYPE_LEN))
+			continue;
+
 		ret = mt->register_mgr(dev);
 		if (ret < 0) {
 			pr_err("nvm: media mgr failed to init (%d) on dev %s\n",
@@ -166,6 +170,20 @@ static struct nvm_dev *nvm_find_nvm_dev(const char *name)
 	return NULL;
 }
 
+struct nvm_block *nvm_get_blk_unlocked(struct nvm_dev *dev, struct nvm_lun *lun,
+							unsigned long flags)
+{
+	return dev->mt->get_blk_unlocked(dev, lun, flags);
+}
+EXPORT_SYMBOL(nvm_get_blk_unlocked);
+
+/* Assumes that all valid pages have already been moved on release to bm */
+void nvm_put_blk_unlocked(struct nvm_dev *dev, struct nvm_block *blk)
+{
+	return dev->mt->put_blk_unlocked(dev, blk);
+}
+EXPORT_SYMBOL(nvm_put_blk_unlocked);
+
 struct nvm_block *nvm_get_blk(struct nvm_dev *dev, struct nvm_lun *lun,
 							unsigned long flags)
 {
@@ -192,6 +210,206 @@ int nvm_erase_blk(struct nvm_dev *dev, struct nvm_block *blk)
 }
 EXPORT_SYMBOL(nvm_erase_blk);
 
+void nvm_addr_to_generic_mode(struct nvm_dev *dev, struct nvm_rq *rqd)
+{
+	int i;
+
+	if (rqd->nr_pages > 1) {
+		for (i = 0; i < rqd->nr_pages; i++)
+			rqd->ppa_list[i] = dev_to_generic_addr(dev,
+							rqd->ppa_list[i]);
+	} else {
+		rqd->ppa_addr = dev_to_generic_addr(dev, rqd->ppa_addr);
+	}
+}
+EXPORT_SYMBOL(nvm_addr_to_generic_mode);
+
+void nvm_generic_to_addr_mode(struct nvm_dev *dev, struct nvm_rq *rqd)
+{
+	int i;
+
+	if (rqd->nr_pages > 1) {
+		for (i = 0; i < rqd->nr_pages; i++)
+			rqd->ppa_list[i] = generic_to_dev_addr(dev,
+							rqd->ppa_list[i]);
+	} else {
+		rqd->ppa_addr = generic_to_dev_addr(dev, rqd->ppa_addr);
+	}
+}
+EXPORT_SYMBOL(nvm_generic_to_addr_mode);
+
+int nvm_set_rqd_ppalist(struct nvm_dev *dev, struct nvm_rq *rqd,
+					struct ppa_addr *ppas, int nr_ppas)
+{
+	int i, plane_cnt, pl_idx;
+
+	if (dev->plane_mode == NVM_PLANE_SINGLE && nr_ppas == 1) {
+		rqd->nr_pages = 1;
+		rqd->ppa_addr = ppas[0];
+
+		return 0;
+	}
+
+	plane_cnt = (1 << dev->plane_mode);
+	rqd->nr_pages = plane_cnt * nr_ppas;
+
+	if (dev->ops->max_phys_sect < rqd->nr_pages)
+		return -EINVAL;
+
+	rqd->ppa_list = nvm_dev_dma_alloc(dev, GFP_KERNEL, &rqd->dma_ppa_list);
+	if (!rqd->ppa_list) {
+		pr_err("nvm: failed to allocate dma memory\n");
+		return -ENOMEM;
+	}
+
+	for (pl_idx = 0; pl_idx < plane_cnt; pl_idx++) {
+		for (i = 0; i < nr_ppas; i++) {
+			ppas[i].g.pl = pl_idx;
+			rqd->ppa_list[(pl_idx * nr_ppas) + i] = ppas[i];
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(nvm_set_rqd_ppalist);
+
+void nvm_free_rqd_ppalist(struct nvm_dev *dev, struct nvm_rq *rqd)
+{
+	if (!rqd->ppa_list)
+		return;
+
+	nvm_dev_dma_free(dev, rqd->ppa_list, rqd->dma_ppa_list);
+}
+EXPORT_SYMBOL(nvm_free_rqd_ppalist);
+
+int nvm_erase_ppa(struct nvm_dev *dev, struct ppa_addr *ppas, int nr_ppas)
+{
+	struct nvm_rq rqd;
+	int ret;
+
+	if (!dev->ops->erase_block)
+		return 0;
+
+	memset(&rqd, 0, sizeof(struct nvm_rq));
+
+	ret = nvm_set_rqd_ppalist(dev, &rqd, ppas, nr_ppas);
+	if (ret)
+		return ret;
+
+	nvm_generic_to_addr_mode(dev, &rqd);
+
+	ret = dev->ops->erase_block(dev, &rqd);
+
+	nvm_free_rqd_ppalist(dev, &rqd);
+
+	return ret;
+}
+EXPORT_SYMBOL(nvm_erase_ppa);
+
+void nvm_end_io(struct nvm_rq *rqd, int error)
+{
+	rqd->error = error;
+	rqd->end_io(rqd);
+}
+EXPORT_SYMBOL(nvm_end_io);
+
+static void nvm_end_io_sync(struct nvm_rq *rqd)
+{
+	struct completion *waiting = rqd->wait;
+
+	rqd->wait = NULL;
+
+	complete(waiting);
+}
+
+int nvm_submit_ppa(struct nvm_dev *dev, struct ppa_addr *ppa, int nr_ppas,
+				int opcode, int flags, void *buf, int len)
+{
+	DECLARE_COMPLETION_ONSTACK(wait);
+	struct nvm_rq rqd;
+	struct bio *bio;
+	int ret;
+	unsigned long hang_check;
+
+	bio = bio_map_kern(dev->q, buf, len, GFP_KERNEL);
+	if (IS_ERR_OR_NULL(bio))
+		return -ENOMEM;
+
+	memset(&rqd, 0, sizeof(struct nvm_rq));
+	ret = nvm_set_rqd_ppalist(dev, &rqd, ppa, nr_ppas);
+	if (ret) {
+		bio_put(bio);
+		return ret;
+	}
+
+	rqd.opcode = opcode;
+	rqd.bio = bio;
+	rqd.wait = &wait;
+	rqd.dev = dev;
+	rqd.end_io = nvm_end_io_sync;
+	rqd.flags = flags;
+	nvm_generic_to_addr_mode(dev, &rqd);
+
+	ret = dev->ops->submit_io(dev, &rqd);
+
+	/* Prevent hang_check timer from firing at us during very long I/O */
+	hang_check = sysctl_hung_task_timeout_secs;
+	if (hang_check)
+		while (!wait_for_completion_io_timeout(&wait, hang_check * (HZ/2)));
+	else
+		wait_for_completion_io(&wait);
+
+	nvm_free_rqd_ppalist(dev, &rqd);
+
+	return rqd.error;
+}
+EXPORT_SYMBOL(nvm_submit_ppa);
+
+static int nvm_init_slc_tbl(struct nvm_dev *dev, struct nvm_id_group *grp)
+{
+	int i;
+
+	dev->lps_per_blk = dev->pgs_per_blk;
+	dev->lptbl = kcalloc(dev->lps_per_blk, sizeof(int), GFP_KERNEL);
+	if (!dev->lptbl)
+		return -ENOMEM;
+
+	/* Just a linear array */
+	for (i = 0; i < dev->lps_per_blk; i++)
+		dev->lptbl[i] = i;
+
+	return 0;
+}
+
+static int nvm_init_mlc_tbl(struct nvm_dev *dev, struct nvm_id_group *grp)
+{
+	int i, p;
+	struct nvm_id_lp_mlc *mlc = &grp->lptbl.mlc;
+
+	if (!mlc->num_pairs)
+		return 0;
+
+	dev->lps_per_blk = mlc->num_pairs;
+	dev->lptbl = kcalloc(dev->lps_per_blk, sizeof(int), GFP_KERNEL);
+	if (!dev->lptbl)
+		return -ENOMEM;
+
+	/* The lower page table encoding consists of a list of bytes, where each
+	 * has a lower and an upper half. The first half byte maintains the
+	 * increment value and every value after is an offset added to the
+	 * previous incrementation value */
+	dev->lptbl[0] = mlc->pairs[0] & 0xF;
+	for (i = 1; i < dev->lps_per_blk; i++) {
+		p = mlc->pairs[i >> 1];
+		if (i & 0x1) /* upper */
+			dev->lptbl[i] = dev->lptbl[i - 1] + ((p & 0xF0) >> 4);
+		else /* lower */
+			dev->lptbl[i] = dev->lptbl[i - 1] + (p & 0xF);
+	}
+
+	return 0;
+}
+
 static int nvm_core_init(struct nvm_dev *dev)
 {
 	struct nvm_id *id = &dev->identity;
@@ -206,6 +424,7 @@ static int nvm_core_init(struct nvm_dev *dev)
 	dev->sec_size = grp->csecs;
 	dev->oob_size = grp->sos;
 	dev->sec_per_pg = grp->fpg_sz / grp->csecs;
+	dev->mccap = grp->mccap;
 	memcpy(&dev->ppaf, &id->ppaf, sizeof(struct nvm_addr_format));
 
 	dev->plane_mode = NVM_PLANE_SINGLE;
@@ -216,11 +435,23 @@ static int nvm_core_init(struct nvm_dev *dev)
 		return -EINVAL;
 	}
 
-	if (grp->fmtype != 0 && grp->fmtype != 1) {
+	switch (grp->fmtype) {
+	case NVM_ID_FMTYPE_SLC:
+		if (nvm_init_slc_tbl(dev, grp))
+			return -ENOMEM;
+		break;
+	case NVM_ID_FMTYPE_MLC:
+		if (nvm_init_mlc_tbl(dev, grp))
+			return -ENOMEM;
+		break;
+	default:
 		pr_err("nvm: flash type not supported\n");
 		return -EINVAL;
 	}
 
+	if (!dev->lps_per_blk)
+		pr_info("nvm: lower page programming table missing\n");
+
 	if (grp->mpos & 0x020202)
 		dev->plane_mode = NVM_PLANE_DOUBLE;
 	if (grp->mpos & 0x040404)
@@ -238,6 +469,7 @@ static int nvm_core_init(struct nvm_dev *dev)
 				dev->nr_chnls;
 	dev->total_pages = dev->total_blocks * dev->pgs_per_blk;
 	INIT_LIST_HEAD(&dev->online_targets);
+	mutex_init(&dev->mlock);
 
 	return 0;
 }
@@ -249,6 +481,8 @@ static void nvm_free(struct nvm_dev *dev)
 
 	if (dev->mt)
 		dev->mt->unregister_mgr(dev);
+
+	kfree(dev->lptbl);
 }
 
 static int nvm_init(struct nvm_dev *dev)
@@ -338,9 +572,18 @@ int nvm_register(struct request_queue *q, char *disk_name,
 		}
 	}
 
+	if (dev->identity.cap & NVM_ID_DCAP_BBLKMGMT) {
+		ret = nvm_get_sysblock(dev, &dev->sb);
+		if (!ret)
+			pr_err("nvm: device not initialized.\n");
+		else if (ret < 0)
+			pr_err("nvm: err (%d) on device initialization\n", ret);
+	}
+
 	/* register device with a supported media manager */
 	down_write(&nvm_lock);
-	dev->mt = nvm_init_mgr(dev);
+	if (ret > 0)
+		dev->mt = nvm_init_mgr(dev);
 	list_add(&dev->devices, &nvm_devices);
 	up_write(&nvm_lock);
 
@@ -788,6 +1031,102 @@ static long nvm_ioctl_dev_remove(struct file *file, void __user *arg)
 	return __nvm_configure_remove(&remove);
 }
 
+static void nvm_setup_nvm_sb_info(struct nvm_sb_info *info)
+{
+	info->seqnr = 1;
+	info->erase_cnt = 0;
+	info->version = 1;
+}
+
+static long __nvm_ioctl_dev_init(struct nvm_ioctl_dev_init *init)
+{
+	struct nvm_dev *dev;
+	struct nvm_sb_info info;
+	int ret;
+
+	down_write(&nvm_lock);
+	dev = nvm_find_nvm_dev(init->dev);
+	up_write(&nvm_lock);
+	if (!dev) {
+		pr_err("nvm: device not found\n");
+		return -EINVAL;
+	}
+
+	nvm_setup_nvm_sb_info(&info);
+
+	strncpy(info.mmtype, init->mmtype, NVM_MMTYPE_LEN);
+	info.fs_ppa.ppa = -1;
+
+	if (dev->identity.cap & NVM_ID_DCAP_BBLKMGMT) {
+		ret = nvm_init_sysblock(dev, &info);
+		if (ret)
+			return ret;
+	}
+
+	memcpy(&dev->sb, &info, sizeof(struct nvm_sb_info));
+
+	down_write(&nvm_lock);
+	dev->mt = nvm_init_mgr(dev);
+	up_write(&nvm_lock);
+
+	return 0;
+}
+
+static long nvm_ioctl_dev_init(struct file *file, void __user *arg)
+{
+	struct nvm_ioctl_dev_init init;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (copy_from_user(&init, arg, sizeof(struct nvm_ioctl_dev_init)))
+		return -EFAULT;
+
+	if (init.flags != 0) {
+		pr_err("nvm: no flags supported\n");
+		return -EINVAL;
+	}
+
+	init.dev[DISK_NAME_LEN - 1] = '\0';
+
+	return __nvm_ioctl_dev_init(&init);
+}
+
+static long nvm_ioctl_dev_factory(struct file *file, void __user *arg)
+{
+	struct nvm_ioctl_dev_factory fact;
+	struct nvm_dev *dev;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if (copy_from_user(&fact, arg, sizeof(struct nvm_ioctl_dev_factory)))
+		return -EFAULT;
+
+	fact.dev[DISK_NAME_LEN - 1] = '\0';
+
+	if (fact.flags & ~(NVM_FACTORY_NR_BITS - 1))
+		return -EINVAL;
+
+	down_write(&nvm_lock);
+	dev = nvm_find_nvm_dev(fact.dev);
+	up_write(&nvm_lock);
+	if (!dev) {
+		pr_err("nvm: device not found\n");
+		return -EINVAL;
+	}
+
+	if (dev->mt) {
+		dev->mt->unregister_mgr(dev);
+		dev->mt = NULL;
+	}
+
+	if (dev->identity.cap & NVM_ID_DCAP_BBLKMGMT)
+		return nvm_dev_factory(dev, fact.flags);
+
+	return 0;
+}
+
 static long nvm_ctl_ioctl(struct file *file, uint cmd, unsigned long arg)
 {
 	void __user *argp = (void __user *)arg;
@@ -801,6 +1140,10 @@ static long nvm_ctl_ioctl(struct file *file, uint cmd, unsigned long arg)
 		return nvm_ioctl_dev_create(file, argp);
 	case NVM_DEV_REMOVE:
 		return nvm_ioctl_dev_remove(file, argp);
+	case NVM_DEV_INIT:
+		return nvm_ioctl_dev_init(file, argp);
+	case NVM_DEV_FACTORY:
+		return nvm_ioctl_dev_factory(file, argp);
 	}
 	return 0;
 }
diff --git a/drivers/lightnvm/gennvm.c b/drivers/lightnvm/gennvm.c
index a54b339951a3..7fb725b16148 100644
--- a/drivers/lightnvm/gennvm.c
+++ b/drivers/lightnvm/gennvm.c
@@ -60,7 +60,8 @@ static int gennvm_luns_init(struct nvm_dev *dev, struct gen_nvm *gn)
 		lun->vlun.lun_id = i % dev->luns_per_chnl;
 		lun->vlun.chnl_id = i / dev->luns_per_chnl;
 		lun->vlun.nr_free_blocks = dev->blks_per_lun;
-		lun->vlun.nr_inuse_blocks = 0;
+		lun->vlun.nr_open_blocks = 0;
+		lun->vlun.nr_closed_blocks = 0;
 		lun->vlun.nr_bad_blocks = 0;
 	}
 	return 0;
@@ -89,6 +90,7 @@ static int gennvm_block_bb(struct ppa_addr ppa, int nr_blocks, u8 *blks,
 
 		list_move_tail(&blk->list, &lun->bb_list);
 		lun->vlun.nr_bad_blocks++;
+		lun->vlun.nr_free_blocks--;
 	}
 
 	return 0;
@@ -133,15 +135,15 @@ static int gennvm_block_map(u64 slba, u32 nlb, __le64 *entries, void *private)
 		pba = pba - (dev->sec_per_lun * lun_id);
 		blk = &lun->vlun.blocks[div_u64(pba, dev->sec_per_blk)];
 
-		if (!blk->type) {
+		if (!blk->state) {
 			/* at this point, we don't know anything about the
 			 * block. It's up to the FTL on top to re-etablish the
-			 * block state
+			 * block state. The block is assumed to be open.
 			 */
 			list_move_tail(&blk->list, &lun->used_list);
-			blk->type = 1;
+			blk->state = NVM_BLK_ST_OPEN;
 			lun->vlun.nr_free_blocks--;
-			lun->vlun.nr_inuse_blocks++;
+			lun->vlun.nr_open_blocks++;
 		}
 	}
 
@@ -255,14 +257,14 @@ static void gennvm_unregister(struct nvm_dev *dev)
 	module_put(THIS_MODULE);
 }
 
-static struct nvm_block *gennvm_get_blk(struct nvm_dev *dev,
+static struct nvm_block *gennvm_get_blk_unlocked(struct nvm_dev *dev,
 				struct nvm_lun *vlun, unsigned long flags)
 {
 	struct gen_lun *lun = container_of(vlun, struct gen_lun, vlun);
 	struct nvm_block *blk = NULL;
 	int is_gc = flags & NVM_IOTYPE_GC;
 
-	spin_lock(&vlun->lock);
+	assert_spin_locked(&vlun->lock);
 
 	if (list_empty(&lun->free_list)) {
 		pr_err_ratelimited("gennvm: lun %u have no free pages available",
@@ -275,83 +277,64 @@ static struct nvm_block *gennvm_get_blk(struct nvm_dev *dev,
 
 	blk = list_first_entry(&lun->free_list, struct nvm_block, list);
 	list_move_tail(&blk->list, &lun->used_list);
-	blk->type = 1;
+	blk->state = NVM_BLK_ST_OPEN;
 
 	lun->vlun.nr_free_blocks--;
-	lun->vlun.nr_inuse_blocks++;
+	lun->vlun.nr_open_blocks++;
 
 out:
+	return blk;
+}
+
+static struct nvm_block *gennvm_get_blk(struct nvm_dev *dev,
+				struct nvm_lun *vlun, unsigned long flags)
+{
+	struct nvm_block *blk;
+
+	spin_lock(&vlun->lock);
+	blk = gennvm_get_blk_unlocked(dev, vlun, flags);
 	spin_unlock(&vlun->lock);
 	return blk;
 }
 
-static void gennvm_put_blk(struct nvm_dev *dev, struct nvm_block *blk)
+static void gennvm_put_blk_unlocked(struct nvm_dev *dev, struct nvm_block *blk)
 {
 	struct nvm_lun *vlun = blk->lun;
 	struct gen_lun *lun = container_of(vlun, struct gen_lun, vlun);
 
-	spin_lock(&vlun->lock);
+	assert_spin_locked(&vlun->lock);
 
-	switch (blk->type) {
-	case 1:
+	if (blk->state & NVM_BLK_ST_OPEN) {
 		list_move_tail(&blk->list, &lun->free_list);
+		lun->vlun.nr_open_blocks--;
 		lun->vlun.nr_free_blocks++;
-		lun->vlun.nr_inuse_blocks--;
-		blk->type = 0;
-		break;
-	case 2:
+		blk->state = NVM_BLK_ST_FREE;
+	} else if (blk->state & NVM_BLK_ST_CLOSED) {
+		list_move_tail(&blk->list, &lun->free_list);
+		lun->vlun.nr_closed_blocks--;
+		lun->vlun.nr_free_blocks++;
+		blk->state = NVM_BLK_ST_FREE;
+	} else if (blk->state & NVM_BLK_ST_BAD) {
 		list_move_tail(&blk->list, &lun->bb_list);
 		lun->vlun.nr_bad_blocks++;
-		lun->vlun.nr_inuse_blocks--;
-		break;
-	default:
+		blk->state = NVM_BLK_ST_BAD;
+	} else {
 		WARN_ON_ONCE(1);
 		pr_err("gennvm: erroneous block type (%lu -> %u)\n",
-							blk->id, blk->type);
+							blk->id, blk->state);
 		list_move_tail(&blk->list, &lun->bb_list);
 		lun->vlun.nr_bad_blocks++;
-		lun->vlun.nr_inuse_blocks--;
-	}
-
-	spin_unlock(&vlun->lock);
-}
-
-static void gennvm_addr_to_generic_mode(struct nvm_dev *dev, struct nvm_rq *rqd)
-{
-	int i;
-
-	if (rqd->nr_pages > 1) {
-		for (i = 0; i < rqd->nr_pages; i++)
-			rqd->ppa_list[i] = dev_to_generic_addr(dev,
-							rqd->ppa_list[i]);
-	} else {
-		rqd->ppa_addr = dev_to_generic_addr(dev, rqd->ppa_addr);
+		blk->state = NVM_BLK_ST_BAD;
 	}
 }
 
-static void gennvm_generic_to_addr_mode(struct nvm_dev *dev, struct nvm_rq *rqd)
-{
-	int i;
-
-	if (rqd->nr_pages > 1) {
-		for (i = 0; i < rqd->nr_pages; i++)
-			rqd->ppa_list[i] = generic_to_dev_addr(dev,
-							rqd->ppa_list[i]);
-	} else {
-		rqd->ppa_addr = generic_to_dev_addr(dev, rqd->ppa_addr);
-	}
-}
-
-static int gennvm_submit_io(struct nvm_dev *dev, struct nvm_rq *rqd)
+static void gennvm_put_blk(struct nvm_dev *dev, struct nvm_block *blk)
 {
-	if (!dev->ops->submit_io)
-		return 0;
-
-	/* Convert address space */
-	gennvm_generic_to_addr_mode(dev, rqd);
+	struct nvm_lun *vlun = blk->lun;
 
-	rqd->dev = dev;
-	return dev->ops->submit_io(dev, rqd);
+	spin_lock(&vlun->lock);
+	gennvm_put_blk_unlocked(dev, blk);
+	spin_unlock(&vlun->lock);
 }
 
 static void gennvm_blk_set_type(struct nvm_dev *dev, struct ppa_addr *ppa,
@@ -376,7 +359,7 @@ static void gennvm_blk_set_type(struct nvm_dev *dev, struct ppa_addr *ppa,
 	blk = &lun->vlun.blocks[ppa->g.blk];
 
 	/* will be moved to bb list on put_blk from target */
-	blk->type = type;
+	blk->state = type;
 }
 
 /* mark block bad. It is expected the target recover from the error. */
@@ -390,77 +373,51 @@ static void gennvm_mark_blk_bad(struct nvm_dev *dev, struct nvm_rq *rqd)
 	if (dev->ops->set_bb_tbl(dev, rqd, 1))
 		return;
 
-	gennvm_addr_to_generic_mode(dev, rqd);
+	nvm_addr_to_generic_mode(dev, rqd);
 
 	/* look up blocks and mark them as bad */
 	if (rqd->nr_pages > 1)
 		for (i = 0; i < rqd->nr_pages; i++)
-			gennvm_blk_set_type(dev, &rqd->ppa_list[i], 2);
+			gennvm_blk_set_type(dev, &rqd->ppa_list[i],
+						NVM_BLK_ST_BAD);
 	else
-		gennvm_blk_set_type(dev, &rqd->ppa_addr, 2);
+		gennvm_blk_set_type(dev, &rqd->ppa_addr, NVM_BLK_ST_BAD);
 }
 
-static int gennvm_end_io(struct nvm_rq *rqd, int error)
+static void gennvm_end_io(struct nvm_rq *rqd)
 {
 	struct nvm_tgt_instance *ins = rqd->ins;
-	int ret = 0;
 
-	switch (error) {
+	switch (rqd->error) {
 	case NVM_RSP_SUCCESS:
-		break;
 	case NVM_RSP_ERR_EMPTYPAGE:
 		break;
 	case NVM_RSP_ERR_FAILWRITE:
 		gennvm_mark_blk_bad(rqd->dev, rqd);
-	default:
-		ret++;
 	}
 
-	ret += ins->tt->end_io(rqd, error);
-
-	return ret;
+	ins->tt->end_io(rqd);
 }
 
-static int gennvm_erase_blk(struct nvm_dev *dev, struct nvm_block *blk,
-							unsigned long flags)
+static int gennvm_submit_io(struct nvm_dev *dev, struct nvm_rq *rqd)
 {
-	int plane_cnt = 0, pl_idx, ret;
-	struct ppa_addr addr;
-	struct nvm_rq rqd;
-
-	if (!dev->ops->erase_block)
-		return 0;
-
-	addr = block_to_ppa(dev, blk);
-
-	if (dev->plane_mode == NVM_PLANE_SINGLE) {
-		rqd.nr_pages = 1;
-		rqd.ppa_addr = addr;
-	} else {
-		plane_cnt = (1 << dev->plane_mode);
-		rqd.nr_pages = plane_cnt;
-
-		rqd.ppa_list = nvm_dev_dma_alloc(dev, GFP_KERNEL,
-							&rqd.dma_ppa_list);
-		if (!rqd.ppa_list) {
-			pr_err("gennvm: failed to allocate dma memory\n");
-			return -ENOMEM;
-		}
-
-		for (pl_idx = 0; pl_idx < plane_cnt; pl_idx++) {
-			addr.g.pl = pl_idx;
-			rqd.ppa_list[pl_idx] = addr;
-		}
-	}
+	if (!dev->ops->submit_io)
+		return -ENODEV;
 
-	gennvm_generic_to_addr_mode(dev, &rqd);
+	/* Convert address space */
+	nvm_generic_to_addr_mode(dev, rqd);
 
-	ret = dev->ops->erase_block(dev, &rqd);
+	rqd->dev = dev;
+	rqd->end_io = gennvm_end_io;
+	return dev->ops->submit_io(dev, rqd);
+}
 
-	if (plane_cnt)
-		nvm_dev_dma_free(dev, rqd.ppa_list, rqd.dma_ppa_list);
+static int gennvm_erase_blk(struct nvm_dev *dev, struct nvm_block *blk,
+							unsigned long flags)
+{
+	struct ppa_addr addr = block_to_ppa(dev, blk);
 
-	return ret;
+	return nvm_erase_ppa(dev, &addr, 1);
 }
 
 static struct nvm_lun *gennvm_get_lun(struct nvm_dev *dev, int lunid)
@@ -480,10 +437,11 @@ static void gennvm_lun_info_print(struct nvm_dev *dev)
 	gennvm_for_each_lun(gn, lun, i) {
 		spin_lock(&lun->vlun.lock);
 
-		pr_info("%s: lun%8u\t%u\t%u\t%u\n",
+		pr_info("%s: lun%8u\t%u\t%u\t%u\t%u\n",
 				dev->name, i,
 				lun->vlun.nr_free_blocks,
-				lun->vlun.nr_inuse_blocks,
+				lun->vlun.nr_open_blocks,
+				lun->vlun.nr_closed_blocks,
 				lun->vlun.nr_bad_blocks);
 
 		spin_unlock(&lun->vlun.lock);
@@ -491,21 +449,23 @@ static void gennvm_lun_info_print(struct nvm_dev *dev)
 }
 
 static struct nvmm_type gennvm = {
-	.name		= "gennvm",
-	.version	= {0, 1, 0},
+	.name			= "gennvm",
+	.version		= {0, 1, 0},
+
+	.register_mgr		= gennvm_register,
+	.unregister_mgr		= gennvm_unregister,
 
-	.register_mgr	= gennvm_register,
-	.unregister_mgr	= gennvm_unregister,
+	.get_blk_unlocked	= gennvm_get_blk_unlocked,
+	.put_blk_unlocked	= gennvm_put_blk_unlocked,
 
-	.get_blk	= gennvm_get_blk,
-	.put_blk	= gennvm_put_blk,
+	.get_blk		= gennvm_get_blk,
+	.put_blk		= gennvm_put_blk,
 
-	.submit_io	= gennvm_submit_io,
-	.end_io		= gennvm_end_io,
-	.erase_blk	= gennvm_erase_blk,
+	.submit_io		= gennvm_submit_io,
+	.erase_blk		= gennvm_erase_blk,
 
-	.get_lun	= gennvm_get_lun,
-	.lun_info_print = gennvm_lun_info_print,
+	.get_lun		= gennvm_get_lun,
+	.lun_info_print		= gennvm_lun_info_print,
 };
 
 static int __init gennvm_module_init(void)
diff --git a/drivers/lightnvm/rrpc.c b/drivers/lightnvm/rrpc.c
index 134e4faba482..307db1ea22de 100644
--- a/drivers/lightnvm/rrpc.c
+++ b/drivers/lightnvm/rrpc.c
@@ -179,16 +179,23 @@ static void rrpc_set_lun_cur(struct rrpc_lun *rlun, struct rrpc_block *rblk)
 static struct rrpc_block *rrpc_get_blk(struct rrpc *rrpc, struct rrpc_lun *rlun,
 							unsigned long flags)
 {
+	struct nvm_lun *lun = rlun->parent;
 	struct nvm_block *blk;
 	struct rrpc_block *rblk;
 
-	blk = nvm_get_blk(rrpc->dev, rlun->parent, flags);
-	if (!blk)
+	spin_lock(&lun->lock);
+	blk = nvm_get_blk_unlocked(rrpc->dev, rlun->parent, flags);
+	if (!blk) {
+		pr_err("nvm: rrpc: cannot get new block from media manager\n");
+		spin_unlock(&lun->lock);
 		return NULL;
+	}
 
 	rblk = &rlun->blocks[blk->id];
-	blk->priv = rblk;
+	list_add_tail(&rblk->list, &rlun->open_list);
+	spin_unlock(&lun->lock);
 
+	blk->priv = rblk;
 	bitmap_zero(rblk->invalid_pages, rrpc->dev->pgs_per_blk);
 	rblk->next_page = 0;
 	rblk->nr_invalid_pages = 0;
@@ -199,7 +206,13 @@ static struct rrpc_block *rrpc_get_blk(struct rrpc *rrpc, struct rrpc_lun *rlun,
 
 static void rrpc_put_blk(struct rrpc *rrpc, struct rrpc_block *rblk)
 {
-	nvm_put_blk(rrpc->dev, rblk->parent);
+	struct rrpc_lun *rlun = rblk->rlun;
+	struct nvm_lun *lun = rlun->parent;
+
+	spin_lock(&lun->lock);
+	nvm_put_blk_unlocked(rrpc->dev, rblk->parent);
+	list_del(&rblk->list);
+	spin_unlock(&lun->lock);
 }
 
 static void rrpc_put_blks(struct rrpc *rrpc)
@@ -287,6 +300,10 @@ static int rrpc_move_valid_pages(struct rrpc *rrpc, struct rrpc_block *rblk)
 	}
 
 	page = mempool_alloc(rrpc->page_pool, GFP_NOIO);
+	if (!page) {
+		bio_put(bio);
+		return -ENOMEM;
+	}
 
 	while ((slot = find_first_zero_bit(rblk->invalid_pages,
 					    nr_pgs_per_blk)) < nr_pgs_per_blk) {
@@ -328,6 +345,10 @@ try:
 			goto finished;
 		}
 		wait_for_completion_io(&wait);
+		if (bio->bi_error) {
+			rrpc_inflight_laddr_release(rrpc, rqd);
+			goto finished;
+		}
 
 		bio_reset(bio);
 		reinit_completion(&wait);
@@ -350,6 +371,8 @@ try:
 		wait_for_completion_io(&wait);
 
 		rrpc_inflight_laddr_release(rrpc, rqd);
+		if (bio->bi_error)
+			goto finished;
 
 		bio_reset(bio);
 	}
@@ -373,16 +396,26 @@ static void rrpc_block_gc(struct work_struct *work)
 	struct rrpc *rrpc = gcb->rrpc;
 	struct rrpc_block *rblk = gcb->rblk;
 	struct nvm_dev *dev = rrpc->dev;
+	struct nvm_lun *lun = rblk->parent->lun;
+	struct rrpc_lun *rlun = &rrpc->luns[lun->id - rrpc->lun_offset];
 
+	mempool_free(gcb, rrpc->gcb_pool);
 	pr_debug("nvm: block '%lu' being reclaimed\n", rblk->parent->id);
 
 	if (rrpc_move_valid_pages(rrpc, rblk))
-		goto done;
+		goto put_back;
+
+	if (nvm_erase_blk(dev, rblk->parent))
+		goto put_back;
 
-	nvm_erase_blk(dev, rblk->parent);
 	rrpc_put_blk(rrpc, rblk);
-done:
-	mempool_free(gcb, rrpc->gcb_pool);
+
+	return;
+
+put_back:
+	spin_lock(&rlun->lock);
+	list_add_tail(&rblk->prio, &rlun->prio_list);
+	spin_unlock(&rlun->lock);
 }
 
 /* the block with highest number of invalid pages, will be in the beginning
@@ -427,7 +460,7 @@ static void rrpc_lun_gc(struct work_struct *work)
 	if (nr_blocks_need < rrpc->nr_luns)
 		nr_blocks_need = rrpc->nr_luns;
 
-	spin_lock(&lun->lock);
+	spin_lock(&rlun->lock);
 	while (nr_blocks_need > lun->nr_free_blocks &&
 					!list_empty(&rlun->prio_list)) {
 		struct rrpc_block *rblock = block_prio_find_max(rlun);
@@ -436,16 +469,16 @@ static void rrpc_lun_gc(struct work_struct *work)
 		if (!rblock->nr_invalid_pages)
 			break;
 
+		gcb = mempool_alloc(rrpc->gcb_pool, GFP_ATOMIC);
+		if (!gcb)
+			break;
+
 		list_del_init(&rblock->prio);
 
 		BUG_ON(!block_is_full(rrpc, rblock));
 
 		pr_debug("rrpc: selected block '%lu' for GC\n", block->id);
 
-		gcb = mempool_alloc(rrpc->gcb_pool, GFP_ATOMIC);
-		if (!gcb)
-			break;
-
 		gcb->rrpc = rrpc;
 		gcb->rblk = rblock;
 		INIT_WORK(&gcb->ws_gc, rrpc_block_gc);
@@ -454,7 +487,7 @@ static void rrpc_lun_gc(struct work_struct *work)
 
 		nr_blocks_need--;
 	}
-	spin_unlock(&lun->lock);
+	spin_unlock(&rlun->lock);
 
 	/* TODO: Hint that request queue can be started again */
 }
@@ -635,12 +668,24 @@ static void rrpc_end_io_write(struct rrpc *rrpc, struct rrpc_rq *rrqd,
 		lun = rblk->parent->lun;
 
 		cmnt_size = atomic_inc_return(&rblk->data_cmnt_size);
-		if (unlikely(cmnt_size == rrpc->dev->pgs_per_blk))
+		if (unlikely(cmnt_size == rrpc->dev->pgs_per_blk)) {
+			struct nvm_block *blk = rblk->parent;
+			struct rrpc_lun *rlun = rblk->rlun;
+
+			spin_lock(&lun->lock);
+			lun->nr_open_blocks--;
+			lun->nr_closed_blocks++;
+			blk->state &= ~NVM_BLK_ST_OPEN;
+			blk->state |= NVM_BLK_ST_CLOSED;
+			list_move_tail(&rblk->list, &rlun->closed_list);
+			spin_unlock(&lun->lock);
+
 			rrpc_run_gc(rrpc, rblk);
+		}
 	}
 }
 
-static int rrpc_end_io(struct nvm_rq *rqd, int error)
+static void rrpc_end_io(struct nvm_rq *rqd)
 {
 	struct rrpc *rrpc = container_of(rqd->ins, struct rrpc, instance);
 	struct rrpc_rq *rrqd = nvm_rq_to_pdu(rqd);
@@ -650,11 +695,12 @@ static int rrpc_end_io(struct nvm_rq *rqd, int error)
 	if (bio_data_dir(rqd->bio) == WRITE)
 		rrpc_end_io_write(rrpc, rrqd, laddr, npages);
 
+	bio_put(rqd->bio);
+
 	if (rrqd->flags & NVM_IOTYPE_GC)
-		return 0;
+		return;
 
 	rrpc_unlock_rq(rrpc, rqd);
-	bio_put(rqd->bio);
 
 	if (npages > 1)
 		nvm_dev_dma_free(rrpc->dev, rqd->ppa_list, rqd->dma_ppa_list);
@@ -662,8 +708,6 @@ static int rrpc_end_io(struct nvm_rq *rqd, int error)
 		nvm_dev_dma_free(rrpc->dev, rqd->metadata, rqd->dma_metadata);
 
 	mempool_free(rqd, rrpc->rq_pool);
-
-	return 0;
 }
 
 static int rrpc_read_ppalist_rq(struct rrpc *rrpc, struct bio *bio,
@@ -841,6 +885,13 @@ static int rrpc_submit_io(struct rrpc *rrpc, struct bio *bio,
 	err = nvm_submit_io(rrpc->dev, rqd);
 	if (err) {
 		pr_err("rrpc: I/O submission failed: %d\n", err);
+		bio_put(bio);
+		if (!(flags & NVM_IOTYPE_GC)) {
+			rrpc_unlock_rq(rrpc, rqd);
+			if (rqd->nr_pages > 1)
+				nvm_dev_dma_free(rrpc->dev,
+			rqd->ppa_list, rqd->dma_ppa_list);
+		}
 		return NVM_IO_ERR;
 	}
 
@@ -1090,6 +1141,11 @@ static int rrpc_luns_init(struct rrpc *rrpc, int lun_begin, int lun_end)
 	struct rrpc_lun *rlun;
 	int i, j;
 
+	if (dev->pgs_per_blk > MAX_INVALID_PAGES_STORAGE * BITS_PER_LONG) {
+		pr_err("rrpc: number of pages per block too high.");
+		return -EINVAL;
+	}
+
 	spin_lock_init(&rrpc->rev_lock);
 
 	rrpc->luns = kcalloc(rrpc->nr_luns, sizeof(struct rrpc_lun),
@@ -1101,16 +1157,13 @@ static int rrpc_luns_init(struct rrpc *rrpc, int lun_begin, int lun_end)
 	for (i = 0; i < rrpc->nr_luns; i++) {
 		struct nvm_lun *lun = dev->mt->get_lun(dev, lun_begin + i);
 
-		if (dev->pgs_per_blk >
-				MAX_INVALID_PAGES_STORAGE * BITS_PER_LONG) {
-			pr_err("rrpc: number of pages per block too high.");
-			goto err;
-		}
-
 		rlun = &rrpc->luns[i];
 		rlun->rrpc = rrpc;
 		rlun->parent = lun;
 		INIT_LIST_HEAD(&rlun->prio_list);
+		INIT_LIST_HEAD(&rlun->open_list);
+		INIT_LIST_HEAD(&rlun->closed_list);
+
 		INIT_WORK(&rlun->ws_gc, rrpc_lun_gc);
 		spin_lock_init(&rlun->lock);
 
@@ -1127,6 +1180,7 @@ static int rrpc_luns_init(struct rrpc *rrpc, int lun_begin, int lun_end)
 			struct nvm_block *blk = &lun->blocks[j];
 
 			rblk->parent = blk;
+			rblk->rlun = rlun;
 			INIT_LIST_HEAD(&rblk->prio);
 			spin_lock_init(&rblk->lock);
 		}
diff --git a/drivers/lightnvm/rrpc.h b/drivers/lightnvm/rrpc.h
index a9696a06c38c..f7b37336353f 100644
--- a/drivers/lightnvm/rrpc.h
+++ b/drivers/lightnvm/rrpc.h
@@ -54,7 +54,9 @@ struct rrpc_rq {
 
 struct rrpc_block {
 	struct nvm_block *parent;
+	struct rrpc_lun *rlun;
 	struct list_head prio;
+	struct list_head list;
 
 #define MAX_INVALID_PAGES_STORAGE 8
 	/* Bitmap for invalid page intries */
@@ -73,7 +75,16 @@ struct rrpc_lun {
 	struct nvm_lun *parent;
 	struct rrpc_block *cur, *gc_cur;
 	struct rrpc_block *blocks;	/* Reference to block allocation */
-	struct list_head prio_list;		/* Blocks that may be GC'ed */
+
+	struct list_head prio_list;	/* Blocks that may be GC'ed */
+	struct list_head open_list;	/* In-use open blocks. These are blocks
+					 * that can be both written to and read
+					 * from
+					 */
+	struct list_head closed_list;	/* In-use closed blocks. These are
+					 * blocks that can _only_ be read from
+					 */
+
 	struct work_struct ws_gc;
 
 	spinlock_t lock;
@@ -163,8 +174,7 @@ static inline sector_t rrpc_get_sector(sector_t laddr)
 static inline int request_intersects(struct rrpc_inflight_rq *r,
 				sector_t laddr_start, sector_t laddr_end)
 {
-	return (laddr_end >= r->l_start && laddr_end <= r->l_end) &&
-		(laddr_start >= r->l_start && laddr_start <= r->l_end);
+	return (laddr_end >= r->l_start) && (laddr_start <= r->l_end);
 }
 
 static int __rrpc_lock_laddr(struct rrpc *rrpc, sector_t laddr,
@@ -173,6 +183,8 @@ static int __rrpc_lock_laddr(struct rrpc *rrpc, sector_t laddr,
 	sector_t laddr_end = laddr + pages - 1;
 	struct rrpc_inflight_rq *rtmp;
 
+	WARN_ON(irqs_disabled());
+
 	spin_lock_irq(&rrpc->inflights.lock);
 	list_for_each_entry(rtmp, &rrpc->inflights.reqs, list) {
 		if (unlikely(request_intersects(rtmp, laddr, laddr_end))) {
diff --git a/drivers/lightnvm/sysblk.c b/drivers/lightnvm/sysblk.c
new file mode 100644
index 000000000000..321de1f154c5
--- /dev/null
+++ b/drivers/lightnvm/sysblk.c
@@ -0,0 +1,741 @@
+/*
+ * Copyright (C) 2015 Matias Bjorling. 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.
+ *
+ * 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.
+ *
+ */
+
+#include <linux/lightnvm.h>
+
+#define MAX_SYSBLKS 3	/* remember to update mapping scheme on change */
+#define MAX_BLKS_PR_SYSBLK 2 /* 2 blks with 256 pages and 3000 erases
+			      * enables ~1.5M updates per sysblk unit
+			      */
+
+struct sysblk_scan {
+	/* A row is a collection of flash blocks for a system block. */
+	int nr_rows;
+	int row;
+	int act_blk[MAX_SYSBLKS];
+
+	int nr_ppas;
+	struct ppa_addr ppas[MAX_SYSBLKS * MAX_BLKS_PR_SYSBLK];/* all sysblks */
+};
+
+static inline int scan_ppa_idx(int row, int blkid)
+{
+	return (row * MAX_BLKS_PR_SYSBLK) + blkid;
+}
+
+void nvm_sysblk_to_cpu(struct nvm_sb_info *info, struct nvm_system_block *sb)
+{
+	info->seqnr = be32_to_cpu(sb->seqnr);
+	info->erase_cnt = be32_to_cpu(sb->erase_cnt);
+	info->version = be16_to_cpu(sb->version);
+	strncpy(info->mmtype, sb->mmtype, NVM_MMTYPE_LEN);
+	info->fs_ppa.ppa = be64_to_cpu(sb->fs_ppa);
+}
+
+void nvm_cpu_to_sysblk(struct nvm_system_block *sb, struct nvm_sb_info *info)
+{
+	sb->magic = cpu_to_be32(NVM_SYSBLK_MAGIC);
+	sb->seqnr = cpu_to_be32(info->seqnr);
+	sb->erase_cnt = cpu_to_be32(info->erase_cnt);
+	sb->version = cpu_to_be16(info->version);
+	strncpy(sb->mmtype, info->mmtype, NVM_MMTYPE_LEN);
+	sb->fs_ppa = cpu_to_be64(info->fs_ppa.ppa);
+}
+
+static int nvm_setup_sysblks(struct nvm_dev *dev, struct ppa_addr *sysblk_ppas)
+{
+	int nr_rows = min_t(int, MAX_SYSBLKS, dev->nr_chnls);
+	int i;
+
+	for (i = 0; i < nr_rows; i++)
+		sysblk_ppas[i].ppa = 0;
+
+	/* if possible, place sysblk at first channel, middle channel and last
+	 * channel of the device. If not, create only one or two sys blocks
+	 */
+	switch (dev->nr_chnls) {
+	case 2:
+		sysblk_ppas[1].g.ch = 1;
+		/* fall-through */
+	case 1:
+		sysblk_ppas[0].g.ch = 0;
+		break;
+	default:
+		sysblk_ppas[0].g.ch = 0;
+		sysblk_ppas[1].g.ch = dev->nr_chnls / 2;
+		sysblk_ppas[2].g.ch = dev->nr_chnls - 1;
+		break;
+	}
+
+	return nr_rows;
+}
+
+void nvm_setup_sysblk_scan(struct nvm_dev *dev, struct sysblk_scan *s,
+						struct ppa_addr *sysblk_ppas)
+{
+	memset(s, 0, sizeof(struct sysblk_scan));
+	s->nr_rows = nvm_setup_sysblks(dev, sysblk_ppas);
+}
+
+static int sysblk_get_host_blks(struct ppa_addr ppa, int nr_blks, u8 *blks,
+								void *private)
+{
+	struct sysblk_scan *s = private;
+	int i, nr_sysblk = 0;
+
+	for (i = 0; i < nr_blks; i++) {
+		if (blks[i] != NVM_BLK_T_HOST)
+			continue;
+
+		if (s->nr_ppas == MAX_BLKS_PR_SYSBLK * MAX_SYSBLKS) {
+			pr_err("nvm: too many host blks\n");
+			return -EINVAL;
+		}
+
+		ppa.g.blk = i;
+
+		s->ppas[scan_ppa_idx(s->row, nr_sysblk)] = ppa;
+		s->nr_ppas++;
+		nr_sysblk++;
+	}
+
+	return 0;
+}
+
+static int nvm_get_all_sysblks(struct nvm_dev *dev, struct sysblk_scan *s,
+				struct ppa_addr *ppas, nvm_bb_update_fn *fn)
+{
+	struct ppa_addr dppa;
+	int i, ret;
+
+	s->nr_ppas = 0;
+
+	for (i = 0; i < s->nr_rows; i++) {
+		dppa = generic_to_dev_addr(dev, ppas[i]);
+		s->row = i;
+
+		ret = dev->ops->get_bb_tbl(dev, dppa, dev->blks_per_lun, fn, s);
+		if (ret) {
+			pr_err("nvm: failed bb tbl for ppa (%u %u)\n",
+							ppas[i].g.ch,
+							ppas[i].g.blk);
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * scans a block for latest sysblk.
+ * Returns:
+ *	0 - newer sysblk not found. PPA is updated to latest page.
+ *	1 - newer sysblk found and stored in *cur. PPA is updated to
+ *	    next valid page.
+ *	<0- error.
+ */
+static int nvm_scan_block(struct nvm_dev *dev, struct ppa_addr *ppa,
+						struct nvm_system_block *sblk)
+{
+	struct nvm_system_block *cur;
+	int pg, cursz, ret, found = 0;
+
+	/* the full buffer for a flash page is allocated. Only the first of it
+	 * contains the system block information
+	 */
+	cursz = dev->sec_size * dev->sec_per_pg * dev->nr_planes;
+	cur = kmalloc(cursz, GFP_KERNEL);
+	if (!cur)
+		return -ENOMEM;
+
+	/* perform linear scan through the block */
+	for (pg = 0; pg < dev->lps_per_blk; pg++) {
+		ppa->g.pg = ppa_to_slc(dev, pg);
+
+		ret = nvm_submit_ppa(dev, ppa, 1, NVM_OP_PREAD, NVM_IO_SLC_MODE,
+								cur, cursz);
+		if (ret) {
+			if (ret == NVM_RSP_ERR_EMPTYPAGE) {
+				pr_debug("nvm: sysblk scan empty ppa (%u %u %u %u)\n",
+							ppa->g.ch,
+							ppa->g.lun,
+							ppa->g.blk,
+							ppa->g.pg);
+				break;
+			}
+			pr_err("nvm: read failed (%x) for ppa (%u %u %u %u)",
+							ret,
+							ppa->g.ch,
+							ppa->g.lun,
+							ppa->g.blk,
+							ppa->g.pg);
+			break; /* if we can't read a page, continue to the
+				* next blk
+				*/
+		}
+
+		if (be32_to_cpu(cur->magic) != NVM_SYSBLK_MAGIC) {
+			pr_debug("nvm: scan break for ppa (%u %u %u %u)\n",
+							ppa->g.ch,
+							ppa->g.lun,
+							ppa->g.blk,
+							ppa->g.pg);
+			break; /* last valid page already found */
+		}
+
+		if (be32_to_cpu(cur->seqnr) < be32_to_cpu(sblk->seqnr))
+			continue;
+
+		memcpy(sblk, cur, sizeof(struct nvm_system_block));
+		found = 1;
+	}
+
+	kfree(cur);
+
+	return found;
+}
+
+static int nvm_set_bb_tbl(struct nvm_dev *dev, struct sysblk_scan *s, int type)
+{
+	struct nvm_rq rqd;
+	int ret;
+
+	if (s->nr_ppas > dev->ops->max_phys_sect) {
+		pr_err("nvm: unable to update all sysblocks atomically\n");
+		return -EINVAL;
+	}
+
+	memset(&rqd, 0, sizeof(struct nvm_rq));
+
+	nvm_set_rqd_ppalist(dev, &rqd, s->ppas, s->nr_ppas);
+	nvm_generic_to_addr_mode(dev, &rqd);
+
+	ret = dev->ops->set_bb_tbl(dev, &rqd, type);
+	nvm_free_rqd_ppalist(dev, &rqd);
+	if (ret) {
+		pr_err("nvm: sysblk failed bb mark\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int sysblk_get_free_blks(struct ppa_addr ppa, int nr_blks, u8 *blks,
+								void *private)
+{
+	struct sysblk_scan *s = private;
+	struct ppa_addr *sppa;
+	int i, blkid = 0;
+
+	for (i = 0; i < nr_blks; i++) {
+		if (blks[i] == NVM_BLK_T_HOST)
+			return -EEXIST;
+
+		if (blks[i] != NVM_BLK_T_FREE)
+			continue;
+
+		sppa = &s->ppas[scan_ppa_idx(s->row, blkid)];
+		sppa->g.ch = ppa.g.ch;
+		sppa->g.lun = ppa.g.lun;
+		sppa->g.blk = i;
+		s->nr_ppas++;
+		blkid++;
+
+		pr_debug("nvm: use (%u %u %u) as sysblk\n",
+					sppa->g.ch, sppa->g.lun, sppa->g.blk);
+		if (blkid > MAX_BLKS_PR_SYSBLK - 1)
+			return 0;
+	}
+
+	pr_err("nvm: sysblk failed get sysblk\n");
+	return -EINVAL;
+}
+
+static int nvm_write_and_verify(struct nvm_dev *dev, struct nvm_sb_info *info,
+							struct sysblk_scan *s)
+{
+	struct nvm_system_block nvmsb;
+	void *buf;
+	int i, sect, ret, bufsz;
+	struct ppa_addr *ppas;
+
+	nvm_cpu_to_sysblk(&nvmsb, info);
+
+	/* buffer for flash page */
+	bufsz = dev->sec_size * dev->sec_per_pg * dev->nr_planes;
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	memcpy(buf, &nvmsb, sizeof(struct nvm_system_block));
+
+	ppas = kcalloc(dev->sec_per_pg, sizeof(struct ppa_addr), GFP_KERNEL);
+	if (!ppas) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Write and verify */
+	for (i = 0; i < s->nr_rows; i++) {
+		ppas[0] = s->ppas[scan_ppa_idx(i, s->act_blk[i])];
+
+		pr_debug("nvm: writing sysblk to ppa (%u %u %u %u)\n",
+							ppas[0].g.ch,
+							ppas[0].g.lun,
+							ppas[0].g.blk,
+							ppas[0].g.pg);
+
+		/* Expand to all sectors within a flash page */
+		if (dev->sec_per_pg > 1) {
+			for (sect = 1; sect < dev->sec_per_pg; sect++) {
+				ppas[sect].ppa = ppas[0].ppa;
+				ppas[sect].g.sec = sect;
+			}
+		}
+
+		ret = nvm_submit_ppa(dev, ppas, dev->sec_per_pg, NVM_OP_PWRITE,
+						NVM_IO_SLC_MODE, buf, bufsz);
+		if (ret) {
+			pr_err("nvm: sysblk failed program (%u %u %u)\n",
+							ppas[0].g.ch,
+							ppas[0].g.lun,
+							ppas[0].g.blk);
+			break;
+		}
+
+		ret = nvm_submit_ppa(dev, ppas, dev->sec_per_pg, NVM_OP_PREAD,
+						NVM_IO_SLC_MODE, buf, bufsz);
+		if (ret) {
+			pr_err("nvm: sysblk failed read (%u %u %u)\n",
+							ppas[0].g.ch,
+							ppas[0].g.lun,
+							ppas[0].g.blk);
+			break;
+		}
+
+		if (memcmp(buf, &nvmsb, sizeof(struct nvm_system_block))) {
+			pr_err("nvm: sysblk failed verify (%u %u %u)\n",
+							ppas[0].g.ch,
+							ppas[0].g.lun,
+							ppas[0].g.blk);
+			ret = -EINVAL;
+			break;
+		}
+	}
+
+	kfree(ppas);
+err:
+	kfree(buf);
+
+	return ret;
+}
+
+static int nvm_prepare_new_sysblks(struct nvm_dev *dev, struct sysblk_scan *s)
+{
+	int i, ret;
+	unsigned long nxt_blk;
+	struct ppa_addr *ppa;
+
+	for (i = 0; i < s->nr_rows; i++) {
+		nxt_blk = (s->act_blk[i] + 1) % MAX_BLKS_PR_SYSBLK;
+		ppa = &s->ppas[scan_ppa_idx(i, nxt_blk)];
+		ppa->g.pg = ppa_to_slc(dev, 0);
+
+		ret = nvm_erase_ppa(dev, ppa, 1);
+		if (ret)
+			return ret;
+
+		s->act_blk[i] = nxt_blk;
+	}
+
+	return 0;
+}
+
+int nvm_get_sysblock(struct nvm_dev *dev, struct nvm_sb_info *info)
+{
+	struct ppa_addr sysblk_ppas[MAX_SYSBLKS];
+	struct sysblk_scan s;
+	struct nvm_system_block *cur;
+	int i, j, found = 0;
+	int ret = -ENOMEM;
+
+	/*
+	 * 1. setup sysblk locations
+	 * 2. get bad block list
+	 * 3. filter on host-specific (type 3)
+	 * 4. iterate through all and find the highest seq nr.
+	 * 5. return superblock information
+	 */
+
+	if (!dev->ops->get_bb_tbl)
+		return -EINVAL;
+
+	nvm_setup_sysblk_scan(dev, &s, sysblk_ppas);
+
+	mutex_lock(&dev->mlock);
+	ret = nvm_get_all_sysblks(dev, &s, sysblk_ppas, sysblk_get_host_blks);
+	if (ret)
+		goto err_sysblk;
+
+	/* no sysblocks initialized */
+	if (!s.nr_ppas)
+		goto err_sysblk;
+
+	cur = kzalloc(sizeof(struct nvm_system_block), GFP_KERNEL);
+	if (!cur)
+		goto err_sysblk;
+
+	/* find the latest block across all sysblocks */
+	for (i = 0; i < s.nr_rows; i++) {
+		for (j = 0; j < MAX_BLKS_PR_SYSBLK; j++) {
+			struct ppa_addr ppa = s.ppas[scan_ppa_idx(i, j)];
+
+			ret = nvm_scan_block(dev, &ppa, cur);
+			if (ret > 0)
+				found = 1;
+			else if (ret < 0)
+				break;
+		}
+	}
+
+	nvm_sysblk_to_cpu(info, cur);
+
+	kfree(cur);
+err_sysblk:
+	mutex_unlock(&dev->mlock);
+
+	if (found)
+		return 1;
+	return ret;
+}
+
+int nvm_update_sysblock(struct nvm_dev *dev, struct nvm_sb_info *new)
+{
+	/* 1. for each latest superblock
+	 * 2. if room
+	 *    a. write new flash page entry with the updated information
+	 * 3. if no room
+	 *    a. find next available block on lun (linear search)
+	 *       if none, continue to next lun
+	 *       if none at all, report error. also report that it wasn't
+	 *       possible to write to all superblocks.
+	 *    c. write data to block.
+	 */
+	struct ppa_addr sysblk_ppas[MAX_SYSBLKS];
+	struct sysblk_scan s;
+	struct nvm_system_block *cur;
+	int i, j, ppaidx, found = 0;
+	int ret = -ENOMEM;
+
+	if (!dev->ops->get_bb_tbl)
+		return -EINVAL;
+
+	nvm_setup_sysblk_scan(dev, &s, sysblk_ppas);
+
+	mutex_lock(&dev->mlock);
+	ret = nvm_get_all_sysblks(dev, &s, sysblk_ppas, sysblk_get_host_blks);
+	if (ret)
+		goto err_sysblk;
+
+	cur = kzalloc(sizeof(struct nvm_system_block), GFP_KERNEL);
+	if (!cur)
+		goto err_sysblk;
+
+	/* Get the latest sysblk for each sysblk row */
+	for (i = 0; i < s.nr_rows; i++) {
+		found = 0;
+		for (j = 0; j < MAX_BLKS_PR_SYSBLK; j++) {
+			ppaidx = scan_ppa_idx(i, j);
+			ret = nvm_scan_block(dev, &s.ppas[ppaidx], cur);
+			if (ret > 0) {
+				s.act_blk[i] = j;
+				found = 1;
+			} else if (ret < 0)
+				break;
+		}
+	}
+
+	if (!found) {
+		pr_err("nvm: no valid sysblks found to update\n");
+		ret = -EINVAL;
+		goto err_cur;
+	}
+
+	/*
+	 * All sysblocks found. Check that they have same page id in their flash
+	 * blocks
+	 */
+	for (i = 1; i < s.nr_rows; i++) {
+		struct ppa_addr l = s.ppas[scan_ppa_idx(0, s.act_blk[0])];
+		struct ppa_addr r = s.ppas[scan_ppa_idx(i, s.act_blk[i])];
+
+		if (l.g.pg != r.g.pg) {
+			pr_err("nvm: sysblks not on same page. Previous update failed.\n");
+			ret = -EINVAL;
+			goto err_cur;
+		}
+	}
+
+	/*
+	 * Check that there haven't been another update to the seqnr since we
+	 * began
+	 */
+	if ((new->seqnr - 1) != be32_to_cpu(cur->seqnr)) {
+		pr_err("nvm: seq is not sequential\n");
+		ret = -EINVAL;
+		goto err_cur;
+	}
+
+	/*
+	 * When all pages in a block has been written, a new block is selected
+	 * and writing is performed on the new block.
+	 */
+	if (s.ppas[scan_ppa_idx(0, s.act_blk[0])].g.pg ==
+						dev->lps_per_blk - 1) {
+		ret = nvm_prepare_new_sysblks(dev, &s);
+		if (ret)
+			goto err_cur;
+	}
+
+	ret = nvm_write_and_verify(dev, new, &s);
+err_cur:
+	kfree(cur);
+err_sysblk:
+	mutex_unlock(&dev->mlock);
+
+	return ret;
+}
+
+int nvm_init_sysblock(struct nvm_dev *dev, struct nvm_sb_info *info)
+{
+	struct ppa_addr sysblk_ppas[MAX_SYSBLKS];
+	struct sysblk_scan s;
+	int ret;
+
+	/*
+	 * 1. select master blocks and select first available blks
+	 * 2. get bad block list
+	 * 3. mark MAX_SYSBLKS block as host-based device allocated.
+	 * 4. write and verify data to block
+	 */
+
+	if (!dev->ops->get_bb_tbl || !dev->ops->set_bb_tbl)
+		return -EINVAL;
+
+	if (!(dev->mccap & NVM_ID_CAP_SLC) || !dev->lps_per_blk) {
+		pr_err("nvm: memory does not support SLC access\n");
+		return -EINVAL;
+	}
+
+	/* Index all sysblocks and mark them as host-driven */
+	nvm_setup_sysblk_scan(dev, &s, sysblk_ppas);
+
+	mutex_lock(&dev->mlock);
+	ret = nvm_get_all_sysblks(dev, &s, sysblk_ppas, sysblk_get_free_blks);
+	if (ret)
+		goto err_mark;
+
+	ret = nvm_set_bb_tbl(dev, &s, NVM_BLK_T_HOST);
+	if (ret)
+		goto err_mark;
+
+	/* Write to the first block of each row */
+	ret = nvm_write_and_verify(dev, info, &s);
+err_mark:
+	mutex_unlock(&dev->mlock);
+	return ret;
+}
+
+struct factory_blks {
+	struct nvm_dev *dev;
+	int flags;
+	unsigned long *blks;
+};
+
+static int factory_nblks(int nblks)
+{
+	/* Round up to nearest BITS_PER_LONG */
+	return (nblks + (BITS_PER_LONG - 1)) & ~(BITS_PER_LONG - 1);
+}
+
+static unsigned int factory_blk_offset(struct nvm_dev *dev, int ch, int lun)
+{
+	int nblks = factory_nblks(dev->blks_per_lun);
+
+	return ((ch * dev->luns_per_chnl * nblks) + (lun * nblks)) /
+								BITS_PER_LONG;
+}
+
+static int nvm_factory_blks(struct ppa_addr ppa, int nr_blks, u8 *blks,
+								void *private)
+{
+	struct factory_blks *f = private;
+	struct nvm_dev *dev = f->dev;
+	int i, lunoff;
+
+	lunoff = factory_blk_offset(dev, ppa.g.ch, ppa.g.lun);
+
+	/* non-set bits correspond to the block must be erased */
+	for (i = 0; i < nr_blks; i++) {
+		switch (blks[i]) {
+		case NVM_BLK_T_FREE:
+			if (f->flags & NVM_FACTORY_ERASE_ONLY_USER)
+				set_bit(i, &f->blks[lunoff]);
+			break;
+		case NVM_BLK_T_HOST:
+			if (!(f->flags & NVM_FACTORY_RESET_HOST_BLKS))
+				set_bit(i, &f->blks[lunoff]);
+			break;
+		case NVM_BLK_T_GRWN_BAD:
+			if (!(f->flags & NVM_FACTORY_RESET_GRWN_BBLKS))
+				set_bit(i, &f->blks[lunoff]);
+			break;
+		default:
+			set_bit(i, &f->blks[lunoff]);
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int nvm_fact_get_blks(struct nvm_dev *dev, struct ppa_addr *erase_list,
+					int max_ppas, struct factory_blks *f)
+{
+	struct ppa_addr ppa;
+	int ch, lun, blkid, idx, done = 0, ppa_cnt = 0;
+	unsigned long *offset;
+
+	while (!done) {
+		done = 1;
+		for (ch = 0; ch < dev->nr_chnls; ch++) {
+			for (lun = 0; lun < dev->luns_per_chnl; lun++) {
+				idx = factory_blk_offset(dev, ch, lun);
+				offset = &f->blks[idx];
+
+				blkid = find_first_zero_bit(offset,
+							dev->blks_per_lun);
+				if (blkid >= dev->blks_per_lun)
+					continue;
+				set_bit(blkid, offset);
+
+				ppa.ppa = 0;
+				ppa.g.ch = ch;
+				ppa.g.lun = lun;
+				ppa.g.blk = blkid;
+				pr_debug("nvm: erase ppa (%u %u %u)\n",
+								ppa.g.ch,
+								ppa.g.lun,
+								ppa.g.blk);
+
+				erase_list[ppa_cnt] = ppa;
+				ppa_cnt++;
+				done = 0;
+
+				if (ppa_cnt == max_ppas)
+					return ppa_cnt;
+			}
+		}
+	}
+
+	return ppa_cnt;
+}
+
+static int nvm_fact_get_bb_tbl(struct nvm_dev *dev, struct ppa_addr ppa,
+					nvm_bb_update_fn *fn, void *priv)
+{
+	struct ppa_addr dev_ppa;
+	int ret;
+
+	dev_ppa = generic_to_dev_addr(dev, ppa);
+
+	ret = dev->ops->get_bb_tbl(dev, dev_ppa, dev->blks_per_lun, fn, priv);
+	if (ret)
+		pr_err("nvm: failed bb tbl for ch%u lun%u\n",
+							ppa.g.ch, ppa.g.blk);
+	return ret;
+}
+
+static int nvm_fact_select_blks(struct nvm_dev *dev, struct factory_blks *f)
+{
+	int ch, lun, ret;
+	struct ppa_addr ppa;
+
+	ppa.ppa = 0;
+	for (ch = 0; ch < dev->nr_chnls; ch++) {
+		for (lun = 0; lun < dev->luns_per_chnl; lun++) {
+			ppa.g.ch = ch;
+			ppa.g.lun = lun;
+
+			ret = nvm_fact_get_bb_tbl(dev, ppa, nvm_factory_blks,
+									f);
+			if (ret)
+				return ret;
+		}
+	}
+
+	return 0;
+}
+
+int nvm_dev_factory(struct nvm_dev *dev, int flags)
+{
+	struct factory_blks f;
+	struct ppa_addr *ppas;
+	int ppa_cnt, ret = -ENOMEM;
+	int max_ppas = dev->ops->max_phys_sect / dev->nr_planes;
+	struct ppa_addr sysblk_ppas[MAX_SYSBLKS];
+	struct sysblk_scan s;
+
+	f.blks = kzalloc(factory_nblks(dev->blks_per_lun) * dev->nr_luns,
+								GFP_KERNEL);
+	if (!f.blks)
+		return ret;
+
+	ppas = kcalloc(max_ppas, sizeof(struct ppa_addr), GFP_KERNEL);
+	if (!ppas)
+		goto err_blks;
+
+	f.dev = dev;
+	f.flags = flags;
+
+	/* create list of blks to be erased */
+	ret = nvm_fact_select_blks(dev, &f);
+	if (ret)
+		goto err_ppas;
+
+	/* continue to erase until list of blks until empty */
+	while ((ppa_cnt = nvm_fact_get_blks(dev, ppas, max_ppas, &f)) > 0)
+		nvm_erase_ppa(dev, ppas, ppa_cnt);
+
+	/* mark host reserved blocks free */
+	if (flags & NVM_FACTORY_RESET_HOST_BLKS) {
+		nvm_setup_sysblk_scan(dev, &s, sysblk_ppas);
+		mutex_lock(&dev->mlock);
+		ret = nvm_get_all_sysblks(dev, &s, sysblk_ppas,
+							sysblk_get_host_blks);
+		if (!ret)
+			ret = nvm_set_bb_tbl(dev, &s, NVM_BLK_T_FREE);
+		mutex_unlock(&dev->mlock);
+	}
+err_ppas:
+	kfree(ppas);
+err_blks:
+	kfree(f.blks);
+	return ret;
+}
+EXPORT_SYMBOL(nvm_dev_factory);
diff --git a/drivers/macintosh/rack-meter.c b/drivers/macintosh/rack-meter.c
index 048901a1111a..caaec654d7ea 100644
--- a/drivers/macintosh/rack-meter.c
+++ b/drivers/macintosh/rack-meter.c
@@ -582,6 +582,7 @@ static struct of_device_id rackmeter_match[] = {
 	{ .name = "i2s" },
 	{ }
 };
+MODULE_DEVICE_TABLE(of, rackmeter_match);
 
 static struct macio_driver rackmeter_driver = {
 	.driver = {
diff --git a/drivers/macintosh/via-pmu.c b/drivers/macintosh/via-pmu.c
index f9512bfa6c3c..01ee736fe0ef 100644
--- a/drivers/macintosh/via-pmu.c
+++ b/drivers/macintosh/via-pmu.c
@@ -425,8 +425,9 @@ static int __init via_pmu_start(void)
 			gpio_irq = irq_of_parse_and_map(gpio_node, 0);
 
 		if (gpio_irq != NO_IRQ) {
-			if (request_irq(gpio_irq, gpio1_interrupt, IRQF_TIMER,
-					"GPIO1 ADB", (void *)0))
+			if (request_irq(gpio_irq, gpio1_interrupt,
+					IRQF_NO_SUSPEND, "GPIO1 ADB",
+					(void *)0))
 				printk(KERN_ERR "pmu: can't get irq %d"
 				       " (GPIO1)\n", gpio_irq);
 			else
diff --git a/drivers/mailbox/Kconfig b/drivers/mailbox/Kconfig
index 546d05f4358a..b2bbe8659bed 100644
--- a/drivers/mailbox/Kconfig
+++ b/drivers/mailbox/Kconfig
@@ -81,6 +81,7 @@ config STI_MBOX
 config MAILBOX_TEST
 	tristate "Mailbox Test Client"
 	depends on OF
+	depends on HAS_IOMEM
 	help
 	  Test client to help with testing new Controller driver
 	  implementations.
diff --git a/drivers/mailbox/mailbox-sti.c b/drivers/mailbox/mailbox-sti.c
index 4835817c5365..2394cfe892b6 100644
--- a/drivers/mailbox/mailbox-sti.c
+++ b/drivers/mailbox/mailbox-sti.c
@@ -384,7 +384,7 @@ static struct mbox_chan *sti_mbox_xlate(struct mbox_controller *mbox,
 	return chan;
 }
 
-static struct mbox_chan_ops sti_mbox_ops = {
+static const struct mbox_chan_ops sti_mbox_ops = {
 	.startup	= sti_mbox_startup_chan,
 	.shutdown	= sti_mbox_shutdown_chan,
 	.send_data	= sti_mbox_send_data,
diff --git a/drivers/mailbox/pcc.c b/drivers/mailbox/pcc.c
index 45d85aea9955..8f779a1ec99c 100644
--- a/drivers/mailbox/pcc.c
+++ b/drivers/mailbox/pcc.c
@@ -81,16 +81,10 @@ static struct mbox_controller pcc_mbox_ctrl = {};
  */
 static struct mbox_chan *get_pcc_channel(int id)
 {
-	struct mbox_chan *pcc_chan;
-
 	if (id < 0 || id > pcc_mbox_ctrl.num_chans)
 		return ERR_PTR(-ENOENT);
 
-	pcc_chan = (struct mbox_chan *)
-		(unsigned long) pcc_mbox_channels +
-		(id * sizeof(*pcc_chan));
-
-	return pcc_chan;
+	return &pcc_mbox_channels[id];
 }
 
 /**
diff --git a/drivers/md/Kconfig b/drivers/md/Kconfig
index 7913fdcfc849..0a2e7273db9e 100644
--- a/drivers/md/Kconfig
+++ b/drivers/md/Kconfig
@@ -240,6 +240,15 @@ config DM_BUFIO
 	 as a cache, holding recently-read blocks in memory and performing
 	 delayed writes.
 
+config DM_DEBUG_BLOCK_STACK_TRACING
+       bool "Keep stack trace of persistent data block lock holders"
+       depends on STACKTRACE_SUPPORT && DM_BUFIO
+       select STACKTRACE
+       ---help---
+	 Enable this for messages that may help debug problems with the
+	 block manager locking used by thin provisioning and caching.
+
+	 If unsure, say N.
 config DM_BIO_PRISON
        tristate
        depends on BLK_DEV_DM
@@ -458,6 +467,18 @@ config DM_VERITY
 
 	  If unsure, say N.
 
+config DM_VERITY_FEC
+	bool "Verity forward error correction support"
+	depends on DM_VERITY
+	select REED_SOLOMON
+	select REED_SOLOMON_DEC8
+	---help---
+	  Add forward error correction support to dm-verity. This option
+	  makes it possible to use pre-generated error correction data to
+	  recover from corrupted blocks.
+
+	  If unsure, say N.
+
 config DM_SWITCH
 	tristate "Switch target support (EXPERIMENTAL)"
 	depends on BLK_DEV_DM
diff --git a/drivers/md/Makefile b/drivers/md/Makefile
index f34979cd141a..62a65764e8e0 100644
--- a/drivers/md/Makefile
+++ b/drivers/md/Makefile
@@ -16,6 +16,7 @@ dm-cache-mq-y   += dm-cache-policy-mq.o
 dm-cache-smq-y   += dm-cache-policy-smq.o
 dm-cache-cleaner-y += dm-cache-policy-cleaner.o
 dm-era-y	+= dm-era-target.o
+dm-verity-y	+= dm-verity-target.o
 md-mod-y	+= md.o bitmap.o
 raid456-y	+= raid5.o raid5-cache.o
 
@@ -63,3 +64,7 @@ obj-$(CONFIG_DM_LOG_WRITES)	+= dm-log-writes.o
 ifeq ($(CONFIG_DM_UEVENT),y)
 dm-mod-objs			+= dm-uevent.o
 endif
+
+ifeq ($(CONFIG_DM_VERITY_FEC),y)
+dm-verity-objs			+= dm-verity-fec.o
+endif
diff --git a/drivers/md/bcache/btree.c b/drivers/md/bcache/btree.c
index 83392f856dfd..22b9e34ceb75 100644
--- a/drivers/md/bcache/btree.c
+++ b/drivers/md/bcache/btree.c
@@ -1741,6 +1741,7 @@ static void bch_btree_gc(struct cache_set *c)
 	do {
 		ret = btree_root(gc_root, c, &op, &writes, &stats);
 		closure_sync(&writes);
+		cond_resched();
 
 		if (ret && ret != -EAGAIN)
 			pr_warn("gc failed!");
@@ -2162,8 +2163,10 @@ int bch_btree_insert_check_key(struct btree *b, struct btree_op *op,
 		rw_lock(true, b, b->level);
 
 		if (b->key.ptr[0] != btree_ptr ||
-		    b->seq != seq + 1)
+                   b->seq != seq + 1) {
+                       op->lock = b->level;
 			goto out;
+               }
 	}
 
 	SET_KEY_PTRS(check_key, 1);
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
index 679a093a3bf6..8d0ead98eb6e 100644
--- a/drivers/md/bcache/super.c
+++ b/drivers/md/bcache/super.c
@@ -685,6 +685,8 @@ static void bcache_device_link(struct bcache_device *d, struct cache_set *c,
 	WARN(sysfs_create_link(&d->kobj, &c->kobj, "cache") ||
 	     sysfs_create_link(&c->kobj, &d->kobj, d->name),
 	     "Couldn't create device <-> cache set symlinks");
+
+	clear_bit(BCACHE_DEV_UNLINK_DONE, &d->flags);
 }
 
 static void bcache_device_detach(struct bcache_device *d)
@@ -847,8 +849,11 @@ void bch_cached_dev_run(struct cached_dev *dc)
 	buf[SB_LABEL_SIZE] = '\0';
 	env[2] = kasprintf(GFP_KERNEL, "CACHED_LABEL=%s", buf);
 
-	if (atomic_xchg(&dc->running, 1))
+	if (atomic_xchg(&dc->running, 1)) {
+		kfree(env[1]);
+		kfree(env[2]);
 		return;
+	}
 
 	if (!d->c &&
 	    BDEV_STATE(&dc->sb) != BDEV_STATE_NONE) {
@@ -1933,6 +1938,8 @@ static ssize_t register_bcache(struct kobject *k, struct kobj_attribute *attr,
 			else
 				err = "device busy";
 			mutex_unlock(&bch_register_lock);
+			if (attr == &ksysfs_register_quiet)
+				goto out;
 		}
 		goto err;
 	}
@@ -1971,8 +1978,7 @@ out:
 err_close:
 	blkdev_put(bdev, FMODE_READ|FMODE_WRITE|FMODE_EXCL);
 err:
-	if (attr != &ksysfs_register_quiet)
-		pr_info("error opening %s: %s", path, err);
+	pr_info("error opening %s: %s", path, err);
 	ret = -EINVAL;
 	goto out;
 }
@@ -2066,8 +2072,10 @@ static int __init bcache_init(void)
 	closure_debug_init();
 
 	bcache_major = register_blkdev(0, "bcache");
-	if (bcache_major < 0)
+	if (bcache_major < 0) {
+		unregister_reboot_notifier(&reboot);
 		return bcache_major;
+	}
 
 	if (!(bcache_wq = create_workqueue("bcache")) ||
 	    !(bcache_kobj = kobject_create_and_add("bcache", fs_kobj)) ||
diff --git a/drivers/md/bcache/util.c b/drivers/md/bcache/util.c
index db3ae4c2b223..dde6172f3f10 100644
--- a/drivers/md/bcache/util.c
+++ b/drivers/md/bcache/util.c
@@ -230,7 +230,7 @@ void bch_bio_map(struct bio *bio, void *base)
 	BUG_ON(!bio->bi_iter.bi_size);
 	BUG_ON(bio->bi_vcnt);
 
-	bv->bv_offset = base ? ((unsigned long) base) % PAGE_SIZE : 0;
+	bv->bv_offset = base ? offset_in_page(base) : 0;
 	goto start;
 
 	for (; size; bio->bi_vcnt++, bv++) {
diff --git a/drivers/md/bcache/writeback.c b/drivers/md/bcache/writeback.c
index b23f88d9f18c..b9346cd9cda1 100644
--- a/drivers/md/bcache/writeback.c
+++ b/drivers/md/bcache/writeback.c
@@ -323,6 +323,10 @@ void bcache_dev_sectors_dirty_add(struct cache_set *c, unsigned inode,
 
 static bool dirty_pred(struct keybuf *buf, struct bkey *k)
 {
+	struct cached_dev *dc = container_of(buf, struct cached_dev, writeback_keys);
+
+	BUG_ON(KEY_INODE(k) != dc->disk.id);
+
 	return KEY_DIRTY(k);
 }
 
@@ -372,11 +376,24 @@ next:
 	}
 }
 
+/*
+ * Returns true if we scanned the entire disk
+ */
 static bool refill_dirty(struct cached_dev *dc)
 {
 	struct keybuf *buf = &dc->writeback_keys;
+	struct bkey start = KEY(dc->disk.id, 0, 0);
 	struct bkey end = KEY(dc->disk.id, MAX_KEY_OFFSET, 0);
-	bool searched_from_start = false;
+	struct bkey start_pos;
+
+	/*
+	 * make sure keybuf pos is inside the range for this disk - at bringup
+	 * we might not be attached yet so this disk's inode nr isn't
+	 * initialized then
+	 */
+	if (bkey_cmp(&buf->last_scanned, &start) < 0 ||
+	    bkey_cmp(&buf->last_scanned, &end) > 0)
+		buf->last_scanned = start;
 
 	if (dc->partial_stripes_expensive) {
 		refill_full_stripes(dc);
@@ -384,14 +401,20 @@ static bool refill_dirty(struct cached_dev *dc)
 			return false;
 	}
 
-	if (bkey_cmp(&buf->last_scanned, &end) >= 0) {
-		buf->last_scanned = KEY(dc->disk.id, 0, 0);
-		searched_from_start = true;
-	}
-
+	start_pos = buf->last_scanned;
 	bch_refill_keybuf(dc->disk.c, buf, &end, dirty_pred);
 
-	return bkey_cmp(&buf->last_scanned, &end) >= 0 && searched_from_start;
+	if (bkey_cmp(&buf->last_scanned, &end) < 0)
+		return false;
+
+	/*
+	 * If we get to the end start scanning again from the beginning, and
+	 * only scan up to where we initially started scanning from:
+	 */
+	buf->last_scanned = start;
+	bch_refill_keybuf(dc->disk.c, buf, &start_pos, dirty_pred);
+
+	return bkey_cmp(&buf->last_scanned, &start_pos) >= 0;
 }
 
 static int bch_writeback_thread(void *arg)
diff --git a/drivers/md/bcache/writeback.h b/drivers/md/bcache/writeback.h
index 0a9dab187b79..073a042aed24 100644
--- a/drivers/md/bcache/writeback.h
+++ b/drivers/md/bcache/writeback.h
@@ -63,7 +63,8 @@ static inline bool should_writeback(struct cached_dev *dc, struct bio *bio,
 
 static inline void bch_writeback_queue(struct cached_dev *dc)
 {
-	wake_up_process(dc->writeback_thread);
+	if (!IS_ERR_OR_NULL(dc->writeback_thread))
+		wake_up_process(dc->writeback_thread);
 }
 
 static inline void bch_writeback_add(struct cached_dev *dc)
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 4f22e919787a..d80cce499a56 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -210,10 +210,6 @@ static int write_sb_page(struct bitmap *bitmap, struct page *page, int wait)
 	struct block_device *bdev;
 	struct mddev *mddev = bitmap->mddev;
 	struct bitmap_storage *store = &bitmap->storage;
-	int node_offset = 0;
-
-	if (mddev_is_clustered(bitmap->mddev))
-		node_offset = bitmap->cluster_slot * store->file_pages;
 
 	while ((rdev = next_active_rdev(rdev, mddev)) != NULL) {
 		int size = PAGE_SIZE;
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index 2dd33085b331..cd77216beff1 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -16,6 +16,7 @@
 #include <linux/shrinker.h>
 #include <linux/module.h>
 #include <linux/rbtree.h>
+#include <linux/stacktrace.h>
 
 #define DM_MSG_PREFIX "bufio"
 
@@ -149,6 +150,11 @@ struct dm_buffer {
 	struct list_head write_list;
 	struct bio bio;
 	struct bio_vec bio_vec[DM_BUFIO_INLINE_VECS];
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+#define MAX_STACK 10
+	struct stack_trace stack_trace;
+	unsigned long stack_entries[MAX_STACK];
+#endif
 };
 
 /*----------------------------------------------------------------*/
@@ -253,6 +259,17 @@ static LIST_HEAD(dm_bufio_all_clients);
  */
 static DEFINE_MUTEX(dm_bufio_clients_lock);
 
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+static void buffer_record_stack(struct dm_buffer *b)
+{
+	b->stack_trace.nr_entries = 0;
+	b->stack_trace.max_entries = MAX_STACK;
+	b->stack_trace.entries = b->stack_entries;
+	b->stack_trace.skip = 2;
+	save_stack_trace(&b->stack_trace);
+}
+#endif
+
 /*----------------------------------------------------------------
  * A red/black tree acts as an index for all the buffers.
  *--------------------------------------------------------------*/
@@ -454,6 +471,9 @@ static struct dm_buffer *alloc_buffer(struct dm_bufio_client *c, gfp_t gfp_mask)
 
 	adjust_total_allocated(b->data_mode, (long)c->block_size);
 
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+	memset(&b->stack_trace, 0, sizeof(b->stack_trace));
+#endif
 	return b;
 }
 
@@ -630,7 +650,7 @@ static void use_inline_bio(struct dm_buffer *b, int rw, sector_t block,
 	do {
 		if (!bio_add_page(&b->bio, virt_to_page(ptr),
 				  len < PAGE_SIZE ? len : PAGE_SIZE,
-				  virt_to_phys(ptr) & (PAGE_SIZE - 1))) {
+				  offset_in_page(ptr))) {
 			BUG_ON(b->c->block_size <= PAGE_SIZE);
 			use_dmio(b, rw, block, end_io);
 			return;
@@ -1063,12 +1083,16 @@ static void *new_read(struct dm_bufio_client *c, sector_t block,
 
 	dm_bufio_lock(c);
 	b = __bufio_new(c, block, nf, &need_submit, &write_list);
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+	if (b && b->hold_count == 1)
+		buffer_record_stack(b);
+#endif
 	dm_bufio_unlock(c);
 
 	__flush_write_list(&write_list);
 
 	if (!b)
-		return b;
+		return NULL;
 
 	if (need_submit)
 		submit_io(b, READ, b->block, read_endio);
@@ -1462,6 +1486,7 @@ static void drop_buffers(struct dm_bufio_client *c)
 {
 	struct dm_buffer *b;
 	int i;
+	bool warned = false;
 
 	BUG_ON(dm_bufio_in_request());
 
@@ -1476,9 +1501,21 @@ static void drop_buffers(struct dm_bufio_client *c)
 		__free_buffer_wake(b);
 
 	for (i = 0; i < LIST_SIZE; i++)
-		list_for_each_entry(b, &c->lru[i], lru_list)
+		list_for_each_entry(b, &c->lru[i], lru_list) {
+			WARN_ON(!warned);
+			warned = true;
 			DMERR("leaked buffer %llx, hold count %u, list %d",
 			      (unsigned long long)b->block, b->hold_count, i);
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+			print_stack_trace(&b->stack_trace, 1);
+			b->hold_count = 0; /* mark unclaimed to avoid BUG_ON below */
+#endif
+		}
+
+#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
+	while ((b = __get_unclaimed_buffer(c)))
+		__free_buffer_wake(b);
+#endif
 
 	for (i = 0; i < LIST_SIZE; i++)
 		BUG_ON(!list_empty(&c->lru[i]));
@@ -1891,8 +1928,7 @@ static void __exit dm_bufio_exit(void)
 		bug = 1;
 	}
 
-	if (bug)
-		BUG();
+	BUG_ON(bug);
 }
 
 module_init(dm_bufio_init)
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 2fd4c8296144..5780accffa30 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -118,14 +118,12 @@ static void iot_io_end(struct io_tracker *iot, sector_t len)
  */
 struct dm_hook_info {
 	bio_end_io_t *bi_end_io;
-	void *bi_private;
 };
 
 static void dm_hook_bio(struct dm_hook_info *h, struct bio *bio,
 			bio_end_io_t *bi_end_io, void *bi_private)
 {
 	h->bi_end_io = bio->bi_end_io;
-	h->bi_private = bio->bi_private;
 
 	bio->bi_end_io = bi_end_io;
 	bio->bi_private = bi_private;
@@ -134,7 +132,6 @@ static void dm_hook_bio(struct dm_hook_info *h, struct bio *bio,
 static void dm_unhook_bio(struct dm_hook_info *h, struct bio *bio)
 {
 	bio->bi_end_io = h->bi_end_io;
-	bio->bi_private = h->bi_private;
 }
 
 /*----------------------------------------------------------------*/
diff --git a/drivers/md/dm-exception-store.h b/drivers/md/dm-exception-store.h
index fae34e7a0b1e..12b5216c2cfe 100644
--- a/drivers/md/dm-exception-store.h
+++ b/drivers/md/dm-exception-store.h
@@ -69,7 +69,7 @@ struct dm_exception_store_type {
 	 * Update the metadata with this exception.
 	 */
 	void (*commit_exception) (struct dm_exception_store *store,
-				  struct dm_exception *e,
+				  struct dm_exception *e, int valid,
 				  void (*callback) (void *, int success),
 				  void *callback_context);
 
diff --git a/drivers/md/dm-io.c b/drivers/md/dm-io.c
index 81c5e1a1f363..06d426eb5a30 100644
--- a/drivers/md/dm-io.c
+++ b/drivers/md/dm-io.c
@@ -246,7 +246,7 @@ static void vm_dp_init(struct dpages *dp, void *data)
 {
 	dp->get_page = vm_get_page;
 	dp->next_page = vm_next_page;
-	dp->context_u = ((unsigned long) data) & (PAGE_SIZE - 1);
+	dp->context_u = offset_in_page(data);
 	dp->context_ptr = data;
 }
 
@@ -271,7 +271,7 @@ static void km_dp_init(struct dpages *dp, void *data)
 {
 	dp->get_page = km_get_page;
 	dp->next_page = km_next_page;
-	dp->context_u = ((unsigned long) data) & (PAGE_SIZE - 1);
+	dp->context_u = offset_in_page(data);
 	dp->context_ptr = data;
 }
 
diff --git a/drivers/md/dm-snap-persistent.c b/drivers/md/dm-snap-persistent.c
index 3164b8bce294..4d3909393f2c 100644
--- a/drivers/md/dm-snap-persistent.c
+++ b/drivers/md/dm-snap-persistent.c
@@ -695,7 +695,7 @@ static int persistent_prepare_exception(struct dm_exception_store *store,
 }
 
 static void persistent_commit_exception(struct dm_exception_store *store,
-					struct dm_exception *e,
+					struct dm_exception *e, int valid,
 					void (*callback) (void *, int success),
 					void *callback_context)
 {
@@ -704,6 +704,9 @@ static void persistent_commit_exception(struct dm_exception_store *store,
 	struct core_exception ce;
 	struct commit_callback *cb;
 
+	if (!valid)
+		ps->valid = 0;
+
 	ce.old_chunk = e->old_chunk;
 	ce.new_chunk = e->new_chunk;
 	write_exception(ps, ps->current_committed++, &ce);
diff --git a/drivers/md/dm-snap-transient.c b/drivers/md/dm-snap-transient.c
index 9b7c8c8049d6..4d50a12cf00c 100644
--- a/drivers/md/dm-snap-transient.c
+++ b/drivers/md/dm-snap-transient.c
@@ -52,12 +52,12 @@ static int transient_prepare_exception(struct dm_exception_store *store,
 }
 
 static void transient_commit_exception(struct dm_exception_store *store,
-				       struct dm_exception *e,
+				       struct dm_exception *e, int valid,
 				       void (*callback) (void *, int success),
 				       void *callback_context)
 {
 	/* Just succeed */
-	callback(callback_context, 1);
+	callback(callback_context, valid);
 }
 
 static void transient_usage(struct dm_exception_store *store,
diff --git a/drivers/md/dm-snap.c b/drivers/md/dm-snap.c
index c06b74e91cd6..3766386080a4 100644
--- a/drivers/md/dm-snap.c
+++ b/drivers/md/dm-snap.c
@@ -207,7 +207,6 @@ struct dm_snap_pending_exception {
 	 */
 	struct bio *full_bio;
 	bio_end_io_t *full_bio_end_io;
-	void *full_bio_private;
 };
 
 /*
@@ -1438,8 +1437,9 @@ static void __invalidate_snapshot(struct dm_snapshot *s, int err)
 	dm_table_event(s->ti->table);
 }
 
-static void pending_complete(struct dm_snap_pending_exception *pe, int success)
+static void pending_complete(void *context, int success)
 {
+	struct dm_snap_pending_exception *pe = context;
 	struct dm_exception *e;
 	struct dm_snapshot *s = pe->snap;
 	struct bio *origin_bios = NULL;
@@ -1485,10 +1485,8 @@ out:
 	snapshot_bios = bio_list_get(&pe->snapshot_bios);
 	origin_bios = bio_list_get(&pe->origin_bios);
 	full_bio = pe->full_bio;
-	if (full_bio) {
+	if (full_bio)
 		full_bio->bi_end_io = pe->full_bio_end_io;
-		full_bio->bi_private = pe->full_bio_private;
-	}
 	increment_pending_exceptions_done_count();
 
 	up_write(&s->lock);
@@ -1509,24 +1507,13 @@ out:
 	free_pending_exception(pe);
 }
 
-static void commit_callback(void *context, int success)
-{
-	struct dm_snap_pending_exception *pe = context;
-
-	pending_complete(pe, success);
-}
-
 static void complete_exception(struct dm_snap_pending_exception *pe)
 {
 	struct dm_snapshot *s = pe->snap;
 
-	if (unlikely(pe->copy_error))
-		pending_complete(pe, 0);
-
-	else
-		/* Update the metadata if we are persistent */
-		s->store->type->commit_exception(s->store, &pe->e,
-						 commit_callback, pe);
+	/* Update the metadata if we are persistent */
+	s->store->type->commit_exception(s->store, &pe->e, !pe->copy_error,
+					 pending_complete, pe);
 }
 
 /*
@@ -1605,7 +1592,6 @@ static void start_full_bio(struct dm_snap_pending_exception *pe,
 
 	pe->full_bio = bio;
 	pe->full_bio_end_io = bio->bi_end_io;
-	pe->full_bio_private = bio->bi_private;
 
 	callback_data = dm_kcopyd_prepare_callback(s->kcopyd_client,
 						   copy_callback, pe);
diff --git a/drivers/md/dm-thin-metadata.c b/drivers/md/dm-thin-metadata.c
index c219a053c7f6..f962d6453afd 100644
--- a/drivers/md/dm-thin-metadata.c
+++ b/drivers/md/dm-thin-metadata.c
@@ -1395,8 +1395,21 @@ static bool __snapshotted_since(struct dm_thin_device *td, uint32_t time)
 	return td->snapshotted_time > time;
 }
 
-int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
-		       int can_issue_io, struct dm_thin_lookup_result *result)
+static void unpack_lookup_result(struct dm_thin_device *td, __le64 value,
+				 struct dm_thin_lookup_result *result)
+{
+	uint64_t block_time = 0;
+	dm_block_t exception_block;
+	uint32_t exception_time;
+
+	block_time = le64_to_cpu(value);
+	unpack_block_time(block_time, &exception_block, &exception_time);
+	result->block = exception_block;
+	result->shared = __snapshotted_since(td, exception_time);
+}
+
+static int __find_block(struct dm_thin_device *td, dm_block_t block,
+			int can_issue_io, struct dm_thin_lookup_result *result)
 {
 	int r;
 	__le64 value;
@@ -1404,39 +1417,56 @@ int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
 	dm_block_t keys[2] = { td->id, block };
 	struct dm_btree_info *info;
 
-	down_read(&pmd->root_lock);
-	if (pmd->fail_io) {
-		up_read(&pmd->root_lock);
-		return -EINVAL;
-	}
-
 	if (can_issue_io) {
 		info = &pmd->info;
 	} else
 		info = &pmd->nb_info;
 
 	r = dm_btree_lookup(info, pmd->root, keys, &value);
-	if (!r) {
-		uint64_t block_time = 0;
-		dm_block_t exception_block;
-		uint32_t exception_time;
-
-		block_time = le64_to_cpu(value);
-		unpack_block_time(block_time, &exception_block,
-				  &exception_time);
-		result->block = exception_block;
-		result->shared = __snapshotted_since(td, exception_time);
+	if (!r)
+		unpack_lookup_result(td, value, result);
+
+	return r;
+}
+
+int dm_thin_find_block(struct dm_thin_device *td, dm_block_t block,
+		       int can_issue_io, struct dm_thin_lookup_result *result)
+{
+	int r;
+	struct dm_pool_metadata *pmd = td->pmd;
+
+	down_read(&pmd->root_lock);
+	if (pmd->fail_io) {
+		up_read(&pmd->root_lock);
+		return -EINVAL;
 	}
 
+	r = __find_block(td, block, can_issue_io, result);
+
 	up_read(&pmd->root_lock);
 	return r;
 }
 
-/* FIXME: write a more efficient one in btree */
-int dm_thin_find_mapped_range(struct dm_thin_device *td,
-			      dm_block_t begin, dm_block_t end,
-			      dm_block_t *thin_begin, dm_block_t *thin_end,
-			      dm_block_t *pool_begin, bool *maybe_shared)
+static int __find_next_mapped_block(struct dm_thin_device *td, dm_block_t block,
+					  dm_block_t *vblock,
+					  struct dm_thin_lookup_result *result)
+{
+	int r;
+	__le64 value;
+	struct dm_pool_metadata *pmd = td->pmd;
+	dm_block_t keys[2] = { td->id, block };
+
+	r = dm_btree_lookup_next(&pmd->info, pmd->root, keys, vblock, &value);
+	if (!r)
+		unpack_lookup_result(td, value, result);
+
+	return r;
+}
+
+static int __find_mapped_range(struct dm_thin_device *td,
+			       dm_block_t begin, dm_block_t end,
+			       dm_block_t *thin_begin, dm_block_t *thin_end,
+			       dm_block_t *pool_begin, bool *maybe_shared)
 {
 	int r;
 	dm_block_t pool_end;
@@ -1445,21 +1475,11 @@ int dm_thin_find_mapped_range(struct dm_thin_device *td,
 	if (end < begin)
 		return -ENODATA;
 
-	/*
-	 * Find first mapped block.
-	 */
-	while (begin < end) {
-		r = dm_thin_find_block(td, begin, true, &lookup);
-		if (r) {
-			if (r != -ENODATA)
-				return r;
-		} else
-			break;
-
-		begin++;
-	}
+	r = __find_next_mapped_block(td, begin, &begin, &lookup);
+	if (r)
+		return r;
 
-	if (begin == end)
+	if (begin >= end)
 		return -ENODATA;
 
 	*thin_begin = begin;
@@ -1469,7 +1489,7 @@ int dm_thin_find_mapped_range(struct dm_thin_device *td,
 	begin++;
 	pool_end = *pool_begin + 1;
 	while (begin != end) {
-		r = dm_thin_find_block(td, begin, true, &lookup);
+		r = __find_block(td, begin, true, &lookup);
 		if (r) {
 			if (r == -ENODATA)
 				break;
@@ -1489,6 +1509,24 @@ int dm_thin_find_mapped_range(struct dm_thin_device *td,
 	return 0;
 }
 
+int dm_thin_find_mapped_range(struct dm_thin_device *td,
+			      dm_block_t begin, dm_block_t end,
+			      dm_block_t *thin_begin, dm_block_t *thin_end,
+			      dm_block_t *pool_begin, bool *maybe_shared)
+{
+	int r = -EINVAL;
+	struct dm_pool_metadata *pmd = td->pmd;
+
+	down_read(&pmd->root_lock);
+	if (!pmd->fail_io) {
+		r = __find_mapped_range(td, begin, end, thin_begin, thin_end,
+					pool_begin, maybe_shared);
+	}
+	up_read(&pmd->root_lock);
+
+	return r;
+}
+
 static int __insert(struct dm_thin_device *td, dm_block_t block,
 		    dm_block_t data_block)
 {
diff --git a/drivers/md/dm-thin.c b/drivers/md/dm-thin.c
index 63903a5a5d9e..72d91f477683 100644
--- a/drivers/md/dm-thin.c
+++ b/drivers/md/dm-thin.c
@@ -3453,8 +3453,8 @@ static void pool_postsuspend(struct dm_target *ti)
 	struct pool_c *pt = ti->private;
 	struct pool *pool = pt->pool;
 
-	cancel_delayed_work(&pool->waker);
-	cancel_delayed_work(&pool->no_space_timeout);
+	cancel_delayed_work_sync(&pool->waker);
+	cancel_delayed_work_sync(&pool->no_space_timeout);
 	flush_workqueue(pool->wq);
 	(void) commit(pool);
 }
@@ -3886,7 +3886,7 @@ static struct target_type pool_target = {
 	.name = "thin-pool",
 	.features = DM_TARGET_SINGLETON | DM_TARGET_ALWAYS_WRITEABLE |
 		    DM_TARGET_IMMUTABLE,
-	.version = {1, 16, 0},
+	.version = {1, 17, 0},
 	.module = THIS_MODULE,
 	.ctr = pool_ctr,
 	.dtr = pool_dtr,
@@ -4260,7 +4260,7 @@ static void thin_io_hints(struct dm_target *ti, struct queue_limits *limits)
 
 static struct target_type thin_target = {
 	.name = "thin",
-	.version = {1, 16, 0},
+	.version = {1, 17, 0},
 	.module	= THIS_MODULE,
 	.ctr = thin_ctr,
 	.dtr = thin_dtr,
diff --git a/drivers/md/dm-verity-fec.c b/drivers/md/dm-verity-fec.c
new file mode 100644
index 000000000000..1cc10c4de701
--- /dev/null
+++ b/drivers/md/dm-verity-fec.c
@@ -0,0 +1,818 @@
+/*
+ * Copyright (C) 2015 Google, Inc.
+ *
+ * Author: Sami Tolvanen <samitolvanen@google.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 "dm-verity-fec.h"
+#include <linux/math64.h>
+
+#define DM_MSG_PREFIX	"verity-fec"
+
+/*
+ * If error correction has been configured, returns true.
+ */
+bool verity_fec_is_enabled(struct dm_verity *v)
+{
+	return v->fec && v->fec->dev;
+}
+
+/*
+ * Return a pointer to dm_verity_fec_io after dm_verity_io and its variable
+ * length fields.
+ */
+static inline struct dm_verity_fec_io *fec_io(struct dm_verity_io *io)
+{
+	return (struct dm_verity_fec_io *) verity_io_digest_end(io->v, io);
+}
+
+/*
+ * Return an interleaved offset for a byte in RS block.
+ */
+static inline u64 fec_interleave(struct dm_verity *v, u64 offset)
+{
+	u32 mod;
+
+	mod = do_div(offset, v->fec->rsn);
+	return offset + mod * (v->fec->rounds << v->data_dev_block_bits);
+}
+
+/*
+ * Decode an RS block using Reed-Solomon.
+ */
+static int fec_decode_rs8(struct dm_verity *v, struct dm_verity_fec_io *fio,
+			  u8 *data, u8 *fec, int neras)
+{
+	int i;
+	uint16_t par[DM_VERITY_FEC_RSM - DM_VERITY_FEC_MIN_RSN];
+
+	for (i = 0; i < v->fec->roots; i++)
+		par[i] = fec[i];
+
+	return decode_rs8(fio->rs, data, par, v->fec->rsn, NULL, neras,
+			  fio->erasures, 0, NULL);
+}
+
+/*
+ * Read error-correcting codes for the requested RS block. Returns a pointer
+ * to the data block. Caller is responsible for releasing buf.
+ */
+static u8 *fec_read_parity(struct dm_verity *v, u64 rsb, int index,
+			   unsigned *offset, struct dm_buffer **buf)
+{
+	u64 position, block;
+	u8 *res;
+
+	position = (index + rsb) * v->fec->roots;
+	block = position >> v->data_dev_block_bits;
+	*offset = (unsigned)(position - (block << v->data_dev_block_bits));
+
+	res = dm_bufio_read(v->fec->bufio, v->fec->start + block, buf);
+	if (unlikely(IS_ERR(res))) {
+		DMERR("%s: FEC %llu: parity read failed (block %llu): %ld",
+		      v->data_dev->name, (unsigned long long)rsb,
+		      (unsigned long long)(v->fec->start + block),
+		      PTR_ERR(res));
+		*buf = NULL;
+	}
+
+	return res;
+}
+
+/* Loop over each preallocated buffer slot. */
+#define fec_for_each_prealloc_buffer(__i) \
+	for (__i = 0; __i < DM_VERITY_FEC_BUF_PREALLOC; __i++)
+
+/* Loop over each extra buffer slot. */
+#define fec_for_each_extra_buffer(io, __i) \
+	for (__i = DM_VERITY_FEC_BUF_PREALLOC; __i < DM_VERITY_FEC_BUF_MAX; __i++)
+
+/* Loop over each allocated buffer. */
+#define fec_for_each_buffer(io, __i) \
+	for (__i = 0; __i < (io)->nbufs; __i++)
+
+/* Loop over each RS block in each allocated buffer. */
+#define fec_for_each_buffer_rs_block(io, __i, __j) \
+	fec_for_each_buffer(io, __i) \
+		for (__j = 0; __j < 1 << DM_VERITY_FEC_BUF_RS_BITS; __j++)
+
+/*
+ * Return a pointer to the current RS block when called inside
+ * fec_for_each_buffer_rs_block.
+ */
+static inline u8 *fec_buffer_rs_block(struct dm_verity *v,
+				      struct dm_verity_fec_io *fio,
+				      unsigned i, unsigned j)
+{
+	return &fio->bufs[i][j * v->fec->rsn];
+}
+
+/*
+ * Return an index to the current RS block when called inside
+ * fec_for_each_buffer_rs_block.
+ */
+static inline unsigned fec_buffer_rs_index(unsigned i, unsigned j)
+{
+	return (i << DM_VERITY_FEC_BUF_RS_BITS) + j;
+}
+
+/*
+ * Decode all RS blocks from buffers and copy corrected bytes into fio->output
+ * starting from block_offset.
+ */
+static int fec_decode_bufs(struct dm_verity *v, struct dm_verity_fec_io *fio,
+			   u64 rsb, int byte_index, unsigned block_offset,
+			   int neras)
+{
+	int r, corrected = 0, res;
+	struct dm_buffer *buf;
+	unsigned n, i, offset;
+	u8 *par, *block;
+
+	par = fec_read_parity(v, rsb, block_offset, &offset, &buf);
+	if (IS_ERR(par))
+		return PTR_ERR(par);
+
+	/*
+	 * Decode the RS blocks we have in bufs. Each RS block results in
+	 * one corrected target byte and consumes fec->roots parity bytes.
+	 */
+	fec_for_each_buffer_rs_block(fio, n, i) {
+		block = fec_buffer_rs_block(v, fio, n, i);
+		res = fec_decode_rs8(v, fio, block, &par[offset], neras);
+		if (res < 0) {
+			dm_bufio_release(buf);
+
+			r = res;
+			goto error;
+		}
+
+		corrected += res;
+		fio->output[block_offset] = block[byte_index];
+
+		block_offset++;
+		if (block_offset >= 1 << v->data_dev_block_bits)
+			goto done;
+
+		/* read the next block when we run out of parity bytes */
+		offset += v->fec->roots;
+		if (offset >= 1 << v->data_dev_block_bits) {
+			dm_bufio_release(buf);
+
+			par = fec_read_parity(v, rsb, block_offset, &offset, &buf);
+			if (unlikely(IS_ERR(par)))
+				return PTR_ERR(par);
+		}
+	}
+done:
+	r = corrected;
+error:
+	if (r < 0 && neras)
+		DMERR_LIMIT("%s: FEC %llu: failed to correct: %d",
+			    v->data_dev->name, (unsigned long long)rsb, r);
+	else if (r > 0)
+		DMWARN_LIMIT("%s: FEC %llu: corrected %d errors",
+			     v->data_dev->name, (unsigned long long)rsb, r);
+
+	return r;
+}
+
+/*
+ * Locate data block erasures using verity hashes.
+ */
+static int fec_is_erasure(struct dm_verity *v, struct dm_verity_io *io,
+			  u8 *want_digest, u8 *data)
+{
+	if (unlikely(verity_hash(v, verity_io_hash_desc(v, io),
+				 data, 1 << v->data_dev_block_bits,
+				 verity_io_real_digest(v, io))))
+		return 0;
+
+	return memcmp(verity_io_real_digest(v, io), want_digest,
+		      v->digest_size) != 0;
+}
+
+/*
+ * Read data blocks that are part of the RS block and deinterleave as much as
+ * fits into buffers. Check for erasure locations if @neras is non-NULL.
+ */
+static int fec_read_bufs(struct dm_verity *v, struct dm_verity_io *io,
+			 u64 rsb, u64 target, unsigned block_offset,
+			 int *neras)
+{
+	bool is_zero;
+	int i, j, target_index = -1;
+	struct dm_buffer *buf;
+	struct dm_bufio_client *bufio;
+	struct dm_verity_fec_io *fio = fec_io(io);
+	u64 block, ileaved;
+	u8 *bbuf, *rs_block;
+	u8 want_digest[v->digest_size];
+	unsigned n, k;
+
+	if (neras)
+		*neras = 0;
+
+	/*
+	 * read each of the rsn data blocks that are part of the RS block, and
+	 * interleave contents to available bufs
+	 */
+	for (i = 0; i < v->fec->rsn; i++) {
+		ileaved = fec_interleave(v, rsb * v->fec->rsn + i);
+
+		/*
+		 * target is the data block we want to correct, target_index is
+		 * the index of this block within the rsn RS blocks
+		 */
+		if (ileaved == target)
+			target_index = i;
+
+		block = ileaved >> v->data_dev_block_bits;
+		bufio = v->fec->data_bufio;
+
+		if (block >= v->data_blocks) {
+			block -= v->data_blocks;
+
+			/*
+			 * blocks outside the area were assumed to contain
+			 * zeros when encoding data was generated
+			 */
+			if (unlikely(block >= v->fec->hash_blocks))
+				continue;
+
+			block += v->hash_start;
+			bufio = v->bufio;
+		}
+
+		bbuf = dm_bufio_read(bufio, block, &buf);
+		if (unlikely(IS_ERR(bbuf))) {
+			DMWARN_LIMIT("%s: FEC %llu: read failed (%llu): %ld",
+				     v->data_dev->name,
+				     (unsigned long long)rsb,
+				     (unsigned long long)block, PTR_ERR(bbuf));
+
+			/* assume the block is corrupted */
+			if (neras && *neras <= v->fec->roots)
+				fio->erasures[(*neras)++] = i;
+
+			continue;
+		}
+
+		/* locate erasures if the block is on the data device */
+		if (bufio == v->fec->data_bufio &&
+		    verity_hash_for_block(v, io, block, want_digest,
+					  &is_zero) == 0) {
+			/* skip known zero blocks entirely */
+			if (is_zero)
+				continue;
+
+			/*
+			 * skip if we have already found the theoretical
+			 * maximum number (i.e. fec->roots) of erasures
+			 */
+			if (neras && *neras <= v->fec->roots &&
+			    fec_is_erasure(v, io, want_digest, bbuf))
+				fio->erasures[(*neras)++] = i;
+		}
+
+		/*
+		 * deinterleave and copy the bytes that fit into bufs,
+		 * starting from block_offset
+		 */
+		fec_for_each_buffer_rs_block(fio, n, j) {
+			k = fec_buffer_rs_index(n, j) + block_offset;
+
+			if (k >= 1 << v->data_dev_block_bits)
+				goto done;
+
+			rs_block = fec_buffer_rs_block(v, fio, n, j);
+			rs_block[i] = bbuf[k];
+		}
+done:
+		dm_bufio_release(buf);
+	}
+
+	return target_index;
+}
+
+/*
+ * Allocate RS control structure and FEC buffers from preallocated mempools,
+ * and attempt to allocate as many extra buffers as available.
+ */
+static int fec_alloc_bufs(struct dm_verity *v, struct dm_verity_fec_io *fio)
+{
+	unsigned n;
+
+	if (!fio->rs) {
+		fio->rs = mempool_alloc(v->fec->rs_pool, 0);
+		if (unlikely(!fio->rs)) {
+			DMERR("failed to allocate RS");
+			return -ENOMEM;
+		}
+	}
+
+	fec_for_each_prealloc_buffer(n) {
+		if (fio->bufs[n])
+			continue;
+
+		fio->bufs[n] = mempool_alloc(v->fec->prealloc_pool, GFP_NOIO);
+		if (unlikely(!fio->bufs[n])) {
+			DMERR("failed to allocate FEC buffer");
+			return -ENOMEM;
+		}
+	}
+
+	/* try to allocate the maximum number of buffers */
+	fec_for_each_extra_buffer(fio, n) {
+		if (fio->bufs[n])
+			continue;
+
+		fio->bufs[n] = mempool_alloc(v->fec->extra_pool, GFP_NOIO);
+		/* we can manage with even one buffer if necessary */
+		if (unlikely(!fio->bufs[n]))
+			break;
+	}
+	fio->nbufs = n;
+
+	if (!fio->output) {
+		fio->output = mempool_alloc(v->fec->output_pool, GFP_NOIO);
+
+		if (!fio->output) {
+			DMERR("failed to allocate FEC page");
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * Initialize buffers and clear erasures. fec_read_bufs() assumes buffers are
+ * zeroed before deinterleaving.
+ */
+static void fec_init_bufs(struct dm_verity *v, struct dm_verity_fec_io *fio)
+{
+	unsigned n;
+
+	fec_for_each_buffer(fio, n)
+		memset(fio->bufs[n], 0, v->fec->rsn << DM_VERITY_FEC_BUF_RS_BITS);
+
+	memset(fio->erasures, 0, sizeof(fio->erasures));
+}
+
+/*
+ * Decode all RS blocks in a single data block and return the target block
+ * (indicated by @offset) in fio->output. If @use_erasures is non-zero, uses
+ * hashes to locate erasures.
+ */
+static int fec_decode_rsb(struct dm_verity *v, struct dm_verity_io *io,
+			  struct dm_verity_fec_io *fio, u64 rsb, u64 offset,
+			  bool use_erasures)
+{
+	int r, neras = 0;
+	unsigned pos;
+
+	r = fec_alloc_bufs(v, fio);
+	if (unlikely(r < 0))
+		return r;
+
+	for (pos = 0; pos < 1 << v->data_dev_block_bits; ) {
+		fec_init_bufs(v, fio);
+
+		r = fec_read_bufs(v, io, rsb, offset, pos,
+				  use_erasures ? &neras : NULL);
+		if (unlikely(r < 0))
+			return r;
+
+		r = fec_decode_bufs(v, fio, rsb, r, pos, neras);
+		if (r < 0)
+			return r;
+
+		pos += fio->nbufs << DM_VERITY_FEC_BUF_RS_BITS;
+	}
+
+	/* Always re-validate the corrected block against the expected hash */
+	r = verity_hash(v, verity_io_hash_desc(v, io), fio->output,
+			1 << v->data_dev_block_bits,
+			verity_io_real_digest(v, io));
+	if (unlikely(r < 0))
+		return r;
+
+	if (memcmp(verity_io_real_digest(v, io), verity_io_want_digest(v, io),
+		   v->digest_size)) {
+		DMERR_LIMIT("%s: FEC %llu: failed to correct (%d erasures)",
+			    v->data_dev->name, (unsigned long long)rsb, neras);
+		return -EILSEQ;
+	}
+
+	return 0;
+}
+
+static int fec_bv_copy(struct dm_verity *v, struct dm_verity_io *io, u8 *data,
+		       size_t len)
+{
+	struct dm_verity_fec_io *fio = fec_io(io);
+
+	memcpy(data, &fio->output[fio->output_pos], len);
+	fio->output_pos += len;
+
+	return 0;
+}
+
+/*
+ * Correct errors in a block. Copies corrected block to dest if non-NULL,
+ * otherwise to a bio_vec starting from iter.
+ */
+int verity_fec_decode(struct dm_verity *v, struct dm_verity_io *io,
+		      enum verity_block_type type, sector_t block, u8 *dest,
+		      struct bvec_iter *iter)
+{
+	int r;
+	struct dm_verity_fec_io *fio = fec_io(io);
+	u64 offset, res, rsb;
+
+	if (!verity_fec_is_enabled(v))
+		return -EOPNOTSUPP;
+
+	if (type == DM_VERITY_BLOCK_TYPE_METADATA)
+		block += v->data_blocks;
+
+	/*
+	 * For RS(M, N), the continuous FEC data is divided into blocks of N
+	 * bytes. Since block size may not be divisible by N, the last block
+	 * is zero padded when decoding.
+	 *
+	 * Each byte of the block is covered by a different RS(M, N) code,
+	 * and each code is interleaved over N blocks to make it less likely
+	 * that bursty corruption will leave us in unrecoverable state.
+	 */
+
+	offset = block << v->data_dev_block_bits;
+
+	res = offset;
+	div64_u64(res, v->fec->rounds << v->data_dev_block_bits);
+
+	/*
+	 * The base RS block we can feed to the interleaver to find out all
+	 * blocks required for decoding.
+	 */
+	rsb = offset - res * (v->fec->rounds << v->data_dev_block_bits);
+
+	/*
+	 * Locating erasures is slow, so attempt to recover the block without
+	 * them first. Do a second attempt with erasures if the corruption is
+	 * bad enough.
+	 */
+	r = fec_decode_rsb(v, io, fio, rsb, offset, false);
+	if (r < 0) {
+		r = fec_decode_rsb(v, io, fio, rsb, offset, true);
+		if (r < 0)
+			return r;
+	}
+
+	if (dest)
+		memcpy(dest, fio->output, 1 << v->data_dev_block_bits);
+	else if (iter) {
+		fio->output_pos = 0;
+		r = verity_for_bv_block(v, io, iter, fec_bv_copy);
+	}
+
+	return r;
+}
+
+/*
+ * Clean up per-bio data.
+ */
+void verity_fec_finish_io(struct dm_verity_io *io)
+{
+	unsigned n;
+	struct dm_verity_fec *f = io->v->fec;
+	struct dm_verity_fec_io *fio = fec_io(io);
+
+	if (!verity_fec_is_enabled(io->v))
+		return;
+
+	mempool_free(fio->rs, f->rs_pool);
+
+	fec_for_each_prealloc_buffer(n)
+		mempool_free(fio->bufs[n], f->prealloc_pool);
+
+	fec_for_each_extra_buffer(fio, n)
+		mempool_free(fio->bufs[n], f->extra_pool);
+
+	mempool_free(fio->output, f->output_pool);
+}
+
+/*
+ * Initialize per-bio data.
+ */
+void verity_fec_init_io(struct dm_verity_io *io)
+{
+	struct dm_verity_fec_io *fio = fec_io(io);
+
+	if (!verity_fec_is_enabled(io->v))
+		return;
+
+	fio->rs = NULL;
+	memset(fio->bufs, 0, sizeof(fio->bufs));
+	fio->nbufs = 0;
+	fio->output = NULL;
+}
+
+/*
+ * Append feature arguments and values to the status table.
+ */
+unsigned verity_fec_status_table(struct dm_verity *v, unsigned sz,
+				 char *result, unsigned maxlen)
+{
+	if (!verity_fec_is_enabled(v))
+		return sz;
+
+	DMEMIT(" " DM_VERITY_OPT_FEC_DEV " %s "
+	       DM_VERITY_OPT_FEC_BLOCKS " %llu "
+	       DM_VERITY_OPT_FEC_START " %llu "
+	       DM_VERITY_OPT_FEC_ROOTS " %d",
+	       v->fec->dev->name,
+	       (unsigned long long)v->fec->blocks,
+	       (unsigned long long)v->fec->start,
+	       v->fec->roots);
+
+	return sz;
+}
+
+void verity_fec_dtr(struct dm_verity *v)
+{
+	struct dm_verity_fec *f = v->fec;
+
+	if (!verity_fec_is_enabled(v))
+		goto out;
+
+	mempool_destroy(f->rs_pool);
+	mempool_destroy(f->prealloc_pool);
+	mempool_destroy(f->extra_pool);
+	kmem_cache_destroy(f->cache);
+
+	if (f->data_bufio)
+		dm_bufio_client_destroy(f->data_bufio);
+	if (f->bufio)
+		dm_bufio_client_destroy(f->bufio);
+
+	if (f->dev)
+		dm_put_device(v->ti, f->dev);
+out:
+	kfree(f);
+	v->fec = NULL;
+}
+
+static void *fec_rs_alloc(gfp_t gfp_mask, void *pool_data)
+{
+	struct dm_verity *v = (struct dm_verity *)pool_data;
+
+	return init_rs(8, 0x11d, 0, 1, v->fec->roots);
+}
+
+static void fec_rs_free(void *element, void *pool_data)
+{
+	struct rs_control *rs = (struct rs_control *)element;
+
+	if (rs)
+		free_rs(rs);
+}
+
+bool verity_is_fec_opt_arg(const char *arg_name)
+{
+	return (!strcasecmp(arg_name, DM_VERITY_OPT_FEC_DEV) ||
+		!strcasecmp(arg_name, DM_VERITY_OPT_FEC_BLOCKS) ||
+		!strcasecmp(arg_name, DM_VERITY_OPT_FEC_START) ||
+		!strcasecmp(arg_name, DM_VERITY_OPT_FEC_ROOTS));
+}
+
+int verity_fec_parse_opt_args(struct dm_arg_set *as, struct dm_verity *v,
+			      unsigned *argc, const char *arg_name)
+{
+	int r;
+	struct dm_target *ti = v->ti;
+	const char *arg_value;
+	unsigned long long num_ll;
+	unsigned char num_c;
+	char dummy;
+
+	if (!*argc) {
+		ti->error = "FEC feature arguments require a value";
+		return -EINVAL;
+	}
+
+	arg_value = dm_shift_arg(as);
+	(*argc)--;
+
+	if (!strcasecmp(arg_name, DM_VERITY_OPT_FEC_DEV)) {
+		r = dm_get_device(ti, arg_value, FMODE_READ, &v->fec->dev);
+		if (r) {
+			ti->error = "FEC device lookup failed";
+			return r;
+		}
+
+	} else if (!strcasecmp(arg_name, DM_VERITY_OPT_FEC_BLOCKS)) {
+		if (sscanf(arg_value, "%llu%c", &num_ll, &dummy) != 1 ||
+		    ((sector_t)(num_ll << (v->data_dev_block_bits - SECTOR_SHIFT))
+		     >> (v->data_dev_block_bits - SECTOR_SHIFT) != num_ll)) {
+			ti->error = "Invalid " DM_VERITY_OPT_FEC_BLOCKS;
+			return -EINVAL;
+		}
+		v->fec->blocks = num_ll;
+
+	} else if (!strcasecmp(arg_name, DM_VERITY_OPT_FEC_START)) {
+		if (sscanf(arg_value, "%llu%c", &num_ll, &dummy) != 1 ||
+		    ((sector_t)(num_ll << (v->data_dev_block_bits - SECTOR_SHIFT)) >>
+		     (v->data_dev_block_bits - SECTOR_SHIFT) != num_ll)) {
+			ti->error = "Invalid " DM_VERITY_OPT_FEC_START;
+			return -EINVAL;
+		}
+		v->fec->start = num_ll;
+
+	} else if (!strcasecmp(arg_name, DM_VERITY_OPT_FEC_ROOTS)) {
+		if (sscanf(arg_value, "%hhu%c", &num_c, &dummy) != 1 || !num_c ||
+		    num_c < (DM_VERITY_FEC_RSM - DM_VERITY_FEC_MAX_RSN) ||
+		    num_c > (DM_VERITY_FEC_RSM - DM_VERITY_FEC_MIN_RSN)) {
+			ti->error = "Invalid " DM_VERITY_OPT_FEC_ROOTS;
+			return -EINVAL;
+		}
+		v->fec->roots = num_c;
+
+	} else {
+		ti->error = "Unrecognized verity FEC feature request";
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Allocate dm_verity_fec for v->fec. Must be called before verity_fec_ctr.
+ */
+int verity_fec_ctr_alloc(struct dm_verity *v)
+{
+	struct dm_verity_fec *f;
+
+	f = kzalloc(sizeof(struct dm_verity_fec), GFP_KERNEL);
+	if (!f) {
+		v->ti->error = "Cannot allocate FEC structure";
+		return -ENOMEM;
+	}
+	v->fec = f;
+
+	return 0;
+}
+
+/*
+ * Validate arguments and preallocate memory. Must be called after arguments
+ * have been parsed using verity_fec_parse_opt_args.
+ */
+int verity_fec_ctr(struct dm_verity *v)
+{
+	struct dm_verity_fec *f = v->fec;
+	struct dm_target *ti = v->ti;
+	u64 hash_blocks;
+
+	if (!verity_fec_is_enabled(v)) {
+		verity_fec_dtr(v);
+		return 0;
+	}
+
+	/*
+	 * FEC is computed over data blocks, possible metadata, and
+	 * hash blocks. In other words, FEC covers total of fec_blocks
+	 * blocks consisting of the following:
+	 *
+	 *  data blocks | hash blocks | metadata (optional)
+	 *
+	 * We allow metadata after hash blocks to support a use case
+	 * where all data is stored on the same device and FEC covers
+	 * the entire area.
+	 *
+	 * If metadata is included, we require it to be available on the
+	 * hash device after the hash blocks.
+	 */
+
+	hash_blocks = v->hash_blocks - v->hash_start;
+
+	/*
+	 * Require matching block sizes for data and hash devices for
+	 * simplicity.
+	 */
+	if (v->data_dev_block_bits != v->hash_dev_block_bits) {
+		ti->error = "Block sizes must match to use FEC";
+		return -EINVAL;
+	}
+
+	if (!f->roots) {
+		ti->error = "Missing " DM_VERITY_OPT_FEC_ROOTS;
+		return -EINVAL;
+	}
+	f->rsn = DM_VERITY_FEC_RSM - f->roots;
+
+	if (!f->blocks) {
+		ti->error = "Missing " DM_VERITY_OPT_FEC_BLOCKS;
+		return -EINVAL;
+	}
+
+	f->rounds = f->blocks;
+	if (sector_div(f->rounds, f->rsn))
+		f->rounds++;
+
+	/*
+	 * Due to optional metadata, f->blocks can be larger than
+	 * data_blocks and hash_blocks combined.
+	 */
+	if (f->blocks < v->data_blocks + hash_blocks || !f->rounds) {
+		ti->error = "Invalid " DM_VERITY_OPT_FEC_BLOCKS;
+		return -EINVAL;
+	}
+
+	/*
+	 * Metadata is accessed through the hash device, so we require
+	 * it to be large enough.
+	 */
+	f->hash_blocks = f->blocks - v->data_blocks;
+	if (dm_bufio_get_device_size(v->bufio) < f->hash_blocks) {
+		ti->error = "Hash device is too small for "
+			DM_VERITY_OPT_FEC_BLOCKS;
+		return -E2BIG;
+	}
+
+	f->bufio = dm_bufio_client_create(f->dev->bdev,
+					  1 << v->data_dev_block_bits,
+					  1, 0, NULL, NULL);
+	if (IS_ERR(f->bufio)) {
+		ti->error = "Cannot initialize FEC bufio client";
+		return PTR_ERR(f->bufio);
+	}
+
+	if (dm_bufio_get_device_size(f->bufio) <
+	    ((f->start + f->rounds * f->roots) >> v->data_dev_block_bits)) {
+		ti->error = "FEC device is too small";
+		return -E2BIG;
+	}
+
+	f->data_bufio = dm_bufio_client_create(v->data_dev->bdev,
+					       1 << v->data_dev_block_bits,
+					       1, 0, NULL, NULL);
+	if (IS_ERR(f->data_bufio)) {
+		ti->error = "Cannot initialize FEC data bufio client";
+		return PTR_ERR(f->data_bufio);
+	}
+
+	if (dm_bufio_get_device_size(f->data_bufio) < v->data_blocks) {
+		ti->error = "Data device is too small";
+		return -E2BIG;
+	}
+
+	/* Preallocate an rs_control structure for each worker thread */
+	f->rs_pool = mempool_create(num_online_cpus(), fec_rs_alloc,
+				    fec_rs_free, (void *) v);
+	if (!f->rs_pool) {
+		ti->error = "Cannot allocate RS pool";
+		return -ENOMEM;
+	}
+
+	f->cache = kmem_cache_create("dm_verity_fec_buffers",
+				     f->rsn << DM_VERITY_FEC_BUF_RS_BITS,
+				     0, 0, NULL);
+	if (!f->cache) {
+		ti->error = "Cannot create FEC buffer cache";
+		return -ENOMEM;
+	}
+
+	/* Preallocate DM_VERITY_FEC_BUF_PREALLOC buffers for each thread */
+	f->prealloc_pool = mempool_create_slab_pool(num_online_cpus() *
+						    DM_VERITY_FEC_BUF_PREALLOC,
+						    f->cache);
+	if (!f->prealloc_pool) {
+		ti->error = "Cannot allocate FEC buffer prealloc pool";
+		return -ENOMEM;
+	}
+
+	f->extra_pool = mempool_create_slab_pool(0, f->cache);
+	if (!f->extra_pool) {
+		ti->error = "Cannot allocate FEC buffer extra pool";
+		return -ENOMEM;
+	}
+
+	/* Preallocate an output buffer for each thread */
+	f->output_pool = mempool_create_kmalloc_pool(num_online_cpus(),
+						     1 << v->data_dev_block_bits);
+	if (!f->output_pool) {
+		ti->error = "Cannot allocate FEC output pool";
+		return -ENOMEM;
+	}
+
+	/* Reserve space for our per-bio data */
+	ti->per_bio_data_size += sizeof(struct dm_verity_fec_io);
+
+	return 0;
+}
diff --git a/drivers/md/dm-verity-fec.h b/drivers/md/dm-verity-fec.h
new file mode 100644
index 000000000000..7fa0298b995e
--- /dev/null
+++ b/drivers/md/dm-verity-fec.h
@@ -0,0 +1,152 @@
+/*
+ * Copyright (C) 2015 Google, Inc.
+ *
+ * Author: Sami Tolvanen <samitolvanen@google.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 DM_VERITY_FEC_H
+#define DM_VERITY_FEC_H
+
+#include "dm-verity.h"
+#include <linux/rslib.h>
+
+/* Reed-Solomon(M, N) parameters */
+#define DM_VERITY_FEC_RSM		255
+#define DM_VERITY_FEC_MAX_RSN		253
+#define DM_VERITY_FEC_MIN_RSN		231	/* ~10% space overhead */
+
+/* buffers for deinterleaving and decoding */
+#define DM_VERITY_FEC_BUF_PREALLOC	1	/* buffers to preallocate */
+#define DM_VERITY_FEC_BUF_RS_BITS	4	/* 1 << RS blocks per buffer */
+/* we need buffers for at most 1 << block size RS blocks */
+#define DM_VERITY_FEC_BUF_MAX \
+	(1 << (PAGE_SHIFT - DM_VERITY_FEC_BUF_RS_BITS))
+
+#define DM_VERITY_OPT_FEC_DEV		"use_fec_from_device"
+#define DM_VERITY_OPT_FEC_BLOCKS	"fec_blocks"
+#define DM_VERITY_OPT_FEC_START		"fec_start"
+#define DM_VERITY_OPT_FEC_ROOTS		"fec_roots"
+
+/* configuration */
+struct dm_verity_fec {
+	struct dm_dev *dev;	/* parity data device */
+	struct dm_bufio_client *data_bufio;	/* for data dev access */
+	struct dm_bufio_client *bufio;		/* for parity data access */
+	sector_t start;		/* parity data start in blocks */
+	sector_t blocks;	/* number of blocks covered */
+	sector_t rounds;	/* number of interleaving rounds */
+	sector_t hash_blocks;	/* blocks covered after v->hash_start */
+	unsigned char roots;	/* number of parity bytes, M-N of RS(M, N) */
+	unsigned char rsn;	/* N of RS(M, N) */
+	mempool_t *rs_pool;	/* mempool for fio->rs */
+	mempool_t *prealloc_pool;	/* mempool for preallocated buffers */
+	mempool_t *extra_pool;	/* mempool for extra buffers */
+	mempool_t *output_pool;	/* mempool for output */
+	struct kmem_cache *cache;	/* cache for buffers */
+};
+
+/* per-bio data */
+struct dm_verity_fec_io {
+	struct rs_control *rs;	/* Reed-Solomon state */
+	int erasures[DM_VERITY_FEC_MAX_RSN];	/* erasures for decode_rs8 */
+	u8 *bufs[DM_VERITY_FEC_BUF_MAX];	/* bufs for deinterleaving */
+	unsigned nbufs;		/* number of buffers allocated */
+	u8 *output;		/* buffer for corrected output */
+	size_t output_pos;
+};
+
+#ifdef CONFIG_DM_VERITY_FEC
+
+/* each feature parameter requires a value */
+#define DM_VERITY_OPTS_FEC	8
+
+extern bool verity_fec_is_enabled(struct dm_verity *v);
+
+extern int verity_fec_decode(struct dm_verity *v, struct dm_verity_io *io,
+			     enum verity_block_type type, sector_t block,
+			     u8 *dest, struct bvec_iter *iter);
+
+extern unsigned verity_fec_status_table(struct dm_verity *v, unsigned sz,
+					char *result, unsigned maxlen);
+
+extern void verity_fec_finish_io(struct dm_verity_io *io);
+extern void verity_fec_init_io(struct dm_verity_io *io);
+
+extern bool verity_is_fec_opt_arg(const char *arg_name);
+extern int verity_fec_parse_opt_args(struct dm_arg_set *as,
+				     struct dm_verity *v, unsigned *argc,
+				     const char *arg_name);
+
+extern void verity_fec_dtr(struct dm_verity *v);
+
+extern int verity_fec_ctr_alloc(struct dm_verity *v);
+extern int verity_fec_ctr(struct dm_verity *v);
+
+#else /* !CONFIG_DM_VERITY_FEC */
+
+#define DM_VERITY_OPTS_FEC	0
+
+static inline bool verity_fec_is_enabled(struct dm_verity *v)
+{
+	return false;
+}
+
+static inline int verity_fec_decode(struct dm_verity *v,
+				    struct dm_verity_io *io,
+				    enum verity_block_type type,
+				    sector_t block, u8 *dest,
+				    struct bvec_iter *iter)
+{
+	return -EOPNOTSUPP;
+}
+
+static inline unsigned verity_fec_status_table(struct dm_verity *v,
+					       unsigned sz, char *result,
+					       unsigned maxlen)
+{
+	return sz;
+}
+
+static inline void verity_fec_finish_io(struct dm_verity_io *io)
+{
+}
+
+static inline void verity_fec_init_io(struct dm_verity_io *io)
+{
+}
+
+static inline bool verity_is_fec_opt_arg(const char *arg_name)
+{
+	return false;
+}
+
+static inline int verity_fec_parse_opt_args(struct dm_arg_set *as,
+					    struct dm_verity *v,
+					    unsigned *argc,
+					    const char *arg_name)
+{
+	return -EINVAL;
+}
+
+static inline void verity_fec_dtr(struct dm_verity *v)
+{
+}
+
+static inline int verity_fec_ctr_alloc(struct dm_verity *v)
+{
+	return 0;
+}
+
+static inline int verity_fec_ctr(struct dm_verity *v)
+{
+	return 0;
+}
+
+#endif /* CONFIG_DM_VERITY_FEC */
+
+#endif /* DM_VERITY_FEC_H */
diff --git a/drivers/md/dm-verity.c b/drivers/md/dm-verity-target.c
index ccf41886ebcf..5c5d30cb6ec5 100644
--- a/drivers/md/dm-verity.c
+++ b/drivers/md/dm-verity-target.c
@@ -14,12 +14,11 @@
  * access behavior.
  */
 
-#include "dm-bufio.h"
+#include "dm-verity.h"
+#include "dm-verity-fec.h"
 
 #include <linux/module.h>
-#include <linux/device-mapper.h>
 #include <linux/reboot.h>
-#include <crypto/hash.h>
 
 #define DM_MSG_PREFIX			"verity"
 
@@ -28,83 +27,18 @@
 
 #define DM_VERITY_DEFAULT_PREFETCH_SIZE	262144
 
-#define DM_VERITY_MAX_LEVELS		63
 #define DM_VERITY_MAX_CORRUPTED_ERRS	100
 
 #define DM_VERITY_OPT_LOGGING		"ignore_corruption"
 #define DM_VERITY_OPT_RESTART		"restart_on_corruption"
+#define DM_VERITY_OPT_IGN_ZEROES	"ignore_zero_blocks"
+
+#define DM_VERITY_OPTS_MAX		(2 + DM_VERITY_OPTS_FEC)
 
 static unsigned dm_verity_prefetch_cluster = DM_VERITY_DEFAULT_PREFETCH_SIZE;
 
 module_param_named(prefetch_cluster, dm_verity_prefetch_cluster, uint, S_IRUGO | S_IWUSR);
 
-enum verity_mode {
-	DM_VERITY_MODE_EIO,
-	DM_VERITY_MODE_LOGGING,
-	DM_VERITY_MODE_RESTART
-};
-
-enum verity_block_type {
-	DM_VERITY_BLOCK_TYPE_DATA,
-	DM_VERITY_BLOCK_TYPE_METADATA
-};
-
-struct dm_verity {
-	struct dm_dev *data_dev;
-	struct dm_dev *hash_dev;
-	struct dm_target *ti;
-	struct dm_bufio_client *bufio;
-	char *alg_name;
-	struct crypto_shash *tfm;
-	u8 *root_digest;	/* digest of the root block */
-	u8 *salt;		/* salt: its size is salt_size */
-	unsigned salt_size;
-	sector_t data_start;	/* data offset in 512-byte sectors */
-	sector_t hash_start;	/* hash start in blocks */
-	sector_t data_blocks;	/* the number of data blocks */
-	sector_t hash_blocks;	/* the number of hash blocks */
-	unsigned char data_dev_block_bits;	/* log2(data blocksize) */
-	unsigned char hash_dev_block_bits;	/* log2(hash blocksize) */
-	unsigned char hash_per_block_bits;	/* log2(hashes in hash block) */
-	unsigned char levels;	/* the number of tree levels */
-	unsigned char version;
-	unsigned digest_size;	/* digest size for the current hash algorithm */
-	unsigned shash_descsize;/* the size of temporary space for crypto */
-	int hash_failed;	/* set to 1 if hash of any block failed */
-	enum verity_mode mode;	/* mode for handling verification errors */
-	unsigned corrupted_errs;/* Number of errors for corrupted blocks */
-
-	struct workqueue_struct *verify_wq;
-
-	/* starting blocks for each tree level. 0 is the lowest level. */
-	sector_t hash_level_block[DM_VERITY_MAX_LEVELS];
-};
-
-struct dm_verity_io {
-	struct dm_verity *v;
-
-	/* original values of bio->bi_end_io and bio->bi_private */
-	bio_end_io_t *orig_bi_end_io;
-	void *orig_bi_private;
-
-	sector_t block;
-	unsigned n_blocks;
-
-	struct bvec_iter iter;
-
-	struct work_struct work;
-
-	/*
-	 * Three variably-size fields follow this struct:
-	 *
-	 * u8 hash_desc[v->shash_descsize];
-	 * u8 real_digest[v->digest_size];
-	 * u8 want_digest[v->digest_size];
-	 *
-	 * To access them use: io_hash_desc(), io_real_digest() and io_want_digest().
-	 */
-};
-
 struct dm_verity_prefetch_work {
 	struct work_struct work;
 	struct dm_verity *v;
@@ -112,21 +46,6 @@ struct dm_verity_prefetch_work {
 	unsigned n_blocks;
 };
 
-static struct shash_desc *io_hash_desc(struct dm_verity *v, struct dm_verity_io *io)
-{
-	return (struct shash_desc *)(io + 1);
-}
-
-static u8 *io_real_digest(struct dm_verity *v, struct dm_verity_io *io)
-{
-	return (u8 *)(io + 1) + v->shash_descsize;
-}
-
-static u8 *io_want_digest(struct dm_verity *v, struct dm_verity_io *io)
-{
-	return (u8 *)(io + 1) + v->shash_descsize + v->digest_size;
-}
-
 /*
  * Auxiliary structure appended to each dm-bufio buffer. If the value
  * hash_verified is nonzero, hash of the block has been verified.
@@ -173,6 +92,84 @@ static sector_t verity_position_at_level(struct dm_verity *v, sector_t block,
 	return block >> (level * v->hash_per_block_bits);
 }
 
+/*
+ * Wrapper for crypto_shash_init, which handles verity salting.
+ */
+static int verity_hash_init(struct dm_verity *v, struct shash_desc *desc)
+{
+	int r;
+
+	desc->tfm = v->tfm;
+	desc->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+	r = crypto_shash_init(desc);
+
+	if (unlikely(r < 0)) {
+		DMERR("crypto_shash_init failed: %d", r);
+		return r;
+	}
+
+	if (likely(v->version >= 1)) {
+		r = crypto_shash_update(desc, v->salt, v->salt_size);
+
+		if (unlikely(r < 0)) {
+			DMERR("crypto_shash_update failed: %d", r);
+			return r;
+		}
+	}
+
+	return 0;
+}
+
+static int verity_hash_update(struct dm_verity *v, struct shash_desc *desc,
+			      const u8 *data, size_t len)
+{
+	int r = crypto_shash_update(desc, data, len);
+
+	if (unlikely(r < 0))
+		DMERR("crypto_shash_update failed: %d", r);
+
+	return r;
+}
+
+static int verity_hash_final(struct dm_verity *v, struct shash_desc *desc,
+			     u8 *digest)
+{
+	int r;
+
+	if (unlikely(!v->version)) {
+		r = crypto_shash_update(desc, v->salt, v->salt_size);
+
+		if (r < 0) {
+			DMERR("crypto_shash_update failed: %d", r);
+			return r;
+		}
+	}
+
+	r = crypto_shash_final(desc, digest);
+
+	if (unlikely(r < 0))
+		DMERR("crypto_shash_final failed: %d", r);
+
+	return r;
+}
+
+int verity_hash(struct dm_verity *v, struct shash_desc *desc,
+		const u8 *data, size_t len, u8 *digest)
+{
+	int r;
+
+	r = verity_hash_init(v, desc);
+	if (unlikely(r < 0))
+		return r;
+
+	r = verity_hash_update(v, desc, data, len);
+	if (unlikely(r < 0))
+		return r;
+
+	return verity_hash_final(v, desc, digest);
+}
+
 static void verity_hash_at_level(struct dm_verity *v, sector_t block, int level,
 				 sector_t *hash_block, unsigned *offset)
 {
@@ -246,17 +243,17 @@ out:
  * Verify hash of a metadata block pertaining to the specified data block
  * ("block" argument) at a specified level ("level" argument).
  *
- * On successful return, io_want_digest(v, io) contains the hash value for
- * a lower tree level or for the data block (if we're at the lowest leve).
+ * On successful return, verity_io_want_digest(v, io) contains the hash value
+ * for a lower tree level or for the data block (if we're at the lowest level).
  *
  * If "skip_unverified" is true, unverified buffer is skipped and 1 is returned.
  * If "skip_unverified" is false, unverified buffer is hashed and verified
- * against current value of io_want_digest(v, io).
+ * against current value of verity_io_want_digest(v, io).
  */
-static int verity_verify_level(struct dm_verity_io *io, sector_t block,
-			       int level, bool skip_unverified)
+static int verity_verify_level(struct dm_verity *v, struct dm_verity_io *io,
+			       sector_t block, int level, bool skip_unverified,
+			       u8 *want_digest)
 {
-	struct dm_verity *v = io->v;
 	struct dm_buffer *buf;
 	struct buffer_aux *aux;
 	u8 *data;
@@ -273,72 +270,128 @@ static int verity_verify_level(struct dm_verity_io *io, sector_t block,
 	aux = dm_bufio_get_aux_data(buf);
 
 	if (!aux->hash_verified) {
-		struct shash_desc *desc;
-		u8 *result;
-
 		if (skip_unverified) {
 			r = 1;
 			goto release_ret_r;
 		}
 
-		desc = io_hash_desc(v, io);
-		desc->tfm = v->tfm;
-		desc->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
-		r = crypto_shash_init(desc);
-		if (r < 0) {
-			DMERR("crypto_shash_init failed: %d", r);
+		r = verity_hash(v, verity_io_hash_desc(v, io),
+				data, 1 << v->hash_dev_block_bits,
+				verity_io_real_digest(v, io));
+		if (unlikely(r < 0))
 			goto release_ret_r;
-		}
-
-		if (likely(v->version >= 1)) {
-			r = crypto_shash_update(desc, v->salt, v->salt_size);
-			if (r < 0) {
-				DMERR("crypto_shash_update failed: %d", r);
-				goto release_ret_r;
-			}
-		}
 
-		r = crypto_shash_update(desc, data, 1 << v->hash_dev_block_bits);
-		if (r < 0) {
-			DMERR("crypto_shash_update failed: %d", r);
+		if (likely(memcmp(verity_io_real_digest(v, io), want_digest,
+				  v->digest_size) == 0))
+			aux->hash_verified = 1;
+		else if (verity_fec_decode(v, io,
+					   DM_VERITY_BLOCK_TYPE_METADATA,
+					   hash_block, data, NULL) == 0)
+			aux->hash_verified = 1;
+		else if (verity_handle_err(v,
+					   DM_VERITY_BLOCK_TYPE_METADATA,
+					   hash_block)) {
+			r = -EIO;
 			goto release_ret_r;
 		}
+	}
 
-		if (!v->version) {
-			r = crypto_shash_update(desc, v->salt, v->salt_size);
-			if (r < 0) {
-				DMERR("crypto_shash_update failed: %d", r);
-				goto release_ret_r;
-			}
-		}
+	data += offset;
+	memcpy(want_digest, data, v->digest_size);
+	r = 0;
 
-		result = io_real_digest(v, io);
-		r = crypto_shash_final(desc, result);
-		if (r < 0) {
-			DMERR("crypto_shash_final failed: %d", r);
-			goto release_ret_r;
-		}
-		if (unlikely(memcmp(result, io_want_digest(v, io), v->digest_size))) {
-			if (verity_handle_err(v, DM_VERITY_BLOCK_TYPE_METADATA,
-					      hash_block)) {
-				r = -EIO;
-				goto release_ret_r;
-			}
-		} else
-			aux->hash_verified = 1;
+release_ret_r:
+	dm_bufio_release(buf);
+	return r;
+}
+
+/*
+ * Find a hash for a given block, write it to digest and verify the integrity
+ * of the hash tree if necessary.
+ */
+int verity_hash_for_block(struct dm_verity *v, struct dm_verity_io *io,
+			  sector_t block, u8 *digest, bool *is_zero)
+{
+	int r = 0, i;
+
+	if (likely(v->levels)) {
+		/*
+		 * First, we try to get the requested hash for
+		 * the current block. If the hash block itself is
+		 * verified, zero is returned. If it isn't, this
+		 * function returns 1 and we fall back to whole
+		 * chain verification.
+		 */
+		r = verity_verify_level(v, io, block, 0, true, digest);
+		if (likely(r <= 0))
+			goto out;
 	}
 
-	data += offset;
+	memcpy(digest, v->root_digest, v->digest_size);
 
-	memcpy(io_want_digest(v, io), data, v->digest_size);
+	for (i = v->levels - 1; i >= 0; i--) {
+		r = verity_verify_level(v, io, block, i, false, digest);
+		if (unlikely(r))
+			goto out;
+	}
+out:
+	if (!r && v->zero_digest)
+		*is_zero = !memcmp(v->zero_digest, digest, v->digest_size);
+	else
+		*is_zero = false;
+
+	return r;
+}
+
+/*
+ * Calls function process for 1 << v->data_dev_block_bits bytes in the bio_vec
+ * starting from iter.
+ */
+int verity_for_bv_block(struct dm_verity *v, struct dm_verity_io *io,
+			struct bvec_iter *iter,
+			int (*process)(struct dm_verity *v,
+				       struct dm_verity_io *io, u8 *data,
+				       size_t len))
+{
+	unsigned todo = 1 << v->data_dev_block_bits;
+	struct bio *bio = dm_bio_from_per_bio_data(io, v->ti->per_bio_data_size);
+
+	do {
+		int r;
+		u8 *page;
+		unsigned len;
+		struct bio_vec bv = bio_iter_iovec(bio, *iter);
+
+		page = kmap_atomic(bv.bv_page);
+		len = bv.bv_len;
+
+		if (likely(len >= todo))
+			len = todo;
+
+		r = process(v, io, page + bv.bv_offset, len);
+		kunmap_atomic(page);
+
+		if (r < 0)
+			return r;
+
+		bio_advance_iter(bio, iter, len);
+		todo -= len;
+	} while (todo);
 
-	dm_bufio_release(buf);
 	return 0;
+}
 
-release_ret_r:
-	dm_bufio_release(buf);
+static int verity_bv_hash_update(struct dm_verity *v, struct dm_verity_io *io,
+				 u8 *data, size_t len)
+{
+	return verity_hash_update(v, verity_io_hash_desc(v, io), data, len);
+}
 
-	return r;
+static int verity_bv_zero(struct dm_verity *v, struct dm_verity_io *io,
+			  u8 *data, size_t len)
+{
+	memset(data, 0, len);
+	return 0;
 }
 
 /*
@@ -346,99 +399,56 @@ release_ret_r:
  */
 static int verity_verify_io(struct dm_verity_io *io)
 {
+	bool is_zero;
 	struct dm_verity *v = io->v;
-	struct bio *bio = dm_bio_from_per_bio_data(io,
-						   v->ti->per_bio_data_size);
+	struct bvec_iter start;
 	unsigned b;
-	int i;
 
 	for (b = 0; b < io->n_blocks; b++) {
-		struct shash_desc *desc;
-		u8 *result;
 		int r;
-		unsigned todo;
+		struct shash_desc *desc = verity_io_hash_desc(v, io);
+
+		r = verity_hash_for_block(v, io, io->block + b,
+					  verity_io_want_digest(v, io),
+					  &is_zero);
+		if (unlikely(r < 0))
+			return r;
 
-		if (likely(v->levels)) {
+		if (is_zero) {
 			/*
-			 * First, we try to get the requested hash for
-			 * the current block. If the hash block itself is
-			 * verified, zero is returned. If it isn't, this
-			 * function returns 0 and we fall back to whole
-			 * chain verification.
+			 * If we expect a zero block, don't validate, just
+			 * return zeros.
 			 */
-			int r = verity_verify_level(io, io->block + b, 0, true);
-			if (likely(!r))
-				goto test_block_hash;
-			if (r < 0)
+			r = verity_for_bv_block(v, io, &io->iter,
+						verity_bv_zero);
+			if (unlikely(r < 0))
 				return r;
-		}
 
-		memcpy(io_want_digest(v, io), v->root_digest, v->digest_size);
-
-		for (i = v->levels - 1; i >= 0; i--) {
-			int r = verity_verify_level(io, io->block + b, i, false);
-			if (unlikely(r))
-				return r;
+			continue;
 		}
 
-test_block_hash:
-		desc = io_hash_desc(v, io);
-		desc->tfm = v->tfm;
-		desc->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
-		r = crypto_shash_init(desc);
-		if (r < 0) {
-			DMERR("crypto_shash_init failed: %d", r);
+		r = verity_hash_init(v, desc);
+		if (unlikely(r < 0))
 			return r;
-		}
-
-		if (likely(v->version >= 1)) {
-			r = crypto_shash_update(desc, v->salt, v->salt_size);
-			if (r < 0) {
-				DMERR("crypto_shash_update failed: %d", r);
-				return r;
-			}
-		}
-		todo = 1 << v->data_dev_block_bits;
-		do {
-			u8 *page;
-			unsigned len;
-			struct bio_vec bv = bio_iter_iovec(bio, io->iter);
-
-			page = kmap_atomic(bv.bv_page);
-			len = bv.bv_len;
-			if (likely(len >= todo))
-				len = todo;
-			r = crypto_shash_update(desc, page + bv.bv_offset, len);
-			kunmap_atomic(page);
-
-			if (r < 0) {
-				DMERR("crypto_shash_update failed: %d", r);
-				return r;
-			}
-
-			bio_advance_iter(bio, &io->iter, len);
-			todo -= len;
-		} while (todo);
 
-		if (!v->version) {
-			r = crypto_shash_update(desc, v->salt, v->salt_size);
-			if (r < 0) {
-				DMERR("crypto_shash_update failed: %d", r);
-				return r;
-			}
-		}
+		start = io->iter;
+		r = verity_for_bv_block(v, io, &io->iter, verity_bv_hash_update);
+		if (unlikely(r < 0))
+			return r;
 
-		result = io_real_digest(v, io);
-		r = crypto_shash_final(desc, result);
-		if (r < 0) {
-			DMERR("crypto_shash_final failed: %d", r);
+		r = verity_hash_final(v, desc, verity_io_real_digest(v, io));
+		if (unlikely(r < 0))
 			return r;
-		}
-		if (unlikely(memcmp(result, io_want_digest(v, io), v->digest_size))) {
-			if (verity_handle_err(v, DM_VERITY_BLOCK_TYPE_DATA,
-					      io->block + b))
-				return -EIO;
-		}
+
+		if (likely(memcmp(verity_io_real_digest(v, io),
+				  verity_io_want_digest(v, io), v->digest_size) == 0))
+			continue;
+		else if (verity_fec_decode(v, io, DM_VERITY_BLOCK_TYPE_DATA,
+					   io->block + b, NULL, &start) == 0)
+			continue;
+		else if (verity_handle_err(v, DM_VERITY_BLOCK_TYPE_DATA,
+					   io->block + b))
+			return -EIO;
 	}
 
 	return 0;
@@ -453,9 +463,10 @@ static void verity_finish_io(struct dm_verity_io *io, int error)
 	struct bio *bio = dm_bio_from_per_bio_data(io, v->ti->per_bio_data_size);
 
 	bio->bi_end_io = io->orig_bi_end_io;
-	bio->bi_private = io->orig_bi_private;
 	bio->bi_error = error;
 
+	verity_fec_finish_io(io);
+
 	bio_endio(bio);
 }
 
@@ -470,7 +481,7 @@ static void verity_end_io(struct bio *bio)
 {
 	struct dm_verity_io *io = bio->bi_private;
 
-	if (bio->bi_error) {
+	if (bio->bi_error && !verity_fec_is_enabled(io->v)) {
 		verity_finish_io(io, bio->bi_error);
 		return;
 	}
@@ -566,7 +577,6 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 	io = dm_per_bio_data(bio, ti->per_bio_data_size);
 	io->v = v;
 	io->orig_bi_end_io = bio->bi_end_io;
-	io->orig_bi_private = bio->bi_private;
 	io->block = bio->bi_iter.bi_sector >> (v->data_dev_block_bits - SECTOR_SHIFT);
 	io->n_blocks = bio->bi_iter.bi_size >> v->data_dev_block_bits;
 
@@ -574,6 +584,8 @@ static int verity_map(struct dm_target *ti, struct bio *bio)
 	bio->bi_private = io;
 	io->iter = bio->bi_iter;
 
+	verity_fec_init_io(io);
+
 	verity_submit_prefetch(v, io);
 
 	generic_make_request(bio);
@@ -588,6 +600,7 @@ static void verity_status(struct dm_target *ti, status_type_t type,
 			  unsigned status_flags, char *result, unsigned maxlen)
 {
 	struct dm_verity *v = ti->private;
+	unsigned args = 0;
 	unsigned sz = 0;
 	unsigned x;
 
@@ -614,8 +627,17 @@ static void verity_status(struct dm_target *ti, status_type_t type,
 		else
 			for (x = 0; x < v->salt_size; x++)
 				DMEMIT("%02x", v->salt[x]);
+		if (v->mode != DM_VERITY_MODE_EIO)
+			args++;
+		if (verity_fec_is_enabled(v))
+			args += DM_VERITY_OPTS_FEC;
+		if (v->zero_digest)
+			args++;
+		if (!args)
+			return;
+		DMEMIT(" %u", args);
 		if (v->mode != DM_VERITY_MODE_EIO) {
-			DMEMIT(" 1 ");
+			DMEMIT(" ");
 			switch (v->mode) {
 			case DM_VERITY_MODE_LOGGING:
 				DMEMIT(DM_VERITY_OPT_LOGGING);
@@ -627,6 +649,9 @@ static void verity_status(struct dm_target *ti, status_type_t type,
 				BUG();
 			}
 		}
+		if (v->zero_digest)
+			DMEMIT(" " DM_VERITY_OPT_IGN_ZEROES);
+		sz = verity_fec_status_table(v, sz, result, maxlen);
 		break;
 	}
 }
@@ -677,6 +702,7 @@ static void verity_dtr(struct dm_target *ti)
 
 	kfree(v->salt);
 	kfree(v->root_digest);
+	kfree(v->zero_digest);
 
 	if (v->tfm)
 		crypto_free_shash(v->tfm);
@@ -689,9 +715,94 @@ static void verity_dtr(struct dm_target *ti)
 	if (v->data_dev)
 		dm_put_device(ti, v->data_dev);
 
+	verity_fec_dtr(v);
+
 	kfree(v);
 }
 
+static int verity_alloc_zero_digest(struct dm_verity *v)
+{
+	int r = -ENOMEM;
+	struct shash_desc *desc;
+	u8 *zero_data;
+
+	v->zero_digest = kmalloc(v->digest_size, GFP_KERNEL);
+
+	if (!v->zero_digest)
+		return r;
+
+	desc = kmalloc(v->shash_descsize, GFP_KERNEL);
+
+	if (!desc)
+		return r; /* verity_dtr will free zero_digest */
+
+	zero_data = kzalloc(1 << v->data_dev_block_bits, GFP_KERNEL);
+
+	if (!zero_data)
+		goto out;
+
+	r = verity_hash(v, desc, zero_data, 1 << v->data_dev_block_bits,
+			v->zero_digest);
+
+out:
+	kfree(desc);
+	kfree(zero_data);
+
+	return r;
+}
+
+static int verity_parse_opt_args(struct dm_arg_set *as, struct dm_verity *v)
+{
+	int r;
+	unsigned argc;
+	struct dm_target *ti = v->ti;
+	const char *arg_name;
+
+	static struct dm_arg _args[] = {
+		{0, DM_VERITY_OPTS_MAX, "Invalid number of feature args"},
+	};
+
+	r = dm_read_arg_group(_args, as, &argc, &ti->error);
+	if (r)
+		return -EINVAL;
+
+	if (!argc)
+		return 0;
+
+	do {
+		arg_name = dm_shift_arg(as);
+		argc--;
+
+		if (!strcasecmp(arg_name, DM_VERITY_OPT_LOGGING)) {
+			v->mode = DM_VERITY_MODE_LOGGING;
+			continue;
+
+		} else if (!strcasecmp(arg_name, DM_VERITY_OPT_RESTART)) {
+			v->mode = DM_VERITY_MODE_RESTART;
+			continue;
+
+		} else if (!strcasecmp(arg_name, DM_VERITY_OPT_IGN_ZEROES)) {
+			r = verity_alloc_zero_digest(v);
+			if (r) {
+				ti->error = "Cannot allocate zero digest";
+				return r;
+			}
+			continue;
+
+		} else if (verity_is_fec_opt_arg(arg_name)) {
+			r = verity_fec_parse_opt_args(as, v, &argc, arg_name);
+			if (r)
+				return r;
+			continue;
+		}
+
+		ti->error = "Unrecognized verity feature request";
+		return -EINVAL;
+	} while (argc && !r);
+
+	return r;
+}
+
 /*
  * Target parameters:
  *	<version>	The current format is version 1.
@@ -710,18 +821,13 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 {
 	struct dm_verity *v;
 	struct dm_arg_set as;
-	const char *opt_string;
-	unsigned int num, opt_params;
+	unsigned int num;
 	unsigned long long num_ll;
 	int r;
 	int i;
 	sector_t hash_position;
 	char dummy;
 
-	static struct dm_arg _args[] = {
-		{0, 1, "Invalid number of feature args"},
-	};
-
 	v = kzalloc(sizeof(struct dm_verity), GFP_KERNEL);
 	if (!v) {
 		ti->error = "Cannot allocate verity structure";
@@ -730,6 +836,10 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	ti->private = v;
 	v->ti = ti;
 
+	r = verity_fec_ctr_alloc(v);
+	if (r)
+		goto bad;
+
 	if ((dm_table_get_mode(ti->table) & ~FMODE_READ)) {
 		ti->error = "Device must be readonly";
 		r = -EINVAL;
@@ -866,29 +976,9 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		as.argc = argc;
 		as.argv = argv;
 
-		r = dm_read_arg_group(_args, &as, &opt_params, &ti->error);
-		if (r)
+		r = verity_parse_opt_args(&as, v);
+		if (r < 0)
 			goto bad;
-
-		while (opt_params) {
-			opt_params--;
-			opt_string = dm_shift_arg(&as);
-			if (!opt_string) {
-				ti->error = "Not enough feature arguments";
-				r = -EINVAL;
-				goto bad;
-			}
-
-			if (!strcasecmp(opt_string, DM_VERITY_OPT_LOGGING))
-				v->mode = DM_VERITY_MODE_LOGGING;
-			else if (!strcasecmp(opt_string, DM_VERITY_OPT_RESTART))
-				v->mode = DM_VERITY_MODE_RESTART;
-			else {
-				ti->error = "Invalid feature arguments";
-				r = -EINVAL;
-				goto bad;
-			}
-		}
 	}
 
 	v->hash_per_block_bits =
@@ -938,8 +1028,6 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto bad;
 	}
 
-	ti->per_bio_data_size = roundup(sizeof(struct dm_verity_io) + v->shash_descsize + v->digest_size * 2, __alignof__(struct dm_verity_io));
-
 	/* WQ_UNBOUND greatly improves performance when running on ramdisk */
 	v->verify_wq = alloc_workqueue("kverityd", WQ_CPU_INTENSIVE | WQ_MEM_RECLAIM | WQ_UNBOUND, num_online_cpus());
 	if (!v->verify_wq) {
@@ -948,6 +1036,16 @@ static int verity_ctr(struct dm_target *ti, unsigned argc, char **argv)
 		goto bad;
 	}
 
+	ti->per_bio_data_size = sizeof(struct dm_verity_io) +
+				v->shash_descsize + v->digest_size * 2;
+
+	r = verity_fec_ctr(v);
+	if (r)
+		goto bad;
+
+	ti->per_bio_data_size = roundup(ti->per_bio_data_size,
+					__alignof__(struct dm_verity_io));
+
 	return 0;
 
 bad:
@@ -958,7 +1056,7 @@ bad:
 
 static struct target_type verity_target = {
 	.name		= "verity",
-	.version	= {1, 2, 0},
+	.version	= {1, 3, 0},
 	.module		= THIS_MODULE,
 	.ctr		= verity_ctr,
 	.dtr		= verity_dtr,
diff --git a/drivers/md/dm-verity.h b/drivers/md/dm-verity.h
new file mode 100644
index 000000000000..fb419f422d73
--- /dev/null
+++ b/drivers/md/dm-verity.h
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2012 Red Hat, Inc.
+ * Copyright (C) 2015 Google, Inc.
+ *
+ * Author: Mikulas Patocka <mpatocka@redhat.com>
+ *
+ * Based on Chromium dm-verity driver (C) 2011 The Chromium OS Authors
+ *
+ * This file is released under the GPLv2.
+ */
+
+#ifndef DM_VERITY_H
+#define DM_VERITY_H
+
+#include "dm-bufio.h"
+#include <linux/device-mapper.h>
+#include <crypto/hash.h>
+
+#define DM_VERITY_MAX_LEVELS		63
+
+enum verity_mode {
+	DM_VERITY_MODE_EIO,
+	DM_VERITY_MODE_LOGGING,
+	DM_VERITY_MODE_RESTART
+};
+
+enum verity_block_type {
+	DM_VERITY_BLOCK_TYPE_DATA,
+	DM_VERITY_BLOCK_TYPE_METADATA
+};
+
+struct dm_verity_fec;
+
+struct dm_verity {
+	struct dm_dev *data_dev;
+	struct dm_dev *hash_dev;
+	struct dm_target *ti;
+	struct dm_bufio_client *bufio;
+	char *alg_name;
+	struct crypto_shash *tfm;
+	u8 *root_digest;	/* digest of the root block */
+	u8 *salt;		/* salt: its size is salt_size */
+	u8 *zero_digest;	/* digest for a zero block */
+	unsigned salt_size;
+	sector_t data_start;	/* data offset in 512-byte sectors */
+	sector_t hash_start;	/* hash start in blocks */
+	sector_t data_blocks;	/* the number of data blocks */
+	sector_t hash_blocks;	/* the number of hash blocks */
+	unsigned char data_dev_block_bits;	/* log2(data blocksize) */
+	unsigned char hash_dev_block_bits;	/* log2(hash blocksize) */
+	unsigned char hash_per_block_bits;	/* log2(hashes in hash block) */
+	unsigned char levels;	/* the number of tree levels */
+	unsigned char version;
+	unsigned digest_size;	/* digest size for the current hash algorithm */
+	unsigned shash_descsize;/* the size of temporary space for crypto */
+	int hash_failed;	/* set to 1 if hash of any block failed */
+	enum verity_mode mode;	/* mode for handling verification errors */
+	unsigned corrupted_errs;/* Number of errors for corrupted blocks */
+
+	struct workqueue_struct *verify_wq;
+
+	/* starting blocks for each tree level. 0 is the lowest level. */
+	sector_t hash_level_block[DM_VERITY_MAX_LEVELS];
+
+	struct dm_verity_fec *fec;	/* forward error correction */
+};
+
+struct dm_verity_io {
+	struct dm_verity *v;
+
+	/* original value of bio->bi_end_io */
+	bio_end_io_t *orig_bi_end_io;
+
+	sector_t block;
+	unsigned n_blocks;
+
+	struct bvec_iter iter;
+
+	struct work_struct work;
+
+	/*
+	 * Three variably-size fields follow this struct:
+	 *
+	 * u8 hash_desc[v->shash_descsize];
+	 * u8 real_digest[v->digest_size];
+	 * u8 want_digest[v->digest_size];
+	 *
+	 * To access them use: verity_io_hash_desc(), verity_io_real_digest()
+	 * and verity_io_want_digest().
+	 */
+};
+
+static inline struct shash_desc *verity_io_hash_desc(struct dm_verity *v,
+						     struct dm_verity_io *io)
+{
+	return (struct shash_desc *)(io + 1);
+}
+
+static inline u8 *verity_io_real_digest(struct dm_verity *v,
+					struct dm_verity_io *io)
+{
+	return (u8 *)(io + 1) + v->shash_descsize;
+}
+
+static inline u8 *verity_io_want_digest(struct dm_verity *v,
+					struct dm_verity_io *io)
+{
+	return (u8 *)(io + 1) + v->shash_descsize + v->digest_size;
+}
+
+static inline u8 *verity_io_digest_end(struct dm_verity *v,
+				       struct dm_verity_io *io)
+{
+	return verity_io_want_digest(v, io) + v->digest_size;
+}
+
+extern int verity_for_bv_block(struct dm_verity *v, struct dm_verity_io *io,
+			       struct bvec_iter *iter,
+			       int (*process)(struct dm_verity *v,
+					      struct dm_verity_io *io,
+					      u8 *data, size_t len));
+
+extern int verity_hash(struct dm_verity *v, struct shash_desc *desc,
+		       const u8 *data, size_t len, u8 *digest);
+
+extern int verity_hash_for_block(struct dm_verity *v, struct dm_verity_io *io,
+				 sector_t block, u8 *digest, bool *is_zero);
+
+#endif /* DM_VERITY_H */
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 5df40480228b..dd834927bc66 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -1191,6 +1191,8 @@ static void dm_unprep_request(struct request *rq)
 
 	if (clone)
 		free_rq_clone(clone);
+	else if (!tio->md->queue->mq_ops)
+		free_rq_tio(tio);
 }
 
 /*
diff --git a/drivers/md/faulty.c b/drivers/md/faulty.c
index 4a8e15058e8b..685aa2d77e25 100644
--- a/drivers/md/faulty.c
+++ b/drivers/md/faulty.c
@@ -170,7 +170,7 @@ static void add_sector(struct faulty_conf *conf, sector_t start, int mode)
 		conf->nfaults = n+1;
 }
 
-static void make_request(struct mddev *mddev, struct bio *bio)
+static void faulty_make_request(struct mddev *mddev, struct bio *bio)
 {
 	struct faulty_conf *conf = mddev->private;
 	int failit = 0;
@@ -226,7 +226,7 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 	generic_make_request(bio);
 }
 
-static void status(struct seq_file *seq, struct mddev *mddev)
+static void faulty_status(struct seq_file *seq, struct mddev *mddev)
 {
 	struct faulty_conf *conf = mddev->private;
 	int n;
@@ -259,7 +259,7 @@ static void status(struct seq_file *seq, struct mddev *mddev)
 }
 
 
-static int reshape(struct mddev *mddev)
+static int faulty_reshape(struct mddev *mddev)
 {
 	int mode = mddev->new_layout & ModeMask;
 	int count = mddev->new_layout >> ModeShift;
@@ -299,7 +299,7 @@ static sector_t faulty_size(struct mddev *mddev, sector_t sectors, int raid_disk
 	return sectors;
 }
 
-static int run(struct mddev *mddev)
+static int faulty_run(struct mddev *mddev)
 {
 	struct md_rdev *rdev;
 	int i;
@@ -327,7 +327,7 @@ static int run(struct mddev *mddev)
 	md_set_array_sectors(mddev, faulty_size(mddev, 0, 0));
 	mddev->private = conf;
 
-	reshape(mddev);
+	faulty_reshape(mddev);
 
 	return 0;
 }
@@ -344,11 +344,11 @@ static struct md_personality faulty_personality =
 	.name		= "faulty",
 	.level		= LEVEL_FAULTY,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= faulty_make_request,
+	.run		= faulty_run,
 	.free		= faulty_free,
-	.status		= status,
-	.check_reshape	= reshape,
+	.status		= faulty_status,
+	.check_reshape	= faulty_reshape,
 	.size		= faulty_size,
 };
 
diff --git a/drivers/md/md-cluster.c b/drivers/md/md-cluster.c
index d6a1126d85ce..dd97d4245822 100644
--- a/drivers/md/md-cluster.c
+++ b/drivers/md/md-cluster.c
@@ -48,13 +48,29 @@ struct resync_info {
 #define		MD_CLUSTER_SUSPEND_READ_BALANCING	2
 #define		MD_CLUSTER_BEGIN_JOIN_CLUSTER		3
 
+/* Lock the send communication. This is done through
+ * bit manipulation as opposed to a mutex in order to
+ * accomodate lock and hold. See next comment.
+ */
+#define		MD_CLUSTER_SEND_LOCK			4
+/* If cluster operations (such as adding a disk) must lock the
+ * communication channel, so as to perform extra operations
+ * (update metadata) and no other operation is allowed on the
+ * MD. Token needs to be locked and held until the operation
+ * completes witha md_update_sb(), which would eventually release
+ * the lock.
+ */
+#define		MD_CLUSTER_SEND_LOCKED_ALREADY		5
+
 
 struct md_cluster_info {
 	/* dlm lock space and resources for clustered raid. */
 	dlm_lockspace_t *lockspace;
 	int slot_number;
 	struct completion completion;
+	struct mutex recv_mutex;
 	struct dlm_lock_resource *bitmap_lockres;
+	struct dlm_lock_resource **other_bitmap_lockres;
 	struct dlm_lock_resource *resync_lockres;
 	struct list_head suspend_list;
 	spinlock_t suspend_lock;
@@ -67,6 +83,7 @@ struct md_cluster_info {
 	struct dlm_lock_resource *no_new_dev_lockres;
 	struct md_thread *recv_thread;
 	struct completion newdisk_completion;
+	wait_queue_head_t wait;
 	unsigned long state;
 };
 
@@ -276,6 +293,7 @@ static void recover_bitmaps(struct md_thread *thread)
 dlm_unlock:
 		dlm_unlock_sync(bm_lockres);
 clear_bit:
+		lockres_free(bm_lockres);
 		clear_bit(slot, &cinfo->recovery_map);
 	}
 }
@@ -431,8 +449,10 @@ static void process_add_new_disk(struct mddev *mddev, struct cluster_msg *cmsg)
 static void process_metadata_update(struct mddev *mddev, struct cluster_msg *msg)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
-	md_reload_sb(mddev, le32_to_cpu(msg->raid_slot));
+	mddev->good_device_nr = le32_to_cpu(msg->raid_slot);
+	set_bit(MD_RELOAD_SB, &mddev->flags);
 	dlm_lock_sync(cinfo->no_new_dev_lockres, DLM_LOCK_CR);
+	md_wakeup_thread(mddev->thread);
 }
 
 static void process_remove_disk(struct mddev *mddev, struct cluster_msg *msg)
@@ -440,8 +460,11 @@ 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));
 
-	if (rdev)
-		md_kick_rdev_from_array(rdev);
+	if (rdev) {
+		set_bit(ClusterRemove, &rdev->flags);
+		set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
+		md_wakeup_thread(mddev->thread);
+	}
 	else
 		pr_warn("%s: %d Could not find disk(%d) to REMOVE\n",
 			__func__, __LINE__, le32_to_cpu(msg->raid_slot));
@@ -502,9 +525,11 @@ static void recv_daemon(struct md_thread *thread)
 	struct cluster_msg msg;
 	int ret;
 
+	mutex_lock(&cinfo->recv_mutex);
 	/*get CR on Message*/
 	if (dlm_lock_sync(message_lockres, DLM_LOCK_CR)) {
 		pr_err("md/raid1:failed to get CR on MESSAGE\n");
+		mutex_unlock(&cinfo->recv_mutex);
 		return;
 	}
 
@@ -528,33 +553,45 @@ static void recv_daemon(struct md_thread *thread)
 	ret = dlm_unlock_sync(message_lockres);
 	if (unlikely(ret != 0))
 		pr_info("unlock msg failed return %d\n", ret);
+	mutex_unlock(&cinfo->recv_mutex);
 }
 
-/* lock_comm()
+/* lock_token()
  * Takes the lock on the TOKEN lock resource so no other
  * node can communicate while the operation is underway.
- * If called again, and the TOKEN lock is alread in EX mode
- * return success. However, care must be taken that unlock_comm()
- * is called only once.
  */
-static int lock_comm(struct md_cluster_info *cinfo)
+static int lock_token(struct md_cluster_info *cinfo)
 {
 	int error;
 
-	if (cinfo->token_lockres->mode == DLM_LOCK_EX)
-		return 0;
-
 	error = dlm_lock_sync(cinfo->token_lockres, DLM_LOCK_EX);
 	if (error)
 		pr_err("md-cluster(%s:%d): failed to get EX on TOKEN (%d)\n",
 				__func__, __LINE__, error);
+
+	/* Lock the receive sequence */
+	mutex_lock(&cinfo->recv_mutex);
 	return error;
 }
 
+/* lock_comm()
+ * Sets the MD_CLUSTER_SEND_LOCK bit to lock the send channel.
+ */
+static int lock_comm(struct md_cluster_info *cinfo)
+{
+	wait_event(cinfo->wait,
+		   !test_and_set_bit(MD_CLUSTER_SEND_LOCK, &cinfo->state));
+
+	return lock_token(cinfo);
+}
+
 static void unlock_comm(struct md_cluster_info *cinfo)
 {
 	WARN_ON(cinfo->token_lockres->mode != DLM_LOCK_EX);
+	mutex_unlock(&cinfo->recv_mutex);
 	dlm_unlock_sync(cinfo->token_lockres);
+	clear_bit(MD_CLUSTER_SEND_LOCK, &cinfo->state);
+	wake_up(&cinfo->wait);
 }
 
 /* __sendmsg()
@@ -646,8 +683,10 @@ static int gather_all_resync_info(struct mddev *mddev, int total_slots)
 		bm_lockres = lockres_init(mddev, str, NULL, 1);
 		if (!bm_lockres)
 			return -ENOMEM;
-		if (i == (cinfo->slot_number - 1))
+		if (i == (cinfo->slot_number - 1)) {
+			lockres_free(bm_lockres);
 			continue;
+		}
 
 		bm_lockres->flags |= DLM_LKF_NOQUEUE;
 		ret = dlm_lock_sync(bm_lockres, DLM_LOCK_PW);
@@ -707,6 +746,8 @@ static int join(struct mddev *mddev, int nodes)
 	spin_lock_init(&cinfo->suspend_lock);
 	init_completion(&cinfo->completion);
 	set_bit(MD_CLUSTER_BEGIN_JOIN_CLUSTER, &cinfo->state);
+	init_waitqueue_head(&cinfo->wait);
+	mutex_init(&cinfo->recv_mutex);
 
 	mddev->cluster_info = cinfo;
 
@@ -800,6 +841,7 @@ static void resync_bitmap(struct mddev *mddev)
 			__func__, __LINE__, err);
 }
 
+static void unlock_all_bitmaps(struct mddev *mddev);
 static int leave(struct mddev *mddev)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
@@ -819,7 +861,9 @@ static int leave(struct mddev *mddev)
 	lockres_free(cinfo->token_lockres);
 	lockres_free(cinfo->ack_lockres);
 	lockres_free(cinfo->no_new_dev_lockres);
+	lockres_free(cinfo->resync_lockres);
 	lockres_free(cinfo->bitmap_lockres);
+	unlock_all_bitmaps(mddev);
 	dlm_release_lockspace(cinfo->lockspace, 2);
 	return 0;
 }
@@ -835,9 +879,25 @@ static int slot_number(struct mddev *mddev)
 	return cinfo->slot_number - 1;
 }
 
+/*
+ * Check if the communication is already locked, else lock the communication
+ * channel.
+ * If it is already locked, token is in EX mode, and hence lock_token()
+ * should not be called.
+ */
 static int metadata_update_start(struct mddev *mddev)
 {
-	return lock_comm(mddev->cluster_info);
+	struct md_cluster_info *cinfo = mddev->cluster_info;
+
+	wait_event(cinfo->wait,
+		   !test_and_set_bit(MD_CLUSTER_SEND_LOCK, &cinfo->state) ||
+		   test_and_clear_bit(MD_CLUSTER_SEND_LOCKED_ALREADY, &cinfo->state));
+
+	/* If token is already locked, return 0 */
+	if (cinfo->token_lockres->mode == DLM_LOCK_EX)
+		return 0;
+
+	return lock_token(cinfo);
 }
 
 static int metadata_update_finish(struct mddev *mddev)
@@ -862,6 +922,7 @@ static int metadata_update_finish(struct mddev *mddev)
 		ret = __sendmsg(cinfo, &cmsg);
 	} else
 		pr_warn("md-cluster: No good device id found to send\n");
+	clear_bit(MD_CLUSTER_SEND_LOCKED_ALREADY, &cinfo->state);
 	unlock_comm(cinfo);
 	return ret;
 }
@@ -869,6 +930,7 @@ static int metadata_update_finish(struct mddev *mddev)
 static void metadata_update_cancel(struct mddev *mddev)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
+	clear_bit(MD_CLUSTER_SEND_LOCKED_ALREADY, &cinfo->state);
 	unlock_comm(cinfo);
 }
 
@@ -882,8 +944,16 @@ static int resync_start(struct mddev *mddev)
 static int resync_info_update(struct mddev *mddev, sector_t lo, sector_t hi)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
+	struct resync_info ri;
 	struct cluster_msg cmsg = {0};
 
+	/* do not send zero again, if we have sent before */
+	if (hi == 0) {
+		memcpy(&ri, cinfo->bitmap_lockres->lksb.sb_lvbptr, sizeof(struct resync_info));
+		if (le64_to_cpu(ri.hi) == 0)
+			return 0;
+	}
+
 	add_resync_info(cinfo->bitmap_lockres, lo, hi);
 	/* Re-acquire the lock to refresh LVB */
 	dlm_lock_sync(cinfo->bitmap_lockres, DLM_LOCK_PW);
@@ -954,14 +1024,30 @@ static int add_new_disk(struct mddev *mddev, struct md_rdev *rdev)
 		ret = -ENOENT;
 	if (ret)
 		unlock_comm(cinfo);
-	else
+	else {
 		dlm_lock_sync(cinfo->no_new_dev_lockres, DLM_LOCK_CR);
+		/* Since MD_CHANGE_DEVS will be set in add_bound_rdev which
+		 * will run soon after add_new_disk, the below path will be
+		 * invoked:
+		 *   md_wakeup_thread(mddev->thread)
+		 *	-> conf->thread (raid1d)
+		 *	-> md_check_recovery -> md_update_sb
+		 *	-> metadata_update_start/finish
+		 * MD_CLUSTER_SEND_LOCKED_ALREADY will be cleared eventually.
+		 *
+		 * For other failure cases, metadata_update_cancel and
+		 * add_new_disk_cancel also clear below bit as well.
+		 * */
+		set_bit(MD_CLUSTER_SEND_LOCKED_ALREADY, &cinfo->state);
+		wake_up(&cinfo->wait);
+	}
 	return ret;
 }
 
 static void add_new_disk_cancel(struct mddev *mddev)
 {
 	struct md_cluster_info *cinfo = mddev->cluster_info;
+	clear_bit(MD_CLUSTER_SEND_LOCKED_ALREADY, &cinfo->state);
 	unlock_comm(cinfo);
 }
 
@@ -986,7 +1072,59 @@ static int remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 	struct md_cluster_info *cinfo = mddev->cluster_info;
 	cmsg.type = cpu_to_le32(REMOVE);
 	cmsg.raid_slot = cpu_to_le32(rdev->desc_nr);
-	return __sendmsg(cinfo, &cmsg);
+	return sendmsg(cinfo, &cmsg);
+}
+
+static int lock_all_bitmaps(struct mddev *mddev)
+{
+	int slot, my_slot, ret, held = 1, i = 0;
+	char str[64];
+	struct md_cluster_info *cinfo = mddev->cluster_info;
+
+	cinfo->other_bitmap_lockres = kzalloc((mddev->bitmap_info.nodes - 1) *
+					     sizeof(struct dlm_lock_resource *),
+					     GFP_KERNEL);
+	if (!cinfo->other_bitmap_lockres) {
+		pr_err("md: can't alloc mem for other bitmap locks\n");
+		return 0;
+	}
+
+	my_slot = slot_number(mddev);
+	for (slot = 0; slot < mddev->bitmap_info.nodes; slot++) {
+		if (slot == my_slot)
+			continue;
+
+		memset(str, '\0', 64);
+		snprintf(str, 64, "bitmap%04d", slot);
+		cinfo->other_bitmap_lockres[i] = lockres_init(mddev, str, NULL, 1);
+		if (!cinfo->other_bitmap_lockres[i])
+			return -ENOMEM;
+
+		cinfo->other_bitmap_lockres[i]->flags |= DLM_LKF_NOQUEUE;
+		ret = dlm_lock_sync(cinfo->other_bitmap_lockres[i], DLM_LOCK_PW);
+		if (ret)
+			held = -1;
+		i++;
+	}
+
+	return held;
+}
+
+static void unlock_all_bitmaps(struct mddev *mddev)
+{
+	struct md_cluster_info *cinfo = mddev->cluster_info;
+	int i;
+
+	/* release other node's bitmap lock if they are existed */
+	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]);
+			}
+		}
+		kfree(cinfo->other_bitmap_lockres);
+	}
 }
 
 static int gather_bitmaps(struct md_rdev *rdev)
@@ -1034,6 +1172,8 @@ static struct md_cluster_operations cluster_ops = {
 	.new_disk_ack = new_disk_ack,
 	.remove_disk = remove_disk,
 	.gather_bitmaps = gather_bitmaps,
+	.lock_all_bitmaps = lock_all_bitmaps,
+	.unlock_all_bitmaps = unlock_all_bitmaps,
 };
 
 static int __init cluster_init(void)
diff --git a/drivers/md/md-cluster.h b/drivers/md/md-cluster.h
index e75ea2613184..45ce6c97d8bd 100644
--- a/drivers/md/md-cluster.h
+++ b/drivers/md/md-cluster.h
@@ -24,6 +24,8 @@ struct md_cluster_operations {
 	int (*new_disk_ack)(struct mddev *mddev, bool ack);
 	int (*remove_disk)(struct mddev *mddev, struct md_rdev *rdev);
 	int (*gather_bitmaps)(struct md_rdev *rdev);
+	int (*lock_all_bitmaps)(struct mddev *mddev);
+	void (*unlock_all_bitmaps)(struct mddev *mddev);
 };
 
 #endif /* _MD_CLUSTER_H */
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 61aacab424cf..e55e6cf9ec17 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -34,6 +34,7 @@
 
 #include <linux/kthread.h>
 #include <linux/blkdev.h>
+#include <linux/badblocks.h>
 #include <linux/sysctl.h>
 #include <linux/seq_file.h>
 #include <linux/fs.h>
@@ -205,15 +206,6 @@ void md_new_event(struct mddev *mddev)
 }
 EXPORT_SYMBOL_GPL(md_new_event);
 
-/* Alternate version that can be called from interrupts
- * when calling sysfs_notify isn't needed.
- */
-static void md_new_event_inintr(struct mddev *mddev)
-{
-	atomic_inc(&md_event_count);
-	wake_up(&md_event_waiters);
-}
-
 /*
  * Enables to iterate over all existing md arrays
  * all_mddevs_lock protects this list.
@@ -259,8 +251,7 @@ static blk_qc_t md_make_request(struct request_queue *q, struct bio *bio)
 
 	blk_queue_split(q, &bio, q->bio_split);
 
-	if (mddev == NULL || mddev->pers == NULL
-	    || !mddev->ready) {
+	if (mddev == NULL || mddev->pers == NULL) {
 		bio_io_error(bio);
 		return BLK_QC_T_NONE;
 	}
@@ -710,8 +701,7 @@ void md_rdev_clear(struct md_rdev *rdev)
 		put_page(rdev->bb_page);
 		rdev->bb_page = NULL;
 	}
-	kfree(rdev->badblocks.page);
-	rdev->badblocks.page = NULL;
+	badblocks_exit(&rdev->badblocks);
 }
 EXPORT_SYMBOL_GPL(md_rdev_clear);
 
@@ -1026,8 +1016,9 @@ static int super_90_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor
 	 * (not needed for Linear and RAID0 as metadata doesn't
 	 * record this size)
 	 */
-	if (rdev->sectors >= (2ULL << 32) && sb->level >= 1)
-		rdev->sectors = (2ULL << 32) - 2;
+	if (IS_ENABLED(CONFIG_LBDAF) && (u64)rdev->sectors >= (2ULL << 32) &&
+	    sb->level >= 1)
+		rdev->sectors = (sector_t)(2ULL << 32) - 2;
 
 	if (rdev->sectors < ((sector_t)sb->size) * 2 && sb->level >= 1)
 		/* "this cannot possibly happen" ... */
@@ -1199,13 +1190,13 @@ static void super_90_sync(struct mddev *mddev, struct md_rdev *rdev)
 	memcpy(&sb->set_uuid2, mddev->uuid+8, 4);
 	memcpy(&sb->set_uuid3, mddev->uuid+12,4);
 
-	sb->ctime = mddev->ctime;
+	sb->ctime = clamp_t(time64_t, mddev->ctime, 0, U32_MAX);
 	sb->level = mddev->level;
 	sb->size = mddev->dev_sectors / 2;
 	sb->raid_disks = mddev->raid_disks;
 	sb->md_minor = mddev->md_minor;
 	sb->not_persistent = 0;
-	sb->utime = mddev->utime;
+	sb->utime = clamp_t(time64_t, mddev->utime, 0, U32_MAX);
 	sb->state = 0;
 	sb->events_hi = (mddev->events>>32);
 	sb->events_lo = (u32)mddev->events;
@@ -1320,8 +1311,9 @@ super_90_rdev_size_change(struct md_rdev *rdev, sector_t num_sectors)
 	/* Limit to 4TB as metadata cannot record more than that.
 	 * 4TB == 2^32 KB, or 2*2^32 sectors.
 	 */
-	if (num_sectors >= (2ULL << 32) && rdev->mddev->level >= 1)
-		num_sectors = (2ULL << 32) - 2;
+	if (IS_ENABLED(CONFIG_LBDAF) && (u64)num_sectors >= (2ULL << 32) &&
+	    rdev->mddev->level >= 1)
+		num_sectors = (sector_t)(2ULL << 32) - 2;
 	md_super_write(rdev->mddev, rdev, rdev->sb_start, rdev->sb_size,
 		       rdev->sb_page);
 	md_super_wait(rdev->mddev);
@@ -1361,8 +1353,6 @@ static __le32 calc_sb_1_csum(struct mdp_superblock_1 *sb)
 	return cpu_to_le32(csum);
 }
 
-static int md_set_badblocks(struct badblocks *bb, sector_t s, int sectors,
-			    int acknowledged);
 static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_version)
 {
 	struct mdp_superblock_1 *sb;
@@ -1487,8 +1477,7 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 			count <<= sb->bblog_shift;
 			if (bb + 1 == 0)
 				break;
-			if (md_set_badblocks(&rdev->badblocks,
-					     sector, count, 1) == 0)
+			if (badblocks_set(&rdev->badblocks, sector, count, 1))
 				return -EINVAL;
 		}
 	} else if (sb->bblog_offset != 0)
@@ -1545,8 +1534,8 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 		mddev->patch_version = 0;
 		mddev->external = 0;
 		mddev->chunk_sectors = le32_to_cpu(sb->chunksize);
-		mddev->ctime = le64_to_cpu(sb->ctime) & ((1ULL << 32)-1);
-		mddev->utime = le64_to_cpu(sb->utime) & ((1ULL << 32)-1);
+		mddev->ctime = le64_to_cpu(sb->ctime);
+		mddev->utime = le64_to_cpu(sb->utime);
 		mddev->level = le32_to_cpu(sb->level);
 		mddev->clevel[0] = 0;
 		mddev->layout = le32_to_cpu(sb->layout);
@@ -1605,6 +1594,11 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 			mddev->new_chunk_sectors = mddev->chunk_sectors;
 		}
 
+		if (le32_to_cpu(sb->feature_map) & MD_FEATURE_JOURNAL) {
+			set_bit(MD_HAS_JOURNAL, &mddev->flags);
+			if (mddev->recovery_cp == MaxSector)
+				set_bit(MD_JOURNAL_CLEAN, &mddev->flags);
+		}
 	} else if (mddev->pers == NULL) {
 		/* Insist of good event counter while assembling, except for
 		 * spares (which don't need an event count) */
@@ -1651,8 +1645,6 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 			}
 			set_bit(Journal, &rdev->flags);
 			rdev->journal_tail = le64_to_cpu(sb->journal_tail);
-			if (mddev->recovery_cp == MaxSector)
-				set_bit(MD_JOURNAL_CLEAN, &mddev->flags);
 			rdev->raid_disk = 0;
 			break;
 		default:
@@ -1672,8 +1664,6 @@ static int super_1_validate(struct mddev *mddev, struct md_rdev *rdev)
 			set_bit(WriteMostly, &rdev->flags);
 		if (le32_to_cpu(sb->feature_map) & MD_FEATURE_REPLACEMENT)
 			set_bit(Replacement, &rdev->flags);
-		if (le32_to_cpu(sb->feature_map) & MD_FEATURE_JOURNAL)
-			set_bit(MD_HAS_JOURNAL, &mddev->flags);
 	} else /* MULTIPATH are always insync */
 		set_bit(In_sync, &rdev->flags);
 
@@ -2017,28 +2007,32 @@ int md_integrity_register(struct mddev *mddev)
 }
 EXPORT_SYMBOL(md_integrity_register);
 
-/* Disable data integrity if non-capable/non-matching disk is being added */
-void md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev)
+/*
+ * Attempt to add an rdev, but only if it is consistent with the current
+ * integrity profile
+ */
+int md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev)
 {
 	struct blk_integrity *bi_rdev;
 	struct blk_integrity *bi_mddev;
+	char name[BDEVNAME_SIZE];
 
 	if (!mddev->gendisk)
-		return;
+		return 0;
 
 	bi_rdev = bdev_get_integrity(rdev->bdev);
 	bi_mddev = blk_get_integrity(mddev->gendisk);
 
 	if (!bi_mddev) /* nothing to do */
-		return;
-	if (rdev->raid_disk < 0) /* skip spares */
-		return;
-	if (bi_rdev && blk_integrity_compare(mddev->gendisk,
-					     rdev->bdev->bd_disk) >= 0)
-		return;
-	WARN_ON_ONCE(!mddev->suspended);
-	printk(KERN_NOTICE "disabling data integrity on %s\n", mdname(mddev));
-	blk_integrity_unregister(mddev->gendisk);
+		return 0;
+
+	if (blk_integrity_compare(mddev->gendisk, rdev->bdev->bd_disk) != 0) {
+		printk(KERN_NOTICE "%s: incompatible integrity profile for %s\n",
+				mdname(mddev), bdevname(rdev->bdev, name));
+		return -ENXIO;
+	}
+
+	return 0;
 }
 EXPORT_SYMBOL(md_integrity_add_rdev);
 
@@ -2053,8 +2047,9 @@ static int bind_rdev_to_array(struct md_rdev *rdev, struct mddev *mddev)
 		return -EEXIST;
 
 	/* make sure rdev->sectors exceeds mddev->dev_sectors */
-	if (rdev->sectors && (mddev->dev_sectors == 0 ||
-			rdev->sectors < mddev->dev_sectors)) {
+	if (!test_bit(Journal, &rdev->flags) &&
+	    rdev->sectors &&
+	    (mddev->dev_sectors == 0 || rdev->sectors < mddev->dev_sectors)) {
 		if (mddev->pers) {
 			/* Cannot change size, so fail
 			 * If mddev->level <= 0, then we don't care
@@ -2085,7 +2080,8 @@ static int bind_rdev_to_array(struct md_rdev *rdev, struct mddev *mddev)
 		}
 	}
 	rcu_read_unlock();
-	if (mddev->max_disks && rdev->desc_nr >= mddev->max_disks) {
+	if (!test_bit(Journal, &rdev->flags) &&
+	    mddev->max_disks && rdev->desc_nr >= mddev->max_disks) {
 		printk(KERN_WARNING "md: %s: array is limited to %d devices\n",
 		       mdname(mddev), mddev->max_disks);
 		return -EBUSY;
@@ -2320,7 +2316,7 @@ repeat:
 			rdev_for_each(rdev, mddev) {
 				if (rdev->badblocks.changed) {
 					rdev->badblocks.changed = 0;
-					md_ack_all_badblocks(&rdev->badblocks);
+					ack_all_badblocks(&rdev->badblocks);
 					md_error(mddev, rdev);
 				}
 				clear_bit(Blocked, &rdev->flags);
@@ -2334,7 +2330,7 @@ repeat:
 
 	spin_lock(&mddev->lock);
 
-	mddev->utime = get_seconds();
+	mddev->utime = ktime_get_real_seconds();
 
 	if (test_and_clear_bit(MD_CHANGE_DEVS, &mddev->flags))
 		force_change = 1;
@@ -2446,7 +2442,7 @@ repeat:
 			clear_bit(Blocked, &rdev->flags);
 
 		if (any_badblocks_changed)
-			md_ack_all_badblocks(&rdev->badblocks);
+			ack_all_badblocks(&rdev->badblocks);
 		clear_bit(BlockedBadBlocks, &rdev->flags);
 		wake_up(&rdev->blocked_wait);
 	}
@@ -2460,15 +2456,20 @@ static int add_bound_rdev(struct md_rdev *rdev)
 {
 	struct mddev *mddev = rdev->mddev;
 	int err = 0;
+	bool add_journal = test_bit(Journal, &rdev->flags);
 
-	if (!mddev->pers->hot_remove_disk) {
+	if (!mddev->pers->hot_remove_disk || add_journal) {
 		/* If there is hot_add_disk but no hot_remove_disk
 		 * then added disks for geometry changes,
 		 * and should be added immediately.
 		 */
 		super_types[mddev->major_version].
 			validate_super(mddev, rdev);
+		if (add_journal)
+			mddev_suspend(mddev);
 		err = mddev->pers->hot_add_disk(mddev, rdev);
+		if (add_journal)
+			mddev_resume(mddev);
 		if (err) {
 			unbind_rdev_from_array(rdev);
 			export_rdev(rdev);
@@ -3054,11 +3055,17 @@ static ssize_t recovery_start_store(struct md_rdev *rdev, const char *buf, size_
 static struct rdev_sysfs_entry rdev_recovery_start =
 __ATTR(recovery_start, S_IRUGO|S_IWUSR, recovery_start_show, recovery_start_store);
 
-static ssize_t
-badblocks_show(struct badblocks *bb, char *page, int unack);
-static ssize_t
-badblocks_store(struct badblocks *bb, const char *page, size_t len, int unack);
-
+/* sysfs access to bad-blocks list.
+ * We present two files.
+ * 'bad-blocks' lists sector numbers and lengths of ranges that
+ *    are recorded as bad.  The list is truncated to fit within
+ *    the one-page limit of sysfs.
+ *    Writing "sector length" to this file adds an acknowledged
+ *    bad block list.
+ * 'unacknowledged-bad-blocks' lists bad blocks that have not yet
+ *    been acknowledged.  Writing to this file adds bad blocks
+ *    without acknowledging them.  This is largely for testing.
+ */
 static ssize_t bb_show(struct md_rdev *rdev, char *page)
 {
 	return badblocks_show(&rdev->badblocks, page, 0);
@@ -3173,14 +3180,7 @@ int md_rdev_init(struct md_rdev *rdev)
 	 * This reserves the space even on arrays where it cannot
 	 * be used - I wonder if that matters
 	 */
-	rdev->badblocks.count = 0;
-	rdev->badblocks.shift = -1; /* disabled until explicitly enabled */
-	rdev->badblocks.page = kmalloc(PAGE_SIZE, GFP_KERNEL);
-	seqlock_init(&rdev->badblocks.lock);
-	if (rdev->badblocks.page == NULL)
-		return -ENOMEM;
-
-	return 0;
+	return badblocks_init(&rdev->badblocks, 0);
 }
 EXPORT_SYMBOL_GPL(md_rdev_init);
 /*
@@ -5303,7 +5303,6 @@ int md_run(struct mddev *mddev)
 	smp_wmb();
 	spin_lock(&mddev->lock);
 	mddev->pers = pers;
-	mddev->ready = 1;
 	spin_unlock(&mddev->lock);
 	rdev_for_each(rdev, mddev)
 		if (rdev->raid_disk >= 0)
@@ -5503,7 +5502,6 @@ static void __md_stop(struct mddev *mddev)
 	/* Ensure ->event_work is done */
 	flush_workqueue(md_misc_wq);
 	spin_lock(&mddev->lock);
-	mddev->ready = 0;
 	mddev->pers = NULL;
 	spin_unlock(&mddev->lock);
 	pers->free(mddev, mddev->private);
@@ -5841,7 +5839,7 @@ static int get_array_info(struct mddev *mddev, void __user *arg)
 	info.major_version = mddev->major_version;
 	info.minor_version = mddev->minor_version;
 	info.patch_version = MD_PATCHLEVEL_VERSION;
-	info.ctime         = mddev->ctime;
+	info.ctime         = clamp_t(time64_t, mddev->ctime, 0, U32_MAX);
 	info.level         = mddev->level;
 	info.size          = mddev->dev_sectors / 2;
 	if (info.size != mddev->dev_sectors / 2) /* overflow */
@@ -5851,7 +5849,7 @@ static int get_array_info(struct mddev *mddev, void __user *arg)
 	info.md_minor      = mddev->md_minor;
 	info.not_persistent= !mddev->persistent;
 
-	info.utime         = mddev->utime;
+	info.utime         = clamp_t(time64_t, mddev->utime, 0, U32_MAX);
 	info.state         = 0;
 	if (mddev->in_sync)
 		info.state = (1<<MD_SB_CLEAN);
@@ -6042,8 +6040,23 @@ static int add_new_disk(struct mddev *mddev, mdu_disk_info_t *info)
 		else
 			clear_bit(WriteMostly, &rdev->flags);
 
-		if (info->state & (1<<MD_DISK_JOURNAL))
+		if (info->state & (1<<MD_DISK_JOURNAL)) {
+			struct md_rdev *rdev2;
+			bool has_journal = false;
+
+			/* make sure no existing journal disk */
+			rdev_for_each(rdev2, mddev) {
+				if (test_bit(Journal, &rdev2->flags)) {
+					has_journal = true;
+					break;
+				}
+			}
+			if (has_journal) {
+				export_rdev(rdev);
+				return -EBUSY;
+			}
 			set_bit(Journal, &rdev->flags);
+		}
 		/*
 		 * check whether the device shows up in other nodes
 		 */
@@ -6134,15 +6147,11 @@ static int hot_remove_disk(struct mddev *mddev, dev_t dev)
 {
 	char b[BDEVNAME_SIZE];
 	struct md_rdev *rdev;
-	int ret = -1;
 
 	rdev = find_rdev(mddev, dev);
 	if (!rdev)
 		return -ENXIO;
 
-	if (mddev_is_clustered(mddev))
-		ret = md_cluster_ops->metadata_update_start(mddev);
-
 	if (rdev->raid_disk < 0)
 		goto kick_rdev;
 
@@ -6153,7 +6162,7 @@ static int hot_remove_disk(struct mddev *mddev, dev_t dev)
 		goto busy;
 
 kick_rdev:
-	if (mddev_is_clustered(mddev) && ret == 0)
+	if (mddev_is_clustered(mddev))
 		md_cluster_ops->remove_disk(mddev, rdev);
 
 	md_kick_rdev_from_array(rdev);
@@ -6162,9 +6171,6 @@ kick_rdev:
 
 	return 0;
 busy:
-	if (mddev_is_clustered(mddev) && ret == 0)
-		md_cluster_ops->metadata_update_cancel(mddev);
-
 	printk(KERN_WARNING "md: cannot remove active disk %s from %s ...\n",
 		bdevname(rdev->bdev,b), mdname(mddev));
 	return -EBUSY;
@@ -6358,13 +6364,13 @@ static int set_array_info(struct mddev *mddev, mdu_array_info_t *info)
 		/* ensure mddev_put doesn't delete this now that there
 		 * is some minimal configuration.
 		 */
-		mddev->ctime         = get_seconds();
+		mddev->ctime         = ktime_get_real_seconds();
 		return 0;
 	}
 	mddev->major_version = MD_MAJOR_VERSION;
 	mddev->minor_version = MD_MINOR_VERSION;
 	mddev->patch_version = MD_PATCHLEVEL_VERSION;
-	mddev->ctime         = get_seconds();
+	mddev->ctime         = ktime_get_real_seconds();
 
 	mddev->level         = info->level;
 	mddev->clevel[0]     = 0;
@@ -6606,6 +6612,19 @@ static int update_array_info(struct mddev *mddev, mdu_array_info_t *info)
 				rv = -EINVAL;
 				goto err;
 			}
+			if (mddev->bitmap_info.nodes) {
+				/* hold PW on all the bitmap lock */
+				if (md_cluster_ops->lock_all_bitmaps(mddev) <= 0) {
+					printk("md: can't change bitmap to none since the"
+					       " array is in use by more than one node\n");
+					rv = -EPERM;
+					md_cluster_ops->unlock_all_bitmaps(mddev);
+					goto err;
+				}
+
+				mddev->bitmap_info.nodes = 0;
+				md_cluster_ops->leave(mddev);
+			}
 			mddev->pers->quiesce(mddev, 1);
 			bitmap_destroy(mddev);
 			mddev->pers->quiesce(mddev, 0);
@@ -7184,7 +7203,7 @@ void md_error(struct mddev *mddev, struct md_rdev *rdev)
 	md_wakeup_thread(mddev->thread);
 	if (mddev->event_work.func)
 		queue_work(md_misc_wq, &mddev->event_work);
-	md_new_event_inintr(mddev);
+	md_new_event(mddev);
 }
 EXPORT_SYMBOL(md_error);
 
@@ -7708,7 +7727,7 @@ EXPORT_SYMBOL(md_write_end);
  * attempting a GFP_KERNEL allocation while holding the mddev lock.
  * Must be called with mddev_lock held.
  *
- * In the ->external case MD_CHANGE_CLEAN can not be cleared until mddev->lock
+ * In the ->external case MD_CHANGE_PENDING can not be cleared until mddev->lock
  * is dropped, so return -EAGAIN after notifying userspace.
  */
 int md_allow_write(struct mddev *mddev)
@@ -8173,19 +8192,20 @@ static int remove_and_add_spares(struct mddev *mddev,
 			continue;
 		if (test_bit(Faulty, &rdev->flags))
 			continue;
-		if (test_bit(Journal, &rdev->flags))
-			continue;
-		if (mddev->ro &&
-		    ! (rdev->saved_raid_disk >= 0 &&
-		       !test_bit(Bitmap_sync, &rdev->flags)))
-			continue;
+		if (!test_bit(Journal, &rdev->flags)) {
+			if (mddev->ro &&
+			    ! (rdev->saved_raid_disk >= 0 &&
+			       !test_bit(Bitmap_sync, &rdev->flags)))
+				continue;
 
-		rdev->recovery_offset = 0;
+			rdev->recovery_offset = 0;
+		}
 		if (mddev->pers->
 		    hot_add_disk(mddev, rdev) == 0) {
 			if (sysfs_link_rdev(mddev, rdev))
 				/* failure here is OK */;
-			spares++;
+			if (!test_bit(Journal, &rdev->flags))
+				spares++;
 			md_new_event(mddev);
 			set_bit(MD_CHANGE_DEVS, &mddev->flags);
 		}
@@ -8280,6 +8300,7 @@ void md_check_recovery(struct mddev *mddev)
 		(mddev->flags & MD_UPDATE_SB_FLAGS & ~ (1<<MD_CHANGE_PENDING)) ||
 		test_bit(MD_RECOVERY_NEEDED, &mddev->recovery) ||
 		test_bit(MD_RECOVERY_DONE, &mddev->recovery) ||
+		test_bit(MD_RELOAD_SB, &mddev->flags) ||
 		(mddev->external == 0 && mddev->safemode == 1) ||
 		(mddev->safemode == 2 && ! atomic_read(&mddev->writes_pending)
 		 && !mddev->in_sync && mddev->recovery_cp == MaxSector)
@@ -8318,6 +8339,21 @@ void md_check_recovery(struct mddev *mddev)
 			goto unlock;
 		}
 
+		if (mddev_is_clustered(mddev)) {
+			struct md_rdev *rdev;
+			/* kick the device if another node issued a
+			 * remove disk.
+			 */
+			rdev_for_each(rdev, mddev) {
+				if (test_and_clear_bit(ClusterRemove, &rdev->flags) &&
+						rdev->raid_disk < 0)
+					md_kick_rdev_from_array(rdev);
+			}
+
+			if (test_and_clear_bit(MD_RELOAD_SB, &mddev->flags))
+				md_reload_sb(mddev, mddev->good_device_nr);
+		}
+
 		if (!mddev->external) {
 			int did_change = 0;
 			spin_lock(&mddev->lock);
@@ -8489,254 +8525,9 @@ void md_finish_reshape(struct mddev *mddev)
 }
 EXPORT_SYMBOL(md_finish_reshape);
 
-/* Bad block management.
- * We can record which blocks on each device are 'bad' and so just
- * fail those blocks, or that stripe, rather than the whole device.
- * Entries in the bad-block table are 64bits wide.  This comprises:
- * Length of bad-range, in sectors: 0-511 for lengths 1-512
- * Start of bad-range, sector offset, 54 bits (allows 8 exbibytes)
- *  A 'shift' can be set so that larger blocks are tracked and
- *  consequently larger devices can be covered.
- * 'Acknowledged' flag - 1 bit. - the most significant bit.
- *
- * Locking of the bad-block table uses a seqlock so md_is_badblock
- * might need to retry if it is very unlucky.
- * We will sometimes want to check for bad blocks in a bi_end_io function,
- * so we use the write_seqlock_irq variant.
- *
- * When looking for a bad block we specify a range and want to
- * know if any block in the range is bad.  So we binary-search
- * to the last range that starts at-or-before the given endpoint,
- * (or "before the sector after the target range")
- * then see if it ends after the given start.
- * We return
- *  0 if there are no known bad blocks in the range
- *  1 if there are known bad block which are all acknowledged
- * -1 if there are bad blocks which have not yet been acknowledged in metadata.
- * plus the start/length of the first bad section we overlap.
- */
-int md_is_badblock(struct badblocks *bb, sector_t s, int sectors,
-		   sector_t *first_bad, int *bad_sectors)
-{
-	int hi;
-	int lo;
-	u64 *p = bb->page;
-	int rv;
-	sector_t target = s + sectors;
-	unsigned seq;
-
-	if (bb->shift > 0) {
-		/* round the start down, and the end up */
-		s >>= bb->shift;
-		target += (1<<bb->shift) - 1;
-		target >>= bb->shift;
-		sectors = target - s;
-	}
-	/* 'target' is now the first block after the bad range */
-
-retry:
-	seq = read_seqbegin(&bb->lock);
-	lo = 0;
-	rv = 0;
-	hi = bb->count;
-
-	/* Binary search between lo and hi for 'target'
-	 * i.e. for the last range that starts before 'target'
-	 */
-	/* INVARIANT: ranges before 'lo' and at-or-after 'hi'
-	 * are known not to be the last range before target.
-	 * VARIANT: hi-lo is the number of possible
-	 * ranges, and decreases until it reaches 1
-	 */
-	while (hi - lo > 1) {
-		int mid = (lo + hi) / 2;
-		sector_t a = BB_OFFSET(p[mid]);
-		if (a < target)
-			/* This could still be the one, earlier ranges
-			 * could not. */
-			lo = mid;
-		else
-			/* This and later ranges are definitely out. */
-			hi = mid;
-	}
-	/* 'lo' might be the last that started before target, but 'hi' isn't */
-	if (hi > lo) {
-		/* need to check all range that end after 's' to see if
-		 * any are unacknowledged.
-		 */
-		while (lo >= 0 &&
-		       BB_OFFSET(p[lo]) + BB_LEN(p[lo]) > s) {
-			if (BB_OFFSET(p[lo]) < target) {
-				/* starts before the end, and finishes after
-				 * the start, so they must overlap
-				 */
-				if (rv != -1 && BB_ACK(p[lo]))
-					rv = 1;
-				else
-					rv = -1;
-				*first_bad = BB_OFFSET(p[lo]);
-				*bad_sectors = BB_LEN(p[lo]);
-			}
-			lo--;
-		}
-	}
-
-	if (read_seqretry(&bb->lock, seq))
-		goto retry;
-
-	return rv;
-}
-EXPORT_SYMBOL_GPL(md_is_badblock);
-
-/*
- * Add a range of bad blocks to the table.
- * This might extend the table, or might contract it
- * if two adjacent ranges can be merged.
- * We binary-search to find the 'insertion' point, then
- * decide how best to handle it.
- */
-static int md_set_badblocks(struct badblocks *bb, sector_t s, int sectors,
-			    int acknowledged)
-{
-	u64 *p;
-	int lo, hi;
-	int rv = 1;
-	unsigned long flags;
-
-	if (bb->shift < 0)
-		/* badblocks are disabled */
-		return 0;
-
-	if (bb->shift) {
-		/* round the start down, and the end up */
-		sector_t next = s + sectors;
-		s >>= bb->shift;
-		next += (1<<bb->shift) - 1;
-		next >>= bb->shift;
-		sectors = next - s;
-	}
-
-	write_seqlock_irqsave(&bb->lock, flags);
-
-	p = bb->page;
-	lo = 0;
-	hi = bb->count;
-	/* Find the last range that starts at-or-before 's' */
-	while (hi - lo > 1) {
-		int mid = (lo + hi) / 2;
-		sector_t a = BB_OFFSET(p[mid]);
-		if (a <= s)
-			lo = mid;
-		else
-			hi = mid;
-	}
-	if (hi > lo && BB_OFFSET(p[lo]) > s)
-		hi = lo;
-
-	if (hi > lo) {
-		/* we found a range that might merge with the start
-		 * of our new range
-		 */
-		sector_t a = BB_OFFSET(p[lo]);
-		sector_t e = a + BB_LEN(p[lo]);
-		int ack = BB_ACK(p[lo]);
-		if (e >= s) {
-			/* Yes, we can merge with a previous range */
-			if (s == a && s + sectors >= e)
-				/* new range covers old */
-				ack = acknowledged;
-			else
-				ack = ack && acknowledged;
-
-			if (e < s + sectors)
-				e = s + sectors;
-			if (e - a <= BB_MAX_LEN) {
-				p[lo] = BB_MAKE(a, e-a, ack);
-				s = e;
-			} else {
-				/* does not all fit in one range,
-				 * make p[lo] maximal
-				 */
-				if (BB_LEN(p[lo]) != BB_MAX_LEN)
-					p[lo] = BB_MAKE(a, BB_MAX_LEN, ack);
-				s = a + BB_MAX_LEN;
-			}
-			sectors = e - s;
-		}
-	}
-	if (sectors && hi < bb->count) {
-		/* 'hi' points to the first range that starts after 's'.
-		 * Maybe we can merge with the start of that range */
-		sector_t a = BB_OFFSET(p[hi]);
-		sector_t e = a + BB_LEN(p[hi]);
-		int ack = BB_ACK(p[hi]);
-		if (a <= s + sectors) {
-			/* merging is possible */
-			if (e <= s + sectors) {
-				/* full overlap */
-				e = s + sectors;
-				ack = acknowledged;
-			} else
-				ack = ack && acknowledged;
-
-			a = s;
-			if (e - a <= BB_MAX_LEN) {
-				p[hi] = BB_MAKE(a, e-a, ack);
-				s = e;
-			} else {
-				p[hi] = BB_MAKE(a, BB_MAX_LEN, ack);
-				s = a + BB_MAX_LEN;
-			}
-			sectors = e - s;
-			lo = hi;
-			hi++;
-		}
-	}
-	if (sectors == 0 && hi < bb->count) {
-		/* we might be able to combine lo and hi */
-		/* Note: 's' is at the end of 'lo' */
-		sector_t a = BB_OFFSET(p[hi]);
-		int lolen = BB_LEN(p[lo]);
-		int hilen = BB_LEN(p[hi]);
-		int newlen = lolen + hilen - (s - a);
-		if (s >= a && newlen < BB_MAX_LEN) {
-			/* yes, we can combine them */
-			int ack = BB_ACK(p[lo]) && BB_ACK(p[hi]);
-			p[lo] = BB_MAKE(BB_OFFSET(p[lo]), newlen, ack);
-			memmove(p + hi, p + hi + 1,
-				(bb->count - hi - 1) * 8);
-			bb->count--;
-		}
-	}
-	while (sectors) {
-		/* didn't merge (it all).
-		 * Need to add a range just before 'hi' */
-		if (bb->count >= MD_MAX_BADBLOCKS) {
-			/* No room for more */
-			rv = 0;
-			break;
-		} else {
-			int this_sectors = sectors;
-			memmove(p + hi + 1, p + hi,
-				(bb->count - hi) * 8);
-			bb->count++;
-
-			if (this_sectors > BB_MAX_LEN)
-				this_sectors = BB_MAX_LEN;
-			p[hi] = BB_MAKE(s, this_sectors, acknowledged);
-			sectors -= this_sectors;
-			s += this_sectors;
-		}
-	}
-
-	bb->changed = 1;
-	if (!acknowledged)
-		bb->unacked_exist = 1;
-	write_sequnlock_irqrestore(&bb->lock, flags);
-
-	return rv;
-}
+/* Bad block management */
 
+/* Returns 1 on success, 0 on failure */
 int rdev_set_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 		       int is_new)
 {
@@ -8745,114 +8536,19 @@ int rdev_set_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 		s += rdev->new_data_offset;
 	else
 		s += rdev->data_offset;
-	rv = md_set_badblocks(&rdev->badblocks,
-			      s, sectors, 0);
-	if (rv) {
+	rv = badblocks_set(&rdev->badblocks, s, sectors, 0);
+	if (rv == 0) {
 		/* Make sure they get written out promptly */
 		sysfs_notify_dirent_safe(rdev->sysfs_state);
 		set_bit(MD_CHANGE_CLEAN, &rdev->mddev->flags);
 		set_bit(MD_CHANGE_PENDING, &rdev->mddev->flags);
 		md_wakeup_thread(rdev->mddev->thread);
-	}
-	return rv;
+		return 1;
+	} else
+		return 0;
 }
 EXPORT_SYMBOL_GPL(rdev_set_badblocks);
 
-/*
- * Remove a range of bad blocks from the table.
- * This may involve extending the table if we spilt a region,
- * but it must not fail.  So if the table becomes full, we just
- * drop the remove request.
- */
-static int md_clear_badblocks(struct badblocks *bb, sector_t s, int sectors)
-{
-	u64 *p;
-	int lo, hi;
-	sector_t target = s + sectors;
-	int rv = 0;
-
-	if (bb->shift > 0) {
-		/* When clearing we round the start up and the end down.
-		 * This should not matter as the shift should align with
-		 * the block size and no rounding should ever be needed.
-		 * However it is better the think a block is bad when it
-		 * isn't than to think a block is not bad when it is.
-		 */
-		s += (1<<bb->shift) - 1;
-		s >>= bb->shift;
-		target >>= bb->shift;
-		sectors = target - s;
-	}
-
-	write_seqlock_irq(&bb->lock);
-
-	p = bb->page;
-	lo = 0;
-	hi = bb->count;
-	/* Find the last range that starts before 'target' */
-	while (hi - lo > 1) {
-		int mid = (lo + hi) / 2;
-		sector_t a = BB_OFFSET(p[mid]);
-		if (a < target)
-			lo = mid;
-		else
-			hi = mid;
-	}
-	if (hi > lo) {
-		/* p[lo] is the last range that could overlap the
-		 * current range.  Earlier ranges could also overlap,
-		 * but only this one can overlap the end of the range.
-		 */
-		if (BB_OFFSET(p[lo]) + BB_LEN(p[lo]) > target) {
-			/* Partial overlap, leave the tail of this range */
-			int ack = BB_ACK(p[lo]);
-			sector_t a = BB_OFFSET(p[lo]);
-			sector_t end = a + BB_LEN(p[lo]);
-
-			if (a < s) {
-				/* we need to split this range */
-				if (bb->count >= MD_MAX_BADBLOCKS) {
-					rv = -ENOSPC;
-					goto out;
-				}
-				memmove(p+lo+1, p+lo, (bb->count - lo) * 8);
-				bb->count++;
-				p[lo] = BB_MAKE(a, s-a, ack);
-				lo++;
-			}
-			p[lo] = BB_MAKE(target, end - target, ack);
-			/* there is no longer an overlap */
-			hi = lo;
-			lo--;
-		}
-		while (lo >= 0 &&
-		       BB_OFFSET(p[lo]) + BB_LEN(p[lo]) > s) {
-			/* This range does overlap */
-			if (BB_OFFSET(p[lo]) < s) {
-				/* Keep the early parts of this range. */
-				int ack = BB_ACK(p[lo]);
-				sector_t start = BB_OFFSET(p[lo]);
-				p[lo] = BB_MAKE(start, s - start, ack);
-				/* now low doesn't overlap, so.. */
-				break;
-			}
-			lo--;
-		}
-		/* 'lo' is strictly before, 'hi' is strictly after,
-		 * anything between needs to be discarded
-		 */
-		if (hi - lo > 1) {
-			memmove(p+lo+1, p+hi, (bb->count - hi) * 8);
-			bb->count -= (hi - lo - 1);
-		}
-	}
-
-	bb->changed = 1;
-out:
-	write_sequnlock_irq(&bb->lock);
-	return rv;
-}
-
 int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 			 int is_new)
 {
@@ -8860,133 +8556,11 @@ int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 		s += rdev->new_data_offset;
 	else
 		s += rdev->data_offset;
-	return md_clear_badblocks(&rdev->badblocks,
+	return badblocks_clear(&rdev->badblocks,
 				  s, sectors);
 }
 EXPORT_SYMBOL_GPL(rdev_clear_badblocks);
 
-/*
- * Acknowledge all bad blocks in a list.
- * This only succeeds if ->changed is clear.  It is used by
- * in-kernel metadata updates
- */
-void md_ack_all_badblocks(struct badblocks *bb)
-{
-	if (bb->page == NULL || bb->changed)
-		/* no point even trying */
-		return;
-	write_seqlock_irq(&bb->lock);
-
-	if (bb->changed == 0 && bb->unacked_exist) {
-		u64 *p = bb->page;
-		int i;
-		for (i = 0; i < bb->count ; i++) {
-			if (!BB_ACK(p[i])) {
-				sector_t start = BB_OFFSET(p[i]);
-				int len = BB_LEN(p[i]);
-				p[i] = BB_MAKE(start, len, 1);
-			}
-		}
-		bb->unacked_exist = 0;
-	}
-	write_sequnlock_irq(&bb->lock);
-}
-EXPORT_SYMBOL_GPL(md_ack_all_badblocks);
-
-/* sysfs access to bad-blocks list.
- * We present two files.
- * 'bad-blocks' lists sector numbers and lengths of ranges that
- *    are recorded as bad.  The list is truncated to fit within
- *    the one-page limit of sysfs.
- *    Writing "sector length" to this file adds an acknowledged
- *    bad block list.
- * 'unacknowledged-bad-blocks' lists bad blocks that have not yet
- *    been acknowledged.  Writing to this file adds bad blocks
- *    without acknowledging them.  This is largely for testing.
- */
-
-static ssize_t
-badblocks_show(struct badblocks *bb, char *page, int unack)
-{
-	size_t len;
-	int i;
-	u64 *p = bb->page;
-	unsigned seq;
-
-	if (bb->shift < 0)
-		return 0;
-
-retry:
-	seq = read_seqbegin(&bb->lock);
-
-	len = 0;
-	i = 0;
-
-	while (len < PAGE_SIZE && i < bb->count) {
-		sector_t s = BB_OFFSET(p[i]);
-		unsigned int length = BB_LEN(p[i]);
-		int ack = BB_ACK(p[i]);
-		i++;
-
-		if (unack && ack)
-			continue;
-
-		len += snprintf(page+len, PAGE_SIZE-len, "%llu %u\n",
-				(unsigned long long)s << bb->shift,
-				length << bb->shift);
-	}
-	if (unack && len == 0)
-		bb->unacked_exist = 0;
-
-	if (read_seqretry(&bb->lock, seq))
-		goto retry;
-
-	return len;
-}
-
-#define DO_DEBUG 1
-
-static ssize_t
-badblocks_store(struct badblocks *bb, const char *page, size_t len, int unack)
-{
-	unsigned long long sector;
-	int length;
-	char newline;
-#ifdef DO_DEBUG
-	/* Allow clearing via sysfs *only* for testing/debugging.
-	 * Normally only a successful write may clear a badblock
-	 */
-	int clear = 0;
-	if (page[0] == '-') {
-		clear = 1;
-		page++;
-	}
-#endif /* DO_DEBUG */
-
-	switch (sscanf(page, "%llu %d%c", &sector, &length, &newline)) {
-	case 3:
-		if (newline != '\n')
-			return -EINVAL;
-	case 2:
-		if (length <= 0)
-			return -EINVAL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-#ifdef DO_DEBUG
-	if (clear) {
-		md_clear_badblocks(bb, sector, length);
-		return len;
-	}
-#endif /* DO_DEBUG */
-	if (md_set_badblocks(bb, sector, length, !unack))
-		return len;
-	else
-		return -ENOSPC;
-}
-
 static int md_notify_reboot(struct notifier_block *this,
 			    unsigned long code, void *x)
 {
@@ -9101,7 +8675,6 @@ static void check_sb_changes(struct mddev *mddev, struct md_rdev *rdev)
 				ret = remove_and_add_spares(mddev, rdev2);
 				pr_info("Activated spare: %s\n",
 						bdevname(rdev2->bdev,b));
-				continue;
 			}
 			/* device faulty
 			 * We just want to do the minimum to mark the disk
diff --git a/drivers/md/md.h b/drivers/md/md.h
index ca0b643fe3c1..b5c4be73e6e4 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -17,6 +17,7 @@
 
 #include <linux/blkdev.h>
 #include <linux/backing-dev.h>
+#include <linux/badblocks.h>
 #include <linux/kobject.h>
 #include <linux/list.h>
 #include <linux/mm.h>
@@ -28,13 +29,6 @@
 
 #define MaxSector (~(sector_t)0)
 
-/* Bad block numbers are stored sorted in a single page.
- * 64bits is used for each block or extent.
- * 54 bits are sector number, 9 bits are extent size,
- * 1 bit is an 'acknowledged' flag.
- */
-#define MD_MAX_BADBLOCKS	(PAGE_SIZE/8)
-
 /*
  * MD's 'extended' device
  */
@@ -117,22 +111,7 @@ struct md_rdev {
 	struct kernfs_node *sysfs_state; /* handle for 'state'
 					   * sysfs entry */
 
-	struct badblocks {
-		int	count;		/* count of bad blocks */
-		int	unacked_exist;	/* there probably are unacknowledged
-					 * bad blocks.  This is only cleared
-					 * when a read discovers none
-					 */
-		int	shift;		/* shift from sectors to block size
-					 * a -ve shift means badblocks are
-					 * disabled.*/
-		u64	*page;		/* badblock list */
-		int	changed;
-		seqlock_t lock;
-
-		sector_t sector;
-		sector_t size;		/* in sectors */
-	} badblocks;
+	struct badblocks badblocks;
 };
 enum flag_bits {
 	Faulty,			/* device is known to have a fault */
@@ -183,24 +162,14 @@ enum flag_bits {
 				 * Usually, this device should be faster
 				 * than other devices in the array
 				 */
+	ClusterRemove,
 };
 
-#define BB_LEN_MASK	(0x00000000000001FFULL)
-#define BB_OFFSET_MASK	(0x7FFFFFFFFFFFFE00ULL)
-#define BB_ACK_MASK	(0x8000000000000000ULL)
-#define BB_MAX_LEN	512
-#define BB_OFFSET(x)	(((x) & BB_OFFSET_MASK) >> 9)
-#define BB_LEN(x)	(((x) & BB_LEN_MASK) + 1)
-#define BB_ACK(x)	(!!((x) & BB_ACK_MASK))
-#define BB_MAKE(a, l, ack) (((a)<<9) | ((l)-1) | ((u64)(!!(ack)) << 63))
-
-extern int md_is_badblock(struct badblocks *bb, sector_t s, int sectors,
-			  sector_t *first_bad, int *bad_sectors);
 static inline int is_badblock(struct md_rdev *rdev, sector_t s, int sectors,
 			      sector_t *first_bad, int *bad_sectors)
 {
 	if (unlikely(rdev->badblocks.count)) {
-		int rv = md_is_badblock(&rdev->badblocks, rdev->data_offset + s,
+		int rv = badblocks_check(&rdev->badblocks, rdev->data_offset + s,
 					sectors,
 					first_bad, bad_sectors);
 		if (rv)
@@ -213,8 +182,6 @@ extern int rdev_set_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 			      int is_new);
 extern int rdev_clear_badblocks(struct md_rdev *rdev, sector_t s, int sectors,
 				int is_new);
-extern void md_ack_all_badblocks(struct badblocks *bb);
-
 struct md_cluster_info;
 
 struct mddev {
@@ -234,6 +201,9 @@ struct mddev {
 				 */
 #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
+				 * updated it.
+				 */
 
 	int				suspended;
 	atomic_t			active_io;
@@ -242,8 +212,6 @@ struct mddev {
 						       * are happening, so run/
 						       * takeover/stop are not safe
 						       */
-	int				ready; /* See when safe to pass
-						* IO requests down */
 	struct gendisk			*gendisk;
 
 	struct kobject			kobj;
@@ -260,7 +228,7 @@ struct mddev {
 							 * managed externally */
 	char				metadata_type[17]; /* externally set*/
 	int				chunk_sectors;
-	time_t				ctime, utime;
+	time64_t			ctime, utime;
 	int				level, layout;
 	char				clevel[16];
 	int				raid_disks;
@@ -464,6 +432,7 @@ struct mddev {
 	struct work_struct event_work;	/* used by dm to report failure event */
 	void (*sync_super)(struct mddev *mddev, struct md_rdev *rdev);
 	struct md_cluster_info		*cluster_info;
+	unsigned int			good_device_nr;	/* good device num within cluster raid */
 };
 
 static inline int __must_check mddev_lock(struct mddev *mddev)
@@ -657,7 +626,7 @@ extern void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev);
 extern void md_set_array_sectors(struct mddev *mddev, sector_t array_sectors);
 extern int md_check_no_bitmap(struct mddev *mddev);
 extern int md_integrity_register(struct mddev *mddev);
-extern void md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev);
+extern int md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev);
 extern int strict_strtoul_scaled(const char *cp, unsigned long *res, int scale);
 
 extern void mddev_init(struct mddev *mddev);
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 7331a80d89f1..0a72ab6e6c20 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -257,6 +257,9 @@ static int multipath_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 			disk_stack_limits(mddev->gendisk, rdev->bdev,
 					  rdev->data_offset << 9);
 
+			err = md_integrity_add_rdev(rdev, mddev);
+			if (err)
+				break;
 			spin_lock_irq(&conf->device_lock);
 			mddev->degraded--;
 			rdev->raid_disk = path;
@@ -264,9 +267,6 @@ static int multipath_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 			spin_unlock_irq(&conf->device_lock);
 			rcu_assign_pointer(p->rdev, rdev);
 			err = 0;
-			mddev_suspend(mddev);
-			md_integrity_add_rdev(rdev, mddev);
-			mddev_resume(mddev);
 			break;
 		}
 
diff --git a/drivers/md/persistent-data/Kconfig b/drivers/md/persistent-data/Kconfig
index 78c74bb71ba4..a53cbc928af1 100644
--- a/drivers/md/persistent-data/Kconfig
+++ b/drivers/md/persistent-data/Kconfig
@@ -7,12 +7,3 @@ config DM_PERSISTENT_DATA
 	 Library providing immutable on-disk data structure support for
 	 device-mapper targets such as the thin provisioning target.
 
-config DM_DEBUG_BLOCK_STACK_TRACING
-       bool "Keep stack trace of persistent data block lock holders"
-       depends on STACKTRACE_SUPPORT && DM_PERSISTENT_DATA
-       select STACKTRACE
-       ---help---
-	 Enable this for messages that may help debug problems with the
-	 block manager locking used by thin provisioning and caching.
-
-	 If unsure, say N.
diff --git a/drivers/md/persistent-data/dm-block-manager.c b/drivers/md/persistent-data/dm-block-manager.c
index f2393ba838eb..1e33dd51c21f 100644
--- a/drivers/md/persistent-data/dm-block-manager.c
+++ b/drivers/md/persistent-data/dm-block-manager.c
@@ -97,10 +97,6 @@ static void __del_holder(struct block_lock *lock, struct task_struct *task)
 static int __check_holder(struct block_lock *lock)
 {
 	unsigned i;
-#ifdef CONFIG_DM_DEBUG_BLOCK_STACK_TRACING
-	static struct stack_trace t;
-	static stack_entries entries;
-#endif
 
 	for (i = 0; i < MAX_HOLDERS; i++) {
 		if (lock->holders[i] == current) {
@@ -110,12 +106,7 @@ static int __check_holder(struct block_lock *lock)
 			print_stack_trace(lock->traces + i, 4);
 
 			DMERR("subsequent acquisition attempted here:");
-			t.nr_entries = 0;
-			t.max_entries = MAX_STACK;
-			t.entries = entries;
-			t.skip = 3;
-			save_stack_trace(&t);
-			print_stack_trace(&t, 4);
+			dump_stack();
 #endif
 			return -EINVAL;
 		}
diff --git a/drivers/md/persistent-data/dm-btree.c b/drivers/md/persistent-data/dm-btree.c
index b1ced58eb5e1..ea3d3b656fd0 100644
--- a/drivers/md/persistent-data/dm-btree.c
+++ b/drivers/md/persistent-data/dm-btree.c
@@ -754,12 +754,19 @@ static int btree_insert_raw(struct shadow_spine *s, dm_block_t root,
 	return 0;
 }
 
+static bool need_insert(struct btree_node *node, uint64_t *keys,
+			unsigned level, unsigned index)
+{
+        return ((index >= le32_to_cpu(node->header.nr_entries)) ||
+		(le64_to_cpu(node->keys[index]) != keys[level]));
+}
+
 static int insert(struct dm_btree_info *info, dm_block_t root,
 		  uint64_t *keys, void *value, dm_block_t *new_root,
 		  int *inserted)
 		  __dm_written_to_disk(value)
 {
-	int r, need_insert;
+	int r;
 	unsigned level, index = -1, last_level = info->levels - 1;
 	dm_block_t block = root;
 	struct shadow_spine spine;
@@ -775,10 +782,8 @@ static int insert(struct dm_btree_info *info, dm_block_t root,
 			goto bad;
 
 		n = dm_block_data(shadow_current(&spine));
-		need_insert = ((index >= le32_to_cpu(n->header.nr_entries)) ||
-			       (le64_to_cpu(n->keys[index]) != keys[level]));
 
-		if (need_insert) {
+		if (need_insert(n, keys, level, index)) {
 			dm_block_t new_tree;
 			__le64 new_le;
 
@@ -805,10 +810,8 @@ static int insert(struct dm_btree_info *info, dm_block_t root,
 		goto bad;
 
 	n = dm_block_data(shadow_current(&spine));
-	need_insert = ((index >= le32_to_cpu(n->header.nr_entries)) ||
-		       (le64_to_cpu(n->keys[index]) != keys[level]));
 
-	if (need_insert) {
+	if (need_insert(n, keys, level, index)) {
 		if (inserted)
 			*inserted = 1;
 
diff --git a/drivers/md/persistent-data/dm-space-map-metadata.c b/drivers/md/persistent-data/dm-space-map-metadata.c
index fca6dbcf9a47..7e44005595c1 100644
--- a/drivers/md/persistent-data/dm-space-map-metadata.c
+++ b/drivers/md/persistent-data/dm-space-map-metadata.c
@@ -152,12 +152,9 @@ static int brb_peek(struct bop_ring_buffer *brb, struct block_op *result)
 
 static int brb_pop(struct bop_ring_buffer *brb)
 {
-	struct block_op *bop;
-
 	if (brb_empty(brb))
 		return -ENODATA;
 
-	bop = brb->bops + brb->begin;
 	brb->begin = brb_next(brb, brb->begin);
 
 	return 0;
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index f8e5db0cb5aa..2ea12c6bf659 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -549,13 +549,13 @@ static void *raid0_takeover_raid10(struct mddev *mddev)
 	 *  - all mirrors must be already degraded
 	 */
 	if (mddev->layout != ((1 << 8) + 2)) {
-		printk(KERN_ERR "md/raid0:%s:: Raid0 cannot takover layout: 0x%x\n",
+		printk(KERN_ERR "md/raid0:%s:: Raid0 cannot takeover layout: 0x%x\n",
 		       mdname(mddev),
 		       mddev->layout);
 		return ERR_PTR(-EINVAL);
 	}
 	if (mddev->raid_disks & 1) {
-		printk(KERN_ERR "md/raid0:%s: Raid0 cannot takover Raid10 with odd disk number.\n",
+		printk(KERN_ERR "md/raid0:%s: Raid0 cannot takeover Raid10 with odd disk number.\n",
 		       mdname(mddev));
 		return ERR_PTR(-EINVAL);
 	}
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index e2169ff6e0f0..4e3843f7d245 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -1044,7 +1044,7 @@ static void raid1_unplug(struct blk_plug_cb *cb, bool from_schedule)
 	kfree(plug);
 }
 
-static void make_request(struct mddev *mddev, struct bio * bio)
+static void raid1_make_request(struct mddev *mddev, struct bio * bio)
 {
 	struct r1conf *conf = mddev->private;
 	struct raid1_info *mirror;
@@ -1422,7 +1422,7 @@ read_again:
 	wake_up(&conf->wait_barrier);
 }
 
-static void status(struct seq_file *seq, struct mddev *mddev)
+static void raid1_status(struct seq_file *seq, struct mddev *mddev)
 {
 	struct r1conf *conf = mddev->private;
 	int i;
@@ -1439,7 +1439,7 @@ static void status(struct seq_file *seq, struct mddev *mddev)
 	seq_printf(seq, "]");
 }
 
-static void error(struct mddev *mddev, struct md_rdev *rdev)
+static void raid1_error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
 	struct r1conf *conf = mddev->private;
@@ -1589,6 +1589,9 @@ static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	if (mddev->recovery_disabled == conf->recovery_disabled)
 		return -EBUSY;
 
+	if (md_integrity_add_rdev(rdev, mddev))
+		return -ENXIO;
+
 	if (rdev->raid_disk >= 0)
 		first = last = rdev->raid_disk;
 
@@ -1632,9 +1635,6 @@ static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 			break;
 		}
 	}
-	mddev_suspend(mddev);
-	md_integrity_add_rdev(rdev, mddev);
-	mddev_resume(mddev);
 	if (mddev->queue && blk_queue_discard(bdev_get_queue(rdev->bdev)))
 		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
 	print_conf(conf);
@@ -2472,7 +2472,8 @@ static int init_resync(struct r1conf *conf)
  * that can be installed to exclude normal IO requests.
  */
 
-static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipped)
+static sector_t raid1_sync_request(struct mddev *mddev, sector_t sector_nr,
+				   int *skipped)
 {
 	struct r1conf *conf = mddev->private;
 	struct r1bio *r1_bio;
@@ -2890,7 +2891,7 @@ static struct r1conf *setup_conf(struct mddev *mddev)
 }
 
 static void raid1_free(struct mddev *mddev, void *priv);
-static int run(struct mddev *mddev)
+static int raid1_run(struct mddev *mddev)
 {
 	struct r1conf *conf;
 	int i;
@@ -3170,15 +3171,15 @@ static struct md_personality raid1_personality =
 	.name		= "raid1",
 	.level		= 1,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= raid1_make_request,
+	.run		= raid1_run,
 	.free		= raid1_free,
-	.status		= status,
-	.error_handler	= error,
+	.status		= raid1_status,
+	.error_handler	= raid1_error,
 	.hot_add_disk	= raid1_add_disk,
 	.hot_remove_disk= raid1_remove_disk,
 	.spare_active	= raid1_spare_active,
-	.sync_request	= sync_request,
+	.sync_request	= raid1_sync_request,
 	.resize		= raid1_resize,
 	.size		= raid1_size,
 	.check_reshape	= raid1_reshape,
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index 84e597e1c489..1c1447dd3417 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -1442,7 +1442,7 @@ retry_write:
 	one_write_done(r10_bio);
 }
 
-static void make_request(struct mddev *mddev, struct bio *bio)
+static void raid10_make_request(struct mddev *mddev, struct bio *bio)
 {
 	struct r10conf *conf = mddev->private;
 	sector_t chunk_mask = (conf->geo.chunk_mask & conf->prev.chunk_mask);
@@ -1484,7 +1484,7 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 	wake_up(&conf->wait_barrier);
 }
 
-static void status(struct seq_file *seq, struct mddev *mddev)
+static void raid10_status(struct seq_file *seq, struct mddev *mddev)
 {
 	struct r10conf *conf = mddev->private;
 	int i;
@@ -1562,7 +1562,7 @@ static int enough(struct r10conf *conf, int ignore)
 		_enough(conf, 1, ignore);
 }
 
-static void error(struct mddev *mddev, struct md_rdev *rdev)
+static void raid10_error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
 	struct r10conf *conf = mddev->private;
@@ -1698,6 +1698,9 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	if (rdev->saved_raid_disk < 0 && !_enough(conf, 1, -1))
 		return -EINVAL;
 
+	if (md_integrity_add_rdev(rdev, mddev))
+		return -ENXIO;
+
 	if (rdev->raid_disk >= 0)
 		first = last = rdev->raid_disk;
 
@@ -1739,9 +1742,6 @@ static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 		rcu_assign_pointer(p->rdev, rdev);
 		break;
 	}
-	mddev_suspend(mddev);
-	md_integrity_add_rdev(rdev, mddev);
-	mddev_resume(mddev);
 	if (mddev->queue && blk_queue_discard(bdev_get_queue(rdev->bdev)))
 		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, mddev->queue);
 
@@ -2802,7 +2802,7 @@ static int init_resync(struct r10conf *conf)
  *
  */
 
-static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
+static sector_t raid10_sync_request(struct mddev *mddev, sector_t sector_nr,
 			     int *skipped)
 {
 	struct r10conf *conf = mddev->private;
@@ -3523,7 +3523,7 @@ static struct r10conf *setup_conf(struct mddev *mddev)
 	return ERR_PTR(err);
 }
 
-static int run(struct mddev *mddev)
+static int raid10_run(struct mddev *mddev)
 {
 	struct r10conf *conf;
 	int i, disk_idx, chunk_size;
@@ -4617,15 +4617,15 @@ static struct md_personality raid10_personality =
 	.name		= "raid10",
 	.level		= 10,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= raid10_make_request,
+	.run		= raid10_run,
 	.free		= raid10_free,
-	.status		= status,
-	.error_handler	= error,
+	.status		= raid10_status,
+	.error_handler	= raid10_error,
 	.hot_add_disk	= raid10_add_disk,
 	.hot_remove_disk= raid10_remove_disk,
 	.spare_active	= raid10_spare_active,
-	.sync_request	= sync_request,
+	.sync_request	= raid10_sync_request,
 	.quiesce	= raid10_quiesce,
 	.size		= raid10_size,
 	.resize		= raid10_resize,
diff --git a/drivers/md/raid5-cache.c b/drivers/md/raid5-cache.c
index b887e04d7e5c..9531f5f05b93 100644
--- a/drivers/md/raid5-cache.c
+++ b/drivers/md/raid5-cache.c
@@ -34,6 +34,12 @@
 #define RECLAIM_MAX_FREE_SPACE (10 * 1024 * 1024 * 2) /* sector */
 #define RECLAIM_MAX_FREE_SPACE_SHIFT (2)
 
+/*
+ * We only need 2 bios per I/O unit to make progress, but ensure we
+ * have a few more available to not get too tight.
+ */
+#define R5L_POOL_SIZE	4
+
 struct r5l_log {
 	struct md_rdev *rdev;
 
@@ -69,7 +75,12 @@ struct r5l_log {
 	struct list_head finished_ios;	/* io_units which settle down in log disk */
 	struct bio flush_bio;
 
+	struct list_head no_mem_stripes;   /* pending stripes, -ENOMEM */
+
 	struct kmem_cache *io_kc;
+	mempool_t *io_pool;
+	struct bio_set *bs;
+	mempool_t *meta_pool;
 
 	struct md_thread *reclaim_thread;
 	unsigned long reclaim_target;	/* number of space that need to be
@@ -150,27 +161,6 @@ static bool r5l_has_free_space(struct r5l_log *log, sector_t size)
 	return log->device_size > used_size + size;
 }
 
-static void r5l_free_io_unit(struct r5l_log *log, struct r5l_io_unit *io)
-{
-	__free_page(io->meta_page);
-	kmem_cache_free(log->io_kc, io);
-}
-
-static void r5l_move_io_unit_list(struct list_head *from, struct list_head *to,
-				  enum r5l_io_unit_state state)
-{
-	struct r5l_io_unit *io;
-
-	while (!list_empty(from)) {
-		io = list_first_entry(from, struct r5l_io_unit, log_sibling);
-		/* don't change list order */
-		if (io->state >= state)
-			list_move_tail(&io->log_sibling, to);
-		else
-			break;
-	}
-}
-
 static void __r5l_set_io_unit_state(struct r5l_io_unit *io,
 				    enum r5l_io_unit_state state)
 {
@@ -206,6 +196,20 @@ static void r5l_log_run_stripes(struct r5l_log *log)
 	}
 }
 
+static void r5l_move_to_end_ios(struct r5l_log *log)
+{
+	struct r5l_io_unit *io, *next;
+
+	assert_spin_locked(&log->io_list_lock);
+
+	list_for_each_entry_safe(io, next, &log->running_ios, log_sibling) {
+		/* don't change list order */
+		if (io->state < IO_UNIT_IO_END)
+			break;
+		list_move_tail(&io->log_sibling, &log->io_end_ios);
+	}
+}
+
 static void r5l_log_endio(struct bio *bio)
 {
 	struct r5l_io_unit *io = bio->bi_private;
@@ -216,12 +220,12 @@ static void r5l_log_endio(struct bio *bio)
 		md_error(log->rdev->mddev, log->rdev);
 
 	bio_put(bio);
+	mempool_free(io->meta_page, log->meta_pool);
 
 	spin_lock_irqsave(&log->io_list_lock, flags);
 	__r5l_set_io_unit_state(io, IO_UNIT_IO_END);
 	if (log->need_cache_flush)
-		r5l_move_io_unit_list(&log->running_ios, &log->io_end_ios,
-				      IO_UNIT_IO_END);
+		r5l_move_to_end_ios(log);
 	else
 		r5l_log_run_stripes(log);
 	spin_unlock_irqrestore(&log->io_list_lock, flags);
@@ -255,7 +259,7 @@ static void r5l_submit_current_io(struct r5l_log *log)
 
 static struct bio *r5l_bio_alloc(struct r5l_log *log)
 {
-	struct bio *bio = bio_kmalloc(GFP_NOIO | __GFP_NOFAIL, BIO_MAX_PAGES);
+	struct bio *bio = bio_alloc_bioset(GFP_NOIO, BIO_MAX_PAGES, log->bs);
 
 	bio->bi_rw = WRITE;
 	bio->bi_bdev = log->rdev->bdev;
@@ -286,15 +290,19 @@ static struct r5l_io_unit *r5l_new_meta(struct r5l_log *log)
 	struct r5l_io_unit *io;
 	struct r5l_meta_block *block;
 
-	/* We can't handle memory allocate failure so far */
-	io = kmem_cache_zalloc(log->io_kc, GFP_NOIO | __GFP_NOFAIL);
+	io = mempool_alloc(log->io_pool, GFP_ATOMIC);
+	if (!io)
+		return NULL;
+	memset(io, 0, sizeof(*io));
+
 	io->log = log;
 	INIT_LIST_HEAD(&io->log_sibling);
 	INIT_LIST_HEAD(&io->stripe_list);
 	io->state = IO_UNIT_RUNNING;
 
-	io->meta_page = alloc_page(GFP_NOIO | __GFP_NOFAIL | __GFP_ZERO);
+	io->meta_page = mempool_alloc(log->meta_pool, GFP_NOIO);
 	block = page_address(io->meta_page);
+	clear_page(block);
 	block->magic = cpu_to_le32(R5LOG_MAGIC);
 	block->version = R5LOG_VERSION;
 	block->seq = cpu_to_le64(log->seq);
@@ -324,8 +332,12 @@ static int r5l_get_meta(struct r5l_log *log, unsigned int payload_size)
 	    log->current_io->meta_offset + payload_size > PAGE_SIZE)
 		r5l_submit_current_io(log);
 
-	if (!log->current_io)
+	if (!log->current_io) {
 		log->current_io = r5l_new_meta(log);
+		if (!log->current_io)
+			return -ENOMEM;
+	}
+
 	return 0;
 }
 
@@ -370,11 +382,12 @@ static void r5l_append_payload_page(struct r5l_log *log, struct page *page)
 	r5_reserve_log_entry(log, io);
 }
 
-static void r5l_log_stripe(struct r5l_log *log, struct stripe_head *sh,
+static int r5l_log_stripe(struct r5l_log *log, struct stripe_head *sh,
 			   int data_pages, int parity_pages)
 {
 	int i;
 	int meta_size;
+	int ret;
 	struct r5l_io_unit *io;
 
 	meta_size =
@@ -383,7 +396,10 @@ static void r5l_log_stripe(struct r5l_log *log, struct stripe_head *sh,
 		sizeof(struct r5l_payload_data_parity) +
 		sizeof(__le32) * parity_pages;
 
-	r5l_get_meta(log, meta_size);
+	ret = r5l_get_meta(log, meta_size);
+	if (ret)
+		return ret;
+
 	io = log->current_io;
 
 	for (i = 0; i < sh->disks; i++) {
@@ -413,6 +429,8 @@ static void r5l_log_stripe(struct r5l_log *log, struct stripe_head *sh,
 	list_add_tail(&sh->log_list, &io->stripe_list);
 	atomic_inc(&io->pending_stripe);
 	sh->log_io = io;
+
+	return 0;
 }
 
 static void r5l_wake_reclaim(struct r5l_log *log, sector_t space);
@@ -427,6 +445,7 @@ int r5l_write_stripe(struct r5l_log *log, struct stripe_head *sh)
 	int meta_size;
 	int reserve;
 	int i;
+	int ret = 0;
 
 	if (!log)
 		return -EAGAIN;
@@ -475,17 +494,22 @@ int r5l_write_stripe(struct r5l_log *log, struct stripe_head *sh)
 	mutex_lock(&log->io_mutex);
 	/* meta + data */
 	reserve = (1 + write_disks) << (PAGE_SHIFT - 9);
-	if (r5l_has_free_space(log, reserve))
-		r5l_log_stripe(log, sh, data_pages, parity_pages);
-	else {
+	if (!r5l_has_free_space(log, reserve)) {
 		spin_lock(&log->no_space_stripes_lock);
 		list_add_tail(&sh->log_list, &log->no_space_stripes);
 		spin_unlock(&log->no_space_stripes_lock);
 
 		r5l_wake_reclaim(log, reserve);
+	} else {
+		ret = r5l_log_stripe(log, sh, data_pages, parity_pages);
+		if (ret) {
+			spin_lock_irq(&log->io_list_lock);
+			list_add_tail(&sh->log_list, &log->no_mem_stripes);
+			spin_unlock_irq(&log->io_list_lock);
+		}
 	}
-	mutex_unlock(&log->io_mutex);
 
+	mutex_unlock(&log->io_mutex);
 	return 0;
 }
 
@@ -538,6 +562,21 @@ static sector_t r5l_reclaimable_space(struct r5l_log *log)
 				 log->next_checkpoint);
 }
 
+static void r5l_run_no_mem_stripe(struct r5l_log *log)
+{
+	struct stripe_head *sh;
+
+	assert_spin_locked(&log->io_list_lock);
+
+	if (!list_empty(&log->no_mem_stripes)) {
+		sh = list_first_entry(&log->no_mem_stripes,
+				      struct stripe_head, log_list);
+		list_del_init(&sh->log_list);
+		set_bit(STRIPE_HANDLE, &sh->state);
+		raid5_release_stripe(sh);
+	}
+}
+
 static bool r5l_complete_finished_ios(struct r5l_log *log)
 {
 	struct r5l_io_unit *io, *next;
@@ -554,7 +593,8 @@ static bool r5l_complete_finished_ios(struct r5l_log *log)
 		log->next_cp_seq = io->seq;
 
 		list_del(&io->log_sibling);
-		r5l_free_io_unit(log, io);
+		mempool_free(io, log->io_pool);
+		r5l_run_no_mem_stripe(log);
 
 		found = true;
 	}
@@ -787,6 +827,13 @@ void r5l_quiesce(struct r5l_log *log, int state)
 		return;
 	if (state == 0) {
 		log->in_teardown = 0;
+		/*
+		 * This is a special case for hotadd. In suspend, the array has
+		 * no journal. In resume, journal is initialized as well as the
+		 * reclaim thread.
+		 */
+		if (log->reclaim_thread)
+			return;
 		log->reclaim_thread = md_register_thread(r5l_reclaim_thread,
 					log->rdev->mddev, "reclaim");
 	} else if (state == 1) {
@@ -806,10 +853,18 @@ void r5l_quiesce(struct r5l_log *log, int state)
 
 bool r5l_log_disk_error(struct r5conf *conf)
 {
+	struct r5l_log *log;
+	bool ret;
 	/* don't allow write if journal disk is missing */
-	if (!conf->log)
-		return test_bit(MD_HAS_JOURNAL, &conf->mddev->flags);
-	return test_bit(Faulty, &conf->log->rdev->flags);
+	rcu_read_lock();
+	log = rcu_dereference(conf->log);
+
+	if (!log)
+		ret = test_bit(MD_HAS_JOURNAL, &conf->mddev->flags);
+	else
+		ret = test_bit(Faulty, &log->rdev->flags);
+	rcu_read_unlock();
+	return ret;
 }
 
 struct r5l_recovery_ctx {
@@ -1160,23 +1215,45 @@ int r5l_init_log(struct r5conf *conf, struct md_rdev *rdev)
 	if (!log->io_kc)
 		goto io_kc;
 
+	log->io_pool = mempool_create_slab_pool(R5L_POOL_SIZE, log->io_kc);
+	if (!log->io_pool)
+		goto io_pool;
+
+	log->bs = bioset_create(R5L_POOL_SIZE, 0);
+	if (!log->bs)
+		goto io_bs;
+
+	log->meta_pool = mempool_create_page_pool(R5L_POOL_SIZE, 0);
+	if (!log->meta_pool)
+		goto out_mempool;
+
 	log->reclaim_thread = md_register_thread(r5l_reclaim_thread,
 						 log->rdev->mddev, "reclaim");
 	if (!log->reclaim_thread)
 		goto reclaim_thread;
 	init_waitqueue_head(&log->iounit_wait);
 
+	INIT_LIST_HEAD(&log->no_mem_stripes);
+
 	INIT_LIST_HEAD(&log->no_space_stripes);
 	spin_lock_init(&log->no_space_stripes_lock);
 
 	if (r5l_load_log(log))
 		goto error;
 
-	conf->log = log;
+	rcu_assign_pointer(conf->log, log);
+	set_bit(MD_HAS_JOURNAL, &conf->mddev->flags);
 	return 0;
+
 error:
 	md_unregister_thread(&log->reclaim_thread);
 reclaim_thread:
+	mempool_destroy(log->meta_pool);
+out_mempool:
+	bioset_free(log->bs);
+io_bs:
+	mempool_destroy(log->io_pool);
+io_pool:
 	kmem_cache_destroy(log->io_kc);
 io_kc:
 	kfree(log);
@@ -1186,6 +1263,9 @@ io_kc:
 void r5l_exit_log(struct r5l_log *log)
 {
 	md_unregister_thread(&log->reclaim_thread);
+	mempool_destroy(log->meta_pool);
+	bioset_free(log->bs);
+	mempool_destroy(log->io_pool);
 	kmem_cache_destroy(log->io_kc);
 	kfree(log);
 }
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 704ef7fcfbf8..b4f02c9959f2 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -772,8 +772,6 @@ static void stripe_add_to_batch_list(struct r5conf *conf, struct stripe_head *sh
 	int hash;
 	int dd_idx;
 
-	if (!stripe_can_batch(sh))
-		return;
 	/* Don't cross chunks, so stripe pd_idx/qd_idx is the same */
 	tmp_sec = sh->sector;
 	if (!sector_div(tmp_sec, conf->chunk_sectors))
@@ -2498,7 +2496,7 @@ static void raid5_build_block(struct stripe_head *sh, int i, int previous)
 	dev->sector = raid5_compute_blocknr(sh, i, previous);
 }
 
-static void error(struct mddev *mddev, struct md_rdev *rdev)
+static void raid5_error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
 	struct r5conf *conf = mddev->private;
@@ -2960,7 +2958,7 @@ static int add_stripe_bio(struct stripe_head *sh, struct bio *bi, int dd_idx,
 	 * If several bio share a stripe. The bio bi_phys_segments acts as a
 	 * reference count to avoid race. The reference count should already be
 	 * increased before this function is called (for example, in
-	 * make_request()), so other bio sharing this stripe will not free the
+	 * raid5_make_request()), so other bio sharing this stripe will not free the
 	 * stripe. If a stripe is owned by one stripe, the stripe lock will
 	 * protect it.
 	 */
@@ -5137,7 +5135,7 @@ static void make_discard_request(struct mddev *mddev, struct bio *bi)
 	}
 }
 
-static void make_request(struct mddev *mddev, struct bio * bi)
+static void raid5_make_request(struct mddev *mddev, struct bio * bi)
 {
 	struct r5conf *conf = mddev->private;
 	int dd_idx;
@@ -5227,7 +5225,7 @@ static void make_request(struct mddev *mddev, struct bio * bi)
 		new_sector = raid5_compute_sector(conf, logical_sector,
 						  previous,
 						  &dd_idx, NULL);
-		pr_debug("raid456: make_request, sector %llu logical %llu\n",
+		pr_debug("raid456: raid5_make_request, sector %llu logical %llu\n",
 			(unsigned long long)new_sector,
 			(unsigned long long)logical_sector);
 
@@ -5577,7 +5575,8 @@ ret:
 	return retn;
 }
 
-static inline sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipped)
+static inline sector_t raid5_sync_request(struct mddev *mddev, sector_t sector_nr,
+					  int *skipped)
 {
 	struct r5conf *conf = mddev->private;
 	struct stripe_head *sh;
@@ -6676,7 +6675,7 @@ static int only_parity(int raid_disk, int algo, int raid_disks, int max_degraded
 	return 0;
 }
 
-static int run(struct mddev *mddev)
+static int raid5_run(struct mddev *mddev)
 {
 	struct r5conf *conf;
 	int working_disks = 0;
@@ -7050,7 +7049,7 @@ static void raid5_free(struct mddev *mddev, void *priv)
 	mddev->to_remove = &raid5_attrs_group;
 }
 
-static void status(struct seq_file *seq, struct mddev *mddev)
+static void raid5_status(struct seq_file *seq, struct mddev *mddev)
 {
 	struct r5conf *conf = mddev->private;
 	int i;
@@ -7141,14 +7140,19 @@ static int raid5_remove_disk(struct mddev *mddev, struct md_rdev *rdev)
 	struct disk_info *p = conf->disks + number;
 
 	print_raid5_conf(conf);
-	if (test_bit(Journal, &rdev->flags)) {
+	if (test_bit(Journal, &rdev->flags) && conf->log) {
+		struct r5l_log *log;
 		/*
-		 * journal disk is not removable, but we need give a chance to
-		 * update superblock of other disks. Otherwise journal disk
-		 * will be considered as 'fresh'
+		 * we can't wait pending write here, as this is called in
+		 * raid5d, wait will deadlock.
 		 */
-		set_bit(MD_CHANGE_DEVS, &mddev->flags);
-		return -EINVAL;
+		if (atomic_read(&mddev->writes_pending))
+			return -EBUSY;
+		log = conf->log;
+		conf->log = NULL;
+		synchronize_rcu();
+		r5l_exit_log(log);
+		return 0;
 	}
 	if (rdev == p->rdev)
 		rdevp = &p->rdev;
@@ -7212,8 +7216,21 @@ static int raid5_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 	int first = 0;
 	int last = conf->raid_disks - 1;
 
-	if (test_bit(Journal, &rdev->flags))
-		return -EINVAL;
+	if (test_bit(Journal, &rdev->flags)) {
+		char b[BDEVNAME_SIZE];
+		if (conf->log)
+			return -EBUSY;
+
+		rdev->raid_disk = 0;
+		/*
+		 * The array is in readonly mode if journal is missing, so no
+		 * write requests running. We should be safe
+		 */
+		r5l_init_log(conf, rdev);
+		printk(KERN_INFO"md/raid:%s: using device %s as journal\n",
+		       mdname(mddev), bdevname(rdev->bdev, b));
+		return 0;
+	}
 	if (mddev->recovery_disabled == conf->recovery_disabled)
 		return -EBUSY;
 
@@ -7848,15 +7865,15 @@ static struct md_personality raid6_personality =
 	.name		= "raid6",
 	.level		= 6,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= raid5_make_request,
+	.run		= raid5_run,
 	.free		= raid5_free,
-	.status		= status,
-	.error_handler	= error,
+	.status		= raid5_status,
+	.error_handler	= raid5_error,
 	.hot_add_disk	= raid5_add_disk,
 	.hot_remove_disk= raid5_remove_disk,
 	.spare_active	= raid5_spare_active,
-	.sync_request	= sync_request,
+	.sync_request	= raid5_sync_request,
 	.resize		= raid5_resize,
 	.size		= raid5_size,
 	.check_reshape	= raid6_check_reshape,
@@ -7871,15 +7888,15 @@ static struct md_personality raid5_personality =
 	.name		= "raid5",
 	.level		= 5,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= raid5_make_request,
+	.run		= raid5_run,
 	.free		= raid5_free,
-	.status		= status,
-	.error_handler	= error,
+	.status		= raid5_status,
+	.error_handler	= raid5_error,
 	.hot_add_disk	= raid5_add_disk,
 	.hot_remove_disk= raid5_remove_disk,
 	.spare_active	= raid5_spare_active,
-	.sync_request	= sync_request,
+	.sync_request	= raid5_sync_request,
 	.resize		= raid5_resize,
 	.size		= raid5_size,
 	.check_reshape	= raid5_check_reshape,
@@ -7895,15 +7912,15 @@ static struct md_personality raid4_personality =
 	.name		= "raid4",
 	.level		= 4,
 	.owner		= THIS_MODULE,
-	.make_request	= make_request,
-	.run		= run,
+	.make_request	= raid5_make_request,
+	.run		= raid5_run,
 	.free		= raid5_free,
-	.status		= status,
-	.error_handler	= error,
+	.status		= raid5_status,
+	.error_handler	= raid5_error,
 	.hot_add_disk	= raid5_add_disk,
 	.hot_remove_disk= raid5_remove_disk,
 	.spare_active	= raid5_spare_active,
-	.sync_request	= sync_request,
+	.sync_request	= raid5_sync_request,
 	.resize		= raid5_resize,
 	.size		= raid5_size,
 	.check_reshape	= raid5_check_reshape,
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index 3ef3d6c6bbf8..a8518fb3bca7 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -9,7 +9,7 @@ menuconfig MEDIA_SUPPORT
 	  If you want to use Webcams, Video grabber devices and/or TV devices
 	  enable this option and other options below.
 	  Additional info and docs are available on the web at
-	  <http://linuxtv.org>
+	  <https://linuxtv.org>
 
 if MEDIA_SUPPORT
 
@@ -51,7 +51,7 @@ config MEDIA_RADIO_SUPPORT
 	  Enable AM/FM radio support.
 
 	  Additional info and docs are available on the web at
-	  <http://linuxtv.org>
+	  <https://linuxtv.org>
 
 	  Say Y when you have a board with radio support.
 
@@ -97,7 +97,6 @@ config MEDIA_CONTROLLER
 config MEDIA_CONTROLLER_DVB
 	bool "Enable Media controller for DVB (EXPERIMENTAL)"
 	depends on MEDIA_CONTROLLER
-	depends on BROKEN
 	---help---
 	  Enable the media controller API support for DVB.
 
diff --git a/drivers/media/common/cx2341x.c b/drivers/media/common/cx2341x.c
index c07b9db51b05..5e4afa0131e6 100644
--- a/drivers/media/common/cx2341x.c
+++ b/drivers/media/common/cx2341x.c
@@ -27,7 +27,7 @@
 #include <linux/videodev2.h>
 
 #include <media/tuner.h>
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 #include <media/v4l2-common.h>
 
 MODULE_DESCRIPTION("cx23415/6/8 driver");
diff --git a/drivers/media/common/saa7146/saa7146_core.c b/drivers/media/common/saa7146/saa7146_core.c
index 1ff9f5323bc3..9f7c5b0a6b45 100644
--- a/drivers/media/common/saa7146/saa7146_core.c
+++ b/drivers/media/common/saa7146/saa7146_core.c
@@ -20,7 +20,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <media/saa7146.h>
+#include <media/drv-intf/saa7146.h>
 #include <linux/module.h>
 
 static int saa7146_num;
diff --git a/drivers/media/common/saa7146/saa7146_fops.c b/drivers/media/common/saa7146/saa7146_fops.c
index df1e8c975cd8..930d2c94d5d3 100644
--- a/drivers/media/common/saa7146/saa7146_fops.c
+++ b/drivers/media/common/saa7146/saa7146_fops.c
@@ -1,6 +1,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <linux/module.h>
 
 /****************************************************************************/
diff --git a/drivers/media/common/saa7146/saa7146_hlp.c b/drivers/media/common/saa7146/saa7146_hlp.c
index 3dc6a838ca6f..6ebcbc6450f5 100644
--- a/drivers/media/common/saa7146/saa7146_hlp.c
+++ b/drivers/media/common/saa7146/saa7146_hlp.c
@@ -2,7 +2,7 @@
 
 #include <linux/kernel.h>
 #include <linux/export.h>
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 
 static void calculate_output_format_register(struct saa7146_dev* saa, u32 palette, u32* clip_format)
 {
diff --git a/drivers/media/common/saa7146/saa7146_i2c.c b/drivers/media/common/saa7146/saa7146_i2c.c
index 22027198129d..239a2db35068 100644
--- a/drivers/media/common/saa7146/saa7146_i2c.c
+++ b/drivers/media/common/saa7146/saa7146_i2c.c
@@ -1,6 +1,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 
 static u32 saa7146_i2c_func(struct i2c_adapter *adapter)
 {
diff --git a/drivers/media/common/saa7146/saa7146_vbi.c b/drivers/media/common/saa7146/saa7146_vbi.c
index 2da995758778..49237518d65f 100644
--- a/drivers/media/common/saa7146/saa7146_vbi.c
+++ b/drivers/media/common/saa7146/saa7146_vbi.c
@@ -1,4 +1,4 @@
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 
 static int vbi_pixel_to_capture = 720 * 2;
 
diff --git a/drivers/media/common/saa7146/saa7146_video.c b/drivers/media/common/saa7146/saa7146_video.c
index 30779498c173..ea2f3bf7368b 100644
--- a/drivers/media/common/saa7146/saa7146_video.c
+++ b/drivers/media/common/saa7146/saa7146_video.c
@@ -1,6 +1,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-ctrls.h>
 #include <linux/module.h>
@@ -502,7 +502,7 @@ static int vidioc_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuf
 	/* check if overlay is running */
 	if (IS_OVERLAY_ACTIVE(fh) != 0) {
 		if (vv->video_fh != fh) {
-			DEB_D("refusing to change framebuffer informations while overlay is active in another open\n");
+			DEB_D("refusing to change framebuffer information while overlay is active in another open\n");
 			return -EBUSY;
 		}
 	}
diff --git a/drivers/media/common/siano/smsdvb-main.c b/drivers/media/common/siano/smsdvb-main.c
index f4305ae800f4..d31f468830cf 100644
--- a/drivers/media/common/siano/smsdvb-main.c
+++ b/drivers/media/common/siano/smsdvb-main.c
@@ -617,6 +617,7 @@ static void smsdvb_media_device_unregister(struct smsdvb_client_t *client)
 	if (!coredev->media_dev)
 		return;
 	media_device_unregister(coredev->media_dev);
+	media_device_cleanup(coredev->media_dev);
 	kfree(coredev->media_dev);
 	coredev->media_dev = NULL;
 #endif
@@ -1183,7 +1184,11 @@ static int smsdvb_hotplug(struct smscore_device_t *coredev,
 	if (smsdvb_debugfs_create(client) < 0)
 		pr_info("failed to create debugfs node\n");
 
-	dvb_create_media_graph(&client->adapter);
+	rc = dvb_create_media_graph(&client->adapter, true);
+	if (rc < 0) {
+		pr_err("dvb_create_media_graph failed %d\n", rc);
+		goto client_error;
+	}
 
 	pr_info("DVB interface registered.\n");
 	return 0;
diff --git a/drivers/media/common/siano/smsir.h b/drivers/media/common/siano/smsir.h
index fc8b7925c532..d9abd96ef48b 100644
--- a/drivers/media/common/siano/smsir.h
+++ b/drivers/media/common/siano/smsir.h
@@ -30,8 +30,6 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #include <linux/input.h>
 #include <media/rc-core.h>
 
-#define IR_DEFAULT_TIMEOUT		100
-
 struct smscore_device_t;
 
 struct ir_t {
diff --git a/drivers/media/dvb-core/demux.h b/drivers/media/dvb-core/demux.h
index ccc1f43cb9a9..6d3b95b8939d 100644
--- a/drivers/media/dvb-core/demux.h
+++ b/drivers/media/dvb-core/demux.h
@@ -32,6 +32,49 @@
 #include <linux/time.h>
 #include <linux/dvb/dmx.h>
 
+/**
+ * DOC: Digital TV Demux
+ *
+ * The Kernel Digital TV Demux kABI defines a driver-internal interface for
+ * registering low-level, hardware specific driver to a hardware independent
+ * demux layer. It is only of interest for Digital TV device driver writers.
+ * The header file for this kABI is named demux.h and located in
+ * drivers/media/dvb-core.
+ *
+ * The demux kABI should be implemented for each demux in the system. It is
+ * used to select the TS source of a demux and to manage the demux resources.
+ * When the demux client allocates a resource via the demux kABI, it receives
+ * a pointer to the kABI of that resource.
+ *
+ * Each demux receives its TS input from a DVB front-end or from memory, as
+ * set via this demux kABI. In a system with more than one front-end, the kABI
+ * can be used to select one of the DVB front-ends as a TS source for a demux,
+ * unless this is fixed in the HW platform.
+ *
+ * The demux kABI only controls front-ends regarding to their connections with
+ * demuxes; the kABI used to set the other front-end parameters, such as
+ * tuning, are devined via the Digital TV Frontend kABI.
+ *
+ * The functions that implement the abstract interface demux should be defined
+ * static or module private and registered to the Demux core for external
+ * access. It is not necessary to implement every function in the struct
+ * &dmx_demux. For example, a demux interface might support Section filtering,
+ * but not PES filtering. The kABI client is expected to check the value of any
+ * function pointer before calling the function: the value of NULL means
+ * that the function is not available.
+ *
+ * Whenever the functions of the demux API modify shared data, the
+ * possibilities of lost update and race condition problems should be
+ * addressed, e.g. by protecting parts of code with mutexes.
+ *
+ * Note that functions called from a bottom half context must not sleep.
+ * Even a simple memory allocation without using %GFP_ATOMIC can result in a
+ * kernel thread being put to sleep if swapping is needed. For example, the
+ * Linux Kernel calls the functions of a network device interface from a
+ * bottom half context. Thus, if a demux kABI function is called from network
+ * device code, the function must not sleep.
+ */
+
 /*
  * Common definitions
  */
@@ -187,8 +230,28 @@ struct dmx_section_feed {
 	int (*stop_filtering)(struct dmx_section_feed *feed);
 };
 
-/*
- * Callback functions
+/**
+ * DOC: Demux Callback
+ *
+ * This kernel-space API comprises the callback functions that deliver filtered
+ * data to the demux client. Unlike the other DVB kABIs, these functions are
+ * provided by the client and called from the demux code.
+ *
+ * The function pointers of this abstract interface are not packed into a
+ * structure as in the other demux APIs, because the callback functions are
+ * registered and used independent of each other. As an example, it is possible
+ * for the API client to provide several callback functions for receiving TS
+ * packets and no callbacks for PES packets or sections.
+ *
+ * The functions that implement the callback API need not be re-entrant: when
+ * a demux driver calls one of these functions, the driver is not allowed to
+ * call the function again before the original call returns. If a callback is
+ * triggered by a hardware interrupt, it is recommended to use the Linux
+ * bottom half mechanism or start a tasklet instead of making the callback
+ * function call directly from a hardware interrupt.
+ *
+ * This mechanism is implemented by dmx_ts_cb() and dmx_section_cb()
+ * callbacks.
  */
 
 /**
diff --git a/drivers/media/dvb-core/dmxdev.c b/drivers/media/dvb-core/dmxdev.c
index ea9abde902e9..a168cbe1c998 100644
--- a/drivers/media/dvb-core/dmxdev.c
+++ b/drivers/media/dvb-core/dmxdev.c
@@ -1244,9 +1244,9 @@ int dvb_dmxdev_init(struct dmxdev *dmxdev, struct dvb_adapter *dvb_adapter)
 	}
 
 	dvb_register_device(dvb_adapter, &dmxdev->dvbdev, &dvbdev_demux, dmxdev,
-			    DVB_DEVICE_DEMUX);
+			    DVB_DEVICE_DEMUX, dmxdev->filternum);
 	dvb_register_device(dvb_adapter, &dmxdev->dvr_dvbdev, &dvbdev_dvr,
-			    dmxdev, DVB_DEVICE_DVR);
+			    dmxdev, DVB_DEVICE_DVR, dmxdev->filternum);
 
 	dvb_ringbuffer_init(&dmxdev->dvr_buffer, NULL, 8192);
 
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 0a46580b5376..1c1c298d2289 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -389,4 +389,5 @@
 #define USB_PID_PCTV_2002E_SE                           0x025d
 #define USB_PID_SVEON_STV27                             0xd3af
 #define USB_PID_TURBOX_DTT_2000                         0xd3a4
+#define USB_PID_WINTV_SOLOHD                            0x0264
 #endif
diff --git a/drivers/media/dvb-core/dvb_ca_en50221.c b/drivers/media/dvb-core/dvb_ca_en50221.c
index fb66184dc9b6..f82cd1ff4f3a 100644
--- a/drivers/media/dvb-core/dvb_ca_en50221.c
+++ b/drivers/media/dvb-core/dvb_ca_en50221.c
@@ -1695,7 +1695,7 @@ int dvb_ca_en50221_init(struct dvb_adapter *dvb_adapter,
 	pubca->private = ca;
 
 	/* register the DVB device */
-	ret = dvb_register_device(dvb_adapter, &ca->dvbdev, &dvbdev_ca, ca, DVB_DEVICE_CA);
+	ret = dvb_register_device(dvb_adapter, &ca->dvbdev, &dvbdev_ca, ca, DVB_DEVICE_CA, 0);
 	if (ret)
 		goto free_slot_info;
 
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index c38ef1a72b4a..40080645341e 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -622,7 +622,7 @@ static int dvb_enable_media_tuner(struct dvb_frontend *fe)
 	struct media_device *mdev = adapter->mdev;
 	struct media_entity  *entity, *source;
 	struct media_link *link, *found_link = NULL;
-	int i, ret, n_links = 0, active_links = 0;
+	int ret, n_links = 0, active_links = 0;
 
 	fepriv->pipe_start_entity = NULL;
 
@@ -632,8 +632,7 @@ static int dvb_enable_media_tuner(struct dvb_frontend *fe)
 	entity = fepriv->dvbdev->entity;
 	fepriv->pipe_start_entity = entity;
 
-	for (i = 0; i < entity->num_links; i++) {
-		link = &entity->links[i];
+	list_for_each_entry(link, &entity->links, list) {
 		if (link->sink->entity == entity) {
 			found_link = link;
 			n_links++;
@@ -659,13 +658,11 @@ static int dvb_enable_media_tuner(struct dvb_frontend *fe)
 
 	source = found_link->source->entity;
 	fepriv->pipe_start_entity = source;
-	for (i = 0; i < source->num_links; i++) {
+	list_for_each_entry(link, &source->links, list) {
 		struct media_entity *sink;
 		int flags = 0;
 
-		link = &source->links[i];
 		sink = link->sink->entity;
-
 		if (sink == entity)
 			flags = MEDIA_LNK_FL_ENABLED;
 
@@ -891,21 +888,21 @@ static void dvb_frontend_stop(struct dvb_frontend *fe)
 }
 
 /*
- * Sleep until gettimeofday() > waketime + add_usec
- * This needs to be as precise as possible, but as the delay is
- * usually between 2ms and 32ms, it is done using a scheduled msleep
- * followed by usleep (normally a busy-wait loop) for the remainder
+ * Sleep for the amount of time given by add_usec parameter
+ *
+ * This needs to be as precise as possible, as it affects the detection of
+ * the dish tone command at the satellite subsystem. The precision is improved
+ * by using a scheduled msleep followed by udelay for the remainder.
  */
 void dvb_frontend_sleep_until(ktime_t *waketime, u32 add_usec)
 {
-	s32 delta, newdelta;
+	s32 delta;
 
-	ktime_add_us(*waketime, add_usec);
+	*waketime = ktime_add_us(*waketime, add_usec);
 	delta = ktime_us_delta(ktime_get_real(), *waketime);
 	if (delta > 2500) {
 		msleep((delta - 1500) / 1000);
-		newdelta = ktime_us_delta(ktime_get_real(), *waketime);
-		delta = (newdelta > delta) ? 0 : newdelta;
+		delta = ktime_us_delta(ktime_get_real(), *waketime);
 	}
 	if (delta > 0)
 		udelay(delta);
@@ -2313,9 +2310,9 @@ static int dvb_frontend_ioctl_legacy(struct file *file,
 		dev_dbg(fe->dvb->device, "%s: current delivery system on cache: %d, V3 type: %d\n",
 				 __func__, c->delivery_system, fe->ops.info.type);
 
-		/* Force the CAN_INVERSION_AUTO bit on. If the frontend doesn't
-		 * do it, it is done for it. */
-		info->caps |= FE_CAN_INVERSION_AUTO;
+		/* Set CAN_INVERSION_AUTO bit on in other than oneshot mode */
+		if (!(fepriv->tune_mode_flags & FE_TUNE_MODE_ONESHOT))
+			info->caps |= FE_CAN_INVERSION_AUTO;
 		err = 0;
 		break;
 	}
@@ -2710,6 +2707,11 @@ int dvb_frontend_resume(struct dvb_frontend *fe)
 	else if (fe->ops.tuner_ops.init)
 		ret = fe->ops.tuner_ops.init(fe);
 
+	if (fe->ops.set_tone && fepriv->tone != -1)
+		fe->ops.set_tone(fe, fepriv->tone);
+	if (fe->ops.set_voltage && fepriv->voltage != -1)
+		fe->ops.set_voltage(fe, fepriv->voltage);
+
 	fe->exit = DVB_FE_NO_EXIT;
 	fepriv->state = FESTATE_RETUNE;
 	dvb_frontend_wakeup(fe);
@@ -2757,7 +2759,7 @@ int dvb_register_frontend(struct dvb_adapter* dvb,
 			fe->dvb->num, fe->id, fe->ops.info.name);
 
 	dvb_register_device (fe->dvb, &fepriv->dvbdev, &dvbdev_template,
-			     fe, DVB_DEVICE_FRONTEND);
+			     fe, DVB_DEVICE_FRONTEND, 0);
 
 	/*
 	 * Initialize the cache to the proper values according with the
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index 97661b2f247a..458bcce20e38 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -42,6 +42,29 @@
 
 #include "dvbdev.h"
 
+/**
+ * DOC: Digital TV Frontend
+ *
+ * The Digital TV Frontend kABI defines a driver-internal interface for
+ * registering low-level, hardware specific driver to a hardware independent
+ * frontend layer. It is only of interest for Digital TV device driver writers.
+ * The header file for this API is named dvb_frontend.h and located in
+ * drivers/media/dvb-core.
+ *
+ * Before using the Digital TV frontend core, the bridge driver should attach
+ * the frontend demod, tuner and SEC devices and call dvb_register_frontend(),
+ * in order to register the new frontend at the subsystem. At device
+ * detach/removal, the bridge driver should call dvb_unregister_frontend() to
+ * remove the frontend from the core and then dvb_frontend_detach() to free the
+ * memory allocated by the frontend drivers.
+ *
+ * The drivers should also call dvb_frontend_suspend() as part of their
+ * handler for the &device_driver.suspend(), and dvb_frontend_resume() as
+ * part of their handler for &device_driver.resume().
+ *
+ * A few other optional functions are provided to handle some special cases.
+ */
+
 /*
  * Maximum number of Delivery systems per frontend. It
  * should be smaller or equal to 32
@@ -112,16 +135,6 @@ struct analog_parameters {
 	u64 std;
 };
 
-enum tuner_param {
-	DVBFE_TUNER_FREQUENCY		= (1 <<  0),
-	DVBFE_TUNER_TUNERSTEP		= (1 <<  1),
-	DVBFE_TUNER_IFFREQ		= (1 <<  2),
-	DVBFE_TUNER_BANDWIDTH		= (1 <<  3),
-	DVBFE_TUNER_REFCLOCK		= (1 <<  4),
-	DVBFE_TUNER_IQSENSE		= (1 <<  5),
-	DVBFE_TUNER_DUMMY		= (1 << 31)
-};
-
 /**
  * enum dvbfe_algo - defines the algorithm used to tune into a channel
  *
@@ -152,15 +165,6 @@ enum dvbfe_algo {
 	DVBFE_ALGO_RECOVERY		= (1 << 31)
 };
 
-struct tuner_state {
-	u32 frequency;
-	u32 tunerstep;
-	u32 ifreq;
-	u32 bandwidth;
-	u32 iqsense;
-	u32 refclock;
-};
-
 /**
  * enum dvbfe_search - search callback possible return status
  *
@@ -209,12 +213,12 @@ enum dvbfe_search {
  *			are stored at @dvb_frontend.dtv_property_cache;. The
  *			tuner demod can change the parameters to reflect the
  *			changes needed for the channel to be tuned, and
- *			update statistics.
+ *			update statistics. This is the recommended way to set
+ *			the tuner parameters and should be used on newer
+ *			drivers.
  * @set_analog_params:	callback function used to tune into an analog TV
  *			channel on hybrid tuners. It passes @analog_parameters;
  *			to the driver.
- * @calc_regs:		callback function used to pass register data settings
- *			for simple tuners.
  * @set_config:		callback function used to send some tuner-specific
  *			parameters.
  * @get_frequency:	get the actual tuned frequency
@@ -227,16 +231,10 @@ enum dvbfe_search {
  *			via DVBv5 API (@dvb_frontend.dtv_property_cache;).
  * @get_afc:		Used only by analog TV core. Reports the frequency
  *			drift due to AFC.
- * @set_frequency:	Set a new frequency. Please notice that using
- *			set_params is preferred.
- * @set_bandwidth:	Set a new frequency. Please notice that using
- *			set_params is preferred.
- * @set_state:		callback function used on some legacy drivers that
- * 			don't implement set_params in order to set properties.
- * 			Shouldn't be used on new drivers.
- * @get_state:		callback function used to get properties by some
- * 			legacy drivers that don't implement set_params.
- * 			Shouldn't be used on new drivers.
+ * @calc_regs:		callback function used to pass register data settings
+ *			for simple tuners.  Shouldn't be used on newer drivers.
+ * @set_frequency:	Set a new frequency. Shouldn't be used on newer drivers.
+ * @set_bandwidth:	Set a new frequency. Shouldn't be used on newer drivers.
  *
  * NOTE: frequencies used on get_frequency and set_frequency are in Hz for
  * terrestrial/cable or kHz for satellite.
@@ -252,14 +250,10 @@ struct dvb_tuner_ops {
 	int (*suspend)(struct dvb_frontend *fe);
 	int (*resume)(struct dvb_frontend *fe);
 
-	/** This is for simple PLLs - set all parameters in one go. */
+	/* This is the recomended way to set the tuner */
 	int (*set_params)(struct dvb_frontend *fe);
 	int (*set_analog_params)(struct dvb_frontend *fe, struct analog_parameters *p);
 
-	/** This is support for demods like the mt352 - fills out the supplied buffer with what to write. */
-	int (*calc_regs)(struct dvb_frontend *fe, u8 *buf, int buf_len);
-
-	/** This is to allow setting tuner-specific configs */
 	int (*set_config)(struct dvb_frontend *fe, void *priv_cfg);
 
 	int (*get_frequency)(struct dvb_frontend *fe, u32 *frequency);
@@ -272,17 +266,23 @@ struct dvb_tuner_ops {
 	int (*get_rf_strength)(struct dvb_frontend *fe, u16 *strength);
 	int (*get_afc)(struct dvb_frontend *fe, s32 *afc);
 
-	/** These are provided separately from set_params in order to facilitate silicon
-	 * tuners which require sophisticated tuning loops, controlling each parameter separately. */
-	int (*set_frequency)(struct dvb_frontend *fe, u32 frequency);
-	int (*set_bandwidth)(struct dvb_frontend *fe, u32 bandwidth);
+	/*
+	 * This is support for demods like the mt352 - fills out the supplied
+	 * buffer with what to write.
+	 *
+	 * Don't use on newer drivers.
+	 */
+	int (*calc_regs)(struct dvb_frontend *fe, u8 *buf, int buf_len);
 
 	/*
-	 * These are provided separately from set_params in order to facilitate silicon
-	 * tuners which require sophisticated tuning loops, controlling each parameter separately.
+	 * These are provided separately from set_params in order to
+	 * facilitate silicon tuners which require sophisticated tuning loops,
+	 * controlling each parameter separately.
+	 *
+	 * Don't use on newer drivers.
 	 */
-	int (*set_state)(struct dvb_frontend *fe, enum tuner_param param, struct tuner_state *state);
-	int (*get_state)(struct dvb_frontend *fe, enum tuner_param param, struct tuner_state *state);
+	int (*set_frequency)(struct dvb_frontend *fe, u32 frequency);
+	int (*set_bandwidth)(struct dvb_frontend *fe, u32 bandwidth);
 };
 
 /**
@@ -404,6 +404,11 @@ struct dtv_frontend_properties;
  *			FE_ENABLE_HIGH_LNB_VOLTAGE ioctl (only Satellite).
  * @dishnetwork_send_legacy_command: callback function to implement the
  *			FE_DISHNETWORK_SEND_LEGACY_CMD ioctl (only Satellite).
+ *			Drivers should not use this, except when the DVB
+ *			core emulation fails to provide proper support (e.g.
+ *			if set_voltage() takes more than 8ms to work), and
+ *			when backward compatibility with this legacy API is
+ *			required.
  * @i2c_gate_ctrl:	controls the I2C gate. Newer drivers should use I2C
  *			mux support instead.
  * @ts_bus_ctrl:	callback function used to take control of the TS bus.
@@ -466,7 +471,8 @@ struct dvb_frontend_ops {
 	int (*ts_bus_ctrl)(struct dvb_frontend* fe, int acquire);
 	int (*set_lna)(struct dvb_frontend *);
 
-	/* These callbacks are for devices that implement their own
+	/*
+	 * These callbacks are for devices that implement their own
 	 * tuning algorithms, rather than a simple swzigzag
 	 */
 	enum dvbfe_search (*search)(struct dvb_frontend *fe);
@@ -682,17 +688,126 @@ struct dvb_frontend {
 	unsigned int exit;
 };
 
-extern int dvb_register_frontend(struct dvb_adapter *dvb,
+/**
+ * dvb_register_frontend() - Registers a DVB frontend at the adapter
+ *
+ * @dvb: pointer to the dvb adapter
+ * @fe: pointer to the frontend struct
+ *
+ * Allocate and initialize the private data needed by the frontend core to
+ * manage the frontend and calls dvb_register_device() to register a new
+ * frontend. It also cleans the property cache that stores the frontend
+ * parameters and selects the first available delivery system.
+ */
+int dvb_register_frontend(struct dvb_adapter *dvb,
 				 struct dvb_frontend *fe);
 
-extern int dvb_unregister_frontend(struct dvb_frontend *fe);
+/**
+ * dvb_unregister_frontend() - Unregisters a DVB frontend
+ *
+ * @fe: pointer to the frontend struct
+ *
+ * Stops the frontend kthread, calls dvb_unregister_device() and frees the
+ * private frontend data allocated by dvb_register_frontend().
+ *
+ * NOTE: This function doesn't frees the memory allocated by the demod,
+ * by the SEC driver and by the tuner. In order to free it, an explicit call to
+ * dvb_frontend_detach() is needed, after calling this function.
+ */
+int dvb_unregister_frontend(struct dvb_frontend *fe);
 
-extern void dvb_frontend_detach(struct dvb_frontend *fe);
+/**
+ * dvb_frontend_detach() - Detaches and frees frontend specific data
+ *
+ * @fe: pointer to the frontend struct
+ *
+ * This function should be called after dvb_unregister_frontend(). It
+ * calls the SEC, tuner and demod release functions:
+ * &dvb_frontend_ops.release_sec, &dvb_frontend_ops.tuner_ops.release,
+ * &dvb_frontend_ops.analog_ops.release and &dvb_frontend_ops.release.
+ *
+ * If the driver is compiled with CONFIG_MEDIA_ATTACH, it also decreases
+ * the module reference count, needed to allow userspace to remove the
+ * previously used DVB frontend modules.
+ */
+void dvb_frontend_detach(struct dvb_frontend *fe);
+
+/**
+ * dvb_frontend_suspend() - Suspends a Digital TV frontend
+ *
+ * @fe: pointer to the frontend struct
+ *
+ * This function prepares a Digital TV frontend to suspend.
+ *
+ * In order to prepare the tuner to suspend, if
+ * &dvb_frontend_ops.tuner_ops.suspend() is available, it calls it. Otherwise,
+ * it will call &dvb_frontend_ops.tuner_ops.sleep(), if available.
+ *
+ * It will also call &dvb_frontend_ops.sleep() to put the demod to suspend.
+ *
+ * The drivers should also call dvb_frontend_suspend() as part of their
+ * handler for the &device_driver.suspend().
+ */
+int dvb_frontend_suspend(struct dvb_frontend *fe);
+
+/**
+ * dvb_frontend_resume() - Resumes a Digital TV frontend
+ *
+ * @fe: pointer to the frontend struct
+ *
+ * This function resumes the usual operation of the tuner after resume.
+ *
+ * In order to resume the frontend, it calls the demod &dvb_frontend_ops.init().
+ *
+ * If &dvb_frontend_ops.tuner_ops.resume() is available, It, it calls it.
+ * Otherwise,t will call &dvb_frontend_ops.tuner_ops.init(), if available.
+ *
+ * Once tuner and demods are resumed, it will enforce that the SEC voltage and
+ * tone are restored to their previous values and wake up the frontend's
+ * kthread in order to retune the frontend.
+ *
+ * The drivers should also call dvb_frontend_resume() as part of their
+ * handler for the &device_driver.resume().
+ */
+int dvb_frontend_resume(struct dvb_frontend *fe);
 
-extern void dvb_frontend_reinitialise(struct dvb_frontend *fe);
-extern int dvb_frontend_suspend(struct dvb_frontend *fe);
-extern int dvb_frontend_resume(struct dvb_frontend *fe);
+/**
+ * dvb_frontend_reinitialise() - forces a reinitialisation at the frontend
+ *
+ * @fe: pointer to the frontend struct
+ *
+ * Calls &dvb_frontend_ops.init() and &dvb_frontend_ops.tuner_ops.init(),
+ * and resets SEC tone and voltage (for Satellite systems).
+ *
+ * NOTE: Currently, this function is used only by one driver (budget-av).
+ * It seems to be due to address some special issue with that specific
+ * frontend.
+ */
+void dvb_frontend_reinitialise(struct dvb_frontend *fe);
 
-extern void dvb_frontend_sleep_until(ktime_t *waketime, u32 add_usec);
+/**
+ * dvb_frontend_sleep_until() - Sleep for the amount of time given by
+ *                      add_usec parameter
+ *
+ * @waketime: pointer to a struct ktime_t
+ * @add_usec: time to sleep, in microseconds
+ *
+ * This function is used to measure the time required for the
+ * %FE_DISHNETWORK_SEND_LEGACY_CMD ioctl to work. It needs to be as precise
+ * as possible, as it affects the detection of the dish tone command at the
+ * satellite subsystem.
+ *
+ * Its used internally by the DVB frontend core, in order to emulate
+ * %FE_DISHNETWORK_SEND_LEGACY_CMD using the &dvb_frontend_ops.set_voltage()
+ * callback.
+ *
+ * NOTE: it should not be used at the drivers, as the emulation for the
+ * legacy callback is provided by the Kernel. The only situation where this
+ * should be at the drivers is when there are some bugs at the hardware that
+ * would prevent the core emulation to work. On such cases, the driver would
+ * be writing a &dvb_frontend_ops.dishnetwork_send_legacy_command() and
+ * calling this function directly.
+ */
+void dvb_frontend_sleep_until(ktime_t *waketime, u32 add_usec);
 
 #endif
diff --git a/drivers/media/dvb-core/dvb_net.c b/drivers/media/dvb-core/dvb_net.c
index ce4332e80a91..ce6a711b42d4 100644
--- a/drivers/media/dvb-core/dvb_net.c
+++ b/drivers/media/dvb-core/dvb_net.c
@@ -1502,6 +1502,6 @@ int dvb_net_init (struct dvb_adapter *adap, struct dvb_net *dvbnet,
 		dvbnet->state[i] = 0;
 
 	return dvb_register_device(adap, &dvbnet->dvbdev, &dvbdev_net,
-			     dvbnet, DVB_DEVICE_NET);
+			     dvbnet, DVB_DEVICE_NET, 0);
 }
 EXPORT_SYMBOL(dvb_net_init);
diff --git a/drivers/media/dvb-core/dvbdev.c b/drivers/media/dvb-core/dvbdev.c
index 13bb57f0457f..560450a0b32a 100644
--- a/drivers/media/dvb-core/dvbdev.c
+++ b/drivers/media/dvb-core/dvbdev.c
@@ -34,6 +34,9 @@
 #include <linux/mutex.h>
 #include "dvbdev.h"
 
+/* Due to enum tuner_pad_index */
+#include <media/tuner.h>
+
 static DEFINE_MUTEX(dvbdev_mutex);
 static int dvbdev_debug;
 
@@ -180,102 +183,255 @@ skip:
 	return -ENFILE;
 }
 
-static void dvb_register_media_device(struct dvb_device *dvbdev,
-				      int type, int minor)
+static void dvb_media_device_free(struct dvb_device *dvbdev)
 {
 #if defined(CONFIG_MEDIA_CONTROLLER_DVB)
-	int ret = 0, npads;
+	if (dvbdev->entity) {
+		media_device_unregister_entity(dvbdev->entity);
+		kfree(dvbdev->entity);
+		kfree(dvbdev->pads);
+		dvbdev->entity = NULL;
+		dvbdev->pads = NULL;
+	}
 
-	if (!dvbdev->adapter->mdev)
-		return;
+	if (dvbdev->tsout_entity) {
+		int i;
 
-	dvbdev->entity = kzalloc(sizeof(*dvbdev->entity), GFP_KERNEL);
-	if (!dvbdev->entity)
-		return;
+		for (i = 0; i < dvbdev->tsout_num_entities; i++) {
+			media_device_unregister_entity(&dvbdev->tsout_entity[i]);
+			kfree(dvbdev->tsout_entity[i].name);
+		}
+		kfree(dvbdev->tsout_entity);
+		kfree(dvbdev->tsout_pads);
+		dvbdev->tsout_entity = NULL;
+		dvbdev->tsout_pads = NULL;
 
-	dvbdev->entity->info.dev.major = DVB_MAJOR;
-	dvbdev->entity->info.dev.minor = minor;
-	dvbdev->entity->name = dvbdev->name;
+		dvbdev->tsout_num_entities = 0;
+	}
+
+	if (dvbdev->intf_devnode) {
+		media_devnode_remove(dvbdev->intf_devnode);
+		dvbdev->intf_devnode = NULL;
+	}
+
+	if (dvbdev->adapter->conn) {
+		media_device_unregister_entity(dvbdev->adapter->conn);
+		dvbdev->adapter->conn = NULL;
+		kfree(dvbdev->adapter->conn_pads);
+		dvbdev->adapter->conn_pads = NULL;
+	}
+#endif
+}
+
+#if defined(CONFIG_MEDIA_CONTROLLER_DVB)
+static int dvb_create_tsout_entity(struct dvb_device *dvbdev,
+				    const char *name, int npads)
+{
+	int i, ret = 0;
+
+	dvbdev->tsout_pads = kcalloc(npads, sizeof(*dvbdev->tsout_pads),
+				     GFP_KERNEL);
+	if (!dvbdev->tsout_pads)
+		return -ENOMEM;
+
+	dvbdev->tsout_entity = kcalloc(npads, sizeof(*dvbdev->tsout_entity),
+				       GFP_KERNEL);
+	if (!dvbdev->tsout_entity)
+		return -ENOMEM;
+
+	dvbdev->tsout_num_entities = npads;
+
+	for (i = 0; i < npads; i++) {
+		struct media_pad *pads = &dvbdev->tsout_pads[i];
+		struct media_entity *entity = &dvbdev->tsout_entity[i];
+
+		entity->name = kasprintf(GFP_KERNEL, "%s #%d", name, i);
+		if (!entity->name)
+			return -ENOMEM;
+
+		entity->function = MEDIA_ENT_F_IO_DTV;
+		pads->flags = MEDIA_PAD_FL_SINK;
+
+		ret = media_entity_pads_init(entity, 1, pads);
+		if (ret < 0)
+			return ret;
+
+		ret = media_device_register_entity(dvbdev->adapter->mdev,
+						   entity);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+#define DEMUX_TSOUT	"demux-tsout"
+#define DVR_TSOUT	"dvr-tsout"
+
+static int dvb_create_media_entity(struct dvb_device *dvbdev,
+				   int type, int demux_sink_pads)
+{
+	int i, ret, npads;
 
 	switch (type) {
-	case DVB_DEVICE_CA:
-	case DVB_DEVICE_DEMUX:
 	case DVB_DEVICE_FRONTEND:
 		npads = 2;
 		break;
-	case DVB_DEVICE_NET:
-		npads = 0;
+	case DVB_DEVICE_DVR:
+		ret = dvb_create_tsout_entity(dvbdev, DVR_TSOUT,
+					      demux_sink_pads);
+		return ret;
+	case DVB_DEVICE_DEMUX:
+		npads = 1 + demux_sink_pads;
+		ret = dvb_create_tsout_entity(dvbdev, DEMUX_TSOUT,
+					      demux_sink_pads);
+		if (ret < 0)
+			return ret;
+		break;
+	case DVB_DEVICE_CA:
+		npads = 2;
 		break;
+	case DVB_DEVICE_NET:
+		/*
+		 * We should be creating entities for the MPE/ULE
+		 * decapsulation hardware (or software implementation).
+		 *
+		 * However, the number of for the MPE/ULE decaps may not be
+		 * fixed. As we don't have yet dynamic support for PADs at
+		 * the Media Controller, let's not create the decap
+		 * entities yet.
+		 */
+		return 0;
 	default:
-		npads = 1;
+		return 0;
 	}
 
+	dvbdev->entity = kzalloc(sizeof(*dvbdev->entity), GFP_KERNEL);
+	if (!dvbdev->entity)
+		return -ENOMEM;
+
+	dvbdev->entity->name = dvbdev->name;
+
 	if (npads) {
 		dvbdev->pads = kcalloc(npads, sizeof(*dvbdev->pads),
 				       GFP_KERNEL);
-		if (!dvbdev->pads) {
-			kfree(dvbdev->entity);
-			return;
-		}
+		if (!dvbdev->pads)
+			return -ENOMEM;
 	}
 
 	switch (type) {
 	case DVB_DEVICE_FRONTEND:
-		dvbdev->entity->type = MEDIA_ENT_T_DEVNODE_DVB_FE;
+		dvbdev->entity->function = MEDIA_ENT_F_DTV_DEMOD;
 		dvbdev->pads[0].flags = MEDIA_PAD_FL_SINK;
 		dvbdev->pads[1].flags = MEDIA_PAD_FL_SOURCE;
 		break;
 	case DVB_DEVICE_DEMUX:
-		dvbdev->entity->type = MEDIA_ENT_T_DEVNODE_DVB_DEMUX;
-		dvbdev->pads[0].flags = MEDIA_PAD_FL_SINK;
-		dvbdev->pads[1].flags = MEDIA_PAD_FL_SOURCE;
-		break;
-	case DVB_DEVICE_DVR:
-		dvbdev->entity->type = MEDIA_ENT_T_DEVNODE_DVB_DVR;
+		dvbdev->entity->function = MEDIA_ENT_F_TS_DEMUX;
 		dvbdev->pads[0].flags = MEDIA_PAD_FL_SINK;
+		for (i = 1; i < npads; i++)
+			dvbdev->pads[i].flags = MEDIA_PAD_FL_SOURCE;
 		break;
 	case DVB_DEVICE_CA:
-		dvbdev->entity->type = MEDIA_ENT_T_DEVNODE_DVB_CA;
+		dvbdev->entity->function = MEDIA_ENT_F_DTV_CA;
 		dvbdev->pads[0].flags = MEDIA_PAD_FL_SINK;
 		dvbdev->pads[1].flags = MEDIA_PAD_FL_SOURCE;
 		break;
-	case DVB_DEVICE_NET:
-		dvbdev->entity->type = MEDIA_ENT_T_DEVNODE_DVB_NET;
-		break;
 	default:
+		/* Should never happen, as the first switch prevents it */
 		kfree(dvbdev->entity);
+		kfree(dvbdev->pads);
 		dvbdev->entity = NULL;
-		return;
+		dvbdev->pads = NULL;
+		return 0;
 	}
 
-	if (npads)
-		ret = media_entity_init(dvbdev->entity, npads, dvbdev->pads, 0);
-	if (!ret)
-		ret = media_device_register_entity(dvbdev->adapter->mdev,
-						   dvbdev->entity);
-	if (ret < 0) {
-		printk(KERN_ERR
-			"%s: media_device_register_entity failed for %s\n",
-			__func__, dvbdev->entity->name);
-		kfree(dvbdev->pads);
-		kfree(dvbdev->entity);
-		dvbdev->entity = NULL;
-		return;
+	if (npads) {
+		ret = media_entity_pads_init(dvbdev->entity, npads, dvbdev->pads);
+		if (ret)
+			return ret;
 	}
+	ret = media_device_register_entity(dvbdev->adapter->mdev,
+					   dvbdev->entity);
+	if (ret)
+		return (ret);
 
-	printk(KERN_DEBUG "%s: media device '%s' registered.\n",
+	printk(KERN_DEBUG "%s: media entity '%s' registered.\n",
 		__func__, dvbdev->entity->name);
+
+	return 0;
+}
+#endif
+
+static int dvb_register_media_device(struct dvb_device *dvbdev,
+				     int type, int minor,
+				     unsigned demux_sink_pads)
+{
+#if defined(CONFIG_MEDIA_CONTROLLER_DVB)
+	struct media_link *link;
+	u32 intf_type;
+	int ret;
+
+	if (!dvbdev->adapter->mdev)
+		return 0;
+
+	ret = dvb_create_media_entity(dvbdev, type, demux_sink_pads);
+	if (ret)
+		return ret;
+
+	switch (type) {
+	case DVB_DEVICE_FRONTEND:
+		intf_type = MEDIA_INTF_T_DVB_FE;
+		break;
+	case DVB_DEVICE_DEMUX:
+		intf_type = MEDIA_INTF_T_DVB_DEMUX;
+		break;
+	case DVB_DEVICE_DVR:
+		intf_type = MEDIA_INTF_T_DVB_DVR;
+		break;
+	case DVB_DEVICE_CA:
+		intf_type = MEDIA_INTF_T_DVB_CA;
+		break;
+	case DVB_DEVICE_NET:
+		intf_type = MEDIA_INTF_T_DVB_NET;
+		break;
+	default:
+		return 0;
+	}
+
+	dvbdev->intf_devnode = media_devnode_create(dvbdev->adapter->mdev,
+						    intf_type, 0,
+						    DVB_MAJOR, minor);
+
+	if (!dvbdev->intf_devnode)
+		return -ENOMEM;
+
+	/*
+	 * Create the "obvious" link, e. g. the ones that represent
+	 * a direct association between an interface and an entity.
+	 * Other links should be created elsewhere, like:
+	 *		DVB FE intf    -> tuner
+	 *		DVB demux intf -> dvr
+	 */
+
+	if (!dvbdev->entity)
+		return 0;
+
+	link = media_create_intf_link(dvbdev->entity, &dvbdev->intf_devnode->intf,
+				      MEDIA_LNK_FL_ENABLED);
+	if (!link)
+		return -ENOMEM;
 #endif
+	return 0;
 }
 
 int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
-			const struct dvb_device *template, void *priv, int type)
+			const struct dvb_device *template, void *priv, int type,
+			int demux_sink_pads)
 {
 	struct dvb_device *dvbdev;
 	struct file_operations *dvbdevfops;
 	struct device *clsdev;
 	int minor;
-	int id;
+	int id, ret;
 
 	mutex_lock(&dvbdev_register_lock);
 
@@ -286,7 +442,7 @@ int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
 		return -ENFILE;
 	}
 
-	*pdvbdev = dvbdev = kmalloc(sizeof(struct dvb_device), GFP_KERNEL);
+	*pdvbdev = dvbdev = kzalloc(sizeof(*dvbdev), GFP_KERNEL);
 
 	if (!dvbdev){
 		mutex_unlock(&dvbdev_register_lock);
@@ -335,6 +491,20 @@ int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
 	dvb_minors[minor] = dvbdev;
 	up_write(&minor_rwsem);
 
+	ret = dvb_register_media_device(dvbdev, type, minor, demux_sink_pads);
+	if (ret) {
+		printk(KERN_ERR
+		      "%s: dvb_register_media_device failed to create the mediagraph\n",
+		      __func__);
+
+		dvb_media_device_free(dvbdev);
+		kfree(dvbdevfops);
+		kfree(dvbdev);
+		up_write(&minor_rwsem);
+		mutex_unlock(&dvbdev_register_lock);
+		return ret;
+	}
+
 	mutex_unlock(&dvbdev_register_lock);
 
 	clsdev = device_create(dvb_class, adap->device,
@@ -348,8 +518,6 @@ int dvb_register_device(struct dvb_adapter *adap, struct dvb_device **pdvbdev,
 	dprintk(KERN_DEBUG "DVB: register adapter%d/%s%d @ minor: %i (0x%02x)\n",
 		adap->num, dnames[type], id, minor, minor);
 
-	dvb_register_media_device(dvbdev, type, minor);
-
 	return 0;
 }
 EXPORT_SYMBOL(dvb_register_device);
@@ -364,15 +532,9 @@ void dvb_unregister_device(struct dvb_device *dvbdev)
 	dvb_minors[dvbdev->minor] = NULL;
 	up_write(&minor_rwsem);
 
-	device_destroy(dvb_class, MKDEV(DVB_MAJOR, dvbdev->minor));
+	dvb_media_device_free(dvbdev);
 
-#if defined(CONFIG_MEDIA_CONTROLLER_DVB)
-	if (dvbdev->entity) {
-		media_device_unregister_entity(dvbdev->entity);
-		kfree(dvbdev->entity);
-		kfree(dvbdev->pads);
-	}
-#endif
+	device_destroy(dvb_class, MKDEV(DVB_MAJOR, dvbdev->minor));
 
 	list_del (&dvbdev->list_head);
 	kfree (dvbdev->fops);
@@ -382,46 +544,212 @@ EXPORT_SYMBOL(dvb_unregister_device);
 
 
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
-void dvb_create_media_graph(struct dvb_adapter *adap)
+
+static int dvb_create_io_intf_links(struct dvb_adapter *adap,
+				    struct media_interface *intf,
+				    char *name)
+{
+	struct media_device *mdev = adap->mdev;
+	struct media_entity *entity;
+	struct media_link *link;
+
+	media_device_for_each_entity(entity, mdev) {
+		if (entity->function == MEDIA_ENT_F_IO_DTV) {
+			if (strncmp(entity->name, name, strlen(name)))
+				continue;
+			link = media_create_intf_link(entity, intf,
+						      MEDIA_LNK_FL_ENABLED);
+			if (!link)
+				return -ENOMEM;
+		}
+	}
+	return 0;
+}
+
+int dvb_create_media_graph(struct dvb_adapter *adap,
+			   bool create_rf_connector)
 {
 	struct media_device *mdev = adap->mdev;
-	struct media_entity *entity, *tuner = NULL, *fe = NULL;
-	struct media_entity *demux = NULL, *dvr = NULL, *ca = NULL;
+	struct media_entity *entity, *tuner = NULL, *demod = NULL, *conn;
+	struct media_entity *demux = NULL, *ca = NULL;
+	struct media_link *link;
+	struct media_interface *intf;
+	unsigned demux_pad = 0;
+	unsigned dvr_pad = 0;
+	unsigned ntuner = 0, ndemod = 0;
+	int ret;
+	static const char *connector_name = "Television";
 
 	if (!mdev)
-		return;
+		return 0;
 
 	media_device_for_each_entity(entity, mdev) {
-		switch (entity->type) {
-		case MEDIA_ENT_T_V4L2_SUBDEV_TUNER:
+		switch (entity->function) {
+		case MEDIA_ENT_F_TUNER:
 			tuner = entity;
+			ntuner++;
 			break;
-		case MEDIA_ENT_T_DEVNODE_DVB_FE:
-			fe = entity;
+		case MEDIA_ENT_F_DTV_DEMOD:
+			demod = entity;
+			ndemod++;
 			break;
-		case MEDIA_ENT_T_DEVNODE_DVB_DEMUX:
+		case MEDIA_ENT_F_TS_DEMUX:
 			demux = entity;
 			break;
-		case MEDIA_ENT_T_DEVNODE_DVB_DVR:
-			dvr = entity;
-			break;
-		case MEDIA_ENT_T_DEVNODE_DVB_CA:
+		case MEDIA_ENT_F_DTV_CA:
 			ca = entity;
 			break;
 		}
 	}
 
-	if (tuner && fe)
-		media_entity_create_link(tuner, 0, fe, 0, 0);
+	/*
+	 * Prepare to signalize to media_create_pad_links() that multiple
+	 * entities of the same type exists and a 1:n or n:1 links need to be
+	 * created.
+	 * NOTE: if both tuner and demod have multiple instances, it is up
+	 * to the caller driver to create such links.
+	 */
+	if (ntuner > 1)
+		tuner = NULL;
+	if (ndemod > 1)
+		demod = NULL;
+
+	if (create_rf_connector) {
+		conn = kzalloc(sizeof(*conn), GFP_KERNEL);
+		if (!conn)
+			return -ENOMEM;
+		adap->conn = conn;
+
+		adap->conn_pads = kcalloc(1, sizeof(*adap->conn_pads),
+					    GFP_KERNEL);
+		if (!adap->conn_pads)
+			return -ENOMEM;
+
+		conn->flags = MEDIA_ENT_FL_CONNECTOR;
+		conn->function = MEDIA_ENT_F_CONN_RF;
+		conn->name = connector_name;
+		adap->conn_pads->flags = MEDIA_PAD_FL_SOURCE;
+
+		ret = media_entity_pads_init(conn, 1, adap->conn_pads);
+		if (ret)
+			return ret;
+
+		ret = media_device_register_entity(mdev, conn);
+		if (ret)
+			return ret;
+
+		if (!ntuner)
+			ret = media_create_pad_links(mdev,
+						     MEDIA_ENT_F_CONN_RF,
+						     conn, 0,
+						     MEDIA_ENT_F_DTV_DEMOD,
+						     demod, 0,
+						     MEDIA_LNK_FL_ENABLED,
+						     false);
+		else
+			ret = media_create_pad_links(mdev,
+						     MEDIA_ENT_F_CONN_RF,
+						     conn, 0,
+						     MEDIA_ENT_F_TUNER,
+						     tuner, TUNER_PAD_RF_INPUT,
+						     MEDIA_LNK_FL_ENABLED,
+						     false);
+		if (ret)
+			return ret;
+	}
+
+	if (ntuner && ndemod) {
+		ret = media_create_pad_links(mdev,
+					     MEDIA_ENT_F_TUNER,
+					     tuner, TUNER_PAD_IF_OUTPUT,
+					     MEDIA_ENT_F_DTV_DEMOD,
+					     demod, 0, MEDIA_LNK_FL_ENABLED,
+					     false);
+		if (ret)
+			return ret;
+	}
+
+	if (ndemod && demux) {
+		ret = media_create_pad_links(mdev,
+					     MEDIA_ENT_F_DTV_DEMOD,
+					     demod, 1,
+					     MEDIA_ENT_F_TS_DEMUX,
+					     demux, 0, MEDIA_LNK_FL_ENABLED,
+					     false);
+		if (ret)
+			return -ENOMEM;
+	}
+	if (demux && ca) {
+		ret = media_create_pad_link(demux, 1, ca,
+					    0, MEDIA_LNK_FL_ENABLED);
+		if (!ret)
+			return -ENOMEM;
+	}
 
-	if (fe && demux)
-		media_entity_create_link(fe, 1, demux, 0, MEDIA_LNK_FL_ENABLED);
+	/* Create demux links for each ringbuffer/pad */
+	if (demux) {
+		media_device_for_each_entity(entity, mdev) {
+			if (entity->function == MEDIA_ENT_F_IO_DTV) {
+				if (!strncmp(entity->name, DVR_TSOUT,
+				    strlen(DVR_TSOUT))) {
+					ret = media_create_pad_link(demux,
+								++dvr_pad,
+							    entity, 0, 0);
+					if (ret)
+						return ret;
+				}
+				if (!strncmp(entity->name, DEMUX_TSOUT,
+				    strlen(DEMUX_TSOUT))) {
+					ret = media_create_pad_link(demux,
+							      ++demux_pad,
+							    entity, 0, 0);
+					if (ret)
+						return ret;
+				}
+			}
+		}
+	}
 
-	if (demux && dvr)
-		media_entity_create_link(demux, 1, dvr, 0, MEDIA_LNK_FL_ENABLED);
+	/* Create interface links for FE->tuner, DVR->demux and CA->ca */
+	media_device_for_each_intf(intf, mdev) {
+		if (intf->type == MEDIA_INTF_T_DVB_CA && ca) {
+			link = media_create_intf_link(ca, intf,
+						      MEDIA_LNK_FL_ENABLED);
+			if (!link)
+				return -ENOMEM;
+		}
 
-	if (demux && ca)
-		media_entity_create_link(demux, 1, ca, 0, MEDIA_LNK_FL_ENABLED);
+		if (intf->type == MEDIA_INTF_T_DVB_FE && tuner) {
+			link = media_create_intf_link(tuner, intf,
+						      MEDIA_LNK_FL_ENABLED);
+			if (!link)
+				return -ENOMEM;
+		}
+#if 0
+		/*
+		 * Indirect link - let's not create yet, as we don't know how
+		 *		   to handle indirect links, nor if this will
+		 *		   actually be needed.
+		 */
+		if (intf->type == MEDIA_INTF_T_DVB_DVR && demux) {
+			link = media_create_intf_link(demux, intf,
+						      MEDIA_LNK_FL_ENABLED);
+			if (!link)
+				return -ENOMEM;
+		}
+#endif
+		if (intf->type == MEDIA_INTF_T_DVB_DVR) {
+			ret = dvb_create_io_intf_links(adap, intf, DVR_TSOUT);
+			if (ret)
+				return ret;
+		}
+		if (intf->type == MEDIA_INTF_T_DVB_DEMUX) {
+			ret = dvb_create_io_intf_links(adap, intf, DEMUX_TSOUT);
+			if (ret)
+				return ret;
+		}
+	}
+	return 0;
 }
 EXPORT_SYMBOL_GPL(dvb_create_media_graph);
 #endif
diff --git a/drivers/media/dvb-core/dvbdev.h b/drivers/media/dvb-core/dvbdev.h
index 1069a776bbdb..4aff7bd3dea8 100644
--- a/drivers/media/dvb-core/dvbdev.h
+++ b/drivers/media/dvb-core/dvbdev.h
@@ -75,6 +75,9 @@ struct dvb_frontend;
  *			used.
  * @mdev:		pointer to struct media_device, used when the media
  *			controller is used.
+ * @conn:		RF connector. Used only if the device has no separate
+ *			tuner.
+ * @conn_pads:		pointer to struct media_pad associated with @conn;
  */
 struct dvb_adapter {
 	int num;
@@ -94,6 +97,8 @@ struct dvb_adapter {
 
 #if defined(CONFIG_MEDIA_CONTROLLER_DVB)
 	struct media_device *mdev;
+	struct media_entity *conn;
+	struct media_pad *conn_pads;
 #endif
 };
 
@@ -120,6 +125,11 @@ struct dvb_adapter {
  * @entity:	pointer to struct media_entity associated with the device node
  * @pads:	pointer to struct media_pad associated with @entity;
  * @priv:	private data
+ * @intf_devnode: Pointer to media_intf_devnode. Used by the dvbdev core to
+ *		store the MC device node interface
+ * @tsout_num_entities: Number of Transport Stream output entities
+ * @tsout_entity: array with MC entities associated to each TS output node
+ * @tsout_pads: array with the source pads for each @tsout_entity
  *
  * This structure is used by the DVB core (frontend, CA, net, demux) in
  * order to create the device nodes. Usually, driver should not initialize
@@ -148,8 +158,11 @@ struct dvb_device {
 	const char *name;
 
 	/* Allocated and filled inside dvbdev.c */
-	struct media_entity *entity;
-	struct media_pad *pads;
+	struct media_intf_devnode *intf_devnode;
+
+	unsigned tsout_num_entities;
+	struct media_entity *entity, *tsout_entity;
+	struct media_pad *pads, *tsout_pads;
 #endif
 
 	void *priv;
@@ -185,14 +198,18 @@ int dvb_unregister_adapter(struct dvb_adapter *adap);
  *		stored
  * @template:	Template used to create &pdvbdev;
  * @priv:	private data
- * @type:	type of the device: DVB_DEVICE_SEC, DVB_DEVICE_FRONTEND,
- *		DVB_DEVICE_DEMUX, DVB_DEVICE_DVR, DVB_DEVICE_CA, DVB_DEVICE_NET
+ * @type:	type of the device: %DVB_DEVICE_SEC, %DVB_DEVICE_FRONTEND,
+ *		%DVB_DEVICE_DEMUX, %DVB_DEVICE_DVR, %DVB_DEVICE_CA,
+ *		%DVB_DEVICE_NET
+ * @demux_sink_pads: Number of demux outputs, to be used to create the TS
+ *		outputs via the Media Controller.
  */
 int dvb_register_device(struct dvb_adapter *adap,
 			struct dvb_device **pdvbdev,
 			const struct dvb_device *template,
 			void *priv,
-			int type);
+			int type,
+			int demux_sink_pads);
 
 /**
  * dvb_unregister_device - Unregisters a DVB device
@@ -202,16 +219,43 @@ int dvb_register_device(struct dvb_adapter *adap,
 void dvb_unregister_device(struct dvb_device *dvbdev);
 
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
-void dvb_create_media_graph(struct dvb_adapter *adap);
+/**
+ * dvb_create_media_graph - Creates media graph for the Digital TV part of the
+ * 				device.
+ *
+ * @adap:			pointer to struct dvb_adapter
+ * @create_rf_connector:	if true, it creates the RF connector too
+ *
+ * This function checks all DVB-related functions at the media controller
+ * entities and creates the needed links for the media graph. It is
+ * capable of working with multiple tuners or multiple frontends, but it
+ * won't create links if the device has multiple tuners and multiple frontends
+ * or if the device has multiple muxes. In such case, the caller driver should
+ * manually create the remaining links.
+ */
+__must_check int dvb_create_media_graph(struct dvb_adapter *adap,
+					bool create_rf_connector);
+
 static inline void dvb_register_media_controller(struct dvb_adapter *adap,
 						 struct media_device *mdev)
 {
 	adap->mdev = mdev;
 }
 
+static inline struct media_device
+*dvb_get_media_controller(struct dvb_adapter *adap)
+{
+	return adap->mdev;
+}
 #else
-static inline void dvb_create_media_graph(struct dvb_adapter *adap) {}
+static inline
+int dvb_create_media_graph(struct dvb_adapter *adap,
+			   bool create_rf_connector)
+{
+	return 0;
+};
 #define dvb_register_media_controller(a, b) {}
+#define dvb_get_media_controller(a) NULL
 #endif
 
 int dvb_generic_open (struct inode *inode, struct file *file);
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index 292c9479bb75..310e4b8beae8 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -264,7 +264,7 @@ config DVB_MB86A16
 config DVB_TDA10071
 	tristate "NXP TDA10071"
 	depends on DVB_CORE && I2C
-	select REGMAP
+	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/au8522_common.c b/drivers/media/dvb-frontends/au8522_common.c
index 3559ff230045..f135126bc373 100644
--- a/drivers/media/dvb-frontends/au8522_common.c
+++ b/drivers/media/dvb-frontends/au8522_common.c
@@ -44,7 +44,7 @@ int au8522_writereg(struct au8522_state *state, u16 reg, u8 data)
 	int ret;
 	u8 buf[] = { (reg >> 8) | 0x80, reg & 0xff, data };
 
-	struct i2c_msg msg = { .addr = state->config->demod_address,
+	struct i2c_msg msg = { .addr = state->config.demod_address,
 			       .flags = 0, .buf = buf, .len = 3 };
 
 	ret = i2c_transfer(state->i2c, &msg, 1);
@@ -64,9 +64,9 @@ u8 au8522_readreg(struct au8522_state *state, u16 reg)
 	u8 b1[] = { 0 };
 
 	struct i2c_msg msg[] = {
-		{ .addr = state->config->demod_address, .flags = 0,
+		{ .addr = state->config.demod_address, .flags = 0,
 		  .buf = b0, .len = 2 },
-		{ .addr = state->config->demod_address, .flags = I2C_M_RD,
+		{ .addr = state->config.demod_address, .flags = I2C_M_RD,
 		  .buf = b1, .len = 1 } };
 
 	ret = i2c_transfer(state->i2c, msg, 2);
@@ -140,7 +140,7 @@ EXPORT_SYMBOL(au8522_release_state);
 
 static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
 {
-	struct au8522_led_config *led_config = state->config->led_cfg;
+	struct au8522_led_config *led_config = state->config.led_cfg;
 	u8 val;
 
 	/* bail out if we can't control an LED */
@@ -170,7 +170,7 @@ static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
  */
 int au8522_led_ctrl(struct au8522_state *state, int led)
 {
-	struct au8522_led_config *led_config = state->config->led_cfg;
+	struct au8522_led_config *led_config = state->config.led_cfg;
 	int i, ret = 0;
 
 	/* bail out if we can't control an LED */
diff --git a/drivers/media/dvb-frontends/au8522_decoder.c b/drivers/media/dvb-frontends/au8522_decoder.c
index 28d7dc2fee34..73612c5353d1 100644
--- a/drivers/media/dvb-frontends/au8522_decoder.c
+++ b/drivers/media/dvb-frontends/au8522_decoder.c
@@ -730,7 +730,9 @@ static int au8522_probe(struct i2c_client *client,
 	struct v4l2_ctrl_handler *hdl;
 	struct v4l2_subdev *sd;
 	int instance;
-	struct au8522_config *demod_config;
+#ifdef CONFIG_MEDIA_CONTROLLER
+	int ret;
+#endif
 
 	/* Check if the adapter supports the needed features */
 	if (!i2c_check_functionality(client->adapter,
@@ -754,19 +756,25 @@ static int au8522_probe(struct i2c_client *client,
 		break;
 	}
 
-	demod_config = kzalloc(sizeof(struct au8522_config), GFP_KERNEL);
-	if (demod_config == NULL) {
-		if (instance == 1)
-			kfree(state);
-		return -ENOMEM;
-	}
-	demod_config->demod_address = 0x8e >> 1;
-
-	state->config = demod_config;
+	state->config.demod_address = 0x8e >> 1;
 	state->i2c = client->adapter;
 
 	sd = &state->sd;
 	v4l2_i2c_subdev_init(sd, client, &au8522_ops);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+
+	state->pads[AU8522_PAD_INPUT].flags = MEDIA_PAD_FL_SINK;
+	state->pads[AU8522_PAD_VID_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	state->pads[AU8522_PAD_VBI_OUT].flags = MEDIA_PAD_FL_SOURCE;
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
+
+	ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(state->pads),
+				state->pads);
+	if (ret < 0) {
+		v4l_info(client, "failed to initialize media entity!\n");
+		return ret;
+	}
+#endif
 
 	hdl = &state->hdl;
 	v4l2_ctrl_handler_init(hdl, 4);
@@ -784,8 +792,7 @@ static int au8522_probe(struct i2c_client *client,
 		int err = hdl->error;
 
 		v4l2_ctrl_handler_free(hdl);
-		kfree(demod_config);
-		kfree(state);
+		au8522_release_state(state);
 		return err;
 	}
 
diff --git a/drivers/media/dvb-frontends/au8522_dig.c b/drivers/media/dvb-frontends/au8522_dig.c
index f956f13fb3dc..6c1e97640f3f 100644
--- a/drivers/media/dvb-frontends/au8522_dig.c
+++ b/drivers/media/dvb-frontends/au8522_dig.c
@@ -566,7 +566,7 @@ static int au8522_enable_modulation(struct dvb_frontend *fe,
 			au8522_writereg(state,
 				VSB_mod_tab[i].reg,
 				VSB_mod_tab[i].data);
-		au8522_set_if(fe, state->config->vsb_if);
+		au8522_set_if(fe, state->config.vsb_if);
 		break;
 	case QAM_64:
 		dprintk("%s() QAM 64\n", __func__);
@@ -574,7 +574,7 @@ static int au8522_enable_modulation(struct dvb_frontend *fe,
 			au8522_writereg(state,
 				QAM64_mod_tab[i].reg,
 				QAM64_mod_tab[i].data);
-		au8522_set_if(fe, state->config->qam_if);
+		au8522_set_if(fe, state->config.qam_if);
 		break;
 	case QAM_256:
 		if (zv_mode) {
@@ -583,7 +583,7 @@ static int au8522_enable_modulation(struct dvb_frontend *fe,
 				au8522_writereg(state,
 					QAM256_mod_tab_zv_mode[i].reg,
 					QAM256_mod_tab_zv_mode[i].data);
-			au8522_set_if(fe, state->config->qam_if);
+			au8522_set_if(fe, state->config.qam_if);
 			msleep(100);
 			au8522_writereg(state, 0x821a, 0x00);
 		} else {
@@ -592,7 +592,7 @@ static int au8522_enable_modulation(struct dvb_frontend *fe,
 				au8522_writereg(state,
 					QAM256_mod_tab[i].reg,
 					QAM256_mod_tab[i].data);
-			au8522_set_if(fe, state->config->qam_if);
+			au8522_set_if(fe, state->config.qam_if);
 		}
 		break;
 	default:
@@ -666,7 +666,7 @@ static int au8522_read_status(struct dvb_frontend *fe, enum fe_status *status)
 			*status |= FE_HAS_LOCK | FE_HAS_SYNC;
 	}
 
-	switch (state->config->status_mode) {
+	switch (state->config.status_mode) {
 	case AU8522_DEMODLOCKING:
 		dprintk("%s() DEMODLOCKING\n", __func__);
 		if (*status & FE_HAS_VITERBI)
@@ -704,7 +704,7 @@ static int au8522_read_status(struct dvb_frontend *fe, enum fe_status *status)
 
 static int au8522_led_status(struct au8522_state *state, const u16 *snr)
 {
-	struct au8522_led_config *led_config = state->config->led_cfg;
+	struct au8522_led_config *led_config = state->config.led_cfg;
 	int led;
 	u16 strong;
 
@@ -758,7 +758,7 @@ static int au8522_read_snr(struct dvb_frontend *fe, u16 *snr)
 					    au8522_readreg(state, 0x4311),
 					    snr);
 
-	if (state->config->led_cfg)
+	if (state->config.led_cfg)
 		au8522_led_status(state, snr);
 
 	return ret;
@@ -866,7 +866,7 @@ struct dvb_frontend *au8522_attach(const struct au8522_config *config,
 	}
 
 	/* setup the state */
-	state->config = config;
+	state->config = *config;
 	state->i2c = i2c;
 	state->operational_mode = AU8522_DIGITAL_MODE;
 
diff --git a/drivers/media/dvb-frontends/au8522_priv.h b/drivers/media/dvb-frontends/au8522_priv.h
index 951b3847e6f6..404a0cb0ed8d 100644
--- a/drivers/media/dvb-frontends/au8522_priv.h
+++ b/drivers/media/dvb-frontends/au8522_priv.h
@@ -39,6 +39,14 @@
 #define AU8522_DIGITAL_MODE 1
 #define AU8522_SUSPEND_MODE 2
 
+enum au8522_media_pads {
+	AU8522_PAD_INPUT,
+	AU8522_PAD_VID_OUT,
+	AU8522_PAD_VBI_OUT,
+
+	AU8522_NUM_PADS
+};
+
 struct au8522_state {
 	struct i2c_client *c;
 	struct i2c_adapter *i2c;
@@ -50,7 +58,7 @@ struct au8522_state {
 	struct list_head hybrid_tuner_instance_list;
 
 	/* configuration settings */
-	const struct au8522_config *config;
+	struct au8522_config config;
 
 	struct dvb_frontend frontend;
 
@@ -68,6 +76,10 @@ struct au8522_state {
 	u32 id;
 	u32 rev;
 	struct v4l2_ctrl_handler hdl;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_pad pads[AU8522_NUM_PADS];
+#endif
 };
 
 /* These are routines shared by both the VSB/QAM demodulator and the analog
diff --git a/drivers/media/dvb-frontends/bsbe1-d01a.h b/drivers/media/dvb-frontends/bsbe1-d01a.h
index 7ed3c424178c..baaf89e768cf 100644
--- a/drivers/media/dvb-frontends/bsbe1-d01a.h
+++ b/drivers/media/dvb-frontends/bsbe1-d01a.h
@@ -21,7 +21,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef BSBE1_D01A_H
diff --git a/drivers/media/dvb-frontends/bsbe1.h b/drivers/media/dvb-frontends/bsbe1.h
index 53e4d0dbb745..4ad766154741 100644
--- a/drivers/media/dvb-frontends/bsbe1.h
+++ b/drivers/media/dvb-frontends/bsbe1.h
@@ -19,7 +19,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef BSBE1_H
diff --git a/drivers/media/dvb-frontends/bsru6.h b/drivers/media/dvb-frontends/bsru6.h
index c2a578e1314d..275c1782597d 100644
--- a/drivers/media/dvb-frontends/bsru6.h
+++ b/drivers/media/dvb-frontends/bsru6.h
@@ -19,7 +19,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef BSRU6_H
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index def6d21d1445..24a457d9d803 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -722,7 +722,7 @@ struct dvb_frontend *cxd2820r_attach(const struct cxd2820r_config *cfg,
 #ifdef CONFIG_GPIOLIB
 		/* add GPIOs */
 		priv->gpio_chip.label = KBUILD_MODNAME;
-		priv->gpio_chip.dev = &priv->i2c->dev;
+		priv->gpio_chip.parent = &priv->i2c->dev;
 		priv->gpio_chip.owner = THIS_MODULE;
 		priv->gpio_chip.direction_output =
 				cxd2820r_gpio_direction_output;
diff --git a/drivers/media/dvb-frontends/isl6405.c b/drivers/media/dvb-frontends/isl6405.c
index b46450a10b80..6913cd687b4d 100644
--- a/drivers/media/dvb-frontends/isl6405.c
+++ b/drivers/media/dvb-frontends/isl6405.c
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/media/dvb-frontends/isl6405.h b/drivers/media/dvb-frontends/isl6405.h
index 3c148b830bd1..4a23d3bdf3e6 100644
--- a/drivers/media/dvb-frontends/isl6405.h
+++ b/drivers/media/dvb-frontends/isl6405.h
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef _ISL6405_H
diff --git a/drivers/media/dvb-frontends/isl6421.c b/drivers/media/dvb-frontends/isl6421.c
index 3a4d4606a426..0b6d3837d5de 100644
--- a/drivers/media/dvb-frontends/isl6421.c
+++ b/drivers/media/dvb-frontends/isl6421.c
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/media/dvb-frontends/isl6421.h b/drivers/media/dvb-frontends/isl6421.h
index 3273597833fd..00f9874ca5a2 100644
--- a/drivers/media/dvb-frontends/isl6421.h
+++ b/drivers/media/dvb-frontends/isl6421.h
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef _ISL6421_H
diff --git a/drivers/media/dvb-frontends/lnbp21.c b/drivers/media/dvb-frontends/lnbp21.c
index 4aca0fb9a8a7..6261460d93a7 100644
--- a/drivers/media/dvb-frontends/lnbp21.c
+++ b/drivers/media/dvb-frontends/lnbp21.c
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/media/dvb-frontends/lnbp21.h b/drivers/media/dvb-frontends/lnbp21.h
index a9b530de62a6..cd9101f6e579 100644
--- a/drivers/media/dvb-frontends/lnbp21.h
+++ b/drivers/media/dvb-frontends/lnbp21.h
@@ -21,7 +21,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef _LNBP21_H
diff --git a/drivers/media/dvb-frontends/lnbp22.c b/drivers/media/dvb-frontends/lnbp22.c
index d7ca0fdd0084..5c5fd04fd4a7 100644
--- a/drivers/media/dvb-frontends/lnbp22.c
+++ b/drivers/media/dvb-frontends/lnbp22.c
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 #include <linux/delay.h>
 #include <linux/errno.h>
diff --git a/drivers/media/dvb-frontends/lnbp22.h b/drivers/media/dvb-frontends/lnbp22.h
index 628148385182..5d01d92814c2 100644
--- a/drivers/media/dvb-frontends/lnbp22.h
+++ b/drivers/media/dvb-frontends/lnbp22.h
@@ -22,7 +22,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org
+ * the project's page is at https://linuxtv.org
  */
 
 #ifndef _LNBP22_H
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
index feeeb70d841e..ce73a5ec6036 100644
--- a/drivers/media/dvb-frontends/m88ds3103.c
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -685,7 +685,7 @@ static int m88ds3103_init(struct dvb_frontend *fe)
 	/* request the firmware, this will block and timeout */
 	ret = request_firmware(&fw, fw_file, &client->dev);
 	if (ret) {
-		dev_err(&client->dev, "firmare file '%s' not found\n", fw_file);
+		dev_err(&client->dev, "firmware file '%s' not found\n", fw_file);
 		goto err;
 	}
 
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index 78b87b260d74..10f2119935da 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -976,7 +976,8 @@ static int rtl2832_regmap_read(void *context, const void *reg_buf,
 
 	ret = __i2c_transfer(client->adapter, msg, 2);
 	if (ret != 2) {
-		dev_warn(&client->dev, "i2c reg read failed %d\n", ret);
+		dev_warn(&client->dev, "i2c reg read failed %d reg %02x\n",
+			 ret, *(u8 *)reg_buf);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
 		return ret;
@@ -999,7 +1000,8 @@ static int rtl2832_regmap_write(void *context, const void *data, size_t count)
 
 	ret = __i2c_transfer(client->adapter, msg, 1);
 	if (ret != 1) {
-		dev_warn(&client->dev, "i2c reg write failed %d\n", ret);
+		dev_warn(&client->dev, "i2c reg write failed %d reg %02x\n",
+			 ret, *(u8 *)data);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
 		return ret;
@@ -1028,7 +1030,8 @@ static int rtl2832_regmap_gather_write(void *context, const void *reg,
 
 	ret = __i2c_transfer(client->adapter, msg, 1);
 	if (ret != 1) {
-		dev_warn(&client->dev, "i2c reg write failed %d\n", ret);
+		dev_warn(&client->dev, "i2c reg write failed %d reg %02x\n",
+			 ret, *(u8 const *)reg);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
 		return ret;
@@ -1097,18 +1100,6 @@ static int rtl2832_enable_slave_ts(struct i2c_client *client)
 	if (ret)
 		goto err;
 
-	ret = rtl2832_bulk_write(client, 0x022, "\x01", 1);
-	if (ret)
-		goto err;
-
-	ret = rtl2832_bulk_write(client, 0x026, "\x1f", 1);
-	if (ret)
-		goto err;
-
-	ret = rtl2832_bulk_write(client, 0x027, "\xff", 1);
-	if (ret)
-		goto err;
-
 	ret = rtl2832_bulk_write(client, 0x192, "\x7f\xf7\xff", 3);
 	if (ret)
 		goto err;
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.c b/drivers/media/dvb-frontends/rtl2832_sdr.c
index dcd8d94c1037..b860f02a4e55 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.c
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.c
@@ -310,7 +310,7 @@ static void rtl2832_sdr_urb_complete(struct urb *urb)
 		len = rtl2832_sdr_convert_stream(dev, ptr, urb->transfer_buffer,
 				urb->actual_length);
 		vb2_set_plane_payload(&fbuf->vb.vb2_buf, 0, len);
-		v4l2_get_timestamp(&fbuf->vb.timestamp);
+		fbuf->vb.vb2_buf.timestamp = ktime_get_ns();
 		fbuf->vb.sequence = dev->sequence++;
 		vb2_buffer_done(&fbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	}
@@ -490,7 +490,7 @@ static int rtl2832_sdr_querycap(struct file *file, void *fh,
 
 /* Videobuf2 operations */
 static int rtl2832_sdr_queue_setup(struct vb2_queue *vq,
-		const void *parg, unsigned int *nbuffers,
+		unsigned int *nbuffers,
 		unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct rtl2832_sdr_dev *dev = vb2_get_drv_priv(vq);
diff --git a/drivers/media/dvb-frontends/si2165.c b/drivers/media/dvb-frontends/si2165.c
index 7c2eeee69757..2b93241d4bc1 100644
--- a/drivers/media/dvb-frontends/si2165.c
+++ b/drivers/media/dvb-frontends/si2165.c
@@ -1,21 +1,21 @@
 /*
-    Driver for Silicon Labs Si2161 DVB-T and Si2165 DVB-C/-T Demodulator
-
-    Copyright (C) 2013-2014 Matthias Schwarzott <zzam@gentoo.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.
-
-    References:
-    http://www.silabs.com/Support%20Documents/TechnicalDocs/Si2165-short.pdf
-*/
+ *  Driver for Silicon Labs Si2161 DVB-T and Si2165 DVB-C/-T Demodulator
+ *
+ *  Copyright (C) 2013-2014 Matthias Schwarzott <zzam@gentoo.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.
+ *
+ *  References:
+ *  http://www.silabs.com/Support%20Documents/TechnicalDocs/Si2165-short.pdf
+ */
 
 #include <linux/delay.h>
 #include <linux/errno.h>
@@ -31,16 +31,18 @@
 #include "si2165_priv.h"
 #include "si2165.h"
 
-/* Hauppauge WinTV-HVR-930C-HD B130 / PCTV QuatroStick 521e 1113xx
- * uses 16 MHz xtal */
-
-/* Hauppauge WinTV-HVR-930C-HD B131 / PCTV QuatroStick 522e 1114xx
- * uses 24 MHz clock provided by tuner */
+/*
+ * Hauppauge WinTV-HVR-930C-HD B130 / PCTV QuatroStick 521e 1113xx
+ * uses 16 MHz xtal
+ *
+ * Hauppauge WinTV-HVR-930C-HD B131 / PCTV QuatroStick 522e 1114xx
+ * uses 24 MHz clock provided by tuner
+ */
 
 struct si2165_state {
 	struct i2c_adapter *i2c;
 
-	struct dvb_frontend frontend;
+	struct dvb_frontend fe;
 
 	struct si2165_config config;
 
@@ -241,6 +243,27 @@ err:
 	return ret;
 }
 
+#define REG16(reg, val) { (reg), (val) & 0xff }, { (reg)+1, (val)>>8 & 0xff }
+struct si2165_reg_value_pair {
+	u16 reg;
+	u8 val;
+};
+
+static int si2165_write_reg_list(struct si2165_state *state,
+				 const struct si2165_reg_value_pair *regs,
+				 int count)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < count; i++) {
+		ret = si2165_writereg8(state, regs[i].reg, regs[i].val);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
 static int si2165_get_tune_settings(struct dvb_frontend *fe,
 				    struct dvb_frontend_tune_settings *s)
 {
@@ -258,8 +281,10 @@ static int si2165_init_pll(struct si2165_state *state)
 	u8 divl = 12;
 	u8 buf[4];
 
-	/* hardcoded values can be deleted if calculation is verified
-	 * or it yields the same values as the windows driver */
+	/*
+	 * hardcoded values can be deleted if calculation is verified
+	 * or it yields the same values as the windows driver
+	 */
 	switch (ref_freq_Hz) {
 	case 16000000u:
 		divn = 56;
@@ -274,8 +299,10 @@ static int si2165_init_pll(struct si2165_state *state)
 		if (ref_freq_Hz > 16000000u)
 			divr = 2;
 
-		/* now select divn and divp such that
-		 * fvco is in 1624..1824 MHz */
+		/*
+		 * now select divn and divp such that
+		 * fvco is in 1624..1824 MHz
+		 */
 		if (1624000000u * divr > ref_freq_Hz * 2u * 63u)
 			divp = 4;
 
@@ -341,10 +368,12 @@ static int si2165_upload_firmware_block(struct si2165_state *state,
 	if (len % 4 != 0)
 		return -EINVAL;
 
-	deb_fw_load("si2165_upload_firmware_block called with len=0x%x offset=0x%x blockcount=0x%x\n",
+	deb_fw_load(
+		"si2165_upload_firmware_block called with len=0x%x offset=0x%x blockcount=0x%x\n",
 				len, offset, block_count);
 	while (offset+12 <= len && cur_block < block_count) {
-		deb_fw_load("si2165_upload_firmware_block in while len=0x%x offset=0x%x cur_block=0x%x blockcount=0x%x\n",
+		deb_fw_load(
+			"si2165_upload_firmware_block in while len=0x%x offset=0x%x cur_block=0x%x blockcount=0x%x\n",
 					len, offset, cur_block, block_count);
 		wordcount = data[offset];
 		if (wordcount < 1 || data[offset+1] ||
@@ -383,7 +412,8 @@ static int si2165_upload_firmware_block(struct si2165_state *state,
 		cur_block++;
 	}
 
-	deb_fw_load("si2165_upload_firmware_block after while len=0x%x offset=0x%x cur_block=0x%x blockcount=0x%x\n",
+	deb_fw_load(
+		"si2165_upload_firmware_block after while len=0x%x offset=0x%x cur_block=0x%x blockcount=0x%x\n",
 				len, offset, cur_block, block_count);
 
 	if (poffset)
@@ -511,7 +541,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 					   &offset, block_count);
 	if (ret < 0) {
 		dev_err(&state->i2c->dev,
-			"%s: firmare could not be uploaded\n",
+			"%s: firmware could not be uploaded\n",
 			KBUILD_MODNAME);
 		goto error;
 	}
@@ -535,7 +565,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 
 	if (len != offset) {
 		dev_err(&state->i2c->dev,
-			"%s: firmare len mismatch %04x != %04x\n",
+			"%s: firmware len mismatch %04x != %04x\n",
 			KBUILD_MODNAME, len, offset);
 		ret = -EINVAL;
 		goto error;
@@ -633,7 +663,7 @@ static int si2165_init(struct dvb_frontend *fe)
 		goto error;
 
 	/* ber_pkt */
-	ret = si2165_writereg16(state, 0x0470 , 0x7530);
+	ret = si2165_writereg16(state, 0x0470, 0x7530);
 	if (ret < 0)
 		goto error;
 
@@ -660,22 +690,19 @@ static int si2165_init(struct dvb_frontend *fe)
 			goto error;
 	}
 
-	/* write adc values after each reset*/
-	ret = si2165_writereg8(state, 0x012a, 0x46);
-	if (ret < 0)
-		goto error;
-	ret = si2165_writereg8(state, 0x012c, 0x00);
+	/* ts output config */
+	ret = si2165_writereg8(state, 0x04e4, 0x20);
 	if (ret < 0)
-		goto error;
-	ret = si2165_writereg8(state, 0x012e, 0x0a);
+		return ret;
+	ret = si2165_writereg16(state, 0x04ef, 0x00fe);
 	if (ret < 0)
-		goto error;
-	ret = si2165_writereg8(state, 0x012f, 0xff);
+		return ret;
+	ret = si2165_writereg24(state, 0x04f4, 0x555555);
 	if (ret < 0)
-		goto error;
-	ret = si2165_writereg8(state, 0x0123, 0x70);
+		return ret;
+	ret = si2165_writereg8(state, 0x04e5, 0x01);
 	if (ret < 0)
-		goto error;
+		return ret;
 
 	return 0;
 error:
@@ -733,16 +760,26 @@ static int si2165_set_oversamp(struct si2165_state *state, u32 dvb_rate)
 	do_div(oversamp, dvb_rate);
 	reg_value = oversamp & 0x3fffffff;
 
-	/* oversamp, usbdump contained 0x03100000; */
+	dprintk("%s: Write oversamp=%#x\n", __func__, reg_value);
 	return si2165_writereg32(state, 0x00e4, reg_value);
 }
 
-static int si2165_set_if_freq_shift(struct si2165_state *state, u32 IF)
+static int si2165_set_if_freq_shift(struct si2165_state *state)
 {
+	struct dvb_frontend *fe = &state->fe;
 	u64 if_freq_shift;
 	s32 reg_value = 0;
 	u32 fe_clk = si2165_get_fe_clk(state);
+	u32 IF = 0;
 
+	if (!fe->ops.tuner_ops.get_if_frequency) {
+		dev_err(&state->i2c->dev,
+			"%s: Error: get_if_frequency() not defined at tuner. Can't work without it!\n",
+			KBUILD_MODNAME);
+		return -EINVAL;
+	}
+
+	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
 	if_freq_shift = IF;
 	if_freq_shift <<= 29;
 
@@ -758,25 +795,39 @@ static int si2165_set_if_freq_shift(struct si2165_state *state, u32 IF)
 	return si2165_writereg32(state, 0x00e8, reg_value);
 }
 
-static int si2165_set_parameters(struct dvb_frontend *fe)
+static const struct si2165_reg_value_pair dvbt_regs[] = {
+	/* standard = DVB-T */
+	{ 0x00ec, 0x01 },
+	{ 0x08f8, 0x00 },
+	/* impulsive_noise_remover */
+	{ 0x031c, 0x01 },
+	{ 0x00cb, 0x00 },
+	/* agc2 */
+	{ 0x016e, 0x41 },
+	{ 0x016c, 0x0e },
+	{ 0x016d, 0x10 },
+	/* agc */
+	{ 0x015b, 0x03 },
+	{ 0x0150, 0x78 },
+	/* agc */
+	{ 0x01a0, 0x78 },
+	{ 0x01c8, 0x68 },
+	/* freq_sync_range */
+	REG16(0x030c, 0x0064),
+	/* gp_reg0 */
+	{ 0x0387, 0x00 }
+};
+
+static int si2165_set_frontend_dvbt(struct dvb_frontend *fe)
 {
 	int ret;
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct si2165_state *state = fe->demodulator_priv;
-	u8 val[3];
-	u32 IF;
 	u32 dvb_rate = 0;
 	u16 bw10k;
 
 	dprintk("%s: called\n", __func__);
 
-	if (!fe->ops.tuner_ops.get_if_frequency) {
-		dev_err(&state->i2c->dev,
-			"%s: Error: get_if_frequency() not defined at tuner. Can't work without it!\n",
-			KBUILD_MODNAME);
-		return -EINVAL;
-	}
-
 	if (!state->has_dvbt)
 		return -EINVAL;
 
@@ -788,34 +839,10 @@ static int si2165_set_parameters(struct dvb_frontend *fe)
 		bw10k = 800;
 	}
 
-	/* standard = DVB-T */
-	ret = si2165_writereg8(state, 0x00ec, 0x01);
-	if (ret < 0)
-		return ret;
 	ret = si2165_adjust_pll_divl(state, 12);
 	if (ret < 0)
 		return ret;
 
-	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
-	ret = si2165_set_if_freq_shift(state, IF);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x08f8, 0x00);
-	if (ret < 0)
-		return ret;
-	/* ts output config */
-	ret = si2165_writereg8(state, 0x04e4, 0x20);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg16(state, 0x04ef, 0x00fe);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg24(state, 0x04f4, 0x555555);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x04e5, 0x01);
-	if (ret < 0)
-		return ret;
 	/* bandwidth in 10KHz steps */
 	ret = si2165_writereg16(state, 0x0308, bw10k);
 	if (ret < 0)
@@ -823,48 +850,115 @@ static int si2165_set_parameters(struct dvb_frontend *fe)
 	ret = si2165_set_oversamp(state, dvb_rate);
 	if (ret < 0)
 		return ret;
-	/* impulsive_noise_remover */
-	ret = si2165_writereg8(state, 0x031c, 0x01);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x00cb, 0x00);
+
+	ret = si2165_write_reg_list(state, dvbt_regs, ARRAY_SIZE(dvbt_regs));
 	if (ret < 0)
 		return ret;
+
+	return 0;
+}
+
+static const struct si2165_reg_value_pair dvbc_regs[] = {
+	/* standard = DVB-C */
+	{ 0x00ec, 0x05 },
+	{ 0x08f8, 0x00 },
+
 	/* agc2 */
-	ret = si2165_writereg8(state, 0x016e, 0x41);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x016c, 0x0e);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x016d, 0x10);
-	if (ret < 0)
-		return ret;
+	{ 0x016e, 0x50 },
+	{ 0x016c, 0x0e },
+	{ 0x016d, 0x10 },
 	/* agc */
-	ret = si2165_writereg8(state, 0x015b, 0x03);
-	if (ret < 0)
-		return ret;
-	ret = si2165_writereg8(state, 0x0150, 0x78);
-	if (ret < 0)
-		return ret;
+	{ 0x015b, 0x03 },
+	{ 0x0150, 0x68 },
 	/* agc */
-	ret = si2165_writereg8(state, 0x01a0, 0x78);
+	{ 0x01a0, 0x68 },
+	{ 0x01c8, 0x50 },
+
+	{ 0x0278, 0x0d },
+
+	{ 0x023a, 0x05 },
+	{ 0x0261, 0x09 },
+	REG16(0x0350, 0x3e80),
+	{ 0x02f4, 0x00 },
+
+	{ 0x00cb, 0x01 },
+	REG16(0x024c, 0x0000),
+	REG16(0x027c, 0x0000),
+	{ 0x0232, 0x03 },
+	{ 0x02f4, 0x0b },
+	{ 0x018b, 0x00 },
+};
+
+static int si2165_set_frontend_dvbc(struct dvb_frontend *fe)
+{
+	struct si2165_state *state = fe->demodulator_priv;
+	int ret;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	const u32 dvb_rate = p->symbol_rate;
+	const u32 bw_hz = p->bandwidth_hz;
+
+	if (!state->has_dvbc)
+		return -EINVAL;
+
+	if (dvb_rate == 0)
+		return -EINVAL;
+
+	ret = si2165_adjust_pll_divl(state, 14);
 	if (ret < 0)
 		return ret;
-	ret = si2165_writereg8(state, 0x01c8, 0x68);
+
+	/* Oversampling */
+	ret = si2165_set_oversamp(state, dvb_rate);
 	if (ret < 0)
 		return ret;
-	/* freq_sync_range */
-	ret = si2165_writereg16(state, 0x030c, 0x0064);
+
+	ret = si2165_writereg32(state, 0x00c4, bw_hz);
 	if (ret < 0)
 		return ret;
-	/* gp_reg0 */
-	ret = si2165_readreg8(state, 0x0387, val);
+
+	ret = si2165_write_reg_list(state, dvbc_regs, ARRAY_SIZE(dvbc_regs));
 	if (ret < 0)
 		return ret;
-	ret = si2165_writereg8(state, 0x0387, 0x00);
+
+	return 0;
+}
+
+static const struct si2165_reg_value_pair agc_rewrite[] = {
+	{ 0x012a, 0x46 },
+	{ 0x012c, 0x00 },
+	{ 0x012e, 0x0a },
+	{ 0x012f, 0xff },
+	{ 0x0123, 0x70 }
+};
+
+static int si2165_set_frontend(struct dvb_frontend *fe)
+{
+	struct si2165_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
+	u32 delsys = p->delivery_system;
+	int ret;
+	u8 val[3];
+
+	/* initial setting of if freq shift */
+	ret = si2165_set_if_freq_shift(state);
 	if (ret < 0)
 		return ret;
+
+	switch (delsys) {
+	case SYS_DVBT:
+		ret = si2165_set_frontend_dvbt(fe);
+		if (ret < 0)
+			return ret;
+		break;
+	case SYS_DVBC_ANNEX_A:
+		ret = si2165_set_frontend_dvbc(fe);
+		if (ret < 0)
+			return ret;
+		break;
+	default:
+		return -EINVAL;
+	}
+
 	/* dsp_addr_jump */
 	ret = si2165_writereg32(state, 0x0348, 0xf4000000);
 	if (ret < 0)
@@ -874,8 +968,7 @@ static int si2165_set_parameters(struct dvb_frontend *fe)
 		fe->ops.tuner_ops.set_params(fe);
 
 	/* recalc if_freq_shift if IF might has changed */
-	fe->ops.tuner_ops.get_if_frequency(fe, &IF);
-	ret = si2165_set_if_freq_shift(state, IF);
+	ret = si2165_set_if_freq_shift(state);
 	if (ret < 0)
 		return ret;
 
@@ -886,6 +979,7 @@ static int si2165_set_parameters(struct dvb_frontend *fe)
 	ret = si2165_writereg8(state, 0x0341, 0x00);
 	if (ret < 0)
 		return ret;
+
 	/* reset all */
 	ret = si2165_writereg8(state, 0x00c0, 0x00);
 	if (ret < 0)
@@ -894,6 +988,13 @@ static int si2165_set_parameters(struct dvb_frontend *fe)
 	ret = si2165_writereg32(state, 0x0384, 0x00000000);
 	if (ret < 0)
 		return ret;
+
+	/* write adc values after each reset*/
+	ret = si2165_write_reg_list(state, agc_rewrite,
+				    ARRAY_SIZE(agc_rewrite));
+	if (ret < 0)
+		return ret;
+
 	/* start_synchro */
 	ret = si2165_writereg8(state, 0x02e0, 0x01);
 	if (ret < 0)
@@ -917,7 +1018,12 @@ static void si2165_release(struct dvb_frontend *fe)
 static struct dvb_frontend_ops si2165_ops = {
 	.info = {
 		.name = "Silicon Labs ",
-		.caps =	FE_CAN_FEC_1_2 |
+		 /* For DVB-C */
+		.symbol_rate_min = 1000000,
+		.symbol_rate_max = 7200000,
+		/* For DVB-T */
+		.frequency_stepsize = 166667,
+		.caps = FE_CAN_FEC_1_2 |
 			FE_CAN_FEC_2_3 |
 			FE_CAN_FEC_3_4 |
 			FE_CAN_FEC_5_6 |
@@ -930,7 +1036,6 @@ static struct dvb_frontend_ops si2165_ops = {
 			FE_CAN_QAM_128 |
 			FE_CAN_QAM_256 |
 			FE_CAN_QAM_AUTO |
-			FE_CAN_TRANSMISSION_MODE_AUTO |
 			FE_CAN_GUARD_INTERVAL_AUTO |
 			FE_CAN_HIERARCHY_AUTO |
 			FE_CAN_MUTE_TS |
@@ -943,7 +1048,7 @@ static struct dvb_frontend_ops si2165_ops = {
 	.init = si2165_init,
 	.sleep = si2165_sleep,
 
-	.set_frontend      = si2165_set_parameters,
+	.set_frontend      = si2165_set_frontend,
 	.read_status       = si2165_read_status,
 
 	.release = si2165_release,
@@ -979,9 +1084,9 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 	}
 
 	/* create dvb_frontend */
-	memcpy(&state->frontend.ops, &si2165_ops,
+	memcpy(&state->fe.ops, &si2165_ops,
 		sizeof(struct dvb_frontend_ops));
-	state->frontend.demodulator_priv = state;
+	state->fe.demodulator_priv = state;
 
 	/* powerup */
 	io_ret = si2165_writereg8(state, 0x0000, state->config.chip_mode);
@@ -1033,20 +1138,22 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 		KBUILD_MODNAME, chip_name, rev_char, state->chip_type,
 		state->chip_revcode);
 
-	strlcat(state->frontend.ops.info.name, chip_name,
-			sizeof(state->frontend.ops.info.name));
+	strlcat(state->fe.ops.info.name, chip_name,
+			sizeof(state->fe.ops.info.name));
 
 	n = 0;
 	if (state->has_dvbt) {
-		state->frontend.ops.delsys[n++] = SYS_DVBT;
-		strlcat(state->frontend.ops.info.name, " DVB-T",
-			sizeof(state->frontend.ops.info.name));
+		state->fe.ops.delsys[n++] = SYS_DVBT;
+		strlcat(state->fe.ops.info.name, " DVB-T",
+			sizeof(state->fe.ops.info.name));
+	}
+	if (state->has_dvbc) {
+		state->fe.ops.delsys[n++] = SYS_DVBC_ANNEX_A;
+		strlcat(state->fe.ops.info.name, " DVB-C",
+			sizeof(state->fe.ops.info.name));
 	}
-	if (state->has_dvbc)
-		dev_warn(&state->i2c->dev, "%s: DVB-C is not yet supported.\n",
-		       KBUILD_MODNAME);
 
-	return &state->frontend;
+	return &state->fe;
 
 error:
 	kfree(state);
diff --git a/drivers/media/dvb-frontends/stb6100.c b/drivers/media/dvb-frontends/stb6100.c
index 4ef8a5c7003e..c978c801c7aa 100644
--- a/drivers/media/dvb-frontends/stb6100.c
+++ b/drivers/media/dvb-frontends/stb6100.c
@@ -252,6 +252,7 @@ static int stb6100_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 {
 	int rc;
 	u8 f;
+	u32 bw;
 	struct stb6100_state *state = fe->tuner_priv;
 
 	rc = stb6100_read_reg(state, STB6100_F);
@@ -259,9 +260,9 @@ static int stb6100_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 		return rc;
 	f = rc & STB6100_F_F;
 
-	state->status.bandwidth = (f + 5) * 2000;	/* x2 for ZIF	*/
+	bw = (f + 5) * 2000;	/* x2 for ZIF	*/
 
-	*bandwidth = state->bandwidth = state->status.bandwidth * 1000;
+	*bandwidth = state->bandwidth = bw * 1000;
 	dprintk(verbose, FE_DEBUG, 1, "bandwidth = %u Hz", state->bandwidth);
 	return 0;
 }
@@ -495,68 +496,28 @@ static int stb6100_sleep(struct dvb_frontend *fe)
 static int stb6100_init(struct dvb_frontend *fe)
 {
 	struct stb6100_state *state = fe->tuner_priv;
-	struct tuner_state *status = &state->status;
+	int refclk = 27000000; /* Hz */
 
-	status->tunerstep	= 125000;
-	status->ifreq		= 0;
-	status->refclock	= 27000000;	/* Hz	*/
-	status->iqsense		= 1;
-	status->bandwidth	= 36000;	/* kHz	*/
-	state->bandwidth	= status->bandwidth * 1000;	/* Hz	*/
-	state->reference	= status->refclock / 1000;	/* kHz	*/
+	/*
+	 * iqsense = 1
+	 * tunerstep = 125000
+	 */
+	state->bandwidth        = 36000000;		/* Hz	*/
+	state->reference	= refclk / 1000;	/* kHz	*/
 
 	/* Set default bandwidth. Modified, PN 13-May-10	*/
 	return 0;
 }
 
-static int stb6100_get_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *state)
+static int stb6100_set_params(struct dvb_frontend *fe)
 {
-	switch (param) {
-	case DVBFE_TUNER_FREQUENCY:
-		stb6100_get_frequency(fe, &state->frequency);
-		break;
-	case DVBFE_TUNER_TUNERSTEP:
-		break;
-	case DVBFE_TUNER_IFFREQ:
-		break;
-	case DVBFE_TUNER_BANDWIDTH:
-		stb6100_get_bandwidth(fe, &state->bandwidth);
-		break;
-	case DVBFE_TUNER_REFCLOCK:
-		break;
-	default:
-		break;
-	}
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 
-	return 0;
-}
+	if (c->frequency > 0)
+		stb6100_set_frequency(fe, c->frequency);
 
-static int stb6100_set_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *state)
-{
-	struct stb6100_state *tstate = fe->tuner_priv;
-
-	switch (param) {
-	case DVBFE_TUNER_FREQUENCY:
-		stb6100_set_frequency(fe, state->frequency);
-		tstate->frequency = state->frequency;
-		break;
-	case DVBFE_TUNER_TUNERSTEP:
-		break;
-	case DVBFE_TUNER_IFFREQ:
-		break;
-	case DVBFE_TUNER_BANDWIDTH:
-		stb6100_set_bandwidth(fe, state->bandwidth);
-		tstate->bandwidth = state->bandwidth;
-		break;
-	case DVBFE_TUNER_REFCLOCK:
-		break;
-	default:
-		break;
-	}
+	if (c->bandwidth_hz > 0)
+		stb6100_set_bandwidth(fe, c->bandwidth_hz);
 
 	return 0;
 }
@@ -572,8 +533,9 @@ static struct dvb_tuner_ops stb6100_ops = {
 	.init		= stb6100_init,
 	.sleep          = stb6100_sleep,
 	.get_status	= stb6100_get_status,
-	.get_state	= stb6100_get_state,
-	.set_state	= stb6100_set_state,
+	.set_params	= stb6100_set_params,
+	.get_frequency  = stb6100_get_frequency,
+	.get_bandwidth  = stb6100_get_bandwidth,
 	.release	= stb6100_release
 };
 
diff --git a/drivers/media/dvb-frontends/stb6100.h b/drivers/media/dvb-frontends/stb6100.h
index 218c8188865d..f7b468b6dc26 100644
--- a/drivers/media/dvb-frontends/stb6100.h
+++ b/drivers/media/dvb-frontends/stb6100.h
@@ -86,7 +86,6 @@ struct stb6100_state {
 	const struct stb6100_config	*config;
 	struct dvb_tuner_ops		ops;
 	struct dvb_frontend		*frontend;
-	struct tuner_state		status;
 
 	u32 frequency;
 	u32 srate;
diff --git a/drivers/media/dvb-frontends/stb6100_cfg.h b/drivers/media/dvb-frontends/stb6100_cfg.h
index 6edc15365847..2ef67aa768b9 100644
--- a/drivers/media/dvb-frontends/stb6100_cfg.h
+++ b/drivers/media/dvb-frontends/stb6100_cfg.h
@@ -19,20 +19,21 @@
 	Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
+#include <linux/dvb/frontend.h>
+#include "dvb_frontend.h"
+
 static int stb6100_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
 	int err = 0;
 
-	if (tuner_ops->get_state) {
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_FREQUENCY, &t_state);
+	if (tuner_ops->get_frequency) {
+		err = tuner_ops->get_frequency(fe, frequency);
 		if (err < 0) {
 			printk("%s: Invalid parameter\n", __func__);
 			return err;
 		}
-		*frequency = t_state.frequency;
 	}
 	return 0;
 }
@@ -41,13 +42,16 @@ static int stb6100_set_frequency(struct dvb_frontend *fe, u32 frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u32 bw = c->bandwidth_hz;
 	int err = 0;
 
-	t_state.frequency = frequency;
+	c->frequency = frequency;
+	c->bandwidth_hz = 0;		/* Don't adjust the bandwidth */
 
-	if (tuner_ops->set_state) {
-		err = tuner_ops->set_state(fe, DVBFE_TUNER_FREQUENCY, &t_state);
+	if (tuner_ops->set_params) {
+		err = tuner_ops->set_params(fe);
+		c->bandwidth_hz = bw;
 		if (err < 0) {
 			printk("%s: Invalid parameter\n", __func__);
 			return err;
@@ -60,16 +64,14 @@ static int stb6100_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
 	int err = 0;
 
-	if (tuner_ops->get_state) {
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_BANDWIDTH, &t_state);
+	if (tuner_ops->get_bandwidth) {
+		err = tuner_ops->get_bandwidth(fe, bandwidth);
 		if (err < 0) {
 			printk("%s: Invalid parameter\n", __func__);
 			return err;
 		}
-		*bandwidth = t_state.bandwidth;
 	}
 	return 0;
 }
@@ -78,13 +80,16 @@ static int stb6100_set_bandwidth(struct dvb_frontend *fe, u32 bandwidth)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u32 freq = c->frequency;
 	int err = 0;
 
-	t_state.bandwidth = bandwidth;
+	c->bandwidth_hz = bandwidth;
+	c->frequency = 0;		/* Don't adjust the frequency */
 
-	if (tuner_ops->set_state) {
-		err = tuner_ops->set_state(fe, DVBFE_TUNER_BANDWIDTH, &t_state);
+	if (tuner_ops->set_params) {
+		err = tuner_ops->set_params(fe);
+		c->frequency = freq;
 		if (err < 0) {
 			printk("%s: Invalid parameter\n", __func__);
 			return err;
diff --git a/drivers/media/dvb-frontends/stb6100_proc.h b/drivers/media/dvb-frontends/stb6100_proc.h
index bd8a0ec9e2cc..50ffa21e3871 100644
--- a/drivers/media/dvb-frontends/stb6100_proc.h
+++ b/drivers/media/dvb-frontends/stb6100_proc.h
@@ -17,27 +17,27 @@
 	Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
+#include <linux/dvb/frontend.h>
+#include "dvb_frontend.h"
+
 static int stb6100_get_freq(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	state;
 	int err = 0;
 
-	if (tuner_ops->get_state) {
+	if (tuner_ops->get_frequency) {
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 1);
 
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_FREQUENCY, &state);
+		err = tuner_ops->get_frequency(fe, frequency);
 		if (err < 0) {
-			printk(KERN_ERR "%s: Invalid parameter\n", __func__);
+			printk("%s: Invalid parameter\n", __func__);
 			return err;
 		}
 
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 0);
-
-		*frequency = state.frequency;
 	}
 
 	return 0;
@@ -47,18 +47,21 @@ static int stb6100_set_freq(struct dvb_frontend *fe, u32 frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	state;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u32 bw = c->bandwidth_hz;
 	int err = 0;
 
-	state.frequency = frequency;
+	c->frequency = frequency;
+	c->bandwidth_hz = 0;		/* Don't adjust the bandwidth */
 
-	if (tuner_ops->set_state) {
+	if (tuner_ops->set_params) {
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 1);
 
-		err = tuner_ops->set_state(fe, DVBFE_TUNER_FREQUENCY, &state);
+		err = tuner_ops->set_params(fe);
+		c->bandwidth_hz = bw;
 		if (err < 0) {
-			printk(KERN_ERR "%s: Invalid parameter\n", __func__);
+			printk("%s: Invalid parameter\n", __func__);
 			return err;
 		}
 
@@ -74,14 +77,13 @@ static int stb6100_get_bandw(struct dvb_frontend *fe, u32 *bandwidth)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	state;
 	int err = 0;
 
-	if (tuner_ops->get_state) {
+	if (tuner_ops->get_bandwidth) {
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 1);
 
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_BANDWIDTH, &state);
+		err = tuner_ops->get_bandwidth(fe, bandwidth);
 		if (err < 0) {
 			printk(KERN_ERR "%s: Invalid parameter\n", __func__);
 			return err;
@@ -89,8 +91,6 @@ static int stb6100_get_bandw(struct dvb_frontend *fe, u32 *bandwidth)
 
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 0);
-
-		*bandwidth = state.bandwidth;
 	}
 
 	return 0;
@@ -100,16 +100,19 @@ static int stb6100_set_bandw(struct dvb_frontend *fe, u32 bandwidth)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	state;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	u32 freq = c->frequency;
 	int err = 0;
 
-	state.bandwidth = bandwidth;
+	c->bandwidth_hz = bandwidth;
+	c->frequency = 0;		/* Don't adjust the frequency */
 
-	if (tuner_ops->set_state) {
+	if (tuner_ops->set_params) {
 		if (frontend_ops->i2c_gate_ctrl)
 			frontend_ops->i2c_gate_ctrl(fe, 1);
 
-		err = tuner_ops->set_state(fe, DVBFE_TUNER_BANDWIDTH, &state);
+		err = tuner_ops->set_params(fe);
+		c->frequency = freq;
 		if (err < 0) {
 			printk(KERN_ERR "%s: Invalid parameter\n", __func__);
 			return err;
diff --git a/drivers/media/dvb-frontends/tda1004x.c b/drivers/media/dvb-frontends/tda1004x.c
index 0e209b56c76c..c6abeb4fba9d 100644
--- a/drivers/media/dvb-frontends/tda1004x.c
+++ b/drivers/media/dvb-frontends/tda1004x.c
@@ -903,9 +903,18 @@ static int tda1004x_get_fe(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *fe_params = &fe->dtv_property_cache;
 	struct tda1004x_state* state = fe->demodulator_priv;
+	int status;
 
 	dprintk("%s\n", __func__);
 
+	status = tda1004x_read_byte(state, TDA1004X_STATUS_CD);
+	if (status == -1)
+		return -EIO;
+
+	/* Only update the properties cache if device is locked */
+	if (!(status & 8))
+		return 0;
+
 	// inversion status
 	fe_params->inversion = INVERSION_OFF;
 	if (tda1004x_read_byte(state, TDA1004X_CONFC1) & 0x20)
diff --git a/drivers/media/dvb-frontends/tda665x.c b/drivers/media/dvb-frontends/tda665x.c
index 63cc12378d9a..82f8cc534f33 100644
--- a/drivers/media/dvb-frontends/tda665x.c
+++ b/drivers/media/dvb-frontends/tda665x.c
@@ -66,26 +66,13 @@ exit:
 	return err;
 }
 
-static int tda665x_get_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *tstate)
+static int tda665x_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct tda665x_state *state = fe->tuner_priv;
-	int err = 0;
 
-	switch (param) {
-	case DVBFE_TUNER_FREQUENCY:
-		tstate->frequency = state->frequency;
-		break;
-	case DVBFE_TUNER_BANDWIDTH:
-		break;
-	default:
-		printk(KERN_ERR "%s: Unknown parameter (param=%d)\n", __func__, param);
-		err = -EINVAL;
-		break;
-	}
+	*frequency = state->frequency;
 
-	return err;
+	return 0;
 }
 
 static int tda665x_get_status(struct dvb_frontend *fe, u32 *status)
@@ -111,9 +98,8 @@ exit:
 	return err;
 }
 
-static int tda665x_set_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *tstate)
+static int tda665x_set_frequency(struct dvb_frontend *fe,
+				 u32 new_frequency)
 {
 	struct tda665x_state *state = fe->tuner_priv;
 	const struct tda665x_config *config = state->config;
@@ -121,88 +107,96 @@ static int tda665x_set_state(struct dvb_frontend *fe,
 	u8 buf[4];
 	int err = 0;
 
-	if (param & DVBFE_TUNER_FREQUENCY) {
-
-		frequency = tstate->frequency;
-		if ((frequency < config->frequency_max) || (frequency > config->frequency_min)) {
-			printk(KERN_ERR "%s: Frequency beyond limits, frequency=%d\n", __func__, frequency);
-			return -EINVAL;
-		}
-
-		frequency += config->frequency_offst;
-		frequency *= config->ref_multiplier;
-		frequency += config->ref_divider >> 1;
-		frequency /= config->ref_divider;
-
-		buf[0] = (u8) ((frequency & 0x7f00) >> 8);
-		buf[1] = (u8) (frequency & 0x00ff) >> 0;
-		buf[2] = 0x80 | 0x40 | 0x02;
-		buf[3] = 0x00;
-
-		/* restore frequency */
-		frequency = tstate->frequency;
-
-		if (frequency < 153000000) {
-			/* VHF-L */
-			buf[3] |= 0x01; /* fc, Low Band, 47 - 153 MHz */
-			if (frequency < 68000000)
-				buf[3] |= 0x40; /* 83uA */
-			if (frequency < 1040000000)
-				buf[3] |= 0x60; /* 122uA */
-			if (frequency < 1250000000)
-				buf[3] |= 0x80; /* 163uA */
-			else
-				buf[3] |= 0xa0; /* 254uA */
-		} else if (frequency < 438000000) {
-			/* VHF-H */
-			buf[3] |= 0x02; /* fc, Mid Band, 153 - 438 MHz */
-			if (frequency < 230000000)
-				buf[3] |= 0x40;
-			if (frequency < 300000000)
-				buf[3] |= 0x60;
-			else
-				buf[3] |= 0x80;
-		} else {
-			/* UHF */
-			buf[3] |= 0x04; /* fc, High Band, 438 - 862 MHz */
-			if (frequency < 470000000)
-				buf[3] |= 0x60;
-			if (frequency < 526000000)
-				buf[3] |= 0x80;
-			else
-				buf[3] |= 0xa0;
-		}
-
-		/* Set params */
-		err = tda665x_write(state, buf, 5);
-		if (err < 0)
-			goto exit;
-
-		/* sleep for some time */
-		printk(KERN_DEBUG "%s: Waiting to Phase LOCK\n", __func__);
-		msleep(20);
-		/* check status */
-		err = tda665x_get_status(fe, &status);
-		if (err < 0)
-			goto exit;
-
-		if (status == 1) {
-			printk(KERN_DEBUG "%s: Tuner Phase locked: status=%d\n", __func__, status);
-			state->frequency = frequency; /* cache successful state */
-		} else {
-			printk(KERN_ERR "%s: No Phase lock: status=%d\n", __func__, status);
-		}
-	} else {
-		printk(KERN_ERR "%s: Unknown parameter (param=%d)\n", __func__, param);
+	if ((new_frequency < config->frequency_max)
+	    || (new_frequency > config->frequency_min)) {
+		printk(KERN_ERR "%s: Frequency beyond limits, frequency=%d\n",
+		       __func__, new_frequency);
 		return -EINVAL;
 	}
 
+	frequency = new_frequency;
+
+	frequency += config->frequency_offst;
+	frequency *= config->ref_multiplier;
+	frequency += config->ref_divider >> 1;
+	frequency /= config->ref_divider;
+
+	buf[0] = (u8) ((frequency & 0x7f00) >> 8);
+	buf[1] = (u8) (frequency & 0x00ff) >> 0;
+	buf[2] = 0x80 | 0x40 | 0x02;
+	buf[3] = 0x00;
+
+	/* restore frequency */
+	frequency = new_frequency;
+
+	if (frequency < 153000000) {
+		/* VHF-L */
+		buf[3] |= 0x01; /* fc, Low Band, 47 - 153 MHz */
+		if (frequency < 68000000)
+			buf[3] |= 0x40; /* 83uA */
+		if (frequency < 1040000000)
+			buf[3] |= 0x60; /* 122uA */
+		if (frequency < 1250000000)
+			buf[3] |= 0x80; /* 163uA */
+		else
+			buf[3] |= 0xa0; /* 254uA */
+	} else if (frequency < 438000000) {
+		/* VHF-H */
+		buf[3] |= 0x02; /* fc, Mid Band, 153 - 438 MHz */
+		if (frequency < 230000000)
+			buf[3] |= 0x40;
+		if (frequency < 300000000)
+			buf[3] |= 0x60;
+		else
+			buf[3] |= 0x80;
+	} else {
+		/* UHF */
+		buf[3] |= 0x04; /* fc, High Band, 438 - 862 MHz */
+		if (frequency < 470000000)
+			buf[3] |= 0x60;
+		if (frequency < 526000000)
+			buf[3] |= 0x80;
+		else
+			buf[3] |= 0xa0;
+	}
+
+	/* Set params */
+	err = tda665x_write(state, buf, 5);
+	if (err < 0)
+		goto exit;
+
+	/* sleep for some time */
+	printk(KERN_DEBUG "%s: Waiting to Phase LOCK\n", __func__);
+	msleep(20);
+	/* check status */
+	err = tda665x_get_status(fe, &status);
+	if (err < 0)
+		goto exit;
+
+	if (status == 1) {
+		printk(KERN_DEBUG "%s: Tuner Phase locked: status=%d\n",
+		       __func__, status);
+		state->frequency = frequency; /* cache successful state */
+	} else {
+		printk(KERN_ERR "%s: No Phase lock: status=%d\n",
+		       __func__, status);
+	}
+
 	return 0;
 exit:
 	printk(KERN_ERR "%s: I/O Error\n", __func__);
 	return err;
 }
 
+static int tda665x_set_params(struct dvb_frontend *fe)
+{
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+	tda665x_set_frequency(fe, c->frequency);
+
+	return 0;
+}
+
 static int tda665x_release(struct dvb_frontend *fe)
 {
 	struct tda665x_state *state = fe->tuner_priv;
@@ -213,10 +207,9 @@ static int tda665x_release(struct dvb_frontend *fe)
 }
 
 static struct dvb_tuner_ops tda665x_ops = {
-
-	.set_state	= tda665x_set_state,
-	.get_state	= tda665x_get_state,
 	.get_status	= tda665x_get_status,
+	.set_params	= tda665x_set_params,
+	.get_frequency	= tda665x_get_frequency,
 	.release	= tda665x_release
 };
 
diff --git a/drivers/media/dvb-frontends/tda8261.c b/drivers/media/dvb-frontends/tda8261.c
index 19c488814e5c..3285b1bc4642 100644
--- a/drivers/media/dvb-frontends/tda8261.c
+++ b/drivers/media/dvb-frontends/tda8261.c
@@ -83,88 +83,71 @@ static int tda8261_get_status(struct dvb_frontend *fe, u32 *status)
 static const u32 div_tab[] = { 2000, 1000,  500,  250,  125 }; /* kHz */
 static const u8  ref_div[] = { 0x00, 0x01, 0x02, 0x05, 0x07 };
 
-static int tda8261_get_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *tstate)
+static int tda8261_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct tda8261_state *state = fe->tuner_priv;
-	int err = 0;
 
-	switch (param) {
-	case DVBFE_TUNER_FREQUENCY:
-		tstate->frequency = state->frequency;
-		break;
-	case DVBFE_TUNER_BANDWIDTH:
-		tstate->bandwidth = 40000000; /* FIXME! need to calculate Bandwidth */
-		break;
-	default:
-		pr_err("%s: Unknown parameter (param=%d)\n", __func__, param);
-		err = -EINVAL;
-		break;
-	}
+	*frequency = state->frequency;
 
-	return err;
+	return 0;
 }
 
-static int tda8261_set_state(struct dvb_frontend *fe,
-			     enum tuner_param param,
-			     struct tuner_state *tstate)
+static int tda8261_set_params(struct dvb_frontend *fe)
 {
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct tda8261_state *state = fe->tuner_priv;
 	const struct tda8261_config *config = state->config;
 	u32 frequency, N, status = 0;
 	u8 buf[4];
 	int err = 0;
 
-	if (param & DVBFE_TUNER_FREQUENCY) {
-		/**
-		 * N = Max VCO Frequency / Channel Spacing
-		 * Max VCO Frequency = VCO frequency + (channel spacing - 1)
-		 * (to account for half channel spacing on either side)
-		 */
-		frequency = tstate->frequency;
-		if ((frequency < 950000) || (frequency > 2150000)) {
-			pr_warn("%s: Frequency beyond limits, frequency=%d\n", __func__, frequency);
-			return -EINVAL;
-		}
-		N = (frequency + (div_tab[config->step_size] - 1)) / div_tab[config->step_size];
-		pr_debug("%s: Step size=%d, Divider=%d, PG=0x%02x (%d)\n",
-			__func__, config->step_size, div_tab[config->step_size], N, N);
-
-		buf[0] = (N >> 8) & 0xff;
-		buf[1] = N & 0xff;
-		buf[2] = (0x01 << 7) | ((ref_div[config->step_size] & 0x07) << 1);
-
-		if (frequency < 1450000)
-			buf[3] = 0x00;
-		else if (frequency < 2000000)
-			buf[3] = 0x40;
-		else if (frequency < 2150000)
-			buf[3] = 0x80;
-
-		/* Set params */
-		if ((err = tda8261_write(state, buf)) < 0) {
-			pr_err("%s: I/O Error\n", __func__);
-			return err;
-		}
-		/* sleep for some time */
-		pr_debug("%s: Waiting to Phase LOCK\n", __func__);
-		msleep(20);
-		/* check status */
-		if ((err = tda8261_get_status(fe, &status)) < 0) {
-			pr_err("%s: I/O Error\n", __func__);
-			return err;
-		}
-		if (status == 1) {
-			pr_debug("%s: Tuner Phase locked: status=%d\n", __func__, status);
-			state->frequency = frequency; /* cache successful state */
-		} else {
-			pr_debug("%s: No Phase lock: status=%d\n", __func__, status);
-		}
-	} else {
-		pr_err("%s: Unknown parameter (param=%d)\n", __func__, param);
+	/*
+	 * N = Max VCO Frequency / Channel Spacing
+	 * Max VCO Frequency = VCO frequency + (channel spacing - 1)
+	 * (to account for half channel spacing on either side)
+	 */
+	frequency = c->frequency;
+	if ((frequency < 950000) || (frequency > 2150000)) {
+		pr_warn("%s: Frequency beyond limits, frequency=%d\n",
+			__func__, frequency);
 		return -EINVAL;
 	}
+	N = (frequency + (div_tab[config->step_size] - 1)) / div_tab[config->step_size];
+	pr_debug("%s: Step size=%d, Divider=%d, PG=0x%02x (%d)\n",
+		__func__, config->step_size, div_tab[config->step_size], N, N);
+
+	buf[0] = (N >> 8) & 0xff;
+	buf[1] = N & 0xff;
+	buf[2] = (0x01 << 7) | ((ref_div[config->step_size] & 0x07) << 1);
+
+	if (frequency < 1450000)
+		buf[3] = 0x00;
+	else if (frequency < 2000000)
+		buf[3] = 0x40;
+	else if (frequency < 2150000)
+		buf[3] = 0x80;
+
+	/* Set params */
+	err = tda8261_write(state, buf);
+	if (err < 0) {
+		pr_err("%s: I/O Error\n", __func__);
+		return err;
+	}
+	/* sleep for some time */
+	pr_debug("%s: Waiting to Phase LOCK\n", __func__);
+	msleep(20);
+	/* check status */
+	if ((err = tda8261_get_status(fe, &status)) < 0) {
+		pr_err("%s: I/O Error\n", __func__);
+		return err;
+	}
+	if (status == 1) {
+		pr_debug("%s: Tuner Phase locked: status=%d\n", __func__,
+			 status);
+		state->frequency = frequency; /* cache successful state */
+	} else {
+		pr_debug("%s: No Phase lock: status=%d\n", __func__, status);
+	}
 
 	return 0;
 }
@@ -182,14 +165,13 @@ static struct dvb_tuner_ops tda8261_ops = {
 
 	.info = {
 		.name		= "TDA8261",
-//		.tuner_name	= NULL,
 		.frequency_min	=  950000,
 		.frequency_max	= 2150000,
 		.frequency_step = 0
 	},
 
-	.set_state	= tda8261_set_state,
-	.get_state	= tda8261_get_state,
+	.set_params	= tda8261_set_params,
+	.get_frequency	= tda8261_get_frequency,
 	.get_status	= tda8261_get_status,
 	.release	= tda8261_release
 };
@@ -210,10 +192,7 @@ struct dvb_frontend *tda8261_attach(struct dvb_frontend *fe,
 	fe->ops.tuner_ops	= tda8261_ops;
 
 	fe->ops.tuner_ops.info.frequency_step = div_tab[config->step_size];
-//	fe->ops.tuner_ops.tuner_name	 = &config->buf;
 
-//	printk("%s: Attaching %s TDA8261 8PSK/QPSK tuner\n",
-//		__func__, fe->ops.tuner_ops.tuner_name);
 	pr_info("%s: Attaching TDA8261 8PSK/QPSK tuner\n", __func__);
 
 	return fe;
diff --git a/drivers/media/dvb-frontends/tda8261_cfg.h b/drivers/media/dvb-frontends/tda8261_cfg.h
index 04a19e14ee5a..fe527ff84df4 100644
--- a/drivers/media/dvb-frontends/tda8261_cfg.h
+++ b/drivers/media/dvb-frontends/tda8261_cfg.h
@@ -21,17 +21,15 @@ static int tda8261_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
 	int err = 0;
 
-	if (tuner_ops->get_state) {
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_FREQUENCY, &t_state);
+	if (tuner_ops->get_frequency) {
+		err = tuner_ops->get_frequency(fe, frequency);
 		if (err < 0) {
-			printk("%s: Invalid parameter\n", __func__);
+			pr_err("%s: Invalid parameter\n", __func__);
 			return err;
 		}
-		*frequency = t_state.frequency;
-		printk("%s: Frequency=%d\n", __func__, t_state.frequency);
+		pr_debug("%s: Frequency=%d\n", __func__, *frequency);
 	}
 	return 0;
 }
@@ -40,37 +38,24 @@ static int tda8261_set_frequency(struct dvb_frontend *fe, u32 frequency)
 {
 	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
 	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int err = 0;
 
-	t_state.frequency = frequency;
-
-	if (tuner_ops->set_state) {
-		err = tuner_ops->set_state(fe, DVBFE_TUNER_FREQUENCY, &t_state);
+	if (tuner_ops->set_params) {
+		err = tuner_ops->set_params(fe);
 		if (err < 0) {
-			printk("%s: Invalid parameter\n", __func__);
+			pr_err("%s: Invalid parameter\n", __func__);
 			return err;
 		}
 	}
-	printk("%s: Frequency=%d\n", __func__, t_state.frequency);
+	pr_debug("%s: Frequency=%d\n", __func__, c->frequency);
 	return 0;
 }
 
 static int tda8261_get_bandwidth(struct dvb_frontend *fe, u32 *bandwidth)
 {
-	struct dvb_frontend_ops	*frontend_ops = &fe->ops;
-	struct dvb_tuner_ops	*tuner_ops = &frontend_ops->tuner_ops;
-	struct tuner_state	t_state;
-	int err = 0;
+	/* FIXME! need to calculate Bandwidth */
+	*bandwidth = 40000000;
 
-	if (tuner_ops->get_state) {
-		err = tuner_ops->get_state(fe, DVBFE_TUNER_BANDWIDTH, &t_state);
-		if (err < 0) {
-			printk("%s: Invalid parameter\n", __func__);
-			return err;
-		}
-		*bandwidth = t_state.bandwidth;
-		printk("%s: Bandwidth=%d\n", __func__, t_state.bandwidth);
-	}
 	return 0;
 }
diff --git a/drivers/media/dvb-frontends/tdhd1.h b/drivers/media/dvb-frontends/tdhd1.h
index 17750985db0c..2b9e8732c802 100644
--- a/drivers/media/dvb-frontends/tdhd1.h
+++ b/drivers/media/dvb-frontends/tdhd1.h
@@ -20,7 +20,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * The project's page is at http://www.linuxtv.org
+ * The project's page is at https://linuxtv.org
  */
 
 #ifndef TDHD1_H
diff --git a/drivers/media/firewire/firedtv-ci.c b/drivers/media/firewire/firedtv-ci.c
index e63f582378bf..edbb30fdd9d9 100644
--- a/drivers/media/firewire/firedtv-ci.c
+++ b/drivers/media/firewire/firedtv-ci.c
@@ -241,7 +241,7 @@ int fdtv_ca_register(struct firedtv *fdtv)
 		return -EFAULT;
 
 	err = dvb_register_device(&fdtv->adapter, &fdtv->cadev,
-				  &fdtv_ca, fdtv, DVB_DEVICE_CA);
+				  &fdtv_ca, fdtv, DVB_DEVICE_CA, 0);
 
 	if (stat.ca_application_info == 0)
 		dev_err(fdtv->device, "CaApplicationInfo is not set\n");
diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
index 521bbf1b29bc..993dc50c12db 100644
--- a/drivers/media/i2c/Kconfig
+++ b/drivers/media/i2c/Kconfig
@@ -83,6 +83,16 @@ config VIDEO_MSP3400
 	  To compile this driver as a module, choose M here: the
 	  module will be called msp3400.
 
+config VIDEO_CS3308
+	tristate "Cirrus Logic CS3308 audio ADC"
+	depends on VIDEO_V4L2 && I2C
+	---help---
+	  Support for the Cirrus Logic CS3308 High Performance 8-Channel
+	  Analog Volume Control
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called cs3308.
+
 config VIDEO_CS5345
 	tristate "Cirrus Logic CS5345 audio ADC"
 	depends on VIDEO_V4L2 && I2C
diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
index 07db257abfc1..94f2c99e890d 100644
--- a/drivers/media/i2c/Makefile
+++ b/drivers/media/i2c/Makefile
@@ -43,6 +43,7 @@ obj-$(CONFIG_VIDEO_TVP7002) += tvp7002.o
 obj-$(CONFIG_VIDEO_TW2804) += tw2804.o
 obj-$(CONFIG_VIDEO_TW9903) += tw9903.o
 obj-$(CONFIG_VIDEO_TW9906) += tw9906.o
+obj-$(CONFIG_VIDEO_CS3308) += cs3308.o
 obj-$(CONFIG_VIDEO_CS5345) += cs5345.o
 obj-$(CONFIG_VIDEO_CS53L32A) += cs53l32a.o
 obj-$(CONFIG_VIDEO_M52790) += m52790.o
diff --git a/drivers/media/i2c/ad9389b.c b/drivers/media/i2c/ad9389b.c
index 69094ab047b1..788967dadd29 100644
--- a/drivers/media/i2c/ad9389b.c
+++ b/drivers/media/i2c/ad9389b.c
@@ -35,7 +35,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-dv-timings.h>
 #include <media/v4l2-ctrls.h>
-#include <media/ad9389b.h>
+#include <media/i2c/ad9389b.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -1158,7 +1158,7 @@ static int ad9389b_probe(struct i2c_client *client, const struct i2c_device_id *
 	state->rgb_quantization_range_ctrl->is_private = true;
 
 	state->pad.flags = MEDIA_PAD_FL_SINK;
-	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
 	if (err)
 		goto err_hdl;
 
diff --git a/drivers/media/i2c/adp1653.c b/drivers/media/i2c/adp1653.c
index 5dd39775d6ca..fb7ed730d932 100644
--- a/drivers/media/i2c/adp1653.c
+++ b/drivers/media/i2c/adp1653.c
@@ -37,7 +37,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/gpio/consumer.h>
-#include <media/adp1653.h>
+#include <media/i2c/adp1653.h>
 #include <media/v4l2-device.h>
 
 #define TIMEOUT_MAX		820000
@@ -497,7 +497,7 @@ static int adp1653_probe(struct i2c_client *client,
 		if (!client->dev.platform_data) {
 			dev_err(&client->dev,
 				"Neither DT not platform data provided\n");
-			return EINVAL;
+			return -EINVAL;
 		}
 		flash->platform_data = client->dev.platform_data;
 	}
@@ -512,11 +512,11 @@ static int adp1653_probe(struct i2c_client *client,
 	if (ret)
 		goto free_and_quit;
 
-	ret = media_entity_init(&flash->subdev.entity, 0, NULL, 0);
+	ret = media_entity_pads_init(&flash->subdev.entity, 0, NULL);
 	if (ret < 0)
 		goto free_and_quit;
 
-	flash->subdev.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+	flash->subdev.entity.function = MEDIA_ENT_F_FLASH;
 
 	return 0;
 
diff --git a/drivers/media/i2c/adv7180.c b/drivers/media/i2c/adv7180.c
index f82c8aa164fa..ff57c1dcb8af 100644
--- a/drivers/media/i2c/adv7180.c
+++ b/drivers/media/i2c/adv7180.c
@@ -1112,7 +1112,7 @@ static int init_device(struct adv7180_state *state)
 	mutex_lock(&state->mutex);
 
 	adv7180_write(state, ADV7180_REG_PWR_MAN, ADV7180_PWR_MAN_RES);
-	usleep_range(2000, 10000);
+	usleep_range(5000, 10000);
 
 	ret = state->chip_info->init(state);
 	if (ret)
@@ -1213,8 +1213,8 @@ static int adv7180_probe(struct i2c_client *client,
 		goto err_unregister_vpp_client;
 
 	state->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.flags |= MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
-	ret = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	sd->entity.flags |= MEDIA_ENT_F_ATV_DECODER;
+	ret = media_entity_pads_init(&sd->entity, 1, &state->pad);
 	if (ret)
 		goto err_free_ctrl;
 
diff --git a/drivers/media/i2c/adv7183.c b/drivers/media/i2c/adv7183.c
index e2dd1617662f..2bec737881e9 100644
--- a/drivers/media/i2c/adv7183.c
+++ b/drivers/media/i2c/adv7183.c
@@ -27,7 +27,7 @@
 #include <linux/types.h>
 #include <linux/videodev2.h>
 
-#include <media/adv7183.h>
+#include <media/i2c/adv7183.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 
diff --git a/drivers/media/i2c/adv7343.c b/drivers/media/i2c/adv7343.c
index f89d0afcd964..11f9029433cf 100644
--- a/drivers/media/i2c/adv7343.c
+++ b/drivers/media/i2c/adv7343.c
@@ -28,7 +28,7 @@
 #include <linux/of.h>
 #include <linux/of_graph.h>
 
-#include <media/adv7343.h>
+#include <media/i2c/adv7343.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/i2c/adv7393.c b/drivers/media/i2c/adv7393.c
index 0215f95c2245..76d987476e35 100644
--- a/drivers/media/i2c/adv7393.c
+++ b/drivers/media/i2c/adv7393.c
@@ -31,7 +31,7 @@
 #include <linux/videodev2.h>
 #include <linux/uaccess.h>
 
-#include <media/adv7393.h>
+#include <media/i2c/adv7393.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 
diff --git a/drivers/media/i2c/adv7511.c b/drivers/media/i2c/adv7511.c
index e4900df1140b..471fd23b5c5c 100644
--- a/drivers/media/i2c/adv7511.c
+++ b/drivers/media/i2c/adv7511.c
@@ -32,7 +32,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/adv7511.h>
+#include <media/i2c/adv7511.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -1116,7 +1116,7 @@ static int adv7511_set_fmt(struct v4l2_subdev *sd,
 	adv7511_wr_and_or(sd, 0x55, 0x9f, y << 5);
 	adv7511_wr_and_or(sd, 0x56, 0x3f, c << 6);
 	adv7511_wr_and_or(sd, 0x57, 0x83, (ec << 4) | (q << 2));
-	adv7511_wr_and_or(sd, 0x59, 0x0f, yq << 4);
+	adv7511_wr_and_or(sd, 0x59, 0x3f, yq << 6);
 	adv7511_wr_and_or(sd, 0x4a, 0xff, 1);
 
 	return 0;
@@ -1482,7 +1482,7 @@ static int adv7511_probe(struct i2c_client *client, const struct i2c_device_id *
 	state->rgb_quantization_range_ctrl->is_private = true;
 
 	state->pad.flags = MEDIA_PAD_FL_SINK;
-	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
 	if (err)
 		goto err_hdl;
 
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index 5631ec004eed..e1719ffdfb3d 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -39,7 +39,7 @@
 #include <linux/workqueue.h>
 #include <linux/regmap.h>
 
-#include <media/adv7604.h>
+#include <media/i2c/adv7604.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-event.h>
@@ -905,7 +905,7 @@ static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
 
 	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
 		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
-					is_digital_input(sd) ? 250000 : 1000000))
+				is_digital_input(sd) ? 250000 : 1000000, false))
 			continue;
 		io_write(sd, 0x00, predef_vid_timings[i].vid_std); /* video std */
 		io_write(sd, 0x01, (predef_vid_timings[i].v_freq << 4) +
@@ -1479,7 +1479,7 @@ static void adv76xx_fill_optional_dv_timings_fields(struct v4l2_subdev *sd,
 
 	for (i = 0; adv76xx_timings[i].bt.width; i++) {
 		if (v4l2_match_dv_timings(timings, &adv76xx_timings[i],
-					is_digital_input(sd) ? 250000 : 1000000)) {
+				is_digital_input(sd) ? 250000 : 1000000, false)) {
 			*timings = adv76xx_timings[i];
 			break;
 		}
@@ -1644,7 +1644,7 @@ static int adv76xx_s_dv_timings(struct v4l2_subdev *sd,
 	if (!timings)
 		return -EINVAL;
 
-	if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
 		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
 		return 0;
 	}
@@ -1960,10 +1960,9 @@ static int adv76xx_isr(struct v4l2_subdev *sd, u32 status, bool *handled)
 	}
 
 	/* tx 5v detect */
-	tx_5v = io_read(sd, 0x70) & info->cable_det_mask;
+	tx_5v = irq_reg_0x70 & info->cable_det_mask;
 	if (tx_5v) {
 		v4l2_dbg(1, debug, sd, "%s: tx_5v: 0x%x\n", __func__, tx_5v);
-		io_write(sd, 0x71, tx_5v);
 		adv76xx_s_detect_tx_5v_ctrl(sd);
 		if (handled)
 			*handled = true;
@@ -3208,8 +3207,8 @@ static int adv76xx_probe(struct i2c_client *client,
 		state->pads[i].flags = MEDIA_PAD_FL_SINK;
 	state->pads[state->source_pad].flags = MEDIA_PAD_FL_SOURCE;
 
-	err = media_entity_init(&sd->entity, state->source_pad + 1,
-				state->pads, 0);
+	err = media_entity_pads_init(&sd->entity, state->source_pad + 1,
+				state->pads);
 	if (err)
 		goto err_work_queues;
 
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index b7269b8f040d..5fbb788e7b59 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -43,7 +43,7 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/adv7842.h>
+#include <media/i2c/adv7842.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -155,7 +155,7 @@ static bool adv7842_check_dv_timings(const struct v4l2_dv_timings *t, void *hdl)
 	int i;
 
 	for (i = 0; adv7842_timings_exceptions[i].bt.width; i++)
-		if (v4l2_match_dv_timings(t, adv7842_timings_exceptions + i, 0))
+		if (v4l2_match_dv_timings(t, adv7842_timings_exceptions + i, 0, false))
 			return false;
 	return true;
 }
@@ -1008,7 +1008,7 @@ static int find_and_set_predefined_video_timings(struct v4l2_subdev *sd,
 
 	for (i = 0; predef_vid_timings[i].timings.bt.width; i++) {
 		if (!v4l2_match_dv_timings(timings, &predef_vid_timings[i].timings,
-					  is_digital_input(sd) ? 250000 : 1000000))
+				  is_digital_input(sd) ? 250000 : 1000000, false))
 			continue;
 		/* video std */
 		io_write(sd, 0x00, predef_vid_timings[i].vid_std);
@@ -1659,7 +1659,7 @@ static int adv7842_s_dv_timings(struct v4l2_subdev *sd,
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
 
-	if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
 		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
 		return 0;
 	}
@@ -3309,7 +3309,7 @@ static int adv7842_probe(struct i2c_client *client,
 			adv7842_delayed_work_enable_hotplug);
 
 	state->pad.flags = MEDIA_PAD_FL_SOURCE;
-	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
 	if (err)
 		goto err_work_queues;
 
diff --git a/drivers/media/i2c/ak881x.c b/drivers/media/i2c/ak881x.c
index d3b965ec3bbc..d9f2b6b76d59 100644
--- a/drivers/media/i2c/ak881x.c
+++ b/drivers/media/i2c/ak881x.c
@@ -15,7 +15,7 @@
 #include <linux/videodev2.h>
 #include <linux/module.h>
 
-#include <media/ak881x.h>
+#include <media/i2c/ak881x.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 
diff --git a/drivers/media/i2c/as3645a.c b/drivers/media/i2c/as3645a.c
index 301084b07887..2e90e4094b79 100644
--- a/drivers/media/i2c/as3645a.c
+++ b/drivers/media/i2c/as3645a.c
@@ -31,7 +31,7 @@
 #include <linux/mutex.h>
 #include <linux/slab.h>
 
-#include <media/as3645a.h>
+#include <media/i2c/as3645a.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 
@@ -827,11 +827,11 @@ static int as3645a_probe(struct i2c_client *client,
 	if (ret < 0)
 		goto done;
 
-	ret = media_entity_init(&flash->subdev.entity, 0, NULL, 0);
+	ret = media_entity_pads_init(&flash->subdev.entity, 0, NULL);
 	if (ret < 0)
 		goto done;
 
-	flash->subdev.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+	flash->subdev.entity.function = MEDIA_ENT_F_FLASH;
 
 	mutex_init(&flash->power_lock);
 
diff --git a/drivers/media/i2c/bt819.c b/drivers/media/i2c/bt819.c
index e00e3104d448..7907bcfbaed3 100644
--- a/drivers/media/i2c/bt819.c
+++ b/drivers/media/i2c/bt819.c
@@ -37,7 +37,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/bt819.h>
+#include <media/i2c/bt819.h>
 
 MODULE_DESCRIPTION("Brooktree-819 video decoder driver");
 MODULE_AUTHOR("Mike Bernson & Dave Perks");
diff --git a/drivers/media/i2c/cs3308.c b/drivers/media/i2c/cs3308.c
new file mode 100644
index 000000000000..d28b4f37fe5f
--- /dev/null
+++ b/drivers/media/i2c/cs3308.c
@@ -0,0 +1,138 @@
+/*
+ * Cirrus Logic cs3308 8-Channel Analog Volume Control
+ *
+ * Copyright (C) 2010 Devin Heitmueller <dheitmueller@kernellabs.com>
+ * Copyright (C) 2012 Steven Toth <stoth@kernellabs.com>
+ *
+ * Derived from cs5345.c Copyright (C) 2007 Hans Verkuil
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-device.h>
+
+MODULE_DESCRIPTION("i2c device driver for cs3308 8-channel volume control");
+MODULE_AUTHOR("Devin Heitmueller");
+MODULE_LICENSE("GPL");
+
+static inline int cs3308_write(struct v4l2_subdev *sd, u8 reg, u8 value)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_write_byte_data(client, reg, value);
+}
+
+static inline int cs3308_read(struct v4l2_subdev *sd, u8 reg)
+{
+	struct i2c_client *client = v4l2_get_subdevdata(sd);
+
+	return i2c_smbus_read_byte_data(client, reg);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int cs3308_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
+{
+	reg->val = cs3308_read(sd, reg->reg & 0xffff);
+	reg->size = 1;
+	return 0;
+}
+
+static int cs3308_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
+{
+	cs3308_write(sd, reg->reg & 0xffff, reg->val & 0xff);
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------------------- */
+
+static const struct v4l2_subdev_core_ops cs3308_core_ops = {
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.g_register = cs3308_g_register,
+	.s_register = cs3308_s_register,
+#endif
+};
+
+static const struct v4l2_subdev_ops cs3308_ops = {
+	.core = &cs3308_core_ops,
+};
+
+/* ----------------------------------------------------------------------- */
+
+static int cs3308_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct v4l2_subdev *sd;
+	unsigned i;
+
+	/* Check if the adapter supports the needed features */
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -EIO;
+
+	if ((i2c_smbus_read_byte_data(client, 0x1c) & 0xf0) != 0xe0)
+		return -ENODEV;
+
+	v4l_info(client, "chip found @ 0x%x (%s)\n",
+		 client->addr << 1, client->adapter->name);
+
+	sd = kzalloc(sizeof(struct v4l2_subdev), GFP_KERNEL);
+	if (sd == NULL)
+		return -ENOMEM;
+
+	v4l2_i2c_subdev_init(sd, client, &cs3308_ops);
+
+	/* Set some reasonable defaults */
+	cs3308_write(sd, 0x0d, 0x00); /* Power up all channels */
+	cs3308_write(sd, 0x0e, 0x00); /* Master Power */
+	cs3308_write(sd, 0x0b, 0x00); /* Device Configuration */
+	/* Set volume for each channel */
+	for (i = 1; i <= 8; i++)
+		cs3308_write(sd, i, 0xd2);
+	cs3308_write(sd, 0x0a, 0x00); /* Unmute all channels */
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static int cs3308_remove(struct i2c_client *client)
+{
+	struct v4l2_subdev *sd = i2c_get_clientdata(client);
+
+	v4l2_device_unregister_subdev(sd);
+	kfree(sd);
+	return 0;
+}
+
+/* ----------------------------------------------------------------------- */
+
+static const struct i2c_device_id cs3308_id[] = {
+	{ "cs3308", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, cs3308_id);
+
+static struct i2c_driver cs3308_driver = {
+	.driver = {
+		.owner  = THIS_MODULE,
+		.name   = "cs3308",
+	},
+	.probe          = cs3308_probe,
+	.remove         = cs3308_remove,
+	.id_table       = cs3308_id,
+};
+
+module_i2c_driver(cs3308_driver);
diff --git a/drivers/media/i2c/cx25840/cx25840-audio.c b/drivers/media/i2c/cx25840/cx25840-audio.c
index 34b96c7cfd62..baf3d9c8710e 100644
--- a/drivers/media/i2c/cx25840/cx25840-audio.c
+++ b/drivers/media/i2c/cx25840/cx25840-audio.c
@@ -19,7 +19,7 @@
 #include <linux/videodev2.h>
 #include <linux/i2c.h>
 #include <media/v4l2-common.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 #include "cx25840-core.h"
 
diff --git a/drivers/media/i2c/cx25840/cx25840-core.c b/drivers/media/i2c/cx25840/cx25840-core.c
index fe6eb78b6914..07a3e7173144 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.c
+++ b/drivers/media/i2c/cx25840/cx25840-core.c
@@ -45,7 +45,7 @@
 #include <linux/delay.h>
 #include <linux/math64.h>
 #include <media/v4l2-common.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 #include "cx25840-core.h"
 
@@ -559,7 +559,10 @@ static void cx23885_initialize(struct i2c_client *client)
 	cx25840_write4(client, 0x414, 0x00107d12);
 
 	/* Chroma */
-	cx25840_write4(client, 0x420, 0x3d008282);
+	if (is_cx23888(state))
+		cx25840_write4(client, 0x418, 0x1d008282);
+	else
+		cx25840_write4(client, 0x420, 0x3d008282);
 
 	/*
 	 * Aux PLL
@@ -666,14 +669,17 @@ static void cx23885_initialize(struct i2c_client *client)
 	cx25840_write4(client, 0x404, 0x0010253e);
 
 	/* CC on  - Undocumented Register */
-	cx25840_write(client, 0x42f, 0x66);
+	cx25840_write(client, state->vbi_regs_offset + 0x42f, 0x66);
 
 	/* HVR-1250 / HVR1850 DIF related */
 	/* Power everything up */
 	cx25840_write4(client, 0x130, 0x0);
 
 	/* Undocumented */
-	cx25840_write4(client, 0x478, 0x6628021F);
+	if (is_cx23888(state))
+		cx25840_write4(client, 0x454, 0x6628021F);
+	else
+		cx25840_write4(client, 0x478, 0x6628021F);
 
 	/* AFE_CLK_OUT_CTRL - Select the clock output source as output */
 	cx25840_write4(client, 0x144, 0x5);
@@ -1106,31 +1112,15 @@ static int set_input(struct i2c_client *client, enum cx25840_video_input vid_inp
 			cx25840_write4(client, 0x410, 0xffff0dbf);
 			cx25840_write4(client, 0x414, 0x00137d03);
 
-			/* on the 887, 0x418 is HSCALE_CTRL, on the 888 it is 
-			   CHROMA_CTRL */
-			if (is_cx23888(state))
-				cx25840_write4(client, 0x418, 0x01008080);
-			else
-				cx25840_write4(client, 0x418, 0x01000000);
+			cx25840_write4(client, state->vbi_regs_offset + 0x42c, 0x42600000);
+			cx25840_write4(client, state->vbi_regs_offset + 0x430, 0x0000039b);
+			cx25840_write4(client, state->vbi_regs_offset + 0x438, 0x00000000);
 
-			cx25840_write4(client, 0x41c, 0x00000000);
-
-			/* on the 887, 0x420 is CHROMA_CTRL, on the 888 it is 
-			   CRUSH_CTRL */
-			if (is_cx23888(state))
-				cx25840_write4(client, 0x420, 0x001c3e0f);
-			else
-				cx25840_write4(client, 0x420, 0x001c8282);
-
-			cx25840_write4(client, 0x42c, 0x42600000);
-			cx25840_write4(client, 0x430, 0x0000039b);
-			cx25840_write4(client, 0x438, 0x00000000);
-
-			cx25840_write4(client, 0x440, 0xF8E3E824);
-			cx25840_write4(client, 0x444, 0x401040dc);
-			cx25840_write4(client, 0x448, 0xcd3f02a0);
-			cx25840_write4(client, 0x44c, 0x161f1000);
-			cx25840_write4(client, 0x450, 0x00000802);
+			cx25840_write4(client, state->vbi_regs_offset + 0x440, 0xF8E3E824);
+			cx25840_write4(client, state->vbi_regs_offset + 0x444, 0x401040dc);
+			cx25840_write4(client, state->vbi_regs_offset + 0x448, 0xcd3f02a0);
+			cx25840_write4(client, state->vbi_regs_offset + 0x44c, 0x161f1000);
+			cx25840_write4(client, state->vbi_regs_offset + 0x450, 0x00000802);
 
 			cx25840_write4(client, 0x91c, 0x01000000);
 			cx25840_write4(client, 0x8e0, 0x03063870);
@@ -1400,8 +1390,14 @@ static int cx25840_set_fmt(struct v4l2_subdev *sd,
 
 	Vlines = fmt->height + (is_50Hz ? 4 : 7);
 
+	/*
+	 * We keep 1 margin for the Vsrc < Vlines check since the
+	 * cx23888 reports a Vsrc of 486 instead of 487 for the NTSC
+	 * height. Without that margin the cx23885 fails in this
+	 * check.
+	 */
 	if ((fmt->width * 16 < Hsrc) || (Hsrc < fmt->width) ||
-			(Vlines * 8 < Vsrc) || (Vsrc < Vlines)) {
+			(Vlines * 8 < Vsrc) || (Vsrc + 1 < Vlines)) {
 		v4l_err(client, "%dx%d is not a valid size!\n",
 				fmt->width, fmt->height);
 		return -ERANGE;
@@ -1426,14 +1422,20 @@ static int cx25840_set_fmt(struct v4l2_subdev *sd,
 			fmt->width, fmt->height, HSC, VSC);
 
 	/* HSCALE=HSC */
-	cx25840_write(client, 0x418, HSC & 0xff);
-	cx25840_write(client, 0x419, (HSC >> 8) & 0xff);
-	cx25840_write(client, 0x41a, HSC >> 16);
-	/* VSCALE=VSC */
-	cx25840_write(client, 0x41c, VSC & 0xff);
-	cx25840_write(client, 0x41d, VSC >> 8);
-	/* VS_INTRLACE=1 VFILT=filter */
-	cx25840_write(client, 0x41e, 0x8 | filter);
+	if (is_cx23888(state)) {
+		cx25840_write4(client, 0x434, HSC | (1 << 24));
+		/* VSCALE=VSC VS_INTRLACE=1 VFILT=filter */
+		cx25840_write4(client, 0x438, VSC | (1 << 19) | (filter << 16));
+	} else {
+		cx25840_write(client, 0x418, HSC & 0xff);
+		cx25840_write(client, 0x419, (HSC >> 8) & 0xff);
+		cx25840_write(client, 0x41a, HSC >> 16);
+		/* VSCALE=VSC */
+		cx25840_write(client, 0x41c, VSC & 0xff);
+		cx25840_write(client, 0x41d, VSC >> 8);
+		/* VS_INTRLACE=1 VFILT=filter */
+		cx25840_write(client, 0x41e, 0x8 | filter);
+	}
 	return 0;
 }
 
@@ -1714,26 +1716,27 @@ static int cx25840_s_stream(struct v4l2_subdev *sd, int enable)
 
 	v4l_dbg(1, cx25840_debug, client, "%s video output\n",
 			enable ? "enable" : "disable");
+
+	/*
+	 * It's not clear what should be done for these devices.
+	 * The original code used the same addresses as for the cx25840, but
+	 * those addresses do something else entirely on the cx2388x and
+	 * cx231xx. Since it never did anything in the first place, just do
+	 * nothing.
+	 */
+	if (is_cx2388x(state) || is_cx231xx(state))
+		return 0;
+
 	if (enable) {
-		if (is_cx2388x(state) || is_cx231xx(state)) {
-			v = cx25840_read(client, 0x421) | 0x0b;
-			cx25840_write(client, 0x421, v);
-		} else {
-			v = cx25840_read(client, 0x115) | 0x0c;
-			cx25840_write(client, 0x115, v);
-			v = cx25840_read(client, 0x116) | 0x04;
-			cx25840_write(client, 0x116, v);
-		}
+		v = cx25840_read(client, 0x115) | 0x0c;
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) | 0x04;
+		cx25840_write(client, 0x116, v);
 	} else {
-		if (is_cx2388x(state) || is_cx231xx(state)) {
-			v = cx25840_read(client, 0x421) & ~(0x0b);
-			cx25840_write(client, 0x421, v);
-		} else {
-			v = cx25840_read(client, 0x115) & ~(0x0c);
-			cx25840_write(client, 0x115, v);
-			v = cx25840_read(client, 0x116) & ~(0x04);
-			cx25840_write(client, 0x116, v);
-		}
+		v = cx25840_read(client, 0x115) & ~(0x0c);
+		cx25840_write(client, 0x115, v);
+		v = cx25840_read(client, 0x116) & ~(0x04);
+		cx25840_write(client, 0x116, v);
 	}
 	return 0;
 }
@@ -4974,7 +4977,7 @@ static void cx23888_std_setup(struct i2c_client *client)
 	cx25840_write4(client, 0x4b4, 0x20524030);
 	cx25840_write4(client, 0x47c, 0x010a8263);
 
-	if (std & V4L2_STD_NTSC) {
+	if (std & V4L2_STD_525_60) {
 		v4l_dbg(1, cx25840_debug, client, "%s() Selecting NTSC",
 			__func__);
 
@@ -5208,10 +5211,10 @@ static int cx25840_probe(struct i2c_client *client,
 	state->pads[CX25840_PAD_INPUT].flags = MEDIA_PAD_FL_SINK;
 	state->pads[CX25840_PAD_VID_OUT].flags = MEDIA_PAD_FL_SOURCE;
 	state->pads[CX25840_PAD_VBI_OUT].flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
+	sd->entity.function = MEDIA_ENT_F_ATV_DECODER;
 
-	ret = media_entity_init(&sd->entity, ARRAY_SIZE(state->pads),
-				state->pads, 0);
+	ret = media_entity_pads_init(&sd->entity, ARRAY_SIZE(state->pads),
+				state->pads);
 	if (ret < 0) {
 		v4l_info(client, "failed to initialize media entity!\n");
 		return ret;
@@ -5264,6 +5267,8 @@ static int cx25840_probe(struct i2c_client *client,
 	state->vbi_line_offset = 8;
 	state->id = id;
 	state->rev = device_id;
+	state->vbi_regs_offset = id == CX23888_AV ? 0x500 - 0x424 : 0;
+	state->std = V4L2_STD_NTSC_M;
 	v4l2_ctrl_handler_init(&state->hdl, 9);
 	v4l2_ctrl_new_std(&state->hdl, &cx25840_ctrl_ops,
 			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
diff --git a/drivers/media/i2c/cx25840/cx25840-core.h b/drivers/media/i2c/cx25840/cx25840-core.h
index fdea48ce0c03..254ef45ce41a 100644
--- a/drivers/media/i2c/cx25840/cx25840-core.h
+++ b/drivers/media/i2c/cx25840/cx25840-core.h
@@ -69,6 +69,7 @@ struct cx25840_state {
 	enum cx25840_model id;
 	u32 rev;
 	int is_initialized;
+	unsigned vbi_regs_offset;
 	wait_queue_head_t fw_wait;    /* wake up when the fw load is finished */
 	struct work_struct fw_work;   /* work entry for fw load */
 	struct cx25840_ir_state *ir_state;
diff --git a/drivers/media/i2c/cx25840/cx25840-firmware.c b/drivers/media/i2c/cx25840/cx25840-firmware.c
index 9bbb31adc29d..37e052923a87 100644
--- a/drivers/media/i2c/cx25840/cx25840-firmware.c
+++ b/drivers/media/i2c/cx25840/cx25840-firmware.c
@@ -19,7 +19,7 @@
 #include <linux/i2c.h>
 #include <linux/firmware.h>
 #include <media/v4l2-common.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 #include "cx25840-core.h"
 
diff --git a/drivers/media/i2c/cx25840/cx25840-ir.c b/drivers/media/i2c/cx25840/cx25840-ir.c
index 4cf8f18bf097..4b782012cadc 100644
--- a/drivers/media/i2c/cx25840/cx25840-ir.c
+++ b/drivers/media/i2c/cx25840/cx25840-ir.c
@@ -24,7 +24,7 @@
 #include <linux/slab.h>
 #include <linux/kfifo.h>
 #include <linux/module.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 #include <media/rc-core.h>
 
 #include "cx25840-core.h"
diff --git a/drivers/media/i2c/cx25840/cx25840-vbi.c b/drivers/media/i2c/cx25840/cx25840-vbi.c
index c39e91dc1137..0470bb6128e1 100644
--- a/drivers/media/i2c/cx25840/cx25840-vbi.c
+++ b/drivers/media/i2c/cx25840/cx25840-vbi.c
@@ -19,7 +19,7 @@
 #include <linux/videodev2.h>
 #include <linux/i2c.h>
 #include <media/v4l2-common.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 #include "cx25840-core.h"
 
@@ -104,7 +104,8 @@ int cx25840_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *
 
 	if (is_pal) {
 		for (i = 7; i <= 23; i++) {
-			u8 v = cx25840_read(client, 0x424 + i - 7);
+			u8 v = cx25840_read(client,
+				 state->vbi_regs_offset + 0x424 + i - 7);
 
 			svbi->service_lines[0][i] = lcr2vbi[v >> 4];
 			svbi->service_lines[1][i] = lcr2vbi[v & 0xf];
@@ -113,7 +114,8 @@ int cx25840_g_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *
 		}
 	} else {
 		for (i = 10; i <= 21; i++) {
-			u8 v = cx25840_read(client, 0x424 + i - 10);
+			u8 v = cx25840_read(client,
+				state->vbi_regs_offset + 0x424 + i - 10);
 
 			svbi->service_lines[0][i] = lcr2vbi[v >> 4];
 			svbi->service_lines[1][i] = lcr2vbi[v & 0xf];
@@ -135,7 +137,10 @@ int cx25840_s_raw_fmt(struct v4l2_subdev *sd, struct v4l2_vbi_format *fmt)
 	cx25840_std_setup(client);
 
 	/* VBI Offset */
-	cx25840_write(client, 0x47f, vbi_offset);
+	if (is_cx23888(state))
+		cx25840_write(client, 0x54f, vbi_offset);
+	else
+		cx25840_write(client, 0x47f, vbi_offset);
 	cx25840_write(client, 0x404, 0x2e);
 	return 0;
 }
@@ -158,7 +163,10 @@ int cx25840_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *
 	/* Sliced VBI */
 	cx25840_write(client, 0x404, 0x32);	/* Ancillary data */
 	cx25840_write(client, 0x406, 0x13);
-	cx25840_write(client, 0x47f, vbi_offset);
+	if (is_cx23888(state))
+		cx25840_write(client, 0x54f, vbi_offset);
+	else
+		cx25840_write(client, 0x47f, vbi_offset);
 
 	if (is_pal) {
 		for (i = 0; i <= 6; i++)
@@ -194,17 +202,23 @@ int cx25840_s_sliced_fmt(struct v4l2_subdev *sd, struct v4l2_sliced_vbi_format *
 	}
 
 	if (is_pal) {
-		for (x = 1, i = 0x424; i <= 0x434; i++, x++)
+		for (x = 1, i = state->vbi_regs_offset + 0x424;
+		     i <= state->vbi_regs_offset + 0x434; i++, x++)
 			cx25840_write(client, i, lcr[6 + x]);
 	} else {
-		for (x = 1, i = 0x424; i <= 0x430; i++, x++)
+		for (x = 1, i = state->vbi_regs_offset + 0x424;
+		     i <= state->vbi_regs_offset + 0x430; i++, x++)
 			cx25840_write(client, i, lcr[9 + x]);
-		for (i = 0x431; i <= 0x434; i++)
+		for (i = state->vbi_regs_offset + 0x431;
+		     i <= state->vbi_regs_offset + 0x434; i++)
 			cx25840_write(client, i, 0);
 	}
 
-	cx25840_write(client, 0x43c, 0x16);
-	cx25840_write(client, 0x474, is_pal ? 0x2a : 0x22);
+	cx25840_write(client, state->vbi_regs_offset + 0x43c, 0x16);
+	if (is_cx23888(state))
+		cx25840_write(client, 0x428, is_pal ? 0x2a : 0x22);
+	else
+		cx25840_write(client, 0x474, is_pal ? 0x2a : 0x22);
 	return 0;
 }
 
diff --git a/drivers/media/i2c/ir-kbd-i2c.c b/drivers/media/i2c/ir-kbd-i2c.c
index 728d2cc8a3e7..bf82726fd3f4 100644
--- a/drivers/media/i2c/ir-kbd-i2c.c
+++ b/drivers/media/i2c/ir-kbd-i2c.c
@@ -47,7 +47,7 @@
 #include <linux/workqueue.h>
 
 #include <media/rc-core.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 
 /* ----------------------------------------------------------------------- */
 /* insmod parameters                                                       */
@@ -478,7 +478,6 @@ static const struct i2c_device_id ir_kbd_id[] = {
 	{ "ir_rx_z8f0811_hdpvr", 0 },
 	{ }
 };
-MODULE_DEVICE_TABLE(i2c, ir_kbd_id);
 
 static struct i2c_driver ir_kbd_driver = {
 	.driver = {
diff --git a/drivers/media/i2c/lm3560.c b/drivers/media/i2c/lm3560.c
index d9ece4b2d047..251a2aaf98c3 100644
--- a/drivers/media/i2c/lm3560.c
+++ b/drivers/media/i2c/lm3560.c
@@ -24,7 +24,7 @@
 #include <linux/mutex.h>
 #include <linux/regmap.h>
 #include <linux/videodev2.h>
-#include <media/lm3560.h>
+#include <media/i2c/lm3560.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 
@@ -365,10 +365,10 @@ static int lm3560_subdev_init(struct lm3560_flash *flash,
 	rval = lm3560_init_controls(flash, led_no);
 	if (rval)
 		goto err_out;
-	rval = media_entity_init(&flash->subdev_led[led_no].entity, 0, NULL, 0);
+	rval = media_entity_pads_init(&flash->subdev_led[led_no].entity, 0, NULL);
 	if (rval < 0)
 		goto err_out;
-	flash->subdev_led[led_no].entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+	flash->subdev_led[led_no].entity.function = MEDIA_ENT_F_FLASH;
 
 	return rval;
 
diff --git a/drivers/media/i2c/lm3646.c b/drivers/media/i2c/lm3646.c
index 626fb4679c02..7e9967af36ec 100644
--- a/drivers/media/i2c/lm3646.c
+++ b/drivers/media/i2c/lm3646.c
@@ -18,7 +18,7 @@
 #include <linux/slab.h>
 #include <linux/regmap.h>
 #include <linux/videodev2.h>
-#include <media/lm3646.h>
+#include <media/i2c/lm3646.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 
@@ -282,10 +282,10 @@ static int lm3646_subdev_init(struct lm3646_flash *flash)
 	rval = lm3646_init_controls(flash);
 	if (rval)
 		goto err_out;
-	rval = media_entity_init(&flash->subdev_led.entity, 0, NULL, 0);
+	rval = media_entity_pads_init(&flash->subdev_led.entity, 0, NULL);
 	if (rval < 0)
 		goto err_out;
-	flash->subdev_led.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+	flash->subdev_led.entity.function = MEDIA_ENT_F_FLASH;
 	return rval;
 
 err_out:
diff --git a/drivers/media/i2c/m52790.c b/drivers/media/i2c/m52790.c
index 77eb07eb667e..81171d8e1c2c 100644
--- a/drivers/media/i2c/m52790.c
+++ b/drivers/media/i2c/m52790.c
@@ -27,7 +27,7 @@
 #include <asm/uaccess.h>
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
-#include <media/m52790.h>
+#include <media/i2c/m52790.h>
 #include <media/v4l2-device.h>
 
 MODULE_DESCRIPTION("i2c device driver for m52790 A/V switch");
diff --git a/drivers/media/i2c/m5mols/m5mols_capture.c b/drivers/media/i2c/m5mols/m5mols_capture.c
index 1a03d02bd4d1..a0cd6dc32eb0 100644
--- a/drivers/media/i2c/m5mols/m5mols_capture.c
+++ b/drivers/media/i2c/m5mols/m5mols_capture.c
@@ -25,8 +25,8 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
-#include <media/m5mols.h>
-#include <media/exynos-fimc.h>
+#include <media/i2c/m5mols.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "m5mols.h"
 #include "m5mols_reg.h"
diff --git a/drivers/media/i2c/m5mols/m5mols_core.c b/drivers/media/i2c/m5mols/m5mols_core.c
index 6404c0d93e7a..acb804bceccb 100644
--- a/drivers/media/i2c/m5mols/m5mols_core.c
+++ b/drivers/media/i2c/m5mols/m5mols_core.c
@@ -25,7 +25,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
-#include <media/m5mols.h>
+#include <media/i2c/m5mols.h>
 
 #include "m5mols.h"
 #include "m5mols_reg.h"
@@ -975,10 +975,10 @@ static int m5mols_probe(struct i2c_client *client,
 
 	sd->internal_ops = &m5mols_subdev_internal_ops;
 	info->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, 1, &info->pad, 0);
+	ret = media_entity_pads_init(&sd->entity, 1, &info->pad);
 	if (ret < 0)
 		return ret;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	init_waitqueue_head(&info->irq_waitq);
 	mutex_init(&info->lock);
diff --git a/drivers/media/i2c/msp3400-driver.c b/drivers/media/i2c/msp3400-driver.c
index bdb94000ba5a..a84561d0d4a8 100644
--- a/drivers/media/i2c/msp3400-driver.c
+++ b/drivers/media/i2c/msp3400-driver.c
@@ -56,8 +56,8 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
-#include <media/msp3400.h>
-#include <media/tvaudio.h>
+#include <media/drv-intf/msp3400.h>
+#include <media/i2c/tvaudio.h>
 #include "msp3400-driver.h"
 
 /* ---------------------------------------------------------------------- */
diff --git a/drivers/media/i2c/msp3400-driver.h b/drivers/media/i2c/msp3400-driver.h
index fbe5e0715f93..6cae21366ed5 100644
--- a/drivers/media/i2c/msp3400-driver.h
+++ b/drivers/media/i2c/msp3400-driver.h
@@ -4,7 +4,7 @@
 #ifndef MSP3400_DRIVER_H
 #define MSP3400_DRIVER_H
 
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 
diff --git a/drivers/media/i2c/msp3400-kthreads.c b/drivers/media/i2c/msp3400-kthreads.c
index f8b51714f2f9..17120804fab7 100644
--- a/drivers/media/i2c/msp3400-kthreads.c
+++ b/drivers/media/i2c/msp3400-kthreads.c
@@ -26,7 +26,7 @@
 #include <linux/freezer.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <linux/kthread.h>
 #include <linux/suspend.h>
 #include "msp3400-driver.h"
diff --git a/drivers/media/i2c/mt9m032.c b/drivers/media/i2c/mt9m032.c
index c7747bd0cabb..da076796999e 100644
--- a/drivers/media/i2c/mt9m032.c
+++ b/drivers/media/i2c/mt9m032.c
@@ -31,7 +31,7 @@
 #include <linux/v4l2-mediabus.h>
 
 #include <media/media-entity.h>
-#include <media/mt9m032.h>
+#include <media/i2c/mt9m032.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
@@ -671,7 +671,7 @@ static int mt9m032_set_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops mt9m032_ctrl_ops = {
+static const struct v4l2_ctrl_ops mt9m032_ctrl_ops = {
 	.s_ctrl = mt9m032_set_ctrl,
 	.try_ctrl = mt9m032_try_ctrl,
 };
@@ -799,7 +799,7 @@ static int mt9m032_probe(struct i2c_client *client,
 
 	sensor->subdev.ctrl_handler = &sensor->ctrls;
 	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sensor->subdev.entity, 1, &sensor->pad, 0);
+	ret = media_entity_pads_init(&sensor->subdev.entity, 1, &sensor->pad);
 	if (ret < 0)
 		goto error_ctrl;
 
diff --git a/drivers/media/i2c/mt9p031.c b/drivers/media/i2c/mt9p031.c
index 0db15f528ac1..237737fec09c 100644
--- a/drivers/media/i2c/mt9p031.c
+++ b/drivers/media/i2c/mt9p031.c
@@ -26,7 +26,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 
-#include <media/mt9p031.h>
+#include <media/i2c/mt9p031.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -817,7 +817,7 @@ static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
+static const struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
 	.s_ctrl = mt9p031_s_ctrl,
 };
 
@@ -1112,7 +1112,7 @@ static int mt9p031_probe(struct i2c_client *client,
 	mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
 
 	mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
+	ret = media_entity_pads_init(&mt9p031->subdev.entity, 1, &mt9p031->pad);
 	if (ret < 0)
 		goto done;
 
diff --git a/drivers/media/i2c/mt9t001.c b/drivers/media/i2c/mt9t001.c
index 8ae99f7f254c..702d562f8e39 100644
--- a/drivers/media/i2c/mt9t001.c
+++ b/drivers/media/i2c/mt9t001.c
@@ -21,7 +21,7 @@
 #include <linux/videodev2.h>
 #include <linux/v4l2-mediabus.h>
 
-#include <media/mt9t001.h>
+#include <media/i2c/mt9t001.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
@@ -626,7 +626,7 @@ static int mt9t001_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
+static const struct v4l2_ctrl_ops mt9t001_ctrl_ops = {
 	.s_ctrl = mt9t001_s_ctrl,
 };
 
@@ -933,7 +933,7 @@ static int mt9t001_probe(struct i2c_client *client,
 	mt9t001->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	mt9t001->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&mt9t001->subdev.entity, 1, &mt9t001->pad, 0);
+	ret = media_entity_pads_init(&mt9t001->subdev.entity, 1, &mt9t001->pad);
 
 done:
 	if (ret < 0) {
diff --git a/drivers/media/i2c/mt9v011.c b/drivers/media/i2c/mt9v011.c
index a4a5c39b599b..b9fea11d6b0b 100644
--- a/drivers/media/i2c/mt9v011.c
+++ b/drivers/media/i2c/mt9v011.c
@@ -13,7 +13,7 @@
 #include <asm/div64.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/mt9v011.h>
+#include <media/i2c/mt9v011.h>
 
 MODULE_DESCRIPTION("Micron mt9v011 sensor driver");
 MODULE_AUTHOR("Mauro Carvalho Chehab");
@@ -454,7 +454,7 @@ static int mt9v011_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops mt9v011_ctrl_ops = {
+static const struct v4l2_ctrl_ops mt9v011_ctrl_ops = {
 	.s_ctrl = mt9v011_s_ctrl,
 };
 
diff --git a/drivers/media/i2c/mt9v032.c b/drivers/media/i2c/mt9v032.c
index a68ce94ee097..2e1d116a64e7 100644
--- a/drivers/media/i2c/mt9v032.c
+++ b/drivers/media/i2c/mt9v032.c
@@ -25,7 +25,7 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/module.h>
 
-#include <media/mt9v032.h>
+#include <media/i2c/mt9v032.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-of.h>
@@ -703,7 +703,7 @@ static int mt9v032_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
+static const struct v4l2_ctrl_ops mt9v032_ctrl_ops = {
 	.s_ctrl = mt9v032_s_ctrl,
 };
 
@@ -1046,7 +1046,7 @@ static int mt9v032_probe(struct i2c_client *client,
 	mt9v032->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	mt9v032->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&mt9v032->subdev.entity, 1, &mt9v032->pad, 0);
+	ret = media_entity_pads_init(&mt9v032->subdev.entity, 1, &mt9v032->pad);
 	if (ret < 0)
 		goto err;
 
diff --git a/drivers/media/i2c/noon010pc30.c b/drivers/media/i2c/noon010pc30.c
index f197b6cbd407..30cb90b88d75 100644
--- a/drivers/media/i2c/noon010pc30.c
+++ b/drivers/media/i2c/noon010pc30.c
@@ -18,7 +18,7 @@
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/regulator/consumer.h>
-#include <media/noon010pc30.h>
+#include <media/i2c/noon010pc30.h>
 #include <linux/videodev2.h>
 #include <linux/module.h>
 #include <media/v4l2-ctrls.h>
@@ -779,8 +779,8 @@ static int noon010_probe(struct i2c_client *client,
 		goto np_err;
 
 	info->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, 1, &info->pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &info->pad);
 	if (ret < 0)
 		goto np_err;
 
diff --git a/drivers/media/i2c/ov2659.c b/drivers/media/i2c/ov2659.c
index 49109f4f5bb4..02b9a3440557 100644
--- a/drivers/media/i2c/ov2659.c
+++ b/drivers/media/i2c/ov2659.c
@@ -37,7 +37,7 @@
 #include <linux/videodev2.h>
 
 #include <media/media-entity.h>
-#include <media/ov2659.h>
+#include <media/i2c/ov2659.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
@@ -1249,7 +1249,7 @@ static int ov2659_s_ctrl(struct v4l2_ctrl *ctrl)
 	return 0;
 }
 
-static struct v4l2_ctrl_ops ov2659_ctrl_ops = {
+static const struct v4l2_ctrl_ops ov2659_ctrl_ops = {
 	.s_ctrl = ov2659_s_ctrl,
 };
 
@@ -1445,8 +1445,8 @@ static int ov2659_probe(struct i2c_client *client,
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	ov2659->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, 1, &ov2659->pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov2659->pad);
 	if (ret < 0) {
 		v4l2_ctrl_handler_free(&ov2659->ctrls);
 		return ret;
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index e1b5dc84c14e..56cfb5ca9c95 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -20,7 +20,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-image-sizes.h>
-#include <media/ov7670.h>
+#include <media/i2c/ov7670.h>
 
 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
 MODULE_DESCRIPTION("A low-level driver for OmniVision ov7670 sensors");
diff --git a/drivers/media/i2c/ov9650.c b/drivers/media/i2c/ov9650.c
index 1ee6a5527c38..a0b3c9bde53d 100644
--- a/drivers/media/i2c/ov9650.c
+++ b/drivers/media/i2c/ov9650.c
@@ -29,7 +29,7 @@
 #include <media/v4l2-image-sizes.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
-#include <media/ov9650.h>
+#include <media/i2c/ov9650.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -1500,8 +1500,8 @@ static int ov965x_probe(struct i2c_client *client,
 		return ret;
 
 	ov965x->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, 1, &ov965x->pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &ov965x->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-core.c b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
index 51b26010403c..57b3d27993a4 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-core.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-core.c
@@ -34,7 +34,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5c73m3.h>
+#include <media/i2c/s5c73m3.h>
 #include <media/v4l2-of.h>
 
 #include "s5c73m3.h"
@@ -1482,11 +1482,11 @@ static int s5c73m3_oif_registered(struct v4l2_subdev *sd)
 		return ret;
 	}
 
-	ret = media_entity_create_link(&state->sensor_sd.entity,
+	ret = media_create_pad_link(&state->sensor_sd.entity,
 			S5C73M3_ISP_PAD, &state->oif_sd.entity, OIF_ISP_PAD,
 			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 
-	ret = media_entity_create_link(&state->sensor_sd.entity,
+	ret = media_create_pad_link(&state->sensor_sd.entity,
 			S5C73M3_JPEG_PAD, &state->oif_sd.entity, OIF_JPEG_PAD,
 			MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
 
@@ -1688,10 +1688,10 @@ static int s5c73m3_probe(struct i2c_client *client,
 
 	state->sensor_pads[S5C73M3_JPEG_PAD].flags = MEDIA_PAD_FL_SOURCE;
 	state->sensor_pads[S5C73M3_ISP_PAD].flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
-	ret = media_entity_init(&sd->entity, S5C73M3_NUM_PADS,
-							state->sensor_pads, 0);
+	ret = media_entity_pads_init(&sd->entity, S5C73M3_NUM_PADS,
+							state->sensor_pads);
 	if (ret < 0)
 		return ret;
 
@@ -1704,10 +1704,10 @@ 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.type = MEDIA_ENT_T_V4L2_SUBDEV;
+	oif_sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
 
-	ret = media_entity_init(&oif_sd->entity, OIF_NUM_PADS,
-							state->oif_pads, 0);
+	ret = media_entity_pads_init(&oif_sd->entity, OIF_NUM_PADS,
+							state->oif_pads);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c b/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c
index 8001cde1db1e..0a060339e516 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-ctrls.c
@@ -32,7 +32,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5c73m3.h>
+#include <media/i2c/s5c73m3.h>
 
 #include "s5c73m3.h"
 
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3-spi.c b/drivers/media/i2c/s5c73m3/s5c73m3-spi.c
index 72ef9f936e6c..7d65b36434b1 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3-spi.c
+++ b/drivers/media/i2c/s5c73m3/s5c73m3-spi.c
@@ -37,6 +37,7 @@ enum spi_direction {
 	SPI_DIR_RX,
 	SPI_DIR_TX
 };
+MODULE_DEVICE_TABLE(of, s5c73m3_spi_ids);
 
 static int spi_xmit(struct spi_device *spi_dev, void *addr, const int len,
 							enum spi_direction dir)
diff --git a/drivers/media/i2c/s5c73m3/s5c73m3.h b/drivers/media/i2c/s5c73m3/s5c73m3.h
index 13aed59f0f5d..653f68e7ea07 100644
--- a/drivers/media/i2c/s5c73m3/s5c73m3.h
+++ b/drivers/media/i2c/s5c73m3/s5c73m3.h
@@ -23,7 +23,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
-#include <media/s5c73m3.h>
+#include <media/i2c/s5c73m3.h>
 
 #define DRIVER_NAME			"S5C73M3"
 
diff --git a/drivers/media/i2c/s5k4ecgx.c b/drivers/media/i2c/s5k4ecgx.c
index 97084237275d..8a0f22da590f 100644
--- a/drivers/media/i2c/s5k4ecgx.c
+++ b/drivers/media/i2c/s5k4ecgx.c
@@ -27,7 +27,7 @@
 #include <asm/unaligned.h>
 
 #include <media/media-entity.h>
-#include <media/s5k4ecgx.h>
+#include <media/i2c/s5k4ecgx.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
@@ -961,8 +961,8 @@ static int s5k4ecgx_probe(struct i2c_client *client,
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, 1, &priv->pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &priv->pad);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
index 774e0d0c94cb..fc3a5a8e6c9c 100644
--- a/drivers/media/i2c/s5k5baf.c
+++ b/drivers/media/i2c/s5k5baf.c
@@ -408,7 +408,7 @@ static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 
 static inline bool s5k5baf_is_cis_subdev(struct v4l2_subdev *sd)
 {
-	return sd->entity.type == MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	return sd->entity.function == MEDIA_ENT_F_CAM_SENSOR;
 }
 
 static inline struct s5k5baf *to_s5k5baf(struct v4l2_subdev *sd)
@@ -1756,7 +1756,7 @@ static int s5k5baf_registered(struct v4l2_subdev *sd)
 		v4l2_err(sd, "failed to register subdev %s\n",
 			 state->cis_sd.name);
 	else
-		ret = media_entity_create_link(&state->cis_sd.entity, PAD_CIS,
+		ret = media_create_pad_link(&state->cis_sd.entity, PAD_CIS,
 					       &state->sd.entity, PAD_CIS,
 					       MEDIA_LNK_FL_IMMUTABLE |
 					       MEDIA_LNK_FL_ENABLED);
@@ -1904,8 +1904,8 @@ static int s5k5baf_configure_subdevs(struct s5k5baf *state,
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	state->cis_pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, NUM_CIS_PADS, &state->cis_pad);
 	if (ret < 0)
 		goto err;
 
@@ -1919,8 +1919,8 @@ static int s5k5baf_configure_subdevs(struct s5k5baf *state,
 
 	state->pads[PAD_CIS].flags = MEDIA_PAD_FL_SINK;
 	state->pads[PAD_OUT].flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
-	ret = media_entity_init(&sd->entity, NUM_ISP_PADS, state->pads, 0);
+	sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
+	ret = media_entity_pads_init(&sd->entity, NUM_ISP_PADS, state->pads);
 
 	if (!ret)
 		return 0;
diff --git a/drivers/media/i2c/s5k6a3.c b/drivers/media/i2c/s5k6a3.c
index b1b1574dfb95..cbe4711e9b31 100644
--- a/drivers/media/i2c/s5k6a3.c
+++ b/drivers/media/i2c/s5k6a3.c
@@ -144,8 +144,7 @@ static int s5k6a3_set_fmt(struct v4l2_subdev *sd,
 	mf = __s5k6a3_get_format(sensor, cfg, fmt->pad, fmt->which);
 	if (mf) {
 		mutex_lock(&sensor->lock);
-		if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
-			*mf = fmt->format;
+		*mf = fmt->format;
 		mutex_unlock(&sensor->lock);
 	}
 	return 0;
@@ -333,7 +332,7 @@ static int s5k6a3_probe(struct i2c_client *client,
 	sensor->format.height = S5K6A3_DEFAULT_HEIGHT;
 
 	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, 1, &sensor->pad, 0);
+	ret = media_entity_pads_init(&sd->entity, 1, &sensor->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/i2c/s5k6aa.c b/drivers/media/i2c/s5k6aa.c
index d0ad6a25bdab..faee11383cb7 100644
--- a/drivers/media/i2c/s5k6aa.c
+++ b/drivers/media/i2c/s5k6aa.c
@@ -28,7 +28,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
-#include <media/s5k6aa.h>
+#include <media/i2c/s5k6aa.h>
 
 static int debug;
 module_param(debug, int, 0644);
@@ -1577,8 +1577,8 @@ static int s5k6aa_probe(struct i2c_client *client,
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 
 	s5k6aa->pad.flags = MEDIA_PAD_FL_SOURCE;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
-	ret = media_entity_init(&sd->entity, 1, &s5k6aa->pad, 0);
+	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
+	ret = media_entity_pads_init(&sd->entity, 1, &s5k6aa->pad);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/i2c/saa6588.c b/drivers/media/i2c/saa6588.c
index 37e65f661d7a..89e458c23983 100644
--- a/drivers/media/i2c/saa6588.c
+++ b/drivers/media/i2c/saa6588.c
@@ -31,7 +31,7 @@
 #include <linux/wait.h>
 #include <asm/uaccess.h>
 
-#include <media/saa6588.h>
+#include <media/i2c/saa6588.h>
 #include <media/v4l2-device.h>
 
 
diff --git a/drivers/media/i2c/saa7115.c b/drivers/media/i2c/saa7115.c
index 91e75222c537..24d2b76dbe97 100644
--- a/drivers/media/i2c/saa7115.c
+++ b/drivers/media/i2c/saa7115.c
@@ -46,7 +46,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <asm/div64.h>
 
 #define VRES_60HZ	(480+16)
diff --git a/drivers/media/i2c/saa7127.c b/drivers/media/i2c/saa7127.c
index a43d96da1017..8d94dcbf4366 100644
--- a/drivers/media/i2c/saa7127.c
+++ b/drivers/media/i2c/saa7127.c
@@ -54,7 +54,7 @@
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
-#include <media/saa7127.h>
+#include <media/i2c/saa7127.h>
 
 static int debug;
 static int test_image;
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index fb39dfd55e75..a215efe7a8ba 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -2487,31 +2487,31 @@ static int smiapp_register_subdevs(struct smiapp_sensor *sensor)
 		if (!last)
 			continue;
 
-		rval = media_entity_init(&this->sd.entity,
-					 this->npads, this->pads, 0);
+		rval = media_entity_pads_init(&this->sd.entity,
+					 this->npads, this->pads);
 		if (rval) {
 			dev_err(&client->dev,
-				"media_entity_init failed\n");
+				"media_entity_pads_init failed\n");
 			return rval;
 		}
 
-		rval = media_entity_create_link(&this->sd.entity,
-						this->source_pad,
-						&last->sd.entity,
-						last->sink_pad,
-						MEDIA_LNK_FL_ENABLED |
-						MEDIA_LNK_FL_IMMUTABLE);
+		rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
+						   &this->sd);
 		if (rval) {
 			dev_err(&client->dev,
-				"media_entity_create_link failed\n");
+				"v4l2_device_register_subdev failed\n");
 			return rval;
 		}
 
-		rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev,
-						   &this->sd);
+		rval = media_create_pad_link(&this->sd.entity,
+					     this->source_pad,
+					     &last->sd.entity,
+					     last->sink_pad,
+					     MEDIA_LNK_FL_ENABLED |
+					     MEDIA_LNK_FL_IMMUTABLE);
 		if (rval) {
 			dev_err(&client->dev,
-				"v4l2_device_register_subdev failed\n");
+				"media_create_pad_link failed\n");
 			return rval;
 		}
 	}
@@ -2763,7 +2763,7 @@ static int smiapp_init(struct smiapp_sensor *sensor)
 
 	dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile);
 
-	sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
+	sensor->pixel_array->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
 
 	/* final steps */
 	smiapp_read_frame_fmt(sensor);
@@ -3077,8 +3077,8 @@ static int smiapp_probe(struct i2c_client *client,
 	sensor->src->sensor = sensor;
 
 	sensor->src->pads[0].flags = MEDIA_PAD_FL_SOURCE;
-	rval = media_entity_init(&sensor->src->sd.entity, 2,
-				 sensor->src->pads, 0);
+	rval = media_entity_pads_init(&sensor->src->sd.entity, 2,
+				 sensor->src->pads);
 	if (rval < 0)
 		return rval;
 
diff --git a/drivers/media/i2c/smiapp/smiapp.h b/drivers/media/i2c/smiapp/smiapp.h
index ed010a8a49d7..f6af0cc4a256 100644
--- a/drivers/media/i2c/smiapp/smiapp.h
+++ b/drivers/media/i2c/smiapp/smiapp.h
@@ -22,7 +22,7 @@
 #include <linux/mutex.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
-#include <media/smiapp.h>
+#include <media/i2c/smiapp.h>
 
 #include "smiapp-pll.h"
 #include "smiapp-reg.h"
diff --git a/drivers/media/i2c/soc_camera/mt9m001.c b/drivers/media/i2c/soc_camera/mt9m001.c
index 4fbdd1e9f7ee..2e14e52ba2e0 100644
--- a/drivers/media/i2c/soc_camera/mt9m001.c
+++ b/drivers/media/i2c/soc_camera/mt9m001.c
@@ -15,7 +15,7 @@
 #include <linux/module.h>
 
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 2f35d31ca58e..6a1b2a9f9a09 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -25,7 +25,7 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
-#include <media/mt9t112.h>
+#include <media/i2c/mt9t112.h>
 #include <media/soc_camera.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
diff --git a/drivers/media/i2c/soc_camera/mt9v022.c b/drivers/media/i2c/soc_camera/mt9v022.c
index f31377408550..c2ba1fb3694d 100644
--- a/drivers/media/i2c/soc_camera/mt9v022.c
+++ b/drivers/media/i2c/soc_camera/mt9v022.c
@@ -15,9 +15,9 @@
 #include <linux/log2.h>
 #include <linux/module.h>
 
-#include <media/mt9v022.h>
+#include <media/i2c/mt9v022.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index f150a8bd94dc..a43410c1e254 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -24,7 +24,7 @@
 #include <linux/v4l2-mediabus.h>
 #include <linux/videodev2.h>
 
-#include <media/ov772x.h>
+#include <media/i2c/ov772x.h>
 #include <media/soc_camera.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-ctrls.h>
diff --git a/drivers/media/i2c/soc_camera/rj54n1cb0c.c b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
index c769cf663f84..aa7bfbb4ad71 100644
--- a/drivers/media/i2c/soc_camera/rj54n1cb0c.c
+++ b/drivers/media/i2c/soc_camera/rj54n1cb0c.c
@@ -15,7 +15,7 @@
 #include <linux/videodev2.h>
 #include <linux/module.h>
 
-#include <media/rj54n1cb0c.h>
+#include <media/i2c/rj54n1cb0c.h>
 #include <media/soc_camera.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-subdev.h>
diff --git a/drivers/media/i2c/soc_camera/tw9910.c b/drivers/media/i2c/soc_camera/tw9910.c
index e939c24bfd3c..06aff81787a7 100644
--- a/drivers/media/i2c/soc_camera/tw9910.c
+++ b/drivers/media/i2c/soc_camera/tw9910.c
@@ -26,7 +26,7 @@
 #include <linux/videodev2.h>
 
 #include <media/soc_camera.h>
-#include <media/tw9910.h>
+#include <media/i2c/tw9910.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-subdev.h>
 
diff --git a/drivers/media/i2c/sr030pc30.c b/drivers/media/i2c/sr030pc30.c
index b04c09dd4bfb..0bf031b7e4fa 100644
--- a/drivers/media/i2c/sr030pc30.c
+++ b/drivers/media/i2c/sr030pc30.c
@@ -24,7 +24,7 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-ctrls.h>
-#include <media/sr030pc30.h>
+#include <media/i2c/sr030pc30.h>
 
 static int debug;
 module_param(debug, int, 0644);
diff --git a/drivers/media/i2c/tc358743.c b/drivers/media/i2c/tc358743.c
index 9ef5baaf8646..3397eb99c67b 100644
--- a/drivers/media/i2c/tc358743.c
+++ b/drivers/media/i2c/tc358743.c
@@ -42,7 +42,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-of.h>
-#include <media/tc358743.h>
+#include <media/i2c/tc358743.h>
 
 #include "tc358743_regs.h"
 
@@ -862,7 +862,7 @@ static void tc358743_format_change(struct v4l2_subdev *sd)
 		v4l2_dbg(1, debug, sd, "%s: Format changed. No signal\n",
 				__func__);
 	} else {
-		if (!v4l2_match_dv_timings(&state->timings, &timings, 0))
+		if (!v4l2_match_dv_timings(&state->timings, &timings, 0, false))
 			enable_stream(sd, false);
 
 		v4l2_print_dv_timings(sd->name,
@@ -1366,7 +1366,7 @@ static int tc358743_s_dv_timings(struct v4l2_subdev *sd,
 		v4l2_print_dv_timings(sd->name, "tc358743_s_dv_timings: ",
 				timings, false);
 
-	if (v4l2_match_dv_timings(&state->timings, timings, 0)) {
+	if (v4l2_match_dv_timings(&state->timings, timings, 0, false)) {
 		v4l2_dbg(1, debug, sd, "%s: no change\n", __func__);
 		return 0;
 	}
@@ -1889,7 +1889,7 @@ static int tc358743_probe(struct i2c_client *client,
 	}
 
 	state->pad.flags = MEDIA_PAD_FL_SOURCE;
-	err = media_entity_init(&sd->entity, 1, &state->pad, 0);
+	err = media_entity_pads_init(&sd->entity, 1, &state->pad);
 	if (err < 0)
 		goto err_hdl;
 
diff --git a/drivers/media/i2c/ths7303.c b/drivers/media/i2c/ths7303.c
index bda3a6540a60..5bbfcab01c75 100644
--- a/drivers/media/i2c/ths7303.c
+++ b/drivers/media/i2c/ths7303.c
@@ -25,7 +25,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 
-#include <media/ths7303.h>
+#include <media/i2c/ths7303.h>
 #include <media/v4l2-device.h>
 
 #define THS7303_CHANNEL_1	1
diff --git a/drivers/media/i2c/tvaudio.c b/drivers/media/i2c/tvaudio.c
index 2a8114a676fd..fece2a4339a1 100644
--- a/drivers/media/i2c/tvaudio.c
+++ b/drivers/media/i2c/tvaudio.c
@@ -36,7 +36,7 @@
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 
-#include <media/tvaudio.h>
+#include <media/i2c/tvaudio.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 
diff --git a/drivers/media/i2c/tvp514x.c b/drivers/media/i2c/tvp514x.c
index a93985a9b070..7fa5f1e4fe37 100644
--- a/drivers/media/i2c/tvp514x.c
+++ b/drivers/media/i2c/tvp514x.c
@@ -44,7 +44,7 @@
 #include <media/v4l2-mediabus.h>
 #include <media/v4l2-of.h>
 #include <media/v4l2-ctrls.h>
-#include <media/tvp514x.h>
+#include <media/i2c/tvp514x.h>
 #include <media/media-entity.h>
 
 #include "tvp514x_regs.h"
@@ -1095,9 +1095,9 @@ tvp514x_probe(struct i2c_client *client, const struct i2c_device_id *id)
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	decoder->pad.flags = MEDIA_PAD_FL_SOURCE;
 	decoder->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	decoder->sd.entity.flags |= MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
+	decoder->sd.entity.flags |= MEDIA_ENT_F_ATV_DECODER;
 
-	ret = media_entity_init(&decoder->sd.entity, 1, &decoder->pad, 0);
+	ret = media_entity_pads_init(&decoder->sd.entity, 1, &decoder->pad);
 	if (ret < 0) {
 		v4l2_err(sd, "%s decoder driver failed to register !!\n",
 			 sd->name);
diff --git a/drivers/media/i2c/tvp5150.c b/drivers/media/i2c/tvp5150.c
index 3c5fb2509c47..6c3769d44b75 100644
--- a/drivers/media/i2c/tvp5150.c
+++ b/drivers/media/i2c/tvp5150.c
@@ -12,7 +12,7 @@
 #include <linux/module.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-device.h>
-#include <media/tvp5150.h>
+#include <media/i2c/tvp5150.h>
 #include <media/v4l2-ctrls.h>
 
 #include "tvp5150_reg.h"
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index f617d8b745ee..83c79fa5f61d 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -32,7 +32,7 @@
 #include <linux/of.h>
 #include <linux/of_graph.h>
 #include <linux/v4l2-dv-timings.h>
-#include <media/tvp7002.h>
+#include <media/i2c/tvp7002.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-common.h>
@@ -1012,9 +1012,9 @@ static int tvp7002_probe(struct i2c_client *c, const struct i2c_device_id *id)
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	device->pad.flags = MEDIA_PAD_FL_SOURCE;
 	device->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
-	device->sd.entity.flags |= MEDIA_ENT_T_V4L2_SUBDEV_DECODER;
+	device->sd.entity.flags |= MEDIA_ENT_F_ATV_DECODER;
 
-	error = media_entity_init(&device->sd.entity, 1, &device->pad, 0);
+	error = media_entity_pads_init(&device->sd.entity, 1, &device->pad);
 	if (error < 0)
 		return error;
 #endif
diff --git a/drivers/media/i2c/uda1342.c b/drivers/media/i2c/uda1342.c
index 081786d176d0..8e17a83920d4 100644
--- a/drivers/media/i2c/uda1342.c
+++ b/drivers/media/i2c/uda1342.c
@@ -20,7 +20,7 @@
 #include <linux/i2c.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
-#include <media/uda1342.h>
+#include <media/i2c/uda1342.h>
 #include <linux/slab.h>
 
 static int write_reg(struct i2c_client *client, int reg, int value)
diff --git a/drivers/media/i2c/upd64031a.c b/drivers/media/i2c/upd64031a.c
index 2c0f955abc72..c03567e993cd 100644
--- a/drivers/media/i2c/upd64031a.c
+++ b/drivers/media/i2c/upd64031a.c
@@ -27,7 +27,7 @@
 #include <linux/videodev2.h>
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
-#include <media/upd64031a.h>
+#include <media/i2c/upd64031a.h>
 
 /* --------------------- read registers functions define -------------------- */
 
diff --git a/drivers/media/i2c/upd64083.c b/drivers/media/i2c/upd64083.c
index f2057a434060..77f122f2e3c9 100644
--- a/drivers/media/i2c/upd64083.c
+++ b/drivers/media/i2c/upd64083.c
@@ -27,7 +27,7 @@
 #include <linux/videodev2.h>
 #include <linux/slab.h>
 #include <media/v4l2-device.h>
-#include <media/upd64083.h>
+#include <media/i2c/upd64083.h>
 
 MODULE_DESCRIPTION("uPD64083 driver");
 MODULE_AUTHOR("T. Adachi, Takeru KOMORIYA, Hans Verkuil");
diff --git a/drivers/media/i2c/wm8775.c b/drivers/media/i2c/wm8775.c
index d33d2cd6d034..6e00f145b948 100644
--- a/drivers/media/i2c/wm8775.c
+++ b/drivers/media/i2c/wm8775.c
@@ -34,7 +34,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/wm8775.h>
+#include <media/i2c/wm8775.h>
 
 MODULE_DESCRIPTION("wm8775 driver");
 MODULE_AUTHOR("Ulf Eklund, Hans Verkuil");
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index 7b39440192d6..e9219f528d7e 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -20,16 +20,23 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+/* We need to access legacy defines from linux/media.h */
+#define __NEED_MEDIA_LEGACY_API
+
 #include <linux/compat.h>
 #include <linux/export.h>
+#include <linux/idr.h>
 #include <linux/ioctl.h>
 #include <linux/media.h>
+#include <linux/slab.h>
 #include <linux/types.h>
 
 #include <media/media-device.h>
 #include <media/media-devnode.h>
 #include <media/media-entity.h>
 
+#ifdef CONFIG_MEDIA_CONTROLLER
+
 /* -----------------------------------------------------------------------------
  * Userspace API
  */
@@ -75,8 +82,8 @@ static struct media_entity *find_entity(struct media_device *mdev, u32 id)
 	spin_lock(&mdev->lock);
 
 	media_device_for_each_entity(entity, mdev) {
-		if ((entity->id == id && !next) ||
-		    (entity->id > id && next)) {
+		if (((media_entity_id(entity) == id) && !next) ||
+		    ((media_entity_id(entity) > id) && next)) {
 			spin_unlock(&mdev->lock);
 			return entity;
 		}
@@ -102,15 +109,35 @@ static long media_device_enum_entities(struct media_device *mdev,
 	if (ent == NULL)
 		return -EINVAL;
 
-	u_ent.id = ent->id;
+	u_ent.id = media_entity_id(ent);
 	if (ent->name)
 		strlcpy(u_ent.name, ent->name, sizeof(u_ent.name));
-	u_ent.type = ent->type;
-	u_ent.revision = ent->revision;
+	u_ent.type = ent->function;
+	u_ent.revision = 0;		/* Unused */
 	u_ent.flags = ent->flags;
-	u_ent.group_id = ent->group_id;
+	u_ent.group_id = 0;		/* Unused */
 	u_ent.pads = ent->num_pads;
 	u_ent.links = ent->num_links - ent->num_backlinks;
+
+	/*
+	 * Workaround for a bug at media-ctl <= v1.10 that makes it to
+	 * do the wrong thing if the entity function doesn't belong to
+	 * either MEDIA_ENT_F_OLD_BASE or MEDIA_ENT_F_OLD_SUBDEV_BASE
+	 * Ranges.
+	 *
+	 * Non-subdevices are expected to be at the MEDIA_ENT_F_OLD_BASE,
+	 * or, otherwise, will be silently ignored by media-ctl when
+	 * printing the graphviz diagram. So, map them into the devnode
+	 * old range.
+	 */
+	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;
+		else if (ent->function != MEDIA_ENT_F_IO_V4L)
+			u_ent.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;
@@ -120,7 +147,7 @@ static long media_device_enum_entities(struct media_device *mdev,
 static void media_device_kpad_to_upad(const struct media_pad *kpad,
 				      struct media_pad_desc *upad)
 {
-	upad->entity = kpad->entity->id;
+	upad->entity = media_entity_id(kpad->entity);
 	upad->index = kpad->index;
 	upad->flags = kpad->flags;
 }
@@ -148,25 +175,25 @@ static long __media_device_enum_links(struct media_device *mdev,
 	}
 
 	if (links->links) {
-		struct media_link_desc __user *ulink;
-		unsigned int l;
+		struct media_link *link;
+		struct media_link_desc __user *ulink_desc = links->links;
 
-		for (l = 0, ulink = links->links; l < entity->num_links; l++) {
-			struct media_link_desc link;
+		list_for_each_entry(link, &entity->links, list) {
+			struct media_link_desc klink_desc;
 
 			/* Ignore backlinks. */
-			if (entity->links[l].source->entity != entity)
+			if (link->source->entity != entity)
 				continue;
-
-			memset(&link, 0, sizeof(link));
-			media_device_kpad_to_upad(entity->links[l].source,
-						  &link.source);
-			media_device_kpad_to_upad(entity->links[l].sink,
-						  &link.sink);
-			link.flags = entity->links[l].flags;
-			if (copy_to_user(ulink, &link, sizeof(*ulink)))
+			memset(&klink_desc, 0, sizeof(klink_desc));
+			media_device_kpad_to_upad(link->source,
+						  &klink_desc.source);
+			media_device_kpad_to_upad(link->sink,
+						  &klink_desc.sink);
+			klink_desc.flags = link->flags;
+			if (copy_to_user(ulink_desc, &klink_desc,
+					 sizeof(*ulink_desc)))
 				return -EFAULT;
-			ulink++;
+			ulink_desc++;
 		}
 	}
 
@@ -230,6 +257,164 @@ static long media_device_setup_link(struct media_device *mdev,
 	return ret;
 }
 
+#if 0 /* Let's postpone it to Kernel 4.6 */
+static long __media_device_get_topology(struct media_device *mdev,
+				      struct media_v2_topology *topo)
+{
+	struct media_entity *entity;
+	struct media_interface *intf;
+	struct media_pad *pad;
+	struct media_link *link;
+	struct media_v2_entity kentity, *uentity;
+	struct media_v2_interface kintf, *uintf;
+	struct media_v2_pad kpad, *upad;
+	struct media_v2_link klink, *ulink;
+	unsigned int i;
+	int ret = 0;
+
+	topo->topology_version = mdev->topology_version;
+
+	/* Get entities and number of entities */
+	i = 0;
+	uentity = media_get_uptr(topo->ptr_entities);
+	media_device_for_each_entity(entity, mdev) {
+		i++;
+		if (ret || !uentity)
+			continue;
+
+		if (i > topo->num_entities) {
+			ret = -ENOSPC;
+			continue;
+		}
+
+		/* Copy fields to userspace struct if not error */
+		memset(&kentity, 0, sizeof(kentity));
+		kentity.id = entity->graph_obj.id;
+		kentity.function = entity->function;
+		strncpy(kentity.name, entity->name,
+			sizeof(kentity.name));
+
+		if (copy_to_user(uentity, &kentity, sizeof(kentity)))
+			ret = -EFAULT;
+		uentity++;
+	}
+	topo->num_entities = i;
+
+	/* Get interfaces and number of interfaces */
+	i = 0;
+	uintf = media_get_uptr(topo->ptr_interfaces);
+	media_device_for_each_intf(intf, mdev) {
+		i++;
+		if (ret || !uintf)
+			continue;
+
+		if (i > topo->num_interfaces) {
+			ret = -ENOSPC;
+			continue;
+		}
+
+		memset(&kintf, 0, sizeof(kintf));
+
+		/* Copy intf fields to userspace struct */
+		kintf.id = intf->graph_obj.id;
+		kintf.intf_type = intf->type;
+		kintf.flags = intf->flags;
+
+		if (media_type(&intf->graph_obj) == MEDIA_GRAPH_INTF_DEVNODE) {
+			struct media_intf_devnode *devnode;
+
+			devnode = intf_to_devnode(intf);
+
+			kintf.devnode.major = devnode->major;
+			kintf.devnode.minor = devnode->minor;
+		}
+
+		if (copy_to_user(uintf, &kintf, sizeof(kintf)))
+			ret = -EFAULT;
+		uintf++;
+	}
+	topo->num_interfaces = i;
+
+	/* Get pads and number of pads */
+	i = 0;
+	upad = media_get_uptr(topo->ptr_pads);
+	media_device_for_each_pad(pad, mdev) {
+		i++;
+		if (ret || !upad)
+			continue;
+
+		if (i > topo->num_pads) {
+			ret = -ENOSPC;
+			continue;
+		}
+
+		memset(&kpad, 0, sizeof(kpad));
+
+		/* Copy pad fields to userspace struct */
+		kpad.id = pad->graph_obj.id;
+		kpad.entity_id = pad->entity->graph_obj.id;
+		kpad.flags = pad->flags;
+
+		if (copy_to_user(upad, &kpad, sizeof(kpad)))
+			ret = -EFAULT;
+		upad++;
+	}
+	topo->num_pads = i;
+
+	/* Get links and number of links */
+	i = 0;
+	ulink = media_get_uptr(topo->ptr_links);
+	media_device_for_each_link(link, mdev) {
+		if (link->is_backlink)
+			continue;
+
+		i++;
+
+		if (ret || !ulink)
+			continue;
+
+		if (i > topo->num_links) {
+			ret = -ENOSPC;
+			continue;
+		}
+
+		memset(&klink, 0, sizeof(klink));
+
+		/* Copy link fields to userspace struct */
+		klink.id = link->graph_obj.id;
+		klink.source_id = link->gobj0->id;
+		klink.sink_id = link->gobj1->id;
+		klink.flags = link->flags;
+
+		if (copy_to_user(ulink, &klink, sizeof(klink)))
+			ret = -EFAULT;
+		ulink++;
+	}
+	topo->num_links = i;
+
+	return ret;
+}
+
+static long media_device_get_topology(struct media_device *mdev,
+				      struct media_v2_topology __user *utopo)
+{
+	struct media_v2_topology ktopo;
+	int ret;
+
+	if (copy_from_user(&ktopo, utopo, sizeof(ktopo)))
+		return -EFAULT;
+
+	ret = __media_device_get_topology(mdev, &ktopo);
+	if (ret < 0)
+		return ret;
+
+	if (copy_to_user(utopo, &ktopo, sizeof(*utopo)))
+		return -EFAULT;
+
+	return 0;
+}
+#endif
+
 static long media_device_ioctl(struct file *filp, unsigned int cmd,
 			       unsigned long arg)
 {
@@ -262,6 +447,14 @@ static long media_device_ioctl(struct file *filp, unsigned int cmd,
 		mutex_unlock(&dev->graph_mutex);
 		break;
 
+#if 0 /* Let's postpone it to Kernel 4.6 */
+	case MEDIA_IOC_G_TOPOLOGY:
+		mutex_lock(&dev->graph_mutex);
+		ret = media_device_get_topology(dev,
+				(struct media_v2_topology __user *)arg);
+		mutex_unlock(&dev->graph_mutex);
+		break;
+#endif
 	default:
 		ret = -ENOIOCTLCMD;
 	}
@@ -310,6 +503,9 @@ static long media_device_compat_ioctl(struct file *filp, unsigned int cmd,
 	case MEDIA_IOC_DEVICE_INFO:
 	case MEDIA_IOC_ENUM_ENTITIES:
 	case MEDIA_IOC_SETUP_LINK:
+#if 0 /* Let's postpone it to Kernel 4.6 */
+	case MEDIA_IOC_G_TOPOLOGY:
+#endif
 		return media_device_ioctl(filp, cmd, arg);
 
 	case MEDIA_IOC_ENUM_LINKS32:
@@ -357,10 +553,107 @@ static DEVICE_ATTR(model, S_IRUGO, show_model, NULL);
 
 static void media_device_release(struct media_devnode *mdev)
 {
+	dev_dbg(mdev->parent, "Media device released\n");
 }
 
 /**
- * media_device_register - register a media device
+ * media_device_register_entity - Register an entity with a media device
+ * @mdev:	The media device
+ * @entity:	The entity
+ */
+int __must_check media_device_register_entity(struct media_device *mdev,
+					      struct media_entity *entity)
+{
+	unsigned int i;
+	int ret;
+
+	if (entity->function == MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN ||
+	    entity->function == MEDIA_ENT_F_UNKNOWN)
+		dev_warn(mdev->dev,
+			 "Entity type for entity %s was not initialized!\n",
+			 entity->name);
+
+	/* Warn if we apparently re-register an entity */
+	WARN_ON(entity->graph_obj.mdev != NULL);
+	entity->graph_obj.mdev = mdev;
+	INIT_LIST_HEAD(&entity->links);
+	entity->num_links = 0;
+	entity->num_backlinks = 0;
+
+	if (!ida_pre_get(&mdev->entity_internal_idx, GFP_KERNEL))
+		return -ENOMEM;
+
+	spin_lock(&mdev->lock);
+
+	ret = ida_get_new_above(&mdev->entity_internal_idx, 1,
+				&entity->internal_idx);
+	if (ret < 0) {
+		spin_unlock(&mdev->lock);
+		return ret;
+	}
+
+	mdev->entity_internal_idx_max =
+		max(mdev->entity_internal_idx_max, entity->internal_idx);
+
+	/* Initialize media_gobj embedded at the entity */
+	media_gobj_create(mdev, MEDIA_GRAPH_ENTITY, &entity->graph_obj);
+
+	/* Initialize objects at the pads */
+	for (i = 0; i < entity->num_pads; i++)
+		media_gobj_create(mdev, MEDIA_GRAPH_PAD,
+			       &entity->pads[i].graph_obj);
+
+	spin_unlock(&mdev->lock);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(media_device_register_entity);
+
+static void __media_device_unregister_entity(struct media_entity *entity)
+{
+	struct media_device *mdev = entity->graph_obj.mdev;
+	struct media_link *link, *tmp;
+	struct media_interface *intf;
+	unsigned int i;
+
+	ida_simple_remove(&mdev->entity_internal_idx, entity->internal_idx);
+
+	/* Remove all interface links pointing to this entity */
+	list_for_each_entry(intf, &mdev->interfaces, graph_obj.list) {
+		list_for_each_entry_safe(link, tmp, &intf->links, list) {
+			if (link->entity == entity)
+				__media_remove_intf_link(link);
+		}
+	}
+
+	/* Remove all data links that belong to this entity */
+	__media_entity_remove_links(entity);
+
+	/* Remove all pads that belong to this entity */
+	for (i = 0; i < entity->num_pads; i++)
+		media_gobj_destroy(&entity->pads[i].graph_obj);
+
+	/* Remove the entity */
+	media_gobj_destroy(&entity->graph_obj);
+
+	entity->graph_obj.mdev = NULL;
+}
+
+void media_device_unregister_entity(struct media_entity *entity)
+{
+	struct media_device *mdev = entity->graph_obj.mdev;
+
+	if (mdev == NULL)
+		return;
+
+	spin_lock(&mdev->lock);
+	__media_device_unregister_entity(entity);
+	spin_unlock(&mdev->lock);
+}
+EXPORT_SYMBOL_GPL(media_device_unregister_entity);
+
+/**
+ * media_device_init() - initialize a media device
  * @mdev:	The media device
  *
  * The caller is responsible for initializing the media device before
@@ -369,23 +662,41 @@ static void media_device_release(struct media_devnode *mdev)
  * - dev must point to the parent device
  * - model must be filled with the device model name
  */
-int __must_check __media_device_register(struct media_device *mdev,
-					 struct module *owner)
+void media_device_init(struct media_device *mdev)
 {
-	int ret;
-
-	if (WARN_ON(mdev->dev == NULL || mdev->model[0] == 0))
-		return -EINVAL;
-
-	mdev->entity_id = 1;
 	INIT_LIST_HEAD(&mdev->entities);
+	INIT_LIST_HEAD(&mdev->interfaces);
+	INIT_LIST_HEAD(&mdev->pads);
+	INIT_LIST_HEAD(&mdev->links);
 	spin_lock_init(&mdev->lock);
 	mutex_init(&mdev->graph_mutex);
+	ida_init(&mdev->entity_internal_idx);
+
+	dev_dbg(mdev->dev, "Media device initialized\n");
+}
+EXPORT_SYMBOL_GPL(media_device_init);
+
+void media_device_cleanup(struct media_device *mdev)
+{
+	ida_destroy(&mdev->entity_internal_idx);
+	mdev->entity_internal_idx_max = 0;
+	mutex_destroy(&mdev->graph_mutex);
+}
+EXPORT_SYMBOL_GPL(media_device_cleanup);
+
+int __must_check __media_device_register(struct media_device *mdev,
+					 struct module *owner)
+{
+	int ret;
 
 	/* Register the device node. */
 	mdev->devnode.fops = &media_device_fops;
 	mdev->devnode.parent = mdev->dev;
 	mdev->devnode.release = media_device_release;
+
+	/* Set version 0 to indicate user-space that the graph is static */
+	mdev->topology_version = 0;
+
 	ret = media_devnode_register(&mdev->devnode, owner);
 	if (ret < 0)
 		return ret;
@@ -396,69 +707,74 @@ int __must_check __media_device_register(struct media_device *mdev,
 		return ret;
 	}
 
+	dev_dbg(mdev->dev, "Media device registered\n");
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(__media_device_register);
 
-/**
- * media_device_unregister - unregister a media device
- * @mdev:	The media device
- *
- */
 void media_device_unregister(struct media_device *mdev)
 {
 	struct media_entity *entity;
 	struct media_entity *next;
+	struct media_interface *intf, *tmp_intf;
+
+	if (mdev == NULL)
+		return;
+
+	spin_lock(&mdev->lock);
 
-	list_for_each_entry_safe(entity, next, &mdev->entities, list)
-		media_device_unregister_entity(entity);
+	/* Check if mdev was ever registered at all */
+	if (!media_devnode_is_registered(&mdev->devnode)) {
+		spin_unlock(&mdev->lock);
+		return;
+	}
+
+	/* Remove all entities from the media device */
+	list_for_each_entry_safe(entity, next, &mdev->entities, graph_obj.list)
+		__media_device_unregister_entity(entity);
+
+	/* Remove all interfaces from the media device */
+	list_for_each_entry_safe(intf, tmp_intf, &mdev->interfaces,
+				 graph_obj.list) {
+		__media_remove_intf_links(intf);
+		media_gobj_destroy(&intf->graph_obj);
+		kfree(intf);
+	}
+
+	spin_unlock(&mdev->lock);
 
 	device_remove_file(&mdev->devnode.dev, &dev_attr_model);
 	media_devnode_unregister(&mdev->devnode);
+
+	dev_dbg(mdev->dev, "Media device unregistered\n");
 }
 EXPORT_SYMBOL_GPL(media_device_unregister);
 
-/**
- * media_device_register_entity - Register an entity with a media device
- * @mdev:	The media device
- * @entity:	The entity
- */
-int __must_check media_device_register_entity(struct media_device *mdev,
-					      struct media_entity *entity)
+static void media_device_release_devres(struct device *dev, void *res)
 {
-	/* Warn if we apparently re-register an entity */
-	WARN_ON(entity->parent != NULL);
-	entity->parent = mdev;
-
-	spin_lock(&mdev->lock);
-	if (entity->id == 0)
-		entity->id = mdev->entity_id++;
-	else
-		mdev->entity_id = max(entity->id + 1, mdev->entity_id);
-	list_add_tail(&entity->list, &mdev->entities);
-	spin_unlock(&mdev->lock);
-
-	return 0;
 }
-EXPORT_SYMBOL_GPL(media_device_register_entity);
 
-/**
- * media_device_unregister_entity - Unregister an entity
- * @entity:	The entity
- *
- * If the entity has never been registered this function will return
- * immediately.
- */
-void media_device_unregister_entity(struct media_entity *entity)
+struct media_device *media_device_get_devres(struct device *dev)
 {
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev;
 
-	if (mdev == NULL)
-		return;
+	mdev = devres_find(dev, media_device_release_devres, NULL, NULL);
+	if (mdev)
+		return mdev;
 
-	spin_lock(&mdev->lock);
-	list_del(&entity->list);
-	spin_unlock(&mdev->lock);
-	entity->parent = NULL;
+	mdev = devres_alloc(media_device_release_devres,
+				sizeof(struct media_device), GFP_KERNEL);
+	if (!mdev)
+		return NULL;
+	return devres_get(dev, mdev, NULL, NULL);
 }
-EXPORT_SYMBOL_GPL(media_device_unregister_entity);
+EXPORT_SYMBOL_GPL(media_device_get_devres);
+
+struct media_device *media_device_find_devres(struct device *dev)
+{
+	return devres_find(dev, media_device_release_devres, NULL, NULL);
+}
+EXPORT_SYMBOL_GPL(media_device_find_devres);
+
+#endif /* CONFIG_MEDIA_CONTROLLER */
diff --git a/drivers/media/media-devnode.c b/drivers/media/media-devnode.c
index ebf9626e5ae5..cea35bf20011 100644
--- a/drivers/media/media-devnode.c
+++ b/drivers/media/media-devnode.c
@@ -217,20 +217,6 @@ static const struct file_operations media_devnode_fops = {
 	.llseek = no_llseek,
 };
 
-/**
- * media_devnode_register - register a media device node
- * @mdev: media device node structure we want to register
- *
- * The registration code assigns minor numbers and registers the new device node
- * with the kernel. An error is returned if no free minor number can be found,
- * or if the registration of the device node fails.
- *
- * Zero is returned on success.
- *
- * Note that if the media_devnode_register call fails, the release() callback of
- * the media_devnode structure is *not* called, so the caller is responsible for
- * freeing any data.
- */
 int __must_check media_devnode_register(struct media_devnode *mdev,
 					struct module *owner)
 {
@@ -285,16 +271,6 @@ error:
 	return ret;
 }
 
-/**
- * media_devnode_unregister - unregister a media device node
- * @mdev: the device node to unregister
- *
- * This unregisters the passed device. Future open calls will be met with
- * errors.
- *
- * This function can safely be called if the device node has never been
- * registered or has already been unregistered.
- */
 void media_devnode_unregister(struct media_devnode *mdev)
 {
 	/* Check if mdev was ever registered at all */
diff --git a/drivers/media/media-entity.c b/drivers/media/media-entity.c
index 767fe55ba08e..e89d85a7d31b 100644
--- a/drivers/media/media-entity.c
+++ b/drivers/media/media-entity.c
@@ -26,65 +26,198 @@
 #include <media/media-entity.h>
 #include <media/media-device.h>
 
+static inline const char *gobj_type(enum media_gobj_type type)
+{
+	switch (type) {
+	case MEDIA_GRAPH_ENTITY:
+		return "entity";
+	case MEDIA_GRAPH_PAD:
+		return "pad";
+	case MEDIA_GRAPH_LINK:
+		return "link";
+	case MEDIA_GRAPH_INTF_DEVNODE:
+		return "intf-devnode";
+	default:
+		return "unknown";
+	}
+}
+
+static inline const char *intf_type(struct media_interface *intf)
+{
+	switch (intf->type) {
+	case MEDIA_INTF_T_DVB_FE:
+		return "frontend";
+	case MEDIA_INTF_T_DVB_DEMUX:
+		return "demux";
+	case MEDIA_INTF_T_DVB_DVR:
+		return "DVR";
+	case MEDIA_INTF_T_DVB_CA:
+		return  "CA";
+	case MEDIA_INTF_T_DVB_NET:
+		return "dvbnet";
+	case MEDIA_INTF_T_V4L_VIDEO:
+		return "video";
+	case MEDIA_INTF_T_V4L_VBI:
+		return "vbi";
+	case MEDIA_INTF_T_V4L_RADIO:
+		return "radio";
+	case MEDIA_INTF_T_V4L_SUBDEV:
+		return "v4l2-subdev";
+	case MEDIA_INTF_T_V4L_SWRADIO:
+		return "swradio";
+	default:
+		return "unknown-intf";
+	}
+};
+
+__must_check int __media_entity_enum_init(struct media_entity_enum *ent_enum,
+					  int idx_max)
+{
+	ent_enum->bmap = kcalloc(DIV_ROUND_UP(idx_max, BITS_PER_LONG),
+				 sizeof(long), GFP_KERNEL);
+	if (!ent_enum->bmap)
+		return -ENOMEM;
+
+	bitmap_zero(ent_enum->bmap, idx_max);
+	ent_enum->idx_max = idx_max;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__media_entity_enum_init);
+
+void media_entity_enum_cleanup(struct media_entity_enum *ent_enum)
+{
+	kfree(ent_enum->bmap);
+}
+EXPORT_SYMBOL_GPL(media_entity_enum_cleanup);
+
 /**
- * media_entity_init - Initialize a media entity
- *
- * @num_pads: Total number of sink and source pads.
- * @extra_links: Initial estimate of the number of extra links.
- * @pads: Array of 'num_pads' pads.
- *
- * The total number of pads is an intrinsic property of entities known by the
- * entity driver, while the total number of links depends on hardware design
- * and is an extrinsic property unknown to the entity driver. However, in most
- * use cases the entity driver can guess the number of links which can safely
- * be assumed to be equal to or larger than the number of pads.
- *
- * For those reasons the links array can be preallocated based on the entity
- * driver guess and will be reallocated later if extra links need to be
- * created.
+ *  dev_dbg_obj - Prints in debug mode a change on some object
  *
- * This function allocates a links array with enough space to hold at least
- * 'num_pads' + 'extra_links' elements. The media_entity::max_links field will
- * be set to the number of allocated elements.
+ * @event_name:	Name of the event to report. Could be __func__
+ * @gobj:	Pointer to the object
  *
- * The pads array is managed by the entity driver and passed to
- * media_entity_init() where its pointer will be stored in the entity structure.
+ * Enabled only if DEBUG or CONFIG_DYNAMIC_DEBUG. Otherwise, it
+ * won't produce any code.
  */
-int
-media_entity_init(struct media_entity *entity, u16 num_pads,
-		  struct media_pad *pads, u16 extra_links)
+static void dev_dbg_obj(const char *event_name,  struct media_gobj *gobj)
 {
-	struct media_link *links;
-	unsigned int max_links = num_pads + extra_links;
-	unsigned int i;
+#if defined(DEBUG) || defined (CONFIG_DYNAMIC_DEBUG)
+	switch (media_type(gobj)) {
+	case MEDIA_GRAPH_ENTITY:
+		dev_dbg(gobj->mdev->dev,
+			"%s id %u: entity '%s'\n",
+			event_name, media_id(gobj),
+			gobj_to_entity(gobj)->name);
+		break;
+	case MEDIA_GRAPH_LINK:
+	{
+		struct media_link *link = gobj_to_link(gobj);
+
+		dev_dbg(gobj->mdev->dev,
+			"%s id %u: %s link id %u ==> id %u\n",
+			event_name, media_id(gobj),
+			media_type(link->gobj0) == MEDIA_GRAPH_PAD ?
+				"data" : "interface",
+			media_id(link->gobj0),
+			media_id(link->gobj1));
+		break;
+	}
+	case MEDIA_GRAPH_PAD:
+	{
+		struct media_pad *pad = gobj_to_pad(gobj);
+
+		dev_dbg(gobj->mdev->dev,
+			"%s id %u: %s%spad '%s':%d\n",
+			event_name, media_id(gobj),
+			pad->flags & MEDIA_PAD_FL_SINK   ? "sink " : "",
+			pad->flags & MEDIA_PAD_FL_SOURCE ? "source " : "",
+			pad->entity->name, pad->index);
+		break;
+	}
+	case MEDIA_GRAPH_INTF_DEVNODE:
+	{
+		struct media_interface *intf = gobj_to_intf(gobj);
+		struct media_intf_devnode *devnode = intf_to_devnode(intf);
+
+		dev_dbg(gobj->mdev->dev,
+			"%s id %u: intf_devnode %s - major: %d, minor: %d\n",
+			event_name, media_id(gobj),
+			intf_type(intf),
+			devnode->major, devnode->minor);
+		break;
+	}
+	}
+#endif
+}
 
-	links = kzalloc(max_links * sizeof(links[0]), GFP_KERNEL);
-	if (links == NULL)
-		return -ENOMEM;
+void media_gobj_create(struct media_device *mdev,
+			   enum media_gobj_type type,
+			   struct media_gobj *gobj)
+{
+	BUG_ON(!mdev);
+
+	gobj->mdev = mdev;
+
+	/* Create a per-type unique object ID */
+	gobj->id = media_gobj_gen_id(type, ++mdev->id);
+
+	switch (type) {
+	case MEDIA_GRAPH_ENTITY:
+		list_add_tail(&gobj->list, &mdev->entities);
+		break;
+	case MEDIA_GRAPH_PAD:
+		list_add_tail(&gobj->list, &mdev->pads);
+		break;
+	case MEDIA_GRAPH_LINK:
+		list_add_tail(&gobj->list, &mdev->links);
+		break;
+	case MEDIA_GRAPH_INTF_DEVNODE:
+		list_add_tail(&gobj->list, &mdev->interfaces);
+		break;
+	}
+
+	mdev->topology_version++;
+
+	dev_dbg_obj(__func__, gobj);
+}
+
+void media_gobj_destroy(struct media_gobj *gobj)
+{
+	dev_dbg_obj(__func__, gobj);
+
+	gobj->mdev->topology_version++;
+
+	/* Remove the object from mdev list */
+	list_del(&gobj->list);
+}
+
+int media_entity_pads_init(struct media_entity *entity, u16 num_pads,
+			   struct media_pad *pads)
+{
+	struct media_device *mdev = entity->graph_obj.mdev;
+	unsigned int i;
 
-	entity->group_id = 0;
-	entity->max_links = max_links;
-	entity->num_links = 0;
-	entity->num_backlinks = 0;
 	entity->num_pads = num_pads;
 	entity->pads = pads;
-	entity->links = links;
+
+	if (mdev)
+		spin_lock(&mdev->lock);
 
 	for (i = 0; i < num_pads; i++) {
 		pads[i].entity = entity;
 		pads[i].index = i;
+		if (mdev)
+			media_gobj_create(mdev, MEDIA_GRAPH_PAD,
+					&entity->pads[i].graph_obj);
 	}
 
-	return 0;
-}
-EXPORT_SYMBOL_GPL(media_entity_init);
+	if (mdev)
+		spin_unlock(&mdev->lock);
 
-void
-media_entity_cleanup(struct media_entity *entity)
-{
-	kfree(entity->links);
+	return 0;
 }
-EXPORT_SYMBOL_GPL(media_entity_cleanup);
+EXPORT_SYMBOL_GPL(media_entity_pads_init);
 
 /* -----------------------------------------------------------------------------
  * Graph traversal
@@ -108,7 +241,7 @@ static void stack_push(struct media_entity_graph *graph,
 		return;
 	}
 	graph->top++;
-	graph->stack[graph->top].link = 0;
+	graph->stack[graph->top].link = entity->links.next;
 	graph->stack[graph->top].entity = entity;
 }
 
@@ -125,43 +258,51 @@ static struct media_entity *stack_pop(struct media_entity_graph *graph)
 #define link_top(en)	((en)->stack[(en)->top].link)
 #define stack_top(en)	((en)->stack[(en)->top].entity)
 
+/*
+ * TODO: Get rid of this.
+ */
+#define MEDIA_ENTITY_MAX_PADS		512
+
 /**
- * media_entity_graph_walk_start - Start walking the media graph at a given entity
+ * media_entity_graph_walk_init - Allocate resources for graph walk
  * @graph: Media graph structure that will be used to walk the graph
- * @entity: Starting entity
+ * @mdev: Media device
  *
- * This function initializes the graph traversal structure to walk the entities
- * graph starting at the given entity. The traversal structure must not be
- * modified by the caller during graph traversal. When done the structure can
- * safely be freed.
+ * Reserve resources for graph walk in media device's current
+ * state. The memory must be released using
+ * media_entity_graph_walk_free().
+ *
+ * Returns error on failure, zero on success.
  */
+__must_check int media_entity_graph_walk_init(
+	struct media_entity_graph *graph, struct media_device *mdev)
+{
+	return media_entity_enum_init(&graph->ent_enum, mdev);
+}
+EXPORT_SYMBOL_GPL(media_entity_graph_walk_init);
+
+/**
+ * media_entity_graph_walk_cleanup - Release resources related to graph walking
+ * @graph: Media graph structure that was used to walk the graph
+ */
+void media_entity_graph_walk_cleanup(struct media_entity_graph *graph)
+{
+	media_entity_enum_cleanup(&graph->ent_enum);
+}
+EXPORT_SYMBOL_GPL(media_entity_graph_walk_cleanup);
+
 void media_entity_graph_walk_start(struct media_entity_graph *graph,
 				   struct media_entity *entity)
 {
+	media_entity_enum_zero(&graph->ent_enum);
+	media_entity_enum_set(&graph->ent_enum, entity);
+
 	graph->top = 0;
 	graph->stack[graph->top].entity = NULL;
-	bitmap_zero(graph->entities, MEDIA_ENTITY_ENUM_MAX_ID);
-
-	if (WARN_ON(entity->id >= MEDIA_ENTITY_ENUM_MAX_ID))
-		return;
-
-	__set_bit(entity->id, graph->entities);
 	stack_push(graph, entity);
 }
 EXPORT_SYMBOL_GPL(media_entity_graph_walk_start);
 
-/**
- * media_entity_graph_walk_next - Get the next entity in the graph
- * @graph: Media graph structure
- *
- * Perform a depth-first traversal of the given media entities graph.
- *
- * The graph structure must have been previously initialized with a call to
- * media_entity_graph_walk_start().
- *
- * Return the next entity in the graph or NULL if the whole graph have been
- * traversed.
- */
 struct media_entity *
 media_entity_graph_walk_next(struct media_entity_graph *graph)
 {
@@ -173,30 +314,30 @@ media_entity_graph_walk_next(struct media_entity_graph *graph)
 	 * top of the stack until no more entities on the level can be
 	 * found.
 	 */
-	while (link_top(graph) < stack_top(graph)->num_links) {
+	while (link_top(graph) != &stack_top(graph)->links) {
 		struct media_entity *entity = stack_top(graph);
-		struct media_link *link = &entity->links[link_top(graph)];
+		struct media_link *link;
 		struct media_entity *next;
 
+		link = list_entry(link_top(graph), typeof(*link), list);
+
 		/* The link is not enabled so we do not follow. */
 		if (!(link->flags & MEDIA_LNK_FL_ENABLED)) {
-			link_top(graph)++;
+			link_top(graph) = link_top(graph)->next;
 			continue;
 		}
 
 		/* Get the entity in the other end of the link . */
 		next = media_entity_other(entity, link);
-		if (WARN_ON(next->id >= MEDIA_ENTITY_ENUM_MAX_ID))
-			return NULL;
 
 		/* Has the entity already been visited? */
-		if (__test_and_set_bit(next->id, graph->entities)) {
-			link_top(graph)++;
+		if (media_entity_enum_test_and_set(&graph->ent_enum, next)) {
+			link_top(graph) = link_top(graph)->next;
 			continue;
 		}
 
 		/* Push the new entity to stack and start over. */
-		link_top(graph)++;
+		link_top(graph) = link_top(graph)->next;
 		stack_push(graph, next);
 	}
 
@@ -208,39 +349,36 @@ EXPORT_SYMBOL_GPL(media_entity_graph_walk_next);
  * Pipeline management
  */
 
-/**
- * media_entity_pipeline_start - Mark a pipeline as streaming
- * @entity: Starting entity
- * @pipe: Media pipeline to be assigned to all entities in the pipeline.
- *
- * Mark all entities connected to a given entity through enabled links, either
- * directly or indirectly, as streaming. The given pipeline object is assigned to
- * every entity in the pipeline and stored in the media_entity pipe field.
- *
- * Calls to this function can be nested, in which case the same number of
- * media_entity_pipeline_stop() calls will be required to stop streaming. The
- * pipeline pointer must be identical for all nested calls to
- * media_entity_pipeline_start().
- */
 __must_check int media_entity_pipeline_start(struct media_entity *entity,
 					     struct media_pipeline *pipe)
 {
-	struct media_device *mdev = entity->parent;
-	struct media_entity_graph graph;
+	struct media_device *mdev = entity->graph_obj.mdev;
+	struct media_entity_graph *graph = &pipe->graph;
 	struct media_entity *entity_err = entity;
+	struct media_link *link;
 	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
 
-	media_entity_graph_walk_start(&graph, entity);
+	if (!pipe->streaming_count++) {
+		ret = media_entity_graph_walk_init(&pipe->graph, mdev);
+		if (ret)
+			goto error_graph_walk_start;
+	}
+
+	media_entity_graph_walk_start(&pipe->graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
+	while ((entity = media_entity_graph_walk_next(graph))) {
 		DECLARE_BITMAP(active, MEDIA_ENTITY_MAX_PADS);
 		DECLARE_BITMAP(has_no_links, MEDIA_ENTITY_MAX_PADS);
-		unsigned int i;
 
 		entity->stream_count++;
-		WARN_ON(entity->pipe && entity->pipe != pipe);
+
+		if (WARN_ON(entity->pipe && entity->pipe != pipe)) {
+			ret = -EBUSY;
+			goto error;
+		}
+
 		entity->pipe = pipe;
 
 		/* Already streaming --- no need to check. */
@@ -253,8 +391,7 @@ __must_check int media_entity_pipeline_start(struct media_entity *entity,
 		bitmap_zero(active, entity->num_pads);
 		bitmap_fill(has_no_links, entity->num_pads);
 
-		for (i = 0; i < entity->num_links; i++) {
-			struct media_link *link = &entity->links[i];
+		list_for_each_entry(link, &entity->links, list) {
 			struct media_pad *pad = link->sink->entity == entity
 						? link->sink : link->source;
 
@@ -280,7 +417,7 @@ __must_check int media_entity_pipeline_start(struct media_entity *entity,
 
 			ret = entity->ops->link_validate(link);
 			if (ret < 0 && ret != -ENOIOCTLCMD) {
-				dev_dbg(entity->parent->dev,
+				dev_dbg(entity->graph_obj.mdev->dev,
 					"link validation failed for \"%s\":%u -> \"%s\":%u, error %d\n",
 					link->source->entity->name,
 					link->source->index,
@@ -294,7 +431,7 @@ __must_check int media_entity_pipeline_start(struct media_entity *entity,
 
 		if (!bitmap_full(active, entity->num_pads)) {
 			ret = -EPIPE;
-			dev_dbg(entity->parent->dev,
+			dev_dbg(entity->graph_obj.mdev->dev,
 				"\"%s\":%u must be connected by an enabled link\n",
 				entity->name,
 				(unsigned)find_first_zero_bit(
@@ -312,9 +449,9 @@ error:
 	 * Link validation on graph failed. We revert what we did and
 	 * return the error.
 	 */
-	media_entity_graph_walk_start(&graph, entity_err);
+	media_entity_graph_walk_start(graph, entity_err);
 
-	while ((entity_err = media_entity_graph_walk_next(&graph))) {
+	while ((entity_err = media_entity_graph_walk_next(graph))) {
 		entity_err->stream_count--;
 		if (entity_err->stream_count == 0)
 			entity_err->pipe = NULL;
@@ -327,39 +464,36 @@ error:
 			break;
 	}
 
+error_graph_walk_start:
+	if (!--pipe->streaming_count)
+		media_entity_graph_walk_cleanup(graph);
+
 	mutex_unlock(&mdev->graph_mutex);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(media_entity_pipeline_start);
 
-/**
- * media_entity_pipeline_stop - Mark a pipeline as not streaming
- * @entity: Starting entity
- *
- * Mark all entities connected to a given entity through enabled links, either
- * directly or indirectly, as not streaming. The media_entity pipe field is
- * reset to NULL.
- *
- * If multiple calls to media_entity_pipeline_start() have been made, the same
- * number of calls to this function are required to mark the pipeline as not
- * streaming.
- */
 void media_entity_pipeline_stop(struct media_entity *entity)
 {
-	struct media_device *mdev = entity->parent;
-	struct media_entity_graph graph;
+	struct media_device *mdev = entity->graph_obj.mdev;
+	struct media_entity_graph *graph = &entity->pipe->graph;
+	struct media_pipeline *pipe = entity->pipe;
 
 	mutex_lock(&mdev->graph_mutex);
 
-	media_entity_graph_walk_start(&graph, entity);
+	WARN_ON(!pipe->streaming_count);
+	media_entity_graph_walk_start(graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
+	while ((entity = media_entity_graph_walk_next(graph))) {
 		entity->stream_count--;
 		if (entity->stream_count == 0)
 			entity->pipe = NULL;
 	}
 
+	if (!--pipe->streaming_count)
+		media_entity_graph_walk_cleanup(graph);
+
 	mutex_unlock(&mdev->graph_mutex);
 }
 EXPORT_SYMBOL_GPL(media_entity_pipeline_stop);
@@ -368,44 +502,26 @@ EXPORT_SYMBOL_GPL(media_entity_pipeline_stop);
  * Module use count
  */
 
-/*
- * media_entity_get - Get a reference to the parent module
- * @entity: The entity
- *
- * Get a reference to the parent media device module.
- *
- * The function will return immediately if @entity is NULL.
- *
- * Return a pointer to the entity on success or NULL on failure.
- */
 struct media_entity *media_entity_get(struct media_entity *entity)
 {
 	if (entity == NULL)
 		return NULL;
 
-	if (entity->parent->dev &&
-	    !try_module_get(entity->parent->dev->driver->owner))
+	if (entity->graph_obj.mdev->dev &&
+	    !try_module_get(entity->graph_obj.mdev->dev->driver->owner))
 		return NULL;
 
 	return entity;
 }
 EXPORT_SYMBOL_GPL(media_entity_get);
 
-/*
- * media_entity_put - Release the reference to the parent module
- * @entity: The entity
- *
- * Release the reference count acquired by media_entity_get().
- *
- * The function will return immediately if @entity is NULL.
- */
 void media_entity_put(struct media_entity *entity)
 {
 	if (entity == NULL)
 		return;
 
-	if (entity->parent->dev)
-		module_put(entity->parent->dev->driver->owner);
+	if (entity->graph_obj.mdev->dev)
+		module_put(entity->graph_obj.mdev->dev->driver->owner);
 }
 EXPORT_SYMBOL_GPL(media_entity_put);
 
@@ -413,29 +529,52 @@ EXPORT_SYMBOL_GPL(media_entity_put);
  * Links management
  */
 
-static struct media_link *media_entity_add_link(struct media_entity *entity)
+static struct media_link *media_add_link(struct list_head *head)
 {
-	if (entity->num_links >= entity->max_links) {
-		struct media_link *links = entity->links;
-		unsigned int max_links = entity->max_links + 2;
-		unsigned int i;
+	struct media_link *link;
+
+	link = kzalloc(sizeof(*link), GFP_KERNEL);
+	if (link == NULL)
+		return NULL;
 
-		links = krealloc(links, max_links * sizeof(*links), GFP_KERNEL);
-		if (links == NULL)
-			return NULL;
+	list_add_tail(&link->list, head);
 
-		for (i = 0; i < entity->num_links; i++)
-			links[i].reverse->reverse = &links[i];
+	return link;
+}
 
-		entity->max_links = max_links;
-		entity->links = links;
-	}
+static void __media_entity_remove_link(struct media_entity *entity,
+				       struct media_link *link)
+{
+	struct media_link *rlink, *tmp;
+	struct media_entity *remote;
+
+	if (link->source->entity == entity)
+		remote = link->sink->entity;
+	else
+		remote = link->source->entity;
 
-	return &entity->links[entity->num_links++];
+	list_for_each_entry_safe(rlink, tmp, &remote->links, list) {
+		if (rlink != link->reverse)
+			continue;
+
+		if (link->source->entity == entity)
+			remote->num_backlinks--;
+
+		/* Remove the remote link */
+		list_del(&rlink->list);
+		media_gobj_destroy(&rlink->graph_obj);
+		kfree(rlink);
+
+		if (--remote->num_links == 0)
+			break;
+	}
+	list_del(&link->list);
+	media_gobj_destroy(&link->graph_obj);
+	kfree(link);
 }
 
 int
-media_entity_create_link(struct media_entity *source, u16 source_pad,
+media_create_pad_link(struct media_entity *source, u16 source_pad,
 			 struct media_entity *sink, u16 sink_pad, u32 flags)
 {
 	struct media_link *link;
@@ -445,68 +584,118 @@ media_entity_create_link(struct media_entity *source, u16 source_pad,
 	BUG_ON(source_pad >= source->num_pads);
 	BUG_ON(sink_pad >= sink->num_pads);
 
-	link = media_entity_add_link(source);
+	link = media_add_link(&source->links);
 	if (link == NULL)
 		return -ENOMEM;
 
 	link->source = &source->pads[source_pad];
 	link->sink = &sink->pads[sink_pad];
-	link->flags = flags;
+	link->flags = flags & ~MEDIA_LNK_FL_INTERFACE_LINK;
+
+	/* Initialize graph object embedded at the new link */
+	media_gobj_create(source->graph_obj.mdev, MEDIA_GRAPH_LINK,
+			&link->graph_obj);
 
 	/* Create the backlink. Backlinks are used to help graph traversal and
 	 * are not reported to userspace.
 	 */
-	backlink = media_entity_add_link(sink);
+	backlink = media_add_link(&sink->links);
 	if (backlink == NULL) {
-		source->num_links--;
+		__media_entity_remove_link(source, link);
 		return -ENOMEM;
 	}
 
 	backlink->source = &source->pads[source_pad];
 	backlink->sink = &sink->pads[sink_pad];
 	backlink->flags = flags;
+	backlink->is_backlink = true;
+
+	/* Initialize graph object embedded at the new link */
+	media_gobj_create(sink->graph_obj.mdev, MEDIA_GRAPH_LINK,
+			&backlink->graph_obj);
 
 	link->reverse = backlink;
 	backlink->reverse = link;
 
 	sink->num_backlinks++;
+	sink->num_links++;
+	source->num_links++;
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(media_entity_create_link);
-
-void __media_entity_remove_links(struct media_entity *entity)
+EXPORT_SYMBOL_GPL(media_create_pad_link);
+
+int media_create_pad_links(const struct media_device *mdev,
+			   const u32 source_function,
+			   struct media_entity *source,
+			   const u16 source_pad,
+			   const u32 sink_function,
+			   struct media_entity *sink,
+			   const u16 sink_pad,
+			   u32 flags,
+			   const bool allow_both_undefined)
 {
-	unsigned int i;
-
-	for (i = 0; i < entity->num_links; i++) {
-		struct media_link *link = &entity->links[i];
-		struct media_entity *remote;
-		unsigned int r = 0;
-
-		if (link->source->entity == entity)
-			remote = link->sink->entity;
-		else
-			remote = link->source->entity;
-
-		while (r < remote->num_links) {
-			struct media_link *rlink = &remote->links[r];
+	struct media_entity *entity;
+	unsigned function;
+	int ret;
 
-			if (rlink != link->reverse) {
-				r++;
+	/* Trivial case: 1:1 relation */
+	if (source && sink)
+		return media_create_pad_link(source, source_pad,
+					     sink, sink_pad, flags);
+
+	/* Worse case scenario: n:n relation */
+	if (!source && !sink) {
+		if (!allow_both_undefined)
+			return 0;
+		media_device_for_each_entity(source, mdev) {
+			if (source->function != source_function)
 				continue;
+			media_device_for_each_entity(sink, mdev) {
+				if (sink->function != sink_function)
+					continue;
+				ret = media_create_pad_link(source, source_pad,
+							    sink, sink_pad,
+							    flags);
+				if (ret)
+					return ret;
+				flags &= ~(MEDIA_LNK_FL_ENABLED |
+					   MEDIA_LNK_FL_IMMUTABLE);
 			}
+		}
+		return 0;
+	}
 
-			if (link->source->entity == entity)
-				remote->num_backlinks--;
+	/* Handle 1:n and n:1 cases */
+	if (source)
+		function = sink_function;
+	else
+		function = source_function;
 
-			if (--remote->num_links == 0)
-				break;
+	media_device_for_each_entity(entity, mdev) {
+		if (entity->function != function)
+			continue;
 
-			/* Insert last entry in place of the dropped link. */
-			*rlink = remote->links[remote->num_links];
-		}
+		if (source)
+			ret = media_create_pad_link(source, source_pad,
+						    entity, sink_pad, flags);
+		else
+			ret = media_create_pad_link(entity, source_pad,
+						    sink, sink_pad, flags);
+		if (ret)
+			return ret;
+		flags &= ~(MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
 	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(media_create_pad_links);
+
+void __media_entity_remove_links(struct media_entity *entity)
+{
+	struct media_link *link, *tmp;
+
+	list_for_each_entry_safe(link, tmp, &entity->links, list)
+		__media_entity_remove_link(entity, link);
 
 	entity->num_links = 0;
 	entity->num_backlinks = 0;
@@ -515,13 +704,15 @@ EXPORT_SYMBOL_GPL(__media_entity_remove_links);
 
 void media_entity_remove_links(struct media_entity *entity)
 {
+	struct media_device *mdev = entity->graph_obj.mdev;
+
 	/* Do nothing if the entity is not registered. */
-	if (entity->parent == NULL)
+	if (mdev == NULL)
 		return;
 
-	mutex_lock(&entity->parent->graph_mutex);
+	spin_lock(&mdev->lock);
 	__media_entity_remove_links(entity);
-	mutex_unlock(&entity->parent->graph_mutex);
+	spin_unlock(&mdev->lock);
 }
 EXPORT_SYMBOL_GPL(media_entity_remove_links);
 
@@ -549,20 +740,6 @@ static int __media_entity_setup_link_notify(struct media_link *link, u32 flags)
 	return 0;
 }
 
-/**
- * __media_entity_setup_link - Configure a media link
- * @link: The link being configured
- * @flags: Link configuration flags
- *
- * The bulk of link setup is handled by the two entities connected through the
- * link. This function notifies both entities of the link configuration change.
- *
- * If the link is immutable or if the current and new configuration are
- * identical, return immediately.
- *
- * The user is expected to hold link->source->parent->mutex. If not,
- * media_entity_setup_link() should be used instead.
- */
 int __media_entity_setup_link(struct media_link *link, u32 flags)
 {
 	const u32 mask = MEDIA_LNK_FL_ENABLED;
@@ -590,7 +767,7 @@ int __media_entity_setup_link(struct media_link *link, u32 flags)
 	    (source->stream_count || sink->stream_count))
 		return -EBUSY;
 
-	mdev = source->parent;
+	mdev = source->graph_obj.mdev;
 
 	if (mdev->link_notify) {
 		ret = mdev->link_notify(link, flags,
@@ -611,31 +788,20 @@ int media_entity_setup_link(struct media_link *link, u32 flags)
 {
 	int ret;
 
-	mutex_lock(&link->source->entity->parent->graph_mutex);
+	mutex_lock(&link->graph_obj.mdev->graph_mutex);
 	ret = __media_entity_setup_link(link, flags);
-	mutex_unlock(&link->source->entity->parent->graph_mutex);
+	mutex_unlock(&link->graph_obj.mdev->graph_mutex);
 
 	return ret;
 }
 EXPORT_SYMBOL_GPL(media_entity_setup_link);
 
-/**
- * media_entity_find_link - Find a link between two pads
- * @source: Source pad
- * @sink: Sink pad
- *
- * Return a pointer to the link between the two entities. If no such link
- * exists, return NULL.
- */
 struct media_link *
 media_entity_find_link(struct media_pad *source, struct media_pad *sink)
 {
 	struct media_link *link;
-	unsigned int i;
-
-	for (i = 0; i < source->entity->num_links; ++i) {
-		link = &source->entity->links[i];
 
+	list_for_each_entry(link, &source->entity->links, list) {
 		if (link->source->entity == source->entity &&
 		    link->source->index == source->index &&
 		    link->sink->entity == sink->entity &&
@@ -647,23 +813,11 @@ media_entity_find_link(struct media_pad *source, struct media_pad *sink)
 }
 EXPORT_SYMBOL_GPL(media_entity_find_link);
 
-/**
- * media_entity_remote_pad - Find the pad at the remote end of a link
- * @pad: Pad at the local end of the link
- *
- * Search for a remote pad connected to the given pad by iterating over all
- * links originating or terminating at that pad until an enabled link is found.
- *
- * Return a pointer to the pad at the remote end of the first found enabled
- * link, or NULL if no enabled link has been found.
- */
 struct media_pad *media_entity_remote_pad(struct media_pad *pad)
 {
-	unsigned int i;
-
-	for (i = 0; i < pad->entity->num_links; i++) {
-		struct media_link *link = &pad->entity->links[i];
+	struct media_link *link;
 
+	list_for_each_entry(link, &pad->entity->links, list) {
 		if (!(link->flags & MEDIA_LNK_FL_ENABLED))
 			continue;
 
@@ -678,3 +832,113 @@ struct media_pad *media_entity_remote_pad(struct media_pad *pad)
 
 }
 EXPORT_SYMBOL_GPL(media_entity_remote_pad);
+
+static void media_interface_init(struct media_device *mdev,
+				 struct media_interface *intf,
+				 u32 gobj_type,
+				 u32 intf_type, u32 flags)
+{
+	intf->type = intf_type;
+	intf->flags = flags;
+	INIT_LIST_HEAD(&intf->links);
+
+	media_gobj_create(mdev, gobj_type, &intf->graph_obj);
+}
+
+/* Functions related to the media interface via device nodes */
+
+struct media_intf_devnode *media_devnode_create(struct media_device *mdev,
+						u32 type, u32 flags,
+						u32 major, u32 minor)
+{
+	struct media_intf_devnode *devnode;
+
+	devnode = kzalloc(sizeof(*devnode), GFP_KERNEL);
+	if (!devnode)
+		return NULL;
+
+	devnode->major = major;
+	devnode->minor = minor;
+
+	media_interface_init(mdev, &devnode->intf, MEDIA_GRAPH_INTF_DEVNODE,
+			     type, flags);
+
+	return devnode;
+}
+EXPORT_SYMBOL_GPL(media_devnode_create);
+
+void media_devnode_remove(struct media_intf_devnode *devnode)
+{
+	media_remove_intf_links(&devnode->intf);
+	media_gobj_destroy(&devnode->intf.graph_obj);
+	kfree(devnode);
+}
+EXPORT_SYMBOL_GPL(media_devnode_remove);
+
+struct media_link *media_create_intf_link(struct media_entity *entity,
+					    struct media_interface *intf,
+					    u32 flags)
+{
+	struct media_link *link;
+
+	link = media_add_link(&intf->links);
+	if (link == NULL)
+		return NULL;
+
+	link->intf = intf;
+	link->entity = entity;
+	link->flags = flags | MEDIA_LNK_FL_INTERFACE_LINK;
+
+	/* Initialize graph object embedded at the new link */
+	media_gobj_create(intf->graph_obj.mdev, MEDIA_GRAPH_LINK,
+			&link->graph_obj);
+
+	return link;
+}
+EXPORT_SYMBOL_GPL(media_create_intf_link);
+
+void __media_remove_intf_link(struct media_link *link)
+{
+	list_del(&link->list);
+	media_gobj_destroy(&link->graph_obj);
+	kfree(link);
+}
+EXPORT_SYMBOL_GPL(__media_remove_intf_link);
+
+void media_remove_intf_link(struct media_link *link)
+{
+	struct media_device *mdev = link->graph_obj.mdev;
+
+	/* Do nothing if the intf is not registered. */
+	if (mdev == NULL)
+		return;
+
+	spin_lock(&mdev->lock);
+	__media_remove_intf_link(link);
+	spin_unlock(&mdev->lock);
+}
+EXPORT_SYMBOL_GPL(media_remove_intf_link);
+
+void __media_remove_intf_links(struct media_interface *intf)
+{
+	struct media_link *link, *tmp;
+
+	list_for_each_entry_safe(link, tmp, &intf->links, list)
+		__media_remove_intf_link(link);
+
+}
+EXPORT_SYMBOL_GPL(__media_remove_intf_links);
+
+void media_remove_intf_links(struct media_interface *intf)
+{
+	struct media_device *mdev = intf->graph_obj.mdev;
+
+	/* Do nothing if the intf is not registered. */
+	if (mdev == NULL)
+		return;
+
+	spin_lock(&mdev->lock);
+	__media_remove_intf_links(intf);
+	spin_unlock(&mdev->lock);
+}
+EXPORT_SYMBOL_GPL(media_remove_intf_links);
diff --git a/drivers/media/pci/bt8xx/bttv-cards.c b/drivers/media/pci/bt8xx/bttv-cards.c
index 4654fb65ca21..8a17cc0bfa07 100644
--- a/drivers/media/pci/bt8xx/bttv-cards.c
+++ b/drivers/media/pci/bt8xx/bttv-cards.c
@@ -41,7 +41,7 @@
 
 #include "bttvp.h"
 #include <media/v4l2-common.h>
-#include <media/tvaudio.h>
+#include <media/i2c/tvaudio.h>
 #include "bttv-audio-hook.h"
 
 /* fwd decl */
@@ -3808,7 +3808,7 @@ static void bttv_tea575x_set_direction(struct snd_tea575x *tea, bool output)
 		gpio_inout(mask, (1 << gpio.clk) | (1 << gpio.wren));
 }
 
-static struct snd_tea575x_ops bttv_tea_ops = {
+static const struct snd_tea575x_ops bttv_tea_ops = {
 	.set_pins = bttv_tea575x_set_pins,
 	.get_pins = bttv_tea575x_get_pins,
 	.set_direction = bttv_tea575x_set_direction,
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index 15a4ebc2844d..9400e996087b 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -50,15 +50,15 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/tvaudio.h>
-#include <media/msp3400.h>
+#include <media/i2c/tvaudio.h>
+#include <media/drv-intf/msp3400.h>
 
 #include <linux/dma-mapping.h>
 
 #include <asm/io.h>
 #include <asm/byteorder.h>
 
-#include <media/saa6588.h>
+#include <media/i2c/saa6588.h>
 
 #define BTTV_VERSION "0.9.19"
 
diff --git a/drivers/media/pci/bt8xx/bttvp.h b/drivers/media/pci/bt8xx/bttvp.h
index 31bf79d3b0d2..b1e0023f923c 100644
--- a/drivers/media/pci/bt8xx/bttvp.h
+++ b/drivers/media/pci/bt8xx/bttvp.h
@@ -41,8 +41,8 @@
 #include <media/videobuf-dma-sg.h>
 #include <media/tveeprom.h>
 #include <media/rc-core.h>
-#include <media/ir-kbd-i2c.h>
-#include <media/tea575x.h>
+#include <media/i2c/ir-kbd-i2c.h>
+#include <media/drv-intf/tea575x.h>
 
 #include "bt848.h"
 #include "bttv.h"
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index c5cc14ef8347..da8b414fd824 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -705,7 +705,8 @@ struct dvb_device *dst_ca_attach(struct dst_state *dst, struct dvb_adapter *dvb_
 	struct dvb_device *dvbdev;
 
 	dprintk(verbose, DST_CA_ERROR, 1, "registering DST-CA device");
-	if (dvb_register_device(dvb_adapter, &dvbdev, &dvbdev_ca, dst, DVB_DEVICE_CA) == 0) {
+	if (dvb_register_device(dvb_adapter, &dvbdev, &dvbdev_ca, dst,
+				DVB_DEVICE_CA, 0) == 0) {
 		dst->dst_ca = dvbdev;
 		return dst->dst_ca;
 	}
diff --git a/drivers/media/pci/cobalt/cobalt-driver.c b/drivers/media/pci/cobalt/cobalt-driver.c
index 8fed61ec712e..8d6f04fc8013 100644
--- a/drivers/media/pci/cobalt/cobalt-driver.c
+++ b/drivers/media/pci/cobalt/cobalt-driver.c
@@ -21,9 +21,9 @@
  */
 
 #include <linux/delay.h>
-#include <media/adv7604.h>
-#include <media/adv7842.h>
-#include <media/adv7511.h>
+#include <media/i2c/adv7604.h>
+#include <media/i2c/adv7842.h>
+#include <media/i2c/adv7511.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-ctrls.h>
 
diff --git a/drivers/media/pci/cobalt/cobalt-irq.c b/drivers/media/pci/cobalt/cobalt-irq.c
index 3de26d0714b5..b190d4f81c6e 100644
--- a/drivers/media/pci/cobalt/cobalt-irq.c
+++ b/drivers/media/pci/cobalt/cobalt-irq.c
@@ -18,7 +18,7 @@
  *  SOFTWARE.
  */
 
-#include <media/adv7604.h>
+#include <media/i2c/adv7604.h>
 
 #include "cobalt-driver.h"
 #include "cobalt-irq.h"
@@ -134,7 +134,7 @@ done:
 		skip = true;
 		s->skip_first_frames--;
 	}
-	v4l2_get_timestamp(&cb->vb.timestamp);
+	cb->vb.vb2_buf.timestamp = ktime_get_ns();
 	/* TODO: the sequence number should be read from the FPGA so we
 	   also know about dropped frames. */
 	cb->vb.sequence = s->sequence++;
diff --git a/drivers/media/pci/cobalt/cobalt-v4l2.c b/drivers/media/pci/cobalt/cobalt-v4l2.c
index ff46e424262f..c0ba458f6cf3 100644
--- a/drivers/media/pci/cobalt/cobalt-v4l2.c
+++ b/drivers/media/pci/cobalt/cobalt-v4l2.c
@@ -29,8 +29,8 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-dv-timings.h>
-#include <media/adv7604.h>
-#include <media/adv7842.h>
+#include <media/i2c/adv7604.h>
+#include <media/i2c/adv7842.h>
 
 #include "cobalt-alsa.h"
 #include "cobalt-cpld.h"
@@ -43,11 +43,10 @@ static const struct v4l2_dv_timings cea1080p60 = V4L2_DV_BT_CEA_1920X1080P60;
 
 /* vb2 DMA streaming ops */
 
-static int cobalt_queue_setup(struct vb2_queue *q, const void *parg,
+static int cobalt_queue_setup(struct vb2_queue *q,
 			unsigned int *num_buffers, unsigned int *num_planes,
 			unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct cobalt_stream *s = q->drv_priv;
 	unsigned size = s->stride * s->height;
 
@@ -55,14 +54,11 @@ static int cobalt_queue_setup(struct vb2_queue *q, const void *parg,
 		*num_buffers = 3;
 	if (*num_buffers > NR_BUFS)
 		*num_buffers = NR_BUFS;
+	alloc_ctxs[0] = s->cobalt->alloc_ctx;
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*num_planes = 1;
-	if (fmt) {
-		if (fmt->fmt.pix.sizeimage < size)
-			return -EINVAL;
-		size = fmt->fmt.pix.sizeimage;
-	}
 	sizes[0] = size;
-	alloc_ctxs[0] = s->cobalt->alloc_ctx;
 	return 0;
 }
 
@@ -649,7 +645,7 @@ static int cobalt_s_dv_timings(struct file *file, void *priv_fh,
 		return 0;
 	}
 
-	if (v4l2_match_dv_timings(timings, &s->timings, 0))
+	if (v4l2_match_dv_timings(timings, &s->timings, 0, false))
 		return 0;
 
 	if (vb2_is_busy(&s->q))
diff --git a/drivers/media/pci/cx18/cx18-cards.c b/drivers/media/pci/cx18/cx18-cards.c
index c07c849b1aaf..5e01ea441dc4 100644
--- a/drivers/media/pci/cx18/cx18-cards.c
+++ b/drivers/media/pci/cx18/cx18-cards.c
@@ -26,7 +26,7 @@
 #include "cx18-cards.h"
 #include "cx18-av-core.h"
 #include "cx18-i2c.h"
-#include <media/cs5345.h>
+#include <media/i2c/cs5345.h>
 
 #define V4L2_STD_PAL_SECAM (V4L2_STD_PAL|V4L2_STD_SECAM)
 
diff --git a/drivers/media/pci/cx18/cx18-controls.c b/drivers/media/pci/cx18/cx18-controls.c
index 71227a155cba..adb5a8c72c06 100644
--- a/drivers/media/pci/cx18/cx18-controls.c
+++ b/drivers/media/pci/cx18/cx18-controls.c
@@ -126,7 +126,7 @@ static int cx18_s_audio_mode(struct cx2341x_handler *cxhdl, u32 val)
 	return 0;
 }
 
-struct cx2341x_handler_ops cx18_cxhdl_ops = {
+const struct cx2341x_handler_ops cx18_cxhdl_ops = {
 	.s_audio_mode = cx18_s_audio_mode,
 	.s_audio_sampling_freq = cx18_s_audio_sampling_freq,
 	.s_video_encoding = cx18_s_video_encoding,
diff --git a/drivers/media/pci/cx18/cx18-controls.h b/drivers/media/pci/cx18/cx18-controls.h
index cb5dfc7b2054..326794887863 100644
--- a/drivers/media/pci/cx18/cx18-controls.h
+++ b/drivers/media/pci/cx18/cx18-controls.h
@@ -21,4 +21,4 @@
  *  02111-1307  USA
  */
 
-extern struct cx2341x_handler_ops cx18_cxhdl_ops;
+extern const struct cx2341x_handler_ops cx18_cxhdl_ops;
diff --git a/drivers/media/pci/cx18/cx18-driver.h b/drivers/media/pci/cx18/cx18-driver.h
index b15beed2dc14..7e31f2a2e085 100644
--- a/drivers/media/pci/cx18/cx18-driver.h
+++ b/drivers/media/pci/cx18/cx18-driver.h
@@ -49,7 +49,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
 #include <media/tuner.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include "cx18-mailbox.h"
 #include "cx18-av-core.h"
 #include "cx23418.h"
diff --git a/drivers/media/pci/cx18/cx18-ioctl.c b/drivers/media/pci/cx18/cx18-ioctl.c
index 55525af1f482..eeb741c7db1b 100644
--- a/drivers/media/pci/cx18/cx18-ioctl.c
+++ b/drivers/media/pci/cx18/cx18-ioctl.c
@@ -453,8 +453,8 @@ static int cx18_cropcap(struct file *file, void *fh,
 
 	if (cropcap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
-	cropcap->pixelaspect.numerator = cx->is_50hz ? 59 : 10;
-	cropcap->pixelaspect.denominator = cx->is_50hz ? 54 : 11;
+	cropcap->pixelaspect.numerator = cx->is_50hz ? 54 : 11;
+	cropcap->pixelaspect.denominator = cx->is_50hz ? 59 : 10;
 	return 0;
 }
 
diff --git a/drivers/media/pci/cx18/cx23418.h b/drivers/media/pci/cx18/cx23418.h
index 767a8d23e3f2..67ffe65b56a3 100644
--- a/drivers/media/pci/cx18/cx23418.h
+++ b/drivers/media/pci/cx18/cx23418.h
@@ -22,7 +22,7 @@
 #ifndef CX23418_H
 #define CX23418_H
 
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 
 #define MGR_CMD_MASK            		0x40000000
 /* The MSB of the command code indicates that this is the completion of a
diff --git a/drivers/media/pci/cx23885/Kconfig b/drivers/media/pci/cx23885/Kconfig
index 2e1b88ccdbf2..3435bbaa3167 100644
--- a/drivers/media/pci/cx23885/Kconfig
+++ b/drivers/media/pci/cx23885/Kconfig
@@ -10,6 +10,7 @@ config VIDEO_CX23885
 	select VIDEOBUF2_DMA_SG
 	select VIDEO_CX25840
 	select VIDEO_CX2341X
+	select VIDEO_CS3308
 	select DVB_DIB7000P if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_DRXK if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_S5H1409 if MEDIA_SUBDRV_AUTOSELECT
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index 88a3afb66d10..bd333875a1f7 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -30,7 +30,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 
 #include "cx23885.h"
 #include "cx23885-ioctl.h"
@@ -1138,7 +1138,7 @@ static int cx23885_initialize_codec(struct cx23885_dev *dev, int startencoder)
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index f384f295676e..310ee769aed4 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -19,7 +19,7 @@
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/delay.h>
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 #include <linux/firmware.h>
 #include <misc/altera.h>
 
@@ -715,6 +715,56 @@ struct cx23885_board cx23885_boards[] = {
 		.portb		= CX23885_MPEG_DVB,
 		.portc		= CX23885_MPEG_DVB,
 	},
+	[CX23885_BOARD_VIEWCAST_260E] = {
+		.name		= "ViewCast 260e",
+		.porta		= CX23885_ANALOG_VIDEO,
+		.force_bff	= 1,
+		.input          = {{
+			.type   = CX23885_VMUX_COMPOSITE1,
+			.vmux   = CX25840_VIN6_CH1,
+			.amux   = CX25840_AUDIO7,
+		}, {
+			.type   = CX23885_VMUX_SVIDEO,
+			.vmux   = CX25840_VIN7_CH3 |
+					CX25840_VIN5_CH1 |
+					CX25840_SVIDEO_ON,
+			.amux   = CX25840_AUDIO7,
+		}, {
+			.type   = CX23885_VMUX_COMPONENT,
+			.vmux   = CX25840_VIN7_CH3 |
+					CX25840_VIN6_CH2 |
+					CX25840_VIN5_CH1 |
+					CX25840_COMPONENT_ON,
+			.amux   = CX25840_AUDIO7,
+		} },
+	},
+	[CX23885_BOARD_VIEWCAST_460E] = {
+		.name		= "ViewCast 460e",
+		.porta		= CX23885_ANALOG_VIDEO,
+		.force_bff	= 1,
+		.input          = {{
+			.type   = CX23885_VMUX_COMPOSITE1,
+			.vmux   = CX25840_VIN4_CH1,
+			.amux   = CX25840_AUDIO7,
+		}, {
+			.type   = CX23885_VMUX_SVIDEO,
+			.vmux   = CX25840_VIN7_CH3 |
+					CX25840_VIN6_CH1 |
+					CX25840_SVIDEO_ON,
+			.amux   = CX25840_AUDIO7,
+		}, {
+			.type   = CX23885_VMUX_COMPONENT,
+			.vmux   = CX25840_VIN7_CH3 |
+					CX25840_VIN6_CH1 |
+					CX25840_VIN5_CH2 |
+					CX25840_COMPONENT_ON,
+			.amux   = CX25840_AUDIO7,
+		}, {
+			.type   = CX23885_VMUX_COMPOSITE2,
+			.vmux   = CX25840_VIN6_CH1,
+			.amux   = CX25840_AUDIO7,
+		} },
+	},
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
 
@@ -1002,6 +1052,14 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x0070,
 		.subdevice = 0xf038,
 		.card      = CX23885_BOARD_HAUPPAUGE_HVR5525,
+	}, {
+		.subvendor = 0x1576,
+		.subdevice = 0x0260,
+		.card      = CX23885_BOARD_VIEWCAST_260E,
+	}, {
+		.subvendor = 0x1576,
+		.subdevice = 0x0460,
+		.card      = CX23885_BOARD_VIEWCAST_460E,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -1034,6 +1092,28 @@ void cx23885_card_list(struct cx23885_dev *dev)
 		       dev->name, i, cx23885_boards[i].name);
 }
 
+static void viewcast_eeprom(struct cx23885_dev *dev, u8 *eeprom_data)
+{
+	u32 sn;
+
+	/* The serial number record begins with tag 0x59 */
+	if (*(eeprom_data + 0x00) != 0x59) {
+		pr_info("%s() eeprom records are undefined, no serial number\n",
+			__func__);
+		return;
+	}
+
+	sn =	(*(eeprom_data + 0x06) << 24) |
+		(*(eeprom_data + 0x05) << 16) |
+		(*(eeprom_data + 0x04) << 8) |
+		(*(eeprom_data + 0x03));
+
+	pr_info("%s: card '%s' sn# MM%d\n",
+		dev->name,
+		cx23885_boards[dev->board].name,
+		sn);
+}
+
 static void hauppauge_eeprom(struct cx23885_dev *dev, u8 *eeprom_data)
 {
 	struct tveeprom tv;
@@ -1671,6 +1751,12 @@ void cx23885_gpio_setup(struct cx23885_dev *dev)
 		cx23885_gpio_set(dev, GPIO_8 | GPIO_9);
 		msleep(100);
 		break;
+	case CX23885_BOARD_VIEWCAST_260E:
+	case CX23885_BOARD_VIEWCAST_460E:
+		/* For documentation purposes, it's worth noting that this
+		 * card does not have any GPIO's connected to subcomponents.
+		 */
+		break;
 	}
 }
 
@@ -1917,6 +2003,14 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		if (dev->i2c_bus[0].i2c_rc == 0)
 			hauppauge_eeprom(dev, eeprom+0xc0);
 		break;
+	case CX23885_BOARD_VIEWCAST_260E:
+	case CX23885_BOARD_VIEWCAST_460E:
+		dev->i2c_bus[1].i2c_client.addr = 0xa0 >> 1;
+		tveeprom_read(&dev->i2c_bus[1].i2c_client,
+			      eeprom, sizeof(eeprom));
+		if (dev->i2c_bus[0].i2c_rc == 0)
+			viewcast_eeprom(dev, eeprom);
+		break;
 	}
 
 	switch (dev->board) {
@@ -2120,6 +2214,8 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_DVBSKY_S950:
 	case CX23885_BOARD_DVBSKY_S952:
 	case CX23885_BOARD_DVBSKY_T982:
+	case CX23885_BOARD_VIEWCAST_260E:
+	case CX23885_BOARD_VIEWCAST_460E:
 		dev->sd_cx25840 = v4l2_i2c_new_subdev(&dev->v4l2_dev,
 				&dev->i2c_bus[2].i2c_adap,
 				"cx25840", 0x88 >> 1, NULL);
@@ -2130,6 +2226,24 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		break;
 	}
 
+	switch (dev->board) {
+	case CX23885_BOARD_VIEWCAST_260E:
+		v4l2_i2c_new_subdev(&dev->v4l2_dev,
+				&dev->i2c_bus[0].i2c_adap,
+				"cs3308", 0x82 >> 1, NULL);
+		break;
+	case CX23885_BOARD_VIEWCAST_460E:
+		/* This cs3308 controls the audio from the breakout cable */
+		v4l2_i2c_new_subdev(&dev->v4l2_dev,
+				&dev->i2c_bus[0].i2c_adap,
+				"cs3308", 0x80 >> 1, NULL);
+		/* This cs3308 controls the audio from the onboard header */
+		v4l2_i2c_new_subdev(&dev->v4l2_dev,
+				&dev->i2c_bus[0].i2c_adap,
+				"cs3308", 0x82 >> 1, NULL);
+		break;
+	}
+
 	/* AUX-PLL 27MHz CLK */
 	switch (dev->board) {
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
diff --git a/drivers/media/pci/cx23885/cx23885-core.c b/drivers/media/pci/cx23885/cx23885-core.c
index e8f847226a19..813c217b5e1a 100644
--- a/drivers/media/pci/cx23885/cx23885-core.c
+++ b/drivers/media/pci/cx23885/cx23885-core.c
@@ -427,7 +427,7 @@ static void cx23885_wakeup(struct cx23885_tsport *port,
 	buf = list_entry(q->active.next,
 			 struct cx23885_buffer, queue);
 
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	buf->vb.sequence = q->count++;
 	dprintk(1, "[%p/%d] wakeup reg=%d buf=%d\n", buf,
 		buf->vb.vb2_buf.index,
@@ -968,6 +968,16 @@ static int cx23885_dev_setup(struct cx23885_dev *dev)
 	call_all(dev, core, s_power, 0);
 	cx23885_ir_init(dev);
 
+	if (dev->board == CX23885_BOARD_VIEWCAST_460E) {
+		/*
+		 * GPIOs 9/8 are input detection bits for the breakout video
+		 * (gpio 8) and audio (gpio 9) cables. When they're attached,
+		 * this gpios are pulled high. Make sure these GPIOs are marked
+		 * as inputs.
+		 */
+		cx23885_gpio_enable(dev, 0x300, 0);
+	}
+
 	if (cx23885_boards[dev->board].porta == CX23885_ANALOG_VIDEO) {
 		if (cx23885_video_register(dev) < 0) {
 			printk(KERN_ERR "%s() Failed to register analog "
diff --git a/drivers/media/pci/cx23885/cx23885-dvb.c b/drivers/media/pci/cx23885/cx23885-dvb.c
index c4307ad8594c..80319bb73d94 100644
--- a/drivers/media/pci/cx23885/cx23885-dvb.c
+++ b/drivers/media/pci/cx23885/cx23885-dvb.c
@@ -92,7 +92,7 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -2168,11 +2168,12 @@ static int dvb_register(struct cx23885_tsport *port)
 		}
 		port->i2c_client_tuner = client_tuner;
 		break;
-	case CX23885_BOARD_HAUPPAUGE_HVR5525:
-		switch (port->nr) {
+	case CX23885_BOARD_HAUPPAUGE_HVR5525: {
 		struct m88rs6000t_config m88rs6000t_config;
 		struct a8293_platform_data a8293_pdata = {};
 
+		switch (port->nr) {
+
 		/* port b - satellite */
 		case 1:
 			/* attach frontend */
@@ -2267,6 +2268,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
+	}
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
 			" isn't supported yet\n",
diff --git a/drivers/media/pci/cx23885/cx23885-i2c.c b/drivers/media/pci/cx23885/cx23885-i2c.c
index 1135ea3f6ce5..ae061b358591 100644
--- a/drivers/media/pci/cx23885/cx23885-i2c.c
+++ b/drivers/media/pci/cx23885/cx23885-i2c.c
@@ -279,6 +279,8 @@ static char *i2c_devs[128] = {
 	[0x10 >> 1] = "tda10048",
 	[0x12 >> 1] = "dib7000pc",
 	[0x1c >> 1] = "lgdt3303",
+	[0x80 >> 1] = "cs3308",
+	[0x82 >> 1] = "cs3308",
 	[0x86 >> 1] = "tda9887",
 	[0x32 >> 1] = "cx24227",
 	[0x88 >> 1] = "cx25837",
diff --git a/drivers/media/pci/cx23885/cx23885-input.c b/drivers/media/pci/cx23885/cx23885-input.c
index 088799c3b49b..64328d08ac2f 100644
--- a/drivers/media/pci/cx23885/cx23885-input.c
+++ b/drivers/media/pci/cx23885/cx23885-input.c
@@ -268,7 +268,7 @@ int cx23885_input_init(struct cx23885_dev *dev)
 	struct rc_dev *rc;
 	char *rc_map;
 	enum rc_driver_type driver_type;
-	unsigned long allowed_protos;
+	u64 allowed_protos;
 
 	int ret;
 
diff --git a/drivers/media/pci/cx23885/cx23885-vbi.c b/drivers/media/pci/cx23885/cx23885-vbi.c
index cf3cb1324c55..39750ebcc04c 100644
--- a/drivers/media/pci/cx23885/cx23885-vbi.c
+++ b/drivers/media/pci/cx23885/cx23885-vbi.c
@@ -83,7 +83,7 @@ int cx23885_vbi_irq(struct cx23885_dev *dev, u32 status)
 	if (status & VID_BC_MSK_VBI_RISCI1) {
 		dprintk(1, "%s() VID_BC_MSK_VBI_RISCI1\n", __func__);
 		spin_lock(&dev->slock);
-		count = cx_read(VID_A_GPCNT);
+		count = cx_read(VBI_A_GPCNT);
 		cx23885_video_wakeup(dev, &dev->vbiq, count);
 		spin_unlock(&dev->slock);
 		handled++;
@@ -103,7 +103,6 @@ static int cx23885_start_vbi_dma(struct cx23885_dev    *dev,
 				VBI_LINE_LENGTH, buf->risc.dma);
 
 	/* reset counter */
-	cx_write(VID_A_GPCNT_CTL, 3);
 	cx_write(VID_A_VBI_CTRL, 3);
 	cx_write(VBI_A_GPCNT_CTL, 3);
 	q->count = 0;
@@ -121,7 +120,7 @@ static int cx23885_start_vbi_dma(struct cx23885_dev    *dev,
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 71a80e2b842c..e1d7d0847167 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -35,7 +35,7 @@
 #include "cx23885-ioctl.h"
 #include "tuner-xc2028.h"
 
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx23885 based TV cards");
 MODULE_AUTHOR("Steven Toth <stoth@linuxtv.org>");
@@ -105,7 +105,7 @@ void cx23885_video_wakeup(struct cx23885_dev *dev,
 			struct cx23885_buffer, queue);
 
 	buf->vb.sequence = q->count++;
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	dprintk(2, "[%p/%d] wakeup reg=%d buf=%d\n", buf,
 			buf->vb.vb2_buf.index, count, q->count);
 	list_del(&buf->queue);
@@ -114,11 +114,19 @@ void cx23885_video_wakeup(struct cx23885_dev *dev,
 
 int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm)
 {
+	struct v4l2_subdev_format format = {
+		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
+		.format.code = MEDIA_BUS_FMT_FIXED,
+	};
+
 	dprintk(1, "%s(norm = 0x%08x) name: [%s]\n",
 		__func__,
 		(unsigned int)norm,
 		v4l2_norm_to_name(norm));
 
+	if (dev->tvnorm == norm)
+		return 0;
+
 	if (dev->tvnorm != norm) {
 		if (vb2_is_busy(&dev->vb2_vidq) || vb2_is_busy(&dev->vb2_vbiq) ||
 		    vb2_is_busy(&dev->vb2_mpegq))
@@ -126,9 +134,17 @@ int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm)
 	}
 
 	dev->tvnorm = norm;
+	dev->width = 720;
+	dev->height = norm_maxh(norm);
+	dev->field = V4L2_FIELD_INTERLACED;
 
 	call_all(dev, video, s_std, norm);
 
+	format.format.width = dev->width;
+	format.format.height = dev->height;
+	format.format.field = dev->field;
+	call_all(dev, pad, set_fmt, NULL, &format);
+
 	return 0;
 }
 
@@ -247,7 +263,9 @@ static int cx23885_video_mux(struct cx23885_dev *dev, unsigned int input)
 		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1255_22111) ||
 		(dev->board == CX23885_BOARD_HAUPPAUGE_HVR1850) ||
 		(dev->board == CX23885_BOARD_MYGICA_X8507) ||
-		(dev->board == CX23885_BOARD_AVERMEDIA_HC81R)) {
+		(dev->board == CX23885_BOARD_AVERMEDIA_HC81R) ||
+		(dev->board == CX23885_BOARD_VIEWCAST_260E) ||
+		(dev->board == CX23885_BOARD_VIEWCAST_460E)) {
 		/* Configure audio routing */
 		v4l2_subdev_call(dev->sd_cx25840, audio, s_routing,
 			INPUT(input)->amux, 0, 0);
@@ -315,7 +333,7 @@ static int cx23885_start_video_dma(struct cx23885_dev *dev,
 	return 0;
 }
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -545,7 +563,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		return -EINVAL;
 
 	field = f->fmt.pix.field;
-	maxw  = norm_maxw(dev->tvnorm);
+	maxw  = 720;
 	maxh  = norm_maxh(dev->tvnorm);
 
 	if (V4L2_FIELD_ANY == field) {
@@ -648,6 +666,26 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 	return 0;
 }
 
+static int vidioc_cropcap(struct file *file, void *priv,
+			  struct v4l2_cropcap *cc)
+{
+	struct cx23885_dev *dev = video_drvdata(file);
+	bool is_50hz = dev->tvnorm & V4L2_STD_625_50;
+
+	if (cc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	cc->bounds.left = 0;
+	cc->bounds.top = 0;
+	cc->bounds.width = 720;
+	cc->bounds.height = norm_maxh(dev->tvnorm);
+	cc->defrect = cc->bounds;
+	cc->pixelaspect.numerator = is_50hz ? 54 : 11;
+	cc->pixelaspect.denominator = is_50hz ? 59 : 10;
+
+	return 0;
+}
+
 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 {
 	struct cx23885_dev *dev = video_drvdata(file);
@@ -1082,6 +1120,7 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
 	.vidioc_streamon      = vb2_ioctl_streamon,
 	.vidioc_streamoff     = vb2_ioctl_streamoff,
+	.vidioc_cropcap       = vidioc_cropcap,
 	.vidioc_s_std         = vidioc_s_std,
 	.vidioc_g_std         = vidioc_g_std,
 	.vidioc_enum_input    = vidioc_enum_input,
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index c5ba0833f47a..b1a5409408c7 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -30,7 +30,7 @@
 #include <media/rc-core.h>
 
 #include "cx23885-reg.h"
-#include "media/cx2341x.h"
+#include "media/drv-intf/cx2341x.h"
 
 #include <linux/mutex.h>
 
@@ -101,6 +101,8 @@
 #define CX23885_BOARD_DVBSKY_T982              51
 #define CX23885_BOARD_HAUPPAUGE_HVR5525        52
 #define CX23885_BOARD_HAUPPAUGE_STARBURST      53
+#define CX23885_BOARD_VIEWCAST_260E            54
+#define CX23885_BOARD_VIEWCAST_460E            55
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
@@ -627,11 +629,6 @@ extern int cx23885_risc_databuffer(struct pci_dev *pci,
 /* ----------------------------------------------------------- */
 /* tv norms                                                    */
 
-static inline unsigned int norm_maxw(v4l2_std_id norm)
-{
-	return (norm & V4L2_STD_525_60) ? 720 : 768;
-}
-
 static inline unsigned int norm_maxh(v4l2_std_id norm)
 {
 	return (norm & V4L2_STD_525_60) ? 480 : 576;
diff --git a/drivers/media/pci/cx25821/cx25821-video.c b/drivers/media/pci/cx25821/cx25821-video.c
index 26e3e296d615..c48bba9daf1f 100644
--- a/drivers/media/pci/cx25821/cx25821-video.c
+++ b/drivers/media/pci/cx25821/cx25821-video.c
@@ -130,7 +130,7 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 			buf = list_entry(dmaq->active.next,
 					 struct cx25821_buffer, queue);
 
-			v4l2_get_timestamp(&buf->vb.timestamp);
+			buf->vb.vb2_buf.timestamp = ktime_get_ns();
 			buf->vb.sequence = dmaq->count++;
 			list_del(&buf->queue);
 			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -141,20 +141,20 @@ int cx25821_video_irq(struct cx25821_dev *dev, int chan_num, u32 status)
 	return handled;
 }
 
-static int cx25821_queue_setup(struct vb2_queue *q, const void *parg,
+static int cx25821_queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct cx25821_channel *chan = q->drv_priv;
 	unsigned size = (chan->fmt->depth * chan->width * chan->height) >> 3;
 
-	if (fmt && fmt->fmt.pix.sizeimage < size)
-		return -EINVAL;
+	alloc_ctxs[0] = chan->dev->alloc_ctx;
+
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
 
 	*num_planes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : size;
-	alloc_ctxs[0] = chan->dev->alloc_ctx;
+	sizes[0] = size;
 	return 0;
 }
 
diff --git a/drivers/media/pci/cx88/cx88-alsa.c b/drivers/media/pci/cx88/cx88-alsa.c
index 1b5268f9bb24..e158a1da1d41 100644
--- a/drivers/media/pci/cx88/cx88-alsa.c
+++ b/drivers/media/pci/cx88/cx88-alsa.c
@@ -40,7 +40,7 @@
 #include <sound/control.h>
 #include <sound/initval.h>
 #include <sound/tlv.h>
-#include <media/wm8775.h>
+#include <media/i2c/wm8775.h>
 
 #include "cx88.h"
 #include "cx88-reg.h"
diff --git a/drivers/media/pci/cx88/cx88-blackbird.c b/drivers/media/pci/cx88/cx88-blackbird.c
index 8b889135be8a..3233d45d1e5b 100644
--- a/drivers/media/pci/cx88/cx88-blackbird.c
+++ b/drivers/media/pci/cx88/cx88-blackbird.c
@@ -36,7 +36,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 
 #include "cx88.h"
 
@@ -637,7 +637,7 @@ static int blackbird_stop_codec(struct cx8802_dev *dev)
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx88/cx88-core.c b/drivers/media/pci/cx88/cx88-core.c
index 9a43c7826b60..46fe8c1eb9d4 100644
--- a/drivers/media/pci/cx88/cx88-core.c
+++ b/drivers/media/pci/cx88/cx88-core.c
@@ -518,7 +518,7 @@ void cx88_wakeup(struct cx88_core *core,
 
 	buf = list_entry(q->active.next,
 			 struct cx88_buffer, list);
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	buf->vb.field = core->field;
 	buf->vb.sequence = q->count++;
 	list_del(&buf->list);
diff --git a/drivers/media/pci/cx88/cx88-dvb.c b/drivers/media/pci/cx88/cx88-dvb.c
index f04835073844..afb20756d7a5 100644
--- a/drivers/media/pci/cx88/cx88-dvb.c
+++ b/drivers/media/pci/cx88/cx88-dvb.c
@@ -82,7 +82,7 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx88/cx88-vbi.c b/drivers/media/pci/cx88/cx88-vbi.c
index 007a5eee8e5e..ccc646d819f2 100644
--- a/drivers/media/pci/cx88/cx88-vbi.c
+++ b/drivers/media/pci/cx88/cx88-vbi.c
@@ -107,7 +107,7 @@ int cx8800_restart_vbi_queue(struct cx8800_dev    *dev,
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index aef9acf351f6..5f331df65fb9 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -41,7 +41,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/wm8775.h>
+#include <media/i2c/wm8775.h>
 
 MODULE_DESCRIPTION("v4l2 driver module for cx2388x based TV cards");
 MODULE_AUTHOR("Gerd Knorr <kraxel@bytesex.org> [SuSE Labs]");
@@ -429,7 +429,7 @@ static int restart_video_queue(struct cx8800_dev    *dev,
 
 /* ------------------------------------------------------------------ */
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/cx88/cx88.h b/drivers/media/pci/cx88/cx88.h
index 2996eb3ea1fc..78f817ee7e41 100644
--- a/drivers/media/pci/cx88/cx88.h
+++ b/drivers/media/pci/cx88/cx88.h
@@ -30,10 +30,10 @@
 #include <media/tuner.h>
 #include <media/tveeprom.h>
 #include <media/videobuf2-dma-sg.h>
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 #include <media/videobuf2-dvb.h>
-#include <media/ir-kbd-i2c.h>
-#include <media/wm8775.h>
+#include <media/i2c/ir-kbd-i2c.h>
+#include <media/i2c/wm8775.h>
 
 #include "cx88-reg.h"
 #include "tuner-xc2028.h"
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index 0ac2dd35fe50..9d5b314142f1 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -81,13 +81,13 @@ static int i2c_read_reg16(struct i2c_adapter *adapter, u8 adr,
 static int ddb_i2c_cmd(struct ddb_i2c *i2c, u32 adr, u32 cmd)
 {
 	struct ddb *dev = i2c->dev;
-	int stat;
+	long stat;
 	u32 val;
 
 	i2c->done = 0;
 	ddbwritel((adr << 9) | cmd, i2c->regs + I2C_COMMAND);
 	stat = wait_event_timeout(i2c->wq, i2c->done == 1, HZ);
-	if (stat <= 0) {
+	if (stat == 0) {
 		printk(KERN_ERR "I2C timeout\n");
 		{ /* MSI debugging*/
 			u32 istat = ddbreadl(INTERRUPT_STATUS);
@@ -1065,7 +1065,7 @@ static int ddb_ci_attach(struct ddb_port *port)
 			    port->en, 0, 1);
 	ret = dvb_register_device(&port->output->adap, &port->output->dev,
 				  &dvbdev_ci, (void *) port->output,
-				  DVB_DEVICE_SEC);
+				  DVB_DEVICE_SEC, 0);
 	return ret;
 }
 
diff --git a/drivers/media/pci/dm1105/dm1105.c b/drivers/media/pci/dm1105/dm1105.c
index 88915fb87e80..5dd504741b12 100644
--- a/drivers/media/pci/dm1105/dm1105.c
+++ b/drivers/media/pci/dm1105/dm1105.c
@@ -1206,7 +1206,6 @@ static void dm1105_remove(struct pci_dev *pdev)
 	i2c_del_adapter(&dev->i2c_adap);
 
 	dm1105_hw_exit(dev);
-	synchronize_irq(pdev->irq);
 	free_irq(pdev->irq, dev);
 	pci_iounmap(pdev, dev->io_mem);
 	pci_release_regions(pdev);
diff --git a/drivers/media/pci/dt3155/dt3155.c b/drivers/media/pci/dt3155/dt3155.c
index d84abde5ea29..568c0c8fb2dc 100644
--- a/drivers/media/pci/dt3155/dt3155.c
+++ b/drivers/media/pci/dt3155/dt3155.c
@@ -131,22 +131,21 @@ static int wait_i2c_reg(void __iomem *addr)
 }
 
 static int
-dt3155_queue_setup(struct vb2_queue *vq, const void *parg,
+dt3155_queue_setup(struct vb2_queue *vq,
 		unsigned int *nbuffers, unsigned int *num_planes,
 		unsigned int sizes[], void *alloc_ctxs[])
 
 {
-	const struct v4l2_format *fmt = parg;
 	struct dt3155_priv *pd = vb2_get_drv_priv(vq);
 	unsigned size = pd->width * pd->height;
 
 	if (vq->num_buffers + *nbuffers < 2)
 		*nbuffers = 2 - vq->num_buffers;
-	if (fmt && fmt->fmt.pix.sizeimage < size)
-		return -EINVAL;
-	*num_planes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : size;
 	alloc_ctxs[0] = pd->alloc_ctx;
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+	*num_planes = 1;
+	sizes[0] = size;
 	return 0;
 }
 
@@ -271,7 +270,7 @@ static irqreturn_t dt3155_irq_handler_even(int irq, void *dev_id)
 
 	spin_lock(&ipd->lock);
 	if (ipd->curr_buf && !list_empty(&ipd->dmaq)) {
-		v4l2_get_timestamp(&ipd->curr_buf->timestamp);
+		ipd->curr_buf->vb2_buf.timestamp = ktime_get_ns();
 		ipd->curr_buf->sequence = ipd->sequence++;
 		ipd->curr_buf->field = V4L2_FIELD_NONE;
 		vb2_buffer_done(&ipd->curr_buf->vb2_buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/media/pci/ivtv/ivtv-cards.c b/drivers/media/pci/ivtv/ivtv-cards.c
index 145e4749a69d..410d97bdf541 100644
--- a/drivers/media/pci/ivtv/ivtv-cards.c
+++ b/drivers/media/pci/ivtv/ivtv-cards.c
@@ -22,12 +22,12 @@
 #include "ivtv-cards.h"
 #include "ivtv-i2c.h"
 
-#include <media/msp3400.h>
-#include <media/m52790.h>
-#include <media/wm8775.h>
-#include <media/cs53l32a.h>
-#include <media/cx25840.h>
-#include <media/upd64031a.h>
+#include <media/drv-intf/msp3400.h>
+#include <media/i2c/m52790.h>
+#include <media/i2c/wm8775.h>
+#include <media/i2c/cs53l32a.h>
+#include <media/drv-intf/cx25840.h>
+#include <media/i2c/upd64031a.h>
 
 #define MSP_TUNER  MSP_INPUT(MSP_IN_SCART1, MSP_IN_TUNER1, \
 				MSP_DSP_IN_TUNER, MSP_DSP_IN_TUNER)
diff --git a/drivers/media/pci/ivtv/ivtv-controls.c b/drivers/media/pci/ivtv/ivtv-controls.c
index 8a55ccb8f0c9..9666ca01549c 100644
--- a/drivers/media/pci/ivtv/ivtv-controls.c
+++ b/drivers/media/pci/ivtv/ivtv-controls.c
@@ -96,7 +96,7 @@ static int ivtv_s_audio_mode(struct cx2341x_handler *cxhdl, u32 val)
 	return 0;
 }
 
-struct cx2341x_handler_ops ivtv_cxhdl_ops = {
+const struct cx2341x_handler_ops ivtv_cxhdl_ops = {
 	.s_audio_mode = ivtv_s_audio_mode,
 	.s_audio_sampling_freq = ivtv_s_audio_sampling_freq,
 	.s_video_encoding = ivtv_s_video_encoding,
diff --git a/drivers/media/pci/ivtv/ivtv-controls.h b/drivers/media/pci/ivtv/ivtv-controls.h
index 3999e6358312..ea397ba837e3 100644
--- a/drivers/media/pci/ivtv/ivtv-controls.h
+++ b/drivers/media/pci/ivtv/ivtv-controls.h
@@ -21,7 +21,7 @@
 #ifndef IVTV_CONTROLS_H
 #define IVTV_CONTROLS_H
 
-extern struct cx2341x_handler_ops ivtv_cxhdl_ops;
+extern const struct cx2341x_handler_ops ivtv_cxhdl_ops;
 extern const struct v4l2_ctrl_ops ivtv_hdl_out_ops;
 int ivtv_g_pts_frame(struct ivtv *itv, s64 *pts, s64 *frame);
 
diff --git a/drivers/media/pci/ivtv/ivtv-driver.c b/drivers/media/pci/ivtv/ivtv-driver.c
index c2e60b4f292d..374033a5bdaf 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.c
+++ b/drivers/media/pci/ivtv/ivtv-driver.c
@@ -57,7 +57,7 @@
 #include "ivtv-gpio.h"
 #include <linux/dma-mapping.h>
 #include <media/tveeprom.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include "tuner-xc2028.h"
 
 /* If you have already X v4l cards, then set this to X. This way
@@ -826,7 +826,7 @@ static void ivtv_init_struct2(struct ivtv *itv)
 				IVTV_CARD_INPUT_VID_TUNER)
 			break;
 	}
-	if (i == itv->nof_inputs)
+	if (i >= itv->nof_inputs)
 		i = 0;
 	itv->active_input = i;
 	itv->audio_input = itv->card->video_inputs[i].audio_index;
diff --git a/drivers/media/pci/ivtv/ivtv-driver.h b/drivers/media/pci/ivtv/ivtv-driver.h
index ee0ef6e48c7d..6c08dae67a73 100644
--- a/drivers/media/pci/ivtv/ivtv-driver.h
+++ b/drivers/media/pci/ivtv/ivtv-driver.h
@@ -64,8 +64,8 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-fh.h>
 #include <media/tuner.h>
-#include <media/cx2341x.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/drv-intf/cx2341x.h>
+#include <media/i2c/ir-kbd-i2c.h>
 
 #include <linux/ivtv.h>
 
diff --git a/drivers/media/pci/ivtv/ivtv-fileops.c b/drivers/media/pci/ivtv/ivtv-fileops.c
index 605d280d8a5f..c9bd018e53de 100644
--- a/drivers/media/pci/ivtv/ivtv-fileops.c
+++ b/drivers/media/pci/ivtv/ivtv-fileops.c
@@ -34,7 +34,7 @@
 #include "ivtv-cards.h"
 #include "ivtv-firmware.h"
 #include <media/v4l2-event.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 
 /* This function tries to claim the stream for a specific file descriptor.
    If no one else is using this stream then the stream is claimed and
diff --git a/drivers/media/pci/ivtv/ivtv-firmware.c b/drivers/media/pci/ivtv/ivtv-firmware.c
index 4b0e758a7bce..5b3095f65dce 100644
--- a/drivers/media/pci/ivtv/ivtv-firmware.c
+++ b/drivers/media/pci/ivtv/ivtv-firmware.c
@@ -26,7 +26,7 @@
 #include "ivtv-ioctl.h"
 #include "ivtv-cards.h"
 #include <linux/firmware.h>
-#include <media/saa7127.h>
+#include <media/i2c/saa7127.h>
 
 #define IVTV_MASK_SPU_ENABLE 		0xFFFFFFFE
 #define IVTV_MASK_VPU_ENABLE15 		0xFFFFFFF6
diff --git a/drivers/media/pci/ivtv/ivtv-i2c.c b/drivers/media/pci/ivtv/ivtv-i2c.c
index 1a41ba5c7d30..bccbf2d18e30 100644
--- a/drivers/media/pci/ivtv/ivtv-i2c.c
+++ b/drivers/media/pci/ivtv/ivtv-i2c.c
@@ -63,7 +63,7 @@
 #include "ivtv-cards.h"
 #include "ivtv-gpio.h"
 #include "ivtv-i2c.h"
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 
 /* i2c implementation for cx23415/6 chip, ivtv project.
  * Author: Kevin Thayer (nufan_wfk at yahoo.com)
diff --git a/drivers/media/pci/ivtv/ivtv-ioctl.c b/drivers/media/pci/ivtv/ivtv-ioctl.c
index 9a21c17fc376..2dc4b20f3ac0 100644
--- a/drivers/media/pci/ivtv/ivtv-ioctl.c
+++ b/drivers/media/pci/ivtv/ivtv-ioctl.c
@@ -32,7 +32,7 @@
 #include "ivtv-gpio.h"
 #include "ivtv-controls.h"
 #include "ivtv-cards.h"
-#include <media/saa7127.h>
+#include <media/i2c/saa7127.h>
 #include <media/tveeprom.h>
 #include <media/v4l2-event.h>
 #include <linux/dvb/audio.h>
@@ -831,11 +831,11 @@ static int ivtv_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cropca
 	struct ivtv *itv = id->itv;
 
 	if (cropcap->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
-		cropcap->pixelaspect.numerator = itv->is_50hz ? 59 : 10;
-		cropcap->pixelaspect.denominator = itv->is_50hz ? 54 : 11;
+		cropcap->pixelaspect.numerator = itv->is_50hz ? 54 : 11;
+		cropcap->pixelaspect.denominator = itv->is_50hz ? 59 : 10;
 	} else if (cropcap->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		cropcap->pixelaspect.numerator = itv->is_out_50hz ? 59 : 10;
-		cropcap->pixelaspect.denominator = itv->is_out_50hz ? 54 : 11;
+		cropcap->pixelaspect.numerator = itv->is_out_50hz ? 54 : 11;
+		cropcap->pixelaspect.denominator = itv->is_out_50hz ? 59 : 10;
 	} else {
 		return -EINVAL;
 	}
diff --git a/drivers/media/pci/ivtv/ivtv-routing.c b/drivers/media/pci/ivtv/ivtv-routing.c
index 8898c569a1c9..0c168f238904 100644
--- a/drivers/media/pci/ivtv/ivtv-routing.c
+++ b/drivers/media/pci/ivtv/ivtv-routing.c
@@ -24,10 +24,10 @@
 #include "ivtv-gpio.h"
 #include "ivtv-routing.h"
 
-#include <media/msp3400.h>
-#include <media/m52790.h>
-#include <media/upd64031a.h>
-#include <media/upd64083.h>
+#include <media/drv-intf/msp3400.h>
+#include <media/i2c/m52790.h>
+#include <media/i2c/upd64031a.h>
+#include <media/i2c/upd64083.h>
 
 /* Selects the audio input and output according to the current
    settings. */
diff --git a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
index 3fdbd81b5580..525ebfefeee8 100644
--- a/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
+++ b/drivers/media/pci/netup_unidvb/netup_unidvb_core.c
@@ -277,7 +277,6 @@ static irqreturn_t netup_unidvb_isr(int irq, void *dev_id)
 }
 
 static int netup_unidvb_queue_setup(struct vb2_queue *vq,
-				    const void *parg,
 				    unsigned int *nbuffers,
 				    unsigned int *nplanes,
 				    unsigned int sizes[],
@@ -388,7 +387,7 @@ static int netup_unidvb_dvb_init(struct netup_unidvb_dev *ndev,
 		vb2_dvb_alloc_frontend(
 			&ndev->frontends[num], 3) == NULL) {
 		dev_dbg(&ndev->pci_dev->dev,
-			"%s(): unable to to alllocate vb2_dvb_frontend\n",
+			"%s(): unable to allocate vb2_dvb_frontend\n",
 			__func__);
 		return -ENOMEM;
 	}
@@ -580,7 +579,7 @@ static void netup_unidvb_dma_worker(struct work_struct *work)
 			dev_dbg(&ndev->pci_dev->dev,
 				"%s(): buffer %p done, size %d\n",
 				__func__, buf, buf->size);
-			v4l2_get_timestamp(&buf->vb.timestamp);
+			buf->vb.vb2_buf.timestamp = ktime_get_ns();
 			vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
 			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 		}
diff --git a/drivers/media/pci/ngene/ngene-core.c b/drivers/media/pci/ngene/ngene-core.c
index 1b92d836a564..4e924e2d1638 100644
--- a/drivers/media/pci/ngene/ngene-core.c
+++ b/drivers/media/pci/ngene/ngene-core.c
@@ -1513,7 +1513,7 @@ static int init_channel(struct ngene_channel *chan)
 		set_transfer(&chan->dev->channel[2], 1);
 		dvb_register_device(adapter, &chan->ci_dev,
 				    &ngene_dvbdev_ci, (void *) chan,
-				    DVB_DEVICE_SEC);
+				    DVB_DEVICE_SEC, 0);
 		if (!chan->ci_dev)
 			goto err;
 	}
diff --git a/drivers/media/pci/saa7134/saa7134-alsa.c b/drivers/media/pci/saa7134/saa7134-alsa.c
index 1d2c310ce838..94f816244407 100644
--- a/drivers/media/pci/saa7134/saa7134-alsa.c
+++ b/drivers/media/pci/saa7134/saa7134-alsa.c
@@ -1211,6 +1211,8 @@ static int alsa_device_init(struct saa7134_dev *dev)
 
 static int alsa_device_exit(struct saa7134_dev *dev)
 {
+	if (!snd_saa7134_cards[dev->nr])
+		return 1;
 
 	snd_card_free(snd_saa7134_cards[dev->nr]);
 	snd_saa7134_cards[dev->nr] = NULL;
@@ -1260,7 +1262,8 @@ static void saa7134_alsa_exit(void)
 	int idx;
 
 	for (idx = 0; idx < SNDRV_CARDS; idx++) {
-		snd_card_free(snd_saa7134_cards[idx]);
+		if (snd_saa7134_cards[idx])
+			snd_card_free(snd_saa7134_cards[idx]);
 	}
 
 	saa7134_dmasound_init = NULL;
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index f720cea80e28..e227b02cc122 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -309,7 +309,7 @@ void saa7134_buffer_finish(struct saa7134_dev *dev,
 	core_dbg("buffer_finish %p\n", q->curr);
 
 	/* finish current buffer */
-	v4l2_get_timestamp(&q->curr->vb2.timestamp);
+	q->curr->vb2.vb2_buf.timestamp = ktime_get_ns();
 	q->curr->vb2.sequence = q->seq_nr++;
 	vb2_buffer_done(&q->curr->vb2.vb2_buf, state);
 	q->curr = NULL;
diff --git a/drivers/media/pci/saa7134/saa7134-ts.c b/drivers/media/pci/saa7134/saa7134-ts.c
index 7fb5ee7e20ac..0584a2adbe99 100644
--- a/drivers/media/pci/saa7134/saa7134-ts.c
+++ b/drivers/media/pci/saa7134/saa7134-ts.c
@@ -116,7 +116,7 @@ int saa7134_ts_buffer_prepare(struct vb2_buffer *vb2)
 }
 EXPORT_SYMBOL_GPL(saa7134_ts_buffer_prepare);
 
-int saa7134_ts_queue_setup(struct vb2_queue *q, const void *parg,
+int saa7134_ts_queue_setup(struct vb2_queue *q,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index 6271b0eb0265..e76da37c4a8a 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -138,7 +138,7 @@ static int buffer_prepare(struct vb2_buffer *vb2)
 				    saa7134_buffer_startpage(buf));
 }
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 518086c7aed5..a63c1366a64e 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -32,7 +32,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-event.h>
-#include <media/saa6588.h>
+#include <media/i2c/saa6588.h>
 
 /* ------------------------------------------------------------------ */
 
@@ -904,7 +904,7 @@ static int buffer_prepare(struct vb2_buffer *vb2)
 				    saa7134_buffer_startpage(buf));
 }
 
-static int queue_setup(struct vb2_queue *q, const void *parg,
+static int queue_setup(struct vb2_queue *q,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index 6b6d234f5cab..5938bc781999 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -42,7 +42,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/tuner.h>
 #include <media/rc-core.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include <media/videobuf2-dma-sg.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
@@ -820,7 +820,7 @@ void saa7134_video_fini(struct saa7134_dev *dev);
 
 int saa7134_ts_buffer_init(struct vb2_buffer *vb2);
 int saa7134_ts_buffer_prepare(struct vb2_buffer *vb2);
-int saa7134_ts_queue_setup(struct vb2_queue *q, const void *parg,
+int saa7134_ts_queue_setup(struct vb2_queue *q,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[]);
 int saa7134_ts_start_streaming(struct vb2_queue *vq, unsigned int count);
diff --git a/drivers/media/pci/saa7146/hexium_gemini.c b/drivers/media/pci/saa7146/hexium_gemini.c
index 03cbcd2095c6..c889ec9f8a5a 100644
--- a/drivers/media/pci/saa7146/hexium_gemini.c
+++ b/drivers/media/pci/saa7146/hexium_gemini.c
@@ -25,7 +25,7 @@
 
 #define DEBUG_VARIABLE debug
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <linux/module.h>
 
 static int debug;
diff --git a/drivers/media/pci/saa7146/hexium_orion.c b/drivers/media/pci/saa7146/hexium_orion.c
index 15f0d66ff78a..c306a92e8909 100644
--- a/drivers/media/pci/saa7146/hexium_orion.c
+++ b/drivers/media/pci/saa7146/hexium_orion.c
@@ -25,7 +25,7 @@
 
 #define DEBUG_VARIABLE debug
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <linux/module.h>
 
 static int debug;
diff --git a/drivers/media/pci/saa7146/mxb.c b/drivers/media/pci/saa7146/mxb.c
index 0ca1e07ae783..504d78807639 100644
--- a/drivers/media/pci/saa7146/mxb.c
+++ b/drivers/media/pci/saa7146/mxb.c
@@ -25,10 +25,10 @@
 
 #define DEBUG_VARIABLE debug
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <media/tuner.h>
 #include <media/v4l2-common.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <linux/module.h>
 
 #include "tea6415c.h"
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
index 4432fd69b7cb..67a14c41c227 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2-enc.c
@@ -531,8 +531,7 @@ static int solo_enc_fillbuf(struct solo_enc_dev *solo_enc,
 
 	if (!ret) {
 		vbuf->sequence = solo_enc->sequence++;
-		vbuf->timestamp.tv_sec = vop_sec(vh);
-		vbuf->timestamp.tv_usec = vop_usec(vh);
+		vb->timestamp = ktime_get_ns();
 
 		/* Check for motion flags */
 		if (solo_is_motion_on(solo_enc) && enc_buf->motion) {
@@ -663,7 +662,6 @@ static int solo_ring_thread(void *data)
 }
 
 static int solo_enc_queue_setup(struct vb2_queue *q,
-				const void *parg,
 				unsigned int *num_buffers,
 				unsigned int *num_planes, unsigned int sizes[],
 				void *alloc_ctxs[])
diff --git a/drivers/media/pci/solo6x10/solo6x10-v4l2.c b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
index f7ce493b1fee..721ff5320de7 100644
--- a/drivers/media/pci/solo6x10/solo6x10-v4l2.c
+++ b/drivers/media/pci/solo6x10/solo6x10-v4l2.c
@@ -225,7 +225,7 @@ finish_buf:
 		vb2_set_plane_payload(vb, 0,
 			solo_vlines(solo_dev) * solo_bytesperline(solo_dev));
 		vbuf->sequence = solo_dev->sequence++;
-		v4l2_get_timestamp(&vbuf->timestamp);
+		vb->timestamp = ktime_get_ns();
 	}
 
 	vb2_buffer_done(vb, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
@@ -313,7 +313,7 @@ static void solo_stop_thread(struct solo_dev *solo_dev)
 	solo_dev->kthread = NULL;
 }
 
-static int solo_queue_setup(struct vb2_queue *q, const void *parg,
+static int solo_queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index 6367b455a7e7..753411cbbc9a 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -265,7 +265,7 @@ static void vip_active_buf_next(struct sta2x11_vip *vip)
 
 
 /* Videobuf2 Operations */
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -817,7 +817,7 @@ static irqreturn_t vip_irq(int irq, struct sta2x11_vip *vip)
 		/* Disable acquisition */
 		reg_write(vip, DVP_CTL, reg_read(vip, DVP_CTL) & ~DVP_CTL_ENA);
 		/* Remove the active buffer from the list */
-		v4l2_get_timestamp(&vip->active->vb.timestamp);
+		vip->active->vb.vb2_buf.timestamp = ktime_get_ns();
 		vip->active->vb.sequence = vip->sequence++;
 		vb2_buffer_done(&vip->active->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	}
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index f89364951ebd..a69dc6a0752b 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -26,7 +26,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 
@@ -1358,7 +1358,7 @@ static int av7110_register(struct av7110 *av7110)
 
 #ifdef CONFIG_DVB_AV7110_OSD
 	dvb_register_device(&av7110->dvb_adapter, &av7110->osd_dev,
-			    &dvbdev_osd, av7110, DVB_DEVICE_OSD);
+			    &dvbdev_osd, av7110, DVB_DEVICE_OSD, 0);
 #endif
 
 	dvb_net_init(&av7110->dvb_adapter, &av7110->dvb_net, &dvbdemux->dmx);
@@ -1537,7 +1537,7 @@ static int get_firmware(struct av7110* av7110)
 			printk(KERN_ERR "dvb-ttpci: usually this should be in "
 			       "/usr/lib/hotplug/firmware or /lib/firmware\n");
 			printk(KERN_ERR "dvb-ttpci: and can be downloaded from"
-			       " http://www.linuxtv.org/download/dvb/firmware/\n");
+			       " https://linuxtv.org/download/dvb/firmware/\n");
 		} else
 			printk(KERN_ERR "dvb-ttpci: cannot request firmware"
 			       " (error %i)\n", ret);
@@ -2314,7 +2314,7 @@ static int frontend_init(struct av7110 *av7110)
 /* Budgetpatch note:
  * Original hardware design by Roberto Deza:
  * There is a DVB_Wiki at
- * http://www.linuxtv.org/
+ * https://linuxtv.org
  *
  * New software triggering design by Emard that works on
  * original Roberto Deza's hardware:
diff --git a/drivers/media/pci/ttpci/av7110.h b/drivers/media/pci/ttpci/av7110.h
index 3a55927edb95..3707ccd02732 100644
--- a/drivers/media/pci/ttpci/av7110.h
+++ b/drivers/media/pci/ttpci/av7110.h
@@ -32,7 +32,7 @@
 #include "stv0297.h"
 #include "l64781.h"
 
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 
 
 #define ANALOG_TUNER_VES1820 1
diff --git a/drivers/media/pci/ttpci/av7110_av.c b/drivers/media/pci/ttpci/av7110_av.c
index 9ed1ec7d3551..26c5696c193b 100644
--- a/drivers/media/pci/ttpci/av7110_av.c
+++ b/drivers/media/pci/ttpci/av7110_av.c
@@ -25,7 +25,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #include <linux/types.h>
@@ -280,9 +280,11 @@ int av7110_pes_play(void *dest, struct dvb_ringbuffer *buf, int dlen)
 }
 
 
-int av7110_set_volume(struct av7110 *av7110, int volleft, int volright)
+int av7110_set_volume(struct av7110 *av7110, unsigned int volleft,
+		      unsigned int volright)
 {
-	int err, vol, val, balance = 0;
+	unsigned int vol, val, balance = 0;
+	int err;
 
 	dprintk(2, "av7110:%p, \n", av7110);
 
@@ -1043,6 +1045,9 @@ static int play_iframe(struct av7110 *av7110, char __user *buf, unsigned int len
 
 	dprintk(2, "av7110:%p, \n", av7110);
 
+	if (len == 0)
+		return 0;
+
 	if (!(av7110->playing & RP_VIDEO)) {
 		if (av7110_av_start_play(av7110, RP_VIDEO) < 0)
 			return -EBUSY;
@@ -1589,10 +1594,10 @@ int av7110_av_register(struct av7110 *av7110)
 	memset(&av7110->video_size, 0, sizeof (video_size_t));
 
 	dvb_register_device(&av7110->dvb_adapter, &av7110->video_dev,
-			    &dvbdev_video, av7110, DVB_DEVICE_VIDEO);
+			    &dvbdev_video, av7110, DVB_DEVICE_VIDEO, 0);
 
 	dvb_register_device(&av7110->dvb_adapter, &av7110->audio_dev,
-			    &dvbdev_audio, av7110, DVB_DEVICE_AUDIO);
+			    &dvbdev_audio, av7110, DVB_DEVICE_AUDIO, 0);
 
 	return 0;
 }
diff --git a/drivers/media/pci/ttpci/av7110_av.h b/drivers/media/pci/ttpci/av7110_av.h
index 5f02ef85e47d..f52276f47709 100644
--- a/drivers/media/pci/ttpci/av7110_av.h
+++ b/drivers/media/pci/ttpci/av7110_av.h
@@ -10,7 +10,8 @@ extern int av7110_record_cb(struct dvb_filter_pes2ts *p2t, u8 *buf, size_t len);
 extern int av7110_pes_play(void *dest, struct dvb_ringbuffer *buf, int dlen);
 extern int av7110_write_to_decoder(struct dvb_demux_feed *feed, const u8 *buf, size_t len);
 
-extern int av7110_set_volume(struct av7110 *av7110, int volleft, int volright);
+extern int av7110_set_volume(struct av7110 *av7110, unsigned int volleft,
+			     unsigned int volright);
 extern int av7110_av_stop(struct av7110 *av7110, int av);
 extern int av7110_av_start_record(struct av7110 *av7110, int av,
 			  struct dvb_demux_feed *dvbdmxfeed);
diff --git a/drivers/media/pci/ttpci/av7110_ca.c b/drivers/media/pci/ttpci/av7110_ca.c
index a6079b90252a..96a130fb4595 100644
--- a/drivers/media/pci/ttpci/av7110_ca.c
+++ b/drivers/media/pci/ttpci/av7110_ca.c
@@ -25,7 +25,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #include <linux/kernel.h>
@@ -378,7 +378,7 @@ static struct dvb_device dvbdev_ca = {
 int av7110_ca_register(struct av7110 *av7110)
 {
 	return dvb_register_device(&av7110->dvb_adapter, &av7110->ca_dev,
-				   &dvbdev_ca, av7110, DVB_DEVICE_CA);
+				   &dvbdev_ca, av7110, DVB_DEVICE_CA, 0);
 }
 
 void av7110_ca_unregister(struct av7110 *av7110)
diff --git a/drivers/media/pci/ttpci/av7110_hw.c b/drivers/media/pci/ttpci/av7110_hw.c
index 300bd3c94738..0583d56ef5ef 100644
--- a/drivers/media/pci/ttpci/av7110_hw.c
+++ b/drivers/media/pci/ttpci/av7110_hw.c
@@ -22,7 +22,7 @@
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
- * the project's page is at http://www.linuxtv.org/
+ * the project's page is at https://linuxtv.org
  */
 
 /* for debugging ARM communication: */
diff --git a/drivers/media/pci/ttpci/av7110_v4l.c b/drivers/media/pci/ttpci/av7110_v4l.c
index 6c4076acb131..479aff02db81 100644
--- a/drivers/media/pci/ttpci/av7110_v4l.c
+++ b/drivers/media/pci/ttpci/av7110_v4l.c
@@ -22,7 +22,7 @@
  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
diff --git a/drivers/media/pci/ttpci/budget-av.c b/drivers/media/pci/ttpci/budget-av.c
index 3e469d4e0c87..6f0d0161970e 100644
--- a/drivers/media/pci/ttpci/budget-av.c
+++ b/drivers/media/pci/ttpci/budget-av.c
@@ -30,7 +30,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -46,7 +46,7 @@
 #include "tda1004x.h"
 #include "tua6100.h"
 #include "dvb-pll.h"
-#include <media/saa7146_vv.h>
+#include <media/drv-intf/saa7146_vv.h>
 #include <linux/module.h>
 #include <linux/errno.h>
 #include <linux/slab.h>
diff --git a/drivers/media/pci/ttpci/budget-ci.c b/drivers/media/pci/ttpci/budget-ci.c
index 1feeeff3681b..7b27af4d9658 100644
--- a/drivers/media/pci/ttpci/budget-ci.c
+++ b/drivers/media/pci/ttpci/budget-ci.c
@@ -26,7 +26,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/
+ * the project's page is at https://linuxtv.org
  */
 
 #include <linux/module.h>
diff --git a/drivers/media/pci/ttpci/budget-core.c b/drivers/media/pci/ttpci/budget-core.c
index e9674b40007c..6d42dcfd4825 100644
--- a/drivers/media/pci/ttpci/budget-core.c
+++ b/drivers/media/pci/ttpci/budget-core.c
@@ -31,7 +31,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 
diff --git a/drivers/media/pci/ttpci/budget-patch.c b/drivers/media/pci/ttpci/budget-patch.c
index b5b65962ce8f..591dbdfa2a13 100644
--- a/drivers/media/pci/ttpci/budget-patch.c
+++ b/drivers/media/pci/ttpci/budget-patch.c
@@ -27,7 +27,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #include "av7110.h"
diff --git a/drivers/media/pci/ttpci/budget.c b/drivers/media/pci/ttpci/budget.c
index 99972beca262..de54310a2660 100644
--- a/drivers/media/pci/ttpci/budget.c
+++ b/drivers/media/pci/ttpci/budget.c
@@ -31,7 +31,7 @@
  * Or, point your browser to http://www.gnu.org/copyleft/gpl.html
  *
  *
- * the project's page is at http://www.linuxtv.org/ 
+ * the project's page is at https://linuxtv.org
  */
 
 #include "budget.h"
diff --git a/drivers/media/pci/ttpci/budget.h b/drivers/media/pci/ttpci/budget.h
index 1ccbe1a49a4b..655eef5236ca 100644
--- a/drivers/media/pci/ttpci/budget.h
+++ b/drivers/media/pci/ttpci/budget.h
@@ -13,7 +13,7 @@
 #include <linux/module.h>
 #include <linux/mutex.h>
 
-#include <media/saa7146.h>
+#include <media/drv-intf/saa7146.h>
 
 extern int budget_debug;
 
diff --git a/drivers/media/pci/tw68/tw68-video.c b/drivers/media/pci/tw68/tw68-video.c
index 46642ef9151b..07116a87a57b 100644
--- a/drivers/media/pci/tw68/tw68-video.c
+++ b/drivers/media/pci/tw68/tw68-video.c
@@ -376,28 +376,28 @@ static int tw68_buffer_count(unsigned int size, unsigned int count)
 /* ------------------------------------------------------------- */
 /* vb2 queue operations                                          */
 
-static int tw68_queue_setup(struct vb2_queue *q, const void *parg,
+static int tw68_queue_setup(struct vb2_queue *q,
 			   unsigned int *num_buffers, unsigned int *num_planes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct tw68_dev *dev = vb2_get_drv_priv(q);
 	unsigned tot_bufs = q->num_buffers + *num_buffers;
+	unsigned size = (dev->fmt->depth * dev->width * dev->height) >> 3;
 
-	sizes[0] = (dev->fmt->depth * dev->width * dev->height) >> 3;
+	if (tot_bufs < 2)
+		tot_bufs = 2;
+	tot_bufs = tw68_buffer_count(size, tot_bufs);
+	*num_buffers = tot_bufs - q->num_buffers;
 	alloc_ctxs[0] = dev->alloc_ctx;
 	/*
-	 * We allow create_bufs, but only if the sizeimage is the same as the
+	 * We allow create_bufs, but only if the sizeimage is >= as the
 	 * current sizeimage. The tw68_buffer_count calculation becomes quite
 	 * difficult otherwise.
 	 */
-	if (fmt && fmt->fmt.pix.sizeimage < sizes[0])
-		return -EINVAL;
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*num_planes = 1;
-	if (tot_bufs < 2)
-		tot_bufs = 2;
-	tot_bufs = tw68_buffer_count(sizes[0], tot_bufs);
-	*num_buffers = tot_bufs - q->num_buffers;
+	sizes[0] = size;
 
 	return 0;
 }
@@ -1016,7 +1016,7 @@ void tw68_irq_video_done(struct tw68_dev *dev, unsigned long status)
 		buf = list_entry(dev->active.next, struct tw68_buf, list);
 		list_del(&buf->list);
 		spin_unlock(&dev->slock);
-		v4l2_get_timestamp(&buf->vb.timestamp);
+		buf->vb.vb2_buf.timestamp = ktime_get_ns();
 		buf->vb.field = dev->field;
 		buf->vb.sequence = dev->seqnr++;
 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/media/pci/zoran/zoran_card.c b/drivers/media/pci/zoran/zoran_card.c
index 1136d92af642..9d2697f5b455 100644
--- a/drivers/media/pci/zoran/zoran_card.c
+++ b/drivers/media/pci/zoran/zoran_card.c
@@ -50,7 +50,7 @@
 #include <linux/mutex.h>
 #include <linux/io.h>
 #include <media/v4l2-common.h>
-#include <media/bt819.h>
+#include <media/i2c/bt819.h>
 
 #include "videocodec.h"
 #include "zoran.h"
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index ccbc9742cb7a..8b89ebe16d94 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -38,7 +38,7 @@ config VIDEO_SH_VOU
 	depends on MEDIA_CAMERA_SUPPORT
 	depends on VIDEO_DEV && I2C && HAS_DMA
 	depends on ARCH_SHMOBILE || COMPILE_TEST
-	select VIDEOBUF_DMA_CONTIG
+	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Support for the Video Output Unit (VOU) on SuperH SoCs.
 
@@ -215,8 +215,8 @@ config VIDEO_SAMSUNG_EXYNOS_GSC
 config VIDEO_STI_BDISP
 	tristate "STMicroelectronics BDISP 2D blitter driver"
 	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on HAS_DMA
 	depends on ARCH_STI || COMPILE_TEST
-	depends on HAVE_DMA_ATTRS
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
diff --git a/drivers/media/platform/am437x/am437x-vpfe.c b/drivers/media/platform/am437x/am437x-vpfe.c
index f0480d687f17..de32e3a3d4d1 100644
--- a/drivers/media/platform/am437x/am437x-vpfe.c
+++ b/drivers/media/platform/am437x/am437x-vpfe.c
@@ -1281,7 +1281,7 @@ static inline void vpfe_schedule_bottom_field(struct vpfe_device *vpfe)
  */
 static inline void vpfe_process_buffer_complete(struct vpfe_device *vpfe)
 {
-	v4l2_get_timestamp(&vpfe->cur_frm->vb.timestamp);
+	vpfe->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 	vpfe->cur_frm->vb.field = vpfe->fmt.fmt.pix.field;
 	vpfe->cur_frm->vb.sequence = vpfe->sequence++;
 	vb2_buffer_done(&vpfe->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -1898,7 +1898,6 @@ static void vpfe_calculate_offsets(struct vpfe_device *vpfe)
 /*
  * vpfe_queue_setup - Callback function for buffer setup.
  * @vq: vb2_queue ptr
- * @fmt: v4l2 format
  * @nbuffers: ptr to number of buffers requested by application
  * @nplanes:: contains number of distinct video planes needed to hold a frame
  * @sizes[]: contains the size (in bytes) of each plane.
@@ -1908,22 +1907,24 @@ static void vpfe_calculate_offsets(struct vpfe_device *vpfe)
  * the buffer count and buffer size
  */
 static int vpfe_queue_setup(struct vb2_queue *vq,
-			    const void *parg,
 			    unsigned int *nbuffers, unsigned int *nplanes,
 			    unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct vpfe_device *vpfe = vb2_get_drv_priv(vq);
-
-	if (fmt && fmt->fmt.pix.sizeimage < vpfe->fmt.fmt.pix.sizeimage)
-		return -EINVAL;
+	unsigned size = vpfe->fmt.fmt.pix.sizeimage;
 
 	if (vq->num_buffers + *nbuffers < 3)
 		*nbuffers = 3 - vq->num_buffers;
+	alloc_ctxs[0] = vpfe->alloc_ctx;
+
+	if (*nplanes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : vpfe->fmt.fmt.pix.sizeimage;
-	alloc_ctxs[0] = vpfe->alloc_ctx;
+	sizes[0] = size;
 
 	vpfe_dbg(1, vpfe,
 		"nbuffers=%d, size=%u\n", *nbuffers, sizes[0]);
diff --git a/drivers/media/platform/blackfin/bfin_capture.c b/drivers/media/platform/blackfin/bfin_capture.c
index 7764b9c482ef..d0092dae7a57 100644
--- a/drivers/media/platform/blackfin/bfin_capture.c
+++ b/drivers/media/platform/blackfin/bfin_capture.c
@@ -202,22 +202,20 @@ static void bcap_free_sensor_formats(struct bcap_device *bcap_dev)
 }
 
 static int bcap_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct bcap_device *bcap_dev = vb2_get_drv_priv(vq);
 
-	if (fmt && fmt->fmt.pix.sizeimage < bcap_dev->fmt.sizeimage)
-		return -EINVAL;
-
 	if (vq->num_buffers + *nbuffers < 2)
 		*nbuffers = 2;
+	alloc_ctxs[0] = bcap_dev->alloc_ctx;
+
+	if (*nplanes)
+		return sizes[0] < bcap_dev->fmt.sizeimage ? -EINVAL : 0;
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : bcap_dev->fmt.sizeimage;
-	alloc_ctxs[0] = bcap_dev->alloc_ctx;
+	sizes[0] = bcap_dev->fmt.sizeimage;
 
 	return 0;
 }
@@ -406,7 +404,7 @@ static irqreturn_t bcap_isr(int irq, void *dev_id)
 	spin_lock(&bcap_dev->lock);
 
 	if (!list_empty(&bcap_dev->dma_queue)) {
-		v4l2_get_timestamp(&vbuf->timestamp);
+		vb->timestamp = ktime_get_ns();
 		if (ppi->err) {
 			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 			ppi->err = false;
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
index 654e964f84a2..7d28899f89ce 100644
--- a/drivers/media/platform/coda/coda-bit.c
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -246,7 +246,7 @@ void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
 
 		/* Drop frames that do not start/end with a SOI/EOI markers */
 		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_JPEG &&
-		    !coda_jpeg_check_buffer(ctx, src_buf)) {
+		    !coda_jpeg_check_buffer(ctx, &src_buf->vb2_buf)) {
 			v4l2_err(&ctx->dev->v4l2_dev,
 				 "dropping invalid JPEG frame %d\n",
 				 ctx->qsequence);
@@ -279,7 +279,7 @@ void coda_fill_bitstream(struct coda_ctx *ctx, bool streaming)
 			if (meta) {
 				meta->sequence = src_buf->sequence;
 				meta->timecode = src_buf->timecode;
-				meta->timestamp = src_buf->timestamp;
+				meta->timestamp = src_buf->vb2_buf.timestamp;
 				meta->start = start;
 				meta->end = ctx->bitstream_fifo.kfifo.in &
 					    ctx->bitstream_fifo.kfifo.mask;
@@ -1364,7 +1364,7 @@ static void coda_finish_encode(struct coda_ctx *ctx)
 		dst_buf->flags &= ~V4L2_BUF_FLAG_KEYFRAME;
 	}
 
-	dst_buf->timestamp = src_buf->timestamp;
+	dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 	dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst_buf->flags |=
 		src_buf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
@@ -2040,7 +2040,7 @@ static void coda_finish_decode(struct coda_ctx *ctx)
 		dst_buf->flags |= ctx->frame_types[ctx->display_idx];
 		meta = &ctx->frame_metas[ctx->display_idx];
 		dst_buf->timecode = meta->timecode;
-		dst_buf->timestamp = meta->timestamp;
+		dst_buf->vb2_buf.timestamp = meta->timestamp;
 
 		trace_coda_dec_rot_done(ctx, dst_buf, meta);
 
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
index 15516a6e3a39..2d782ce94a67 100644
--- a/drivers/media/platform/coda/coda-common.c
+++ b/drivers/media/platform/coda/coda-common.c
@@ -28,7 +28,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/of.h>
-#include <linux/platform_data/coda.h>
+#include <linux/platform_data/media/coda.h>
 #include <linux/reset.h>
 
 #include <media/v4l2-ctrls.h>
@@ -131,6 +131,7 @@ static const struct coda_codec coda7_codecs[] = {
 	CODA_CODEC(CODA7_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1280, 720),
 	CODA_CODEC(CODA7_MODE_ENCODE_MJPG, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_JPEG,   8192, 8192),
 	CODA_CODEC(CODA7_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA7_MODE_DECODE_MP2,  V4L2_PIX_FMT_MPEG2,  V4L2_PIX_FMT_YUV420, 1920, 1088),
 	CODA_CODEC(CODA7_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
 	CODA_CODEC(CODA7_MODE_DECODE_MJPG, V4L2_PIX_FMT_JPEG,   V4L2_PIX_FMT_YUV420, 8192, 8192),
 };
@@ -139,6 +140,7 @@ static const struct coda_codec coda9_codecs[] = {
 	CODA_CODEC(CODA9_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1920, 1088),
 	CODA_CODEC(CODA9_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1920, 1088),
 	CODA_CODEC(CODA9_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA9_MODE_DECODE_MP2,  V4L2_PIX_FMT_MPEG2,  V4L2_PIX_FMT_YUV420, 1920, 1088),
 	CODA_CODEC(CODA9_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
 };
 
@@ -187,6 +189,7 @@ static const struct coda_video_device coda_bit_decoder = {
 	.ops = &coda_bit_decode_ops,
 	.src_formats = {
 		V4L2_PIX_FMT_H264,
+		V4L2_PIX_FMT_MPEG2,
 		V4L2_PIX_FMT_MPEG4,
 	},
 	.dst_formats = {
@@ -293,7 +296,8 @@ static void coda_get_max_dimensions(struct coda_dev *dev,
 		*max_h = h;
 }
 
-const struct coda_video_device *to_coda_video_device(struct video_device *vdev)
+static const struct coda_video_device *to_coda_video_device(struct video_device
+							    *vdev)
 {
 	struct coda_dev *dev = video_get_drvdata(vdev);
 	unsigned int i = vdev - dev->vfd;
@@ -469,6 +473,7 @@ static int coda_try_fmt(struct coda_ctx *ctx, const struct coda_codec *codec,
 		/* fallthrough */
 	case V4L2_PIX_FMT_H264:
 	case V4L2_PIX_FMT_MPEG4:
+	case V4L2_PIX_FMT_MPEG2:
 		f->fmt.pix.bytesperline = 0;
 		f->fmt.pix.sizeimage = coda_estimate_sizeimage(ctx,
 							f->fmt.pix.sizeimage,
@@ -920,6 +925,7 @@ static const struct v4l2_ioctl_ops coda_ioctl_ops = {
 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
 	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
+	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
 
 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
 	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
@@ -1131,7 +1137,7 @@ static void set_default_params(struct coda_ctx *ctx)
 /*
  * Queue operations
  */
-static int coda_queue_setup(struct vb2_queue *vq, const void *parg,
+static int coda_queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -1250,6 +1256,9 @@ static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
 	struct vb2_v4l2_buffer *buf;
 	int ret = 0;
 
+	if (count < 1)
+		return -EINVAL;
+
 	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
 		if (ctx->inst_type == CODA_INST_DECODER && ctx->use_bit) {
@@ -1262,20 +1271,10 @@ static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
 				ret = -EINVAL;
 				goto err;
 			}
-		} else {
-			if (count < 1) {
-				ret = -EINVAL;
-				goto err;
-			}
 		}
 
 		ctx->streamon_out = 1;
 	} else {
-		if (count < 1) {
-			ret = -EINVAL;
-			goto err;
-		}
-
 		ctx->streamon_cap = 1;
 	}
 
diff --git a/drivers/media/platform/coda/coda-jpeg.c b/drivers/media/platform/coda/coda-jpeg.c
index 96cd42a0baaf..9f899a6cefed 100644
--- a/drivers/media/platform/coda/coda-jpeg.c
+++ b/drivers/media/platform/coda/coda-jpeg.c
@@ -178,14 +178,28 @@ int coda_jpeg_write_tables(struct coda_ctx *ctx)
 	return 0;
 }
 
-bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_v4l2_buffer *vb)
+bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_buffer *vb)
 {
-	void *vaddr = vb2_plane_vaddr(&vb->vb2_buf, 0);
-	u16 soi = be16_to_cpup((__be16 *)vaddr);
-	u16 eoi = be16_to_cpup((__be16 *)(vaddr +
-			  vb2_get_plane_payload(&vb->vb2_buf, 0) - 2));
+	void *vaddr = vb2_plane_vaddr(vb, 0);
+	u16 soi, eoi;
+	int len, i;
+
+	soi = be16_to_cpup((__be16 *)vaddr);
+	if (soi != SOI_MARKER)
+		return false;
+
+	len = vb2_get_plane_payload(vb, 0);
+	vaddr += len - 2;
+	for (i = 0; i < 32; i++) {
+		eoi = be16_to_cpup((__be16 *)(vaddr - i));
+		if (eoi == EOI_MARKER) {
+			if (i > 0)
+				vb2_set_plane_payload(vb, 0, len - i);
+			return true;
+		}
+	}
 
-	return soi == SOI_MARKER && eoi == EOI_MARKER;
+	return false;
 }
 
 /*
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
index 96532b06bd9e..d08e9843e9f2 100644
--- a/drivers/media/platform/coda/coda.h
+++ b/drivers/media/platform/coda/coda.h
@@ -138,7 +138,7 @@ struct coda_buffer_meta {
 	struct list_head	list;
 	u32			sequence;
 	struct v4l2_timecode	timecode;
-	struct timeval		timestamp;
+	u64			timestamp;
 	u32			start;
 	u32			end;
 };
@@ -289,7 +289,7 @@ void coda_m2m_buf_done(struct coda_ctx *ctx, struct vb2_v4l2_buffer *buf,
 
 int coda_h264_padding(int size, char *p);
 
-bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_v4l2_buffer *vb);
+bool coda_jpeg_check_buffer(struct coda_ctx *ctx, struct vb2_buffer *vb);
 int coda_jpeg_write_tables(struct coda_ctx *ctx);
 void coda_set_jpeg_compression_quality(struct coda_ctx *ctx, int quality);
 
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
index 469e9d28cec0..554e710de487 100644
--- a/drivers/media/platform/davinci/Kconfig
+++ b/drivers/media/platform/davinci/Kconfig
@@ -3,6 +3,7 @@ config VIDEO_DAVINCI_VPIF_DISPLAY
 	depends on VIDEO_V4L2
 	depends on ARCH_DAVINCI || COMPILE_TEST
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	select VIDEO_ADV7343 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEO_THS7303 if MEDIA_SUBDRV_AUTOSELECT
@@ -19,6 +20,7 @@ config VIDEO_DAVINCI_VPIF_CAPTURE
 	depends on VIDEO_V4L2
 	depends on ARCH_DAVINCI || COMPILE_TEST
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Enables Davinci VPIF module used for capture devices.
@@ -33,6 +35,7 @@ config VIDEO_DM6446_CCDC
 	depends on VIDEO_V4L2
 	depends on ARCH_DAVINCI || COMPILE_TEST
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
@@ -49,6 +52,7 @@ config VIDEO_DM355_CCDC
 	depends on VIDEO_V4L2
 	depends on ARCH_DAVINCI || COMPILE_TEST
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DM355 CCD hw module. DM355 CCDC hw interfaces
@@ -64,6 +68,7 @@ config VIDEO_DM365_ISIF
 	tristate "TI DM365 ISIF video capture driver"
 	depends on VIDEO_V4L2 && ARCH_DAVINCI
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables ISIF hw module. This is the hardware module for
@@ -77,6 +82,7 @@ config VIDEO_DAVINCI_VPBE_DISPLAY
 	tristate "TI DaVinci VPBE V4L2-Display driver"
 	depends on VIDEO_V4L2 && ARCH_DAVINCI
 	depends on HAS_DMA
+	depends on I2C
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	    Enables Davinci VPBE module used for display devices.
diff --git a/drivers/media/platform/davinci/vpbe_display.c b/drivers/media/platform/davinci/vpbe_display.c
index 6d91422c4e4c..0abcdfe97a6c 100644
--- a/drivers/media/platform/davinci/vpbe_display.c
+++ b/drivers/media/platform/davinci/vpbe_display.c
@@ -74,7 +74,7 @@ static void vpbe_isr_even_field(struct vpbe_display *disp_obj,
 	if (layer->cur_frm == layer->next_frm)
 		return;
 
-	v4l2_get_timestamp(&layer->cur_frm->vb.timestamp);
+	layer->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&layer->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	/* Make cur_frm pointing to next_frm */
 	layer->cur_frm = layer->next_frm;
@@ -228,28 +228,27 @@ static int vpbe_buffer_prepare(struct vb2_buffer *vb)
  * This function allocates memory for the buffers
  */
 static int
-vpbe_buffer_queue_setup(struct vb2_queue *vq, const void *parg,
+vpbe_buffer_queue_setup(struct vb2_queue *vq,
 			unsigned int *nbuffers, unsigned int *nplanes,
 			unsigned int sizes[], void *alloc_ctxs[])
 
 {
-	const struct v4l2_format *fmt = parg;
 	/* Get the file handle object and layer object */
 	struct vpbe_layer *layer = vb2_get_drv_priv(vq);
 	struct vpbe_device *vpbe_dev = layer->disp_dev->vpbe_dev;
 
 	v4l2_dbg(1, debug, &vpbe_dev->v4l2_dev, "vpbe_buffer_setup\n");
 
-	if (fmt && fmt->fmt.pix.sizeimage < layer->pix_fmt.sizeimage)
-		return -EINVAL;
-
 	/* Store number of buffers allocated in numbuffer member */
 	if (vq->num_buffers + *nbuffers < VPBE_DEFAULT_NUM_BUFS)
 		*nbuffers = VPBE_DEFAULT_NUM_BUFS - vq->num_buffers;
+	alloc_ctxs[0] = layer->alloc_ctx;
+
+	if (*nplanes)
+		return sizes[0] < layer->pix_fmt.sizeimage ? -EINVAL : 0;
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : layer->pix_fmt.sizeimage;
-	alloc_ctxs[0] = layer->alloc_ctx;
+	sizes[0] = layer->pix_fmt.sizeimage;
 
 	return 0;
 }
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index c1e573b7cc6f..08f7028c7560 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -104,7 +104,6 @@ static int vpif_buffer_prepare(struct vb2_buffer *vb)
 /**
  * vpif_buffer_queue_setup : Callback function for buffer setup.
  * @vq: vb2_queue ptr
- * @fmt: v4l2 format
  * @nbuffers: ptr to number of buffers requested by application
  * @nplanes:: contains number of distinct video planes needed to hold a frame
  * @sizes[]: contains the size (in bytes) of each plane.
@@ -114,26 +113,26 @@ static int vpif_buffer_prepare(struct vb2_buffer *vb)
  * the buffer count and buffer size
  */
 static int vpif_buffer_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct channel_obj *ch = vb2_get_drv_priv(vq);
-	struct common_obj *common;
-
-	common = &ch->common[VPIF_VIDEO_INDEX];
+	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	unsigned size = common->fmt.fmt.pix.sizeimage;
 
 	vpif_dbg(2, debug, "vpif_buffer_setup\n");
 
-	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
-		return -EINVAL;
+	if (*nplanes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
 
 	if (vq->num_buffers + *nbuffers < 3)
 		*nbuffers = 3 - vq->num_buffers;
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
+	sizes[0] = size;
 	alloc_ctxs[0] = common->alloc_ctx;
 
 	/* Calculate the offset for Y and C data in the buffer */
@@ -331,7 +330,7 @@ static struct vb2_ops video_qops = {
  */
 static void vpif_process_buffer_complete(struct common_obj *common)
 {
-	v4l2_get_timestamp(&common->cur_frm->vb.timestamp);
+	common->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&common->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	/* Make curFrm pointing to nextFrm */
 	common->cur_frm = common->next_frm;
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index fd2780306c17..f40755cf1bf2 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -99,7 +99,6 @@ static int vpif_buffer_prepare(struct vb2_buffer *vb)
 /**
  * vpif_buffer_queue_setup : Callback function for buffer setup.
  * @vq: vb2_queue ptr
- * @fmt: v4l2 format
  * @nbuffers: ptr to number of buffers requested by application
  * @nplanes:: contains number of distinct video planes needed to hold a frame
  * @sizes[]: contains the size (in bytes) of each plane.
@@ -109,22 +108,24 @@ static int vpif_buffer_prepare(struct vb2_buffer *vb)
  * the buffer count and buffer size
  */
 static int vpif_buffer_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct channel_obj *ch = vb2_get_drv_priv(vq);
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
+	unsigned size = common->fmt.fmt.pix.sizeimage;
 
-	if (fmt && fmt->fmt.pix.sizeimage < common->fmt.fmt.pix.sizeimage)
-		return -EINVAL;
+	if (*nplanes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
 
 	if (vq->num_buffers + *nbuffers < 3)
 		*nbuffers = 3 - vq->num_buffers;
 
 	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : common->fmt.fmt.pix.sizeimage;
+	sizes[0] = size;
 	alloc_ctxs[0] = common->alloc_ctx;
 
 	/* Calculate the offset for Y and C data  in the buffer */
@@ -330,7 +331,7 @@ static void process_interlaced_mode(int fid, struct common_obj *common)
 		/* one frame is displayed If next frame is
 		 *  available, release cur_frm and move on */
 		/* Copy frame display time */
-		v4l2_get_timestamp(&common->cur_frm->vb.timestamp);
+		common->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 		/* Change status of the cur_frm */
 		vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
 					VB2_BUF_STATE_DONE);
@@ -386,8 +387,8 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 			if (!channel_first_int[i][channel_id]) {
 				/* Mark status of the cur_frm to
 				 * done and unlock semaphore on it */
-				v4l2_get_timestamp(
-					&common->cur_frm->vb.timestamp);
+				common->cur_frm->vb.vb2_buf.timestamp =
+						ktime_get_ns();
 				vb2_buffer_done(&common->cur_frm->vb.vb2_buf,
 						VB2_BUF_STATE_DONE);
 				/* Make cur_frm pointing to next_frm */
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index d82e717acba7..93782f15b825 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -86,7 +86,7 @@ void gsc_m2m_job_finish(struct gsc_ctx *ctx, int vb_state)
 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 
 	if (src_vb && dst_vb) {
-		dst_vb->timestamp = src_vb->timestamp;
+		dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 		dst_vb->timecode = src_vb->timecode;
 		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 		dst_vb->flags |=
@@ -125,7 +125,7 @@ static int gsc_get_bufs(struct gsc_ctx *ctx)
 	if (ret)
 		return ret;
 
-	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 
 	return 0;
 }
@@ -212,7 +212,6 @@ put_device:
 }
 
 static int gsc_m2m_queue_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *num_buffers, unsigned int *num_planes,
 			unsigned int sizes[], void *allocators[])
 {
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index 40423c6c5324..57d42c6172c5 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -1,6 +1,6 @@
 
 config VIDEO_SAMSUNG_EXYNOS4_IS
-	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
+	tristate "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
 	depends on ARCH_S5PV210 || ARCH_EXYNOS || COMPILE_TEST
 	depends on OF && COMMON_CLK
diff --git a/drivers/media/platform/exynos4-is/common.c b/drivers/media/platform/exynos4-is/common.c
index 0eb34ecb8ee4..b90f5bb15517 100644
--- a/drivers/media/platform/exynos4-is/common.c
+++ b/drivers/media/platform/exynos4-is/common.c
@@ -10,7 +10,7 @@
  */
 
 #include <linux/module.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 #include "common.h"
 
 /* Called with the media graph mutex held or entity->stream_count > 0. */
@@ -22,8 +22,7 @@ struct v4l2_subdev *fimc_find_remote_sensor(struct media_entity *entity)
 	while (pad->flags & MEDIA_PAD_FL_SINK) {
 		/* source pad */
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		sd = media_entity_to_v4l2_subdev(pad->entity);
diff --git a/drivers/media/platform/exynos4-is/fimc-capture.c b/drivers/media/platform/exynos4-is/fimc-capture.c
index 99e57320e6f7..bf47d3b9cbe7 100644
--- a/drivers/media/platform/exynos4-is/fimc-capture.c
+++ b/drivers/media/platform/exynos4-is/fimc-capture.c
@@ -193,7 +193,7 @@ void fimc_capture_irq_handler(struct fimc_dev *fimc, int deq_buf)
 	    test_bit(ST_CAPT_RUN, &fimc->state) && deq_buf) {
 		v_buf = fimc_active_queue_pop(cap);
 
-		v4l2_get_timestamp(&v_buf->vb.timestamp);
+		v_buf->vb.vb2_buf.timestamp = ktime_get_ns();
 		v_buf->vb.sequence = cap->frame_count++;
 
 		vb2_buffer_done(&v_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -338,37 +338,36 @@ int fimc_capture_resume(struct fimc_dev *fimc)
 
 }
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *num_buffers, unsigned int *num_planes,
 		       unsigned int sizes[], void *allocators[])
 {
-	const struct v4l2_format *pfmt = parg;
-	const struct v4l2_pix_format_mplane *pixm = NULL;
 	struct fimc_ctx *ctx = vq->drv_priv;
 	struct fimc_frame *frame = &ctx->d_frame;
 	struct fimc_fmt *fmt = frame->fmt;
-	unsigned long wh;
+	unsigned long wh = frame->f_width * frame->f_height;
 	int i;
 
-	if (pfmt) {
-		pixm = &pfmt->fmt.pix_mp;
-		fmt = fimc_find_format(&pixm->pixelformat, NULL,
-				       FMT_FLAGS_CAM | FMT_FLAGS_M2M, -1);
-		wh = pixm->width * pixm->height;
-	} else {
-		wh = frame->f_width * frame->f_height;
-	}
-
 	if (fmt == NULL)
 		return -EINVAL;
 
+	if (*num_planes) {
+		if (*num_planes != fmt->memplanes)
+			return -EINVAL;
+		for (i = 0; i < *num_planes; i++) {
+			if (sizes[i] < (wh * fmt->depth[i]) / 8)
+				return -EINVAL;
+			allocators[i] = ctx->fimc_dev->alloc_ctx;
+		}
+		return 0;
+	}
+
 	*num_planes = fmt->memplanes;
 
 	for (i = 0; i < fmt->memplanes; i++) {
 		unsigned int size = (wh * fmt->depth[i]) / 8;
-		if (pixm)
-			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
-		else if (fimc_fmt_is_user_defined(fmt->color))
+
+		if (fimc_fmt_is_user_defined(fmt->color))
 			sizes[i] = frame->payload[i];
 		else
 			sizes[i] = max_t(u32, size, frame->payload[i]);
@@ -1137,8 +1136,7 @@ static int fimc_pipeline_validate(struct fimc_dev *fimc)
 			}
 		}
 
-		if (src_pad == NULL ||
-		    media_entity_type(src_pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!src_pad || !is_media_entity_v4l2_subdev(src_pad->entity))
 			break;
 
 		/* Don't call FIMC subdev operation to avoid nested locking */
@@ -1393,7 +1391,7 @@ static int fimc_link_setup(struct media_entity *entity,
 	struct fimc_vid_cap *vc = &fimc->vid_cap;
 	struct v4l2_subdev *sensor;
 
-	if (media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!is_media_entity_v4l2_subdev(remote->entity))
 		return -EINVAL;
 
 	if (WARN_ON(fimc == NULL))
@@ -1801,7 +1799,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;
-	ret = media_entity_init(&vfd->entity, 1, &vid_cap->vd_pad, 0);
+	ret = media_entity_pads_init(&vfd->entity, 1, &vid_cap->vd_pad);
 	if (ret)
 		goto err_free_ctx;
 
@@ -1893,8 +1891,8 @@ int fimc_initialize_capture_subdev(struct fimc_dev *fimc)
 	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_CAM].flags = MEDIA_PAD_FL_SINK;
 	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SINK_FIFO].flags = MEDIA_PAD_FL_SINK;
 	fimc->vid_cap.sd_pads[FIMC_SD_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, FIMC_SD_PADS_NUM,
-				fimc->vid_cap.sd_pads, 0);
+	ret = media_entity_pads_init(&sd->entity, FIMC_SD_PADS_NUM,
+				fimc->vid_cap.sd_pads);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-core.h b/drivers/media/platform/exynos4-is/fimc-core.h
index d336fa2916df..6b7435453d2a 100644
--- a/drivers/media/platform/exynos4-is/fimc-core.h
+++ b/drivers/media/platform/exynos4-is/fimc-core.h
@@ -27,7 +27,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-mem2mem.h>
 #include <media/v4l2-mediabus.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #define dbg(fmt, args...) \
 	pr_debug("%s:%d: " fmt "\n", __func__, __LINE__, ##args)
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 49658ca39e51..979c388ebf60 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -631,6 +631,12 @@ static int fimc_is_hw_open_sensor(struct fimc_is *is,
 
 	fimc_is_mem_barrier();
 
+	/*
+	 * Some user space use cases hang up here without this
+	 * empirically chosen delay.
+	 */
+	udelay(100);
+
 	mcuctl_write(HIC_OPEN_SENSOR, is, MCUCTL_REG_ISSR(0));
 	mcuctl_write(is->sensor_index, is, MCUCTL_REG_ISSR(1));
 	mcuctl_write(sensor->drvdata->id, is, MCUCTL_REG_ISSR(2));
diff --git a/drivers/media/platform/exynos4-is/fimc-isp-video.c b/drivers/media/platform/exynos4-is/fimc-isp-video.c
index 6e6648446f00..c0816728cbfe 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp-video.c
+++ b/drivers/media/platform/exynos4-is/fimc-isp-video.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-v4l2.h>
 #include <media/videobuf2-dma-contig.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "common.h"
 #include "media-dev.h"
@@ -39,39 +39,36 @@
 #include "fimc-is-param.h"
 
 static int isp_video_capture_queue_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *num_buffers, unsigned int *num_planes,
 			unsigned int sizes[], void *allocators[])
 {
-	const struct v4l2_format *pfmt = parg;
 	struct fimc_isp *isp = vb2_get_drv_priv(vq);
 	struct v4l2_pix_format_mplane *vid_fmt = &isp->video_capture.pixfmt;
-	const struct v4l2_pix_format_mplane *pixm = NULL;
-	const struct fimc_fmt *fmt;
+	const struct fimc_fmt *fmt = isp->video_capture.format;
 	unsigned int wh, i;
 
-	if (pfmt) {
-		pixm = &pfmt->fmt.pix_mp;
-		fmt = fimc_isp_find_format(&pixm->pixelformat, NULL, -1);
-		wh = pixm->width * pixm->height;
-	} else {
-		fmt = isp->video_capture.format;
-		wh = vid_fmt->width * vid_fmt->height;
-	}
+	wh = vid_fmt->width * vid_fmt->height;
 
 	if (fmt == NULL)
 		return -EINVAL;
 
 	*num_buffers = clamp_t(u32, *num_buffers, FIMC_ISP_REQ_BUFS_MIN,
 						FIMC_ISP_REQ_BUFS_MAX);
+	if (*num_planes) {
+		if (*num_planes != fmt->memplanes)
+			return -EINVAL;
+		for (i = 0; i < *num_planes; i++) {
+			if (sizes[i] < (wh * fmt->depth[i]) / 8)
+				return -EINVAL;
+			allocators[i] = isp->alloc_ctx;
+		}
+		return 0;
+	}
+
 	*num_planes = fmt->memplanes;
 
 	for (i = 0; i < fmt->memplanes; i++) {
-		unsigned int size = (wh * fmt->depth[i]) / 8;
-		if (pixm)
-			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
-		else
-			sizes[i] = size;
+		sizes[i] = (wh * fmt->depth[i]) / 8;
 		allocators[i] = isp->alloc_ctx;
 	}
 
@@ -221,8 +218,8 @@ static void isp_video_capture_buffer_queue(struct vb2_buffer *vb)
 							ivb->dma_addr[i];
 
 			isp_dbg(2, &video->ve.vdev,
-				"dma_buf %pad (%d/%d/%d) addr: %pad\n",
-				&buf_index, ivb->index, i, vb->index,
+				"dma_buf %d (%d/%d/%d) addr: %pad\n",
+				buf_index, ivb->index, i, vb->index,
 				&ivb->dma_addr[i]);
 		}
 
@@ -254,7 +251,7 @@ void fimc_isp_video_irq_handler(struct fimc_is *is)
 	buf_index = (is->i2h_cmd.args[1] - 1) % video->buf_count;
 	vbuf = &video->buffers[buf_index]->vb;
 
-	v4l2_get_timestamp(&vbuf->timestamp);
+	vbuf->vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
 
 	video->buf_mask &= ~BIT(buf_index);
@@ -290,7 +287,7 @@ static int isp_video_open(struct file *file)
 		goto rel_fh;
 
 	if (v4l2_fh_is_singular_file(file)) {
-		mutex_lock(&me->parent->graph_mutex);
+		mutex_lock(&me->graph_obj.mdev->graph_mutex);
 
 		ret = fimc_pipeline_call(ve, open, me, true);
 
@@ -298,7 +295,7 @@ static int isp_video_open(struct file *file)
 		if (ret == 0)
 			me->use_count++;
 
-		mutex_unlock(&me->parent->graph_mutex);
+		mutex_unlock(&me->graph_obj.mdev->graph_mutex);
 	}
 	if (!ret)
 		goto unlock;
@@ -314,7 +311,7 @@ static int isp_video_release(struct file *file)
 	struct fimc_isp *isp = video_drvdata(file);
 	struct fimc_is_video *ivc = &isp->video_capture;
 	struct media_entity *entity = &ivc->ve.vdev.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 
 	mutex_lock(&isp->video_lock);
 
@@ -469,8 +466,7 @@ static int isp_video_pipeline_validate(struct fimc_isp *isp)
 
 		/* Retrieve format at the source pad */
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		sd = media_entity_to_v4l2_subdev(pad->entity);
@@ -620,7 +616,7 @@ int fimc_isp_video_device_register(struct fimc_isp *isp,
 	vdev->lock = &isp->video_lock;
 
 	iv->pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&vdev->entity, 1, &iv->pad, 0);
+	ret = media_entity_pads_init(&vdev->entity, 1, &iv->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.c b/drivers/media/platform/exynos4-is/fimc-isp.c
index 5d78f5716f3b..293b807020c4 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp.c
+++ b/drivers/media/platform/exynos4-is/fimc-isp.c
@@ -708,8 +708,8 @@ int fimc_isp_subdev_create(struct fimc_isp *isp)
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_FIFO].flags = MEDIA_PAD_FL_SOURCE;
 	isp->subdev_pads[FIMC_ISP_SD_PAD_SRC_DMA].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, FIMC_ISP_SD_PADS_NUM,
-				isp->subdev_pads, 0);
+	ret = media_entity_pads_init(&sd->entity, FIMC_ISP_SD_PADS_NUM,
+				isp->subdev_pads);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-isp.h b/drivers/media/platform/exynos4-is/fimc-isp.h
index c2d25df85db9..e0686b5f1bf8 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp.h
+++ b/drivers/media/platform/exynos4-is/fimc-isp.h
@@ -24,7 +24,7 @@
 #include <media/videobuf2-v4l2.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 extern int fimc_isp_debug;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-lite-reg.c b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
index 0477716a20db..f0acc550d065 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite-reg.c
@@ -12,7 +12,7 @@
 #include <linux/bitops.h>
 #include <linux/delay.h>
 #include <linux/io.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "fimc-lite-reg.h"
 #include "fimc-lite.h"
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.c b/drivers/media/platform/exynos4-is/fimc-lite.c
index 60660c3a5de0..e85649147dc8 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.c
+++ b/drivers/media/platform/exynos4-is/fimc-lite.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-mem2mem.h>
 #include <media/videobuf2-v4l2.h>
 #include <media/videobuf2-dma-contig.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "common.h"
 #include "fimc-core.h"
@@ -292,7 +292,7 @@ static irqreturn_t flite_irq_handler(int irq, void *priv)
 	    test_bit(ST_FLITE_RUN, &fimc->state) &&
 	    !list_empty(&fimc->active_buf_q)) {
 		vbuf = fimc_lite_active_queue_pop(fimc);
-		v4l2_get_timestamp(&vbuf->vb.timestamp);
+		vbuf->vb.vb2_buf.timestamp = ktime_get_ns();
 		vbuf->vb.sequence = fimc->frame_count++;
 		flite_hw_mask_dma_buffer(fimc, vbuf->index);
 		vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
@@ -355,37 +355,34 @@ static void stop_streaming(struct vb2_queue *q)
 	fimc_lite_stop_capture(fimc, false);
 }
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *num_buffers, unsigned int *num_planes,
 		       unsigned int sizes[], void *allocators[])
 {
-	const struct v4l2_format *pfmt = parg;
-	const struct v4l2_pix_format_mplane *pixm = NULL;
 	struct fimc_lite *fimc = vq->drv_priv;
 	struct flite_frame *frame = &fimc->out_frame;
 	const struct fimc_fmt *fmt = frame->fmt;
-	unsigned long wh;
+	unsigned long wh = frame->f_width * frame->f_height;
 	int i;
 
-	if (pfmt) {
-		pixm = &pfmt->fmt.pix_mp;
-		fmt = fimc_lite_find_format(&pixm->pixelformat, NULL, 0, -1);
-		wh = pixm->width * pixm->height;
-	} else {
-		wh = frame->f_width * frame->f_height;
-	}
-
 	if (fmt == NULL)
 		return -EINVAL;
 
+	if (*num_planes) {
+		if (*num_planes != fmt->memplanes)
+			return -EINVAL;
+		for (i = 0; i < *num_planes; i++) {
+			if (sizes[i] < (wh * fmt->depth[i]) / 8)
+				return -EINVAL;
+			allocators[i] = fimc->alloc_ctx;
+		}
+		return 0;
+	}
+
 	*num_planes = fmt->memplanes;
 
 	for (i = 0; i < fmt->memplanes; i++) {
-		unsigned int size = (wh * fmt->depth[i]) / 8;
-		if (pixm)
-			sizes[i] = max(size, pixm->plane_fmt[i].sizeimage);
-		else
-			sizes[i] = size;
+		sizes[i] = (wh * fmt->depth[i]) / 8;
 		allocators[i] = fimc->alloc_ctx;
 	}
 
@@ -497,7 +494,7 @@ static int fimc_lite_open(struct file *file)
 	    atomic_read(&fimc->out_path) != FIMC_IO_DMA)
 		goto unlock;
 
-	mutex_lock(&me->parent->graph_mutex);
+	mutex_lock(&me->graph_obj.mdev->graph_mutex);
 
 	ret = fimc_pipeline_call(&fimc->ve, open, me, true);
 
@@ -505,7 +502,7 @@ static int fimc_lite_open(struct file *file)
 	if (ret == 0)
 		me->use_count++;
 
-	mutex_unlock(&me->parent->graph_mutex);
+	mutex_unlock(&me->graph_obj.mdev->graph_mutex);
 
 	if (!ret) {
 		fimc_lite_clear_event_counters(fimc);
@@ -538,9 +535,9 @@ static int fimc_lite_release(struct file *file)
 		fimc_pipeline_call(&fimc->ve, close);
 		clear_bit(ST_FLITE_IN_USE, &fimc->state);
 
-		mutex_lock(&entity->parent->graph_mutex);
+		mutex_lock(&entity->graph_obj.mdev->graph_mutex);
 		entity->use_count--;
-		mutex_unlock(&entity->parent->graph_mutex);
+		mutex_unlock(&entity->graph_obj.mdev->graph_mutex);
 	}
 
 	_vb2_fop_release(file, NULL);
@@ -811,8 +808,7 @@ static int fimc_pipeline_validate(struct fimc_lite *fimc)
 		}
 		/* Retrieve format at the source pad */
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		sd = media_entity_to_v4l2_subdev(pad->entity);
@@ -985,7 +981,6 @@ static int fimc_lite_link_setup(struct media_entity *entity,
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct fimc_lite *fimc = v4l2_get_subdevdata(sd);
-	unsigned int remote_ent_type = media_entity_type(remote->entity);
 	int ret = 0;
 
 	if (WARN_ON(fimc == NULL))
@@ -997,7 +992,7 @@ static int fimc_lite_link_setup(struct media_entity *entity,
 
 	switch (local->index) {
 	case FLITE_SD_PAD_SINK:
-		if (remote_ent_type != MEDIA_ENT_T_V4L2_SUBDEV) {
+		if (!is_media_entity_v4l2_subdev(remote->entity)) {
 			ret = -EINVAL;
 			break;
 		}
@@ -1015,7 +1010,7 @@ static int fimc_lite_link_setup(struct media_entity *entity,
 	case FLITE_SD_PAD_SOURCE_DMA:
 		if (!(flags & MEDIA_LNK_FL_ENABLED))
 			atomic_set(&fimc->out_path, FIMC_IO_NONE);
-		else if (remote_ent_type == MEDIA_ENT_T_DEVNODE)
+		else if (is_media_entity_v4l2_io(remote->entity))
 			atomic_set(&fimc->out_path, FIMC_IO_DMA);
 		else
 			ret = -EINVAL;
@@ -1024,7 +1019,7 @@ static int fimc_lite_link_setup(struct media_entity *entity,
 	case FLITE_SD_PAD_SOURCE_ISP:
 		if (!(flags & MEDIA_LNK_FL_ENABLED))
 			atomic_set(&fimc->out_path, FIMC_IO_NONE);
-		else if (remote_ent_type == MEDIA_ENT_T_V4L2_SUBDEV)
+		else if (is_media_entity_v4l2_subdev(remote->entity))
 			atomic_set(&fimc->out_path, FIMC_IO_ISP);
 		else
 			ret = -EINVAL;
@@ -1319,7 +1314,7 @@ static int fimc_lite_subdev_registered(struct v4l2_subdev *sd)
 		return ret;
 
 	fimc->vd_pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&vfd->entity, 1, &fimc->vd_pad, 0);
+	ret = media_entity_pads_init(&vfd->entity, 1, &fimc->vd_pad);
 	if (ret < 0)
 		return ret;
 
@@ -1433,8 +1428,8 @@ static int fimc_lite_create_capture_subdev(struct fimc_lite *fimc)
 	fimc->subdev_pads[FLITE_SD_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	fimc->subdev_pads[FLITE_SD_PAD_SOURCE_DMA].flags = MEDIA_PAD_FL_SOURCE;
 	fimc->subdev_pads[FLITE_SD_PAD_SOURCE_ISP].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&sd->entity, FLITE_SD_PADS_NUM,
-				fimc->subdev_pads, 0);
+	ret = media_entity_pads_init(&sd->entity, FLITE_SD_PADS_NUM,
+				fimc->subdev_pads);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-lite.h b/drivers/media/platform/exynos4-is/fimc-lite.h
index b302305dedbe..11690d563e06 100644
--- a/drivers/media/platform/exynos4-is/fimc-lite.h
+++ b/drivers/media/platform/exynos4-is/fimc-lite.h
@@ -23,7 +23,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #define FIMC_LITE_DRV_NAME	"exynos-fimc-lite"
 #define FLITE_CLK_NAME		"flite"
diff --git a/drivers/media/platform/exynos4-is/fimc-m2m.c b/drivers/media/platform/exynos4-is/fimc-m2m.c
index 4d1d64a46b21..55ec4c99d484 100644
--- a/drivers/media/platform/exynos4-is/fimc-m2m.c
+++ b/drivers/media/platform/exynos4-is/fimc-m2m.c
@@ -132,7 +132,7 @@ static void fimc_device_run(void *priv)
 	if (ret)
 		goto dma_unlock;
 
-	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst_vb->flags |=
 		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
@@ -176,7 +176,7 @@ static void fimc_job_abort(void *priv)
 	fimc_m2m_shutdown(priv);
 }
 
-static int fimc_queue_setup(struct vb2_queue *vq, const void *parg,
+static int fimc_queue_setup(struct vb2_queue *vq,
 			    unsigned int *num_buffers, unsigned int *num_planes,
 			    unsigned int sizes[], void *allocators[])
 {
@@ -739,7 +739,7 @@ int fimc_register_m2m_device(struct fimc_dev *fimc,
 		return PTR_ERR(fimc->m2m.m2m_dev);
 	}
 
-	ret = media_entity_init(&vfd->entity, 0, NULL, 0);
+	ret = media_entity_pads_init(&vfd->entity, 0, NULL);
 	if (ret)
 		goto err_me;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-reg.c b/drivers/media/platform/exynos4-is/fimc-reg.c
index df0cbcb69b6b..0806724553a2 100644
--- a/drivers/media/platform/exynos4-is/fimc-reg.c
+++ b/drivers/media/platform/exynos4-is/fimc-reg.c
@@ -13,7 +13,7 @@
 #include <linux/io.h>
 #include <linux/regmap.h>
 
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 #include "media-dev.h"
 
 #include "fimc-reg.h"
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index 4f5586a4cbff..e79ddbb1e14f 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-of.h>
 #include <media/media-device.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "media-dev.h"
 #include "fimc-core.h"
@@ -88,8 +88,7 @@ static void fimc_pipeline_prepare(struct fimc_pipeline *p,
 				break;
 		}
 
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 		sd = media_entity_to_v4l2_subdev(pad->entity);
 
@@ -187,6 +186,37 @@ error:
 }
 
 /**
+ * __fimc_pipeline_enable - enable power of all pipeline subdevs
+ *			    and the sensor clock
+ * @ep: video pipeline structure
+ * @fmd: fimc media device
+ *
+ * Called with the graph mutex held.
+ */
+static int __fimc_pipeline_enable(struct exynos_media_pipeline *ep,
+				  struct fimc_md *fmd)
+{
+	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+	int ret;
+
+	/* Enable PXLASYNC clock if this pipeline includes FIMC-IS */
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP]) {
+		ret = clk_prepare_enable(fmd->wbclk[CLK_IDX_WB_B]);
+		if (ret < 0)
+			return ret;
+	}
+
+	ret = fimc_pipeline_s_power(p, 1);
+	if (!ret)
+		return 0;
+
+	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
+		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
+
+	return ret;
+}
+
+/**
  * __fimc_pipeline_open - update the pipeline information, enable power
  *                        of all pipeline subdevs and the sensor clock
  * @me: media entity to start graph walk with
@@ -200,7 +230,6 @@ static int __fimc_pipeline_open(struct exynos_media_pipeline *ep,
 	struct fimc_md *fmd = entity_to_fimc_mdev(me);
 	struct fimc_pipeline *p = to_fimc_pipeline(ep);
 	struct v4l2_subdev *sd;
-	int ret;
 
 	if (WARN_ON(p == NULL || me == NULL))
 		return -EINVAL;
@@ -209,24 +238,16 @@ static int __fimc_pipeline_open(struct exynos_media_pipeline *ep,
 		fimc_pipeline_prepare(p, me);
 
 	sd = p->subdevs[IDX_SENSOR];
-	if (sd == NULL)
-		return -EINVAL;
-
-	/* Disable PXLASYNC clock if this pipeline includes FIMC-IS */
-	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP]) {
-		ret = clk_prepare_enable(fmd->wbclk[CLK_IDX_WB_B]);
-		if (ret < 0)
-			return ret;
-	}
-
-	ret = fimc_pipeline_s_power(p, 1);
-	if (!ret)
+	if (sd == NULL) {
+		pr_warn("%s(): No sensor subdev\n", __func__);
+		/*
+		 * Pipeline open cannot fail so as to make it possible
+		 * for the user space to configure the pipeline.
+		 */
 		return 0;
+	}
 
-	if (!IS_ERR(fmd->wbclk[CLK_IDX_WB_B]) && p->subdevs[IDX_IS_ISP])
-		clk_disable_unprepare(fmd->wbclk[CLK_IDX_WB_B]);
-
-	return ret;
+	return __fimc_pipeline_enable(ep, fmd);
 }
 
 /**
@@ -270,10 +291,43 @@ static int __fimc_pipeline_s_stream(struct exynos_media_pipeline *ep, bool on)
 		{ IDX_CSIS, IDX_FLITE, IDX_FIMC, IDX_SENSOR, IDX_IS_ISP },
 	};
 	struct fimc_pipeline *p = to_fimc_pipeline(ep);
+	struct fimc_md *fmd = entity_to_fimc_mdev(&p->subdevs[IDX_CSIS]->entity);
+	enum fimc_subdev_index sd_id;
 	int i, ret = 0;
 
-	if (p->subdevs[IDX_SENSOR] == NULL)
-		return -ENODEV;
+	if (p->subdevs[IDX_SENSOR] == NULL) {
+		if (!fmd->user_subdev_api) {
+			/*
+			 * Sensor must be already discovered if we
+			 * aren't in the user_subdev_api mode
+			 */
+			return -ENODEV;
+		}
+
+		/* Get pipeline sink entity */
+		if (p->subdevs[IDX_FIMC])
+			sd_id = IDX_FIMC;
+		else if (p->subdevs[IDX_IS_ISP])
+			sd_id = IDX_IS_ISP;
+		else if (p->subdevs[IDX_FLITE])
+			sd_id = IDX_FLITE;
+		else
+			return -ENODEV;
+
+		/*
+		 * Sensor could have been linked between open and STREAMON -
+		 * check if this is the case.
+		 */
+		fimc_pipeline_prepare(p, &p->subdevs[sd_id]->entity);
+
+		if (p->subdevs[IDX_SENSOR] == NULL)
+			return -ENODEV;
+
+		ret = __fimc_pipeline_enable(ep, fmd);
+		if (ret < 0)
+			return ret;
+
+	}
 
 	for (i = 0; i < IDX_MAX; i++) {
 		unsigned int idx = seq[on][i];
@@ -283,8 +337,10 @@ static int __fimc_pipeline_s_stream(struct exynos_media_pipeline *ep, bool on)
 		if (ret < 0 && ret != -ENOIOCTLCMD && ret != -ENODEV)
 			goto error;
 	}
+
 	return 0;
 error:
+	fimc_pipeline_s_power(p, !on);
 	for (; i >= 0; i--) {
 		unsigned int idx = seq[on][i];
 		v4l2_subdev_call(p->subdevs[idx], video, s_stream, !on);
@@ -729,7 +785,7 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 		flags = ((1 << i) & link_mask) ? MEDIA_LNK_FL_ENABLED : 0;
 
 		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
-		ret = media_entity_create_link(source, pad, sink,
+		ret = media_create_pad_link(source, pad, sink,
 					      FIMC_SD_PAD_SINK_CAM, flags);
 		if (ret)
 			return ret;
@@ -749,7 +805,7 @@ static int __fimc_md_create_fimc_sink_links(struct fimc_md *fmd,
 			continue;
 
 		sink = &fmd->fimc_lite[i]->subdev.entity;
-		ret = media_entity_create_link(source, pad, sink,
+		ret = media_create_pad_link(source, pad, sink,
 					       FLITE_SD_PAD_SINK, 0);
 		if (ret)
 			return ret;
@@ -781,13 +837,13 @@ static int __fimc_md_create_flite_source_links(struct fimc_md *fmd)
 		source = &fimc->subdev.entity;
 		sink = &fimc->ve.vdev.entity;
 		/* FIMC-LITE's subdev and video node */
-		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_DMA,
+		ret = media_create_pad_link(source, FLITE_SD_PAD_SOURCE_DMA,
 					       sink, 0, 0);
 		if (ret)
 			break;
 		/* Link from FIMC-LITE to IS-ISP subdev */
 		sink = &fmd->fimc_is->isp.subdev.entity;
-		ret = media_entity_create_link(source, FLITE_SD_PAD_SOURCE_ISP,
+		ret = media_create_pad_link(source, FLITE_SD_PAD_SOURCE_ISP,
 					       sink, 0, 0);
 		if (ret)
 			break;
@@ -811,7 +867,7 @@ static int __fimc_md_create_fimc_is_links(struct fimc_md *fmd)
 
 		/* Link from FIMC-IS-ISP subdev to FIMC */
 		sink = &fmd->fimc[i]->vid_cap.subdev.entity;
-		ret = media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_FIFO,
+		ret = media_create_pad_link(source, FIMC_ISP_SD_PAD_SRC_FIFO,
 					       sink, FIMC_SD_PAD_SINK_FIFO, 0);
 		if (ret)
 			return ret;
@@ -824,7 +880,7 @@ static int __fimc_md_create_fimc_is_links(struct fimc_md *fmd)
 	if (sink->num_pads == 0)
 		return 0;
 
-	return media_entity_create_link(source, FIMC_ISP_SD_PAD_SRC_DMA,
+	return media_create_pad_link(source, FIMC_ISP_SD_PAD_SRC_DMA,
 					sink, 0, 0);
 }
 
@@ -873,7 +929,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 				return -EINVAL;
 
 			pad = sensor->entity.num_pads - 1;
-			ret = media_entity_create_link(&sensor->entity, pad,
+			ret = media_create_pad_link(&sensor->entity, pad,
 					      &csis->entity, CSIS_PAD_SINK,
 					      MEDIA_LNK_FL_IMMUTABLE |
 					      MEDIA_LNK_FL_ENABLED);
@@ -927,7 +983,7 @@ static int fimc_md_create_links(struct fimc_md *fmd)
 		source = &fmd->fimc[i]->vid_cap.subdev.entity;
 		sink = &fmd->fimc[i]->vid_cap.ve.vdev.entity;
 
-		ret = media_entity_create_link(source, FIMC_SD_PAD_SOURCE,
+		ret = media_create_pad_link(source, FIMC_SD_PAD_SOURCE,
 					      sink, 0, flags);
 		if (ret)
 			break;
@@ -1046,11 +1102,11 @@ static int __fimc_md_modify_pipeline(struct media_entity *entity, bool enable)
 	return ret;
 }
 
-/* Locking: called with entity->parent->graph_mutex mutex held. */
-static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable)
+/* Locking: called with entity->graph_obj.mdev->graph_mutex mutex held. */
+static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable,
+				      struct media_entity_graph *graph)
 {
 	struct media_entity *entity_err = entity;
-	struct media_entity_graph graph;
 	int ret;
 
 	/*
@@ -1059,10 +1115,10 @@ static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable)
 	 * through active links. This is needed as we cannot power on/off the
 	 * subdevs in random order.
 	 */
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+	while ((entity = media_entity_graph_walk_next(graph))) {
+		if (!is_media_entity_v4l2_io(entity))
 			continue;
 
 		ret  = __fimc_md_modify_pipeline(entity, enable);
@@ -1072,11 +1128,12 @@ static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable)
 	}
 
 	return 0;
- err:
-	media_entity_graph_walk_start(&graph, entity_err);
 
-	while ((entity_err = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(entity_err) != MEDIA_ENT_T_DEVNODE)
+err:
+	media_entity_graph_walk_start(graph, entity_err);
+
+	while ((entity_err = media_entity_graph_walk_next(graph))) {
+		if (!is_media_entity_v4l2_io(entity_err))
 			continue;
 
 		__fimc_md_modify_pipeline(entity_err, !enable);
@@ -1091,21 +1148,29 @@ static int __fimc_md_modify_pipelines(struct media_entity *entity, bool enable)
 static int fimc_md_link_notify(struct media_link *link, unsigned int flags,
 				unsigned int notification)
 {
+	struct media_entity_graph *graph =
+		&container_of(link->graph_obj.mdev, struct fimc_md,
+			      media_dev)->link_setup_graph;
 	struct media_entity *sink = link->sink->entity;
 	int ret = 0;
 
 	/* Before link disconnection */
 	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH) {
+		ret = media_entity_graph_walk_init(graph,
+						   link->graph_obj.mdev);
+		if (ret)
+			return ret;
 		if (!(flags & MEDIA_LNK_FL_ENABLED))
-			ret = __fimc_md_modify_pipelines(sink, false);
+			ret = __fimc_md_modify_pipelines(sink, false, graph);
 #if 0
 		else
 			/* TODO: Link state change validation */
 #endif
 	/* After link activation */
-	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
-		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
-		ret = __fimc_md_modify_pipelines(sink, true);
+	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH) {
+		if (link->flags & MEDIA_LNK_FL_ENABLED)
+			ret = __fimc_md_modify_pipelines(sink, true, graph);
+		media_entity_graph_walk_cleanup(graph);
 	}
 
 	return ret ? -EPIPE : 0;
@@ -1314,7 +1379,10 @@ static int subdev_notifier_complete(struct v4l2_async_notifier *notifier)
 	ret = v4l2_device_register_subdev_nodes(&fmd->v4l2_dev);
 unlock:
 	mutex_unlock(&fmd->media_dev.graph_mutex);
-	return ret;
+	if (ret < 0)
+		return ret;
+
+	return media_device_register(&fmd->media_dev);
 }
 
 static int fimc_md_probe(struct platform_device *pdev)
@@ -1345,18 +1413,14 @@ static int fimc_md_probe(struct platform_device *pdev)
 	fmd->use_isp = fimc_md_is_isp_available(dev->of_node);
 	fmd->user_subdev_api = true;
 
+	media_device_init(&fmd->media_dev);
+
 	ret = v4l2_device_register(dev, &fmd->v4l2_dev);
 	if (ret < 0) {
 		v4l2_err(v4l2_dev, "Failed to register v4l2_device: %d\n", ret);
 		return ret;
 	}
 
-	ret = media_device_register(&fmd->media_dev);
-	if (ret < 0) {
-		v4l2_err(v4l2_dev, "Failed to register media device: %d\n", ret);
-		goto err_v4l2_dev;
-	}
-
 	ret = fimc_md_get_clocks(fmd);
 	if (ret)
 		goto err_md;
@@ -1425,8 +1489,7 @@ err_clk:
 err_m_ent:
 	fimc_md_unregister_entities(fmd);
 err_md:
-	media_device_unregister(&fmd->media_dev);
-err_v4l2_dev:
+	media_device_cleanup(&fmd->media_dev);
 	v4l2_device_unregister(&fmd->v4l2_dev);
 	return ret;
 }
@@ -1446,6 +1509,7 @@ static int fimc_md_remove(struct platform_device *pdev)
 	fimc_md_unregister_entities(fmd);
 	fimc_md_pipelines_free(fmd);
 	media_device_unregister(&fmd->media_dev);
+	media_device_cleanup(&fmd->media_dev);
 	fimc_md_put_clocks(fmd);
 
 	return 0;
diff --git a/drivers/media/platform/exynos4-is/media-dev.h b/drivers/media/platform/exynos4-is/media-dev.h
index 03214541f149..ed122cb2dd74 100644
--- a/drivers/media/platform/exynos4-is/media-dev.h
+++ b/drivers/media/platform/exynos4-is/media-dev.h
@@ -19,7 +19,7 @@
 #include <media/media-entity.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-subdev.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 
 #include "fimc-core.h"
 #include "fimc-lite.h"
@@ -154,6 +154,7 @@ struct fimc_md {
 	bool user_subdev_api;
 	spinlock_t slock;
 	struct list_head pipelines;
+	struct media_entity_graph link_setup_graph;
 };
 
 static inline
@@ -164,8 +165,8 @@ struct fimc_sensor_info *source_to_sensor_info(struct fimc_source_info *si)
 
 static inline struct fimc_md *entity_to_fimc_mdev(struct media_entity *me)
 {
-	return me->parent == NULL ? NULL :
-		container_of(me->parent, struct fimc_md, media_dev);
+	return me->graph_obj.mdev == NULL ? NULL :
+		container_of(me->graph_obj.mdev, struct fimc_md, media_dev);
 }
 
 static inline struct fimc_md *notifier_to_fimc_md(struct v4l2_async_notifier *n)
@@ -175,12 +176,12 @@ static inline struct fimc_md *notifier_to_fimc_md(struct v4l2_async_notifier *n)
 
 static inline void fimc_md_graph_lock(struct exynos_video_entity *ve)
 {
-	mutex_lock(&ve->vdev.entity.parent->graph_mutex);
+	mutex_lock(&ve->vdev.entity.graph_obj.mdev->graph_mutex);
 }
 
 static inline void fimc_md_graph_unlock(struct exynos_video_entity *ve)
 {
-	mutex_unlock(&ve->vdev.entity.parent->graph_mutex);
+	mutex_unlock(&ve->vdev.entity.graph_obj.mdev->graph_mutex);
 }
 
 int fimc_md_set_camclk(struct v4l2_subdev *sd, bool on);
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index 4b85105dc159..ac5e50e595be 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -29,7 +29,7 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
-#include <media/exynos-fimc.h>
+#include <media/drv-intf/exynos-fimc.h>
 #include <media/v4l2-of.h>
 #include <media/v4l2-subdev.h>
 
@@ -866,8 +866,8 @@ static int s5pcsis_probe(struct platform_device *pdev)
 
 	state->pads[CSIS_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 	state->pads[CSIS_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
-	ret = media_entity_init(&state->sd.entity,
-				CSIS_PADS_NUM, state->pads, 0);
+	ret = media_entity_pads_init(&state->sd.entity,
+				CSIS_PADS_NUM, state->pads);
 	if (ret < 0)
 		goto e_clkdis;
 
diff --git a/drivers/media/platform/m2m-deinterlace.c b/drivers/media/platform/m2m-deinterlace.c
index 29973f9bf8db..7383818c2be6 100644
--- a/drivers/media/platform/m2m-deinterlace.c
+++ b/drivers/media/platform/m2m-deinterlace.c
@@ -207,7 +207,7 @@ static void dma_callback(void *data)
 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
-	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 	dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst_vb->flags |=
 		src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
@@ -798,7 +798,6 @@ struct vb2_dc_conf {
 };
 
 static int deinterlace_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index aa2b44041d3f..9b878deb1437 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -25,7 +25,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-event.h>
-#include <media/ov7670.h>
+#include <media/i2c/ov7670.h>
 #include <media/videobuf2-vmalloc.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/videobuf2-dma-sg.h>
@@ -226,7 +226,7 @@ static void mcam_buffer_done(struct mcam_camera *cam, int frame,
 	vbuf->vb2_buf.planes[0].bytesused = cam->pix_format.sizeimage;
 	vbuf->sequence = cam->buf_seq[frame];
 	vbuf->field = V4L2_FIELD_NONE;
-	v4l2_get_timestamp(&vbuf->timestamp);
+	vbuf->vb2_buf.timestamp = ktime_get_ns();
 	vb2_set_plane_payload(&vbuf->vb2_buf, 0, cam->pix_format.sizeimage);
 	vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
 }
@@ -1049,24 +1049,25 @@ static int mcam_read_setup(struct mcam_camera *cam)
  */
 
 static int mcam_vb_queue_setup(struct vb2_queue *vq,
-		const void *parg, unsigned int *nbufs,
+		unsigned int *nbufs,
 		unsigned int *num_planes, unsigned int sizes[],
 		void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct mcam_camera *cam = vb2_get_drv_priv(vq);
 	int minbufs = (cam->buffer_mode == B_DMA_contig) ? 3 : 2;
+	unsigned size = cam->pix_format.sizeimage;
 
-	if (fmt && fmt->fmt.pix.sizeimage < cam->pix_format.sizeimage)
-		return -EINVAL;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : cam->pix_format.sizeimage;
-	*num_planes = 1; /* Someday we have to support planar formats... */
 	if (*nbufs < minbufs)
 		*nbufs = minbufs;
 	if (cam->buffer_mode == B_DMA_contig)
 		alloc_ctxs[0] = cam->vb_alloc_ctx;
 	else if (cam->buffer_mode == B_DMA_sg)
 		alloc_ctxs[0] = cam->vb_alloc_ctx_sg;
+
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+	sizes[0] = size;
+	*num_planes = 1; /* Someday we have to support planar formats... */
 	return 0;
 }
 
diff --git a/drivers/media/platform/marvell-ccic/mmp-driver.c b/drivers/media/platform/marvell-ccic/mmp-driver.c
index b5f165a68566..816f4b6a7b8e 100644
--- a/drivers/media/platform/marvell-ccic/mmp-driver.c
+++ b/drivers/media/platform/marvell-ccic/mmp-driver.c
@@ -18,7 +18,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-device.h>
-#include <media/mmp-camera.h>
+#include <linux/platform_data/media/mmp-camera.h>
 #include <linux/device.h>
 #include <linux/platform_device.h>
 #include <linux/gpio.h>
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index 03a1b606655d..3c4012d42d69 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -375,7 +375,7 @@ static irqreturn_t emmaprp_irq(int irq_emma, void *data)
 			src_vb = v4l2_m2m_src_buf_remove(curr_ctx->m2m_ctx);
 			dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->m2m_ctx);
 
-			dst_vb->timestamp = src_vb->timestamp;
+			dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 			dst_vb->flags &=
 				~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 			dst_vb->flags |=
@@ -689,7 +689,6 @@ static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = {
  * Queue operations
  */
 static int emmaprp_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/omap/Kconfig b/drivers/media/platform/omap/Kconfig
index 217d613b0fe7..e8e2db181a7a 100644
--- a/drivers/media/platform/omap/Kconfig
+++ b/drivers/media/platform/omap/Kconfig
@@ -5,9 +5,9 @@ config VIDEO_OMAP2_VOUT
 	tristate "OMAP2/OMAP3 V4L2-Display driver"
 	depends on MMU
 	depends on ARCH_OMAP2 || ARCH_OMAP3
+	depends on FB_OMAP2
 	select VIDEOBUF_GEN
 	select VIDEOBUF_DMA_CONTIG
-	select OMAP2_DSS if HAS_IOMEM && ARCH_OMAP2PLUS
 	select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
 	select VIDEO_OMAP2_VOUT_VRFB if VIDEO_OMAP2_VOUT && OMAP2_VRFB
 	select FRAME_VECTOR
diff --git a/drivers/media/platform/omap/omap_vout_vrfb.c b/drivers/media/platform/omap/omap_vout_vrfb.c
index c6e252760c62..b8638e4e1627 100644
--- a/drivers/media/platform/omap/omap_vout_vrfb.c
+++ b/drivers/media/platform/omap/omap_vout_vrfb.c
@@ -79,10 +79,12 @@ void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout)
 	int j;
 
 	for (j = 0; j < VRFB_NUM_BUFS; j++) {
-		omap_vout_free_buffer(vout->smsshado_virt_addr[j],
-				vout->smsshado_size);
-		vout->smsshado_virt_addr[j] = 0;
-		vout->smsshado_phy_addr[j] = 0;
+		if (vout->smsshado_virt_addr[j]) {
+			omap_vout_free_buffer(vout->smsshado_virt_addr[j],
+					      vout->smsshado_size);
+			vout->smsshado_virt_addr[j] = 0;
+			vout->smsshado_phy_addr[j] = 0;
+		}
 	}
 }
 
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 56e683b19a73..0bcfa553c1aa 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -683,15 +683,15 @@ static irqreturn_t isp_isr(int irq, void *_isp)
  *
  * Return the total number of users of all video device nodes in the pipeline.
  */
-static int isp_pipeline_pm_use_count(struct media_entity *entity)
+static int isp_pipeline_pm_use_count(struct media_entity *entity,
+	struct media_entity_graph *graph)
 {
-	struct media_entity_graph graph;
 	int use = 0;
 
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+	while ((entity = media_entity_graph_walk_next(graph))) {
+		if (is_media_entity_v4l2_io(entity))
 			use += entity->use_count;
 	}
 
@@ -714,7 +714,7 @@ static int isp_pipeline_pm_power_one(struct media_entity *entity, int change)
 	struct v4l2_subdev *subdev;
 	int ret;
 
-	subdev = media_entity_type(entity) == MEDIA_ENT_T_V4L2_SUBDEV
+	subdev = is_media_entity_v4l2_subdev(entity)
 	       ? media_entity_to_v4l2_subdev(entity) : NULL;
 
 	if (entity->use_count == 0 && change > 0 && subdev != NULL) {
@@ -742,29 +742,29 @@ static int isp_pipeline_pm_power_one(struct media_entity *entity, int change)
  *
  * Return 0 on success or a negative error code on failure.
  */
-static int isp_pipeline_pm_power(struct media_entity *entity, int change)
+static int isp_pipeline_pm_power(struct media_entity *entity, int change,
+	struct media_entity_graph *graph)
 {
-	struct media_entity_graph graph;
 	struct media_entity *first = entity;
 	int ret = 0;
 
 	if (!change)
 		return 0;
 
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(graph, entity);
 
-	while (!ret && (entity = media_entity_graph_walk_next(&graph)))
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+	while (!ret && (entity = media_entity_graph_walk_next(graph)))
+		if (is_media_entity_v4l2_subdev(entity))
 			ret = isp_pipeline_pm_power_one(entity, change);
 
 	if (!ret)
-		return 0;
+		return ret;
 
-	media_entity_graph_walk_start(&graph, first);
+	media_entity_graph_walk_start(graph, first);
 
-	while ((first = media_entity_graph_walk_next(&graph))
+	while ((first = media_entity_graph_walk_next(graph))
 	       && first != entity)
-		if (media_entity_type(first) != MEDIA_ENT_T_DEVNODE)
+		if (is_media_entity_v4l2_subdev(first))
 			isp_pipeline_pm_power_one(first, -change);
 
 	return ret;
@@ -782,23 +782,24 @@ static int isp_pipeline_pm_power(struct media_entity *entity, int change)
  * off is assumed to never fail. No failure can occur when the use parameter is
  * set to 0.
  */
-int omap3isp_pipeline_pm_use(struct media_entity *entity, int use)
+int omap3isp_pipeline_pm_use(struct media_entity *entity, int use,
+			     struct media_entity_graph *graph)
 {
 	int change = use ? 1 : -1;
 	int ret;
 
-	mutex_lock(&entity->parent->graph_mutex);
+	mutex_lock(&entity->graph_obj.mdev->graph_mutex);
 
 	/* Apply use count to node. */
 	entity->use_count += change;
 	WARN_ON(entity->use_count < 0);
 
 	/* Apply power change to connected non-nodes. */
-	ret = isp_pipeline_pm_power(entity, change);
+	ret = isp_pipeline_pm_power(entity, change, graph);
 	if (ret < 0)
 		entity->use_count -= change;
 
-	mutex_unlock(&entity->parent->graph_mutex);
+	mutex_unlock(&entity->graph_obj.mdev->graph_mutex);
 
 	return ret;
 }
@@ -820,35 +821,49 @@ int omap3isp_pipeline_pm_use(struct media_entity *entity, int use)
 static int isp_pipeline_link_notify(struct media_link *link, u32 flags,
 				    unsigned int notification)
 {
+	struct media_entity_graph *graph =
+		&container_of(link->graph_obj.mdev, struct isp_device,
+			      media_dev)->pm_count_graph;
 	struct media_entity *source = link->source->entity;
 	struct media_entity *sink = link->sink->entity;
-	int source_use = isp_pipeline_pm_use_count(source);
-	int sink_use = isp_pipeline_pm_use_count(sink);
-	int ret;
+	int source_use;
+	int sink_use;
+	int ret = 0;
+
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH) {
+		ret = media_entity_graph_walk_init(graph,
+						   link->graph_obj.mdev);
+		if (ret)
+			return ret;
+	}
+
+	source_use = isp_pipeline_pm_use_count(source, graph);
+	sink_use = isp_pipeline_pm_use_count(sink, graph);
 
 	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
 	    !(flags & MEDIA_LNK_FL_ENABLED)) {
 		/* Powering off entities is assumed to never fail. */
-		isp_pipeline_pm_power(source, -sink_use);
-		isp_pipeline_pm_power(sink, -source_use);
+		isp_pipeline_pm_power(source, -sink_use, graph);
+		isp_pipeline_pm_power(sink, -source_use, graph);
 		return 0;
 	}
 
 	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
 		(flags & MEDIA_LNK_FL_ENABLED)) {
 
-		ret = isp_pipeline_pm_power(source, sink_use);
+		ret = isp_pipeline_pm_power(source, sink_use, graph);
 		if (ret < 0)
 			return ret;
 
-		ret = isp_pipeline_pm_power(sink, source_use);
+		ret = isp_pipeline_pm_power(sink, source_use, graph);
 		if (ret < 0)
-			isp_pipeline_pm_power(source, -sink_use);
-
-		return ret;
+			isp_pipeline_pm_power(source, -sink_use, graph);
 	}
 
-	return 0;
+	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH)
+		media_entity_graph_walk_cleanup(graph);
+
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -881,7 +896,7 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 	 * starting entities if the pipeline won't start anyway (those entities
 	 * would then likely fail to stop, making the problem worse).
 	 */
-	if (pipe->entities & isp->crashed)
+	if (media_entity_enum_intersects(&pipe->ent_enum, &isp->crashed))
 		return -EIO;
 
 	spin_lock_irqsave(&pipe->lock, flags);
@@ -897,8 +912,7 @@ static int isp_pipeline_enable(struct isp_pipeline *pipe,
 			break;
 
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = pad->entity;
@@ -987,8 +1001,7 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 			break;
 
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = pad->entity;
@@ -1028,7 +1041,8 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 			dev_info(isp->dev, "Unable to stop %s\n", subdev->name);
 			isp->stop_failure = true;
 			if (subdev == &isp->isp_prev.subdev)
-				isp->crashed |= 1U << subdev->entity.id;
+				media_entity_enum_set(&isp->crashed,
+						      &subdev->entity);
 			failure = -ETIMEDOUT;
 		}
 	}
@@ -1234,7 +1248,7 @@ static int isp_reset(struct isp_device *isp)
 	}
 
 	isp->stop_failure = false;
-	isp->crashed = 0;
+	media_entity_enum_zero(&isp->crashed);
 	return 0;
 }
 
@@ -1645,7 +1659,8 @@ static void __omap3isp_put(struct isp_device *isp, bool save_ctx)
 		/* Reset the ISP if an entity has failed to stop. This is the
 		 * only way to recover from such conditions.
 		 */
-		if (isp->crashed || isp->stop_failure)
+		if (!media_entity_enum_empty(&isp->crashed) ||
+		    isp->stop_failure)
 			isp_reset(isp);
 		isp_disable_clocks(isp);
 	}
@@ -1792,6 +1807,7 @@ static void isp_unregister_entities(struct isp_device *isp)
 
 	v4l2_device_unregister(&isp->v4l2_dev);
 	media_device_unregister(&isp->media_dev);
+	media_device_cleanup(&isp->media_dev);
 }
 
 static int isp_link_entity(
@@ -1862,7 +1878,7 @@ static int isp_link_entity(
 		return -EINVAL;
 	}
 
-	return media_entity_create_link(entity, i, input, pad, flags);
+	return media_create_pad_link(entity, i, input, pad, flags);
 }
 
 static int isp_register_entities(struct isp_device *isp)
@@ -1874,12 +1890,7 @@ static int isp_register_entities(struct isp_device *isp)
 		sizeof(isp->media_dev.model));
 	isp->media_dev.hw_revision = isp->revision;
 	isp->media_dev.link_notify = isp_pipeline_link_notify;
-	ret = media_device_register(&isp->media_dev);
-	if (ret < 0) {
-		dev_err(isp->dev, "%s: Media device registration failed (%d)\n",
-			__func__, ret);
-		return ret;
-	}
+	media_device_init(&isp->media_dev);
 
 	isp->v4l2_dev.mdev = &isp->media_dev;
 	ret = v4l2_device_register(isp->dev, &isp->v4l2_dev);
@@ -1930,6 +1941,118 @@ done:
 	return ret;
 }
 
+/*
+ * isp_create_links() - Create links for internal and external ISP entities
+ * @isp : Pointer to ISP device
+ *
+ * This function creates all links between ISP internal and external entities.
+ *
+ * Return: A negative error code on failure or zero on success. Possible error
+ * codes are those returned by media_create_pad_link().
+ */
+static int isp_create_links(struct isp_device *isp)
+{
+	int ret;
+
+	/* Create links between entities and video nodes. */
+	ret = media_create_pad_link(
+			&isp->isp_csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&isp->isp_csi2a.video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccp2.video_in.video.entity, 0,
+			&isp->isp_ccp2.subdev.entity, CCP2_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_OF,
+			&isp->isp_ccdc.video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_prev.video_in.video.entity, 0,
+			&isp->isp_prev.subdev.entity, PREV_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_prev.subdev.entity, PREV_PAD_SOURCE,
+			&isp->isp_prev.video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_res.video_in.video.entity, 0,
+			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_res.subdev.entity, RESZ_PAD_SOURCE,
+			&isp->isp_res.video_out.video.entity, 0, 0);
+
+	if (ret < 0)
+		return ret;
+
+	/* Create links between entities. */
+	ret = media_create_pad_link(
+			&isp->isp_csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccp2.subdev.entity, CCP2_PAD_SOURCE,
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_prev.subdev.entity, PREV_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_OF,
+			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_prev.subdev.entity, PREV_PAD_SOURCE,
+			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_aewb.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_af.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
+			&isp->isp_hist.subdev.entity, 0,
+			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
 static void isp_cleanup_modules(struct isp_device *isp)
 {
 	omap3isp_h3a_aewb_cleanup(isp);
@@ -2000,62 +2123,8 @@ static int isp_initialize_modules(struct isp_device *isp)
 		goto error_h3a_af;
 	}
 
-	/* Connect the submodules. */
-	ret = media_entity_create_link(
-			&isp->isp_csi2a.subdev.entity, CSI2_PAD_SOURCE,
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccp2.subdev.entity, CCP2_PAD_SOURCE,
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
-			&isp->isp_prev.subdev.entity, PREV_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_OF,
-			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_prev.subdev.entity, PREV_PAD_SOURCE,
-			&isp->isp_res.subdev.entity, RESZ_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
-			&isp->isp_aewb.subdev.entity, 0,
-			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
-			&isp->isp_af.subdev.entity, 0,
-			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&isp->isp_ccdc.subdev.entity, CCDC_PAD_SOURCE_VP,
-			&isp->isp_hist.subdev.entity, 0,
-			MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap3isp_h3a_af_cleanup(isp);
 error_h3a_af:
 	omap3isp_h3a_aewb_cleanup(isp);
 error_h3a_aewb:
@@ -2149,6 +2218,8 @@ static int isp_remove(struct platform_device *pdev)
 	isp_detach_iommu(isp);
 	__omap3isp_put(isp, false);
 
+	media_entity_enum_cleanup(&isp->crashed);
+
 	return 0;
 }
 
@@ -2278,28 +2349,43 @@ static int isp_subdev_notifier_bound(struct v4l2_async_notifier *async,
 				     struct v4l2_subdev *subdev,
 				     struct v4l2_async_subdev *asd)
 {
-	struct isp_device *isp = container_of(async, struct isp_device,
-					      notifier);
 	struct isp_async_subdev *isd =
 		container_of(asd, struct isp_async_subdev, asd);
-	int ret;
-
-	ret = isp_link_entity(isp, &subdev->entity, isd->bus.interface);
-	if (ret < 0)
-		return ret;
 
 	isd->sd = subdev;
 	isd->sd->host_priv = &isd->bus;
 
-	return ret;
+	return 0;
 }
 
 static int isp_subdev_notifier_complete(struct v4l2_async_notifier *async)
 {
 	struct isp_device *isp = container_of(async, struct isp_device,
 					      notifier);
+	struct v4l2_device *v4l2_dev = &isp->v4l2_dev;
+	struct v4l2_subdev *sd;
+	struct isp_bus_cfg *bus;
+	int ret;
+
+	ret = media_entity_enum_init(&isp->crashed, &isp->media_dev);
+	if (ret)
+		return ret;
 
-	return v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+	list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
+		/* Only try to link entities whose interface was set on bound */
+		if (sd->host_priv) {
+			bus = (struct isp_bus_cfg *)sd->host_priv;
+			ret = isp_link_entity(isp, &sd->entity, bus->interface);
+			if (ret < 0)
+				return ret;
+		}
+	}
+
+	ret = v4l2_device_register_subdev_nodes(&isp->v4l2_dev);
+	if (ret < 0)
+		return ret;
+
+	return media_device_register(&isp->media_dev);
 }
 
 /*
@@ -2465,6 +2551,10 @@ static int isp_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto error_modules;
 
+	ret = isp_create_links(isp);
+	if (ret < 0)
+		goto error_register_entities;
+
 	isp->notifier.bound = isp_subdev_notifier_bound;
 	isp->notifier.complete = isp_subdev_notifier_complete;
 
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
index 5acc2e6511a5..49b7f71ac968 100644
--- a/drivers/media/platform/omap3isp/isp.h
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -17,6 +17,7 @@
 #ifndef OMAP3_ISP_CORE_H
 #define OMAP3_ISP_CORE_H
 
+#include <media/media-entity.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-device.h>
 #include <linux/clk-provider.h>
@@ -152,7 +153,7 @@ struct isp_xclk {
  * @stat_lock: Spinlock for handling statistics
  * @isp_mutex: Mutex for serializing requests to ISP.
  * @stop_failure: Indicates that an entity failed to stop.
- * @crashed: Bitmask of crashed entities (indexed by entity ID)
+ * @crashed: Crashed ent_enum
  * @has_context: Context has been saved at least once and can be restored.
  * @ref_count: Reference count for handling multiple ISP requests.
  * @cam_ick: Pointer to camera interface clock structure.
@@ -176,6 +177,7 @@ struct isp_device {
 	struct v4l2_device v4l2_dev;
 	struct v4l2_async_notifier notifier;
 	struct media_device media_dev;
+	struct media_entity_graph pm_count_graph;
 	struct device *dev;
 	u32 revision;
 
@@ -194,7 +196,7 @@ struct isp_device {
 	spinlock_t stat_lock;	/* common lock for statistic drivers */
 	struct mutex isp_mutex;	/* For handling ref_count field */
 	bool stop_failure;
-	u32 crashed;
+	struct media_entity_enum crashed;
 	int has_context;
 	int ref_count;
 	unsigned int autoidle;
@@ -265,7 +267,8 @@ void omap3isp_subclk_enable(struct isp_device *isp,
 void omap3isp_subclk_disable(struct isp_device *isp,
 			     enum isp_subclk_resource res);
 
-int omap3isp_pipeline_pm_use(struct media_entity *entity, int use);
+int omap3isp_pipeline_pm_use(struct media_entity *entity, int use,
+			     struct media_entity_graph *graph);
 
 int omap3isp_register_entities(struct platform_device *pdev,
 			       struct v4l2_device *v4l2_dev);
diff --git a/drivers/media/platform/omap3isp/ispccdc.c b/drivers/media/platform/omap3isp/ispccdc.c
index a6a61cce43dd..bb3974c98e37 100644
--- a/drivers/media/platform/omap3isp/ispccdc.c
+++ b/drivers/media/platform/omap3isp/ispccdc.c
@@ -1608,7 +1608,7 @@ static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
 	/* Wait for the CCDC to become idle. */
 	if (ccdc_sbl_wait_idle(ccdc, 1000)) {
 		dev_info(isp->dev, "CCDC won't become idle!\n");
-		isp->crashed |= 1U << ccdc->subdev.entity.id;
+		media_entity_enum_set(&isp->crashed, &ccdc->subdev.entity);
 		omap3isp_pipeline_cancel_stream(pipe);
 		return 0;
 	}
@@ -2513,9 +2513,14 @@ static int ccdc_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct isp_ccdc_device *ccdc = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = to_isp_device(ccdc);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case CCDC_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case CCDC_PAD_SINK | 2 << 16:
 		/* Read from the sensor (parallel interface), CCP2, CSI2a or
 		 * CSI2c.
 		 */
@@ -2543,7 +2548,7 @@ static int ccdc_link_setup(struct media_entity *entity,
 	 * Revisit this when it will be implemented, and return -EBUSY for now.
 	 */
 
-	case CCDC_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CCDC_PAD_SOURCE_VP | 2 << 16:
 		/* Write to preview engine, histogram and H3A. When none of
 		 * those links are active, the video port can be disabled.
 		 */
@@ -2556,7 +2561,7 @@ static int ccdc_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case CCDC_PAD_SOURCE_OF | MEDIA_ENT_T_DEVNODE:
+	case CCDC_PAD_SOURCE_OF:
 		/* Write to memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ccdc->output & ~CCDC_OUTPUT_MEMORY)
@@ -2567,7 +2572,7 @@ static int ccdc_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case CCDC_PAD_SOURCE_OF | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CCDC_PAD_SOURCE_OF | 2 << 16:
 		/* Write to resizer */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ccdc->output & ~CCDC_OUTPUT_RESIZER)
@@ -2650,7 +2655,7 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
 	pads[CCDC_PAD_SOURCE_OF].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &ccdc_media_ops;
-	ret = media_entity_init(me, CCDC_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, CCDC_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -2664,19 +2669,11 @@ static int ccdc_init_entities(struct isp_ccdc_device *ccdc)
 
 	ret = omap3isp_video_init(&ccdc->video_out, "CCDC");
 	if (ret < 0)
-		goto error_video;
-
-	/* Connect the CCDC subdev to the video node. */
-	ret = media_entity_create_link(&ccdc->subdev.entity, CCDC_PAD_SOURCE_OF,
-			&ccdc->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		goto error_link;
+		goto error;
 
 	return 0;
 
-error_link:
-	omap3isp_video_cleanup(&ccdc->video_out);
-error_video:
+error:
 	media_entity_cleanup(me);
 	return ret;
 }
diff --git a/drivers/media/platform/omap3isp/ispccp2.c b/drivers/media/platform/omap3isp/ispccp2.c
index 38e6a974c5b1..ca095238510d 100644
--- a/drivers/media/platform/omap3isp/ispccp2.c
+++ b/drivers/media/platform/omap3isp/ispccp2.c
@@ -956,9 +956,14 @@ static int ccp2_link_setup(struct media_entity *entity,
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct isp_ccp2_device *ccp2 = v4l2_get_subdevdata(sd);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case CCP2_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case CCP2_PAD_SINK:
 		/* read from memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ccp2->input == CCP2_INPUT_SENSOR)
@@ -970,7 +975,7 @@ static int ccp2_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case CCP2_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CCP2_PAD_SINK | 2 << 16:
 		/* read from sensor/phy */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ccp2->input == CCP2_INPUT_MEMORY)
@@ -981,7 +986,7 @@ static int ccp2_link_setup(struct media_entity *entity,
 				ccp2->input = CCP2_INPUT_NONE;
 		} break;
 
-	case CCP2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CCP2_PAD_SOURCE | 2 << 16:
 		/* write to video port/ccdc */
 		if (flags & MEDIA_LNK_FL_ENABLED)
 			ccp2->output = CCP2_OUTPUT_CCDC;
@@ -1071,7 +1076,7 @@ static int ccp2_init_entities(struct isp_ccp2_device *ccp2)
 	pads[CCP2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &ccp2_media_ops;
-	ret = media_entity_init(me, CCP2_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, CCP2_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -1097,19 +1102,11 @@ static int ccp2_init_entities(struct isp_ccp2_device *ccp2)
 
 	ret = omap3isp_video_init(&ccp2->video_in, "CCP2");
 	if (ret < 0)
-		goto error_video;
-
-	/* Connect the video node to the ccp2 subdev. */
-	ret = media_entity_create_link(&ccp2->video_in.video.entity, 0,
-				       &ccp2->subdev.entity, CCP2_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
+		goto error;
 
 	return 0;
 
-error_link:
-	omap3isp_video_cleanup(&ccp2->video_in);
-error_video:
+error:
 	media_entity_cleanup(&ccp2->subdev.entity);
 	return ret;
 }
diff --git a/drivers/media/platform/omap3isp/ispcsi2.c b/drivers/media/platform/omap3isp/ispcsi2.c
index a78338d012b4..f75a1be29d84 100644
--- a/drivers/media/platform/omap3isp/ispcsi2.c
+++ b/drivers/media/platform/omap3isp/ispcsi2.c
@@ -1144,14 +1144,19 @@ static int csi2_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct isp_csi2_device *csi2 = v4l2_get_subdevdata(sd);
 	struct isp_csi2_ctrl_cfg *ctrl = &csi2->ctrl;
+	unsigned int index = local->index;
 
 	/*
 	 * The ISP core doesn't support pipelines with multiple video outputs.
 	 * Revisit this when it will be implemented, and return -EBUSY for now.
 	 */
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case CSI2_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case CSI2_PAD_SOURCE:
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (csi2->output & ~CSI2_OUTPUT_MEMORY)
 				return -EBUSY;
@@ -1161,7 +1166,7 @@ static int csi2_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case CSI2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CSI2_PAD_SOURCE | 2 << 16:
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (csi2->output & ~CSI2_OUTPUT_CCDC)
 				return -EBUSY;
@@ -1245,7 +1250,7 @@ static int csi2_init_entities(struct isp_csi2_device *csi2)
 				    | MEDIA_PAD_FL_MUST_CONNECT;
 
 	me->ops = &csi2_media_ops;
-	ret = media_entity_init(me, CSI2_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, CSI2_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -1264,16 +1269,8 @@ static int csi2_init_entities(struct isp_csi2_device *csi2)
 	if (ret < 0)
 		goto error_video;
 
-	/* Connect the CSI2 subdev to the video node. */
-	ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
-				       &csi2->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap3isp_video_cleanup(&csi2->video_out);
 error_video:
 	media_entity_cleanup(&csi2->subdev.entity);
 	return ret;
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
index 13803270d104..84a96670e2e7 100644
--- a/drivers/media/platform/omap3isp/isppreview.c
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -2144,9 +2144,14 @@ static int preview_link_setup(struct media_entity *entity,
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct isp_prev_device *prev = v4l2_get_subdevdata(sd);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case PREV_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case PREV_PAD_SINK:
 		/* read from memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (prev->input == PREVIEW_INPUT_CCDC)
@@ -2158,7 +2163,7 @@ static int preview_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case PREV_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	case PREV_PAD_SINK | 2 << 16:
 		/* read from ccdc */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (prev->input == PREVIEW_INPUT_MEMORY)
@@ -2175,7 +2180,7 @@ static int preview_link_setup(struct media_entity *entity,
 	 * Revisit this when it will be implemented, and return -EBUSY for now.
 	 */
 
-	case PREV_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+	case PREV_PAD_SOURCE:
 		/* write to memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (prev->output & ~PREVIEW_OUTPUT_MEMORY)
@@ -2186,7 +2191,7 @@ static int preview_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case PREV_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case PREV_PAD_SOURCE | 2 << 16:
 		/* write to resizer */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (prev->output & ~PREVIEW_OUTPUT_RESIZER)
@@ -2282,7 +2287,7 @@ static int preview_init_entities(struct isp_prev_device *prev)
 	pads[PREV_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &preview_media_ops;
-	ret = media_entity_init(me, PREV_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, PREV_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -2311,21 +2316,8 @@ static int preview_init_entities(struct isp_prev_device *prev)
 	if (ret < 0)
 		goto error_video_out;
 
-	/* Connect the video nodes to the previewer subdev. */
-	ret = media_entity_create_link(&prev->video_in.video.entity, 0,
-			&prev->subdev.entity, PREV_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(&prev->subdev.entity, PREV_PAD_SOURCE,
-			&prev->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap3isp_video_cleanup(&prev->video_out);
 error_video_out:
 	omap3isp_video_cleanup(&prev->video_in);
 error_video_in:
diff --git a/drivers/media/platform/omap3isp/ispresizer.c b/drivers/media/platform/omap3isp/ispresizer.c
index 7cfb43dc0ffd..0b6a87508584 100644
--- a/drivers/media/platform/omap3isp/ispresizer.c
+++ b/drivers/media/platform/omap3isp/ispresizer.c
@@ -1623,9 +1623,14 @@ static int resizer_link_setup(struct media_entity *entity,
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct isp_res_device *res = v4l2_get_subdevdata(sd);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case RESZ_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case RESZ_PAD_SINK:
 		/* read from memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (res->input == RESIZER_INPUT_VP)
@@ -1637,7 +1642,7 @@ static int resizer_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case RESZ_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	case RESZ_PAD_SINK | 2 << 16:
 		/* read from ccdc or previewer */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (res->input == RESIZER_INPUT_MEMORY)
@@ -1649,7 +1654,7 @@ static int resizer_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case RESZ_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+	case RESZ_PAD_SOURCE:
 		/* resizer always write to memory */
 		break;
 
@@ -1728,7 +1733,7 @@ static int resizer_init_entities(struct isp_res_device *res)
 	pads[RESZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &resizer_media_ops;
-	ret = media_entity_init(me, RESZ_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, RESZ_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -1755,21 +1760,8 @@ static int resizer_init_entities(struct isp_res_device *res)
 
 	res->video_out.video.entity.flags |= MEDIA_ENT_FL_DEFAULT;
 
-	/* Connect the video nodes to the resizer subdev. */
-	ret = media_entity_create_link(&res->video_in.video.entity, 0,
-			&res->subdev.entity, RESZ_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(&res->subdev.entity, RESZ_PAD_SOURCE,
-			&res->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap3isp_video_cleanup(&res->video_out);
 error_video_out:
 	omap3isp_video_cleanup(&res->video_in);
 error_video_in:
diff --git a/drivers/media/platform/omap3isp/ispstat.c b/drivers/media/platform/omap3isp/ispstat.c
index 94d4c295d3d0..1b9217d3b1b6 100644
--- a/drivers/media/platform/omap3isp/ispstat.c
+++ b/drivers/media/platform/omap3isp/ispstat.c
@@ -1028,7 +1028,7 @@ static int isp_stat_init_entities(struct ispstat *stat, const char *name,
 	stat->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
 	me->ops = NULL;
 
-	return media_entity_init(me, 1, &stat->pad, 0);
+	return media_entity_pads_init(me, 1, &stat->pad);
 }
 
 int omap3isp_stat_init(struct ispstat *stat, const char *name,
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
index f4f591652432..994dfc0813f6 100644
--- a/drivers/media/platform/omap3isp/ispvideo.c
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -210,8 +210,7 @@ isp_video_remote_subdev(struct isp_video *video, u32 *pad)
 
 	remote = media_entity_remote_pad(&video->pad);
 
-	if (remote == NULL ||
-	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 		return NULL;
 
 	if (pad)
@@ -226,16 +225,23 @@ static int isp_video_get_graph_data(struct isp_video *video,
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 	struct isp_video *far_end = NULL;
+	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
+	ret = media_entity_graph_walk_init(&graph, entity->graph_obj.mdev);
+	if (ret) {
+		mutex_unlock(&mdev->graph_mutex);
+		return ret;
+	}
+
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
 		struct isp_video *__video;
 
-		pipe->entities |= 1 << entity->id;
+		media_entity_enum_set(&pipe->ent_enum, entity);
 
 		if (far_end != NULL)
 			continue;
@@ -243,7 +249,7 @@ static int isp_video_get_graph_data(struct isp_video *video,
 		if (entity == &video->video.entity)
 			continue;
 
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+		if (!is_media_entity_v4l2_io(entity))
 			continue;
 
 		__video = to_isp_video(media_entity_to_video_device(entity));
@@ -253,6 +259,8 @@ static int isp_video_get_graph_data(struct isp_video *video,
 
 	mutex_unlock(&mdev->graph_mutex);
 
+	media_entity_graph_walk_cleanup(&graph);
+
 	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
 		pipe->input = far_end;
 		pipe->output = video;
@@ -320,7 +328,6 @@ isp_video_check_format(struct isp_video *video, struct isp_video_fh *vfh)
  */
 
 static int isp_video_queue_setup(struct vb2_queue *queue,
-				 const void *parg,
 				 unsigned int *count, unsigned int *num_planes,
 				 unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -467,7 +474,7 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
 	list_del(&buf->irqlist);
 	spin_unlock_irqrestore(&video->irqlock, flags);
 
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 
 	/* Do frame number propagation only if this is the output video node.
 	 * Frame number either comes from the CSI receivers or it gets
@@ -901,7 +908,7 @@ static int isp_video_check_external_subdevs(struct isp_video *video,
 
 	for (i = 0; i < ARRAY_SIZE(ents); i++) {
 		/* Is the entity part of the pipeline? */
-		if (!(pipe->entities & (1 << ents[i]->id)))
+		if (!media_entity_enum_test(&pipe->ent_enum, ents[i]))
 			continue;
 
 		/* ISP entities have always sink pad == 0. Find source. */
@@ -919,7 +926,7 @@ static int isp_video_check_external_subdevs(struct isp_video *video,
 		return -EINVAL;
 	}
 
-	if (media_entity_type(source) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!is_media_entity_v4l2_subdev(source))
 		return 0;
 
 	pipe->external = media_entity_to_v4l2_subdev(source);
@@ -953,7 +960,8 @@ static int isp_video_check_external_subdevs(struct isp_video *video,
 
 	pipe->external_rate = ctrl.value64;
 
-	if (pipe->entities & (1 << isp->isp_ccdc.subdev.entity.id)) {
+	if (media_entity_enum_test(&pipe->ent_enum,
+				   &isp->isp_ccdc.subdev.entity)) {
 		unsigned int rate = UINT_MAX;
 		/*
 		 * Check that maximum allowed CCDC pixel rate isn't
@@ -1019,7 +1027,9 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	pipe = video->video.entity.pipe
 	     ? to_isp_pipeline(&video->video.entity) : &video->pipe;
 
-	pipe->entities = 0;
+	ret = media_entity_enum_init(&pipe->ent_enum, &video->isp->media_dev);
+	if (ret)
+		goto err_enum_init;
 
 	/* TODO: Implement PM QoS */
 	pipe->l3_ick = clk_get_rate(video->isp->clock[ISP_CLK_L3_ICK]);
@@ -1093,6 +1103,7 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	}
 
 	mutex_unlock(&video->stream_lock);
+
 	return 0;
 
 err_set_stream:
@@ -1113,7 +1124,11 @@ err_pipeline_start:
 	INIT_LIST_HEAD(&video->dmaqueue);
 	video->queue = NULL;
 
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
+err_enum_init:
 	mutex_unlock(&video->stream_lock);
+
 	return ret;
 }
 
@@ -1165,6 +1180,8 @@ isp_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 	/* TODO: Implement PM QoS */
 	media_entity_pipeline_stop(&video->video.entity);
 
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
 done:
 	mutex_unlock(&video->stream_lock);
 	return 0;
@@ -1244,7 +1261,12 @@ static int isp_video_open(struct file *file)
 		goto done;
 	}
 
-	ret = omap3isp_pipeline_pm_use(&video->video.entity, 1);
+	ret = media_entity_graph_walk_init(&handle->graph,
+					   &video->isp->media_dev);
+	if (ret)
+		goto done;
+
+	ret = omap3isp_pipeline_pm_use(&video->video.entity, 1, &handle->graph);
 	if (ret < 0) {
 		omap3isp_put(video->isp);
 		goto done;
@@ -1275,6 +1297,7 @@ static int isp_video_open(struct file *file)
 done:
 	if (ret < 0) {
 		v4l2_fh_del(&handle->vfh);
+		media_entity_graph_walk_cleanup(&handle->graph);
 		kfree(handle);
 	}
 
@@ -1294,7 +1317,8 @@ static int isp_video_release(struct file *file)
 	vb2_queue_release(&handle->queue);
 	mutex_unlock(&video->queue_lock);
 
-	omap3isp_pipeline_pm_use(&video->video.entity, 0);
+	omap3isp_pipeline_pm_use(&video->video.entity, 0, &handle->graph);
+	media_entity_graph_walk_cleanup(&handle->graph);
 
 	/* Release the file handle. */
 	v4l2_fh_del(vfh);
@@ -1368,7 +1392,7 @@ int omap3isp_video_init(struct isp_video *video, const char *name)
 	if (IS_ERR(video->alloc_ctx))
 		return PTR_ERR(video->alloc_ctx);
 
-	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	ret = media_entity_pads_init(&video->video.entity, 1, &video->pad);
 	if (ret < 0) {
 		vb2_dma_contig_cleanup_ctx(video->alloc_ctx);
 		return ret;
diff --git a/drivers/media/platform/omap3isp/ispvideo.h b/drivers/media/platform/omap3isp/ispvideo.h
index bcf0e0acc8f3..156429878d64 100644
--- a/drivers/media/platform/omap3isp/ispvideo.h
+++ b/drivers/media/platform/omap3isp/ispvideo.h
@@ -80,7 +80,7 @@ enum isp_pipeline_state {
  * struct isp_pipeline - An ISP hardware pipeline
  * @field: The field being processed by the pipeline
  * @error: A hardware error occurred during capture
- * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
+ * @ent_enum: Entities in the pipeline
  */
 struct isp_pipeline {
 	struct media_pipeline pipe;
@@ -89,7 +89,7 @@ struct isp_pipeline {
 	enum isp_pipeline_stream_state stream_state;
 	struct isp_video *input;
 	struct isp_video *output;
-	u32 entities;
+	struct media_entity_enum ent_enum;
 	unsigned long l3_ick;
 	unsigned int max_rate;
 	enum v4l2_field field;
@@ -189,6 +189,7 @@ struct isp_video_fh {
 	struct vb2_queue queue;
 	struct v4l2_format format;
 	struct v4l2_fract timeperframe;
+	struct media_entity_graph graph;
 };
 
 #define to_isp_video_fh(fh)	container_of(fh, struct isp_video_fh, vfh)
diff --git a/drivers/media/platform/rcar_jpu.c b/drivers/media/platform/rcar_jpu.c
index f8e3e83c52a2..485f5259acb0 100644
--- a/drivers/media/platform/rcar_jpu.c
+++ b/drivers/media/platform/rcar_jpu.c
@@ -1015,28 +1015,33 @@ error_free:
  * ============================================================================
  */
 static int jpu_queue_setup(struct vb2_queue *vq,
-			   const void *parg,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct jpu_ctx *ctx = vb2_get_drv_priv(vq);
 	struct jpu_q_data *q_data;
 	unsigned int i;
 
 	q_data = jpu_get_q_data(ctx, vq->type);
 
-	*nplanes = q_data->format.num_planes;
+	if (*nplanes) {
+		if (*nplanes != q_data->format.num_planes)
+			return -EINVAL;
 
-	for (i = 0; i < *nplanes; i++) {
-		unsigned int q_size = q_data->format.plane_fmt[i].sizeimage;
-		unsigned int f_size = fmt ?
-			fmt->fmt.pix_mp.plane_fmt[i].sizeimage : 0;
+		for (i = 0; i < *nplanes; i++) {
+			unsigned int q_size = q_data->format.plane_fmt[i].sizeimage;
 
-		if (fmt && f_size < q_size)
-			return -EINVAL;
+			if (sizes[i] < q_size)
+				return -EINVAL;
+			alloc_ctxs[i] = ctx->jpu->alloc_ctx;
+		}
+		return 0;
+	}
 
-		sizes[i] = fmt ? f_size : q_size;
+	*nplanes = q_data->format.num_planes;
+
+	for (i = 0; i < *nplanes; i++) {
+		sizes[i] = q_data->format.plane_fmt[i].sizeimage;
 		alloc_ctxs[i] = ctx->jpu->alloc_ctx;
 	}
 
@@ -1300,17 +1305,17 @@ static int jpu_release(struct file *file)
 	struct jpu *jpu = video_drvdata(file);
 	struct jpu_ctx *ctx = fh_to_ctx(file->private_data);
 
-	mutex_lock(&jpu->mutex);
-	if (--jpu->ref_count == 0)
-		clk_disable_unprepare(jpu->clk);
-	mutex_unlock(&jpu->mutex);
-
 	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);
 	kfree(ctx);
 
+	mutex_lock(&jpu->mutex);
+	if (--jpu->ref_count == 0)
+		clk_disable_unprepare(jpu->clk);
+	mutex_unlock(&jpu->mutex);
+
 	return 0;
 }
 
@@ -1560,12 +1565,9 @@ static irqreturn_t jpu_irq_handler(int irq, void *dev_id)
 		}
 
 		dst_buf->field = src_buf->field;
-		dst_buf->timestamp = src_buf->timestamp;
+		dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 		if (src_buf->flags & V4L2_BUF_FLAG_TIMECODE)
 			dst_buf->timecode = src_buf->timecode;
-		dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
-		dst_buf->flags |= src_buf->flags &
-					V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 		dst_buf->flags = src_buf->flags &
 			(V4L2_BUF_FLAG_TIMECODE | V4L2_BUF_FLAG_KEYFRAME |
 			 V4L2_BUF_FLAG_PFRAME | V4L2_BUF_FLAG_BFRAME |
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index 537b858cb94a..bd060ef5d1e1 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -338,7 +338,7 @@ irqreturn_t s3c_camif_irq_handler(int irq, void *priv)
 
 		if (!WARN_ON(vbuf == NULL)) {
 			/* Dequeue a filled buffer */
-			v4l2_get_timestamp(&vbuf->vb.timestamp);
+			vbuf->vb.vb2_buf.timestamp = ktime_get_ns();
 			vbuf->vb.sequence = vp->frame_sequence++;
 			vb2_buffer_done(&vbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 
@@ -435,39 +435,28 @@ static void stop_streaming(struct vb2_queue *vq)
 	camif_stop_capture(vp);
 }
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *num_buffers, unsigned int *num_planes,
 		       unsigned int sizes[], void *allocators[])
 {
-	const struct v4l2_format *pfmt = parg;
-	const struct v4l2_pix_format *pix = NULL;
 	struct camif_vp *vp = vb2_get_drv_priv(vq);
 	struct camif_dev *camif = vp->camif;
 	struct camif_frame *frame = &vp->out_frame;
-	const struct camif_fmt *fmt;
+	const struct camif_fmt *fmt = vp->out_fmt;
 	unsigned int size;
 
-	if (pfmt) {
-		pix = &pfmt->fmt.pix;
-		fmt = s3c_camif_find_format(vp, &pix->pixelformat, -1);
-		if (fmt == NULL)
-			return -EINVAL;
-		size = (pix->width * pix->height * fmt->depth) / 8;
-	} else {
-		fmt = vp->out_fmt;
-		if (fmt == NULL)
-			return -EINVAL;
-		size = (frame->f_width * frame->f_height * fmt->depth) / 8;
-	}
-
-	*num_planes = 1;
+	if (fmt == NULL)
+		return -EINVAL;
 
-	if (pix)
-		sizes[0] = max(size, pix->sizeimage);
-	else
-		sizes[0] = size;
+	size = (frame->f_width * frame->f_height * fmt->depth) / 8;
 	allocators[0] = camif->alloc_ctx;
 
+	if (*num_planes)
+		return sizes[0] < size ? -EINVAL : 0;
+
+	*num_planes = 1;
+	sizes[0] = size;
+
 	pr_debug("size: %u\n", sizes[0]);
 	return 0;
 }
@@ -833,7 +822,7 @@ static int camif_pipeline_validate(struct camif_dev *camif)
 
 	/* Retrieve format at the sensor subdev source pad */
 	pad = media_entity_remote_pad(&camif->pads[0]);
-	if (!pad || media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 		return -EPIPE;
 
 	src_fmt.pad = pad->index;
@@ -1155,7 +1144,7 @@ int s3c_camif_register_video_node(struct camif_dev *camif, int idx)
 		goto err_vd_rel;
 
 	vp->pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&vfd->entity, 1, &vp->pad, 0);
+	ret = media_entity_pads_init(&vfd->entity, 1, &vp->pad);
 	if (ret)
 		goto err_vd_rel;
 
@@ -1570,8 +1559,8 @@ int s3c_camif_create_subdev(struct camif_dev *camif)
 	camif->pads[CAMIF_SD_PAD_SOURCE_C].flags = MEDIA_PAD_FL_SOURCE;
 	camif->pads[CAMIF_SD_PAD_SOURCE_P].flags = MEDIA_PAD_FL_SOURCE;
 
-	ret = media_entity_init(&sd->entity, CAMIF_SD_PADS_NUM,
-				camif->pads, 0);
+	ret = media_entity_pads_init(&sd->entity, CAMIF_SD_PADS_NUM,
+				camif->pads);
 	if (ret)
 		return ret;
 
diff --git a/drivers/media/platform/s3c-camif/camif-core.c b/drivers/media/platform/s3c-camif/camif-core.c
index 1ba9bb08f5da..0b44b9accf50 100644
--- a/drivers/media/platform/s3c-camif/camif-core.c
+++ b/drivers/media/platform/s3c-camif/camif-core.c
@@ -263,7 +263,7 @@ static int camif_create_media_links(struct camif_dev *camif)
 {
 	int i, ret;
 
-	ret = media_entity_create_link(&camif->sensor.sd->entity, 0,
+	ret = media_create_pad_link(&camif->sensor.sd->entity, 0,
 				&camif->subdev.entity, CAMIF_SD_PAD_SINK,
 				MEDIA_LNK_FL_IMMUTABLE |
 				MEDIA_LNK_FL_ENABLED);
@@ -271,7 +271,7 @@ static int camif_create_media_links(struct camif_dev *camif)
 		return ret;
 
 	for (i = 1; i < CAMIF_SD_PADS_NUM && !ret; i++) {
-		ret = media_entity_create_link(&camif->subdev.entity, i,
+		ret = media_create_pad_link(&camif->subdev.entity, i,
 				&camif->vp[i - 1].vdev.entity, 0,
 				MEDIA_LNK_FL_IMMUTABLE |
 				MEDIA_LNK_FL_ENABLED);
@@ -305,7 +305,7 @@ static void camif_unregister_media_entities(struct camif_dev *camif)
 /*
  * Media device
  */
-static int camif_media_dev_register(struct camif_dev *camif)
+static int camif_media_dev_init(struct camif_dev *camif)
 {
 	struct media_device *md = &camif->media_dev;
 	struct v4l2_device *v4l2_dev = &camif->v4l2_dev;
@@ -324,14 +324,12 @@ static int camif_media_dev_register(struct camif_dev *camif)
 	strlcpy(v4l2_dev->name, "s3c-camif", sizeof(v4l2_dev->name));
 	v4l2_dev->mdev = md;
 
+	media_device_init(md);
+
 	ret = v4l2_device_register(camif->dev, v4l2_dev);
 	if (ret < 0)
 		return ret;
 
-	ret = media_device_register(md);
-	if (ret < 0)
-		v4l2_device_unregister(v4l2_dev);
-
 	return ret;
 }
 
@@ -483,7 +481,7 @@ static int s3c_camif_probe(struct platform_device *pdev)
 		goto err_alloc;
 	}
 
-	ret = camif_media_dev_register(camif);
+	ret = camif_media_dev_init(camif);
 	if (ret < 0)
 		goto err_mdev;
 
@@ -510,6 +508,11 @@ static int s3c_camif_probe(struct platform_device *pdev)
 		goto err_unlock;
 
 	mutex_unlock(&camif->media_dev.graph_mutex);
+
+	ret = media_device_register(&camif->media_dev);
+	if (ret < 0)
+		goto err_sens;
+
 	pm_runtime_put(dev);
 	return 0;
 
@@ -518,6 +521,7 @@ err_unlock:
 err_sens:
 	v4l2_device_unregister(&camif->v4l2_dev);
 	media_device_unregister(&camif->media_dev);
+	media_device_cleanup(&camif->media_dev);
 	camif_unregister_media_entities(camif);
 err_mdev:
 	vb2_dma_contig_cleanup_ctx(camif->alloc_ctx);
@@ -539,6 +543,7 @@ static int s3c_camif_remove(struct platform_device *pdev)
 	struct s3c_camif_plat_data *pdata = &camif->pdata;
 
 	media_device_unregister(&camif->media_dev);
+	media_device_cleanup(&camif->media_dev);
 	camif_unregister_media_entities(camif);
 	v4l2_device_unregister(&camif->v4l2_dev);
 
diff --git a/drivers/media/platform/s3c-camif/camif-core.h b/drivers/media/platform/s3c-camif/camif-core.h
index adaf1969ef63..57cbc3d9725d 100644
--- a/drivers/media/platform/s3c-camif/camif-core.h
+++ b/drivers/media/platform/s3c-camif/camif-core.h
@@ -26,7 +26,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
 #include <media/videobuf2-v4l2.h>
-#include <media/s3c_camif.h>
+#include <media/drv-intf/s3c_camif.h>
 
 #define S3C_CAMIF_DRIVER_NAME	"s3c-camif"
 #define CAMIF_REQ_BUFS_MIN	3
diff --git a/drivers/media/platform/s3c-camif/camif-regs.h b/drivers/media/platform/s3c-camif/camif-regs.h
index af2d472ea1dd..5ad36c1c2a5d 100644
--- a/drivers/media/platform/s3c-camif/camif-regs.h
+++ b/drivers/media/platform/s3c-camif/camif-regs.h
@@ -13,7 +13,7 @@
 #define CAMIF_REGS_H_
 
 #include "camif-core.h"
-#include <media/s3c_camif.h>
+#include <media/drv-intf/s3c_camif.h>
 
 /*
  * The id argument indicates the processing path:
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index e1936d9d27da..74bd46ca7942 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -101,7 +101,7 @@ static struct g2d_frame *get_frame(struct g2d_ctx *ctx,
 	}
 }
 
-static int g2d_queue_setup(struct vb2_queue *vq, const void *parg,
+static int g2d_queue_setup(struct vb2_queue *vq,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -552,7 +552,7 @@ static irqreturn_t g2d_isr(int irq, void *prv)
 	BUG_ON(dst == NULL);
 
 	dst->timecode = src->timecode;
-	dst->timestamp = src->timestamp;
+	dst->vb2_buf.timestamp = src->vb2_buf.timestamp;
 	dst->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst->flags |=
 		src->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index 4a608cbe0fdb..c3b13a630edf 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -2430,7 +2430,6 @@ static struct v4l2_m2m_ops exynos4_jpeg_m2m_ops = {
  */
 
 static int s5p_jpeg_queue_setup(struct vb2_queue *vq,
-			   const void *parg,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -2621,7 +2620,7 @@ static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
 	}
 
 	dst_buf->timecode = src_buf->timecode;
-	dst_buf->timestamp = src_buf->timestamp;
+	dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 	dst_buf->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst_buf->flags |=
 		src_buf->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
@@ -2752,7 +2751,7 @@ static irqreturn_t exynos3250_jpeg_irq(int irq, void *dev_id)
 	dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
 
 	dst_buf->timecode = src_buf->timecode;
-	dst_buf->timestamp = src_buf->timestamp;
+	dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 
 	v4l2_m2m_buf_done(src_buf, state);
 	if (curr_ctx->mode == S5P_JPEG_ENCODE)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index 3ffe2ecfd5ef..927ab4928779 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -85,6 +85,26 @@ void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx)
 	spin_unlock_irqrestore(&dev->condlock, flags);
 }
 
+int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
+{
+	unsigned long flags;
+	int ctx;
+
+	spin_lock_irqsave(&dev->condlock, flags);
+	ctx = dev->curr_ctx;
+	do {
+		ctx = (ctx + 1) % MFC_NUM_CONTEXTS;
+		if (ctx == dev->curr_ctx) {
+			if (!test_bit(ctx, &dev->ctx_work_bits))
+				ctx = -EAGAIN;
+			break;
+		}
+	} while (!test_bit(ctx, &dev->ctx_work_bits));
+	spin_unlock_irqrestore(&dev->condlock, flags);
+
+	return ctx;
+}
+
 /* Wake up context wait_queue */
 static void wake_up_ctx(struct s5p_mfc_ctx *ctx, unsigned int reason,
 			unsigned int err)
@@ -105,6 +125,20 @@ static void wake_up_dev(struct s5p_mfc_dev *dev, unsigned int reason,
 	wake_up(&dev->queue);
 }
 
+void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq)
+{
+	struct s5p_mfc_buf *b;
+	int i;
+
+	while (!list_empty(lh)) {
+		b = list_entry(lh->next, struct s5p_mfc_buf, list);
+		for (i = 0; i < b->b->vb2_buf.num_planes; i++)
+			vb2_set_plane_payload(&b->b->vb2_buf, i, 0);
+		vb2_buffer_done(&b->b->vb2_buf, VB2_BUF_STATE_ERROR);
+		list_del(&b->list);
+	}
+}
+
 static void s5p_mfc_watchdog(unsigned long arg)
 {
 	struct s5p_mfc_dev *dev = (struct s5p_mfc_dev *)arg;
@@ -150,10 +184,8 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
 		if (!ctx)
 			continue;
 		ctx->state = MFCINST_ERROR;
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->dst_queue, &ctx->vq_dst);
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->src_queue, &ctx->vq_src);
+		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
 		clear_work_bit(ctx);
 		wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
 	}
@@ -233,8 +265,8 @@ static void s5p_mfc_handle_frame_copy_time(struct s5p_mfc_ctx *ctx)
 				== dec_y_addr) {
 			dst_buf->b->timecode =
 						src_buf->b->timecode;
-			dst_buf->b->timestamp =
-						src_buf->b->timestamp;
+			dst_buf->b->vb2_buf.timestamp =
+						src_buf->b->vb2_buf.timestamp;
 			dst_buf->b->flags &=
 				~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 			dst_buf->b->flags |=
@@ -327,7 +359,6 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 	unsigned int dst_frame_status;
 	unsigned int dec_frame_status;
 	struct s5p_mfc_buf *src_buf;
-	unsigned long flags;
 	unsigned int res_change;
 
 	dst_frame_status = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_status, dev)
@@ -343,17 +374,16 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 	if (res_change == S5P_FIMV_RES_INCREASE ||
 		res_change == S5P_FIMV_RES_DECREASE) {
 		ctx->state = MFCINST_RES_CHANGE_INIT;
-		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_ctx(ctx, reason, err);
 		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
 		s5p_mfc_clock_off();
-		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		return;
 	}
 	if (ctx->dpb_flush_flag)
 		ctx->dpb_flush_flag = 0;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	/* All frames remaining in the buffer have been extracted  */
 	if (dst_frame_status == S5P_FIMV_DEC_STATUS_DECODING_EMPTY) {
 		if (ctx->state == MFCINST_RES_CHANGE_FLUSH) {
@@ -413,11 +443,10 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 		}
 	}
 leave_handle_frame:
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
 				    || ctx->dst_queue_cnt < ctx->pb_count)
 		clear_work_bit(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	wake_up_ctx(ctx, reason, err);
 	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
 	s5p_mfc_clock_off();
@@ -425,15 +454,13 @@ leave_handle_frame:
 	if (test_bit(0, &dev->enter_suspend))
 		wake_up_dev(dev, reason, err);
 	else
-		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Error handling for interrupt */
 static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 		struct s5p_mfc_ctx *ctx, unsigned int reason, unsigned int err)
 {
-	unsigned long flags;
-
 	mfc_err("Interrupt Error: %08x\n", err);
 
 	if (ctx != NULL) {
@@ -450,13 +477,9 @@ static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 			clear_work_bit(ctx);
 			ctx->state = MFCINST_ERROR;
 			/* Mark all dst buffers as having an error */
-			spin_lock_irqsave(&dev->irqlock, flags);
-			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->dst_queue, &ctx->vq_dst);
+			s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
 			/* Mark all src buffers as having an error */
-			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->src_queue, &ctx->vq_src);
-			spin_unlock_irqrestore(&dev->irqlock, flags);
+			s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
 			wake_up_ctx(ctx, reason, err);
 			break;
 		default:
@@ -467,7 +490,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 		}
 	}
 	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
-	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	s5p_mfc_clock_off();
 	wake_up_dev(dev, reason, err);
 	return;
@@ -491,7 +514,7 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 		ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
 				dev);
 
-		s5p_mfc_hw_call_void(dev->mfc_ops, dec_calc_dpb_size, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, dec_calc_dpb_size, ctx);
 
 		ctx->pb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
 				dev);
@@ -518,11 +541,11 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 			ctx->head_processed = 1;
 		}
 	}
-	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	clear_work_bit(ctx);
 	WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
 	s5p_mfc_clock_off();
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	wake_up_ctx(ctx, reason, err);
 }
 
@@ -532,12 +555,11 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 {
 	struct s5p_mfc_buf *src_buf;
 	struct s5p_mfc_dev *dev;
-	unsigned long flags;
 
 	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
-	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
@@ -545,7 +567,6 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 	if (err == 0) {
 		ctx->state = MFCINST_RUNNING;
 		if (!ctx->dpb_flush_flag && ctx->head_processed) {
-			spin_lock_irqsave(&dev->irqlock, flags);
 			if (!list_empty(&ctx->src_queue)) {
 				src_buf = list_entry(ctx->src_queue.next,
 					     struct s5p_mfc_buf, list);
@@ -554,7 +575,6 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 				vb2_buffer_done(&src_buf->b->vb2_buf,
 						VB2_BUF_STATE_DONE);
 			}
-			spin_unlock_irqrestore(&dev->irqlock, flags);
 		} else {
 			ctx->dpb_flush_flag = 0;
 		}
@@ -563,7 +583,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 		s5p_mfc_clock_off();
 
 		wake_up(&ctx->queue);
-		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	} else {
 		WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
 
@@ -582,7 +602,6 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx)
 
 	ctx->state = MFCINST_FINISHED;
 
-	spin_lock(&dev->irqlock);
 	if (!list_empty(&ctx->dst_queue)) {
 		mb_entry = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf,
 									list);
@@ -591,7 +610,6 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx)
 		vb2_set_plane_payload(&mb_entry->b->vb2_buf, 0, 0);
 		vb2_buffer_done(&mb_entry->b->vb2_buf, VB2_BUF_STATE_DONE);
 	}
-	spin_unlock(&dev->irqlock);
 
 	clear_work_bit(ctx);
 
@@ -599,7 +617,7 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx)
 
 	s5p_mfc_clock_off();
 	wake_up(&ctx->queue);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 /* Interrupt processing */
@@ -613,6 +631,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 	mfc_debug_enter();
 	/* Reset the timeout watchdog */
 	atomic_set(&dev->watchdog_cnt, 0);
+	spin_lock(&dev->irqlock);
 	ctx = dev->ctx[dev->curr_ctx];
 	/* Get the reason of interrupt and the error code */
 	reason = s5p_mfc_hw_call(dev->mfc_ops, get_int_reason, dev);
@@ -639,15 +658,15 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 
 			if (ctx->state == MFCINST_FINISHING &&
 						list_empty(&ctx->ref_queue)) {
-				s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+				s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 				s5p_mfc_handle_stream_complete(ctx);
 				break;
 			}
-			s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+			s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 			wake_up_ctx(ctx, reason, err);
 			WARN_ON(test_and_clear_bit(0, &dev->hw_lock) == 0);
 			s5p_mfc_clock_off();
-			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		} else {
 			s5p_mfc_handle_frame(ctx, reason, err);
 		}
@@ -677,7 +696,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 	case S5P_MFC_R2H_CMD_WAKEUP_RET:
 		if (ctx)
 			clear_work_bit(ctx);
-		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_dev(dev, reason, err);
 		clear_bit(0, &dev->hw_lock);
 		clear_bit(0, &dev->enter_suspend);
@@ -688,7 +707,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 		break;
 
 	case S5P_MFC_R2H_CMD_COMPLETE_SEQ_RET:
-		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 		ctx->int_type = reason;
 		ctx->int_err = err;
 		s5p_mfc_handle_stream_complete(ctx);
@@ -702,12 +721,13 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 
 	default:
 		mfc_debug(2, "Unknown int reason\n");
-		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	}
+	spin_unlock(&dev->irqlock);
 	mfc_debug_leave();
 	return IRQ_HANDLED;
 irq_cleanup_hw:
-	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
@@ -716,7 +736,8 @@ irq_cleanup_hw:
 
 	s5p_mfc_clock_off();
 
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	spin_unlock(&dev->irqlock);
 	mfc_debug(2, "Exit via irq_cleanup_hw\n");
 	return IRQ_HANDLED;
 }
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index d1a3f9b1bc44..9eb2481ec292 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -308,7 +308,7 @@ struct s5p_mfc_dev {
 	struct s5p_mfc_pm	pm;
 	struct s5p_mfc_variant	*variant;
 	int num_inst;
-	spinlock_t irqlock;	/* lock when operating on videobuf2 queues */
+	spinlock_t irqlock;	/* lock when operating on context */
 	spinlock_t condlock;	/* lock when changing/checking if a context is
 					ready to be processed */
 	struct mutex mfc_mutex; /* video_device lock */
@@ -653,7 +653,7 @@ struct s5p_mfc_ctx {
 		unsigned int bits;
 	} slice_size;
 
-	struct s5p_mfc_codec_ops *c_ops;
+	const struct s5p_mfc_codec_ops *c_ops;
 
 	struct v4l2_ctrl *ctrls[MFC_MAX_CTRLS];
 	struct v4l2_ctrl_handler ctrl_handler;
@@ -694,13 +694,7 @@ struct mfc_control {
 
 /* Macro for making hardware specific calls */
 #define s5p_mfc_hw_call(f, op, args...) \
-	((f && f->op) ? f->op(args) : -ENODEV)
-
-#define s5p_mfc_hw_call_void(f, op, args...) \
-do { \
-	if (f && f->op) \
-		f->op(args); \
-} while (0)
+	((f && f->op) ? f->op(args) : (typeof(f->op(args)))(-ENODEV))
 
 #define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
 #define ctrl_to_ctx(__ctrl) \
@@ -710,6 +704,8 @@ void clear_work_bit(struct s5p_mfc_ctx *ctx);
 void set_work_bit(struct s5p_mfc_ctx *ctx);
 void clear_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
 void set_work_bit_irqsave(struct s5p_mfc_ctx *ctx);
+int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev);
+void s5p_mfc_cleanup_queue(struct list_head *lh, struct vb2_queue *vq);
 
 #define HAS_PORTNUM(dev)	(dev ? (dev->variant ? \
 				(dev->variant->port_num ? 1 : 0) : 0) : 0)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index 40d8a03a141d..cc888713b3b6 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -319,7 +319,7 @@ void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
 	s5p_mfc_clock_on();
 
 	s5p_mfc_reset(dev);
-	s5p_mfc_hw_call_void(dev->mfc_ops, release_dev_context_buffer, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, release_dev_context_buffer, dev);
 
 	s5p_mfc_clock_off();
 }
@@ -468,7 +468,7 @@ int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 	}
 
 	set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	if (s5p_mfc_wait_for_done_ctx(ctx,
 		S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
 		/* Error or timeout */
@@ -482,9 +482,9 @@ int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 
 err_free_desc_buf:
 	if (ctx->type == MFCINST_DECODER)
-		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer, ctx);
 err_free_inst_buf:
-	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
+	s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
 err:
 	return ret;
 }
@@ -493,17 +493,17 @@ void s5p_mfc_close_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 {
 	ctx->state = MFCINST_RETURN_INST;
 	set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	/* Wait until instance is returned or timeout occurred */
 	if (s5p_mfc_wait_for_done_ctx(ctx,
 				S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0))
 		mfc_err("Err returning instance\n");
 
 	/* Free resources */
-	s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
+	s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
+	s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
 	if (ctx->type == MFCINST_DECODER)
-		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer, ctx);
 
 	ctx->inst_no = MFC_NO_INSTANCE_SET;
 	ctx->state = MFCINST_FREE;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 8c5060a7534f..f2d6376ce618 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -252,7 +252,7 @@ static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
 	return 0;
 }
 
-static struct s5p_mfc_codec_ops decoder_codec_ops = {
+static const struct s5p_mfc_codec_ops decoder_codec_ops = {
 	.pre_seq_start		= NULL,
 	.post_seq_start		= NULL,
 	.pre_frame_start	= NULL,
@@ -523,7 +523,7 @@ static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
 		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
 		if (ret)
 			goto out;
-		s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
 		ctx->dst_bufs_cnt = 0;
 	} else if (ctx->capture_state == QUEUE_FREE) {
 		WARN_ON(ctx->dst_bufs_cnt != 0);
@@ -551,7 +551,7 @@ static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
 
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
-		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_INIT_BUFFERS_RET,
 					  0);
 	} else {
@@ -831,7 +831,7 @@ static int vidioc_decoder_cmd(struct file *file, void *priv,
 			if (s5p_mfc_ctx_ready(ctx))
 				set_work_bit_irqsave(ctx);
 			spin_unlock_irqrestore(&dev->irqlock, flags);
-			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		} else {
 			mfc_err("EOS: marking last buffer of stream");
 			buf = list_entry(ctx->src_queue.prev,
@@ -888,7 +888,7 @@ static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 };
 
 static int s5p_mfc_queue_setup(struct vb2_queue *vq,
-			const void *parg, unsigned int *buf_count,
+			unsigned int *buf_count,
 			unsigned int *plane_count, unsigned int psize[],
 			void *allocators[])
 {
@@ -1012,7 +1012,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	return 0;
 }
 
@@ -1023,42 +1023,41 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 	struct s5p_mfc_dev *dev = ctx->dev;
 	int aborted = 0;
 
+	spin_lock_irqsave(&dev->irqlock, flags);
 	if ((ctx->state == MFCINST_FINISHING ||
 		ctx->state ==  MFCINST_RUNNING) &&
 		dev->curr_ctx == ctx->num && dev->hw_lock) {
 		ctx->state = MFCINST_ABORT;
+		spin_unlock_irqrestore(&dev->irqlock, flags);
 		s5p_mfc_wait_for_done_ctx(ctx,
 					S5P_MFC_R2H_CMD_FRAME_DONE_RET, 0);
 		aborted = 1;
+		spin_lock_irqsave(&dev->irqlock, flags);
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		spin_lock_irqsave(&dev->irqlock, flags);
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 		ctx->dpb_flush_flag = 1;
 		ctx->dec_dst_flag = 0;
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		if (IS_MFCV6_PLUS(dev) && (ctx->state == MFCINST_RUNNING)) {
 			ctx->state = MFCINST_FLUSH;
 			set_work_bit_irqsave(ctx);
-			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+			spin_unlock_irqrestore(&dev->irqlock, flags);
 			if (s5p_mfc_wait_for_done_ctx(ctx,
 				S5P_MFC_R2H_CMD_DPB_FLUSH_RET, 0))
 				mfc_err("Err flushing buffers\n");
+			spin_lock_irqsave(&dev->irqlock, flags);
 		}
-	}
-	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
-		spin_lock_irqsave(&dev->irqlock, flags);
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->src_queue, &ctx->vq_src);
+	} else if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
+		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 	}
 	if (aborted)
 		ctx->state = MFCINST_RUNNING;
+	spin_unlock_irqrestore(&dev->irqlock, flags);
 }
 
 
@@ -1091,7 +1090,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_dec_qops = {
@@ -1104,7 +1103,7 @@ static struct vb2_ops s5p_mfc_dec_qops = {
 	.buf_queue		= s5p_mfc_buf_queue,
 };
 
-struct s5p_mfc_codec_ops *get_dec_codec_ops(void)
+const struct s5p_mfc_codec_ops *get_dec_codec_ops(void)
 {
 	return &decoder_codec_ops;
 }
@@ -1119,7 +1118,7 @@ const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void)
 	return &s5p_mfc_dec_ioctl_ops;
 }
 
-#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2WHICH(x) == V4L2_CTRL_CLASS_MPEG) \
 						&& V4L2_CTRL_DRIVER_PRIV(x))
 
 int s5p_mfc_dec_ctrls_setup(struct s5p_mfc_ctx *ctx)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
index d06a7cab5eb1..886628b153f0 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.h
@@ -13,7 +13,7 @@
 #ifndef S5P_MFC_DEC_H_
 #define S5P_MFC_DEC_H_
 
-struct s5p_mfc_codec_ops *get_dec_codec_ops(void);
+const struct s5p_mfc_codec_ops *get_dec_codec_ops(void);
 struct vb2_ops *get_dec_queue_ops(void);
 const struct v4l2_ioctl_ops *get_dec_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_dec_def_fmt(bool src);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index 5c678ec9c9f2..0434f02a7175 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -769,15 +769,12 @@ static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
 	struct s5p_mfc_buf *dst_mb;
 	unsigned long dst_addr;
 	unsigned int dst_size;
-	unsigned long flags;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
 	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
-	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
 			dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	return 0;
 }
 
@@ -786,11 +783,9 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_enc_params *p = &ctx->enc_params;
 	struct s5p_mfc_buf *dst_mb;
-	unsigned long flags;
 	unsigned int enc_pb_count;
 
 	if (p->seq_hdr_mode == V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE) {
-		spin_lock_irqsave(&dev->irqlock, flags);
 		if (!list_empty(&ctx->dst_queue)) {
 			dst_mb = list_entry(ctx->dst_queue.next,
 					struct s5p_mfc_buf, list);
@@ -802,14 +797,13 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
 			vb2_buffer_done(&dst_mb->b->vb2_buf,
 					VB2_BUF_STATE_DONE);
 		}
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 	}
 
 	if (!IS_MFCV6_PLUS(dev)) {
 		ctx->state = MFCINST_RUNNING;
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
-		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 	} else {
 		enc_pb_count = s5p_mfc_hw_call(dev->mfc_ops,
 				get_enc_dpb_count, dev);
@@ -826,25 +820,20 @@ static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf *dst_mb;
 	struct s5p_mfc_buf *src_mb;
-	unsigned long flags;
 	unsigned long src_y_addr, src_c_addr, dst_addr;
 	unsigned int dst_size;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	src_y_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 0);
 	src_c_addr = vb2_dma_contig_plane_dma_addr(&src_mb->b->vb2_buf, 1);
-	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_frame_buffer, ctx,
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_frame_buffer, ctx,
 							src_y_addr, src_c_addr);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
 	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
-	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
 			dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 
 	return 0;
 }
@@ -857,7 +846,6 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 	unsigned long mb_y_addr, mb_c_addr;
 	int slice_type;
 	unsigned int strm_size;
-	unsigned long flags;
 
 	slice_type = s5p_mfc_hw_call(dev->mfc_ops, get_enc_slice_type, dev);
 	strm_size = s5p_mfc_hw_call(dev->mfc_ops, get_enc_strm_size, dev);
@@ -865,9 +853,8 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 	mfc_debug(2, "Encoded stream size: %d\n", strm_size);
 	mfc_debug(2, "Display order: %d\n",
 		  mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
-	spin_lock_irqsave(&dev->irqlock, flags);
 	if (slice_type >= 0) {
-		s5p_mfc_hw_call_void(dev->mfc_ops, get_enc_frame_buffer, ctx,
+		s5p_mfc_hw_call(dev->mfc_ops, get_enc_frame_buffer, ctx,
 				&enc_y_addr, &enc_c_addr);
 		list_for_each_entry(mb_entry, &ctx->src_queue, list) {
 			mb_y_addr = vb2_dma_contig_plane_dma_addr(
@@ -929,14 +916,13 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 		vb2_set_plane_payload(&mb_entry->b->vb2_buf, 0, strm_size);
 		vb2_buffer_done(&mb_entry->b->vb2_buf, VB2_BUF_STATE_DONE);
 	}
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	if ((ctx->src_queue_cnt == 0) || (ctx->dst_queue_cnt == 0))
 		clear_work_bit(ctx);
 
 	return 0;
 }
 
-static struct s5p_mfc_codec_ops encoder_codec_ops = {
+static const struct s5p_mfc_codec_ops encoder_codec_ops = {
 	.pre_seq_start		= enc_pre_seq_start,
 	.post_seq_start		= enc_post_seq_start,
 	.pre_frame_start	= enc_pre_frame_start,
@@ -1120,7 +1106,7 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			pix_fmt_mp->width, pix_fmt_mp->height,
 			ctx->img_width, ctx->img_height);
 
-		s5p_mfc_hw_call_void(dev->mfc_ops, enc_calc_src_size, ctx);
+		s5p_mfc_hw_call(dev->mfc_ops, enc_calc_src_size, ctx);
 		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
 		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
 		pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
@@ -1178,7 +1164,7 @@ static int vidioc_reqbufs(struct file *file, void *priv,
 		if (reqbufs->count == 0) {
 			mfc_debug(2, "Freeing buffers\n");
 			ret = vb2_reqbufs(&ctx->vq_src, reqbufs);
-			s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers,
+			s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers,
 					ctx);
 			ctx->output_state = QUEUE_FREE;
 			return ret;
@@ -1741,7 +1727,7 @@ static int vidioc_encoder_cmd(struct file *file, void *priv,
 			if (s5p_mfc_ctx_ready(ctx))
 				set_work_bit_irqsave(ctx);
 			spin_unlock_irqrestore(&dev->irqlock, flags);
-			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 		} else {
 			mfc_debug(2, "EOS: marking last buffer of stream\n");
 			buf = list_entry(ctx->src_queue.prev,
@@ -1818,7 +1804,6 @@ static int check_vb_with_fmt(struct s5p_mfc_fmt *fmt, struct vb2_buffer *vb)
 }
 
 static int s5p_mfc_queue_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *buf_count, unsigned int *plane_count,
 			unsigned int psize[], void *allocators[])
 {
@@ -1969,7 +1954,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 
 	return 0;
 }
@@ -1990,15 +1975,13 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 	ctx->state = MFCINST_FINISHED;
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
-						&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_cleanup_queue(&ctx->dst_queue, &ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		cleanup_ref_queue(ctx);
-		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
-				&ctx->vq_src);
+		s5p_mfc_cleanup_queue(&ctx->src_queue, &ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
 	}
@@ -2038,7 +2021,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_enc_qops = {
@@ -2052,7 +2035,7 @@ static struct vb2_ops s5p_mfc_enc_qops = {
 	.buf_queue		= s5p_mfc_buf_queue,
 };
 
-struct s5p_mfc_codec_ops *get_enc_codec_ops(void)
+const struct s5p_mfc_codec_ops *get_enc_codec_ops(void)
 {
 	return &encoder_codec_ops;
 }
@@ -2067,7 +2050,7 @@ const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void)
 	return &s5p_mfc_enc_ioctl_ops;
 }
 
-#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2CLASS(x) == V4L2_CTRL_CLASS_MPEG) \
+#define IS_MFC51_PRIV(x) ((V4L2_CTRL_ID2WHICH(x) == V4L2_CTRL_CLASS_MPEG) \
 						&& V4L2_CTRL_DRIVER_PRIV(x))
 
 int s5p_mfc_enc_ctrls_setup(struct s5p_mfc_ctx *ctx)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
index 5118d46b3a9e..d0d42f818832 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.h
@@ -13,7 +13,7 @@
 #ifndef S5P_MFC_ENC_H_
 #define S5P_MFC_ENC_H_
 
-struct s5p_mfc_codec_ops *get_enc_codec_ops(void);
+const struct s5p_mfc_codec_ops *get_enc_codec_ops(void);
 struct vb2_ops *get_enc_queue_ops(void);
 const struct v4l2_ioctl_ops *get_enc_v4l2_ioctl_ops(void);
 struct s5p_mfc_fmt *get_enc_def_fmt(bool src);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 77a08b19b46d..b6ac417ab63e 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -20,254 +20,254 @@
 struct s5p_mfc_regs {
 
 	/* codec common registers */
-	volatile void __iomem *risc_on;
-	volatile void __iomem *risc2host_int;
-	volatile void __iomem *host2risc_int;
-	volatile void __iomem *risc_base_address;
-	volatile void __iomem *mfc_reset;
-	volatile void __iomem *host2risc_command;
-	volatile void __iomem *risc2host_command;
-	volatile void __iomem *mfc_bus_reset_ctrl;
-	volatile void __iomem *firmware_version;
-	volatile void __iomem *instance_id;
-	volatile void __iomem *codec_type;
-	volatile void __iomem *context_mem_addr;
-	volatile void __iomem *context_mem_size;
-	volatile void __iomem *pixel_format;
-	volatile void __iomem *metadata_enable;
-	volatile void __iomem *mfc_version;
-	volatile void __iomem *dbg_info_enable;
-	volatile void __iomem *dbg_buffer_addr;
-	volatile void __iomem *dbg_buffer_size;
-	volatile void __iomem *hed_control;
-	volatile void __iomem *mfc_timeout_value;
-	volatile void __iomem *hed_shared_mem_addr;
-	volatile void __iomem *dis_shared_mem_addr;/* only v7 */
-	volatile void __iomem *ret_instance_id;
-	volatile void __iomem *error_code;
-	volatile void __iomem *dbg_buffer_output_size;
-	volatile void __iomem *metadata_status;
-	volatile void __iomem *metadata_addr_mb_info;
-	volatile void __iomem *metadata_size_mb_info;
-	volatile void __iomem *dbg_info_stage_counter;
+	void __iomem *risc_on;
+	void __iomem *risc2host_int;
+	void __iomem *host2risc_int;
+	void __iomem *risc_base_address;
+	void __iomem *mfc_reset;
+	void __iomem *host2risc_command;
+	void __iomem *risc2host_command;
+	void __iomem *mfc_bus_reset_ctrl;
+	void __iomem *firmware_version;
+	void __iomem *instance_id;
+	void __iomem *codec_type;
+	void __iomem *context_mem_addr;
+	void __iomem *context_mem_size;
+	void __iomem *pixel_format;
+	void __iomem *metadata_enable;
+	void __iomem *mfc_version;
+	void __iomem *dbg_info_enable;
+	void __iomem *dbg_buffer_addr;
+	void __iomem *dbg_buffer_size;
+	void __iomem *hed_control;
+	void __iomem *mfc_timeout_value;
+	void __iomem *hed_shared_mem_addr;
+	void __iomem *dis_shared_mem_addr;/* only v7 */
+	void __iomem *ret_instance_id;
+	void __iomem *error_code;
+	void __iomem *dbg_buffer_output_size;
+	void __iomem *metadata_status;
+	void __iomem *metadata_addr_mb_info;
+	void __iomem *metadata_size_mb_info;
+	void __iomem *dbg_info_stage_counter;
 
 	/* decoder registers */
-	volatile void __iomem *d_crc_ctrl;
-	volatile void __iomem *d_dec_options;
-	volatile void __iomem *d_display_delay;
-	volatile void __iomem *d_set_frame_width;
-	volatile void __iomem *d_set_frame_height;
-	volatile void __iomem *d_sei_enable;
-	volatile void __iomem *d_min_num_dpb;
-	volatile void __iomem *d_min_first_plane_dpb_size;
-	volatile void __iomem *d_min_second_plane_dpb_size;
-	volatile void __iomem *d_min_third_plane_dpb_size;/* only v8 */
-	volatile void __iomem *d_min_num_mv;
-	volatile void __iomem *d_mvc_num_views;
-	volatile void __iomem *d_min_num_dis;/* only v7 */
-	volatile void __iomem *d_min_first_dis_size;/* only v7 */
-	volatile void __iomem *d_min_second_dis_size;/* only v7 */
-	volatile void __iomem *d_min_third_dis_size;/* only v7 */
-	volatile void __iomem *d_post_filter_luma_dpb0;/*  v7 and v8 */
-	volatile void __iomem *d_post_filter_luma_dpb1;/* v7 and v8 */
-	volatile void __iomem *d_post_filter_luma_dpb2;/* only v7 */
-	volatile void __iomem *d_post_filter_chroma_dpb0;/* v7 and v8 */
-	volatile void __iomem *d_post_filter_chroma_dpb1;/* v7 and v8 */
-	volatile void __iomem *d_post_filter_chroma_dpb2;/* only v7 */
-	volatile void __iomem *d_num_dpb;
-	volatile void __iomem *d_num_mv;
-	volatile void __iomem *d_init_buffer_options;
-	volatile void __iomem *d_first_plane_dpb_stride_size;/* only v8 */
-	volatile void __iomem *d_second_plane_dpb_stride_size;/* only v8 */
-	volatile void __iomem *d_third_plane_dpb_stride_size;/* only v8 */
-	volatile void __iomem *d_first_plane_dpb_size;
-	volatile void __iomem *d_second_plane_dpb_size;
-	volatile void __iomem *d_third_plane_dpb_size;/* only v8 */
-	volatile void __iomem *d_mv_buffer_size;
-	volatile void __iomem *d_first_plane_dpb;
-	volatile void __iomem *d_second_plane_dpb;
-	volatile void __iomem *d_third_plane_dpb;
-	volatile void __iomem *d_mv_buffer;
-	volatile void __iomem *d_scratch_buffer_addr;
-	volatile void __iomem *d_scratch_buffer_size;
-	volatile void __iomem *d_metadata_buffer_addr;
-	volatile void __iomem *d_metadata_buffer_size;
-	volatile void __iomem *d_nal_start_options;/* v7 and v8 */
-	volatile void __iomem *d_cpb_buffer_addr;
-	volatile void __iomem *d_cpb_buffer_size;
-	volatile void __iomem *d_available_dpb_flag_upper;
-	volatile void __iomem *d_available_dpb_flag_lower;
-	volatile void __iomem *d_cpb_buffer_offset;
-	volatile void __iomem *d_slice_if_enable;
-	volatile void __iomem *d_picture_tag;
-	volatile void __iomem *d_stream_data_size;
-	volatile void __iomem *d_dynamic_dpb_flag_upper;/* v7 and v8 */
-	volatile void __iomem *d_dynamic_dpb_flag_lower;/* v7 and v8 */
-	volatile void __iomem *d_display_frame_width;
-	volatile void __iomem *d_display_frame_height;
-	volatile void __iomem *d_display_status;
-	volatile void __iomem *d_display_first_plane_addr;
-	volatile void __iomem *d_display_second_plane_addr;
-	volatile void __iomem *d_display_third_plane_addr;/* only v8 */
-	volatile void __iomem *d_display_frame_type;
-	volatile void __iomem *d_display_crop_info1;
-	volatile void __iomem *d_display_crop_info2;
-	volatile void __iomem *d_display_picture_profile;
-	volatile void __iomem *d_display_luma_crc;/* v7 and v8 */
-	volatile void __iomem *d_display_chroma0_crc;/* v7 and v8 */
-	volatile void __iomem *d_display_chroma1_crc;/* only v8 */
-	volatile void __iomem *d_display_luma_crc_top;/* only v6 */
-	volatile void __iomem *d_display_chroma_crc_top;/* only v6 */
-	volatile void __iomem *d_display_luma_crc_bot;/* only v6 */
-	volatile void __iomem *d_display_chroma_crc_bot;/* only v6 */
-	volatile void __iomem *d_display_aspect_ratio;
-	volatile void __iomem *d_display_extended_ar;
-	volatile void __iomem *d_decoded_frame_width;
-	volatile void __iomem *d_decoded_frame_height;
-	volatile void __iomem *d_decoded_status;
-	volatile void __iomem *d_decoded_first_plane_addr;
-	volatile void __iomem *d_decoded_second_plane_addr;
-	volatile void __iomem *d_decoded_third_plane_addr;/* only v8 */
-	volatile void __iomem *d_decoded_frame_type;
-	volatile void __iomem *d_decoded_crop_info1;
-	volatile void __iomem *d_decoded_crop_info2;
-	volatile void __iomem *d_decoded_picture_profile;
-	volatile void __iomem *d_decoded_nal_size;
-	volatile void __iomem *d_decoded_luma_crc;
-	volatile void __iomem *d_decoded_chroma0_crc;
-	volatile void __iomem *d_decoded_chroma1_crc;/* only v8 */
-	volatile void __iomem *d_ret_picture_tag_top;
-	volatile void __iomem *d_ret_picture_tag_bot;
-	volatile void __iomem *d_ret_picture_time_top;
-	volatile void __iomem *d_ret_picture_time_bot;
-	volatile void __iomem *d_chroma_format;
-	volatile void __iomem *d_vc1_info;/* v7 and v8 */
-	volatile void __iomem *d_mpeg4_info;
-	volatile void __iomem *d_h264_info;
-	volatile void __iomem *d_metadata_addr_concealed_mb;
-	volatile void __iomem *d_metadata_size_concealed_mb;
-	volatile void __iomem *d_metadata_addr_vc1_param;
-	volatile void __iomem *d_metadata_size_vc1_param;
-	volatile void __iomem *d_metadata_addr_sei_nal;
-	volatile void __iomem *d_metadata_size_sei_nal;
-	volatile void __iomem *d_metadata_addr_vui;
-	volatile void __iomem *d_metadata_size_vui;
-	volatile void __iomem *d_metadata_addr_mvcvui;/* v7 and v8 */
-	volatile void __iomem *d_metadata_size_mvcvui;/* v7 and v8 */
-	volatile void __iomem *d_mvc_view_id;
-	volatile void __iomem *d_frame_pack_sei_avail;
-	volatile void __iomem *d_frame_pack_arrgment_id;
-	volatile void __iomem *d_frame_pack_sei_info;
-	volatile void __iomem *d_frame_pack_grid_pos;
-	volatile void __iomem *d_display_recovery_sei_info;/* v7 and v8 */
-	volatile void __iomem *d_decoded_recovery_sei_info;/* v7 and v8 */
-	volatile void __iomem *d_display_first_addr;/* only v7 */
-	volatile void __iomem *d_display_second_addr;/* only v7 */
-	volatile void __iomem *d_display_third_addr;/* only v7 */
-	volatile void __iomem *d_decoded_first_addr;/* only v7 */
-	volatile void __iomem *d_decoded_second_addr;/* only v7 */
-	volatile void __iomem *d_decoded_third_addr;/* only v7 */
-	volatile void __iomem *d_used_dpb_flag_upper;/* v7 and v8 */
-	volatile void __iomem *d_used_dpb_flag_lower;/* v7 and v8 */
+	void __iomem *d_crc_ctrl;
+	void __iomem *d_dec_options;
+	void __iomem *d_display_delay;
+	void __iomem *d_set_frame_width;
+	void __iomem *d_set_frame_height;
+	void __iomem *d_sei_enable;
+	void __iomem *d_min_num_dpb;
+	void __iomem *d_min_first_plane_dpb_size;
+	void __iomem *d_min_second_plane_dpb_size;
+	void __iomem *d_min_third_plane_dpb_size;/* only v8 */
+	void __iomem *d_min_num_mv;
+	void __iomem *d_mvc_num_views;
+	void __iomem *d_min_num_dis;/* only v7 */
+	void __iomem *d_min_first_dis_size;/* only v7 */
+	void __iomem *d_min_second_dis_size;/* only v7 */
+	void __iomem *d_min_third_dis_size;/* only v7 */
+	void __iomem *d_post_filter_luma_dpb0;/*  v7 and v8 */
+	void __iomem *d_post_filter_luma_dpb1;/* v7 and v8 */
+	void __iomem *d_post_filter_luma_dpb2;/* only v7 */
+	void __iomem *d_post_filter_chroma_dpb0;/* v7 and v8 */
+	void __iomem *d_post_filter_chroma_dpb1;/* v7 and v8 */
+	void __iomem *d_post_filter_chroma_dpb2;/* only v7 */
+	void __iomem *d_num_dpb;
+	void __iomem *d_num_mv;
+	void __iomem *d_init_buffer_options;
+	void __iomem *d_first_plane_dpb_stride_size;/* only v8 */
+	void __iomem *d_second_plane_dpb_stride_size;/* only v8 */
+	void __iomem *d_third_plane_dpb_stride_size;/* only v8 */
+	void __iomem *d_first_plane_dpb_size;
+	void __iomem *d_second_plane_dpb_size;
+	void __iomem *d_third_plane_dpb_size;/* only v8 */
+	void __iomem *d_mv_buffer_size;
+	void __iomem *d_first_plane_dpb;
+	void __iomem *d_second_plane_dpb;
+	void __iomem *d_third_plane_dpb;
+	void __iomem *d_mv_buffer;
+	void __iomem *d_scratch_buffer_addr;
+	void __iomem *d_scratch_buffer_size;
+	void __iomem *d_metadata_buffer_addr;
+	void __iomem *d_metadata_buffer_size;
+	void __iomem *d_nal_start_options;/* v7 and v8 */
+	void __iomem *d_cpb_buffer_addr;
+	void __iomem *d_cpb_buffer_size;
+	void __iomem *d_available_dpb_flag_upper;
+	void __iomem *d_available_dpb_flag_lower;
+	void __iomem *d_cpb_buffer_offset;
+	void __iomem *d_slice_if_enable;
+	void __iomem *d_picture_tag;
+	void __iomem *d_stream_data_size;
+	void __iomem *d_dynamic_dpb_flag_upper;/* v7 and v8 */
+	void __iomem *d_dynamic_dpb_flag_lower;/* v7 and v8 */
+	void __iomem *d_display_frame_width;
+	void __iomem *d_display_frame_height;
+	void __iomem *d_display_status;
+	void __iomem *d_display_first_plane_addr;
+	void __iomem *d_display_second_plane_addr;
+	void __iomem *d_display_third_plane_addr;/* only v8 */
+	void __iomem *d_display_frame_type;
+	void __iomem *d_display_crop_info1;
+	void __iomem *d_display_crop_info2;
+	void __iomem *d_display_picture_profile;
+	void __iomem *d_display_luma_crc;/* v7 and v8 */
+	void __iomem *d_display_chroma0_crc;/* v7 and v8 */
+	void __iomem *d_display_chroma1_crc;/* only v8 */
+	void __iomem *d_display_luma_crc_top;/* only v6 */
+	void __iomem *d_display_chroma_crc_top;/* only v6 */
+	void __iomem *d_display_luma_crc_bot;/* only v6 */
+	void __iomem *d_display_chroma_crc_bot;/* only v6 */
+	void __iomem *d_display_aspect_ratio;
+	void __iomem *d_display_extended_ar;
+	void __iomem *d_decoded_frame_width;
+	void __iomem *d_decoded_frame_height;
+	void __iomem *d_decoded_status;
+	void __iomem *d_decoded_first_plane_addr;
+	void __iomem *d_decoded_second_plane_addr;
+	void __iomem *d_decoded_third_plane_addr;/* only v8 */
+	void __iomem *d_decoded_frame_type;
+	void __iomem *d_decoded_crop_info1;
+	void __iomem *d_decoded_crop_info2;
+	void __iomem *d_decoded_picture_profile;
+	void __iomem *d_decoded_nal_size;
+	void __iomem *d_decoded_luma_crc;
+	void __iomem *d_decoded_chroma0_crc;
+	void __iomem *d_decoded_chroma1_crc;/* only v8 */
+	void __iomem *d_ret_picture_tag_top;
+	void __iomem *d_ret_picture_tag_bot;
+	void __iomem *d_ret_picture_time_top;
+	void __iomem *d_ret_picture_time_bot;
+	void __iomem *d_chroma_format;
+	void __iomem *d_vc1_info;/* v7 and v8 */
+	void __iomem *d_mpeg4_info;
+	void __iomem *d_h264_info;
+	void __iomem *d_metadata_addr_concealed_mb;
+	void __iomem *d_metadata_size_concealed_mb;
+	void __iomem *d_metadata_addr_vc1_param;
+	void __iomem *d_metadata_size_vc1_param;
+	void __iomem *d_metadata_addr_sei_nal;
+	void __iomem *d_metadata_size_sei_nal;
+	void __iomem *d_metadata_addr_vui;
+	void __iomem *d_metadata_size_vui;
+	void __iomem *d_metadata_addr_mvcvui;/* v7 and v8 */
+	void __iomem *d_metadata_size_mvcvui;/* v7 and v8 */
+	void __iomem *d_mvc_view_id;
+	void __iomem *d_frame_pack_sei_avail;
+	void __iomem *d_frame_pack_arrgment_id;
+	void __iomem *d_frame_pack_sei_info;
+	void __iomem *d_frame_pack_grid_pos;
+	void __iomem *d_display_recovery_sei_info;/* v7 and v8 */
+	void __iomem *d_decoded_recovery_sei_info;/* v7 and v8 */
+	void __iomem *d_display_first_addr;/* only v7 */
+	void __iomem *d_display_second_addr;/* only v7 */
+	void __iomem *d_display_third_addr;/* only v7 */
+	void __iomem *d_decoded_first_addr;/* only v7 */
+	void __iomem *d_decoded_second_addr;/* only v7 */
+	void __iomem *d_decoded_third_addr;/* only v7 */
+	void __iomem *d_used_dpb_flag_upper;/* v7 and v8 */
+	void __iomem *d_used_dpb_flag_lower;/* v7 and v8 */
 
 	/* encoder registers */
-	volatile void __iomem *e_frame_width;
-	volatile void __iomem *e_frame_height;
-	volatile void __iomem *e_cropped_frame_width;
-	volatile void __iomem *e_cropped_frame_height;
-	volatile void __iomem *e_frame_crop_offset;
-	volatile void __iomem *e_enc_options;
-	volatile void __iomem *e_picture_profile;
-	volatile void __iomem *e_vbv_buffer_size;
-	volatile void __iomem *e_vbv_init_delay;
-	volatile void __iomem *e_fixed_picture_qp;
-	volatile void __iomem *e_rc_config;
-	volatile void __iomem *e_rc_qp_bound;
-	volatile void __iomem *e_rc_qp_bound_pb;/* v7 and v8 */
-	volatile void __iomem *e_rc_mode;
-	volatile void __iomem *e_mb_rc_config;
-	volatile void __iomem *e_padding_ctrl;
-	volatile void __iomem *e_air_threshold;
-	volatile void __iomem *e_mv_hor_range;
-	volatile void __iomem *e_mv_ver_range;
-	volatile void __iomem *e_num_dpb;
-	volatile void __iomem *e_luma_dpb;
-	volatile void __iomem *e_chroma_dpb;
-	volatile void __iomem *e_me_buffer;
-	volatile void __iomem *e_scratch_buffer_addr;
-	volatile void __iomem *e_scratch_buffer_size;
-	volatile void __iomem *e_tmv_buffer0;
-	volatile void __iomem *e_tmv_buffer1;
-	volatile void __iomem *e_ir_buffer_addr;/* v7 and v8 */
-	volatile void __iomem *e_source_first_plane_addr;
-	volatile void __iomem *e_source_second_plane_addr;
-	volatile void __iomem *e_source_third_plane_addr;/* v7 and v8 */
-	volatile void __iomem *e_source_first_plane_stride;/* v7 and v8 */
-	volatile void __iomem *e_source_second_plane_stride;/* v7 and v8 */
-	volatile void __iomem *e_source_third_plane_stride;/* v7 and v8 */
-	volatile void __iomem *e_stream_buffer_addr;
-	volatile void __iomem *e_stream_buffer_size;
-	volatile void __iomem *e_roi_buffer_addr;
-	volatile void __iomem *e_param_change;
-	volatile void __iomem *e_ir_size;
-	volatile void __iomem *e_gop_config;
-	volatile void __iomem *e_mslice_mode;
-	volatile void __iomem *e_mslice_size_mb;
-	volatile void __iomem *e_mslice_size_bits;
-	volatile void __iomem *e_frame_insertion;
-	volatile void __iomem *e_rc_frame_rate;
-	volatile void __iomem *e_rc_bit_rate;
-	volatile void __iomem *e_rc_roi_ctrl;
-	volatile void __iomem *e_picture_tag;
-	volatile void __iomem *e_bit_count_enable;
-	volatile void __iomem *e_max_bit_count;
-	volatile void __iomem *e_min_bit_count;
-	volatile void __iomem *e_metadata_buffer_addr;
-	volatile void __iomem *e_metadata_buffer_size;
-	volatile void __iomem *e_encoded_source_first_plane_addr;
-	volatile void __iomem *e_encoded_source_second_plane_addr;
-	volatile void __iomem *e_encoded_source_third_plane_addr;/* v7 and v8 */
-	volatile void __iomem *e_stream_size;
-	volatile void __iomem *e_slice_type;
-	volatile void __iomem *e_picture_count;
-	volatile void __iomem *e_ret_picture_tag;
-	volatile void __iomem *e_stream_buffer_write_pointer; /*  only v6 */
-	volatile void __iomem *e_recon_luma_dpb_addr;
-	volatile void __iomem *e_recon_chroma_dpb_addr;
-	volatile void __iomem *e_metadata_addr_enc_slice;
-	volatile void __iomem *e_metadata_size_enc_slice;
-	volatile void __iomem *e_mpeg4_options;
-	volatile void __iomem *e_mpeg4_hec_period;
-	volatile void __iomem *e_aspect_ratio;
-	volatile void __iomem *e_extended_sar;
-	volatile void __iomem *e_h264_options;
-	volatile void __iomem *e_h264_options_2;/* v7 and v8 */
-	volatile void __iomem *e_h264_lf_alpha_offset;
-	volatile void __iomem *e_h264_lf_beta_offset;
-	volatile void __iomem *e_h264_i_period;
-	volatile void __iomem *e_h264_fmo_slice_grp_map_type;
-	volatile void __iomem *e_h264_fmo_num_slice_grp_minus1;
-	volatile void __iomem *e_h264_fmo_slice_grp_change_dir;
-	volatile void __iomem *e_h264_fmo_slice_grp_change_rate_minus1;
-	volatile void __iomem *e_h264_fmo_run_length_minus1_0;
-	volatile void __iomem *e_h264_aso_slice_order_0;
-	volatile void __iomem *e_h264_chroma_qp_offset;
-	volatile void __iomem *e_h264_num_t_layer;
-	volatile void __iomem *e_h264_hierarchical_qp_layer0;
-	volatile void __iomem *e_h264_frame_packing_sei_info;
-	volatile void __iomem *e_h264_nal_control;/* v7 and v8 */
-	volatile void __iomem *e_mvc_frame_qp_view1;
-	volatile void __iomem *e_mvc_rc_bit_rate_view1;
-	volatile void __iomem *e_mvc_rc_qbound_view1;
-	volatile void __iomem *e_mvc_rc_mode_view1;
-	volatile void __iomem *e_mvc_inter_view_prediction_on;
-	volatile void __iomem *e_vp8_options;/* v7 and v8 */
-	volatile void __iomem *e_vp8_filter_options;/* v7 and v8 */
-	volatile void __iomem *e_vp8_golden_frame_option;/* v7 and v8 */
-	volatile void __iomem *e_vp8_num_t_layer;/* v7 and v8 */
-	volatile void __iomem *e_vp8_hierarchical_qp_layer0;/* v7 and v8 */
-	volatile void __iomem *e_vp8_hierarchical_qp_layer1;/* v7 and v8 */
-	volatile void __iomem *e_vp8_hierarchical_qp_layer2;/* v7 and v8 */
+	void __iomem *e_frame_width;
+	void __iomem *e_frame_height;
+	void __iomem *e_cropped_frame_width;
+	void __iomem *e_cropped_frame_height;
+	void __iomem *e_frame_crop_offset;
+	void __iomem *e_enc_options;
+	void __iomem *e_picture_profile;
+	void __iomem *e_vbv_buffer_size;
+	void __iomem *e_vbv_init_delay;
+	void __iomem *e_fixed_picture_qp;
+	void __iomem *e_rc_config;
+	void __iomem *e_rc_qp_bound;
+	void __iomem *e_rc_qp_bound_pb;/* v7 and v8 */
+	void __iomem *e_rc_mode;
+	void __iomem *e_mb_rc_config;
+	void __iomem *e_padding_ctrl;
+	void __iomem *e_air_threshold;
+	void __iomem *e_mv_hor_range;
+	void __iomem *e_mv_ver_range;
+	void __iomem *e_num_dpb;
+	void __iomem *e_luma_dpb;
+	void __iomem *e_chroma_dpb;
+	void __iomem *e_me_buffer;
+	void __iomem *e_scratch_buffer_addr;
+	void __iomem *e_scratch_buffer_size;
+	void __iomem *e_tmv_buffer0;
+	void __iomem *e_tmv_buffer1;
+	void __iomem *e_ir_buffer_addr;/* v7 and v8 */
+	void __iomem *e_source_first_plane_addr;
+	void __iomem *e_source_second_plane_addr;
+	void __iomem *e_source_third_plane_addr;/* v7 and v8 */
+	void __iomem *e_source_first_plane_stride;/* v7 and v8 */
+	void __iomem *e_source_second_plane_stride;/* v7 and v8 */
+	void __iomem *e_source_third_plane_stride;/* v7 and v8 */
+	void __iomem *e_stream_buffer_addr;
+	void __iomem *e_stream_buffer_size;
+	void __iomem *e_roi_buffer_addr;
+	void __iomem *e_param_change;
+	void __iomem *e_ir_size;
+	void __iomem *e_gop_config;
+	void __iomem *e_mslice_mode;
+	void __iomem *e_mslice_size_mb;
+	void __iomem *e_mslice_size_bits;
+	void __iomem *e_frame_insertion;
+	void __iomem *e_rc_frame_rate;
+	void __iomem *e_rc_bit_rate;
+	void __iomem *e_rc_roi_ctrl;
+	void __iomem *e_picture_tag;
+	void __iomem *e_bit_count_enable;
+	void __iomem *e_max_bit_count;
+	void __iomem *e_min_bit_count;
+	void __iomem *e_metadata_buffer_addr;
+	void __iomem *e_metadata_buffer_size;
+	void __iomem *e_encoded_source_first_plane_addr;
+	void __iomem *e_encoded_source_second_plane_addr;
+	void __iomem *e_encoded_source_third_plane_addr;/* v7 and v8 */
+	void __iomem *e_stream_size;
+	void __iomem *e_slice_type;
+	void __iomem *e_picture_count;
+	void __iomem *e_ret_picture_tag;
+	void __iomem *e_stream_buffer_write_pointer; /*  only v6 */
+	void __iomem *e_recon_luma_dpb_addr;
+	void __iomem *e_recon_chroma_dpb_addr;
+	void __iomem *e_metadata_addr_enc_slice;
+	void __iomem *e_metadata_size_enc_slice;
+	void __iomem *e_mpeg4_options;
+	void __iomem *e_mpeg4_hec_period;
+	void __iomem *e_aspect_ratio;
+	void __iomem *e_extended_sar;
+	void __iomem *e_h264_options;
+	void __iomem *e_h264_options_2;/* v7 and v8 */
+	void __iomem *e_h264_lf_alpha_offset;
+	void __iomem *e_h264_lf_beta_offset;
+	void __iomem *e_h264_i_period;
+	void __iomem *e_h264_fmo_slice_grp_map_type;
+	void __iomem *e_h264_fmo_num_slice_grp_minus1;
+	void __iomem *e_h264_fmo_slice_grp_change_dir;
+	void __iomem *e_h264_fmo_slice_grp_change_rate_minus1;
+	void __iomem *e_h264_fmo_run_length_minus1_0;
+	void __iomem *e_h264_aso_slice_order_0;
+	void __iomem *e_h264_chroma_qp_offset;
+	void __iomem *e_h264_num_t_layer;
+	void __iomem *e_h264_hierarchical_qp_layer0;
+	void __iomem *e_h264_frame_packing_sei_info;
+	void __iomem *e_h264_nal_control;/* v7 and v8 */
+	void __iomem *e_mvc_frame_qp_view1;
+	void __iomem *e_mvc_rc_bit_rate_view1;
+	void __iomem *e_mvc_rc_qbound_view1;
+	void __iomem *e_mvc_rc_mode_view1;
+	void __iomem *e_mvc_inter_view_prediction_on;
+	void __iomem *e_vp8_options;/* v7 and v8 */
+	void __iomem *e_vp8_filter_options;/* v7 and v8 */
+	void __iomem *e_vp8_golden_frame_option;/* v7 and v8 */
+	void __iomem *e_vp8_num_t_layer;/* v7 and v8 */
+	void __iomem *e_vp8_hierarchical_qp_layer0;/* v7 and v8 */
+	void __iomem *e_vp8_hierarchical_qp_layer1;/* v7 and v8 */
+	void __iomem *e_vp8_hierarchical_qp_layer2;/* v7 and v8 */
 };
 
 struct s5p_mfc_hw_ops {
@@ -281,28 +281,14 @@ struct s5p_mfc_hw_ops {
 	void (*release_dev_context_buffer)(struct s5p_mfc_dev *dev);
 	void (*dec_calc_dpb_size)(struct s5p_mfc_ctx *ctx);
 	void (*enc_calc_src_size)(struct s5p_mfc_ctx *ctx);
-	int (*set_dec_stream_buffer)(struct s5p_mfc_ctx *ctx,
-			int buf_addr, unsigned int start_num_byte,
-			unsigned int buf_size);
-	int (*set_dec_frame_buffer)(struct s5p_mfc_ctx *ctx);
 	int (*set_enc_stream_buffer)(struct s5p_mfc_ctx *ctx,
 			unsigned long addr, unsigned int size);
 	void (*set_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
 			unsigned long y_addr, unsigned long c_addr);
 	void (*get_enc_frame_buffer)(struct s5p_mfc_ctx *ctx,
 			unsigned long *y_addr, unsigned long *c_addr);
-	int (*set_enc_ref_buffer)(struct s5p_mfc_ctx *ctx);
-	int (*init_decode)(struct s5p_mfc_ctx *ctx);
-	int (*init_encode)(struct s5p_mfc_ctx *ctx);
-	int (*encode_one_frame)(struct s5p_mfc_ctx *ctx);
 	void (*try_run)(struct s5p_mfc_dev *dev);
-	void (*cleanup_queue)(struct list_head *lh,
-			struct vb2_queue *vq);
 	void (*clear_int_flags)(struct s5p_mfc_dev *dev);
-	void (*write_info)(struct s5p_mfc_ctx *ctx, unsigned int data,
-			unsigned int ofs);
-	unsigned int (*read_info)(struct s5p_mfc_ctx *ctx,
-			unsigned long ofs);
 	int (*get_dspl_y_adr)(struct s5p_mfc_dev *dev);
 	int (*get_dec_y_adr)(struct s5p_mfc_dev *dev);
 	int (*get_dspl_status)(struct s5p_mfc_dev *dev);
@@ -313,7 +299,6 @@ struct s5p_mfc_hw_ops {
 	int (*get_int_reason)(struct s5p_mfc_dev *dev);
 	int (*get_int_err)(struct s5p_mfc_dev *dev);
 	int (*err_dec)(unsigned int err);
-	int (*err_dspl)(unsigned int err);
 	int (*get_img_width)(struct s5p_mfc_dev *dev);
 	int (*get_img_height)(struct s5p_mfc_dev *dev);
 	int (*get_dpb_count)(struct s5p_mfc_dev *dev);
@@ -322,10 +307,6 @@ struct s5p_mfc_hw_ops {
 	int (*get_enc_strm_size)(struct s5p_mfc_dev *dev);
 	int (*get_enc_slice_type)(struct s5p_mfc_dev *dev);
 	int (*get_enc_dpb_count)(struct s5p_mfc_dev *dev);
-	int (*get_enc_pic_count)(struct s5p_mfc_dev *dev);
-	int (*get_sei_avail_status)(struct s5p_mfc_ctx *ctx);
-	int (*get_mvc_num_views)(struct s5p_mfc_dev *dev);
-	int (*get_mvc_view_id)(struct s5p_mfc_dev *dev);
 	unsigned int (*get_pic_type_top)(struct s5p_mfc_ctx *ctx);
 	unsigned int (*get_pic_type_bot)(struct s5p_mfc_ctx *ctx);
 	unsigned int (*get_crop_info_h)(struct s5p_mfc_ctx *ctx);
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index 873c933bc7d4..81e1e4ce6c24 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -1153,27 +1153,6 @@ static int s5p_mfc_encode_one_frame_v5(struct s5p_mfc_ctx *ctx)
 	return 0;
 }
 
-static int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
-{
-	unsigned long flags;
-	int new_ctx;
-	int cnt;
-
-	spin_lock_irqsave(&dev->condlock, flags);
-	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
-	cnt = 0;
-	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
-		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
-		if (++cnt > MFC_NUM_CONTEXTS) {
-			/* No contexts to run */
-			spin_unlock_irqrestore(&dev->condlock, flags);
-			return -EAGAIN;
-		}
-	}
-	spin_unlock_irqrestore(&dev->condlock, flags);
-	return new_ctx;
-}
-
 static void s5p_mfc_run_res_change(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1187,7 +1166,6 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf *temp_vb;
-	unsigned long flags;
 
 	if (ctx->state == MFCINST_FINISHING) {
 		last_frame = MFC_DEC_LAST_FRAME;
@@ -1197,11 +1175,9 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 		return 0;
 	}
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
 		mfc_debug(2, "No src buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 	/* Get the next source buffer */
@@ -1210,7 +1186,6 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 	s5p_mfc_set_dec_stream_buffer_v5(ctx,
 		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
 		ctx->consumed_stream, temp_vb->b->vb2_buf.planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	if (temp_vb->b->vb2_buf.planes[0].bytesused == 0) {
 		last_frame = MFC_DEC_LAST_FRAME;
@@ -1224,21 +1199,17 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *dst_mb;
 	struct s5p_mfc_buf *src_mb;
 	unsigned long src_y_addr, src_c_addr, dst_addr;
 	unsigned int dst_size;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
 		mfc_debug(2, "no src buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 	if (list_empty(&ctx->dst_queue)) {
 		mfc_debug(2, "no dst buffers\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 	if (list_empty(&ctx->src_queue)) {
@@ -1270,7 +1241,6 @@ static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
 	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
 	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	mfc_debug(2, "encoding buffer with index=%d state=%d\n",
 		  src_mb ? src_mb->b->vb2_buf.index : -1, ctx->state);
@@ -1281,11 +1251,9 @@ static int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *temp_vb;
 
 	/* Initializing decoding - parsing header */
-	spin_lock_irqsave(&dev->irqlock, flags);
 	mfc_debug(2, "Preparing to init decoding\n");
 	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	s5p_mfc_set_dec_desc_buffer(ctx);
@@ -1294,7 +1262,6 @@ static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
 	s5p_mfc_set_dec_stream_buffer_v5(ctx,
 			vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
 			0, temp_vb->b->vb2_buf.planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_init_decode_v5(ctx);
 }
@@ -1302,18 +1269,15 @@ static void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
 static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *dst_mb;
 	unsigned long dst_addr;
 	unsigned int dst_size;
 
 	s5p_mfc_set_enc_ref_buffer_v5(ctx);
-	spin_lock_irqsave(&dev->irqlock, flags);
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
 	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
 	s5p_mfc_set_enc_stream_buffer_v5(ctx, dst_addr, dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_init_encode_v5(ctx);
 }
@@ -1321,7 +1285,6 @@ static void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
 static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *temp_vb;
 	int ret;
 
@@ -1335,11 +1298,9 @@ static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 			"before starting processing\n");
 		return -EAGAIN;
 	}
-	spin_lock_irqsave(&dev->irqlock, flags);
 	if (list_empty(&ctx->src_queue)) {
 		mfc_err("Header has been deallocated in the middle of"
 			" initialization\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EIO;
 	}
 	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
@@ -1348,7 +1309,6 @@ static int s5p_mfc_run_init_dec_buffers(struct s5p_mfc_ctx *ctx)
 	s5p_mfc_set_dec_stream_buffer_v5(ctx,
 			vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
 			0, temp_vb->b->vb2_buf.planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	ret = s5p_mfc_set_dec_frame_buffer_v5(ctx);
 	if (ret) {
@@ -1472,21 +1432,6 @@ static void s5p_mfc_try_run_v5(struct s5p_mfc_dev *dev)
 	}
 }
 
-
-static void s5p_mfc_cleanup_queue_v5(struct list_head *lh, struct vb2_queue *vq)
-{
-	struct s5p_mfc_buf *b;
-	int i;
-
-	while (!list_empty(lh)) {
-		b = list_entry(lh->next, struct s5p_mfc_buf, list);
-		for (i = 0; i < b->b->vb2_buf.num_planes; i++)
-			vb2_set_plane_payload(&b->b->vb2_buf, i, 0);
-		vb2_buffer_done(&b->b->vb2_buf, VB2_BUF_STATE_ERROR);
-		list_del(&b->list);
-	}
-}
-
 static void s5p_mfc_clear_int_flags_v5(struct s5p_mfc_dev *dev)
 {
 	mfc_write(dev, 0, S5P_FIMV_RISC_HOST_INT);
@@ -1590,11 +1535,6 @@ static int s5p_mfc_err_dec_v5(unsigned int err)
 	return (err & S5P_FIMV_ERR_DEC_MASK) >> S5P_FIMV_ERR_DEC_SHIFT;
 }
 
-static int s5p_mfc_err_dspl_v5(unsigned int err)
-{
-	return (err & S5P_FIMV_ERR_DSPL_MASK) >> S5P_FIMV_ERR_DSPL_SHIFT;
-}
-
 static int s5p_mfc_get_img_width_v5(struct s5p_mfc_dev *dev)
 {
 	return mfc_read(dev, S5P_FIMV_SI_HRESOL);
@@ -1636,26 +1576,6 @@ static int s5p_mfc_get_enc_dpb_count_v5(struct s5p_mfc_dev *dev)
 	return -1;
 }
 
-static int s5p_mfc_get_enc_pic_count_v5(struct s5p_mfc_dev *dev)
-{
-	return mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT);
-}
-
-static int s5p_mfc_get_sei_avail_status_v5(struct s5p_mfc_ctx *ctx)
-{
-	return s5p_mfc_read_info_v5(ctx, FRAME_PACK_SEI_AVAIL);
-}
-
-static int s5p_mfc_get_mvc_num_views_v5(struct s5p_mfc_dev *dev)
-{
-	return -1;
-}
-
-static int s5p_mfc_get_mvc_view_id_v5(struct s5p_mfc_dev *dev)
-{
-	return -1;
-}
-
 static unsigned int s5p_mfc_get_pic_type_top_v5(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v5(ctx, PIC_TIME_TOP);
@@ -1688,20 +1608,11 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
 	.release_dev_context_buffer = s5p_mfc_release_dev_context_buffer_v5,
 	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v5,
 	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v5,
-	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v5,
-	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v5,
 	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v5,
 	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v5,
 	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v5,
-	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v5,
-	.init_decode = s5p_mfc_init_decode_v5,
-	.init_encode = s5p_mfc_init_encode_v5,
-	.encode_one_frame = s5p_mfc_encode_one_frame_v5,
 	.try_run = s5p_mfc_try_run_v5,
-	.cleanup_queue = s5p_mfc_cleanup_queue_v5,
 	.clear_int_flags = s5p_mfc_clear_int_flags_v5,
-	.write_info = s5p_mfc_write_info_v5,
-	.read_info = s5p_mfc_read_info_v5,
 	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v5,
 	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v5,
 	.get_dspl_status = s5p_mfc_get_dspl_status_v5,
@@ -1712,7 +1623,6 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
 	.get_int_reason = s5p_mfc_get_int_reason_v5,
 	.get_int_err = s5p_mfc_get_int_err_v5,
 	.err_dec = s5p_mfc_err_dec_v5,
-	.err_dspl = s5p_mfc_err_dspl_v5,
 	.get_img_width = s5p_mfc_get_img_width_v5,
 	.get_img_height = s5p_mfc_get_img_height_v5,
 	.get_dpb_count = s5p_mfc_get_dpb_count_v5,
@@ -1721,10 +1631,6 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v5 = {
 	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v5,
 	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v5,
 	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v5,
-	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v5,
-	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v5,
-	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v5,
-	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v5,
 	.get_pic_type_top = s5p_mfc_get_pic_type_top_v5,
 	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v5,
 	.get_crop_info_h = s5p_mfc_get_crop_info_h_v5,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index b95845347348..d6f207e859ab 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -505,7 +505,7 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 	}
 
 	writel(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
 	mfc_debug(2, "After setting buffers.\n");
@@ -603,7 +603,7 @@ static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 	}
 
 	writel(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
 	mfc_debug_leave();
@@ -1378,7 +1378,7 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 	writel(ctx->sei_fp_parse & 0x1, mfc_regs->d_sei_enable);
 
 	writel(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
 	mfc_debug_leave();
@@ -1393,7 +1393,7 @@ static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 	if (flush) {
 		dev->curr_ctx = ctx->num;
 		writel(ctx->inst_no, mfc_regs->instance_id);
-		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_H2R_CMD_FLUSH_V6, NULL);
 	}
 }
@@ -1413,11 +1413,11 @@ static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 	 * is the last frame or not. */
 	switch (last_frame) {
 	case 0:
-		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_CH_FRAME_START_V6, NULL);
 		break;
 	case 1:
-		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_CH_LAST_FRAME_V6, NULL);
 		break;
 	default:
@@ -1455,7 +1455,7 @@ static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 	}
 
 	writel(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
 	return 0;
@@ -1500,37 +1500,13 @@ static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
 		cmd = S5P_FIMV_CH_LAST_FRAME_V6;
 
 	writel(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev, cmd, NULL);
+	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev, cmd, NULL);
 
 	mfc_debug(2, "--\n");
 
 	return 0;
 }
 
-static inline int s5p_mfc_get_new_ctx(struct s5p_mfc_dev *dev)
-{
-	unsigned long flags;
-	int new_ctx;
-	int cnt;
-
-	spin_lock_irqsave(&dev->condlock, flags);
-	mfc_debug(2, "Previous context: %d (bits %08lx)\n", dev->curr_ctx,
-							dev->ctx_work_bits);
-	new_ctx = (dev->curr_ctx + 1) % MFC_NUM_CONTEXTS;
-	cnt = 0;
-	while (!test_bit(new_ctx, &dev->ctx_work_bits)) {
-		new_ctx = (new_ctx + 1) % MFC_NUM_CONTEXTS;
-		cnt++;
-		if (cnt > MFC_NUM_CONTEXTS) {
-			/* No contexts to run */
-			spin_unlock_irqrestore(&dev->condlock, flags);
-			return -EAGAIN;
-		}
-	}
-	spin_unlock_irqrestore(&dev->condlock, flags);
-	return new_ctx;
-}
-
 static inline void s5p_mfc_run_dec_last_frames(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1544,7 +1520,6 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf *temp_vb;
-	unsigned long flags;
 	int last_frame = 0;
 
 	if (ctx->state == MFCINST_FINISHING) {
@@ -1556,11 +1531,9 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 		return 0;
 	}
 
-	spin_lock_irqsave(&dev->irqlock, flags);
 	/* Frames are being decoded */
 	if (list_empty(&ctx->src_queue)) {
 		mfc_debug(2, "No src buffers.\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 	/* Get the next source buffer */
@@ -1570,7 +1543,6 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0),
 			ctx->consumed_stream,
 			temp_vb->b->vb2_buf.planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 
 	dev->curr_ctx = ctx->num;
 	if (temp_vb->b->vb2_buf.planes[0].bytesused == 0) {
@@ -1586,7 +1558,6 @@ static inline int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx)
 static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *dst_mb;
 	struct s5p_mfc_buf *src_mb;
 	unsigned long src_y_addr, src_c_addr, dst_addr;
@@ -1595,17 +1566,13 @@ static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 	*/
 	unsigned int dst_size;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
-
 	if (list_empty(&ctx->src_queue) && ctx->state != MFCINST_FINISHING) {
 		mfc_debug(2, "no src buffers.\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 
 	if (list_empty(&ctx->dst_queue)) {
 		mfc_debug(2, "no dst buffers.\n");
-		spin_unlock_irqrestore(&dev->irqlock, flags);
 		return -EAGAIN;
 	}
 
@@ -1639,8 +1606,6 @@ static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 
 	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
 
-	spin_unlock_irqrestore(&dev->irqlock, flags);
-
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_encode_one_frame_v6(ctx);
 
@@ -1650,18 +1615,15 @@ static inline int s5p_mfc_run_enc_frame(struct s5p_mfc_ctx *ctx)
 static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *temp_vb;
 
 	/* Initializing decoding - parsing header */
-	spin_lock_irqsave(&dev->irqlock, flags);
 	mfc_debug(2, "Preparing to init decoding.\n");
 	temp_vb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	mfc_debug(2, "Header size: %d\n", temp_vb->b->vb2_buf.planes[0].bytesused);
 	s5p_mfc_set_dec_stream_buffer_v6(ctx,
 		vb2_dma_contig_plane_dma_addr(&temp_vb->b->vb2_buf, 0), 0,
 			temp_vb->b->vb2_buf.planes[0].bytesused);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_init_decode_v6(ctx);
 }
@@ -1669,18 +1631,14 @@ static inline void s5p_mfc_run_init_dec(struct s5p_mfc_ctx *ctx)
 static inline void s5p_mfc_run_init_enc(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	unsigned long flags;
 	struct s5p_mfc_buf *dst_mb;
 	unsigned long dst_addr;
 	unsigned int dst_size;
 
-	spin_lock_irqsave(&dev->irqlock, flags);
-
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(&dst_mb->b->vb2_buf, 0);
 	dst_size = vb2_plane_size(&dst_mb->b->vb2_buf, 0);
 	s5p_mfc_set_enc_stream_buffer_v6(ctx, dst_addr, dst_size);
-	spin_unlock_irqrestore(&dev->irqlock, flags);
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_init_encode_v6(ctx);
 }
@@ -1846,21 +1804,6 @@ static void s5p_mfc_try_run_v6(struct s5p_mfc_dev *dev)
 	}
 }
 
-
-static void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
-{
-	struct s5p_mfc_buf *b;
-	int i;
-
-	while (!list_empty(lh)) {
-		b = list_entry(lh->next, struct s5p_mfc_buf, list);
-		for (i = 0; i < b->b->vb2_buf.num_planes; i++)
-			vb2_set_plane_payload(&b->b->vb2_buf, i, 0);
-		vb2_buffer_done(&b->b->vb2_buf, VB2_BUF_STATE_ERROR);
-		list_del(&b->list);
-	}
-}
-
 static void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
 {
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
@@ -1868,14 +1811,6 @@ static void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
 	writel(0, mfc_regs->risc2host_int);
 }
 
-static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
-		unsigned int ofs)
-{
-	s5p_mfc_clock_on();
-	writel(data, (void __iomem *)((unsigned long)ofs));
-	s5p_mfc_clock_off();
-}
-
 static unsigned int
 s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned long ofs)
 {
@@ -1942,11 +1877,6 @@ static int s5p_mfc_err_dec_v6(unsigned int err)
 	return (err & S5P_FIMV_ERR_DEC_MASK_V6) >> S5P_FIMV_ERR_DEC_SHIFT_V6;
 }
 
-static int s5p_mfc_err_dspl_v6(unsigned int err)
-{
-	return (err & S5P_FIMV_ERR_DSPL_MASK_V6) >> S5P_FIMV_ERR_DSPL_SHIFT_V6;
-}
-
 static int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
 {
 	return readl(dev->mfc_regs->d_display_frame_width);
@@ -1987,27 +1917,6 @@ static int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
 	return readl(dev->mfc_regs->e_slice_type);
 }
 
-static int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
-{
-	return readl(dev->mfc_regs->e_picture_count);
-}
-
-static int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
-{
-	struct s5p_mfc_dev *dev = ctx->dev;
-	return readl(dev->mfc_regs->d_frame_pack_sei_avail);
-}
-
-static int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
-{
-	return readl(dev->mfc_regs->d_mvc_num_views);
-}
-
-static int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
-{
-	return readl(dev->mfc_regs->d_mvc_view_id);
-}
-
 static unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx,
@@ -2282,20 +2191,11 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
 		s5p_mfc_release_dev_context_buffer_v6,
 	.dec_calc_dpb_size = s5p_mfc_dec_calc_dpb_size_v6,
 	.enc_calc_src_size = s5p_mfc_enc_calc_src_size_v6,
-	.set_dec_stream_buffer = s5p_mfc_set_dec_stream_buffer_v6,
-	.set_dec_frame_buffer = s5p_mfc_set_dec_frame_buffer_v6,
 	.set_enc_stream_buffer = s5p_mfc_set_enc_stream_buffer_v6,
 	.set_enc_frame_buffer = s5p_mfc_set_enc_frame_buffer_v6,
 	.get_enc_frame_buffer = s5p_mfc_get_enc_frame_buffer_v6,
-	.set_enc_ref_buffer = s5p_mfc_set_enc_ref_buffer_v6,
-	.init_decode = s5p_mfc_init_decode_v6,
-	.init_encode = s5p_mfc_init_encode_v6,
-	.encode_one_frame = s5p_mfc_encode_one_frame_v6,
 	.try_run = s5p_mfc_try_run_v6,
-	.cleanup_queue = s5p_mfc_cleanup_queue_v6,
 	.clear_int_flags = s5p_mfc_clear_int_flags_v6,
-	.write_info = s5p_mfc_write_info_v6,
-	.read_info = s5p_mfc_read_info_v6,
 	.get_dspl_y_adr = s5p_mfc_get_dspl_y_adr_v6,
 	.get_dec_y_adr = s5p_mfc_get_dec_y_adr_v6,
 	.get_dspl_status = s5p_mfc_get_dspl_status_v6,
@@ -2306,7 +2206,6 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
 	.get_int_reason = s5p_mfc_get_int_reason_v6,
 	.get_int_err = s5p_mfc_get_int_err_v6,
 	.err_dec = s5p_mfc_err_dec_v6,
-	.err_dspl = s5p_mfc_err_dspl_v6,
 	.get_img_width = s5p_mfc_get_img_width_v6,
 	.get_img_height = s5p_mfc_get_img_height_v6,
 	.get_dpb_count = s5p_mfc_get_dpb_count_v6,
@@ -2315,10 +2214,6 @@ static struct s5p_mfc_hw_ops s5p_mfc_ops_v6 = {
 	.get_enc_strm_size = s5p_mfc_get_enc_strm_size_v6,
 	.get_enc_slice_type = s5p_mfc_get_enc_slice_type_v6,
 	.get_enc_dpb_count = s5p_mfc_get_enc_dpb_count_v6,
-	.get_enc_pic_count = s5p_mfc_get_enc_pic_count_v6,
-	.get_sei_avail_status = s5p_mfc_get_sei_avail_status_v6,
-	.get_mvc_num_views = s5p_mfc_get_mvc_num_views_v6,
-	.get_mvc_view_id = s5p_mfc_get_mvc_view_id_v6,
 	.get_pic_type_top = s5p_mfc_get_pic_type_top_v6,
 	.get_pic_type_bot = s5p_mfc_get_pic_type_bot_v6,
 	.get_crop_info_h = s5p_mfc_get_crop_info_h_v6,
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 79940757b34f..e71b13e40f59 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -33,7 +33,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/v4l2-dv-timings.h>
 
-#include <media/s5p_hdmi.h>
+#include <linux/platform_data/media/s5p_hdmi.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-device.h>
@@ -627,7 +627,7 @@ static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
 
 	for (i = 0; i < ARRAY_SIZE(hdmi_timings); i++)
 		if (v4l2_match_dv_timings(&hdmi_timings[i].dv_timings,
-					timings, 0))
+					timings, 0, false))
 			break;
 	if (i == ARRAY_SIZE(hdmi_timings)) {
 		dev_err(dev, "timings not supported\n");
diff --git a/drivers/media/platform/s5p-tv/mixer_video.c b/drivers/media/platform/s5p-tv/mixer_video.c
index dc1c679e136c..d9e7f030294c 100644
--- a/drivers/media/platform/s5p-tv/mixer_video.c
+++ b/drivers/media/platform/s5p-tv/mixer_video.c
@@ -881,7 +881,7 @@ static const struct v4l2_file_operations mxr_fops = {
 	.unlocked_ioctl = video_ioctl2,
 };
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 	unsigned int *nbuffers, unsigned int *nplanes, unsigned int sizes[],
 	void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
index 8d171310af8f..0a97f9ab4f76 100644
--- a/drivers/media/platform/s5p-tv/sii9234_drv.c
+++ b/drivers/media/platform/s5p-tv/sii9234_drv.c
@@ -23,7 +23,7 @@
 #include <linux/regulator/machine.h>
 #include <linux/slab.h>
 
-#include <media/sii9234.h>
+#include <linux/platform_data/media/sii9234.h>
 #include <media/v4l2-subdev.h>
 
 MODULE_AUTHOR("Tomasz Stanislawski <t.stanislaws@samsung.com>");
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index d6ab33e7060a..82b5d69b87fa 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -865,32 +865,14 @@ static const struct v4l2_ioctl_ops sh_veu_ioctl_ops = {
 		/* ========== Queue operations ========== */
 
 static int sh_veu_queue_setup(struct vb2_queue *vq,
-			      const void *parg,
 			      unsigned int *nbuffers, unsigned int *nplanes,
 			      unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *f = parg;
 	struct sh_veu_dev *veu = vb2_get_drv_priv(vq);
-	struct sh_veu_vfmt *vfmt;
-	unsigned int size, count = *nbuffers;
-
-	if (f) {
-		const struct v4l2_pix_format *pix = &f->fmt.pix;
-		const struct sh_veu_format *fmt = sh_veu_find_fmt(f);
-		struct v4l2_format ftmp = *f;
-
-		if (fmt->fourcc != pix->pixelformat)
-			return -EINVAL;
-		sh_veu_try_fmt(&ftmp, fmt);
-		if (ftmp.fmt.pix.width != pix->width ||
-		    ftmp.fmt.pix.height != pix->height)
-			return -EINVAL;
-		size = pix->bytesperline ? pix->bytesperline * pix->height * fmt->depth / fmt->ydepth :
-			pix->width * pix->height * fmt->depth / fmt->ydepth;
-	} else {
-		vfmt = sh_veu_get_vfmt(veu, vq->type);
-		size = vfmt->bytesperline * vfmt->frame.height * vfmt->fmt->depth / vfmt->fmt->ydepth;
-	}
+	struct sh_veu_vfmt *vfmt = sh_veu_get_vfmt(veu, vq->type);
+	unsigned int count = *nbuffers;
+	unsigned int size = vfmt->bytesperline * vfmt->frame.height *
+		vfmt->fmt->depth / vfmt->fmt->ydepth;
 
 	if (count < 2)
 		*nbuffers = count = 2;
@@ -900,6 +882,11 @@ static int sh_veu_queue_setup(struct vb2_queue *vq,
 		*nbuffers = count;
 	}
 
+	if (*nplanes) {
+		alloc_ctxs[0] = veu->alloc_ctx;
+		return sizes[0] < size ? -EINVAL : 0;
+	}
+
 	*nplanes = 1;
 	sizes[0] = size;
 	alloc_ctxs[0] = veu->alloc_ctx;
@@ -1107,7 +1094,7 @@ static irqreturn_t sh_veu_isr(int irq, void *dev_id)
 	if (!src || !dst)
 		return IRQ_NONE;
 
-	dst->timestamp = src->timestamp;
+	dst->vb2_buf.timestamp = src->vb2_buf.timestamp;
 	dst->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 	dst->flags |=
 		src->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
diff --git a/drivers/media/platform/sh_vou.c b/drivers/media/platform/sh_vou.c
index 2231f8922df3..115740498274 100644
--- a/drivers/media/platform/sh_vou.c
+++ b/drivers/media/platform/sh_vou.c
@@ -22,7 +22,7 @@
 #include <linux/videodev2.h>
 #include <linux/module.h>
 
-#include <media/sh_vou.h>
+#include <media/drv-intf/sh_vou.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
@@ -243,22 +243,21 @@ static void sh_vou_stream_config(struct sh_vou_device *vou_dev)
 }
 
 /* Locking: caller holds fop_lock mutex */
-static int sh_vou_queue_setup(struct vb2_queue *vq, const void *parg,
+static int sh_vou_queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct sh_vou_device *vou_dev = vb2_get_drv_priv(vq);
 	struct v4l2_pix_format *pix = &vou_dev->pix;
 	int bytes_per_line = vou_fmt[vou_dev->pix_idx].bpp * pix->width / 8;
 
 	dev_dbg(vou_dev->v4l2_dev.dev, "%s()\n", __func__);
 
-	if (fmt && fmt->fmt.pix.sizeimage < pix->height * bytes_per_line)
-		return -EINVAL;
-	*nplanes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : pix->height * bytes_per_line;
 	alloc_ctxs[0] = vou_dev->alloc_ctx;
+	if (*nplanes)
+		return sizes[0] < pix->height * bytes_per_line ? -EINVAL : 0;
+	*nplanes = 1;
+	sizes[0] = pix->height * bytes_per_line;
 	return 0;
 }
 
@@ -1071,7 +1070,7 @@ static irqreturn_t sh_vou_isr(int irq, void *dev_id)
 
 	list_del(&vb->list);
 
-	v4l2_get_timestamp(&vb->vb.timestamp);
+	vb->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb->vb.sequence = vou_dev->sequence++;
 	vb->vb.field = V4L2_FIELD_INTERLACED;
 	vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 454f68f0cdad..1af779ee3c74 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -24,7 +24,7 @@
 #include <linux/slab.h>
 
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-of.h>
 #include <media/videobuf2-dma-contig.h>
 
@@ -79,6 +79,7 @@ struct atmel_isi {
 	dma_addr_t			fb_descriptors_phys;
 	struct				list_head dma_desc_head;
 	struct isi_dma_desc		dma_desc[MAX_BUFFER_NUM];
+	bool				enable_preview_path;
 
 	struct completion		complete;
 	/* ISI peripherial clock */
@@ -103,13 +104,55 @@ static u32 isi_readl(struct atmel_isi *isi, u32 reg)
 	return readl(isi->regs + reg);
 }
 
+static u32 setup_cfg2_yuv_swap(struct atmel_isi *isi,
+		const struct soc_camera_format_xlate *xlate)
+{
+	if (xlate->host_fmt->fourcc == V4L2_PIX_FMT_YUYV) {
+		/* all convert to YUYV */
+		switch (xlate->code) {
+		case MEDIA_BUS_FMT_VYUY8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_3;
+		case MEDIA_BUS_FMT_UYVY8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_2;
+		case MEDIA_BUS_FMT_YVYU8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_1;
+		}
+	} else if (xlate->host_fmt->fourcc == V4L2_PIX_FMT_RGB565) {
+		/*
+		 * Preview path is enabled, it will convert UYVY to RGB format.
+		 * But if sensor output format is not UYVY, we need to set
+		 * YCC_SWAP_MODE to convert it as UYVY.
+		 */
+		switch (xlate->code) {
+		case MEDIA_BUS_FMT_VYUY8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_1;
+		case MEDIA_BUS_FMT_YUYV8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_2;
+		case MEDIA_BUS_FMT_YVYU8_2X8:
+			return ISI_CFG2_YCC_SWAP_MODE_3;
+		}
+	}
+
+	/*
+	 * By default, no swap for the codec path of Atmel ISI. So codec
+	 * output is same as sensor's output.
+	 * For instance, if sensor's output is YUYV, then codec outputs YUYV.
+	 * And if sensor's output is UYVY, then codec outputs UYVY.
+	 */
+	return ISI_CFG2_YCC_SWAP_DEFAULT;
+}
+
 static void configure_geometry(struct atmel_isi *isi, u32 width,
-			u32 height, u32 code)
+		u32 height, const struct soc_camera_format_xlate *xlate)
 {
-	u32 cfg2;
+	u32 cfg2, psize;
+	u32 fourcc = xlate->host_fmt->fourcc;
+
+	isi->enable_preview_path = fourcc == V4L2_PIX_FMT_RGB565 ||
+				   fourcc == V4L2_PIX_FMT_RGB32;
 
 	/* According to sensor's output format to set cfg2 */
-	switch (code) {
+	switch (xlate->code) {
 	default:
 	/* Grey */
 	case MEDIA_BUS_FMT_Y8_1X8:
@@ -117,16 +160,11 @@ static void configure_geometry(struct atmel_isi *isi, u32 width,
 		break;
 	/* YUV */
 	case MEDIA_BUS_FMT_VYUY8_2X8:
-		cfg2 = ISI_CFG2_YCC_SWAP_MODE_3 | ISI_CFG2_COL_SPACE_YCbCr;
-		break;
 	case MEDIA_BUS_FMT_UYVY8_2X8:
-		cfg2 = ISI_CFG2_YCC_SWAP_MODE_2 | ISI_CFG2_COL_SPACE_YCbCr;
-		break;
 	case MEDIA_BUS_FMT_YVYU8_2X8:
-		cfg2 = ISI_CFG2_YCC_SWAP_MODE_1 | ISI_CFG2_COL_SPACE_YCbCr;
-		break;
 	case MEDIA_BUS_FMT_YUYV8_2X8:
-		cfg2 = ISI_CFG2_YCC_SWAP_DEFAULT | ISI_CFG2_COL_SPACE_YCbCr;
+		cfg2 = ISI_CFG2_COL_SPACE_YCbCr |
+				setup_cfg2_yuv_swap(isi, xlate);
 		break;
 	/* RGB, TODO */
 	}
@@ -139,6 +177,16 @@ static void configure_geometry(struct atmel_isi *isi, u32 width,
 	cfg2 |= ((height - 1) << ISI_CFG2_IM_VSIZE_OFFSET)
 			& ISI_CFG2_IM_VSIZE_MASK;
 	isi_writel(isi, ISI_CFG2, cfg2);
+
+	/* No down sampling, preview size equal to sensor output size */
+	psize = ((width - 1) << ISI_PSIZE_PREV_HSIZE_OFFSET) &
+		ISI_PSIZE_PREV_HSIZE_MASK;
+	psize |= ((height - 1) << ISI_PSIZE_PREV_VSIZE_OFFSET) &
+		ISI_PSIZE_PREV_VSIZE_MASK;
+	isi_writel(isi, ISI_PSIZE, psize);
+	isi_writel(isi, ISI_PDECF, ISI_PDECF_NO_SAMPLING);
+
+	return;
 }
 
 static bool is_supported(struct soc_camera_device *icd,
@@ -151,8 +199,9 @@ static bool is_supported(struct soc_camera_device *icd,
 	case V4L2_PIX_FMT_UYVY:
 	case V4L2_PIX_FMT_YVYU:
 	case V4L2_PIX_FMT_VYUY:
+	/* RGB */
+	case V4L2_PIX_FMT_RGB565:
 		return true;
-	/* RGB, TODO */
 	default:
 		return false;
 	}
@@ -165,7 +214,7 @@ static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
 		struct frame_buffer *buf = isi->active;
 
 		list_del_init(&buf->list);
-		v4l2_get_timestamp(&vbuf->timestamp);
+		vbuf->vb2_buf.timestamp = ktime_get_ns();
 		vbuf->sequence = isi->sequence++;
 		vb2_buffer_done(&vbuf->vb2_buf, VB2_BUF_STATE_DONE);
 	}
@@ -176,11 +225,19 @@ static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
 		/* start next dma frame. */
 		isi->active = list_entry(isi->video_buffer_list.next,
 					struct frame_buffer, list);
-		isi_writel(isi, ISI_DMA_C_DSCR,
-			(u32)isi->active->p_dma_desc->fbd_phys);
-		isi_writel(isi, ISI_DMA_C_CTRL,
-			ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+		if (!isi->enable_preview_path) {
+			isi_writel(isi, ISI_DMA_C_DSCR,
+				(u32)isi->active->p_dma_desc->fbd_phys);
+			isi_writel(isi, ISI_DMA_C_CTRL,
+				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+			isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+		} else {
+			isi_writel(isi, ISI_DMA_P_DSCR,
+				(u32)isi->active->p_dma_desc->fbd_phys);
+			isi_writel(isi, ISI_DMA_P_CTRL,
+				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+			isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
+		}
 	}
 	return IRQ_HANDLED;
 }
@@ -207,7 +264,8 @@ static irqreturn_t isi_interrupt(int irq, void *dev_id)
 		isi_writel(isi, ISI_INTDIS, ISI_CTRL_DIS);
 		ret = IRQ_HANDLED;
 	} else {
-		if (likely(pending & ISI_SR_CXFR_DONE))
+		if (likely(pending & ISI_SR_CXFR_DONE) ||
+				likely(pending & ISI_SR_PXFR_DONE))
 			ret = atmel_isi_handle_streaming(isi);
 	}
 
@@ -245,7 +303,7 @@ static int atmel_isi_wait_status(struct atmel_isi *isi, int wait_reset)
 /* ------------------------------------------------------------------
 	Videobuf operations
    ------------------------------------------------------------------*/
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -352,21 +410,35 @@ static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
 			ISI_SR_CXFR_DONE | ISI_SR_PXFR_DONE);
 
 	/* Check if already in a frame */
-	if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
-		dev_err(isi->soc_host.icd->parent, "Already in frame handling.\n");
-		return;
-	}
+	if (!isi->enable_preview_path) {
+		if (isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) {
+			dev_err(isi->soc_host.icd->parent, "Already in frame handling.\n");
+			return;
+		}
 
-	isi_writel(isi, ISI_DMA_C_DSCR, (u32)buffer->p_dma_desc->fbd_phys);
-	isi_writel(isi, ISI_DMA_C_CTRL, ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
-	isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+		isi_writel(isi, ISI_DMA_C_DSCR,
+				(u32)buffer->p_dma_desc->fbd_phys);
+		isi_writel(isi, ISI_DMA_C_CTRL,
+				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
+	} else {
+		isi_writel(isi, ISI_DMA_P_DSCR,
+				(u32)buffer->p_dma_desc->fbd_phys);
+		isi_writel(isi, ISI_DMA_P_CTRL,
+				ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
+		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_P_CH);
+	}
 
 	cfg1 &= ~ISI_CFG1_FRATE_DIV_MASK;
 	/* Enable linked list */
 	cfg1 |= isi->pdata.frate | ISI_CFG1_DISCR;
 
-	/* Enable codec path and ISI */
-	ctrl = ISI_CTRL_CDC | ISI_CTRL_EN;
+	/* Enable ISI */
+	ctrl = ISI_CTRL_EN;
+
+	if (!isi->enable_preview_path)
+		ctrl |= ISI_CTRL_CDC;
+
 	isi_writel(isi, ISI_CTRL, ctrl);
 	isi_writel(isi, ISI_CFG1, cfg1);
 }
@@ -411,7 +483,7 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	isi_writel(isi, ISI_INTDIS, (u32)~0UL);
 
 	configure_geometry(isi, icd->user_width, icd->user_height,
-				icd->current_fmt->code);
+				icd->current_fmt);
 
 	spin_lock_irq(&isi->lock);
 	/* Clear any pending interrupt */
@@ -443,15 +515,17 @@ static void stop_streaming(struct vb2_queue *vq)
 	}
 	spin_unlock_irq(&isi->lock);
 
-	timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
-	/* Wait until the end of the current frame. */
-	while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
-			time_before(jiffies, timeout))
-		msleep(1);
+	if (!isi->enable_preview_path) {
+		timeout = jiffies + FRAME_INTERVAL_MILLI_SEC * HZ;
+		/* Wait until the end of the current frame. */
+		while ((isi_readl(isi, ISI_STATUS) & ISI_CTRL_CDC) &&
+				time_before(jiffies, timeout))
+			msleep(1);
 
-	if (time_after(jiffies, timeout))
-		dev_err(icd->parent,
-			"Timeout waiting for finishing codec request\n");
+		if (time_after(jiffies, timeout))
+			dev_err(icd->parent,
+				"Timeout waiting for finishing codec request\n");
+	}
 
 	/* Disable interrupts */
 	isi_writel(isi, ISI_INTDIS,
@@ -617,6 +691,14 @@ static const struct soc_mbus_pixelfmt isi_camera_formats[] = {
 		.order			= SOC_MBUS_ORDER_LE,
 		.layout			= SOC_MBUS_LAYOUT_PACKED,
 	},
+	{
+		.fourcc			= V4L2_PIX_FMT_RGB565,
+		.name			= "RGB565",
+		.bits_per_sample	= 8,
+		.packing		= SOC_MBUS_PACKING_2X8_PADHI,
+		.order			= SOC_MBUS_ORDER_LE,
+		.layout			= SOC_MBUS_LAYOUT_PACKED,
+	},
 };
 
 /* This will be corrected as we get more formats */
@@ -673,7 +755,7 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 				  struct soc_camera_format_xlate *xlate)
 {
 	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
-	int formats = 0, ret;
+	int formats = 0, ret, i, n;
 	/* sensor format */
 	struct v4l2_subdev_mbus_code_enum code = {
 		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
@@ -707,13 +789,13 @@ static int isi_camera_get_formats(struct soc_camera_device *icd,
 	case MEDIA_BUS_FMT_VYUY8_2X8:
 	case MEDIA_BUS_FMT_YUYV8_2X8:
 	case MEDIA_BUS_FMT_YVYU8_2X8:
-		formats++;
-		if (xlate) {
-			xlate->host_fmt	= &isi_camera_formats[0];
+		n = ARRAY_SIZE(isi_camera_formats);
+		formats += n;
+		for (i = 0; xlate && i < n; i++, xlate++) {
+			xlate->host_fmt	= &isi_camera_formats[i];
 			xlate->code	= code.code;
-			xlate++;
 			dev_dbg(icd->parent, "Providing format %s using code %d\n",
-				isi_camera_formats[0].name, code.code);
+				xlate->host_fmt->name, xlate->code);
 		}
 		break;
 	default:
diff --git a/drivers/media/platform/soc_camera/atmel-isi.h b/drivers/media/platform/soc_camera/atmel-isi.h
index 5acc771d2edc..0acb32a2b65c 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.h
+++ b/drivers/media/platform/soc_camera/atmel-isi.h
@@ -79,6 +79,16 @@
 #define ISI_CFG2_IM_VSIZE_MASK		(0x7FF << ISI_CFG2_IM_VSIZE_OFFSET)
 #define ISI_CFG2_IM_HSIZE_MASK		(0x7FF << ISI_CFG2_IM_HSIZE_OFFSET)
 
+/* Bitfields in PSIZE */
+#define ISI_PSIZE_PREV_VSIZE_OFFSET	0
+#define ISI_PSIZE_PREV_HSIZE_OFFSET	16
+#define ISI_PSIZE_PREV_VSIZE_MASK	(0x3FF << ISI_PSIZE_PREV_VSIZE_OFFSET)
+#define ISI_PSIZE_PREV_HSIZE_MASK	(0x3FF << ISI_PSIZE_PREV_HSIZE_OFFSET)
+
+/* Bitfields in PDECF */
+#define ISI_PDECF_DEC_FACTOR_MASK	(0xFF << 0)
+#define	ISI_PDECF_NO_SAMPLING		(16)
+
 /* Bitfields in CTRL */
 /* Also using in SR(ISI_V2) */
 #define ISI_CTRL_EN				(1 << 0)
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index 1f28d21a3c9a..48dd5b7851b5 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -35,11 +35,11 @@
 #include <media/videobuf2-v4l2.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 
 #include <linux/videodev2.h>
 
-#include <linux/platform_data/camera-mx2.h>
+#include <linux/platform_data/media/camera-mx2.h>
 
 #include <asm/dma.h>
 
@@ -469,21 +469,15 @@ static void mx2_camera_clock_stop(struct soc_camera_host *ici)
  *  Videobuf operations
  */
 static int mx2_videobuf_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *count, unsigned int *num_planes,
 			unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx2_camera_dev *pcdev = ici->priv;
 
 	dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]);
 
-	/* TODO: support for VIDIOC_CREATE_BUFS not ready */
-	if (fmt != NULL)
-		return -ENOTTY;
-
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
 	sizes[0] = icd->sizeimage;
@@ -1351,7 +1345,7 @@ static void mx27_camera_frame_done_emma(struct mx2_camera_dev *pcdev,
 				vb2_get_plane_payload(vb, 0));
 
 		list_del_init(&buf->internal.queue);
-		v4l2_get_timestamp(&vbuf->timestamp);
+		vb->timestamp = ktime_get_ns();
 		vbuf->sequence = pcdev->frame_count;
 		if (err)
 			vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index 49c3a257a916..169ed1150226 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -23,9 +23,9 @@
 #include <media/v4l2-dev.h>
 #include <media/videobuf2-dma-contig.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 
-#include <linux/platform_data/camera-mx3.h>
+#include <linux/platform_data/media/camera-mx3.h>
 #include <linux/platform_data/dma-imx.h>
 
 #define MX3_CAM_DRV_NAME "mx3-camera"
@@ -155,7 +155,7 @@ static void mx3_cam_dma_done(void *arg)
 		struct mx3_camera_buffer *buf = to_mx3_vb(vb);
 
 		list_del_init(&buf->queue);
-		v4l2_get_timestamp(&vb->timestamp);
+		vb->vb2_buf.timestamp = ktime_get_ns();
 		vb->field = mx3_cam->field;
 		vb->sequence = mx3_cam->sequence++;
 		vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
@@ -185,11 +185,9 @@ static void mx3_cam_dma_done(void *arg)
  * Calculate the __buffer__ (not data) size and number of buffers.
  */
 static int mx3_videobuf_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *count, unsigned int *num_planes,
 			unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct mx3_camera_dev *mx3_cam = ici->priv;
@@ -197,33 +195,6 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	if (!mx3_cam->idmac_channel[0])
 		return -EINVAL;
 
-	if (fmt) {
-		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
-								fmt->fmt.pix.pixelformat);
-		unsigned int bytes_per_line;
-		int ret;
-
-		if (!xlate)
-			return -EINVAL;
-
-		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-					      xlate->host_fmt);
-		if (ret < 0)
-			return ret;
-
-		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
-
-		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
-					  fmt->fmt.pix.height);
-		if (ret < 0)
-			return ret;
-
-		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
-	} else {
-		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		sizes[0] = icd->sizeimage;
-	}
-
 	alloc_ctxs[0] = mx3_cam->alloc_ctx;
 
 	if (!vq->num_buffers)
@@ -232,9 +203,14 @@ static int mx3_videobuf_setup(struct vb2_queue *vq,
 	if (!*count)
 		*count = 2;
 
+	/* Called from VIDIOC_REQBUFS or in compatibility mode */
+	if (!*num_planes)
+		sizes[0] = icd->sizeimage;
+	else if (sizes[0] < icd->sizeimage)
+		return -EINVAL;
+
 	/* If *num_planes != 0, we have already verified *count. */
-	if (!*num_planes &&
-	    sizes[0] * *count + mx3_cam->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
+	if (sizes[0] * *count + mx3_cam->buf_total > MAX_VIDEO_MEM * 1024 * 1024)
 		*count = (MAX_VIDEO_MEM * 1024 * 1024 - mx3_cam->buf_total) /
 			sizes[0];
 
diff --git a/drivers/media/platform/soc_camera/omap1_camera.c b/drivers/media/platform/soc_camera/omap1_camera.c
index ba8dcd11ae0e..bd721e35474a 100644
--- a/drivers/media/platform/soc_camera/omap1_camera.c
+++ b/drivers/media/platform/soc_camera/omap1_camera.c
@@ -28,9 +28,9 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
-#include <media/omap1_camera.h>
+#include <linux/platform_data/media/omap1_camera.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/videobuf-dma-contig.h>
 #include <media/videobuf-dma-sg.h>
 
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index fcb942de0c7f..415f3bda60bf 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -33,13 +33,13 @@
 #include <media/v4l2-dev.h>
 #include <media/videobuf-dma-sg.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-of.h>
 
 #include <linux/videodev2.h>
 
 #include <mach/dma.h>
-#include <linux/platform_data/camera-pxa.h>
+#include <linux/platform_data/media/camera-pxa.h>
 
 #define PXA_CAM_VERSION "0.0.6"
 #define PXA_CAM_DRV_NAME "pxa27x-camera"
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index efe57b23fac1..b7fd695b9ed5 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -21,14 +21,13 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
-#include <linux/platform_data/camera-rcar.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/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-device.h>
@@ -138,6 +137,11 @@
 
 #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_GEN2,
 	RCAR_H1,
@@ -527,46 +531,14 @@ struct rcar_vin_cam {
  * required
  */
 static int rcar_vin_videobuf_setup(struct vb2_queue *vq,
-				   const void *parg,
 				   unsigned int *count,
 				   unsigned int *num_planes,
 				   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	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 (fmt) {
-		const struct soc_camera_format_xlate *xlate;
-		unsigned int bytes_per_line;
-		int ret;
-
-		if (fmt->fmt.pix.sizeimage < icd->sizeimage)
-			return -EINVAL;
-
-		xlate = soc_camera_xlate_by_fourcc(icd,
-						   fmt->fmt.pix.pixelformat);
-		if (!xlate)
-			return -EINVAL;
-		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-					      xlate->host_fmt);
-		if (ret < 0)
-			return ret;
-
-		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
-
-		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
-					  fmt->fmt.pix.height);
-		if (ret < 0)
-			return ret;
-
-		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
-	} else {
-		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		sizes[0] = icd->sizeimage;
-	}
-
 	alloc_ctxs[0] = priv->alloc_ctx;
 
 	if (!vq->num_buffers)
@@ -576,14 +548,18 @@ static int rcar_vin_videobuf_setup(struct vb2_queue *vq,
 		*count = 2;
 	priv->vb_count = *count;
 
-	*num_planes = 1;
-
 	/* 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;
@@ -912,7 +888,7 @@ static irqreturn_t rcar_vin_irq(int irq, void *data)
 
 		priv->queue_buf[slot]->field = priv->field;
 		priv->queue_buf[slot]->sequence = priv->sequence++;
-		v4l2_get_timestamp(&priv->queue_buf[slot]->timestamp);
+		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;
@@ -1853,63 +1829,43 @@ static const struct of_device_id rcar_vin_of_table[] = {
 MODULE_DEVICE_TABLE(of, rcar_vin_of_table);
 #endif
 
-static struct platform_device_id rcar_vin_id_table[] = {
-	{ "r8a7779-vin",  RCAR_H1 },
-	{ "r8a7778-vin",  RCAR_M1 },
-	{ "uPD35004-vin", RCAR_E1 },
-	{},
-};
-MODULE_DEVICE_TABLE(platform, rcar_vin_id_table);
-
 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;
-	struct rcar_vin_platform_data *pdata;
 	unsigned int pdata_flags;
 	int irq, ret;
 
-	if (pdev->dev.of_node) {
-		struct v4l2_of_endpoint ep;
-		struct device_node *np;
+	match = of_match_device(of_match_ptr(rcar_vin_of_table), &pdev->dev);
 
-		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;
-		}
+	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;
-		}
+	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;
-		}
+	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);
+	of_node_put(np);
 
-		dev_dbg(&pdev->dev, "pdata_flags = %08x\n", pdata_flags);
-	} else {
-		pdata = pdev->dev.platform_data;
-		if (!pdata || !pdata->flags) {
-			dev_err(&pdev->dev, "platform data not set\n");
-			return -EINVAL;
-		}
-		pdata_flags = pdata->flags;
-	}
+	dev_dbg(&pdev->dev, "pdata_flags = %08x\n", pdata_flags);
 
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (mem == NULL)
@@ -1992,7 +1948,6 @@ static struct platform_driver rcar_vin_driver = {
 		.name		= DRV_NAME,
 		.of_match_table	= of_match_ptr(rcar_vin_of_table),
 	},
-	.id_table	= rcar_vin_id_table,
 };
 
 module_platform_driver(rcar_vin_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 67a669d826b8..90c87f2b4ec0 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_ceu_camera.c
@@ -40,11 +40,11 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
 #include <media/soc_camera.h>
-#include <media/sh_mobile_ceu.h>
-#include <media/sh_mobile_csi2.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/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 
 #include "soc_scale_crop.h"
 
@@ -210,43 +210,14 @@ static int sh_mobile_ceu_soft_reset(struct sh_mobile_ceu_dev *pcdev)
  *		  for the current frame format if required
  */
 static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
-			const void *parg,
 			unsigned int *count, unsigned int *num_planes,
 			unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct soc_camera_device *icd = container_of(vq,
 			struct soc_camera_device, vb2_vidq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct sh_mobile_ceu_dev *pcdev = ici->priv;
 
-	if (fmt) {
-		const struct soc_camera_format_xlate *xlate = soc_camera_xlate_by_fourcc(icd,
-								fmt->fmt.pix.pixelformat);
-		unsigned int bytes_per_line;
-		int ret;
-
-		if (!xlate)
-			return -EINVAL;
-
-		ret = soc_mbus_bytes_per_line(fmt->fmt.pix.width,
-					      xlate->host_fmt);
-		if (ret < 0)
-			return ret;
-
-		bytes_per_line = max_t(u32, fmt->fmt.pix.bytesperline, ret);
-
-		ret = soc_mbus_image_size(xlate->host_fmt, bytes_per_line,
-					  fmt->fmt.pix.height);
-		if (ret < 0)
-			return ret;
-
-		sizes[0] = max_t(u32, fmt->fmt.pix.sizeimage, ret);
-	} else {
-		/* Called from VIDIOC_REQBUFS or in compatibility mode */
-		sizes[0] = icd->sizeimage;
-	}
-
 	alloc_ctxs[0] = pcdev->alloc_ctx;
 
 	if (!vq->num_buffers)
@@ -255,8 +226,14 @@ static int sh_mobile_ceu_videobuf_setup(struct vb2_queue *vq,
 	if (!*count)
 		*count = 2;
 
+	/* Called from VIDIOC_REQBUFS or in compatibility mode */
+	if (!*num_planes)
+		sizes[0] = icd->sizeimage;
+	else if (sizes[0] < icd->sizeimage)
+		return -EINVAL;
+
 	/* If *num_planes != 0, we have already verified *count. */
-	if (pcdev->video_limit && !*num_planes) {
+	if (pcdev->video_limit) {
 		size_t size = PAGE_ALIGN(sizes[0]) * *count;
 
 		if (size + pcdev->buf_total > pcdev->video_limit)
@@ -533,7 +510,7 @@ static irqreturn_t sh_mobile_ceu_irq(int irq, void *data)
 		pcdev->active = NULL;
 
 	ret = sh_mobile_ceu_capture(pcdev);
-	v4l2_get_timestamp(&vbuf->timestamp);
+	vbuf->vb2_buf.timestamp = ktime_get_ns();
 	if (!ret) {
 		vbuf->field = pcdev->field;
 		vbuf->sequence = pcdev->sequence++;
diff --git a/drivers/media/platform/soc_camera/sh_mobile_csi2.c b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
index 12d3626ecf22..09b18365a4b1 100644
--- a/drivers/media/platform/soc_camera/sh_mobile_csi2.c
+++ b/drivers/media/platform/soc_camera/sh_mobile_csi2.c
@@ -18,10 +18,10 @@
 #include <linux/videodev2.h>
 #include <linux/module.h>
 
-#include <media/sh_mobile_ceu.h>
-#include <media/sh_mobile_csi2.h>
+#include <media/drv-intf/sh_mobile_ceu.h>
+#include <media/drv-intf/sh_mobile_csi2.h>
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-dev.h>
 #include <media/v4l2-device.h>
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index dc98122e78dc..46c7186f7867 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -30,7 +30,7 @@
 #include <linux/vmalloc.h>
 
 #include <media/soc_camera.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 #include <media/v4l2-async.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
@@ -1360,7 +1360,7 @@ static int soc_camera_i2c_init(struct soc_camera_device *icd,
 	struct soc_camera_host_desc *shd = &sdesc->host_desc;
 	struct i2c_adapter *adap;
 	struct v4l2_subdev *subdev;
-	char clk_name[V4L2_SUBDEV_NAME_SIZE];
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	int ret;
 
 	/* First find out how we link the main client */
@@ -1391,8 +1391,8 @@ static int soc_camera_i2c_init(struct soc_camera_device *icd,
 	ssdd->sd_pdata.regulators = NULL;
 	shd->board_info->platform_data = ssdd;
 
-	snprintf(clk_name, sizeof(clk_name), "%d-%04x",
-		 shd->i2c_adapter_id, shd->board_info->addr);
+	v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+			  shd->i2c_adapter_id, shd->board_info->addr);
 
 	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
 	if (IS_ERR(icd->clk)) {
@@ -1493,6 +1493,8 @@ static void soc_camera_async_unbind(struct v4l2_async_notifier *notifier,
 					struct soc_camera_async_client, notifier);
 	struct soc_camera_device *icd = platform_get_drvdata(sasc->pdev);
 
+	icd->control = NULL;
+
 	if (icd->clk) {
 		v4l2_clk_unregister(icd->clk);
 		icd->clk = NULL;
@@ -1526,7 +1528,7 @@ static int scan_async_group(struct soc_camera_host *ici,
 	struct soc_camera_async_client *sasc;
 	struct soc_camera_device *icd;
 	struct soc_camera_desc sdesc = {.host_desc.bus_id = ici->nr,};
-	char clk_name[V4L2_SUBDEV_NAME_SIZE];
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	unsigned int i;
 	int ret;
 
@@ -1572,8 +1574,9 @@ static int scan_async_group(struct soc_camera_host *ici,
 	icd->sasc = sasc;
 	icd->parent = ici->v4l2_dev.dev;
 
-	snprintf(clk_name, sizeof(clk_name), "%d-%04x",
-		 sasd->asd.match.i2c.adapter_id, sasd->asd.match.i2c.address);
+	v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+			  sasd->asd.match.i2c.adapter_id,
+			  sasd->asd.match.i2c.address);
 
 	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
 	if (IS_ERR(icd->clk)) {
@@ -1631,7 +1634,7 @@ static int soc_of_bind(struct soc_camera_host *ici,
 	struct soc_camera_async_client *sasc;
 	struct soc_of_info *info;
 	struct i2c_client *client;
-	char clk_name[V4L2_SUBDEV_NAME_SIZE + 32];
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	int ret;
 
 	/* allocate a new subdev and add match info to it */
@@ -1674,11 +1677,11 @@ static int soc_of_bind(struct soc_camera_host *ici,
 	client = of_find_i2c_device_by_node(remote);
 
 	if (client)
-		snprintf(clk_name, sizeof(clk_name), "%d-%04x",
-			 client->adapter->nr, client->addr);
+		v4l2_clk_name_i2c(clk_name, sizeof(clk_name),
+				  client->adapter->nr, client->addr);
 	else
-		snprintf(clk_name, sizeof(clk_name), "of-%s",
-			 of_node_full_name(remote));
+		v4l2_clk_name_of(clk_name, sizeof(clk_name),
+				 of_node_full_name(remote));
 
 	icd->clk = v4l2_clk_register(&soc_camera_clk_ops, clk_name, icd);
 	if (IS_ERR(icd->clk)) {
diff --git a/drivers/media/platform/soc_camera/soc_camera_platform.c b/drivers/media/platform/soc_camera/soc_camera_platform.c
index cc8eb0758219..a51d2a42998c 100644
--- a/drivers/media/platform/soc_camera/soc_camera_platform.c
+++ b/drivers/media/platform/soc_camera/soc_camera_platform.c
@@ -18,7 +18,7 @@
 #include <linux/videodev2.h>
 #include <media/v4l2-subdev.h>
 #include <media/soc_camera.h>
-#include <media/soc_camera_platform.h>
+#include <linux/platform_data/media/soc_camera_platform.h>
 
 struct soc_camera_platform_priv {
 	struct v4l2_subdev subdev;
diff --git a/drivers/media/platform/soc_camera/soc_mediabus.c b/drivers/media/platform/soc_camera/soc_mediabus.c
index 1dbcd426683c..e3e665e1c503 100644
--- a/drivers/media/platform/soc_camera/soc_mediabus.c
+++ b/drivers/media/platform/soc_camera/soc_mediabus.c
@@ -13,7 +13,7 @@
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
-#include <media/soc_mediabus.h>
+#include <media/drv-intf/soc_mediabus.h>
 
 static const struct soc_mbus_lookup mbus_fmt[] = {
 {
diff --git a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
index a0d267e017f6..d12a419c044a 100644
--- a/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
+++ b/drivers/media/platform/sti/bdisp/bdisp-v4l2.c
@@ -191,7 +191,7 @@ static void bdisp_job_finish(struct bdisp_ctx *ctx, int vb_state)
 	dst_vb = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 
 	if (src_vb && dst_vb) {
-		dst_vb->timestamp = src_vb->timestamp;
+		dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 		dst_vb->timecode = src_vb->timecode;
 		dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
 		dst_vb->flags |= src_vb->flags &
@@ -297,7 +297,7 @@ static int bdisp_get_bufs(struct bdisp_ctx *ctx)
 	if (ret)
 		return ret;
 
-	dst_vb->timestamp = src_vb->timestamp;
+	dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp;
 
 	return 0;
 }
@@ -438,11 +438,9 @@ static void bdisp_ctrls_delete(struct bdisp_ctx *ctx)
 }
 
 static int bdisp_queue_setup(struct vb2_queue *vq,
-			     const void *parg,
 			     unsigned int *nb_buf, unsigned int *nb_planes,
 			     unsigned int sizes[], void *allocators[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct bdisp_ctx *ctx = vb2_get_drv_priv(vq);
 	struct bdisp_frame *frame = ctx_get_frame(ctx, vq->type);
 
@@ -455,13 +453,13 @@ static int bdisp_queue_setup(struct vb2_queue *vq,
 		dev_err(ctx->bdisp_dev->dev, "Invalid format\n");
 		return -EINVAL;
 	}
+	allocators[0] = ctx->bdisp_dev->alloc_ctx;
 
-	if (fmt && fmt->fmt.pix.sizeimage < frame->sizeimage)
-		return -EINVAL;
+	if (*nb_planes)
+		return sizes[0] < frame->sizeimage ? -EINVAL : 0;
 
 	*nb_planes = 1;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : frame->sizeimage;
-	allocators[0] = ctx->bdisp_dev->alloc_ctx;
+	sizes[0] = frame->sizeimage;
 
 	return 0;
 }
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
index 95223ab71e19..2dfbe8ab5214 100644
--- a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-common.c
@@ -209,18 +209,18 @@ void c8sectpfe_tuner_unregister_frontend(struct c8sectpfe *c8sectpfe,
 
 		tsin = fei->channel_data[n];
 
-		if (tsin && tsin->frontend) {
-			dvb_unregister_frontend(tsin->frontend);
-			dvb_frontend_detach(tsin->frontend);
-		}
+		if (tsin) {
+			if (tsin->frontend) {
+				dvb_unregister_frontend(tsin->frontend);
+				dvb_frontend_detach(tsin->frontend);
+			}
 
-		if (tsin && tsin->i2c_adapter)
 			i2c_put_adapter(tsin->i2c_adapter);
 
-		if (tsin && tsin->i2c_client) {
-			if (tsin->i2c_client->dev.driver->owner)
+			if (tsin->i2c_client) {
 				module_put(tsin->i2c_client->dev.driver->owner);
-			i2c_unregister_device(tsin->i2c_client);
+				i2c_unregister_device(tsin->i2c_client);
+			}
 		}
 	}
 
diff --git a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
index 8490a65ae1c6..78e3cb9a628f 100644
--- a/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
+++ b/drivers/media/platform/sti/c8sectpfe/c8sectpfe-core.c
@@ -823,7 +823,7 @@ static int c8sectpfe_probe(struct platform_device *pdev)
 		}
 		of_node_put(i2c_bus);
 
-		tsin->rst_gpio = of_get_named_gpio(child, "rst-gpio", 0);
+		tsin->rst_gpio = of_get_named_gpio(child, "reset-gpios", 0);
 
 		ret = gpio_is_valid(tsin->rst_gpio);
 		if (!ret) {
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index de24effd984f..1fa00c2cf3d7 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -1288,7 +1288,7 @@ static irqreturn_t vpe_irq(int irq_vpe, void *data)
 	d_vb = ctx->dst_vb;
 
 	d_vb->flags = s_vb->flags;
-	d_vb->timestamp = s_vb->timestamp;
+	d_vb->vb2_buf.timestamp = s_vb->vb2_buf.timestamp;
 
 	if (s_vb->flags & V4L2_BUF_FLAG_TIMECODE)
 		d_vb->timecode = s_vb->timecode;
@@ -1796,7 +1796,6 @@ static const struct v4l2_ioctl_ops vpe_ioctl_ops = {
  * Queue operations
  */
 static int vpe_queue_setup(struct vb2_queue *vq,
-			   const void *parg,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/timblogiw.c b/drivers/media/platform/timblogiw.c
index 5820e45b3a9f..113c9f3c0b3e 100644
--- a/drivers/media/platform/timblogiw.c
+++ b/drivers/media/platform/timblogiw.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
 #include <media/videobuf-dma-contig.h>
-#include <media/timb_video.h>
+#include <linux/platform_data/media/timb_video.h>
 
 #define DRIVER_NAME			"timb-video"
 
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
index 32e4ff46daf3..1254f7e4d732 100644
--- a/drivers/media/platform/via-camera.c
+++ b/drivers/media/platform/via-camera.c
@@ -19,7 +19,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-image-sizes.h>
-#include <media/ov7670.h>
+#include <media/i2c/ov7670.h>
 #include <media/videobuf-dma-sg.h>
 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
diff --git a/drivers/media/platform/vim2m.c b/drivers/media/platform/vim2m.c
index e18fb9f9ed2f..418113c99801 100644
--- a/drivers/media/platform/vim2m.c
+++ b/drivers/media/platform/vim2m.c
@@ -235,7 +235,7 @@ static int device_process(struct vim2m_ctx *ctx,
 	out_vb->sequence =
 		get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
 	in_vb->sequence = q_data->sequence++;
-	out_vb->timestamp = in_vb->timestamp;
+	out_vb->vb2_buf.timestamp = in_vb->vb2_buf.timestamp;
 
 	if (in_vb->flags & V4L2_BUF_FLAG_TIMECODE)
 		out_vb->timecode = in_vb->timecode;
@@ -710,11 +710,9 @@ static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
  */
 
 static int vim2m_queue_setup(struct vb2_queue *vq,
-				const void *parg,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
 	struct vim2m_q_data *q_data;
 	unsigned int size, count = *nbuffers;
@@ -723,17 +721,14 @@ static int vim2m_queue_setup(struct vb2_queue *vq,
 
 	size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
 
-	if (fmt) {
-		if (fmt->fmt.pix.sizeimage < size)
-			return -EINVAL;
-		size = fmt->fmt.pix.sizeimage;
-	}
-
 	while (size * count > MEM2MEM_VID_MEM_LIMIT)
 		(count)--;
+	*nbuffers = count;
+
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 
 	*nplanes = 1;
-	*nbuffers = count;
 	sizes[0] = size;
 
 	/*
diff --git a/drivers/media/platform/vivid/vivid-core.h b/drivers/media/platform/vivid/vivid-core.h
index 55b304a705d5..751c1ba391e9 100644
--- a/drivers/media/platform/vivid/vivid-core.h
+++ b/drivers/media/platform/vivid/vivid-core.h
@@ -264,6 +264,7 @@ struct vivid_dev {
 	bool				vflip;
 	bool				vbi_cap_interlaced;
 	bool				loop_video;
+	bool				reduced_fps;
 
 	/* Framebuffer */
 	unsigned long			video_pbase;
@@ -285,7 +286,7 @@ struct vivid_dev {
 	bool				dqbuf_error;
 	bool				seq_wrap;
 	bool				time_wrap;
-	__kernel_time_t			time_wrap_offset;
+	u64				time_wrap_offset;
 	unsigned			perc_dropped_buffers;
 	enum vivid_signal_mode		std_signal_mode;
 	unsigned			query_std_last;
diff --git a/drivers/media/platform/vivid/vivid-ctrls.c b/drivers/media/platform/vivid/vivid-ctrls.c
index f41ac0b01fec..b98089c95ef5 100644
--- a/drivers/media/platform/vivid/vivid-ctrls.c
+++ b/drivers/media/platform/vivid/vivid-ctrls.c
@@ -78,6 +78,7 @@
 #define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 39)
 #define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 40)
 #define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 41)
+#define VIVID_CID_REDUCED_FPS		(VIVID_CID_VIVID_BASE + 42)
 
 #define VIVID_CID_STD_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 60)
 #define VIVID_CID_STANDARD		(VIVID_CID_VIVID_BASE + 61)
@@ -424,6 +425,10 @@ static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
 		dev->sensor_vflip = ctrl->val;
 		tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
 		break;
+	case VIVID_CID_REDUCED_FPS:
+		dev->reduced_fps = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
 	case VIVID_CID_HAS_CROP_CAP:
 		dev->has_crop_cap = ctrl->val;
 		vivid_update_format_cap(dev, true);
@@ -601,6 +606,15 @@ static const struct v4l2_ctrl_config vivid_ctrl_vflip = {
 	.step = 1,
 };
 
+static const struct v4l2_ctrl_config vivid_ctrl_reduced_fps = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_REDUCED_FPS,
+	.name = "Reduced Framerate",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
 static const struct v4l2_ctrl_config vivid_ctrl_has_crop_cap = {
 	.ops = &vivid_vid_cap_ctrl_ops,
 	.id = VIVID_CID_HAS_CROP_CAP,
@@ -940,7 +954,7 @@ static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_out = {
 static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_streaming);
-	struct timeval tv;
+	u64 rem;
 
 	switch (ctrl->id) {
 	case VIVID_CID_DQBUF_ERROR:
@@ -979,8 +993,16 @@ static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
 			dev->time_wrap_offset = 0;
 			break;
 		}
-		v4l2_get_timestamp(&tv);
-		dev->time_wrap_offset = -tv.tv_sec - 16;
+		/*
+		 * We want to set the time 16 seconds before the 32 bit tv_sec
+		 * value of struct timeval would wrap around. So first we
+		 * calculate ktime_get_ns() % ((1 << 32) * NSEC_PER_SEC), and
+		 * then we set the offset to ((1 << 32) - 16) * NSEC_PER_SEC).
+		 */
+		div64_u64_rem(ktime_get_ns(),
+			0x100000000ULL * NSEC_PER_SEC, &rem);
+		dev->time_wrap_offset =
+			(0x100000000ULL - 16) * NSEC_PER_SEC - rem;
 		break;
 	}
 	return 0;
@@ -1340,11 +1362,13 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 	v4l2_ctrl_handler_init(hdl_vid_cap, 55);
 	v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_vid_out, 26);
-	v4l2_ctrl_new_custom(hdl_vid_out, &vivid_ctrl_class, NULL);
+	if (!no_error_inj)
+		v4l2_ctrl_new_custom(hdl_vid_out, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_vbi_cap, 21);
 	v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_vbi_out, 19);
-	v4l2_ctrl_new_custom(hdl_vbi_out, &vivid_ctrl_class, NULL);
+	if (!no_error_inj)
+		v4l2_ctrl_new_custom(hdl_vbi_out, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_radio_rx, 17);
 	v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_class, NULL);
 	v4l2_ctrl_handler_init(hdl_radio_tx, 17);
@@ -1414,6 +1438,7 @@ int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vflip, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_sav, NULL);
 		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_eav, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_reduced_fps, NULL);
 		if (show_ccs_cap) {
 			dev->ctrl_has_crop_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
 				&vivid_ctrl_has_crop_cap, NULL);
diff --git a/drivers/media/platform/vivid/vivid-kthread-cap.c b/drivers/media/platform/vivid/vivid-kthread-cap.c
index 83cc6d3b4784..9034281944a4 100644
--- a/drivers/media/platform/vivid/vivid-kthread-cap.c
+++ b/drivers/media/platform/vivid/vivid-kthread-cap.c
@@ -441,7 +441,7 @@ static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 	 * "Start of Exposure".
 	 */
 	if (dev->tstamp_src_is_soe)
-		v4l2_get_timestamp(&buf->vb.timestamp);
+		buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
 		/*
 		 * 60 Hz standards start with the bottom field, 50 Hz standards
@@ -558,8 +558,8 @@ static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
 	 * the timestamp now.
 	 */
 	if (!dev->tstamp_src_is_soe)
-		v4l2_get_timestamp(&buf->vb.timestamp);
-	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		buf->vb.vb2_buf.timestamp = ktime_get_ns();
+	buf->vb.vb2_buf.timestamp += dev->time_wrap_offset;
 }
 
 /*
diff --git a/drivers/media/platform/vivid/vivid-kthread-out.c b/drivers/media/platform/vivid/vivid-kthread-out.c
index c2c46dcdbe95..98eed5889bc1 100644
--- a/drivers/media/platform/vivid/vivid-kthread-out.c
+++ b/drivers/media/platform/vivid/vivid-kthread-out.c
@@ -95,8 +95,8 @@ static void vivid_thread_vid_out_tick(struct vivid_dev *dev)
 			 */
 			vid_out_buf->vb.sequence /= 2;
 		}
-		v4l2_get_timestamp(&vid_out_buf->vb.timestamp);
-		vid_out_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		vid_out_buf->vb.vb2_buf.timestamp =
+			ktime_get_ns() + dev->time_wrap_offset;
 		vb2_buffer_done(&vid_out_buf->vb.vb2_buf, dev->dqbuf_error ?
 				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 		dprintk(dev, 2, "vid_out buffer %d done\n",
@@ -108,8 +108,8 @@ static void vivid_thread_vid_out_tick(struct vivid_dev *dev)
 			vivid_sliced_vbi_out_process(dev, vbi_out_buf);
 
 		vbi_out_buf->vb.sequence = dev->vbi_out_seq_count;
-		v4l2_get_timestamp(&vbi_out_buf->vb.timestamp);
-		vbi_out_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		vbi_out_buf->vb.vb2_buf.timestamp =
+			ktime_get_ns() + dev->time_wrap_offset;
 		vb2_buffer_done(&vbi_out_buf->vb.vb2_buf, dev->dqbuf_error ?
 				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 		dprintk(dev, 2, "vbi_out buffer %d done\n",
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.c b/drivers/media/platform/vivid/vivid-sdr-cap.c
index 082c401764ce..3d1604cb982f 100644
--- a/drivers/media/platform/vivid/vivid-sdr-cap.c
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.c
@@ -117,8 +117,8 @@ static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev)
 	if (sdr_cap_buf) {
 		sdr_cap_buf->vb.sequence = dev->sdr_cap_seq_count;
 		vivid_sdr_cap_process(dev, sdr_cap_buf);
-		v4l2_get_timestamp(&sdr_cap_buf->vb.timestamp);
-		sdr_cap_buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+		sdr_cap_buf->vb.vb2_buf.timestamp =
+			ktime_get_ns() + dev->time_wrap_offset;
 		vb2_buffer_done(&sdr_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
 				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 		dev->dqbuf_error = false;
@@ -213,7 +213,7 @@ static int vivid_thread_sdr_cap(void *data)
 	return 0;
 }
 
-static int sdr_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+static int sdr_cap_queue_setup(struct vb2_queue *vq,
 		       unsigned *nbuffers, unsigned *nplanes,
 		       unsigned sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/vivid/vivid-vbi-cap.c b/drivers/media/platform/vivid/vivid-vbi-cap.c
index e903d023e9df..cda45a582bfe 100644
--- a/drivers/media/platform/vivid/vivid-vbi-cap.c
+++ b/drivers/media/platform/vivid/vivid-vbi-cap.c
@@ -108,8 +108,7 @@ void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
 	if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode))
 		vivid_vbi_gen_raw(&dev->vbi_gen, &vbi, vbuf);
 
-	v4l2_get_timestamp(&buf->vb.timestamp);
-	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+	buf->vb.vb2_buf.timestamp = ktime_get_ns() + dev->time_wrap_offset;
 }
 
 
@@ -133,11 +132,10 @@ void vivid_sliced_vbi_cap_process(struct vivid_dev *dev,
 			vbuf[i] = dev->vbi_gen.data[i];
 	}
 
-	v4l2_get_timestamp(&buf->vb.timestamp);
-	buf->vb.timestamp.tv_sec += dev->time_wrap_offset;
+	buf->vb.vb2_buf.timestamp = ktime_get_ns() + dev->time_wrap_offset;
 }
 
-static int vbi_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vbi_cap_queue_setup(struct vb2_queue *vq,
 		       unsigned *nbuffers, unsigned *nplanes,
 		       unsigned sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/vivid/vivid-vbi-out.c b/drivers/media/platform/vivid/vivid-vbi-out.c
index 75c5709f938e..3c5a469e6f49 100644
--- a/drivers/media/platform/vivid/vivid-vbi-out.c
+++ b/drivers/media/platform/vivid/vivid-vbi-out.c
@@ -27,7 +27,7 @@
 #include "vivid-vbi-out.h"
 #include "vivid-vbi-cap.h"
 
-static int vbi_out_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vbi_out_queue_setup(struct vb2_queue *vq,
 		       unsigned *nbuffers, unsigned *nplanes,
 		       unsigned sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
index ef5412311b2f..b84f081c1b92 100644
--- a/drivers/media/platform/vivid/vivid-vid-cap.c
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -95,11 +95,10 @@ static const struct v4l2_discrete_probe webcam_probe = {
 	VIVID_WEBCAM_SIZES
 };
 
-static int vid_cap_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vid_cap_queue_setup(struct vb2_queue *vq,
 		       unsigned *nbuffers, unsigned *nplanes,
 		       unsigned sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
 	unsigned buffers = tpg_g_buffers(&dev->tpg);
 	unsigned h = dev->fmt_cap_rect.height;
@@ -122,27 +121,16 @@ static int vid_cap_queue_setup(struct vb2_queue *vq, const void *parg,
 		dev->queue_setup_error = false;
 		return -EINVAL;
 	}
-	if (fmt) {
-		const struct v4l2_pix_format_mplane *mp;
-		struct v4l2_format mp_fmt;
-		const struct vivid_fmt *vfmt;
-
-		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
-			fmt_sp2mp(fmt, &mp_fmt);
-			fmt = &mp_fmt;
-		}
-		mp = &fmt->fmt.pix_mp;
+	if (*nplanes) {
 		/*
-		 * Check if the number of planes in the specified format match
+		 * Check if the number of requested planes match
 		 * the number of buffers in the current format. You can't mix that.
 		 */
-		if (mp->num_planes != buffers)
+		if (*nplanes != buffers)
 			return -EINVAL;
-		vfmt = vivid_get_format(dev, mp->pixelformat);
 		for (p = 0; p < buffers; p++) {
-			sizes[p] = mp->plane_fmt[p].sizeimage;
 			if (sizes[p] < tpg_g_line_width(&dev->tpg, p) * h +
-							vfmt->data_offset[p])
+						dev->fmt_cap->data_offset[p])
 				return -EINVAL;
 		}
 	} else {
@@ -405,6 +393,7 @@ void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls)
 {
 	struct v4l2_bt_timings *bt = &dev->dv_timings_cap.bt;
 	unsigned size;
+	u64 pixelclock;
 
 	switch (dev->input_type[dev->input]) {
 	case WEBCAM:
@@ -434,8 +423,15 @@ void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls)
 		dev->src_rect.width = bt->width;
 		dev->src_rect.height = bt->height;
 		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+		if (dev->reduced_fps && can_reduce_fps(bt)) {
+			pixelclock = div_u64(bt->pixelclock * 1000, 1001);
+			bt->flags |= V4L2_DV_FL_REDUCED_FPS;
+		} else {
+			pixelclock = bt->pixelclock;
+			bt->flags &= ~V4L2_DV_FL_REDUCED_FPS;
+		}
 		dev->timeperframe_vid_cap = (struct v4l2_fract) {
-			size / 100, (u32)bt->pixelclock / 100
+			size / 100, (u32)pixelclock / 100
 		};
 		if (bt->interlaced)
 			dev->field_cap = V4L2_FIELD_ALTERNATE;
@@ -1662,7 +1658,7 @@ int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh,
 	    !valid_cvt_gtf_timings(timings))
 		return -EINVAL;
 
-	if (v4l2_match_dv_timings(timings, &dev->dv_timings_cap, 0))
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_cap, 0, false))
 		return 0;
 	if (vb2_is_busy(&dev->vb_vid_cap_q))
 		return -EBUSY;
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
index b77acb6a7013..64e4d66482c1 100644
--- a/drivers/media/platform/vivid/vivid-vid-out.c
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -31,11 +31,10 @@
 #include "vivid-kthread-out.h"
 #include "vivid-vid-out.h"
 
-static int vid_out_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vid_out_queue_setup(struct vb2_queue *vq,
 		       unsigned *nbuffers, unsigned *nplanes,
 		       unsigned sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct vivid_dev *dev = vb2_get_drv_priv(vq);
 	const struct vivid_fmt *vfmt = dev->fmt_out;
 	unsigned planes = vfmt->buffers;
@@ -64,26 +63,16 @@ static int vid_out_queue_setup(struct vb2_queue *vq, const void *parg,
 		return -EINVAL;
 	}
 
-	if (fmt) {
-		const struct v4l2_pix_format_mplane *mp;
-		struct v4l2_format mp_fmt;
-
-		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
-			fmt_sp2mp(fmt, &mp_fmt);
-			fmt = &mp_fmt;
-		}
-		mp = &fmt->fmt.pix_mp;
+	if (*nplanes) {
 		/*
-		 * Check if the number of planes in the specified format match
+		 * Check if the number of requested planes match
 		 * the number of planes in the current format. You can't mix that.
 		 */
-		if (mp->num_planes != planes)
+		if (*nplanes != planes)
 			return -EINVAL;
-		sizes[0] = mp->plane_fmt[0].sizeimage;
 		if (sizes[0] < size)
 			return -EINVAL;
 		for (p = 1; p < planes; p++) {
-			sizes[p] = mp->plane_fmt[p].sizeimage;
 			if (sizes[p] < dev->bytesperline_out[p] * h)
 				return -EINVAL;
 		}
@@ -224,6 +213,7 @@ void vivid_update_format_out(struct vivid_dev *dev)
 {
 	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
 	unsigned size, p;
+	u64 pixelclock;
 
 	switch (dev->output_type[dev->output]) {
 	case SVID:
@@ -245,8 +235,14 @@ void vivid_update_format_out(struct vivid_dev *dev)
 		dev->sink_rect.width = bt->width;
 		dev->sink_rect.height = bt->height;
 		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+
+		if (can_reduce_fps(bt) && (bt->flags & V4L2_DV_FL_REDUCED_FPS))
+			pixelclock = div_u64(bt->pixelclock * 1000, 1001);
+		else
+			pixelclock = bt->pixelclock;
+
 		dev->timeperframe_vid_out = (struct v4l2_fract) {
-			size / 100, (u32)bt->pixelclock / 100
+			size / 100, (u32)pixelclock / 100
 		};
 		if (bt->interlaced)
 			dev->field_out = V4L2_FIELD_ALTERNATE;
@@ -1149,7 +1145,7 @@ int vivid_vid_out_s_dv_timings(struct file *file, void *_fh,
 				0, NULL, NULL) &&
 	    !valid_cvt_gtf_timings(timings))
 		return -EINVAL;
-	if (v4l2_match_dv_timings(timings, &dev->dv_timings_out, 0))
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_out, 0, true))
 		return 0;
 	if (vb2_is_busy(&dev->vb_vid_out_q))
 		return -EBUSY;
diff --git a/drivers/media/platform/vsp1/vsp1_drv.c b/drivers/media/platform/vsp1/vsp1_drv.c
index 4e61886384e3..533bc796391e 100644
--- a/drivers/media/platform/vsp1/vsp1_drv.c
+++ b/drivers/media/platform/vsp1/vsp1_drv.c
@@ -101,7 +101,7 @@ static int vsp1_create_links(struct vsp1_device *vsp1, struct vsp1_entity *sink)
 			if (!(entity->pads[pad].flags & MEDIA_PAD_FL_SINK))
 				continue;
 
-			ret = media_entity_create_link(&source->subdev.entity,
+			ret = media_create_pad_link(&source->subdev.entity,
 						       source->source_pad,
 						       entity, pad, flags);
 			if (ret < 0)
@@ -127,6 +127,7 @@ static void vsp1_destroy_entities(struct vsp1_device *vsp1)
 
 	v4l2_device_unregister(&vsp1->v4l2_dev);
 	media_device_unregister(&vsp1->media_dev);
+	media_device_cleanup(&vsp1->media_dev);
 }
 
 static int vsp1_create_entities(struct vsp1_device *vsp1)
@@ -141,12 +142,7 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 	strlcpy(mdev->model, "VSP1", sizeof(mdev->model));
 	snprintf(mdev->bus_info, sizeof(mdev->bus_info), "platform:%s",
 		 dev_name(mdev->dev));
-	ret = media_device_register(mdev);
-	if (ret < 0) {
-		dev_err(vsp1->dev, "media device registration failed (%d)\n",
-			ret);
-		return ret;
-	}
+	media_device_init(mdev);
 
 	vdev->mdev = mdev;
 	ret = v4l2_device_register(vsp1->dev, vdev);
@@ -250,34 +246,47 @@ static int vsp1_create_entities(struct vsp1_device *vsp1)
 		list_add_tail(&wpf->entity.list_dev, &vsp1->entities);
 	}
 
-	/* Create links. */
+	/* Register all subdevs. */
 	list_for_each_entry(entity, &vsp1->entities, list_dev) {
-		if (entity->type == VSP1_ENTITY_LIF ||
-		    entity->type == VSP1_ENTITY_RPF)
-			continue;
-
-		ret = vsp1_create_links(vsp1, entity);
+		ret = v4l2_device_register_subdev(&vsp1->v4l2_dev,
+						  &entity->subdev);
 		if (ret < 0)
 			goto done;
 	}
 
+	/* Create links. */
+	list_for_each_entry(entity, &vsp1->entities, list_dev) {
+		if (entity->type == VSP1_ENTITY_WPF) {
+			ret = vsp1_wpf_create_links(vsp1, entity);
+			if (ret < 0)
+				goto done;
+		} else if (entity->type == VSP1_ENTITY_RPF) {
+			ret = vsp1_rpf_create_links(vsp1, entity);
+			if (ret < 0)
+				goto done;
+		}
+
+		if (entity->type != VSP1_ENTITY_LIF &&
+		    entity->type != VSP1_ENTITY_RPF) {
+			ret = vsp1_create_links(vsp1, entity);
+			if (ret < 0)
+				goto done;
+		}
+	}
+
 	if (vsp1->pdata.features & VSP1_HAS_LIF) {
-		ret = media_entity_create_link(
+		ret = media_create_pad_link(
 			&vsp1->wpf[0]->entity.subdev.entity, RWPF_PAD_SOURCE,
 			&vsp1->lif->entity.subdev.entity, LIF_PAD_SINK, 0);
 		if (ret < 0)
 			return ret;
 	}
 
-	/* Register all subdevs. */
-	list_for_each_entry(entity, &vsp1->entities, list_dev) {
-		ret = v4l2_device_register_subdev(&vsp1->v4l2_dev,
-						  &entity->subdev);
-		if (ret < 0)
-			goto done;
-	}
-
 	ret = v4l2_device_register_subdev_nodes(&vsp1->v4l2_dev);
+	if (ret < 0)
+		goto done;
+
+	ret = media_device_register(mdev);
 
 done:
 	if (ret < 0)
diff --git a/drivers/media/platform/vsp1/vsp1_entity.c b/drivers/media/platform/vsp1/vsp1_entity.c
index fd95a75b04f4..d7308530952f 100644
--- a/drivers/media/platform/vsp1/vsp1_entity.c
+++ b/drivers/media/platform/vsp1/vsp1_entity.c
@@ -219,8 +219,8 @@ int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
 	entity->pads[num_pads - 1].flags = MEDIA_PAD_FL_SOURCE;
 
 	/* Initialize the media entity. */
-	return media_entity_init(&entity->subdev.entity, num_pads,
-				 entity->pads, 0);
+	return media_entity_pads_init(&entity->subdev.entity, num_pads,
+				 entity->pads);
 }
 
 void vsp1_entity_destroy(struct vsp1_entity *entity)
diff --git a/drivers/media/platform/vsp1/vsp1_rpf.c b/drivers/media/platform/vsp1/vsp1_rpf.c
index cd5248a9a271..924538223d3e 100644
--- a/drivers/media/platform/vsp1/vsp1_rpf.c
+++ b/drivers/media/platform/vsp1/vsp1_rpf.c
@@ -277,18 +277,29 @@ struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index)
 
 	rpf->entity.video = video;
 
-	/* Connect the video device to the RPF. */
-	ret = media_entity_create_link(&rpf->video.video.entity, 0,
-				       &rpf->entity.subdev.entity,
-				       RWPF_PAD_SINK,
-				       MEDIA_LNK_FL_ENABLED |
-				       MEDIA_LNK_FL_IMMUTABLE);
-	if (ret < 0)
-		goto error;
-
 	return rpf;
 
 error:
 	vsp1_entity_destroy(&rpf->entity);
 	return ERR_PTR(ret);
 }
+
+/*
+ * vsp1_rpf_create_links() - RPF pads links creation
+ * @vsp1: Pointer to VSP1 device
+ * @entity: Pointer to VSP1 entity
+ *
+ * return negative error code or zero on success
+ */
+int vsp1_rpf_create_links(struct vsp1_device *vsp1,
+			       struct vsp1_entity *entity)
+{
+	struct vsp1_rwpf *rpf = to_rwpf(&entity->subdev);
+
+	/* Connect the video device to the RPF. */
+	return media_create_pad_link(&rpf->video.video.entity, 0,
+				     &rpf->entity.subdev.entity,
+				     RWPF_PAD_SINK,
+				     MEDIA_LNK_FL_ENABLED |
+				     MEDIA_LNK_FL_IMMUTABLE);
+}
diff --git a/drivers/media/platform/vsp1/vsp1_rwpf.h b/drivers/media/platform/vsp1/vsp1_rwpf.h
index f452dce1a931..731d36e5258d 100644
--- a/drivers/media/platform/vsp1/vsp1_rwpf.h
+++ b/drivers/media/platform/vsp1/vsp1_rwpf.h
@@ -50,6 +50,11 @@ static inline struct vsp1_rwpf *to_rwpf(struct v4l2_subdev *subdev)
 struct vsp1_rwpf *vsp1_rpf_create(struct vsp1_device *vsp1, unsigned int index);
 struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index);
 
+int vsp1_rpf_create_links(struct vsp1_device *vsp1,
+			       struct vsp1_entity *entity);
+int vsp1_wpf_create_links(struct vsp1_device *vsp1,
+			       struct vsp1_entity *entity);
+
 int vsp1_rwpf_enum_mbus_code(struct v4l2_subdev *subdev,
 			     struct v4l2_subdev_pad_config *cfg,
 			     struct v4l2_subdev_mbus_code_enum *code);
diff --git a/drivers/media/platform/vsp1/vsp1_video.c b/drivers/media/platform/vsp1/vsp1_video.c
index 5ce88e1f5d71..b4dca57d1ae3 100644
--- a/drivers/media/platform/vsp1/vsp1_video.c
+++ b/drivers/media/platform/vsp1/vsp1_video.c
@@ -160,8 +160,7 @@ vsp1_video_remote_subdev(struct media_pad *local, u32 *pad)
 	struct media_pad *remote;
 
 	remote = media_entity_remote_pad(local);
-	if (remote == NULL ||
-	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 		return NULL;
 
 	if (pad)
@@ -274,35 +273,6 @@ static int __vsp1_video_try_format(struct vsp1_video *video,
 	return 0;
 }
 
-static bool
-vsp1_video_format_adjust(struct vsp1_video *video,
-			 const struct v4l2_pix_format_mplane *format,
-			 struct v4l2_pix_format_mplane *adjust)
-{
-	unsigned int i;
-
-	*adjust = *format;
-	__vsp1_video_try_format(video, adjust, NULL);
-
-	if (format->width != adjust->width ||
-	    format->height != adjust->height ||
-	    format->pixelformat != adjust->pixelformat ||
-	    format->num_planes != adjust->num_planes)
-		return false;
-
-	for (i = 0; i < format->num_planes; ++i) {
-		if (format->plane_fmt[i].bytesperline !=
-		    adjust->plane_fmt[i].bytesperline)
-			return false;
-
-		adjust->plane_fmt[i].sizeimage =
-			max(adjust->plane_fmt[i].sizeimage,
-			    format->plane_fmt[i].sizeimage);
-	}
-
-	return true;
-}
-
 /* -----------------------------------------------------------------------------
  * Pipeline Management
  */
@@ -312,24 +282,35 @@ static int vsp1_pipeline_validate_branch(struct vsp1_pipeline *pipe,
 					 struct vsp1_rwpf *output)
 {
 	struct vsp1_entity *entity;
-	unsigned int entities = 0;
+	struct media_entity_enum ent_enum;
 	struct media_pad *pad;
+	int rval;
 	bool bru_found = false;
 
 	input->location.left = 0;
 	input->location.top = 0;
 
+	rval = media_entity_enum_init(
+		&ent_enum, input->entity.pads[RWPF_PAD_SOURCE].graph_obj.mdev);
+	if (rval)
+		return rval;
+
 	pad = media_entity_remote_pad(&input->entity.pads[RWPF_PAD_SOURCE]);
 
 	while (1) {
-		if (pad == NULL)
-			return -EPIPE;
+		if (pad == NULL) {
+			rval = -EPIPE;
+			goto out;
+		}
 
 		/* We've reached a video node, that shouldn't have happened. */
-		if (media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
-			return -EPIPE;
+		if (!is_media_entity_v4l2_subdev(pad->entity)) {
+			rval = -EPIPE;
+			goto out;
+		}
 
-		entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
+		entity = to_vsp1_entity(
+			media_entity_to_v4l2_subdev(pad->entity));
 
 		/* A BRU is present in the pipeline, store the compose rectangle
 		 * location in the input RPF for use when configuring the RPF.
@@ -352,15 +333,18 @@ static int vsp1_pipeline_validate_branch(struct vsp1_pipeline *pipe,
 			break;
 
 		/* Ensure the branch has no loop. */
-		if (entities & (1 << entity->subdev.entity.id))
-			return -EPIPE;
-
-		entities |= 1 << entity->subdev.entity.id;
+		if (media_entity_enum_test_and_set(&ent_enum,
+						   &entity->subdev.entity)) {
+			rval = -EPIPE;
+			goto out;
+		}
 
 		/* UDS can't be chained. */
 		if (entity->type == VSP1_ENTITY_UDS) {
-			if (pipe->uds)
-				return -EPIPE;
+			if (pipe->uds) {
+				rval = -EPIPE;
+				goto out;
+			}
 
 			pipe->uds = entity;
 			pipe->uds_input = bru_found ? pipe->bru
@@ -378,9 +362,12 @@ static int vsp1_pipeline_validate_branch(struct vsp1_pipeline *pipe,
 
 	/* The last entity must be the output WPF. */
 	if (entity != &output->entity)
-		return -EPIPE;
+		rval = -EPIPE;
 
-	return 0;
+out:
+	media_entity_enum_cleanup(&ent_enum);
+
+	return rval;
 }
 
 static void __vsp1_pipeline_cleanup(struct vsp1_pipeline *pipe)
@@ -409,13 +396,19 @@ static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 	unsigned int i;
 	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
 
 	/* Walk the graph to locate the entities and video nodes. */
+	ret = media_entity_graph_walk_init(&graph, mdev);
+	if (ret) {
+		mutex_unlock(&mdev->graph_mutex);
+		return ret;
+	}
+
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
@@ -423,7 +416,7 @@ static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
 		struct vsp1_rwpf *rwpf;
 		struct vsp1_entity *e;
 
-		if (media_entity_type(entity) != MEDIA_ENT_T_V4L2_SUBDEV) {
+		if (is_media_entity_v4l2_io(entity)) {
 			pipe->num_video++;
 			continue;
 		}
@@ -449,6 +442,8 @@ static int vsp1_pipeline_validate(struct vsp1_pipeline *pipe,
 
 	mutex_unlock(&mdev->graph_mutex);
 
+	media_entity_graph_walk_cleanup(&graph);
+
 	/* We need one output and at least one input. */
 	if (pipe->num_inputs == 0 || !pipe->output) {
 		ret = -EPIPE;
@@ -520,7 +515,7 @@ static bool vsp1_pipeline_stopped(struct vsp1_pipeline *pipe)
 	bool stopped;
 
 	spin_lock_irqsave(&pipe->irqlock, flags);
-	stopped = pipe->state == VSP1_PIPELINE_STOPPED,
+	stopped = pipe->state == VSP1_PIPELINE_STOPPED;
 	spin_unlock_irqrestore(&pipe->irqlock, flags);
 
 	return stopped;
@@ -611,7 +606,7 @@ vsp1_video_complete_buffer(struct vsp1_video *video)
 	spin_unlock_irqrestore(&video->irqlock, flags);
 
 	done->buf.sequence = video->sequence++;
-	v4l2_get_timestamp(&done->buf.timestamp);
+	done->buf.vb2_buf.timestamp = ktime_get_ns();
 	for (i = 0; i < done->buf.vb2_buf.num_planes; ++i)
 		vb2_set_plane_payload(&done->buf.vb2_buf, i, done->length[i]);
 	vb2_buffer_done(&done->buf.vb2_buf, VB2_BUF_STATE_DONE);
@@ -692,7 +687,7 @@ void vsp1_pipeline_propagate_alpha(struct vsp1_pipeline *pipe,
 	pad = media_entity_remote_pad(&input->pads[RWPF_PAD_SOURCE]);
 
 	while (pad) {
-		if (media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = to_vsp1_entity(media_entity_to_v4l2_subdev(pad->entity));
@@ -787,26 +782,24 @@ void vsp1_pipelines_resume(struct vsp1_device *vsp1)
  */
 
 static int
-vsp1_video_queue_setup(struct vb2_queue *vq, const void *parg,
+vsp1_video_queue_setup(struct vb2_queue *vq,
 		     unsigned int *nbuffers, unsigned int *nplanes,
 		     unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct vsp1_video *video = vb2_get_drv_priv(vq);
-	const struct v4l2_pix_format_mplane *format;
-	struct v4l2_pix_format_mplane pix_mp;
+	const struct v4l2_pix_format_mplane *format = &video->format;
 	unsigned int i;
 
-	if (fmt) {
-		/* Make sure the format is valid and adjust the sizeimage field
-		 * if needed.
-		 */
-		if (!vsp1_video_format_adjust(video, &fmt->fmt.pix_mp, &pix_mp))
+	if (*nplanes) {
+		if (*nplanes != format->num_planes)
 			return -EINVAL;
 
-		format = &pix_mp;
-	} else {
-		format = &video->format;
+		for (i = 0; i < *nplanes; i++) {
+			if (sizes[i] < format->plane_fmt[i].sizeimage)
+				return -EINVAL;
+			alloc_ctxs[i] = video->alloc_ctx;
+		}
+		return 0;
 	}
 
 	*nplanes = format->num_planes;
@@ -1224,7 +1217,7 @@ int vsp1_video_init(struct vsp1_video *video, struct vsp1_entity *rwpf)
 	video->pipe.state = VSP1_PIPELINE_STOPPED;
 
 	/* Initialize the media entity... */
-	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	ret = media_entity_pads_init(&video->video.entity, 1, &video->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/media/platform/vsp1/vsp1_wpf.c b/drivers/media/platform/vsp1/vsp1_wpf.c
index 95b62f4f77e7..cbf514a6582d 100644
--- a/drivers/media/platform/vsp1/vsp1_wpf.c
+++ b/drivers/media/platform/vsp1/vsp1_wpf.c
@@ -220,7 +220,6 @@ struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
 	struct v4l2_subdev *subdev;
 	struct vsp1_video *video;
 	struct vsp1_rwpf *wpf;
-	unsigned int flags;
 	int ret;
 
 	wpf = devm_kzalloc(vsp1->dev, sizeof(*wpf), GFP_KERNEL);
@@ -276,20 +275,6 @@ struct vsp1_rwpf *vsp1_wpf_create(struct vsp1_device *vsp1, unsigned int index)
 		goto error;
 
 	wpf->entity.video = video;
-
-	/* Connect the video device to the WPF. All connections are immutable
-	 * except for the WPF0 source link if a LIF is present.
-	 */
-	flags = MEDIA_LNK_FL_ENABLED;
-	if (!(vsp1->pdata.features & VSP1_HAS_LIF) || index != 0)
-		flags |= MEDIA_LNK_FL_IMMUTABLE;
-
-	ret = media_entity_create_link(&wpf->entity.subdev.entity,
-				       RWPF_PAD_SOURCE,
-				       &wpf->video.video.entity, 0, flags);
-	if (ret < 0)
-		goto error;
-
 	wpf->entity.sink = &wpf->video.video.entity;
 
 	return wpf;
@@ -298,3 +283,28 @@ error:
 	vsp1_entity_destroy(&wpf->entity);
 	return ERR_PTR(ret);
 }
+
+/*
+ * vsp1_wpf_create_links() - RPF pads links creation
+ * @vsp1: Pointer to VSP1 device
+ * @entity: Pointer to VSP1 entity
+ *
+ * return negative error code or zero on success
+ */
+int vsp1_wpf_create_links(struct vsp1_device *vsp1,
+			       struct vsp1_entity *entity)
+{
+	struct vsp1_rwpf *wpf = to_rwpf(&entity->subdev);
+	unsigned int flags;
+
+	/* Connect the video device to the WPF. All connections are immutable
+	 * except for the WPF0 source link if a LIF is present.
+	 */
+	flags = MEDIA_LNK_FL_ENABLED;
+	if (!(vsp1->pdata.features & VSP1_HAS_LIF) || entity->index != 0)
+		flags |= MEDIA_LNK_FL_IMMUTABLE;
+
+	return media_create_pad_link(&wpf->entity.subdev.entity,
+				     RWPF_PAD_SOURCE,
+				     &wpf->video.video.entity, 0, flags);
+}
diff --git a/drivers/media/platform/xilinx/xilinx-dma.c b/drivers/media/platform/xilinx/xilinx-dma.c
index d11cc7072cd5..7f6898b13cac 100644
--- a/drivers/media/platform/xilinx/xilinx-dma.c
+++ b/drivers/media/platform/xilinx/xilinx-dma.c
@@ -49,8 +49,7 @@ xvip_dma_remote_subdev(struct media_pad *local, u32 *pad)
 	struct media_pad *remote;
 
 	remote = media_entity_remote_pad(local);
-	if (remote == NULL ||
-	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 		return NULL;
 
 	if (pad)
@@ -113,8 +112,7 @@ static int xvip_pipeline_start_stop(struct xvip_pipeline *pipe, bool start)
 			break;
 
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = pad->entity;
@@ -181,19 +179,26 @@ static int xvip_pipeline_validate(struct xvip_pipeline *pipe,
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &start->video.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 	unsigned int num_inputs = 0;
 	unsigned int num_outputs = 0;
+	int ret;
 
 	mutex_lock(&mdev->graph_mutex);
 
 	/* Walk the graph to locate the video nodes. */
+	ret = media_entity_graph_walk_init(&graph, entity->graph_obj.mdev);
+	if (ret) {
+		mutex_unlock(&mdev->graph_mutex);
+		return ret;
+	}
+
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
 		struct xvip_dma *dma;
 
-		if (entity->type != MEDIA_ENT_T_DEVNODE_V4L)
+		if (entity->function != MEDIA_ENT_F_IO_V4L)
 			continue;
 
 		dma = to_xvip_dma(media_entity_to_video_device(entity));
@@ -208,6 +213,8 @@ static int xvip_pipeline_validate(struct xvip_pipeline *pipe,
 
 	mutex_unlock(&mdev->graph_mutex);
 
+	media_entity_graph_walk_cleanup(&graph);
+
 	/* We need exactly one output and zero or one input. */
 	if (num_outputs != 1 || num_inputs > 1)
 		return -EPIPE;
@@ -303,27 +310,25 @@ static void xvip_dma_complete(void *param)
 
 	buf->buf.field = V4L2_FIELD_NONE;
 	buf->buf.sequence = dma->sequence++;
-	v4l2_get_timestamp(&buf->buf.timestamp);
+	buf->buf.vb2_buf.timestamp = ktime_get_ns();
 	vb2_set_plane_payload(&buf->buf.vb2_buf, 0, dma->format.sizeimage);
 	vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
 }
 
 static int
-xvip_dma_queue_setup(struct vb2_queue *vq, const void *parg,
+xvip_dma_queue_setup(struct vb2_queue *vq,
 		     unsigned int *nbuffers, unsigned int *nplanes,
 		     unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct xvip_dma *dma = vb2_get_drv_priv(vq);
 
+	alloc_ctxs[0] = dma->alloc_ctx;
 	/* Make sure the image size is large enough. */
-	if (fmt && fmt->fmt.pix.sizeimage < dma->format.sizeimage)
-		return -EINVAL;
+	if (*nplanes)
+		return sizes[0] < dma->format.sizeimage ? -EINVAL : 0;
 
 	*nplanes = 1;
-
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : dma->format.sizeimage;
-	alloc_ctxs[0] = dma->alloc_ctx;
+	sizes[0] = dma->format.sizeimage;
 
 	return 0;
 }
@@ -679,7 +684,7 @@ int xvip_dma_init(struct xvip_composite_device *xdev, struct xvip_dma *dma,
 	dma->pad.flags = type == V4L2_BUF_TYPE_VIDEO_CAPTURE
 		       ? MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE;
 
-	ret = media_entity_init(&dma->video.entity, 1, &dma->pad, 0);
+	ret = media_entity_pads_init(&dma->video.entity, 1, &dma->pad);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/media/platform/xilinx/xilinx-tpg.c b/drivers/media/platform/xilinx/xilinx-tpg.c
index b5f7d5ecb7f6..2ec1f6c4b274 100644
--- a/drivers/media/platform/xilinx/xilinx-tpg.c
+++ b/drivers/media/platform/xilinx/xilinx-tpg.c
@@ -731,6 +731,7 @@ static int xtpg_parse_of(struct xtpg_device *xtpg)
 		format = xvip_of_get_format(port);
 		if (IS_ERR(format)) {
 			dev_err(dev, "invalid format in DT");
+			of_node_put(port);
 			return PTR_ERR(format);
 		}
 
@@ -739,6 +740,7 @@ static int xtpg_parse_of(struct xtpg_device *xtpg)
 			xtpg->vip_format = format;
 		} else if (xtpg->vip_format != format) {
 			dev_err(dev, "in/out format mismatch in DT");
+			of_node_put(port);
 			return -EINVAL;
 		}
 
@@ -836,7 +838,7 @@ static int xtpg_probe(struct platform_device *pdev)
 	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	subdev->entity.ops = &xtpg_media_ops;
 
-	ret = media_entity_init(&subdev->entity, xtpg->npads, xtpg->pads, 0);
+	ret = media_entity_pads_init(&subdev->entity, xtpg->npads, xtpg->pads);
 	if (ret < 0)
 		goto error;
 
diff --git a/drivers/media/platform/xilinx/xilinx-vipp.c b/drivers/media/platform/xilinx/xilinx-vipp.c
index 7b7cb9c28d2c..e795a4501e8b 100644
--- a/drivers/media/platform/xilinx/xilinx-vipp.c
+++ b/drivers/media/platform/xilinx/xilinx-vipp.c
@@ -156,7 +156,7 @@ static int xvip_graph_build_one(struct xvip_composite_device *xdev,
 			local->name, local_pad->index,
 			remote->name, remote_pad->index);
 
-		ret = media_entity_create_link(local, local_pad->index,
+		ret = media_create_pad_link(local, local_pad->index,
 					       remote, remote_pad->index,
 					       link_flags);
 		if (ret < 0) {
@@ -270,7 +270,7 @@ static int xvip_graph_build_dma(struct xvip_composite_device *xdev)
 			source->name, source_pad->index,
 			sink->name, sink_pad->index);
 
-		ret = media_entity_create_link(source, source_pad->index,
+		ret = media_create_pad_link(source, source_pad->index,
 					       sink, sink_pad->index,
 					       link_flags);
 		if (ret < 0) {
@@ -311,7 +311,7 @@ static int xvip_graph_notify_complete(struct v4l2_async_notifier *notifier)
 	if (ret < 0)
 		dev_err(xdev->dev, "failed to register subdev nodes\n");
 
-	return ret;
+	return media_device_register(&xdev->media_dev);
 }
 
 static int xvip_graph_notify_bound(struct v4l2_async_notifier *notifier,
@@ -476,8 +476,10 @@ static int xvip_graph_dma_init(struct xvip_composite_device *xdev)
 
 	for_each_child_of_node(ports, port) {
 		ret = xvip_graph_dma_init_one(xdev, port);
-		if (ret < 0)
+		if (ret < 0) {
+			of_node_put(port);
 			return ret;
+		}
 	}
 
 	return 0;
@@ -571,6 +573,7 @@ static void xvip_composite_v4l2_cleanup(struct xvip_composite_device *xdev)
 {
 	v4l2_device_unregister(&xdev->v4l2_dev);
 	media_device_unregister(&xdev->media_dev);
+	media_device_cleanup(&xdev->media_dev);
 }
 
 static int xvip_composite_v4l2_init(struct xvip_composite_device *xdev)
@@ -582,19 +585,14 @@ static int xvip_composite_v4l2_init(struct xvip_composite_device *xdev)
 		sizeof(xdev->media_dev.model));
 	xdev->media_dev.hw_revision = 0;
 
-	ret = media_device_register(&xdev->media_dev);
-	if (ret < 0) {
-		dev_err(xdev->dev, "media device registration failed (%d)\n",
-			ret);
-		return ret;
-	}
+	media_device_init(&xdev->media_dev);
 
 	xdev->v4l2_dev.mdev = &xdev->media_dev;
 	ret = v4l2_device_register(xdev->dev, &xdev->v4l2_dev);
 	if (ret < 0) {
 		dev_err(xdev->dev, "V4L2 device registration failed (%d)\n",
 			ret);
-		media_device_unregister(&xdev->media_dev);
+		media_device_cleanup(&xdev->media_dev);
 		return ret;
 	}
 
diff --git a/drivers/media/radio/radio-maxiradio.c b/drivers/media/radio/radio-maxiradio.c
index 5236035f0f2a..70fd8e80198a 100644
--- a/drivers/media/radio/radio-maxiradio.c
+++ b/drivers/media/radio/radio-maxiradio.c
@@ -42,7 +42,7 @@
 #include <linux/videodev2.h>
 #include <linux/io.h>
 #include <linux/slab.h>
-#include <media/tea575x.h>
+#include <media/drv-intf/tea575x.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-fh.h>
@@ -108,7 +108,7 @@ static void maxiradio_tea575x_set_direction(struct snd_tea575x *tea, bool output
 {
 }
 
-static struct snd_tea575x_ops maxiradio_tea_ops = {
+static const struct snd_tea575x_ops maxiradio_tea_ops = {
 	.set_pins = maxiradio_tea575x_set_pins,
 	.get_pins = maxiradio_tea575x_get_pins,
 	.set_direction = maxiradio_tea575x_set_direction,
diff --git a/drivers/media/radio/radio-sf16fmr2.c b/drivers/media/radio/radio-sf16fmr2.c
index b8d61cbc18cb..dc81d422b394 100644
--- a/drivers/media/radio/radio-sf16fmr2.c
+++ b/drivers/media/radio/radio-sf16fmr2.c
@@ -14,7 +14,7 @@
 #include <linux/io.h>		/* outb, outb_p			*/
 #include <linux/isa.h>
 #include <linux/pnp.h>
-#include <media/tea575x.h>
+#include <media/drv-intf/tea575x.h>
 
 MODULE_AUTHOR("Ondrej Zary");
 MODULE_DESCRIPTION("MediaForte SF16-FMR2 and SF16-FMD2 FM radio card driver");
@@ -82,7 +82,7 @@ static void fmr2_tea575x_set_direction(struct snd_tea575x *tea, bool output)
 {
 }
 
-static struct snd_tea575x_ops fmr2_tea_ops = {
+static const struct snd_tea575x_ops fmr2_tea_ops = {
 	.set_pins = fmr2_tea575x_set_pins,
 	.get_pins = fmr2_tea575x_get_pins,
 	.set_direction = fmr2_tea575x_set_direction,
diff --git a/drivers/media/radio/radio-shark.c b/drivers/media/radio/radio-shark.c
index 050b3bb96fec..85667a95f003 100644
--- a/drivers/media/radio/radio-shark.c
+++ b/drivers/media/radio/radio-shark.c
@@ -33,7 +33,7 @@
 #include <linux/usb.h>
 #include <linux/workqueue.h>
 #include <media/v4l2-device.h>
-#include <media/tea575x.h>
+#include <media/drv-intf/tea575x.h>
 
 #if defined(CONFIG_LEDS_CLASS) || \
     (defined(CONFIG_LEDS_CLASS_MODULE) && defined(CONFIG_RADIO_SHARK_MODULE))
@@ -150,7 +150,7 @@ static u32 shark_read_val(struct snd_tea575x *tea)
 	return val;
 }
 
-static struct snd_tea575x_ops shark_tea_ops = {
+static const struct snd_tea575x_ops shark_tea_ops = {
 	.write_val = shark_write_val,
 	.read_val  = shark_read_val,
 };
diff --git a/drivers/media/radio/radio-shark2.c b/drivers/media/radio/radio-shark2.c
index 8654e0dc5c95..0e65a85d52c6 100644
--- a/drivers/media/radio/radio-shark2.c
+++ b/drivers/media/radio/radio-shark2.c
@@ -137,7 +137,7 @@ static int shark_read_reg(struct radio_tea5777 *tea, u32 *reg_ret)
 	return 0;
 }
 
-static struct radio_tea5777_ops shark_tea_ops = {
+static const struct radio_tea5777_ops shark_tea_ops = {
 	.write_reg = shark_write_reg,
 	.read_reg  = shark_read_reg,
 };
diff --git a/drivers/media/radio/radio-si476x.c b/drivers/media/radio/radio-si476x.c
index 9cbb8cdf0ac0..859f0c08ee05 100644
--- a/drivers/media/radio/radio-si476x.c
+++ b/drivers/media/radio/radio-si476x.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-device.h>
 
-#include <media/si476x.h>
+#include <media/drv-intf/si476x.h>
 #include <linux/mfd/si476x-core.h>
 
 #define FM_FREQ_RANGE_LOW   64000000
diff --git a/drivers/media/radio/radio-tea5777.h b/drivers/media/radio/radio-tea5777.h
index 4ea43a90a151..4bd942526a1b 100644
--- a/drivers/media/radio/radio-tea5777.h
+++ b/drivers/media/radio/radio-tea5777.h
@@ -76,7 +76,7 @@ struct radio_tea5777 {
 	u32 read_reg;
 	u64 write_reg;
 	struct mutex mutex;
-	struct radio_tea5777_ops *ops;
+	const struct radio_tea5777_ops *ops;
 	void *private_data;
 	u8 card[32];
 	u8 bus_info[32];
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index 04baafe5e901..a82eb9678d6c 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -26,7 +26,7 @@
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/module.h>
-#include <media/timb_radio.h>
+#include <linux/platform_data/media/timb_radio.h>
 
 #define DRIVER_NAME "timb-radio"
 
diff --git a/drivers/media/radio/si4713/radio-usb-si4713.c b/drivers/media/radio/si4713/radio-usb-si4713.c
index a77319dcba05..5146be2a1a50 100644
--- a/drivers/media/radio/si4713/radio-usb-si4713.c
+++ b/drivers/media/radio/si4713/radio-usb-si4713.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/si4713.h>
+#include <linux/platform_data/media/si4713.h>
 
 #include "si4713.h"
 
diff --git a/drivers/media/radio/si4713/si4713.h b/drivers/media/radio/si4713/si4713.h
index 8a376e142188..29d0e1f104d2 100644
--- a/drivers/media/radio/si4713/si4713.h
+++ b/drivers/media/radio/si4713/si4713.h
@@ -20,7 +20,7 @@
 #include <linux/gpio/consumer.h>
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-ctrls.h>
-#include <media/si4713.h>
+#include <linux/platform_data/media/si4713.h>
 
 #define SI4713_PRODUCT_NUMBER		0x0D
 
diff --git a/drivers/media/radio/tea575x.c b/drivers/media/radio/tea575x.c
index 43d1ea53cb66..3e08475af579 100644
--- a/drivers/media/radio/tea575x.c
+++ b/drivers/media/radio/tea575x.c
@@ -31,7 +31,7 @@
 #include <media/v4l2-fh.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/tea575x.h>
+#include <media/drv-intf/tea575x.h>
 
 MODULE_AUTHOR("Jaroslav Kysela <perex@perex.cz>");
 MODULE_DESCRIPTION("Routines for control of TEA5757/5759 Philips AM/FM radio tuner chips");
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index b6e13116c6f5..bd4d68500085 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -101,7 +101,8 @@ config IR_SHARP_DECODER
 
 	---help---
 	   Enable this option if you have an infrared remote control which
-	   uses the Sharp protocol, and you need software decoding support.
+	   uses the Sharp protocol (Sharp, Denon), and you need software
+	   decoding support.
 
 config IR_MCE_KBD_DECODER
 	tristate "Enable IR raw decoder for the MCE keyboard/mouse protocol"
diff --git a/drivers/media/rc/gpio-ir-recv.c b/drivers/media/rc/gpio-ir-recv.c
index 7dbc9ca6d885..5b63b1f15cb1 100644
--- a/drivers/media/rc/gpio-ir-recv.c
+++ b/drivers/media/rc/gpio-ir-recv.c
@@ -21,7 +21,7 @@
 #include <linux/platform_device.h>
 #include <linux/irq.h>
 #include <media/rc-core.h>
-#include <media/gpio-ir-recv.h>
+#include <linux/platform_data/media/gpio-ir-recv.h>
 
 #define GPIO_IR_DRIVER_NAME	"gpio-rc-recv"
 #define GPIO_IR_DEVICE_NAME	"gpio_ir_recv"
@@ -30,6 +30,7 @@ struct gpio_rc_dev {
 	struct rc_dev *rcdev;
 	int gpio_nr;
 	bool active_low;
+	struct timer_list flush_timer;
 };
 
 #ifdef CONFIG_OF
@@ -93,12 +94,26 @@ static irqreturn_t gpio_ir_recv_irq(int irq, void *dev_id)
 	if (rc < 0)
 		goto err_get_value;
 
+	mod_timer(&gpio_dev->flush_timer,
+		  jiffies + nsecs_to_jiffies(gpio_dev->rcdev->timeout));
+
 	ir_raw_event_handle(gpio_dev->rcdev);
 
 err_get_value:
 	return IRQ_HANDLED;
 }
 
+static void flush_timer(unsigned long arg)
+{
+	struct gpio_rc_dev *gpio_dev = (struct gpio_rc_dev *)arg;
+	DEFINE_IR_RAW_EVENT(ev);
+
+	ev.timeout = true;
+	ev.duration = gpio_dev->rcdev->timeout;
+	ir_raw_event_store(gpio_dev->rcdev, &ev);
+	ir_raw_event_handle(gpio_dev->rcdev);
+}
+
 static int gpio_ir_recv_probe(struct platform_device *pdev)
 {
 	struct gpio_rc_dev *gpio_dev;
@@ -144,6 +159,9 @@ static int gpio_ir_recv_probe(struct platform_device *pdev)
 	rcdev->input_id.version = 0x0100;
 	rcdev->dev.parent = &pdev->dev;
 	rcdev->driver_name = GPIO_IR_DRIVER_NAME;
+	rcdev->min_timeout = 0;
+	rcdev->timeout = IR_DEFAULT_TIMEOUT;
+	rcdev->max_timeout = 10 * IR_DEFAULT_TIMEOUT;
 	if (pdata->allowed_protos)
 		rcdev->allowed_protocols = pdata->allowed_protos;
 	else
@@ -154,6 +172,9 @@ static int gpio_ir_recv_probe(struct platform_device *pdev)
 	gpio_dev->gpio_nr = pdata->gpio_nr;
 	gpio_dev->active_low = pdata->active_low;
 
+	setup_timer(&gpio_dev->flush_timer, flush_timer,
+		    (unsigned long)gpio_dev);
+
 	rc = gpio_request(pdata->gpio_nr, "gpio-ir-recv");
 	if (rc < 0)
 		goto err_gpio_request;
@@ -196,6 +217,7 @@ static int gpio_ir_recv_remove(struct platform_device *pdev)
 	struct gpio_rc_dev *gpio_dev = platform_get_drvdata(pdev);
 
 	free_irq(gpio_to_irq(gpio_dev->gpio_nr), gpio_dev);
+	del_timer_sync(&gpio_dev->flush_timer);
 	rc_unregister_device(gpio_dev->rcdev);
 	gpio_free(gpio_dev->gpio_nr);
 	kfree(gpio_dev);
diff --git a/drivers/media/rc/ir-jvc-decoder.c b/drivers/media/rc/ir-jvc-decoder.c
index 30bcf188d377..182402f7b4d1 100644
--- a/drivers/media/rc/ir-jvc-decoder.c
+++ b/drivers/media/rc/ir-jvc-decoder.c
@@ -47,9 +47,6 @@ static int ir_jvc_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct jvc_dec *data = &dev->raw->jvc;
 
-	if (!(dev->enabled_protocols & RC_BIT_JVC))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-lirc-codec.c b/drivers/media/rc/ir-lirc-codec.c
index a32659fcd266..5effc65d2947 100644
--- a/drivers/media/rc/ir-lirc-codec.c
+++ b/drivers/media/rc/ir-lirc-codec.c
@@ -415,6 +415,7 @@ static int ir_lirc_unregister(struct rc_dev *dev)
 
 	lirc_unregister_driver(lirc->drv->minor);
 	lirc_buffer_free(lirc->drv->rbuf);
+	kfree(lirc->drv->rbuf);
 	kfree(lirc->drv);
 
 	return 0;
diff --git a/drivers/media/rc/ir-mce_kbd-decoder.c b/drivers/media/rc/ir-mce_kbd-decoder.c
index 9f3c9b59f30c..d80986251ee0 100644
--- a/drivers/media/rc/ir-mce_kbd-decoder.c
+++ b/drivers/media/rc/ir-mce_kbd-decoder.c
@@ -216,9 +216,6 @@ static int ir_mce_kbd_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	unsigned long delay;
 
-	if (!(dev->enabled_protocols & RC_BIT_MCE_KBD))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-nec-decoder.c b/drivers/media/rc/ir-nec-decoder.c
index 7b81fec0820f..bea0d1eedee0 100644
--- a/drivers/media/rc/ir-nec-decoder.c
+++ b/drivers/media/rc/ir-nec-decoder.c
@@ -52,9 +52,6 @@ static int ir_nec_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 address, not_address, command, not_command;
 	bool send_32bits = false;
 
-	if (!(dev->enabled_protocols & RC_BIT_NEC))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-rc5-decoder.c b/drivers/media/rc/ir-rc5-decoder.c
index 84fa6e9b59a1..6ffe776abf6b 100644
--- a/drivers/media/rc/ir-rc5-decoder.c
+++ b/drivers/media/rc/ir-rc5-decoder.c
@@ -53,9 +53,6 @@ static int ir_rc5_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	enum rc_type protocol;
 
-	if (!(dev->enabled_protocols & (RC_BIT_RC5 | RC_BIT_RC5X | RC_BIT_RC5_SZ)))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-rc6-decoder.c b/drivers/media/rc/ir-rc6-decoder.c
index d16bc67af732..e0e2edefa651 100644
--- a/drivers/media/rc/ir-rc6-decoder.c
+++ b/drivers/media/rc/ir-rc6-decoder.c
@@ -90,11 +90,6 @@ static int ir_rc6_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u8 toggle;
 	enum rc_type protocol;
 
-	if (!(dev->enabled_protocols &
-	      (RC_BIT_RC6_0 | RC_BIT_RC6_6A_20 | RC_BIT_RC6_6A_24 |
-	       RC_BIT_RC6_6A_32 | RC_BIT_RC6_MCE)))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-rx51.c b/drivers/media/rc/ir-rx51.c
index b1e19a26208d..4e1711a40466 100644
--- a/drivers/media/rc/ir-rx51.c
+++ b/drivers/media/rc/ir-rx51.c
@@ -31,7 +31,7 @@
 
 #include <media/lirc.h>
 #include <media/lirc_dev.h>
-#include <media/ir-rx51.h>
+#include <linux/platform_data/media/ir-rx51.h>
 
 #define LIRC_RX51_DRIVER_FEATURES (LIRC_CAN_SET_SEND_DUTY_CYCLE |	\
 				   LIRC_CAN_SET_SEND_CARRIER |		\
diff --git a/drivers/media/rc/ir-sanyo-decoder.c b/drivers/media/rc/ir-sanyo-decoder.c
index ad1dc6ae21fc..7331e5e7c497 100644
--- a/drivers/media/rc/ir-sanyo-decoder.c
+++ b/drivers/media/rc/ir-sanyo-decoder.c
@@ -58,9 +58,6 @@ static int ir_sanyo_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 address, command, not_command;
 
-	if (!(dev->enabled_protocols & RC_BIT_SANYO))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset) {
 			IR_dprintk(1, "SANYO event reset received. reset to state 0\n");
diff --git a/drivers/media/rc/ir-sharp-decoder.c b/drivers/media/rc/ir-sharp-decoder.c
index b7acdbae8159..317677f06f2c 100644
--- a/drivers/media/rc/ir-sharp-decoder.c
+++ b/drivers/media/rc/ir-sharp-decoder.c
@@ -48,9 +48,6 @@ static int ir_sharp_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	struct sharp_dec *data = &dev->raw->sharp;
 	u32 msg, echo, address, command, scancode;
 
-	if (!(dev->enabled_protocols & RC_BIT_SHARP))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
@@ -118,7 +115,9 @@ static int ir_sharp_decode(struct rc_dev *dev, struct ir_raw_event ev)
 
 		if (data->count == SHARP_NBITS) {
 			/* exp,chk bits should be 1,0 */
-			if ((data->bits & 0x3) != 0x2)
+			if ((data->bits & 0x3) != 0x2 &&
+			/* DENON variant, both chk bits 0 */
+			    (data->bits & 0x3) != 0x0)
 				break;
 			data->state = STATE_ECHO_SPACE;
 		} else {
diff --git a/drivers/media/rc/ir-sony-decoder.c b/drivers/media/rc/ir-sony-decoder.c
index 58ef06f35175..baa972c76e0e 100644
--- a/drivers/media/rc/ir-sony-decoder.c
+++ b/drivers/media/rc/ir-sony-decoder.c
@@ -46,10 +46,6 @@ static int ir_sony_decode(struct rc_dev *dev, struct ir_raw_event ev)
 	u32 scancode;
 	u8 device, subdevice, function;
 
-	if (!(dev->enabled_protocols &
-	      (RC_BIT_SONY12 | RC_BIT_SONY15 | RC_BIT_SONY20)))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/ir-xmp-decoder.c b/drivers/media/rc/ir-xmp-decoder.c
index 1017d4816e8d..18596190bbb8 100644
--- a/drivers/media/rc/ir-xmp-decoder.c
+++ b/drivers/media/rc/ir-xmp-decoder.c
@@ -43,9 +43,6 @@ static int ir_xmp_decode(struct rc_dev *dev, struct ir_raw_event ev)
 {
 	struct xmp_dec *data = &dev->raw->xmp;
 
-	if (!(dev->enabled_protocols & RC_BIT_XMP))
-		return 0;
-
 	if (!is_timing_event(ev)) {
 		if (ev.reset)
 			data->state = STATE_INACTIVE;
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 85af7a869167..18adf580f502 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -39,6 +39,18 @@
 
 #include "nuvoton-cir.h"
 
+static const struct nvt_chip nvt_chips[] = {
+	{ "w83667hg", NVT_W83667HG },
+	{ "NCT6775F", NVT_6775F },
+	{ "NCT6776F", NVT_6776F },
+	{ "NCT6779D", NVT_6779D },
+};
+
+static inline bool is_w83667hg(struct nvt_dev *nvt)
+{
+	return nvt->chip_ver == NVT_W83667HG;
+}
+
 /* write val to config reg */
 static inline void nvt_cr_write(struct nvt_dev *nvt, u8 val, u8 reg)
 {
@@ -224,74 +236,60 @@ static void cir_wake_dump_regs(struct nvt_dev *nvt)
 	pr_cont("\n");
 }
 
+static inline const char *nvt_find_chip(struct nvt_dev *nvt, int id)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(nvt_chips); i++)
+		if ((id & SIO_ID_MASK) == nvt_chips[i].chip_ver) {
+			nvt->chip_ver = nvt_chips[i].chip_ver;
+			return nvt_chips[i].name;
+		}
+
+	return NULL;
+}
+
+
 /* detect hardware features */
-static int nvt_hw_detect(struct nvt_dev *nvt)
+static void nvt_hw_detect(struct nvt_dev *nvt)
 {
-	unsigned long flags;
-	u8 chip_major, chip_minor;
-	char chip_id[12];
-	bool chip_unknown = false;
+	const char *chip_name;
+	int chip_id;
 
 	nvt_efm_enable(nvt);
 
 	/* Check if we're wired for the alternate EFER setup */
-	chip_major = nvt_cr_read(nvt, CR_CHIP_ID_HI);
-	if (chip_major == 0xff) {
+	nvt->chip_major = nvt_cr_read(nvt, CR_CHIP_ID_HI);
+	if (nvt->chip_major == 0xff) {
 		nvt->cr_efir = CR_EFIR2;
 		nvt->cr_efdr = CR_EFDR2;
 		nvt_efm_enable(nvt);
-		chip_major = nvt_cr_read(nvt, CR_CHIP_ID_HI);
+		nvt->chip_major = nvt_cr_read(nvt, CR_CHIP_ID_HI);
 	}
 
-	chip_minor = nvt_cr_read(nvt, CR_CHIP_ID_LO);
-
-	/* these are the known working chip revisions... */
-	switch (chip_major) {
-	case CHIP_ID_HIGH_667:
-		strcpy(chip_id, "w83667hg\0");
-		if (chip_minor != CHIP_ID_LOW_667)
-			chip_unknown = true;
-		break;
-	case CHIP_ID_HIGH_677B:
-		strcpy(chip_id, "w83677hg\0");
-		if (chip_minor != CHIP_ID_LOW_677B2 &&
-		    chip_minor != CHIP_ID_LOW_677B3)
-			chip_unknown = true;
-		break;
-	case CHIP_ID_HIGH_677C:
-		strcpy(chip_id, "w83677hg-c\0");
-		if (chip_minor != CHIP_ID_LOW_677C)
-			chip_unknown = true;
-		break;
-	default:
-		strcpy(chip_id, "w836x7hg\0");
-		chip_unknown = true;
-		break;
-	}
+	nvt->chip_minor = nvt_cr_read(nvt, CR_CHIP_ID_LO);
+
+	chip_id = nvt->chip_major << 8 | nvt->chip_minor;
+	chip_name = nvt_find_chip(nvt, chip_id);
 
 	/* warn, but still let the driver load, if we don't know this chip */
-	if (chip_unknown)
-		nvt_pr(KERN_WARNING, "%s: unknown chip, id: 0x%02x 0x%02x, "
-		       "it may not work...", chip_id, chip_major, chip_minor);
+	if (!chip_name)
+		dev_warn(&nvt->pdev->dev,
+			 "unknown chip, id: 0x%02x 0x%02x, it may not work...",
+			 nvt->chip_major, nvt->chip_minor);
 	else
-		nvt_dbg("%s: chip id: 0x%02x 0x%02x",
-			chip_id, chip_major, chip_minor);
+		dev_info(&nvt->pdev->dev,
+			 "found %s or compatible: chip id: 0x%02x 0x%02x",
+			 chip_name, nvt->chip_major, nvt->chip_minor);
 
 	nvt_efm_disable(nvt);
-
-	spin_lock_irqsave(&nvt->nvt_lock, flags);
-	nvt->chip_major = chip_major;
-	nvt->chip_minor = chip_minor;
-	spin_unlock_irqrestore(&nvt->nvt_lock, flags);
-
-	return 0;
 }
 
 static void nvt_cir_ldev_init(struct nvt_dev *nvt)
 {
 	u8 val, psreg, psmask, psval;
 
-	if (nvt->chip_major == CHIP_ID_HIGH_667) {
+	if (is_w83667hg(nvt)) {
 		psreg = CR_MULTIFUNC_PIN_SEL;
 		psmask = MULTIFUNC_PIN_SEL_MASK;
 		psval = MULTIFUNC_ENABLE_CIR | MULTIFUNC_ENABLE_CIRWB;
@@ -485,8 +483,9 @@ static u32 nvt_rx_carrier_detect(struct nvt_dev *nvt)
 	duration *= SAMPLE_PERIOD;
 
 	if (!count || !duration) {
-		nvt_pr(KERN_NOTICE, "Unable to determine carrier! (c:%u, d:%u)",
-		       count, duration);
+		dev_notice(&nvt->pdev->dev,
+			   "Unable to determine carrier! (c:%u, d:%u)",
+			   count, duration);
 		return 0;
 	}
 
@@ -661,7 +660,7 @@ static void nvt_process_rx_ir_data(struct nvt_dev *nvt)
 
 static void nvt_handle_rx_fifo_overrun(struct nvt_dev *nvt)
 {
-	nvt_pr(KERN_WARNING, "RX FIFO overrun detected, flushing data!");
+	dev_warn(&nvt->pdev->dev, "RX FIFO overrun detected, flushing data!");
 
 	nvt->pkts = 0;
 	nvt_clear_cir_fifo(nvt);
@@ -719,7 +718,7 @@ static void nvt_get_rx_ir_data(struct nvt_dev *nvt)
 
 static void nvt_cir_log_irqs(u8 status, u8 iren)
 {
-	nvt_pr(KERN_INFO, "IRQ 0x%02x (IREN 0x%02x) :%s%s%s%s%s%s%s%s%s",
+	nvt_dbg("IRQ 0x%02x (IREN 0x%02x) :%s%s%s%s%s%s%s%s%s",
 		status, iren,
 		status & CIR_IRSTS_RDR	? " RDR"	: "",
 		status & CIR_IRSTS_RTR	? " RTR"	: "",
@@ -779,7 +778,7 @@ static irqreturn_t nvt_cir_isr(int irq, void *data)
 	if (!status) {
 		nvt_dbg_verbose("%s exiting, IRSTS 0x0", __func__);
 		nvt_cir_reg_write(nvt, 0xff, CIR_IRSTS);
-		return IRQ_RETVAL(IRQ_NONE);
+		return IRQ_NONE;
 	}
 
 	/* ack/clear all irq flags we've got */
@@ -790,11 +789,10 @@ static irqreturn_t nvt_cir_isr(int irq, void *data)
 	iren = nvt_cir_reg_read(nvt, CIR_IREN);
 	if (!iren) {
 		nvt_dbg_verbose("%s exiting, CIR not enabled", __func__);
-		return IRQ_RETVAL(IRQ_NONE);
+		return IRQ_NONE;
 	}
 
-	if (debug)
-		nvt_cir_log_irqs(status, iren);
+	nvt_cir_log_irqs(status, iren);
 
 	if (status & CIR_IRSTS_RTR) {
 		/* FIXME: add code for study/learn mode */
@@ -853,7 +851,7 @@ static irqreturn_t nvt_cir_isr(int irq, void *data)
 	}
 
 	nvt_dbg_verbose("%s done", __func__);
-	return IRQ_RETVAL(IRQ_HANDLED);
+	return IRQ_HANDLED;
 }
 
 /* Interrupt service routine for CIR Wake */
@@ -867,7 +865,7 @@ static irqreturn_t nvt_cir_wake_isr(int irq, void *data)
 
 	status = nvt_cir_wake_reg_read(nvt, CIR_WAKE_IRSTS);
 	if (!status)
-		return IRQ_RETVAL(IRQ_NONE);
+		return IRQ_NONE;
 
 	if (status & CIR_WAKE_IRSTS_IR_PENDING)
 		nvt_clear_cir_wake_fifo(nvt);
@@ -879,7 +877,7 @@ static irqreturn_t nvt_cir_wake_isr(int irq, void *data)
 	iren = nvt_cir_wake_reg_read(nvt, CIR_WAKE_IREN);
 	if (!iren) {
 		nvt_dbg_wake("%s exiting, wake not enabled", __func__);
-		return IRQ_RETVAL(IRQ_HANDLED);
+		return IRQ_HANDLED;
 	}
 
 	if ((status & CIR_WAKE_IRSTS_PE) &&
@@ -896,7 +894,7 @@ static irqreturn_t nvt_cir_wake_isr(int irq, void *data)
 	}
 
 	nvt_dbg_wake("%s done", __func__);
-	return IRQ_RETVAL(IRQ_HANDLED);
+	return IRQ_HANDLED;
 }
 
 static void nvt_enable_cir(struct nvt_dev *nvt)
@@ -974,7 +972,7 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 	struct rc_dev *rdev;
 	int ret = -ENOMEM;
 
-	nvt = kzalloc(sizeof(struct nvt_dev), GFP_KERNEL);
+	nvt = devm_kzalloc(&pdev->dev, sizeof(struct nvt_dev), GFP_KERNEL);
 	if (!nvt)
 		return ret;
 
@@ -1026,9 +1024,7 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	init_waitqueue_head(&nvt->tx.queue);
 
-	ret = nvt_hw_detect(nvt);
-	if (ret)
-		goto exit_free_dev_rdev;
+	nvt_hw_detect(nvt);
 
 	/* Initialize CIR & CIR Wake Logical Devices */
 	nvt_efm_enable(nvt);
@@ -1074,25 +1070,26 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	ret = -EBUSY;
 	/* now claim resources */
-	if (!request_region(nvt->cir_addr,
+	if (!devm_request_region(&pdev->dev, nvt->cir_addr,
 			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
 		goto exit_unregister_device;
 
-	if (request_irq(nvt->cir_irq, nvt_cir_isr, IRQF_SHARED,
-			NVT_DRIVER_NAME, (void *)nvt))
-		goto exit_release_cir_addr;
+	if (devm_request_irq(&pdev->dev, nvt->cir_irq, nvt_cir_isr,
+			     IRQF_SHARED, NVT_DRIVER_NAME, (void *)nvt))
+		goto exit_unregister_device;
 
-	if (!request_region(nvt->cir_wake_addr,
+	if (!devm_request_region(&pdev->dev, nvt->cir_wake_addr,
 			    CIR_IOREG_LENGTH, NVT_DRIVER_NAME))
-		goto exit_free_irq;
+		goto exit_unregister_device;
 
-	if (request_irq(nvt->cir_wake_irq, nvt_cir_wake_isr, IRQF_SHARED,
-			NVT_DRIVER_NAME, (void *)nvt))
-		goto exit_release_cir_wake_addr;
+	if (devm_request_irq(&pdev->dev, nvt->cir_wake_irq,
+			     nvt_cir_wake_isr, IRQF_SHARED,
+			     NVT_DRIVER_NAME, (void *)nvt))
+		goto exit_unregister_device;
 
 	device_init_wakeup(&pdev->dev, true);
 
-	nvt_pr(KERN_NOTICE, "driver has been successfully loaded\n");
+	dev_notice(&pdev->dev, "driver has been successfully loaded\n");
 	if (debug) {
 		cir_dump_regs(nvt);
 		cir_wake_dump_regs(nvt);
@@ -1100,18 +1097,11 @@ static int nvt_probe(struct pnp_dev *pdev, const struct pnp_device_id *dev_id)
 
 	return 0;
 
-exit_release_cir_wake_addr:
-	release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
-exit_free_irq:
-	free_irq(nvt->cir_irq, nvt);
-exit_release_cir_addr:
-	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
 exit_unregister_device:
 	rc_unregister_device(rdev);
 	rdev = NULL;
 exit_free_dev_rdev:
 	rc_free_device(rdev);
-	kfree(nvt);
 
 	return ret;
 }
@@ -1129,15 +1119,7 @@ static void nvt_remove(struct pnp_dev *pdev)
 	nvt_enable_wake(nvt);
 	spin_unlock_irqrestore(&nvt->nvt_lock, flags);
 
-	/* free resources */
-	free_irq(nvt->cir_irq, nvt);
-	free_irq(nvt->cir_wake_irq, nvt);
-	release_region(nvt->cir_addr, CIR_IOREG_LENGTH);
-	release_region(nvt->cir_wake_addr, CIR_IOREG_LENGTH);
-
 	rc_unregister_device(nvt->rdev);
-
-	kfree(nvt);
 }
 
 static int nvt_suspend(struct pnp_dev *pdev, pm_message_t state)
diff --git a/drivers/media/rc/nuvoton-cir.h b/drivers/media/rc/nuvoton-cir.h
index e1cf23c3875b..0ad15d34e9c9 100644
--- a/drivers/media/rc/nuvoton-cir.h
+++ b/drivers/media/rc/nuvoton-cir.h
@@ -35,9 +35,6 @@
 static int debug;
 
 
-#define nvt_pr(level, text, ...) \
-	printk(level KBUILD_MODNAME ": " text, ## __VA_ARGS__)
-
 #define nvt_dbg(text, ...) \
 	if (debug) \
 		printk(KERN_DEBUG \
@@ -64,6 +61,21 @@ static int debug;
 #define TX_BUF_LEN 256
 #define RX_BUF_LEN 32
 
+#define SIO_ID_MASK 0xfff0
+
+enum nvt_chip_ver {
+	NVT_UNKNOWN	= 0,
+	NVT_W83667HG	= 0xa510,
+	NVT_6775F	= 0xb470,
+	NVT_6776F	= 0xc330,
+	NVT_6779D	= 0xc560
+};
+
+struct nvt_chip {
+	const char *name;
+	enum nvt_chip_ver chip_ver;
+};
+
 struct nvt_dev {
 	struct pnp_dev *pdev;
 	struct rc_dev *rdev;
@@ -93,6 +105,7 @@ struct nvt_dev {
 	int cir_irq;
 	int cir_wake_irq;
 
+	enum nvt_chip_ver chip_ver;
 	/* hardware id */
 	u8 chip_major;
 	u8 chip_minor;
@@ -326,15 +339,6 @@ struct nvt_dev {
 #define EFER_EFM_ENABLE		0x87
 #define EFER_EFM_DISABLE	0xaa
 
-/* Chip IDs found in CR_CHIP_ID_{HI,LO} */
-#define CHIP_ID_HIGH_667	0xa5
-#define CHIP_ID_HIGH_677B	0xb4
-#define CHIP_ID_HIGH_677C	0xc3
-#define CHIP_ID_LOW_667		0x13
-#define CHIP_ID_LOW_677B2	0x72
-#define CHIP_ID_LOW_677B3	0x73
-#define CHIP_ID_LOW_677C	0x33
-
 /* Config regs we need to care about */
 #define CR_SOFTWARE_RESET	0x02
 #define CR_LOGICAL_DEV_SEL	0x07
diff --git a/drivers/media/rc/rc-core-priv.h b/drivers/media/rc/rc-core-priv.h
index b68d4f762734..7359f3d03b64 100644
--- a/drivers/media/rc/rc-core-priv.h
+++ b/drivers/media/rc/rc-core-priv.h
@@ -167,75 +167,4 @@ void ir_raw_init(void);
  * loads the compiled decoders for their usage with IR raw events
  */
 
-/* from ir-nec-decoder.c */
-#ifdef CONFIG_IR_NEC_DECODER_MODULE
-#define load_nec_decode()	request_module_nowait("ir-nec-decoder")
-#else
-static inline void load_nec_decode(void) { }
-#endif
-
-/* from ir-rc5-decoder.c */
-#ifdef CONFIG_IR_RC5_DECODER_MODULE
-#define load_rc5_decode()	request_module_nowait("ir-rc5-decoder")
-#else
-static inline void load_rc5_decode(void) { }
-#endif
-
-/* from ir-rc6-decoder.c */
-#ifdef CONFIG_IR_RC6_DECODER_MODULE
-#define load_rc6_decode()	request_module_nowait("ir-rc6-decoder")
-#else
-static inline void load_rc6_decode(void) { }
-#endif
-
-/* from ir-jvc-decoder.c */
-#ifdef CONFIG_IR_JVC_DECODER_MODULE
-#define load_jvc_decode()	request_module_nowait("ir-jvc-decoder")
-#else
-static inline void load_jvc_decode(void) { }
-#endif
-
-/* from ir-sony-decoder.c */
-#ifdef CONFIG_IR_SONY_DECODER_MODULE
-#define load_sony_decode()	request_module_nowait("ir-sony-decoder")
-#else
-static inline void load_sony_decode(void) { }
-#endif
-
-/* from ir-sanyo-decoder.c */
-#ifdef CONFIG_IR_SANYO_DECODER_MODULE
-#define load_sanyo_decode()	request_module_nowait("ir-sanyo-decoder")
-#else
-static inline void load_sanyo_decode(void) { }
-#endif
-
-/* from ir-sharp-decoder.c */
-#ifdef CONFIG_IR_SHARP_DECODER_MODULE
-#define load_sharp_decode()	request_module_nowait("ir-sharp-decoder")
-#else
-static inline void load_sharp_decode(void) { }
-#endif
-
-/* from ir-mce_kbd-decoder.c */
-#ifdef CONFIG_IR_MCE_KBD_DECODER_MODULE
-#define load_mce_kbd_decode()	request_module_nowait("ir-mce_kbd-decoder")
-#else
-static inline void load_mce_kbd_decode(void) { }
-#endif
-
-/* from ir-lirc-codec.c */
-#ifdef CONFIG_IR_LIRC_CODEC_MODULE
-#define load_lirc_codec()	request_module_nowait("ir-lirc-codec")
-#else
-static inline void load_lirc_codec(void) { }
-#endif
-
-/* from ir-xmp-decoder.c */
-#ifdef CONFIG_IR_XMP_DECODER_MODULE
-#define load_xmp_decode()      request_module_nowait("ir-xmp-decoder")
-#else
-static inline void load_xmp_decode(void) { }
-#endif
-
-
 #endif /* _RC_CORE_PRIV */
diff --git a/drivers/media/rc/rc-ir-raw.c b/drivers/media/rc/rc-ir-raw.c
index ad260520a9d4..c69807fe2fef 100644
--- a/drivers/media/rc/rc-ir-raw.c
+++ b/drivers/media/rc/rc-ir-raw.c
@@ -59,7 +59,9 @@ static int ir_raw_event_thread(void *data)
 
 		mutex_lock(&ir_raw_handler_lock);
 		list_for_each_entry(handler, &ir_raw_handler_list, list)
-			handler->decode(raw->dev, ev);
+			if (raw->dev->enabled_protocols & handler->protocols ||
+			    !handler->protocols)
+				handler->decode(raw->dev, ev);
 		raw->prev_ev = ev;
 		mutex_unlock(&ir_raw_handler_lock);
 	}
@@ -246,6 +248,14 @@ static int change_protocol(struct rc_dev *dev, u64 *rc_type)
 	return 0;
 }
 
+static void ir_raw_disable_protocols(struct rc_dev *dev, u64 protocols)
+{
+	mutex_lock(&dev->lock);
+	dev->enabled_protocols &= ~protocols;
+	dev->enabled_wakeup_protocols &= ~protocols;
+	mutex_unlock(&dev->lock);
+}
+
 /*
  * Used to (un)register raw event clients
  */
@@ -337,33 +347,16 @@ EXPORT_SYMBOL(ir_raw_handler_register);
 void ir_raw_handler_unregister(struct ir_raw_handler *ir_raw_handler)
 {
 	struct ir_raw_event_ctrl *raw;
+	u64 protocols = ir_raw_handler->protocols;
 
 	mutex_lock(&ir_raw_handler_lock);
 	list_del(&ir_raw_handler->list);
-	if (ir_raw_handler->raw_unregister)
-		list_for_each_entry(raw, &ir_raw_client_list, list)
+	list_for_each_entry(raw, &ir_raw_client_list, list) {
+		ir_raw_disable_protocols(raw->dev, protocols);
+		if (ir_raw_handler->raw_unregister)
 			ir_raw_handler->raw_unregister(raw->dev);
-	available_protocols &= ~ir_raw_handler->protocols;
+	}
+	available_protocols &= ~protocols;
 	mutex_unlock(&ir_raw_handler_lock);
 }
 EXPORT_SYMBOL(ir_raw_handler_unregister);
-
-void ir_raw_init(void)
-{
-	/* Load the decoder modules */
-
-	load_nec_decode();
-	load_rc5_decode();
-	load_rc6_decode();
-	load_jvc_decode();
-	load_sony_decode();
-	load_sanyo_decode();
-	load_sharp_decode();
-	load_mce_kbd_decode();
-	load_lirc_codec();
-	load_xmp_decode();
-
-	/* If needed, we may later add some init code. In this case,
-	   it is needed to change the CONFIG_MODULE test at rc-core.h
-	 */
-}
diff --git a/drivers/media/rc/rc-main.c b/drivers/media/rc/rc-main.c
index 3f0f71adabb4..1042fa331a07 100644
--- a/drivers/media/rc/rc-main.c
+++ b/drivers/media/rc/rc-main.c
@@ -61,7 +61,7 @@ struct rc_map *rc_map_get(const char *name)
 	struct rc_map_list *map;
 
 	map = seek_rc_map(name);
-#ifdef MODULE
+#ifdef CONFIG_MODULES
 	if (!map) {
 		int rc = request_module("%s", name);
 		if (rc < 0) {
@@ -777,30 +777,31 @@ static struct class rc_class = {
  * used by the sysfs protocols file. Note that the order
  * of the entries is relevant.
  */
-static struct {
+static const struct {
 	u64	type;
-	char	*name;
+	const char	*name;
+	const char	*module_name;
 } proto_names[] = {
-	{ RC_BIT_NONE,		"none"		},
-	{ RC_BIT_OTHER,		"other"		},
-	{ RC_BIT_UNKNOWN,	"unknown"	},
+	{ RC_BIT_NONE,		"none",		NULL			},
+	{ RC_BIT_OTHER,		"other",	NULL			},
+	{ RC_BIT_UNKNOWN,	"unknown",	NULL			},
 	{ RC_BIT_RC5 |
-	  RC_BIT_RC5X,		"rc-5"		},
-	{ RC_BIT_NEC,		"nec"		},
+	  RC_BIT_RC5X,		"rc-5",		"ir-rc5-decoder"	},
+	{ RC_BIT_NEC,		"nec",		"ir-nec-decoder"	},
 	{ RC_BIT_RC6_0 |
 	  RC_BIT_RC6_6A_20 |
 	  RC_BIT_RC6_6A_24 |
 	  RC_BIT_RC6_6A_32 |
-	  RC_BIT_RC6_MCE,	"rc-6"		},
-	{ RC_BIT_JVC,		"jvc"		},
+	  RC_BIT_RC6_MCE,	"rc-6",		"ir-rc6-decoder"	},
+	{ RC_BIT_JVC,		"jvc",		"ir-jvc-decoder"	},
 	{ RC_BIT_SONY12 |
 	  RC_BIT_SONY15 |
-	  RC_BIT_SONY20,	"sony"		},
-	{ RC_BIT_RC5_SZ,	"rc-5-sz"	},
-	{ RC_BIT_SANYO,		"sanyo"		},
-	{ RC_BIT_SHARP,		"sharp"		},
-	{ RC_BIT_MCE_KBD,	"mce_kbd"	},
-	{ RC_BIT_XMP,		"xmp"		},
+	  RC_BIT_SONY20,	"sony",		"ir-sony-decoder"	},
+	{ RC_BIT_RC5_SZ,	"rc-5-sz",	"ir-rc5-decoder"	},
+	{ RC_BIT_SANYO,		"sanyo",	"ir-sanyo-decoder"	},
+	{ RC_BIT_SHARP,		"sharp",	"ir-sharp-decoder"	},
+	{ RC_BIT_MCE_KBD,	"mce_kbd",	"ir-mce_kbd-decoder"	},
+	{ RC_BIT_XMP,		"xmp",		"ir-xmp-decoder"	},
 };
 
 /**
@@ -979,6 +980,48 @@ static int parse_protocol_change(u64 *protocols, const char *buf)
 	return count;
 }
 
+static void ir_raw_load_modules(u64 *protocols)
+
+{
+	u64 available;
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(proto_names); i++) {
+		if (proto_names[i].type == RC_BIT_NONE ||
+		    proto_names[i].type & (RC_BIT_OTHER | RC_BIT_UNKNOWN))
+			continue;
+
+		available = ir_raw_get_allowed_protocols();
+		if (!(*protocols & proto_names[i].type & ~available))
+			continue;
+
+		if (!proto_names[i].module_name) {
+			pr_err("Can't enable IR protocol %s\n",
+			       proto_names[i].name);
+			*protocols &= ~proto_names[i].type;
+			continue;
+		}
+
+		ret = request_module("%s", proto_names[i].module_name);
+		if (ret < 0) {
+			pr_err("Couldn't load IR protocol module %s\n",
+			       proto_names[i].module_name);
+			*protocols &= ~proto_names[i].type;
+			continue;
+		}
+		msleep(20);
+		available = ir_raw_get_allowed_protocols();
+		if (!(*protocols & proto_names[i].type & ~available))
+			continue;
+
+		pr_err("Loaded IR protocol module %s, \
+		       but protocol %s still not available\n",
+		       proto_names[i].module_name,
+		       proto_names[i].name);
+		*protocols &= ~proto_names[i].type;
+	}
+}
+
 /**
  * store_protocols() - changes the current/wakeup IR protocol(s)
  * @device:	the device descriptor
@@ -1045,6 +1088,9 @@ static ssize_t store_protocols(struct device *device,
 		goto out;
 	}
 
+	if (dev->driver_type == RC_DRIVER_IR_RAW)
+		ir_raw_load_modules(&new_protocols);
+
 	if (new_protocols != old_protocols) {
 		*current_protocols = new_protocols;
 		IR_dprintk(1, "Protocols changed to 0x%llx\n",
@@ -1420,17 +1466,13 @@ int rc_register_device(struct rc_dev *dev)
 	dev->input_dev->rep[REP_PERIOD] = 125;
 
 	path = kobject_get_path(&dev->dev.kobj, GFP_KERNEL);
-	printk(KERN_INFO "%s: %s as %s\n",
-		dev_name(&dev->dev),
-		dev->input_name ? dev->input_name : "Unspecified device",
-		path ? path : "N/A");
+	dev_info(&dev->dev, "%s as %s\n",
+		dev->input_name ?: "Unspecified device", path ?: "N/A");
 	kfree(path);
 
 	if (dev->driver_type == RC_DRIVER_IR_RAW) {
-		/* Load raw decoders, if they aren't already */
 		if (!raw_init) {
-			IR_dprintk(1, "Loading raw decoders\n");
-			ir_raw_init();
+			request_module_nowait("ir-lirc-codec");
 			raw_init = true;
 		}
 		/* calls ir_register_device so unlock mutex here*/
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index 37d040158dff..1fa0c9d1c508 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -16,6 +16,7 @@
 #include <linux/reset.h>
 #include <media/rc-core.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/pm_wakeirq.h>
 
 struct st_rc_device {
 	struct device			*dev;
@@ -190,6 +191,9 @@ static void st_rc_hardware_init(struct st_rc_device *dev)
 static int st_rc_remove(struct platform_device *pdev)
 {
 	struct st_rc_device *rc_dev = platform_get_drvdata(pdev);
+
+	dev_pm_clear_wake_irq(&pdev->dev);
+	device_init_wakeup(&pdev->dev, false);
 	clk_disable_unprepare(rc_dev->sys_clock);
 	rc_unregister_device(rc_dev->rdev);
 	return 0;
@@ -298,22 +302,22 @@ static int st_rc_probe(struct platform_device *pdev)
 	rdev->map_name = RC_MAP_LIRC;
 	rdev->input_name = "ST Remote Control Receiver";
 
-	/* enable wake via this device */
-	device_set_wakeup_capable(dev, true);
-	device_set_wakeup_enable(dev, true);
-
 	ret = rc_register_device(rdev);
 	if (ret < 0)
 		goto clkerr;
 
 	rc_dev->rdev = rdev;
 	if (devm_request_irq(dev, rc_dev->irq, st_rc_rx_interrupt,
-			IRQF_NO_SUSPEND, IR_ST_NAME, rc_dev) < 0) {
+			     0, IR_ST_NAME, rc_dev) < 0) {
 		dev_err(dev, "IRQ %d register failed\n", rc_dev->irq);
 		ret = -EINVAL;
 		goto rcerr;
 	}
 
+	/* enable wake via this device */
+	device_init_wakeup(dev, true);
+	dev_pm_set_wake_irq(dev, rc_dev->irq);
+
 	/**
 	 * for LIRC_MODE_MODE2 or LIRC_MODE_PULSE or LIRC_MODE_RAW
 	 * lircd expects a long space first before a signal train to sync.
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index 5a17cb88ff27..815243c65bc3 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -34,6 +34,7 @@
 #include <linux/device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include <linux/ktime.h>
 #include <linux/usb.h>
 #include <linux/usb/input.h>
 #include <media/rc-core.h>
@@ -96,8 +97,8 @@ struct streamzap_ir {
 	/* sum of signal lengths received since signal start */
 	unsigned long		sum;
 	/* start time of signal; necessary for gap tracking */
-	struct timeval		signal_last;
-	struct timeval		signal_start;
+	ktime_t			signal_last;
+	ktime_t			signal_start;
 	bool			timeout_enabled;
 
 	char			name[128];
@@ -136,20 +137,18 @@ static void sz_push_full_pulse(struct streamzap_ir *sz,
 	DEFINE_IR_RAW_EVENT(rawir);
 
 	if (sz->idle) {
-		long deltv;
+		int delta;
 
 		sz->signal_last = sz->signal_start;
-		do_gettimeofday(&sz->signal_start);
+		sz->signal_start = ktime_get_real();
 
-		deltv = sz->signal_start.tv_sec - sz->signal_last.tv_sec;
+		delta = ktime_us_delta(sz->signal_start, sz->signal_last);
 		rawir.pulse = false;
-		if (deltv > 15) {
+		if (delta > (15 * USEC_PER_SEC)) {
 			/* really long time */
 			rawir.duration = IR_MAX_DURATION;
 		} else {
-			rawir.duration = (int)(deltv * 1000000 +
-				sz->signal_start.tv_usec -
-				sz->signal_last.tv_usec);
+			rawir.duration = delta;
 			rawir.duration -= sz->sum;
 			rawir.duration = US_TO_NS(rawir.duration);
 			rawir.duration = (rawir.duration > IR_MAX_DURATION) ?
@@ -428,7 +427,7 @@ static int streamzap_probe(struct usb_interface *intf,
 	sz->max_timeout = US_TO_NS(SZ_TIMEOUT * SZ_RESOLUTION);
 	#endif
 
-	do_gettimeofday(&sz->signal_start);
+	sz->signal_start = ktime_get_real();
 
 	/* Complete final initialisations */
 	usb_fill_int_urb(sz->urb_in, usbdev, pipe, sz->buf_in,
diff --git a/drivers/media/rc/sunxi-cir.c b/drivers/media/rc/sunxi-cir.c
index 7830aef3db45..40f77685cc4a 100644
--- a/drivers/media/rc/sunxi-cir.c
+++ b/drivers/media/rc/sunxi-cir.c
@@ -153,6 +153,8 @@ static int sunxi_ir_probe(struct platform_device *pdev)
 	if (!ir)
 		return -ENOMEM;
 
+	spin_lock_init(&ir->ir_lock);
+
 	if (of_device_is_compatible(dn, "allwinner,sun5i-a13-ir"))
 		ir->fifo_size = 64;
 	else
diff --git a/drivers/media/tuners/max2165.c b/drivers/media/tuners/max2165.c
index 95ed46f2cd26..353b178becf6 100644
--- a/drivers/media/tuners/max2165.c
+++ b/drivers/media/tuners/max2165.c
@@ -385,7 +385,7 @@ static const struct dvb_tuner_ops max2165_tuner_ops = {
 	.info = {
 		.name           = "Maxim MAX2165",
 		.frequency_min  = 470000000,
-		.frequency_max  = 780000000,
+		.frequency_max  = 862000000,
 		.frequency_step =     50000,
 	},
 
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index 9e9c5eb4cb66..6457ac91ef09 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -225,7 +225,6 @@ struct mt2063_state {
 	const struct mt2063_config *config;
 	struct dvb_tuner_ops ops;
 	struct dvb_frontend *frontend;
-	struct tuner_state status;
 
 	u32 frequency;
 	u32 srate;
diff --git a/drivers/media/tuners/si2157.c b/drivers/media/tuners/si2157.c
index ce157edd45fa..0e1ca2b00e61 100644
--- a/drivers/media/tuners/si2157.c
+++ b/drivers/media/tuners/si2157.c
@@ -168,6 +168,7 @@ static int si2157_init(struct dvb_frontend *fe)
 		len = fw->data[fw->size - remaining];
 		if (len > SI2157_ARGLEN) {
 			dev_err(&client->dev, "Bad firmware length\n");
+			ret = -EINVAL;
 			goto err_release_firmware;
 		}
 		memcpy(cmd.args, &fw->data[(fw->size - remaining) + 1], len);
diff --git a/drivers/media/usb/airspy/airspy.c b/drivers/media/usb/airspy/airspy.c
index 565a59310747..0d4ac5947f3a 100644
--- a/drivers/media/usb/airspy/airspy.c
+++ b/drivers/media/usb/airspy/airspy.c
@@ -316,7 +316,7 @@ static void airspy_urb_complete(struct urb *urb)
 		len = airspy_convert_stream(s, ptr, urb->transfer_buffer,
 				urb->actual_length);
 		vb2_set_plane_payload(&fbuf->vb.vb2_buf, 0, len);
-		v4l2_get_timestamp(&fbuf->vb.timestamp);
+		fbuf->vb.vb2_buf.timestamp = ktime_get_ns();
 		fbuf->vb.sequence = s->sequence++;
 		vb2_buffer_done(&fbuf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	}
@@ -488,7 +488,7 @@ static void airspy_disconnect(struct usb_interface *intf)
 
 /* Videobuf2 operations */
 static int airspy_queue_setup(struct vb2_queue *vq,
-		const void *parg, unsigned int *nbuffers,
+		unsigned int *nbuffers,
 		unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct airspy *s = vb2_get_drv_priv(vq);
diff --git a/drivers/media/usb/as102/as102_fw.c b/drivers/media/usb/as102/as102_fw.c
index 07d08c49f4d4..5a28ce3a1d49 100644
--- a/drivers/media/usb/as102/as102_fw.c
+++ b/drivers/media/usb/as102/as102_fw.c
@@ -198,6 +198,7 @@ int as102_fw_upload(struct as10x_bus_adapter_t *bus_adap)
 	pr_info("%s: firmware: %s loaded with success\n",
 		DRIVER_NAME, fw1);
 	release_firmware(firmware);
+	firmware = NULL;
 
 	/* wait for boot to complete */
 	mdelay(100);
diff --git a/drivers/media/usb/au0828/au0828-cards.c b/drivers/media/usb/au0828/au0828-cards.c
index 6b469e8c4c6e..ca861aea68a5 100644
--- a/drivers/media/usb/au0828/au0828-cards.c
+++ b/drivers/media/usb/au0828/au0828-cards.c
@@ -228,6 +228,10 @@ void au0828_card_analog_fe_setup(struct au0828_dev *dev)
 				"au8522", 0x8e >> 1, NULL);
 		if (sd == NULL)
 			pr_err("analog subdev registration failed\n");
+#ifdef CONFIG_MEDIA_CONTROLLER
+		if (sd)
+			dev->decoder = &sd->entity;
+#endif
 	}
 
 	/* Setup tuners */
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index 0934024fb89d..9e29e70a78d7 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -27,6 +27,9 @@
 #include <media/v4l2-common.h>
 #include <linux/mutex.h>
 
+/* Due to enum tuner_pad_index */
+#include <media/tuner.h>
+
 /*
  * 1 = General debug messages
  * 2 = USB handling
@@ -127,8 +130,23 @@ static int recv_control_msg(struct au0828_dev *dev, u16 request, u32 value,
 	return status;
 }
 
+static void au0828_unregister_media_device(struct au0828_dev *dev)
+{
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	if (dev->media_dev) {
+		media_device_unregister(dev->media_dev);
+		media_device_cleanup(dev->media_dev);
+		kfree(dev->media_dev);
+		dev->media_dev = NULL;
+	}
+#endif
+}
+
 static void au0828_usb_release(struct au0828_dev *dev)
 {
+	au0828_unregister_media_device(dev);
+
 	/* I2C */
 	au0828_i2c_unregister(dev);
 
@@ -136,6 +154,20 @@ static void au0828_usb_release(struct au0828_dev *dev)
 }
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
+
+static void au0828_usb_v4l2_media_release(struct au0828_dev *dev)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER
+	int i;
+
+	for (i = 0; i < AU0828_MAX_INPUT; i++) {
+		if (AUVI_INPUT(i).type == AU0828_VMUX_UNDEFINED)
+			return;
+		media_device_unregister_entity(&dev->input_ent[i]);
+	}
+#endif
+}
+
 static void au0828_usb_v4l2_release(struct v4l2_device *v4l2_dev)
 {
 	struct au0828_dev *dev =
@@ -143,6 +175,7 @@ static void au0828_usb_v4l2_release(struct v4l2_device *v4l2_dev)
 
 	v4l2_ctrl_handler_free(&dev->v4l2_ctrl_hdl);
 	v4l2_device_unregister(&dev->v4l2_dev);
+	au0828_usb_v4l2_media_release(dev);
 	au0828_usb_release(dev);
 }
 #endif
@@ -174,12 +207,123 @@ static void au0828_usb_disconnect(struct usb_interface *interface)
 		au0828_analog_unregister(dev);
 		v4l2_device_disconnect(&dev->v4l2_dev);
 		v4l2_device_put(&dev->v4l2_dev);
+		/*
+		 * No need to call au0828_usb_release() if V4L2 is enabled,
+		 * as this is already called via au0828_usb_v4l2_release()
+		 */
 		return;
 	}
 #endif
 	au0828_usb_release(dev);
 }
 
+static int au0828_media_device_init(struct au0828_dev *dev,
+				    struct usb_device *udev)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_device *mdev;
+
+	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
+	if (!mdev)
+		return -ENOMEM;
+
+	mdev->dev = &udev->dev;
+
+	if (!dev->board.name)
+		strlcpy(mdev->model, "unknown au0828", sizeof(mdev->model));
+	else
+		strlcpy(mdev->model, dev->board.name, sizeof(mdev->model));
+	if (udev->serial)
+		strlcpy(mdev->serial, udev->serial, sizeof(mdev->serial));
+	strcpy(mdev->bus_info, udev->devpath);
+	mdev->hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
+	mdev->driver_version = LINUX_VERSION_CODE;
+
+	media_device_init(mdev);
+
+	dev->media_dev = mdev;
+#endif
+	return 0;
+}
+
+
+static int au0828_create_media_graph(struct au0828_dev *dev)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_device *mdev = dev->media_dev;
+	struct media_entity *entity;
+	struct media_entity *tuner = NULL, *decoder = NULL;
+	int i, ret;
+
+	if (!mdev)
+		return 0;
+
+	media_device_for_each_entity(entity, mdev) {
+		switch (entity->function) {
+		case MEDIA_ENT_F_TUNER:
+			tuner = entity;
+			break;
+		case MEDIA_ENT_F_ATV_DECODER:
+			decoder = entity;
+			break;
+		}
+	}
+
+	/* Analog setup, using tuner as a link */
+
+	/* Something bad happened! */
+	if (!decoder)
+		return -EINVAL;
+
+	if (tuner) {
+		ret = media_create_pad_link(tuner, TUNER_PAD_IF_OUTPUT,
+					    decoder, 0,
+					    MEDIA_LNK_FL_ENABLED);
+		if (ret)
+			return ret;
+	}
+	ret = media_create_pad_link(decoder, 1, &dev->vdev.entity, 0,
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret)
+		return ret;
+	ret = media_create_pad_link(decoder, 2, &dev->vbi_dev.entity, 0,
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < AU0828_MAX_INPUT; i++) {
+		struct media_entity *ent = &dev->input_ent[i];
+
+		if (AUVI_INPUT(i).type == AU0828_VMUX_UNDEFINED)
+			break;
+
+		switch (AUVI_INPUT(i).type) {
+		case AU0828_VMUX_CABLE:
+		case AU0828_VMUX_TELEVISION:
+		case AU0828_VMUX_DVB:
+			if (!tuner)
+				break;
+
+			ret = media_create_pad_link(ent, 0, tuner,
+						    TUNER_PAD_RF_INPUT,
+						    MEDIA_LNK_FL_ENABLED);
+			if (ret)
+				return ret;
+			break;
+		case AU0828_VMUX_COMPOSITE:
+		case AU0828_VMUX_SVIDEO:
+		default: /* AU0828_VMUX_DEBUG */
+			/* FIXME: fix the decoder PAD */
+			ret = media_create_pad_link(ent, 0, decoder, 0, 0);
+			if (ret)
+				return ret;
+			break;
+		}
+	}
+#endif
+	return 0;
+}
+
 static int au0828_usb_probe(struct usb_interface *interface,
 	const struct usb_device_id *id)
 {
@@ -224,11 +368,23 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	dev->boardnr = id->driver_info;
 	dev->board = au0828_boards[dev->boardnr];
 
+	/* Initialize the media controller */
+	retval = au0828_media_device_init(dev, usbdev);
+	if (retval) {
+		pr_err("%s() au0828_media_device_init failed\n",
+		       __func__);
+		mutex_unlock(&dev->lock);
+		kfree(dev);
+		return retval;
+	}
 
 #ifdef CONFIG_VIDEO_AU0828_V4L2
 	dev->v4l2_dev.release = au0828_usb_v4l2_release;
 
 	/* Create the v4l2_device */
+#ifdef CONFIG_MEDIA_CONTROLLER
+	dev->v4l2_dev.mdev = dev->media_dev;
+#endif
 	retval = v4l2_device_register(&interface->dev, &dev->v4l2_dev);
 	if (retval) {
 		pr_err("%s() v4l2_device_register failed\n",
@@ -287,6 +443,21 @@ static int au0828_usb_probe(struct usb_interface *interface,
 
 	mutex_unlock(&dev->lock);
 
+	retval = au0828_create_media_graph(dev);
+	if (retval) {
+		pr_err("%s() au0282_dev_register failed to create graph\n",
+		       __func__);
+		goto done;
+	}
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	retval = media_device_register(dev->media_dev);
+#endif
+
+done:
+	if (retval < 0)
+		au0828_usb_disconnect(interface);
+
 	return retval;
 }
 
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index c267d76f5b3c..94363a3ba400 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -415,6 +415,11 @@ static int dvb_register(struct au0828_dev *dev)
 		       result);
 		goto fail_adapter;
 	}
+
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	dvb->adapter.mdev = dev->media_dev;
+#endif
+
 	dvb->adapter.priv = dev;
 
 	/* register frontend */
@@ -480,8 +485,15 @@ static int dvb_register(struct au0828_dev *dev)
 
 	dvb->start_count = 0;
 	dvb->stop_count = 0;
+
+	result = dvb_create_media_graph(&dvb->adapter, false);
+	if (result < 0)
+		goto fail_create_graph;
+
 	return 0;
 
+fail_create_graph:
+	dvb_net_release(&dvb->net);
 fail_fe_conn:
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_mem);
 fail_fe_mem:
diff --git a/drivers/media/usb/au0828/au0828-vbi.c b/drivers/media/usb/au0828/au0828-vbi.c
index 130c8b49bf7f..b4efc103ae57 100644
--- a/drivers/media/usb/au0828/au0828-vbi.c
+++ b/drivers/media/usb/au0828/au0828-vbi.c
@@ -30,23 +30,17 @@
 
 /* ------------------------------------------------------------------ */
 
-static int vbi_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vbi_queue_setup(struct vb2_queue *vq,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct au0828_dev *dev = vb2_get_drv_priv(vq);
-	unsigned long img_size = dev->vbi_width * dev->vbi_height * 2;
-	unsigned long size;
-
-	size = fmt ? (fmt->fmt.vbi.samples_per_line *
-		(fmt->fmt.vbi.count[0] + fmt->fmt.vbi.count[1])) : img_size;
-	if (size < img_size)
-		return -EINVAL;
+	unsigned long size = dev->vbi_width * dev->vbi_height * 2;
 
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
 	sizes[0] = size;
-
 	return 0;
 }
 
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 45c622e234f7..a13625722848 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -314,7 +314,7 @@ static inline void buffer_filled(struct au0828_dev *dev,
 		vb->sequence = dev->vbi_frame_count++;
 
 	vb->field = V4L2_FIELD_INTERLACED;
-	v4l2_get_timestamp(&vb->timestamp);
+	vb->vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&vb->vb2_buf, VB2_BUF_STATE_DONE);
 }
 
@@ -638,22 +638,78 @@ static inline int au0828_isoc_copy(struct au0828_dev *dev, struct urb *urb)
 	return rc;
 }
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int au0828_enable_analog_tuner(struct au0828_dev *dev)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_device *mdev = dev->media_dev;
+	struct media_entity *source;
+	struct media_link *link, *found_link = NULL;
+	int ret, active_links = 0;
+
+	if (!mdev || !dev->decoder)
+		return 0;
+
+	/*
+	 * This will find the tuner that is connected into the decoder.
+	 * Technically, this is not 100% correct, as the device may be
+	 * using an analog input instead of the tuner. However, as we can't
+	 * do DVB streaming while the DMA engine is being used for V4L2,
+	 * this should be enough for the actual needs.
+	 */
+	list_for_each_entry(link, &dev->decoder->links, list) {
+		if (link->sink->entity == dev->decoder) {
+			found_link = link;
+			if (link->flags & MEDIA_LNK_FL_ENABLED)
+				active_links++;
+			break;
+		}
+	}
+
+	if (active_links == 1 || !found_link)
+		return 0;
+
+	source = found_link->source->entity;
+	list_for_each_entry(link, &source->links, list) {
+		struct media_entity *sink;
+		int flags = 0;
+
+		sink = link->sink->entity;
+
+		if (sink == dev->decoder)
+			flags = MEDIA_LNK_FL_ENABLED;
+
+		ret = media_entity_setup_link(link, flags);
+		if (ret) {
+			pr_err(
+				"Couldn't change link %s->%s to %s. Error %d\n",
+				source->name, sink->name,
+				flags ? "enabled" : "disabled",
+				ret);
+			return ret;
+		} else
+			au0828_isocdbg(
+				"link %s->%s was %s\n",
+				source->name, sink->name,
+				flags ? "ENABLED" : "disabled");
+	}
+#endif
+	return 0;
+}
+
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct au0828_dev *dev = vb2_get_drv_priv(vq);
-	unsigned long img_size = dev->height * dev->bytesperline;
-	unsigned long size;
-
-	size = fmt ? fmt->fmt.pix.sizeimage : img_size;
-	if (size < img_size)
-		return -EINVAL;
+	unsigned long size = dev->height * dev->bytesperline;
 
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
 	sizes[0] = size;
 
+	au0828_enable_analog_tuner(dev);
+
 	return 0;
 }
 
@@ -1739,6 +1795,68 @@ static int au0828_vb2_setup(struct au0828_dev *dev)
 	return 0;
 }
 
+static void au0828_analog_create_entities(struct au0828_dev *dev)
+{
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	static const char * const inames[] = {
+		[AU0828_VMUX_COMPOSITE] = "Composite",
+		[AU0828_VMUX_SVIDEO] = "S-Video",
+		[AU0828_VMUX_CABLE] = "Cable TV",
+		[AU0828_VMUX_TELEVISION] = "Television",
+		[AU0828_VMUX_DVB] = "DVB",
+		[AU0828_VMUX_DEBUG] = "tv debug"
+	};
+	int ret, i;
+
+	/* Initialize Video and VBI pads */
+	dev->video_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&dev->vdev.entity, 1, &dev->video_pad);
+	if (ret < 0)
+		pr_err("failed to initialize video media entity!\n");
+
+	dev->vbi_pad.flags = MEDIA_PAD_FL_SINK;
+	ret = media_entity_pads_init(&dev->vbi_dev.entity, 1, &dev->vbi_pad);
+	if (ret < 0)
+		pr_err("failed to initialize vbi media entity!\n");
+
+	/* Create entities for each input connector */
+	for (i = 0; i < AU0828_MAX_INPUT; i++) {
+		struct media_entity *ent = &dev->input_ent[i];
+
+		if (AUVI_INPUT(i).type == AU0828_VMUX_UNDEFINED)
+			break;
+
+		ent->name = inames[AUVI_INPUT(i).type];
+		ent->flags = MEDIA_ENT_FL_CONNECTOR;
+		dev->input_pad[i].flags = MEDIA_PAD_FL_SOURCE;
+
+		switch (AUVI_INPUT(i).type) {
+		case AU0828_VMUX_COMPOSITE:
+			ent->function = MEDIA_ENT_F_CONN_COMPOSITE;
+			break;
+		case AU0828_VMUX_SVIDEO:
+			ent->function = MEDIA_ENT_F_CONN_SVIDEO;
+			break;
+		case AU0828_VMUX_CABLE:
+		case AU0828_VMUX_TELEVISION:
+		case AU0828_VMUX_DVB:
+			ent->function = MEDIA_ENT_F_CONN_RF;
+			break;
+		default: /* AU0828_VMUX_DEBUG */
+			continue;
+		}
+
+		ret = media_entity_pads_init(ent, 1, &dev->input_pad[i]);
+		if (ret < 0)
+			pr_err("failed to initialize input pad[%d]!\n", i);
+
+		ret = media_device_register_entity(dev->media_dev, ent);
+		if (ret < 0)
+			pr_err("failed to register input entity %d!\n", i);
+	}
+#endif
+}
+
 /**************************************************************************/
 
 int au0828_analog_register(struct au0828_dev *dev,
@@ -1827,6 +1945,9 @@ int au0828_analog_register(struct au0828_dev *dev,
 	dev->vbi_dev.queue->lock = &dev->vb_vbi_queue_lock;
 	strcpy(dev->vbi_dev.name, "au0828a vbi");
 
+	/* Init entities at the Media Controller */
+	au0828_analog_create_entities(dev);
+
 	/* initialize videobuf2 stuff */
 	retval = au0828_vb2_setup(dev);
 	if (retval != 0) {
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index 60b59391ea2a..8276072bc55a 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -33,6 +33,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fh.h>
+#include <media/media-device.h>
 
 /* DVB */
 #include "demux.h"
@@ -93,7 +94,6 @@ struct au0828_board {
 	unsigned char has_ir_i2c:1;
 	unsigned char has_analog:1;
 	struct au0828_input input[AU0828_MAX_INPUT];
-
 };
 
 struct au0828_dvb {
@@ -276,6 +276,14 @@ struct au0828_dev {
 	/* Preallocated transfer digital transfer buffers */
 
 	char *dig_transfer_buffer[URB_COUNT];
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	struct media_device *media_dev;
+	struct media_pad video_pad, vbi_pad;
+	struct media_entity *decoder;
+	struct media_entity input_ent[AU0828_MAX_INPUT];
+	struct media_pad input_pad[AU0828_MAX_INPUT];
+#endif
 };
 
 
diff --git a/drivers/media/usb/cpia2/cpia2_usb.c b/drivers/media/usb/cpia2/cpia2_usb.c
index 351a78a84c3d..c1aa1ab2ece9 100644
--- a/drivers/media/usb/cpia2/cpia2_usb.c
+++ b/drivers/media/usb/cpia2/cpia2_usb.c
@@ -890,8 +890,7 @@ static void cpia2_usb_disconnect(struct usb_interface *intf)
 		DBG("Wakeup waiting processes\n");
 		cam->curbuff->status = FRAME_READY;
 		cam->curbuff->length = 0;
-		if (waitqueue_active(&cam->wq_stream))
-			wake_up_interruptible(&cam->wq_stream);
+		wake_up_interruptible(&cam->wq_stream);
 	}
 
 	DBG("Releasing interface\n");
diff --git a/drivers/media/usb/cx231xx/cx231xx-417.c b/drivers/media/usb/cx231xx/cx231xx-417.c
index 47a98a2014a5..48643b94e694 100644
--- a/drivers/media/usb/cx231xx/cx231xx-417.c
+++ b/drivers/media/usb/cx231xx/cx231xx-417.c
@@ -37,7 +37,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 #include <media/tuner.h>
 
 #define CX231xx_FIRM_IMAGE_SIZE 376836
@@ -1492,6 +1492,27 @@ static struct videobuf_queue_ops cx231xx_qops = {
 
 /* ------------------------------------------------------------------ */
 
+static int vidioc_cropcap(struct file *file, void *priv,
+			  struct v4l2_cropcap *cc)
+{
+	struct cx231xx_fh *fh = priv;
+	struct cx231xx *dev = fh->dev;
+	bool is_50hz = dev->encodernorm.id & V4L2_STD_625_50;
+
+	if (cc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	cc->bounds.left = 0;
+	cc->bounds.top = 0;
+	cc->bounds.width = dev->ts1.width;
+	cc->bounds.height = dev->ts1.height;
+	cc->defrect = cc->bounds;
+	cc->pixelaspect.numerator = is_50hz ? 54 : 11;
+	cc->pixelaspect.denominator = is_50hz ? 59 : 10;
+
+	return 0;
+}
+
 static int vidioc_g_std(struct file *file, void *fh0, v4l2_std_id *norm)
 {
 	struct cx231xx_fh  *fh  = file->private_data;
@@ -1834,6 +1855,7 @@ static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_g_input		 = cx231xx_g_input,
 	.vidioc_s_input		 = cx231xx_s_input,
 	.vidioc_s_ctrl		 = vidioc_s_ctrl,
+	.vidioc_cropcap		 = vidioc_cropcap,
 	.vidioc_querycap	 = cx231xx_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap	 = vidioc_g_fmt_vid_cap,
@@ -1901,7 +1923,7 @@ static int cx231xx_s_audio_sampling_freq(struct cx2341x_handler *cxhdl, u32 idx)
 	return 0;
 }
 
-static struct cx2341x_handler_ops cx231xx_ops = {
+static const struct cx2341x_handler_ops cx231xx_ops = {
 	/* needed for the video clock freq */
 	.s_audio_sampling_freq = cx231xx_s_audio_sampling_freq,
 	/* needed for setting up the video resolution */
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 4a117a58c39a..620b83d03f75 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -30,7 +30,7 @@
 #include <media/tveeprom.h>
 #include <media/v4l2-common.h>
 
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 #include "dvb-usb-ids.h"
 #include "xc5000.h"
 #include "tda18271.h"
@@ -352,7 +352,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x0c,
 		.gpio_pin_status_mask = 0x4001000,
 		.tuner_i2c_master = I2C_1_MUX_1,
-		.demod_i2c_master = I2C_2,
+		.demod_i2c_master = I2C_1_MUX_1,
 		.has_dvb = 1,
 		.demod_addr = 0x0e,
 		.norm = V4L2_STD_NTSC,
@@ -713,7 +713,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x0c,
 		.gpio_pin_status_mask = 0x4001000,
 		.tuner_i2c_master = I2C_1_MUX_3,
-		.demod_i2c_master = I2C_2,
+		.demod_i2c_master = I2C_1_MUX_3,
 		.has_dvb = 1,
 		.demod_addr = 0x0e,
 		.norm = V4L2_STD_PAL,
@@ -752,7 +752,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x0c,
 		.gpio_pin_status_mask = 0x4001000,
 		.tuner_i2c_master = I2C_1_MUX_3,
-		.demod_i2c_master = I2C_2,
+		.demod_i2c_master = I2C_1_MUX_3,
 		.has_dvb = 1,
 		.demod_addr = 0x0e,
 		.norm = V4L2_STD_PAL,
@@ -791,7 +791,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		.agc_analog_digital_select_gpio = 0x0c,
 		.gpio_pin_status_mask = 0x4001000,
 		.tuner_i2c_master = I2C_1_MUX_3,
-		.demod_i2c_master = I2C_2,
+		.demod_i2c_master = I2C_1_MUX_3,
 		.has_dvb = 1,
 		.demod_addr = 0x0e,
 		.norm = V4L2_STD_NTSC,
@@ -1172,6 +1172,7 @@ static void cx231xx_unregister_media_device(struct cx231xx *dev)
 #ifdef CONFIG_MEDIA_CONTROLLER
 	if (dev->media_dev) {
 		media_device_unregister(dev->media_dev);
+		media_device_cleanup(dev->media_dev);
 		kfree(dev->media_dev);
 		dev->media_dev = NULL;
 	}
@@ -1185,8 +1186,6 @@ static void cx231xx_unregister_media_device(struct cx231xx *dev)
 */
 void cx231xx_release_resources(struct cx231xx *dev)
 {
-	cx231xx_unregister_media_device(dev);
-
 	cx231xx_release_analog_resources(dev);
 
 	cx231xx_remove_from_devlist(dev);
@@ -1199,22 +1198,23 @@ void cx231xx_release_resources(struct cx231xx *dev)
 	/* delete v4l2 device */
 	v4l2_device_unregister(&dev->v4l2_dev);
 
+	cx231xx_unregister_media_device(dev);
+
 	usb_put_dev(dev->udev);
 
 	/* Mark device as unused */
 	clear_bit(dev->devno, &cx231xx_devused);
 }
 
-static void cx231xx_media_device_register(struct cx231xx *dev,
-					  struct usb_device *udev)
+static int cx231xx_media_device_init(struct cx231xx *dev,
+				      struct usb_device *udev)
 {
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct media_device *mdev;
-	int ret;
 
 	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 	if (!mdev)
-		return;
+		return -ENOMEM;
 
 	mdev->dev = dev->dev;
 	strlcpy(mdev->model, dev->board.name, sizeof(mdev->model));
@@ -1224,35 +1224,30 @@ static void cx231xx_media_device_register(struct cx231xx *dev,
 	mdev->hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
 	mdev->driver_version = LINUX_VERSION_CODE;
 
-	ret = media_device_register(mdev);
-	if (ret) {
-		dev_err(dev->dev,
-			"Couldn't create a media device. Error: %d\n",
-			ret);
-		kfree(mdev);
-		return;
-	}
+	media_device_init(mdev);
 
 	dev->media_dev = mdev;
 #endif
+	return 0;
 }
 
-static void cx231xx_create_media_graph(struct cx231xx *dev)
+static int cx231xx_create_media_graph(struct cx231xx *dev)
 {
 #ifdef CONFIG_MEDIA_CONTROLLER
 	struct media_device *mdev = dev->media_dev;
 	struct media_entity *entity;
 	struct media_entity *tuner = NULL, *decoder = NULL;
+	int ret;
 
 	if (!mdev)
-		return;
+		return 0;
 
 	media_device_for_each_entity(entity, mdev) {
-		switch (entity->type) {
-		case MEDIA_ENT_T_V4L2_SUBDEV_TUNER:
+		switch (entity->function) {
+		case MEDIA_ENT_F_TUNER:
 			tuner = entity;
 			break;
-		case MEDIA_ENT_T_V4L2_SUBDEV_DECODER:
+		case MEDIA_ENT_F_ATV_DECODER:
 			decoder = entity;
 			break;
 		}
@@ -1261,16 +1256,24 @@ static void cx231xx_create_media_graph(struct cx231xx *dev)
 	/* Analog setup, using tuner as a link */
 
 	if (!decoder)
-		return;
+		return 0;
 
-	if (tuner)
-		media_entity_create_link(tuner, 0, decoder, 0,
-					 MEDIA_LNK_FL_ENABLED);
-	media_entity_create_link(decoder, 1, &dev->vdev.entity, 0,
-				 MEDIA_LNK_FL_ENABLED);
-	media_entity_create_link(decoder, 2, &dev->vbi_dev.entity, 0,
-				 MEDIA_LNK_FL_ENABLED);
+	if (tuner) {
+		ret = media_create_pad_link(tuner, TUNER_PAD_IF_OUTPUT, decoder, 0,
+					    MEDIA_LNK_FL_ENABLED);
+		if (ret < 0)
+			return ret;
+	}
+	ret = media_create_pad_link(decoder, 1, &dev->vdev.entity, 0,
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret < 0)
+		return ret;
+	ret = media_create_pad_link(decoder, 2, &dev->vbi_dev.entity, 0,
+				    MEDIA_LNK_FL_ENABLED);
+	if (ret < 0)
+		return ret;
 #endif
+	return 0;
 }
 
 /*
@@ -1660,8 +1663,12 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	/* save our data pointer in this interface device */
 	usb_set_intfdata(interface, dev);
 
-	/* Register the media controller */
-	cx231xx_media_device_register(dev, udev);
+	/* Initialize the media controller */
+	retval = cx231xx_media_device_init(dev, udev);
+	if (retval) {
+		dev_err(d, "cx231xx_media_device_init failed\n");
+		goto err_media_init;
+	}
 
 	/* Create v4l2 device */
 #ifdef CONFIG_MEDIA_CONTROLLER
@@ -1732,9 +1739,19 @@ static int cx231xx_usb_probe(struct usb_interface *interface,
 	/* load other modules required */
 	request_modules(dev);
 
-	cx231xx_create_media_graph(dev);
+	retval = cx231xx_create_media_graph(dev);
+	if (retval < 0)
+		goto done;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+	retval = media_device_register(dev->media_dev);
+#endif
+
+done:
+	if (retval < 0)
+		cx231xx_release_resources(dev);
+	return retval;
 
-	return 0;
 err_video_alt:
 	/* cx231xx_uninit_dev: */
 	cx231xx_close_extension(dev);
@@ -1746,6 +1763,8 @@ err_video_alt:
 err_init:
 	v4l2_device_unregister(&dev->v4l2_dev);
 err_v4l2:
+	cx231xx_unregister_media_device(dev);
+err_media_init:
 	usb_set_intfdata(interface, NULL);
 err_if:
 	usb_put_dev(udev);
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index a2fd49b6be83..f497888d94bf 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -914,6 +914,7 @@ EXPORT_SYMBOL_GPL(cx231xx_uninit_isoc);
  */
 void cx231xx_uninit_bulk(struct cx231xx *dev)
 {
+	struct cx231xx_dmaqueue *dma_q = &dev->video_mode.vidq;
 	struct urb *urb;
 	int i;
 
@@ -931,7 +932,7 @@ void cx231xx_uninit_bulk(struct cx231xx *dev)
 			if (dev->video_mode.bulk_ctl.transfer_buffer[i]) {
 				usb_free_coherent(dev->udev,
 						urb->transfer_buffer_length,
-						dev->video_mode.isoc_ctl.
+						dev->video_mode.bulk_ctl.
 						transfer_buffer[i],
 						urb->transfer_dma);
 			}
@@ -943,10 +944,12 @@ void cx231xx_uninit_bulk(struct cx231xx *dev)
 
 	kfree(dev->video_mode.bulk_ctl.urb);
 	kfree(dev->video_mode.bulk_ctl.transfer_buffer);
+	kfree(dma_q->p_left_data);
 
 	dev->video_mode.bulk_ctl.urb = NULL;
 	dev->video_mode.bulk_ctl.transfer_buffer = NULL;
 	dev->video_mode.bulk_ctl.num_bufs = 0;
+	dma_q->p_left_data = NULL;
 
 	if (dev->mode_tv == 0)
 		cx231xx_capture_start(dev, 0, Raw_Video);
@@ -1196,6 +1199,16 @@ int cx231xx_init_bulk(struct cx231xx *dev, int max_packets,
 				  sb_size, cx231xx_bulk_irq_callback, dma_q);
 	}
 
+	/* clear halt */
+	rc = usb_clear_halt(dev->udev, dev->video_mode.bulk_ctl.urb[0]->pipe);
+	if (rc < 0) {
+		dev_err(dev->dev,
+			"failed to clear USB bulk endpoint stall/halt condition (error=%i)\n",
+			rc);
+		cx231xx_uninit_bulk(dev);
+		return rc;
+	}
+
 	init_waitqueue_head(&dma_q->wq);
 
 	/* submit urbs and enables IRQ */
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 66ee161fc7ba..b8d5b2be9293 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -551,10 +551,14 @@ static int register_dvb(struct cx231xx_dvb *dvb,
 
 	/* register network adapter */
 	dvb_net_init(&dvb->adapter, &dvb->net, &dvb->demux.dmx);
-	dvb_create_media_graph(&dvb->adapter);
+	result = dvb_create_media_graph(&dvb->adapter, false);
+	if (result < 0)
+		goto fail_create_graph;
 
 	return 0;
 
+fail_create_graph:
+	dvb_net_release(&dvb->net);
 fail_fe_conn:
 	dvb->demux.dmx.remove_frontend(&dvb->demux.dmx, &dvb->fe_mem);
 fail_fe_mem:
@@ -725,7 +729,7 @@ static int dvb_init(struct cx231xx *dev)
 
 		dev->dvb->frontend = dvb_attach(lgdt3305_attach,
 						&hcw_lgdt3305_config,
-						tuner_i2c);
+						demod_i2c);
 
 		if (dev->dvb->frontend == NULL) {
 			dev_err(dev->dev,
@@ -746,7 +750,7 @@ static int dvb_init(struct cx231xx *dev)
 
 		dev->dvb->frontend = dvb_attach(si2165_attach,
 			&hauppauge_930C_HD_1113xx_si2165_config,
-			tuner_i2c
+			demod_i2c
 			);
 
 		if (dev->dvb->frontend == NULL) {
@@ -779,7 +783,7 @@ static int dvb_init(struct cx231xx *dev)
 
 		dev->dvb->frontend = dvb_attach(si2165_attach,
 			&pctv_quatro_stick_1114xx_si2165_config,
-			tuner_i2c
+			demod_i2c
 			);
 
 		if (dev->dvb->frontend == NULL) {
@@ -835,7 +839,7 @@ static int dvb_init(struct cx231xx *dev)
 
 		dev->dvb->frontend = dvb_attach(lgdt3306a_attach,
 			&hauppauge_955q_lgdt3306a_config,
-			tuner_i2c
+			demod_i2c
 			);
 
 		if (dev->dvb->frontend == NULL) {
diff --git a/drivers/media/usb/cx231xx/cx231xx-vbi.c b/drivers/media/usb/cx231xx/cx231xx-vbi.c
index a08014d20a5c..15bb573b78ac 100644
--- a/drivers/media/usb/cx231xx/cx231xx-vbi.c
+++ b/drivers/media/usb/cx231xx/cx231xx-vbi.c
@@ -32,7 +32,7 @@
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <media/tuner.h>
 
 #include "cx231xx-vbi.h"
diff --git a/drivers/media/usb/cx231xx/cx231xx-video.c b/drivers/media/usb/cx231xx/cx231xx-video.c
index d0d8f08e37c8..9b88cd8127ac 100644
--- a/drivers/media/usb/cx231xx/cx231xx-video.c
+++ b/drivers/media/usb/cx231xx/cx231xx-video.c
@@ -36,7 +36,7 @@
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <media/tuner.h>
 
 #include "dvb_frontend.h"
@@ -106,7 +106,7 @@ static int cx231xx_enable_analog_tuner(struct cx231xx *dev)
 	struct media_device *mdev = dev->media_dev;
 	struct media_entity  *entity, *decoder = NULL, *source;
 	struct media_link *link, *found_link = NULL;
-	int i, ret, active_links = 0;
+	int ret, active_links = 0;
 
 	if (!mdev)
 		return 0;
@@ -119,7 +119,7 @@ static int cx231xx_enable_analog_tuner(struct cx231xx *dev)
 	 * this should be enough for the actual needs.
 	 */
 	media_device_for_each_entity(entity, mdev) {
-		if (entity->type == MEDIA_ENT_T_V4L2_SUBDEV_DECODER) {
+		if (entity->function == MEDIA_ENT_F_ATV_DECODER) {
 			decoder = entity;
 			break;
 		}
@@ -127,8 +127,7 @@ static int cx231xx_enable_analog_tuner(struct cx231xx *dev)
 	if (!decoder)
 		return 0;
 
-	for (i = 0; i < decoder->num_links; i++) {
-		link = &decoder->links[i];
+	list_for_each_entry(link, &decoder->links, list) {
 		if (link->sink->entity == decoder) {
 			found_link = link;
 			if (link->flags & MEDIA_LNK_FL_ENABLED)
@@ -141,11 +140,10 @@ static int cx231xx_enable_analog_tuner(struct cx231xx *dev)
 		return 0;
 
 	source = found_link->source->entity;
-	for (i = 0; i < source->num_links; i++) {
+	list_for_each_entry(link, &source->links, list) {
 		struct media_entity *sink;
 		int flags = 0;
 
-		link = &source->links[i];
 		sink = link->sink->entity;
 
 		if (sink == entity)
@@ -1444,6 +1442,7 @@ static int vidioc_cropcap(struct file *file, void *priv,
 {
 	struct cx231xx_fh *fh = priv;
 	struct cx231xx *dev = fh->dev;
+	bool is_50hz = dev->norm & V4L2_STD_625_50;
 
 	if (cc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 		return -EINVAL;
@@ -1453,8 +1452,8 @@ static int vidioc_cropcap(struct file *file, void *priv,
 	cc->bounds.width = dev->width;
 	cc->bounds.height = dev->height;
 	cc->defrect = cc->bounds;
-	cc->pixelaspect.numerator = 54;	/* 4:3 FIXME: remove magic numbers */
-	cc->pixelaspect.denominator = 59;
+	cc->pixelaspect.numerator = is_50hz ? 54 : 11;
+	cc->pixelaspect.denominator = is_50hz ? 59 : 10;
 
 	return 0;
 }
@@ -2176,7 +2175,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 	cx231xx_vdev_init(dev, &dev->vdev, &cx231xx_video_template, "video");
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	dev->video_pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&dev->vdev.entity, 1, &dev->video_pad, 0);
+	ret = media_entity_pads_init(&dev->vdev.entity, 1, &dev->video_pad);
 	if (ret < 0)
 		dev_err(dev->dev, "failed to initialize video media entity!\n");
 #endif
@@ -2203,7 +2202,7 @@ int cx231xx_register_analog_devices(struct cx231xx *dev)
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	dev->vbi_pad.flags = MEDIA_PAD_FL_SINK;
-	ret = media_entity_init(&dev->vbi_dev.entity, 1, &dev->vbi_pad, 0);
+	ret = media_entity_pads_init(&dev->vbi_dev.entity, 1, &dev->vbi_pad);
 	if (ret < 0)
 		dev_err(dev->dev, "failed to initialize vbi media entity!\n");
 #endif
diff --git a/drivers/media/usb/cx231xx/cx231xx.h b/drivers/media/usb/cx231xx/cx231xx.h
index 54790fbe8fdc..ec6d3f5bc36d 100644
--- a/drivers/media/usb/cx231xx/cx231xx.h
+++ b/drivers/media/usb/cx231xx/cx231xx.h
@@ -30,14 +30,14 @@
 #include <linux/mutex.h>
 #include <linux/usb.h>
 
-#include <media/cx2341x.h>
+#include <media/drv-intf/cx2341x.h>
 
 #include <media/videobuf-vmalloc.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fh.h>
 #include <media/rc-core.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include <media/videobuf-dvb.h>
 
 #include "cx231xx-reg.h"
diff --git a/drivers/media/usb/dvb-usb-v2/Kconfig b/drivers/media/usb/dvb-usb-v2/Kconfig
index 9facc92c8dea..3dc8ef004f8b 100644
--- a/drivers/media/usb/dvb-usb-v2/Kconfig
+++ b/drivers/media/usb/dvb-usb-v2/Kconfig
@@ -9,7 +9,7 @@ config DVB_USB_V2
 	  <file:Documentation/dvb/README.dvb-usb>.
 
 	  For a complete list of supported USB devices see the LinuxTV DVB Wiki:
-	  <http://www.linuxtv.org/wiki/index.php/DVB_USB>
+	  <https://linuxtv.org/wiki/index.php/DVB_USB>
 
 	  Say Y if you own a USB DVB device.
 
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 f5df9eaba04f..f0565bf3673e 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -400,17 +400,16 @@ skip_feed_stop:
 	return ret;
 }
 
-static void dvb_usbv2_media_device_register(struct dvb_usb_adapter *adap)
+static int dvb_usbv2_media_device_init(struct dvb_usb_adapter *adap)
 {
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
 	struct media_device *mdev;
 	struct dvb_usb_device *d = adap_to_d(adap);
 	struct usb_device *udev = d->udev;
-	int ret;
 
 	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 	if (!mdev)
-		return;
+		return -ENOMEM;
 
 	mdev->dev = &udev->dev;
 	strlcpy(mdev->model, d->name, sizeof(mdev->model));
@@ -420,19 +419,21 @@ static void dvb_usbv2_media_device_register(struct dvb_usb_adapter *adap)
 	mdev->hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
 	mdev->driver_version = LINUX_VERSION_CODE;
 
-	ret = media_device_register(mdev);
-	if (ret) {
-		dev_err(&d->udev->dev,
-			"Couldn't create a media device. Error: %d\n",
-			ret);
-		kfree(mdev);
-		return;
-	}
+	media_device_init(mdev);
 
 	dvb_register_media_controller(&adap->dvb_adap, mdev);
 
 	dev_info(&d->udev->dev, "media controller created\n");
+#endif
+	return 0;
+}
 
+static int dvb_usbv2_media_device_register(struct dvb_usb_adapter *adap)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	return media_device_register(adap->dvb_adap.mdev);
+#else
+	return 0;
 #endif
 }
 
@@ -444,6 +445,7 @@ static void dvb_usbv2_media_device_unregister(struct dvb_usb_adapter *adap)
 		return;
 
 	media_device_unregister(adap->dvb_adap.mdev);
+	media_device_cleanup(adap->dvb_adap.mdev);
 	kfree(adap->dvb_adap.mdev);
 	adap->dvb_adap.mdev = NULL;
 
@@ -467,7 +469,12 @@ static int dvb_usbv2_adapter_dvb_init(struct dvb_usb_adapter *adap)
 
 	adap->dvb_adap.priv = adap;
 
-	dvb_usbv2_media_device_register(adap);
+	ret = dvb_usbv2_media_device_init(adap);
+	if (ret < 0) {
+		dev_dbg(&d->udev->dev, "%s: dvb_usbv2_media_device_init() failed=%d\n",
+				__func__, ret);
+		goto err_dvb_register_mc;
+	}
 
 	if (d->props->read_mac_address) {
 		ret = d->props->read_mac_address(adap,
@@ -518,6 +525,7 @@ err_dvb_dmxdev_init:
 	dvb_dmx_release(&adap->demux);
 err_dvb_dmx_init:
 	dvb_usbv2_media_device_unregister(adap);
+err_dvb_register_mc:
 	dvb_unregister_adapter(&adap->dvb_adap);
 err_dvb_register_adapter:
 	adap->dvb_adap.priv = NULL;
@@ -534,7 +542,6 @@ static int dvb_usbv2_adapter_dvb_exit(struct dvb_usb_adapter *adap)
 		adap->demux.dmx.close(&adap->demux.dmx);
 		dvb_dmxdev_release(&adap->dmxdev);
 		dvb_dmx_release(&adap->demux);
-		dvb_usbv2_media_device_unregister(adap);
 		dvb_unregister_adapter(&adap->dvb_adap);
 	}
 
@@ -698,9 +705,13 @@ static int dvb_usbv2_adapter_frontend_init(struct dvb_usb_adapter *adap)
 		}
 	}
 
-	dvb_create_media_graph(&adap->dvb_adap);
+	ret = dvb_create_media_graph(&adap->dvb_adap, true);
+	if (ret < 0)
+		goto err_dvb_unregister_frontend;
 
-	return 0;
+	ret = dvb_usbv2_media_device_register(adap);
+
+	return ret;
 
 err_dvb_unregister_frontend:
 	for (i = count_registered - 1; i >= 0; i--)
@@ -840,6 +851,7 @@ static int dvb_usbv2_adapter_exit(struct dvb_usb_device *d)
 			dvb_usbv2_adapter_dvb_exit(&d->adapter[i]);
 			dvb_usbv2_adapter_stream_exit(&d->adapter[i]);
 			dvb_usbv2_adapter_frontend_exit(&d->adapter[i]);
+			dvb_usbv2_media_device_unregister(&d->adapter[i]);
 		}
 	}
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
index ea3753653368..84f6de6fa07d 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.c
@@ -35,7 +35,7 @@ MODULE_PARM_DESC(debug, "set debugging level (1=info (or-able)).");
 struct mxl111sf_demod_state {
 	struct mxl111sf_state *mxl_state;
 
-	struct mxl111sf_demod_config *cfg;
+	const struct mxl111sf_demod_config *cfg;
 
 	struct dvb_frontend fe;
 };
@@ -579,7 +579,7 @@ static struct dvb_frontend_ops mxl111sf_demod_ops = {
 };
 
 struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
-					   struct mxl111sf_demod_config *cfg)
+				   const struct mxl111sf_demod_config *cfg)
 {
 	struct mxl111sf_demod_state *state = NULL;
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
index 0bd83e52669c..7065aca81252 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf-demod.h
@@ -35,11 +35,11 @@ struct mxl111sf_demod_config {
 #if IS_ENABLED(CONFIG_DVB_USB_MXL111SF)
 extern
 struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
-					   struct mxl111sf_demod_config *cfg);
+				   const struct mxl111sf_demod_config *cfg);
 #else
 static inline
 struct dvb_frontend *mxl111sf_demod_attach(struct mxl111sf_state *mxl_state,
-					   struct mxl111sf_demod_config *cfg)
+				   const struct mxl111sf_demod_config *cfg)
 {
 	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
 	return NULL;
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index bec12b0e076b..b669deccc34c 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -10,6 +10,7 @@
 
 #include <linux/vmalloc.h>
 #include <linux/i2c.h>
+#include <media/tuner.h>
 
 #include "mxl111sf.h"
 #include "mxl111sf-reg.h"
@@ -288,9 +289,9 @@ static int mxl111sf_adap_fe_init(struct dvb_frontend *fe)
 	err = mxl1x1sf_set_device_mode(state, adap_state->device_mode);
 
 	mxl_fail(err);
-	mxl111sf_enable_usb_output(state);
+	err = mxl111sf_enable_usb_output(state);
 	mxl_fail(err);
-	mxl1x1sf_top_master_ctrl(state, 1);
+	err = mxl1x1sf_top_master_ctrl(state, 1);
 	mxl_fail(err);
 
 	if ((MXL111SF_GPIO_MOD_DVBT != adap_state->gpio_mode) &&
@@ -731,7 +732,7 @@ fail:
 	return ret;
 }
 
-static struct mxl111sf_demod_config mxl_demod_config = {
+static const struct mxl111sf_demod_config mxl_demod_config = {
 	.read_reg        = mxl111sf_read_reg,
 	.write_reg       = mxl111sf_write_reg,
 	.program_regs    = mxl111sf_ctrl_program_regs,
@@ -868,6 +869,10 @@ static struct mxl111sf_tuner_config mxl_tuner_config = {
 static int mxl111sf_attach_tuner(struct dvb_usb_adapter *adap)
 {
 	struct mxl111sf_state *state = adap_to_priv(adap);
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	struct media_device *mdev = dvb_get_media_controller(&adap->dvb_adap);
+	int ret;
+#endif
 	int i;
 
 	pr_debug("%s()\n", __func__);
@@ -879,6 +884,21 @@ static int mxl111sf_attach_tuner(struct dvb_usb_adapter *adap)
 		adap->fe[i]->ops.read_signal_strength = adap->fe[i]->ops.tuner_ops.get_rf_strength;
 	}
 
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	state->tuner.function = MEDIA_ENT_F_TUNER;
+	state->tuner.name = "mxl111sf tuner";
+	state->tuner_pads[TUNER_PAD_RF_INPUT].flags = MEDIA_PAD_FL_SINK;
+	state->tuner_pads[TUNER_PAD_IF_OUTPUT].flags = MEDIA_PAD_FL_SOURCE;
+
+	ret = media_entity_pads_init(&state->tuner,
+				     TUNER_NUM_PADS, state->tuner_pads);
+	if (ret)
+		return ret;
+
+	ret = media_device_register_entity(mdev, &state->tuner);
+	if (ret)
+		return ret;
+#endif
 	return 0;
 }
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.h b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
index ee70df1f1e94..846260e0eec0 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.h
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.h
@@ -17,6 +17,7 @@
 #define DVB_USB_LOG_PREFIX "mxl111sf"
 #include "dvb_usb.h"
 #include <media/tveeprom.h>
+#include <media/media-entity.h>
 
 #define MXL_EP1_REG_READ     1
 #define MXL_EP2_REG_WRITE    2
@@ -85,6 +86,10 @@ struct mxl111sf_state {
 	struct mutex fe_lock;
 	u8 num_frontends;
 	struct mxl111sf_adap_state adap_state[3];
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	struct media_entity tuner;
+	struct media_pad tuner_pads[2];
+#endif
 };
 
 int mxl111sf_read_reg(struct mxl111sf_state *state, u8 addr, u8 *data);
diff --git a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
index 5a503a6bb8c5..eb5787a3191e 100644
--- a/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
+++ b/drivers/media/usb/dvb-usb-v2/rtl28xxu.c
@@ -181,11 +181,17 @@ static int rtl28xxu_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 			goto err_mutex_unlock;
 		} else if (msg[0].addr == 0x10) {
 			/* method 1 - integrated demod */
-			req.value = (msg[0].buf[0] << 8) | (msg[0].addr << 1);
-			req.index = CMD_DEMOD_RD | dev->page;
-			req.size = msg[1].len;
-			req.data = &msg[1].buf[0];
-			ret = rtl28xxu_ctrl_msg(d, &req);
+			if (msg[0].buf[0] == 0x00) {
+				/* return demod page from driver cache */
+				msg[1].buf[0] = dev->page;
+				ret = 0;
+			} else {
+				req.value = (msg[0].buf[0] << 8) | (msg[0].addr << 1);
+				req.index = CMD_DEMOD_RD | dev->page;
+				req.size = msg[1].len;
+				req.data = &msg[1].buf[0];
+				ret = rtl28xxu_ctrl_msg(d, &req);
+			}
 		} else if (msg[0].len < 2) {
 			/* method 2 - old I2C */
 			req.value = (msg[0].buf[0] << 8) | (msg[0].addr << 1);
diff --git a/drivers/media/usb/dvb-usb/Kconfig b/drivers/media/usb/dvb-usb/Kconfig
index 128eee61570d..f03b0b70c901 100644
--- a/drivers/media/usb/dvb-usb/Kconfig
+++ b/drivers/media/usb/dvb-usb/Kconfig
@@ -9,7 +9,7 @@ config DVB_USB
 	  <file:Documentation/dvb/README.dvb-usb>.
 
 	  For a complete list of supported USB devices see the LinuxTV DVB Wiki:
-	  <http://www.linuxtv.org/wiki/index.php/DVB_USB>
+	  <https://linuxtv.org/wiki/index.php/DVB_USB>
 
 	  Say Y if you own a USB DVB device.
 
diff --git a/drivers/media/usb/dvb-usb/dvb-usb-dvb.c b/drivers/media/usb/dvb-usb/dvb-usb-dvb.c
index 8a260c854653..9ddfcab268be 100644
--- a/drivers/media/usb/dvb-usb/dvb-usb-dvb.c
+++ b/drivers/media/usb/dvb-usb/dvb-usb-dvb.c
@@ -95,17 +95,16 @@ static int dvb_usb_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	return dvb_usb_ctrl_feed(dvbdmxfeed, 0);
 }
 
-static void dvb_usb_media_device_register(struct dvb_usb_adapter *adap)
+static int dvb_usb_media_device_init(struct dvb_usb_adapter *adap)
 {
 #ifdef CONFIG_MEDIA_CONTROLLER_DVB
 	struct media_device *mdev;
 	struct dvb_usb_device *d = adap->dev;
 	struct usb_device *udev = d->udev;
-	int ret;
 
 	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 	if (!mdev)
-		return;
+		return -ENOMEM;
 
 	mdev->dev = &udev->dev;
 	strlcpy(mdev->model, d->desc->name, sizeof(mdev->model));
@@ -115,18 +114,22 @@ static void dvb_usb_media_device_register(struct dvb_usb_adapter *adap)
 	mdev->hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
 	mdev->driver_version = LINUX_VERSION_CODE;
 
-	ret = media_device_register(mdev);
-	if (ret) {
-		dev_err(&d->udev->dev,
-			"Couldn't create a media device. Error: %d\n",
-			ret);
-		kfree(mdev);
-		return;
-	}
+	media_device_init(mdev);
+
 	dvb_register_media_controller(&adap->dvb_adap, mdev);
 
 	dev_info(&d->udev->dev, "media controller created\n");
 #endif
+	return 0;
+}
+
+static int  dvb_usb_media_device_register(struct dvb_usb_adapter *adap)
+{
+#ifdef CONFIG_MEDIA_CONTROLLER_DVB
+	return media_device_register(adap->dvb_adap.mdev);
+#else
+	return 0;
+#endif
 }
 
 static void dvb_usb_media_device_unregister(struct dvb_usb_adapter *adap)
@@ -136,6 +139,7 @@ static void dvb_usb_media_device_unregister(struct dvb_usb_adapter *adap)
 		return;
 
 	media_device_unregister(adap->dvb_adap.mdev);
+	media_device_cleanup(adap->dvb_adap.mdev);
 	kfree(adap->dvb_adap.mdev);
 	adap->dvb_adap.mdev = NULL;
 #endif
@@ -154,7 +158,11 @@ int dvb_usb_adapter_dvb_init(struct dvb_usb_adapter *adap, short *adapter_nums)
 	}
 	adap->dvb_adap.priv = adap;
 
-	dvb_usb_media_device_register(adap);
+	ret = dvb_usb_media_device_init(adap);
+	if (ret < 0) {
+		deb_info("dvb_usb_media_device_init failed: error %d", ret);
+		goto err_mc;
+	}
 
 	if (adap->dev->props.read_mac_address) {
 		if (adap->dev->props.read_mac_address(adap->dev, adap->dvb_adap.proposed_mac) == 0)
@@ -204,6 +212,7 @@ err_dmx_dev:
 	dvb_dmx_release(&adap->demux);
 err_dmx:
 	dvb_usb_media_device_unregister(adap);
+err_mc:
 	dvb_unregister_adapter(&adap->dvb_adap);
 err:
 	return ret;
@@ -318,10 +327,16 @@ int dvb_usb_adapter_frontend_init(struct dvb_usb_adapter *adap)
 
 		adap->num_frontends_initialized++;
 	}
+	if (ret)
+		return ret;
 
-	dvb_create_media_graph(&adap->dvb_adap);
+	ret = dvb_create_media_graph(&adap->dvb_adap, true);
+	if (ret)
+		return ret;
 
-	return 0;
+	ret = dvb_usb_media_device_register(adap);
+
+	return ret;
 }
 
 int dvb_usb_adapter_frontend_exit(struct dvb_usb_adapter *adap)
diff --git a/drivers/media/usb/em28xx/em28xx-camera.c b/drivers/media/usb/em28xx/em28xx-camera.c
index ed0b3a87983e..b58acd3fcd99 100644
--- a/drivers/media/usb/em28xx/em28xx-camera.c
+++ b/drivers/media/usb/em28xx/em28xx-camera.c
@@ -21,7 +21,7 @@
 
 #include <linux/i2c.h>
 #include <media/soc_camera.h>
-#include <media/mt9v011.h>
+#include <media/i2c/mt9v011.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
 
@@ -322,7 +322,7 @@ int em28xx_detect_sensor(struct em28xx *dev)
 
 int em28xx_init_camera(struct em28xx *dev)
 {
-	char clk_name[V4L2_SUBDEV_NAME_SIZE];
+	char clk_name[V4L2_CLK_NAME_SIZE];
 	struct i2c_client *client = &dev->i2c_client[dev->def_i2c_bus];
 	struct i2c_adapter *adap = &dev->i2c_adap[dev->def_i2c_bus];
 	struct em28xx_v4l2 *v4l2 = dev->v4l2;
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index 394004607059..a1b6ef5894a6 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -30,10 +30,10 @@
 #include <linux/i2c.h>
 #include <linux/usb.h>
 #include <media/tuner.h>
-#include <media/msp3400.h>
-#include <media/saa7115.h>
-#include <media/tvp5150.h>
-#include <media/tvaudio.h>
+#include <media/drv-intf/msp3400.h>
+#include <media/i2c/saa7115.h>
+#include <media/i2c/tvp5150.h>
+#include <media/i2c/tvaudio.h>
 #include <media/i2c-addr.h>
 #include <media/tveeprom.h>
 #include <media/v4l2-common.h>
@@ -1051,8 +1051,12 @@ struct em28xx_board em28xx_boards[] = {
 	},
 	[EM2870_BOARD_TERRATEC_XS_MT2060] = {
 		.name         = "Terratec Cinergy T XS (MT2060)",
-		.valid        = EM28XX_BOARD_NOT_VALIDATED,
+		.xclk         = EM28XX_XCLK_IR_RC5_MODE |
+				EM28XX_XCLK_FREQUENCY_12MHZ,
+		.i2c_speed    = EM28XX_I2C_CLK_WAIT_ENABLE,
 		.tuner_type   = TUNER_ABSENT, /* MT2060 */
+		.has_dvb      = 1,
+		.tuner_gpio   = default_tuner_gpio,
 	},
 	[EM2870_BOARD_KWORLD_350U] = {
 		.name         = "Kworld 350 U DVB-T",
@@ -2368,7 +2372,7 @@ struct usb_device_id em28xx_id_table[] = {
 	{ USB_DEVICE(0x0ccd, 0x0042),
 			.driver_info = EM2882_BOARD_TERRATEC_HYBRID_XS },
 	{ USB_DEVICE(0x0ccd, 0x0043),
-			.driver_info = EM2870_BOARD_TERRATEC_XS },
+			.driver_info = EM2870_BOARD_TERRATEC_XS_MT2060 },
 	{ USB_DEVICE(0x0ccd, 0x008e),	/* Cinergy HTC USB XS Rev. 1 */
 			.driver_info = EM2884_BOARD_TERRATEC_HTC_USB_XS },
 	{ USB_DEVICE(0x0ccd, 0x00ac),	/* Cinergy HTC USB XS Rev. 2 */
@@ -2471,6 +2475,8 @@ struct usb_device_id em28xx_id_table[] = {
 			.driver_info = EM28178_BOARD_PCTV_461E },
 	{ USB_DEVICE(0x2013, 0x025f),
 			.driver_info = EM28178_BOARD_PCTV_292E },
+	{ USB_DEVICE(0x2040, 0x0264), /* Hauppauge WinTV-soloHD */
+			.driver_info = EM28178_BOARD_PCTV_292E },
 	{ USB_DEVICE(0x0413, 0x6f07),
 			.driver_info = EM2861_BOARD_LEADTEK_VC100 },
 	{ USB_DEVICE(0xeb1a, 0x8179),
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 357be76c7a55..bf5c24467c65 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -38,6 +38,7 @@
 #include "lgdt3305.h"
 #include "zl10353.h"
 #include "s5h1409.h"
+#include "mt2060.h"
 #include "mt352.h"
 #include "mt352_priv.h" /* FIXME */
 #include "tda1002x.h"
@@ -815,6 +816,10 @@ static struct zl10353_config em28xx_zl10353_no_i2c_gate_dev = {
 	.parallel_ts = 1,
 };
 
+static struct mt2060_config em28xx_mt2060_config = {
+	.i2c_address = 0x60,
+};
+
 static struct qt1010_config em28xx_qt1010_config = {
 	.i2c_address = 0x62
 };
@@ -1142,6 +1147,16 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			goto out_free;
 		}
 		break;
+	case EM2870_BOARD_TERRATEC_XS_MT2060:
+		dvb->fe[0] = dvb_attach(zl10353_attach,
+						&em28xx_zl10353_no_i2c_gate_dev,
+						&dev->i2c_adap[dev->def_i2c_bus]);
+		if (dvb->fe[0] != NULL) {
+			dvb_attach(mt2060_attach, dvb->fe[0],
+					&dev->i2c_adap[dev->def_i2c_bus],
+					&em28xx_mt2060_config, 1220);
+		}
+		break;
 	case EM2870_BOARD_KWORLD_355U:
 		dvb->fe[0] = dvb_attach(zl10353_attach,
 					   &em28xx_zl10353_no_i2c_gate_dev,
diff --git a/drivers/media/usb/em28xx/em28xx-vbi.c b/drivers/media/usb/em28xx/em28xx-vbi.c
index e23c285b3108..fe94c9225dd7 100644
--- a/drivers/media/usb/em28xx/em28xx-vbi.c
+++ b/drivers/media/usb/em28xx/em28xx-vbi.c
@@ -31,26 +31,22 @@
 
 /* ------------------------------------------------------------------ */
 
-static int vbi_queue_setup(struct vb2_queue *vq, const void *parg,
+static int vbi_queue_setup(struct vb2_queue *vq,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct em28xx *dev = vb2_get_drv_priv(vq);
 	struct em28xx_v4l2 *v4l2 = dev->v4l2;
-	unsigned long size;
+	unsigned long size = v4l2->vbi_width * v4l2->vbi_height * 2;
 
-	if (fmt)
-		size = fmt->fmt.pix.sizeimage;
-	else
-		size = v4l2->vbi_width * v4l2->vbi_height * 2;
-
-	if (0 == *nbuffers)
-		*nbuffers = 32;
 	if (*nbuffers < 2)
 		*nbuffers = 2;
-	if (*nbuffers > 32)
-		*nbuffers = 32;
+
+	if (*nplanes) {
+		if (sizes[0] < size)
+			return -EINVAL;
+		size = sizes[0];
+	}
 
 	*nplanes = 1;
 	sizes[0] = size;
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 6a3cf342e087..0e86ff423c49 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -43,7 +43,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
 #include <media/v4l2-clk.h>
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <media/tuner.h>
 
 #define DRIVER_AUTHOR "Ludovico Cavedon <cavedon@sssup.it>, " \
@@ -438,7 +438,7 @@ static inline void finish_buffer(struct em28xx *dev,
 		buf->vb.field = V4L2_FIELD_NONE;
 	else
 		buf->vb.field = V4L2_FIELD_INTERLACED;
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 
 	vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 }
@@ -871,30 +871,19 @@ static void res_free(struct em28xx *dev, enum v4l2_buf_type f_type)
 	Videobuf2 operations
    ------------------------------------------------------------------*/
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct em28xx *dev = vb2_get_drv_priv(vq);
 	struct em28xx_v4l2 *v4l2 = dev->v4l2;
-	unsigned long size;
-
-	if (fmt)
-		size = fmt->fmt.pix.sizeimage;
-	else
-		size =
+	unsigned long size =
 		    (v4l2->width * v4l2->height * v4l2->format->depth + 7) >> 3;
 
-	if (size == 0)
-		return -EINVAL;
-
-	if (0 == *nbuffers)
-		*nbuffers = 32;
-
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
 	sizes[0] = size;
-
 	return 0;
 }
 
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index 76bf8ba372b3..8ff066c977d9 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -40,7 +40,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-fh.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include <media/rc-core.h>
 #include "tuner-xc2028.h"
 #include "xc5000.h"
diff --git a/drivers/media/usb/go7007/go7007-driver.c b/drivers/media/usb/go7007/go7007-driver.c
index ae1cfa792c58..05b1126f263e 100644
--- a/drivers/media/usb/go7007/go7007-driver.c
+++ b/drivers/media/usb/go7007/go7007-driver.c
@@ -466,7 +466,7 @@ static struct go7007_buffer *frame_boundary(struct go7007 *go, struct go7007_buf
 	else
 		go7007_set_motion_regions(go, vb, 0);
 
-	v4l2_get_timestamp(&vb->vb.timestamp);
+	vb->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb_tmp = vb;
 	spin_lock(&go->spinlock);
 	list_del(&vb->list);
diff --git a/drivers/media/usb/go7007/go7007-usb.c b/drivers/media/usb/go7007/go7007-usb.c
index 4857c467e76c..3dbf14c85c5c 100644
--- a/drivers/media/usb/go7007/go7007-usb.c
+++ b/drivers/media/usb/go7007/go7007-usb.c
@@ -23,9 +23,9 @@
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <asm/byteorder.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <media/tuner.h>
-#include <media/uda1342.h>
+#include <media/i2c/uda1342.h>
 
 #include "go7007-priv.h"
 
@@ -1289,7 +1289,7 @@ static int go7007_usb_probe(struct usb_interface *intf,
 
 	/* Allocate the URBs and buffers for receiving the audio stream */
 	if ((board->flags & GO7007_USB_EZUSB) &&
-	    (board->flags & GO7007_BOARD_HAS_AUDIO)) {
+	    (board->main_info.flags & GO7007_BOARD_HAS_AUDIO)) {
 		for (i = 0; i < 8; ++i) {
 			usb->audio_urbs[i] = usb_alloc_urb(0, GFP_KERNEL);
 			if (usb->audio_urbs[i] == NULL)
diff --git a/drivers/media/usb/go7007/go7007-v4l2.c b/drivers/media/usb/go7007/go7007-v4l2.c
index f3d187db9368..358c1c186d03 100644
--- a/drivers/media/usb/go7007/go7007-v4l2.c
+++ b/drivers/media/usb/go7007/go7007-v4l2.c
@@ -30,7 +30,7 @@
 #include <media/v4l2-subdev.h>
 #include <media/v4l2-event.h>
 #include <media/videobuf2-vmalloc.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 
 #include "go7007-priv.h"
 
@@ -369,7 +369,6 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 }
 
 static int go7007_queue_setup(struct vb2_queue *q,
-		const void *parg,
 		unsigned int *num_buffers, unsigned int *num_planes,
 		unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/usb/gspca/ov534.c b/drivers/media/usb/gspca/ov534.c
index 146071b8e116..bfff1d1c70ab 100644
--- a/drivers/media/usb/gspca/ov534.c
+++ b/drivers/media/usb/gspca/ov534.c
@@ -1491,8 +1491,13 @@ static void sd_set_streamparm(struct gspca_dev *gspca_dev,
 	struct v4l2_fract *tpf = &cp->timeperframe;
 	struct sd *sd = (struct sd *) gspca_dev;
 
-	/* Set requested framerate */
-	sd->frame_rate = tpf->denominator / tpf->numerator;
+	if (tpf->numerator == 0 || tpf->denominator == 0)
+		/* Set default framerate */
+		sd->frame_rate = 30;
+	else
+		/* Set requested framerate */
+		sd->frame_rate = tpf->denominator / tpf->numerator;
+
 	if (gspca_dev->streaming)
 		set_frame_rate(gspca_dev);
 
diff --git a/drivers/media/usb/gspca/topro.c b/drivers/media/usb/gspca/topro.c
index c70ff406b07a..c028a5c2438e 100644
--- a/drivers/media/usb/gspca/topro.c
+++ b/drivers/media/usb/gspca/topro.c
@@ -4802,7 +4802,11 @@ static void sd_set_streamparm(struct gspca_dev *gspca_dev,
 	struct v4l2_fract *tpf = &cp->timeperframe;
 	int fr, i;
 
-	sd->framerate = tpf->denominator / tpf->numerator;
+	if (tpf->numerator == 0 || tpf->denominator == 0)
+		sd->framerate = 30;
+	else
+		sd->framerate = tpf->denominator / tpf->numerator;
+
 	if (gspca_dev->streaming)
 		setframerate(gspca_dev, v4l2_ctrl_g_ctrl(gspca_dev->exposure));
 
diff --git a/drivers/media/usb/hackrf/hackrf.c b/drivers/media/usb/hackrf/hackrf.c
index 0fe5cb2c260c..9e700caf0d66 100644
--- a/drivers/media/usb/hackrf/hackrf.c
+++ b/drivers/media/usb/hackrf/hackrf.c
@@ -526,7 +526,7 @@ static void hackrf_urb_complete_in(struct urb *urb)
 		    urb->transfer_buffer, len);
 	vb2_set_plane_payload(&buffer->vb.vb2_buf, 0, len);
 	buffer->vb.sequence = dev->sequence++;
-	v4l2_get_timestamp(&buffer->vb.timestamp);
+	buffer->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&buffer->vb.vb2_buf, VB2_BUF_STATE_DONE);
 exit_usb_submit_urb:
 	usb_submit_urb(urb, GFP_ATOMIC);
@@ -571,7 +571,7 @@ static void hackrf_urb_complete_out(struct urb *urb)
 			   vb2_plane_vaddr(&buffer->vb.vb2_buf, 0), len);
 	urb->actual_length = len;
 	buffer->vb.sequence = dev->sequence++;
-	v4l2_get_timestamp(&buffer->vb.timestamp);
+	buffer->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&buffer->vb.vb2_buf, VB2_BUF_STATE_DONE);
 exit_usb_submit_urb:
 	usb_submit_urb(urb, GFP_ATOMIC);
@@ -759,7 +759,7 @@ static void hackrf_return_all_buffers(struct vb2_queue *vq,
 }
 
 static int hackrf_queue_setup(struct vb2_queue *vq,
-		const void *parg, unsigned int *nbuffers,
+		unsigned int *nbuffers,
 		unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
 {
 	struct hackrf_dev *dev = vb2_get_drv_priv(vq);
diff --git a/drivers/media/usb/hdpvr/hdpvr-video.c b/drivers/media/usb/hdpvr/hdpvr-video.c
index d8d8c0f519fc..7dee22deebf3 100644
--- a/drivers/media/usb/hdpvr/hdpvr-video.c
+++ b/drivers/media/usb/hdpvr/hdpvr-video.c
@@ -642,7 +642,7 @@ static int vidioc_s_dv_timings(struct file *file, void *_fh,
 	if (dev->status != STATUS_IDLE)
 		return -EBUSY;
 	for (i = 0; i < ARRAY_SIZE(hdpvr_dv_timings); i++)
-		if (v4l2_match_dv_timings(timings, hdpvr_dv_timings + i, 0))
+		if (v4l2_match_dv_timings(timings, hdpvr_dv_timings + i, 0, false))
 			break;
 	if (i == ARRAY_SIZE(hdpvr_dv_timings))
 		return -EINVAL;
diff --git a/drivers/media/usb/hdpvr/hdpvr.h b/drivers/media/usb/hdpvr/hdpvr.h
index a3194304182d..78e815441f95 100644
--- a/drivers/media/usb/hdpvr/hdpvr.h
+++ b/drivers/media/usb/hdpvr/hdpvr.h
@@ -17,7 +17,7 @@
 
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 
 #define HDPVR_MAX 8
 #define HDPVR_I2C_MAX_SIZE 128
diff --git a/drivers/media/usb/msi2500/msi2500.c b/drivers/media/usb/msi2500/msi2500.c
index e06a21a4fbd9..c104315fdc17 100644
--- a/drivers/media/usb/msi2500/msi2500.c
+++ b/drivers/media/usb/msi2500/msi2500.c
@@ -616,7 +616,6 @@ static int msi2500_querycap(struct file *file, void *fh,
 
 /* Videobuf2 operations */
 static int msi2500_queue_setup(struct vb2_queue *vq,
-			       const void *parg,
 			       unsigned int *nbuffers,
 			       unsigned int *nplanes, unsigned int sizes[],
 			       void *alloc_ctxs[])
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-audio.c b/drivers/media/usb/pvrusb2/pvrusb2-audio.c
index 45276c628482..5f953d837bf1 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-audio.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-audio.c
@@ -23,7 +23,7 @@
 #include "pvrusb2-hdw-internal.h"
 #include "pvrusb2-debug.h"
 #include <linux/videodev2.h>
-#include <media/msp3400.h>
+#include <media/drv-intf/msp3400.h>
 #include <media/v4l2-common.h>
 
 
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-cx2584x-v4l.c b/drivers/media/usb/pvrusb2/pvrusb2-cx2584x-v4l.c
index 1a81aa70509b..7d675fae1846 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-cx2584x-v4l.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-cx2584x-v4l.c
@@ -32,7 +32,7 @@
 
 #include "pvrusb2-hdw-internal.h"
 #include "pvrusb2-debug.h"
-#include <media/cx25840.h>
+#include <media/drv-intf/cx25840.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <linux/errno.h>
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h b/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
index 1f9c02801cee..60141b16d731 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
+++ b/drivers/media/usb/pvrusb2/pvrusb2-hdw-internal.h
@@ -39,8 +39,8 @@
 #include "pvrusb2-hdw.h"
 #include "pvrusb2-io.h"
 #include <media/v4l2-device.h>
-#include <media/cx2341x.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/drv-intf/cx2341x.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include "pvrusb2-devattr.h"
 
 /* Legal values for PVR2_CID_HSM */
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
index 4baa9d632a4e..14321d0a1833 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-i2c-core.c
@@ -20,7 +20,7 @@
 
 #include <linux/i2c.h>
 #include <linux/module.h>
-#include <media/ir-kbd-i2c.h>
+#include <media/i2c/ir-kbd-i2c.h>
 #include "pvrusb2-i2c-core.h"
 #include "pvrusb2-hdw-internal.h"
 #include "pvrusb2-debug.h"
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
index 1c5f85bf7ed4..81f788b7b242 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-v4l2.c
@@ -628,6 +628,7 @@ static int pvr2_g_ext_ctrls(struct file *file, void *priv,
 	struct pvr2_v4l2_fh *fh = file->private_data;
 	struct pvr2_hdw *hdw = fh->channel.mc_head->hdw;
 	struct v4l2_ext_control *ctrl;
+	struct pvr2_ctrl *cptr;
 	unsigned int idx;
 	int val;
 	int ret;
@@ -635,8 +636,15 @@ static int pvr2_g_ext_ctrls(struct file *file, void *priv,
 	ret = 0;
 	for (idx = 0; idx < ctls->count; idx++) {
 		ctrl = ctls->controls + idx;
-		ret = pvr2_ctrl_get_value(
-				pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id), &val);
+		cptr = pvr2_hdw_get_ctrl_v4l(hdw, ctrl->id);
+		if (cptr) {
+			if (ctls->which == V4L2_CTRL_WHICH_DEF_VAL)
+				pvr2_ctrl_get_def(cptr, &val);
+			else
+				ret = pvr2_ctrl_get_value(cptr, &val);
+		} else
+			ret = -EINVAL;
+
 		if (ret) {
 			ctls->error_idx = idx;
 			return ret;
@@ -658,6 +666,10 @@ static int pvr2_s_ext_ctrls(struct file *file, void *priv,
 	unsigned int idx;
 	int ret;
 
+	/* Default value cannot be changed */
+	if (ctls->which == V4L2_CTRL_WHICH_DEF_VAL)
+		return -EINVAL;
+
 	ret = 0;
 	for (idx = 0; idx < ctls->count; idx++) {
 		ctrl = ctls->controls + idx;
diff --git a/drivers/media/usb/pvrusb2/pvrusb2-video-v4l.c b/drivers/media/usb/pvrusb2/pvrusb2-video-v4l.c
index 139b39740534..105123ab36aa 100644
--- a/drivers/media/usb/pvrusb2/pvrusb2-video-v4l.c
+++ b/drivers/media/usb/pvrusb2/pvrusb2-video-v4l.c
@@ -35,7 +35,7 @@
 #include "pvrusb2-debug.h"
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <linux/errno.h>
 
 struct routing_scheme {
diff --git a/drivers/media/usb/pwc/pwc-if.c b/drivers/media/usb/pwc/pwc-if.c
index b79c36fd8cd2..086cf1c7bd7d 100644
--- a/drivers/media/usb/pwc/pwc-if.c
+++ b/drivers/media/usb/pwc/pwc-if.c
@@ -316,8 +316,7 @@ static void pwc_isoc_handler(struct urb *urb)
 			struct pwc_frame_buf *fbuf = pdev->fill_buf;
 
 			if (pdev->vsync == 1) {
-				v4l2_get_timestamp(
-					&fbuf->vb.timestamp);
+				fbuf->vb.vb2_buf.timestamp = ktime_get_ns();
 				pdev->vsync = 2;
 			}
 
@@ -571,7 +570,7 @@ static void pwc_video_release(struct v4l2_device *v)
 /***************************************************************************/
 /* Videobuf2 operations */
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index e7acb12ad21d..9acdaa3716fb 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -574,7 +574,7 @@ static void s2255_got_frame(struct s2255_vc *vc, int jpgsize)
 	buf = list_entry(vc->buf_list.next,
 			 struct s2255_buffer, list);
 	list_del(&buf->list);
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 	buf->vb.field = vc->field;
 	buf->vb.sequence = vc->frame_count;
 	spin_unlock_irqrestore(&vc->qlock, flags);
@@ -660,7 +660,7 @@ static void s2255_fillbuff(struct s2255_vc *vc,
    Videobuf operations
    ------------------------------------------------------------------*/
 
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 		       unsigned int *nbuffers, unsigned int *nplanes,
 		       unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/usb/siano/smsusb.c b/drivers/media/usb/siano/smsusb.c
index c945e4c2fbd4..8abbd3cc8eba 100644
--- a/drivers/media/usb/siano/smsusb.c
+++ b/drivers/media/usb/siano/smsusb.c
@@ -361,10 +361,11 @@ static void *siano_media_device_register(struct smsusb_device_t *dev,
 	mdev->hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
 	mdev->driver_version = LINUX_VERSION_CODE;
 
+	media_device_init(mdev);
+
 	ret = media_device_register(mdev);
 	if (ret) {
-		pr_err("Couldn't create a media device. Error: %d\n",
-			ret);
+		media_device_cleanup(mdev);
 		kfree(mdev);
 		return NULL;
 	}
diff --git a/drivers/media/usb/stk1160/stk1160-core.c b/drivers/media/usb/stk1160/stk1160-core.c
index 1b6836f15370..bc029478065a 100644
--- a/drivers/media/usb/stk1160/stk1160-core.c
+++ b/drivers/media/usb/stk1160/stk1160-core.c
@@ -34,7 +34,7 @@
 #include <linux/usb.h>
 #include <linux/mm.h>
 #include <linux/vmalloc.h>
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 
 #include "stk1160.h"
 #include "stk1160-reg.h"
diff --git a/drivers/media/usb/stk1160/stk1160-v4l.c b/drivers/media/usb/stk1160/stk1160-v4l.c
index 0bd34f1e7fa9..77131fd614a5 100644
--- a/drivers/media/usb/stk1160/stk1160-v4l.c
+++ b/drivers/media/usb/stk1160/stk1160-v4l.c
@@ -33,7 +33,7 @@
 #include <media/v4l2-event.h>
 #include <media/videobuf2-vmalloc.h>
 
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 
 #include "stk1160.h"
 #include "stk1160-reg.h"
@@ -664,7 +664,7 @@ static const struct v4l2_ioctl_ops stk1160_ioctl_ops = {
 /*
  * Videobuf2 operations
  */
-static int queue_setup(struct vb2_queue *vq, const void *parg,
+static int queue_setup(struct vb2_queue *vq,
 				unsigned int *nbuffers, unsigned int *nplanes,
 				unsigned int sizes[], void *alloc_ctxs[])
 {
diff --git a/drivers/media/usb/stk1160/stk1160-video.c b/drivers/media/usb/stk1160/stk1160-video.c
index 75654e676e80..46191d5262eb 100644
--- a/drivers/media/usb/stk1160/stk1160-video.c
+++ b/drivers/media/usb/stk1160/stk1160-video.c
@@ -99,7 +99,7 @@ void stk1160_buffer_done(struct stk1160 *dev)
 	buf->vb.sequence = dev->sequence++;
 	buf->vb.field = V4L2_FIELD_INTERLACED;
 	buf->vb.vb2_buf.planes[0].bytesused = buf->bytesused;
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 
 	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->bytesused);
 	vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/media/usb/tm6000/tm6000-cards.c b/drivers/media/usb/tm6000/tm6000-cards.c
index 2e8c3afe4ec4..8902ee36bc94 100644
--- a/drivers/media/usb/tm6000/tm6000-cards.c
+++ b/drivers/media/usb/tm6000/tm6000-cards.c
@@ -26,7 +26,7 @@
 #include <linux/slab.h>
 #include <media/v4l2-common.h>
 #include <media/tuner.h>
-#include <media/tvaudio.h>
+#include <media/i2c/tvaudio.h>
 #include <media/i2c-addr.h>
 #include <media/rc-map.h>
 
diff --git a/drivers/media/usb/ttusb-dec/ttusb_dec.c b/drivers/media/usb/ttusb-dec/ttusb_dec.c
index a5de46f04247..4e36e24cb3a6 100644
--- a/drivers/media/usb/ttusb-dec/ttusb_dec.c
+++ b/drivers/media/usb/ttusb-dec/ttusb_dec.c
@@ -1606,7 +1606,7 @@ static int fe_send_command(struct dvb_frontend* fe, const u8 command,
 	return ttusb_dec_send_command(dec, command, param_length, params, result_length, cmd_result);
 }
 
-static struct ttusbdecfe_config fe_config = {
+static const struct ttusbdecfe_config fe_config = {
 	.send_command = fe_send_command
 };
 
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index e645c9df2d94..4ebb33943f9a 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -322,7 +322,7 @@ static void usbtv_image_chunk(struct usbtv *usbtv, __be32 *chunk)
 
 		buf->vb.field = V4L2_FIELD_INTERLACED;
 		buf->vb.sequence = usbtv->sequence++;
-		v4l2_get_timestamp(&buf->vb.timestamp);
+		buf->vb.vb2_buf.timestamp = ktime_get_ns();
 		vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
 		vb2_buffer_done(&buf->vb.vb2_buf, state);
 		list_del(&buf->list);
@@ -599,19 +599,18 @@ static struct v4l2_file_operations usbtv_fops = {
 };
 
 static int usbtv_queue_setup(struct vb2_queue *vq,
-	const void *parg, unsigned int *nbuffers,
+	unsigned int *nbuffers,
 	unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct usbtv *usbtv = vb2_get_drv_priv(vq);
 	unsigned size = USBTV_CHUNK * usbtv->n_chunks * 2 * sizeof(u32);
 
 	if (vq->num_buffers + *nbuffers < 2)
 		*nbuffers = 2 - vq->num_buffers;
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
-	if (fmt && fmt->fmt.pix.sizeimage < size)
-		return -EINVAL;
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage : size;
+	sizes[0] = size;
 
 	return 0;
 }
diff --git a/drivers/media/usb/usbvision/usbvision-core.c b/drivers/media/usb/usbvision/usbvision-core.c
index dc3b4d5155c5..1ea04e75fb36 100644
--- a/drivers/media/usb/usbvision/usbvision-core.c
+++ b/drivers/media/usb/usbvision/usbvision-core.c
@@ -37,7 +37,7 @@
 #include <linux/videodev2.h>
 #include <linux/i2c.h>
 
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <media/v4l2-common.h>
 #include <media/tuner.h>
 
diff --git a/drivers/media/usb/usbvision/usbvision-video.c b/drivers/media/usb/usbvision/usbvision-video.c
index b693206f66dd..de9ff3bb8edd 100644
--- a/drivers/media/usb/usbvision/usbvision-video.c
+++ b/drivers/media/usb/usbvision/usbvision-video.c
@@ -59,7 +59,7 @@
 #include <linux/videodev2.h>
 #include <linux/i2c.h>
 
-#include <media/saa7115.h>
+#include <media/i2c/saa7115.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
@@ -1461,11 +1461,32 @@ static int usbvision_probe(struct usb_interface *intf,
 	printk(KERN_INFO "%s: %s found\n", __func__,
 				usbvision_device_data[model].model_string);
 
+	/*
+	 * this is a security check.
+	 * an exploit using an incorrect bInterfaceNumber is known
+	 */
+	if (ifnum >= USB_MAXINTERFACES || !dev->actconfig->interface[ifnum])
+		return -ENODEV;
+
 	if (usbvision_device_data[model].interface >= 0)
 		interface = &dev->actconfig->interface[usbvision_device_data[model].interface]->altsetting[0];
-	else
+	else if (ifnum < dev->actconfig->desc.bNumInterfaces)
 		interface = &dev->actconfig->interface[ifnum]->altsetting[0];
+	else {
+		dev_err(&intf->dev, "interface %d is invalid, max is %d\n",
+		    ifnum, dev->actconfig->desc.bNumInterfaces - 1);
+		ret = -ENODEV;
+		goto err_usb;
+	}
+
+	if (interface->desc.bNumEndpoints < 2) {
+		dev_err(&intf->dev, "interface %d has %d endpoints, but must"
+		    " have minimum 2\n", ifnum, interface->desc.bNumEndpoints);
+		ret = -ENODEV;
+		goto err_usb;
+	}
 	endpoint = &interface->endpoint[1].desc;
+
 	if (!usb_endpoint_xfer_isoc(endpoint)) {
 		dev_err(&intf->dev, "%s: interface %d. has non-ISO endpoint!\n",
 		    __func__, ifnum);
diff --git a/drivers/media/usb/uvc/uvc_ctrl.c b/drivers/media/usb/uvc/uvc_ctrl.c
index 3e59b288b8a8..c2ee6e39fd0c 100644
--- a/drivers/media/usb/uvc/uvc_ctrl.c
+++ b/drivers/media/usb/uvc/uvc_ctrl.c
@@ -227,7 +227,8 @@ static struct uvc_control_info uvc_ctrls[] = {
 		.size		= 4,
 		.flags		= UVC_CTRL_FLAG_SET_CUR
 				| UVC_CTRL_FLAG_GET_RANGE
-				| UVC_CTRL_FLAG_RESTORE,
+				| UVC_CTRL_FLAG_RESTORE
+				| UVC_CTRL_FLAG_AUTO_UPDATE,
 	},
 	{
 		.entity		= UVC_GUID_UVC_CAMERA,
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index d11fd6ac2df0..4e7148815a78 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -1656,6 +1656,7 @@ static void uvc_delete(struct uvc_device *dev)
 #ifdef CONFIG_MEDIA_CONTROLLER
 	if (media_devnode_is_registered(&dev->mdev.devnode))
 		media_device_unregister(&dev->mdev);
+	media_device_cleanup(&dev->mdev);
 #endif
 
 	list_for_each_safe(p, n, &dev->chains) {
@@ -1906,7 +1907,7 @@ static int uvc_probe(struct usb_interface *intf,
 			"linux-uvc-devel mailing list.\n");
 	}
 
-	/* Register the media and V4L2 devices. */
+	/* Initialize the media device and register the V4L2 device. */
 #ifdef CONFIG_MEDIA_CONTROLLER
 	dev->mdev.dev = &intf->dev;
 	strlcpy(dev->mdev.model, dev->name, sizeof(dev->mdev.model));
@@ -1916,8 +1917,7 @@ static int uvc_probe(struct usb_interface *intf,
 	strcpy(dev->mdev.bus_info, udev->devpath);
 	dev->mdev.hw_revision = le16_to_cpu(udev->descriptor.bcdDevice);
 	dev->mdev.driver_version = LINUX_VERSION_CODE;
-	if (media_device_register(&dev->mdev) < 0)
-		goto error;
+	media_device_init(&dev->mdev);
 
 	dev->vdev.mdev = &dev->mdev;
 #endif
@@ -1936,6 +1936,11 @@ static int uvc_probe(struct usb_interface *intf,
 	if (uvc_register_chains(dev) < 0)
 		goto error;
 
+#ifdef CONFIG_MEDIA_CONTROLLER
+	/* Register the media device node */
+	if (media_device_register(&dev->mdev) < 0)
+		goto error;
+#endif
 	/* Save our data pointer in the interface data. */
 	usb_set_intfdata(intf, dev);
 
@@ -2540,7 +2545,8 @@ static struct usb_device_id uvc_ids[] = {
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= UVC_QUIRK_FORCE_Y8 },
 	/* Generic USB Video Class */
-	{ USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, 0) },
+	{ USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_UNDEFINED) },
+	{ USB_INTERFACE_INFO(USB_CLASS_VIDEO, 1, UVC_PC_PROTOCOL_15) },
 	{}
 };
 
diff --git a/drivers/media/usb/uvc/uvc_entity.c b/drivers/media/usb/uvc/uvc_entity.c
index dc56a59ecadc..ac386bb547e6 100644
--- a/drivers/media/usb/uvc/uvc_entity.c
+++ b/drivers/media/usb/uvc/uvc_entity.c
@@ -19,12 +19,8 @@
 
 #include "uvcvideo.h"
 
-/* ------------------------------------------------------------------------
- * Video subdevices registration and unregistration
- */
-
-static int uvc_mc_register_entity(struct uvc_video_chain *chain,
-	struct uvc_entity *entity)
+static int uvc_mc_create_links(struct uvc_video_chain *chain,
+				    struct uvc_entity *entity)
 {
 	const u32 flags = MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
 	struct media_entity *sink;
@@ -56,16 +52,13 @@ static int uvc_mc_register_entity(struct uvc_video_chain *chain,
 			continue;
 
 		remote_pad = remote->num_pads - 1;
-		ret = media_entity_create_link(source, remote_pad,
+		ret = media_create_pad_link(source, remote_pad,
 					       sink, i, flags);
 		if (ret < 0)
 			return ret;
 	}
 
-	if (UVC_ENTITY_TYPE(entity) == UVC_TT_STREAMING)
-		return 0;
-
-	return v4l2_device_register_subdev(&chain->dev->vdev, &entity->subdev);
+	return 0;
 }
 
 static struct v4l2_subdev_ops uvc_subdev_ops = {
@@ -79,7 +72,8 @@ void uvc_mc_cleanup_entity(struct uvc_entity *entity)
 		media_entity_cleanup(&entity->vdev->entity);
 }
 
-static int uvc_mc_init_entity(struct uvc_entity *entity)
+static int uvc_mc_init_entity(struct uvc_video_chain *chain,
+			      struct uvc_entity *entity)
 {
 	int ret;
 
@@ -88,11 +82,17 @@ static int uvc_mc_init_entity(struct uvc_entity *entity)
 		strlcpy(entity->subdev.name, entity->name,
 			sizeof(entity->subdev.name));
 
-		ret = media_entity_init(&entity->subdev.entity,
-					entity->num_pads, entity->pads, 0);
+		ret = media_entity_pads_init(&entity->subdev.entity,
+					entity->num_pads, entity->pads);
+
+		if (ret < 0)
+			return ret;
+
+		ret = v4l2_device_register_subdev(&chain->dev->vdev,
+						  &entity->subdev);
 	} else if (entity->vdev != NULL) {
-		ret = media_entity_init(&entity->vdev->entity,
-					entity->num_pads, entity->pads, 0);
+		ret = media_entity_pads_init(&entity->vdev->entity,
+					entity->num_pads, entity->pads);
 		if (entity->flags & UVC_ENTITY_FLAG_DEFAULT)
 			entity->vdev->entity.flags |= MEDIA_ENT_FL_DEFAULT;
 	} else
@@ -107,7 +107,7 @@ int uvc_mc_register_entities(struct uvc_video_chain *chain)
 	int ret;
 
 	list_for_each_entry(entity, &chain->entities, chain) {
-		ret = uvc_mc_init_entity(entity);
+		ret = uvc_mc_init_entity(chain, entity);
 		if (ret < 0) {
 			uvc_printk(KERN_INFO, "Failed to initialize entity for "
 				   "entity %u\n", entity->id);
@@ -116,9 +116,9 @@ int uvc_mc_register_entities(struct uvc_video_chain *chain)
 	}
 
 	list_for_each_entry(entity, &chain->entities, chain) {
-		ret = uvc_mc_register_entity(chain, entity);
+		ret = uvc_mc_create_links(chain, entity);
 		if (ret < 0) {
-			uvc_printk(KERN_INFO, "Failed to register entity for "
+			uvc_printk(KERN_INFO, "Failed to create links for "
 				   "entity %u\n", entity->id);
 			return ret;
 		}
diff --git a/drivers/media/usb/uvc/uvc_queue.c b/drivers/media/usb/uvc/uvc_queue.c
index cfb868a48b5f..54394722756f 100644
--- a/drivers/media/usb/uvc/uvc_queue.c
+++ b/drivers/media/usb/uvc/uvc_queue.c
@@ -69,23 +69,19 @@ static void uvc_queue_return_buffers(struct uvc_video_queue *queue,
  * videobuf2 queue operations
  */
 
-static int uvc_queue_setup(struct vb2_queue *vq, const void *parg,
+static int uvc_queue_setup(struct vb2_queue *vq,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	const struct v4l2_format *fmt = parg;
 	struct uvc_video_queue *queue = vb2_get_drv_priv(vq);
 	struct uvc_streaming *stream = uvc_queue_to_stream(queue);
+	unsigned size = stream->ctrl.dwMaxVideoFrameSize;
 
 	/* Make sure the image size is large enough. */
-	if (fmt && fmt->fmt.pix.sizeimage < stream->ctrl.dwMaxVideoFrameSize)
-		return -EINVAL;
-
+	if (*nplanes)
+		return sizes[0] < size ? -EINVAL : 0;
 	*nplanes = 1;
-
-	sizes[0] = fmt ? fmt->fmt.pix.sizeimage
-		 : stream->ctrl.dwMaxVideoFrameSize;
-
+	sizes[0] = size;
 	return 0;
 }
 
diff --git a/drivers/media/usb/uvc/uvc_v4l2.c b/drivers/media/usb/uvc/uvc_v4l2.c
index 2764f43607c1..d7723ce772b3 100644
--- a/drivers/media/usb/uvc/uvc_v4l2.c
+++ b/drivers/media/usb/uvc/uvc_v4l2.c
@@ -983,6 +983,22 @@ static int uvc_ioctl_g_ext_ctrls(struct file *file, void *fh,
 	unsigned int i;
 	int ret;
 
+	if (ctrls->which == V4L2_CTRL_WHICH_DEF_VAL) {
+		for (i = 0; i < ctrls->count; ++ctrl, ++i) {
+			struct v4l2_queryctrl qc = { .id = ctrl->id };
+
+			ret = uvc_query_v4l2_ctrl(chain, &qc);
+			if (ret < 0) {
+				ctrls->error_idx = i;
+				return ret;
+			}
+
+			ctrl->value = qc.default_value;
+		}
+
+		return 0;
+	}
+
 	ret = uvc_ctrl_begin(chain);
 	if (ret < 0)
 		return ret;
@@ -1010,6 +1026,10 @@ static int uvc_ioctl_s_try_ext_ctrls(struct uvc_fh *handle,
 	unsigned int i;
 	int ret;
 
+	/* Default value cannot be changed */
+	if (ctrls->which == V4L2_CTRL_WHICH_DEF_VAL)
+		return -EINVAL;
+
 	ret = uvc_ctrl_begin(chain);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 2b276ab7764f..075a0fe77485 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -694,22 +694,19 @@ void uvc_video_clock_update(struct uvc_streaming *stream,
 		ts.tv_nsec -= NSEC_PER_SEC;
 	}
 
-	uvc_trace(UVC_TRACE_CLOCK, "%s: SOF %u.%06llu y %llu ts %lu.%06lu "
-		  "buf ts %lu.%06lu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %u)\n",
+	uvc_trace(UVC_TRACE_CLOCK, "%s: SOF %u.%06llu y %llu ts %llu "
+		  "buf ts %llu (x1 %u/%u/%u x2 %u/%u/%u y1 %u y2 %u)\n",
 		  stream->dev->name,
 		  sof >> 16, div_u64(((u64)sof & 0xffff) * 1000000LLU, 65536),
-		  y, ts.tv_sec, ts.tv_nsec / NSEC_PER_USEC,
-		  vbuf->timestamp.tv_sec,
-		  (unsigned long)vbuf->timestamp.tv_usec,
+		  y, timespec_to_ns(&ts), vbuf->vb2_buf.timestamp,
 		  x1, first->host_sof, first->dev_sof,
 		  x2, last->host_sof, last->dev_sof, y1, y2);
 
 	/* Update the V4L2 buffer. */
-	vbuf->timestamp.tv_sec = ts.tv_sec;
-	vbuf->timestamp.tv_usec = ts.tv_nsec / NSEC_PER_USEC;
+	vbuf->vb2_buf.timestamp = timespec_to_ns(&ts);
 
 done:
-	spin_unlock_irqrestore(&stream->clock.lock, flags);
+	spin_unlock_irqrestore(&clock->lock, flags);
 }
 
 /* ------------------------------------------------------------------------
@@ -1034,9 +1031,7 @@ static int uvc_video_decode_start(struct uvc_streaming *stream,
 
 		buf->buf.field = V4L2_FIELD_NONE;
 		buf->buf.sequence = stream->sequence;
-		buf->buf.timestamp.tv_sec = ts.tv_sec;
-		buf->buf.timestamp.tv_usec =
-			ts.tv_nsec / NSEC_PER_USEC;
+		buf->buf.vb2_buf.timestamp = timespec_to_ns(&ts);
 
 		/* TODO: Handle PTS and SCR. */
 		buf->state = UVC_BUF_STATE_ACTIVE;
diff --git a/drivers/media/v4l2-core/tuner-core.c b/drivers/media/v4l2-core/tuner-core.c
index 581e21ad6801..76496fd282aa 100644
--- a/drivers/media/v4l2-core/tuner-core.c
+++ b/drivers/media/v4l2-core/tuner-core.c
@@ -134,8 +134,9 @@ struct tuner {
 	unsigned int        type; /* chip type id */
 	void                *config;
 	const char          *name;
+
 #if defined(CONFIG_MEDIA_CONTROLLER)
-	struct media_pad	pad;
+	struct media_pad	pad[TUNER_NUM_PADS];
 #endif
 };
 
@@ -695,11 +696,12 @@ static int tuner_probe(struct i2c_client *client,
 	/* Should be just before return */
 register_client:
 #if defined(CONFIG_MEDIA_CONTROLLER)
-	t->pad.flags = MEDIA_PAD_FL_SOURCE;
-	t->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_TUNER;
+	t->pad[TUNER_PAD_RF_INPUT].flags = MEDIA_PAD_FL_SINK;
+	t->pad[TUNER_PAD_IF_OUTPUT].flags = MEDIA_PAD_FL_SOURCE;
+	t->sd.entity.function = MEDIA_ENT_F_TUNER;
 	t->sd.entity.name = t->name;
 
-	ret = media_entity_init(&t->sd.entity, 1, &t->pad, 0);
+	ret = media_entity_pads_init(&t->sd.entity, TUNER_NUM_PADS, &t->pad[0]);
 	if (ret < 0) {
 		tuner_err("failed to initialize media entity!\n");
 		kfree(t);
diff --git a/drivers/media/v4l2-core/v4l2-clk.c b/drivers/media/v4l2-core/v4l2-clk.c
index 34e416a554f6..297e10e69898 100644
--- a/drivers/media/v4l2-core/v4l2-clk.c
+++ b/drivers/media/v4l2-core/v4l2-clk.c
@@ -15,6 +15,7 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
 #include <linux/slab.h>
 #include <linux/string.h>
 
@@ -39,6 +40,7 @@ struct v4l2_clk *v4l2_clk_get(struct device *dev, const char *id)
 {
 	struct v4l2_clk *clk;
 	struct clk *ccf_clk = clk_get(dev, id);
+	char clk_name[V4L2_CLK_NAME_SIZE];
 
 	if (PTR_ERR(ccf_clk) == -EPROBE_DEFER)
 		return ERR_PTR(-EPROBE_DEFER);
@@ -57,6 +59,13 @@ struct v4l2_clk *v4l2_clk_get(struct device *dev, const char *id)
 	mutex_lock(&clk_lock);
 	clk = v4l2_clk_find(dev_name(dev));
 
+	/* if dev_name is not found, try use the OF name to find again  */
+	if (PTR_ERR(clk) == -ENODEV && dev->of_node) {
+		v4l2_clk_name_of(clk_name, sizeof(clk_name),
+				 of_node_full_name(dev->of_node));
+		clk = v4l2_clk_find(clk_name);
+	}
+
 	if (!IS_ERR(clk))
 		atomic_inc(&clk->use_count);
 	mutex_unlock(&clk_lock);
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index 327e83ac2469..8fd84a67478a 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -630,7 +630,7 @@ static inline int put_v4l2_input32(struct v4l2_input *kp, struct v4l2_input32 __
 }
 
 struct v4l2_ext_controls32 {
-	__u32 ctrl_class;
+	__u32 which;
 	__u32 count;
 	__u32 error_idx;
 	__u32 reserved[2];
@@ -673,7 +673,7 @@ static int get_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 	compat_caddr_t p;
 
 	if (!access_ok(VERIFY_READ, up, sizeof(struct v4l2_ext_controls32)) ||
-		get_user(kp->ctrl_class, &up->ctrl_class) ||
+		get_user(kp->which, &up->which) ||
 		get_user(kp->count, &up->count) ||
 		get_user(kp->error_idx, &up->error_idx) ||
 		copy_from_user(kp->reserved, up->reserved,
@@ -723,7 +723,7 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 	compat_caddr_t p;
 
 	if (!access_ok(VERIFY_WRITE, up, sizeof(struct v4l2_ext_controls32)) ||
-		put_user(kp->ctrl_class, &up->ctrl_class) ||
+		put_user(kp->which, &up->which) ||
 		put_user(kp->count, &up->count) ||
 		put_user(kp->error_idx, &up->error_idx) ||
 		copy_to_user(up->reserved, kp->reserved, sizeof(up->reserved)))
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index 4a1d9fdd14bb..c9d5537b6af7 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -1491,6 +1491,17 @@ static int new_to_user(struct v4l2_ext_control *c,
 	return ptr_to_user(c, ctrl, ctrl->p_new);
 }
 
+/* Helper function: copy the initial control value back to the caller */
+static int def_to_user(struct v4l2_ext_control *c, struct v4l2_ctrl *ctrl)
+{
+	int idx;
+
+	for (idx = 0; idx < ctrl->elems; idx++)
+		ctrl->type_ops->init(ctrl, idx, ctrl->p_new);
+
+	return ptr_to_user(c, ctrl, ctrl->p_new);
+}
+
 /* Helper function: copy the caller-provider value to the given control value */
 static int user_to_ptr(struct v4l2_ext_control *c,
 		       struct v4l2_ctrl *ctrl,
@@ -1762,7 +1773,7 @@ static struct v4l2_ctrl_ref *find_private_ref(
 	list_for_each_entry(ref, &hdl->ctrl_refs, node) {
 		/* Search for private user controls that are compatible with
 		   VIDIOC_G/S_CTRL. */
-		if (V4L2_CTRL_ID2CLASS(ref->ctrl->id) == V4L2_CTRL_CLASS_USER &&
+		if (V4L2_CTRL_ID2WHICH(ref->ctrl->id) == V4L2_CTRL_CLASS_USER &&
 		    V4L2_CTRL_DRIVER_PRIV(ref->ctrl->id)) {
 			if (!ref->ctrl->is_int)
 				continue;
@@ -1831,7 +1842,7 @@ static int handler_new_ref(struct v4l2_ctrl_handler *hdl,
 	struct v4l2_ctrl_ref *ref;
 	struct v4l2_ctrl_ref *new_ref;
 	u32 id = ctrl->id;
-	u32 class_ctrl = V4L2_CTRL_ID2CLASS(id) | 1;
+	u32 class_ctrl = V4L2_CTRL_ID2WHICH(id) | 1;
 	int bucket = id % hdl->nr_of_buckets;	/* which bucket to use */
 
 	/*
@@ -2253,9 +2264,9 @@ EXPORT_SYMBOL(v4l2_ctrl_add_handler);
 
 bool v4l2_ctrl_radio_filter(const struct v4l2_ctrl *ctrl)
 {
-	if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_FM_TX)
+	if (V4L2_CTRL_ID2WHICH(ctrl->id) == V4L2_CTRL_CLASS_FM_TX)
 		return true;
-	if (V4L2_CTRL_ID2CLASS(ctrl->id) == V4L2_CTRL_CLASS_FM_RX)
+	if (V4L2_CTRL_ID2WHICH(ctrl->id) == V4L2_CTRL_CLASS_FM_RX)
 		return true;
 	switch (ctrl->id) {
 	case V4L2_CID_AUDIO_MUTE:
@@ -2710,7 +2721,9 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 
 		cs->error_idx = i;
 
-		if (cs->ctrl_class && V4L2_CTRL_ID2CLASS(id) != cs->ctrl_class)
+		if (cs->which &&
+		    cs->which != V4L2_CTRL_WHICH_DEF_VAL &&
+		    V4L2_CTRL_ID2WHICH(id) != cs->which)
 			return -EINVAL;
 
 		/* Old-style private controls are not allowed for
@@ -2787,11 +2800,11 @@ static int prepare_ext_ctrls(struct v4l2_ctrl_handler *hdl,
 /* Handles the corner case where cs->count == 0. It checks whether the
    specified control class exists. If that class ID is 0, then it checks
    whether there are any controls at all. */
-static int class_check(struct v4l2_ctrl_handler *hdl, u32 ctrl_class)
+static int class_check(struct v4l2_ctrl_handler *hdl, u32 which)
 {
-	if (ctrl_class == 0)
+	if (which == 0 || which == V4L2_CTRL_WHICH_DEF_VAL)
 		return list_empty(&hdl->ctrl_refs) ? -EINVAL : 0;
-	return find_ref_lock(hdl, ctrl_class | 1) ? 0 : -EINVAL;
+	return find_ref_lock(hdl, which | 1) ? 0 : -EINVAL;
 }
 
 
@@ -2803,15 +2816,18 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 	struct v4l2_ctrl_helper *helpers = helper;
 	int ret;
 	int i, j;
+	bool def_value;
+
+	def_value = (cs->which == V4L2_CTRL_WHICH_DEF_VAL);
 
 	cs->error_idx = cs->count;
-	cs->ctrl_class = V4L2_CTRL_ID2CLASS(cs->ctrl_class);
+	cs->which = V4L2_CTRL_ID2WHICH(cs->which);
 
 	if (hdl == NULL)
 		return -EINVAL;
 
 	if (cs->count == 0)
-		return class_check(hdl, cs->ctrl_class);
+		return class_check(hdl, cs->which);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
 		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
@@ -2829,9 +2845,11 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 
 	for (i = 0; !ret && i < cs->count; i++) {
 		int (*ctrl_to_user)(struct v4l2_ext_control *c,
-				    struct v4l2_ctrl *ctrl) = cur_to_user;
+				    struct v4l2_ctrl *ctrl);
 		struct v4l2_ctrl *master;
 
+		ctrl_to_user = def_value ? def_to_user : cur_to_user;
+
 		if (helpers[i].mref == NULL)
 			continue;
 
@@ -2841,8 +2859,9 @@ int v4l2_g_ext_ctrls(struct v4l2_ctrl_handler *hdl, struct v4l2_ext_controls *cs
 		v4l2_ctrl_lock(master);
 
 		/* g_volatile_ctrl will update the new control values */
-		if ((master->flags & V4L2_CTRL_FLAG_VOLATILE) ||
-			(master->has_volatiles && !is_cur_manual(master))) {
+		if (!def_value &&
+		    ((master->flags & V4L2_CTRL_FLAG_VOLATILE) ||
+		    (master->has_volatiles && !is_cur_manual(master)))) {
 			for (j = 0; j < master->ncontrols; j++)
 				cur_to_new(master->cluster[j]);
 			ret = call_op(master, g_volatile_ctrl);
@@ -3064,13 +3083,18 @@ static int try_set_ext_ctrls(struct v4l2_fh *fh, struct v4l2_ctrl_handler *hdl,
 	int ret;
 
 	cs->error_idx = cs->count;
-	cs->ctrl_class = V4L2_CTRL_ID2CLASS(cs->ctrl_class);
+
+	/* Default value cannot be changed */
+	if (cs->which == V4L2_CTRL_WHICH_DEF_VAL)
+		return -EINVAL;
+
+	cs->which = V4L2_CTRL_ID2WHICH(cs->which);
 
 	if (hdl == NULL)
 		return -EINVAL;
 
 	if (cs->count == 0)
-		return class_check(hdl, cs->ctrl_class);
+		return class_check(hdl, cs->which);
 
 	if (cs->count > ARRAY_SIZE(helper)) {
 		helpers = kmalloc_array(cs->count, sizeof(helper[0]),
@@ -3300,7 +3324,8 @@ EXPORT_SYMBOL(v4l2_ctrl_notify);
 int __v4l2_ctrl_modify_range(struct v4l2_ctrl *ctrl,
 			s64 min, s64 max, u64 step, s64 def)
 {
-	bool changed;
+	bool value_changed;
+	bool range_changed = false;
 	int ret;
 
 	lockdep_assert_held(ctrl->handler->lock);
@@ -3324,10 +3349,14 @@ int __v4l2_ctrl_modify_range(struct v4l2_ctrl *ctrl,
 	default:
 		return -EINVAL;
 	}
-	ctrl->minimum = min;
-	ctrl->maximum = max;
-	ctrl->step = step;
-	ctrl->default_value = def;
+	if ((ctrl->minimum != min) || (ctrl->maximum != max) ||
+		(ctrl->step != step) || ctrl->default_value != def) {
+		range_changed = true;
+		ctrl->minimum = min;
+		ctrl->maximum = max;
+		ctrl->step = step;
+		ctrl->default_value = def;
+	}
 	cur_to_new(ctrl);
 	if (validate_new(ctrl, ctrl->p_new)) {
 		if (ctrl->type == V4L2_CTRL_TYPE_INTEGER64)
@@ -3337,12 +3366,12 @@ int __v4l2_ctrl_modify_range(struct v4l2_ctrl *ctrl,
 	}
 
 	if (ctrl->type == V4L2_CTRL_TYPE_INTEGER64)
-		changed = *ctrl->p_new.p_s64 != *ctrl->p_cur.p_s64;
+		value_changed = *ctrl->p_new.p_s64 != *ctrl->p_cur.p_s64;
 	else
-		changed = *ctrl->p_new.p_s32 != *ctrl->p_cur.p_s32;
-	if (changed)
+		value_changed = *ctrl->p_new.p_s32 != *ctrl->p_cur.p_s32;
+	if (value_changed)
 		ret = set_ctrl(NULL, ctrl, V4L2_EVENT_CTRL_CH_RANGE);
-	else
+	else if (range_changed)
 		send_event(NULL, ctrl, V4L2_EVENT_CTRL_CH_RANGE);
 	return ret;
 }
diff --git a/drivers/media/v4l2-core/v4l2-dev.c b/drivers/media/v4l2-core/v4l2-dev.c
index 6b1eaeddbdb3..d8e5994cccf1 100644
--- a/drivers/media/v4l2-core/v4l2-dev.c
+++ b/drivers/media/v4l2-core/v4l2-dev.c
@@ -194,9 +194,12 @@ static void v4l2_device_release(struct device *cd)
 	mutex_unlock(&videodev_lock);
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
-	if (v4l2_dev->mdev &&
-	    vdev->vfl_type != VFL_TYPE_SUBDEV)
-		media_device_unregister_entity(&vdev->entity);
+	if (v4l2_dev->mdev) {
+		/* Remove interfaces and interface links */
+		media_devnode_remove(vdev->intf_devnode);
+		if (vdev->entity.function != MEDIA_ENT_F_UNKNOWN)
+			media_device_unregister_entity(&vdev->entity);
+	}
 #endif
 
 	/* Do not call v4l2_device_put if there is no release callback set.
@@ -723,6 +726,91 @@ static void determine_valid_ioctls(struct video_device *vdev)
 			BASE_VIDIOC_PRIVATE);
 }
 
+static int video_register_media_controller(struct video_device *vdev, int type)
+{
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	u32 intf_type;
+	int ret;
+
+	if (!vdev->v4l2_dev->mdev)
+		return 0;
+
+	vdev->entity.function = MEDIA_ENT_F_UNKNOWN;
+
+	switch (type) {
+	case VFL_TYPE_GRABBER:
+		intf_type = MEDIA_INTF_T_V4L_VIDEO;
+		vdev->entity.function = MEDIA_ENT_F_IO_V4L;
+		break;
+	case VFL_TYPE_VBI:
+		intf_type = MEDIA_INTF_T_V4L_VBI;
+		vdev->entity.function = MEDIA_ENT_F_IO_VBI;
+		break;
+	case VFL_TYPE_SDR:
+		intf_type = MEDIA_INTF_T_V4L_SWRADIO;
+		vdev->entity.function = MEDIA_ENT_F_IO_SWRADIO;
+		break;
+	case VFL_TYPE_RADIO:
+		intf_type = MEDIA_INTF_T_V4L_RADIO;
+		/*
+		 * Radio doesn't have an entity at the V4L2 side to represent
+		 * radio input or output. Instead, the audio input/output goes
+		 * via either physical wires or ALSA.
+		 */
+		break;
+	case VFL_TYPE_SUBDEV:
+		intf_type = MEDIA_INTF_T_V4L_SUBDEV;
+		/* Entity will be created via v4l2_device_register_subdev() */
+		break;
+	default:
+		return 0;
+	}
+
+	if (vdev->entity.function != MEDIA_ENT_F_UNKNOWN) {
+		vdev->entity.name = vdev->name;
+
+		/* Needed just for backward compatibility with legacy MC API */
+		vdev->entity.info.dev.major = VIDEO_MAJOR;
+		vdev->entity.info.dev.minor = vdev->minor;
+
+		ret = media_device_register_entity(vdev->v4l2_dev->mdev,
+						   &vdev->entity);
+		if (ret < 0) {
+			printk(KERN_WARNING
+				"%s: media_device_register_entity failed\n",
+				__func__);
+			return ret;
+		}
+	}
+
+	vdev->intf_devnode = media_devnode_create(vdev->v4l2_dev->mdev,
+						  intf_type,
+						  0, VIDEO_MAJOR,
+						  vdev->minor);
+	if (!vdev->intf_devnode) {
+		media_device_unregister_entity(&vdev->entity);
+		return -ENOMEM;
+	}
+
+	if (vdev->entity.function != MEDIA_ENT_F_UNKNOWN) {
+		struct media_link *link;
+
+		link = media_create_intf_link(&vdev->entity,
+					      &vdev->intf_devnode->intf,
+					      MEDIA_LNK_FL_ENABLED);
+		if (!link) {
+			media_devnode_remove(vdev->intf_devnode);
+			media_device_unregister_entity(&vdev->entity);
+			return -ENOMEM;
+		}
+	}
+
+	/* FIXME: how to create the other interface links? */
+
+#endif
+	return 0;
+}
+
 /**
  *	__video_register_device - register video4linux devices
  *	@vdev: video device structure we want to register
@@ -918,22 +1006,9 @@ int __video_register_device(struct video_device *vdev, int type, int nr,
 	/* Increase v4l2_device refcount */
 	v4l2_device_get(vdev->v4l2_dev);
 
-#if defined(CONFIG_MEDIA_CONTROLLER)
 	/* Part 5: Register the entity. */
-	if (vdev->v4l2_dev->mdev &&
-	    vdev->vfl_type != VFL_TYPE_SUBDEV) {
-		vdev->entity.type = MEDIA_ENT_T_DEVNODE_V4L;
-		vdev->entity.name = vdev->name;
-		vdev->entity.info.dev.major = VIDEO_MAJOR;
-		vdev->entity.info.dev.minor = vdev->minor;
-		ret = media_device_register_entity(vdev->v4l2_dev->mdev,
-			&vdev->entity);
-		if (ret < 0)
-			printk(KERN_WARNING
-			       "%s: media_device_register_entity failed\n",
-			       __func__);
-	}
-#endif
+	ret = video_register_media_controller(vdev, type);
+
 	/* Part 6: Activate this minor. The char device can now be used. */
 	set_bit(V4L2_FL_REGISTERED, &vdev->flags);
 
diff --git a/drivers/media/v4l2-core/v4l2-device.c b/drivers/media/v4l2-core/v4l2-device.c
index 5b0a30b9252b..06fa5f1b2cff 100644
--- a/drivers/media/v4l2-core/v4l2-device.c
+++ b/drivers/media/v4l2-core/v4l2-device.c
@@ -118,11 +118,20 @@ void v4l2_device_unregister(struct v4l2_device *v4l2_dev)
 		if (sd->flags & V4L2_SUBDEV_FL_IS_I2C) {
 			struct i2c_client *client = v4l2_get_subdevdata(sd);
 
-			/* We need to unregister the i2c client explicitly.
-			   We cannot rely on i2c_del_adapter to always
-			   unregister clients for us, since if the i2c bus
-			   is a platform bus, then it is never deleted. */
-			if (client)
+			/*
+			 * We need to unregister the i2c client
+			 * explicitly. We cannot rely on
+			 * i2c_del_adapter to always unregister
+			 * clients for us, since if the i2c bus is a
+			 * platform bus, then it is never deleted.
+			 *
+			 * Device tree or ACPI based devices must not
+			 * be unregistered as they have not been
+			 * registered by us, and would not be
+			 * re-created by just probing the V4L2 driver.
+			 */
+			if (client &&
+			    !client->dev.of_node && !client->dev.fwnode)
 				i2c_unregister_device(client);
 			continue;
 		}
@@ -131,7 +140,7 @@ void v4l2_device_unregister(struct v4l2_device *v4l2_dev)
 		if (sd->flags & V4L2_SUBDEV_FL_IS_SPI) {
 			struct spi_device *spi = v4l2_get_subdevdata(sd);
 
-			if (spi)
+			if (spi && !spi->dev.of_node && !spi->dev.fwnode)
 				spi_unregister_device(spi);
 			continue;
 		}
@@ -171,26 +180,26 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 		return -ENODEV;
 
 	sd->v4l2_dev = v4l2_dev;
-	if (sd->internal_ops && sd->internal_ops->registered) {
-		err = sd->internal_ops->registered(sd);
-		if (err)
-			goto error_module;
-	}
-
 	/* This just returns 0 if either of the two args is NULL */
 	err = v4l2_ctrl_add_handler(v4l2_dev->ctrl_handler, sd->ctrl_handler, NULL);
 	if (err)
-		goto error_unregister;
+		goto error_module;
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	/* Register the entity. */
 	if (v4l2_dev->mdev) {
 		err = media_device_register_entity(v4l2_dev->mdev, entity);
 		if (err < 0)
-			goto error_unregister;
+			goto error_module;
 	}
 #endif
 
+	if (sd->internal_ops && sd->internal_ops->registered) {
+		err = sd->internal_ops->registered(sd);
+		if (err)
+			goto error_unregister;
+	}
+
 	spin_lock(&v4l2_dev->lock);
 	list_add_tail(&sd->list, &v4l2_dev->subdevs);
 	spin_unlock(&v4l2_dev->lock);
@@ -198,8 +207,9 @@ int v4l2_device_register_subdev(struct v4l2_device *v4l2_dev,
 	return 0;
 
 error_unregister:
-	if (sd->internal_ops && sd->internal_ops->unregistered)
-		sd->internal_ops->unregistered(sd);
+#if defined(CONFIG_MEDIA_CONTROLLER)
+	media_device_unregister_entity(entity);
+#endif
 error_module:
 	if (!sd->owner_v4l2_dev)
 		module_put(sd->owner);
@@ -249,6 +259,19 @@ int v4l2_device_register_subdev_nodes(struct v4l2_device *v4l2_dev)
 #if defined(CONFIG_MEDIA_CONTROLLER)
 		sd->entity.info.dev.major = VIDEO_MAJOR;
 		sd->entity.info.dev.minor = vdev->minor;
+
+		/* Interface is created by __video_register_device() */
+		if (vdev->v4l2_dev->mdev) {
+			struct media_link *link;
+
+			link = media_create_intf_link(&sd->entity,
+						      &vdev->intf_devnode->intf,
+						      MEDIA_LNK_FL_ENABLED);
+			if (!link) {
+				err = -ENOMEM;
+				goto clean_up;
+			}
+		}
 #endif
 		sd->devnode = vdev;
 	}
@@ -285,7 +308,10 @@ void v4l2_device_unregister_subdev(struct v4l2_subdev *sd)
 
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	if (v4l2_dev->mdev) {
-		media_entity_remove_links(&sd->entity);
+		/*
+		 * No need to explicitly remove links, as both pads and
+		 * links are removed by the function below, in the right order
+		 */
 		media_device_unregister_entity(&sd->entity);
 	}
 #endif
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index 6a83d6191684..ec258b73001a 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -209,8 +209,13 @@ bool v4l2_find_dv_timings_cap(struct v4l2_dv_timings *t,
 		if (v4l2_valid_dv_timings(v4l2_dv_timings_presets + i, cap,
 					  fnc, fnc_handle) &&
 		    v4l2_match_dv_timings(t, v4l2_dv_timings_presets + i,
-					  pclock_delta)) {
+					  pclock_delta, false)) {
+			u32 flags = t->bt.flags & V4L2_DV_FL_REDUCED_FPS;
+
 			*t = v4l2_dv_timings_presets[i];
+			if (can_reduce_fps(&t->bt))
+				t->bt.flags |= flags;
+
 			return true;
 		}
 	}
@@ -223,12 +228,14 @@ EXPORT_SYMBOL_GPL(v4l2_find_dv_timings_cap);
  * @t1 - compare this v4l2_dv_timings struct...
  * @t2 - with this struct.
  * @pclock_delta - the allowed pixelclock deviation.
+ * @match_reduced_fps - if true, then fail if V4L2_DV_FL_REDUCED_FPS does not
+ * match.
  *
  * Compare t1 with t2 with a given margin of error for the pixelclock.
  */
 bool v4l2_match_dv_timings(const struct v4l2_dv_timings *t1,
 			   const struct v4l2_dv_timings *t2,
-			   unsigned pclock_delta)
+			   unsigned pclock_delta, bool match_reduced_fps)
 {
 	if (t1->type != t2->type || t1->type != V4L2_DV_BT_656_1120)
 		return false;
@@ -239,9 +246,14 @@ bool v4l2_match_dv_timings(const struct v4l2_dv_timings *t1,
 	    t1->bt.pixelclock >= t2->bt.pixelclock - pclock_delta &&
 	    t1->bt.pixelclock <= t2->bt.pixelclock + pclock_delta &&
 	    t1->bt.hfrontporch == t2->bt.hfrontporch &&
+	    t1->bt.hsync == t2->bt.hsync &&
+	    t1->bt.hbackporch == t2->bt.hbackporch &&
 	    t1->bt.vfrontporch == t2->bt.vfrontporch &&
 	    t1->bt.vsync == t2->bt.vsync &&
 	    t1->bt.vbackporch == t2->bt.vbackporch &&
+	    (!match_reduced_fps ||
+	     (t1->bt.flags & V4L2_DV_FL_REDUCED_FPS) ==
+		(t2->bt.flags & V4L2_DV_FL_REDUCED_FPS)) &&
 	    (!t1->bt.interlaced ||
 		(t1->bt.il_vfrontporch == t2->bt.il_vfrontporch &&
 		 t1->bt.il_vsync == t2->bt.il_vsync &&
diff --git a/drivers/media/v4l2-core/v4l2-flash-led-class.c b/drivers/media/v4l2-core/v4l2-flash-led-class.c
index 5bdfb8d5263a..fc5ff8b215f9 100644
--- a/drivers/media/v4l2-core/v4l2-flash-led-class.c
+++ b/drivers/media/v4l2-core/v4l2-flash-led-class.c
@@ -107,10 +107,10 @@ static void v4l2_flash_set_led_brightness(struct v4l2_flash *v4l2_flash,
 		if (ctrls[LED_MODE]->val != V4L2_FLASH_LED_MODE_TORCH)
 			return;
 
-		led_set_brightness(&v4l2_flash->fled_cdev->led_cdev,
+		led_set_brightness_sync(&v4l2_flash->fled_cdev->led_cdev,
 					brightness);
 	} else {
-		led_set_brightness(&v4l2_flash->iled_cdev->led_cdev,
+		led_set_brightness_sync(&v4l2_flash->iled_cdev->led_cdev,
 					brightness);
 	}
 }
@@ -206,11 +206,11 @@ static int v4l2_flash_s_ctrl(struct v4l2_ctrl *c)
 	case V4L2_CID_FLASH_LED_MODE:
 		switch (c->val) {
 		case V4L2_FLASH_LED_MODE_NONE:
-			led_set_brightness(led_cdev, LED_OFF);
+			led_set_brightness_sync(led_cdev, LED_OFF);
 			return led_set_flash_strobe(fled_cdev, false);
 		case V4L2_FLASH_LED_MODE_FLASH:
 			/* Turn the torch LED off */
-			led_set_brightness(led_cdev, LED_OFF);
+			led_set_brightness_sync(led_cdev, LED_OFF);
 			if (ctrls[STROBE_SOURCE]) {
 				external_strobe = (ctrls[STROBE_SOURCE]->val ==
 					V4L2_FLASH_STROBE_SOURCE_EXTERNAL);
@@ -651,11 +651,11 @@ struct v4l2_flash *v4l2_flash_init(
 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 	strlcpy(sd->name, config->dev_name, sizeof(sd->name));
 
-	ret = media_entity_init(&sd->entity, 0, NULL, 0);
+	ret = media_entity_pads_init(&sd->entity, 0, NULL);
 	if (ret < 0)
 		return ERR_PTR(ret);
 
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_FLASH;
+	sd->entity.function = MEDIA_ENT_F_FLASH;
 
 	ret = v4l2_flash_init_controls(v4l2_flash, config);
 	if (ret < 0)
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index 7486af2c8ae4..8a018c6dd16a 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -565,8 +565,8 @@ static void v4l_print_ext_controls(const void *arg, bool write_only)
 	const struct v4l2_ext_controls *p = arg;
 	int i;
 
-	pr_cont("class=0x%x, count=%d, error_idx=%d",
-			p->ctrl_class, p->count, p->error_idx);
+	pr_cont("which=0x%x, count=%d, error_idx=%d",
+			p->which, p->count, p->error_idx);
 	for (i = 0; i < p->count; i++) {
 		if (!p->controls[i].size)
 			pr_cont(", id/val=0x%x/0x%x",
@@ -902,13 +902,13 @@ static int check_ext_ctrls(struct v4l2_ext_controls *c, int allow_priv)
 	   Only when passed in through VIDIOC_G_CTRL and VIDIOC_S_CTRL
 	   is it allowed for backwards compatibility.
 	 */
-	if (!allow_priv && c->ctrl_class == V4L2_CID_PRIVATE_BASE)
+	if (!allow_priv && c->which == V4L2_CID_PRIVATE_BASE)
 		return 0;
-	if (c->ctrl_class == 0)
+	if (!c->which)
 		return 1;
 	/* Check that all controls are from the same control class. */
 	for (i = 0; i < c->count; i++) {
-		if (V4L2_CTRL_ID2CLASS(c->controls[i].id) != c->ctrl_class) {
+		if (V4L2_CTRL_ID2WHICH(c->controls[i].id) != c->which) {
 			c->error_idx = i;
 			return 0;
 		}
@@ -1969,7 +1969,7 @@ static int v4l_g_ctrl(const struct v4l2_ioctl_ops *ops,
 	if (ops->vidioc_g_ext_ctrls == NULL)
 		return -ENOTTY;
 
-	ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
+	ctrls.which = V4L2_CTRL_ID2WHICH(p->id);
 	ctrls.count = 1;
 	ctrls.controls = &ctrl;
 	ctrl.id = p->id;
@@ -2003,7 +2003,7 @@ static int v4l_s_ctrl(const struct v4l2_ioctl_ops *ops,
 	if (ops->vidioc_s_ext_ctrls == NULL)
 		return -ENOTTY;
 
-	ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(p->id);
+	ctrls.which = V4L2_CTRL_ID2WHICH(p->id);
 	ctrls.count = 1;
 	ctrls.controls = &ctrl;
 	ctrl.id = p->id;
diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index 83615b8fb46a..d63083803144 100644
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -526,7 +526,7 @@ static int
 v4l2_subdev_link_validate_get_format(struct media_pad *pad,
 				     struct v4l2_subdev_format *fmt)
 {
-	if (media_entity_type(pad->entity) == MEDIA_ENT_T_V4L2_SUBDEV) {
+	if (is_media_entity_v4l2_subdev(pad->entity)) {
 		struct v4l2_subdev *sd =
 			media_entity_to_v4l2_subdev(pad->entity);
 
@@ -535,9 +535,9 @@ v4l2_subdev_link_validate_get_format(struct media_pad *pad,
 		return v4l2_subdev_call(sd, pad, get_fmt, NULL, fmt);
 	}
 
-	WARN(pad->entity->type != MEDIA_ENT_T_DEVNODE_V4L,
+	WARN(pad->entity->function != MEDIA_ENT_F_IO_V4L,
 	     "Driver bug! Wrong media entity type 0x%08x, entity %s\n",
-	     pad->entity->type, pad->entity->name);
+	     pad->entity->function, pad->entity->name);
 
 	return -EINVAL;
 }
@@ -584,7 +584,7 @@ void v4l2_subdev_init(struct v4l2_subdev *sd, const struct v4l2_subdev_ops *ops)
 	sd->host_priv = NULL;
 #if defined(CONFIG_MEDIA_CONTROLLER)
 	sd->entity.name = sd->name;
-	sd->entity.type = MEDIA_ENT_T_V4L2_SUBDEV;
+	sd->entity.function = MEDIA_ENT_F_V4L2_SUBDEV_UNKNOWN;
 #endif
 }
 EXPORT_SYMBOL(v4l2_subdev_init);
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index 33bdd81065e8..ff8953ae52d1 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -28,11 +28,161 @@
 
 #include <trace/events/vb2.h>
 
-#include "videobuf2-internal.h"
+static int debug;
+module_param(debug, int, 0644);
 
-int vb2_debug;
-EXPORT_SYMBOL_GPL(vb2_debug);
-module_param_named(debug, vb2_debug, int, 0644);
+#define dprintk(level, fmt, arg...)					      \
+	do {								      \
+		if (debug >= level)					      \
+			pr_info("vb2-core: %s: " fmt, __func__, ## arg); \
+	} while (0)
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+
+/*
+ * If advanced debugging is on, then count how often each op is called
+ * successfully, which can either be per-buffer or per-queue.
+ *
+ * This makes it easy to check that the 'init' and 'cleanup'
+ * (and variations thereof) stay balanced.
+ */
+
+#define log_memop(vb, op)						\
+	dprintk(2, "call_memop(%p, %d, %s)%s\n",			\
+		(vb)->vb2_queue, (vb)->index, #op,			\
+		(vb)->vb2_queue->mem_ops->op ? "" : " (nop)")
+
+#define call_memop(vb, op, args...)					\
+({									\
+	struct vb2_queue *_q = (vb)->vb2_queue;				\
+	int err;							\
+									\
+	log_memop(vb, op);						\
+	err = _q->mem_ops->op ? _q->mem_ops->op(args) : 0;		\
+	if (!err)							\
+		(vb)->cnt_mem_ ## op++;					\
+	err;								\
+})
+
+#define call_ptr_memop(vb, op, args...)					\
+({									\
+	struct vb2_queue *_q = (vb)->vb2_queue;				\
+	void *ptr;							\
+									\
+	log_memop(vb, op);						\
+	ptr = _q->mem_ops->op ? _q->mem_ops->op(args) : NULL;		\
+	if (!IS_ERR_OR_NULL(ptr))					\
+		(vb)->cnt_mem_ ## op++;					\
+	ptr;								\
+})
+
+#define call_void_memop(vb, op, args...)				\
+({									\
+	struct vb2_queue *_q = (vb)->vb2_queue;				\
+									\
+	log_memop(vb, op);						\
+	if (_q->mem_ops->op)						\
+		_q->mem_ops->op(args);					\
+	(vb)->cnt_mem_ ## op++;						\
+})
+
+#define log_qop(q, op)							\
+	dprintk(2, "call_qop(%p, %s)%s\n", q, #op,			\
+		(q)->ops->op ? "" : " (nop)")
+
+#define call_qop(q, op, args...)					\
+({									\
+	int err;							\
+									\
+	log_qop(q, op);							\
+	err = (q)->ops->op ? (q)->ops->op(args) : 0;			\
+	if (!err)							\
+		(q)->cnt_ ## op++;					\
+	err;								\
+})
+
+#define call_void_qop(q, op, args...)					\
+({									\
+	log_qop(q, op);							\
+	if ((q)->ops->op)						\
+		(q)->ops->op(args);					\
+	(q)->cnt_ ## op++;						\
+})
+
+#define log_vb_qop(vb, op, args...)					\
+	dprintk(2, "call_vb_qop(%p, %d, %s)%s\n",			\
+		(vb)->vb2_queue, (vb)->index, #op,			\
+		(vb)->vb2_queue->ops->op ? "" : " (nop)")
+
+#define call_vb_qop(vb, op, args...)					\
+({									\
+	int err;							\
+									\
+	log_vb_qop(vb, op);						\
+	err = (vb)->vb2_queue->ops->op ?				\
+		(vb)->vb2_queue->ops->op(args) : 0;			\
+	if (!err)							\
+		(vb)->cnt_ ## op++;					\
+	err;								\
+})
+
+#define call_void_vb_qop(vb, op, args...)				\
+({									\
+	log_vb_qop(vb, op);						\
+	if ((vb)->vb2_queue->ops->op)					\
+		(vb)->vb2_queue->ops->op(args);				\
+	(vb)->cnt_ ## op++;						\
+})
+
+#else
+
+#define call_memop(vb, op, args...)					\
+	((vb)->vb2_queue->mem_ops->op ?					\
+		(vb)->vb2_queue->mem_ops->op(args) : 0)
+
+#define call_ptr_memop(vb, op, args...)					\
+	((vb)->vb2_queue->mem_ops->op ?					\
+		(vb)->vb2_queue->mem_ops->op(args) : NULL)
+
+#define call_void_memop(vb, op, args...)				\
+	do {								\
+		if ((vb)->vb2_queue->mem_ops->op)			\
+			(vb)->vb2_queue->mem_ops->op(args);		\
+	} while (0)
+
+#define call_qop(q, op, args...)					\
+	((q)->ops->op ? (q)->ops->op(args) : 0)
+
+#define call_void_qop(q, op, args...)					\
+	do {								\
+		if ((q)->ops->op)					\
+			(q)->ops->op(args);				\
+	} while (0)
+
+#define call_vb_qop(vb, op, args...)					\
+	((vb)->vb2_queue->ops->op ? (vb)->vb2_queue->ops->op(args) : 0)
+
+#define call_void_vb_qop(vb, op, args...)				\
+	do {								\
+		if ((vb)->vb2_queue->ops->op)				\
+			(vb)->vb2_queue->ops->op(args);			\
+	} while (0)
+
+#endif
+
+#define call_bufop(q, op, args...)					\
+({									\
+	int ret = 0;							\
+	if (q && q->buf_ops && q->buf_ops->op)				\
+		ret = q->buf_ops->op(args);				\
+	ret;								\
+})
+
+#define call_void_bufop(q, op, args...)					\
+({									\
+	if (q && q->buf_ops && q->buf_ops->op)				\
+		q->buf_ops->op(args);					\
+})
 
 static void __vb2_queue_cancel(struct vb2_queue *q);
 static void __enqueue_in_driver(struct vb2_buffer *vb);
@@ -53,7 +203,7 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 	 * NOTE: mmapped areas should be page aligned
 	 */
 	for (plane = 0; plane < vb->num_planes; ++plane) {
-		unsigned long size = PAGE_ALIGN(q->plane_sizes[plane]);
+		unsigned long size = PAGE_ALIGN(vb->planes[plane].length);
 
 		mem_priv = call_ptr_memop(vb, alloc, q->alloc_ctx[plane],
 				      size, dma_dir, q->gfp_flags);
@@ -62,7 +212,6 @@ static int __vb2_buf_mem_alloc(struct vb2_buffer *vb)
 
 		/* Associate allocator private data with this plane */
 		vb->planes[plane].mem_priv = mem_priv;
-		vb->planes[plane].length = q->plane_sizes[plane];
 	}
 
 	return 0;
@@ -137,57 +286,30 @@ static void __vb2_buf_dmabuf_put(struct vb2_buffer *vb)
 }
 
 /**
- * __setup_lengths() - setup initial lengths for every plane in
- * every buffer on the queue
- */
-static void __setup_lengths(struct vb2_queue *q, unsigned int n)
-{
-	unsigned int buffer, plane;
-	struct vb2_buffer *vb;
-
-	for (buffer = q->num_buffers; buffer < q->num_buffers + n; ++buffer) {
-		vb = q->bufs[buffer];
-		if (!vb)
-			continue;
-
-		for (plane = 0; plane < vb->num_planes; ++plane)
-			vb->planes[plane].length = q->plane_sizes[plane];
-	}
-}
-
-/**
  * __setup_offsets() - setup unique offsets ("cookies") for every plane in
- * every buffer on the queue
+ * the buffer.
  */
-static void __setup_offsets(struct vb2_queue *q, unsigned int n)
+static void __setup_offsets(struct vb2_buffer *vb)
 {
-	unsigned int buffer, plane;
-	struct vb2_buffer *vb;
-	unsigned long off;
+	struct vb2_queue *q = vb->vb2_queue;
+	unsigned int plane;
+	unsigned long off = 0;
+
+	if (vb->index) {
+		struct vb2_buffer *prev = q->bufs[vb->index - 1];
+		struct vb2_plane *p = &prev->planes[prev->num_planes - 1];
 
-	if (q->num_buffers) {
-		struct vb2_plane *p;
-		vb = q->bufs[q->num_buffers - 1];
-		p = &vb->planes[vb->num_planes - 1];
 		off = PAGE_ALIGN(p->m.offset + p->length);
-	} else {
-		off = 0;
 	}
 
-	for (buffer = q->num_buffers; buffer < q->num_buffers + n; ++buffer) {
-		vb = q->bufs[buffer];
-		if (!vb)
-			continue;
-
-		for (plane = 0; plane < vb->num_planes; ++plane) {
-			vb->planes[plane].m.offset = off;
+	for (plane = 0; plane < vb->num_planes; ++plane) {
+		vb->planes[plane].m.offset = off;
 
-			dprintk(3, "buffer %d, plane %d offset 0x%08lx\n",
-					buffer, plane, off);
+		dprintk(3, "buffer %d, plane %d offset 0x%08lx\n",
+				vb->index, plane, off);
 
-			off += vb->planes[plane].length;
-			off = PAGE_ALIGN(off);
-		}
+		off += vb->planes[plane].length;
+		off = PAGE_ALIGN(off);
 	}
 }
 
@@ -199,9 +321,10 @@ static void __setup_offsets(struct vb2_queue *q, unsigned int n)
  * Returns the number of buffers successfully allocated.
  */
 static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
-			     unsigned int num_buffers, unsigned int num_planes)
+			     unsigned int num_buffers, unsigned int num_planes,
+			     const unsigned plane_sizes[VB2_MAX_PLANES])
 {
-	unsigned int buffer;
+	unsigned int buffer, plane;
 	struct vb2_buffer *vb;
 	int ret;
 
@@ -219,6 +342,11 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 		vb->index = q->num_buffers + buffer;
 		vb->type = q->type;
 		vb->memory = memory;
+		for (plane = 0; plane < num_planes; ++plane) {
+			vb->planes[plane].length = plane_sizes[plane];
+			vb->planes[plane].min_length = plane_sizes[plane];
+		}
+		q->bufs[vb->index] = vb;
 
 		/* Allocate video buffer memory for the MMAP type */
 		if (memory == VB2_MEMORY_MMAP) {
@@ -226,9 +354,11 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 			if (ret) {
 				dprintk(1, "failed allocating memory for "
 						"buffer %d\n", buffer);
+				q->bufs[vb->index] = NULL;
 				kfree(vb);
 				break;
 			}
+			__setup_offsets(vb);
 			/*
 			 * Call the driver-provided buffer initialization
 			 * callback, if given. An error in initialization
@@ -239,18 +369,13 @@ static int __vb2_queue_alloc(struct vb2_queue *q, enum vb2_memory memory,
 				dprintk(1, "buffer %d %p initialization"
 					" failed\n", buffer, vb);
 				__vb2_buf_mem_free(vb);
+				q->bufs[vb->index] = NULL;
 				kfree(vb);
 				break;
 			}
 		}
-
-		q->bufs[q->num_buffers + buffer] = vb;
 	}
 
-	__setup_lengths(q, buffer);
-	if (memory == VB2_MEMORY_MMAP)
-		__setup_offsets(q, buffer);
-
 	dprintk(1, "allocated %d buffers, %d plane(s) each\n",
 			buffer, num_planes);
 
@@ -330,7 +455,7 @@ static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 		bool unbalanced = q->cnt_start_streaming != q->cnt_stop_streaming ||
 				  q->cnt_wait_prepare != q->cnt_wait_finish;
 
-		if (unbalanced || vb2_debug) {
+		if (unbalanced || debug) {
 			pr_info("vb2: counters for queue %p:%s\n", q,
 				unbalanced ? " UNBALANCED!" : "");
 			pr_info("vb2:     setup: %u start_streaming: %u stop_streaming: %u\n",
@@ -356,7 +481,7 @@ static int __vb2_queue_free(struct vb2_queue *q, unsigned int buffers)
 				  vb->cnt_buf_prepare != vb->cnt_buf_finish ||
 				  vb->cnt_buf_init != vb->cnt_buf_cleanup;
 
-		if (unbalanced || vb2_debug) {
+		if (unbalanced || debug) {
 			pr_info("vb2:   counters for queue %p, buffer %d:%s\n",
 				q, buffer, unbalanced ? " UNBALANCED!" : "");
 			pr_info("vb2:     buf_init: %u buf_cleanup: %u buf_prepare: %u buf_finish: %u\n",
@@ -442,13 +567,10 @@ static bool __buffers_in_use(struct vb2_queue *q)
  * 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.
- *
- * The return values from this function are intended to be directly returned
- * from vidioc_querybuf handler in driver.
  */
-int vb2_core_querybuf(struct vb2_queue *q, unsigned int index, void *pb)
+void vb2_core_querybuf(struct vb2_queue *q, unsigned int index, void *pb)
 {
-	return call_bufop(q, fill_user_buffer, q->bufs[index], pb);
+	call_void_bufop(q, fill_user_buffer, q->bufs[index], pb);
 }
 EXPORT_SYMBOL_GPL(vb2_core_querybuf);
 
@@ -570,6 +692,7 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 		unsigned int *count)
 {
 	unsigned int num_buffers, allocated_buffers, num_planes = 0;
+	unsigned plane_sizes[VB2_MAX_PLANES] = { };
 	int ret;
 
 	if (q->streaming) {
@@ -613,7 +736,6 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 */
 	num_buffers = min_t(unsigned int, *count, VB2_MAX_FRAME);
 	num_buffers = max_t(unsigned int, num_buffers, q->min_buffers_needed);
-	memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 	memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 	q->memory = memory;
 
@@ -621,14 +743,14 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 * Ask the driver how many buffers and planes per buffer it requires.
 	 * Driver also sets the size and allocator context for each plane.
 	 */
-	ret = call_qop(q, queue_setup, q, NULL, &num_buffers, &num_planes,
-		       q->plane_sizes, q->alloc_ctx);
+	ret = call_qop(q, queue_setup, q, &num_buffers, &num_planes,
+		       plane_sizes, q->alloc_ctx);
 	if (ret)
 		return ret;
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers =
-		__vb2_queue_alloc(q, memory, num_buffers, num_planes);
+		__vb2_queue_alloc(q, memory, num_buffers, num_planes, plane_sizes);
 	if (allocated_buffers == 0) {
 		dprintk(1, "memory allocation failed\n");
 		return -ENOMEM;
@@ -646,9 +768,16 @@ int vb2_core_reqbufs(struct vb2_queue *q, enum vb2_memory memory,
 	 */
 	if (!ret && allocated_buffers < num_buffers) {
 		num_buffers = allocated_buffers;
+		/*
+		 * num_planes is set by the previous queue_setup(), but since it
+		 * signals to queue_setup() whether it is called from create_bufs()
+		 * vs reqbufs() we zero it here to signal that queue_setup() is
+		 * called for the reqbufs() case.
+		 */
+		num_planes = 0;
 
-		ret = call_qop(q, queue_setup, q, NULL, &num_buffers,
-			       &num_planes, q->plane_sizes, q->alloc_ctx);
+		ret = call_qop(q, queue_setup, q, &num_buffers,
+			       &num_planes, plane_sizes, q->alloc_ctx);
 
 		if (!ret && allocated_buffers < num_buffers)
 			ret = -ENOMEM;
@@ -701,9 +830,11 @@ EXPORT_SYMBOL_GPL(vb2_core_reqbufs);
  * from vidioc_create_bufs handler in driver.
  */
 int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
-		unsigned int *count, const void *parg)
+		unsigned int *count, unsigned requested_planes,
+		const unsigned requested_sizes[])
 {
 	unsigned int num_planes = 0, num_buffers, allocated_buffers;
+	unsigned plane_sizes[VB2_MAX_PLANES] = { };
 	int ret;
 
 	if (q->num_buffers == VB2_MAX_FRAME) {
@@ -712,7 +843,6 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 	}
 
 	if (!q->num_buffers) {
-		memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 		memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 		q->memory = memory;
 		q->waiting_for_buffers = !q->is_output;
@@ -720,18 +850,23 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 
 	num_buffers = min(*count, VB2_MAX_FRAME - q->num_buffers);
 
+	if (requested_planes && requested_sizes) {
+		num_planes = requested_planes;
+		memcpy(plane_sizes, requested_sizes, sizeof(plane_sizes));
+	}
+
 	/*
 	 * Ask the driver, whether the requested number of buffers, planes per
 	 * buffer and their sizes are acceptable
 	 */
-	ret = call_qop(q, queue_setup, q, parg, &num_buffers,
-		       &num_planes, q->plane_sizes, q->alloc_ctx);
+	ret = call_qop(q, queue_setup, q, &num_buffers,
+		       &num_planes, plane_sizes, q->alloc_ctx);
 	if (ret)
 		return ret;
 
 	/* Finally, allocate buffers and video memory */
 	allocated_buffers = __vb2_queue_alloc(q, memory, num_buffers,
-				num_planes);
+				num_planes, plane_sizes);
 	if (allocated_buffers == 0) {
 		dprintk(1, "memory allocation failed\n");
 		return -ENOMEM;
@@ -747,8 +882,8 @@ int vb2_core_create_bufs(struct vb2_queue *q, enum vb2_memory memory,
 		 * q->num_buffers contains the total number of buffers, that the
 		 * queue driver has set up
 		 */
-		ret = call_qop(q, queue_setup, q, parg, &num_buffers,
-			       &num_planes, q->plane_sizes, q->alloc_ctx);
+		ret = call_qop(q, queue_setup, q, &num_buffers,
+			       &num_planes, plane_sizes, q->alloc_ctx);
 
 		if (!ret && allocated_buffers < num_buffers)
 			ret = -ENOMEM;
@@ -928,8 +1063,11 @@ EXPORT_SYMBOL_GPL(vb2_discard_done);
  */
 static int __qbuf_mmap(struct vb2_buffer *vb, const void *pb)
 {
-	int ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
-			vb, pb, vb->planes);
+	int ret = 0;
+
+	if (pb)
+		ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
+				 vb, pb, vb->planes);
 	return ret ? ret : call_vb_qop(vb, buf_prepare, vb);
 }
 
@@ -942,14 +1080,16 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const void *pb)
 	struct vb2_queue *q = vb->vb2_queue;
 	void *mem_priv;
 	unsigned int plane;
-	int ret;
+	int ret = 0;
 	enum dma_data_direction dma_dir =
 		q->is_output ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
 	bool reacquired = vb->planes[0].mem_priv == NULL;
 
 	memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
 	/* Copy relevant information provided by the userspace */
-	ret = call_bufop(vb->vb2_queue, fill_vb2_buffer, vb, pb, planes);
+	if (pb)
+		ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
+				 vb, pb, planes);
 	if (ret)
 		return ret;
 
@@ -964,11 +1104,12 @@ static int __qbuf_userptr(struct vb2_buffer *vb, const void *pb)
 				"reacquiring memory\n", plane);
 
 		/* Check if the provided plane buffer is large enough */
-		if (planes[plane].length < q->plane_sizes[plane]) {
+		if (planes[plane].length < vb->planes[plane].min_length) {
 			dprintk(1, "provided buffer size %u is less than "
 						"setup size %u for plane %d\n",
 						planes[plane].length,
-						q->plane_sizes[plane], plane);
+						vb->planes[plane].min_length,
+						plane);
 			ret = -EINVAL;
 			goto err;
 		}
@@ -1056,14 +1197,16 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
 	struct vb2_queue *q = vb->vb2_queue;
 	void *mem_priv;
 	unsigned int plane;
-	int ret;
+	int ret = 0;
 	enum dma_data_direction dma_dir =
 		q->is_output ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
 	bool reacquired = vb->planes[0].mem_priv == NULL;
 
 	memset(planes, 0, sizeof(planes[0]) * vb->num_planes);
 	/* Copy relevant information provided by the userspace */
-	ret = call_bufop(vb->vb2_queue, fill_vb2_buffer, vb, pb, planes);
+	if (pb)
+		ret = call_bufop(vb->vb2_queue, fill_vb2_buffer,
+				 vb, pb, planes);
 	if (ret)
 		return ret;
 
@@ -1081,7 +1224,7 @@ static int __qbuf_dmabuf(struct vb2_buffer *vb, const void *pb)
 		if (planes[plane].length == 0)
 			planes[plane].length = dbuf->size;
 
-		if (planes[plane].length < q->plane_sizes[plane]) {
+		if (planes[plane].length < vb->planes[plane].min_length) {
 			dprintk(1, "invalid dmabuf length for plane %d\n",
 				plane);
 			ret = -EINVAL;
@@ -1263,9 +1406,7 @@ int vb2_core_prepare_buf(struct vb2_queue *q, unsigned int index, void *pb)
 		return ret;
 
 	/* Fill buffer information for the userspace */
-	ret = call_bufop(q, fill_user_buffer, vb, pb);
-	if (ret)
-		return ret;
+	call_void_bufop(q, fill_user_buffer, vb, pb);
 
 	dprintk(1, "prepare of buffer %d succeeded\n", vb->index);
 
@@ -1386,7 +1527,8 @@ int vb2_core_qbuf(struct vb2_queue *q, unsigned int index, void *pb)
 	q->waiting_for_buffers = false;
 	vb->state = VB2_BUF_STATE_QUEUED;
 
-	call_bufop(q, set_timestamp, vb, pb);
+	if (pb)
+		call_void_bufop(q, copy_timestamp, vb, pb);
 
 	trace_vb2_qbuf(q, vb);
 
@@ -1398,9 +1540,8 @@ int vb2_core_qbuf(struct vb2_queue *q, unsigned int index, void *pb)
 		__enqueue_in_driver(vb);
 
 	/* Fill buffer information for the userspace */
-	ret = call_bufop(q, fill_user_buffer, vb, pb);
-	if (ret)
-		return ret;
+	if (pb)
+		call_void_bufop(q, fill_user_buffer, vb, pb);
 
 	/*
 	 * If streamon has been called, and we haven't yet called
@@ -1599,7 +1740,8 @@ static void __vb2_dqbuf(struct vb2_buffer *vb)
  * 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, void *pb, bool nonblocking)
+int vb2_core_dqbuf(struct vb2_queue *q, unsigned int *pindex, void *pb,
+		   bool nonblocking)
 {
 	struct vb2_buffer *vb = NULL;
 	int ret;
@@ -1622,10 +1764,12 @@ int vb2_core_dqbuf(struct vb2_queue *q, void *pb, bool nonblocking)
 
 	call_void_vb_qop(vb, buf_finish, vb);
 
+	if (pindex)
+		*pindex = vb->index;
+
 	/* Fill buffer information for the userspace */
-	ret = call_bufop(q, fill_user_buffer, vb, pb);
-	if (ret)
-		return ret;
+	if (pb)
+		call_void_bufop(q, fill_user_buffer, vb, pb);
 
 	/* Remove from videobuf queue */
 	list_del(&vb->queued_entry);
@@ -1698,7 +1842,7 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 	 * that's done in dqbuf, but that's not going to happen when we
 	 * cancel the whole queue. Note: this code belongs here, not in
 	 * __vb2_dqbuf() since in vb2_internal_dqbuf() there is a critical
-	 * call to __fill_v4l2_buffer() after buf_finish(). That order can't
+	 * call to __fill_user_buffer() after buf_finish(). That order can't
 	 * be changed, so we can't move the buf_finish() to __vb2_dqbuf().
 	 */
 	for (i = 0; i < q->num_buffers; ++i) {
@@ -2073,6 +2217,8 @@ int vb2_core_queue_init(struct vb2_queue *q)
 }
 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
@@ -2083,6 +2229,7 @@ EXPORT_SYMBOL_GPL(vb2_core_queue_init);
  */
 void vb2_core_queue_release(struct vb2_queue *q)
 {
+	__vb2_cleanup_fileio(q);
 	__vb2_queue_cancel(q);
 	mutex_lock(&q->mmap_lock);
 	__vb2_queue_free(q, q->num_buffers);
@@ -2090,6 +2237,596 @@ void vb2_core_queue_release(struct vb2_queue *q)
 }
 EXPORT_SYMBOL_GPL(vb2_core_queue_release);
 
-MODULE_DESCRIPTION("Driver helper framework for Video for Linux 2");
+/**
+ * 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)
+{
+	unsigned long req_events = poll_requested_events(wait);
+	struct vb2_buffer *vb = NULL;
+	unsigned long flags;
+
+	if (!q->is_output && !(req_events & (POLLIN | POLLRDNORM)))
+		return 0;
+	if (q->is_output && !(req_events & (POLLOUT | POLLWRNORM)))
+		return 0;
+
+	/*
+	 * Start file I/O emulator only if streaming API has not been used yet.
+	 */
+	if (q->num_buffers == 0 && !vb2_fileio_is_active(q)) {
+		if (!q->is_output && (q->io_modes & VB2_READ) &&
+				(req_events & (POLLIN | POLLRDNORM))) {
+			if (__vb2_init_fileio(q, 1))
+				return POLLERR;
+		}
+		if (q->is_output && (q->io_modes & VB2_WRITE) &&
+				(req_events & (POLLOUT | POLLWRNORM))) {
+			if (__vb2_init_fileio(q, 0))
+				return POLLERR;
+			/*
+			 * Write to OUTPUT queue can be done immediately.
+			 */
+			return POLLOUT | POLLWRNORM;
+		}
+	}
+
+	/*
+	 * There is nothing to wait for if the queue isn't streaming, or if the
+	 * error flag is set.
+	 */
+	if (!vb2_is_streaming(q) || q->error)
+		return POLLERR;
+
+	/*
+	 * For output streams you can call write() as long as there are fewer
+	 * buffers queued than there are buffers available.
+	 */
+	if (q->is_output && q->fileio && q->queued_count < q->num_buffers)
+		return POLLOUT | POLLWRNORM;
+
+	if (list_empty(&q->done_list)) {
+		/*
+		 * If the last buffer was dequeued from a capture queue,
+		 * return immediately. DQBUF will return -EPIPE.
+		 */
+		if (q->last_buffer_dequeued)
+			return POLLIN | POLLRDNORM;
+
+		poll_wait(file, &q->done_wq, wait);
+	}
+
+	/*
+	 * Take first buffer available for dequeuing.
+	 */
+	spin_lock_irqsave(&q->done_lock, flags);
+	if (!list_empty(&q->done_list))
+		vb = list_first_entry(&q->done_list, struct vb2_buffer,
+					done_entry);
+	spin_unlock_irqrestore(&q->done_lock, flags);
+
+	if (vb && (vb->state == VB2_BUF_STATE_DONE
+			|| vb->state == VB2_BUF_STATE_ERROR)) {
+		return (q->is_output) ?
+				POLLOUT | POLLWRNORM :
+				POLLIN | POLLRDNORM;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(vb2_core_poll);
+
+/**
+ * struct vb2_fileio_buf - buffer context used by file io emulator
+ *
+ * vb2 provides a compatibility layer and emulator of file io (read and
+ * write) calls on top of streaming API. This structure is used for
+ * tracking context related to the buffers.
+ */
+struct vb2_fileio_buf {
+	void *vaddr;
+	unsigned int size;
+	unsigned int pos;
+	unsigned int queued:1;
+};
+
+/**
+ * struct vb2_fileio_data - queue context used by file io emulator
+ *
+ * @cur_index:	the index of the buffer currently being read from or
+ *		written to. If equal to q->num_buffers then a new buffer
+ *		must be dequeued.
+ * @initial_index: in the read() case all buffers are queued up immediately
+ *		in __vb2_init_fileio() and __vb2_perform_fileio() just cycles
+ *		buffers. However, in the write() case no buffers are initially
+ *		queued, instead whenever a buffer is full it is queued up by
+ *		__vb2_perform_fileio(). Only once all available buffers have
+ *		been queued up will __vb2_perform_fileio() start to dequeue
+ *		buffers. This means that initially __vb2_perform_fileio()
+ *		needs to know what buffer index to use when it is queuing up
+ *		the buffers for the first time. That initial index is stored
+ *		in this field. Once it is equal to q->num_buffers all
+ *		available buffers have been queued and __vb2_perform_fileio()
+ *		should start the normal dequeue/queue cycle.
+ *
+ * vb2 provides a compatibility layer and emulator of file io (read and
+ * write) calls on top of streaming API. For proper operation it required
+ * this structure to save the driver state between each call of the read
+ * or write function.
+ */
+struct vb2_fileio_data {
+	unsigned int count;
+	unsigned int type;
+	unsigned int memory;
+	struct vb2_fileio_buf bufs[VB2_MAX_FRAME];
+	unsigned int cur_index;
+	unsigned int initial_index;
+	unsigned int q_count;
+	unsigned int dq_count;
+	unsigned read_once:1;
+	unsigned write_immediately:1;
+};
+
+/**
+ * __vb2_init_fileio() - initialize file io emulator
+ * @q:		videobuf2 queue
+ * @read:	mode selector (1 means read, 0 means write)
+ */
+static int __vb2_init_fileio(struct vb2_queue *q, int read)
+{
+	struct vb2_fileio_data *fileio;
+	int i, ret;
+	unsigned int count = 0;
+
+	/*
+	 * Sanity check
+	 */
+	if (WARN_ON((read && !(q->io_modes & VB2_READ)) ||
+		    (!read && !(q->io_modes & VB2_WRITE))))
+		return -EINVAL;
+
+	/*
+	 * Check if device supports mapping buffers to kernel virtual space.
+	 */
+	if (!q->mem_ops->vaddr)
+		return -EBUSY;
+
+	/*
+	 * Check if streaming api has not been already activated.
+	 */
+	if (q->streaming || q->num_buffers > 0)
+		return -EBUSY;
+
+	/*
+	 * Start with count 1, driver can increase it in queue_setup()
+	 */
+	count = 1;
+
+	dprintk(3, "setting up file io: mode %s, count %d, read_once %d, write_immediately %d\n",
+		(read) ? "read" : "write", count, q->fileio_read_once,
+		q->fileio_write_immediately);
+
+	fileio = kzalloc(sizeof(*fileio), GFP_KERNEL);
+	if (fileio == NULL)
+		return -ENOMEM;
+
+	fileio->read_once = q->fileio_read_once;
+	fileio->write_immediately = q->fileio_write_immediately;
+
+	/*
+	 * Request buffers and use MMAP type to force driver
+	 * to allocate buffers by itself.
+	 */
+	fileio->count = count;
+	fileio->memory = VB2_MEMORY_MMAP;
+	fileio->type = q->type;
+	q->fileio = fileio;
+	ret = vb2_core_reqbufs(q, fileio->memory, &fileio->count);
+	if (ret)
+		goto err_kfree;
+
+	/*
+	 * Check if plane_count is correct
+	 * (multiplane buffers are not supported).
+	 */
+	if (q->bufs[0]->num_planes != 1) {
+		ret = -EBUSY;
+		goto err_reqbufs;
+	}
+
+	/*
+	 * Get kernel address of each buffer.
+	 */
+	for (i = 0; i < q->num_buffers; i++) {
+		fileio->bufs[i].vaddr = vb2_plane_vaddr(q->bufs[i], 0);
+		if (fileio->bufs[i].vaddr == NULL) {
+			ret = -EINVAL;
+			goto err_reqbufs;
+		}
+		fileio->bufs[i].size = vb2_plane_size(q->bufs[i], 0);
+	}
+
+	/*
+	 * Read mode requires pre queuing of all buffers.
+	 */
+	if (read) {
+		/*
+		 * Queue all buffers.
+		 */
+		for (i = 0; i < q->num_buffers; i++) {
+			ret = vb2_core_qbuf(q, i, NULL);
+			if (ret)
+				goto err_reqbufs;
+			fileio->bufs[i].queued = 1;
+		}
+		/*
+		 * All buffers have been queued, so mark that by setting
+		 * initial_index to q->num_buffers
+		 */
+		fileio->initial_index = q->num_buffers;
+		fileio->cur_index = q->num_buffers;
+	}
+
+	/*
+	 * Start streaming.
+	 */
+	ret = vb2_core_streamon(q, q->type);
+	if (ret)
+		goto err_reqbufs;
+
+	return ret;
+
+err_reqbufs:
+	fileio->count = 0;
+	vb2_core_reqbufs(q, fileio->memory, &fileio->count);
+
+err_kfree:
+	q->fileio = NULL;
+	kfree(fileio);
+	return ret;
+}
+
+/**
+ * __vb2_cleanup_fileio() - free resourced used by file io emulator
+ * @q:		videobuf2 queue
+ */
+static int __vb2_cleanup_fileio(struct vb2_queue *q)
+{
+	struct vb2_fileio_data *fileio = q->fileio;
+
+	if (fileio) {
+		vb2_core_streamoff(q, q->type);
+		q->fileio = NULL;
+		fileio->count = 0;
+		vb2_core_reqbufs(q, fileio->memory, &fileio->count);
+		kfree(fileio);
+		dprintk(3, "file io emulator closed\n");
+	}
+	return 0;
+}
+
+/**
+ * __vb2_perform_fileio() - perform a single file io (read or write) operation
+ * @q:		videobuf2 queue
+ * @data:	pointed to target userspace buffer
+ * @count:	number of bytes to read or write
+ * @ppos:	file handle position tracking pointer
+ * @nonblock:	mode selector (1 means blocking calls, 0 means nonblocking)
+ * @read:	access mode selector (1 means read, 0 means write)
+ */
+static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_t count,
+		loff_t *ppos, int nonblock, int read)
+{
+	struct vb2_fileio_data *fileio;
+	struct vb2_fileio_buf *buf;
+	bool is_multiplanar = q->is_multiplanar;
+	/*
+	 * When using write() to write data to an output video node the vb2 core
+	 * should copy timestamps if V4L2_BUF_FLAG_TIMESTAMP_COPY is set. Nobody
+	 * else is able to provide this information with the write() operation.
+	 */
+	bool copy_timestamp = !read && q->copy_timestamp;
+	unsigned index;
+	int ret;
+
+	dprintk(3, "mode %s, offset %ld, count %zd, %sblocking\n",
+		read ? "read" : "write", (long)*ppos, count,
+		nonblock ? "non" : "");
+
+	if (!data)
+		return -EINVAL;
+
+	/*
+	 * Initialize emulator on first call.
+	 */
+	if (!vb2_fileio_is_active(q)) {
+		ret = __vb2_init_fileio(q, read);
+		dprintk(3, "vb2_init_fileio result: %d\n", ret);
+		if (ret)
+			return ret;
+	}
+	fileio = q->fileio;
+
+	/*
+	 * Check if we need to dequeue the buffer.
+	 */
+	index = fileio->cur_index;
+	if (index >= q->num_buffers) {
+		struct vb2_buffer *b;
+
+		/*
+		 * Call vb2_dqbuf to get buffer back.
+		 */
+		ret = vb2_core_dqbuf(q, &index, NULL, nonblock);
+		dprintk(5, "vb2_dqbuf result: %d\n", ret);
+		if (ret)
+			return ret;
+		fileio->dq_count += 1;
+
+		fileio->cur_index = index;
+		buf = &fileio->bufs[index];
+		b = q->bufs[index];
+
+		/*
+		 * Get number of bytes filled by the driver
+		 */
+		buf->pos = 0;
+		buf->queued = 0;
+		buf->size = read ? vb2_get_plane_payload(q->bufs[index], 0)
+				 : vb2_plane_size(q->bufs[index], 0);
+		/* Compensate for data_offset on read in the multiplanar case. */
+		if (is_multiplanar && read &&
+				b->planes[0].data_offset < buf->size) {
+			buf->pos = b->planes[0].data_offset;
+			buf->size -= buf->pos;
+		}
+	} else {
+		buf = &fileio->bufs[index];
+	}
+
+	/*
+	 * Limit count on last few bytes of the buffer.
+	 */
+	if (buf->pos + count > buf->size) {
+		count = buf->size - buf->pos;
+		dprintk(5, "reducing read count: %zd\n", count);
+	}
+
+	/*
+	 * Transfer data to userspace.
+	 */
+	dprintk(3, "copying %zd bytes - buffer %d, offset %u\n",
+		count, index, buf->pos);
+	if (read)
+		ret = copy_to_user(data, buf->vaddr + buf->pos, count);
+	else
+		ret = copy_from_user(buf->vaddr + buf->pos, data, count);
+	if (ret) {
+		dprintk(3, "error copying data\n");
+		return -EFAULT;
+	}
+
+	/*
+	 * Update counters.
+	 */
+	buf->pos += count;
+	*ppos += count;
+
+	/*
+	 * Queue next buffer if required.
+	 */
+	if (buf->pos == buf->size || (!read && fileio->write_immediately)) {
+		struct vb2_buffer *b = q->bufs[index];
+
+		/*
+		 * Check if this is the last buffer to read.
+		 */
+		if (read && fileio->read_once && fileio->dq_count == 1) {
+			dprintk(3, "read limit reached\n");
+			return __vb2_cleanup_fileio(q);
+		}
+
+		/*
+		 * Call vb2_qbuf and give buffer to the driver.
+		 */
+		b->planes[0].bytesused = buf->pos;
+
+		if (copy_timestamp)
+			b->timestamp = ktime_get_ns();
+		ret = vb2_core_qbuf(q, index, NULL);
+		dprintk(5, "vb2_dbuf result: %d\n", ret);
+		if (ret)
+			return ret;
+
+		/*
+		 * Buffer has been queued, update the status
+		 */
+		buf->pos = 0;
+		buf->queued = 1;
+		buf->size = vb2_plane_size(q->bufs[index], 0);
+		fileio->q_count += 1;
+		/*
+		 * If we are queuing up buffers for the first time, then
+		 * increase initial_index by one.
+		 */
+		if (fileio->initial_index < q->num_buffers)
+			fileio->initial_index++;
+		/*
+		 * The next buffer to use is either a buffer that's going to be
+		 * queued for the first time (initial_index < q->num_buffers)
+		 * or it is equal to q->num_buffers, meaning that the next
+		 * time we need to dequeue a buffer since we've now queued up
+		 * all the 'first time' buffers.
+		 */
+		fileio->cur_index = fileio->initial_index;
+	}
+
+	/*
+	 * Return proper number of bytes processed.
+	 */
+	if (ret == 0)
+		ret = count;
+	return ret;
+}
+
+size_t vb2_read(struct vb2_queue *q, char __user *data, size_t count,
+		loff_t *ppos, int nonblocking)
+{
+	return __vb2_perform_fileio(q, data, count, ppos, nonblocking, 1);
+}
+EXPORT_SYMBOL_GPL(vb2_read);
+
+size_t vb2_write(struct vb2_queue *q, const char __user *data, size_t count,
+		loff_t *ppos, int nonblocking)
+{
+	return __vb2_perform_fileio(q, (char __user *) data, count,
+							ppos, nonblocking, 0);
+}
+EXPORT_SYMBOL_GPL(vb2_write);
+
+struct vb2_threadio_data {
+	struct task_struct *thread;
+	vb2_thread_fnc fnc;
+	void *priv;
+	bool stop;
+};
+
+static int vb2_thread(void *data)
+{
+	struct vb2_queue *q = data;
+	struct vb2_threadio_data *threadio = q->threadio;
+	bool copy_timestamp = false;
+	unsigned prequeue = 0;
+	unsigned index = 0;
+	int ret = 0;
+
+	if (q->is_output) {
+		prequeue = q->num_buffers;
+		copy_timestamp = q->copy_timestamp;
+	}
+
+	set_freezable();
+
+	for (;;) {
+		struct vb2_buffer *vb;
+
+		/*
+		 * Call vb2_dqbuf to get buffer back.
+		 */
+		if (prequeue) {
+			vb = q->bufs[index++];
+			prequeue--;
+		} else {
+			call_void_qop(q, wait_finish, q);
+			if (!threadio->stop)
+				ret = vb2_core_dqbuf(q, &index, NULL, 0);
+			call_void_qop(q, wait_prepare, q);
+			dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
+			if (!ret)
+				vb = q->bufs[index];
+		}
+		if (ret || threadio->stop)
+			break;
+		try_to_freeze();
+
+		if (vb->state != VB2_BUF_STATE_ERROR)
+			if (threadio->fnc(vb, threadio->priv))
+				break;
+		call_void_qop(q, wait_finish, q);
+		if (copy_timestamp)
+			vb->timestamp = ktime_get_ns();;
+		if (!threadio->stop)
+			ret = vb2_core_qbuf(q, vb->index, NULL);
+		call_void_qop(q, wait_prepare, q);
+		if (ret || threadio->stop)
+			break;
+	}
+
+	/* Hmm, linux becomes *very* unhappy without this ... */
+	while (!kthread_should_stop()) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule();
+	}
+	return 0;
+}
+
+/*
+ * This function should not be used for anything else but the videobuf2-dvb
+ * support. If you think you have another good use-case for this, then please
+ * contact the linux-media mailinglist first.
+ */
+int vb2_thread_start(struct vb2_queue *q, vb2_thread_fnc fnc, void *priv,
+		     const char *thread_name)
+{
+	struct vb2_threadio_data *threadio;
+	int ret = 0;
+
+	if (q->threadio)
+		return -EBUSY;
+	if (vb2_is_busy(q))
+		return -EBUSY;
+	if (WARN_ON(q->fileio))
+		return -EBUSY;
+
+	threadio = kzalloc(sizeof(*threadio), GFP_KERNEL);
+	if (threadio == NULL)
+		return -ENOMEM;
+	threadio->fnc = fnc;
+	threadio->priv = priv;
+
+	ret = __vb2_init_fileio(q, !q->is_output);
+	dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
+	if (ret)
+		goto nomem;
+	q->threadio = threadio;
+	threadio->thread = kthread_run(vb2_thread, q, "vb2-%s", thread_name);
+	if (IS_ERR(threadio->thread)) {
+		ret = PTR_ERR(threadio->thread);
+		threadio->thread = NULL;
+		goto nothread;
+	}
+	return 0;
+
+nothread:
+	__vb2_cleanup_fileio(q);
+nomem:
+	kfree(threadio);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(vb2_thread_start);
+
+int vb2_thread_stop(struct vb2_queue *q)
+{
+	struct vb2_threadio_data *threadio = q->threadio;
+	int err;
+
+	if (threadio == NULL)
+		return 0;
+	threadio->stop = true;
+	/* Wake up all pending sleeps in the thread */
+	vb2_queue_error(q);
+	err = kthread_stop(threadio->thread);
+	__vb2_cleanup_fileio(q);
+	threadio->thread = NULL;
+	kfree(threadio);
+	q->threadio = NULL;
+	return err;
+}
+EXPORT_SYMBOL_GPL(vb2_thread_stop);
+
+MODULE_DESCRIPTION("Media buffer core framework");
 MODULE_AUTHOR("Pawel Osciak <pawel@osciak.com>, Marek Szyprowski");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/v4l2-core/videobuf2-internal.h b/drivers/media/v4l2-core/videobuf2-internal.h
deleted file mode 100644
index 79018c749282..000000000000
--- a/drivers/media/v4l2-core/videobuf2-internal.h
+++ /dev/null
@@ -1,161 +0,0 @@
-#ifndef _MEDIA_VIDEOBUF2_INTERNAL_H
-#define _MEDIA_VIDEOBUF2_INTERNAL_H
-
-#include <linux/err.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <media/videobuf2-core.h>
-
-extern int vb2_debug;
-
-#define dprintk(level, fmt, arg...)					      \
-	do {								      \
-		if (vb2_debug >= level)					      \
-			pr_info("vb2: %s: " fmt, __func__, ## arg); \
-	} while (0)
-
-#ifdef CONFIG_VIDEO_ADV_DEBUG
-
-/*
- * If advanced debugging is on, then count how often each op is called
- * successfully, which can either be per-buffer or per-queue.
- *
- * This makes it easy to check that the 'init' and 'cleanup'
- * (and variations thereof) stay balanced.
- */
-
-#define log_memop(vb, op)						\
-	dprintk(2, "call_memop(%p, %d, %s)%s\n",			\
-		(vb)->vb2_queue, (vb)->index, #op,			\
-		(vb)->vb2_queue->mem_ops->op ? "" : " (nop)")
-
-#define call_memop(vb, op, args...)					\
-({									\
-	struct vb2_queue *_q = (vb)->vb2_queue;				\
-	int err;							\
-									\
-	log_memop(vb, op);						\
-	err = _q->mem_ops->op ? _q->mem_ops->op(args) : 0;		\
-	if (!err)							\
-		(vb)->cnt_mem_ ## op++;					\
-	err;								\
-})
-
-#define call_ptr_memop(vb, op, args...)					\
-({									\
-	struct vb2_queue *_q = (vb)->vb2_queue;				\
-	void *ptr;							\
-									\
-	log_memop(vb, op);						\
-	ptr = _q->mem_ops->op ? _q->mem_ops->op(args) : NULL;		\
-	if (!IS_ERR_OR_NULL(ptr))					\
-		(vb)->cnt_mem_ ## op++;					\
-	ptr;								\
-})
-
-#define call_void_memop(vb, op, args...)				\
-({									\
-	struct vb2_queue *_q = (vb)->vb2_queue;				\
-									\
-	log_memop(vb, op);						\
-	if (_q->mem_ops->op)						\
-		_q->mem_ops->op(args);					\
-	(vb)->cnt_mem_ ## op++;						\
-})
-
-#define log_qop(q, op)							\
-	dprintk(2, "call_qop(%p, %s)%s\n", q, #op,			\
-		(q)->ops->op ? "" : " (nop)")
-
-#define call_qop(q, op, args...)					\
-({									\
-	int err;							\
-									\
-	log_qop(q, op);							\
-	err = (q)->ops->op ? (q)->ops->op(args) : 0;			\
-	if (!err)							\
-		(q)->cnt_ ## op++;					\
-	err;								\
-})
-
-#define call_void_qop(q, op, args...)					\
-({									\
-	log_qop(q, op);							\
-	if ((q)->ops->op)						\
-		(q)->ops->op(args);					\
-	(q)->cnt_ ## op++;						\
-})
-
-#define log_vb_qop(vb, op, args...)					\
-	dprintk(2, "call_vb_qop(%p, %d, %s)%s\n",			\
-		(vb)->vb2_queue, (vb)->index, #op,			\
-		(vb)->vb2_queue->ops->op ? "" : " (nop)")
-
-#define call_vb_qop(vb, op, args...)					\
-({									\
-	int err;							\
-									\
-	log_vb_qop(vb, op);						\
-	err = (vb)->vb2_queue->ops->op ?				\
-		(vb)->vb2_queue->ops->op(args) : 0;			\
-	if (!err)							\
-		(vb)->cnt_ ## op++;					\
-	err;								\
-})
-
-#define call_void_vb_qop(vb, op, args...)				\
-({									\
-	log_vb_qop(vb, op);						\
-	if ((vb)->vb2_queue->ops->op)					\
-		(vb)->vb2_queue->ops->op(args);				\
-	(vb)->cnt_ ## op++;						\
-})
-
-#else
-
-#define call_memop(vb, op, args...)					\
-	((vb)->vb2_queue->mem_ops->op ?					\
-		(vb)->vb2_queue->mem_ops->op(args) : 0)
-
-#define call_ptr_memop(vb, op, args...)					\
-	((vb)->vb2_queue->mem_ops->op ?					\
-		(vb)->vb2_queue->mem_ops->op(args) : NULL)
-
-#define call_void_memop(vb, op, args...)				\
-	do {								\
-		if ((vb)->vb2_queue->mem_ops->op)			\
-			(vb)->vb2_queue->mem_ops->op(args);		\
-	} while (0)
-
-#define call_qop(q, op, args...)					\
-	((q)->ops->op ? (q)->ops->op(args) : 0)
-
-#define call_void_qop(q, op, args...)					\
-	do {								\
-		if ((q)->ops->op)					\
-			(q)->ops->op(args);				\
-	} while (0)
-
-#define call_vb_qop(vb, op, args...)					\
-	((vb)->vb2_queue->ops->op ? (vb)->vb2_queue->ops->op(args) : 0)
-
-#define call_void_vb_qop(vb, op, args...)				\
-	do {								\
-		if ((vb)->vb2_queue->ops->op)				\
-			(vb)->vb2_queue->ops->op(args);			\
-	} while (0)
-
-#endif
-
-#define call_bufop(q, op, args...)					\
-({									\
-	int ret = 0;							\
-	if (q && q->buf_ops && q->buf_ops->op)				\
-		ret = q->buf_ops->op(args);				\
-	ret;								\
-})
-
-bool vb2_buffer_in_use(struct vb2_queue *q, struct vb2_buffer *vb);
-int vb2_verify_memory_type(struct vb2_queue *q,
-		enum vb2_memory memory, unsigned int type);
-#endif /* _MEDIA_VIDEOBUF2_INTERNAL_H */
diff --git a/drivers/media/v4l2-core/videobuf2-v4l2.c b/drivers/media/v4l2-core/videobuf2-v4l2.c
index 27b4b9e7c0c2..91f552124050 100644
--- a/drivers/media/v4l2-core/videobuf2-v4l2.c
+++ b/drivers/media/v4l2-core/videobuf2-v4l2.c
@@ -31,7 +31,14 @@
 
 #include <media/videobuf2-v4l2.h>
 
-#include "videobuf2-internal.h"
+static int debug;
+module_param(debug, int, 0644);
+
+#define dprintk(level, fmt, arg...)					      \
+	do {								      \
+		if (debug >= level)					      \
+			pr_info("vb2-v4l2: %s: " fmt, __func__, ## arg); \
+	} while (0)
 
 /* Flags that are set by the vb2 core */
 #define V4L2_BUFFER_MASK_FLAGS	(V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_QUEUED | \
@@ -52,7 +59,7 @@ static int __verify_planes_array(struct vb2_buffer *vb, const struct v4l2_buffer
 		return 0;
 
 	/* Is memory for copying plane information present? */
-	if (NULL == b->m.planes) {
+	if (b->m.planes == NULL) {
 		dprintk(1, "multi-planar buffer passed but "
 			   "planes array not provided\n");
 		return -EINVAL;
@@ -107,7 +114,7 @@ static int __verify_length(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 	return 0;
 }
 
-static int __set_timestamp(struct vb2_buffer *vb, const void *pb)
+static void __copy_timestamp(struct vb2_buffer *vb, const void *pb)
 {
 	const struct v4l2_buffer *b = pb;
 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
@@ -118,14 +125,12 @@ static int __set_timestamp(struct vb2_buffer *vb, const void *pb)
 		 * For output buffers copy the timestamp if needed,
 		 * and the timecode field and flag if needed.
 		 */
-		if ((q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
-				V4L2_BUF_FLAG_TIMESTAMP_COPY)
-			vbuf->timestamp = b->timestamp;
+		if (q->copy_timestamp)
+			vb->timestamp = timeval_to_ns(&b->timestamp);
 		vbuf->flags |= b->flags & V4L2_BUF_FLAG_TIMECODE;
 		if (b->flags & V4L2_BUF_FLAG_TIMECODE)
 			vbuf->timecode = b->timecode;
 	}
-	return 0;
 };
 
 static void vb2_warn_zero_bytesused(struct vb2_buffer *vb)
@@ -176,7 +181,7 @@ static int vb2_queue_or_prepare_buf(struct vb2_queue *q, struct v4l2_buffer *b,
  * __fill_v4l2_buffer() - fill in a struct v4l2_buffer with information to be
  * returned to userspace
  */
-static int __fill_v4l2_buffer(struct vb2_buffer *vb, void *pb)
+static void __fill_v4l2_buffer(struct vb2_buffer *vb, void *pb)
 {
 	struct v4l2_buffer *b = pb;
 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
@@ -191,7 +196,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, void *pb)
 
 	b->flags = vbuf->flags;
 	b->field = vbuf->field;
-	b->timestamp = vbuf->timestamp;
+	b->timestamp = ns_to_timeval(vb->timestamp);
 	b->timecode = vbuf->timecode;
 	b->sequence = vbuf->sequence;
 	b->reserved2 = 0;
@@ -238,8 +243,7 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, void *pb)
 	 */
 	b->flags &= ~V4L2_BUFFER_MASK_FLAGS;
 	b->flags |= q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK;
-	if ((q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) !=
-	    V4L2_BUF_FLAG_TIMESTAMP_COPY) {
+	if (!q->copy_timestamp) {
 		/*
 		 * For non-COPY timestamps, drop timestamp source bits
 		 * and obtain the timestamp source from the queue.
@@ -272,7 +276,10 @@ static int __fill_v4l2_buffer(struct vb2_buffer *vb, void *pb)
 	if (vb2_buffer_in_use(q, vb))
 		b->flags |= V4L2_BUF_FLAG_MAPPED;
 
-	return 0;
+	if (!q->is_output &&
+		b->flags & V4L2_BUF_FLAG_DONE &&
+		b->flags & V4L2_BUF_FLAG_LAST)
+		q->last_buffer_dequeued = true;
 }
 
 /**
@@ -308,8 +315,7 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb,
 					"for an output buffer\n");
 		return -EINVAL;
 	}
-	vbuf->timestamp.tv_sec = 0;
-	vbuf->timestamp.tv_usec = 0;
+	vb->timestamp = 0;
 	vbuf->sequence = 0;
 
 	if (V4L2_TYPE_IS_MULTIPLANAR(b->type)) {
@@ -404,8 +410,7 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb,
 
 	/* Zero flags that the vb2 core handles */
 	vbuf->flags = b->flags & ~V4L2_BUFFER_MASK_FLAGS;
-	if ((vb->vb2_queue->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) !=
-	    V4L2_BUF_FLAG_TIMESTAMP_COPY || !V4L2_TYPE_IS_OUTPUT(b->type)) {
+	if (!vb->vb2_queue->copy_timestamp || !V4L2_TYPE_IS_OUTPUT(b->type)) {
 		/*
 		 * Non-COPY timestamps and non-OUTPUT queues will get
 		 * their timestamp and timestamp source flags from the
@@ -434,7 +439,7 @@ static int __fill_vb2_buffer(struct vb2_buffer *vb,
 static const struct vb2_buf_ops v4l2_buf_ops = {
 	.fill_user_buffer	= __fill_v4l2_buffer,
 	.fill_vb2_buffer	= __fill_vb2_buffer,
-	.set_timestamp		= __set_timestamp,
+	.copy_timestamp		= __copy_timestamp,
 };
 
 /**
@@ -466,8 +471,9 @@ int vb2_querybuf(struct vb2_queue *q, struct v4l2_buffer *b)
 	}
 	vb = q->bufs[b->index];
 	ret = __verify_planes_array(vb, b);
-
-	return ret ? ret : vb2_core_querybuf(q, b->index, b);
+	if (!ret)
+		vb2_core_querybuf(q, b->index, b);
+	return ret;
 }
 EXPORT_SYMBOL(vb2_querybuf);
 
@@ -525,14 +531,52 @@ EXPORT_SYMBOL_GPL(vb2_prepare_buf);
  */
 int vb2_create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create)
 {
-	int ret = vb2_verify_memory_type(q, create->memory,
-			create->format.type);
+	unsigned requested_planes = 1;
+	unsigned requested_sizes[VIDEO_MAX_PLANES];
+	struct v4l2_format *f = &create->format;
+	int ret = vb2_verify_memory_type(q, create->memory, f->type);
+	unsigned i;
 
 	create->index = q->num_buffers;
 	if (create->count == 0)
 		return ret != -EBUSY ? ret : 0;
+
+	switch (f->type) {
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+		requested_planes = f->fmt.pix_mp.num_planes;
+		if (requested_planes == 0 ||
+		    requested_planes > VIDEO_MAX_PLANES)
+			return -EINVAL;
+		for (i = 0; i < requested_planes; i++)
+			requested_sizes[i] =
+				f->fmt.pix_mp.plane_fmt[i].sizeimage;
+		break;
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		requested_sizes[0] = f->fmt.pix.sizeimage;
+		break;
+	case V4L2_BUF_TYPE_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_VBI_OUTPUT:
+		requested_sizes[0] = f->fmt.vbi.samples_per_line *
+			(f->fmt.vbi.count[0] + f->fmt.vbi.count[1]);
+		break;
+	case V4L2_BUF_TYPE_SLICED_VBI_CAPTURE:
+	case V4L2_BUF_TYPE_SLICED_VBI_OUTPUT:
+		requested_sizes[0] = f->fmt.sliced.io_size;
+		break;
+	case V4L2_BUF_TYPE_SDR_CAPTURE:
+	case V4L2_BUF_TYPE_SDR_OUTPUT:
+		requested_sizes[0] = f->fmt.sdr.buffersize;
+		break;
+	default:
+		return -EINVAL;
+	}
+	for (i = 0; i < requested_planes; i++)
+		if (requested_sizes[i] == 0)
+			return -EINVAL;
 	return ret ? ret : vb2_core_create_bufs(q, create->memory,
-		&create->count, &create->format);
+		&create->count, requested_planes, requested_sizes);
 }
 EXPORT_SYMBOL_GPL(vb2_create_bufs);
 
@@ -581,11 +625,7 @@ static int vb2_internal_dqbuf(struct vb2_queue *q, struct v4l2_buffer *b,
 		return -EINVAL;
 	}
 
-	ret = vb2_core_dqbuf(q, b, nonblocking);
-
-	if (!ret && !q->is_output &&
-			b->flags & V4L2_BUF_FLAG_LAST)
-		q->last_buffer_dequeued = true;
+	ret = vb2_core_dqbuf(q, NULL, b, nonblocking);
 
 	return ret;
 }
@@ -723,14 +763,13 @@ int vb2_queue_init(struct vb2_queue *q)
 	q->buf_ops = &v4l2_buf_ops;
 	q->is_multiplanar = V4L2_TYPE_IS_MULTIPLANAR(q->type);
 	q->is_output = V4L2_TYPE_IS_OUTPUT(q->type);
+	q->copy_timestamp = (q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK)
+			== V4L2_BUF_FLAG_TIMESTAMP_COPY;
 
 	return vb2_core_queue_init(q);
 }
 EXPORT_SYMBOL_GPL(vb2_queue_init);
 
-static int __vb2_init_fileio(struct vb2_queue *q, int read);
-static int __vb2_cleanup_fileio(struct vb2_queue *q);
-
 /**
  * vb2_queue_release() - stop streaming, release the queue and free memory
  * @q:		videobuf2 queue
@@ -741,7 +780,6 @@ static int __vb2_cleanup_fileio(struct vb2_queue *q);
  */
 void vb2_queue_release(struct vb2_queue *q)
 {
-	__vb2_cleanup_fileio(q);
 	vb2_core_queue_release(q);
 }
 EXPORT_SYMBOL_GPL(vb2_queue_release);
@@ -769,9 +807,7 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 {
 	struct video_device *vfd = video_devdata(file);
 	unsigned long req_events = poll_requested_events(wait);
-	struct vb2_buffer *vb = NULL;
 	unsigned int res = 0;
-	unsigned long flags;
 
 	if (test_bit(V4L2_FL_USES_V4L2_FH, &vfd->flags)) {
 		struct v4l2_fh *fh = file->private_data;
@@ -782,611 +818,18 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 			poll_wait(file, &fh->wait, wait);
 	}
 
-	if (!q->is_output && !(req_events & (POLLIN | POLLRDNORM)))
-		return res;
-	if (q->is_output && !(req_events & (POLLOUT | POLLWRNORM)))
-		return res;
-
-	/*
-	 * Start file I/O emulator only if streaming API has not been used yet.
-	 */
-	if (q->num_buffers == 0 && !vb2_fileio_is_active(q)) {
-		if (!q->is_output && (q->io_modes & VB2_READ) &&
-				(req_events & (POLLIN | POLLRDNORM))) {
-			if (__vb2_init_fileio(q, 1))
-				return res | POLLERR;
-		}
-		if (q->is_output && (q->io_modes & VB2_WRITE) &&
-				(req_events & (POLLOUT | POLLWRNORM))) {
-			if (__vb2_init_fileio(q, 0))
-				return res | POLLERR;
-			/*
-			 * Write to OUTPUT queue can be done immediately.
-			 */
-			return res | POLLOUT | POLLWRNORM;
-		}
-	}
-
-	/*
-	 * There is nothing to wait for if the queue isn't streaming, or if the
-	 * error flag is set.
-	 */
-	if (!vb2_is_streaming(q) || q->error)
-		return res | POLLERR;
 	/*
 	 * For compatibility with vb1: if QBUF hasn't been called yet, then
 	 * return POLLERR as well. This only affects capture queues, output
 	 * queues will always initialize waiting_for_buffers to false.
 	 */
-	if (q->waiting_for_buffers)
-		return res | POLLERR;
-
-	/*
-	 * For output streams you can write as long as there are fewer buffers
-	 * queued than there are buffers available.
-	 */
-	if (q->is_output && q->queued_count < q->num_buffers)
-		return res | POLLOUT | POLLWRNORM;
-
-	if (list_empty(&q->done_list)) {
-		/*
-		 * If the last buffer was dequeued from a capture queue,
-		 * return immediately. DQBUF will return -EPIPE.
-		 */
-		if (q->last_buffer_dequeued)
-			return res | POLLIN | POLLRDNORM;
-
-		poll_wait(file, &q->done_wq, wait);
-	}
+	if (q->waiting_for_buffers && (req_events & (POLLIN | POLLRDNORM)))
+		return POLLERR;
 
-	/*
-	 * Take first buffer available for dequeuing.
-	 */
-	spin_lock_irqsave(&q->done_lock, flags);
-	if (!list_empty(&q->done_list))
-		vb = list_first_entry(&q->done_list, struct vb2_buffer,
-					done_entry);
-	spin_unlock_irqrestore(&q->done_lock, flags);
-
-	if (vb && (vb->state == VB2_BUF_STATE_DONE
-			|| vb->state == VB2_BUF_STATE_ERROR)) {
-		return (q->is_output) ?
-				res | POLLOUT | POLLWRNORM :
-				res | POLLIN | POLLRDNORM;
-	}
-	return res;
+	return res | vb2_core_poll(q, file, wait);
 }
 EXPORT_SYMBOL_GPL(vb2_poll);
 
-/**
- * struct vb2_fileio_buf - buffer context used by file io emulator
- *
- * vb2 provides a compatibility layer and emulator of file io (read and
- * write) calls on top of streaming API. This structure is used for
- * tracking context related to the buffers.
- */
-struct vb2_fileio_buf {
-	void *vaddr;
-	unsigned int size;
-	unsigned int pos;
-	unsigned int queued:1;
-};
-
-/**
- * struct vb2_fileio_data - queue context used by file io emulator
- *
- * @cur_index:	the index of the buffer currently being read from or
- *		written to. If equal to q->num_buffers then a new buffer
- *		must be dequeued.
- * @initial_index: in the read() case all buffers are queued up immediately
- *		in __vb2_init_fileio() and __vb2_perform_fileio() just cycles
- *		buffers. However, in the write() case no buffers are initially
- *		queued, instead whenever a buffer is full it is queued up by
- *		__vb2_perform_fileio(). Only once all available buffers have
- *		been queued up will __vb2_perform_fileio() start to dequeue
- *		buffers. This means that initially __vb2_perform_fileio()
- *		needs to know what buffer index to use when it is queuing up
- *		the buffers for the first time. That initial index is stored
- *		in this field. Once it is equal to q->num_buffers all
- *		available buffers have been queued and __vb2_perform_fileio()
- *		should start the normal dequeue/queue cycle.
- *
- * vb2 provides a compatibility layer and emulator of file io (read and
- * write) calls on top of streaming API. For proper operation it required
- * this structure to save the driver state between each call of the read
- * or write function.
- */
-struct vb2_fileio_data {
-	struct v4l2_requestbuffers req;
-	struct v4l2_plane p;
-	struct v4l2_buffer b;
-	struct vb2_fileio_buf bufs[VB2_MAX_FRAME];
-	unsigned int cur_index;
-	unsigned int initial_index;
-	unsigned int q_count;
-	unsigned int dq_count;
-	unsigned read_once:1;
-	unsigned write_immediately:1;
-};
-
-/**
- * __vb2_init_fileio() - initialize file io emulator
- * @q:		videobuf2 queue
- * @read:	mode selector (1 means read, 0 means write)
- */
-static int __vb2_init_fileio(struct vb2_queue *q, int read)
-{
-	struct vb2_fileio_data *fileio;
-	int i, ret;
-	unsigned int count = 0;
-
-	/*
-	 * Sanity check
-	 */
-	if (WARN_ON((read && !(q->io_modes & VB2_READ)) ||
-		    (!read && !(q->io_modes & VB2_WRITE))))
-		return -EINVAL;
-
-	/*
-	 * Check if device supports mapping buffers to kernel virtual space.
-	 */
-	if (!q->mem_ops->vaddr)
-		return -EBUSY;
-
-	/*
-	 * Check if streaming api has not been already activated.
-	 */
-	if (q->streaming || q->num_buffers > 0)
-		return -EBUSY;
-
-	/*
-	 * Start with count 1, driver can increase it in queue_setup()
-	 */
-	count = 1;
-
-	dprintk(3, "setting up file io: mode %s, count %d, read_once %d, write_immediately %d\n",
-		(read) ? "read" : "write", count, q->fileio_read_once,
-		q->fileio_write_immediately);
-
-	fileio = kzalloc(sizeof(struct vb2_fileio_data), GFP_KERNEL);
-	if (fileio == NULL)
-		return -ENOMEM;
-
-	fileio->read_once = q->fileio_read_once;
-	fileio->write_immediately = q->fileio_write_immediately;
-
-	/*
-	 * Request buffers and use MMAP type to force driver
-	 * to allocate buffers by itself.
-	 */
-	fileio->req.count = count;
-	fileio->req.memory = VB2_MEMORY_MMAP;
-	fileio->req.type = q->type;
-	q->fileio = fileio;
-	ret = vb2_core_reqbufs(q, fileio->req.memory, &fileio->req.count);
-	if (ret)
-		goto err_kfree;
-
-	/*
-	 * Check if plane_count is correct
-	 * (multiplane buffers are not supported).
-	 */
-	if (q->bufs[0]->num_planes != 1) {
-		ret = -EBUSY;
-		goto err_reqbufs;
-	}
-
-	/*
-	 * Get kernel address of each buffer.
-	 */
-	for (i = 0; i < q->num_buffers; i++) {
-		fileio->bufs[i].vaddr = vb2_plane_vaddr(q->bufs[i], 0);
-		if (fileio->bufs[i].vaddr == NULL) {
-			ret = -EINVAL;
-			goto err_reqbufs;
-		}
-		fileio->bufs[i].size = vb2_plane_size(q->bufs[i], 0);
-	}
-
-	/*
-	 * Read mode requires pre queuing of all buffers.
-	 */
-	if (read) {
-		bool is_multiplanar = q->is_multiplanar;
-
-		/*
-		 * Queue all buffers.
-		 */
-		for (i = 0; i < q->num_buffers; i++) {
-			struct v4l2_buffer *b = &fileio->b;
-
-			memset(b, 0, sizeof(*b));
-			b->type = q->type;
-			if (is_multiplanar) {
-				memset(&fileio->p, 0, sizeof(fileio->p));
-				b->m.planes = &fileio->p;
-				b->length = 1;
-			}
-			b->memory = q->memory;
-			b->index = i;
-			ret = vb2_internal_qbuf(q, b);
-			if (ret)
-				goto err_reqbufs;
-			fileio->bufs[i].queued = 1;
-		}
-		/*
-		 * All buffers have been queued, so mark that by setting
-		 * initial_index to q->num_buffers
-		 */
-		fileio->initial_index = q->num_buffers;
-		fileio->cur_index = q->num_buffers;
-	}
-
-	/*
-	 * Start streaming.
-	 */
-	ret = vb2_core_streamon(q, q->type);
-	if (ret)
-		goto err_reqbufs;
-
-	return ret;
-
-err_reqbufs:
-	fileio->req.count = 0;
-	vb2_core_reqbufs(q, fileio->req.memory, &fileio->req.count);
-
-err_kfree:
-	q->fileio = NULL;
-	kfree(fileio);
-	return ret;
-}
-
-/**
- * __vb2_cleanup_fileio() - free resourced used by file io emulator
- * @q:		videobuf2 queue
- */
-static int __vb2_cleanup_fileio(struct vb2_queue *q)
-{
-	struct vb2_fileio_data *fileio = q->fileio;
-
-	if (fileio) {
-		vb2_core_streamoff(q, q->type);
-		q->fileio = NULL;
-		fileio->req.count = 0;
-		vb2_reqbufs(q, &fileio->req);
-		kfree(fileio);
-		dprintk(3, "file io emulator closed\n");
-	}
-	return 0;
-}
-
-/**
- * __vb2_perform_fileio() - perform a single file io (read or write) operation
- * @q:		videobuf2 queue
- * @data:	pointed to target userspace buffer
- * @count:	number of bytes to read or write
- * @ppos:	file handle position tracking pointer
- * @nonblock:	mode selector (1 means blocking calls, 0 means nonblocking)
- * @read:	access mode selector (1 means read, 0 means write)
- */
-static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_t count,
-		loff_t *ppos, int nonblock, int read)
-{
-	struct vb2_fileio_data *fileio;
-	struct vb2_fileio_buf *buf;
-	bool is_multiplanar = q->is_multiplanar;
-	/*
-	 * When using write() to write data to an output video node the vb2 core
-	 * should set timestamps if V4L2_BUF_FLAG_TIMESTAMP_COPY is set. Nobody
-	 * else is able to provide this information with the write() operation.
-	 */
-	bool set_timestamp = !read &&
-		(q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
-		V4L2_BUF_FLAG_TIMESTAMP_COPY;
-	int ret, index;
-
-	dprintk(3, "mode %s, offset %ld, count %zd, %sblocking\n",
-		read ? "read" : "write", (long)*ppos, count,
-		nonblock ? "non" : "");
-
-	if (!data)
-		return -EINVAL;
-
-	/*
-	 * Initialize emulator on first call.
-	 */
-	if (!vb2_fileio_is_active(q)) {
-		ret = __vb2_init_fileio(q, read);
-		dprintk(3, "vb2_init_fileio result: %d\n", ret);
-		if (ret)
-			return ret;
-	}
-	fileio = q->fileio;
-
-	/*
-	 * Check if we need to dequeue the buffer.
-	 */
-	index = fileio->cur_index;
-	if (index >= q->num_buffers) {
-		/*
-		 * Call vb2_dqbuf to get buffer back.
-		 */
-		memset(&fileio->b, 0, sizeof(fileio->b));
-		fileio->b.type = q->type;
-		fileio->b.memory = q->memory;
-		if (is_multiplanar) {
-			memset(&fileio->p, 0, sizeof(fileio->p));
-			fileio->b.m.planes = &fileio->p;
-			fileio->b.length = 1;
-		}
-		ret = vb2_internal_dqbuf(q, &fileio->b, nonblock);
-		dprintk(5, "vb2_dqbuf result: %d\n", ret);
-		if (ret)
-			return ret;
-		fileio->dq_count += 1;
-
-		fileio->cur_index = index = fileio->b.index;
-		buf = &fileio->bufs[index];
-
-		/*
-		 * Get number of bytes filled by the driver
-		 */
-		buf->pos = 0;
-		buf->queued = 0;
-		buf->size = read ? vb2_get_plane_payload(q->bufs[index], 0)
-				 : vb2_plane_size(q->bufs[index], 0);
-		/* Compensate for data_offset on read in the multiplanar case. */
-		if (is_multiplanar && read &&
-		    fileio->b.m.planes[0].data_offset < buf->size) {
-			buf->pos = fileio->b.m.planes[0].data_offset;
-			buf->size -= buf->pos;
-		}
-	} else {
-		buf = &fileio->bufs[index];
-	}
-
-	/*
-	 * Limit count on last few bytes of the buffer.
-	 */
-	if (buf->pos + count > buf->size) {
-		count = buf->size - buf->pos;
-		dprintk(5, "reducing read count: %zd\n", count);
-	}
-
-	/*
-	 * Transfer data to userspace.
-	 */
-	dprintk(3, "copying %zd bytes - buffer %d, offset %u\n",
-		count, index, buf->pos);
-	if (read)
-		ret = copy_to_user(data, buf->vaddr + buf->pos, count);
-	else
-		ret = copy_from_user(buf->vaddr + buf->pos, data, count);
-	if (ret) {
-		dprintk(3, "error copying data\n");
-		return -EFAULT;
-	}
-
-	/*
-	 * Update counters.
-	 */
-	buf->pos += count;
-	*ppos += count;
-
-	/*
-	 * Queue next buffer if required.
-	 */
-	if (buf->pos == buf->size || (!read && fileio->write_immediately)) {
-		/*
-		 * Check if this is the last buffer to read.
-		 */
-		if (read && fileio->read_once && fileio->dq_count == 1) {
-			dprintk(3, "read limit reached\n");
-			return __vb2_cleanup_fileio(q);
-		}
-
-		/*
-		 * Call vb2_qbuf and give buffer to the driver.
-		 */
-		memset(&fileio->b, 0, sizeof(fileio->b));
-		fileio->b.type = q->type;
-		fileio->b.memory = q->memory;
-		fileio->b.index = index;
-		fileio->b.bytesused = buf->pos;
-		if (is_multiplanar) {
-			memset(&fileio->p, 0, sizeof(fileio->p));
-			fileio->p.bytesused = buf->pos;
-			fileio->b.m.planes = &fileio->p;
-			fileio->b.length = 1;
-		}
-		if (set_timestamp)
-			v4l2_get_timestamp(&fileio->b.timestamp);
-		ret = vb2_internal_qbuf(q, &fileio->b);
-		dprintk(5, "vb2_dbuf result: %d\n", ret);
-		if (ret)
-			return ret;
-
-		/*
-		 * Buffer has been queued, update the status
-		 */
-		buf->pos = 0;
-		buf->queued = 1;
-		buf->size = vb2_plane_size(q->bufs[index], 0);
-		fileio->q_count += 1;
-		/*
-		 * If we are queuing up buffers for the first time, then
-		 * increase initial_index by one.
-		 */
-		if (fileio->initial_index < q->num_buffers)
-			fileio->initial_index++;
-		/*
-		 * The next buffer to use is either a buffer that's going to be
-		 * queued for the first time (initial_index < q->num_buffers)
-		 * or it is equal to q->num_buffers, meaning that the next
-		 * time we need to dequeue a buffer since we've now queued up
-		 * all the 'first time' buffers.
-		 */
-		fileio->cur_index = fileio->initial_index;
-	}
-
-	/*
-	 * Return proper number of bytes processed.
-	 */
-	if (ret == 0)
-		ret = count;
-	return ret;
-}
-
-size_t vb2_read(struct vb2_queue *q, char __user *data, size_t count,
-		loff_t *ppos, int nonblocking)
-{
-	return __vb2_perform_fileio(q, data, count, ppos, nonblocking, 1);
-}
-EXPORT_SYMBOL_GPL(vb2_read);
-
-size_t vb2_write(struct vb2_queue *q, const char __user *data, size_t count,
-		loff_t *ppos, int nonblocking)
-{
-	return __vb2_perform_fileio(q, (char __user *) data, count,
-							ppos, nonblocking, 0);
-}
-EXPORT_SYMBOL_GPL(vb2_write);
-
-struct vb2_threadio_data {
-	struct task_struct *thread;
-	vb2_thread_fnc fnc;
-	void *priv;
-	bool stop;
-};
-
-static int vb2_thread(void *data)
-{
-	struct vb2_queue *q = data;
-	struct vb2_threadio_data *threadio = q->threadio;
-	struct vb2_fileio_data *fileio = q->fileio;
-	bool set_timestamp = false;
-	int prequeue = 0;
-	int index = 0;
-	int ret = 0;
-
-	if (q->is_output) {
-		prequeue = q->num_buffers;
-		set_timestamp =
-			(q->timestamp_flags & V4L2_BUF_FLAG_TIMESTAMP_MASK) ==
-			V4L2_BUF_FLAG_TIMESTAMP_COPY;
-	}
-
-	set_freezable();
-
-	for (;;) {
-		struct vb2_buffer *vb;
-
-		/*
-		 * Call vb2_dqbuf to get buffer back.
-		 */
-		memset(&fileio->b, 0, sizeof(fileio->b));
-		fileio->b.type = q->type;
-		fileio->b.memory = q->memory;
-		if (prequeue) {
-			fileio->b.index = index++;
-			prequeue--;
-		} else {
-			call_void_qop(q, wait_finish, q);
-			if (!threadio->stop)
-				ret = vb2_internal_dqbuf(q, &fileio->b, 0);
-			call_void_qop(q, wait_prepare, q);
-			dprintk(5, "file io: vb2_dqbuf result: %d\n", ret);
-		}
-		if (ret || threadio->stop)
-			break;
-		try_to_freeze();
-
-		vb = q->bufs[fileio->b.index];
-		if (!(fileio->b.flags & V4L2_BUF_FLAG_ERROR))
-			if (threadio->fnc(vb, threadio->priv))
-				break;
-		call_void_qop(q, wait_finish, q);
-		if (set_timestamp)
-			v4l2_get_timestamp(&fileio->b.timestamp);
-		if (!threadio->stop)
-			ret = vb2_internal_qbuf(q, &fileio->b);
-		call_void_qop(q, wait_prepare, q);
-		if (ret || threadio->stop)
-			break;
-	}
-
-	/* Hmm, linux becomes *very* unhappy without this ... */
-	while (!kthread_should_stop()) {
-		set_current_state(TASK_INTERRUPTIBLE);
-		schedule();
-	}
-	return 0;
-}
-
-/*
- * This function should not be used for anything else but the videobuf2-dvb
- * support. If you think you have another good use-case for this, then please
- * contact the linux-media mailinglist first.
- */
-int vb2_thread_start(struct vb2_queue *q, vb2_thread_fnc fnc, void *priv,
-		     const char *thread_name)
-{
-	struct vb2_threadio_data *threadio;
-	int ret = 0;
-
-	if (q->threadio)
-		return -EBUSY;
-	if (vb2_is_busy(q))
-		return -EBUSY;
-	if (WARN_ON(q->fileio))
-		return -EBUSY;
-
-	threadio = kzalloc(sizeof(*threadio), GFP_KERNEL);
-	if (threadio == NULL)
-		return -ENOMEM;
-	threadio->fnc = fnc;
-	threadio->priv = priv;
-
-	ret = __vb2_init_fileio(q, !q->is_output);
-	dprintk(3, "file io: vb2_init_fileio result: %d\n", ret);
-	if (ret)
-		goto nomem;
-	q->threadio = threadio;
-	threadio->thread = kthread_run(vb2_thread, q, "vb2-%s", thread_name);
-	if (IS_ERR(threadio->thread)) {
-		ret = PTR_ERR(threadio->thread);
-		threadio->thread = NULL;
-		goto nothread;
-	}
-	return 0;
-
-nothread:
-	__vb2_cleanup_fileio(q);
-nomem:
-	kfree(threadio);
-	return ret;
-}
-EXPORT_SYMBOL_GPL(vb2_thread_start);
-
-int vb2_thread_stop(struct vb2_queue *q)
-{
-	struct vb2_threadio_data *threadio = q->threadio;
-	int err;
-
-	if (threadio == NULL)
-		return 0;
-	threadio->stop = true;
-	/* Wake up all pending sleeps in the thread */
-	vb2_queue_error(q);
-	err = kthread_stop(threadio->thread);
-	__vb2_cleanup_fileio(q);
-	threadio->thread = NULL;
-	kfree(threadio);
-	q->threadio = NULL;
-	return err;
-}
-EXPORT_SYMBOL_GPL(vb2_thread_stop);
-
 /*
  * The following functions are not part of the vb2 core API, but are helper
  * functions that plug into struct v4l2_ioctl_ops, struct v4l2_file_operations
@@ -1440,8 +883,8 @@ int vb2_ioctl_create_bufs(struct file *file, void *priv,
 		return res;
 	if (vb2_queue_is_busy(vdev, file))
 		return -EBUSY;
-	res = vb2_core_create_bufs(vdev->queue, p->memory, &p->count,
-			&p->format);
+
+	res = vb2_create_bufs(vdev->queue, p);
 	if (res == 0)
 		vdev->queue->owner = file->private_data;
 	return res;
diff --git a/drivers/memory/tegra/tegra124.c b/drivers/memory/tegra/tegra124.c
index 21e7255e3d96..5a58e440f4a7 100644
--- a/drivers/memory/tegra/tegra124.c
+++ b/drivers/memory/tegra/tegra124.c
@@ -1007,6 +1007,7 @@ static const struct tegra_smmu_soc tegra124_smmu_soc = {
 	.num_swgroups = ARRAY_SIZE(tegra124_swgroups),
 	.supports_round_robin_arbitration = true,
 	.supports_request_limit = true,
+	.num_tlb_lines = 32,
 	.num_asids = 128,
 };
 
diff --git a/drivers/memstick/core/ms_block.c b/drivers/memstick/core/ms_block.c
index 24f2f8473dee..84abf9d3c24e 100644
--- a/drivers/memstick/core/ms_block.c
+++ b/drivers/memstick/core/ms_block.c
@@ -1909,7 +1909,7 @@ static void msb_io_work(struct work_struct *work)
 		lba = blk_rq_pos(msb->req);
 
 		sector_div(lba, msb->page_size / 512);
-		page = do_div(lba, msb->pages_in_block);
+		page = sector_div(lba, msb->pages_in_block);
 
 		if (rq_data_dir(msb->req) == READ)
 			error = msb_do_read_request(msb, lba, page, sg,
diff --git a/drivers/mfd/88pm80x.c b/drivers/mfd/88pm80x.c
index 63445ea6b0bf..3f24ecbe2576 100644
--- a/drivers/mfd/88pm80x.c
+++ b/drivers/mfd/88pm80x.c
@@ -135,7 +135,7 @@ EXPORT_SYMBOL_GPL(pm80x_deinit);
 #ifdef CONFIG_PM_SLEEP
 static int pm80x_suspend(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct pm80x_chip *chip = i2c_get_clientdata(client);
 
 	if (chip && chip->wu_flag)
@@ -147,7 +147,7 @@ static int pm80x_suspend(struct device *dev)
 
 static int pm80x_resume(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct pm80x_chip *chip = i2c_get_clientdata(client);
 
 	if (chip && chip->wu_flag)
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 3269a9990b24..25e1aafae60c 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -705,10 +705,12 @@ int pm8606_osc_disable(struct pm860x_chip *chip, unsigned short client)
 			chip->osc_status);
 
 	mutex_lock(&chip->osc_lock);
-	/*Update voting status */
+	/* Update voting status */
 	chip->osc_vote &= ~(client);
-	/* If reference group is off and this is the last client to release
-	 * - turn off */
+	/*
+	 * If reference group is off and this is the last client to release
+	 * - turn off
+	 */
 	if ((chip->osc_status != PM8606_REF_GP_OSC_OFF) &&
 			(chip->osc_vote == REF_GP_NO_CLIENTS)) {
 		chip->osc_status = PM8606_REF_GP_OSC_UNKNOWN;
@@ -1218,7 +1220,7 @@ static int pm860x_remove(struct i2c_client *client)
 #ifdef CONFIG_PM_SLEEP
 static int pm860x_suspend(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct pm860x_chip *chip = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(dev) && chip->wakeup_flag)
@@ -1228,7 +1230,7 @@ static int pm860x_suspend(struct device *dev)
 
 static int pm860x_resume(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct pm860x_chip *chip = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(dev) && chip->wakeup_flag)
@@ -1265,6 +1267,7 @@ static struct i2c_driver pm860x_driver = {
 static int __init pm860x_i2c_init(void)
 {
 	int ret;
+
 	ret = i2c_add_driver(&pm860x_driver);
 	if (ret != 0)
 		pr_err("Failed to register 88PM860x I2C driver: %d\n", ret);
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index 4d92df6ef9fe..9ca66de0c1c1 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -211,7 +211,7 @@ config MFD_DA9062
 	  of the device.
 
 config MFD_DA9063
-	bool "Dialog Semiconductor DA9063 PMIC Support"
+	tristate "Dialog Semiconductor DA9063 PMIC Support"
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
@@ -1370,24 +1370,30 @@ config MFD_ARIZONA
 	bool
 
 config MFD_ARIZONA_I2C
-	tristate "Wolfson Microelectronics Arizona platform with I2C"
+	tristate "Cirrus Logic/Wolfson Microelectronics Arizona platform with I2C"
 	select MFD_ARIZONA
 	select MFD_CORE
 	select REGMAP_I2C
 	depends on I2C
 	help
-	  Support for the Wolfson Microelectronics Arizona platform audio SoC
-	  core functionality controlled via I2C.
+	  Support for the Cirrus Logic/Wolfson Microelectronics Arizona platform
+	  audio SoC core functionality controlled via I2C.
 
 config MFD_ARIZONA_SPI
-	tristate "Wolfson Microelectronics Arizona platform with SPI"
+	tristate "Cirrus Logic/Wolfson Microelectronics Arizona platform with SPI"
 	select MFD_ARIZONA
 	select MFD_CORE
 	select REGMAP_SPI
 	depends on SPI_MASTER
 	help
-	  Support for the Wolfson Microelectronics Arizona platform audio SoC
-	  core functionality controlled via I2C.
+	  Support for the Cirrus Logic/Wolfson Microelectronics Arizona platform
+	  audio SoC core functionality controlled via I2C.
+
+config MFD_CS47L24
+	bool "Cirrus Logic CS47L24 and WM1831"
+	depends on MFD_ARIZONA
+	help
+	  Support for Cirrus Logic CS47L24 and WM1831 low power audio SoC
 
 config MFD_WM5102
 	bool "Wolfson Microelectronics WM5102"
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index a8b76b81b467..0f230a6103f8 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -51,6 +51,9 @@ endif
 ifeq ($(CONFIG_MFD_WM8998),y)
 obj-$(CONFIG_MFD_ARIZONA)	+= wm8998-tables.o
 endif
+ifeq ($(CONFIG_MFD_CS47L24),y)
+obj-$(CONFIG_MFD_ARIZONA)	+= cs47l24-tables.o
+endif
 obj-$(CONFIG_MFD_WM8400)	+= wm8400-core.o
 wm831x-objs			:= wm831x-core.o wm831x-irq.o wm831x-otp.o
 wm831x-objs			+= wm831x-auxadc.o
@@ -61,7 +64,8 @@ wm8350-objs			:= wm8350-core.o wm8350-regmap.o wm8350-gpio.o
 wm8350-objs			+= wm8350-irq.o
 obj-$(CONFIG_MFD_WM8350)	+= wm8350.o
 obj-$(CONFIG_MFD_WM8350_I2C)	+= wm8350-i2c.o
-obj-$(CONFIG_MFD_WM8994)	+= wm8994-core.o wm8994-irq.o wm8994-regmap.o
+wm8994-objs			:= wm8994-core.o wm8994-irq.o wm8994-regmap.o
+obj-$(CONFIG_MFD_WM8994)	+= wm8994.o
 
 obj-$(CONFIG_TPS6105X)		+= tps6105x.o
 obj-$(CONFIG_TPS65010)		+= tps65010.o
diff --git a/drivers/mfd/aat2870-core.c b/drivers/mfd/aat2870-core.c
index 29b6a2d4ac72..3ba19a45f199 100644
--- a/drivers/mfd/aat2870-core.c
+++ b/drivers/mfd/aat2870-core.c
@@ -373,11 +373,8 @@ static int aat2870_i2c_probe(struct i2c_client *client,
 
 	aat2870 = devm_kzalloc(&client->dev, sizeof(struct aat2870_data),
 				GFP_KERNEL);
-	if (!aat2870) {
-		dev_err(&client->dev,
-			"Failed to allocate memory for aat2870\n");
+	if (!aat2870)
 		return -ENOMEM;
-	}
 
 	aat2870->dev = &client->dev;
 	dev_set_drvdata(aat2870->dev, aat2870);
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index f0afb44271f8..6a5a98806cb8 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -381,9 +381,11 @@ static int ab3100_event_registers_startup_state_get(struct device *dev,
 					     u8 *event)
 {
 	struct ab3100 *ab3100 = dev_get_drvdata(dev->parent);
+
 	if (!ab3100->startup_events_read)
 		return -EAGAIN; /* Try again later */
 	memcpy(event, ab3100->startup_events, 3);
+
 	return 0;
 }
 
@@ -858,10 +860,8 @@ static int ab3100_probe(struct i2c_client *client,
 	int i;
 
 	ab3100 = devm_kzalloc(&client->dev, sizeof(struct ab3100), GFP_KERNEL);
-	if (!ab3100) {
-		dev_err(&client->dev, "could not allocate AB3100 device\n");
+	if (!ab3100)
 		return -ENOMEM;
-	}
 
 	/* Initialize data structure */
 	mutex_init(&ab3100->access_mutex);
@@ -883,20 +883,17 @@ static int ab3100_probe(struct i2c_client *client,
 
 	for (i = 0; ids[i].id != 0x0; i++) {
 		if (ids[i].id == ab3100->chip_id) {
-			if (ids[i].name != NULL) {
-				snprintf(&ab3100->chip_name[0],
-					 sizeof(ab3100->chip_name) - 1,
-					 "AB3100 %s",
-					 ids[i].name);
+			if (ids[i].name)
 				break;
-			} else {
-				dev_err(&client->dev,
-					"AB3000 is not supported\n");
-				goto exit_no_detect;
-			}
+
+			dev_err(&client->dev, "AB3000 is not supported\n");
+			goto exit_no_detect;
 		}
 	}
 
+	snprintf(&ab3100->chip_name[0],
+		 sizeof(ab3100->chip_name) - 1, "AB3100 %s", ids[i].name);
+
 	if (ids[i].id == 0x0) {
 		dev_err(&client->dev, "unknown analog baseband chip id: 0x%x\n",
 			ab3100->chip_id);
diff --git a/drivers/mfd/ab3100-otp.c b/drivers/mfd/ab3100-otp.c
index f391c5fee1b0..55b207a4b336 100644
--- a/drivers/mfd/ab3100-otp.c
+++ b/drivers/mfd/ab3100-otp.c
@@ -188,10 +188,9 @@ static int __init ab3100_otp_probe(struct platform_device *pdev)
 	int i;
 
 	otp = devm_kzalloc(&pdev->dev, sizeof(struct ab3100_otp), GFP_KERNEL);
-	if (!otp) {
-		dev_err(&pdev->dev, "could not allocate AB3100 OTP device\n");
+	if (!otp)
 		return -ENOMEM;
-	}
+
 	otp->dev = &pdev->dev;
 
 	/* Replace platform data coming in with a local struct */
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index fefbe4cfa61d..f3d689176fc2 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -113,7 +113,7 @@
 #define AB8500_SWITCH_OFF_STATUS	0x00
 
 #define AB8500_TURN_ON_STATUS		0x00
-#define AB8505_TURN_ON_STATUS_2	0x04
+#define AB8505_TURN_ON_STATUS_2		0x04
 
 #define AB8500_CH_USBCH_STAT1_REG	0x02
 #define VBUS_DET_DBNC100		0x02
@@ -211,7 +211,7 @@ static int set_register_interruptible(struct ab8500 *ab8500, u8 bank,
 	/*
 	 * Put the u8 bank and u8 register together into a an u16.
 	 * The bank on higher 8 bits and register in lower 8 bits.
-	 * */
+	 */
 	u16 addr = ((u16)bank) << 8 | reg;
 
 	dev_vdbg(ab8500->dev, "wr: addr %#x <= %#x\n", addr, data);
@@ -243,8 +243,6 @@ static int get_register_interruptible(struct ab8500 *ab8500, u8 bank,
 	u8 reg, u8 *value)
 {
 	int ret;
-	/* put the u8 bank and u8 reg together into a an u16.
-	 * bank on higher 8 bits and reg in lower */
 	u16 addr = ((u16)bank) << 8 | reg;
 
 	mutex_lock(&ab8500->lock);
@@ -278,8 +276,6 @@ static int mask_and_set_register_interruptible(struct ab8500 *ab8500, u8 bank,
 	u8 reg, u8 bitmask, u8 bitvalues)
 {
 	int ret;
-	/* put the u8 bank and u8 reg together into a an u16.
-	 * bank on higher 8 bits and reg in lower */
 	u16 addr = ((u16)bank) << 8 | reg;
 
 	mutex_lock(&ab8500->lock);
@@ -449,12 +445,12 @@ static void update_latch_offset(u8 *offset, int i)
 {
 	/* Fix inconsistent ITFromLatch25 bit mapping... */
 	if (unlikely(*offset == 17))
-			*offset = 24;
+		*offset = 24;
 	/* Fix inconsistent ab8540 bit mapping... */
 	if (unlikely(*offset == 16))
-			*offset = 25;
+		*offset = 25;
 	if ((i == 3) && (*offset >= 24))
-			*offset += 2;
+		*offset += 2;
 }
 
 static int ab8500_handle_hierarchical_line(struct ab8500 *ab8500,
@@ -590,12 +586,12 @@ static int ab8500_irq_init(struct ab8500 *ab8500, struct device_node *np)
 
 	/* If ->irq_base is zero this will give a linear mapping */
 	ab8500->domain = irq_domain_add_simple(ab8500->dev->of_node,
-			num_irqs, 0,
-			&ab8500_irq_ops, ab8500);
+					       num_irqs, 0,
+					       &ab8500_irq_ops, ab8500);
 
 	if (!ab8500->domain) {
 		dev_err(ab8500->dev, "Failed to create irqdomain\n");
-		return -ENOSYS;
+		return -ENODEV;
 	}
 
 	return 0;
@@ -609,442 +605,28 @@ int ab8500_suspend(struct ab8500 *ab8500)
 	return 0;
 }
 
-static struct resource ab8500_gpadc_resources[] = {
-	{
-		.name	= "HW_CONV_END",
-		.start	= AB8500_INT_GP_HW_ADC_CONV_END,
-		.end	= AB8500_INT_GP_HW_ADC_CONV_END,
-		.flags	= IORESOURCE_IRQ,
-	},
-	{
-		.name	= "SW_CONV_END",
-		.start	= AB8500_INT_GP_SW_ADC_CONV_END,
-		.end	= AB8500_INT_GP_SW_ADC_CONV_END,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8505_gpadc_resources[] = {
-	{
-		.name	= "SW_CONV_END",
-		.start	= AB8500_INT_GP_SW_ADC_CONV_END,
-		.end	= AB8500_INT_GP_SW_ADC_CONV_END,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_rtc_resources[] = {
-	{
-		.name	= "60S",
-		.start	= AB8500_INT_RTC_60S,
-		.end	= AB8500_INT_RTC_60S,
-		.flags	= IORESOURCE_IRQ,
-	},
-	{
-		.name	= "ALARM",
-		.start	= AB8500_INT_RTC_ALARM,
-		.end	= AB8500_INT_RTC_ALARM,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8540_rtc_resources[] = {
-	{
-		.name	= "1S",
-		.start	= AB8540_INT_RTC_1S,
-		.end	= AB8540_INT_RTC_1S,
-		.flags	= IORESOURCE_IRQ,
-	},
-	{
-		.name	= "ALARM",
-		.start	= AB8500_INT_RTC_ALARM,
-		.end	= AB8500_INT_RTC_ALARM,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_poweronkey_db_resources[] = {
-	{
-		.name	= "ONKEY_DBF",
-		.start	= AB8500_INT_PON_KEY1DB_F,
-		.end	= AB8500_INT_PON_KEY1DB_F,
-		.flags	= IORESOURCE_IRQ,
-	},
-	{
-		.name	= "ONKEY_DBR",
-		.start	= AB8500_INT_PON_KEY1DB_R,
-		.end	= AB8500_INT_PON_KEY1DB_R,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_av_acc_detect_resources[] = {
-	{
-	       .name = "ACC_DETECT_1DB_F",
-	       .start = AB8500_INT_ACC_DETECT_1DB_F,
-	       .end = AB8500_INT_ACC_DETECT_1DB_F,
-	       .flags = IORESOURCE_IRQ,
-	},
-	{
-	       .name = "ACC_DETECT_1DB_R",
-	       .start = AB8500_INT_ACC_DETECT_1DB_R,
-	       .end = AB8500_INT_ACC_DETECT_1DB_R,
-	       .flags = IORESOURCE_IRQ,
-	},
-	{
-	       .name = "ACC_DETECT_21DB_F",
-	       .start = AB8500_INT_ACC_DETECT_21DB_F,
-	       .end = AB8500_INT_ACC_DETECT_21DB_F,
-	       .flags = IORESOURCE_IRQ,
-	},
-	{
-	       .name = "ACC_DETECT_21DB_R",
-	       .start = AB8500_INT_ACC_DETECT_21DB_R,
-	       .end = AB8500_INT_ACC_DETECT_21DB_R,
-	       .flags = IORESOURCE_IRQ,
-	},
-	{
-	       .name = "ACC_DETECT_22DB_F",
-	       .start = AB8500_INT_ACC_DETECT_22DB_F,
-	       .end = AB8500_INT_ACC_DETECT_22DB_F,
-	       .flags = IORESOURCE_IRQ,
-	},
-	{
-	       .name = "ACC_DETECT_22DB_R",
-	       .start = AB8500_INT_ACC_DETECT_22DB_R,
-	       .end = AB8500_INT_ACC_DETECT_22DB_R,
-	       .flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_charger_resources[] = {
-	{
-		.name = "MAIN_CH_UNPLUG_DET",
-		.start = AB8500_INT_MAIN_CH_UNPLUG_DET,
-		.end = AB8500_INT_MAIN_CH_UNPLUG_DET,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "MAIN_CHARGE_PLUG_DET",
-		.start = AB8500_INT_MAIN_CH_PLUG_DET,
-		.end = AB8500_INT_MAIN_CH_PLUG_DET,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_R",
-		.start = AB8500_INT_VBUS_DET_R,
-		.end = AB8500_INT_VBUS_DET_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_F",
-		.start = AB8500_INT_VBUS_DET_F,
-		.end = AB8500_INT_VBUS_DET_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_LINK_STATUS",
-		.start = AB8500_INT_USB_LINK_STATUS,
-		.end = AB8500_INT_USB_LINK_STATUS,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_OVV",
-		.start = AB8500_INT_VBUS_OVV,
-		.end = AB8500_INT_VBUS_OVV,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_CH_TH_PROT_R",
-		.start = AB8500_INT_USB_CH_TH_PROT_R,
-		.end = AB8500_INT_USB_CH_TH_PROT_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_CH_TH_PROT_F",
-		.start = AB8500_INT_USB_CH_TH_PROT_F,
-		.end = AB8500_INT_USB_CH_TH_PROT_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "MAIN_EXT_CH_NOT_OK",
-		.start = AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.end = AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "MAIN_CH_TH_PROT_R",
-		.start = AB8500_INT_MAIN_CH_TH_PROT_R,
-		.end = AB8500_INT_MAIN_CH_TH_PROT_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "MAIN_CH_TH_PROT_F",
-		.start = AB8500_INT_MAIN_CH_TH_PROT_F,
-		.end = AB8500_INT_MAIN_CH_TH_PROT_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_CHARGER_NOT_OKR",
-		.start = AB8500_INT_USB_CHARGER_NOT_OKR,
-		.end = AB8500_INT_USB_CHARGER_NOT_OKR,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "CH_WD_EXP",
-		.start = AB8500_INT_CH_WD_EXP,
-		.end = AB8500_INT_CH_WD_EXP,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_CH_DROP_END",
-		.start = AB8500_INT_VBUS_CH_DROP_END,
-		.end = AB8500_INT_VBUS_CH_DROP_END,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_btemp_resources[] = {
-	{
-		.name = "BAT_CTRL_INDB",
-		.start = AB8500_INT_BAT_CTRL_INDB,
-		.end = AB8500_INT_BAT_CTRL_INDB,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "BTEMP_LOW",
-		.start = AB8500_INT_BTEMP_LOW,
-		.end = AB8500_INT_BTEMP_LOW,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "BTEMP_HIGH",
-		.start = AB8500_INT_BTEMP_HIGH,
-		.end = AB8500_INT_BTEMP_HIGH,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "BTEMP_LOW_MEDIUM",
-		.start = AB8500_INT_BTEMP_LOW_MEDIUM,
-		.end = AB8500_INT_BTEMP_LOW_MEDIUM,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "BTEMP_MEDIUM_HIGH",
-		.start = AB8500_INT_BTEMP_MEDIUM_HIGH,
-		.end = AB8500_INT_BTEMP_MEDIUM_HIGH,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_fg_resources[] = {
-	{
-		.name = "NCONV_ACCU",
-		.start = AB8500_INT_CCN_CONV_ACC,
-		.end = AB8500_INT_CCN_CONV_ACC,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "BATT_OVV",
-		.start = AB8500_INT_BATT_OVV,
-		.end = AB8500_INT_BATT_OVV,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "LOW_BAT_F",
-		.start = AB8500_INT_LOW_BAT_F,
-		.end = AB8500_INT_LOW_BAT_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "LOW_BAT_R",
-		.start = AB8500_INT_LOW_BAT_R,
-		.end = AB8500_INT_LOW_BAT_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "CC_INT_CALIB",
-		.start = AB8500_INT_CC_INT_CALIB,
-		.end = AB8500_INT_CC_INT_CALIB,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "CCEOC",
-		.start = AB8500_INT_CCEOC,
-		.end = AB8500_INT_CCEOC,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_chargalg_resources[] = {};
-
-#ifdef CONFIG_DEBUG_FS
-static struct resource ab8500_debug_resources[] = {
-	{
-		.name	= "IRQ_AB8500",
-		/*
-		 * Number will be filled in. NOTE: this is deliberately
-		 * not flagged as an IRQ in ordet to avoid remapping using
-		 * the irqdomain in the MFD core, so that this IRQ passes
-		 * unremapped to the debug code.
-		 */
-	},
-	{
-		.name	= "IRQ_FIRST",
-		.start	= AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.end	= AB8500_INT_MAIN_EXT_CH_NOT_OK,
-		.flags	= IORESOURCE_IRQ,
-	},
-	{
-		.name	= "IRQ_LAST",
-		.start	= AB8500_INT_XTAL32K_KO,
-		.end	= AB8500_INT_XTAL32K_KO,
-		.flags	= IORESOURCE_IRQ,
-	},
-};
-#endif
-
-static struct resource ab8500_usb_resources[] = {
-	{
-		.name = "ID_WAKEUP_R",
-		.start = AB8500_INT_ID_WAKEUP_R,
-		.end = AB8500_INT_ID_WAKEUP_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "ID_WAKEUP_F",
-		.start = AB8500_INT_ID_WAKEUP_F,
-		.end = AB8500_INT_ID_WAKEUP_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_F",
-		.start = AB8500_INT_VBUS_DET_F,
-		.end = AB8500_INT_VBUS_DET_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_R",
-		.start = AB8500_INT_VBUS_DET_R,
-		.end = AB8500_INT_VBUS_DET_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_LINK_STATUS",
-		.start = AB8500_INT_USB_LINK_STATUS,
-		.end = AB8500_INT_USB_LINK_STATUS,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_ADP_PROBE_PLUG",
-		.start = AB8500_INT_ADP_PROBE_PLUG,
-		.end = AB8500_INT_ADP_PROBE_PLUG,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "USB_ADP_PROBE_UNPLUG",
-		.start = AB8500_INT_ADP_PROBE_UNPLUG,
-		.end = AB8500_INT_ADP_PROBE_UNPLUG,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8505_iddet_resources[] = {
-	{
-		.name  = "KeyDeglitch",
-		.start = AB8505_INT_KEYDEGLITCH,
-		.end   = AB8505_INT_KEYDEGLITCH,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name  = "KP",
-		.start = AB8505_INT_KP,
-		.end   = AB8505_INT_KP,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name  = "IKP",
-		.start = AB8505_INT_IKP,
-		.end   = AB8505_INT_IKP,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name  = "IKR",
-		.start = AB8505_INT_IKR,
-		.end   = AB8505_INT_IKR,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name  = "KeyStuck",
-		.start = AB8505_INT_KEYSTUCK,
-		.end   = AB8505_INT_KEYSTUCK,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_R",
-		.start = AB8500_INT_VBUS_DET_R,
-		.end = AB8500_INT_VBUS_DET_R,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "VBUS_DET_F",
-		.start = AB8500_INT_VBUS_DET_F,
-		.end = AB8500_INT_VBUS_DET_F,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "ID_DET_PLUGR",
-		.start = AB8500_INT_ID_DET_PLUGR,
-		.end = AB8500_INT_ID_DET_PLUGR,
-		.flags = IORESOURCE_IRQ,
-	},
-	{
-		.name = "ID_DET_PLUGF",
-		.start = AB8500_INT_ID_DET_PLUGF,
-		.end = AB8500_INT_ID_DET_PLUGF,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
-static struct resource ab8500_temp_resources[] = {
-	{
-		.name  = "ABX500_TEMP_WARM",
-		.start = AB8500_INT_TEMP_WARM,
-		.end   = AB8500_INT_TEMP_WARM,
-		.flags = IORESOURCE_IRQ,
-	},
-};
-
 static const struct mfd_cell ab8500_bm_devs[] = {
 	{
 		.name = "ab8500-charger",
 		.of_compatible = "stericsson,ab8500-charger",
-		.num_resources = ARRAY_SIZE(ab8500_charger_resources),
-		.resources = ab8500_charger_resources,
 		.platform_data = &ab8500_bm_data,
 		.pdata_size = sizeof(ab8500_bm_data),
 	},
 	{
 		.name = "ab8500-btemp",
 		.of_compatible = "stericsson,ab8500-btemp",
-		.num_resources = ARRAY_SIZE(ab8500_btemp_resources),
-		.resources = ab8500_btemp_resources,
 		.platform_data = &ab8500_bm_data,
 		.pdata_size = sizeof(ab8500_bm_data),
 	},
 	{
 		.name = "ab8500-fg",
 		.of_compatible = "stericsson,ab8500-fg",
-		.num_resources = ARRAY_SIZE(ab8500_fg_resources),
-		.resources = ab8500_fg_resources,
 		.platform_data = &ab8500_bm_data,
 		.pdata_size = sizeof(ab8500_bm_data),
 	},
 	{
 		.name = "ab8500-chargalg",
 		.of_compatible = "stericsson,ab8500-chargalg",
-		.num_resources = ARRAY_SIZE(ab8500_chargalg_resources),
-		.resources = ab8500_chargalg_resources,
 		.platform_data = &ab8500_bm_data,
 		.pdata_size = sizeof(ab8500_bm_data),
 	},
@@ -1055,8 +637,6 @@ static const struct mfd_cell ab8500_devs[] = {
 	{
 		.name = "ab8500-debug",
 		.of_compatible = "stericsson,ab8500-debug",
-		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
-		.resources = ab8500_debug_resources,
 	},
 #endif
 	{
@@ -1078,27 +658,19 @@ static const struct mfd_cell ab8500_devs[] = {
 	{
 		.name = "ab8500-gpadc",
 		.of_compatible = "stericsson,ab8500-gpadc",
-		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
-		.resources = ab8500_gpadc_resources,
 	},
 	{
 		.name = "ab8500-rtc",
 		.of_compatible = "stericsson,ab8500-rtc",
-		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
-		.resources = ab8500_rtc_resources,
 	},
 	{
 		.name = "ab8500-acc-det",
 		.of_compatible = "stericsson,ab8500-acc-det",
-		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
-		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
 
 		.name = "ab8500-poweron-key",
 		.of_compatible = "stericsson,ab8500-poweron-key",
-		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
-		.resources = ab8500_poweronkey_db_resources,
 	},
 	{
 		.name = "ab8500-pwm",
@@ -1126,14 +698,10 @@ static const struct mfd_cell ab8500_devs[] = {
 	{
 		.name = "abx500-temp",
 		.of_compatible = "stericsson,abx500-temp",
-		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
-		.resources = ab8500_temp_resources,
 	},
 	{
 		.name = "ab8500-usb",
 		.of_compatible = "stericsson,ab8500-usb",
-		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
-		.resources = ab8500_usb_resources,
 	},
 	{
 		.name = "ab8500-codec",
@@ -1145,8 +713,6 @@ static const struct mfd_cell ab9540_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
-		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
-		.resources = ab8500_debug_resources,
 	},
 #endif
 	{
@@ -1165,23 +731,15 @@ static const struct mfd_cell ab9540_devs[] = {
 	{
 		.name = "ab8500-gpadc",
 		.of_compatible = "stericsson,ab8500-gpadc",
-		.num_resources = ARRAY_SIZE(ab8500_gpadc_resources),
-		.resources = ab8500_gpadc_resources,
 	},
 	{
 		.name = "ab8500-rtc",
-		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
-		.resources = ab8500_rtc_resources,
 	},
 	{
 		.name = "ab8500-acc-det",
-		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
-		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
 		.name = "ab8500-poweron-key",
-		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
-		.resources = ab8500_poweronkey_db_resources,
 	},
 	{
 		.name = "ab8500-pwm",
@@ -1189,8 +747,6 @@ static const struct mfd_cell ab9540_devs[] = {
 	},
 	{
 		.name = "abx500-temp",
-		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
-		.resources = ab8500_temp_resources,
 	},
 	{
 		.name = "pinctrl-ab9540",
@@ -1198,16 +754,12 @@ static const struct mfd_cell ab9540_devs[] = {
 	},
 	{
 		.name = "ab9540-usb",
-		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
-		.resources = ab8500_usb_resources,
 	},
 	{
 		.name = "ab9540-codec",
 	},
 	{
 		.name = "ab-iddet",
-		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
-		.resources = ab8505_iddet_resources,
 	},
 };
 
@@ -1216,8 +768,6 @@ static const struct mfd_cell ab8505_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
-		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
-		.resources = ab8500_debug_resources,
 	},
 #endif
 	{
@@ -1233,23 +783,15 @@ static const struct mfd_cell ab8505_devs[] = {
 	{
 		.name = "ab8500-gpadc",
 		.of_compatible = "stericsson,ab8500-gpadc",
-		.num_resources = ARRAY_SIZE(ab8505_gpadc_resources),
-		.resources = ab8505_gpadc_resources,
 	},
 	{
 		.name = "ab8500-rtc",
-		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
-		.resources = ab8500_rtc_resources,
 	},
 	{
 		.name = "ab8500-acc-det",
-		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
-		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
 		.name = "ab8500-poweron-key",
-		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
-		.resources = ab8500_poweronkey_db_resources,
 	},
 	{
 		.name = "ab8500-pwm",
@@ -1260,16 +802,12 @@ static const struct mfd_cell ab8505_devs[] = {
 	},
 	{
 		.name = "ab8500-usb",
-		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
-		.resources = ab8500_usb_resources,
 	},
 	{
 		.name = "ab8500-codec",
 	},
 	{
 		.name = "ab-iddet",
-		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
-		.resources = ab8505_iddet_resources,
 	},
 };
 
@@ -1277,8 +815,6 @@ static const struct mfd_cell ab8540_devs[] = {
 #ifdef CONFIG_DEBUG_FS
 	{
 		.name = "ab8500-debug",
-		.num_resources = ARRAY_SIZE(ab8500_debug_resources),
-		.resources = ab8500_debug_resources,
 	},
 #endif
 	{
@@ -1297,18 +833,12 @@ static const struct mfd_cell ab8540_devs[] = {
 	{
 		.name = "ab8500-gpadc",
 		.of_compatible = "stericsson,ab8500-gpadc",
-		.num_resources = ARRAY_SIZE(ab8505_gpadc_resources),
-		.resources = ab8505_gpadc_resources,
 	},
 	{
 		.name = "ab8500-acc-det",
-		.num_resources = ARRAY_SIZE(ab8500_av_acc_detect_resources),
-		.resources = ab8500_av_acc_detect_resources,
 	},
 	{
 		.name = "ab8500-poweron-key",
-		.num_resources = ARRAY_SIZE(ab8500_poweronkey_db_resources),
-		.resources = ab8500_poweronkey_db_resources,
 	},
 	{
 		.name = "ab8500-pwm",
@@ -1316,24 +846,18 @@ static const struct mfd_cell ab8540_devs[] = {
 	},
 	{
 		.name = "abx500-temp",
-		.num_resources = ARRAY_SIZE(ab8500_temp_resources),
-		.resources = ab8500_temp_resources,
 	},
 	{
 		.name = "pinctrl-ab8540",
 	},
 	{
 		.name = "ab8540-usb",
-		.num_resources = ARRAY_SIZE(ab8500_usb_resources),
-		.resources = ab8500_usb_resources,
 	},
 	{
 		.name = "ab8540-codec",
 	},
 	{
 		.name = "ab-iddet",
-		.num_resources = ARRAY_SIZE(ab8505_iddet_resources),
-		.resources = ab8505_iddet_resources,
 	},
 };
 
@@ -1341,8 +865,6 @@ static const struct mfd_cell ab8540_cut1_devs[] = {
 	{
 		.name = "ab8500-rtc",
 		.of_compatible = "stericsson,ab8500-rtc",
-		.num_resources = ARRAY_SIZE(ab8500_rtc_resources),
-		.resources = ab8500_rtc_resources,
 	},
 };
 
@@ -1350,8 +872,6 @@ static const struct mfd_cell ab8540_cut2_devs[] = {
 	{
 		.name = "ab8540-rtc",
 		.of_compatible = "stericsson,ab8540-rtc",
-		.num_resources = ARRAY_SIZE(ab8540_rtc_resources),
-		.resources = ab8540_rtc_resources,
 	},
 };
 
@@ -1549,7 +1069,7 @@ static struct attribute_group ab9540_attr_group = {
 
 static int ab8500_probe(struct platform_device *pdev)
 {
-	static const char *switch_off_status[] = {
+	static const char * const switch_off_status[] = {
 		"Swoff bit programming",
 		"Thermal protection activation",
 		"Vbat lower then BattOk falling threshold",
@@ -1558,7 +1078,7 @@ static int ab8500_probe(struct platform_device *pdev)
 		"Battery level lower than power on reset threshold",
 		"Power on key 1 pressed longer than 10 seconds",
 		"DB8500 thermal shutdown"};
-	static const char *turn_on_status[] = {
+	static const char * const turn_on_status[] = {
 		"Battery rising (Vbat)",
 		"Power On Key 1 dbF",
 		"Power On Key 2 dbF",
@@ -1750,12 +1270,6 @@ static int ab8500_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-#ifdef CONFIG_DEBUG_FS
-	/* Pass to debugfs */
-	ab8500_debug_resources[0].start = ab8500->irq;
-	ab8500_debug_resources[0].end = ab8500->irq;
-#endif
-
 	if (is_ab9540(ab8500))
 		ret = mfd_add_devices(ab8500->dev, 0, ab9540_devs,
 				ARRAY_SIZE(ab9540_devs), NULL,
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 0236cd7cdce4..69d9fffe5b5c 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -242,8 +242,10 @@ static struct ab8500_prcmu_ranges ab8500_debug_ranges[AB8500_NUM_BANKS] = {
 				.first = 0x40,
 				.last = 0x44,
 			},
-			/* 0x80-0x8B is SIM registers and should
-			 * not be accessed from here */
+			/*
+			 * 0x80-0x8B are SIM registers and should
+			 * not be accessed from here
+			 */
 		},
 	},
 	[AB8500_USB] = {
@@ -587,8 +589,10 @@ static struct ab8500_prcmu_ranges ab8505_debug_ranges[AB8500_NUM_BANKS] = {
 				.first = 0x40,
 				.last = 0x48,
 			},
-			/* 0x80-0x8B is SIM registers and should
-			 * not be accessed from here */
+			/*
+			 * 0x80-0x8B are SIM registers and should
+			 * not be accessed from here
+			 */
 		},
 	},
 	[AB8500_USB] = {
@@ -1306,8 +1310,10 @@ static int ab8500_registers_print(struct device *dev, u32 bank,
 			if (s) {
 				seq_printf(s, "  [0x%02X/0x%02X]: 0x%02X\n",
 					   bank, reg, value);
-				/* Error is not returned here since
-				 * the output is wanted in any case */
+				/*
+				 * Error is not returned here since
+				 * the output is wanted in any case
+				 */
 				if (seq_has_overflowed(s))
 					return 0;
 			} else {
@@ -2740,10 +2746,9 @@ static ssize_t hwreg_common_write(char *b, struct hwreg_cfg *cfg,
 	*cfg = loc;
 
 #ifdef ABB_HWREG_DEBUG
-	pr_warn("HWREG request: %s, %s,\n"
-		"  addr=0x%08X, mask=0x%X, shift=%d" "value=0x%X\n",
-		(write) ? "write" : "read",
-		REG_FMT_DEC(cfg) ? "decimal" : "hexa",
+	pr_warn("HWREG request: %s, %s,\n", (write) ? "write" : "read",
+		REG_FMT_DEC(cfg) ? "decimal" : "hexa");
+	pr_warn("  addr=0x%08X, mask=0x%X, shift=%d" "value=0x%X\n",
 		cfg->addr, cfg->mask, cfg->shift, val);
 #endif
 
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
index c51c1b188d64..97dcadc8fa8b 100644
--- a/drivers/mfd/ab8500-gpadc.c
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -49,61 +49,61 @@
  * OTP register offsets
  * Bank : 0x15
  */
-#define AB8500_GPADC_CAL_1		0x0F
-#define AB8500_GPADC_CAL_2		0x10
-#define AB8500_GPADC_CAL_3		0x11
-#define AB8500_GPADC_CAL_4		0x12
-#define AB8500_GPADC_CAL_5		0x13
-#define AB8500_GPADC_CAL_6		0x14
-#define AB8500_GPADC_CAL_7		0x15
+#define AB8500_GPADC_CAL_1	0x0F
+#define AB8500_GPADC_CAL_2	0x10
+#define AB8500_GPADC_CAL_3	0x11
+#define AB8500_GPADC_CAL_4	0x12
+#define AB8500_GPADC_CAL_5	0x13
+#define AB8500_GPADC_CAL_6	0x14
+#define AB8500_GPADC_CAL_7	0x15
 /* New calibration for 8540 */
 #define AB8540_GPADC_OTP4_REG_7	0x38
 #define AB8540_GPADC_OTP4_REG_6	0x39
 #define AB8540_GPADC_OTP4_REG_5	0x3A
 
 /* gpadc constants */
-#define EN_VINTCORE12			0x04
-#define EN_VTVOUT			0x02
-#define EN_GPADC			0x01
-#define DIS_GPADC			0x00
-#define AVG_1				0x00
-#define AVG_4				0x20
-#define AVG_8				0x40
-#define AVG_16				0x60
-#define ADC_SW_CONV			0x04
-#define EN_ICHAR			0x80
-#define BTEMP_PULL_UP			0x08
-#define EN_BUF				0x40
-#define DIS_ZERO			0x00
-#define GPADC_BUSY			0x01
-#define EN_FALLING			0x10
-#define EN_TRIG_EDGE			0x02
-#define EN_VBIAS_XTAL_TEMP		0x02
+#define EN_VINTCORE12		0x04
+#define EN_VTVOUT		0x02
+#define EN_GPADC		0x01
+#define DIS_GPADC		0x00
+#define AVG_1			0x00
+#define AVG_4			0x20
+#define AVG_8			0x40
+#define AVG_16			0x60
+#define ADC_SW_CONV		0x04
+#define EN_ICHAR		0x80
+#define BTEMP_PULL_UP		0x08
+#define EN_BUF			0x40
+#define DIS_ZERO		0x00
+#define GPADC_BUSY		0x01
+#define EN_FALLING		0x10
+#define EN_TRIG_EDGE		0x02
+#define EN_VBIAS_XTAL_TEMP	0x02
 
 /* GPADC constants from AB8500 spec, UM0836 */
-#define ADC_RESOLUTION			1024
-#define ADC_CH_BTEMP_MIN		0
-#define ADC_CH_BTEMP_MAX		1350
-#define ADC_CH_DIETEMP_MIN		0
-#define ADC_CH_DIETEMP_MAX		1350
-#define ADC_CH_CHG_V_MIN		0
-#define ADC_CH_CHG_V_MAX		20030
-#define ADC_CH_ACCDET2_MIN		0
-#define ADC_CH_ACCDET2_MAX		2500
-#define ADC_CH_VBAT_MIN			2300
-#define ADC_CH_VBAT_MAX			4800
-#define ADC_CH_CHG_I_MIN		0
-#define ADC_CH_CHG_I_MAX		1500
-#define ADC_CH_BKBAT_MIN		0
-#define ADC_CH_BKBAT_MAX		3200
+#define ADC_RESOLUTION		1024
+#define ADC_CH_BTEMP_MIN	0
+#define ADC_CH_BTEMP_MAX	1350
+#define ADC_CH_DIETEMP_MIN	0
+#define ADC_CH_DIETEMP_MAX	1350
+#define ADC_CH_CHG_V_MIN	0
+#define ADC_CH_CHG_V_MAX	20030
+#define ADC_CH_ACCDET2_MIN	0
+#define ADC_CH_ACCDET2_MAX	2500
+#define ADC_CH_VBAT_MIN		2300
+#define ADC_CH_VBAT_MAX		4800
+#define ADC_CH_CHG_I_MIN	0
+#define ADC_CH_CHG_I_MAX	1500
+#define ADC_CH_BKBAT_MIN	0
+#define ADC_CH_BKBAT_MAX	3200
 
 /* GPADC constants from AB8540 spec */
-#define ADC_CH_IBAT_MIN			(-6000) /* mA range measured by ADC for ibat*/
-#define ADC_CH_IBAT_MAX			6000
-#define ADC_CH_IBAT_MIN_V		(-60)	/* mV range measured by ADC for ibat*/
-#define ADC_CH_IBAT_MAX_V		60
-#define IBAT_VDROP_L			(-56)  /* mV */
-#define IBAT_VDROP_H			56
+#define ADC_CH_IBAT_MIN		(-6000) /* mA range measured by ADC for ibat */
+#define ADC_CH_IBAT_MAX		6000
+#define ADC_CH_IBAT_MIN_V	(-60)	/* mV range measured by ADC for ibat */
+#define ADC_CH_IBAT_MAX_V	60
+#define IBAT_VDROP_L		(-56)  /* mV */
+#define IBAT_VDROP_H		56
 
 /* This is used to not lose precision when dividing to get gain and offset */
 #define CALIB_SCALE		1000
@@ -179,7 +179,7 @@ struct ab8500_gpadc *ab8500_gpadc_get(char *name)
 
 	list_for_each_entry(gpadc, &ab8500_gpadc_list, node) {
 		if (!strcmp(name, dev_name(gpadc->dev)))
-		    return gpadc;
+			return gpadc;
 	}
 
 	return ERR_PTR(-ENOENT);
@@ -315,11 +315,12 @@ int ab8500_gpadc_sw_hw_convert(struct ab8500_gpadc *gpadc, u8 channel,
 
 	ad_value = ab8500_gpadc_read_raw(gpadc, channel, avg_sample,
 			trig_edge, trig_timer, conv_type);
-/* On failure retry a second time */
+
+	/* On failure retry a second time */
 	if (ad_value < 0)
 		ad_value = ab8500_gpadc_read_raw(gpadc, channel, avg_sample,
 			trig_edge, trig_timer, conv_type);
-if (ad_value < 0) {
+	if (ad_value < 0) {
 		dev_err(gpadc->dev, "GPADC raw value failed ch: %d\n",
 				channel);
 		return ad_value;
@@ -327,8 +328,9 @@ if (ad_value < 0) {
 
 	voltage = ab8500_gpadc_ad_to_voltage(gpadc, channel, ad_value);
 	if (voltage < 0)
-		dev_err(gpadc->dev, "GPADC to voltage conversion failed ch:"
-			" %d AD: 0x%x\n", channel, ad_value);
+		dev_err(gpadc->dev,
+			"GPADC to voltage conversion failed ch: %d AD: 0x%x\n",
+			channel, ad_value);
 
 	return voltage;
 }
@@ -348,10 +350,9 @@ EXPORT_SYMBOL(ab8500_gpadc_sw_hw_convert);
 int ab8500_gpadc_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
 		u8 avg_sample, u8 trig_edge, u8 trig_timer, u8 conv_type)
 {
-	int raw_data;
-	raw_data = ab8500_gpadc_double_read_raw(gpadc, channel,
-			avg_sample, trig_edge, trig_timer, conv_type, NULL);
-	return raw_data;
+	return ab8500_gpadc_double_read_raw(gpadc, channel, avg_sample,
+					    trig_edge, trig_timer, conv_type,
+					    NULL);
 }
 
 int ab8500_gpadc_double_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
@@ -388,7 +389,7 @@ int ab8500_gpadc_double_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
 			goto out;
 		if (!(val & GPADC_BUSY))
 			break;
-		msleep(10);
+		msleep(20);
 	} while (++looplimit < 10);
 	if (looplimit >= 10 && (val & GPADC_BUSY)) {
 		dev_err(gpadc->dev, "gpadc_conversion: GPADC busy");
@@ -421,8 +422,7 @@ int ab8500_gpadc_double_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
 		val_reg1 |= EN_TRIG_EDGE;
 		if (trig_edge)
 			val_reg1 |= EN_FALLING;
-	}
-	else
+	} else
 		ret = abx500_set_register_interruptible(gpadc->dev,
 				AB8500_GPADC, AB8500_GPADC_CTRL2_REG, val);
 	if (ret < 0) {
@@ -449,7 +449,7 @@ int ab8500_gpadc_double_read_raw(struct ab8500_gpadc *gpadc, u8 channel,
 			* remove when hardware will be availible
 			*/
 			delay_min = 1000; /* Delay in micro seconds */
-			delay_max = 10000; /* large range to optimise sleep mode */
+			delay_max = 10000; /* large range optimises sleepmode */
 			break;
 		}
 		/* Intentional fallthrough */
@@ -785,9 +785,10 @@ static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
 				<< CALIB_SHIFT_IBAT)
 				/ (ADC_CH_IBAT_MAX_V - ADC_CH_IBAT_MIN_V);
 
-			gpadc->cal_data[ADC_INPUT_IBAT].gain = V_gain * V2A_gain;
-			gpadc->cal_data[ADC_INPUT_IBAT].offset = V_offset *
-				V2A_gain + V2A_offset;
+			gpadc->cal_data[ADC_INPUT_IBAT].gain =
+				V_gain * V2A_gain;
+			gpadc->cal_data[ADC_INPUT_IBAT].offset =
+				V_offset * V2A_gain + V2A_offset;
 		} else {
 			gpadc->cal_data[ADC_INPUT_IBAT].gain = 0;
 		}
@@ -923,11 +924,10 @@ static int ab8500_gpadc_probe(struct platform_device *pdev)
 	int ret = 0;
 	struct ab8500_gpadc *gpadc;
 
-	gpadc = devm_kzalloc(&pdev->dev, sizeof(struct ab8500_gpadc), GFP_KERNEL);
-	if (!gpadc) {
-		dev_err(&pdev->dev, "Error: No memory\n");
+	gpadc = devm_kzalloc(&pdev->dev,
+			     sizeof(struct ab8500_gpadc), GFP_KERNEL);
+	if (!gpadc)
 		return -ENOMEM;
-	}
 
 	gpadc->irq_sw = platform_get_irq_byname(pdev, "SW_CONV_END");
 	if (gpadc->irq_sw < 0)
@@ -1072,18 +1072,19 @@ void ab8540_gpadc_get_otp(struct ab8500_gpadc *gpadc,
 	*vmain_h = gpadc->cal_data[ADC_INPUT_VMAIN].otp_calib_hi;
 	*btemp_l = gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_lo;
 	*btemp_h = gpadc->cal_data[ADC_INPUT_BTEMP].otp_calib_hi;
-	*vbat_l = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_lo;
-	*vbat_h = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_hi;
-	*ibat_l = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_lo;
-	*ibat_h = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_hi;
-	return ;
+	*vbat_l  = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_lo;
+	*vbat_h  = gpadc->cal_data[ADC_INPUT_VBAT].otp_calib_hi;
+	*ibat_l  = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_lo;
+	*ibat_h  = gpadc->cal_data[ADC_INPUT_IBAT].otp_calib_hi;
 }
 
 subsys_initcall_sync(ab8500_gpadc_init);
 module_exit(ab8500_gpadc_exit);
 
 MODULE_LICENSE("GPL v2");
-MODULE_AUTHOR("Arun R Murthy, Daniel Willerud, Johan Palsson,"
-		"M'boumba Cedric Madianga");
+MODULE_AUTHOR("Arun R Murthy");
+MODULE_AUTHOR("Daniel Willerud");
+MODULE_AUTHOR("Johan Palsson");
+MODULE_AUTHOR("M'boumba Cedric Madianga");
 MODULE_ALIAS("platform:ab8500_gpadc");
 MODULE_DESCRIPTION("AB8500 GPADC driver");
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
index 0d1825696153..b9f0010309f9 100644
--- a/drivers/mfd/ab8500-sysctrl.c
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -27,7 +27,7 @@ static void ab8500_power_off(void)
 {
 	sigset_t old;
 	sigset_t all;
-	static char *pss[] = {"ab8500_ac", "pm2301", "ab8500_usb"};
+	static const char * const pss[] = {"ab8500_ac", "pm2301", "ab8500_usb"};
 	int i;
 	bool charger_present = false;
 	union power_supply_propval val;
@@ -68,10 +68,9 @@ static void ab8500_power_off(void)
 		ret = power_supply_get_property(psy,
 				POWER_SUPPLY_PROP_TECHNOLOGY, &val);
 		if (!ret && val.intval != POWER_SUPPLY_TECHNOLOGY_UNKNOWN) {
-			printk(KERN_INFO
-			       "Charger \"%s\" is connected with known battery."
-			       " Rebooting.\n",
-			       pss[i]);
+			pr_info("Charger '%s' is connected with known battery",
+				pss[i]);
+			pr_info(" - Rebooting.\n");
 			machine_restart("charging");
 		}
 		power_supply_put(psy);
@@ -161,8 +160,8 @@ static int ab8500_sysctrl_probe(struct platform_device *pdev)
 					pdata->initial_req_buf_config[j]);
 			if (ret < 0) {
 				dev_err(&pdev->dev,
-					"unable to set sysClkReq%dRfClkBuf: "
-					"%d\n", j + 1, ret);
+					"Can't set sysClkReq%dRfClkBuf: %d\n",
+					j + 1, ret);
 			}
 		}
 	}
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index ae88654595dc..d817f202da5b 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -9,10 +9,10 @@
  *
  * Derived from da903x:
  * Copyright (C) 2008 Compulab, Ltd.
- * 	Mike Rapoport <mike@compulab.co.il>
+ *	Mike Rapoport <mike@compulab.co.il>
  *
  * Copyright (C) 2006-2008 Marvell International Ltd.
- * 	Eric Miao <eric.miao@marvell.com>
+ *	Eric Miao <eric.miao@marvell.com>
  *
  * Licensed under the GPL-2 or later.
  */
@@ -355,7 +355,7 @@ static struct i2c_driver adp5520_driver = {
 	},
 	.probe		= adp5520_probe,
 	.remove		= adp5520_remove,
-	.id_table 	= adp5520_id,
+	.id_table	= adp5520_id,
 };
 
 module_i2c_driver(adp5520_driver);
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index d474732cc65c..5319f252790b 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -238,7 +238,7 @@ static int arizona_poll_reg(struct arizona *arizona,
 		if ((val & mask) == target)
 			return 0;
 
-		msleep(1);
+		usleep_range(1000, 5000);
 	}
 
 	dev_err(arizona->dev, "Polling reg %u timed out: %x\n", reg, val);
@@ -279,14 +279,14 @@ static void arizona_disable_reset(struct arizona *arizona)
 		case WM5110:
 		case WM8280:
 			/* Meet requirements for minimum reset duration */
-			msleep(5);
+			usleep_range(5000, 10000);
 			break;
 		default:
 			break;
 		}
 
 		gpio_set_value_cansleep(arizona->pdata.reset, 1);
-		msleep(1);
+		usleep_range(1000, 5000);
 	}
 }
 
@@ -598,6 +598,12 @@ static int arizona_runtime_resume(struct device *dev)
 			goto err;
 		}
 		break;
+	case WM1831:
+	case CS47L24:
+		ret = arizona_wait_for_boot(arizona);
+		if (ret != 0)
+			goto err;
+		break;
 	default:
 		ret = arizona_wait_for_boot(arizona);
 		if (ret != 0)
@@ -682,6 +688,9 @@ static int arizona_runtime_suspend(struct device *dev)
 			}
 		}
 		break;
+	case WM1831:
+	case CS47L24:
+		break;
 	default:
 		jd_active = arizona_is_jack_det_active(arizona);
 		if (jd_active < 0)
@@ -852,6 +861,16 @@ static int arizona_of_get_core_pdata(struct arizona *arizona)
 		count++;
 	}
 
+	count = 0;
+	of_property_for_each_u32(arizona->dev->of_node, "wlf,out-mono", prop,
+				 cur, val) {
+		if (count == ARRAY_SIZE(pdata->out_mono))
+			break;
+
+		pdata->out_mono[count] = !!val;
+		count++;
+	}
+
 	return 0;
 }
 
@@ -862,6 +881,8 @@ const struct of_device_id arizona_of_match[] = {
 	{ .compatible = "wlf,wm8997", .data = (void *)WM8997 },
 	{ .compatible = "wlf,wm8998", .data = (void *)WM8998 },
 	{ .compatible = "wlf,wm1814", .data = (void *)WM1814 },
+	{ .compatible = "wlf,wm1831", .data = (void *)WM1831 },
+	{ .compatible = "cirrus,cs47l24", .data = (void *)CS47L24 },
 	{},
 };
 EXPORT_SYMBOL_GPL(arizona_of_match);
@@ -919,6 +940,23 @@ static const struct mfd_cell wm5110_devs[] = {
 	},
 };
 
+static const char * const cs47l24_supplies[] = {
+	"MICVDD",
+	"CPVDD",
+	"SPKVDD",
+};
+
+static const struct mfd_cell cs47l24_devs[] = {
+	{ .name = "arizona-gpio" },
+	{ .name = "arizona-haptics" },
+	{ .name = "arizona-pwm" },
+	{
+		.name = "cs47l24-codec",
+		.parent_supplies = cs47l24_supplies,
+		.num_parent_supplies = ARRAY_SIZE(cs47l24_supplies),
+	},
+};
+
 static const char * const wm8997_supplies[] = {
 	"MICVDD",
 	"DBVDD2",
@@ -963,7 +1001,7 @@ static const struct mfd_cell wm8998_devs[] = {
 int arizona_dev_init(struct arizona *arizona)
 {
 	struct device *dev = arizona->dev;
-	const char *type_name;
+	const char *type_name = NULL;
 	unsigned int reg, val, mask;
 	int (*apply_patch)(struct arizona *) = NULL;
 	const struct mfd_cell *subdevs = NULL;
@@ -987,6 +1025,8 @@ int arizona_dev_init(struct arizona *arizona)
 	case WM8997:
 	case WM8998:
 	case WM1814:
+	case WM1831:
+	case CS47L24:
 		for (i = 0; i < ARRAY_SIZE(wm5102_core_supplies); i++)
 			arizona->core_supplies[i].supply
 				= wm5102_core_supplies[i];
@@ -1001,11 +1041,18 @@ int arizona_dev_init(struct arizona *arizona)
 	/* Mark DCVDD as external, LDO1 driver will clear if internal */
 	arizona->external_dcvdd = true;
 
-	ret = mfd_add_devices(arizona->dev, -1, early_devs,
-			      ARRAY_SIZE(early_devs), NULL, 0, NULL);
-	if (ret != 0) {
-		dev_err(dev, "Failed to add early children: %d\n", ret);
-		return ret;
+	switch (arizona->type) {
+	case WM1831:
+	case CS47L24:
+		break; /* No LDO1 regulator */
+	default:
+		ret = mfd_add_devices(arizona->dev, -1, early_devs,
+				      ARRAY_SIZE(early_devs), NULL, 0, NULL);
+		if (ret != 0) {
+			dev_err(dev, "Failed to add early children: %d\n", ret);
+			return ret;
+		}
+		break;
 	}
 
 	ret = devm_regulator_bulk_get(dev, arizona->num_core_supplies,
@@ -1069,6 +1116,7 @@ int arizona_dev_init(struct arizona *arizona)
 	case 0x5102:
 	case 0x5110:
 	case 0x6349:
+	case 0x6363:
 	case 0x8997:
 		break;
 	default:
@@ -1084,7 +1132,7 @@ int arizona_dev_init(struct arizona *arizona)
 			goto err_reset;
 		}
 
-		msleep(1);
+		usleep_range(1000, 5000);
 	}
 
 	/* Ensure device startup is complete */
@@ -1167,6 +1215,30 @@ int arizona_dev_init(struct arizona *arizona)
 			n_subdevs = ARRAY_SIZE(wm5110_devs);
 		}
 		break;
+	case 0x6363:
+		if (IS_ENABLED(CONFIG_MFD_CS47L24)) {
+			switch (arizona->type) {
+			case CS47L24:
+				type_name = "CS47L24";
+				break;
+
+			case WM1831:
+				type_name = "WM1831";
+				break;
+
+			default:
+				dev_warn(arizona->dev,
+					 "CS47L24 registered as %d\n",
+					 arizona->type);
+				arizona->type = CS47L24;
+				break;
+			}
+
+			apply_patch = cs47l24_patch;
+			subdevs = cs47l24_devs;
+			n_subdevs = ARRAY_SIZE(cs47l24_devs);
+		}
+		break;
 	case 0x8997:
 		if (IS_ENABLED(CONFIG_MFD_WM8997)) {
 			type_name = "WM8997";
diff --git a/drivers/mfd/arizona-i2c.c b/drivers/mfd/arizona-i2c.c
index 4e3afd1861fc..5fe12961cfe5 100644
--- a/drivers/mfd/arizona-i2c.c
+++ b/drivers/mfd/arizona-i2c.c
@@ -88,7 +88,9 @@ static int arizona_i2c_probe(struct i2c_client *i2c,
 static int arizona_i2c_remove(struct i2c_client *i2c)
 {
 	struct arizona *arizona = dev_get_drvdata(&i2c->dev);
+
 	arizona_dev_exit(arizona);
+
 	return 0;
 }
 
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index 3d425e93ce84..5fef014920a3 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -30,11 +30,13 @@ static int arizona_map_irq(struct arizona *arizona, int irq)
 {
 	int ret;
 
-	ret = regmap_irq_get_virq(arizona->aod_irq_chip, irq);
-	if (ret < 0)
-		ret = regmap_irq_get_virq(arizona->irq_chip, irq);
+	if (arizona->aod_irq_chip) {
+		ret = regmap_irq_get_virq(arizona->aod_irq_chip, irq);
+		if (ret >= 0)
+			return ret;
+	}
 
-	return ret;
+	return regmap_irq_get_virq(arizona->irq_chip, irq);
 }
 
 int arizona_request_irq(struct arizona *arizona, int irq, char *name,
@@ -107,8 +109,8 @@ static irqreturn_t arizona_irq_thread(int irq, void *data)
 	do {
 		poll = false;
 
-		/* Always handle the AoD domain */
-		handle_nested_irq(irq_find_mapping(arizona->virq, 0));
+		if (arizona->aod_irq_chip)
+			handle_nested_irq(irq_find_mapping(arizona->virq, 0));
 
 		/*
 		 * Check if one of the main interrupts is asserted and only
@@ -219,6 +221,15 @@ int arizona_irq_init(struct arizona *arizona)
 		arizona->ctrlif_error = false;
 		break;
 #endif
+#ifdef CONFIG_MFD_CS47L24
+	case WM1831:
+	case CS47L24:
+		aod = NULL;
+		irq = &cs47l24_irq;
+
+		arizona->ctrlif_error = false;
+		break;
+#endif
 #ifdef CONFIG_MFD_WM8997
 	case WM8997:
 		aod = &wm8997_aod;
@@ -291,13 +302,16 @@ int arizona_irq_init(struct arizona *arizona)
 		goto err;
 	}
 
-	ret = regmap_add_irq_chip(arizona->regmap,
-				  irq_create_mapping(arizona->virq, 0),
-				  IRQF_ONESHOT, 0, aod,
-				  &arizona->aod_irq_chip);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to add AOD IRQs: %d\n", ret);
-		goto err_domain;
+	if (aod) {
+		ret = regmap_add_irq_chip(arizona->regmap,
+					  irq_create_mapping(arizona->virq, 0),
+					  IRQF_ONESHOT, 0, aod,
+					  &arizona->aod_irq_chip);
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Failed to add AOD IRQs: %d\n", ret);
+			goto err;
+		}
 	}
 
 	ret = regmap_add_irq_chip(arizona->regmap,
@@ -309,30 +323,6 @@ int arizona_irq_init(struct arizona *arizona)
 		goto err_aod;
 	}
 
-	/* Make sure the boot done IRQ is unmasked for resumes */
-	i = arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE);
-	ret = request_threaded_irq(i, NULL, arizona_boot_done, IRQF_ONESHOT,
-				   "Boot done", arizona);
-	if (ret != 0) {
-		dev_err(arizona->dev, "Failed to request boot done %d: %d\n",
-			arizona->irq, ret);
-		goto err_boot_done;
-	}
-
-	/* Handle control interface errors in the core */
-	if (arizona->ctrlif_error) {
-		i = arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR);
-		ret = request_threaded_irq(i, NULL, arizona_ctrlif_err,
-					   IRQF_ONESHOT,
-					   "Control interface error", arizona);
-		if (ret != 0) {
-			dev_err(arizona->dev,
-				"Failed to request CTRLIF_ERR %d: %d\n",
-				arizona->irq, ret);
-			goto err_ctrlif;
-		}
-	}
-
 	/* Used to emulate edge trigger and to work around broken pinmux */
 	if (arizona->pdata.irq_gpio) {
 		if (gpio_to_irq(arizona->pdata.irq_gpio) != arizona->irq) {
@@ -362,21 +352,42 @@ int arizona_irq_init(struct arizona *arizona)
 		goto err_main_irq;
 	}
 
+	/* Make sure the boot done IRQ is unmasked for resumes */
+	i = arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE);
+	ret = request_threaded_irq(i, NULL, arizona_boot_done, IRQF_ONESHOT,
+				   "Boot done", arizona);
+	if (ret != 0) {
+		dev_err(arizona->dev, "Failed to request boot done %d: %d\n",
+			arizona->irq, ret);
+		goto err_boot_done;
+	}
+
+	/* Handle control interface errors in the core */
+	if (arizona->ctrlif_error) {
+		i = arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR);
+		ret = request_threaded_irq(i, NULL, arizona_ctrlif_err,
+					   IRQF_ONESHOT,
+					   "Control interface error", arizona);
+		if (ret != 0) {
+			dev_err(arizona->dev,
+				"Failed to request CTRLIF_ERR %d: %d\n",
+				arizona->irq, ret);
+			goto err_ctrlif;
+		}
+	}
+
 	return 0;
 
-err_main_irq:
-	if (arizona->ctrlif_error)
-		free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_CTRLIF_ERR),
-			 arizona);
 err_ctrlif:
 	free_irq(arizona_map_irq(arizona, ARIZONA_IRQ_BOOT_DONE), arizona);
 err_boot_done:
+	free_irq(arizona->irq, arizona);
+err_main_irq:
 	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 1),
 			    arizona->irq_chip);
 err_aod:
 	regmap_del_irq_chip(irq_create_mapping(arizona->virq, 0),
 			    arizona->aod_irq_chip);
-err_domain:
 err:
 	return ret;
 }
diff --git a/drivers/mfd/arizona-spi.c b/drivers/mfd/arizona-spi.c
index befbc89bfd34..5c1ccdeb9b70 100644
--- a/drivers/mfd/arizona-spi.c
+++ b/drivers/mfd/arizona-spi.c
@@ -46,6 +46,11 @@ static int arizona_spi_probe(struct spi_device *spi)
 		if (IS_ENABLED(CONFIG_MFD_WM5110))
 			regmap_config = &wm5110_spi_regmap;
 		break;
+	case WM1831:
+	case CS47L24:
+		if (IS_ENABLED(CONFIG_MFD_CS47L24))
+			regmap_config = &cs47l24_spi_regmap;
+		break;
 	default:
 		dev_err(&spi->dev, "Unknown device type %ld\n", type);
 		return -EINVAL;
@@ -89,6 +94,8 @@ static const struct spi_device_id arizona_spi_ids[] = {
 	{ "wm5102", WM5102 },
 	{ "wm5110", WM5110 },
 	{ "wm8280", WM8280 },
+	{ "wm1831", WM1831 },
+	{ "cs47l24", CS47L24 },
 	{ },
 };
 MODULE_DEVICE_TABLE(spi, arizona_spi_ids);
diff --git a/drivers/mfd/arizona.h b/drivers/mfd/arizona.h
index 3af12e938f57..198e9cea77f9 100644
--- a/drivers/mfd/arizona.h
+++ b/drivers/mfd/arizona.h
@@ -25,6 +25,8 @@ extern const struct regmap_config wm5102_spi_regmap;
 extern const struct regmap_config wm5110_i2c_regmap;
 extern const struct regmap_config wm5110_spi_regmap;
 
+extern const struct regmap_config cs47l24_spi_regmap;
+
 extern const struct regmap_config wm8997_i2c_regmap;
 
 extern const struct regmap_config wm8998_i2c_regmap;
@@ -40,6 +42,8 @@ extern const struct regmap_irq_chip wm5110_aod;
 extern const struct regmap_irq_chip wm5110_irq;
 extern const struct regmap_irq_chip wm5110_revd_irq;
 
+extern const struct regmap_irq_chip cs47l24_irq;
+
 extern const struct regmap_irq_chip wm8997_aod;
 extern const struct regmap_irq_chip wm8997_irq;
 
diff --git a/drivers/mfd/as3711.c b/drivers/mfd/as3711.c
index d001f7e238f5..94d67a6e1eb7 100644
--- a/drivers/mfd/as3711.c
+++ b/drivers/mfd/as3711.c
@@ -136,17 +136,13 @@ static int as3711_i2c_probe(struct i2c_client *client,
 	} else {
 		pdata = devm_kzalloc(&client->dev,
 				     sizeof(*pdata), GFP_KERNEL);
-		if (!pdata) {
-			dev_err(&client->dev, "Failed to allocate pdata\n");
+		if (!pdata)
 			return -ENOMEM;
-		}
 	}
 
 	as3711 = devm_kzalloc(&client->dev, sizeof(struct as3711), GFP_KERNEL);
-	if (!as3711) {
-		dev_err(&client->dev, "Memory allocation failed\n");
+	if (!as3711)
 		return -ENOMEM;
-	}
 
 	as3711->dev = &client->dev;
 	i2c_set_clientdata(client, as3711);
@@ -157,7 +153,8 @@ static int as3711_i2c_probe(struct i2c_client *client,
 	as3711->regmap = devm_regmap_init_i2c(client, &as3711_regmap_config);
 	if (IS_ERR(as3711->regmap)) {
 		ret = PTR_ERR(as3711->regmap);
-		dev_err(&client->dev, "regmap initialization failed: %d\n", ret);
+		dev_err(&client->dev,
+			"regmap initialization failed: %d\n", ret);
 		return ret;
 	}
 
@@ -172,12 +169,19 @@ static int as3711_i2c_probe(struct i2c_client *client,
 		return -ENODEV;
 	dev_info(as3711->dev, "AS3711 detected: %x:%x\n", id1, id2);
 
-	/* We can reuse as3711_subdevs[], it will be copied in mfd_add_devices() */
+	/*
+	 * We can reuse as3711_subdevs[],
+	 * it will be copied in mfd_add_devices()
+	 */
 	if (pdata) {
-		as3711_subdevs[AS3711_REGULATOR].platform_data = &pdata->regulator;
-		as3711_subdevs[AS3711_REGULATOR].pdata_size = sizeof(pdata->regulator);
-		as3711_subdevs[AS3711_BACKLIGHT].platform_data = &pdata->backlight;
-		as3711_subdevs[AS3711_BACKLIGHT].pdata_size = sizeof(pdata->backlight);
+		as3711_subdevs[AS3711_REGULATOR].platform_data =
+			&pdata->regulator;
+		as3711_subdevs[AS3711_REGULATOR].pdata_size =
+			sizeof(pdata->regulator);
+		as3711_subdevs[AS3711_BACKLIGHT].platform_data =
+			&pdata->backlight;
+		as3711_subdevs[AS3711_BACKLIGHT].pdata_size =
+			sizeof(pdata->backlight);
 	} else {
 		as3711_subdevs[AS3711_REGULATOR].platform_data = NULL;
 		as3711_subdevs[AS3711_REGULATOR].pdata_size = 0;
diff --git a/drivers/mfd/as3722.c b/drivers/mfd/as3722.c
index 924ea90494ae..e1f597f97f86 100644
--- a/drivers/mfd/as3722.c
+++ b/drivers/mfd/as3722.c
@@ -405,6 +405,8 @@ static int as3722_i2c_probe(struct i2c_client *i2c,
 		goto scrub;
 	}
 
+	device_init_wakeup(as3722->dev, true);
+
 	dev_dbg(as3722->dev, "AS3722 core driver initialized successfully\n");
 	return 0;
 
@@ -422,6 +424,29 @@ static int as3722_i2c_remove(struct i2c_client *i2c)
 	return 0;
 }
 
+static int __maybe_unused as3722_i2c_suspend(struct device *dev)
+{
+	struct as3722 *as3722 = dev_get_drvdata(dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(as3722->chip_irq);
+	disable_irq(as3722->chip_irq);
+
+	return 0;
+}
+
+static int __maybe_unused as3722_i2c_resume(struct device *dev)
+{
+	struct as3722 *as3722 = dev_get_drvdata(dev);
+
+	enable_irq(as3722->chip_irq);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(as3722->chip_irq);
+
+	return 0;
+}
+
 static const struct of_device_id as3722_of_match[] = {
 	{ .compatible = "ams,as3722", },
 	{},
@@ -434,10 +459,15 @@ static const struct i2c_device_id as3722_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, as3722_i2c_id);
 
+static const struct dev_pm_ops as3722_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(as3722_i2c_suspend, as3722_i2c_resume)
+};
+
 static struct i2c_driver as3722_i2c_driver = {
 	.driver = {
 		.name = "as3722",
 		.of_match_table = as3722_of_match,
+		.pm = &as3722_pm_ops,
 	},
 	.probe = as3722_i2c_probe,
 	.remove = as3722_i2c_remove,
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index a726f01e3b02..4dca6bc61f5b 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -167,7 +167,6 @@ static void asic3_irq_demux(struct irq_desc *desc)
 
 				base = ASIC3_GPIO_A_BASE
 				       + bank * ASIC3_GPIO_BASE_INCR;
-
 				spin_lock_irqsave(&asic->lock, flags);
 				istat = asic3_read_register(asic,
 							    base +
@@ -502,7 +501,8 @@ static int asic3_gpio_get(struct gpio_chip *chip,
 		return -EINVAL;
 	}
 
-	return asic3_read_register(asic, gpio_base + ASIC3_GPIO_STATUS) & mask;
+	return !!(asic3_read_register(asic,
+				      gpio_base + ASIC3_GPIO_STATUS) & mask);
 }
 
 static void asic3_gpio_set(struct gpio_chip *chip,
@@ -536,8 +536,6 @@ static void asic3_gpio_set(struct gpio_chip *chip,
 	asic3_write_register(asic, gpio_base + ASIC3_GPIO_OUT, out_reg);
 
 	spin_unlock_irqrestore(&asic->lock, flags);
-
-	return;
 }
 
 static int asic3_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
@@ -665,18 +663,18 @@ static int ds1wm_enable(struct platform_device *pdev)
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_EX0]);
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_EX1]);
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_OWM]);
-	msleep(1);
+	usleep_range(1000, 5000);
 
 	/* Reset and enable DS1WM */
 	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, RESET),
 			   ASIC3_EXTCF_OWM_RESET, 1);
-	msleep(1);
+	usleep_range(1000, 5000);
 	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, RESET),
 			   ASIC3_EXTCF_OWM_RESET, 0);
-	msleep(1);
+	usleep_range(1000, 5000);
 	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, SELECT),
 			   ASIC3_EXTCF_OWM_EN, 1);
-	msleep(1);
+	usleep_range(1000, 5000);
 
 	return 0;
 }
@@ -757,7 +755,7 @@ static int asic3_mmc_enable(struct platform_device *pdev)
 	 * when HCLK is stopped.
 	 */
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_EX1]);
-	msleep(1);
+	usleep_range(1000, 5000);
 
 	/* HCLK 24.576 MHz, BCLK 12.288 MHz: */
 	asic3_write_register(asic, ASIC3_OFFSET(CLOCK, SEL),
@@ -765,7 +763,7 @@ static int asic3_mmc_enable(struct platform_device *pdev)
 
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_SD_HOST]);
 	asic3_clk_enable(asic, &asic->clocks[ASIC3_CLOCK_SD_BUS]);
-	msleep(1);
+	usleep_range(1000, 5000);
 
 	asic3_set_register(asic, ASIC3_OFFSET(EXTCF, SELECT),
 			   ASIC3_EXTCF_SD_MEM_ENABLE, 1);
@@ -841,7 +839,7 @@ static int asic3_leds_suspend(struct platform_device *pdev)
 	struct asic3 *asic = dev_get_drvdata(pdev->dev.parent);
 
 	while (asic3_gpio_get(&asic->gpio, ASIC3_GPIO(C, cell->id)) != 0)
-		msleep(1);
+		usleep_range(1000, 5000);
 
 	asic3_clk_disable(asic, &asic->clocks[clock_ledn[cell->id]]);
 
@@ -900,8 +898,8 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
 
 	/* MMC */
 	if (mem_sdio) {
-		asic->tmio_cnf = ioremap((ASIC3_SD_CONFIG_BASE >> asic->bus_shift) +
-				 mem_sdio->start,
+		asic->tmio_cnf = ioremap((ASIC3_SD_CONFIG_BASE >>
+					  asic->bus_shift) + mem_sdio->start,
 				 ASIC3_SD_CONFIG_SIZE >> asic->bus_shift);
 		if (!asic->tmio_cnf) {
 			ret = -ENOMEM;
@@ -962,10 +960,8 @@ static int __init asic3_probe(struct platform_device *pdev)
 
 	asic = devm_kzalloc(&pdev->dev,
 			    sizeof(struct asic3), GFP_KERNEL);
-	if (asic == NULL) {
-		printk(KERN_ERR "kzalloc failed\n");
+	if (!asic)
 		return -ENOMEM;
-	}
 
 	spin_lock_init(&asic->lock);
 	platform_set_drvdata(pdev, asic);
@@ -1074,7 +1070,9 @@ static struct platform_driver asic3_device_driver = {
 static int __init asic3_init(void)
 {
 	int retval = 0;
+
 	retval = platform_driver_probe(&asic3_device_driver, asic3_probe);
+
 	return retval;
 }
 
diff --git a/drivers/mfd/cros_ec_i2c.c b/drivers/mfd/cros_ec_i2c.c
index 56a466469664..9f70de1e4c70 100644
--- a/drivers/mfd/cros_ec_i2c.c
+++ b/drivers/mfd/cros_ec_i2c.c
@@ -292,7 +292,7 @@ static int cros_ec_i2c_probe(struct i2c_client *client,
 	struct cros_ec_device *ec_dev = NULL;
 	int err;
 
- 	ec_dev = devm_kzalloc(dev, sizeof(*ec_dev), GFP_KERNEL);
+	ec_dev = devm_kzalloc(dev, sizeof(*ec_dev), GFP_KERNEL);
 	if (!ec_dev)
 		return -ENOMEM;
 
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
index 6a0f6ec67c6b..ebe9b9477cb2 100644
--- a/drivers/mfd/cros_ec_spi.c
+++ b/drivers/mfd/cros_ec_spi.c
@@ -113,7 +113,7 @@ static int terminate_request(struct cros_ec_device *ec_dev)
 	trans.delay_usecs = ec_spi->end_of_msg_delay;
 	spi_message_add_tail(&trans, &msg);
 
-	ret = spi_sync(ec_spi->spi, &msg);
+	ret = spi_sync_locked(ec_spi->spi, &msg);
 
 	/* Reset end-of-response timer */
 	ec_spi->last_transfer_ns = ktime_get_ns();
@@ -147,7 +147,7 @@ static int receive_n_bytes(struct cros_ec_device *ec_dev, u8 *buf, int n)
 
 	spi_message_init(&msg);
 	spi_message_add_tail(&trans, &msg);
-	ret = spi_sync(ec_spi->spi, &msg);
+	ret = spi_sync_locked(ec_spi->spi, &msg);
 	if (ret < 0)
 		dev_err(ec_dev->dev, "spi transfer failed: %d\n", ret);
 
@@ -175,7 +175,7 @@ static int cros_ec_spi_receive_packet(struct cros_ec_device *ec_dev,
 	unsigned long deadline;
 	int todo;
 
-	BUG_ON(EC_MSG_PREAMBLE_COUNT > ec_dev->din_size);
+	BUG_ON(ec_dev->din_size < EC_MSG_PREAMBLE_COUNT);
 
 	/* Receive data until we see the header byte */
 	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
@@ -283,7 +283,7 @@ static int cros_ec_spi_receive_response(struct cros_ec_device *ec_dev,
 	unsigned long deadline;
 	int todo;
 
-	BUG_ON(EC_MSG_PREAMBLE_COUNT > ec_dev->din_size);
+	BUG_ON(ec_dev->din_size < EC_MSG_PREAMBLE_COUNT);
 
 	/* Receive data until we see the header byte */
 	deadline = jiffies + msecs_to_jiffies(EC_MSG_DEADLINE_MS);
@@ -391,10 +391,10 @@ static int cros_ec_pkt_xfer_spi(struct cros_ec_device *ec_dev,
 	}
 
 	rx_buf = kzalloc(len, GFP_KERNEL);
-	if (!rx_buf) {
-		ret = -ENOMEM;
-		goto exit;
-	}
+	if (!rx_buf)
+		return -ENOMEM;
+
+	spi_bus_lock(ec_spi->spi->master);
 
 	/*
 	 * Leave a gap between CS assertion and clocking of data to allow the
@@ -414,7 +414,7 @@ static int cros_ec_pkt_xfer_spi(struct cros_ec_device *ec_dev,
 	trans.len = len;
 	trans.cs_change = 1;
 	spi_message_add_tail(&trans, &msg);
-	ret = spi_sync(ec_spi->spi, &msg);
+	ret = spi_sync_locked(ec_spi->spi, &msg);
 
 	/* Get the response */
 	if (!ret) {
@@ -440,6 +440,9 @@ static int cros_ec_pkt_xfer_spi(struct cros_ec_device *ec_dev,
 	}
 
 	final_ret = terminate_request(ec_dev);
+
+	spi_bus_unlock(ec_spi->spi->master);
+
 	if (!ret)
 		ret = final_ret;
 	if (ret < 0)
@@ -520,10 +523,10 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	}
 
 	rx_buf = kzalloc(len, GFP_KERNEL);
-	if (!rx_buf) {
-		ret = -ENOMEM;
-		goto exit;
-	}
+	if (!rx_buf)
+		return -ENOMEM;
+
+	spi_bus_lock(ec_spi->spi->master);
 
 	/* Transmit phase - send our message */
 	debug_packet(ec_dev->dev, "out", ec_dev->dout, len);
@@ -534,7 +537,7 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	trans.cs_change = 1;
 	spi_message_init(&msg);
 	spi_message_add_tail(&trans, &msg);
-	ret = spi_sync(ec_spi->spi, &msg);
+	ret = spi_sync_locked(ec_spi->spi, &msg);
 
 	/* Get the response */
 	if (!ret) {
@@ -560,6 +563,9 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	}
 
 	final_ret = terminate_request(ec_dev);
+
+	spi_bus_unlock(ec_spi->spi->master);
+
 	if (!ret)
 		ret = final_ret;
 	if (ret < 0)
diff --git a/drivers/mfd/cs47l24-tables.c b/drivers/mfd/cs47l24-tables.c
new file mode 100644
index 000000000000..870800657594
--- /dev/null
+++ b/drivers/mfd/cs47l24-tables.c
@@ -0,0 +1,1629 @@
+/*
+ * Data tables for CS47L24 codec
+ *
+ * Copyright 2015 Cirrus Logic, Inc.
+ *
+ * Author: Richard Fitzgerald <rf@opensource.wolfsonmicro.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/mfd/arizona/core.h>
+#include <linux/mfd/arizona/registers.h>
+#include <linux/device.h>
+
+#include "arizona.h"
+
+#define CS47L24_NUM_ISR 5
+
+static const struct reg_sequence cs47l24_reva_patch[] = {
+	{ 0x80,  0x3 },
+	{ 0x27C, 0x0010 },
+	{ 0x221, 0x0070 },
+	{ 0x80,  0x0 },
+};
+
+int cs47l24_patch(struct arizona *arizona)
+{
+	return regmap_register_patch(arizona->regmap,
+				     cs47l24_reva_patch,
+				     ARRAY_SIZE(cs47l24_reva_patch));
+}
+EXPORT_SYMBOL_GPL(cs47l24_patch);
+
+static const struct regmap_irq cs47l24_irqs[ARIZONA_NUM_IRQ] = {
+	[ARIZONA_IRQ_GP2] = { .reg_offset = 0, .mask = ARIZONA_GP2_EINT1 },
+	[ARIZONA_IRQ_GP1] = { .reg_offset = 0, .mask = ARIZONA_GP1_EINT1 },
+
+	[ARIZONA_IRQ_DSP3_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP3_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP2_RAM_RDY] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP2_RAM_RDY_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ8] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ8_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ7] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ7_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ6] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ6_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ5] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ5_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ4] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ4_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ3] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ3_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ2] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ2_EINT1
+	},
+	[ARIZONA_IRQ_DSP_IRQ1] = {
+		.reg_offset = 1, .mask = ARIZONA_DSP_IRQ1_EINT1
+	},
+
+	[ARIZONA_IRQ_SPK_OVERHEAT_WARN] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_OVERHEAT_WARN_EINT1
+	},
+	[ARIZONA_IRQ_SPK_OVERHEAT] = {
+		.reg_offset = 2, .mask = ARIZONA_SPK_OVERHEAT_EINT1
+	},
+	[ARIZONA_IRQ_WSEQ_DONE] = {
+		.reg_offset = 2, .mask = ARIZONA_WSEQ_DONE_EINT1
+	},
+	[ARIZONA_IRQ_DRC2_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC2_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_DRC1_SIG_DET] = {
+		.reg_offset = 2, .mask = ARIZONA_DRC1_SIG_DET_EINT1
+	},
+	[ARIZONA_IRQ_ASRC2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_ASRC1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_ASRC1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_UNDERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_UNDERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_OVERCLOCKED] = {
+		.reg_offset = 2, .mask = ARIZONA_OVERCLOCKED_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL2_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_LOCK] = {
+		.reg_offset = 2, .mask = ARIZONA_FLL1_LOCK_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_EINT1
+	},
+	[ARIZONA_IRQ_CLKGEN_ERR_ASYNC] = {
+		.reg_offset = 2, .mask = ARIZONA_CLKGEN_ERR_ASYNC_EINT1
+	},
+
+	[ARIZONA_IRQ_CTRLIF_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_CTRLIF_ERR_EINT1
+	},
+	[ARIZONA_IRQ_MIXER_DROPPED_SAMPLES] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_MIXER_DROPPED_SAMPLE_EINT1
+	},
+	[ARIZONA_IRQ_ASYNC_CLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_ASYNC_CLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_SYSCLK_ENA_LOW] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_SYSCLK_ENA_LOW_EINT1
+	},
+	[ARIZONA_IRQ_ISRC1_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_ISRC1_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_ISRC2_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_ISRC2_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_ISRC3_CFG_ERR] = {
+		.reg_offset = 3, .mask = ARIZONA_V2_ISRC3_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_HP1R_DONE] = {
+		.reg_offset = 3, .mask = ARIZONA_HP1R_DONE_EINT1
+	},
+	[ARIZONA_IRQ_HP1L_DONE] = {
+		.reg_offset = 3, .mask = ARIZONA_HP1L_DONE_EINT1
+	},
+
+	[ARIZONA_IRQ_BOOT_DONE] = {
+		.reg_offset = 4, .mask = ARIZONA_BOOT_DONE_EINT1
+	},
+	[ARIZONA_IRQ_ASRC_CFG_ERR] = {
+		.reg_offset = 4, .mask = ARIZONA_V2_ASRC_CFG_ERR_EINT1
+	},
+	[ARIZONA_IRQ_FLL2_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL2_CLOCK_OK_EINT1
+	},
+	[ARIZONA_IRQ_FLL1_CLOCK_OK] = {
+		.reg_offset = 4, .mask = ARIZONA_FLL1_CLOCK_OK_EINT1
+	},
+
+	[ARIZONA_IRQ_DSP_SHARED_WR_COLL] = {
+		.reg_offset = 5, .mask = ARIZONA_DSP_SHARED_WR_COLL_EINT1
+	},
+	[ARIZONA_IRQ_SPK_SHUTDOWN] = {
+		.reg_offset = 5, .mask = ARIZONA_SPK_SHUTDOWN_EINT1
+	},
+	[ARIZONA_IRQ_SPK1R_SHORT] = {
+		.reg_offset = 5, .mask = ARIZONA_SPK1R_SHORT_EINT1
+	},
+	[ARIZONA_IRQ_SPK1L_SHORT] = {
+		.reg_offset = 5, .mask = ARIZONA_SPK1L_SHORT_EINT1
+	},
+	[ARIZONA_IRQ_HP1R_SC_POS] = {
+		.reg_offset = 5, .mask = ARIZONA_HP1R_SC_POS_EINT1
+	},
+	[ARIZONA_IRQ_HP1L_SC_POS] = {
+		.reg_offset = 5, .mask = ARIZONA_HP1L_SC_POS_EINT1
+	},
+};
+
+const struct regmap_irq_chip cs47l24_irq = {
+	.name = "cs47l24 IRQ",
+	.status_base = ARIZONA_INTERRUPT_STATUS_1,
+	.mask_base = ARIZONA_INTERRUPT_STATUS_1_MASK,
+	.ack_base = ARIZONA_INTERRUPT_STATUS_1,
+	.num_regs = 6,
+	.irqs = cs47l24_irqs,
+	.num_irqs = ARRAY_SIZE(cs47l24_irqs),
+};
+EXPORT_SYMBOL_GPL(cs47l24_irq);
+
+static const struct reg_default cs47l24_reg_default[] = {
+	{ 0x00000008, 0x0019 },    /* R8     - Ctrl IF SPI CFG 1 */
+	{ 0x00000020, 0x0000 },    /* R32    - Tone Generator 1 */
+	{ 0x00000021, 0x1000 },    /* R33    - Tone Generator 2 */
+	{ 0x00000022, 0x0000 },    /* R34    - Tone Generator 3 */
+	{ 0x00000023, 0x1000 },    /* R35    - Tone Generator 4 */
+	{ 0x00000024, 0x0000 },    /* R36    - Tone Generator 5 */
+	{ 0x00000030, 0x0000 },    /* R48    - PWM Drive 1 */
+	{ 0x00000031, 0x0100 },    /* R49    - PWM Drive 2 */
+	{ 0x00000032, 0x0100 },    /* R50    - PWM Drive 3 */
+	{ 0x00000041, 0x0000 },    /* R65    - Sequence control */
+	{ 0x00000061, 0x01FF },    /* R97    - Sample Rate Sequence Select 1 */
+	{ 0x00000062, 0x01FF },    /* R98    - Sample Rate Sequence Select 2 */
+	{ 0x00000063, 0x01FF },    /* R99    - Sample Rate Sequence Select 3 */
+	{ 0x00000064, 0x01FF },    /* R100   - Sample Rate Sequence Select 4 */
+	{ 0x00000070, 0x0000 },    /* R112   - Comfort Noise Generator */
+	{ 0x00000090, 0x0000 },    /* R144   - Haptics Control 1 */
+	{ 0x00000091, 0x7FFF },    /* R145   - Haptics Control 2 */
+	{ 0x00000092, 0x0000 },    /* R146   - Haptics phase 1 intensity */
+	{ 0x00000093, 0x0000 },    /* R147   - Haptics phase 1 duration */
+	{ 0x00000094, 0x0000 },    /* R148   - Haptics phase 2 intensity */
+	{ 0x00000095, 0x0000 },    /* R149   - Haptics phase 2 duration */
+	{ 0x00000096, 0x0000 },    /* R150   - Haptics phase 3 intensity */
+	{ 0x00000097, 0x0000 },    /* R151   - Haptics phase 3 duration */
+	{ 0x00000100, 0x0002 },    /* R256   - Clock 32k 1 */
+	{ 0x00000101, 0x0504 },    /* R257   - System Clock 1 */
+	{ 0x00000102, 0x0011 },    /* R258   - Sample rate 1 */
+	{ 0x00000103, 0x0011 },    /* R259   - Sample rate 2 */
+	{ 0x00000104, 0x0011 },    /* R260   - Sample rate 3 */
+	{ 0x00000112, 0x0305 },    /* R274   - Async clock 1 */
+	{ 0x00000113, 0x0011 },    /* R275   - Async sample rate 1 */
+	{ 0x00000114, 0x0011 },    /* R276   - Async sample rate 2 */
+	{ 0x00000149, 0x0000 },    /* R329   - Output system clock */
+	{ 0x0000014A, 0x0000 },    /* R330   - Output async clock */
+	{ 0x00000152, 0x0000 },    /* R338   - Rate Estimator 1 */
+	{ 0x00000153, 0x0000 },    /* R339   - Rate Estimator 2 */
+	{ 0x00000154, 0x0000 },    /* R340   - Rate Estimator 3 */
+	{ 0x00000155, 0x0000 },    /* R341   - Rate Estimator 4 */
+	{ 0x00000156, 0x0000 },    /* R342   - Rate Estimator 5 */
+	{ 0x00000171, 0x0002 },    /* R369   - FLL1 Control 1 */
+	{ 0x00000172, 0x0008 },    /* R370   - FLL1 Control 2 */
+	{ 0x00000173, 0x0018 },    /* R371   - FLL1 Control 3 */
+	{ 0x00000174, 0x007D },    /* R372   - FLL1 Control 4 */
+	{ 0x00000175, 0x0006 },    /* R373   - FLL1 Control 5 */
+	{ 0x00000176, 0x0000 },    /* R374   - FLL1 Control 6 */
+	{ 0x00000177, 0x0281 },    /* R375   - FLL1 Loop Filter Test 1 */
+	{ 0x00000178, 0x0000 },    /* R376   - FLL1 NCO Test 0 */
+	{ 0x00000179, 0x0000 },    /* R376   - FLL1 Control 7 */
+	{ 0x00000181, 0x0000 },    /* R385   - FLL1 Synchroniser 1 */
+	{ 0x00000182, 0x0000 },    /* R386   - FLL1 Synchroniser 2 */
+	{ 0x00000183, 0x0000 },    /* R387   - FLL1 Synchroniser 3 */
+	{ 0x00000184, 0x0000 },    /* R388   - FLL1 Synchroniser 4 */
+	{ 0x00000185, 0x0000 },    /* R389   - FLL1 Synchroniser 5 */
+	{ 0x00000186, 0x0000 },    /* R390   - FLL1 Synchroniser 6 */
+	{ 0x00000187, 0x0001 },    /* R390   - FLL1 Synchroniser 7 */
+	{ 0x00000189, 0x0000 },    /* R393   - FLL1 Spread Spectrum */
+	{ 0x0000018A, 0x000C },    /* R394   - FLL1 GPIO Clock */
+	{ 0x00000191, 0x0002 },    /* R401   - FLL2 Control 1 */
+	{ 0x00000192, 0x0008 },    /* R402   - FLL2 Control 2 */
+	{ 0x00000193, 0x0018 },    /* R403   - FLL2 Control 3 */
+	{ 0x00000194, 0x007D },    /* R404   - FLL2 Control 4 */
+	{ 0x00000195, 0x000C },    /* R405   - FLL2 Control 5 */
+	{ 0x00000196, 0x0000 },    /* R406   - FLL2 Control 6 */
+	{ 0x00000197, 0x0000 },    /* R407   - FLL2 Loop Filter Test 1 */
+	{ 0x00000198, 0x0000 },    /* R408   - FLL2 NCO Test 0 */
+	{ 0x00000199, 0x0000 },    /* R408   - FLL2 Control 7 */
+	{ 0x000001A1, 0x0000 },    /* R417   - FLL2 Synchroniser 1 */
+	{ 0x000001A2, 0x0000 },    /* R418   - FLL2 Synchroniser 2 */
+	{ 0x000001A3, 0x0000 },    /* R419   - FLL2 Synchroniser 3 */
+	{ 0x000001A4, 0x0000 },    /* R420   - FLL2 Synchroniser 4 */
+	{ 0x000001A5, 0x0000 },    /* R421   - FLL2 Synchroniser 5 */
+	{ 0x000001A6, 0x0000 },    /* R422   - FLL2 Synchroniser 6 */
+	{ 0x000001A7, 0x0001 },    /* R422   - FLL2 Synchroniser 7 */
+	{ 0x000001A9, 0x0000 },    /* R425   - FLL2 Spread Spectrum */
+	{ 0x000001AA, 0x000C },    /* R426   - FLL2 GPIO Clock */
+	{ 0x00000218, 0x00E6 },    /* R536   - Mic Bias Ctrl 1 */
+	{ 0x00000219, 0x00E6 },    /* R537   - Mic Bias Ctrl 2 */
+	{ 0x00000300, 0x0000 },    /* R768   - Input Enables */
+	{ 0x00000308, 0x0000 },    /* R776   - Input Rate */
+	{ 0x00000309, 0x0022 },    /* R777   - Input Volume Ramp */
+	{ 0x0000030C, 0x0002 },    /* R780   - HPF Control */
+	{ 0x00000310, 0x2000 },    /* R784   - IN1L Control */
+	{ 0x00000311, 0x0180 },    /* R785   - ADC Digital Volume 1L */
+	{ 0x00000312, 0x0000 },    /* R786   - DMIC1L Control */
+	{ 0x00000314, 0x0000 },    /* R788   - IN1R Control */
+	{ 0x00000315, 0x0180 },    /* R789   - ADC Digital Volume 1R */
+	{ 0x00000316, 0x0000 },    /* R790   - DMIC1R Control */
+	{ 0x00000318, 0x2000 },    /* R792   - IN2L Control */
+	{ 0x00000319, 0x0180 },    /* R793   - ADC Digital Volume 2L */
+	{ 0x0000031A, 0x0000 },    /* R794   - DMIC2L Control */
+	{ 0x0000031C, 0x0000 },    /* R796   - IN2R Control */
+	{ 0x0000031D, 0x0180 },    /* R797   - ADC Digital Volume 2R */
+	{ 0x0000031E, 0x0000 },    /* R798   - DMIC2R Control */
+	{ 0x00000400, 0x0000 },    /* R1024  - Output Enables 1 */
+	{ 0x00000408, 0x0000 },    /* R1032  - Output Rate 1 */
+	{ 0x00000409, 0x0022 },    /* R1033  - Output Volume Ramp */
+	{ 0x00000410, 0x0080 },    /* R1040  - Output Path Config 1L */
+	{ 0x00000411, 0x0180 },    /* R1041  - DAC Digital Volume 1L */
+	{ 0x00000412, 0x0081 },    /* R1042  - DAC Volume Limit 1L */
+	{ 0x00000413, 0x0001 },    /* R1043  - Noise Gate Select 1L */
+	{ 0x00000415, 0x0180 },    /* R1045  - DAC Digital Volume 1R */
+	{ 0x00000416, 0x0081 },    /* R1046  - DAC Volume Limit 1R */
+	{ 0x00000417, 0x0002 },    /* R1047  - Noise Gate Select 1R */
+	{ 0x00000429, 0x0180 },    /* R1065  - DAC Digital Volume 4L */
+	{ 0x0000042A, 0x0081 },    /* R1066  - Out Volume 4L */
+	{ 0x0000042B, 0x0040 },    /* R1067  - Noise Gate Select 4L */
+	{ 0x00000450, 0x0000 },    /* R1104  - DAC AEC Control 1 */
+	{ 0x00000458, 0x0000 },    /* R1112  - Noise Gate Control */
+	{ 0x000004A0, 0x3480 },    /* R1184  - HP1 Short Circuit Ctrl */
+	{ 0x00000500, 0x000C },    /* R1280  - AIF1 BCLK Ctrl */
+	{ 0x00000501, 0x0008 },    /* R1281  - AIF1 Tx Pin Ctrl */
+	{ 0x00000502, 0x0000 },    /* R1282  - AIF1 Rx Pin Ctrl */
+	{ 0x00000503, 0x0000 },    /* R1283  - AIF1 Rate Ctrl */
+	{ 0x00000504, 0x0000 },    /* R1284  - AIF1 Format */
+	{ 0x00000506, 0x0040 },    /* R1286  - AIF1 Rx BCLK Rate */
+	{ 0x00000507, 0x1818 },    /* R1287  - AIF1 Frame Ctrl 1 */
+	{ 0x00000508, 0x1818 },    /* R1288  - AIF1 Frame Ctrl 2 */
+	{ 0x00000509, 0x0000 },    /* R1289  - AIF1 Frame Ctrl 3 */
+	{ 0x0000050A, 0x0001 },    /* R1290  - AIF1 Frame Ctrl 4 */
+	{ 0x0000050B, 0x0002 },    /* R1291  - AIF1 Frame Ctrl 5 */
+	{ 0x0000050C, 0x0003 },    /* R1292  - AIF1 Frame Ctrl 6 */
+	{ 0x0000050D, 0x0004 },    /* R1293  - AIF1 Frame Ctrl 7 */
+	{ 0x0000050E, 0x0005 },    /* R1294  - AIF1 Frame Ctrl 8 */
+	{ 0x0000050F, 0x0006 },    /* R1295  - AIF1 Frame Ctrl 9 */
+	{ 0x00000510, 0x0007 },    /* R1296  - AIF1 Frame Ctrl 10 */
+	{ 0x00000511, 0x0000 },    /* R1297  - AIF1 Frame Ctrl 11 */
+	{ 0x00000512, 0x0001 },    /* R1298  - AIF1 Frame Ctrl 12 */
+	{ 0x00000513, 0x0002 },    /* R1299  - AIF1 Frame Ctrl 13 */
+	{ 0x00000514, 0x0003 },    /* R1300  - AIF1 Frame Ctrl 14 */
+	{ 0x00000515, 0x0004 },    /* R1301  - AIF1 Frame Ctrl 15 */
+	{ 0x00000516, 0x0005 },    /* R1302  - AIF1 Frame Ctrl 16 */
+	{ 0x00000517, 0x0006 },    /* R1303  - AIF1 Frame Ctrl 17 */
+	{ 0x00000518, 0x0007 },    /* R1304  - AIF1 Frame Ctrl 18 */
+	{ 0x00000519, 0x0000 },    /* R1305  - AIF1 Tx Enables */
+	{ 0x0000051A, 0x0000 },    /* R1306  - AIF1 Rx Enables */
+	{ 0x00000540, 0x000C },    /* R1344  - AIF2 BCLK Ctrl */
+	{ 0x00000541, 0x0008 },    /* R1345  - AIF2 Tx Pin Ctrl */
+	{ 0x00000542, 0x0000 },    /* R1346  - AIF2 Rx Pin Ctrl */
+	{ 0x00000543, 0x0000 },    /* R1347  - AIF2 Rate Ctrl */
+	{ 0x00000544, 0x0000 },    /* R1348  - AIF2 Format */
+	{ 0x00000546, 0x0040 },    /* R1350  - AIF2 Rx BCLK Rate */
+	{ 0x00000547, 0x1818 },    /* R1351  - AIF2 Frame Ctrl 1 */
+	{ 0x00000548, 0x1818 },    /* R1352  - AIF2 Frame Ctrl 2 */
+	{ 0x00000549, 0x0000 },    /* R1353  - AIF2 Frame Ctrl 3 */
+	{ 0x0000054A, 0x0001 },    /* R1354  - AIF2 Frame Ctrl 4 */
+	{ 0x0000054B, 0x0002 },    /* R1355  - AIF2 Frame Ctrl 5 */
+	{ 0x0000054C, 0x0003 },    /* R1356  - AIF2 Frame Ctrl 6 */
+	{ 0x0000054D, 0x0004 },    /* R1357  - AIF2 Frame Ctrl 7 */
+	{ 0x0000054E, 0x0005 },    /* R1358  - AIF2 Frame Ctrl 8 */
+	{ 0x00000551, 0x0000 },    /* R1361  - AIF2 Frame Ctrl 11 */
+	{ 0x00000552, 0x0001 },    /* R1362  - AIF2 Frame Ctrl 12 */
+	{ 0x00000553, 0x0002 },    /* R1363  - AIF2 Frame Ctrl 13 */
+	{ 0x00000554, 0x0003 },    /* R1364  - AIF2 Frame Ctrl 14 */
+	{ 0x00000555, 0x0004 },    /* R1365  - AIF2 Frame Ctrl 15 */
+	{ 0x00000556, 0x0005 },    /* R1366  - AIF2 Frame Ctrl 16 */
+	{ 0x00000559, 0x0000 },    /* R1369  - AIF2 Tx Enables */
+	{ 0x0000055A, 0x0000 },    /* R1370  - AIF2 Rx Enables */
+	{ 0x00000580, 0x000C },    /* R1408  - AIF3 BCLK Ctrl */
+	{ 0x00000581, 0x0008 },    /* R1409  - AIF3 Tx Pin Ctrl */
+	{ 0x00000582, 0x0000 },    /* R1410  - AIF3 Rx Pin Ctrl */
+	{ 0x00000583, 0x0000 },    /* R1411  - AIF3 Rate Ctrl */
+	{ 0x00000584, 0x0000 },    /* R1412  - AIF3 Format */
+	{ 0x00000586, 0x0040 },    /* R1414  - AIF3 Rx BCLK Rate */
+	{ 0x00000587, 0x1818 },    /* R1415  - AIF3 Frame Ctrl 1 */
+	{ 0x00000588, 0x1818 },    /* R1416  - AIF3 Frame Ctrl 2 */
+	{ 0x00000589, 0x0000 },    /* R1417  - AIF3 Frame Ctrl 3 */
+	{ 0x0000058A, 0x0001 },    /* R1418  - AIF3 Frame Ctrl 4 */
+	{ 0x00000591, 0x0000 },    /* R1425  - AIF3 Frame Ctrl 11 */
+	{ 0x00000592, 0x0001 },    /* R1426  - AIF3 Frame Ctrl 12 */
+	{ 0x00000599, 0x0000 },    /* R1433  - AIF3 Tx Enables */
+	{ 0x0000059A, 0x0000 },    /* R1434  - AIF3 Rx Enables */
+	{ 0x00000640, 0x0000 },    /* R1600  - PWM1MIX Input 1 Source */
+	{ 0x00000641, 0x0080 },    /* R1601  - PWM1MIX Input 1 Volume */
+	{ 0x00000642, 0x0000 },    /* R1602  - PWM1MIX Input 2 Source */
+	{ 0x00000643, 0x0080 },    /* R1603  - PWM1MIX Input 2 Volume */
+	{ 0x00000644, 0x0000 },    /* R1604  - PWM1MIX Input 3 Source */
+	{ 0x00000645, 0x0080 },    /* R1605  - PWM1MIX Input 3 Volume */
+	{ 0x00000646, 0x0000 },    /* R1606  - PWM1MIX Input 4 Source */
+	{ 0x00000647, 0x0080 },    /* R1607  - PWM1MIX Input 4 Volume */
+	{ 0x00000648, 0x0000 },    /* R1608  - PWM2MIX Input 1 Source */
+	{ 0x00000649, 0x0080 },    /* R1609  - PWM2MIX Input 1 Volume */
+	{ 0x0000064A, 0x0000 },    /* R1610  - PWM2MIX Input 2 Source */
+	{ 0x0000064B, 0x0080 },    /* R1611  - PWM2MIX Input 2 Volume */
+	{ 0x0000064C, 0x0000 },    /* R1612  - PWM2MIX Input 3 Source */
+	{ 0x0000064D, 0x0080 },    /* R1613  - PWM2MIX Input 3 Volume */
+	{ 0x0000064E, 0x0000 },    /* R1614  - PWM2MIX Input 4 Source */
+	{ 0x0000064F, 0x0080 },    /* R1615  - PWM2MIX Input 4 Volume */
+	{ 0x00000680, 0x0000 },    /* R1664  - OUT1LMIX Input 1 Source */
+	{ 0x00000681, 0x0080 },    /* R1665  - OUT1LMIX Input 1 Volume */
+	{ 0x00000682, 0x0000 },    /* R1666  - OUT1LMIX Input 2 Source */
+	{ 0x00000683, 0x0080 },    /* R1667  - OUT1LMIX Input 2 Volume */
+	{ 0x00000684, 0x0000 },    /* R1668  - OUT1LMIX Input 3 Source */
+	{ 0x00000685, 0x0080 },    /* R1669  - OUT1LMIX Input 3 Volume */
+	{ 0x00000686, 0x0000 },    /* R1670  - OUT1LMIX Input 4 Source */
+	{ 0x00000687, 0x0080 },    /* R1671  - OUT1LMIX Input 4 Volume */
+	{ 0x00000688, 0x0000 },    /* R1672  - OUT1RMIX Input 1 Source */
+	{ 0x00000689, 0x0080 },    /* R1673  - OUT1RMIX Input 1 Volume */
+	{ 0x0000068A, 0x0000 },    /* R1674  - OUT1RMIX Input 2 Source */
+	{ 0x0000068B, 0x0080 },    /* R1675  - OUT1RMIX Input 2 Volume */
+	{ 0x0000068C, 0x0000 },    /* R1676  - OUT1RMIX Input 3 Source */
+	{ 0x0000068D, 0x0080 },    /* R1677  - OUT1RMIX Input 3 Volume */
+	{ 0x0000068E, 0x0000 },    /* R1678  - OUT1RMIX Input 4 Source */
+	{ 0x0000068F, 0x0080 },    /* R1679  - OUT1RMIX Input 4 Volume */
+	{ 0x000006B0, 0x0000 },    /* R1712  - OUT4LMIX Input 1 Source */
+	{ 0x000006B1, 0x0080 },    /* R1713  - OUT4LMIX Input 1 Volume */
+	{ 0x000006B2, 0x0000 },    /* R1714  - OUT4LMIX Input 2 Source */
+	{ 0x000006B3, 0x0080 },    /* R1715  - OUT4LMIX Input 2 Volume */
+	{ 0x000006B4, 0x0000 },    /* R1716  - OUT4LMIX Input 3 Source */
+	{ 0x000006B5, 0x0080 },    /* R1717  - OUT4LMIX Input 3 Volume */
+	{ 0x000006B6, 0x0000 },    /* R1718  - OUT4LMIX Input 4 Source */
+	{ 0x000006B7, 0x0080 },    /* R1719  - OUT4LMIX Input 4 Volume */
+	{ 0x00000700, 0x0000 },    /* R1792  - AIF1TX1MIX Input 1 Source */
+	{ 0x00000701, 0x0080 },    /* R1793  - AIF1TX1MIX Input 1 Volume */
+	{ 0x00000702, 0x0000 },    /* R1794  - AIF1TX1MIX Input 2 Source */
+	{ 0x00000703, 0x0080 },    /* R1795  - AIF1TX1MIX Input 2 Volume */
+	{ 0x00000704, 0x0000 },    /* R1796  - AIF1TX1MIX Input 3 Source */
+	{ 0x00000705, 0x0080 },    /* R1797  - AIF1TX1MIX Input 3 Volume */
+	{ 0x00000706, 0x0000 },    /* R1798  - AIF1TX1MIX Input 4 Source */
+	{ 0x00000707, 0x0080 },    /* R1799  - AIF1TX1MIX Input 4 Volume */
+	{ 0x00000708, 0x0000 },    /* R1800  - AIF1TX2MIX Input 1 Source */
+	{ 0x00000709, 0x0080 },    /* R1801  - AIF1TX2MIX Input 1 Volume */
+	{ 0x0000070A, 0x0000 },    /* R1802  - AIF1TX2MIX Input 2 Source */
+	{ 0x0000070B, 0x0080 },    /* R1803  - AIF1TX2MIX Input 2 Volume */
+	{ 0x0000070C, 0x0000 },    /* R1804  - AIF1TX2MIX Input 3 Source */
+	{ 0x0000070D, 0x0080 },    /* R1805  - AIF1TX2MIX Input 3 Volume */
+	{ 0x0000070E, 0x0000 },    /* R1806  - AIF1TX2MIX Input 4 Source */
+	{ 0x0000070F, 0x0080 },    /* R1807  - AIF1TX2MIX Input 4 Volume */
+	{ 0x00000710, 0x0000 },    /* R1808  - AIF1TX3MIX Input 1 Source */
+	{ 0x00000711, 0x0080 },    /* R1809  - AIF1TX3MIX Input 1 Volume */
+	{ 0x00000712, 0x0000 },    /* R1810  - AIF1TX3MIX Input 2 Source */
+	{ 0x00000713, 0x0080 },    /* R1811  - AIF1TX3MIX Input 2 Volume */
+	{ 0x00000714, 0x0000 },    /* R1812  - AIF1TX3MIX Input 3 Source */
+	{ 0x00000715, 0x0080 },    /* R1813  - AIF1TX3MIX Input 3 Volume */
+	{ 0x00000716, 0x0000 },    /* R1814  - AIF1TX3MIX Input 4 Source */
+	{ 0x00000717, 0x0080 },    /* R1815  - AIF1TX3MIX Input 4 Volume */
+	{ 0x00000718, 0x0000 },    /* R1816  - AIF1TX4MIX Input 1 Source */
+	{ 0x00000719, 0x0080 },    /* R1817  - AIF1TX4MIX Input 1 Volume */
+	{ 0x0000071A, 0x0000 },    /* R1818  - AIF1TX4MIX Input 2 Source */
+	{ 0x0000071B, 0x0080 },    /* R1819  - AIF1TX4MIX Input 2 Volume */
+	{ 0x0000071C, 0x0000 },    /* R1820  - AIF1TX4MIX Input 3 Source */
+	{ 0x0000071D, 0x0080 },    /* R1821  - AIF1TX4MIX Input 3 Volume */
+	{ 0x0000071E, 0x0000 },    /* R1822  - AIF1TX4MIX Input 4 Source */
+	{ 0x0000071F, 0x0080 },    /* R1823  - AIF1TX4MIX Input 4 Volume */
+	{ 0x00000720, 0x0000 },    /* R1824  - AIF1TX5MIX Input 1 Source */
+	{ 0x00000721, 0x0080 },    /* R1825  - AIF1TX5MIX Input 1 Volume */
+	{ 0x00000722, 0x0000 },    /* R1826  - AIF1TX5MIX Input 2 Source */
+	{ 0x00000723, 0x0080 },    /* R1827  - AIF1TX5MIX Input 2 Volume */
+	{ 0x00000724, 0x0000 },    /* R1828  - AIF1TX5MIX Input 3 Source */
+	{ 0x00000725, 0x0080 },    /* R1829  - AIF1TX5MIX Input 3 Volume */
+	{ 0x00000726, 0x0000 },    /* R1830  - AIF1TX5MIX Input 4 Source */
+	{ 0x00000727, 0x0080 },    /* R1831  - AIF1TX5MIX Input 4 Volume */
+	{ 0x00000728, 0x0000 },    /* R1832  - AIF1TX6MIX Input 1 Source */
+	{ 0x00000729, 0x0080 },    /* R1833  - AIF1TX6MIX Input 1 Volume */
+	{ 0x0000072A, 0x0000 },    /* R1834  - AIF1TX6MIX Input 2 Source */
+	{ 0x0000072B, 0x0080 },    /* R1835  - AIF1TX6MIX Input 2 Volume */
+	{ 0x0000072C, 0x0000 },    /* R1836  - AIF1TX6MIX Input 3 Source */
+	{ 0x0000072D, 0x0080 },    /* R1837  - AIF1TX6MIX Input 3 Volume */
+	{ 0x0000072E, 0x0000 },    /* R1838  - AIF1TX6MIX Input 4 Source */
+	{ 0x0000072F, 0x0080 },    /* R1839  - AIF1TX6MIX Input 4 Volume */
+	{ 0x00000730, 0x0000 },    /* R1840  - AIF1TX7MIX Input 1 Source */
+	{ 0x00000731, 0x0080 },    /* R1841  - AIF1TX7MIX Input 1 Volume */
+	{ 0x00000732, 0x0000 },    /* R1842  - AIF1TX7MIX Input 2 Source */
+	{ 0x00000733, 0x0080 },    /* R1843  - AIF1TX7MIX Input 2 Volume */
+	{ 0x00000734, 0x0000 },    /* R1844  - AIF1TX7MIX Input 3 Source */
+	{ 0x00000735, 0x0080 },    /* R1845  - AIF1TX7MIX Input 3 Volume */
+	{ 0x00000736, 0x0000 },    /* R1846  - AIF1TX7MIX Input 4 Source */
+	{ 0x00000737, 0x0080 },    /* R1847  - AIF1TX7MIX Input 4 Volume */
+	{ 0x00000738, 0x0000 },    /* R1848  - AIF1TX8MIX Input 1 Source */
+	{ 0x00000739, 0x0080 },    /* R1849  - AIF1TX8MIX Input 1 Volume */
+	{ 0x0000073A, 0x0000 },    /* R1850  - AIF1TX8MIX Input 2 Source */
+	{ 0x0000073B, 0x0080 },    /* R1851  - AIF1TX8MIX Input 2 Volume */
+	{ 0x0000073C, 0x0000 },    /* R1852  - AIF1TX8MIX Input 3 Source */
+	{ 0x0000073D, 0x0080 },    /* R1853  - AIF1TX8MIX Input 3 Volume */
+	{ 0x0000073E, 0x0000 },    /* R1854  - AIF1TX8MIX Input 4 Source */
+	{ 0x0000073F, 0x0080 },    /* R1855  - AIF1TX8MIX Input 4 Volume */
+	{ 0x00000740, 0x0000 },    /* R1856  - AIF2TX1MIX Input 1 Source */
+	{ 0x00000741, 0x0080 },    /* R1857  - AIF2TX1MIX Input 1 Volume */
+	{ 0x00000742, 0x0000 },    /* R1858  - AIF2TX1MIX Input 2 Source */
+	{ 0x00000743, 0x0080 },    /* R1859  - AIF2TX1MIX Input 2 Volume */
+	{ 0x00000744, 0x0000 },    /* R1860  - AIF2TX1MIX Input 3 Source */
+	{ 0x00000745, 0x0080 },    /* R1861  - AIF2TX1MIX Input 3 Volume */
+	{ 0x00000746, 0x0000 },    /* R1862  - AIF2TX1MIX Input 4 Source */
+	{ 0x00000747, 0x0080 },    /* R1863  - AIF2TX1MIX Input 4 Volume */
+	{ 0x00000748, 0x0000 },    /* R1864  - AIF2TX2MIX Input 1 Source */
+	{ 0x00000749, 0x0080 },    /* R1865  - AIF2TX2MIX Input 1 Volume */
+	{ 0x0000074A, 0x0000 },    /* R1866  - AIF2TX2MIX Input 2 Source */
+	{ 0x0000074B, 0x0080 },    /* R1867  - AIF2TX2MIX Input 2 Volume */
+	{ 0x0000074C, 0x0000 },    /* R1868  - AIF2TX2MIX Input 3 Source */
+	{ 0x0000074D, 0x0080 },    /* R1869  - AIF2TX2MIX Input 3 Volume */
+	{ 0x0000074E, 0x0000 },    /* R1870  - AIF2TX2MIX Input 4 Source */
+	{ 0x0000074F, 0x0080 },    /* R1871  - AIF2TX2MIX Input 4 Volume */
+	{ 0x00000750, 0x0000 },    /* R1872  - AIF2TX3MIX Input 1 Source */
+	{ 0x00000751, 0x0080 },    /* R1873  - AIF2TX3MIX Input 1 Volume */
+	{ 0x00000752, 0x0000 },    /* R1874  - AIF2TX3MIX Input 2 Source */
+	{ 0x00000753, 0x0080 },    /* R1875  - AIF2TX3MIX Input 2 Volume */
+	{ 0x00000754, 0x0000 },    /* R1876  - AIF2TX3MIX Input 3 Source */
+	{ 0x00000755, 0x0080 },    /* R1877  - AIF2TX3MIX Input 3 Volume */
+	{ 0x00000756, 0x0000 },    /* R1878  - AIF2TX3MIX Input 4 Source */
+	{ 0x00000757, 0x0080 },    /* R1879  - AIF2TX3MIX Input 4 Volume */
+	{ 0x00000758, 0x0000 },    /* R1880  - AIF2TX4MIX Input 1 Source */
+	{ 0x00000759, 0x0080 },    /* R1881  - AIF2TX4MIX Input 1 Volume */
+	{ 0x0000075A, 0x0000 },    /* R1882  - AIF2TX4MIX Input 2 Source */
+	{ 0x0000075B, 0x0080 },    /* R1883  - AIF2TX4MIX Input 2 Volume */
+	{ 0x0000075C, 0x0000 },    /* R1884  - AIF2TX4MIX Input 3 Source */
+	{ 0x0000075D, 0x0080 },    /* R1885  - AIF2TX4MIX Input 3 Volume */
+	{ 0x0000075E, 0x0000 },    /* R1886  - AIF2TX4MIX Input 4 Source */
+	{ 0x0000075F, 0x0080 },    /* R1887  - AIF2TX4MIX Input 4 Volume */
+	{ 0x00000760, 0x0000 },    /* R1888  - AIF2TX5MIX Input 1 Source */
+	{ 0x00000761, 0x0080 },    /* R1889  - AIF2TX5MIX Input 1 Volume */
+	{ 0x00000762, 0x0000 },    /* R1890  - AIF2TX5MIX Input 2 Source */
+	{ 0x00000763, 0x0080 },    /* R1891  - AIF2TX5MIX Input 2 Volume */
+	{ 0x00000764, 0x0000 },    /* R1892  - AIF2TX5MIX Input 3 Source */
+	{ 0x00000765, 0x0080 },    /* R1893  - AIF2TX5MIX Input 3 Volume */
+	{ 0x00000766, 0x0000 },    /* R1894  - AIF2TX5MIX Input 4 Source */
+	{ 0x00000767, 0x0080 },    /* R1895  - AIF2TX5MIX Input 4 Volume */
+	{ 0x00000768, 0x0000 },    /* R1896  - AIF2TX6MIX Input 1 Source */
+	{ 0x00000769, 0x0080 },    /* R1897  - AIF2TX6MIX Input 1 Volume */
+	{ 0x0000076A, 0x0000 },    /* R1898  - AIF2TX6MIX Input 2 Source */
+	{ 0x0000076B, 0x0080 },    /* R1899  - AIF2TX6MIX Input 2 Volume */
+	{ 0x0000076C, 0x0000 },    /* R1900  - AIF2TX6MIX Input 3 Source */
+	{ 0x0000076D, 0x0080 },    /* R1901  - AIF2TX6MIX Input 3 Volume */
+	{ 0x0000076E, 0x0000 },    /* R1902  - AIF2TX6MIX Input 4 Source */
+	{ 0x0000076F, 0x0080 },    /* R1903  - AIF2TX6MIX Input 4 Volume */
+	{ 0x00000780, 0x0000 },    /* R1920  - AIF3TX1MIX Input 1 Source */
+	{ 0x00000781, 0x0080 },    /* R1921  - AIF3TX1MIX Input 1 Volume */
+	{ 0x00000782, 0x0000 },    /* R1922  - AIF3TX1MIX Input 2 Source */
+	{ 0x00000783, 0x0080 },    /* R1923  - AIF3TX1MIX Input 2 Volume */
+	{ 0x00000784, 0x0000 },    /* R1924  - AIF3TX1MIX Input 3 Source */
+	{ 0x00000785, 0x0080 },    /* R1925  - AIF3TX1MIX Input 3 Volume */
+	{ 0x00000786, 0x0000 },    /* R1926  - AIF3TX1MIX Input 4 Source */
+	{ 0x00000787, 0x0080 },    /* R1927  - AIF3TX1MIX Input 4 Volume */
+	{ 0x00000788, 0x0000 },    /* R1928  - AIF3TX2MIX Input 1 Source */
+	{ 0x00000789, 0x0080 },    /* R1929  - AIF3TX2MIX Input 1 Volume */
+	{ 0x0000078A, 0x0000 },    /* R1930  - AIF3TX2MIX Input 2 Source */
+	{ 0x0000078B, 0x0080 },    /* R1931  - AIF3TX2MIX Input 2 Volume */
+	{ 0x0000078C, 0x0000 },    /* R1932  - AIF3TX2MIX Input 3 Source */
+	{ 0x0000078D, 0x0080 },    /* R1933  - AIF3TX2MIX Input 3 Volume */
+	{ 0x0000078E, 0x0000 },    /* R1934  - AIF3TX2MIX Input 4 Source */
+	{ 0x0000078F, 0x0080 },    /* R1935  - AIF3TX2MIX Input 4 Volume */
+	{ 0x00000880, 0x0000 },    /* R2176  - EQ1MIX Input 1 Source */
+	{ 0x00000881, 0x0080 },    /* R2177  - EQ1MIX Input 1 Volume */
+	{ 0x00000882, 0x0000 },    /* R2178  - EQ1MIX Input 2 Source */
+	{ 0x00000883, 0x0080 },    /* R2179  - EQ1MIX Input 2 Volume */
+	{ 0x00000884, 0x0000 },    /* R2180  - EQ1MIX Input 3 Source */
+	{ 0x00000885, 0x0080 },    /* R2181  - EQ1MIX Input 3 Volume */
+	{ 0x00000886, 0x0000 },    /* R2182  - EQ1MIX Input 4 Source */
+	{ 0x00000887, 0x0080 },    /* R2183  - EQ1MIX Input 4 Volume */
+	{ 0x00000888, 0x0000 },    /* R2184  - EQ2MIX Input 1 Source */
+	{ 0x00000889, 0x0080 },    /* R2185  - EQ2MIX Input 1 Volume */
+	{ 0x0000088A, 0x0000 },    /* R2186  - EQ2MIX Input 2 Source */
+	{ 0x0000088B, 0x0080 },    /* R2187  - EQ2MIX Input 2 Volume */
+	{ 0x0000088C, 0x0000 },    /* R2188  - EQ2MIX Input 3 Source */
+	{ 0x0000088D, 0x0080 },    /* R2189  - EQ2MIX Input 3 Volume */
+	{ 0x0000088E, 0x0000 },    /* R2190  - EQ2MIX Input 4 Source */
+	{ 0x0000088F, 0x0080 },    /* R2191  - EQ2MIX Input 4 Volume */
+	{ 0x000008C0, 0x0000 },    /* R2240  - DRC1LMIX Input 1 Source */
+	{ 0x000008C1, 0x0080 },    /* R2241  - DRC1LMIX Input 1 Volume */
+	{ 0x000008C2, 0x0000 },    /* R2242  - DRC1LMIX Input 2 Source */
+	{ 0x000008C3, 0x0080 },    /* R2243  - DRC1LMIX Input 2 Volume */
+	{ 0x000008C4, 0x0000 },    /* R2244  - DRC1LMIX Input 3 Source */
+	{ 0x000008C5, 0x0080 },    /* R2245  - DRC1LMIX Input 3 Volume */
+	{ 0x000008C6, 0x0000 },    /* R2246  - DRC1LMIX Input 4 Source */
+	{ 0x000008C7, 0x0080 },    /* R2247  - DRC1LMIX Input 4 Volume */
+	{ 0x000008C8, 0x0000 },    /* R2248  - DRC1RMIX Input 1 Source */
+	{ 0x000008C9, 0x0080 },    /* R2249  - DRC1RMIX Input 1 Volume */
+	{ 0x000008CA, 0x0000 },    /* R2250  - DRC1RMIX Input 2 Source */
+	{ 0x000008CB, 0x0080 },    /* R2251  - DRC1RMIX Input 2 Volume */
+	{ 0x000008CC, 0x0000 },    /* R2252  - DRC1RMIX Input 3 Source */
+	{ 0x000008CD, 0x0080 },    /* R2253  - DRC1RMIX Input 3 Volume */
+	{ 0x000008CE, 0x0000 },    /* R2254  - DRC1RMIX Input 4 Source */
+	{ 0x000008CF, 0x0080 },    /* R2255  - DRC1RMIX Input 4 Volume */
+	{ 0x000008D0, 0x0000 },    /* R2256  - DRC2LMIX Input 1 Source */
+	{ 0x000008D1, 0x0080 },    /* R2257  - DRC2LMIX Input 1 Volume */
+	{ 0x000008D2, 0x0000 },    /* R2258  - DRC2LMIX Input 2 Source */
+	{ 0x000008D3, 0x0080 },    /* R2259  - DRC2LMIX Input 2 Volume */
+	{ 0x000008D4, 0x0000 },    /* R2260  - DRC2LMIX Input 3 Source */
+	{ 0x000008D5, 0x0080 },    /* R2261  - DRC2LMIX Input 3 Volume */
+	{ 0x000008D6, 0x0000 },    /* R2262  - DRC2LMIX Input 4 Source */
+	{ 0x000008D7, 0x0080 },    /* R2263  - DRC2LMIX Input 4 Volume */
+	{ 0x000008D8, 0x0000 },    /* R2264  - DRC2RMIX Input 1 Source */
+	{ 0x000008D9, 0x0080 },    /* R2265  - DRC2RMIX Input 1 Volume */
+	{ 0x000008DA, 0x0000 },    /* R2266  - DRC2RMIX Input 2 Source */
+	{ 0x000008DB, 0x0080 },    /* R2267  - DRC2RMIX Input 2 Volume */
+	{ 0x000008DC, 0x0000 },    /* R2268  - DRC2RMIX Input 3 Source */
+	{ 0x000008DD, 0x0080 },    /* R2269  - DRC2RMIX Input 3 Volume */
+	{ 0x000008DE, 0x0000 },    /* R2270  - DRC2RMIX Input 4 Source */
+	{ 0x000008DF, 0x0080 },    /* R2271  - DRC2RMIX Input 4 Volume */
+	{ 0x00000900, 0x0000 },    /* R2304  - HPLP1MIX Input 1 Source */
+	{ 0x00000901, 0x0080 },    /* R2305  - HPLP1MIX Input 1 Volume */
+	{ 0x00000902, 0x0000 },    /* R2306  - HPLP1MIX Input 2 Source */
+	{ 0x00000903, 0x0080 },    /* R2307  - HPLP1MIX Input 2 Volume */
+	{ 0x00000904, 0x0000 },    /* R2308  - HPLP1MIX Input 3 Source */
+	{ 0x00000905, 0x0080 },    /* R2309  - HPLP1MIX Input 3 Volume */
+	{ 0x00000906, 0x0000 },    /* R2310  - HPLP1MIX Input 4 Source */
+	{ 0x00000907, 0x0080 },    /* R2311  - HPLP1MIX Input 4 Volume */
+	{ 0x00000908, 0x0000 },    /* R2312  - HPLP2MIX Input 1 Source */
+	{ 0x00000909, 0x0080 },    /* R2313  - HPLP2MIX Input 1 Volume */
+	{ 0x0000090A, 0x0000 },    /* R2314  - HPLP2MIX Input 2 Source */
+	{ 0x0000090B, 0x0080 },    /* R2315  - HPLP2MIX Input 2 Volume */
+	{ 0x0000090C, 0x0000 },    /* R2316  - HPLP2MIX Input 3 Source */
+	{ 0x0000090D, 0x0080 },    /* R2317  - HPLP2MIX Input 3 Volume */
+	{ 0x0000090E, 0x0000 },    /* R2318  - HPLP2MIX Input 4 Source */
+	{ 0x0000090F, 0x0080 },    /* R2319  - HPLP2MIX Input 4 Volume */
+	{ 0x00000910, 0x0000 },    /* R2320  - HPLP3MIX Input 1 Source */
+	{ 0x00000911, 0x0080 },    /* R2321  - HPLP3MIX Input 1 Volume */
+	{ 0x00000912, 0x0000 },    /* R2322  - HPLP3MIX Input 2 Source */
+	{ 0x00000913, 0x0080 },    /* R2323  - HPLP3MIX Input 2 Volume */
+	{ 0x00000914, 0x0000 },    /* R2324  - HPLP3MIX Input 3 Source */
+	{ 0x00000915, 0x0080 },    /* R2325  - HPLP3MIX Input 3 Volume */
+	{ 0x00000916, 0x0000 },    /* R2326  - HPLP3MIX Input 4 Source */
+	{ 0x00000917, 0x0080 },    /* R2327  - HPLP3MIX Input 4 Volume */
+	{ 0x00000918, 0x0000 },    /* R2328  - HPLP4MIX Input 1 Source */
+	{ 0x00000919, 0x0080 },    /* R2329  - HPLP4MIX Input 1 Volume */
+	{ 0x0000091A, 0x0000 },    /* R2330  - HPLP4MIX Input 2 Source */
+	{ 0x0000091B, 0x0080 },    /* R2331  - HPLP4MIX Input 2 Volume */
+	{ 0x0000091C, 0x0000 },    /* R2332  - HPLP4MIX Input 3 Source */
+	{ 0x0000091D, 0x0080 },    /* R2333  - HPLP4MIX Input 3 Volume */
+	{ 0x0000091E, 0x0000 },    /* R2334  - HPLP4MIX Input 4 Source */
+	{ 0x0000091F, 0x0080 },    /* R2335  - HPLP4MIX Input 4 Volume */
+	{ 0x00000980, 0x0000 },    /* R2432  - DSP2LMIX Input 1 Source */
+	{ 0x00000981, 0x0080 },    /* R2433  - DSP2LMIX Input 1 Volume */
+	{ 0x00000982, 0x0000 },    /* R2434  - DSP2LMIX Input 2 Source */
+	{ 0x00000983, 0x0080 },    /* R2435  - DSP2LMIX Input 2 Volume */
+	{ 0x00000984, 0x0000 },    /* R2436  - DSP2LMIX Input 3 Source */
+	{ 0x00000985, 0x0080 },    /* R2437  - DSP2LMIX Input 3 Volume */
+	{ 0x00000986, 0x0000 },    /* R2438  - DSP2LMIX Input 4 Source */
+	{ 0x00000987, 0x0080 },    /* R2439  - DSP2LMIX Input 4 Volume */
+	{ 0x00000988, 0x0000 },    /* R2440  - DSP2RMIX Input 1 Source */
+	{ 0x00000989, 0x0080 },    /* R2441  - DSP2RMIX Input 1 Volume */
+	{ 0x0000098A, 0x0000 },    /* R2442  - DSP2RMIX Input 2 Source */
+	{ 0x0000098B, 0x0080 },    /* R2443  - DSP2RMIX Input 2 Volume */
+	{ 0x0000098C, 0x0000 },    /* R2444  - DSP2RMIX Input 3 Source */
+	{ 0x0000098D, 0x0080 },    /* R2445  - DSP2RMIX Input 3 Volume */
+	{ 0x0000098E, 0x0000 },    /* R2446  - DSP2RMIX Input 4 Source */
+	{ 0x0000098F, 0x0080 },    /* R2447  - DSP2RMIX Input 4 Volume */
+	{ 0x00000990, 0x0000 },    /* R2448  - DSP2AUX1MIX Input 1 Source */
+	{ 0x00000998, 0x0000 },    /* R2456  - DSP2AUX2MIX Input 1 Source */
+	{ 0x000009A0, 0x0000 },    /* R2464  - DSP2AUX3MIX Input 1 Source */
+	{ 0x000009A8, 0x0000 },    /* R2472  - DSP2AUX4MIX Input 1 Source */
+	{ 0x000009B0, 0x0000 },    /* R2480  - DSP2AUX5MIX Input 1 Source */
+	{ 0x000009B8, 0x0000 },    /* R2488  - DSP2AUX6MIX Input 1 Source */
+	{ 0x000009C0, 0x0000 },    /* R2496  - DSP3LMIX Input 1 Source */
+	{ 0x000009C1, 0x0080 },    /* R2497  - DSP3LMIX Input 1 Volume */
+	{ 0x000009C2, 0x0000 },    /* R2498  - DSP3LMIX Input 2 Source */
+	{ 0x000009C3, 0x0080 },    /* R2499  - DSP3LMIX Input 2 Volume */
+	{ 0x000009C4, 0x0000 },    /* R2500  - DSP3LMIX Input 3 Source */
+	{ 0x000009C5, 0x0080 },    /* R2501  - DSP3LMIX Input 3 Volume */
+	{ 0x000009C6, 0x0000 },    /* R2502  - DSP3LMIX Input 4 Source */
+	{ 0x000009C7, 0x0080 },    /* R2503  - DSP3LMIX Input 4 Volume */
+	{ 0x000009C8, 0x0000 },    /* R2504  - DSP3RMIX Input 1 Source */
+	{ 0x000009C9, 0x0080 },    /* R2505  - DSP3RMIX Input 1 Volume */
+	{ 0x000009CA, 0x0000 },    /* R2506  - DSP3RMIX Input 2 Source */
+	{ 0x000009CB, 0x0080 },    /* R2507  - DSP3RMIX Input 2 Volume */
+	{ 0x000009CC, 0x0000 },    /* R2508  - DSP3RMIX Input 3 Source */
+	{ 0x000009CD, 0x0080 },    /* R2509  - DSP3RMIX Input 3 Volume */
+	{ 0x000009CE, 0x0000 },    /* R2510  - DSP3RMIX Input 4 Source */
+	{ 0x000009CF, 0x0080 },    /* R2511  - DSP3RMIX Input 4 Volume */
+	{ 0x000009D0, 0x0000 },    /* R2512  - DSP3AUX1MIX Input 1 Source */
+	{ 0x000009D8, 0x0000 },    /* R2520  - DSP3AUX2MIX Input 1 Source */
+	{ 0x000009E0, 0x0000 },    /* R2528  - DSP3AUX3MIX Input 1 Source */
+	{ 0x000009E8, 0x0000 },    /* R2536  - DSP3AUX4MIX Input 1 Source */
+	{ 0x000009F0, 0x0000 },    /* R2544  - DSP3AUX5MIX Input 1 Source */
+	{ 0x000009F8, 0x0000 },    /* R2552  - DSP3AUX6MIX Input 1 Source */
+	{ 0x00000A80, 0x0000 },    /* R2688  - ASRC1LMIX Input 1 Source */
+	{ 0x00000A88, 0x0000 },    /* R2696  - ASRC1RMIX Input 1 Source */
+	{ 0x00000A90, 0x0000 },    /* R2704  - ASRC2LMIX Input 1 Source */
+	{ 0x00000A98, 0x0000 },    /* R2712  - ASRC2RMIX Input 1 Source */
+	{ 0x00000B00, 0x0000 },    /* R2816  - ISRC1DEC1MIX Input 1 Source */
+	{ 0x00000B08, 0x0000 },    /* R2824  - ISRC1DEC2MIX Input 1 Source */
+	{ 0x00000B10, 0x0000 },    /* R2832  - ISRC1DEC3MIX Input 1 Source */
+	{ 0x00000B18, 0x0000 },    /* R2840  - ISRC1DEC4MIX Input 1 Source */
+	{ 0x00000B20, 0x0000 },    /* R2848  - ISRC1INT1MIX Input 1 Source */
+	{ 0x00000B28, 0x0000 },    /* R2856  - ISRC1INT2MIX Input 1 Source */
+	{ 0x00000B30, 0x0000 },    /* R2864  - ISRC1INT3MIX Input 1 Source */
+	{ 0x00000B38, 0x0000 },    /* R2872  - ISRC1INT4MIX Input 1 Source */
+	{ 0x00000B40, 0x0000 },    /* R2880  - ISRC2DEC1MIX Input 1 Source */
+	{ 0x00000B48, 0x0000 },    /* R2888  - ISRC2DEC2MIX Input 1 Source */
+	{ 0x00000B50, 0x0000 },    /* R2896  - ISRC2DEC3MIX Input 1 Source */
+	{ 0x00000B58, 0x0000 },    /* R2904  - ISRC2DEC4MIX Input 1 Source */
+	{ 0x00000B60, 0x0000 },    /* R2912  - ISRC2INT1MIX Input 1 Source */
+	{ 0x00000B68, 0x0000 },    /* R2920  - ISRC2INT2MIX Input 1 Source */
+	{ 0x00000B70, 0x0000 },    /* R2928  - ISRC2INT3MIX Input 1 Source */
+	{ 0x00000B78, 0x0000 },    /* R2936  - ISRC2INT4MIX Input 1 Source */
+	{ 0x00000B80, 0x0000 },    /* R2944  - ISRC3DEC1MIX Input 1 Source */
+	{ 0x00000B88, 0x0000 },    /* R2952  - ISRC3DEC2MIX Input 1 Source */
+	{ 0x00000B90, 0x0000 },    /* R2960  - ISRC3DEC3MIX Input 1 Source */
+	{ 0x00000B98, 0x0000 },    /* R2968  - ISRC3DEC4MIX Input 1 Source */
+	{ 0x00000BA0, 0x0000 },    /* R2976  - ISRC3INT1MIX Input 1 Source */
+	{ 0x00000BA8, 0x0000 },    /* R2984  - ISRC3INT2MIX Input 1 Source */
+	{ 0x00000BB0, 0x0000 },    /* R2992  - ISRC3INT3MIX Input 1 Source */
+	{ 0x00000BB8, 0x0000 },    /* R3000  - ISRC3INT4MIX Input 1 Source */
+	{ 0x00000C00, 0xA101 },    /* R3072  - GPIO1 CTRL */
+	{ 0x00000C01, 0xA101 },    /* R3073  - GPIO2 CTRL */
+	{ 0x00000C0F, 0x0400 },    /* R3087  - IRQ CTRL 1 */
+	{ 0x00000C10, 0x1000 },    /* R3088  - GPIO Debounce Config */
+	{ 0x00000C20, 0x0002 },    /* R3104  - Misc Pad Ctrl 1 */
+	{ 0x00000C21, 0x8001 },    /* R3105  - Misc Pad Ctrl 2 */
+	{ 0x00000C22, 0x0000 },    /* R3106  - Misc Pad Ctrl 3 */
+	{ 0x00000C23, 0x0000 },    /* R3107  - Misc Pad Ctrl 4 */
+	{ 0x00000C24, 0x0000 },    /* R3108  - Misc Pad Ctrl 5 */
+	{ 0x00000C25, 0x0000 },    /* R3109  - Misc Pad Ctrl 6 */
+	{ 0x00000C30, 0x0404 },    /* R3120  - Misc Pad Ctrl 7 */
+	{ 0x00000C32, 0x0404 },    /* R3122  - Misc Pad Ctrl 9 */
+	{ 0x00000C33, 0x0404 },    /* R3123  - Misc Pad Ctrl 10 */
+	{ 0x00000C34, 0x0404 },    /* R3124  - Misc Pad Ctrl 11 */
+	{ 0x00000C35, 0x0404 },    /* R3125  - Misc Pad Ctrl 12 */
+	{ 0x00000C36, 0x0400 },    /* R3126  - Misc Pad Ctrl 13 */
+	{ 0x00000C37, 0x0404 },    /* R3127  - Misc Pad Ctrl 14 */
+	{ 0x00000C39, 0x0400 },    /* R3129  - Misc Pad Ctrl 16 */
+	{ 0x00000D08, 0x0007 },    /* R3336  - Interrupt Status 1 Mask */
+	{ 0x00000D09, 0x06FF },    /* R3337  - Interrupt Status 2 Mask */
+	{ 0x00000D0A, 0xCFEF },    /* R3338  - Interrupt Status 3 Mask */
+	{ 0x00000D0B, 0xFFC3 },    /* R3339  - Interrupt Status 4 Mask */
+	{ 0x00000D0C, 0x000B },    /* R3340  - Interrupt Status 5 Mask */
+	{ 0x00000D0D, 0xD005 },    /* R3341  - Interrupt Status 6 Mask */
+	{ 0x00000D0F, 0x0000 },    /* R3343  - Interrupt Control */
+	{ 0x00000D18, 0x0007 },    /* R3352  - IRQ2 Status 1 Mask */
+	{ 0x00000D19, 0x06FF },    /* R3353  - IRQ2 Status 2 Mask */
+	{ 0x00000D1A, 0xCFEF },    /* R3354  - IRQ2 Status 3 Mask */
+	{ 0x00000D1B, 0xFFC3 },    /* R3355  - IRQ2 Status 4 Mask */
+	{ 0x00000D1C, 0x000B },    /* R3356  - IRQ2 Status 5 Mask */
+	{ 0x00000D1D, 0xD005 },    /* R3357  - IRQ2 Status 6 Mask */
+	{ 0x00000D1F, 0x0000 },    /* R3359  - IRQ2 Control */
+	{ 0x00000E00, 0x0000 },    /* R3584  - FX_Ctrl1 */
+	{ 0x00000E10, 0x6318 },    /* R3600  - EQ1_1 */
+	{ 0x00000E11, 0x6300 },    /* R3601  - EQ1_2 */
+	{ 0x00000E12, 0x0FC8 },    /* R3602  - EQ1_3 */
+	{ 0x00000E13, 0x03FE },    /* R3603  - EQ1_4 */
+	{ 0x00000E14, 0x00E0 },    /* R3604  - EQ1_5 */
+	{ 0x00000E15, 0x1EC4 },    /* R3605  - EQ1_6 */
+	{ 0x00000E16, 0xF136 },    /* R3606  - EQ1_7 */
+	{ 0x00000E17, 0x0409 },    /* R3607  - EQ1_8 */
+	{ 0x00000E18, 0x04CC },    /* R3608  - EQ1_9 */
+	{ 0x00000E19, 0x1C9B },    /* R3609  - EQ1_10 */
+	{ 0x00000E1A, 0xF337 },    /* R3610  - EQ1_11 */
+	{ 0x00000E1B, 0x040B },    /* R3611  - EQ1_12 */
+	{ 0x00000E1C, 0x0CBB },    /* R3612  - EQ1_13 */
+	{ 0x00000E1D, 0x16F8 },    /* R3613  - EQ1_14 */
+	{ 0x00000E1E, 0xF7D9 },    /* R3614  - EQ1_15 */
+	{ 0x00000E1F, 0x040A },    /* R3615  - EQ1_16 */
+	{ 0x00000E20, 0x1F14 },    /* R3616  - EQ1_17 */
+	{ 0x00000E21, 0x058C },    /* R3617  - EQ1_18 */
+	{ 0x00000E22, 0x0563 },    /* R3618  - EQ1_19 */
+	{ 0x00000E23, 0x4000 },    /* R3619  - EQ1_20 */
+	{ 0x00000E24, 0x0B75 },    /* R3620  - EQ1_21 */
+	{ 0x00000E26, 0x6318 },    /* R3622  - EQ2_1 */
+	{ 0x00000E27, 0x6300 },    /* R3623  - EQ2_2 */
+	{ 0x00000E28, 0x0FC8 },    /* R3624  - EQ2_3 */
+	{ 0x00000E29, 0x03FE },    /* R3625  - EQ2_4 */
+	{ 0x00000E2A, 0x00E0 },    /* R3626  - EQ2_5 */
+	{ 0x00000E2B, 0x1EC4 },    /* R3627  - EQ2_6 */
+	{ 0x00000E2C, 0xF136 },    /* R3628  - EQ2_7 */
+	{ 0x00000E2D, 0x0409 },    /* R3629  - EQ2_8 */
+	{ 0x00000E2E, 0x04CC },    /* R3630  - EQ2_9 */
+	{ 0x00000E2F, 0x1C9B },    /* R3631  - EQ2_10 */
+	{ 0x00000E30, 0xF337 },    /* R3632  - EQ2_11 */
+	{ 0x00000E31, 0x040B },    /* R3633  - EQ2_12 */
+	{ 0x00000E32, 0x0CBB },    /* R3634  - EQ2_13 */
+	{ 0x00000E33, 0x16F8 },    /* R3635  - EQ2_14 */
+	{ 0x00000E34, 0xF7D9 },    /* R3636  - EQ2_15 */
+	{ 0x00000E35, 0x040A },    /* R3637  - EQ2_16 */
+	{ 0x00000E36, 0x1F14 },    /* R3638  - EQ2_17 */
+	{ 0x00000E37, 0x058C },    /* R3639  - EQ2_18 */
+	{ 0x00000E38, 0x0563 },    /* R3640  - EQ2_19 */
+	{ 0x00000E39, 0x4000 },    /* R3641  - EQ2_20 */
+	{ 0x00000E3A, 0x0B75 },    /* R3642  - EQ2_21 */
+	{ 0x00000E80, 0x0018 },    /* R3712  - DRC1 ctrl1 */
+	{ 0x00000E81, 0x0933 },    /* R3713  - DRC1 ctrl2 */
+	{ 0x00000E82, 0x0018 },    /* R3714  - DRC1 ctrl3 */
+	{ 0x00000E83, 0x0000 },    /* R3715  - DRC1 ctrl4 */
+	{ 0x00000E84, 0x0000 },    /* R3716  - DRC1 ctrl5 */
+	{ 0x00000E89, 0x0018 },    /* R3721  - DRC2 ctrl1 */
+	{ 0x00000E8A, 0x0933 },    /* R3722  - DRC2 ctrl2 */
+	{ 0x00000E8B, 0x0018 },    /* R3723  - DRC2 ctrl3 */
+	{ 0x00000E8C, 0x0000 },    /* R3724  - DRC2 ctrl4 */
+	{ 0x00000E8D, 0x0000 },    /* R3725  - DRC2 ctrl5 */
+	{ 0x00000EC0, 0x0000 },    /* R3776  - HPLPF1_1 */
+	{ 0x00000EC1, 0x0000 },    /* R3777  - HPLPF1_2 */
+	{ 0x00000EC4, 0x0000 },    /* R3780  - HPLPF2_1 */
+	{ 0x00000EC5, 0x0000 },    /* R3781  - HPLPF2_2 */
+	{ 0x00000EC8, 0x0000 },    /* R3784  - HPLPF3_1 */
+	{ 0x00000EC9, 0x0000 },    /* R3785  - HPLPF3_2 */
+	{ 0x00000ECC, 0x0000 },    /* R3788  - HPLPF4_1 */
+	{ 0x00000ECD, 0x0000 },    /* R3789  - HPLPF4_2 */
+	{ 0x00000EE0, 0x0000 },    /* R3808  - ASRC_ENABLE */
+	{ 0x00000EE2, 0x0000 },    /* R3810  - ASRC_RATE1 */
+	{ 0x00000EE3, 0x4000 },    /* R3811  - ASRC_RATE2 */
+	{ 0x00000EF0, 0x0000 },    /* R3824  - ISRC 1 CTRL 1 */
+	{ 0x00000EF1, 0x0000 },    /* R3825  - ISRC 1 CTRL 2 */
+	{ 0x00000EF2, 0x0000 },    /* R3826  - ISRC 1 CTRL 3 */
+	{ 0x00000EF3, 0x0000 },    /* R3827  - ISRC 2 CTRL 1 */
+	{ 0x00000EF4, 0x0000 },    /* R3828  - ISRC 2 CTRL 2 */
+	{ 0x00000EF5, 0x0000 },    /* R3829  - ISRC 2 CTRL 3 */
+	{ 0x00000EF6, 0x0000 },    /* R3830  - ISRC 3 CTRL 1 */
+	{ 0x00000EF7, 0x0000 },    /* R3831  - ISRC 3 CTRL 2 */
+	{ 0x00000EF8, 0x0000 },    /* R3832  - ISRC 3 CTRL 3 */
+	{ 0x00001200, 0x0010 },    /* R4608  - DSP2 Control 1 */
+	{ 0x00001300, 0x0010 },    /* R4864  - DSP3 Control 1 */
+};
+
+static bool cs47l24_is_adsp_memory(unsigned int reg)
+{
+	switch (reg) {
+	case 0x200000 ... 0x205fff:	/* DSP2 PM */
+	case 0x280000 ... 0x281fff:	/* DSP2 ZM */
+	case 0x290000 ... 0x2a7fff:	/* DSP2 XM */
+	case 0x2a8000 ... 0x2b3fff:	/* DSP2 YM */
+	case 0x300000 ... 0x308fff:	/* DSP3 PM */
+	case 0x380000 ... 0x381fff:	/* DSP3 ZM */
+	case 0x390000 ... 0x3a7fff:	/* DSP3 XM */
+	case 0x3a8000 ... 0x3b3fff:	/* DSP3 YM */
+		return true;
+	default:
+		return false;
+	}
+}
+
+static bool cs47l24_readable_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_CTRL_IF_SPI_CFG_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_TONE_GENERATOR_1:
+	case ARIZONA_TONE_GENERATOR_2:
+	case ARIZONA_TONE_GENERATOR_3:
+	case ARIZONA_TONE_GENERATOR_4:
+	case ARIZONA_TONE_GENERATOR_5:
+	case ARIZONA_PWM_DRIVE_1:
+	case ARIZONA_PWM_DRIVE_2:
+	case ARIZONA_PWM_DRIVE_3:
+	case ARIZONA_SEQUENCE_CONTROL:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_1:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_2:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_3:
+	case ARIZONA_SAMPLE_RATE_SEQUENCE_SELECT_4:
+	case ARIZONA_COMFORT_NOISE_GENERATOR:
+	case ARIZONA_HAPTICS_CONTROL_1:
+	case ARIZONA_HAPTICS_CONTROL_2:
+	case ARIZONA_HAPTICS_PHASE_1_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_1_DURATION:
+	case ARIZONA_HAPTICS_PHASE_2_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_2_DURATION:
+	case ARIZONA_HAPTICS_PHASE_3_INTENSITY:
+	case ARIZONA_HAPTICS_PHASE_3_DURATION:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_CLOCK_32K_1:
+	case ARIZONA_SYSTEM_CLOCK_1:
+	case ARIZONA_SAMPLE_RATE_1:
+	case ARIZONA_SAMPLE_RATE_2:
+	case ARIZONA_SAMPLE_RATE_3:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_ASYNC_CLOCK_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_OUTPUT_SYSTEM_CLOCK:
+	case ARIZONA_OUTPUT_ASYNC_CLOCK:
+	case ARIZONA_RATE_ESTIMATOR_1:
+	case ARIZONA_RATE_ESTIMATOR_2:
+	case ARIZONA_RATE_ESTIMATOR_3:
+	case ARIZONA_RATE_ESTIMATOR_4:
+	case ARIZONA_RATE_ESTIMATOR_5:
+	case ARIZONA_FLL1_CONTROL_1:
+	case ARIZONA_FLL1_CONTROL_2:
+	case ARIZONA_FLL1_CONTROL_3:
+	case ARIZONA_FLL1_CONTROL_4:
+	case ARIZONA_FLL1_CONTROL_5:
+	case ARIZONA_FLL1_CONTROL_6:
+	case ARIZONA_FLL1_CONTROL_7:
+	case ARIZONA_FLL1_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL1_NCO_TEST_0:
+	case ARIZONA_FLL1_SYNCHRONISER_1:
+	case ARIZONA_FLL1_SYNCHRONISER_2:
+	case ARIZONA_FLL1_SYNCHRONISER_3:
+	case ARIZONA_FLL1_SYNCHRONISER_4:
+	case ARIZONA_FLL1_SYNCHRONISER_5:
+	case ARIZONA_FLL1_SYNCHRONISER_6:
+	case ARIZONA_FLL1_SYNCHRONISER_7:
+	case ARIZONA_FLL1_SPREAD_SPECTRUM:
+	case ARIZONA_FLL1_GPIO_CLOCK:
+	case ARIZONA_FLL2_CONTROL_1:
+	case ARIZONA_FLL2_CONTROL_2:
+	case ARIZONA_FLL2_CONTROL_3:
+	case ARIZONA_FLL2_CONTROL_4:
+	case ARIZONA_FLL2_CONTROL_5:
+	case ARIZONA_FLL2_CONTROL_6:
+	case ARIZONA_FLL2_CONTROL_7:
+	case ARIZONA_FLL2_LOOP_FILTER_TEST_1:
+	case ARIZONA_FLL2_NCO_TEST_0:
+	case ARIZONA_FLL2_SYNCHRONISER_1:
+	case ARIZONA_FLL2_SYNCHRONISER_2:
+	case ARIZONA_FLL2_SYNCHRONISER_3:
+	case ARIZONA_FLL2_SYNCHRONISER_4:
+	case ARIZONA_FLL2_SYNCHRONISER_5:
+	case ARIZONA_FLL2_SYNCHRONISER_6:
+	case ARIZONA_FLL2_SYNCHRONISER_7:
+	case ARIZONA_FLL2_SPREAD_SPECTRUM:
+	case ARIZONA_FLL2_GPIO_CLOCK:
+	case ARIZONA_MIC_BIAS_CTRL_1:
+	case ARIZONA_MIC_BIAS_CTRL_2:
+	case ARIZONA_HP_CTRL_1L:
+	case ARIZONA_HP_CTRL_1R:
+	case ARIZONA_INPUT_ENABLES:
+	case ARIZONA_INPUT_ENABLES_STATUS:
+	case ARIZONA_INPUT_RATE:
+	case ARIZONA_INPUT_VOLUME_RAMP:
+	case ARIZONA_HPF_CONTROL:
+	case ARIZONA_IN1L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DMIC1L_CONTROL:
+	case ARIZONA_IN1R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DMIC1R_CONTROL:
+	case ARIZONA_IN2L_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2L:
+	case ARIZONA_DMIC2L_CONTROL:
+	case ARIZONA_IN2R_CONTROL:
+	case ARIZONA_ADC_DIGITAL_VOLUME_2R:
+	case ARIZONA_DMIC2R_CONTROL:
+	case ARIZONA_OUTPUT_ENABLES_1:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_RAW_OUTPUT_STATUS_1:
+	case ARIZONA_OUTPUT_RATE_1:
+	case ARIZONA_OUTPUT_VOLUME_RAMP:
+	case ARIZONA_OUTPUT_PATH_CONFIG_1L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1L:
+	case ARIZONA_DAC_VOLUME_LIMIT_1L:
+	case ARIZONA_NOISE_GATE_SELECT_1L:
+	case ARIZONA_DAC_DIGITAL_VOLUME_1R:
+	case ARIZONA_DAC_VOLUME_LIMIT_1R:
+	case ARIZONA_NOISE_GATE_SELECT_1R:
+	case ARIZONA_DAC_DIGITAL_VOLUME_4L:
+	case ARIZONA_OUT_VOLUME_4L:
+	case ARIZONA_NOISE_GATE_SELECT_4L:
+	case ARIZONA_DAC_AEC_CONTROL_1:
+	case ARIZONA_NOISE_GATE_CONTROL:
+	case ARIZONA_HP1_SHORT_CIRCUIT_CTRL:
+	case ARIZONA_AIF1_BCLK_CTRL:
+	case ARIZONA_AIF1_TX_PIN_CTRL:
+	case ARIZONA_AIF1_RX_PIN_CTRL:
+	case ARIZONA_AIF1_RATE_CTRL:
+	case ARIZONA_AIF1_FORMAT:
+	case ARIZONA_AIF1_RX_BCLK_RATE:
+	case ARIZONA_AIF1_FRAME_CTRL_1:
+	case ARIZONA_AIF1_FRAME_CTRL_2:
+	case ARIZONA_AIF1_FRAME_CTRL_3:
+	case ARIZONA_AIF1_FRAME_CTRL_4:
+	case ARIZONA_AIF1_FRAME_CTRL_5:
+	case ARIZONA_AIF1_FRAME_CTRL_6:
+	case ARIZONA_AIF1_FRAME_CTRL_7:
+	case ARIZONA_AIF1_FRAME_CTRL_8:
+	case ARIZONA_AIF1_FRAME_CTRL_9:
+	case ARIZONA_AIF1_FRAME_CTRL_10:
+	case ARIZONA_AIF1_FRAME_CTRL_11:
+	case ARIZONA_AIF1_FRAME_CTRL_12:
+	case ARIZONA_AIF1_FRAME_CTRL_13:
+	case ARIZONA_AIF1_FRAME_CTRL_14:
+	case ARIZONA_AIF1_FRAME_CTRL_15:
+	case ARIZONA_AIF1_FRAME_CTRL_16:
+	case ARIZONA_AIF1_FRAME_CTRL_17:
+	case ARIZONA_AIF1_FRAME_CTRL_18:
+	case ARIZONA_AIF1_TX_ENABLES:
+	case ARIZONA_AIF1_RX_ENABLES:
+	case ARIZONA_AIF2_BCLK_CTRL:
+	case ARIZONA_AIF2_TX_PIN_CTRL:
+	case ARIZONA_AIF2_RX_PIN_CTRL:
+	case ARIZONA_AIF2_RATE_CTRL:
+	case ARIZONA_AIF2_FORMAT:
+	case ARIZONA_AIF2_RX_BCLK_RATE:
+	case ARIZONA_AIF2_FRAME_CTRL_1:
+	case ARIZONA_AIF2_FRAME_CTRL_2:
+	case ARIZONA_AIF2_FRAME_CTRL_3:
+	case ARIZONA_AIF2_FRAME_CTRL_4:
+	case ARIZONA_AIF2_FRAME_CTRL_5:
+	case ARIZONA_AIF2_FRAME_CTRL_6:
+	case ARIZONA_AIF2_FRAME_CTRL_7:
+	case ARIZONA_AIF2_FRAME_CTRL_8:
+	case ARIZONA_AIF2_FRAME_CTRL_11:
+	case ARIZONA_AIF2_FRAME_CTRL_12:
+	case ARIZONA_AIF2_FRAME_CTRL_13:
+	case ARIZONA_AIF2_FRAME_CTRL_14:
+	case ARIZONA_AIF2_FRAME_CTRL_15:
+	case ARIZONA_AIF2_FRAME_CTRL_16:
+	case ARIZONA_AIF2_TX_ENABLES:
+	case ARIZONA_AIF2_RX_ENABLES:
+	case ARIZONA_AIF3_BCLK_CTRL:
+	case ARIZONA_AIF3_TX_PIN_CTRL:
+	case ARIZONA_AIF3_RX_PIN_CTRL:
+	case ARIZONA_AIF3_RATE_CTRL:
+	case ARIZONA_AIF3_FORMAT:
+	case ARIZONA_AIF3_RX_BCLK_RATE:
+	case ARIZONA_AIF3_FRAME_CTRL_1:
+	case ARIZONA_AIF3_FRAME_CTRL_2:
+	case ARIZONA_AIF3_FRAME_CTRL_3:
+	case ARIZONA_AIF3_FRAME_CTRL_4:
+	case ARIZONA_AIF3_FRAME_CTRL_11:
+	case ARIZONA_AIF3_FRAME_CTRL_12:
+	case ARIZONA_AIF3_TX_ENABLES:
+	case ARIZONA_AIF3_RX_ENABLES:
+	case ARIZONA_PWM1MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM1MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM1MIX_INPUT_4_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_1_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_1_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_2_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_2_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_3_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_3_VOLUME:
+	case ARIZONA_PWM2MIX_INPUT_4_SOURCE:
+	case ARIZONA_PWM2MIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_1_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_1_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_2_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_2_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_3_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_3_VOLUME:
+	case ARIZONA_OUT4LMIX_INPUT_4_SOURCE:
+	case ARIZONA_OUT4LMIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX7MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF1TX8MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX3MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX3MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX3MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX3MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX3MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX3MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX3MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX4MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX4MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX4MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX4MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX4MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX4MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX4MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX5MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX5MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX5MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX5MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX5MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX5MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX5MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF2TX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF2TX6MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF2TX6MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF2TX6MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF2TX6MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF2TX6MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF2TX6MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF2TX6MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX1MIX_INPUT_4_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_1_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_2_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_3_VOLUME:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_SOURCE:
+	case ARIZONA_AIF3TX2MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ1MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ1MIX_INPUT_4_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_1_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_1_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_2_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_2_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_3_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_3_VOLUME:
+	case ARIZONA_EQ2MIX_INPUT_4_SOURCE:
+	case ARIZONA_EQ2MIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC1RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC1RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DRC2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DRC2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP1MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP1MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP2MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP2MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP3MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP3MIX_INPUT_4_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_1_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_1_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_2_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_2_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_3_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_3_VOLUME:
+	case ARIZONA_HPLP4MIX_INPUT_4_SOURCE:
+	case ARIZONA_HPLP4MIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP2LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP2LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP2RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP2RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP2AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP2AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP3LMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP3LMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_1_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_2_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_2_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_3_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_3_VOLUME:
+	case ARIZONA_DSP3RMIX_INPUT_4_SOURCE:
+	case ARIZONA_DSP3RMIX_INPUT_4_VOLUME:
+	case ARIZONA_DSP3AUX1MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX2MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX3MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX4MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX5MIX_INPUT_1_SOURCE:
+	case ARIZONA_DSP3AUX6MIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC1RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2LMIX_INPUT_1_SOURCE:
+	case ARIZONA_ASRC2RMIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC1INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC2INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3DEC4MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT1MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT2MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT3MIX_INPUT_1_SOURCE:
+	case ARIZONA_ISRC3INT4MIX_INPUT_1_SOURCE:
+	case ARIZONA_GPIO1_CTRL:
+	case ARIZONA_GPIO2_CTRL:
+	case ARIZONA_IRQ_CTRL_1:
+	case ARIZONA_GPIO_DEBOUNCE_CONFIG:
+	case ARIZONA_MISC_PAD_CTRL_1:
+	case ARIZONA_MISC_PAD_CTRL_2:
+	case ARIZONA_MISC_PAD_CTRL_3:
+	case ARIZONA_MISC_PAD_CTRL_4:
+	case ARIZONA_MISC_PAD_CTRL_5:
+	case ARIZONA_MISC_PAD_CTRL_6:
+	case ARIZONA_MISC_PAD_CTRL_7:
+	case ARIZONA_MISC_PAD_CTRL_9:
+	case ARIZONA_MISC_PAD_CTRL_10:
+	case ARIZONA_MISC_PAD_CTRL_11:
+	case ARIZONA_MISC_PAD_CTRL_12:
+	case ARIZONA_MISC_PAD_CTRL_13:
+	case ARIZONA_MISC_PAD_CTRL_14:
+	case ARIZONA_MISC_PAD_CTRL_16:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_INTERRUPT_STATUS_6:
+	case ARIZONA_INTERRUPT_STATUS_1_MASK:
+	case ARIZONA_INTERRUPT_STATUS_2_MASK:
+	case ARIZONA_INTERRUPT_STATUS_3_MASK:
+	case ARIZONA_INTERRUPT_STATUS_4_MASK:
+	case ARIZONA_INTERRUPT_STATUS_5_MASK:
+	case ARIZONA_INTERRUPT_STATUS_6_MASK:
+	case ARIZONA_INTERRUPT_CONTROL:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_6:
+	case ARIZONA_IRQ2_STATUS_1_MASK:
+	case ARIZONA_IRQ2_STATUS_2_MASK:
+	case ARIZONA_IRQ2_STATUS_3_MASK:
+	case ARIZONA_IRQ2_STATUS_4_MASK:
+	case ARIZONA_IRQ2_STATUS_5_MASK:
+	case ARIZONA_IRQ2_STATUS_6_MASK:
+	case ARIZONA_IRQ2_CONTROL:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_INTERRUPT_RAW_STATUS_9:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_FX_CTRL1:
+	case ARIZONA_FX_CTRL2:
+	case ARIZONA_EQ1_1:
+	case ARIZONA_EQ1_2:
+	case ARIZONA_EQ1_3:
+	case ARIZONA_EQ1_4:
+	case ARIZONA_EQ1_5:
+	case ARIZONA_EQ1_6:
+	case ARIZONA_EQ1_7:
+	case ARIZONA_EQ1_8:
+	case ARIZONA_EQ1_9:
+	case ARIZONA_EQ1_10:
+	case ARIZONA_EQ1_11:
+	case ARIZONA_EQ1_12:
+	case ARIZONA_EQ1_13:
+	case ARIZONA_EQ1_14:
+	case ARIZONA_EQ1_15:
+	case ARIZONA_EQ1_16:
+	case ARIZONA_EQ1_17:
+	case ARIZONA_EQ1_18:
+	case ARIZONA_EQ1_19:
+	case ARIZONA_EQ1_20:
+	case ARIZONA_EQ1_21:
+	case ARIZONA_EQ2_1:
+	case ARIZONA_EQ2_2:
+	case ARIZONA_EQ2_3:
+	case ARIZONA_EQ2_4:
+	case ARIZONA_EQ2_5:
+	case ARIZONA_EQ2_6:
+	case ARIZONA_EQ2_7:
+	case ARIZONA_EQ2_8:
+	case ARIZONA_EQ2_9:
+	case ARIZONA_EQ2_10:
+	case ARIZONA_EQ2_11:
+	case ARIZONA_EQ2_12:
+	case ARIZONA_EQ2_13:
+	case ARIZONA_EQ2_14:
+	case ARIZONA_EQ2_15:
+	case ARIZONA_EQ2_16:
+	case ARIZONA_EQ2_17:
+	case ARIZONA_EQ2_18:
+	case ARIZONA_EQ2_19:
+	case ARIZONA_EQ2_20:
+	case ARIZONA_EQ2_21:
+	case ARIZONA_DRC1_CTRL1:
+	case ARIZONA_DRC1_CTRL2:
+	case ARIZONA_DRC1_CTRL3:
+	case ARIZONA_DRC1_CTRL4:
+	case ARIZONA_DRC1_CTRL5:
+	case ARIZONA_DRC2_CTRL1:
+	case ARIZONA_DRC2_CTRL2:
+	case ARIZONA_DRC2_CTRL3:
+	case ARIZONA_DRC2_CTRL4:
+	case ARIZONA_DRC2_CTRL5:
+	case ARIZONA_HPLPF1_1:
+	case ARIZONA_HPLPF1_2:
+	case ARIZONA_HPLPF2_1:
+	case ARIZONA_HPLPF2_2:
+	case ARIZONA_HPLPF3_1:
+	case ARIZONA_HPLPF3_2:
+	case ARIZONA_HPLPF4_1:
+	case ARIZONA_HPLPF4_2:
+	case ARIZONA_ASRC_ENABLE:
+	case ARIZONA_ASRC_STATUS:
+	case ARIZONA_ASRC_RATE1:
+	case ARIZONA_ASRC_RATE2:
+	case ARIZONA_ISRC_1_CTRL_1:
+	case ARIZONA_ISRC_1_CTRL_2:
+	case ARIZONA_ISRC_1_CTRL_3:
+	case ARIZONA_ISRC_2_CTRL_1:
+	case ARIZONA_ISRC_2_CTRL_2:
+	case ARIZONA_ISRC_2_CTRL_3:
+	case ARIZONA_ISRC_3_CTRL_1:
+	case ARIZONA_ISRC_3_CTRL_2:
+	case ARIZONA_ISRC_3_CTRL_3:
+	case ARIZONA_DSP2_CONTROL_1:
+	case ARIZONA_DSP2_CLOCKING_1:
+	case ARIZONA_DSP2_STATUS_1:
+	case ARIZONA_DSP2_STATUS_2:
+	case ARIZONA_DSP2_STATUS_3:
+	case ARIZONA_DSP2_STATUS_4:
+	case ARIZONA_DSP2_WDMA_BUFFER_1:
+	case ARIZONA_DSP2_WDMA_BUFFER_2:
+	case ARIZONA_DSP2_WDMA_BUFFER_3:
+	case ARIZONA_DSP2_WDMA_BUFFER_4:
+	case ARIZONA_DSP2_WDMA_BUFFER_5:
+	case ARIZONA_DSP2_WDMA_BUFFER_6:
+	case ARIZONA_DSP2_WDMA_BUFFER_7:
+	case ARIZONA_DSP2_WDMA_BUFFER_8:
+	case ARIZONA_DSP2_RDMA_BUFFER_1:
+	case ARIZONA_DSP2_RDMA_BUFFER_2:
+	case ARIZONA_DSP2_RDMA_BUFFER_3:
+	case ARIZONA_DSP2_RDMA_BUFFER_4:
+	case ARIZONA_DSP2_RDMA_BUFFER_5:
+	case ARIZONA_DSP2_RDMA_BUFFER_6:
+	case ARIZONA_DSP2_WDMA_CONFIG_1:
+	case ARIZONA_DSP2_WDMA_CONFIG_2:
+	case ARIZONA_DSP2_WDMA_OFFSET_1:
+	case ARIZONA_DSP2_RDMA_CONFIG_1:
+	case ARIZONA_DSP2_RDMA_OFFSET_1:
+	case ARIZONA_DSP2_EXTERNAL_START_SELECT_1:
+	case ARIZONA_DSP2_SCRATCH_0:
+	case ARIZONA_DSP2_SCRATCH_1:
+	case ARIZONA_DSP2_SCRATCH_2:
+	case ARIZONA_DSP2_SCRATCH_3:
+	case ARIZONA_DSP3_CONTROL_1:
+	case ARIZONA_DSP3_CLOCKING_1:
+	case ARIZONA_DSP3_STATUS_1:
+	case ARIZONA_DSP3_STATUS_2:
+	case ARIZONA_DSP3_STATUS_3:
+	case ARIZONA_DSP3_STATUS_4:
+	case ARIZONA_DSP3_WDMA_BUFFER_1:
+	case ARIZONA_DSP3_WDMA_BUFFER_2:
+	case ARIZONA_DSP3_WDMA_BUFFER_3:
+	case ARIZONA_DSP3_WDMA_BUFFER_4:
+	case ARIZONA_DSP3_WDMA_BUFFER_5:
+	case ARIZONA_DSP3_WDMA_BUFFER_6:
+	case ARIZONA_DSP3_WDMA_BUFFER_7:
+	case ARIZONA_DSP3_WDMA_BUFFER_8:
+	case ARIZONA_DSP3_RDMA_BUFFER_1:
+	case ARIZONA_DSP3_RDMA_BUFFER_2:
+	case ARIZONA_DSP3_RDMA_BUFFER_3:
+	case ARIZONA_DSP3_RDMA_BUFFER_4:
+	case ARIZONA_DSP3_RDMA_BUFFER_5:
+	case ARIZONA_DSP3_RDMA_BUFFER_6:
+	case ARIZONA_DSP3_WDMA_CONFIG_1:
+	case ARIZONA_DSP3_WDMA_CONFIG_2:
+	case ARIZONA_DSP3_WDMA_OFFSET_1:
+	case ARIZONA_DSP3_RDMA_CONFIG_1:
+	case ARIZONA_DSP3_RDMA_OFFSET_1:
+	case ARIZONA_DSP3_EXTERNAL_START_SELECT_1:
+	case ARIZONA_DSP3_SCRATCH_0:
+	case ARIZONA_DSP3_SCRATCH_1:
+	case ARIZONA_DSP3_SCRATCH_2:
+	case ARIZONA_DSP3_SCRATCH_3:
+		return true;
+	default:
+		return cs47l24_is_adsp_memory(reg);
+	}
+}
+
+static bool cs47l24_volatile_register(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case ARIZONA_SOFTWARE_RESET:
+	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_HAPTICS_STATUS:
+	case ARIZONA_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_SAMPLE_RATE_3_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
+	case ARIZONA_HP_CTRL_1L:
+	case ARIZONA_HP_CTRL_1R:
+	case ARIZONA_INPUT_ENABLES_STATUS:
+	case ARIZONA_OUTPUT_STATUS_1:
+	case ARIZONA_RAW_OUTPUT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_1:
+	case ARIZONA_INTERRUPT_STATUS_2:
+	case ARIZONA_INTERRUPT_STATUS_3:
+	case ARIZONA_INTERRUPT_STATUS_4:
+	case ARIZONA_INTERRUPT_STATUS_5:
+	case ARIZONA_INTERRUPT_STATUS_6:
+	case ARIZONA_IRQ2_STATUS_1:
+	case ARIZONA_IRQ2_STATUS_2:
+	case ARIZONA_IRQ2_STATUS_3:
+	case ARIZONA_IRQ2_STATUS_4:
+	case ARIZONA_IRQ2_STATUS_5:
+	case ARIZONA_IRQ2_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_2:
+	case ARIZONA_INTERRUPT_RAW_STATUS_3:
+	case ARIZONA_INTERRUPT_RAW_STATUS_4:
+	case ARIZONA_INTERRUPT_RAW_STATUS_5:
+	case ARIZONA_INTERRUPT_RAW_STATUS_6:
+	case ARIZONA_INTERRUPT_RAW_STATUS_7:
+	case ARIZONA_INTERRUPT_RAW_STATUS_8:
+	case ARIZONA_INTERRUPT_RAW_STATUS_9:
+	case ARIZONA_IRQ_PIN_STATUS:
+	case ARIZONA_FX_CTRL2:
+	case ARIZONA_ASRC_STATUS:
+	case ARIZONA_DSP2_STATUS_1:
+	case ARIZONA_DSP2_STATUS_2:
+	case ARIZONA_DSP2_STATUS_3:
+	case ARIZONA_DSP2_STATUS_4:
+	case ARIZONA_DSP2_WDMA_BUFFER_1:
+	case ARIZONA_DSP2_WDMA_BUFFER_2:
+	case ARIZONA_DSP2_WDMA_BUFFER_3:
+	case ARIZONA_DSP2_WDMA_BUFFER_4:
+	case ARIZONA_DSP2_WDMA_BUFFER_5:
+	case ARIZONA_DSP2_WDMA_BUFFER_6:
+	case ARIZONA_DSP2_WDMA_BUFFER_7:
+	case ARIZONA_DSP2_WDMA_BUFFER_8:
+	case ARIZONA_DSP2_RDMA_BUFFER_1:
+	case ARIZONA_DSP2_RDMA_BUFFER_2:
+	case ARIZONA_DSP2_RDMA_BUFFER_3:
+	case ARIZONA_DSP2_RDMA_BUFFER_4:
+	case ARIZONA_DSP2_RDMA_BUFFER_5:
+	case ARIZONA_DSP2_RDMA_BUFFER_6:
+	case ARIZONA_DSP2_WDMA_CONFIG_1:
+	case ARIZONA_DSP2_WDMA_CONFIG_2:
+	case ARIZONA_DSP2_WDMA_OFFSET_1:
+	case ARIZONA_DSP2_RDMA_CONFIG_1:
+	case ARIZONA_DSP2_RDMA_OFFSET_1:
+	case ARIZONA_DSP2_EXTERNAL_START_SELECT_1:
+	case ARIZONA_DSP2_SCRATCH_0:
+	case ARIZONA_DSP2_SCRATCH_1:
+	case ARIZONA_DSP2_SCRATCH_2:
+	case ARIZONA_DSP2_SCRATCH_3:
+	case ARIZONA_DSP2_CLOCKING_1:
+	case ARIZONA_DSP3_STATUS_1:
+	case ARIZONA_DSP3_STATUS_2:
+	case ARIZONA_DSP3_STATUS_3:
+	case ARIZONA_DSP3_STATUS_4:
+	case ARIZONA_DSP3_WDMA_BUFFER_1:
+	case ARIZONA_DSP3_WDMA_BUFFER_2:
+	case ARIZONA_DSP3_WDMA_BUFFER_3:
+	case ARIZONA_DSP3_WDMA_BUFFER_4:
+	case ARIZONA_DSP3_WDMA_BUFFER_5:
+	case ARIZONA_DSP3_WDMA_BUFFER_6:
+	case ARIZONA_DSP3_WDMA_BUFFER_7:
+	case ARIZONA_DSP3_WDMA_BUFFER_8:
+	case ARIZONA_DSP3_RDMA_BUFFER_1:
+	case ARIZONA_DSP3_RDMA_BUFFER_2:
+	case ARIZONA_DSP3_RDMA_BUFFER_3:
+	case ARIZONA_DSP3_RDMA_BUFFER_4:
+	case ARIZONA_DSP3_RDMA_BUFFER_5:
+	case ARIZONA_DSP3_RDMA_BUFFER_6:
+	case ARIZONA_DSP3_WDMA_CONFIG_1:
+	case ARIZONA_DSP3_WDMA_CONFIG_2:
+	case ARIZONA_DSP3_WDMA_OFFSET_1:
+	case ARIZONA_DSP3_RDMA_CONFIG_1:
+	case ARIZONA_DSP3_RDMA_OFFSET_1:
+	case ARIZONA_DSP3_EXTERNAL_START_SELECT_1:
+	case ARIZONA_DSP3_SCRATCH_0:
+	case ARIZONA_DSP3_SCRATCH_1:
+	case ARIZONA_DSP3_SCRATCH_2:
+	case ARIZONA_DSP3_SCRATCH_3:
+	case ARIZONA_DSP3_CLOCKING_1:
+		return true;
+	default:
+		return cs47l24_is_adsp_memory(reg);
+	}
+}
+
+#define CS47L24_MAX_REGISTER 0x3b3fff
+
+const struct regmap_config cs47l24_spi_regmap = {
+	.reg_bits = 32,
+	.pad_bits = 16,
+	.val_bits = 16,
+	.reg_format_endian = REGMAP_ENDIAN_BIG,
+	.val_format_endian = REGMAP_ENDIAN_BIG,
+
+	.max_register = CS47L24_MAX_REGISTER,
+	.readable_reg = cs47l24_readable_register,
+	.volatile_reg = cs47l24_volatile_register,
+
+	.cache_type = REGCACHE_RBTREE,
+	.reg_defaults = cs47l24_reg_default,
+	.num_reg_defaults = ARRAY_SIZE(cs47l24_reg_default),
+};
+EXPORT_SYMBOL_GPL(cs47l24_spi_regmap);
+
diff --git a/drivers/mfd/cs5535-mfd.c b/drivers/mfd/cs5535-mfd.c
index be91cb5d6e78..f9d277ff4aaf 100644
--- a/drivers/mfd/cs5535-mfd.c
+++ b/drivers/mfd/cs5535-mfd.c
@@ -60,6 +60,7 @@ static int cs5535_mfd_res_enable(struct platform_device *pdev)
 static int cs5535_mfd_res_disable(struct platform_device *pdev)
 {
 	struct resource *res;
+
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!res) {
 		dev_err(&pdev->dev, "can't fetch device resource info\n");
@@ -114,7 +115,10 @@ static struct mfd_cell cs5535_mfd_cells[] = {
 #ifdef CONFIG_OLPC
 static void cs5535_clone_olpc_cells(void)
 {
-	const char *acpi_clones[] = { "olpc-xo1-pm-acpi", "olpc-xo1-sci-acpi" };
+	static const char *acpi_clones[] = {
+		"olpc-xo1-pm-acpi",
+		"olpc-xo1-sci-acpi"
+	};
 
 	if (!machine_is_olpc())
 		return;
diff --git a/drivers/mfd/da903x.c b/drivers/mfd/da903x.c
index 37e4426ef061..09f367571c58 100644
--- a/drivers/mfd/da903x.c
+++ b/drivers/mfd/da903x.c
@@ -2,10 +2,10 @@
  * Base driver for Dialog Semiconductor DA9030/DA9034
  *
  * Copyright (C) 2008 Compulab, Ltd.
- * 	Mike Rapoport <mike@compulab.co.il>
+ *	Mike Rapoport <mike@compulab.co.il>
  *
  * Copyright (C) 2006-2008 Marvell International Ltd.
- * 	Eric Miao <eric.miao@marvell.com>
+ *	Eric Miao <eric.miao@marvell.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 +60,7 @@ struct da903x_chip_ops {
 struct da903x_chip {
 	struct i2c_client	*client;
 	struct device		*dev;
-	struct da903x_chip_ops	*ops;
+	const struct da903x_chip_ops *ops;
 
 	int			type;
 	uint32_t		events_mask;
@@ -424,7 +424,7 @@ static irqreturn_t da903x_irq_handler(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static struct da903x_chip_ops da903x_ops[] = {
+static const struct da903x_chip_ops da903x_ops[] = {
 	[0] = {
 		.init_chip	= da9030_init_chip,
 		.unmask_events	= da9030_unmask_events,
@@ -565,6 +565,6 @@ static void __exit da903x_exit(void)
 module_exit(da903x_exit);
 
 MODULE_DESCRIPTION("PMIC Driver for Dialog Semiconductor DA9034");
-MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>"
-	      "Mike Rapoport <mike@compulab.co.il>");
-MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Eric Miao <eric.miao@marvell.com>");
+MODULE_AUTHOR("Mike Rapoport <mike@compulab.co.il>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/da9052-i2c.c b/drivers/mfd/da9052-i2c.c
index 2697ffb08009..578e881067a5 100644
--- a/drivers/mfd/da9052-i2c.c
+++ b/drivers/mfd/da9052-i2c.c
@@ -70,7 +70,7 @@ static int da9052_i2c_fix(struct da9052 *da9052, unsigned char reg)
 	case DA9053_BA:
 	case DA9053_BB:
 		/* A dummy read to a safe register address. */
-	if (!i2c_safe_reg(reg))
+		if (!i2c_safe_reg(reg))
 			return regmap_read(da9052->regmap,
 					   DA9052_PARK_REGISTER,
 					   &val);
diff --git a/drivers/mfd/da9052-irq.c b/drivers/mfd/da9052-irq.c
index f4cb4613140b..cd4ca849ca44 100644
--- a/drivers/mfd/da9052-irq.c
+++ b/drivers/mfd/da9052-irq.c
@@ -283,7 +283,7 @@ regmap_err:
 
 int da9052_irq_exit(struct da9052 *da9052)
 {
-	da9052_free_irq(da9052, DA9052_IRQ_ADC_EOM , da9052);
+	da9052_free_irq(da9052, DA9052_IRQ_ADC_EOM, da9052);
 	regmap_del_irq_chip(da9052->chip_irq, da9052->irq_data);
 
 	return 0;
diff --git a/drivers/mfd/davinci_voicecodec.c b/drivers/mfd/davinci_voicecodec.c
index 9bbc642a7b9d..dff2f19296b8 100644
--- a/drivers/mfd/davinci_voicecodec.c
+++ b/drivers/mfd/davinci_voicecodec.c
@@ -47,11 +47,8 @@ static int __init davinci_vc_probe(struct platform_device *pdev)
 
 	davinci_vc = devm_kzalloc(&pdev->dev,
 				  sizeof(struct davinci_vc), GFP_KERNEL);
-	if (!davinci_vc) {
-		dev_dbg(&pdev->dev,
-			    "could not allocate memory for private data\n");
+	if (!davinci_vc)
 		return -ENOMEM;
-	}
 
 	davinci_vc->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(davinci_vc->clk)) {
diff --git a/drivers/mfd/db8500-prcmu.c b/drivers/mfd/db8500-prcmu.c
index e6e4bacb09ee..12099b09a9a7 100644
--- a/drivers/mfd/db8500-prcmu.c
+++ b/drivers/mfd/db8500-prcmu.c
@@ -2048,6 +2048,7 @@ int db8500_prcmu_config_hotmon(u8 low, u8 high)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(db8500_prcmu_config_hotmon);
 
 static int config_hot_period(u16 val)
 {
@@ -2074,11 +2075,13 @@ int db8500_prcmu_start_temp_sense(u16 cycles32k)
 
 	return config_hot_period(cycles32k);
 }
+EXPORT_SYMBOL_GPL(db8500_prcmu_start_temp_sense);
 
 int db8500_prcmu_stop_temp_sense(void)
 {
 	return config_hot_period(0xFFFF);
 }
+EXPORT_SYMBOL_GPL(db8500_prcmu_stop_temp_sense);
 
 static int prcmu_a9wdog(u8 cmd, u8 d0, u8 d1, u8 d2, u8 d3)
 {
diff --git a/drivers/mfd/dm355evm_msp.c b/drivers/mfd/dm355evm_msp.c
index 4c826f78acd0..ec4438ed2faf 100644
--- a/drivers/mfd/dm355evm_msp.c
+++ b/drivers/mfd/dm355evm_msp.c
@@ -147,7 +147,7 @@ static int msp_gpio_get(struct gpio_chip *chip, unsigned offset)
 		return status;
 	if (reg == DM355EVM_MSP_LED)
 		msp_led_cache = status;
-	return status & MSP_GPIO_MASK(offset);
+	return !!(status & MSP_GPIO_MASK(offset));
 }
 
 static int msp_gpio_out(struct gpio_chip *chip, unsigned offset, int value)
@@ -259,7 +259,7 @@ static int add_children(struct i2c_client *client)
 	int		i;
 
 	/* GPIO-ish stuff */
-	dm355evm_msp_gpio.dev = &client->dev;
+	dm355evm_msp_gpio.parent = &client->dev;
 	status = gpiochip_add(&dm355evm_msp_gpio);
 	if (status < 0)
 		return status;
diff --git a/drivers/mfd/htc-egpio.c b/drivers/mfd/htc-egpio.c
index 6ccaf90d98fd..c636b5f83cfb 100644
--- a/drivers/mfd/htc-egpio.c
+++ b/drivers/mfd/htc-egpio.c
@@ -163,7 +163,7 @@ static int egpio_get(struct gpio_chip *chip, unsigned offset)
 	value = egpio_readw(ei, reg);
 	pr_debug("readw(%p + %x) = %x\n",
 			ei->base_addr, reg << ei->bus_shift, value);
-	return value & bit;
+	return !!(value & bit);
 }
 
 static int egpio_direction_input(struct gpio_chip *chip, unsigned offset)
@@ -321,7 +321,7 @@ static int __init egpio_probe(struct platform_device *pdev)
 		ei->chip[i].dev = &(pdev->dev);
 		chip = &(ei->chip[i].chip);
 		chip->label           = "htc-egpio";
-		chip->dev             = &pdev->dev;
+		chip->parent          = &pdev->dev;
 		chip->owner           = THIS_MODULE;
 		chip->get             = egpio_get;
 		chip->set             = egpio_set;
diff --git a/drivers/mfd/htc-i2cpld.c b/drivers/mfd/htc-i2cpld.c
index 0c6ff727b2ec..bd6b96d07ab8 100644
--- a/drivers/mfd/htc-i2cpld.c
+++ b/drivers/mfd/htc-i2cpld.c
@@ -429,7 +429,7 @@ static int htcpld_register_chip_gpio(
 	/* Setup the GPIO chips */
 	gpio_chip = &(chip->chip_out);
 	gpio_chip->label           = "htcpld-out";
-	gpio_chip->dev             = dev;
+	gpio_chip->parent             = dev;
 	gpio_chip->owner           = THIS_MODULE;
 	gpio_chip->get             = htcpld_chip_get;
 	gpio_chip->set             = htcpld_chip_set;
@@ -440,7 +440,7 @@ static int htcpld_register_chip_gpio(
 
 	gpio_chip = &(chip->chip_in);
 	gpio_chip->label           = "htcpld-in";
-	gpio_chip->dev             = dev;
+	gpio_chip->parent             = dev;
 	gpio_chip->owner           = THIS_MODULE;
 	gpio_chip->get             = htcpld_chip_get;
 	gpio_chip->set             = NULL;
diff --git a/drivers/mfd/intel-lpss-acpi.c b/drivers/mfd/intel-lpss-acpi.c
index b6fd9041f82f..06f00d60be46 100644
--- a/drivers/mfd/intel-lpss-acpi.c
+++ b/drivers/mfd/intel-lpss-acpi.c
@@ -18,6 +18,7 @@
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_device.h>
+#include <linux/property.h>
 
 #include "intel-lpss.h"
 
@@ -25,6 +26,20 @@ static const struct intel_lpss_platform_info spt_info = {
 	.clk_rate = 120000000,
 };
 
+static struct property_entry spt_i2c_properties[] = {
+	PROPERTY_ENTRY_U32("i2c-sda-hold-time-ns", 230),
+	{ },
+};
+
+static struct property_set spt_i2c_pset = {
+	.properties = spt_i2c_properties,
+};
+
+static const struct intel_lpss_platform_info spt_i2c_info = {
+	.clk_rate = 120000000,
+	.pset = &spt_i2c_pset,
+};
+
 static const struct intel_lpss_platform_info bxt_info = {
 	.clk_rate = 100000000,
 };
@@ -35,8 +50,8 @@ static const struct intel_lpss_platform_info bxt_i2c_info = {
 
 static const struct acpi_device_id intel_lpss_acpi_ids[] = {
 	/* SPT */
-	{ "INT3446", (kernel_ulong_t)&spt_info },
-	{ "INT3447", (kernel_ulong_t)&spt_info },
+	{ "INT3446", (kernel_ulong_t)&spt_i2c_info },
+	{ "INT3447", (kernel_ulong_t)&spt_i2c_info },
 	/* BXT */
 	{ "80860AAC", (kernel_ulong_t)&bxt_i2c_info },
 	{ "80860ABC", (kernel_ulong_t)&bxt_info },
diff --git a/drivers/mfd/intel-lpss-pci.c b/drivers/mfd/intel-lpss-pci.c
index 5bfdfccbb9a1..a7136c7ae9fb 100644
--- a/drivers/mfd/intel-lpss-pci.c
+++ b/drivers/mfd/intel-lpss-pci.c
@@ -17,6 +17,7 @@
 #include <linux/pci.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
+#include <linux/property.h>
 
 #include "intel-lpss.h"
 
@@ -65,9 +66,35 @@ static const struct intel_lpss_platform_info spt_info = {
 	.clk_rate = 120000000,
 };
 
+static struct property_entry spt_i2c_properties[] = {
+	PROPERTY_ENTRY_U32("i2c-sda-hold-time-ns", 230),
+	{ },
+};
+
+static struct property_set spt_i2c_pset = {
+	.properties = spt_i2c_properties,
+};
+
+static const struct intel_lpss_platform_info spt_i2c_info = {
+	.clk_rate = 120000000,
+	.pset = &spt_i2c_pset,
+};
+
+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 struct property_set uart_pset = {
+	.properties = uart_properties,
+};
+
 static const struct intel_lpss_platform_info spt_uart_info = {
 	.clk_rate = 120000000,
 	.clk_con_id = "baudclk",
+	.pset = &uart_pset,
 };
 
 static const struct intel_lpss_platform_info bxt_info = {
@@ -77,6 +104,7 @@ static const struct intel_lpss_platform_info bxt_info = {
 static const struct intel_lpss_platform_info bxt_uart_info = {
 	.clk_rate = 100000000,
 	.clk_con_id = "baudclk",
+	.pset = &uart_pset,
 };
 
 static const struct intel_lpss_platform_info bxt_i2c_info = {
@@ -121,20 +149,20 @@ static const struct pci_device_id intel_lpss_pci_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x9d28), (kernel_ulong_t)&spt_uart_info },
 	{ PCI_VDEVICE(INTEL, 0x9d29), (kernel_ulong_t)&spt_info },
 	{ PCI_VDEVICE(INTEL, 0x9d2a), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d60), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d61), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d62), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d63), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d64), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0x9d65), (kernel_ulong_t)&spt_info },
+	{ PCI_VDEVICE(INTEL, 0x9d60), (kernel_ulong_t)&spt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x9d61), (kernel_ulong_t)&spt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x9d62), (kernel_ulong_t)&spt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x9d63), (kernel_ulong_t)&spt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x9d64), (kernel_ulong_t)&spt_i2c_info },
+	{ PCI_VDEVICE(INTEL, 0x9d65), (kernel_ulong_t)&spt_i2c_info },
 	{ PCI_VDEVICE(INTEL, 0x9d66), (kernel_ulong_t)&spt_uart_info },
 	/* SPT-H */
 	{ PCI_VDEVICE(INTEL, 0xa127), (kernel_ulong_t)&spt_uart_info },
 	{ PCI_VDEVICE(INTEL, 0xa128), (kernel_ulong_t)&spt_uart_info },
 	{ PCI_VDEVICE(INTEL, 0xa129), (kernel_ulong_t)&spt_info },
 	{ PCI_VDEVICE(INTEL, 0xa12a), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0xa160), (kernel_ulong_t)&spt_info },
-	{ PCI_VDEVICE(INTEL, 0xa161), (kernel_ulong_t)&spt_info },
+	{ 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 },
 	{ }
 };
diff --git a/drivers/mfd/intel-lpss.c b/drivers/mfd/intel-lpss.c
index 6255513f54c7..1743788f1595 100644
--- a/drivers/mfd/intel-lpss.c
+++ b/drivers/mfd/intel-lpss.c
@@ -24,6 +24,7 @@
 #include <linux/mfd/core.h>
 #include <linux/pm_qos.h>
 #include <linux/pm_runtime.h>
+#include <linux/property.h>
 #include <linux/seq_file.h>
 #include <linux/io-64-nonatomic-lo-hi.h>
 
@@ -72,7 +73,7 @@ struct intel_lpss {
 	enum intel_lpss_dev_type type;
 	struct clk *clk;
 	struct clk_lookup *clock;
-	const struct mfd_cell *cell;
+	struct mfd_cell *cell;
 	struct device *dev;
 	void __iomem *priv;
 	int devid;
@@ -217,6 +218,7 @@ static void intel_lpss_ltr_hide(struct intel_lpss *lpss)
 
 static int intel_lpss_assign_devs(struct intel_lpss *lpss)
 {
+	const struct mfd_cell *cell;
 	unsigned int type;
 
 	type = lpss->caps & LPSS_PRIV_CAPS_TYPE_MASK;
@@ -224,18 +226,22 @@ static int intel_lpss_assign_devs(struct intel_lpss *lpss)
 
 	switch (type) {
 	case LPSS_DEV_I2C:
-		lpss->cell = &intel_lpss_i2c_cell;
+		cell = &intel_lpss_i2c_cell;
 		break;
 	case LPSS_DEV_UART:
-		lpss->cell = &intel_lpss_uart_cell;
+		cell = &intel_lpss_uart_cell;
 		break;
 	case LPSS_DEV_SPI:
-		lpss->cell = &intel_lpss_spi_cell;
+		cell = &intel_lpss_spi_cell;
 		break;
 	default:
 		return -ENODEV;
 	}
 
+	lpss->cell = devm_kmemdup(lpss->dev, cell, sizeof(*cell), GFP_KERNEL);
+	if (!lpss->cell)
+		return -ENOMEM;
+
 	lpss->type = type;
 
 	return 0;
@@ -401,6 +407,8 @@ int intel_lpss_probe(struct device *dev,
 	if (ret)
 		return ret;
 
+	lpss->cell->pset = info->pset;
+
 	intel_lpss_init_dev(lpss);
 
 	lpss->devid = ida_simple_get(&intel_lpss_devid_ida, 0, 0, GFP_KERNEL);
diff --git a/drivers/mfd/intel-lpss.h b/drivers/mfd/intel-lpss.h
index 2c7f8d7c0595..0dcea9eb2d03 100644
--- a/drivers/mfd/intel-lpss.h
+++ b/drivers/mfd/intel-lpss.h
@@ -16,12 +16,14 @@
 
 struct device;
 struct resource;
+struct property_set;
 
 struct intel_lpss_platform_info {
 	struct resource *mem;
 	int irq;
 	unsigned long clk_rate;
 	const char *clk_con_id;
+	struct property_set *pset;
 };
 
 int intel_lpss_probe(struct device *dev,
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
index b514f3cf140d..bd3aa4578346 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -55,6 +55,7 @@
  *	document number TBD : Coleto Creek
  *	document number TBD : Wildcat Point-LP
  *	document number TBD : 9 Series
+ *	document number TBD : Lewisburg
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -213,6 +214,7 @@ enum lpc_chipsets {
 	LPC_COLETO,	/* Coleto Creek */
 	LPC_WPT_LP,	/* Wildcat Point-LP */
 	LPC_BRASWELL,	/* Braswell SoC */
+	LPC_LEWISBURG,	/* Lewisburg */
 	LPC_9S,		/* 9 Series */
 };
 
@@ -521,6 +523,10 @@ static struct lpc_ich_info lpc_chipset_info[] = {
 		.name = "Braswell SoC",
 		.iTCO_version = 3,
 	},
+	[LPC_LEWISBURG] = {
+		.name = "Lewisburg",
+		.iTCO_version = 2,
+	},
 	[LPC_9S] = {
 		.name = "9 Series",
 		.iTCO_version = 2,
@@ -757,6 +763,15 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x9cc6), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc7), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc9), LPC_WPT_LP},
+	{ PCI_VDEVICE(INTEL, 0xa1c1), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c2), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c3), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c4), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c5), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c6), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa1c7), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa242), LPC_LEWISBURG},
+	{ PCI_VDEVICE(INTEL, 0xa243), LPC_LEWISBURG},
 	{ 0, },			/* End of list */
 };
 MODULE_DEVICE_TABLE(pci, lpc_ich_ids);
diff --git a/drivers/mfd/max14577.c b/drivers/mfd/max14577.c
index 56e216dedc91..2280b3fdcf68 100644
--- a/drivers/mfd/max14577.c
+++ b/drivers/mfd/max14577.c
@@ -495,7 +495,7 @@ MODULE_DEVICE_TABLE(i2c, max14577_i2c_id);
 #ifdef CONFIG_PM_SLEEP
 static int max14577_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max14577 *max14577 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -516,7 +516,7 @@ static int max14577_suspend(struct device *dev)
 
 static int max14577_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max14577 *max14577 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
diff --git a/drivers/mfd/max77686.c b/drivers/mfd/max77686.c
index d19be64cd32b..d959ebbb2194 100644
--- a/drivers/mfd/max77686.c
+++ b/drivers/mfd/max77686.c
@@ -352,7 +352,7 @@ MODULE_DEVICE_TABLE(i2c, max77686_i2c_id);
 #ifdef CONFIG_PM_SLEEP
 static int max77686_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77686_dev *max77686 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -374,7 +374,7 @@ static int max77686_suspend(struct device *dev)
 
 static int max77686_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77686_dev *max77686 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
index 007f729e150b..b83b7a7da1ae 100644
--- a/drivers/mfd/max77693.c
+++ b/drivers/mfd/max77693.c
@@ -334,7 +334,7 @@ MODULE_DEVICE_TABLE(i2c, max77693_i2c_id);
 
 static int max77693_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev)) {
@@ -347,7 +347,7 @@ static int max77693_suspend(struct device *dev)
 
 static int max77693_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77693_dev *max77693 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev)) {
diff --git a/drivers/mfd/max77843.c b/drivers/mfd/max77843.c
index 586098f1b233..7cfc95b49c5d 100644
--- a/drivers/mfd/max77843.c
+++ b/drivers/mfd/max77843.c
@@ -197,7 +197,7 @@ MODULE_DEVICE_TABLE(i2c, max77843_id);
 
 static int __maybe_unused max77843_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77693_dev *max77843 = i2c_get_clientdata(i2c);
 
 	disable_irq(max77843->irq);
@@ -209,7 +209,7 @@ static int __maybe_unused max77843_suspend(struct device *dev)
 
 static int __maybe_unused max77843_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max77693_dev *max77843 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
diff --git a/drivers/mfd/max8925-i2c.c b/drivers/mfd/max8925-i2c.c
index b0fe8103e401..70443b161a5b 100644
--- a/drivers/mfd/max8925-i2c.c
+++ b/drivers/mfd/max8925-i2c.c
@@ -215,7 +215,7 @@ static int max8925_remove(struct i2c_client *client)
 #ifdef CONFIG_PM_SLEEP
 static int max8925_suspend(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct max8925_chip *chip = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(dev) && chip->wakeup_flag)
@@ -225,7 +225,7 @@ static int max8925_suspend(struct device *dev)
 
 static int max8925_resume(struct device *dev)
 {
-	struct i2c_client *client = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *client = to_i2c_client(dev);
 	struct max8925_chip *chip = i2c_get_clientdata(client);
 
 	if (device_may_wakeup(dev) && chip->wakeup_flag)
diff --git a/drivers/mfd/max8997.c b/drivers/mfd/max8997.c
index 156ed6f92aa3..f316348e3d98 100644
--- a/drivers/mfd/max8997.c
+++ b/drivers/mfd/max8997.c
@@ -437,7 +437,7 @@ static u8 max8997_dumpaddr_haptic[] = {
 
 static int max8997_freeze(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
 	int i;
 
@@ -459,7 +459,7 @@ static int max8997_freeze(struct device *dev)
 
 static int max8997_restore(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
 	int i;
 
@@ -481,7 +481,7 @@ static int max8997_restore(struct device *dev)
 
 static int max8997_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -491,7 +491,7 @@ static int max8997_suspend(struct device *dev)
 
 static int max8997_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
diff --git a/drivers/mfd/max8998.c b/drivers/mfd/max8998.c
index a7afe3bf27fc..ab28b29400f6 100644
--- a/drivers/mfd/max8998.c
+++ b/drivers/mfd/max8998.c
@@ -274,7 +274,7 @@ MODULE_DEVICE_TABLE(i2c, max8998_i2c_id);
 
 static int max8998_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8998_dev *max8998 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -284,7 +284,7 @@ static int max8998_suspend(struct device *dev)
 
 static int max8998_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct max8998_dev *max8998 = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -344,7 +344,7 @@ static struct max8998_reg_dump max8998_dump[] = {
 /* Save registers before hibernation */
 static int max8998_freeze(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(max8998_dump); i++)
@@ -357,7 +357,7 @@ static int max8998_freeze(struct device *dev)
 /* Restore registers after hibernation */
 static int max8998_restore(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(max8998_dump); i++)
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 3f9f4c874d2a..d7f54e492aa6 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -383,16 +383,16 @@ static int mc13xxx_probe_flags_dt(struct mc13xxx *mc13xxx)
 	if (!np)
 		return -ENODEV;
 
-	if (of_get_property(np, "fsl,mc13xxx-uses-adc", NULL))
+	if (of_property_read_bool(np, "fsl,mc13xxx-uses-adc"))
 		mc13xxx->flags |= MC13XXX_USE_ADC;
 
-	if (of_get_property(np, "fsl,mc13xxx-uses-codec", NULL))
+	if (of_property_read_bool(np, "fsl,mc13xxx-uses-codec"))
 		mc13xxx->flags |= MC13XXX_USE_CODEC;
 
-	if (of_get_property(np, "fsl,mc13xxx-uses-rtc", NULL))
+	if (of_property_read_bool(np, "fsl,mc13xxx-uses-rtc"))
 		mc13xxx->flags |= MC13XXX_USE_RTC;
 
-	if (of_get_property(np, "fsl,mc13xxx-uses-touch", NULL))
+	if (of_property_read_bool(np, "fsl,mc13xxx-uses-touch"))
 		mc13xxx->flags |= MC13XXX_USE_TOUCHSCREEN;
 
 	return 0;
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 60b60dc63ddd..88bd1b1e47be 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -14,6 +14,7 @@
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
 #include <linux/acpi.h>
+#include <linux/property.h>
 #include <linux/mfd/core.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
@@ -192,6 +193,12 @@ static int mfd_add_device(struct device *parent, int id,
 			goto fail_alias;
 	}
 
+	if (cell->pset) {
+		ret = platform_device_add_properties(pdev, cell->pset);
+		if (ret)
+			goto fail_alias;
+	}
+
 	ret = mfd_platform_add_cell(pdev, cell, usage_count);
 	if (ret)
 		goto fail_alias;
diff --git a/drivers/mfd/qcom-spmi-pmic.c b/drivers/mfd/qcom-spmi-pmic.c
index af6ac1c4b45c..8653e8b9bb4f 100644
--- a/drivers/mfd/qcom-spmi-pmic.c
+++ b/drivers/mfd/qcom-spmi-pmic.c
@@ -127,7 +127,9 @@ static int pmic_spmi_probe(struct spmi_device *sdev)
 	if (IS_ERR(regmap))
 		return PTR_ERR(regmap);
 
-	pmic_spmi_show_revid(regmap, &sdev->dev);
+	/* Only the first slave id for a PMIC contains this information */
+	if (sdev->usid % 2 == 0)
+		pmic_spmi_show_revid(regmap, &sdev->dev);
 
 	return of_platform_populate(root, NULL, NULL, &sdev->dev);
 }
diff --git a/drivers/mfd/qcom_rpm.c b/drivers/mfd/qcom_rpm.c
index 207a3bd68559..1be47ad6441b 100644
--- a/drivers/mfd/qcom_rpm.c
+++ b/drivers/mfd/qcom_rpm.c
@@ -495,6 +495,8 @@ static int qcom_rpm_probe(struct platform_device *pdev)
 	}
 
 	match = of_match_device(qcom_rpm_of_match, &pdev->dev);
+	if (!match)
+		return -ENODEV;
 	rpm->data = match->data;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
diff --git a/drivers/mfd/sec-core.c b/drivers/mfd/sec-core.c
index 989076d6cb83..400e1d7d8d08 100644
--- a/drivers/mfd/sec-core.c
+++ b/drivers/mfd/sec-core.c
@@ -29,6 +29,7 @@
 #include <linux/mfd/samsung/s2mps11.h>
 #include <linux/mfd/samsung/s2mps13.h>
 #include <linux/mfd/samsung/s2mps14.h>
+#include <linux/mfd/samsung/s2mps15.h>
 #include <linux/mfd/samsung/s2mpu02.h>
 #include <linux/mfd/samsung/s5m8763.h>
 #include <linux/mfd/samsung/s5m8767.h>
@@ -67,7 +68,7 @@ static const struct mfd_cell s5m8767_devs[] = {
 
 static const struct mfd_cell s2mps11_devs[] = {
 	{
-		.name = "s2mps11-pmic",
+		.name = "s2mps11-regulator",
 	}, {
 		.name = "s2mps14-rtc",
 	}, {
@@ -77,7 +78,7 @@ static const struct mfd_cell s2mps11_devs[] = {
 };
 
 static const struct mfd_cell s2mps13_devs[] = {
-	{ .name = "s2mps13-pmic", },
+	{ .name = "s2mps13-regulator", },
 	{ .name = "s2mps13-rtc", },
 	{
 		.name = "s2mps13-clk",
@@ -87,7 +88,7 @@ static const struct mfd_cell s2mps13_devs[] = {
 
 static const struct mfd_cell s2mps14_devs[] = {
 	{
-		.name = "s2mps14-pmic",
+		.name = "s2mps14-regulator",
 	}, {
 		.name = "s2mps14-rtc",
 	}, {
@@ -96,6 +97,17 @@ static const struct mfd_cell s2mps14_devs[] = {
 	}
 };
 
+static const struct mfd_cell s2mps15_devs[] = {
+	{
+		.name = "s2mps15-regulator",
+	}, {
+		.name = "s2mps15-rtc",
+	}, {
+		.name = "s2mps13-clk",
+		.of_compatible = "samsung,s2mps13-clk",
+	},
+};
+
 static const struct mfd_cell s2mpa01_devs[] = {
 	{
 		.name = "s2mpa01-pmic",
@@ -104,7 +116,7 @@ static const struct mfd_cell s2mpa01_devs[] = {
 
 static const struct mfd_cell s2mpu02_devs[] = {
 	{
-		.name = "s2mpu02-pmic",
+		.name = "s2mpu02-regulator",
 	},
 };
 
@@ -122,6 +134,9 @@ static const struct of_device_id sec_dt_match[] = {
 		.compatible = "samsung,s2mps14-pmic",
 		.data = (void *)S2MPS14X,
 	}, {
+		.compatible = "samsung,s2mps15-pmic",
+		.data = (void *)S2MPS15X,
+	}, {
 		.compatible = "samsung,s2mpa01-pmic",
 		.data = (void *)S2MPA01,
 	}, {
@@ -223,6 +238,15 @@ static const struct regmap_config s2mps14_regmap_config = {
 	.cache_type = REGCACHE_FLAT,
 };
 
+static const struct regmap_config s2mps15_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = S2MPS15_REG_LDODSCH4,
+	.volatile_reg = s2mps11_volatile,
+	.cache_type = REGCACHE_FLAT,
+};
+
 static const struct regmap_config s2mpu02_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
@@ -384,6 +408,9 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 	case S2MPS14X:
 		regmap = &s2mps14_regmap_config;
 		break;
+	case S2MPS15X:
+		regmap = &s2mps15_regmap_config;
+		break;
 	case S5M8763X:
 		regmap = &s5m8763_regmap_config;
 		break;
@@ -442,6 +469,10 @@ static int sec_pmic_probe(struct i2c_client *i2c,
 		sec_devs = s2mps14_devs;
 		num_sec_devs = ARRAY_SIZE(s2mps14_devs);
 		break;
+	case S2MPS15X:
+		sec_devs = s2mps15_devs;
+		num_sec_devs = ARRAY_SIZE(s2mps15_devs);
+		break;
 	case S2MPU02:
 		sec_devs = s2mpu02_devs;
 		num_sec_devs = ARRAY_SIZE(s2mpu02_devs);
@@ -505,7 +536,7 @@ static void sec_pmic_shutdown(struct i2c_client *i2c)
 #ifdef CONFIG_PM_SLEEP
 static int sec_pmic_suspend(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct sec_pmic_dev *sec_pmic = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
@@ -526,7 +557,7 @@ static int sec_pmic_suspend(struct device *dev)
 
 static int sec_pmic_resume(struct device *dev)
 {
-	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct i2c_client *i2c = to_i2c_client(dev);
 	struct sec_pmic_dev *sec_pmic = i2c_get_clientdata(i2c);
 
 	if (device_may_wakeup(dev))
diff --git a/drivers/mfd/sec-irq.c b/drivers/mfd/sec-irq.c
index 806fa8dbb22d..d77de431cc50 100644
--- a/drivers/mfd/sec-irq.c
+++ b/drivers/mfd/sec-irq.c
@@ -407,6 +407,11 @@ static const struct regmap_irq_chip s2mps14_irq_chip = {
 	S2MPS1X_IRQ_CHIP_COMMON_DATA,
 };
 
+static const struct regmap_irq_chip s2mps15_irq_chip = {
+	.name = "s2mps15",
+	S2MPS1X_IRQ_CHIP_COMMON_DATA,
+};
+
 static const struct regmap_irq_chip s2mpu02_irq_chip = {
 	.name = "s2mpu02",
 	.irqs = s2mpu02_irqs,
@@ -466,6 +471,9 @@ int sec_irq_init(struct sec_pmic_dev *sec_pmic)
 	case S2MPS14X:
 		sec_irq_chip = &s2mps14_irq_chip;
 		break;
+	case S2MPS15X:
+		sec_irq_chip = &s2mps15_irq_chip;
+		break;
 	case S2MPU02:
 		sec_irq_chip = &s2mpu02_irq_chip;
 		break;
diff --git a/drivers/mfd/sta2x11-mfd.c b/drivers/mfd/sta2x11-mfd.c
index b3e5c6f45105..9292202039ee 100644
--- a/drivers/mfd/sta2x11-mfd.c
+++ b/drivers/mfd/sta2x11-mfd.c
@@ -372,12 +372,6 @@ static struct platform_driver sta2x11_sctl_platform_driver = {
 	.probe		= sta2x11_sctl_probe,
 };
 
-static int __init sta2x11_sctl_init(void)
-{
-	pr_info("%s\n", __func__);
-	return platform_driver_register(&sta2x11_sctl_platform_driver);
-}
-
 static struct platform_driver sta2x11_platform_driver = {
 	.driver = {
 		.name	= STA2X11_MFD_APBREG_NAME,
@@ -385,12 +379,6 @@ static struct platform_driver sta2x11_platform_driver = {
 	.probe		= sta2x11_apbreg_probe,
 };
 
-static int __init sta2x11_apbreg_init(void)
-{
-	pr_info("%s\n", __func__);
-	return platform_driver_register(&sta2x11_platform_driver);
-}
-
 static struct platform_driver sta2x11_apb_soc_regs_platform_driver = {
 	.driver = {
 		.name	= STA2X11_MFD_APB_SOC_REGS_NAME,
@@ -398,12 +386,6 @@ static struct platform_driver sta2x11_apb_soc_regs_platform_driver = {
 	.probe		= sta2x11_apb_soc_regs_probe,
 };
 
-static int __init sta2x11_apb_soc_regs_init(void)
-{
-	pr_info("%s\n", __func__);
-	return platform_driver_register(&sta2x11_apb_soc_regs_platform_driver);
-}
-
 static struct platform_driver sta2x11_scr_platform_driver = {
 	.driver = {
 		.name = STA2X11_MFD_SCR_NAME,
@@ -411,13 +393,18 @@ static struct platform_driver sta2x11_scr_platform_driver = {
 	.probe = sta2x11_scr_probe,
 };
 
-static int __init sta2x11_scr_init(void)
+static struct platform_driver * const drivers[] = {
+	&sta2x11_platform_driver,
+	&sta2x11_sctl_platform_driver,
+	&sta2x11_apb_soc_regs_platform_driver,
+	&sta2x11_scr_platform_driver,
+};
+
+static int __init sta2x11_drivers_init(void)
 {
-	pr_info("%s\n", __func__);
-	return platform_driver_register(&sta2x11_scr_platform_driver);
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
-
 /*
  * What follows are the PCI devices that host the above pdevs.
  * Each logic block is 4kB and they are all consecutive: we use this info.
@@ -664,10 +651,7 @@ static int __init sta2x11_mfd_init(void)
  * prepares platform drivers very early and probe the PCI device later,
  * but before other PCI devices.
  */
-subsys_initcall(sta2x11_apbreg_init);
-subsys_initcall(sta2x11_sctl_init);
-subsys_initcall(sta2x11_apb_soc_regs_init);
-subsys_initcall(sta2x11_scr_init);
+subsys_initcall(sta2x11_drivers_init);
 rootfs_initcall(sta2x11_mfd_init);
 
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/syscon.c b/drivers/mfd/syscon.c
index 176bf0fa2685..b7aabeefab07 100644
--- a/drivers/mfd/syscon.c
+++ b/drivers/mfd/syscon.c
@@ -47,6 +47,7 @@ static struct syscon *of_syscon_register(struct device_node *np)
 	struct syscon *syscon;
 	struct regmap *regmap;
 	void __iomem *base;
+	u32 reg_io_width;
 	int ret;
 	struct regmap_config syscon_config = syscon_regmap_config;
 
@@ -69,6 +70,18 @@ static struct syscon *of_syscon_register(struct device_node *np)
 	 else if (of_property_read_bool(np, "little-endian"))
 		syscon_config.val_format_endian = REGMAP_ENDIAN_LITTLE;
 
+	/*
+	 * search for reg-io-width property in DT. If it is not provided,
+	 * default to 4 bytes. regmap_init_mmio will return an error if values
+	 * are invalid so there is no need to check them here.
+	 */
+	ret = of_property_read_u32(np, "reg-io-width", &reg_io_width);
+	if (ret)
+		reg_io_width = 4;
+
+	syscon_config.reg_stride = reg_io_width;
+	syscon_config.val_bits = reg_io_width * 8;
+
 	regmap = regmap_init_mmio(NULL, base, &syscon_config);
 	if (IS_ERR(regmap)) {
 		pr_err("regmap init failed\n");
diff --git a/drivers/mfd/tc6393xb.c b/drivers/mfd/tc6393xb.c
index 8c84a513016b..1ecbfa40d1b3 100644
--- a/drivers/mfd/tc6393xb.c
+++ b/drivers/mfd/tc6393xb.c
@@ -437,8 +437,8 @@ static int tc6393xb_gpio_get(struct gpio_chip *chip,
 	struct tc6393xb *tc6393xb = container_of(chip, struct tc6393xb, gpio);
 
 	/* XXX: does dsr also represent inputs? */
-	return tmio_ioread8(tc6393xb->scr + SCR_GPO_DSR(offset / 8))
-		& TC_GPIO_BIT(offset);
+	return !!(tmio_ioread8(tc6393xb->scr + SCR_GPO_DSR(offset / 8))
+		  & TC_GPIO_BIT(offset));
 }
 
 static void __tc6393xb_gpio_set(struct gpio_chip *chip,
diff --git a/drivers/mfd/timberdale.c b/drivers/mfd/timberdale.c
index 6ce36d6970a4..c9339f85359b 100644
--- a/drivers/mfd/timberdale.c
+++ b/drivers/mfd/timberdale.c
@@ -39,8 +39,8 @@
 #include <linux/spi/max7301.h>
 #include <linux/spi/mc33880.h>
 
-#include <media/timb_radio.h>
-#include <media/timb_video.h>
+#include <linux/platform_data/media/timb_radio.h>
+#include <linux/platform_data/media/timb_video.h>
 
 #include <linux/timb_dma.h>
 
diff --git a/drivers/mfd/tps65010.c b/drivers/mfd/tps65010.c
index 448f0a182dc4..83e615ed100a 100644
--- a/drivers/mfd/tps65010.c
+++ b/drivers/mfd/tps65010.c
@@ -499,11 +499,11 @@ static int tps65010_gpio_get(struct gpio_chip *chip, unsigned offset)
 	if (offset < 4) {
 		value = i2c_smbus_read_byte_data(tps->client, TPS_DEFGPIO);
 		if (value < 0)
-			return 0;
+			return value;
 		if (value & (1 << (offset + 4)))	/* output */
 			return !(value & (1 << offset));
 		else					/* input */
-			return (value & (1 << offset));
+			return !!(value & (1 << offset));
 	}
 
 	/* REVISIT we *could* report LED1/nPG and LED2 state ... */
@@ -638,7 +638,7 @@ static int tps65010_probe(struct i2c_client *client,
 		tps->outmask = board->outmask;
 
 		tps->chip.label = client->name;
-		tps->chip.dev = &client->dev;
+		tps->chip.parent = &client->dev;
 		tps->chip.owner = THIS_MODULE;
 
 		tps->chip.set = tps65010_gpio_set;
diff --git a/drivers/mfd/ucb1x00-core.c b/drivers/mfd/ucb1x00-core.c
index f691d7ecad52..bcafe1ecd71c 100644
--- a/drivers/mfd/ucb1x00-core.c
+++ b/drivers/mfd/ucb1x00-core.c
@@ -133,7 +133,7 @@ static int ucb1x00_gpio_get(struct gpio_chip *chip, unsigned offset)
 	val = ucb1x00_reg_read(ucb, UCB_IO_DATA);
 	ucb1x00_disable(ucb);
 
-	return val & (1 << offset);
+	return !!(val & (1 << offset));
 }
 
 static int ucb1x00_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
@@ -570,7 +570,7 @@ static int ucb1x00_probe(struct mcp *mcp)
 
 	if (pdata && pdata->gpio_base) {
 		ucb->gpio.label = dev_name(&ucb->dev);
-		ucb->gpio.dev = &ucb->dev;
+		ucb->gpio.parent = &ucb->dev;
 		ucb->gpio.owner = THIS_MODULE;
 		ucb->gpio.base = pdata->gpio_base;
 		ucb->gpio.ngpio = 10;
diff --git a/drivers/mfd/vexpress-sysreg.c b/drivers/mfd/vexpress-sysreg.c
index 3e628df9280c..855c0204f09a 100644
--- a/drivers/mfd/vexpress-sysreg.c
+++ b/drivers/mfd/vexpress-sysreg.c
@@ -11,7 +11,7 @@
  * Copyright (C) 2012 ARM Limited
  */
 
-#include <linux/basic_mmio_gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/mfd/core.h>
@@ -164,7 +164,7 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 {
 	struct resource *mem;
 	void __iomem *base;
-	struct bgpio_chip *mmc_gpio_chip;
+	struct gpio_chip *mmc_gpio_chip;
 	int master;
 	u32 dt_hbi;
 
@@ -201,8 +201,8 @@ static int vexpress_sysreg_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	bgpio_init(mmc_gpio_chip, &pdev->dev, 0x4, base + SYS_MCI,
 			NULL, NULL, NULL, NULL, 0);
-	mmc_gpio_chip->gc.ngpio = 2;
-	gpiochip_add(&mmc_gpio_chip->gc);
+	mmc_gpio_chip->ngpio = 2;
+	gpiochip_add(mmc_gpio_chip);
 
 	return mfd_add_devices(&pdev->dev, PLATFORM_DEVID_AUTO,
 			vexpress_sysreg_cells,
diff --git a/drivers/mfd/wm5110-tables.c b/drivers/mfd/wm5110-tables.c
index 2bb2d0467a92..c18e11f42b3f 100644
--- a/drivers/mfd/wm5110-tables.c
+++ b/drivers/mfd/wm5110-tables.c
@@ -762,9 +762,9 @@ static const struct reg_default wm5110_reg_default[] = {
 	{ 0x00000200, 0x0006 },    /* R512   - Mic Charge Pump 1 */
 	{ 0x00000210, 0x0184 },    /* R528   - LDO1 Control 1 */
 	{ 0x00000213, 0x03E4 },    /* R531   - LDO2 Control 1 */
-	{ 0x00000218, 0x01A6 },    /* R536   - Mic Bias Ctrl 1 */
-	{ 0x00000219, 0x01A6 },    /* R537   - Mic Bias Ctrl 2 */
-	{ 0x0000021A, 0x01A6 },    /* R538   - Mic Bias Ctrl 3 */
+	{ 0x00000218, 0x00E6 },    /* R536   - Mic Bias Ctrl 1 */
+	{ 0x00000219, 0x00E6 },    /* R537   - Mic Bias Ctrl 2 */
+	{ 0x0000021A, 0x00E6 },    /* R538   - Mic Bias Ctrl 3 */
 	{ 0x00000293, 0x0000 },    /* R659   - Accessory Detect Mode 1 */
 	{ 0x0000029B, 0x0028 },    /* R667   - Headphone Detect 1 */
 	{ 0x000002A2, 0x0000 },    /* R674   - Micd clamp control */
diff --git a/drivers/mfd/wm831x-otp.c b/drivers/mfd/wm831x-otp.c
index b90f3e06b6c9..ebac0027f8e0 100644
--- a/drivers/mfd/wm831x-otp.c
+++ b/drivers/mfd/wm831x-otp.c
@@ -47,20 +47,14 @@ static ssize_t wm831x_unique_id_show(struct device *dev,
 				     struct device_attribute *attr, char *buf)
 {
 	struct wm831x *wm831x = dev_get_drvdata(dev);
-	int i, rval;
+	int rval;
 	char id[WM831X_UNIQUE_ID_LEN];
-	ssize_t ret = 0;
 
 	rval = wm831x_unique_id_read(wm831x, id);
 	if (rval < 0)
 		return 0;
 
-	for (i = 0; i < WM831X_UNIQUE_ID_LEN; i++)
-		ret += sprintf(&buf[ret], "%02x", buf[i]);
-
-	ret += sprintf(&buf[ret], "\n");
-
-	return ret;
+	return sprintf(buf, "%*phN\n", WM831X_UNIQUE_ID_LEN, id);
 }
 
 static DEVICE_ATTR(unique_id, 0444, wm831x_unique_id_show, NULL);
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 22892c701c63..054fc10cb3b6 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -95,6 +95,7 @@ config DUMMY_IRQ
 config IBM_ASM
 	tristate "Device driver for IBM RSA service processor"
 	depends on X86 && PCI && INPUT
+	depends on SERIAL_8250 || SERIAL_8250=n
 	---help---
 	  This option enables device driver support for in-band access to the
 	  IBM RSA (Condor) service processor in eServer xSeries systems.
diff --git a/drivers/misc/cxl/Makefile b/drivers/misc/cxl/Makefile
index 6982f603fadc..be2ac5ce349f 100644
--- a/drivers/misc/cxl/Makefile
+++ b/drivers/misc/cxl/Makefile
@@ -1,4 +1,5 @@
-ccflags-y := -Werror -Wno-unused-const-variable
+ccflags-y			:= $(call cc-disable-warning, unused-const-variable)
+ccflags-$(CONFIG_PPC_WERROR)	+= -Werror
 
 cxl-y				+= main.o file.o irq.o fault.o native.o
 cxl-y				+= context.o sysfs.o debugfs.o pci.o trace.o
diff --git a/drivers/misc/cxl/api.c b/drivers/misc/cxl/api.c
index 103baf0e0c5b..ea3eeb7011e1 100644
--- a/drivers/misc/cxl/api.c
+++ b/drivers/misc/cxl/api.c
@@ -25,7 +25,6 @@ struct cxl_context *cxl_dev_context_init(struct pci_dev *dev)
 
 	afu = cxl_pci_to_afu(dev);
 
-	get_device(&afu->dev);
 	ctx = cxl_context_alloc();
 	if (IS_ERR(ctx)) {
 		rc = PTR_ERR(ctx);
@@ -61,7 +60,6 @@ err_mapping:
 err_ctx:
 	kfree(ctx);
 err_dev:
-	put_device(&afu->dev);
 	return ERR_PTR(rc);
 }
 EXPORT_SYMBOL_GPL(cxl_dev_context_init);
@@ -87,8 +85,6 @@ int cxl_release_context(struct cxl_context *ctx)
 	if (ctx->status >= STARTED)
 		return -EBUSY;
 
-	put_device(&ctx->afu->dev);
-
 	cxl_context_free(ctx);
 
 	return 0;
@@ -176,7 +172,7 @@ int cxl_start_context(struct cxl_context *ctx, u64 wed,
 
 	if (task) {
 		ctx->pid = get_task_pid(task, PIDTYPE_PID);
-		get_pid(ctx->pid);
+		ctx->glpid = get_task_pid(task->group_leader, PIDTYPE_PID);
 		kernel = false;
 	}
 
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
index 2faa1270d085..262b88eac414 100644
--- a/drivers/misc/cxl/context.c
+++ b/drivers/misc/cxl/context.c
@@ -42,7 +42,7 @@ int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master,
 	spin_lock_init(&ctx->sste_lock);
 	ctx->afu = afu;
 	ctx->master = master;
-	ctx->pid = NULL; /* Set in start work ioctl */
+	ctx->pid = ctx->glpid = NULL; /* Set in start work ioctl */
 	mutex_init(&ctx->mapping_lock);
 	ctx->mapping = mapping;
 
@@ -97,6 +97,12 @@ int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master,
 	ctx->pe = i;
 	ctx->elem = &ctx->afu->spa[i];
 	ctx->pe_inserted = false;
+
+	/*
+	 * take a ref on the afu so that it stays alive at-least till
+	 * this context is reclaimed inside reclaim_ctx.
+	 */
+	cxl_afu_get(afu);
 	return 0;
 }
 
@@ -211,7 +217,11 @@ int __detach_context(struct cxl_context *ctx)
 	WARN_ON(cxl_detach_process(ctx) &&
 		cxl_adapter_link_ok(ctx->afu->adapter));
 	flush_work(&ctx->fault_work); /* Only needed for dedicated process */
+
+	/* release the reference to the group leader and mm handling pid */
 	put_pid(ctx->pid);
+	put_pid(ctx->glpid);
+
 	cxl_ctx_put();
 	return 0;
 }
@@ -278,6 +288,9 @@ static void reclaim_ctx(struct rcu_head *rcu)
 	if (ctx->irq_bitmap)
 		kfree(ctx->irq_bitmap);
 
+	/* Drop ref to the afu device taken during cxl_context_init */
+	cxl_afu_put(ctx->afu);
+
 	kfree(ctx);
 }
 
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
index 0cfb9c129f27..a521bc72cec2 100644
--- a/drivers/misc/cxl/cxl.h
+++ b/drivers/misc/cxl/cxl.h
@@ -403,6 +403,18 @@ struct cxl_afu {
 	bool enabled;
 };
 
+/* AFU refcount management */
+static inline struct cxl_afu *cxl_afu_get(struct cxl_afu *afu)
+{
+
+	return (get_device(&afu->dev) == NULL) ? NULL : afu;
+}
+
+static inline void  cxl_afu_put(struct cxl_afu *afu)
+{
+	put_device(&afu->dev);
+}
+
 
 struct cxl_irq_name {
 	struct list_head list;
@@ -433,6 +445,9 @@ struct cxl_context {
 	unsigned int sst_size, sst_lru;
 
 	wait_queue_head_t wq;
+	/* pid of the group leader associated with the pid */
+	struct pid *glpid;
+	/* use mm context associated with this pid for ds faults */
 	struct pid *pid;
 	spinlock_t lock; /* Protects pending_irq_mask, pending_fault and fault_addr */
 	/* Only used in PR mode */
diff --git a/drivers/misc/cxl/fault.c b/drivers/misc/cxl/fault.c
index 25a5418c55cb..81c3f75b7330 100644
--- a/drivers/misc/cxl/fault.c
+++ b/drivers/misc/cxl/fault.c
@@ -166,13 +166,92 @@ static void cxl_handle_page_fault(struct cxl_context *ctx,
 	cxl_ack_irq(ctx, CXL_PSL_TFC_An_R, 0);
 }
 
+/*
+ * Returns the mm_struct corresponding to the context ctx via ctx->pid
+ * In case the task has exited we use the task group leader accessible
+ * via ctx->glpid to find the next task in the thread group that has a
+ * valid  mm_struct associated with it. If a task with valid mm_struct
+ * is found the ctx->pid is updated to use the task struct for subsequent
+ * translations. In case no valid mm_struct is found in the task group to
+ * service the fault a NULL is returned.
+ */
+static struct mm_struct *get_mem_context(struct cxl_context *ctx)
+{
+	struct task_struct *task = NULL;
+	struct mm_struct *mm = NULL;
+	struct pid *old_pid = ctx->pid;
+
+	if (old_pid == NULL) {
+		pr_warn("%s: Invalid context for pe=%d\n",
+			 __func__, ctx->pe);
+		return NULL;
+	}
+
+	task = get_pid_task(old_pid, PIDTYPE_PID);
+
+	/*
+	 * pid_alive may look racy but this saves us from costly
+	 * get_task_mm when the task is a zombie. In worst case
+	 * we may think a task is alive, which is about to die
+	 * but get_task_mm will return NULL.
+	 */
+	if (task != NULL && pid_alive(task))
+		mm = get_task_mm(task);
+
+	/* release the task struct that was taken earlier */
+	if (task)
+		put_task_struct(task);
+	else
+		pr_devel("%s: Context owning pid=%i for pe=%i dead\n",
+			__func__, pid_nr(old_pid), ctx->pe);
+
+	/*
+	 * If we couldn't find the mm context then use the group
+	 * leader to iterate over the task group and find a task
+	 * that gives us mm_struct.
+	 */
+	if (unlikely(mm == NULL && ctx->glpid != NULL)) {
+
+		rcu_read_lock();
+		task = pid_task(ctx->glpid, PIDTYPE_PID);
+		if (task)
+			do {
+				mm = get_task_mm(task);
+				if (mm) {
+					ctx->pid = get_task_pid(task,
+								PIDTYPE_PID);
+					break;
+				}
+				task = next_thread(task);
+			} while (task && !thread_group_leader(task));
+		rcu_read_unlock();
+
+		/* check if we switched pid */
+		if (ctx->pid != old_pid) {
+			if (mm)
+				pr_devel("%s:pe=%i switch pid %i->%i\n",
+					 __func__, ctx->pe, pid_nr(old_pid),
+					 pid_nr(ctx->pid));
+			else
+				pr_devel("%s:Cannot find mm for pid=%i\n",
+					 __func__, pid_nr(old_pid));
+
+			/* drop the reference to older pid */
+			put_pid(old_pid);
+		}
+	}
+
+	return mm;
+}
+
+
+
 void cxl_handle_fault(struct work_struct *fault_work)
 {
 	struct cxl_context *ctx =
 		container_of(fault_work, struct cxl_context, fault_work);
 	u64 dsisr = ctx->dsisr;
 	u64 dar = ctx->dar;
-	struct task_struct *task = NULL;
 	struct mm_struct *mm = NULL;
 
 	if (cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An) != dsisr ||
@@ -195,17 +274,17 @@ void cxl_handle_fault(struct work_struct *fault_work)
 		"DSISR: %#llx DAR: %#llx\n", ctx->pe, dsisr, dar);
 
 	if (!ctx->kernel) {
-		if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
-			pr_devel("cxl_handle_fault unable to get task %i\n",
-				 pid_nr(ctx->pid));
+
+		mm = get_mem_context(ctx);
+		/* indicates all the thread in task group have exited */
+		if (mm == NULL) {
+			pr_devel("%s: unable to get mm for pe=%d pid=%i\n",
+				 __func__, ctx->pe, pid_nr(ctx->pid));
 			cxl_ack_ae(ctx);
 			return;
-		}
-		if (!(mm = get_task_mm(task))) {
-			pr_devel("cxl_handle_fault unable to get mm %i\n",
-				 pid_nr(ctx->pid));
-			cxl_ack_ae(ctx);
-			goto out;
+		} else {
+			pr_devel("Handling page fault for pe=%d pid=%i\n",
+				 ctx->pe, pid_nr(ctx->pid));
 		}
 	}
 
@@ -218,33 +297,22 @@ void cxl_handle_fault(struct work_struct *fault_work)
 
 	if (mm)
 		mmput(mm);
-out:
-	if (task)
-		put_task_struct(task);
 }
 
 static void cxl_prefault_one(struct cxl_context *ctx, u64 ea)
 {
-	int rc;
-	struct task_struct *task;
 	struct mm_struct *mm;
 
-	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
-		pr_devel("cxl_prefault_one unable to get task %i\n",
-			 pid_nr(ctx->pid));
-		return;
-	}
-	if (!(mm = get_task_mm(task))) {
+	mm = get_mem_context(ctx);
+	if (mm == NULL) {
 		pr_devel("cxl_prefault_one unable to get mm %i\n",
 			 pid_nr(ctx->pid));
-		put_task_struct(task);
 		return;
 	}
 
-	rc = cxl_fault_segment(ctx, mm, ea);
+	cxl_fault_segment(ctx, mm, ea);
 
 	mmput(mm);
-	put_task_struct(task);
 }
 
 static u64 next_segment(u64 ea, u64 vsid)
@@ -263,18 +331,13 @@ static void cxl_prefault_vma(struct cxl_context *ctx)
 	struct copro_slb slb;
 	struct vm_area_struct *vma;
 	int rc;
-	struct task_struct *task;
 	struct mm_struct *mm;
 
-	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
-		pr_devel("cxl_prefault_vma unable to get task %i\n",
-			 pid_nr(ctx->pid));
-		return;
-	}
-	if (!(mm = get_task_mm(task))) {
+	mm = get_mem_context(ctx);
+	if (mm == NULL) {
 		pr_devel("cxl_prefault_vm unable to get mm %i\n",
 			 pid_nr(ctx->pid));
-		goto out1;
+		return;
 	}
 
 	down_read(&mm->mmap_sem);
@@ -295,8 +358,6 @@ static void cxl_prefault_vma(struct cxl_context *ctx)
 	up_read(&mm->mmap_sem);
 
 	mmput(mm);
-out1:
-	put_task_struct(task);
 }
 
 void cxl_prefault(struct cxl_context *ctx, u64 wed)
diff --git a/drivers/misc/cxl/file.c b/drivers/misc/cxl/file.c
index 7ccd2998be92..783337d22f36 100644
--- a/drivers/misc/cxl/file.c
+++ b/drivers/misc/cxl/file.c
@@ -67,7 +67,13 @@ static int __afu_open(struct inode *inode, struct file *file, bool master)
 		spin_unlock(&adapter->afu_list_lock);
 		goto err_put_adapter;
 	}
-	get_device(&afu->dev);
+
+	/*
+	 * taking a ref to the afu so that it doesn't go away
+	 * for rest of the function. This ref is released before
+	 * we return.
+	 */
+	cxl_afu_get(afu);
 	spin_unlock(&adapter->afu_list_lock);
 
 	if (!afu->current_mode)
@@ -90,13 +96,12 @@ static int __afu_open(struct inode *inode, struct file *file, bool master)
 	file->private_data = ctx;
 	cxl_ctx_get();
 
-	/* Our ref on the AFU will now hold the adapter */
-	put_device(&adapter->dev);
-
-	return 0;
+	/* indicate success */
+	rc = 0;
 
 err_put_afu:
-	put_device(&afu->dev);
+	/* release the ref taken earlier */
+	cxl_afu_put(afu);
 err_put_adapter:
 	put_device(&adapter->dev);
 	return rc;
@@ -131,8 +136,6 @@ int afu_release(struct inode *inode, struct file *file)
 		mutex_unlock(&ctx->mapping_lock);
 	}
 
-	put_device(&ctx->afu->dev);
-
 	/*
 	 * At this this point all bottom halfs have finished and we should be
 	 * getting no more IRQs from the hardware for this context.  Once it's
@@ -198,8 +201,12 @@ static long afu_ioctl_start_work(struct cxl_context *ctx,
 	 * where a process (master, some daemon, etc) has opened the chardev on
 	 * behalf of another process, so the AFU's mm gets bound to the process
 	 * that performs this ioctl and not the process that opened the file.
+	 * Also we grab the PID of the group leader so that if the task that
+	 * has performed the attach operation exits the mm context of the
+	 * process is still accessible.
 	 */
-	ctx->pid = get_pid(get_task_pid(current, PIDTYPE_PID));
+	ctx->pid = get_task_pid(current, PIDTYPE_PID);
+	ctx->glpid = get_task_pid(current->group_leader, PIDTYPE_PID);
 
 	trace_cxl_attach(ctx, work.work_element_descriptor, work.num_interrupts, amr);
 
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
index 85761d7eb333..0c6c17a1c59e 100644
--- a/drivers/misc/cxl/pci.c
+++ b/drivers/misc/cxl/pci.c
@@ -138,6 +138,7 @@ static const struct pci_device_id cxl_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x0477), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x044b), },
 	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x04cf), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x0601), },
 	{ PCI_DEVICE_CLASS(0x120000, ~0), },
 
 	{ }
@@ -414,7 +415,7 @@ static int cxl_setup_psl_timebase(struct cxl *adapter, struct pci_dev *dev)
 		delta = mftb() - psl_tb;
 		if (delta < 0)
 			delta = -delta;
-	} while (cputime_to_usecs(delta) > 16);
+	} while (tb_to_ns(delta) > 16000);
 
 	return 0;
 }
diff --git a/drivers/misc/cxl/vphb.c b/drivers/misc/cxl/vphb.c
index c241e15cacb1..cbd4331fb45c 100644
--- a/drivers/misc/cxl/vphb.c
+++ b/drivers/misc/cxl/vphb.c
@@ -203,7 +203,7 @@ static int cxl_pcie_write_config(struct pci_bus *bus, unsigned int devfn,
 	mask <<= shift;
 	val <<= shift;
 
-	v = (in_le32(ioaddr) & ~mask) || (val & mask);
+	v = (in_le32(ioaddr) & ~mask) | (val & mask);
 
 	out_le32(ioaddr, v);
 	return PCIBIOS_SUCCESSFUL;
diff --git a/drivers/misc/ibmasm/ibmasm.h b/drivers/misc/ibmasm/ibmasm.h
index 9b083448814d..5bd127727d8e 100644
--- a/drivers/misc/ibmasm/ibmasm.h
+++ b/drivers/misc/ibmasm/ibmasm.h
@@ -211,7 +211,7 @@ void ibmasmfs_unregister(void);
 void ibmasmfs_add_sp(struct service_processor *sp);
 
 /* uart */
-#ifdef CONFIG_SERIAL_8250
+#if IS_ENABLED(CONFIG_SERIAL_8250)
 void ibmasm_register_uart(struct service_processor *sp);
 void ibmasm_unregister_uart(struct service_processor *sp);
 #else
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index b2f2486b3d75..80f9afcb1382 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -458,7 +458,11 @@ static int mei_ioctl_client_notify_request(struct file *file, u32 request)
 {
 	struct mei_cl *cl = file->private_data;
 
-	return mei_cl_notify_request(cl, file, request);
+	if (request != MEI_HBM_NOTIFICATION_START &&
+	    request != MEI_HBM_NOTIFICATION_STOP)
+		return -EINVAL;
+
+	return mei_cl_notify_request(cl, file, (u8)request);
 }
 
 /**
@@ -657,7 +661,9 @@ out:
  * @file: pointer to file structure
  * @band: band bitmap
  *
- * Return: poll mask
+ * Return: negative on error,
+ *         0 if it did no changes,
+ *         and positive a process was added or deleted
  */
 static int mei_fasync(int fd, struct file *file, int band)
 {
@@ -665,7 +671,7 @@ static int mei_fasync(int fd, struct file *file, int band)
 	struct mei_cl *cl = file->private_data;
 
 	if (!mei_cl_is_connected(cl))
-		return POLLERR;
+		return -ENODEV;
 
 	return fasync_helper(fd, file, band, &cl->ev_async);
 }
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index 27678d8154e0..75fc9c688df8 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -31,6 +31,7 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
@@ -436,7 +437,7 @@ static inline void mei_me_set_pm_domain(struct mei_device *dev)
 		dev->pg_domain.ops.runtime_resume = mei_me_pm_runtime_resume;
 		dev->pg_domain.ops.runtime_idle = mei_me_pm_runtime_idle;
 
-		pdev->dev.pm_domain = &dev->pg_domain;
+		dev_pm_domain_set(&pdev->dev, &dev->pg_domain);
 	}
 }
 
@@ -448,7 +449,7 @@ static inline void mei_me_set_pm_domain(struct mei_device *dev)
 static inline void mei_me_unset_pm_domain(struct mei_device *dev)
 {
 	/* stop using pm callbacks if any */
-	dev->dev->pm_domain = NULL;
+	dev_pm_domain_set(dev->dev, NULL);
 }
 
 static const struct dev_pm_ops mei_me_pm_ops = {
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index 0882c0201907..71f8a7475717 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -27,6 +27,7 @@
 #include <linux/jiffies.h>
 #include <linux/interrupt.h>
 #include <linux/workqueue.h>
+#include <linux/pm_domain.h>
 #include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
@@ -388,7 +389,7 @@ static inline void mei_txe_set_pm_domain(struct mei_device *dev)
 		dev->pg_domain.ops.runtime_resume = mei_txe_pm_runtime_resume;
 		dev->pg_domain.ops.runtime_idle = mei_txe_pm_runtime_idle;
 
-		pdev->dev.pm_domain = &dev->pg_domain;
+		dev_pm_domain_set(&pdev->dev, &dev->pg_domain);
 	}
 }
 
@@ -400,7 +401,7 @@ static inline void mei_txe_set_pm_domain(struct mei_device *dev)
 static inline void mei_txe_unset_pm_domain(struct mei_device *dev)
 {
 	/* stop using pm callbacks if any */
-	dev->dev->pm_domain = NULL;
+	dev_pm_domain_set(dev->dev, NULL);
 }
 
 static const struct dev_pm_ops mei_txe_pm_ops = {
diff --git a/drivers/misc/mic/card/mic_virtio.c b/drivers/misc/mic/card/mic_virtio.c
index e486a0c26267..f6ed57d3125c 100644
--- a/drivers/misc/mic/card/mic_virtio.c
+++ b/drivers/misc/mic/card/mic_virtio.c
@@ -311,7 +311,7 @@ unmap:
 static int mic_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 			struct virtqueue *vqs[],
 			vq_callback_t *callbacks[],
-			const char *names[])
+			const char * const names[])
 {
 	struct mic_vdev *mvdev = to_micvdev(vdev);
 	struct mic_device_ctrl __iomem *dc = mvdev->dc;
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index d8486168415a..fe207e542032 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -47,13 +47,10 @@
 #include "queue.h"
 
 MODULE_ALIAS("mmc:block");
-
-#ifdef KERNEL
 #ifdef MODULE_PARAM_PREFIX
 #undef MODULE_PARAM_PREFIX
 #endif
 #define MODULE_PARAM_PREFIX "mmcblk."
-#endif
 
 #define INAND_CMD38_ARG_EXT_CSD  113
 #define INAND_CMD38_ARG_ERASE    0x00
@@ -171,11 +168,7 @@ static struct mmc_blk_data *mmc_blk_get(struct gendisk *disk)
 
 static inline int mmc_get_devidx(struct gendisk *disk)
 {
-	int devmaj = MAJOR(disk_devt(disk));
-	int devidx = MINOR(disk_devt(disk)) / perdev_minors;
-
-	if (!devmaj)
-		devidx = disk->first_minor / perdev_minors;
+	int devidx = disk->first_minor / perdev_minors;
 	return devidx;
 }
 
@@ -344,7 +337,7 @@ static struct mmc_blk_ioc_data *mmc_blk_ioctl_copy_from_user(
 	struct mmc_blk_ioc_data *idata;
 	int err;
 
-	idata = kzalloc(sizeof(*idata), GFP_KERNEL);
+	idata = kmalloc(sizeof(*idata), GFP_KERNEL);
 	if (!idata) {
 		err = -ENOMEM;
 		goto out;
@@ -364,7 +357,7 @@ static struct mmc_blk_ioc_data *mmc_blk_ioctl_copy_from_user(
 	if (!idata->buf_bytes)
 		return idata;
 
-	idata->buf = kzalloc(idata->buf_bytes, GFP_KERNEL);
+	idata->buf = kmalloc(idata->buf_bytes, GFP_KERNEL);
 	if (!idata->buf) {
 		err = -ENOMEM;
 		goto idata_err;
@@ -659,8 +652,10 @@ static int mmc_blk_ioctl_multi_cmd(struct block_device *bdev,
 	}
 
 	md = mmc_blk_get(bdev->bd_disk);
-	if (!md)
+	if (!md) {
+		err = -EINVAL;
 		goto cmd_err;
+	}
 
 	card = md->queue.card;
 	if (IS_ERR(card)) {
@@ -2244,6 +2239,7 @@ static struct mmc_blk_data *mmc_blk_alloc_req(struct mmc_card *card,
 	md->disk->queue = md->queue.queue;
 	md->disk->driverfs_dev = parent;
 	set_disk_ro(md->disk, md->read_only || default_ro);
+	md->disk->flags = GENHD_FL_EXT_DEVT;
 	if (area_type & (MMC_BLK_DATA_AREA_RPMB | MMC_BLK_DATA_AREA_BOOT))
 		md->disk->flags |= GENHD_FL_NO_PART_SCAN;
 
diff --git a/drivers/mmc/core/bus.c b/drivers/mmc/core/bus.c
index 972ff844cf5a..4bc48f10452f 100644
--- a/drivers/mmc/core/bus.c
+++ b/drivers/mmc/core/bus.c
@@ -349,6 +349,8 @@ int mmc_add_card(struct mmc_card *card)
 
 	card->dev.of_node = mmc_of_find_child_device(card->host, 0);
 
+	device_enable_async_suspend(&card->dev);
+
 	ret = device_add(&card->dev);
 	if (ret)
 		return ret;
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index 5ae89e48fd85..f95d41ffc766 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -55,7 +55,6 @@
  */
 #define MMC_BKOPS_MAX_TIMEOUT	(4 * 60 * 1000) /* max time to wait in ms */
 
-static struct workqueue_struct *workqueue;
 static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 
 /*
@@ -66,21 +65,16 @@ static const unsigned freqs[] = { 400000, 300000, 200000, 100000 };
 bool use_spi_crc = 1;
 module_param(use_spi_crc, bool, 0);
 
-/*
- * Internal function. Schedule delayed work in the MMC work queue.
- */
 static int mmc_schedule_delayed_work(struct delayed_work *work,
 				     unsigned long delay)
 {
-	return queue_delayed_work(workqueue, work, delay);
-}
-
-/*
- * Internal function. Flush all scheduled work from the MMC work queue.
- */
-static void mmc_flush_scheduled_work(void)
-{
-	flush_workqueue(workqueue);
+	/*
+	 * We use the system_freezable_wq, because of two reasons.
+	 * First, it allows several works (not the same work item) to be
+	 * executed simultaneously. Second, the queue becomes frozen when
+	 * userspace becomes frozen during system PM.
+	 */
+	return queue_delayed_work(system_freezable_wq, work, delay);
 }
 
 #ifdef CONFIG_FAIL_MMC_REQUEST
@@ -1485,7 +1479,7 @@ int mmc_regulator_get_supply(struct mmc_host *mmc)
 	if (IS_ERR(mmc->supply.vmmc)) {
 		if (PTR_ERR(mmc->supply.vmmc) == -EPROBE_DEFER)
 			return -EPROBE_DEFER;
-		dev_info(dev, "No vmmc regulator found\n");
+		dev_dbg(dev, "No vmmc regulator found\n");
 	} else {
 		ret = mmc_regulator_get_ocrmask(mmc->supply.vmmc);
 		if (ret > 0)
@@ -1497,7 +1491,7 @@ int mmc_regulator_get_supply(struct mmc_host *mmc)
 	if (IS_ERR(mmc->supply.vqmmc)) {
 		if (PTR_ERR(mmc->supply.vqmmc) == -EPROBE_DEFER)
 			return -EPROBE_DEFER;
-		dev_info(dev, "No vqmmc regulator found\n");
+		dev_dbg(dev, "No vqmmc regulator found\n");
 	}
 
 	return 0;
@@ -2476,15 +2470,20 @@ static int mmc_rescan_try_freq(struct mmc_host *host, unsigned freq)
 	 * sdio_reset sends CMD52 to reset card.  Since we do not know
 	 * if the card is being re-initialized, just send it.  CMD52
 	 * should be ignored by SD/eMMC cards.
+	 * Skip it if we already know that we do not support SDIO commands
 	 */
-	sdio_reset(host);
+	if (!(host->caps2 & MMC_CAP2_NO_SDIO))
+		sdio_reset(host);
+
 	mmc_go_idle(host);
 
 	mmc_send_if_cond(host, host->ocr_avail);
 
 	/* Order's important: probe SDIO, then SD, then MMC */
-	if (!mmc_attach_sdio(host))
-		return 0;
+	if (!(host->caps2 & MMC_CAP2_NO_SDIO))
+		if (!mmc_attach_sdio(host))
+			return 0;
+
 	if (!mmc_attach_sd(host))
 		return 0;
 	if (!mmc_attach_mmc(host))
@@ -2498,9 +2497,6 @@ int _mmc_detect_card_removed(struct mmc_host *host)
 {
 	int ret;
 
-	if (host->caps & MMC_CAP_NONREMOVABLE)
-		return 0;
-
 	if (!host->card || mmc_card_removed(host->card))
 		return 1;
 
@@ -2536,6 +2532,9 @@ int mmc_detect_card_removed(struct mmc_host *host)
 	if (!card)
 		return 1;
 
+	if (host->caps & MMC_CAP_NONREMOVABLE)
+		return 0;
+
 	ret = mmc_card_removed(card);
 	/*
 	 * The card will be considered unchanged unless we have been asked to
@@ -2567,11 +2566,6 @@ void mmc_rescan(struct work_struct *work)
 		container_of(work, struct mmc_host, detect.work);
 	int i;
 
-	if (host->trigger_card_event && host->ops->card_event) {
-		host->ops->card_event(host);
-		host->trigger_card_event = false;
-	}
-
 	if (host->rescan_disable)
 		return;
 
@@ -2580,6 +2574,13 @@ void mmc_rescan(struct work_struct *work)
 		return;
 	host->rescan_entered = 1;
 
+	if (host->trigger_card_event && host->ops->card_event) {
+		mmc_claim_host(host);
+		host->ops->card_event(host);
+		mmc_release_host(host);
+		host->trigger_card_event = false;
+	}
+
 	mmc_bus_get(host);
 
 	/*
@@ -2611,15 +2612,14 @@ void mmc_rescan(struct work_struct *work)
 	 */
 	mmc_bus_put(host);
 
+	mmc_claim_host(host);
 	if (!(host->caps & MMC_CAP_NONREMOVABLE) && host->ops->get_cd &&
 			host->ops->get_cd(host) == 0) {
-		mmc_claim_host(host);
 		mmc_power_off(host);
 		mmc_release_host(host);
 		goto out;
 	}
 
-	mmc_claim_host(host);
 	for (i = 0; i < ARRAY_SIZE(freqs); i++) {
 		if (!mmc_rescan_try_freq(host, max(freqs[i], host->f_min)))
 			break;
@@ -2663,7 +2663,6 @@ void mmc_stop_host(struct mmc_host *host)
 
 	host->rescan_disable = 1;
 	cancel_delayed_work_sync(&host->detect);
-	mmc_flush_scheduled_work();
 
 	/* clear pm flags now and let card drivers set them as needed */
 	host->pm_flags = 0;
@@ -2759,14 +2758,13 @@ int mmc_flush_cache(struct mmc_card *card)
 }
 EXPORT_SYMBOL(mmc_flush_cache);
 
-#ifdef CONFIG_PM
-
+#ifdef CONFIG_PM_SLEEP
 /* Do the card removal on suspend if card is assumed removeable
  * Do that in pm notifier while userspace isn't yet frozen, so we will be able
    to sync the card.
 */
-int mmc_pm_notify(struct notifier_block *notify_block,
-					unsigned long mode, void *unused)
+static int mmc_pm_notify(struct notifier_block *notify_block,
+			unsigned long mode, void *unused)
 {
 	struct mmc_host *host = container_of(
 		notify_block, struct mmc_host, pm_notify);
@@ -2813,6 +2811,17 @@ int mmc_pm_notify(struct notifier_block *notify_block,
 
 	return 0;
 }
+
+void mmc_register_pm_notifier(struct mmc_host *host)
+{
+	host->pm_notify.notifier_call = mmc_pm_notify;
+	register_pm_notifier(&host->pm_notify);
+}
+
+void mmc_unregister_pm_notifier(struct mmc_host *host)
+{
+	unregister_pm_notifier(&host->pm_notify);
+}
 #endif
 
 /**
@@ -2836,13 +2845,9 @@ static int __init mmc_init(void)
 {
 	int ret;
 
-	workqueue = alloc_ordered_workqueue("kmmcd", 0);
-	if (!workqueue)
-		return -ENOMEM;
-
 	ret = mmc_register_bus();
 	if (ret)
-		goto destroy_workqueue;
+		return ret;
 
 	ret = mmc_register_host_class();
 	if (ret)
@@ -2858,9 +2863,6 @@ unregister_host_class:
 	mmc_unregister_host_class();
 unregister_bus:
 	mmc_unregister_bus();
-destroy_workqueue:
-	destroy_workqueue(workqueue);
-
 	return ret;
 }
 
@@ -2869,7 +2871,6 @@ static void __exit mmc_exit(void)
 	sdio_unregister_bus();
 	mmc_unregister_host_class();
 	mmc_unregister_bus();
-	destroy_workqueue(workqueue);
 }
 
 subsys_initcall(mmc_init);
diff --git a/drivers/mmc/core/core.h b/drivers/mmc/core/core.h
index 09241e56d628..0fa86a2afc26 100644
--- a/drivers/mmc/core/core.h
+++ b/drivers/mmc/core/core.h
@@ -90,5 +90,13 @@ int mmc_execute_tuning(struct mmc_card *card);
 int mmc_hs200_to_hs400(struct mmc_card *card);
 int mmc_hs400_to_hs200(struct mmc_card *card);
 
+#ifdef CONFIG_PM_SLEEP
+void mmc_register_pm_notifier(struct mmc_host *host);
+void mmc_unregister_pm_notifier(struct mmc_host *host);
+#else
+static inline void mmc_register_pm_notifier(struct mmc_host *host) { }
+static inline void mmc_unregister_pm_notifier(struct mmc_host *host) { }
+#endif
+
 #endif
 
diff --git a/drivers/mmc/core/debugfs.c b/drivers/mmc/core/debugfs.c
index 154aced0b91b..65cc0ac9b82d 100644
--- a/drivers/mmc/core/debugfs.c
+++ b/drivers/mmc/core/debugfs.c
@@ -170,7 +170,7 @@ static int mmc_ios_show(struct seq_file *s, void *data)
 		str = "invalid";
 		break;
 	}
-	seq_printf(s, "signal voltage:\t%u (%s)\n", ios->chip_select, str);
+	seq_printf(s, "signal voltage:\t%u (%s)\n", ios->signal_voltage, str);
 
 	switch (ios->drv_type) {
 	case MMC_SET_DRIVER_TYPE_A:
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index da950c44204d..0aecd5c00b86 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -21,7 +21,6 @@
 #include <linux/export.h>
 #include <linux/leds.h>
 #include <linux/slab.h>
-#include <linux/suspend.h>
 
 #include <linux/mmc/host.h>
 #include <linux/mmc/card.h>
@@ -275,7 +274,8 @@ int mmc_of_parse(struct mmc_host *host)
 		host->caps2 |= MMC_CAP2_FULL_PWR_CYCLE;
 	if (of_property_read_bool(np, "keep-power-in-suspend"))
 		host->pm_caps |= MMC_PM_KEEP_POWER;
-	if (of_property_read_bool(np, "enable-sdio-wakeup"))
+	if (of_property_read_bool(np, "wakeup-source") ||
+	    of_property_read_bool(np, "enable-sdio-wakeup")) /* legacy */
 		host->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
 	if (of_property_read_bool(np, "mmc-ddr-1_8v"))
 		host->caps |= MMC_CAP_1_8V_DDR;
@@ -348,9 +348,6 @@ struct mmc_host *mmc_alloc_host(int extra, struct device *dev)
 	spin_lock_init(&host->lock);
 	init_waitqueue_head(&host->wq);
 	INIT_DELAYED_WORK(&host->detect, mmc_rescan);
-#ifdef CONFIG_PM
-	host->pm_notify.notifier_call = mmc_pm_notify;
-#endif
 	setup_timer(&host->retune_timer, mmc_retune_timer, (unsigned long)host);
 
 	/*
@@ -395,7 +392,7 @@ int mmc_add_host(struct mmc_host *host)
 #endif
 
 	mmc_start_host(host);
-	register_pm_notifier(&host->pm_notify);
+	mmc_register_pm_notifier(host);
 
 	return 0;
 }
@@ -412,7 +409,7 @@ EXPORT_SYMBOL(mmc_add_host);
  */
 void mmc_remove_host(struct mmc_host *host)
 {
-	unregister_pm_notifier(&host->pm_notify);
+	mmc_unregister_pm_notifier(host);
 	mmc_stop_host(host);
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 3a9a79ec4343..bf49e44571f2 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -1076,8 +1076,7 @@ static int mmc_select_hs400(struct mmc_card *card)
 	mmc_set_clock(host, max_dtr);
 
 	/* Switch card to HS mode */
-	val = EXT_CSD_TIMING_HS |
-	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
+	val = EXT_CSD_TIMING_HS;
 	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 			   EXT_CSD_HS_TIMING, val,
 			   card->ext_csd.generic_cmd6_time,
@@ -1160,8 +1159,7 @@ int mmc_hs400_to_hs200(struct mmc_card *card)
 	mmc_set_clock(host, max_dtr);
 
 	/* Switch HS400 to HS DDR */
-	val = EXT_CSD_TIMING_HS |
-	      card->drive_strength << EXT_CSD_DRV_STR_SHIFT;
+	val = EXT_CSD_TIMING_HS;
 	err = __mmc_switch(card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_HS_TIMING,
 			   val, card->ext_csd.generic_cmd6_time,
 			   true, send_status, true);
@@ -1907,16 +1905,8 @@ static int mmc_shutdown(struct mmc_host *host)
  */
 static int mmc_resume(struct mmc_host *host)
 {
-	int err = 0;
-
-	if (!(host->caps & MMC_CAP_RUNTIME_RESUME)) {
-		err = _mmc_resume(host);
-		pm_runtime_set_active(&host->card->dev);
-		pm_runtime_mark_last_busy(&host->card->dev);
-	}
 	pm_runtime_enable(&host->card->dev);
-
-	return err;
+	return 0;
 }
 
 /*
@@ -1944,12 +1934,9 @@ static int mmc_runtime_resume(struct mmc_host *host)
 {
 	int err;
 
-	if (!(host->caps & (MMC_CAP_AGGRESSIVE_PM | MMC_CAP_RUNTIME_RESUME)))
-		return 0;
-
 	err = _mmc_resume(host);
-	if (err)
-		pr_err("%s: error %d doing aggressive resume\n",
+	if (err && err != -ENOMEDIUM)
+		pr_err("%s: error %d doing runtime resume\n",
 			mmc_hostname(host), err);
 
 	return 0;
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index 1f444269ebbe..2c90635c89af 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -489,6 +489,7 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 	unsigned long timeout;
 	u32 status = 0;
 	bool use_r1b_resp = use_busy_signal;
+	bool expired = false;
 
 	mmc_retune_hold(host);
 
@@ -545,6 +546,12 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 	timeout = jiffies + msecs_to_jiffies(timeout_ms);
 	do {
 		if (send_status) {
+			/*
+			 * Due to the possibility of being preempted after
+			 * sending the status command, check the expiration
+			 * time first.
+			 */
+			expired = time_after(jiffies, timeout);
 			err = __mmc_send_status(card, &status, ignore_crc);
 			if (err)
 				goto out;
@@ -565,7 +572,7 @@ int __mmc_switch(struct mmc_card *card, u8 set, u8 index, u8 value,
 		}
 
 		/* Timeout if the device never leaves the program state. */
-		if (time_after(jiffies, timeout)) {
+		if (expired && R1_CURRENT_STATE(status) == R1_STATE_PRG) {
 			pr_err("%s: Card stuck in programming state! %s\n",
 				mmc_hostname(host), __func__);
 			err = -ETIMEDOUT;
diff --git a/drivers/mmc/core/pwrseq.h b/drivers/mmc/core/pwrseq.h
index 096da48c6a7e..133de0426687 100644
--- a/drivers/mmc/core/pwrseq.h
+++ b/drivers/mmc/core/pwrseq.h
@@ -16,7 +16,7 @@ struct mmc_pwrseq_ops {
 };
 
 struct mmc_pwrseq {
-	struct mmc_pwrseq_ops *ops;
+	const struct mmc_pwrseq_ops *ops;
 };
 
 #ifdef CONFIG_OF
diff --git a/drivers/mmc/core/pwrseq_emmc.c b/drivers/mmc/core/pwrseq_emmc.c
index ad4f94ec7e8d..4a82bc77fe49 100644
--- a/drivers/mmc/core/pwrseq_emmc.c
+++ b/drivers/mmc/core/pwrseq_emmc.c
@@ -51,7 +51,7 @@ static void mmc_pwrseq_emmc_free(struct mmc_host *host)
 	kfree(pwrseq);
 }
 
-static struct mmc_pwrseq_ops mmc_pwrseq_emmc_ops = {
+static const struct mmc_pwrseq_ops mmc_pwrseq_emmc_ops = {
 	.post_power_on = mmc_pwrseq_emmc_reset,
 	.free = mmc_pwrseq_emmc_free,
 };
diff --git a/drivers/mmc/core/pwrseq_simple.c b/drivers/mmc/core/pwrseq_simple.c
index d10538bb5e07..aba786daebca 100644
--- a/drivers/mmc/core/pwrseq_simple.c
+++ b/drivers/mmc/core/pwrseq_simple.c
@@ -29,15 +29,18 @@ struct mmc_pwrseq_simple {
 static void mmc_pwrseq_simple_set_gpios_value(struct mmc_pwrseq_simple *pwrseq,
 					      int value)
 {
-	int i;
 	struct gpio_descs *reset_gpios = pwrseq->reset_gpios;
-	int values[reset_gpios->ndescs];
 
-	for (i = 0; i < reset_gpios->ndescs; i++)
-		values[i] = value;
+	if (!IS_ERR(reset_gpios)) {
+		int i;
+		int values[reset_gpios->ndescs];
 
-	gpiod_set_array_value_cansleep(reset_gpios->ndescs, reset_gpios->desc,
-				       values);
+		for (i = 0; i < reset_gpios->ndescs; i++)
+			values[i] = value;
+
+		gpiod_set_array_value_cansleep(
+			reset_gpios->ndescs, reset_gpios->desc, values);
+	}
 }
 
 static void mmc_pwrseq_simple_pre_power_on(struct mmc_host *host)
@@ -79,7 +82,8 @@ static void mmc_pwrseq_simple_free(struct mmc_host *host)
 	struct mmc_pwrseq_simple *pwrseq = container_of(host->pwrseq,
 					struct mmc_pwrseq_simple, pwrseq);
 
-	gpiod_put_array(pwrseq->reset_gpios);
+	if (!IS_ERR(pwrseq->reset_gpios))
+		gpiod_put_array(pwrseq->reset_gpios);
 
 	if (!IS_ERR(pwrseq->ext_clk))
 		clk_put(pwrseq->ext_clk);
@@ -87,7 +91,7 @@ static void mmc_pwrseq_simple_free(struct mmc_host *host)
 	kfree(pwrseq);
 }
 
-static struct mmc_pwrseq_ops mmc_pwrseq_simple_ops = {
+static const struct mmc_pwrseq_ops mmc_pwrseq_simple_ops = {
 	.pre_power_on = mmc_pwrseq_simple_pre_power_on,
 	.post_power_on = mmc_pwrseq_simple_post_power_on,
 	.power_off = mmc_pwrseq_simple_power_off,
@@ -112,7 +116,9 @@ struct mmc_pwrseq *mmc_pwrseq_simple_alloc(struct mmc_host *host,
 	}
 
 	pwrseq->reset_gpios = gpiod_get_array(dev, "reset", GPIOD_OUT_HIGH);
-	if (IS_ERR(pwrseq->reset_gpios)) {
+	if (IS_ERR(pwrseq->reset_gpios) &&
+	    PTR_ERR(pwrseq->reset_gpios) != -ENOENT &&
+	    PTR_ERR(pwrseq->reset_gpios) != -ENOSYS) {
 		ret = PTR_ERR(pwrseq->reset_gpios);
 		goto clk_put;
 	}
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 141eaa923e18..bb39a29b2db6 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -329,6 +329,7 @@ static int mmc_read_switch(struct mmc_card *card)
 		card->sw_caps.sd3_bus_mode = status[13];
 		/* Driver Strengths supported by the card */
 		card->sw_caps.sd3_drv_type = status[9];
+		card->sw_caps.sd3_curr_limit = status[7] | status[6] << 8;
 	}
 
 out:
@@ -545,14 +546,25 @@ static int sd_set_current_limit(struct mmc_card *card, u8 *status)
 	 * when we set current limit to 200ma, the card will draw 200ma, and
 	 * when we set current limit to 400/600/800ma, the card will draw its
 	 * maximum 300ma from the host.
+	 *
+	 * The above is incorrect: if we try to set a current limit that is
+	 * not supported by the card, the card can rightfully error out the
+	 * attempt, and remain at the default current limit.  This results
+	 * in a 300mA card being limited to 200mA even though the host
+	 * supports 800mA. Failures seen with SanDisk 8GB UHS cards with
+	 * an iMX6 host. --rmk
 	 */
-	if (max_current >= 800)
+	if (max_current >= 800 &&
+	    card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_800)
 		current_limit = SD_SET_CURRENT_LIMIT_800;
-	else if (max_current >= 600)
+	else if (max_current >= 600 &&
+		 card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_600)
 		current_limit = SD_SET_CURRENT_LIMIT_600;
-	else if (max_current >= 400)
+	else if (max_current >= 400 &&
+		 card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_400)
 		current_limit = SD_SET_CURRENT_LIMIT_400;
-	else if (max_current >= 200)
+	else if (max_current >= 200 &&
+		 card->sw_caps.sd3_curr_limit & SD_MAX_CURRENT_200)
 		current_limit = SD_SET_CURRENT_LIMIT_200;
 
 	if (current_limit != SD_SET_CURRENT_NO_CHANGE) {
@@ -626,9 +638,9 @@ static int mmc_sd_init_uhs_card(struct mmc_card *card)
 	 * SDR104 mode SD-cards. Note that tuning is mandatory for SDR104.
 	 */
 	if (!mmc_host_is_spi(card->host) &&
-		(card->sd_bus_speed == UHS_SDR50_BUS_SPEED ||
-		 card->sd_bus_speed == UHS_DDR50_BUS_SPEED ||
-		 card->sd_bus_speed == UHS_SDR104_BUS_SPEED)) {
+		(card->host->ios.timing == MMC_TIMING_UHS_SDR50 ||
+		 card->host->ios.timing == MMC_TIMING_UHS_DDR50 ||
+		 card->host->ios.timing == MMC_TIMING_UHS_SDR104)) {
 		err = mmc_execute_tuning(card);
 
 		/*
@@ -638,7 +650,7 @@ static int mmc_sd_init_uhs_card(struct mmc_card *card)
 		 * difference between v3.00 and 3.01 spec means that CMD19
 		 * tuning is also available for DDR50 mode.
 		 */
-		if (err && card->sd_bus_speed == UHS_DDR50_BUS_SPEED) {
+		if (err && card->host->ios.timing == MMC_TIMING_UHS_DDR50) {
 			pr_warn("%s: ddr50 tuning failed\n",
 				mmc_hostname(card->host));
 			err = 0;
@@ -1128,16 +1140,8 @@ out:
  */
 static int mmc_sd_resume(struct mmc_host *host)
 {
-	int err = 0;
-
-	if (!(host->caps & MMC_CAP_RUNTIME_RESUME)) {
-		err = _mmc_sd_resume(host);
-		pm_runtime_set_active(&host->card->dev);
-		pm_runtime_mark_last_busy(&host->card->dev);
-	}
 	pm_runtime_enable(&host->card->dev);
-
-	return err;
+	return 0;
 }
 
 /*
@@ -1165,12 +1169,9 @@ static int mmc_sd_runtime_resume(struct mmc_host *host)
 {
 	int err;
 
-	if (!(host->caps & (MMC_CAP_AGGRESSIVE_PM | MMC_CAP_RUNTIME_RESUME)))
-		return 0;
-
 	err = _mmc_sd_resume(host);
-	if (err)
-		pr_err("%s: error %d doing aggressive resume\n",
+	if (err && err != -ENOMEDIUM)
+		pr_err("%s: error %d doing runtime resume\n",
 			mmc_hostname(host), err);
 
 	return 0;
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index 16d838e6d623..467b3cf80c44 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -535,8 +535,8 @@ static int mmc_sdio_init_uhs_card(struct mmc_card *card)
 	 * SDR104 mode SD-cards. Note that tuning is mandatory for SDR104.
 	 */
 	if (!mmc_host_is_spi(card->host) &&
-	    ((card->sw_caps.sd3_bus_mode & SD_MODE_UHS_SDR50) ||
-	     (card->sw_caps.sd3_bus_mode & SD_MODE_UHS_SDR104)))
+	    ((card->host->ios.timing == MMC_TIMING_UHS_SDR50) ||
+	      (card->host->ios.timing == MMC_TIMING_UHS_SDR104)))
 		err = mmc_execute_tuning(card);
 out:
 	return err;
@@ -630,7 +630,7 @@ try_again:
 	 */
 	if (!powered_resume && (rocr & ocr & R4_18V_PRESENT)) {
 		err = mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180,
-					ocr);
+					ocr_card);
 		if (err == -EAGAIN) {
 			sdio_reset(host);
 			mmc_go_idle(host);
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 7e327a6dd53d..86f5b3223aae 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -322,6 +322,7 @@ int sdio_add_func(struct sdio_func *func)
 
 	sdio_set_of_node(func);
 	sdio_acpi_set_handle(func);
+	device_enable_async_suspend(&func->dev);
 	ret = device_add(&func->dev);
 	if (ret == 0)
 		sdio_func_set_present(func);
diff --git a/drivers/mmc/core/sdio_cis.c b/drivers/mmc/core/sdio_cis.c
index 8e94e555b788..6f6fc527a263 100644
--- a/drivers/mmc/core/sdio_cis.c
+++ b/drivers/mmc/core/sdio_cis.c
@@ -223,6 +223,7 @@ static const struct cis_tpl cis_tpl_list[] = {
 	{	0x20,	4,	cistpl_manfid		},
 	{	0x21,	2,	/* cistpl_funcid */	},
 	{	0x22,	0,	cistpl_funce		},
+	{	0x91,	2,	/* cistpl_sdio_std */	},
 };
 
 static int sdio_read_cis(struct mmc_card *card, struct sdio_func *func)
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 1dee533634c9..1526b8a10b09 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -455,6 +455,7 @@ config MMC_TIFM_SD
 config MMC_MVSDIO
 	tristate "Marvell MMC/SD/SDIO host driver"
 	depends on PLAT_ORION
+	depends on OF
 	---help---
 	  This selects the Marvell SDIO host driver.
 	  SDIO may currently be found on the Kirkwood 88F6281 and 88F6192
diff --git a/drivers/mmc/host/atmel-mci-regs.h b/drivers/mmc/host/atmel-mci-regs.h
deleted file mode 100644
index 0aa44e679df4..000000000000
--- a/drivers/mmc/host/atmel-mci-regs.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * Atmel MultiMedia Card Interface driver
- *
- * Copyright (C) 2004-2006 Atmel Corporation
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-/*
- * Superset of MCI IP registers integrated in Atmel AVR32 and AT91 Processors
- * Registers and bitfields marked with [2] are only available in MCI2
- */
-
-#ifndef __DRIVERS_MMC_ATMEL_MCI_H__
-#define __DRIVERS_MMC_ATMEL_MCI_H__
-
-/* MCI Register Definitions */
-#define ATMCI_CR			0x0000	/* Control */
-# define ATMCI_CR_MCIEN			(  1 <<  0)	/* MCI Enable */
-# define ATMCI_CR_MCIDIS		(  1 <<  1)	/* MCI Disable */
-# define ATMCI_CR_PWSEN			(  1 <<  2)	/* Power Save Enable */
-# define ATMCI_CR_PWSDIS		(  1 <<  3)	/* Power Save Disable */
-# define ATMCI_CR_SWRST			(  1 <<  7)	/* Software Reset */
-#define ATMCI_MR			0x0004	/* Mode */
-# define ATMCI_MR_CLKDIV(x)		((x) <<  0)	/* Clock Divider */
-# define ATMCI_MR_PWSDIV(x)		((x) <<  8)	/* Power Saving Divider */
-# define ATMCI_MR_RDPROOF		(  1 << 11)	/* Read Proof */
-# define ATMCI_MR_WRPROOF		(  1 << 12)	/* Write Proof */
-# define ATMCI_MR_PDCFBYTE		(  1 << 13)	/* Force Byte Transfer */
-# define ATMCI_MR_PDCPADV		(  1 << 14)	/* Padding Value */
-# define ATMCI_MR_PDCMODE		(  1 << 15)	/* PDC-oriented Mode */
-# define ATMCI_MR_CLKODD(x)		((x) << 16)	/* LSB of Clock Divider */
-#define ATMCI_DTOR			0x0008	/* Data Timeout */
-# define ATMCI_DTOCYC(x)		((x) <<  0)	/* Data Timeout Cycles */
-# define ATMCI_DTOMUL(x)		((x) <<  4)	/* Data Timeout Multiplier */
-#define ATMCI_SDCR			0x000c	/* SD Card / SDIO */
-# define ATMCI_SDCSEL_SLOT_A		(  0 <<  0)	/* Select SD slot A */
-# define ATMCI_SDCSEL_SLOT_B		(  1 <<  0)	/* Select SD slot A */
-# define ATMCI_SDCSEL_MASK		(  3 <<  0)
-# define ATMCI_SDCBUS_1BIT		(  0 <<  6)	/* 1-bit data bus */
-# define ATMCI_SDCBUS_4BIT		(  2 <<  6)	/* 4-bit data bus */
-# define ATMCI_SDCBUS_8BIT		(  3 <<  6)	/* 8-bit data bus[2] */
-# define ATMCI_SDCBUS_MASK		(  3 <<  6)
-#define ATMCI_ARGR			0x0010	/* Command Argument */
-#define ATMCI_CMDR			0x0014	/* Command */
-# define ATMCI_CMDR_CMDNB(x)		((x) <<  0)	/* Command Opcode */
-# define ATMCI_CMDR_RSPTYP_NONE		(  0 <<  6)	/* No response */
-# define ATMCI_CMDR_RSPTYP_48BIT	(  1 <<  6)	/* 48-bit response */
-# define ATMCI_CMDR_RSPTYP_136BIT	(  2 <<  6)	/* 136-bit response */
-# define ATMCI_CMDR_SPCMD_INIT		(  1 <<  8)	/* Initialization command */
-# define ATMCI_CMDR_SPCMD_SYNC		(  2 <<  8)	/* Synchronized command */
-# define ATMCI_CMDR_SPCMD_INT		(  4 <<  8)	/* Interrupt command */
-# define ATMCI_CMDR_SPCMD_INTRESP	(  5 <<  8)	/* Interrupt response */
-# define ATMCI_CMDR_OPDCMD		(  1 << 11)	/* Open Drain */
-# define ATMCI_CMDR_MAXLAT_5CYC		(  0 << 12)	/* Max latency 5 cycles */
-# define ATMCI_CMDR_MAXLAT_64CYC	(  1 << 12)	/* Max latency 64 cycles */
-# define ATMCI_CMDR_START_XFER		(  1 << 16)	/* Start data transfer */
-# define ATMCI_CMDR_STOP_XFER		(  2 << 16)	/* Stop data transfer */
-# define ATMCI_CMDR_TRDIR_WRITE		(  0 << 18)	/* Write data */
-# define ATMCI_CMDR_TRDIR_READ		(  1 << 18)	/* Read data */
-# define ATMCI_CMDR_BLOCK		(  0 << 19)	/* Single-block transfer */
-# define ATMCI_CMDR_MULTI_BLOCK		(  1 << 19)	/* Multi-block transfer */
-# define ATMCI_CMDR_STREAM		(  2 << 19)	/* MMC Stream transfer */
-# define ATMCI_CMDR_SDIO_BYTE		(  4 << 19)	/* SDIO Byte transfer */
-# define ATMCI_CMDR_SDIO_BLOCK		(  5 << 19)	/* SDIO Block transfer */
-# define ATMCI_CMDR_SDIO_SUSPEND	(  1 << 24)	/* SDIO Suspend Command */
-# define ATMCI_CMDR_SDIO_RESUME		(  2 << 24)	/* SDIO Resume Command */
-#define ATMCI_BLKR			0x0018	/* Block */
-# define ATMCI_BCNT(x)			((x) <<  0)	/* Data Block Count */
-# define ATMCI_BLKLEN(x)		((x) << 16)	/* Data Block Length */
-#define ATMCI_CSTOR			0x001c	/* Completion Signal Timeout[2] */
-# define ATMCI_CSTOCYC(x)		((x) <<  0)	/* CST cycles */
-# define ATMCI_CSTOMUL(x)		((x) <<  4)	/* CST multiplier */
-#define ATMCI_RSPR			0x0020	/* Response 0 */
-#define ATMCI_RSPR1			0x0024	/* Response 1 */
-#define ATMCI_RSPR2			0x0028	/* Response 2 */
-#define ATMCI_RSPR3			0x002c	/* Response 3 */
-#define ATMCI_RDR			0x0030	/* Receive Data */
-#define ATMCI_TDR			0x0034	/* Transmit Data */
-#define ATMCI_SR			0x0040	/* Status */
-#define ATMCI_IER			0x0044	/* Interrupt Enable */
-#define ATMCI_IDR			0x0048	/* Interrupt Disable */
-#define ATMCI_IMR			0x004c	/* Interrupt Mask */
-# define ATMCI_CMDRDY			(  1 <<   0)	/* Command Ready */
-# define ATMCI_RXRDY			(  1 <<   1)	/* Receiver Ready */
-# define ATMCI_TXRDY			(  1 <<   2)	/* Transmitter Ready */
-# define ATMCI_BLKE			(  1 <<   3)	/* Data Block Ended */
-# define ATMCI_DTIP			(  1 <<   4)	/* Data Transfer In Progress */
-# define ATMCI_NOTBUSY			(  1 <<   5)	/* Data Not Busy */
-# define ATMCI_ENDRX			(  1 <<   6)    /* End of RX Buffer */
-# define ATMCI_ENDTX			(  1 <<   7)    /* End of TX Buffer */
-# define ATMCI_SDIOIRQA			(  1 <<   8)	/* SDIO IRQ in slot A */
-# define ATMCI_SDIOIRQB			(  1 <<   9)	/* SDIO IRQ in slot B */
-# define ATMCI_SDIOWAIT			(  1 <<  12)    /* SDIO Read Wait Operation Status */
-# define ATMCI_CSRCV			(  1 <<  13)    /* CE-ATA Completion Signal Received */
-# define ATMCI_RXBUFF			(  1 <<  14)    /* RX Buffer Full */
-# define ATMCI_TXBUFE			(  1 <<  15)    /* TX Buffer Empty */
-# define ATMCI_RINDE			(  1 <<  16)	/* Response Index Error */
-# define ATMCI_RDIRE			(  1 <<  17)	/* Response Direction Error */
-# define ATMCI_RCRCE			(  1 <<  18)	/* Response CRC Error */
-# define ATMCI_RENDE			(  1 <<  19)	/* Response End Bit Error */
-# define ATMCI_RTOE			(  1 <<  20)	/* Response Time-Out Error */
-# define ATMCI_DCRCE			(  1 <<  21)	/* Data CRC Error */
-# define ATMCI_DTOE			(  1 <<  22)	/* Data Time-Out Error */
-# define ATMCI_CSTOE			(  1 <<  23)    /* Completion Signal Time-out Error */
-# define ATMCI_BLKOVRE			(  1 <<  24)    /* DMA Block Overrun Error */
-# define ATMCI_DMADONE			(  1 <<  25)    /* DMA Transfer Done */
-# define ATMCI_FIFOEMPTY		(  1 <<  26)    /* FIFO Empty Flag */
-# define ATMCI_XFRDONE			(  1 <<  27)    /* Transfer Done Flag */
-# define ATMCI_ACKRCV			(  1 <<  28)    /* Boot Operation Acknowledge Received */
-# define ATMCI_ACKRCVE			(  1 <<  29)    /* Boot Operation Acknowledge Error */
-# define ATMCI_OVRE			(  1 <<  30)	/* RX Overrun Error */
-# define ATMCI_UNRE			(  1 <<  31)	/* TX Underrun Error */
-#define ATMCI_DMA			0x0050	/* DMA Configuration[2] */
-# define ATMCI_DMA_OFFSET(x)		((x) <<  0)	/* DMA Write Buffer Offset */
-# define ATMCI_DMA_CHKSIZE(x)		((x) <<  4)	/* DMA Channel Read and Write Chunk Size */
-# define ATMCI_DMAEN			(  1 <<  8)	/* DMA Hardware Handshaking Enable */
-#define ATMCI_CFG			0x0054	/* Configuration[2] */
-# define ATMCI_CFG_FIFOMODE_1DATA	(  1 <<  0)	/* MCI Internal FIFO control mode */
-# define ATMCI_CFG_FERRCTRL_COR		(  1 <<  4)	/* Flow Error flag reset control mode */
-# define ATMCI_CFG_HSMODE		(  1 <<  8)	/* High Speed Mode */
-# define ATMCI_CFG_LSYNC		(  1 << 12)	/* Synchronize on the last block */
-#define ATMCI_WPMR			0x00e4	/* Write Protection Mode[2] */
-# define ATMCI_WP_EN			(  1 <<  0)	/* WP Enable */
-# define ATMCI_WP_KEY			(0x4d4349 << 8)	/* WP Key */
-#define ATMCI_WPSR			0x00e8	/* Write Protection Status[2] */
-# define ATMCI_GET_WP_VS(x)		((x) & 0x0f)
-# define ATMCI_GET_WP_VSRC(x)		(((x) >> 8) & 0xffff)
-#define ATMCI_VERSION			0x00FC  /* Version */
-#define ATMCI_FIFO_APERTURE		0x0200	/* FIFO Aperture[2] */
-
-/* This is not including the FIFO Aperture on MCI2 */
-#define ATMCI_REGS_SIZE		0x100
-
-/* Register access macros */
-#ifdef CONFIG_AVR32
-#define atmci_readl(port, reg)			\
-	__raw_readl((port)->regs + reg)
-#define atmci_writel(port, reg, value)			\
-	__raw_writel((value), (port)->regs + reg)
-#else
-#define atmci_readl(port, reg)			\
-	readl_relaxed((port)->regs + reg)
-#define atmci_writel(port, reg, value)			\
-	writel_relaxed((value), (port)->regs + reg)
-#endif
-
-/* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
-#ifdef CONFIG_AVR32
-#	define ATMCI_PDC_CONNECTED	0
-#else
-#	define ATMCI_PDC_CONNECTED	1
-#endif
-
-/*
- * Fix sconfig's burst size according to atmel MCI. We need to convert them as:
- * 1 -> 0, 4 -> 1, 8 -> 2, 16 -> 3.
- *
- * This can be done by finding most significant bit set.
- */
-static inline unsigned int atmci_convert_chksize(unsigned int maxburst)
-{
-	if (maxburst > 1)
-		return fls(maxburst) - 2;
-	else
-		return 0;
-}
-
-#endif /* __DRIVERS_MMC_ATMEL_MCI_H__ */
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index bf62e429f7fc..851ccd9ac868 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -29,7 +29,6 @@
 #include <linux/slab.h>
 #include <linux/stat.h>
 #include <linux/types.h>
-#include <linux/platform_data/mmc-atmel-mci.h>
 
 #include <linux/mmc/host.h>
 #include <linux/mmc/sdio.h>
@@ -44,7 +43,141 @@
 #include <asm/io.h>
 #include <asm/unaligned.h>
 
-#include "atmel-mci-regs.h"
+/*
+ * Superset of MCI IP registers integrated in Atmel AVR32 and AT91 Processors
+ * Registers and bitfields marked with [2] are only available in MCI2
+ */
+
+/* MCI Register Definitions */
+#define	ATMCI_CR			0x0000	/* Control */
+#define		ATMCI_CR_MCIEN			BIT(0)		/* MCI Enable */
+#define		ATMCI_CR_MCIDIS			BIT(1)		/* MCI Disable */
+#define		ATMCI_CR_PWSEN			BIT(2)		/* Power Save Enable */
+#define		ATMCI_CR_PWSDIS			BIT(3)		/* Power Save Disable */
+#define		ATMCI_CR_SWRST			BIT(7)		/* Software Reset */
+#define	ATMCI_MR			0x0004	/* Mode */
+#define		ATMCI_MR_CLKDIV(x)		((x) <<  0)	/* Clock Divider */
+#define		ATMCI_MR_PWSDIV(x)		((x) <<  8)	/* Power Saving Divider */
+#define		ATMCI_MR_RDPROOF		BIT(11)		/* Read Proof */
+#define		ATMCI_MR_WRPROOF		BIT(12)		/* Write Proof */
+#define		ATMCI_MR_PDCFBYTE		BIT(13)		/* Force Byte Transfer */
+#define		ATMCI_MR_PDCPADV		BIT(14)		/* Padding Value */
+#define		ATMCI_MR_PDCMODE		BIT(15)		/* PDC-oriented Mode */
+#define		ATMCI_MR_CLKODD(x)		((x) << 16)	/* LSB of Clock Divider */
+#define	ATMCI_DTOR			0x0008	/* Data Timeout */
+#define		ATMCI_DTOCYC(x)			((x) <<  0)	/* Data Timeout Cycles */
+#define		ATMCI_DTOMUL(x)			((x) <<  4)	/* Data Timeout Multiplier */
+#define	ATMCI_SDCR			0x000c	/* SD Card / SDIO */
+#define		ATMCI_SDCSEL_SLOT_A		(0 <<  0)	/* Select SD slot A */
+#define		ATMCI_SDCSEL_SLOT_B		(1 <<  0)	/* Select SD slot A */
+#define		ATMCI_SDCSEL_MASK		(3 <<  0)
+#define		ATMCI_SDCBUS_1BIT		(0 <<  6)	/* 1-bit data bus */
+#define		ATMCI_SDCBUS_4BIT		(2 <<  6)	/* 4-bit data bus */
+#define		ATMCI_SDCBUS_8BIT		(3 <<  6)	/* 8-bit data bus[2] */
+#define		ATMCI_SDCBUS_MASK		(3 <<  6)
+#define	ATMCI_ARGR			0x0010	/* Command Argument */
+#define	ATMCI_CMDR			0x0014	/* Command */
+#define		ATMCI_CMDR_CMDNB(x)		((x) <<  0)	/* Command Opcode */
+#define		ATMCI_CMDR_RSPTYP_NONE		(0 <<  6)	/* No response */
+#define		ATMCI_CMDR_RSPTYP_48BIT		(1 <<  6)	/* 48-bit response */
+#define		ATMCI_CMDR_RSPTYP_136BIT	(2 <<  6)	/* 136-bit response */
+#define		ATMCI_CMDR_SPCMD_INIT		(1 <<  8)	/* Initialization command */
+#define		ATMCI_CMDR_SPCMD_SYNC		(2 <<  8)	/* Synchronized command */
+#define		ATMCI_CMDR_SPCMD_INT		(4 <<  8)	/* Interrupt command */
+#define		ATMCI_CMDR_SPCMD_INTRESP	(5 <<  8)	/* Interrupt response */
+#define		ATMCI_CMDR_OPDCMD		(1 << 11)	/* Open Drain */
+#define		ATMCI_CMDR_MAXLAT_5CYC		(0 << 12)	/* Max latency 5 cycles */
+#define		ATMCI_CMDR_MAXLAT_64CYC		(1 << 12)	/* Max latency 64 cycles */
+#define		ATMCI_CMDR_START_XFER		(1 << 16)	/* Start data transfer */
+#define		ATMCI_CMDR_STOP_XFER		(2 << 16)	/* Stop data transfer */
+#define		ATMCI_CMDR_TRDIR_WRITE		(0 << 18)	/* Write data */
+#define		ATMCI_CMDR_TRDIR_READ		(1 << 18)	/* Read data */
+#define		ATMCI_CMDR_BLOCK		(0 << 19)	/* Single-block transfer */
+#define		ATMCI_CMDR_MULTI_BLOCK		(1 << 19)	/* Multi-block transfer */
+#define		ATMCI_CMDR_STREAM		(2 << 19)	/* MMC Stream transfer */
+#define		ATMCI_CMDR_SDIO_BYTE		(4 << 19)	/* SDIO Byte transfer */
+#define		ATMCI_CMDR_SDIO_BLOCK		(5 << 19)	/* SDIO Block transfer */
+#define		ATMCI_CMDR_SDIO_SUSPEND		(1 << 24)	/* SDIO Suspend Command */
+#define		ATMCI_CMDR_SDIO_RESUME		(2 << 24)	/* SDIO Resume Command */
+#define	ATMCI_BLKR			0x0018	/* Block */
+#define		ATMCI_BCNT(x)			((x) <<  0)	/* Data Block Count */
+#define		ATMCI_BLKLEN(x)			((x) << 16)	/* Data Block Length */
+#define	ATMCI_CSTOR			0x001c	/* Completion Signal Timeout[2] */
+#define		ATMCI_CSTOCYC(x)		((x) <<  0)	/* CST cycles */
+#define		ATMCI_CSTOMUL(x)		((x) <<  4)	/* CST multiplier */
+#define	ATMCI_RSPR			0x0020	/* Response 0 */
+#define	ATMCI_RSPR1			0x0024	/* Response 1 */
+#define	ATMCI_RSPR2			0x0028	/* Response 2 */
+#define	ATMCI_RSPR3			0x002c	/* Response 3 */
+#define	ATMCI_RDR			0x0030	/* Receive Data */
+#define	ATMCI_TDR			0x0034	/* Transmit Data */
+#define	ATMCI_SR			0x0040	/* Status */
+#define	ATMCI_IER			0x0044	/* Interrupt Enable */
+#define	ATMCI_IDR			0x0048	/* Interrupt Disable */
+#define	ATMCI_IMR			0x004c	/* Interrupt Mask */
+#define		ATMCI_CMDRDY			BIT(0)		/* Command Ready */
+#define		ATMCI_RXRDY			BIT(1)		/* Receiver Ready */
+#define		ATMCI_TXRDY			BIT(2)		/* Transmitter Ready */
+#define		ATMCI_BLKE			BIT(3)		/* Data Block Ended */
+#define		ATMCI_DTIP			BIT(4)		/* Data Transfer In Progress */
+#define		ATMCI_NOTBUSY			BIT(5)		/* Data Not Busy */
+#define		ATMCI_ENDRX			BIT(6)		/* End of RX Buffer */
+#define		ATMCI_ENDTX			BIT(7)		/* End of TX Buffer */
+#define		ATMCI_SDIOIRQA			BIT(8)		/* SDIO IRQ in slot A */
+#define		ATMCI_SDIOIRQB			BIT(9)		/* SDIO IRQ in slot B */
+#define		ATMCI_SDIOWAIT			BIT(12)		/* SDIO Read Wait Operation Status */
+#define		ATMCI_CSRCV			BIT(13)		/* CE-ATA Completion Signal Received */
+#define		ATMCI_RXBUFF			BIT(14)		/* RX Buffer Full */
+#define		ATMCI_TXBUFE			BIT(15)		/* TX Buffer Empty */
+#define		ATMCI_RINDE			BIT(16)		/* Response Index Error */
+#define		ATMCI_RDIRE			BIT(17)		/* Response Direction Error */
+#define		ATMCI_RCRCE			BIT(18)		/* Response CRC Error */
+#define		ATMCI_RENDE			BIT(19)		/* Response End Bit Error */
+#define		ATMCI_RTOE			BIT(20)		/* Response Time-Out Error */
+#define		ATMCI_DCRCE			BIT(21)		/* Data CRC Error */
+#define		ATMCI_DTOE			BIT(22)		/* Data Time-Out Error */
+#define		ATMCI_CSTOE			BIT(23)		/* Completion Signal Time-out Error */
+#define		ATMCI_BLKOVRE			BIT(24)		/* DMA Block Overrun Error */
+#define		ATMCI_DMADONE			BIT(25)		/* DMA Transfer Done */
+#define		ATMCI_FIFOEMPTY			BIT(26)		/* FIFO Empty Flag */
+#define		ATMCI_XFRDONE			BIT(27)		/* Transfer Done Flag */
+#define		ATMCI_ACKRCV			BIT(28)		/* Boot Operation Acknowledge Received */
+#define		ATMCI_ACKRCVE			BIT(29)		/* Boot Operation Acknowledge Error */
+#define		ATMCI_OVRE			BIT(30)		/* RX Overrun Error */
+#define		ATMCI_UNRE			BIT(31)		/* TX Underrun Error */
+#define	ATMCI_DMA			0x0050	/* DMA Configuration[2] */
+#define		ATMCI_DMA_OFFSET(x)		((x) <<  0)	/* DMA Write Buffer Offset */
+#define		ATMCI_DMA_CHKSIZE(x)		((x) <<  4)	/* DMA Channel Read and Write Chunk Size */
+#define		ATMCI_DMAEN			BIT(8)	/* DMA Hardware Handshaking Enable */
+#define	ATMCI_CFG			0x0054	/* Configuration[2] */
+#define		ATMCI_CFG_FIFOMODE_1DATA	BIT(0)		/* MCI Internal FIFO control mode */
+#define		ATMCI_CFG_FERRCTRL_COR		BIT(4)		/* Flow Error flag reset control mode */
+#define		ATMCI_CFG_HSMODE		BIT(8)		/* High Speed Mode */
+#define		ATMCI_CFG_LSYNC			BIT(12)		/* Synchronize on the last block */
+#define	ATMCI_WPMR			0x00e4	/* Write Protection Mode[2] */
+#define		ATMCI_WP_EN			BIT(0)		/* WP Enable */
+#define		ATMCI_WP_KEY			(0x4d4349 << 8)	/* WP Key */
+#define	ATMCI_WPSR			0x00e8	/* Write Protection Status[2] */
+#define		ATMCI_GET_WP_VS(x)		((x) & 0x0f)
+#define		ATMCI_GET_WP_VSRC(x)		(((x) >> 8) & 0xffff)
+#define	ATMCI_VERSION			0x00FC  /* Version */
+#define	ATMCI_FIFO_APERTURE		0x0200	/* FIFO Aperture[2] */
+
+/* This is not including the FIFO Aperture on MCI2 */
+#define	ATMCI_REGS_SIZE		0x100
+
+/* Register access macros */
+#define	atmci_readl(port, reg)				\
+	__raw_readl((port)->regs + reg)
+#define	atmci_writel(port, reg, value)			\
+	__raw_writel((value), (port)->regs + reg)
+
+/* On AVR chips the Peripheral DMA Controller is not connected to MCI. */
+#ifdef CONFIG_AVR32
+#	define ATMCI_PDC_CONNECTED	0
+#else
+#	define ATMCI_PDC_CONNECTED	1
+#endif
 
 #define AUTOSUSPEND_DELAY	50
 
@@ -584,6 +717,29 @@ static inline unsigned int atmci_get_version(struct atmel_mci *host)
 	return atmci_readl(host, ATMCI_VERSION) & 0x00000fff;
 }
 
+/*
+ * Fix sconfig's burst size according to atmel MCI. We need to convert them as:
+ * 1 -> 0, 4 -> 1, 8 -> 2, 16 -> 3.
+ * With version 0x600, we need to convert them as: 1 -> 0, 2 -> 1, 4 -> 2,
+ * 8 -> 3, 16 -> 4.
+ *
+ * This can be done by finding most significant bit set.
+ */
+static inline unsigned int atmci_convert_chksize(struct atmel_mci *host,
+						 unsigned int maxburst)
+{
+	unsigned int version = atmci_get_version(host);
+	unsigned int offset = 2;
+
+	if (version >= 0x600)
+		offset = 1;
+
+	if (maxburst > 1)
+		return fls(maxburst) - offset;
+	else
+		return 0;
+}
+
 static void atmci_timeout_timer(unsigned long data)
 {
 	struct atmel_mci *host;
@@ -1034,11 +1190,13 @@ atmci_prepare_data_dma(struct atmel_mci *host, struct mmc_data *data)
 	if (data->flags & MMC_DATA_READ) {
 		direction = DMA_FROM_DEVICE;
 		host->dma_conf.direction = slave_dirn = DMA_DEV_TO_MEM;
-		maxburst = atmci_convert_chksize(host->dma_conf.src_maxburst);
+		maxburst = atmci_convert_chksize(host,
+						 host->dma_conf.src_maxburst);
 	} else {
 		direction = DMA_TO_DEVICE;
 		host->dma_conf.direction = slave_dirn = DMA_MEM_TO_DEV;
-		maxburst = atmci_convert_chksize(host->dma_conf.dst_maxburst);
+		maxburst = atmci_convert_chksize(host,
+						 host->dma_conf.dst_maxburst);
 	}
 
 	if (host->caps.has_dma_conf_reg)
@@ -2280,6 +2438,23 @@ static int atmci_configure_dma(struct atmel_mci *host)
 {
 	host->dma.chan = dma_request_slave_channel_reason(&host->pdev->dev,
 							"rxtx");
+
+	if (PTR_ERR(host->dma.chan) == -ENODEV) {
+		struct mci_platform_data *pdata = host->pdev->dev.platform_data;
+		dma_cap_mask_t mask;
+
+		if (!pdata->dma_filter)
+			return -ENODEV;
+
+		dma_cap_zero(mask);
+		dma_cap_set(DMA_SLAVE, mask);
+
+		host->dma.chan = dma_request_channel(mask, pdata->dma_filter,
+						     pdata->dma_slave);
+		if (!host->dma.chan)
+			host->dma.chan = ERR_PTR(-ENODEV);
+	}
+
 	if (IS_ERR(host->dma.chan))
 		return PTR_ERR(host->dma.chan);
 
diff --git a/drivers/mmc/host/cb710-mmc.h b/drivers/mmc/host/cb710-mmc.h
index 8984ec878fc9..8ecd9e56636a 100644
--- a/drivers/mmc/host/cb710-mmc.h
+++ b/drivers/mmc/host/cb710-mmc.h
@@ -29,8 +29,7 @@ static inline struct mmc_host *cb710_slot_to_mmc(struct cb710_slot *slot)
 
 static inline struct cb710_slot *cb710_mmc_to_slot(struct mmc_host *mmc)
 {
-	struct platform_device *pdev = container_of(mmc_dev(mmc),
-		struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(mmc_dev(mmc));
 	return cb710_pdev_to_slot(pdev);
 }
 
diff --git a/drivers/mmc/host/dw_mmc-pltfm.c b/drivers/mmc/host/dw_mmc-pltfm.c
index 7e1d13b68b06..81bdeeb05a4d 100644
--- a/drivers/mmc/host/dw_mmc-pltfm.c
+++ b/drivers/mmc/host/dw_mmc-pltfm.c
@@ -60,7 +60,7 @@ int dw_mci_pltfm_register(struct platform_device *pdev,
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	/* Get registers' physical base address */
-	host->phy_regs = (void *)(regs->start);
+	host->phy_regs = regs->start;
 	host->regs = devm_ioremap_resource(&pdev->dev, regs);
 	if (IS_ERR(host->regs))
 		return PTR_ERR(host->regs);
diff --git a/drivers/mmc/host/dw_mmc-rockchip.c b/drivers/mmc/host/dw_mmc-rockchip.c
index 9becebeeccd1..d9c92f31da64 100644
--- a/drivers/mmc/host/dw_mmc-rockchip.c
+++ b/drivers/mmc/host/dw_mmc-rockchip.c
@@ -239,20 +239,12 @@ static int dw_mci_rockchip_init(struct dw_mci *host)
 	return 0;
 }
 
-/* Common capabilities of RK3288 SoC */
-static unsigned long dw_mci_rk3288_dwmmc_caps[4] = {
-	MMC_CAP_RUNTIME_RESUME, /* emmc */
-	MMC_CAP_RUNTIME_RESUME, /* sdmmc */
-	MMC_CAP_RUNTIME_RESUME, /* sdio0 */
-	MMC_CAP_RUNTIME_RESUME, /* sdio1 */
-};
 static const struct dw_mci_drv_data rk2928_drv_data = {
 	.prepare_command        = dw_mci_rockchip_prepare_command,
 	.init			= dw_mci_rockchip_init,
 };
 
 static const struct dw_mci_drv_data rk3288_drv_data = {
-	.caps			= dw_mci_rk3288_dwmmc_caps,
 	.prepare_command        = dw_mci_rockchip_prepare_command,
 	.set_ios		= dw_mci_rk3288_set_ios,
 	.execute_tuning		= dw_mci_rk3288_execute_tuning,
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 7a6cedbe48a8..712835177e8b 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -699,7 +699,7 @@ static int dw_mci_edmac_start_dma(struct dw_mci *host,
 	int ret = 0;
 
 	/* Set external dma config: burst size, burst width */
-	cfg.dst_addr = (dma_addr_t)(host->phy_regs + fifo_offset);
+	cfg.dst_addr = host->phy_regs + fifo_offset;
 	cfg.src_addr = cfg.dst_addr;
 	cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
 	cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
@@ -1634,12 +1634,6 @@ static int dw_mci_command_complete(struct dw_mci *host, struct mmc_command *cmd)
 	else
 		cmd->error = 0;
 
-	if (cmd->error) {
-		/* newer ip versions need a delay between retries */
-		if (host->quirks & DW_MCI_QUIRK_RETRY_DELAY)
-			mdelay(20);
-	}
-
 	return cmd->error;
 }
 
@@ -2355,16 +2349,6 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 
 	pending = mci_readl(host, MINTSTS); /* read-only mask reg */
 
-	/*
-	 * DTO fix - version 2.10a and below, and only if internal DMA
-	 * is configured.
-	 */
-	if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO) {
-		if (!pending &&
-		    ((mci_readl(host, STATUS) >> 17) & 0x1fff))
-			pending |= SDMMC_INT_DATA_OVER;
-	}
-
 	if (pending) {
 		/* Check volt switch first, since it can look like an error */
 		if ((host->state == STATE_SENDING_CMD11) &&
@@ -3165,9 +3149,6 @@ int dw_mci_probe(struct dw_mci *host)
 	/* Now that slots are all setup, we can enable card detect */
 	dw_mci_enable_cd(host);
 
-	if (host->quirks & DW_MCI_QUIRK_IDMAC_DTO)
-		dev_info(host->dev, "Internal DMAC interrupt fix enabled.\n");
-
 	return 0;
 
 err_dmaunmap:
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index 1c1b45ef3faf..3446097a43c0 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -925,6 +925,10 @@ mmc_spi_data_do(struct mmc_spi_host *host, struct mmc_command *cmd,
 
 			dma_addr = dma_map_page(dma_dev, sg_page(sg), 0,
 						PAGE_SIZE, dir);
+			if (dma_mapping_error(dma_dev, dma_addr)) {
+				data->error = -EFAULT;
+				break;
+			}
 			if (direction == DMA_TO_DEVICE)
 				t->tx_dma = dma_addr + sg->offset;
 			else
@@ -1393,10 +1397,12 @@ static int mmc_spi_probe(struct spi_device *spi)
 		host->dma_dev = dev;
 		host->ones_dma = dma_map_single(dev, ones,
 				MMC_SPI_BLOCKSIZE, DMA_TO_DEVICE);
+		if (dma_mapping_error(dev, host->ones_dma))
+			goto fail_ones_dma;
 		host->data_dma = dma_map_single(dev, host->data,
 				sizeof(*host->data), DMA_BIDIRECTIONAL);
-
-		/* REVISIT in theory those map operations can fail... */
+		if (dma_mapping_error(dev, host->data_dma))
+			goto fail_data_dma;
 
 		dma_sync_single_for_cpu(host->dma_dev,
 				host->data_dma, sizeof(*host->data),
@@ -1462,6 +1468,11 @@ fail_glue_init:
 	if (host->dma_dev)
 		dma_unmap_single(host->dma_dev, host->data_dma,
 				sizeof(*host->data), DMA_BIDIRECTIONAL);
+fail_data_dma:
+	if (host->dma_dev)
+		dma_unmap_single(host->dma_dev, host->ones_dma,
+				MMC_SPI_BLOCKSIZE, DMA_TO_DEVICE);
+fail_ones_dma:
 	kfree(host->data);
 
 fail_nobuf1:
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index fb266745f824..0d6ca4116f3d 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -151,6 +151,7 @@ static struct variant_data variant_nomadik = {
 	.fifosize		= 16 * 4,
 	.fifohalfsize		= 8 * 4,
 	.clkreg			= MCI_CLK_ENABLE,
+	.clkreg_8bit_bus_enable = MCI_ST_8BIT_BUS,
 	.datalength_bits	= 24,
 	.datactrl_mask_sdio	= MCI_ST_DPSM_SDIOEN,
 	.st_sdio		= true,
@@ -1886,7 +1887,7 @@ static struct amba_id mmci_ids[] = {
 	{
 		.id     = 0x00280180,
 		.mask   = 0x00ffffff,
-		.data	= &variant_u300,
+		.data	= &variant_nomadik,
 	},
 	{
 		.id     = 0x00480180,
diff --git a/drivers/mmc/host/mtk-sd.c b/drivers/mmc/host/mtk-sd.c
index 33dfd7e72516..82a97ac4e956 100644
--- a/drivers/mmc/host/mtk-sd.c
+++ b/drivers/mmc/host/mtk-sd.c
@@ -972,7 +972,7 @@ static bool msdc_data_xfer_done(struct msdc_host *host, u32 events,
 		if ((events & MSDC_INT_XFER_COMPL) && (!stop || !stop->error)) {
 			data->bytes_xfered = data->blocks * data->blksz;
 		} else {
-			dev_err(host->dev, "interrupt events: %x\n", events);
+			dev_dbg(host->dev, "interrupt events: %x\n", events);
 			msdc_reset_hw(host);
 			host->error |= REQ_DAT_ERR;
 			data->bytes_xfered = 0;
@@ -982,10 +982,10 @@ static bool msdc_data_xfer_done(struct msdc_host *host, u32 events,
 			else if (events & MSDC_INT_DATCRCERR)
 				data->error = -EILSEQ;
 
-			dev_err(host->dev, "%s: cmd=%d; blocks=%d",
+			dev_dbg(host->dev, "%s: cmd=%d; blocks=%d",
 				__func__, mrq->cmd->opcode, data->blocks);
-			dev_err(host->dev, "data_error=%d xfer_size=%d\n",
-					(int)data->error, data->bytes_xfered);
+			dev_dbg(host->dev, "data_error=%d xfer_size=%d\n",
+				(int)data->error, data->bytes_xfered);
 		}
 
 		msdc_data_xfer_next(host, mrq, data);
@@ -1543,7 +1543,6 @@ static int msdc_drv_probe(struct platform_device *pdev)
 	mmc->f_min = host->src_clk_freq / (4 * 255);
 
 	mmc->caps |= MMC_CAP_ERASE | MMC_CAP_CMD23;
-	mmc->caps |= MMC_CAP_RUNTIME_RESUME;
 	/* MMC core transfer sizes tunable parameters */
 	mmc->max_segs = MAX_BD_NUM;
 	mmc->max_seg_size = BDMA_DESC_BUFLEN;
diff --git a/drivers/mmc/host/mvsdio.c b/drivers/mmc/host/mvsdio.c
index a448498e3af2..42296e55b9de 100644
--- a/drivers/mmc/host/mvsdio.c
+++ b/drivers/mmc/host/mvsdio.c
@@ -20,15 +20,12 @@
 #include <linux/scatterlist.h>
 #include <linux/irq.h>
 #include <linux/clk.h>
-#include <linux/gpio.h>
-#include <linux/of_gpio.h>
 #include <linux/of_irq.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/slot-gpio.h>
 
 #include <asm/sizes.h>
 #include <asm/unaligned.h>
-#include <linux/platform_data/mmc-mvsdio.h>
 
 #include "mvsdio.h"
 
@@ -704,6 +701,10 @@ static int mvsd_probe(struct platform_device *pdev)
 	struct resource *r;
 	int ret, irq;
 
+	if (!np) {
+		dev_err(&pdev->dev, "no DT node\n");
+		return -ENODEV;
+	}
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
 	if (!r || irq < 0)
@@ -727,8 +728,12 @@ static int mvsd_probe(struct platform_device *pdev)
 	 * fixed rate clock).
 	 */
 	host->clk = devm_clk_get(&pdev->dev, NULL);
-	if (!IS_ERR(host->clk))
-		clk_prepare_enable(host->clk);
+	if (IS_ERR(host->clk)) {
+		dev_err(&pdev->dev, "no clock associated\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	clk_prepare_enable(host->clk);
 
 	mmc->ops = &mvsd_ops;
 
@@ -744,45 +749,10 @@ static int mvsd_probe(struct platform_device *pdev)
 	mmc->max_seg_size = mmc->max_blk_size * mmc->max_blk_count;
 	mmc->max_req_size = mmc->max_blk_size * mmc->max_blk_count;
 
-	if (np) {
-		if (IS_ERR(host->clk)) {
-			dev_err(&pdev->dev, "DT platforms must have a clock associated\n");
-			ret = -EINVAL;
-			goto out;
-		}
-
-		host->base_clock = clk_get_rate(host->clk) / 2;
-		ret = mmc_of_parse(mmc);
-		if (ret < 0)
-			goto out;
-	} else {
-		const struct mvsdio_platform_data *mvsd_data;
-
-		mvsd_data = pdev->dev.platform_data;
-		if (!mvsd_data) {
-			ret = -ENXIO;
-			goto out;
-		}
-		mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SDIO_IRQ |
-			    MMC_CAP_SD_HIGHSPEED | MMC_CAP_MMC_HIGHSPEED;
-		host->base_clock = mvsd_data->clock / 2;
-		/* GPIO 0 regarded as invalid for backward compatibility */
-		if (mvsd_data->gpio_card_detect &&
-		    gpio_is_valid(mvsd_data->gpio_card_detect)) {
-			ret = mmc_gpio_request_cd(mmc,
-						  mvsd_data->gpio_card_detect,
-						  0);
-			if (ret)
-				goto out;
-		} else {
-			mmc->caps |= MMC_CAP_NEEDS_POLL;
-		}
-
-		if (mvsd_data->gpio_write_protect &&
-		    gpio_is_valid(mvsd_data->gpio_write_protect))
-			mmc_gpio_request_ro(mmc, mvsd_data->gpio_write_protect);
-	}
-
+	host->base_clock = clk_get_rate(host->clk) / 2;
+	ret = mmc_of_parse(mmc);
+	if (ret < 0)
+		goto out;
 	if (maxfreq)
 		mmc->f_max = maxfreq;
 
diff --git a/drivers/mmc/host/of_mmc_spi.c b/drivers/mmc/host/of_mmc_spi.c
index 6e218fb1a669..660170cd04d9 100644
--- a/drivers/mmc/host/of_mmc_spi.c
+++ b/drivers/mmc/host/of_mmc_spi.c
@@ -55,8 +55,8 @@ static int of_mmc_spi_init(struct device *dev,
 {
 	struct of_mmc_spi *oms = to_of_mmc_spi(dev);
 
-	return request_threaded_irq(oms->detect_irq, NULL, irqhandler, 0,
-				    dev_name(dev), mmc);
+	return request_threaded_irq(oms->detect_irq, NULL, irqhandler,
+					IRQF_ONESHOT, dev_name(dev), mmc);
 }
 
 static void of_mmc_spi_exit(struct device *dev, void *mmc)
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 7fb0753abe30..f6e4d9718035 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -2232,6 +2232,7 @@ err_irq:
 		dma_release_channel(host->tx_chan);
 	if (host->rx_chan)
 		dma_release_channel(host->rx_chan);
+	pm_runtime_dont_use_autosuspend(host->dev);
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	if (host->dbclk)
@@ -2250,11 +2251,10 @@ static int omap_hsmmc_remove(struct platform_device *pdev)
 	pm_runtime_get_sync(host->dev);
 	mmc_remove_host(host->mmc);
 
-	if (host->tx_chan)
-		dma_release_channel(host->tx_chan);
-	if (host->rx_chan)
-		dma_release_channel(host->rx_chan);
+	dma_release_channel(host->tx_chan);
+	dma_release_channel(host->rx_chan);
 
+	pm_runtime_dont_use_autosuspend(host->dev);
 	pm_runtime_put_sync(host->dev);
 	pm_runtime_disable(host->dev);
 	device_init_wakeup(&pdev->dev, false);
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index ce08896b9d69..da824772bbb4 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -86,7 +86,7 @@ struct pxamci_host {
 static inline void pxamci_init_ocr(struct pxamci_host *host)
 {
 #ifdef CONFIG_REGULATOR
-	host->vcc = regulator_get_optional(mmc_dev(host->mmc), "vmmc");
+	host->vcc = devm_regulator_get_optional(mmc_dev(host->mmc), "vmmc");
 
 	if (IS_ERR(host->vcc))
 		host->vcc = NULL;
@@ -654,12 +654,8 @@ static int pxamci_probe(struct platform_device *pdev)
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irq = platform_get_irq(pdev, 0);
-	if (!r || irq < 0)
-		return -ENXIO;
-
-	r = request_mem_region(r->start, SZ_4K, DRIVER_NAME);
-	if (!r)
-		return -EBUSY;
+	if (irq < 0)
+		return irq;
 
 	mmc = mmc_alloc_host(sizeof(struct pxamci_host), &pdev->dev);
 	if (!mmc) {
@@ -695,7 +691,7 @@ static int pxamci_probe(struct platform_device *pdev)
 	host->pdata = pdev->dev.platform_data;
 	host->clkrt = CLKRT_OFF;
 
-	host->clk = clk_get(&pdev->dev, NULL);
+	host->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(host->clk)) {
 		ret = PTR_ERR(host->clk);
 		host->clk = NULL;
@@ -727,9 +723,9 @@ static int pxamci_probe(struct platform_device *pdev)
 	host->irq = irq;
 	host->imask = MMC_I_MASK_ALL;
 
-	host->base = ioremap(r->start, SZ_4K);
-	if (!host->base) {
-		ret = -ENOMEM;
+	host->base = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(host->base)) {
+		ret = PTR_ERR(host->base);
 		goto out;
 	}
 
@@ -742,7 +738,8 @@ static int pxamci_probe(struct platform_device *pdev)
 	writel(64, host->base + MMC_RESTO);
 	writel(host->imask, host->base + MMC_I_MASK);
 
-	ret = request_irq(host->irq, pxamci_irq, 0, DRIVER_NAME, host);
+	ret = devm_request_irq(&pdev->dev, host->irq, pxamci_irq, 0,
+			       DRIVER_NAME, host);
 	if (ret)
 		goto out;
 
@@ -804,7 +801,7 @@ static int pxamci_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "Failed requesting gpio_ro %d\n", gpio_ro);
 		goto out;
 	} else {
-		mmc->caps |= host->pdata->gpio_card_ro_invert ?
+		mmc->caps2 |= host->pdata->gpio_card_ro_invert ?
 			0 : MMC_CAP2_RO_ACTIVE_HIGH;
 	}
 
@@ -833,14 +830,9 @@ out:
 			dma_release_channel(host->dma_chan_rx);
 		if (host->dma_chan_tx)
 			dma_release_channel(host->dma_chan_tx);
-		if (host->base)
-			iounmap(host->base);
-		if (host->clk)
-			clk_put(host->clk);
 	}
 	if (mmc)
 		mmc_free_host(mmc);
-	release_resource(r);
 	return ret;
 }
 
@@ -859,9 +851,6 @@ static int pxamci_remove(struct platform_device *pdev)
 			gpio_ro = host->pdata->gpio_card_ro;
 			gpio_power = host->pdata->gpio_power;
 		}
-		if (host->vcc)
-			regulator_put(host->vcc);
-
 		if (host->pdata && host->pdata->exit)
 			host->pdata->exit(&pdev->dev, mmc);
 
@@ -870,16 +859,10 @@ static int pxamci_remove(struct platform_device *pdev)
 		       END_CMD_RES|PRG_DONE|DATA_TRAN_DONE,
 		       host->base + MMC_I_MASK);
 
-		free_irq(host->irq, host);
 		dmaengine_terminate_all(host->dma_chan_rx);
 		dmaengine_terminate_all(host->dma_chan_tx);
 		dma_release_channel(host->dma_chan_rx);
 		dma_release_channel(host->dma_chan_tx);
-		iounmap(host->base);
-
-		clk_put(host->clk);
-
-		release_resource(host->res);
 
 		mmc_free_host(mmc);
 	}
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index f6047fc94062..a5cda926d38e 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -146,6 +146,33 @@ static const struct sdhci_acpi_chip sdhci_acpi_chip_int = {
 	.ops = &sdhci_acpi_ops_int,
 };
 
+static int bxt_get_cd(struct mmc_host *mmc)
+{
+	int gpio_cd = mmc_gpio_get_cd(mmc);
+	struct sdhci_host *host = mmc_priv(mmc);
+	unsigned long flags;
+	int ret = 0;
+
+	if (!gpio_cd)
+		return 0;
+
+	pm_runtime_get_sync(mmc->parent);
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (host->flags & SDHCI_DEVICE_DEAD)
+		goto out;
+
+	ret = !!(sdhci_readl(host, SDHCI_PRESENT_STATE) & SDHCI_CARD_PRESENT);
+out:
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	pm_runtime_mark_last_busy(mmc->parent);
+	pm_runtime_put_autosuspend(mmc->parent);
+
+	return ret;
+}
+
 static int sdhci_acpi_emmc_probe_slot(struct platform_device *pdev,
 				      const char *hid, const char *uid)
 {
@@ -196,6 +223,9 @@ static int sdhci_acpi_sd_probe_slot(struct platform_device *pdev,
 
 	/* Platform specific code during sd probe slot goes here */
 
+	if (hid && !strcmp(hid, "80865ACA"))
+		host->mmc_host_ops.get_cd = bxt_get_cd;
+
 	return 0;
 }
 
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index 1f1582f6cccb..f25f29253595 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -76,6 +76,7 @@
 #define ESDHC_STD_TUNING_EN		(1 << 24)
 /* NOTE: the minimum valid tuning start tap for mx6sl is 1 */
 #define ESDHC_TUNING_START_TAP		0x1
+#define ESDHC_TUNING_STEP_MASK		0x00070000
 #define ESDHC_TUNING_STEP_SHIFT		16
 
 /* pinctrl state */
@@ -489,9 +490,11 @@ static void esdhc_writew_le(struct sdhci_host *host, u16 val, int reg)
 				m |= ESDHC_MIX_CTRL_FBCLK_SEL;
 				tuning_ctrl = readl(host->ioaddr + ESDHC_TUNING_CTRL);
 				tuning_ctrl |= ESDHC_STD_TUNING_EN | ESDHC_TUNING_START_TAP;
-				if (imx_data->boarddata.tuning_step)
+				if (imx_data->boarddata.tuning_step) {
+					tuning_ctrl &= ~ESDHC_TUNING_STEP_MASK;
 					tuning_ctrl |= imx_data->boarddata.tuning_step << ESDHC_TUNING_STEP_SHIFT;
-					writel(tuning_ctrl, host->ioaddr + ESDHC_TUNING_CTRL);
+				}
+				writel(tuning_ctrl, host->ioaddr + ESDHC_TUNING_CTRL);
 			} else {
 				v &= ~ESDHC_MIX_CTRL_EXE_TUNE;
 			}
diff --git a/drivers/mmc/host/sdhci-of-at91.c b/drivers/mmc/host/sdhci-of-at91.c
index 06d0b50dfe71..9cb86fb25976 100644
--- a/drivers/mmc/host/sdhci-of-at91.c
+++ b/drivers/mmc/host/sdhci-of-at91.c
@@ -21,6 +21,8 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
 
 #include "sdhci-pltfm.h"
 
@@ -51,6 +53,60 @@ static const struct of_device_id sdhci_at91_dt_match[] = {
 	{}
 };
 
+#ifdef CONFIG_PM
+static int sdhci_at91_runtime_suspend(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_at91_priv *priv = pltfm_host->priv;
+	int ret;
+
+	ret = sdhci_runtime_suspend_host(host);
+
+	clk_disable_unprepare(priv->gck);
+	clk_disable_unprepare(priv->hclock);
+	clk_disable_unprepare(priv->mainck);
+
+	return ret;
+}
+
+static int sdhci_at91_runtime_resume(struct device *dev)
+{
+	struct sdhci_host *host = dev_get_drvdata(dev);
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_at91_priv *priv = pltfm_host->priv;
+	int ret;
+
+	ret = clk_prepare_enable(priv->mainck);
+	if (ret) {
+		dev_err(dev, "can't enable mainck\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(priv->hclock);
+	if (ret) {
+		dev_err(dev, "can't enable hclock\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(priv->gck);
+	if (ret) {
+		dev_err(dev, "can't enable gck\n");
+		return ret;
+	}
+
+	return sdhci_runtime_resume_host(host);
+}
+#endif /* CONFIG_PM */
+
+static const struct dev_pm_ops sdhci_at91_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+				pm_runtime_force_resume)
+	SET_RUNTIME_PM_OPS(sdhci_at91_runtime_suspend,
+			   sdhci_at91_runtime_resume,
+			   NULL)
+};
+
 static int sdhci_at91_probe(struct platform_device *pdev)
 {
 	const struct of_device_id	*match;
@@ -144,12 +200,24 @@ static int sdhci_at91_probe(struct platform_device *pdev)
 
 	sdhci_get_of_property(pdev);
 
+	pm_runtime_get_noresume(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
+	pm_runtime_use_autosuspend(&pdev->dev);
+
 	ret = sdhci_add_host(host);
 	if (ret)
-		goto clocks_disable_unprepare;
+		goto pm_runtime_disable;
+
+	pm_runtime_put_autosuspend(&pdev->dev);
 
 	return 0;
 
+pm_runtime_disable:
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
+	pm_runtime_put_noidle(&pdev->dev);
 clocks_disable_unprepare:
 	clk_disable_unprepare(priv->gck);
 	clk_disable_unprepare(priv->mainck);
@@ -165,6 +233,10 @@ static int sdhci_at91_remove(struct platform_device *pdev)
 	struct sdhci_pltfm_host	*pltfm_host = sdhci_priv(host);
 	struct sdhci_at91_priv	*priv = pltfm_host->priv;
 
+	pm_runtime_get_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_put_noidle(&pdev->dev);
+
 	sdhci_pltfm_unregister(pdev);
 
 	clk_disable_unprepare(priv->gck);
@@ -178,7 +250,7 @@ static struct platform_driver sdhci_at91_driver = {
 	.driver		= {
 		.name	= "sdhci-at91",
 		.of_match_table = sdhci_at91_dt_match,
-		.pm	= SDHCI_PLTFM_PMOPS,
+		.pm	= &sdhci_at91_dev_pm_ops,
 	},
 	.probe		= sdhci_at91_probe,
 	.remove		= sdhci_at91_remove,
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 90e94a028a49..83b1226471c1 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -584,6 +584,8 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 {
 	struct sdhci_host *host;
 	struct device_node *np;
+	struct sdhci_pltfm_host *pltfm_host;
+	struct sdhci_esdhc *esdhc;
 	int ret;
 
 	np = pdev->dev.of_node;
@@ -600,6 +602,14 @@ static int sdhci_esdhc_probe(struct platform_device *pdev)
 
 	sdhci_get_of_property(pdev);
 
+	pltfm_host = sdhci_priv(host);
+	esdhc = pltfm_host->priv;
+	if (esdhc->vendor_ver == VENDOR_V_22)
+		host->quirks2 |= SDHCI_QUIRK2_HOST_NO_CMD23;
+
+	if (esdhc->vendor_ver > VENDOR_V_22)
+		host->quirks &= ~SDHCI_QUIRK_NO_BUSY_IRQ;
+
 	if (of_device_is_compatible(np, "fsl,p5040-esdhc") ||
 	    of_device_is_compatible(np, "fsl,p5020-esdhc") ||
 	    of_device_is_compatible(np, "fsl,p4080-esdhc") ||
diff --git a/drivers/mmc/host/sdhci-pci-core.c b/drivers/mmc/host/sdhci-pci-core.c
index cf7ad458b4f4..df3b8eced8c4 100644
--- a/drivers/mmc/host/sdhci-pci-core.c
+++ b/drivers/mmc/host/sdhci-pci-core.c
@@ -277,7 +277,7 @@ static int spt_select_drive_strength(struct sdhci_host *host,
 	if (sdhci_pci_spt_drive_strength > 0)
 		drive_strength = sdhci_pci_spt_drive_strength & 0xf;
 	else
-		drive_strength = 1; /* 33-ohm */
+		drive_strength = 0; /* Default 50-ohm */
 
 	if ((mmc_driver_type_mask(drive_strength) & card_drv) == 0)
 		drive_strength = 0; /* Default 50-ohm */
@@ -330,6 +330,33 @@ static void spt_read_drive_strength(struct sdhci_host *host)
 	sdhci_pci_spt_drive_strength = 0x10 | ((val >> 12) & 0xf);
 }
 
+static int bxt_get_cd(struct mmc_host *mmc)
+{
+	int gpio_cd = mmc_gpio_get_cd(mmc);
+	struct sdhci_host *host = mmc_priv(mmc);
+	unsigned long flags;
+	int ret = 0;
+
+	if (!gpio_cd)
+		return 0;
+
+	pm_runtime_get_sync(mmc->parent);
+
+	spin_lock_irqsave(&host->lock, flags);
+
+	if (host->flags & SDHCI_DEVICE_DEAD)
+		goto out;
+
+	ret = !!(sdhci_readl(host, SDHCI_PRESENT_STATE) & SDHCI_CARD_PRESENT);
+out:
+	spin_unlock_irqrestore(&host->lock, flags);
+
+	pm_runtime_mark_last_busy(mmc->parent);
+	pm_runtime_put_autosuspend(mmc->parent);
+
+	return ret;
+}
+
 static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 {
 	slot->host->mmc->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
@@ -362,6 +389,10 @@ static int byt_sd_probe_slot(struct sdhci_pci_slot *slot)
 	slot->cd_con_id = NULL;
 	slot->cd_idx = 0;
 	slot->cd_override_level = true;
+	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_BXT_SD ||
+	    slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_APL_SD)
+		slot->host->mmc_host_ops.get_cd = bxt_get_cd;
+
 	return 0;
 }
 
@@ -1464,7 +1495,7 @@ static int sdhci_pci_resume(struct device *dev)
 
 static int sdhci_pci_runtime_suspend(struct device *dev)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct sdhci_pci_chip *chip;
 	struct sdhci_pci_slot *slot;
 	int i, ret;
@@ -1500,7 +1531,7 @@ err_pci_runtime_suspend:
 
 static int sdhci_pci_runtime_resume(struct device *dev)
 {
-	struct pci_dev *pdev = container_of(dev, struct pci_dev, dev);
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct sdhci_pci_chip *chip;
 	struct sdhci_pci_slot *slot;
 	int i, ret;
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 87fb5ea8ebe7..072bb27a65cf 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -104,7 +104,8 @@ void sdhci_get_of_property(struct platform_device *pdev)
 	if (of_find_property(np, "keep-power-in-suspend", NULL))
 		host->mmc->pm_caps |= MMC_PM_KEEP_POWER;
 
-	if (of_find_property(np, "enable-sdio-wakeup", NULL))
+	if (of_property_read_bool(np, "wakeup-source") ||
+	    of_property_read_bool(np, "enable-sdio-wakeup")) /* legacy */
 		host->mmc->pm_caps |= MMC_PM_WAKE_SDIO_IRQ;
 }
 #else
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index ad28b49f0203..83c4bf7bc16c 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -22,12 +22,20 @@
 #include <linux/of_device.h>
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
+#include <linux/mmc/mmc.h>
 #include <linux/mmc/slot-gpio.h>
 #include <linux/gpio/consumer.h>
 
 #include "sdhci-pltfm.h"
 
 /* Tegra SDHOST controller vendor register definitions */
+#define SDHCI_TEGRA_VENDOR_CLOCK_CTRL			0x100
+#define SDHCI_CLOCK_CTRL_TAP_MASK			0x00ff0000
+#define SDHCI_CLOCK_CTRL_TAP_SHIFT			16
+#define SDHCI_CLOCK_CTRL_SDR50_TUNING_OVERRIDE		BIT(5)
+#define SDHCI_CLOCK_CTRL_PADPIPE_CLKEN_OVERRIDE		BIT(3)
+#define SDHCI_CLOCK_CTRL_SPI_MODE_CLKEN_OVERRIDE	BIT(2)
+
 #define SDHCI_TEGRA_VENDOR_MISC_CTRL		0x120
 #define SDHCI_MISC_CTRL_ENABLE_SDR104		0x8
 #define SDHCI_MISC_CTRL_ENABLE_SDR50		0x10
@@ -37,9 +45,9 @@
 #define NVQUIRK_FORCE_SDHCI_SPEC_200	BIT(0)
 #define NVQUIRK_ENABLE_BLOCK_GAP_DET	BIT(1)
 #define NVQUIRK_ENABLE_SDHCI_SPEC_300	BIT(2)
-#define NVQUIRK_DISABLE_SDR50		BIT(3)
-#define NVQUIRK_DISABLE_SDR104		BIT(4)
-#define NVQUIRK_DISABLE_DDR50		BIT(5)
+#define NVQUIRK_ENABLE_SDR50		BIT(3)
+#define NVQUIRK_ENABLE_SDR104		BIT(4)
+#define NVQUIRK_ENABLE_DDR50		BIT(5)
 
 struct sdhci_tegra_soc_data {
 	const struct sdhci_pltfm_data *pdata;
@@ -49,6 +57,7 @@ struct sdhci_tegra_soc_data {
 struct sdhci_tegra {
 	const struct sdhci_tegra_soc_data *soc_data;
 	struct gpio_desc *power_gpio;
+	bool ddr_signaling;
 };
 
 static u16 tegra_sdhci_readw(struct sdhci_host *host, int reg)
@@ -124,25 +133,33 @@ static void tegra_sdhci_reset(struct sdhci_host *host, u8 mask)
 	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
 	struct sdhci_tegra *tegra_host = pltfm_host->priv;
 	const struct sdhci_tegra_soc_data *soc_data = tegra_host->soc_data;
-	u32 misc_ctrl;
+	u32 misc_ctrl, clk_ctrl;
 
 	sdhci_reset(host, mask);
 
 	if (!(mask & SDHCI_RESET_ALL))
 		return;
 
-	misc_ctrl = sdhci_readw(host, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+	misc_ctrl = sdhci_readl(host, SDHCI_TEGRA_VENDOR_MISC_CTRL);
 	/* Erratum: Enable SDHCI spec v3.00 support */
 	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDHCI_SPEC_300)
 		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_SDHCI_SPEC_300;
-	/* Don't advertise UHS modes which aren't supported yet */
-	if (soc_data->nvquirks & NVQUIRK_DISABLE_SDR50)
-		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_SDR50;
-	if (soc_data->nvquirks & NVQUIRK_DISABLE_DDR50)
-		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_DDR50;
-	if (soc_data->nvquirks & NVQUIRK_DISABLE_SDR104)
-		misc_ctrl &= ~SDHCI_MISC_CTRL_ENABLE_SDR104;
-	sdhci_writew(host, misc_ctrl, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+	/* Advertise UHS modes as supported by host */
+	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDR50)
+		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_SDR50;
+	if (soc_data->nvquirks & NVQUIRK_ENABLE_DDR50)
+		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_DDR50;
+	if (soc_data->nvquirks & NVQUIRK_ENABLE_SDR104)
+		misc_ctrl |= SDHCI_MISC_CTRL_ENABLE_SDR104;
+	sdhci_writel(host, misc_ctrl, SDHCI_TEGRA_VENDOR_MISC_CTRL);
+
+	clk_ctrl = sdhci_readl(host, SDHCI_TEGRA_VENDOR_CLOCK_CTRL);
+	clk_ctrl &= ~SDHCI_CLOCK_CTRL_SPI_MODE_CLKEN_OVERRIDE;
+	if (soc_data->nvquirks & SDHCI_MISC_CTRL_ENABLE_SDR50)
+		clk_ctrl |= SDHCI_CLOCK_CTRL_SDR50_TUNING_OVERRIDE;
+	sdhci_writel(host, clk_ctrl, SDHCI_TEGRA_VENDOR_CLOCK_CTRL);
+
+	tegra_host->ddr_signaling = false;
 }
 
 static void tegra_sdhci_set_bus_width(struct sdhci_host *host, int bus_width)
@@ -164,15 +181,99 @@ static void tegra_sdhci_set_bus_width(struct sdhci_host *host, int bus_width)
 	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
 }
 
+static void tegra_sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_tegra *tegra_host = pltfm_host->priv;
+	unsigned long host_clk;
+
+	if (!clock)
+		return;
+
+	host_clk = tegra_host->ddr_signaling ? clock * 2 : clock;
+	clk_set_rate(pltfm_host->clk, host_clk);
+	host->max_clk = clk_get_rate(pltfm_host->clk);
+
+	return sdhci_set_clock(host, clock);
+}
+
+static void tegra_sdhci_set_uhs_signaling(struct sdhci_host *host,
+					  unsigned timing)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct sdhci_tegra *tegra_host = pltfm_host->priv;
+
+	if (timing == MMC_TIMING_UHS_DDR50)
+		tegra_host->ddr_signaling = true;
+
+	return sdhci_set_uhs_signaling(host, timing);
+}
+
+static unsigned int tegra_sdhci_get_max_clock(struct sdhci_host *host)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+
+	/*
+	 * DDR modes require the host to run at double the card frequency, so
+	 * the maximum rate we can support is half of the module input clock.
+	 */
+	return clk_round_rate(pltfm_host->clk, UINT_MAX) / 2;
+}
+
+static void tegra_sdhci_set_tap(struct sdhci_host *host, unsigned int tap)
+{
+	u32 reg;
+
+	reg = sdhci_readl(host, SDHCI_TEGRA_VENDOR_CLOCK_CTRL);
+	reg &= ~SDHCI_CLOCK_CTRL_TAP_MASK;
+	reg |= tap << SDHCI_CLOCK_CTRL_TAP_SHIFT;
+	sdhci_writel(host, reg, SDHCI_TEGRA_VENDOR_CLOCK_CTRL);
+}
+
+static int tegra_sdhci_execute_tuning(struct sdhci_host *host, u32 opcode)
+{
+	unsigned int min, max;
+
+	/*
+	 * Start search for minimum tap value at 10, as smaller values are
+	 * may wrongly be reported as working but fail at higher speeds,
+	 * according to the TRM.
+	 */
+	min = 10;
+	while (min < 255) {
+		tegra_sdhci_set_tap(host, min);
+		if (!mmc_send_tuning(host->mmc, opcode, NULL))
+			break;
+		min++;
+	}
+
+	/* Find the maximum tap value that still passes. */
+	max = min + 1;
+	while (max < 255) {
+		tegra_sdhci_set_tap(host, max);
+		if (mmc_send_tuning(host->mmc, opcode, NULL)) {
+			max--;
+			break;
+		}
+		max++;
+	}
+
+	/* The TRM states the ideal tap value is at 75% in the passing range. */
+	tegra_sdhci_set_tap(host, min + ((max - min) * 3 / 4));
+
+	return mmc_send_tuning(host->mmc, opcode, NULL);
+}
+
 static const struct sdhci_ops tegra_sdhci_ops = {
 	.get_ro     = tegra_sdhci_get_ro,
 	.read_w     = tegra_sdhci_readw,
 	.write_l    = tegra_sdhci_writel,
-	.set_clock  = sdhci_set_clock,
+	.set_clock  = tegra_sdhci_set_clock,
 	.set_bus_width = tegra_sdhci_set_bus_width,
 	.reset      = tegra_sdhci_reset,
-	.set_uhs_signaling = sdhci_set_uhs_signaling,
-	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
+	.platform_execute_tuning = tegra_sdhci_execute_tuning,
+	.set_uhs_signaling = tegra_sdhci_set_uhs_signaling,
+	.get_max_clock = tegra_sdhci_get_max_clock,
 };
 
 static const struct sdhci_pltfm_data sdhci_tegra20_pdata = {
@@ -184,7 +285,7 @@ static const struct sdhci_pltfm_data sdhci_tegra20_pdata = {
 	.ops  = &tegra_sdhci_ops,
 };
 
-static struct sdhci_tegra_soc_data soc_data_tegra20 = {
+static const struct sdhci_tegra_soc_data soc_data_tegra20 = {
 	.pdata = &sdhci_tegra20_pdata,
 	.nvquirks = NVQUIRK_FORCE_SDHCI_SPEC_200 |
 		    NVQUIRK_ENABLE_BLOCK_GAP_DET,
@@ -197,14 +298,15 @@ static const struct sdhci_pltfm_data sdhci_tegra30_pdata = {
 		  SDHCI_QUIRK_NO_HISPD_BIT |
 		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
 		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
+	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
 	.ops  = &tegra_sdhci_ops,
 };
 
-static struct sdhci_tegra_soc_data soc_data_tegra30 = {
+static const struct sdhci_tegra_soc_data soc_data_tegra30 = {
 	.pdata = &sdhci_tegra30_pdata,
 	.nvquirks = NVQUIRK_ENABLE_SDHCI_SPEC_300 |
-		    NVQUIRK_DISABLE_SDR50 |
-		    NVQUIRK_DISABLE_SDR104,
+		    NVQUIRK_ENABLE_SDR50 |
+		    NVQUIRK_ENABLE_SDR104,
 };
 
 static const struct sdhci_ops tegra114_sdhci_ops = {
@@ -212,11 +314,12 @@ static const struct sdhci_ops tegra114_sdhci_ops = {
 	.read_w     = tegra_sdhci_readw,
 	.write_w    = tegra_sdhci_writew,
 	.write_l    = tegra_sdhci_writel,
-	.set_clock  = sdhci_set_clock,
+	.set_clock  = tegra_sdhci_set_clock,
 	.set_bus_width = tegra_sdhci_set_bus_width,
 	.reset      = tegra_sdhci_reset,
-	.set_uhs_signaling = sdhci_set_uhs_signaling,
-	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
+	.platform_execute_tuning = tegra_sdhci_execute_tuning,
+	.set_uhs_signaling = tegra_sdhci_set_uhs_signaling,
+	.get_max_clock = tegra_sdhci_get_max_clock,
 };
 
 static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
@@ -226,17 +329,34 @@ static const struct sdhci_pltfm_data sdhci_tegra114_pdata = {
 		  SDHCI_QUIRK_NO_HISPD_BIT |
 		  SDHCI_QUIRK_BROKEN_ADMA_ZEROLEN_DESC |
 		  SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
+	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
 	.ops  = &tegra114_sdhci_ops,
 };
 
-static struct sdhci_tegra_soc_data soc_data_tegra114 = {
+static const struct sdhci_tegra_soc_data soc_data_tegra114 = {
 	.pdata = &sdhci_tegra114_pdata,
-	.nvquirks = NVQUIRK_DISABLE_SDR50 |
-		    NVQUIRK_DISABLE_DDR50 |
-		    NVQUIRK_DISABLE_SDR104,
+	.nvquirks = NVQUIRK_ENABLE_SDR50 |
+		    NVQUIRK_ENABLE_DDR50 |
+		    NVQUIRK_ENABLE_SDR104,
+};
+
+static const struct sdhci_pltfm_data sdhci_tegra210_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,
+	.ops  = &tegra114_sdhci_ops,
+};
+
+static const struct sdhci_tegra_soc_data soc_data_tegra210 = {
+	.pdata = &sdhci_tegra210_pdata,
 };
 
 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,tegra114-sdhci", .data = &soc_data_tegra114 },
 	{ .compatible = "nvidia,tegra30-sdhci", .data = &soc_data_tegra30 },
@@ -271,6 +391,7 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 		rc = -ENOMEM;
 		goto err_alloc_tegra_host;
 	}
+	tegra_host->ddr_signaling = false;
 	tegra_host->soc_data = soc_data;
 	pltfm_host->priv = tegra_host;
 
@@ -278,6 +399,9 @@ static int sdhci_tegra_probe(struct platform_device *pdev)
 	if (rc)
 		goto err_parse_dt;
 
+	if (tegra_host->soc_data->nvquirks & NVQUIRK_ENABLE_DDR50)
+		host->mmc->caps |= MMC_CAP_1_8V_DDR;
+
 	tegra_host->power_gpio = devm_gpiod_get_optional(&pdev->dev, "power",
 							 GPIOD_OUT_HIGH);
 	if (IS_ERR(tegra_host->power_gpio)) {
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index b48565ed5616..add9fdfd1d8f 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -492,7 +492,7 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 		host->align_buffer, host->align_buffer_sz, direction);
 	if (dma_mapping_error(mmc_dev(host->mmc), host->align_addr))
 		goto fail;
-	BUG_ON(host->align_addr & host->align_mask);
+	BUG_ON(host->align_addr & SDHCI_ADMA2_MASK);
 
 	host->sg_count = sdhci_pre_dma_transfer(host, data);
 	if (host->sg_count < 0)
@@ -514,8 +514,8 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 		 * the (up to three) bytes that screw up the
 		 * alignment.
 		 */
-		offset = (host->align_sz - (addr & host->align_mask)) &
-			 host->align_mask;
+		offset = (SDHCI_ADMA2_ALIGN - (addr & SDHCI_ADMA2_MASK)) &
+			 SDHCI_ADMA2_MASK;
 		if (offset) {
 			if (data->flags & MMC_DATA_WRITE) {
 				buffer = sdhci_kmap_atomic(sg, &flags);
@@ -529,8 +529,8 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 
 			BUG_ON(offset > 65536);
 
-			align += host->align_sz;
-			align_addr += host->align_sz;
+			align += SDHCI_ADMA2_ALIGN;
+			align_addr += SDHCI_ADMA2_ALIGN;
 
 			desc += host->desc_sz;
 
@@ -540,9 +540,12 @@ static int sdhci_adma_table_pre(struct sdhci_host *host,
 
 		BUG_ON(len > 65536);
 
-		/* tran, valid */
-		sdhci_adma_write_desc(host, desc, addr, len, ADMA2_TRAN_VALID);
-		desc += host->desc_sz;
+		if (len) {
+			/* tran, valid */
+			sdhci_adma_write_desc(host, desc, addr, len,
+					      ADMA2_TRAN_VALID);
+			desc += host->desc_sz;
+		}
 
 		/*
 		 * If this triggers then we have a calculation bug
@@ -608,7 +611,7 @@ static void sdhci_adma_table_post(struct sdhci_host *host,
 	/* Do a quick scan of the SG list for any unaligned mappings */
 	has_unaligned = false;
 	for_each_sg(data->sg, sg, host->sg_count, i)
-		if (sg_dma_address(sg) & host->align_mask) {
+		if (sg_dma_address(sg) & SDHCI_ADMA2_MASK) {
 			has_unaligned = true;
 			break;
 		}
@@ -620,15 +623,15 @@ static void sdhci_adma_table_post(struct sdhci_host *host,
 		align = host->align_buffer;
 
 		for_each_sg(data->sg, sg, host->sg_count, i) {
-			if (sg_dma_address(sg) & host->align_mask) {
-				size = host->align_sz -
-				       (sg_dma_address(sg) & host->align_mask);
+			if (sg_dma_address(sg) & SDHCI_ADMA2_MASK) {
+				size = SDHCI_ADMA2_ALIGN -
+				       (sg_dma_address(sg) & SDHCI_ADMA2_MASK);
 
 				buffer = sdhci_kmap_atomic(sg, &flags);
 				memcpy(buffer, align, size);
 				sdhci_kunmap_atomic(buffer, &flags);
 
-				align += host->align_sz;
+				align += SDHCI_ADMA2_ALIGN;
 			}
 		}
 	}
@@ -768,8 +771,7 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
 		if (unlikely(broken)) {
 			for_each_sg(data->sg, sg, data->sg_len, i) {
 				if (sg->length & 0x3) {
-					DBG("Reverting to PIO because of "
-						"transfer size (%d)\n",
+					DBG("Reverting to PIO because of transfer size (%d)\n",
 						sg->length);
 					host->flags &= ~SDHCI_REQ_USE_DMA;
 					break;
@@ -803,8 +805,7 @@ static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
 		if (unlikely(broken)) {
 			for_each_sg(data->sg, sg, data->sg_len, i) {
 				if (sg->offset & 0x3) {
-					DBG("Reverting to PIO because of "
-						"bad alignment\n");
+					DBG("Reverting to PIO because of bad alignment\n");
 					host->flags &= ~SDHCI_REQ_USE_DMA;
 					break;
 				}
@@ -1016,8 +1017,8 @@ void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 
 	while (sdhci_readl(host, SDHCI_PRESENT_STATE) & mask) {
 		if (timeout == 0) {
-			pr_err("%s: Controller never released "
-				"inhibit bit(s).\n", mmc_hostname(host->mmc));
+			pr_err("%s: Controller never released inhibit bit(s).\n",
+			       mmc_hostname(host->mmc));
 			sdhci_dumpregs(host);
 			cmd->error = -EIO;
 			tasklet_schedule(&host->finish_tasklet);
@@ -1254,8 +1255,8 @@ clock_set:
 	while (!((clk = sdhci_readw(host, SDHCI_CLOCK_CONTROL))
 		& SDHCI_CLOCK_INT_STABLE)) {
 		if (timeout == 0) {
-			pr_err("%s: Internal clock never "
-				"stabilised.\n", mmc_hostname(host->mmc));
+			pr_err("%s: Internal clock never stabilised.\n",
+			       mmc_hostname(host->mmc));
 			sdhci_dumpregs(host);
 			return;
 		}
@@ -1274,19 +1275,6 @@ static void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
 	struct mmc_host *mmc = host->mmc;
 	u8 pwr = 0;
 
-	if (!IS_ERR(mmc->supply.vmmc)) {
-		spin_unlock_irq(&host->lock);
-		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, vdd);
-		spin_lock_irq(&host->lock);
-
-		if (mode != MMC_POWER_OFF)
-			sdhci_writeb(host, SDHCI_POWER_ON, SDHCI_POWER_CONTROL);
-		else
-			sdhci_writeb(host, 0, SDHCI_POWER_CONTROL);
-
-		return;
-	}
-
 	if (mode != MMC_POWER_OFF) {
 		switch (1 << vdd) {
 		case MMC_VDD_165_195:
@@ -1301,7 +1289,9 @@ static void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
 			pwr = SDHCI_POWER_330;
 			break;
 		default:
-			BUG();
+			WARN(1, "%s: Invalid vdd %#x\n",
+			     mmc_hostname(host->mmc), vdd);
+			break;
 		}
 	}
 
@@ -1345,6 +1335,12 @@ static void sdhci_set_power(struct sdhci_host *host, unsigned char mode,
 		if (host->quirks & SDHCI_QUIRK_DELAY_AFTER_POWER)
 			mdelay(10);
 	}
+
+	if (!IS_ERR(mmc->supply.vmmc)) {
+		spin_unlock_irq(&host->lock);
+		mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, vdd);
+		spin_lock_irq(&host->lock);
+	}
 }
 
 /*****************************************************************************\
@@ -1364,7 +1360,7 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 	sdhci_runtime_pm_get(host);
 
 	/* Firstly check card presence */
-	present = sdhci_do_get_cd(host);
+	present = mmc->ops->get_cd(mmc);
 
 	spin_lock_irqsave(&host->lock, flags);
 
@@ -1540,8 +1536,8 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 			else if (ios->drv_type == MMC_SET_DRIVER_TYPE_D)
 				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_D;
 			else {
-				pr_warn("%s: invalid driver type, default to "
-					"driver type B\n", mmc_hostname(mmc));
+				pr_warn("%s: invalid driver type, default to driver type B\n",
+					mmc_hostname(mmc));
 				ctrl_2 |= SDHCI_CTRL_DRV_TYPE_B;
 			}
 
@@ -2015,10 +2011,7 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 		spin_lock_irqsave(&host->lock, flags);
 
 		if (!host->tuning_done) {
-			pr_info(DRIVER_NAME ": Timeout waiting for "
-				"Buffer Read Ready interrupt during tuning "
-				"procedure, falling back to fixed sampling "
-				"clock\n");
+			pr_info(DRIVER_NAME ": Timeout waiting for Buffer Read Ready interrupt during tuning procedure, falling back to fixed sampling clock\n");
 			ctrl = sdhci_readw(host, SDHCI_HOST_CONTROL2);
 			ctrl &= ~SDHCI_CTRL_TUNED_CLK;
 			ctrl &= ~SDHCI_CTRL_EXEC_TUNING;
@@ -2046,9 +2039,7 @@ static int sdhci_execute_tuning(struct mmc_host *mmc, u32 opcode)
 		sdhci_writew(host, ctrl, SDHCI_HOST_CONTROL2);
 	}
 	if (!(ctrl & SDHCI_CTRL_TUNED_CLK)) {
-		pr_info(DRIVER_NAME ": Tuning procedure"
-			" failed, falling back to fixed sampling"
-			" clock\n");
+		pr_info(DRIVER_NAME ": Tuning procedure failed, falling back to fixed sampling clock\n");
 		err = -EIO;
 	}
 
@@ -2293,8 +2284,8 @@ static void sdhci_timeout_timer(unsigned long data)
 	spin_lock_irqsave(&host->lock, flags);
 
 	if (host->mrq) {
-		pr_err("%s: Timeout waiting for hardware "
-			"interrupt.\n", mmc_hostname(host->mmc));
+		pr_err("%s: Timeout waiting for hardware interrupt.\n",
+		       mmc_hostname(host->mmc));
 		sdhci_dumpregs(host);
 
 		if (host->data) {
@@ -2325,9 +2316,8 @@ static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask, u32 *mask)
 	BUG_ON(intmask == 0);
 
 	if (!host->cmd) {
-		pr_err("%s: Got command interrupt 0x%08x even "
-			"though no command operation was in progress.\n",
-			mmc_hostname(host->mmc), (unsigned)intmask);
+		pr_err("%s: Got command interrupt 0x%08x even though no command operation was in progress.\n",
+		       mmc_hostname(host->mmc), (unsigned)intmask);
 		sdhci_dumpregs(host);
 		return;
 	}
@@ -2356,8 +2346,7 @@ static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask, u32 *mask)
 	 */
 	if (host->cmd->flags & MMC_RSP_BUSY) {
 		if (host->cmd->data)
-			DBG("Cannot wait for busy signal when also "
-				"doing a data transfer");
+			DBG("Cannot wait for busy signal when also doing a data transfer");
 		else if (!(host->quirks & SDHCI_QUIRK_NO_BUSY_IRQ)
 				&& !host->busy_handle) {
 			/* Mark that command complete before busy is ended */
@@ -2451,9 +2440,8 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 			}
 		}
 
-		pr_err("%s: Got data interrupt 0x%08x even "
-			"though no data operation was in progress.\n",
-			mmc_hostname(host->mmc), (unsigned)intmask);
+		pr_err("%s: Got data interrupt 0x%08x even though no data operation was in progress.\n",
+		       mmc_hostname(host->mmc), (unsigned)intmask);
 		sdhci_dumpregs(host);
 
 		return;
@@ -2760,7 +2748,7 @@ static int sdhci_runtime_pm_put(struct sdhci_host *host)
 
 static void sdhci_runtime_pm_bus_on(struct sdhci_host *host)
 {
-	if (host->runtime_suspended || host->bus_on)
+	if (host->bus_on)
 		return;
 	host->bus_on = true;
 	pm_runtime_get_noresume(host->mmc->parent);
@@ -2768,7 +2756,7 @@ static void sdhci_runtime_pm_bus_on(struct sdhci_host *host)
 
 static void sdhci_runtime_pm_bus_off(struct sdhci_host *host)
 {
-	if (host->runtime_suspended || !host->bus_on)
+	if (!host->bus_on)
 		return;
 	host->bus_on = false;
 	pm_runtime_put_noidle(host->mmc->parent);
@@ -2861,6 +2849,8 @@ struct sdhci_host *sdhci_alloc_host(struct device *dev,
 
 	host = mmc_priv(mmc);
 	host->mmc = mmc;
+	host->mmc_host_ops = sdhci_ops;
+	mmc->ops = &host->mmc_host_ops;
 
 	return host;
 }
@@ -2896,9 +2886,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	host->version = (host->version & SDHCI_SPEC_VER_MASK)
 				>> SDHCI_SPEC_VER_SHIFT;
 	if (host->version > SDHCI_SPEC_300) {
-		pr_err("%s: Unknown controller version (%d). "
-			"You may experience problems.\n", mmc_hostname(mmc),
-			host->version);
+		pr_err("%s: Unknown controller version (%d). You may experience problems.\n",
+		       mmc_hostname(mmc), host->version);
 	}
 
 	caps[0] = (host->quirks & SDHCI_QUIRK_MISSING_CAPS) ? host->caps :
@@ -2967,24 +2956,17 @@ int sdhci_add_host(struct sdhci_host *host)
 		if (host->flags & SDHCI_USE_64_BIT_DMA) {
 			host->adma_table_sz = (SDHCI_MAX_SEGS * 2 + 1) *
 					      SDHCI_ADMA2_64_DESC_SZ;
-			host->align_buffer_sz = SDHCI_MAX_SEGS *
-						SDHCI_ADMA2_64_ALIGN;
 			host->desc_sz = SDHCI_ADMA2_64_DESC_SZ;
-			host->align_sz = SDHCI_ADMA2_64_ALIGN;
-			host->align_mask = SDHCI_ADMA2_64_ALIGN - 1;
 		} else {
 			host->adma_table_sz = (SDHCI_MAX_SEGS * 2 + 1) *
 					      SDHCI_ADMA2_32_DESC_SZ;
-			host->align_buffer_sz = SDHCI_MAX_SEGS *
-						SDHCI_ADMA2_32_ALIGN;
 			host->desc_sz = SDHCI_ADMA2_32_DESC_SZ;
-			host->align_sz = SDHCI_ADMA2_32_ALIGN;
-			host->align_mask = SDHCI_ADMA2_32_ALIGN - 1;
 		}
 		host->adma_table = dma_alloc_coherent(mmc_dev(mmc),
 						      host->adma_table_sz,
 						      &host->adma_addr,
 						      GFP_KERNEL);
+		host->align_buffer_sz = SDHCI_MAX_SEGS * SDHCI_ADMA2_ALIGN;
 		host->align_buffer = kmalloc(host->align_buffer_sz, GFP_KERNEL);
 		if (!host->adma_table || !host->align_buffer) {
 			if (host->adma_table)
@@ -2998,7 +2980,7 @@ int sdhci_add_host(struct sdhci_host *host)
 			host->flags &= ~SDHCI_USE_ADMA;
 			host->adma_table = NULL;
 			host->align_buffer = NULL;
-		} else if (host->adma_addr & host->align_mask) {
+		} else if (host->adma_addr & (SDHCI_ADMA2_DESC_ALIGN - 1)) {
 			pr_warn("%s: unable to allocate aligned ADMA descriptor\n",
 				mmc_hostname(mmc));
 			host->flags &= ~SDHCI_USE_ADMA;
@@ -3031,8 +3013,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (host->max_clk == 0 || host->quirks &
 			SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN) {
 		if (!host->ops->get_max_clock) {
-			pr_err("%s: Hardware doesn't specify base clock "
-			       "frequency.\n", mmc_hostname(mmc));
+			pr_err("%s: Hardware doesn't specify base clock frequency.\n",
+			       mmc_hostname(mmc));
 			return -ENODEV;
 		}
 		host->max_clk = host->ops->get_max_clock(host);
@@ -3057,7 +3039,6 @@ int sdhci_add_host(struct sdhci_host *host)
 	/*
 	 * Set host parameters.
 	 */
-	mmc->ops = &sdhci_ops;
 	max_clk = host->max_clk;
 
 	if (host->ops->get_min_clock)
@@ -3294,8 +3275,8 @@ int sdhci_add_host(struct sdhci_host *host)
 		mmc->ocr_avail_mmc &= host->ocr_avail_mmc;
 
 	if (mmc->ocr_avail == 0) {
-		pr_err("%s: Hardware doesn't report any "
-			"support voltages.\n", mmc_hostname(mmc));
+		pr_err("%s: Hardware doesn't report any support voltages.\n",
+		       mmc_hostname(mmc));
 		return -ENODEV;
 	}
 
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 9d4aa31b683a..0115e9907bf8 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -272,22 +272,27 @@
 /* ADMA2 32-bit DMA descriptor size */
 #define SDHCI_ADMA2_32_DESC_SZ	8
 
-/* ADMA2 32-bit DMA alignment */
-#define SDHCI_ADMA2_32_ALIGN	4
-
 /* ADMA2 32-bit descriptor */
 struct sdhci_adma2_32_desc {
 	__le16	cmd;
 	__le16	len;
 	__le32	addr;
-}  __packed __aligned(SDHCI_ADMA2_32_ALIGN);
+}  __packed __aligned(4);
+
+/* ADMA2 data alignment */
+#define SDHCI_ADMA2_ALIGN	4
+#define SDHCI_ADMA2_MASK	(SDHCI_ADMA2_ALIGN - 1)
+
+/*
+ * ADMA2 descriptor alignment.  Some controllers (e.g. Intel) require 8 byte
+ * alignment for the descriptor table even in 32-bit DMA mode.  Memory
+ * allocation is at least 8 byte aligned anyway, so just stipulate 8 always.
+ */
+#define SDHCI_ADMA2_DESC_ALIGN	8
 
 /* ADMA2 64-bit DMA descriptor size */
 #define SDHCI_ADMA2_64_DESC_SZ	12
 
-/* ADMA2 64-bit DMA alignment */
-#define SDHCI_ADMA2_64_ALIGN	8
-
 /*
  * ADMA2 64-bit descriptor. Note 12-byte descriptor can't always be 8-byte
  * aligned.
@@ -425,6 +430,7 @@ struct sdhci_host {
 
 	/* Internal data */
 	struct mmc_host *mmc;	/* MMC structure */
+	struct mmc_host_ops mmc_host_ops;	/* MMC host ops */
 	u64 dma_mask;		/* custom DMA mask */
 
 #if defined(CONFIG_LEDS_CLASS) || defined(CONFIG_LEDS_CLASS_MODULE)
@@ -482,8 +488,6 @@ struct sdhci_host {
 	dma_addr_t align_addr;	/* Mapped bounce buffer */
 
 	unsigned int desc_sz;	/* ADMA descriptor size */
-	unsigned int align_sz;	/* ADMA alignment */
-	unsigned int align_mask;	/* ADMA alignment mask */
 
 	struct tasklet_struct finish_tasklet;	/* Tasklet structures */
 
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index ad9ffea7d659..6234eab38ff3 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -397,38 +397,26 @@ static void sh_mmcif_start_dma_tx(struct sh_mmcif_host *host)
 }
 
 static struct dma_chan *
-sh_mmcif_request_dma_one(struct sh_mmcif_host *host,
-			 struct sh_mmcif_plat_data *pdata,
-			 enum dma_transfer_direction direction)
+sh_mmcif_request_dma_pdata(struct sh_mmcif_host *host, uintptr_t slave_id)
 {
-	struct dma_slave_config cfg = { 0, };
-	struct dma_chan *chan;
-	void *slave_data = NULL;
-	struct resource *res;
-	struct device *dev = sh_mmcif_host_to_dev(host);
 	dma_cap_mask_t mask;
-	int ret;
 
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
+	if (slave_id <= 0)
+		return NULL;
 
-	if (pdata)
-		slave_data = direction == DMA_MEM_TO_DEV ?
-			(void *)pdata->slave_id_tx :
-			(void *)pdata->slave_id_rx;
-
-	chan = dma_request_slave_channel_compat(mask, shdma_chan_filter,
-				slave_data, dev,
-				direction == DMA_MEM_TO_DEV ? "tx" : "rx");
-
-	dev_dbg(dev, "%s: %s: got channel %p\n", __func__,
-		direction == DMA_MEM_TO_DEV ? "TX" : "RX", chan);
+	return dma_request_channel(mask, shdma_chan_filter, (void *)slave_id);
+}
 
-	if (!chan)
-		return NULL;
+static int sh_mmcif_dma_slave_config(struct sh_mmcif_host *host,
+				     struct dma_chan *chan,
+				     enum dma_transfer_direction direction)
+{
+	struct resource *res;
+	struct dma_slave_config cfg = { 0, };
 
 	res = platform_get_resource(host->pd, IORESOURCE_MEM, 0);
-
 	cfg.direction = direction;
 
 	if (direction == DMA_DEV_TO_MEM) {
@@ -439,38 +427,42 @@ sh_mmcif_request_dma_one(struct sh_mmcif_host *host,
 		cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
 	}
 
-	ret = dmaengine_slave_config(chan, &cfg);
-	if (ret < 0) {
-		dma_release_channel(chan);
-		return NULL;
-	}
-
-	return chan;
+	return dmaengine_slave_config(chan, &cfg);
 }
 
-static void sh_mmcif_request_dma(struct sh_mmcif_host *host,
-				 struct sh_mmcif_plat_data *pdata)
+static void sh_mmcif_request_dma(struct sh_mmcif_host *host)
 {
 	struct device *dev = sh_mmcif_host_to_dev(host);
 	host->dma_active = false;
 
-	if (pdata) {
-		if (pdata->slave_id_tx <= 0 || pdata->slave_id_rx <= 0)
-			return;
-	} else if (!dev->of_node) {
-		return;
+	/* We can only either use DMA for both Tx and Rx or not use it at all */
+	if (IS_ENABLED(CONFIG_SUPERH) && dev->platform_data) {
+		struct sh_mmcif_plat_data *pdata = dev->platform_data;
+
+		host->chan_tx = sh_mmcif_request_dma_pdata(host,
+							pdata->slave_id_tx);
+		host->chan_rx = sh_mmcif_request_dma_pdata(host,
+							pdata->slave_id_rx);
+	} else {
+		host->chan_tx = dma_request_slave_channel(dev, "tx");
+		host->chan_rx = dma_request_slave_channel(dev, "rx");
 	}
+	dev_dbg(dev, "%s: got channel TX %p RX %p\n", __func__, host->chan_tx,
+		host->chan_rx);
 
-	/* We can only either use DMA for both Tx and Rx or not use it at all */
-	host->chan_tx = sh_mmcif_request_dma_one(host, pdata, DMA_MEM_TO_DEV);
-	if (!host->chan_tx)
-		return;
+	if (!host->chan_tx || !host->chan_rx ||
+	    sh_mmcif_dma_slave_config(host, host->chan_tx, DMA_MEM_TO_DEV) ||
+	    sh_mmcif_dma_slave_config(host, host->chan_rx, DMA_DEV_TO_MEM))
+		goto error;
 
-	host->chan_rx = sh_mmcif_request_dma_one(host, pdata, DMA_DEV_TO_MEM);
-	if (!host->chan_rx) {
+	return;
+
+error:
+	if (host->chan_tx)
 		dma_release_channel(host->chan_tx);
-		host->chan_tx = NULL;
-	}
+	if (host->chan_rx)
+		dma_release_channel(host->chan_rx);
+	host->chan_tx = host->chan_rx = NULL;
 }
 
 static void sh_mmcif_release_dma(struct sh_mmcif_host *host)
@@ -1102,7 +1094,7 @@ static void sh_mmcif_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	if (ios->power_mode == MMC_POWER_UP) {
 		if (!host->card_present) {
 			/* See if we also get DMA */
-			sh_mmcif_request_dma(host, dev->platform_data);
+			sh_mmcif_request_dma(host);
 			host->card_present = true;
 		}
 		sh_mmcif_set_power(host, ios);
diff --git a/drivers/mmc/host/tmio_mmc_dma.c b/drivers/mmc/host/tmio_mmc_dma.c
index e4b05dbb9ca8..4a0d6b80eaa3 100644
--- a/drivers/mmc/host/tmio_mmc_dma.c
+++ b/drivers/mmc/host/tmio_mmc_dma.c
@@ -94,9 +94,9 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
 			desc = NULL;
 			ret = cookie;
 		}
+		dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
+			__func__, host->sg_len, ret, cookie, host->mrq);
 	}
-	dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
-		__func__, host->sg_len, ret, cookie, host->mrq);
 
 pio:
 	if (!desc) {
@@ -116,8 +116,8 @@ pio:
 			 "DMA failed: %d, falling back to PIO\n", ret);
 	}
 
-	dev_dbg(&host->pdev->dev, "%s(): desc %p, cookie %d, sg[%d]\n", __func__,
-		desc, cookie, host->sg_len);
+	dev_dbg(&host->pdev->dev, "%s(): desc %p, sg[%d]\n", __func__,
+		desc, host->sg_len);
 }
 
 static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
@@ -174,9 +174,9 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
 			desc = NULL;
 			ret = cookie;
 		}
+		dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
+			__func__, host->sg_len, ret, cookie, host->mrq);
 	}
-	dev_dbg(&host->pdev->dev, "%s(): mapped %d -> %d, cookie %d, rq %p\n",
-		__func__, host->sg_len, ret, cookie, host->mrq);
 
 pio:
 	if (!desc) {
@@ -196,8 +196,7 @@ pio:
 			 "DMA failed: %d, falling back to PIO\n", ret);
 	}
 
-	dev_dbg(&host->pdev->dev, "%s(): desc %p, cookie %d\n", __func__,
-		desc, cookie);
+	dev_dbg(&host->pdev->dev, "%s(): desc %p\n", __func__, desc);
 }
 
 void tmio_mmc_start_dma(struct tmio_mmc_host *host,
diff --git a/drivers/mmc/host/usdhi6rol0.c b/drivers/mmc/host/usdhi6rol0.c
index 4498e92116b8..b47122d3e8d8 100644
--- a/drivers/mmc/host/usdhi6rol0.c
+++ b/drivers/mmc/host/usdhi6rol0.c
@@ -1634,7 +1634,7 @@ static void usdhi6_timeout_work(struct work_struct *work)
 	struct usdhi6_host *host = container_of(d, struct usdhi6_host, timeout_work);
 	struct mmc_request *mrq = host->mrq;
 	struct mmc_data *data = mrq ? mrq->data : NULL;
-	struct scatterlist *sg = host->sg ?: data->sg;
+	struct scatterlist *sg;
 
 	dev_warn(mmc_dev(host->mmc),
 		 "%s timeout wait %u CMD%d: IRQ 0x%08x:0x%08x, last IRQ 0x%08x\n",
@@ -1666,6 +1666,7 @@ static void usdhi6_timeout_work(struct work_struct *work)
 	case USDHI6_WAIT_FOR_MWRITE:
 	case USDHI6_WAIT_FOR_READ:
 	case USDHI6_WAIT_FOR_WRITE:
+		sg = host->sg ?: data->sg;
 		dev_dbg(mmc_dev(host->mmc),
 			"%c: page #%u @ +0x%zx %ux%u in SG%u. Current SG %u bytes @ %u\n",
 			data->flags & MMC_DATA_READ ? 'R' : 'W', host->page_idx,
diff --git a/drivers/mtd/Kconfig b/drivers/mtd/Kconfig
index a03ad2951c7b..42cc953309f1 100644
--- a/drivers/mtd/Kconfig
+++ b/drivers/mtd/Kconfig
@@ -112,7 +112,7 @@ config MTD_CMDLINE_PARTS
 
 config MTD_AFS_PARTS
 	tristate "ARM Firmware Suite partition parsing"
-	depends on ARM
+	depends on (ARM || ARM64)
 	---help---
 	  The ARM Firmware Suite allows the user to divide flash devices into
 	  multiple 'images'. Each such image has a header containing its name
diff --git a/drivers/mtd/afs.c b/drivers/mtd/afs.c
index 96a33e3f7b00..d61b7edfc938 100644
--- a/drivers/mtd/afs.c
+++ b/drivers/mtd/afs.c
@@ -34,7 +34,9 @@
 #include <linux/mtd/map.h>
 #include <linux/mtd/partitions.h>
 
-struct footer_struct {
+#define AFSV1_FOOTER_MAGIC 0xA0FFFF9F
+
+struct footer_v1 {
 	u32 image_info_base;	/* Address of first word of ImageFooter  */
 	u32 image_start;	/* Start of area reserved by this footer */
 	u32 signature;		/* 'Magic' number proves it's a footer   */
@@ -42,7 +44,7 @@ struct footer_struct {
 	u32 checksum;		/* Just this structure                   */
 };
 
-struct image_info_struct {
+struct image_info_v1 {
 	u32 bootFlags;		/* Boot flags, compression etc.          */
 	u32 imageNumber;	/* Unique number, selects for boot etc.  */
 	u32 loadAddress;	/* Address program should be loaded to   */
@@ -67,10 +69,10 @@ static u32 word_sum(void *words, int num)
 }
 
 static int
-afs_read_footer(struct mtd_info *mtd, u_int *img_start, u_int *iis_start,
-		u_int off, u_int mask)
+afs_read_footer_v1(struct mtd_info *mtd, u_int *img_start, u_int *iis_start,
+		   u_int off, u_int mask)
 {
-	struct footer_struct fs;
+	struct footer_v1 fs;
 	u_int ptr = off + mtd->erasesize - sizeof(fs);
 	size_t sz;
 	int ret;
@@ -85,25 +87,23 @@ afs_read_footer(struct mtd_info *mtd, u_int *img_start, u_int *iis_start,
 		return ret;
 	}
 
-	ret = 1;
-
 	/*
 	 * Does it contain the magic number?
 	 */
-	if (fs.signature != 0xa0ffff9f)
-		ret = 0;
+	if (fs.signature != AFSV1_FOOTER_MAGIC)
+		return 0;
 
 	/*
 	 * Check the checksum.
 	 */
 	if (word_sum(&fs, sizeof(fs) / sizeof(u32)) != 0xffffffff)
-		ret = 0;
+		return 0;
 
 	/*
 	 * Don't touch the SIB.
 	 */
 	if (fs.type == 2)
-		ret = 0;
+		return 0;
 
 	*iis_start = fs.image_info_base & mask;
 	*img_start = fs.image_start & mask;
@@ -113,20 +113,20 @@ afs_read_footer(struct mtd_info *mtd, u_int *img_start, u_int *iis_start,
 	 * be located after the footer structure.
 	 */
 	if (*iis_start >= ptr)
-		ret = 0;
+		return 0;
 
 	/*
 	 * Check the start of this image.  The image
 	 * data can not be located after this block.
 	 */
 	if (*img_start > off)
-		ret = 0;
+		return 0;
 
-	return ret;
+	return 1;
 }
 
 static int
-afs_read_iis(struct mtd_info *mtd, struct image_info_struct *iis, u_int ptr)
+afs_read_iis_v1(struct mtd_info *mtd, struct image_info_v1 *iis, u_int ptr)
 {
 	size_t sz;
 	int ret, i;
@@ -162,7 +162,7 @@ afs_read_iis(struct mtd_info *mtd, struct image_info_struct *iis, u_int ptr)
 }
 
 static int parse_afs_partitions(struct mtd_info *mtd,
-				struct mtd_partition **pparts,
+				const struct mtd_partition **pparts,
 				struct mtd_part_parser_data *data)
 {
 	struct mtd_partition *parts;
@@ -182,24 +182,23 @@ static int parse_afs_partitions(struct mtd_info *mtd,
 	 * the strings.
 	 */
 	for (idx = off = sz = 0; off < mtd->size; off += mtd->erasesize) {
-		struct image_info_struct iis;
+		struct image_info_v1 iis;
 		u_int iis_ptr, img_ptr;
 
-		ret = afs_read_footer(mtd, &img_ptr, &iis_ptr, off, mask);
-		if (ret < 0)
-			break;
-		if (ret == 0)
-			continue;
-
-		ret = afs_read_iis(mtd, &iis, iis_ptr);
+		ret = afs_read_footer_v1(mtd, &img_ptr, &iis_ptr, off, mask);
 		if (ret < 0)
 			break;
-		if (ret == 0)
-			continue;
-
-		sz += sizeof(struct mtd_partition);
-		sz += strlen(iis.name) + 1;
-		idx += 1;
+		if (ret) {
+			ret = afs_read_iis_v1(mtd, &iis, iis_ptr);
+			if (ret < 0)
+				break;
+			if (ret == 0)
+				continue;
+
+			sz += sizeof(struct mtd_partition);
+			sz += strlen(iis.name) + 1;
+			idx += 1;
+		}
 	}
 
 	if (!sz)
@@ -215,18 +214,18 @@ static int parse_afs_partitions(struct mtd_info *mtd,
 	 * Identify the partitions
 	 */
 	for (idx = off = 0; off < mtd->size; off += mtd->erasesize) {
-		struct image_info_struct iis;
+		struct image_info_v1 iis;
 		u_int iis_ptr, img_ptr;
 
 		/* Read the footer. */
-		ret = afs_read_footer(mtd, &img_ptr, &iis_ptr, off, mask);
+		ret = afs_read_footer_v1(mtd, &img_ptr, &iis_ptr, off, mask);
 		if (ret < 0)
 			break;
 		if (ret == 0)
 			continue;
 
 		/* Read the image info block */
-		ret = afs_read_iis(mtd, &iis, iis_ptr);
+		ret = afs_read_iis_v1(mtd, &iis, iis_ptr);
 		if (ret < 0)
 			break;
 		if (ret == 0)
@@ -257,25 +256,10 @@ static int parse_afs_partitions(struct mtd_info *mtd,
 }
 
 static struct mtd_part_parser afs_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = parse_afs_partitions,
 	.name = "afs",
 };
-
-static int __init afs_parser_init(void)
-{
-	register_mtd_parser(&afs_parser);
-	return 0;
-}
-
-static void __exit afs_parser_exit(void)
-{
-	deregister_mtd_parser(&afs_parser);
-}
-
-module_init(afs_parser_init);
-module_exit(afs_parser_exit);
-
+module_mtd_part_parser(afs_parser);
 
 MODULE_AUTHOR("ARM Ltd");
 MODULE_DESCRIPTION("ARM Firmware Suite partition parser");
diff --git a/drivers/mtd/ar7part.c b/drivers/mtd/ar7part.c
index 7c9172ad2621..90575deff0ae 100644
--- a/drivers/mtd/ar7part.c
+++ b/drivers/mtd/ar7part.c
@@ -43,7 +43,7 @@ struct ar7_bin_rec {
 };
 
 static int create_mtd_partitions(struct mtd_info *master,
-				 struct mtd_partition **pparts,
+				 const struct mtd_partition **pparts,
 				 struct mtd_part_parser_data *data)
 {
 	struct ar7_bin_rec header;
@@ -132,24 +132,10 @@ static int create_mtd_partitions(struct mtd_info *master,
 }
 
 static struct mtd_part_parser ar7_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = create_mtd_partitions,
 	.name = "ar7part",
 };
-
-static int __init ar7_parser_init(void)
-{
-	register_mtd_parser(&ar7_parser);
-	return 0;
-}
-
-static void __exit ar7_parser_exit(void)
-{
-	deregister_mtd_parser(&ar7_parser);
-}
-
-module_init(ar7_parser_init);
-module_exit(ar7_parser_exit);
+module_mtd_part_parser(ar7_parser);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR(	"Felix Fietkau <nbd@openwrt.org>, "
diff --git a/drivers/mtd/bcm47xxpart.c b/drivers/mtd/bcm47xxpart.c
index c0720c1ee4c9..8282f47bcf5d 100644
--- a/drivers/mtd/bcm47xxpart.c
+++ b/drivers/mtd/bcm47xxpart.c
@@ -82,7 +82,7 @@ out_default:
 }
 
 static int bcm47xxpart_parse(struct mtd_info *master,
-			     struct mtd_partition **pparts,
+			     const struct mtd_partition **pparts,
 			     struct mtd_part_parser_data *data)
 {
 	struct mtd_partition *parts;
@@ -313,24 +313,10 @@ static int bcm47xxpart_parse(struct mtd_info *master,
 };
 
 static struct mtd_part_parser bcm47xxpart_mtd_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = bcm47xxpart_parse,
 	.name = "bcm47xxpart",
 };
-
-static int __init bcm47xxpart_init(void)
-{
-	register_mtd_parser(&bcm47xxpart_mtd_parser);
-	return 0;
-}
-
-static void __exit bcm47xxpart_exit(void)
-{
-	deregister_mtd_parser(&bcm47xxpart_mtd_parser);
-}
-
-module_init(bcm47xxpart_init);
-module_exit(bcm47xxpart_exit);
+module_mtd_part_parser(bcm47xxpart_mtd_parser);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MTD partitioning for BCM47XX flash memories");
diff --git a/drivers/mtd/bcm63xxpart.c b/drivers/mtd/bcm63xxpart.c
index b2443f7031c9..cec3188a170d 100644
--- a/drivers/mtd/bcm63xxpart.c
+++ b/drivers/mtd/bcm63xxpart.c
@@ -24,6 +24,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/bcm963xx_tag.h>
 #include <linux/crc32.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -34,11 +35,8 @@
 #include <linux/mtd/partitions.h>
 
 #include <asm/mach-bcm63xx/bcm63xx_nvram.h>
-#include <asm/mach-bcm63xx/bcm963xx_tag.h>
 #include <asm/mach-bcm63xx/board_bcm963xx.h>
 
-#define BCM63XX_EXTENDED_SIZE	0xBFC00000	/* Extended flash address */
-
 #define BCM63XX_CFE_BLOCK_SIZE	SZ_64K		/* always at least 64KiB */
 
 #define BCM63XX_CFE_MAGIC_OFFSET 0x4e0
@@ -68,7 +66,7 @@ static int bcm63xx_detect_cfe(struct mtd_info *master)
 }
 
 static int bcm63xx_parse_cfe_partitions(struct mtd_info *master,
-					struct mtd_partition **pparts,
+					const struct mtd_partition **pparts,
 					struct mtd_part_parser_data *data)
 {
 	/* CFE, NVRAM and global Linux are always present */
@@ -123,8 +121,8 @@ static int bcm63xx_parse_cfe_partitions(struct mtd_info *master,
 		pr_info("CFE boot tag found with version %s and board type %s\n",
 			tagversion, boardid);
 
-		kerneladdr = kerneladdr - BCM63XX_EXTENDED_SIZE;
-		rootfsaddr = rootfsaddr - BCM63XX_EXTENDED_SIZE;
+		kerneladdr = kerneladdr - BCM963XX_EXTENDED_SIZE;
+		rootfsaddr = rootfsaddr - BCM963XX_EXTENDED_SIZE;
 		spareaddr = roundup(totallen, master->erasesize) + cfelen;
 
 		if (rootfsaddr < kerneladdr) {
@@ -214,24 +212,10 @@ static int bcm63xx_parse_cfe_partitions(struct mtd_info *master,
 };
 
 static struct mtd_part_parser bcm63xx_cfe_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = bcm63xx_parse_cfe_partitions,
 	.name = "bcm63xxpart",
 };
-
-static int __init bcm63xx_cfe_parser_init(void)
-{
-	register_mtd_parser(&bcm63xx_cfe_parser);
-	return 0;
-}
-
-static void __exit bcm63xx_cfe_parser_exit(void)
-{
-	deregister_mtd_parser(&bcm63xx_cfe_parser);
-}
-
-module_init(bcm63xx_cfe_parser_init);
-module_exit(bcm63xx_cfe_parser_exit);
+module_mtd_part_parser(bcm63xx_cfe_parser);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Daniel Dickinson <openwrt@cshore.neomailbox.net>");
diff --git a/drivers/mtd/chips/Kconfig b/drivers/mtd/chips/Kconfig
index 54479c481a7a..3b3dabce58de 100644
--- a/drivers/mtd/chips/Kconfig
+++ b/drivers/mtd/chips/Kconfig
@@ -67,6 +67,10 @@ endchoice
 config MTD_CFI_GEOMETRY
 	bool "Specific CFI Flash geometry selection"
 	depends on MTD_CFI_ADV_OPTIONS
+	select MTD_MAP_BANK_WIDTH_1 if  !(MTD_MAP_BANK_WIDTH_2 || \
+		 MTD_MAP_BANK_WIDTH_4  || MTD_MAP_BANK_WIDTH_8 || \
+		 MTD_MAP_BANK_WIDTH_16 || MTD_MAP_BANK_WIDTH_32)
+	select MTD_CFI_I1 if !(MTD_CFI_I2 || MTD_CFI_I4 || MTD_CFI_I8)
 	help
 	  This option does not affect the code directly, but will enable
 	  some other configuration options which would allow you to reduce
diff --git a/drivers/mtd/chips/cfi_cmdset_0001.c b/drivers/mtd/chips/cfi_cmdset_0001.c
index 286b97a304cf..5e1b68cbcd0a 100644
--- a/drivers/mtd/chips/cfi_cmdset_0001.c
+++ b/drivers/mtd/chips/cfi_cmdset_0001.c
@@ -596,7 +596,7 @@ static struct mtd_info *cfi_intelext_setup(struct mtd_info *mtd)
 	mtd->size = devsize * cfi->numchips;
 
 	mtd->numeraseregions = cfi->cfiq->NumEraseRegions * cfi->numchips;
-	mtd->eraseregions = kmalloc(sizeof(struct mtd_erase_region_info)
+	mtd->eraseregions = kzalloc(sizeof(struct mtd_erase_region_info)
 			* mtd->numeraseregions, GFP_KERNEL);
 	if (!mtd->eraseregions)
 		goto setup_err;
@@ -614,6 +614,8 @@ static struct mtd_info *cfi_intelext_setup(struct mtd_info *mtd)
 			mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].erasesize = ersize;
 			mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].numblocks = ernum;
 			mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].lockmap = kmalloc(ernum / 8 + 1, GFP_KERNEL);
+			if (!mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].lockmap)
+				goto setup_err;
 		}
 		offset += (ersize * ernum);
 	}
@@ -650,6 +652,10 @@ static struct mtd_info *cfi_intelext_setup(struct mtd_info *mtd)
 	return mtd;
 
  setup_err:
+	if (mtd->eraseregions)
+		for (i=0; i<cfi->cfiq->NumEraseRegions; i++)
+			for (j=0; j<cfi->numchips; j++)
+				kfree(mtd->eraseregions[(j*cfi->cfiq->NumEraseRegions)+i].lockmap);
 	kfree(mtd->eraseregions);
 	kfree(mtd);
 	kfree(cfi->cmdset_priv);
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index c3624eb571d1..9dca881bb378 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -615,11 +615,9 @@ struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
 
 				for (i=0; i<cfi->cfiq->NumEraseRegions / 2; i++) {
 					int j = (cfi->cfiq->NumEraseRegions-1)-i;
-					__u32 swap;
 
-					swap = cfi->cfiq->EraseRegionInfo[i];
-					cfi->cfiq->EraseRegionInfo[i] = cfi->cfiq->EraseRegionInfo[j];
-					cfi->cfiq->EraseRegionInfo[j] = swap;
+					swap(cfi->cfiq->EraseRegionInfo[i],
+					     cfi->cfiq->EraseRegionInfo[j]);
 				}
 			}
 			/* Set the default CFI lock/unlock addresses */
diff --git a/drivers/mtd/cmdlinepart.c b/drivers/mtd/cmdlinepart.c
index 08f62987cc37..fbd5affc0acf 100644
--- a/drivers/mtd/cmdlinepart.c
+++ b/drivers/mtd/cmdlinepart.c
@@ -304,7 +304,7 @@ static int mtdpart_setup_real(char *s)
  * the first one in the chain if a NULL mtd_id is passed in.
  */
 static int parse_cmdline_partitions(struct mtd_info *master,
-				    struct mtd_partition **pparts,
+				    const struct mtd_partition **pparts,
 				    struct mtd_part_parser_data *data)
 {
 	unsigned long long offset;
@@ -382,7 +382,6 @@ static int __init mtdpart_setup(char *s)
 __setup("mtdparts=", mtdpart_setup);
 
 static struct mtd_part_parser cmdline_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = parse_cmdline_partitions,
 	.name = "cmdlinepart",
 };
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index fe9ceb7b5405..c9c3b7fa3051 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -152,22 +152,6 @@ static int m25p80_read(struct spi_nor *nor, loff_t from, size_t len,
 	return 0;
 }
 
-static int m25p80_erase(struct spi_nor *nor, loff_t offset)
-{
-	struct m25p *flash = nor->priv;
-
-	dev_dbg(nor->dev, "%dKiB at 0x%08x\n",
-		flash->spi_nor.mtd.erasesize / 1024, (u32)offset);
-
-	/* Set up command buffer. */
-	flash->command[0] = nor->erase_opcode;
-	m25p_addr2cmd(nor, offset, flash->command);
-
-	spi_write(flash->spi, flash->command, m25p_cmdsz(nor));
-
-	return 0;
-}
-
 /*
  * board specific setup should have ensured the SPI clock used here
  * matches what the READ command supports, at least until this driver
@@ -175,12 +159,11 @@ static int m25p80_erase(struct spi_nor *nor, loff_t offset)
  */
 static int m25p_probe(struct spi_device *spi)
 {
-	struct mtd_part_parser_data	ppdata;
 	struct flash_platform_data	*data;
 	struct m25p *flash;
 	struct spi_nor *nor;
 	enum read_mode mode = SPI_NOR_NORMAL;
-	char *flash_name = NULL;
+	char *flash_name;
 	int ret;
 
 	data = dev_get_platdata(&spi->dev);
@@ -194,12 +177,11 @@ static int m25p_probe(struct spi_device *spi)
 	/* install the hooks */
 	nor->read = m25p80_read;
 	nor->write = m25p80_write;
-	nor->erase = m25p80_erase;
 	nor->write_reg = m25p80_write_reg;
 	nor->read_reg = m25p80_read_reg;
 
 	nor->dev = &spi->dev;
-	nor->flash_node = spi->dev.of_node;
+	spi_nor_set_flash_node(nor, spi->dev.of_node);
 	nor->priv = flash;
 
 	spi_set_drvdata(spi, flash);
@@ -220,6 +202,8 @@ static int m25p_probe(struct spi_device *spi)
 	 */
 	if (data && data->type)
 		flash_name = data->type;
+	else if (!strcmp(spi->modalias, "spi-nor"))
+		flash_name = NULL; /* auto-detect */
 	else
 		flash_name = spi->modalias;
 
@@ -227,11 +211,8 @@ static int m25p_probe(struct spi_device *spi)
 	if (ret)
 		return ret;
 
-	ppdata.of_node = spi->dev.of_node;
-
-	return mtd_device_parse_register(&nor->mtd, NULL, &ppdata,
-			data ? data->parts : NULL,
-			data ? data->nr_parts : 0);
+	return mtd_device_register(&nor->mtd, data ? data->parts : NULL,
+				   data ? data->nr_parts : 0);
 }
 
 
@@ -257,14 +238,21 @@ static int m25p_remove(struct spi_device *spi)
  */
 static const struct spi_device_id m25p_ids[] = {
 	/*
+	 * Allow non-DT platform devices to bind to the "spi-nor" modalias, and
+	 * hack around the fact that the SPI core does not provide uevent
+	 * matching for .of_match_table
+	 */
+	{"spi-nor"},
+
+	/*
 	 * Entries not used in DTs that should be safe to drop after replacing
-	 * them with "nor-jedec" in platform data.
+	 * them with "spi-nor" in platform data.
 	 */
 	{"s25sl064a"},	{"w25x16"},	{"m25p10"},	{"m25px64"},
 
 	/*
-	 * Entries that were used in DTs without "nor-jedec" fallback and should
-	 * be kept for backward compatibility.
+	 * Entries that were used in DTs without "jedec,spi-nor" fallback and
+	 * should be kept for backward compatibility.
 	 */
 	{"at25df321a"},	{"at25df641"},	{"at26df081a"},
 	{"mr25h256"},
diff --git a/drivers/mtd/devices/mtd_dataflash.c b/drivers/mtd/devices/mtd_dataflash.c
index e4a88715a844..f9e9bd1cfaa0 100644
--- a/drivers/mtd/devices/mtd_dataflash.c
+++ b/drivers/mtd/devices/mtd_dataflash.c
@@ -624,7 +624,6 @@ static int add_dataflash_otp(struct spi_device *spi, char *name, int nr_pages,
 {
 	struct dataflash		*priv;
 	struct mtd_info			*device;
-	struct mtd_part_parser_data	ppdata;
 	struct flash_platform_data	*pdata = dev_get_platdata(&spi->dev);
 	char				*otp_tag = "";
 	int				err = 0;
@@ -656,6 +655,7 @@ static int add_dataflash_otp(struct spi_device *spi, char *name, int nr_pages,
 	device->priv = priv;
 
 	device->dev.parent = &spi->dev;
+	mtd_set_of_node(device, spi->dev.of_node);
 
 	if (revision >= 'c')
 		otp_tag = otp_setup(device, revision);
@@ -665,8 +665,7 @@ static int add_dataflash_otp(struct spi_device *spi, char *name, int nr_pages,
 			pagesize, otp_tag);
 	spi_set_drvdata(spi, priv);
 
-	ppdata.of_node = spi->dev.of_node;
-	err = mtd_device_parse_register(device, NULL, &ppdata,
+	err = mtd_device_register(device,
 			pdata ? pdata->parts : NULL,
 			pdata ? pdata->nr_parts : 0);
 
diff --git a/drivers/mtd/devices/spear_smi.c b/drivers/mtd/devices/spear_smi.c
index 64c7458344d4..dd5069876537 100644
--- a/drivers/mtd/devices/spear_smi.c
+++ b/drivers/mtd/devices/spear_smi.c
@@ -810,7 +810,6 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 				 u32 bank, struct device_node *np)
 {
 	struct spear_smi *dev = platform_get_drvdata(pdev);
-	struct mtd_part_parser_data ppdata = {};
 	struct spear_smi_flash_info *flash_info;
 	struct spear_smi_plat_data *pdata;
 	struct spear_snor_flash *flash;
@@ -855,6 +854,7 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 		flash->mtd.name = flash_devices[flash_index].name;
 
 	flash->mtd.dev.parent = &pdev->dev;
+	mtd_set_of_node(&flash->mtd, np);
 	flash->mtd.type = MTD_NORFLASH;
 	flash->mtd.writesize = 1;
 	flash->mtd.flags = MTD_CAP_NORFLASH;
@@ -881,10 +881,8 @@ static int spear_smi_setup_banks(struct platform_device *pdev,
 		count = flash_info->nr_partitions;
 	}
 #endif
-	ppdata.of_node = np;
 
-	ret = mtd_device_parse_register(&flash->mtd, NULL, &ppdata, parts,
-					count);
+	ret = mtd_device_register(&flash->mtd, parts, count);
 	if (ret) {
 		dev_err(&dev->pdev->dev, "Err MTD partition=%d\n", ret);
 		return ret;
diff --git a/drivers/mtd/devices/st_spi_fsm.c b/drivers/mtd/devices/st_spi_fsm.c
index 3060025c8af4..5454b4113589 100644
--- a/drivers/mtd/devices/st_spi_fsm.c
+++ b/drivers/mtd/devices/st_spi_fsm.c
@@ -2025,7 +2025,6 @@ boot_device_fail:
 static int stfsm_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	struct mtd_part_parser_data ppdata;
 	struct flash_info *info;
 	struct resource *res;
 	struct stfsm *fsm;
@@ -2035,7 +2034,6 @@ static int stfsm_probe(struct platform_device *pdev)
 		dev_err(&pdev->dev, "No DT found\n");
 		return -EINVAL;
 	}
-	ppdata.of_node = np;
 
 	fsm = devm_kzalloc(&pdev->dev, sizeof(*fsm), GFP_KERNEL);
 	if (!fsm)
@@ -2106,6 +2104,7 @@ static int stfsm_probe(struct platform_device *pdev)
 
 	fsm->mtd.name		= info->name;
 	fsm->mtd.dev.parent	= &pdev->dev;
+	mtd_set_of_node(&fsm->mtd, np);
 	fsm->mtd.type		= MTD_NORFLASH;
 	fsm->mtd.writesize	= 4;
 	fsm->mtd.writebufsize	= fsm->mtd.writesize;
@@ -2124,7 +2123,7 @@ static int stfsm_probe(struct platform_device *pdev)
 		(long long)fsm->mtd.size, (long long)(fsm->mtd.size >> 20),
 		fsm->mtd.erasesize, (fsm->mtd.erasesize >> 10));
 
-	return mtd_device_parse_register(&fsm->mtd, NULL, &ppdata, NULL, 0);
+	return mtd_device_register(&fsm->mtd, NULL, 0);
 }
 
 static int stfsm_remove(struct platform_device *pdev)
diff --git a/drivers/mtd/ftl.c b/drivers/mtd/ftl.c
index dabf08450d0b..9fb3b0dcdac2 100644
--- a/drivers/mtd/ftl.c
+++ b/drivers/mtd/ftl.c
@@ -571,12 +571,8 @@ static int copy_erase_unit(partition_t *part, uint16_t srcunit,
 
 
     /* Update the maps and usage stats*/
-    i = xfer->EraseCount;
-    xfer->EraseCount = eun->EraseCount;
-    eun->EraseCount = i;
-    i = xfer->Offset;
-    xfer->Offset = eun->Offset;
-    eun->Offset = i;
+    swap(xfer->EraseCount, eun->EraseCount);
+    swap(xfer->Offset, eun->Offset);
     part->FreeTotal -= eun->Free;
     part->FreeTotal += free;
     eun->Free = free;
diff --git a/drivers/mtd/maps/lantiq-flash.c b/drivers/mtd/maps/lantiq-flash.c
index 93852054977e..c8febb326fa6 100644
--- a/drivers/mtd/maps/lantiq-flash.c
+++ b/drivers/mtd/maps/lantiq-flash.c
@@ -110,7 +110,6 @@ ltq_copy_to(struct map_info *map, unsigned long to,
 static int
 ltq_mtd_probe(struct platform_device *pdev)
 {
-	struct mtd_part_parser_data ppdata;
 	struct ltq_mtd *ltq_mtd;
 	struct cfi_private *cfi;
 	int err;
@@ -161,13 +160,13 @@ ltq_mtd_probe(struct platform_device *pdev)
 	}
 
 	ltq_mtd->mtd->dev.parent = &pdev->dev;
+	mtd_set_of_node(ltq_mtd->mtd, pdev->dev.of_node);
 
 	cfi = ltq_mtd->map->fldrv_priv;
 	cfi->addr_unlock1 ^= 1;
 	cfi->addr_unlock2 ^= 1;
 
-	ppdata.of_node = pdev->dev.of_node;
-	err = mtd_device_parse_register(ltq_mtd->mtd, NULL, &ppdata, NULL, 0);
+	err = mtd_device_register(ltq_mtd->mtd, NULL, 0);
 	if (err) {
 		dev_err(&pdev->dev, "failed to add partitions\n");
 		goto err_destroy;
diff --git a/drivers/mtd/maps/pcmciamtd.c b/drivers/mtd/maps/pcmciamtd.c
index 3dad2111b7e3..70bb403f69f7 100644
--- a/drivers/mtd/maps/pcmciamtd.c
+++ b/drivers/mtd/maps/pcmciamtd.c
@@ -30,7 +30,7 @@
 
 struct pcmciamtd_dev {
 	struct pcmcia_device	*p_dev;
-	caddr_t		win_base;	/* ioremapped address of PCMCIA window */
+	void __iomem	*win_base;	/* ioremapped address of PCMCIA window */
 	unsigned int	win_size;	/* size of window */
 	unsigned int	offset;		/* offset into card the window currently points at */
 	struct map_info	pcmcia_map;
@@ -80,7 +80,7 @@ MODULE_PARM_DESC(mem_type, "Set Memory type (0=Flash, 1=RAM, 2=ROM, default=0)")
 /* read/write{8,16} copy_{from,to} routines with window remapping
  * to access whole card
  */
-static caddr_t remap_window(struct map_info *map, unsigned long to)
+static void __iomem *remap_window(struct map_info *map, unsigned long to)
 {
 	struct pcmciamtd_dev *dev = (struct pcmciamtd_dev *)map->map_priv_1;
 	struct resource *win = (struct resource *) map->map_priv_2;
@@ -107,7 +107,7 @@ static caddr_t remap_window(struct map_info *map, unsigned long to)
 
 static map_word pcmcia_read8_remap(struct map_info *map, unsigned long ofs)
 {
-	caddr_t addr;
+	void __iomem *addr;
 	map_word d = {{0}};
 
 	addr = remap_window(map, ofs);
@@ -122,7 +122,7 @@ static map_word pcmcia_read8_remap(struct map_info *map, unsigned long ofs)
 
 static map_word pcmcia_read16_remap(struct map_info *map, unsigned long ofs)
 {
-	caddr_t addr;
+	void __iomem *addr;
 	map_word d = {{0}};
 
 	addr = remap_window(map, ofs);
@@ -143,7 +143,7 @@ static void pcmcia_copy_from_remap(struct map_info *map, void *to, unsigned long
 	pr_debug("to = %p from = %lu len = %zd\n", to, from, len);
 	while(len) {
 		int toread = win_size - (from & (win_size-1));
-		caddr_t addr;
+		void __iomem *addr;
 
 		if(toread > len)
 			toread = len;
@@ -163,7 +163,7 @@ static void pcmcia_copy_from_remap(struct map_info *map, void *to, unsigned long
 
 static void pcmcia_write8_remap(struct map_info *map, map_word d, unsigned long adr)
 {
-	caddr_t addr = remap_window(map, adr);
+	void __iomem *addr = remap_window(map, adr);
 
 	if(!addr)
 		return;
@@ -175,7 +175,7 @@ static void pcmcia_write8_remap(struct map_info *map, map_word d, unsigned long
 
 static void pcmcia_write16_remap(struct map_info *map, map_word d, unsigned long adr)
 {
-	caddr_t addr = remap_window(map, adr);
+	void __iomem *addr = remap_window(map, adr);
 	if(!addr)
 		return;
 
@@ -192,7 +192,7 @@ static void pcmcia_copy_to_remap(struct map_info *map, unsigned long to, const v
 	pr_debug("to = %lu from = %p len = %zd\n", to, from, len);
 	while(len) {
 		int towrite = win_size - (to & (win_size-1));
-		caddr_t addr;
+		void __iomem *addr;
 
 		if(towrite > len)
 			towrite = len;
@@ -216,7 +216,7 @@ static void pcmcia_copy_to_remap(struct map_info *map, unsigned long to, const v
 
 static map_word pcmcia_read8(struct map_info *map, unsigned long ofs)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 	map_word d = {{0}};
 
 	if(DEV_REMOVED(map))
@@ -231,7 +231,7 @@ static map_word pcmcia_read8(struct map_info *map, unsigned long ofs)
 
 static map_word pcmcia_read16(struct map_info *map, unsigned long ofs)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 	map_word d = {{0}};
 
 	if(DEV_REMOVED(map))
@@ -246,7 +246,7 @@ static map_word pcmcia_read16(struct map_info *map, unsigned long ofs)
 
 static void pcmcia_copy_from(struct map_info *map, void *to, unsigned long from, ssize_t len)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 
 	if(DEV_REMOVED(map))
 		return;
@@ -258,7 +258,7 @@ static void pcmcia_copy_from(struct map_info *map, void *to, unsigned long from,
 
 static void pcmcia_write8(struct map_info *map, map_word d, unsigned long adr)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 
 	if(DEV_REMOVED(map))
 		return;
@@ -271,7 +271,7 @@ static void pcmcia_write8(struct map_info *map, map_word d, unsigned long adr)
 
 static void pcmcia_write16(struct map_info *map, map_word d, unsigned long adr)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 
 	if(DEV_REMOVED(map))
 		return;
@@ -284,7 +284,7 @@ static void pcmcia_write16(struct map_info *map, map_word d, unsigned long adr)
 
 static void pcmcia_copy_to(struct map_info *map, unsigned long to, const void *from, ssize_t len)
 {
-	caddr_t win_base = (caddr_t)map->map_priv_2;
+	void __iomem *win_base = (void __iomem *)map->map_priv_2;
 
 	if(DEV_REMOVED(map))
 		return;
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index e46b4e983666..70c453144f00 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -166,7 +166,6 @@ static int of_flash_probe(struct platform_device *dev)
 	int reg_tuple_size;
 	struct mtd_info **mtd_list = NULL;
 	resource_size_t res_size;
-	struct mtd_part_parser_data ppdata;
 	bool map_indirect;
 	const char *mtd_name = NULL;
 
@@ -310,13 +309,14 @@ static int of_flash_probe(struct platform_device *dev)
 	if (err)
 		goto err_out;
 
-	ppdata.of_node = dp;
+	info->cmtd->dev.parent = &dev->dev;
+	mtd_set_of_node(info->cmtd, dp);
 	part_probe_types = of_get_probes(dp);
 	if (!part_probe_types) {
 		err = -ENOMEM;
 		goto err_out;
 	}
-	mtd_device_parse_register(info->cmtd, part_probe_types, &ppdata,
+	mtd_device_parse_register(info->cmtd, part_probe_types, NULL,
 			NULL, 0);
 	of_free_probes(part_probe_types);
 
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index ffa288474820..309625130b21 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -32,6 +32,7 @@
 #include <linux/err.h>
 #include <linux/ioctl.h>
 #include <linux/init.h>
+#include <linux/of.h>
 #include <linux/proc_fs.h>
 #include <linux/idr.h>
 #include <linux/backing-dev.h>
@@ -445,6 +446,7 @@ int add_mtd_device(struct mtd_info *mtd)
 	mtd->dev.devt = MTD_DEVT(i);
 	dev_set_name(&mtd->dev, "mtd%d", i);
 	dev_set_drvdata(&mtd->dev, mtd);
+	of_node_get(mtd_get_of_node(mtd));
 	error = device_register(&mtd->dev);
 	if (error)
 		goto fail_added;
@@ -467,6 +469,7 @@ int add_mtd_device(struct mtd_info *mtd)
 	return 0;
 
 fail_added:
+	of_node_put(mtd_get_of_node(mtd));
 	idr_remove(&mtd_idr, i);
 fail_locked:
 	mutex_unlock(&mtd_table_mutex);
@@ -508,6 +511,7 @@ int del_mtd_device(struct mtd_info *mtd)
 		device_unregister(&mtd->dev);
 
 		idr_remove(&mtd_idr, mtd->index);
+		of_node_put(mtd_get_of_node(mtd));
 
 		module_put(THIS_MODULE);
 		ret = 0;
@@ -519,9 +523,10 @@ out_error:
 }
 
 static int mtd_add_device_partitions(struct mtd_info *mtd,
-				     struct mtd_partition *real_parts,
-				     int nbparts)
+				     struct mtd_partitions *parts)
 {
+	const struct mtd_partition *real_parts = parts->parts;
+	int nbparts = parts->nr_parts;
 	int ret;
 
 	if (nbparts == 0 || IS_ENABLED(CONFIG_MTD_PARTITIONED_MASTER)) {
@@ -590,29 +595,29 @@ int mtd_device_parse_register(struct mtd_info *mtd, const char * const *types,
 			      const struct mtd_partition *parts,
 			      int nr_parts)
 {
+	struct mtd_partitions parsed;
 	int ret;
-	struct mtd_partition *real_parts = NULL;
 
 	mtd_set_dev_defaults(mtd);
 
-	ret = parse_mtd_partitions(mtd, types, &real_parts, parser_data);
-	if (ret <= 0 && nr_parts && parts) {
-		real_parts = kmemdup(parts, sizeof(*parts) * nr_parts,
-				     GFP_KERNEL);
-		if (!real_parts)
-			ret = -ENOMEM;
-		else
-			ret = nr_parts;
-	}
-	/* Didn't come up with either parsed OR fallback partitions */
-	if (ret < 0) {
+	memset(&parsed, 0, sizeof(parsed));
+
+	ret = parse_mtd_partitions(mtd, types, &parsed, parser_data);
+	if ((ret < 0 || parsed.nr_parts == 0) && parts && nr_parts) {
+		/* Fall back to driver-provided partitions */
+		parsed = (struct mtd_partitions){
+			.parts		= parts,
+			.nr_parts	= nr_parts,
+		};
+	} else if (ret < 0) {
+		/* Didn't come up with parsed OR fallback partitions */
 		pr_info("mtd: failed to find partitions; one or more parsers reports errors (%d)\n",
 			ret);
 		/* Don't abort on errors; we can still use unpartitioned MTD */
-		ret = 0;
+		memset(&parsed, 0, sizeof(parsed));
 	}
 
-	ret = mtd_add_device_partitions(mtd, real_parts, ret);
+	ret = mtd_add_device_partitions(mtd, &parsed);
 	if (ret)
 		goto out;
 
@@ -632,7 +637,8 @@ int mtd_device_parse_register(struct mtd_info *mtd, const char * const *types,
 	}
 
 out:
-	kfree(real_parts);
+	/* Cleanup any parsed partitions */
+	mtd_part_parser_cleanup(&parsed);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(mtd_device_parse_register);
diff --git a/drivers/mtd/mtdcore.h b/drivers/mtd/mtdcore.h
index 7b0353399a10..55fdb8e1fd2a 100644
--- a/drivers/mtd/mtdcore.h
+++ b/drivers/mtd/mtdcore.h
@@ -10,10 +10,15 @@ int add_mtd_device(struct mtd_info *mtd);
 int del_mtd_device(struct mtd_info *mtd);
 int add_mtd_partitions(struct mtd_info *, const struct mtd_partition *, int);
 int del_mtd_partitions(struct mtd_info *);
+
+struct mtd_partitions;
+
 int parse_mtd_partitions(struct mtd_info *master, const char * const *types,
-			 struct mtd_partition **pparts,
+			 struct mtd_partitions *pparts,
 			 struct mtd_part_parser_data *data);
 
+void mtd_part_parser_cleanup(struct mtd_partitions *parts);
+
 int __init init_mtdchar(void);
 void __exit cleanup_mtdchar(void);
 
diff --git a/drivers/mtd/mtdpart.c b/drivers/mtd/mtdpart.c
index f8ba153f63bf..10bf304027dd 100644
--- a/drivers/mtd/mtdpart.c
+++ b/drivers/mtd/mtdpart.c
@@ -48,9 +48,12 @@ struct mtd_part {
 
 /*
  * Given a pointer to the MTD object in the mtd_part structure, we can retrieve
- * the pointer to that structure with this macro.
+ * the pointer to that structure.
  */
-#define PART(x)  ((struct mtd_part *)(x))
+static inline struct mtd_part *mtd_to_part(const struct mtd_info *mtd)
+{
+	return container_of(mtd, struct mtd_part, mtd);
+}
 
 
 /*
@@ -61,7 +64,7 @@ struct mtd_part {
 static int part_read(struct mtd_info *mtd, loff_t from, size_t len,
 		size_t *retlen, u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	struct mtd_ecc_stats stats;
 	int res;
 
@@ -80,7 +83,7 @@ static int part_read(struct mtd_info *mtd, loff_t from, size_t len,
 static int part_point(struct mtd_info *mtd, loff_t from, size_t len,
 		size_t *retlen, void **virt, resource_size_t *phys)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 
 	return part->master->_point(part->master, from + part->offset, len,
 				    retlen, virt, phys);
@@ -88,7 +91,7 @@ static int part_point(struct mtd_info *mtd, loff_t from, size_t len,
 
 static int part_unpoint(struct mtd_info *mtd, loff_t from, size_t len)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 
 	return part->master->_unpoint(part->master, from + part->offset, len);
 }
@@ -98,7 +101,7 @@ static unsigned long part_get_unmapped_area(struct mtd_info *mtd,
 					    unsigned long offset,
 					    unsigned long flags)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 
 	offset += part->offset;
 	return part->master->_get_unmapped_area(part->master, len, offset,
@@ -108,7 +111,7 @@ static unsigned long part_get_unmapped_area(struct mtd_info *mtd,
 static int part_read_oob(struct mtd_info *mtd, loff_t from,
 		struct mtd_oob_ops *ops)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	int res;
 
 	if (from >= mtd->size)
@@ -146,7 +149,7 @@ static int part_read_oob(struct mtd_info *mtd, loff_t from,
 static int part_read_user_prot_reg(struct mtd_info *mtd, loff_t from,
 		size_t len, size_t *retlen, u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_read_user_prot_reg(part->master, from, len,
 						 retlen, buf);
 }
@@ -154,7 +157,7 @@ static int part_read_user_prot_reg(struct mtd_info *mtd, loff_t from,
 static int part_get_user_prot_info(struct mtd_info *mtd, size_t len,
 				   size_t *retlen, struct otp_info *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_get_user_prot_info(part->master, len, retlen,
 						 buf);
 }
@@ -162,7 +165,7 @@ static int part_get_user_prot_info(struct mtd_info *mtd, size_t len,
 static int part_read_fact_prot_reg(struct mtd_info *mtd, loff_t from,
 		size_t len, size_t *retlen, u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_read_fact_prot_reg(part->master, from, len,
 						 retlen, buf);
 }
@@ -170,7 +173,7 @@ static int part_read_fact_prot_reg(struct mtd_info *mtd, loff_t from,
 static int part_get_fact_prot_info(struct mtd_info *mtd, size_t len,
 				   size_t *retlen, struct otp_info *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_get_fact_prot_info(part->master, len, retlen,
 						 buf);
 }
@@ -178,7 +181,7 @@ static int part_get_fact_prot_info(struct mtd_info *mtd, size_t len,
 static int part_write(struct mtd_info *mtd, loff_t to, size_t len,
 		size_t *retlen, const u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_write(part->master, to + part->offset, len,
 				    retlen, buf);
 }
@@ -186,7 +189,7 @@ static int part_write(struct mtd_info *mtd, loff_t to, size_t len,
 static int part_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
 		size_t *retlen, const u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_panic_write(part->master, to + part->offset, len,
 					  retlen, buf);
 }
@@ -194,7 +197,7 @@ static int part_panic_write(struct mtd_info *mtd, loff_t to, size_t len,
 static int part_write_oob(struct mtd_info *mtd, loff_t to,
 		struct mtd_oob_ops *ops)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 
 	if (to >= mtd->size)
 		return -EINVAL;
@@ -206,7 +209,7 @@ static int part_write_oob(struct mtd_info *mtd, loff_t to,
 static int part_write_user_prot_reg(struct mtd_info *mtd, loff_t from,
 		size_t len, size_t *retlen, u_char *buf)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_write_user_prot_reg(part->master, from, len,
 						  retlen, buf);
 }
@@ -214,21 +217,21 @@ static int part_write_user_prot_reg(struct mtd_info *mtd, loff_t from,
 static int part_lock_user_prot_reg(struct mtd_info *mtd, loff_t from,
 		size_t len)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_lock_user_prot_reg(part->master, from, len);
 }
 
 static int part_writev(struct mtd_info *mtd, const struct kvec *vecs,
 		unsigned long count, loff_t to, size_t *retlen)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_writev(part->master, vecs, count,
 				     to + part->offset, retlen);
 }
 
 static int part_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	int ret;
 
 	instr->addr += part->offset;
@@ -244,7 +247,7 @@ static int part_erase(struct mtd_info *mtd, struct erase_info *instr)
 void mtd_erase_callback(struct erase_info *instr)
 {
 	if (instr->mtd->_erase == part_erase) {
-		struct mtd_part *part = PART(instr->mtd);
+		struct mtd_part *part = mtd_to_part(instr->mtd);
 
 		if (instr->fail_addr != MTD_FAIL_ADDR_UNKNOWN)
 			instr->fail_addr -= part->offset;
@@ -257,57 +260,57 @@ EXPORT_SYMBOL_GPL(mtd_erase_callback);
 
 static int part_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_lock(part->master, ofs + part->offset, len);
 }
 
 static int part_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_unlock(part->master, ofs + part->offset, len);
 }
 
 static int part_is_locked(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_is_locked(part->master, ofs + part->offset, len);
 }
 
 static void part_sync(struct mtd_info *mtd)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	part->master->_sync(part->master);
 }
 
 static int part_suspend(struct mtd_info *mtd)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return part->master->_suspend(part->master);
 }
 
 static void part_resume(struct mtd_info *mtd)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	part->master->_resume(part->master);
 }
 
 static int part_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	ofs += part->offset;
 	return part->master->_block_isreserved(part->master, ofs);
 }
 
 static int part_block_isbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	ofs += part->offset;
 	return part->master->_block_isbad(part->master, ofs);
 }
 
 static int part_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	int res;
 
 	ofs += part->offset;
@@ -558,7 +561,7 @@ static ssize_t mtd_partition_offset_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct mtd_info *mtd = dev_get_drvdata(dev);
-	struct mtd_part *part = PART(mtd);
+	struct mtd_part *part = mtd_to_part(mtd);
 	return snprintf(buf, PAGE_SIZE, "%lld\n", part->offset);
 }
 
@@ -596,11 +599,10 @@ int mtd_add_partition(struct mtd_info *master, const char *name,
 	if (length <= 0)
 		return -EINVAL;
 
+	memset(&part, 0, sizeof(part));
 	part.name = name;
 	part.size = length;
 	part.offset = offset;
-	part.mask_flags = 0;
-	part.ecclayout = NULL;
 
 	new = allocate_partition(master, &part, -1, offset);
 	if (IS_ERR(new))
@@ -685,7 +687,7 @@ int add_mtd_partitions(struct mtd_info *master,
 static DEFINE_SPINLOCK(part_parser_lock);
 static LIST_HEAD(part_parsers);
 
-static struct mtd_part_parser *get_partition_parser(const char *name)
+static struct mtd_part_parser *mtd_part_parser_get(const char *name)
 {
 	struct mtd_part_parser *p, *ret = NULL;
 
@@ -702,15 +704,35 @@ static struct mtd_part_parser *get_partition_parser(const char *name)
 	return ret;
 }
 
-#define put_partition_parser(p) do { module_put((p)->owner); } while (0)
+static inline void mtd_part_parser_put(const struct mtd_part_parser *p)
+{
+	module_put(p->owner);
+}
+
+/*
+ * Many partition parsers just expected the core to kfree() all their data in
+ * one chunk. Do that by default.
+ */
+static void mtd_part_parser_cleanup_default(const struct mtd_partition *pparts,
+					    int nr_parts)
+{
+	kfree(pparts);
+}
 
-void register_mtd_parser(struct mtd_part_parser *p)
+int __register_mtd_parser(struct mtd_part_parser *p, struct module *owner)
 {
+	p->owner = owner;
+
+	if (!p->cleanup)
+		p->cleanup = &mtd_part_parser_cleanup_default;
+
 	spin_lock(&part_parser_lock);
 	list_add(&p->list, &part_parsers);
 	spin_unlock(&part_parser_lock);
+
+	return 0;
 }
-EXPORT_SYMBOL_GPL(register_mtd_parser);
+EXPORT_SYMBOL_GPL(__register_mtd_parser);
 
 void deregister_mtd_parser(struct mtd_part_parser *p)
 {
@@ -734,7 +756,7 @@ static const char * const default_mtd_part_types[] = {
  * parse_mtd_partitions - parse MTD partitions
  * @master: the master partition (describes whole MTD device)
  * @types: names of partition parsers to try or %NULL
- * @pparts: array of partitions found is returned here
+ * @pparts: info about partitions found is returned here
  * @data: MTD partition parser-specific data
  *
  * This function tries to find partition on MTD device @master. It uses MTD
@@ -746,12 +768,13 @@ static const char * const default_mtd_part_types[] = {
  *
  * This function may return:
  * o a negative error code in case of failure
- * o zero if no partitions were found
- * o a positive number of found partitions, in which case on exit @pparts will
- *   point to an array containing this number of &struct mtd_info objects.
+ * o zero otherwise, and @pparts will describe the partitions, number of
+ *   partitions, and the parser which parsed them. Caller must release
+ *   resources with mtd_part_parser_cleanup() when finished with the returned
+ *   data.
  */
 int parse_mtd_partitions(struct mtd_info *master, const char *const *types,
-			 struct mtd_partition **pparts,
+			 struct mtd_partitions *pparts,
 			 struct mtd_part_parser_data *data)
 {
 	struct mtd_part_parser *parser;
@@ -762,22 +785,24 @@ int parse_mtd_partitions(struct mtd_info *master, const char *const *types,
 
 	for ( ; *types; types++) {
 		pr_debug("%s: parsing partitions %s\n", master->name, *types);
-		parser = get_partition_parser(*types);
+		parser = mtd_part_parser_get(*types);
 		if (!parser && !request_module("%s", *types))
-			parser = get_partition_parser(*types);
+			parser = mtd_part_parser_get(*types);
 		pr_debug("%s: got parser %s\n", master->name,
 			 parser ? parser->name : NULL);
 		if (!parser)
 			continue;
-		ret = (*parser->parse_fn)(master, pparts, data);
+		ret = (*parser->parse_fn)(master, &pparts->parts, data);
 		pr_debug("%s: parser %s: %i\n",
 			 master->name, parser->name, ret);
-		put_partition_parser(parser);
 		if (ret > 0) {
 			printk(KERN_NOTICE "%d %s partitions found on MTD device %s\n",
 			       ret, parser->name, master->name);
-			return ret;
+			pparts->nr_parts = ret;
+			pparts->parser = parser;
+			return 0;
 		}
+		mtd_part_parser_put(parser);
 		/*
 		 * Stash the first error we see; only report it if no parser
 		 * succeeds
@@ -788,6 +813,22 @@ int parse_mtd_partitions(struct mtd_info *master, const char *const *types,
 	return err;
 }
 
+void mtd_part_parser_cleanup(struct mtd_partitions *parts)
+{
+	const struct mtd_part_parser *parser;
+
+	if (!parts)
+		return;
+
+	parser = parts->parser;
+	if (parser) {
+		if (parser->cleanup)
+			parser->cleanup(parts->parts, parts->nr_parts);
+
+		mtd_part_parser_put(parser);
+	}
+}
+
 int mtd_is_partition(const struct mtd_info *mtd)
 {
 	struct mtd_part *part;
@@ -811,6 +852,6 @@ uint64_t mtd_get_device_size(const struct mtd_info *mtd)
 	if (!mtd_is_partition(mtd))
 		return mtd->size;
 
-	return PART(mtd)->master->size;
+	return mtd_to_part(mtd)->master->size;
 }
 EXPORT_SYMBOL_GPL(mtd_get_device_size);
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index 289664089cf3..20f01b3ec23d 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -55,7 +55,7 @@ config MTD_NAND_DENALI_PCI
 config MTD_NAND_DENALI_DT
 	tristate "Support Denali NAND controller as a DT device"
 	select MTD_NAND_DENALI
-	depends on HAS_DMA && HAVE_CLK
+	depends on HAS_DMA && HAVE_CLK && OF
 	help
 	  Enable the driver for NAND flash on platforms using a Denali NAND
 	  controller as a DT device.
@@ -480,7 +480,7 @@ config MTD_NAND_MXC
 
 config MTD_NAND_SH_FLCTL
 	tristate "Support for NAND on Renesas SuperH FLCTL"
-	depends on SUPERH || ARCH_SHMOBILE || COMPILE_TEST
+	depends on SUPERH || COMPILE_TEST
 	depends on HAS_IOMEM
 	depends on HAS_DMA
 	help
@@ -519,6 +519,13 @@ config MTD_NAND_JZ4740
 	help
 		Enables support for NAND Flash on JZ4740 SoC based boards.
 
+config MTD_NAND_JZ4780
+	tristate "Support for NAND on JZ4780 SoC"
+	depends on MACH_JZ4780 && JZ4780_NEMC
+	help
+	  Enables support for NAND Flash connected to the NEMC on JZ4780 SoC
+	  based boards, using the BCH controller for hardware error correction.
+
 config MTD_NAND_FSMC
 	tristate "Support for NAND on ST Micros FSMC"
 	depends on PLAT_SPEAR || ARCH_NOMADIK || ARCH_U8500 || MACH_U300
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 2c7f014b349e..9e3623308509 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -49,6 +49,7 @@ obj-$(CONFIG_MTD_NAND_MPC5121_NFC)	+= mpc5121_nfc.o
 obj-$(CONFIG_MTD_NAND_VF610_NFC)	+= vf610_nfc.o
 obj-$(CONFIG_MTD_NAND_RICOH)		+= r852.o
 obj-$(CONFIG_MTD_NAND_JZ4740)		+= jz4740_nand.o
+obj-$(CONFIG_MTD_NAND_JZ4780)		+= jz4780_nand.o jz4780_bch.o
 obj-$(CONFIG_MTD_NAND_GPMI_NAND)	+= gpmi-nand/
 obj-$(CONFIG_MTD_NAND_XWAY)		+= xway_nand.o
 obj-$(CONFIG_MTD_NAND_BCM47XXNFLASH)	+= bcm47xxnflash/
diff --git a/drivers/mtd/nand/ams-delta.c b/drivers/mtd/nand/ams-delta.c
index 842f8fe91b56..68b58c85789c 100644
--- a/drivers/mtd/nand/ams-delta.c
+++ b/drivers/mtd/nand/ams-delta.c
@@ -64,8 +64,8 @@ static struct mtd_partition partition_info[] = {
 
 static void ams_delta_write_byte(struct mtd_info *mtd, u_char byte)
 {
-	struct nand_chip *this = mtd->priv;
-	void __iomem *io_base = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	void __iomem *io_base = (void __iomem *)nand_get_controller_data(this);
 
 	writew(0, io_base + OMAP_MPUIO_IO_CNTL);
 	writew(byte, this->IO_ADDR_W);
@@ -77,8 +77,8 @@ static void ams_delta_write_byte(struct mtd_info *mtd, u_char byte)
 static u_char ams_delta_read_byte(struct mtd_info *mtd)
 {
 	u_char res;
-	struct nand_chip *this = mtd->priv;
-	void __iomem *io_base = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	void __iomem *io_base = (void __iomem *)nand_get_controller_data(this);
 
 	gpio_set_value(AMS_DELTA_GPIO_PIN_NAND_NRE, 0);
 	ndelay(40);
@@ -183,22 +183,16 @@ static int ams_delta_init(struct platform_device *pdev)
 		return -ENXIO;
 
 	/* Allocate memory for MTD device structure and private data */
-	ams_delta_mtd = kzalloc(sizeof(struct mtd_info) +
-				sizeof(struct nand_chip), GFP_KERNEL);
-	if (!ams_delta_mtd) {
+	this = kzalloc(sizeof(struct nand_chip), GFP_KERNEL);
+	if (!this) {
 		printk (KERN_WARNING "Unable to allocate E3 NAND MTD device structure.\n");
 		err = -ENOMEM;
 		goto out;
 	}
 
+	ams_delta_mtd = nand_to_mtd(this);
 	ams_delta_mtd->owner = THIS_MODULE;
 
-	/* Get pointer to private data */
-	this = (struct nand_chip *) (&ams_delta_mtd[1]);
-
-	/* Link the private data with the MTD structure */
-	ams_delta_mtd->priv = this;
-
 	/*
 	 * Don't try to request the memory region from here,
 	 * it should have been already requested from the
@@ -212,7 +206,7 @@ static int ams_delta_init(struct platform_device *pdev)
 		goto out_free;
 	}
 
-	this->priv = io_base;
+	nand_set_controller_data(this, (void *)io_base);
 
 	/* Set address of NAND IO lines */
 	this->IO_ADDR_R = io_base + OMAP_MPUIO_INPUT_LATCH;
@@ -256,7 +250,7 @@ out_gpio:
 	gpio_free(AMS_DELTA_GPIO_PIN_NAND_RB);
 	iounmap(io_base);
 out_free:
-	kfree(ams_delta_mtd);
+	kfree(this);
  out:
 	return err;
 }
@@ -276,7 +270,7 @@ static int ams_delta_cleanup(struct platform_device *pdev)
 	iounmap(io_base);
 
 	/* Free the MTD device structure */
-	kfree(ams_delta_mtd);
+	kfree(mtd_to_nand(ams_delta_mtd));
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index 583cdd9bb971..bddcf83d6859 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -116,7 +116,6 @@ static struct atmel_nfc	nand_nfc;
 
 struct atmel_nand_host {
 	struct nand_chip	nand_chip;
-	struct mtd_info		mtd;
 	void __iomem		*io_base;
 	dma_addr_t		io_phys;
 	struct atmel_nand_data	board;
@@ -128,7 +127,7 @@ struct atmel_nand_host {
 
 	struct atmel_nfc	*nfc;
 
-	struct atmel_nand_caps	*caps;
+	const struct atmel_nand_caps	*caps;
 	bool			has_pmecc;
 	u8			pmecc_corr_cap;
 	u16			pmecc_sector_size;
@@ -182,8 +181,8 @@ static void atmel_nand_disable(struct atmel_nand_host *host)
  */
 static void atmel_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		if (ctrl & NAND_NCE)
@@ -205,8 +204,8 @@ static void atmel_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl
  */
 static int atmel_nand_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	return gpio_get_value(host->board.rdy_pin) ^
                 !!host->board.rdy_pin_active_low;
@@ -215,8 +214,8 @@ static int atmel_nand_device_ready(struct mtd_info *mtd)
 /* Set up for hardware ready pin and enable pin. */
 static int atmel_nand_set_enable_ready_pins(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	int res = 0;
 
 	if (gpio_is_valid(host->board.rdy_pin)) {
@@ -267,8 +266,8 @@ static int atmel_nand_set_enable_ready_pins(struct mtd_info *mtd)
  */
 static void atmel_read_buf8(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip	*nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip	*nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (host->nfc && host->nfc->use_nfc_sram && host->nfc->data_in_sram) {
 		memcpy(buf, host->nfc->data_in_sram, len);
@@ -280,8 +279,8 @@ static void atmel_read_buf8(struct mtd_info *mtd, u8 *buf, int len)
 
 static void atmel_read_buf16(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip	*nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip	*nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (host->nfc && host->nfc->use_nfc_sram && host->nfc->data_in_sram) {
 		memcpy(buf, host->nfc->data_in_sram, len);
@@ -293,14 +292,14 @@ static void atmel_read_buf16(struct mtd_info *mtd, u8 *buf, int len)
 
 static void atmel_write_buf8(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip	*nand_chip = mtd->priv;
+	struct nand_chip	*nand_chip = mtd_to_nand(mtd);
 
 	__raw_writesb(nand_chip->IO_ADDR_W, buf, len);
 }
 
 static void atmel_write_buf16(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip	*nand_chip = mtd->priv;
+	struct nand_chip	*nand_chip = mtd_to_nand(mtd);
 
 	__raw_writesw(nand_chip->IO_ADDR_W, buf, len / 2);
 }
@@ -317,8 +316,10 @@ static int nfc_set_sram_bank(struct atmel_nand_host *host, unsigned int bank)
 		return -EINVAL;
 
 	if (bank) {
+		struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
+
 		/* Only for a 2k-page or lower flash, NFC can handle 2 banks */
-		if (host->mtd.writesize > 2048)
+		if (mtd->writesize > 2048)
 			return -EINVAL;
 		nfc_writel(host->nfc->hsmc_regs, BANK, ATMEL_HSMC_NFC_BANK1);
 	} else {
@@ -352,8 +353,8 @@ static int atmel_nand_dma_op(struct mtd_info *mtd, void *buf, int len,
 	dma_addr_t dma_src_addr, dma_dst_addr, phys_addr;
 	struct dma_async_tx_descriptor *tx = NULL;
 	dma_cookie_t cookie;
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	void *p = buf;
 	int err = -EIO;
 	enum dma_data_direction dir = is_read ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
@@ -425,8 +426,8 @@ err_buf:
 
 static void atmel_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 
 	if (use_dma && len > mtd->oobsize)
 		/* only use DMA for bigger than oob size: better performances */
@@ -441,8 +442,8 @@ static void atmel_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 
 static void atmel_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 
 	if (use_dma && len > mtd->oobsize)
 		/* only use DMA for bigger than oob size: better performances */
@@ -533,8 +534,8 @@ static int pmecc_data_alloc(struct atmel_nand_host *host)
 
 static void pmecc_gen_syndrome(struct mtd_info *mtd, int sector)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i;
 	uint32_t value;
 
@@ -550,8 +551,8 @@ static void pmecc_gen_syndrome(struct mtd_info *mtd, int sector)
 
 static void pmecc_substitute(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int16_t __iomem *alpha_to = host->pmecc_alpha_to;
 	int16_t __iomem *index_of = host->pmecc_index_of;
 	int16_t *partial_syn = host->pmecc_partial_syn;
@@ -592,8 +593,8 @@ static void pmecc_substitute(struct mtd_info *mtd)
 
 static void pmecc_get_sigma(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	int16_t *lmu = host->pmecc_lmu;
 	int16_t *si = host->pmecc_si;
@@ -750,8 +751,8 @@ static void pmecc_get_sigma(struct mtd_info *mtd)
 
 static int pmecc_err_location(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	unsigned long end_time;
 	const int cap = host->pmecc_corr_cap;
 	const int num = 2 * cap + 1;
@@ -802,8 +803,8 @@ static int pmecc_err_location(struct mtd_info *mtd)
 static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
 		int sector_num, int extra_bytes, int err_nbr)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i = 0;
 	int byte_pos, bit_pos, sector_size, pos;
 	uint32_t tmp;
@@ -848,8 +849,8 @@ static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
 static int pmecc_correction(struct mtd_info *mtd, u32 pmecc_stat, uint8_t *buf,
 	u8 *ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	int i, err_nbr;
 	uint8_t *buf_pos;
 	int max_bitflips = 0;
@@ -919,7 +920,7 @@ static void pmecc_enable(struct atmel_nand_host *host, int ecc_op)
 static int atmel_nand_pmecc_read_page(struct mtd_info *mtd,
 	struct nand_chip *chip, uint8_t *buf, int oob_required, int page)
 {
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	int eccsize = chip->ecc.size * chip->ecc.steps;
 	uint8_t *oob = chip->oob_poi;
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
@@ -957,7 +958,7 @@ static int atmel_nand_pmecc_write_page(struct mtd_info *mtd,
 		struct nand_chip *chip, const uint8_t *buf, int oob_required,
 		int page)
 {
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
 	int i, j;
 	unsigned long end_time;
@@ -992,8 +993,8 @@ static int atmel_nand_pmecc_write_page(struct mtd_info *mtd,
 
 static void atmel_pmecc_core_init(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	uint32_t val = 0;
 	struct nand_ecclayout *ecc_layout;
 
@@ -1159,8 +1160,8 @@ static uint16_t *create_lookup_table(struct device *dev, int sector_size)
 static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 					 struct atmel_nand_host *host)
 {
-	struct mtd_info *mtd = &host->mtd;
 	struct nand_chip *nand_chip = &host->nand_chip;
+	struct mtd_info *mtd = nand_to_mtd(nand_chip);
 	struct resource *regs, *regs_pmerr, *regs_rom;
 	uint16_t *galois_table;
 	int cap, sector_size, err_no;
@@ -1308,8 +1309,8 @@ err:
 static int atmel_nand_calculate(struct mtd_info *mtd,
 		const u_char *dat, unsigned char *ecc_code)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	unsigned int ecc_value;
 
 	/* get the first 2 ECC bytes */
@@ -1355,7 +1356,7 @@ static int atmel_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 	 * Workaround: Reset the parity registers before reading the
 	 * actual data.
 	 */
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	if (host->board.need_reset_workaround)
 		ecc_writel(host->ecc, CR, ATMEL_ECC_RST);
 
@@ -1412,8 +1413,8 @@ static int atmel_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
 		u_char *read_ecc, u_char *isnull)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 	unsigned int ecc_status;
 	unsigned int ecc_word, ecc_bit;
 
@@ -1444,7 +1445,7 @@ static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
 		 * We can't correct so many errors */
 		dev_dbg(host->dev, "atmel_nand : multiple errors detected."
 				" Unable to correct.\n");
-		return -EIO;
+		return -EBADMSG;
 	}
 
 	/* if there's a single bit error : we can correct it */
@@ -1478,8 +1479,8 @@ static int atmel_nand_correct(struct mtd_info *mtd, u_char *dat,
  */
 static void atmel_nand_hwctl(struct mtd_info *mtd, int mode)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (host->board.need_reset_workaround)
 		ecc_writel(host->ecc, CR, ATMEL_ECC_RST);
@@ -1586,8 +1587,8 @@ static int atmel_of_init_port(struct atmel_nand_host *host,
 static int atmel_hw_nand_init_params(struct platform_device *pdev,
 					 struct atmel_nand_host *host)
 {
-	struct mtd_info *mtd = &host->mtd;
 	struct nand_chip *nand_chip = &host->nand_chip;
+	struct mtd_info *mtd = nand_to_mtd(nand_chip);
 	struct resource		*regs;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -1771,8 +1772,8 @@ static int nfc_send_command(struct atmel_nand_host *host,
 static int nfc_device_ready(struct mtd_info *mtd)
 {
 	u32 status, mask;
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	status = nfc_read_status(host);
 	mask = nfc_readl(host->nfc->hsmc_regs, IMR);
@@ -1787,8 +1788,8 @@ static int nfc_device_ready(struct mtd_info *mtd)
 
 static void nfc_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct atmel_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (chip == -1)
 		nfc_writel(host->nfc->hsmc_regs, CTRL, NFC_CTRL_DISABLE);
@@ -1799,7 +1800,7 @@ static void nfc_select_chip(struct mtd_info *mtd, int chip)
 static int nfc_make_addr(struct mtd_info *mtd, int command, int column,
 		int page_addr, unsigned int *addr1234, unsigned int *cycle0)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	int acycle = 0;
 	unsigned char addr_bytes[8];
@@ -1839,8 +1840,8 @@ static int nfc_make_addr(struct mtd_info *mtd, int command, int column,
 static void nfc_nand_command(struct mtd_info *mtd, unsigned int command,
 				int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	unsigned long timeout;
 	unsigned int nfc_addr_cmd = 0;
 
@@ -1966,7 +1967,7 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 {
 	int cfg, len;
 	int status = 0;
-	struct atmel_nand_host *host = chip->priv;
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	void *sram = host->nfc->sram_bank0 + nfc_get_sram_off(host);
 
 	/* Subpage write is not supported */
@@ -2026,8 +2027,8 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 static int nfc_sram_init(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct atmel_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct atmel_nand_host *host = nand_get_controller_data(chip);
 	int res = 0;
 
 	/* Initialize the NFC CFG register */
@@ -2093,7 +2094,6 @@ static int atmel_nand_probe(struct platform_device *pdev)
 	struct mtd_info *mtd;
 	struct nand_chip *nand_chip;
 	struct resource *mem;
-	struct mtd_part_parser_data ppdata = {};
 	int res, irq;
 
 	/* Allocate memory for the device structure (and zero it) */
@@ -2113,10 +2113,11 @@ static int atmel_nand_probe(struct platform_device *pdev)
 	}
 	host->io_phys = (dma_addr_t)mem->start;
 
-	mtd = &host->mtd;
 	nand_chip = &host->nand_chip;
+	mtd = nand_to_mtd(nand_chip);
 	host->dev = &pdev->dev;
 	if (IS_ENABLED(CONFIG_OF) && pdev->dev.of_node) {
+		nand_set_flash_node(nand_chip, pdev->dev.of_node);
 		/* Only when CONFIG_OF is enabled of_node can be parsed */
 		res = atmel_of_init_port(host, pdev->dev.of_node);
 		if (res)
@@ -2126,8 +2127,8 @@ static int atmel_nand_probe(struct platform_device *pdev)
 		       sizeof(struct atmel_nand_data));
 	}
 
-	nand_chip->priv = host;		/* link the private data structures */
-	mtd->priv = nand_chip;
+	 /* link the private data structures */
+	nand_set_controller_data(nand_chip, host);
 	mtd->dev.parent = &pdev->dev;
 
 	/* Set address of NAND IO lines */
@@ -2259,9 +2260,8 @@ static int atmel_nand_probe(struct platform_device *pdev)
 	}
 
 	mtd->name = "atmel_nand";
-	ppdata.of_node = pdev->dev.of_node;
-	res = mtd_device_parse_register(mtd, NULL, &ppdata,
-			host->board.parts, host->board.num_parts);
+	res = mtd_device_register(mtd, host->board.parts,
+				  host->board.num_parts);
 	if (!res)
 		return res;
 
@@ -2284,7 +2284,7 @@ err_nand_ioremap:
 static int atmel_nand_remove(struct platform_device *pdev)
 {
 	struct atmel_nand_host *host = platform_get_drvdata(pdev);
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 
 	nand_release(mtd);
 
@@ -2304,11 +2304,11 @@ static int atmel_nand_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct atmel_nand_caps at91rm9200_caps = {
+static const struct atmel_nand_caps at91rm9200_caps = {
 	.pmecc_correct_erase_page = false,
 };
 
-static struct atmel_nand_caps sama5d4_caps = {
+static const struct atmel_nand_caps sama5d4_caps = {
 	.pmecc_correct_erase_page = true,
 };
 
diff --git a/drivers/mtd/nand/au1550nd.c b/drivers/mtd/nand/au1550nd.c
index 08a130f63faf..341ea4904164 100644
--- a/drivers/mtd/nand/au1550nd.c
+++ b/drivers/mtd/nand/au1550nd.c
@@ -23,7 +23,6 @@
 
 
 struct au1550nd_ctx {
-	struct mtd_info info;
 	struct nand_chip chip;
 
 	int cs;
@@ -39,7 +38,7 @@ struct au1550nd_ctx {
  */
 static u_char au_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ret = readb(this->IO_ADDR_R);
 	wmb(); /* drain writebuffer */
 	return ret;
@@ -54,7 +53,7 @@ static u_char au_read_byte(struct mtd_info *mtd)
  */
 static void au_write_byte(struct mtd_info *mtd, u_char byte)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	writeb(byte, this->IO_ADDR_W);
 	wmb(); /* drain writebuffer */
 }
@@ -67,7 +66,7 @@ static void au_write_byte(struct mtd_info *mtd, u_char byte)
  */
 static u_char au_read_byte16(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u_char ret = (u_char) cpu_to_le16(readw(this->IO_ADDR_R));
 	wmb(); /* drain writebuffer */
 	return ret;
@@ -82,7 +81,7 @@ static u_char au_read_byte16(struct mtd_info *mtd)
  */
 static void au_write_byte16(struct mtd_info *mtd, u_char byte)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	writew(le16_to_cpu((u16) byte), this->IO_ADDR_W);
 	wmb(); /* drain writebuffer */
 }
@@ -95,7 +94,7 @@ static void au_write_byte16(struct mtd_info *mtd, u_char byte)
  */
 static u16 au_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u16 ret = readw(this->IO_ADDR_R);
 	wmb(); /* drain writebuffer */
 	return ret;
@@ -112,7 +111,7 @@ static u16 au_read_word(struct mtd_info *mtd)
 static void au_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	for (i = 0; i < len; i++) {
 		writeb(buf[i], this->IO_ADDR_W);
@@ -131,7 +130,7 @@ static void au_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 static void au_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	for (i = 0; i < len; i++) {
 		buf[i] = readb(this->IO_ADDR_R);
@@ -150,7 +149,7 @@ static void au_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 static void au_write_buf16(struct mtd_info *mtd, const u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 	len >>= 1;
 
@@ -172,7 +171,7 @@ static void au_write_buf16(struct mtd_info *mtd, const u_char *buf, int len)
 static void au_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 	len >>= 1;
 
@@ -197,8 +196,9 @@ static void au_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
 
 static void au1550_hwcontrol(struct mtd_info *mtd, int cmd)
 {
-	struct au1550nd_ctx *ctx = container_of(mtd, struct au1550nd_ctx, info);
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct au1550nd_ctx *ctx = container_of(this, struct au1550nd_ctx,
+						chip);
 
 	switch (cmd) {
 
@@ -267,8 +267,9 @@ static void au1550_select_chip(struct mtd_info *mtd, int chip)
  */
 static void au1550_command(struct mtd_info *mtd, unsigned command, int column, int page_addr)
 {
-	struct au1550nd_ctx *ctx = container_of(mtd, struct au1550nd_ctx, info);
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct au1550nd_ctx *ctx = container_of(this, struct au1550nd_ctx,
+						chip);
 	int ce_override = 0, i;
 	unsigned long flags = 0;
 
@@ -405,6 +406,7 @@ static int au1550nd_probe(struct platform_device *pdev)
 	struct au1550nd_platdata *pd;
 	struct au1550nd_ctx *ctx;
 	struct nand_chip *this;
+	struct mtd_info *mtd;
 	struct resource *r;
 	int ret, cs;
 
@@ -438,8 +440,8 @@ static int au1550nd_probe(struct platform_device *pdev)
 	}
 
 	this = &ctx->chip;
-	ctx->info.priv = this;
-	ctx->info.dev.parent = &pdev->dev;
+	mtd = nand_to_mtd(this);
+	mtd->dev.parent = &pdev->dev;
 
 	/* figure out which CS# r->start belongs to */
 	cs = find_nand_cs(r->start);
@@ -467,13 +469,13 @@ static int au1550nd_probe(struct platform_device *pdev)
 	this->write_buf = (pd->devwidth) ? au_write_buf16 : au_write_buf;
 	this->read_buf = (pd->devwidth) ? au_read_buf16 : au_read_buf;
 
-	ret = nand_scan(&ctx->info, 1);
+	ret = nand_scan(mtd, 1);
 	if (ret) {
 		dev_err(&pdev->dev, "NAND scan failed with %d\n", ret);
 		goto out3;
 	}
 
-	mtd_device_register(&ctx->info, pd->parts, pd->num_parts);
+	mtd_device_register(mtd, pd->parts, pd->num_parts);
 
 	platform_set_drvdata(pdev, ctx);
 
@@ -493,7 +495,7 @@ static int au1550nd_remove(struct platform_device *pdev)
 	struct au1550nd_ctx *ctx = platform_get_drvdata(pdev);
 	struct resource *r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
-	nand_release(&ctx->info);
+	nand_release(nand_to_mtd(&ctx->chip));
 	iounmap(ctx->base);
 	release_mem_region(r->start, 0x1000);
 	kfree(ctx);
diff --git a/drivers/mtd/nand/bcm47xxnflash/bcm47xxnflash.h b/drivers/mtd/nand/bcm47xxnflash/bcm47xxnflash.h
index c005a62330b1..8ea75710a854 100644
--- a/drivers/mtd/nand/bcm47xxnflash/bcm47xxnflash.h
+++ b/drivers/mtd/nand/bcm47xxnflash/bcm47xxnflash.h
@@ -12,7 +12,6 @@ struct bcm47xxnflash {
 	struct bcma_drv_cc *cc;
 
 	struct nand_chip nand_chip;
-	struct mtd_info mtd;
 
 	unsigned curr_command;
 	int curr_page_addr;
diff --git a/drivers/mtd/nand/bcm47xxnflash/main.c b/drivers/mtd/nand/bcm47xxnflash/main.c
index 9ba0c0f2cd9b..fb31429b70a9 100644
--- a/drivers/mtd/nand/bcm47xxnflash/main.c
+++ b/drivers/mtd/nand/bcm47xxnflash/main.c
@@ -27,15 +27,16 @@ static int bcm47xxnflash_probe(struct platform_device *pdev)
 {
 	struct bcma_nflash *nflash = dev_get_platdata(&pdev->dev);
 	struct bcm47xxnflash *b47n;
+	struct mtd_info *mtd;
 	int err = 0;
 
 	b47n = devm_kzalloc(&pdev->dev, sizeof(*b47n), GFP_KERNEL);
 	if (!b47n)
 		return -ENOMEM;
 
-	b47n->nand_chip.priv = b47n;
-	b47n->mtd.dev.parent = &pdev->dev;
-	b47n->mtd.priv = &b47n->nand_chip; /* Required */
+	nand_set_controller_data(&b47n->nand_chip, b47n);
+	mtd = nand_to_mtd(&b47n->nand_chip);
+	mtd->dev.parent = &pdev->dev;
 	b47n->cc = container_of(nflash, struct bcma_drv_cc, nflash);
 
 	if (b47n->cc->core->bus->chipinfo.id == BCMA_CHIP_ID_BCM4706) {
@@ -49,7 +50,9 @@ static int bcm47xxnflash_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	err = mtd_device_parse_register(&b47n->mtd, probes, NULL, NULL, 0);
+	platform_set_drvdata(pdev, b47n);
+
+	err = mtd_device_parse_register(mtd, probes, NULL, NULL, 0);
 	if (err) {
 		pr_err("Failed to register MTD device: %d\n", err);
 		return err;
@@ -60,10 +63,9 @@ static int bcm47xxnflash_probe(struct platform_device *pdev)
 
 static int bcm47xxnflash_remove(struct platform_device *pdev)
 {
-	struct bcma_nflash *nflash = dev_get_platdata(&pdev->dev);
+	struct bcm47xxnflash *nflash = platform_get_drvdata(pdev);
 
-	if (nflash->mtd)
-		mtd_device_unregister(nflash->mtd);
+	nand_release(nand_to_mtd(&nflash->nand_chip));
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
index 592befc7ffa1..f1da4ea88f2c 100644
--- a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
+++ b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
@@ -89,8 +89,8 @@ static int bcm47xxnflash_ops_bcm4706_poll(struct bcma_drv_cc *cc)
 static void bcm47xxnflash_ops_bcm4706_read(struct mtd_info *mtd, uint8_t *buf,
 					   int len)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 
 	u32 ctlcode;
 	u32 *dest = (u32 *)buf;
@@ -139,8 +139,8 @@ static void bcm47xxnflash_ops_bcm4706_read(struct mtd_info *mtd, uint8_t *buf,
 static void bcm47xxnflash_ops_bcm4706_write(struct mtd_info *mtd,
 					    const uint8_t *buf, int len)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 	struct bcma_drv_cc *cc = b47n->cc;
 
 	u32 ctlcode;
@@ -173,8 +173,8 @@ static void bcm47xxnflash_ops_bcm4706_write(struct mtd_info *mtd,
 static void bcm47xxnflash_ops_bcm4706_cmd_ctrl(struct mtd_info *mtd, int cmd,
 					       unsigned int ctrl)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 	u32 code = 0;
 
 	if (cmd == NAND_CMD_NONE)
@@ -199,8 +199,8 @@ static void bcm47xxnflash_ops_bcm4706_select_chip(struct mtd_info *mtd,
 
 static int bcm47xxnflash_ops_bcm4706_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 
 	return !!(bcma_cc_read32(b47n->cc, BCMA_CC_NFLASH_CTL) & NCTL_READY);
 }
@@ -216,8 +216,8 @@ static void bcm47xxnflash_ops_bcm4706_cmdfunc(struct mtd_info *mtd,
 					      unsigned command, int column,
 					      int page_addr)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 	struct bcma_drv_cc *cc = b47n->cc;
 	u32 ctlcode;
 	int i;
@@ -312,8 +312,8 @@ static void bcm47xxnflash_ops_bcm4706_cmdfunc(struct mtd_info *mtd,
 
 static u8 bcm47xxnflash_ops_bcm4706_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 	struct bcma_drv_cc *cc = b47n->cc;
 	u32 tmp = 0;
 
@@ -341,8 +341,8 @@ static u8 bcm47xxnflash_ops_bcm4706_read_byte(struct mtd_info *mtd)
 static void bcm47xxnflash_ops_bcm4706_read_buf(struct mtd_info *mtd,
 					       uint8_t *buf, int len)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 
 	switch (b47n->curr_command) {
 	case NAND_CMD_READ0:
@@ -357,8 +357,8 @@ static void bcm47xxnflash_ops_bcm4706_read_buf(struct mtd_info *mtd,
 static void bcm47xxnflash_ops_bcm4706_write_buf(struct mtd_info *mtd,
 						const uint8_t *buf, int len)
 {
-	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
-	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct bcm47xxnflash *b47n = nand_get_controller_data(nand_chip);
 
 	switch (b47n->curr_command) {
 	case NAND_CMD_SEQIN:
@@ -421,7 +421,7 @@ int bcm47xxnflash_ops_bcm4706_init(struct bcm47xxnflash *b47n)
 			(w4 << 24 | w3 << 18 | w2 << 12 | w1 << 6 | w0));
 
 	/* Scan NAND */
-	err = nand_scan(&b47n->mtd, 1);
+	err = nand_scan(nand_to_mtd(&b47n->nand_chip), 1);
 	if (err) {
 		pr_err("Could not scan NAND flash: %d\n", err);
 		goto exit;
diff --git a/drivers/mtd/nand/bf5xx_nand.c b/drivers/mtd/nand/bf5xx_nand.c
index 61bd2160717c..7f6b30e615b7 100644
--- a/drivers/mtd/nand/bf5xx_nand.c
+++ b/drivers/mtd/nand/bf5xx_nand.c
@@ -142,7 +142,6 @@ static struct nand_ecclayout bootrom_ecclayout = {
 struct bf5xx_nand_info {
 	/* mtd info */
 	struct nand_hw_control		controller;
-	struct mtd_info			mtd;
 	struct nand_chip		chip;
 
 	/* platform info */
@@ -160,7 +159,8 @@ struct bf5xx_nand_info {
  */
 static struct bf5xx_nand_info *mtd_to_nand_info(struct mtd_info *mtd)
 {
-	return container_of(mtd, struct bf5xx_nand_info, mtd);
+	return container_of(mtd_to_nand(mtd), struct bf5xx_nand_info,
+			    chip);
 }
 
 static struct bf5xx_nand_info *to_nand_info(struct platform_device *pdev)
@@ -252,7 +252,7 @@ static int bf5xx_nand_correct_data_256(struct mtd_info *mtd, u_char *dat,
 	 */
 	if (hweight32(syndrome[0]) == 1) {
 		dev_err(info->device, "ECC data was incorrect!\n");
-		return 1;
+		return -EBADMSG;
 	}
 
 	syndrome[1] = (calced & 0x7FF) ^ (stored & 0x7FF);
@@ -285,7 +285,7 @@ static int bf5xx_nand_correct_data_256(struct mtd_info *mtd, u_char *dat,
 		data = data ^ (0x1 << failing_bit);
 		*(dat + failing_byte) = data;
 
-		return 0;
+		return 1;
 	}
 
 	/*
@@ -298,26 +298,34 @@ static int bf5xx_nand_correct_data_256(struct mtd_info *mtd, u_char *dat,
 	dev_err(info->device,
 		"Please discard data, mark bad block\n");
 
-	return 1;
+	return -EBADMSG;
 }
 
 static int bf5xx_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 					u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *chip = mtd->priv;
-	int ret;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	int ret, bitflips = 0;
 
 	ret = bf5xx_nand_correct_data_256(mtd, dat, read_ecc, calc_ecc);
+	if (ret < 0)
+		return ret;
+
+	bitflips = ret;
 
 	/* If ecc size is 512, correct second 256 bytes */
 	if (chip->ecc.size == 512) {
 		dat += 256;
 		read_ecc += 3;
 		calc_ecc += 3;
-		ret |= bf5xx_nand_correct_data_256(mtd, dat, read_ecc, calc_ecc);
+		ret = bf5xx_nand_correct_data_256(mtd, dat, read_ecc, calc_ecc);
+		if (ret < 0)
+			return ret;
+
+		bitflips += ret;
 	}
 
-	return ret;
+	return bitflips;
 }
 
 static void bf5xx_nand_enable_hwecc(struct mtd_info *mtd, int mode)
@@ -329,7 +337,7 @@ static int bf5xx_nand_calculate_ecc(struct mtd_info *mtd,
 		const u_char *dat, u_char *ecc_code)
 {
 	struct bf5xx_nand_info *info = mtd_to_nand_info(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 ecc0, ecc1;
 	u32 code[2];
 	u8 *p;
@@ -466,7 +474,7 @@ static void bf5xx_nand_dma_rw(struct mtd_info *mtd,
 				uint8_t *buf, int is_read)
 {
 	struct bf5xx_nand_info *info = mtd_to_nand_info(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	unsigned short val;
 
 	dev_dbg(info->device, " mtd->%p, buf->%p, is_read %d\n",
@@ -532,7 +540,7 @@ static void bf5xx_nand_dma_read_buf(struct mtd_info *mtd,
 					uint8_t *buf, int len)
 {
 	struct bf5xx_nand_info *info = mtd_to_nand_info(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	dev_dbg(info->device, "mtd->%p, buf->%p, int %d\n", mtd, buf, len);
 
@@ -546,7 +554,7 @@ static void bf5xx_nand_dma_write_buf(struct mtd_info *mtd,
 				const uint8_t *buf, int len)
 {
 	struct bf5xx_nand_info *info = mtd_to_nand_info(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	dev_dbg(info->device, "mtd->%p, buf->%p, len %d\n", mtd, buf, len);
 
@@ -660,7 +668,7 @@ static int bf5xx_nand_hw_init(struct bf5xx_nand_info *info)
  */
 static int bf5xx_nand_add_partition(struct bf5xx_nand_info *info)
 {
-	struct mtd_info *mtd = &info->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&info->chip);
 	struct mtd_partition *parts = info->platform->partitions;
 	int nr = info->platform->nr_partitions;
 
@@ -675,7 +683,7 @@ static int bf5xx_nand_remove(struct platform_device *pdev)
 	 * and their partitions, then go through freeing the
 	 * resources used
 	 */
-	nand_release(&info->mtd);
+	nand_release(nand_to_mtd(&info->chip));
 
 	peripheral_free_list(bfin_nfc_pin_req);
 	bf5xx_nand_dma_remove(info);
@@ -685,7 +693,7 @@ static int bf5xx_nand_remove(struct platform_device *pdev)
 
 static int bf5xx_nand_scan(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret;
 
 	ret = nand_scan_ident(mtd, 1, NULL);
@@ -756,6 +764,7 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 
 	/* initialise chip data struct */
 	chip = &info->chip;
+	mtd = nand_to_mtd(&info->chip);
 
 	if (plat->data_width)
 		chip->options |= NAND_BUSWIDTH_16;
@@ -772,7 +781,7 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 	chip->cmd_ctrl     = bf5xx_nand_hwcontrol;
 	chip->dev_ready    = bf5xx_nand_devready;
 
-	chip->priv	   = &info->mtd;
+	nand_set_controller_data(chip, mtd);
 	chip->controller   = &info->controller;
 
 	chip->IO_ADDR_R    = (void __iomem *) NFC_READ;
@@ -781,8 +790,6 @@ static int bf5xx_nand_probe(struct platform_device *pdev)
 	chip->chip_delay   = 0;
 
 	/* initialise mtd info data struct */
-	mtd 		= &info->mtd;
-	mtd->priv	= chip;
 	mtd->dev.parent = &pdev->dev;
 
 	/* initialise the hardware */
diff --git a/drivers/mtd/nand/brcmnand/Makefile b/drivers/mtd/nand/brcmnand/Makefile
index 3b1fbfd27d4f..b28ffb59eb43 100644
--- a/drivers/mtd/nand/brcmnand/Makefile
+++ b/drivers/mtd/nand/brcmnand/Makefile
@@ -2,5 +2,6 @@
 # more specific iproc_nand.o, for instance
 obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= iproc_nand.o
 obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= bcm63138_nand.o
+obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= bcm6368_nand.o
 obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmstb_nand.o
 obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmnand.o
diff --git a/drivers/mtd/nand/brcmnand/bcm6368_nand.c b/drivers/mtd/nand/brcmnand/bcm6368_nand.c
new file mode 100644
index 000000000000..34c91b0e1e69
--- /dev/null
+++ b/drivers/mtd/nand/brcmnand/bcm6368_nand.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright 2015 Simon Arlott
+ *
+ * This program is free software; you can 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.
+ *
+ * Derived from bcm63138_nand.c:
+ * Copyright © 2015 Broadcom Corporation
+ *
+ * Derived from bcm963xx_4.12L.06B_consumer/shared/opensource/include/bcm963xx/63268_map_part.h:
+ * Copyright 2000-2010 Broadcom Corporation
+ *
+ * Derived from bcm963xx_4.12L.06B_consumer/shared/opensource/flash/nandflash.c:
+ * Copyright 2000-2010 Broadcom Corporation
+ */
+
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "brcmnand.h"
+
+struct bcm6368_nand_soc {
+	struct brcmnand_soc soc;
+	void __iomem *base;
+};
+
+#define BCM6368_NAND_INT		0x00
+#define  BCM6368_NAND_STATUS_SHIFT	0
+#define  BCM6368_NAND_STATUS_MASK	(0xfff << BCM6368_NAND_STATUS_SHIFT)
+#define  BCM6368_NAND_ENABLE_SHIFT	16
+#define  BCM6368_NAND_ENABLE_MASK	(0xffff << BCM6368_NAND_ENABLE_SHIFT)
+#define BCM6368_NAND_BASE_ADDR0	0x04
+#define BCM6368_NAND_BASE_ADDR1	0x0c
+
+enum {
+	BCM6368_NP_READ		= BIT(0),
+	BCM6368_BLOCK_ERASE	= BIT(1),
+	BCM6368_COPY_BACK	= BIT(2),
+	BCM6368_PAGE_PGM	= BIT(3),
+	BCM6368_CTRL_READY	= BIT(4),
+	BCM6368_DEV_RBPIN	= BIT(5),
+	BCM6368_ECC_ERR_UNC	= BIT(6),
+	BCM6368_ECC_ERR_CORR	= BIT(7),
+};
+
+static bool bcm6368_nand_intc_ack(struct brcmnand_soc *soc)
+{
+	struct bcm6368_nand_soc *priv =
+			container_of(soc, struct bcm6368_nand_soc, soc);
+	void __iomem *mmio = priv->base + BCM6368_NAND_INT;
+	u32 val = brcmnand_readl(mmio);
+
+	if (val & (BCM6368_CTRL_READY << BCM6368_NAND_STATUS_SHIFT)) {
+		/* Ack interrupt */
+		val &= ~BCM6368_NAND_STATUS_MASK;
+		val |= BCM6368_CTRL_READY << BCM6368_NAND_STATUS_SHIFT;
+		brcmnand_writel(val, mmio);
+		return true;
+	}
+
+	return false;
+}
+
+static void bcm6368_nand_intc_set(struct brcmnand_soc *soc, bool en)
+{
+	struct bcm6368_nand_soc *priv =
+			container_of(soc, struct bcm6368_nand_soc, soc);
+	void __iomem *mmio = priv->base + BCM6368_NAND_INT;
+	u32 val = brcmnand_readl(mmio);
+
+	/* Don't ack any interrupts */
+	val &= ~BCM6368_NAND_STATUS_MASK;
+
+	if (en)
+		val |= BCM6368_CTRL_READY << BCM6368_NAND_ENABLE_SHIFT;
+	else
+		val &= ~(BCM6368_CTRL_READY << BCM6368_NAND_ENABLE_SHIFT);
+
+	brcmnand_writel(val, mmio);
+}
+
+static int bcm6368_nand_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct bcm6368_nand_soc *priv;
+	struct brcmnand_soc *soc;
+	struct resource *res;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	soc = &priv->soc;
+
+	res = platform_get_resource_byname(pdev,
+		IORESOURCE_MEM, "nand-int-base");
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(priv->base))
+		return PTR_ERR(priv->base);
+
+	soc->ctlrdy_ack = bcm6368_nand_intc_ack;
+	soc->ctlrdy_set_enabled = bcm6368_nand_intc_set;
+
+	/* Disable and ack all interrupts  */
+	brcmnand_writel(0, priv->base + BCM6368_NAND_INT);
+	brcmnand_writel(BCM6368_NAND_STATUS_MASK,
+			priv->base + BCM6368_NAND_INT);
+
+	return brcmnand_probe(pdev, soc);
+}
+
+static const struct of_device_id bcm6368_nand_of_match[] = {
+	{ .compatible = "brcm,nand-bcm6368" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcm6368_nand_of_match);
+
+static struct platform_driver bcm6368_nand_driver = {
+	.probe			= bcm6368_nand_probe,
+	.remove			= brcmnand_remove,
+	.driver = {
+		.name		= "bcm6368_nand",
+		.pm		= &brcmnand_pm_ops,
+		.of_match_table	= bcm6368_nand_of_match,
+	}
+};
+module_platform_driver(bcm6368_nand_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Simon Arlott");
+MODULE_DESCRIPTION("NAND driver for BCM6368");
diff --git a/drivers/mtd/nand/brcmnand/brcmnand.c b/drivers/mtd/nand/brcmnand/brcmnand.c
index 12c6190c6e33..844fc07d22cd 100644
--- a/drivers/mtd/nand/brcmnand/brcmnand.c
+++ b/drivers/mtd/nand/brcmnand/brcmnand.c
@@ -11,6 +11,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/clk.h>
 #include <linux/version.h>
 #include <linux/module.h>
 #include <linux/init.h>
@@ -122,6 +123,9 @@ struct brcmnand_controller {
 	/* Some SoCs provide custom interrupt status register(s) */
 	struct brcmnand_soc	*soc;
 
+	/* Some SoCs have a gateable clock for the controller */
+	struct clk		*clk;
+
 	int			cmd_pending;
 	bool			dma_pending;
 	struct completion	done;
@@ -134,7 +138,7 @@ struct brcmnand_controller {
 	dma_addr_t		dma_pa;
 
 	/* in-memory cache of the FLASH_CACHE, used only for some commands */
-	u32			flash_cache[FC_WORDS];
+	u8			flash_cache[FC_BYTES];
 
 	/* Controller revision details */
 	const u16		*reg_offsets;
@@ -176,10 +180,8 @@ struct brcmnand_cfg {
 
 struct brcmnand_host {
 	struct list_head	node;
-	struct device_node	*of_node;
 
 	struct nand_chip	chip;
-	struct mtd_info		mtd;
 	struct platform_device	*pdev;
 	int			cs;
 
@@ -874,8 +876,8 @@ static struct nand_ecclayout *brcmstb_choose_ecc_layout(
 
 static void brcmnand_wp(struct mtd_info *mtd, int wp)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct brcmnand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 
 	if ((ctrl->features & BRCMNAND_HAS_WP) && wp_on == 1) {
@@ -1040,8 +1042,8 @@ static void brcmnand_cmd_ctrl(struct mtd_info *mtd, int dat,
 
 static int brcmnand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct brcmnand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	unsigned long timeo = msecs_to_jiffies(100);
 
@@ -1075,7 +1077,7 @@ static int brcmnand_low_level_op(struct brcmnand_host *host,
 				 enum brcmnand_llop_type type, u32 data,
 				 bool last_op)
 {
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->chip);
 	struct nand_chip *chip = &host->chip;
 	struct brcmnand_controller *ctrl = host->ctrl;
 	u32 tmp;
@@ -1114,8 +1116,8 @@ static int brcmnand_low_level_op(struct brcmnand_host *host,
 static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 			     int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct brcmnand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	u64 addr = (u64)page_addr << chip->page_shift;
 	int native_cmd = 0;
@@ -1188,6 +1190,8 @@ static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 
 	if (native_cmd == CMD_PARAMETER_READ ||
 			native_cmd == CMD_PARAMETER_CHANGE_COL) {
+		/* Copy flash cache word-wise */
+		u32 *flash_cache = (u32 *)ctrl->flash_cache;
 		int i;
 
 		brcmnand_soc_data_bus_prepare(ctrl->soc);
@@ -1197,7 +1201,11 @@ static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 		 * SECTOR_SIZE_1K may invalidate it
 		 */
 		for (i = 0; i < FC_WORDS; i++)
-			ctrl->flash_cache[i] = brcmnand_read_fc(ctrl, i);
+			/*
+			 * Flash cache is big endian for parameter pages, at
+			 * least on STB SoCs
+			 */
+			flash_cache[i] = be32_to_cpu(brcmnand_read_fc(ctrl, i));
 
 		brcmnand_soc_data_bus_unprepare(ctrl->soc);
 
@@ -1214,8 +1222,8 @@ static void brcmnand_cmdfunc(struct mtd_info *mtd, unsigned command,
 
 static uint8_t brcmnand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct brcmnand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	uint8_t ret = 0;
 	int addr, offs;
@@ -1250,8 +1258,7 @@ static uint8_t brcmnand_read_byte(struct mtd_info *mtd)
 		if (host->last_byte > 0 && offs == 0)
 			chip->cmdfunc(mtd, NAND_CMD_RNDOUT, addr, -1);
 
-		ret = ctrl->flash_cache[offs >> 2] >>
-					(24 - ((offs & 0x03) << 3));
+		ret = ctrl->flash_cache[offs];
 		break;
 	case NAND_CMD_GET_FEATURES:
 		if (host->last_byte >= ONFI_SUBFEATURE_PARAM_LEN) {
@@ -1282,8 +1289,8 @@ static void brcmnand_write_buf(struct mtd_info *mtd, const uint8_t *buf,
 				   int len)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
-	struct brcmnand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 
 	switch (host->last_cmd) {
 	case NAND_CMD_SET_FEATURES:
@@ -1393,13 +1400,15 @@ static int brcmnand_read_by_pio(struct mtd_info *mtd, struct nand_chip *chip,
 				u64 addr, unsigned int trans, u32 *buf,
 				u8 *oob, u64 *err_addr)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	int i, j, ret = 0;
 
 	/* Clear error addresses */
 	brcmnand_write_reg(ctrl, BRCMNAND_UNCORR_ADDR, 0);
 	brcmnand_write_reg(ctrl, BRCMNAND_CORR_ADDR, 0);
+	brcmnand_write_reg(ctrl, BRCMNAND_UNCORR_EXT_ADDR, 0);
+	brcmnand_write_reg(ctrl, BRCMNAND_CORR_EXT_ADDR, 0);
 
 	brcmnand_write_reg(ctrl, BRCMNAND_CMD_EXT_ADDRESS,
 			(host->cs << 16) | ((addr >> 32) & 0xffff));
@@ -1454,7 +1463,7 @@ static int brcmnand_read_by_pio(struct mtd_info *mtd, struct nand_chip *chip,
 static int brcmnand_read(struct mtd_info *mtd, struct nand_chip *chip,
 			 u64 addr, unsigned int trans, u32 *buf, u8 *oob)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	u64 err_addr = 0;
 	int err;
@@ -1504,7 +1513,7 @@ static int brcmnand_read(struct mtd_info *mtd, struct nand_chip *chip,
 static int brcmnand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			      uint8_t *buf, int oob_required, int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	u8 *oob = oob_required ? (u8 *)chip->oob_poi : NULL;
 
 	return brcmnand_read(mtd, chip, host->last_addr,
@@ -1514,7 +1523,7 @@ static int brcmnand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 static int brcmnand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				  uint8_t *buf, int oob_required, int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	u8 *oob = oob_required ? (u8 *)chip->oob_poi : NULL;
 	int ret;
 
@@ -1536,7 +1545,7 @@ static int brcmnand_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
 static int brcmnand_read_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				 int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 
 	brcmnand_set_ecc_enabled(host, 0);
 	brcmnand_read(mtd, chip, (u64)page << chip->page_shift,
@@ -1546,20 +1555,10 @@ static int brcmnand_read_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
 	return 0;
 }
 
-static int brcmnand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
-				 uint32_t data_offs, uint32_t readlen,
-				 uint8_t *bufpoi, int page)
-{
-	struct brcmnand_host *host = chip->priv;
-
-	return brcmnand_read(mtd, chip, host->last_addr + data_offs,
-			readlen >> FC_SHIFT, (u32 *)bufpoi, NULL);
-}
-
 static int brcmnand_write(struct mtd_info *mtd, struct nand_chip *chip,
 			  u64 addr, const u32 *buf, u8 *oob)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_controller *ctrl = host->ctrl;
 	unsigned int i, j, trans = mtd->writesize >> FC_SHIFT;
 	int status, ret = 0;
@@ -1630,7 +1629,7 @@ out:
 static int brcmnand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 			       const uint8_t *buf, int oob_required, int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	void *oob = oob_required ? chip->oob_poi : NULL;
 
 	brcmnand_write(mtd, chip, host->last_addr, (const u32 *)buf, oob);
@@ -1641,7 +1640,7 @@ static int brcmnand_write_page_raw(struct mtd_info *mtd,
 				   struct nand_chip *chip, const uint8_t *buf,
 				   int oob_required, int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	void *oob = oob_required ? chip->oob_poi : NULL;
 
 	brcmnand_set_ecc_enabled(host, 0);
@@ -1660,7 +1659,7 @@ static int brcmnand_write_oob(struct mtd_info *mtd, struct nand_chip *chip,
 static int brcmnand_write_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				  int page)
 {
-	struct brcmnand_host *host = chip->priv;
+	struct brcmnand_host *host = nand_get_controller_data(chip);
 	int ret;
 
 	brcmnand_set_ecc_enabled(host, 0);
@@ -1806,7 +1805,7 @@ static inline int get_blk_adr_bytes(u64 size, u32 writesize)
 
 static int brcmnand_setup_dev(struct brcmnand_host *host)
 {
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->chip);
 	struct nand_chip *chip = &host->chip;
 	struct brcmnand_controller *ctrl = host->ctrl;
 	struct brcmnand_cfg *cfg = &host->hwcfg;
@@ -1816,7 +1815,7 @@ static int brcmnand_setup_dev(struct brcmnand_host *host)
 
 	memset(cfg, 0, sizeof(*cfg));
 
-	ret = of_property_read_u32(chip->flash_node,
+	ret = of_property_read_u32(nand_get_flash_node(chip),
 				   "brcm,nand-oob-sector-size",
 				   &oob_sector);
 	if (ret) {
@@ -1905,16 +1904,14 @@ static int brcmnand_setup_dev(struct brcmnand_host *host)
 	return 0;
 }
 
-static int brcmnand_init_cs(struct brcmnand_host *host)
+static int brcmnand_init_cs(struct brcmnand_host *host, struct device_node *dn)
 {
 	struct brcmnand_controller *ctrl = host->ctrl;
-	struct device_node *dn = host->of_node;
 	struct platform_device *pdev = host->pdev;
 	struct mtd_info *mtd;
 	struct nand_chip *chip;
 	int ret;
 	u16 cfg_offs;
-	struct mtd_part_parser_data ppdata = { .of_node = dn };
 
 	ret = of_property_read_u32(dn, "reg", &host->cs);
 	if (ret) {
@@ -1922,12 +1919,11 @@ static int brcmnand_init_cs(struct brcmnand_host *host)
 		return -ENXIO;
 	}
 
-	mtd = &host->mtd;
+	mtd = nand_to_mtd(&host->chip);
 	chip = &host->chip;
 
-	chip->flash_node = dn;
-	chip->priv = host;
-	mtd->priv = chip;
+	nand_set_flash_node(chip, dn);
+	nand_set_controller_data(chip, host);
 	mtd->name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "brcmnand.%d",
 				   host->cs);
 	mtd->owner = THIS_MODULE;
@@ -1945,7 +1941,6 @@ static int brcmnand_init_cs(struct brcmnand_host *host)
 
 	chip->ecc.mode = NAND_ECC_HW;
 	chip->ecc.read_page = brcmnand_read_page;
-	chip->ecc.read_subpage = brcmnand_read_subpage;
 	chip->ecc.write_page = brcmnand_write_page;
 	chip->ecc.read_page_raw = brcmnand_read_page_raw;
 	chip->ecc.write_page_raw = brcmnand_write_page_raw;
@@ -1993,7 +1988,7 @@ static int brcmnand_init_cs(struct brcmnand_host *host)
 	if (nand_scan_tail(mtd))
 		return -ENXIO;
 
-	return mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	return mtd_device_register(mtd, NULL, 0);
 }
 
 static void brcmnand_save_restore_cs_config(struct brcmnand_host *host,
@@ -2067,8 +2062,8 @@ static int brcmnand_resume(struct device *dev)
 	}
 
 	list_for_each_entry(host, &ctrl->host_list, node) {
-		struct mtd_info *mtd = &host->mtd;
-		struct nand_chip *chip = mtd->priv;
+		struct nand_chip *chip = &host->chip;
+		struct mtd_info *mtd = nand_to_mtd(chip);
 
 		brcmnand_save_restore_cs_config(host, 1);
 
@@ -2134,10 +2129,24 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 	if (IS_ERR(ctrl->nand_base))
 		return PTR_ERR(ctrl->nand_base);
 
+	/* Enable clock before using NAND registers */
+	ctrl->clk = devm_clk_get(dev, "nand");
+	if (!IS_ERR(ctrl->clk)) {
+		ret = clk_prepare_enable(ctrl->clk);
+		if (ret)
+			return ret;
+	} else {
+		ret = PTR_ERR(ctrl->clk);
+		if (ret == -EPROBE_DEFER)
+			return ret;
+
+		ctrl->clk = NULL;
+	}
+
 	/* Initialize NAND revision */
 	ret = brcmnand_revision_init(ctrl);
 	if (ret)
-		return ret;
+		goto err;
 
 	/*
 	 * Most chips have this cache at a fixed offset within 'nand' block.
@@ -2146,8 +2155,10 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "nand-cache");
 	if (res) {
 		ctrl->nand_fc = devm_ioremap_resource(dev, res);
-		if (IS_ERR(ctrl->nand_fc))
-			return PTR_ERR(ctrl->nand_fc);
+		if (IS_ERR(ctrl->nand_fc)) {
+			ret = PTR_ERR(ctrl->nand_fc);
+			goto err;
+		}
 	} else {
 		ctrl->nand_fc = ctrl->nand_base +
 				ctrl->reg_offsets[BRCMNAND_FC_BASE];
@@ -2157,8 +2168,10 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "flash-dma");
 	if (res) {
 		ctrl->flash_dma_base = devm_ioremap_resource(dev, res);
-		if (IS_ERR(ctrl->flash_dma_base))
-			return PTR_ERR(ctrl->flash_dma_base);
+		if (IS_ERR(ctrl->flash_dma_base)) {
+			ret = PTR_ERR(ctrl->flash_dma_base);
+			goto err;
+		}
 
 		flash_dma_writel(ctrl, FLASH_DMA_MODE, 1); /* linked-list */
 		flash_dma_writel(ctrl, FLASH_DMA_ERROR_STATUS, 0);
@@ -2167,13 +2180,16 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 		ctrl->dma_desc = dmam_alloc_coherent(dev,
 						     sizeof(*ctrl->dma_desc),
 						     &ctrl->dma_pa, GFP_KERNEL);
-		if (!ctrl->dma_desc)
-			return -ENOMEM;
+		if (!ctrl->dma_desc) {
+			ret = -ENOMEM;
+			goto err;
+		}
 
 		ctrl->dma_irq = platform_get_irq(pdev, 1);
 		if ((int)ctrl->dma_irq < 0) {
 			dev_err(dev, "missing FLASH_DMA IRQ\n");
-			return -ENODEV;
+			ret = -ENODEV;
+			goto err;
 		}
 
 		ret = devm_request_irq(dev, ctrl->dma_irq,
@@ -2182,7 +2198,7 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 		if (ret < 0) {
 			dev_err(dev, "can't allocate IRQ %d: error %d\n",
 					ctrl->dma_irq, ret);
-			return ret;
+			goto err;
 		}
 
 		dev_info(dev, "enabling FLASH_DMA\n");
@@ -2206,7 +2222,8 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 	ctrl->irq = platform_get_irq(pdev, 0);
 	if ((int)ctrl->irq < 0) {
 		dev_err(dev, "no IRQ defined\n");
-		return -ENODEV;
+		ret = -ENODEV;
+		goto err;
 	}
 
 	/*
@@ -2230,7 +2247,7 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 	if (ret < 0) {
 		dev_err(dev, "can't allocate IRQ %d: error %d\n",
 			ctrl->irq, ret);
-		return ret;
+		goto err;
 	}
 
 	for_each_available_child_of_node(dn, child) {
@@ -2238,25 +2255,36 @@ int brcmnand_probe(struct platform_device *pdev, struct brcmnand_soc *soc)
 			struct brcmnand_host *host;
 
 			host = devm_kzalloc(dev, sizeof(*host), GFP_KERNEL);
-			if (!host)
-				return -ENOMEM;
+			if (!host) {
+				of_node_put(child);
+				ret = -ENOMEM;
+				goto err;
+			}
 			host->pdev = pdev;
 			host->ctrl = ctrl;
-			host->of_node = child;
 
-			ret = brcmnand_init_cs(host);
-			if (ret)
+			ret = brcmnand_init_cs(host, child);
+			if (ret) {
+				devm_kfree(dev, host);
 				continue; /* Try all chip-selects */
+			}
 
 			list_add_tail(&host->node, &ctrl->host_list);
 		}
 	}
 
 	/* No chip-selects could initialize properly */
-	if (list_empty(&ctrl->host_list))
-		return -ENODEV;
+	if (list_empty(&ctrl->host_list)) {
+		ret = -ENODEV;
+		goto err;
+	}
 
 	return 0;
+
+err:
+	clk_disable_unprepare(ctrl->clk);
+	return ret;
+
 }
 EXPORT_SYMBOL_GPL(brcmnand_probe);
 
@@ -2266,7 +2294,9 @@ int brcmnand_remove(struct platform_device *pdev)
 	struct brcmnand_host *host;
 
 	list_for_each_entry(host, &ctrl->host_list, node)
-		nand_release(&host->mtd);
+		nand_release(nand_to_mtd(&host->chip));
+
+	clk_disable_unprepare(ctrl->clk);
 
 	dev_set_drvdata(&pdev->dev, NULL);
 
diff --git a/drivers/mtd/nand/cafe_nand.c b/drivers/mtd/nand/cafe_nand.c
index 9de78d2a2eb1..aa1a616b9fb6 100644
--- a/drivers/mtd/nand/cafe_nand.c
+++ b/drivers/mtd/nand/cafe_nand.c
@@ -101,7 +101,8 @@ static const char *part_probes[] = { "cmdlinepart", "RedBoot", NULL };
 
 static int cafe_device_ready(struct mtd_info *mtd)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 	int result = !!(cafe_readl(cafe, NAND_STATUS) & 0x40000000);
 	uint32_t irqs = cafe_readl(cafe, NAND_IRQ);
 
@@ -117,7 +118,8 @@ static int cafe_device_ready(struct mtd_info *mtd)
 
 static void cafe_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
 	if (usedma)
 		memcpy(cafe->dmabuf + cafe->datalen, buf, len);
@@ -132,7 +134,8 @@ static void cafe_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 
 static void cafe_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
 	if (usedma)
 		memcpy(buf, cafe->dmabuf + cafe->datalen, len);
@@ -146,7 +149,8 @@ static void cafe_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 
 static uint8_t cafe_read_byte(struct mtd_info *mtd)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 	uint8_t d;
 
 	cafe_read_buf(mtd, &d, 1);
@@ -158,7 +162,8 @@ static uint8_t cafe_read_byte(struct mtd_info *mtd)
 static void cafe_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 			      int column, int page_addr)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 	int adrbytes = 0;
 	uint32_t ctl1;
 	uint32_t doneint = 0x80000000;
@@ -313,7 +318,8 @@ static void cafe_nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 
 static void cafe_select_chip(struct mtd_info *mtd, int chipnr)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
 	cafe_dev_dbg(&cafe->pdev->dev, "select_chip %d\n", chipnr);
 
@@ -328,7 +334,8 @@ static void cafe_select_chip(struct mtd_info *mtd, int chipnr)
 static irqreturn_t cafe_nand_interrupt(int irq, void *id)
 {
 	struct mtd_info *mtd = id;
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 	uint32_t irqs = cafe_readl(cafe, NAND_IRQ);
 	cafe_writel(cafe, irqs & ~0x90000000, NAND_IRQ);
 	if (!irqs)
@@ -377,7 +384,7 @@ static int cafe_nand_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
 static int cafe_nand_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			       uint8_t *buf, int oob_required, int page)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 	unsigned int max_bitflips = 0;
 
 	cafe_dev_dbg(&cafe->pdev->dev, "ECC result %08x SYN1,2 %08x\n",
@@ -519,7 +526,7 @@ static int cafe_nand_write_page_lowlevel(struct mtd_info *mtd,
 					  const uint8_t *buf, int oob_required,
 					  int page)
 {
-	struct cafe_priv *cafe = mtd->priv;
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
 	chip->write_buf(mtd, buf, mtd->writesize);
 	chip->write_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -598,13 +605,13 @@ static int cafe_nand_probe(struct pci_dev *pdev,
 
 	pci_set_master(pdev);
 
-	mtd = kzalloc(sizeof(*mtd) + sizeof(struct cafe_priv), GFP_KERNEL);
-	if (!mtd)
+	cafe = kzalloc(sizeof(*cafe), GFP_KERNEL);
+	if (!cafe)
 		return  -ENOMEM;
-	cafe = (void *)(&mtd[1]);
 
+	mtd = nand_to_mtd(&cafe->nand);
 	mtd->dev.parent = &pdev->dev;
-	mtd->priv = cafe;
+	nand_set_controller_data(&cafe->nand, cafe);
 
 	cafe->pdev = pdev;
 	cafe->mmio = pci_iomap(pdev, 0, 0);
@@ -784,7 +791,7 @@ static int cafe_nand_probe(struct pci_dev *pdev,
  out_ior:
 	pci_iounmap(pdev, cafe->mmio);
  out_free_mtd:
-	kfree(mtd);
+	kfree(cafe);
  out:
 	return err;
 }
@@ -792,7 +799,8 @@ static int cafe_nand_probe(struct pci_dev *pdev,
 static void cafe_nand_remove(struct pci_dev *pdev)
 {
 	struct mtd_info *mtd = pci_get_drvdata(pdev);
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
 	/* Disable NAND IRQ in global IRQ mask register */
 	cafe_writel(cafe, ~1 & cafe_readl(cafe, GLOBAL_IRQ_MASK), GLOBAL_IRQ_MASK);
@@ -804,7 +812,7 @@ static void cafe_nand_remove(struct pci_dev *pdev)
 			2112 + sizeof(struct nand_buffers) +
 			mtd->writesize + mtd->oobsize,
 			cafe->dmabuf, cafe->dmaaddr);
-	kfree(mtd);
+	kfree(cafe);
 }
 
 static const struct pci_device_id cafe_nand_tbl[] = {
@@ -819,7 +827,8 @@ static int cafe_nand_resume(struct pci_dev *pdev)
 {
 	uint32_t ctrl;
 	struct mtd_info *mtd = pci_get_drvdata(pdev);
-	struct cafe_priv *cafe = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct cafe_priv *cafe = nand_get_controller_data(chip);
 
        /* Start off by resetting the NAND controller completely */
 	cafe_writel(cafe, 1, NAND_RESET);
diff --git a/drivers/mtd/nand/cmx270_nand.c b/drivers/mtd/nand/cmx270_nand.c
index 66ec95e6ca6c..6f97ebba52c4 100644
--- a/drivers/mtd/nand/cmx270_nand.c
+++ b/drivers/mtd/nand/cmx270_nand.c
@@ -53,7 +53,7 @@ static struct mtd_partition partition_info[] = {
 
 static u_char cmx270_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	return (readl(this->IO_ADDR_R) >> 16);
 }
@@ -61,7 +61,7 @@ static u_char cmx270_read_byte(struct mtd_info *mtd)
 static void cmx270_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	for (i=0; i<len; i++)
 		writel((*buf++ << 16), this->IO_ADDR_W);
@@ -70,7 +70,7 @@ static void cmx270_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 static void cmx270_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	for (i=0; i<len; i++)
 		*buf++ = readl(this->IO_ADDR_R) >> 16;
@@ -94,7 +94,7 @@ static void nand_cs_off(void)
 static void cmx270_hwcontrol(struct mtd_info *mtd, int dat,
 			     unsigned int ctrl)
 {
-	struct nand_chip* this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	unsigned int nandaddr = (unsigned int)this->IO_ADDR_W;
 
 	dsb();
@@ -160,10 +160,8 @@ static int __init cmx270_init(void)
 	gpio_direction_input(GPIO_NAND_RB);
 
 	/* Allocate memory for MTD device structure and private data */
-	cmx270_nand_mtd = kzalloc(sizeof(struct mtd_info) +
-				  sizeof(struct nand_chip),
-				  GFP_KERNEL);
-	if (!cmx270_nand_mtd) {
+	this = kzalloc(sizeof(struct nand_chip), GFP_KERNEL);
+	if (!this) {
 		ret = -ENOMEM;
 		goto err_kzalloc;
 	}
@@ -175,12 +173,10 @@ static int __init cmx270_init(void)
 		goto err_ioremap;
 	}
 
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&cmx270_nand_mtd[1]);
+	cmx270_nand_mtd = nand_to_mtd(this);
 
 	/* Link the private data with the MTD structure */
 	cmx270_nand_mtd->owner = THIS_MODULE;
-	cmx270_nand_mtd->priv = this;
 
 	/* insert callbacks */
 	this->IO_ADDR_R = cmx270_nand_io;
@@ -216,7 +212,7 @@ static int __init cmx270_init(void)
 err_scan:
 	iounmap(cmx270_nand_io);
 err_ioremap:
-	kfree(cmx270_nand_mtd);
+	kfree(this);
 err_kzalloc:
 	gpio_free(GPIO_NAND_RB);
 err_gpio_request:
@@ -240,8 +236,7 @@ static void __exit cmx270_cleanup(void)
 
 	iounmap(cmx270_nand_io);
 
-	/* Free the MTD device structure */
-	kfree (cmx270_nand_mtd);
+	kfree(mtd_to_nand(cmx270_nand_mtd));
 }
 module_exit(cmx270_cleanup);
 
diff --git a/drivers/mtd/nand/cs553x_nand.c b/drivers/mtd/nand/cs553x_nand.c
index aec6045058c7..a65e4e0f57a1 100644
--- a/drivers/mtd/nand/cs553x_nand.c
+++ b/drivers/mtd/nand/cs553x_nand.c
@@ -97,7 +97,7 @@
 
 static void cs553x_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	while (unlikely(len > 0x800)) {
 		memcpy_fromio(buf, this->IO_ADDR_R, 0x800);
@@ -109,7 +109,7 @@ static void cs553x_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 
 static void cs553x_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	while (unlikely(len > 0x800)) {
 		memcpy_toio(this->IO_ADDR_R, buf, 0x800);
@@ -121,13 +121,13 @@ static void cs553x_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 
 static unsigned char cs553x_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	return readb(this->IO_ADDR_R);
 }
 
 static void cs553x_write_byte(struct mtd_info *mtd, u_char byte)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i = 100000;
 
 	while (i && readb(this->IO_ADDR_R + MM_NAND_STS) & CS_NAND_CTLR_BUSY) {
@@ -140,7 +140,7 @@ static void cs553x_write_byte(struct mtd_info *mtd, u_char byte)
 static void cs553x_hwcontrol(struct mtd_info *mtd, int cmd,
 			     unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	void __iomem *mmio_base = this->IO_ADDR_R;
 	if (ctrl & NAND_CTRL_CHANGE) {
 		unsigned char ctl = (ctrl & ~NAND_CTRL_CHANGE ) ^ 0x01;
@@ -152,7 +152,7 @@ static void cs553x_hwcontrol(struct mtd_info *mtd, int cmd,
 
 static int cs553x_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	void __iomem *mmio_base = this->IO_ADDR_R;
 	unsigned char foo = readb(mmio_base + MM_NAND_STS);
 
@@ -161,7 +161,7 @@ static int cs553x_device_ready(struct mtd_info *mtd)
 
 static void cs_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	void __iomem *mmio_base = this->IO_ADDR_R;
 
 	writeb(0x07, mmio_base + MM_NAND_ECC_CTL);
@@ -170,7 +170,7 @@ static void cs_enable_hwecc(struct mtd_info *mtd, int mode)
 static int cs_calculate_ecc(struct mtd_info *mtd, const u_char *dat, u_char *ecc_code)
 {
 	uint32_t ecc;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	void __iomem *mmio_base = this->IO_ADDR_R;
 
 	ecc = readl(mmio_base + MM_NAND_STS);
@@ -197,17 +197,15 @@ static int __init cs553x_init_one(int cs, int mmio, unsigned long adr)
 	}
 
 	/* Allocate memory for MTD device structure and private data */
-	new_mtd = kzalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip), GFP_KERNEL);
-	if (!new_mtd) {
+	this = kzalloc(sizeof(struct nand_chip), GFP_KERNEL);
+	if (!this) {
 		err = -ENOMEM;
 		goto out;
 	}
 
-	/* Get pointer to private data */
-	this = (struct nand_chip *)(&new_mtd[1]);
+	new_mtd = nand_to_mtd(this);
 
 	/* Link the private data with the MTD structure */
-	new_mtd->priv = this;
 	new_mtd->owner = THIS_MODULE;
 
 	/* map physical address */
@@ -257,7 +255,7 @@ out_free:
 out_ior:
 	iounmap(this->IO_ADDR_R);
 out_mtd:
-	kfree(new_mtd);
+	kfree(this);
 out:
 	return err;
 }
@@ -337,19 +335,19 @@ static void __exit cs553x_cleanup(void)
 		if (!mtd)
 			continue;
 
-		this = cs553x_mtd[i]->priv;
+		this = mtd_to_nand(mtd);
 		mmio_base = this->IO_ADDR_R;
 
 		/* Release resources, unregister device */
-		nand_release(cs553x_mtd[i]);
-		kfree(cs553x_mtd[i]->name);
+		nand_release(mtd);
+		kfree(mtd->name);
 		cs553x_mtd[i] = NULL;
 
 		/* unmap physical address */
 		iounmap(mmio_base);
 
 		/* Free the MTD device structure */
-		kfree(mtd);
+		kfree(this);
 	}
 }
 
diff --git a/drivers/mtd/nand/davinci_nand.c b/drivers/mtd/nand/davinci_nand.c
index c72313d66cf6..8cb821b6686e 100644
--- a/drivers/mtd/nand/davinci_nand.c
+++ b/drivers/mtd/nand/davinci_nand.c
@@ -53,7 +53,6 @@
  * outputs in a "wire-AND" configuration, with no per-chip signals.
  */
 struct davinci_nand_info {
-	struct mtd_info		mtd;
 	struct nand_chip	chip;
 	struct nand_ecclayout	ecclayout;
 
@@ -80,8 +79,10 @@ struct davinci_nand_info {
 static DEFINE_SPINLOCK(davinci_nand_lock);
 static bool ecc4_busy;
 
-#define to_davinci_nand(m) container_of(m, struct davinci_nand_info, mtd)
-
+static inline struct davinci_nand_info *to_davinci_nand(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct davinci_nand_info, chip);
+}
 
 static inline unsigned int davinci_nand_readl(struct davinci_nand_info *info,
 		int offset)
@@ -106,7 +107,7 @@ static void nand_davinci_hwcontrol(struct mtd_info *mtd, int cmd,
 {
 	struct davinci_nand_info	*info = to_davinci_nand(mtd);
 	uint32_t			addr = info->current_cs;
-	struct nand_chip		*nand = mtd->priv;
+	struct nand_chip		*nand = mtd_to_nand(mtd);
 
 	/* Did the control lines change? */
 	if (ctrl & NAND_CTRL_CHANGE) {
@@ -192,7 +193,7 @@ static int nand_davinci_calculate_1bit(struct mtd_info *mtd,
 static int nand_davinci_correct_1bit(struct mtd_info *mtd, u_char *dat,
 				     u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint32_t eccNand = read_ecc[0] | (read_ecc[1] << 8) |
 					  (read_ecc[2] << 16);
 	uint32_t eccCalc = calc_ecc[0] | (calc_ecc[1] << 8) |
@@ -206,7 +207,7 @@ static int nand_davinci_correct_1bit(struct mtd_info *mtd, u_char *dat,
 				dat[diff >> (12 + 3)] ^= BIT((diff >> 12) & 7);
 				return 1;
 			} else {
-				return -1;
+				return -EBADMSG;
 			}
 		} else if (!(diff & (diff - 1))) {
 			/* Single bit ECC error in the ECC itself,
@@ -214,7 +215,7 @@ static int nand_davinci_correct_1bit(struct mtd_info *mtd, u_char *dat,
 			return 1;
 		} else {
 			/* Uncorrectable error */
-			return -1;
+			return -EBADMSG;
 		}
 
 	}
@@ -316,14 +317,6 @@ static int nand_davinci_correct_4bit(struct mtd_info *mtd,
 	unsigned num_errors, corrected;
 	unsigned long timeo;
 
-	/* All bytes 0xff?  It's an erased page; ignore its ECC. */
-	for (i = 0; i < 10; i++) {
-		if (ecc_code[i] != 0xff)
-			goto compare;
-	}
-	return 0;
-
-compare:
 	/* Unpack ten bytes into eight 10 bit values.  We know we're
 	 * little-endian, and use type punning for less shifting/masking.
 	 */
@@ -390,7 +383,7 @@ compare:
 			return 0;
 		case 1:		/* five or more errors detected */
 			davinci_nand_readl(info, NAND_ERR_ERRVAL1_OFFSET);
-			return -EIO;
+			return -EBADMSG;
 		case 2:		/* error addresses computed */
 		case 3:
 			num_errors = 1 + ((fsr >> 16) & 0x03);
@@ -447,7 +440,7 @@ correct:
  */
 static void nand_davinci_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if ((0x03 & ((unsigned)buf)) == 0 && (0x03 & len) == 0)
 		ioread32_rep(chip->IO_ADDR_R, buf, len >> 2);
@@ -460,7 +453,7 @@ static void nand_davinci_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void nand_davinci_write_buf(struct mtd_info *mtd,
 		const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if ((0x03 & ((unsigned)buf)) == 0 && (0x03 & len) == 0)
 		iowrite32_rep(chip->IO_ADDR_R, buf, len >> 2);
@@ -636,6 +629,7 @@ static int nand_davinci_probe(struct platform_device *pdev)
 	int				ret;
 	uint32_t			val;
 	nand_ecc_modes_t		ecc_mode;
+	struct mtd_info			*mtd;
 
 	pdata = nand_davinci_get_pdata(pdev);
 	if (IS_ERR(pdata))
@@ -682,8 +676,9 @@ static int nand_davinci_probe(struct platform_device *pdev)
 	info->base		= base;
 	info->vaddr		= vaddr;
 
-	info->mtd.priv		= &info->chip;
-	info->mtd.dev.parent	= &pdev->dev;
+	mtd			= nand_to_mtd(&info->chip);
+	mtd->dev.parent		= &pdev->dev;
+	nand_set_flash_node(&info->chip, pdev->dev.of_node);
 
 	info->chip.IO_ADDR_R	= vaddr;
 	info->chip.IO_ADDR_W	= vaddr;
@@ -746,6 +741,7 @@ static int nand_davinci_probe(struct platform_device *pdev)
 			info->chip.ecc.correct = nand_davinci_correct_4bit;
 			info->chip.ecc.hwctl = nand_davinci_hwctl_4bit;
 			info->chip.ecc.bytes = 10;
+			info->chip.ecc.options = NAND_ECC_GENERIC_ERASED_CHECK;
 		} else {
 			info->chip.ecc.calculate = nand_davinci_calculate_1bit;
 			info->chip.ecc.correct = nand_davinci_correct_1bit;
@@ -784,7 +780,7 @@ static int nand_davinci_probe(struct platform_device *pdev)
 	spin_unlock_irq(&davinci_nand_lock);
 
 	/* Scan to find existence of the device(s) */
-	ret = nand_scan_ident(&info->mtd, pdata->mask_chipsel ? 2 : 1, NULL);
+	ret = nand_scan_ident(mtd, pdata->mask_chipsel ? 2 : 1, NULL);
 	if (ret < 0) {
 		dev_dbg(&pdev->dev, "no NAND chip(s) found\n");
 		goto err;
@@ -796,9 +792,9 @@ static int nand_davinci_probe(struct platform_device *pdev)
 	 * usable:  10 bytes are needed, not 6.
 	 */
 	if (pdata->ecc_bits == 4) {
-		int	chunks = info->mtd.writesize / 512;
+		int	chunks = mtd->writesize / 512;
 
-		if (!chunks || info->mtd.oobsize < 16) {
+		if (!chunks || mtd->oobsize < 16) {
 			dev_dbg(&pdev->dev, "too small\n");
 			ret = -EINVAL;
 			goto err;
@@ -810,8 +806,7 @@ static int nand_davinci_probe(struct platform_device *pdev)
 		 */
 		if (chunks == 1) {
 			info->ecclayout = hwecc4_small;
-			info->ecclayout.oobfree[1].length =
-				info->mtd.oobsize - 16;
+			info->ecclayout.oobfree[1].length = mtd->oobsize - 16;
 			goto syndrome_done;
 		}
 		if (chunks == 4) {
@@ -832,20 +827,15 @@ syndrome_done:
 		info->chip.ecc.layout = &info->ecclayout;
 	}
 
-	ret = nand_scan_tail(&info->mtd);
+	ret = nand_scan_tail(mtd);
 	if (ret < 0)
 		goto err;
 
 	if (pdata->parts)
-		ret = mtd_device_parse_register(&info->mtd, NULL, NULL,
+		ret = mtd_device_parse_register(mtd, NULL, NULL,
 					pdata->parts, pdata->nr_parts);
-	else {
-		struct mtd_part_parser_data	ppdata;
-
-		ppdata.of_node = pdev->dev.of_node;
-		ret = mtd_device_parse_register(&info->mtd, NULL, &ppdata,
-						NULL, 0);
-	}
+	else
+		ret = mtd_device_register(mtd, NULL, 0);
 	if (ret < 0)
 		goto err;
 
@@ -875,7 +865,7 @@ static int nand_davinci_remove(struct platform_device *pdev)
 		ecc4_busy = false;
 	spin_unlock_irq(&davinci_nand_lock);
 
-	nand_release(&info->mtd);
+	nand_release(nand_to_mtd(&info->chip));
 
 	clk_disable_unprepare(info->clk);
 
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index 67eb2be0db87..30bf5f690f78 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -75,7 +75,10 @@ MODULE_PARM_DESC(onfi_timing_mode,
  * this macro allows us to convert from an MTD structure to our own
  * device context (denali) structure.
  */
-#define mtd_to_denali(m) container_of(m, struct denali_nand_info, mtd)
+static inline struct denali_nand_info *mtd_to_denali(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct denali_nand_info, nand);
+}
 
 /*
  * These constants are defined by the driver to enable common driver
@@ -986,6 +989,8 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 				 * than one NAND connected.
 				 */
 				if (err_byte < ECC_SECTOR_SIZE) {
+					struct mtd_info *mtd =
+						nand_to_mtd(&denali->nand);
 					int offset;
 
 					offset = (err_sector *
@@ -995,7 +1000,7 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 							err_device;
 					/* correct the ECC error */
 					buf[offset] ^= err_correction_value;
-					denali->mtd.ecc_stats.corrected++;
+					mtd->ecc_stats.corrected++;
 					bitflips++;
 				}
 			} else {
@@ -1062,7 +1067,7 @@ static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 	dma_addr_t addr = denali->buf.dma_buf;
-	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
+	size_t size = mtd->writesize + mtd->oobsize;
 	uint32_t irq_status;
 	uint32_t irq_mask = INTR_STATUS__DMA_CMD_COMP |
 						INTR_STATUS__PROGRAM_FAIL;
@@ -1160,7 +1165,7 @@ static int denali_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 
 	dma_addr_t addr = denali->buf.dma_buf;
-	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
+	size_t size = mtd->writesize + mtd->oobsize;
 
 	uint32_t irq_status;
 	uint32_t irq_mask = INTR_STATUS__ECC_TRANSACTION_DONE |
@@ -1193,14 +1198,14 @@ static int denali_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 	denali_enable_dma(denali, false);
 
 	if (check_erased_page) {
-		read_oob_data(&denali->mtd, chip->oob_poi, denali->page);
+		read_oob_data(mtd, chip->oob_poi, denali->page);
 
 		/* check ECC failures that may have occurred on erased pages */
 		if (check_erased_page) {
-			if (!is_erased(buf, denali->mtd.writesize))
-				denali->mtd.ecc_stats.failed++;
-			if (!is_erased(buf, denali->mtd.oobsize))
-				denali->mtd.ecc_stats.failed++;
+			if (!is_erased(buf, mtd->writesize))
+				mtd->ecc_stats.failed++;
+			if (!is_erased(buf, mtd->oobsize))
+				mtd->ecc_stats.failed++;
 		}
 	}
 	return max_bitflips;
@@ -1211,7 +1216,7 @@ static int denali_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 	dma_addr_t addr = denali->buf.dma_buf;
-	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
+	size_t size = mtd->writesize + mtd->oobsize;
 	uint32_t irq_mask = INTR_STATUS__DMA_CMD_COMP;
 
 	if (page != denali->page) {
@@ -1428,6 +1433,7 @@ static void denali_drv_init(struct denali_nand_info *denali)
 
 int denali_init(struct denali_nand_info *denali)
 {
+	struct mtd_info *mtd = nand_to_mtd(&denali->nand);
 	int ret;
 
 	if (denali->platform == INTEL_CE4100) {
@@ -1447,7 +1453,7 @@ int denali_init(struct denali_nand_info *denali)
 	if (!denali->buf.buf)
 		return -ENOMEM;
 
-	denali->mtd.dev.parent = denali->dev;
+	mtd->dev.parent = denali->dev;
 	denali_hw_init(denali);
 	denali_drv_init(denali);
 
@@ -1463,8 +1469,7 @@ int denali_init(struct denali_nand_info *denali)
 
 	/* now that our ISR is registered, we can enable interrupts */
 	denali_set_intr_modes(denali, true);
-	denali->mtd.name = "denali-nand";
-	denali->mtd.priv = &denali->nand;
+	mtd->name = "denali-nand";
 
 	/* register the driver with the NAND core subsystem */
 	denali->nand.select_chip = denali_select_chip;
@@ -1477,7 +1482,7 @@ int denali_init(struct denali_nand_info *denali)
 	 * this is the first stage in a two step process to register
 	 * with the nand subsystem
 	 */
-	if (nand_scan_ident(&denali->mtd, denali->max_banks, NULL)) {
+	if (nand_scan_ident(mtd, denali->max_banks, NULL)) {
 		ret = -ENXIO;
 		goto failed_req_irq;
 	}
@@ -1485,7 +1490,7 @@ int denali_init(struct denali_nand_info *denali)
 	/* allocate the right size buffer now */
 	devm_kfree(denali->dev, denali->buf.buf);
 	denali->buf.buf = devm_kzalloc(denali->dev,
-			     denali->mtd.writesize + denali->mtd.oobsize,
+			     mtd->writesize + mtd->oobsize,
 			     GFP_KERNEL);
 	if (!denali->buf.buf) {
 		ret = -ENOMEM;
@@ -1500,7 +1505,7 @@ int denali_init(struct denali_nand_info *denali)
 	}
 
 	denali->buf.dma_buf = dma_map_single(denali->dev, denali->buf.buf,
-			     denali->mtd.writesize + denali->mtd.oobsize,
+			     mtd->writesize + mtd->oobsize,
 			     DMA_BIDIRECTIONAL);
 	if (dma_mapping_error(denali->dev, denali->buf.dma_buf)) {
 		dev_err(denali->dev, "Spectra: failed to map DMA buffer\n");
@@ -1521,10 +1526,10 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.bbt_erase_shift += (denali->devnum - 1);
 	denali->nand.phys_erase_shift = denali->nand.bbt_erase_shift;
 	denali->nand.chip_shift += (denali->devnum - 1);
-	denali->mtd.writesize <<= (denali->devnum - 1);
-	denali->mtd.oobsize <<= (denali->devnum - 1);
-	denali->mtd.erasesize <<= (denali->devnum - 1);
-	denali->mtd.size = denali->nand.numchips * denali->nand.chipsize;
+	mtd->writesize <<= (denali->devnum - 1);
+	mtd->oobsize <<= (denali->devnum - 1);
+	mtd->erasesize <<= (denali->devnum - 1);
+	mtd->size = denali->nand.numchips * denali->nand.chipsize;
 	denali->bbtskipbytes *= denali->devnum;
 
 	/*
@@ -1551,16 +1556,16 @@ int denali_init(struct denali_nand_info *denali)
 	 * SLC if possible.
 	 * */
 	if (!nand_is_slc(&denali->nand) &&
-			(denali->mtd.oobsize > (denali->bbtskipbytes +
-			ECC_15BITS * (denali->mtd.writesize /
+			(mtd->oobsize > (denali->bbtskipbytes +
+			ECC_15BITS * (mtd->writesize /
 			ECC_SECTOR_SIZE)))) {
 		/* if MLC OOB size is large enough, use 15bit ECC*/
 		denali->nand.ecc.strength = 15;
 		denali->nand.ecc.layout = &nand_15bit_oob;
 		denali->nand.ecc.bytes = ECC_15BITS;
 		iowrite32(15, denali->flash_reg + ECC_CORRECTION);
-	} else if (denali->mtd.oobsize < (denali->bbtskipbytes +
-			ECC_8BITS * (denali->mtd.writesize /
+	} else if (mtd->oobsize < (denali->bbtskipbytes +
+			ECC_8BITS * (mtd->writesize /
 			ECC_SECTOR_SIZE))) {
 		pr_err("Your NAND chip OOB is not large enough to contain 8bit ECC correction codes");
 		goto failed_req_irq;
@@ -1574,11 +1579,11 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.ecc.bytes *= denali->devnum;
 	denali->nand.ecc.strength *= denali->devnum;
 	denali->nand.ecc.layout->eccbytes *=
-		denali->mtd.writesize / ECC_SECTOR_SIZE;
+		mtd->writesize / ECC_SECTOR_SIZE;
 	denali->nand.ecc.layout->oobfree[0].offset =
 		denali->bbtskipbytes + denali->nand.ecc.layout->eccbytes;
 	denali->nand.ecc.layout->oobfree[0].length =
-		denali->mtd.oobsize - denali->nand.ecc.layout->eccbytes -
+		mtd->oobsize - denali->nand.ecc.layout->eccbytes -
 		denali->bbtskipbytes;
 
 	/*
@@ -1586,7 +1591,7 @@ int denali_init(struct denali_nand_info *denali)
 	 * contained by each nand chip. blksperchip will help driver to
 	 * know how many blocks is taken by FW.
 	 */
-	denali->totalblks = denali->mtd.size >> denali->nand.phys_erase_shift;
+	denali->totalblks = mtd->size >> denali->nand.phys_erase_shift;
 	denali->blksperchip = denali->totalblks / denali->nand.numchips;
 
 	/* override the default read operations */
@@ -1599,12 +1604,12 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.ecc.write_oob = denali_write_oob;
 	denali->nand.erase = denali_erase;
 
-	if (nand_scan_tail(&denali->mtd)) {
+	if (nand_scan_tail(mtd)) {
 		ret = -ENXIO;
 		goto failed_req_irq;
 	}
 
-	ret = mtd_device_register(&denali->mtd, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 	if (ret) {
 		dev_err(denali->dev, "Spectra: Failed to register MTD: %d\n",
 				ret);
@@ -1622,9 +1627,17 @@ EXPORT_SYMBOL(denali_init);
 /* driver exit point */
 void denali_remove(struct denali_nand_info *denali)
 {
+	struct mtd_info *mtd = nand_to_mtd(&denali->nand);
+	/*
+	 * Pre-compute DMA buffer size to avoid any problems in case
+	 * nand_release() ever changes in a way that mtd->writesize and
+	 * mtd->oobsize are not reliable after this call.
+	 */
+	int bufsize = mtd->writesize + mtd->oobsize;
+
+	nand_release(mtd);
 	denali_irq_cleanup(denali->irq, denali);
-	dma_unmap_single(denali->dev, denali->buf.dma_buf,
-			 denali->mtd.writesize + denali->mtd.oobsize,
+	dma_unmap_single(denali->dev, denali->buf.dma_buf, bufsize,
 			 DMA_BIDIRECTIONAL);
 }
 EXPORT_SYMBOL(denali_remove);
diff --git a/drivers/mtd/nand/denali.h b/drivers/mtd/nand/denali.h
index 4b12cd302819..e7ab4866a5da 100644
--- a/drivers/mtd/nand/denali.h
+++ b/drivers/mtd/nand/denali.h
@@ -450,7 +450,6 @@ struct nand_buf {
 #define DT		3
 
 struct denali_nand_info {
-	struct mtd_info mtd;
 	struct nand_chip nand;
 	int flash_bank; /* currently selected chip */
 	int status;
diff --git a/drivers/mtd/nand/diskonchip.c b/drivers/mtd/nand/diskonchip.c
index 0802158a3f75..f170f3c31b34 100644
--- a/drivers/mtd/nand/diskonchip.c
+++ b/drivers/mtd/nand/diskonchip.c
@@ -74,10 +74,6 @@ struct doc_priv {
 	int (*late_init)(struct mtd_info *mtd);
 };
 
-/* This is the syndrome computed by the HW ecc generator upon reading an empty
-   page, one with all 0xff for data and stored ecc code. */
-static u_char empty_read_syndrome[6] = { 0x26, 0xff, 0x6d, 0x47, 0x73, 0x7a };
-
 /* This is the ecc value computed by the HW ecc generator upon writing an empty
    page, one with all 0xff for data. */
 static u_char empty_write_ecc[6] = { 0x4b, 0x00, 0xe2, 0x0e, 0x93, 0xf7 };
@@ -299,8 +295,8 @@ static inline int DoC_WaitReady(struct doc_priv *doc)
 
 static void doc2000_write_byte(struct mtd_info *mtd, u_char datum)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	if (debug)
@@ -311,8 +307,8 @@ static void doc2000_write_byte(struct mtd_info *mtd, u_char datum)
 
 static u_char doc2000_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	u_char ret;
 
@@ -326,8 +322,8 @@ static u_char doc2000_read_byte(struct mtd_info *mtd)
 
 static void doc2000_writebuf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 	if (debug)
@@ -343,8 +339,8 @@ static void doc2000_writebuf(struct mtd_info *mtd, const u_char *buf, int len)
 
 static void doc2000_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -358,8 +354,8 @@ static void doc2000_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 
 static void doc2000_readbuf_dword(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -379,8 +375,8 @@ static void doc2000_readbuf_dword(struct mtd_info *mtd, u_char *buf, int len)
 
 static uint16_t __init doc200x_ident_chip(struct mtd_info *mtd, int nr)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	uint16_t ret;
 
 	doc200x_select_chip(mtd, nr);
@@ -425,8 +421,8 @@ static uint16_t __init doc200x_ident_chip(struct mtd_info *mtd, int nr)
 
 static void __init doc2000_count_chips(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	uint16_t mfrid;
 	int i;
 
@@ -447,7 +443,7 @@ static void __init doc2000_count_chips(struct mtd_info *mtd)
 
 static int doc200x_wait(struct mtd_info *mtd, struct nand_chip *this)
 {
-	struct doc_priv *doc = this->priv;
+	struct doc_priv *doc = nand_get_controller_data(this);
 
 	int status;
 
@@ -461,8 +457,8 @@ static int doc200x_wait(struct mtd_info *mtd, struct nand_chip *this)
 
 static void doc2001_write_byte(struct mtd_info *mtd, u_char datum)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	WriteDOC(datum, docptr, CDSNSlowIO);
@@ -472,8 +468,8 @@ static void doc2001_write_byte(struct mtd_info *mtd, u_char datum)
 
 static u_char doc2001_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	//ReadDOC(docptr, CDSNSlowIO);
@@ -486,8 +482,8 @@ static u_char doc2001_read_byte(struct mtd_info *mtd)
 
 static void doc2001_writebuf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -499,8 +495,8 @@ static void doc2001_writebuf(struct mtd_info *mtd, const u_char *buf, int len)
 
 static void doc2001_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -516,8 +512,8 @@ static void doc2001_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 
 static u_char doc2001plus_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	u_char ret;
 
@@ -531,8 +527,8 @@ static u_char doc2001plus_read_byte(struct mtd_info *mtd)
 
 static void doc2001plus_writebuf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -549,8 +545,8 @@ static void doc2001plus_writebuf(struct mtd_info *mtd, const u_char *buf, int le
 
 static void doc2001plus_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 
@@ -580,8 +576,8 @@ static void doc2001plus_readbuf(struct mtd_info *mtd, u_char *buf, int len)
 
 static void doc2001plus_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int floor = 0;
 
@@ -607,8 +603,8 @@ static void doc2001plus_select_chip(struct mtd_info *mtd, int chip)
 
 static void doc200x_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int floor = 0;
 
@@ -638,8 +634,8 @@ static void doc200x_select_chip(struct mtd_info *mtd, int chip)
 static void doc200x_hwcontrol(struct mtd_info *mtd, int cmd,
 			      unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	if (ctrl & NAND_CTRL_CHANGE) {
@@ -661,8 +657,8 @@ static void doc200x_hwcontrol(struct mtd_info *mtd, int cmd,
 
 static void doc2001plus_command(struct mtd_info *mtd, unsigned command, int column, int page_addr)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	/*
@@ -767,8 +763,8 @@ static void doc2001plus_command(struct mtd_info *mtd, unsigned command, int colu
 
 static int doc200x_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	if (DoC_is_MillenniumPlus(doc)) {
@@ -807,8 +803,8 @@ static int doc200x_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
 
 static void doc200x_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	/* Prime the ECC engine */
@@ -826,8 +822,8 @@ static void doc200x_enable_hwecc(struct mtd_info *mtd, int mode)
 
 static void doc2001plus_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 
 	/* Prime the ECC engine */
@@ -846,8 +842,8 @@ static void doc2001plus_enable_hwecc(struct mtd_info *mtd, int mode)
 /* This code is only called on write */
 static int doc200x_calculate_ecc(struct mtd_info *mtd, const u_char *dat, unsigned char *ecc_code)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	int i;
 	int emptymatch = 1;
@@ -907,12 +903,11 @@ static int doc200x_correct_data(struct mtd_info *mtd, u_char *dat,
 				u_char *read_ecc, u_char *isnull)
 {
 	int i, ret = 0;
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	void __iomem *docptr = doc->virtadr;
 	uint8_t calc_ecc[6];
 	volatile u_char dummy;
-	int emptymatch = 1;
 
 	/* flush the pipeline */
 	if (DoC_is_2000(doc)) {
@@ -936,37 +931,9 @@ static int doc200x_correct_data(struct mtd_info *mtd, u_char *dat,
 				calc_ecc[i] = ReadDOC_(docptr, DoC_Mplus_ECCSyndrome0 + i);
 			else
 				calc_ecc[i] = ReadDOC_(docptr, DoC_ECCSyndrome0 + i);
-			if (calc_ecc[i] != empty_read_syndrome[i])
-				emptymatch = 0;
-		}
-		/* If emptymatch=1, the read syndrome is consistent with an
-		   all-0xff data and stored ecc block.  Check the stored ecc. */
-		if (emptymatch) {
-			for (i = 0; i < 6; i++) {
-				if (read_ecc[i] == 0xff)
-					continue;
-				emptymatch = 0;
-				break;
-			}
 		}
-		/* If emptymatch still =1, check the data block. */
-		if (emptymatch) {
-			/* Note: this somewhat expensive test should not be triggered
-			   often.  It could be optimized away by examining the data in
-			   the readbuf routine, and remembering the result. */
-			for (i = 0; i < 512; i++) {
-				if (dat[i] == 0xff)
-					continue;
-				emptymatch = 0;
-				break;
-			}
-		}
-		/* If emptymatch still =1, this is almost certainly a freshly-
-		   erased block, in which case the ECC will not come out right.
-		   We'll suppress the error and tell the caller everything's
-		   OK.  Because it is. */
-		if (!emptymatch)
-			ret = doc_ecc_decode(rs_decoder, dat, calc_ecc);
+
+		ret = doc_ecc_decode(rs_decoder, dat, calc_ecc);
 		if (ret > 0)
 			printk(KERN_ERR "doc200x_correct_data corrected %d errors\n", ret);
 	}
@@ -1007,8 +974,8 @@ static struct nand_ecclayout doc200x_oobinfo = {
    mh1_page in the DOC private structure. */
 static int __init find_media_headers(struct mtd_info *mtd, u_char *buf, const char *id, int findmirror)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	unsigned offs;
 	int ret;
 	size_t retlen;
@@ -1050,8 +1017,8 @@ static int __init find_media_headers(struct mtd_info *mtd, u_char *buf, const ch
 
 static inline int __init nftl_partscan(struct mtd_info *mtd, struct mtd_partition *parts)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	int ret = 0;
 	u_char *buf;
 	struct NFTLMediaHeader *mh;
@@ -1152,8 +1119,8 @@ static inline int __init nftl_partscan(struct mtd_info *mtd, struct mtd_partitio
 /* This is a stripped-down copy of the code in inftlmount.c */
 static inline int __init inftl_partscan(struct mtd_info *mtd, struct mtd_partition *parts)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	int ret = 0;
 	u_char *buf;
 	struct INFTLMediaHeader *mh;
@@ -1272,8 +1239,8 @@ static inline int __init inftl_partscan(struct mtd_info *mtd, struct mtd_partiti
 static int __init nftl_scan_bbt(struct mtd_info *mtd)
 {
 	int ret, numparts;
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	struct mtd_partition parts[2];
 
 	memset((char *)parts, 0, sizeof(parts));
@@ -1307,8 +1274,8 @@ static int __init nftl_scan_bbt(struct mtd_info *mtd)
 static int __init inftl_scan_bbt(struct mtd_info *mtd)
 {
 	int ret, numparts;
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 	struct mtd_partition parts[5];
 
 	if (this->numchips > doc->chips_per_floor) {
@@ -1360,8 +1327,8 @@ static int __init inftl_scan_bbt(struct mtd_info *mtd)
 
 static inline int __init doc2000_init(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 
 	this->read_byte = doc2000_read_byte;
 	this->write_buf = doc2000_writebuf;
@@ -1376,8 +1343,8 @@ static inline int __init doc2000_init(struct mtd_info *mtd)
 
 static inline int __init doc2001_init(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 
 	this->read_byte = doc2001_read_byte;
 	this->write_buf = doc2001_writebuf;
@@ -1406,8 +1373,8 @@ static inline int __init doc2001_init(struct mtd_info *mtd)
 
 static inline int __init doc2001plus_init(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
-	struct doc_priv *doc = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct doc_priv *doc = nand_get_controller_data(this);
 
 	this->read_byte = doc2001plus_read_byte;
 	this->write_buf = doc2001plus_writebuf;
@@ -1523,8 +1490,8 @@ static int __init doc_probe(unsigned long physadr)
 	for (mtd = doclist; mtd; mtd = doc->nextdoc) {
 		unsigned char oldval;
 		unsigned char newval;
-		nand = mtd->priv;
-		doc = nand->priv;
+		nand = mtd_to_nand(mtd);
+		doc = nand_get_controller_data(nand);
 		/* Use the alias resolution register to determine if this is
 		   in fact the same DOC aliased to a new address.  If writes
 		   to one chip's alias resolution register change the value on
@@ -1556,23 +1523,22 @@ static int __init doc_probe(unsigned long physadr)
 
 	printk(KERN_NOTICE "DiskOnChip found at 0x%lx\n", physadr);
 
-	len = sizeof(struct mtd_info) +
-	    sizeof(struct nand_chip) + sizeof(struct doc_priv) + (2 * sizeof(struct nand_bbt_descr));
-	mtd = kzalloc(len, GFP_KERNEL);
-	if (!mtd) {
+	len = sizeof(struct nand_chip) + sizeof(struct doc_priv) +
+	      (2 * sizeof(struct nand_bbt_descr));
+	nand = kzalloc(len, GFP_KERNEL);
+	if (!nand) {
 		ret = -ENOMEM;
 		goto fail;
 	}
 
-	nand			= (struct nand_chip *) (mtd + 1);
+	mtd			= nand_to_mtd(nand);
 	doc			= (struct doc_priv *) (nand + 1);
 	nand->bbt_td		= (struct nand_bbt_descr *) (doc + 1);
 	nand->bbt_md		= nand->bbt_td + 1;
 
-	mtd->priv		= nand;
 	mtd->owner		= THIS_MODULE;
 
-	nand->priv		= doc;
+	nand_set_controller_data(nand, doc);
 	nand->select_chip	= doc200x_select_chip;
 	nand->cmd_ctrl		= doc200x_hwcontrol;
 	nand->dev_ready		= doc200x_dev_ready;
@@ -1587,6 +1553,7 @@ static int __init doc_probe(unsigned long physadr)
 	nand->ecc.size		= 512;
 	nand->ecc.bytes		= 6;
 	nand->ecc.strength	= 2;
+	nand->ecc.options	= NAND_ECC_GENERIC_ERASED_CHECK;
 	nand->bbt_options	= NAND_BBT_USE_FLASH;
 	/* Skip the automatic BBT scan so we can run it manually */
 	nand->options		|= NAND_SKIP_BBTSCAN;
@@ -1615,7 +1582,7 @@ static int __init doc_probe(unsigned long physadr)
 		   haven't yet added it.  This is handled without incident by
 		   mtd_device_unregister, as far as I can tell. */
 		nand_release(mtd);
-		kfree(mtd);
+		kfree(nand);
 		goto fail;
 	}
 
@@ -1643,14 +1610,14 @@ static void release_nanddoc(void)
 	struct doc_priv *doc;
 
 	for (mtd = doclist; mtd; mtd = nextmtd) {
-		nand = mtd->priv;
-		doc = nand->priv;
+		nand = mtd_to_nand(mtd);
+		doc = nand_get_controller_data(nand);
 
 		nextmtd = doc->nextdoc;
 		nand_release(mtd);
 		iounmap(doc->virtadr);
 		release_mem_region(doc->physadr, DOC_IOREMAP_LEN);
-		kfree(mtd);
+		kfree(nand);
 	}
 }
 
diff --git a/drivers/mtd/nand/docg4.c b/drivers/mtd/nand/docg4.c
index 408cf69b854b..df4165b02c62 100644
--- a/drivers/mtd/nand/docg4.c
+++ b/drivers/mtd/nand/docg4.c
@@ -242,7 +242,7 @@ static inline void write_nop(void __iomem *docptr)
 static void docg4_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	uint16_t *p = (uint16_t *) buf;
 	len >>= 1;
 
@@ -253,7 +253,7 @@ static void docg4_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void docg4_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	uint16_t *p = (uint16_t *) buf;
 	len >>= 1;
 
@@ -297,7 +297,7 @@ static int poll_status(struct docg4_priv *doc)
 static int docg4_wait(struct mtd_info *mtd, struct nand_chip *nand)
 {
 
-	struct docg4_priv *doc = nand->priv;
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	int status = NAND_STATUS_WP;       /* inverse logic?? */
 	dev_dbg(doc->dev, "%s...\n", __func__);
 
@@ -318,8 +318,8 @@ static void docg4_select_chip(struct mtd_info *mtd, int chip)
 	 * Select among multiple cascaded chips ("floors").  Multiple floors are
 	 * not yet supported, so the only valid non-negative value is 0.
 	 */
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 
 	dev_dbg(doc->dev, "%s: chip %d\n", __func__, chip);
@@ -337,8 +337,8 @@ static void reset(struct mtd_info *mtd)
 {
 	/* full device reset */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 
 	writew(DOC_ASICMODE_RESET | DOC_ASICMODE_MDWREN,
@@ -375,8 +375,8 @@ static int correct_data(struct mtd_info *mtd, uint8_t *buf, int page)
 	 * Up to four bitflips can be corrected.
 	 */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	int i, numerrs, errpos[4];
 	const uint8_t blank_read_hwecc[8] = {
@@ -464,8 +464,8 @@ static int correct_data(struct mtd_info *mtd, uint8_t *buf, int page)
 
 static uint8_t docg4_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 
 	dev_dbg(doc->dev, "%s\n", __func__);
 
@@ -545,8 +545,8 @@ static int pageprog(struct mtd_info *mtd)
 	 * internal buffer out to the flash array, or some such.
 	 */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	int retval = 0;
 
@@ -582,8 +582,8 @@ static void sequence_reset(struct mtd_info *mtd)
 {
 	/* common starting sequence for all operations */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 
 	writew(DOC_CTRL_UNKNOWN | DOC_CTRL_CE, docptr + DOC_FLASHCONTROL);
@@ -599,8 +599,8 @@ static void read_page_prologue(struct mtd_info *mtd, uint32_t docg4_addr)
 {
 	/* first step in reading a page */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 
 	dev_dbg(doc->dev,
@@ -626,8 +626,8 @@ static void write_page_prologue(struct mtd_info *mtd, uint32_t docg4_addr)
 {
 	/* first step in writing a page */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 
 	dev_dbg(doc->dev,
@@ -691,8 +691,8 @@ static void docg4_command(struct mtd_info *mtd, unsigned command, int column,
 {
 	/* handle standard nand commands */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	uint32_t g4_addr = mtd_to_docg4_address(page_addr, column);
 
 	dev_dbg(doc->dev, "%s %x, page_addr=%x, column=%x\n",
@@ -756,7 +756,7 @@ static void docg4_command(struct mtd_info *mtd, unsigned command, int column,
 static int read_page(struct mtd_info *mtd, struct nand_chip *nand,
 		     uint8_t *buf, int page, bool use_ecc)
 {
-	struct docg4_priv *doc = nand->priv;
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	uint16_t status, edc_err, *buf16;
 	int bits_corrected = 0;
@@ -836,7 +836,7 @@ static int docg4_read_page(struct mtd_info *mtd, struct nand_chip *nand,
 static int docg4_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 			  int page)
 {
-	struct docg4_priv *doc = nand->priv;
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	uint16_t status;
 
@@ -874,8 +874,8 @@ static int docg4_read_oob(struct mtd_info *mtd, struct nand_chip *nand,
 
 static int docg4_erase_block(struct mtd_info *mtd, int page)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	uint16_t g4_page;
 
@@ -923,7 +923,7 @@ static int docg4_erase_block(struct mtd_info *mtd, int page)
 static int write_page(struct mtd_info *mtd, struct nand_chip *nand,
 		       const uint8_t *buf, bool use_ecc)
 {
-	struct docg4_priv *doc = nand->priv;
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	uint8_t ecc_buf[8];
 
@@ -1003,7 +1003,7 @@ static int docg4_write_oob(struct mtd_info *mtd, struct nand_chip *nand,
 	 */
 
 	/* note that bytes 7..14 are hw generated hamming/ecc and overwritten */
-	struct docg4_priv *doc = nand->priv;
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	doc->oob_page = page;
 	memcpy(doc->oob_buf, nand->oob_poi, 16);
 	return 0;
@@ -1016,8 +1016,8 @@ static int __init read_factory_bbt(struct mtd_info *mtd)
 	 * update the memory-based bbt accordingly.
 	 */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	uint32_t g4_addr = mtd_to_docg4_address(DOCG4_FACTORY_BBT_PAGE, 0);
 	uint8_t *buf;
 	int i, block;
@@ -1089,8 +1089,8 @@ static int docg4_block_markbad(struct mtd_info *mtd, loff_t ofs)
 
 	int ret, i;
 	uint8_t *buf;
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	struct nand_bbt_descr *bbtd = nand->badblock_pattern;
 	int page = (int)(ofs >> nand->page_shift);
 	uint32_t g4_addr = mtd_to_docg4_address(page, 0);
@@ -1202,8 +1202,8 @@ static void __init init_mtd_structs(struct mtd_info *mtd)
 	 * things as well, such as call nand_set_defaults().
 	 */
 
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 
 	mtd->size = DOCG4_CHIP_SIZE;
 	mtd->name = "Msys_Diskonchip_G4";
@@ -1261,8 +1261,8 @@ static void __init init_mtd_structs(struct mtd_info *mtd)
 
 static int __init read_id_reg(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct docg4_priv *doc = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct docg4_priv *doc = nand_get_controller_data(nand);
 	void __iomem *docptr = doc->virtadr;
 	uint16_t id1, id2;
 
@@ -1305,17 +1305,16 @@ static int __init probe_docg4(struct platform_device *pdev)
 		return -EIO;
 	}
 
-	len = sizeof(struct mtd_info) + sizeof(struct nand_chip) +
-		sizeof(struct docg4_priv);
-	mtd = kzalloc(len, GFP_KERNEL);
-	if (mtd == NULL) {
+	len = sizeof(struct nand_chip) + sizeof(struct docg4_priv);
+	nand = kzalloc(len, GFP_KERNEL);
+	if (nand == NULL) {
 		retval = -ENOMEM;
-		goto fail;
+		goto fail_unmap;
 	}
-	nand = (struct nand_chip *) (mtd + 1);
+
+	mtd = nand_to_mtd(nand);
 	doc = (struct docg4_priv *) (nand + 1);
-	mtd->priv = nand;
-	nand->priv = doc;
+	nand_set_controller_data(nand, doc);
 	mtd->dev.parent = &pdev->dev;
 	doc->virtadr = virtadr;
 	doc->dev = dev;
@@ -1353,16 +1352,13 @@ static int __init probe_docg4(struct platform_device *pdev)
 	doc->mtd = mtd;
 	return 0;
 
- fail:
+fail:
+	nand_release(mtd); /* deletes partitions and mtd devices */
+	free_bch(doc->bch);
+	kfree(nand);
+
+fail_unmap:
 	iounmap(virtadr);
-	if (mtd) {
-		/* re-declarations avoid compiler warning */
-		struct nand_chip *nand = mtd->priv;
-		struct docg4_priv *doc = nand->priv;
-		nand_release(mtd); /* deletes partitions and mtd devices */
-		free_bch(doc->bch);
-		kfree(mtd);
-	}
 
 	return retval;
 }
@@ -1372,7 +1368,7 @@ static int __exit cleanup_docg4(struct platform_device *pdev)
 	struct docg4_priv *doc = platform_get_drvdata(pdev);
 	nand_release(doc->mtd);
 	free_bch(doc->bch);
-	kfree(doc->mtd);
+	kfree(mtd_to_nand(doc->mtd));
 	iounmap(doc->virtadr);
 	return 0;
 }
diff --git a/drivers/mtd/nand/fsl_elbc_nand.c b/drivers/mtd/nand/fsl_elbc_nand.c
index dcb1f7f4873f..059d5f7ec124 100644
--- a/drivers/mtd/nand/fsl_elbc_nand.c
+++ b/drivers/mtd/nand/fsl_elbc_nand.c
@@ -48,7 +48,6 @@
 /* mtd information per set */
 
 struct fsl_elbc_mtd {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	struct fsl_lbc_ctrl *ctrl;
 
@@ -144,8 +143,8 @@ static struct nand_bbt_descr bbt_mirror_descr = {
  */
 static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = ctrl->nand;
@@ -195,8 +194,8 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
  */
 static int fsl_elbc_run_command(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = ctrl->nand;
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
@@ -268,7 +267,7 @@ static int fsl_elbc_run_command(struct mtd_info *mtd)
 
 static void fsl_elbc_do_read(struct nand_chip *chip, int oob)
 {
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
 
@@ -300,8 +299,8 @@ static void fsl_elbc_do_read(struct nand_chip *chip, int oob)
 static void fsl_elbc_cmdfunc(struct mtd_info *mtd, unsigned int command,
                              int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = ctrl->nand;
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
@@ -525,8 +524,8 @@ static void fsl_elbc_select_chip(struct mtd_info *mtd, int chip)
  */
 static void fsl_elbc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
 	unsigned int bufsize = mtd->writesize + mtd->oobsize;
 
@@ -563,8 +562,8 @@ static void fsl_elbc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
  */
 static u8 fsl_elbc_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
 
 	/* If there are still bytes in the FCM, then use the next byte. */
@@ -580,8 +579,8 @@ static u8 fsl_elbc_read_byte(struct mtd_info *mtd)
  */
 static void fsl_elbc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
 	int avail;
 
@@ -605,7 +604,7 @@ static void fsl_elbc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
  */
 static int fsl_elbc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
 
 	if (elbc_fcm_ctrl->status != LTESR_CC)
@@ -619,8 +618,8 @@ static int fsl_elbc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 
 static int fsl_elbc_chip_init_tail(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
 	unsigned int al;
@@ -697,7 +696,7 @@ static int fsl_elbc_chip_init_tail(struct mtd_info *mtd)
 static int fsl_elbc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			      uint8_t *buf, int oob_required, int page)
 {
-	struct fsl_elbc_mtd *priv = chip->priv;
+	struct fsl_elbc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_lbc_ctrl *ctrl = priv->ctrl;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = ctrl->nand;
 
@@ -742,12 +741,13 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	struct fsl_lbc_regs __iomem *lbc = ctrl->regs;
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = ctrl->nand;
 	struct nand_chip *chip = &priv->chip;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 
 	dev_dbg(priv->dev, "eLBC Set Information for bank %d\n", priv->bank);
 
 	/* Fill in fsl_elbc_mtd structure */
-	priv->mtd.priv = chip;
-	priv->mtd.dev.parent = priv->dev;
+	mtd->dev.parent = priv->dev;
+	nand_set_flash_node(chip, priv->dev->of_node);
 
 	/* set timeout to maximum */
 	priv->fmr = 15 << FMR_CWTO_SHIFT;
@@ -770,7 +770,7 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 	chip->bbt_options = NAND_BBT_USE_FLASH;
 
 	chip->controller = &elbc_fcm_ctrl->controller;
-	chip->priv = priv;
+	nand_set_controller_data(chip, priv);
 
 	chip->ecc.read_page = fsl_elbc_read_page;
 	chip->ecc.write_page = fsl_elbc_write_page;
@@ -797,9 +797,11 @@ static int fsl_elbc_chip_init(struct fsl_elbc_mtd *priv)
 static int fsl_elbc_chip_remove(struct fsl_elbc_mtd *priv)
 {
 	struct fsl_elbc_fcm_ctrl *elbc_fcm_ctrl = priv->ctrl->nand;
-	nand_release(&priv->mtd);
+	struct mtd_info *mtd = nand_to_mtd(&priv->chip);
 
-	kfree(priv->mtd.name);
+	nand_release(mtd);
+
+	kfree(mtd->name);
 
 	if (priv->vbase)
 		iounmap(priv->vbase);
@@ -823,9 +825,8 @@ static int fsl_elbc_nand_probe(struct platform_device *pdev)
 	int bank;
 	struct device *dev;
 	struct device_node *node = pdev->dev.of_node;
-	struct mtd_part_parser_data ppdata;
+	struct mtd_info *mtd;
 
-	ppdata.of_node = pdev->dev.of_node;
 	if (!fsl_lbc_ctrl_dev || !fsl_lbc_ctrl_dev->regs)
 		return -ENODEV;
 	lbc = fsl_lbc_ctrl_dev->regs;
@@ -887,8 +888,9 @@ static int fsl_elbc_nand_probe(struct platform_device *pdev)
 		goto err;
 	}
 
-	priv->mtd.name = kasprintf(GFP_KERNEL, "%llx.flash", (u64)res.start);
-	if (!priv->mtd.name) {
+	mtd = nand_to_mtd(&priv->chip);
+	mtd->name = kasprintf(GFP_KERNEL, "%llx.flash", (u64)res.start);
+	if (!nand_to_mtd(&priv->chip)->name) {
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -897,21 +899,21 @@ static int fsl_elbc_nand_probe(struct platform_device *pdev)
 	if (ret)
 		goto err;
 
-	ret = nand_scan_ident(&priv->mtd, 1, NULL);
+	ret = nand_scan_ident(mtd, 1, NULL);
 	if (ret)
 		goto err;
 
-	ret = fsl_elbc_chip_init_tail(&priv->mtd);
+	ret = fsl_elbc_chip_init_tail(mtd);
 	if (ret)
 		goto err;
 
-	ret = nand_scan_tail(&priv->mtd);
+	ret = nand_scan_tail(mtd);
 	if (ret)
 		goto err;
 
 	/* First look for RedBoot table or partitions on the command
 	 * line, these take precedence over device tree information */
-	mtd_device_parse_register(&priv->mtd, part_probe_types, &ppdata,
+	mtd_device_parse_register(mtd, part_probe_types, NULL,
 				  NULL, 0);
 
 	printk(KERN_INFO "eLBC NAND device at 0x%llx, bank %d\n",
diff --git a/drivers/mtd/nand/fsl_ifc_nand.c b/drivers/mtd/nand/fsl_ifc_nand.c
index 7f4ac8c19001..43f5a3a4873f 100644
--- a/drivers/mtd/nand/fsl_ifc_nand.c
+++ b/drivers/mtd/nand/fsl_ifc_nand.c
@@ -40,7 +40,6 @@ struct fsl_ifc_ctrl;
 
 /* mtd information per set */
 struct fsl_ifc_mtd {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	struct fsl_ifc_ctrl *ctrl;
 
@@ -230,8 +229,8 @@ static struct nand_bbt_descr bbt_mirror_descr = {
  */
 static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 	int buf_num;
@@ -253,8 +252,8 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr, int oob)
 
 static int is_blank(struct mtd_info *mtd, unsigned int bufnum)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	u8 __iomem *addr = priv->vbase + bufnum * (mtd->writesize * 2);
 	u32 __iomem *mainarea = (u32 __iomem *)addr;
 	u8 __iomem *oob = addr + mtd->writesize;
@@ -292,8 +291,8 @@ static int check_read_ecc(struct mtd_info *mtd, struct fsl_ifc_ctrl *ctrl,
  */
 static void fsl_ifc_run_command(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_nand_ctrl *nctrl = ifc_nand_ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
@@ -370,7 +369,7 @@ static void fsl_ifc_do_read(struct nand_chip *chip,
 			    int oob,
 			    struct mtd_info *mtd)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 
@@ -409,8 +408,8 @@ static void fsl_ifc_do_read(struct nand_chip *chip,
 /* cmdfunc send commands to the IFC NAND Machine */
 static void fsl_ifc_cmdfunc(struct mtd_info *mtd, unsigned int command,
 			     int column, int page_addr) {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 
@@ -624,8 +623,8 @@ static void fsl_ifc_select_chip(struct mtd_info *mtd, int chip)
  */
 static void fsl_ifc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	unsigned int bufsize = mtd->writesize + mtd->oobsize;
 
 	if (len <= 0) {
@@ -650,8 +649,8 @@ static void fsl_ifc_write_buf(struct mtd_info *mtd, const u8 *buf, int len)
  */
 static uint8_t fsl_ifc_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	unsigned int offset;
 
 	/*
@@ -673,8 +672,8 @@ static uint8_t fsl_ifc_read_byte(struct mtd_info *mtd)
  */
 static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	uint16_t data;
 
 	/*
@@ -696,8 +695,8 @@ static uint8_t fsl_ifc_read_byte16(struct mtd_info *mtd)
  */
 static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	int avail;
 
 	if (len < 0) {
@@ -722,7 +721,7 @@ static void fsl_ifc_read_buf(struct mtd_info *mtd, u8 *buf, int len)
  */
 static int fsl_ifc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 	u32 nand_fsr;
@@ -751,7 +750,7 @@ static int fsl_ifc_wait(struct mtd_info *mtd, struct nand_chip *chip)
 static int fsl_ifc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			     uint8_t *buf, int oob_required, int page)
 {
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_nand_ctrl *nctrl = ifc_nand_ctrl;
 
@@ -782,8 +781,8 @@ static int fsl_ifc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 static int fsl_ifc_chip_init_tail(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct fsl_ifc_mtd *priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsl_ifc_mtd *priv = nand_get_controller_data(chip);
 
 	dev_dbg(priv->dev, "%s: nand->numchips = %d\n", __func__,
 							chip->numchips);
@@ -877,12 +876,13 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 	struct fsl_ifc_ctrl *ctrl = priv->ctrl;
 	struct fsl_ifc_regs __iomem *ifc = ctrl->regs;
 	struct nand_chip *chip = &priv->chip;
+	struct mtd_info *mtd = nand_to_mtd(&priv->chip);
 	struct nand_ecclayout *layout;
 	u32 csor;
 
 	/* Fill in fsl_ifc_mtd structure */
-	priv->mtd.priv = chip;
-	priv->mtd.dev.parent = priv->dev;
+	mtd->dev.parent = priv->dev;
+	nand_set_flash_node(chip, priv->dev->of_node);
 
 	/* fill in nand_chip structure */
 	/* set up function call table */
@@ -914,7 +914,7 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 	}
 
 	chip->controller = &ifc_nand_ctrl->controller;
-	chip->priv = priv;
+	nand_set_controller_data(chip, priv);
 
 	chip->ecc.read_page = fsl_ifc_read_page;
 	chip->ecc.write_page = fsl_ifc_write_page;
@@ -993,9 +993,11 @@ static int fsl_ifc_chip_init(struct fsl_ifc_mtd *priv)
 
 static int fsl_ifc_chip_remove(struct fsl_ifc_mtd *priv)
 {
-	nand_release(&priv->mtd);
+	struct mtd_info *mtd = nand_to_mtd(&priv->chip);
 
-	kfree(priv->mtd.name);
+	nand_release(mtd);
+
+	kfree(mtd->name);
 
 	if (priv->vbase)
 		iounmap(priv->vbase);
@@ -1030,9 +1032,8 @@ static int fsl_ifc_nand_probe(struct platform_device *dev)
 	int ret;
 	int bank;
 	struct device_node *node = dev->dev.of_node;
-	struct mtd_part_parser_data ppdata;
+	struct mtd_info *mtd;
 
-	ppdata.of_node = dev->dev.of_node;
 	if (!fsl_ifc_ctrl_dev || !fsl_ifc_ctrl_dev->regs)
 		return -ENODEV;
 	ifc = fsl_ifc_ctrl_dev->regs;
@@ -1104,8 +1105,10 @@ static int fsl_ifc_nand_probe(struct platform_device *dev)
 		  IFC_NAND_EVTER_INTR_FTOERIR_EN |
 		  IFC_NAND_EVTER_INTR_WPERIR_EN,
 		  &ifc->ifc_nand.nand_evter_intr_en);
-	priv->mtd.name = kasprintf(GFP_KERNEL, "%llx.flash", (u64)res.start);
-	if (!priv->mtd.name) {
+
+	mtd = nand_to_mtd(&priv->chip);
+	mtd->name = kasprintf(GFP_KERNEL, "%llx.flash", (u64)res.start);
+	if (!mtd->name) {
 		ret = -ENOMEM;
 		goto err;
 	}
@@ -1114,22 +1117,21 @@ static int fsl_ifc_nand_probe(struct platform_device *dev)
 	if (ret)
 		goto err;
 
-	ret = nand_scan_ident(&priv->mtd, 1, NULL);
+	ret = nand_scan_ident(mtd, 1, NULL);
 	if (ret)
 		goto err;
 
-	ret = fsl_ifc_chip_init_tail(&priv->mtd);
+	ret = fsl_ifc_chip_init_tail(mtd);
 	if (ret)
 		goto err;
 
-	ret = nand_scan_tail(&priv->mtd);
+	ret = nand_scan_tail(mtd);
 	if (ret)
 		goto err;
 
 	/* First look for RedBoot table or partitions on the command
 	 * line, these take precedence over device tree information */
-	mtd_device_parse_register(&priv->mtd, part_probe_types, &ppdata,
-						NULL, 0);
+	mtd_device_parse_register(mtd, part_probe_types, NULL, NULL, 0);
 
 	dev_info(priv->dev, "IFC NAND device at 0x%llx, bank %d\n",
 		 (unsigned long long)res.start, priv->bank);
diff --git a/drivers/mtd/nand/fsl_upm.c b/drivers/mtd/nand/fsl_upm.c
index d326369980c4..cafd12de7276 100644
--- a/drivers/mtd/nand/fsl_upm.c
+++ b/drivers/mtd/nand/fsl_upm.c
@@ -31,7 +31,6 @@
 
 struct fsl_upm_nand {
 	struct device *dev;
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	int last_ctrl;
 	struct mtd_partition *parts;
@@ -49,7 +48,8 @@ struct fsl_upm_nand {
 
 static inline struct fsl_upm_nand *to_fsl_upm_nand(struct mtd_info *mtdinfo)
 {
-	return container_of(mtdinfo, struct fsl_upm_nand, mtd);
+	return container_of(mtd_to_nand(mtdinfo), struct fsl_upm_nand,
+			    chip);
 }
 
 static int fun_chip_ready(struct mtd_info *mtd)
@@ -66,9 +66,10 @@ static int fun_chip_ready(struct mtd_info *mtd)
 static void fun_wait_rnb(struct fsl_upm_nand *fun)
 {
 	if (fun->rnb_gpio[fun->mchip_number] >= 0) {
+		struct mtd_info *mtd = nand_to_mtd(&fun->chip);
 		int cnt = 1000000;
 
-		while (--cnt && !fun_chip_ready(&fun->mtd))
+		while (--cnt && !fun_chip_ready(mtd))
 			cpu_relax();
 		if (!cnt)
 			dev_err(fun->dev, "tired waiting for RNB\n");
@@ -79,7 +80,7 @@ static void fun_wait_rnb(struct fsl_upm_nand *fun)
 
 static void fun_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct fsl_upm_nand *fun = to_fsl_upm_nand(mtd);
 	u32 mar;
 
@@ -109,7 +110,7 @@ static void fun_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 
 static void fun_select_chip(struct mtd_info *mtd, int mchip_nr)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct fsl_upm_nand *fun = to_fsl_upm_nand(mtd);
 
 	if (mchip_nr == -1) {
@@ -157,9 +158,9 @@ static int fun_chip_init(struct fsl_upm_nand *fun,
 			 const struct device_node *upm_np,
 			 const struct resource *io_res)
 {
+	struct mtd_info *mtd = nand_to_mtd(&fun->chip);
 	int ret;
 	struct device_node *flash_np;
-	struct mtd_part_parser_data ppdata;
 
 	fun->chip.IO_ADDR_R = fun->io_base;
 	fun->chip.IO_ADDR_W = fun->io_base;
@@ -175,30 +176,29 @@ static int fun_chip_init(struct fsl_upm_nand *fun,
 	if (fun->rnb_gpio[0] >= 0)
 		fun->chip.dev_ready = fun_chip_ready;
 
-	fun->mtd.priv = &fun->chip;
-	fun->mtd.dev.parent = fun->dev;
+	mtd->dev.parent = fun->dev;
 
 	flash_np = of_get_next_child(upm_np, NULL);
 	if (!flash_np)
 		return -ENODEV;
 
-	fun->mtd.name = kasprintf(GFP_KERNEL, "0x%llx.%s", (u64)io_res->start,
-				  flash_np->name);
-	if (!fun->mtd.name) {
+	nand_set_flash_node(&fun->chip, flash_np);
+	mtd->name = kasprintf(GFP_KERNEL, "0x%llx.%s", (u64)io_res->start,
+			      flash_np->name);
+	if (!mtd->name) {
 		ret = -ENOMEM;
 		goto err;
 	}
 
-	ret = nand_scan(&fun->mtd, fun->mchip_count);
+	ret = nand_scan(mtd, fun->mchip_count);
 	if (ret)
 		goto err;
 
-	ppdata.of_node = flash_np;
-	ret = mtd_device_parse_register(&fun->mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 err:
 	of_node_put(flash_np);
 	if (ret)
-		kfree(fun->mtd.name);
+		kfree(mtd->name);
 	return ret;
 }
 
@@ -322,10 +322,11 @@ err1:
 static int fun_remove(struct platform_device *ofdev)
 {
 	struct fsl_upm_nand *fun = dev_get_drvdata(&ofdev->dev);
+	struct mtd_info *mtd = nand_to_mtd(&fun->chip);
 	int i;
 
-	nand_release(&fun->mtd);
-	kfree(fun->mtd.name);
+	nand_release(mtd);
+	kfree(mtd->name);
 
 	for (i = 0; i < fun->mchip_count; i++) {
 		if (fun->rnb_gpio[i] < 0)
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index 07af3dc7a4d2..1bdcd4fa26d4 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -299,7 +299,6 @@ static struct fsmc_eccplace fsmc_ecc4_sp_place = {
  */
 struct fsmc_nand_data {
 	u32			pid;
-	struct mtd_info		mtd;
 	struct nand_chip	nand;
 	struct mtd_partition	*partitions;
 	unsigned int		nr_partitions;
@@ -326,13 +325,18 @@ struct fsmc_nand_data {
 	void			(*select_chip)(uint32_t bank, uint32_t busw);
 };
 
+static inline struct fsmc_nand_data *mtd_to_fsmc(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct fsmc_nand_data, nand);
+}
+
 /* Assert CS signal based on chipnr */
 static void fsmc_select_chip(struct mtd_info *mtd, int chipnr)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct fsmc_nand_data *host;
 
-	host = container_of(mtd, struct fsmc_nand_data, mtd);
+	host = mtd_to_fsmc(mtd);
 
 	switch (chipnr) {
 	case -1:
@@ -358,9 +362,8 @@ static void fsmc_select_chip(struct mtd_info *mtd, int chipnr)
  */
 static void fsmc_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	void __iomem *regs = host->regs_va;
 	unsigned int bank = host->bank;
 
@@ -445,8 +448,7 @@ static void fsmc_nand_setup(void __iomem *regs, uint32_t bank,
  */
 static void fsmc_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	void __iomem *regs = host->regs_va;
 	uint32_t bank = host->bank;
 
@@ -466,8 +468,7 @@ static void fsmc_enable_hwecc(struct mtd_info *mtd, int mode)
 static int fsmc_read_hwecc_ecc4(struct mtd_info *mtd, const uint8_t *data,
 				uint8_t *ecc)
 {
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	void __iomem *regs = host->regs_va;
 	uint32_t bank = host->bank;
 	uint32_t ecc_tmp;
@@ -517,8 +518,7 @@ static int fsmc_read_hwecc_ecc4(struct mtd_info *mtd, const uint8_t *data,
 static int fsmc_read_hwecc_ecc1(struct mtd_info *mtd, const uint8_t *data,
 				uint8_t *ecc)
 {
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	void __iomem *regs = host->regs_va;
 	uint32_t bank = host->bank;
 	uint32_t ecc_tmp;
@@ -629,7 +629,7 @@ unmap_dma:
 static void fsmc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (IS_ALIGNED((uint32_t)buf, sizeof(uint32_t)) &&
 			IS_ALIGNED(len, sizeof(uint32_t))) {
@@ -652,7 +652,7 @@ static void fsmc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 static void fsmc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (IS_ALIGNED((uint32_t)buf, sizeof(uint32_t)) &&
 			IS_ALIGNED(len, sizeof(uint32_t))) {
@@ -674,9 +674,8 @@ static void fsmc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
  */
 static void fsmc_read_buf_dma(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct fsmc_nand_data *host;
+	struct fsmc_nand_data *host  = mtd_to_fsmc(mtd);
 
-	host = container_of(mtd, struct fsmc_nand_data, mtd);
 	dma_xfer(host, buf, len, DMA_FROM_DEVICE);
 }
 
@@ -689,9 +688,8 @@ static void fsmc_read_buf_dma(struct mtd_info *mtd, uint8_t *buf, int len)
 static void fsmc_write_buf_dma(struct mtd_info *mtd, const uint8_t *buf,
 		int len)
 {
-	struct fsmc_nand_data *host;
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 
-	host = container_of(mtd, struct fsmc_nand_data, mtd);
 	dma_xfer(host, (void *)buf, len, DMA_TO_DEVICE);
 }
 
@@ -712,8 +710,7 @@ static void fsmc_write_buf_dma(struct mtd_info *mtd, const uint8_t *buf,
 static int fsmc_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 				 uint8_t *buf, int oob_required, int page)
 {
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	struct fsmc_eccplace *ecc_place = host->ecc_place;
 	int i, j, s, stat, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
@@ -782,9 +779,8 @@ static int fsmc_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 static int fsmc_bch8_correct_data(struct mtd_info *mtd, uint8_t *dat,
 			     uint8_t *read_ecc, uint8_t *calc_ecc)
 {
-	struct fsmc_nand_data *host = container_of(mtd,
-					struct fsmc_nand_data, mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct fsmc_nand_data *host = mtd_to_fsmc(mtd);
 	void __iomem *regs = host->regs_va;
 	unsigned int bank = host->bank;
 	uint32_t err_idx[8];
@@ -926,7 +922,6 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 {
 	struct fsmc_nand_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct device_node __maybe_unused *np = pdev->dev.of_node;
-	struct mtd_part_parser_data ppdata = {};
 	struct fsmc_nand_data *host;
 	struct mtd_info *mtd;
 	struct nand_chip *nand;
@@ -1012,12 +1007,12 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 		init_completion(&host->dma_access_complete);
 
 	/* Link all private pointers */
-	mtd = &host->mtd;
+	mtd = nand_to_mtd(&host->nand);
 	nand = &host->nand;
-	mtd->priv = nand;
-	nand->priv = host;
+	nand_set_controller_data(nand, host);
+	nand_set_flash_node(nand, np);
 
-	host->mtd.dev.parent = &pdev->dev;
+	mtd->dev.parent = &pdev->dev;
 	nand->IO_ADDR_R = host->data_va;
 	nand->IO_ADDR_W = host->data_va;
 	nand->cmd_ctrl = fsmc_cmd_ctrl;
@@ -1033,7 +1028,7 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 	nand->options = pdata->options;
 	nand->select_chip = fsmc_select_chip;
 	nand->badblockbits = 7;
-	nand->flash_node = np;
+	nand_set_flash_node(nand, np);
 
 	if (pdata->width == FSMC_NAND_BW16)
 		nand->options |= NAND_BUSWIDTH_16;
@@ -1080,14 +1075,14 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 	/*
 	 * Scan to find existence of the device
 	 */
-	if (nand_scan_ident(&host->mtd, 1, NULL)) {
+	if (nand_scan_ident(mtd, 1, NULL)) {
 		ret = -ENXIO;
 		dev_err(&pdev->dev, "No NAND Device found!\n");
 		goto err_scan_ident;
 	}
 
 	if (AMBA_REV_BITS(host->pid) >= 8) {
-		switch (host->mtd.oobsize) {
+		switch (mtd->oobsize) {
 		case 16:
 			nand->ecc.layout = &fsmc_ecc4_16_layout;
 			host->ecc_place = &fsmc_ecc4_sp_place;
@@ -1138,7 +1133,7 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 		 * generated later in nand_bch_init() later.
 		 */
 		if (nand->ecc.mode != NAND_ECC_SOFT_BCH) {
-			switch (host->mtd.oobsize) {
+			switch (mtd->oobsize) {
 			case 16:
 				nand->ecc.layout = &fsmc_ecc1_16_layout;
 				break;
@@ -1159,7 +1154,7 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 	}
 
 	/* Second stage of scan to fill MTD data-structures */
-	if (nand_scan_tail(&host->mtd)) {
+	if (nand_scan_tail(mtd)) {
 		ret = -ENXIO;
 		goto err_probe;
 	}
@@ -1174,10 +1169,8 @@ static int __init fsmc_nand_probe(struct platform_device *pdev)
 	/*
 	 * Check for partition info passed
 	 */
-	host->mtd.name = "nand";
-	ppdata.of_node = np;
-	ret = mtd_device_parse_register(&host->mtd, NULL, &ppdata,
-					host->partitions, host->nr_partitions);
+	mtd->name = "nand";
+	ret = mtd_device_register(mtd, host->partitions, host->nr_partitions);
 	if (ret)
 		goto err_probe;
 
@@ -1207,7 +1200,7 @@ static int fsmc_nand_remove(struct platform_device *pdev)
 	struct fsmc_nand_data *host = platform_get_drvdata(pdev);
 
 	if (host) {
-		nand_release(&host->mtd);
+		nand_release(nand_to_mtd(&host->nand));
 
 		if (host->mode == USE_DMA_ACCESS) {
 			dma_release_channel(host->write_dma_chan);
diff --git a/drivers/mtd/nand/gpio.c b/drivers/mtd/nand/gpio.c
index 9ab97f934c37..ded658fc7d73 100644
--- a/drivers/mtd/nand/gpio.c
+++ b/drivers/mtd/nand/gpio.c
@@ -35,12 +35,14 @@
 
 struct gpiomtd {
 	void __iomem		*io_sync;
-	struct mtd_info		mtd_info;
 	struct nand_chip	nand_chip;
 	struct gpio_nand_platdata plat;
 };
 
-#define gpio_nand_getpriv(x) container_of(x, struct gpiomtd, mtd_info)
+static inline struct gpiomtd *gpio_nand_getpriv(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct gpiomtd, nand_chip);
+}
 
 
 #ifdef CONFIG_ARM
@@ -195,7 +197,7 @@ static int gpio_nand_remove(struct platform_device *pdev)
 {
 	struct gpiomtd *gpiomtd = platform_get_drvdata(pdev);
 
-	nand_release(&gpiomtd->mtd_info);
+	nand_release(nand_to_mtd(&gpiomtd->nand_chip));
 
 	if (gpio_is_valid(gpiomtd->plat.gpio_nwp))
 		gpio_set_value(gpiomtd->plat.gpio_nwp, 0);
@@ -208,8 +210,8 @@ static int gpio_nand_probe(struct platform_device *pdev)
 {
 	struct gpiomtd *gpiomtd;
 	struct nand_chip *chip;
+	struct mtd_info *mtd;
 	struct resource *res;
-	struct mtd_part_parser_data ppdata = {};
 	int ret = 0;
 
 	if (!pdev->dev.of_node && !dev_get_platdata(&pdev->dev))
@@ -268,33 +270,31 @@ static int gpio_nand_probe(struct platform_device *pdev)
 		chip->dev_ready = gpio_nand_devready;
 	}
 
+	nand_set_flash_node(chip, pdev->dev.of_node);
 	chip->IO_ADDR_W		= chip->IO_ADDR_R;
 	chip->ecc.mode		= NAND_ECC_SOFT;
 	chip->options		= gpiomtd->plat.options;
 	chip->chip_delay	= gpiomtd->plat.chip_delay;
 	chip->cmd_ctrl		= gpio_nand_cmd_ctrl;
 
-	gpiomtd->mtd_info.priv	= chip;
-	gpiomtd->mtd_info.dev.parent = &pdev->dev;
+	mtd			= nand_to_mtd(chip);
+	mtd->dev.parent		= &pdev->dev;
 
 	platform_set_drvdata(pdev, gpiomtd);
 
 	if (gpio_is_valid(gpiomtd->plat.gpio_nwp))
 		gpio_direction_output(gpiomtd->plat.gpio_nwp, 1);
 
-	if (nand_scan(&gpiomtd->mtd_info, 1)) {
+	if (nand_scan(mtd, 1)) {
 		ret = -ENXIO;
 		goto err_wp;
 	}
 
 	if (gpiomtd->plat.adjust_parts)
-		gpiomtd->plat.adjust_parts(&gpiomtd->plat,
-					   gpiomtd->mtd_info.size);
+		gpiomtd->plat.adjust_parts(&gpiomtd->plat, mtd->size);
 
-	ppdata.of_node = pdev->dev.of_node;
-	ret = mtd_device_parse_register(&gpiomtd->mtd_info, NULL, &ppdata,
-					gpiomtd->plat.parts,
-					gpiomtd->plat.num_parts);
+	ret = mtd_device_register(mtd, gpiomtd->plat.parts,
+				  gpiomtd->plat.num_parts);
 	if (!ret)
 		return 0;
 
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
index 43fa16b5f510..0f68a99fc4ad 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-lib.c
@@ -919,7 +919,7 @@ static int enable_edo_mode(struct gpmi_nand_data *this, int mode)
 {
 	struct resources  *r = &this->resources;
 	struct nand_chip *nand = &this->nand;
-	struct mtd_info	 *mtd = &this->mtd;
+	struct mtd_info	 *mtd = nand_to_mtd(nand);
 	uint8_t *feature;
 	unsigned long rate;
 	int ret;
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
index 2064adac1d17..235ddcb58f39 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.c
@@ -107,7 +107,7 @@ static irqreturn_t bch_irq(int irq, void *cookie)
 static inline int get_ecc_strength(struct gpmi_nand_data *this)
 {
 	struct bch_geometry *geo = &this->bch_geometry;
-	struct mtd_info	*mtd = &this->mtd;
+	struct mtd_info	*mtd = nand_to_mtd(&this->nand);
 	int ecc_strength;
 
 	ecc_strength = ((mtd->oobsize - geo->metadata_size) * 8)
@@ -139,8 +139,8 @@ static inline bool gpmi_check_ecc(struct gpmi_nand_data *this)
 static bool set_geometry_by_ecc_info(struct gpmi_nand_data *this)
 {
 	struct bch_geometry *geo = &this->bch_geometry;
-	struct mtd_info *mtd = &this->mtd;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = &this->nand;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	struct nand_oobfree *of = gpmi_hw_ecclayout.oobfree;
 	unsigned int block_mark_bit_offset;
 
@@ -257,7 +257,7 @@ static bool set_geometry_by_ecc_info(struct gpmi_nand_data *this)
 static int legacy_set_geometry(struct gpmi_nand_data *this)
 {
 	struct bch_geometry *geo = &this->bch_geometry;
-	struct mtd_info *mtd = &this->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&this->nand);
 	unsigned int metadata_size;
 	unsigned int status_size;
 	unsigned int block_mark_bit_offset;
@@ -804,7 +804,7 @@ static int gpmi_alloc_dma_buffer(struct gpmi_nand_data *this)
 {
 	struct bch_geometry *geo = &this->bch_geometry;
 	struct device *dev = this->dev;
-	struct mtd_info *mtd = &this->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&this->nand);
 
 	/* [1] Allocate a command buffer. PAGE_SIZE is enough. */
 	this->cmd_buffer = kzalloc(PAGE_SIZE, GFP_DMA | GFP_KERNEL);
@@ -856,8 +856,8 @@ error_alloc:
 
 static void gpmi_cmd_ctrl(struct mtd_info *mtd, int data, unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	int ret;
 
 	/*
@@ -890,16 +890,16 @@ static void gpmi_cmd_ctrl(struct mtd_info *mtd, int data, unsigned int ctrl)
 
 static int gpmi_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 
 	return gpmi_is_ready(this, this->current_chip);
 }
 
 static void gpmi_select_chip(struct mtd_info *mtd, int chipnr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 
 	if ((this->current_chip < 0) && (chipnr >= 0))
 		gpmi_begin(this);
@@ -911,8 +911,8 @@ static void gpmi_select_chip(struct mtd_info *mtd, int chipnr)
 
 static void gpmi_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 
 	dev_dbg(this->dev, "len is %d\n", len);
 	this->upper_buf	= buf;
@@ -923,8 +923,8 @@ static void gpmi_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 
 static void gpmi_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 
 	dev_dbg(this->dev, "len is %d\n", len);
 	this->upper_buf	= (uint8_t *)buf;
@@ -935,8 +935,8 @@ static void gpmi_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 
 static uint8_t gpmi_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	uint8_t *buf = this->data_buffer_dma;
 
 	gpmi_read_buf(mtd, buf, 1);
@@ -994,7 +994,7 @@ static void block_mark_swapping(struct gpmi_nand_data *this,
 static int gpmi_ecc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 				uint8_t *buf, int oob_required, int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
 	void          *payload_virt;
 	dma_addr_t    payload_phys;
@@ -1074,7 +1074,7 @@ static int gpmi_ecc_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 static int gpmi_ecc_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 			uint32_t offs, uint32_t len, uint8_t *buf, int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	void __iomem *bch_regs = this->resources.bch_regs;
 	struct bch_geometry old_geo = this->bch_geometry;
 	struct bch_geometry *geo = &this->bch_geometry;
@@ -1162,7 +1162,7 @@ static int gpmi_ecc_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 static int gpmi_ecc_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required, int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
 	const void *payload_virt;
 	dma_addr_t payload_phys;
@@ -1298,7 +1298,7 @@ exit_auxiliary:
 static int gpmi_ecc_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
 				int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 
 	dev_dbg(this->dev, "page number is %d\n", page);
 	/* clear the OOB buffer */
@@ -1359,7 +1359,7 @@ static int gpmi_ecc_read_page_raw(struct mtd_info *mtd,
 				  struct nand_chip *chip, uint8_t *buf,
 				  int oob_required, int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
 	int eccsize = nfc_geo->ecc_chunk_size;
 	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
@@ -1448,7 +1448,7 @@ static int gpmi_ecc_write_page_raw(struct mtd_info *mtd,
 				   const uint8_t *buf,
 				   int oob_required, int page)
 {
-	struct gpmi_nand_data *this = chip->priv;
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	struct bch_geometry *nfc_geo = &this->bch_geometry;
 	int eccsize = nfc_geo->ecc_chunk_size;
 	int eccbits = nfc_geo->ecc_strength * nfc_geo->gf_len;
@@ -1538,8 +1538,8 @@ static int gpmi_ecc_write_oob_raw(struct mtd_info *mtd, struct nand_chip *chip,
 
 static int gpmi_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct gpmi_nand_data *this = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct gpmi_nand_data *this = nand_get_controller_data(chip);
 	int ret = 0;
 	uint8_t *block_mark;
 	int column, page, status, chipnr;
@@ -1600,8 +1600,8 @@ static int mx23_check_transcription_stamp(struct gpmi_nand_data *this)
 {
 	struct boot_rom_geometry *rom_geo = &this->rom_geometry;
 	struct device *dev = this->dev;
-	struct mtd_info *mtd = &this->mtd;
 	struct nand_chip *chip = &this->nand;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	unsigned int search_area_size_in_strides;
 	unsigned int stride;
 	unsigned int page;
@@ -1655,8 +1655,8 @@ static int mx23_write_transcription_stamp(struct gpmi_nand_data *this)
 {
 	struct device *dev = this->dev;
 	struct boot_rom_geometry *rom_geo = &this->rom_geometry;
-	struct mtd_info *mtd = &this->mtd;
 	struct nand_chip *chip = &this->nand;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	unsigned int block_size_in_pages;
 	unsigned int search_area_size_in_strides;
 	unsigned int search_area_size_in_pages;
@@ -1735,7 +1735,7 @@ static int mx23_boot_init(struct gpmi_nand_data  *this)
 {
 	struct device *dev = this->dev;
 	struct nand_chip *chip = &this->nand;
-	struct mtd_info *mtd = &this->mtd;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	unsigned int block_count;
 	unsigned int block;
 	int     chipnr;
@@ -1831,14 +1831,13 @@ static int gpmi_set_geometry(struct gpmi_nand_data *this)
 
 static void gpmi_nand_exit(struct gpmi_nand_data *this)
 {
-	nand_release(&this->mtd);
+	nand_release(nand_to_mtd(&this->nand));
 	gpmi_free_dma_buffer(this);
 }
 
 static int gpmi_init_last(struct gpmi_nand_data *this)
 {
-	struct mtd_info *mtd = &this->mtd;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = &this->nand;
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	struct bch_geometry *bch_geo = &this->bch_geometry;
 	int ret;
@@ -1886,21 +1885,20 @@ static int gpmi_init_last(struct gpmi_nand_data *this)
 
 static int gpmi_nand_init(struct gpmi_nand_data *this)
 {
-	struct mtd_info  *mtd = &this->mtd;
 	struct nand_chip *chip = &this->nand;
-	struct mtd_part_parser_data ppdata = {};
+	struct mtd_info  *mtd = nand_to_mtd(chip);
 	int ret;
 
 	/* init current chip */
 	this->current_chip	= -1;
 
 	/* init the MTD data structures */
-	mtd->priv		= chip;
 	mtd->name		= "gpmi-nand";
 	mtd->dev.parent		= this->dev;
 
 	/* init the nand_chip{}, we don't support a 16-bit NAND Flash bus. */
-	chip->priv		= this;
+	nand_set_controller_data(chip, this);
+	nand_set_flash_node(chip, this->pdev->dev.of_node);
 	chip->select_chip	= gpmi_select_chip;
 	chip->cmd_ctrl		= gpmi_cmd_ctrl;
 	chip->dev_ready		= gpmi_dev_ready;
@@ -1954,8 +1952,7 @@ static int gpmi_nand_init(struct gpmi_nand_data *this)
 	if (ret)
 		goto err_out;
 
-	ppdata.of_node = this->pdev->dev.of_node;
-	ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 	if (ret)
 		goto err_out;
 	return 0;
diff --git a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
index 544062f65020..4e49a1f5fa27 100644
--- a/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
+++ b/drivers/mtd/nand/gpmi-nand/gpmi-nand.h
@@ -160,7 +160,6 @@ struct gpmi_nand_data {
 
 	/* MTD / NAND */
 	struct nand_chip	nand;
-	struct mtd_info		mtd;
 
 	/* General-use Variables */
 	int			current_chip;
diff --git a/drivers/mtd/nand/hisi504_nand.c b/drivers/mtd/nand/hisi504_nand.c
index 0cb2e886937d..f8d37f36a81c 100644
--- a/drivers/mtd/nand/hisi504_nand.c
+++ b/drivers/mtd/nand/hisi504_nand.c
@@ -134,7 +134,6 @@
 
 struct hinfc_host {
 	struct nand_chip	chip;
-	struct mtd_info		mtd;
 	struct device		*dev;
 	void __iomem		*iobase;
 	void __iomem		*mmio;
@@ -189,8 +188,8 @@ static void wait_controller_finished(struct hinfc_host *host)
 
 static void hisi_nfc_dma_transfer(struct hinfc_host *host, int todev)
 {
-	struct mtd_info	*mtd = &host->mtd;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = &host->chip;
+	struct mtd_info	*mtd = nand_to_mtd(chip);
 	unsigned long val;
 	int ret;
 
@@ -262,7 +261,7 @@ static int hisi_nfc_send_cmd_pageprog(struct hinfc_host *host)
 
 static int hisi_nfc_send_cmd_readstart(struct hinfc_host *host)
 {
-	struct mtd_info	*mtd = &host->mtd;
+	struct mtd_info	*mtd = nand_to_mtd(&host->chip);
 
 	if ((host->addr_value[0] == host->cache_addr_value[0]) &&
 	    (host->addr_value[1] == host->cache_addr_value[1]))
@@ -357,8 +356,8 @@ static int hisi_nfc_send_cmd_reset(struct hinfc_host *host, int chipselect)
 
 static void hisi_nfc_select_chip(struct mtd_info *mtd, int chipselect)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	if (chipselect < 0)
 		return;
@@ -368,8 +367,8 @@ static void hisi_nfc_select_chip(struct mtd_info *mtd, int chipselect)
 
 static uint8_t hisi_nfc_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	if (host->command == NAND_CMD_STATUS)
 		return *(uint8_t *)(host->mmio);
@@ -384,8 +383,8 @@ static uint8_t hisi_nfc_read_byte(struct mtd_info *mtd)
 
 static u16 hisi_nfc_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	host->offset += 2;
 	return *(u16 *)(host->buffer + host->offset - 2);
@@ -394,8 +393,8 @@ static u16 hisi_nfc_read_word(struct mtd_info *mtd)
 static void
 hisi_nfc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	memcpy(host->buffer + host->offset, buf, len);
 	host->offset += len;
@@ -403,8 +402,8 @@ hisi_nfc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 
 static void hisi_nfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	memcpy(buf, host->buffer + host->offset, len);
 	host->offset += len;
@@ -412,8 +411,8 @@ static void hisi_nfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 
 static void set_addr(struct mtd_info *mtd, int column, int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 	unsigned int command = host->command;
 
 	host->addr_cycle    = 0;
@@ -448,8 +447,8 @@ static void set_addr(struct mtd_info *mtd, int column, int page_addr)
 static void hisi_nfc_cmdfunc(struct mtd_info *mtd, unsigned command, int column,
 		int page_addr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct hinfc_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct hinfc_host *host = nand_get_controller_data(chip);
 	int is_cache_invalid = 1;
 	unsigned int flag = 0;
 
@@ -543,7 +542,7 @@ static irqreturn_t hinfc_irq_handle(int irq, void *devid)
 static int hisi_nand_read_page_hwecc(struct mtd_info *mtd,
 	struct nand_chip *chip, uint8_t *buf, int oob_required, int page)
 {
-	struct hinfc_host *host = chip->priv;
+	struct hinfc_host *host = nand_get_controller_data(chip);
 	int max_bitflips = 0, stat = 0, stat_max = 0, status_ecc;
 	int stat_1, stat_2;
 
@@ -575,7 +574,7 @@ static int hisi_nand_read_page_hwecc(struct mtd_info *mtd,
 static int hisi_nand_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
 				int page)
 {
-	struct hinfc_host *host = chip->priv;
+	struct hinfc_host *host = nand_get_controller_data(chip);
 
 	chip->cmdfunc(mtd, NAND_CMD_READOOB, 0, page);
 	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
@@ -643,7 +642,7 @@ static int hisi_nfc_ecc_probe(struct hinfc_host *host)
 	int size, strength, ecc_bits;
 	struct device *dev = host->dev;
 	struct nand_chip *chip = &host->chip;
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	struct device_node *np = host->dev->of_node;
 
 	size = of_get_nand_ecc_step_size(np);
@@ -704,7 +703,6 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 	struct mtd_info   *mtd;
 	struct resource	  *res;
 	struct device_node *np = dev->of_node;
-	struct mtd_part_parser_data ppdata;
 
 	host = devm_kzalloc(dev, sizeof(*host), GFP_KERNEL);
 	if (!host)
@@ -713,7 +711,7 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, host);
 	chip = &host->chip;
-	mtd  = &host->mtd;
+	mtd  = nand_to_mtd(chip);
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -737,11 +735,11 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 		goto err_res;
 	}
 
-	mtd->priv		= chip;
 	mtd->name		= "hisi_nand";
 	mtd->dev.parent         = &pdev->dev;
 
-	chip->priv		= host;
+	nand_set_controller_data(chip, host);
+	nand_set_flash_node(chip, np);
 	chip->cmdfunc		= hisi_nfc_cmdfunc;
 	chip->select_chip	= hisi_nfc_select_chip;
 	chip->read_byte		= hisi_nfc_read_byte;
@@ -805,8 +803,7 @@ static int hisi_nfc_probe(struct platform_device *pdev)
 		goto err_res;
 	}
 
-	ppdata.of_node = np;
-	ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 	if (ret) {
 		dev_err(dev, "Err MTD partition=%d\n", ret);
 		goto err_mtd;
@@ -823,7 +820,7 @@ err_res:
 static int hisi_nfc_remove(struct platform_device *pdev)
 {
 	struct hinfc_host *host = platform_get_drvdata(pdev);
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->chip);
 
 	nand_release(mtd);
 
diff --git a/drivers/mtd/nand/jz4740_nand.c b/drivers/mtd/nand/jz4740_nand.c
index 5a99a93ed025..b19d2a9a5eb9 100644
--- a/drivers/mtd/nand/jz4740_nand.c
+++ b/drivers/mtd/nand/jz4740_nand.c
@@ -59,7 +59,6 @@
 #define JZ_NAND_MEM_ADDR_OFFSET 0x10000
 
 struct jz_nand {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	void __iomem *base;
 	struct resource *mem;
@@ -76,13 +75,13 @@ struct jz_nand {
 
 static inline struct jz_nand *mtd_to_jz_nand(struct mtd_info *mtd)
 {
-	return container_of(mtd, struct jz_nand, mtd);
+	return container_of(mtd_to_nand(mtd), struct jz_nand, chip);
 }
 
 static void jz_nand_select_chip(struct mtd_info *mtd, int chipnr)
 {
 	struct jz_nand *nand = mtd_to_jz_nand(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint32_t ctrl;
 	int banknr;
 
@@ -104,7 +103,7 @@ static void jz_nand_select_chip(struct mtd_info *mtd, int chipnr)
 static void jz_nand_cmd_ctrl(struct mtd_info *mtd, int dat, unsigned int ctrl)
 {
 	struct jz_nand *nand = mtd_to_jz_nand(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint32_t reg;
 	void __iomem *bank_base = nand->bank_base[nand->selected_bank];
 
@@ -225,24 +224,6 @@ static int jz_nand_correct_ecc_rs(struct mtd_info *mtd, uint8_t *dat,
 	uint32_t t;
 	unsigned int timeout = 1000;
 
-	t = read_ecc[0];
-
-	if (t == 0xff) {
-		for (i = 1; i < 9; ++i)
-			t &= read_ecc[i];
-
-		t &= dat[0];
-		t &= dat[nand->chip.ecc.size / 2];
-		t &= dat[nand->chip.ecc.size - 1];
-
-		if (t == 0xff) {
-			for (i = 1; i < nand->chip.ecc.size - 1; ++i)
-				t &= dat[i];
-			if (t == 0xff)
-				return 0;
-		}
-	}
-
 	for (i = 0; i < 9; ++i)
 		writeb(read_ecc[i], nand->base + JZ_REG_NAND_PAR0 + i);
 
@@ -255,7 +236,7 @@ static int jz_nand_correct_ecc_rs(struct mtd_info *mtd, uint8_t *dat,
 	} while (!(status & JZ_NAND_STATUS_DEC_FINISH) && --timeout);
 
 	if (timeout == 0)
-	    return -1;
+		return -ETIMEDOUT;
 
 	reg = readl(nand->base + JZ_REG_NAND_ECC_CTRL);
 	reg &= ~JZ_NAND_ECC_CTRL_ENABLE;
@@ -263,7 +244,7 @@ static int jz_nand_correct_ecc_rs(struct mtd_info *mtd, uint8_t *dat,
 
 	if (status & JZ_NAND_STATUS_ERROR) {
 		if (status & JZ_NAND_STATUS_UNCOR_ERROR)
-			return -1;
+			return -EBADMSG;
 
 		error_count = (status & JZ_NAND_STATUS_ERR_COUNT) >> 29;
 
@@ -334,8 +315,8 @@ static int jz_nand_detect_bank(struct platform_device *pdev,
 	char gpio_name[9];
 	char res_name[6];
 	uint32_t ctrl;
-	struct mtd_info *mtd = &nand->mtd;
 	struct nand_chip *chip = &nand->chip;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 
 	/* Request GPIO port. */
 	gpio = JZ_GPIO_MEM_CS0 + bank - 1;
@@ -432,9 +413,8 @@ static int jz_nand_probe(struct platform_device *pdev)
 		goto err_iounmap_mmio;
 	}
 
-	mtd		= &nand->mtd;
 	chip		= &nand->chip;
-	mtd->priv	= chip;
+	mtd		= nand_to_mtd(chip);
 	mtd->dev.parent = &pdev->dev;
 	mtd->name	= "jz4740-nand";
 
@@ -445,6 +425,7 @@ static int jz_nand_probe(struct platform_device *pdev)
 	chip->ecc.size		= 512;
 	chip->ecc.bytes		= 9;
 	chip->ecc.strength	= 4;
+	chip->ecc.options	= NAND_ECC_GENERIC_ERASED_CHECK;
 
 	if (pdata)
 		chip->ecc.layout = pdata->ecc_layout;
@@ -543,7 +524,7 @@ static int jz_nand_remove(struct platform_device *pdev)
 	struct jz_nand *nand = platform_get_drvdata(pdev);
 	size_t i;
 
-	nand_release(&nand->mtd);
+	nand_release(nand_to_mtd(&nand->chip));
 
 	/* Deassert and disable all chips */
 	writel(0, nand->base + JZ_REG_NAND_CTRL);
diff --git a/drivers/mtd/nand/jz4780_bch.c b/drivers/mtd/nand/jz4780_bch.c
new file mode 100644
index 000000000000..755499c6650e
--- /dev/null
+++ b/drivers/mtd/nand/jz4780_bch.c
@@ -0,0 +1,381 @@
+/*
+ * JZ4780 BCH controller
+ *
+ * Copyright (c) 2015 Imagination Technologies
+ * Author: Alex Smith <alex.smith@imgtec.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/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/iopoll.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include "jz4780_bch.h"
+
+#define BCH_BHCR			0x0
+#define BCH_BHCCR			0x8
+#define BCH_BHCNT			0xc
+#define BCH_BHDR			0x10
+#define BCH_BHPAR0			0x14
+#define BCH_BHERR0			0x84
+#define BCH_BHINT			0x184
+#define BCH_BHINTES			0x188
+#define BCH_BHINTEC			0x18c
+#define BCH_BHINTE			0x190
+
+#define BCH_BHCR_BSEL_SHIFT		4
+#define BCH_BHCR_BSEL_MASK		(0x7f << BCH_BHCR_BSEL_SHIFT)
+#define BCH_BHCR_ENCE			BIT(2)
+#define BCH_BHCR_INIT			BIT(1)
+#define BCH_BHCR_BCHE			BIT(0)
+
+#define BCH_BHCNT_PARITYSIZE_SHIFT	16
+#define BCH_BHCNT_PARITYSIZE_MASK	(0x7f << BCH_BHCNT_PARITYSIZE_SHIFT)
+#define BCH_BHCNT_BLOCKSIZE_SHIFT	0
+#define BCH_BHCNT_BLOCKSIZE_MASK	(0x7ff << BCH_BHCNT_BLOCKSIZE_SHIFT)
+
+#define BCH_BHERR_MASK_SHIFT		16
+#define BCH_BHERR_MASK_MASK		(0xffff << BCH_BHERR_MASK_SHIFT)
+#define BCH_BHERR_INDEX_SHIFT		0
+#define BCH_BHERR_INDEX_MASK		(0x7ff << BCH_BHERR_INDEX_SHIFT)
+
+#define BCH_BHINT_ERRC_SHIFT		24
+#define BCH_BHINT_ERRC_MASK		(0x7f << BCH_BHINT_ERRC_SHIFT)
+#define BCH_BHINT_TERRC_SHIFT		16
+#define BCH_BHINT_TERRC_MASK		(0x7f << BCH_BHINT_TERRC_SHIFT)
+#define BCH_BHINT_DECF			BIT(3)
+#define BCH_BHINT_ENCF			BIT(2)
+#define BCH_BHINT_UNCOR			BIT(1)
+#define BCH_BHINT_ERR			BIT(0)
+
+#define BCH_CLK_RATE			(200 * 1000 * 1000)
+
+/* Timeout for BCH calculation/correction. */
+#define BCH_TIMEOUT_US			100000
+
+struct jz4780_bch {
+	struct device *dev;
+	void __iomem *base;
+	struct clk *clk;
+	struct mutex lock;
+};
+
+static void jz4780_bch_init(struct jz4780_bch *bch,
+			    struct jz4780_bch_params *params, bool encode)
+{
+	u32 reg;
+
+	/* Clear interrupt status. */
+	writel(readl(bch->base + BCH_BHINT), bch->base + BCH_BHINT);
+
+	/* Set up BCH count register. */
+	reg = params->size << BCH_BHCNT_BLOCKSIZE_SHIFT;
+	reg |= params->bytes << BCH_BHCNT_PARITYSIZE_SHIFT;
+	writel(reg, bch->base + BCH_BHCNT);
+
+	/* Initialise and enable BCH. */
+	reg = BCH_BHCR_BCHE | BCH_BHCR_INIT;
+	reg |= params->strength << BCH_BHCR_BSEL_SHIFT;
+	if (encode)
+		reg |= BCH_BHCR_ENCE;
+	writel(reg, bch->base + BCH_BHCR);
+}
+
+static void jz4780_bch_disable(struct jz4780_bch *bch)
+{
+	writel(readl(bch->base + BCH_BHINT), bch->base + BCH_BHINT);
+	writel(BCH_BHCR_BCHE, bch->base + BCH_BHCCR);
+}
+
+static void jz4780_bch_write_data(struct jz4780_bch *bch, const void *buf,
+				  size_t size)
+{
+	size_t size32 = size / sizeof(u32);
+	size_t size8 = size % sizeof(u32);
+	const u32 *src32;
+	const u8 *src8;
+
+	src32 = (const u32 *)buf;
+	while (size32--)
+		writel(*src32++, bch->base + BCH_BHDR);
+
+	src8 = (const u8 *)src32;
+	while (size8--)
+		writeb(*src8++, bch->base + BCH_BHDR);
+}
+
+static void jz4780_bch_read_parity(struct jz4780_bch *bch, void *buf,
+				   size_t size)
+{
+	size_t size32 = size / sizeof(u32);
+	size_t size8 = size % sizeof(u32);
+	u32 *dest32;
+	u8 *dest8;
+	u32 val, offset = 0;
+
+	dest32 = (u32 *)buf;
+	while (size32--) {
+		*dest32++ = readl(bch->base + BCH_BHPAR0 + offset);
+		offset += sizeof(u32);
+	}
+
+	dest8 = (u8 *)dest32;
+	val = readl(bch->base + BCH_BHPAR0 + offset);
+	switch (size8) {
+	case 3:
+		dest8[2] = (val >> 16) & 0xff;
+	case 2:
+		dest8[1] = (val >> 8) & 0xff;
+	case 1:
+		dest8[0] = val & 0xff;
+		break;
+	}
+}
+
+static bool jz4780_bch_wait_complete(struct jz4780_bch *bch, unsigned int irq,
+				     u32 *status)
+{
+	u32 reg;
+	int ret;
+
+	/*
+	 * While we could use interrupts here and sleep until the operation
+	 * completes, the controller works fairly quickly (usually a few
+	 * microseconds) and so the overhead of sleeping until we get an
+	 * interrupt quite noticeably decreases performance.
+	 */
+	ret = readl_poll_timeout(bch->base + BCH_BHINT, reg,
+				 (reg & irq) == irq, 0, BCH_TIMEOUT_US);
+	if (ret)
+		return false;
+
+	if (status)
+		*status = reg;
+
+	writel(reg, bch->base + BCH_BHINT);
+	return true;
+}
+
+/**
+ * jz4780_bch_calculate() - calculate ECC for a data buffer
+ * @bch: BCH device.
+ * @params: BCH parameters.
+ * @buf: input buffer with raw data.
+ * @ecc_code: output buffer with ECC.
+ *
+ * Return: 0 on success, -ETIMEDOUT if timed out while waiting for BCH
+ * controller.
+ */
+int jz4780_bch_calculate(struct jz4780_bch *bch, struct jz4780_bch_params *params,
+			 const u8 *buf, u8 *ecc_code)
+{
+	int ret = 0;
+
+	mutex_lock(&bch->lock);
+	jz4780_bch_init(bch, params, true);
+	jz4780_bch_write_data(bch, buf, params->size);
+
+	if (jz4780_bch_wait_complete(bch, BCH_BHINT_ENCF, NULL)) {
+		jz4780_bch_read_parity(bch, ecc_code, params->bytes);
+	} else {
+		dev_err(bch->dev, "timed out while calculating ECC\n");
+		ret = -ETIMEDOUT;
+	}
+
+	jz4780_bch_disable(bch);
+	mutex_unlock(&bch->lock);
+	return ret;
+}
+EXPORT_SYMBOL(jz4780_bch_calculate);
+
+/**
+ * jz4780_bch_correct() - detect and correct bit errors
+ * @bch: BCH device.
+ * @params: BCH parameters.
+ * @buf: raw data read from the chip.
+ * @ecc_code: ECC read from the chip.
+ *
+ * Given the raw data and the ECC read from the NAND device, detects and
+ * corrects errors in the data.
+ *
+ * Return: the number of bit errors corrected, -EBADMSG if there are too many
+ * errors to correct or -ETIMEDOUT if we timed out waiting for the controller.
+ */
+int jz4780_bch_correct(struct jz4780_bch *bch, struct jz4780_bch_params *params,
+		       u8 *buf, u8 *ecc_code)
+{
+	u32 reg, mask, index;
+	int i, ret, count;
+
+	mutex_lock(&bch->lock);
+
+	jz4780_bch_init(bch, params, false);
+	jz4780_bch_write_data(bch, buf, params->size);
+	jz4780_bch_write_data(bch, ecc_code, params->bytes);
+
+	if (!jz4780_bch_wait_complete(bch, BCH_BHINT_DECF, &reg)) {
+		dev_err(bch->dev, "timed out while correcting data\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	}
+
+	if (reg & BCH_BHINT_UNCOR) {
+		dev_warn(bch->dev, "uncorrectable ECC error\n");
+		ret = -EBADMSG;
+		goto out;
+	}
+
+	/* Correct any detected errors. */
+	if (reg & BCH_BHINT_ERR) {
+		count = (reg & BCH_BHINT_ERRC_MASK) >> BCH_BHINT_ERRC_SHIFT;
+		ret = (reg & BCH_BHINT_TERRC_MASK) >> BCH_BHINT_TERRC_SHIFT;
+
+		for (i = 0; i < count; i++) {
+			reg = readl(bch->base + BCH_BHERR0 + (i * 4));
+			mask = (reg & BCH_BHERR_MASK_MASK) >>
+						BCH_BHERR_MASK_SHIFT;
+			index = (reg & BCH_BHERR_INDEX_MASK) >>
+						BCH_BHERR_INDEX_SHIFT;
+			buf[(index * 2) + 0] ^= mask;
+			buf[(index * 2) + 1] ^= mask >> 8;
+		}
+	} else {
+		ret = 0;
+	}
+
+out:
+	jz4780_bch_disable(bch);
+	mutex_unlock(&bch->lock);
+	return ret;
+}
+EXPORT_SYMBOL(jz4780_bch_correct);
+
+/**
+ * jz4780_bch_get() - get the BCH controller device
+ * @np: BCH device tree node.
+ *
+ * Gets the BCH controller device from the specified device tree node. The
+ * device must be released with jz4780_bch_release() when it is no longer being
+ * used.
+ *
+ * Return: a pointer to jz4780_bch, errors are encoded into the pointer.
+ * PTR_ERR(-EPROBE_DEFER) if the device hasn't been initialised yet.
+ */
+static struct jz4780_bch *jz4780_bch_get(struct device_node *np)
+{
+	struct platform_device *pdev;
+	struct jz4780_bch *bch;
+
+	pdev = of_find_device_by_node(np);
+	if (!pdev || !platform_get_drvdata(pdev))
+		return ERR_PTR(-EPROBE_DEFER);
+
+	get_device(&pdev->dev);
+
+	bch = platform_get_drvdata(pdev);
+	clk_prepare_enable(bch->clk);
+
+	bch->dev = &pdev->dev;
+	return bch;
+}
+
+/**
+ * of_jz4780_bch_get() - get the BCH controller from a DT node
+ * @of_node: the node that contains a bch-controller property.
+ *
+ * Get the bch-controller property from the given device tree
+ * node and pass it to jz4780_bch_get to do the work.
+ *
+ * Return: a pointer to jz4780_bch, errors are encoded into the pointer.
+ * PTR_ERR(-EPROBE_DEFER) if the device hasn't been initialised yet.
+ */
+struct jz4780_bch *of_jz4780_bch_get(struct device_node *of_node)
+{
+	struct jz4780_bch *bch = NULL;
+	struct device_node *np;
+
+	np = of_parse_phandle(of_node, "ingenic,bch-controller", 0);
+
+	if (np) {
+		bch = jz4780_bch_get(np);
+		of_node_put(np);
+	}
+	return bch;
+}
+EXPORT_SYMBOL(of_jz4780_bch_get);
+
+/**
+ * jz4780_bch_release() - release the BCH controller device
+ * @bch: BCH device.
+ */
+void jz4780_bch_release(struct jz4780_bch *bch)
+{
+	clk_disable_unprepare(bch->clk);
+	put_device(bch->dev);
+}
+EXPORT_SYMBOL(jz4780_bch_release);
+
+static int jz4780_bch_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct jz4780_bch *bch;
+	struct resource *res;
+
+	bch = devm_kzalloc(dev, sizeof(*bch), GFP_KERNEL);
+	if (!bch)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	bch->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(bch->base))
+		return PTR_ERR(bch->base);
+
+	jz4780_bch_disable(bch);
+
+	bch->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(bch->clk)) {
+		dev_err(dev, "failed to get clock: %ld\n", PTR_ERR(bch->clk));
+		return PTR_ERR(bch->clk);
+	}
+
+	clk_set_rate(bch->clk, BCH_CLK_RATE);
+
+	mutex_init(&bch->lock);
+
+	bch->dev = dev;
+	platform_set_drvdata(pdev, bch);
+
+	return 0;
+}
+
+static const struct of_device_id jz4780_bch_dt_match[] = {
+	{ .compatible = "ingenic,jz4780-bch" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, jz4780_bch_dt_match);
+
+static struct platform_driver jz4780_bch_driver = {
+	.probe		= jz4780_bch_probe,
+	.driver	= {
+		.name	= "jz4780-bch",
+		.of_match_table = of_match_ptr(jz4780_bch_dt_match),
+	},
+};
+module_platform_driver(jz4780_bch_driver);
+
+MODULE_AUTHOR("Alex Smith <alex@alex-smith.me.uk>");
+MODULE_AUTHOR("Harvey Hunt <harvey.hunt@imgtec.com>");
+MODULE_DESCRIPTION("Ingenic JZ4780 BCH error correction driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mtd/nand/jz4780_bch.h b/drivers/mtd/nand/jz4780_bch.h
new file mode 100644
index 000000000000..bf4718088a3a
--- /dev/null
+++ b/drivers/mtd/nand/jz4780_bch.h
@@ -0,0 +1,43 @@
+/*
+ * JZ4780 BCH controller
+ *
+ * Copyright (c) 2015 Imagination Technologies
+ * Author: Alex Smith <alex.smith@imgtec.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#ifndef __DRIVERS_MTD_NAND_JZ4780_BCH_H__
+#define __DRIVERS_MTD_NAND_JZ4780_BCH_H__
+
+#include <linux/types.h>
+
+struct device;
+struct device_node;
+struct jz4780_bch;
+
+/**
+ * struct jz4780_bch_params - BCH parameters
+ * @size: data bytes per ECC step.
+ * @bytes: ECC bytes per step.
+ * @strength: number of correctable bits per ECC step.
+ */
+struct jz4780_bch_params {
+	int size;
+	int bytes;
+	int strength;
+};
+
+int jz4780_bch_calculate(struct jz4780_bch *bch,
+				struct jz4780_bch_params *params,
+				const u8 *buf, u8 *ecc_code);
+int jz4780_bch_correct(struct jz4780_bch *bch,
+			      struct jz4780_bch_params *params, u8 *buf,
+			      u8 *ecc_code);
+
+void jz4780_bch_release(struct jz4780_bch *bch);
+struct jz4780_bch *of_jz4780_bch_get(struct device_node *np);
+
+#endif /* __DRIVERS_MTD_NAND_JZ4780_BCH_H__ */
diff --git a/drivers/mtd/nand/jz4780_nand.c b/drivers/mtd/nand/jz4780_nand.c
new file mode 100644
index 000000000000..e1c016c9d32d
--- /dev/null
+++ b/drivers/mtd/nand/jz4780_nand.c
@@ -0,0 +1,428 @@
+/*
+ * JZ4780 NAND driver
+ *
+ * Copyright (c) 2015 Imagination Technologies
+ * Author: Alex Smith <alex.smith@imgtec.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/gpio/consumer.h>
+#include <linux/of_mtd.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/partitions.h>
+
+#include <linux/jz4780-nemc.h>
+
+#include "jz4780_bch.h"
+
+#define DRV_NAME	"jz4780-nand"
+
+#define OFFSET_DATA	0x00000000
+#define OFFSET_CMD	0x00400000
+#define OFFSET_ADDR	0x00800000
+
+/* Command delay when there is no R/B pin. */
+#define RB_DELAY_US	100
+
+struct jz4780_nand_cs {
+	unsigned int bank;
+	void __iomem *base;
+};
+
+struct jz4780_nand_controller {
+	struct device *dev;
+	struct jz4780_bch *bch;
+	struct nand_hw_control controller;
+	unsigned int num_banks;
+	struct list_head chips;
+	int selected;
+	struct jz4780_nand_cs cs[];
+};
+
+struct jz4780_nand_chip {
+	struct nand_chip chip;
+	struct list_head chip_list;
+
+	struct nand_ecclayout ecclayout;
+
+	struct gpio_desc *busy_gpio;
+	struct gpio_desc *wp_gpio;
+	unsigned int reading: 1;
+};
+
+static inline struct jz4780_nand_chip *to_jz4780_nand_chip(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct jz4780_nand_chip, chip);
+}
+
+static inline struct jz4780_nand_controller *to_jz4780_nand_controller(struct nand_hw_control *ctrl)
+{
+	return container_of(ctrl, struct jz4780_nand_controller, controller);
+}
+
+static void jz4780_nand_select_chip(struct mtd_info *mtd, int chipnr)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+	struct jz4780_nand_controller *nfc = to_jz4780_nand_controller(nand->chip.controller);
+	struct jz4780_nand_cs *cs;
+
+	/* Ensure the currently selected chip is deasserted. */
+	if (chipnr == -1 && nfc->selected >= 0) {
+		cs = &nfc->cs[nfc->selected];
+		jz4780_nemc_assert(nfc->dev, cs->bank, false);
+	}
+
+	nfc->selected = chipnr;
+}
+
+static void jz4780_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
+				 unsigned int ctrl)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+	struct jz4780_nand_controller *nfc = to_jz4780_nand_controller(nand->chip.controller);
+	struct jz4780_nand_cs *cs;
+
+	if (WARN_ON(nfc->selected < 0))
+		return;
+
+	cs = &nfc->cs[nfc->selected];
+
+	jz4780_nemc_assert(nfc->dev, cs->bank, ctrl & NAND_NCE);
+
+	if (cmd == NAND_CMD_NONE)
+		return;
+
+	if (ctrl & NAND_ALE)
+		writeb(cmd, cs->base + OFFSET_ADDR);
+	else if (ctrl & NAND_CLE)
+		writeb(cmd, cs->base + OFFSET_CMD);
+}
+
+static int jz4780_nand_dev_ready(struct mtd_info *mtd)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+
+	return !gpiod_get_value_cansleep(nand->busy_gpio);
+}
+
+static void jz4780_nand_ecc_hwctl(struct mtd_info *mtd, int mode)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+
+	nand->reading = (mode == NAND_ECC_READ);
+}
+
+static int jz4780_nand_ecc_calculate(struct mtd_info *mtd, const u8 *dat,
+				     u8 *ecc_code)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+	struct jz4780_nand_controller *nfc = to_jz4780_nand_controller(nand->chip.controller);
+	struct jz4780_bch_params params;
+
+	/*
+	 * Don't need to generate the ECC when reading, BCH does it for us as
+	 * part of decoding/correction.
+	 */
+	if (nand->reading)
+		return 0;
+
+	params.size = nand->chip.ecc.size;
+	params.bytes = nand->chip.ecc.bytes;
+	params.strength = nand->chip.ecc.strength;
+
+	return jz4780_bch_calculate(nfc->bch, &params, dat, ecc_code);
+}
+
+static int jz4780_nand_ecc_correct(struct mtd_info *mtd, u8 *dat,
+				   u8 *read_ecc, u8 *calc_ecc)
+{
+	struct jz4780_nand_chip *nand = to_jz4780_nand_chip(mtd);
+	struct jz4780_nand_controller *nfc = to_jz4780_nand_controller(nand->chip.controller);
+	struct jz4780_bch_params params;
+
+	params.size = nand->chip.ecc.size;
+	params.bytes = nand->chip.ecc.bytes;
+	params.strength = nand->chip.ecc.strength;
+
+	return jz4780_bch_correct(nfc->bch, &params, dat, read_ecc);
+}
+
+static int jz4780_nand_init_ecc(struct jz4780_nand_chip *nand, struct device *dev)
+{
+	struct nand_chip *chip = &nand->chip;
+	struct mtd_info *mtd = nand_to_mtd(chip);
+	struct jz4780_nand_controller *nfc = to_jz4780_nand_controller(chip->controller);
+	struct nand_ecclayout *layout = &nand->ecclayout;
+	u32 start, i;
+
+	chip->ecc.bytes = fls((1 + 8) * chip->ecc.size)	*
+				(chip->ecc.strength / 8);
+
+	switch (chip->ecc.mode) {
+	case NAND_ECC_HW:
+		if (!nfc->bch) {
+			dev_err(dev, "HW BCH selected, but BCH controller not found\n");
+			return -ENODEV;
+		}
+
+		chip->ecc.hwctl = jz4780_nand_ecc_hwctl;
+		chip->ecc.calculate = jz4780_nand_ecc_calculate;
+		chip->ecc.correct = jz4780_nand_ecc_correct;
+		/* fall through */
+	case NAND_ECC_SOFT:
+	case NAND_ECC_SOFT_BCH:
+		dev_info(dev, "using %s (strength %d, size %d, bytes %d)\n",
+			(nfc->bch) ? "hardware BCH" : "software ECC",
+			chip->ecc.strength, chip->ecc.size, chip->ecc.bytes);
+		break;
+	case NAND_ECC_NONE:
+		dev_info(dev, "not using ECC\n");
+		break;
+	default:
+		dev_err(dev, "ECC mode %d not supported\n", chip->ecc.mode);
+		return -EINVAL;
+	}
+
+	/* The NAND core will generate the ECC layout for SW ECC */
+	if (chip->ecc.mode != NAND_ECC_HW)
+		return 0;
+
+	/* Generate ECC layout. ECC codes are right aligned in the OOB area. */
+	layout->eccbytes = mtd->writesize / chip->ecc.size * chip->ecc.bytes;
+
+	if (layout->eccbytes > mtd->oobsize - 2) {
+		dev_err(dev,
+			"invalid ECC config: required %d ECC bytes, but only %d are available",
+			layout->eccbytes, mtd->oobsize - 2);
+		return -EINVAL;
+	}
+
+	start = mtd->oobsize - layout->eccbytes;
+	for (i = 0; i < layout->eccbytes; i++)
+		layout->eccpos[i] = start + i;
+
+	layout->oobfree[0].offset = 2;
+	layout->oobfree[0].length = mtd->oobsize - layout->eccbytes - 2;
+
+	chip->ecc.layout = layout;
+	return 0;
+}
+
+static int jz4780_nand_init_chip(struct platform_device *pdev,
+				struct jz4780_nand_controller *nfc,
+				struct device_node *np,
+				unsigned int chipnr)
+{
+	struct device *dev = &pdev->dev;
+	struct jz4780_nand_chip *nand;
+	struct jz4780_nand_cs *cs;
+	struct resource *res;
+	struct nand_chip *chip;
+	struct mtd_info *mtd;
+	const __be32 *reg;
+	int ret = 0;
+
+	cs = &nfc->cs[chipnr];
+
+	reg = of_get_property(np, "reg", NULL);
+	if (!reg)
+		return -EINVAL;
+
+	cs->bank = be32_to_cpu(*reg);
+
+	jz4780_nemc_set_type(nfc->dev, cs->bank, JZ4780_NEMC_BANK_NAND);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, chipnr);
+	cs->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(cs->base))
+		return PTR_ERR(cs->base);
+
+	nand = devm_kzalloc(dev, sizeof(*nand), GFP_KERNEL);
+	if (!nand)
+		return -ENOMEM;
+
+	nand->busy_gpio = devm_gpiod_get_optional(dev, "rb", GPIOD_IN);
+
+	if (IS_ERR(nand->busy_gpio)) {
+		ret = PTR_ERR(nand->busy_gpio);
+		dev_err(dev, "failed to request busy GPIO: %d\n", ret);
+		return ret;
+	} else if (nand->busy_gpio) {
+		nand->chip.dev_ready = jz4780_nand_dev_ready;
+	}
+
+	nand->wp_gpio = devm_gpiod_get_optional(dev, "wp", GPIOD_OUT_LOW);
+
+	if (IS_ERR(nand->wp_gpio)) {
+		ret = PTR_ERR(nand->wp_gpio);
+		dev_err(dev, "failed to request WP GPIO: %d\n", ret);
+		return ret;
+	}
+
+	chip = &nand->chip;
+	mtd = nand_to_mtd(chip);
+	mtd->name = devm_kasprintf(dev, GFP_KERNEL, "%s.%d", dev_name(dev),
+				   cs->bank);
+	if (!mtd->name)
+		return -ENOMEM;
+	mtd->dev.parent = dev;
+
+	chip->IO_ADDR_R = cs->base + OFFSET_DATA;
+	chip->IO_ADDR_W = cs->base + OFFSET_DATA;
+	chip->chip_delay = RB_DELAY_US;
+	chip->options = NAND_NO_SUBPAGE_WRITE;
+	chip->select_chip = jz4780_nand_select_chip;
+	chip->cmd_ctrl = jz4780_nand_cmd_ctrl;
+	chip->ecc.mode = NAND_ECC_HW;
+	chip->controller = &nfc->controller;
+	nand_set_flash_node(chip, np);
+
+	ret = nand_scan_ident(mtd, 1, NULL);
+	if (ret)
+		return ret;
+
+	ret = jz4780_nand_init_ecc(nand, dev);
+	if (ret)
+		return ret;
+
+	ret = nand_scan_tail(mtd);
+	if (ret)
+		return ret;
+
+	ret = mtd_device_register(mtd, NULL, 0);
+	if (ret) {
+		nand_release(mtd);
+		return ret;
+	}
+
+	list_add_tail(&nand->chip_list, &nfc->chips);
+
+	return 0;
+}
+
+static void jz4780_nand_cleanup_chips(struct jz4780_nand_controller *nfc)
+{
+	struct jz4780_nand_chip *chip;
+
+	while (!list_empty(&nfc->chips)) {
+		chip = list_first_entry(&nfc->chips, struct jz4780_nand_chip, chip_list);
+		nand_release(nand_to_mtd(&chip->chip));
+		list_del(&chip->chip_list);
+	}
+}
+
+static int jz4780_nand_init_chips(struct jz4780_nand_controller *nfc,
+				  struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np;
+	int i = 0;
+	int ret;
+	int num_chips = of_get_child_count(dev->of_node);
+
+	if (num_chips > nfc->num_banks) {
+		dev_err(dev, "found %d chips but only %d banks\n", num_chips, nfc->num_banks);
+		return -EINVAL;
+	}
+
+	for_each_child_of_node(dev->of_node, np) {
+		ret = jz4780_nand_init_chip(pdev, nfc, np, i);
+		if (ret) {
+			jz4780_nand_cleanup_chips(nfc);
+			return ret;
+		}
+
+		i++;
+	}
+
+	return 0;
+}
+
+static int jz4780_nand_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	unsigned int num_banks;
+	struct jz4780_nand_controller *nfc;
+	int ret;
+
+	num_banks = jz4780_nemc_num_banks(dev);
+	if (num_banks == 0) {
+		dev_err(dev, "no banks found\n");
+		return -ENODEV;
+	}
+
+	nfc = devm_kzalloc(dev, sizeof(*nfc) + (sizeof(nfc->cs[0]) * num_banks), GFP_KERNEL);
+	if (!nfc)
+		return -ENOMEM;
+
+	/*
+	 * Check for BCH HW before we call nand_scan_ident, to prevent us from
+	 * having to call it again if the BCH driver returns -EPROBE_DEFER.
+	 */
+	nfc->bch = of_jz4780_bch_get(dev->of_node);
+	if (IS_ERR(nfc->bch))
+		return PTR_ERR(nfc->bch);
+
+	nfc->dev = dev;
+	nfc->num_banks = num_banks;
+
+	spin_lock_init(&nfc->controller.lock);
+	INIT_LIST_HEAD(&nfc->chips);
+	init_waitqueue_head(&nfc->controller.wq);
+
+	ret = jz4780_nand_init_chips(nfc, pdev);
+	if (ret) {
+		if (nfc->bch)
+			jz4780_bch_release(nfc->bch);
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, nfc);
+	return 0;
+}
+
+static int jz4780_nand_remove(struct platform_device *pdev)
+{
+	struct jz4780_nand_controller *nfc = platform_get_drvdata(pdev);
+
+	if (nfc->bch)
+		jz4780_bch_release(nfc->bch);
+
+	jz4780_nand_cleanup_chips(nfc);
+
+	return 0;
+}
+
+static const struct of_device_id jz4780_nand_dt_match[] = {
+	{ .compatible = "ingenic,jz4780-nand" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, jz4780_nand_dt_match);
+
+static struct platform_driver jz4780_nand_driver = {
+	.probe		= jz4780_nand_probe,
+	.remove		= jz4780_nand_remove,
+	.driver	= {
+		.name	= DRV_NAME,
+		.of_match_table = of_match_ptr(jz4780_nand_dt_match),
+	},
+};
+module_platform_driver(jz4780_nand_driver);
+
+MODULE_AUTHOR("Alex Smith <alex@alex-smith.me.uk>");
+MODULE_AUTHOR("Harvey Hunt <harvey.hunt@imgtec.com>");
+MODULE_DESCRIPTION("Ingenic JZ4780 NAND driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mtd/nand/lpc32xx_mlc.c b/drivers/mtd/nand/lpc32xx_mlc.c
index 347510978484..9bc435d72a86 100644
--- a/drivers/mtd/nand/lpc32xx_mlc.c
+++ b/drivers/mtd/nand/lpc32xx_mlc.c
@@ -173,7 +173,6 @@ struct lpc32xx_nand_host {
 	struct nand_chip	nand_chip;
 	struct lpc32xx_mlc_platform_data *pdata;
 	struct clk		*clk;
-	struct mtd_info		mtd;
 	void __iomem		*io_base;
 	int			irq;
 	struct lpc32xx_nand_cfg_mlc	*ncfg;
@@ -275,8 +274,8 @@ static void lpc32xx_nand_setup(struct lpc32xx_nand_host *host)
 static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
 				  unsigned int ctrl)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct lpc32xx_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (cmd != NAND_CMD_NONE) {
 		if (ctrl & NAND_CLE)
@@ -291,8 +290,8 @@ static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
  */
 static int lpc32xx_nand_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct lpc32xx_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if ((readb(MLC_ISR(host->io_base)) &
 	     (MLCISR_CONTROLLER_READY | MLCISR_NAND_READY)) ==
@@ -318,7 +317,7 @@ static irqreturn_t lpc3xxx_nand_irq(int irq, struct lpc32xx_nand_host *host)
 
 static int lpc32xx_waitfunc_nand(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	if (readb(MLC_ISR(host->io_base)) & MLCISR_NAND_READY)
 		goto exit;
@@ -338,7 +337,7 @@ exit:
 static int lpc32xx_waitfunc_controller(struct mtd_info *mtd,
 				       struct nand_chip *chip)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	if (readb(MLC_ISR(host->io_base)) & MLCISR_CONTROLLER_READY)
 		goto exit;
@@ -389,8 +388,8 @@ static void lpc32xx_dma_complete_func(void *completion)
 static int lpc32xx_xmit_dma(struct mtd_info *mtd, void *mem, int len,
 			    enum dma_transfer_direction dir)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	struct dma_async_tx_descriptor *desc;
 	int flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
 	int res;
@@ -431,7 +430,7 @@ out1:
 static int lpc32xx_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 			     uint8_t *buf, int oob_required, int page)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	int i, j;
 	uint8_t *oobbuf = chip->oob_poi;
 	uint32_t mlc_isr;
@@ -498,7 +497,7 @@ static int lpc32xx_write_page_lowlevel(struct mtd_info *mtd,
 				       const uint8_t *buf, int oob_required,
 				       int page)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	const uint8_t *oobbuf = chip->oob_poi;
 	uint8_t *dma_buf = (uint8_t *)buf;
 	int res;
@@ -543,7 +542,7 @@ static int lpc32xx_write_page_lowlevel(struct mtd_info *mtd,
 static int lpc32xx_read_oob(struct mtd_info *mtd, struct nand_chip *chip,
 			    int page)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	/* Read whole page - necessary with MLC controller! */
 	lpc32xx_read_page(mtd, chip, host->dummy_buf, 1, page);
@@ -566,7 +565,7 @@ static void lpc32xx_ecc_enable(struct mtd_info *mtd, int mode)
 
 static int lpc32xx_dma_setup(struct lpc32xx_nand_host *host)
 {
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 	dma_cap_mask_t mask;
 
 	if (!host->pdata || !host->pdata->dma_filter) {
@@ -647,7 +646,6 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 	struct nand_chip *nand_chip;
 	struct resource *rc;
 	int res;
-	struct mtd_part_parser_data ppdata = {};
 
 	/* Allocate memory for the device structure (and zero it) */
 	host = devm_kzalloc(&pdev->dev, sizeof(*host), GFP_KERNEL);
@@ -661,8 +659,8 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 	
 	host->io_base_phy = rc->start;
 
-	mtd = &host->mtd;
 	nand_chip = &host->nand_chip;
+	mtd = nand_to_mtd(nand_chip);
 	if (pdev->dev.of_node)
 		host->ncfg = lpc32xx_parse_dt(&pdev->dev);
 	if (!host->ncfg) {
@@ -681,8 +679,9 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 
 	host->pdata = dev_get_platdata(&pdev->dev);
 
-	nand_chip->priv = host;		/* link the private data structures */
-	mtd->priv = nand_chip;
+	/* link the private data structures */
+	nand_set_controller_data(nand_chip, host);
+	nand_set_flash_node(nand_chip, pdev->dev.of_node);
 	mtd->dev.parent = &pdev->dev;
 
 	/* Get NAND clock */
@@ -786,9 +785,8 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 
 	mtd->name = DRV_NAME;
 
-	ppdata.of_node = pdev->dev.of_node;
-	res = mtd_device_parse_register(mtd, NULL, &ppdata, host->ncfg->parts,
-					host->ncfg->num_parts);
+	res = mtd_device_register(mtd, host->ncfg->parts,
+				  host->ncfg->num_parts);
 	if (!res)
 		return res;
 
@@ -815,7 +813,7 @@ err_exit1:
 static int lpc32xx_nand_remove(struct platform_device *pdev)
 {
 	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 
 	nand_release(mtd);
 	free_irq(host->irq, host);
diff --git a/drivers/mtd/nand/lpc32xx_slc.c b/drivers/mtd/nand/lpc32xx_slc.c
index 4f3d4eb17da0..3b8f3735f3e8 100644
--- a/drivers/mtd/nand/lpc32xx_slc.c
+++ b/drivers/mtd/nand/lpc32xx_slc.c
@@ -204,7 +204,6 @@ struct lpc32xx_nand_host {
 	struct nand_chip	nand_chip;
 	struct lpc32xx_slc_platform_data *pdata;
 	struct clk		*clk;
-	struct mtd_info		mtd;
 	void __iomem		*io_base;
 	struct lpc32xx_nand_cfg_slc *ncfg;
 
@@ -260,8 +259,8 @@ static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
 	unsigned int ctrl)
 {
 	uint32_t tmp;
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	/* Does CE state need to be changed? */
 	tmp = readl(SLC_CFG(host->io_base));
@@ -284,8 +283,8 @@ static void lpc32xx_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
  */
 static int lpc32xx_nand_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	int rdy = 0;
 
 	if ((readl(SLC_STAT(host->io_base)) & SLCSTAT_NAND_READY) != 0)
@@ -339,8 +338,8 @@ static int lpc32xx_nand_ecc_calculate(struct mtd_info *mtd,
  */
 static uint8_t lpc32xx_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	return (uint8_t)readl(SLC_DATA(host->io_base));
 }
@@ -350,8 +349,8 @@ static uint8_t lpc32xx_nand_read_byte(struct mtd_info *mtd)
  */
 static void lpc32xx_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	/* Direct device read with no ECC */
 	while (len-- > 0)
@@ -363,8 +362,8 @@ static void lpc32xx_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
  */
 static void lpc32xx_nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 
 	/* Direct device write with no ECC */
 	while (len-- > 0)
@@ -428,8 +427,8 @@ static void lpc32xx_dma_complete_func(void *completion)
 static int lpc32xx_xmit_dma(struct mtd_info *mtd, dma_addr_t dma,
 			    void *mem, int len, enum dma_transfer_direction dir)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	struct dma_async_tx_descriptor *desc;
 	int flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT;
 	int res;
@@ -488,8 +487,8 @@ out1:
 static int lpc32xx_xfer(struct mtd_info *mtd, uint8_t *buf, int eccsubpages,
 			int read)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	int i, status = 0;
 	unsigned long timeout;
 	int res;
@@ -604,7 +603,7 @@ static int lpc32xx_nand_read_page_syndrome(struct mtd_info *mtd,
 					   struct nand_chip *chip, uint8_t *buf,
 					   int oob_required, int page)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	int stat, i, status;
 	uint8_t *oobecc, tmpecc[LPC32XX_ECC_SAVE_SIZE];
 
@@ -666,7 +665,7 @@ static int lpc32xx_nand_write_page_syndrome(struct mtd_info *mtd,
 					    const uint8_t *buf,
 					    int oob_required, int page)
 {
-	struct lpc32xx_nand_host *host = chip->priv;
+	struct lpc32xx_nand_host *host = nand_get_controller_data(chip);
 	uint8_t *pb = chip->oob_poi + chip->ecc.layout->eccpos[0];
 	int error;
 
@@ -703,7 +702,7 @@ static int lpc32xx_nand_write_page_raw_syndrome(struct mtd_info *mtd,
 
 static int lpc32xx_nand_dma_setup(struct lpc32xx_nand_host *host)
 {
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 	dma_cap_mask_t mask;
 
 	if (!host->pdata || !host->pdata->dma_filter) {
@@ -763,7 +762,6 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 	struct mtd_info *mtd;
 	struct nand_chip *chip;
 	struct resource *rc;
-	struct mtd_part_parser_data ppdata = {};
 	int res;
 
 	rc = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -800,10 +798,10 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 
 	host->pdata = dev_get_platdata(&pdev->dev);
 
-	mtd = &host->mtd;
 	chip = &host->nand_chip;
-	chip->priv = host;
-	mtd->priv = chip;
+	mtd = nand_to_mtd(chip);
+	nand_set_controller_data(chip, host);
+	nand_set_flash_node(chip, pdev->dev.of_node);
 	mtd->owner = THIS_MODULE;
 	mtd->dev.parent = &pdev->dev;
 
@@ -908,9 +906,8 @@ static int lpc32xx_nand_probe(struct platform_device *pdev)
 	}
 
 	mtd->name = "nxp_lpc3220_slc";
-	ppdata.of_node = pdev->dev.of_node;
-	res = mtd_device_parse_register(mtd, NULL, &ppdata, host->ncfg->parts,
-					host->ncfg->num_parts);
+	res = mtd_device_register(mtd, host->ncfg->parts,
+				  host->ncfg->num_parts);
 	if (!res)
 		return res;
 
@@ -933,7 +930,7 @@ static int lpc32xx_nand_remove(struct platform_device *pdev)
 {
 	uint32_t tmp;
 	struct lpc32xx_nand_host *host = platform_get_drvdata(pdev);
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 
 	nand_release(mtd);
 	dma_release_channel(host->dma_chan);
diff --git a/drivers/mtd/nand/mpc5121_nfc.c b/drivers/mtd/nand/mpc5121_nfc.c
index d6bbde4a5331..6b93e899d4e9 100644
--- a/drivers/mtd/nand/mpc5121_nfc.c
+++ b/drivers/mtd/nand/mpc5121_nfc.c
@@ -118,7 +118,6 @@
 #define NFC_TIMEOUT		(HZ / 10)	/* 1/10 s */
 
 struct mpc5121_nfc_prv {
-	struct mtd_info		mtd;
 	struct nand_chip	chip;
 	int			irq;
 	void __iomem		*regs;
@@ -135,8 +134,8 @@ static void mpc5121_nfc_done(struct mtd_info *mtd);
 /* Read NFC register */
 static inline u16 nfc_read(struct mtd_info *mtd, uint reg)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	return in_be16(prv->regs + reg);
 }
@@ -144,8 +143,8 @@ static inline u16 nfc_read(struct mtd_info *mtd, uint reg)
 /* Write NFC register */
 static inline void nfc_write(struct mtd_info *mtd, uint reg, u16 val)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	out_be16(prv->regs + reg, val);
 }
@@ -214,8 +213,8 @@ static inline void mpc5121_nfc_send_read_status(struct mtd_info *mtd)
 static irqreturn_t mpc5121_nfc_irq(int irq, void *data)
 {
 	struct mtd_info *mtd = data;
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	nfc_set(mtd, NFC_CONFIG1, NFC_INT_MASK);
 	wake_up(&prv->irq_waitq);
@@ -226,8 +225,8 @@ static irqreturn_t mpc5121_nfc_irq(int irq, void *data)
 /* Wait for operation complete */
 static void mpc5121_nfc_done(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 	int rv;
 
 	if ((nfc_read(mtd, NFC_CONFIG2) & NFC_INT) == 0) {
@@ -246,7 +245,7 @@ static void mpc5121_nfc_done(struct mtd_info *mtd)
 /* Do address cycle(s) */
 static void mpc5121_nfc_addr_cycle(struct mtd_info *mtd, int column, int page)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 pagemask = chip->pagemask;
 
 	if (column != -1) {
@@ -281,8 +280,8 @@ static void mpc5121_nfc_select_chip(struct mtd_info *mtd, int chip)
 /* Init external chip select logic on ADS5121 board */
 static int ads5121_chipselect_init(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 	struct device_node *dn;
 
 	dn = of_find_compatible_node(NULL, NULL, "fsl,mpc5121ads-cpld");
@@ -303,8 +302,8 @@ static int ads5121_chipselect_init(struct mtd_info *mtd)
 /* Control chips select signal on ADS5121 board */
 static void ads5121_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mpc5121_nfc_prv *prv = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(nand);
 	u8 v;
 
 	v = in_8(prv->csreg);
@@ -333,8 +332,8 @@ static int mpc5121_nfc_dev_ready(struct mtd_info *mtd)
 static void mpc5121_nfc_command(struct mtd_info *mtd, unsigned command,
 							int column, int page)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	prv->column = (column >= 0) ? column : 0;
 	prv->spareonly = 0;
@@ -406,8 +405,8 @@ static void mpc5121_nfc_command(struct mtd_info *mtd, unsigned command,
 static void mpc5121_nfc_copy_spare(struct mtd_info *mtd, uint offset,
 						u8 *buffer, uint size, int wr)
 {
-	struct nand_chip *nand = mtd->priv;
-	struct mpc5121_nfc_prv *prv = nand->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(nand);
 	uint o, s, sbsize, blksize;
 
 	/*
@@ -458,8 +457,8 @@ static void mpc5121_nfc_copy_spare(struct mtd_info *mtd, uint offset,
 static void mpc5121_nfc_buf_copy(struct mtd_info *mtd, u_char *buf, int len,
 									int wr)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 	uint c = prv->column;
 	uint l;
 
@@ -536,8 +535,8 @@ static u16 mpc5121_nfc_read_word(struct mtd_info *mtd)
  */
 static int mpc5121_nfc_read_hw_config(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 	struct mpc512x_reset_module *rm;
 	struct device_node *rmnode;
 	uint rcw_pagesize = 0;
@@ -615,8 +614,8 @@ out:
 /* Free driver resources */
 static void mpc5121_nfc_free(struct device *dev, struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mpc5121_nfc_prv *prv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mpc5121_nfc_prv *prv = nand_get_controller_data(chip);
 
 	if (prv->clk)
 		clk_disable_unprepare(prv->clk);
@@ -639,7 +638,6 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	int resettime = 0;
 	int retval = 0;
 	int rev, len;
-	struct mtd_part_parser_data ppdata;
 
 	/*
 	 * Check SoC revision. This driver supports only NFC
@@ -655,12 +653,12 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	if (!prv)
 		return -ENOMEM;
 
-	mtd = &prv->mtd;
 	chip = &prv->chip;
+	mtd = nand_to_mtd(chip);
 
-	mtd->priv = chip;
 	mtd->dev.parent = dev;
-	chip->priv = prv;
+	nand_set_controller_data(chip, prv);
+	nand_set_flash_node(chip, dn);
 	prv->dev = dev;
 
 	/* Read NFC configuration from Reset Config Word */
@@ -703,7 +701,6 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	}
 
 	mtd->name = "MPC5121 NAND";
-	ppdata.of_node = dn;
 	chip->dev_ready = mpc5121_nfc_dev_ready;
 	chip->cmdfunc = mpc5121_nfc_command;
 	chip->read_byte = mpc5121_nfc_read_byte;
@@ -815,7 +812,7 @@ static int mpc5121_nfc_probe(struct platform_device *op)
 	dev_set_drvdata(dev, mtd);
 
 	/* Register device in MTD */
-	retval = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	retval = mtd_device_register(mtd, NULL, 0);
 	if (retval) {
 		dev_err(dev, "Error adding MTD device!\n");
 		goto error;
diff --git a/drivers/mtd/nand/mxc_nand.c b/drivers/mtd/nand/mxc_nand.c
index 136e73a3e07e..854c832597aa 100644
--- a/drivers/mtd/nand/mxc_nand.c
+++ b/drivers/mtd/nand/mxc_nand.c
@@ -173,7 +173,6 @@ struct mxc_nand_devtype_data {
 };
 
 struct mxc_nand_host {
-	struct mtd_info		mtd;
 	struct nand_chip	nand;
 	struct device		*dev;
 
@@ -532,8 +531,8 @@ static void send_addr_v1_v2(struct mxc_nand_host *host, uint16_t addr, int islas
 
 static void send_page_v3(struct mtd_info *mtd, unsigned int ops)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint32_t tmp;
 
 	tmp = readl(NFC_V3_CONFIG1);
@@ -548,8 +547,8 @@ static void send_page_v3(struct mtd_info *mtd, unsigned int ops)
 
 static void send_page_v2(struct mtd_info *mtd, unsigned int ops)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	/* NANDFC buffer 0 is used for page read/write */
 	writew(host->active_cs << 4, NFC_V1_V2_BUF_ADDR);
@@ -562,8 +561,8 @@ static void send_page_v2(struct mtd_info *mtd, unsigned int ops)
 
 static void send_page_v1(struct mtd_info *mtd, unsigned int ops)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	int bufs, i;
 
 	if (mtd->writesize > 512)
@@ -663,8 +662,8 @@ static void mxc_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 static int mxc_nand_correct_data_v1(struct mtd_info *mtd, u_char *dat,
 				 u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	/*
 	 * 1-Bit errors are automatically corrected in HW.  No need for
@@ -675,7 +674,7 @@ static int mxc_nand_correct_data_v1(struct mtd_info *mtd, u_char *dat,
 
 	if (((ecc_status & 0x3) == 2) || ((ecc_status >> 2) == 2)) {
 		pr_debug("MXC_NAND: HWECC uncorrectable 2-bit ECC error\n");
-		return -1;
+		return -EBADMSG;
 	}
 
 	return 0;
@@ -684,8 +683,8 @@ static int mxc_nand_correct_data_v1(struct mtd_info *mtd, u_char *dat,
 static int mxc_nand_correct_data_v2_v3(struct mtd_info *mtd, u_char *dat,
 				 u_char *read_ecc, u_char *calc_ecc)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	u32 ecc_stat, err;
 	int no_subpages = 1;
 	int ret = 0;
@@ -702,7 +701,7 @@ static int mxc_nand_correct_data_v2_v3(struct mtd_info *mtd, u_char *dat,
 		err = ecc_stat & ecc_bit_mask;
 		if (err > err_limit) {
 			printk(KERN_WARNING "UnCorrectable RS-ECC Error\n");
-			return -1;
+			return -EBADMSG;
 		} else {
 			ret += err;
 		}
@@ -722,8 +721,8 @@ static int mxc_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 
 static u_char mxc_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint8_t ret;
 
 	/* Check for status request */
@@ -746,8 +745,8 @@ static u_char mxc_nand_read_byte(struct mtd_info *mtd)
 
 static uint16_t mxc_nand_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint16_t ret;
 
 	ret = *(uint16_t *)(host->data_buf + host->buf_start);
@@ -762,8 +761,8 @@ static uint16_t mxc_nand_read_word(struct mtd_info *mtd)
 static void mxc_nand_write_buf(struct mtd_info *mtd,
 				const u_char *buf, int len)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	u16 col = host->buf_start;
 	int n = mtd->oobsize + mtd->writesize - col;
 
@@ -780,8 +779,8 @@ static void mxc_nand_write_buf(struct mtd_info *mtd,
  */
 static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	u16 col = host->buf_start;
 	int n = mtd->oobsize + mtd->writesize - col;
 
@@ -796,8 +795,8 @@ static void mxc_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
  * deselect of the NAND chip */
 static void mxc_nand_select_chip_v1_v3(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (chip == -1) {
 		/* Disable the NFC clock */
@@ -817,8 +816,8 @@ static void mxc_nand_select_chip_v1_v3(struct mtd_info *mtd, int chip)
 
 static void mxc_nand_select_chip_v2(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (chip == -1) {
 		/* Disable the NFC clock */
@@ -850,8 +849,8 @@ static void mxc_nand_select_chip_v2(struct mtd_info *mtd, int chip)
  */
 static void copy_spare(struct mtd_info *mtd, bool bfrom)
 {
-	struct nand_chip *this = mtd->priv;
-	struct mxc_nand_host *host = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(this);
 	u16 i, oob_chunk_size;
 	u16 num_chunks = mtd->writesize / 512;
 
@@ -893,8 +892,8 @@ static void copy_spare(struct mtd_info *mtd, bool bfrom)
  */
 static void mxc_do_addr_cycle(struct mtd_info *mtd, int column, int page_addr)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	/* Write out column address, if necessary */
 	if (column != -1) {
@@ -979,8 +978,8 @@ static void ecc_8bit_layout_4k(struct nand_ecclayout *layout)
 
 static void preset_v1(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint16_t config1 = 0;
 
 	if (nand_chip->ecc.mode == NAND_ECC_HW && mtd->writesize)
@@ -1007,8 +1006,8 @@ static void preset_v1(struct mtd_info *mtd)
 
 static void preset_v2(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 	uint16_t config1 = 0;
 
 	config1 |= NFC_V2_CONFIG1_FP_INT;
@@ -1053,8 +1052,8 @@ static void preset_v2(struct mtd_info *mtd)
 
 static void preset_v3(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct mxc_nand_host *host = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(chip);
 	uint32_t config2, config3;
 	int i, addr_phases;
 
@@ -1067,8 +1066,7 @@ static void preset_v3(struct mtd_info *mtd)
 
 	/* Blocks to be unlocked */
 	for (i = 0; i < NAND_MAX_CHIPS; i++)
-		writel(0x0 |	(0xffff << 16),
-				NFC_V3_WRPROT_UNLOCK_BLK_ADD0 + (i << 2));
+		writel(0xffff << 16, NFC_V3_WRPROT_UNLOCK_BLK_ADD0 + (i << 2));
 
 	writel(0, NFC_V3_IPC);
 
@@ -1125,8 +1123,8 @@ static void preset_v3(struct mtd_info *mtd)
 static void mxc_nand_command(struct mtd_info *mtd, unsigned command,
 				int column, int page_addr)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct mxc_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct mxc_nand_host *host = nand_get_controller_data(nand_chip);
 
 	pr_debug("mxc_nand_command (cmd = 0x%x, col = 0x%x, page = 0x%x)\n",
 	      command, column, page_addr);
@@ -1515,15 +1513,15 @@ static int mxcnd_probe(struct platform_device *pdev)
 	host->dev = &pdev->dev;
 	/* structures must be linked */
 	this = &host->nand;
-	mtd = &host->mtd;
-	mtd->priv = this;
+	mtd = nand_to_mtd(this);
 	mtd->dev.parent = &pdev->dev;
 	mtd->name = DRIVER_NAME;
 
 	/* 50 us command delay time */
 	this->chip_delay = 5;
 
-	this->priv = host;
+	nand_set_controller_data(this, host);
+	nand_set_flash_node(this, pdev->dev.of_node),
 	this->dev_ready = mxc_nand_dev_ready;
 	this->cmdfunc = mxc_nand_command;
 	this->read_byte = mxc_nand_read_byte;
@@ -1683,9 +1681,7 @@ static int mxcnd_probe(struct platform_device *pdev)
 
 	/* Register the partitions */
 	mtd_device_parse_register(mtd, part_probes,
-			&(struct mtd_part_parser_data){
-				.of_node = pdev->dev.of_node,
-			},
+			NULL,
 			host->pdata.parts,
 			host->pdata.nr_parts);
 
@@ -1704,7 +1700,7 @@ static int mxcnd_remove(struct platform_device *pdev)
 {
 	struct mxc_nand_host *host = platform_get_drvdata(pdev);
 
-	nand_release(&host->mtd);
+	nand_release(nand_to_mtd(&host->nand));
 	if (host->clk_act)
 		clk_disable_unprepare(host->clk);
 
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index ece544efccc3..f2c8ff398d6c 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -106,7 +106,7 @@ DEFINE_LED_TRIGGER(nand_led_trigger);
 static int check_offs_len(struct mtd_info *mtd,
 					loff_t ofs, uint64_t len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret = 0;
 
 	/* Start address must align on block boundary */
@@ -132,7 +132,7 @@ static int check_offs_len(struct mtd_info *mtd,
  */
 static void nand_release_device(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Release the controller and the chip */
 	spin_lock(&chip->controller->lock);
@@ -150,7 +150,7 @@ static void nand_release_device(struct mtd_info *mtd)
  */
 static uint8_t nand_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return readb(chip->IO_ADDR_R);
 }
 
@@ -163,7 +163,7 @@ static uint8_t nand_read_byte(struct mtd_info *mtd)
  */
 static uint8_t nand_read_byte16(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return (uint8_t) cpu_to_le16(readw(chip->IO_ADDR_R));
 }
 
@@ -175,7 +175,7 @@ static uint8_t nand_read_byte16(struct mtd_info *mtd)
  */
 static u16 nand_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	return readw(chip->IO_ADDR_R);
 }
 
@@ -188,7 +188,7 @@ static u16 nand_read_word(struct mtd_info *mtd)
  */
 static void nand_select_chip(struct mtd_info *mtd, int chipnr)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	switch (chipnr) {
 	case -1:
@@ -211,7 +211,7 @@ static void nand_select_chip(struct mtd_info *mtd, int chipnr)
  */
 static void nand_write_byte(struct mtd_info *mtd, uint8_t byte)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	chip->write_buf(mtd, &byte, 1);
 }
@@ -225,7 +225,7 @@ static void nand_write_byte(struct mtd_info *mtd, uint8_t byte)
  */
 static void nand_write_byte16(struct mtd_info *mtd, uint8_t byte)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint16_t word = byte;
 
 	/*
@@ -257,7 +257,7 @@ static void nand_write_byte16(struct mtd_info *mtd, uint8_t byte)
  */
 static void nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	iowrite8_rep(chip->IO_ADDR_W, buf, len);
 }
@@ -272,7 +272,7 @@ static void nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
  */
 static void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	ioread8_rep(chip->IO_ADDR_R, buf, len);
 }
@@ -287,7 +287,7 @@ static void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
  */
 static void nand_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 
 	iowrite16_rep(chip->IO_ADDR_W, p, len >> 1);
@@ -303,7 +303,7 @@ static void nand_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
  */
 static void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 *p = (u16 *) buf;
 
 	ioread16_rep(chip->IO_ADDR_R, p, len >> 1);
@@ -320,7 +320,7 @@ static void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
 static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
 {
 	int page, chipnr, res = 0, i = 0;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u16 bad;
 
 	if (chip->bbt_options & NAND_BBT_SCANLASTPAGE)
@@ -380,7 +380,7 @@ static int nand_block_bad(struct mtd_info *mtd, loff_t ofs, int getchip)
  */
 static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_oob_ops ops;
 	uint8_t buf[2] = { 0, 0 };
 	int ret = 0, res, i = 0;
@@ -430,7 +430,7 @@ static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
 */
 static int nand_block_markbad_lowlevel(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int res, ret = 0;
 
 	if (!(chip->bbt_options & NAND_BBT_NO_OOB_BBM)) {
@@ -471,7 +471,7 @@ static int nand_block_markbad_lowlevel(struct mtd_info *mtd, loff_t ofs)
  */
 static int nand_check_wp(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Broken xD cards report WP despite being writable */
 	if (chip->options & NAND_BROKEN_XD)
@@ -491,7 +491,7 @@ static int nand_check_wp(struct mtd_info *mtd)
  */
 static int nand_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (!chip->bbt)
 		return 0;
@@ -512,7 +512,7 @@ static int nand_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 static int nand_block_checkbad(struct mtd_info *mtd, loff_t ofs, int getchip,
 			       int allowbbt)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (!chip->bbt)
 		return chip->block_bad(mtd, ofs, getchip);
@@ -531,7 +531,7 @@ static int nand_block_checkbad(struct mtd_info *mtd, loff_t ofs, int getchip,
  */
 static void panic_nand_wait_ready(struct mtd_info *mtd, unsigned long timeo)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int i;
 
 	/* Wait for the device to get ready */
@@ -551,7 +551,7 @@ static void panic_nand_wait_ready(struct mtd_info *mtd, unsigned long timeo)
  */
 void nand_wait_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	unsigned long timeo = 400;
 
 	if (in_interrupt() || oops_in_progress)
@@ -582,7 +582,7 @@ EXPORT_SYMBOL_GPL(nand_wait_ready);
  */
 static void nand_wait_status_ready(struct mtd_info *mtd, unsigned long timeo)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 
 	timeo = jiffies + msecs_to_jiffies(timeo);
 	do {
@@ -605,7 +605,7 @@ static void nand_wait_status_ready(struct mtd_info *mtd, unsigned long timeo)
 static void nand_command(struct mtd_info *mtd, unsigned int command,
 			 int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 	int ctrl = NAND_CTRL_CLE | NAND_CTRL_CHANGE;
 
 	/* Write out the command to the device */
@@ -708,7 +708,7 @@ static void nand_command(struct mtd_info *mtd, unsigned int command,
 static void nand_command_lp(struct mtd_info *mtd, unsigned int command,
 			    int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
+	register struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Emulate NAND_CMD_READOOB */
 	if (command == NAND_CMD_READOOB) {
@@ -832,7 +832,7 @@ static void panic_nand_get_device(struct nand_chip *chip,
 static int
 nand_get_device(struct mtd_info *mtd, int new_state)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	spinlock_t *lock = &chip->controller->lock;
 	wait_queue_head_t *wq = &chip->controller->wq;
 	DECLARE_WAITQUEUE(wait, current);
@@ -952,7 +952,7 @@ static int __nand_unlock(struct mtd_info *mtd, loff_t ofs,
 {
 	int ret = 0;
 	int status, page;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/* Submit address of first page to unlock */
 	page = ofs >> chip->page_shift;
@@ -987,7 +987,7 @@ int nand_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
 	int ret = 0;
 	int chipnr;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("%s: start = 0x%012llx, len = %llu\n",
 			__func__, (unsigned long long)ofs, len);
@@ -1050,7 +1050,7 @@ int nand_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
 	int ret = 0;
 	int chipnr, status, page;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("%s: start = 0x%012llx, len = %llu\n",
 			__func__, (unsigned long long)ofs, len);
@@ -1426,6 +1426,16 @@ static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip,
 
 		stat = chip->ecc.correct(mtd, p,
 			&chip->buffers->ecccode[i], &chip->buffers->ecccalc[i]);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, chip->ecc.size,
+						&chip->buffers->ecccode[i],
+						chip->ecc.bytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1475,6 +1485,15 @@ static int nand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 		int stat;
 
 		stat = chip->ecc.correct(mtd, p, &ecc_code[i], &ecc_calc[i]);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+						&ecc_code[i], eccbytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1527,6 +1546,15 @@ static int nand_read_page_hwecc_oob_first(struct mtd_info *mtd,
 		chip->ecc.calculate(mtd, p, &ecc_calc[i]);
 
 		stat = chip->ecc.correct(mtd, p, &ecc_code[i], NULL);
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, eccsize,
+						&ecc_code[i], eccbytes,
+						NULL, 0,
+						chip->ecc.strength);
+		}
+
 		if (stat < 0) {
 			mtd->ecc_stats.failed++;
 		} else {
@@ -1554,6 +1582,7 @@ static int nand_read_page_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
 	int i, eccsize = chip->ecc.size;
 	int eccbytes = chip->ecc.bytes;
 	int eccsteps = chip->ecc.steps;
+	int eccpadbytes = eccbytes + chip->ecc.prepad + chip->ecc.postpad;
 	uint8_t *p = buf;
 	uint8_t *oob = chip->oob_poi;
 	unsigned int max_bitflips = 0;
@@ -1573,19 +1602,29 @@ static int nand_read_page_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
 		chip->read_buf(mtd, oob, eccbytes);
 		stat = chip->ecc.correct(mtd, p, oob, NULL);
 
-		if (stat < 0) {
-			mtd->ecc_stats.failed++;
-		} else {
-			mtd->ecc_stats.corrected += stat;
-			max_bitflips = max_t(unsigned int, max_bitflips, stat);
-		}
-
 		oob += eccbytes;
 
 		if (chip->ecc.postpad) {
 			chip->read_buf(mtd, oob, chip->ecc.postpad);
 			oob += chip->ecc.postpad;
 		}
+
+		if (stat == -EBADMSG &&
+		    (chip->ecc.options & NAND_ECC_GENERIC_ERASED_CHECK)) {
+			/* check for empty pages with bitflips */
+			stat = nand_check_erased_ecc_chunk(p, chip->ecc.size,
+							   oob - eccpadbytes,
+							   eccpadbytes,
+							   NULL, 0,
+							   chip->ecc.strength);
+		}
+
+		if (stat < 0) {
+			mtd->ecc_stats.failed++;
+		} else {
+			mtd->ecc_stats.corrected += stat;
+			max_bitflips = max_t(unsigned int, max_bitflips, stat);
+		}
 	}
 
 	/* Calculate remaining oob bytes */
@@ -1655,7 +1694,7 @@ static uint8_t *nand_transfer_oob(struct nand_chip *chip, uint8_t *oob,
  */
 static int nand_setup_read_retry(struct mtd_info *mtd, int retry_mode)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("setting READ RETRY mode %d\n", retry_mode);
 
@@ -1680,7 +1719,7 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
 	int chipnr, page, realpage, col, bytes, aligned, oob_required;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret = 0;
 	uint32_t readlen = ops->len;
 	uint32_t oobreadlen = ops->ooblen;
@@ -2024,7 +2063,7 @@ static int nand_do_read_oob(struct mtd_info *mtd, loff_t from,
 			    struct mtd_oob_ops *ops)
 {
 	int page, realpage, chipnr;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_ecc_stats stats;
 	int readlen = ops->ooblen;
 	int len;
@@ -2472,7 +2511,7 @@ static int nand_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 static uint8_t *nand_fill_oob(struct mtd_info *mtd, uint8_t *oob, size_t len,
 			      struct mtd_oob_ops *ops)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	/*
 	 * Initialise to all 0xFF, to avoid the possibility of left over OOB
@@ -2532,7 +2571,7 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 			     struct mtd_oob_ops *ops)
 {
 	int chipnr, realpage, page, blockmask, column;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint32_t writelen = ops->len;
 
 	uint32_t oobwritelen = ops->ooblen;
@@ -2662,7 +2701,7 @@ err_out:
 static int panic_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 			    size_t *retlen, const uint8_t *buf)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct mtd_oob_ops ops;
 	int ret;
 
@@ -2722,7 +2761,7 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 			     struct mtd_oob_ops *ops)
 {
 	int chipnr, page, status, len;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	pr_debug("%s: to = 0x%08x, len = %i\n",
 			 __func__, (unsigned int)to, (int)ops->ooblen);
@@ -2847,7 +2886,7 @@ out:
  */
 static int single_erase(struct mtd_info *mtd, int page)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	/* Send commands to erase a block */
 	chip->cmdfunc(mtd, NAND_CMD_ERASE1, -1, page);
 	chip->cmdfunc(mtd, NAND_CMD_ERASE2, -1, -1);
@@ -2879,7 +2918,7 @@ int nand_erase_nand(struct mtd_info *mtd, struct erase_info *instr,
 		    int allowbbt)
 {
 	int page, status, pages_per_block, ret, chipnr;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	loff_t len;
 
 	pr_debug("%s: start = 0x%012llx, len = %llu\n",
@@ -3094,7 +3133,7 @@ static int nand_suspend(struct mtd_info *mtd)
  */
 static void nand_resume(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (chip->state == FL_PM_SUSPENDED)
 		nand_release_device(mtd);
@@ -3266,7 +3305,7 @@ ext_out:
 
 static int nand_setup_read_retry_micron(struct mtd_info *mtd, int retry_mode)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	uint8_t feature[ONFI_SUBFEATURE_PARAM_LEN] = {retry_mode};
 
 	return chip->onfi_set_features(mtd, chip, ONFI_FEATURE_ADDR_READ_RETRY,
@@ -3937,11 +3976,14 @@ ident_done:
 	return type;
 }
 
-static int nand_dt_init(struct mtd_info *mtd, struct nand_chip *chip,
-			struct device_node *dn)
+static int nand_dt_init(struct nand_chip *chip)
 {
+	struct device_node *dn = nand_get_flash_node(chip);
 	int ecc_mode, ecc_strength, ecc_step;
 
+	if (!dn)
+		return 0;
+
 	if (of_get_nand_bus_width(dn) == 16)
 		chip->options |= NAND_BUSWIDTH_16;
 
@@ -3985,15 +4027,16 @@ int nand_scan_ident(struct mtd_info *mtd, int maxchips,
 		    struct nand_flash_dev *table)
 {
 	int i, nand_maf_id, nand_dev_id;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_flash_dev *type;
 	int ret;
 
-	if (chip->flash_node) {
-		ret = nand_dt_init(mtd, chip, chip->flash_node);
-		if (ret)
-			return ret;
-	}
+	ret = nand_dt_init(chip);
+	if (ret)
+		return ret;
+
+	if (!mtd->name && mtd->dev.parent)
+		mtd->name = dev_name(mtd->dev.parent);
 
 	/* Set the default functions */
 	nand_set_defaults(chip, chip->options & NAND_BUSWIDTH_16);
@@ -4053,7 +4096,7 @@ EXPORT_SYMBOL(nand_scan_ident);
  */
 static bool nand_ecc_strength_good(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	int corr, ds_corr;
 
@@ -4082,7 +4125,7 @@ static bool nand_ecc_strength_good(struct mtd_info *mtd)
 int nand_scan_tail(struct mtd_info *mtd)
 {
 	int i;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &chip->ecc;
 	struct nand_buffers *nbuf;
 
@@ -4166,7 +4209,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 			ecc->write_oob = nand_write_oob_std;
 		if (!ecc->read_subpage)
 			ecc->read_subpage = nand_read_subpage;
-		if (!ecc->write_subpage)
+		if (!ecc->write_subpage && ecc->hwctl && ecc->calculate)
 			ecc->write_subpage = nand_write_subpage_hwecc;
 
 	case NAND_ECC_HW_SYNDROME:
@@ -4426,7 +4469,7 @@ EXPORT_SYMBOL(nand_scan);
  */
 void nand_release(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (chip->ecc.mode == NAND_ECC_SOFT_BCH)
 		nand_bch_free((struct nand_bch_control *)chip->ecc.priv);
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index b1d4f813aedc..4b6a7085b442 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -172,7 +172,7 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
 		struct nand_bbt_descr *td, int offs)
 {
 	int res, ret = 0, i, j, act = 0;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	size_t retlen, len, totlen;
 	loff_t from;
 	int bits = td->options & NAND_BBT_NRBITS_MSK;
@@ -263,7 +263,7 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
  */
 static int read_abs_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *td, int chip)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int res = 0, i;
 
 	if (td->options & NAND_BBT_PERCHIP) {
@@ -388,7 +388,7 @@ static u32 bbt_get_ver_offs(struct mtd_info *mtd, struct nand_bbt_descr *td)
 static void read_abs_bbts(struct mtd_info *mtd, uint8_t *buf,
 			  struct nand_bbt_descr *td, struct nand_bbt_descr *md)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	/* Read the primary version, if available */
 	if (td->options & NAND_BBT_VERSION) {
@@ -454,7 +454,7 @@ static int scan_block_fast(struct mtd_info *mtd, struct nand_bbt_descr *bd,
 static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
 	struct nand_bbt_descr *bd, int chip)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, numblocks, numpages;
 	int startblock;
 	loff_t from;
@@ -523,7 +523,7 @@ static int create_bbt(struct mtd_info *mtd, uint8_t *buf,
  */
 static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *td)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, chips;
 	int startblock, block, dir;
 	int scanlen = mtd->writesize + mtd->oobsize;
@@ -618,7 +618,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 		     struct nand_bbt_descr *td, struct nand_bbt_descr *md,
 		     int chipsel)
 {
-	struct nand_chip *this = mtd->priv;
+	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;
@@ -819,7 +819,7 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
  */
 static inline int nand_memory_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 
 	return create_bbt(mtd, this->buffers->databuf, bd, -1);
 }
@@ -838,7 +838,7 @@ static inline int nand_memory_bbt(struct mtd_info *mtd, struct nand_bbt_descr *b
 static int check_create(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr *bd)
 {
 	int i, chips, writeops, create, chipsel, res, res2;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	struct nand_bbt_descr *td = this->bbt_td;
 	struct nand_bbt_descr *md = this->bbt_md;
 	struct nand_bbt_descr *rd, *rd2;
@@ -962,7 +962,7 @@ static int check_create(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_desc
  */
 static void mark_bbt_region(struct mtd_info *mtd, struct nand_bbt_descr *td)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int i, j, chips, block, nrblocks, update;
 	uint8_t oldval;
 
@@ -1022,7 +1022,7 @@ static void mark_bbt_region(struct mtd_info *mtd, struct nand_bbt_descr *td)
  */
 static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	u32 pattern_len;
 	u32 bits;
 	u32 table_size;
@@ -1074,7 +1074,7 @@ static void verify_bbt_descr(struct mtd_info *mtd, struct nand_bbt_descr *bd)
  */
 static int nand_scan_bbt(struct mtd_info *mtd, struct nand_bbt_descr *bd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int len, res;
 	uint8_t *buf;
 	struct nand_bbt_descr *td = this->bbt_td;
@@ -1147,7 +1147,7 @@ err:
  */
 static int nand_update_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int len, res = 0;
 	int chip, chipsel;
 	uint8_t *buf;
@@ -1281,7 +1281,7 @@ static int nand_create_badblock_pattern(struct nand_chip *this)
  */
 int nand_default_bbt(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int ret;
 
 	/* Is a flash based bad block table requested? */
@@ -1317,7 +1317,7 @@ int nand_default_bbt(struct mtd_info *mtd)
  */
 int nand_isreserved_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block;
 
 	block = (int)(offs >> this->bbt_erase_shift);
@@ -1332,7 +1332,7 @@ int nand_isreserved_bbt(struct mtd_info *mtd, loff_t offs)
  */
 int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block, res;
 
 	block = (int)(offs >> this->bbt_erase_shift);
@@ -1359,7 +1359,7 @@ int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
  */
 int nand_markbad_bbt(struct mtd_info *mtd, loff_t offs)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	int block, ret = 0;
 
 	block = (int)(offs >> this->bbt_erase_shift);
diff --git a/drivers/mtd/nand/nand_bch.c b/drivers/mtd/nand/nand_bch.c
index 3803e0bba23b..a87c1b628dfc 100644
--- a/drivers/mtd/nand/nand_bch.c
+++ b/drivers/mtd/nand/nand_bch.c
@@ -52,7 +52,7 @@ struct nand_bch_control {
 int nand_bch_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
 			   unsigned char *code)
 {
-	const struct nand_chip *chip = mtd->priv;
+	const struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_bch_control *nbc = chip->ecc.priv;
 	unsigned int i;
 
@@ -79,7 +79,7 @@ EXPORT_SYMBOL(nand_bch_calculate_ecc);
 int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
 			  unsigned char *read_ecc, unsigned char *calc_ecc)
 {
-	const struct nand_chip *chip = mtd->priv;
+	const struct nand_chip *chip = mtd_to_nand(mtd);
 	struct nand_bch_control *nbc = chip->ecc.priv;
 	unsigned int *errloc = nbc->errloc;
 	int i, count;
@@ -98,7 +98,7 @@ int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
 		}
 	} else if (count < 0) {
 		printk(KERN_ERR "ecc unrecoverable error\n");
-		count = -1;
+		count = -EBADMSG;
 	}
 	return count;
 }
diff --git a/drivers/mtd/nand/nand_ecc.c b/drivers/mtd/nand/nand_ecc.c
index 97c4c0216c90..d1770b066396 100644
--- a/drivers/mtd/nand/nand_ecc.c
+++ b/drivers/mtd/nand/nand_ecc.c
@@ -424,7 +424,7 @@ int nand_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
 		       unsigned char *code)
 {
 	__nand_calculate_ecc(buf,
-			((struct nand_chip *)mtd->priv)->ecc.size, code);
+			mtd_to_nand(mtd)->ecc.size, code);
 
 	return 0;
 }
@@ -507,7 +507,7 @@ int __nand_correct_data(unsigned char *buf,
 		return 1;	/* error in ECC data; no action needed */
 
 	pr_err("%s: uncorrectable ECC error\n", __func__);
-	return -1;
+	return -EBADMSG;
 }
 EXPORT_SYMBOL(__nand_correct_data);
 
@@ -524,7 +524,7 @@ int nand_correct_data(struct mtd_info *mtd, unsigned char *buf,
 		      unsigned char *read_ecc, unsigned char *calc_ecc)
 {
 	return __nand_correct_data(buf, read_ecc, calc_ecc,
-				   ((struct nand_chip *)mtd->priv)->ecc.size);
+				   mtd_to_nand(mtd)->ecc.size);
 }
 EXPORT_SYMBOL(nand_correct_data);
 
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index b16d70aafd9e..1fd519503bb1 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -666,8 +666,8 @@ static char *get_partition_name(int i)
  */
 static int init_nandsim(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct nandsim   *ns   = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim   *ns   = nand_get_controller_data(chip);
 	int i, ret = 0;
 	uint64_t remains;
 	uint64_t next_offset;
@@ -1908,7 +1908,8 @@ static void switch_state(struct nandsim *ns)
 
 static u_char ns_nand_read_byte(struct mtd_info *mtd)
 {
-	struct nandsim *ns = ((struct nand_chip *)mtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 	u_char outb = 0x00;
 
 	/* Sanity and correctness checks */
@@ -1969,7 +1970,8 @@ static u_char ns_nand_read_byte(struct mtd_info *mtd)
 
 static void ns_nand_write_byte(struct mtd_info *mtd, u_char byte)
 {
-	struct nandsim *ns = ((struct nand_chip *)mtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 
 	/* Sanity and correctness checks */
 	if (!ns->lines.ce) {
@@ -2123,7 +2125,8 @@ static void ns_nand_write_byte(struct mtd_info *mtd, u_char byte)
 
 static void ns_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int bitmask)
 {
-	struct nandsim *ns = ((struct nand_chip *)mtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 
 	ns->lines.cle = bitmask & NAND_CLE ? 1 : 0;
 	ns->lines.ale = bitmask & NAND_ALE ? 1 : 0;
@@ -2141,7 +2144,7 @@ static int ns_device_ready(struct mtd_info *mtd)
 
 static uint16_t ns_nand_read_word(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = (struct nand_chip *)mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	NS_DBG("read_word\n");
 
@@ -2150,7 +2153,8 @@ static uint16_t ns_nand_read_word(struct mtd_info *mtd)
 
 static void ns_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nandsim *ns = ((struct nand_chip *)mtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 
 	/* Check that chip is expecting data input */
 	if (!(ns->state & STATE_DATAIN_MASK)) {
@@ -2177,7 +2181,8 @@ static void ns_nand_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 
 static void ns_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nandsim *ns = ((struct nand_chip *)mtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 
 	/* Sanity and correctness checks */
 	if (!ns->lines.ce) {
@@ -2198,7 +2203,7 @@ static void ns_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 		int i;
 
 		for (i = 0; i < len; i++)
-			buf[i] = ((struct nand_chip *)mtd->priv)->read_byte(mtd);
+			buf[i] = mtd_to_nand(mtd)->read_byte(mtd);
 
 		return;
 	}
@@ -2236,16 +2241,15 @@ static int __init ns_init_module(void)
 	}
 
 	/* Allocate and initialize mtd_info, nand_chip and nandsim structures */
-	nsmtd = kzalloc(sizeof(struct mtd_info) + sizeof(struct nand_chip)
-				+ sizeof(struct nandsim), GFP_KERNEL);
-	if (!nsmtd) {
+	chip = kzalloc(sizeof(struct nand_chip) + sizeof(struct nandsim),
+		       GFP_KERNEL);
+	if (!chip) {
 		NS_ERR("unable to allocate core structures.\n");
 		return -ENOMEM;
 	}
-	chip        = (struct nand_chip *)(nsmtd + 1);
-        nsmtd->priv = (void *)chip;
+	nsmtd       = nand_to_mtd(chip);
 	nand        = (struct nandsim *)(chip + 1);
-	chip->priv  = (void *)nand;
+	nand_set_controller_data(chip, (void *)nand);
 
 	/*
 	 * Register simulator's callbacks.
@@ -2392,7 +2396,7 @@ err_exit:
 	for (i = 0;i < ARRAY_SIZE(nand->partitions); ++i)
 		kfree(nand->partitions[i].name);
 error:
-	kfree(nsmtd);
+	kfree(chip);
 	free_lists();
 
 	return retval;
@@ -2405,7 +2409,8 @@ module_init(ns_init_module);
  */
 static void __exit ns_cleanup_module(void)
 {
-	struct nandsim *ns = ((struct nand_chip *)nsmtd->priv)->priv;
+	struct nand_chip *chip = mtd_to_nand(nsmtd);
+	struct nandsim *ns = nand_get_controller_data(chip);
 	int i;
 
 	nandsim_debugfs_remove(ns);
@@ -2413,7 +2418,7 @@ static void __exit ns_cleanup_module(void)
 	nand_release(nsmtd); /* Unregister driver */
 	for (i = 0;i < ARRAY_SIZE(ns->partitions); ++i)
 		kfree(ns->partitions[i].name);
-	kfree(nsmtd);        /* Free other structures */
+	kfree(mtd_to_nand(nsmtd));        /* Free other structures */
 	free_lists();
 }
 
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 4f0d62f9d22c..218c789ca7ab 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -37,7 +37,6 @@
 struct ndfc_controller {
 	struct platform_device *ofdev;
 	void __iomem *ndfcbase;
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	int chip_select;
 	struct nand_hw_control ndfc_control;
@@ -48,8 +47,8 @@ static struct ndfc_controller ndfc_ctrl[NDFC_MAX_CS];
 static void ndfc_select_chip(struct mtd_info *mtd, int chip)
 {
 	uint32_t ccr;
-	struct nand_chip *nchip = mtd->priv;
-	struct ndfc_controller *ndfc = nchip->priv;
+	struct nand_chip *nchip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(nchip);
 
 	ccr = in_be32(ndfc->ndfcbase + NDFC_CCR);
 	if (chip >= 0) {
@@ -62,8 +61,8 @@ static void ndfc_select_chip(struct mtd_info *mtd, int chip)
 
 static void ndfc_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 
 	if (cmd == NAND_CMD_NONE)
 		return;
@@ -76,8 +75,8 @@ static void ndfc_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 
 static int ndfc_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 
 	return in_be32(ndfc->ndfcbase + NDFC_STAT) & NDFC_STAT_IS_READY;
 }
@@ -85,8 +84,8 @@ static int ndfc_ready(struct mtd_info *mtd)
 static void ndfc_enable_hwecc(struct mtd_info *mtd, int mode)
 {
 	uint32_t ccr;
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 
 	ccr = in_be32(ndfc->ndfcbase + NDFC_CCR);
 	ccr |= NDFC_CCR_RESET_ECC;
@@ -97,8 +96,8 @@ static void ndfc_enable_hwecc(struct mtd_info *mtd, int mode)
 static int ndfc_calculate_ecc(struct mtd_info *mtd,
 			      const u_char *dat, u_char *ecc_code)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 	uint32_t ecc;
 	uint8_t *p = (uint8_t *)&ecc;
 
@@ -121,8 +120,8 @@ static int ndfc_calculate_ecc(struct mtd_info *mtd,
  */
 static void ndfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 	uint32_t *p = (uint32_t *) buf;
 
 	for(;len > 0; len -= 4)
@@ -131,8 +130,8 @@ static void ndfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 
 static void ndfc_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct ndfc_controller *ndfc = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct ndfc_controller *ndfc = nand_get_controller_data(chip);
 	uint32_t *p = (uint32_t *) buf;
 
 	for(;len > 0; len -= 4)
@@ -147,7 +146,7 @@ static int ndfc_chip_init(struct ndfc_controller *ndfc,
 {
 	struct device_node *flash_np;
 	struct nand_chip *chip = &ndfc->chip;
-	struct mtd_part_parser_data ppdata;
+	struct mtd_info *mtd = nand_to_mtd(chip);
 	int ret;
 
 	chip->IO_ADDR_R = ndfc->ndfcbase + NDFC_DATA;
@@ -166,33 +165,32 @@ static int ndfc_chip_init(struct ndfc_controller *ndfc,
 	chip->ecc.size = 256;
 	chip->ecc.bytes = 3;
 	chip->ecc.strength = 1;
-	chip->priv = ndfc;
+	nand_set_controller_data(chip, ndfc);
 
-	ndfc->mtd.priv = chip;
-	ndfc->mtd.dev.parent = &ndfc->ofdev->dev;
+	mtd->dev.parent = &ndfc->ofdev->dev;
 
 	flash_np = of_get_next_child(node, NULL);
 	if (!flash_np)
 		return -ENODEV;
+	nand_set_flash_node(chip, flash_np);
 
-	ppdata.of_node = flash_np;
-	ndfc->mtd.name = kasprintf(GFP_KERNEL, "%s.%s",
-			dev_name(&ndfc->ofdev->dev), flash_np->name);
-	if (!ndfc->mtd.name) {
+	mtd->name = kasprintf(GFP_KERNEL, "%s.%s", dev_name(&ndfc->ofdev->dev),
+			      flash_np->name);
+	if (!mtd->name) {
 		ret = -ENOMEM;
 		goto err;
 	}
 
-	ret = nand_scan(&ndfc->mtd, 1);
+	ret = nand_scan(mtd, 1);
 	if (ret)
 		goto err;
 
-	ret = mtd_device_parse_register(&ndfc->mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 
 err:
 	of_node_put(flash_np);
 	if (ret)
-		kfree(ndfc->mtd.name);
+		kfree(mtd->name);
 	return ret;
 }
 
@@ -259,9 +257,10 @@ static int ndfc_probe(struct platform_device *ofdev)
 static int ndfc_remove(struct platform_device *ofdev)
 {
 	struct ndfc_controller *ndfc = dev_get_drvdata(&ofdev->dev);
+	struct mtd_info *mtd = nand_to_mtd(&ndfc->chip);
 
-	nand_release(&ndfc->mtd);
-	kfree(ndfc->mtd.name);
+	nand_release(mtd);
+	kfree(mtd->name);
 
 	return 0;
 }
diff --git a/drivers/mtd/nand/nuc900_nand.c b/drivers/mtd/nand/nuc900_nand.c
index f0687f71fbd8..220ddfcf29f5 100644
--- a/drivers/mtd/nand/nuc900_nand.c
+++ b/drivers/mtd/nand/nuc900_nand.c
@@ -55,13 +55,17 @@
 	__raw_writel((val), (dev)->reg + REG_SMADDR)
 
 struct nuc900_nand {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	void __iomem *reg;
 	struct clk *clk;
 	spinlock_t lock;
 };
 
+static inline struct nuc900_nand *mtd_to_nuc900(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct nuc900_nand, chip);
+}
+
 static const struct mtd_partition partitions[] = {
 	{
 	 .name = "NAND FS 0",
@@ -78,9 +82,7 @@ static const struct mtd_partition partitions[] = {
 static unsigned char nuc900_nand_read_byte(struct mtd_info *mtd)
 {
 	unsigned char ret;
-	struct nuc900_nand *nand;
-
-	nand = container_of(mtd, struct nuc900_nand, mtd);
+	struct nuc900_nand *nand = mtd_to_nuc900(mtd);
 
 	ret = (unsigned char)read_data_reg(nand);
 
@@ -91,9 +93,7 @@ static void nuc900_nand_read_buf(struct mtd_info *mtd,
 				 unsigned char *buf, int len)
 {
 	int i;
-	struct nuc900_nand *nand;
-
-	nand = container_of(mtd, struct nuc900_nand, mtd);
+	struct nuc900_nand *nand = mtd_to_nuc900(mtd);
 
 	for (i = 0; i < len; i++)
 		buf[i] = (unsigned char)read_data_reg(nand);
@@ -103,9 +103,7 @@ static void nuc900_nand_write_buf(struct mtd_info *mtd,
 				  const unsigned char *buf, int len)
 {
 	int i;
-	struct nuc900_nand *nand;
-
-	nand = container_of(mtd, struct nuc900_nand, mtd);
+	struct nuc900_nand *nand = mtd_to_nuc900(mtd);
 
 	for (i = 0; i < len; i++)
 		write_data_reg(nand, buf[i]);
@@ -124,11 +122,9 @@ static int nuc900_check_rb(struct nuc900_nand *nand)
 
 static int nuc900_nand_devready(struct mtd_info *mtd)
 {
-	struct nuc900_nand *nand;
+	struct nuc900_nand *nand = mtd_to_nuc900(mtd);
 	int ready;
 
-	nand = container_of(mtd, struct nuc900_nand, mtd);
-
 	ready = (nuc900_check_rb(nand)) ? 1 : 0;
 	return ready;
 }
@@ -136,10 +132,8 @@ static int nuc900_nand_devready(struct mtd_info *mtd)
 static void nuc900_nand_command_lp(struct mtd_info *mtd, unsigned int command,
 				   int column, int page_addr)
 {
-	register struct nand_chip *chip = mtd->priv;
-	struct nuc900_nand *nand;
-
-	nand = container_of(mtd, struct nuc900_nand, mtd);
+	register struct nand_chip *chip = mtd_to_nand(mtd);
+	struct nuc900_nand *nand = mtd_to_nuc900(mtd);
 
 	if (command == NAND_CMD_READOOB) {
 		column += mtd->writesize;
@@ -241,6 +235,7 @@ static int nuc900_nand_probe(struct platform_device *pdev)
 {
 	struct nuc900_nand *nuc900_nand;
 	struct nand_chip *chip;
+	struct mtd_info *mtd;
 	struct resource *res;
 
 	nuc900_nand = devm_kzalloc(&pdev->dev, sizeof(struct nuc900_nand),
@@ -248,9 +243,9 @@ static int nuc900_nand_probe(struct platform_device *pdev)
 	if (!nuc900_nand)
 		return -ENOMEM;
 	chip = &(nuc900_nand->chip);
+	mtd = nand_to_mtd(chip);
 
-	nuc900_nand->mtd.priv	= chip;
-	nuc900_nand->mtd.dev.parent = &pdev->dev;
+	mtd->dev.parent		= &pdev->dev;
 	spin_lock_init(&nuc900_nand->lock);
 
 	nuc900_nand->clk = devm_clk_get(&pdev->dev, NULL);
@@ -274,11 +269,10 @@ static int nuc900_nand_probe(struct platform_device *pdev)
 
 	nuc900_nand_enable(nuc900_nand);
 
-	if (nand_scan(&(nuc900_nand->mtd), 1))
+	if (nand_scan(mtd, 1))
 		return -ENXIO;
 
-	mtd_device_register(&(nuc900_nand->mtd), partitions,
-			    ARRAY_SIZE(partitions));
+	mtd_device_register(mtd, partitions, ARRAY_SIZE(partitions));
 
 	platform_set_drvdata(pdev, nuc900_nand);
 
@@ -289,7 +283,7 @@ static int nuc900_nand_remove(struct platform_device *pdev)
 {
 	struct nuc900_nand *nuc900_nand = platform_get_drvdata(pdev);
 
-	nand_release(&nuc900_nand->mtd);
+	nand_release(nand_to_mtd(&nuc900_nand->chip));
 	clk_disable(nuc900_nand->clk);
 
 	return 0;
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index 93f664cd1c90..c553f78ab83f 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -152,7 +152,6 @@ static struct nand_hw_control omap_gpmc_controller = {
 
 struct omap_nand_info {
 	struct omap_nand_platform_data	*pdata;
-	struct mtd_info			mtd;
 	struct nand_chip		nand;
 	struct platform_device		*pdev;
 
@@ -177,6 +176,11 @@ struct omap_nand_info {
 	struct device_node		*of_node;
 };
 
+static inline struct omap_nand_info *mtd_to_omap(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct omap_nand_info, nand);
+}
+
 /**
  * omap_prefetch_enable - configures and starts prefetch transfer
  * @cs: cs (chip select) number
@@ -247,8 +251,7 @@ static int omap_prefetch_reset(int cs, struct omap_nand_info *info)
  */
 static void omap_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct omap_nand_info *info = container_of(mtd,
-					struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 
 	if (cmd != NAND_CMD_NONE) {
 		if (ctrl & NAND_CLE)
@@ -270,7 +273,7 @@ static void omap_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
  */
 static void omap_read_buf8(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 
 	ioread8_rep(nand->IO_ADDR_R, buf, len);
 }
@@ -283,8 +286,7 @@ static void omap_read_buf8(struct mtd_info *mtd, u_char *buf, int len)
  */
 static void omap_write_buf8(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	u_char *p = (u_char *)buf;
 	u32	status = 0;
 
@@ -306,7 +308,7 @@ static void omap_write_buf8(struct mtd_info *mtd, const u_char *buf, int len)
  */
 static void omap_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 
 	ioread16_rep(nand->IO_ADDR_R, buf, len / 2);
 }
@@ -319,8 +321,7 @@ static void omap_read_buf16(struct mtd_info *mtd, u_char *buf, int len)
  */
 static void omap_write_buf16(struct mtd_info *mtd, const u_char * buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	u16 *p = (u16 *) buf;
 	u32	status = 0;
 	/* FIXME try bursts of writesw() or DMA ... */
@@ -344,8 +345,7 @@ static void omap_write_buf16(struct mtd_info *mtd, const u_char * buf, int len)
  */
 static void omap_read_buf_pref(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	uint32_t r_count = 0;
 	int ret = 0;
 	u32 *p = (u32 *)buf;
@@ -392,8 +392,7 @@ static void omap_read_buf_pref(struct mtd_info *mtd, u_char *buf, int len)
 static void omap_write_buf_pref(struct mtd_info *mtd,
 					const u_char *buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	uint32_t w_count = 0;
 	int i = 0, ret = 0;
 	u16 *p = (u16 *)buf;
@@ -458,8 +457,7 @@ static void omap_nand_dma_callback(void *data)
 static inline int omap_nand_dma_transfer(struct mtd_info *mtd, void *addr,
 					unsigned int len, int is_write)
 {
-	struct omap_nand_info *info = container_of(mtd,
-					struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	struct dma_async_tx_descriptor *tx;
 	enum dma_data_direction dir = is_write ? DMA_TO_DEVICE :
 							DMA_FROM_DEVICE;
@@ -623,8 +621,7 @@ done:
  */
 static void omap_read_buf_irq_pref(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	int ret = 0;
 
 	if (len <= mtd->oobsize) {
@@ -671,8 +668,7 @@ out_copy:
 static void omap_write_buf_irq_pref(struct mtd_info *mtd,
 					const u_char *buf, int len)
 {
-	struct omap_nand_info *info = container_of(mtd,
-						struct omap_nand_info, mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	int ret = 0;
 	unsigned long tim, limit;
 	u32 val;
@@ -830,12 +826,12 @@ static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
 	case 1:
 		/* Uncorrectable error */
 		pr_debug("ECC UNCORRECTED_ERROR 1\n");
-		return -1;
+		return -EBADMSG;
 
 	case 11:
 		/* UN-Correctable error */
 		pr_debug("ECC UNCORRECTED_ERROR B\n");
-		return -1;
+		return -EBADMSG;
 
 	case 12:
 		/* Correctable error */
@@ -865,7 +861,7 @@ static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
 				return 0;
 		}
 		pr_debug("UNCORRECTED_ERROR default\n");
-		return -1;
+		return -EBADMSG;
 	}
 }
 
@@ -886,8 +882,7 @@ static int omap_compare_ecc(u8 *ecc_data1,	/* read from NAND memory */
 static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
 				u_char *read_ecc, u_char *calc_ecc)
 {
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	int blockCnt = 0, i = 0, ret = 0;
 	int stat = 0;
 
@@ -928,8 +923,7 @@ static int omap_correct_data(struct mtd_info *mtd, u_char *dat,
 static int omap_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 				u_char *ecc_code)
 {
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	u32 val;
 
 	val = readl(info->reg.gpmc_ecc_config);
@@ -953,9 +947,8 @@ static int omap_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
  */
 static void omap_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct omap_nand_info *info = mtd_to_omap(mtd);
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	unsigned int dev_width = (chip->options & NAND_BUSWIDTH_16) ? 1 : 0;
 	u32 val;
 
@@ -1001,9 +994,8 @@ static void omap_enable_hwecc(struct mtd_info *mtd, int mode)
  */
 static int omap_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct nand_chip *this = mtd->priv;
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	unsigned long timeo = jiffies;
 	int status, state = this->state;
 
@@ -1031,8 +1023,7 @@ static int omap_wait(struct mtd_info *mtd, struct nand_chip *chip)
 static int omap_dev_ready(struct mtd_info *mtd)
 {
 	unsigned int val = 0;
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 
 	val = readl(info->reg.gpmc_status);
 
@@ -1058,10 +1049,9 @@ static void __maybe_unused omap_enable_hwecc_bch(struct mtd_info *mtd, int mode)
 {
 	unsigned int bch_type;
 	unsigned int dev_width, nsectors;
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-						   mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	enum omap_ecc ecc_opt = info->ecc_opt;
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	u32 val, wr_mode;
 	unsigned int ecc_size1, ecc_size0;
 
@@ -1162,8 +1152,7 @@ static u8  bch8_polynomial[] = {0xef, 0x51, 0x2e, 0x09, 0xed, 0x93, 0x9a, 0xc2,
 static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 					const u_char *dat, u_char *ecc_calc)
 {
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-						   mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	int eccbytes	= info->nand.ecc.bytes;
 	struct gpmc_nand_regs	*gpmc_regs = &info->reg;
 	u8 *ecc_code;
@@ -1334,8 +1323,7 @@ static int erased_sector_bitflips(u_char *data, u_char *oob,
 static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 				u_char *read_ecc, u_char *calc_ecc)
 {
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-			mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	struct nand_ecc_ctrl *ecc = &info->nand.ecc;
 	int eccsteps = info->nand.ecc.steps;
 	int i , j, stat = 0;
@@ -1663,7 +1651,6 @@ static int omap_nand_probe(struct platform_device *pdev)
 	unsigned			sig;
 	unsigned			oob_index;
 	struct resource			*res;
-	struct mtd_part_parser_data	ppdata = {};
 
 	pdata = dev_get_platdata(&pdev->dev);
 	if (pdata == NULL) {
@@ -1683,11 +1670,11 @@ static int omap_nand_probe(struct platform_device *pdev)
 	info->reg		= pdata->reg;
 	info->of_node		= pdata->of_node;
 	info->ecc_opt		= pdata->ecc_opt;
-	mtd			= &info->mtd;
-	mtd->priv		= &info->nand;
-	mtd->dev.parent		= &pdev->dev;
 	nand_chip		= &info->nand;
+	mtd			= nand_to_mtd(nand_chip);
+	mtd->dev.parent		= &pdev->dev;
 	nand_chip->ecc.priv	= NULL;
+	nand_set_flash_node(nand_chip, pdata->of_node);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	nand_chip->IO_ADDR_R = devm_ioremap_resource(&pdev->dev, res);
@@ -1909,7 +1896,7 @@ static int omap_nand_probe(struct platform_device *pdev)
 				ecclayout->eccpos[ecclayout->eccbytes - 1] + 1;
 
 		err = elm_config(info->elm_dev, BCH4_ECC,
-				 info->mtd.writesize / nand_chip->ecc.size,
+				 mtd->writesize / nand_chip->ecc.size,
 				 nand_chip->ecc.size, nand_chip->ecc.bytes);
 		if (err < 0)
 			goto return_error;
@@ -1963,7 +1950,7 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.write_page	= omap_write_page_bch;
 
 		err = elm_config(info->elm_dev, BCH8_ECC,
-				 info->mtd.writesize / nand_chip->ecc.size,
+				 mtd->writesize / nand_chip->ecc.size,
 				 nand_chip->ecc.size, nand_chip->ecc.bytes);
 		if (err < 0)
 			goto return_error;
@@ -1993,7 +1980,7 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.write_page	= omap_write_page_bch;
 
 		err = elm_config(info->elm_dev, BCH16_ECC,
-				 info->mtd.writesize / nand_chip->ecc.size,
+				 mtd->writesize / nand_chip->ecc.size,
 				 nand_chip->ecc.size, nand_chip->ecc.bytes);
 		if (err < 0)
 			goto return_error;
@@ -2037,9 +2024,7 @@ scan_tail:
 		goto return_error;
 	}
 
-	ppdata.of_node = pdata->of_node;
-	mtd_device_parse_register(mtd, NULL, &ppdata, pdata->parts,
-				  pdata->nr_parts);
+	mtd_device_register(mtd, pdata->parts, pdata->nr_parts);
 
 	platform_set_drvdata(pdev, mtd);
 
@@ -2058,9 +2043,8 @@ return_error:
 static int omap_nand_remove(struct platform_device *pdev)
 {
 	struct mtd_info *mtd = platform_get_drvdata(pdev);
-	struct nand_chip *nand_chip = mtd->priv;
-	struct omap_nand_info *info = container_of(mtd, struct omap_nand_info,
-							mtd);
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct omap_nand_info *info = mtd_to_omap(mtd);
 	if (nand_chip->ecc.priv) {
 		nand_bch_free(nand_chip->ecc.priv);
 		nand_chip->ecc.priv = NULL;
diff --git a/drivers/mtd/nand/omap_elm.c b/drivers/mtd/nand/omap_elm.c
index 235ec7992b4c..a3f32f939cc1 100644
--- a/drivers/mtd/nand/omap_elm.c
+++ b/drivers/mtd/nand/omap_elm.c
@@ -414,7 +414,7 @@ static int elm_probe(struct platform_device *pdev)
 	ret = devm_request_irq(&pdev->dev, irq->start, elm_isr, 0,
 			pdev->name, info);
 	if (ret) {
-		dev_err(&pdev->dev, "failure requesting irq %i\n", irq->start);
+		dev_err(&pdev->dev, "failure requesting %pr\n", irq);
 		return ret;
 	}
 
diff --git a/drivers/mtd/nand/orion_nand.c b/drivers/mtd/nand/orion_nand.c
index ee83749fb1d3..d4614bfbfed6 100644
--- a/drivers/mtd/nand/orion_nand.c
+++ b/drivers/mtd/nand/orion_nand.c
@@ -25,8 +25,8 @@
 
 static void orion_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *nc = mtd->priv;
-	struct orion_nand_data *board = nc->priv;
+	struct nand_chip *nc = mtd_to_nand(mtd);
+	struct orion_nand_data *board = nand_get_controller_data(nc);
 	u32 offs;
 
 	if (cmd == NAND_CMD_NONE)
@@ -47,7 +47,7 @@ static void orion_nand_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl
 
 static void orion_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	void __iomem *io_base = chip->IO_ADDR_R;
 	uint64_t *buf64;
 	int i = 0;
@@ -76,7 +76,6 @@ static void orion_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static int __init orion_nand_probe(struct platform_device *pdev)
 {
 	struct mtd_info *mtd;
-	struct mtd_part_parser_data ppdata = {};
 	struct nand_chip *nc;
 	struct orion_nand_data *board;
 	struct resource *res;
@@ -86,11 +85,11 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 	u32 val = 0;
 
 	nc = devm_kzalloc(&pdev->dev,
-			sizeof(struct nand_chip) + sizeof(struct mtd_info),
+			sizeof(struct nand_chip),
 			GFP_KERNEL);
 	if (!nc)
 		return -ENOMEM;
-	mtd = (struct mtd_info *)(nc + 1);
+	mtd = nand_to_mtd(nc);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	io_base = devm_ioremap_resource(&pdev->dev, res);
@@ -123,10 +122,10 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 		board = dev_get_platdata(&pdev->dev);
 	}
 
-	mtd->priv = nc;
 	mtd->dev.parent = &pdev->dev;
 
-	nc->priv = board;
+	nand_set_controller_data(nc, board);
+	nand_set_flash_node(nc, pdev->dev.of_node);
 	nc->IO_ADDR_R = nc->IO_ADDR_W = io_base;
 	nc->cmd_ctrl = orion_nand_cmd_ctrl;
 	nc->read_buf = orion_nand_read_buf;
@@ -161,9 +160,7 @@ static int __init orion_nand_probe(struct platform_device *pdev)
 	}
 
 	mtd->name = "orion_nand";
-	ppdata.of_node = pdev->dev.of_node;
-	ret = mtd_device_parse_register(mtd, NULL, &ppdata,
-			board->parts, board->nr_parts);
+	ret = mtd_device_register(mtd, board->parts, board->nr_parts);
 	if (ret) {
 		nand_release(mtd);
 		goto no_dev;
diff --git a/drivers/mtd/nand/pasemi_nand.c b/drivers/mtd/nand/pasemi_nand.c
index 83cf021b9651..3ab53ca53cca 100644
--- a/drivers/mtd/nand/pasemi_nand.c
+++ b/drivers/mtd/nand/pasemi_nand.c
@@ -45,7 +45,7 @@ static const char driver_name[] = "pasemi-nand";
 
 static void pasemi_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	while (len > 0x800) {
 		memcpy_fromio(buf, chip->IO_ADDR_R, 0x800);
@@ -57,7 +57,7 @@ static void pasemi_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 
 static void pasemi_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	while (len > 0x800) {
 		memcpy_toio(chip->IO_ADDR_R, buf, 0x800);
@@ -70,7 +70,7 @@ static void pasemi_write_buf(struct mtd_info *mtd, const u_char *buf, int len)
 static void pasemi_hwcontrol(struct mtd_info *mtd, int cmd,
 			     unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (cmd == NAND_CMD_NONE)
 		return;
@@ -110,20 +110,17 @@ static int pasemi_nand_probe(struct platform_device *ofdev)
 	pr_debug("pasemi_nand at %pR\n", &res);
 
 	/* Allocate memory for MTD device structure and private data */
-	pasemi_nand_mtd = kzalloc(sizeof(struct mtd_info) +
-				  sizeof(struct nand_chip), GFP_KERNEL);
-	if (!pasemi_nand_mtd) {
+	chip = kzalloc(sizeof(struct nand_chip), GFP_KERNEL);
+	if (!chip) {
 		printk(KERN_WARNING
 		       "Unable to allocate PASEMI NAND MTD device structure\n");
 		err = -ENOMEM;
 		goto out;
 	}
 
-	/* Get pointer to private data */
-	chip = (struct nand_chip *)&pasemi_nand_mtd[1];
+	pasemi_nand_mtd = nand_to_mtd(chip);
 
 	/* Link the private data with the MTD structure */
-	pasemi_nand_mtd->priv = chip;
 	pasemi_nand_mtd->dev.parent = &ofdev->dev;
 
 	chip->IO_ADDR_R = of_iomap(np, 0);
@@ -180,7 +177,7 @@ static int pasemi_nand_probe(struct platform_device *ofdev)
  out_ior:
 	iounmap(chip->IO_ADDR_R);
  out_mtd:
-	kfree(pasemi_nand_mtd);
+	kfree(chip);
  out:
 	return err;
 }
@@ -192,7 +189,7 @@ static int pasemi_nand_remove(struct platform_device *ofdev)
 	if (!pasemi_nand_mtd)
 		return 0;
 
-	chip = pasemi_nand_mtd->priv;
+	chip = mtd_to_nand(pasemi_nand_mtd);
 
 	/* Release resources, unregister device */
 	nand_release(pasemi_nand_mtd);
@@ -202,7 +199,7 @@ static int pasemi_nand_remove(struct platform_device *ofdev)
 	iounmap(chip->IO_ADDR_R);
 
 	/* Free the MTD device structure */
-	kfree(pasemi_nand_mtd);
+	kfree(chip);
 
 	pasemi_nand_mtd = NULL;
 
diff --git a/drivers/mtd/nand/plat_nand.c b/drivers/mtd/nand/plat_nand.c
index 65b9dbbe6d6a..a0e26dea1424 100644
--- a/drivers/mtd/nand/plat_nand.c
+++ b/drivers/mtd/nand/plat_nand.c
@@ -20,7 +20,6 @@
 
 struct plat_nand_data {
 	struct nand_chip	chip;
-	struct mtd_info		mtd;
 	void __iomem		*io_base;
 };
 
@@ -30,8 +29,8 @@ struct plat_nand_data {
 static int plat_nand_probe(struct platform_device *pdev)
 {
 	struct platform_nand_data *pdata = dev_get_platdata(&pdev->dev);
-	struct mtd_part_parser_data ppdata;
 	struct plat_nand_data *data;
+	struct mtd_info *mtd;
 	struct resource *res;
 	const char **part_types;
 	int err = 0;
@@ -57,9 +56,9 @@ static int plat_nand_probe(struct platform_device *pdev)
 	if (IS_ERR(data->io_base))
 		return PTR_ERR(data->io_base);
 
-	data->chip.priv = &data;
-	data->mtd.priv = &data->chip;
-	data->mtd.dev.parent = &pdev->dev;
+	nand_set_flash_node(&data->chip, pdev->dev.of_node);
+	mtd = nand_to_mtd(&data->chip);
+	mtd->dev.parent = &pdev->dev;
 
 	data->chip.IO_ADDR_R = data->io_base;
 	data->chip.IO_ADDR_W = data->io_base;
@@ -87,22 +86,21 @@ static int plat_nand_probe(struct platform_device *pdev)
 	}
 
 	/* Scan to find existence of the device */
-	if (nand_scan(&data->mtd, pdata->chip.nr_chips)) {
+	if (nand_scan(mtd, pdata->chip.nr_chips)) {
 		err = -ENXIO;
 		goto out;
 	}
 
 	part_types = pdata->chip.part_probe_types;
 
-	ppdata.of_node = pdev->dev.of_node;
-	err = mtd_device_parse_register(&data->mtd, part_types, &ppdata,
+	err = mtd_device_parse_register(mtd, part_types, NULL,
 					pdata->chip.partitions,
 					pdata->chip.nr_partitions);
 
 	if (!err)
 		return err;
 
-	nand_release(&data->mtd);
+	nand_release(mtd);
 out:
 	if (pdata->ctrl.remove)
 		pdata->ctrl.remove(pdev);
@@ -117,7 +115,7 @@ static int plat_nand_remove(struct platform_device *pdev)
 	struct plat_nand_data *data = platform_get_drvdata(pdev);
 	struct platform_nand_data *pdata = dev_get_platdata(&pdev->dev);
 
-	nand_release(&data->mtd);
+	nand_release(nand_to_mtd(&data->chip));
 	if (pdata->ctrl.remove)
 		pdata->ctrl.remove(pdev);
 
diff --git a/drivers/mtd/nand/pxa3xx_nand.c b/drivers/mtd/nand/pxa3xx_nand.c
index e453ae9a17fa..86fc245dc71a 100644
--- a/drivers/mtd/nand/pxa3xx_nand.c
+++ b/drivers/mtd/nand/pxa3xx_nand.c
@@ -30,11 +30,6 @@
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/of_mtd.h>
-
-#if defined(CONFIG_ARM) && (defined(CONFIG_ARCH_PXA) || defined(CONFIG_ARCH_MMP))
-#define ARCH_HAS_DMA
-#endif
-
 #include <linux/platform_data/mtd-nand-pxa3xx.h>
 
 #define	CHIP_DELAY_TIMEOUT	msecs_to_jiffies(200)
@@ -172,7 +167,6 @@ enum pxa3xx_nand_variant {
 
 struct pxa3xx_nand_host {
 	struct nand_chip	chip;
-	struct mtd_info         *mtd;
 	void			*info_data;
 
 	/* page size of attached chip */
@@ -455,14 +449,15 @@ static int pxa3xx_nand_init_timings_compat(struct pxa3xx_nand_host *host,
 	struct nand_chip *chip = &host->chip;
 	struct pxa3xx_nand_info *info = host->info_data;
 	const struct pxa3xx_nand_flash *f = NULL;
+	struct mtd_info *mtd = nand_to_mtd(&host->chip);
 	int i, id, ntypes;
 
 	ntypes = ARRAY_SIZE(builtin_flash_types);
 
-	chip->cmdfunc(host->mtd, NAND_CMD_READID, 0x00, -1);
+	chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
 
-	id = chip->read_byte(host->mtd);
-	id |= chip->read_byte(host->mtd) << 0x8;
+	id = chip->read_byte(mtd);
+	id |= chip->read_byte(mtd) << 0x8;
 
 	for (i = 0; i < ntypes; i++) {
 		f = &builtin_flash_types[i];
@@ -895,7 +890,7 @@ static void set_command_address(struct pxa3xx_nand_info *info,
 static void prepare_start_command(struct pxa3xx_nand_info *info, int command)
 {
 	struct pxa3xx_nand_host *host = info->host[info->cs];
-	struct mtd_info *mtd = host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->chip);
 
 	/* reset data and oob column point to handle data */
 	info->buf_start		= 0;
@@ -948,7 +943,7 @@ static int prepare_set_command(struct pxa3xx_nand_info *info, int command,
 	struct mtd_info *mtd;
 
 	host = info->host[info->cs];
-	mtd = host->mtd;
+	mtd = nand_to_mtd(&host->chip);
 	addr_cycle = 0;
 	exec_cmd = 1;
 
@@ -1118,7 +1113,8 @@ static int prepare_set_command(struct pxa3xx_nand_info *info, int command,
 static void nand_cmdfunc(struct mtd_info *mtd, unsigned command,
 			 int column, int page_addr)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int exec_cmd;
 
@@ -1166,7 +1162,8 @@ static void nand_cmdfunc_extended(struct mtd_info *mtd,
 				  const unsigned command,
 				  int column, int page_addr)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int exec_cmd, ext_cmd_type;
 
@@ -1286,7 +1283,7 @@ static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
 		struct nand_chip *chip, uint8_t *buf, int oob_required,
 		int page)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 
 	chip->read_buf(mtd, buf, mtd->writesize);
@@ -1312,7 +1309,8 @@ static int pxa3xx_nand_read_page_hwecc(struct mtd_info *mtd,
 
 static uint8_t pxa3xx_nand_read_byte(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	char retval = 0xFF;
 
@@ -1325,7 +1323,8 @@ static uint8_t pxa3xx_nand_read_byte(struct mtd_info *mtd)
 
 static u16 pxa3xx_nand_read_word(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	u16 retval = 0xFFFF;
 
@@ -1338,7 +1337,8 @@ static u16 pxa3xx_nand_read_word(struct mtd_info *mtd)
 
 static void pxa3xx_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int real_len = min_t(size_t, len, info->buf_count - info->buf_start);
 
@@ -1349,7 +1349,8 @@ static void pxa3xx_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void pxa3xx_nand_write_buf(struct mtd_info *mtd,
 		const uint8_t *buf, int len)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	int real_len = min_t(size_t, len, info->buf_count - info->buf_start);
 
@@ -1364,7 +1365,8 @@ static void pxa3xx_nand_select_chip(struct mtd_info *mtd, int chip)
 
 static int pxa3xx_nand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 
 	if (info->need_wait) {
@@ -1387,37 +1389,53 @@ static int pxa3xx_nand_waitfunc(struct mtd_info *mtd, struct nand_chip *this)
 	return NAND_STATUS_READY;
 }
 
-static int pxa3xx_nand_config_flash(struct pxa3xx_nand_info *info)
+static int pxa3xx_nand_config_ident(struct pxa3xx_nand_info *info)
 {
+	struct pxa3xx_nand_host *host = info->host[info->cs];
 	struct platform_device *pdev = info->pdev;
 	struct pxa3xx_nand_platform_data *pdata = dev_get_platdata(&pdev->dev);
-	struct pxa3xx_nand_host *host = info->host[info->cs];
-	struct mtd_info *mtd = host->mtd;
-	struct nand_chip *chip = mtd->priv;
+	const struct nand_sdr_timings *timings;
 
-	/* configure default flash values */
+	/* Configure default flash values */
+	info->chunk_size = PAGE_CHUNK_SIZE;
 	info->reg_ndcr = 0x0; /* enable all interrupts */
 	info->reg_ndcr |= (pdata->enable_arbiter) ? NDCR_ND_ARB_EN : 0;
 	info->reg_ndcr |= NDCR_RD_ID_CNT(READ_ID_BYTES);
-	info->reg_ndcr |= NDCR_SPARE_EN; /* enable spare by default */
+	info->reg_ndcr |= NDCR_SPARE_EN;
+
+	/* use the common timing to make a try */
+	timings = onfi_async_timing_mode_to_sdr_timings(0);
+	if (IS_ERR(timings))
+		return PTR_ERR(timings);
+
+	pxa3xx_nand_set_sdr_timing(host, timings);
+	return 0;
+}
+
+static void pxa3xx_nand_config_tail(struct pxa3xx_nand_info *info)
+{
+	struct pxa3xx_nand_host *host = info->host[info->cs];
+	struct nand_chip *chip = &host->chip;
+	struct mtd_info *mtd = nand_to_mtd(chip);
+
 	info->reg_ndcr |= (host->col_addr_cycles == 2) ? NDCR_RA_START : 0;
 	info->reg_ndcr |= (chip->page_shift == 6) ? NDCR_PG_PER_BLK : 0;
 	info->reg_ndcr |= (mtd->writesize == 2048) ? NDCR_PAGE_SZ : 0;
-
-	return 0;
 }
 
-static int pxa3xx_nand_detect_config(struct pxa3xx_nand_info *info)
+static void pxa3xx_nand_detect_config(struct pxa3xx_nand_info *info)
 {
+	struct platform_device *pdev = info->pdev;
+	struct pxa3xx_nand_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	uint32_t ndcr = nand_readl(info, NDCR);
 
 	/* Set an initial chunk size */
 	info->chunk_size = ndcr & NDCR_PAGE_SZ ? 2048 : 512;
 	info->reg_ndcr = ndcr &
 		~(NDCR_INT_MASK | NDCR_ND_ARB_EN | NFCV1_NDCR_ARB_CNTL);
+	info->reg_ndcr |= (pdata->enable_arbiter) ? NDCR_ND_ARB_EN : 0;
 	info->ndtr0cs0 = nand_readl(info, NDTR0CS0);
 	info->ndtr1cs0 = nand_readl(info, NDTR1CS0);
-	return 0;
 }
 
 static int pxa3xx_nand_init_buff(struct pxa3xx_nand_info *info)
@@ -1483,32 +1501,6 @@ static void pxa3xx_nand_free_buff(struct pxa3xx_nand_info *info)
 	kfree(info->data_buff);
 }
 
-static int pxa3xx_nand_sensing(struct pxa3xx_nand_host *host)
-{
-	struct pxa3xx_nand_info *info = host->info_data;
-	struct mtd_info *mtd;
-	struct nand_chip *chip;
-	const struct nand_sdr_timings *timings;
-	int ret;
-
-	mtd = info->host[info->cs]->mtd;
-	chip = mtd->priv;
-
-	/* use the common timing to make a try */
-	timings = onfi_async_timing_mode_to_sdr_timings(0);
-	if (IS_ERR(timings))
-		return PTR_ERR(timings);
-
-	pxa3xx_nand_set_sdr_timing(host, timings);
-
-	chip->cmdfunc(mtd, NAND_CMD_RESET, 0, 0);
-	ret = chip->waitfunc(mtd, chip);
-	if (ret & NAND_STATUS_FAIL)
-		return -ENODEV;
-
-	return 0;
-}
-
 static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 			struct nand_ecc_ctrl *ecc,
 			int strength, int ecc_stepsize, int page_size)
@@ -1580,34 +1572,22 @@ static int pxa_ecc_init(struct pxa3xx_nand_info *info,
 
 static int pxa3xx_nand_scan(struct mtd_info *mtd)
 {
-	struct pxa3xx_nand_host *host = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct pxa3xx_nand_host *host = nand_get_controller_data(chip);
 	struct pxa3xx_nand_info *info = host->info_data;
 	struct platform_device *pdev = info->pdev;
 	struct pxa3xx_nand_platform_data *pdata = dev_get_platdata(&pdev->dev);
-	struct nand_chip *chip = mtd->priv;
 	int ret;
 	uint16_t ecc_strength, ecc_step;
 
-	if (pdata->keep_config && !pxa3xx_nand_detect_config(info))
-		goto KEEP_CONFIG;
-
-	/* Set a default chunk size */
-	info->chunk_size = 512;
-
-	ret = pxa3xx_nand_config_flash(info);
-	if (ret)
-		return ret;
-
-	ret = pxa3xx_nand_sensing(host);
-	if (ret) {
-		dev_info(&info->pdev->dev, "There is no chip on cs %d!\n",
-			 info->cs);
-
-		return ret;
+	if (pdata->keep_config) {
+		pxa3xx_nand_detect_config(info);
+	} else {
+		ret = pxa3xx_nand_config_ident(info);
+		if (ret)
+			return ret;
 	}
 
-KEEP_CONFIG:
-	info->reg_ndcr |= (pdata->enable_arbiter) ? NDCR_ND_ARB_EN : 0;
 	if (info->reg_ndcr & NDCR_DWIDTH_M)
 		chip->options |= NAND_BUSWIDTH_16;
 
@@ -1692,11 +1672,16 @@ KEEP_CONFIG:
 		host->row_addr_cycles = 3;
 	else
 		host->row_addr_cycles = 2;
+
+	if (!pdata->keep_config)
+		pxa3xx_nand_config_tail(info);
+
 	return nand_scan_tail(mtd);
 }
 
 static int alloc_nand_resource(struct platform_device *pdev)
 {
+	struct device_node *np = pdev->dev.of_node;
 	struct pxa3xx_nand_platform_data *pdata;
 	struct pxa3xx_nand_info *info;
 	struct pxa3xx_nand_host *host;
@@ -1708,24 +1693,27 @@ static int alloc_nand_resource(struct platform_device *pdev)
 	pdata = dev_get_platdata(&pdev->dev);
 	if (pdata->num_cs <= 0)
 		return -ENODEV;
-	info = devm_kzalloc(&pdev->dev, sizeof(*info) + (sizeof(*mtd) +
-			    sizeof(*host)) * pdata->num_cs, GFP_KERNEL);
+	info = devm_kzalloc(&pdev->dev,
+			    sizeof(*info) + sizeof(*host) * pdata->num_cs,
+			    GFP_KERNEL);
 	if (!info)
 		return -ENOMEM;
 
 	info->pdev = pdev;
 	info->variant = pxa3xx_nand_get_variant(pdev);
 	for (cs = 0; cs < pdata->num_cs; cs++) {
-		mtd = (void *)&info[1] + (sizeof(*mtd) + sizeof(*host)) * cs;
-		chip = (struct nand_chip *)(&mtd[1]);
-		host = (struct pxa3xx_nand_host *)chip;
+		host = (void *)&info[1] + sizeof(*host) * cs;
+		chip = &host->chip;
+		nand_set_controller_data(chip, host);
+		mtd = nand_to_mtd(chip);
 		info->host[cs] = host;
-		host->mtd = mtd;
 		host->cs = cs;
 		host->info_data = info;
-		mtd->priv = host;
 		mtd->dev.parent = &pdev->dev;
+		/* FIXME: all chips use the same device tree partitions */
+		nand_set_flash_node(chip, np);
 
+		nand_set_controller_data(chip, host);
 		chip->ecc.read_page	= pxa3xx_nand_read_page_hwecc;
 		chip->ecc.write_page	= pxa3xx_nand_write_page_hwecc;
 		chip->controller        = &info->controller;
@@ -1845,7 +1833,7 @@ static int pxa3xx_nand_remove(struct platform_device *pdev)
 	clk_disable_unprepare(info->clk);
 
 	for (cs = 0; cs < pdata->num_cs; cs++)
-		nand_release(info->host[cs]->mtd);
+		nand_release(nand_to_mtd(&info->host[cs]->chip));
 	return 0;
 }
 
@@ -1886,7 +1874,6 @@ static int pxa3xx_nand_probe_dt(struct platform_device *pdev)
 static int pxa3xx_nand_probe(struct platform_device *pdev)
 {
 	struct pxa3xx_nand_platform_data *pdata;
-	struct mtd_part_parser_data ppdata = {};
 	struct pxa3xx_nand_info *info;
 	int ret, cs, probe_success, dma_available;
 
@@ -1917,7 +1904,7 @@ static int pxa3xx_nand_probe(struct platform_device *pdev)
 	info = platform_get_drvdata(pdev);
 	probe_success = 0;
 	for (cs = 0; cs < pdata->num_cs; cs++) {
-		struct mtd_info *mtd = info->host[cs]->mtd;
+		struct mtd_info *mtd = nand_to_mtd(&info->host[cs]->chip);
 
 		/*
 		 * The mtd name matches the one used in 'mtdparts' kernel
@@ -1933,10 +1920,8 @@ static int pxa3xx_nand_probe(struct platform_device *pdev)
 			continue;
 		}
 
-		ppdata.of_node = pdev->dev.of_node;
-		ret = mtd_device_parse_register(mtd, NULL,
-						&ppdata, pdata->parts[cs],
-						pdata->nr_parts[cs]);
+		ret = mtd_device_register(mtd, pdata->parts[cs],
+					  pdata->nr_parts[cs]);
 		if (!ret)
 			probe_success = 1;
 	}
@@ -1959,12 +1944,18 @@ static int pxa3xx_nand_suspend(struct device *dev)
 		return -EAGAIN;
 	}
 
+	clk_disable(info->clk);
 	return 0;
 }
 
 static int pxa3xx_nand_resume(struct device *dev)
 {
 	struct pxa3xx_nand_info *info = dev_get_drvdata(dev);
+	int ret;
+
+	ret = clk_enable(info->clk);
+	if (ret < 0)
+		return ret;
 
 	/* We don't want to handle interrupt without calling mtd routine */
 	disable_int(info, NDCR_INT_MASK);
diff --git a/drivers/mtd/nand/r852.c b/drivers/mtd/nand/r852.c
index d8bb2be327f1..fc9287af4614 100644
--- a/drivers/mtd/nand/r852.c
+++ b/drivers/mtd/nand/r852.c
@@ -64,8 +64,8 @@ static inline void r852_write_reg_dword(struct r852_device *dev,
 /* returns pointer to our private structure */
 static inline struct r852_device *r852_get_dev(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	return chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	return nand_get_controller_data(chip);
 }
 
 
@@ -361,7 +361,7 @@ static void r852_cmdctl(struct mtd_info *mtd, int dat, unsigned int ctrl)
  */
 static int r852_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct r852_device *dev = chip->priv;
+	struct r852_device *dev = nand_get_controller_data(chip);
 
 	unsigned long timeout;
 	int status;
@@ -477,7 +477,7 @@ static int r852_ecc_correct(struct mtd_info *mtd, uint8_t *dat,
 
 	if (dev->dma_error) {
 		dev->dma_error = 0;
-		return -1;
+		return -EIO;
 	}
 
 	r852_write_reg(dev, R852_CTL, dev->ctlreg | R852_CTL_ECC_ACCESS);
@@ -491,7 +491,7 @@ static int r852_ecc_correct(struct mtd_info *mtd, uint8_t *dat,
 		/* ecc uncorrectable error */
 		if (ecc_status & R852_ECC_FAIL) {
 			dbg("ecc: unrecoverable error, in half %d", i);
-			error = -1;
+			error = -EBADMSG;
 			goto exit;
 		}
 
@@ -634,25 +634,21 @@ static void r852_update_media_status(struct r852_device *dev)
  */
 static int r852_register_nand_device(struct r852_device *dev)
 {
-	dev->mtd = kzalloc(sizeof(struct mtd_info), GFP_KERNEL);
-
-	if (!dev->mtd)
-		goto error1;
+	struct mtd_info *mtd = nand_to_mtd(dev->chip);
 
 	WARN_ON(dev->card_registred);
 
-	dev->mtd->priv = dev->chip;
-	dev->mtd->dev.parent = &dev->pci_dev->dev;
+	mtd->dev.parent = &dev->pci_dev->dev;
 
 	if (dev->readonly)
 		dev->chip->options |= NAND_ROM;
 
 	r852_engine_enable(dev);
 
-	if (sm_register_device(dev->mtd, dev->sm))
-		goto error2;
+	if (sm_register_device(mtd, dev->sm))
+		goto error1;
 
-	if (device_create_file(&dev->mtd->dev, &dev_attr_media_type)) {
+	if (device_create_file(&mtd->dev, &dev_attr_media_type)) {
 		message("can't create media type sysfs attribute");
 		goto error3;
 	}
@@ -660,9 +656,7 @@ static int r852_register_nand_device(struct r852_device *dev)
 	dev->card_registred = 1;
 	return 0;
 error3:
-	nand_release(dev->mtd);
-error2:
-	kfree(dev->mtd);
+	nand_release(mtd);
 error1:
 	/* Force card redetect */
 	dev->card_detected = 0;
@@ -675,15 +669,15 @@ error1:
 
 static void r852_unregister_nand_device(struct r852_device *dev)
 {
+	struct mtd_info *mtd = nand_to_mtd(dev->chip);
+
 	if (!dev->card_registred)
 		return;
 
-	device_remove_file(&dev->mtd->dev, &dev_attr_media_type);
-	nand_release(dev->mtd);
+	device_remove_file(&mtd->dev, &dev_attr_media_type);
+	nand_release(mtd);
 	r852_engine_disable(dev);
 	dev->card_registred = 0;
-	kfree(dev->mtd);
-	dev->mtd = NULL;
 }
 
 /* Card state updater */
@@ -885,7 +879,7 @@ static int  r852_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 	if (!dev)
 		goto error5;
 
-	chip->priv = dev;
+	nand_set_controller_data(chip, dev);
 	dev->chip = chip;
 	dev->pci_dev = pci_dev;
 	pci_set_drvdata(pci_dev, dev);
@@ -980,7 +974,6 @@ static void r852_remove(struct pci_dev *pci_dev)
 
 	/* Stop interrupts */
 	r852_disable_irqs(dev);
-	synchronize_irq(dev->irq);
 	free_irq(dev->irq, dev);
 
 	/* Cleanup */
@@ -1032,6 +1025,7 @@ static int r852_suspend(struct device *device)
 static int r852_resume(struct device *device)
 {
 	struct r852_device *dev = pci_get_drvdata(to_pci_dev(device));
+	struct mtd_info *mtd = nand_to_mtd(dev->chip);
 
 	r852_disable_irqs(dev);
 	r852_card_update_present(dev);
@@ -1051,9 +1045,9 @@ static int r852_resume(struct device *device)
 	/* Otherwise, initialize the card */
 	if (dev->card_registred) {
 		r852_engine_enable(dev);
-		dev->chip->select_chip(dev->mtd, 0);
-		dev->chip->cmdfunc(dev->mtd, NAND_CMD_RESET, -1, -1);
-		dev->chip->select_chip(dev->mtd, -1);
+		dev->chip->select_chip(mtd, 0);
+		dev->chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+		dev->chip->select_chip(mtd, -1);
 	}
 
 	/* Program card detection IRQ */
diff --git a/drivers/mtd/nand/r852.h b/drivers/mtd/nand/r852.h
index e6a21d9d22c6..d042ddb71a8b 100644
--- a/drivers/mtd/nand/r852.h
+++ b/drivers/mtd/nand/r852.h
@@ -108,7 +108,6 @@
 
 struct r852_device {
 	void __iomem *mmio;		/* mmio */
-	struct mtd_info *mtd;		/* mtd backpointer */
 	struct nand_chip *chip;		/* nand chip backpointer */
 	struct pci_dev *pci_dev;	/* pci backpointer */
 
diff --git a/drivers/mtd/nand/s3c2410.c b/drivers/mtd/nand/s3c2410.c
index 05105cadd0db..01ac74fa3b95 100644
--- a/drivers/mtd/nand/s3c2410.c
+++ b/drivers/mtd/nand/s3c2410.c
@@ -104,7 +104,6 @@ struct s3c2410_nand_info;
  * @scan_res: The result from calling nand_scan_ident().
 */
 struct s3c2410_nand_mtd {
-	struct mtd_info			mtd;
 	struct nand_chip		chip;
 	struct s3c2410_nand_set		*set;
 	struct s3c2410_nand_info	*info;
@@ -168,7 +167,8 @@ struct s3c2410_nand_info {
 
 static struct s3c2410_nand_mtd *s3c2410_nand_mtd_toours(struct mtd_info *mtd)
 {
-	return container_of(mtd, struct s3c2410_nand_mtd, mtd);
+	return container_of(mtd_to_nand(mtd), struct s3c2410_nand_mtd,
+			    chip);
 }
 
 static struct s3c2410_nand_info *s3c2410_nand_mtd_toinfo(struct mtd_info *mtd)
@@ -382,10 +382,10 @@ static void s3c2410_nand_select_chip(struct mtd_info *mtd, int chip)
 {
 	struct s3c2410_nand_info *info;
 	struct s3c2410_nand_mtd *nmtd;
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	unsigned long cur;
 
-	nmtd = this->priv;
+	nmtd = nand_get_controller_data(this);
 	info = nmtd->info;
 
 	if (chip != -1)
@@ -634,7 +634,7 @@ static int s3c2440_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 
 static void s3c2410_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	readsb(this->IO_ADDR_R, buf, len);
 }
 
@@ -656,7 +656,7 @@ static void s3c2440_nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 static void s3c2410_nand_write_buf(struct mtd_info *mtd, const u_char *buf,
 				   int len)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	writesb(this->IO_ADDR_W, buf, len);
 }
 
@@ -745,7 +745,7 @@ static int s3c24xx_nand_remove(struct platform_device *pdev)
 
 		for (mtdno = 0; mtdno < info->mtd_count; mtdno++, ptr++) {
 			pr_debug("releasing mtd %d (%p)\n", mtdno, ptr);
-			nand_release(&ptr->mtd);
+			nand_release(nand_to_mtd(&ptr->chip));
 		}
 	}
 
@@ -762,9 +762,11 @@ static int s3c2410_nand_add_partition(struct s3c2410_nand_info *info,
 				      struct s3c2410_nand_set *set)
 {
 	if (set) {
-		mtd->mtd.name = set->name;
+		struct mtd_info *mtdinfo = nand_to_mtd(&mtd->chip);
 
-		return mtd_device_parse_register(&mtd->mtd, NULL, NULL,
+		mtdinfo->name = set->name;
+
+		return mtd_device_parse_register(mtdinfo, NULL, NULL,
 					 set->partitions, set->nr_partitions);
 	}
 
@@ -792,7 +794,7 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 	chip->read_buf     = s3c2410_nand_read_buf;
 	chip->select_chip  = s3c2410_nand_select_chip;
 	chip->chip_delay   = 50;
-	chip->priv	   = nmtd;
+	nand_set_controller_data(chip, nmtd);
 	chip->options	   = set->options;
 	chip->controller   = &info->controller;
 
@@ -831,7 +833,6 @@ static void s3c2410_nand_init_chip(struct s3c2410_nand_info *info,
 	chip->IO_ADDR_R = chip->IO_ADDR_W;
 
 	nmtd->info	   = info;
-	nmtd->mtd.priv	   = chip;
 	nmtd->set	   = set;
 
 #ifdef CONFIG_MTD_NAND_S3C2410_HWECC
@@ -1012,19 +1013,21 @@ static int s3c24xx_nand_probe(struct platform_device *pdev)
 	nmtd = info->mtds;
 
 	for (setno = 0; setno < nr_sets; setno++, nmtd++) {
+		struct mtd_info *mtd = nand_to_mtd(&nmtd->chip);
+
 		pr_debug("initialising set %d (%p, info %p)\n",
 			 setno, nmtd, info);
 
-		nmtd->mtd.dev.parent = &pdev->dev;
+		mtd->dev.parent = &pdev->dev;
 		s3c2410_nand_init_chip(info, nmtd, sets);
 
-		nmtd->scan_res = nand_scan_ident(&nmtd->mtd,
+		nmtd->scan_res = nand_scan_ident(mtd,
 						 (sets) ? sets->nr_chips : 1,
 						 NULL);
 
 		if (nmtd->scan_res == 0) {
 			s3c2410_nand_update_chip(info, nmtd);
-			nand_scan_tail(&nmtd->mtd);
+			nand_scan_tail(mtd);
 			s3c2410_nand_add_partition(info, nmtd, sets);
 		}
 
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index bcba1a924c75..4814402902f9 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -160,7 +160,7 @@ static void flctl_setup_dma(struct sh_flctl *flctl)
 
 	memset(&cfg, 0, sizeof(cfg));
 	cfg.direction = DMA_MEM_TO_DEV;
-	cfg.dst_addr = (dma_addr_t)FLDTFIFO(flctl);
+	cfg.dst_addr = flctl->fifo;
 	cfg.src_addr = 0;
 	ret = dmaengine_slave_config(flctl->chan_fifo0_tx, &cfg);
 	if (ret < 0)
@@ -176,7 +176,7 @@ static void flctl_setup_dma(struct sh_flctl *flctl)
 
 	cfg.direction = DMA_DEV_TO_MEM;
 	cfg.dst_addr = 0;
-	cfg.src_addr = (dma_addr_t)FLDTFIFO(flctl);
+	cfg.src_addr = flctl->fifo;
 	ret = dmaengine_slave_config(flctl->chan_fifo0_rx, &cfg);
 	if (ret < 0)
 		goto err;
@@ -607,13 +607,13 @@ static void execmd_read_page_sector(struct mtd_info *mtd, int page_addr)
 		case FL_REPAIRABLE:
 			dev_info(&flctl->pdev->dev,
 				"applied ecc on page 0x%x", page_addr);
-			flctl->mtd.ecc_stats.corrected++;
+			mtd->ecc_stats.corrected++;
 			break;
 		case FL_ERROR:
 			dev_warn(&flctl->pdev->dev,
 				"page 0x%x contains corrupted data\n",
 				page_addr);
-			flctl->mtd.ecc_stats.failed++;
+			mtd->ecc_stats.failed++;
 			break;
 		default:
 			;
@@ -1086,7 +1086,6 @@ static int flctl_probe(struct platform_device *pdev)
 	struct sh_flctl_platform_data *pdata;
 	int ret;
 	int irq;
-	struct mtd_part_parser_data ppdata = {};
 
 	flctl = devm_kzalloc(&pdev->dev, sizeof(struct sh_flctl), GFP_KERNEL);
 	if (!flctl)
@@ -1096,6 +1095,7 @@ static int flctl_probe(struct platform_device *pdev)
 	flctl->reg = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(flctl->reg))
 		return PTR_ERR(flctl->reg);
+	flctl->fifo = res->start + 0x24; /* FLDTFIFO */
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -1121,9 +1121,9 @@ static int flctl_probe(struct platform_device *pdev)
 	}
 
 	platform_set_drvdata(pdev, flctl);
-	flctl_mtd = &flctl->mtd;
 	nand = &flctl->chip;
-	flctl_mtd->priv = nand;
+	flctl_mtd = nand_to_mtd(nand);
+	nand_set_flash_node(nand, pdev->dev.of_node);
 	flctl_mtd->dev.parent = &pdev->dev;
 	flctl->pdev = pdev;
 	flctl->hwecc = pdata->has_hwecc;
@@ -1163,9 +1163,7 @@ static int flctl_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_chip;
 
-	ppdata.of_node = pdev->dev.of_node;
-	ret = mtd_device_parse_register(flctl_mtd, NULL, &ppdata, pdata->parts,
-			pdata->nr_parts);
+	ret = mtd_device_register(flctl_mtd, pdata->parts, pdata->nr_parts);
 
 	return 0;
 
@@ -1180,7 +1178,7 @@ static int flctl_remove(struct platform_device *pdev)
 	struct sh_flctl *flctl = platform_get_drvdata(pdev);
 
 	flctl_release_dma(flctl);
-	nand_release(&flctl->mtd);
+	nand_release(nand_to_mtd(&flctl->chip));
 	pm_runtime_disable(&pdev->dev);
 
 	return 0;
diff --git a/drivers/mtd/nand/sharpsl.c b/drivers/mtd/nand/sharpsl.c
index 082b6009736d..b7d1b55a160b 100644
--- a/drivers/mtd/nand/sharpsl.c
+++ b/drivers/mtd/nand/sharpsl.c
@@ -29,13 +29,15 @@
 #include <asm/mach-types.h>
 
 struct sharpsl_nand {
-	struct mtd_info		mtd;
 	struct nand_chip	chip;
 
 	void __iomem		*io;
 };
 
-#define mtd_to_sharpsl(_mtd)	container_of(_mtd, struct sharpsl_nand, mtd)
+static inline struct sharpsl_nand *mtd_to_sharpsl(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct sharpsl_nand, chip);
+}
 
 /* register offset */
 #define ECCLPLB		0x00	/* line parity 7 - 0 bit */
@@ -66,7 +68,7 @@ static void sharpsl_nand_hwcontrol(struct mtd_info *mtd, int cmd,
 				   unsigned int ctrl)
 {
 	struct sharpsl_nand *sharpsl = mtd_to_sharpsl(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		unsigned char bits = ctrl & 0x07;
@@ -109,6 +111,7 @@ static int sharpsl_nand_calculate_ecc(struct mtd_info *mtd, const u_char * dat,
 static int sharpsl_nand_probe(struct platform_device *pdev)
 {
 	struct nand_chip *this;
+	struct mtd_info *mtd;
 	struct resource *r;
 	int err = 0;
 	struct sharpsl_nand *sharpsl;
@@ -143,8 +146,8 @@ static int sharpsl_nand_probe(struct platform_device *pdev)
 	this = (struct nand_chip *)(&sharpsl->chip);
 
 	/* Link the private data with the MTD structure */
-	sharpsl->mtd.priv = this;
-	sharpsl->mtd.dev.parent = &pdev->dev;
+	mtd = nand_to_mtd(this);
+	mtd->dev.parent = &pdev->dev;
 
 	platform_set_drvdata(pdev, sharpsl);
 
@@ -173,14 +176,14 @@ static int sharpsl_nand_probe(struct platform_device *pdev)
 	this->ecc.correct = nand_correct_data;
 
 	/* Scan to find existence of the device */
-	err = nand_scan(&sharpsl->mtd, 1);
+	err = nand_scan(mtd, 1);
 	if (err)
 		goto err_scan;
 
 	/* Register the partitions */
-	sharpsl->mtd.name = "sharpsl-nand";
+	mtd->name = "sharpsl-nand";
 
-	err = mtd_device_parse_register(&sharpsl->mtd, NULL, NULL,
+	err = mtd_device_parse_register(mtd, NULL, NULL,
 					data->partitions, data->nr_partitions);
 	if (err)
 		goto err_add;
@@ -189,7 +192,7 @@ static int sharpsl_nand_probe(struct platform_device *pdev)
 	return 0;
 
 err_add:
-	nand_release(&sharpsl->mtd);
+	nand_release(mtd);
 
 err_scan:
 	iounmap(sharpsl->io);
@@ -207,7 +210,7 @@ static int sharpsl_nand_remove(struct platform_device *pdev)
 	struct sharpsl_nand *sharpsl = platform_get_drvdata(pdev);
 
 	/* Release resources, unregister device */
-	nand_release(&sharpsl->mtd);
+	nand_release(nand_to_mtd(&sharpsl->chip));
 
 	iounmap(sharpsl->io);
 
diff --git a/drivers/mtd/nand/sm_common.c b/drivers/mtd/nand/sm_common.c
index e06b5e5d3287..c514740f9a83 100644
--- a/drivers/mtd/nand/sm_common.c
+++ b/drivers/mtd/nand/sm_common.c
@@ -102,7 +102,7 @@ static struct nand_flash_dev nand_xd_flash_ids[] = {
 
 int sm_register_device(struct mtd_info *mtd, int smartmedia)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret;
 
 	chip->options |= NAND_SKIP_BBTSCAN;
diff --git a/drivers/mtd/nand/socrates_nand.c b/drivers/mtd/nand/socrates_nand.c
index b94f53427f0f..e3305f9dd6fb 100644
--- a/drivers/mtd/nand/socrates_nand.c
+++ b/drivers/mtd/nand/socrates_nand.c
@@ -30,7 +30,6 @@
 
 struct socrates_nand_host {
 	struct nand_chip	nand_chip;
-	struct mtd_info		mtd;
 	void __iomem		*io_base;
 	struct device		*dev;
 };
@@ -45,8 +44,8 @@ static void socrates_nand_write_buf(struct mtd_info *mtd,
 		const uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
-	struct socrates_nand_host *host = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct socrates_nand_host *host = nand_get_controller_data(this);
 
 	for (i = 0; i < len; i++) {
 		out_be32(host->io_base, FPGA_NAND_ENABLE |
@@ -64,8 +63,8 @@ static void socrates_nand_write_buf(struct mtd_info *mtd,
 static void socrates_nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
 	int i;
-	struct nand_chip *this = mtd->priv;
-	struct socrates_nand_host *host = this->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
+	struct socrates_nand_host *host = nand_get_controller_data(this);
 	uint32_t val;
 
 	val = FPGA_NAND_ENABLE | FPGA_NAND_CMD_READ;
@@ -105,8 +104,8 @@ static uint16_t socrates_nand_read_word(struct mtd_info *mtd)
 static void socrates_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
 		unsigned int ctrl)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct socrates_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct socrates_nand_host *host = nand_get_controller_data(nand_chip);
 	uint32_t val;
 
 	if (cmd == NAND_CMD_NONE)
@@ -130,8 +129,8 @@ static void socrates_nand_cmd_ctrl(struct mtd_info *mtd, int cmd,
  */
 static int socrates_nand_device_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *nand_chip = mtd->priv;
-	struct socrates_nand_host *host = nand_chip->priv;
+	struct nand_chip *nand_chip = mtd_to_nand(mtd);
+	struct socrates_nand_host *host = nand_get_controller_data(nand_chip);
 
 	if (in_be32(host->io_base) & FPGA_NAND_BUSY)
 		return 0; /* busy */
@@ -147,7 +146,6 @@ static int socrates_nand_probe(struct platform_device *ofdev)
 	struct mtd_info *mtd;
 	struct nand_chip *nand_chip;
 	int res;
-	struct mtd_part_parser_data ppdata;
 
 	/* Allocate memory for the device structure (and zero it) */
 	host = devm_kzalloc(&ofdev->dev, sizeof(*host), GFP_KERNEL);
@@ -160,15 +158,15 @@ static int socrates_nand_probe(struct platform_device *ofdev)
 		return -EIO;
 	}
 
-	mtd = &host->mtd;
 	nand_chip = &host->nand_chip;
+	mtd = nand_to_mtd(nand_chip);
 	host->dev = &ofdev->dev;
 
-	nand_chip->priv = host;		/* link the private data structures */
-	mtd->priv = nand_chip;
+	/* link the private data structures */
+	nand_set_controller_data(nand_chip, host);
+	nand_set_flash_node(nand_chip, ofdev->dev.of_node);
 	mtd->name = "socrates_nand";
 	mtd->dev.parent = &ofdev->dev;
-	ppdata.of_node = ofdev->dev.of_node;
 
 	/*should never be accessed directly */
 	nand_chip->IO_ADDR_R = (void *)0xdeadbeef;
@@ -200,7 +198,7 @@ static int socrates_nand_probe(struct platform_device *ofdev)
 		goto out;
 	}
 
-	res = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	res = mtd_device_register(mtd, NULL, 0);
 	if (!res)
 		return res;
 
@@ -217,7 +215,7 @@ out:
 static int socrates_nand_remove(struct platform_device *ofdev)
 {
 	struct socrates_nand_host *host = dev_get_drvdata(&ofdev->dev);
-	struct mtd_info *mtd = &host->mtd;
+	struct mtd_info *mtd = nand_to_mtd(&host->nand_chip);
 
 	nand_release(mtd);
 
diff --git a/drivers/mtd/nand/sunxi_nand.c b/drivers/mtd/nand/sunxi_nand.c
index 824711845c44..51e10a35fe08 100644
--- a/drivers/mtd/nand/sunxi_nand.c
+++ b/drivers/mtd/nand/sunxi_nand.c
@@ -234,7 +234,6 @@ struct sunxi_nand_hw_ecc {
 struct sunxi_nand_chip {
 	struct list_head node;
 	struct nand_chip nand;
-	struct mtd_info mtd;
 	unsigned long clk_rate;
 	u32 timing_cfg;
 	u32 timing_ctl;
@@ -350,7 +349,7 @@ static int sunxi_nfc_rst(struct sunxi_nfc *nfc)
 
 static int sunxi_nfc_dev_ready(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	struct sunxi_nand_rb *rb;
@@ -388,7 +387,7 @@ static int sunxi_nfc_dev_ready(struct mtd_info *mtd)
 
 static void sunxi_nfc_select_chip(struct mtd_info *mtd, int chip)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	struct sunxi_nand_chip_sel *sel;
@@ -433,7 +432,7 @@ static void sunxi_nfc_select_chip(struct mtd_info *mtd, int chip)
 
 static void sunxi_nfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	int ret;
@@ -466,7 +465,7 @@ static void sunxi_nfc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void sunxi_nfc_write_buf(struct mtd_info *mtd, const uint8_t *buf,
 				int len)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	int ret;
@@ -507,7 +506,7 @@ static uint8_t sunxi_nfc_read_byte(struct mtd_info *mtd)
 static void sunxi_nfc_cmd_ctrl(struct mtd_info *mtd, int dat,
 			       unsigned int ctrl)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	int ret;
@@ -541,7 +540,7 @@ static void sunxi_nfc_cmd_ctrl(struct mtd_info *mtd, int dat,
 
 static void sunxi_nfc_hw_ecc_enable(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(nand->controller);
 	struct sunxi_nand_hw_ecc *data = nand->ecc.priv;
 	u32 ecc_ctl;
@@ -556,7 +555,7 @@ static void sunxi_nfc_hw_ecc_enable(struct mtd_info *mtd)
 
 static void sunxi_nfc_hw_ecc_disable(struct mtd_info *mtd)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(nand->controller);
 
 	writel(readl(nfc->regs + NFC_REG_ECC_CTL) & ~NFC_ECC_EN,
@@ -577,7 +576,7 @@ static int sunxi_nfc_hw_ecc_read_chunk(struct mtd_info *mtd,
 				       int *cur_off,
 				       unsigned int *max_bitflips)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(nand->controller);
 	struct nand_ecc_ctrl *ecc = &nand->ecc;
 	u32 status;
@@ -638,7 +637,7 @@ static int sunxi_nfc_hw_ecc_read_chunk(struct mtd_info *mtd,
 static void sunxi_nfc_hw_ecc_read_extra_oob(struct mtd_info *mtd,
 					    u8 *oob, int *cur_off)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &nand->ecc;
 	int offset = ((ecc->bytes + 4) * ecc->steps);
 	int len = mtd->oobsize - offset;
@@ -665,7 +664,7 @@ static int sunxi_nfc_hw_ecc_write_chunk(struct mtd_info *mtd,
 					const u8 *oob, int oob_off,
 					int *cur_off)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(nand->controller);
 	struct nand_ecc_ctrl *ecc = &nand->ecc;
 	int ret;
@@ -702,7 +701,7 @@ static int sunxi_nfc_hw_ecc_write_chunk(struct mtd_info *mtd,
 static void sunxi_nfc_hw_ecc_write_extra_oob(struct mtd_info *mtd,
 					     u8 *oob, int *cur_off)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct nand_ecc_ctrl *ecc = &nand->ecc;
 	int offset = ((ecc->bytes + 4) * ecc->steps);
 	int len = mtd->oobsize - offset;
@@ -991,6 +990,7 @@ static int sunxi_nand_chip_set_timings(struct sunxi_nand_chip *chip,
 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;
@@ -1008,12 +1008,11 @@ static int sunxi_nand_chip_init_timings(struct sunxi_nand_chip *chip,
 
 		feature[0] = mode;
 		for (i = 0; i < chip->nsels; i++) {
-			chip->nand.select_chip(&chip->mtd, i);
-			ret = chip->nand.onfi_set_features(&chip->mtd,
-						&chip->nand,
+			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(&chip->mtd, -1);
+			chip->nand.select_chip(mtd, -1);
 			if (ret)
 				return ret;
 		}
@@ -1031,7 +1030,7 @@ static int sunxi_nand_hw_common_ecc_ctrl_init(struct mtd_info *mtd,
 					      struct device_node *np)
 {
 	static const u8 strengths[] = { 16, 24, 28, 32, 40, 48, 56, 60, 64 };
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	struct sunxi_nand_chip *sunxi_nand = to_sunxi_nand(nand);
 	struct sunxi_nfc *nfc = to_sunxi_nfc(sunxi_nand->nand.controller);
 	struct sunxi_nand_hw_ecc *data;
@@ -1189,7 +1188,7 @@ static void sunxi_nand_ecc_cleanup(struct nand_ecc_ctrl *ecc)
 static int sunxi_nand_ecc_init(struct mtd_info *mtd, struct nand_ecc_ctrl *ecc,
 			       struct device_node *np)
 {
-	struct nand_chip *nand = mtd->priv;
+	struct nand_chip *nand = mtd_to_nand(mtd);
 	int ret;
 
 	if (!ecc->size) {
@@ -1232,7 +1231,6 @@ static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 {
 	const struct nand_sdr_timings *timings;
 	struct sunxi_nand_chip *chip;
-	struct mtd_part_parser_data ppdata;
 	struct mtd_info *mtd;
 	struct nand_chip *nand;
 	int nsels;
@@ -1330,16 +1328,15 @@ static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 	 * in the DT.
 	 */
 	nand->ecc.mode = NAND_ECC_HW;
-	nand->flash_node = np;
+	nand_set_flash_node(nand, np);
 	nand->select_chip = sunxi_nfc_select_chip;
 	nand->cmd_ctrl = sunxi_nfc_cmd_ctrl;
 	nand->read_buf = sunxi_nfc_read_buf;
 	nand->write_buf = sunxi_nfc_write_buf;
 	nand->read_byte = sunxi_nfc_read_byte;
 
-	mtd = &chip->mtd;
+	mtd = nand_to_mtd(nand);
 	mtd->dev.parent = dev;
-	mtd->priv = nand;
 
 	ret = nand_scan_ident(mtd, nsels, NULL);
 	if (ret)
@@ -1366,8 +1363,7 @@ static int sunxi_nand_chip_init(struct device *dev, struct sunxi_nfc *nfc,
 		return ret;
 	}
 
-	ppdata.of_node = np;
-	ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(mtd, NULL, 0);
 	if (ret) {
 		dev_err(dev, "failed to register mtd device: %d\n", ret);
 		nand_release(mtd);
@@ -1393,8 +1389,10 @@ static int sunxi_nand_chips_init(struct device *dev, struct sunxi_nfc *nfc)
 
 	for_each_child_of_node(np, nand_np) {
 		ret = sunxi_nand_chip_init(dev, nfc, nand_np);
-		if (ret)
+		if (ret) {
+			of_node_put(nand_np);
 			return ret;
+		}
 	}
 
 	return 0;
@@ -1407,7 +1405,7 @@ static void sunxi_nand_chips_cleanup(struct sunxi_nfc *nfc)
 	while (!list_empty(&nfc->chips)) {
 		chip = list_first_entry(&nfc->chips, struct sunxi_nand_chip,
 					node);
-		nand_release(&chip->mtd);
+		nand_release(nand_to_mtd(&chip->nand));
 		sunxi_nand_ecc_cleanup(&chip->nand.ecc);
 		list_del(&chip->node);
 	}
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index befddf0776e4..08b30549ec0a 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -103,7 +103,6 @@
 /*--------------------------------------------------------------------------*/
 
 struct tmio_nand {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 
 	struct platform_device *dev;
@@ -119,7 +118,10 @@ struct tmio_nand {
 	unsigned read_good:1;
 };
 
-#define mtd_to_tmio(m)			container_of(m, struct tmio_nand, mtd)
+static inline struct tmio_nand *mtd_to_tmio(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct tmio_nand, chip);
+}
 
 
 /*--------------------------------------------------------------------------*/
@@ -128,7 +130,7 @@ static void tmio_nand_hwcontrol(struct mtd_info *mtd, int cmd,
 				   unsigned int ctrl)
 {
 	struct tmio_nand *tmio = mtd_to_tmio(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		u8 mode;
@@ -378,9 +380,8 @@ static int tmio_probe(struct platform_device *dev)
 	tmio->dev = dev;
 
 	platform_set_drvdata(dev, tmio);
-	mtd = &tmio->mtd;
 	nand_chip = &tmio->chip;
-	mtd->priv = nand_chip;
+	mtd = nand_to_mtd(nand_chip);
 	mtd->name = "tmio-nand";
 	mtd->dev.parent = &dev->dev;
 
@@ -456,7 +457,7 @@ static int tmio_remove(struct platform_device *dev)
 {
 	struct tmio_nand *tmio = platform_get_drvdata(dev);
 
-	nand_release(&tmio->mtd);
+	nand_release(nand_to_mtd(&tmio->chip));
 	tmio_hw_stop(dev, tmio);
 	return 0;
 }
diff --git a/drivers/mtd/nand/txx9ndfmc.c b/drivers/mtd/nand/txx9ndfmc.c
index 8572519b8441..04d63f56baa4 100644
--- a/drivers/mtd/nand/txx9ndfmc.c
+++ b/drivers/mtd/nand/txx9ndfmc.c
@@ -63,7 +63,6 @@
 struct txx9ndfmc_priv {
 	struct platform_device *dev;
 	struct nand_chip chip;
-	struct mtd_info mtd;
 	int cs;
 	const char *mtdname;
 };
@@ -79,8 +78,8 @@ struct txx9ndfmc_drvdata {
 
 static struct platform_device *mtd_to_platdev(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct txx9ndfmc_priv *txx9_priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct txx9ndfmc_priv *txx9_priv = nand_get_controller_data(chip);
 	return txx9_priv->dev;
 }
 
@@ -135,8 +134,8 @@ static void txx9ndfmc_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 static void txx9ndfmc_cmd_ctrl(struct mtd_info *mtd, int cmd,
 			       unsigned int ctrl)
 {
-	struct nand_chip *chip = mtd->priv;
-	struct txx9ndfmc_priv *txx9_priv = chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct txx9ndfmc_priv *txx9_priv = nand_get_controller_data(chip);
 	struct platform_device *dev = txx9_priv->dev;
 	struct txx9ndfmc_platform_data *plat = dev_get_platdata(&dev->dev);
 
@@ -175,7 +174,7 @@ static int txx9ndfmc_calculate_ecc(struct mtd_info *mtd, const uint8_t *dat,
 				   uint8_t *ecc_code)
 {
 	struct platform_device *dev = mtd_to_platdev(mtd);
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int eccbytes;
 	u32 mcr = txx9ndfmc_read(dev, TXX9_NDFMCR);
 
@@ -195,7 +194,7 @@ static int txx9ndfmc_calculate_ecc(struct mtd_info *mtd, const uint8_t *dat,
 static int txx9ndfmc_correct_data(struct mtd_info *mtd, unsigned char *buf,
 		unsigned char *read_ecc, unsigned char *calc_ecc)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int eccsize;
 	int corrected = 0;
 	int stat;
@@ -257,7 +256,7 @@ static void txx9ndfmc_initialize(struct platform_device *dev)
 
 static int txx9ndfmc_nand_scan(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = mtd->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
 	int ret;
 
 	ret = nand_scan_ident(mtd, 1, NULL);
@@ -322,11 +321,9 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 		if (!txx9_priv)
 			continue;
 		chip = &txx9_priv->chip;
-		mtd = &txx9_priv->mtd;
+		mtd = nand_to_mtd(chip);
 		mtd->dev.parent = &dev->dev;
 
-		mtd->priv = chip;
-
 		chip->read_byte = txx9ndfmc_read_byte;
 		chip->read_buf = txx9ndfmc_read_buf;
 		chip->write_buf = txx9ndfmc_write_buf;
@@ -343,7 +340,7 @@ static int __init txx9ndfmc_probe(struct platform_device *dev)
 		chip->chip_delay = 100;
 		chip->controller = &drvdata->hw_control;
 
-		chip->priv = txx9_priv;
+		nand_set_controller_data(chip, txx9_priv);
 		txx9_priv->dev = dev;
 
 		if (plat->ch_mask != 1) {
@@ -391,8 +388,8 @@ static int __exit txx9ndfmc_remove(struct platform_device *dev)
 
 		if (!mtd)
 			continue;
-		chip = mtd->priv;
-		txx9_priv = chip->priv;
+		chip = mtd_to_nand(mtd);
+		txx9_priv = nand_get_controller_data(chip);
 
 		nand_release(mtd);
 		kfree(txx9_priv->mtdname);
diff --git a/drivers/mtd/nand/vf610_nfc.c b/drivers/mtd/nand/vf610_nfc.c
index 8805d6325579..034420f313d5 100644
--- a/drivers/mtd/nand/vf610_nfc.c
+++ b/drivers/mtd/nand/vf610_nfc.c
@@ -156,7 +156,6 @@ enum vf610_nfc_variant {
 };
 
 struct vf610_nfc {
-	struct mtd_info mtd;
 	struct nand_chip chip;
 	struct device *dev;
 	void __iomem *regs;
@@ -171,7 +170,10 @@ struct vf610_nfc {
 	u32 ecc_mode;
 };
 
-#define mtd_to_nfc(_mtd) container_of(_mtd, struct vf610_nfc, mtd)
+static inline struct vf610_nfc *mtd_to_nfc(struct mtd_info *mtd)
+{
+	return container_of(mtd_to_nand(mtd), struct vf610_nfc, chip);
+}
 
 static struct nand_ecclayout vf610_nfc_ecc45 = {
 	.eccbytes = 45,
@@ -674,10 +676,9 @@ static int vf610_nfc_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	nfc->dev = &pdev->dev;
-	mtd = &nfc->mtd;
 	chip = &nfc->chip;
+	mtd = nand_to_mtd(chip);
 
-	mtd->priv = chip;
 	mtd->owner = THIS_MODULE;
 	mtd->dev.parent = nfc->dev;
 	mtd->name = DRV_NAME;
@@ -707,18 +708,18 @@ static int vf610_nfc_probe(struct platform_device *pdev)
 	for_each_available_child_of_node(nfc->dev->of_node, child) {
 		if (of_device_is_compatible(child, "fsl,vf610-nfc-nandcs")) {
 
-			if (chip->flash_node) {
+			if (nand_get_flash_node(chip)) {
 				dev_err(nfc->dev,
 					"Only one NAND chip supported!\n");
 				err = -EINVAL;
 				goto error;
 			}
 
-			chip->flash_node = child;
+			nand_set_flash_node(chip, child);
 		}
 	}
 
-	if (!chip->flash_node) {
+	if (!nand_get_flash_node(chip)) {
 		dev_err(nfc->dev, "NAND chip sub-node missing!\n");
 		err = -ENODEV;
 		goto err_clk;
@@ -811,14 +812,10 @@ static int vf610_nfc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, mtd);
 
 	/* Register device in MTD */
-	return mtd_device_parse_register(mtd, NULL,
-		&(struct mtd_part_parser_data){
-			.of_node = chip->flash_node,
-		},
-		NULL, 0);
+	return mtd_device_register(mtd, NULL, 0);
 
 error:
-	of_node_put(chip->flash_node);
+	of_node_put(nand_get_flash_node(chip));
 err_clk:
 	clk_disable_unprepare(nfc->clk);
 	return err;
diff --git a/drivers/mtd/nand/xway_nand.c b/drivers/mtd/nand/xway_nand.c
index 3b28db458ea0..0cf0ac07a8c2 100644
--- a/drivers/mtd/nand/xway_nand.c
+++ b/drivers/mtd/nand/xway_nand.c
@@ -89,7 +89,7 @@ static void xway_select_chip(struct mtd_info *mtd, int chip)
 
 static void xway_cmd_ctrl(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	unsigned long nandaddr = (unsigned long) this->IO_ADDR_W;
 	unsigned long flags;
 
@@ -118,7 +118,7 @@ static int xway_dev_ready(struct mtd_info *mtd)
 
 static unsigned char xway_read_byte(struct mtd_info *mtd)
 {
-	struct nand_chip *this = mtd->priv;
+	struct nand_chip *this = mtd_to_nand(mtd);
 	unsigned long nandaddr = (unsigned long) this->IO_ADDR_R;
 	unsigned long flags;
 	int ret;
diff --git a/drivers/mtd/ofpart.c b/drivers/mtd/ofpart.c
index 9ed6038e47d2..ede407d6e106 100644
--- a/drivers/mtd/ofpart.c
+++ b/drivers/mtd/ofpart.c
@@ -26,9 +26,10 @@ static bool node_has_compatible(struct device_node *pp)
 }
 
 static int parse_ofpart_partitions(struct mtd_info *master,
-				   struct mtd_partition **pparts,
+				   const struct mtd_partition **pparts,
 				   struct mtd_part_parser_data *data)
 {
+	struct mtd_partition *parts;
 	struct device_node *mtd_node;
 	struct device_node *ofpart_node;
 	const char *partname;
@@ -37,10 +38,8 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 	bool dedicated = true;
 
 
-	if (!data)
-		return 0;
-
-	mtd_node = data->of_node;
+	/* Pull of_node from the master device node */
+	mtd_node = mtd_get_of_node(master);
 	if (!mtd_node)
 		return 0;
 
@@ -72,8 +71,8 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 	if (nr_parts == 0)
 		return 0;
 
-	*pparts = kzalloc(nr_parts * sizeof(**pparts), GFP_KERNEL);
-	if (!*pparts)
+	parts = kzalloc(nr_parts * sizeof(*parts), GFP_KERNEL);
+	if (!parts)
 		return -ENOMEM;
 
 	i = 0;
@@ -107,19 +106,19 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 			goto ofpart_fail;
 		}
 
-		(*pparts)[i].offset = of_read_number(reg, a_cells);
-		(*pparts)[i].size = of_read_number(reg + a_cells, s_cells);
+		parts[i].offset = of_read_number(reg, a_cells);
+		parts[i].size = of_read_number(reg + a_cells, s_cells);
 
 		partname = of_get_property(pp, "label", &len);
 		if (!partname)
 			partname = of_get_property(pp, "name", &len);
-		(*pparts)[i].name = partname;
+		parts[i].name = partname;
 
 		if (of_get_property(pp, "read-only", &len))
-			(*pparts)[i].mask_flags |= MTD_WRITEABLE;
+			parts[i].mask_flags |= MTD_WRITEABLE;
 
 		if (of_get_property(pp, "lock", &len))
-			(*pparts)[i].mask_flags |= MTD_POWERUP_LOCK;
+			parts[i].mask_flags |= MTD_POWERUP_LOCK;
 
 		i++;
 	}
@@ -127,6 +126,7 @@ static int parse_ofpart_partitions(struct mtd_info *master,
 	if (!nr_parts)
 		goto ofpart_none;
 
+	*pparts = parts;
 	return nr_parts;
 
 ofpart_fail:
@@ -135,21 +135,20 @@ ofpart_fail:
 	ret = -EINVAL;
 ofpart_none:
 	of_node_put(pp);
-	kfree(*pparts);
-	*pparts = NULL;
+	kfree(parts);
 	return ret;
 }
 
 static struct mtd_part_parser ofpart_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = parse_ofpart_partitions,
 	.name = "ofpart",
 };
 
 static int parse_ofoldpart_partitions(struct mtd_info *master,
-				      struct mtd_partition **pparts,
+				      const struct mtd_partition **pparts,
 				      struct mtd_part_parser_data *data)
 {
+	struct mtd_partition *parts;
 	struct device_node *dp;
 	int i, plen, nr_parts;
 	const struct {
@@ -157,10 +156,8 @@ static int parse_ofoldpart_partitions(struct mtd_info *master,
 	} *part;
 	const char *names;
 
-	if (!data)
-		return 0;
-
-	dp = data->of_node;
+	/* Pull of_node from the master device node */
+	dp = mtd_get_of_node(master);
 	if (!dp)
 		return 0;
 
@@ -173,37 +170,37 @@ static int parse_ofoldpart_partitions(struct mtd_info *master,
 
 	nr_parts = plen / sizeof(part[0]);
 
-	*pparts = kzalloc(nr_parts * sizeof(*(*pparts)), GFP_KERNEL);
-	if (!*pparts)
+	parts = kzalloc(nr_parts * sizeof(*parts), GFP_KERNEL);
+	if (!parts)
 		return -ENOMEM;
 
 	names = of_get_property(dp, "partition-names", &plen);
 
 	for (i = 0; i < nr_parts; i++) {
-		(*pparts)[i].offset = be32_to_cpu(part->offset);
-		(*pparts)[i].size   = be32_to_cpu(part->len) & ~1;
+		parts[i].offset = be32_to_cpu(part->offset);
+		parts[i].size   = be32_to_cpu(part->len) & ~1;
 		/* bit 0 set signifies read only partition */
 		if (be32_to_cpu(part->len) & 1)
-			(*pparts)[i].mask_flags = MTD_WRITEABLE;
+			parts[i].mask_flags = MTD_WRITEABLE;
 
 		if (names && (plen > 0)) {
 			int len = strlen(names) + 1;
 
-			(*pparts)[i].name = names;
+			parts[i].name = names;
 			plen -= len;
 			names += len;
 		} else {
-			(*pparts)[i].name = "unnamed";
+			parts[i].name = "unnamed";
 		}
 
 		part++;
 	}
 
+	*pparts = parts;
 	return nr_parts;
 }
 
 static struct mtd_part_parser ofoldpart_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = parse_ofoldpart_partitions,
 	.name = "ofoldpart",
 };
diff --git a/drivers/mtd/onenand/omap2.c b/drivers/mtd/onenand/omap2.c
index 3e0285696227..0aacf125938b 100644
--- a/drivers/mtd/onenand/omap2.c
+++ b/drivers/mtd/onenand/omap2.c
@@ -614,7 +614,6 @@ static int omap2_onenand_probe(struct platform_device *pdev)
 	struct onenand_chip *this;
 	int r;
 	struct resource *res;
-	struct mtd_part_parser_data ppdata = {};
 
 	pdata = dev_get_platdata(&pdev->dev);
 	if (pdata == NULL) {
@@ -713,6 +712,7 @@ static int omap2_onenand_probe(struct platform_device *pdev)
 	c->mtd.priv = &c->onenand;
 
 	c->mtd.dev.parent = &pdev->dev;
+	mtd_set_of_node(&c->mtd, pdata->of_node);
 
 	this = &c->onenand;
 	if (c->dma_channel >= 0) {
@@ -743,10 +743,8 @@ static int omap2_onenand_probe(struct platform_device *pdev)
 	if ((r = onenand_scan(&c->mtd, 1)) < 0)
 		goto err_release_regulator;
 
-	ppdata.of_node = pdata->of_node;
-	r = mtd_device_parse_register(&c->mtd, NULL, &ppdata,
-				      pdata ? pdata->parts : NULL,
-				      pdata ? pdata->nr_parts : 0);
+	r = mtd_device_register(&c->mtd, pdata ? pdata->parts : NULL,
+				pdata ? pdata->nr_parts : 0);
 	if (r)
 		goto err_release_onenand;
 
diff --git a/drivers/mtd/redboot.c b/drivers/mtd/redboot.c
index 5da911ebdf49..7623ac5fc586 100644
--- a/drivers/mtd/redboot.c
+++ b/drivers/mtd/redboot.c
@@ -57,7 +57,7 @@ static inline int redboot_checksum(struct fis_image_desc *img)
 }
 
 static int parse_redboot_partitions(struct mtd_info *master,
-				    struct mtd_partition **pparts,
+				    const struct mtd_partition **pparts,
 				    struct mtd_part_parser_data *data)
 {
 	int nrparts = 0;
@@ -290,28 +290,13 @@ static int parse_redboot_partitions(struct mtd_info *master,
 }
 
 static struct mtd_part_parser redboot_parser = {
-	.owner = THIS_MODULE,
 	.parse_fn = parse_redboot_partitions,
 	.name = "RedBoot",
 };
+module_mtd_part_parser(redboot_parser);
 
 /* mtd parsers will request the module by parser name */
 MODULE_ALIAS("RedBoot");
-
-static int __init redboot_parser_init(void)
-{
-	register_mtd_parser(&redboot_parser);
-	return 0;
-}
-
-static void __exit redboot_parser_exit(void)
-{
-	deregister_mtd_parser(&redboot_parser);
-}
-
-module_init(redboot_parser_init);
-module_exit(redboot_parser_exit);
-
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("David Woodhouse <dwmw2@infradead.org>");
 MODULE_DESCRIPTION("Parsing code for RedBoot Flash Image System (FIS) tables");
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index c23184a47fc4..b096f8bb05ba 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -206,9 +206,10 @@ static loff_t sm_mkoffset(struct sm_ftl *ftl, int zone, int block, int boffset)
 }
 
 /* Breaks offset into parts */
-static void sm_break_offset(struct sm_ftl *ftl, loff_t offset,
+static void sm_break_offset(struct sm_ftl *ftl, loff_t loffset,
 			    int *zone, int *block, int *boffset)
 {
+	u64 offset = loffset;
 	*boffset = do_div(offset, ftl->block_size);
 	*block = do_div(offset, ftl->max_lba);
 	*zone = offset >= ftl->zone_count ? -1 : offset;
diff --git a/drivers/mtd/spi-nor/Kconfig b/drivers/mtd/spi-nor/Kconfig
index 2fe2a7e90fa9..0dc927540b3d 100644
--- a/drivers/mtd/spi-nor/Kconfig
+++ b/drivers/mtd/spi-nor/Kconfig
@@ -7,6 +7,13 @@ menuconfig MTD_SPI_NOR
 
 if MTD_SPI_NOR
 
+config MTD_MT81xx_NOR
+	tristate "Mediatek MT81xx SPI NOR flash controller"
+	help
+	  This enables access to SPI NOR flash, using MT81xx SPI NOR flash
+	  controller. This controller does not support generic SPI BUS, it only
+	  supports SPI NOR Flash.
+
 config MTD_SPI_NOR_USE_4K_SECTORS
 	bool "Use small 4096 B erase sectors"
 	default y
diff --git a/drivers/mtd/spi-nor/Makefile b/drivers/mtd/spi-nor/Makefile
index e53333ef8582..0bf3a7f81675 100644
--- a/drivers/mtd/spi-nor/Makefile
+++ b/drivers/mtd/spi-nor/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_MTD_SPI_NOR)	+= spi-nor.o
 obj-$(CONFIG_SPI_FSL_QUADSPI)	+= fsl-quadspi.o
+obj-$(CONFIG_MTD_MT81xx_NOR)    += mtk-quadspi.o
 obj-$(CONFIG_SPI_NXP_SPIFI)	+= nxp-spifi.o
diff --git a/drivers/mtd/spi-nor/fsl-quadspi.c b/drivers/mtd/spi-nor/fsl-quadspi.c
index 7b10ed413983..54640f1eb3a1 100644
--- a/drivers/mtd/spi-nor/fsl-quadspi.c
+++ b/drivers/mtd/spi-nor/fsl-quadspi.c
@@ -269,7 +269,7 @@ struct fsl_qspi {
 	struct clk *clk, *clk_en;
 	struct device *dev;
 	struct completion c;
-	struct fsl_qspi_devtype_data *devtype_data;
+	const struct fsl_qspi_devtype_data *devtype_data;
 	u32 nor_size;
 	u32 nor_num;
 	u32 clk_rate;
@@ -927,15 +927,12 @@ static void fsl_qspi_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
 static int fsl_qspi_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	struct mtd_part_parser_data ppdata;
 	struct device *dev = &pdev->dev;
 	struct fsl_qspi *q;
 	struct resource *res;
 	struct spi_nor *nor;
 	struct mtd_info *mtd;
 	int ret, i = 0;
-	const struct of_device_id *of_id =
-			of_match_device(fsl_qspi_dt_ids, &pdev->dev);
 
 	q = devm_kzalloc(dev, sizeof(*q), GFP_KERNEL);
 	if (!q)
@@ -946,7 +943,9 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	q->dev = dev;
-	q->devtype_data = (struct fsl_qspi_devtype_data *)of_id->data;
+	q->devtype_data = of_device_get_match_data(dev);
+	if (!q->devtype_data)
+		return -ENODEV;
 	platform_set_drvdata(pdev, q);
 
 	/* find the resources */
@@ -1013,7 +1012,7 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 		mtd = &nor->mtd;
 
 		nor->dev = dev;
-		nor->flash_node = np;
+		spi_nor_set_flash_node(nor, np);
 		nor->priv = q;
 
 		/* fill the hooks */
@@ -1038,8 +1037,7 @@ static int fsl_qspi_probe(struct platform_device *pdev)
 		if (ret)
 			goto mutex_failed;
 
-		ppdata.of_node = np;
-		ret = mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+		ret = mtd_device_register(mtd, NULL, 0);
 		if (ret)
 			goto mutex_failed;
 
diff --git a/drivers/mtd/spi-nor/mtk-quadspi.c b/drivers/mtd/spi-nor/mtk-quadspi.c
new file mode 100644
index 000000000000..d5f850d035bb
--- /dev/null
+++ b/drivers/mtd/spi-nor/mtk-quadspi.c
@@ -0,0 +1,485 @@
+/*
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author: Bayi Cheng <bayi.cheng@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/device.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/ioport.h>
+#include <linux/math64.h>
+#include <linux/module.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/mtd/spi-nor.h>
+
+#define MTK_NOR_CMD_REG			0x00
+#define MTK_NOR_CNT_REG			0x04
+#define MTK_NOR_RDSR_REG		0x08
+#define MTK_NOR_RDATA_REG		0x0c
+#define MTK_NOR_RADR0_REG		0x10
+#define MTK_NOR_RADR1_REG		0x14
+#define MTK_NOR_RADR2_REG		0x18
+#define MTK_NOR_WDATA_REG		0x1c
+#define MTK_NOR_PRGDATA0_REG		0x20
+#define MTK_NOR_PRGDATA1_REG		0x24
+#define MTK_NOR_PRGDATA2_REG		0x28
+#define MTK_NOR_PRGDATA3_REG		0x2c
+#define MTK_NOR_PRGDATA4_REG		0x30
+#define MTK_NOR_PRGDATA5_REG		0x34
+#define MTK_NOR_SHREG0_REG		0x38
+#define MTK_NOR_SHREG1_REG		0x3c
+#define MTK_NOR_SHREG2_REG		0x40
+#define MTK_NOR_SHREG3_REG		0x44
+#define MTK_NOR_SHREG4_REG		0x48
+#define MTK_NOR_SHREG5_REG		0x4c
+#define MTK_NOR_SHREG6_REG		0x50
+#define MTK_NOR_SHREG7_REG		0x54
+#define MTK_NOR_SHREG8_REG		0x58
+#define MTK_NOR_SHREG9_REG		0x5c
+#define MTK_NOR_CFG1_REG		0x60
+#define MTK_NOR_CFG2_REG		0x64
+#define MTK_NOR_CFG3_REG		0x68
+#define MTK_NOR_STATUS0_REG		0x70
+#define MTK_NOR_STATUS1_REG		0x74
+#define MTK_NOR_STATUS2_REG		0x78
+#define MTK_NOR_STATUS3_REG		0x7c
+#define MTK_NOR_FLHCFG_REG		0x84
+#define MTK_NOR_TIME_REG		0x94
+#define MTK_NOR_PP_DATA_REG		0x98
+#define MTK_NOR_PREBUF_STUS_REG		0x9c
+#define MTK_NOR_DELSEL0_REG		0xa0
+#define MTK_NOR_DELSEL1_REG		0xa4
+#define MTK_NOR_INTRSTUS_REG		0xa8
+#define MTK_NOR_INTREN_REG		0xac
+#define MTK_NOR_CHKSUM_CTL_REG		0xb8
+#define MTK_NOR_CHKSUM_REG		0xbc
+#define MTK_NOR_CMD2_REG		0xc0
+#define MTK_NOR_WRPROT_REG		0xc4
+#define MTK_NOR_RADR3_REG		0xc8
+#define MTK_NOR_DUAL_REG		0xcc
+#define MTK_NOR_DELSEL2_REG		0xd0
+#define MTK_NOR_DELSEL3_REG		0xd4
+#define MTK_NOR_DELSEL4_REG		0xd8
+
+/* commands for mtk nor controller */
+#define MTK_NOR_READ_CMD		0x0
+#define MTK_NOR_RDSR_CMD		0x2
+#define MTK_NOR_PRG_CMD			0x4
+#define MTK_NOR_WR_CMD			0x10
+#define MTK_NOR_PIO_WR_CMD		0x90
+#define MTK_NOR_WRSR_CMD		0x20
+#define MTK_NOR_PIO_READ_CMD		0x81
+#define MTK_NOR_WR_BUF_ENABLE		0x1
+#define MTK_NOR_WR_BUF_DISABLE		0x0
+#define MTK_NOR_ENABLE_SF_CMD		0x30
+#define MTK_NOR_DUAD_ADDR_EN		0x8
+#define MTK_NOR_QUAD_READ_EN		0x4
+#define MTK_NOR_DUAL_ADDR_EN		0x2
+#define MTK_NOR_DUAL_READ_EN		0x1
+#define MTK_NOR_DUAL_DISABLE		0x0
+#define MTK_NOR_FAST_READ		0x1
+
+#define SFLASH_WRBUF_SIZE		128
+
+/* Can shift up to 48 bits (6 bytes) of TX/RX */
+#define MTK_NOR_MAX_RX_TX_SHIFT		6
+/* can shift up to 56 bits (7 bytes) transfer by MTK_NOR_PRG_CMD */
+#define MTK_NOR_MAX_SHIFT		7
+
+/* Helpers for accessing the program data / shift data registers */
+#define MTK_NOR_PRG_REG(n)		(MTK_NOR_PRGDATA0_REG + 4 * (n))
+#define MTK_NOR_SHREG(n)		(MTK_NOR_SHREG0_REG + 4 * (n))
+
+struct mt8173_nor {
+	struct spi_nor nor;
+	struct device *dev;
+	void __iomem *base;	/* nor flash base address */
+	struct clk *spi_clk;
+	struct clk *nor_clk;
+};
+
+static void mt8173_nor_set_read_mode(struct mt8173_nor *mt8173_nor)
+{
+	struct spi_nor *nor = &mt8173_nor->nor;
+
+	switch (nor->flash_read) {
+	case SPI_NOR_FAST:
+		writeb(nor->read_opcode, mt8173_nor->base +
+		       MTK_NOR_PRGDATA3_REG);
+		writeb(MTK_NOR_FAST_READ, mt8173_nor->base +
+		       MTK_NOR_CFG1_REG);
+		break;
+	case SPI_NOR_DUAL:
+		writeb(nor->read_opcode, mt8173_nor->base +
+		       MTK_NOR_PRGDATA3_REG);
+		writeb(MTK_NOR_DUAL_READ_EN, mt8173_nor->base +
+		       MTK_NOR_DUAL_REG);
+		break;
+	case SPI_NOR_QUAD:
+		writeb(nor->read_opcode, mt8173_nor->base +
+		       MTK_NOR_PRGDATA4_REG);
+		writeb(MTK_NOR_QUAD_READ_EN, mt8173_nor->base +
+		       MTK_NOR_DUAL_REG);
+		break;
+	default:
+		writeb(MTK_NOR_DUAL_DISABLE, mt8173_nor->base +
+		       MTK_NOR_DUAL_REG);
+		break;
+	}
+}
+
+static int mt8173_nor_execute_cmd(struct mt8173_nor *mt8173_nor, u8 cmdval)
+{
+	int reg;
+	u8 val = cmdval & 0x1f;
+
+	writeb(cmdval, mt8173_nor->base + MTK_NOR_CMD_REG);
+	return readl_poll_timeout(mt8173_nor->base + MTK_NOR_CMD_REG, reg,
+				  !(reg & val), 100, 10000);
+}
+
+static int mt8173_nor_do_tx_rx(struct mt8173_nor *mt8173_nor, u8 op,
+			       u8 *tx, int txlen, u8 *rx, int rxlen)
+{
+	int len = 1 + txlen + rxlen;
+	int i, ret, idx;
+
+	if (len > MTK_NOR_MAX_SHIFT)
+		return -EINVAL;
+
+	writeb(len * 8, mt8173_nor->base + MTK_NOR_CNT_REG);
+
+	/* start at PRGDATA5, go down to PRGDATA0 */
+	idx = MTK_NOR_MAX_RX_TX_SHIFT - 1;
+
+	/* opcode */
+	writeb(op, mt8173_nor->base + MTK_NOR_PRG_REG(idx));
+	idx--;
+
+	/* program TX data */
+	for (i = 0; i < txlen; i++, idx--)
+		writeb(tx[i], mt8173_nor->base + MTK_NOR_PRG_REG(idx));
+
+	/* clear out rest of TX registers */
+	while (idx >= 0) {
+		writeb(0, mt8173_nor->base + MTK_NOR_PRG_REG(idx));
+		idx--;
+	}
+
+	ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PRG_CMD);
+	if (ret)
+		return ret;
+
+	/* restart at first RX byte */
+	idx = rxlen - 1;
+
+	/* read out RX data */
+	for (i = 0; i < rxlen; i++, idx--)
+		rx[i] = readb(mt8173_nor->base + MTK_NOR_SHREG(idx));
+
+	return 0;
+}
+
+/* Do a WRSR (Write Status Register) command */
+static int mt8173_nor_wr_sr(struct mt8173_nor *mt8173_nor, u8 sr)
+{
+	writeb(sr, mt8173_nor->base + MTK_NOR_PRGDATA5_REG);
+	writeb(8, mt8173_nor->base + MTK_NOR_CNT_REG);
+	return mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_WRSR_CMD);
+}
+
+static int mt8173_nor_write_buffer_enable(struct mt8173_nor *mt8173_nor)
+{
+	u8 reg;
+
+	/* the bit0 of MTK_NOR_CFG2_REG is pre-fetch buffer
+	 * 0: pre-fetch buffer use for read
+	 * 1: pre-fetch buffer use for page program
+	 */
+	writel(MTK_NOR_WR_BUF_ENABLE, mt8173_nor->base + MTK_NOR_CFG2_REG);
+	return readb_poll_timeout(mt8173_nor->base + MTK_NOR_CFG2_REG, reg,
+				  0x01 == (reg & 0x01), 100, 10000);
+}
+
+static int mt8173_nor_write_buffer_disable(struct mt8173_nor *mt8173_nor)
+{
+	u8 reg;
+
+	writel(MTK_NOR_WR_BUF_DISABLE, mt8173_nor->base + MTK_NOR_CFG2_REG);
+	return readb_poll_timeout(mt8173_nor->base + MTK_NOR_CFG2_REG, reg,
+				  MTK_NOR_WR_BUF_DISABLE == (reg & 0x1), 100,
+				  10000);
+}
+
+static void mt8173_nor_set_addr(struct mt8173_nor *mt8173_nor, u32 addr)
+{
+	int i;
+
+	for (i = 0; i < 3; i++) {
+		writeb(addr & 0xff, mt8173_nor->base + MTK_NOR_RADR0_REG + i * 4);
+		addr >>= 8;
+	}
+	/* Last register is non-contiguous */
+	writeb(addr & 0xff, mt8173_nor->base + MTK_NOR_RADR3_REG);
+}
+
+static int mt8173_nor_read(struct spi_nor *nor, loff_t from, size_t length,
+			   size_t *retlen, u_char *buffer)
+{
+	int i, ret;
+	int addr = (int)from;
+	u8 *buf = (u8 *)buffer;
+	struct mt8173_nor *mt8173_nor = nor->priv;
+
+	/* set mode for fast read mode ,dual mode or quad mode */
+	mt8173_nor_set_read_mode(mt8173_nor);
+	mt8173_nor_set_addr(mt8173_nor, addr);
+
+	for (i = 0; i < length; i++, (*retlen)++) {
+		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PIO_READ_CMD);
+		if (ret < 0)
+			return ret;
+		buf[i] = readb(mt8173_nor->base + MTK_NOR_RDATA_REG);
+	}
+	return 0;
+}
+
+static int mt8173_nor_write_single_byte(struct mt8173_nor *mt8173_nor,
+					int addr, int length, u8 *data)
+{
+	int i, ret;
+
+	mt8173_nor_set_addr(mt8173_nor, addr);
+
+	for (i = 0; i < length; i++) {
+		writeb(*data++, mt8173_nor->base + MTK_NOR_WDATA_REG);
+		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PIO_WR_CMD);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int mt8173_nor_write_buffer(struct mt8173_nor *mt8173_nor, int addr,
+				   const u8 *buf)
+{
+	int i, bufidx, data;
+
+	mt8173_nor_set_addr(mt8173_nor, addr);
+
+	bufidx = 0;
+	for (i = 0; i < SFLASH_WRBUF_SIZE; i += 4) {
+		data = buf[bufidx + 3]<<24 | buf[bufidx + 2]<<16 |
+		       buf[bufidx + 1]<<8 | buf[bufidx];
+		bufidx += 4;
+		writel(data, mt8173_nor->base + MTK_NOR_PP_DATA_REG);
+	}
+	return mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_WR_CMD);
+}
+
+static void mt8173_nor_write(struct spi_nor *nor, loff_t to, size_t len,
+			     size_t *retlen, const u_char *buf)
+{
+	int ret;
+	struct mt8173_nor *mt8173_nor = nor->priv;
+
+	ret = mt8173_nor_write_buffer_enable(mt8173_nor);
+	if (ret < 0)
+		dev_warn(mt8173_nor->dev, "write buffer enable failed!\n");
+
+	while (len >= SFLASH_WRBUF_SIZE) {
+		ret = mt8173_nor_write_buffer(mt8173_nor, to, buf);
+		if (ret < 0)
+			dev_err(mt8173_nor->dev, "write buffer failed!\n");
+		len -= SFLASH_WRBUF_SIZE;
+		to += SFLASH_WRBUF_SIZE;
+		buf += SFLASH_WRBUF_SIZE;
+		(*retlen) += SFLASH_WRBUF_SIZE;
+	}
+	ret = mt8173_nor_write_buffer_disable(mt8173_nor);
+	if (ret < 0)
+		dev_warn(mt8173_nor->dev, "write buffer disable failed!\n");
+
+	if (len) {
+		ret = mt8173_nor_write_single_byte(mt8173_nor, to, (int)len,
+						   (u8 *)buf);
+		if (ret < 0)
+			dev_err(mt8173_nor->dev, "write single byte failed!\n");
+		(*retlen) += len;
+	}
+}
+
+static int mt8173_nor_read_reg(struct spi_nor *nor, u8 opcode, u8 *buf, int len)
+{
+	int ret;
+	struct mt8173_nor *mt8173_nor = nor->priv;
+
+	switch (opcode) {
+	case SPINOR_OP_RDSR:
+		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_RDSR_CMD);
+		if (ret < 0)
+			return ret;
+		if (len == 1)
+			*buf = readb(mt8173_nor->base + MTK_NOR_RDSR_REG);
+		else
+			dev_err(mt8173_nor->dev, "len should be 1 for read status!\n");
+		break;
+	default:
+		ret = mt8173_nor_do_tx_rx(mt8173_nor, opcode, NULL, 0, buf, len);
+		break;
+	}
+	return ret;
+}
+
+static int mt8173_nor_write_reg(struct spi_nor *nor, u8 opcode, u8 *buf,
+				int len)
+{
+	int ret;
+	struct mt8173_nor *mt8173_nor = nor->priv;
+
+	switch (opcode) {
+	case SPINOR_OP_WRSR:
+		/* We only handle 1 byte */
+		ret = mt8173_nor_wr_sr(mt8173_nor, *buf);
+		break;
+	default:
+		ret = mt8173_nor_do_tx_rx(mt8173_nor, opcode, buf, len, NULL, 0);
+		if (ret)
+			dev_warn(mt8173_nor->dev, "write reg failure!\n");
+		break;
+	}
+	return ret;
+}
+
+static int __init mtk_nor_init(struct mt8173_nor *mt8173_nor,
+			       struct device_node *flash_node)
+{
+	int ret;
+	struct spi_nor *nor;
+
+	/* initialize controller to accept commands */
+	writel(MTK_NOR_ENABLE_SF_CMD, mt8173_nor->base + MTK_NOR_WRPROT_REG);
+
+	nor = &mt8173_nor->nor;
+	nor->dev = mt8173_nor->dev;
+	nor->priv = mt8173_nor;
+	spi_nor_set_flash_node(nor, flash_node);
+
+	/* fill the hooks to spi nor */
+	nor->read = mt8173_nor_read;
+	nor->read_reg = mt8173_nor_read_reg;
+	nor->write = mt8173_nor_write;
+	nor->write_reg = mt8173_nor_write_reg;
+	nor->mtd.name = "mtk_nor";
+	/* initialized with NULL */
+	ret = spi_nor_scan(nor, NULL, SPI_NOR_DUAL);
+	if (ret)
+		return ret;
+
+	return mtd_device_register(&nor->mtd, NULL, 0);
+}
+
+static int mtk_nor_drv_probe(struct platform_device *pdev)
+{
+	struct device_node *flash_np;
+	struct resource *res;
+	int ret;
+	struct mt8173_nor *mt8173_nor;
+
+	if (!pdev->dev.of_node) {
+		dev_err(&pdev->dev, "No DT found\n");
+		return -EINVAL;
+	}
+
+	mt8173_nor = devm_kzalloc(&pdev->dev, sizeof(*mt8173_nor), GFP_KERNEL);
+	if (!mt8173_nor)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, mt8173_nor);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mt8173_nor->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mt8173_nor->base))
+		return PTR_ERR(mt8173_nor->base);
+
+	mt8173_nor->spi_clk = devm_clk_get(&pdev->dev, "spi");
+	if (IS_ERR(mt8173_nor->spi_clk))
+		return PTR_ERR(mt8173_nor->spi_clk);
+
+	mt8173_nor->nor_clk = devm_clk_get(&pdev->dev, "sf");
+	if (IS_ERR(mt8173_nor->nor_clk))
+		return PTR_ERR(mt8173_nor->nor_clk);
+
+	mt8173_nor->dev = &pdev->dev;
+	ret = clk_prepare_enable(mt8173_nor->spi_clk);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(mt8173_nor->nor_clk);
+	if (ret) {
+		clk_disable_unprepare(mt8173_nor->spi_clk);
+		return ret;
+	}
+	/* only support one attached flash */
+	flash_np = of_get_next_available_child(pdev->dev.of_node, NULL);
+	if (!flash_np) {
+		dev_err(&pdev->dev, "no SPI flash device to configure\n");
+		ret = -ENODEV;
+		goto nor_free;
+	}
+	ret = mtk_nor_init(mt8173_nor, flash_np);
+
+nor_free:
+	if (ret) {
+		clk_disable_unprepare(mt8173_nor->spi_clk);
+		clk_disable_unprepare(mt8173_nor->nor_clk);
+	}
+	return ret;
+}
+
+static int mtk_nor_drv_remove(struct platform_device *pdev)
+{
+	struct mt8173_nor *mt8173_nor = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(mt8173_nor->spi_clk);
+	clk_disable_unprepare(mt8173_nor->nor_clk);
+	return 0;
+}
+
+static const struct of_device_id mtk_nor_of_ids[] = {
+	{ .compatible = "mediatek,mt8173-nor"},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mtk_nor_of_ids);
+
+static struct platform_driver mtk_nor_driver = {
+	.probe = mtk_nor_drv_probe,
+	.remove = mtk_nor_drv_remove,
+	.driver = {
+		.name = "mtk-nor",
+		.of_match_table = mtk_nor_of_ids,
+	},
+};
+
+module_platform_driver(mtk_nor_driver);
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("MediaTek SPI NOR Flash Driver");
diff --git a/drivers/mtd/spi-nor/nxp-spifi.c b/drivers/mtd/spi-nor/nxp-spifi.c
index 9e82098ae644..ae428cb0e04b 100644
--- a/drivers/mtd/spi-nor/nxp-spifi.c
+++ b/drivers/mtd/spi-nor/nxp-spifi.c
@@ -271,7 +271,6 @@ static void nxp_spifi_dummy_id_read(struct spi_nor *nor)
 static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 				 struct device_node *np)
 {
-	struct mtd_part_parser_data ppdata;
 	enum read_mode flash_read;
 	u32 ctrl, property;
 	u16 mode = 0;
@@ -330,7 +329,7 @@ static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 	writel(ctrl, spifi->io_base + SPIFI_CTRL);
 
 	spifi->nor.dev   = spifi->dev;
-	spifi->nor.flash_node = np;
+	spi_nor_set_flash_node(&spifi->nor, np);
 	spifi->nor.priv  = spifi;
 	spifi->nor.read  = nxp_spifi_read;
 	spifi->nor.write = nxp_spifi_write;
@@ -361,8 +360,7 @@ static int nxp_spifi_setup_flash(struct nxp_spifi *spifi,
 		return ret;
 	}
 
-	ppdata.of_node = np;
-	ret = mtd_device_parse_register(&spifi->nor.mtd, NULL, &ppdata, NULL, 0);
+	ret = mtd_device_register(&spifi->nor.mtd, NULL, 0);
 	if (ret) {
 		dev_err(spifi->dev, "mtd device parse failed\n");
 		return ret;
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
index 32477c4eb421..ed0c19c558b5 100644
--- a/drivers/mtd/spi-nor/spi-nor.c
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -38,6 +38,7 @@
 #define CHIP_ERASE_2MB_READY_WAIT_JIFFIES	(40UL * HZ)
 
 #define SPI_NOR_MAX_ID_LEN	6
+#define SPI_NOR_MAX_ADDR_WIDTH	4
 
 struct flash_info {
 	char		*name;
@@ -313,6 +314,29 @@ static void spi_nor_unlock_and_unprep(struct spi_nor *nor, enum spi_nor_ops ops)
 }
 
 /*
+ * Initiate the erasure of a single sector
+ */
+static int spi_nor_erase_sector(struct spi_nor *nor, u32 addr)
+{
+	u8 buf[SPI_NOR_MAX_ADDR_WIDTH];
+	int i;
+
+	if (nor->erase)
+		return nor->erase(nor, addr);
+
+	/*
+	 * Default implementation, if driver doesn't have a specialized HW
+	 * control
+	 */
+	for (i = nor->addr_width - 1; i >= 0; i--) {
+		buf[i] = addr & 0xff;
+		addr >>= 8;
+	}
+
+	return nor->write_reg(nor, nor->erase_opcode, buf, nor->addr_width);
+}
+
+/*
  * Erase an address range on the nor chip.  The address range may extend
  * one or more erase sectors.  Return an error is there is a problem erasing.
  */
@@ -371,10 +395,9 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 		while (len) {
 			write_enable(nor);
 
-			if (nor->erase(nor, addr)) {
-				ret = -EIO;
+			ret = spi_nor_erase_sector(nor, addr);
+			if (ret)
 				goto erase_err;
-			}
 
 			addr += mtd->erasesize;
 			len -= mtd->erasesize;
@@ -387,17 +410,13 @@ static int spi_nor_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 	write_disable(nor);
 
+erase_err:
 	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_ERASE);
 
-	instr->state = MTD_ERASE_DONE;
+	instr->state = ret ? MTD_ERASE_FAILED : MTD_ERASE_DONE;
 	mtd_erase_callback(instr);
 
 	return ret;
-
-erase_err:
-	spi_nor_unlock_and_unprep(nor, SPI_NOR_OPS_ERASE);
-	instr->state = MTD_ERASE_FAILED;
-	return ret;
 }
 
 static void stm_get_locked_range(struct spi_nor *nor, u8 sr, loff_t *ofs,
@@ -459,11 +478,14 @@ static int stm_is_locked_sr(struct spi_nor *nor, loff_t ofs, uint64_t len,
 static int stm_lock(struct spi_nor *nor, loff_t ofs, uint64_t len)
 {
 	struct mtd_info *mtd = &nor->mtd;
-	u8 status_old, status_new;
+	int status_old, status_new;
 	u8 mask = SR_BP2 | SR_BP1 | SR_BP0;
 	u8 shift = ffs(mask) - 1, pow, val;
+	int ret;
 
 	status_old = read_sr(nor);
+	if (status_old < 0)
+		return status_old;
 
 	/* SPI NOR always locks to the end */
 	if (ofs + len != mtd->size) {
@@ -498,7 +520,10 @@ static int stm_lock(struct spi_nor *nor, loff_t ofs, uint64_t len)
 		return -EINVAL;
 
 	write_enable(nor);
-	return write_sr(nor, status_new);
+	ret = write_sr(nor, status_new);
+	if (ret)
+		return ret;
+	return spi_nor_wait_till_ready(nor);
 }
 
 /*
@@ -509,11 +534,14 @@ static int stm_lock(struct spi_nor *nor, loff_t ofs, uint64_t len)
 static int stm_unlock(struct spi_nor *nor, loff_t ofs, uint64_t len)
 {
 	struct mtd_info *mtd = &nor->mtd;
-	uint8_t status_old, status_new;
+	int status_old, status_new;
 	u8 mask = SR_BP2 | SR_BP1 | SR_BP0;
 	u8 shift = ffs(mask) - 1, pow, val;
+	int ret;
 
 	status_old = read_sr(nor);
+	if (status_old < 0)
+		return status_old;
 
 	/* Cannot unlock; would unlock larger region than requested */
 	if (stm_is_locked_sr(nor, ofs - mtd->erasesize, mtd->erasesize,
@@ -546,7 +574,10 @@ static int stm_unlock(struct spi_nor *nor, loff_t ofs, uint64_t len)
 		return -EINVAL;
 
 	write_enable(nor);
-	return write_sr(nor, status_new);
+	ret = write_sr(nor, status_new);
+	if (ret)
+		return ret;
+	return spi_nor_wait_till_ready(nor);
 }
 
 /*
@@ -715,9 +746,9 @@ static const struct flash_info spi_nor_ids[] = {
 	{ "mx25l4005a",  INFO(0xc22013, 0, 64 * 1024,   8, SECT_4K) },
 	{ "mx25l8005",   INFO(0xc22014, 0, 64 * 1024,  16, 0) },
 	{ "mx25l1606e",  INFO(0xc22015, 0, 64 * 1024,  32, SECT_4K) },
-	{ "mx25l3205d",  INFO(0xc22016, 0, 64 * 1024,  64, 0) },
+	{ "mx25l3205d",  INFO(0xc22016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "mx25l3255e",  INFO(0xc29e16, 0, 64 * 1024,  64, SECT_4K) },
-	{ "mx25l6405d",  INFO(0xc22017, 0, 64 * 1024, 128, 0) },
+	{ "mx25l6405d",  INFO(0xc22017, 0, 64 * 1024, 128, SECT_4K) },
 	{ "mx25u6435f",  INFO(0xc22537, 0, 64 * 1024, 128, SECT_4K) },
 	{ "mx25l12805d", INFO(0xc22018, 0, 64 * 1024, 256, 0) },
 	{ "mx25l12855e", INFO(0xc22618, 0, 64 * 1024, 256, 0) },
@@ -856,7 +887,7 @@ static const struct flash_info *spi_nor_read_id(struct spi_nor *nor)
 
 	tmp = nor->read_reg(nor, SPINOR_OP_RDID, id, SPI_NOR_MAX_ID_LEN);
 	if (tmp < 0) {
-		dev_dbg(nor->dev, " error %d reading JEDEC ID\n", tmp);
+		dev_dbg(nor->dev, "error %d reading JEDEC ID\n", tmp);
 		return ERR_PTR(tmp);
 	}
 
@@ -867,7 +898,7 @@ static const struct flash_info *spi_nor_read_id(struct spi_nor *nor)
 				return &spi_nor_ids[tmp];
 		}
 	}
-	dev_err(nor->dev, "unrecognized JEDEC id bytes: %02x, %2x, %2x\n",
+	dev_err(nor->dev, "unrecognized JEDEC id bytes: %02x, %02x, %02x\n",
 		id[0], id[1], id[2]);
 	return ERR_PTR(-ENODEV);
 }
@@ -1013,6 +1044,8 @@ static int macronix_quad_enable(struct spi_nor *nor)
 	int ret, val;
 
 	val = read_sr(nor);
+	if (val < 0)
+		return val;
 	write_enable(nor);
 
 	write_sr(nor, val | SR_QUAD_EN_MX);
@@ -1138,7 +1171,7 @@ static int set_quad_mode(struct spi_nor *nor, const struct flash_info *info)
 static int spi_nor_check(struct spi_nor *nor)
 {
 	if (!nor->dev || !nor->read || !nor->write ||
-		!nor->read_reg || !nor->write_reg || !nor->erase) {
+		!nor->read_reg || !nor->write_reg) {
 		pr_err("spi-nor: please fill all the necessary fields!\n");
 		return -EINVAL;
 	}
@@ -1151,7 +1184,7 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 	const struct flash_info *info = NULL;
 	struct device *dev = nor->dev;
 	struct mtd_info *mtd = &nor->mtd;
-	struct device_node *np = nor->flash_node;
+	struct device_node *np = spi_nor_get_flash_node(nor);
 	int ret;
 	int i;
 
@@ -1338,6 +1371,12 @@ int spi_nor_scan(struct spi_nor *nor, const char *name, enum read_mode mode)
 		nor->addr_width = 3;
 	}
 
+	if (nor->addr_width > SPI_NOR_MAX_ADDR_WIDTH) {
+		dev_err(dev, "address width is too large: %u\n",
+			nor->addr_width);
+		return -EINVAL;
+	}
+
 	nor->read_dummy = spi_nor_read_dummy_cycles(nor);
 
 	dev_info(dev, "%s (%lld Kbytes)\n", info->name,
diff --git a/drivers/mtd/tests/mtd_nandecctest.c b/drivers/mtd/tests/mtd_nandecctest.c
index 79316159eec6..88b6c81cebbe 100644
--- a/drivers/mtd/tests/mtd_nandecctest.c
+++ b/drivers/mtd/tests/mtd_nandecctest.c
@@ -187,7 +187,7 @@ static int double_bit_error_detect(void *error_data, void *error_ecc,
 	__nand_calculate_ecc(error_data, size, calc_ecc);
 	ret = __nand_correct_data(error_data, error_ecc, calc_ecc, size);
 
-	return (ret == -1) ? 0 : -EINVAL;
+	return (ret == -EBADMSG) ? 0 : -EINVAL;
 }
 
 static const struct nand_ecc_test nand_ecc_test[] = {
diff --git a/drivers/mtd/tests/pagetest.c b/drivers/mtd/tests/pagetest.c
index ba1890d5632c..ff1e0565b020 100644
--- a/drivers/mtd/tests/pagetest.c
+++ b/drivers/mtd/tests/pagetest.c
@@ -127,13 +127,12 @@ static int crosstest(void)
 	unsigned char *pp1, *pp2, *pp3, *pp4;
 
 	pr_info("crosstest\n");
-	pp1 = kmalloc(pgsize * 4, GFP_KERNEL);
+	pp1 = kzalloc(pgsize * 4, GFP_KERNEL);
 	if (!pp1)
 		return -ENOMEM;
 	pp2 = pp1 + pgsize;
 	pp3 = pp2 + pgsize;
 	pp4 = pp3 + pgsize;
-	memset(pp1, 0, pgsize * 4);
 
 	addr0 = 0;
 	for (i = 0; i < ebcnt && bbt[i]; ++i)
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index 54e056d3be02..ee2b74d1d1b5 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -174,9 +174,9 @@ static int vol_cdev_fsync(struct file *file, loff_t start, loff_t end,
 	struct ubi_device *ubi = desc->vol->ubi;
 	struct inode *inode = file_inode(file);
 	int err;
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	err = ubi_sync(ubi->ubi_num);
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 	return err;
 }
 
diff --git a/drivers/mtd/ubi/upd.c b/drivers/mtd/ubi/upd.c
index 2a1b6e037e1a..0134ba32a057 100644
--- a/drivers/mtd/ubi/upd.c
+++ b/drivers/mtd/ubi/upd.c
@@ -193,7 +193,7 @@ int ubi_start_leb_change(struct ubi_device *ubi, struct ubi_volume *vol,
 	vol->changing_leb = 1;
 	vol->ch_lnum = req->lnum;
 
-	vol->upd_buf = vmalloc(req->bytes);
+	vol->upd_buf = vmalloc(ALIGN((int)req->bytes, ubi->min_io_size));
 	if (!vol->upd_buf)
 		return -ENOMEM;
 
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 56065632a5b8..17ec948ac40e 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -628,6 +628,7 @@ static int do_sync_erase(struct ubi_device *ubi, struct ubi_wl_entry *e,
 	return __erase_worker(ubi, &wl_wrk);
 }
 
+static int ensure_wear_leveling(struct ubi_device *ubi, int nested);
 /**
  * wear_leveling_worker - wear-leveling worker function.
  * @ubi: UBI device description object
@@ -649,6 +650,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 #endif
 	struct ubi_wl_entry *e1, *e2;
 	struct ubi_vid_hdr *vid_hdr;
+	int dst_leb_clean = 0;
 
 	kfree(wrk);
 	if (shutdown)
@@ -753,6 +755,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 
 	err = ubi_io_read_vid_hdr(ubi, e1->pnum, vid_hdr, 0);
 	if (err && err != UBI_IO_BITFLIPS) {
+		dst_leb_clean = 1;
 		if (err == UBI_IO_FF) {
 			/*
 			 * We are trying to move PEB without a VID header. UBI
@@ -798,10 +801,12 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 			 * protection queue.
 			 */
 			protect = 1;
+			dst_leb_clean = 1;
 			goto out_not_moved;
 		}
 		if (err == MOVE_RETRY) {
 			scrubbing = 1;
+			dst_leb_clean = 1;
 			goto out_not_moved;
 		}
 		if (err == MOVE_TARGET_BITFLIPS || err == MOVE_TARGET_WR_ERR ||
@@ -827,6 +832,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 					ubi->erroneous_peb_count);
 				goto out_error;
 			}
+			dst_leb_clean = 1;
 			erroneous = 1;
 			goto out_not_moved;
 		}
@@ -897,15 +903,24 @@ out_not_moved:
 		wl_tree_add(e1, &ubi->scrub);
 	else
 		wl_tree_add(e1, &ubi->used);
+	if (dst_leb_clean) {
+		wl_tree_add(e2, &ubi->free);
+		ubi->free_count++;
+	}
+
 	ubi_assert(!ubi->move_to_put);
 	ubi->move_from = ubi->move_to = NULL;
 	ubi->wl_scheduled = 0;
 	spin_unlock(&ubi->wl_lock);
 
 	ubi_free_vid_hdr(ubi, vid_hdr);
-	err = do_sync_erase(ubi, e2, vol_id, lnum, torture);
-	if (err)
-		goto out_ro;
+	if (dst_leb_clean) {
+		ensure_wear_leveling(ubi, 1);
+	} else {
+		err = do_sync_erase(ubi, e2, vol_id, lnum, torture);
+		if (err)
+			goto out_ro;
+	}
 
 	mutex_unlock(&ubi->move_mutex);
 	return 0;
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index 940e2ebbdea8..4cbb8b27a891 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -93,7 +93,8 @@ enum ad_link_speed_type {
 	AD_LINK_SPEED_10000MBPS,
 	AD_LINK_SPEED_20000MBPS,
 	AD_LINK_SPEED_40000MBPS,
-	AD_LINK_SPEED_56000MBPS
+	AD_LINK_SPEED_56000MBPS,
+	AD_LINK_SPEED_100000MBPS,
 };
 
 /* compare MAC addresses */
@@ -258,6 +259,7 @@ static inline int __check_agg_selection_timer(struct port *port)
  *     %AD_LINK_SPEED_20000MBPS
  *     %AD_LINK_SPEED_40000MBPS
  *     %AD_LINK_SPEED_56000MBPS
+ *     %AD_LINK_SPEED_100000MBPS
  */
 static u16 __get_link_speed(struct port *port)
 {
@@ -305,6 +307,10 @@ static u16 __get_link_speed(struct port *port)
 			speed = AD_LINK_SPEED_56000MBPS;
 			break;
 
+		case SPEED_100000:
+			speed = AD_LINK_SPEED_100000MBPS;
+			break;
+
 		default:
 			/* unknown speed value from ethtool. shouldn't happen */
 			speed = 0;
@@ -681,6 +687,9 @@ static u32 __get_agg_bandwidth(struct aggregator *aggregator)
 		case AD_LINK_SPEED_56000MBPS:
 			bandwidth = aggregator->num_of_ports * 56000;
 			break;
+		case AD_LINK_SPEED_100000MBPS:
+			bandwidth = aggregator->num_of_ports * 100000;
+			break;
 		default:
 			bandwidth = 0; /* to silence the compiler */
 		}
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index 9e0f8a7ef8b1..b7f1a9919033 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -214,6 +214,8 @@ static void bond_uninit(struct net_device *bond_dev);
 static struct rtnl_link_stats64 *bond_get_stats(struct net_device *bond_dev,
 						struct rtnl_link_stats64 *stats);
 static void bond_slave_arr_handler(struct work_struct *work);
+static bool bond_time_in_interval(struct bonding *bond, unsigned long last_act,
+				  int mod);
 
 /*---------------------------- General routines -----------------------------*/
 
@@ -830,7 +832,8 @@ void bond_change_active_slave(struct bonding *bond, struct slave *new_active)
 			}
 
 			new_active->delay = 0;
-			bond_set_slave_link_state(new_active, BOND_LINK_UP);
+			bond_set_slave_link_state(new_active, BOND_LINK_UP,
+						  BOND_SLAVE_NOTIFY_NOW);
 
 			if (BOND_MODE(bond) == BOND_MODE_8023AD)
 				bond_3ad_handle_link_change(new_active, BOND_LINK_UP);
@@ -1066,12 +1069,12 @@ static netdev_features_t bond_fix_features(struct net_device *dev,
 	return features;
 }
 
-#define BOND_VLAN_FEATURES	(NETIF_F_ALL_CSUM | NETIF_F_SG | \
+#define BOND_VLAN_FEATURES	(NETIF_F_HW_CSUM | NETIF_F_SG | \
 				 NETIF_F_FRAGLIST | NETIF_F_ALL_TSO | \
 				 NETIF_F_HIGHDMA | NETIF_F_LRO)
 
-#define BOND_ENC_FEATURES	(NETIF_F_ALL_CSUM | NETIF_F_SG | NETIF_F_RXCSUM |\
-				 NETIF_F_ALL_TSO)
+#define BOND_ENC_FEATURES	(NETIF_F_HW_CSUM | NETIF_F_SG | \
+				 NETIF_F_RXCSUM | NETIF_F_ALL_TSO)
 
 static void bond_compute_features(struct bonding *bond)
 {
@@ -1198,26 +1201,42 @@ static rx_handler_result_t bond_handle_frame(struct sk_buff **pskb)
 	return ret;
 }
 
-static int bond_master_upper_dev_link(struct net_device *bond_dev,
-				      struct net_device *slave_dev,
-				      struct slave *slave)
+static enum netdev_lag_tx_type bond_lag_tx_type(struct bonding *bond)
 {
+	switch (BOND_MODE(bond)) {
+	case BOND_MODE_ROUNDROBIN:
+		return NETDEV_LAG_TX_TYPE_ROUNDROBIN;
+	case BOND_MODE_ACTIVEBACKUP:
+		return NETDEV_LAG_TX_TYPE_ACTIVEBACKUP;
+	case BOND_MODE_BROADCAST:
+		return NETDEV_LAG_TX_TYPE_BROADCAST;
+	case BOND_MODE_XOR:
+	case BOND_MODE_8023AD:
+		return NETDEV_LAG_TX_TYPE_HASH;
+	default:
+		return NETDEV_LAG_TX_TYPE_UNKNOWN;
+	}
+}
+
+static int bond_master_upper_dev_link(struct bonding *bond, struct slave *slave)
+{
+	struct netdev_lag_upper_info lag_upper_info;
 	int err;
 
-	err = netdev_master_upper_dev_link_private(slave_dev, bond_dev, slave);
+	lag_upper_info.tx_type = bond_lag_tx_type(bond);
+	err = netdev_master_upper_dev_link(slave->dev, bond->dev, slave,
+					   &lag_upper_info);
 	if (err)
 		return err;
-	slave_dev->flags |= IFF_SLAVE;
-	rtmsg_ifinfo(RTM_NEWLINK, slave_dev, IFF_SLAVE, GFP_KERNEL);
+	rtmsg_ifinfo(RTM_NEWLINK, slave->dev, IFF_SLAVE, GFP_KERNEL);
 	return 0;
 }
 
-static void bond_upper_dev_unlink(struct net_device *bond_dev,
-				  struct net_device *slave_dev)
+static void bond_upper_dev_unlink(struct bonding *bond, struct slave *slave)
 {
-	netdev_upper_dev_unlink(slave_dev, bond_dev);
-	slave_dev->flags &= ~IFF_SLAVE;
-	rtmsg_ifinfo(RTM_NEWLINK, slave_dev, IFF_SLAVE, GFP_KERNEL);
+	netdev_upper_dev_unlink(slave->dev, bond->dev);
+	slave->dev->flags &= ~IFF_SLAVE;
+	rtmsg_ifinfo(RTM_NEWLINK, slave->dev, IFF_SLAVE, GFP_KERNEL);
 }
 
 static struct slave *bond_alloc_slave(struct bonding *bond)
@@ -1299,6 +1318,16 @@ void bond_queue_slave_event(struct slave *slave)
 	queue_delayed_work(slave->bond->wq, &nnw->work, 0);
 }
 
+void bond_lower_state_changed(struct slave *slave)
+{
+	struct netdev_lag_lower_state_info info;
+
+	info.link_up = slave->link == BOND_LINK_UP ||
+		       slave->link == BOND_LINK_FAIL;
+	info.tx_enabled = bond_is_active_slave(slave);
+	netdev_lower_state_changed(slave->dev, &info);
+}
+
 /* enslave device <slave> to bond device <master> */
 int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 {
@@ -1351,7 +1380,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	 * the current ifenslave will set the interface down prior to
 	 * enslaving it; the old ifenslave will not.
 	 */
-	if ((slave_dev->flags & IFF_UP)) {
+	if (slave_dev->flags & IFF_UP) {
 		netdev_err(bond_dev, "%s is up - this may be due to an out of date ifenslave\n",
 			   slave_dev->name);
 		res = -EPERM;
@@ -1465,6 +1494,9 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		}
 	}
 
+	/* set slave flag before open to prevent IPv6 addrconf */
+	slave_dev->flags |= IFF_SLAVE;
+
 	/* open the slave since the application closed it */
 	res = dev_open(slave_dev);
 	if (res) {
@@ -1563,21 +1595,26 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		if (bond_check_dev_link(bond, slave_dev, 0) == BMSR_LSTATUS) {
 			if (bond->params.updelay) {
 				bond_set_slave_link_state(new_slave,
-							  BOND_LINK_BACK);
+							  BOND_LINK_BACK,
+							  BOND_SLAVE_NOTIFY_NOW);
 				new_slave->delay = bond->params.updelay;
 			} else {
 				bond_set_slave_link_state(new_slave,
-							  BOND_LINK_UP);
+							  BOND_LINK_UP,
+							  BOND_SLAVE_NOTIFY_NOW);
 			}
 		} else {
-			bond_set_slave_link_state(new_slave, BOND_LINK_DOWN);
+			bond_set_slave_link_state(new_slave, BOND_LINK_DOWN,
+						  BOND_SLAVE_NOTIFY_NOW);
 		}
 	} else if (bond->params.arp_interval) {
 		bond_set_slave_link_state(new_slave,
 					  (netif_carrier_ok(slave_dev) ?
-					  BOND_LINK_UP : BOND_LINK_DOWN));
+					  BOND_LINK_UP : BOND_LINK_DOWN),
+					  BOND_SLAVE_NOTIFY_NOW);
 	} else {
-		bond_set_slave_link_state(new_slave, BOND_LINK_UP);
+		bond_set_slave_link_state(new_slave, BOND_LINK_UP,
+					  BOND_SLAVE_NOTIFY_NOW);
 	}
 
 	if (new_slave->link != BOND_LINK_DOWN)
@@ -1662,7 +1699,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		goto err_detach;
 	}
 
-	res = bond_master_upper_dev_link(bond_dev, slave_dev, new_slave);
+	res = bond_master_upper_dev_link(bond, new_slave);
 	if (res) {
 		netdev_dbg(bond_dev, "Error %d calling bond_master_upper_dev_link\n", res);
 		goto err_unregister;
@@ -1698,7 +1735,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 /* Undo stages on error */
 err_upper_unlink:
-	bond_upper_dev_unlink(bond_dev, slave_dev);
+	bond_upper_dev_unlink(bond, new_slave);
 
 err_unregister:
 	netdev_rx_handler_unregister(slave_dev);
@@ -1725,6 +1762,7 @@ err_close:
 	dev_close(slave_dev);
 
 err_restore_mac:
+	slave_dev->flags &= ~IFF_SLAVE;
 	if (!bond->params.fail_over_mac ||
 	    BOND_MODE(bond) != BOND_MODE_ACTIVEBACKUP) {
 		/* XXX TODO - fom follow mode needs to change master's
@@ -1799,12 +1837,14 @@ static int __bond_release_one(struct net_device *bond_dev,
 		return -EINVAL;
 	}
 
+	bond_set_slave_inactive_flags(slave, BOND_SLAVE_NOTIFY_NOW);
+
 	bond_sysfs_slave_del(slave);
 
 	/* recompute stats just before removing the slave */
 	bond_get_stats(bond->dev, &bond->bond_stats);
 
-	bond_upper_dev_unlink(bond_dev, slave_dev);
+	bond_upper_dev_unlink(bond, slave);
 	/* unregister rx_handler early so bond_handle_frame wouldn't be called
 	 * for this slave anymore.
 	 */
@@ -1996,7 +2036,8 @@ static int bond_miimon_inspect(struct bonding *bond)
 			if (link_state)
 				continue;
 
-			bond_set_slave_link_state(slave, BOND_LINK_FAIL);
+			bond_set_slave_link_state(slave, BOND_LINK_FAIL,
+						  BOND_SLAVE_NOTIFY_LATER);
 			slave->delay = bond->params.downdelay;
 			if (slave->delay) {
 				netdev_info(bond->dev, "link status down for %sinterface %s, disabling it in %d ms\n",
@@ -2011,7 +2052,8 @@ static int bond_miimon_inspect(struct bonding *bond)
 		case BOND_LINK_FAIL:
 			if (link_state) {
 				/* recovered before downdelay expired */
-				bond_set_slave_link_state(slave, BOND_LINK_UP);
+				bond_set_slave_link_state(slave, BOND_LINK_UP,
+							  BOND_SLAVE_NOTIFY_LATER);
 				slave->last_link_up = jiffies;
 				netdev_info(bond->dev, "link status up again after %d ms for interface %s\n",
 					    (bond->params.downdelay - slave->delay) *
@@ -2033,7 +2075,8 @@ static int bond_miimon_inspect(struct bonding *bond)
 			if (!link_state)
 				continue;
 
-			bond_set_slave_link_state(slave, BOND_LINK_BACK);
+			bond_set_slave_link_state(slave, BOND_LINK_BACK,
+						  BOND_SLAVE_NOTIFY_LATER);
 			slave->delay = bond->params.updelay;
 
 			if (slave->delay) {
@@ -2047,7 +2090,8 @@ static int bond_miimon_inspect(struct bonding *bond)
 		case BOND_LINK_BACK:
 			if (!link_state) {
 				bond_set_slave_link_state(slave,
-							  BOND_LINK_DOWN);
+							  BOND_LINK_DOWN,
+							  BOND_SLAVE_NOTIFY_LATER);
 				netdev_info(bond->dev, "link status down again after %d ms for interface %s\n",
 					    (bond->params.updelay - slave->delay) *
 					    bond->params.miimon,
@@ -2085,7 +2129,9 @@ static void bond_miimon_commit(struct bonding *bond)
 			continue;
 
 		case BOND_LINK_UP:
-			bond_set_slave_link_state(slave, BOND_LINK_UP);
+			bond_update_speed_duplex(slave);
+			bond_set_slave_link_state(slave, BOND_LINK_UP,
+						  BOND_SLAVE_NOTIFY_NOW);
 			slave->last_link_up = jiffies;
 
 			primary = rtnl_dereference(bond->primary_slave);
@@ -2125,7 +2171,8 @@ static void bond_miimon_commit(struct bonding *bond)
 			if (slave->link_failure_count < UINT_MAX)
 				slave->link_failure_count++;
 
-			bond_set_slave_link_state(slave, BOND_LINK_DOWN);
+			bond_set_slave_link_state(slave, BOND_LINK_DOWN,
+						  BOND_SLAVE_NOTIFY_NOW);
 
 			if (BOND_MODE(bond) == BOND_MODE_ACTIVEBACKUP ||
 			    BOND_MODE(bond) == BOND_MODE_8023AD)
@@ -2415,7 +2462,7 @@ int bond_arp_rcv(const struct sk_buff *skb, struct bonding *bond,
 		 struct slave *slave)
 {
 	struct arphdr *arp = (struct arphdr *)skb->data;
-	struct slave *curr_active_slave;
+	struct slave *curr_active_slave, *curr_arp_slave;
 	unsigned char *arp_ptr;
 	__be32 sip, tip;
 	int alen, is_arp = skb->protocol == __cpu_to_be16(ETH_P_ARP);
@@ -2462,26 +2509,41 @@ int bond_arp_rcv(const struct sk_buff *skb, struct bonding *bond,
 		     &sip, &tip);
 
 	curr_active_slave = rcu_dereference(bond->curr_active_slave);
+	curr_arp_slave = rcu_dereference(bond->current_arp_slave);
 
-	/* Backup slaves won't see the ARP reply, but do come through
-	 * here for each ARP probe (so we swap the sip/tip to validate
-	 * the probe).  In a "redundant switch, common router" type of
-	 * configuration, the ARP probe will (hopefully) travel from
-	 * the active, through one switch, the router, then the other
-	 * switch before reaching the backup.
+	/* We 'trust' the received ARP enough to validate it if:
 	 *
-	 * We 'trust' the arp requests if there is an active slave and
-	 * it received valid arp reply(s) after it became active. This
-	 * is done to avoid endless looping when we can't reach the
+	 * (a) the slave receiving the ARP is active (which includes the
+	 * current ARP slave, if any), or
+	 *
+	 * (b) the receiving slave isn't active, but there is a currently
+	 * active slave and it received valid arp reply(s) after it became
+	 * the currently active slave, or
+	 *
+	 * (c) there is an ARP slave that sent an ARP during the prior ARP
+	 * interval, and we receive an ARP reply on any slave.  We accept
+	 * these because switch FDB update delays may deliver the ARP
+	 * reply to a slave other than the sender of the ARP request.
+	 *
+	 * Note: for (b), backup slaves are receiving the broadcast ARP
+	 * request, not a reply.  This request passes from the sending
+	 * slave through the L2 switch(es) to the receiving slave.  Since
+	 * this is checking the request, sip/tip are swapped for
+	 * validation.
+	 *
+	 * This is done to avoid endless looping when we can't reach the
 	 * arp_ip_target and fool ourselves with our own arp requests.
 	 */
-
 	if (bond_is_active_slave(slave))
 		bond_validate_arp(bond, slave, sip, tip);
 	else if (curr_active_slave &&
 		 time_after(slave_last_rx(bond, curr_active_slave),
 			    curr_active_slave->last_link_up))
 		bond_validate_arp(bond, slave, tip, sip);
+	else if (curr_arp_slave && (arp->ar_op == htons(ARPOP_REPLY)) &&
+		 bond_time_in_interval(bond,
+				       dev_trans_start(curr_arp_slave->dev), 1))
+		bond_validate_arp(bond, slave, sip, tip);
 
 out_unlock:
 	if (arp != (struct arphdr *)skb->data)
@@ -2708,7 +2770,8 @@ static void bond_ab_arp_commit(struct bonding *bond)
 				struct slave *current_arp_slave;
 
 				current_arp_slave = rtnl_dereference(bond->current_arp_slave);
-				bond_set_slave_link_state(slave, BOND_LINK_UP);
+				bond_set_slave_link_state(slave, BOND_LINK_UP,
+							  BOND_SLAVE_NOTIFY_NOW);
 				if (current_arp_slave) {
 					bond_set_slave_inactive_flags(
 						current_arp_slave,
@@ -2731,7 +2794,8 @@ static void bond_ab_arp_commit(struct bonding *bond)
 			if (slave->link_failure_count < UINT_MAX)
 				slave->link_failure_count++;
 
-			bond_set_slave_link_state(slave, BOND_LINK_DOWN);
+			bond_set_slave_link_state(slave, BOND_LINK_DOWN,
+						  BOND_SLAVE_NOTIFY_NOW);
 			bond_set_slave_inactive_flags(slave,
 						      BOND_SLAVE_NOTIFY_NOW);
 
@@ -2810,7 +2874,8 @@ static bool bond_ab_arp_probe(struct bonding *bond)
 		 * up when it is actually down
 		 */
 		if (!bond_slave_is_up(slave) && slave->link == BOND_LINK_UP) {
-			bond_set_slave_link_state(slave, BOND_LINK_DOWN);
+			bond_set_slave_link_state(slave, BOND_LINK_DOWN,
+						  BOND_SLAVE_NOTIFY_LATER);
 			if (slave->link_failure_count < UINT_MAX)
 				slave->link_failure_count++;
 
@@ -2830,7 +2895,8 @@ static bool bond_ab_arp_probe(struct bonding *bond)
 	if (!new_slave)
 		goto check_state;
 
-	bond_set_slave_link_state(new_slave, BOND_LINK_BACK);
+	bond_set_slave_link_state(new_slave, BOND_LINK_BACK,
+				  BOND_SLAVE_NOTIFY_LATER);
 	bond_set_slave_active_flags(new_slave, BOND_SLAVE_NOTIFY_LATER);
 	bond_arp_send_all(bond, new_slave);
 	new_slave->last_link_up = jiffies;
@@ -2838,7 +2904,7 @@ static bool bond_ab_arp_probe(struct bonding *bond)
 
 check_state:
 	bond_for_each_slave_rcu(bond, slave, iter) {
-		if (slave->should_notify) {
+		if (slave->should_notify || slave->should_notify_link) {
 			should_notify_rtnl = BOND_SLAVE_NOTIFY_NOW;
 			break;
 		}
@@ -2893,8 +2959,10 @@ re_arm:
 		if (should_notify_peers)
 			call_netdevice_notifiers(NETDEV_NOTIFY_PEERS,
 						 bond->dev);
-		if (should_notify_rtnl)
+		if (should_notify_rtnl) {
 			bond_slave_state_notify(bond);
+			bond_slave_link_notify(bond);
+		}
 
 		rtnl_unlock();
 	}
@@ -4135,7 +4203,6 @@ void bond_setup(struct net_device *bond_dev)
 				NETIF_F_HW_VLAN_CTAG_RX |
 				NETIF_F_HW_VLAN_CTAG_FILTER;
 
-	bond_dev->hw_features &= ~(NETIF_F_ALL_CSUM & ~NETIF_F_HW_CSUM);
 	bond_dev->hw_features |= NETIF_F_GSO_ENCAP_ALL;
 	bond_dev->features |= bond_dev->hw_features;
 }
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index f4ae72086215..e23c3ed737de 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -42,7 +42,6 @@
 
 #include <net/bonding.h>
 
-#define to_dev(obj)	container_of(obj, struct device, kobj)
 #define to_bond(cd)	((struct bonding *)(netdev_priv(to_net_dev(cd))))
 
 /* "show" function for the bond_masters attribute.
@@ -481,7 +480,7 @@ static ssize_t bonding_show_mii_status(struct device *d,
 				       char *buf)
 {
 	struct bonding *bond = to_bond(d);
-	bool active = !!rcu_access_pointer(bond->curr_active_slave);
+	bool active = netif_carrier_ok(bond->dev);
 
 	return sprintf(buf, "%s\n", active ? "up" : "down");
 }
diff --git a/drivers/net/can/spi/mcp251x.c b/drivers/net/can/spi/mcp251x.c
index 575790e8a75a..74a7dfecee27 100644
--- a/drivers/net/can/spi/mcp251x.c
+++ b/drivers/net/can/spi/mcp251x.c
@@ -843,7 +843,7 @@ static irqreturn_t mcp251x_can_ist(int irq, void *dev_id)
 		if (clear_intf)
 			mcp251x_write_bits(spi, CANINTF, clear_intf, 0x00);
 
-		if (eflag)
+		if (eflag & (EFLG_RX0OVR | EFLG_RX1OVR))
 			mcp251x_write_bits(spi, EFLG, eflag, 0x00);
 
 		/* Update can state */
diff --git a/drivers/net/can/usb/ems_usb.c b/drivers/net/can/usb/ems_usb.c
index fc5b75675cd8..eb7192fab593 100644
--- a/drivers/net/can/usb/ems_usb.c
+++ b/drivers/net/can/usb/ems_usb.c
@@ -117,6 +117,9 @@ MODULE_LICENSE("GPL v2");
  */
 #define EMS_USB_ARM7_CLOCK 8000000
 
+#define CPC_TX_QUEUE_TRIGGER_LOW	25
+#define CPC_TX_QUEUE_TRIGGER_HIGH	35
+
 /*
  * CAN-Message representation in a CPC_MSG. Message object type is
  * CPC_MSG_TYPE_CAN_FRAME or CPC_MSG_TYPE_RTR_FRAME or
@@ -278,6 +281,11 @@ static void ems_usb_read_interrupt_callback(struct urb *urb)
 	switch (urb->status) {
 	case 0:
 		dev->free_slots = dev->intr_in_buffer[1];
+		if(dev->free_slots > CPC_TX_QUEUE_TRIGGER_HIGH){
+			if (netif_queue_stopped(netdev)){
+				netif_wake_queue(netdev);
+			}
+		}
 		break;
 
 	case -ECONNRESET: /* unlink */
@@ -526,8 +534,6 @@ static void ems_usb_write_bulk_callback(struct urb *urb)
 	/* Release context */
 	context->echo_index = MAX_TX_URBS;
 
-	if (netif_queue_stopped(netdev))
-		netif_wake_queue(netdev);
 }
 
 /*
@@ -587,7 +593,7 @@ static int ems_usb_start(struct ems_usb *dev)
 	int err, i;
 
 	dev->intr_in_buffer[0] = 0;
-	dev->free_slots = 15; /* initial size */
+	dev->free_slots = 50; /* initial size */
 
 	for (i = 0; i < MAX_RX_URBS; i++) {
 		struct urb *urb = NULL;
@@ -835,7 +841,7 @@ static netdev_tx_t ems_usb_start_xmit(struct sk_buff *skb, struct net_device *ne
 
 		/* Slow down tx path */
 		if (atomic_read(&dev->active_tx_urbs) >= MAX_TX_URBS ||
-		    dev->free_slots < 5) {
+		    dev->free_slots < CPC_TX_QUEUE_TRIGGER_LOW) {
 			netif_stop_queue(netdev);
 		}
 	}
diff --git a/drivers/net/can/usb/gs_usb.c b/drivers/net/can/usb/gs_usb.c
index 5eee62badf45..cbc99d5649af 100644
--- a/drivers/net/can/usb/gs_usb.c
+++ b/drivers/net/can/usb/gs_usb.c
@@ -826,9 +826,8 @@ static struct gs_can *gs_make_candev(unsigned int channel, struct usb_interface
 static void gs_destroy_candev(struct gs_can *dev)
 {
 	unregister_candev(dev->netdev);
-	free_candev(dev->netdev);
 	usb_kill_anchored_urbs(&dev->tx_submitted);
-	kfree(dev);
+	free_candev(dev->netdev);
 }
 
 static int gs_usb_probe(struct usb_interface *intf, const struct usb_device_id *id)
@@ -913,12 +912,15 @@ static int gs_usb_probe(struct usb_interface *intf, const struct usb_device_id *
 	for (i = 0; i < icount; i++) {
 		dev->canch[i] = gs_make_candev(i, intf);
 		if (IS_ERR_OR_NULL(dev->canch[i])) {
+			/* save error code to return later */
+			rc = PTR_ERR(dev->canch[i]);
+
 			/* on failure destroy previously created candevs */
 			icount = i;
-			for (i = 0; i < icount; i++) {
+			for (i = 0; i < icount; i++)
 				gs_destroy_candev(dev->canch[i]);
-				dev->canch[i] = NULL;
-			}
+
+			usb_kill_anchored_urbs(&dev->rx_submitted);
 			kfree(dev);
 			return rc;
 		}
@@ -939,16 +941,12 @@ static void gs_usb_disconnect(struct usb_interface *intf)
 		return;
 	}
 
-	for (i = 0; i < GS_MAX_INTF; i++) {
-		struct gs_can *can = dev->canch[i];
-
-		if (!can)
-			continue;
-
-		gs_destroy_candev(can);
-	}
+	for (i = 0; i < GS_MAX_INTF; i++)
+		if (dev->canch[i])
+			gs_destroy_candev(dev->canch[i]);
 
 	usb_kill_anchored_urbs(&dev->rx_submitted);
+	kfree(dev);
 }
 
 static const struct usb_device_id gs_usb_table[] = {
diff --git a/drivers/net/can/xilinx_can.c b/drivers/net/can/xilinx_can.c
index 51670b322409..c71a03593595 100644
--- a/drivers/net/can/xilinx_can.c
+++ b/drivers/net/can/xilinx_can.c
@@ -32,6 +32,7 @@
 #include <linux/can/dev.h>
 #include <linux/can/error.h>
 #include <linux/can/led.h>
+#include <linux/pm_runtime.h>
 
 #define DRIVER_NAME	"xilinx_can"
 
@@ -138,7 +139,7 @@ struct xcan_priv {
 	u32 (*read_reg)(const struct xcan_priv *priv, enum xcan_reg reg);
 	void (*write_reg)(const struct xcan_priv *priv, enum xcan_reg reg,
 			u32 val);
-	struct net_device *dev;
+	struct device *dev;
 	void __iomem *reg_base;
 	unsigned long irq_flags;
 	struct clk *bus_clk;
@@ -840,6 +841,13 @@ static int xcan_open(struct net_device *ndev)
 	struct xcan_priv *priv = netdev_priv(ndev);
 	int ret;
 
+	ret = pm_runtime_get_sync(priv->dev);
+	if (ret < 0) {
+		netdev_err(ndev, "%s: pm_runtime_get failed(%d)\n",
+				__func__, ret);
+		return ret;
+	}
+
 	ret = request_irq(ndev->irq, xcan_interrupt, priv->irq_flags,
 			ndev->name, ndev);
 	if (ret < 0) {
@@ -847,29 +855,17 @@ static int xcan_open(struct net_device *ndev)
 		goto err;
 	}
 
-	ret = clk_prepare_enable(priv->can_clk);
-	if (ret) {
-		netdev_err(ndev, "unable to enable device clock\n");
-		goto err_irq;
-	}
-
-	ret = clk_prepare_enable(priv->bus_clk);
-	if (ret) {
-		netdev_err(ndev, "unable to enable bus clock\n");
-		goto err_can_clk;
-	}
-
 	/* Set chip into reset mode */
 	ret = set_reset_mode(ndev);
 	if (ret < 0) {
 		netdev_err(ndev, "mode resetting failed!\n");
-		goto err_bus_clk;
+		goto err_irq;
 	}
 
 	/* Common open */
 	ret = open_candev(ndev);
 	if (ret)
-		goto err_bus_clk;
+		goto err_irq;
 
 	ret = xcan_chip_start(ndev);
 	if (ret < 0) {
@@ -885,13 +881,11 @@ static int xcan_open(struct net_device *ndev)
 
 err_candev:
 	close_candev(ndev);
-err_bus_clk:
-	clk_disable_unprepare(priv->bus_clk);
-err_can_clk:
-	clk_disable_unprepare(priv->can_clk);
 err_irq:
 	free_irq(ndev->irq, ndev);
 err:
+	pm_runtime_put(priv->dev);
+
 	return ret;
 }
 
@@ -908,12 +902,11 @@ static int xcan_close(struct net_device *ndev)
 	netif_stop_queue(ndev);
 	napi_disable(&priv->napi);
 	xcan_chip_stop(ndev);
-	clk_disable_unprepare(priv->bus_clk);
-	clk_disable_unprepare(priv->can_clk);
 	free_irq(ndev->irq, ndev);
 	close_candev(ndev);
 
 	can_led_event(ndev, CAN_LED_EVENT_STOP);
+	pm_runtime_put(priv->dev);
 
 	return 0;
 }
@@ -932,27 +925,20 @@ static int xcan_get_berr_counter(const struct net_device *ndev,
 	struct xcan_priv *priv = netdev_priv(ndev);
 	int ret;
 
-	ret = clk_prepare_enable(priv->can_clk);
-	if (ret)
-		goto err;
-
-	ret = clk_prepare_enable(priv->bus_clk);
-	if (ret)
-		goto err_clk;
+	ret = pm_runtime_get_sync(priv->dev);
+	if (ret < 0) {
+		netdev_err(ndev, "%s: pm_runtime_get failed(%d)\n",
+				__func__, ret);
+		return ret;
+	}
 
 	bec->txerr = priv->read_reg(priv, XCAN_ECR_OFFSET) & XCAN_ECR_TEC_MASK;
 	bec->rxerr = ((priv->read_reg(priv, XCAN_ECR_OFFSET) &
 			XCAN_ECR_REC_MASK) >> XCAN_ESR_REC_SHIFT);
 
-	clk_disable_unprepare(priv->bus_clk);
-	clk_disable_unprepare(priv->can_clk);
+	pm_runtime_put(priv->dev);
 
 	return 0;
-
-err_clk:
-	clk_disable_unprepare(priv->can_clk);
-err:
-	return ret;
 }
 
 
@@ -965,15 +951,45 @@ static const struct net_device_ops xcan_netdev_ops = {
 
 /**
  * xcan_suspend - Suspend method for the driver
- * @dev:	Address of the platform_device structure
+ * @dev:	Address of the device structure
  *
  * Put the driver into low power mode.
- * Return: 0 always
+ * Return: 0 on success and failure value on error
  */
 static int __maybe_unused xcan_suspend(struct device *dev)
 {
-	struct platform_device *pdev = dev_get_drvdata(dev);
-	struct net_device *ndev = platform_get_drvdata(pdev);
+	if (!device_may_wakeup(dev))
+		return pm_runtime_force_suspend(dev);
+
+	return 0;
+}
+
+/**
+ * xcan_resume - Resume from suspend
+ * @dev:	Address of the device structure
+ *
+ * Resume operation after suspend.
+ * Return: 0 on success and failure value on error
+ */
+static int __maybe_unused xcan_resume(struct device *dev)
+{
+	if (!device_may_wakeup(dev))
+		return pm_runtime_force_resume(dev);
+
+	return 0;
+
+}
+
+/**
+ * xcan_runtime_suspend - Runtime suspend method for the driver
+ * @dev:	Address of the device structure
+ *
+ * Put the driver into low power mode.
+ * Return: 0 always
+ */
+static int __maybe_unused xcan_runtime_suspend(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
 	struct xcan_priv *priv = netdev_priv(ndev);
 
 	if (netif_running(ndev)) {
@@ -984,43 +1000,55 @@ static int __maybe_unused xcan_suspend(struct device *dev)
 	priv->write_reg(priv, XCAN_MSR_OFFSET, XCAN_MSR_SLEEP_MASK);
 	priv->can.state = CAN_STATE_SLEEPING;
 
-	clk_disable(priv->bus_clk);
-	clk_disable(priv->can_clk);
+	clk_disable_unprepare(priv->bus_clk);
+	clk_disable_unprepare(priv->can_clk);
 
 	return 0;
 }
 
 /**
- * xcan_resume - Resume from suspend
- * @dev:	Address of the platformdevice structure
+ * xcan_runtime_resume - Runtime resume from suspend
+ * @dev:	Address of the device structure
  *
  * Resume operation after suspend.
  * Return: 0 on success and failure value on error
  */
-static int __maybe_unused xcan_resume(struct device *dev)
+static int __maybe_unused xcan_runtime_resume(struct device *dev)
 {
-	struct platform_device *pdev = dev_get_drvdata(dev);
-	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct net_device *ndev = dev_get_drvdata(dev);
 	struct xcan_priv *priv = netdev_priv(ndev);
 	int ret;
+	u32 isr, status;
 
-	ret = clk_enable(priv->bus_clk);
+	ret = clk_prepare_enable(priv->bus_clk);
 	if (ret) {
 		dev_err(dev, "Cannot enable clock.\n");
 		return ret;
 	}
-	ret = clk_enable(priv->can_clk);
+	ret = clk_prepare_enable(priv->can_clk);
 	if (ret) {
 		dev_err(dev, "Cannot enable clock.\n");
 		clk_disable_unprepare(priv->bus_clk);
 		return ret;
 	}
 
-	priv->write_reg(priv, XCAN_MSR_OFFSET, 0);
-	priv->write_reg(priv, XCAN_SRR_OFFSET, XCAN_SRR_CEN_MASK);
-	priv->can.state = CAN_STATE_ERROR_ACTIVE;
+	priv->write_reg(priv, XCAN_SRR_OFFSET, XCAN_SRR_RESET_MASK);
+	isr = priv->read_reg(priv, XCAN_ISR_OFFSET);
+	status = priv->read_reg(priv, XCAN_SR_OFFSET);
 
 	if (netif_running(ndev)) {
+		if (isr & XCAN_IXR_BSOFF_MASK) {
+			priv->can.state = CAN_STATE_BUS_OFF;
+			priv->write_reg(priv, XCAN_SRR_OFFSET,
+					XCAN_SRR_RESET_MASK);
+		} else if ((status & XCAN_SR_ESTAT_MASK) ==
+					XCAN_SR_ESTAT_MASK) {
+			priv->can.state = CAN_STATE_ERROR_PASSIVE;
+		} else if (status & XCAN_SR_ERRWRN_MASK) {
+			priv->can.state = CAN_STATE_ERROR_WARNING;
+		} else {
+			priv->can.state = CAN_STATE_ERROR_ACTIVE;
+		}
 		netif_device_attach(ndev);
 		netif_start_queue(ndev);
 	}
@@ -1028,7 +1056,10 @@ static int __maybe_unused xcan_resume(struct device *dev)
 	return 0;
 }
 
-static SIMPLE_DEV_PM_OPS(xcan_dev_pm_ops, xcan_suspend, xcan_resume);
+static const struct dev_pm_ops xcan_dev_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(xcan_suspend, xcan_resume)
+	SET_RUNTIME_PM_OPS(xcan_runtime_suspend, xcan_runtime_resume, NULL)
+};
 
 /**
  * xcan_probe - Platform registration call
@@ -1069,7 +1100,7 @@ static int xcan_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	priv = netdev_priv(ndev);
-	priv->dev = ndev;
+	priv->dev = &pdev->dev;
 	priv->can.bittiming_const = &xcan_bittiming_const;
 	priv->can.do_set_mode = xcan_do_set_mode;
 	priv->can.do_get_berr_counter = xcan_get_berr_counter;
@@ -1111,21 +1142,17 @@ static int xcan_probe(struct platform_device *pdev)
 		}
 	}
 
-	ret = clk_prepare_enable(priv->can_clk);
-	if (ret) {
-		dev_err(&pdev->dev, "unable to enable device clock\n");
-		goto err_free;
-	}
-
-	ret = clk_prepare_enable(priv->bus_clk);
-	if (ret) {
-		dev_err(&pdev->dev, "unable to enable bus clock\n");
-		goto err_unprepare_disable_dev;
-	}
-
 	priv->write_reg = xcan_write_reg_le;
 	priv->read_reg = xcan_read_reg_le;
 
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0) {
+		netdev_err(ndev, "%s: pm_runtime_get failed(%d)\n",
+			__func__, ret);
+		goto err_pmdisable;
+	}
+
 	if (priv->read_reg(priv, XCAN_SR_OFFSET) != XCAN_SR_CONFIG_MASK) {
 		priv->write_reg = xcan_write_reg_be;
 		priv->read_reg = xcan_read_reg_be;
@@ -1138,22 +1165,23 @@ static int xcan_probe(struct platform_device *pdev)
 	ret = register_candev(ndev);
 	if (ret) {
 		dev_err(&pdev->dev, "fail to register failed (err=%d)\n", ret);
-		goto err_unprepare_disable_busclk;
+		goto err_disableclks;
 	}
 
 	devm_can_led_init(ndev);
-	clk_disable_unprepare(priv->bus_clk);
-	clk_disable_unprepare(priv->can_clk);
+
+	pm_runtime_put(&pdev->dev);
+
 	netdev_dbg(ndev, "reg_base=0x%p irq=%d clock=%d, tx fifo depth:%d\n",
 			priv->reg_base, ndev->irq, priv->can.clock.freq,
 			priv->tx_max);
 
 	return 0;
 
-err_unprepare_disable_busclk:
-	clk_disable_unprepare(priv->bus_clk);
-err_unprepare_disable_dev:
-	clk_disable_unprepare(priv->can_clk);
+err_disableclks:
+	pm_runtime_put(priv->dev);
+err_pmdisable:
+	pm_runtime_disable(&pdev->dev);
 err_free:
 	free_candev(ndev);
 err:
@@ -1172,10 +1200,8 @@ static int xcan_remove(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct xcan_priv *priv = netdev_priv(ndev);
 
-	if (set_reset_mode(ndev) < 0)
-		netdev_err(ndev, "mode resetting failed!\n");
-
 	unregister_candev(ndev);
+	pm_runtime_disable(&pdev->dev);
 	netif_napi_del(&priv->napi);
 	free_candev(ndev);
 
diff --git a/drivers/net/dsa/mv88e6352.c b/drivers/net/dsa/mv88e6352.c
index cc6c54553418..a47f52f44b0d 100644
--- a/drivers/net/dsa/mv88e6352.c
+++ b/drivers/net/dsa/mv88e6352.c
@@ -25,6 +25,7 @@
 static const struct mv88e6xxx_switch_id mv88e6352_table[] = {
 	{ PORT_SWITCH_ID_6172, "Marvell 88E6172" },
 	{ PORT_SWITCH_ID_6176, "Marvell 88E6176" },
+	{ PORT_SWITCH_ID_6240, "Marvell 88E6240" },
 	{ PORT_SWITCH_ID_6320, "Marvell 88E6320" },
 	{ PORT_SWITCH_ID_6320_A1, "Marvell 88E6320 (A1)" },
 	{ PORT_SWITCH_ID_6320_A2, "Marvell 88e6320 (A2)" },
diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index b06dba05594a..512c8c0be1b4 100644
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -19,6 +19,7 @@
 #include <linux/list.h>
 #include <linux/module.h>
 #include <linux/netdevice.h>
+#include <linux/gpio/consumer.h>
 #include <linux/phy.h>
 #include <net/dsa.h>
 #include <net/switchdev.h>
@@ -616,98 +617,112 @@ static void _mv88e6xxx_stats_read(struct dsa_switch *ds, int stat, u32 *val)
 }
 
 static struct mv88e6xxx_hw_stat mv88e6xxx_hw_stats[] = {
-	{ "in_good_octets", 8, 0x00, },
-	{ "in_bad_octets", 4, 0x02, },
-	{ "in_unicast", 4, 0x04, },
-	{ "in_broadcasts", 4, 0x06, },
-	{ "in_multicasts", 4, 0x07, },
-	{ "in_pause", 4, 0x16, },
-	{ "in_undersize", 4, 0x18, },
-	{ "in_fragments", 4, 0x19, },
-	{ "in_oversize", 4, 0x1a, },
-	{ "in_jabber", 4, 0x1b, },
-	{ "in_rx_error", 4, 0x1c, },
-	{ "in_fcs_error", 4, 0x1d, },
-	{ "out_octets", 8, 0x0e, },
-	{ "out_unicast", 4, 0x10, },
-	{ "out_broadcasts", 4, 0x13, },
-	{ "out_multicasts", 4, 0x12, },
-	{ "out_pause", 4, 0x15, },
-	{ "excessive", 4, 0x11, },
-	{ "collisions", 4, 0x1e, },
-	{ "deferred", 4, 0x05, },
-	{ "single", 4, 0x14, },
-	{ "multiple", 4, 0x17, },
-	{ "out_fcs_error", 4, 0x03, },
-	{ "late", 4, 0x1f, },
-	{ "hist_64bytes", 4, 0x08, },
-	{ "hist_65_127bytes", 4, 0x09, },
-	{ "hist_128_255bytes", 4, 0x0a, },
-	{ "hist_256_511bytes", 4, 0x0b, },
-	{ "hist_512_1023bytes", 4, 0x0c, },
-	{ "hist_1024_max_bytes", 4, 0x0d, },
-	/* Not all devices have the following counters */
-	{ "sw_in_discards", 4, 0x110, },
-	{ "sw_in_filtered", 2, 0x112, },
-	{ "sw_out_filtered", 2, 0x113, },
-
+	{ "in_good_octets",	8, 0x00, BANK0, },
+	{ "in_bad_octets",	4, 0x02, BANK0, },
+	{ "in_unicast",		4, 0x04, BANK0, },
+	{ "in_broadcasts",	4, 0x06, BANK0, },
+	{ "in_multicasts",	4, 0x07, BANK0, },
+	{ "in_pause",		4, 0x16, BANK0, },
+	{ "in_undersize",	4, 0x18, BANK0, },
+	{ "in_fragments",	4, 0x19, BANK0, },
+	{ "in_oversize",	4, 0x1a, BANK0, },
+	{ "in_jabber",		4, 0x1b, BANK0, },
+	{ "in_rx_error",	4, 0x1c, BANK0, },
+	{ "in_fcs_error",	4, 0x1d, BANK0, },
+	{ "out_octets",		8, 0x0e, BANK0, },
+	{ "out_unicast",	4, 0x10, BANK0, },
+	{ "out_broadcasts",	4, 0x13, BANK0, },
+	{ "out_multicasts",	4, 0x12, BANK0, },
+	{ "out_pause",		4, 0x15, BANK0, },
+	{ "excessive",		4, 0x11, BANK0, },
+	{ "collisions",		4, 0x1e, BANK0, },
+	{ "deferred",		4, 0x05, BANK0, },
+	{ "single",		4, 0x14, BANK0, },
+	{ "multiple",		4, 0x17, BANK0, },
+	{ "out_fcs_error",	4, 0x03, BANK0, },
+	{ "late",		4, 0x1f, BANK0, },
+	{ "hist_64bytes",	4, 0x08, BANK0, },
+	{ "hist_65_127bytes",	4, 0x09, BANK0, },
+	{ "hist_128_255bytes",	4, 0x0a, BANK0, },
+	{ "hist_256_511bytes",	4, 0x0b, BANK0, },
+	{ "hist_512_1023bytes", 4, 0x0c, BANK0, },
+	{ "hist_1024_max_bytes", 4, 0x0d, BANK0, },
+	{ "sw_in_discards",	4, 0x10, PORT, },
+	{ "sw_in_filtered",	2, 0x12, PORT, },
+	{ "sw_out_filtered",	2, 0x13, PORT, },
+	{ "in_discards",	4, 0x00 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_filtered",	4, 0x01 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_accepted",	4, 0x02 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_bad_accepted",	4, 0x03 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_good_avb_class_a", 4, 0x04 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_good_avb_class_b", 4, 0x05 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_bad_avb_class_a", 4, 0x06 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_bad_avb_class_b", 4, 0x07 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "tcam_counter_0",	4, 0x08 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "tcam_counter_1",	4, 0x09 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "tcam_counter_2",	4, 0x0a | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "tcam_counter_3",	4, 0x0b | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_da_unknown",	4, 0x0e | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "in_management",	4, 0x0f | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_0",	4, 0x10 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_1",	4, 0x11 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_2",	4, 0x12 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_3",	4, 0x13 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_4",	4, 0x14 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_5",	4, 0x15 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_6",	4, 0x16 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_queue_7",	4, 0x17 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_cut_through",	4, 0x18 | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_octets_a",	4, 0x1a | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_octets_b",	4, 0x1b | GLOBAL_STATS_OP_BANK_1, BANK1, },
+	{ "out_management",	4, 0x1f | GLOBAL_STATS_OP_BANK_1, BANK1, },
 };
 
-static bool have_sw_in_discards(struct dsa_switch *ds)
+static bool mv88e6xxx_has_stat(struct dsa_switch *ds,
+			       struct mv88e6xxx_hw_stat *stat)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-
-	switch (ps->id) {
-	case PORT_SWITCH_ID_6095: case PORT_SWITCH_ID_6161:
-	case PORT_SWITCH_ID_6165: case PORT_SWITCH_ID_6171:
-	case PORT_SWITCH_ID_6172: case PORT_SWITCH_ID_6176:
-	case PORT_SWITCH_ID_6182: case PORT_SWITCH_ID_6185:
-	case PORT_SWITCH_ID_6352:
+	switch (stat->type) {
+	case BANK0:
 		return true;
-	default:
-		return false;
-	}
-}
-
-static void _mv88e6xxx_get_strings(struct dsa_switch *ds,
-				   int nr_stats,
-				   struct mv88e6xxx_hw_stat *stats,
-				   int port, uint8_t *data)
-{
-	int i;
-
-	for (i = 0; i < nr_stats; i++) {
-		memcpy(data + i * ETH_GSTRING_LEN,
-		       stats[i].string, ETH_GSTRING_LEN);
+	case BANK1:
+		return mv88e6xxx_6320_family(ds);
+	case PORT:
+		return mv88e6xxx_6095_family(ds) ||
+			mv88e6xxx_6185_family(ds) ||
+			mv88e6xxx_6097_family(ds) ||
+			mv88e6xxx_6165_family(ds) ||
+			mv88e6xxx_6351_family(ds) ||
+			mv88e6xxx_6352_family(ds);
 	}
+	return false;
 }
 
 static uint64_t _mv88e6xxx_get_ethtool_stat(struct dsa_switch *ds,
-					    int stat,
-					    struct mv88e6xxx_hw_stat *stats,
+					    struct mv88e6xxx_hw_stat *s,
 					    int port)
 {
-	struct mv88e6xxx_hw_stat *s = stats + stat;
 	u32 low;
 	u32 high = 0;
 	int ret;
 	u64 value;
 
-	if (s->reg >= 0x100) {
-		ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
-					  s->reg - 0x100);
+	switch (s->type) {
+	case PORT:
+		ret = _mv88e6xxx_reg_read(ds, REG_PORT(port), s->reg);
 		if (ret < 0)
 			return UINT64_MAX;
 
 		low = ret;
 		if (s->sizeof_stat == 4) {
 			ret = _mv88e6xxx_reg_read(ds, REG_PORT(port),
-						  s->reg - 0x100 + 1);
+						  s->reg + 1);
 			if (ret < 0)
 				return UINT64_MAX;
 			high = ret;
 		}
-	} else {
+		break;
+	case BANK0:
+	case BANK1:
 		_mv88e6xxx_stats_read(ds, s->reg, &low);
 		if (s->sizeof_stat == 8)
 			_mv88e6xxx_stats_read(ds, s->reg + 1, &high);
@@ -716,61 +731,59 @@ static uint64_t _mv88e6xxx_get_ethtool_stat(struct dsa_switch *ds,
 	return value;
 }
 
-static void _mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
-					 int nr_stats,
-					 struct mv88e6xxx_hw_stat *stats,
-					 int port, uint64_t *data)
+void mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	int ret;
-	int i;
-
-	mutex_lock(&ps->smi_mutex);
+	struct mv88e6xxx_hw_stat *stat;
+	int i, j;
 
-	ret = _mv88e6xxx_stats_snapshot(ds, port);
-	if (ret < 0) {
-		mutex_unlock(&ps->smi_mutex);
-		return;
+	for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
+		stat = &mv88e6xxx_hw_stats[i];
+		if (mv88e6xxx_has_stat(ds, stat)) {
+			memcpy(data + j * ETH_GSTRING_LEN, stat->string,
+			       ETH_GSTRING_LEN);
+			j++;
+		}
 	}
-
-	/* Read each of the counters. */
-	for (i = 0; i < nr_stats; i++)
-		data[i] = _mv88e6xxx_get_ethtool_stat(ds, i, stats, port);
-
-	mutex_unlock(&ps->smi_mutex);
-}
-
-/* All the statistics in the table */
-void
-mv88e6xxx_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
-{
-	if (have_sw_in_discards(ds))
-		_mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
-				       mv88e6xxx_hw_stats, port, data);
-	else
-		_mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
-				       mv88e6xxx_hw_stats, port, data);
 }
 
 int mv88e6xxx_get_sset_count(struct dsa_switch *ds)
 {
-	if (have_sw_in_discards(ds))
-		return ARRAY_SIZE(mv88e6xxx_hw_stats);
-	return ARRAY_SIZE(mv88e6xxx_hw_stats) - 3;
+	struct mv88e6xxx_hw_stat *stat;
+	int i, j;
+
+	for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
+		stat = &mv88e6xxx_hw_stats[i];
+		if (mv88e6xxx_has_stat(ds, stat))
+			j++;
+	}
+	return j;
 }
 
 void
 mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 			    int port, uint64_t *data)
 {
-	if (have_sw_in_discards(ds))
-		_mv88e6xxx_get_ethtool_stats(
-			ds, ARRAY_SIZE(mv88e6xxx_hw_stats),
-			mv88e6xxx_hw_stats, port, data);
-	else
-		_mv88e6xxx_get_ethtool_stats(
-			ds, ARRAY_SIZE(mv88e6xxx_hw_stats) - 3,
-			mv88e6xxx_hw_stats, port, data);
+	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	struct mv88e6xxx_hw_stat *stat;
+	int ret;
+	int i, j;
+
+	mutex_lock(&ps->smi_mutex);
+
+	ret = _mv88e6xxx_stats_snapshot(ds, port);
+	if (ret < 0) {
+		mutex_unlock(&ps->smi_mutex);
+		return;
+	}
+	for (i = 0, j = 0; i < ARRAY_SIZE(mv88e6xxx_hw_stats); i++) {
+		stat = &mv88e6xxx_hw_stats[i];
+		if (mv88e6xxx_has_stat(ds, stat)) {
+			data[j] = _mv88e6xxx_get_ethtool_stat(ds, stat, port);
+			j++;
+		}
+	}
+
+	mutex_unlock(&ps->smi_mutex);
 }
 
 int mv88e6xxx_get_regs_len(struct dsa_switch *ds, int port)
@@ -1519,7 +1532,7 @@ int mv88e6xxx_port_vlan_add(struct dsa_switch *ds, int port,
 
 	/* no PVID with ranges, otherwise it's a bug */
 	if (pvid)
-		err = _mv88e6xxx_port_pvid_set(ds, port, vid);
+		err = _mv88e6xxx_port_pvid_set(ds, port, vlan->vid_end);
 unlock:
 	mutex_unlock(&ps->smi_mutex);
 
@@ -1542,7 +1555,7 @@ static int _mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port, u16 vid)
 
 	if (vlan.vid != vid || !vlan.valid ||
 	    vlan.data[port] == GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER)
-		return -ENOENT;
+		return -EOPNOTSUPP;
 
 	vlan.data[port] = GLOBAL_VTU_DATA_MEMBER_TAG_NON_MEMBER;
 
@@ -1569,6 +1582,7 @@ int mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port,
 			    const struct switchdev_obj_port_vlan *vlan)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	const u16 defpvid = 4000 + ds->index * DSA_MAX_PORTS + port;
 	u16 pvid, vid;
 	int err = 0;
 
@@ -1584,7 +1598,8 @@ int mv88e6xxx_port_vlan_del(struct dsa_switch *ds, int port,
 			goto unlock;
 
 		if (vid == pvid) {
-			err = _mv88e6xxx_port_pvid_set(ds, port, 0);
+			/* restore reserved VLAN ID */
+			err = _mv88e6xxx_port_pvid_set(ds, port, defpvid);
 			if (err)
 				goto unlock;
 		}
@@ -1876,26 +1891,20 @@ unlock:
 
 int mv88e6xxx_port_bridge_join(struct dsa_switch *ds, int port, u32 members)
 {
-	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
-	const u16 pvid = 4000 + ds->index * DSA_MAX_PORTS + port;
-	int err;
-
-	/* The port joined a bridge, so leave its reserved VLAN */
-	mutex_lock(&ps->smi_mutex);
-	err = _mv88e6xxx_port_vlan_del(ds, port, pvid);
-	if (!err)
-		err = _mv88e6xxx_port_pvid_set(ds, port, 0);
-	mutex_unlock(&ps->smi_mutex);
-	return err;
+	return 0;
 }
 
 int mv88e6xxx_port_bridge_leave(struct dsa_switch *ds, int port, u32 members)
 {
+	return 0;
+}
+
+static int mv88e6xxx_setup_port_default_vlan(struct dsa_switch *ds, int port)
+{
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	const u16 pvid = 4000 + ds->index * DSA_MAX_PORTS + port;
 	int err;
 
-	/* The port left the bridge, so join its reserved VLAN */
 	mutex_lock(&ps->smi_mutex);
 	err = _mv88e6xxx_port_vlan_add(ds, port, pvid, true);
 	if (!err)
@@ -2150,7 +2159,8 @@ static int mv88e6xxx_setup_port(struct dsa_switch *ds, int port)
 	 * database, and allow every port to egress frames on all other ports.
 	 */
 	reg = BIT(ps->num_ports) - 1; /* all ports */
-	ret = _mv88e6xxx_port_vlan_map_set(ds, port, reg & ~port);
+	reg &= ~BIT(port); /* except itself */
+	ret = _mv88e6xxx_port_vlan_map_set(ds, port, reg);
 	if (ret)
 		goto abort;
 
@@ -2178,8 +2188,7 @@ int mv88e6xxx_setup_ports(struct dsa_switch *ds)
 		if (dsa_is_cpu_port(ds, i) || dsa_is_dsa_port(ds, i))
 			continue;
 
-		/* setup the unbridged state */
-		ret = mv88e6xxx_port_bridge_leave(ds, i, 0);
+		ret = mv88e6xxx_setup_port_default_vlan(ds, i);
 		if (ret < 0)
 			return ret;
 	}
@@ -2323,6 +2332,7 @@ int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
 	u16 is_reset = (ppu_active ? 0x8800 : 0xc800);
+	struct gpio_desc *gpiod = ds->pd->reset;
 	unsigned long timeout;
 	int ret;
 	int i;
@@ -2336,6 +2346,14 @@ int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active)
 	/* Wait for transmit queues to drain. */
 	usleep_range(2000, 4000);
 
+	/* If there is a gpio connected to the reset pin, toggle it */
+	if (gpiod) {
+		gpiod_set_value_cansleep(gpiod, 1);
+		usleep_range(10000, 20000);
+		gpiod_set_value_cansleep(gpiod, 0);
+		usleep_range(10000, 20000);
+	}
+
 	/* Reset the switch. Keep the PPU active if requested. The PPU
 	 * needs to be active to support indirect phy register access
 	 * through global registers 0x18 and 0x19.
diff --git a/drivers/net/dsa/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx.h
index 21c8daa03f78..ca08f913d302 100644
--- a/drivers/net/dsa/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx.h
@@ -288,6 +288,7 @@
 #define GLOBAL_STATS_OP_HIST_RX		((1 << 10) | GLOBAL_STATS_OP_BUSY)
 #define GLOBAL_STATS_OP_HIST_TX		((2 << 10) | GLOBAL_STATS_OP_BUSY)
 #define GLOBAL_STATS_OP_HIST_RX_TX	((3 << 10) | GLOBAL_STATS_OP_BUSY)
+#define GLOBAL_STATS_OP_BANK_1	BIT(9)
 #define GLOBAL_STATS_COUNTER_32	0x1e
 #define GLOBAL_STATS_COUNTER_01	0x1f
 
@@ -420,10 +421,17 @@ struct mv88e6xxx_priv_state {
 	struct work_struct bridge_work;
 };
 
+enum stat_type {
+	BANK0,
+	BANK1,
+	PORT,
+};
+
 struct mv88e6xxx_hw_stat {
 	char string[ETH_GSTRING_LEN];
 	int sizeof_stat;
 	int reg;
+	enum stat_type type;
 };
 
 int mv88e6xxx_switch_reset(struct dsa_switch *ds, bool ppu_active);
diff --git a/drivers/net/ethernet/3com/3c509.c b/drivers/net/ethernet/3com/3c509.c
index 4547a1b8b958..7677c745fb30 100644
--- a/drivers/net/ethernet/3com/3c509.c
+++ b/drivers/net/ethernet/3com/3c509.c
@@ -562,7 +562,7 @@ static void el3_common_remove (struct net_device *dev)
 }
 
 #ifdef CONFIG_EISA
-static int __init el3_eisa_probe (struct device *device)
+static int el3_eisa_probe(struct device *device)
 {
 	short i;
 	int ioaddr, irq, if_port;
diff --git a/drivers/net/ethernet/3com/3c59x.c b/drivers/net/ethernet/3com/3c59x.c
index 2839af00f20c..17b2126075e0 100644
--- a/drivers/net/ethernet/3com/3c59x.c
+++ b/drivers/net/ethernet/3com/3c59x.c
@@ -907,7 +907,7 @@ static struct eisa_device_id vortex_eisa_ids[] = {
 };
 MODULE_DEVICE_TABLE(eisa, vortex_eisa_ids);
 
-static int __init vortex_eisa_probe(struct device *device)
+static int vortex_eisa_probe(struct device *device)
 {
 	void __iomem *ioaddr;
 	struct eisa_device *edev;
@@ -2459,8 +2459,13 @@ boomerang_interrupt(int irq, void *dev_id)
 					struct sk_buff *skb = vp->tx_skbuff[entry];
 #if DO_ZEROCOPY
 					int i;
-					for (i=0; i<=skb_shinfo(skb)->nr_frags; i++)
-							pci_unmap_single(VORTEX_PCI(vp),
+					pci_unmap_single(VORTEX_PCI(vp),
+							le32_to_cpu(vp->tx_ring[entry].frag[0].addr),
+							le32_to_cpu(vp->tx_ring[entry].frag[0].length)&0xFFF,
+							PCI_DMA_TODEVICE);
+
+					for (i=1; i<=skb_shinfo(skb)->nr_frags; i++)
+							pci_unmap_page(VORTEX_PCI(vp),
 											 le32_to_cpu(vp->tx_ring[entry].frag[i].addr),
 											 le32_to_cpu(vp->tx_ring[entry].frag[i].length)&0xFFF,
 											 PCI_DMA_TODEVICE);
diff --git a/drivers/net/ethernet/8390/ax88796.c b/drivers/net/ethernet/8390/ax88796.c
index 0443654f0339..c89b9aeeceb6 100644
--- a/drivers/net/ethernet/8390/ax88796.c
+++ b/drivers/net/ethernet/8390/ax88796.c
@@ -372,7 +372,7 @@ static int ax_mii_probe(struct net_device *dev)
 	ax->phy_dev = phy_dev;
 
 	netdev_info(dev, "PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		    phy_dev->drv->name, dev_name(&phy_dev->dev), phy_dev->irq);
+		    phy_dev->drv->name, phydev_name(phy_dev), phy_dev->irq);
 
 	return 0;
 }
@@ -627,7 +627,7 @@ static int ax_mii_init(struct net_device *dev)
 	struct platform_device *pdev = to_platform_device(dev->dev.parent);
 	struct ei_device *ei_local = netdev_priv(dev);
 	struct ax_device *ax = to_ax_dev(dev);
-	int err, i;
+	int err;
 
 	ax->bb_ctrl.ops = &bb_ops;
 	ax->addr_memr = ei_local->mem + AX_MEMR;
@@ -642,23 +642,12 @@ static int ax_mii_init(struct net_device *dev)
 	snprintf(ax->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		pdev->name, pdev->id);
 
-	ax->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!ax->mii_bus->irq) {
-		err = -ENOMEM;
-		goto out_free_mdio_bitbang;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		ax->mii_bus->irq[i] = PHY_POLL;
-
 	err = mdiobus_register(ax->mii_bus);
 	if (err)
-		goto out_free_irq;
+		goto out_free_mdio_bitbang;
 
 	return 0;
 
- out_free_irq:
-	kfree(ax->mii_bus->irq);
  out_free_mdio_bitbang:
 	free_mdio_bitbang(ax->mii_bus);
  out:
diff --git a/drivers/net/ethernet/8390/pcnet_cs.c b/drivers/net/ethernet/8390/pcnet_cs.c
index 2777289a26c0..2f79d29f17f2 100644
--- a/drivers/net/ethernet/8390/pcnet_cs.c
+++ b/drivers/net/ethernet/8390/pcnet_cs.c
@@ -1501,6 +1501,7 @@ static const struct pcmcia_device_id pcnet_ids[] = {
 	PCMCIA_DEVICE_MANF_CARD(0x026f, 0x030a),
 	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1103),
 	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1121),
+	PCMCIA_DEVICE_MANF_CARD(0xc001, 0x0009),
 	PCMCIA_DEVICE_PROD_ID12("2408LAN", "Ethernet", 0x352fff7f, 0x00b2e941),
 	PCMCIA_DEVICE_PROD_ID1234("Socket", "CF 10/100 Ethernet Card", "Revision B", "05/11/06", 0xb38bcc2e, 0x4de88352, 0xeaca6c8d, 0x7e57c22e),
 	PCMCIA_DEVICE_PROD_ID123("Cardwell", "PCMCIA", "ETHERNET", 0x9533672e, 0x281f1c5d, 0x3ff7175b),
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index 31c5e476fd64..0b13af8e4070 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -122,6 +122,7 @@ config FEALNX
 	  cards. <http://www.myson.com.tw/>
 
 source "drivers/net/ethernet/natsemi/Kconfig"
+source "drivers/net/ethernet/netronome/Kconfig"
 source "drivers/net/ethernet/8390/Kconfig"
 
 config NET_NETX
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 071f84eb6f3f..38dc1a776a2b 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_NET_VENDOR_MOXART) += moxa/
 obj-$(CONFIG_NET_VENDOR_MYRI) += myricom/
 obj-$(CONFIG_FEALNX) += fealnx.o
 obj-$(CONFIG_NET_VENDOR_NATSEMI) += natsemi/
+obj-$(CONFIG_NET_VENDOR_NETRONOME) += netronome/
 obj-$(CONFIG_NET_NETX) += netx-eth.o
 obj-$(CONFIG_NET_VENDOR_NUVOTON) += nuvoton/
 obj-$(CONFIG_NET_VENDOR_NVIDIA) += nvidia/
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index 096531a73124..74139cb7f849 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -380,9 +380,8 @@ static void bfin_mac_adjust_link(struct net_device *dev)
 static int mii_probe(struct net_device *dev, int phy_mode)
 {
 	struct bfin_mac_local *lp = netdev_priv(dev);
-	struct phy_device *phydev = NULL;
+	struct phy_device *phydev;
 	unsigned short sysctl;
-	int i;
 	u32 sclk, mdc_div;
 
 	/* Enable PHY output early */
@@ -396,18 +395,7 @@ static int mii_probe(struct net_device *dev, int phy_mode)
 	sysctl = (sysctl & ~MDCDIV) | SET_MDCDIV(mdc_div);
 	bfin_write_EMAC_SYSCTL(sysctl);
 
-	/* search for connected PHY device */
-	for (i = 0; i < PHY_MAX_ADDR; ++i) {
-		struct phy_device *const tmp_phydev = lp->mii_bus->phy_map[i];
-
-		if (!tmp_phydev)
-			continue; /* no PHY here... */
-
-		phydev = tmp_phydev;
-		break; /* found it */
-	}
-
-	/* now we are supposed to have a proper phydev, to attach to... */
+	phydev = phy_find_first(lp->mii_bus);
 	if (!phydev) {
 		netdev_err(dev, "no phy device found\n");
 		return -ENODEV;
@@ -419,7 +407,7 @@ static int mii_probe(struct net_device *dev, int phy_mode)
 		return -EINVAL;
 	}
 
-	phydev = phy_connect(dev, dev_name(&phydev->dev),
+	phydev = phy_connect(dev, phydev_name(phydev),
 			     &bfin_mac_adjust_link, phy_mode);
 
 	if (IS_ERR(phydev)) {
@@ -444,10 +432,8 @@ static int mii_probe(struct net_device *dev, int phy_mode)
 	lp->old_duplex = -1;
 	lp->phydev = phydev;
 
-	pr_info("attached PHY driver [%s] "
-	        "(mii_bus:phy_addr=%s, irq=%d, mdc_clk=%dHz(mdc_div=%d)@sclk=%dMHz)\n",
-	        phydev->drv->name, dev_name(&phydev->dev), phydev->irq,
-	        MDC_CLK, mdc_div, sclk/1000000);
+	phy_attached_print(phydev, "mdc_clk=%dHz(mdc_div=%d)@sclk=%dMHz)\n",
+			   MDC_CLK, mdc_div, sclk / 1000000);
 
 	return 0;
 }
@@ -1840,12 +1826,6 @@ static int bfin_mii_bus_probe(struct platform_device *pdev)
 
 	snprintf(miibus->id, MII_BUS_ID_SIZE, "%s-%x",
 		pdev->name, pdev->id);
-	miibus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-	if (!miibus->irq)
-		goto out_err_irq_alloc;
-
-	for (i = rc; i < PHY_MAX_ADDR; ++i)
-		miibus->irq[i] = PHY_POLL;
 
 	rc = clamp(mii_bus_pd->phydev_number, 0, PHY_MAX_ADDR);
 	if (rc != mii_bus_pd->phydev_number)
@@ -1864,14 +1844,12 @@ static int bfin_mii_bus_probe(struct platform_device *pdev)
 	rc = mdiobus_register(miibus);
 	if (rc) {
 		dev_err(&pdev->dev, "Cannot register MDIO bus!\n");
-		goto out_err_mdiobus_register;
+		goto out_err_irq_alloc;
 	}
 
 	platform_set_drvdata(pdev, miibus);
 	return 0;
 
-out_err_mdiobus_register:
-	kfree(miibus->irq);
 out_err_irq_alloc:
 	mdiobus_free(miibus);
 out_err_alloc:
@@ -1887,7 +1865,6 @@ static int bfin_mii_bus_remove(struct platform_device *pdev)
 		dev_get_platdata(&pdev->dev);
 
 	mdiobus_unregister(miibus);
-	kfree(miibus->irq);
 	mdiobus_free(miibus);
 	peripheral_free_list(mii_bus_pd->mac_peripherals);
 
@@ -1912,21 +1889,21 @@ static struct platform_driver bfin_mac_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&bfin_mii_bus_driver,
+	&bfin_mac_driver,
+};
+
 static int __init bfin_mac_init(void)
 {
-	int ret;
-	ret = platform_driver_register(&bfin_mii_bus_driver);
-	if (!ret)
-		return platform_driver_register(&bfin_mac_driver);
-	return -ENODEV;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_init(bfin_mac_init);
 
 static void __exit bfin_mac_cleanup(void)
 {
-	platform_driver_unregister(&bfin_mac_driver);
-	platform_driver_unregister(&bfin_mii_bus_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_exit(bfin_mac_cleanup);
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 20bf55dbd76f..b873531c5575 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -1337,11 +1337,6 @@ static int greth_mdio_init(struct greth_private *greth)
 	greth->mdio->write = greth_mdio_write;
 	greth->mdio->priv = greth;
 
-	greth->mdio->irq = greth->mdio_irqs;
-
-	for (phy = 0; phy < PHY_MAX_ADDR; phy++)
-		greth->mdio->irq[phy] = PHY_POLL;
-
 	ret = mdiobus_register(greth->mdio);
 	if (ret) {
 		goto error;
diff --git a/drivers/net/ethernet/aeroflex/greth.h b/drivers/net/ethernet/aeroflex/greth.h
index ae16ac94daf8..92dd918e4a83 100644
--- a/drivers/net/ethernet/aeroflex/greth.h
+++ b/drivers/net/ethernet/aeroflex/greth.h
@@ -125,7 +125,6 @@ struct greth_private {
 
 	struct phy_device *phy;
 	struct mii_bus *mdio;
-	int mdio_irqs[PHY_MAX_ADDR];
 	unsigned int link;
 	unsigned int speed;
 	unsigned int duplex;
diff --git a/drivers/net/ethernet/agere/et131x.c b/drivers/net/ethernet/agere/et131x.c
index e0f3d197e7f2..0907ab6ff309 100644
--- a/drivers/net/ethernet/agere/et131x.c
+++ b/drivers/net/ethernet/agere/et131x.c
@@ -1235,7 +1235,7 @@ static int et131x_mii_read(struct et131x_adapter *adapter, u8 reg, u16 *value)
 	if (!phydev)
 		return -EIO;
 
-	return et131x_phy_mii_read(adapter, phydev->addr, reg, value);
+	return et131x_phy_mii_read(adapter, phydev->mdio.addr, reg, value);
 }
 
 static int et131x_mii_write(struct et131x_adapter *adapter, u8 addr, u8 reg,
@@ -1462,7 +1462,7 @@ static void et1310_phy_power_switch(struct et131x_adapter *adapter, bool down)
 	data &= ~BMCR_PDOWN;
 	if (down)
 		data |= BMCR_PDOWN;
-	et131x_mii_write(adapter, phydev->addr, MII_BMCR, data);
+	et131x_mii_write(adapter, phydev->mdio.addr, MII_BMCR, data);
 }
 
 /* et131x_xcvr_init - Init the phy if we are setting it into force mode */
@@ -1490,7 +1490,7 @@ static void et131x_xcvr_init(struct et131x_adapter *adapter)
 		else
 			lcr2 |= (LED_VAL_LINKON << LED_TXRX_SHIFT);
 
-		et131x_mii_write(adapter, phydev->addr, PHY_LED_2, lcr2);
+		et131x_mii_write(adapter, phydev->mdio.addr, PHY_LED_2, lcr2);
 	}
 }
 
@@ -2380,7 +2380,7 @@ static int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
 						    sizeof(u32),
 						    &tx_ring->tx_status_pa,
 						    GFP_KERNEL);
-	if (!tx_ring->tx_status_pa) {
+	if (!tx_ring->tx_status) {
 		dev_err(&adapter->pdev->dev,
 			"Cannot alloc memory for Tx status block\n");
 		return -ENOMEM;
@@ -3192,14 +3192,14 @@ static void et131x_adjust_link(struct net_device *netdev)
 
 			et131x_mii_read(adapter, PHY_MPHY_CONTROL_REG,
 					&register18);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_MPHY_CONTROL_REG,
 					 register18 | 0x4);
-			et131x_mii_write(adapter, phydev->addr, PHY_INDEX_REG,
-					 register18 | 0x8402);
-			et131x_mii_write(adapter, phydev->addr, PHY_DATA_REG,
-					 register18 | 511);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
+					 PHY_INDEX_REG, register18 | 0x8402);
+			et131x_mii_write(adapter, phydev->mdio.addr,
+					 PHY_DATA_REG, register18 | 511);
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_MPHY_CONTROL_REG, register18);
 		}
 
@@ -3212,8 +3212,8 @@ static void et131x_adjust_link(struct net_device *netdev)
 			et131x_mii_read(adapter, PHY_CONFIG, &reg);
 			reg &= ~ET_PHY_CONFIG_TX_FIFO_DEPTH;
 			reg |= ET_PHY_CONFIG_FIFO_DEPTH_32;
-			et131x_mii_write(adapter, phydev->addr, PHY_CONFIG,
-					 reg);
+			et131x_mii_write(adapter, phydev->mdio.addr,
+					 PHY_CONFIG, reg);
 		}
 
 		et131x_set_rx_dma_timer(adapter);
@@ -3226,14 +3226,14 @@ static void et131x_adjust_link(struct net_device *netdev)
 
 			et131x_mii_read(adapter, PHY_MPHY_CONTROL_REG,
 					&register18);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_MPHY_CONTROL_REG,
 					 register18 | 0x4);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_INDEX_REG, register18 | 0x8402);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_DATA_REG, register18 | 511);
-			et131x_mii_write(adapter, phydev->addr,
+			et131x_mii_write(adapter, phydev->mdio.addr,
 					 PHY_MPHY_CONTROL_REG, register18);
 		}
 
@@ -3265,7 +3265,7 @@ static int et131x_mii_probe(struct net_device *netdev)
 		return -ENODEV;
 	}
 
-	phydev = phy_connect(netdev, dev_name(&phydev->dev),
+	phydev = phy_connect(netdev, phydev_name(phydev),
 			     &et131x_adjust_link, PHY_INTERFACE_MODE_MII);
 
 	if (IS_ERR(phydev)) {
@@ -3289,9 +3289,7 @@ static int et131x_mii_probe(struct net_device *netdev)
 	phydev->autoneg = AUTONEG_ENABLE;
 	adapter->phydev = phydev;
 
-	dev_info(&adapter->pdev->dev,
-		 "attached PHY driver [%s] (mii_bus:phy_addr=%s)\n",
-		 phydev->drv->name, dev_name(&phydev->dev));
+	phy_attached_info(phydev);
 
 	return 0;
 }
@@ -3327,7 +3325,6 @@ static void et131x_pci_remove(struct pci_dev *pdev)
 	netif_napi_del(&adapter->napi);
 	phy_disconnect(adapter->phydev);
 	mdiobus_unregister(adapter->mii_bus);
-	kfree(adapter->mii_bus->irq);
 	mdiobus_free(adapter->mii_bus);
 
 	et131x_adapter_memory_free(adapter);
@@ -3946,7 +3943,6 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 	struct net_device *netdev;
 	struct et131x_adapter *adapter;
 	int rc;
-	int ii;
 
 	rc = pci_enable_device(pdev);
 	if (rc < 0) {
@@ -4036,18 +4032,11 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 	adapter->mii_bus->priv = netdev;
 	adapter->mii_bus->read = et131x_mdio_read;
 	adapter->mii_bus->write = et131x_mdio_write;
-	adapter->mii_bus->irq = kmalloc_array(PHY_MAX_ADDR, sizeof(int),
-					      GFP_KERNEL);
-	if (!adapter->mii_bus->irq)
-		goto err_mdio_free;
-
-	for (ii = 0; ii < PHY_MAX_ADDR; ii++)
-		adapter->mii_bus->irq[ii] = PHY_POLL;
 
 	rc = mdiobus_register(adapter->mii_bus);
 	if (rc < 0) {
 		dev_err(&pdev->dev, "failed to register MII bus\n");
-		goto err_mdio_free_irq;
+		goto err_mdio_free;
 	}
 
 	rc = et131x_mii_probe(netdev);
@@ -4087,8 +4076,6 @@ err_phy_disconnect:
 	phy_disconnect(adapter->phydev);
 err_mdio_unregister:
 	mdiobus_unregister(adapter->mii_bus);
-err_mdio_free_irq:
-	kfree(adapter->mii_bus->irq);
 err_mdio_free:
 	mdiobus_free(adapter->mii_bus);
 err_mem_free:
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index fe644823ceaf..f749e4d389eb 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -131,7 +131,6 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 {
 	struct altera_tse_private *priv = netdev_priv(dev);
 	int ret;
-	int i;
 	struct device_node *mdio_node = NULL;
 	struct mii_bus *mdio = NULL;
 	struct device_node *child_node = NULL;
@@ -161,14 +160,6 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 	mdio->write = &altera_tse_mdio_write;
 	snprintf(mdio->id, MII_BUS_ID_SIZE, "%s-%u", mdio->name, id);
 
-	mdio->irq = kcalloc(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
-	if (mdio->irq == NULL) {
-		ret = -ENOMEM;
-		goto out_free_mdio;
-	}
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		mdio->irq[i] = PHY_POLL;
-
 	mdio->priv = dev;
 	mdio->parent = priv->device;
 
@@ -176,7 +167,7 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 	if (ret != 0) {
 		netdev_err(dev, "Cannot register MDIO bus %s\n",
 			   mdio->id);
-		goto out_free_mdio_irq;
+		goto out_free_mdio;
 	}
 
 	if (netif_msg_drv(priv))
@@ -184,8 +175,6 @@ static int altera_tse_mdio_create(struct net_device *dev, unsigned int id)
 
 	priv->mdio = mdio;
 	return 0;
-out_free_mdio_irq:
-	kfree(mdio->irq);
 out_free_mdio:
 	mdiobus_free(mdio);
 	mdio = NULL;
@@ -204,7 +193,6 @@ static void altera_tse_mdio_destroy(struct net_device *dev)
 			    priv->mdio->id);
 
 	mdiobus_unregister(priv->mdio);
-	kfree(priv->mdio->irq);
 	mdiobus_free(priv->mdio);
 	priv->mdio = NULL;
 }
@@ -855,7 +843,7 @@ static int init_phy(struct net_device *dev)
 	}
 
 	netdev_dbg(dev, "attached to PHY %d UID 0x%08x Link = %d\n",
-		   phydev->addr, phydev->phy_id, phydev->link);
+		   phydev->mdio.addr, phydev->phy_id, phydev->link);
 
 	priv->phydev = phydev;
 	return 0;
diff --git a/drivers/net/ethernet/amd/am79c961a.c b/drivers/net/ethernet/amd/am79c961a.c
index 87e727b921dc..fcdf5dda448f 100644
--- a/drivers/net/ethernet/amd/am79c961a.c
+++ b/drivers/net/ethernet/amd/am79c961a.c
@@ -50,8 +50,8 @@ static const char version[] =
 static void write_rreg(u_long base, u_int reg, u_int val)
 {
 	asm volatile(
-	"str%?h	%1, [%2]	@ NET_RAP\n\t"
-	"str%?h	%0, [%2, #-4]	@ NET_RDP"
+	"strh	%1, [%2]	@ NET_RAP\n\t"
+	"strh	%0, [%2, #-4]	@ NET_RDP"
 	:
 	: "r" (val), "r" (reg), "r" (ISAIO_BASE + 0x0464));
 }
@@ -60,8 +60,8 @@ static inline unsigned short read_rreg(u_long base_addr, u_int reg)
 {
 	unsigned short v;
 	asm volatile(
-	"str%?h	%1, [%2]	@ NET_RAP\n\t"
-	"ldr%?h	%0, [%2, #-4]	@ NET_RDP"
+	"strh	%1, [%2]	@ NET_RAP\n\t"
+	"ldrh	%0, [%2, #-4]	@ NET_RDP"
 	: "=r" (v)
 	: "r" (reg), "r" (ISAIO_BASE + 0x0464));
 	return v;
@@ -70,8 +70,8 @@ static inline unsigned short read_rreg(u_long base_addr, u_int reg)
 static inline void write_ireg(u_long base, u_int reg, u_int val)
 {
 	asm volatile(
-	"str%?h	%1, [%2]	@ NET_RAP\n\t"
-	"str%?h	%0, [%2, #8]	@ NET_IDP"
+	"strh	%1, [%2]	@ NET_RAP\n\t"
+	"strh	%0, [%2, #8]	@ NET_IDP"
 	:
 	: "r" (val), "r" (reg), "r" (ISAIO_BASE + 0x0464));
 }
@@ -80,8 +80,8 @@ static inline unsigned short read_ireg(u_long base_addr, u_int reg)
 {
 	u_short v;
 	asm volatile(
-	"str%?h	%1, [%2]	@ NAT_RAP\n\t"
-	"ldr%?h	%0, [%2, #8]	@ NET_IDP\n\t"
+	"strh	%1, [%2]	@ NAT_RAP\n\t"
+	"ldrh	%0, [%2, #8]	@ NET_IDP\n\t"
 	: "=r" (v)
 	: "r" (reg), "r" (ISAIO_BASE + 0x0464));
 	return v;
@@ -96,7 +96,7 @@ am_writebuffer(struct net_device *dev, u_int offset, unsigned char *buf, unsigne
 	offset = ISAMEM_BASE + (offset << 1);
 	length = (length + 1) & ~1;
 	if ((int)buf & 2) {
-		asm volatile("str%?h	%2, [%0], #4"
+		asm volatile("strh	%2, [%0], #4"
 		 : "=&r" (offset) : "0" (offset), "r" (buf[0] | (buf[1] << 8)));
 		buf += 2;
 		length -= 2;
@@ -104,20 +104,20 @@ am_writebuffer(struct net_device *dev, u_int offset, unsigned char *buf, unsigne
 	while (length > 8) {
 		register unsigned int tmp asm("r2"), tmp2 asm("r3");
 		asm volatile(
-			"ldm%?ia	%0!, {%1, %2}"
+			"ldmia	%0!, {%1, %2}"
 			: "+r" (buf), "=&r" (tmp), "=&r" (tmp2));
 		length -= 8;
 		asm volatile(
-			"str%?h	%1, [%0], #4\n\t"
-			"mov%?	%1, %1, lsr #16\n\t"
-			"str%?h	%1, [%0], #4\n\t"
-			"str%?h	%2, [%0], #4\n\t"
-			"mov%?	%2, %2, lsr #16\n\t"
-			"str%?h	%2, [%0], #4"
+			"strh	%1, [%0], #4\n\t"
+			"mov	%1, %1, lsr #16\n\t"
+			"strh	%1, [%0], #4\n\t"
+			"strh	%2, [%0], #4\n\t"
+			"mov	%2, %2, lsr #16\n\t"
+			"strh	%2, [%0], #4"
 		: "+r" (offset), "=&r" (tmp), "=&r" (tmp2));
 	}
 	while (length > 0) {
-		asm volatile("str%?h	%2, [%0], #4"
+		asm volatile("strh	%2, [%0], #4"
 		 : "=&r" (offset) : "0" (offset), "r" (buf[0] | (buf[1] << 8)));
 		buf += 2;
 		length -= 2;
@@ -132,23 +132,23 @@ am_readbuffer(struct net_device *dev, u_int offset, unsigned char *buf, unsigned
 	if ((int)buf & 2) {
 		unsigned int tmp;
 		asm volatile(
-			"ldr%?h	%2, [%0], #4\n\t"
-			"str%?b	%2, [%1], #1\n\t"
-			"mov%?	%2, %2, lsr #8\n\t"
-			"str%?b	%2, [%1], #1"
+			"ldrh	%2, [%0], #4\n\t"
+			"strb	%2, [%1], #1\n\t"
+			"mov	%2, %2, lsr #8\n\t"
+			"strb	%2, [%1], #1"
 		: "=&r" (offset), "=&r" (buf), "=r" (tmp): "0" (offset), "1" (buf));
 		length -= 2;
 	}
 	while (length > 8) {
 		register unsigned int tmp asm("r2"), tmp2 asm("r3"), tmp3;
 		asm volatile(
-			"ldr%?h	%2, [%0], #4\n\t"
-			"ldr%?h	%4, [%0], #4\n\t"
-			"ldr%?h	%3, [%0], #4\n\t"
-			"orr%?	%2, %2, %4, lsl #16\n\t"
-			"ldr%?h	%4, [%0], #4\n\t"
-			"orr%?	%3, %3, %4, lsl #16\n\t"
-			"stm%?ia	%1!, {%2, %3}"
+			"ldrh	%2, [%0], #4\n\t"
+			"ldrh	%4, [%0], #4\n\t"
+			"ldrh	%3, [%0], #4\n\t"
+			"orr	%2, %2, %4, lsl #16\n\t"
+			"ldrh	%4, [%0], #4\n\t"
+			"orr	%3, %3, %4, lsl #16\n\t"
+			"stmia	%1!, {%2, %3}"
 		: "=&r" (offset), "=&r" (buf), "=r" (tmp), "=r" (tmp2), "=r" (tmp3)
 		: "0" (offset), "1" (buf));
 		length -= 8;
@@ -156,10 +156,10 @@ am_readbuffer(struct net_device *dev, u_int offset, unsigned char *buf, unsigned
 	while (length > 0) {
 		unsigned int tmp;
 		asm volatile(
-			"ldr%?h	%2, [%0], #4\n\t"
-			"str%?b	%2, [%1], #1\n\t"
-			"mov%?	%2, %2, lsr #8\n\t"
-			"str%?b	%2, [%1], #1"
+			"ldrh	%2, [%0], #4\n\t"
+			"strb	%2, [%1], #1\n\t"
+			"mov	%2, %2, lsr #8\n\t"
+			"strb	%2, [%1], #1"
 		: "=&r" (offset), "=&r" (buf), "=r" (tmp) : "0" (offset), "1" (buf));
 		length -= 2;
 	}
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index 5330bcb8a944..d3977d032b48 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -344,9 +344,6 @@ static void au1000_mdio_write(struct net_device *dev, int phy_addr,
 
 static int au1000_mdiobus_read(struct mii_bus *bus, int phy_addr, int regnum)
 {
-	/* WARNING: bus->phy_map[phy_addr].attached_dev == dev does
-	 * _NOT_ hold (e.g. when PHY is accessed through other MAC's MII bus)
-	 */
 	struct net_device *const dev = bus->priv;
 
 	/* make sure the MAC associated with this
@@ -502,7 +499,7 @@ static int au1000_mii_probe(struct net_device *dev)
 		BUG_ON(aup->mac_id < 0 || aup->mac_id > 1);
 
 		if (aup->phy_addr)
-			phydev = aup->mii_bus->phy_map[aup->phy_addr];
+			phydev = mdiobus_get_phy(aup->mii_bus, aup->phy_addr);
 		else
 			netdev_info(dev, "using PHY-less setup\n");
 		return 0;
@@ -512,8 +509,8 @@ static int au1000_mii_probe(struct net_device *dev)
 	 * on the current MAC's MII bus
 	 */
 	for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++)
-		if (aup->mii_bus->phy_map[phy_addr]) {
-			phydev = aup->mii_bus->phy_map[phy_addr];
+		if (mdiobus_get_phy(aup->mii_bus, aup->phy_addr)) {
+			phydev = mdiobus_get_phy(aup->mii_bus, aup->phy_addr);
 			if (!aup->phy_search_highest_addr)
 				/* break out with first one found */
 				break;
@@ -531,7 +528,8 @@ static int au1000_mii_probe(struct net_device *dev)
 			 */
 			for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
 				struct phy_device *const tmp_phydev =
-					aup->mii_bus->phy_map[phy_addr];
+					mdiobus_get_phy(aup->mii_bus,
+							phy_addr);
 
 				if (aup->mac_id == 1)
 					break;
@@ -558,7 +556,7 @@ static int au1000_mii_probe(struct net_device *dev)
 	/* now we are supposed to have a proper phydev, to attach to... */
 	BUG_ON(phydev->attached_dev);
 
-	phydev = phy_connect(dev, dev_name(&phydev->dev),
+	phydev = phy_connect(dev, phydev_name(phydev),
 			     &au1000_adjust_link, PHY_INTERFACE_MODE_MII);
 
 	if (IS_ERR(phydev)) {
@@ -583,9 +581,7 @@ static int au1000_mii_probe(struct net_device *dev)
 	aup->old_duplex = -1;
 	aup->phy_dev = phydev;
 
-	netdev_info(dev, "attached PHY driver [%s] "
-	       "(mii_bus:phy_addr=%s, irq=%d)\n",
-	       phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
 
 	return 0;
 }
@@ -1293,14 +1289,7 @@ static int au1000_probe(struct platform_device *pdev)
 	aup->mii_bus->name = "au1000_eth_mii";
 	snprintf(aup->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		pdev->name, aup->mac_id);
-	aup->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-	if (aup->mii_bus->irq == NULL) {
-		err = -ENOMEM;
-		goto err_out;
-	}
 
-	for (i = 0; i < PHY_MAX_ADDR; ++i)
-		aup->mii_bus->irq[i] = PHY_POLL;
 	/* if known, set corresponding PHY IRQs */
 	if (aup->phy_static_config)
 		if (aup->phy_irq && aup->phy_busid == aup->mac_id)
diff --git a/drivers/net/ethernet/amd/lance.c b/drivers/net/ethernet/amd/lance.c
index 256f590f6bb1..3a7ebfdda57d 100644
--- a/drivers/net/ethernet/amd/lance.c
+++ b/drivers/net/ethernet/amd/lance.c
@@ -547,8 +547,8 @@ static int __init lance_probe1(struct net_device *dev, int ioaddr, int irq, int
 	/* Make certain the data structures used by the LANCE are aligned and DMAble. */
 
 	lp = kzalloc(sizeof(*lp), GFP_DMA | GFP_KERNEL);
-	if(lp==NULL)
-		return -ENODEV;
+	if (!lp)
+		return -ENOMEM;
 	if (lance_debug > 6) printk(" (#0x%05lx)", (unsigned long)lp);
 	dev->ml_priv = lp;
 	lp->name = chipname;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index 53ce1222b11d..8a9b493566c9 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -2024,7 +2024,6 @@ read_again:
 		skb->dev = netdev;
 		skb->protocol = eth_type_trans(skb, netdev);
 		skb_record_rx_queue(skb, channel->queue_index);
-		skb_mark_napi_id(skb, napi);
 
 		napi_gro_receive(napi, skb);
 
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index c31e691d11fc..db55c9f6e8e1 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -869,7 +869,7 @@ void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata)
 	pdata->mdio_bus = NULL;
 }
 
-struct xgene_mac_ops xgene_gmac_ops = {
+const struct xgene_mac_ops xgene_gmac_ops = {
 	.init = xgene_gmac_init,
 	.reset = xgene_gmac_reset,
 	.rx_enable = xgene_gmac_rx_enable,
@@ -879,7 +879,7 @@ struct xgene_mac_ops xgene_gmac_ops = {
 	.set_mac_addr = xgene_gmac_set_mac_addr,
 };
 
-struct xgene_port_ops xgene_gport_ops = {
+const struct xgene_port_ops xgene_gport_ops = {
 	.reset = xgene_enet_reset,
 	.cle_bypass = xgene_enet_cle_bypass,
 	.shutdown = xgene_gport_shutdown,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index c153a1dc5ff7..8a9091039ab4 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -340,8 +340,8 @@ int xgene_enet_mdio_config(struct xgene_enet_pdata *pdata);
 void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata);
 bool xgene_ring_mgr_init(struct xgene_enet_pdata *p);
 
-extern struct xgene_mac_ops xgene_gmac_ops;
-extern struct xgene_port_ops xgene_gport_ops;
+extern const struct xgene_mac_ops xgene_gmac_ops;
+extern const struct xgene_port_ops xgene_gport_ops;
 extern struct xgene_ring_ops xgene_ring1_ops;
 
 #endif /* __XGENE_ENET_HW_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index d0ae1a6cc212..5eb9b20c0eea 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -628,6 +628,7 @@ static int xgene_enet_register_irq(struct net_device *ndev)
 	int ret;
 
 	ring = pdata->rx_ring;
+	irq_set_status_flags(ring->irq, IRQ_DISABLE_UNLAZY);
 	ret = devm_request_irq(dev, ring->irq, xgene_enet_rx_irq,
 			       IRQF_SHARED, ring->irq_name, ring);
 	if (ret)
@@ -635,6 +636,7 @@ static int xgene_enet_register_irq(struct net_device *ndev)
 
 	if (pdata->cq_cnt) {
 		ring = pdata->tx_ring->cp_ring;
+		irq_set_status_flags(ring->irq, IRQ_DISABLE_UNLAZY);
 		ret = devm_request_irq(dev, ring->irq, xgene_enet_rx_irq,
 				       IRQF_SHARED, ring->irq_name, ring);
 		if (ret) {
@@ -649,15 +651,19 @@ static int xgene_enet_register_irq(struct net_device *ndev)
 static void xgene_enet_free_irq(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata;
+	struct xgene_enet_desc_ring *ring;
 	struct device *dev;
 
 	pdata = netdev_priv(ndev);
 	dev = ndev_to_dev(ndev);
-	devm_free_irq(dev, pdata->rx_ring->irq, pdata->rx_ring);
+	ring = pdata->rx_ring;
+	irq_clear_status_flags(ring->irq, IRQ_DISABLE_UNLAZY);
+	devm_free_irq(dev, ring->irq, ring);
 
 	if (pdata->cq_cnt) {
-		devm_free_irq(dev, pdata->tx_ring->cp_ring->irq,
-			      pdata->tx_ring->cp_ring);
+		ring = pdata->tx_ring->cp_ring;
+		irq_clear_status_flags(ring->irq, IRQ_DISABLE_UNLAZY);
+		devm_free_irq(dev, ring->irq, ring);
 	}
 }
 
@@ -690,7 +696,7 @@ static void xgene_enet_napi_disable(struct xgene_enet_pdata *pdata)
 static int xgene_enet_open(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
-	struct xgene_mac_ops *mac_ops = pdata->mac_ops;
+	const struct xgene_mac_ops *mac_ops = pdata->mac_ops;
 	int ret;
 
 	mac_ops->tx_enable(pdata);
@@ -714,7 +720,7 @@ static int xgene_enet_open(struct net_device *ndev)
 static int xgene_enet_close(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
-	struct xgene_mac_ops *mac_ops = pdata->mac_ops;
+	const struct xgene_mac_ops *mac_ops = pdata->mac_ops;
 
 	netif_stop_queue(ndev);
 
@@ -1090,7 +1096,7 @@ static const struct net_device_ops xgene_ndev_ops = {
 };
 
 #ifdef CONFIG_ACPI
-static int xgene_get_port_id_acpi(struct device *dev,
+static void xgene_get_port_id_acpi(struct device *dev,
 				  struct xgene_enet_pdata *pdata)
 {
 	acpi_status status;
@@ -1103,24 +1109,19 @@ static int xgene_get_port_id_acpi(struct device *dev,
 		pdata->port_id = temp;
 	}
 
-	return 0;
+	return;
 }
 #endif
 
-static int xgene_get_port_id_dt(struct device *dev, struct xgene_enet_pdata *pdata)
+static void xgene_get_port_id_dt(struct device *dev, struct xgene_enet_pdata *pdata)
 {
 	u32 id = 0;
-	int ret;
 
-	ret = of_property_read_u32(dev->of_node, "port-id", &id);
-	if (ret) {
-		pdata->port_id = 0;
-		ret = 0;
-	} else {
-		pdata->port_id = id & BIT(0);
-	}
+	of_property_read_u32(dev->of_node, "port-id", &id);
 
-	return ret;
+	pdata->port_id = id & BIT(0);
+
+	return;
 }
 
 static int xgene_get_tx_delay(struct xgene_enet_pdata *pdata)
@@ -1215,13 +1216,11 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	}
 
 	if (dev->of_node)
-		ret = xgene_get_port_id_dt(dev, pdata);
+		xgene_get_port_id_dt(dev, pdata);
 #ifdef CONFIG_ACPI
 	else
-		ret = xgene_get_port_id_acpi(dev, pdata);
+		xgene_get_port_id_acpi(dev, pdata);
 #endif
-	if (ret)
-		return ret;
 
 	if (!device_get_mac_address(dev, ndev->dev_addr, ETH_ALEN))
 		eth_hw_addr_random(ndev);
@@ -1429,7 +1428,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	struct net_device *ndev;
 	struct xgene_enet_pdata *pdata;
 	struct device *dev = &pdev->dev;
-	struct xgene_mac_ops *mac_ops;
+	const struct xgene_mac_ops *mac_ops;
 	const struct of_device_id *of_id;
 	int ret;
 
@@ -1516,7 +1515,7 @@ err:
 static int xgene_enet_remove(struct platform_device *pdev)
 {
 	struct xgene_enet_pdata *pdata;
-	struct xgene_mac_ops *mac_ops;
+	const struct xgene_mac_ops *mac_ops;
 	struct net_device *ndev;
 
 	pdata = platform_get_drvdata(pdev);
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index 1aa72c787f8d..248dfc40a761 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -25,6 +25,7 @@
 #include <linux/acpi.h>
 #include <linux/clk.h>
 #include <linux/efi.h>
+#include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/of_platform.h>
 #include <linux/of_net.h>
@@ -174,8 +175,8 @@ struct xgene_enet_pdata {
 	int phy_mode;
 	enum xgene_enet_rm rm;
 	struct rtnl_link_stats64 stats;
-	struct xgene_mac_ops *mac_ops;
-	struct xgene_port_ops *port_ops;
+	const struct xgene_mac_ops *mac_ops;
+	const struct xgene_port_ops *port_ops;
 	struct xgene_ring_ops *ring_ops;
 	struct delayed_work link_work;
 	u32 port_id;
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
index 05b817e56fde..78475512b683 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
@@ -405,7 +405,7 @@ static void xgene_enet_link_state(struct work_struct *work)
 	schedule_delayed_work(&p->link_work, poll_interval);
 }
 
-struct xgene_mac_ops xgene_sgmac_ops = {
+const struct xgene_mac_ops xgene_sgmac_ops = {
 	.init		= xgene_sgmac_init,
 	.reset		= xgene_sgmac_reset,
 	.rx_enable	= xgene_sgmac_rx_enable,
@@ -416,7 +416,7 @@ struct xgene_mac_ops xgene_sgmac_ops = {
 	.link_state	= xgene_enet_link_state
 };
 
-struct xgene_port_ops xgene_sgport_ops = {
+const struct xgene_port_ops xgene_sgport_ops = {
 	.reset		= xgene_enet_reset,
 	.cle_bypass	= xgene_enet_cle_bypass,
 	.shutdown	= xgene_enet_shutdown
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h
index de432465009c..29a71b4dcc44 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h
@@ -35,7 +35,7 @@
 #define MPA_IDLE_WITH_QMI_EMPTY		BIT(12)
 #define SG_RX_DV_GATE_REG_0_ADDR	0x0dfc
 
-extern struct xgene_mac_ops xgene_sgmac_ops;
-extern struct xgene_port_ops xgene_sgport_ops;
+extern const struct xgene_mac_ops xgene_sgmac_ops;
+extern const struct xgene_port_ops xgene_sgport_ops;
 
 #endif  /* __XGENE_ENET_SGMAC_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
index 7a28a48cb2c7..ba030dc1940b 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
@@ -326,7 +326,7 @@ static void xgene_enet_link_state(struct work_struct *work)
 	schedule_delayed_work(&pdata->link_work, poll_interval);
 }
 
-struct xgene_mac_ops xgene_xgmac_ops = {
+const struct xgene_mac_ops xgene_xgmac_ops = {
 	.init = xgene_xgmac_init,
 	.reset = xgene_xgmac_reset,
 	.rx_enable = xgene_xgmac_rx_enable,
@@ -338,7 +338,7 @@ struct xgene_mac_ops xgene_xgmac_ops = {
 	.link_state = xgene_enet_link_state
 };
 
-struct xgene_port_ops xgene_xgport_ops = {
+const struct xgene_port_ops xgene_xgport_ops = {
 	.reset = xgene_enet_reset,
 	.cle_bypass = xgene_enet_xgcle_bypass,
 	.shutdown = xgene_enet_shutdown,
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
index f8f908dbf51c..0a2dca8a1725 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
@@ -69,7 +69,7 @@
 #define XG_ENET_SPARE_CFG_REG_1_ADDR	0x0410
 #define XGENET_RX_DV_GATE_REG_0_ADDR	0x0804
 
-extern struct xgene_mac_ops xgene_xgmac_ops;
-extern struct xgene_port_ops xgene_xgport_ops;
+extern const struct xgene_mac_ops xgene_xgmac_ops;
+extern const struct xgene_port_ops xgene_xgport_ops;
 
 #endif /* __XGENE_ENET_XGMAC_H__ */
diff --git a/drivers/net/ethernet/arc/Kconfig b/drivers/net/ethernet/arc/Kconfig
index 52a6b16f57d2..689045186064 100644
--- a/drivers/net/ethernet/arc/Kconfig
+++ b/drivers/net/ethernet/arc/Kconfig
@@ -34,9 +34,9 @@ config EMAC_ROCKCHIP
 	select ARC_EMAC_CORE
 	depends on OF_IRQ && OF_NET && REGULATOR && HAS_DMA
 	---help---
-	  Support for Rockchip RK3066/RK3188 EMAC ethernet controllers.
+	  Support for Rockchip RK3036/RK3066/RK3188 EMAC ethernet controllers.
 	  This selects Rockchip SoC glue layer support for the
-	  emac device driver. This driver is used for RK3066/RK3188
+	  emac device driver. This driver is used for RK3036/RK3066/RK3188
 	  EMAC ethernet controller.
 
 endif # NET_VENDOR_ARC
diff --git a/drivers/net/ethernet/arc/emac_main.c b/drivers/net/ethernet/arc/emac_main.c
index abe1eabc0171..6446af1403f7 100644
--- a/drivers/net/ethernet/arc/emac_main.c
+++ b/drivers/net/ethernet/arc/emac_main.c
@@ -163,7 +163,7 @@ static void arc_emac_tx_clean(struct net_device *ndev)
 		struct sk_buff *skb = tx_buff->skb;
 		unsigned int info = le32_to_cpu(txbd->info);
 
-		if ((info & FOR_EMAC) || !txbd->data)
+		if ((info & FOR_EMAC) || !txbd->data || !skb)
 			break;
 
 		if (unlikely(info & (DROP | DEFR | LTCL | UFLO))) {
@@ -191,6 +191,7 @@ static void arc_emac_tx_clean(struct net_device *ndev)
 
 		txbd->data = 0;
 		txbd->info = 0;
+		tx_buff->skb = NULL;
 
 		*txbd_dirty = (*txbd_dirty + 1) % TX_BD_NUM;
 	}
@@ -446,6 +447,9 @@ static int arc_emac_open(struct net_device *ndev)
 		*last_rx_bd = (*last_rx_bd + 1) % RX_BD_NUM;
 	}
 
+	priv->txbd_curr = 0;
+	priv->txbd_dirty = 0;
+
 	/* Clean Tx BD's */
 	memset(priv->txbd, 0, TX_RING_SZ);
 
@@ -514,6 +518,64 @@ static void arc_emac_set_rx_mode(struct net_device *ndev)
 }
 
 /**
+ * arc_free_tx_queue - free skb from tx queue
+ * @ndev:	Pointer to the network device.
+ *
+ * This function must be called while EMAC disable
+ */
+static void arc_free_tx_queue(struct net_device *ndev)
+{
+	struct arc_emac_priv *priv = netdev_priv(ndev);
+	unsigned int i;
+
+	for (i = 0; i < TX_BD_NUM; i++) {
+		struct arc_emac_bd *txbd = &priv->txbd[i];
+		struct buffer_state *tx_buff = &priv->tx_buff[i];
+
+		if (tx_buff->skb) {
+			dma_unmap_single(&ndev->dev, dma_unmap_addr(tx_buff, addr),
+					 dma_unmap_len(tx_buff, len), DMA_TO_DEVICE);
+
+			/* return the sk_buff to system */
+			dev_kfree_skb_irq(tx_buff->skb);
+		}
+
+		txbd->info = 0;
+		txbd->data = 0;
+		tx_buff->skb = NULL;
+	}
+}
+
+/**
+ * arc_free_rx_queue - free skb from rx queue
+ * @ndev:	Pointer to the network device.
+ *
+ * This function must be called while EMAC disable
+ */
+static void arc_free_rx_queue(struct net_device *ndev)
+{
+	struct arc_emac_priv *priv = netdev_priv(ndev);
+	unsigned int i;
+
+	for (i = 0; i < RX_BD_NUM; i++) {
+		struct arc_emac_bd *rxbd = &priv->rxbd[i];
+		struct buffer_state *rx_buff = &priv->rx_buff[i];
+
+		if (rx_buff->skb) {
+			dma_unmap_single(&ndev->dev, dma_unmap_addr(rx_buff, addr),
+					dma_unmap_len(rx_buff, len), DMA_FROM_DEVICE);
+
+			/* return the sk_buff to system */
+			dev_kfree_skb_irq(rx_buff->skb);
+		}
+
+		rxbd->info = 0;
+		rxbd->data = 0;
+		rx_buff->skb = NULL;
+	}
+}
+
+/**
  * arc_emac_stop - Close the network device.
  * @ndev:	Pointer to the network device.
  *
@@ -534,6 +596,10 @@ static int arc_emac_stop(struct net_device *ndev)
 	/* Disable EMAC */
 	arc_reg_clr(priv, R_CTRL, EN_MASK);
 
+	/* Return the sk_buff to system */
+	arc_free_tx_queue(ndev);
+	arc_free_rx_queue(ndev);
+
 	return 0;
 }
 
@@ -610,7 +676,6 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
 	dma_unmap_addr_set(&priv->tx_buff[*txbd_curr], addr, addr);
 	dma_unmap_len_set(&priv->tx_buff[*txbd_curr], len, len);
 
-	priv->tx_buff[*txbd_curr].skb = skb;
 	priv->txbd[*txbd_curr].data = cpu_to_le32(addr);
 
 	/* Make sure pointer to data buffer is set */
@@ -620,6 +685,11 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
 
 	*info = cpu_to_le32(FOR_EMAC | FIRST_OR_LAST_MASK | len);
 
+	/* Make sure info word is set */
+	wmb();
+
+	priv->tx_buff[*txbd_curr].skb = skb;
+
 	/* Increment index to point to the next BD */
 	*txbd_curr = (*txbd_curr + 1) % TX_BD_NUM;
 
diff --git a/drivers/net/ethernet/arc/emac_rockchip.c b/drivers/net/ethernet/arc/emac_rockchip.c
index c31c7407b753..85e821ccfcd2 100644
--- a/drivers/net/ethernet/arc/emac_rockchip.c
+++ b/drivers/net/ethernet/arc/emac_rockchip.c
@@ -25,17 +25,13 @@
 #include "emac.h"
 
 #define DRV_NAME        "rockchip_emac"
-#define DRV_VERSION     "1.0"
-
-#define GRF_MODE_MII		(1UL << 0)
-#define GRF_MODE_RMII		(0UL << 0)
-#define GRF_SPEED_10M		(0UL << 1)
-#define GRF_SPEED_100M		(1UL << 1)
-#define GRF_SPEED_ENABLE_BIT	(1UL << 17)
-#define GRF_MODE_ENABLE_BIT	(1UL << 16)
+#define DRV_VERSION     "1.1"
 
 struct emac_rockchip_soc_data {
-	int grf_offset;
+	unsigned int grf_offset;
+	unsigned int grf_mode_offset;
+	unsigned int grf_speed_offset;
+	bool need_div_macclk;
 };
 
 struct rockchip_priv_data {
@@ -44,23 +40,22 @@ struct rockchip_priv_data {
 	const struct emac_rockchip_soc_data *soc_data;
 	struct regulator *regulator;
 	struct clk *refclk;
+	struct clk *macclk;
 };
 
 static void emac_rockchip_set_mac_speed(void *priv, unsigned int speed)
 {
 	struct rockchip_priv_data *emac = priv;
+	u32 speed_offset = emac->soc_data->grf_speed_offset;
 	u32 data;
 	int err = 0;
 
-	/* write-enable bits */
-	data = GRF_SPEED_ENABLE_BIT;
-
 	switch(speed) {
 	case 10:
-		data |= GRF_SPEED_10M;
+		data = (1 << (speed_offset + 16)) | (0 << speed_offset);
 		break;
 	case 100:
-		data |= GRF_SPEED_100M;
+		data = (1 << (speed_offset + 16)) | (1 << speed_offset);
 		break;
 	default:
 		pr_err("speed %u not supported\n", speed);
@@ -72,14 +67,25 @@ static void emac_rockchip_set_mac_speed(void *priv, unsigned int speed)
 		pr_err("unable to apply speed %u to grf (%d)\n", speed, err);
 }
 
-static const struct emac_rockchip_soc_data emac_rockchip_dt_data[] = {
-	{ .grf_offset = 0x154 }, /* rk3066 */
-	{ .grf_offset = 0x0a4 }, /* rk3188 */
+static const struct emac_rockchip_soc_data emac_rk3036_emac_data = {
+	.grf_offset = 0x140,   .grf_mode_offset = 8,
+	.grf_speed_offset = 9, .need_div_macclk = 1,
+};
+
+static const struct emac_rockchip_soc_data emac_rk3066_emac_data = {
+	.grf_offset = 0x154,   .grf_mode_offset = 0,
+	.grf_speed_offset = 1, .need_div_macclk = 0,
+};
+
+static const struct emac_rockchip_soc_data emac_rk3188_emac_data = {
+	.grf_offset = 0x0a4,   .grf_mode_offset = 0,
+	.grf_speed_offset = 1, .need_div_macclk = 0,
 };
 
 static const struct of_device_id emac_rockchip_dt_ids[] = {
-	{ .compatible = "rockchip,rk3066-emac", .data = &emac_rockchip_dt_data[0] },
-	{ .compatible = "rockchip,rk3188-emac", .data = &emac_rockchip_dt_data[1] },
+	{ .compatible = "rockchip,rk3036-emac", .data = &emac_rk3036_emac_data },
+	{ .compatible = "rockchip,rk3066-emac", .data = &emac_rk3066_emac_data },
+	{ .compatible = "rockchip,rk3188-emac", .data = &emac_rk3188_emac_data },
 	{ /* Sentinel */ }
 };
 
@@ -110,7 +116,7 @@ static int emac_rockchip_probe(struct platform_device *pdev)
 
 	interface = of_get_phy_mode(dev->of_node);
 
-	/* RK3066 and RK3188 SoCs only support RMII */
+	/* RK3036/RK3066/RK3188 SoCs only support RMII */
 	if (interface != PHY_INTERFACE_MODE_RMII) {
 		dev_err(dev, "unsupported phy interface mode %d\n", interface);
 		err = -ENOTSUPP;
@@ -164,15 +170,12 @@ static int emac_rockchip_probe(struct platform_device *pdev)
 		}
 	}
 
-	err = arc_emac_probe(ndev, interface);
-	if (err)
-		goto out_regulator_disable;
-
-	/* write-enable bits */
-	data = GRF_MODE_ENABLE_BIT | GRF_SPEED_ENABLE_BIT;
-
-	data |= GRF_SPEED_100M;
-	data |= GRF_MODE_RMII;
+	/* Set speed 100M */
+	data = (1 << (priv->soc_data->grf_speed_offset + 16)) |
+	       (1 << priv->soc_data->grf_speed_offset);
+	/* Set RMII mode */
+	data |= (1 << (priv->soc_data->grf_mode_offset + 16)) |
+		(0 << priv->soc_data->grf_mode_offset);
 
 	err = regmap_write(priv->grf, priv->soc_data->grf_offset, data);
 	if (err) {
@@ -184,6 +187,33 @@ static int emac_rockchip_probe(struct platform_device *pdev)
 	err = clk_set_rate(priv->refclk, 50000000);
 	if (err)
 		dev_err(dev, "failed to change reference clock rate (%d)\n", err);
+
+	if (priv->soc_data->need_div_macclk) {
+		priv->macclk = devm_clk_get(dev, "macclk");
+		if (IS_ERR(priv->macclk)) {
+			dev_err(dev, "failed to retrieve mac clock (%ld)\n", PTR_ERR(priv->macclk));
+			err = PTR_ERR(priv->macclk);
+			goto out_regulator_disable;
+		}
+
+		err = clk_prepare_enable(priv->macclk);
+		if (err) {
+			dev_err(dev, "failed to enable mac clock (%d)\n", err);
+			goto out_regulator_disable;
+		}
+
+		/* RMII TX/RX needs always a rate of 25MHz */
+		err = clk_set_rate(priv->macclk, 25000000);
+		if (err)
+			dev_err(dev, "failed to change mac clock rate (%d)\n", err);
+	}
+
+	err = arc_emac_probe(ndev, interface);
+	if (err) {
+		dev_err(dev, "failed to probe arc emac (%d)\n", err);
+		goto out_regulator_disable;
+	}
+
 	return 0;
 
 out_regulator_disable:
diff --git a/drivers/net/ethernet/atheros/alx/hw.c b/drivers/net/ethernet/atheros/alx/hw.c
index 7712f068f6d4..1fe35e453d43 100644
--- a/drivers/net/ethernet/atheros/alx/hw.c
+++ b/drivers/net/ethernet/atheros/alx/hw.c
@@ -958,13 +958,13 @@ void alx_configure_basic(struct alx_hw *hw)
 	alx_write_mem32(hw, ALX_TINT_TPD_THRSHLD, hw->ith_tpd);
 	alx_write_mem32(hw, ALX_TINT_TIMER, hw->imt);
 
-	raw_mtu = hw->mtu + ETH_HLEN;
-	alx_write_mem32(hw, ALX_MTU, raw_mtu + 8);
-	if (raw_mtu > ALX_MTU_JUMBO_TH)
+	raw_mtu = ALX_RAW_MTU(hw->mtu);
+	alx_write_mem32(hw, ALX_MTU, raw_mtu);
+	if (raw_mtu > (ALX_MTU_JUMBO_TH + ETH_FCS_LEN + VLAN_HLEN))
 		hw->rx_ctrl &= ~ALX_MAC_CTRL_FAST_PAUSE;
 
-	if ((raw_mtu + 8) < ALX_TXQ1_JUMBO_TSO_TH)
-		val = (raw_mtu + 8 + 7) >> 3;
+	if (raw_mtu < ALX_TXQ1_JUMBO_TSO_TH)
+		val = (raw_mtu + 7) >> 3;
 	else
 		val = ALX_TXQ1_JUMBO_TSO_TH >> 3;
 	alx_write_mem32(hw, ALX_TXQ1, val | ALX_TXQ1_ERRLGPKT_DROP_EN);
diff --git a/drivers/net/ethernet/atheros/alx/hw.h b/drivers/net/ethernet/atheros/alx/hw.h
index 15548802d6f8..f289c05f5cb4 100644
--- a/drivers/net/ethernet/atheros/alx/hw.h
+++ b/drivers/net/ethernet/atheros/alx/hw.h
@@ -37,6 +37,7 @@
 #include <linux/types.h>
 #include <linux/mdio.h>
 #include <linux/pci.h>
+#include <linux/if_vlan.h>
 #include "reg.h"
 
 /* Transmit Packet Descriptor, contains 4 32-bit words.
@@ -343,12 +344,14 @@ struct alx_rrd {
 					 ALX_RSS_HASH_TYPE_IPV4_TCP | \
 					 ALX_RSS_HASH_TYPE_IPV6 | \
 					 ALX_RSS_HASH_TYPE_IPV6_TCP)
-#define ALX_DEF_RXBUF_SIZE	1536
+#define ALX_FRAME_PAD		16
+#define ALX_RAW_MTU(_mtu)	(_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN)
+#define ALX_MAX_FRAME_LEN(_mtu)	(ALIGN((ALX_RAW_MTU(_mtu) + ALX_FRAME_PAD), 8))
+#define ALX_DEF_RXBUF_SIZE	ALX_MAX_FRAME_LEN(1500)
 #define ALX_MAX_JUMBO_PKT_SIZE	(9*1024)
 #define ALX_MAX_TSO_PKT_SIZE	(7*1024)
 #define ALX_MAX_FRAME_SIZE	ALX_MAX_JUMBO_PKT_SIZE
-#define ALX_MIN_FRAME_SIZE	68
-#define ALX_RAW_MTU(_mtu)	(_mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN)
+#define ALX_MIN_FRAME_SIZE	(ETH_ZLEN + ETH_FCS_LEN + VLAN_HLEN)
 
 #define ALX_MAX_RX_QUEUES	8
 #define ALX_MAX_TX_QUEUES	4
diff --git a/drivers/net/ethernet/atheros/alx/main.c b/drivers/net/ethernet/atheros/alx/main.c
index bd377a6b067d..55b118e876fd 100644
--- a/drivers/net/ethernet/atheros/alx/main.c
+++ b/drivers/net/ethernet/atheros/alx/main.c
@@ -577,7 +577,6 @@ static int alx_alloc_rings(struct alx_priv *alx)
 
 	alx->int_mask &= ~ALX_ISR_ALL_QUEUES;
 	alx->int_mask |= ALX_ISR_TX_Q0 | ALX_ISR_RX_Q0;
-	alx->tx_ringsz = alx->tx_ringsz;
 
 	netif_napi_add(alx->dev, &alx->napi, alx_poll, 64);
 
@@ -705,7 +704,7 @@ static int alx_init_sw(struct alx_priv *alx)
 
 	hw->smb_timer = 400;
 	hw->mtu = alx->dev->mtu;
-	alx->rxbuf_size = ALIGN(ALX_RAW_MTU(hw->mtu), 8);
+	alx->rxbuf_size = ALX_MAX_FRAME_LEN(hw->mtu);
 	alx->tx_ringsz = 256;
 	alx->rx_ringsz = 512;
 	hw->imt = 200;
@@ -806,7 +805,7 @@ static void alx_reinit(struct alx_priv *alx)
 static int alx_change_mtu(struct net_device *netdev, int mtu)
 {
 	struct alx_priv *alx = netdev_priv(netdev);
-	int max_frame = mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
+	int max_frame = ALX_MAX_FRAME_LEN(mtu);
 
 	if ((max_frame < ALX_MIN_FRAME_SIZE) ||
 	    (max_frame > ALX_MAX_FRAME_SIZE))
@@ -817,8 +816,7 @@ static int alx_change_mtu(struct net_device *netdev, int mtu)
 
 	netdev->mtu = mtu;
 	alx->hw.mtu = mtu;
-	alx->rxbuf_size = mtu > ALX_DEF_RXBUF_SIZE ?
-			   ALIGN(max_frame, 8) : ALX_DEF_RXBUF_SIZE;
+	alx->rxbuf_size = max(max_frame, ALX_DEF_RXBUF_SIZE);
 	netdev_update_features(netdev);
 	if (netif_running(netdev))
 		alx_reinit(alx);
diff --git a/drivers/net/ethernet/aurora/nb8800.c b/drivers/net/ethernet/aurora/nb8800.c
index ecc4a334c507..08a23e6b60e9 100644
--- a/drivers/net/ethernet/aurora/nb8800.c
+++ b/drivers/net/ethernet/aurora/nb8800.c
@@ -302,7 +302,7 @@ static int nb8800_poll(struct napi_struct *napi, int budget)
 	nb8800_tx_done(dev);
 
 again:
-	while (work < budget) {
+	do {
 		struct nb8800_rx_buf *rxb;
 		unsigned int len;
 
@@ -330,7 +330,7 @@ again:
 		rxd->report = 0;
 		last = next;
 		work++;
-	}
+	} while (work < budget);
 
 	if (work) {
 		priv->rx_descs[last].desc.config |= DESC_EOC;
@@ -1460,7 +1460,19 @@ static int nb8800_probe(struct platform_device *pdev)
 		goto err_disable_clk;
 	}
 
-	priv->phy_node = of_parse_phandle(pdev->dev.of_node, "phy-handle", 0);
+	if (of_phy_is_fixed_link(pdev->dev.of_node)) {
+		ret = of_phy_register_fixed_link(pdev->dev.of_node);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "bad fixed-link spec\n");
+			goto err_free_bus;
+		}
+		priv->phy_node = of_node_get(pdev->dev.of_node);
+	}
+
+	if (!priv->phy_node)
+		priv->phy_node = of_parse_phandle(pdev->dev.of_node,
+						  "phy-handle", 0);
+
 	if (!priv->phy_node) {
 		dev_err(&pdev->dev, "no PHY specified\n");
 		ret = -ENODEV;
diff --git a/drivers/net/ethernet/broadcom/Kconfig b/drivers/net/ethernet/broadcom/Kconfig
index 8550df189ceb..19f7cd02e085 100644
--- a/drivers/net/ethernet/broadcom/Kconfig
+++ b/drivers/net/ethernet/broadcom/Kconfig
@@ -151,8 +151,11 @@ config BNX2X_VXLAN
 
 config BGMAC
 	tristate "BCMA bus GBit core support"
-	depends on BCMA_HOST_SOC && HAS_DMA && (BCM47XX || ARCH_BCM_5301X)
+	depends on BCMA && BCMA_HOST_SOC
+	depends on HAS_DMA
+	depends on BCM47XX || ARCH_BCM_5301X || COMPILE_TEST
 	select PHYLIB
+	select FIXED_PHY
 	---help---
 	  This driver supports GBit MAC and BCM4706 GBit MAC cores on BCMA bus.
 	  They can be found on BCM47xx SoCs and provide gigabit ethernet.
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index a3b1c07ae0af..74f0a37c4eb6 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -2263,24 +2263,16 @@ static int b44_register_phy_one(struct b44 *bp)
 	mii_bus->parent = sdev->dev;
 	mii_bus->phy_mask = ~(1 << bp->phy_addr);
 	snprintf(mii_bus->id, MII_BUS_ID_SIZE, "%x", instance);
-	mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!mii_bus->irq) {
-		dev_err(sdev->dev, "mii_bus irq allocation failed\n");
-		err = -ENOMEM;
-		goto err_out_mdiobus;
-	}
-
-	memset(mii_bus->irq, PHY_POLL, sizeof(int) * PHY_MAX_ADDR);
 
 	bp->mii_bus = mii_bus;
 
 	err = mdiobus_register(mii_bus);
 	if (err) {
 		dev_err(sdev->dev, "failed to register MII bus\n");
-		goto err_out_mdiobus_irq;
+		goto err_out_mdiobus;
 	}
 
-	if (!bp->mii_bus->phy_map[bp->phy_addr] &&
+	if (!mdiobus_is_registered_device(bp->mii_bus, bp->phy_addr) &&
 	    (sprom->boardflags_lo & (B44_BOARDFLAG_ROBO | B44_BOARDFLAG_ADM))) {
 
 		dev_info(sdev->dev,
@@ -2313,19 +2305,15 @@ static int b44_register_phy_one(struct b44 *bp)
 
 	bp->phydev = phydev;
 	bp->old_link = 0;
-	bp->phy_addr = phydev->addr;
+	bp->phy_addr = phydev->mdio.addr;
 
-	dev_info(sdev->dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s)\n",
-		 phydev->drv->name, dev_name(&phydev->dev));
+	phy_attached_info(phydev);
 
 	return 0;
 
 err_out_mdiobus_unregister:
 	mdiobus_unregister(mii_bus);
 
-err_out_mdiobus_irq:
-	kfree(mii_bus->irq);
-
 err_out_mdiobus:
 	mdiobus_free(mii_bus);
 
@@ -2339,7 +2327,6 @@ static void b44_unregister_phy_one(struct b44 *bp)
 
 	phy_disconnect(bp->phydev);
 	mdiobus_unregister(mii_bus);
-	kfree(mii_bus->irq);
 	mdiobus_free(mii_bus);
 }
 
diff --git a/drivers/net/ethernet/broadcom/bcm63xx_enet.c b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
index 8b1929e9f698..87c6b5bdd616 100644
--- a/drivers/net/ethernet/broadcom/bcm63xx_enet.c
+++ b/drivers/net/ethernet/broadcom/bcm63xx_enet.c
@@ -908,8 +908,7 @@ static int bcm_enet_open(struct net_device *dev)
 		else
 			phydev->advertising &= ~SUPPORTED_Pause;
 
-		dev_info(kdev, "attached PHY at address %d [%s]\n",
-			 phydev->addr, phydev->drv->name);
+		phy_attached_info(phydev);
 
 		priv->old_link = 0;
 		priv->old_duplex = -1;
@@ -1849,17 +1848,8 @@ static int bcm_enet_probe(struct platform_device *pdev)
 		 * if a slave is not present on hw */
 		bus->phy_mask = ~(1 << priv->phy_id);
 
-		bus->irq = devm_kzalloc(&pdev->dev, sizeof(int) * PHY_MAX_ADDR,
-					GFP_KERNEL);
-		if (!bus->irq) {
-			ret = -ENOMEM;
-			goto out_free_mdio;
-		}
-
 		if (priv->has_phy_interrupt)
 			bus->irq[priv->phy_id] = priv->phy_interrupt;
-		else
-			bus->irq[priv->phy_id] = PHY_POLL;
 
 		ret = mdiobus_register(bus);
 		if (ret) {
@@ -2884,33 +2874,21 @@ struct platform_driver bcm63xx_enet_shared_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&bcm63xx_enet_shared_driver,
+	&bcm63xx_enet_driver,
+	&bcm63xx_enetsw_driver,
+};
+
 /* entry point */
 static int __init bcm_enet_init(void)
 {
-	int ret;
-
-	ret = platform_driver_register(&bcm63xx_enet_shared_driver);
-	if (ret)
-		return ret;
-
-	ret = platform_driver_register(&bcm63xx_enet_driver);
-	if (ret)
-		platform_driver_unregister(&bcm63xx_enet_shared_driver);
-
-	ret = platform_driver_register(&bcm63xx_enetsw_driver);
-	if (ret) {
-		platform_driver_unregister(&bcm63xx_enet_driver);
-		platform_driver_unregister(&bcm63xx_enet_shared_driver);
-	}
-
-	return ret;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit bcm_enet_exit(void)
 {
-	platform_driver_unregister(&bcm63xx_enet_driver);
-	platform_driver_unregister(&bcm63xx_enetsw_driver);
-	platform_driver_unregister(&bcm63xx_enet_shared_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 858106352ce9..993c780bdfab 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -1216,7 +1216,7 @@ static int bcm_sysport_init_tx_ring(struct bcm_sysport_priv *priv,
 	/* Initialize SW view of the ring */
 	spin_lock_init(&ring->lock);
 	ring->priv = priv;
-	netif_napi_add(priv->netdev, &ring->napi, bcm_sysport_tx_poll, 64);
+	netif_tx_napi_add(priv->netdev, &ring->napi, bcm_sysport_tx_poll, 64);
 	ring->index = index;
 	ring->size = size;
 	ring->alloc_size = ring->size;
diff --git a/drivers/net/ethernet/broadcom/bgmac.c b/drivers/net/ethernet/broadcom/bgmac.c
index 28f7610b03fe..06f6cffdfaf5 100644
--- a/drivers/net/ethernet/broadcom/bgmac.c
+++ b/drivers/net/ethernet/broadcom/bgmac.c
@@ -466,6 +466,11 @@ static int bgmac_dma_rx_read(struct bgmac *bgmac, struct bgmac_dma_ring *ring,
 			len -= ETH_FCS_LEN;
 
 			skb = build_skb(buf, BGMAC_RX_ALLOC_SIZE);
+			if (unlikely(!skb)) {
+				bgmac_err(bgmac, "build_skb failed\n");
+				put_page(virt_to_head_page(buf));
+				break;
+			}
 			skb_put(skb, BGMAC_RX_FRAME_OFFSET +
 				BGMAC_RX_BUF_OFFSET + len);
 			skb_pull(skb, BGMAC_RX_FRAME_OFFSET +
@@ -1471,7 +1476,7 @@ static int bgmac_mii_register(struct bgmac *bgmac)
 	struct mii_bus *mii_bus;
 	struct phy_device *phy_dev;
 	char bus_id[MII_BUS_ID_SIZE + 3];
-	int i, err = 0;
+	int err = 0;
 
 	if (ci->id == BCMA_CHIP_ID_BCM4707 ||
 	    ci->id == BCMA_CHIP_ID_BCM53018)
@@ -1490,18 +1495,10 @@ static int bgmac_mii_register(struct bgmac *bgmac)
 	mii_bus->parent = &bgmac->core->dev;
 	mii_bus->phy_mask = ~(1 << bgmac->phyaddr);
 
-	mii_bus->irq = kmalloc_array(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
-	if (!mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_free_bus;
-	}
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		mii_bus->irq[i] = PHY_POLL;
-
 	err = mdiobus_register(mii_bus);
 	if (err) {
 		bgmac_err(bgmac, "Registration of mii bus failed\n");
-		goto err_free_irq;
+		goto err_free_bus;
 	}
 
 	bgmac->mii_bus = mii_bus;
@@ -1522,8 +1519,6 @@ static int bgmac_mii_register(struct bgmac *bgmac)
 
 err_unregister_bus:
 	mdiobus_unregister(mii_bus);
-err_free_irq:
-	kfree(mii_bus->irq);
 err_free_bus:
 	mdiobus_free(mii_bus);
 	return err;
@@ -1534,7 +1529,6 @@ static void bgmac_mii_unregister(struct bgmac *bgmac)
 	struct mii_bus *mii_bus = bgmac->mii_bus;
 
 	mdiobus_unregister(mii_bus);
-	kfree(mii_bus->irq);
 	mdiobus_free(mii_bus);
 }
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index b5e64b02200c..cae0956186ce 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -540,10 +540,6 @@ struct bnx2x_fastpath {
 
 	struct napi_struct	napi;
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	unsigned long		busy_poll_state;
-#endif
-
 	union host_hc_status_block	status_blk;
 	/* chip independent shortcuts into sb structure */
 	__le16			*sb_index_values;
@@ -594,8 +590,6 @@ struct bnx2x_fastpath {
 	/* The last maximal completed SGE */
 	u16			last_max_sge;
 	__le16			*rx_cons_sb;
-	unsigned long		rx_pkt,
-				rx_calls;
 
 	/* TPA related */
 	struct bnx2x_agg_info	*tpa_info;
@@ -617,115 +611,6 @@ struct bnx2x_fastpath {
 #define bnx2x_fp_stats(bp, fp)	(&((bp)->fp_stats[(fp)->index]))
 #define bnx2x_fp_qstats(bp, fp)	(&((bp)->fp_stats[(fp)->index].eth_q_stats))
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-
-enum bnx2x_fp_state {
-	BNX2X_STATE_FP_NAPI	= BIT(0), /* NAPI handler owns the queue */
-
-	BNX2X_STATE_FP_NAPI_REQ_BIT = 1, /* NAPI would like to own the queue */
-	BNX2X_STATE_FP_NAPI_REQ = BIT(1),
-
-	BNX2X_STATE_FP_POLL_BIT = 2,
-	BNX2X_STATE_FP_POLL     = BIT(2), /* busy_poll owns the queue */
-
-	BNX2X_STATE_FP_DISABLE_BIT = 3, /* queue is dismantled */
-};
-
-static inline void bnx2x_fp_busy_poll_init(struct bnx2x_fastpath *fp)
-{
-	WRITE_ONCE(fp->busy_poll_state, 0);
-}
-
-/* called from the device poll routine to get ownership of a FP */
-static inline bool bnx2x_fp_lock_napi(struct bnx2x_fastpath *fp)
-{
-	unsigned long prev, old = READ_ONCE(fp->busy_poll_state);
-
-	while (1) {
-		switch (old) {
-		case BNX2X_STATE_FP_POLL:
-			/* make sure bnx2x_fp_lock_poll() wont starve us */
-			set_bit(BNX2X_STATE_FP_NAPI_REQ_BIT,
-				&fp->busy_poll_state);
-			/* fallthrough */
-		case BNX2X_STATE_FP_POLL | BNX2X_STATE_FP_NAPI_REQ:
-			return false;
-		default:
-			break;
-		}
-		prev = cmpxchg(&fp->busy_poll_state, old, BNX2X_STATE_FP_NAPI);
-		if (unlikely(prev != old)) {
-			old = prev;
-			continue;
-		}
-		return true;
-	}
-}
-
-static inline void bnx2x_fp_unlock_napi(struct bnx2x_fastpath *fp)
-{
-	smp_wmb();
-	fp->busy_poll_state = 0;
-}
-
-/* called from bnx2x_low_latency_poll() */
-static inline bool bnx2x_fp_lock_poll(struct bnx2x_fastpath *fp)
-{
-	return cmpxchg(&fp->busy_poll_state, 0, BNX2X_STATE_FP_POLL) == 0;
-}
-
-static inline void bnx2x_fp_unlock_poll(struct bnx2x_fastpath *fp)
-{
-	smp_mb__before_atomic();
-	clear_bit(BNX2X_STATE_FP_POLL_BIT, &fp->busy_poll_state);
-}
-
-/* true if a socket is polling */
-static inline bool bnx2x_fp_ll_polling(struct bnx2x_fastpath *fp)
-{
-	return READ_ONCE(fp->busy_poll_state) & BNX2X_STATE_FP_POLL;
-}
-
-/* false if fp is currently owned */
-static inline bool bnx2x_fp_ll_disable(struct bnx2x_fastpath *fp)
-{
-	set_bit(BNX2X_STATE_FP_DISABLE_BIT, &fp->busy_poll_state);
-	return !bnx2x_fp_ll_polling(fp);
-
-}
-#else
-static inline void bnx2x_fp_busy_poll_init(struct bnx2x_fastpath *fp)
-{
-}
-
-static inline bool bnx2x_fp_lock_napi(struct bnx2x_fastpath *fp)
-{
-	return true;
-}
-
-static inline void bnx2x_fp_unlock_napi(struct bnx2x_fastpath *fp)
-{
-}
-
-static inline bool bnx2x_fp_lock_poll(struct bnx2x_fastpath *fp)
-{
-	return false;
-}
-
-static inline void bnx2x_fp_unlock_poll(struct bnx2x_fastpath *fp)
-{
-}
-
-static inline bool bnx2x_fp_ll_polling(struct bnx2x_fastpath *fp)
-{
-	return false;
-}
-static inline bool bnx2x_fp_ll_disable(struct bnx2x_fastpath *fp)
-{
-	return true;
-}
-#endif /* CONFIG_NET_RX_BUSY_POLL */
-
 /* Use 2500 as a mini-jumbo MTU for FCoE */
 #define BNX2X_FCOE_MINI_JUMBO_MTU	2500
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index c82ab87fcbe8..9695a4c4a434 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -46,7 +46,6 @@ static void bnx2x_add_all_napi_cnic(struct bnx2x *bp)
 	for_each_rx_queue_cnic(bp, i) {
 		netif_napi_add(bp->dev, &bnx2x_fp(bp, i, napi),
 			       bnx2x_poll, NAPI_POLL_WEIGHT);
-		napi_hash_add(&bnx2x_fp(bp, i, napi));
 	}
 }
 
@@ -58,7 +57,6 @@ static void bnx2x_add_all_napi(struct bnx2x *bp)
 	for_each_eth_queue(bp, i) {
 		netif_napi_add(bp->dev, &bnx2x_fp(bp, i, napi),
 			       bnx2x_poll, NAPI_POLL_WEIGHT);
-		napi_hash_add(&bnx2x_fp(bp, i, napi));
 	}
 }
 
@@ -560,10 +558,8 @@ static int bnx2x_alloc_rx_sge(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			put_page(pool->page);
 
 		pool->page = alloc_pages(gfp_mask, PAGES_PER_SGE_SHIFT);
-		if (unlikely(!pool->page)) {
-			BNX2X_ERR("Can't alloc sge\n");
+		if (unlikely(!pool->page))
 			return -ENOMEM;
-		}
 
 		pool->offset = 0;
 	}
@@ -747,7 +743,7 @@ static void bnx2x_gro_receive(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 			bnx2x_gro_csum(bp, skb, bnx2x_gro_ipv6_csum);
 			break;
 		default:
-			BNX2X_ERR("Error: FW GRO supports only IPv4/IPv6, not 0x%04x\n",
+			WARN_ONCE(1, "Error: FW GRO supports only IPv4/IPv6, not 0x%04x\n",
 				  be16_to_cpu(skb->protocol));
 		}
 	}
@@ -1094,12 +1090,7 @@ reuse_rx:
 			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
 					       le16_to_cpu(cqe_fp->vlan_tag));
 
-		skb_mark_napi_id(skb, &fp->napi);
-
-		if (bnx2x_fp_ll_polling(fp))
-			netif_receive_skb(skb);
-		else
-			napi_gro_receive(&fp->napi, skb);
+		napi_gro_receive(&fp->napi, skb);
 next_rx:
 		rx_buf->data = NULL;
 
@@ -1131,9 +1122,6 @@ next_cqe:
 	bnx2x_update_rx_prod(bp, fp, bd_prod_fw, sw_comp_prod,
 			     fp->rx_sge_prod);
 
-	fp->rx_pkt += rx_pkt;
-	fp->rx_calls++;
-
 	return rx_pkt;
 }
 
@@ -1869,7 +1857,6 @@ static void bnx2x_napi_enable_cnic(struct bnx2x *bp)
 	int i;
 
 	for_each_rx_queue_cnic(bp, i) {
-		bnx2x_fp_busy_poll_init(&bp->fp[i]);
 		napi_enable(&bnx2x_fp(bp, i, napi));
 	}
 }
@@ -1879,7 +1866,6 @@ static void bnx2x_napi_enable(struct bnx2x *bp)
 	int i;
 
 	for_each_eth_queue(bp, i) {
-		bnx2x_fp_busy_poll_init(&bp->fp[i]);
 		napi_enable(&bnx2x_fp(bp, i, napi));
 	}
 }
@@ -1890,8 +1876,6 @@ static void bnx2x_napi_disable_cnic(struct bnx2x *bp)
 
 	for_each_rx_queue_cnic(bp, i) {
 		napi_disable(&bnx2x_fp(bp, i, napi));
-		while (!bnx2x_fp_ll_disable(&bp->fp[i]))
-			usleep_range(1000, 2000);
 	}
 }
 
@@ -1901,8 +1885,6 @@ static void bnx2x_napi_disable(struct bnx2x *bp)
 
 	for_each_eth_queue(bp, i) {
 		napi_disable(&bnx2x_fp(bp, i, napi));
-		while (!bnx2x_fp_ll_disable(&bp->fp[i]))
-			usleep_range(1000, 2000);
 	}
 }
 
@@ -3219,49 +3201,32 @@ int bnx2x_set_power_state(struct bnx2x *bp, pci_power_t state)
  */
 static int bnx2x_poll(struct napi_struct *napi, int budget)
 {
-	int work_done = 0;
-	u8 cos;
 	struct bnx2x_fastpath *fp = container_of(napi, struct bnx2x_fastpath,
 						 napi);
 	struct bnx2x *bp = fp->bp;
+	int rx_work_done;
+	u8 cos;
 
-	while (1) {
 #ifdef BNX2X_STOP_ON_ERROR
-		if (unlikely(bp->panic)) {
-			napi_complete(napi);
-			return 0;
-		}
+	if (unlikely(bp->panic)) {
+		napi_complete(napi);
+		return 0;
+	}
 #endif
-		if (!bnx2x_fp_lock_napi(fp))
-			return budget;
-
-		for_each_cos_in_tx_queue(fp, cos)
-			if (bnx2x_tx_queue_has_work(fp->txdata_ptr[cos]))
-				bnx2x_tx_int(bp, fp->txdata_ptr[cos]);
-
-		if (bnx2x_has_rx_work(fp)) {
-			work_done += bnx2x_rx_int(fp, budget - work_done);
-
-			/* must not complete if we consumed full budget */
-			if (work_done >= budget) {
-				bnx2x_fp_unlock_napi(fp);
-				break;
-			}
-		}
-
-		bnx2x_fp_unlock_napi(fp);
+	for_each_cos_in_tx_queue(fp, cos)
+		if (bnx2x_tx_queue_has_work(fp->txdata_ptr[cos]))
+			bnx2x_tx_int(bp, fp->txdata_ptr[cos]);
 
-		/* Fall out from the NAPI loop if needed */
-		if (!(bnx2x_has_rx_work(fp) || bnx2x_has_tx_work(fp))) {
+	rx_work_done = (bnx2x_has_rx_work(fp)) ? bnx2x_rx_int(fp, budget) : 0;
 
-			/* No need to update SB for FCoE L2 ring as long as
-			 * it's connected to the default SB and the SB
-			 * has been updated when NAPI was scheduled.
-			 */
-			if (IS_FCOE_FP(fp)) {
-				napi_complete(napi);
-				break;
-			}
+	if (rx_work_done < budget) {
+		/* No need to update SB for FCoE L2 ring as long as
+		 * it's connected to the default SB and the SB
+		 * has been updated when NAPI was scheduled.
+		 */
+		if (IS_FCOE_FP(fp)) {
+			napi_complete(napi);
+		} else {
 			bnx2x_update_fpsb_idx(fp);
 			/* bnx2x_has_rx_work() reads the status block,
 			 * thus we need to ensure that status block indices
@@ -3286,40 +3251,15 @@ static int bnx2x_poll(struct napi_struct *napi, int budget)
 				bnx2x_ack_sb(bp, fp->igu_sb_id, USTORM_ID,
 					     le16_to_cpu(fp->fp_hc_idx),
 					     IGU_INT_ENABLE, 1);
-				break;
+			} else {
+				rx_work_done = budget;
 			}
 		}
 	}
 
-	return work_done;
+	return rx_work_done;
 }
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-/* must be called with local_bh_disable()d */
-int bnx2x_low_latency_recv(struct napi_struct *napi)
-{
-	struct bnx2x_fastpath *fp = container_of(napi, struct bnx2x_fastpath,
-						 napi);
-	struct bnx2x *bp = fp->bp;
-	int found = 0;
-
-	if ((bp->state == BNX2X_STATE_CLOSED) ||
-	    (bp->state == BNX2X_STATE_ERROR) ||
-	    (bp->dev->features & (NETIF_F_LRO | NETIF_F_GRO)))
-		return LL_FLUSH_FAILED;
-
-	if (!bnx2x_fp_lock_poll(fp))
-		return LL_FLUSH_BUSY;
-
-	if (bnx2x_has_rx_work(fp))
-		found = bnx2x_rx_int(fp, 4);
-
-	bnx2x_fp_unlock_poll(fp);
-
-	return found;
-}
-#endif
-
 /* we split the first BD into headers and data BDs
  * to ease the pain of our fellow microcode engineers
  * we use one mapping for both BDs
@@ -4494,7 +4434,6 @@ static int bnx2x_alloc_rx_bds(struct bnx2x_fastpath *fp,
 	/* Limit the CQE producer by the CQE ring size */
 	fp->rx_comp_prod = min_t(u16, NUM_RCQ_RINGS*RCQ_DESC_CNT,
 			       cqe_ring_prod);
-	fp->rx_pkt = fp->rx_calls = 0;
 
 	bnx2x_fp_stats(bp, fp)->eth_q_stats.rx_skb_alloc_failed += failure_cnt;
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index b7d32e8412f1..4cbb03f87b5a 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -570,13 +570,6 @@ int bnx2x_enable_msix(struct bnx2x *bp);
 int bnx2x_enable_msi(struct bnx2x *bp);
 
 /**
- * bnx2x_low_latency_recv - LL callback
- *
- * @napi:	napi structure
- */
-int bnx2x_low_latency_recv(struct napi_struct *napi);
-
-/**
  * bnx2x_alloc_mem_bp - allocate memories outsize main driver structure
  *
  * @bp:		driver handle
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index d84efcd34fac..820b7e04bb5f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -52,7 +52,7 @@ static const struct {
 	{ Q_STATS_OFFSET32(rx_skb_alloc_failed),
 					 4, "[%s]: rx_skb_alloc_discard" },
 	{ Q_STATS_OFFSET32(hw_csum_err), 4, "[%s]: rx_csum_offload_errors" },
-
+	{ Q_STATS_OFFSET32(driver_xoff), 4, "[%s]: tx_exhaustion_events" },
 	{ Q_STATS_OFFSET32(total_bytes_transmitted_hi),	8, "[%s]: tx_bytes" },
 /* 10 */{ Q_STATS_OFFSET32(total_unicast_packets_transmitted_hi),
 						8, "[%s]: tx_ucast_packets" },
@@ -74,117 +74,115 @@ static const struct {
 static const struct {
 	long offset;
 	int size;
-	u32 flags;
-#define STATS_FLAGS_PORT		1
-#define STATS_FLAGS_FUNC		2
-#define STATS_FLAGS_BOTH		(STATS_FLAGS_FUNC | STATS_FLAGS_PORT)
+	bool is_port_stat;
 	char string[ETH_GSTRING_LEN];
 } bnx2x_stats_arr[] = {
 /* 1 */	{ STATS_OFFSET32(total_bytes_received_hi),
-				8, STATS_FLAGS_BOTH, "rx_bytes" },
+				8, false, "rx_bytes" },
 	{ STATS_OFFSET32(error_bytes_received_hi),
-				8, STATS_FLAGS_BOTH, "rx_error_bytes" },
+				8, false, "rx_error_bytes" },
 	{ STATS_OFFSET32(total_unicast_packets_received_hi),
-				8, STATS_FLAGS_BOTH, "rx_ucast_packets" },
+				8, false, "rx_ucast_packets" },
 	{ STATS_OFFSET32(total_multicast_packets_received_hi),
-				8, STATS_FLAGS_BOTH, "rx_mcast_packets" },
+				8, false, "rx_mcast_packets" },
 	{ STATS_OFFSET32(total_broadcast_packets_received_hi),
-				8, STATS_FLAGS_BOTH, "rx_bcast_packets" },
+				8, false, "rx_bcast_packets" },
 	{ STATS_OFFSET32(rx_stat_dot3statsfcserrors_hi),
-				8, STATS_FLAGS_PORT, "rx_crc_errors" },
+				8, true, "rx_crc_errors" },
 	{ STATS_OFFSET32(rx_stat_dot3statsalignmenterrors_hi),
-				8, STATS_FLAGS_PORT, "rx_align_errors" },
+				8, true, "rx_align_errors" },
 	{ STATS_OFFSET32(rx_stat_etherstatsundersizepkts_hi),
-				8, STATS_FLAGS_PORT, "rx_undersize_packets" },
+				8, true, "rx_undersize_packets" },
 	{ STATS_OFFSET32(etherstatsoverrsizepkts_hi),
-				8, STATS_FLAGS_PORT, "rx_oversize_packets" },
+				8, true, "rx_oversize_packets" },
 /* 10 */{ STATS_OFFSET32(rx_stat_etherstatsfragments_hi),
-				8, STATS_FLAGS_PORT, "rx_fragments" },
+				8, true, "rx_fragments" },
 	{ STATS_OFFSET32(rx_stat_etherstatsjabbers_hi),
-				8, STATS_FLAGS_PORT, "rx_jabbers" },
+				8, true, "rx_jabbers" },
 	{ STATS_OFFSET32(no_buff_discard_hi),
-				8, STATS_FLAGS_BOTH, "rx_discards" },
+				8, false, "rx_discards" },
 	{ STATS_OFFSET32(mac_filter_discard),
-				4, STATS_FLAGS_PORT, "rx_filtered_packets" },
+				4, true, "rx_filtered_packets" },
 	{ STATS_OFFSET32(mf_tag_discard),
-				4, STATS_FLAGS_PORT, "rx_mf_tag_discard" },
+				4, true, "rx_mf_tag_discard" },
 	{ STATS_OFFSET32(pfc_frames_received_hi),
-				8, STATS_FLAGS_PORT, "pfc_frames_received" },
+				8, true, "pfc_frames_received" },
 	{ STATS_OFFSET32(pfc_frames_sent_hi),
-				8, STATS_FLAGS_PORT, "pfc_frames_sent" },
+				8, true, "pfc_frames_sent" },
 	{ STATS_OFFSET32(brb_drop_hi),
-				8, STATS_FLAGS_PORT, "rx_brb_discard" },
+				8, true, "rx_brb_discard" },
 	{ STATS_OFFSET32(brb_truncate_hi),
-				8, STATS_FLAGS_PORT, "rx_brb_truncate" },
+				8, true, "rx_brb_truncate" },
 	{ STATS_OFFSET32(pause_frames_received_hi),
-				8, STATS_FLAGS_PORT, "rx_pause_frames" },
+				8, true, "rx_pause_frames" },
 	{ STATS_OFFSET32(rx_stat_maccontrolframesreceived_hi),
-				8, STATS_FLAGS_PORT, "rx_mac_ctrl_frames" },
+				8, true, "rx_mac_ctrl_frames" },
 	{ STATS_OFFSET32(nig_timer_max),
-			4, STATS_FLAGS_PORT, "rx_constant_pause_events" },
+				4, true, "rx_constant_pause_events" },
 /* 20 */{ STATS_OFFSET32(rx_err_discard_pkt),
-				4, STATS_FLAGS_BOTH, "rx_phy_ip_err_discards"},
+				4, false, "rx_phy_ip_err_discards"},
 	{ STATS_OFFSET32(rx_skb_alloc_failed),
-				4, STATS_FLAGS_BOTH, "rx_skb_alloc_discard" },
+				4, false, "rx_skb_alloc_discard" },
 	{ STATS_OFFSET32(hw_csum_err),
-				4, STATS_FLAGS_BOTH, "rx_csum_offload_errors" },
-
+				4, false, "rx_csum_offload_errors" },
+	{ STATS_OFFSET32(driver_xoff),
+				4, false, "tx_exhaustion_events" },
 	{ STATS_OFFSET32(total_bytes_transmitted_hi),
-				8, STATS_FLAGS_BOTH, "tx_bytes" },
+				8, false, "tx_bytes" },
 	{ STATS_OFFSET32(tx_stat_ifhcoutbadoctets_hi),
-				8, STATS_FLAGS_PORT, "tx_error_bytes" },
+				8, true, "tx_error_bytes" },
 	{ STATS_OFFSET32(total_unicast_packets_transmitted_hi),
-				8, STATS_FLAGS_BOTH, "tx_ucast_packets" },
+				8, false, "tx_ucast_packets" },
 	{ STATS_OFFSET32(total_multicast_packets_transmitted_hi),
-				8, STATS_FLAGS_BOTH, "tx_mcast_packets" },
+				8, false, "tx_mcast_packets" },
 	{ STATS_OFFSET32(total_broadcast_packets_transmitted_hi),
-				8, STATS_FLAGS_BOTH, "tx_bcast_packets" },
+				8, false, "tx_bcast_packets" },
 	{ STATS_OFFSET32(tx_stat_dot3statsinternalmactransmiterrors_hi),
-				8, STATS_FLAGS_PORT, "tx_mac_errors" },
+				8, true, "tx_mac_errors" },
 	{ STATS_OFFSET32(rx_stat_dot3statscarriersenseerrors_hi),
-				8, STATS_FLAGS_PORT, "tx_carrier_errors" },
+				8, true, "tx_carrier_errors" },
 /* 30 */{ STATS_OFFSET32(tx_stat_dot3statssinglecollisionframes_hi),
-				8, STATS_FLAGS_PORT, "tx_single_collisions" },
+				8, true, "tx_single_collisions" },
 	{ STATS_OFFSET32(tx_stat_dot3statsmultiplecollisionframes_hi),
-				8, STATS_FLAGS_PORT, "tx_multi_collisions" },
+				8, true, "tx_multi_collisions" },
 	{ STATS_OFFSET32(tx_stat_dot3statsdeferredtransmissions_hi),
-				8, STATS_FLAGS_PORT, "tx_deferred" },
+				8, true, "tx_deferred" },
 	{ STATS_OFFSET32(tx_stat_dot3statsexcessivecollisions_hi),
-				8, STATS_FLAGS_PORT, "tx_excess_collisions" },
+				8, true, "tx_excess_collisions" },
 	{ STATS_OFFSET32(tx_stat_dot3statslatecollisions_hi),
-				8, STATS_FLAGS_PORT, "tx_late_collisions" },
+				8, true, "tx_late_collisions" },
 	{ STATS_OFFSET32(tx_stat_etherstatscollisions_hi),
-				8, STATS_FLAGS_PORT, "tx_total_collisions" },
+				8, true, "tx_total_collisions" },
 	{ STATS_OFFSET32(tx_stat_etherstatspkts64octets_hi),
-				8, STATS_FLAGS_PORT, "tx_64_byte_packets" },
+				8, true, "tx_64_byte_packets" },
 	{ STATS_OFFSET32(tx_stat_etherstatspkts65octetsto127octets_hi),
-			8, STATS_FLAGS_PORT, "tx_65_to_127_byte_packets" },
+				8, true, "tx_65_to_127_byte_packets" },
 	{ STATS_OFFSET32(tx_stat_etherstatspkts128octetsto255octets_hi),
-			8, STATS_FLAGS_PORT, "tx_128_to_255_byte_packets" },
+				8, true, "tx_128_to_255_byte_packets" },
 	{ STATS_OFFSET32(tx_stat_etherstatspkts256octetsto511octets_hi),
-			8, STATS_FLAGS_PORT, "tx_256_to_511_byte_packets" },
+				8, true, "tx_256_to_511_byte_packets" },
 /* 40 */{ STATS_OFFSET32(tx_stat_etherstatspkts512octetsto1023octets_hi),
-			8, STATS_FLAGS_PORT, "tx_512_to_1023_byte_packets" },
+				8, true, "tx_512_to_1023_byte_packets" },
 	{ STATS_OFFSET32(etherstatspkts1024octetsto1522octets_hi),
-			8, STATS_FLAGS_PORT, "tx_1024_to_1522_byte_packets" },
+				8, true, "tx_1024_to_1522_byte_packets" },
 	{ STATS_OFFSET32(etherstatspktsover1522octets_hi),
-			8, STATS_FLAGS_PORT, "tx_1523_to_9022_byte_packets" },
+				8, true, "tx_1523_to_9022_byte_packets" },
 	{ STATS_OFFSET32(pause_frames_sent_hi),
-				8, STATS_FLAGS_PORT, "tx_pause_frames" },
+				8, true, "tx_pause_frames" },
 	{ STATS_OFFSET32(total_tpa_aggregations_hi),
-			8, STATS_FLAGS_FUNC, "tpa_aggregations" },
+				8, false, "tpa_aggregations" },
 	{ STATS_OFFSET32(total_tpa_aggregated_frames_hi),
-			8, STATS_FLAGS_FUNC, "tpa_aggregated_frames"},
+				8, false, "tpa_aggregated_frames"},
 	{ STATS_OFFSET32(total_tpa_bytes_hi),
-			8, STATS_FLAGS_FUNC, "tpa_bytes"},
+				8, false, "tpa_bytes"},
 	{ STATS_OFFSET32(recoverable_error),
-			4, STATS_FLAGS_FUNC, "recoverable_errors" },
+				4, false, "recoverable_errors" },
 	{ STATS_OFFSET32(unrecoverable_error),
-			4, STATS_FLAGS_FUNC, "unrecoverable_errors" },
+				4, false, "unrecoverable_errors" },
 	{ STATS_OFFSET32(driver_filtered_tx_pkt),
-			4, STATS_FLAGS_FUNC, "driver_filtered_tx_pkt" },
+				4, false, "driver_filtered_tx_pkt" },
 	{ STATS_OFFSET32(eee_tx_lpi),
-			4, STATS_FLAGS_PORT, "Tx LPI entry count"}
+				4, true, "Tx LPI entry count"}
 };
 
 #define BNX2X_NUM_STATS		ARRAY_SIZE(bnx2x_stats_arr)
@@ -3065,12 +3063,8 @@ static void bnx2x_self_test(struct net_device *dev,
 	}
 }
 
-#define IS_PORT_STAT(i) \
-	((bnx2x_stats_arr[i].flags & STATS_FLAGS_BOTH) == STATS_FLAGS_PORT)
-#define IS_FUNC_STAT(i)		(bnx2x_stats_arr[i].flags & STATS_FLAGS_FUNC)
-#define HIDE_PORT_STAT(bp) \
-		((IS_MF(bp) && !(bp->msg_enable & BNX2X_MSG_STATS)) || \
-		 IS_VF(bp))
+#define IS_PORT_STAT(i)		(bnx2x_stats_arr[i].is_port_stat)
+#define HIDE_PORT_STAT(bp)	IS_VF(bp)
 
 /* ethtool statistics are displayed for all regular ethernet queues and the
  * fcoe L2 queue if not disabled
@@ -3094,7 +3088,7 @@ static int bnx2x_get_sset_count(struct net_device *dev, int stringset)
 			num_strings = 0;
 		if (HIDE_PORT_STAT(bp)) {
 			for (i = 0; i < BNX2X_NUM_STATS; i++)
-				if (IS_FUNC_STAT(i))
+				if (!IS_PORT_STAT(i))
 					num_strings++;
 		} else
 			num_strings += BNX2X_NUM_STATS;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index cafd5de675cf..91874d24fd56 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -3013,8 +3013,8 @@ struct afex_stats {
 };
 
 #define BCM_5710_FW_MAJOR_VERSION			7
-#define BCM_5710_FW_MINOR_VERSION			12
-#define BCM_5710_FW_REVISION_VERSION		30
+#define BCM_5710_FW_MINOR_VERSION			13
+#define BCM_5710_FW_REVISION_VERSION		1
 #define BCM_5710_FW_ENGINEERING_VERSION		0
 #define BCM_5710_FW_COMPILE_FLAGS			1
 
@@ -3583,7 +3583,7 @@ enum classify_rule {
 	CLASSIFY_RULE_OPCODE_MAC,
 	CLASSIFY_RULE_OPCODE_VLAN,
 	CLASSIFY_RULE_OPCODE_PAIR,
-	CLASSIFY_RULE_OPCODE_VXLAN,
+	CLASSIFY_RULE_OPCODE_IMAC_VNI,
 	MAX_CLASSIFY_RULE
 };
 
@@ -3826,6 +3826,17 @@ struct eth_classify_header {
 	__le32 echo;
 };
 
+/*
+ * Command for adding/removing a Inner-MAC/VNI classification rule
+ */
+struct eth_classify_imac_vni_cmd {
+	struct eth_classify_cmd_header header;
+	__le32 vni;
+	__le16 imac_lsb;
+	__le16 imac_mid;
+	__le16 imac_msb;
+	__le16 reserved1;
+};
 
 /*
  * Command for adding/removing a MAC classification rule
@@ -3869,14 +3880,6 @@ struct eth_classify_vlan_cmd {
 /*
  * Command for adding/removing a VXLAN classification rule
  */
-struct eth_classify_vxlan_cmd {
-	struct eth_classify_cmd_header header;
-	__le32 vni;
-	__le16 inner_mac_lsb;
-	__le16 inner_mac_mid;
-	__le16 inner_mac_msb;
-	__le16 reserved1;
-};
 
 /*
  * union for eth classification rule
@@ -3885,7 +3888,7 @@ union eth_classify_rule_cmd {
 	struct eth_classify_mac_cmd mac;
 	struct eth_classify_vlan_cmd vlan;
 	struct eth_classify_pair_cmd pair;
-	struct eth_classify_vxlan_cmd vxlan;
+	struct eth_classify_imac_vni_cmd imac_vni;
 };
 
 /*
@@ -4893,9 +4896,9 @@ struct c2s_pri_trans_table_entry {
  * cfc delete event data
  */
 struct cfc_del_event_data {
-	u32 cid;
-	u32 reserved0;
-	u32 reserved1;
+	__le32 cid;
+	__le32 reserved0;
+	__le32 reserved1;
 };
 
 
@@ -5111,15 +5114,9 @@ struct vf_pf_channel_zone_trigger {
  * zone that triggers the in-bound interrupt
  */
 struct trigger_vf_zone {
-#if defined(__BIG_ENDIAN)
-	u16 reserved1;
-	u8 reserved0;
-	struct vf_pf_channel_zone_trigger vf_pf_channel;
-#elif defined(__LITTLE_ENDIAN)
 	struct vf_pf_channel_zone_trigger vf_pf_channel;
 	u8 reserved0;
 	u16 reserved1;
-#endif
 	u32 reserved2;
 };
 
@@ -5204,9 +5201,9 @@ struct e2_integ_data {
  * set mac event data
  */
 struct eth_event_data {
-	u32 echo;
-	u32 reserved0;
-	u32 reserved1;
+	__le32 echo;
+	__le32 reserved0;
+	__le32 reserved1;
 };
 
 
@@ -5216,9 +5213,9 @@ struct eth_event_data {
 struct vf_pf_event_data {
 	u8 vf_id;
 	u8 reserved0;
-	u16 reserved1;
-	u32 msg_addr_lo;
-	u32 msg_addr_hi;
+	__le16 reserved1;
+	__le32 msg_addr_lo;
+	__le32 msg_addr_hi;
 };
 
 /*
@@ -5227,9 +5224,9 @@ struct vf_pf_event_data {
 struct vf_flr_event_data {
 	u8 vf_id;
 	u8 reserved0;
-	u16 reserved1;
-	u32 reserved2;
-	u32 reserved3;
+	__le16 reserved1;
+	__le32 reserved2;
+	__le32 reserved3;
 };
 
 /*
@@ -5238,9 +5235,9 @@ struct vf_flr_event_data {
 struct malicious_vf_event_data {
 	u8 vf_id;
 	u8 err_id;
-	u16 reserved1;
-	u32 reserved2;
-	u32 reserved3;
+	__le16 reserved1;
+	__le32 reserved2;
+	__le32 reserved3;
 };
 
 /*
@@ -5623,6 +5620,14 @@ enum igu_mode {
 	MAX_IGU_MODE
 };
 
+/*
+ * Inner Headers Classification Type
+ */
+enum inner_clss_type {
+	INNER_CLSS_DISABLED,
+	INNER_CLSS_USE_VLAN,
+	INNER_CLSS_USE_VNI,
+	MAX_INNER_CLSS_TYPE};
 
 /*
  * IP versions
@@ -5953,14 +5958,6 @@ enum ts_offset_cmd {
 	MAX_TS_OFFSET_CMD
 };
 
-/* Tunnel Mode */
-enum tunnel_mode {
-	TUNN_MODE_NONE,
-	TUNN_MODE_VXLAN,
-	TUNN_MODE_GRE,
-	MAX_TUNNEL_MODE
-};
-
  /* zone A per-queue data */
 struct ustorm_queue_zone_data {
 	struct ustorm_eth_rx_producers eth_rx_producers;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index d946bba43726..1fb80100e5e7 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -6185,26 +6185,80 @@ static int bnx2x_format_ver(u32 num, u8 *str, u16 *len)
 		shift -= 4;
 		digit = ((num & mask) >> shift);
 		if (digit == 0 && remove_leading_zeros) {
-			mask = mask >> 4;
-			continue;
-		} else if (digit < 0xa)
-			*str_ptr = digit + '0';
-		else
-			*str_ptr = digit - 0xa + 'a';
-		remove_leading_zeros = 0;
-		str_ptr++;
-		(*len)--;
+			*str_ptr = '0';
+		} else {
+			if (digit < 0xa)
+				*str_ptr = digit + '0';
+			else
+				*str_ptr = digit - 0xa + 'a';
+
+			remove_leading_zeros = 0;
+			str_ptr++;
+			(*len)--;
+		}
 		mask = mask >> 4;
 		if (shift == 4*4) {
+			if (remove_leading_zeros) {
+				str_ptr++;
+				(*len)--;
+			}
 			*str_ptr = '.';
 			str_ptr++;
 			(*len)--;
 			remove_leading_zeros = 1;
 		}
 	}
+	if (remove_leading_zeros)
+		(*len)--;
 	return 0;
 }
 
+static int bnx2x_3_seq_format_ver(u32 num, u8 *str, u16 *len)
+{
+	u8 *str_ptr = str;
+	u32 mask = 0x00f00000;
+	u8 shift = 8*3;
+	u8 digit;
+	u8 remove_leading_zeros = 1;
+
+	if (*len < 10) {
+		/* Need more than 10chars for this format */
+		*str_ptr = '\0';
+		(*len)--;
+		return -EINVAL;
+	}
+
+	while (shift > 0) {
+		shift -= 4;
+		digit = ((num & mask) >> shift);
+		if (digit == 0 && remove_leading_zeros) {
+			*str_ptr = '0';
+		} else {
+			if (digit < 0xa)
+				*str_ptr = digit + '0';
+			else
+				*str_ptr = digit - 0xa + 'a';
+
+			remove_leading_zeros = 0;
+			str_ptr++;
+			(*len)--;
+		}
+		mask = mask >> 4;
+		if ((shift == 4*4) || (shift == 4*2)) {
+			if (remove_leading_zeros) {
+				str_ptr++;
+				(*len)--;
+			}
+			*str_ptr = '.';
+			str_ptr++;
+			(*len)--;
+			remove_leading_zeros = 1;
+		}
+	}
+	if (remove_leading_zeros)
+		(*len)--;
+	return 0;
+}
 
 static int bnx2x_null_format_ver(u32 spirom_ver, u8 *str, u16 *len)
 {
@@ -9677,8 +9731,9 @@ static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 
 	if (bnx2x_is_8483x_8485x(phy)) {
 		bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD, 0x400f, &fw_ver1);
-		bnx2x_save_spirom_version(bp, port, fw_ver1 & 0xfff,
-				phy->ver_addr);
+		if (phy->type != PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858)
+			fw_ver1 &= 0xfff;
+		bnx2x_save_spirom_version(bp, port, fw_ver1, phy->ver_addr);
 	} else {
 		/* For 32-bit registers in 848xx, access via MDIO2ARM i/f. */
 		/* (1) set reg 0xc200_0014(SPI_BRIDGE_CTRL_2) to 0x03000000 */
@@ -9732,16 +9787,32 @@ static void bnx2x_save_848xx_spirom_version(struct bnx2x_phy *phy,
 static void bnx2x_848xx_set_led(struct bnx2x *bp,
 				struct bnx2x_phy *phy)
 {
-	u16 val, offset, i;
+	u16 val, led3_blink_rate, offset, i;
 	static struct bnx2x_reg_set reg_set[] = {
 		{MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_LED1_MASK, 0x0080},
 		{MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_LED2_MASK, 0x0018},
 		{MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_LED3_MASK, 0x0006},
-		{MDIO_PMA_DEVAD, MDIO_PMA_REG_8481_LED3_BLINK, 0x0000},
 		{MDIO_PMA_DEVAD, MDIO_PMA_REG_84823_CTL_SLOW_CLK_CNT_HIGH,
 			MDIO_PMA_REG_84823_BLINK_RATE_VAL_15P9HZ},
 		{MDIO_AN_DEVAD, 0xFFFB, 0xFFFD}
 	};
+
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858) {
+		/* Set LED5 source */
+		bnx2x_cl45_write(bp, phy,
+				 MDIO_PMA_DEVAD,
+				 MDIO_PMA_REG_8481_LED5_MASK,
+				 0x90);
+		led3_blink_rate = 0x000f;
+	} else {
+		led3_blink_rate = 0x0000;
+	}
+	/* Set LED3 BLINK */
+	bnx2x_cl45_write(bp, phy,
+			 MDIO_PMA_DEVAD,
+			 MDIO_PMA_REG_8481_LED3_BLINK,
+			 led3_blink_rate);
+
 	/* PHYC_CTL_LED_CTL */
 	bnx2x_cl45_read(bp, phy,
 			MDIO_PMA_DEVAD,
@@ -9749,6 +9820,9 @@ static void bnx2x_848xx_set_led(struct bnx2x *bp,
 	val &= 0xFE00;
 	val |= 0x0092;
 
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858)
+		val |= 2 << 12; /* LED5 ON based on source */
+
 	bnx2x_cl45_write(bp, phy,
 			 MDIO_PMA_DEVAD,
 			 MDIO_PMA_REG_8481_LINK_SIGNAL, val);
@@ -9762,10 +9836,17 @@ static void bnx2x_848xx_set_led(struct bnx2x *bp,
 	else
 		offset = MDIO_PMA_REG_84823_CTL_LED_CTL_1;
 
-	/* stretch_en for LED3*/
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858)
+		val = MDIO_PMA_REG_84858_ALLOW_GPHY_ACT |
+		      MDIO_PMA_REG_84823_LED3_STRETCH_EN;
+	else
+		val = MDIO_PMA_REG_84823_LED3_STRETCH_EN;
+
+	/* stretch_en for LEDs */
 	bnx2x_cl45_read_or_write(bp, phy,
-				 MDIO_PMA_DEVAD, offset,
-				 MDIO_PMA_REG_84823_LED3_STRETCH_EN);
+				 MDIO_PMA_DEVAD,
+				 offset,
+				 val);
 }
 
 static void bnx2x_848xx_specific_func(struct bnx2x_phy *phy,
@@ -9775,7 +9856,7 @@ static void bnx2x_848xx_specific_func(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	switch (action) {
 	case PHY_INIT:
-		if (!bnx2x_is_8483x_8485x(phy)) {
+		if (bnx2x_is_8483x_8485x(phy)) {
 			/* Save spirom version */
 			bnx2x_save_848xx_spirom_version(phy, bp, params->port);
 		}
@@ -10036,15 +10117,20 @@ static int bnx2x_84858_cmd_hdlr(struct bnx2x_phy *phy,
 
 static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 				struct link_params *params, u16 fw_cmd,
-				u16 cmd_args[], int argc)
+				u16 cmd_args[], int argc, int process)
 {
 	int idx;
 	u16 val;
 	struct bnx2x *bp = params->bp;
-	/* Write CMD_OPEN_OVERRIDE to STATUS reg */
-	bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
-			MDIO_848xx_CMD_HDLR_STATUS,
-			PHY84833_STATUS_CMD_OPEN_OVERRIDE);
+	int rc = 0;
+
+	if (process == PHY84833_MB_PROCESS2) {
+		/* Write CMD_OPEN_OVERRIDE to STATUS reg */
+		bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
+				 MDIO_848xx_CMD_HDLR_STATUS,
+				 PHY84833_STATUS_CMD_OPEN_OVERRIDE);
+	}
+
 	for (idx = 0; idx < PHY848xx_CMDHDLR_WAIT; idx++) {
 		bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
 				MDIO_848xx_CMD_HDLR_STATUS, &val);
@@ -10054,15 +10140,27 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 	}
 	if (idx >= PHY848xx_CMDHDLR_WAIT) {
 		DP(NETIF_MSG_LINK, "FW cmd: FW not ready.\n");
+		/* if the status is CMD_COMPLETE_PASS or CMD_COMPLETE_ERROR
+		 * clear the status to CMD_CLEAR_COMPLETE
+		 */
+		if (val == PHY84833_STATUS_CMD_COMPLETE_PASS ||
+		    val == PHY84833_STATUS_CMD_COMPLETE_ERROR) {
+			bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
+					 MDIO_848xx_CMD_HDLR_STATUS,
+					 PHY84833_STATUS_CMD_CLEAR_COMPLETE);
+		}
 		return -EINVAL;
 	}
-
-	/* Prepare argument(s) and issue command */
-	for (idx = 0; idx < argc; idx++) {
-		bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
-				MDIO_848xx_CMD_HDLR_DATA1 + idx,
-				cmd_args[idx]);
+	if (process == PHY84833_MB_PROCESS1 ||
+	    process == PHY84833_MB_PROCESS2) {
+		/* Prepare argument(s) */
+		for (idx = 0; idx < argc; idx++) {
+			bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
+					 MDIO_848xx_CMD_HDLR_DATA1 + idx,
+					 cmd_args[idx]);
+		}
 	}
+
 	bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
 			MDIO_848xx_CMD_HDLR_COMMAND, fw_cmd);
 	for (idx = 0; idx < PHY848xx_CMDHDLR_WAIT; idx++) {
@@ -10076,24 +10174,30 @@ static int bnx2x_84833_cmd_hdlr(struct bnx2x_phy *phy,
 	if ((idx >= PHY848xx_CMDHDLR_WAIT) ||
 	    (val == PHY84833_STATUS_CMD_COMPLETE_ERROR)) {
 		DP(NETIF_MSG_LINK, "FW cmd failed.\n");
-		return -EINVAL;
+		rc = -EINVAL;
 	}
-	/* Gather returning data */
-	for (idx = 0; idx < argc; idx++) {
-		bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
-				MDIO_848xx_CMD_HDLR_DATA1 + idx,
-				&cmd_args[idx]);
+	if (process == PHY84833_MB_PROCESS3 && rc == 0) {
+		/* Gather returning data */
+		for (idx = 0; idx < argc; idx++) {
+			bnx2x_cl45_read(bp, phy, MDIO_CTL_DEVAD,
+					MDIO_848xx_CMD_HDLR_DATA1 + idx,
+					&cmd_args[idx]);
+		}
 	}
-	bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
-			MDIO_848xx_CMD_HDLR_STATUS,
-			PHY84833_STATUS_CMD_CLEAR_COMPLETE);
-	return 0;
+	if (val == PHY84833_STATUS_CMD_COMPLETE_ERROR ||
+	    val == PHY84833_STATUS_CMD_COMPLETE_PASS) {
+		bnx2x_cl45_write(bp, phy, MDIO_CTL_DEVAD,
+				 MDIO_848xx_CMD_HDLR_STATUS,
+				 PHY84833_STATUS_CMD_CLEAR_COMPLETE);
+	}
+	return rc;
 }
 
 static int bnx2x_848xx_cmd_hdlr(struct bnx2x_phy *phy,
 				struct link_params *params,
 				u16 fw_cmd,
-				u16 cmd_args[], int argc)
+					   u16 cmd_args[], int argc,
+					   int process)
 {
 	struct bnx2x *bp = params->bp;
 
@@ -10106,7 +10210,7 @@ static int bnx2x_848xx_cmd_hdlr(struct bnx2x_phy *phy,
 					    argc);
 	} else {
 		return bnx2x_84833_cmd_hdlr(phy, params, fw_cmd, cmd_args,
-					    argc);
+					    argc, process);
 	}
 }
 
@@ -10133,7 +10237,7 @@ static int bnx2x_848xx_pair_swap_cfg(struct bnx2x_phy *phy,
 
 	status = bnx2x_848xx_cmd_hdlr(phy, params,
 				      PHY848xx_CMD_SET_PAIR_SWAP, data,
-				      PHY848xx_CMDHDLR_MAX_ARGS);
+				      2, PHY84833_MB_PROCESS2);
 	if (status == 0)
 		DP(NETIF_MSG_LINK, "Pairswap OK, val=0x%x\n", data[1]);
 
@@ -10222,8 +10326,8 @@ static int bnx2x_8483x_disable_eee(struct bnx2x_phy *phy,
 	DP(NETIF_MSG_LINK, "Don't Advertise 10GBase-T EEE\n");
 
 	/* Prevent Phy from working in EEE and advertising it */
-	rc = bnx2x_848xx_cmd_hdlr(phy, params,
-				  PHY848xx_CMD_SET_EEE_MODE, &cmd_args, 1);
+	rc = bnx2x_848xx_cmd_hdlr(phy, params, PHY848xx_CMD_SET_EEE_MODE,
+				  &cmd_args, 1, PHY84833_MB_PROCESS1);
 	if (rc) {
 		DP(NETIF_MSG_LINK, "EEE disable failed.\n");
 		return rc;
@@ -10240,8 +10344,8 @@ static int bnx2x_8483x_enable_eee(struct bnx2x_phy *phy,
 	struct bnx2x *bp = params->bp;
 	u16 cmd_args = 1;
 
-	rc = bnx2x_848xx_cmd_hdlr(phy, params,
-				  PHY848xx_CMD_SET_EEE_MODE, &cmd_args, 1);
+	rc = bnx2x_848xx_cmd_hdlr(phy, params, PHY848xx_CMD_SET_EEE_MODE,
+				  &cmd_args, 1, PHY84833_MB_PROCESS1);
 	if (rc) {
 		DP(NETIF_MSG_LINK, "EEE enable failed.\n");
 		return rc;
@@ -10362,7 +10466,7 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 		cmd_args[3] = PHY84833_CONSTANT_LATENCY;
 		rc = bnx2x_848xx_cmd_hdlr(phy, params,
 					  PHY848xx_CMD_SET_EEE_MODE, cmd_args,
-					  PHY848xx_CMDHDLR_MAX_ARGS);
+					  4, PHY84833_MB_PROCESS1);
 		if (rc)
 			DP(NETIF_MSG_LINK, "Cfg AutogrEEEn failed.\n");
 	}
@@ -10416,6 +10520,32 @@ static int bnx2x_848x3_config_init(struct bnx2x_phy *phy,
 		vars->eee_status &= ~SHMEM_EEE_SUPPORTED_MASK;
 	}
 
+	if (phy->type == PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84833) {
+		/* Additional settings for jumbo packets in 1000BASE-T mode */
+		/* Allow rx extended length */
+		bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD,
+				MDIO_AN_REG_8481_AUX_CTRL, &val);
+		val |= 0x4000;
+		bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
+				 MDIO_AN_REG_8481_AUX_CTRL, val);
+		/* TX FIFO Elasticity LSB */
+		bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD,
+				MDIO_AN_REG_8481_1G_100T_EXT_CTRL, &val);
+		val |= 0x1;
+		bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
+				 MDIO_AN_REG_8481_1G_100T_EXT_CTRL, val);
+		/* TX FIFO Elasticity MSB */
+		/* Enable expansion register 0x46 (Pattern Generator status) */
+		bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
+				 MDIO_AN_REG_8481_EXPANSION_REG_ACCESS, 0xf46);
+
+		bnx2x_cl45_read(bp, phy, MDIO_AN_DEVAD,
+				MDIO_AN_REG_8481_EXPANSION_REG_RD_RW, &val);
+		val |= 0x4000;
+		bnx2x_cl45_write(bp, phy, MDIO_AN_DEVAD,
+				 MDIO_AN_REG_8481_EXPANSION_REG_RD_RW, val);
+	}
+
 	if (bnx2x_is_8483x_8485x(phy)) {
 		/* Bring PHY out of super isolate mode as the final step. */
 		bnx2x_cl45_read_and_write(bp, phy,
@@ -10555,6 +10685,17 @@ static u8 bnx2x_848xx_read_status(struct bnx2x_phy *phy,
 	return link_up;
 }
 
+static int bnx2x_8485x_format_ver(u32 raw_ver, u8 *str, u16 *len)
+{
+	int status = 0;
+	u32 num;
+
+	num = ((raw_ver & 0xF80) >> 7) << 16 | ((raw_ver & 0x7F) << 8) |
+	      ((raw_ver & 0xF000) >> 12);
+	status = bnx2x_3_seq_format_ver(num, str, len);
+	return status;
+}
+
 static int bnx2x_848xx_format_ver(u32 raw_ver, u8 *str, u16 *len)
 {
 	int status = 0;
@@ -10651,10 +10792,25 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 					0x0);
 
 		} else {
+			/* LED 1 OFF */
 			bnx2x_cl45_write(bp, phy,
 					 MDIO_PMA_DEVAD,
 					 MDIO_PMA_REG_8481_LED1_MASK,
 					 0x0);
+
+			if (phy->type ==
+				PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858) {
+				/* LED 2 OFF */
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED2_MASK,
+						 0x0);
+				/* LED 3 OFF */
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED3_MASK,
+						 0x0);
+			}
 		}
 		break;
 	case LED_MODE_FRONT_PANEL_OFF:
@@ -10713,6 +10869,19 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 						 MDIO_PMA_REG_8481_SIGNAL_MASK,
 						 0x0);
 			}
+			if (phy->type ==
+				PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858) {
+				/* LED 2 OFF */
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED2_MASK,
+						 0x0);
+				/* LED 3 OFF */
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED3_MASK,
+						 0x0);
+			}
 		}
 		break;
 	case LED_MODE_ON:
@@ -10776,6 +10945,25 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 						params->port*4,
 						NIG_MASK_MI_INT);
 				}
+			}
+			if (phy->type ==
+			    PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858) {
+				/* Tell LED3 to constant on */
+				bnx2x_cl45_read(bp, phy,
+						MDIO_PMA_DEVAD,
+						MDIO_PMA_REG_8481_LINK_SIGNAL,
+						&val);
+				val &= ~(7<<6);
+				val |= (2<<6);  /* A83B[8:6]= 2 */
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LINK_SIGNAL,
+						 val);
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED3_MASK,
+						 0x20);
+			} else {
 				bnx2x_cl45_write(bp, phy,
 						 MDIO_PMA_DEVAD,
 						 MDIO_PMA_REG_8481_SIGNAL_MASK,
@@ -10854,6 +11042,17 @@ static void bnx2x_848xx_set_link_led(struct bnx2x_phy *phy,
 					 MDIO_PMA_REG_8481_LINK_SIGNAL,
 					 val);
 			if (phy->type ==
+			    PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84858) {
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED2_MASK,
+						 0x18);
+				bnx2x_cl45_write(bp, phy,
+						 MDIO_PMA_DEVAD,
+						 MDIO_PMA_REG_8481_LED3_MASK,
+						 0x06);
+			}
+			if (phy->type ==
 			    PORT_HW_CFG_XGXS_EXT_PHY_TYPE_BCM84834) {
 				/* Restore LED4 source to external link,
 				 * and re-enable interrupts.
@@ -11982,7 +12181,7 @@ static const struct bnx2x_phy phy_84858 = {
 	.read_status	= (read_status_t)bnx2x_848xx_read_status,
 	.link_reset	= (link_reset_t)bnx2x_848x3_link_reset,
 	.config_loopback = (config_loopback_t)NULL,
-	.format_fw_ver	= (format_fw_ver_t)bnx2x_848xx_format_ver,
+	.format_fw_ver	= (format_fw_ver_t)bnx2x_8485x_format_ver,
 	.hw_reset	= (hw_reset_t)bnx2x_84833_hw_reset_phy,
 	.set_link_led	= (set_link_led_t)bnx2x_848xx_set_link_led,
 	.phy_specific_func = (phy_specific_func_t)bnx2x_848xx_specific_func
@@ -13807,8 +14006,10 @@ void bnx2x_period_func(struct link_params *params, struct link_vars *vars)
 	if (CHIP_IS_E3(bp)) {
 		struct bnx2x_phy *phy = &params->phy[INT_PHY];
 		bnx2x_set_aer_mmd(params, phy);
-		if ((phy->supported & SUPPORTED_20000baseKR2_Full) &&
-		    (phy->speed_cap_mask & PORT_HW_CFG_SPEED_CAPABILITY_D0_20G))
+		if (((phy->req_line_speed == SPEED_AUTO_NEG) &&
+		     (phy->speed_cap_mask &
+		      PORT_HW_CFG_SPEED_CAPABILITY_D0_20G)) ||
+		    (phy->req_line_speed == SPEED_20000))
 			bnx2x_check_kr2_wa(params, vars, phy);
 		bnx2x_check_over_curr(params, vars);
 		if (vars->rx_tx_asic_rst)
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index 2e611dc5f162..2bf9c871144f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -5280,14 +5280,14 @@ static void bnx2x_handle_classification_eqe(struct bnx2x *bp,
 {
 	unsigned long ramrod_flags = 0;
 	int rc = 0;
-	u32 cid = elem->message.data.eth_event.echo & BNX2X_SWCID_MASK;
+	u32 echo = le32_to_cpu(elem->message.data.eth_event.echo);
+	u32 cid = echo & BNX2X_SWCID_MASK;
 	struct bnx2x_vlan_mac_obj *vlan_mac_obj;
 
 	/* Always push next commands out, don't wait here */
 	__set_bit(RAMROD_CONT, &ramrod_flags);
 
-	switch (le32_to_cpu((__force __le32)elem->message.data.eth_event.echo)
-			    >> BNX2X_SWCID_SHIFT) {
+	switch (echo >> BNX2X_SWCID_SHIFT) {
 	case BNX2X_FILTER_MAC_PENDING:
 		DP(BNX2X_MSG_SP, "Got SETUP_MAC completions\n");
 		if (CNIC_LOADED(bp) && (cid == BNX2X_ISCSI_ETH_CID(bp)))
@@ -5308,8 +5308,7 @@ static void bnx2x_handle_classification_eqe(struct bnx2x *bp,
 		bnx2x_handle_mcast_eqe(bp);
 		return;
 	default:
-		BNX2X_ERR("Unsupported classification command: %d\n",
-			  elem->message.data.eth_event.echo);
+		BNX2X_ERR("Unsupported classification command: 0x%x\n", echo);
 		return;
 	}
 
@@ -5478,9 +5477,6 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 			goto next_spqe;
 		}
 
-		/* elem CID originates from FW; actually LE */
-		cid = SW_CID((__force __le32)
-			     elem->message.data.cfc_del_event.cid);
 		opcode = elem->message.opcode;
 
 		/* handle eq element */
@@ -5503,6 +5499,10 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 			 * we may want to verify here that the bp state is
 			 * HALTING
 			 */
+
+			/* elem CID originates from FW; actually LE */
+			cid = SW_CID(elem->message.data.cfc_del_event.cid);
+
 			DP(BNX2X_MSG_SP,
 			   "got delete ramrod for MULTI[%d]\n", cid);
 
@@ -5596,10 +5596,8 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 		      BNX2X_STATE_OPENING_WAIT4_PORT):
 		case (EVENT_RING_OPCODE_RSS_UPDATE_RULES |
 		      BNX2X_STATE_CLOSING_WAIT4_HALT):
-			cid = elem->message.data.eth_event.echo &
-				BNX2X_SWCID_MASK;
 			DP(BNX2X_MSG_SP, "got RSS_UPDATE ramrod. CID %d\n",
-			   cid);
+			   SW_CID(elem->message.data.eth_event.echo));
 			rss_raw->clear_pending(rss_raw);
 			break;
 
@@ -5684,7 +5682,7 @@ static void bnx2x_sp_task(struct work_struct *work)
 		if (status & BNX2X_DEF_SB_IDX) {
 			struct bnx2x_fastpath *fp = bnx2x_fcoe_fp(bp);
 
-		if (FCOE_INIT(bp) &&
+			if (FCOE_INIT(bp) &&
 			    (bnx2x_has_rx_work(fp) || bnx2x_has_tx_work(fp))) {
 				/* Prevent local bottom-halves from running as
 				 * we are going to change the local NAPI list.
@@ -13004,9 +13002,6 @@ static const struct net_device_ops bnx2x_netdev_ops = {
 	.ndo_fcoe_get_wwn	= bnx2x_fcoe_get_wwn,
 #endif
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	.ndo_busy_poll		= bnx2x_low_latency_recv,
-#endif
 	.ndo_get_phys_port_id	= bnx2x_get_phys_port_id,
 	.ndo_set_vf_link_state	= bnx2x_set_vf_link_state,
 	.ndo_features_check	= bnx2x_features_check,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index 4dead49bd5cb..a43dea259b12 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -7296,6 +7296,8 @@ Theotherbitsarereservedandshouldbezero*/
 #define MDIO_PMA_REG_84823_CTL_LED_CTL_1			0xa8e3
 #define MDIO_PMA_REG_84833_CTL_LED_CTL_1			0xa8ec
 #define MDIO_PMA_REG_84823_LED3_STRETCH_EN			0x0080
+/* BCM84858 only */
+#define MDIO_PMA_REG_84858_ALLOW_GPHY_ACT			0x8000
 
 /* BCM84833 only */
 #define MDIO_84833_TOP_CFG_FW_REV			0x400f
@@ -7337,6 +7339,10 @@ Theotherbitsarereservedandshouldbezero*/
 #define PHY84833_STATUS_CMD_NOT_OPEN_FOR_CMDS		0x0040
 #define PHY84833_STATUS_CMD_CLEAR_COMPLETE		0x0080
 #define PHY84833_STATUS_CMD_OPEN_OVERRIDE		0xa5a5
+/* Mailbox Process */
+#define PHY84833_MB_PROCESS1				1
+#define PHY84833_MB_PROCESS2				2
+#define PHY84833_MB_PROCESS3				3
 
 /* Mailbox status set used by 84858 only */
 #define PHY84858_STATUS_CMD_RECEIVED			0x0001
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index 9d027348cd09..632daff117d3 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -1672,11 +1672,12 @@ void bnx2x_vf_handle_classification_eqe(struct bnx2x *bp,
 {
 	unsigned long ramrod_flags = 0;
 	int rc = 0;
+	u32 echo = le32_to_cpu(elem->message.data.eth_event.echo);
 
 	/* Always push next commands out, don't wait here */
 	set_bit(RAMROD_CONT, &ramrod_flags);
 
-	switch (elem->message.data.eth_event.echo >> BNX2X_SWCID_SHIFT) {
+	switch (echo >> BNX2X_SWCID_SHIFT) {
 	case BNX2X_FILTER_MAC_PENDING:
 		rc = vfq->mac_obj.complete(bp, &vfq->mac_obj, elem,
 					   &ramrod_flags);
@@ -1686,8 +1687,7 @@ void bnx2x_vf_handle_classification_eqe(struct bnx2x *bp,
 					    &ramrod_flags);
 		break;
 	default:
-		BNX2X_ERR("Unsupported classification command: %d\n",
-			  elem->message.data.eth_event.echo);
+		BNX2X_ERR("Unsupported classification command: 0x%x\n", echo);
 		return;
 	}
 	if (rc < 0)
@@ -1747,16 +1747,14 @@ int bnx2x_iov_eq_sp_event(struct bnx2x *bp, union event_ring_elem *elem)
 
 	switch (opcode) {
 	case EVENT_RING_OPCODE_CFC_DEL:
-		cid = SW_CID((__force __le32)
-			     elem->message.data.cfc_del_event.cid);
+		cid = SW_CID(elem->message.data.cfc_del_event.cid);
 		DP(BNX2X_MSG_IOV, "checking cfc-del comp cid=%d\n", cid);
 		break;
 	case EVENT_RING_OPCODE_CLASSIFICATION_RULES:
 	case EVENT_RING_OPCODE_MULTICAST_RULES:
 	case EVENT_RING_OPCODE_FILTERS_RULES:
 	case EVENT_RING_OPCODE_RSS_UPDATE_RULES:
-		cid = (elem->message.data.eth_event.echo &
-		       BNX2X_SWCID_MASK);
+		cid = SW_CID(elem->message.data.eth_event.echo);
 		DP(BNX2X_MSG_IOV, "checking filtering comp cid=%d\n", cid);
 		break;
 	case EVENT_RING_OPCODE_VF_FLR:
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index 1374e5394a79..bfae300cf25f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -2187,8 +2187,10 @@ void bnx2x_vf_mbx_schedule(struct bnx2x *bp,
 
 	/* Update VFDB with current message and schedule its handling */
 	mutex_lock(&BP_VFDB(bp)->event_mutex);
-	BP_VF_MBX(bp, vf_idx)->vf_addr_hi = vfpf_event->msg_addr_hi;
-	BP_VF_MBX(bp, vf_idx)->vf_addr_lo = vfpf_event->msg_addr_lo;
+	BP_VF_MBX(bp, vf_idx)->vf_addr_hi =
+		le32_to_cpu(vfpf_event->msg_addr_hi);
+	BP_VF_MBX(bp, vf_idx)->vf_addr_lo =
+		le32_to_cpu(vfpf_event->msg_addr_lo);
 	BP_VFDB(bp)->event_occur |= (1ULL << vf_idx);
 	mutex_unlock(&BP_VFDB(bp)->event_mutex);
 
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.c b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
index 07f5f239cb65..82f191382989 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.c
@@ -69,11 +69,13 @@ MODULE_VERSION(DRV_MODULE_VERSION);
 #define BNXT_RX_DMA_OFFSET NET_SKB_PAD
 #define BNXT_RX_COPY_THRESH 256
 
-#define BNXT_TX_PUSH_THRESH 92
+#define BNXT_TX_PUSH_THRESH 164
 
 enum board_idx {
+	BCM57301,
 	BCM57302,
 	BCM57304,
+	BCM57402,
 	BCM57404,
 	BCM57406,
 	BCM57304_VF,
@@ -84,17 +86,21 @@ enum board_idx {
 static const struct {
 	char *name;
 } board_info[] = {
-	{ "Broadcom BCM57302 NetXtreme-C Single-port 10Gb/25Gb/40Gb/50Gb Ethernet" },
+	{ "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 BCM57402 NetXtreme-E Dual-port 10Gb Ethernet" },
 	{ "Broadcom BCM57404 NetXtreme-E Dual-port 10Gb/25Gb Ethernet" },
-	{ "Broadcom BCM57406 NetXtreme-E Dual-port 10Gb Ethernet" },
+	{ "Broadcom BCM57406 NetXtreme-E Dual-port 10GBase-T Ethernet" },
 	{ "Broadcom BCM57304 NetXtreme-C Ethernet Virtual Function" },
 	{ "Broadcom BCM57404 NetXtreme-E Ethernet Virtual Function" },
 };
 
 static const struct pci_device_id bnxt_pci_tbl[] = {
+	{ PCI_VDEVICE(BROADCOM, 0x16c8), .driver_data = BCM57301 },
 	{ PCI_VDEVICE(BROADCOM, 0x16c9), .driver_data = BCM57302 },
 	{ PCI_VDEVICE(BROADCOM, 0x16ca), .driver_data = BCM57304 },
+	{ PCI_VDEVICE(BROADCOM, 0x16d0), .driver_data = BCM57402 },
 	{ PCI_VDEVICE(BROADCOM, 0x16d1), .driver_data = BCM57404 },
 	{ PCI_VDEVICE(BROADCOM, 0x16d2), .driver_data = BCM57406 },
 #ifdef CONFIG_BNXT_SRIOV
@@ -173,7 +179,6 @@ static netdev_tx_t bnxt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	u32 len, free_size, vlan_tag_flags, cfa_action, flags;
 	u16 prod, last_frag;
 	struct pci_dev *pdev = bp->pdev;
-	struct bnxt_napi *bnapi;
 	struct bnxt_tx_ring_info *txr;
 	struct bnxt_sw_tx_bd *tx_buf;
 
@@ -183,8 +188,7 @@ static netdev_tx_t bnxt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		return NETDEV_TX_OK;
 	}
 
-	bnapi = bp->bnapi[i];
-	txr = &bnapi->tx_ring;
+	txr = &bp->tx_ring[i];
 	txq = netdev_get_tx_queue(dev, i);
 	prod = txr->tx_prod;
 
@@ -219,11 +223,12 @@ static netdev_tx_t bnxt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	if (free_size == bp->tx_ring_size && length <= bp->tx_push_thresh) {
-		struct tx_push_bd *push = txr->tx_push;
-		struct tx_bd *tx_push = &push->txbd1;
-		struct tx_bd_ext *tx_push1 = &push->txbd2;
-		void *pdata = tx_push1 + 1;
-		int j;
+		struct tx_push_buffer *tx_push_buf = txr->tx_push;
+		struct tx_push_bd *tx_push = &tx_push_buf->push_bd;
+		struct tx_bd_ext *tx_push1 = &tx_push->txbd2;
+		void *pdata = tx_push_buf->data;
+		u64 *end;
+		int j, push_len;
 
 		/* Set COAL_NOW to be ready quickly for the next push */
 		tx_push->tx_bd_len_flags_type =
@@ -243,6 +248,10 @@ static netdev_tx_t bnxt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		tx_push1->tx_bd_cfa_meta = cpu_to_le32(vlan_tag_flags);
 		tx_push1->tx_bd_cfa_action = cpu_to_le32(cfa_action);
 
+		end = pdata + length;
+		end = PTR_ALIGN(end, 8) - 1;
+		*end = 0;
+
 		skb_copy_from_linear_data(skb, pdata, len);
 		pdata += len;
 		for (j = 0; j < last_frag; j++) {
@@ -257,22 +266,29 @@ static netdev_tx_t bnxt_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			pdata += skb_frag_size(frag);
 		}
 
-		memcpy(txbd, tx_push, sizeof(*txbd));
+		txbd->tx_bd_len_flags_type = tx_push->tx_bd_len_flags_type;
+		txbd->tx_bd_haddr = txr->data_mapping;
 		prod = NEXT_TX(prod);
 		txbd = &txr->tx_desc_ring[TX_RING(prod)][TX_IDX(prod)];
 		memcpy(txbd, tx_push1, sizeof(*txbd));
 		prod = NEXT_TX(prod);
-		push->doorbell =
+		tx_push->doorbell =
 			cpu_to_le32(DB_KEY_TX_PUSH | DB_LONG_TX_PUSH | prod);
 		txr->tx_prod = prod;
 
 		netdev_tx_sent_queue(txq, skb->len);
 
-		__iowrite64_copy(txr->tx_doorbell, push,
-				 (length + sizeof(*push) + 8) / 8);
+		push_len = (length + sizeof(*tx_push) + 7) / 8;
+		if (push_len > 16) {
+			__iowrite64_copy(txr->tx_doorbell, tx_push_buf, 16);
+			__iowrite64_copy(txr->tx_doorbell + 4, tx_push_buf + 1,
+					 push_len - 16);
+		} else {
+			__iowrite64_copy(txr->tx_doorbell, tx_push_buf,
+					 push_len);
+		}
 
 		tx_buf->is_push = 1;
-
 		goto tx_done;
 	}
 
@@ -417,8 +433,8 @@ tx_dma_error:
 
 static void bnxt_tx_int(struct bnxt *bp, struct bnxt_napi *bnapi, int nr_pkts)
 {
-	struct bnxt_tx_ring_info *txr = &bnapi->tx_ring;
-	int index = bnapi->index;
+	struct bnxt_tx_ring_info *txr = bnapi->tx_ring;
+	int index = txr - &bp->tx_ring[0];
 	struct netdev_queue *txq = netdev_get_tx_queue(bp->dev, index);
 	u16 cons = txr->tx_cons;
 	struct pci_dev *pdev = bp->pdev;
@@ -596,7 +612,7 @@ static void bnxt_reuse_rx_agg_bufs(struct bnxt_napi *bnapi, u16 cp_cons,
 {
 	struct bnxt *bp = bnapi->bp;
 	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-	struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+	struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 	u16 prod = rxr->rx_agg_prod;
 	u16 sw_prod = rxr->rx_sw_agg_prod;
 	u32 i;
@@ -675,7 +691,7 @@ static struct sk_buff *bnxt_rx_pages(struct bnxt *bp, struct bnxt_napi *bnapi,
 {
 	struct pci_dev *pdev = bp->pdev;
 	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-	struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+	struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 	u16 prod = rxr->rx_agg_prod;
 	u32 i;
 
@@ -856,8 +872,13 @@ static inline struct sk_buff *bnxt_gro_skb(struct bnxt_tpa_info *tpa_info,
 	struct tcphdr *th;
 	int payload_off, tcp_opt_len = 0;
 	int len, nw_off;
+	u16 segs;
 
-	NAPI_GRO_CB(skb)->count = TPA_END_TPA_SEGS(tpa_end);
+	segs = TPA_END_TPA_SEGS(tpa_end);
+	if (segs == 1)
+		return skb;
+
+	NAPI_GRO_CB(skb)->count = segs;
 	skb_shinfo(skb)->gso_size =
 		le32_to_cpu(tpa_end1->rx_tpa_end_cmp_seg_len);
 	skb_shinfo(skb)->gso_type = tpa_info->gso_type;
@@ -929,7 +950,7 @@ static inline struct sk_buff *bnxt_tpa_end(struct bnxt *bp,
 					   bool *agg_event)
 {
 	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-	struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+	struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 	u8 agg_id = TPA_END_AGG_ID(tpa_end);
 	u8 *data, agg_bufs;
 	u16 cp_cons = RING_CMP(*raw_cons);
@@ -1045,7 +1066,7 @@ static int bnxt_rx_pkt(struct bnxt *bp, struct bnxt_napi *bnapi, u32 *raw_cons,
 		       bool *agg_event)
 {
 	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-	struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+	struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 	struct net_device *dev = bp->dev;
 	struct rx_cmp *rxcmp;
 	struct rx_cmp_ext *rxcmp1;
@@ -1187,8 +1208,10 @@ static int bnxt_rx_pkt(struct bnxt *bp, struct bnxt_napi *bnapi, u32 *raw_cons,
 			skb->csum_level = RX_CMP_ENCAP(rxcmp1);
 		}
 	} else {
-		if (rxcmp1->rx_cmp_cfa_code_errors_v2 & RX_CMP_L4_CS_ERR_BITS)
-			cpr->rx_l4_csum_errors++;
+		if (rxcmp1->rx_cmp_cfa_code_errors_v2 & RX_CMP_L4_CS_ERR_BITS) {
+			if (dev->features & NETIF_F_RXCSUM)
+				cpr->rx_l4_csum_errors++;
+		}
 	}
 
 	skb_record_rx_queue(skb, bnapi->index);
@@ -1377,7 +1400,7 @@ static int bnxt_poll_work(struct bnxt *bp, struct bnxt_napi *bnapi, int budget)
 		bnxt_tx_int(bp, bnapi, tx_pkts);
 
 	if (rx_event) {
-		struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+		struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
 
 		writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
 		writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
@@ -1446,19 +1469,14 @@ static void bnxt_free_tx_skbs(struct bnxt *bp)
 	int i, max_idx;
 	struct pci_dev *pdev = bp->pdev;
 
-	if (!bp->bnapi)
+	if (!bp->tx_ring)
 		return;
 
 	max_idx = bp->tx_nr_pages * TX_DESC_CNT;
 	for (i = 0; i < bp->tx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_tx_ring_info *txr;
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
 		int j;
 
-		if (!bnapi)
-			continue;
-
-		txr = &bnapi->tx_ring;
 		for (j = 0; j < max_idx;) {
 			struct bnxt_sw_tx_bd *tx_buf = &txr->tx_buf_ring[j];
 			struct sk_buff *skb = tx_buf->skb;
@@ -1484,10 +1502,11 @@ static void bnxt_free_tx_skbs(struct bnxt *bp)
 
 			last = tx_buf->nr_frags;
 			j += 2;
-			for (k = 0; k < last; k++, j = NEXT_TX(j)) {
+			for (k = 0; k < last; k++, j++) {
+				int ring_idx = j & bp->tx_ring_mask;
 				skb_frag_t *frag = &skb_shinfo(skb)->frags[k];
 
-				tx_buf = &txr->tx_buf_ring[j];
+				tx_buf = &txr->tx_buf_ring[ring_idx];
 				dma_unmap_page(
 					&pdev->dev,
 					dma_unmap_addr(tx_buf, mapping),
@@ -1504,21 +1523,15 @@ static void bnxt_free_rx_skbs(struct bnxt *bp)
 	int i, max_idx, max_agg_idx;
 	struct pci_dev *pdev = bp->pdev;
 
-	if (!bp->bnapi)
+	if (!bp->rx_ring)
 		return;
 
 	max_idx = bp->rx_nr_pages * RX_DESC_CNT;
 	max_agg_idx = bp->rx_agg_nr_pages * RX_DESC_CNT;
 	for (i = 0; i < bp->rx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_rx_ring_info *rxr;
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
 		int j;
 
-		if (!bnapi)
-			continue;
-
-		rxr = &bnapi->rx_ring;
-
 		if (rxr->rx_tpa) {
 			for (j = 0; j < MAX_TPA; j++) {
 				struct bnxt_tpa_info *tpa_info =
@@ -1646,19 +1659,13 @@ static void bnxt_free_rx_rings(struct bnxt *bp)
 {
 	int i;
 
-	if (!bp->bnapi)
+	if (!bp->rx_ring)
 		return;
 
 	for (i = 0; i < bp->rx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_rx_ring_info *rxr;
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
 		struct bnxt_ring_struct *ring;
 
-		if (!bnapi)
-			continue;
-
-		rxr = &bnapi->rx_ring;
-
 		kfree(rxr->rx_tpa);
 		rxr->rx_tpa = NULL;
 
@@ -1677,6 +1684,9 @@ static int bnxt_alloc_rx_rings(struct bnxt *bp)
 {
 	int i, rc, agg_rings = 0, tpa_rings = 0;
 
+	if (!bp->rx_ring)
+		return -ENOMEM;
+
 	if (bp->flags & BNXT_FLAG_AGG_RINGS)
 		agg_rings = 1;
 
@@ -1684,14 +1694,9 @@ static int bnxt_alloc_rx_rings(struct bnxt *bp)
 		tpa_rings = 1;
 
 	for (i = 0; i < bp->rx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_rx_ring_info *rxr;
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
 		struct bnxt_ring_struct *ring;
 
-		if (!bnapi)
-			continue;
-
-		rxr = &bnapi->rx_ring;
 		ring = &rxr->rx_ring_struct;
 
 		rc = bnxt_alloc_ring(bp, ring);
@@ -1729,19 +1734,13 @@ static void bnxt_free_tx_rings(struct bnxt *bp)
 	int i;
 	struct pci_dev *pdev = bp->pdev;
 
-	if (!bp->bnapi)
+	if (!bp->tx_ring)
 		return;
 
 	for (i = 0; i < bp->tx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_tx_ring_info *txr;
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
 		struct bnxt_ring_struct *ring;
 
-		if (!bnapi)
-			continue;
-
-		txr = &bnapi->tx_ring;
-
 		if (txr->tx_push) {
 			dma_free_coherent(&pdev->dev, bp->tx_push_size,
 					  txr->tx_push, txr->tx_push_mapping);
@@ -1766,7 +1765,7 @@ static int bnxt_alloc_tx_rings(struct bnxt *bp)
 		push_size  = L1_CACHE_ALIGN(sizeof(struct tx_push_bd) +
 					bp->tx_push_thresh);
 
-		if (push_size > 128) {
+		if (push_size > 256) {
 			push_size = 0;
 			bp->tx_push_thresh = 0;
 		}
@@ -1775,14 +1774,9 @@ static int bnxt_alloc_tx_rings(struct bnxt *bp)
 	}
 
 	for (i = 0, j = 0; i < bp->tx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_tx_ring_info *txr;
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
 		struct bnxt_ring_struct *ring;
 
-		if (!bnapi)
-			continue;
-
-		txr = &bnapi->tx_ring;
 		ring = &txr->tx_ring_struct;
 
 		rc = bnxt_alloc_ring(bp, ring);
@@ -1790,7 +1784,6 @@ static int bnxt_alloc_tx_rings(struct bnxt *bp)
 			return rc;
 
 		if (bp->tx_push_size) {
-			struct tx_bd *txbd;
 			dma_addr_t mapping;
 
 			/* One pre-allocated DMA buffer to backup
@@ -1804,13 +1797,11 @@ static int bnxt_alloc_tx_rings(struct bnxt *bp)
 			if (!txr->tx_push)
 				return -ENOMEM;
 
-			txbd = &txr->tx_push->txbd1;
-
 			mapping = txr->tx_push_mapping +
 				sizeof(struct tx_push_bd);
-			txbd->tx_bd_haddr = cpu_to_le64(mapping);
+			txr->data_mapping = cpu_to_le64(mapping);
 
-			memset(txbd + 1, 0, sizeof(struct tx_bd_ext));
+			memset(txr->tx_push, 0, sizeof(struct tx_push_bd));
 		}
 		ring->queue_id = bp->q_info[j].queue_id;
 		if (i % bp->tx_nr_rings_per_tc == (bp->tx_nr_rings_per_tc - 1))
@@ -1885,7 +1876,10 @@ static void bnxt_init_ring_struct(struct bnxt *bp)
 		ring->dma_arr = cpr->cp_desc_mapping;
 		ring->vmem_size = 0;
 
-		rxr = &bnapi->rx_ring;
+		rxr = bnapi->rx_ring;
+		if (!rxr)
+			goto skip_rx;
+
 		ring = &rxr->rx_ring_struct;
 		ring->nr_pages = bp->rx_nr_pages;
 		ring->page_size = HW_RXBD_RING_SIZE;
@@ -1902,7 +1896,11 @@ static void bnxt_init_ring_struct(struct bnxt *bp)
 		ring->vmem_size = SW_RXBD_AGG_RING_SIZE * bp->rx_agg_nr_pages;
 		ring->vmem = (void **)&rxr->rx_agg_ring;
 
-		txr = &bnapi->tx_ring;
+skip_rx:
+		txr = bnapi->tx_ring;
+		if (!txr)
+			continue;
+
 		ring = &txr->tx_ring_struct;
 		ring->nr_pages = bp->tx_nr_pages;
 		ring->page_size = HW_RXBD_RING_SIZE;
@@ -1938,22 +1936,18 @@ static void bnxt_init_rxbd_pages(struct bnxt_ring_struct *ring, u32 type)
 static int bnxt_init_one_rx_ring(struct bnxt *bp, int ring_nr)
 {
 	struct net_device *dev = bp->dev;
-	struct bnxt_napi *bnapi = bp->bnapi[ring_nr];
 	struct bnxt_rx_ring_info *rxr;
 	struct bnxt_ring_struct *ring;
 	u32 prod, type;
 	int i;
 
-	if (!bnapi)
-		return -EINVAL;
-
 	type = (bp->rx_buf_use_size << RX_BD_LEN_SHIFT) |
 		RX_BD_TYPE_RX_PACKET_BD | RX_BD_FLAGS_EOP;
 
 	if (NET_IP_ALIGN == 2)
 		type |= RX_BD_FLAGS_SOP;
 
-	rxr = &bnapi->rx_ring;
+	rxr = &bp->rx_ring[ring_nr];
 	ring = &rxr->rx_ring_struct;
 	bnxt_init_rxbd_pages(ring, type);
 
@@ -1969,11 +1963,12 @@ static int bnxt_init_one_rx_ring(struct bnxt *bp, int ring_nr)
 	rxr->rx_prod = prod;
 	ring->fw_ring_id = INVALID_HW_RING_ID;
 
+	ring = &rxr->rx_agg_ring_struct;
+	ring->fw_ring_id = INVALID_HW_RING_ID;
+
 	if (!(bp->flags & BNXT_FLAG_AGG_RINGS))
 		return 0;
 
-	ring = &rxr->rx_agg_ring_struct;
-
 	type = ((u32)PAGE_SIZE << RX_BD_LEN_SHIFT) |
 		RX_BD_TYPE_RX_AGG_BD | RX_BD_FLAGS_SOP;
 
@@ -1989,7 +1984,6 @@ static int bnxt_init_one_rx_ring(struct bnxt *bp, int ring_nr)
 		prod = NEXT_RX_AGG(prod);
 	}
 	rxr->rx_agg_prod = prod;
-	ring->fw_ring_id = INVALID_HW_RING_ID;
 
 	if (bp->flags & BNXT_FLAG_TPA) {
 		if (rxr->rx_tpa) {
@@ -2035,8 +2029,7 @@ static int bnxt_init_tx_rings(struct bnxt *bp)
 				   MAX_SKB_FRAGS + 1);
 
 	for (i = 0; i < bp->tx_nr_rings; i++) {
-		struct bnxt_napi *bnapi = bp->bnapi[i];
-		struct bnxt_tx_ring_info *txr = &bnapi->tx_ring;
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
 		struct bnxt_ring_struct *ring = &txr->tx_ring_struct;
 
 		ring->fw_ring_id = INVALID_HW_RING_ID;
@@ -2423,14 +2416,18 @@ static void bnxt_clear_ring_indices(struct bnxt *bp)
 		cpr = &bnapi->cp_ring;
 		cpr->cp_raw_cons = 0;
 
-		txr = &bnapi->tx_ring;
-		txr->tx_prod = 0;
-		txr->tx_cons = 0;
+		txr = bnapi->tx_ring;
+		if (txr) {
+			txr->tx_prod = 0;
+			txr->tx_cons = 0;
+		}
 
-		rxr = &bnapi->rx_ring;
-		rxr->rx_prod = 0;
-		rxr->rx_agg_prod = 0;
-		rxr->rx_sw_agg_prod = 0;
+		rxr = bnapi->rx_ring;
+		if (rxr) {
+			rxr->rx_prod = 0;
+			rxr->rx_agg_prod = 0;
+			rxr->rx_sw_agg_prod = 0;
+		}
 	}
 }
 
@@ -2496,6 +2493,10 @@ static void bnxt_free_mem(struct bnxt *bp, bool irq_re_init)
 		bnxt_free_stats(bp);
 		bnxt_free_ring_grps(bp);
 		bnxt_free_vnics(bp);
+		kfree(bp->tx_ring);
+		bp->tx_ring = NULL;
+		kfree(bp->rx_ring);
+		bp->rx_ring = NULL;
 		kfree(bp->bnapi);
 		bp->bnapi = NULL;
 	} else {
@@ -2505,7 +2506,7 @@ static void bnxt_free_mem(struct bnxt *bp, bool irq_re_init)
 
 static int bnxt_alloc_mem(struct bnxt *bp, bool irq_re_init)
 {
-	int i, rc, size, arr_size;
+	int i, j, rc, size, arr_size;
 	void *bnapi;
 
 	if (irq_re_init) {
@@ -2527,6 +2528,33 @@ static int bnxt_alloc_mem(struct bnxt *bp, bool irq_re_init)
 			bp->bnapi[i]->bp = bp;
 		}
 
+		bp->rx_ring = kcalloc(bp->rx_nr_rings,
+				      sizeof(struct bnxt_rx_ring_info),
+				      GFP_KERNEL);
+		if (!bp->rx_ring)
+			return -ENOMEM;
+
+		for (i = 0; i < bp->rx_nr_rings; i++) {
+			bp->rx_ring[i].bnapi = bp->bnapi[i];
+			bp->bnapi[i]->rx_ring = &bp->rx_ring[i];
+		}
+
+		bp->tx_ring = kcalloc(bp->tx_nr_rings,
+				      sizeof(struct bnxt_tx_ring_info),
+				      GFP_KERNEL);
+		if (!bp->tx_ring)
+			return -ENOMEM;
+
+		if (bp->flags & BNXT_FLAG_SHARED_RINGS)
+			j = 0;
+		else
+			j = bp->rx_nr_rings;
+
+		for (i = 0; i < bp->tx_nr_rings; i++, j++) {
+			bp->tx_ring[i].bnapi = bp->bnapi[j];
+			bp->bnapi[j]->tx_ring = &bp->tx_ring[i];
+		}
+
 		rc = bnxt_alloc_stats(bp);
 		if (rc)
 			goto alloc_mem_err;
@@ -2596,6 +2624,9 @@ int _hwrm_send_message(struct bnxt *bp, void *msg, u32 msg_len, int timeout)
 	/* Write request msg to hwrm channel */
 	__iowrite32_copy(bp->bar0, data, msg_len / 4);
 
+	for (i = msg_len; i < HWRM_MAX_REQ_LEN; i += 4)
+		writel(0, bp->bar0 + i);
+
 	/* currently supports only one outstanding message */
 	if (intr_process)
 		bp->hwrm_intr_seq_id = le32_to_cpu(req->target_id_seq_id) &
@@ -2710,6 +2741,14 @@ static int bnxt_hwrm_func_drv_rgtr(struct bnxt *bp)
 	return hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
 }
 
+static int bnxt_hwrm_func_drv_unrgtr(struct bnxt *bp)
+{
+	struct hwrm_func_drv_unrgtr_input req = {0};
+
+	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FUNC_DRV_UNRGTR, -1, -1);
+	return hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
+}
+
 static int bnxt_hwrm_tunnel_dst_port_free(struct bnxt *bp, u8 tunnel_type)
 {
 	u32 rc = 0;
@@ -2772,7 +2811,7 @@ static int bnxt_hwrm_cfa_l2_set_rx_mask(struct bnxt *bp, u16 vnic_id)
 	struct bnxt_vnic_info *vnic = &bp->vnic_info[vnic_id];
 
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_CFA_L2_SET_RX_MASK, -1, -1);
-	req.dflt_vnic_id = cpu_to_le32(vnic->fw_vnic_id);
+	req.vnic_id = cpu_to_le32(vnic->fw_vnic_id);
 
 	req.num_mc_entries = cpu_to_le32(vnic->mc_list_count);
 	req.mc_tbl_addr = cpu_to_le64(vnic->mc_list_mapping);
@@ -2805,7 +2844,7 @@ static int bnxt_hwrm_cfa_ntuple_filter_free(struct bnxt *bp,
 	 CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_SRC_PORT_MASK |	\
 	 CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_PORT |		\
 	 CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_PORT_MASK |	\
-	 CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_VNIC_ID)
+	 CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_ID)
 
 static int bnxt_hwrm_cfa_ntuple_filter_alloc(struct bnxt *bp,
 					     struct bnxt_ntuple_filter *fltr)
@@ -2824,7 +2863,7 @@ static int bnxt_hwrm_cfa_ntuple_filter_alloc(struct bnxt *bp,
 
 	req.ethertype = htons(ETH_P_IP);
 	memcpy(req.src_macaddr, fltr->src_mac_addr, ETH_ALEN);
-	req.ipaddr_type = 4;
+	req.ip_addr_type = CFA_NTUPLE_FILTER_ALLOC_REQ_IP_ADDR_TYPE_IPV4;
 	req.ip_protocol = keys->basic.ip_proto;
 
 	req.src_ipaddr[0] = keys->addrs.v4addrs.src;
@@ -2837,7 +2876,7 @@ static int bnxt_hwrm_cfa_ntuple_filter_alloc(struct bnxt *bp,
 	req.dst_port = keys->ports.dst;
 	req.dst_port_mask = cpu_to_be16(0xffff);
 
-	req.dst_vnic_id = cpu_to_le16(vnic->fw_vnic_id);
+	req.dst_id = cpu_to_le16(vnic->fw_vnic_id);
 	mutex_lock(&bp->hwrm_cmd_lock);
 	rc = _hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
 	if (!rc)
@@ -2857,10 +2896,10 @@ static int bnxt_hwrm_set_vnic_filter(struct bnxt *bp, u16 vnic_id, u16 idx,
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_CFA_L2_FILTER_ALLOC, -1, -1);
 	req.flags = cpu_to_le32(CFA_L2_FILTER_ALLOC_REQ_FLAGS_PATH_RX |
 				CFA_L2_FILTER_ALLOC_REQ_FLAGS_OUTERMOST);
-	req.dst_vnic_id = cpu_to_le16(bp->vnic_info[vnic_id].fw_vnic_id);
+	req.dst_id = cpu_to_le16(bp->vnic_info[vnic_id].fw_vnic_id);
 	req.enables =
 		cpu_to_le32(CFA_L2_FILTER_ALLOC_REQ_ENABLES_L2_ADDR |
-			    CFA_L2_FILTER_ALLOC_REQ_ENABLES_DST_VNIC_ID |
+			    CFA_L2_FILTER_ALLOC_REQ_ENABLES_DST_ID |
 			    CFA_L2_FILTER_ALLOC_REQ_ENABLES_L2_ADDR_MASK);
 	memcpy(req.l2_addr, mac_addr, ETH_ALEN);
 	req.l2_addr_mask[0] = 0xff;
@@ -2930,7 +2969,8 @@ static int bnxt_hwrm_vnic_set_tpa(struct bnxt *bp, u16 vnic_id, u32 tpa_flags)
 
 		req.enables =
 			cpu_to_le32(VNIC_TPA_CFG_REQ_ENABLES_MAX_AGG_SEGS |
-				    VNIC_TPA_CFG_REQ_ENABLES_MAX_AGGS);
+				    VNIC_TPA_CFG_REQ_ENABLES_MAX_AGGS |
+				    VNIC_TPA_CFG_REQ_ENABLES_MIN_AGG_LEN);
 
 		/* Number of segs are log2 units, and first packet is not
 		 * included as part of this units.
@@ -2948,6 +2988,8 @@ static int bnxt_hwrm_vnic_set_tpa(struct bnxt *bp, u16 vnic_id, u32 tpa_flags)
 		segs = ilog2(nsegs);
 		req.max_agg_segs = cpu_to_le16(segs);
 		req.max_aggs = cpu_to_le16(VNIC_TPA_CFG_REQ_MAX_AGGS_MAX);
+
+		req.min_agg_len = cpu_to_le32(512);
 	}
 	req.vnic_id = cpu_to_le16(vnic->fw_vnic_id);
 
@@ -3058,7 +3100,7 @@ static int bnxt_hwrm_vnic_ctx_alloc(struct bnxt *bp, u16 vnic_id)
 
 static int bnxt_hwrm_vnic_cfg(struct bnxt *bp, u16 vnic_id)
 {
-	int grp_idx = 0;
+	unsigned int ring = 0, grp_idx;
 	struct bnxt_vnic_info *vnic = &bp->vnic_info[vnic_id];
 	struct hwrm_vnic_cfg_input req = {0};
 
@@ -3069,10 +3111,11 @@ static int bnxt_hwrm_vnic_cfg(struct bnxt *bp, u16 vnic_id)
 	req.rss_rule = cpu_to_le16(vnic->fw_rss_cos_lb_ctx);
 	req.cos_rule = cpu_to_le16(0xffff);
 	if (vnic->flags & BNXT_VNIC_RSS_FLAG)
-		grp_idx = 0;
+		ring = 0;
 	else if (vnic->flags & BNXT_VNIC_RFS_FLAG)
-		grp_idx = vnic_id - 1;
+		ring = vnic_id - 1;
 
+	grp_idx = bp->rx_ring[ring].bnapi->index;
 	req.vnic_id = cpu_to_le16(vnic->fw_vnic_id);
 	req.dflt_ring_grp = cpu_to_le16(bp->grp_info[grp_idx].fw_grp_id);
 
@@ -3113,22 +3156,25 @@ static void bnxt_hwrm_vnic_free(struct bnxt *bp)
 		bnxt_hwrm_vnic_free_one(bp, i);
 }
 
-static int bnxt_hwrm_vnic_alloc(struct bnxt *bp, u16 vnic_id, u16 start_grp_id,
-				u16 end_grp_id)
+static int bnxt_hwrm_vnic_alloc(struct bnxt *bp, u16 vnic_id,
+				unsigned int start_rx_ring_idx,
+				unsigned int nr_rings)
 {
-	u32 rc = 0, i, j;
+	int rc = 0;
+	unsigned int i, j, grp_idx, end_idx = start_rx_ring_idx + nr_rings;
 	struct hwrm_vnic_alloc_input req = {0};
 	struct hwrm_vnic_alloc_output *resp = bp->hwrm_cmd_resp_addr;
 
 	/* map ring groups to this vnic */
-	for (i = start_grp_id, j = 0; i < end_grp_id; i++, j++) {
-		if (bp->grp_info[i].fw_grp_id == INVALID_HW_RING_ID) {
+	for (i = start_rx_ring_idx, j = 0; i < end_idx; i++, j++) {
+		grp_idx = bp->rx_ring[i].bnapi->index;
+		if (bp->grp_info[grp_idx].fw_grp_id == INVALID_HW_RING_ID) {
 			netdev_err(bp->dev, "Not enough ring groups avail:%x req:%x\n",
-				   j, (end_grp_id - start_grp_id));
+				   j, nr_rings);
 			break;
 		}
 		bp->vnic_info[vnic_id].fw_grp_ids[j] =
-					bp->grp_info[i].fw_grp_id;
+					bp->grp_info[grp_idx].fw_grp_id;
 	}
 
 	bp->vnic_info[vnic_id].fw_rss_cos_lb_ctx = INVALID_HW_RING_ID;
@@ -3155,20 +3201,22 @@ static int bnxt_hwrm_ring_grp_alloc(struct bnxt *bp)
 		struct hwrm_ring_grp_alloc_input req = {0};
 		struct hwrm_ring_grp_alloc_output *resp =
 					bp->hwrm_cmd_resp_addr;
+		unsigned int grp_idx = bp->rx_ring[i].bnapi->index;
 
 		bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_RING_GRP_ALLOC, -1, -1);
 
-		req.cr = cpu_to_le16(bp->grp_info[i].cp_fw_ring_id);
-		req.rr = cpu_to_le16(bp->grp_info[i].rx_fw_ring_id);
-		req.ar = cpu_to_le16(bp->grp_info[i].agg_fw_ring_id);
-		req.sc = cpu_to_le16(bp->grp_info[i].fw_stats_ctx);
+		req.cr = cpu_to_le16(bp->grp_info[grp_idx].cp_fw_ring_id);
+		req.rr = cpu_to_le16(bp->grp_info[grp_idx].rx_fw_ring_id);
+		req.ar = cpu_to_le16(bp->grp_info[grp_idx].agg_fw_ring_id);
+		req.sc = cpu_to_le16(bp->grp_info[grp_idx].fw_stats_ctx);
 
 		rc = _hwrm_send_message(bp, &req, sizeof(req),
 					HWRM_CMD_TIMEOUT);
 		if (rc)
 			break;
 
-		bp->grp_info[i].fw_grp_id = le32_to_cpu(resp->ring_group_id);
+		bp->grp_info[grp_idx].fw_grp_id =
+			le32_to_cpu(resp->ring_group_id);
 	}
 	mutex_unlock(&bp->hwrm_cmd_lock);
 	return rc;
@@ -3293,75 +3341,66 @@ static int bnxt_hwrm_ring_alloc(struct bnxt *bp)
 {
 	int i, rc = 0;
 
-	if (bp->cp_nr_rings) {
-		for (i = 0; i < bp->cp_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-			struct bnxt_ring_struct *ring = &cpr->cp_ring_struct;
+	for (i = 0; i < bp->cp_nr_rings; i++) {
+		struct bnxt_napi *bnapi = bp->bnapi[i];
+		struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
+		struct bnxt_ring_struct *ring = &cpr->cp_ring_struct;
 
-			rc = hwrm_ring_alloc_send_msg(bp, ring,
-						      HWRM_RING_ALLOC_CMPL, i,
-						      INVALID_STATS_CTX_ID);
-			if (rc)
-				goto err_out;
-			cpr->cp_doorbell = bp->bar1 + i * 0x80;
-			BNXT_CP_DB(cpr->cp_doorbell, cpr->cp_raw_cons);
-			bp->grp_info[i].cp_fw_ring_id = ring->fw_ring_id;
-		}
+		rc = hwrm_ring_alloc_send_msg(bp, ring, HWRM_RING_ALLOC_CMPL, i,
+					      INVALID_STATS_CTX_ID);
+		if (rc)
+			goto err_out;
+		cpr->cp_doorbell = bp->bar1 + i * 0x80;
+		BNXT_CP_DB(cpr->cp_doorbell, cpr->cp_raw_cons);
+		bp->grp_info[i].cp_fw_ring_id = ring->fw_ring_id;
 	}
 
-	if (bp->tx_nr_rings) {
-		for (i = 0; i < bp->tx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_tx_ring_info *txr = &bnapi->tx_ring;
-			struct bnxt_ring_struct *ring = &txr->tx_ring_struct;
-			u16 fw_stats_ctx = bp->grp_info[i].fw_stats_ctx;
+	for (i = 0; i < bp->tx_nr_rings; i++) {
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
+		struct bnxt_ring_struct *ring = &txr->tx_ring_struct;
+		u32 map_idx = txr->bnapi->index;
+		u16 fw_stats_ctx = bp->grp_info[map_idx].fw_stats_ctx;
 
-			rc = hwrm_ring_alloc_send_msg(bp, ring,
-						      HWRM_RING_ALLOC_TX, i,
-						      fw_stats_ctx);
-			if (rc)
-				goto err_out;
-			txr->tx_doorbell = bp->bar1 + i * 0x80;
-		}
+		rc = hwrm_ring_alloc_send_msg(bp, ring, HWRM_RING_ALLOC_TX,
+					      map_idx, fw_stats_ctx);
+		if (rc)
+			goto err_out;
+		txr->tx_doorbell = bp->bar1 + map_idx * 0x80;
 	}
 
-	if (bp->rx_nr_rings) {
-		for (i = 0; i < bp->rx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
-			struct bnxt_ring_struct *ring = &rxr->rx_ring_struct;
+	for (i = 0; i < bp->rx_nr_rings; i++) {
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
+		struct bnxt_ring_struct *ring = &rxr->rx_ring_struct;
+		u32 map_idx = rxr->bnapi->index;
 
-			rc = hwrm_ring_alloc_send_msg(bp, ring,
-						      HWRM_RING_ALLOC_RX, i,
-						      INVALID_STATS_CTX_ID);
-			if (rc)
-				goto err_out;
-			rxr->rx_doorbell = bp->bar1 + i * 0x80;
-			writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
-			bp->grp_info[i].rx_fw_ring_id = ring->fw_ring_id;
-		}
+		rc = hwrm_ring_alloc_send_msg(bp, ring, HWRM_RING_ALLOC_RX,
+					      map_idx, INVALID_STATS_CTX_ID);
+		if (rc)
+			goto err_out;
+		rxr->rx_doorbell = bp->bar1 + map_idx * 0x80;
+		writel(DB_KEY_RX | rxr->rx_prod, rxr->rx_doorbell);
+		bp->grp_info[map_idx].rx_fw_ring_id = ring->fw_ring_id;
 	}
 
 	if (bp->flags & BNXT_FLAG_AGG_RINGS) {
 		for (i = 0; i < bp->rx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
+			struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
 			struct bnxt_ring_struct *ring =
 						&rxr->rx_agg_ring_struct;
+			u32 grp_idx = rxr->bnapi->index;
+			u32 map_idx = grp_idx + bp->rx_nr_rings;
 
 			rc = hwrm_ring_alloc_send_msg(bp, ring,
 						      HWRM_RING_ALLOC_AGG,
-						      bp->rx_nr_rings + i,
+						      map_idx,
 						      INVALID_STATS_CTX_ID);
 			if (rc)
 				goto err_out;
 
-			rxr->rx_agg_doorbell =
-				bp->bar1 + (bp->rx_nr_rings + i) * 0x80;
+			rxr->rx_agg_doorbell = bp->bar1 + map_idx * 0x80;
 			writel(DB_KEY_RX | rxr->rx_agg_prod,
 			       rxr->rx_agg_doorbell);
-			bp->grp_info[i].agg_fw_ring_id = ring->fw_ring_id;
+			bp->grp_info[grp_idx].agg_fw_ring_id = ring->fw_ring_id;
 		}
 	}
 err_out:
@@ -3377,7 +3416,7 @@ static int hwrm_ring_free_send_msg(struct bnxt *bp,
 	struct hwrm_ring_free_output *resp = bp->hwrm_cmd_resp_addr;
 	u16 error_code;
 
-	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_RING_FREE, -1, -1);
+	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_RING_FREE, cmpl_ring_id, -1);
 	req.ring_type = ring_type;
 	req.ring_id = cpu_to_le16(ring->fw_ring_id);
 
@@ -3408,91 +3447,75 @@ static int hwrm_ring_free_send_msg(struct bnxt *bp,
 	return 0;
 }
 
-static int bnxt_hwrm_ring_free(struct bnxt *bp, bool close_path)
+static void bnxt_hwrm_ring_free(struct bnxt *bp, bool close_path)
 {
-	int i, rc = 0;
+	int i;
 
 	if (!bp->bnapi)
-		return 0;
+		return;
 
-	if (bp->tx_nr_rings) {
-		for (i = 0; i < bp->tx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_tx_ring_info *txr = &bnapi->tx_ring;
-			struct bnxt_ring_struct *ring = &txr->tx_ring_struct;
-			u32 cmpl_ring_id = bp->grp_info[i].cp_fw_ring_id;
-
-			if (ring->fw_ring_id != INVALID_HW_RING_ID) {
-				hwrm_ring_free_send_msg(
-					bp, ring,
-					RING_FREE_REQ_RING_TYPE_TX,
-					close_path ? cmpl_ring_id :
-					INVALID_HW_RING_ID);
-				ring->fw_ring_id = INVALID_HW_RING_ID;
-			}
+	for (i = 0; i < bp->tx_nr_rings; i++) {
+		struct bnxt_tx_ring_info *txr = &bp->tx_ring[i];
+		struct bnxt_ring_struct *ring = &txr->tx_ring_struct;
+		u32 grp_idx = txr->bnapi->index;
+		u32 cmpl_ring_id = bp->grp_info[grp_idx].cp_fw_ring_id;
+
+		if (ring->fw_ring_id != INVALID_HW_RING_ID) {
+			hwrm_ring_free_send_msg(bp, ring,
+						RING_FREE_REQ_RING_TYPE_TX,
+						close_path ? cmpl_ring_id :
+						INVALID_HW_RING_ID);
+			ring->fw_ring_id = INVALID_HW_RING_ID;
 		}
 	}
 
-	if (bp->rx_nr_rings) {
-		for (i = 0; i < bp->rx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
-			struct bnxt_ring_struct *ring = &rxr->rx_ring_struct;
-			u32 cmpl_ring_id = bp->grp_info[i].cp_fw_ring_id;
-
-			if (ring->fw_ring_id != INVALID_HW_RING_ID) {
-				hwrm_ring_free_send_msg(
-					bp, ring,
-					RING_FREE_REQ_RING_TYPE_RX,
-					close_path ? cmpl_ring_id :
-					INVALID_HW_RING_ID);
-				ring->fw_ring_id = INVALID_HW_RING_ID;
-				bp->grp_info[i].rx_fw_ring_id =
-					INVALID_HW_RING_ID;
-			}
+	for (i = 0; i < bp->rx_nr_rings; i++) {
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
+		struct bnxt_ring_struct *ring = &rxr->rx_ring_struct;
+		u32 grp_idx = rxr->bnapi->index;
+		u32 cmpl_ring_id = bp->grp_info[grp_idx].cp_fw_ring_id;
+
+		if (ring->fw_ring_id != INVALID_HW_RING_ID) {
+			hwrm_ring_free_send_msg(bp, ring,
+						RING_FREE_REQ_RING_TYPE_RX,
+						close_path ? cmpl_ring_id :
+						INVALID_HW_RING_ID);
+			ring->fw_ring_id = INVALID_HW_RING_ID;
+			bp->grp_info[grp_idx].rx_fw_ring_id =
+				INVALID_HW_RING_ID;
 		}
 	}
 
-	if (bp->rx_agg_nr_pages) {
-		for (i = 0; i < bp->rx_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_rx_ring_info *rxr = &bnapi->rx_ring;
-			struct bnxt_ring_struct *ring =
-						&rxr->rx_agg_ring_struct;
-			u32 cmpl_ring_id = bp->grp_info[i].cp_fw_ring_id;
-
-			if (ring->fw_ring_id != INVALID_HW_RING_ID) {
-				hwrm_ring_free_send_msg(
-					bp, ring,
-					RING_FREE_REQ_RING_TYPE_RX,
-					close_path ? cmpl_ring_id :
-					INVALID_HW_RING_ID);
-				ring->fw_ring_id = INVALID_HW_RING_ID;
-				bp->grp_info[i].agg_fw_ring_id =
-					INVALID_HW_RING_ID;
-			}
+	for (i = 0; i < bp->rx_nr_rings; i++) {
+		struct bnxt_rx_ring_info *rxr = &bp->rx_ring[i];
+		struct bnxt_ring_struct *ring = &rxr->rx_agg_ring_struct;
+		u32 grp_idx = rxr->bnapi->index;
+		u32 cmpl_ring_id = bp->grp_info[grp_idx].cp_fw_ring_id;
+
+		if (ring->fw_ring_id != INVALID_HW_RING_ID) {
+			hwrm_ring_free_send_msg(bp, ring,
+						RING_FREE_REQ_RING_TYPE_RX,
+						close_path ? cmpl_ring_id :
+						INVALID_HW_RING_ID);
+			ring->fw_ring_id = INVALID_HW_RING_ID;
+			bp->grp_info[grp_idx].agg_fw_ring_id =
+				INVALID_HW_RING_ID;
 		}
 	}
 
-	if (bp->cp_nr_rings) {
-		for (i = 0; i < bp->cp_nr_rings; i++) {
-			struct bnxt_napi *bnapi = bp->bnapi[i];
-			struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
-			struct bnxt_ring_struct *ring = &cpr->cp_ring_struct;
-
-			if (ring->fw_ring_id != INVALID_HW_RING_ID) {
-				hwrm_ring_free_send_msg(
-					bp, ring,
-					RING_FREE_REQ_RING_TYPE_CMPL,
-					INVALID_HW_RING_ID);
-				ring->fw_ring_id = INVALID_HW_RING_ID;
-				bp->grp_info[i].cp_fw_ring_id =
-							INVALID_HW_RING_ID;
-			}
+	for (i = 0; i < bp->cp_nr_rings; i++) {
+		struct bnxt_napi *bnapi = bp->bnapi[i];
+		struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
+		struct bnxt_ring_struct *ring = &cpr->cp_ring_struct;
+
+		if (ring->fw_ring_id != INVALID_HW_RING_ID) {
+			hwrm_ring_free_send_msg(bp, ring,
+						RING_FREE_REQ_RING_TYPE_CMPL,
+						INVALID_HW_RING_ID);
+			ring->fw_ring_id = INVALID_HW_RING_ID;
+			bp->grp_info[i].cp_fw_ring_id = INVALID_HW_RING_ID;
 		}
 	}
-
-	return rc;
 }
 
 int bnxt_hwrm_set_coal(struct bnxt *bp)
@@ -3604,7 +3627,7 @@ static int bnxt_hwrm_stat_ctx_alloc(struct bnxt *bp)
 	return 0;
 }
 
-static int bnxt_hwrm_func_qcaps(struct bnxt *bp)
+int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 {
 	int rc = 0;
 	struct hwrm_func_qcaps_input req = {0};
@@ -3628,9 +3651,10 @@ static int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 		pf->max_rsscos_ctxs = le16_to_cpu(resp->max_rsscos_ctx);
 		pf->max_cp_rings = le16_to_cpu(resp->max_cmpl_rings);
 		pf->max_tx_rings = le16_to_cpu(resp->max_tx_rings);
-		pf->max_pf_tx_rings = pf->max_tx_rings;
 		pf->max_rx_rings = le16_to_cpu(resp->max_rx_rings);
-		pf->max_pf_rx_rings = pf->max_rx_rings;
+		pf->max_hw_ring_grps = le32_to_cpu(resp->max_hw_ring_grps);
+		if (!pf->max_hw_ring_grps)
+			pf->max_hw_ring_grps = pf->max_tx_rings;
 		pf->max_l2_ctxs = le16_to_cpu(resp->max_l2_ctxs);
 		pf->max_vnics = le16_to_cpu(resp->max_vnics);
 		pf->max_stat_ctxs = le16_to_cpu(resp->max_stat_ctx);
@@ -3658,6 +3682,9 @@ static int bnxt_hwrm_func_qcaps(struct bnxt *bp)
 		vf->max_cp_rings = le16_to_cpu(resp->max_cmpl_rings);
 		vf->max_tx_rings = le16_to_cpu(resp->max_tx_rings);
 		vf->max_rx_rings = le16_to_cpu(resp->max_rx_rings);
+		vf->max_hw_ring_grps = le32_to_cpu(resp->max_hw_ring_grps);
+		if (!vf->max_hw_ring_grps)
+			vf->max_hw_ring_grps = vf->max_tx_rings;
 		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);
@@ -3734,14 +3761,11 @@ static int bnxt_hwrm_ver_get(struct bnxt *bp)
 
 	memcpy(&bp->ver_resp, resp, sizeof(struct hwrm_ver_get_output));
 
-	if (req.hwrm_intf_maj != resp->hwrm_intf_maj ||
-	    req.hwrm_intf_min != resp->hwrm_intf_min ||
-	    req.hwrm_intf_upd != resp->hwrm_intf_upd) {
-		netdev_warn(bp->dev, "HWRM interface %d.%d.%d does not match driver interface %d.%d.%d.\n",
+	if (resp->hwrm_intf_maj < 1) {
+		netdev_warn(bp->dev, "HWRM interface %d.%d.%d is older than 1.0.0.\n",
 			    resp->hwrm_intf_maj, resp->hwrm_intf_min,
-			    resp->hwrm_intf_upd, req.hwrm_intf_maj,
-			    req.hwrm_intf_min, req.hwrm_intf_upd);
-		netdev_warn(bp->dev, "Please update driver or firmware with matching interface versions.\n");
+			    resp->hwrm_intf_upd);
+		netdev_warn(bp->dev, "Please update firmware with HWRM interface 1.0.0 or newer.\n");
 	}
 	snprintf(bp->fw_ver_str, BC_HWRM_STR_LEN, "bc %d.%d.%d rm %d.%d.%d",
 		 resp->hwrm_fw_maj, resp->hwrm_fw_min, resp->hwrm_fw_bld,
@@ -3867,7 +3891,7 @@ static int bnxt_alloc_rfs_vnics(struct bnxt *bp)
 			break;
 
 		bp->vnic_info[vnic_id].flags |= BNXT_VNIC_RFS_FLAG;
-		rc = bnxt_hwrm_vnic_alloc(bp, vnic_id, ring_id, ring_id + 1);
+		rc = bnxt_hwrm_vnic_alloc(bp, vnic_id, ring_id, 1);
 		if (rc) {
 			netdev_err(bp->dev, "hwrm vnic %d alloc failure rc: %x\n",
 				   vnic_id, rc);
@@ -3944,8 +3968,7 @@ static int bnxt_init_chip(struct bnxt *bp, bool irq_re_init)
 	}
 	bp->vnic_info[0].uc_filter_count = 1;
 
-	bp->vnic_info[0].rx_mask = CFA_L2_SET_RX_MASK_REQ_MASK_UNICAST |
-				   CFA_L2_SET_RX_MASK_REQ_MASK_BCAST;
+	bp->vnic_info[0].rx_mask = CFA_L2_SET_RX_MASK_REQ_MASK_BCAST;
 
 	if ((bp->dev->flags & IFF_PROMISC) && BNXT_PF(bp))
 		bp->vnic_info[0].rx_mask |=
@@ -4026,20 +4049,42 @@ static int bnxt_set_real_num_queues(struct bnxt *bp)
 		return rc;
 
 #ifdef CONFIG_RFS_ACCEL
-	if (bp->rx_nr_rings)
+	if (bp->flags & BNXT_FLAG_RFS)
 		dev->rx_cpu_rmap = alloc_irq_cpu_rmap(bp->rx_nr_rings);
-	if (!dev->rx_cpu_rmap)
-		rc = -ENOMEM;
 #endif
 
 	return rc;
 }
 
+static int bnxt_trim_rings(struct bnxt *bp, int *rx, int *tx, int max,
+			   bool shared)
+{
+	int _rx = *rx, _tx = *tx;
+
+	if (shared) {
+		*rx = min_t(int, _rx, max);
+		*tx = min_t(int, _tx, max);
+	} else {
+		if (max < 2)
+			return -ENOMEM;
+
+		while (_rx + _tx > max) {
+			if (_rx > _tx && _rx > 1)
+				_rx--;
+			else if (_tx > 1)
+				_tx--;
+		}
+		*rx = _rx;
+		*tx = _tx;
+	}
+	return 0;
+}
+
 static int bnxt_setup_msix(struct bnxt *bp)
 {
 	struct msix_entry *msix_ent;
 	struct net_device *dev = bp->dev;
-	int i, total_vecs, rc = 0;
+	int i, total_vecs, rc = 0, min = 1;
 	const int len = sizeof(bp->irq_tbl[0].name);
 
 	bp->flags &= ~BNXT_FLAG_USING_MSIX;
@@ -4054,7 +4099,10 @@ static int bnxt_setup_msix(struct bnxt *bp)
 		msix_ent[i].vector = 0;
 	}
 
-	total_vecs = pci_enable_msix_range(bp->pdev, msix_ent, 1, total_vecs);
+	if (!(bp->flags & BNXT_FLAG_SHARED_RINGS))
+		min = 2;
+
+	total_vecs = pci_enable_msix_range(bp->pdev, msix_ent, min, total_vecs);
 	if (total_vecs < 0) {
 		rc = -ENODEV;
 		goto msix_setup_exit;
@@ -4065,8 +4113,11 @@ static int bnxt_setup_msix(struct bnxt *bp)
 		int tcs;
 
 		/* Trim rings based upon num of vectors allocated */
-		bp->rx_nr_rings = min_t(int, total_vecs, bp->rx_nr_rings);
-		bp->tx_nr_rings = min_t(int, total_vecs, bp->tx_nr_rings);
+		rc = bnxt_trim_rings(bp, &bp->rx_nr_rings, &bp->tx_nr_rings,
+				     total_vecs, min == 1);
+		if (rc)
+			goto msix_setup_exit;
+
 		bp->tx_nr_rings_per_tc = bp->tx_nr_rings;
 		tcs = netdev_get_num_tc(dev);
 		if (tcs > 1) {
@@ -4085,12 +4136,21 @@ static int bnxt_setup_msix(struct bnxt *bp)
 				}
 			}
 		}
-		bp->cp_nr_rings = max_t(int, bp->rx_nr_rings, bp->tx_nr_rings);
+		bp->cp_nr_rings = total_vecs;
 
 		for (i = 0; i < bp->cp_nr_rings; i++) {
+			char *attr;
+
 			bp->irq_tbl[i].vector = msix_ent[i].vector;
+			if (bp->flags & BNXT_FLAG_SHARED_RINGS)
+				attr = "TxRx";
+			else if (i < bp->rx_nr_rings)
+				attr = "rx";
+			else
+				attr = "tx";
+
 			snprintf(bp->irq_tbl[i].name, len,
-				 "%s-%s-%d", dev->name, "TxRx", i);
+				 "%s-%s-%d", dev->name, attr, i);
 			bp->irq_tbl[i].handler = bnxt_msix;
 		}
 		rc = bnxt_set_real_num_queues(bp);
@@ -4128,6 +4188,7 @@ static int bnxt_setup_inta(struct bnxt *bp)
 	bp->tx_nr_rings = 1;
 	bp->cp_nr_rings = 1;
 	bp->tx_nr_rings_per_tc = bp->tx_nr_rings;
+	bp->flags |= BNXT_FLAG_SHARED_RINGS;
 	bp->irq_tbl[0].vector = bp->pdev->irq;
 	snprintf(bp->irq_tbl[0].name, len,
 		 "%s-%s-%d", bp->dev->name, "TxRx", 0);
@@ -4176,7 +4237,7 @@ static void bnxt_free_irq(struct bnxt *bp)
 
 static int bnxt_request_irq(struct bnxt *bp)
 {
-	int i, rc = 0;
+	int i, j, rc = 0;
 	unsigned long flags = 0;
 #ifdef CONFIG_RFS_ACCEL
 	struct cpu_rmap *rmap = bp->dev->rx_cpu_rmap;
@@ -4185,14 +4246,15 @@ static int bnxt_request_irq(struct bnxt *bp)
 	if (!(bp->flags & BNXT_FLAG_USING_MSIX))
 		flags = IRQF_SHARED;
 
-	for (i = 0; i < bp->cp_nr_rings; i++) {
+	for (i = 0, j = 0; i < bp->cp_nr_rings; i++) {
 		struct bnxt_irq *irq = &bp->irq_tbl[i];
 #ifdef CONFIG_RFS_ACCEL
-		if (rmap && (i < bp->rx_nr_rings)) {
+		if (rmap && bp->bnapi[i]->rx_ring) {
 			rc = irq_cpu_rmap_add(rmap, irq->vector);
 			if (rc)
 				netdev_warn(bp->dev, "failed adding irq rmap for ring %d\n",
-					    i);
+					    j);
+			j++;
 		}
 #endif
 		rc = request_irq(irq->vector, irq->handler, flags, irq->name,
@@ -4230,12 +4292,10 @@ static void bnxt_init_napi(struct bnxt *bp)
 			bnapi = bp->bnapi[i];
 			netif_napi_add(bp->dev, &bnapi->napi,
 				       bnxt_poll, 64);
-			napi_hash_add(&bnapi->napi);
 		}
 	} else {
 		bnapi = bp->bnapi[0];
 		netif_napi_add(bp->dev, &bnapi->napi, bnxt_poll, 64);
-		napi_hash_add(&bnapi->napi);
 	}
 }
 
@@ -4265,14 +4325,12 @@ static void bnxt_enable_napi(struct bnxt *bp)
 static void bnxt_tx_disable(struct bnxt *bp)
 {
 	int i;
-	struct bnxt_napi *bnapi;
 	struct bnxt_tx_ring_info *txr;
 	struct netdev_queue *txq;
 
-	if (bp->bnapi) {
+	if (bp->tx_ring) {
 		for (i = 0; i < bp->tx_nr_rings; i++) {
-			bnapi = bp->bnapi[i];
-			txr = &bnapi->tx_ring;
+			txr = &bp->tx_ring[i];
 			txq = netdev_get_tx_queue(bp->dev, i);
 			__netif_tx_lock(txq, smp_processor_id());
 			txr->dev_state = BNXT_DEV_STATE_CLOSING;
@@ -4287,13 +4345,11 @@ static void bnxt_tx_disable(struct bnxt *bp)
 static void bnxt_tx_enable(struct bnxt *bp)
 {
 	int i;
-	struct bnxt_napi *bnapi;
 	struct bnxt_tx_ring_info *txr;
 	struct netdev_queue *txq;
 
 	for (i = 0; i < bp->tx_nr_rings; i++) {
-		bnapi = bp->bnapi[i];
-		txr = &bnapi->tx_ring;
+		txr = &bp->tx_ring[i];
 		txq = netdev_get_tx_queue(bp->dev, i);
 		txr->dev_state = 0;
 	}
@@ -4355,7 +4411,7 @@ static int bnxt_update_link(struct bnxt *bp, bool chng_link_state)
 	link_info->auto_mode = resp->auto_mode;
 	link_info->auto_pause_setting = resp->auto_pause;
 	link_info->force_pause_setting = resp->force_pause;
-	link_info->duplex_setting = resp->duplex_setting;
+	link_info->duplex_setting = resp->duplex;
 	if (link_info->phy_link_status == BNXT_LINK_LINK)
 		link_info->link_speed = le16_to_cpu(resp->link_speed);
 	else
@@ -4499,20 +4555,18 @@ static int bnxt_update_phy_setting(struct bnxt *bp)
 	if (!(link_info->autoneg & BNXT_AUTONEG_FLOW_CTRL) &&
 	    link_info->force_pause_setting != link_info->req_flow_ctrl)
 		update_pause = true;
-	if (link_info->req_duplex != link_info->duplex_setting)
-		update_link = true;
 	if (!(link_info->autoneg & BNXT_AUTONEG_SPEED)) {
 		if (BNXT_AUTO_MODE(link_info->auto_mode))
 			update_link = true;
 		if (link_info->req_link_speed != link_info->force_link_speed)
 			update_link = true;
+		if (link_info->req_duplex != link_info->duplex_setting)
+			update_link = true;
 	} else {
 		if (link_info->auto_mode == BNXT_LINK_AUTO_NONE)
 			update_link = true;
 		if (link_info->advertising != link_info->auto_link_speeds)
 			update_link = true;
-		if (link_info->req_link_speed != link_info->auto_link_speed)
-			update_link = true;
 	}
 
 	if (update_link)
@@ -4589,7 +4643,7 @@ static int __bnxt_open_nic(struct bnxt *bp, bool irq_re_init, bool link_re_init)
 	if (link_re_init) {
 		rc = bnxt_update_phy_setting(bp);
 		if (rc)
-			goto open_err;
+			netdev_warn(bp->dev, "failed to update phy settings\n");
 	}
 
 	if (irq_re_init) {
@@ -4607,6 +4661,7 @@ static int __bnxt_open_nic(struct bnxt *bp, bool irq_re_init, bool link_re_init)
 	/* Enable TX queues */
 	bnxt_tx_enable(bp);
 	mod_timer(&bp->timer, jiffies + bp->current_interval);
+	bnxt_update_link(bp, true);
 
 	return 0;
 
@@ -4773,8 +4828,6 @@ bnxt_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
 
 		stats->multicast += le64_to_cpu(hw_stats->rx_mcast_pkts);
 
-		stats->rx_dropped += le64_to_cpu(hw_stats->rx_drop_pkts);
-
 		stats->tx_dropped += le64_to_cpu(hw_stats->tx_drop_pkts);
 	}
 
@@ -4932,9 +4985,32 @@ skip_uc:
 	return rc;
 }
 
+static bool bnxt_rfs_capable(struct bnxt *bp)
+{
+#ifdef CONFIG_RFS_ACCEL
+	struct bnxt_pf_info *pf = &bp->pf;
+	int vnics;
+
+	if (BNXT_VF(bp) || !(bp->flags & BNXT_FLAG_MSIX_CAP))
+		return false;
+
+	vnics = 1 + bp->rx_nr_rings;
+	if (vnics > pf->max_rsscos_ctxs || vnics > pf->max_vnics)
+		return false;
+
+	return true;
+#else
+	return false;
+#endif
+}
+
 static netdev_features_t bnxt_fix_features(struct net_device *dev,
 					   netdev_features_t features)
 {
+	struct bnxt *bp = netdev_priv(dev);
+
+	if (!bnxt_rfs_capable(bp))
+		features &= ~NETIF_F_NTUPLE;
 	return features;
 }
 
@@ -4975,7 +5051,7 @@ static int bnxt_set_features(struct net_device *dev, netdev_features_t features)
 
 		bp->flags = flags;
 
-		if (!netif_running(dev)) {
+		if (!test_bit(BNXT_STATE_OPEN, &bp->state)) {
 			if (update_tpa)
 				bnxt_set_ring_params(bp);
 			return rc;
@@ -4999,31 +5075,53 @@ static int bnxt_set_features(struct net_device *dev, netdev_features_t features)
 	return rc;
 }
 
+static void bnxt_dump_tx_sw_state(struct bnxt_napi *bnapi)
+{
+	struct bnxt_tx_ring_info *txr = bnapi->tx_ring;
+	int i = bnapi->index;
+
+	if (!txr)
+		return;
+
+	netdev_info(bnapi->bp->dev, "[%d]: tx{fw_ring: %d prod: %x cons: %x}\n",
+		    i, txr->tx_ring_struct.fw_ring_id, txr->tx_prod,
+		    txr->tx_cons);
+}
+
+static void bnxt_dump_rx_sw_state(struct bnxt_napi *bnapi)
+{
+	struct bnxt_rx_ring_info *rxr = bnapi->rx_ring;
+	int i = bnapi->index;
+
+	if (!rxr)
+		return;
+
+	netdev_info(bnapi->bp->dev, "[%d]: rx{fw_ring: %d prod: %x} rx_agg{fw_ring: %d agg_prod: %x sw_agg_prod: %x}\n",
+		    i, rxr->rx_ring_struct.fw_ring_id, rxr->rx_prod,
+		    rxr->rx_agg_ring_struct.fw_ring_id, rxr->rx_agg_prod,
+		    rxr->rx_sw_agg_prod);
+}
+
+static void bnxt_dump_cp_sw_state(struct bnxt_napi *bnapi)
+{
+	struct bnxt_cp_ring_info *cpr = &bnapi->cp_ring;
+	int i = bnapi->index;
+
+	netdev_info(bnapi->bp->dev, "[%d]: cp{fw_ring: %d raw_cons: %x}\n",
+		    i, cpr->cp_ring_struct.fw_ring_id, cpr->cp_raw_cons);
+}
+
 static void bnxt_dbg_dump_states(struct bnxt *bp)
 {
 	int i;
 	struct bnxt_napi *bnapi;
-	struct bnxt_tx_ring_info *txr;
-	struct bnxt_rx_ring_info *rxr;
-	struct bnxt_cp_ring_info *cpr;
 
 	for (i = 0; i < bp->cp_nr_rings; i++) {
 		bnapi = bp->bnapi[i];
-		txr = &bnapi->tx_ring;
-		rxr = &bnapi->rx_ring;
-		cpr = &bnapi->cp_ring;
 		if (netif_msg_drv(bp)) {
-			netdev_info(bp->dev, "[%d]: tx{fw_ring: %d prod: %x cons: %x}\n",
-				    i, txr->tx_ring_struct.fw_ring_id,
-				    txr->tx_prod, txr->tx_cons);
-			netdev_info(bp->dev, "[%d]: rx{fw_ring: %d prod: %x} rx_agg{fw_ring: %d agg_prod: %x sw_agg_prod: %x}\n",
-				    i, rxr->rx_ring_struct.fw_ring_id,
-				    rxr->rx_prod,
-				    rxr->rx_agg_ring_struct.fw_ring_id,
-				    rxr->rx_agg_prod, rxr->rx_sw_agg_prod);
-			netdev_info(bp->dev, "[%d]: cp{fw_ring: %d raw_cons: %x}\n",
-				    i, cpr->cp_ring_struct.fw_ring_id,
-				    cpr->cp_raw_cons);
+			bnxt_dump_tx_sw_state(bnapi);
+			bnxt_dump_rx_sw_state(bnapi);
+			bnxt_dump_cp_sw_state(bnapi);
 		}
 	}
 }
@@ -5294,10 +5392,14 @@ static int bnxt_setup_tc(struct net_device *dev, u8 tc)
 		return 0;
 
 	if (tc) {
-		int max_rx_rings, max_tx_rings;
+		int max_rx_rings, max_tx_rings, rc;
+		bool sh = false;
+
+		if (bp->flags & BNXT_FLAG_SHARED_RINGS)
+			sh = true;
 
-		bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings);
-		if (bp->tx_nr_rings_per_tc * tc > max_tx_rings)
+		rc = bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, sh);
+		if (rc || bp->tx_nr_rings_per_tc * tc > max_tx_rings)
 			return -ENOMEM;
 	}
 
@@ -5551,6 +5653,7 @@ static void bnxt_remove_one(struct pci_dev *pdev)
 	cancel_work_sync(&bp->sp_task);
 	bp->sp_event = 0;
 
+	bnxt_hwrm_func_drv_unrgtr(bp);
 	bnxt_free_hwrm_resources(bp);
 	pci_iounmap(pdev, bp->bar2);
 	pci_iounmap(pdev, bp->bar1);
@@ -5575,22 +5678,16 @@ static int bnxt_probe_phy(struct bnxt *bp)
 	}
 
 	/*initialize the ethool setting copy with NVM settings */
-	if (BNXT_AUTO_MODE(link_info->auto_mode))
-		link_info->autoneg |= BNXT_AUTONEG_SPEED;
-
-	if (link_info->auto_pause_setting & BNXT_LINK_PAUSE_BOTH) {
-		if (link_info->auto_pause_setting == BNXT_LINK_PAUSE_BOTH)
-			link_info->autoneg |= BNXT_AUTONEG_FLOW_CTRL;
+	if (BNXT_AUTO_MODE(link_info->auto_mode)) {
+		link_info->autoneg = BNXT_AUTONEG_SPEED |
+				     BNXT_AUTONEG_FLOW_CTRL;
+		link_info->advertising = link_info->auto_link_speeds;
 		link_info->req_flow_ctrl = link_info->auto_pause_setting;
-	} else if (link_info->force_pause_setting & BNXT_LINK_PAUSE_BOTH) {
+	} else {
+		link_info->req_link_speed = link_info->force_link_speed;
+		link_info->req_duplex = link_info->duplex_setting;
 		link_info->req_flow_ctrl = link_info->force_pause_setting;
 	}
-	link_info->req_duplex = link_info->duplex_setting;
-	if (link_info->autoneg & BNXT_AUTONEG_SPEED)
-		link_info->req_link_speed = link_info->auto_link_speed;
-	else
-		link_info->req_link_speed = link_info->force_link_speed;
-	link_info->advertising = link_info->auto_link_speeds;
 	snprintf(phy_ver, PHY_VER_STR_LEN, " ph %d.%d.%d",
 		 link_info->phy_ver[0],
 		 link_info->phy_ver[1],
@@ -5610,28 +5707,64 @@ static int bnxt_get_max_irq(struct pci_dev *pdev)
 	return (ctrl & PCI_MSIX_FLAGS_QSIZE) + 1;
 }
 
-void bnxt_get_max_rings(struct bnxt *bp, int *max_rx, int *max_tx)
+static void _bnxt_get_max_rings(struct bnxt *bp, int *max_rx, int *max_tx,
+				int *max_cp)
 {
-	int max_rings = 0;
+	int max_ring_grps = 0;
 
-	if (BNXT_PF(bp)) {
-		*max_tx = bp->pf.max_pf_tx_rings;
-		*max_rx = bp->pf.max_pf_rx_rings;
-		max_rings = min_t(int, bp->pf.max_irqs, bp->pf.max_cp_rings);
-		max_rings = min_t(int, max_rings, bp->pf.max_stat_ctxs);
-	} else {
 #ifdef CONFIG_BNXT_SRIOV
+	if (!BNXT_PF(bp)) {
 		*max_tx = bp->vf.max_tx_rings;
 		*max_rx = bp->vf.max_rx_rings;
-		max_rings = min_t(int, bp->vf.max_irqs, bp->vf.max_cp_rings);
-		max_rings = min_t(int, max_rings, bp->vf.max_stat_ctxs);
+		*max_cp = min_t(int, bp->vf.max_irqs, bp->vf.max_cp_rings);
+		*max_cp = min_t(int, *max_cp, bp->vf.max_stat_ctxs);
+		max_ring_grps = bp->vf.max_hw_ring_grps;
+	} else
 #endif
+	{
+		*max_tx = bp->pf.max_tx_rings;
+		*max_rx = bp->pf.max_rx_rings;
+		*max_cp = min_t(int, bp->pf.max_irqs, bp->pf.max_cp_rings);
+		*max_cp = min_t(int, *max_cp, bp->pf.max_stat_ctxs);
+		max_ring_grps = bp->pf.max_hw_ring_grps;
 	}
+
 	if (bp->flags & BNXT_FLAG_AGG_RINGS)
 		*max_rx >>= 1;
+	*max_rx = min_t(int, *max_rx, max_ring_grps);
+}
+
+int bnxt_get_max_rings(struct bnxt *bp, int *max_rx, int *max_tx, bool shared)
+{
+	int rx, tx, cp;
 
-	*max_rx = min_t(int, *max_rx, max_rings);
-	*max_tx = min_t(int, *max_tx, max_rings);
+	_bnxt_get_max_rings(bp, &rx, &tx, &cp);
+	if (!rx || !tx || !cp)
+		return -ENOMEM;
+
+	*max_rx = rx;
+	*max_tx = tx;
+	return bnxt_trim_rings(bp, max_rx, max_tx, cp, shared);
+}
+
+static int bnxt_set_dflt_rings(struct bnxt *bp)
+{
+	int dflt_rings, max_rx_rings, max_tx_rings, rc;
+	bool sh = true;
+
+	if (sh)
+		bp->flags |= BNXT_FLAG_SHARED_RINGS;
+	dflt_rings = netif_get_num_default_rss_queues();
+	rc = bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, sh);
+	if (rc)
+		return rc;
+	bp->rx_nr_rings = min_t(int, dflt_rings, max_rx_rings);
+	bp->tx_nr_rings_per_tc = min_t(int, dflt_rings, max_tx_rings);
+	bp->tx_nr_rings = bp->tx_nr_rings_per_tc;
+	bp->cp_nr_rings = sh ? max_t(int, bp->tx_nr_rings, bp->rx_nr_rings) :
+			       bp->tx_nr_rings + bp->rx_nr_rings;
+	bp->num_stat_ctxs = bp->cp_nr_rings;
+	return rc;
 }
 
 static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
@@ -5639,7 +5772,7 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	static int version_printed;
 	struct net_device *dev;
 	struct bnxt *bp;
-	int rc, max_rx_rings, max_tx_rings, max_irqs, dflt_rings;
+	int rc, max_irqs;
 
 	if (version_printed++ == 0)
 		pr_info("%s", version);
@@ -5654,11 +5787,8 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (bnxt_vf_pciid(ent->driver_data))
 		bp->flags |= BNXT_FLAG_VF;
 
-	if (pdev->msix_cap) {
+	if (pdev->msix_cap)
 		bp->flags |= BNXT_FLAG_MSIX_CAP;
-		if (BNXT_PF(bp))
-			bp->flags |= BNXT_FLAG_RFS;
-	}
 
 	rc = bnxt_init_board(pdev, dev);
 	if (rc < 0)
@@ -5677,9 +5807,6 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			   NETIF_F_RXHASH |
 			   NETIF_F_RXCSUM | NETIF_F_LRO | NETIF_F_GRO;
 
-	if (bp->flags & BNXT_FLAG_RFS)
-		dev->hw_features |= NETIF_F_NTUPLE;
-
 	dev->hw_enc_features =
 			NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_SG |
 			NETIF_F_TSO | NETIF_F_TSO6 |
@@ -5724,19 +5851,21 @@ static int bnxt_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	bnxt_set_tpa_flags(bp);
 	bnxt_set_ring_params(bp);
-	dflt_rings = netif_get_num_default_rss_queues();
 	if (BNXT_PF(bp))
 		bp->pf.max_irqs = max_irqs;
 #if defined(CONFIG_BNXT_SRIOV)
 	else
 		bp->vf.max_irqs = max_irqs;
 #endif
-	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings);
-	bp->rx_nr_rings = min_t(int, dflt_rings, max_rx_rings);
-	bp->tx_nr_rings_per_tc = min_t(int, dflt_rings, max_tx_rings);
-	bp->tx_nr_rings = bp->tx_nr_rings_per_tc;
-	bp->cp_nr_rings = max_t(int, bp->rx_nr_rings, bp->tx_nr_rings);
-	bp->num_stat_ctxs = bp->cp_nr_rings;
+	bnxt_set_dflt_rings(bp);
+
+	if (BNXT_PF(bp)) {
+		dev->hw_features |= NETIF_F_NTUPLE;
+		if (bnxt_rfs_capable(bp)) {
+			bp->flags |= BNXT_FLAG_RFS;
+			dev->features |= NETIF_F_NTUPLE;
+		}
+	}
 
 	if (dev->hw_features & NETIF_F_HW_VLAN_CTAG_RX)
 		bp->flags |= BNXT_FLAG_STRIP_VLAN;
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt.h b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
index f199f4cc8ffe..2be51b332652 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt.h
@@ -11,11 +11,11 @@
 #define BNXT_H
 
 #define DRV_MODULE_NAME		"bnxt_en"
-#define DRV_MODULE_VERSION	"0.1.24"
+#define DRV_MODULE_VERSION	"1.0.0"
 
-#define DRV_VER_MAJ	0
-#define DRV_VER_MIN	1
-#define DRV_VER_UPD	24
+#define DRV_VER_MAJ	1
+#define DRV_VER_MIN	0
+#define DRV_VER_UPD	0
 
 struct tx_bd {
 	__le32 tx_bd_len_flags_type;
@@ -411,8 +411,8 @@ struct rx_tpa_end_cmp_ext {
 
 #define BNXT_NUM_TESTS(bp)	0
 
-#define BNXT_DEFAULT_RX_RING_SIZE	1023
-#define BNXT_DEFAULT_TX_RING_SIZE	512
+#define BNXT_DEFAULT_RX_RING_SIZE	511
+#define BNXT_DEFAULT_TX_RING_SIZE	511
 
 #define MAX_TPA		64
 
@@ -523,11 +523,18 @@ struct bnxt_ring_struct {
 
 struct tx_push_bd {
 	__le32			doorbell;
-	struct tx_bd		txbd1;
+	__le32			tx_bd_len_flags_type;
+	u32			tx_bd_opaque;
 	struct tx_bd_ext	txbd2;
 };
 
+struct tx_push_buffer {
+	struct tx_push_bd	push_bd;
+	u32			data[25];
+};
+
 struct bnxt_tx_ring_info {
+	struct bnxt_napi	*bnapi;
 	u16			tx_prod;
 	u16			tx_cons;
 	void __iomem		*tx_doorbell;
@@ -537,8 +544,9 @@ struct bnxt_tx_ring_info {
 
 	dma_addr_t		tx_desc_mapping[MAX_TX_PAGES];
 
-	struct tx_push_bd	*tx_push;
+	struct tx_push_buffer	*tx_push;
 	dma_addr_t		tx_push_mapping;
+	__le64			data_mapping;
 
 #define BNXT_DEV_STATE_CLOSING	0x1
 	u32			dev_state;
@@ -558,6 +566,7 @@ struct bnxt_tpa_info {
 };
 
 struct bnxt_rx_ring_info {
+	struct bnxt_napi	*bnapi;
 	u16			rx_prod;
 	u16			rx_agg_prod;
 	u16			rx_sw_agg_prod;
@@ -604,8 +613,8 @@ struct bnxt_napi {
 
 	int			index;
 	struct bnxt_cp_ring_info	cp_ring;
-	struct bnxt_rx_ring_info	rx_ring;
-	struct bnxt_tx_ring_info	tx_ring;
+	struct bnxt_rx_ring_info	*rx_ring;
+	struct bnxt_tx_ring_info	*tx_ring;
 
 #ifdef CONFIG_NET_RX_BUSY_POLL
 	atomic_t		poll_state;
@@ -695,6 +704,7 @@ struct bnxt_vf_info {
 	u16	max_cp_rings;
 	u16	max_tx_rings;
 	u16	max_rx_rings;
+	u16	max_hw_ring_grps;
 	u16	max_l2_ctxs;
 	u16	max_irqs;
 	u16	max_vnics;
@@ -722,9 +732,8 @@ struct bnxt_pf_info {
 	u16	max_rsscos_ctxs;
 	u16	max_cp_rings;
 	u16	max_tx_rings; /* HW assigned max tx rings for this PF */
-	u16	max_pf_tx_rings; /* runtime max tx rings owned by PF */
 	u16	max_rx_rings; /* HW assigned max rx rings for this PF */
-	u16	max_pf_rx_rings; /* runtime max rx rings owned by PF */
+	u16	max_hw_ring_grps;
 	u16	max_irqs;
 	u16	max_l2_ctxs;
 	u16	max_vnics;
@@ -875,6 +884,8 @@ struct bnxt {
 	#define BNXT_FLAG_USING_MSIX	0x40
 	#define BNXT_FLAG_MSIX_CAP	0x80
 	#define BNXT_FLAG_RFS		0x100
+	#define BNXT_FLAG_SHARED_RINGS	0x200
+
 	#define BNXT_FLAG_ALL_CONFIG_FEATS (BNXT_FLAG_TPA |		\
 					    BNXT_FLAG_RFS |		\
 					    BNXT_FLAG_STRIP_VLAN)
@@ -884,6 +895,9 @@ struct bnxt {
 
 	struct bnxt_napi	**bnapi;
 
+	struct bnxt_rx_ring_info	*rx_ring;
+	struct bnxt_tx_ring_info	*tx_ring;
+
 	u32			rx_buf_size;
 	u32			rx_buf_use_size;	/* useable size */
 	u32			rx_ring_size;
@@ -913,6 +927,8 @@ struct bnxt {
 	int			cp_nr_rings;
 
 	int			num_stat_ctxs;
+
+	/* grp_info indexed by completion ring index */
 	struct bnxt_ring_grp_info	*grp_info;
 	struct bnxt_vnic_info	*vnic_info;
 	int			nr_vnics;
@@ -1084,9 +1100,10 @@ void bnxt_hwrm_cmd_hdr_init(struct bnxt *, void *, u16, u16, u16);
 int _hwrm_send_message(struct bnxt *, void *, u32, int);
 int hwrm_send_message(struct bnxt *, void *, u32, int);
 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);
 int bnxt_open_nic(struct bnxt *, bool, bool);
 int bnxt_close_nic(struct bnxt *, bool, bool);
-void bnxt_get_max_rings(struct bnxt *, int *, int *);
+int bnxt_get_max_rings(struct bnxt *, int *, int *, bool);
 #endif
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
index 45bd628eaf3a..3238817dfd5f 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_ethtool.c
@@ -211,7 +211,10 @@ static void bnxt_get_channels(struct net_device *dev,
 	struct bnxt *bp = netdev_priv(dev);
 	int max_rx_rings, max_tx_rings, tcs;
 
-	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings);
+	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, true);
+	channel->max_combined = max_rx_rings;
+
+	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, false);
 	tcs = netdev_get_num_tc(dev);
 	if (tcs > 1)
 		max_tx_rings /= tcs;
@@ -219,9 +222,12 @@ static void bnxt_get_channels(struct net_device *dev,
 	channel->max_rx = max_rx_rings;
 	channel->max_tx = max_tx_rings;
 	channel->max_other = 0;
-	channel->max_combined = 0;
-	channel->rx_count = bp->rx_nr_rings;
-	channel->tx_count = bp->tx_nr_rings_per_tc;
+	if (bp->flags & BNXT_FLAG_SHARED_RINGS) {
+		channel->combined_count = bp->rx_nr_rings;
+	} else {
+		channel->rx_count = bp->rx_nr_rings;
+		channel->tx_count = bp->tx_nr_rings_per_tc;
+	}
 }
 
 static int bnxt_set_channels(struct net_device *dev,
@@ -230,19 +236,35 @@ static int bnxt_set_channels(struct net_device *dev,
 	struct bnxt *bp = netdev_priv(dev);
 	int max_rx_rings, max_tx_rings, tcs;
 	u32 rc = 0;
+	bool sh = false;
+
+	if (channel->other_count)
+		return -EINVAL;
+
+	if (!channel->combined_count &&
+	    (!channel->rx_count || !channel->tx_count))
+		return -EINVAL;
 
-	if (channel->other_count || channel->combined_count ||
-	    !channel->rx_count || !channel->tx_count)
+	if (channel->combined_count &&
+	    (channel->rx_count || channel->tx_count))
 		return -EINVAL;
 
-	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings);
+	if (channel->combined_count)
+		sh = true;
+
+	bnxt_get_max_rings(bp, &max_rx_rings, &max_tx_rings, sh);
+
 	tcs = netdev_get_num_tc(dev);
 	if (tcs > 1)
 		max_tx_rings /= tcs;
 
-	if (channel->rx_count > max_rx_rings ||
-	    channel->tx_count > max_tx_rings)
-		return -EINVAL;
+	if (sh && (channel->combined_count > max_rx_rings ||
+		   channel->combined_count > max_tx_rings))
+		return -ENOMEM;
+
+	if (!sh && (channel->rx_count > max_rx_rings ||
+		    channel->tx_count > max_tx_rings))
+		return -ENOMEM;
 
 	if (netif_running(dev)) {
 		if (BNXT_PF(bp)) {
@@ -258,14 +280,27 @@ static int bnxt_set_channels(struct net_device *dev,
 		}
 	}
 
-	bp->rx_nr_rings = channel->rx_count;
-	bp->tx_nr_rings_per_tc = channel->tx_count;
+	if (sh) {
+		bp->flags |= BNXT_FLAG_SHARED_RINGS;
+		bp->rx_nr_rings = channel->combined_count;
+		bp->tx_nr_rings_per_tc = channel->combined_count;
+	} else {
+		bp->flags &= ~BNXT_FLAG_SHARED_RINGS;
+		bp->rx_nr_rings = channel->rx_count;
+		bp->tx_nr_rings_per_tc = channel->tx_count;
+	}
+
 	bp->tx_nr_rings = bp->tx_nr_rings_per_tc;
 	if (tcs > 1)
 		bp->tx_nr_rings = bp->tx_nr_rings_per_tc * tcs;
-	bp->cp_nr_rings = max_t(int, bp->tx_nr_rings, bp->rx_nr_rings);
+
+	bp->cp_nr_rings = sh ? max_t(int, bp->tx_nr_rings, bp->rx_nr_rings) :
+			       bp->tx_nr_rings + bp->rx_nr_rings;
+
 	bp->num_stat_ctxs = bp->cp_nr_rings;
 
+	/* After changing number of rx channels, update NTUPLE feature. */
+	netdev_update_features(dev);
 	if (netif_running(dev)) {
 		rc = bnxt_open_nic(bp, true, false);
 		if ((!rc) && BNXT_PF(bp)) {
@@ -451,15 +486,8 @@ static u32 bnxt_fw_to_ethtool_support_spds(struct bnxt_link_info *link_info)
 		speed_mask |= SUPPORTED_2500baseX_Full;
 	if (fw_speeds & BNXT_LINK_SPEED_MSK_10GB)
 		speed_mask |= SUPPORTED_10000baseT_Full;
-	/* TODO: support 25GB, 50GB with different cable type */
-	if (fw_speeds & BNXT_LINK_SPEED_MSK_20GB)
-		speed_mask |= SUPPORTED_20000baseMLD2_Full |
-			SUPPORTED_20000baseKR2_Full;
 	if (fw_speeds & BNXT_LINK_SPEED_MSK_40GB)
-		speed_mask |= SUPPORTED_40000baseKR4_Full |
-			SUPPORTED_40000baseCR4_Full |
-			SUPPORTED_40000baseSR4_Full |
-			SUPPORTED_40000baseLR4_Full;
+		speed_mask |= SUPPORTED_40000baseCR4_Full;
 
 	return speed_mask;
 }
@@ -479,15 +507,8 @@ static u32 bnxt_fw_to_ethtool_advertised_spds(struct bnxt_link_info *link_info)
 		speed_mask |= ADVERTISED_2500baseX_Full;
 	if (fw_speeds & BNXT_LINK_SPEED_MSK_10GB)
 		speed_mask |= ADVERTISED_10000baseT_Full;
-	/* TODO: how to advertise 20, 25, 40, 50GB with different cable type ?*/
-	if (fw_speeds & BNXT_LINK_SPEED_MSK_20GB)
-		speed_mask |= ADVERTISED_20000baseMLD2_Full |
-			      ADVERTISED_20000baseKR2_Full;
 	if (fw_speeds & BNXT_LINK_SPEED_MSK_40GB)
-		speed_mask |= ADVERTISED_40000baseKR4_Full |
-			      ADVERTISED_40000baseCR4_Full |
-			      ADVERTISED_40000baseSR4_Full |
-			      ADVERTISED_40000baseLR4_Full;
+		speed_mask |= ADVERTISED_40000baseCR4_Full;
 	return speed_mask;
 }
 
@@ -522,11 +543,12 @@ static int bnxt_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	u16 ethtool_speed;
 
 	cmd->supported = bnxt_fw_to_ethtool_support_spds(link_info);
+	cmd->supported |= SUPPORTED_Pause | SUPPORTED_Asym_Pause;
 
 	if (link_info->auto_link_speeds)
 		cmd->supported |= SUPPORTED_Autoneg;
 
-	if (BNXT_AUTO_MODE(link_info->auto_mode)) {
+	if (link_info->autoneg) {
 		cmd->advertising =
 			bnxt_fw_to_ethtool_advertised_spds(link_info);
 		cmd->advertising |= ADVERTISED_Autoneg;
@@ -535,28 +557,16 @@ static int bnxt_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		cmd->autoneg = AUTONEG_DISABLE;
 		cmd->advertising = 0;
 	}
-	if (link_info->auto_pause_setting & BNXT_LINK_PAUSE_BOTH) {
+	if (link_info->autoneg & BNXT_AUTONEG_FLOW_CTRL) {
 		if ((link_info->auto_pause_setting & BNXT_LINK_PAUSE_BOTH) ==
 		    BNXT_LINK_PAUSE_BOTH) {
 			cmd->advertising |= ADVERTISED_Pause;
-			cmd->supported |= SUPPORTED_Pause;
 		} else {
 			cmd->advertising |= ADVERTISED_Asym_Pause;
-			cmd->supported |= SUPPORTED_Asym_Pause;
 			if (link_info->auto_pause_setting &
 			    BNXT_LINK_PAUSE_RX)
 				cmd->advertising |= ADVERTISED_Pause;
 		}
-	} else if (link_info->force_pause_setting & BNXT_LINK_PAUSE_BOTH) {
-		if ((link_info->force_pause_setting & BNXT_LINK_PAUSE_BOTH) ==
-		    BNXT_LINK_PAUSE_BOTH) {
-			cmd->supported |= SUPPORTED_Pause;
-		} else {
-			cmd->supported |= SUPPORTED_Asym_Pause;
-			if (link_info->force_pause_setting &
-			    BNXT_LINK_PAUSE_RX)
-				cmd->supported |= SUPPORTED_Pause;
-		}
 	}
 
 	cmd->port = PORT_NONE;
@@ -635,6 +645,9 @@ static u16 bnxt_get_fw_auto_link_speeds(u32 advertising)
 	if (advertising & ADVERTISED_10000baseT_Full)
 		fw_speed_mask |= BNXT_LINK_SPEED_MSK_10GB;
 
+	if (advertising & ADVERTISED_40000baseCR4_Full)
+		fw_speed_mask |= BNXT_LINK_SPEED_MSK_40GB;
+
 	return fw_speed_mask;
 }
 
@@ -694,7 +707,7 @@ static int bnxt_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		speed = ethtool_cmd_speed(cmd);
 		link_info->req_link_speed = bnxt_get_fw_speed(dev, speed);
 		link_info->req_duplex = BNXT_LINK_DUPLEX_FULL;
-		link_info->autoneg &= ~BNXT_AUTONEG_SPEED;
+		link_info->autoneg = 0;
 		link_info->advertising = 0;
 	}
 
@@ -713,8 +726,7 @@ static void bnxt_get_pauseparam(struct net_device *dev,
 
 	if (BNXT_VF(bp))
 		return;
-	epause->autoneg = !!(link_info->auto_pause_setting &
-			     BNXT_LINK_PAUSE_BOTH);
+	epause->autoneg = !!(link_info->autoneg & BNXT_AUTONEG_FLOW_CTRL);
 	epause->rx_pause = ((link_info->pause & BNXT_LINK_PAUSE_RX) != 0);
 	epause->tx_pause = ((link_info->pause & BNXT_LINK_PAUSE_TX) != 0);
 }
@@ -730,6 +742,9 @@ static int bnxt_set_pauseparam(struct net_device *dev,
 		return rc;
 
 	if (epause->autoneg) {
+		if (!(link_info->autoneg & BNXT_AUTONEG_SPEED))
+			return -EINVAL;
+
 		link_info->autoneg |= BNXT_AUTONEG_FLOW_CTRL;
 		link_info->req_flow_ctrl |= BNXT_LINK_PAUSE_BOTH;
 	} else {
@@ -802,6 +817,45 @@ static int bnxt_flash_nvram(struct net_device *dev,
 	return rc;
 }
 
+static int bnxt_firmware_reset(struct net_device *dev,
+			       u16 dir_type)
+{
+	struct bnxt *bp = netdev_priv(dev);
+	struct hwrm_fw_reset_input req = {0};
+
+	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FW_RESET, -1, -1);
+
+	/* TODO: Support ASAP ChiMP self-reset (e.g. upon PF driver unload) */
+	/* TODO: Address self-reset of APE/KONG/BONO/TANG or ungraceful reset */
+	/*       (e.g. when firmware isn't already running) */
+	switch (dir_type) {
+	case BNX_DIR_TYPE_CHIMP_PATCH:
+	case BNX_DIR_TYPE_BOOTCODE:
+	case BNX_DIR_TYPE_BOOTCODE_2:
+		req.embedded_proc_type = FW_RESET_REQ_EMBEDDED_PROC_TYPE_BOOT;
+		/* Self-reset ChiMP upon next PCIe reset: */
+		req.selfrst_status = FW_RESET_REQ_SELFRST_STATUS_SELFRSTPCIERST;
+		break;
+	case BNX_DIR_TYPE_APE_FW:
+	case BNX_DIR_TYPE_APE_PATCH:
+		req.embedded_proc_type = FW_RESET_REQ_EMBEDDED_PROC_TYPE_MGMT;
+		break;
+	case BNX_DIR_TYPE_KONG_FW:
+	case BNX_DIR_TYPE_KONG_PATCH:
+		req.embedded_proc_type =
+			FW_RESET_REQ_EMBEDDED_PROC_TYPE_NETCTRL;
+		break;
+	case BNX_DIR_TYPE_BONO_FW:
+	case BNX_DIR_TYPE_BONO_PATCH:
+		req.embedded_proc_type = FW_RESET_REQ_EMBEDDED_PROC_TYPE_ROCE;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
+}
+
 static int bnxt_flash_firmware(struct net_device *dev,
 			       u16 dir_type,
 			       const u8 *fw_data,
@@ -818,6 +872,9 @@ static int bnxt_flash_firmware(struct net_device *dev,
 	case BNX_DIR_TYPE_BOOTCODE_2:
 		code_type = CODE_BOOT;
 		break;
+	case BNX_DIR_TYPE_APE_FW:
+		code_type = CODE_MCTP_PASSTHRU;
+		break;
 	default:
 		netdev_err(dev, "Unsupported directory entry type: %u\n",
 			   dir_type);
@@ -856,10 +913,9 @@ static int bnxt_flash_firmware(struct net_device *dev,
 	/* 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 */
-		/* TODO: Notify processor it needs to reset itself
-		 */
-	}
+	if (rc == 0)	/* Firmware update successful */
+		rc = bnxt_firmware_reset(dev, dir_type);
+
 	return rc;
 }
 
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h b/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
index 70fc8253c07f..4badbedcb421 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_hsi.h
@@ -103,19 +103,22 @@ struct hwrm_async_event_cmpl {
 	#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_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       (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_HWRM_ERROR	   (0xffUL << 0)
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_V			    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_OPAQUE_MASK		    0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_OPAQUE_SFT		    1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 };
 
@@ -132,9 +135,16 @@ struct hwrm_async_event_cmpl_link_status_change {
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_OPAQUE_SFT 1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
-	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_LINK_UP 0x1UL
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_LINK_CHANGE 0x1UL
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_LINK_CHANGE_DOWN (0x0UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_LINK_CHANGE_UP (0x1UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_PORT_MASK 0xeUL
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_PORT_SFT 1
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_PORT_ID_MASK 0xffff0UL
+	#define HWRM_ASYNC_EVENT_CMPL_LINK_STATUS_CHANGE_EVENT_DATA1_PORT_ID_SFT 4
 };
 
 /* HWRM Asynchronous Event Completion Record for link MTU change (16 bytes) */
@@ -150,7 +160,8 @@ struct hwrm_async_event_cmpl_link_mtu_change {
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_OPAQUE_MASK  0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_OPAQUE_SFT   1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_EVENT_DATA1_NEW_MTU_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_MTU_CHANGE_EVENT_DATA1_NEW_MTU_SFT 0
@@ -169,7 +180,8 @@ struct hwrm_async_event_cmpl_link_speed_change {
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_OPAQUE_SFT 1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_FORCE 0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_LINK_SPEED_CHANGE_EVENT_DATA1_NEW_LINK_SPEED_100MBPS_MASK 0xfffeUL
@@ -200,7 +212,8 @@ struct hwrm_async_event_cmpl_dcb_config_change {
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_OPAQUE_SFT 1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_EVENT_DATA1_PORT_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_DCB_CONFIG_CHANGE_EVENT_DATA1_PORT_ID_SFT 0
@@ -219,7 +232,8 @@ struct hwrm_async_event_cmpl_port_conn_not_allowed {
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_V      0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_OPAQUE_SFT 1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_EVENT_DATA1_PORT_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_PORT_CONN_NOT_ALLOWED_EVENT_DATA1_PORT_ID_SFT 0
@@ -238,7 +252,8 @@ struct hwrm_async_event_cmpl_func_drvr_unload {
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_OPAQUE_SFT  1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_EVENT_DATA1_FUNC_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_UNLOAD_EVENT_DATA1_FUNC_ID_SFT 0
@@ -257,7 +272,8 @@ struct hwrm_async_event_cmpl_func_drvr_load {
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_V		    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_OPAQUE_MASK   0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_OPAQUE_SFT    1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_EVENT_DATA1_FUNC_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_FUNC_DRVR_LOAD_EVENT_DATA1_FUNC_ID_SFT 0
@@ -276,10 +292,13 @@ struct hwrm_async_event_cmpl_pf_drvr_unload {
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_V		    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_OPAQUE_MASK   0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_OPAQUE_SFT    1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_DATA1_FUNC_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_DATA1_FUNC_ID_SFT 0
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_DATA1_PORT_MASK 0x70000UL
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_UNLOAD_EVENT_DATA1_PORT_SFT 16
 };
 
 /* HWRM Asynchronous Event Completion Record for PF Driver load (16 bytes) */
@@ -289,16 +308,19 @@ struct hwrm_async_event_cmpl_pf_drvr_load {
 	#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)
 	__le16 event_id;
-	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_ID_PF_DRVR_LOAD (0x20UL << 0)
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_ID_PF_DRVR_LOAD (0x21UL << 0)
 	__le32 event_data2;
 	u8 opaque_v;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_V		    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_OPAQUE_MASK     0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_OPAQUE_SFT      1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_DATA1_FUNC_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_DATA1_FUNC_ID_SFT 0
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_DATA1_PORT_MASK 0x70000UL
+	#define HWRM_ASYNC_EVENT_CMPL_PF_DRVR_LOAD_EVENT_DATA1_PORT_SFT 16
 };
 
 /* HWRM Asynchronous Event Completion Record for VF FLR (16 bytes) */
@@ -314,7 +336,8 @@ struct hwrm_async_event_cmpl_vf_flr {
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_V			    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_OPAQUE_MASK	    0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_OPAQUE_SFT	    1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_EVENT_DATA1_VF_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_FLR_EVENT_DATA1_VF_ID_SFT 0
@@ -333,7 +356,8 @@ struct hwrm_async_event_cmpl_vf_mac_addr_change {
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_V	    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_OPAQUE_MASK 0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_OPAQUE_SFT 1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_EVENT_DATA1_VF_ID_MASK 0xffffUL
 	#define HWRM_ASYNC_EVENT_CMPL_VF_MAC_ADDR_CHANGE_EVENT_DATA1_VF_ID_SFT 0
@@ -357,18 +381,20 @@ struct hwrm_async_event_cmpl_hwrm_error {
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_V		    0x1UL
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_OPAQUE_MASK       0xfeUL
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_OPAQUE_SFT	    1
-	u8 unused_1[3];
+	u8 timestamp_lo;
+	__le16 timestamp_hi;
 	__le32 event_data1;
 	#define HWRM_ASYNC_EVENT_CMPL_HWRM_ERROR_EVENT_DATA1_TIMESTAMP 0x1UL
 };
 
-/* HW Resource Manager Specification 0.7.8 */
-#define HWRM_VERSION_MAJOR	0
-#define HWRM_VERSION_MINOR	7
-#define HWRM_VERSION_UPDATE	8
+/* HW Resource Manager Specification 1.0.0 */
+#define HWRM_VERSION_MAJOR	1
+#define HWRM_VERSION_MINOR	0
+#define HWRM_VERSION_UPDATE	0
 
-#define HWRM_VERSION_STR	"0.7.8"
-/* Following is the signature for HWRM message field that indicates not
+#define HWRM_VERSION_STR	"1.0.0"
+/*
+ * 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.
  */
 #define HWRM_NA_SIGNATURE	((__le32)(-1))
@@ -398,7 +424,9 @@ struct output {
 struct cmd_nums {
 	__le16 req_type;
 	#define HWRM_VER_GET					   (0x0UL)
-	#define HWRM_FUNC_DISABLE				   (0x10UL)
+	#define HWRM_FUNC_BUF_UNRGTR				   (0xeUL)
+	#define HWRM_FUNC_VF_CFG				   (0xfUL)
+	#define RESERVED1					   (0x10UL)
 	#define HWRM_FUNC_RESET				   (0x11UL)
 	#define HWRM_FUNC_GETFID				   (0x12UL)
 	#define HWRM_FUNC_VF_ALLOC				   (0x13UL)
@@ -414,10 +442,9 @@ struct cmd_nums {
 	#define HWRM_FUNC_DRV_RGTR				   (0x1dUL)
 	#define HWRM_FUNC_DRV_QVER				   (0x1eUL)
 	#define HWRM_FUNC_BUF_RGTR				   (0x1fUL)
-	#define HWRM_FUNC_VF_CFG				   (0x20UL)
 	#define HWRM_PORT_PHY_CFG				   (0x20UL)
 	#define HWRM_PORT_MAC_CFG				   (0x21UL)
-	#define HWRM_PORT_ENABLE				   (0x22UL)
+	#define RESERVED2					   (0x22UL)
 	#define HWRM_PORT_QSTATS				   (0x23UL)
 	#define HWRM_PORT_LPBK_QSTATS				   (0x24UL)
 	#define HWRM_PORT_CLR_STATS				   (0x25UL)
@@ -455,13 +482,11 @@ struct cmd_nums {
 	#define HWRM_RING_GRP_FREE				   (0x61UL)
 	#define HWRM_VNIC_RSS_COS_LB_CTX_ALLOC			   (0x70UL)
 	#define HWRM_VNIC_RSS_COS_LB_CTX_FREE			   (0x71UL)
-	#define HWRM_ARB_GRP_ALLOC				   (0x80UL)
-	#define HWRM_ARB_GRP_CFG				   (0x81UL)
 	#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 HWRM_CFA_L2_SET_BCASTMCAST_MIRRORING		   (0x94UL)
+	#define RESERVED3					   (0x94UL)
 	#define HWRM_CFA_TUNNEL_FILTER_ALLOC			   (0x95UL)
 	#define HWRM_CFA_TUNNEL_FILTER_FREE			   (0x96UL)
 	#define HWRM_CFA_ENCAP_RECORD_ALLOC			   (0x97UL)
@@ -469,6 +494,9 @@ struct cmd_nums {
 	#define HWRM_CFA_NTUPLE_FILTER_ALLOC			   (0x99UL)
 	#define HWRM_CFA_NTUPLE_FILTER_FREE			   (0x9aUL)
 	#define HWRM_CFA_NTUPLE_FILTER_CFG			   (0x9bUL)
+	#define HWRM_CFA_EM_FLOW_ALLOC				   (0x9cUL)
+	#define HWRM_CFA_EM_FLOW_FREE				   (0x9dUL)
+	#define HWRM_CFA_EM_FLOW_CFG				   (0x9eUL)
 	#define HWRM_TUNNEL_DST_PORT_QUERY			   (0xa0UL)
 	#define HWRM_TUNNEL_DST_PORT_ALLOC			   (0xa1UL)
 	#define HWRM_TUNNEL_DST_PORT_FREE			   (0xa2UL)
@@ -483,8 +511,6 @@ struct cmd_nums {
 	#define HWRM_FWD_RESP					   (0xd2UL)
 	#define HWRM_FWD_ASYNC_EVENT_CMPL			   (0xd3UL)
 	#define HWRM_TEMP_MONITOR_QUERY			   (0xe0UL)
-	#define HWRM_MGMT_L2_FILTER_ALLOC			   (0x100UL)
-	#define HWRM_MGMT_L2_FILTER_FREE			   (0x101UL)
 	#define HWRM_DBG_READ_DIRECT				   (0xff10UL)
 	#define HWRM_DBG_READ_INDIRECT				   (0xff11UL)
 	#define HWRM_DBG_WRITE_DIRECT				   (0xff12UL)
@@ -505,7 +531,6 @@ struct cmd_nums {
 	__le16 unused_0[3];
 };
 
-/* Return Codes (8 bytes) */
 struct ret_codes {
 	__le16 error_code;
 	#define HWRM_ERR_CODE_SUCCESS				   (0x0UL)
@@ -529,7 +554,7 @@ struct hwrm_err_output {
 	__le16 resp_len;
 	__le32 opaque_0;
 	__le16 opaque_1;
-	u8 opaque_2;
+	u8 cmd_err;
 	u8 valid;
 };
 
@@ -686,65 +711,38 @@ struct hwrm_ver_get_output {
 	u8 hwrm_fw_min;
 	u8 hwrm_fw_bld;
 	u8 hwrm_fw_rsvd;
-	u8 ape_fw_maj;
-	u8 ape_fw_min;
-	u8 ape_fw_bld;
-	u8 ape_fw_rsvd;
-	u8 kong_fw_maj;
-	u8 kong_fw_min;
-	u8 kong_fw_bld;
-	u8 kong_fw_rsvd;
-	u8 tang_fw_maj;
-	u8 tang_fw_min;
-	u8 tang_fw_bld;
-	u8 tang_fw_rsvd;
-	u8 bono_fw_maj;
-	u8 bono_fw_min;
-	u8 bono_fw_bld;
-	u8 bono_fw_rsvd;
+	u8 mgmt_fw_maj;
+	u8 mgmt_fw_min;
+	u8 mgmt_fw_bld;
+	u8 mgmt_fw_rsvd;
+	u8 netctrl_fw_maj;
+	u8 netctrl_fw_min;
+	u8 netctrl_fw_bld;
+	u8 netctrl_fw_rsvd;
+	__le32 reserved1;
+	u8 roce_fw_maj;
+	u8 roce_fw_min;
+	u8 roce_fw_bld;
+	u8 roce_fw_rsvd;
 	char hwrm_fw_name[16];
-	char ape_fw_name[16];
-	char kong_fw_name[16];
-	char tang_fw_name[16];
-	char bono_fw_name[16];
+	char mgmt_fw_name[16];
+	char netctrl_fw_name[16];
+	__le32 reserved2[4];
+	char roce_fw_name[16];
 	__le16 chip_num;
 	u8 chip_rev;
 	u8 chip_metal;
 	u8 chip_bond_id;
-	u8 unused_0;
+	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)
 	__le16 max_req_win_len;
 	__le16 max_resp_len;
 	__le16 def_req_timeout;
+	u8 unused_0;
 	u8 unused_1;
 	u8 unused_2;
-	u8 unused_3;
-	u8 valid;
-};
-
-/* hwrm_func_disable */
-/* Input (24 bytes) */
-struct hwrm_func_disable_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 enables;
-	#define FUNC_DISABLE_REQ_ENABLES_VF_ID_VALID		    0x1UL
-	__le16 vf_id;
-	__le16 unused_0;
-};
-
-/* Output (16 bytes) */
-struct hwrm_func_disable_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;
 };
 
@@ -759,7 +757,12 @@ struct hwrm_func_reset_input {
 	__le32 enables;
 	#define FUNC_RESET_REQ_ENABLES_VF_ID_VALID		    0x1UL
 	__le16 vf_id;
-	__le16 unused_0;
+	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)
+	u8 unused_0;
 };
 
 /* Output (16 bytes) */
@@ -861,7 +864,7 @@ struct hwrm_func_vf_free_output {
 };
 
 /* hwrm_func_vf_cfg */
-/* Input (24 bytes) */
+/* Input (32 bytes) */
 struct hwrm_func_vf_cfg_input {
 	__le16 req_type;
 	__le16 cmpl_ring;
@@ -871,8 +874,11 @@ struct hwrm_func_vf_cfg_input {
 	__le32 enables;
 	#define FUNC_VF_CFG_REQ_ENABLES_MTU			    0x1UL
 	#define FUNC_VF_CFG_REQ_ENABLES_GUEST_VLAN		    0x2UL
+	#define FUNC_VF_CFG_REQ_ENABLES_ASYNC_EVENT_CR		    0x4UL
 	__le16 mtu;
 	__le16 guest_vlan;
+	__le16 async_event_cr;
+	__le16 unused_0[3];
 };
 
 /* Output (16 bytes) */
@@ -944,7 +950,7 @@ struct hwrm_func_cfg_input {
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le16 vf_id;
+	__le16 fid;
 	u8 unused_0;
 	u8 unused_1;
 	__le32 flags;
@@ -1000,10 +1006,6 @@ struct hwrm_func_cfg_input {
 	#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)
 	u8 allowed_vlan_pris;
-	#define FUNC_CFG_REQ_ALLOWED_VLAN_PRIS_NOCHECK		   (0x0UL << 0)
-	#define FUNC_CFG_REQ_ALLOWED_VLAN_PRIS_VALIDATE_VLAN      (0x1UL << 0)
-	#define FUNC_CFG_REQ_ALLOWED_VLAN_PRIS_INSERT_IF_VLANDNE  (0x2UL << 0)
-	#define FUNC_CFG_REQ_ALLOWED_VLAN_PRIS_INSERT_OR_OVERRIDE_VLAN (0x3UL << 0)
 	u8 evb_mode;
 	#define FUNC_CFG_REQ_EVB_MODE_NO_EVB			   (0x0UL << 0)
 	#define FUNC_CFG_REQ_EVB_MODE_VEB			   (0x1UL << 0)
@@ -1166,6 +1168,15 @@ 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_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)
 	u8 ver_maj;
 	u8 ver_min;
 	u8 ver_upd;
@@ -1276,9 +1287,7 @@ struct hwrm_func_drv_qver_input {
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le32 enables;
-	#define FUNC_DRV_QVER_REQ_ENABLES_OS_TYPE_VALID	    0x1UL
-	#define FUNC_DRV_QVER_REQ_ENABLES_VER_VALID		    0x2UL
+	__le32 reserved;
 	__le16 fid;
 	__le16 unused_0;
 };
@@ -1290,6 +1299,15 @@ 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_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)
 	u8 ver_maj;
 	u8 ver_min;
 	u8 ver_upd;
@@ -1498,9 +1516,7 @@ struct hwrm_port_phy_qcfg_output {
 	u8 force_pause;
 	#define PORT_PHY_QCFG_RESP_FORCE_PAUSE_TX		    0x1UL
 	#define PORT_PHY_QCFG_RESP_FORCE_PAUSE_RX		    0x2UL
-	u8 duplex_setting;
-	#define PORT_PHY_QCFG_RESP_DUPLEX_SETTING_HALF		   (0x0UL << 0)
-	#define PORT_PHY_QCFG_RESP_DUPLEX_SETTING_FULL		   (0x1UL << 0)
+	u8 reserved1;
 	__le32 preemphasis;
 	u8 phy_maj;
 	u8 phy_min;
@@ -1601,33 +1617,6 @@ struct hwrm_port_mac_cfg_output {
 	u8 valid;
 };
 
-/* hwrm_port_enable */
-/* Input (24 bytes) */
-struct hwrm_port_enable_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 flags;
-	#define PORT_ENABLE_REQ_FLAGS_FORWARD_TRAFFIC		    0x1UL
-	__le16 port_id;
-	__le16 unused_0;
-};
-
-/* Output (16 bytes) */
-struct hwrm_port_enable_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_port_qstats */
 /* Input (40 bytes) */
 struct hwrm_port_qstats_input {
@@ -1651,10 +1640,11 @@ struct hwrm_port_qstats_output {
 	__le16 req_type;
 	__le16 seq_id;
 	__le16 resp_len;
-	__le32 unused_0;
+	__le16 tx_stat_size;
+	__le16 rx_stat_size;
+	u8 unused_0;
 	u8 unused_1;
 	u8 unused_2;
-	u8 unused_3;
 	u8 valid;
 };
 
@@ -1668,7 +1658,7 @@ struct hwrm_port_lpbk_qstats_input {
 	__le64 resp_addr;
 };
 
-/* Output (64 bytes) */
+/* Output (96 bytes) */
 struct hwrm_port_lpbk_qstats_output {
 	__le16 error_code;
 	__le16 req_type;
@@ -1680,6 +1670,10 @@ struct hwrm_port_lpbk_qstats_output {
 	__le64 lpbk_ucast_bytes;
 	__le64 lpbk_mcast_bytes;
 	__le64 lpbk_bcast_bytes;
+	__le64 tx_stat_discard;
+	__le64 tx_stat_error;
+	__le64 rx_stat_discard;
+	__le64 rx_stat_error;
 	__le32 unused_0;
 	u8 unused_1;
 	u8 unused_2;
@@ -1884,12 +1878,11 @@ struct hwrm_queue_buffers_cfg_input {
 	__le32 enables;
 	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_RESERVED		    0x1UL
 	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_SHARED		    0x2UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_GROUP		    0x4UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_XOFF		    0x8UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_XON		    0x10UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_FULL		    0x20UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_NOTFULL		    0x40UL
-	#define QUEUE_BUFFERS_CFG_REQ_ENABLES_MAX		    0x80UL
+	#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;
@@ -1921,15 +1914,15 @@ struct hwrm_queue_pfcenable_cfg_input {
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le32 enables;
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI0_PFC_ENABLED   0x1UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI1_PFC_ENABLED   0x2UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI2_PFC_ENABLED   0x4UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI3_PFC_ENABLED   0x8UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI4_PFC_ENABLED   0x10UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI5_PFC_ENABLED   0x20UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI6_PFC_ENABLED   0x40UL
-	#define QUEUE_PFCENABLE_CFG_REQ_ENABLES_PRI7_PFC_ENABLED   0x80UL
+	__le32 flags;
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI0_PFC_ENABLED     0x1UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI1_PFC_ENABLED     0x2UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI2_PFC_ENABLED     0x4UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI3_PFC_ENABLED     0x8UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI4_PFC_ENABLED     0x10UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI5_PFC_ENABLED     0x20UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI6_PFC_ENABLED     0x40UL
+	#define QUEUE_PFCENABLE_CFG_REQ_FLAGS_PRI7_PFC_ENABLED     0x80UL
 	__le16 port_id;
 	__le16 unused_0;
 };
@@ -1962,14 +1955,14 @@ struct hwrm_queue_pri2cos_cfg_input {
 	#define QUEUE_PRI2COS_CFG_REQ_FLAGS_IVLAN		    0x2UL
 	__le32 enables;
 	u8 port_id;
-	u8 pri0_cos;
-	u8 pri1_cos;
-	u8 pri2_cos;
-	u8 pri3_cos;
-	u8 pri4_cos;
-	u8 pri5_cos;
-	u8 pri6_cos;
-	u8 pri7_cos;
+	u8 pri0_cos_queue_id;
+	u8 pri1_cos_queue_id;
+	u8 pri2_cos_queue_id;
+	u8 pri3_cos_queue_id;
+	u8 pri4_cos_queue_id;
+	u8 pri5_cos_queue_id;
+	u8 pri6_cos_queue_id;
+	u8 pri7_cos_queue_id;
 	u8 unused_0[7];
 };
 
@@ -2164,6 +2157,7 @@ struct hwrm_vnic_cfg_input {
 	__le32 flags;
 	#define VNIC_CFG_REQ_FLAGS_DEFAULT			    0x1UL
 	#define VNIC_CFG_REQ_FLAGS_VLAN_STRIP_MODE		    0x2UL
+	#define VNIC_CFG_REQ_FLAGS_BD_STALL_MODE		    0x4UL
 	__le32 enables;
 	#define VNIC_CFG_REQ_ENABLES_DFLT_RING_GRP		    0x1UL
 	#define VNIC_CFG_REQ_ENABLES_RSS_RULE			    0x2UL
@@ -2380,18 +2374,16 @@ struct hwrm_ring_alloc_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	__le32 enables;
-	#define RING_ALLOC_REQ_ENABLES_ARB_GRP_ID_VALID	    0x1UL
-	#define RING_ALLOC_REQ_ENABLES_INPUT_NUM_VALID		    0x2UL
-	#define RING_ALLOC_REQ_ENABLES_WEIGHT_VALID		    0x4UL
+	#define RING_ALLOC_REQ_ENABLES_RESERVED1		    0x1UL
+	#define RING_ALLOC_REQ_ENABLES_RESERVED2		    0x2UL
+	#define RING_ALLOC_REQ_ENABLES_RESERVED3		    0x4UL
 	#define RING_ALLOC_REQ_ENABLES_STAT_CTX_ID_VALID	    0x8UL
-	#define RING_ALLOC_REQ_ENABLES_MIN_BW_VALID		    0x10UL
+	#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_STATUS		   (0x3UL << 0)
-	#define RING_ALLOC_REQ_RING_TYPE_CMD			   (0x4UL << 0)
 	u8 unused_0;
 	__le16 unused_1;
 	__le64 page_tbl_addr;
@@ -2406,17 +2398,17 @@ struct hwrm_ring_alloc_input {
 	__le16 queue_id;
 	u8 unused_4;
 	u8 unused_5;
-	__le32 arb_grp_id;
-	__le16 input_number;
+	__le32 reserved1;
+	__le16 reserved2;
 	u8 unused_6;
 	u8 unused_7;
-	__le32 weight;
+	__le32 reserved3;
 	__le32 stat_ctx_id;
-	__le32 min_bw;
+	__le32 reserved4;
 	__le32 max_bw;
 	u8 int_mode;
 	#define RING_ALLOC_REQ_INT_MODE_LEGACY			   (0x0UL << 0)
-	#define RING_ALLOC_REQ_INT_MODE_MSI			   (0x1UL << 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)
 	u8 unused_8[3];
@@ -2448,8 +2440,6 @@ struct hwrm_ring_free_input {
 	#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_STATUS			   (0x3UL << 0)
-	#define RING_FREE_REQ_RING_TYPE_CMD			   (0x4UL << 0)
 	u8 unused_0;
 	__le16 ring_id;
 	__le32 unused_1;
@@ -2550,8 +2540,6 @@ struct hwrm_ring_reset_input {
 	#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_STATUS		   (0x3UL << 0)
-	#define RING_RESET_REQ_RING_TYPE_CMD			   (0x4UL << 0)
 	u8 unused_0;
 	__le16 ring_id;
 	__le32 unused_1;
@@ -2622,61 +2610,6 @@ struct hwrm_ring_grp_free_output {
 	u8 valid;
 };
 
-/* hwrm_arb_grp_alloc */
-/* Input (24 bytes) */
-struct hwrm_arb_grp_alloc_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le16 input_number;
-	__le16 unused_0[3];
-};
-
-/* Output (16 bytes) */
-struct hwrm_arb_grp_alloc_output {
-	__le16 error_code;
-	__le16 req_type;
-	__le16 seq_id;
-	__le16 resp_len;
-	__le16 arb_grp_id;
-	u8 unused_0;
-	u8 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 unused_4;
-	u8 valid;
-};
-
-/* hwrm_arb_grp_cfg */
-/* Input (32 bytes) */
-struct hwrm_arb_grp_cfg_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 arb_grp_id;
-	__le16 input_number;
-	__le16 tx_ring;
-	__le32 weight;
-	__le32 unused_0;
-};
-
-/* Output (16 bytes) */
-struct hwrm_arb_grp_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_cfa_l2_filter_alloc */
 /* Input (96 bytes) */
 struct hwrm_cfa_l2_filter_alloc_input {
@@ -2708,7 +2641,7 @@ struct hwrm_cfa_l2_filter_alloc_input {
 	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_SRC_TYPE	    0x1000UL
 	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_SRC_ID		    0x2000UL
 	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_TUNNEL_TYPE	    0x4000UL
-	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_DST_VNIC_ID	    0x8000UL
+	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_DST_ID		    0x8000UL
 	#define CFA_L2_FILTER_ALLOC_REQ_ENABLES_MIRROR_VNIC_ID     0x10000UL
 	u8 l2_addr[6];
 	u8 unused_0;
@@ -2751,7 +2684,7 @@ struct hwrm_cfa_l2_filter_alloc_input {
 	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE	   (0x8UL << 0)
 	#define CFA_L2_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL     (0xffUL << 0)
 	u8 unused_7;
-	__le16 dst_vnic_id;
+	__le16 dst_id;
 	__le16 mirror_vnic_id;
 	u8 pri_hint;
 	#define CFA_L2_FILTER_ALLOC_REQ_PRI_HINT_NO_PREFER	   (0x0UL << 0)
@@ -2816,10 +2749,11 @@ struct hwrm_cfa_l2_filter_cfg_input {
 	#define CFA_L2_FILTER_CFG_REQ_FLAGS_PATH_RX		   (0x1UL << 0)
 	#define CFA_L2_FILTER_CFG_REQ_FLAGS_DROP		    0x2UL
 	__le32 enables;
-	#define CFA_L2_FILTER_CFG_REQ_ENABLES_DST_VNIC_ID_VALID    0x1UL
+	#define CFA_L2_FILTER_CFG_REQ_ENABLES_DST_ID		    0x1UL
+	#define CFA_L2_FILTER_CFG_REQ_ENABLES_NEW_MIRROR_VNIC_ID   0x2UL
 	__le64 l2_filter_id;
-	__le32 dst_vnic_id;
-	__le32 unused_0;
+	__le32 dst_id;
+	__le32 new_mirror_vnic_id;
 };
 
 /* Output (16 bytes) */
@@ -2843,9 +2777,9 @@ struct hwrm_cfa_l2_set_rx_mask_input {
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le32 dflt_vnic_id;
+	__le32 vnic_id;
 	__le32 mask;
-	#define CFA_L2_SET_RX_MASK_REQ_MASK_UNICAST		    0x1UL
+	#define CFA_L2_SET_RX_MASK_REQ_MASK_RESERVED		    0x1UL
 	#define CFA_L2_SET_RX_MASK_REQ_MASK_MCAST		    0x2UL
 	#define CFA_L2_SET_RX_MASK_REQ_MASK_ALL_MCAST		    0x4UL
 	#define CFA_L2_SET_RX_MASK_REQ_MASK_BCAST		    0x8UL
@@ -2869,46 +2803,6 @@ struct hwrm_cfa_l2_set_rx_mask_output {
 	u8 valid;
 };
 
-/* hwrm_cfa_l2_set_bcastmcast_mirroring */
-/* Input (32 bytes) */
-struct hwrm_cfa_l2_set_bcastmcast_mirroring_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 dflt_vnic_id;
-	__le32 mirroring_flags;
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MIRRORING_FLAGS_BCAST_MIRRORING 0x1UL
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MIRRORING_FLAGS_MCAST_MIRRORING 0x2UL
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MIRRORING_FLAGS_BCAST_SRC_KNOCKOUT 0x4UL
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MIRRORING_FLAGS_MCAST_SRC_KNOCKOUT 0x8UL
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MIRRORING_FLAGS_VLAN_ID_VALID 0x10UL
-	__le16 vlan_id;
-	u8 bcast_domain;
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_BCAST_DOMAIN_PFONLY (0x0UL << 0)
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_BCAST_DOMAIN_ALLPFS (0x1UL << 0)
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_BCAST_DOMAIN_ALLPFSVFS (0x2UL << 0)
-	u8 mcast_domain;
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MCAST_DOMAIN_PFONLY (0x0UL << 0)
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MCAST_DOMAIN_ALLPFS (0x1UL << 0)
-	#define CFA_L2_SET_BCASTMCAST_MIRRORING_REQ_MCAST_DOMAIN_ALLPFSVFS (0x2UL << 0)
-	__le32 unused_0;
-};
-
-/* Output (16 bytes) */
-struct hwrm_cfa_l2_set_bcastmcast_mirroring_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_cfa_tunnel_filter_alloc */
 /* Input (88 bytes) */
 struct hwrm_cfa_tunnel_filter_alloc_input {
@@ -3017,17 +2911,16 @@ struct hwrm_cfa_encap_record_alloc_input {
 	__le32 encap_data[16];
 };
 
-/* Output (24 bytes) */
+/* Output (16 bytes) */
 struct hwrm_cfa_encap_record_alloc_output {
 	__le16 error_code;
 	__le16 req_type;
 	__le16 seq_id;
 	__le16 resp_len;
-	__le64 encap_record_id;
-	__le32 unused_0;
+	__le32 encap_record_id;
+	u8 unused_0;
 	u8 unused_1;
 	u8 unused_2;
-	u8 unused_3;
 	u8 valid;
 };
 
@@ -3039,7 +2932,8 @@ struct hwrm_cfa_encap_record_free_input {
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le64 encap_record_id;
+	__le32 encap_record_id;
+	__le32 unused_0;
 };
 
 /* Output (16 bytes) */
@@ -3083,14 +2977,21 @@ struct hwrm_cfa_ntuple_filter_alloc_input {
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_PORT_MASK  0x2000UL
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_PRI_HINT       0x4000UL
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_NTUPLE_FILTER_ID 0x8000UL
-	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_VNIC_ID    0x10000UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_ID	    0x10000UL
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_MIRROR_VNIC_ID 0x20000UL
+	#define CFA_NTUPLE_FILTER_ALLOC_REQ_ENABLES_DST_MACADDR    0x40000UL
 	__le64 l2_filter_id;
 	u8 src_macaddr[6];
 	__be16 ethertype;
-	u8 ipaddr_type;
+	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)
 	u8 ip_protocol;
-	__le16 dst_vnic_id;
+	#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)
+	__le16 dst_id;
 	__le16 mirror_vnic_id;
 	u8 tunnel_type;
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_NONTUNNEL (0x0UL << 0)
@@ -3104,6 +3005,11 @@ struct hwrm_cfa_ntuple_filter_alloc_input {
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_IPGRE     (0x8UL << 0)
 	#define CFA_NTUPLE_FILTER_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL (0xffUL << 0)
 	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)
 	__be32 src_ipaddr[4];
 	__be32 src_ipaddr_mask[4];
 	__be32 dst_ipaddr[4];
@@ -3162,11 +3068,11 @@ struct hwrm_cfa_ntuple_filter_cfg_input {
 	__le16 target_id;
 	__le64 resp_addr;
 	__le32 enables;
-	#define CFA_NTUPLE_FILTER_CFG_REQ_ENABLES_NEW_DST_VNIC_ID_VALID 0x1UL
-	#define CFA_NTUPLE_FILTER_CFG_REQ_ENABLES_NEW_MIRROR_VNIC_ID_VALID 0x2UL
+	#define CFA_NTUPLE_FILTER_CFG_REQ_ENABLES_NEW_DST_ID       0x1UL
+	#define CFA_NTUPLE_FILTER_CFG_REQ_ENABLES_NEW_MIRROR_VNIC_ID 0x2UL
 	__le32 unused_0;
 	__le64 ntuple_filter_id;
-	__le32 new_dst_vnic_id;
+	__le32 new_dst_id;
 	__le32 new_mirror_vnic_id;
 };
 
@@ -3192,16 +3098,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_NONTUNNEL   (0x0UL << 0)
 	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_VXLAN       (0x1UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_NVGRE       (0x2UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_L2GRE       (0x3UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_IPIP	   (0x4UL << 0)
 	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_GENEVE      (0x5UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_MPLS	   (0x6UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_STT	   (0x7UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_IPGRE       (0x8UL << 0)
-	#define TUNNEL_DST_PORT_QUERY_REQ_TUNNEL_TYPE_ANYTUNNEL   (0xffUL << 0)
 	u8 unused_0[7];
 };
 
@@ -3228,16 +3126,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_NONTUNNEL   (0x0UL << 0)
 	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_VXLAN       (0x1UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_NVGRE       (0x2UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_L2GRE       (0x3UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_IPIP	   (0x4UL << 0)
 	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_GENEVE      (0x5UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_MPLS	   (0x6UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_STT	   (0x7UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_IPGRE       (0x8UL << 0)
-	#define TUNNEL_DST_PORT_ALLOC_REQ_TUNNEL_TYPE_ANYTUNNEL   (0xffUL << 0)
 	u8 unused_0;
 	__be16 tunnel_dst_port_val;
 	__le32 unused_1;
@@ -3267,16 +3157,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_NONTUNNEL    (0x0UL << 0)
 	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_VXLAN	   (0x1UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_NVGRE	   (0x2UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_L2GRE	   (0x3UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_IPIP	   (0x4UL << 0)
 	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_GENEVE       (0x5UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_MPLS	   (0x6UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_STT	   (0x7UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_IPGRE	   (0x8UL << 0)
-	#define TUNNEL_DST_PORT_FREE_REQ_TUNNEL_TYPE_ANYTUNNEL    (0xffUL << 0)
 	u8 unused_0;
 	__le16 tunnel_dst_port_id;
 	__le32 unused_1;
@@ -3416,68 +3298,145 @@ struct hwrm_stat_ctx_clr_stats_output {
 	u8 valid;
 };
 
-/* hwrm_mgmt_l2_filter_alloc */
-/* Input (56 bytes) */
-struct hwrm_mgmt_l2_filter_alloc_input {
+/* hwrm_fw_reset */
+/* Input (24 bytes) */
+struct hwrm_fw_reset_input {
 	__le16 req_type;
 	__le16 cmpl_ring;
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le32 flags;
-	#define MGMT_L2_FILTER_ALLOC_REQ_FLAGS_PATH		    0x1UL
-	#define MGMT_L2_FILTER_ALLOC_REQ_FLAGS_PATH_TX		   (0x0UL << 0)
-	#define MGMT_L2_FILTER_ALLOC_REQ_FLAGS_PATH_RX		   (0x1UL << 0)
-	__le32 enables;
-	#define MGMT_L2_FILTER_ALLOC_REQ_ENABLES_L2_ADDRESS	    0x1UL
-	#define MGMT_L2_FILTER_ALLOC_REQ_ENABLES_OVLAN		    0x2UL
-	#define MGMT_L2_FILTER_ALLOC_REQ_ENABLES_IVLAN		    0x4UL
-	#define MGMT_L2_FILTER_ALLOC_REQ_ENABLES_ACTION_ID	    0x8UL
-	u8 l2_address[6];
+	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)
+	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)
+	__le16 unused_0[3];
+};
+
+/* Output (16 bytes) */
+struct hwrm_fw_reset_output {
+	__le16 error_code;
+	__le16 req_type;
+	__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)
 	u8 unused_0;
+	__le16 unused_1;
+	u8 unused_2;
+	u8 unused_3;
+	u8 unused_4;
+	u8 valid;
+};
+
+/* hwrm_exec_fwd_resp */
+/* Input (128 bytes) */
+struct hwrm_exec_fwd_resp_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+	__le32 encap_request[26];
+	__le16 encap_resp_target_id;
+	__le16 unused_0[3];
+};
+
+/* Output (16 bytes) */
+struct hwrm_exec_fwd_resp_output {
+	__le16 error_code;
+	__le16 req_type;
+	__le16 seq_id;
+	__le16 resp_len;
+	__le32 unused_0;
 	u8 unused_1;
-	u8 l2_address_mask[6];
-	__le16 ovlan;
-	__le16 ovlan_mask;
-	__le16 ivlan;
-	__le16 ivlan_mask;
 	u8 unused_2;
 	u8 unused_3;
-	__le32 action_id;
-	u8 action_bypass;
-	#define MGMT_L2_FILTER_ALLOC_REQ_ACTION_BYPASS		    0x1UL
-	u8 unused_5[3];
+	u8 valid;
+};
+
+/* hwrm_reject_fwd_resp */
+/* Input (128 bytes) */
+struct hwrm_reject_fwd_resp_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+	__le32 encap_request[26];
+	__le16 encap_resp_target_id;
+	__le16 unused_0[3];
 };
 
 /* Output (16 bytes) */
-struct hwrm_mgmt_l2_filter_alloc_output {
+struct hwrm_reject_fwd_resp_output {
 	__le16 error_code;
 	__le16 req_type;
 	__le16 seq_id;
 	__le16 resp_len;
-	__le16 mgmt_l2_filter_id;
+	__le32 unused_0;
+	u8 unused_1;
+	u8 unused_2;
+	u8 unused_3;
+	u8 valid;
+};
+
+/* hwrm_fwd_resp */
+/* Input (40 bytes) */
+struct hwrm_fwd_resp_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+	__le16 encap_resp_target_id;
+	__le16 encap_resp_cmpl_ring;
+	__le16 encap_resp_len;
 	u8 unused_0;
 	u8 unused_1;
+	__le64 encap_resp_addr;
+	__le32 encap_resp[24];
+};
+
+/* Output (16 bytes) */
+struct hwrm_fwd_resp_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 unused_4;
 	u8 valid;
 };
 
-/* hwrm_mgmt_l2_filter_free */
-/* Input (24 bytes) */
-struct hwrm_mgmt_l2_filter_free_input {
+/* hwrm_fwd_async_event_cmpl */
+/* Input (32 bytes) */
+struct hwrm_fwd_async_event_cmpl_input {
 	__le16 req_type;
 	__le16 cmpl_ring;
 	__le16 seq_id;
 	__le16 target_id;
 	__le64 resp_addr;
-	__le16 mgmt_l2_filter_id;
-	__le16 unused_0[3];
+	__le16 encap_async_event_target_id;
+	u8 unused_0;
+	u8 unused_1;
+	u8 unused_2[3];
+	u8 unused_3;
+	__le32 encap_async_event_cmpl[4];
 };
 
 /* Output (16 bytes) */
-struct hwrm_mgmt_l2_filter_free_output {
+struct hwrm_fwd_async_event_cmpl_output {
 	__le16 error_code;
 	__le16 req_type;
 	__le16 seq_id;
@@ -3489,6 +3448,31 @@ struct hwrm_mgmt_l2_filter_free_output {
 	u8 valid;
 };
 
+/* hwrm_temp_monitor_query */
+/* Input (16 bytes) */
+struct hwrm_temp_monitor_query_input {
+	__le16 req_type;
+	__le16 cmpl_ring;
+	__le16 seq_id;
+	__le16 target_id;
+	__le64 resp_addr;
+};
+
+/* Output (16 bytes) */
+struct hwrm_temp_monitor_query_output {
+	__le16 error_code;
+	__le16 req_type;
+	__le16 seq_id;
+	__le16 resp_len;
+	u8 temp;
+	u8 unused_0;
+	__le16 unused_1;
+	u8 unused_2;
+	u8 unused_3;
+	u8 unused_4;
+	u8 valid;
+};
+
 /* hwrm_nvm_raw_write_blk */
 /* Input (32 bytes) */
 struct hwrm_nvm_raw_write_blk_input {
@@ -3621,7 +3605,7 @@ struct hwrm_nvm_get_dir_info_output {
 };
 
 /* hwrm_nvm_write */
-/* Input (40 bytes) */
+/* Input (48 bytes) */
 struct hwrm_nvm_write_input {
 	__le16 req_type;
 	__le16 cmpl_ring;
@@ -3637,6 +3621,8 @@ struct hwrm_nvm_write_input {
 	__le16 option;
 	__le16 flags;
 	#define NVM_WRITE_REQ_FLAGS_KEEP_ORIG_ACTIVE_IMG	    0x1UL
+	__le32 dir_item_length;
+	__le32 unused_0;
 };
 
 /* Output (16 bytes) */
@@ -3645,10 +3631,9 @@ struct hwrm_nvm_write_output {
 	__le16 req_type;
 	__le16 seq_id;
 	__le16 resp_len;
-	__le32 unused_0;
-	u8 unused_1;
-	u8 unused_2;
-	u8 unused_3;
+	__le32 dir_item_length;
+	__le16 dir_idx;
+	u8 unused_0;
 	u8 valid;
 };
 
@@ -3833,214 +3818,4 @@ struct hwrm_nvm_verify_update_output {
 	u8 valid;
 };
 
-/* hwrm_exec_fwd_resp */
-/* Input (120 bytes) */
-struct hwrm_exec_fwd_resp_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 encap_request[24];
-	__le16 encap_resp_target_id;
-	__le16 unused_0[3];
-};
-
-/* Output (16 bytes) */
-struct hwrm_exec_fwd_resp_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_reject_fwd_resp */
-/* Input (120 bytes) */
-struct hwrm_reject_fwd_resp_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le32 encap_request[24];
-	__le16 encap_resp_target_id;
-	__le16 unused_0[3];
-};
-
-/* Output (16 bytes) */
-struct hwrm_reject_fwd_resp_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_fwd_resp */
-/* Input (40 bytes) */
-struct hwrm_fwd_resp_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le16 encap_resp_target_id;
-	__le16 encap_resp_cmpl_ring;
-	__le16 encap_resp_len;
-	u8 unused_0;
-	u8 unused_1;
-	__le64 encap_resp_addr;
-	__le32 encap_resp[24];
-};
-
-/* Output (16 bytes) */
-struct hwrm_fwd_resp_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_fwd_async_event_cmpl */
-/* Input (32 bytes) */
-struct hwrm_fwd_async_event_cmpl_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	__le16 encap_async_event_target_id;
-	u8 unused_0;
-	u8 unused_1;
-	u8 unused_2[3];
-	u8 unused_3;
-	__le32 encap_async_event_cmpl[4];
-};
-
-/* Output (16 bytes) */
-struct hwrm_fwd_async_event_cmpl_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_fw_reset */
-/* Input (24 bytes) */
-struct hwrm_fw_reset_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	u8 embedded_proc_type;
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_CHIMP		   (0x0UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_APE		   (0x1UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_KONG		   (0x2UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_BONO		   (0x3UL << 0)
-	#define FW_RESET_REQ_EMBEDDED_PROC_TYPE_TANG		   (0x4UL << 0)
-	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)
-	__le16 unused_0[3];
-};
-
-/* Output (16 bytes) */
-struct hwrm_fw_reset_output {
-	__le16 error_code;
-	__le16 req_type;
-	__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)
-	u8 unused_0;
-	__le16 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 unused_4;
-	u8 valid;
-};
-
-/* hwrm_fw_qstatus */
-/* Input (24 bytes) */
-struct hwrm_fw_qstatus_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-	u8 embedded_proc_type;
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_CHIMP	   (0x0UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_APE		   (0x1UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_KONG		   (0x2UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_BONO		   (0x3UL << 0)
-	#define FW_QSTATUS_REQ_EMBEDDED_PROC_TYPE_TANG		   (0x4UL << 0)
-	u8 unused_0[7];
-};
-
-/* Output (16 bytes) */
-struct hwrm_fw_qstatus_output {
-	__le16 error_code;
-	__le16 req_type;
-	__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)
-	u8 unused_0;
-	__le16 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 unused_4;
-	u8 valid;
-};
-
-/* hwrm_temp_monitor_query */
-/* Input (16 bytes) */
-struct hwrm_temp_monitor_query_input {
-	__le16 req_type;
-	__le16 cmpl_ring;
-	__le16 seq_id;
-	__le16 target_id;
-	__le64 resp_addr;
-};
-
-/* Output (16 bytes) */
-struct hwrm_temp_monitor_query_output {
-	__le16 error_code;
-	__le16 req_type;
-	__le16 seq_id;
-	__le16 resp_len;
-	u8 temp;
-	u8 unused_0;
-	__le16 unused_1;
-	u8 unused_2;
-	u8 unused_3;
-	u8 unused_4;
-	u8 valid;
-};
-
 #endif
diff --git a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
index ea044bbcd384..c1cc83d7e38c 100644
--- a/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnxt/bnxt_sriov.c
@@ -64,7 +64,7 @@ int bnxt_set_vf_spoofchk(struct net_device *dev, int vf_id, bool setting)
 	 * the spoof check should also include vlan anti-spoofing
 	 */
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FUNC_CFG, -1, -1);
-	req.vf_id = cpu_to_le16(vf->fw_fid);
+	req.fid = cpu_to_le16(vf->fw_fid);
 	req.flags = cpu_to_le32(func_flags);
 	rc = hwrm_send_message(bp, &req, sizeof(req), HWRM_CMD_TIMEOUT);
 	if (!rc) {
@@ -128,7 +128,7 @@ int bnxt_set_vf_mac(struct net_device *dev, int vf_id, u8 *mac)
 
 	memcpy(vf->mac_addr, mac, ETH_ALEN);
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FUNC_CFG, -1, -1);
-	req.vf_id = cpu_to_le16(vf->fw_fid);
+	req.fid = cpu_to_le16(vf->fw_fid);
 	req.flags = cpu_to_le32(vf->func_flags);
 	req.enables = cpu_to_le32(FUNC_CFG_REQ_ENABLES_DFLT_MAC_ADDR);
 	memcpy(req.dflt_mac_addr, mac, ETH_ALEN);
@@ -159,7 +159,7 @@ int bnxt_set_vf_vlan(struct net_device *dev, int vf_id, u16 vlan_id, u8 qos)
 		return 0;
 
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FUNC_CFG, -1, -1);
-	req.vf_id = cpu_to_le16(vf->fw_fid);
+	req.fid = cpu_to_le16(vf->fw_fid);
 	req.flags = cpu_to_le32(vf->func_flags);
 	req.dflt_vlan = cpu_to_le16(vlan_tag);
 	req.enables = cpu_to_le32(FUNC_CFG_REQ_ENABLES_DFLT_VLAN);
@@ -198,7 +198,7 @@ int bnxt_set_vf_bw(struct net_device *dev, int vf_id, int min_tx_rate,
 	if (min_tx_rate == vf->min_tx_rate && max_tx_rate == vf->max_tx_rate)
 		return 0;
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_FUNC_CFG, -1, -1);
-	req.vf_id = cpu_to_le16(vf->fw_fid);
+	req.fid = cpu_to_le16(vf->fw_fid);
 	req.flags = cpu_to_le32(vf->func_flags);
 	req.enables = cpu_to_le32(FUNC_CFG_REQ_ENABLES_MAX_BW);
 	req.max_bw = cpu_to_le32(max_tx_rate);
@@ -363,10 +363,11 @@ static int bnxt_hwrm_func_buf_rgtr(struct bnxt *bp)
 }
 
 /* only call by PF to reserve resources for VF */
-static int bnxt_hwrm_func_cfg(struct bnxt *bp, int *num_vfs)
+static int bnxt_hwrm_func_cfg(struct bnxt *bp, int num_vfs)
 {
 	u32 rc = 0, mtu, i;
 	u16 vf_tx_rings, vf_rx_rings, vf_cp_rings, vf_stat_ctx, vf_vnics;
+	u16 vf_ring_grps;
 	struct hwrm_func_cfg_input req = {0};
 	struct bnxt_pf_info *pf = &bp->pf;
 
@@ -378,18 +379,18 @@ static int bnxt_hwrm_func_cfg(struct bnxt *bp, int *num_vfs)
 	 * be removed once new HWRM provides HW ring groups capability in
 	 * hwrm_func_qcap.
 	 */
-	vf_cp_rings = min_t(u16, bp->pf.max_cp_rings, bp->pf.max_stat_ctxs);
-	vf_cp_rings = (vf_cp_rings - bp->cp_nr_rings) / *num_vfs;
+	vf_cp_rings = min_t(u16, pf->max_cp_rings, pf->max_stat_ctxs);
+	vf_cp_rings = (vf_cp_rings - bp->cp_nr_rings) / num_vfs;
 	/* TODO: restore this logic below once the WA above is removed */
-	/* vf_cp_rings = (bp->pf.max_cp_rings - bp->cp_nr_rings) / *num_vfs; */
-	vf_stat_ctx = (bp->pf.max_stat_ctxs - bp->num_stat_ctxs) / *num_vfs;
+	/* vf_cp_rings = (pf->max_cp_rings - bp->cp_nr_rings) / num_vfs; */
+	vf_stat_ctx = (pf->max_stat_ctxs - bp->num_stat_ctxs) / num_vfs;
 	if (bp->flags & BNXT_FLAG_AGG_RINGS)
-		vf_rx_rings = (bp->pf.max_rx_rings - bp->rx_nr_rings * 2) /
-			      *num_vfs;
+		vf_rx_rings = (pf->max_rx_rings - bp->rx_nr_rings * 2) /
+			      num_vfs;
 	else
-		vf_rx_rings = (bp->pf.max_rx_rings - bp->rx_nr_rings) /
-			      *num_vfs;
-	vf_tx_rings = (bp->pf.max_tx_rings - bp->tx_nr_rings) / *num_vfs;
+		vf_rx_rings = (pf->max_rx_rings - bp->rx_nr_rings) / num_vfs;
+	vf_ring_grps = (bp->pf.max_hw_ring_grps - bp->rx_nr_rings) / num_vfs;
+	vf_tx_rings = (pf->max_tx_rings - bp->tx_nr_rings) / num_vfs;
 
 	req.enables = cpu_to_le32(FUNC_CFG_REQ_ENABLES_MTU |
 				  FUNC_CFG_REQ_ENABLES_MRU |
@@ -399,7 +400,8 @@ static int bnxt_hwrm_func_cfg(struct bnxt *bp, int *num_vfs)
 				  FUNC_CFG_REQ_ENABLES_NUM_TX_RINGS |
 				  FUNC_CFG_REQ_ENABLES_NUM_RX_RINGS |
 				  FUNC_CFG_REQ_ENABLES_NUM_L2_CTXS |
-				  FUNC_CFG_REQ_ENABLES_NUM_VNICS);
+				  FUNC_CFG_REQ_ENABLES_NUM_VNICS |
+				  FUNC_CFG_REQ_ENABLES_NUM_HW_RING_GRPS);
 
 	mtu = bp->dev->mtu + ETH_HLEN + ETH_FCS_LEN + VLAN_HLEN;
 	req.mru = cpu_to_le16(mtu);
@@ -409,6 +411,7 @@ static int bnxt_hwrm_func_cfg(struct bnxt *bp, int *num_vfs)
 	req.num_cmpl_rings = cpu_to_le16(vf_cp_rings);
 	req.num_tx_rings = cpu_to_le16(vf_tx_rings);
 	req.num_rx_rings = cpu_to_le16(vf_rx_rings);
+	req.num_hw_ring_grps = cpu_to_le16(vf_ring_grps);
 	req.num_l2_ctxs = cpu_to_le16(4);
 	vf_vnics = 1;
 
@@ -417,22 +420,24 @@ static int bnxt_hwrm_func_cfg(struct bnxt *bp, int *num_vfs)
 	req.num_stat_ctxs = cpu_to_le16(vf_stat_ctx);
 
 	mutex_lock(&bp->hwrm_cmd_lock);
-	for (i = 0; i < *num_vfs; i++) {
-		req.vf_id = cpu_to_le16(pf->first_vf_id + i);
+	for (i = 0; i < num_vfs; i++) {
+		req.fid = cpu_to_le16(pf->first_vf_id + i);
 		rc = _hwrm_send_message(bp, &req, sizeof(req),
 					HWRM_CMD_TIMEOUT);
 		if (rc)
 			break;
-		bp->pf.active_vfs = i + 1;
-		bp->pf.vf[i].fw_fid = le16_to_cpu(req.vf_id);
+		pf->active_vfs = i + 1;
+		pf->vf[i].fw_fid = le16_to_cpu(req.fid);
 	}
 	mutex_unlock(&bp->hwrm_cmd_lock);
 	if (!rc) {
-		bp->pf.max_pf_tx_rings = bp->tx_nr_rings;
-		if (bp->flags & BNXT_FLAG_AGG_RINGS)
-			bp->pf.max_pf_rx_rings = bp->rx_nr_rings * 2;
-		else
-			bp->pf.max_pf_rx_rings = bp->rx_nr_rings;
+		pf->max_tx_rings -= vf_tx_rings * num_vfs;
+		pf->max_rx_rings -= vf_rx_rings * num_vfs;
+		pf->max_hw_ring_grps -= vf_ring_grps * num_vfs;
+		pf->max_cp_rings -= vf_cp_rings * num_vfs;
+		pf->max_rsscos_ctxs -= num_vfs;
+		pf->max_stat_ctxs -= vf_stat_ctx * num_vfs;
+		pf->max_vnics -= vf_vnics * num_vfs;
 	}
 	return rc;
 }
@@ -492,7 +497,7 @@ static int bnxt_sriov_enable(struct bnxt *bp, int *num_vfs)
 		goto err_out1;
 
 	/* Reserve resources for VFs */
-	rc = bnxt_hwrm_func_cfg(bp, num_vfs);
+	rc = bnxt_hwrm_func_cfg(bp, *num_vfs);
 	if (rc)
 		goto err_out2;
 
@@ -536,8 +541,8 @@ void bnxt_sriov_disable(struct bnxt *bp)
 	bnxt_free_vf_resources(bp);
 
 	bp->pf.active_vfs = 0;
-	bp->pf.max_pf_rx_rings = bp->pf.max_rx_rings;
-	bp->pf.max_pf_tx_rings = bp->pf.max_tx_rings;
+	/* Reclaim all resources for the PF. */
+	bnxt_hwrm_func_qcaps(bp);
 }
 
 int bnxt_sriov_configure(struct pci_dev *pdev, int num_vfs)
@@ -595,6 +600,7 @@ static int bnxt_hwrm_fwd_resp(struct bnxt *bp, struct bnxt_vf_info *vf,
 
 	/* Set the new target id */
 	req.target_id = cpu_to_le16(vf->fw_fid);
+	req.encap_resp_target_id = cpu_to_le16(vf->fw_fid);
 	req.encap_resp_len = cpu_to_le16(msg_size);
 	req.encap_resp_addr = encap_resp_addr;
 	req.encap_resp_cmpl_ring = encap_resp_cpr;
@@ -629,6 +635,7 @@ static int bnxt_hwrm_fwd_err_resp(struct bnxt *bp, struct bnxt_vf_info *vf,
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_REJECT_FWD_RESP, -1, -1);
 	/* Set the new target id */
 	req.target_id = cpu_to_le16(vf->fw_fid);
+	req.encap_resp_target_id = cpu_to_le16(vf->fw_fid);
 	memcpy(req.encap_request, vf->hwrm_cmd_req_addr, msg_size);
 
 	mutex_lock(&bp->hwrm_cmd_lock);
@@ -660,6 +667,7 @@ static int bnxt_hwrm_exec_fwd_resp(struct bnxt *bp, struct bnxt_vf_info *vf,
 	bnxt_hwrm_cmd_hdr_init(bp, &req, HWRM_EXEC_FWD_RESP, -1, -1);
 	/* Set the new target id */
 	req.target_id = cpu_to_le16(vf->fw_fid);
+	req.encap_resp_target_id = cpu_to_le16(vf->fw_fid);
 	memcpy(req.encap_request, vf->hwrm_cmd_req_addr, msg_size);
 
 	mutex_lock(&bp->hwrm_cmd_lock);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 17f017ab4dac..d7e01a74e927 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -2041,11 +2041,11 @@ static void bcmgenet_init_tx_napi(struct bcmgenet_priv *priv)
 
 	for (i = 0; i < priv->hw_params->tx_queues; ++i) {
 		ring = &priv->tx_rings[i];
-		netif_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
+		netif_tx_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
 	}
 
 	ring = &priv->tx_rings[DESC_INDEX];
-	netif_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
+	netif_tx_napi_add(priv->dev, &ring->napi, bcmgenet_tx_poll, 64);
 }
 
 static void bcmgenet_enable_tx_napi(struct bcmgenet_priv *priv)
@@ -2445,8 +2445,7 @@ static void bcmgenet_irq_task(struct work_struct *work)
 	}
 
 	/* Link UP/DOWN event */
-	if ((priv->hw_params->flags & GENET_HAS_MDIO_INTR) &&
-	    (priv->irq0_stat & UMAC_IRQ_LINK_EVENT)) {
+	if (priv->irq0_stat & UMAC_IRQ_LINK_EVENT) {
 		phy_mac_interrupt(priv->phydev,
 				  !!(priv->irq0_stat & UMAC_IRQ_LINK_UP));
 		priv->irq0_stat &= ~UMAC_IRQ_LINK_EVENT;
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index 8bdfe53754ba..457c3bc8cfff 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -401,9 +401,7 @@ int bcmgenet_mii_probe(struct net_device *dev)
 	 * Ethernet MAC ISRs
 	 */
 	if (priv->internal_phy)
-		priv->mii_bus->irq[phydev->addr] = PHY_IGNORE_INTERRUPT;
-	else
-		priv->mii_bus->irq[phydev->addr] = PHY_POLL;
+		priv->phydev->irq = PHY_IGNORE_INTERRUPT;
 
 	return 0;
 }
@@ -477,12 +475,6 @@ static int bcmgenet_mii_alloc(struct bcmgenet_priv *priv)
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-%d",
 		 priv->pdev->name, priv->pdev->id);
 
-	bus->irq = kcalloc(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
-	if (!bus->irq) {
-		mdiobus_free(priv->mii_bus);
-		return -ENOMEM;
-	}
-
 	return 0;
 }
 
@@ -581,7 +573,7 @@ static int bcmgenet_mii_pd_init(struct bcmgenet_priv *priv)
 		}
 
 		if (pd->phy_address >= 0 && pd->phy_address < PHY_MAX_ADDR)
-			phydev = mdio->phy_map[pd->phy_address];
+			phydev = mdiobus_get_phy(mdio, pd->phy_address);
 		else
 			phydev = phy_find_first(mdio);
 
@@ -648,7 +640,6 @@ int bcmgenet_mii_init(struct net_device *dev)
 out:
 	of_node_put(priv->phy_dn);
 	mdiobus_unregister(priv->mii_bus);
-	kfree(priv->mii_bus->irq);
 	mdiobus_free(priv->mii_bus);
 	return ret;
 }
@@ -659,6 +650,5 @@ void bcmgenet_mii_exit(struct net_device *dev)
 
 	of_node_put(priv->phy_dn);
 	mdiobus_unregister(priv->mii_bus);
-	kfree(priv->mii_bus->irq);
 	mdiobus_free(priv->mii_bus);
 }
diff --git a/drivers/net/ethernet/broadcom/sb1250-mac.c b/drivers/net/ethernet/broadcom/sb1250-mac.c
index f557a2aaec23..eacc559679bf 100644
--- a/drivers/net/ethernet/broadcom/sb1250-mac.c
+++ b/drivers/net/ethernet/broadcom/sb1250-mac.c
@@ -238,7 +238,6 @@ struct sbmac_softc {
 	struct napi_struct	napi;
 	struct phy_device	*phy_dev;	/* the associated PHY device */
 	struct mii_bus		*mii_bus;	/* the MII bus */
-	int			phy_irq[PHY_MAX_ADDR];
 	spinlock_t		sbm_lock;	/* spin lock */
 	int			sbm_devflags;	/* current device flags */
 
@@ -2250,9 +2249,6 @@ static int sbmac_init(struct platform_device *pldev, long long base)
 	sc->mii_bus->priv = sc;
 	sc->mii_bus->read = sbmac_mii_read;
 	sc->mii_bus->write = sbmac_mii_write;
-	sc->mii_bus->irq = sc->phy_irq;
-	for (i = 0; i < PHY_MAX_ADDR; ++i)
-		sc->mii_bus->irq[i] = SBMAC_PHY_INT;
 
 	sc->mii_bus->parent = &pldev->dev;
 	/*
@@ -2358,20 +2354,15 @@ static int sbmac_mii_probe(struct net_device *dev)
 {
 	struct sbmac_softc *sc = netdev_priv(dev);
 	struct phy_device *phy_dev;
-	int i;
 
-	for (i = 0; i < PHY_MAX_ADDR; i++) {
-		phy_dev = sc->mii_bus->phy_map[i];
-		if (phy_dev)
-			break;
-	}
+	phy_dev = phy_find_first(sc->mii_bus);
 	if (!phy_dev) {
 		printk(KERN_ERR "%s: no PHY found\n", dev->name);
 		return -ENXIO;
 	}
 
-	phy_dev = phy_connect(dev, dev_name(&phy_dev->dev), &sbmac_mii_poll,
-			      PHY_INTERFACE_MODE_GMII);
+	phy_dev = phy_connect(dev, dev_name(&phy_dev->mdio.dev),
+			      &sbmac_mii_poll, PHY_INTERFACE_MODE_GMII);
 	if (IS_ERR(phy_dev)) {
 		printk(KERN_ERR "%s: could not attach to PHY\n", dev->name);
 		return PTR_ERR(phy_dev);
@@ -2388,11 +2379,10 @@ static int sbmac_mii_probe(struct net_device *dev)
 			      SUPPORTED_MII |
 			      SUPPORTED_Pause |
 			      SUPPORTED_Asym_Pause;
-	phy_dev->advertising = phy_dev->supported;
 
-	pr_info("%s: attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		dev->name, phy_dev->drv->name,
-		dev_name(&phy_dev->dev), phy_dev->irq);
+	phy_attached_info(phy_dev);
+
+	phy_dev->advertising = phy_dev->supported;
 
 	sc->phy_dev = phy_dev;
 
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 79789d8e52da..3010080cfeee 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -1406,7 +1406,7 @@ static void tg3_mdio_config_5785(struct tg3 *tp)
 	u32 val;
 	struct phy_device *phydev;
 
-	phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+	phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 	switch (phydev->drv->phy_id & phydev->drv->phy_id_mask) {
 	case PHY_ID_BCM50610:
 	case PHY_ID_BCM50610M:
@@ -1538,10 +1538,6 @@ static int tg3_mdio_init(struct tg3 *tp)
 	tp->mdio_bus->read     = &tg3_mdio_read;
 	tp->mdio_bus->write    = &tg3_mdio_write;
 	tp->mdio_bus->phy_mask = ~(1 << tp->phy_addr);
-	tp->mdio_bus->irq      = &tp->mdio_irq[0];
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		tp->mdio_bus->irq[i] = PHY_POLL;
 
 	/* The bus registration will look for all the PHYs on the mdio bus.
 	 * Unfortunately, it does not ensure the PHY is powered up before
@@ -1558,7 +1554,7 @@ static int tg3_mdio_init(struct tg3 *tp)
 		return i;
 	}
 
-	phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+	phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 	if (!phydev || !phydev->drv) {
 		dev_warn(&tp->pdev->dev, "No PHY devices\n");
@@ -1968,7 +1964,7 @@ static void tg3_setup_flow_control(struct tg3 *tp, u32 lcladv, u32 rmtadv)
 	u32 old_tx_mode = tp->tx_mode;
 
 	if (tg3_flag(tp, USE_PHYLIB))
-		autoneg = tp->mdio_bus->phy_map[tp->phy_addr]->autoneg;
+		autoneg = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr)->autoneg;
 	else
 		autoneg = tp->link_config.autoneg;
 
@@ -2004,7 +2000,7 @@ static void tg3_adjust_link(struct net_device *dev)
 	u8 oldflowctrl, linkmesg = 0;
 	u32 mac_mode, lcl_adv, rmt_adv;
 	struct tg3 *tp = netdev_priv(dev);
-	struct phy_device *phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+	struct phy_device *phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 	spin_lock_bh(&tp->lock);
 
@@ -2093,10 +2089,10 @@ static int tg3_phy_init(struct tg3 *tp)
 	/* Bring the PHY back to a known state. */
 	tg3_bmcr_reset(tp);
 
-	phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+	phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 	/* Attach the MAC to the PHY. */
-	phydev = phy_connect(tp->dev, dev_name(&phydev->dev),
+	phydev = phy_connect(tp->dev, phydev_name(phydev),
 			     tg3_adjust_link, phydev->interface);
 	if (IS_ERR(phydev)) {
 		dev_err(&tp->pdev->dev, "Could not attach to PHY\n");
@@ -2120,7 +2116,7 @@ static int tg3_phy_init(struct tg3 *tp)
 				      SUPPORTED_Asym_Pause);
 		break;
 	default:
-		phy_disconnect(tp->mdio_bus->phy_map[tp->phy_addr]);
+		phy_disconnect(mdiobus_get_phy(tp->mdio_bus, tp->phy_addr));
 		return -EINVAL;
 	}
 
@@ -2128,6 +2124,8 @@ static int tg3_phy_init(struct tg3 *tp)
 
 	phydev->advertising = phydev->supported;
 
+	phy_attached_info(phydev);
+
 	return 0;
 }
 
@@ -2138,7 +2136,7 @@ static void tg3_phy_start(struct tg3 *tp)
 	if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 		return;
 
-	phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+	phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 	if (tp->phy_flags & TG3_PHYFLG_IS_LOW_POWER) {
 		tp->phy_flags &= ~TG3_PHYFLG_IS_LOW_POWER;
@@ -2158,13 +2156,13 @@ static void tg3_phy_stop(struct tg3 *tp)
 	if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 		return;
 
-	phy_stop(tp->mdio_bus->phy_map[tp->phy_addr]);
+	phy_stop(mdiobus_get_phy(tp->mdio_bus, tp->phy_addr));
 }
 
 static void tg3_phy_fini(struct tg3 *tp)
 {
 	if (tp->phy_flags & TG3_PHYFLG_IS_CONNECTED) {
-		phy_disconnect(tp->mdio_bus->phy_map[tp->phy_addr]);
+		phy_disconnect(mdiobus_get_phy(tp->mdio_bus, tp->phy_addr));
 		tp->phy_flags &= ~TG3_PHYFLG_IS_CONNECTED;
 	}
 }
@@ -4048,7 +4046,7 @@ static int tg3_power_down_prepare(struct tg3 *tp)
 			struct phy_device *phydev;
 			u32 phyid, advertising;
 
-			phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+			phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 			tp->phy_flags |= TG3_PHYFLG_IS_LOW_POWER;
 
@@ -7833,6 +7831,14 @@ static int tigon3_dma_hwbug_workaround(struct tg3_napi *tnapi,
 	return ret;
 }
 
+static bool tg3_tso_bug_gso_check(struct tg3_napi *tnapi, struct sk_buff *skb)
+{
+	/* Check if we will never have enough descriptors,
+	 * as gso_segs can be more than current ring size
+	 */
+	return skb_shinfo(skb)->gso_segs < tnapi->tx_pending / 3;
+}
+
 static netdev_tx_t tg3_start_xmit(struct sk_buff *, struct net_device *);
 
 /* Use GSO to workaround all TSO packets that meet HW bug conditions
@@ -7936,14 +7942,19 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		 * vlan encapsulated.
 		 */
 		if (skb->protocol == htons(ETH_P_8021Q) ||
-		    skb->protocol == htons(ETH_P_8021AD))
-			return tg3_tso_bug(tp, tnapi, txq, skb);
+		    skb->protocol == htons(ETH_P_8021AD)) {
+			if (tg3_tso_bug_gso_check(tnapi, skb))
+				return tg3_tso_bug(tp, tnapi, txq, skb);
+			goto drop;
+		}
 
 		if (!skb_is_gso_v6(skb)) {
 			if (unlikely((ETH_HLEN + hdr_len) > 80) &&
-			    tg3_flag(tp, TSO_BUG))
-				return tg3_tso_bug(tp, tnapi, txq, skb);
-
+			    tg3_flag(tp, TSO_BUG)) {
+				if (tg3_tso_bug_gso_check(tnapi, skb))
+					return tg3_tso_bug(tp, tnapi, txq, skb);
+				goto drop;
+			}
 			ip_csum = iph->check;
 			ip_tot_len = iph->tot_len;
 			iph->check = 0;
@@ -8075,7 +8086,7 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (would_hit_hwbug) {
 		tg3_tx_skb_unmap(tnapi, tnapi->tx_prod, i);
 
-		if (mss) {
+		if (mss && tg3_tso_bug_gso_check(tnapi, skb)) {
 			/* If it's a TSO packet, do GSO instead of
 			 * allocating and copying to a large linear SKB
 			 */
@@ -12018,7 +12029,7 @@ static int tg3_set_eeprom(struct net_device *dev, struct ethtool_eeprom *eeprom,
 	int ret;
 	u32 offset, len, b_offset, odd_len;
 	u8 *buf;
-	__be32 start, end;
+	__be32 start = 0, end;
 
 	if (tg3_flag(tp, NO_NVRAM) ||
 	    eeprom->magic != TG3_EEPROM_MAGIC)
@@ -12076,7 +12087,7 @@ static int tg3_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		struct phy_device *phydev;
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
-		phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 		return phy_ethtool_gset(phydev, cmd);
 	}
 
@@ -12143,7 +12154,7 @@ static int tg3_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 		struct phy_device *phydev;
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
-		phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 		return phy_ethtool_sset(phydev, cmd);
 	}
 
@@ -12298,7 +12309,7 @@ static int tg3_nway_reset(struct net_device *dev)
 	if (tg3_flag(tp, USE_PHYLIB)) {
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
-		r = phy_start_aneg(tp->mdio_bus->phy_map[tp->phy_addr]);
+		r = phy_start_aneg(mdiobus_get_phy(tp->mdio_bus, tp->phy_addr));
 	} else {
 		u32 bmcr;
 
@@ -12416,7 +12427,7 @@ static int tg3_set_pauseparam(struct net_device *dev, struct ethtool_pauseparam
 		u32 newadv;
 		struct phy_device *phydev;
 
-		phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 
 		if (!(phydev->supported & SUPPORTED_Pause) ||
 		    (!(phydev->supported & SUPPORTED_Asym_Pause) &&
@@ -13926,7 +13937,7 @@ static int tg3_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 		struct phy_device *phydev;
 		if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED))
 			return -EAGAIN;
-		phydev = tp->mdio_bus->phy_map[tp->phy_addr];
+		phydev = mdiobus_get_phy(tp->mdio_bus, tp->phy_addr);
 		return phy_mii_ioctl(phydev, ifr, cmd);
 	}
 
@@ -17898,13 +17909,7 @@ static int tg3_init_one(struct pci_dev *pdev,
 		    tg3_bus_string(tp, str),
 		    dev->dev_addr);
 
-	if (tp->phy_flags & TG3_PHYFLG_IS_CONNECTED) {
-		struct phy_device *phydev;
-		phydev = tp->mdio_bus->phy_map[tp->phy_addr];
-		netdev_info(dev,
-			    "attached PHY driver [%s] (mii_bus:phy_addr=%s)\n",
-			    phydev->drv->name, dev_name(&phydev->dev));
-	} else {
+	if (!(tp->phy_flags & TG3_PHYFLG_IS_CONNECTED)) {
 		char *ethtype;
 
 		if (tp->phy_flags & TG3_PHYFLG_10_100_ONLY)
diff --git a/drivers/net/ethernet/broadcom/tg3.h b/drivers/net/ethernet/broadcom/tg3.h
index 31c9f8295953..3b5e98ecba00 100644
--- a/drivers/net/ethernet/broadcom/tg3.h
+++ b/drivers/net/ethernet/broadcom/tg3.h
@@ -3254,7 +3254,6 @@ struct tg3 {
 	int				pcie_readrq;
 
 	struct mii_bus			*mdio_bus;
-	int				mdio_irq[PHY_MAX_ADDR];
 	int				old_link;
 
 	u8				phy_addr;
diff --git a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
index 04b0d16b210e..95bc470ae441 100644
--- a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
+++ b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
@@ -987,7 +987,7 @@ bna_rxf_ucast_cfg_apply(struct bna_rxf *rxf)
 	if (!list_empty(&rxf->ucast_pending_add_q)) {
 		mac = list_first_entry(&rxf->ucast_pending_add_q,
 				       struct bna_mac, qe);
-		list_add_tail(&mac->qe, &rxf->ucast_active_q);
+		list_move_tail(&mac->qe, &rxf->ucast_active_q);
 		bna_bfi_ucast_req(rxf, mac, BFI_ENET_H2I_MAC_UCAST_ADD_REQ);
 		return 1;
 	}
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index 21a0cfc3e7ec..771cc267f217 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -542,39 +542,50 @@ bnad_cq_drop_packet(struct bnad *bnad, struct bna_rcb *rcb,
 }
 
 static void
-bnad_cq_setup_skb_frags(struct bna_rcb *rcb, struct sk_buff *skb,
-			u32 sop_ci, u32 nvecs, u32 last_fraglen)
+bnad_cq_setup_skb_frags(struct bna_ccb *ccb, struct sk_buff *skb, u32 nvecs)
 {
+	struct bna_rcb *rcb;
 	struct bnad *bnad;
-	u32 ci, vec, len, totlen = 0;
 	struct bnad_rx_unmap_q *unmap_q;
-	struct bnad_rx_unmap *unmap;
+	struct bna_cq_entry *cq, *cmpl;
+	u32 ci, pi, totlen = 0;
+
+	cq = ccb->sw_q;
+	pi = ccb->producer_index;
+	cmpl = &cq[pi];
 
+	rcb = bna_is_small_rxq(cmpl->rxq_id) ? ccb->rcb[1] : ccb->rcb[0];
 	unmap_q = rcb->unmap_q;
 	bnad = rcb->bnad;
+	ci = rcb->consumer_index;
 
 	/* prefetch header */
-	prefetch(page_address(unmap_q->unmap[sop_ci].page) +
-			unmap_q->unmap[sop_ci].page_offset);
+	prefetch(page_address(unmap_q->unmap[ci].page) +
+		 unmap_q->unmap[ci].page_offset);
+
+	while (nvecs--) {
+		struct bnad_rx_unmap *unmap;
+		u32 len;
 
-	for (vec = 1, ci = sop_ci; vec <= nvecs; vec++) {
 		unmap = &unmap_q->unmap[ci];
 		BNA_QE_INDX_INC(ci, rcb->q_depth);
 
 		dma_unmap_page(&bnad->pcidev->dev,
-				dma_unmap_addr(&unmap->vector, dma_addr),
-				unmap->vector.len, DMA_FROM_DEVICE);
+			       dma_unmap_addr(&unmap->vector, dma_addr),
+			       unmap->vector.len, DMA_FROM_DEVICE);
 
-		len = (vec == nvecs) ?
-			last_fraglen : unmap->vector.len;
+		len = ntohs(cmpl->length);
 		skb->truesize += unmap->vector.len;
 		totlen += len;
 
 		skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
-				unmap->page, unmap->page_offset, len);
+				   unmap->page, unmap->page_offset, len);
 
 		unmap->page = NULL;
 		unmap->vector.len = 0;
+
+		BNA_QE_INDX_INC(pi, ccb->q_depth);
+		cmpl = &cq[pi];
 	}
 
 	skb->len += totlen;
@@ -704,7 +715,7 @@ bnad_cq_process(struct bnad *bnad, struct bna_ccb *ccb, int budget)
 		if (BNAD_RXBUF_IS_SK_BUFF(unmap_q->type))
 			bnad_cq_setup_skb(bnad, skb, unmap, len);
 		else
-			bnad_cq_setup_skb_frags(rcb, skb, sop_ci, nvecs, len);
+			bnad_cq_setup_skb_frags(ccb, skb, nvecs);
 
 		rcb->rxq->rx_packets++;
 		rcb->rxq->rx_bytes += totlen;
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index 169059c92f80..50c94104f19c 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -19,6 +19,7 @@
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/interrupt.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -28,6 +29,7 @@
 #include <linux/phy.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/of_mdio.h>
 #include <linux/of_net.h>
 
@@ -439,12 +441,6 @@ static int macb_mii_init(struct macb *bp)
 	bp->mii_bus->parent = &bp->dev->dev;
 	pdata = dev_get_platdata(&bp->pdev->dev);
 
-	bp->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-	if (!bp->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
 	dev_set_drvdata(&bp->dev->dev, bp->mii_bus);
 
 	np = bp->pdev->dev.of_node;
@@ -469,9 +465,6 @@ static int macb_mii_init(struct macb *bp)
 				goto err_out_unregister_bus;
 		}
 	} else {
-		for (i = 0; i < PHY_MAX_ADDR; i++)
-			bp->mii_bus->irq[i] = PHY_POLL;
-
 		if (pdata)
 			bp->mii_bus->phy_mask = pdata->phy_mask;
 
@@ -479,7 +472,7 @@ static int macb_mii_init(struct macb *bp)
 	}
 
 	if (err)
-		goto err_out_free_mdio_irq;
+		goto err_out_free_mdiobus;
 
 	err = macb_mii_probe(bp->dev);
 	if (err)
@@ -489,8 +482,6 @@ static int macb_mii_init(struct macb *bp)
 
 err_out_unregister_bus:
 	mdiobus_unregister(bp->mii_bus);
-err_out_free_mdio_irq:
-	kfree(bp->mii_bus->irq);
 err_out_free_mdiobus:
 	mdiobus_free(bp->mii_bus);
 err_out:
@@ -1049,6 +1040,8 @@ static irqreturn_t macb_interrupt(int irq, void *dev_id)
 		/* close possible race with dev_close */
 		if (unlikely(!netif_running(dev))) {
 			queue_writel(queue, IDR, -1);
+			if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+				queue_writel(queue, ISR, -1);
 			break;
 		}
 
@@ -1570,6 +1563,8 @@ static void macb_reset_hw(struct macb *bp)
 	for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
 		queue_writel(queue, IDR, -1);
 		queue_readl(queue, ISR);
+		if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
+			queue_writel(queue, ISR, -1);
 	}
 }
 
@@ -2122,7 +2117,8 @@ static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
 	regs_buff[10] = macb_tx_dma(&bp->queues[0], tail);
 	regs_buff[11] = macb_tx_dma(&bp->queues[0], head);
 
-	regs_buff[12] = macb_or_gem_readl(bp, USRIO);
+	if (!(bp->caps & MACB_CAPS_USRIO_DISABLED))
+		regs_buff[12] = macb_or_gem_readl(bp, USRIO);
 	if (macb_is_gem(bp)) {
 		regs_buff[13] = gem_readl(bp, DMACFG);
 	}
@@ -2401,19 +2397,21 @@ static int macb_init(struct platform_device *pdev)
 		dev->hw_features &= ~NETIF_F_SG;
 	dev->features = dev->hw_features;
 
-	val = 0;
-	if (bp->phy_interface == PHY_INTERFACE_MODE_RGMII)
-		val = GEM_BIT(RGMII);
-	else if (bp->phy_interface == PHY_INTERFACE_MODE_RMII &&
-		 (bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
-		val = MACB_BIT(RMII);
-	else if (!(bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
-		val = MACB_BIT(MII);
+	if (!(bp->caps & MACB_CAPS_USRIO_DISABLED)) {
+		val = 0;
+		if (bp->phy_interface == PHY_INTERFACE_MODE_RGMII)
+			val = GEM_BIT(RGMII);
+		else if (bp->phy_interface == PHY_INTERFACE_MODE_RMII &&
+			 (bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
+			val = MACB_BIT(RMII);
+		else if (!(bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII))
+			val = MACB_BIT(MII);
 
-	if (bp->caps & MACB_CAPS_USRIO_HAS_CLKEN)
-		val |= MACB_BIT(CLKEN);
+		if (bp->caps & MACB_CAPS_USRIO_HAS_CLKEN)
+			val |= MACB_BIT(CLKEN);
 
-	macb_or_gem_writel(bp, USRIO, val);
+		macb_or_gem_writel(bp, USRIO, val);
+	}
 
 	/* Set MII management clock divider */
 	val = macb_mdc_clk_div(bp);
@@ -2776,6 +2774,11 @@ static const struct macb_config emac_config = {
 	.init = at91ether_init,
 };
 
+static const struct macb_config np4_config = {
+	.caps = MACB_CAPS_USRIO_DISABLED,
+	.clk_init = macb_clk_init,
+	.init = macb_init,
+};
 
 static const struct macb_config zynqmp_config = {
 	.caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE | MACB_CAPS_JUMBO,
@@ -2796,6 +2799,7 @@ static const struct of_device_id macb_dt_ids[] = {
 	{ .compatible = "cdns,at32ap7000-macb" },
 	{ .compatible = "cdns,at91sam9260-macb", .data = &at91sam9260_config },
 	{ .compatible = "cdns,macb" },
+	{ .compatible = "cdns,np4-macb", .data = &np4_config },
 	{ .compatible = "cdns,pc302-gem", .data = &pc302gem_config },
 	{ .compatible = "cdns,gem", .data = &pc302gem_config },
 	{ .compatible = "atmel,sama5d2-gem", .data = &sama5d2_config },
@@ -2817,8 +2821,9 @@ static int macb_probe(struct platform_device *pdev)
 					      = 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, *tx_clk;
+	struct clk *pclk, *hclk = NULL, *tx_clk = NULL;
 	unsigned int queue_mask, num_queues;
 	struct macb_platform_data *pdata;
 	bool native_io;
@@ -2904,6 +2909,16 @@ static int macb_probe(struct platform_device *pdev)
 	else
 		macb_get_hwaddr(bp);
 
+	/* Power up the PHY if there is a GPIO reset */
+	phy_node =  of_get_next_available_child(np, NULL);
+	if (phy_node) {
+		int gpio = of_get_named_gpio(phy_node, "reset-gpios", 0);
+		if (gpio_is_valid(gpio))
+			bp->reset_gpio = gpio_to_desc(gpio);
+		gpiod_set_value(bp->reset_gpio, GPIOD_OUT_HIGH);
+	}
+	of_node_put(phy_node);
+
 	err = of_get_phy_mode(np);
 	if (err < 0) {
 		pdata = dev_get_platdata(&pdev->dev);
@@ -2937,8 +2952,7 @@ static int macb_probe(struct platform_device *pdev)
 		    dev->base_addr, dev->irq, dev->dev_addr);
 
 	phydev = bp->phy_dev;
-	netdev_info(dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		    phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
 
 	return 0;
 
@@ -2968,8 +2982,11 @@ static int macb_remove(struct platform_device *pdev)
 		if (bp->phy_dev)
 			phy_disconnect(bp->phy_dev);
 		mdiobus_unregister(bp->mii_bus);
-		kfree(bp->mii_bus->irq);
 		mdiobus_free(bp->mii_bus);
+
+		/* Shutdown the PHY if there is a GPIO reset */
+		gpiod_set_value(bp->reset_gpio, GPIOD_OUT_LOW);
+
 		unregister_netdev(dev);
 		clk_disable_unprepare(bp->tx_clk);
 		clk_disable_unprepare(bp->hclk);
diff --git a/drivers/net/ethernet/cadence/macb.h b/drivers/net/ethernet/cadence/macb.h
index d83b0db77821..0d4ecfcd60b7 100644
--- a/drivers/net/ethernet/cadence/macb.h
+++ b/drivers/net/ethernet/cadence/macb.h
@@ -400,6 +400,7 @@
 #define MACB_CAPS_USRIO_HAS_CLKEN		0x00000002
 #define MACB_CAPS_USRIO_DEFAULT_IS_MII		0x00000004
 #define MACB_CAPS_NO_GIGABIT_HALF		0x00000008
+#define MACB_CAPS_USRIO_DISABLED		0x00000010
 #define MACB_CAPS_FIFO_MODE			0x10000000
 #define MACB_CAPS_GIGABIT_MODE_AVAILABLE	0x20000000
 #define MACB_CAPS_SG_DISABLED			0x40000000
@@ -829,6 +830,7 @@ struct macb {
 	unsigned int		dma_burst_length;
 
 	phy_interface_t		phy_interface;
+	struct gpio_desc	*reset_gpio;
 
 	/* AT91RM9200 transmit */
 	struct sk_buff *skb;			/* holds skb until xmit interrupt completes */
diff --git a/drivers/net/ethernet/cavium/liquidio/lio_main.c b/drivers/net/ethernet/cavium/liquidio/lio_main.c
index b89504405b72..34d269cd5579 100644
--- a/drivers/net/ethernet/cavium/liquidio/lio_main.c
+++ b/drivers/net/ethernet/cavium/liquidio/lio_main.c
@@ -1526,7 +1526,6 @@ static int liquidio_ptp_gettime(struct ptp_clock_info *ptp,
 				struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 	struct lio *lio = container_of(ptp, struct lio, ptp_info);
 	struct octeon_device *oct = (struct octeon_device *)lio->oct_dev;
@@ -1536,8 +1535,7 @@ static int liquidio_ptp_gettime(struct ptp_clock_info *ptp,
 	ns += lio->ptp_adjust;
 	spin_unlock_irqrestore(&lio->ptp_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 
 	return 0;
 }
@@ -1685,7 +1683,7 @@ static int octeon_setup_droq(struct octeon_device *oct, int q_no, int num_descs,
 	dev_dbg(&oct->pci_dev->dev, "Creating Droq: %d\n", q_no);
 	/* droq creation and local register settings. */
 	ret_val = octeon_create_droq(oct, q_no, num_descs, desc_size, app_ctx);
-	if (ret_val == -1)
+	if (ret_val < 0)
 		return ret_val;
 
 	if (ret_val == 1) {
@@ -2526,7 +2524,7 @@ static void handle_timestamp(struct octeon_device *oct,
 
 	octeon_swap_8B_data(&resp->timestamp, 1);
 
-	if (unlikely((skb_shinfo(skb)->tx_flags | SKBTX_IN_PROGRESS) != 0)) {
+	if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) != 0)) {
 		struct skb_shared_hwtstamps ts;
 		u64 ns = resp->timestamp;
 
diff --git a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
index 4dba86eaa045..174072b3740b 100644
--- a/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
+++ b/drivers/net/ethernet/cavium/liquidio/octeon_droq.c
@@ -983,5 +983,5 @@ int octeon_create_droq(struct octeon_device *oct,
 
 create_droq_fail:
 	octeon_delete_droq(oct, q_no);
-	return -1;
+	return -ENOMEM;
 }
diff --git a/drivers/net/ethernet/cavium/thunder/nic.h b/drivers/net/ethernet/cavium/thunder/nic.h
index 39ca6744a4e6..34e9acea8747 100644
--- a/drivers/net/ethernet/cavium/thunder/nic.h
+++ b/drivers/net/ethernet/cavium/thunder/nic.h
@@ -116,6 +116,15 @@
 #define NIC_PF_INTR_ID_MBOX0		8
 #define NIC_PF_INTR_ID_MBOX1		9
 
+/* Minimum FIFO level before all packets for the CQ are dropped
+ *
+ * This value ensures that once a packet has been "accepted"
+ * for reception it will not get dropped due to non-availability
+ * of CQ descriptor. An errata in HW mandates this value to be
+ * atleast 0x100.
+ */
+#define NICPF_CQM_MIN_DROP_LEVEL       0x100
+
 /* Global timer for CQ timer thresh interrupts
  * Calculated for SCLK of 700Mhz
  * value written should be a 1/16th of what is expected
@@ -265,6 +274,7 @@ struct nicvf {
 	u8			tns_mode:1;
 	u8			sqs_mode:1;
 	u8			loopback_supported:1;
+	bool			hw_tso;
 	u16			mtu;
 	struct queue_set	*qs;
 #define	MAX_SQS_PER_VF_SINGLE_NODE		5
@@ -489,6 +499,11 @@ static inline int nic_get_node_id(struct pci_dev *pdev)
 	return ((addr >> NIC_NODE_ID_SHIFT) & NIC_NODE_ID_MASK);
 }
 
+static inline bool pass1_silicon(struct pci_dev *pdev)
+{
+	return pdev->revision < 8;
+}
+
 int nicvf_set_real_num_queues(struct net_device *netdev,
 			      int tx_queues, int rx_queues);
 int nicvf_open(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 5f24d11cb16a..95f17f8cadac 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nic_main.c
@@ -54,11 +54,6 @@ struct nicpf {
 	bool			irq_allocated[NIC_PF_MSIX_VECTORS];
 };
 
-static inline bool pass1_silicon(struct nicpf *nic)
-{
-	return nic->pdev->revision < 8;
-}
-
 /* Supported devices */
 static const struct pci_device_id nic_id_table[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_CAVIUM, PCI_DEVICE_ID_THUNDER_NIC_PF) },
@@ -122,7 +117,7 @@ static void nic_send_msg_to_vf(struct nicpf *nic, int vf, union nic_mbx *mbx)
 	 * when PF writes to MBOX(1), in next revisions when
 	 * PF writes to MBOX(0)
 	 */
-	if (pass1_silicon(nic)) {
+	if (pass1_silicon(nic->pdev)) {
 		/* see the comment for nic_reg_write()/nic_reg_read()
 		 * functions above
 		 */
@@ -309,6 +304,7 @@ static void nic_set_lmac_vf_mapping(struct nicpf *nic)
 static void nic_init_hw(struct nicpf *nic)
 {
 	int i;
+	u64 cqm_cfg;
 
 	/* Enable NIC HW block */
 	nic_reg_write(nic, NIC_PF_CFG, 0x3);
@@ -345,6 +341,11 @@ static void nic_init_hw(struct nicpf *nic)
 	/* Enable VLAN ethertype matching and stripping */
 	nic_reg_write(nic, NIC_PF_RX_ETYPE_0_7,
 		      (2 << 19) | (ETYPE_ALG_VLAN_STRIP << 16) | ETH_P_8021Q);
+
+	/* Check if HW expected value is higher (could be in future chips) */
+	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);
 }
 
 /* Channel parse index configuration */
@@ -397,7 +398,7 @@ static void nic_config_cpi(struct nicpf *nic, struct cpi_cfg_msg *cfg)
 			padd = cpi % 8; /* 3 bits CS out of 6bits DSCP */
 
 		/* Leave RSS_SIZE as '0' to disable RSS */
-		if (pass1_silicon(nic)) {
+		if (pass1_silicon(nic->pdev)) {
 			nic_reg_write(nic, NIC_PF_CPI_0_2047_CFG | (cpi << 3),
 				      (vnic << 24) | (padd << 16) |
 				      (rssi_base + rssi));
@@ -467,7 +468,7 @@ static void nic_config_rss(struct nicpf *nic, struct rss_cfg_msg *cfg)
 	}
 
 	cpi_base = nic->cpi_base[cfg->vf_id];
-	if (pass1_silicon(nic))
+	if (pass1_silicon(nic->pdev))
 		idx_addr = NIC_PF_CPI_0_2047_CFG;
 	else
 		idx_addr = NIC_PF_MPI_0_2047_CFG;
diff --git a/drivers/net/ethernet/cavium/thunder/nic_reg.h b/drivers/net/ethernet/cavium/thunder/nic_reg.h
index dd536be20193..afb10e326b4f 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_reg.h
+++ b/drivers/net/ethernet/cavium/thunder/nic_reg.h
@@ -21,7 +21,7 @@
 #define   NIC_PF_TCP_TIMER			(0x0060)
 #define   NIC_PF_BP_CFG				(0x0080)
 #define   NIC_PF_RRM_CFG			(0x0088)
-#define   NIC_PF_CQM_CF				(0x00A0)
+#define   NIC_PF_CQM_CFG			(0x00A0)
 #define   NIC_PF_CNM_CF				(0x00A8)
 #define   NIC_PF_CNM_STATUS			(0x00B0)
 #define   NIC_PF_CQ_AVG_CFG			(0x00C0)
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_main.c b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
index dde8dc720cd3..a009bc30dc4d 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
@@ -525,14 +525,22 @@ static void nicvf_snd_pkt_handler(struct net_device *netdev,
 		   __func__, cqe_tx->sq_qs, cqe_tx->sq_idx,
 		   cqe_tx->sqe_ptr, hdr->subdesc_cnt);
 
-	nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
 	nicvf_check_cqe_tx_errs(nic, cq, cqe_tx);
 	skb = (struct sk_buff *)sq->skbuff[cqe_tx->sqe_ptr];
-	/* For TSO offloaded packets only one head SKB needs to be freed */
+	/* For TSO offloaded packets only one SQE will have a valid SKB */
 	if (skb) {
+		nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
 		prefetch(skb);
 		dev_consume_skb_any(skb);
 		sq->skbuff[cqe_tx->sqe_ptr] = (u64)NULL;
+	} else {
+		/* In case of HW TSO, HW sends a CQE for each segment of a TSO
+		 * packet instead of a single CQE for the whole TSO packet
+		 * transmitted. Each of this CQE points to the same SQE, so
+		 * avoid freeing same SQE multiple times.
+		 */
+		if (!nic->hw_tso)
+			nicvf_put_sq_desc(sq, hdr->subdesc_cnt + 1);
 	}
 }
 
@@ -566,8 +574,7 @@ static inline void nicvf_set_rxhash(struct net_device *netdev,
 
 static void nicvf_rcv_pkt_handler(struct net_device *netdev,
 				  struct napi_struct *napi,
-				  struct cmp_queue *cq,
-				  struct cqe_rx_t *cqe_rx, int cqe_type)
+				  struct cqe_rx_t *cqe_rx)
 {
 	struct sk_buff *skb;
 	struct nicvf *nic = netdev_priv(netdev);
@@ -583,7 +590,7 @@ static void nicvf_rcv_pkt_handler(struct net_device *netdev,
 	}
 
 	/* Check for errors */
-	err = nicvf_check_cqe_rx_errs(nic, cq, cqe_rx);
+	err = nicvf_check_cqe_rx_errs(nic, cqe_rx);
 	if (err && !cqe_rx->rb_cnt)
 		return;
 
@@ -674,8 +681,7 @@ loop:
 			   cq_idx, cq_desc->cqe_type);
 		switch (cq_desc->cqe_type) {
 		case CQE_TYPE_RX:
-			nicvf_rcv_pkt_handler(netdev, napi, cq,
-					      cq_desc, CQE_TYPE_RX);
+			nicvf_rcv_pkt_handler(netdev, napi, cq_desc);
 			work_done++;
 		break;
 		case CQE_TYPE_SEND:
@@ -1117,7 +1123,6 @@ int nicvf_stop(struct net_device *netdev)
 
 	/* Clear multiqset info */
 	nic->pnicvf = nic;
-	nic->sqs_count = 0;
 
 	return 0;
 }
@@ -1346,6 +1351,9 @@ void nicvf_update_stats(struct nicvf *nic)
 	drv_stats->tx_frames_ok = stats->tx_ucast_frames_ok +
 				  stats->tx_bcast_frames_ok +
 				  stats->tx_mcast_frames_ok;
+	drv_stats->rx_frames_ok = stats->rx_ucast_frames +
+				  stats->rx_bcast_frames +
+				  stats->rx_mcast_frames;
 	drv_stats->rx_drops = stats->rx_drop_red +
 			      stats->rx_drop_overrun;
 	drv_stats->tx_drops = stats->tx_drops;
@@ -1530,6 +1538,9 @@ static int nicvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	nicvf_send_vf_struct(nic);
 
+	if (!pass1_silicon(nic->pdev))
+		nic->hw_tso = true;
+
 	/* Check if this VF is in QS only mode */
 	if (nic->sqs_mode)
 		return 0;
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
index 206b6a71a545..767347b1f631 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
@@ -18,14 +18,6 @@
 #include "q_struct.h"
 #include "nicvf_queues.h"
 
-struct rbuf_info {
-	struct page *page;
-	void	*data;
-	u64	offset;
-};
-
-#define GET_RBUF_INFO(x) ((struct rbuf_info *)(x - NICVF_RCV_BUF_ALIGN_BYTES))
-
 /* Poll a register for a specific value */
 static int nicvf_poll_reg(struct nicvf *nic, int qidx,
 			  u64 reg, int bit_pos, int bits, int val)
@@ -86,8 +78,6 @@ static void nicvf_free_q_desc_mem(struct nicvf *nic, struct q_desc_mem *dmem)
 static inline int nicvf_alloc_rcv_buffer(struct nicvf *nic, gfp_t gfp,
 					 u32 buf_len, u64 **rbuf)
 {
-	u64 data;
-	struct rbuf_info *rinfo;
 	int order = get_order(buf_len);
 
 	/* Check if request can be accomodated in previous allocated page */
@@ -113,46 +103,28 @@ static inline int nicvf_alloc_rcv_buffer(struct nicvf *nic, gfp_t gfp,
 		nic->rb_page_offset = 0;
 	}
 
-	data = (u64)page_address(nic->rb_page) + nic->rb_page_offset;
-
-	/* Align buffer addr to cache line i.e 128 bytes */
-	rinfo = (struct rbuf_info *)(data + NICVF_RCV_BUF_ALIGN_LEN(data));
-	/* Save page address for reference updation */
-	rinfo->page = nic->rb_page;
-	/* Store start address for later retrieval */
-	rinfo->data = (void *)data;
-	/* Store alignment offset */
-	rinfo->offset = NICVF_RCV_BUF_ALIGN_LEN(data);
+	*rbuf = (u64 *)((u64)page_address(nic->rb_page) + nic->rb_page_offset);
 
-	data += rinfo->offset;
-
-	/* Give next aligned address to hw for DMA */
-	*rbuf = (u64 *)(data + NICVF_RCV_BUF_ALIGN_BYTES);
 	return 0;
 }
 
-/* Retrieve actual buffer start address and build skb for received packet */
+/* Build skb around receive buffer */
 static struct sk_buff *nicvf_rb_ptr_to_skb(struct nicvf *nic,
 					   u64 rb_ptr, int len)
 {
+	void *data;
 	struct sk_buff *skb;
-	struct rbuf_info *rinfo;
 
-	rb_ptr = (u64)phys_to_virt(rb_ptr);
-	/* Get buffer start address and alignment offset */
-	rinfo = GET_RBUF_INFO(rb_ptr);
+	data = phys_to_virt(rb_ptr);
 
 	/* Now build an skb to give to stack */
-	skb = build_skb(rinfo->data, RCV_FRAG_LEN);
+	skb = build_skb(data, RCV_FRAG_LEN);
 	if (!skb) {
-		put_page(rinfo->page);
+		put_page(virt_to_page(data));
 		return NULL;
 	}
 
-	/* Set correct skb->data */
-	skb_reserve(skb, rinfo->offset + NICVF_RCV_BUF_ALIGN_BYTES);
-
-	prefetch((void *)rb_ptr);
+	prefetch(skb->data);
 	return skb;
 }
 
@@ -196,7 +168,6 @@ static void nicvf_free_rbdr(struct nicvf *nic, struct rbdr *rbdr)
 	int head, tail;
 	u64 buf_addr;
 	struct rbdr_entry_t *desc;
-	struct rbuf_info *rinfo;
 
 	if (!rbdr)
 		return;
@@ -212,16 +183,14 @@ static void nicvf_free_rbdr(struct nicvf *nic, struct rbdr *rbdr)
 	while (head != tail) {
 		desc = GET_RBDR_DESC(rbdr, head);
 		buf_addr = desc->buf_addr << NICVF_RCV_BUF_ALIGN;
-		rinfo = GET_RBUF_INFO((u64)phys_to_virt(buf_addr));
-		put_page(rinfo->page);
+		put_page(virt_to_page(phys_to_virt(buf_addr)));
 		head++;
 		head &= (rbdr->dmem.q_len - 1);
 	}
 	/* Free SKB of tail desc */
 	desc = GET_RBDR_DESC(rbdr, tail);
 	buf_addr = desc->buf_addr << NICVF_RCV_BUF_ALIGN;
-	rinfo = GET_RBUF_INFO((u64)phys_to_virt(buf_addr));
-	put_page(rinfo->page);
+	put_page(virt_to_page(phys_to_virt(buf_addr)));
 
 	/* Free RBDR ring */
 	nicvf_free_q_desc_mem(nic, &rbdr->dmem);
@@ -330,7 +299,7 @@ static int nicvf_init_cmp_queue(struct nicvf *nic,
 		return err;
 
 	cq->desc = cq->dmem.base;
-	cq->thresh = CMP_QUEUE_CQE_THRESH;
+	cq->thresh = pass1_silicon(nic->pdev) ? 0 : CMP_QUEUE_CQE_THRESH;
 	nic->cq_coalesce_usecs = (CMP_QUEUE_TIMER_THRESH * 0.05) - 1;
 
 	return 0;
@@ -956,7 +925,7 @@ static int nicvf_sq_subdesc_required(struct nicvf *nic, struct sk_buff *skb)
 {
 	int subdesc_cnt = MIN_SQ_DESC_PER_PKT_XMIT;
 
-	if (skb_shinfo(skb)->gso_size) {
+	if (skb_shinfo(skb)->gso_size && !nic->hw_tso) {
 		subdesc_cnt = nicvf_tso_count_subdescs(skb);
 		return subdesc_cnt;
 	}
@@ -971,7 +940,7 @@ static int nicvf_sq_subdesc_required(struct nicvf *nic, struct sk_buff *skb)
  * First subdescriptor for every send descriptor.
  */
 static inline void
-nicvf_sq_add_hdr_subdesc(struct snd_queue *sq, int qentry,
+nicvf_sq_add_hdr_subdesc(struct nicvf *nic, struct snd_queue *sq, int qentry,
 			 int subdesc_cnt, struct sk_buff *skb, int len)
 {
 	int proto;
@@ -1007,6 +976,15 @@ nicvf_sq_add_hdr_subdesc(struct snd_queue *sq, int qentry,
 			break;
 		}
 	}
+
+	if (nic->hw_tso && skb_shinfo(skb)->gso_size) {
+		hdr->tso = 1;
+		hdr->tso_start = skb_transport_offset(skb) + tcp_hdrlen(skb);
+		hdr->tso_max_paysize = skb_shinfo(skb)->gso_size;
+		/* For non-tunneled pkts, point this to L2 ethertype */
+		hdr->inner_l3_offset = skb_network_offset(skb) - 2;
+		nic->drv_stats.tx_tso++;
+	}
 }
 
 /* SQ GATHER subdescriptor
@@ -1076,7 +1054,7 @@ static int nicvf_sq_append_tso(struct nicvf *nic, struct snd_queue *sq,
 			data_left -= size;
 			tso_build_data(skb, &tso, size);
 		}
-		nicvf_sq_add_hdr_subdesc(sq, hdr_qentry,
+		nicvf_sq_add_hdr_subdesc(nic, sq, hdr_qentry,
 					 seg_subdescs - 1, skb, seg_len);
 		sq->skbuff[hdr_qentry] = (u64)NULL;
 		qentry = nicvf_get_nxt_sqentry(sq, qentry);
@@ -1129,11 +1107,12 @@ int nicvf_sq_append_skb(struct nicvf *nic, struct sk_buff *skb)
 	qentry = nicvf_get_sq_desc(sq, subdesc_cnt);
 
 	/* Check if its a TSO packet */
-	if (skb_shinfo(skb)->gso_size)
+	if (skb_shinfo(skb)->gso_size && !nic->hw_tso)
 		return nicvf_sq_append_tso(nic, sq, sq_num, qentry, skb);
 
 	/* Add SQ header subdesc */
-	nicvf_sq_add_hdr_subdesc(sq, qentry, subdesc_cnt - 1, skb, skb->len);
+	nicvf_sq_add_hdr_subdesc(nic, sq, qentry, subdesc_cnt - 1,
+				 skb, skb->len);
 
 	/* Add SQ gather subdescs */
 	qentry = nicvf_get_nxt_sqentry(sq, qentry);
@@ -1234,153 +1213,93 @@ struct sk_buff *nicvf_get_rcv_skb(struct nicvf *nic, struct cqe_rx_t *cqe_rx)
 	return skb;
 }
 
-/* Enable interrupt */
-void nicvf_enable_intr(struct nicvf *nic, int int_type, int q_idx)
+static u64 nicvf_int_type_to_mask(int int_type, int q_idx)
 {
 	u64 reg_val;
 
-	reg_val = nicvf_reg_read(nic, NIC_VF_ENA_W1S);
-
 	switch (int_type) {
 	case NICVF_INTR_CQ:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
 		break;
 	case NICVF_INTR_SQ:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
 		break;
 	case NICVF_INTR_RBDR:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
+		reg_val = ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
 		break;
 	case NICVF_INTR_PKT_DROP:
-		reg_val |= (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
+		reg_val = (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
 		break;
 	case NICVF_INTR_TCP_TIMER:
-		reg_val |= (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
+		reg_val = (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
 		break;
 	case NICVF_INTR_MBOX:
-		reg_val |= (1ULL << NICVF_INTR_MBOX_SHIFT);
+		reg_val = (1ULL << NICVF_INTR_MBOX_SHIFT);
 		break;
 	case NICVF_INTR_QS_ERR:
-		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
+		reg_val = (1ULL << NICVF_INTR_QS_ERR_SHIFT);
 		break;
 	default:
-		netdev_err(nic->netdev,
-			   "Failed to enable interrupt: unknown type\n");
-		break;
+		reg_val = 0;
 	}
 
-	nicvf_reg_write(nic, NIC_VF_ENA_W1S, reg_val);
+	return reg_val;
+}
+
+/* Enable interrupt */
+void nicvf_enable_intr(struct nicvf *nic, int int_type, int q_idx)
+{
+	u64 mask = nicvf_int_type_to_mask(int_type, q_idx);
+
+	if (!mask) {
+		netdev_dbg(nic->netdev,
+			   "Failed to enable interrupt: unknown type\n");
+		return;
+	}
+	nicvf_reg_write(nic, NIC_VF_ENA_W1S,
+			nicvf_reg_read(nic, NIC_VF_ENA_W1S) | mask);
 }
 
 /* Disable interrupt */
 void nicvf_disable_intr(struct nicvf *nic, int int_type, int q_idx)
 {
-	u64 reg_val = 0;
+	u64 mask = nicvf_int_type_to_mask(int_type, q_idx);
 
-	switch (int_type) {
-	case NICVF_INTR_CQ:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
-		break;
-	case NICVF_INTR_SQ:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
-		break;
-	case NICVF_INTR_RBDR:
-		reg_val |= ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
-		break;
-	case NICVF_INTR_PKT_DROP:
-		reg_val |= (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
-		break;
-	case NICVF_INTR_TCP_TIMER:
-		reg_val |= (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
-		break;
-	case NICVF_INTR_MBOX:
-		reg_val |= (1ULL << NICVF_INTR_MBOX_SHIFT);
-		break;
-	case NICVF_INTR_QS_ERR:
-		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
-		break;
-	default:
-		netdev_err(nic->netdev,
+	if (!mask) {
+		netdev_dbg(nic->netdev,
 			   "Failed to disable interrupt: unknown type\n");
-		break;
+		return;
 	}
 
-	nicvf_reg_write(nic, NIC_VF_ENA_W1C, reg_val);
+	nicvf_reg_write(nic, NIC_VF_ENA_W1C, mask);
 }
 
 /* Clear interrupt */
 void nicvf_clear_intr(struct nicvf *nic, int int_type, int q_idx)
 {
-	u64 reg_val = 0;
+	u64 mask = nicvf_int_type_to_mask(int_type, q_idx);
 
-	switch (int_type) {
-	case NICVF_INTR_CQ:
-		reg_val = ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
-		break;
-	case NICVF_INTR_SQ:
-		reg_val = ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
-		break;
-	case NICVF_INTR_RBDR:
-		reg_val = ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
-		break;
-	case NICVF_INTR_PKT_DROP:
-		reg_val = (1ULL << NICVF_INTR_PKT_DROP_SHIFT);
-		break;
-	case NICVF_INTR_TCP_TIMER:
-		reg_val = (1ULL << NICVF_INTR_TCP_TIMER_SHIFT);
-		break;
-	case NICVF_INTR_MBOX:
-		reg_val = (1ULL << NICVF_INTR_MBOX_SHIFT);
-		break;
-	case NICVF_INTR_QS_ERR:
-		reg_val |= (1ULL << NICVF_INTR_QS_ERR_SHIFT);
-		break;
-	default:
-		netdev_err(nic->netdev,
+	if (!mask) {
+		netdev_dbg(nic->netdev,
 			   "Failed to clear interrupt: unknown type\n");
-		break;
+		return;
 	}
 
-	nicvf_reg_write(nic, NIC_VF_INT, reg_val);
+	nicvf_reg_write(nic, NIC_VF_INT, mask);
 }
 
 /* Check if interrupt is enabled */
 int nicvf_is_intr_enabled(struct nicvf *nic, int int_type, int q_idx)
 {
-	u64 reg_val;
-	u64 mask = 0xff;
-
-	reg_val = nicvf_reg_read(nic, NIC_VF_ENA_W1S);
-
-	switch (int_type) {
-	case NICVF_INTR_CQ:
-		mask = ((1ULL << q_idx) << NICVF_INTR_CQ_SHIFT);
-		break;
-	case NICVF_INTR_SQ:
-		mask = ((1ULL << q_idx) << NICVF_INTR_SQ_SHIFT);
-		break;
-	case NICVF_INTR_RBDR:
-		mask = ((1ULL << q_idx) << NICVF_INTR_RBDR_SHIFT);
-		break;
-	case NICVF_INTR_PKT_DROP:
-		mask = NICVF_INTR_PKT_DROP_MASK;
-		break;
-	case NICVF_INTR_TCP_TIMER:
-		mask = NICVF_INTR_TCP_TIMER_MASK;
-		break;
-	case NICVF_INTR_MBOX:
-		mask = NICVF_INTR_MBOX_MASK;
-		break;
-	case NICVF_INTR_QS_ERR:
-		mask = NICVF_INTR_QS_ERR_MASK;
-		break;
-	default:
-		netdev_err(nic->netdev,
+	u64 mask = nicvf_int_type_to_mask(int_type, q_idx);
+	/* If interrupt type is unknown, we treat it disabled. */
+	if (!mask) {
+		netdev_dbg(nic->netdev,
 			   "Failed to check interrupt enable: unknown type\n");
-		break;
+		return 0;
 	}
 
-	return (reg_val & mask);
+	return mask & nicvf_reg_read(nic, NIC_VF_ENA_W1S);
 }
 
 void nicvf_update_rq_stats(struct nicvf *nic, int rq_idx)
@@ -1410,16 +1329,12 @@ void nicvf_update_sq_stats(struct nicvf *nic, int sq_idx)
 }
 
 /* Check for errors in the receive cmp.queue entry */
-int nicvf_check_cqe_rx_errs(struct nicvf *nic,
-			    struct cmp_queue *cq, struct cqe_rx_t *cqe_rx)
+int nicvf_check_cqe_rx_errs(struct nicvf *nic, struct cqe_rx_t *cqe_rx)
 {
 	struct nicvf_hw_stats *stats = &nic->hw_stats;
-	struct nicvf_drv_stats *drv_stats = &nic->drv_stats;
 
-	if (!cqe_rx->err_level && !cqe_rx->err_opcode) {
-		drv_stats->rx_frames_ok++;
+	if (!cqe_rx->err_level && !cqe_rx->err_opcode)
 		return 0;
-	}
 
 	if (netif_msg_rx_err(nic))
 		netdev_err(nic->netdev,
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.h b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
index 033e8306e91c..6673e1133523 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.h
@@ -75,7 +75,7 @@
  */
 #define CMP_QSIZE		CMP_QUEUE_SIZE2
 #define CMP_QUEUE_LEN		(1ULL << (CMP_QSIZE + 10))
-#define CMP_QUEUE_CQE_THRESH	0
+#define CMP_QUEUE_CQE_THRESH	(NAPI_POLL_WEIGHT / 2)
 #define CMP_QUEUE_TIMER_THRESH	80 /* ~2usec */
 
 #define RBDR_SIZE		RBDR_SIZE0
@@ -83,10 +83,8 @@
 #define MAX_RCV_BUF_COUNT	(1ULL << (RBDR_SIZE6 + 13))
 #define RBDR_THRESH		(RCV_BUF_COUNT / 2)
 #define DMA_BUFFER_LEN		2048 /* In multiples of 128bytes */
-#define RCV_FRAG_LEN	(SKB_DATA_ALIGN(DMA_BUFFER_LEN + NET_SKB_PAD) + \
-			 SKB_DATA_ALIGN(sizeof(struct skb_shared_info)) + \
-			 (NICVF_RCV_BUF_ALIGN_BYTES * 2))
-#define RCV_DATA_OFFSET		NICVF_RCV_BUF_ALIGN_BYTES
+#define RCV_FRAG_LEN	 (SKB_DATA_ALIGN(DMA_BUFFER_LEN + NET_SKB_PAD) + \
+			 SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
 
 #define MAX_CQES_FOR_TX		((SND_QUEUE_LEN / MIN_SQ_DESC_PER_PKT_XMIT) * \
 				 MAX_CQE_PER_PKT_XMIT)
@@ -108,10 +106,6 @@
 #define NICVF_SQ_BASE_ALIGN_BYTES	128  /* 7 bits */
 
 #define NICVF_ALIGNED_ADDR(ADDR, ALIGN_BYTES)	ALIGN(ADDR, ALIGN_BYTES)
-#define NICVF_ADDR_ALIGN_LEN(ADDR, BYTES)\
-	(NICVF_ALIGNED_ADDR(ADDR, BYTES) - BYTES)
-#define NICVF_RCV_BUF_ALIGN_LEN(X)\
-	(NICVF_ALIGNED_ADDR(X, NICVF_RCV_BUF_ALIGN_BYTES) - X)
 
 /* Queue enable/disable */
 #define NICVF_SQ_EN		BIT_ULL(19)
@@ -344,8 +338,7 @@ u64  nicvf_queue_reg_read(struct nicvf *nic,
 /* Stats */
 void nicvf_update_rq_stats(struct nicvf *nic, int rq_idx);
 void nicvf_update_sq_stats(struct nicvf *nic, int sq_idx);
-int nicvf_check_cqe_rx_errs(struct nicvf *nic,
-			    struct cmp_queue *cq, struct cqe_rx_t *cqe_rx);
+int nicvf_check_cqe_rx_errs(struct nicvf *nic, struct cqe_rx_t *cqe_rx);
 int nicvf_check_cqe_tx_errs(struct nicvf *nic,
 			    struct cmp_queue *cq, struct cqe_send_t *cqe_tx);
 #endif /* NICVF_QUEUES_H */
diff --git a/drivers/net/ethernet/cavium/thunder/q_struct.h b/drivers/net/ethernet/cavium/thunder/q_struct.h
index 3c1de97b1add..9e6d9876bfd0 100644
--- a/drivers/net/ethernet/cavium/thunder/q_struct.h
+++ b/drivers/net/ethernet/cavium/thunder/q_struct.h
@@ -545,25 +545,28 @@ struct sq_hdr_subdesc {
 	u64    subdesc_cnt:8;
 	u64    csum_l4:2;
 	u64    csum_l3:1;
-	u64    rsvd0:5;
+	u64    csum_inner_l4:2;
+	u64    csum_inner_l3:1;
+	u64    rsvd0:2;
 	u64    l4_offset:8;
 	u64    l3_offset:8;
 	u64    rsvd1:4;
 	u64    tot_len:20; /* W0 */
 
-	u64    tso_sdc_cont:8;
-	u64    tso_sdc_first:8;
-	u64    tso_l4_offset:8;
-	u64    tso_flags_last:12;
-	u64    tso_flags_first:12;
-	u64    rsvd2:2;
+	u64    rsvd2:24;
+	u64    inner_l4_offset:8;
+	u64    inner_l3_offset:8;
+	u64    tso_start:8;
+	u64    rsvd3:2;
 	u64    tso_max_paysize:14; /* W1 */
 #elif defined(__LITTLE_ENDIAN_BITFIELD)
 	u64    tot_len:20;
 	u64    rsvd1:4;
 	u64    l3_offset:8;
 	u64    l4_offset:8;
-	u64    rsvd0:5;
+	u64    rsvd0:2;
+	u64    csum_inner_l3:1;
+	u64    csum_inner_l4:2;
 	u64    csum_l3:1;
 	u64    csum_l4:2;
 	u64    subdesc_cnt:8;
@@ -574,12 +577,11 @@ struct sq_hdr_subdesc {
 	u64    subdesc_type:4; /* W0 */
 
 	u64    tso_max_paysize:14;
-	u64    rsvd2:2;
-	u64    tso_flags_first:12;
-	u64    tso_flags_last:12;
-	u64    tso_l4_offset:8;
-	u64    tso_sdc_first:8;
-	u64    tso_sdc_cont:8; /* W1 */
+	u64    rsvd3:2;
+	u64    tso_start:8;
+	u64    inner_l3_offset:8;
+	u64    inner_l4_offset:8;
+	u64    rsvd2:24; /* W1 */
 #endif
 };
 
diff --git a/drivers/net/ethernet/chelsio/Kconfig b/drivers/net/ethernet/chelsio/Kconfig
index a79813a17b6e..4d187f22c48b 100644
--- a/drivers/net/ethernet/chelsio/Kconfig
+++ b/drivers/net/ethernet/chelsio/Kconfig
@@ -65,13 +65,14 @@ config CHELSIO_T3
 	  will be called cxgb3.
 
 config CHELSIO_T4
-	tristate "Chelsio Communications T4/T5 Ethernet support"
+	tristate "Chelsio Communications T4/T5/T6 Ethernet support"
 	depends on PCI && (IPV6 || IPV6=n)
 	select FW_LOADER
 	select MDIO
 	---help---
-	  This driver supports Chelsio T4 and T5 based gigabit, 10Gb Ethernet
-	  adapter and T5 based 40Gb Ethernet adapter.
+	  This driver supports Chelsio T4, T5 & T6 based gigabit, 10Gb Ethernet
+	  adapter and T5/T6 based 40Gb and T6 based 25Gb, 50Gb and 100Gb
+	  Ethernet adapters.
 
 	  For general information about Chelsio and our products, visit
 	  our website at <http://www.chelsio.com>.
@@ -85,7 +86,7 @@ config CHELSIO_T4
 	  will be called cxgb4.
 
 config CHELSIO_T4_DCB
-	bool "Data Center Bridging (DCB) Support for Chelsio T4/T5 cards"
+	bool "Data Center Bridging (DCB) Support for Chelsio T4/T5/T6 cards"
 	default n
 	depends on CHELSIO_T4 && DCB
 	---help---
@@ -107,12 +108,12 @@ config CHELSIO_T4_FCOE
 	  If unsure, say N.
 
 config CHELSIO_T4VF
-	tristate "Chelsio Communications T4/T5 Virtual Function Ethernet support"
+	tristate "Chelsio Communications T4/T5/T6 Virtual Function Ethernet support"
 	depends on PCI
 	---help---
-	  This driver supports Chelsio T4 and T5 based gigabit, 10Gb Ethernet
-	  adapters and T5 based 40Gb Ethernet adapters with PCI-E SR-IOV Virtual
-	  Functions.
+	  This driver supports Chelsio T4, T5 & T6 based gigabit, 10Gb Ethernet
+	  adapters and T5/T6 based 40Gb and T6 based 25Gb, 50Gb and 100Gb
+	  Ethernet adapters with PCI-E SR-IOV Virtual Functions.
 
 	  For general information about Chelsio and our products, visit
 	  our website at <http://www.chelsio.com>.
diff --git a/drivers/net/ethernet/chelsio/cxgb/cphy.h b/drivers/net/ethernet/chelsio/cxgb/cphy.h
index a4d2a4c08d3f..bf43da6c6a63 100644
--- a/drivers/net/ethernet/chelsio/cxgb/cphy.h
+++ b/drivers/net/ethernet/chelsio/cxgb/cphy.h
@@ -137,7 +137,7 @@ static inline int simple_mdio_write(struct cphy *cphy, int reg,
 
 /* Convenience initializer */
 static inline void cphy_init(struct cphy *phy, struct net_device *dev,
-			     int phy_addr, struct cphy_ops *phy_ops,
+			     int phy_addr, const struct cphy_ops *phy_ops,
 			     const struct mdio_ops *mdio_ops)
 {
 	struct adapter *adapter = netdev_priv(dev);
diff --git a/drivers/net/ethernet/chelsio/cxgb/mv88e1xxx.c b/drivers/net/ethernet/chelsio/cxgb/mv88e1xxx.c
index 71018a4fdf15..76ce6e538326 100644
--- a/drivers/net/ethernet/chelsio/cxgb/mv88e1xxx.c
+++ b/drivers/net/ethernet/chelsio/cxgb/mv88e1xxx.c
@@ -337,7 +337,7 @@ static void mv88e1xxx_destroy(struct cphy *cphy)
 	kfree(cphy);
 }
 
-static struct cphy_ops mv88e1xxx_ops = {
+static const struct cphy_ops mv88e1xxx_ops = {
 	.destroy              = mv88e1xxx_destroy,
 	.reset                = mv88e1xxx_reset,
 	.interrupt_enable     = mv88e1xxx_interrupt_enable,
diff --git a/drivers/net/ethernet/chelsio/cxgb/mv88x201x.c b/drivers/net/ethernet/chelsio/cxgb/mv88x201x.c
index d0cf611551a1..7ddb301bcba0 100644
--- a/drivers/net/ethernet/chelsio/cxgb/mv88x201x.c
+++ b/drivers/net/ethernet/chelsio/cxgb/mv88x201x.c
@@ -195,7 +195,7 @@ static void mv88x201x_destroy(struct cphy *cphy)
 	kfree(cphy);
 }
 
-static struct cphy_ops mv88x201x_ops = {
+static const struct cphy_ops mv88x201x_ops = {
 	.destroy           = mv88x201x_destroy,
 	.reset             = mv88x201x_reset,
 	.interrupt_enable  = mv88x201x_interrupt_enable,
diff --git a/drivers/net/ethernet/chelsio/cxgb/my3126.c b/drivers/net/ethernet/chelsio/cxgb/my3126.c
index a683fd3bb624..d546f46c8ef7 100644
--- a/drivers/net/ethernet/chelsio/cxgb/my3126.c
+++ b/drivers/net/ethernet/chelsio/cxgb/my3126.c
@@ -154,7 +154,7 @@ static void my3126_destroy(struct cphy *cphy)
 	kfree(cphy);
 }
 
-static struct cphy_ops my3126_ops = {
+static const struct cphy_ops my3126_ops = {
 	.destroy		= my3126_destroy,
 	.reset			= my3126_reset,
 	.interrupt_enable	= my3126_interrupt_enable,
diff --git a/drivers/net/ethernet/chelsio/cxgb/pm3393.c b/drivers/net/ethernet/chelsio/cxgb/pm3393.c
index ec5e05052d99..eb462d7db427 100644
--- a/drivers/net/ethernet/chelsio/cxgb/pm3393.c
+++ b/drivers/net/ethernet/chelsio/cxgb/pm3393.c
@@ -570,7 +570,7 @@ static void pm3393_destroy(struct cmac *cmac)
 	kfree(cmac);
 }
 
-static struct cmac_ops pm3393_ops = {
+static const struct cmac_ops pm3393_ops = {
 	.destroy                 = pm3393_destroy,
 	.reset                   = pm3393_reset,
 	.interrupt_enable        = pm3393_interrupt_enable,
diff --git a/drivers/net/ethernet/chelsio/cxgb/vsc7326.c b/drivers/net/ethernet/chelsio/cxgb/vsc7326.c
index b0cb388f5e12..6f30b6f78553 100644
--- a/drivers/net/ethernet/chelsio/cxgb/vsc7326.c
+++ b/drivers/net/ethernet/chelsio/cxgb/vsc7326.c
@@ -666,7 +666,7 @@ static void mac_destroy(struct cmac *mac)
 	kfree(mac);
 }
 
-static struct cmac_ops vsc7326_ops = {
+static const struct cmac_ops vsc7326_ops = {
 	.destroy                  = mac_destroy,
 	.reset                    = mac_reset,
 	.interrupt_handler        = mac_intr_handler,
diff --git a/drivers/net/ethernet/chelsio/cxgb3/ael1002.c b/drivers/net/ethernet/chelsio/cxgb3/ael1002.c
index 2028da95afa1..dadf11e3dddb 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/ael1002.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/ael1002.c
@@ -198,7 +198,7 @@ static int get_link_status_r(struct cphy *phy, int *link_ok, int *speed,
 	return 0;
 }
 
-static struct cphy_ops ael1002_ops = {
+static const struct cphy_ops ael1002_ops = {
 	.reset = ael1002_reset,
 	.intr_enable = ael1002_intr_noop,
 	.intr_disable = ael1002_intr_noop,
@@ -224,7 +224,7 @@ static int ael1006_reset(struct cphy *phy, int wait)
 	return t3_phy_reset(phy, MDIO_MMD_PMAPMD, wait);
 }
 
-static struct cphy_ops ael1006_ops = {
+static const struct cphy_ops ael1006_ops = {
 	.reset = ael1006_reset,
 	.intr_enable = t3_phy_lasi_intr_enable,
 	.intr_disable = t3_phy_lasi_intr_disable,
@@ -495,7 +495,7 @@ static int ael2005_intr_handler(struct cphy *phy)
 	return ret ? ret : cphy_cause_link_change;
 }
 
-static struct cphy_ops ael2005_ops = {
+static const struct cphy_ops ael2005_ops = {
 	.reset           = ael2005_reset,
 	.intr_enable     = ael2005_intr_enable,
 	.intr_disable    = ael2005_intr_disable,
@@ -801,7 +801,7 @@ static int ael2020_intr_handler(struct cphy *phy)
 	return ret ? ret : cphy_cause_link_change;
 }
 
-static struct cphy_ops ael2020_ops = {
+static const struct cphy_ops ael2020_ops = {
 	.reset           = ael2020_reset,
 	.intr_enable     = ael2020_intr_enable,
 	.intr_disable    = ael2020_intr_disable,
@@ -856,7 +856,7 @@ static int get_link_status_x(struct cphy *phy, int *link_ok, int *speed,
 	return 0;
 }
 
-static struct cphy_ops qt2045_ops = {
+static const struct cphy_ops qt2045_ops = {
 	.reset = ael1006_reset,
 	.intr_enable = t3_phy_lasi_intr_enable,
 	.intr_disable = t3_phy_lasi_intr_disable,
@@ -921,7 +921,7 @@ static int xaui_direct_power_down(struct cphy *phy, int enable)
 	return 0;
 }
 
-static struct cphy_ops xaui_direct_ops = {
+static const struct cphy_ops xaui_direct_ops = {
 	.reset = xaui_direct_reset,
 	.intr_enable = ael1002_intr_noop,
 	.intr_disable = ael1002_intr_noop,
diff --git a/drivers/net/ethernet/chelsio/cxgb3/aq100x.c b/drivers/net/ethernet/chelsio/cxgb3/aq100x.c
index 341b7ef1508f..6af5d200e44f 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/aq100x.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/aq100x.c
@@ -247,7 +247,7 @@ static int aq100x_get_link_status(struct cphy *phy, int *link_ok,
 	return 0;
 }
 
-static struct cphy_ops aq100x_ops = {
+static const struct cphy_ops aq100x_ops = {
 	.reset             = aq100x_reset,
 	.intr_enable       = aq100x_intr_enable,
 	.intr_disable      = aq100x_intr_disable,
diff --git a/drivers/net/ethernet/chelsio/cxgb3/common.h b/drivers/net/ethernet/chelsio/cxgb3/common.h
index 442480982d3f..1bd7d89666c4 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/common.h
+++ b/drivers/net/ethernet/chelsio/cxgb3/common.h
@@ -575,7 +575,7 @@ static inline int t3_mdio_write(struct cphy *phy, int mmd, int reg,
 
 /* Convenience initializer */
 static inline void cphy_init(struct cphy *phy, struct adapter *adapter,
-			     int phy_addr, struct cphy_ops *phy_ops,
+			     int phy_addr, const struct cphy_ops *phy_ops,
 			     const struct mdio_ops *mdio_ops,
 			      unsigned int caps, const char *desc)
 {
diff --git a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
index 8f7aa53a4c4b..60908eab3b3a 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/cxgb3_main.c
@@ -701,15 +701,16 @@ static ssize_t attr_store(struct device *d,
 			  ssize_t(*set) (struct net_device *, unsigned int),
 			  unsigned int min_val, unsigned int max_val)
 {
-	char *endp;
 	ssize_t ret;
 	unsigned int val;
 
 	if (!capable(CAP_NET_ADMIN))
 		return -EPERM;
 
-	val = simple_strtoul(buf, &endp, 0);
-	if (endp == buf || val < min_val || val > max_val)
+	ret = kstrtouint(buf, 0, &val);
+	if (ret)
+		return ret;
+	if (val < min_val || val > max_val)
 		return -EINVAL;
 
 	rtnl_lock();
@@ -829,14 +830,15 @@ static ssize_t tm_attr_store(struct device *d,
 	struct port_info *pi = netdev_priv(to_net_dev(d));
 	struct adapter *adap = pi->adapter;
 	unsigned int val;
-	char *endp;
 	ssize_t ret;
 
 	if (!capable(CAP_NET_ADMIN))
 		return -EPERM;
 
-	val = simple_strtoul(buf, &endp, 0);
-	if (endp == buf || val > 10000000)
+	ret = kstrtouint(buf, 0, &val);
+	if (ret)
+		return ret;
+	if (val > 10000000)
 		return -EINVAL;
 
 	rtnl_lock();
diff --git a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
index a22768c94200..a89721fad633 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/t3_hw.c
@@ -681,6 +681,24 @@ int t3_seeprom_wp(struct adapter *adapter, int enable)
 	return t3_seeprom_write(adapter, EEPROM_STAT_ADDR, enable ? 0xc : 0);
 }
 
+static int vpdstrtouint(char *s, int len, unsigned int base, unsigned int *val)
+{
+	char tok[len + 1];
+
+	memcpy(tok, s, len);
+	tok[len] = 0;
+	return kstrtouint(strim(tok), base, val);
+}
+
+static int vpdstrtou16(char *s, int len, unsigned int base, u16 *val)
+{
+	char tok[len + 1];
+
+	memcpy(tok, s, len);
+	tok[len] = 0;
+	return kstrtou16(strim(tok), base, val);
+}
+
 /**
  *	get_vpd_params - read VPD parameters from VPD EEPROM
  *	@adapter: adapter to read
@@ -709,11 +727,21 @@ static int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 			return ret;
 	}
 
-	p->cclk = simple_strtoul(vpd.cclk_data, NULL, 10);
-	p->mclk = simple_strtoul(vpd.mclk_data, NULL, 10);
-	p->uclk = simple_strtoul(vpd.uclk_data, NULL, 10);
-	p->mdc = simple_strtoul(vpd.mdc_data, NULL, 10);
-	p->mem_timing = simple_strtoul(vpd.mt_data, NULL, 10);
+	ret = vpdstrtouint(vpd.cclk_data, vpd.cclk_len, 10, &p->cclk);
+	if (ret)
+		return ret;
+	ret = vpdstrtouint(vpd.mclk_data, vpd.mclk_len, 10, &p->mclk);
+	if (ret)
+		return ret;
+	ret = vpdstrtouint(vpd.uclk_data, vpd.uclk_len, 10, &p->uclk);
+	if (ret)
+		return ret;
+	ret = vpdstrtouint(vpd.mdc_data, vpd.mdc_len, 10, &p->mdc);
+	if (ret)
+		return ret;
+	ret = vpdstrtouint(vpd.mt_data, vpd.mt_len, 10, &p->mem_timing);
+	if (ret)
+		return ret;
 	memcpy(p->sn, vpd.sn_data, SERNUM_LEN);
 
 	/* Old eeproms didn't have port information */
@@ -723,8 +751,14 @@ static int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	} else {
 		p->port_type[0] = hex_to_bin(vpd.port0_data[0]);
 		p->port_type[1] = hex_to_bin(vpd.port1_data[0]);
-		p->xauicfg[0] = simple_strtoul(vpd.xaui0cfg_data, NULL, 16);
-		p->xauicfg[1] = simple_strtoul(vpd.xaui1cfg_data, NULL, 16);
+		ret = vpdstrtou16(vpd.xaui0cfg_data, vpd.xaui0cfg_len, 16,
+				  &p->xauicfg[0]);
+		if (ret)
+			return ret;
+		ret = vpdstrtou16(vpd.xaui1cfg_data, vpd.xaui1cfg_len, 16,
+				  &p->xauicfg[1]);
+		if (ret)
+			return ret;
 	}
 
 	ret = hex2bin(p->eth_base, vpd.na_data, 6);
diff --git a/drivers/net/ethernet/chelsio/cxgb3/vsc8211.c b/drivers/net/ethernet/chelsio/cxgb3/vsc8211.c
index 4f9a1c2724f4..8638ad42bf60 100644
--- a/drivers/net/ethernet/chelsio/cxgb3/vsc8211.c
+++ b/drivers/net/ethernet/chelsio/cxgb3/vsc8211.c
@@ -336,7 +336,7 @@ static int vsc8211_intr_handler(struct cphy *cphy)
 	return cphy_cause;
 }
 
-static struct cphy_ops vsc8211_ops = {
+static const struct cphy_ops vsc8211_ops = {
 	.reset = vsc8211_reset,
 	.intr_enable = vsc8211_intr_enable,
 	.intr_disable = vsc8211_intr_disable,
@@ -350,7 +350,7 @@ static struct cphy_ops vsc8211_ops = {
 	.power_down = vsc8211_power_down,
 };
 
-static struct cphy_ops vsc8211_fiber_ops = {
+static const struct cphy_ops vsc8211_fiber_ops = {
 	.reset = vsc8211_reset,
 	.intr_enable = vsc8211_intr_enable,
 	.intr_disable = vsc8211_intr_disable,
diff --git a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
index 11dd91e4db56..7ad43af6bde1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/clip_tbl.c
@@ -118,6 +118,11 @@ int cxgb4_clip_get(const struct net_device *dev, const u32 *lip, u8 v6)
 			ret = clip6_get_mbox(dev, (const struct in6_addr *)lip);
 			if (ret) {
 				write_unlock_bh(&ctbl->lock);
+				dev_err(adap->pdev_dev,
+					"CLIP FW cmd failed with error %d, "
+					"Connections using %pI6c wont be "
+					"offloaded",
+					ret, ce->addr6.sin6_addr.s6_addr);
 				return ret;
 			}
 		} else {
@@ -127,6 +132,9 @@ int cxgb4_clip_get(const struct net_device *dev, const u32 *lip, u8 v6)
 		}
 	} else {
 		write_unlock_bh(&ctbl->lock);
+		dev_info(adap->pdev_dev, "CLIP table overflow, "
+			 "Connections using %pI6c wont be offloaded",
+			 (void *)lip);
 		return -ENOMEM;
 	}
 	write_unlock_bh(&ctbl->lock);
@@ -146,6 +154,9 @@ void cxgb4_clip_release(const struct net_device *dev, const u32 *lip, u8 v6)
 	int hash;
 	int ret = -1;
 
+	if (!ctbl)
+		return;
+
 	hash = clip_addr_hash(ctbl, addr, v6);
 
 	read_lock_bh(&ctbl->lock);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index 55a47de544ea..ec6e849676c1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -301,6 +301,8 @@ struct devlog_params {
 /* Stores chip specific parameters */
 struct arch_specific_params {
 	u8 nchan;
+	u8 pm_stats_cnt;
+	u8 cng_ch_bits_log;		/* congestion channel map bits width */
 	u16 mps_rplc_size;
 	u16 vfcount;
 	u32 sge_fl_db;
@@ -398,11 +400,10 @@ struct link_config {
 
 enum {
 	MAX_ETH_QSETS = 32,           /* # of Ethernet Tx/Rx queue sets */
-	MAX_OFLD_QSETS = 16,          /* # of offload 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 */
-	MAX_ISCSI_QUEUES = NCHAN,     /* # of streaming iSCSI Rx queues */
 };
 
 enum {
@@ -420,7 +421,7 @@ 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_ISCSI_QUEUES + INGQ_EXTRAS,
+		   + MAX_RDMA_CIQS + INGQ_EXTRAS,
 };
 
 struct adapter;
@@ -483,6 +484,8 @@ struct sge_fl {                     /* SGE free-buffer queue state */
 	unsigned int pidx;          /* producer index */
 	unsigned long alloc_failed; /* # of times buffer allocation failed */
 	unsigned long large_alloc_failed;
+	unsigned long mapping_err;  /* # of RX Buffer DMA Mapping failures */
+	unsigned long low;          /* # of times momentarily starving */
 	unsigned long starving;
 	/* RO fields */
 	unsigned int cntxt_id;      /* SGE context id for the free list */
@@ -618,6 +621,7 @@ struct sge_ofld_txq {               /* state for an SGE offload Tx queue */
 	struct adapter *adap;
 	struct sk_buff_head sendq;  /* list of backpressured packets */
 	struct tasklet_struct qresume_tsk; /* restarts the queue */
+	bool service_ofldq_running; /* service_ofldq() is processing sendq */
 	u8 full;                    /* the Tx ring is full */
 	unsigned long mapping_err;  /* # of I/O MMU packet mapping errors */
 } ____cacheline_aligned_in_smp;
@@ -636,7 +640,7 @@ struct sge {
 	struct sge_ctrl_txq ctrlq[MAX_CTRL_QUEUES];
 
 	struct sge_eth_rxq ethrxq[MAX_ETH_QSETS];
-	struct sge_ofld_rxq ofldrxq[MAX_OFLD_QSETS];
+	struct sge_ofld_rxq iscsirxq[MAX_OFLD_QSETS];
 	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;
@@ -647,10 +651,10 @@ 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 ofldqsets;              /* # of active offload queue sets */
+	u16 iscsiqsets;              /* # of active iSCSI queue sets */
 	u16 rdmaqs;                 /* # of available RDMA Rx queues */
 	u16 rdmaciqs;               /* # of available RDMA concentrator IQs */
-	u16 ofld_rxq[MAX_OFLD_QSETS];
+	u16 iscsi_rxq[MAX_OFLD_QSETS];
 	u16 rdma_rxq[MAX_RDMA_QUEUES];
 	u16 rdma_ciq[MAX_RDMA_CIQS];
 	u16 timer_val[SGE_NTIMERS];
@@ -676,7 +680,7 @@ struct sge {
 };
 
 #define for_each_ethrxq(sge, i) for (i = 0; i < (sge)->ethqsets; i++)
-#define for_each_ofldrxq(sge, i) for (i = 0; i < (sge)->ofldqsets; i++)
+#define for_each_iscsirxq(sge, i) for (i = 0; i < (sge)->iscsiqsets; 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++)
 
@@ -1253,6 +1257,7 @@ int t4_phy_fw_ver(struct adapter *adap, int *phy_fw_ver);
 int t4_fwcache(struct adapter *adap, enum fw_params_param_dev_fwcache op);
 int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 		  const u8 *fw_data, unsigned int size, int force);
+int t4_fl_pkt_align(struct adapter *adap);
 unsigned int t4_flash_cfg_addr(struct adapter *adapter);
 int t4_check_fw_version(struct adapter *adap);
 int t4_get_fw_version(struct adapter *adapter, u32 *vers);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
index 4269944c5db5..e6a4072b494b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_debugfs.c
@@ -757,8 +757,8 @@ static int pm_stats_show(struct seq_file *seq, void *v)
 	};
 
 	int i;
-	u32 tx_cnt[PM_NSTATS], rx_cnt[PM_NSTATS];
-	u64 tx_cyc[PM_NSTATS], rx_cyc[PM_NSTATS];
+	u32 tx_cnt[T6_PM_NSTATS], rx_cnt[T6_PM_NSTATS];
+	u64 tx_cyc[T6_PM_NSTATS], rx_cyc[T6_PM_NSTATS];
 	struct adapter *adap = seq->private;
 
 	t4_pmtx_get_stats(adap, tx_cnt, tx_cyc);
@@ -773,6 +773,32 @@ static int pm_stats_show(struct seq_file *seq, void *v)
 	for (i = 0; i < PM_NSTATS - 1; i++)
 		seq_printf(seq, "%-13s %10u  %20llu\n",
 			   rx_pm_stats[i], rx_cnt[i], rx_cyc[i]);
+
+	if (CHELSIO_CHIP_VERSION(adap->params.chip) > CHELSIO_T5) {
+		/* In T5 the granularity of the total wait is too fine.
+		 * It is not useful as it reaches the max value too fast.
+		 * Hence display this Input FIFO wait for T6 onwards.
+		 */
+		seq_printf(seq, "%13s %10s  %20s\n",
+			   " ", "Total wait", "Total Occupancy");
+		seq_printf(seq, "Tx FIFO wait  %10u  %20llu\n",
+			   tx_cnt[i], tx_cyc[i]);
+		seq_printf(seq, "Rx FIFO wait  %10u  %20llu\n",
+			   rx_cnt[i], rx_cyc[i]);
+
+		/* Skip index 6 as there is nothing useful ihere */
+		i += 2;
+
+		/* At index 7, a new stat for read latency (count, total wait)
+		 * is added.
+		 */
+		seq_printf(seq, "%13s %10s  %20s\n",
+			   " ", "Reads", "Total wait");
+		seq_printf(seq, "Tx latency    %10u  %20llu\n",
+			   tx_cnt[i], tx_cyc[i]);
+		seq_printf(seq, "Rx latency    %10u  %20llu\n",
+			   rx_cnt[i], rx_cyc[i]);
+	}
 	return 0;
 }
 
@@ -1559,25 +1585,35 @@ static int mps_tcam_show(struct seq_file *seq, void *v)
 {
 	struct adapter *adap = seq->private;
 	unsigned int chip_ver = CHELSIO_CHIP_VERSION(adap->params.chip);
-
 	if (v == SEQ_START_TOKEN) {
-		if (adap->params.arch.mps_rplc_size > 128)
+		if (chip_ver > CHELSIO_T5) {
 			seq_puts(seq, "Idx  Ethernet address     Mask     "
+				 "  VNI   Mask   IVLAN Vld "
+				 "DIP_Hit   Lookup  Port "
 				 "Vld Ports PF  VF                           "
 				 "Replication                                "
 				 "    P0 P1 P2 P3  ML\n");
-		else
-			seq_puts(seq, "Idx  Ethernet address     Mask     "
-				 "Vld Ports PF  VF              Replication"
-				 "	         P0 P1 P2 P3  ML\n");
+		} else {
+			if (adap->params.arch.mps_rplc_size > 128)
+				seq_puts(seq, "Idx  Ethernet address     Mask     "
+					 "Vld Ports PF  VF                           "
+					 "Replication                                "
+					 "    P0 P1 P2 P3  ML\n");
+			else
+				seq_puts(seq, "Idx  Ethernet address     Mask     "
+					 "Vld Ports PF  VF              Replication"
+					 "	         P0 P1 P2 P3  ML\n");
+		}
 	} else {
 		u64 mask;
 		u8 addr[ETH_ALEN];
-		bool replicate;
+		bool replicate, dip_hit = false, vlan_vld = false;
 		unsigned int idx = (uintptr_t)v - 2;
 		u64 tcamy, tcamx, val;
-		u32 cls_lo, cls_hi, ctl;
+		u32 cls_lo, cls_hi, ctl, data2, vnix = 0, vniy = 0;
 		u32 rplc[8] = {0};
+		u8 lookup_type = 0, port_num = 0;
+		u16 ivlan = 0;
 
 		if (chip_ver > CHELSIO_T5) {
 			/* CtlCmdType - 0: Read, 1: Write
@@ -1596,6 +1632,22 @@ static int mps_tcam_show(struct seq_file *seq, void *v)
 			val = t4_read_reg(adap, MPS_CLS_TCAM_DATA1_A);
 			tcamy = DMACH_G(val) << 32;
 			tcamy |= t4_read_reg(adap, MPS_CLS_TCAM_DATA0_A);
+			data2 = t4_read_reg(adap, MPS_CLS_TCAM_DATA2_CTL_A);
+			lookup_type = DATALKPTYPE_G(data2);
+			/* 0 - Outer header, 1 - Inner header
+			 * [71:48] bit locations are overloaded for
+			 * outer vs. inner lookup types.
+			 */
+			if (lookup_type && (lookup_type != DATALKPTYPE_M)) {
+				/* Inner header VNI */
+				vniy = ((data2 & DATAVIDH2_F) << 23) |
+				       (DATAVIDH1_G(data2) << 16) | VIDL_G(val);
+				dip_hit = data2 & DATADIPHIT_F;
+			} else {
+				vlan_vld = data2 & DATAVIDH2_F;
+				ivlan = VIDL_G(val);
+			}
+			port_num = DATAPORTNUM_G(data2);
 
 			/* Read tcamx. Change the control param */
 			ctl |= CTLXYBITSEL_V(1);
@@ -1603,6 +1655,12 @@ static int mps_tcam_show(struct seq_file *seq, void *v)
 			val = t4_read_reg(adap, MPS_CLS_TCAM_DATA1_A);
 			tcamx = DMACH_G(val) << 32;
 			tcamx |= t4_read_reg(adap, MPS_CLS_TCAM_DATA0_A);
+			data2 = t4_read_reg(adap, MPS_CLS_TCAM_DATA2_CTL_A);
+			if (lookup_type && (lookup_type != DATALKPTYPE_M)) {
+				/* Inner header VNI mask */
+				vnix = ((data2 & DATAVIDH2_F) << 23) |
+				       (DATAVIDH1_G(data2) << 16) | VIDL_G(val);
+			}
 		} else {
 			tcamy = t4_read_reg64(adap, MPS_CLS_TCAM_Y_L(idx));
 			tcamx = t4_read_reg64(adap, MPS_CLS_TCAM_X_L(idx));
@@ -1662,17 +1720,47 @@ static int mps_tcam_show(struct seq_file *seq, void *v)
 		}
 
 		tcamxy2valmask(tcamx, tcamy, addr, &mask);
-		if (chip_ver > CHELSIO_T5)
-			seq_printf(seq, "%3u %02x:%02x:%02x:%02x:%02x:%02x "
-				   "%012llx%3c   %#x%4u%4d",
-				   idx, addr[0], addr[1], addr[2], addr[3],
-				   addr[4], addr[5], (unsigned long long)mask,
-				   (cls_lo & T6_SRAM_VLD_F) ? 'Y' : 'N',
-				   PORTMAP_G(cls_hi),
-				   T6_PF_G(cls_lo),
-				   (cls_lo & T6_VF_VALID_F) ?
-				   T6_VF_G(cls_lo) : -1);
-		else
+		if (chip_ver > CHELSIO_T5) {
+			/* Inner header lookup */
+			if (lookup_type && (lookup_type != DATALKPTYPE_M)) {
+				seq_printf(seq,
+					   "%3u %02x:%02x:%02x:%02x:%02x:%02x "
+					   "%012llx %06x %06x    -    -   %3c"
+					   "      'I'  %4x   "
+					   "%3c   %#x%4u%4d", idx, addr[0],
+					   addr[1], addr[2], addr[3],
+					   addr[4], addr[5],
+					   (unsigned long long)mask,
+					   vniy, vnix, dip_hit ? 'Y' : 'N',
+					   port_num,
+					   (cls_lo & T6_SRAM_VLD_F) ? 'Y' : 'N',
+					   PORTMAP_G(cls_hi),
+					   T6_PF_G(cls_lo),
+					   (cls_lo & T6_VF_VALID_F) ?
+					   T6_VF_G(cls_lo) : -1);
+			} else {
+				seq_printf(seq,
+					   "%3u %02x:%02x:%02x:%02x:%02x:%02x "
+					   "%012llx    -       -   ",
+					   idx, addr[0], addr[1], addr[2],
+					   addr[3], addr[4], addr[5],
+					   (unsigned long long)mask);
+
+				if (vlan_vld)
+					seq_printf(seq, "%4u   Y     ", ivlan);
+				else
+					seq_puts(seq, "  -    N     ");
+
+				seq_printf(seq,
+					   "-      %3c  %4x   %3c   %#x%4u%4d",
+					   lookup_type ? 'I' : 'O', port_num,
+					   (cls_lo & T6_SRAM_VLD_F) ? 'Y' : 'N',
+					   PORTMAP_G(cls_hi),
+					   T6_PF_G(cls_lo),
+					   (cls_lo & T6_VF_VALID_F) ?
+					   T6_VF_G(cls_lo) : -1);
+			}
+		} else
 			seq_printf(seq, "%3u %02x:%02x:%02x:%02x:%02x:%02x "
 				   "%012llx%3c   %#x%4u%4d",
 				   idx, addr[0], addr[1], addr[2], addr[3],
@@ -2245,7 +2333,7 @@ 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.ofldqsets, 4);
+	int iscsi_entries = DIV_ROUND_UP(adap->sge.iscsiqsets, 4);
 	int rdma_entries = DIV_ROUND_UP(adap->sge.rdmaqs, 4);
 	int ciq_entries = DIV_ROUND_UP(adap->sge.rdmaciqs, 4);
 	int ctrl_entries = DIV_ROUND_UP(MAX_CTRL_QUEUES, 4);
@@ -2325,14 +2413,16 @@ do { \
 		TL("TxMapErr:", mapping_err);
 		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 (iscsi_idx < iscsi_entries) {
 		const struct sge_ofld_rxq *rx =
-			&adap->sge.ofldrxq[iscsi_idx * 4];
+			&adap->sge.iscsirxq[iscsi_idx * 4];
 		const struct sge_ofld_txq *tx =
 			&adap->sge.ofldtxq[iscsi_idx * 4];
-		int n = min(4, adap->sge.ofldqsets - 4 * iscsi_idx);
+		int n = min(4, adap->sge.iscsiqsets - 4 * iscsi_idx);
 
 		S("QType:", "iSCSI");
 		T("TxQ ID:", q.cntxt_id);
@@ -2359,6 +2449,8 @@ do { \
 		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) {
@@ -2388,6 +2480,8 @@ do { \
 		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) {
@@ -2448,7 +2542,7 @@ do { \
 static int sge_queue_entries(const struct adapter *adap)
 {
 	return DIV_ROUND_UP(adap->sge.ethqsets, 4) +
-	       DIV_ROUND_UP(adap->sge.ofldqsets, 4) +
+	       DIV_ROUND_UP(adap->sge.iscsiqsets, 4) +
 	       DIV_ROUND_UP(adap->sge.rdmaqs, 4) +
 	       DIV_ROUND_UP(adap->sge.rdmaciqs, 4) +
 	       DIV_ROUND_UP(MAX_CTRL_QUEUES, 4) + 1;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
index a077f9476daf..7a0b92b2f73c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_ethtool.c
@@ -35,79 +35,79 @@ static void set_msglevel(struct net_device *dev, u32 val)
 }
 
 static const char stats_strings[][ETH_GSTRING_LEN] = {
-	"tx_octets_ok		",
-	"tx_frames_ok		",
-	"tx_broadcast_frames	",
-	"tx_multicast_frames	",
-	"tx_unicast_frames	",
-	"tx_error_frames	",
-
-	"tx_frames_64		",
-	"tx_frames_65_to_127	",
-	"tx_frames_128_to_255	",
-	"tx_frames_256_to_511	",
-	"tx_frames_512_to_1023	",
-	"tx_frames_1024_to_1518	",
-	"tx_frames_1519_to_max	",
-
-	"tx_frames_dropped	",
-	"tx_pause_frames	",
-	"tx_ppp0_frames		",
-	"tx_ppp1_frames		",
-	"tx_ppp2_frames		",
-	"tx_ppp3_frames		",
-	"tx_ppp4_frames		",
-	"tx_ppp5_frames		",
-	"tx_ppp6_frames		",
-	"tx_ppp7_frames		",
-
-	"rx_octets_ok		",
-	"rx_frames_ok		",
-	"rx_broadcast_frames	",
-	"rx_multicast_frames	",
-	"rx_unicast_frames	",
-
-	"rx_frames_too_long	",
-	"rx_jabber_errors	",
-	"rx_fcs_errors		",
-	"rx_length_errors	",
-	"rx_symbol_errors	",
-	"rx_runt_frames		",
-
-	"rx_frames_64		",
-	"rx_frames_65_to_127	",
-	"rx_frames_128_to_255	",
-	"rx_frames_256_to_511	",
-	"rx_frames_512_to_1023	",
-	"rx_frames_1024_to_1518	",
-	"rx_frames_1519_to_max	",
-
-	"rx_pause_frames	",
-	"rx_ppp0_frames		",
-	"rx_ppp1_frames		",
-	"rx_ppp2_frames		",
-	"rx_ppp3_frames		",
-	"rx_ppp4_frames		",
-	"rx_ppp5_frames		",
-	"rx_ppp6_frames		",
-	"rx_ppp7_frames		",
-
-	"rx_bg0_frames_dropped	",
-	"rx_bg1_frames_dropped	",
-	"rx_bg2_frames_dropped	",
-	"rx_bg3_frames_dropped	",
-	"rx_bg0_frames_trunc	",
-	"rx_bg1_frames_trunc	",
-	"rx_bg2_frames_trunc	",
-	"rx_bg3_frames_trunc	",
-
-	"tso			",
-	"tx_csum_offload	",
-	"rx_csum_good		",
-	"vlan_extractions	",
-	"vlan_insertions	",
-	"gro_packets		",
-	"gro_merged		",
+	"tx_octets_ok           ",
+	"tx_frames_ok           ",
+	"tx_broadcast_frames    ",
+	"tx_multicast_frames    ",
+	"tx_unicast_frames      ",
+	"tx_error_frames        ",
+
+	"tx_frames_64           ",
+	"tx_frames_65_to_127    ",
+	"tx_frames_128_to_255   ",
+	"tx_frames_256_to_511   ",
+	"tx_frames_512_to_1023  ",
+	"tx_frames_1024_to_1518 ",
+	"tx_frames_1519_to_max  ",
+
+	"tx_frames_dropped      ",
+	"tx_pause_frames        ",
+	"tx_ppp0_frames         ",
+	"tx_ppp1_frames         ",
+	"tx_ppp2_frames         ",
+	"tx_ppp3_frames         ",
+	"tx_ppp4_frames         ",
+	"tx_ppp5_frames         ",
+	"tx_ppp6_frames         ",
+	"tx_ppp7_frames         ",
+
+	"rx_octets_ok           ",
+	"rx_frames_ok           ",
+	"rx_broadcast_frames    ",
+	"rx_multicast_frames    ",
+	"rx_unicast_frames      ",
+
+	"rx_frames_too_long     ",
+	"rx_jabber_errors       ",
+	"rx_fcs_errors          ",
+	"rx_length_errors       ",
+	"rx_symbol_errors       ",
+	"rx_runt_frames         ",
+
+	"rx_frames_64           ",
+	"rx_frames_65_to_127    ",
+	"rx_frames_128_to_255   ",
+	"rx_frames_256_to_511   ",
+	"rx_frames_512_to_1023  ",
+	"rx_frames_1024_to_1518 ",
+	"rx_frames_1519_to_max  ",
+
+	"rx_pause_frames        ",
+	"rx_ppp0_frames         ",
+	"rx_ppp1_frames         ",
+	"rx_ppp2_frames         ",
+	"rx_ppp3_frames         ",
+	"rx_ppp4_frames         ",
+	"rx_ppp5_frames         ",
+	"rx_ppp6_frames         ",
+	"rx_ppp7_frames         ",
+
+	"rx_bg0_frames_dropped  ",
+	"rx_bg1_frames_dropped  ",
+	"rx_bg2_frames_dropped  ",
+	"rx_bg3_frames_dropped  ",
+	"rx_bg0_frames_trunc    ",
+	"rx_bg1_frames_trunc    ",
+	"rx_bg2_frames_trunc    ",
+	"rx_bg3_frames_trunc    ",
+
+	"tso                    ",
+	"tx_csum_offload        ",
+	"rx_csum_good           ",
+	"vlan_extractions       ",
+	"vlan_insertions        ",
+	"gro_packets            ",
+	"gro_merged             ",
 };
 
 static char adapter_stats_strings[][ETH_GSTRING_LEN] = {
@@ -686,7 +686,7 @@ static int set_pauseparam(struct net_device *dev,
 	if (epause->tx_pause)
 		lc->requested_fc |= PAUSE_TX;
 	if (netif_running(dev))
-		return t4_link_l1cfg(p->adapter, p->adapter->pf, p->tx_chan,
+		return t4_link_l1cfg(p->adapter, p->adapter->mbox, p->tx_chan,
 				     lc);
 	return 0;
 }
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 0d147610a06f..b8a5fb0c32d4 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -162,19 +162,8 @@ MODULE_FIRMWARE(FW6_FNAME);
 static uint force_init;
 
 module_param(force_init, uint, 0644);
-MODULE_PARM_DESC(force_init, "Forcibly become Master PF and initialize adapter");
-
-/*
- * Normally if the firmware we connect to has Configuration File support, we
- * use that and only fall back to the old Driver-based initialization if the
- * Configuration File fails for some reason.  If force_old_init is set, then
- * we'll always use the old Driver-based initialization sequence.
- */
-static uint force_old_init;
-
-module_param(force_old_init, uint, 0644);
-MODULE_PARM_DESC(force_old_init, "Force old initialization sequence, deprecated"
-		 " parameter");
+MODULE_PARM_DESC(force_init, "Forcibly become Master PF and initialize adapter,"
+		 "deprecated parameter");
 
 static int dflt_msg_enable = DFLT_MSG_ENABLE;
 
@@ -196,23 +185,6 @@ module_param(msi, int, 0644);
 MODULE_PARM_DESC(msi, "whether to use INTx (0), MSI (1) or MSI-X (2)");
 
 /*
- * Queue interrupt hold-off timer values.  Queues default to the first of these
- * upon creation.
- */
-static unsigned int intr_holdoff[SGE_NTIMERS - 1] = { 5, 10, 20, 50, 100 };
-
-module_param_array(intr_holdoff, uint, NULL, 0644);
-MODULE_PARM_DESC(intr_holdoff, "values for queue interrupt hold-off timers "
-		 "0..4 in microseconds, deprecated parameter");
-
-static unsigned int intr_cnt[SGE_NCOUNTERS - 1] = { 4, 8, 16 };
-
-module_param_array(intr_cnt, uint, NULL, 0644);
-MODULE_PARM_DESC(intr_cnt,
-		 "thresholds 1..3 for queue interrupt packet counters, "
-		 "deprecated parameter");
-
-/*
  * Normally we tell the chip to deliver Ingress Packets into our DMA buffers
  * offset by 2 bytes in order to have the IP headers line up on 4-byte
  * boundaries.  This is a requirement for many architectures which will throw
@@ -226,13 +198,7 @@ MODULE_PARM_DESC(intr_cnt,
  */
 static int rx_dma_offset = 2;
 
-static bool vf_acls;
-
 #ifdef CONFIG_PCI_IOV
-module_param(vf_acls, bool, 0644);
-MODULE_PARM_DESC(vf_acls, "if set enable virtualization L2 ACL enforcement, "
-		 "deprecated parameter");
-
 /* Configure the number of PCI-E Virtual Function which are to be instantiated
  * on SR-IOV Capable Physical Functions.
  */
@@ -253,12 +219,6 @@ module_param(select_queue, int, 0644);
 MODULE_PARM_DESC(select_queue,
 		 "Select between kernel provided method of selecting or driver method of selecting TX queue. Default is kernel method.");
 
-static unsigned int tp_vlan_pri_map = HW_TPL_FR_MT_PR_IV_P_FC;
-
-module_param(tp_vlan_pri_map, uint, 0644);
-MODULE_PARM_DESC(tp_vlan_pri_map, "global compressed filter configuration, "
-		 "deprecated parameter");
-
 static struct dentry *cxgb4_debugfs_root;
 
 static LIST_HEAD(adapter_list);
@@ -766,8 +726,8 @@ static void name_msix_vecs(struct adapter *adap)
 	}
 
 	/* offload queues */
-	for_each_ofldrxq(&adap->sge, i)
-		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-ofld%d",
+	for_each_iscsirxq(&adap->sge, i)
+		snprintf(adap->msix_info[msi_idx++].desc, n, "%s-iscsi%d",
 			 adap->port[0]->name, i);
 
 	for_each_rdmarxq(&adap->sge, i)
@@ -782,7 +742,7 @@ static void name_msix_vecs(struct adapter *adap)
 static int request_msix_queue_irqs(struct adapter *adap)
 {
 	struct sge *s = &adap->sge;
-	int err, ethqidx, ofldqidx = 0, rdmaqidx = 0, rdmaciqqidx = 0;
+	int err, ethqidx, iscsiqidx = 0, rdmaqidx = 0, rdmaciqqidx = 0;
 	int msi_index = 2;
 
 	err = request_irq(adap->msix_info[1].vec, t4_sge_intr_msix, 0,
@@ -799,11 +759,11 @@ static int request_msix_queue_irqs(struct adapter *adap)
 			goto unwind;
 		msi_index++;
 	}
-	for_each_ofldrxq(s, ofldqidx) {
+	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->ofldrxq[ofldqidx].rspq);
+				  &s->iscsirxq[iscsiqidx].rspq);
 		if (err)
 			goto unwind;
 		msi_index++;
@@ -835,9 +795,9 @@ unwind:
 	while (--rdmaqidx >= 0)
 		free_irq(adap->msix_info[--msi_index].vec,
 			 &s->rdmarxq[rdmaqidx].rspq);
-	while (--ofldqidx >= 0)
+	while (--iscsiqidx >= 0)
 		free_irq(adap->msix_info[--msi_index].vec,
-			 &s->ofldrxq[ofldqidx].rspq);
+			 &s->iscsirxq[iscsiqidx].rspq);
 	while (--ethqidx >= 0)
 		free_irq(adap->msix_info[--msi_index].vec,
 			 &s->ethrxq[ethqidx].rspq);
@@ -853,8 +813,9 @@ 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_ofldrxq(s, i)
-		free_irq(adap->msix_info[msi_index++].vec, &s->ofldrxq[i].rspq);
+	for_each_iscsirxq(s, i)
+		free_irq(adap->msix_info[msi_index++].vec,
+			 &s->iscsirxq[i].rspq);
 	for_each_rdmarxq(s, i)
 		free_irq(adap->msix_info[msi_index++].vec, &s->rdmarxq[i].rspq);
 	for_each_rdmaciq(s, i)
@@ -1093,8 +1054,8 @@ freeout:	t4_free_sge_resources(adap);
 		}
 	}
 
-	j = s->ofldqsets / adap->params.nports; /* ofld queues per channel */
-	for_each_ofldrxq(s, i) {
+	j = s->iscsiqsets / adap->params.nports; /* iscsi queues per channel */
+	for_each_iscsirxq(s, i) {
 		err = t4_sge_alloc_ofld_txq(adap, &s->ofldtxq[i],
 					    adap->port[i / j],
 					    s->fw_evtq.cntxt_id);
@@ -1110,7 +1071,7 @@ freeout:	t4_free_sge_resources(adap);
 		msi_idx += nq; \
 } while (0)
 
-	ALLOC_OFLD_RXQS(s->ofldrxq, s->ofldqsets, j, s->ofld_rxq);
+	ALLOC_OFLD_RXQS(s->iscsirxq, s->iscsiqsets, j, s->iscsi_rxq);
 	ALLOC_OFLD_RXQS(s->rdmarxq, s->rdmaqs, 1, s->rdma_rxq);
 	j = s->rdmaciqs / adap->params.nports; /* rdmaq queues per channel */
 	ALLOC_OFLD_RXQS(s->rdmaciq, s->rdmaciqs, j, s->rdma_ciq);
@@ -1181,16 +1142,10 @@ static int set_filter_wr(struct adapter *adapter, int fidx)
 	 */
 	if (f->fs.newdmac || f->fs.newvlan) {
 		/* allocate L2T entry for new filter */
-		f->l2t = t4_l2t_alloc_switching(adapter->l2t);
+		f->l2t = t4_l2t_alloc_switching(adapter, f->fs.vlan,
+						f->fs.eport, f->fs.dmac);
 		if (f->l2t == NULL) {
 			kfree_skb(skb);
-			return -EAGAIN;
-		}
-		if (t4_l2t_set_switching(adapter, f->l2t, f->fs.vlan,
-					f->fs.eport, f->fs.dmac)) {
-			cxgb4_l2t_release(f->l2t);
-			f->l2t = NULL;
-			kfree_skb(skb);
 			return -ENOMEM;
 		}
 	}
@@ -1511,7 +1466,7 @@ int cxgb4_alloc_stid(struct tid_info *t, int family, void *data)
 		else
 			stid = -1;
 	} else {
-		stid = bitmap_find_free_region(t->stid_bmap, t->nstids, 2);
+		stid = bitmap_find_free_region(t->stid_bmap, t->nstids, 1);
 		if (stid < 0)
 			stid = -1;
 	}
@@ -1525,7 +1480,7 @@ int cxgb4_alloc_stid(struct tid_info *t, int family, void *data)
 		if (family == PF_INET)
 			t->stids_in_use++;
 		else
-			t->stids_in_use += 4;
+			t->stids_in_use += 2;
 	}
 	spin_unlock_bh(&t->stid_lock);
 	return stid;
@@ -1576,13 +1531,13 @@ void cxgb4_free_stid(struct tid_info *t, unsigned int stid, int family)
 	if (family == PF_INET)
 		__clear_bit(stid, t->stid_bmap);
 	else
-		bitmap_release_region(t->stid_bmap, stid, 2);
+		bitmap_release_region(t->stid_bmap, stid, 1);
 	t->stid_tab[stid].data = NULL;
 	if (stid < t->nstids) {
 		if (family == PF_INET)
 			t->stids_in_use--;
 		else
-			t->stids_in_use -= 4;
+			t->stids_in_use -= 2;
 	} else {
 		t->sftids_in_use--;
 	}
@@ -2283,7 +2238,7 @@ static void disable_dbs(struct adapter *adap)
 
 	for_each_ethrxq(&adap->sge, i)
 		disable_txq_db(&adap->sge.ethtxq[i].q);
-	for_each_ofldrxq(&adap->sge, i)
+	for_each_iscsirxq(&adap->sge, i)
 		disable_txq_db(&adap->sge.ofldtxq[i].q);
 	for_each_port(adap, i)
 		disable_txq_db(&adap->sge.ctrlq[i].q);
@@ -2295,7 +2250,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_ofldrxq(&adap->sge, i)
+	for_each_iscsirxq(&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);
@@ -2365,7 +2320,7 @@ static void recover_all_queues(struct adapter *adap)
 
 	for_each_ethrxq(&adap->sge, i)
 		sync_txq_pidx(adap, &adap->sge.ethtxq[i].q);
-	for_each_ofldrxq(&adap->sge, i)
+	for_each_iscsirxq(&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);
@@ -2449,10 +2404,10 @@ static void uld_attach(struct adapter *adap, unsigned int uld)
 		lli.nrxq = adap->sge.rdmaqs;
 		lli.nciq = adap->sge.rdmaciqs;
 	} else if (uld == CXGB4_ULD_ISCSI) {
-		lli.rxq_ids = adap->sge.ofld_rxq;
-		lli.nrxq = adap->sge.ofldqsets;
+		lli.rxq_ids = adap->sge.iscsi_rxq;
+		lli.nrxq = adap->sge.iscsiqsets;
 	}
-	lli.ntxq = adap->sge.ofldqsets;
+	lli.ntxq = adap->sge.iscsiqsets;
 	lli.nchan = adap->params.nports;
 	lli.nports = adap->params.nports;
 	lli.wr_cred = adap->params.ofldq_wr_cred;
@@ -3146,16 +3101,6 @@ static int adap_init1(struct adapter *adap, struct fw_caps_config_cmd *c)
 	if (ret < 0)
 		return ret;
 
-	/* select capabilities we'll be using */
-	if (c->niccaps & htons(FW_CAPS_CONFIG_NIC_VM)) {
-		if (!vf_acls)
-			c->niccaps ^= htons(FW_CAPS_CONFIG_NIC_VM);
-		else
-			c->niccaps = htons(FW_CAPS_CONFIG_NIC_VM);
-	} else if (vf_acls) {
-		dev_err(adap->pdev_dev, "virtualization ACLs not supported");
-		return ret;
-	}
 	c->op_to_write = htonl(FW_CMD_OP_V(FW_CAPS_CONFIG_CMD) |
 			       FW_CMD_REQUEST_F | FW_CMD_WRITE_F);
 	ret = t4_wr_mbox(adap, adap->mbox, c, sizeof(*c), NULL);
@@ -4348,11 +4293,11 @@ static void cfg_queues(struct adapter *adap)
 		 * capped by the number of available cores.
 		 */
 		if (n10g) {
-			i = min_t(int, ARRAY_SIZE(s->ofldrxq),
+			i = min_t(int, ARRAY_SIZE(s->iscsirxq),
 				  num_online_cpus());
-			s->ofldqsets = roundup(i, adap->params.nports);
+			s->iscsiqsets = roundup(i, adap->params.nports);
 		} else
-			s->ofldqsets = adap->params.nports;
+			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
@@ -4383,8 +4328,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->ofldrxq); i++) {
-		struct sge_ofld_rxq *r = &s->ofldrxq[i];
+	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;
@@ -4465,7 +4410,7 @@ static int enable_msix(struct adapter *adap)
 
 	want = s->max_ethqsets + EXTRA_VECS;
 	if (is_offload(adap)) {
-		want += s->rdmaqs + s->rdmaciqs + s->ofldqsets;
+		want += s->rdmaqs + s->rdmaciqs + s->iscsiqsets;
 		/* need nchan for each possible ULD */
 		ofld_need = 3 * nchan;
 	}
@@ -4504,13 +4449,13 @@ static int enable_msix(struct adapter *adap)
 		/* leftovers go to OFLD */
 		i = allocated - EXTRA_VECS - s->max_ethqsets -
 		    s->rdmaqs - s->rdmaciqs;
-		s->ofldqsets = (i / nchan) * nchan;  /* round down */
+		s->iscsiqsets = (i / nchan) * nchan;  /* round down */
 	}
 	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->ofldqsets, s->rdmaqs,
+		 allocated, s->max_ethqsets, s->iscsiqsets, s->rdmaqs,
 		 s->rdmaciqs);
 
 	kfree(entries);
@@ -4538,6 +4483,79 @@ static int init_rss(struct adapter *adap)
 	return 0;
 }
 
+static int cxgb4_get_pcie_dev_link_caps(struct adapter *adap,
+					enum pci_bus_speed *speed,
+					enum pcie_link_width *width)
+{
+	u32 lnkcap1, lnkcap2;
+	int err1, err2;
+
+#define  PCIE_MLW_CAP_SHIFT 4   /* start of MLW mask in link capabilities */
+
+	*speed = PCI_SPEED_UNKNOWN;
+	*width = PCIE_LNK_WIDTH_UNKNOWN;
+
+	err1 = pcie_capability_read_dword(adap->pdev, PCI_EXP_LNKCAP,
+					  &lnkcap1);
+	err2 = pcie_capability_read_dword(adap->pdev, PCI_EXP_LNKCAP2,
+					  &lnkcap2);
+	if (!err2 && lnkcap2) { /* PCIe r3.0-compliant */
+		if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_8_0GB)
+			*speed = PCIE_SPEED_8_0GT;
+		else if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_5_0GB)
+			*speed = PCIE_SPEED_5_0GT;
+		else if (lnkcap2 & PCI_EXP_LNKCAP2_SLS_2_5GB)
+			*speed = PCIE_SPEED_2_5GT;
+	}
+	if (!err1) {
+		*width = (lnkcap1 & PCI_EXP_LNKCAP_MLW) >> PCIE_MLW_CAP_SHIFT;
+		if (!lnkcap2) { /* pre-r3.0 */
+			if (lnkcap1 & PCI_EXP_LNKCAP_SLS_5_0GB)
+				*speed = PCIE_SPEED_5_0GT;
+			else if (lnkcap1 & PCI_EXP_LNKCAP_SLS_2_5GB)
+				*speed = PCIE_SPEED_2_5GT;
+		}
+	}
+
+	if (*speed == PCI_SPEED_UNKNOWN || *width == PCIE_LNK_WIDTH_UNKNOWN)
+		return err1 ? err1 : err2 ? err2 : -EINVAL;
+	return 0;
+}
+
+static void cxgb4_check_pcie_caps(struct adapter *adap)
+{
+	enum pcie_link_width width, width_cap;
+	enum pci_bus_speed speed, speed_cap;
+
+#define PCIE_SPEED_STR(speed) \
+	(speed == PCIE_SPEED_8_0GT ? "8.0GT/s" : \
+	 speed == PCIE_SPEED_5_0GT ? "5.0GT/s" : \
+	 speed == PCIE_SPEED_2_5GT ? "2.5GT/s" : \
+	 "Unknown")
+
+	if (cxgb4_get_pcie_dev_link_caps(adap, &speed_cap, &width_cap)) {
+		dev_warn(adap->pdev_dev,
+			 "Unable to determine PCIe device BW capabilities\n");
+		return;
+	}
+
+	if (pcie_get_minimum_link(adap->pdev, &speed, &width) ||
+	    speed == PCI_SPEED_UNKNOWN || width == PCIE_LNK_WIDTH_UNKNOWN) {
+		dev_warn(adap->pdev_dev,
+			 "Unable to determine PCI Express bandwidth.\n");
+		return;
+	}
+
+	dev_info(adap->pdev_dev, "PCIe link speed is %s, device supports %s\n",
+		 PCIE_SPEED_STR(speed), PCIE_SPEED_STR(speed_cap));
+	dev_info(adap->pdev_dev, "PCIe link width is x%d, device supports x%d\n",
+		 width, width_cap);
+	if (speed < speed_cap || width < width_cap)
+		dev_info(adap->pdev_dev,
+			 "A slot with more lanes and/or higher speed is "
+			 "suggested for optimal performance.\n");
+}
+
 static void print_port_info(const struct net_device *dev)
 {
 	char buf[80];
@@ -4565,10 +4583,10 @@ static void print_port_info(const struct net_device *dev)
 		--bufp;
 	sprintf(bufp, "BASE-%s", t4_get_port_type_description(pi->port_type));
 
-	netdev_info(dev, "Chelsio %s rev %d %s %sNIC PCIe x%d%s%s\n",
+	netdev_info(dev, "Chelsio %s rev %d %s %sNIC %s\n",
 		    adap->params.vpd.id,
 		    CHELSIO_CHIP_RELEASE(adap->params.chip), buf,
-		    is_offload(adap) ? "R" : "", adap->params.pci.width, spd,
+		    is_offload(adap) ? "R" : "",
 		    (adap->flags & USING_MSIX) ? " MSI-X" :
 		    (adap->flags & USING_MSI) ? " MSI" : "");
 	netdev_info(dev, "S/N: %s, P/N: %s\n",
@@ -4787,8 +4805,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* configure SGE_STAT_CFG_A to read WC stats */
 	if (!is_t4(adapter->params.chip))
-		t4_write_reg(adapter, SGE_STAT_CFG_A,
-			     STATSOURCE_T5_V(7) | STATMODE_V(0));
+		t4_write_reg(adapter, SGE_STAT_CFG_A, STATSOURCE_T5_V(7) |
+			     (is_t5(adapter->params.chip) ? STATMODE_V(0) :
+			      T6_STATMODE_V(0)));
 
 	for_each_port(adapter, i) {
 		struct net_device *netdev;
@@ -4865,15 +4884,25 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 #if IS_ENABLED(CONFIG_IPV6)
-	adapter->clipt = t4_init_clip_tbl(adapter->clipt_start,
-					  adapter->clipt_end);
-	if (!adapter->clipt) {
-		/* We tolerate a lack of clip_table, giving up
-		 * some functionality
+	if ((CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5) &&
+	    (!(t4_read_reg(adapter, LE_DB_CONFIG_A) & ASLIPCOMPEN_F))) {
+		/* CLIP functionality is not present in hardware,
+		 * hence disable all offload features
 		 */
 		dev_warn(&pdev->dev,
-			 "could not allocate Clip table, continuing\n");
+			 "CLIP not enabled in hardware, continuing\n");
 		adapter->params.offload = 0;
+	} else {
+		adapter->clipt = t4_init_clip_tbl(adapter->clipt_start,
+						  adapter->clipt_end);
+		if (!adapter->clipt) {
+			/* We tolerate a lack of clip_table, giving up
+			 * some functionality
+			 */
+			dev_warn(&pdev->dev,
+				 "could not allocate Clip table, continuing\n");
+			adapter->params.offload = 0;
+		}
 	}
 #endif
 	if (is_offload(adapter) && tid_init(&adapter->tids) < 0) {
@@ -4904,6 +4933,9 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	else if (msi > 0 && pci_enable_msi(pdev) == 0)
 		adapter->flags |= USING_MSI;
 
+	/* check for PCI Express bandwidth capabiltites */
+	cxgb4_check_pcie_caps(adapter);
+
 	err = init_rss(adapter);
 	if (err)
 		goto out_free_dev;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/l2t.c b/drivers/net/ethernet/chelsio/cxgb4/l2t.c
index ac27898c6ab0..5b0f3ef348e9 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/l2t.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/l2t.c
@@ -66,7 +66,7 @@ struct l2t_data {
 
 static inline unsigned int vlan_prio(const struct l2t_entry *e)
 {
-	return e->vlan >> 13;
+	return e->vlan >> VLAN_PRIO_SHIFT;
 }
 
 static inline void l2t_hold(struct l2t_data *d, struct l2t_entry *e)
@@ -161,8 +161,7 @@ static int write_l2e(struct adapter *adap, struct l2t_entry *e, int sync)
 		memcpy(e->dmac, e->neigh->ha, sizeof(e->dmac));
 	memcpy(req->dst_mac, e->dmac, sizeof(req->dst_mac));
 
-	set_wr_txq(skb, CPL_PRIORITY_CONTROL, 0);
-	t4_ofld_send(adap, skb);
+	t4_mgmt_tx(adap, skb);
 
 	if (sync && e->state != L2T_STATE_SWITCHING)
 		e->state = L2T_STATE_SYNC_WRITE;
@@ -175,14 +174,10 @@ static int write_l2e(struct adapter *adap, struct l2t_entry *e, int sync)
  */
 static void send_pending(struct adapter *adap, struct l2t_entry *e)
 {
-	while (e->arpq_head) {
-		struct sk_buff *skb = e->arpq_head;
+	struct sk_buff *skb;
 
-		e->arpq_head = skb->next;
-		skb->next = NULL;
+	while ((skb = __skb_dequeue(&e->arpq)) != NULL)
 		t4_ofld_send(adap, skb);
-	}
-	e->arpq_tail = NULL;
 }
 
 /*
@@ -222,12 +217,7 @@ void do_l2t_write_rpl(struct adapter *adap, const struct cpl_l2t_write_rpl *rpl)
  */
 static inline void arpq_enqueue(struct l2t_entry *e, struct sk_buff *skb)
 {
-	skb->next = NULL;
-	if (e->arpq_head)
-		e->arpq_tail->next = skb;
-	else
-		e->arpq_head = skb;
-	e->arpq_tail = skb;
+	__skb_queue_tail(&e->arpq, skb);
 }
 
 int cxgb4_l2t_send(struct net_device *dev, struct sk_buff *skb,
@@ -259,7 +249,8 @@ again:
 		if (e->state == L2T_STATE_RESOLVING &&
 		    !neigh_event_send(e->neigh, NULL)) {
 			spin_lock_bh(&e->lock);
-			if (e->state == L2T_STATE_RESOLVING && e->arpq_head)
+			if (e->state == L2T_STATE_RESOLVING &&
+			    !skb_queue_empty(&e->arpq))
 				write_l2e(adap, e, 1);
 			spin_unlock_bh(&e->lock);
 		}
@@ -305,12 +296,82 @@ found:
 	return e;
 }
 
-/*
- * Called when an L2T entry has no more users.
+static struct l2t_entry *find_or_alloc_l2e(struct l2t_data *d, u16 vlan,
+					   u8 port, u8 *dmac)
+{
+	struct l2t_entry *end, *e, **p;
+	struct l2t_entry *first_free = NULL;
+
+	for (e = &d->l2tab[0], end = &d->l2tab[d->l2t_size]; e != end; ++e) {
+		if (atomic_read(&e->refcnt) == 0) {
+			if (!first_free)
+				first_free = e;
+		} else {
+			if (e->state == L2T_STATE_SWITCHING) {
+				if (ether_addr_equal(e->dmac, dmac) &&
+				    (e->vlan == vlan) && (e->lport == port))
+					goto exists;
+			}
+		}
+	}
+
+	if (first_free) {
+		e = first_free;
+		goto found;
+	}
+
+	return NULL;
+
+found:
+	/* The entry we found may be an inactive entry that is
+	 * presently in the hash table.  We need to remove it.
+	 */
+	if (e->state < L2T_STATE_SWITCHING)
+		for (p = &d->l2tab[e->hash].first; *p; p = &(*p)->next)
+			if (*p == e) {
+				*p = e->next;
+				e->next = NULL;
+				break;
+			}
+	e->state = L2T_STATE_UNUSED;
+
+exists:
+	return e;
+}
+
+/* Called when an L2T entry has no more users.  The entry is left in the hash
+ * table since it is likely to be reused but we also bump nfree to indicate
+ * that the entry can be reallocated for a different neighbor.  We also drop
+ * the existing neighbor reference in case the neighbor is going away and is
+ * waiting on our reference.
+ *
+ * Because entries can be reallocated to other neighbors once their ref count
+ * drops to 0 we need to take the entry's lock to avoid races with a new
+ * incarnation.
  */
+static void _t4_l2e_free(struct l2t_entry *e)
+{
+	struct l2t_data *d;
+	struct sk_buff *skb;
+
+	if (atomic_read(&e->refcnt) == 0) {  /* hasn't been recycled */
+		if (e->neigh) {
+			neigh_release(e->neigh);
+			e->neigh = NULL;
+		}
+		while ((skb = __skb_dequeue(&e->arpq)) != NULL)
+			kfree_skb(skb);
+	}
+
+	d = container_of(e, struct l2t_data, l2tab[e->idx]);
+	atomic_inc(&d->nfree);
+}
+
+/* Locked version of _t4_l2e_free */
 static void t4_l2e_free(struct l2t_entry *e)
 {
 	struct l2t_data *d;
+	struct sk_buff *skb;
 
 	spin_lock_bh(&e->lock);
 	if (atomic_read(&e->refcnt) == 0) {  /* hasn't been recycled */
@@ -318,13 +379,8 @@ static void t4_l2e_free(struct l2t_entry *e)
 			neigh_release(e->neigh);
 			e->neigh = NULL;
 		}
-		while (e->arpq_head) {
-			struct sk_buff *skb = e->arpq_head;
-
-			e->arpq_head = skb->next;
+		while ((skb = __skb_dequeue(&e->arpq)) != NULL)
 			kfree_skb(skb);
-		}
-		e->arpq_tail = NULL;
 	}
 	spin_unlock_bh(&e->lock);
 
@@ -457,18 +513,19 @@ EXPORT_SYMBOL(cxgb4_select_ntuple);
  * on the arpq head.  If a packet specifies a failure handler it is invoked,
  * otherwise the packet is sent to the device.
  */
-static void handle_failed_resolution(struct adapter *adap, struct sk_buff *arpq)
+static void handle_failed_resolution(struct adapter *adap, struct l2t_entry *e)
 {
-	while (arpq) {
-		struct sk_buff *skb = arpq;
+	struct sk_buff *skb;
+
+	while ((skb = __skb_dequeue(&e->arpq)) != NULL) {
 		const struct l2t_skb_cb *cb = L2T_SKB_CB(skb);
 
-		arpq = skb->next;
-		skb->next = NULL;
+		spin_unlock(&e->lock);
 		if (cb->arp_err_handler)
 			cb->arp_err_handler(cb->handle, skb);
 		else
 			t4_ofld_send(adap, skb);
+		spin_lock(&e->lock);
 	}
 }
 
@@ -479,7 +536,7 @@ static void handle_failed_resolution(struct adapter *adap, struct sk_buff *arpq)
 void t4_l2t_update(struct adapter *adap, struct neighbour *neigh)
 {
 	struct l2t_entry *e;
-	struct sk_buff *arpq = NULL;
+	struct sk_buff_head *arpq = NULL;
 	struct l2t_data *d = adap->l2t;
 	int addr_len = neigh->tbl->key_len;
 	u32 *addr = (u32 *) neigh->primary_key;
@@ -506,10 +563,9 @@ void t4_l2t_update(struct adapter *adap, struct neighbour *neigh)
 
 	if (e->state == L2T_STATE_RESOLVING) {
 		if (neigh->nud_state & NUD_FAILED) {
-			arpq = e->arpq_head;
-			e->arpq_head = e->arpq_tail = NULL;
+			arpq = &e->arpq;
 		} else if ((neigh->nud_state & (NUD_CONNECTED | NUD_STALE)) &&
-			   e->arpq_head) {
+			   !skb_queue_empty(&e->arpq)) {
 			write_l2e(adap, e, 1);
 		}
 	} else {
@@ -519,43 +575,66 @@ void t4_l2t_update(struct adapter *adap, struct neighbour *neigh)
 			write_l2e(adap, e, 0);
 	}
 
-	spin_unlock_bh(&e->lock);
-
 	if (arpq)
-		handle_failed_resolution(adap, arpq);
+		handle_failed_resolution(adap, e);
+	spin_unlock_bh(&e->lock);
 }
 
 /* Allocate an L2T entry for use by a switching rule.  Such need to be
  * explicitly freed and while busy they are not on any hash chain, so normal
  * address resolution updates do not see them.
  */
-struct l2t_entry *t4_l2t_alloc_switching(struct l2t_data *d)
+struct l2t_entry *t4_l2t_alloc_switching(struct adapter *adap, u16 vlan,
+					 u8 port, u8 *eth_addr)
 {
+	struct l2t_data *d = adap->l2t;
 	struct l2t_entry *e;
+	int ret;
 
 	write_lock_bh(&d->lock);
-	e = alloc_l2e(d);
+	e = find_or_alloc_l2e(d, vlan, port, eth_addr);
 	if (e) {
 		spin_lock(&e->lock);          /* avoid race with t4_l2t_free */
-		e->state = L2T_STATE_SWITCHING;
-		atomic_set(&e->refcnt, 1);
+		if (!atomic_read(&e->refcnt)) {
+			e->state = L2T_STATE_SWITCHING;
+			e->vlan = vlan;
+			e->lport = port;
+			ether_addr_copy(e->dmac, eth_addr);
+			atomic_set(&e->refcnt, 1);
+			ret = write_l2e(adap, e, 0);
+			if (ret < 0) {
+				_t4_l2e_free(e);
+				spin_unlock(&e->lock);
+				write_unlock_bh(&d->lock);
+				return NULL;
+			}
+		} else {
+			atomic_inc(&e->refcnt);
+		}
+
 		spin_unlock(&e->lock);
 	}
 	write_unlock_bh(&d->lock);
 	return e;
 }
 
-/* Sets/updates the contents of a switching L2T entry that has been allocated
- * with an earlier call to @t4_l2t_alloc_switching.
+/**
+ * @dev: net_device pointer
+ * @vlan: VLAN Id
+ * @port: Associated port
+ * @dmac: Destination MAC address to add to L2T
+ * Returns pointer to the allocated l2t entry
+ *
+ * Allocates an L2T entry for use by switching rule of a filter
  */
-int t4_l2t_set_switching(struct adapter *adap, struct l2t_entry *e, u16 vlan,
-		u8 port, u8 *eth_addr)
+struct l2t_entry *cxgb4_l2t_alloc_switching(struct net_device *dev, u16 vlan,
+					    u8 port, u8 *dmac)
 {
-	e->vlan = vlan;
-	e->lport = port;
-	memcpy(e->dmac, eth_addr, ETH_ALEN);
-	return write_l2e(adap, e, 0);
+	struct adapter *adap = netdev2adap(dev);
+
+	return t4_l2t_alloc_switching(adap, vlan, port, dmac);
 }
+EXPORT_SYMBOL(cxgb4_l2t_alloc_switching);
 
 struct l2t_data *t4_init_l2t(unsigned int l2t_start, unsigned int l2t_end)
 {
@@ -585,6 +664,7 @@ struct l2t_data *t4_init_l2t(unsigned int l2t_start, unsigned int l2t_end)
 		d->l2tab[i].state = L2T_STATE_UNUSED;
 		spin_lock_init(&d->l2tab[i].lock);
 		atomic_set(&d->l2tab[i].refcnt, 0);
+		skb_queue_head_init(&d->l2tab[i].arpq);
 	}
 	return d;
 }
@@ -619,7 +699,8 @@ static char l2e_state(const struct l2t_entry *e)
 	case L2T_STATE_VALID: return 'V';
 	case L2T_STATE_STALE: return 'S';
 	case L2T_STATE_SYNC_WRITE: return 'W';
-	case L2T_STATE_RESOLVING: return e->arpq_head ? 'A' : 'R';
+	case L2T_STATE_RESOLVING:
+		return skb_queue_empty(&e->arpq) ? 'R' : 'A';
 	case L2T_STATE_SWITCHING: return 'X';
 	default:
 		return 'U';
diff --git a/drivers/net/ethernet/chelsio/cxgb4/l2t.h b/drivers/net/ethernet/chelsio/cxgb4/l2t.h
index b38dc526aad5..4e2d47ac102b 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/l2t.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/l2t.h
@@ -76,8 +76,7 @@ struct l2t_entry {
 	struct neighbour *neigh;    /* associated neighbour */
 	struct l2t_entry *first;    /* start of hash chain */
 	struct l2t_entry *next;     /* next l2t_entry on chain */
-	struct sk_buff *arpq_head;  /* queue of packets awaiting resolution */
-	struct sk_buff *arpq_tail;
+	struct sk_buff_head arpq;   /* packet queue awaiting resolution */
 	spinlock_t lock;
 	atomic_t refcnt;            /* entry reference count */
 	u16 hash;                   /* hash bucket the entry is on */
@@ -114,10 +113,11 @@ struct l2t_entry *cxgb4_l2t_get(struct l2t_data *d, struct neighbour *neigh,
 				unsigned int priority);
 u64 cxgb4_select_ntuple(struct net_device *dev,
 			const struct l2t_entry *l2t);
+struct l2t_entry *cxgb4_l2t_alloc_switching(struct net_device *dev, u16 vlan,
+					    u8 port, u8 *dmac);
 void t4_l2t_update(struct adapter *adap, struct neighbour *neigh);
-struct l2t_entry *t4_l2t_alloc_switching(struct l2t_data *d);
-int t4_l2t_set_switching(struct adapter *adap, struct l2t_entry *e, u16 vlan,
-			 u8 port, u8 *eth_addr);
+struct l2t_entry *t4_l2t_alloc_switching(struct adapter *adap, u16 vlan,
+					 u8 port, u8 *dmac);
 struct l2t_data *t4_init_l2t(unsigned int l2t_start, unsigned int l2t_end);
 void do_l2t_write_rpl(struct adapter *p, const struct cpl_l2t_write_rpl *rpl);
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index b7b93e7a643d..b4eb4680a27c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -406,7 +406,7 @@ static void free_tx_desc(struct adapter *adap, struct sge_txq *q,
  */
 static inline int reclaimable(const struct sge_txq *q)
 {
-	int hw_cidx = ntohs(q->stat->cidx);
+	int hw_cidx = ntohs(ACCESS_ONCE(q->stat->cidx));
 	hw_cidx -= q->cidx;
 	return hw_cidx < 0 ? hw_cidx + q->size : hw_cidx;
 }
@@ -613,6 +613,7 @@ static unsigned int refill_fl(struct adapter *adap, struct sge_fl *q, int n,
 				       PCI_DMA_FROMDEVICE);
 		if (unlikely(dma_mapping_error(adap->pdev_dev, mapping))) {
 			__free_pages(pg, s->fl_pg_order);
+			q->mapping_err++;
 			goto out;   /* do not try small pages for this error */
 		}
 		mapping |= RX_LARGE_PG_BUF;
@@ -642,6 +643,7 @@ alloc_small_pages:
 				       PCI_DMA_FROMDEVICE);
 		if (unlikely(dma_mapping_error(adap->pdev_dev, mapping))) {
 			put_page(pg);
+			q->mapping_err++;
 			goto out;
 		}
 		*d++ = cpu_to_be64(mapping);
@@ -663,6 +665,7 @@ out:	cred = q->avail - cred;
 
 	if (unlikely(fl_starving(adap, q))) {
 		smp_wmb();
+		q->low++;
 		set_bit(q->cntxt_id - adap->sge.egr_start,
 			adap->sge.starving_fl);
 	}
@@ -1029,6 +1032,30 @@ static void inline_tx_skb(const struct sk_buff *skb, const struct sge_txq *q,
 		*p = 0;
 }
 
+static void *inline_tx_skb_header(const struct sk_buff *skb,
+				  const struct sge_txq *q,  void *pos,
+				  int length)
+{
+	u64 *p;
+	int left = (void *)q->stat - pos;
+
+	if (likely(length <= left)) {
+		memcpy(pos, skb->data, length);
+		pos += length;
+	} else {
+		memcpy(pos, skb->data, left);
+		memcpy(q->desc, skb->data + left, length - left);
+		pos = (void *)q->desc + (length - left);
+	}
+	/* 0-pad to multiple of 16 */
+	p = PTR_ALIGN(pos, 8);
+	if ((uintptr_t)p & 8) {
+		*p = 0;
+		return p + 1;
+	}
+	return p;
+}
+
 /*
  * Figure out what HW csum a packet wants and return the appropriate control
  * bits.
@@ -1320,7 +1347,7 @@ out_free:	dev_kfree_skb_any(skb);
  */
 static inline void reclaim_completed_tx_imm(struct sge_txq *q)
 {
-	int hw_cidx = ntohs(q->stat->cidx);
+	int hw_cidx = ntohs(ACCESS_ONCE(q->stat->cidx));
 	int reclaim = hw_cidx - q->cidx;
 
 	if (reclaim < 0)
@@ -1542,24 +1569,50 @@ static void ofldtxq_stop(struct sge_ofld_txq *q, struct sk_buff *skb)
 }
 
 /**
- *	service_ofldq - restart a suspended offload queue
+ *	service_ofldq - service/restart a suspended offload queue
  *	@q: the offload queue
  *
- *	Services an offload Tx queue by moving packets from its packet queue
- *	to the HW Tx ring.  The function starts and ends with the queue locked.
+ *	Services an offload Tx queue by moving packets from its Pending Send
+ *	Queue to the Hardware TX ring.  The function starts and ends with the
+ *	Send Queue locked, but drops the lock while putting the skb at the
+ *	head of the Send Queue onto the Hardware TX Ring.  Dropping the lock
+ *	allows more skbs to be added to the Send Queue by other threads.
+ *	The packet being processed at the head of the Pending Send Queue is
+ *	left on the queue in case we experience DMA Mapping errors, etc.
+ *	and need to give up and restart later.
+ *
+ *	service_ofldq() can be thought of as a task which opportunistically
+ *	uses other threads execution contexts.  We use the Offload Queue
+ *	boolean "service_ofldq_running" to make sure that only one instance
+ *	is ever running at a time ...
  */
 static void service_ofldq(struct sge_ofld_txq *q)
 {
-	u64 *pos;
+	u64 *pos, *before, *end;
 	int credits;
 	struct sk_buff *skb;
+	struct sge_txq *txq;
+	unsigned int left;
 	unsigned int written = 0;
 	unsigned int flits, ndesc;
 
+	/* If another thread is currently in service_ofldq() processing the
+	 * Pending Send Queue then there's nothing to do. Otherwise, flag
+	 * that we're doing the work and continue.  Examining/modifying
+	 * the Offload Queue boolean "service_ofldq_running" must be done
+	 * while holding the Pending Send Queue Lock.
+	 */
+	if (q->service_ofldq_running)
+		return;
+	q->service_ofldq_running = true;
+
 	while ((skb = skb_peek(&q->sendq)) != NULL && !q->full) {
-		/*
-		 * We drop the lock but leave skb on sendq, thus retaining
-		 * exclusive access to the state of the queue.
+		/* We drop the lock while we're working with the skb at the
+		 * head of the Pending Send Queue.  This allows more skbs to
+		 * be added to the Pending Send Queue while we're working on
+		 * this one.  We don't need to lock to guard the TX Ring
+		 * updates because only one thread of execution is ever
+		 * allowed into service_ofldq() at a time.
 		 */
 		spin_unlock(&q->sendq.lock);
 
@@ -1583,9 +1636,32 @@ static void service_ofldq(struct sge_ofld_txq *q)
 		} else {
 			int last_desc, hdr_len = skb_transport_offset(skb);
 
-			memcpy(pos, skb->data, hdr_len);
-			write_sgl(skb, &q->q, (void *)pos + hdr_len,
-				  pos + flits, hdr_len,
+			/* The WR headers  may not fit within one descriptor.
+			 * So we need to deal with wrap-around here.
+			 */
+			before = (u64 *)pos;
+			end = (u64 *)pos + flits;
+			txq = &q->q;
+			pos = (void *)inline_tx_skb_header(skb, &q->q,
+							   (void *)pos,
+							   hdr_len);
+			if (before > (u64 *)pos) {
+				left = (u8 *)end - (u8 *)txq->stat;
+				end = (void *)txq->desc + left;
+			}
+
+			/* If current position is already at the end of the
+			 * ofld queue, reset the current to point to
+			 * start of the queue and update the end ptr as well.
+			 */
+			if (pos == (u64 *)txq->stat) {
+				left = (u8 *)end - (u8 *)txq->stat;
+				end = (void *)txq->desc + left;
+				pos = (void *)txq->desc;
+			}
+
+			write_sgl(skb, &q->q, (void *)pos,
+				  end, hdr_len,
 				  (dma_addr_t *)skb->head);
 #ifdef CONFIG_NEED_DMA_MAP_STATE
 			skb->dev = q->adap->port[0];
@@ -1604,6 +1680,11 @@ static void service_ofldq(struct sge_ofld_txq *q)
 			written = 0;
 		}
 
+		/* Reacquire the Pending Send Queue Lock so we can unlink the
+		 * skb we've just successfully transferred to the TX Ring and
+		 * loop for the next skb which may be at the head of the
+		 * Pending Send Queue.
+		 */
 		spin_lock(&q->sendq.lock);
 		__skb_unlink(skb, &q->sendq);
 		if (is_ofld_imm(skb))
@@ -1611,6 +1692,11 @@ static void service_ofldq(struct sge_ofld_txq *q)
 	}
 	if (likely(written))
 		ring_tx_db(q->adap, &q->q, written);
+
+	/*Indicate that no thread is processing the Pending Send Queue
+	 * currently.
+	 */
+	q->service_ofldq_running = false;
 }
 
 /**
@@ -1624,9 +1710,19 @@ static int ofld_xmit(struct sge_ofld_txq *q, struct sk_buff *skb)
 {
 	skb->priority = calc_tx_flits_ofld(skb);       /* save for restart */
 	spin_lock(&q->sendq.lock);
+
+	/* Queue the new skb onto the Offload Queue's Pending Send Queue.  If
+	 * that results in this new skb being the only one on the queue, start
+	 * servicing it.  If there are other skbs already on the list, then
+	 * either the queue is currently being processed or it's been stopped
+	 * for some reason and it'll be restarted at a later time.  Restart
+	 * paths are triggered by events like experiencing a DMA Mapping Error
+	 * or filling the Hardware TX Ring.
+	 */
 	__skb_queue_tail(&q->sendq, skb);
 	if (q->sendq.qlen == 1)
 		service_ofldq(q);
+
 	spin_unlock(&q->sendq.lock);
 	return NET_XMIT_SUCCESS;
 }
@@ -1864,7 +1960,6 @@ static void do_gro(struct sge_eth_rxq *rxq, const struct pkt_gl *gl,
 	skb->truesize += skb->data_len;
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
 	skb_record_rx_queue(skb, rxq->rspq.idx);
-	skb_mark_napi_id(skb, &rxq->rspq.napi);
 	pi = netdev_priv(skb->dev);
 	if (pi->rxtstamp)
 		cxgb4_sgetim_to_hwtstamp(adapter, skb_hwtstamps(skb),
@@ -2193,7 +2288,7 @@ static int napi_rx_handler(struct napi_struct *napi, int budget)
 	if (likely(work_done < budget)) {
 		int timer_index;
 
-		napi_complete(napi);
+		napi_complete_done(napi, work_done);
 		timer_index = QINTR_TIMER_IDX_G(q->next_intr_params);
 
 		if (q->adaptive_rx) {
@@ -2460,7 +2555,8 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 	iq->size = roundup(iq->size, 16);
 
 	iq->desc = alloc_ring(adap->pdev_dev, iq->size, iq->iqe_len, 0,
-			      &iq->phys_addr, NULL, 0, NUMA_NO_NODE);
+			      &iq->phys_addr, NULL, 0,
+			      dev_to_node(adap->pdev_dev));
 	if (!iq->desc)
 		return -ENOMEM;
 
@@ -2500,7 +2596,8 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		fl->size = roundup(fl->size, 8);
 		fl->desc = alloc_ring(adap->pdev_dev, fl->size, sizeof(__be64),
 				      sizeof(struct rx_sw_desc), &fl->addr,
-				      &fl->sdesc, s->stat_len, NUMA_NO_NODE);
+				      &fl->sdesc, s->stat_len,
+				      dev_to_node(adap->pdev_dev));
 		if (!fl->desc)
 			goto fl_nomem;
 
@@ -2528,7 +2625,6 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		goto err;
 
 	netif_napi_add(dev, &iq->napi, napi_rx_handler, 64);
-	napi_hash_add(&iq->napi);
 	iq->cur_desc = iq->desc;
 	iq->cidx = 0;
 	iq->gen = 1;
@@ -2574,8 +2670,9 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 	 * simple (and hopefully less wrong).
 	 */
 	if (!is_t4(adap->params.chip) && cong >= 0) {
-		u32 param, val;
+		u32 param, val, ch_map = 0;
 		int i;
+		u16 cng_ch_bits_log = adap->params.arch.cng_ch_bits_log;
 
 		param = (FW_PARAMS_MNEM_V(FW_PARAMS_MNEM_DMAQ) |
 			 FW_PARAMS_PARAM_X_V(FW_PARAMS_PARAM_DMAQ_CONM_CTXT) |
@@ -2587,9 +2684,9 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 			    CONMCTXT_CNGTPMODE_V(CONMCTXT_CNGTPMODE_CHANNEL_X);
 			for (i = 0; i < 4; i++) {
 				if (cong & (1 << i))
-					val |=
-					     CONMCTXT_CNGCHMAP_V(1 << (i << 2));
+					ch_map |= 1 << (i << cng_ch_bits_log);
 			}
+			val |= CONMCTXT_CNGCHMAP_V(ch_map);
 		}
 		ret = t4_set_params(adap, adap->mbox, adap->pf, 0, 1,
 				    &param, &val);
@@ -2884,7 +2981,7 @@ void t4_free_sge_resources(struct adapter *adap)
 	}
 
 	/* clean up RDMA and iSCSI Rx queues */
-	t4_free_ofld_rxqs(adap, adap->sge.ofldqsets, adap->sge.ofldrxq);
+	t4_free_ofld_rxqs(adap, adap->sge.iscsiqsets, adap->sge.iscsirxq);
 	t4_free_ofld_rxqs(adap, adap->sge.rdmaqs, adap->sge.rdmarxq);
 	t4_free_ofld_rxqs(adap, adap->sge.rdmaciqs, adap->sge.rdmaciq);
 
@@ -3077,8 +3174,7 @@ static int t4_sge_init_soft(struct adapter *adap)
 int t4_sge_init(struct adapter *adap)
 {
 	struct sge *s = &adap->sge;
-	u32 sge_control, sge_control2, sge_conm_ctrl;
-	unsigned int ingpadboundary, ingpackboundary;
+	u32 sge_control, sge_conm_ctrl;
 	int ret, egress_threshold;
 
 	/*
@@ -3089,35 +3185,7 @@ int t4_sge_init(struct adapter *adap)
 	s->pktshift = PKTSHIFT_G(sge_control);
 	s->stat_len = (sge_control & EGRSTATUSPAGESIZE_F) ? 128 : 64;
 
-	/* T4 uses a single control field to specify both the PCIe Padding and
-	 * Packing Boundary.  T5 introduced the ability to specify these
-	 * separately.  The actual Ingress Packet Data alignment boundary
-	 * within Packed Buffer Mode is the maximum of these two
-	 * specifications.  (Note that it makes no real practical sense to
-	 * have the Pading Boudary be larger than the Packing Boundary but you
-	 * could set the chip up that way and, in fact, legacy T4 code would
-	 * end doing this because it would initialize the Padding Boundary and
-	 * leave the Packing Boundary initialized to 0 (16 bytes).)
-	 */
-	ingpadboundary = 1 << (INGPADBOUNDARY_G(sge_control) +
-			       INGPADBOUNDARY_SHIFT_X);
-	if (is_t4(adap->params.chip)) {
-		s->fl_align = ingpadboundary;
-	} else {
-		/* T5 has a different interpretation of one of the PCIe Packing
-		 * Boundary values.
-		 */
-		sge_control2 = t4_read_reg(adap, SGE_CONTROL2_A);
-		ingpackboundary = INGPACKBOUNDARY_G(sge_control2);
-		if (ingpackboundary == INGPACKBOUNDARY_16B_X)
-			ingpackboundary = 16;
-		else
-			ingpackboundary = 1 << (ingpackboundary +
-						INGPACKBOUNDARY_SHIFT_X);
-
-		s->fl_align = max(ingpadboundary, ingpackboundary);
-	}
-
+	s->fl_align = t4_fl_pkt_align(adap);
 	ret = t4_sge_init_soft(adap);
 	if (ret < 0)
 		return ret;
@@ -3135,10 +3203,21 @@ int t4_sge_init(struct adapter *adap)
 	 * buffers.
 	 */
 	sge_conm_ctrl = t4_read_reg(adap, SGE_CONM_CTRL_A);
-	if (is_t4(adap->params.chip))
+	switch (CHELSIO_CHIP_VERSION(adap->params.chip)) {
+	case CHELSIO_T4:
 		egress_threshold = EGRTHRESHOLD_G(sge_conm_ctrl);
-	else
+		break;
+	case CHELSIO_T5:
 		egress_threshold = EGRTHRESHOLDPACKING_G(sge_conm_ctrl);
+		break;
+	case CHELSIO_T6:
+		egress_threshold = T6_EGRTHRESHOLDPACKING_G(sge_conm_ctrl);
+		break;
+	default:
+		dev_err(adap->pdev_dev, "Unsupported Chip version %d\n",
+			CHELSIO_CHIP_VERSION(adap->params.chip));
+		return -EINVAL;
+	}
 	s->fl_starve_thres = 2*egress_threshold + 1;
 
 	t4_idma_monitor_init(adap, &s->idma_monitor);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index cf61a5869c6e..636b4691f252 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -1942,8 +1942,12 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x1190, 0x1194,
 		0x11a0, 0x11a4,
 		0x11b0, 0x11b4,
-		0x11fc, 0x1254,
-		0x1280, 0x133c,
+		0x11fc, 0x1258,
+		0x1280, 0x12d4,
+		0x12d9, 0x12d9,
+		0x12de, 0x12de,
+		0x12e3, 0x12e3,
+		0x12e8, 0x133c,
 		0x1800, 0x18fc,
 		0x3000, 0x302c,
 		0x3060, 0x30b0,
@@ -1973,7 +1977,7 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x5e50, 0x5e94,
 		0x5ea0, 0x5eb0,
 		0x5ec0, 0x5ec0,
-		0x5ec8, 0x5ecc,
+		0x5ec8, 0x5ed0,
 		0x6000, 0x6020,
 		0x6028, 0x6040,
 		0x6058, 0x609c,
@@ -2048,7 +2052,8 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x19150, 0x19194,
 		0x1919c, 0x191b0,
 		0x191d0, 0x191e8,
-		0x19238, 0x192b0,
+		0x19238, 0x19290,
+		0x192a4, 0x192b0,
 		0x192bc, 0x192bc,
 		0x19348, 0x1934c,
 		0x193f8, 0x19418,
@@ -2442,7 +2447,8 @@ void t4_get_regs(struct adapter *adap, void *buf, size_t buf_size)
 		0x40280, 0x40280,
 		0x40304, 0x40304,
 		0x40330, 0x4033c,
-		0x41304, 0x413c8,
+		0x41304, 0x413b8,
+		0x413c0, 0x413c8,
 		0x413d0, 0x413dc,
 		0x413f0, 0x413f0,
 		0x41400, 0x4140c,
@@ -5254,7 +5260,7 @@ void t4_pmtx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[])
 	int i;
 	u32 data[2];
 
-	for (i = 0; i < PM_NSTATS; i++) {
+	for (i = 0; i < adap->params.arch.pm_stats_cnt; i++) {
 		t4_write_reg(adap, PM_TX_STAT_CONFIG_A, i + 1);
 		cnt[i] = t4_read_reg(adap, PM_TX_STAT_COUNT_A);
 		if (is_t4(adap->params.chip)) {
@@ -5281,7 +5287,7 @@ void t4_pmrx_get_stats(struct adapter *adap, u32 cnt[], u64 cycles[])
 	int i;
 	u32 data[2];
 
-	for (i = 0; i < PM_NSTATS; i++) {
+	for (i = 0; i < adap->params.arch.pm_stats_cnt; i++) {
 		t4_write_reg(adap, PM_RX_STAT_CONFIG_A, i + 1);
 		cnt[i] = t4_read_reg(adap, PM_RX_STAT_COUNT_A);
 		if (is_t4(adap->params.chip)) {
@@ -5310,7 +5316,14 @@ unsigned int t4_get_mps_bg_map(struct adapter *adap, int idx)
 
 	if (n == 0)
 		return idx == 0 ? 0xf : 0;
-	if (n == 1)
+	/* In T6 (which is a 2 port card),
+	 * port 0 is mapped to channel 0 and port 1 is mapped to channel 1.
+	 * For 2 port T4/T5 adapter,
+	 * port 0 is mapped to channel 0 and 1,
+	 * port 1 is mapped to channel 2 and 3.
+	 */
+	if ((n == 1) &&
+	    (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5))
 		return idx < 2 ? (3 << (2 * idx)) : 0;
 	return 1 << idx;
 }
@@ -5689,6 +5702,39 @@ void t4_sge_decode_idma_state(struct adapter *adapter, int state)
 		"IDMA_FL_SEND_PADDING",
 		"IDMA_FL_SEND_COMPLETION_TO_IMSG",
 	};
+	static const char * const t6_decode[] = {
+		"IDMA_IDLE",
+		"IDMA_PUSH_MORE_CPL_FIFO",
+		"IDMA_PUSH_CPL_MSG_HEADER_TO_FIFO",
+		"IDMA_SGEFLRFLUSH_SEND_PCIEHDR",
+		"IDMA_PHYSADDR_SEND_PCIEHDR",
+		"IDMA_PHYSADDR_SEND_PAYLOAD_FIRST",
+		"IDMA_PHYSADDR_SEND_PAYLOAD",
+		"IDMA_FL_REQ_DATA_FL",
+		"IDMA_FL_DROP",
+		"IDMA_FL_DROP_SEND_INC",
+		"IDMA_FL_H_REQ_HEADER_FL",
+		"IDMA_FL_H_SEND_PCIEHDR",
+		"IDMA_FL_H_PUSH_CPL_FIFO",
+		"IDMA_FL_H_SEND_CPL",
+		"IDMA_FL_H_SEND_IP_HDR_FIRST",
+		"IDMA_FL_H_SEND_IP_HDR",
+		"IDMA_FL_H_REQ_NEXT_HEADER_FL",
+		"IDMA_FL_H_SEND_NEXT_PCIEHDR",
+		"IDMA_FL_H_SEND_IP_HDR_PADDING",
+		"IDMA_FL_D_SEND_PCIEHDR",
+		"IDMA_FL_D_SEND_CPL_AND_IP_HDR",
+		"IDMA_FL_D_REQ_NEXT_DATA_FL",
+		"IDMA_FL_SEND_PCIEHDR",
+		"IDMA_FL_PUSH_CPL_FIFO",
+		"IDMA_FL_SEND_CPL",
+		"IDMA_FL_SEND_PAYLOAD_FIRST",
+		"IDMA_FL_SEND_PAYLOAD",
+		"IDMA_FL_REQ_NEXT_DATA_FL",
+		"IDMA_FL_SEND_NEXT_PCIEHDR",
+		"IDMA_FL_SEND_PADDING",
+		"IDMA_FL_SEND_COMPLETION_TO_IMSG",
+	};
 	static const u32 sge_regs[] = {
 		SGE_DEBUG_DATA_LOW_INDEX_2_A,
 		SGE_DEBUG_DATA_LOW_INDEX_3_A,
@@ -5697,6 +5743,32 @@ void t4_sge_decode_idma_state(struct adapter *adapter, int state)
 	const char **sge_idma_decode;
 	int sge_idma_decode_nstates;
 	int i;
+	unsigned int chip_version = CHELSIO_CHIP_VERSION(adapter->params.chip);
+
+	/* Select the right set of decode strings to dump depending on the
+	 * adapter chip type.
+	 */
+	switch (chip_version) {
+	case CHELSIO_T4:
+		sge_idma_decode = (const char **)t4_decode;
+		sge_idma_decode_nstates = ARRAY_SIZE(t4_decode);
+		break;
+
+	case CHELSIO_T5:
+		sge_idma_decode = (const char **)t5_decode;
+		sge_idma_decode_nstates = ARRAY_SIZE(t5_decode);
+		break;
+
+	case CHELSIO_T6:
+		sge_idma_decode = (const char **)t6_decode;
+		sge_idma_decode_nstates = ARRAY_SIZE(t6_decode);
+		break;
+
+	default:
+		dev_err(adapter->pdev_dev,
+			"Unsupported chip version %d\n", chip_version);
+		return;
+	}
 
 	if (is_t4(adapter->params.chip)) {
 		sge_idma_decode = (const char **)t4_decode;
@@ -6097,6 +6169,59 @@ int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
 }
 
 /**
+ *	t4_fl_pkt_align - return the fl packet alignment
+ *	@adap: the adapter
+ *
+ *	T4 has a single field to specify the packing and padding boundary.
+ *	T5 onwards has separate fields for this and hence the alignment for
+ *	next packet offset is maximum of these two.
+ *
+ */
+int t4_fl_pkt_align(struct adapter *adap)
+{
+	u32 sge_control, sge_control2;
+	unsigned int ingpadboundary, ingpackboundary, fl_align, ingpad_shift;
+
+	sge_control = t4_read_reg(adap, SGE_CONTROL_A);
+
+	/* T4 uses a single control field to specify both the PCIe Padding and
+	 * Packing Boundary.  T5 introduced the ability to specify these
+	 * separately.  The actual Ingress Packet Data alignment boundary
+	 * within Packed Buffer Mode is the maximum of these two
+	 * specifications.  (Note that it makes no real practical sense to
+	 * have the Pading Boudary be larger than the Packing Boundary but you
+	 * could set the chip up that way and, in fact, legacy T4 code would
+	 * end doing this because it would initialize the Padding Boundary and
+	 * leave the Packing Boundary initialized to 0 (16 bytes).)
+	 * Padding Boundary values in T6 starts from 8B,
+	 * where as it is 32B for T4 and T5.
+	 */
+	if (CHELSIO_CHIP_VERSION(adap->params.chip) <= CHELSIO_T5)
+		ingpad_shift = INGPADBOUNDARY_SHIFT_X;
+	else
+		ingpad_shift = T6_INGPADBOUNDARY_SHIFT_X;
+
+	ingpadboundary = 1 << (INGPADBOUNDARY_G(sge_control) + ingpad_shift);
+
+	fl_align = ingpadboundary;
+	if (!is_t4(adap->params.chip)) {
+		/* T5 has a weird interpretation of one of the PCIe Packing
+		 * Boundary values.  No idea why ...
+		 */
+		sge_control2 = t4_read_reg(adap, SGE_CONTROL2_A);
+		ingpackboundary = INGPACKBOUNDARY_G(sge_control2);
+		if (ingpackboundary == INGPACKBOUNDARY_16B_X)
+			ingpackboundary = 16;
+		else
+			ingpackboundary = 1 << (ingpackboundary +
+						INGPACKBOUNDARY_SHIFT_X);
+
+		fl_align = max(ingpadboundary, ingpackboundary);
+	}
+	return fl_align;
+}
+
+/**
  *	t4_fixup_host_params - fix up host-dependent parameters
  *	@adap: the adapter
  *	@page_size: the host's Base Page Size
@@ -6114,6 +6239,7 @@ int t4_fixup_host_params(struct adapter *adap, unsigned int page_size,
 	unsigned int stat_len = cache_line_size > 64 ? 128 : 64;
 	unsigned int fl_align = cache_line_size < 32 ? 32 : cache_line_size;
 	unsigned int fl_align_log = fls(fl_align) - 1;
+	unsigned int ingpad;
 
 	t4_write_reg(adap, SGE_HOST_PAGE_SIZE_A,
 		     HOSTPAGESIZEPF0_V(sge_hps) |
@@ -6161,10 +6287,16 @@ int t4_fixup_host_params(struct adapter *adap, unsigned int page_size,
 			fl_align = 64;
 			fl_align_log = 6;
 		}
+
+		if (is_t5(adap->params.chip))
+			ingpad = INGPCIEBOUNDARY_32B_X;
+		else
+			ingpad = T6_INGPADBOUNDARY_32B_X;
+
 		t4_set_reg_field(adap, SGE_CONTROL_A,
 				 INGPADBOUNDARY_V(INGPADBOUNDARY_M) |
 				 EGRSTATUSPAGESIZE_F,
-				 INGPADBOUNDARY_V(INGPCIEBOUNDARY_32B_X) |
+				 INGPADBOUNDARY_V(ingpad) |
 				 EGRSTATUSPAGESIZE_V(stat_len != 64));
 		t4_set_reg_field(adap, SGE_CONTROL2_A,
 				 INGPACKBOUNDARY_V(INGPACKBOUNDARY_M),
@@ -7060,7 +7192,12 @@ int t4_prep_adapter(struct adapter *adapter)
 				 NUM_MPS_CLS_SRAM_L_INSTANCES;
 		adapter->params.arch.mps_rplc_size = 128;
 		adapter->params.arch.nchan = NCHAN;
+		adapter->params.arch.pm_stats_cnt = PM_NSTATS;
 		adapter->params.arch.vfcount = 128;
+		/* Congestion map is for 4 channels so that
+		 * MPS can have 4 priority per port.
+		 */
+		adapter->params.arch.cng_ch_bits_log = 2;
 		break;
 	case CHELSIO_T5:
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T5, pl_rev);
@@ -7069,7 +7206,9 @@ int t4_prep_adapter(struct adapter *adapter)
 				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 		adapter->params.arch.mps_rplc_size = 128;
 		adapter->params.arch.nchan = NCHAN;
+		adapter->params.arch.pm_stats_cnt = PM_NSTATS;
 		adapter->params.arch.vfcount = 128;
+		adapter->params.arch.cng_ch_bits_log = 2;
 		break;
 	case CHELSIO_T6:
 		adapter->params.chip |= CHELSIO_CHIP_CODE(CHELSIO_T6, pl_rev);
@@ -7078,7 +7217,12 @@ int t4_prep_adapter(struct adapter *adapter)
 				 NUM_MPS_T5_CLS_SRAM_L_INSTANCES;
 		adapter->params.arch.mps_rplc_size = 256;
 		adapter->params.arch.nchan = 2;
+		adapter->params.arch.pm_stats_cnt = T6_PM_NSTATS;
 		adapter->params.arch.vfcount = 256;
+		/* Congestion map will be for 2 channels so that
+		 * MPS can have 8 priority per port.
+		 */
+		adapter->params.arch.cng_ch_bits_log = 3;
 		break;
 	default:
 		dev_err(adapter->pdev_dev, "Device %d is not supported\n",
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
index 13708fde1668..2fc60e83a7a1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
@@ -48,6 +48,7 @@ enum {
 	NMTUS          = 16,    /* size of MTU table */
 	NCCTRL_WIN     = 32,    /* # of congestion control windows */
 	PM_NSTATS      = 5,     /* # of PM stats */
+	T6_PM_NSTATS   = 7,     /* # of PM stats in T6 */
 	MBOX_LEN       = 64,    /* mailbox size in bytes */
 	TRACE_LEN      = 112,   /* length of trace data and mask */
 	FILTER_OPT_LEN = 36,    /* filter tuple width for optional components */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
index 03ed00c49823..06bc2d2e7a73 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_pci_id_tbl.h
@@ -162,6 +162,10 @@ CH_PCI_DEVICE_ID_TABLE_DEFINE_BEGIN
 	CH_PCI_ID_TABLE_FENTRY(0x5095),	/* Custom T540-CR-SO */
 	CH_PCI_ID_TABLE_FENTRY(0x5096),	/* Custom T580-CR */
 	CH_PCI_ID_TABLE_FENTRY(0x5097),	/* Custom T520-KR */
+	CH_PCI_ID_TABLE_FENTRY(0x5098),	/* Custom 2x40G QSFP */
+	CH_PCI_ID_TABLE_FENTRY(0x5099),	/* Custom 2x40G QSFP */
+	CH_PCI_ID_TABLE_FENTRY(0x509a),	/* Custom T520-CR */
+	CH_PCI_ID_TABLE_FENTRY(0x509b),	/* Custom T540-CR LOM */
 
 	/* T6 adapters:
 	 */
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index fc3044c8ac1c..9fea255c7e87 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -492,6 +492,9 @@
 #define STATSOURCE_T5_V(x) ((x) << STATSOURCE_T5_S)
 #define STATSOURCE_T5_G(x) (((x) >> STATSOURCE_T5_S) & STATSOURCE_T5_M)
 
+#define T6_STATMODE_S    0
+#define T6_STATMODE_V(x) ((x) << T6_STATMODE_S)
+
 #define SGE_DBFIFO_STATUS2_A 0x1118
 
 #define HP_INT_THRESH_T5_S    10
@@ -2395,6 +2398,30 @@
 #define MPS_CLS_TCAM_DATA0_A 0xf000
 #define MPS_CLS_TCAM_DATA1_A 0xf004
 
+#define VIDL_S    16
+#define VIDL_M    0xffffU
+#define VIDL_G(x) (((x) >> VIDL_S) & VIDL_M)
+
+#define DATALKPTYPE_S    10
+#define DATALKPTYPE_M    0x3U
+#define DATALKPTYPE_G(x) (((x) >> DATALKPTYPE_S) & DATALKPTYPE_M)
+
+#define DATAPORTNUM_S    12
+#define DATAPORTNUM_M    0xfU
+#define DATAPORTNUM_G(x) (((x) >> DATAPORTNUM_S) & DATAPORTNUM_M)
+
+#define DATADIPHIT_S    8
+#define DATADIPHIT_V(x) ((x) << DATADIPHIT_S)
+#define DATADIPHIT_F    DATADIPHIT_V(1U)
+
+#define DATAVIDH2_S    7
+#define DATAVIDH2_V(x) ((x) << DATAVIDH2_S)
+#define DATAVIDH2_F    DATAVIDH2_V(1U)
+
+#define DATAVIDH1_S    0
+#define DATAVIDH1_M    0x7fU
+#define DATAVIDH1_G(x) (((x) >> DATAVIDH1_S) & DATAVIDH1_M)
+
 #define USED_S    16
 #define USED_M    0x7ffU
 #define USED_G(x) (((x) >> USED_S) & USED_M)
@@ -2802,6 +2829,10 @@
 #define HASHEN_V(x) ((x) << HASHEN_S)
 #define HASHEN_F    HASHEN_V(1U)
 
+#define ASLIPCOMPEN_S    17
+#define ASLIPCOMPEN_V(x) ((x) << ASLIPCOMPEN_S)
+#define ASLIPCOMPEN_F    ASLIPCOMPEN_V(1U)
+
 #define REQQPARERR_S    16
 #define REQQPARERR_V(x) ((x) << REQQPARERR_S)
 #define REQQPARERR_F    REQQPARERR_V(1U)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_values.h b/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
index 7bdee3bf75ec..a5231fa771db 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_values.h
@@ -53,6 +53,9 @@
 
 #define INGPADBOUNDARY_SHIFT_X		5
 
+#define T6_INGPADBOUNDARY_SHIFT_X	3
+#define T6_INGPADBOUNDARY_32B_X		2
+
 /* CONTROL2 register */
 #define INGPACKBOUNDARY_SHIFT_X		5
 #define INGPACKBOUNDARY_16B_X		0
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index fa3786a9d30e..6528231d8a59 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -2607,7 +2607,7 @@ int t4vf_sge_init(struct adapter *adapter)
 	u32 fl0 = sge_params->sge_fl_buffer_size[0];
 	u32 fl1 = sge_params->sge_fl_buffer_size[1];
 	struct sge *s = &adapter->sge;
-	unsigned int ingpadboundary, ingpackboundary;
+	unsigned int ingpadboundary, ingpackboundary, ingpad_shift;
 
 	/*
 	 * Start by vetting the basic SGE parameters which have been set up by
@@ -2642,9 +2642,16 @@ int t4vf_sge_init(struct adapter *adapter)
 	 * could set the chip up that way and, in fact, legacy T4 code would
 	 * end doing this because it would initialize the Padding Boundary and
 	 * leave the Packing Boundary initialized to 0 (16 bytes).)
+	 * Padding Boundary values in T6 starts from 8B,
+	 * where as it is 32B for T4 and T5.
 	 */
+	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
+		ingpad_shift = INGPADBOUNDARY_SHIFT_X;
+	else
+		ingpad_shift = T6_INGPADBOUNDARY_SHIFT_X;
+
 	ingpadboundary = 1 << (INGPADBOUNDARY_G(sge_params->sge_control) +
-			       INGPADBOUNDARY_SHIFT_X);
+			       ingpad_shift);
 	if (is_t4(adapter->params.chip)) {
 		s->fl_align = ingpadboundary;
 	} else {
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_defs.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_defs.h
index b516b12b1884..f859db3d254c 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_defs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_defs.h
@@ -54,6 +54,7 @@
 #define T4VF_MPS_BASE_ADDR	0x0100
 #define T4VF_PL_BASE_ADDR	0x0200
 #define T4VF_MBDATA_BASE_ADDR	0x0240
+#define T6VF_MBDATA_BASE_ADDR	0x0280
 #define T4VF_CIM_BASE_ADDR	0x0300
 
 #define T4VF_REGMAP_START	0x0000
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 63dd5fdac5b9..b6fa74aafe47 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -120,12 +120,19 @@ int t4vf_wr_mbox_core(struct adapter *adapter, const void *cmd, int size,
 		1, 1, 3, 5, 10, 10, 20, 50, 100
 	};
 
-	u32 v;
+	u32 v, mbox_data;
 	int i, ms, delay_idx;
 	const __be64 *p;
-	u32 mbox_data = T4VF_MBDATA_BASE_ADDR;
 	u32 mbox_ctl = T4VF_CIM_BASE_ADDR + CIM_VF_EXT_MAILBOX_CTRL;
 
+	/* In T6, mailbox size is changed to 128 bytes to avoid
+	 * invalidating the entire prefetch buffer.
+	 */
+	if (CHELSIO_CHIP_VERSION(adapter->params.chip) <= CHELSIO_T5)
+		mbox_data = T4VF_MBDATA_BASE_ADDR;
+	else
+		mbox_data = T6VF_MBDATA_BASE_ADDR;
+
 	/*
 	 * Commands must be multiples of 16 bytes in length and may not be
 	 * larger than the size of the Mailbox Data register array.
diff --git a/drivers/net/ethernet/cisco/enic/enic.h b/drivers/net/ethernet/cisco/enic/enic.h
index 1671fa3332c2..7ba6d530b0c0 100644
--- a/drivers/net/ethernet/cisco/enic/enic.h
+++ b/drivers/net/ethernet/cisco/enic/enic.h
@@ -33,7 +33,7 @@
 
 #define DRV_NAME		"enic"
 #define DRV_DESCRIPTION		"Cisco VIC Ethernet NIC Driver"
-#define DRV_VERSION		"2.3.0.12"
+#define DRV_VERSION		"2.3.0.20"
 #define DRV_COPYRIGHT		"Copyright 2008-2013 Cisco Systems, Inc"
 
 #define ENIC_BARS_MAX		6
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index b36643ef0593..b2182d3ba3cc 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -2458,13 +2458,11 @@ static int enic_dev_init(struct enic *enic)
 	switch (vnic_dev_get_intr_mode(enic->vdev)) {
 	default:
 		netif_napi_add(netdev, &enic->napi[0], enic_poll, 64);
-		napi_hash_add(&enic->napi[0]);
 		break;
 	case VNIC_DEV_INTR_MODE_MSIX:
 		for (i = 0; i < enic->rq_count; i++) {
 			netif_napi_add(netdev, &enic->napi[i],
 				enic_poll_msix_rq, NAPI_POLL_WEIGHT);
-			napi_hash_add(&enic->napi[i]);
 		}
 		for (i = 0; i < enic->wq_count; i++)
 			netif_napi_add(netdev, &enic->napi[enic_cq_wq(enic, i)],
diff --git a/drivers/net/ethernet/cisco/enic/vnic_dev.c b/drivers/net/ethernet/cisco/enic/vnic_dev.c
index 1ffd1050860b..1fdf5fe12a95 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_dev.c
+++ b/drivers/net/ethernet/cisco/enic/vnic_dev.c
@@ -298,7 +298,8 @@ static int _vnic_dev_cmd2(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
 			  int wait)
 {
 	struct devcmd2_controller *dc2c = vdev->devcmd2;
-	struct devcmd2_result *result = dc2c->result + dc2c->next_result;
+	struct devcmd2_result *result;
+	u8 color;
 	unsigned int i;
 	int delay, err;
 	u32 fetch_index, new_posted;
@@ -336,13 +337,17 @@ static int _vnic_dev_cmd2(struct vnic_dev *vdev, enum vnic_devcmd_cmd cmd,
 	if (dc2c->cmd_ring[posted].flags & DEVCMD2_FNORESULT)
 		return 0;
 
+	result = dc2c->result + dc2c->next_result;
+	color = dc2c->color;
+
+	dc2c->next_result++;
+	if (dc2c->next_result == dc2c->result_size) {
+		dc2c->next_result = 0;
+		dc2c->color = dc2c->color ? 0 : 1;
+	}
+
 	for (delay = 0; delay < wait; delay++) {
-		if (result->color == dc2c->color) {
-			dc2c->next_result++;
-			if (dc2c->next_result == dc2c->result_size) {
-				dc2c->next_result = 0;
-				dc2c->color = dc2c->color ? 0 : 1;
-			}
+		if (result->color == color) {
 			if (result->error) {
 				err = result->error;
 				if (err != ERR_ECMDUNKNOWN ||
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index cf94b72dbacd..48d91941408d 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -128,7 +128,6 @@ struct board_info {
 	struct resource *data_res;
 	struct resource	*addr_req;   /* resources requested */
 	struct resource *data_req;
-	struct resource *irq_res;
 
 	int		 irq_wake;
 
@@ -1300,22 +1299,16 @@ static int
 dm9000_open(struct net_device *dev)
 {
 	struct board_info *db = netdev_priv(dev);
-	unsigned long irqflags = db->irq_res->flags & IRQF_TRIGGER_MASK;
 
 	if (netif_msg_ifup(db))
 		dev_dbg(db->dev, "enabling %s\n", dev->name);
 
-	/* If there is no IRQ type specified, default to something that
-	 * may work, and tell the user that this is a problem */
-
-	if (irqflags == IRQF_TRIGGER_NONE)
-		irqflags = irq_get_trigger_type(dev->irq);
-
-	if (irqflags == IRQF_TRIGGER_NONE)
+	/* If there is no IRQ type specified, tell the user that this is a
+	 * problem
+	 */
+	if (irq_get_trigger_type(dev->irq) == IRQF_TRIGGER_NONE)
 		dev_warn(db->dev, "WARNING: no IRQ resource flags set.\n");
 
-	irqflags |= IRQF_SHARED;
-
 	/* GPIO0 on pre-activate PHY, Reg 1F is not set by reset */
 	iow(db, DM9000_GPR, 0);	/* REG_1F bit0 activate phyxcer */
 	mdelay(1); /* delay needs by DM9000B */
@@ -1323,7 +1316,8 @@ dm9000_open(struct net_device *dev)
 	/* Initialize DM9000 board */
 	dm9000_init_dm9000(dev);
 
-	if (request_irq(dev->irq, dm9000_interrupt, irqflags, dev->name, dev))
+	if (request_irq(dev->irq, dm9000_interrupt, IRQF_SHARED,
+			dev->name, dev))
 		return -EAGAIN;
 	/* Now that we have an interrupt handler hooked up we can unmask
 	 * our interrupts
@@ -1500,15 +1494,22 @@ dm9000_probe(struct platform_device *pdev)
 
 	db->addr_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	db->data_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	db->irq_res  = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 
-	if (db->addr_res == NULL || db->data_res == NULL ||
-	    db->irq_res == NULL) {
-		dev_err(db->dev, "insufficient resources\n");
+	if (!db->addr_res || !db->data_res) {
+		dev_err(db->dev, "insufficient resources addr=%p data=%p\n",
+			db->addr_res, db->data_res);
 		ret = -ENOENT;
 		goto out;
 	}
 
+	ndev->irq = platform_get_irq(pdev, 0);
+	if (ndev->irq < 0) {
+		dev_err(db->dev, "interrupt resource unavailable: %d\n",
+			ndev->irq);
+		ret = ndev->irq;
+		goto out;
+	}
+
 	db->irq_wake = platform_get_irq(pdev, 1);
 	if (db->irq_wake >= 0) {
 		dev_dbg(db->dev, "wakeup irq %d\n", db->irq_wake);
@@ -1570,7 +1571,6 @@ dm9000_probe(struct platform_device *pdev)
 
 	/* fill in parameters for net-dev structure */
 	ndev->base_addr = (unsigned long)db->io_addr;
-	ndev->irq	= db->irq_res->start;
 
 	/* ensure at least we have a default set of IO routines */
 	dm9000_set_io(db, iosize);
diff --git a/drivers/net/ethernet/dec/tulip/de4x5.c b/drivers/net/ethernet/dec/tulip/de4x5.c
index 8966f3159bb2..3acde3b9b767 100644
--- a/drivers/net/ethernet/dec/tulip/de4x5.c
+++ b/drivers/net/ethernet/dec/tulip/de4x5.c
@@ -1990,7 +1990,7 @@ SetMulticastFilter(struct net_device *dev)
 
 static u_char de4x5_irq[] = EISA_ALLOWED_IRQ_LIST;
 
-static int __init de4x5_eisa_probe (struct device *gendev)
+static int de4x5_eisa_probe(struct device *gendev)
 {
 	struct eisa_device *edev;
 	u_long iobase;
diff --git a/drivers/net/ethernet/dlink/dl2k.c b/drivers/net/ethernet/dlink/dl2k.c
index ccca4799c27b..f92b6d948398 100644
--- a/drivers/net/ethernet/dlink/dl2k.c
+++ b/drivers/net/ethernet/dlink/dl2k.c
@@ -70,7 +70,6 @@ static const int multicast_filter_limit = 0x40;
 static int rio_open (struct net_device *dev);
 static void rio_timer (unsigned long data);
 static void rio_tx_timeout (struct net_device *dev);
-static void alloc_list (struct net_device *dev);
 static netdev_tx_t start_xmit (struct sk_buff *skb, struct net_device *dev);
 static irqreturn_t rio_interrupt (int irq, void *dev_instance);
 static void rio_free_tx (struct net_device *dev, int irq);
@@ -253,19 +252,6 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (err)
 		goto err_out_unmap_rx;
 
-	if (np->chip_id == CHIP_IP1000A &&
-	    (np->pdev->revision == 0x40 || np->pdev->revision == 0x41)) {
-		/* PHY magic taken from ipg driver, undocumented registers */
-		mii_write(dev, np->phy_addr, 31, 0x0001);
-		mii_write(dev, np->phy_addr, 27, 0x01e0);
-		mii_write(dev, np->phy_addr, 31, 0x0002);
-		mii_write(dev, np->phy_addr, 27, 0xeb8e);
-		mii_write(dev, np->phy_addr, 31, 0x0000);
-		mii_write(dev, np->phy_addr, 30, 0x005e);
-		/* advertise 1000BASE-T half & full duplex, prefer MASTER */
-		mii_write(dev, np->phy_addr, MII_CTRL1000, 0x0700);
-	}
-
 	/* Fiber device? */
 	np->phy_media = (dr16(ASICCtrl) & PhyMedia) ? 1 : 0;
 	np->link_status = 0;
@@ -275,13 +261,11 @@ rio_probe1 (struct pci_dev *pdev, const struct pci_device_id *ent)
 	 	if (np->an_enable == 2) {
 			np->an_enable = 1;
 		}
-		mii_set_media_pcs (dev);
 	} else {
 		/* Auto-Negotiation is mandatory for 1000BASE-T,
 		   IEEE 802.3ab Annex 28D page 14 */
 		if (np->speed == 1000)
 			np->an_enable = 1;
-		mii_set_media (dev);
 	}
 
 	err = register_netdev (dev);
@@ -446,19 +430,106 @@ static void rio_set_led_mode(struct net_device *dev)
 	dw32(ASICCtrl, mode);
 }
 
-static int
-rio_open (struct net_device *dev)
+static inline dma_addr_t desc_to_dma(struct netdev_desc *desc)
+{
+	return le64_to_cpu(desc->fraginfo) & DMA_BIT_MASK(48);
+}
+
+static void free_list(struct net_device *dev)
+{
+	struct netdev_private *np = netdev_priv(dev);
+	struct sk_buff *skb;
+	int i;
+
+	/* Free all the skbuffs in the queue. */
+	for (i = 0; i < RX_RING_SIZE; i++) {
+		skb = np->rx_skbuff[i];
+		if (skb) {
+			pci_unmap_single(np->pdev, desc_to_dma(&np->rx_ring[i]),
+					 skb->len, PCI_DMA_FROMDEVICE);
+			dev_kfree_skb(skb);
+			np->rx_skbuff[i] = NULL;
+		}
+		np->rx_ring[i].status = 0;
+		np->rx_ring[i].fraginfo = 0;
+	}
+	for (i = 0; i < TX_RING_SIZE; i++) {
+		skb = np->tx_skbuff[i];
+		if (skb) {
+			pci_unmap_single(np->pdev, desc_to_dma(&np->tx_ring[i]),
+					 skb->len, PCI_DMA_TODEVICE);
+			dev_kfree_skb(skb);
+			np->tx_skbuff[i] = NULL;
+		}
+	}
+}
+
+static void rio_reset_ring(struct netdev_private *np)
+{
+	int i;
+
+	np->cur_rx = 0;
+	np->cur_tx = 0;
+	np->old_rx = 0;
+	np->old_tx = 0;
+
+	for (i = 0; i < TX_RING_SIZE; i++)
+		np->tx_ring[i].status = cpu_to_le64(TFDDone);
+
+	for (i = 0; i < RX_RING_SIZE; i++)
+		np->rx_ring[i].status = 0;
+}
+
+ /* allocate and initialize Tx and Rx descriptors */
+static int alloc_list(struct net_device *dev)
+{
+	struct netdev_private *np = netdev_priv(dev);
+	int i;
+
+	rio_reset_ring(np);
+	np->rx_buf_sz = (dev->mtu <= 1500 ? PACKET_SIZE : dev->mtu + 32);
+
+	/* Initialize Tx descriptors, TFDListPtr leaves in start_xmit(). */
+	for (i = 0; i < TX_RING_SIZE; i++) {
+		np->tx_skbuff[i] = NULL;
+		np->tx_ring[i].next_desc = cpu_to_le64(np->tx_ring_dma +
+					      ((i + 1) % TX_RING_SIZE) *
+					      sizeof(struct netdev_desc));
+	}
+
+	/* Initialize Rx descriptors & allocate buffers */
+	for (i = 0; i < RX_RING_SIZE; i++) {
+		/* Allocated fixed size of skbuff */
+		struct sk_buff *skb;
+
+		skb = netdev_alloc_skb_ip_align(dev, np->rx_buf_sz);
+		np->rx_skbuff[i] = skb;
+		if (!skb) {
+			free_list(dev);
+			return -ENOMEM;
+		}
+
+		np->rx_ring[i].next_desc = cpu_to_le64(np->rx_ring_dma +
+						((i + 1) % RX_RING_SIZE) *
+						sizeof(struct netdev_desc));
+		/* Rubicon now supports 40 bits of addressing space. */
+		np->rx_ring[i].fraginfo =
+		    cpu_to_le64(pci_map_single(
+				  np->pdev, skb->data, np->rx_buf_sz,
+				  PCI_DMA_FROMDEVICE));
+		np->rx_ring[i].fraginfo |= cpu_to_le64((u64)np->rx_buf_sz << 48);
+	}
+
+	return 0;
+}
+
+static void rio_hw_init(struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
 	void __iomem *ioaddr = np->ioaddr;
-	const int irq = np->pdev->irq;
 	int i;
 	u16 macctrl;
 
-	i = request_irq(irq, rio_interrupt, IRQF_SHARED, dev->name, dev);
-	if (i)
-		return i;
-
 	/* Reset all logic functions */
 	dw16(ASICCtrl + 2,
 	     GlobalReset | DMAReset | FIFOReset | NetworkReset | HostReset);
@@ -469,11 +540,31 @@ rio_open (struct net_device *dev)
 	/* DebugCtrl bit 4, 5, 9 must set */
 	dw32(DebugCtrl, dr32(DebugCtrl) | 0x0230);
 
+	if (np->chip_id == CHIP_IP1000A &&
+	    (np->pdev->revision == 0x40 || np->pdev->revision == 0x41)) {
+		/* PHY magic taken from ipg driver, undocumented registers */
+		mii_write(dev, np->phy_addr, 31, 0x0001);
+		mii_write(dev, np->phy_addr, 27, 0x01e0);
+		mii_write(dev, np->phy_addr, 31, 0x0002);
+		mii_write(dev, np->phy_addr, 27, 0xeb8e);
+		mii_write(dev, np->phy_addr, 31, 0x0000);
+		mii_write(dev, np->phy_addr, 30, 0x005e);
+		/* advertise 1000BASE-T half & full duplex, prefer MASTER */
+		mii_write(dev, np->phy_addr, MII_CTRL1000, 0x0700);
+	}
+
+	if (np->phy_media)
+		mii_set_media_pcs(dev);
+	else
+		mii_set_media(dev);
+
 	/* Jumbo frame */
 	if (np->jumbo != 0)
 		dw16(MaxFrameSize, MAX_JUMBO+14);
 
-	alloc_list (dev);
+	/* Set RFDListPtr */
+	dw32(RFDListPtr0, np->rx_ring_dma);
+	dw32(RFDListPtr1, 0);
 
 	/* Set station address */
 	/* 16 or 32-bit access is required by TC9020 datasheet but 8-bit works
@@ -509,10 +600,6 @@ rio_open (struct net_device *dev)
 		dw32(MACCtrl, dr32(MACCtrl) | AutoVLANuntagging);
 	}
 
-	setup_timer(&np->timer, rio_timer, (unsigned long)dev);
-	np->timer.expires = jiffies + 1*HZ;
-	add_timer (&np->timer);
-
 	/* Start Tx/Rx */
 	dw32(MACCtrl, dr32(MACCtrl) | StatsEnable | RxEnable | TxEnable);
 
@@ -522,6 +609,42 @@ rio_open (struct net_device *dev)
 	macctrl |= (np->tx_flow) ? TxFlowControlEnable : 0;
 	macctrl |= (np->rx_flow) ? RxFlowControlEnable : 0;
 	dw16(MACCtrl, macctrl);
+}
+
+static void rio_hw_stop(struct net_device *dev)
+{
+	struct netdev_private *np = netdev_priv(dev);
+	void __iomem *ioaddr = np->ioaddr;
+
+	/* Disable interrupts */
+	dw16(IntEnable, 0);
+
+	/* Stop Tx and Rx logics */
+	dw32(MACCtrl, TxDisable | RxDisable | StatsDisable);
+}
+
+static int rio_open(struct net_device *dev)
+{
+	struct netdev_private *np = netdev_priv(dev);
+	const int irq = np->pdev->irq;
+	int i;
+
+	i = alloc_list(dev);
+	if (i)
+		return i;
+
+	rio_hw_init(dev);
+
+	i = request_irq(irq, rio_interrupt, IRQF_SHARED, dev->name, dev);
+	if (i) {
+		rio_hw_stop(dev);
+		free_list(dev);
+		return i;
+	}
+
+	setup_timer(&np->timer, rio_timer, (unsigned long)dev);
+	np->timer.expires = jiffies + 1 * HZ;
+	add_timer(&np->timer);
 
 	netif_start_queue (dev);
 
@@ -586,60 +709,6 @@ rio_tx_timeout (struct net_device *dev)
 	dev->trans_start = jiffies; /* prevent tx timeout */
 }
 
- /* allocate and initialize Tx and Rx descriptors */
-static void
-alloc_list (struct net_device *dev)
-{
-	struct netdev_private *np = netdev_priv(dev);
-	void __iomem *ioaddr = np->ioaddr;
-	int i;
-
-	np->cur_rx = np->cur_tx = 0;
-	np->old_rx = np->old_tx = 0;
-	np->rx_buf_sz = (dev->mtu <= 1500 ? PACKET_SIZE : dev->mtu + 32);
-
-	/* Initialize Tx descriptors, TFDListPtr leaves in start_xmit(). */
-	for (i = 0; i < TX_RING_SIZE; i++) {
-		np->tx_skbuff[i] = NULL;
-		np->tx_ring[i].status = cpu_to_le64 (TFDDone);
-		np->tx_ring[i].next_desc = cpu_to_le64 (np->tx_ring_dma +
-					      ((i+1)%TX_RING_SIZE) *
-					      sizeof (struct netdev_desc));
-	}
-
-	/* Initialize Rx descriptors */
-	for (i = 0; i < RX_RING_SIZE; i++) {
-		np->rx_ring[i].next_desc = cpu_to_le64 (np->rx_ring_dma +
-						((i + 1) % RX_RING_SIZE) *
-						sizeof (struct netdev_desc));
-		np->rx_ring[i].status = 0;
-		np->rx_ring[i].fraginfo = 0;
-		np->rx_skbuff[i] = NULL;
-	}
-
-	/* Allocate the rx buffers */
-	for (i = 0; i < RX_RING_SIZE; i++) {
-		/* Allocated fixed size of skbuff */
-		struct sk_buff *skb;
-
-		skb = netdev_alloc_skb_ip_align(dev, np->rx_buf_sz);
-		np->rx_skbuff[i] = skb;
-		if (skb == NULL)
-			break;
-
-		/* Rubicon now supports 40 bits of addressing space. */
-		np->rx_ring[i].fraginfo =
-		    cpu_to_le64 ( pci_map_single (
-			 	  np->pdev, skb->data, np->rx_buf_sz,
-				  PCI_DMA_FROMDEVICE));
-		np->rx_ring[i].fraginfo |= cpu_to_le64((u64)np->rx_buf_sz << 48);
-	}
-
-	/* Set RFDListPtr */
-	dw32(RFDListPtr0, np->rx_ring_dma);
-	dw32(RFDListPtr1, 0);
-}
-
 static netdev_tx_t
 start_xmit (struct sk_buff *skb, struct net_device *dev)
 {
@@ -748,11 +817,6 @@ rio_interrupt (int irq, void *dev_instance)
 	return IRQ_RETVAL(handled);
 }
 
-static inline dma_addr_t desc_to_dma(struct netdev_desc *desc)
-{
-	return le64_to_cpu(desc->fraginfo) & DMA_BIT_MASK(48);
-}
-
 static void
 rio_free_tx (struct net_device *dev, int irq)
 {
@@ -1730,44 +1794,16 @@ static int
 rio_close (struct net_device *dev)
 {
 	struct netdev_private *np = netdev_priv(dev);
-	void __iomem *ioaddr = np->ioaddr;
-
 	struct pci_dev *pdev = np->pdev;
-	struct sk_buff *skb;
-	int i;
 
 	netif_stop_queue (dev);
 
-	/* Disable interrupts */
-	dw16(IntEnable, 0);
-
-	/* Stop Tx and Rx logics */
-	dw32(MACCtrl, TxDisable | RxDisable | StatsDisable);
+	rio_hw_stop(dev);
 
 	free_irq(pdev->irq, dev);
 	del_timer_sync (&np->timer);
 
-	/* Free all the skbuffs in the queue. */
-	for (i = 0; i < RX_RING_SIZE; i++) {
-		skb = np->rx_skbuff[i];
-		if (skb) {
-			pci_unmap_single(pdev, desc_to_dma(&np->rx_ring[i]),
-					 skb->len, PCI_DMA_FROMDEVICE);
-			dev_kfree_skb (skb);
-			np->rx_skbuff[i] = NULL;
-		}
-		np->rx_ring[i].status = 0;
-		np->rx_ring[i].fraginfo = 0;
-	}
-	for (i = 0; i < TX_RING_SIZE; i++) {
-		skb = np->tx_skbuff[i];
-		if (skb) {
-			pci_unmap_single(pdev, desc_to_dma(&np->tx_ring[i]),
-					 skb->len, PCI_DMA_TODEVICE);
-			dev_kfree_skb (skb);
-			np->tx_skbuff[i] = NULL;
-		}
-	}
+	free_list(dev);
 
 	return 0;
 }
@@ -1795,11 +1831,55 @@ rio_remove1 (struct pci_dev *pdev)
 	}
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int rio_suspend(struct device *device)
+{
+	struct net_device *dev = dev_get_drvdata(device);
+	struct netdev_private *np = netdev_priv(dev);
+
+	if (!netif_running(dev))
+		return 0;
+
+	netif_device_detach(dev);
+	del_timer_sync(&np->timer);
+	rio_hw_stop(dev);
+
+	return 0;
+}
+
+static int rio_resume(struct device *device)
+{
+	struct net_device *dev = dev_get_drvdata(device);
+	struct netdev_private *np = netdev_priv(dev);
+
+	if (!netif_running(dev))
+		return 0;
+
+	rio_reset_ring(np);
+	rio_hw_init(dev);
+	np->timer.expires = jiffies + 1 * HZ;
+	add_timer(&np->timer);
+	netif_device_attach(dev);
+	dl2k_enable_int(np);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(rio_pm_ops, rio_suspend, rio_resume);
+#define RIO_PM_OPS    (&rio_pm_ops)
+
+#else
+
+#define RIO_PM_OPS	NULL
+
+#endif /* CONFIG_PM_SLEEP */
+
 static struct pci_driver rio_driver = {
 	.name		= "dl2k",
 	.id_table	= rio_pci_tbl,
 	.probe		= rio_probe1,
 	.remove		= rio_remove1,
+	.driver.pm	= RIO_PM_OPS,
 };
 
 module_pci_driver(rio_driver);
diff --git a/drivers/net/ethernet/dnet.c b/drivers/net/ethernet/dnet.c
index 13d00a38a5bd..b69a9eacc531 100644
--- a/drivers/net/ethernet/dnet.c
+++ b/drivers/net/ethernet/dnet.c
@@ -255,15 +255,9 @@ static int dnet_mii_probe(struct net_device *dev)
 {
 	struct dnet *bp = netdev_priv(dev);
 	struct phy_device *phydev = NULL;
-	int phy_addr;
 
 	/* find the first phy */
-	for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
-		if (bp->mii_bus->phy_map[phy_addr]) {
-			phydev = bp->mii_bus->phy_map[phy_addr];
-			break;
-		}
-	}
+	phydev = phy_find_first(bp->mii_bus);
 
 	if (!phydev) {
 		printk(KERN_ERR "%s: no PHY found\n", dev->name);
@@ -274,11 +268,11 @@ static int dnet_mii_probe(struct net_device *dev)
 
 	/* attach the mac to the phy */
 	if (bp->capabilities & DNET_HAS_RMII) {
-		phydev = phy_connect(dev, dev_name(&phydev->dev),
+		phydev = phy_connect(dev, phydev_name(phydev),
 				     &dnet_handle_link_change,
 				     PHY_INTERFACE_MODE_RMII);
 	} else {
-		phydev = phy_connect(dev, dev_name(&phydev->dev),
+		phydev = phy_connect(dev, phydev_name(phydev),
 				     &dnet_handle_link_change,
 				     PHY_INTERFACE_MODE_MII);
 	}
@@ -308,7 +302,7 @@ static int dnet_mii_probe(struct net_device *dev)
 
 static int dnet_mii_init(struct dnet *bp)
 {
-	int err, i;
+	int err;
 
 	bp->mii_bus = mdiobus_alloc();
 	if (bp->mii_bus == NULL)
@@ -323,16 +317,6 @@ static int dnet_mii_init(struct dnet *bp)
 
 	bp->mii_bus->priv = bp;
 
-	bp->mii_bus->irq = devm_kmalloc(&bp->pdev->dev,
-					sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!bp->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		bp->mii_bus->irq[i] = PHY_POLL;
-
 	if (mdiobus_register(bp->mii_bus)) {
 		err = -ENXIO;
 		goto err_out;
@@ -892,9 +876,7 @@ static int dnet_probe(struct platform_device *pdev)
 	       (bp->capabilities & DNET_HAS_GIGABIT) ? "" : "no ",
 	       (bp->capabilities & DNET_HAS_DMA) ? "" : "no ");
 	phydev = bp->phy_dev;
-	dev_info(&pdev->dev, "attached PHY driver [%s] "
-	       "(mii_bus:phy_addr=%s, irq=%d)\n",
-	       phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
 
 	return 0;
 
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 6ee78c203eca..f9751294ece7 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			"10.6.0.3"
+#define DRV_VER			"11.0.0.0"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"Emulex BladeEngine2"
 #define BE3_NAME		"Emulex BladeEngine3"
@@ -521,7 +521,7 @@ struct be_adapter {
 	struct be_drv_stats drv_stats;
 	struct be_aic_obj aic_obj[MAX_EVT_QS];
 	u8 vlan_prio_bmap;	/* Available Priority BitMap */
-	u16 recommended_prio;	/* Recommended Priority */
+	u16 recommended_prio_bits;/* Recommended Priority bits in vlan tag */
 	struct be_dma_mem rx_filter; /* Cmd DMA mem for rx-filter */
 
 	struct be_dma_mem stats_cmd;
@@ -531,6 +531,7 @@ struct be_adapter {
 
 	struct delayed_work be_err_detection_work;
 	u8 err_flags;
+	bool pcicfg_mapped;	/* pcicfg obtained via pci_iomap() */
 	u32 flags;
 	u32 cmd_privileges;
 	/* Ethtool knobs and info */
@@ -547,10 +548,6 @@ struct be_adapter {
 
 	u32 beacon_state;	/* for set_phys_id */
 
-	bool eeh_error;
-	bool fw_timeout;
-	bool hw_error;
-
 	u32 port_num;
 	char port_name;
 	u8 mc_type;
@@ -574,6 +571,8 @@ struct be_adapter {
 	struct be_resources pool_res;	/* resources available for the port */
 	struct be_resources res;	/* resources available for the func */
 	u16 num_vfs;			/* Number of VFs provisioned by PF */
+	u8 pf_num;			/* Numbering used by FW, starts at 0 */
+	u8 vf_num;			/* Numbering used by FW, starts at 1 */
 	u8 virtfn;
 	struct be_vf_cfg *vf_cfg;
 	bool be3_native;
@@ -591,11 +590,10 @@ struct be_adapter {
 	u32 msg_enable;
 	int be_get_temp_freq;
 	struct be_hwmon hwmon_info;
-	u8 pf_number;
-	u8 pci_func_num;
 	struct rss_info rss_info;
 	/* Filters for packets that need to be sent to BMC */
 	u32 bmc_filt_mask;
+	u32 fat_dump_len;
 	u16 serial_num[CNTL_SERIAL_NUM_WORDS];
 };
 
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 1795c935ff02..b63d8ad2e115 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -308,8 +308,7 @@ static void be_async_grp5_cos_priority_process(struct be_adapter *adapter,
 
 	if (evt->valid) {
 		adapter->vlan_prio_bmap = evt->available_priority_bmap;
-		adapter->recommended_prio &= ~VLAN_PRIO_MASK;
-		adapter->recommended_prio =
+		adapter->recommended_prio_bits =
 			evt->reco_default_priority << VLAN_PRIO_SHIFT;
 	}
 }
@@ -1713,49 +1712,40 @@ err:
 }
 
 /* Uses synchronous mcc */
-int be_cmd_get_reg_len(struct be_adapter *adapter, u32 *log_size)
+int be_cmd_get_fat_dump_len(struct be_adapter *adapter, u32 *dump_size)
 {
-	struct be_mcc_wrb *wrb;
+	struct be_mcc_wrb wrb = {0};
 	struct be_cmd_req_get_fat *req;
 	int status;
 
-	spin_lock_bh(&adapter->mcc_lock);
-
-	wrb = wrb_from_mccq(adapter);
-	if (!wrb) {
-		status = -EBUSY;
-		goto err;
-	}
-	req = embedded_payload(wrb);
+	req = embedded_payload(&wrb);
 
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
-			       OPCODE_COMMON_MANAGE_FAT, sizeof(*req), wrb,
-			       NULL);
+			       OPCODE_COMMON_MANAGE_FAT, sizeof(*req),
+			       &wrb, NULL);
 	req->fat_operation = cpu_to_le32(QUERY_FAT);
-	status = be_mcc_notify_wait(adapter);
+	status = be_cmd_notify_wait(adapter, &wrb);
 	if (!status) {
-		struct be_cmd_resp_get_fat *resp = embedded_payload(wrb);
+		struct be_cmd_resp_get_fat *resp = embedded_payload(&wrb);
 
-		if (log_size && resp->log_size)
-			*log_size = le32_to_cpu(resp->log_size) -
+		if (dump_size && resp->log_size)
+			*dump_size = le32_to_cpu(resp->log_size) -
 					sizeof(u32);
 	}
-err:
-	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
 
-int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
+int be_cmd_get_fat_dump(struct be_adapter *adapter, u32 buf_len, void *buf)
 {
 	struct be_dma_mem get_fat_cmd;
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_fat *req;
 	u32 offset = 0, total_size, buf_size,
 				log_offset = sizeof(u32), payload_len;
-	int status = 0;
+	int status;
 
 	if (buf_len == 0)
-		return -EIO;
+		return 0;
 
 	total_size = buf_len;
 
@@ -1763,11 +1753,8 @@ int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 	get_fat_cmd.va = dma_zalloc_coherent(&adapter->pdev->dev,
 					     get_fat_cmd.size,
 					     &get_fat_cmd.dma, GFP_ATOMIC);
-	if (!get_fat_cmd.va) {
-		dev_err(&adapter->pdev->dev,
-			"Memory allocation failure while reading FAT data\n");
+	if (!get_fat_cmd.va)
 		return -ENOMEM;
-	}
 
 	spin_lock_bh(&adapter->mcc_lock);
 
@@ -2291,10 +2278,11 @@ err:
 	return status;
 }
 
-int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
-			    u32 data_size, u32 data_offset,
-			    const char *obj_name, u32 *data_written,
-			    u8 *change_status, u8 *addn_status)
+static int lancer_cmd_write_object(struct be_adapter *adapter,
+				   struct be_dma_mem *cmd, u32 data_size,
+				   u32 data_offset, const char *obj_name,
+				   u32 *data_written, u8 *change_status,
+				   u8 *addn_status)
 {
 	struct be_mcc_wrb *wrb;
 	struct lancer_cmd_req_write_object *req;
@@ -2410,7 +2398,8 @@ int be_cmd_query_sfp_info(struct be_adapter *adapter)
 	return status;
 }
 
-int lancer_cmd_delete_object(struct be_adapter *adapter, const char *obj_name)
+static int lancer_cmd_delete_object(struct be_adapter *adapter,
+				    const char *obj_name)
 {
 	struct lancer_cmd_req_delete_object *req;
 	struct be_mcc_wrb *wrb;
@@ -2485,9 +2474,9 @@ err_unlock:
 	return status;
 }
 
-int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
-			  u32 flash_type, u32 flash_opcode, u32 img_offset,
-			  u32 buf_size)
+static int be_cmd_write_flashrom(struct be_adapter *adapter,
+				 struct be_dma_mem *cmd, u32 flash_type,
+				 u32 flash_opcode, u32 img_offset, u32 buf_size)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_write_flashrom *req;
@@ -2533,8 +2522,8 @@ err_unlock:
 	return status;
 }
 
-int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
-			 u16 img_optype, u32 img_offset, u32 crc_offset)
+static int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
+				u16 img_optype, u32 img_offset, u32 crc_offset)
 {
 	struct be_cmd_read_flash_crc *req;
 	struct be_mcc_wrb *wrb;
@@ -2571,6 +2560,579 @@ err:
 	return status;
 }
 
+static char flash_cookie[2][16] = {"*** SE FLAS", "H DIRECTORY *** "};
+
+static bool phy_flashing_required(struct be_adapter *adapter)
+{
+	return (adapter->phy.phy_type == PHY_TYPE_TN_8022 &&
+		adapter->phy.interface_type == PHY_TYPE_BASET_10GB);
+}
+
+static bool is_comp_in_ufi(struct be_adapter *adapter,
+			   struct flash_section_info *fsec, int type)
+{
+	int i = 0, img_type = 0;
+	struct flash_section_info_g2 *fsec_g2 = NULL;
+
+	if (BE2_chip(adapter))
+		fsec_g2 = (struct flash_section_info_g2 *)fsec;
+
+	for (i = 0; i < MAX_FLASH_COMP; i++) {
+		if (fsec_g2)
+			img_type = le32_to_cpu(fsec_g2->fsec_entry[i].type);
+		else
+			img_type = le32_to_cpu(fsec->fsec_entry[i].type);
+
+		if (img_type == type)
+			return true;
+	}
+	return false;
+}
+
+static struct flash_section_info *get_fsec_info(struct be_adapter *adapter,
+						int header_size,
+						const struct firmware *fw)
+{
+	struct flash_section_info *fsec = NULL;
+	const u8 *p = fw->data;
+
+	p += header_size;
+	while (p < (fw->data + fw->size)) {
+		fsec = (struct flash_section_info *)p;
+		if (!memcmp(flash_cookie, fsec->cookie, sizeof(flash_cookie)))
+			return fsec;
+		p += 32;
+	}
+	return NULL;
+}
+
+static int be_check_flash_crc(struct be_adapter *adapter, const u8 *p,
+			      u32 img_offset, u32 img_size, int hdr_size,
+			      u16 img_optype, bool *crc_match)
+{
+	u32 crc_offset;
+	int status;
+	u8 crc[4];
+
+	status = be_cmd_get_flash_crc(adapter, crc, img_optype, img_offset,
+				      img_size - 4);
+	if (status)
+		return status;
+
+	crc_offset = hdr_size + img_offset + img_size - 4;
+
+	/* Skip flashing, if crc of flashed region matches */
+	if (!memcmp(crc, p + crc_offset, 4))
+		*crc_match = true;
+	else
+		*crc_match = false;
+
+	return status;
+}
+
+static int be_flash(struct be_adapter *adapter, const u8 *img,
+		    struct be_dma_mem *flash_cmd, int optype, int img_size,
+		    u32 img_offset)
+{
+	u32 flash_op, num_bytes, total_bytes = img_size, bytes_sent = 0;
+	struct be_cmd_write_flashrom *req = flash_cmd->va;
+	int status;
+
+	while (total_bytes) {
+		num_bytes = min_t(u32, 32 * 1024, total_bytes);
+
+		total_bytes -= num_bytes;
+
+		if (!total_bytes) {
+			if (optype == OPTYPE_PHY_FW)
+				flash_op = FLASHROM_OPER_PHY_FLASH;
+			else
+				flash_op = FLASHROM_OPER_FLASH;
+		} else {
+			if (optype == OPTYPE_PHY_FW)
+				flash_op = FLASHROM_OPER_PHY_SAVE;
+			else
+				flash_op = FLASHROM_OPER_SAVE;
+		}
+
+		memcpy(req->data_buf, img, num_bytes);
+		img += num_bytes;
+		status = be_cmd_write_flashrom(adapter, flash_cmd, optype,
+					       flash_op, img_offset +
+					       bytes_sent, num_bytes);
+		if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST &&
+		    optype == OPTYPE_PHY_FW)
+			break;
+		else if (status)
+			return status;
+
+		bytes_sent += num_bytes;
+	}
+	return 0;
+}
+
+/* For BE2, BE3 and BE3-R */
+static int be_flash_BEx(struct be_adapter *adapter,
+			const struct firmware *fw,
+			struct be_dma_mem *flash_cmd, int num_of_images)
+{
+	int img_hdrs_size = (num_of_images * sizeof(struct image_hdr));
+	struct device *dev = &adapter->pdev->dev;
+	struct flash_section_info *fsec = NULL;
+	int status, i, filehdr_size, num_comp;
+	const struct flash_comp *pflashcomp;
+	bool crc_match;
+	const u8 *p;
+
+	struct flash_comp gen3_flash_types[] = {
+		{ BE3_ISCSI_PRIMARY_IMAGE_START, OPTYPE_ISCSI_ACTIVE,
+			BE3_COMP_MAX_SIZE, IMAGE_FIRMWARE_ISCSI},
+		{ BE3_REDBOOT_START, OPTYPE_REDBOOT,
+			BE3_REDBOOT_COMP_MAX_SIZE, IMAGE_BOOT_CODE},
+		{ BE3_ISCSI_BIOS_START, OPTYPE_BIOS,
+			BE3_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_ISCSI},
+		{ BE3_PXE_BIOS_START, OPTYPE_PXE_BIOS,
+			BE3_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_PXE},
+		{ BE3_FCOE_BIOS_START, OPTYPE_FCOE_BIOS,
+			BE3_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_FCOE},
+		{ BE3_ISCSI_BACKUP_IMAGE_START, OPTYPE_ISCSI_BACKUP,
+			BE3_COMP_MAX_SIZE, IMAGE_FIRMWARE_BACKUP_ISCSI},
+		{ BE3_FCOE_PRIMARY_IMAGE_START, OPTYPE_FCOE_FW_ACTIVE,
+			BE3_COMP_MAX_SIZE, IMAGE_FIRMWARE_FCOE},
+		{ BE3_FCOE_BACKUP_IMAGE_START, OPTYPE_FCOE_FW_BACKUP,
+			BE3_COMP_MAX_SIZE, IMAGE_FIRMWARE_BACKUP_FCOE},
+		{ BE3_NCSI_START, OPTYPE_NCSI_FW,
+			BE3_NCSI_COMP_MAX_SIZE, IMAGE_NCSI},
+		{ BE3_PHY_FW_START, OPTYPE_PHY_FW,
+			BE3_PHY_FW_COMP_MAX_SIZE, IMAGE_FIRMWARE_PHY}
+	};
+
+	struct flash_comp gen2_flash_types[] = {
+		{ BE2_ISCSI_PRIMARY_IMAGE_START, OPTYPE_ISCSI_ACTIVE,
+			BE2_COMP_MAX_SIZE, IMAGE_FIRMWARE_ISCSI},
+		{ BE2_REDBOOT_START, OPTYPE_REDBOOT,
+			BE2_REDBOOT_COMP_MAX_SIZE, IMAGE_BOOT_CODE},
+		{ BE2_ISCSI_BIOS_START, OPTYPE_BIOS,
+			BE2_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_ISCSI},
+		{ BE2_PXE_BIOS_START, OPTYPE_PXE_BIOS,
+			BE2_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_PXE},
+		{ BE2_FCOE_BIOS_START, OPTYPE_FCOE_BIOS,
+			BE2_BIOS_COMP_MAX_SIZE, IMAGE_OPTION_ROM_FCOE},
+		{ BE2_ISCSI_BACKUP_IMAGE_START, OPTYPE_ISCSI_BACKUP,
+			BE2_COMP_MAX_SIZE, IMAGE_FIRMWARE_BACKUP_ISCSI},
+		{ BE2_FCOE_PRIMARY_IMAGE_START, OPTYPE_FCOE_FW_ACTIVE,
+			BE2_COMP_MAX_SIZE, IMAGE_FIRMWARE_FCOE},
+		{ BE2_FCOE_BACKUP_IMAGE_START, OPTYPE_FCOE_FW_BACKUP,
+			 BE2_COMP_MAX_SIZE, IMAGE_FIRMWARE_BACKUP_FCOE}
+	};
+
+	if (BE3_chip(adapter)) {
+		pflashcomp = gen3_flash_types;
+		filehdr_size = sizeof(struct flash_file_hdr_g3);
+		num_comp = ARRAY_SIZE(gen3_flash_types);
+	} else {
+		pflashcomp = gen2_flash_types;
+		filehdr_size = sizeof(struct flash_file_hdr_g2);
+		num_comp = ARRAY_SIZE(gen2_flash_types);
+		img_hdrs_size = 0;
+	}
+
+	/* Get flash section info*/
+	fsec = get_fsec_info(adapter, filehdr_size + img_hdrs_size, fw);
+	if (!fsec) {
+		dev_err(dev, "Invalid Cookie. FW image may be corrupted\n");
+		return -1;
+	}
+	for (i = 0; i < num_comp; i++) {
+		if (!is_comp_in_ufi(adapter, fsec, pflashcomp[i].img_type))
+			continue;
+
+		if ((pflashcomp[i].optype == OPTYPE_NCSI_FW) &&
+		    memcmp(adapter->fw_ver, "3.102.148.0", 11) < 0)
+			continue;
+
+		if (pflashcomp[i].optype == OPTYPE_PHY_FW  &&
+		    !phy_flashing_required(adapter))
+			continue;
+
+		if (pflashcomp[i].optype == OPTYPE_REDBOOT) {
+			status = be_check_flash_crc(adapter, fw->data,
+						    pflashcomp[i].offset,
+						    pflashcomp[i].size,
+						    filehdr_size +
+						    img_hdrs_size,
+						    OPTYPE_REDBOOT, &crc_match);
+			if (status) {
+				dev_err(dev,
+					"Could not get CRC for 0x%x region\n",
+					pflashcomp[i].optype);
+				continue;
+			}
+
+			if (crc_match)
+				continue;
+		}
+
+		p = fw->data + filehdr_size + pflashcomp[i].offset +
+			img_hdrs_size;
+		if (p + pflashcomp[i].size > fw->data + fw->size)
+			return -1;
+
+		status = be_flash(adapter, p, flash_cmd, pflashcomp[i].optype,
+				  pflashcomp[i].size, 0);
+		if (status) {
+			dev_err(dev, "Flashing section type 0x%x failed\n",
+				pflashcomp[i].img_type);
+			return status;
+		}
+	}
+	return 0;
+}
+
+static u16 be_get_img_optype(struct flash_section_entry fsec_entry)
+{
+	u32 img_type = le32_to_cpu(fsec_entry.type);
+	u16 img_optype = le16_to_cpu(fsec_entry.optype);
+
+	if (img_optype != 0xFFFF)
+		return img_optype;
+
+	switch (img_type) {
+	case IMAGE_FIRMWARE_ISCSI:
+		img_optype = OPTYPE_ISCSI_ACTIVE;
+		break;
+	case IMAGE_BOOT_CODE:
+		img_optype = OPTYPE_REDBOOT;
+		break;
+	case IMAGE_OPTION_ROM_ISCSI:
+		img_optype = OPTYPE_BIOS;
+		break;
+	case IMAGE_OPTION_ROM_PXE:
+		img_optype = OPTYPE_PXE_BIOS;
+		break;
+	case IMAGE_OPTION_ROM_FCOE:
+		img_optype = OPTYPE_FCOE_BIOS;
+		break;
+	case IMAGE_FIRMWARE_BACKUP_ISCSI:
+		img_optype = OPTYPE_ISCSI_BACKUP;
+		break;
+	case IMAGE_NCSI:
+		img_optype = OPTYPE_NCSI_FW;
+		break;
+	case IMAGE_FLASHISM_JUMPVECTOR:
+		img_optype = OPTYPE_FLASHISM_JUMPVECTOR;
+		break;
+	case IMAGE_FIRMWARE_PHY:
+		img_optype = OPTYPE_SH_PHY_FW;
+		break;
+	case IMAGE_REDBOOT_DIR:
+		img_optype = OPTYPE_REDBOOT_DIR;
+		break;
+	case IMAGE_REDBOOT_CONFIG:
+		img_optype = OPTYPE_REDBOOT_CONFIG;
+		break;
+	case IMAGE_UFI_DIR:
+		img_optype = OPTYPE_UFI_DIR;
+		break;
+	default:
+		break;
+	}
+
+	return img_optype;
+}
+
+static int be_flash_skyhawk(struct be_adapter *adapter,
+			    const struct firmware *fw,
+			    struct be_dma_mem *flash_cmd, int num_of_images)
+{
+	int img_hdrs_size = num_of_images * sizeof(struct image_hdr);
+	bool crc_match, old_fw_img, flash_offset_support = true;
+	struct device *dev = &adapter->pdev->dev;
+	struct flash_section_info *fsec = NULL;
+	u32 img_offset, img_size, img_type;
+	u16 img_optype, flash_optype;
+	int status, i, filehdr_size;
+	const u8 *p;
+
+	filehdr_size = sizeof(struct flash_file_hdr_g3);
+	fsec = get_fsec_info(adapter, filehdr_size + img_hdrs_size, fw);
+	if (!fsec) {
+		dev_err(dev, "Invalid Cookie. FW image may be corrupted\n");
+		return -EINVAL;
+	}
+
+retry_flash:
+	for (i = 0; i < le32_to_cpu(fsec->fsec_hdr.num_images); i++) {
+		img_offset = le32_to_cpu(fsec->fsec_entry[i].offset);
+		img_size   = le32_to_cpu(fsec->fsec_entry[i].pad_size);
+		img_type   = le32_to_cpu(fsec->fsec_entry[i].type);
+		img_optype = be_get_img_optype(fsec->fsec_entry[i]);
+		old_fw_img = fsec->fsec_entry[i].optype == 0xFFFF;
+
+		if (img_optype == 0xFFFF)
+			continue;
+
+		if (flash_offset_support)
+			flash_optype = OPTYPE_OFFSET_SPECIFIED;
+		else
+			flash_optype = img_optype;
+
+		/* Don't bother verifying CRC if an old FW image is being
+		 * flashed
+		 */
+		if (old_fw_img)
+			goto flash;
+
+		status = be_check_flash_crc(adapter, fw->data, img_offset,
+					    img_size, filehdr_size +
+					    img_hdrs_size, flash_optype,
+					    &crc_match);
+		if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST ||
+		    base_status(status) == MCC_STATUS_ILLEGAL_FIELD) {
+			/* The current FW image on the card does not support
+			 * OFFSET based flashing. Retry using older mechanism
+			 * of OPTYPE based flashing
+			 */
+			if (flash_optype == OPTYPE_OFFSET_SPECIFIED) {
+				flash_offset_support = false;
+				goto retry_flash;
+			}
+
+			/* The current FW image on the card does not recognize
+			 * the new FLASH op_type. The FW download is partially
+			 * complete. Reboot the server now to enable FW image
+			 * to recognize the new FLASH op_type. To complete the
+			 * remaining process, download the same FW again after
+			 * the reboot.
+			 */
+			dev_err(dev, "Flash incomplete. Reset the server\n");
+			dev_err(dev, "Download FW image again after reset\n");
+			return -EAGAIN;
+		} else if (status) {
+			dev_err(dev, "Could not get CRC for 0x%x region\n",
+				img_optype);
+			return -EFAULT;
+		}
+
+		if (crc_match)
+			continue;
+
+flash:
+		p = fw->data + filehdr_size + img_offset + img_hdrs_size;
+		if (p + img_size > fw->data + fw->size)
+			return -1;
+
+		status = be_flash(adapter, p, flash_cmd, flash_optype, img_size,
+				  img_offset);
+
+		/* The current FW image on the card does not support OFFSET
+		 * based flashing. Retry using older mechanism of OPTYPE based
+		 * flashing
+		 */
+		if (base_status(status) == MCC_STATUS_ILLEGAL_FIELD &&
+		    flash_optype == OPTYPE_OFFSET_SPECIFIED) {
+			flash_offset_support = false;
+			goto retry_flash;
+		}
+
+		/* For old FW images ignore ILLEGAL_FIELD error or errors on
+		 * UFI_DIR region
+		 */
+		if (old_fw_img &&
+		    (base_status(status) == MCC_STATUS_ILLEGAL_FIELD ||
+		     (img_optype == OPTYPE_UFI_DIR &&
+		      base_status(status) == MCC_STATUS_FAILED))) {
+			continue;
+		} else if (status) {
+			dev_err(dev, "Flashing section type 0x%x failed\n",
+				img_type);
+
+			switch (addl_status(status)) {
+			case MCC_ADDL_STATUS_MISSING_SIGNATURE:
+				dev_err(dev,
+					"Digital signature missing in FW\n");
+				return -EINVAL;
+			case MCC_ADDL_STATUS_INVALID_SIGNATURE:
+				dev_err(dev,
+					"Invalid digital signature in FW\n");
+				return -EINVAL;
+			default:
+				return -EFAULT;
+			}
+		}
+	}
+	return 0;
+}
+
+int lancer_fw_download(struct be_adapter *adapter,
+		       const struct firmware *fw)
+{
+	struct device *dev = &adapter->pdev->dev;
+	struct be_dma_mem flash_cmd;
+	const u8 *data_ptr = NULL;
+	u8 *dest_image_ptr = NULL;
+	size_t image_size = 0;
+	u32 chunk_size = 0;
+	u32 data_written = 0;
+	u32 offset = 0;
+	int status = 0;
+	u8 add_status = 0;
+	u8 change_status;
+
+	if (!IS_ALIGNED(fw->size, sizeof(u32))) {
+		dev_err(dev, "FW image size should be multiple of 4\n");
+		return -EINVAL;
+	}
+
+	flash_cmd.size = sizeof(struct lancer_cmd_req_write_object)
+				+ LANCER_FW_DOWNLOAD_CHUNK;
+	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size,
+					   &flash_cmd.dma, GFP_KERNEL);
+	if (!flash_cmd.va)
+		return -ENOMEM;
+
+	dest_image_ptr = flash_cmd.va +
+				sizeof(struct lancer_cmd_req_write_object);
+	image_size = fw->size;
+	data_ptr = fw->data;
+
+	while (image_size) {
+		chunk_size = min_t(u32, image_size, LANCER_FW_DOWNLOAD_CHUNK);
+
+		/* Copy the image chunk content. */
+		memcpy(dest_image_ptr, data_ptr, chunk_size);
+
+		status = lancer_cmd_write_object(adapter, &flash_cmd,
+						 chunk_size, offset,
+						 LANCER_FW_DOWNLOAD_LOCATION,
+						 &data_written, &change_status,
+						 &add_status);
+		if (status)
+			break;
+
+		offset += data_written;
+		data_ptr += data_written;
+		image_size -= data_written;
+	}
+
+	if (!status) {
+		/* Commit the FW written */
+		status = lancer_cmd_write_object(adapter, &flash_cmd,
+						 0, offset,
+						 LANCER_FW_DOWNLOAD_LOCATION,
+						 &data_written, &change_status,
+						 &add_status);
+	}
+
+	dma_free_coherent(dev, flash_cmd.size, flash_cmd.va, flash_cmd.dma);
+	if (status) {
+		dev_err(dev, "Firmware load error\n");
+		return be_cmd_status(status);
+	}
+
+	dev_info(dev, "Firmware flashed successfully\n");
+
+	if (change_status == LANCER_FW_RESET_NEEDED) {
+		dev_info(dev, "Resetting adapter to activate new FW\n");
+		status = lancer_physdev_ctrl(adapter,
+					     PHYSDEV_CONTROL_FW_RESET_MASK);
+		if (status) {
+			dev_err(dev, "Adapter busy, could not reset FW\n");
+			dev_err(dev, "Reboot server to activate new FW\n");
+		}
+	} else if (change_status != LANCER_NO_RESET_NEEDED) {
+		dev_info(dev, "Reboot server to activate new FW\n");
+	}
+
+	return 0;
+}
+
+/* Check if the flash image file is compatible with the adapter that
+ * is being flashed.
+ */
+static bool be_check_ufi_compatibility(struct be_adapter *adapter,
+				       struct flash_file_hdr_g3 *fhdr)
+{
+	if (!fhdr) {
+		dev_err(&adapter->pdev->dev, "Invalid FW UFI file");
+		return false;
+	}
+
+	/* First letter of the build version is used to identify
+	 * which chip this image file is meant for.
+	 */
+	switch (fhdr->build[0]) {
+	case BLD_STR_UFI_TYPE_SH:
+		if (!skyhawk_chip(adapter))
+			return false;
+		break;
+	case BLD_STR_UFI_TYPE_BE3:
+		if (!BE3_chip(adapter))
+			return false;
+		break;
+	case BLD_STR_UFI_TYPE_BE2:
+		if (!BE2_chip(adapter))
+			return false;
+		break;
+	default:
+		return false;
+	}
+
+	/* In BE3 FW images the "asic_type_rev" field doesn't track the
+	 * asic_rev of the chips it is compatible with.
+	 * When asic_type_rev is 0 the image is compatible only with
+	 * pre-BE3-R chips (asic_rev < 0x10)
+	 */
+	if (BEx_chip(adapter) && fhdr->asic_type_rev == 0)
+		return adapter->asic_rev < 0x10;
+	else
+		return (fhdr->asic_type_rev >= adapter->asic_rev);
+}
+
+int be_fw_download(struct be_adapter *adapter, const struct firmware *fw)
+{
+	struct device *dev = &adapter->pdev->dev;
+	struct flash_file_hdr_g3 *fhdr3;
+	struct image_hdr *img_hdr_ptr;
+	int status = 0, i, num_imgs;
+	struct be_dma_mem flash_cmd;
+
+	fhdr3 = (struct flash_file_hdr_g3 *)fw->data;
+	if (!be_check_ufi_compatibility(adapter, fhdr3)) {
+		dev_err(dev, "Flash image is not compatible with adapter\n");
+		return -EINVAL;
+	}
+
+	flash_cmd.size = sizeof(struct be_cmd_write_flashrom);
+	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size, &flash_cmd.dma,
+					   GFP_KERNEL);
+	if (!flash_cmd.va)
+		return -ENOMEM;
+
+	num_imgs = le32_to_cpu(fhdr3->num_imgs);
+	for (i = 0; i < num_imgs; i++) {
+		img_hdr_ptr = (struct image_hdr *)(fw->data +
+				(sizeof(struct flash_file_hdr_g3) +
+				 i * sizeof(struct image_hdr)));
+		if (!BE2_chip(adapter) &&
+		    le32_to_cpu(img_hdr_ptr->imageid) != 1)
+			continue;
+
+		if (skyhawk_chip(adapter))
+			status = be_flash_skyhawk(adapter, fw, &flash_cmd,
+						  num_imgs);
+		else
+			status = be_flash_BEx(adapter, fw, &flash_cmd,
+					      num_imgs);
+	}
+
+	dma_free_coherent(dev, flash_cmd.size, flash_cmd.va, flash_cmd.dma);
+	if (!status)
+		dev_info(dev, "Firmware flashed successfully\n");
+
+	return status;
+}
+
 int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
 			    struct be_dma_mem *nonemb_cmd)
 {
@@ -2892,7 +3454,6 @@ int be_cmd_get_cntl_attributes(struct be_adapter *adapter)
 	if (!status) {
 		attribs = attribs_cmd.va + sizeof(struct be_cmd_resp_hdr);
 		adapter->hba_port_num = attribs->hba_attribs.phy_port;
-		adapter->pci_func_num = attribs->pci_func_num;
 		serial_num = attribs->hba_attribs.controller_serial_number;
 		for (i = 0; i < CNTL_SERIAL_NUM_WORDS; i++)
 			adapter->serial_num[i] = le32_to_cpu(serial_num[i]) &
@@ -3575,14 +4136,16 @@ int be_cmd_query_port_name(struct be_adapter *adapter)
 	return status;
 }
 
-/* Descriptor type */
-enum {
-	FUNC_DESC = 1,
-	VFT_DESC = 2
-};
-
+/* When more than 1 NIC descriptor is present in the descriptor list,
+ * the caller must specify the pf_num to obtain the NIC descriptor
+ * corresponding to its pci function.
+ * get_vft must be true when the caller wants the VF-template desc of the
+ * PF-pool.
+ * The pf_num should be set to PF_NUM_IGNORE when the caller knows
+ * that only it's NIC descriptor is present in the descriptor list.
+ */
 static struct be_nic_res_desc *be_get_nic_desc(u8 *buf, u32 desc_count,
-					       int desc_type)
+					       bool get_vft, u8 pf_num)
 {
 	struct be_res_desc_hdr *hdr = (struct be_res_desc_hdr *)buf;
 	struct be_nic_res_desc *nic;
@@ -3592,40 +4155,42 @@ static struct be_nic_res_desc *be_get_nic_desc(u8 *buf, u32 desc_count,
 		if (hdr->desc_type == NIC_RESOURCE_DESC_TYPE_V0 ||
 		    hdr->desc_type == NIC_RESOURCE_DESC_TYPE_V1) {
 			nic = (struct be_nic_res_desc *)hdr;
-			if (desc_type == FUNC_DESC ||
-			    (desc_type == VFT_DESC &&
-			     nic->flags & (1 << VFT_SHIFT)))
+
+			if ((pf_num == PF_NUM_IGNORE ||
+			     nic->pf_num == pf_num) &&
+			    (!get_vft || nic->flags & BIT(VFT_SHIFT)))
 				return nic;
 		}
-
 		hdr->desc_len = hdr->desc_len ? : RESOURCE_DESC_SIZE_V0;
 		hdr = (void *)hdr + hdr->desc_len;
 	}
 	return NULL;
 }
 
-static struct be_nic_res_desc *be_get_vft_desc(u8 *buf, u32 desc_count)
+static struct be_nic_res_desc *be_get_vft_desc(u8 *buf, u32 desc_count,
+					       u8 pf_num)
 {
-	return be_get_nic_desc(buf, desc_count, VFT_DESC);
+	return be_get_nic_desc(buf, desc_count, true, pf_num);
 }
 
-static struct be_nic_res_desc *be_get_func_nic_desc(u8 *buf, u32 desc_count)
+static struct be_nic_res_desc *be_get_func_nic_desc(u8 *buf, u32 desc_count,
+						    u8 pf_num)
 {
-	return be_get_nic_desc(buf, desc_count, FUNC_DESC);
+	return be_get_nic_desc(buf, desc_count, false, pf_num);
 }
 
-static struct be_pcie_res_desc *be_get_pcie_desc(u8 devfn, u8 *buf,
-						 u32 desc_count)
+static struct be_pcie_res_desc *be_get_pcie_desc(u8 *buf, u32 desc_count,
+						 u8 pf_num)
 {
 	struct be_res_desc_hdr *hdr = (struct be_res_desc_hdr *)buf;
 	struct be_pcie_res_desc *pcie;
 	int i;
 
 	for (i = 0; i < desc_count; i++) {
-		if ((hdr->desc_type == PCIE_RESOURCE_DESC_TYPE_V0 ||
-		     hdr->desc_type == PCIE_RESOURCE_DESC_TYPE_V1)) {
-			pcie = (struct be_pcie_res_desc	*)hdr;
-			if (pcie->pf_num == devfn)
+		if (hdr->desc_type == PCIE_RESOURCE_DESC_TYPE_V0 ||
+		    hdr->desc_type == PCIE_RESOURCE_DESC_TYPE_V1) {
+			pcie = (struct be_pcie_res_desc *)hdr;
+			if (pcie->pf_num == pf_num)
 				return pcie;
 		}
 
@@ -3710,13 +4275,23 @@ int be_cmd_get_func_config(struct be_adapter *adapter, struct be_resources *res)
 		u32 desc_count = le32_to_cpu(resp->desc_count);
 		struct be_nic_res_desc *desc;
 
-		desc = be_get_func_nic_desc(resp->func_param, desc_count);
+		/* GET_FUNC_CONFIG returns resource descriptors of the
+		 * current function only. So, pf_num should be set to
+		 * PF_NUM_IGNORE.
+		 */
+		desc = be_get_func_nic_desc(resp->func_param, desc_count,
+					    PF_NUM_IGNORE);
 		if (!desc) {
 			status = -EINVAL;
 			goto err;
 		}
-		adapter->pf_number = desc->pf_num;
-		be_copy_nic_desc(res, desc);
+
+		/* Store pf_num & vf_num for later use in GET_PROFILE_CONFIG */
+		adapter->pf_num = desc->pf_num;
+		adapter->vf_num = desc->vf_num;
+
+		if (res)
+			be_copy_nic_desc(res, desc);
 	}
 err:
 	mutex_unlock(&adapter->mbox_lock);
@@ -3726,10 +4301,7 @@ err:
 	return status;
 }
 
-/* Will use MBOX only if MCCQ has not been created
- * non-zero domain => a PF is querying this on behalf of a VF
- * zero domain => a PF or a VF is querying this for itself
- */
+/* Will use MBOX only if MCCQ has not been created */
 int be_cmd_get_profile_config(struct be_adapter *adapter,
 			      struct be_resources *res, u8 query, u8 domain)
 {
@@ -3759,12 +4331,7 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 	if (!lancer_chip(adapter))
 		req->hdr.version = 1;
 	req->type = ACTIVE_PROFILE_TYPE;
-	/* When a function is querying profile information relating to
-	 * itself hdr.pf_number must be set to it's pci_func_num + 1
-	 */
 	req->hdr.domain = domain;
-	if (domain == 0)
-		req->hdr.pf_num = adapter->pci_func_num + 1;
 
 	/* When QUERY_MODIFIABLE_FIELDS_TYPE bit is set, cmd returns the
 	 * descriptors with all bits set to "1" for the fields which can be
@@ -3780,8 +4347,8 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 	resp = cmd.va;
 	desc_count = le16_to_cpu(resp->desc_count);
 
-	pcie = be_get_pcie_desc(adapter->pdev->devfn, resp->func_param,
-				desc_count);
+	pcie = be_get_pcie_desc(resp->func_param, desc_count,
+				adapter->pf_num);
 	if (pcie)
 		res->max_vfs = le16_to_cpu(pcie->num_vfs);
 
@@ -3789,11 +4356,13 @@ int be_cmd_get_profile_config(struct be_adapter *adapter,
 	if (port)
 		adapter->mc_type = port->mc_type;
 
-	nic = be_get_func_nic_desc(resp->func_param, desc_count);
+	nic = be_get_func_nic_desc(resp->func_param, desc_count,
+				   adapter->pf_num);
 	if (nic)
 		be_copy_nic_desc(res, nic);
 
-	vf_res = be_get_vft_desc(resp->func_param, desc_count);
+	vf_res = be_get_vft_desc(resp->func_param, desc_count,
+				 adapter->pf_num);
 	if (vf_res)
 		res->vf_if_cap_flags = vf_res->cap_flags;
 err:
@@ -3883,7 +4452,7 @@ int be_cmd_config_qos(struct be_adapter *adapter, u32 max_rate, u16 link_speed,
 		return be_cmd_set_qos(adapter, max_rate / 10, domain);
 
 	be_reset_nic_desc(&nic_desc);
-	nic_desc.pf_num = adapter->pf_number;
+	nic_desc.pf_num = adapter->pf_num;
 	nic_desc.vf_num = domain;
 	nic_desc.bw_min = 0;
 	if (lancer_chip(adapter)) {
@@ -4260,16 +4829,13 @@ err:
 	return status;
 }
 
-int be_cmd_set_logical_link_config(struct be_adapter *adapter,
-				   int link_state, u8 domain)
+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;
 
-	if (BEx_chip(adapter) || lancer_chip(adapter))
-		return -EOPNOTSUPP;
-
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
@@ -4284,14 +4850,15 @@ int be_cmd_set_logical_link_config(struct be_adapter *adapter,
 			       OPCODE_COMMON_SET_LOGICAL_LINK_CONFIG,
 			       sizeof(*req), wrb, NULL);
 
-	req->hdr.version = 1;
+	req->hdr.version = version;
 	req->hdr.domain = domain;
 
-	if (link_state == IFLA_VF_LINK_STATE_ENABLE)
-		req->link_config |= 1;
+	if (link_state == IFLA_VF_LINK_STATE_ENABLE ||
+	    link_state == IFLA_VF_LINK_STATE_AUTO)
+		req->link_config |= PLINK_ENABLE;
 
 	if (link_state == IFLA_VF_LINK_STATE_AUTO)
-		req->link_config |= 1 << PLINK_TRACK_SHIFT;
+		req->link_config |= PLINK_TRACK;
 
 	status = be_mcc_notify_wait(adapter);
 err:
@@ -4299,6 +4866,25 @@ err:
 	return status;
 }
 
+int be_cmd_set_logical_link_config(struct be_adapter *adapter,
+				   int link_state, u8 domain)
+{
+	int status;
+
+	if (BEx_chip(adapter))
+		return -EOPNOTSUPP;
+
+	status = __be_cmd_set_logical_link_config(adapter, link_state,
+						  2, domain);
+
+	/* Version 2 of the command will not be recognized by older FW.
+	 * On such a failure issue version 1 of the command.
+	 */
+	if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST)
+		status = __be_cmd_set_logical_link_config(adapter, link_state,
+							  1, domain);
+	return status;
+}
 int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 		    int wrb_payload_size, u16 *cmd_status, u16 *ext_status)
 {
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index 91155ea74f34..6d9a8d78e8ad 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -66,7 +66,9 @@ enum mcc_addl_status {
 	MCC_ADDL_STATUS_INSUFFICIENT_RESOURCES = 0x16,
 	MCC_ADDL_STATUS_FLASH_IMAGE_CRC_MISMATCH = 0x4d,
 	MCC_ADDL_STATUS_TOO_MANY_INTERFACES = 0x4a,
-	MCC_ADDL_STATUS_INSUFFICIENT_VLANS = 0xab
+	MCC_ADDL_STATUS_INSUFFICIENT_VLANS = 0xab,
+	MCC_ADDL_STATUS_INVALID_SIGNATURE = 0x56,
+	MCC_ADDL_STATUS_MISSING_SIGNATURE = 0x57
 };
 
 #define CQE_BASE_STATUS_MASK		0xFFFF
@@ -289,9 +291,7 @@ struct be_cmd_req_hdr {
 	u32 timeout;		/* dword 1 */
 	u32 request_length;	/* dword 2 */
 	u8 version;		/* dword 3 */
-	u8 rsvd1;		/* dword 3 */
-	u8 pf_num;		/* dword 3 */
-	u8 rsvd2;		/* dword 3 */
+	u8 rsvd[3];		/* dword 3 */
 };
 
 #define RESP_HDR_INFO_OPCODE_SHIFT	0	/* bits 0 - 7 */
@@ -622,10 +622,13 @@ enum be_if_flags {
 					 BE_IF_FLAGS_VLAN_PROMISCUOUS |\
 					 BE_IF_FLAGS_MCAST_PROMISCUOUS)
 
-#define BE_IF_EN_FLAGS	(BE_IF_FLAGS_BROADCAST | BE_IF_FLAGS_PASS_L3L4_ERRORS |\
-			BE_IF_FLAGS_MULTICAST | BE_IF_FLAGS_UNTAGGED)
+#define BE_IF_FILT_FLAGS_BASIC (BE_IF_FLAGS_BROADCAST | \
+				BE_IF_FLAGS_PASS_L3L4_ERRORS | \
+				BE_IF_FLAGS_UNTAGGED)
 
-#define BE_IF_ALL_FILT_FLAGS	(BE_IF_EN_FLAGS | BE_IF_FLAGS_ALL_PROMISCUOUS)
+#define BE_IF_ALL_FILT_FLAGS	(BE_IF_FILT_FLAGS_BASIC | \
+				 BE_IF_FLAGS_MULTICAST | \
+				 BE_IF_FLAGS_ALL_PROMISCUOUS)
 
 /* An RX interface is an object with one or more MAC addresses and
  * filtering capabilities. */
@@ -1207,68 +1210,85 @@ struct be_cmd_resp_get_beacon_state {
 /* Flashrom related descriptors */
 #define MAX_FLASH_COMP			32
 
-#define OPTYPE_ISCSI_ACTIVE		0
-#define OPTYPE_REDBOOT			1
-#define OPTYPE_BIOS			2
-#define OPTYPE_PXE_BIOS			3
-#define OPTYPE_OFFSET_SPECIFIED		7
-#define OPTYPE_FCOE_BIOS		8
-#define OPTYPE_ISCSI_BACKUP		9
-#define OPTYPE_FCOE_FW_ACTIVE		10
-#define OPTYPE_FCOE_FW_BACKUP		11
-#define OPTYPE_NCSI_FW			13
-#define OPTYPE_REDBOOT_DIR		18
-#define OPTYPE_REDBOOT_CONFIG		19
-#define OPTYPE_SH_PHY_FW		21
-#define OPTYPE_FLASHISM_JUMPVECTOR	22
-#define OPTYPE_UFI_DIR			23
-#define OPTYPE_PHY_FW			99
-
-#define FLASH_BIOS_IMAGE_MAX_SIZE_g2	262144  /* Max OPTION ROM image sz */
-#define FLASH_REDBOOT_IMAGE_MAX_SIZE_g2	262144  /* Max Redboot image sz    */
-#define FLASH_IMAGE_MAX_SIZE_g2		1310720 /* Max firmware image size */
-
-#define FLASH_NCSI_IMAGE_MAX_SIZE_g3	262144
-#define FLASH_PHY_FW_IMAGE_MAX_SIZE_g3	262144
-#define FLASH_BIOS_IMAGE_MAX_SIZE_g3	524288  /* Max OPTION ROM image sz */
-#define FLASH_REDBOOT_IMAGE_MAX_SIZE_g3	1048576 /* Max Redboot image sz    */
-#define FLASH_IMAGE_MAX_SIZE_g3		2097152 /* Max firmware image size */
-
-/* Offsets for components on Flash. */
-#define FLASH_REDBOOT_START_g2			0
-#define FLASH_FCoE_BIOS_START_g2		524288
-#define FLASH_iSCSI_PRIMARY_IMAGE_START_g2	1048576
-#define FLASH_iSCSI_BACKUP_IMAGE_START_g2	2359296
-#define FLASH_FCoE_PRIMARY_IMAGE_START_g2	3670016
-#define FLASH_FCoE_BACKUP_IMAGE_START_g2	4980736
-#define FLASH_iSCSI_BIOS_START_g2		7340032
-#define FLASH_PXE_BIOS_START_g2			7864320
-
-#define FLASH_REDBOOT_START_g3			262144
-#define FLASH_PHY_FW_START_g3			1310720
-#define FLASH_iSCSI_PRIMARY_IMAGE_START_g3	2097152
-#define FLASH_iSCSI_BACKUP_IMAGE_START_g3	4194304
-#define FLASH_FCoE_PRIMARY_IMAGE_START_g3	6291456
-#define FLASH_FCoE_BACKUP_IMAGE_START_g3	8388608
-#define FLASH_iSCSI_BIOS_START_g3		12582912
-#define FLASH_PXE_BIOS_START_g3			13107200
-#define FLASH_FCoE_BIOS_START_g3		13631488
-#define FLASH_NCSI_START_g3			15990784
-
-#define IMAGE_NCSI			16
-#define IMAGE_OPTION_ROM_PXE		32
-#define IMAGE_OPTION_ROM_FCoE		33
-#define IMAGE_OPTION_ROM_ISCSI		34
-#define IMAGE_FLASHISM_JUMPVECTOR	48
-#define IMAGE_FIRMWARE_iSCSI		160
-#define IMAGE_FIRMWARE_FCoE		162
-#define IMAGE_FIRMWARE_BACKUP_iSCSI	176
-#define IMAGE_FIRMWARE_BACKUP_FCoE	178
-#define IMAGE_FIRMWARE_PHY		192
-#define IMAGE_REDBOOT_DIR		208
-#define IMAGE_REDBOOT_CONFIG		209
-#define IMAGE_UFI_DIR			210
-#define IMAGE_BOOT_CODE			224
+/* Optypes of each component in the UFI */
+enum {
+	OPTYPE_ISCSI_ACTIVE = 0,
+	OPTYPE_REDBOOT = 1,
+	OPTYPE_BIOS = 2,
+	OPTYPE_PXE_BIOS = 3,
+	OPTYPE_OFFSET_SPECIFIED = 7,
+	OPTYPE_FCOE_BIOS = 8,
+	OPTYPE_ISCSI_BACKUP = 9,
+	OPTYPE_FCOE_FW_ACTIVE = 10,
+	OPTYPE_FCOE_FW_BACKUP = 11,
+	OPTYPE_NCSI_FW = 13,
+	OPTYPE_REDBOOT_DIR = 18,
+	OPTYPE_REDBOOT_CONFIG = 19,
+	OPTYPE_SH_PHY_FW = 21,
+	OPTYPE_FLASHISM_JUMPVECTOR = 22,
+	OPTYPE_UFI_DIR = 23,
+	OPTYPE_PHY_FW = 99
+};
+
+/* Maximum sizes of components in BE2 FW UFI */
+enum {
+	BE2_BIOS_COMP_MAX_SIZE = 0x40000,
+	BE2_REDBOOT_COMP_MAX_SIZE = 0x40000,
+	BE2_COMP_MAX_SIZE = 0x140000
+};
+
+/* Maximum sizes of components in BE3 FW UFI */
+enum {
+	BE3_NCSI_COMP_MAX_SIZE = 0x40000,
+	BE3_PHY_FW_COMP_MAX_SIZE = 0x40000,
+	BE3_BIOS_COMP_MAX_SIZE = 0x80000,
+	BE3_REDBOOT_COMP_MAX_SIZE = 0x100000,
+	BE3_COMP_MAX_SIZE = 0x200000
+};
+
+/* Offsets for components in BE2 FW UFI */
+enum {
+	BE2_REDBOOT_START = 0x8000,
+	BE2_FCOE_BIOS_START = 0x80000,
+	BE2_ISCSI_PRIMARY_IMAGE_START = 0x100000,
+	BE2_ISCSI_BACKUP_IMAGE_START = 0x240000,
+	BE2_FCOE_PRIMARY_IMAGE_START = 0x380000,
+	BE2_FCOE_BACKUP_IMAGE_START = 0x4c0000,
+	BE2_ISCSI_BIOS_START = 0x700000,
+	BE2_PXE_BIOS_START = 0x780000
+};
+
+/* Offsets for components in BE3 FW UFI */
+enum {
+	BE3_REDBOOT_START = 0x40000,
+	BE3_PHY_FW_START = 0x140000,
+	BE3_ISCSI_PRIMARY_IMAGE_START = 0x200000,
+	BE3_ISCSI_BACKUP_IMAGE_START = 0x400000,
+	BE3_FCOE_PRIMARY_IMAGE_START = 0x600000,
+	BE3_FCOE_BACKUP_IMAGE_START = 0x800000,
+	BE3_ISCSI_BIOS_START = 0xc00000,
+	BE3_PXE_BIOS_START = 0xc80000,
+	BE3_FCOE_BIOS_START = 0xd00000,
+	BE3_NCSI_START = 0xf40000
+};
+
+/* Component entry types */
+enum {
+	IMAGE_NCSI = 0x10,
+	IMAGE_OPTION_ROM_PXE = 0x20,
+	IMAGE_OPTION_ROM_FCOE = 0x21,
+	IMAGE_OPTION_ROM_ISCSI = 0x22,
+	IMAGE_FLASHISM_JUMPVECTOR = 0x30,
+	IMAGE_FIRMWARE_ISCSI = 0xa0,
+	IMAGE_FIRMWARE_FCOE = 0xa2,
+	IMAGE_FIRMWARE_BACKUP_ISCSI = 0xb0,
+	IMAGE_FIRMWARE_BACKUP_FCOE = 0xb2,
+	IMAGE_FIRMWARE_PHY = 0xc0,
+	IMAGE_REDBOOT_DIR = 0xd0,
+	IMAGE_REDBOOT_CONFIG = 0xd1,
+	IMAGE_UFI_DIR = 0xd2,
+	IMAGE_BOOT_CODE = 0xe2
+};
 
 struct controller_id {
 	u32 vendor;
@@ -1394,6 +1414,9 @@ struct be_cmd_read_flash_crc {
 } __packed;
 
 /**************** Lancer Firmware Flash ************/
+#define LANCER_FW_DOWNLOAD_CHUNK      (32 * 1024)
+#define LANCER_FW_DOWNLOAD_LOCATION   "/prg"
+
 struct amap_lancer_write_obj_context {
 	u8 write_length[24];
 	u8 reserved1[7];
@@ -1654,11 +1677,7 @@ struct mgmt_hba_attribs {
 
 struct mgmt_controller_attrib {
 	struct mgmt_hba_attribs hba_attribs;
-	u32 rsvd0[2];
-	u16 rsvd1;
-	u8 pci_func_num;
-	u8 rsvd2;
-	u32 rsvd3[7];
+	u32 rsvd0[10];
 } __packed;
 
 struct be_cmd_req_cntl_attribs {
@@ -2083,6 +2102,7 @@ struct be_port_res_desc {
 #define NV_TYPE_VXLAN				3
 #define SOCVID_SHIFT				2	/* Strip outer vlan */
 #define RCVID_SHIFT				4	/* Report vlan */
+#define PF_NUM_IGNORE				255
 	u8 nv_flags;
 	u8 rsvd2;
 	__le16 nv_port;					/* vxlan/gre port */
@@ -2246,7 +2266,8 @@ struct be_cmd_resp_get_iface_list {
 };
 
 /*************** Set logical link ********************/
-#define PLINK_TRACK_SHIFT	8
+#define PLINK_ENABLE            BIT(0)
+#define PLINK_TRACK             BIT(8)
 struct be_cmd_req_set_ll_link {
 	struct be_cmd_req_hdr hdr;
 	u32 link_config; /* Bit 0: UP_DOWN, Bit 9: PLINK */
@@ -2321,19 +2342,11 @@ int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
 				      u8 page_num, u8 *data);
 int be_cmd_query_cable_type(struct be_adapter *adapter);
 int be_cmd_query_sfp_info(struct be_adapter *adapter);
-int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
-			  u32 flash_oper, u32 flash_opcode, u32 img_offset,
-			  u32 buf_size);
-int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
-			    u32 data_size, u32 data_offset,
-			    const char *obj_name, u32 *data_written,
-			    u8 *change_status, u8 *addn_status);
 int lancer_cmd_read_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 			   u32 data_size, u32 data_offset, const char *obj_name,
 			   u32 *data_read, u32 *eof, u8 *addn_status);
-int lancer_cmd_delete_object(struct be_adapter *adapter, const char *obj_name);
-int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
-			 u16 img_optype, u32 img_offset, u32 crc_offset);
+int lancer_fw_download(struct be_adapter *adapter, const struct firmware *fw);
+int be_fw_download(struct be_adapter *adapter, const struct firmware *fw);
 int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
 			    struct be_dma_mem *nonemb_cmd);
 int be_cmd_fw_init(struct be_adapter *adapter);
@@ -2355,9 +2368,9 @@ int be_cmd_config_qos(struct be_adapter *adapter, u32 max_rate,
 void be_detect_error(struct be_adapter *adapter);
 int be_cmd_get_die_temperature(struct be_adapter *adapter);
 int be_cmd_get_cntl_attributes(struct be_adapter *adapter);
+int be_cmd_get_fat_dump_len(struct be_adapter *adapter, u32 *dump_size);
+int be_cmd_get_fat_dump(struct be_adapter *adapter, u32 buf_len, void *buf);
 int be_cmd_req_native_mode(struct be_adapter *adapter);
-int be_cmd_get_reg_len(struct be_adapter *adapter, u32 *log_size);
-int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf);
 int be_cmd_get_fn_privileges(struct be_adapter *adapter, u32 *privilege,
 			     u32 domain);
 int be_cmd_set_fn_privileges(struct be_adapter *adapter, u32 privileges,
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index 734f655c99c1..a19ac441336f 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -241,17 +241,28 @@ static u32 lancer_cmd_get_file_len(struct be_adapter *adapter, u8 *file_name)
 	u32 data_read = 0, eof;
 	u8 addn_status;
 	struct be_dma_mem data_len_cmd;
-	int status;
 
 	memset(&data_len_cmd, 0, sizeof(data_len_cmd));
 	/* data_offset and data_size should be 0 to get reg len */
-	status = lancer_cmd_read_object(adapter, &data_len_cmd, 0, 0,
-					file_name, &data_read, &eof,
-					&addn_status);
+	lancer_cmd_read_object(adapter, &data_len_cmd, 0, 0, file_name,
+			       &data_read, &eof, &addn_status);
 
 	return data_read;
 }
 
+static int be_get_dump_len(struct be_adapter *adapter)
+{
+	u32 dump_size = 0;
+
+	if (lancer_chip(adapter))
+		dump_size = lancer_cmd_get_file_len(adapter,
+						    LANCER_FW_DUMP_FILE);
+	else
+		dump_size = adapter->fat_dump_len;
+
+	return dump_size;
+}
+
 static int lancer_cmd_read_file(struct be_adapter *adapter, u8 *file_name,
 				u32 buf_len, void *buf)
 {
@@ -293,37 +304,18 @@ static int lancer_cmd_read_file(struct be_adapter *adapter, u8 *file_name,
 	return status;
 }
 
-static int be_get_reg_len(struct net_device *netdev)
+static int be_read_dump_data(struct be_adapter *adapter, u32 dump_len,
+			     void *buf)
 {
-	struct be_adapter *adapter = netdev_priv(netdev);
-	u32 log_size = 0;
-
-	if (!check_privilege(adapter, MAX_PRIVILEGES))
-		return 0;
-
-	if (be_physfn(adapter)) {
-		if (lancer_chip(adapter))
-			log_size = lancer_cmd_get_file_len(adapter,
-							   LANCER_FW_DUMP_FILE);
-		else
-			be_cmd_get_reg_len(adapter, &log_size);
-	}
-	return log_size;
-}
+	int status = 0;
 
-static void
-be_get_regs(struct net_device *netdev, struct ethtool_regs *regs, void *buf)
-{
-	struct be_adapter *adapter = netdev_priv(netdev);
+	if (lancer_chip(adapter))
+		status = lancer_cmd_read_file(adapter, LANCER_FW_DUMP_FILE,
+					      dump_len, buf);
+	else
+		status = be_cmd_get_fat_dump(adapter, dump_len, buf);
 
-	if (be_physfn(adapter)) {
-		memset(buf, 0, regs->len);
-		if (lancer_chip(adapter))
-			lancer_cmd_read_file(adapter, LANCER_FW_DUMP_FILE,
-					     regs->len, buf);
-		else
-			be_cmd_get_regs(adapter, regs->len, buf);
-	}
+	return status;
 }
 
 static int be_get_coalesce(struct net_device *netdev,
@@ -916,6 +908,34 @@ static int be_do_flash(struct net_device *netdev, struct ethtool_flash *efl)
 	return be_load_fw(adapter, efl->data);
 }
 
+static int
+be_get_dump_flag(struct net_device *netdev, struct ethtool_dump *dump)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	if (!check_privilege(adapter, MAX_PRIVILEGES))
+		return -EOPNOTSUPP;
+
+	dump->len = be_get_dump_len(adapter);
+	dump->version = 1;
+	dump->flag = 0x1;	/* FW dump is enabled */
+	return 0;
+}
+
+static int
+be_get_dump_data(struct net_device *netdev, struct ethtool_dump *dump,
+		 void *buf)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	int status;
+
+	if (!check_privilege(adapter, MAX_PRIVILEGES))
+		return -EOPNOTSUPP;
+
+	status = be_read_dump_data(adapter, dump->len, buf);
+	return be_cmd_status(status);
+}
+
 static int be_get_eeprom_len(struct net_device *netdev)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
@@ -1313,8 +1333,6 @@ const struct ethtool_ops be_ethtool_ops = {
 	.set_msglevel = be_set_msg_level,
 	.get_sset_count = be_get_sset_count,
 	.get_ethtool_stats = be_get_ethtool_stats,
-	.get_regs_len = be_get_reg_len,
-	.get_regs = be_get_regs,
 	.flash_device = be_do_flash,
 	.self_test = be_self_test,
 	.get_rxnfc = be_get_rxnfc,
@@ -1323,6 +1341,8 @@ const struct ethtool_ops be_ethtool_ops = {
 	.get_rxfh_key_size = be_get_rxfh_key_size,
 	.get_rxfh = be_get_rxfh,
 	.set_rxfh = be_set_rxfh,
+	.get_dump_flag = be_get_dump_flag,
+	.get_dump_data = be_get_dump_data,
 	.get_channels = be_get_channels,
 	.set_channels = be_set_channels,
 	.get_module_info = be_get_module_info,
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 8a1d9fffd7d6..d1cf1274fc2f 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -125,6 +125,11 @@ static const char * const ue_status_hi_desc[] = {
 	"Unknown"
 };
 
+#define BE_VF_IF_EN_FLAGS	(BE_IF_FLAGS_UNTAGGED | \
+				 BE_IF_FLAGS_BROADCAST | \
+				 BE_IF_FLAGS_MULTICAST | \
+				 BE_IF_FLAGS_PASS_L3L4_ERRORS)
+
 static void be_queue_free(struct be_adapter *adapter, struct be_queue_info *q)
 {
 	struct be_dma_mem *mem = &q->dma_mem;
@@ -729,7 +734,7 @@ static inline u16 be_get_tx_vlan_tag(struct be_adapter *adapter,
 	/* If vlan priority provided by OS is NOT in available bmap */
 	if (!(adapter->vlan_prio_bmap & (1 << vlan_prio)))
 		vlan_tag = (vlan_tag & ~VLAN_PRIO_MASK) |
-				adapter->recommended_prio;
+				adapter->recommended_prio_bits;
 
 	return vlan_tag;
 }
@@ -2184,7 +2189,6 @@ static void be_rx_compl_process_gro(struct be_rx_obj *rxo,
 		skb_set_hash(skb, rxcp->rss_hash, PKT_HASH_TYPE_L3);
 
 	skb->csum_level = rxcp->tunneled;
-	skb_mark_napi_id(skb, napi);
 
 	if (rxcp->vlanf)
 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), rxcp->vlan_tag);
@@ -2631,7 +2635,6 @@ static int be_evt_queues_create(struct be_adapter *adapter)
 				eqo->affinity_mask);
 		netif_napi_add(adapter->netdev, &eqo->napi, be_poll,
 			       BE_NAPI_WEIGHT);
-		napi_hash_add(&eqo->napi);
 	}
 	return 0;
 }
@@ -3539,7 +3542,7 @@ static int be_enable_if_filters(struct be_adapter *adapter)
 {
 	int status;
 
-	status = be_cmd_rx_filter(adapter, BE_IF_EN_FLAGS, ON);
+	status = be_cmd_rx_filter(adapter, BE_IF_FILT_FLAGS_BASIC, ON);
 	if (status)
 		return status;
 
@@ -3859,8 +3862,7 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 	int status;
 
 	/* If a FW profile exists, then cap_flags are updated */
-	cap_flags = BE_IF_FLAGS_UNTAGGED | BE_IF_FLAGS_BROADCAST |
-		    BE_IF_FLAGS_MULTICAST | BE_IF_FLAGS_PASS_L3L4_ERRORS;
+	cap_flags = BE_VF_IF_EN_FLAGS;
 
 	for_all_vfs(adapter, vf_cfg, vf) {
 		if (!BE3_chip(adapter)) {
@@ -3876,10 +3878,8 @@ static int be_vfs_if_create(struct be_adapter *adapter)
 			}
 		}
 
-		en_flags = cap_flags & (BE_IF_FLAGS_UNTAGGED |
-					BE_IF_FLAGS_BROADCAST |
-					BE_IF_FLAGS_MULTICAST |
-					BE_IF_FLAGS_PASS_L3L4_ERRORS);
+		/* PF should enable IF flags during proxy if_create call */
+		en_flags = cap_flags & BE_VF_IF_EN_FLAGS;
 		status = be_cmd_if_create(adapter, cap_flags, en_flags,
 					  &vf_cfg->if_handle, vf + 1);
 		if (status)
@@ -4204,10 +4204,17 @@ static int be_get_config(struct be_adapter *adapter)
 	int status, level;
 	u16 profile_id;
 
+	status = be_cmd_get_cntl_attributes(adapter);
+	if (status)
+		return status;
+
 	status = be_cmd_query_fw_cfg(adapter);
 	if (status)
 		return status;
 
+	if (!lancer_chip(adapter) && be_physfn(adapter))
+		be_cmd_get_fat_dump_len(adapter, &adapter->fat_dump_len);
+
 	if (BEx_chip(adapter)) {
 		level = be_cmd_get_fw_log_level(adapter);
 		adapter->msg_enable =
@@ -4402,10 +4409,14 @@ static int be_setup(struct be_adapter *adapter)
 	if (!lancer_chip(adapter))
 		be_cmd_req_native_mode(adapter);
 
-	/* Need to invoke this cmd first to get the PCI Function Number */
-	status = be_cmd_get_cntl_attributes(adapter);
-	if (status)
-		return status;
+	/* invoke this cmd first to get pf_num and vf_num which are needed
+	 * for issuing profile related cmds
+	 */
+	if (!BEx_chip(adapter)) {
+		status = be_cmd_get_func_config(adapter, NULL);
+		if (status)
+			return status;
+	}
 
 	if (!BE2_chip(adapter) && be_physfn(adapter))
 		be_alloc_sriov_res(adapter);
@@ -4490,570 +4501,6 @@ static void be_netpoll(struct net_device *netdev)
 }
 #endif
 
-static char flash_cookie[2][16] = {"*** SE FLAS", "H DIRECTORY *** "};
-
-static bool phy_flashing_required(struct be_adapter *adapter)
-{
-	return (adapter->phy.phy_type == PHY_TYPE_TN_8022 &&
-		adapter->phy.interface_type == PHY_TYPE_BASET_10GB);
-}
-
-static bool is_comp_in_ufi(struct be_adapter *adapter,
-			   struct flash_section_info *fsec, int type)
-{
-	int i = 0, img_type = 0;
-	struct flash_section_info_g2 *fsec_g2 = NULL;
-
-	if (BE2_chip(adapter))
-		fsec_g2 = (struct flash_section_info_g2 *)fsec;
-
-	for (i = 0; i < MAX_FLASH_COMP; i++) {
-		if (fsec_g2)
-			img_type = le32_to_cpu(fsec_g2->fsec_entry[i].type);
-		else
-			img_type = le32_to_cpu(fsec->fsec_entry[i].type);
-
-		if (img_type == type)
-			return true;
-	}
-	return false;
-
-}
-
-static struct flash_section_info *get_fsec_info(struct be_adapter *adapter,
-						int header_size,
-						const struct firmware *fw)
-{
-	struct flash_section_info *fsec = NULL;
-	const u8 *p = fw->data;
-
-	p += header_size;
-	while (p < (fw->data + fw->size)) {
-		fsec = (struct flash_section_info *)p;
-		if (!memcmp(flash_cookie, fsec->cookie, sizeof(flash_cookie)))
-			return fsec;
-		p += 32;
-	}
-	return NULL;
-}
-
-static int be_check_flash_crc(struct be_adapter *adapter, const u8 *p,
-			      u32 img_offset, u32 img_size, int hdr_size,
-			      u16 img_optype, bool *crc_match)
-{
-	u32 crc_offset;
-	int status;
-	u8 crc[4];
-
-	status = be_cmd_get_flash_crc(adapter, crc, img_optype, img_offset,
-				      img_size - 4);
-	if (status)
-		return status;
-
-	crc_offset = hdr_size + img_offset + img_size - 4;
-
-	/* Skip flashing, if crc of flashed region matches */
-	if (!memcmp(crc, p + crc_offset, 4))
-		*crc_match = true;
-	else
-		*crc_match = false;
-
-	return status;
-}
-
-static int be_flash(struct be_adapter *adapter, const u8 *img,
-		    struct be_dma_mem *flash_cmd, int optype, int img_size,
-		    u32 img_offset)
-{
-	u32 flash_op, num_bytes, total_bytes = img_size, bytes_sent = 0;
-	struct be_cmd_write_flashrom *req = flash_cmd->va;
-	int status;
-
-	while (total_bytes) {
-		num_bytes = min_t(u32, 32*1024, total_bytes);
-
-		total_bytes -= num_bytes;
-
-		if (!total_bytes) {
-			if (optype == OPTYPE_PHY_FW)
-				flash_op = FLASHROM_OPER_PHY_FLASH;
-			else
-				flash_op = FLASHROM_OPER_FLASH;
-		} else {
-			if (optype == OPTYPE_PHY_FW)
-				flash_op = FLASHROM_OPER_PHY_SAVE;
-			else
-				flash_op = FLASHROM_OPER_SAVE;
-		}
-
-		memcpy(req->data_buf, img, num_bytes);
-		img += num_bytes;
-		status = be_cmd_write_flashrom(adapter, flash_cmd, optype,
-					       flash_op, img_offset +
-					       bytes_sent, num_bytes);
-		if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST &&
-		    optype == OPTYPE_PHY_FW)
-			break;
-		else if (status)
-			return status;
-
-		bytes_sent += num_bytes;
-	}
-	return 0;
-}
-
-/* For BE2, BE3 and BE3-R */
-static int be_flash_BEx(struct be_adapter *adapter,
-			const struct firmware *fw,
-			struct be_dma_mem *flash_cmd, int num_of_images)
-{
-	int img_hdrs_size = (num_of_images * sizeof(struct image_hdr));
-	struct device *dev = &adapter->pdev->dev;
-	struct flash_section_info *fsec = NULL;
-	int status, i, filehdr_size, num_comp;
-	const struct flash_comp *pflashcomp;
-	bool crc_match;
-	const u8 *p;
-
-	struct flash_comp gen3_flash_types[] = {
-		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g3, OPTYPE_ISCSI_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_iSCSI},
-		{ FLASH_REDBOOT_START_g3, OPTYPE_REDBOOT,
-			FLASH_REDBOOT_IMAGE_MAX_SIZE_g3, IMAGE_BOOT_CODE},
-		{ FLASH_iSCSI_BIOS_START_g3, OPTYPE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_ISCSI},
-		{ FLASH_PXE_BIOS_START_g3, OPTYPE_PXE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_PXE},
-		{ FLASH_FCoE_BIOS_START_g3, OPTYPE_FCOE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g3, IMAGE_OPTION_ROM_FCoE},
-		{ FLASH_iSCSI_BACKUP_IMAGE_START_g3, OPTYPE_ISCSI_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_BACKUP_iSCSI},
-		{ FLASH_FCoE_PRIMARY_IMAGE_START_g3, OPTYPE_FCOE_FW_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_FCoE},
-		{ FLASH_FCoE_BACKUP_IMAGE_START_g3, OPTYPE_FCOE_FW_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_BACKUP_FCoE},
-		{ FLASH_NCSI_START_g3, OPTYPE_NCSI_FW,
-			FLASH_NCSI_IMAGE_MAX_SIZE_g3, IMAGE_NCSI},
-		{ FLASH_PHY_FW_START_g3, OPTYPE_PHY_FW,
-			FLASH_PHY_FW_IMAGE_MAX_SIZE_g3, IMAGE_FIRMWARE_PHY}
-	};
-
-	struct flash_comp gen2_flash_types[] = {
-		{ FLASH_iSCSI_PRIMARY_IMAGE_START_g2, OPTYPE_ISCSI_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_iSCSI},
-		{ FLASH_REDBOOT_START_g2, OPTYPE_REDBOOT,
-			FLASH_REDBOOT_IMAGE_MAX_SIZE_g2, IMAGE_BOOT_CODE},
-		{ FLASH_iSCSI_BIOS_START_g2, OPTYPE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_ISCSI},
-		{ FLASH_PXE_BIOS_START_g2, OPTYPE_PXE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_PXE},
-		{ FLASH_FCoE_BIOS_START_g2, OPTYPE_FCOE_BIOS,
-			FLASH_BIOS_IMAGE_MAX_SIZE_g2, IMAGE_OPTION_ROM_FCoE},
-		{ FLASH_iSCSI_BACKUP_IMAGE_START_g2, OPTYPE_ISCSI_BACKUP,
-			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_BACKUP_iSCSI},
-		{ FLASH_FCoE_PRIMARY_IMAGE_START_g2, OPTYPE_FCOE_FW_ACTIVE,
-			FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_FCoE},
-		{ FLASH_FCoE_BACKUP_IMAGE_START_g2, OPTYPE_FCOE_FW_BACKUP,
-			 FLASH_IMAGE_MAX_SIZE_g2, IMAGE_FIRMWARE_BACKUP_FCoE}
-	};
-
-	if (BE3_chip(adapter)) {
-		pflashcomp = gen3_flash_types;
-		filehdr_size = sizeof(struct flash_file_hdr_g3);
-		num_comp = ARRAY_SIZE(gen3_flash_types);
-	} else {
-		pflashcomp = gen2_flash_types;
-		filehdr_size = sizeof(struct flash_file_hdr_g2);
-		num_comp = ARRAY_SIZE(gen2_flash_types);
-		img_hdrs_size = 0;
-	}
-
-	/* Get flash section info*/
-	fsec = get_fsec_info(adapter, filehdr_size + img_hdrs_size, fw);
-	if (!fsec) {
-		dev_err(dev, "Invalid Cookie. FW image may be corrupted\n");
-		return -1;
-	}
-	for (i = 0; i < num_comp; i++) {
-		if (!is_comp_in_ufi(adapter, fsec, pflashcomp[i].img_type))
-			continue;
-
-		if ((pflashcomp[i].optype == OPTYPE_NCSI_FW) &&
-		    memcmp(adapter->fw_ver, "3.102.148.0", 11) < 0)
-			continue;
-
-		if (pflashcomp[i].optype == OPTYPE_PHY_FW  &&
-		    !phy_flashing_required(adapter))
-				continue;
-
-		if (pflashcomp[i].optype == OPTYPE_REDBOOT) {
-			status = be_check_flash_crc(adapter, fw->data,
-						    pflashcomp[i].offset,
-						    pflashcomp[i].size,
-						    filehdr_size +
-						    img_hdrs_size,
-						    OPTYPE_REDBOOT, &crc_match);
-			if (status) {
-				dev_err(dev,
-					"Could not get CRC for 0x%x region\n",
-					pflashcomp[i].optype);
-				continue;
-			}
-
-			if (crc_match)
-				continue;
-		}
-
-		p = fw->data + filehdr_size + pflashcomp[i].offset +
-			img_hdrs_size;
-		if (p + pflashcomp[i].size > fw->data + fw->size)
-			return -1;
-
-		status = be_flash(adapter, p, flash_cmd, pflashcomp[i].optype,
-				  pflashcomp[i].size, 0);
-		if (status) {
-			dev_err(dev, "Flashing section type 0x%x failed\n",
-				pflashcomp[i].img_type);
-			return status;
-		}
-	}
-	return 0;
-}
-
-static u16 be_get_img_optype(struct flash_section_entry fsec_entry)
-{
-	u32 img_type = le32_to_cpu(fsec_entry.type);
-	u16 img_optype = le16_to_cpu(fsec_entry.optype);
-
-	if (img_optype != 0xFFFF)
-		return img_optype;
-
-	switch (img_type) {
-	case IMAGE_FIRMWARE_iSCSI:
-		img_optype = OPTYPE_ISCSI_ACTIVE;
-		break;
-	case IMAGE_BOOT_CODE:
-		img_optype = OPTYPE_REDBOOT;
-		break;
-	case IMAGE_OPTION_ROM_ISCSI:
-		img_optype = OPTYPE_BIOS;
-		break;
-	case IMAGE_OPTION_ROM_PXE:
-		img_optype = OPTYPE_PXE_BIOS;
-		break;
-	case IMAGE_OPTION_ROM_FCoE:
-		img_optype = OPTYPE_FCOE_BIOS;
-		break;
-	case IMAGE_FIRMWARE_BACKUP_iSCSI:
-		img_optype = OPTYPE_ISCSI_BACKUP;
-		break;
-	case IMAGE_NCSI:
-		img_optype = OPTYPE_NCSI_FW;
-		break;
-	case IMAGE_FLASHISM_JUMPVECTOR:
-		img_optype = OPTYPE_FLASHISM_JUMPVECTOR;
-		break;
-	case IMAGE_FIRMWARE_PHY:
-		img_optype = OPTYPE_SH_PHY_FW;
-		break;
-	case IMAGE_REDBOOT_DIR:
-		img_optype = OPTYPE_REDBOOT_DIR;
-		break;
-	case IMAGE_REDBOOT_CONFIG:
-		img_optype = OPTYPE_REDBOOT_CONFIG;
-		break;
-	case IMAGE_UFI_DIR:
-		img_optype = OPTYPE_UFI_DIR;
-		break;
-	default:
-		break;
-	}
-
-	return img_optype;
-}
-
-static int be_flash_skyhawk(struct be_adapter *adapter,
-			    const struct firmware *fw,
-			    struct be_dma_mem *flash_cmd, int num_of_images)
-{
-	int img_hdrs_size = num_of_images * sizeof(struct image_hdr);
-	bool crc_match, old_fw_img, flash_offset_support = true;
-	struct device *dev = &adapter->pdev->dev;
-	struct flash_section_info *fsec = NULL;
-	u32 img_offset, img_size, img_type;
-	u16 img_optype, flash_optype;
-	int status, i, filehdr_size;
-	const u8 *p;
-
-	filehdr_size = sizeof(struct flash_file_hdr_g3);
-	fsec = get_fsec_info(adapter, filehdr_size + img_hdrs_size, fw);
-	if (!fsec) {
-		dev_err(dev, "Invalid Cookie. FW image may be corrupted\n");
-		return -EINVAL;
-	}
-
-retry_flash:
-	for (i = 0; i < le32_to_cpu(fsec->fsec_hdr.num_images); i++) {
-		img_offset = le32_to_cpu(fsec->fsec_entry[i].offset);
-		img_size   = le32_to_cpu(fsec->fsec_entry[i].pad_size);
-		img_type   = le32_to_cpu(fsec->fsec_entry[i].type);
-		img_optype = be_get_img_optype(fsec->fsec_entry[i]);
-		old_fw_img = fsec->fsec_entry[i].optype == 0xFFFF;
-
-		if (img_optype == 0xFFFF)
-			continue;
-
-		if (flash_offset_support)
-			flash_optype = OPTYPE_OFFSET_SPECIFIED;
-		else
-			flash_optype = img_optype;
-
-		/* Don't bother verifying CRC if an old FW image is being
-		 * flashed
-		 */
-		if (old_fw_img)
-			goto flash;
-
-		status = be_check_flash_crc(adapter, fw->data, img_offset,
-					    img_size, filehdr_size +
-					    img_hdrs_size, flash_optype,
-					    &crc_match);
-		if (base_status(status) == MCC_STATUS_ILLEGAL_REQUEST ||
-		    base_status(status) == MCC_STATUS_ILLEGAL_FIELD) {
-			/* The current FW image on the card does not support
-			 * OFFSET based flashing. Retry using older mechanism
-			 * of OPTYPE based flashing
-			 */
-			if (flash_optype == OPTYPE_OFFSET_SPECIFIED) {
-				flash_offset_support = false;
-				goto retry_flash;
-			}
-
-			/* The current FW image on the card does not recognize
-			 * the new FLASH op_type. The FW download is partially
-			 * complete. Reboot the server now to enable FW image
-			 * to recognize the new FLASH op_type. To complete the
-			 * remaining process, download the same FW again after
-			 * the reboot.
-			 */
-			dev_err(dev, "Flash incomplete. Reset the server\n");
-			dev_err(dev, "Download FW image again after reset\n");
-			return -EAGAIN;
-		} else if (status) {
-			dev_err(dev, "Could not get CRC for 0x%x region\n",
-				img_optype);
-			return -EFAULT;
-		}
-
-		if (crc_match)
-			continue;
-
-flash:
-		p = fw->data + filehdr_size + img_offset + img_hdrs_size;
-		if (p + img_size > fw->data + fw->size)
-			return -1;
-
-		status = be_flash(adapter, p, flash_cmd, flash_optype, img_size,
-				  img_offset);
-
-		/* The current FW image on the card does not support OFFSET
-		 * based flashing. Retry using older mechanism of OPTYPE based
-		 * flashing
-		 */
-		if (base_status(status) == MCC_STATUS_ILLEGAL_FIELD &&
-		    flash_optype == OPTYPE_OFFSET_SPECIFIED) {
-			flash_offset_support = false;
-			goto retry_flash;
-		}
-
-		/* For old FW images ignore ILLEGAL_FIELD error or errors on
-		 * UFI_DIR region
-		 */
-		if (old_fw_img &&
-		    (base_status(status) == MCC_STATUS_ILLEGAL_FIELD ||
-		     (img_optype == OPTYPE_UFI_DIR &&
-		      base_status(status) == MCC_STATUS_FAILED))) {
-			continue;
-		} else if (status) {
-			dev_err(dev, "Flashing section type 0x%x failed\n",
-				img_type);
-			return -EFAULT;
-		}
-	}
-	return 0;
-}
-
-static int lancer_fw_download(struct be_adapter *adapter,
-			      const struct firmware *fw)
-{
-#define LANCER_FW_DOWNLOAD_CHUNK      (32 * 1024)
-#define LANCER_FW_DOWNLOAD_LOCATION   "/prg"
-	struct device *dev = &adapter->pdev->dev;
-	struct be_dma_mem flash_cmd;
-	const u8 *data_ptr = NULL;
-	u8 *dest_image_ptr = NULL;
-	size_t image_size = 0;
-	u32 chunk_size = 0;
-	u32 data_written = 0;
-	u32 offset = 0;
-	int status = 0;
-	u8 add_status = 0;
-	u8 change_status;
-
-	if (!IS_ALIGNED(fw->size, sizeof(u32))) {
-		dev_err(dev, "FW image size should be multiple of 4\n");
-		return -EINVAL;
-	}
-
-	flash_cmd.size = sizeof(struct lancer_cmd_req_write_object)
-				+ LANCER_FW_DOWNLOAD_CHUNK;
-	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size,
-					   &flash_cmd.dma, GFP_KERNEL);
-	if (!flash_cmd.va)
-		return -ENOMEM;
-
-	dest_image_ptr = flash_cmd.va +
-				sizeof(struct lancer_cmd_req_write_object);
-	image_size = fw->size;
-	data_ptr = fw->data;
-
-	while (image_size) {
-		chunk_size = min_t(u32, image_size, LANCER_FW_DOWNLOAD_CHUNK);
-
-		/* Copy the image chunk content. */
-		memcpy(dest_image_ptr, data_ptr, chunk_size);
-
-		status = lancer_cmd_write_object(adapter, &flash_cmd,
-						 chunk_size, offset,
-						 LANCER_FW_DOWNLOAD_LOCATION,
-						 &data_written, &change_status,
-						 &add_status);
-		if (status)
-			break;
-
-		offset += data_written;
-		data_ptr += data_written;
-		image_size -= data_written;
-	}
-
-	if (!status) {
-		/* Commit the FW written */
-		status = lancer_cmd_write_object(adapter, &flash_cmd,
-						 0, offset,
-						 LANCER_FW_DOWNLOAD_LOCATION,
-						 &data_written, &change_status,
-						 &add_status);
-	}
-
-	dma_free_coherent(dev, flash_cmd.size, flash_cmd.va, flash_cmd.dma);
-	if (status) {
-		dev_err(dev, "Firmware load error\n");
-		return be_cmd_status(status);
-	}
-
-	dev_info(dev, "Firmware flashed successfully\n");
-
-	if (change_status == LANCER_FW_RESET_NEEDED) {
-		dev_info(dev, "Resetting adapter to activate new FW\n");
-		status = lancer_physdev_ctrl(adapter,
-					     PHYSDEV_CONTROL_FW_RESET_MASK);
-		if (status) {
-			dev_err(dev, "Adapter busy, could not reset FW\n");
-			dev_err(dev, "Reboot server to activate new FW\n");
-		}
-	} else if (change_status != LANCER_NO_RESET_NEEDED) {
-		dev_info(dev, "Reboot server to activate new FW\n");
-	}
-
-	return 0;
-}
-
-/* Check if the flash image file is compatible with the adapter that
- * is being flashed.
- */
-static bool be_check_ufi_compatibility(struct be_adapter *adapter,
-				       struct flash_file_hdr_g3 *fhdr)
-{
-	if (!fhdr) {
-		dev_err(&adapter->pdev->dev, "Invalid FW UFI file");
-		return false;
-	}
-
-	/* First letter of the build version is used to identify
-	 * which chip this image file is meant for.
-	 */
-	switch (fhdr->build[0]) {
-	case BLD_STR_UFI_TYPE_SH:
-		if (!skyhawk_chip(adapter))
-			return false;
-		break;
-	case BLD_STR_UFI_TYPE_BE3:
-		if (!BE3_chip(adapter))
-			return false;
-		break;
-	case BLD_STR_UFI_TYPE_BE2:
-		if (!BE2_chip(adapter))
-			return false;
-		break;
-	default:
-		return false;
-	}
-
-	/* In BE3 FW images the "asic_type_rev" field doesn't track the
-	 * asic_rev of the chips it is compatible with.
-	 * When asic_type_rev is 0 the image is compatible only with
-	 * pre-BE3-R chips (asic_rev < 0x10)
-	 */
-	if (BEx_chip(adapter) && fhdr->asic_type_rev == 0)
-		return adapter->asic_rev < 0x10;
-	else
-		return (fhdr->asic_type_rev >= adapter->asic_rev);
-}
-
-static int be_fw_download(struct be_adapter *adapter, const struct firmware* fw)
-{
-	struct device *dev = &adapter->pdev->dev;
-	struct flash_file_hdr_g3 *fhdr3;
-	struct image_hdr *img_hdr_ptr;
-	int status = 0, i, num_imgs;
-	struct be_dma_mem flash_cmd;
-
-	fhdr3 = (struct flash_file_hdr_g3 *)fw->data;
-	if (!be_check_ufi_compatibility(adapter, fhdr3)) {
-		dev_err(dev, "Flash image is not compatible with adapter\n");
-		return -EINVAL;
-	}
-
-	flash_cmd.size = sizeof(struct be_cmd_write_flashrom);
-	flash_cmd.va = dma_zalloc_coherent(dev, flash_cmd.size, &flash_cmd.dma,
-					   GFP_KERNEL);
-	if (!flash_cmd.va)
-		return -ENOMEM;
-
-	num_imgs = le32_to_cpu(fhdr3->num_imgs);
-	for (i = 0; i < num_imgs; i++) {
-		img_hdr_ptr = (struct image_hdr *)(fw->data +
-				(sizeof(struct flash_file_hdr_g3) +
-				 i * sizeof(struct image_hdr)));
-		if (!BE2_chip(adapter) &&
-		    le32_to_cpu(img_hdr_ptr->imageid) != 1)
-			continue;
-
-		if (skyhawk_chip(adapter))
-			status = be_flash_skyhawk(adapter, fw, &flash_cmd,
-						  num_imgs);
-		else
-			status = be_flash_BEx(adapter, fw, &flash_cmd,
-					      num_imgs);
-	}
-
-	dma_free_coherent(dev, flash_cmd.size, flash_cmd.va, flash_cmd.dma);
-	if (!status)
-		dev_info(dev, "Firmware flashed successfully\n");
-
-	return status;
-}
-
 int be_load_fw(struct be_adapter *adapter, u8 *fw_file)
 {
 	const struct firmware *fw;
@@ -5108,6 +4555,9 @@ static int be_ndo_bridge_setlink(struct net_device *dev, struct nlmsghdr *nlh,
 			return -EINVAL;
 
 		mode = nla_get_u16(attr);
+		if (BE3_chip(adapter) && mode == BRIDGE_MODE_VEPA)
+			return -EOPNOTSUPP;
+
 		if (mode != BRIDGE_MODE_VEPA && mode != BRIDGE_MODE_VEB)
 			return -EINVAL;
 
@@ -5289,7 +4739,7 @@ static netdev_features_t be_features_check(struct sk_buff *skb,
 	    skb->inner_protocol != htons(ETH_P_TEB) ||
 	    skb_inner_mac_header(skb) - skb_transport_header(skb) !=
 	    sizeof(struct udphdr) + sizeof(struct vxlanhdr))
-		return features & ~(NETIF_F_ALL_CSUM | NETIF_F_GSO_MASK);
+		return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
 
 	return features;
 }
@@ -5520,6 +4970,8 @@ static void be_unmap_pci_bars(struct be_adapter *adapter)
 		pci_iounmap(adapter->pdev, adapter->csr);
 	if (adapter->db)
 		pci_iounmap(adapter->pdev, adapter->db);
+	if (adapter->pcicfg && adapter->pcicfg_mapped)
+		pci_iounmap(adapter->pdev, adapter->pcicfg);
 }
 
 static int db_bar(struct be_adapter *adapter)
@@ -5571,8 +5023,10 @@ static int be_map_pci_bars(struct be_adapter *adapter)
 			if (!addr)
 				goto pci_map_err;
 			adapter->pcicfg = addr;
+			adapter->pcicfg_mapped = true;
 		} else {
 			adapter->pcicfg = adapter->db + SRIOV_VF_PCICFG_OFFSET;
+			adapter->pcicfg_mapped = false;
 		}
 	}
 
diff --git a/drivers/net/ethernet/ethoc.c b/drivers/net/ethernet/ethoc.c
index ff665493ca97..41b010645100 100644
--- a/drivers/net/ethernet/ethoc.c
+++ b/drivers/net/ethernet/ethoc.c
@@ -678,7 +678,7 @@ static int ethoc_mdio_probe(struct net_device *dev)
 	int err;
 
 	if (priv->phy_id != -1)
-		phy = priv->mdio->phy_map[priv->phy_id];
+		phy = mdiobus_get_phy(priv->mdio, priv->phy_id);
 	else
 		phy = phy_find_first(priv->mdio);
 
@@ -766,7 +766,7 @@ static int ethoc_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 		if (mdio->phy_id >= PHY_MAX_ADDR)
 			return -ERANGE;
 
-		phy = priv->mdio->phy_map[mdio->phy_id];
+		phy = mdiobus_get_phy(priv->mdio, mdio->phy_id);
 		if (!phy)
 			return -ENODEV;
 	} else {
@@ -1015,7 +1015,6 @@ static int ethoc_probe(struct platform_device *pdev)
 	struct resource *mmio = NULL;
 	struct resource *mem = NULL;
 	struct ethoc *priv = NULL;
-	unsigned int phy;
 	int num_bd;
 	int ret = 0;
 	bool random_mac = false;
@@ -1206,19 +1205,10 @@ static int ethoc_probe(struct platform_device *pdev)
 	priv->mdio->write = ethoc_mdio_write;
 	priv->mdio->priv = priv;
 
-	priv->mdio->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!priv->mdio->irq) {
-		ret = -ENOMEM;
-		goto free_mdio;
-	}
-
-	for (phy = 0; phy < PHY_MAX_ADDR; phy++)
-		priv->mdio->irq[phy] = PHY_POLL;
-
 	ret = mdiobus_register(priv->mdio);
 	if (ret) {
 		dev_err(&netdev->dev, "failed to register MDIO bus\n");
-		goto free_mdio;
+		goto free;
 	}
 
 	ret = ethoc_mdio_probe(netdev);
@@ -1250,8 +1240,6 @@ error2:
 	netif_napi_del(&priv->napi);
 error:
 	mdiobus_unregister(priv->mdio);
-free_mdio:
-	kfree(priv->mdio->irq);
 	mdiobus_free(priv->mdio);
 free:
 	if (priv->clk)
@@ -1277,7 +1265,6 @@ static int ethoc_remove(struct platform_device *pdev)
 
 		if (priv->mdio) {
 			mdiobus_unregister(priv->mdio);
-			kfree(priv->mdio->irq);
 			mdiobus_free(priv->mdio);
 		}
 		if (priv->clk)
diff --git a/drivers/net/ethernet/ezchip/Kconfig b/drivers/net/ethernet/ezchip/Kconfig
index 48ecbc8aaaea..b423ad380b6a 100644
--- a/drivers/net/ethernet/ezchip/Kconfig
+++ b/drivers/net/ethernet/ezchip/Kconfig
@@ -18,6 +18,7 @@ if NET_VENDOR_EZCHIP
 config EZCHIP_NPS_MANAGEMENT_ENET
 	tristate "EZchip NPS management enet support"
 	depends on OF_IRQ && OF_NET
+	depends on HAS_IOMEM
 	---help---
 	  Simple LAN device for debug or management purposes.
 	  Device supports interrupts for RX and TX(completion).
diff --git a/drivers/net/ethernet/faraday/ftgmac100.c b/drivers/net/ethernet/faraday/ftgmac100.c
index 6d0c5d5eea6d..84384e1585a5 100644
--- a/drivers/net/ethernet/faraday/ftgmac100.c
+++ b/drivers/net/ethernet/faraday/ftgmac100.c
@@ -71,7 +71,6 @@ struct ftgmac100 {
 	struct napi_struct napi;
 
 	struct mii_bus *mii_bus;
-	int phy_irq[PHY_MAX_ADDR];
 	struct phy_device *phydev;
 	int old_speed;
 };
@@ -835,26 +834,15 @@ static void ftgmac100_adjust_link(struct net_device *netdev)
 static int ftgmac100_mii_probe(struct ftgmac100 *priv)
 {
 	struct net_device *netdev = priv->netdev;
-	struct phy_device *phydev = NULL;
-	int i;
-
-	/* search for connect PHY device */
-	for (i = 0; i < PHY_MAX_ADDR; i++) {
-		struct phy_device *tmp = priv->mii_bus->phy_map[i];
-
-		if (tmp) {
-			phydev = tmp;
-			break;
-		}
-	}
+	struct phy_device *phydev;
 
-	/* now we are supposed to have a proper phydev, to attach to... */
+	phydev = phy_find_first(priv->mii_bus);
 	if (!phydev) {
 		netdev_info(netdev, "%s: no PHY found\n", netdev->name);
 		return -ENODEV;
 	}
 
-	phydev = phy_connect(netdev, dev_name(&phydev->dev),
+	phydev = phy_connect(netdev, phydev_name(phydev),
 			     &ftgmac100_adjust_link, PHY_INTERFACE_MODE_GMII);
 
 	if (IS_ERR(phydev)) {
@@ -1188,7 +1176,6 @@ static int ftgmac100_probe(struct platform_device *pdev)
 	struct net_device *netdev;
 	struct ftgmac100 *priv;
 	int err;
-	int i;
 
 	if (!pdev)
 		return -ENODEV;
@@ -1257,10 +1244,6 @@ static int ftgmac100_probe(struct platform_device *pdev)
 	priv->mii_bus->priv = netdev;
 	priv->mii_bus->read = ftgmac100_mdiobus_read;
 	priv->mii_bus->write = ftgmac100_mdiobus_write;
-	priv->mii_bus->irq = priv->phy_irq;
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		priv->mii_bus->irq[i] = PHY_POLL;
 
 	err = mdiobus_register(priv->mii_bus);
 	if (err) {
diff --git a/drivers/net/ethernet/freescale/Kconfig b/drivers/net/ethernet/freescale/Kconfig
index bee32a9d9876..d1ca45fbb164 100644
--- a/drivers/net/ethernet/freescale/Kconfig
+++ b/drivers/net/ethernet/freescale/Kconfig
@@ -54,6 +54,7 @@ config FEC_MPC52xx_MDIO
 	  If compiled as module, it will be called fec_mpc52xx_phy.
 
 source "drivers/net/ethernet/freescale/fs_enet/Kconfig"
+source "drivers/net/ethernet/freescale/fman/Kconfig"
 
 config FSL_PQ_MDIO
 	tristate "Freescale PQ MDIO"
diff --git a/drivers/net/ethernet/freescale/Makefile b/drivers/net/ethernet/freescale/Makefile
index 71debd1c18c9..cbe21dc7e37e 100644
--- a/drivers/net/ethernet/freescale/Makefile
+++ b/drivers/net/ethernet/freescale/Makefile
@@ -4,6 +4,9 @@
 
 obj-$(CONFIG_FEC) += fec.o
 fec-objs :=fec_main.o fec_ptp.o
+CFLAGS_fec_main.o := -D__CHECK_ENDIAN__
+CFLAGS_fec_ptp.o := -D__CHECK_ENDIAN__
+
 obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx.o
 ifeq ($(CONFIG_FEC_MPC52xx_MDIO),y)
 	obj-$(CONFIG_FEC_MPC52xx) += fec_mpc52xx_phy.o
@@ -17,3 +20,5 @@ gianfar_driver-objs := gianfar.o \
 		gianfar_ethtool.o
 obj-$(CONFIG_UCC_GETH) += ucc_geth_driver.o
 ucc_geth_driver-objs := ucc_geth.o ucc_geth_ethtool.o
+
+obj-$(CONFIG_FSL_FMAN) += fman/
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 99d33e2d35e6..2106d72c91dc 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -19,8 +19,7 @@
 #include <linux/timecounter.h>
 
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-    defined(CONFIG_M520x) || defined(CONFIG_M532x) || \
-    defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
+    defined(CONFIG_M520x) || defined(CONFIG_M532x) || defined(CONFIG_ARM)
 /*
  *	Just figures, Motorola would have to change the offsets for
  *	registers in the same peripheral device on different models
@@ -190,28 +189,45 @@
 
 /*
  *	Define the buffer descriptor structure.
+ *
+ *	Evidently, ARM SoCs have the FEC block generated in a
+ *	little endian mode so adjust endianness accordingly.
  */
-#if defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
+#if defined(CONFIG_ARM)
+#define fec32_to_cpu le32_to_cpu
+#define fec16_to_cpu le16_to_cpu
+#define cpu_to_fec32 cpu_to_le32
+#define cpu_to_fec16 cpu_to_le16
+#define __fec32 __le32
+#define __fec16 __le16
+
 struct bufdesc {
-	unsigned short cbd_datlen;	/* Data length */
-	unsigned short cbd_sc;	/* Control and status info */
-	unsigned long cbd_bufaddr;	/* Buffer address */
+	__fec16 cbd_datlen;	/* Data length */
+	__fec16 cbd_sc;		/* Control and status info */
+	__fec32 cbd_bufaddr;	/* Buffer address */
 };
 #else
+#define fec32_to_cpu be32_to_cpu
+#define fec16_to_cpu be16_to_cpu
+#define cpu_to_fec32 cpu_to_be32
+#define cpu_to_fec16 cpu_to_be16
+#define __fec32 __be32
+#define __fec16 __be16
+
 struct bufdesc {
-	unsigned short	cbd_sc;			/* Control and status info */
-	unsigned short	cbd_datlen;		/* Data length */
-	unsigned long	cbd_bufaddr;		/* Buffer address */
+	__fec16	cbd_sc;		/* Control and status info */
+	__fec16	cbd_datlen;	/* Data length */
+	__fec32	cbd_bufaddr;	/* Buffer address */
 };
 #endif
 
 struct bufdesc_ex {
 	struct bufdesc desc;
-	unsigned long cbd_esc;
-	unsigned long cbd_prot;
-	unsigned long cbd_bdu;
-	unsigned long ts;
-	unsigned short res0[4];
+	__fec32 cbd_esc;
+	__fec32 cbd_prot;
+	__fec32 cbd_bdu;
+	__fec32 ts;
+	__fec16 res0[4];
 };
 
 /*
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index b2a32209ffbf..41c81f6ec630 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -48,6 +48,7 @@
 #include <linux/irq.h>
 #include <linux/clk.h>
 #include <linux/platform_device.h>
+#include <linux/mdio.h>
 #include <linux/phy.h>
 #include <linux/fec.h>
 #include <linux/of.h>
@@ -331,11 +332,13 @@ static void fec_dump(struct net_device *ndev)
 	bdp = txq->tx_bd_base;
 
 	do {
-		pr_info("%3u %c%c 0x%04x 0x%08lx %4u %p\n",
+		pr_info("%3u %c%c 0x%04x 0x%08x %4u %p\n",
 			index,
 			bdp == txq->cur_tx ? 'S' : ' ',
 			bdp == txq->dirty_tx ? 'H' : ' ',
-			bdp->cbd_sc, bdp->cbd_bufaddr, bdp->cbd_datlen,
+			fec16_to_cpu(bdp->cbd_sc),
+			fec32_to_cpu(bdp->cbd_bufaddr),
+			fec16_to_cpu(bdp->cbd_datlen),
 			txq->tx_skbuff[index]);
 		bdp = fec_enet_get_nextdesc(bdp, fep, 0);
 		index++;
@@ -388,7 +391,7 @@ fec_enet_txq_submit_frag_skb(struct fec_enet_priv_tx_q *txq,
 		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 		ebdp = (struct bufdesc_ex *)bdp;
 
-		status = bdp->cbd_sc;
+		status = fec16_to_cpu(bdp->cbd_sc);
 		status &= ~BD_ENET_TX_STATS;
 		status |= (BD_ENET_TX_TC | BD_ENET_TX_READY);
 		frag_len = skb_shinfo(skb)->frags[frag].size;
@@ -410,7 +413,7 @@ fec_enet_txq_submit_frag_skb(struct fec_enet_priv_tx_q *txq,
 			if (skb->ip_summed == CHECKSUM_PARTIAL)
 				estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 			ebdp->cbd_bdu = 0;
-			ebdp->cbd_esc = estatus;
+			ebdp->cbd_esc = cpu_to_fec32(estatus);
 		}
 
 		bufaddr = page_address(this_frag->page.p) + this_frag->page_offset;
@@ -434,9 +437,9 @@ fec_enet_txq_submit_frag_skb(struct fec_enet_priv_tx_q *txq,
 			goto dma_mapping_error;
 		}
 
-		bdp->cbd_bufaddr = addr;
-		bdp->cbd_datlen = frag_len;
-		bdp->cbd_sc = status;
+		bdp->cbd_bufaddr = cpu_to_fec32(addr);
+		bdp->cbd_datlen = cpu_to_fec16(frag_len);
+		bdp->cbd_sc = cpu_to_fec16(status);
 	}
 
 	return bdp;
@@ -444,8 +447,8 @@ dma_mapping_error:
 	bdp = txq->cur_tx;
 	for (i = 0; i < frag; i++) {
 		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
-		dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
-				bdp->cbd_datlen, DMA_TO_DEVICE);
+		dma_unmap_single(&fep->pdev->dev, fec32_to_cpu(bdp->cbd_bufaddr),
+				 fec16_to_cpu(bdp->cbd_datlen), DMA_TO_DEVICE);
 	}
 	return ERR_PTR(-ENOMEM);
 }
@@ -482,7 +485,7 @@ static int fec_enet_txq_submit_skb(struct fec_enet_priv_tx_q *txq,
 	/* Fill in a Tx ring entry */
 	bdp = txq->cur_tx;
 	last_bdp = bdp;
-	status = bdp->cbd_sc;
+	status = fec16_to_cpu(bdp->cbd_sc);
 	status &= ~BD_ENET_TX_STATS;
 
 	/* Set buffer length and buffer pointer */
@@ -538,21 +541,21 @@ static int fec_enet_txq_submit_skb(struct fec_enet_priv_tx_q *txq,
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 
 		ebdp->cbd_bdu = 0;
-		ebdp->cbd_esc = estatus;
+		ebdp->cbd_esc = cpu_to_fec32(estatus);
 	}
 
 	index = fec_enet_get_bd_index(txq->tx_bd_base, last_bdp, fep);
 	/* Save skb pointer */
 	txq->tx_skbuff[index] = skb;
 
-	bdp->cbd_datlen = buflen;
-	bdp->cbd_bufaddr = addr;
+	bdp->cbd_datlen = cpu_to_fec16(buflen);
+	bdp->cbd_bufaddr = cpu_to_fec32(addr);
 
 	/* Send it on its way.  Tell FEC it's ready, interrupt when done,
 	 * it's the last BD of the frame, and to put the CRC on the end.
 	 */
 	status |= (BD_ENET_TX_READY | BD_ENET_TX_TC);
-	bdp->cbd_sc = status;
+	bdp->cbd_sc = cpu_to_fec16(status);
 
 	/* If this was the last BD in the ring, start at the beginning again. */
 	bdp = fec_enet_get_nextdesc(last_bdp, fep, queue);
@@ -584,7 +587,7 @@ fec_enet_txq_put_data_tso(struct fec_enet_priv_tx_q *txq, struct sk_buff *skb,
 	unsigned int estatus = 0;
 	dma_addr_t addr;
 
-	status = bdp->cbd_sc;
+	status = fec16_to_cpu(bdp->cbd_sc);
 	status &= ~BD_ENET_TX_STATS;
 
 	status |= (BD_ENET_TX_TC | BD_ENET_TX_READY);
@@ -606,8 +609,8 @@ fec_enet_txq_put_data_tso(struct fec_enet_priv_tx_q *txq, struct sk_buff *skb,
 		return NETDEV_TX_BUSY;
 	}
 
-	bdp->cbd_datlen = size;
-	bdp->cbd_bufaddr = addr;
+	bdp->cbd_datlen = cpu_to_fec16(size);
+	bdp->cbd_bufaddr = cpu_to_fec32(addr);
 
 	if (fep->bufdesc_ex) {
 		if (fep->quirks & FEC_QUIRK_HAS_AVB)
@@ -615,7 +618,7 @@ fec_enet_txq_put_data_tso(struct fec_enet_priv_tx_q *txq, struct sk_buff *skb,
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 		ebdp->cbd_bdu = 0;
-		ebdp->cbd_esc = estatus;
+		ebdp->cbd_esc = cpu_to_fec32(estatus);
 	}
 
 	/* Handle the last BD specially */
@@ -624,10 +627,10 @@ fec_enet_txq_put_data_tso(struct fec_enet_priv_tx_q *txq, struct sk_buff *skb,
 	if (is_last) {
 		status |= BD_ENET_TX_INTR;
 		if (fep->bufdesc_ex)
-			ebdp->cbd_esc |= BD_ENET_TX_INT;
+			ebdp->cbd_esc |= cpu_to_fec32(BD_ENET_TX_INT);
 	}
 
-	bdp->cbd_sc = status;
+	bdp->cbd_sc = cpu_to_fec16(status);
 
 	return 0;
 }
@@ -646,7 +649,7 @@ fec_enet_txq_put_hdr_tso(struct fec_enet_priv_tx_q *txq,
 	unsigned short status;
 	unsigned int estatus = 0;
 
-	status = bdp->cbd_sc;
+	status = fec16_to_cpu(bdp->cbd_sc);
 	status &= ~BD_ENET_TX_STATS;
 	status |= (BD_ENET_TX_TC | BD_ENET_TX_READY);
 
@@ -670,8 +673,8 @@ fec_enet_txq_put_hdr_tso(struct fec_enet_priv_tx_q *txq,
 		}
 	}
 
-	bdp->cbd_bufaddr = dmabuf;
-	bdp->cbd_datlen = hdr_len;
+	bdp->cbd_bufaddr = cpu_to_fec32(dmabuf);
+	bdp->cbd_datlen = cpu_to_fec16(hdr_len);
 
 	if (fep->bufdesc_ex) {
 		if (fep->quirks & FEC_QUIRK_HAS_AVB)
@@ -679,10 +682,10 @@ fec_enet_txq_put_hdr_tso(struct fec_enet_priv_tx_q *txq,
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 		ebdp->cbd_bdu = 0;
-		ebdp->cbd_esc = estatus;
+		ebdp->cbd_esc = cpu_to_fec32(estatus);
 	}
 
-	bdp->cbd_sc = status;
+	bdp->cbd_sc = cpu_to_fec16(status);
 
 	return 0;
 }
@@ -822,15 +825,15 @@ static void fec_enet_bd_init(struct net_device *dev)
 
 			/* Initialize the BD for every fragment in the page. */
 			if (bdp->cbd_bufaddr)
-				bdp->cbd_sc = BD_ENET_RX_EMPTY;
+				bdp->cbd_sc = cpu_to_fec16(BD_ENET_RX_EMPTY);
 			else
-				bdp->cbd_sc = 0;
+				bdp->cbd_sc = cpu_to_fec16(0);
 			bdp = fec_enet_get_nextdesc(bdp, fep, q);
 		}
 
 		/* Set the last buffer to wrap */
 		bdp = fec_enet_get_prevdesc(bdp, fep, q);
-		bdp->cbd_sc |= BD_SC_WRAP;
+		bdp->cbd_sc |= cpu_to_fec16(BD_SC_WRAP);
 
 		rxq->cur_rx = rxq->rx_bd_base;
 	}
@@ -843,18 +846,18 @@ static void fec_enet_bd_init(struct net_device *dev)
 
 		for (i = 0; i < txq->tx_ring_size; i++) {
 			/* Initialize the BD for every fragment in the page. */
-			bdp->cbd_sc = 0;
+			bdp->cbd_sc = cpu_to_fec16(0);
 			if (txq->tx_skbuff[i]) {
 				dev_kfree_skb_any(txq->tx_skbuff[i]);
 				txq->tx_skbuff[i] = NULL;
 			}
-			bdp->cbd_bufaddr = 0;
+			bdp->cbd_bufaddr = cpu_to_fec32(0);
 			bdp = fec_enet_get_nextdesc(bdp, fep, q);
 		}
 
 		/* Set the last buffer to wrap */
 		bdp = fec_enet_get_prevdesc(bdp, fep, q);
-		bdp->cbd_sc |= BD_SC_WRAP;
+		bdp->cbd_sc |= cpu_to_fec16(BD_SC_WRAP);
 		txq->dirty_tx = bdp;
 	}
 }
@@ -946,8 +949,10 @@ fec_restart(struct net_device *ndev)
 	 */
 	if (fep->quirks & FEC_QUIRK_ENET_MAC) {
 		memcpy(&temp_mac, ndev->dev_addr, ETH_ALEN);
-		writel(cpu_to_be32(temp_mac[0]), fep->hwp + FEC_ADDR_LOW);
-		writel(cpu_to_be32(temp_mac[1]), fep->hwp + FEC_ADDR_HIGH);
+		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. */
@@ -1221,7 +1226,7 @@ fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 	while (bdp != READ_ONCE(txq->cur_tx)) {
 		/* Order the load of cur_tx and cbd_sc */
 		rmb();
-		status = READ_ONCE(bdp->cbd_sc);
+		status = fec16_to_cpu(READ_ONCE(bdp->cbd_sc));
 		if (status & BD_ENET_TX_READY)
 			break;
 
@@ -1229,10 +1234,12 @@ fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 
 		skb = txq->tx_skbuff[index];
 		txq->tx_skbuff[index] = NULL;
-		if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
-			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
-					bdp->cbd_datlen, DMA_TO_DEVICE);
-		bdp->cbd_bufaddr = 0;
+		if (!IS_TSO_HEADER(txq, fec32_to_cpu(bdp->cbd_bufaddr)))
+			dma_unmap_single(&fep->pdev->dev,
+					 fec32_to_cpu(bdp->cbd_bufaddr),
+					 fec16_to_cpu(bdp->cbd_datlen),
+					 DMA_TO_DEVICE);
+		bdp->cbd_bufaddr = cpu_to_fec32(0);
 		if (!skb) {
 			bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
 			continue;
@@ -1263,7 +1270,7 @@ fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 			struct skb_shared_hwtstamps shhwtstamps;
 			struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
 
-			fec_enet_hwtstamp(fep, ebdp->ts, &shhwtstamps);
+			fec_enet_hwtstamp(fep, fec32_to_cpu(ebdp->ts), &shhwtstamps);
 			skb_tstamp_tx(skb, &shhwtstamps);
 		}
 
@@ -1323,10 +1330,8 @@ fec_enet_new_rxbdp(struct net_device *ndev, struct bufdesc *bdp, struct sk_buff
 	if (off)
 		skb_reserve(skb, fep->rx_align + 1 - off);
 
-	bdp->cbd_bufaddr = dma_map_single(&fep->pdev->dev, skb->data,
-					  FEC_ENET_RX_FRSIZE - fep->rx_align,
-					  DMA_FROM_DEVICE);
-	if (dma_mapping_error(&fep->pdev->dev, bdp->cbd_bufaddr)) {
+	bdp->cbd_bufaddr = cpu_to_fec32(dma_map_single(&fep->pdev->dev, skb->data, FEC_ENET_RX_FRSIZE - fep->rx_align, DMA_FROM_DEVICE));
+	if (dma_mapping_error(&fep->pdev->dev, fec32_to_cpu(bdp->cbd_bufaddr))) {
 		if (net_ratelimit())
 			netdev_err(ndev, "Rx DMA memory map failed\n");
 		return -ENOMEM;
@@ -1348,7 +1353,8 @@ static bool fec_enet_copybreak(struct net_device *ndev, struct sk_buff **skb,
 	if (!new_skb)
 		return false;
 
-	dma_sync_single_for_cpu(&fep->pdev->dev, bdp->cbd_bufaddr,
+	dma_sync_single_for_cpu(&fep->pdev->dev,
+				fec32_to_cpu(bdp->cbd_bufaddr),
 				FEC_ENET_RX_FRSIZE - fep->rx_align,
 				DMA_FROM_DEVICE);
 	if (!swap)
@@ -1395,7 +1401,7 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 	 */
 	bdp = rxq->cur_rx;
 
-	while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) {
+	while (!((status = fec16_to_cpu(bdp->cbd_sc)) & BD_ENET_RX_EMPTY)) {
 
 		if (pkt_received >= budget)
 			break;
@@ -1437,7 +1443,7 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 
 		/* Process the incoming frame. */
 		ndev->stats.rx_packets++;
-		pkt_len = bdp->cbd_datlen;
+		pkt_len = fec16_to_cpu(bdp->cbd_datlen);
 		ndev->stats.rx_bytes += pkt_len;
 
 		index = fec_enet_get_bd_index(rxq->rx_bd_base, bdp, fep);
@@ -1455,7 +1461,8 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 				ndev->stats.rx_dropped++;
 				goto rx_processing_done;
 			}
-			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+			dma_unmap_single(&fep->pdev->dev,
+					 fec32_to_cpu(bdp->cbd_bufaddr),
 					 FEC_ENET_RX_FRSIZE - fep->rx_align,
 					 DMA_FROM_DEVICE);
 		}
@@ -1474,7 +1481,8 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 		/* If this is a VLAN packet remove the VLAN Tag */
 		vlan_packet_rcvd = false;
 		if ((ndev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
-			fep->bufdesc_ex && (ebdp->cbd_esc & BD_ENET_RX_VLAN)) {
+		    fep->bufdesc_ex &&
+		    (ebdp->cbd_esc & cpu_to_fec32(BD_ENET_RX_VLAN))) {
 			/* Push and remove the vlan tag */
 			struct vlan_hdr *vlan_header =
 					(struct vlan_hdr *) (data + ETH_HLEN);
@@ -1490,12 +1498,12 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 
 		/* Get receive timestamp from the skb */
 		if (fep->hwts_rx_en && fep->bufdesc_ex)
-			fec_enet_hwtstamp(fep, ebdp->ts,
+			fec_enet_hwtstamp(fep, fec32_to_cpu(ebdp->ts),
 					  skb_hwtstamps(skb));
 
 		if (fep->bufdesc_ex &&
 		    (fep->csum_flags & FLAG_RX_CSUM_ENABLED)) {
-			if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) {
+			if (!(ebdp->cbd_esc & cpu_to_fec32(FLAG_RX_CSUM_ERROR))) {
 				/* don't check it */
 				skb->ip_summed = CHECKSUM_UNNECESSARY;
 			} else {
@@ -1512,7 +1520,8 @@ fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 		napi_gro_receive(&fep->napi, skb);
 
 		if (is_copybreak) {
-			dma_sync_single_for_device(&fep->pdev->dev, bdp->cbd_bufaddr,
+			dma_sync_single_for_device(&fep->pdev->dev,
+						   fec32_to_cpu(bdp->cbd_bufaddr),
 						   FEC_ENET_RX_FRSIZE - fep->rx_align,
 						   DMA_FROM_DEVICE);
 		} else {
@@ -1526,12 +1535,12 @@ rx_processing_done:
 
 		/* Mark the buffer empty */
 		status |= BD_ENET_RX_EMPTY;
-		bdp->cbd_sc = status;
+		bdp->cbd_sc = cpu_to_fec16(status);
 
 		if (fep->bufdesc_ex) {
 			struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
 
-			ebdp->cbd_esc = BD_ENET_RX_INT;
+			ebdp->cbd_esc = cpu_to_fec32(BD_ENET_RX_INT);
 			ebdp->cbd_prot = 0;
 			ebdp->cbd_bdu = 0;
 		}
@@ -1926,11 +1935,7 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	} else {
 		/* check for attached phy */
 		for (phy_id = 0; (phy_id < PHY_MAX_ADDR); phy_id++) {
-			if ((fep->mii_bus->phy_mask & (1 << phy_id)))
-				continue;
-			if (fep->mii_bus->phy_map[phy_id] == NULL)
-				continue;
-			if (fep->mii_bus->phy_map[phy_id]->phy_id == 0)
+			if (!mdiobus_is_registered_device(fep->mii_bus, phy_id))
 				continue;
 			if (dev_id--)
 				continue;
@@ -1972,9 +1977,7 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 	fep->link = 0;
 	fep->full_duplex = 0;
 
-	netdev_info(ndev, "Freescale FEC PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		    fep->phy_dev->drv->name, dev_name(&fep->phy_dev->dev),
-		    fep->phy_dev->irq);
+	phy_attached_info(phy_dev);
 
 	return 0;
 }
@@ -1985,7 +1988,7 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	struct device_node *node;
-	int err = -ENXIO, i;
+	int err = -ENXIO;
 	u32 mii_speed, holdtime;
 
 	/*
@@ -2067,15 +2070,6 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	fep->mii_bus->priv = fep;
 	fep->mii_bus->parent = &pdev->dev;
 
-	fep->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!fep->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		fep->mii_bus->irq[i] = PHY_POLL;
-
 	node = of_get_child_by_name(pdev->dev.of_node, "mdio");
 	if (node) {
 		err = of_mdiobus_register(fep->mii_bus, node);
@@ -2085,7 +2079,7 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 	}
 
 	if (err)
-		goto err_out_free_mdio_irq;
+		goto err_out_free_mdiobus;
 
 	mii_cnt++;
 
@@ -2095,8 +2089,6 @@ static int fec_enet_mii_init(struct platform_device *pdev)
 
 	return 0;
 
-err_out_free_mdio_irq:
-	kfree(fep->mii_bus->irq);
 err_out_free_mdiobus:
 	mdiobus_free(fep->mii_bus);
 err_out:
@@ -2107,7 +2099,6 @@ static void fec_enet_mii_remove(struct fec_enet_private *fep)
 {
 	if (--mii_cnt == 0) {
 		mdiobus_unregister(fep->mii_bus);
-		kfree(fep->mii_bus->irq);
 		mdiobus_free(fep->mii_bus);
 	}
 }
@@ -2162,8 +2153,7 @@ static int fec_enet_get_regs_len(struct net_device *ndev)
 
 /* List of registers that can be safety be read to dump them with ethtool */
 #if defined(CONFIG_M523x) || defined(CONFIG_M527x) || defined(CONFIG_M528x) || \
-	defined(CONFIG_M520x) || defined(CONFIG_M532x) ||		\
-	defined(CONFIG_ARCH_MXC) || defined(CONFIG_SOC_IMX28)
+	defined(CONFIG_M520x) || defined(CONFIG_M532x) || defined(CONFIG_ARM)
 static u32 fec_enet_register_offset[] = {
 	FEC_IEVENT, FEC_IMASK, FEC_R_DES_ACTIVE_0, FEC_X_DES_ACTIVE_0,
 	FEC_ECNTRL, FEC_MII_DATA, FEC_MII_SPEED, FEC_MIB_CTRLSTAT, FEC_R_CNTRL,
@@ -2679,7 +2669,7 @@ static void fec_enet_free_buffers(struct net_device *ndev)
 			rxq->rx_skbuff[i] = NULL;
 			if (skb) {
 				dma_unmap_single(&fep->pdev->dev,
-						 bdp->cbd_bufaddr,
+						 fec32_to_cpu(bdp->cbd_bufaddr),
 						 FEC_ENET_RX_FRSIZE - fep->rx_align,
 						 DMA_FROM_DEVICE);
 				dev_kfree_skb(skb);
@@ -2794,11 +2784,11 @@ fec_enet_alloc_rxq_buffers(struct net_device *ndev, unsigned int queue)
 		}
 
 		rxq->rx_skbuff[i] = skb;
-		bdp->cbd_sc = BD_ENET_RX_EMPTY;
+		bdp->cbd_sc = cpu_to_fec16(BD_ENET_RX_EMPTY);
 
 		if (fep->bufdesc_ex) {
 			struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
-			ebdp->cbd_esc = BD_ENET_RX_INT;
+			ebdp->cbd_esc = cpu_to_fec32(BD_ENET_RX_INT);
 		}
 
 		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
@@ -2806,7 +2796,7 @@ fec_enet_alloc_rxq_buffers(struct net_device *ndev, unsigned int queue)
 
 	/* Set the last buffer to wrap. */
 	bdp = fec_enet_get_prevdesc(bdp, fep, queue);
-	bdp->cbd_sc |= BD_SC_WRAP;
+	bdp->cbd_sc |= cpu_to_fec16(BD_SC_WRAP);
 	return 0;
 
  err_alloc:
@@ -2829,12 +2819,12 @@ fec_enet_alloc_txq_buffers(struct net_device *ndev, unsigned int queue)
 		if (!txq->tx_bounce[i])
 			goto err_alloc;
 
-		bdp->cbd_sc = 0;
-		bdp->cbd_bufaddr = 0;
+		bdp->cbd_sc = cpu_to_fec16(0);
+		bdp->cbd_bufaddr = cpu_to_fec32(0);
 
 		if (fep->bufdesc_ex) {
 			struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
-			ebdp->cbd_esc = BD_ENET_TX_INT;
+			ebdp->cbd_esc = cpu_to_fec32(BD_ENET_TX_INT);
 		}
 
 		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
@@ -2842,7 +2832,7 @@ fec_enet_alloc_txq_buffers(struct net_device *ndev, unsigned int queue)
 
 	/* Set the last buffer to wrap. */
 	bdp = fec_enet_get_prevdesc(bdp, fep, queue);
-	bdp->cbd_sc |= BD_SC_WRAP;
+	bdp->cbd_sc |= cpu_to_fec16(BD_SC_WRAP);
 
 	return 0;
 
@@ -3277,7 +3267,6 @@ static void
 fec_enet_get_queue_num(struct platform_device *pdev, int *num_tx, int *num_rx)
 {
 	struct device_node *np = pdev->dev.of_node;
-	int err;
 
 	*num_tx = *num_rx = 1;
 
@@ -3285,13 +3274,9 @@ fec_enet_get_queue_num(struct platform_device *pdev, int *num_tx, int *num_rx)
 		return;
 
 	/* parse the num of tx and rx queues */
-	err = of_property_read_u32(np, "fsl,num-tx-queues", num_tx);
-	if (err)
-		*num_tx = 1;
+	of_property_read_u32(np, "fsl,num-tx-queues", num_tx);
 
-	err = of_property_read_u32(np, "fsl,num-rx-queues", num_rx);
-	if (err)
-		*num_rx = 1;
+	of_property_read_u32(np, "fsl,num-rx-queues", num_rx);
 
 	if (*num_tx < 1 || *num_tx > FEC_ENET_MAX_TX_QS) {
 		dev_warn(&pdev->dev, "Invalid num_tx(=%d), fall back to 1\n",
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx.c b/drivers/net/ethernet/freescale/fec_mpc52xx.c
index afe7f39cdd7c..25553ee857b4 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx.c
@@ -1084,27 +1084,23 @@ static struct platform_driver mpc52xx_fec_driver = {
 /* Module                                                                   */
 /* ======================================================================== */
 
+static struct platform_driver * const drivers[] = {
+#ifdef CONFIG_FEC_MPC52xx_MDIO
+	&mpc52xx_fec_mdio_driver,
+#endif
+	&mpc52xx_fec_driver,
+};
+
 static int __init
 mpc52xx_fec_init(void)
 {
-#ifdef CONFIG_FEC_MPC52xx_MDIO
-	int ret;
-	ret = platform_driver_register(&mpc52xx_fec_mdio_driver);
-	if (ret) {
-		pr_err("failed to register mdio driver\n");
-		return ret;
-	}
-#endif
-	return platform_driver_register(&mpc52xx_fec_driver);
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 static void __exit
 mpc52xx_fec_exit(void)
 {
-	platform_driver_unregister(&mpc52xx_fec_driver);
-#ifdef CONFIG_FEC_MPC52xx_MDIO
-	platform_driver_unregister(&mpc52xx_fec_mdio_driver);
-#endif
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 
diff --git a/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c b/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
index 1e647beaf989..b5497e308302 100644
--- a/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
+++ b/drivers/net/ethernet/freescale/fec_mpc52xx_phy.c
@@ -22,7 +22,6 @@
 
 struct mpc52xx_fec_mdio_priv {
 	struct mpc52xx_fec __iomem *regs;
-	int mdio_irqs[PHY_MAX_ADDR];
 };
 
 static int mpc52xx_fec_mdio_transfer(struct mii_bus *bus, int phy_id,
@@ -83,9 +82,6 @@ static int mpc52xx_fec_mdio_probe(struct platform_device *of)
 	bus->read = mpc52xx_fec_mdio_read;
 	bus->write = mpc52xx_fec_mdio_write;
 
-	/* setup irqs */
-	bus->irq = priv->mdio_irqs;
-
 	/* setup registers */
 	err = of_address_to_resource(np, 0, &res);
 	if (err)
diff --git a/drivers/net/ethernet/freescale/fman/Kconfig b/drivers/net/ethernet/freescale/fman/Kconfig
new file mode 100644
index 000000000000..79b7c84b7869
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/Kconfig
@@ -0,0 +1,9 @@
+config FSL_FMAN
+	tristate "FMan support"
+	depends on FSL_SOC || COMPILE_TEST
+	select GENERIC_ALLOCATOR
+	select PHYLIB
+	default n
+	help
+		Freescale Data-Path Acceleration Architecture Frame Manager
+		(FMan) support
diff --git a/drivers/net/ethernet/freescale/fman/Makefile b/drivers/net/ethernet/freescale/fman/Makefile
new file mode 100644
index 000000000000..51fd2e6c1b84
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/Makefile
@@ -0,0 +1,7 @@
+subdir-ccflags-y +=  -I$(srctree)/drivers/net/ethernet/freescale/fman
+
+obj-y		+= fsl_fman.o fsl_fman_mac.o 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
diff --git a/drivers/net/ethernet/freescale/fman/fman.c b/drivers/net/ethernet/freescale/fman/fman.c
new file mode 100644
index 000000000000..79a210aaf0bb
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman.c
@@ -0,0 +1,2871 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "fman.h"
+#include "fman_muram.h"
+
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/clk.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/interrupt.h>
+#include <linux/libfdt_env.h>
+
+/* General defines */
+#define FMAN_LIODN_TBL			64	/* size of LIODN table */
+#define MAX_NUM_OF_MACS			10
+#define FM_NUM_OF_FMAN_CTRL_EVENT_REGS	4
+#define BASE_RX_PORTID			0x08
+#define BASE_TX_PORTID			0x28
+
+/* Modules registers offsets */
+#define BMI_OFFSET		0x00080000
+#define QMI_OFFSET		0x00080400
+#define DMA_OFFSET		0x000C2000
+#define FPM_OFFSET		0x000C3000
+#define IMEM_OFFSET		0x000C4000
+#define CGP_OFFSET		0x000DB000
+
+/* Exceptions bit map */
+#define EX_DMA_BUS_ERROR		0x80000000
+#define EX_DMA_READ_ECC			0x40000000
+#define EX_DMA_SYSTEM_WRITE_ECC	0x20000000
+#define EX_DMA_FM_WRITE_ECC		0x10000000
+#define EX_FPM_STALL_ON_TASKS		0x08000000
+#define EX_FPM_SINGLE_ECC		0x04000000
+#define EX_FPM_DOUBLE_ECC		0x02000000
+#define EX_QMI_SINGLE_ECC		0x01000000
+#define EX_QMI_DEQ_FROM_UNKNOWN_PORTID	0x00800000
+#define EX_QMI_DOUBLE_ECC		0x00400000
+#define EX_BMI_LIST_RAM_ECC		0x00200000
+#define EX_BMI_STORAGE_PROFILE_ECC	0x00100000
+#define EX_BMI_STATISTICS_RAM_ECC	0x00080000
+#define EX_IRAM_ECC			0x00040000
+#define EX_MURAM_ECC			0x00020000
+#define EX_BMI_DISPATCH_RAM_ECC	0x00010000
+#define EX_DMA_SINGLE_PORT_ECC		0x00008000
+
+/* DMA defines */
+/* masks */
+#define DMA_MODE_BER			0x00200000
+#define DMA_MODE_ECC			0x00000020
+#define DMA_MODE_SECURE_PROT		0x00000800
+#define DMA_MODE_AXI_DBG_MASK		0x0F000000
+
+#define DMA_TRANSFER_PORTID_MASK	0xFF000000
+#define DMA_TRANSFER_TNUM_MASK		0x00FF0000
+#define DMA_TRANSFER_LIODN_MASK	0x00000FFF
+
+#define DMA_STATUS_BUS_ERR		0x08000000
+#define DMA_STATUS_READ_ECC		0x04000000
+#define DMA_STATUS_SYSTEM_WRITE_ECC	0x02000000
+#define DMA_STATUS_FM_WRITE_ECC	0x01000000
+#define DMA_STATUS_FM_SPDAT_ECC	0x00080000
+
+#define DMA_MODE_CACHE_OR_SHIFT		30
+#define DMA_MODE_AXI_DBG_SHIFT			24
+#define DMA_MODE_CEN_SHIFT			13
+#define DMA_MODE_CEN_MASK			0x00000007
+#define DMA_MODE_DBG_SHIFT			7
+#define DMA_MODE_AID_MODE_SHIFT		4
+
+#define DMA_THRESH_COMMQ_SHIFT			24
+#define DMA_THRESH_READ_INT_BUF_SHIFT		16
+#define DMA_THRESH_READ_INT_BUF_MASK		0x0000003f
+#define DMA_THRESH_WRITE_INT_BUF_MASK		0x0000003f
+
+#define DMA_TRANSFER_PORTID_SHIFT		24
+#define DMA_TRANSFER_TNUM_SHIFT		16
+
+#define DMA_CAM_SIZEOF_ENTRY			0x40
+#define DMA_CAM_UNITS				8
+
+#define DMA_LIODN_SHIFT		16
+#define DMA_LIODN_BASE_MASK	0x00000FFF
+
+/* FPM defines */
+#define FPM_EV_MASK_DOUBLE_ECC		0x80000000
+#define FPM_EV_MASK_STALL		0x40000000
+#define FPM_EV_MASK_SINGLE_ECC		0x20000000
+#define FPM_EV_MASK_RELEASE_FM		0x00010000
+#define FPM_EV_MASK_DOUBLE_ECC_EN	0x00008000
+#define FPM_EV_MASK_STALL_EN		0x00004000
+#define FPM_EV_MASK_SINGLE_ECC_EN	0x00002000
+#define FPM_EV_MASK_EXTERNAL_HALT	0x00000008
+#define FPM_EV_MASK_ECC_ERR_HALT	0x00000004
+
+#define FPM_RAM_MURAM_ECC		0x00008000
+#define FPM_RAM_IRAM_ECC		0x00004000
+#define FPM_IRAM_ECC_ERR_EX_EN		0x00020000
+#define FPM_MURAM_ECC_ERR_EX_EN	0x00040000
+#define FPM_RAM_IRAM_ECC_EN		0x40000000
+#define FPM_RAM_RAMS_ECC_EN		0x80000000
+#define FPM_RAM_RAMS_ECC_EN_SRC_SEL	0x08000000
+
+#define FPM_REV1_MAJOR_MASK		0x0000FF00
+#define FPM_REV1_MINOR_MASK		0x000000FF
+
+#define FPM_DISP_LIMIT_SHIFT		24
+
+#define FPM_PRT_FM_CTL1			0x00000001
+#define FPM_PRT_FM_CTL2			0x00000002
+#define FPM_PORT_FM_CTL_PORTID_SHIFT	24
+#define FPM_PRC_ORA_FM_CTL_SEL_SHIFT	16
+
+#define FPM_THR1_PRS_SHIFT		24
+#define FPM_THR1_KG_SHIFT		16
+#define FPM_THR1_PLCR_SHIFT		8
+#define FPM_THR1_BMI_SHIFT		0
+
+#define FPM_THR2_QMI_ENQ_SHIFT		24
+#define FPM_THR2_QMI_DEQ_SHIFT		0
+#define FPM_THR2_FM_CTL1_SHIFT		16
+#define FPM_THR2_FM_CTL2_SHIFT		8
+
+#define FPM_EV_MASK_CAT_ERR_SHIFT	1
+#define FPM_EV_MASK_DMA_ERR_SHIFT	0
+
+#define FPM_REV1_MAJOR_SHIFT		8
+
+#define FPM_RSTC_FM_RESET		0x80000000
+#define FPM_RSTC_MAC0_RESET		0x40000000
+#define FPM_RSTC_MAC1_RESET		0x20000000
+#define FPM_RSTC_MAC2_RESET		0x10000000
+#define FPM_RSTC_MAC3_RESET		0x08000000
+#define FPM_RSTC_MAC8_RESET		0x04000000
+#define FPM_RSTC_MAC4_RESET		0x02000000
+#define FPM_RSTC_MAC5_RESET		0x01000000
+#define FPM_RSTC_MAC6_RESET		0x00800000
+#define FPM_RSTC_MAC7_RESET		0x00400000
+#define FPM_RSTC_MAC9_RESET		0x00200000
+
+#define FPM_TS_INT_SHIFT		16
+#define FPM_TS_CTL_EN			0x80000000
+
+/* BMI defines */
+#define BMI_INIT_START				0x80000000
+#define BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC	0x80000000
+#define BMI_ERR_INTR_EN_LIST_RAM_ECC		0x40000000
+#define BMI_ERR_INTR_EN_STATISTICS_RAM_ECC	0x20000000
+#define BMI_ERR_INTR_EN_DISPATCH_RAM_ECC	0x10000000
+#define BMI_NUM_OF_TASKS_MASK			0x3F000000
+#define BMI_NUM_OF_EXTRA_TASKS_MASK		0x000F0000
+#define BMI_NUM_OF_DMAS_MASK			0x00000F00
+#define BMI_NUM_OF_EXTRA_DMAS_MASK		0x0000000F
+#define BMI_FIFO_SIZE_MASK			0x000003FF
+#define BMI_EXTRA_FIFO_SIZE_MASK		0x03FF0000
+#define BMI_CFG2_DMAS_MASK			0x0000003F
+#define BMI_CFG2_TASKS_MASK			0x0000003F
+
+#define BMI_CFG2_TASKS_SHIFT		16
+#define BMI_CFG2_DMAS_SHIFT		0
+#define BMI_CFG1_FIFO_SIZE_SHIFT	16
+#define BMI_NUM_OF_TASKS_SHIFT		24
+#define BMI_EXTRA_NUM_OF_TASKS_SHIFT	16
+#define BMI_NUM_OF_DMAS_SHIFT		8
+#define BMI_EXTRA_NUM_OF_DMAS_SHIFT	0
+
+#define BMI_FIFO_ALIGN			0x100
+
+#define BMI_EXTRA_FIFO_SIZE_SHIFT	16
+
+/* QMI defines */
+#define QMI_CFG_ENQ_EN			0x80000000
+#define QMI_CFG_DEQ_EN			0x40000000
+#define QMI_CFG_EN_COUNTERS		0x10000000
+#define QMI_CFG_DEQ_MASK		0x0000003F
+#define QMI_CFG_ENQ_MASK		0x00003F00
+#define QMI_CFG_ENQ_SHIFT		8
+
+#define QMI_ERR_INTR_EN_DOUBLE_ECC	0x80000000
+#define QMI_ERR_INTR_EN_DEQ_FROM_DEF	0x40000000
+#define QMI_INTR_EN_SINGLE_ECC		0x80000000
+
+#define QMI_GS_HALT_NOT_BUSY		0x00000002
+
+/* IRAM defines */
+#define IRAM_IADD_AIE			0x80000000
+#define IRAM_READY			0x80000000
+
+/* Default values */
+#define DEFAULT_CATASTROPHIC_ERR		0
+#define DEFAULT_DMA_ERR				0
+#define DEFAULT_AID_MODE			FMAN_DMA_AID_OUT_TNUM
+#define DEFAULT_DMA_COMM_Q_LOW			0x2A
+#define DEFAULT_DMA_COMM_Q_HIGH		0x3F
+#define DEFAULT_CACHE_OVERRIDE			0
+#define DEFAULT_DMA_CAM_NUM_OF_ENTRIES		64
+#define DEFAULT_DMA_DBG_CNT_MODE		0
+#define DEFAULT_DMA_SOS_EMERGENCY		0
+#define DEFAULT_DMA_WATCHDOG			0
+#define DEFAULT_DISP_LIMIT			0
+#define DEFAULT_PRS_DISP_TH			16
+#define DEFAULT_PLCR_DISP_TH			16
+#define DEFAULT_KG_DISP_TH			16
+#define DEFAULT_BMI_DISP_TH			16
+#define DEFAULT_QMI_ENQ_DISP_TH		16
+#define DEFAULT_QMI_DEQ_DISP_TH		16
+#define DEFAULT_FM_CTL1_DISP_TH		16
+#define DEFAULT_FM_CTL2_DISP_TH		16
+
+#define DFLT_AXI_DBG_NUM_OF_BEATS		1
+
+#define DFLT_DMA_READ_INT_BUF_LOW(dma_thresh_max_buf)	\
+	((dma_thresh_max_buf + 1) / 2)
+#define DFLT_DMA_READ_INT_BUF_HIGH(dma_thresh_max_buf)	\
+	((dma_thresh_max_buf + 1) * 3 / 4)
+#define DFLT_DMA_WRITE_INT_BUF_LOW(dma_thresh_max_buf)	\
+	((dma_thresh_max_buf + 1) / 2)
+#define DFLT_DMA_WRITE_INT_BUF_HIGH(dma_thresh_max_buf)\
+	((dma_thresh_max_buf + 1) * 3 / 4)
+
+#define DMA_COMM_Q_LOW_FMAN_V3		0x2A
+#define DMA_COMM_Q_LOW_FMAN_V2(dma_thresh_max_commq)		\
+	((dma_thresh_max_commq + 1) / 2)
+#define DFLT_DMA_COMM_Q_LOW(major, dma_thresh_max_commq)	\
+	((major == 6) ? DMA_COMM_Q_LOW_FMAN_V3 :		\
+	DMA_COMM_Q_LOW_FMAN_V2(dma_thresh_max_commq))
+
+#define DMA_COMM_Q_HIGH_FMAN_V3	0x3f
+#define DMA_COMM_Q_HIGH_FMAN_V2(dma_thresh_max_commq)		\
+	((dma_thresh_max_commq + 1) * 3 / 4)
+#define DFLT_DMA_COMM_Q_HIGH(major, dma_thresh_max_commq)	\
+	((major == 6) ? DMA_COMM_Q_HIGH_FMAN_V3 :		\
+	DMA_COMM_Q_HIGH_FMAN_V2(dma_thresh_max_commq))
+
+#define TOTAL_NUM_OF_TASKS_FMAN_V3L	59
+#define TOTAL_NUM_OF_TASKS_FMAN_V3H	124
+#define DFLT_TOTAL_NUM_OF_TASKS(major, minor, bmi_max_num_of_tasks)	\
+	((major == 6) ? ((minor == 1 || minor == 4) ?			\
+	TOTAL_NUM_OF_TASKS_FMAN_V3L : TOTAL_NUM_OF_TASKS_FMAN_V3H) :	\
+	bmi_max_num_of_tasks)
+
+#define DMA_CAM_NUM_OF_ENTRIES_FMAN_V3		64
+#define DMA_CAM_NUM_OF_ENTRIES_FMAN_V2		32
+#define DFLT_DMA_CAM_NUM_OF_ENTRIES(major)			\
+	(major == 6 ? DMA_CAM_NUM_OF_ENTRIES_FMAN_V3 :		\
+	DMA_CAM_NUM_OF_ENTRIES_FMAN_V2)
+
+#define FM_TIMESTAMP_1_USEC_BIT             8
+
+/* Defines used for enabling/disabling FMan interrupts */
+#define ERR_INTR_EN_DMA         0x00010000
+#define ERR_INTR_EN_FPM         0x80000000
+#define ERR_INTR_EN_BMI         0x00800000
+#define ERR_INTR_EN_QMI         0x00400000
+#define ERR_INTR_EN_MURAM       0x00040000
+#define ERR_INTR_EN_MAC0        0x00004000
+#define ERR_INTR_EN_MAC1        0x00002000
+#define ERR_INTR_EN_MAC2        0x00001000
+#define ERR_INTR_EN_MAC3        0x00000800
+#define ERR_INTR_EN_MAC4        0x00000400
+#define ERR_INTR_EN_MAC5        0x00000200
+#define ERR_INTR_EN_MAC6        0x00000100
+#define ERR_INTR_EN_MAC7        0x00000080
+#define ERR_INTR_EN_MAC8        0x00008000
+#define ERR_INTR_EN_MAC9        0x00000040
+
+#define INTR_EN_QMI             0x40000000
+#define INTR_EN_MAC0            0x00080000
+#define INTR_EN_MAC1            0x00040000
+#define INTR_EN_MAC2            0x00020000
+#define INTR_EN_MAC3            0x00010000
+#define INTR_EN_MAC4            0x00000040
+#define INTR_EN_MAC5            0x00000020
+#define INTR_EN_MAC6            0x00000008
+#define INTR_EN_MAC7            0x00000002
+#define INTR_EN_MAC8            0x00200000
+#define INTR_EN_MAC9            0x00100000
+#define INTR_EN_REV0            0x00008000
+#define INTR_EN_REV1            0x00004000
+#define INTR_EN_REV2            0x00002000
+#define INTR_EN_REV3            0x00001000
+#define INTR_EN_TMR             0x01000000
+
+enum fman_dma_aid_mode {
+	FMAN_DMA_AID_OUT_PORT_ID = 0,		  /* 4 LSB of PORT_ID */
+	FMAN_DMA_AID_OUT_TNUM			  /* 4 LSB of TNUM */
+};
+
+struct fman_iram_regs {
+	u32 iadd;	/* FM IRAM instruction address register */
+	u32 idata;	/* FM IRAM instruction data register */
+	u32 itcfg;	/* FM IRAM timing config register */
+	u32 iready;	/* FM IRAM ready register */
+};
+
+struct fman_fpm_regs {
+	u32 fmfp_tnc;		/* FPM TNUM Control 0x00 */
+	u32 fmfp_prc;		/* FPM Port_ID FmCtl Association 0x04 */
+	u32 fmfp_brkc;		/* FPM Breakpoint Control 0x08 */
+	u32 fmfp_mxd;		/* FPM Flush Control 0x0c */
+	u32 fmfp_dist1;		/* FPM Dispatch Thresholds1 0x10 */
+	u32 fmfp_dist2;		/* FPM Dispatch Thresholds2 0x14 */
+	u32 fm_epi;		/* FM Error Pending Interrupts 0x18 */
+	u32 fm_rie;		/* FM Error Interrupt Enable 0x1c */
+	u32 fmfp_fcev[4];	/* FPM FMan-Controller Event 1-4 0x20-0x2f */
+	u32 res0030[4];		/* res 0x30 - 0x3f */
+	u32 fmfp_cee[4];	/* PM FMan-Controller Event 1-4 0x40-0x4f */
+	u32 res0050[4];		/* res 0x50-0x5f */
+	u32 fmfp_tsc1;		/* FPM TimeStamp Control1 0x60 */
+	u32 fmfp_tsc2;		/* FPM TimeStamp Control2 0x64 */
+	u32 fmfp_tsp;		/* FPM Time Stamp 0x68 */
+	u32 fmfp_tsf;		/* FPM Time Stamp Fraction 0x6c */
+	u32 fm_rcr;		/* FM Rams Control 0x70 */
+	u32 fmfp_extc;		/* FPM External Requests Control 0x74 */
+	u32 fmfp_ext1;		/* FPM External Requests Config1 0x78 */
+	u32 fmfp_ext2;		/* FPM External Requests Config2 0x7c */
+	u32 fmfp_drd[16];	/* FPM Data_Ram Data 0-15 0x80 - 0xbf */
+	u32 fmfp_dra;		/* FPM Data Ram Access 0xc0 */
+	u32 fm_ip_rev_1;	/* FM IP Block Revision 1 0xc4 */
+	u32 fm_ip_rev_2;	/* FM IP Block Revision 2 0xc8 */
+	u32 fm_rstc;		/* FM Reset Command 0xcc */
+	u32 fm_cld;		/* FM Classifier Debug 0xd0 */
+	u32 fm_npi;		/* FM Normal Pending Interrupts 0xd4 */
+	u32 fmfp_exte;		/* FPM External Requests Enable 0xd8 */
+	u32 fmfp_ee;		/* FPM Event&Mask 0xdc */
+	u32 fmfp_cev[4];	/* FPM CPU Event 1-4 0xe0-0xef */
+	u32 res00f0[4];		/* res 0xf0-0xff */
+	u32 fmfp_ps[50];	/* FPM Port Status 0x100-0x1c7 */
+	u32 res01c8[14];	/* res 0x1c8-0x1ff */
+	u32 fmfp_clfabc;	/* FPM CLFABC 0x200 */
+	u32 fmfp_clfcc;		/* FPM CLFCC 0x204 */
+	u32 fmfp_clfaval;	/* FPM CLFAVAL 0x208 */
+	u32 fmfp_clfbval;	/* FPM CLFBVAL 0x20c */
+	u32 fmfp_clfcval;	/* FPM CLFCVAL 0x210 */
+	u32 fmfp_clfamsk;	/* FPM CLFAMSK 0x214 */
+	u32 fmfp_clfbmsk;	/* FPM CLFBMSK 0x218 */
+	u32 fmfp_clfcmsk;	/* FPM CLFCMSK 0x21c */
+	u32 fmfp_clfamc;	/* FPM CLFAMC 0x220 */
+	u32 fmfp_clfbmc;	/* FPM CLFBMC 0x224 */
+	u32 fmfp_clfcmc;	/* FPM CLFCMC 0x228 */
+	u32 fmfp_decceh;	/* FPM DECCEH 0x22c */
+	u32 res0230[116];	/* res 0x230 - 0x3ff */
+	u32 fmfp_ts[128];	/* 0x400: FPM Task Status 0x400 - 0x5ff */
+	u32 res0600[0x400 - 384];
+};
+
+struct fman_bmi_regs {
+	u32 fmbm_init;		/* BMI Initialization 0x00 */
+	u32 fmbm_cfg1;		/* BMI Configuration 1 0x04 */
+	u32 fmbm_cfg2;		/* BMI Configuration 2 0x08 */
+	u32 res000c[5];		/* 0x0c - 0x1f */
+	u32 fmbm_ievr;		/* Interrupt Event Register 0x20 */
+	u32 fmbm_ier;		/* Interrupt Enable Register 0x24 */
+	u32 fmbm_ifr;		/* Interrupt Force Register 0x28 */
+	u32 res002c[5];		/* 0x2c - 0x3f */
+	u32 fmbm_arb[8];	/* BMI Arbitration 0x40 - 0x5f */
+	u32 res0060[12];	/* 0x60 - 0x8f */
+	u32 fmbm_dtc[3];	/* Debug Trap Counter 0x90 - 0x9b */
+	u32 res009c;		/* 0x9c */
+	u32 fmbm_dcv[3][4];	/* Debug Compare val 0xa0-0xcf */
+	u32 fmbm_dcm[3][4];	/* Debug Compare Mask 0xd0-0xff */
+	u32 fmbm_gde;		/* BMI Global Debug Enable 0x100 */
+	u32 fmbm_pp[63];	/* BMI Port Parameters 0x104 - 0x1ff */
+	u32 res0200;		/* 0x200 */
+	u32 fmbm_pfs[63];	/* BMI Port FIFO Size 0x204 - 0x2ff */
+	u32 res0300;		/* 0x300 */
+	u32 fmbm_spliodn[63];	/* Port Partition ID 0x304 - 0x3ff */
+};
+
+struct fman_qmi_regs {
+	u32 fmqm_gc;		/* General Configuration Register 0x00 */
+	u32 res0004;		/* 0x04 */
+	u32 fmqm_eie;		/* Error Interrupt Event Register 0x08 */
+	u32 fmqm_eien;		/* Error Interrupt Enable Register 0x0c */
+	u32 fmqm_eif;		/* Error Interrupt Force Register 0x10 */
+	u32 fmqm_ie;		/* Interrupt Event Register 0x14 */
+	u32 fmqm_ien;		/* Interrupt Enable Register 0x18 */
+	u32 fmqm_if;		/* Interrupt Force Register 0x1c */
+	u32 fmqm_gs;		/* Global Status Register 0x20 */
+	u32 fmqm_ts;		/* Task Status Register 0x24 */
+	u32 fmqm_etfc;		/* Enqueue Total Frame Counter 0x28 */
+	u32 fmqm_dtfc;		/* Dequeue Total Frame Counter 0x2c */
+	u32 fmqm_dc0;		/* Dequeue Counter 0 0x30 */
+	u32 fmqm_dc1;		/* Dequeue Counter 1 0x34 */
+	u32 fmqm_dc2;		/* Dequeue Counter 2 0x38 */
+	u32 fmqm_dc3;		/* Dequeue Counter 3 0x3c */
+	u32 fmqm_dfdc;		/* Dequeue FQID from Default Counter 0x40 */
+	u32 fmqm_dfcc;		/* Dequeue FQID from Context Counter 0x44 */
+	u32 fmqm_dffc;		/* Dequeue FQID from FD Counter 0x48 */
+	u32 fmqm_dcc;		/* Dequeue Confirm Counter 0x4c */
+	u32 res0050[7];		/* 0x50 - 0x6b */
+	u32 fmqm_tapc;		/* Tnum Aging Period Control 0x6c */
+	u32 fmqm_dmcvc;		/* Dequeue MAC Command Valid Counter 0x70 */
+	u32 fmqm_difdcc;	/* Dequeue Invalid FD Command Counter 0x74 */
+	u32 fmqm_da1v;		/* Dequeue A1 Valid Counter 0x78 */
+	u32 res007c;		/* 0x7c */
+	u32 fmqm_dtc;		/* 0x80 Debug Trap Counter 0x80 */
+	u32 fmqm_efddd;		/* 0x84 Enqueue Frame desc Dynamic dbg 0x84 */
+	u32 res0088[2];		/* 0x88 - 0x8f */
+	struct {
+		u32 fmqm_dtcfg1;	/* 0x90 dbg trap cfg 1 Register 0x00 */
+		u32 fmqm_dtval1;	/* Debug Trap Value 1 Register 0x04 */
+		u32 fmqm_dtm1;		/* Debug Trap Mask 1 Register 0x08 */
+		u32 fmqm_dtc1;		/* Debug Trap Counter 1 Register 0x0c */
+		u32 fmqm_dtcfg2;	/* dbg Trap cfg 2 Register 0x10 */
+		u32 fmqm_dtval2;	/* Debug Trap Value 2 Register 0x14 */
+		u32 fmqm_dtm2;		/* Debug Trap Mask 2 Register 0x18 */
+		u32 res001c;		/* 0x1c */
+	} dbg_traps[3];			/* 0x90 - 0xef */
+	u8 res00f0[0x400 - 0xf0];	/* 0xf0 - 0x3ff */
+};
+
+struct fman_dma_regs {
+	u32 fmdmsr;	/* FM DMA status register 0x00 */
+	u32 fmdmmr;	/* FM DMA mode register 0x04 */
+	u32 fmdmtr;	/* FM DMA bus threshold register 0x08 */
+	u32 fmdmhy;	/* FM DMA bus hysteresis register 0x0c */
+	u32 fmdmsetr;	/* FM DMA SOS emergency Threshold Register 0x10 */
+	u32 fmdmtah;	/* FM DMA transfer bus address high reg 0x14 */
+	u32 fmdmtal;	/* FM DMA transfer bus address low reg 0x18 */
+	u32 fmdmtcid;	/* FM DMA transfer bus communication ID reg 0x1c */
+	u32 fmdmra;	/* FM DMA bus internal ram address register 0x20 */
+	u32 fmdmrd;	/* FM DMA bus internal ram data register 0x24 */
+	u32 fmdmwcr;	/* FM DMA CAM watchdog counter value 0x28 */
+	u32 fmdmebcr;	/* FM DMA CAM base in MURAM register 0x2c */
+	u32 fmdmccqdr;	/* FM DMA CAM and CMD Queue Debug reg 0x30 */
+	u32 fmdmccqvr1;	/* FM DMA CAM and CMD Queue Value reg #1 0x34 */
+	u32 fmdmccqvr2;	/* FM DMA CAM and CMD Queue Value reg #2 0x38 */
+	u32 fmdmcqvr3;	/* FM DMA CMD Queue Value register #3 0x3c */
+	u32 fmdmcqvr4;	/* FM DMA CMD Queue Value register #4 0x40 */
+	u32 fmdmcqvr5;	/* FM DMA CMD Queue Value register #5 0x44 */
+	u32 fmdmsefrc;	/* FM DMA Semaphore Entry Full Reject Cntr 0x48 */
+	u32 fmdmsqfrc;	/* FM DMA Semaphore Queue Full Reject Cntr 0x4c */
+	u32 fmdmssrc;	/* FM DMA Semaphore SYNC Reject Counter 0x50 */
+	u32 fmdmdcr;	/* FM DMA Debug Counter 0x54 */
+	u32 fmdmemsr;	/* FM DMA Emergency Smoother Register 0x58 */
+	u32 res005c;	/* 0x5c */
+	u32 fmdmplr[FMAN_LIODN_TBL / 2];	/* DMA LIODN regs 0x60-0xdf */
+	u32 res00e0[0x400 - 56];
+};
+
+/* Structure that holds current FMan state.
+ * Used for saving run time information.
+ */
+struct fman_state_struct {
+	u8 fm_id;
+	u16 fm_clk_freq;
+	struct fman_rev_info rev_info;
+	bool enabled_time_stamp;
+	u8 count1_micro_bit;
+	u8 total_num_of_tasks;
+	u8 accumulated_num_of_tasks;
+	u32 accumulated_fifo_size;
+	u8 accumulated_num_of_open_dmas;
+	u8 accumulated_num_of_deq_tnums;
+	u32 exceptions;
+	u32 extra_fifo_pool_size;
+	u8 extra_tasks_pool_size;
+	u8 extra_open_dmas_pool_size;
+	u16 port_mfl[MAX_NUM_OF_MACS];
+	u16 mac_mfl[MAX_NUM_OF_MACS];
+
+	/* SOC specific */
+	u32 fm_iram_size;
+	/* DMA */
+	u32 dma_thresh_max_commq;
+	u32 dma_thresh_max_buf;
+	u32 max_num_of_open_dmas;
+	/* QMI */
+	u32 qmi_max_num_of_tnums;
+	u32 qmi_def_tnums_thresh;
+	/* BMI */
+	u32 bmi_max_num_of_tasks;
+	u32 bmi_max_fifo_size;
+	/* General */
+	u32 fm_port_num_of_cg;
+	u32 num_of_rx_ports;
+	u32 total_fifo_size;
+
+	u32 qman_channel_base;
+	u32 num_of_qman_channels;
+
+	struct resource *res;
+};
+
+/* Structure that holds FMan initial configuration */
+struct fman_cfg {
+	u8 disp_limit_tsh;
+	u8 prs_disp_tsh;
+	u8 plcr_disp_tsh;
+	u8 kg_disp_tsh;
+	u8 bmi_disp_tsh;
+	u8 qmi_enq_disp_tsh;
+	u8 qmi_deq_disp_tsh;
+	u8 fm_ctl1_disp_tsh;
+	u8 fm_ctl2_disp_tsh;
+	int dma_cache_override;
+	enum fman_dma_aid_mode dma_aid_mode;
+	u32 dma_axi_dbg_num_of_beats;
+	u32 dma_cam_num_of_entries;
+	u32 dma_watchdog;
+	u8 dma_comm_qtsh_asrt_emer;
+	u32 dma_write_buf_tsh_asrt_emer;
+	u32 dma_read_buf_tsh_asrt_emer;
+	u8 dma_comm_qtsh_clr_emer;
+	u32 dma_write_buf_tsh_clr_emer;
+	u32 dma_read_buf_tsh_clr_emer;
+	u32 dma_sos_emergency;
+	int dma_dbg_cnt_mode;
+	int catastrophic_err;
+	int dma_err;
+	u32 exceptions;
+	u16 clk_freq;
+	u32 cam_base_addr;
+	u32 fifo_base_addr;
+	u32 total_fifo_size;
+	u32 total_num_of_tasks;
+	u32 qmi_def_tnums_thresh;
+};
+
+/* Structure that holds information received from device tree */
+struct fman_dts_params {
+	void __iomem *base_addr;		/* FMan virtual address */
+	struct resource *res;			/* FMan memory resource */
+	u8 id;					/* FMan ID */
+
+	int err_irq;				/* FMan Error IRQ */
+
+	u16 clk_freq;				/* FMan clock freq (In Mhz) */
+
+	u32 qman_channel_base;			/* QMan channels base */
+	u32 num_of_qman_channels;		/* Number of QMan channels */
+
+	struct resource muram_res;		/* MURAM resource */
+};
+
+/** fman_exceptions_cb
+ * fman		- Pointer to FMan
+ * exception	- The exception.
+ *
+ * Exceptions user callback routine, will be called upon an exception
+ * passing the exception identification.
+ *
+ * Return: irq status
+ */
+typedef irqreturn_t (fman_exceptions_cb)(struct fman *fman,
+					 enum fman_exceptions exception);
+
+/** fman_bus_error_cb
+ * fman		- Pointer to FMan
+ * port_id	- Port id
+ * addr		- Address that caused the error
+ * tnum		- Owner of error
+ * liodn	- Logical IO device number
+ *
+ * Bus error user callback routine, will be called upon bus error,
+ * passing parameters describing the errors and the owner.
+ *
+ * Return: IRQ status
+ */
+typedef irqreturn_t (fman_bus_error_cb)(struct fman *fman, u8 port_id,
+					u64 addr, u8 tnum, u16 liodn);
+
+struct fman {
+	struct device *dev;
+	void __iomem *base_addr;
+	struct fman_intr_src intr_mng[FMAN_EV_CNT];
+
+	struct fman_fpm_regs __iomem *fpm_regs;
+	struct fman_bmi_regs __iomem *bmi_regs;
+	struct fman_qmi_regs __iomem *qmi_regs;
+	struct fman_dma_regs __iomem *dma_regs;
+	fman_exceptions_cb *exception_cb;
+	fman_bus_error_cb *bus_error_cb;
+	/* Spinlock for FMan use */
+	spinlock_t spinlock;
+	struct fman_state_struct *state;
+
+	struct fman_cfg *cfg;
+	struct muram_info *muram;
+	/* cam section in muram */
+	int cam_offset;
+	size_t cam_size;
+	/* Fifo in MURAM */
+	int fifo_offset;
+	size_t fifo_size;
+
+	u32 liodn_base[64];
+	u32 liodn_offset[64];
+
+	struct fman_dts_params dts_params;
+};
+
+static irqreturn_t fman_exceptions(struct fman *fman,
+				   enum fman_exceptions exception)
+{
+	dev_dbg(fman->dev, "%s: FMan[%d] exception %d\n",
+		__func__, fman->state->fm_id, exception);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t fman_bus_error(struct fman *fman, u8 __maybe_unused port_id,
+				  u64 __maybe_unused addr,
+				  u8 __maybe_unused tnum,
+				  u16 __maybe_unused liodn)
+{
+	dev_dbg(fman->dev, "%s: FMan[%d] bus error: port_id[%d]\n",
+		__func__, fman->state->fm_id, port_id);
+
+	return IRQ_HANDLED;
+}
+
+static inline irqreturn_t call_mac_isr(struct fman *fman, u8 id)
+{
+	if (fman->intr_mng[id].isr_cb) {
+		fman->intr_mng[id].isr_cb(fman->intr_mng[id].src_handle);
+
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static inline u8 hw_port_id_to_sw_port_id(u8 major, u8 hw_port_id)
+{
+	u8 sw_port_id = 0;
+
+	if (hw_port_id >= BASE_TX_PORTID)
+		sw_port_id = hw_port_id - BASE_TX_PORTID;
+	else if (hw_port_id >= BASE_RX_PORTID)
+		sw_port_id = hw_port_id - BASE_RX_PORTID;
+	else
+		sw_port_id = 0;
+
+	return sw_port_id;
+}
+
+static void set_port_order_restoration(struct fman_fpm_regs __iomem *fpm_rg,
+				       u8 port_id)
+{
+	u32 tmp = 0;
+
+	tmp = port_id << FPM_PORT_FM_CTL_PORTID_SHIFT;
+
+	tmp |= FPM_PRT_FM_CTL2 | FPM_PRT_FM_CTL1;
+
+	/* order restoration */
+	if (port_id % 2)
+		tmp |= FPM_PRT_FM_CTL1 << FPM_PRC_ORA_FM_CTL_SEL_SHIFT;
+	else
+		tmp |= FPM_PRT_FM_CTL2 << FPM_PRC_ORA_FM_CTL_SEL_SHIFT;
+
+	iowrite32be(tmp, &fpm_rg->fmfp_prc);
+}
+
+static void set_port_liodn(struct fman *fman, u8 port_id,
+			   u32 liodn_base, u32 liodn_ofst)
+{
+	u32 tmp;
+
+	/* set LIODN base for this port */
+	tmp = ioread32be(&fman->dma_regs->fmdmplr[port_id / 2]);
+	if (port_id % 2) {
+		tmp &= ~DMA_LIODN_BASE_MASK;
+		tmp |= liodn_base;
+	} else {
+		tmp &= ~(DMA_LIODN_BASE_MASK << DMA_LIODN_SHIFT);
+		tmp |= liodn_base << DMA_LIODN_SHIFT;
+	}
+	iowrite32be(tmp, &fman->dma_regs->fmdmplr[port_id / 2]);
+	iowrite32be(liodn_ofst, &fman->bmi_regs->fmbm_spliodn[port_id - 1]);
+}
+
+static void enable_rams_ecc(struct fman_fpm_regs __iomem *fpm_rg)
+{
+	u32 tmp;
+
+	tmp = ioread32be(&fpm_rg->fm_rcr);
+	if (tmp & FPM_RAM_RAMS_ECC_EN_SRC_SEL)
+		iowrite32be(tmp | FPM_RAM_IRAM_ECC_EN, &fpm_rg->fm_rcr);
+	else
+		iowrite32be(tmp | FPM_RAM_RAMS_ECC_EN |
+			    FPM_RAM_IRAM_ECC_EN, &fpm_rg->fm_rcr);
+}
+
+static void disable_rams_ecc(struct fman_fpm_regs __iomem *fpm_rg)
+{
+	u32 tmp;
+
+	tmp = ioread32be(&fpm_rg->fm_rcr);
+	if (tmp & FPM_RAM_RAMS_ECC_EN_SRC_SEL)
+		iowrite32be(tmp & ~FPM_RAM_IRAM_ECC_EN, &fpm_rg->fm_rcr);
+	else
+		iowrite32be(tmp & ~(FPM_RAM_RAMS_ECC_EN | FPM_RAM_IRAM_ECC_EN),
+			    &fpm_rg->fm_rcr);
+}
+
+static void fman_defconfig(struct fman_cfg *cfg)
+{
+	memset(cfg, 0, sizeof(struct fman_cfg));
+
+	cfg->catastrophic_err = DEFAULT_CATASTROPHIC_ERR;
+	cfg->dma_err = DEFAULT_DMA_ERR;
+	cfg->dma_aid_mode = DEFAULT_AID_MODE;
+	cfg->dma_comm_qtsh_clr_emer = DEFAULT_DMA_COMM_Q_LOW;
+	cfg->dma_comm_qtsh_asrt_emer = DEFAULT_DMA_COMM_Q_HIGH;
+	cfg->dma_cache_override = DEFAULT_CACHE_OVERRIDE;
+	cfg->dma_cam_num_of_entries = DEFAULT_DMA_CAM_NUM_OF_ENTRIES;
+	cfg->dma_dbg_cnt_mode = DEFAULT_DMA_DBG_CNT_MODE;
+	cfg->dma_sos_emergency = DEFAULT_DMA_SOS_EMERGENCY;
+	cfg->dma_watchdog = DEFAULT_DMA_WATCHDOG;
+	cfg->disp_limit_tsh = DEFAULT_DISP_LIMIT;
+	cfg->prs_disp_tsh = DEFAULT_PRS_DISP_TH;
+	cfg->plcr_disp_tsh = DEFAULT_PLCR_DISP_TH;
+	cfg->kg_disp_tsh = DEFAULT_KG_DISP_TH;
+	cfg->bmi_disp_tsh = DEFAULT_BMI_DISP_TH;
+	cfg->qmi_enq_disp_tsh = DEFAULT_QMI_ENQ_DISP_TH;
+	cfg->qmi_deq_disp_tsh = DEFAULT_QMI_DEQ_DISP_TH;
+	cfg->fm_ctl1_disp_tsh = DEFAULT_FM_CTL1_DISP_TH;
+	cfg->fm_ctl2_disp_tsh = DEFAULT_FM_CTL2_DISP_TH;
+}
+
+static int dma_init(struct fman *fman)
+{
+	struct fman_dma_regs __iomem *dma_rg = fman->dma_regs;
+	struct fman_cfg *cfg = fman->cfg;
+	u32 tmp_reg;
+
+	/* Init DMA Registers */
+
+	/* clear status reg events */
+	tmp_reg = (DMA_STATUS_BUS_ERR | DMA_STATUS_READ_ECC |
+		   DMA_STATUS_SYSTEM_WRITE_ECC | DMA_STATUS_FM_WRITE_ECC);
+	iowrite32be(ioread32be(&dma_rg->fmdmsr) | tmp_reg, &dma_rg->fmdmsr);
+
+	/* configure mode register */
+	tmp_reg = 0;
+	tmp_reg |= cfg->dma_cache_override << DMA_MODE_CACHE_OR_SHIFT;
+	if (cfg->exceptions & EX_DMA_BUS_ERROR)
+		tmp_reg |= DMA_MODE_BER;
+	if ((cfg->exceptions & EX_DMA_SYSTEM_WRITE_ECC) |
+	    (cfg->exceptions & EX_DMA_READ_ECC) |
+	    (cfg->exceptions & EX_DMA_FM_WRITE_ECC))
+		tmp_reg |= DMA_MODE_ECC;
+	if (cfg->dma_axi_dbg_num_of_beats)
+		tmp_reg |= (DMA_MODE_AXI_DBG_MASK &
+			((cfg->dma_axi_dbg_num_of_beats - 1)
+			<< DMA_MODE_AXI_DBG_SHIFT));
+
+	tmp_reg |= (((cfg->dma_cam_num_of_entries / DMA_CAM_UNITS) - 1) &
+		DMA_MODE_CEN_MASK) << DMA_MODE_CEN_SHIFT;
+	tmp_reg |= DMA_MODE_SECURE_PROT;
+	tmp_reg |= cfg->dma_dbg_cnt_mode << DMA_MODE_DBG_SHIFT;
+	tmp_reg |= cfg->dma_aid_mode << DMA_MODE_AID_MODE_SHIFT;
+
+	iowrite32be(tmp_reg, &dma_rg->fmdmmr);
+
+	/* configure thresholds register */
+	tmp_reg = ((u32)cfg->dma_comm_qtsh_asrt_emer <<
+		DMA_THRESH_COMMQ_SHIFT);
+	tmp_reg |= (cfg->dma_read_buf_tsh_asrt_emer &
+		DMA_THRESH_READ_INT_BUF_MASK) << DMA_THRESH_READ_INT_BUF_SHIFT;
+	tmp_reg |= cfg->dma_write_buf_tsh_asrt_emer &
+		DMA_THRESH_WRITE_INT_BUF_MASK;
+
+	iowrite32be(tmp_reg, &dma_rg->fmdmtr);
+
+	/* configure hysteresis register */
+	tmp_reg = ((u32)cfg->dma_comm_qtsh_clr_emer <<
+		DMA_THRESH_COMMQ_SHIFT);
+	tmp_reg |= (cfg->dma_read_buf_tsh_clr_emer &
+		DMA_THRESH_READ_INT_BUF_MASK) << DMA_THRESH_READ_INT_BUF_SHIFT;
+	tmp_reg |= cfg->dma_write_buf_tsh_clr_emer &
+		DMA_THRESH_WRITE_INT_BUF_MASK;
+
+	iowrite32be(tmp_reg, &dma_rg->fmdmhy);
+
+	/* configure emergency threshold */
+	iowrite32be(cfg->dma_sos_emergency, &dma_rg->fmdmsetr);
+
+	/* configure Watchdog */
+	iowrite32be((cfg->dma_watchdog * cfg->clk_freq), &dma_rg->fmdmwcr);
+
+	iowrite32be(cfg->cam_base_addr, &dma_rg->fmdmebcr);
+
+	/* Allocate MURAM for CAM */
+	fman->cam_size =
+		(u32)(fman->cfg->dma_cam_num_of_entries * DMA_CAM_SIZEOF_ENTRY);
+	fman->cam_offset = fman_muram_alloc(fman->muram, fman->cam_size);
+	if (IS_ERR_VALUE(fman->cam_offset)) {
+		dev_err(fman->dev, "%s: MURAM alloc for DMA CAM failed\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	if (fman->state->rev_info.major == 2) {
+		u32 __iomem *cam_base_addr;
+
+		fman_muram_free_mem(fman->muram, fman->cam_offset,
+				    fman->cam_size);
+
+		fman->cam_size = fman->cfg->dma_cam_num_of_entries * 72 + 128;
+		fman->cam_offset = fman_muram_alloc(fman->muram,
+						    fman->cam_size);
+		if (IS_ERR_VALUE(fman->cam_offset)) {
+			dev_err(fman->dev, "%s: MURAM alloc for DMA CAM failed\n",
+				__func__);
+			return -ENOMEM;
+		}
+
+		if (fman->cfg->dma_cam_num_of_entries % 8 ||
+		    fman->cfg->dma_cam_num_of_entries > 32) {
+			dev_err(fman->dev, "%s: wrong dma_cam_num_of_entries\n",
+				__func__);
+			return -EINVAL;
+		}
+
+		cam_base_addr = (u32 __iomem *)
+			fman_muram_offset_to_vbase(fman->muram,
+						   fman->cam_offset);
+		iowrite32be(~((1 <<
+			    (32 - fman->cfg->dma_cam_num_of_entries)) - 1),
+			    cam_base_addr);
+	}
+
+	fman->cfg->cam_base_addr = fman->cam_offset;
+
+	return 0;
+}
+
+static void fpm_init(struct fman_fpm_regs __iomem *fpm_rg, struct fman_cfg *cfg)
+{
+	u32 tmp_reg;
+	int i;
+
+	/* Init FPM Registers */
+
+	tmp_reg = (u32)(cfg->disp_limit_tsh << FPM_DISP_LIMIT_SHIFT);
+	iowrite32be(tmp_reg, &fpm_rg->fmfp_mxd);
+
+	tmp_reg = (((u32)cfg->prs_disp_tsh << FPM_THR1_PRS_SHIFT) |
+		   ((u32)cfg->kg_disp_tsh << FPM_THR1_KG_SHIFT) |
+		   ((u32)cfg->plcr_disp_tsh << FPM_THR1_PLCR_SHIFT) |
+		   ((u32)cfg->bmi_disp_tsh << FPM_THR1_BMI_SHIFT));
+	iowrite32be(tmp_reg, &fpm_rg->fmfp_dist1);
+
+	tmp_reg =
+		(((u32)cfg->qmi_enq_disp_tsh << FPM_THR2_QMI_ENQ_SHIFT) |
+		 ((u32)cfg->qmi_deq_disp_tsh << FPM_THR2_QMI_DEQ_SHIFT) |
+		 ((u32)cfg->fm_ctl1_disp_tsh << FPM_THR2_FM_CTL1_SHIFT) |
+		 ((u32)cfg->fm_ctl2_disp_tsh << FPM_THR2_FM_CTL2_SHIFT));
+	iowrite32be(tmp_reg, &fpm_rg->fmfp_dist2);
+
+	/* define exceptions and error behavior */
+	tmp_reg = 0;
+	/* Clear events */
+	tmp_reg |= (FPM_EV_MASK_STALL | FPM_EV_MASK_DOUBLE_ECC |
+		    FPM_EV_MASK_SINGLE_ECC);
+	/* enable interrupts */
+	if (cfg->exceptions & EX_FPM_STALL_ON_TASKS)
+		tmp_reg |= FPM_EV_MASK_STALL_EN;
+	if (cfg->exceptions & EX_FPM_SINGLE_ECC)
+		tmp_reg |= FPM_EV_MASK_SINGLE_ECC_EN;
+	if (cfg->exceptions & EX_FPM_DOUBLE_ECC)
+		tmp_reg |= FPM_EV_MASK_DOUBLE_ECC_EN;
+	tmp_reg |= (cfg->catastrophic_err << FPM_EV_MASK_CAT_ERR_SHIFT);
+	tmp_reg |= (cfg->dma_err << FPM_EV_MASK_DMA_ERR_SHIFT);
+	/* FMan is not halted upon external halt activation */
+	tmp_reg |= FPM_EV_MASK_EXTERNAL_HALT;
+	/* Man is not halted upon  Unrecoverable ECC error behavior */
+	tmp_reg |= FPM_EV_MASK_ECC_ERR_HALT;
+	iowrite32be(tmp_reg, &fpm_rg->fmfp_ee);
+
+	/* clear all fmCtls event registers */
+	for (i = 0; i < FM_NUM_OF_FMAN_CTRL_EVENT_REGS; i++)
+		iowrite32be(0xFFFFFFFF, &fpm_rg->fmfp_cev[i]);
+
+	/* RAM ECC -  enable and clear events */
+	/* first we need to clear all parser memory,
+	 * as it is uninitialized and may cause ECC errors
+	 */
+	/* event bits */
+	tmp_reg = (FPM_RAM_MURAM_ECC | FPM_RAM_IRAM_ECC);
+
+	iowrite32be(tmp_reg, &fpm_rg->fm_rcr);
+
+	tmp_reg = 0;
+	if (cfg->exceptions & EX_IRAM_ECC) {
+		tmp_reg |= FPM_IRAM_ECC_ERR_EX_EN;
+		enable_rams_ecc(fpm_rg);
+	}
+	if (cfg->exceptions & EX_MURAM_ECC) {
+		tmp_reg |= FPM_MURAM_ECC_ERR_EX_EN;
+		enable_rams_ecc(fpm_rg);
+	}
+	iowrite32be(tmp_reg, &fpm_rg->fm_rie);
+}
+
+static void bmi_init(struct fman_bmi_regs __iomem *bmi_rg,
+		     struct fman_cfg *cfg)
+{
+	u32 tmp_reg;
+
+	/* Init BMI Registers */
+
+	/* define common resources */
+	tmp_reg = cfg->fifo_base_addr;
+	tmp_reg = tmp_reg / BMI_FIFO_ALIGN;
+
+	tmp_reg |= ((cfg->total_fifo_size / FMAN_BMI_FIFO_UNITS - 1) <<
+		    BMI_CFG1_FIFO_SIZE_SHIFT);
+	iowrite32be(tmp_reg, &bmi_rg->fmbm_cfg1);
+
+	tmp_reg = ((cfg->total_num_of_tasks - 1) & BMI_CFG2_TASKS_MASK) <<
+		   BMI_CFG2_TASKS_SHIFT;
+	/* num of DMA's will be dynamically updated when each port is set */
+	iowrite32be(tmp_reg, &bmi_rg->fmbm_cfg2);
+
+	/* define unmaskable exceptions, enable and clear events */
+	tmp_reg = 0;
+	iowrite32be(BMI_ERR_INTR_EN_LIST_RAM_ECC |
+		    BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC |
+		    BMI_ERR_INTR_EN_STATISTICS_RAM_ECC |
+		    BMI_ERR_INTR_EN_DISPATCH_RAM_ECC, &bmi_rg->fmbm_ievr);
+
+	if (cfg->exceptions & EX_BMI_LIST_RAM_ECC)
+		tmp_reg |= BMI_ERR_INTR_EN_LIST_RAM_ECC;
+	if (cfg->exceptions & EX_BMI_STORAGE_PROFILE_ECC)
+		tmp_reg |= BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC;
+	if (cfg->exceptions & EX_BMI_STATISTICS_RAM_ECC)
+		tmp_reg |= BMI_ERR_INTR_EN_STATISTICS_RAM_ECC;
+	if (cfg->exceptions & EX_BMI_DISPATCH_RAM_ECC)
+		tmp_reg |= BMI_ERR_INTR_EN_DISPATCH_RAM_ECC;
+	iowrite32be(tmp_reg, &bmi_rg->fmbm_ier);
+}
+
+static void qmi_init(struct fman_qmi_regs __iomem *qmi_rg,
+		     struct fman_cfg *cfg)
+{
+	u32 tmp_reg;
+
+	/* Init QMI Registers */
+
+	/* Clear error interrupt events */
+
+	iowrite32be(QMI_ERR_INTR_EN_DOUBLE_ECC | QMI_ERR_INTR_EN_DEQ_FROM_DEF,
+		    &qmi_rg->fmqm_eie);
+	tmp_reg = 0;
+	if (cfg->exceptions & EX_QMI_DEQ_FROM_UNKNOWN_PORTID)
+		tmp_reg |= QMI_ERR_INTR_EN_DEQ_FROM_DEF;
+	if (cfg->exceptions & EX_QMI_DOUBLE_ECC)
+		tmp_reg |= QMI_ERR_INTR_EN_DOUBLE_ECC;
+	/* enable events */
+	iowrite32be(tmp_reg, &qmi_rg->fmqm_eien);
+
+	tmp_reg = 0;
+	/* Clear interrupt events */
+	iowrite32be(QMI_INTR_EN_SINGLE_ECC, &qmi_rg->fmqm_ie);
+	if (cfg->exceptions & EX_QMI_SINGLE_ECC)
+		tmp_reg |= QMI_INTR_EN_SINGLE_ECC;
+	/* enable events */
+	iowrite32be(tmp_reg, &qmi_rg->fmqm_ien);
+}
+
+static int enable(struct fman *fman, struct fman_cfg *cfg)
+{
+	u32 cfg_reg = 0;
+
+	/* Enable all modules */
+
+	/* clear&enable global counters - calculate reg and save for later,
+	 * because it's the same reg for QMI enable
+	 */
+	cfg_reg = QMI_CFG_EN_COUNTERS;
+
+	/* Set enqueue and dequeue thresholds */
+	cfg_reg |= (cfg->qmi_def_tnums_thresh << 8) | cfg->qmi_def_tnums_thresh;
+
+	iowrite32be(BMI_INIT_START, &fman->bmi_regs->fmbm_init);
+	iowrite32be(cfg_reg | QMI_CFG_ENQ_EN | QMI_CFG_DEQ_EN,
+		    &fman->qmi_regs->fmqm_gc);
+
+	return 0;
+}
+
+static int set_exception(struct fman *fman,
+			 enum fman_exceptions exception, bool enable)
+{
+	u32 tmp;
+
+	switch (exception) {
+	case FMAN_EX_DMA_BUS_ERROR:
+		tmp = ioread32be(&fman->dma_regs->fmdmmr);
+		if (enable)
+			tmp |= DMA_MODE_BER;
+		else
+			tmp &= ~DMA_MODE_BER;
+		/* disable bus error */
+		iowrite32be(tmp, &fman->dma_regs->fmdmmr);
+		break;
+	case FMAN_EX_DMA_READ_ECC:
+	case FMAN_EX_DMA_SYSTEM_WRITE_ECC:
+	case FMAN_EX_DMA_FM_WRITE_ECC:
+		tmp = ioread32be(&fman->dma_regs->fmdmmr);
+		if (enable)
+			tmp |= DMA_MODE_ECC;
+		else
+			tmp &= ~DMA_MODE_ECC;
+		iowrite32be(tmp, &fman->dma_regs->fmdmmr);
+		break;
+	case FMAN_EX_FPM_STALL_ON_TASKS:
+		tmp = ioread32be(&fman->fpm_regs->fmfp_ee);
+		if (enable)
+			tmp |= FPM_EV_MASK_STALL_EN;
+		else
+			tmp &= ~FPM_EV_MASK_STALL_EN;
+		iowrite32be(tmp, &fman->fpm_regs->fmfp_ee);
+		break;
+	case FMAN_EX_FPM_SINGLE_ECC:
+		tmp = ioread32be(&fman->fpm_regs->fmfp_ee);
+		if (enable)
+			tmp |= FPM_EV_MASK_SINGLE_ECC_EN;
+		else
+			tmp &= ~FPM_EV_MASK_SINGLE_ECC_EN;
+		iowrite32be(tmp, &fman->fpm_regs->fmfp_ee);
+		break;
+	case FMAN_EX_FPM_DOUBLE_ECC:
+		tmp = ioread32be(&fman->fpm_regs->fmfp_ee);
+		if (enable)
+			tmp |= FPM_EV_MASK_DOUBLE_ECC_EN;
+		else
+			tmp &= ~FPM_EV_MASK_DOUBLE_ECC_EN;
+		iowrite32be(tmp, &fman->fpm_regs->fmfp_ee);
+		break;
+	case FMAN_EX_QMI_SINGLE_ECC:
+		tmp = ioread32be(&fman->qmi_regs->fmqm_ien);
+		if (enable)
+			tmp |= QMI_INTR_EN_SINGLE_ECC;
+		else
+			tmp &= ~QMI_INTR_EN_SINGLE_ECC;
+		iowrite32be(tmp, &fman->qmi_regs->fmqm_ien);
+		break;
+	case FMAN_EX_QMI_DOUBLE_ECC:
+		tmp = ioread32be(&fman->qmi_regs->fmqm_eien);
+		if (enable)
+			tmp |= QMI_ERR_INTR_EN_DOUBLE_ECC;
+		else
+			tmp &= ~QMI_ERR_INTR_EN_DOUBLE_ECC;
+		iowrite32be(tmp, &fman->qmi_regs->fmqm_eien);
+		break;
+	case FMAN_EX_QMI_DEQ_FROM_UNKNOWN_PORTID:
+		tmp = ioread32be(&fman->qmi_regs->fmqm_eien);
+		if (enable)
+			tmp |= QMI_ERR_INTR_EN_DEQ_FROM_DEF;
+		else
+			tmp &= ~QMI_ERR_INTR_EN_DEQ_FROM_DEF;
+		iowrite32be(tmp, &fman->qmi_regs->fmqm_eien);
+		break;
+	case FMAN_EX_BMI_LIST_RAM_ECC:
+		tmp = ioread32be(&fman->bmi_regs->fmbm_ier);
+		if (enable)
+			tmp |= BMI_ERR_INTR_EN_LIST_RAM_ECC;
+		else
+			tmp &= ~BMI_ERR_INTR_EN_LIST_RAM_ECC;
+		iowrite32be(tmp, &fman->bmi_regs->fmbm_ier);
+		break;
+	case FMAN_EX_BMI_STORAGE_PROFILE_ECC:
+		tmp = ioread32be(&fman->bmi_regs->fmbm_ier);
+		if (enable)
+			tmp |= BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC;
+		else
+			tmp &= ~BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC;
+		iowrite32be(tmp, &fman->bmi_regs->fmbm_ier);
+		break;
+	case FMAN_EX_BMI_STATISTICS_RAM_ECC:
+		tmp = ioread32be(&fman->bmi_regs->fmbm_ier);
+		if (enable)
+			tmp |= BMI_ERR_INTR_EN_STATISTICS_RAM_ECC;
+		else
+			tmp &= ~BMI_ERR_INTR_EN_STATISTICS_RAM_ECC;
+		iowrite32be(tmp, &fman->bmi_regs->fmbm_ier);
+		break;
+	case FMAN_EX_BMI_DISPATCH_RAM_ECC:
+		tmp = ioread32be(&fman->bmi_regs->fmbm_ier);
+		if (enable)
+			tmp |= BMI_ERR_INTR_EN_DISPATCH_RAM_ECC;
+		else
+			tmp &= ~BMI_ERR_INTR_EN_DISPATCH_RAM_ECC;
+		iowrite32be(tmp, &fman->bmi_regs->fmbm_ier);
+		break;
+	case FMAN_EX_IRAM_ECC:
+		tmp = ioread32be(&fman->fpm_regs->fm_rie);
+		if (enable) {
+			/* enable ECC if not enabled */
+			enable_rams_ecc(fman->fpm_regs);
+			/* enable ECC interrupts */
+			tmp |= FPM_IRAM_ECC_ERR_EX_EN;
+		} else {
+			/* ECC mechanism may be disabled,
+			 * depending on driver status
+			 */
+			disable_rams_ecc(fman->fpm_regs);
+			tmp &= ~FPM_IRAM_ECC_ERR_EX_EN;
+		}
+		iowrite32be(tmp, &fman->fpm_regs->fm_rie);
+		break;
+	case FMAN_EX_MURAM_ECC:
+		tmp = ioread32be(&fman->fpm_regs->fm_rie);
+		if (enable) {
+			/* enable ECC if not enabled */
+			enable_rams_ecc(fman->fpm_regs);
+			/* enable ECC interrupts */
+			tmp |= FPM_MURAM_ECC_ERR_EX_EN;
+		} else {
+			/* ECC mechanism may be disabled,
+			 * depending on driver status
+			 */
+			disable_rams_ecc(fman->fpm_regs);
+			tmp &= ~FPM_MURAM_ECC_ERR_EX_EN;
+		}
+		iowrite32be(tmp, &fman->fpm_regs->fm_rie);
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static void resume(struct fman_fpm_regs __iomem *fpm_rg)
+{
+	u32 tmp;
+
+	tmp = ioread32be(&fpm_rg->fmfp_ee);
+	/* clear tmp_reg event bits in order not to clear standing events */
+	tmp &= ~(FPM_EV_MASK_DOUBLE_ECC |
+		 FPM_EV_MASK_STALL | FPM_EV_MASK_SINGLE_ECC);
+	tmp |= FPM_EV_MASK_RELEASE_FM;
+
+	iowrite32be(tmp, &fpm_rg->fmfp_ee);
+}
+
+static int fill_soc_specific_params(struct fman_state_struct *state)
+{
+	u8 minor = state->rev_info.minor;
+	/* P4080 - Major 2
+	 * P2041/P3041/P5020/P5040 - Major 3
+	 * Tx/Bx - Major 6
+	 */
+	switch (state->rev_info.major) {
+	case 3:
+		state->bmi_max_fifo_size	= 160 * 1024;
+		state->fm_iram_size		= 64 * 1024;
+		state->dma_thresh_max_commq	= 31;
+		state->dma_thresh_max_buf	= 127;
+		state->qmi_max_num_of_tnums	= 64;
+		state->qmi_def_tnums_thresh	= 48;
+		state->bmi_max_num_of_tasks	= 128;
+		state->max_num_of_open_dmas	= 32;
+		state->fm_port_num_of_cg	= 256;
+		state->num_of_rx_ports	= 6;
+		state->total_fifo_size	= 122 * 1024;
+		break;
+
+	case 2:
+		state->bmi_max_fifo_size	= 160 * 1024;
+		state->fm_iram_size		= 64 * 1024;
+		state->dma_thresh_max_commq	= 31;
+		state->dma_thresh_max_buf	= 127;
+		state->qmi_max_num_of_tnums	= 64;
+		state->qmi_def_tnums_thresh	= 48;
+		state->bmi_max_num_of_tasks	= 128;
+		state->max_num_of_open_dmas	= 32;
+		state->fm_port_num_of_cg	= 256;
+		state->num_of_rx_ports	= 5;
+		state->total_fifo_size	= 100 * 1024;
+		break;
+
+	case 6:
+		state->dma_thresh_max_commq	= 83;
+		state->dma_thresh_max_buf	= 127;
+		state->qmi_max_num_of_tnums	= 64;
+		state->qmi_def_tnums_thresh	= 32;
+		state->fm_port_num_of_cg	= 256;
+
+		/* FManV3L */
+		if (minor == 1 || minor == 4) {
+			state->bmi_max_fifo_size	= 192 * 1024;
+			state->bmi_max_num_of_tasks	= 64;
+			state->max_num_of_open_dmas	= 32;
+			state->num_of_rx_ports		= 5;
+			if (minor == 1)
+				state->fm_iram_size	= 32 * 1024;
+			else
+				state->fm_iram_size	= 64 * 1024;
+			state->total_fifo_size		= 156 * 1024;
+		}
+		/* FManV3H */
+		else if (minor == 0 || minor == 2 || minor == 3) {
+			state->bmi_max_fifo_size	= 384 * 1024;
+			state->fm_iram_size		= 64 * 1024;
+			state->bmi_max_num_of_tasks	= 128;
+			state->max_num_of_open_dmas	= 84;
+			state->num_of_rx_ports		= 8;
+			state->total_fifo_size		= 295 * 1024;
+		} else {
+			pr_err("Unsupported FManv3 version\n");
+			return -EINVAL;
+		}
+
+		break;
+	default:
+		pr_err("Unsupported FMan version\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static bool is_init_done(struct fman_cfg *cfg)
+{
+	/* Checks if FMan driver parameters were initialized */
+	if (!cfg)
+		return true;
+
+	return false;
+}
+
+static void free_init_resources(struct fman *fman)
+{
+	if (fman->cam_offset)
+		fman_muram_free_mem(fman->muram, fman->cam_offset,
+				    fman->cam_size);
+	if (fman->fifo_offset)
+		fman_muram_free_mem(fman->muram, fman->fifo_offset,
+				    fman->fifo_size);
+}
+
+static irqreturn_t bmi_err_event(struct fman *fman)
+{
+	u32 event, mask, force;
+	struct fman_bmi_regs __iomem *bmi_rg = fman->bmi_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	event = ioread32be(&bmi_rg->fmbm_ievr);
+	mask = ioread32be(&bmi_rg->fmbm_ier);
+	event &= mask;
+	/* clear the forced events */
+	force = ioread32be(&bmi_rg->fmbm_ifr);
+	if (force & event)
+		iowrite32be(force & ~event, &bmi_rg->fmbm_ifr);
+	/* clear the acknowledged events */
+	iowrite32be(event, &bmi_rg->fmbm_ievr);
+
+	if (event & BMI_ERR_INTR_EN_STORAGE_PROFILE_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_BMI_STORAGE_PROFILE_ECC);
+	if (event & BMI_ERR_INTR_EN_LIST_RAM_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_BMI_LIST_RAM_ECC);
+	if (event & BMI_ERR_INTR_EN_STATISTICS_RAM_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_BMI_STATISTICS_RAM_ECC);
+	if (event & BMI_ERR_INTR_EN_DISPATCH_RAM_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_BMI_DISPATCH_RAM_ECC);
+
+	return ret;
+}
+
+static irqreturn_t qmi_err_event(struct fman *fman)
+{
+	u32 event, mask, force;
+	struct fman_qmi_regs __iomem *qmi_rg = fman->qmi_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	event = ioread32be(&qmi_rg->fmqm_eie);
+	mask = ioread32be(&qmi_rg->fmqm_eien);
+	event &= mask;
+
+	/* clear the forced events */
+	force = ioread32be(&qmi_rg->fmqm_eif);
+	if (force & event)
+		iowrite32be(force & ~event, &qmi_rg->fmqm_eif);
+	/* clear the acknowledged events */
+	iowrite32be(event, &qmi_rg->fmqm_eie);
+
+	if (event & QMI_ERR_INTR_EN_DOUBLE_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_QMI_DOUBLE_ECC);
+	if (event & QMI_ERR_INTR_EN_DEQ_FROM_DEF)
+		ret = fman->exception_cb(fman,
+					 FMAN_EX_QMI_DEQ_FROM_UNKNOWN_PORTID);
+
+	return ret;
+}
+
+static irqreturn_t dma_err_event(struct fman *fman)
+{
+	u32 status, mask, com_id;
+	u8 tnum, port_id, relative_port_id;
+	u16 liodn;
+	struct fman_dma_regs __iomem *dma_rg = fman->dma_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	status = ioread32be(&dma_rg->fmdmsr);
+	mask = ioread32be(&dma_rg->fmdmmr);
+
+	/* clear DMA_STATUS_BUS_ERR if mask has no DMA_MODE_BER */
+	if ((mask & DMA_MODE_BER) != DMA_MODE_BER)
+		status &= ~DMA_STATUS_BUS_ERR;
+
+	/* clear relevant bits if mask has no DMA_MODE_ECC */
+	if ((mask & DMA_MODE_ECC) != DMA_MODE_ECC)
+		status &= ~(DMA_STATUS_FM_SPDAT_ECC |
+			    DMA_STATUS_READ_ECC |
+			    DMA_STATUS_SYSTEM_WRITE_ECC |
+			    DMA_STATUS_FM_WRITE_ECC);
+
+	/* clear set events */
+	iowrite32be(status, &dma_rg->fmdmsr);
+
+	if (status & DMA_STATUS_BUS_ERR) {
+		u64 addr;
+
+		addr = (u64)ioread32be(&dma_rg->fmdmtal);
+		addr |= ((u64)(ioread32be(&dma_rg->fmdmtah)) << 32);
+
+		com_id = ioread32be(&dma_rg->fmdmtcid);
+		port_id = (u8)(((com_id & DMA_TRANSFER_PORTID_MASK) >>
+			       DMA_TRANSFER_PORTID_SHIFT));
+		relative_port_id =
+		hw_port_id_to_sw_port_id(fman->state->rev_info.major, port_id);
+		tnum = (u8)((com_id & DMA_TRANSFER_TNUM_MASK) >>
+			    DMA_TRANSFER_TNUM_SHIFT);
+		liodn = (u16)(com_id & DMA_TRANSFER_LIODN_MASK);
+		ret = fman->bus_error_cb(fman, relative_port_id, addr, tnum,
+					 liodn);
+	}
+	if (status & DMA_STATUS_FM_SPDAT_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_DMA_SINGLE_PORT_ECC);
+	if (status & DMA_STATUS_READ_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_DMA_READ_ECC);
+	if (status & DMA_STATUS_SYSTEM_WRITE_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_DMA_SYSTEM_WRITE_ECC);
+	if (status & DMA_STATUS_FM_WRITE_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_DMA_FM_WRITE_ECC);
+
+	return ret;
+}
+
+static irqreturn_t fpm_err_event(struct fman *fman)
+{
+	u32 event;
+	struct fman_fpm_regs __iomem *fpm_rg = fman->fpm_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	event = ioread32be(&fpm_rg->fmfp_ee);
+	/* clear the all occurred events */
+	iowrite32be(event, &fpm_rg->fmfp_ee);
+
+	if ((event & FPM_EV_MASK_DOUBLE_ECC) &&
+	    (event & FPM_EV_MASK_DOUBLE_ECC_EN))
+		ret = fman->exception_cb(fman, FMAN_EX_FPM_DOUBLE_ECC);
+	if ((event & FPM_EV_MASK_STALL) && (event & FPM_EV_MASK_STALL_EN))
+		ret = fman->exception_cb(fman, FMAN_EX_FPM_STALL_ON_TASKS);
+	if ((event & FPM_EV_MASK_SINGLE_ECC) &&
+	    (event & FPM_EV_MASK_SINGLE_ECC_EN))
+		ret = fman->exception_cb(fman, FMAN_EX_FPM_SINGLE_ECC);
+
+	return ret;
+}
+
+static irqreturn_t muram_err_intr(struct fman *fman)
+{
+	u32 event, mask;
+	struct fman_fpm_regs __iomem *fpm_rg = fman->fpm_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	event = ioread32be(&fpm_rg->fm_rcr);
+	mask = ioread32be(&fpm_rg->fm_rie);
+
+	/* clear MURAM event bit (do not clear IRAM event) */
+	iowrite32be(event & ~FPM_RAM_IRAM_ECC, &fpm_rg->fm_rcr);
+
+	if ((mask & FPM_MURAM_ECC_ERR_EX_EN) && (event & FPM_RAM_MURAM_ECC))
+		ret = fman->exception_cb(fman, FMAN_EX_MURAM_ECC);
+
+	return ret;
+}
+
+static irqreturn_t qmi_event(struct fman *fman)
+{
+	u32 event, mask, force;
+	struct fman_qmi_regs __iomem *qmi_rg = fman->qmi_regs;
+	irqreturn_t ret = IRQ_NONE;
+
+	event = ioread32be(&qmi_rg->fmqm_ie);
+	mask = ioread32be(&qmi_rg->fmqm_ien);
+	event &= mask;
+	/* clear the forced events */
+	force = ioread32be(&qmi_rg->fmqm_if);
+	if (force & event)
+		iowrite32be(force & ~event, &qmi_rg->fmqm_if);
+	/* clear the acknowledged events */
+	iowrite32be(event, &qmi_rg->fmqm_ie);
+
+	if (event & QMI_INTR_EN_SINGLE_ECC)
+		ret = fman->exception_cb(fman, FMAN_EX_QMI_SINGLE_ECC);
+
+	return ret;
+}
+
+static void enable_time_stamp(struct fman *fman)
+{
+	struct fman_fpm_regs __iomem *fpm_rg = fman->fpm_regs;
+	u16 fm_clk_freq = fman->state->fm_clk_freq;
+	u32 tmp, intgr, ts_freq;
+	u64 frac;
+
+	ts_freq = (u32)(1 << fman->state->count1_micro_bit);
+	/* configure timestamp so that bit 8 will count 1 microsecond
+	 * Find effective count rate at TIMESTAMP least significant bits:
+	 * Effective_Count_Rate = 1MHz x 2^8 = 256MHz
+	 * Find frequency ratio between effective count rate and the clock:
+	 * Effective_Count_Rate / CLK e.g. for 600 MHz clock:
+	 * 256/600 = 0.4266666...
+	 */
+
+	intgr = ts_freq / fm_clk_freq;
+	/* we multiply by 2^16 to keep the fraction of the division
+	 * we do not div back, since we write this value as a fraction
+	 * see spec
+	 */
+
+	frac = ((ts_freq << 16) - (intgr << 16) * fm_clk_freq) / fm_clk_freq;
+	/* we check remainder of the division in order to round up if not int */
+	if (((ts_freq << 16) - (intgr << 16) * fm_clk_freq) % fm_clk_freq)
+		frac++;
+
+	tmp = (intgr << FPM_TS_INT_SHIFT) | (u16)frac;
+	iowrite32be(tmp, &fpm_rg->fmfp_tsc2);
+
+	/* enable timestamp with original clock */
+	iowrite32be(FPM_TS_CTL_EN, &fpm_rg->fmfp_tsc1);
+	fman->state->enabled_time_stamp = true;
+}
+
+static int clear_iram(struct fman *fman)
+{
+	struct fman_iram_regs __iomem *iram;
+	int i, count;
+
+	iram = fman->base_addr + IMEM_OFFSET;
+
+	/* Enable the auto-increment */
+	iowrite32be(IRAM_IADD_AIE, &iram->iadd);
+	count = 100;
+	do {
+		udelay(1);
+	} while ((ioread32be(&iram->iadd) != IRAM_IADD_AIE) && --count);
+	if (count == 0)
+		return -EBUSY;
+
+	for (i = 0; i < (fman->state->fm_iram_size / 4); i++)
+		iowrite32be(0xffffffff, &iram->idata);
+
+	iowrite32be(fman->state->fm_iram_size - 4, &iram->iadd);
+	count = 100;
+	do {
+		udelay(1);
+	} while ((ioread32be(&iram->idata) != 0xffffffff) && --count);
+	if (count == 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static u32 get_exception_flag(enum fman_exceptions exception)
+{
+	u32 bit_mask;
+
+	switch (exception) {
+	case FMAN_EX_DMA_BUS_ERROR:
+		bit_mask = EX_DMA_BUS_ERROR;
+		break;
+	case FMAN_EX_DMA_SINGLE_PORT_ECC:
+		bit_mask = EX_DMA_SINGLE_PORT_ECC;
+		break;
+	case FMAN_EX_DMA_READ_ECC:
+		bit_mask = EX_DMA_READ_ECC;
+		break;
+	case FMAN_EX_DMA_SYSTEM_WRITE_ECC:
+		bit_mask = EX_DMA_SYSTEM_WRITE_ECC;
+		break;
+	case FMAN_EX_DMA_FM_WRITE_ECC:
+		bit_mask = EX_DMA_FM_WRITE_ECC;
+		break;
+	case FMAN_EX_FPM_STALL_ON_TASKS:
+		bit_mask = EX_FPM_STALL_ON_TASKS;
+		break;
+	case FMAN_EX_FPM_SINGLE_ECC:
+		bit_mask = EX_FPM_SINGLE_ECC;
+		break;
+	case FMAN_EX_FPM_DOUBLE_ECC:
+		bit_mask = EX_FPM_DOUBLE_ECC;
+		break;
+	case FMAN_EX_QMI_SINGLE_ECC:
+		bit_mask = EX_QMI_SINGLE_ECC;
+		break;
+	case FMAN_EX_QMI_DOUBLE_ECC:
+		bit_mask = EX_QMI_DOUBLE_ECC;
+		break;
+	case FMAN_EX_QMI_DEQ_FROM_UNKNOWN_PORTID:
+		bit_mask = EX_QMI_DEQ_FROM_UNKNOWN_PORTID;
+		break;
+	case FMAN_EX_BMI_LIST_RAM_ECC:
+		bit_mask = EX_BMI_LIST_RAM_ECC;
+		break;
+	case FMAN_EX_BMI_STORAGE_PROFILE_ECC:
+		bit_mask = EX_BMI_STORAGE_PROFILE_ECC;
+		break;
+	case FMAN_EX_BMI_STATISTICS_RAM_ECC:
+		bit_mask = EX_BMI_STATISTICS_RAM_ECC;
+		break;
+	case FMAN_EX_BMI_DISPATCH_RAM_ECC:
+		bit_mask = EX_BMI_DISPATCH_RAM_ECC;
+		break;
+	case FMAN_EX_MURAM_ECC:
+		bit_mask = EX_MURAM_ECC;
+		break;
+	default:
+		bit_mask = 0;
+		break;
+	}
+
+	return bit_mask;
+}
+
+static int get_module_event(enum fman_event_modules module, u8 mod_id,
+			    enum fman_intr_type intr_type)
+{
+	int event;
+
+	switch (module) {
+	case FMAN_MOD_MAC:
+		if (intr_type == FMAN_INTR_TYPE_ERR)
+			event = FMAN_EV_ERR_MAC0 + mod_id;
+		else
+			event = FMAN_EV_MAC0 + mod_id;
+		break;
+	case FMAN_MOD_FMAN_CTRL:
+		if (intr_type == FMAN_INTR_TYPE_ERR)
+			event = FMAN_EV_CNT;
+		else
+			event = (FMAN_EV_FMAN_CTRL_0 + mod_id);
+		break;
+	case FMAN_MOD_DUMMY_LAST:
+		event = FMAN_EV_CNT;
+		break;
+	default:
+		event = FMAN_EV_CNT;
+		break;
+	}
+
+	return event;
+}
+
+static int set_size_of_fifo(struct fman *fman, u8 port_id, u32 *size_of_fifo,
+			    u32 *extra_size_of_fifo)
+{
+	struct fman_bmi_regs __iomem *bmi_rg = fman->bmi_regs;
+	u32 fifo = *size_of_fifo;
+	u32 extra_fifo = *extra_size_of_fifo;
+	u32 tmp;
+
+	/* if this is the first time a port requires extra_fifo_pool_size,
+	 * the total extra_fifo_pool_size must be initialized to 1 buffer per
+	 * port
+	 */
+	if (extra_fifo && !fman->state->extra_fifo_pool_size)
+		fman->state->extra_fifo_pool_size =
+			fman->state->num_of_rx_ports * FMAN_BMI_FIFO_UNITS;
+
+	fman->state->extra_fifo_pool_size =
+		max(fman->state->extra_fifo_pool_size, extra_fifo);
+
+	/* check that there are enough uncommitted fifo size */
+	if ((fman->state->accumulated_fifo_size + fifo) >
+	    (fman->state->total_fifo_size -
+	    fman->state->extra_fifo_pool_size)) {
+		dev_err(fman->dev, "%s: Requested fifo size and extra size exceed total FIFO size.\n",
+			__func__);
+		return -EAGAIN;
+	}
+
+	/* Read, modify and write to HW */
+	tmp = (fifo / FMAN_BMI_FIFO_UNITS - 1) |
+	       ((extra_fifo / FMAN_BMI_FIFO_UNITS) <<
+	       BMI_EXTRA_FIFO_SIZE_SHIFT);
+	iowrite32be(tmp, &bmi_rg->fmbm_pfs[port_id - 1]);
+
+	/* update accumulated */
+	fman->state->accumulated_fifo_size += fifo;
+
+	return 0;
+}
+
+static int set_num_of_tasks(struct fman *fman, u8 port_id, u8 *num_of_tasks,
+			    u8 *num_of_extra_tasks)
+{
+	struct fman_bmi_regs __iomem *bmi_rg = fman->bmi_regs;
+	u8 tasks = *num_of_tasks;
+	u8 extra_tasks = *num_of_extra_tasks;
+	u32 tmp;
+
+	if (extra_tasks)
+		fman->state->extra_tasks_pool_size =
+		max(fman->state->extra_tasks_pool_size, extra_tasks);
+
+	/* check that there are enough uncommitted tasks */
+	if ((fman->state->accumulated_num_of_tasks + tasks) >
+	    (fman->state->total_num_of_tasks -
+	     fman->state->extra_tasks_pool_size)) {
+		dev_err(fman->dev, "%s: Requested num_of_tasks and extra tasks pool for fm%d exceed total num_of_tasks.\n",
+			__func__, fman->state->fm_id);
+		return -EAGAIN;
+	}
+	/* update accumulated */
+	fman->state->accumulated_num_of_tasks += tasks;
+
+	/* Write to HW */
+	tmp = ioread32be(&bmi_rg->fmbm_pp[port_id - 1]) &
+	    ~(BMI_NUM_OF_TASKS_MASK | BMI_NUM_OF_EXTRA_TASKS_MASK);
+	tmp |= ((u32)((tasks - 1) << BMI_NUM_OF_TASKS_SHIFT) |
+		(u32)(extra_tasks << BMI_EXTRA_NUM_OF_TASKS_SHIFT));
+	iowrite32be(tmp, &bmi_rg->fmbm_pp[port_id - 1]);
+
+	return 0;
+}
+
+static int set_num_of_open_dmas(struct fman *fman, u8 port_id,
+				u8 *num_of_open_dmas,
+				u8 *num_of_extra_open_dmas)
+{
+	struct fman_bmi_regs __iomem *bmi_rg = fman->bmi_regs;
+	u8 open_dmas = *num_of_open_dmas;
+	u8 extra_open_dmas = *num_of_extra_open_dmas;
+	u8 total_num_dmas = 0, current_val = 0, current_extra_val = 0;
+	u32 tmp;
+
+	if (!open_dmas) {
+		/* Configuration according to values in the HW.
+		 * read the current number of open Dma's
+		 */
+		tmp = ioread32be(&bmi_rg->fmbm_pp[port_id - 1]);
+		current_extra_val = (u8)((tmp & BMI_NUM_OF_EXTRA_DMAS_MASK) >>
+					 BMI_EXTRA_NUM_OF_DMAS_SHIFT);
+
+		tmp = ioread32be(&bmi_rg->fmbm_pp[port_id - 1]);
+		current_val = (u8)(((tmp & BMI_NUM_OF_DMAS_MASK) >>
+				   BMI_NUM_OF_DMAS_SHIFT) + 1);
+
+		/* This is the first configuration and user did not
+		 * specify value (!open_dmas), reset values will be used
+		 * and we just save these values for resource management
+		 */
+		fman->state->extra_open_dmas_pool_size =
+			(u8)max(fman->state->extra_open_dmas_pool_size,
+				current_extra_val);
+		fman->state->accumulated_num_of_open_dmas += current_val;
+		*num_of_open_dmas = current_val;
+		*num_of_extra_open_dmas = current_extra_val;
+		return 0;
+	}
+
+	if (extra_open_dmas > current_extra_val)
+		fman->state->extra_open_dmas_pool_size =
+		    (u8)max(fman->state->extra_open_dmas_pool_size,
+			    extra_open_dmas);
+
+	if ((fman->state->rev_info.major < 6) &&
+	    (fman->state->accumulated_num_of_open_dmas - current_val +
+	     open_dmas > fman->state->max_num_of_open_dmas)) {
+		dev_err(fman->dev, "%s: Requested num_of_open_dmas for fm%d exceeds total num_of_open_dmas.\n",
+			__func__, fman->state->fm_id);
+		return -EAGAIN;
+	} else if ((fman->state->rev_info.major >= 6) &&
+		   !((fman->state->rev_info.major == 6) &&
+		   (fman->state->rev_info.minor == 0)) &&
+		   (fman->state->accumulated_num_of_open_dmas -
+		   current_val + open_dmas >
+		   fman->state->dma_thresh_max_commq + 1)) {
+		dev_err(fman->dev, "%s: Requested num_of_open_dmas for fm%d exceeds DMA Command queue (%d)\n",
+			__func__, fman->state->fm_id,
+		       fman->state->dma_thresh_max_commq + 1);
+		return -EAGAIN;
+	}
+
+	WARN_ON(fman->state->accumulated_num_of_open_dmas < current_val);
+	/* update acummulated */
+	fman->state->accumulated_num_of_open_dmas -= current_val;
+	fman->state->accumulated_num_of_open_dmas += open_dmas;
+
+	if (fman->state->rev_info.major < 6)
+		total_num_dmas =
+		    (u8)(fman->state->accumulated_num_of_open_dmas +
+		    fman->state->extra_open_dmas_pool_size);
+
+	/* calculate reg */
+	tmp = ioread32be(&bmi_rg->fmbm_pp[port_id - 1]) &
+	    ~(BMI_NUM_OF_DMAS_MASK | BMI_NUM_OF_EXTRA_DMAS_MASK);
+	tmp |= (u32)(((open_dmas - 1) << BMI_NUM_OF_DMAS_SHIFT) |
+			   (extra_open_dmas << BMI_EXTRA_NUM_OF_DMAS_SHIFT));
+	iowrite32be(tmp, &bmi_rg->fmbm_pp[port_id - 1]);
+
+	/* update total num of DMA's with committed number of open DMAS,
+	 * and max uncommitted pool.
+	 */
+	if (total_num_dmas) {
+		tmp = ioread32be(&bmi_rg->fmbm_cfg2) & ~BMI_CFG2_DMAS_MASK;
+		tmp |= (u32)(total_num_dmas - 1) << BMI_CFG2_DMAS_SHIFT;
+		iowrite32be(tmp, &bmi_rg->fmbm_cfg2);
+	}
+
+	return 0;
+}
+
+static int fman_config(struct fman *fman)
+{
+	void __iomem *base_addr;
+	int err;
+
+	base_addr = fman->dts_params.base_addr;
+
+	fman->state = kzalloc(sizeof(*fman->state), GFP_KERNEL);
+	if (!fman->state)
+		goto err_fm_state;
+
+	/* Allocate the FM driver's parameters structure */
+	fman->cfg = kzalloc(sizeof(*fman->cfg), GFP_KERNEL);
+	if (!fman->cfg)
+		goto err_fm_drv;
+
+	/* Initialize MURAM block */
+	fman->muram =
+		fman_muram_init(fman->dts_params.muram_res.start,
+				resource_size(&fman->dts_params.muram_res));
+	if (!fman->muram)
+		goto err_fm_soc_specific;
+
+	/* Initialize FM parameters which will be kept by the driver */
+	fman->state->fm_id = fman->dts_params.id;
+	fman->state->fm_clk_freq = fman->dts_params.clk_freq;
+	fman->state->qman_channel_base = fman->dts_params.qman_channel_base;
+	fman->state->num_of_qman_channels =
+		fman->dts_params.num_of_qman_channels;
+	fman->state->res = fman->dts_params.res;
+	fman->exception_cb = fman_exceptions;
+	fman->bus_error_cb = fman_bus_error;
+	fman->fpm_regs = base_addr + FPM_OFFSET;
+	fman->bmi_regs = base_addr + BMI_OFFSET;
+	fman->qmi_regs = base_addr + QMI_OFFSET;
+	fman->dma_regs = base_addr + DMA_OFFSET;
+	fman->base_addr = base_addr;
+
+	spin_lock_init(&fman->spinlock);
+	fman_defconfig(fman->cfg);
+
+	fman->state->extra_fifo_pool_size = 0;
+	fman->state->exceptions = (EX_DMA_BUS_ERROR                 |
+					EX_DMA_READ_ECC              |
+					EX_DMA_SYSTEM_WRITE_ECC      |
+					EX_DMA_FM_WRITE_ECC          |
+					EX_FPM_STALL_ON_TASKS        |
+					EX_FPM_SINGLE_ECC            |
+					EX_FPM_DOUBLE_ECC            |
+					EX_QMI_DEQ_FROM_UNKNOWN_PORTID |
+					EX_BMI_LIST_RAM_ECC          |
+					EX_BMI_STORAGE_PROFILE_ECC   |
+					EX_BMI_STATISTICS_RAM_ECC    |
+					EX_MURAM_ECC                 |
+					EX_BMI_DISPATCH_RAM_ECC      |
+					EX_QMI_DOUBLE_ECC            |
+					EX_QMI_SINGLE_ECC);
+
+	/* Read FMan revision for future use*/
+	fman_get_revision(fman, &fman->state->rev_info);
+
+	err = fill_soc_specific_params(fman->state);
+	if (err)
+		goto err_fm_soc_specific;
+
+	/* FM_AID_MODE_NO_TNUM_SW005 Errata workaround */
+	if (fman->state->rev_info.major >= 6)
+		fman->cfg->dma_aid_mode = FMAN_DMA_AID_OUT_PORT_ID;
+
+	fman->cfg->qmi_def_tnums_thresh = fman->state->qmi_def_tnums_thresh;
+
+	fman->state->total_num_of_tasks =
+	(u8)DFLT_TOTAL_NUM_OF_TASKS(fman->state->rev_info.major,
+				    fman->state->rev_info.minor,
+				    fman->state->bmi_max_num_of_tasks);
+
+	if (fman->state->rev_info.major < 6) {
+		fman->cfg->dma_comm_qtsh_clr_emer =
+		(u8)DFLT_DMA_COMM_Q_LOW(fman->state->rev_info.major,
+					fman->state->dma_thresh_max_commq);
+
+		fman->cfg->dma_comm_qtsh_asrt_emer =
+		(u8)DFLT_DMA_COMM_Q_HIGH(fman->state->rev_info.major,
+					 fman->state->dma_thresh_max_commq);
+
+		fman->cfg->dma_cam_num_of_entries =
+		DFLT_DMA_CAM_NUM_OF_ENTRIES(fman->state->rev_info.major);
+
+		fman->cfg->dma_read_buf_tsh_clr_emer =
+		DFLT_DMA_READ_INT_BUF_LOW(fman->state->dma_thresh_max_buf);
+
+		fman->cfg->dma_read_buf_tsh_asrt_emer =
+		DFLT_DMA_READ_INT_BUF_HIGH(fman->state->dma_thresh_max_buf);
+
+		fman->cfg->dma_write_buf_tsh_clr_emer =
+		DFLT_DMA_WRITE_INT_BUF_LOW(fman->state->dma_thresh_max_buf);
+
+		fman->cfg->dma_write_buf_tsh_asrt_emer =
+		DFLT_DMA_WRITE_INT_BUF_HIGH(fman->state->dma_thresh_max_buf);
+
+		fman->cfg->dma_axi_dbg_num_of_beats =
+		DFLT_AXI_DBG_NUM_OF_BEATS;
+	}
+
+	return 0;
+
+err_fm_soc_specific:
+	kfree(fman->cfg);
+err_fm_drv:
+	kfree(fman->state);
+err_fm_state:
+	kfree(fman);
+	return -EINVAL;
+}
+
+static int fman_init(struct fman *fman)
+{
+	struct fman_cfg *cfg = NULL;
+	int err = 0, i, count;
+
+	if (is_init_done(fman->cfg))
+		return -EINVAL;
+
+	fman->state->count1_micro_bit = FM_TIMESTAMP_1_USEC_BIT;
+
+	cfg = fman->cfg;
+
+	/* clear revision-dependent non existing exception */
+	if (fman->state->rev_info.major < 6)
+		fman->state->exceptions &= ~FMAN_EX_BMI_DISPATCH_RAM_ECC;
+
+	if (fman->state->rev_info.major >= 6)
+		fman->state->exceptions &= ~FMAN_EX_QMI_SINGLE_ECC;
+
+	/* clear CPG */
+	memset_io((void __iomem *)(fman->base_addr + CGP_OFFSET), 0,
+		  fman->state->fm_port_num_of_cg);
+
+	/* Save LIODN info before FMan reset
+	 * Skipping non-existent port 0 (i = 1)
+	 */
+	for (i = 1; i < FMAN_LIODN_TBL; i++) {
+		u32 liodn_base;
+
+		fman->liodn_offset[i] =
+			ioread32be(&fman->bmi_regs->fmbm_spliodn[i - 1]);
+		liodn_base = ioread32be(&fman->dma_regs->fmdmplr[i / 2]);
+		if (i % 2) {
+			/* FMDM_PLR LSB holds LIODN base for odd ports */
+			liodn_base &= DMA_LIODN_BASE_MASK;
+		} else {
+			/* FMDM_PLR MSB holds LIODN base for even ports */
+			liodn_base >>= DMA_LIODN_SHIFT;
+			liodn_base &= DMA_LIODN_BASE_MASK;
+		}
+		fman->liodn_base[i] = liodn_base;
+	}
+
+	/* FMan Reset (supported only for FMan V2) */
+	if (fman->state->rev_info.major >= 6) {
+		/* Errata A007273 */
+		dev_dbg(fman->dev, "%s: FManV3 reset is not supported!\n",
+			__func__);
+	} else {
+		iowrite32be(FPM_RSTC_FM_RESET, &fman->fpm_regs->fm_rstc);
+		/* Wait for reset completion */
+		count = 100;
+		do {
+			udelay(1);
+		} while (((ioread32be(&fman->fpm_regs->fm_rstc)) &
+			 FPM_RSTC_FM_RESET) && --count);
+		if (count == 0)
+			return -EBUSY;
+	}
+
+	if (ioread32be(&fman->qmi_regs->fmqm_gs) & QMI_GS_HALT_NOT_BUSY) {
+		resume(fman->fpm_regs);
+		/* Wait until QMI is not in halt not busy state */
+		count = 100;
+		do {
+			udelay(1);
+		} while (((ioread32be(&fman->qmi_regs->fmqm_gs)) &
+			 QMI_GS_HALT_NOT_BUSY) && --count);
+		if (count == 0)
+			dev_warn(fman->dev, "%s: QMI is in halt not busy state\n",
+				 __func__);
+	}
+
+	if (clear_iram(fman) != 0)
+		return -EINVAL;
+
+	cfg->exceptions = fman->state->exceptions;
+
+	/* Init DMA Registers */
+
+	err = dma_init(fman);
+	if (err != 0) {
+		free_init_resources(fman);
+		return err;
+	}
+
+	/* Init FPM Registers */
+	fpm_init(fman->fpm_regs, fman->cfg);
+
+	/* define common resources */
+	/* 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)) {
+		free_init_resources(fman);
+		dev_err(fman->dev, "%s: MURAM alloc for BMI FIFO failed\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	cfg->fifo_base_addr = fman->fifo_offset;
+	cfg->total_fifo_size = fman->state->total_fifo_size;
+	cfg->total_num_of_tasks = fman->state->total_num_of_tasks;
+	cfg->clk_freq = fman->state->fm_clk_freq;
+
+	/* Init BMI Registers */
+	bmi_init(fman->bmi_regs, fman->cfg);
+
+	/* Init QMI Registers */
+	qmi_init(fman->qmi_regs, fman->cfg);
+
+	err = enable(fman, cfg);
+	if (err != 0)
+		return err;
+
+	enable_time_stamp(fman);
+
+	kfree(fman->cfg);
+	fman->cfg = NULL;
+
+	return 0;
+}
+
+static int fman_set_exception(struct fman *fman,
+			      enum fman_exceptions exception, bool enable)
+{
+	u32 bit_mask = 0;
+
+	if (!is_init_done(fman->cfg))
+		return -EINVAL;
+
+	bit_mask = get_exception_flag(exception);
+	if (bit_mask) {
+		if (enable)
+			fman->state->exceptions |= bit_mask;
+		else
+			fman->state->exceptions &= ~bit_mask;
+	} else {
+		dev_err(fman->dev, "%s: Undefined exception (%d)\n",
+			__func__, exception);
+		return -EINVAL;
+	}
+
+	return set_exception(fman, exception, enable);
+}
+
+/**
+ * fman_register_intr
+ * @fman:	A Pointer to FMan device
+ * @mod:	Calling module
+ * @mod_id:	Module id (if more than 1 exists, '0' if not)
+ * @intr_type:	Interrupt type (error/normal) selection.
+ * @f_isr:	The interrupt service routine.
+ * @h_src_arg:	Argument to be passed to f_isr.
+ *
+ * Used to register an event handler to be processed by FMan
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+void fman_register_intr(struct fman *fman, enum fman_event_modules module,
+			u8 mod_id, enum fman_intr_type intr_type,
+			void (*isr_cb)(void *src_arg), void *src_arg)
+{
+	int event = 0;
+
+	event = get_module_event(module, mod_id, intr_type);
+	WARN_ON(event >= FMAN_EV_CNT);
+
+	/* register in local FM structure */
+	fman->intr_mng[event].isr_cb = isr_cb;
+	fman->intr_mng[event].src_handle = src_arg;
+}
+
+/**
+ * fman_unregister_intr
+ * @fman:	A Pointer to FMan device
+ * @mod:	Calling module
+ * @mod_id:	Module id (if more than 1 exists, '0' if not)
+ * @intr_type:	Interrupt type (error/normal) selection.
+ *
+ * Used to unregister an event handler to be processed by FMan
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+void fman_unregister_intr(struct fman *fman, enum fman_event_modules module,
+			  u8 mod_id, enum fman_intr_type intr_type)
+{
+	int event = 0;
+
+	event = get_module_event(module, mod_id, intr_type);
+	WARN_ON(event >= FMAN_EV_CNT);
+
+	fman->intr_mng[event].isr_cb = NULL;
+	fman->intr_mng[event].src_handle = NULL;
+}
+
+/**
+ * fman_set_port_params
+ * @fman:		A Pointer to FMan device
+ * @port_params:	Port parameters
+ *
+ * Used by FMan Port to pass parameters to the FMan
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_set_port_params(struct fman *fman,
+			 struct fman_port_init_params *port_params)
+{
+	int err;
+	unsigned long flags;
+	u8 port_id = port_params->port_id, mac_id;
+
+	spin_lock_irqsave(&fman->spinlock, flags);
+
+	err = set_num_of_tasks(fman, port_params->port_id,
+			       &port_params->num_of_tasks,
+			       &port_params->num_of_extra_tasks);
+	if (err)
+		goto return_err;
+
+	/* TX Ports */
+	if (port_params->port_type != FMAN_PORT_TYPE_RX) {
+		u32 enq_th, deq_th, reg;
+
+		/* update qmi ENQ/DEQ threshold */
+		fman->state->accumulated_num_of_deq_tnums +=
+			port_params->deq_pipeline_depth;
+		enq_th = (ioread32be(&fman->qmi_regs->fmqm_gc) &
+			  QMI_CFG_ENQ_MASK) >> QMI_CFG_ENQ_SHIFT;
+		/* if enq_th is too big, we reduce it to the max value
+		 * that is still 0
+		 */
+		if (enq_th >= (fman->state->qmi_max_num_of_tnums -
+		    fman->state->accumulated_num_of_deq_tnums)) {
+			enq_th =
+			fman->state->qmi_max_num_of_tnums -
+			fman->state->accumulated_num_of_deq_tnums - 1;
+
+			reg = ioread32be(&fman->qmi_regs->fmqm_gc);
+			reg &= ~QMI_CFG_ENQ_MASK;
+			reg |= (enq_th << QMI_CFG_ENQ_SHIFT);
+			iowrite32be(reg, &fman->qmi_regs->fmqm_gc);
+		}
+
+		deq_th = ioread32be(&fman->qmi_regs->fmqm_gc) &
+				    QMI_CFG_DEQ_MASK;
+		/* if deq_th is too small, we enlarge it to the min
+		 * value that is still 0.
+		 * depTh may not be larger than 63
+		 * (fman->state->qmi_max_num_of_tnums-1).
+		 */
+		if ((deq_th <= fman->state->accumulated_num_of_deq_tnums) &&
+		    (deq_th < fman->state->qmi_max_num_of_tnums - 1)) {
+			deq_th = fman->state->accumulated_num_of_deq_tnums + 1;
+			reg = ioread32be(&fman->qmi_regs->fmqm_gc);
+			reg &= ~QMI_CFG_DEQ_MASK;
+			reg |= deq_th;
+			iowrite32be(reg, &fman->qmi_regs->fmqm_gc);
+		}
+	}
+
+	err = set_size_of_fifo(fman, port_params->port_id,
+			       &port_params->size_of_fifo,
+			       &port_params->extra_size_of_fifo);
+	if (err)
+		goto return_err;
+
+	err = set_num_of_open_dmas(fman, port_params->port_id,
+				   &port_params->num_of_open_dmas,
+				   &port_params->num_of_extra_open_dmas);
+	if (err)
+		goto return_err;
+
+	set_port_liodn(fman, port_id, fman->liodn_base[port_id],
+		       fman->liodn_offset[port_id]);
+
+	if (fman->state->rev_info.major < 6)
+		set_port_order_restoration(fman->fpm_regs, port_id);
+
+	mac_id = hw_port_id_to_sw_port_id(fman->state->rev_info.major, port_id);
+
+	if (port_params->max_frame_length >= fman->state->mac_mfl[mac_id]) {
+		fman->state->port_mfl[mac_id] = port_params->max_frame_length;
+	} else {
+		dev_warn(fman->dev, "%s: Port (%d) max_frame_length is smaller than MAC (%d) current MTU\n",
+			 __func__, port_id, mac_id);
+		err = -EINVAL;
+		goto return_err;
+	}
+
+	spin_unlock_irqrestore(&fman->spinlock, flags);
+
+	return 0;
+
+return_err:
+	spin_unlock_irqrestore(&fman->spinlock, flags);
+	return err;
+}
+
+/**
+ * fman_reset_mac
+ * @fman:	A Pointer to FMan device
+ * @mac_id:	MAC id to be reset
+ *
+ * Reset a specific MAC
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_reset_mac(struct fman *fman, u8 mac_id)
+{
+	struct fman_fpm_regs __iomem *fpm_rg = fman->fpm_regs;
+	u32 msk, timeout = 100;
+
+	if (fman->state->rev_info.major >= 6) {
+		dev_err(fman->dev, "%s: FMan MAC reset no available for FMan V3!\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Get the relevant bit mask */
+	switch (mac_id) {
+	case 0:
+		msk = FPM_RSTC_MAC0_RESET;
+		break;
+	case 1:
+		msk = FPM_RSTC_MAC1_RESET;
+		break;
+	case 2:
+		msk = FPM_RSTC_MAC2_RESET;
+		break;
+	case 3:
+		msk = FPM_RSTC_MAC3_RESET;
+		break;
+	case 4:
+		msk = FPM_RSTC_MAC4_RESET;
+		break;
+	case 5:
+		msk = FPM_RSTC_MAC5_RESET;
+		break;
+	case 6:
+		msk = FPM_RSTC_MAC6_RESET;
+		break;
+	case 7:
+		msk = FPM_RSTC_MAC7_RESET;
+		break;
+	case 8:
+		msk = FPM_RSTC_MAC8_RESET;
+		break;
+	case 9:
+		msk = FPM_RSTC_MAC9_RESET;
+		break;
+	default:
+		dev_warn(fman->dev, "%s: Illegal MAC Id [%d]\n",
+			 __func__, mac_id);
+		return -EINVAL;
+	}
+
+	/* reset */
+	iowrite32be(msk, &fpm_rg->fm_rstc);
+	while ((ioread32be(&fpm_rg->fm_rstc) & msk) && --timeout)
+		udelay(10);
+
+	if (!timeout)
+		return -EIO;
+
+	return 0;
+}
+
+/**
+ * fman_set_mac_max_frame
+ * @fman:	A Pointer to FMan device
+ * @mac_id:	MAC id
+ * @mfl:	Maximum frame length
+ *
+ * Set maximum frame length of specific MAC in FMan driver
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_set_mac_max_frame(struct fman *fman, u8 mac_id, u16 mfl)
+{
+	/* if port is already initialized, check that MaxFrameLength is smaller
+	 * 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]))) {
+		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",
+			 __func__);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/**
+ * fman_get_clock_freq
+ * @fman:	A Pointer to FMan device
+ *
+ * Get FMan clock frequency
+ *
+ * Return: FMan clock frequency
+ */
+u16 fman_get_clock_freq(struct fman *fman)
+{
+	return fman->state->fm_clk_freq;
+}
+
+/**
+ * fman_get_bmi_max_fifo_size
+ * @fman:	A Pointer to FMan device
+ *
+ * Get FMan maximum FIFO size
+ *
+ * Return: FMan Maximum FIFO size
+ */
+u32 fman_get_bmi_max_fifo_size(struct fman *fman)
+{
+	return fman->state->bmi_max_fifo_size;
+}
+
+/**
+ * fman_get_revision
+ * @fman		- Pointer to the FMan module
+ * @rev_info		- A structure of revision information parameters.
+ *
+ * Returns the FM revision
+ *
+ * Allowed only following fman_init().
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+void fman_get_revision(struct fman *fman, struct fman_rev_info *rev_info)
+{
+	u32 tmp;
+
+	tmp = ioread32be(&fman->fpm_regs->fm_ip_rev_1);
+	rev_info->major = (u8)((tmp & FPM_REV1_MAJOR_MASK) >>
+				FPM_REV1_MAJOR_SHIFT);
+	rev_info->minor = tmp & FPM_REV1_MINOR_MASK;
+}
+
+/**
+ * fman_get_qman_channel_id
+ * @fman:	A Pointer to FMan device
+ * @port_id:	Port id
+ *
+ * Get QMan channel ID associated to the Port id
+ *
+ * Return: QMan channel ID
+ */
+u32 fman_get_qman_channel_id(struct fman *fman, u32 port_id)
+{
+	int i;
+
+	if (fman->state->rev_info.major >= 6) {
+		u32 port_ids[] = {0x30, 0x31, 0x28, 0x29, 0x2a, 0x2b,
+				  0x2c, 0x2d, 0x2, 0x3, 0x4, 0x5, 0x7, 0x7};
+		for (i = 0; i < fman->state->num_of_qman_channels; i++) {
+			if (port_ids[i] == port_id)
+				break;
+		}
+	} else {
+		u32 port_ids[] = {0x30, 0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x1,
+				  0x2, 0x3, 0x4, 0x5, 0x7, 0x7};
+		for (i = 0; i < fman->state->num_of_qman_channels; i++) {
+			if (port_ids[i] == port_id)
+				break;
+		}
+	}
+
+	if (i == fman->state->num_of_qman_channels)
+		return 0;
+
+	return fman->state->qman_channel_base + i;
+}
+
+/**
+ * fman_get_mem_region
+ * @fman:	A Pointer to FMan device
+ *
+ * Get FMan memory region
+ *
+ * Return: A structure with FMan memory region information
+ */
+struct resource *fman_get_mem_region(struct fman *fman)
+{
+	return fman->state->res;
+}
+
+/* Bootargs defines */
+/* Extra headroom for RX buffers - Default, min and max */
+#define FSL_FM_RX_EXTRA_HEADROOM	64
+#define FSL_FM_RX_EXTRA_HEADROOM_MIN	16
+#define FSL_FM_RX_EXTRA_HEADROOM_MAX	384
+
+/* Maximum frame length */
+#define FSL_FM_MAX_FRAME_SIZE			1522
+#define FSL_FM_MAX_POSSIBLE_FRAME_SIZE		9600
+#define FSL_FM_MIN_POSSIBLE_FRAME_SIZE		64
+
+/* Extra headroom for Rx buffers.
+ * FMan is instructed to allocate, on the Rx path, this amount of
+ * space at the beginning of a data buffer, beside the DPA private
+ * data area and the IC fields.
+ * Does not impact Tx buffer layout.
+ * Configurable from bootargs. 64 by default, it's needed on
+ * particular forwarding scenarios that add extra headers to the
+ * forwarded frame.
+ */
+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");
+
+/* Max frame size, across all interfaces.
+ * Configurable from bootargs, to avoid allocating oversized (socket)
+ * buffers when not using jumbo frames.
+ * Must be large enough to accommodate the network MTU, but small enough
+ * to avoid wasting skb memory.
+ *
+ * Could be overridden once, at boot-time, via the
+ * fm_set_max_frm() callback.
+ */
+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");
+
+/**
+ * fman_get_max_frm
+ *
+ * Return: Max frame length configured in the FM driver
+ */
+u16 fman_get_max_frm(void)
+{
+	static bool fm_check_mfl;
+
+	if (!fm_check_mfl) {
+		if (fsl_fm_max_frm > FSL_FM_MAX_POSSIBLE_FRAME_SIZE ||
+		    fsl_fm_max_frm < FSL_FM_MIN_POSSIBLE_FRAME_SIZE) {
+			pr_warn("Invalid fsl_fm_max_frm value (%d) in bootargs, valid range is %d-%d. Falling back to the default (%d)\n",
+				fsl_fm_max_frm,
+				FSL_FM_MIN_POSSIBLE_FRAME_SIZE,
+				FSL_FM_MAX_POSSIBLE_FRAME_SIZE,
+				FSL_FM_MAX_FRAME_SIZE);
+			fsl_fm_max_frm = FSL_FM_MAX_FRAME_SIZE;
+		}
+		fm_check_mfl = true;
+	}
+
+	return fsl_fm_max_frm;
+}
+EXPORT_SYMBOL(fman_get_max_frm);
+
+/**
+ * fman_get_rx_extra_headroom
+ *
+ * Return: Extra headroom size configured in the FM driver
+ */
+int fman_get_rx_extra_headroom(void)
+{
+	static bool fm_check_rx_extra_headroom;
+
+	if (!fm_check_rx_extra_headroom) {
+		if (fsl_fm_rx_extra_headroom > FSL_FM_RX_EXTRA_HEADROOM_MAX ||
+		    fsl_fm_rx_extra_headroom < FSL_FM_RX_EXTRA_HEADROOM_MIN) {
+			pr_warn("Invalid fsl_fm_rx_extra_headroom value (%d) in bootargs, valid range is %d-%d. Falling back to the default (%d)\n",
+				fsl_fm_rx_extra_headroom,
+				FSL_FM_RX_EXTRA_HEADROOM_MIN,
+				FSL_FM_RX_EXTRA_HEADROOM_MAX,
+				FSL_FM_RX_EXTRA_HEADROOM);
+			fsl_fm_rx_extra_headroom = FSL_FM_RX_EXTRA_HEADROOM;
+		}
+
+		fm_check_rx_extra_headroom = true;
+		fsl_fm_rx_extra_headroom = ALIGN(fsl_fm_rx_extra_headroom, 16);
+	}
+
+	return fsl_fm_rx_extra_headroom;
+}
+EXPORT_SYMBOL(fman_get_rx_extra_headroom);
+
+/**
+ * fman_bind
+ * @dev:	FMan OF device pointer
+ *
+ * Bind to a specific FMan device.
+ *
+ * Allowed only after the port was created.
+ *
+ * Return: A pointer to the FMan device
+ */
+struct fman *fman_bind(struct device *fm_dev)
+{
+	return (struct fman *)(dev_get_drvdata(get_device(fm_dev)));
+}
+
+static irqreturn_t fman_err_irq(int irq, void *handle)
+{
+	struct fman *fman = (struct fman *)handle;
+	u32 pending;
+	struct fman_fpm_regs __iomem *fpm_rg;
+	irqreturn_t single_ret, ret = IRQ_NONE;
+
+	if (!is_init_done(fman->cfg))
+		return IRQ_NONE;
+
+	fpm_rg = fman->fpm_regs;
+
+	/* error interrupts */
+	pending = ioread32be(&fpm_rg->fm_epi);
+	if (!pending)
+		return IRQ_NONE;
+
+	if (pending & ERR_INTR_EN_BMI) {
+		single_ret = bmi_err_event(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_QMI) {
+		single_ret = qmi_err_event(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_FPM) {
+		single_ret = fpm_err_event(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_DMA) {
+		single_ret = dma_err_event(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MURAM) {
+		single_ret = muram_err_intr(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	/* MAC error interrupts */
+	if (pending & ERR_INTR_EN_MAC0) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 0);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC1) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 1);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC2) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 2);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC3) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 3);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC4) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 4);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC5) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 5);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC6) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 6);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC7) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 7);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC8) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 8);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & ERR_INTR_EN_MAC9) {
+		single_ret = call_mac_isr(fman, FMAN_EV_ERR_MAC0 + 9);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static irqreturn_t fman_irq(int irq, void *handle)
+{
+	struct fman *fman = (struct fman *)handle;
+	u32 pending;
+	struct fman_fpm_regs __iomem *fpm_rg;
+	irqreturn_t single_ret, ret = IRQ_NONE;
+
+	if (!is_init_done(fman->cfg))
+		return IRQ_NONE;
+
+	fpm_rg = fman->fpm_regs;
+
+	/* normal interrupts */
+	pending = ioread32be(&fpm_rg->fm_npi);
+	if (!pending)
+		return IRQ_NONE;
+
+	if (pending & INTR_EN_QMI) {
+		single_ret = qmi_event(fman);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	/* MAC interrupts */
+	if (pending & INTR_EN_MAC0) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 0);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC1) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 1);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC2) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 2);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC3) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 3);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC4) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 4);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC5) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 5);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC6) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 6);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC7) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 7);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC8) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 8);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+	if (pending & INTR_EN_MAC9) {
+		single_ret = call_mac_isr(fman, FMAN_EV_MAC0 + 9);
+		if (single_ret == IRQ_HANDLED)
+			ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static const struct of_device_id fman_muram_match[] = {
+	{
+		.compatible = "fsl,fman-muram"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, fman_muram_match);
+
+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;
+	struct clk *clk;
+	u32 clk_rate;
+	phys_addr_t phys_base_addr;
+	resource_size_t mem_size;
+
+	fman = kzalloc(sizeof(*fman), GFP_KERNEL);
+	if (!fman)
+		return NULL;
+
+	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",
+			__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]);
+
+	/* Get the FM interrupt */
+	res = platform_get_resource(of_dev, IORESOURCE_IRQ, 0);
+	if (!res) {
+		dev_err(&of_dev->dev, "%s: Can't get FMan IRQ resource\n",
+			__func__);
+		goto fman_node_put;
+	}
+	irq = res->start;
+
+	/* Get the FM error interrupt */
+	res = platform_get_resource(of_dev, IORESOURCE_IRQ, 1);
+	if (!res) {
+		dev_err(&of_dev->dev, "%s: Can't get FMan Error IRQ resource\n",
+			__func__);
+		goto fman_node_put;
+	}
+	fman->dts_params.err_irq = res->start;
+
+	/* Get the FM address */
+	res = platform_get_resource(of_dev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&of_dev->dev, "%s: Can't get FMan memory resouce\n",
+			__func__);
+		goto fman_node_put;
+	}
+
+	phys_base_addr = res->start;
+	mem_size = resource_size(res);
+
+	clk = of_clk_get(fm_node, 0);
+	if (IS_ERR(clk)) {
+		dev_err(&of_dev->dev, "%s: Failed to get FM%d clock structure\n",
+			__func__, fman->dts_params.id);
+		goto fman_node_put;
+	}
+
+	clk_rate = clk_get_rate(clk);
+	if (!clk_rate) {
+		dev_err(&of_dev->dev, "%s: Failed to determine FM%d clock rate\n",
+			__func__, fman->dts_params.id);
+		goto fman_node_put;
+	}
+	/* 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",
+			__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]);
+
+	/* Get the MURAM base address and size */
+	muram_node = of_find_matching_node(fm_node, fman_muram_match);
+	if (!muram_node) {
+		dev_err(&of_dev->dev, "%s: could not find MURAM node\n",
+			__func__);
+		goto fman_node_put;
+	}
+
+	err = of_address_to_resource(muram_node, 0,
+				     &fman->dts_params.muram_res);
+	if (err) {
+		of_node_put(muram_node);
+		dev_err(&of_dev->dev, "%s: of_address_to_resource() = %d\n",
+			__func__, err);
+		goto fman_node_put;
+	}
+
+	of_node_put(muram_node);
+	of_node_put(fm_node);
+
+	err = devm_request_irq(&of_dev->dev, irq, fman_irq, 0, "fman", fman);
+	if (err < 0) {
+		dev_err(&of_dev->dev, "%s: irq %d allocation failed (error = %d)\n",
+			__func__, irq, err);
+		goto fman_free;
+	}
+
+	if (fman->dts_params.err_irq != 0) {
+		err = devm_request_irq(&of_dev->dev, fman->dts_params.err_irq,
+				       fman_err_irq, IRQF_SHARED,
+				       "fman-err", fman);
+		if (err < 0) {
+			dev_err(&of_dev->dev, "%s: irq %d allocation failed (error = %d)\n",
+				__func__, fman->dts_params.err_irq, err);
+			goto fman_free;
+		}
+	}
+
+	fman->dts_params.res =
+		devm_request_mem_region(&of_dev->dev, phys_base_addr,
+					mem_size, "fman");
+	if (!fman->dts_params.res) {
+		dev_err(&of_dev->dev, "%s: request_mem_region() failed\n",
+			__func__);
+		goto fman_free;
+	}
+
+	fman->dts_params.base_addr =
+		devm_ioremap(&of_dev->dev, phys_base_addr, mem_size);
+	if (fman->dts_params.base_addr == 0) {
+		dev_err(&of_dev->dev, "%s: devm_ioremap() failed\n", __func__);
+		goto fman_free;
+	}
+
+	fman->dev = &of_dev->dev;
+
+	return fman;
+
+fman_node_put:
+	of_node_put(fm_node);
+fman_free:
+	kfree(fman);
+	return NULL;
+}
+
+static int fman_probe(struct platform_device *of_dev)
+{
+	struct fman *fman;
+	struct device *dev;
+	int err;
+
+	dev = &of_dev->dev;
+
+	fman = read_dts_node(of_dev);
+	if (!fman)
+		return -EIO;
+
+	err = fman_config(fman);
+	if (err) {
+		dev_err(dev, "%s: FMan config failed\n", __func__);
+		return -EINVAL;
+	}
+
+	if (fman_init(fman) != 0) {
+		dev_err(dev, "%s: FMan init failed\n", __func__);
+		return -EINVAL;
+	}
+
+	if (fman->dts_params.err_irq == 0) {
+		fman_set_exception(fman, FMAN_EX_DMA_BUS_ERROR, false);
+		fman_set_exception(fman, FMAN_EX_DMA_READ_ECC, false);
+		fman_set_exception(fman, FMAN_EX_DMA_SYSTEM_WRITE_ECC, false);
+		fman_set_exception(fman, FMAN_EX_DMA_FM_WRITE_ECC, false);
+		fman_set_exception(fman, FMAN_EX_DMA_SINGLE_PORT_ECC, false);
+		fman_set_exception(fman, FMAN_EX_FPM_STALL_ON_TASKS, false);
+		fman_set_exception(fman, FMAN_EX_FPM_SINGLE_ECC, false);
+		fman_set_exception(fman, FMAN_EX_FPM_DOUBLE_ECC, false);
+		fman_set_exception(fman, FMAN_EX_QMI_SINGLE_ECC, false);
+		fman_set_exception(fman, FMAN_EX_QMI_DOUBLE_ECC, false);
+		fman_set_exception(fman,
+				   FMAN_EX_QMI_DEQ_FROM_UNKNOWN_PORTID, false);
+		fman_set_exception(fman, FMAN_EX_BMI_LIST_RAM_ECC, false);
+		fman_set_exception(fman, FMAN_EX_BMI_STORAGE_PROFILE_ECC,
+				   false);
+		fman_set_exception(fman, FMAN_EX_BMI_STATISTICS_RAM_ECC, false);
+		fman_set_exception(fman, FMAN_EX_BMI_DISPATCH_RAM_ECC, false);
+	}
+
+	dev_set_drvdata(dev, fman);
+
+	dev_dbg(dev, "FMan%d probed\n", fman->dts_params.id);
+
+	return 0;
+}
+
+static const struct of_device_id fman_match[] = {
+	{
+		.compatible = "fsl,fman"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, fm_match);
+
+static struct platform_driver fman_driver = {
+	.driver = {
+		.name = "fsl-fman",
+		.of_match_table = fman_match,
+	},
+	.probe = fman_probe,
+};
+
+builtin_platform_driver(fman_driver);
diff --git a/drivers/net/ethernet/freescale/fman/fman.h b/drivers/net/ethernet/freescale/fman/fman.h
new file mode 100644
index 000000000000..57aae8d17d77
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman.h
@@ -0,0 +1,325 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __FM_H
+#define __FM_H
+
+#include <linux/io.h>
+
+/* FM Frame descriptor macros  */
+/* Frame queue Context Override */
+#define FM_FD_CMD_FCO                   0x80000000
+#define FM_FD_CMD_RPD                   0x40000000  /* Read Prepended Data */
+#define FM_FD_CMD_DTC                   0x10000000  /* Do L4 Checksum */
+
+/* TX-Port: Unsupported Format */
+#define FM_FD_ERR_UNSUPPORTED_FORMAT    0x04000000
+/* TX Port: Length Error */
+#define FM_FD_ERR_LENGTH                0x02000000
+#define FM_FD_ERR_DMA                   0x01000000  /* DMA Data error */
+
+/* IPR frame (not error) */
+#define FM_FD_IPR                       0x00000001
+/* IPR non-consistent-sp */
+#define FM_FD_ERR_IPR_NCSP              (0x00100000 | FM_FD_IPR)
+/* IPR error */
+#define FM_FD_ERR_IPR                   (0x00200000 | FM_FD_IPR)
+/* IPR timeout */
+#define FM_FD_ERR_IPR_TO                (0x00300000 | FM_FD_IPR)
+/* TX Port: Length Error */
+#define FM_FD_ERR_IPRE                  (FM_FD_ERR_IPR & ~FM_FD_IPR)
+
+/* Rx FIFO overflow, FCS error, code error, running disparity error
+ * (SGMII and TBI modes), FIFO parity error. PHY Sequence error,
+ * PHY error control character detected.
+ */
+#define FM_FD_ERR_PHYSICAL              0x00080000
+/* Frame too long OR Frame size exceeds max_length_frame  */
+#define FM_FD_ERR_SIZE                  0x00040000
+/* classification discard */
+#define FM_FD_ERR_CLS_DISCARD           0x00020000
+/* Extract Out of Frame */
+#define FM_FD_ERR_EXTRACTION            0x00008000
+/* No Scheme Selected */
+#define FM_FD_ERR_NO_SCHEME             0x00004000
+/* Keysize Overflow */
+#define FM_FD_ERR_KEYSIZE_OVERFLOW      0x00002000
+/* Frame color is red */
+#define FM_FD_ERR_COLOR_RED             0x00000800
+/* Frame color is yellow */
+#define FM_FD_ERR_COLOR_YELLOW          0x00000400
+/* Parser Time out Exceed */
+#define FM_FD_ERR_PRS_TIMEOUT           0x00000080
+/* Invalid Soft Parser instruction */
+#define FM_FD_ERR_PRS_ILL_INSTRUCT      0x00000040
+/* Header error was identified during parsing */
+#define FM_FD_ERR_PRS_HDR_ERR           0x00000020
+/* Frame parsed beyind 256 first bytes */
+#define FM_FD_ERR_BLOCK_LIMIT_EXCEEDED  0x00000008
+
+/* non Frame-Manager error */
+#define FM_FD_RX_STATUS_ERR_NON_FM      0x00400000
+
+/* FMan driver defines */
+#define FMAN_BMI_FIFO_UNITS		0x100
+#define OFFSET_UNITS			16
+
+/* BMan defines */
+#define BM_MAX_NUM_OF_POOLS		64 /* Buffers pools */
+#define FMAN_PORT_MAX_EXT_POOLS_NUM	8  /* External BM pools per Rx port */
+
+struct fman; /* FMan data */
+
+/* Enum for defining port types */
+enum fman_port_type {
+	FMAN_PORT_TYPE_TX = 0,	/* TX Port */
+	FMAN_PORT_TYPE_RX,	/* RX Port */
+};
+
+struct fman_rev_info {
+	u8 major;			/* Major revision */
+	u8 minor;			/* Minor revision */
+};
+
+enum fman_exceptions {
+	FMAN_EX_DMA_BUS_ERROR = 0,	/* DMA bus error. */
+	FMAN_EX_DMA_READ_ECC,		/* Read Buffer ECC error */
+	FMAN_EX_DMA_SYSTEM_WRITE_ECC,	/* Write Buffer ECC err on sys side */
+	FMAN_EX_DMA_FM_WRITE_ECC,	/* Write Buffer ECC error on FM side */
+	FMAN_EX_DMA_SINGLE_PORT_ECC,	/* Single Port ECC error on FM side */
+	FMAN_EX_FPM_STALL_ON_TASKS,	/* Stall of tasks on FPM */
+	FMAN_EX_FPM_SINGLE_ECC,		/* Single ECC on FPM. */
+	FMAN_EX_FPM_DOUBLE_ECC,		/* Double ECC error on FPM ram access */
+	FMAN_EX_QMI_SINGLE_ECC,	/* Single ECC on QMI. */
+	FMAN_EX_QMI_DOUBLE_ECC,	/* Double bit ECC occurred on QMI */
+	FMAN_EX_QMI_DEQ_FROM_UNKNOWN_PORTID,/* DeQ from unknown port id */
+	FMAN_EX_BMI_LIST_RAM_ECC,	/* Linked List RAM ECC error */
+	FMAN_EX_BMI_STORAGE_PROFILE_ECC,/* storage profile */
+	FMAN_EX_BMI_STATISTICS_RAM_ECC,/* Statistics RAM ECC Err Enable */
+	FMAN_EX_BMI_DISPATCH_RAM_ECC,	/* Dispatch RAM ECC Error Enable */
+	FMAN_EX_IRAM_ECC,		/* Double bit ECC occurred on IRAM */
+	FMAN_EX_MURAM_ECC		/* Double bit ECC occurred on MURAM */
+};
+
+/* Parse results memory layout */
+struct fman_prs_result {
+	u8 lpid;		/* Logical port id */
+	u8 shimr;		/* Shim header result  */
+	u16 l2r;		/* Layer 2 result */
+	u16 l3r;		/* Layer 3 result */
+	u8 l4r;		/* Layer 4 result */
+	u8 cplan;		/* Classification plan id */
+	u16 nxthdr;		/* Next Header  */
+	u16 cksum;		/* Running-sum */
+	/* Flags&fragment-offset field of the last IP-header */
+	u16 flags_frag_off;
+	/* Routing type field of a IPV6 routing extension header */
+	u8 route_type;
+	/* Routing Extension Header Present; last bit is IP valid */
+	u8 rhp_ip_valid;
+	u8 shim_off[2];		/* Shim offset */
+	u8 ip_pid_off;		/* IP PID (last IP-proto) offset */
+	u8 eth_off;		/* ETH offset */
+	u8 llc_snap_off;	/* LLC_SNAP offset */
+	u8 vlan_off[2];		/* VLAN offset */
+	u8 etype_off;		/* ETYPE offset */
+	u8 pppoe_off;		/* PPP offset */
+	u8 mpls_off[2];		/* MPLS offset */
+	u8 ip_off[2];		/* IP offset */
+	u8 gre_off;		/* GRE offset */
+	u8 l4_off;		/* Layer 4 offset */
+	u8 nxthdr_off;		/* Parser end point */
+};
+
+/* A structure for defining buffer prefix area content. */
+struct fman_buffer_prefix_content {
+	/* Number of bytes to be left at the beginning of the external
+	 * buffer; Note that the private-area will start from the base
+	 * of the buffer address.
+	 */
+	u16 priv_data_size;
+	/* true to pass the parse result to/from the FM;
+	 * User may use FM_PORT_GetBufferPrsResult() in
+	 * order to get the parser-result from a buffer.
+	 */
+	bool pass_prs_result;
+	/* true to pass the timeStamp to/from the FM User */
+	bool pass_time_stamp;
+	/* true to pass the KG hash result to/from the FM User may
+	 * use FM_PORT_GetBufferHashResult() in order to get the
+	 * parser-result from a buffer.
+	 */
+	bool pass_hash_result;
+	/* Add all other Internal-Context information: AD,
+	 * hash-result, key, etc.
+	 */
+	u16 data_align;
+};
+
+/* A structure of information about each of the external
+ * buffer pools used by a port or storage-profile.
+ */
+struct fman_ext_pool_params {
+	u8 id;		    /* External buffer pool id */
+	u16 size;		    /* External buffer pool buffer size */
+};
+
+/* A structure for informing the driver about the external
+ * buffer pools allocated in the BM and used by a port or a
+ * storage-profile.
+ */
+struct fman_ext_pools {
+	u8 num_of_pools_used; /* Number of pools use by this port */
+	struct fman_ext_pool_params ext_buf_pool[FMAN_PORT_MAX_EXT_POOLS_NUM];
+					/* Parameters for each port */
+};
+
+/* A structure for defining BM pool depletion criteria */
+struct fman_buf_pool_depletion {
+	/* select mode in which pause frames will be sent after a
+	 * number of pools (all together!) are depleted
+	 */
+	bool pools_grp_mode_enable;
+	/* the number of depleted pools that will invoke pause
+	 * frames transmission.
+	 */
+	u8 num_of_pools;
+	/* For each pool, true if it should be considered for
+	 * depletion (Note - this pool must be used by this port!).
+	 */
+	bool pools_to_consider[BM_MAX_NUM_OF_POOLS];
+	/* select mode in which pause frames will be sent
+	 * after a single-pool is depleted;
+	 */
+	bool single_pool_mode_enable;
+	/* For each pool, true if it should be considered
+	 * for depletion (Note - this pool must be used by this port!)
+	 */
+	bool pools_to_consider_for_single_mode[BM_MAX_NUM_OF_POOLS];
+};
+
+/* Enum for inter-module interrupts registration */
+enum fman_event_modules {
+	FMAN_MOD_MAC = 0,		/* MAC event */
+	FMAN_MOD_FMAN_CTRL,	/* FMAN Controller */
+	FMAN_MOD_DUMMY_LAST
+};
+
+/* Enum for interrupts types */
+enum fman_intr_type {
+	FMAN_INTR_TYPE_ERR,
+	FMAN_INTR_TYPE_NORMAL
+};
+
+/* Enum for inter-module interrupts registration */
+enum fman_inter_module_event {
+	FMAN_EV_ERR_MAC0 = 0,	/* MAC 0 error event */
+	FMAN_EV_ERR_MAC1,		/* MAC 1 error event */
+	FMAN_EV_ERR_MAC2,		/* MAC 2 error event */
+	FMAN_EV_ERR_MAC3,		/* MAC 3 error event */
+	FMAN_EV_ERR_MAC4,		/* MAC 4 error event */
+	FMAN_EV_ERR_MAC5,		/* MAC 5 error event */
+	FMAN_EV_ERR_MAC6,		/* MAC 6 error event */
+	FMAN_EV_ERR_MAC7,		/* MAC 7 error event */
+	FMAN_EV_ERR_MAC8,		/* MAC 8 error event */
+	FMAN_EV_ERR_MAC9,		/* MAC 9 error event */
+	FMAN_EV_MAC0,		/* MAC 0 event (Magic packet detection) */
+	FMAN_EV_MAC1,		/* MAC 1 event (Magic packet detection) */
+	FMAN_EV_MAC2,		/* MAC 2 (Magic packet detection) */
+	FMAN_EV_MAC3,		/* MAC 3 (Magic packet detection) */
+	FMAN_EV_MAC4,		/* MAC 4 (Magic packet detection) */
+	FMAN_EV_MAC5,		/* MAC 5 (Magic packet detection) */
+	FMAN_EV_MAC6,		/* MAC 6 (Magic packet detection) */
+	FMAN_EV_MAC7,		/* MAC 7 (Magic packet detection) */
+	FMAN_EV_MAC8,		/* MAC 8 event (Magic packet detection) */
+	FMAN_EV_MAC9,		/* MAC 9 event (Magic packet detection) */
+	FMAN_EV_FMAN_CTRL_0,	/* Fman controller event 0 */
+	FMAN_EV_FMAN_CTRL_1,	/* Fman controller event 1 */
+	FMAN_EV_FMAN_CTRL_2,	/* Fman controller event 2 */
+	FMAN_EV_FMAN_CTRL_3,	/* Fman controller event 3 */
+	FMAN_EV_CNT
+};
+
+struct fman_intr_src {
+	void (*isr_cb)(void *src_arg);
+	void *src_handle;
+};
+
+/* Structure for port-FM communication during fman_port_init. */
+struct fman_port_init_params {
+	u8 port_id;			/* port Id */
+	enum fman_port_type port_type;	/* Port type */
+	u16 port_speed;			/* Port speed */
+	u16 liodn_offset;		/* Port's requested resource */
+	u8 num_of_tasks;		/* Port's requested resource */
+	u8 num_of_extra_tasks;		/* Port's requested resource */
+	u8 num_of_open_dmas;		/* Port's requested resource */
+	u8 num_of_extra_open_dmas;	/* Port's requested resource */
+	u32 size_of_fifo;		/* Port's requested resource */
+	u32 extra_size_of_fifo;		/* Port's requested resource */
+	u8 deq_pipeline_depth;		/* Port's requested resource */
+	u16 max_frame_length;		/* Port's max frame length. */
+	u16 liodn_base;
+	/* LIODN base for this port, to be used together with LIODN offset. */
+};
+
+void fman_get_revision(struct fman *fman, struct fman_rev_info *rev_info);
+
+void fman_register_intr(struct fman *fman, enum fman_event_modules mod,
+			u8 mod_id, enum fman_intr_type intr_type,
+			void (*f_isr)(void *h_src_arg), void *h_src_arg);
+
+void fman_unregister_intr(struct fman *fman, enum fman_event_modules mod,
+			  u8 mod_id, enum fman_intr_type intr_type);
+
+int fman_set_port_params(struct fman *fman,
+			 struct fman_port_init_params *port_params);
+
+int fman_reset_mac(struct fman *fman, u8 mac_id);
+
+u16 fman_get_clock_freq(struct fman *fman);
+
+u32 fman_get_bmi_max_fifo_size(struct fman *fman);
+
+int fman_set_mac_max_frame(struct fman *fman, u8 mac_id, u16 mfl);
+
+u32 fman_get_qman_channel_id(struct fman *fman, u32 port_id);
+
+struct resource *fman_get_mem_region(struct fman *fman);
+
+u16 fman_get_max_frm(void);
+
+int fman_get_rx_extra_headroom(void);
+
+struct fman *fman_bind(struct device *dev);
+
+#endif /* __FM_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_dtsec.c b/drivers/net/ethernet/freescale/fman/fman_dtsec.c
new file mode 100644
index 000000000000..7c92eb854925
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_dtsec.c
@@ -0,0 +1,1452 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "fman_dtsec.h"
+#include "fman.h"
+
+#include <linux/slab.h>
+#include <linux/bitrev.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/phy.h>
+#include <linux/crc32.h>
+#include <linux/of_mdio.h>
+#include <linux/mii.h>
+
+/* TBI register addresses */
+#define MII_TBICON		0x11
+
+/* TBICON register bit fields */
+#define TBICON_SOFT_RESET	0x8000	/* Soft reset */
+#define TBICON_DISABLE_RX_DIS	0x2000	/* Disable receive disparity */
+#define TBICON_DISABLE_TX_DIS	0x1000	/* Disable transmit disparity */
+#define TBICON_AN_SENSE		0x0100	/* Auto-negotiation sense enable */
+#define TBICON_CLK_SELECT	0x0020	/* Clock select */
+#define TBICON_MI_MODE		0x0010	/* GMII mode (TBI if not set) */
+
+#define TBIANA_SGMII		0x4001
+#define TBIANA_1000X		0x01a0
+
+/* Interrupt Mask Register (IMASK) */
+#define DTSEC_IMASK_BREN	0x80000000
+#define DTSEC_IMASK_RXCEN	0x40000000
+#define DTSEC_IMASK_MSROEN	0x04000000
+#define DTSEC_IMASK_GTSCEN	0x02000000
+#define DTSEC_IMASK_BTEN	0x01000000
+#define DTSEC_IMASK_TXCEN	0x00800000
+#define DTSEC_IMASK_TXEEN	0x00400000
+#define DTSEC_IMASK_LCEN	0x00040000
+#define DTSEC_IMASK_CRLEN	0x00020000
+#define DTSEC_IMASK_XFUNEN	0x00010000
+#define DTSEC_IMASK_ABRTEN	0x00008000
+#define DTSEC_IMASK_IFERREN	0x00004000
+#define DTSEC_IMASK_MAGEN	0x00000800
+#define DTSEC_IMASK_MMRDEN	0x00000400
+#define DTSEC_IMASK_MMWREN	0x00000200
+#define DTSEC_IMASK_GRSCEN	0x00000100
+#define DTSEC_IMASK_TDPEEN	0x00000002
+#define DTSEC_IMASK_RDPEEN	0x00000001
+
+#define DTSEC_EVENTS_MASK		\
+	 ((u32)(DTSEC_IMASK_BREN    |	\
+		DTSEC_IMASK_RXCEN   |	\
+		DTSEC_IMASK_BTEN    |	\
+		DTSEC_IMASK_TXCEN   |	\
+		DTSEC_IMASK_TXEEN   |	\
+		DTSEC_IMASK_ABRTEN  |	\
+		DTSEC_IMASK_LCEN    |	\
+		DTSEC_IMASK_CRLEN   |	\
+		DTSEC_IMASK_XFUNEN  |	\
+		DTSEC_IMASK_IFERREN |	\
+		DTSEC_IMASK_MAGEN   |	\
+		DTSEC_IMASK_TDPEEN  |	\
+		DTSEC_IMASK_RDPEEN))
+
+/* dtsec timestamp event bits */
+#define TMR_PEMASK_TSREEN	0x00010000
+#define TMR_PEVENT_TSRE		0x00010000
+
+/* Group address bit indication */
+#define MAC_GROUP_ADDRESS	0x0000010000000000ULL
+
+/* Defaults */
+#define DEFAULT_HALFDUP_RETRANSMIT		0xf
+#define DEFAULT_HALFDUP_COLL_WINDOW		0x37
+#define DEFAULT_TX_PAUSE_TIME			0xf000
+#define DEFAULT_RX_PREPEND			0
+#define DEFAULT_PREAMBLE_LEN			7
+#define DEFAULT_TX_PAUSE_TIME_EXTD		0
+#define DEFAULT_NON_BACK_TO_BACK_IPG1		0x40
+#define DEFAULT_NON_BACK_TO_BACK_IPG2		0x60
+#define DEFAULT_MIN_IFG_ENFORCEMENT		0x50
+#define DEFAULT_BACK_TO_BACK_IPG		0x60
+#define DEFAULT_MAXIMUM_FRAME			0x600
+
+/* register related defines (bits, field offsets..) */
+#define DTSEC_ID2_INT_REDUCED_OFF	0x00010000
+
+#define DTSEC_ECNTRL_GMIIM		0x00000040
+#define DTSEC_ECNTRL_TBIM		0x00000020
+#define DTSEC_ECNTRL_SGMIIM		0x00000002
+#define DTSEC_ECNTRL_RPM		0x00000010
+#define DTSEC_ECNTRL_R100M		0x00000008
+#define DTSEC_ECNTRL_QSGMIIM		0x00000001
+
+#define DTSEC_TCTRL_GTS			0x00000020
+
+#define RCTRL_PAL_MASK			0x001f0000
+#define RCTRL_PAL_SHIFT			16
+#define RCTRL_GHTX			0x00000400
+#define RCTRL_GRS			0x00000020
+#define RCTRL_MPROM			0x00000008
+#define RCTRL_RSF			0x00000004
+#define RCTRL_UPROM			0x00000001
+
+#define MACCFG1_SOFT_RESET		0x80000000
+#define MACCFG1_RX_FLOW			0x00000020
+#define MACCFG1_TX_FLOW			0x00000010
+#define MACCFG1_TX_EN			0x00000001
+#define MACCFG1_RX_EN			0x00000004
+
+#define MACCFG2_NIBBLE_MODE		0x00000100
+#define MACCFG2_BYTE_MODE		0x00000200
+#define MACCFG2_PAD_CRC_EN		0x00000004
+#define MACCFG2_FULL_DUPLEX		0x00000001
+#define MACCFG2_PREAMBLE_LENGTH_MASK	0x0000f000
+#define MACCFG2_PREAMBLE_LENGTH_SHIFT	12
+
+#define IPGIFG_NON_BACK_TO_BACK_IPG_1_SHIFT	24
+#define IPGIFG_NON_BACK_TO_BACK_IPG_2_SHIFT	16
+#define IPGIFG_MIN_IFG_ENFORCEMENT_SHIFT	8
+
+#define IPGIFG_NON_BACK_TO_BACK_IPG_1	0x7F000000
+#define IPGIFG_NON_BACK_TO_BACK_IPG_2	0x007F0000
+#define IPGIFG_MIN_IFG_ENFORCEMENT	0x0000FF00
+#define IPGIFG_BACK_TO_BACK_IPG	0x0000007F
+
+#define HAFDUP_EXCESS_DEFER			0x00010000
+#define HAFDUP_COLLISION_WINDOW		0x000003ff
+#define HAFDUP_RETRANSMISSION_MAX_SHIFT	12
+#define HAFDUP_RETRANSMISSION_MAX		0x0000f000
+
+#define NUM_OF_HASH_REGS	8	/* Number of hash table registers */
+
+#define PTV_PTE_MASK		0xffff0000
+#define PTV_PT_MASK		0x0000ffff
+#define PTV_PTE_SHIFT		16
+
+#define MAX_PACKET_ALIGNMENT		31
+#define MAX_INTER_PACKET_GAP		0x7f
+#define MAX_RETRANSMISSION		0x0f
+#define MAX_COLLISION_WINDOW		0x03ff
+
+/* Hash table size (32 bits*8 regs) */
+#define DTSEC_HASH_TABLE_SIZE		256
+/* Extended Hash table size (32 bits*16 regs) */
+#define EXTENDED_HASH_TABLE_SIZE	512
+
+/* dTSEC Memory Map registers */
+struct dtsec_regs {
+	/* dTSEC General Control and Status Registers */
+	u32 tsec_id;		/* 0x000 ETSEC_ID register */
+	u32 tsec_id2;		/* 0x004 ETSEC_ID2 register */
+	u32 ievent;		/* 0x008 Interrupt event register */
+	u32 imask;		/* 0x00C Interrupt mask register */
+	u32 reserved0010[1];
+	u32 ecntrl;		/* 0x014 E control register */
+	u32 ptv;		/* 0x018 Pause time value register */
+	u32 tbipa;		/* 0x01C TBI PHY address register */
+	u32 tmr_ctrl;		/* 0x020 Time-stamp Control register */
+	u32 tmr_pevent;		/* 0x024 Time-stamp event register */
+	u32 tmr_pemask;		/* 0x028 Timer event mask register */
+	u32 reserved002c[5];
+	u32 tctrl;		/* 0x040 Transmit control register */
+	u32 reserved0044[3];
+	u32 rctrl;		/* 0x050 Receive control register */
+	u32 reserved0054[11];
+	u32 igaddr[8];		/* 0x080-0x09C Individual/group address */
+	u32 gaddr[8];		/* 0x0A0-0x0BC Group address registers 0-7 */
+	u32 reserved00c0[16];
+	u32 maccfg1;		/* 0x100 MAC configuration #1 */
+	u32 maccfg2;		/* 0x104 MAC configuration #2 */
+	u32 ipgifg;		/* 0x108 IPG/IFG */
+	u32 hafdup;		/* 0x10C Half-duplex */
+	u32 maxfrm;		/* 0x110 Maximum frame */
+	u32 reserved0114[10];
+	u32 ifstat;		/* 0x13C Interface status */
+	u32 macstnaddr1;	/* 0x140 Station Address,part 1 */
+	u32 macstnaddr2;	/* 0x144 Station Address,part 2 */
+	struct {
+		u32 exact_match1;	/* octets 1-4 */
+		u32 exact_match2;	/* octets 5-6 */
+	} macaddr[15];		/* 0x148-0x1BC mac exact match addresses 1-15 */
+	u32 reserved01c0[16];
+	u32 tr64;	/* 0x200 Tx and Rx 64 byte frame counter */
+	u32 tr127;	/* 0x204 Tx and Rx 65 to 127 byte frame counter */
+	u32 tr255;	/* 0x208 Tx and Rx 128 to 255 byte frame counter */
+	u32 tr511;	/* 0x20C Tx and Rx 256 to 511 byte frame counter */
+	u32 tr1k;	/* 0x210 Tx and Rx 512 to 1023 byte frame counter */
+	u32 trmax;	/* 0x214 Tx and Rx 1024 to 1518 byte frame counter */
+	u32 trmgv;
+	/* 0x218 Tx and Rx 1519 to 1522 byte good VLAN frame count */
+	u32 rbyt;	/* 0x21C receive byte counter */
+	u32 rpkt;	/* 0x220 receive packet counter */
+	u32 rfcs;	/* 0x224 receive FCS error counter */
+	u32 rmca;	/* 0x228 RMCA Rx multicast packet counter */
+	u32 rbca;	/* 0x22C Rx broadcast packet counter */
+	u32 rxcf;	/* 0x230 Rx control frame packet counter */
+	u32 rxpf;	/* 0x234 Rx pause frame packet counter */
+	u32 rxuo;	/* 0x238 Rx unknown OP code counter */
+	u32 raln;	/* 0x23C Rx alignment error counter */
+	u32 rflr;	/* 0x240 Rx frame length error counter */
+	u32 rcde;	/* 0x244 Rx code error counter */
+	u32 rcse;	/* 0x248 Rx carrier sense error counter */
+	u32 rund;	/* 0x24C Rx undersize packet counter */
+	u32 rovr;	/* 0x250 Rx oversize packet counter */
+	u32 rfrg;	/* 0x254 Rx fragments counter */
+	u32 rjbr;	/* 0x258 Rx jabber counter */
+	u32 rdrp;	/* 0x25C Rx drop */
+	u32 tbyt;	/* 0x260 Tx byte counter */
+	u32 tpkt;	/* 0x264 Tx packet counter */
+	u32 tmca;	/* 0x268 Tx multicast packet counter */
+	u32 tbca;	/* 0x26C Tx broadcast packet counter */
+	u32 txpf;	/* 0x270 Tx pause control frame counter */
+	u32 tdfr;	/* 0x274 Tx deferral packet counter */
+	u32 tedf;	/* 0x278 Tx excessive deferral packet counter */
+	u32 tscl;	/* 0x27C Tx single collision packet counter */
+	u32 tmcl;	/* 0x280 Tx multiple collision packet counter */
+	u32 tlcl;	/* 0x284 Tx late collision packet counter */
+	u32 txcl;	/* 0x288 Tx excessive collision packet counter */
+	u32 tncl;	/* 0x28C Tx total collision counter */
+	u32 reserved0290[1];
+	u32 tdrp;	/* 0x294 Tx drop frame counter */
+	u32 tjbr;	/* 0x298 Tx jabber frame counter */
+	u32 tfcs;	/* 0x29C Tx FCS error counter */
+	u32 txcf;	/* 0x2A0 Tx control frame counter */
+	u32 tovr;	/* 0x2A4 Tx oversize frame counter */
+	u32 tund;	/* 0x2A8 Tx undersize frame counter */
+	u32 tfrg;	/* 0x2AC Tx fragments frame counter */
+	u32 car1;	/* 0x2B0 carry register one register* */
+	u32 car2;	/* 0x2B4 carry register two register* */
+	u32 cam1;	/* 0x2B8 carry register one mask register */
+	u32 cam2;	/* 0x2BC carry register two mask register */
+	u32 reserved02c0[848];
+};
+
+/* struct dtsec_cfg - dTSEC configuration
+ * Transmit half-duplex flow control, under software control for 10/100-Mbps
+ * half-duplex media. If set, back pressure is applied to media by raising
+ * carrier.
+ * halfdup_retransmit:
+ * Number of retransmission attempts following a collision.
+ * If this is exceeded dTSEC aborts transmission due to excessive collisions.
+ * The standard specifies the attempt limit to be 15.
+ * halfdup_coll_window:
+ * The number of bytes of the frame during which collisions may occur.
+ * The default value of 55 corresponds to the frame byte at the end of the
+ * standard 512-bit slot time window. If collisions are detected after this
+ * byte, the late collision event is asserted and transmission of current
+ * frame is aborted.
+ * tx_pad_crc:
+ * Pad and append CRC. If set, the MAC pads all ransmitted short frames and
+ * appends a CRC to every frame regardless of padding requirement.
+ * tx_pause_time:
+ * Transmit pause time value. This pause value is used as part of the pause
+ * frame to be sent when a transmit pause frame is initiated.
+ * If set to 0 this disables transmission of pause frames.
+ * preamble_len:
+ * Length, in bytes, of the preamble field preceding each Ethernet
+ * start-of-frame delimiter byte. The default value of 0x7 should be used in
+ * order to guarantee reliable operation with IEEE 802.3 compliant hardware.
+ * rx_prepend:
+ * Packet alignment padding length. The specified number of bytes (1-31)
+ * of zero padding are inserted before the start of each received frame.
+ * For Ethernet, where optional preamble extraction is enabled, the padding
+ * appears before the preamble, otherwise the padding precedes the
+ * layer 2 header.
+ *
+ * This structure contains basic dTSEC configuration and must be passed to
+ * init() function. A default set of configuration values can be
+ * obtained by calling set_dflts().
+ */
+struct dtsec_cfg {
+	u16 halfdup_retransmit;
+	u16 halfdup_coll_window;
+	bool tx_pad_crc;
+	u16 tx_pause_time;
+	bool ptp_tsu_en;
+	bool ptp_exception_en;
+	u32 preamble_len;
+	u32 rx_prepend;
+	u16 tx_pause_time_extd;
+	u16 maximum_frame;
+	u32 non_back_to_back_ipg1;
+	u32 non_back_to_back_ipg2;
+	u32 min_ifg_enforcement;
+	u32 back_to_back_ipg;
+};
+
+struct fman_mac {
+	/* pointer to dTSEC memory mapped registers */
+	struct dtsec_regs __iomem *regs;
+	/* MAC address of device */
+	u64 addr;
+	/* Ethernet physical interface */
+	phy_interface_t phy_if;
+	u16 max_speed;
+	void *dev_id; /* device cookie used by the exception cbs */
+	fman_mac_exception_cb *exception_cb;
+	fman_mac_exception_cb *event_cb;
+	/* Number of individual addresses in registers for this station */
+	u8 num_of_ind_addr_in_regs;
+	/* pointer to driver's global address hash table */
+	struct eth_hash_t *multicast_addr_hash;
+	/* pointer to driver's individual address hash table */
+	struct eth_hash_t *unicast_addr_hash;
+	u8 mac_id;
+	u32 exceptions;
+	bool ptp_tsu_enabled;
+	bool en_tsu_err_exeption;
+	struct dtsec_cfg *dtsec_drv_param;
+	void *fm;
+	struct fman_rev_info fm_rev_info;
+	bool basex_if;
+	struct phy_device *tbiphy;
+};
+
+static void set_dflts(struct dtsec_cfg *cfg)
+{
+	cfg->halfdup_retransmit = DEFAULT_HALFDUP_RETRANSMIT;
+	cfg->halfdup_coll_window = DEFAULT_HALFDUP_COLL_WINDOW;
+	cfg->tx_pad_crc = true;
+	cfg->tx_pause_time = DEFAULT_TX_PAUSE_TIME;
+	/* PHY address 0 is reserved (DPAA RM) */
+	cfg->rx_prepend = DEFAULT_RX_PREPEND;
+	cfg->ptp_tsu_en = true;
+	cfg->ptp_exception_en = true;
+	cfg->preamble_len = DEFAULT_PREAMBLE_LEN;
+	cfg->tx_pause_time_extd = DEFAULT_TX_PAUSE_TIME_EXTD;
+	cfg->non_back_to_back_ipg1 = DEFAULT_NON_BACK_TO_BACK_IPG1;
+	cfg->non_back_to_back_ipg2 = DEFAULT_NON_BACK_TO_BACK_IPG2;
+	cfg->min_ifg_enforcement = DEFAULT_MIN_IFG_ENFORCEMENT;
+	cfg->back_to_back_ipg = DEFAULT_BACK_TO_BACK_IPG;
+	cfg->maximum_frame = DEFAULT_MAXIMUM_FRAME;
+}
+
+static int init(struct dtsec_regs __iomem *regs, struct dtsec_cfg *cfg,
+		phy_interface_t iface, u16 iface_speed, u8 *macaddr,
+		u32 exception_mask, u8 tbi_addr)
+{
+	bool is_rgmii, is_sgmii, is_qsgmii;
+	int i;
+	u32 tmp;
+
+	/* Soft reset */
+	iowrite32be(MACCFG1_SOFT_RESET, &regs->maccfg1);
+	iowrite32be(0, &regs->maccfg1);
+
+	/* dtsec_id2 */
+	tmp = ioread32be(&regs->tsec_id2);
+
+	/* check RGMII support */
+	if (iface == PHY_INTERFACE_MODE_RGMII ||
+	    iface == PHY_INTERFACE_MODE_RMII)
+		if (tmp & DTSEC_ID2_INT_REDUCED_OFF)
+			return -EINVAL;
+
+	if (iface == PHY_INTERFACE_MODE_SGMII ||
+	    iface == PHY_INTERFACE_MODE_MII)
+		if (tmp & DTSEC_ID2_INT_REDUCED_OFF)
+			return -EINVAL;
+
+	is_rgmii = iface == PHY_INTERFACE_MODE_RGMII;
+	is_sgmii = iface == PHY_INTERFACE_MODE_SGMII;
+	is_qsgmii = iface == PHY_INTERFACE_MODE_QSGMII;
+
+	tmp = 0;
+	if (is_rgmii || iface == PHY_INTERFACE_MODE_GMII)
+		tmp |= DTSEC_ECNTRL_GMIIM;
+	if (is_sgmii)
+		tmp |= (DTSEC_ECNTRL_SGMIIM | DTSEC_ECNTRL_TBIM);
+	if (is_qsgmii)
+		tmp |= (DTSEC_ECNTRL_SGMIIM | DTSEC_ECNTRL_TBIM |
+			DTSEC_ECNTRL_QSGMIIM);
+	if (is_rgmii)
+		tmp |= DTSEC_ECNTRL_RPM;
+	if (iface_speed == SPEED_100)
+		tmp |= DTSEC_ECNTRL_R100M;
+
+	iowrite32be(tmp, &regs->ecntrl);
+
+	tmp = 0;
+
+	if (cfg->tx_pause_time)
+		tmp |= cfg->tx_pause_time;
+	if (cfg->tx_pause_time_extd)
+		tmp |= cfg->tx_pause_time_extd << PTV_PTE_SHIFT;
+	iowrite32be(tmp, &regs->ptv);
+
+	tmp = 0;
+	tmp |= (cfg->rx_prepend << RCTRL_PAL_SHIFT) & RCTRL_PAL_MASK;
+	/* Accept short frames */
+	tmp |= RCTRL_RSF;
+
+	iowrite32be(tmp, &regs->rctrl);
+
+	/* Assign a Phy Address to the TBI (TBIPA).
+	 * Done also in cases where TBI is not selected to avoid conflict with
+	 * the external PHY's Physical address
+	 */
+	iowrite32be(tbi_addr, &regs->tbipa);
+
+	iowrite32be(0, &regs->tmr_ctrl);
+
+	if (cfg->ptp_tsu_en) {
+		tmp = 0;
+		tmp |= TMR_PEVENT_TSRE;
+		iowrite32be(tmp, &regs->tmr_pevent);
+
+		if (cfg->ptp_exception_en) {
+			tmp = 0;
+			tmp |= TMR_PEMASK_TSREEN;
+			iowrite32be(tmp, &regs->tmr_pemask);
+		}
+	}
+
+	tmp = 0;
+	tmp |= MACCFG1_RX_FLOW;
+	tmp |= MACCFG1_TX_FLOW;
+	iowrite32be(tmp, &regs->maccfg1);
+
+	tmp = 0;
+
+	if (iface_speed < SPEED_1000)
+		tmp |= MACCFG2_NIBBLE_MODE;
+	else if (iface_speed == SPEED_1000)
+		tmp |= MACCFG2_BYTE_MODE;
+
+	tmp |= (cfg->preamble_len << MACCFG2_PREAMBLE_LENGTH_SHIFT) &
+		MACCFG2_PREAMBLE_LENGTH_MASK;
+	if (cfg->tx_pad_crc)
+		tmp |= MACCFG2_PAD_CRC_EN;
+	/* Full Duplex */
+	tmp |= MACCFG2_FULL_DUPLEX;
+	iowrite32be(tmp, &regs->maccfg2);
+
+	tmp = (((cfg->non_back_to_back_ipg1 <<
+		 IPGIFG_NON_BACK_TO_BACK_IPG_1_SHIFT)
+		& IPGIFG_NON_BACK_TO_BACK_IPG_1)
+	       | ((cfg->non_back_to_back_ipg2 <<
+		   IPGIFG_NON_BACK_TO_BACK_IPG_2_SHIFT)
+		 & IPGIFG_NON_BACK_TO_BACK_IPG_2)
+	       | ((cfg->min_ifg_enforcement << IPGIFG_MIN_IFG_ENFORCEMENT_SHIFT)
+		 & IPGIFG_MIN_IFG_ENFORCEMENT)
+	       | (cfg->back_to_back_ipg & IPGIFG_BACK_TO_BACK_IPG));
+	iowrite32be(tmp, &regs->ipgifg);
+
+	tmp = 0;
+	tmp |= HAFDUP_EXCESS_DEFER;
+	tmp |= ((cfg->halfdup_retransmit << HAFDUP_RETRANSMISSION_MAX_SHIFT)
+		& HAFDUP_RETRANSMISSION_MAX);
+	tmp |= (cfg->halfdup_coll_window & HAFDUP_COLLISION_WINDOW);
+
+	iowrite32be(tmp, &regs->hafdup);
+
+	/* Initialize Maximum frame length */
+	iowrite32be(cfg->maximum_frame, &regs->maxfrm);
+
+	iowrite32be(0xffffffff, &regs->cam1);
+	iowrite32be(0xffffffff, &regs->cam2);
+
+	iowrite32be(exception_mask, &regs->imask);
+
+	iowrite32be(0xffffffff, &regs->ievent);
+
+	tmp = (u32)((macaddr[5] << 24) |
+		    (macaddr[4] << 16) | (macaddr[3] << 8) | macaddr[2]);
+	iowrite32be(tmp, &regs->macstnaddr1);
+
+	tmp = (u32)((macaddr[1] << 24) | (macaddr[0] << 16));
+	iowrite32be(tmp, &regs->macstnaddr2);
+
+	/* HASH */
+	for (i = 0; i < NUM_OF_HASH_REGS; i++) {
+		/* Initialize IADDRx */
+		iowrite32be(0, &regs->igaddr[i]);
+		/* Initialize GADDRx */
+		iowrite32be(0, &regs->gaddr[i]);
+	}
+
+	return 0;
+}
+
+static void set_mac_address(struct dtsec_regs __iomem *regs, u8 *adr)
+{
+	u32 tmp;
+
+	tmp = (u32)((adr[5] << 24) |
+		    (adr[4] << 16) | (adr[3] << 8) | adr[2]);
+	iowrite32be(tmp, &regs->macstnaddr1);
+
+	tmp = (u32)((adr[1] << 24) | (adr[0] << 16));
+	iowrite32be(tmp, &regs->macstnaddr2);
+}
+
+static void set_bucket(struct dtsec_regs __iomem *regs, int bucket,
+		       bool enable)
+{
+	int reg_idx = (bucket >> 5) & 0xf;
+	int bit_idx = bucket & 0x1f;
+	u32 bit_mask = 0x80000000 >> bit_idx;
+	u32 __iomem *reg;
+
+	if (reg_idx > 7)
+		reg = &regs->gaddr[reg_idx - 8];
+	else
+		reg = &regs->igaddr[reg_idx];
+
+	if (enable)
+		iowrite32be(ioread32be(reg) | bit_mask, reg);
+	else
+		iowrite32be(ioread32be(reg) & (~bit_mask), reg);
+}
+
+static int check_init_parameters(struct fman_mac *dtsec)
+{
+	if (dtsec->max_speed >= SPEED_10000) {
+		pr_err("1G MAC driver supports 1G or lower speeds\n");
+		return -EINVAL;
+	}
+	if (dtsec->addr == 0) {
+		pr_err("Ethernet MAC Must have a valid MAC Address\n");
+		return -EINVAL;
+	}
+	if ((dtsec->dtsec_drv_param)->rx_prepend >
+	    MAX_PACKET_ALIGNMENT) {
+		pr_err("packetAlignmentPadding can't be > than %d\n",
+		       MAX_PACKET_ALIGNMENT);
+		return -EINVAL;
+	}
+	if (((dtsec->dtsec_drv_param)->non_back_to_back_ipg1 >
+	     MAX_INTER_PACKET_GAP) ||
+	    ((dtsec->dtsec_drv_param)->non_back_to_back_ipg2 >
+	     MAX_INTER_PACKET_GAP) ||
+	     ((dtsec->dtsec_drv_param)->back_to_back_ipg >
+	      MAX_INTER_PACKET_GAP)) {
+		pr_err("Inter packet gap can't be greater than %d\n",
+		       MAX_INTER_PACKET_GAP);
+		return -EINVAL;
+	}
+	if ((dtsec->dtsec_drv_param)->halfdup_retransmit >
+	    MAX_RETRANSMISSION) {
+		pr_err("maxRetransmission can't be greater than %d\n",
+		       MAX_RETRANSMISSION);
+		return -EINVAL;
+	}
+	if ((dtsec->dtsec_drv_param)->halfdup_coll_window >
+	    MAX_COLLISION_WINDOW) {
+		pr_err("collisionWindow can't be greater than %d\n",
+		       MAX_COLLISION_WINDOW);
+		return -EINVAL;
+	/* If Auto negotiation process is disabled, need to set up the PHY
+	 * using the MII Management Interface
+	 */
+	}
+	if (!dtsec->exception_cb) {
+		pr_err("uninitialized exception_cb\n");
+		return -EINVAL;
+	}
+	if (!dtsec->event_cb) {
+		pr_err("uninitialized event_cb\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int get_exception_flag(enum fman_mac_exceptions exception)
+{
+	u32 bit_mask;
+
+	switch (exception) {
+	case FM_MAC_EX_1G_BAB_RX:
+		bit_mask = DTSEC_IMASK_BREN;
+		break;
+	case FM_MAC_EX_1G_RX_CTL:
+		bit_mask = DTSEC_IMASK_RXCEN;
+		break;
+	case FM_MAC_EX_1G_GRATEFUL_TX_STP_COMPLET:
+		bit_mask = DTSEC_IMASK_GTSCEN;
+		break;
+	case FM_MAC_EX_1G_BAB_TX:
+		bit_mask = DTSEC_IMASK_BTEN;
+		break;
+	case FM_MAC_EX_1G_TX_CTL:
+		bit_mask = DTSEC_IMASK_TXCEN;
+		break;
+	case FM_MAC_EX_1G_TX_ERR:
+		bit_mask = DTSEC_IMASK_TXEEN;
+		break;
+	case FM_MAC_EX_1G_LATE_COL:
+		bit_mask = DTSEC_IMASK_LCEN;
+		break;
+	case FM_MAC_EX_1G_COL_RET_LMT:
+		bit_mask = DTSEC_IMASK_CRLEN;
+		break;
+	case FM_MAC_EX_1G_TX_FIFO_UNDRN:
+		bit_mask = DTSEC_IMASK_XFUNEN;
+		break;
+	case FM_MAC_EX_1G_MAG_PCKT:
+		bit_mask = DTSEC_IMASK_MAGEN;
+		break;
+	case FM_MAC_EX_1G_MII_MNG_RD_COMPLET:
+		bit_mask = DTSEC_IMASK_MMRDEN;
+		break;
+	case FM_MAC_EX_1G_MII_MNG_WR_COMPLET:
+		bit_mask = DTSEC_IMASK_MMWREN;
+		break;
+	case FM_MAC_EX_1G_GRATEFUL_RX_STP_COMPLET:
+		bit_mask = DTSEC_IMASK_GRSCEN;
+		break;
+	case FM_MAC_EX_1G_DATA_ERR:
+		bit_mask = DTSEC_IMASK_TDPEEN;
+		break;
+	case FM_MAC_EX_1G_RX_MIB_CNT_OVFL:
+		bit_mask = DTSEC_IMASK_MSROEN;
+		break;
+	default:
+		bit_mask = 0;
+		break;
+	}
+
+	return bit_mask;
+}
+
+static bool is_init_done(struct dtsec_cfg *dtsec_drv_params)
+{
+	/* Checks if dTSEC driver parameters were initialized */
+	if (!dtsec_drv_params)
+		return true;
+
+	return false;
+}
+
+static u16 dtsec_get_max_frame_length(struct fman_mac *dtsec)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+
+	if (is_init_done(dtsec->dtsec_drv_param))
+		return 0;
+
+	return (u16)ioread32be(&regs->maxfrm);
+}
+
+static void dtsec_isr(void *handle)
+{
+	struct fman_mac *dtsec = (struct fman_mac *)handle;
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 event;
+
+	/* do not handle MDIO events */
+	event = ioread32be(&regs->ievent) &
+		(u32)(~(DTSEC_IMASK_MMRDEN | DTSEC_IMASK_MMWREN));
+
+	event &= ioread32be(&regs->imask);
+
+	iowrite32be(event, &regs->ievent);
+
+	if (event & DTSEC_IMASK_BREN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_BAB_RX);
+	if (event & DTSEC_IMASK_RXCEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_RX_CTL);
+	if (event & DTSEC_IMASK_GTSCEN)
+		dtsec->exception_cb(dtsec->dev_id,
+				    FM_MAC_EX_1G_GRATEFUL_TX_STP_COMPLET);
+	if (event & DTSEC_IMASK_BTEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_BAB_TX);
+	if (event & DTSEC_IMASK_TXCEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_TX_CTL);
+	if (event & DTSEC_IMASK_TXEEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_TX_ERR);
+	if (event & DTSEC_IMASK_LCEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_LATE_COL);
+	if (event & DTSEC_IMASK_CRLEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_COL_RET_LMT);
+	if (event & DTSEC_IMASK_XFUNEN) {
+		/* FM_TX_LOCKUP_ERRATA_DTSEC6 Errata workaround */
+		if (dtsec->fm_rev_info.major == 2) {
+			u32 tpkt1, tmp_reg1, tpkt2, tmp_reg2, i;
+			/* a. Write 0x00E0_0C00 to DTSEC_ID
+			 *	This is a read only register
+			 * b. Read and save the value of TPKT
+			 */
+			tpkt1 = ioread32be(&regs->tpkt);
+
+			/* c. Read the register at dTSEC address offset 0x32C */
+			tmp_reg1 = ioread32be(&regs->reserved02c0[27]);
+
+			/* d. Compare bits [9:15] to bits [25:31] of the
+			 * register at address offset 0x32C.
+			 */
+			if ((tmp_reg1 & 0x007F0000) !=
+				(tmp_reg1 & 0x0000007F)) {
+				/* If they are not equal, save the value of
+				 * this register and wait for at least
+				 * MAXFRM*16 ns
+				 */
+				usleep_range((u32)(min
+					(dtsec_get_max_frame_length(dtsec) *
+					16 / 1000, 1)), (u32)
+					(min(dtsec_get_max_frame_length
+					(dtsec) * 16 / 1000, 1) + 1));
+			}
+
+			/* e. Read and save TPKT again and read the register
+			 * at dTSEC address offset 0x32C again
+			 */
+			tpkt2 = ioread32be(&regs->tpkt);
+			tmp_reg2 = ioread32be(&regs->reserved02c0[27]);
+
+			/* f. Compare the value of TPKT saved in step b to
+			 * value read in step e. Also compare bits [9:15] of
+			 * the register at offset 0x32C saved in step d to the
+			 * value of bits [9:15] saved in step e. If the two
+			 * registers values are unchanged, then the transmit
+			 * portion of the dTSEC controller is locked up and
+			 * the user should proceed to the recover sequence.
+			 */
+			if ((tpkt1 == tpkt2) && ((tmp_reg1 & 0x007F0000) ==
+				(tmp_reg2 & 0x007F0000))) {
+				/* recover sequence */
+
+				/* a.Write a 1 to RCTRL[GRS] */
+
+				iowrite32be(ioread32be(&regs->rctrl) |
+					    RCTRL_GRS, &regs->rctrl);
+
+				/* b.Wait until IEVENT[GRSC]=1, or at least
+				 * 100 us has elapsed.
+				 */
+				for (i = 0; i < 100; i++) {
+					if (ioread32be(&regs->ievent) &
+					    DTSEC_IMASK_GRSCEN)
+						break;
+					udelay(1);
+				}
+				if (ioread32be(&regs->ievent) &
+				    DTSEC_IMASK_GRSCEN)
+					iowrite32be(DTSEC_IMASK_GRSCEN,
+						    &regs->ievent);
+				else
+					pr_debug("Rx lockup due to Tx lockup\n");
+
+				/* c.Write a 1 to bit n of FM_RSTC
+				 * (offset 0x0CC of FPM)
+				 */
+				fman_reset_mac(dtsec->fm, dtsec->mac_id);
+
+				/* d.Wait 4 Tx clocks (32 ns) */
+				udelay(1);
+
+				/* e.Write a 0 to bit n of FM_RSTC. */
+				/* cleared by FMAN
+				 */
+			}
+		}
+
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_TX_FIFO_UNDRN);
+	}
+	if (event & DTSEC_IMASK_MAGEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_MAG_PCKT);
+	if (event & DTSEC_IMASK_GRSCEN)
+		dtsec->exception_cb(dtsec->dev_id,
+				    FM_MAC_EX_1G_GRATEFUL_RX_STP_COMPLET);
+	if (event & DTSEC_IMASK_TDPEEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_EX_1G_DATA_ERR);
+	if (event & DTSEC_IMASK_RDPEEN)
+		dtsec->exception_cb(dtsec->dev_id, FM_MAC_1G_RX_DATA_ERR);
+
+	/* masked interrupts */
+	WARN_ON(event & DTSEC_IMASK_ABRTEN);
+	WARN_ON(event & DTSEC_IMASK_IFERREN);
+}
+
+static void dtsec_1588_isr(void *handle)
+{
+	struct fman_mac *dtsec = (struct fman_mac *)handle;
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 event;
+
+	if (dtsec->ptp_tsu_enabled) {
+		event = ioread32be(&regs->tmr_pevent);
+		event &= ioread32be(&regs->tmr_pemask);
+
+		if (event) {
+			iowrite32be(event, &regs->tmr_pevent);
+			WARN_ON(event & TMR_PEVENT_TSRE);
+			dtsec->exception_cb(dtsec->dev_id,
+					    FM_MAC_EX_1G_1588_TS_RX_ERR);
+		}
+	}
+}
+
+static void free_init_resources(struct fman_mac *dtsec)
+{
+	fman_unregister_intr(dtsec->fm, FMAN_MOD_MAC, dtsec->mac_id,
+			     FMAN_INTR_TYPE_ERR);
+	fman_unregister_intr(dtsec->fm, FMAN_MOD_MAC, dtsec->mac_id,
+			     FMAN_INTR_TYPE_NORMAL);
+
+	/* release the driver's group hash table */
+	free_hash_table(dtsec->multicast_addr_hash);
+	dtsec->multicast_addr_hash = NULL;
+
+	/* release the driver's individual hash table */
+	free_hash_table(dtsec->unicast_addr_hash);
+	dtsec->unicast_addr_hash = NULL;
+}
+
+int dtsec_cfg_max_frame_len(struct fman_mac *dtsec, u16 new_val)
+{
+	if (is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	dtsec->dtsec_drv_param->maximum_frame = new_val;
+
+	return 0;
+}
+
+int dtsec_cfg_pad_and_crc(struct fman_mac *dtsec, bool new_val)
+{
+	if (is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	dtsec->dtsec_drv_param->tx_pad_crc = new_val;
+
+	return 0;
+}
+
+int dtsec_enable(struct fman_mac *dtsec, enum comm_mode mode)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 tmp;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	/* Enable */
+	tmp = ioread32be(&regs->maccfg1);
+	if (mode & COMM_MODE_RX)
+		tmp |= MACCFG1_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp |= MACCFG1_TX_EN;
+
+	iowrite32be(tmp, &regs->maccfg1);
+
+	/* Graceful start - clear the graceful receive stop bit */
+	if (mode & COMM_MODE_TX)
+		iowrite32be(ioread32be(&regs->tctrl) & ~DTSEC_TCTRL_GTS,
+			    &regs->tctrl);
+	if (mode & COMM_MODE_RX)
+		iowrite32be(ioread32be(&regs->rctrl) & ~RCTRL_GRS,
+			    &regs->rctrl);
+
+	return 0;
+}
+
+int dtsec_disable(struct fman_mac *dtsec, enum comm_mode mode)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 tmp;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	/* Gracefull stop - Assert the graceful transmit stop bit */
+	if (mode & COMM_MODE_RX) {
+		tmp = ioread32be(&regs->rctrl) | RCTRL_GRS;
+		iowrite32be(tmp, &regs->rctrl);
+
+		if (dtsec->fm_rev_info.major == 2)
+			usleep_range(100, 200);
+		else
+			udelay(10);
+	}
+
+	if (mode & COMM_MODE_TX) {
+		if (dtsec->fm_rev_info.major == 2)
+			pr_debug("GTS not supported due to DTSEC_A004 errata.\n");
+		else
+			pr_debug("GTS not supported due to DTSEC_A0014 errata.\n");
+	}
+
+	tmp = ioread32be(&regs->maccfg1);
+	if (mode & COMM_MODE_RX)
+		tmp &= ~MACCFG1_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp &= ~MACCFG1_TX_EN;
+
+	iowrite32be(tmp, &regs->maccfg1);
+
+	return 0;
+}
+
+int dtsec_set_tx_pause_frames(struct fman_mac *dtsec,
+			      u8 __maybe_unused priority,
+			      u16 pause_time, u16 __maybe_unused thresh_time)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 ptv = 0;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	/* FM_BAD_TX_TS_IN_B_2_B_ERRATA_DTSEC_A003 Errata workaround */
+	if (dtsec->fm_rev_info.major == 2)
+		if (pause_time <= 320) {
+			pr_warn("pause-time: %d illegal.Should be > 320\n",
+				pause_time);
+			return -EINVAL;
+		}
+
+	if (pause_time) {
+		ptv = ioread32be(&regs->ptv);
+		ptv &= PTV_PTE_MASK;
+		ptv |= pause_time & PTV_PT_MASK;
+		iowrite32be(ptv, &regs->ptv);
+
+		/* trigger the transmission of a flow-control pause frame */
+		iowrite32be(ioread32be(&regs->maccfg1) | MACCFG1_TX_FLOW,
+			    &regs->maccfg1);
+	} else
+		iowrite32be(ioread32be(&regs->maccfg1) & ~MACCFG1_TX_FLOW,
+			    &regs->maccfg1);
+
+	return 0;
+}
+
+int dtsec_accept_rx_pause_frames(struct fman_mac *dtsec, bool en)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 tmp;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->maccfg1);
+	if (en)
+		tmp |= MACCFG1_RX_FLOW;
+	else
+		tmp &= ~MACCFG1_RX_FLOW;
+	iowrite32be(tmp, &regs->maccfg1);
+
+	return 0;
+}
+
+int dtsec_modify_mac_address(struct fman_mac *dtsec, enet_addr_t *enet_addr)
+{
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	/* Initialize MAC Station Address registers (1 & 2)
+	 * Station address have to be swapped (big endian to little endian
+	 */
+	dtsec->addr = ENET_ADDR_TO_UINT64(*enet_addr);
+	set_mac_address(dtsec->regs, (u8 *)(*enet_addr));
+
+	return 0;
+}
+
+int dtsec_add_hash_mac_address(struct fman_mac *dtsec, enet_addr_t *eth_addr)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	struct eth_hash_entry *hash_entry;
+	u64 addr;
+	s32 bucket;
+	u32 crc = 0xFFFFFFFF;
+	bool mcast, ghtx;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	addr = ENET_ADDR_TO_UINT64(*eth_addr);
+
+	ghtx = (bool)((ioread32be(&regs->rctrl) & RCTRL_GHTX) ? true : false);
+	mcast = (bool)((addr & MAC_GROUP_ADDRESS) ? true : false);
+
+	/* Cannot handle unicast mac addr when GHTX is on */
+	if (ghtx && !mcast) {
+		pr_err("Could not compute hash bucket\n");
+		return -EINVAL;
+	}
+	crc = crc32_le(crc, (u8 *)eth_addr, ETH_ALEN);
+	crc = bitrev32(crc);
+
+	/* considering the 9 highest order bits in crc H[8:0]:
+	 *if ghtx = 0 H[8:6] (highest order 3 bits) identify the hash register
+	 *and H[5:1] (next 5 bits) identify the hash bit
+	 *if ghts = 1 H[8:5] (highest order 4 bits) identify the hash register
+	 *and H[4:0] (next 5 bits) identify the hash bit.
+	 *
+	 *In bucket index output the low 5 bits identify the hash register
+	 *bit, while the higher 4 bits identify the hash register
+	 */
+
+	if (ghtx) {
+		bucket = (s32)((crc >> 23) & 0x1ff);
+	} else {
+		bucket = (s32)((crc >> 24) & 0xff);
+		/* if !ghtx and mcast the bit must be set in gaddr instead of
+		 *igaddr.
+		 */
+		if (mcast)
+			bucket += 0x100;
+	}
+
+	set_bucket(dtsec->regs, bucket, true);
+
+	/* Create element to be added to the driver hash table */
+	hash_entry = kmalloc(sizeof(*hash_entry), GFP_KERNEL);
+	if (!hash_entry)
+		return -ENOMEM;
+	hash_entry->addr = addr;
+	INIT_LIST_HEAD(&hash_entry->node);
+
+	if (addr & MAC_GROUP_ADDRESS)
+		/* Group Address */
+		list_add_tail(&hash_entry->node,
+			      &dtsec->multicast_addr_hash->lsts[bucket]);
+	else
+		list_add_tail(&hash_entry->node,
+			      &dtsec->unicast_addr_hash->lsts[bucket]);
+
+	return 0;
+}
+
+int dtsec_del_hash_mac_address(struct fman_mac *dtsec, enet_addr_t *eth_addr)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	struct list_head *pos;
+	struct eth_hash_entry *hash_entry = NULL;
+	u64 addr;
+	s32 bucket;
+	u32 crc = 0xFFFFFFFF;
+	bool mcast, ghtx;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	addr = ENET_ADDR_TO_UINT64(*eth_addr);
+
+	ghtx = (bool)((ioread32be(&regs->rctrl) & RCTRL_GHTX) ? true : false);
+	mcast = (bool)((addr & MAC_GROUP_ADDRESS) ? true : false);
+
+	/* Cannot handle unicast mac addr when GHTX is on */
+	if (ghtx && !mcast) {
+		pr_err("Could not compute hash bucket\n");
+		return -EINVAL;
+	}
+	crc = crc32_le(crc, (u8 *)eth_addr, ETH_ALEN);
+	crc = bitrev32(crc);
+
+	if (ghtx) {
+		bucket = (s32)((crc >> 23) & 0x1ff);
+	} else {
+		bucket = (s32)((crc >> 24) & 0xff);
+		/* if !ghtx and mcast the bit must be set
+		 * in gaddr instead of igaddr.
+		 */
+		if (mcast)
+			bucket += 0x100;
+	}
+
+	if (addr & MAC_GROUP_ADDRESS) {
+		/* Group Address */
+		list_for_each(pos,
+			      &dtsec->multicast_addr_hash->lsts[bucket]) {
+			hash_entry = ETH_HASH_ENTRY_OBJ(pos);
+			if (hash_entry->addr == addr) {
+				list_del_init(&hash_entry->node);
+				kfree(hash_entry);
+				break;
+			}
+		}
+		if (list_empty(&dtsec->multicast_addr_hash->lsts[bucket]))
+			set_bucket(dtsec->regs, bucket, false);
+	} else {
+		/* Individual Address */
+		list_for_each(pos,
+			      &dtsec->unicast_addr_hash->lsts[bucket]) {
+			hash_entry = ETH_HASH_ENTRY_OBJ(pos);
+			if (hash_entry->addr == addr) {
+				list_del_init(&hash_entry->node);
+				kfree(hash_entry);
+				break;
+			}
+		}
+		if (list_empty(&dtsec->unicast_addr_hash->lsts[bucket]))
+			set_bucket(dtsec->regs, bucket, false);
+	}
+
+	/* address does not exist */
+	WARN_ON(!hash_entry);
+
+	return 0;
+}
+
+int dtsec_set_promiscuous(struct fman_mac *dtsec, bool new_val)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 tmp;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	/* Set unicast promiscuous */
+	tmp = ioread32be(&regs->rctrl);
+	if (new_val)
+		tmp |= RCTRL_UPROM;
+	else
+		tmp &= ~RCTRL_UPROM;
+
+	iowrite32be(tmp, &regs->rctrl);
+
+	/* Set multicast promiscuous */
+	tmp = ioread32be(&regs->rctrl);
+	if (new_val)
+		tmp |= RCTRL_MPROM;
+	else
+		tmp &= ~RCTRL_MPROM;
+
+	iowrite32be(tmp, &regs->rctrl);
+
+	return 0;
+}
+
+int dtsec_adjust_link(struct fman_mac *dtsec, u16 speed)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 tmp;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->maccfg2);
+
+	/* Full Duplex */
+	tmp |= MACCFG2_FULL_DUPLEX;
+
+	tmp &= ~(MACCFG2_NIBBLE_MODE | MACCFG2_BYTE_MODE);
+	if (speed < SPEED_1000)
+		tmp |= MACCFG2_NIBBLE_MODE;
+	else if (speed == SPEED_1000)
+		tmp |= MACCFG2_BYTE_MODE;
+	iowrite32be(tmp, &regs->maccfg2);
+
+	tmp = ioread32be(&regs->ecntrl);
+	if (speed == SPEED_100)
+		tmp |= DTSEC_ECNTRL_R100M;
+	else
+		tmp &= ~DTSEC_ECNTRL_R100M;
+	iowrite32be(tmp, &regs->ecntrl);
+
+	return 0;
+}
+
+int dtsec_restart_autoneg(struct fman_mac *dtsec)
+{
+	u16 tmp_reg16;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	tmp_reg16 = phy_read(dtsec->tbiphy, MII_BMCR);
+
+	tmp_reg16 &= ~(BMCR_SPEED100 | BMCR_SPEED1000);
+	tmp_reg16 |= (BMCR_ANENABLE | BMCR_ANRESTART |
+		      BMCR_FULLDPLX | BMCR_SPEED1000);
+
+	phy_write(dtsec->tbiphy, MII_BMCR, tmp_reg16);
+
+	return 0;
+}
+
+int dtsec_get_version(struct fman_mac *dtsec, u32 *mac_version)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	*mac_version = ioread32be(&regs->tsec_id);
+
+	return 0;
+}
+
+int dtsec_set_exception(struct fman_mac *dtsec,
+			enum fman_mac_exceptions exception, bool enable)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	u32 bit_mask = 0;
+
+	if (!is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	if (exception != FM_MAC_EX_1G_1588_TS_RX_ERR) {
+		bit_mask = get_exception_flag(exception);
+		if (bit_mask) {
+			if (enable)
+				dtsec->exceptions |= bit_mask;
+			else
+				dtsec->exceptions &= ~bit_mask;
+		} else {
+			pr_err("Undefined exception\n");
+			return -EINVAL;
+		}
+		if (enable)
+			iowrite32be(ioread32be(&regs->imask) | bit_mask,
+				    &regs->imask);
+		else
+			iowrite32be(ioread32be(&regs->imask) & ~bit_mask,
+				    &regs->imask);
+	} else {
+		if (!dtsec->ptp_tsu_enabled) {
+			pr_err("Exception valid for 1588 only\n");
+			return -EINVAL;
+		}
+		switch (exception) {
+		case FM_MAC_EX_1G_1588_TS_RX_ERR:
+			if (enable) {
+				dtsec->en_tsu_err_exeption = true;
+				iowrite32be(ioread32be(&regs->tmr_pemask) |
+					    TMR_PEMASK_TSREEN,
+					    &regs->tmr_pemask);
+			} else {
+				dtsec->en_tsu_err_exeption = false;
+				iowrite32be(ioread32be(&regs->tmr_pemask) &
+					    ~TMR_PEMASK_TSREEN,
+					    &regs->tmr_pemask);
+			}
+			break;
+		default:
+			pr_err("Undefined exception\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+int dtsec_init(struct fman_mac *dtsec)
+{
+	struct dtsec_regs __iomem *regs = dtsec->regs;
+	struct dtsec_cfg *dtsec_drv_param;
+	int err;
+	u16 max_frm_ln;
+	enet_addr_t eth_addr;
+
+	if (is_init_done(dtsec->dtsec_drv_param))
+		return -EINVAL;
+
+	if (DEFAULT_RESET_ON_INIT &&
+	    (fman_reset_mac(dtsec->fm, dtsec->mac_id) != 0)) {
+		pr_err("Can't reset MAC!\n");
+		return -EINVAL;
+	}
+
+	err = check_init_parameters(dtsec);
+	if (err)
+		return err;
+
+	dtsec_drv_param = dtsec->dtsec_drv_param;
+
+	MAKE_ENET_ADDR_FROM_UINT64(dtsec->addr, eth_addr);
+
+	err = init(dtsec->regs, dtsec_drv_param, dtsec->phy_if,
+		   dtsec->max_speed, (u8 *)eth_addr, dtsec->exceptions,
+		   dtsec->tbiphy->mdio.addr);
+	if (err) {
+		free_init_resources(dtsec);
+		pr_err("DTSEC version doesn't support this i/f mode\n");
+		return err;
+	}
+
+	if (dtsec->phy_if == PHY_INTERFACE_MODE_SGMII) {
+		u16 tmp_reg16;
+
+		/* Configure the TBI PHY Control Register */
+		tmp_reg16 = TBICON_CLK_SELECT | TBICON_SOFT_RESET;
+		phy_write(dtsec->tbiphy, MII_TBICON, tmp_reg16);
+
+		tmp_reg16 = TBICON_CLK_SELECT;
+		phy_write(dtsec->tbiphy, MII_TBICON, tmp_reg16);
+
+		tmp_reg16 = (BMCR_RESET | BMCR_ANENABLE |
+			     BMCR_FULLDPLX | BMCR_SPEED1000);
+		phy_write(dtsec->tbiphy, MII_BMCR, tmp_reg16);
+
+		if (dtsec->basex_if)
+			tmp_reg16 = TBIANA_1000X;
+		else
+			tmp_reg16 = TBIANA_SGMII;
+		phy_write(dtsec->tbiphy, MII_ADVERTISE, tmp_reg16);
+
+		tmp_reg16 = (BMCR_ANENABLE | BMCR_ANRESTART |
+			     BMCR_FULLDPLX | BMCR_SPEED1000);
+
+		phy_write(dtsec->tbiphy, MII_BMCR, tmp_reg16);
+	}
+
+	/* Max Frame Length */
+	max_frm_ln = (u16)ioread32be(&regs->maxfrm);
+	err = fman_set_mac_max_frame(dtsec->fm, dtsec->mac_id, max_frm_ln);
+	if (err) {
+		pr_err("Setting max frame length failed\n");
+		free_init_resources(dtsec);
+		return -EINVAL;
+	}
+
+	dtsec->multicast_addr_hash =
+	alloc_hash_table(EXTENDED_HASH_TABLE_SIZE);
+	if (!dtsec->multicast_addr_hash) {
+		free_init_resources(dtsec);
+		pr_err("MC hash table is failed\n");
+		return -ENOMEM;
+	}
+
+	dtsec->unicast_addr_hash = alloc_hash_table(DTSEC_HASH_TABLE_SIZE);
+	if (!dtsec->unicast_addr_hash) {
+		free_init_resources(dtsec);
+		pr_err("UC hash table is failed\n");
+		return -ENOMEM;
+	}
+
+	/* register err intr handler for dtsec to FPM (err) */
+	fman_register_intr(dtsec->fm, FMAN_MOD_MAC, dtsec->mac_id,
+			   FMAN_INTR_TYPE_ERR, dtsec_isr, dtsec);
+	/* register 1588 intr handler for TMR to FPM (normal) */
+	fman_register_intr(dtsec->fm, FMAN_MOD_MAC, dtsec->mac_id,
+			   FMAN_INTR_TYPE_NORMAL, dtsec_1588_isr, dtsec);
+
+	kfree(dtsec_drv_param);
+	dtsec->dtsec_drv_param = NULL;
+
+	return 0;
+}
+
+int dtsec_free(struct fman_mac *dtsec)
+{
+	free_init_resources(dtsec);
+
+	kfree(dtsec->dtsec_drv_param);
+	dtsec->dtsec_drv_param = NULL;
+	kfree(dtsec);
+
+	return 0;
+}
+
+struct fman_mac *dtsec_config(struct fman_mac_params *params)
+{
+	struct fman_mac *dtsec;
+	struct dtsec_cfg *dtsec_drv_param;
+	void __iomem *base_addr;
+
+	base_addr = params->base_addr;
+
+	/* allocate memory for the UCC GETH data structure. */
+	dtsec = kzalloc(sizeof(*dtsec), GFP_KERNEL);
+	if (!dtsec)
+		return NULL;
+
+	/* allocate memory for the d_tsec driver parameters data structure. */
+	dtsec_drv_param = kzalloc(sizeof(*dtsec_drv_param), GFP_KERNEL);
+	if (!dtsec_drv_param)
+		goto err_dtsec;
+
+	/* Plant parameter structure pointer */
+	dtsec->dtsec_drv_param = dtsec_drv_param;
+
+	set_dflts(dtsec_drv_param);
+
+	dtsec->regs = base_addr;
+	dtsec->addr = ENET_ADDR_TO_UINT64(params->addr);
+	dtsec->max_speed = params->max_speed;
+	dtsec->phy_if = params->phy_if;
+	dtsec->mac_id = params->mac_id;
+	dtsec->exceptions = (DTSEC_IMASK_BREN	|
+			     DTSEC_IMASK_RXCEN	|
+			     DTSEC_IMASK_BTEN	|
+			     DTSEC_IMASK_TXCEN	|
+			     DTSEC_IMASK_TXEEN	|
+			     DTSEC_IMASK_ABRTEN	|
+			     DTSEC_IMASK_LCEN	|
+			     DTSEC_IMASK_CRLEN	|
+			     DTSEC_IMASK_XFUNEN	|
+			     DTSEC_IMASK_IFERREN |
+			     DTSEC_IMASK_MAGEN	|
+			     DTSEC_IMASK_TDPEEN	|
+			     DTSEC_IMASK_RDPEEN);
+	dtsec->exception_cb = params->exception_cb;
+	dtsec->event_cb = params->event_cb;
+	dtsec->dev_id = params->dev_id;
+	dtsec->ptp_tsu_enabled = dtsec->dtsec_drv_param->ptp_tsu_en;
+	dtsec->en_tsu_err_exeption = dtsec->dtsec_drv_param->ptp_exception_en;
+
+	dtsec->fm = params->fm;
+	dtsec->basex_if = params->basex_if;
+
+	if (!params->internal_phy_node) {
+		pr_err("TBI PHY node is not available\n");
+		goto err_dtsec_drv_param;
+	}
+
+	dtsec->tbiphy = of_phy_find_device(params->internal_phy_node);
+	if (!dtsec->tbiphy) {
+		pr_err("of_phy_find_device (TBI PHY) failed\n");
+		goto err_dtsec_drv_param;
+	}
+
+	put_device(&dtsec->tbiphy->mdio.dev);
+
+	/* Save FMan revision */
+	fman_get_revision(dtsec->fm, &dtsec->fm_rev_info);
+
+	return dtsec;
+
+err_dtsec_drv_param:
+	kfree(dtsec_drv_param);
+err_dtsec:
+	kfree(dtsec);
+	return NULL;
+}
diff --git a/drivers/net/ethernet/freescale/fman/fman_dtsec.h b/drivers/net/ethernet/freescale/fman/fman_dtsec.h
new file mode 100644
index 000000000000..c4467c072058
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_dtsec.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __DTSEC_H
+#define __DTSEC_H
+
+#include "fman_mac.h"
+
+struct fman_mac *dtsec_config(struct fman_mac_params *params);
+int dtsec_set_promiscuous(struct fman_mac *dtsec, bool new_val);
+int dtsec_modify_mac_address(struct fman_mac *dtsec, enet_addr_t *enet_addr);
+int dtsec_adjust_link(struct fman_mac *dtsec,
+		      u16 speed);
+int dtsec_restart_autoneg(struct fman_mac *dtsec);
+int dtsec_cfg_max_frame_len(struct fman_mac *dtsec, u16 new_val);
+int dtsec_cfg_pad_and_crc(struct fman_mac *dtsec, bool new_val);
+int dtsec_enable(struct fman_mac *dtsec, enum comm_mode mode);
+int dtsec_disable(struct fman_mac *dtsec, enum comm_mode mode);
+int dtsec_init(struct fman_mac *dtsec);
+int dtsec_free(struct fman_mac *dtsec);
+int dtsec_accept_rx_pause_frames(struct fman_mac *dtsec, bool en);
+int dtsec_set_tx_pause_frames(struct fman_mac *dtsec, u8 priority,
+			      u16 pause_time, u16 thresh_time);
+int dtsec_set_exception(struct fman_mac *dtsec,
+			enum fman_mac_exceptions exception, bool enable);
+int dtsec_add_hash_mac_address(struct fman_mac *dtsec, enet_addr_t *eth_addr);
+int dtsec_del_hash_mac_address(struct fman_mac *dtsec, enet_addr_t *eth_addr);
+int dtsec_get_version(struct fman_mac *dtsec, u32 *mac_version);
+
+#endif /* __DTSEC_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_mac.h b/drivers/net/ethernet/freescale/fman/fman_mac.h
new file mode 100644
index 000000000000..8ddeedbcef9c
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_mac.h
@@ -0,0 +1,278 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+/* FM MAC ... */
+#ifndef __FM_MAC_H
+#define __FM_MAC_H
+
+#include "fman.h"
+
+#include <linux/slab.h>
+#include <linux/phy.h>
+#include <linux/if_ether.h>
+
+struct fman_mac;
+
+/* Ethernet Address */
+typedef u8 enet_addr_t[ETH_ALEN];
+
+#define ENET_ADDR_TO_UINT64(_enet_addr)		\
+	(u64)(((u64)(_enet_addr)[0] << 40) |		\
+	      ((u64)(_enet_addr)[1] << 32) |		\
+	      ((u64)(_enet_addr)[2] << 24) |		\
+	      ((u64)(_enet_addr)[3] << 16) |		\
+	      ((u64)(_enet_addr)[4] << 8) |		\
+	      ((u64)(_enet_addr)[5]))
+
+#define MAKE_ENET_ADDR_FROM_UINT64(_addr64, _enet_addr) \
+	do { \
+		int i; \
+		for (i = 0; i < ETH_ALEN; i++) \
+			(_enet_addr)[i] = \
+			(u8)((_addr64) >> ((5 - i) * 8)); \
+	} while (0)
+
+/* defaults */
+#define DEFAULT_RESET_ON_INIT                 false
+
+/* PFC defines */
+#define FSL_FM_PAUSE_TIME_ENABLE	0xf000
+#define FSL_FM_PAUSE_TIME_DISABLE	0
+#define FSL_FM_PAUSE_THRESH_DEFAULT	0
+
+#define FM_MAC_NO_PFC   0xff
+
+/* HASH defines */
+#define ETH_HASH_ENTRY_OBJ(ptr)	\
+	hlist_entry_safe(ptr, struct eth_hash_entry, node)
+
+/* Enumeration (bit flags) of communication modes (Transmit,
+ * receive or both).
+ */
+enum comm_mode {
+	COMM_MODE_NONE = 0,	/* No transmit/receive communication */
+	COMM_MODE_RX = 1,	/* Only receive communication */
+	COMM_MODE_TX = 2,	/* Only transmit communication */
+	COMM_MODE_RX_AND_TX = 3	/* Both transmit and receive communication */
+};
+
+/* FM MAC Exceptions */
+enum fman_mac_exceptions {
+	FM_MAC_EX_10G_MDIO_SCAN_EVENT = 0
+	/* 10GEC MDIO scan event interrupt */
+	, FM_MAC_EX_10G_MDIO_CMD_CMPL
+	/* 10GEC MDIO command completion interrupt */
+	, FM_MAC_EX_10G_REM_FAULT
+	/* 10GEC, mEMAC Remote fault interrupt */
+	, FM_MAC_EX_10G_LOC_FAULT
+	/* 10GEC, mEMAC Local fault interrupt */
+	, FM_MAC_EX_10G_TX_ECC_ER
+	/* 10GEC, mEMAC Transmit frame ECC error interrupt */
+	, FM_MAC_EX_10G_TX_FIFO_UNFL
+	/* 10GEC, mEMAC Transmit FIFO underflow interrupt */
+	, FM_MAC_EX_10G_TX_FIFO_OVFL
+	/* 10GEC, mEMAC Transmit FIFO overflow interrupt */
+	, FM_MAC_EX_10G_TX_ER
+	/* 10GEC Transmit frame error interrupt */
+	, FM_MAC_EX_10G_RX_FIFO_OVFL
+	/* 10GEC, mEMAC Receive FIFO overflow interrupt */
+	, FM_MAC_EX_10G_RX_ECC_ER
+	/* 10GEC, mEMAC Receive frame ECC error interrupt */
+	, FM_MAC_EX_10G_RX_JAB_FRM
+	/* 10GEC Receive jabber frame interrupt */
+	, FM_MAC_EX_10G_RX_OVRSZ_FRM
+	/* 10GEC Receive oversized frame interrupt */
+	, FM_MAC_EX_10G_RX_RUNT_FRM
+	/* 10GEC Receive runt frame interrupt */
+	, FM_MAC_EX_10G_RX_FRAG_FRM
+	/* 10GEC Receive fragment frame interrupt */
+	, FM_MAC_EX_10G_RX_LEN_ER
+	/* 10GEC Receive payload length error interrupt */
+	, FM_MAC_EX_10G_RX_CRC_ER
+	/* 10GEC Receive CRC error interrupt */
+	, FM_MAC_EX_10G_RX_ALIGN_ER
+	/* 10GEC Receive alignment error interrupt */
+	, FM_MAC_EX_1G_BAB_RX
+	/* dTSEC Babbling receive error */
+	, FM_MAC_EX_1G_RX_CTL
+	/* dTSEC Receive control (pause frame) interrupt */
+	, FM_MAC_EX_1G_GRATEFUL_TX_STP_COMPLET
+	/* dTSEC Graceful transmit stop complete */
+	, FM_MAC_EX_1G_BAB_TX
+	/* dTSEC Babbling transmit error */
+	, FM_MAC_EX_1G_TX_CTL
+	/* dTSEC Transmit control (pause frame) interrupt */
+	, FM_MAC_EX_1G_TX_ERR
+	/* dTSEC Transmit error */
+	, FM_MAC_EX_1G_LATE_COL
+	/* dTSEC Late collision */
+	, FM_MAC_EX_1G_COL_RET_LMT
+	/* dTSEC Collision retry limit */
+	, FM_MAC_EX_1G_TX_FIFO_UNDRN
+	/* dTSEC Transmit FIFO underrun */
+	, FM_MAC_EX_1G_MAG_PCKT
+	/* dTSEC Magic Packet detection */
+	, FM_MAC_EX_1G_MII_MNG_RD_COMPLET
+	/* dTSEC MII management read completion */
+	, FM_MAC_EX_1G_MII_MNG_WR_COMPLET
+	/* dTSEC MII management write completion */
+	, FM_MAC_EX_1G_GRATEFUL_RX_STP_COMPLET
+	/* dTSEC Graceful receive stop complete */
+	, FM_MAC_EX_1G_DATA_ERR
+	/* dTSEC Internal data error on transmit */
+	, FM_MAC_1G_RX_DATA_ERR
+	/* dTSEC Internal data error on receive */
+	, FM_MAC_EX_1G_1588_TS_RX_ERR
+	/* dTSEC Time-Stamp Receive Error */
+	, FM_MAC_EX_1G_RX_MIB_CNT_OVFL
+	/* dTSEC MIB counter overflow */
+	, FM_MAC_EX_TS_FIFO_ECC_ERR
+	/* mEMAC Time-stamp FIFO ECC error interrupt;
+	 * not supported on T4240/B4860 rev1 chips
+	 */
+	, FM_MAC_EX_MAGIC_PACKET_INDICATION = FM_MAC_EX_1G_MAG_PCKT
+	/* mEMAC Magic Packet Indication Interrupt */
+};
+
+struct eth_hash_entry {
+	u64 addr;		/* Ethernet Address  */
+	struct list_head node;
+};
+
+typedef void (fman_mac_exception_cb)(void *dev_id,
+				    enum fman_mac_exceptions exceptions);
+
+/* FMan MAC config input */
+struct fman_mac_params {
+	/* Base of memory mapped FM MAC registers */
+	void __iomem *base_addr;
+	/* MAC address of device; First octet is sent first */
+	enet_addr_t addr;
+	/* MAC ID; numbering of dTSEC and 1G-mEMAC:
+	 * 0 - FM_MAX_NUM_OF_1G_MACS;
+	 * numbering of 10G-MAC (TGEC) and 10G-mEMAC:
+	 * 0 - FM_MAX_NUM_OF_10G_MACS
+	 */
+	u8 mac_id;
+	/* PHY interface */
+	phy_interface_t	 phy_if;
+	/* Note that the speed should indicate the maximum rate that
+	 * this MAC should support rather than the actual speed;
+	 */
+	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 */
+	/* SGMII/QSGII interface with 1000BaseX auto-negotiation between MAC
+	 * and phy or backplane; Note: 1000BaseX auto-negotiation relates only
+	 * to interface between MAC and phy/backplane, SGMII phy can still
+	 * synchronize with far-end phy at 10Mbps, 100Mbps or 1000Mbps
+	*/
+	bool basex_if;
+	/* Pointer to TBI/PCS PHY node, used for TBI/PCS PHY access */
+	struct device_node *internal_phy_node;
+};
+
+struct eth_hash_t {
+	u16 size;
+	struct list_head *lsts;
+};
+
+static inline struct eth_hash_entry
+*dequeue_addr_from_hash_entry(struct list_head *addr_lst)
+{
+	struct eth_hash_entry *hash_entry = NULL;
+
+	if (!list_empty(addr_lst)) {
+		hash_entry = ETH_HASH_ENTRY_OBJ(addr_lst->next);
+		list_del_init(&hash_entry->node);
+	}
+	return hash_entry;
+}
+
+static inline void free_hash_table(struct eth_hash_t *hash)
+{
+	struct eth_hash_entry *hash_entry;
+	int i = 0;
+
+	if (hash) {
+		if (hash->lsts) {
+			for (i = 0; i < hash->size; i++) {
+				hash_entry =
+				dequeue_addr_from_hash_entry(&hash->lsts[i]);
+				while (hash_entry) {
+					kfree(hash_entry);
+					hash_entry =
+					dequeue_addr_from_hash_entry(&hash->
+								     lsts[i]);
+				}
+			}
+
+			kfree(hash->lsts);
+		}
+
+		kfree(hash);
+	}
+}
+
+static inline struct eth_hash_t *alloc_hash_table(u16 size)
+{
+	u32 i;
+	struct eth_hash_t *hash;
+
+	/* Allocate address hash table */
+	hash = kmalloc_array(size, sizeof(struct eth_hash_t *), GFP_KERNEL);
+	if (!hash)
+		return NULL;
+
+	hash->size = size;
+
+	hash->lsts = kmalloc_array(hash->size, sizeof(struct list_head),
+				   GFP_KERNEL);
+	if (!hash->lsts) {
+		kfree(hash);
+		return NULL;
+	}
+
+	for (i = 0; i < hash->size; i++)
+		INIT_LIST_HEAD(&hash->lsts[i]);
+
+	return hash;
+}
+
+#endif /* __FM_MAC_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_memac.c b/drivers/net/ethernet/freescale/fman/fman_memac.c
new file mode 100644
index 000000000000..45e98fd8b79e
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_memac.c
@@ -0,0 +1,1170 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "fman_memac.h"
+#include "fman.h"
+
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/phy.h>
+#include <linux/of_mdio.h>
+
+/* PCS registers */
+#define MDIO_SGMII_CR			0x00
+#define MDIO_SGMII_DEV_ABIL_SGMII	0x04
+#define MDIO_SGMII_LINK_TMR_L		0x12
+#define MDIO_SGMII_LINK_TMR_H		0x13
+#define MDIO_SGMII_IF_MODE		0x14
+
+/* SGMII Control defines */
+#define SGMII_CR_AN_EN			0x1000
+#define SGMII_CR_RESTART_AN		0x0200
+#define SGMII_CR_FD			0x0100
+#define SGMII_CR_SPEED_SEL1_1G		0x0040
+#define SGMII_CR_DEF_VAL		(SGMII_CR_AN_EN | SGMII_CR_FD | \
+					 SGMII_CR_SPEED_SEL1_1G)
+
+/* SGMII Device Ability for SGMII defines */
+#define MDIO_SGMII_DEV_ABIL_SGMII_MODE	0x4001
+#define MDIO_SGMII_DEV_ABIL_BASEX_MODE	0x01A0
+
+/* Link timer define */
+#define LINK_TMR_L			0xa120
+#define LINK_TMR_H			0x0007
+#define LINK_TMR_L_BASEX		0xaf08
+#define LINK_TMR_H_BASEX		0x002f
+
+/* SGMII IF Mode defines */
+#define IF_MODE_USE_SGMII_AN		0x0002
+#define IF_MODE_SGMII_EN		0x0001
+#define IF_MODE_SGMII_SPEED_100M	0x0004
+#define IF_MODE_SGMII_SPEED_1G		0x0008
+#define IF_MODE_SGMII_DUPLEX_HALF	0x0010
+
+/* Num of additional exact match MAC adr regs */
+#define MEMAC_NUM_OF_PADDRS 7
+
+/* Control and Configuration Register (COMMAND_CONFIG) */
+#define CMD_CFG_REG_LOWP_RXETY	0x01000000 /* 07 Rx low power indication */
+#define CMD_CFG_TX_LOWP_ENA	0x00800000 /* 08 Tx Low Power Idle Enable */
+#define CMD_CFG_PFC_MODE	0x00080000 /* 12 Enable PFC */
+#define CMD_CFG_NO_LEN_CHK	0x00020000 /* 14 Payload length check disable */
+#define CMD_CFG_SW_RESET	0x00001000 /* 19 S/W Reset, self clearing bit */
+#define CMD_CFG_TX_PAD_EN	0x00000800 /* 20 Enable Tx padding of frames */
+#define CMD_CFG_PAUSE_IGNORE	0x00000100 /* 23 Ignore Pause frame quanta */
+#define CMD_CFG_CRC_FWD		0x00000040 /* 25 Terminate/frwd CRC of frames */
+#define CMD_CFG_PAD_EN		0x00000020 /* 26 Frame padding removal */
+#define CMD_CFG_PROMIS_EN	0x00000010 /* 27 Promiscuous operation enable */
+#define CMD_CFG_RX_EN		0x00000002 /* 30 MAC receive path enable */
+#define CMD_CFG_TX_EN		0x00000001 /* 31 MAC transmit path enable */
+
+/* Transmit FIFO Sections Register (TX_FIFO_SECTIONS) */
+#define TX_FIFO_SECTIONS_TX_EMPTY_MASK			0xFFFF0000
+#define TX_FIFO_SECTIONS_TX_AVAIL_MASK			0x0000FFFF
+#define TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_10G		0x00400000
+#define TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_1G		0x00100000
+#define TX_FIFO_SECTIONS_TX_AVAIL_10G			0x00000019
+#define TX_FIFO_SECTIONS_TX_AVAIL_1G			0x00000020
+#define TX_FIFO_SECTIONS_TX_AVAIL_SLOW_10G		0x00000060
+
+#define GET_TX_EMPTY_DEFAULT_VALUE(_val)				\
+do {									\
+	_val &= ~TX_FIFO_SECTIONS_TX_EMPTY_MASK;			\
+	((_val == TX_FIFO_SECTIONS_TX_AVAIL_10G) ?			\
+			(_val |= TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_10G) :\
+			(_val |= TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_1G));\
+} while (0)
+
+/* Interface Mode Register (IF_MODE) */
+
+#define IF_MODE_MASK		0x00000003 /* 30-31 Mask on i/f mode bits */
+#define IF_MODE_XGMII		0x00000000 /* 30-31 XGMII (10G) interface */
+#define IF_MODE_GMII		0x00000002 /* 30-31 GMII (1G) interface */
+#define IF_MODE_RGMII		0x00000004
+#define IF_MODE_RGMII_AUTO	0x00008000
+#define IF_MODE_RGMII_1000	0x00004000 /* 10 - 1000Mbps RGMII */
+#define IF_MODE_RGMII_100	0x00000000 /* 00 - 100Mbps RGMII */
+#define IF_MODE_RGMII_10	0x00002000 /* 01 - 10Mbps RGMII */
+#define IF_MODE_RGMII_SP_MASK	0x00006000 /* Setsp mask bits */
+#define IF_MODE_RGMII_FD	0x00001000 /* Full duplex RGMII */
+#define IF_MODE_HD		0x00000040 /* Half duplex operation */
+
+/* Hash table Control Register (HASHTABLE_CTRL) */
+#define HASH_CTRL_MCAST_EN	0x00000100
+/* 26-31 Hash table address code */
+#define HASH_CTRL_ADDR_MASK	0x0000003F
+/* MAC mcast indication */
+#define GROUP_ADDRESS		0x0000010000000000LL
+#define HASH_TABLE_SIZE		64	/* Hash tbl size */
+
+/* Interrupt Mask Register (IMASK) */
+#define MEMAC_IMASK_MGI		0x40000000 /* 1 Magic pkt detect indication */
+#define MEMAC_IMASK_TSECC_ER	0x20000000 /* 2 Timestamp FIFO ECC error evnt */
+#define MEMAC_IMASK_TECC_ER	0x02000000 /* 6 Transmit frame ECC error evnt */
+#define MEMAC_IMASK_RECC_ER	0x01000000 /* 7 Receive frame ECC error evnt */
+
+#define MEMAC_ALL_ERRS_IMASK					\
+		((u32)(MEMAC_IMASK_TSECC_ER	|	\
+		       MEMAC_IMASK_TECC_ER		|	\
+		       MEMAC_IMASK_RECC_ER		|	\
+		       MEMAC_IMASK_MGI))
+
+#define MEMAC_IEVNT_PCS			0x80000000 /* PCS (XG). Link sync (G) */
+#define MEMAC_IEVNT_AN			0x40000000 /* Auto-negotiation */
+#define MEMAC_IEVNT_LT			0x20000000 /* Link Training/New page */
+#define MEMAC_IEVNT_MGI			0x00004000 /* Magic pkt detection */
+#define MEMAC_IEVNT_TS_ECC_ER		0x00002000 /* Timestamp FIFO ECC error*/
+#define MEMAC_IEVNT_RX_FIFO_OVFL	0x00001000 /* Rx FIFO overflow */
+#define MEMAC_IEVNT_TX_FIFO_UNFL	0x00000800 /* Tx FIFO underflow */
+#define MEMAC_IEVNT_TX_FIFO_OVFL	0x00000400 /* Tx FIFO overflow */
+#define MEMAC_IEVNT_TX_ECC_ER		0x00000200 /* Tx frame ECC error */
+#define MEMAC_IEVNT_RX_ECC_ER		0x00000100 /* Rx frame ECC error */
+#define MEMAC_IEVNT_LI_FAULT		0x00000080 /* Link Interruption flt */
+#define MEMAC_IEVNT_RX_EMPTY		0x00000040 /* Rx FIFO empty */
+#define MEMAC_IEVNT_TX_EMPTY		0x00000020 /* Tx FIFO empty */
+#define MEMAC_IEVNT_RX_LOWP		0x00000010 /* Low Power Idle */
+#define MEMAC_IEVNT_PHY_LOS		0x00000004 /* Phy loss of signal */
+#define MEMAC_IEVNT_REM_FAULT		0x00000002 /* Remote fault (XGMII) */
+#define MEMAC_IEVNT_LOC_FAULT		0x00000001 /* Local fault (XGMII) */
+
+#define DEFAULT_PAUSE_QUANTA	0xf000
+#define DEFAULT_FRAME_LENGTH	0x600
+#define DEFAULT_TX_IPG_LENGTH	12
+
+#define CLXY_PAUSE_QUANTA_CLX_PQNT	0x0000FFFF
+#define CLXY_PAUSE_QUANTA_CLY_PQNT	0xFFFF0000
+#define CLXY_PAUSE_THRESH_CLX_QTH	0x0000FFFF
+#define CLXY_PAUSE_THRESH_CLY_QTH	0xFFFF0000
+
+struct mac_addr {
+	/* Lower 32 bits of 48-bit MAC address */
+	u32 mac_addr_l;
+	/* Upper 16 bits of 48-bit MAC address */
+	u32 mac_addr_u;
+};
+
+/* memory map */
+struct memac_regs {
+	u32 res0000[2];			/* General Control and Status */
+	u32 command_config;		/* 0x008 Ctrl and cfg */
+	struct mac_addr mac_addr0;	/* 0x00C-0x010 MAC_ADDR_0...1 */
+	u32 maxfrm;			/* 0x014 Max frame length */
+	u32 res0018[1];
+	u32 rx_fifo_sections;		/* Receive FIFO configuration reg */
+	u32 tx_fifo_sections;		/* Transmit FIFO configuration reg */
+	u32 res0024[2];
+	u32 hashtable_ctrl;		/* 0x02C Hash table control */
+	u32 res0030[4];
+	u32 ievent;			/* 0x040 Interrupt event */
+	u32 tx_ipg_length;		/* 0x044 Transmitter inter-packet-gap */
+	u32 res0048;
+	u32 imask;			/* 0x04C Interrupt mask */
+	u32 res0050;
+	u32 pause_quanta[4];		/* 0x054 Pause quanta */
+	u32 pause_thresh[4];		/* 0x064 Pause quanta threshold */
+	u32 rx_pause_status;		/* 0x074 Receive pause status */
+	u32 res0078[2];
+	struct mac_addr mac_addr[MEMAC_NUM_OF_PADDRS];/* 0x80-0x0B4 mac padr */
+	u32 lpwake_timer;		/* 0x0B8 Low Power Wakeup Timer */
+	u32 sleep_timer;		/* 0x0BC Transmit EEE Low Power Timer */
+	u32 res00c0[8];
+	u32 statn_config;		/* 0x0E0 Statistics configuration */
+	u32 res00e4[7];
+	/* Rx Statistics Counter */
+	u32 reoct_l;
+	u32 reoct_u;
+	u32 roct_l;
+	u32 roct_u;
+	u32 raln_l;
+	u32 raln_u;
+	u32 rxpf_l;
+	u32 rxpf_u;
+	u32 rfrm_l;
+	u32 rfrm_u;
+	u32 rfcs_l;
+	u32 rfcs_u;
+	u32 rvlan_l;
+	u32 rvlan_u;
+	u32 rerr_l;
+	u32 rerr_u;
+	u32 ruca_l;
+	u32 ruca_u;
+	u32 rmca_l;
+	u32 rmca_u;
+	u32 rbca_l;
+	u32 rbca_u;
+	u32 rdrp_l;
+	u32 rdrp_u;
+	u32 rpkt_l;
+	u32 rpkt_u;
+	u32 rund_l;
+	u32 rund_u;
+	u32 r64_l;
+	u32 r64_u;
+	u32 r127_l;
+	u32 r127_u;
+	u32 r255_l;
+	u32 r255_u;
+	u32 r511_l;
+	u32 r511_u;
+	u32 r1023_l;
+	u32 r1023_u;
+	u32 r1518_l;
+	u32 r1518_u;
+	u32 r1519x_l;
+	u32 r1519x_u;
+	u32 rovr_l;
+	u32 rovr_u;
+	u32 rjbr_l;
+	u32 rjbr_u;
+	u32 rfrg_l;
+	u32 rfrg_u;
+	u32 rcnp_l;
+	u32 rcnp_u;
+	u32 rdrntp_l;
+	u32 rdrntp_u;
+	u32 res01d0[12];
+	/* Tx Statistics Counter */
+	u32 teoct_l;
+	u32 teoct_u;
+	u32 toct_l;
+	u32 toct_u;
+	u32 res0210[2];
+	u32 txpf_l;
+	u32 txpf_u;
+	u32 tfrm_l;
+	u32 tfrm_u;
+	u32 tfcs_l;
+	u32 tfcs_u;
+	u32 tvlan_l;
+	u32 tvlan_u;
+	u32 terr_l;
+	u32 terr_u;
+	u32 tuca_l;
+	u32 tuca_u;
+	u32 tmca_l;
+	u32 tmca_u;
+	u32 tbca_l;
+	u32 tbca_u;
+	u32 res0258[2];
+	u32 tpkt_l;
+	u32 tpkt_u;
+	u32 tund_l;
+	u32 tund_u;
+	u32 t64_l;
+	u32 t64_u;
+	u32 t127_l;
+	u32 t127_u;
+	u32 t255_l;
+	u32 t255_u;
+	u32 t511_l;
+	u32 t511_u;
+	u32 t1023_l;
+	u32 t1023_u;
+	u32 t1518_l;
+	u32 t1518_u;
+	u32 t1519x_l;
+	u32 t1519x_u;
+	u32 res02a8[6];
+	u32 tcnp_l;
+	u32 tcnp_u;
+	u32 res02c8[14];
+	/* Line Interface Control */
+	u32 if_mode;		/* 0x300 Interface Mode Control */
+	u32 if_status;		/* 0x304 Interface Status */
+	u32 res0308[14];
+	/* HiGig/2 */
+	u32 hg_config;		/* 0x340 Control and cfg */
+	u32 res0344[3];
+	u32 hg_pause_quanta;	/* 0x350 Pause quanta */
+	u32 res0354[3];
+	u32 hg_pause_thresh;	/* 0x360 Pause quanta threshold */
+	u32 res0364[3];
+	u32 hgrx_pause_status;	/* 0x370 Receive pause status */
+	u32 hg_fifos_status;	/* 0x374 fifos status */
+	u32 rhm;		/* 0x378 rx messages counter */
+	u32 thm;		/* 0x37C tx messages counter */
+};
+
+struct memac_cfg {
+	bool reset_on_init;
+	bool pause_ignore;
+	bool promiscuous_mode_enable;
+	struct fixed_phy_status *fixed_link;
+	u16 max_frame_length;
+	u16 pause_quanta;
+	u32 tx_ipg_length;
+};
+
+struct fman_mac {
+	/* Pointer to MAC memory mapped registers */
+	struct memac_regs __iomem *regs;
+	/* MAC address of device */
+	u64 addr;
+	/* Ethernet physical interface */
+	phy_interface_t phy_if;
+	u16 max_speed;
+	void *dev_id; /* device cookie used by the exception cbs */
+	fman_mac_exception_cb *exception_cb;
+	fman_mac_exception_cb *event_cb;
+	/* Pointer to driver's global address hash table  */
+	struct eth_hash_t *multicast_addr_hash;
+	/* Pointer to driver's individual address hash table  */
+	struct eth_hash_t *unicast_addr_hash;
+	u8 mac_id;
+	u32 exceptions;
+	struct memac_cfg *memac_drv_param;
+	void *fm;
+	struct fman_rev_info fm_rev_info;
+	bool basex_if;
+	struct phy_device *pcsphy;
+};
+
+static void add_addr_in_paddr(struct memac_regs __iomem *regs, u8 *adr,
+			      u8 paddr_num)
+{
+	u32 tmp0, tmp1;
+
+	tmp0 = (u32)(adr[0] | adr[1] << 8 | adr[2] << 16 | adr[3] << 24);
+	tmp1 = (u32)(adr[4] | adr[5] << 8);
+
+	if (paddr_num == 0) {
+		iowrite32be(tmp0, &regs->mac_addr0.mac_addr_l);
+		iowrite32be(tmp1, &regs->mac_addr0.mac_addr_u);
+	} else {
+		iowrite32be(tmp0, &regs->mac_addr[paddr_num - 1].mac_addr_l);
+		iowrite32be(tmp1, &regs->mac_addr[paddr_num - 1].mac_addr_u);
+	}
+}
+
+static int reset(struct memac_regs __iomem *regs)
+{
+	u32 tmp;
+	int count;
+
+	tmp = ioread32be(&regs->command_config);
+
+	tmp |= CMD_CFG_SW_RESET;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	count = 100;
+	do {
+		udelay(1);
+	} while ((ioread32be(&regs->command_config) & CMD_CFG_SW_RESET) &&
+		 --count);
+
+	if (count == 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static void set_exception(struct memac_regs __iomem *regs, u32 val,
+			  bool enable)
+{
+	u32 tmp;
+
+	tmp = ioread32be(&regs->imask);
+	if (enable)
+		tmp |= val;
+	else
+		tmp &= ~val;
+
+	iowrite32be(tmp, &regs->imask);
+}
+
+static int init(struct memac_regs __iomem *regs, struct memac_cfg *cfg,
+		phy_interface_t phy_if, u16 speed, bool slow_10g_if,
+		u32 exceptions)
+{
+	u32 tmp;
+
+	/* Config */
+	tmp = 0;
+	if (cfg->promiscuous_mode_enable)
+		tmp |= CMD_CFG_PROMIS_EN;
+	if (cfg->pause_ignore)
+		tmp |= CMD_CFG_PAUSE_IGNORE;
+
+	/* Payload length check disable */
+	tmp |= CMD_CFG_NO_LEN_CHK;
+	/* Enable padding of frames in transmit direction */
+	tmp |= CMD_CFG_TX_PAD_EN;
+
+	tmp |= CMD_CFG_CRC_FWD;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	/* Max Frame Length */
+	iowrite32be((u32)cfg->max_frame_length, &regs->maxfrm);
+
+	/* Pause Time */
+	iowrite32be((u32)cfg->pause_quanta, &regs->pause_quanta[0]);
+	iowrite32be((u32)0, &regs->pause_thresh[0]);
+
+	/* IF_MODE */
+	tmp = 0;
+	switch (phy_if) {
+	case PHY_INTERFACE_MODE_XGMII:
+		tmp |= IF_MODE_XGMII;
+		break;
+	default:
+		tmp |= IF_MODE_GMII;
+		if (phy_if == PHY_INTERFACE_MODE_RGMII)
+			tmp |= IF_MODE_RGMII | IF_MODE_RGMII_AUTO;
+	}
+	iowrite32be(tmp, &regs->if_mode);
+
+	/* TX_FIFO_SECTIONS */
+	tmp = 0;
+	if (phy_if == PHY_INTERFACE_MODE_XGMII) {
+		if (slow_10g_if) {
+			tmp |= (TX_FIFO_SECTIONS_TX_AVAIL_SLOW_10G |
+				TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_10G);
+		} else {
+			tmp |= (TX_FIFO_SECTIONS_TX_AVAIL_10G |
+				TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_10G);
+		}
+	} else {
+		tmp |= (TX_FIFO_SECTIONS_TX_AVAIL_1G |
+			TX_FIFO_SECTIONS_TX_EMPTY_DEFAULT_1G);
+	}
+	iowrite32be(tmp, &regs->tx_fifo_sections);
+
+	/* clear all pending events and set-up interrupts */
+	iowrite32be(0xffffffff, &regs->ievent);
+	set_exception(regs, exceptions, true);
+
+	return 0;
+}
+
+static void set_dflts(struct memac_cfg *cfg)
+{
+	cfg->reset_on_init = false;
+	cfg->promiscuous_mode_enable = false;
+	cfg->pause_ignore = false;
+	cfg->tx_ipg_length = DEFAULT_TX_IPG_LENGTH;
+	cfg->max_frame_length = DEFAULT_FRAME_LENGTH;
+	cfg->pause_quanta = DEFAULT_PAUSE_QUANTA;
+}
+
+static u32 get_mac_addr_hash_code(u64 eth_addr)
+{
+	u64 mask1, mask2;
+	u32 xor_val = 0;
+	u8 i, j;
+
+	for (i = 0; i < 6; i++) {
+		mask1 = eth_addr & (u64)0x01;
+		eth_addr >>= 1;
+
+		for (j = 0; j < 7; j++) {
+			mask2 = eth_addr & (u64)0x01;
+			mask1 ^= mask2;
+			eth_addr >>= 1;
+		}
+
+		xor_val |= (mask1 << (5 - i));
+	}
+
+	return xor_val;
+}
+
+static void setup_sgmii_internal_phy(struct fman_mac *memac,
+				     struct fixed_phy_status *fixed_link)
+{
+	u16 tmp_reg16;
+
+	/* SGMII mode */
+	tmp_reg16 = IF_MODE_SGMII_EN;
+	if (!fixed_link)
+		/* AN enable */
+		tmp_reg16 |= IF_MODE_USE_SGMII_AN;
+	else {
+		switch (fixed_link->speed) {
+		case 10:
+			/* For 10M: IF_MODE[SPEED_10M] = 0 */
+		break;
+		case 100:
+			tmp_reg16 |= IF_MODE_SGMII_SPEED_100M;
+		break;
+		case 1000: /* fallthrough */
+		default:
+			tmp_reg16 |= IF_MODE_SGMII_SPEED_1G;
+		break;
+		}
+		if (!fixed_link->duplex)
+			tmp_reg16 |= IF_MODE_SGMII_DUPLEX_HALF;
+	}
+	phy_write(memac->pcsphy, MDIO_SGMII_IF_MODE, tmp_reg16);
+
+	/* Device ability according to SGMII specification */
+	tmp_reg16 = MDIO_SGMII_DEV_ABIL_SGMII_MODE;
+	phy_write(memac->pcsphy, MDIO_SGMII_DEV_ABIL_SGMII, tmp_reg16);
+
+	/* Adjust link timer for SGMII  -
+	 * According to Cisco SGMII specification the timer should be 1.6 ms.
+	 * The link_timer register is configured in units of the clock.
+	 * - When running as 1G SGMII, Serdes clock is 125 MHz, so
+	 * unit = 1 / (125*10^6 Hz) = 8 ns.
+	 * 1.6 ms in units of 8 ns = 1.6ms / 8ns = 2*10^5 = 0x30d40
+	 * - When running as 2.5G SGMII, Serdes clock is 312.5 MHz, so
+	 * unit = 1 / (312.5*10^6 Hz) = 3.2 ns.
+	 * 1.6 ms in units of 3.2 ns = 1.6ms / 3.2ns = 5*10^5 = 0x7a120.
+	 * Since link_timer value of 1G SGMII will be too short for 2.5 SGMII,
+	 * we always set up here a value of 2.5 SGMII.
+	 */
+	phy_write(memac->pcsphy, MDIO_SGMII_LINK_TMR_H, LINK_TMR_H);
+	phy_write(memac->pcsphy, MDIO_SGMII_LINK_TMR_L, LINK_TMR_L);
+
+	if (!fixed_link)
+		/* Restart AN */
+		tmp_reg16 = SGMII_CR_DEF_VAL | SGMII_CR_RESTART_AN;
+	else
+		/* AN disabled */
+		tmp_reg16 = SGMII_CR_DEF_VAL & ~SGMII_CR_AN_EN;
+	phy_write(memac->pcsphy, 0x0, tmp_reg16);
+}
+
+static void setup_sgmii_internal_phy_base_x(struct fman_mac *memac)
+{
+	u16 tmp_reg16;
+
+	/* AN Device capability  */
+	tmp_reg16 = MDIO_SGMII_DEV_ABIL_BASEX_MODE;
+	phy_write(memac->pcsphy, MDIO_SGMII_DEV_ABIL_SGMII, tmp_reg16);
+
+	/* Adjust link timer for SGMII  -
+	 * For Serdes 1000BaseX auto-negotiation the timer should be 10 ms.
+	 * The link_timer register is configured in units of the clock.
+	 * - When running as 1G SGMII, Serdes clock is 125 MHz, so
+	 * unit = 1 / (125*10^6 Hz) = 8 ns.
+	 * 10 ms in units of 8 ns = 10ms / 8ns = 1250000 = 0x1312d0
+	 * - When running as 2.5G SGMII, Serdes clock is 312.5 MHz, so
+	 * unit = 1 / (312.5*10^6 Hz) = 3.2 ns.
+	 * 10 ms in units of 3.2 ns = 10ms / 3.2ns = 3125000 = 0x2faf08.
+	 * Since link_timer value of 1G SGMII will be too short for 2.5 SGMII,
+	 * we always set up here a value of 2.5 SGMII.
+	 */
+	phy_write(memac->pcsphy, MDIO_SGMII_LINK_TMR_H, LINK_TMR_H_BASEX);
+	phy_write(memac->pcsphy, MDIO_SGMII_LINK_TMR_L, LINK_TMR_L_BASEX);
+
+	/* Restart AN */
+	tmp_reg16 = SGMII_CR_DEF_VAL | SGMII_CR_RESTART_AN;
+	phy_write(memac->pcsphy, 0x0, tmp_reg16);
+}
+
+static int check_init_parameters(struct fman_mac *memac)
+{
+	if (memac->addr == 0) {
+		pr_err("Ethernet MAC must have a valid MAC address\n");
+		return -EINVAL;
+	}
+	if (!memac->exception_cb) {
+		pr_err("Uninitialized exception handler\n");
+		return -EINVAL;
+	}
+	if (!memac->event_cb) {
+		pr_warn("Uninitialize event handler\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int get_exception_flag(enum fman_mac_exceptions exception)
+{
+	u32 bit_mask;
+
+	switch (exception) {
+	case FM_MAC_EX_10G_TX_ECC_ER:
+		bit_mask = MEMAC_IMASK_TECC_ER;
+		break;
+	case FM_MAC_EX_10G_RX_ECC_ER:
+		bit_mask = MEMAC_IMASK_RECC_ER;
+		break;
+	case FM_MAC_EX_TS_FIFO_ECC_ERR:
+		bit_mask = MEMAC_IMASK_TSECC_ER;
+		break;
+	case FM_MAC_EX_MAGIC_PACKET_INDICATION:
+		bit_mask = MEMAC_IMASK_MGI;
+		break;
+	default:
+		bit_mask = 0;
+		break;
+	}
+
+	return bit_mask;
+}
+
+static void memac_err_exception(void *handle)
+{
+	struct fman_mac *memac = (struct fman_mac *)handle;
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 event, imask;
+
+	event = ioread32be(&regs->ievent);
+	imask = ioread32be(&regs->imask);
+
+	/* Imask include both error and notification/event bits.
+	 * Leaving only error bits enabled by imask.
+	 * The imask error bits are shifted by 16 bits offset from
+	 * their corresponding location in the ievent - hence the >> 16
+	 */
+	event &= ((imask & MEMAC_ALL_ERRS_IMASK) >> 16);
+
+	iowrite32be(event, &regs->ievent);
+
+	if (event & MEMAC_IEVNT_TS_ECC_ER)
+		memac->exception_cb(memac->dev_id, FM_MAC_EX_TS_FIFO_ECC_ERR);
+	if (event & MEMAC_IEVNT_TX_ECC_ER)
+		memac->exception_cb(memac->dev_id, FM_MAC_EX_10G_TX_ECC_ER);
+	if (event & MEMAC_IEVNT_RX_ECC_ER)
+		memac->exception_cb(memac->dev_id, FM_MAC_EX_10G_RX_ECC_ER);
+}
+
+static void memac_exception(void *handle)
+{
+	struct fman_mac *memac = (struct fman_mac *)handle;
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 event, imask;
+
+	event = ioread32be(&regs->ievent);
+	imask = ioread32be(&regs->imask);
+
+	/* Imask include both error and notification/event bits.
+	 * Leaving only error bits enabled by imask.
+	 * The imask error bits are shifted by 16 bits offset from
+	 * their corresponding location in the ievent - hence the >> 16
+	 */
+	event &= ((imask & MEMAC_ALL_ERRS_IMASK) >> 16);
+
+	iowrite32be(event, &regs->ievent);
+
+	if (event & MEMAC_IEVNT_MGI)
+		memac->exception_cb(memac->dev_id,
+				    FM_MAC_EX_MAGIC_PACKET_INDICATION);
+}
+
+static void free_init_resources(struct fman_mac *memac)
+{
+	fman_unregister_intr(memac->fm, FMAN_MOD_MAC, memac->mac_id,
+			     FMAN_INTR_TYPE_ERR);
+
+	fman_unregister_intr(memac->fm, FMAN_MOD_MAC, memac->mac_id,
+			     FMAN_INTR_TYPE_NORMAL);
+
+	/* release the driver's group hash table */
+	free_hash_table(memac->multicast_addr_hash);
+	memac->multicast_addr_hash = NULL;
+
+	/* release the driver's individual hash table */
+	free_hash_table(memac->unicast_addr_hash);
+	memac->unicast_addr_hash = NULL;
+}
+
+static bool is_init_done(struct memac_cfg *memac_drv_params)
+{
+	/* Checks if mEMAC driver parameters were initialized */
+	if (!memac_drv_params)
+		return true;
+
+	return false;
+}
+
+int memac_enable(struct fman_mac *memac, enum comm_mode mode)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (mode & COMM_MODE_RX)
+		tmp |= CMD_CFG_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp |= CMD_CFG_TX_EN;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int memac_disable(struct fman_mac *memac, enum comm_mode mode)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (mode & COMM_MODE_RX)
+		tmp &= ~CMD_CFG_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp &= ~CMD_CFG_TX_EN;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int memac_set_promiscuous(struct fman_mac *memac, bool new_val)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (new_val)
+		tmp |= CMD_CFG_PROMIS_EN;
+	else
+		tmp &= ~CMD_CFG_PROMIS_EN;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int memac_adjust_link(struct fman_mac *memac, u16 speed)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->if_mode);
+
+	/* Set full duplex */
+	tmp &= ~IF_MODE_HD;
+
+	if (memac->phy_if == PHY_INTERFACE_MODE_RGMII) {
+		/* Configure RGMII in manual mode */
+		tmp &= ~IF_MODE_RGMII_AUTO;
+		tmp &= ~IF_MODE_RGMII_SP_MASK;
+		/* Full duplex */
+		tmp |= IF_MODE_RGMII_FD;
+
+		switch (speed) {
+		case SPEED_1000:
+			tmp |= IF_MODE_RGMII_1000;
+			break;
+		case SPEED_100:
+			tmp |= IF_MODE_RGMII_100;
+			break;
+		case SPEED_10:
+			tmp |= IF_MODE_RGMII_10;
+			break;
+		default:
+			break;
+		}
+	}
+
+	iowrite32be(tmp, &regs->if_mode);
+
+	return 0;
+}
+
+int memac_cfg_max_frame_len(struct fman_mac *memac, u16 new_val)
+{
+	if (is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	memac->memac_drv_param->max_frame_length = new_val;
+
+	return 0;
+}
+
+int memac_cfg_reset_on_init(struct fman_mac *memac, bool enable)
+{
+	if (is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	memac->memac_drv_param->reset_on_init = enable;
+
+	return 0;
+}
+
+int memac_cfg_fixed_link(struct fman_mac *memac,
+			 struct fixed_phy_status *fixed_link)
+{
+	if (is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	memac->memac_drv_param->fixed_link = fixed_link;
+
+	return 0;
+}
+
+int memac_set_tx_pause_frames(struct fman_mac *memac, u8 priority,
+			      u16 pause_time, u16 thresh_time)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->tx_fifo_sections);
+
+	GET_TX_EMPTY_DEFAULT_VALUE(tmp);
+	iowrite32be(tmp, &regs->tx_fifo_sections);
+
+	tmp = ioread32be(&regs->command_config);
+	tmp &= ~CMD_CFG_PFC_MODE;
+	priority = 0;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	tmp = ioread32be(&regs->pause_quanta[priority / 2]);
+	if (priority % 2)
+		tmp &= CLXY_PAUSE_QUANTA_CLX_PQNT;
+	else
+		tmp &= CLXY_PAUSE_QUANTA_CLY_PQNT;
+	tmp |= ((u32)pause_time << (16 * (priority % 2)));
+	iowrite32be(tmp, &regs->pause_quanta[priority / 2]);
+
+	tmp = ioread32be(&regs->pause_thresh[priority / 2]);
+	if (priority % 2)
+		tmp &= CLXY_PAUSE_THRESH_CLX_QTH;
+	else
+		tmp &= CLXY_PAUSE_THRESH_CLY_QTH;
+	tmp |= ((u32)thresh_time << (16 * (priority % 2)));
+	iowrite32be(tmp, &regs->pause_thresh[priority / 2]);
+
+	return 0;
+}
+
+int memac_accept_rx_pause_frames(struct fman_mac *memac, bool en)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	u32 tmp;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (en)
+		tmp &= ~CMD_CFG_PAUSE_IGNORE;
+	else
+		tmp |= CMD_CFG_PAUSE_IGNORE;
+
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int memac_modify_mac_address(struct fman_mac *memac, enet_addr_t *enet_addr)
+{
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	add_addr_in_paddr(memac->regs, (u8 *)(*enet_addr), 0);
+
+	return 0;
+}
+
+int memac_add_hash_mac_address(struct fman_mac *memac, enet_addr_t *eth_addr)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	struct eth_hash_entry *hash_entry;
+	u32 hash;
+	u64 addr;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	addr = ENET_ADDR_TO_UINT64(*eth_addr);
+
+	if (!(addr & GROUP_ADDRESS)) {
+		/* Unicast addresses not supported in hash */
+		pr_err("Unicast Address\n");
+		return -EINVAL;
+	}
+	hash = get_mac_addr_hash_code(addr) & HASH_CTRL_ADDR_MASK;
+
+	/* Create element to be added to the driver hash table */
+	hash_entry = kmalloc(sizeof(*hash_entry), GFP_KERNEL);
+	if (!hash_entry)
+		return -ENOMEM;
+	hash_entry->addr = addr;
+	INIT_LIST_HEAD(&hash_entry->node);
+
+	list_add_tail(&hash_entry->node,
+		      &memac->multicast_addr_hash->lsts[hash]);
+	iowrite32be(hash | HASH_CTRL_MCAST_EN, &regs->hashtable_ctrl);
+
+	return 0;
+}
+
+int memac_del_hash_mac_address(struct fman_mac *memac, enet_addr_t *eth_addr)
+{
+	struct memac_regs __iomem *regs = memac->regs;
+	struct eth_hash_entry *hash_entry = NULL;
+	struct list_head *pos;
+	u32 hash;
+	u64 addr;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	addr = ENET_ADDR_TO_UINT64(*eth_addr);
+
+	hash = get_mac_addr_hash_code(addr) & HASH_CTRL_ADDR_MASK;
+
+	list_for_each(pos, &memac->multicast_addr_hash->lsts[hash]) {
+		hash_entry = ETH_HASH_ENTRY_OBJ(pos);
+		if (hash_entry->addr == addr) {
+			list_del_init(&hash_entry->node);
+			kfree(hash_entry);
+			break;
+		}
+	}
+	if (list_empty(&memac->multicast_addr_hash->lsts[hash]))
+		iowrite32be(hash & ~HASH_CTRL_MCAST_EN, &regs->hashtable_ctrl);
+
+	return 0;
+}
+
+int memac_set_exception(struct fman_mac *memac,
+			enum fman_mac_exceptions exception, bool enable)
+{
+	u32 bit_mask = 0;
+
+	if (!is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	bit_mask = get_exception_flag(exception);
+	if (bit_mask) {
+		if (enable)
+			memac->exceptions |= bit_mask;
+		else
+			memac->exceptions &= ~bit_mask;
+	} else {
+		pr_err("Undefined exception\n");
+		return -EINVAL;
+	}
+	set_exception(memac->regs, bit_mask, enable);
+
+	return 0;
+}
+
+int memac_init(struct fman_mac *memac)
+{
+	struct memac_cfg *memac_drv_param;
+	u8 i;
+	enet_addr_t eth_addr;
+	bool slow_10g_if = false;
+	struct fixed_phy_status *fixed_link;
+	int err;
+	u32 reg32 = 0;
+
+	if (is_init_done(memac->memac_drv_param))
+		return -EINVAL;
+
+	err = check_init_parameters(memac);
+	if (err)
+		return err;
+
+	memac_drv_param = memac->memac_drv_param;
+
+	if (memac->fm_rev_info.major == 6 && memac->fm_rev_info.minor == 4)
+		slow_10g_if = true;
+
+	/* First, reset the MAC if desired. */
+	if (memac_drv_param->reset_on_init) {
+		err = reset(memac->regs);
+		if (err) {
+			pr_err("mEMAC reset failed\n");
+			return err;
+		}
+	}
+
+	/* MAC Address */
+	MAKE_ENET_ADDR_FROM_UINT64(memac->addr, eth_addr);
+	add_addr_in_paddr(memac->regs, (u8 *)eth_addr, 0);
+
+	fixed_link = memac_drv_param->fixed_link;
+
+	init(memac->regs, memac->memac_drv_param, memac->phy_if,
+	     memac->max_speed, slow_10g_if, memac->exceptions);
+
+	/* FM_RX_FIFO_CORRUPT_ERRATA_10GMAC_A006320 errata workaround
+	 * Exists only in FMan 6.0 and 6.3.
+	 */
+	if ((memac->fm_rev_info.major == 6) &&
+	    ((memac->fm_rev_info.minor == 0) ||
+	    (memac->fm_rev_info.minor == 3))) {
+		/* MAC strips CRC from received frames - this workaround
+		 * should decrease the likelihood of bug appearance
+		 */
+		reg32 = ioread32be(&memac->regs->command_config);
+		reg32 &= ~CMD_CFG_CRC_FWD;
+		iowrite32be(reg32, &memac->regs->command_config);
+	}
+
+	if (memac->phy_if == PHY_INTERFACE_MODE_SGMII) {
+		/* Configure internal SGMII PHY */
+		if (memac->basex_if)
+			setup_sgmii_internal_phy_base_x(memac);
+		else
+			setup_sgmii_internal_phy(memac, fixed_link);
+	} else if (memac->phy_if == PHY_INTERFACE_MODE_QSGMII) {
+		/* Configure 4 internal SGMII PHYs */
+		for (i = 0; i < 4; i++) {
+			u8 qsmgii_phy_addr, phy_addr;
+			/* QSGMII PHY address occupies 3 upper bits of 5-bit
+			 * phy_address; the lower 2 bits are used to extend
+			 * register address space and access each one of 4
+			 * ports inside QSGMII.
+			 */
+			phy_addr = memac->pcsphy->mdio.addr;
+			qsmgii_phy_addr = (u8)((phy_addr << 2) | i);
+			memac->pcsphy->mdio.addr = qsmgii_phy_addr;
+			if (memac->basex_if)
+				setup_sgmii_internal_phy_base_x(memac);
+			else
+				setup_sgmii_internal_phy(memac, fixed_link);
+
+			memac->pcsphy->mdio.addr = phy_addr;
+		}
+	}
+
+	/* Max Frame Length */
+	err = fman_set_mac_max_frame(memac->fm, memac->mac_id,
+				     memac_drv_param->max_frame_length);
+	if (err) {
+		pr_err("settings Mac max frame length is FAILED\n");
+		return err;
+	}
+
+	memac->multicast_addr_hash = alloc_hash_table(HASH_TABLE_SIZE);
+	if (!memac->multicast_addr_hash) {
+		free_init_resources(memac);
+		pr_err("allocation hash table is FAILED\n");
+		return -ENOMEM;
+	}
+
+	memac->unicast_addr_hash = alloc_hash_table(HASH_TABLE_SIZE);
+	if (!memac->unicast_addr_hash) {
+		free_init_resources(memac);
+		pr_err("allocation hash table is FAILED\n");
+		return -ENOMEM;
+	}
+
+	fman_register_intr(memac->fm, FMAN_MOD_MAC, memac->mac_id,
+			   FMAN_INTR_TYPE_ERR, memac_err_exception, memac);
+
+	fman_register_intr(memac->fm, FMAN_MOD_MAC, memac->mac_id,
+			   FMAN_INTR_TYPE_NORMAL, memac_exception, memac);
+
+	kfree(memac_drv_param);
+	memac->memac_drv_param = NULL;
+
+	return 0;
+}
+
+int memac_free(struct fman_mac *memac)
+{
+	free_init_resources(memac);
+
+	kfree(memac->memac_drv_param);
+	kfree(memac);
+
+	return 0;
+}
+
+struct fman_mac *memac_config(struct fman_mac_params *params)
+{
+	struct fman_mac *memac;
+	struct memac_cfg *memac_drv_param;
+	void __iomem *base_addr;
+
+	base_addr = params->base_addr;
+	/* allocate memory for the m_emac data structure */
+	memac = kzalloc(sizeof(*memac), GFP_KERNEL);
+	if (!memac)
+		return NULL;
+
+	/* allocate memory for the m_emac driver parameters data structure */
+	memac_drv_param = kzalloc(sizeof(*memac_drv_param), GFP_KERNEL);
+	if (!memac_drv_param) {
+		memac_free(memac);
+		return NULL;
+	}
+
+	/* Plant parameter structure pointer */
+	memac->memac_drv_param = memac_drv_param;
+
+	set_dflts(memac_drv_param);
+
+	memac->addr = ENET_ADDR_TO_UINT64(params->addr);
+
+	memac->regs = base_addr;
+	memac->max_speed = params->max_speed;
+	memac->phy_if = params->phy_if;
+	memac->mac_id = params->mac_id;
+	memac->exceptions = (MEMAC_IMASK_TSECC_ER | MEMAC_IMASK_TECC_ER |
+			     MEMAC_IMASK_RECC_ER | MEMAC_IMASK_MGI);
+	memac->exception_cb = params->exception_cb;
+	memac->event_cb = params->event_cb;
+	memac->dev_id = params->dev_id;
+	memac->fm = params->fm;
+	memac->basex_if = params->basex_if;
+
+	/* Save FMan revision */
+	fman_get_revision(memac->fm, &memac->fm_rev_info);
+
+	if (memac->phy_if == PHY_INTERFACE_MODE_SGMII) {
+		if (!params->internal_phy_node) {
+			pr_err("PCS PHY node is not available\n");
+			memac_free(memac);
+			return NULL;
+		}
+
+		memac->pcsphy = of_phy_find_device(params->internal_phy_node);
+		if (!memac->pcsphy) {
+			pr_err("of_phy_find_device (PCS PHY) failed\n");
+			memac_free(memac);
+			return NULL;
+		}
+	}
+
+	return memac;
+}
diff --git a/drivers/net/ethernet/freescale/fman/fman_memac.h b/drivers/net/ethernet/freescale/fman/fman_memac.h
new file mode 100644
index 000000000000..173d8e0fd716
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_memac.h
@@ -0,0 +1,60 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __MEMAC_H
+#define __MEMAC_H
+
+#include "fman_mac.h"
+
+#include <linux/netdevice.h>
+
+struct fman_mac *memac_config(struct fman_mac_params *params);
+int memac_set_promiscuous(struct fman_mac *memac, bool new_val);
+int memac_modify_mac_address(struct fman_mac *memac, enet_addr_t *enet_addr);
+int memac_adjust_link(struct fman_mac *memac, u16 speed);
+int memac_cfg_max_frame_len(struct fman_mac *memac, u16 new_val);
+int memac_cfg_reset_on_init(struct fman_mac *memac, bool enable);
+int memac_cfg_fixed_link(struct fman_mac *memac,
+			 struct fixed_phy_status *fixed_link);
+int memac_enable(struct fman_mac *memac, enum comm_mode mode);
+int memac_disable(struct fman_mac *memac, enum comm_mode mode);
+int memac_init(struct fman_mac *memac);
+int memac_free(struct fman_mac *memac);
+int memac_accept_rx_pause_frames(struct fman_mac *memac, bool en);
+int memac_set_tx_pause_frames(struct fman_mac *memac, u8 priority,
+			      u16 pause_time, u16 thresh_time);
+int memac_set_exception(struct fman_mac *memac,
+			enum fman_mac_exceptions exception, bool enable);
+int memac_add_hash_mac_address(struct fman_mac *memac, enet_addr_t *eth_addr);
+int memac_del_hash_mac_address(struct fman_mac *memac, enet_addr_t *eth_addr);
+
+#endif /* __MEMAC_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_muram.c b/drivers/net/ethernet/freescale/fman/fman_muram.c
new file mode 100644
index 000000000000..4eb0e9ac7182
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_muram.c
@@ -0,0 +1,158 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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 "fman_muram.h"
+
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/genalloc.h>
+
+struct muram_info {
+	struct gen_pool *pool;
+	void __iomem *vbase;
+	size_t size;
+	phys_addr_t pbase;
+};
+
+static unsigned long fman_muram_vbase_to_offset(struct muram_info *muram,
+						unsigned long vaddr)
+{
+	return vaddr - (unsigned long)muram->vbase;
+}
+
+/**
+ * fman_muram_init
+ * @base:	Pointer to base of memory mapped FM-MURAM.
+ * @size:	Size of the FM-MURAM partition.
+ *
+ * Creates partition in the MURAM.
+ * The routine returns a pointer to the MURAM partition.
+ * This pointer must be passed as to all other FM-MURAM function calls.
+ * No actual initialization or configuration of FM_MURAM hardware is done by
+ * this routine.
+ *
+ * Return: pointer to FM-MURAM object, or NULL for Failure.
+ */
+struct muram_info *fman_muram_init(phys_addr_t base, size_t size)
+{
+	struct muram_info *muram;
+	void __iomem *vaddr;
+	int ret;
+
+	muram = kzalloc(sizeof(*muram), GFP_KERNEL);
+	if (!muram)
+		return NULL;
+
+	muram->pool = gen_pool_create(ilog2(64), -1);
+	if (!muram->pool) {
+		pr_err("%s(): MURAM pool create failed\n", __func__);
+		goto  muram_free;
+	}
+
+	vaddr = ioremap(base, size);
+	if (!vaddr) {
+		pr_err("%s(): MURAM ioremap failed\n", __func__);
+		goto pool_destroy;
+	}
+
+	ret = gen_pool_add_virt(muram->pool, (unsigned long)vaddr,
+				base, size, -1);
+	if (ret < 0) {
+		pr_err("%s(): MURAM pool add failed\n", __func__);
+		iounmap(vaddr);
+		goto pool_destroy;
+	}
+
+	memset_io(vaddr, 0, (int)size);
+
+	muram->vbase = vaddr;
+	muram->pbase = base;
+	return muram;
+
+pool_destroy:
+	gen_pool_destroy(muram->pool);
+muram_free:
+	kfree(muram);
+	return NULL;
+}
+
+/**
+ * fman_muram_offset_to_vbase
+ * @muram:	FM-MURAM module pointer.
+ * @offset:	the offset of the memory block
+ *
+ * Gives the address of the memory region from specific offset
+ *
+ * Return: The address of the memory block
+ */
+unsigned long fman_muram_offset_to_vbase(struct muram_info *muram,
+					 unsigned long offset)
+{
+	return offset + (unsigned long)muram->vbase;
+}
+
+/**
+ * fman_muram_alloc
+ * @muram:	FM-MURAM module pointer.
+ * @size:	Size of the memory to be allocated.
+ *
+ * Allocate some memory from FM-MURAM partition.
+ *
+ * Return: address of the allocated memory; NULL otherwise.
+ */
+int fman_muram_alloc(struct muram_info *muram, size_t size)
+{
+	unsigned long vaddr;
+
+	vaddr = gen_pool_alloc(muram->pool, size);
+	if (!vaddr)
+		return -ENOMEM;
+
+	memset_io((void __iomem *)vaddr, 0, size);
+
+	return fman_muram_vbase_to_offset(muram, vaddr);
+}
+
+/**
+ * fman_muram_free_mem
+ * muram:	FM-MURAM module pointer.
+ * offset:	offset of the memory region to be freed.
+ * size:	size of the memory to be freed.
+ *
+ * Free an allocated memory from FM-MURAM partition.
+ */
+void fman_muram_free_mem(struct muram_info *muram, u32 offset, size_t size)
+{
+	unsigned long addr = fman_muram_offset_to_vbase(muram, offset);
+
+	gen_pool_free(muram->pool, addr, size);
+}
diff --git a/drivers/net/ethernet/freescale/fman/fman_muram.h b/drivers/net/ethernet/freescale/fman/fman_muram.h
new file mode 100644
index 000000000000..dbf0af9e5bb5
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_muram.h
@@ -0,0 +1,51 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef __FM_MURAM_EXT
+#define __FM_MURAM_EXT
+
+#include <linux/types.h>
+
+#define FM_MURAM_INVALID_ALLOCATION	-1
+
+/* Structure for FM MURAM information */
+struct muram_info;
+
+struct muram_info *fman_muram_init(phys_addr_t base, size_t size);
+
+unsigned long fman_muram_offset_to_vbase(struct muram_info *muram,
+					 unsigned long offset);
+
+int fman_muram_alloc(struct muram_info *muram, size_t size);
+
+void fman_muram_free_mem(struct muram_info *muram, u32 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
new file mode 100644
index 000000000000..70c198d072dc
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_port.c
@@ -0,0 +1,1778 @@
+/*
+ * Copyright 2008 - 2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "fman_port.h"
+#include "fman.h"
+#include "fman_sp.h"
+
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/of_platform.h>
+#include <linux/of_address.h>
+#include <linux/delay.h>
+#include <linux/libfdt_env.h>
+
+/* Queue ID */
+#define DFLT_FQ_ID		0x00FFFFFF
+
+/* General defines */
+#define PORT_BMI_FIFO_UNITS		0x100
+
+#define MAX_PORT_FIFO_SIZE(bmi_max_fifo_size)	\
+	min((u32)bmi_max_fifo_size, (u32)1024 * FMAN_BMI_FIFO_UNITS)
+
+#define PORT_CG_MAP_NUM			8
+#define PORT_PRS_RESULT_WORDS_NUM	8
+#define PORT_IC_OFFSET_UNITS		0x10
+
+#define MIN_EXT_BUF_SIZE		64
+
+#define BMI_PORT_REGS_OFFSET				0
+#define QMI_PORT_REGS_OFFSET				0x400
+
+/* Default values */
+#define DFLT_PORT_BUFFER_PREFIX_CONTEXT_DATA_ALIGN		\
+	DFLT_FM_SP_BUFFER_PREFIX_CONTEXT_DATA_ALIGN
+
+#define DFLT_PORT_CUT_BYTES_FROM_END		4
+
+#define DFLT_PORT_ERRORS_TO_DISCARD		FM_PORT_FRM_ERR_CLS_DISCARD
+#define DFLT_PORT_MAX_FRAME_LENGTH		9600
+
+#define DFLT_PORT_RX_FIFO_PRI_ELEVATION_LEV(bmi_max_fifo_size)	\
+	MAX_PORT_FIFO_SIZE(bmi_max_fifo_size)
+
+#define DFLT_PORT_RX_FIFO_THRESHOLD(major, bmi_max_fifo_size)	\
+	(major == 6 ?						\
+	MAX_PORT_FIFO_SIZE(bmi_max_fifo_size) :		\
+	(MAX_PORT_FIFO_SIZE(bmi_max_fifo_size) * 3 / 4))	\
+
+#define DFLT_PORT_EXTRA_NUM_OF_FIFO_BUFS		0
+
+/* QMI defines */
+#define QMI_DEQ_CFG_SUBPORTAL_MASK		0x1f
+
+#define QMI_PORT_CFG_EN				0x80000000
+#define QMI_PORT_STATUS_DEQ_FD_BSY		0x20000000
+
+#define QMI_DEQ_CFG_PRI				0x80000000
+#define QMI_DEQ_CFG_TYPE1			0x10000000
+#define QMI_DEQ_CFG_TYPE2			0x20000000
+#define QMI_DEQ_CFG_TYPE3			0x30000000
+#define QMI_DEQ_CFG_PREFETCH_PARTIAL		0x01000000
+#define QMI_DEQ_CFG_PREFETCH_FULL		0x03000000
+#define QMI_DEQ_CFG_SP_MASK			0xf
+#define QMI_DEQ_CFG_SP_SHIFT			20
+
+#define QMI_BYTE_COUNT_LEVEL_CONTROL(_type)	\
+	(_type == FMAN_PORT_TYPE_TX ? 0x1400 : 0x400)
+
+/* BMI defins */
+#define BMI_EBD_EN				0x80000000
+
+#define BMI_PORT_CFG_EN				0x80000000
+
+#define BMI_PORT_STATUS_BSY			0x80000000
+
+#define BMI_DMA_ATTR_SWP_SHIFT			FMAN_SP_DMA_ATTR_SWP_SHIFT
+#define BMI_DMA_ATTR_WRITE_OPTIMIZE		FMAN_SP_DMA_ATTR_WRITE_OPTIMIZE
+
+#define BMI_RX_FIFO_PRI_ELEVATION_SHIFT	16
+#define BMI_RX_FIFO_THRESHOLD_ETHE		0x80000000
+
+#define BMI_FRAME_END_CS_IGNORE_SHIFT		24
+#define BMI_FRAME_END_CS_IGNORE_MASK		0x0000001f
+
+#define BMI_RX_FRAME_END_CUT_SHIFT		16
+#define BMI_RX_FRAME_END_CUT_MASK		0x0000001f
+
+#define BMI_IC_TO_EXT_SHIFT			FMAN_SP_IC_TO_EXT_SHIFT
+#define BMI_IC_TO_EXT_MASK			0x0000001f
+#define BMI_IC_FROM_INT_SHIFT			FMAN_SP_IC_FROM_INT_SHIFT
+#define BMI_IC_FROM_INT_MASK			0x0000000f
+#define BMI_IC_SIZE_MASK			0x0000001f
+
+#define BMI_INT_BUF_MARG_SHIFT			28
+#define BMI_INT_BUF_MARG_MASK			0x0000000f
+#define BMI_EXT_BUF_MARG_START_SHIFT		FMAN_SP_EXT_BUF_MARG_START_SHIFT
+#define BMI_EXT_BUF_MARG_START_MASK		0x000001ff
+#define BMI_EXT_BUF_MARG_END_MASK		0x000001ff
+
+#define BMI_CMD_MR_LEAC				0x00200000
+#define BMI_CMD_MR_SLEAC			0x00100000
+#define BMI_CMD_MR_MA				0x00080000
+#define BMI_CMD_MR_DEAS				0x00040000
+#define BMI_CMD_RX_MR_DEF			(BMI_CMD_MR_LEAC | \
+						BMI_CMD_MR_SLEAC | \
+						BMI_CMD_MR_MA | \
+						BMI_CMD_MR_DEAS)
+#define BMI_CMD_TX_MR_DEF			0
+
+#define BMI_CMD_ATTR_ORDER			0x80000000
+#define BMI_CMD_ATTR_SYNC			0x02000000
+#define BMI_CMD_ATTR_COLOR_SHIFT		26
+
+#define BMI_FIFO_PIPELINE_DEPTH_SHIFT		12
+#define BMI_FIFO_PIPELINE_DEPTH_MASK		0x0000000f
+#define BMI_NEXT_ENG_FD_BITS_SHIFT		24
+
+#define BMI_EXT_BUF_POOL_VALID			FMAN_SP_EXT_BUF_POOL_VALID
+#define BMI_EXT_BUF_POOL_EN_COUNTER		FMAN_SP_EXT_BUF_POOL_EN_COUNTER
+#define BMI_EXT_BUF_POOL_BACKUP		FMAN_SP_EXT_BUF_POOL_BACKUP
+#define BMI_EXT_BUF_POOL_ID_SHIFT		16
+#define BMI_EXT_BUF_POOL_ID_MASK		0x003F0000
+#define BMI_POOL_DEP_NUM_OF_POOLS_SHIFT	16
+
+#define BMI_TX_FIFO_MIN_FILL_SHIFT		16
+
+#define BMI_PRIORITY_ELEVATION_LEVEL ((0x3FF + 1) * PORT_BMI_FIFO_UNITS)
+#define BMI_FIFO_THRESHOLD	      ((0x3FF + 1) * PORT_BMI_FIFO_UNITS)
+
+#define BMI_DEQUEUE_PIPELINE_DEPTH(_type, _speed)		\
+	((_type == FMAN_PORT_TYPE_TX && _speed == 10000) ? 4 : 1)
+
+#define RX_ERRS_TO_ENQ				  \
+	(FM_PORT_FRM_ERR_DMA			| \
+	FM_PORT_FRM_ERR_PHYSICAL		| \
+	FM_PORT_FRM_ERR_SIZE			| \
+	FM_PORT_FRM_ERR_EXTRACTION		| \
+	FM_PORT_FRM_ERR_NO_SCHEME		| \
+	FM_PORT_FRM_ERR_PRS_TIMEOUT		| \
+	FM_PORT_FRM_ERR_PRS_ILL_INSTRUCT	| \
+	FM_PORT_FRM_ERR_BLOCK_LIMIT_EXCEEDED	| \
+	FM_PORT_FRM_ERR_PRS_HDR_ERR		| \
+	FM_PORT_FRM_ERR_KEYSIZE_OVERFLOW	| \
+	FM_PORT_FRM_ERR_IPRE)
+
+/* NIA defines */
+#define NIA_ORDER_RESTOR				0x00800000
+#define NIA_ENG_BMI					0x00500000
+#define NIA_ENG_QMI_ENQ					0x00540000
+#define NIA_ENG_QMI_DEQ					0x00580000
+
+#define NIA_BMI_AC_ENQ_FRAME				0x00000002
+#define NIA_BMI_AC_TX_RELEASE				0x000002C0
+#define NIA_BMI_AC_RELEASE				0x000000C0
+#define NIA_BMI_AC_TX					0x00000274
+#define NIA_BMI_AC_FETCH_ALL_FRAME			0x0000020c
+
+/* Port IDs */
+#define TX_10G_PORT_BASE		0x30
+#define RX_10G_PORT_BASE		0x10
+
+/* BMI Rx port register map */
+struct fman_port_rx_bmi_regs {
+	u32 fmbm_rcfg;		/* Rx Configuration */
+	u32 fmbm_rst;		/* Rx Status */
+	u32 fmbm_rda;		/* Rx DMA attributes */
+	u32 fmbm_rfp;		/* Rx FIFO Parameters */
+	u32 fmbm_rfed;		/* Rx Frame End Data */
+	u32 fmbm_ricp;		/* Rx Internal Context Parameters */
+	u32 fmbm_rim;		/* Rx Internal Buffer Margins */
+	u32 fmbm_rebm;		/* Rx External Buffer Margins */
+	u32 fmbm_rfne;		/* Rx Frame Next Engine */
+	u32 fmbm_rfca;		/* Rx Frame Command Attributes. */
+	u32 fmbm_rfpne;		/* Rx Frame Parser Next Engine */
+	u32 fmbm_rpso;		/* Rx Parse Start Offset */
+	u32 fmbm_rpp;		/* Rx Policer Profile  */
+	u32 fmbm_rccb;		/* Rx Coarse Classification Base */
+	u32 fmbm_reth;		/* Rx Excessive Threshold */
+	u32 reserved003c[1];	/* (0x03C 0x03F) */
+	u32 fmbm_rprai[PORT_PRS_RESULT_WORDS_NUM];
+	/* Rx Parse Results Array Init */
+	u32 fmbm_rfqid;		/* Rx Frame Queue ID */
+	u32 fmbm_refqid;	/* Rx Error Frame Queue ID */
+	u32 fmbm_rfsdm;		/* Rx Frame Status Discard Mask */
+	u32 fmbm_rfsem;		/* Rx Frame Status Error Mask */
+	u32 fmbm_rfene;		/* Rx Frame Enqueue Next Engine */
+	u32 reserved0074[0x2];	/* (0x074-0x07C)  */
+	u32 fmbm_rcmne;		/* Rx Frame Continuous Mode Next Engine */
+	u32 reserved0080[0x20];	/* (0x080 0x0FF)  */
+	u32 fmbm_ebmpi[FMAN_PORT_MAX_EXT_POOLS_NUM];
+	/* Buffer Manager pool Information- */
+	u32 fmbm_acnt[FMAN_PORT_MAX_EXT_POOLS_NUM];	/* Allocate Counter- */
+	u32 reserved0130[8];	/* 0x130/0x140 - 0x15F reserved - */
+	u32 fmbm_rcgm[PORT_CG_MAP_NUM];	/* Congestion Group Map */
+	u32 fmbm_mpd;		/* BM Pool Depletion  */
+	u32 reserved0184[0x1F];	/* (0x184 0x1FF) */
+	u32 fmbm_rstc;		/* Rx Statistics Counters */
+	u32 fmbm_rfrc;		/* Rx Frame Counter */
+	u32 fmbm_rfbc;		/* Rx Bad Frames Counter */
+	u32 fmbm_rlfc;		/* Rx Large Frames Counter */
+	u32 fmbm_rffc;		/* Rx Filter Frames Counter */
+	u32 fmbm_rfdc;		/* Rx Frame Discard Counter */
+	u32 fmbm_rfldec;		/* Rx Frames List DMA Error Counter */
+	u32 fmbm_rodc;		/* Rx Out of Buffers Discard nntr */
+	u32 fmbm_rbdc;		/* Rx Buffers Deallocate Counter */
+	u32 fmbm_rpec;		/* RX Prepare to enqueue Counte */
+	u32 reserved0224[0x16];	/* (0x224 0x27F) */
+	u32 fmbm_rpc;		/* Rx Performance Counters */
+	u32 fmbm_rpcp;		/* Rx Performance Count Parameters */
+	u32 fmbm_rccn;		/* Rx Cycle Counter */
+	u32 fmbm_rtuc;		/* Rx Tasks Utilization Counter */
+	u32 fmbm_rrquc;		/* Rx Receive Queue Utilization cntr */
+	u32 fmbm_rduc;		/* Rx DMA Utilization Counter */
+	u32 fmbm_rfuc;		/* Rx FIFO Utilization Counter */
+	u32 fmbm_rpac;		/* Rx Pause Activation Counter */
+	u32 reserved02a0[0x18];	/* (0x2A0 0x2FF) */
+	u32 fmbm_rdcfg[0x3];	/* Rx Debug Configuration */
+	u32 fmbm_rgpr;		/* Rx General Purpose Register */
+	u32 reserved0310[0x3a];
+};
+
+/* BMI Tx port register map */
+struct fman_port_tx_bmi_regs {
+	u32 fmbm_tcfg;		/* Tx Configuration */
+	u32 fmbm_tst;		/* Tx Status */
+	u32 fmbm_tda;		/* Tx DMA attributes */
+	u32 fmbm_tfp;		/* Tx FIFO Parameters */
+	u32 fmbm_tfed;		/* Tx Frame End Data */
+	u32 fmbm_ticp;		/* Tx Internal Context Parameters */
+	u32 fmbm_tfdne;		/* Tx Frame Dequeue Next Engine. */
+	u32 fmbm_tfca;		/* Tx Frame Command attribute. */
+	u32 fmbm_tcfqid;	/* Tx Confirmation Frame Queue ID. */
+	u32 fmbm_tefqid;	/* Tx Frame Error Queue ID */
+	u32 fmbm_tfene;		/* Tx Frame Enqueue Next Engine */
+	u32 fmbm_trlmts;	/* Tx Rate Limiter Scale */
+	u32 fmbm_trlmt;		/* Tx Rate Limiter */
+	u32 reserved0034[0x0e];	/* (0x034-0x6c) */
+	u32 fmbm_tccb;		/* Tx Coarse Classification base */
+	u32 fmbm_tfne;		/* Tx Frame Next Engine */
+	u32 fmbm_tpfcm[0x02];
+	/* Tx Priority based Flow Control (PFC) Mapping */
+	u32 fmbm_tcmne;		/* Tx Frame Continuous Mode Next Engine */
+	u32 reserved0080[0x60];	/* (0x080-0x200) */
+	u32 fmbm_tstc;		/* Tx Statistics Counters */
+	u32 fmbm_tfrc;		/* Tx Frame Counter */
+	u32 fmbm_tfdc;		/* Tx Frames Discard Counter */
+	u32 fmbm_tfledc;	/* Tx Frame len error discard cntr */
+	u32 fmbm_tfufdc;	/* Tx Frame unsprt frmt discard cntr */
+	u32 fmbm_tbdc;		/* Tx Buffers Deallocate Counter */
+	u32 reserved0218[0x1A];	/* (0x218-0x280) */
+	u32 fmbm_tpc;		/* Tx Performance Counters */
+	u32 fmbm_tpcp;		/* Tx Performance Count Parameters */
+	u32 fmbm_tccn;		/* Tx Cycle Counter */
+	u32 fmbm_ttuc;		/* Tx Tasks Utilization Counter */
+	u32 fmbm_ttcquc;	/* Tx Transmit conf Q util Counter */
+	u32 fmbm_tduc;		/* Tx DMA Utilization Counter */
+	u32 fmbm_tfuc;		/* Tx FIFO Utilization Counter */
+	u32 reserved029c[16];	/* (0x29C-0x2FF) */
+	u32 fmbm_tdcfg[0x3];	/* Tx Debug Configuration */
+	u32 fmbm_tgpr;		/* Tx General Purpose Register */
+	u32 reserved0310[0x3a]; /* (0x310-0x3FF) */
+};
+
+/* BMI port register map */
+union fman_port_bmi_regs {
+	struct fman_port_rx_bmi_regs rx;
+	struct fman_port_tx_bmi_regs tx;
+};
+
+/* QMI port register map */
+struct fman_port_qmi_regs {
+	u32 fmqm_pnc;		/* PortID n Configuration Register */
+	u32 fmqm_pns;		/* PortID n Status Register */
+	u32 fmqm_pnts;		/* PortID n Task Status Register */
+	u32 reserved00c[4];	/* 0xn00C - 0xn01B */
+	u32 fmqm_pnen;		/* PortID n Enqueue NIA Register */
+	u32 fmqm_pnetfc;		/* PortID n Enq Total Frame Counter */
+	u32 reserved024[2];	/* 0xn024 - 0x02B */
+	u32 fmqm_pndn;		/* PortID n Dequeue NIA Register */
+	u32 fmqm_pndc;		/* PortID n Dequeue Config Register */
+	u32 fmqm_pndtfc;		/* PortID n Dequeue tot Frame cntr */
+	u32 fmqm_pndfdc;		/* PortID n Dequeue FQID Dflt Cntr */
+	u32 fmqm_pndcc;		/* PortID n Dequeue Confirm Counter */
+};
+
+/* QMI dequeue prefetch modes */
+enum fman_port_deq_prefetch {
+	FMAN_PORT_DEQ_NO_PREFETCH, /* No prefetch mode */
+	FMAN_PORT_DEQ_PART_PREFETCH, /* Partial prefetch mode */
+	FMAN_PORT_DEQ_FULL_PREFETCH /* Full prefetch mode */
+};
+
+/* A structure for defining FM port resources */
+struct fman_port_rsrc {
+	u32 num; /* Committed required resource */
+	u32 extra; /* Extra (not committed) required resource */
+};
+
+enum fman_port_dma_swap {
+	FMAN_PORT_DMA_NO_SWAP,	/* No swap, transfer data as is */
+	FMAN_PORT_DMA_SWAP_LE,
+	/* The transferred data should be swapped in PPC Little Endian mode */
+	FMAN_PORT_DMA_SWAP_BE
+	/* The transferred data should be swapped in Big Endian mode */
+};
+
+/* Default port color */
+enum fman_port_color {
+	FMAN_PORT_COLOR_GREEN,	/* Default port color is green */
+	FMAN_PORT_COLOR_YELLOW,	/* Default port color is yellow */
+	FMAN_PORT_COLOR_RED,		/* Default port color is red */
+	FMAN_PORT_COLOR_OVERRIDE	/* Ignore color */
+};
+
+/* QMI dequeue from the SP channel - types */
+enum fman_port_deq_type {
+	FMAN_PORT_DEQ_BY_PRI,
+	/* Priority precedence and Intra-Class scheduling */
+	FMAN_PORT_DEQ_ACTIVE_FQ,
+	/* Active FQ precedence and Intra-Class scheduling */
+	FMAN_PORT_DEQ_ACTIVE_FQ_NO_ICS
+	/* Active FQ precedence and override Intra-Class scheduling */
+};
+
+/* External buffer pools configuration */
+struct fman_port_bpools {
+	u8 count;			/* Num of pools to set up */
+	bool counters_enable;		/* Enable allocate counters */
+	u8 grp_bp_depleted_num;
+	/* Number of depleted pools - if reached the BMI indicates
+	 * the MAC to send a pause frame
+	 */
+	struct {
+		u8 bpid;		/* BM pool ID */
+		u16 size;
+		/* Pool's size - must be in ascending order */
+		bool is_backup;
+		/* If this is a backup pool */
+		bool grp_bp_depleted;
+		/* Consider this buffer in multiple pools depletion criteria */
+		bool single_bp_depleted;
+		/* Consider this buffer in single pool depletion criteria */
+	} bpool[FMAN_PORT_MAX_EXT_POOLS_NUM];
+};
+
+struct fman_port_cfg {
+	u32 dflt_fqid;
+	u32 err_fqid;
+	u8 deq_sp;
+	bool deq_high_priority;
+	enum fman_port_deq_type deq_type;
+	enum fman_port_deq_prefetch deq_prefetch_option;
+	u16 deq_byte_cnt;
+	u8 cheksum_last_bytes_ignore;
+	u8 rx_cut_end_bytes;
+	struct fman_buf_pool_depletion buf_pool_depletion;
+	struct fman_ext_pools ext_buf_pools;
+	u32 tx_fifo_min_level;
+	u32 tx_fifo_low_comf_level;
+	u32 rx_pri_elevation;
+	u32 rx_fifo_thr;
+	struct fman_sp_buf_margins buf_margins;
+	u32 int_buf_start_margin;
+	struct fman_sp_int_context_data_copy int_context;
+	u32 discard_mask;
+	u32 err_mask;
+	struct fman_buffer_prefix_content buffer_prefix_content;
+	bool dont_release_buf;
+
+	u8 rx_fd_bits;
+	u32 tx_fifo_deq_pipeline_depth;
+	bool errata_A006320;
+	bool excessive_threshold_register;
+	bool fmbm_tfne_has_features;
+
+	enum fman_port_dma_swap dma_swap_data;
+	enum fman_port_color color;
+};
+
+struct fman_port_rx_pools_params {
+	u8 num_of_pools;
+	u16 second_largest_buf_size;
+	u16 largest_buf_size;
+};
+
+struct fman_port_dts_params {
+	void __iomem *base_addr;	/* FMan port virtual memory */
+	enum fman_port_type type;	/* Port type */
+	u16 speed;			/* Port speed */
+	u8 id;				/* HW Port Id */
+	u32 qman_channel_id;		/* QMan channel id (non RX only) */
+	struct fman *fman;		/* FMan Handle */
+};
+
+struct fman_port {
+	void *fm;
+	struct device *dev;
+	struct fman_rev_info rev_info;
+	u8 port_id;
+	enum fman_port_type port_type;
+	u16 port_speed;
+
+	union fman_port_bmi_regs __iomem *bmi_regs;
+	struct fman_port_qmi_regs __iomem *qmi_regs;
+
+	struct fman_sp_buffer_offsets buffer_offsets;
+
+	u8 internal_buf_offset;
+	struct fman_ext_pools ext_buf_pools;
+
+	u16 max_frame_length;
+	struct fman_port_rsrc open_dmas;
+	struct fman_port_rsrc tasks;
+	struct fman_port_rsrc fifo_bufs;
+	struct fman_port_rx_pools_params rx_pools_params;
+
+	struct fman_port_cfg *cfg;
+	struct fman_port_dts_params dts_params;
+
+	u8 ext_pools_num;
+	u32 max_port_fifo_size;
+	u32 max_num_of_ext_pools;
+	u32 max_num_of_sub_portals;
+	u32 bm_max_num_of_pools;
+};
+
+static int init_bmi_rx(struct fman_port *port)
+{
+	struct fman_port_rx_bmi_regs __iomem *regs = &port->bmi_regs->rx;
+	struct fman_port_cfg *cfg = port->cfg;
+	u32 tmp;
+
+	/* DMA attributes */
+	tmp = (u32)cfg->dma_swap_data << BMI_DMA_ATTR_SWP_SHIFT;
+	/* Enable write optimization */
+	tmp |= BMI_DMA_ATTR_WRITE_OPTIMIZE;
+	iowrite32be(tmp, &regs->fmbm_rda);
+
+	/* Rx FIFO parameters */
+	tmp = (cfg->rx_pri_elevation / PORT_BMI_FIFO_UNITS - 1) <<
+		BMI_RX_FIFO_PRI_ELEVATION_SHIFT;
+	tmp |= cfg->rx_fifo_thr / PORT_BMI_FIFO_UNITS - 1;
+	iowrite32be(tmp, &regs->fmbm_rfp);
+
+	if (cfg->excessive_threshold_register)
+		/* always allow access to the extra resources */
+		iowrite32be(BMI_RX_FIFO_THRESHOLD_ETHE, &regs->fmbm_reth);
+
+	/* Frame end data */
+	tmp = (cfg->cheksum_last_bytes_ignore & BMI_FRAME_END_CS_IGNORE_MASK) <<
+		BMI_FRAME_END_CS_IGNORE_SHIFT;
+	tmp |= (cfg->rx_cut_end_bytes & BMI_RX_FRAME_END_CUT_MASK) <<
+		BMI_RX_FRAME_END_CUT_SHIFT;
+	if (cfg->errata_A006320)
+		tmp &= 0xffe0ffff;
+	iowrite32be(tmp, &regs->fmbm_rfed);
+
+	/* Internal context parameters */
+	tmp = ((cfg->int_context.ext_buf_offset / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_TO_EXT_MASK) << BMI_IC_TO_EXT_SHIFT;
+	tmp |= ((cfg->int_context.int_context_offset / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_FROM_INT_MASK) << BMI_IC_FROM_INT_SHIFT;
+	tmp |= (cfg->int_context.size / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_SIZE_MASK;
+	iowrite32be(tmp, &regs->fmbm_ricp);
+
+	/* Internal buffer offset */
+	tmp = ((cfg->int_buf_start_margin / PORT_IC_OFFSET_UNITS) &
+		BMI_INT_BUF_MARG_MASK) << BMI_INT_BUF_MARG_SHIFT;
+	iowrite32be(tmp, &regs->fmbm_rim);
+
+	/* External buffer margins */
+	tmp = (cfg->buf_margins.start_margins & BMI_EXT_BUF_MARG_START_MASK) <<
+		BMI_EXT_BUF_MARG_START_SHIFT;
+	tmp |= cfg->buf_margins.end_margins & BMI_EXT_BUF_MARG_END_MASK;
+	iowrite32be(tmp, &regs->fmbm_rebm);
+
+	/* Frame attributes */
+	tmp = BMI_CMD_RX_MR_DEF;
+	tmp |= BMI_CMD_ATTR_ORDER;
+	tmp |= (u32)cfg->color << BMI_CMD_ATTR_COLOR_SHIFT;
+	/* Synchronization request */
+	tmp |= BMI_CMD_ATTR_SYNC;
+
+	iowrite32be(tmp, &regs->fmbm_rfca);
+
+	/* NIA */
+	tmp = (u32)cfg->rx_fd_bits << BMI_NEXT_ENG_FD_BITS_SHIFT;
+
+	tmp |= NIA_ENG_BMI | NIA_BMI_AC_ENQ_FRAME;
+	iowrite32be(tmp, &regs->fmbm_rfne);
+
+	/* Enqueue NIA */
+	iowrite32be(NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR, &regs->fmbm_rfene);
+
+	/* Default/error queues */
+	iowrite32be((cfg->dflt_fqid & DFLT_FQ_ID), &regs->fmbm_rfqid);
+	iowrite32be((cfg->err_fqid & DFLT_FQ_ID), &regs->fmbm_refqid);
+
+	/* Discard/error masks */
+	iowrite32be(cfg->discard_mask, &regs->fmbm_rfsdm);
+	iowrite32be(cfg->err_mask, &regs->fmbm_rfsem);
+
+	return 0;
+}
+
+static int init_bmi_tx(struct fman_port *port)
+{
+	struct fman_port_tx_bmi_regs __iomem *regs = &port->bmi_regs->tx;
+	struct fman_port_cfg *cfg = port->cfg;
+	u32 tmp;
+
+	/* Tx Configuration register */
+	tmp = 0;
+	iowrite32be(tmp, &regs->fmbm_tcfg);
+
+	/* DMA attributes */
+	tmp = (u32)cfg->dma_swap_data << BMI_DMA_ATTR_SWP_SHIFT;
+	iowrite32be(tmp, &regs->fmbm_tda);
+
+	/* Tx FIFO parameters */
+	tmp = (cfg->tx_fifo_min_level / PORT_BMI_FIFO_UNITS) <<
+		BMI_TX_FIFO_MIN_FILL_SHIFT;
+	tmp |= ((cfg->tx_fifo_deq_pipeline_depth - 1) &
+		BMI_FIFO_PIPELINE_DEPTH_MASK) << BMI_FIFO_PIPELINE_DEPTH_SHIFT;
+	tmp |= (cfg->tx_fifo_low_comf_level / PORT_BMI_FIFO_UNITS) - 1;
+	iowrite32be(tmp, &regs->fmbm_tfp);
+
+	/* Frame end data */
+	tmp = (cfg->cheksum_last_bytes_ignore & BMI_FRAME_END_CS_IGNORE_MASK) <<
+		BMI_FRAME_END_CS_IGNORE_SHIFT;
+	iowrite32be(tmp, &regs->fmbm_tfed);
+
+	/* Internal context parameters */
+	tmp = ((cfg->int_context.ext_buf_offset / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_TO_EXT_MASK) << BMI_IC_TO_EXT_SHIFT;
+	tmp |= ((cfg->int_context.int_context_offset / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_FROM_INT_MASK) << BMI_IC_FROM_INT_SHIFT;
+	tmp |= (cfg->int_context.size / PORT_IC_OFFSET_UNITS) &
+		BMI_IC_SIZE_MASK;
+	iowrite32be(tmp, &regs->fmbm_ticp);
+
+	/* Frame attributes */
+	tmp = BMI_CMD_TX_MR_DEF;
+	tmp |= BMI_CMD_ATTR_ORDER;
+	tmp |= (u32)cfg->color << BMI_CMD_ATTR_COLOR_SHIFT;
+	iowrite32be(tmp, &regs->fmbm_tfca);
+
+	/* Dequeue NIA + enqueue NIA */
+	iowrite32be(NIA_ENG_QMI_DEQ, &regs->fmbm_tfdne);
+	iowrite32be(NIA_ENG_QMI_ENQ | NIA_ORDER_RESTOR, &regs->fmbm_tfene);
+	if (cfg->fmbm_tfne_has_features)
+		iowrite32be(!cfg->dflt_fqid ?
+			    BMI_EBD_EN | NIA_BMI_AC_FETCH_ALL_FRAME :
+			    NIA_BMI_AC_FETCH_ALL_FRAME, &regs->fmbm_tfne);
+	if (!cfg->dflt_fqid && cfg->dont_release_buf) {
+		iowrite32be(DFLT_FQ_ID, &regs->fmbm_tcfqid);
+		iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE,
+			    &regs->fmbm_tfene);
+		if (cfg->fmbm_tfne_has_features)
+			iowrite32be(ioread32be(&regs->fmbm_tfne) & ~BMI_EBD_EN,
+				    &regs->fmbm_tfne);
+	}
+
+	/* Confirmation/error queues */
+	if (cfg->dflt_fqid || !cfg->dont_release_buf)
+		iowrite32be(cfg->dflt_fqid & DFLT_FQ_ID, &regs->fmbm_tcfqid);
+	iowrite32be((cfg->err_fqid & DFLT_FQ_ID), &regs->fmbm_tefqid);
+
+	return 0;
+}
+
+static int init_qmi(struct fman_port *port)
+{
+	struct fman_port_qmi_regs __iomem *regs = port->qmi_regs;
+	struct fman_port_cfg *cfg = port->cfg;
+	u32 tmp;
+
+	/* Rx port configuration */
+	if (port->port_type == FMAN_PORT_TYPE_RX) {
+		/* Enqueue NIA */
+		iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_RELEASE, &regs->fmqm_pnen);
+		return 0;
+	}
+
+	/* Continue with Tx port configuration */
+	if (port->port_type == FMAN_PORT_TYPE_TX) {
+		/* Enqueue NIA */
+		iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE,
+			    &regs->fmqm_pnen);
+		/* Dequeue NIA */
+		iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX, &regs->fmqm_pndn);
+	}
+
+	/* Dequeue Configuration register */
+	tmp = 0;
+	if (cfg->deq_high_priority)
+		tmp |= QMI_DEQ_CFG_PRI;
+
+	switch (cfg->deq_type) {
+	case FMAN_PORT_DEQ_BY_PRI:
+		tmp |= QMI_DEQ_CFG_TYPE1;
+		break;
+	case FMAN_PORT_DEQ_ACTIVE_FQ:
+		tmp |= QMI_DEQ_CFG_TYPE2;
+		break;
+	case FMAN_PORT_DEQ_ACTIVE_FQ_NO_ICS:
+		tmp |= QMI_DEQ_CFG_TYPE3;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (cfg->deq_prefetch_option) {
+	case FMAN_PORT_DEQ_NO_PREFETCH:
+		break;
+	case FMAN_PORT_DEQ_PART_PREFETCH:
+		tmp |= QMI_DEQ_CFG_PREFETCH_PARTIAL;
+		break;
+	case FMAN_PORT_DEQ_FULL_PREFETCH:
+		tmp |= QMI_DEQ_CFG_PREFETCH_FULL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	tmp |= (cfg->deq_sp & QMI_DEQ_CFG_SP_MASK) << QMI_DEQ_CFG_SP_SHIFT;
+	tmp |= cfg->deq_byte_cnt;
+	iowrite32be(tmp, &regs->fmqm_pndc);
+
+	return 0;
+}
+
+static int init(struct fman_port *port)
+{
+	int err;
+
+	/* Init BMI registers */
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		err = init_bmi_rx(port);
+		break;
+	case FMAN_PORT_TYPE_TX:
+		err = init_bmi_tx(port);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (err)
+		return err;
+
+	/* Init QMI registers */
+	err = init_qmi(port);
+	return err;
+
+	return 0;
+}
+
+static int set_bpools(const struct fman_port *port,
+		      const struct fman_port_bpools *bp)
+{
+	u32 __iomem *bp_reg, *bp_depl_reg;
+	u32 tmp;
+	u8 i, max_bp_num;
+	bool grp_depl_used = false, rx_port;
+
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		max_bp_num = port->ext_pools_num;
+		rx_port = true;
+		bp_reg = port->bmi_regs->rx.fmbm_ebmpi;
+		bp_depl_reg = &port->bmi_regs->rx.fmbm_mpd;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (rx_port) {
+		/* Check buffers are provided in ascending order */
+		for (i = 0; (i < (bp->count - 1) &&
+			     (i < FMAN_PORT_MAX_EXT_POOLS_NUM - 1)); i++) {
+			if (bp->bpool[i].size > bp->bpool[i + 1].size)
+				return -EINVAL;
+		}
+	}
+
+	/* Set up external buffers pools */
+	for (i = 0; i < bp->count; i++) {
+		tmp = BMI_EXT_BUF_POOL_VALID;
+		tmp |= ((u32)bp->bpool[i].bpid <<
+			BMI_EXT_BUF_POOL_ID_SHIFT) & BMI_EXT_BUF_POOL_ID_MASK;
+
+		if (rx_port) {
+			if (bp->counters_enable)
+				tmp |= BMI_EXT_BUF_POOL_EN_COUNTER;
+
+			if (bp->bpool[i].is_backup)
+				tmp |= BMI_EXT_BUF_POOL_BACKUP;
+
+			tmp |= (u32)bp->bpool[i].size;
+		}
+
+		iowrite32be(tmp, &bp_reg[i]);
+	}
+
+	/* Clear unused pools */
+	for (i = bp->count; i < max_bp_num; i++)
+		iowrite32be(0, &bp_reg[i]);
+
+	/* Pools depletion */
+	tmp = 0;
+	for (i = 0; i < FMAN_PORT_MAX_EXT_POOLS_NUM; i++) {
+		if (bp->bpool[i].grp_bp_depleted) {
+			grp_depl_used = true;
+			tmp |= 0x80000000 >> i;
+		}
+
+		if (bp->bpool[i].single_bp_depleted)
+			tmp |= 0x80 >> i;
+	}
+
+	if (grp_depl_used)
+		tmp |= ((u32)bp->grp_bp_depleted_num - 1) <<
+		    BMI_POOL_DEP_NUM_OF_POOLS_SHIFT;
+
+	iowrite32be(tmp, bp_depl_reg);
+	return 0;
+}
+
+static bool is_init_done(struct fman_port_cfg *cfg)
+{
+	/* Checks if FMan port driver parameters were initialized */
+	if (!cfg)
+		return true;
+
+	return false;
+}
+
+static int verify_size_of_fifo(struct fman_port *port)
+{
+	u32 min_fifo_size_required = 0, opt_fifo_size_for_b2b = 0;
+
+	/* TX Ports */
+	if (port->port_type == FMAN_PORT_TYPE_TX) {
+		min_fifo_size_required = (u32)
+		    (roundup(port->max_frame_length,
+			     FMAN_BMI_FIFO_UNITS) + (3 * FMAN_BMI_FIFO_UNITS));
+
+		min_fifo_size_required +=
+		    port->cfg->tx_fifo_deq_pipeline_depth *
+		    FMAN_BMI_FIFO_UNITS;
+
+		opt_fifo_size_for_b2b = min_fifo_size_required;
+
+		/* Add some margin for back-to-back capability to improve
+		 * performance, allows the hardware to pipeline new frame dma
+		 * while the previous frame not yet transmitted.
+		 */
+		if (port->port_speed == 10000)
+			opt_fifo_size_for_b2b += 3 * FMAN_BMI_FIFO_UNITS;
+		else
+			opt_fifo_size_for_b2b += 2 * FMAN_BMI_FIFO_UNITS;
+	}
+
+	/* RX Ports */
+	else if (port->port_type == FMAN_PORT_TYPE_RX) {
+		if (port->rev_info.major >= 6)
+			min_fifo_size_required = (u32)
+			(roundup(port->max_frame_length,
+				 FMAN_BMI_FIFO_UNITS) +
+				 (5 * FMAN_BMI_FIFO_UNITS));
+			/* 4 according to spec + 1 for FOF>0 */
+		else
+			min_fifo_size_required = (u32)
+			(roundup(min(port->max_frame_length,
+				     port->rx_pools_params.largest_buf_size),
+				     FMAN_BMI_FIFO_UNITS) +
+				     (7 * FMAN_BMI_FIFO_UNITS));
+
+		opt_fifo_size_for_b2b = min_fifo_size_required;
+
+		/* Add some margin for back-to-back capability to improve
+		 * performance,allows the hardware to pipeline new frame dma
+		 * while the previous frame not yet transmitted.
+		 */
+		if (port->port_speed == 10000)
+			opt_fifo_size_for_b2b += 8 * FMAN_BMI_FIFO_UNITS;
+		else
+			opt_fifo_size_for_b2b += 3 * FMAN_BMI_FIFO_UNITS;
+	}
+
+	WARN_ON(min_fifo_size_required <= 0);
+	WARN_ON(opt_fifo_size_for_b2b < min_fifo_size_required);
+
+	/* Verify the size  */
+	if (port->fifo_bufs.num < min_fifo_size_required)
+		dev_dbg(port->dev, "%s: FIFO size should be enlarged to %d bytes\n",
+			__func__, min_fifo_size_required);
+	else if (port->fifo_bufs.num < opt_fifo_size_for_b2b)
+		dev_dbg(port->dev, "%s: For b2b processing,FIFO may be enlarged to %d bytes\n",
+			__func__, opt_fifo_size_for_b2b);
+
+	return 0;
+}
+
+static int set_ext_buffer_pools(struct fman_port *port)
+{
+	struct fman_ext_pools *ext_buf_pools = &port->cfg->ext_buf_pools;
+	struct fman_buf_pool_depletion *buf_pool_depletion =
+	&port->cfg->buf_pool_depletion;
+	u8 ordered_array[FMAN_PORT_MAX_EXT_POOLS_NUM];
+	u16 sizes_array[BM_MAX_NUM_OF_POOLS];
+	int i = 0, j = 0, err;
+	struct fman_port_bpools bpools;
+
+	memset(&ordered_array, 0, sizeof(u8) * FMAN_PORT_MAX_EXT_POOLS_NUM);
+	memset(&sizes_array, 0, sizeof(u16) * BM_MAX_NUM_OF_POOLS);
+	memcpy(&port->ext_buf_pools, ext_buf_pools,
+	       sizeof(struct fman_ext_pools));
+
+	fman_sp_set_buf_pools_in_asc_order_of_buf_sizes(ext_buf_pools,
+							ordered_array,
+							sizes_array);
+
+	memset(&bpools, 0, sizeof(struct fman_port_bpools));
+	bpools.count = ext_buf_pools->num_of_pools_used;
+	bpools.counters_enable = true;
+	for (i = 0; i < ext_buf_pools->num_of_pools_used; i++) {
+		bpools.bpool[i].bpid = ordered_array[i];
+		bpools.bpool[i].size = sizes_array[ordered_array[i]];
+	}
+
+	/* save pools parameters for later use */
+	port->rx_pools_params.num_of_pools = ext_buf_pools->num_of_pools_used;
+	port->rx_pools_params.largest_buf_size =
+	    sizes_array[ordered_array[ext_buf_pools->num_of_pools_used - 1]];
+	port->rx_pools_params.second_largest_buf_size =
+	    sizes_array[ordered_array[ext_buf_pools->num_of_pools_used - 2]];
+
+	/* FMBM_RMPD reg. - pool depletion */
+	if (buf_pool_depletion->pools_grp_mode_enable) {
+		bpools.grp_bp_depleted_num = buf_pool_depletion->num_of_pools;
+		for (i = 0; i < port->bm_max_num_of_pools; i++) {
+			if (buf_pool_depletion->pools_to_consider[i]) {
+				for (j = 0; j < ext_buf_pools->
+				     num_of_pools_used; j++) {
+					if (i == ordered_array[j]) {
+						bpools.bpool[j].
+						    grp_bp_depleted = true;
+						break;
+					}
+				}
+			}
+		}
+	}
+
+	if (buf_pool_depletion->single_pool_mode_enable) {
+		for (i = 0; i < port->bm_max_num_of_pools; i++) {
+			if (buf_pool_depletion->
+			    pools_to_consider_for_single_mode[i]) {
+				for (j = 0; j < ext_buf_pools->
+				     num_of_pools_used; j++) {
+					if (i == ordered_array[j]) {
+						bpools.bpool[j].
+						    single_bp_depleted = true;
+						break;
+					}
+				}
+			}
+		}
+	}
+
+	err = set_bpools(port, &bpools);
+	if (err != 0) {
+		dev_err(port->dev, "%s: set_bpools() failed\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int init_low_level_driver(struct fman_port *port)
+{
+	struct fman_port_cfg *cfg = port->cfg;
+	u32 tmp_val;
+
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		cfg->err_mask = (RX_ERRS_TO_ENQ & ~cfg->discard_mask);
+		break;
+	default:
+		break;
+	}
+
+	tmp_val = (u32)((port->internal_buf_offset % OFFSET_UNITS) ?
+		(port->internal_buf_offset / OFFSET_UNITS + 1) :
+		(port->internal_buf_offset / OFFSET_UNITS));
+	port->internal_buf_offset = (u8)(tmp_val * OFFSET_UNITS);
+	port->cfg->int_buf_start_margin = port->internal_buf_offset;
+
+	if (init(port) != 0) {
+		dev_err(port->dev, "%s: fman port initialization failed\n",
+			__func__);
+		return -ENODEV;
+	}
+
+	/* The code bellow is a trick so the FM will not release the buffer
+	 * to BM nor will try to enqueue the frame to QM
+	 */
+	if (port->port_type == FMAN_PORT_TYPE_TX) {
+		if (!cfg->dflt_fqid && cfg->dont_release_buf) {
+			/* override fmbm_tcfqid 0 with a false non-0 value.
+			 * This will force FM to act according to tfene.
+			 * Otherwise, if fmbm_tcfqid is 0 the FM will release
+			 * buffers to BM regardless of fmbm_tfene
+			 */
+			iowrite32be(0xFFFFFF, &port->bmi_regs->tx.fmbm_tcfqid);
+			iowrite32be(NIA_ENG_BMI | NIA_BMI_AC_TX_RELEASE,
+				    &port->bmi_regs->tx.fmbm_tfene);
+		}
+	}
+
+	return 0;
+}
+
+static int fill_soc_specific_params(struct fman_port *port)
+{
+	u32 bmi_max_fifo_size;
+
+	bmi_max_fifo_size = fman_get_bmi_max_fifo_size(port->fm);
+	port->max_port_fifo_size = MAX_PORT_FIFO_SIZE(bmi_max_fifo_size);
+	port->bm_max_num_of_pools = 64;
+
+	/* P4080 - Major 2
+	 * P2041/P3041/P5020/P5040 - Major 3
+	 * Tx/Bx - Major 6
+	 */
+	switch (port->rev_info.major) {
+	case 2:
+	case 3:
+		port->max_num_of_ext_pools		= 4;
+		port->max_num_of_sub_portals		= 12;
+		break;
+
+	case 6:
+		port->max_num_of_ext_pools		= 8;
+		port->max_num_of_sub_portals		= 16;
+		break;
+
+	default:
+		dev_err(port->dev, "%s: Unsupported FMan version\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int get_dflt_fifo_deq_pipeline_depth(u8 major, enum fman_port_type type,
+					    u16 speed)
+{
+	switch (type) {
+	case FMAN_PORT_TYPE_RX:
+	case FMAN_PORT_TYPE_TX:
+		switch (speed) {
+		case 10000:
+			return 4;
+		case 1000:
+			if (major >= 6)
+				return 2;
+			else
+				return 1;
+		default:
+			return 0;
+		}
+	default:
+		return 0;
+	}
+}
+
+static int get_dflt_num_of_tasks(u8 major, enum fman_port_type type,
+				 u16 speed)
+{
+	switch (type) {
+	case FMAN_PORT_TYPE_RX:
+	case FMAN_PORT_TYPE_TX:
+		switch (speed) {
+		case 10000:
+			return 16;
+		case 1000:
+			if (major >= 6)
+				return 4;
+			else
+				return 3;
+		default:
+			return 0;
+		}
+	default:
+		return 0;
+	}
+}
+
+static int get_dflt_extra_num_of_tasks(u8 major, enum fman_port_type type,
+				       u16 speed)
+{
+	switch (type) {
+	case FMAN_PORT_TYPE_RX:
+		/* FMan V3 */
+		if (major >= 6)
+			return 0;
+
+		/* FMan V2 */
+		if (speed == 10000)
+			return 8;
+		else
+			return 2;
+	case FMAN_PORT_TYPE_TX:
+	default:
+		return 0;
+	}
+}
+
+static int get_dflt_num_of_open_dmas(u8 major, enum fman_port_type type,
+				     u16 speed)
+{
+	int val;
+
+	if (major >= 6) {
+		switch (type) {
+		case FMAN_PORT_TYPE_TX:
+			if (speed == 10000)
+				val = 12;
+			else
+				val = 3;
+			break;
+		case FMAN_PORT_TYPE_RX:
+			if (speed == 10000)
+				val = 8;
+			else
+				val = 2;
+			break;
+		default:
+			return 0;
+		}
+	} else {
+		switch (type) {
+		case FMAN_PORT_TYPE_TX:
+		case FMAN_PORT_TYPE_RX:
+			if (speed == 10000)
+				val = 8;
+			else
+				val = 1;
+			break;
+		default:
+			val = 0;
+		}
+	}
+
+	return val;
+}
+
+static int get_dflt_extra_num_of_open_dmas(u8 major, enum fman_port_type type,
+					   u16 speed)
+{
+	/* FMan V3 */
+	if (major >= 6)
+		return 0;
+
+	/* FMan V2 */
+	switch (type) {
+	case FMAN_PORT_TYPE_RX:
+	case FMAN_PORT_TYPE_TX:
+		if (speed == 10000)
+			return 8;
+		else
+			return 1;
+	default:
+		return 0;
+	}
+}
+
+static int get_dflt_num_of_fifo_bufs(u8 major, enum fman_port_type type,
+				     u16 speed)
+{
+	int val;
+
+	if (major >= 6) {
+		switch (type) {
+		case FMAN_PORT_TYPE_TX:
+			if (speed == 10000)
+				val = 64;
+			else
+				val = 50;
+			break;
+		case FMAN_PORT_TYPE_RX:
+			if (speed == 10000)
+				val = 96;
+			else
+				val = 50;
+			break;
+		default:
+			val = 0;
+		}
+	} else {
+		switch (type) {
+		case FMAN_PORT_TYPE_TX:
+			if (speed == 10000)
+				val = 48;
+			else
+				val = 44;
+			break;
+		case FMAN_PORT_TYPE_RX:
+			if (speed == 10000)
+				val = 48;
+			else
+				val = 45;
+			break;
+		default:
+			val = 0;
+		}
+	}
+
+	return val;
+}
+
+static void set_dflt_cfg(struct fman_port *port,
+			 struct fman_port_params *port_params)
+{
+	struct fman_port_cfg *cfg = port->cfg;
+
+	cfg->dma_swap_data = FMAN_PORT_DMA_NO_SWAP;
+	cfg->color = FMAN_PORT_COLOR_GREEN;
+	cfg->rx_cut_end_bytes = DFLT_PORT_CUT_BYTES_FROM_END;
+	cfg->rx_pri_elevation = BMI_PRIORITY_ELEVATION_LEVEL;
+	cfg->rx_fifo_thr = BMI_FIFO_THRESHOLD;
+	cfg->tx_fifo_low_comf_level = (5 * 1024);
+	cfg->deq_type = FMAN_PORT_DEQ_BY_PRI;
+	cfg->deq_prefetch_option = FMAN_PORT_DEQ_FULL_PREFETCH;
+	cfg->tx_fifo_deq_pipeline_depth =
+		BMI_DEQUEUE_PIPELINE_DEPTH(port->port_type, port->port_speed);
+	cfg->deq_byte_cnt = QMI_BYTE_COUNT_LEVEL_CONTROL(port->port_type);
+
+	cfg->rx_pri_elevation =
+		DFLT_PORT_RX_FIFO_PRI_ELEVATION_LEV(port->max_port_fifo_size);
+	port->cfg->rx_fifo_thr =
+		DFLT_PORT_RX_FIFO_THRESHOLD(port->rev_info.major,
+					    port->max_port_fifo_size);
+
+	if ((port->rev_info.major == 6) &&
+	    ((port->rev_info.minor == 0) || (port->rev_info.minor == 3)))
+		cfg->errata_A006320 = true;
+
+	/* Excessive Threshold register - exists for pre-FMv3 chips only */
+	if (port->rev_info.major < 6)
+		cfg->excessive_threshold_register = true;
+	else
+		cfg->fmbm_tfne_has_features = true;
+
+	cfg->buffer_prefix_content.data_align =
+		DFLT_PORT_BUFFER_PREFIX_CONTEXT_DATA_ALIGN;
+}
+
+static void set_rx_dflt_cfg(struct fman_port *port,
+			    struct fman_port_params *port_params)
+{
+	port->cfg->discard_mask = DFLT_PORT_ERRORS_TO_DISCARD;
+
+	memcpy(&port->cfg->ext_buf_pools,
+	       &port_params->specific_params.rx_params.ext_buf_pools,
+	       sizeof(struct fman_ext_pools));
+	port->cfg->err_fqid =
+		port_params->specific_params.rx_params.err_fqid;
+	port->cfg->dflt_fqid =
+		port_params->specific_params.rx_params.dflt_fqid;
+}
+
+static void set_tx_dflt_cfg(struct fman_port *port,
+			    struct fman_port_params *port_params,
+			    struct fman_port_dts_params *dts_params)
+{
+	port->cfg->tx_fifo_deq_pipeline_depth =
+		get_dflt_fifo_deq_pipeline_depth(port->rev_info.major,
+						 port->port_type,
+						 port->port_speed);
+	port->cfg->err_fqid =
+		port_params->specific_params.non_rx_params.err_fqid;
+	port->cfg->deq_sp =
+		(u8)(dts_params->qman_channel_id & QMI_DEQ_CFG_SUBPORTAL_MASK);
+	port->cfg->dflt_fqid =
+		port_params->specific_params.non_rx_params.dflt_fqid;
+	port->cfg->deq_high_priority = true;
+}
+
+/**
+ * fman_port_config
+ * @port:	Pointer to the port structure
+ * @params:	Pointer to data structure of parameters
+ *
+ * Creates a descriptor for the FM PORT module.
+ * The routine returns a pointer to the FM PORT object.
+ * This descriptor must be passed as first parameter to all other FM PORT
+ * function calls.
+ * No actual initialization or configuration of FM hardware is done by this
+ * routine.
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_port_config(struct fman_port *port, struct fman_port_params *params)
+{
+	void __iomem *base_addr = port->dts_params.base_addr;
+	int err;
+
+	/* Allocate the FM driver's parameters structure */
+	port->cfg = kzalloc(sizeof(*port->cfg), GFP_KERNEL);
+	if (!port->cfg)
+		goto err_params;
+
+	/* Initialize FM port parameters which will be kept by the driver */
+	port->port_type = port->dts_params.type;
+	port->port_speed = port->dts_params.speed;
+	port->port_id = port->dts_params.id;
+	port->fm = port->dts_params.fman;
+	port->ext_pools_num = (u8)8;
+
+	/* get FM revision */
+	fman_get_revision(port->fm, &port->rev_info);
+
+	err = fill_soc_specific_params(port);
+	if (err)
+		goto err_port_cfg;
+
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		set_rx_dflt_cfg(port, params);
+	case FMAN_PORT_TYPE_TX:
+		set_tx_dflt_cfg(port, params, &port->dts_params);
+	default:
+		set_dflt_cfg(port, params);
+	}
+
+	/* Continue with other parameters */
+	/* set memory map pointers */
+	port->bmi_regs = base_addr + BMI_PORT_REGS_OFFSET;
+	port->qmi_regs = base_addr + QMI_PORT_REGS_OFFSET;
+
+	port->max_frame_length = DFLT_PORT_MAX_FRAME_LENGTH;
+	/* resource distribution. */
+
+	port->fifo_bufs.num =
+	get_dflt_num_of_fifo_bufs(port->rev_info.major, port->port_type,
+				  port->port_speed) * FMAN_BMI_FIFO_UNITS;
+	port->fifo_bufs.extra =
+	DFLT_PORT_EXTRA_NUM_OF_FIFO_BUFS * FMAN_BMI_FIFO_UNITS;
+
+	port->open_dmas.num =
+	get_dflt_num_of_open_dmas(port->rev_info.major,
+				  port->port_type, port->port_speed);
+	port->open_dmas.extra =
+	get_dflt_extra_num_of_open_dmas(port->rev_info.major,
+					port->port_type, port->port_speed);
+	port->tasks.num =
+	get_dflt_num_of_tasks(port->rev_info.major,
+			      port->port_type, port->port_speed);
+	port->tasks.extra =
+	get_dflt_extra_num_of_tasks(port->rev_info.major,
+				    port->port_type, port->port_speed);
+
+	/* FM_HEAVY_TRAFFIC_SEQUENCER_HANG_ERRATA_FMAN_A006981 errata
+	 * workaround
+	 */
+	if ((port->rev_info.major == 6) && (port->rev_info.minor == 0) &&
+	    (((port->port_type == FMAN_PORT_TYPE_TX) &&
+	    (port->port_speed == 1000)))) {
+		port->open_dmas.num = 16;
+		port->open_dmas.extra = 0;
+	}
+
+	if (port->rev_info.major >= 6 &&
+	    port->port_type == FMAN_PORT_TYPE_TX &&
+	    port->port_speed == 1000) {
+		/* FM_WRONG_RESET_VALUES_ERRATA_FMAN_A005127 Errata
+		 * workaround
+		 */
+		if (port->rev_info.major >= 6) {
+			u32 reg;
+
+			reg = 0x00001013;
+			iowrite32be(reg, &port->bmi_regs->tx.fmbm_tfp);
+		}
+	}
+
+	return 0;
+
+err_port_cfg:
+	kfree(port->cfg);
+err_params:
+	kfree(port);
+	return -EINVAL;
+}
+EXPORT_SYMBOL(fman_port_config);
+
+/**
+ * fman_port_init
+ * port:	A pointer to a FM Port module.
+ * Initializes the FM PORT module by defining the software structure and
+ * configuring the hardware registers.
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_port_init(struct fman_port *port)
+{
+	struct fman_port_cfg *cfg;
+	int err;
+	struct fman_port_init_params params;
+
+	if (is_init_done(port->cfg))
+		return -EINVAL;
+
+	err = fman_sp_build_buffer_struct(&port->cfg->int_context,
+					  &port->cfg->buffer_prefix_content,
+					  &port->cfg->buf_margins,
+					  &port->buffer_offsets,
+					  &port->internal_buf_offset);
+	if (err)
+		return err;
+
+	cfg = port->cfg;
+
+	if (port->port_type == FMAN_PORT_TYPE_RX) {
+		/* Call the external Buffer routine which also checks fifo
+		 * size and updates it if necessary
+		 */
+		/* define external buffer pools and pool depletion */
+		err = set_ext_buffer_pools(port);
+		if (err)
+			return err;
+		/* check if the largest external buffer pool is large enough */
+		if (cfg->buf_margins.start_margins + MIN_EXT_BUF_SIZE +
+		    cfg->buf_margins.end_margins >
+		    port->rx_pools_params.largest_buf_size) {
+			dev_err(port->dev, "%s: buf_margins.start_margins (%d) + minimum buf size (64) + buf_margins.end_margins (%d) is larger than maximum external buffer size (%d)\n",
+				__func__, cfg->buf_margins.start_margins,
+				cfg->buf_margins.end_margins,
+				port->rx_pools_params.largest_buf_size);
+			return -EINVAL;
+		}
+	}
+
+	/* Call FM module routine for communicating parameters */
+	memset(&params, 0, sizeof(params));
+	params.port_id = port->port_id;
+	params.port_type = port->port_type;
+	params.port_speed = port->port_speed;
+	params.num_of_tasks = (u8)port->tasks.num;
+	params.num_of_extra_tasks = (u8)port->tasks.extra;
+	params.num_of_open_dmas = (u8)port->open_dmas.num;
+	params.num_of_extra_open_dmas = (u8)port->open_dmas.extra;
+
+	if (port->fifo_bufs.num) {
+		err = verify_size_of_fifo(port);
+		if (err)
+			return err;
+	}
+	params.size_of_fifo = port->fifo_bufs.num;
+	params.extra_size_of_fifo = port->fifo_bufs.extra;
+	params.deq_pipeline_depth = port->cfg->tx_fifo_deq_pipeline_depth;
+	params.max_frame_length = port->max_frame_length;
+
+	err = fman_set_port_params(port->fm, &params);
+	if (err)
+		return err;
+
+	err = init_low_level_driver(port);
+	if (err)
+		return err;
+
+	kfree(port->cfg);
+	port->cfg = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL(fman_port_init);
+
+/**
+ * fman_port_cfg_buf_prefix_content
+ * @port			A pointer to a FM Port module.
+ * @buffer_prefix_content	A structure of parameters describing
+ *				the structure of the buffer.
+ *				Out parameter:
+ *				Start margin - offset of data from
+ *				start of external buffer.
+ * Defines the structure, size and content of the application buffer.
+ * The prefix, in Tx ports, if 'pass_prs_result', the application should set
+ * a value to their offsets in the prefix of the FM will save the first
+ * 'priv_data_size', than, depending on 'pass_prs_result' and
+ * 'pass_time_stamp', copy parse result and timeStamp, and the packet itself
+ * (in this order), to the application buffer, and to offset.
+ * Calling this routine changes the buffer margins definitions in the internal
+ * driver data base from its default configuration:
+ * Data size:  [DEFAULT_PORT_BUFFER_PREFIX_CONTENT_PRIV_DATA_SIZE]
+ * Pass Parser result: [DEFAULT_PORT_BUFFER_PREFIX_CONTENT_PASS_PRS_RESULT].
+ * Pass timestamp: [DEFAULT_PORT_BUFFER_PREFIX_CONTENT_PASS_TIME_STAMP].
+ * May be used for all ports
+ *
+ * Allowed only following fman_port_config() and before fman_port_init().
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_port_cfg_buf_prefix_content(struct fman_port *port,
+				     struct fman_buffer_prefix_content *
+				     buffer_prefix_content)
+{
+	if (is_init_done(port->cfg))
+		return -EINVAL;
+
+	memcpy(&port->cfg->buffer_prefix_content,
+	       buffer_prefix_content,
+	       sizeof(struct fman_buffer_prefix_content));
+	/* if data_align was not initialized by user,
+	 * we return to driver's default
+	 */
+	if (!port->cfg->buffer_prefix_content.data_align)
+		port->cfg->buffer_prefix_content.data_align =
+		DFLT_PORT_BUFFER_PREFIX_CONTEXT_DATA_ALIGN;
+
+	return 0;
+}
+EXPORT_SYMBOL(fman_port_cfg_buf_prefix_content);
+
+/**
+ * fman_port_disable
+ * port:	A pointer to a FM Port module.
+ *
+ * Gracefully disable an FM port. The port will not start new	tasks after all
+ * tasks associated with the port are terminated.
+ *
+ * This is a blocking routine, it returns after port is gracefully stopped,
+ * i.e. the port will not except new frames, but it will finish all frames
+ * or tasks which were already began.
+ * Allowed only following fman_port_init().
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_port_disable(struct fman_port *port)
+{
+	u32 __iomem *bmi_cfg_reg, *bmi_status_reg, tmp;
+	bool rx_port, failure = false;
+	int count;
+
+	if (!is_init_done(port->cfg))
+		return -EINVAL;
+
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		bmi_cfg_reg = &port->bmi_regs->rx.fmbm_rcfg;
+		bmi_status_reg = &port->bmi_regs->rx.fmbm_rst;
+		rx_port = true;
+		break;
+	case FMAN_PORT_TYPE_TX:
+		bmi_cfg_reg = &port->bmi_regs->tx.fmbm_tcfg;
+		bmi_status_reg = &port->bmi_regs->tx.fmbm_tst;
+		rx_port = false;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Disable QMI */
+	if (!rx_port) {
+		tmp = ioread32be(&port->qmi_regs->fmqm_pnc) & ~QMI_PORT_CFG_EN;
+		iowrite32be(tmp, &port->qmi_regs->fmqm_pnc);
+
+		/* Wait for QMI to finish FD handling */
+		count = 100;
+		do {
+			udelay(10);
+			tmp = ioread32be(&port->qmi_regs->fmqm_pns);
+		} while ((tmp & QMI_PORT_STATUS_DEQ_FD_BSY) && --count);
+
+		if (count == 0) {
+			/* Timeout */
+			failure = true;
+		}
+	}
+
+	/* Disable BMI */
+	tmp = ioread32be(bmi_cfg_reg) & ~BMI_PORT_CFG_EN;
+	iowrite32be(tmp, bmi_cfg_reg);
+
+	/* Wait for graceful stop end */
+	count = 500;
+	do {
+		udelay(10);
+		tmp = ioread32be(bmi_status_reg);
+	} while ((tmp & BMI_PORT_STATUS_BSY) && --count);
+
+	if (count == 0) {
+		/* Timeout */
+		failure = true;
+	}
+
+	if (failure)
+		dev_dbg(port->dev, "%s: FMan Port[%d]: BMI or QMI is Busy. Port forced down\n",
+			__func__,  port->port_id);
+
+	return 0;
+}
+EXPORT_SYMBOL(fman_port_disable);
+
+/**
+ * fman_port_enable
+ * port:	A pointer to a FM Port module.
+ *
+ * A runtime routine provided to allow disable/enable of port.
+ *
+ * Allowed only following fman_port_init().
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_port_enable(struct fman_port *port)
+{
+	u32 __iomem *bmi_cfg_reg, tmp;
+	bool rx_port;
+
+	if (!is_init_done(port->cfg))
+		return -EINVAL;
+
+	switch (port->port_type) {
+	case FMAN_PORT_TYPE_RX:
+		bmi_cfg_reg = &port->bmi_regs->rx.fmbm_rcfg;
+		rx_port = true;
+		break;
+	case FMAN_PORT_TYPE_TX:
+		bmi_cfg_reg = &port->bmi_regs->tx.fmbm_tcfg;
+		rx_port = false;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* Enable QMI */
+	if (!rx_port) {
+		tmp = ioread32be(&port->qmi_regs->fmqm_pnc) | QMI_PORT_CFG_EN;
+		iowrite32be(tmp, &port->qmi_regs->fmqm_pnc);
+	}
+
+	/* Enable BMI */
+	tmp = ioread32be(bmi_cfg_reg) | BMI_PORT_CFG_EN;
+	iowrite32be(tmp, bmi_cfg_reg);
+
+	return 0;
+}
+EXPORT_SYMBOL(fman_port_enable);
+
+/**
+ * fman_port_bind
+ * dev:		FMan Port OF device pointer
+ *
+ * Bind to a specific FMan Port.
+ *
+ * Allowed only after the port was created.
+ *
+ * Return: A pointer to the FMan port device.
+ */
+struct fman_port *fman_port_bind(struct device *dev)
+{
+	return (struct fman_port *)(dev_get_drvdata(get_device(dev)));
+}
+EXPORT_SYMBOL(fman_port_bind);
+
+/**
+ * fman_port_get_qman_channel_id
+ * port:	Pointer to the FMan port devuce
+ *
+ * Get the QMan channel ID for the specific port
+ *
+ * Return: QMan channel ID
+ */
+u32 fman_port_get_qman_channel_id(struct fman_port *port)
+{
+	return port->dts_params.qman_channel_id;
+}
+EXPORT_SYMBOL(fman_port_get_qman_channel_id);
+
+static int fman_port_probe(struct platform_device *of_dev)
+{
+	struct fman_port *port;
+	struct fman *fman;
+	struct device_node *fm_node, *port_node;
+	struct resource res;
+	struct resource *dev_res;
+	const u32 *u32_prop;
+	int err = 0, lenp;
+	enum fman_port_type port_type;
+	u16 port_speed;
+	u8 port_id;
+
+	port = kzalloc(sizeof(*port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+
+	port->dev = &of_dev->dev;
+
+	port_node = of_node_get(of_dev->dev.of_node);
+
+	/* Get the FM node */
+	fm_node = of_get_parent(port_node);
+	if (!fm_node) {
+		dev_err(port->dev, "%s: of_get_parent() failed\n", __func__);
+		err = -ENODEV;
+		goto return_err;
+	}
+
+	fman = dev_get_drvdata(&of_find_device_by_node(fm_node)->dev);
+	of_node_put(fm_node);
+	if (!fman) {
+		err = -EINVAL;
+		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",
+			__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->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)
+			port_speed = 10000;
+
+	} else if (of_device_is_compatible(port_node, "fsl,fman-v2-port-tx")) {
+		if (port_id >= TX_10G_PORT_BASE)
+			port_speed = 10000;
+		else
+			port_speed = 1000;
+		port_type = FMAN_PORT_TYPE_TX;
+
+	} 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)
+			port_speed = 10000;
+
+	} else if (of_device_is_compatible(port_node, "fsl,fman-v2-port-rx")) {
+		if (port_id >= RX_10G_PORT_BASE)
+			port_speed = 10000;
+		else
+			port_speed = 1000;
+		port_type = FMAN_PORT_TYPE_RX;
+
+	}  else {
+		dev_err(port->dev, "%s: Illegal port type\n", __func__);
+		err = -EINVAL;
+		goto return_err;
+	}
+
+	port->dts_params.type = port_type;
+	port->dts_params.speed = port_speed;
+
+	if (port_type == FMAN_PORT_TYPE_TX) {
+		u32 qman_channel_id;
+
+		qman_channel_id = fman_get_qman_channel_id(fman, port_id);
+		if (qman_channel_id == 0) {
+			dev_err(port->dev, "%s: incorrect qman-channel-id\n",
+				__func__);
+			err = -EINVAL;
+			goto return_err;
+		}
+		port->dts_params.qman_channel_id = qman_channel_id;
+	}
+
+	err = of_address_to_resource(port_node, 0, &res);
+	if (err < 0) {
+		dev_err(port->dev, "%s: of_address_to_resource() failed\n",
+			__func__);
+		err = -ENOMEM;
+		goto return_err;
+	}
+
+	port->dts_params.fman = fman;
+
+	of_node_put(port_node);
+
+	dev_res = __devm_request_region(port->dev, &res, res.start,
+					resource_size(&res), "fman-port");
+	if (!dev_res) {
+		dev_err(port->dev, "%s: __devm_request_region() failed\n",
+			__func__);
+		err = -EINVAL;
+		goto free_port;
+	}
+
+	port->dts_params.base_addr = devm_ioremap(port->dev, res.start,
+						  resource_size(&res));
+	if (port->dts_params.base_addr == 0)
+		dev_err(port->dev, "%s: devm_ioremap() failed\n", __func__);
+
+	dev_set_drvdata(&of_dev->dev, port);
+
+	return 0;
+
+return_err:
+	of_node_put(port_node);
+free_port:
+	kfree(port);
+	return err;
+}
+
+static const struct of_device_id fman_port_match[] = {
+	{.compatible = "fsl,fman-v3-port-rx"},
+	{.compatible = "fsl,fman-v2-port-rx"},
+	{.compatible = "fsl,fman-v3-port-tx"},
+	{.compatible = "fsl,fman-v2-port-tx"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(of, fman_port_match);
+
+static struct platform_driver fman_port_driver = {
+	.driver = {
+		.name = "fsl-fman-port",
+		.of_match_table = fman_port_match,
+	},
+	.probe = fman_port_probe,
+};
+
+builtin_platform_driver(fman_port_driver);
diff --git a/drivers/net/ethernet/freescale/fman/fman_port.h b/drivers/net/ethernet/freescale/fman/fman_port.h
new file mode 100644
index 000000000000..8ba901737048
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_port.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright 2008 - 2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __FMAN_PORT_H
+#define __FMAN_PORT_H
+
+#include "fman.h"
+
+/* FM Port API
+ * The FM uses a general module called "port" to represent a Tx port (MAC),
+ * an Rx port (MAC).
+ * The number of ports in an FM varies between SOCs.
+ * The SW driver manages these ports as sub-modules of the FM,i.e. after an
+ * FM is initialized, its ports may be initialized and operated upon.
+ * The port is initialized aware of its type, but other functions on a port
+ * may be indifferent to its type. When necessary, the driver verifies
+ * coherence and returns error if applicable.
+ * On initialization, user specifies the port type and it's index (relative
+ * to the port's type) - always starting at 0.
+ */
+
+/* FM Frame error */
+/* Frame Descriptor errors */
+/* Not for Rx-Port! Unsupported Format */
+#define FM_PORT_FRM_ERR_UNSUPPORTED_FORMAT	FM_FD_ERR_UNSUPPORTED_FORMAT
+/* Not for Rx-Port! Length Error */
+#define FM_PORT_FRM_ERR_LENGTH			FM_FD_ERR_LENGTH
+/* DMA Data error */
+#define FM_PORT_FRM_ERR_DMA			FM_FD_ERR_DMA
+/* non Frame-Manager error; probably come from SEC that was chained to FM */
+#define FM_PORT_FRM_ERR_NON_FM			FM_FD_RX_STATUS_ERR_NON_FM
+ /* IPR error */
+#define FM_PORT_FRM_ERR_IPRE			(FM_FD_ERR_IPR & ~FM_FD_IPR)
+/* IPR non-consistent-sp */
+#define FM_PORT_FRM_ERR_IPR_NCSP		(FM_FD_ERR_IPR_NCSP &	\
+						~FM_FD_IPR)
+
+/* Rx FIFO overflow, FCS error, code error, running disparity
+ * error (SGMII and TBI modes), FIFO parity error.
+ * PHY Sequence error, PHY error control character detected.
+ */
+#define FM_PORT_FRM_ERR_PHYSICAL                FM_FD_ERR_PHYSICAL
+/* Frame too long OR Frame size exceeds max_length_frame  */
+#define FM_PORT_FRM_ERR_SIZE                    FM_FD_ERR_SIZE
+/* indicates a classifier "drop" operation */
+#define FM_PORT_FRM_ERR_CLS_DISCARD             FM_FD_ERR_CLS_DISCARD
+/* Extract Out of Frame */
+#define FM_PORT_FRM_ERR_EXTRACTION              FM_FD_ERR_EXTRACTION
+/* No Scheme Selected */
+#define FM_PORT_FRM_ERR_NO_SCHEME               FM_FD_ERR_NO_SCHEME
+/* Keysize Overflow */
+#define FM_PORT_FRM_ERR_KEYSIZE_OVERFLOW        FM_FD_ERR_KEYSIZE_OVERFLOW
+/* Frame color is red */
+#define FM_PORT_FRM_ERR_COLOR_RED               FM_FD_ERR_COLOR_RED
+/* Frame color is yellow */
+#define FM_PORT_FRM_ERR_COLOR_YELLOW            FM_FD_ERR_COLOR_YELLOW
+/* Parser Time out Exceed */
+#define FM_PORT_FRM_ERR_PRS_TIMEOUT             FM_FD_ERR_PRS_TIMEOUT
+/* Invalid Soft Parser instruction */
+#define FM_PORT_FRM_ERR_PRS_ILL_INSTRUCT        FM_FD_ERR_PRS_ILL_INSTRUCT
+/* Header error was identified during parsing */
+#define FM_PORT_FRM_ERR_PRS_HDR_ERR             FM_FD_ERR_PRS_HDR_ERR
+/* Frame parsed beyind 256 first bytes */
+#define FM_PORT_FRM_ERR_BLOCK_LIMIT_EXCEEDED    FM_FD_ERR_BLOCK_LIMIT_EXCEEDED
+/* FPM Frame Processing Timeout Exceeded */
+#define FM_PORT_FRM_ERR_PROCESS_TIMEOUT         0x00000001
+
+struct fman_port;
+
+/* A structure for additional Rx port parameters */
+struct fman_port_rx_params {
+	u32 err_fqid;			/* Error Queue Id. */
+	u32 dflt_fqid;			/* Default Queue Id. */
+	/* Which external buffer pools are used
+	 * (up to FMAN_PORT_MAX_EXT_POOLS_NUM), and their sizes.
+	 */
+	struct fman_ext_pools ext_buf_pools;
+};
+
+/* A structure for additional non-Rx port parameters */
+struct fman_port_non_rx_params {
+	/* Error Queue Id. */
+	u32 err_fqid;
+	/* For Tx - Default Confirmation queue, 0 means no Tx confirmation
+	 * for processed frames. For OP port - default Rx queue.
+	 */
+	u32 dflt_fqid;
+};
+
+/* A union for additional parameters depending on port type */
+union fman_port_specific_params {
+	/* Rx port parameters structure */
+	struct fman_port_rx_params rx_params;
+	/* Non-Rx port parameters structure */
+	struct fman_port_non_rx_params non_rx_params;
+};
+
+/* A structure representing FM initialization parameters */
+struct fman_port_params {
+	/* Virtual Address of memory mapped FM Port registers. */
+	void *fm;
+	union fman_port_specific_params specific_params;
+	/* Additional parameters depending on port type. */
+};
+
+int fman_port_config(struct fman_port *port, struct fman_port_params *params);
+
+int fman_port_init(struct fman_port *port);
+
+int fman_port_cfg_buf_prefix_content(struct fman_port *port,
+				     struct fman_buffer_prefix_content
+				     *buffer_prefix_content);
+
+int fman_port_disable(struct fman_port *port);
+
+int fman_port_enable(struct fman_port *port);
+
+u32 fman_port_get_qman_channel_id(struct fman_port *port);
+
+struct fman_port *fman_port_bind(struct device *dev);
+
+#endif /* __FMAN_PORT_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_sp.c b/drivers/net/ethernet/freescale/fman/fman_sp.c
new file mode 100644
index 000000000000..f9e7aa385cba
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_sp.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright 2008 - 2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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 "fman_sp.h"
+#include "fman.h"
+
+void fman_sp_set_buf_pools_in_asc_order_of_buf_sizes(struct fman_ext_pools
+						     *fm_ext_pools,
+						     u8 *ordered_array,
+						     u16 *sizes_array)
+{
+	u16 buf_size = 0;
+	int i = 0, j = 0, k = 0;
+
+	/* First we copy the external buffers pools information
+	 * to an ordered local array
+	 */
+	for (i = 0; i < fm_ext_pools->num_of_pools_used; i++) {
+		/* get pool size */
+		buf_size = fm_ext_pools->ext_buf_pool[i].size;
+
+		/* keep sizes in an array according to poolId
+		 * for direct access
+		 */
+		sizes_array[fm_ext_pools->ext_buf_pool[i].id] = buf_size;
+
+		/* save poolId in an ordered array according to size */
+		for (j = 0; j <= i; j++) {
+			/* this is the next free place in the array */
+			if (j == i)
+				ordered_array[i] =
+				    fm_ext_pools->ext_buf_pool[i].id;
+			else {
+				/* find the right place for this poolId */
+				if (buf_size < sizes_array[ordered_array[j]]) {
+					/* move the pool_ids one place ahead
+					 * to make room for this poolId
+					 */
+					for (k = i; k > j; k--)
+						ordered_array[k] =
+						    ordered_array[k - 1];
+
+					/* now k==j, this is the place for
+					 * the new size
+					 */
+					ordered_array[k] =
+					    fm_ext_pools->ext_buf_pool[i].id;
+					break;
+				}
+			}
+		}
+	}
+}
+
+int fman_sp_build_buffer_struct(struct fman_sp_int_context_data_copy *
+				int_context_data_copy,
+				struct fman_buffer_prefix_content *
+				buffer_prefix_content,
+				struct fman_sp_buf_margins *buf_margins,
+				struct fman_sp_buffer_offsets *buffer_offsets,
+				u8 *internal_buf_offset)
+{
+	u32 tmp;
+
+	/* Align start of internal context data to 16 byte */
+	int_context_data_copy->ext_buf_offset = (u16)
+		((buffer_prefix_content->priv_data_size & (OFFSET_UNITS - 1)) ?
+		((buffer_prefix_content->priv_data_size + OFFSET_UNITS) &
+			~(u16)(OFFSET_UNITS - 1)) :
+		buffer_prefix_content->priv_data_size);
+
+	/* Translate margin and int_context params to FM parameters */
+	/* Initialize with illegal value. Later we'll set legal values. */
+	buffer_offsets->prs_result_offset = (u32)ILLEGAL_BASE;
+	buffer_offsets->time_stamp_offset = (u32)ILLEGAL_BASE;
+	buffer_offsets->hash_result_offset = (u32)ILLEGAL_BASE;
+
+	/* Internally the driver supports 4 options
+	 * 1. prsResult/timestamp/hashResult selection (in fact 8 options,
+	 * but for simplicity we'll
+	 * relate to it as 1).
+	 * 2. All IC context (from AD) not including debug.
+	 */
+
+	/* This case covers the options under 1 */
+	/* Copy size must be in 16-byte granularity. */
+	int_context_data_copy->size =
+	    (u16)((buffer_prefix_content->pass_prs_result ? 32 : 0) +
+		  ((buffer_prefix_content->pass_time_stamp ||
+		  buffer_prefix_content->pass_hash_result) ? 16 : 0));
+
+	/* Align start of internal context data to 16 byte */
+	int_context_data_copy->int_context_offset =
+	    (u8)(buffer_prefix_content->pass_prs_result ? 32 :
+		 ((buffer_prefix_content->pass_time_stamp ||
+		 buffer_prefix_content->pass_hash_result) ? 64 : 0));
+
+	if (buffer_prefix_content->pass_prs_result)
+		buffer_offsets->prs_result_offset =
+		    int_context_data_copy->ext_buf_offset;
+	if (buffer_prefix_content->pass_time_stamp)
+		buffer_offsets->time_stamp_offset =
+		    buffer_prefix_content->pass_prs_result ?
+		    (int_context_data_copy->ext_buf_offset +
+			sizeof(struct fman_prs_result)) :
+		    int_context_data_copy->ext_buf_offset;
+	if (buffer_prefix_content->pass_hash_result)
+		/* If PR is not requested, whether TS is
+		 * requested or not, IC will be copied from TS
+			 */
+		buffer_offsets->hash_result_offset =
+		buffer_prefix_content->pass_prs_result ?
+			(int_context_data_copy->ext_buf_offset +
+				sizeof(struct fman_prs_result) + 8) :
+			int_context_data_copy->ext_buf_offset + 8;
+
+	if (int_context_data_copy->size)
+		buf_margins->start_margins =
+		    (u16)(int_context_data_copy->ext_buf_offset +
+			  int_context_data_copy->size);
+	else
+		/* No Internal Context passing, STartMargin is
+		 * immediately after private_info
+		 */
+		buf_margins->start_margins =
+		    buffer_prefix_content->priv_data_size;
+
+	/* align data start */
+	tmp = (u32)(buf_margins->start_margins %
+		    buffer_prefix_content->data_align);
+	if (tmp)
+		buf_margins->start_margins +=
+		    (buffer_prefix_content->data_align - tmp);
+	buffer_offsets->data_offset = buf_margins->start_margins;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/freescale/fman/fman_sp.h b/drivers/net/ethernet/freescale/fman/fman_sp.h
new file mode 100644
index 000000000000..820b7f63088f
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_sp.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2008 - 2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *	 notice, this list of conditions and the following disclaimer in the
+ *	 documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __FM_SP_H
+#define __FM_SP_H
+
+#include "fman.h"
+#include <linux/types.h>
+
+#define ILLEGAL_BASE    (~0)
+
+/* defaults */
+#define DFLT_FM_SP_BUFFER_PREFIX_CONTEXT_DATA_ALIGN	64
+
+/* Registers bit fields */
+#define FMAN_SP_EXT_BUF_POOL_EN_COUNTER		0x40000000
+#define FMAN_SP_EXT_BUF_POOL_VALID			0x80000000
+#define FMAN_SP_EXT_BUF_POOL_BACKUP			0x20000000
+#define FMAN_SP_DMA_ATTR_WRITE_OPTIMIZE		0x00100000
+#define FMAN_SP_SG_DISABLE				0x80000000
+
+/* shifts */
+#define FMAN_SP_EXT_BUF_MARG_START_SHIFT		16
+#define FMAN_SP_DMA_ATTR_SWP_SHIFT			30
+#define FMAN_SP_IC_TO_EXT_SHIFT			16
+#define FMAN_SP_IC_FROM_INT_SHIFT			8
+
+/* structure for defining internal context copying */
+struct fman_sp_int_context_data_copy {
+	/* < Offset in External buffer to which internal
+	 *  context is copied to (Rx) or taken from (Tx, Op).
+	 */
+	u16 ext_buf_offset;
+	/* Offset within internal context to copy from
+	 * (Rx) or to copy to (Tx, Op).
+	 */
+	u8 int_context_offset;
+	/* Internal offset size to be copied */
+	u16 size;
+};
+
+/*  struct for defining external buffer margins */
+struct fman_sp_buf_margins {
+	/* Number of bytes to be left at the beginning
+	 * of the external buffer (must be divisible by 16)
+	 */
+	u16 start_margins;
+	/* number of bytes to be left at the end
+	 * of the external buffer(must be divisible by 16)
+	 */
+	u16 end_margins;
+};
+
+struct fman_sp_buffer_offsets {
+	u32 data_offset;
+	u32 prs_result_offset;
+	u32 time_stamp_offset;
+	u32 hash_result_offset;
+};
+
+int fman_sp_build_buffer_struct(struct fman_sp_int_context_data_copy
+				*int_context_data_copy,
+				struct fman_buffer_prefix_content
+				*buffer_prefix_content,
+				struct fman_sp_buf_margins *buf_margins,
+				struct fman_sp_buffer_offsets
+				*buffer_offsets,
+				u8 *internal_buf_offset);
+
+void fman_sp_set_buf_pools_in_asc_order_of_buf_sizes(struct fman_ext_pools
+						     *fm_ext_pools,
+						     u8 *ordered_array,
+						     u16 *sizes_array);
+
+#endif	/* __FM_SP_H */
diff --git a/drivers/net/ethernet/freescale/fman/fman_tgec.c b/drivers/net/ethernet/freescale/fman/fman_tgec.c
new file mode 100644
index 000000000000..efabb04a1ae8
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_tgec.c
@@ -0,0 +1,786 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include "fman_tgec.h"
+#include "fman.h"
+
+#include <linux/slab.h>
+#include <linux/bitrev.h>
+#include <linux/io.h>
+#include <linux/crc32.h>
+
+/* Transmit Inter-Packet Gap Length Register (TX_IPG_LENGTH) */
+#define TGEC_TX_IPG_LENGTH_MASK	0x000003ff
+
+/* Command and Configuration Register (COMMAND_CONFIG) */
+#define CMD_CFG_NO_LEN_CHK		0x00020000
+#define CMD_CFG_PAUSE_IGNORE		0x00000100
+#define CMF_CFG_CRC_FWD			0x00000040
+#define CMD_CFG_PROMIS_EN		0x00000010
+#define CMD_CFG_RX_EN			0x00000002
+#define CMD_CFG_TX_EN			0x00000001
+
+/* Interrupt Mask Register (IMASK) */
+#define TGEC_IMASK_MDIO_SCAN_EVENT	0x00010000
+#define TGEC_IMASK_MDIO_CMD_CMPL	0x00008000
+#define TGEC_IMASK_REM_FAULT		0x00004000
+#define TGEC_IMASK_LOC_FAULT		0x00002000
+#define TGEC_IMASK_TX_ECC_ER		0x00001000
+#define TGEC_IMASK_TX_FIFO_UNFL	0x00000800
+#define TGEC_IMASK_TX_FIFO_OVFL	0x00000400
+#define TGEC_IMASK_TX_ER		0x00000200
+#define TGEC_IMASK_RX_FIFO_OVFL	0x00000100
+#define TGEC_IMASK_RX_ECC_ER		0x00000080
+#define TGEC_IMASK_RX_JAB_FRM		0x00000040
+#define TGEC_IMASK_RX_OVRSZ_FRM	0x00000020
+#define TGEC_IMASK_RX_RUNT_FRM		0x00000010
+#define TGEC_IMASK_RX_FRAG_FRM		0x00000008
+#define TGEC_IMASK_RX_LEN_ER		0x00000004
+#define TGEC_IMASK_RX_CRC_ER		0x00000002
+#define TGEC_IMASK_RX_ALIGN_ER		0x00000001
+
+/* Hashtable Control Register (HASHTABLE_CTRL) */
+#define TGEC_HASH_MCAST_SHIFT		23
+#define TGEC_HASH_MCAST_EN		0x00000200
+#define TGEC_HASH_ADR_MSK		0x000001ff
+
+#define DEFAULT_TX_IPG_LENGTH			12
+#define DEFAULT_MAX_FRAME_LENGTH		0x600
+#define DEFAULT_PAUSE_QUANT			0xf000
+
+/* number of pattern match registers (entries) */
+#define TGEC_NUM_OF_PADDRS          1
+
+/* Group address bit indication */
+#define GROUP_ADDRESS               0x0000010000000000LL
+
+/* Hash table size (= 32 bits*8 regs) */
+#define TGEC_HASH_TABLE_SIZE             512
+
+/* tGEC memory map */
+struct tgec_regs {
+	u32 tgec_id;		/* 0x000 Controller ID */
+	u32 reserved001[1];	/* 0x004 */
+	u32 command_config;	/* 0x008 Control and configuration */
+	u32 mac_addr_0;		/* 0x00c Lower 32 bits of the MAC adr */
+	u32 mac_addr_1;		/* 0x010 Upper 16 bits of the MAC adr */
+	u32 maxfrm;		/* 0x014 Maximum frame length */
+	u32 pause_quant;	/* 0x018 Pause quanta */
+	u32 rx_fifo_sections;	/* 0x01c  */
+	u32 tx_fifo_sections;	/* 0x020  */
+	u32 rx_fifo_almost_f_e;	/* 0x024  */
+	u32 tx_fifo_almost_f_e;	/* 0x028  */
+	u32 hashtable_ctrl;	/* 0x02c Hash table control */
+	u32 mdio_cfg_status;	/* 0x030  */
+	u32 mdio_command;	/* 0x034  */
+	u32 mdio_data;		/* 0x038  */
+	u32 mdio_regaddr;	/* 0x03c  */
+	u32 status;		/* 0x040  */
+	u32 tx_ipg_len;		/* 0x044 Transmitter inter-packet-gap */
+	u32 mac_addr_2;		/* 0x048 Lower 32 bits of 2nd MAC adr */
+	u32 mac_addr_3;		/* 0x04c Upper 16 bits of 2nd MAC adr */
+	u32 rx_fifo_ptr_rd;	/* 0x050  */
+	u32 rx_fifo_ptr_wr;	/* 0x054  */
+	u32 tx_fifo_ptr_rd;	/* 0x058  */
+	u32 tx_fifo_ptr_wr;	/* 0x05c  */
+	u32 imask;		/* 0x060 Interrupt mask */
+	u32 ievent;		/* 0x064 Interrupt event */
+	u32 udp_port;		/* 0x068 Defines a UDP Port number */
+	u32 type_1588v2;	/* 0x06c Type field for 1588v2 */
+	u32 reserved070[4];	/* 0x070 */
+	/* 10Ge Statistics Counter */
+	u32 tfrm_u;		/* 80 aFramesTransmittedOK */
+	u32 tfrm_l;		/* 84 aFramesTransmittedOK */
+	u32 rfrm_u;		/* 88 aFramesReceivedOK */
+	u32 rfrm_l;		/* 8c aFramesReceivedOK */
+	u32 rfcs_u;		/* 90 aFrameCheckSequenceErrors */
+	u32 rfcs_l;		/* 94 aFrameCheckSequenceErrors */
+	u32 raln_u;		/* 98 aAlignmentErrors */
+	u32 raln_l;		/* 9c aAlignmentErrors */
+	u32 txpf_u;		/* A0 aPAUSEMACCtrlFramesTransmitted */
+	u32 txpf_l;		/* A4 aPAUSEMACCtrlFramesTransmitted */
+	u32 rxpf_u;		/* A8 aPAUSEMACCtrlFramesReceived */
+	u32 rxpf_l;		/* Ac aPAUSEMACCtrlFramesReceived */
+	u32 rlong_u;		/* B0 aFrameTooLongErrors */
+	u32 rlong_l;		/* B4 aFrameTooLongErrors */
+	u32 rflr_u;		/* B8 aInRangeLengthErrors */
+	u32 rflr_l;		/* Bc aInRangeLengthErrors */
+	u32 tvlan_u;		/* C0 VLANTransmittedOK */
+	u32 tvlan_l;		/* C4 VLANTransmittedOK */
+	u32 rvlan_u;		/* C8 VLANReceivedOK */
+	u32 rvlan_l;		/* Cc VLANReceivedOK */
+	u32 toct_u;		/* D0 if_out_octets */
+	u32 toct_l;		/* D4 if_out_octets */
+	u32 roct_u;		/* D8 if_in_octets */
+	u32 roct_l;		/* Dc if_in_octets */
+	u32 ruca_u;		/* E0 if_in_ucast_pkts */
+	u32 ruca_l;		/* E4 if_in_ucast_pkts */
+	u32 rmca_u;		/* E8 ifInMulticastPkts */
+	u32 rmca_l;		/* Ec ifInMulticastPkts */
+	u32 rbca_u;		/* F0 ifInBroadcastPkts */
+	u32 rbca_l;		/* F4 ifInBroadcastPkts */
+	u32 terr_u;		/* F8 if_out_errors */
+	u32 terr_l;		/* Fc if_out_errors */
+	u32 reserved100[2];	/* 100-108 */
+	u32 tuca_u;		/* 108 if_out_ucast_pkts */
+	u32 tuca_l;		/* 10c if_out_ucast_pkts */
+	u32 tmca_u;		/* 110 ifOutMulticastPkts */
+	u32 tmca_l;		/* 114 ifOutMulticastPkts */
+	u32 tbca_u;		/* 118 ifOutBroadcastPkts */
+	u32 tbca_l;		/* 11c ifOutBroadcastPkts */
+	u32 rdrp_u;		/* 120 etherStatsDropEvents */
+	u32 rdrp_l;		/* 124 etherStatsDropEvents */
+	u32 reoct_u;		/* 128 etherStatsOctets */
+	u32 reoct_l;		/* 12c etherStatsOctets */
+	u32 rpkt_u;		/* 130 etherStatsPkts */
+	u32 rpkt_l;		/* 134 etherStatsPkts */
+	u32 trund_u;		/* 138 etherStatsUndersizePkts */
+	u32 trund_l;		/* 13c etherStatsUndersizePkts */
+	u32 r64_u;		/* 140 etherStatsPkts64Octets */
+	u32 r64_l;		/* 144 etherStatsPkts64Octets */
+	u32 r127_u;		/* 148 etherStatsPkts65to127Octets */
+	u32 r127_l;		/* 14c etherStatsPkts65to127Octets */
+	u32 r255_u;		/* 150 etherStatsPkts128to255Octets */
+	u32 r255_l;		/* 154 etherStatsPkts128to255Octets */
+	u32 r511_u;		/* 158 etherStatsPkts256to511Octets */
+	u32 r511_l;		/* 15c etherStatsPkts256to511Octets */
+	u32 r1023_u;		/* 160 etherStatsPkts512to1023Octets */
+	u32 r1023_l;		/* 164 etherStatsPkts512to1023Octets */
+	u32 r1518_u;		/* 168 etherStatsPkts1024to1518Octets */
+	u32 r1518_l;		/* 16c etherStatsPkts1024to1518Octets */
+	u32 r1519x_u;		/* 170 etherStatsPkts1519toX */
+	u32 r1519x_l;		/* 174 etherStatsPkts1519toX */
+	u32 trovr_u;		/* 178 etherStatsOversizePkts */
+	u32 trovr_l;		/* 17c etherStatsOversizePkts */
+	u32 trjbr_u;		/* 180 etherStatsJabbers */
+	u32 trjbr_l;		/* 184 etherStatsJabbers */
+	u32 trfrg_u;		/* 188 etherStatsFragments */
+	u32 trfrg_l;		/* 18C etherStatsFragments */
+	u32 rerr_u;		/* 190 if_in_errors */
+	u32 rerr_l;		/* 194 if_in_errors */
+};
+
+struct tgec_cfg {
+	bool pause_ignore;
+	bool promiscuous_mode_enable;
+	u16 max_frame_length;
+	u16 pause_quant;
+	u32 tx_ipg_length;
+};
+
+struct fman_mac {
+	/* Pointer to the memory mapped registers. */
+	struct tgec_regs __iomem *regs;
+	/* MAC address of device; */
+	u64 addr;
+	u16 max_speed;
+	void *dev_id; /* device cookie used by the exception cbs */
+	fman_mac_exception_cb *exception_cb;
+	fman_mac_exception_cb *event_cb;
+	/* pointer to driver's global address hash table  */
+	struct eth_hash_t *multicast_addr_hash;
+	/* pointer to driver's individual address hash table  */
+	struct eth_hash_t *unicast_addr_hash;
+	u8 mac_id;
+	u32 exceptions;
+	struct tgec_cfg *cfg;
+	void *fm;
+	struct fman_rev_info fm_rev_info;
+};
+
+static void set_mac_address(struct tgec_regs __iomem *regs, u8 *adr)
+{
+	u32 tmp0, tmp1;
+
+	tmp0 = (u32)(adr[0] | adr[1] << 8 | adr[2] << 16 | adr[3] << 24);
+	tmp1 = (u32)(adr[4] | adr[5] << 8);
+	iowrite32be(tmp0, &regs->mac_addr_0);
+	iowrite32be(tmp1, &regs->mac_addr_1);
+}
+
+static void set_dflts(struct tgec_cfg *cfg)
+{
+	cfg->promiscuous_mode_enable = false;
+	cfg->pause_ignore = false;
+	cfg->tx_ipg_length = DEFAULT_TX_IPG_LENGTH;
+	cfg->max_frame_length = DEFAULT_MAX_FRAME_LENGTH;
+	cfg->pause_quant = DEFAULT_PAUSE_QUANT;
+}
+
+static int init(struct tgec_regs __iomem *regs, struct tgec_cfg *cfg,
+		u32 exception_mask)
+{
+	u32 tmp;
+
+	/* Config */
+	tmp = CMF_CFG_CRC_FWD;
+	if (cfg->promiscuous_mode_enable)
+		tmp |= CMD_CFG_PROMIS_EN;
+	if (cfg->pause_ignore)
+		tmp |= CMD_CFG_PAUSE_IGNORE;
+	/* Payload length check disable */
+	tmp |= CMD_CFG_NO_LEN_CHK;
+	iowrite32be(tmp, &regs->command_config);
+
+	/* Max Frame Length */
+	iowrite32be((u32)cfg->max_frame_length, &regs->maxfrm);
+	/* Pause Time */
+	iowrite32be(cfg->pause_quant, &regs->pause_quant);
+
+	/* clear all pending events and set-up interrupts */
+	iowrite32be(0xffffffff, &regs->ievent);
+	iowrite32be(ioread32be(&regs->imask) | exception_mask, &regs->imask);
+
+	return 0;
+}
+
+static int check_init_parameters(struct fman_mac *tgec)
+{
+	if (tgec->max_speed < SPEED_10000) {
+		pr_err("10G MAC driver only support 10G speed\n");
+		return -EINVAL;
+	}
+	if (tgec->addr == 0) {
+		pr_err("Ethernet 10G MAC Must have valid MAC Address\n");
+		return -EINVAL;
+	}
+	if (!tgec->exception_cb) {
+		pr_err("uninitialized exception_cb\n");
+		return -EINVAL;
+	}
+	if (!tgec->event_cb) {
+		pr_err("uninitialized event_cb\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int get_exception_flag(enum fman_mac_exceptions exception)
+{
+	u32 bit_mask;
+
+	switch (exception) {
+	case FM_MAC_EX_10G_MDIO_SCAN_EVENT:
+		bit_mask = TGEC_IMASK_MDIO_SCAN_EVENT;
+		break;
+	case FM_MAC_EX_10G_MDIO_CMD_CMPL:
+		bit_mask = TGEC_IMASK_MDIO_CMD_CMPL;
+		break;
+	case FM_MAC_EX_10G_REM_FAULT:
+		bit_mask = TGEC_IMASK_REM_FAULT;
+		break;
+	case FM_MAC_EX_10G_LOC_FAULT:
+		bit_mask = TGEC_IMASK_LOC_FAULT;
+		break;
+	case FM_MAC_EX_10G_TX_ECC_ER:
+		bit_mask = TGEC_IMASK_TX_ECC_ER;
+		break;
+	case FM_MAC_EX_10G_TX_FIFO_UNFL:
+		bit_mask = TGEC_IMASK_TX_FIFO_UNFL;
+		break;
+	case FM_MAC_EX_10G_TX_FIFO_OVFL:
+		bit_mask = TGEC_IMASK_TX_FIFO_OVFL;
+		break;
+	case FM_MAC_EX_10G_TX_ER:
+		bit_mask = TGEC_IMASK_TX_ER;
+		break;
+	case FM_MAC_EX_10G_RX_FIFO_OVFL:
+		bit_mask = TGEC_IMASK_RX_FIFO_OVFL;
+		break;
+	case FM_MAC_EX_10G_RX_ECC_ER:
+		bit_mask = TGEC_IMASK_RX_ECC_ER;
+		break;
+	case FM_MAC_EX_10G_RX_JAB_FRM:
+		bit_mask = TGEC_IMASK_RX_JAB_FRM;
+		break;
+	case FM_MAC_EX_10G_RX_OVRSZ_FRM:
+		bit_mask = TGEC_IMASK_RX_OVRSZ_FRM;
+		break;
+	case FM_MAC_EX_10G_RX_RUNT_FRM:
+		bit_mask = TGEC_IMASK_RX_RUNT_FRM;
+		break;
+	case FM_MAC_EX_10G_RX_FRAG_FRM:
+		bit_mask = TGEC_IMASK_RX_FRAG_FRM;
+		break;
+	case FM_MAC_EX_10G_RX_LEN_ER:
+		bit_mask = TGEC_IMASK_RX_LEN_ER;
+		break;
+	case FM_MAC_EX_10G_RX_CRC_ER:
+		bit_mask = TGEC_IMASK_RX_CRC_ER;
+		break;
+	case FM_MAC_EX_10G_RX_ALIGN_ER:
+		bit_mask = TGEC_IMASK_RX_ALIGN_ER;
+		break;
+	default:
+		bit_mask = 0;
+		break;
+	}
+
+	return bit_mask;
+}
+
+static void tgec_err_exception(void *handle)
+{
+	struct fman_mac *tgec = (struct fman_mac *)handle;
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 event;
+
+	/* do not handle MDIO events */
+	event = ioread32be(&regs->ievent) &
+			   ~(TGEC_IMASK_MDIO_SCAN_EVENT |
+			   TGEC_IMASK_MDIO_CMD_CMPL);
+
+	event &= ioread32be(&regs->imask);
+
+	iowrite32be(event, &regs->ievent);
+
+	if (event & TGEC_IMASK_REM_FAULT)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_REM_FAULT);
+	if (event & TGEC_IMASK_LOC_FAULT)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_LOC_FAULT);
+	if (event & TGEC_IMASK_TX_ECC_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_TX_ECC_ER);
+	if (event & TGEC_IMASK_TX_FIFO_UNFL)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_TX_FIFO_UNFL);
+	if (event & TGEC_IMASK_TX_FIFO_OVFL)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_TX_FIFO_OVFL);
+	if (event & TGEC_IMASK_TX_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_TX_ER);
+	if (event & TGEC_IMASK_RX_FIFO_OVFL)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_FIFO_OVFL);
+	if (event & TGEC_IMASK_RX_ECC_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_ECC_ER);
+	if (event & TGEC_IMASK_RX_JAB_FRM)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_JAB_FRM);
+	if (event & TGEC_IMASK_RX_OVRSZ_FRM)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_OVRSZ_FRM);
+	if (event & TGEC_IMASK_RX_RUNT_FRM)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_RUNT_FRM);
+	if (event & TGEC_IMASK_RX_FRAG_FRM)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_FRAG_FRM);
+	if (event & TGEC_IMASK_RX_LEN_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_LEN_ER);
+	if (event & TGEC_IMASK_RX_CRC_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_CRC_ER);
+	if (event & TGEC_IMASK_RX_ALIGN_ER)
+		tgec->exception_cb(tgec->dev_id, FM_MAC_EX_10G_RX_ALIGN_ER);
+}
+
+static void free_init_resources(struct fman_mac *tgec)
+{
+	fman_unregister_intr(tgec->fm, FMAN_MOD_MAC, tgec->mac_id,
+			     FMAN_INTR_TYPE_ERR);
+
+	/* release the driver's group hash table */
+	free_hash_table(tgec->multicast_addr_hash);
+	tgec->multicast_addr_hash = NULL;
+
+	/* release the driver's individual hash table */
+	free_hash_table(tgec->unicast_addr_hash);
+	tgec->unicast_addr_hash = NULL;
+}
+
+static bool is_init_done(struct tgec_cfg *cfg)
+{
+	/* Checks if tGEC driver parameters were initialized */
+	if (!cfg)
+		return true;
+
+	return false;
+}
+
+int tgec_enable(struct fman_mac *tgec, enum comm_mode mode)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 tmp;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (mode & COMM_MODE_RX)
+		tmp |= CMD_CFG_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp |= CMD_CFG_TX_EN;
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int tgec_disable(struct fman_mac *tgec, enum comm_mode mode)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 tmp;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (mode & COMM_MODE_RX)
+		tmp &= ~CMD_CFG_RX_EN;
+	if (mode & COMM_MODE_TX)
+		tmp &= ~CMD_CFG_TX_EN;
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int tgec_set_promiscuous(struct fman_mac *tgec, bool new_val)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 tmp;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (new_val)
+		tmp |= CMD_CFG_PROMIS_EN;
+	else
+		tmp &= ~CMD_CFG_PROMIS_EN;
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int tgec_cfg_max_frame_len(struct fman_mac *tgec, u16 new_val)
+{
+	if (is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tgec->cfg->max_frame_length = new_val;
+
+	return 0;
+}
+
+int tgec_set_tx_pause_frames(struct fman_mac *tgec, u8 __maybe_unused priority,
+			     u16 pause_time, u16 __maybe_unused thresh_time)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	iowrite32be((u32)pause_time, &regs->pause_quant);
+
+	return 0;
+}
+
+int tgec_accept_rx_pause_frames(struct fman_mac *tgec, bool en)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 tmp;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tmp = ioread32be(&regs->command_config);
+	if (!en)
+		tmp |= CMD_CFG_PAUSE_IGNORE;
+	else
+		tmp &= ~CMD_CFG_PAUSE_IGNORE;
+	iowrite32be(tmp, &regs->command_config);
+
+	return 0;
+}
+
+int tgec_modify_mac_address(struct fman_mac *tgec, enet_addr_t *p_enet_addr)
+{
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	tgec->addr = ENET_ADDR_TO_UINT64(*p_enet_addr);
+	set_mac_address(tgec->regs, (u8 *)(*p_enet_addr));
+
+	return 0;
+}
+
+int tgec_add_hash_mac_address(struct fman_mac *tgec, enet_addr_t *eth_addr)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	struct eth_hash_entry *hash_entry;
+	u32 crc = 0xFFFFFFFF, hash;
+	u64 addr;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	addr = ENET_ADDR_TO_UINT64(*eth_addr);
+
+	if (!(addr & GROUP_ADDRESS)) {
+		/* Unicast addresses not supported in hash */
+		pr_err("Unicast Address\n");
+		return -EINVAL;
+	}
+	/* CRC calculation */
+	crc = crc32_le(crc, (u8 *)eth_addr, ETH_ALEN);
+	crc = bitrev32(crc);
+	/* Take 9 MSB bits */
+	hash = (crc >> TGEC_HASH_MCAST_SHIFT) & TGEC_HASH_ADR_MSK;
+
+	/* Create element to be added to the driver hash table */
+	hash_entry = kmalloc(sizeof(*hash_entry), GFP_KERNEL);
+	if (!hash_entry)
+		return -ENOMEM;
+	hash_entry->addr = addr;
+	INIT_LIST_HEAD(&hash_entry->node);
+
+	list_add_tail(&hash_entry->node,
+		      &tgec->multicast_addr_hash->lsts[hash]);
+	iowrite32be((hash | TGEC_HASH_MCAST_EN), &regs->hashtable_ctrl);
+
+	return 0;
+}
+
+int tgec_del_hash_mac_address(struct fman_mac *tgec, enet_addr_t *eth_addr)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	struct eth_hash_entry *hash_entry = NULL;
+	struct list_head *pos;
+	u32 crc = 0xFFFFFFFF, hash;
+	u64 addr;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	addr = ((*(u64 *)eth_addr) >> 16);
+
+	/* CRC calculation */
+	crc = crc32_le(crc, (u8 *)eth_addr, ETH_ALEN);
+	crc = bitrev32(crc);
+	/* Take 9 MSB bits */
+	hash = (crc >> TGEC_HASH_MCAST_SHIFT) & TGEC_HASH_ADR_MSK;
+
+	list_for_each(pos, &tgec->multicast_addr_hash->lsts[hash]) {
+		hash_entry = ETH_HASH_ENTRY_OBJ(pos);
+		if (hash_entry->addr == addr) {
+			list_del_init(&hash_entry->node);
+			kfree(hash_entry);
+			break;
+		}
+	}
+	if (list_empty(&tgec->multicast_addr_hash->lsts[hash]))
+		iowrite32be((hash & ~TGEC_HASH_MCAST_EN),
+			    &regs->hashtable_ctrl);
+
+	return 0;
+}
+
+int tgec_get_version(struct fman_mac *tgec, u32 *mac_version)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	*mac_version = ioread32be(&regs->tgec_id);
+
+	return 0;
+}
+
+int tgec_set_exception(struct fman_mac *tgec,
+		       enum fman_mac_exceptions exception, bool enable)
+{
+	struct tgec_regs __iomem *regs = tgec->regs;
+	u32 bit_mask = 0;
+
+	if (!is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	bit_mask = get_exception_flag(exception);
+	if (bit_mask) {
+		if (enable)
+			tgec->exceptions |= bit_mask;
+		else
+			tgec->exceptions &= ~bit_mask;
+	} else {
+		pr_err("Undefined exception\n");
+		return -EINVAL;
+	}
+	if (enable)
+		iowrite32be(ioread32be(&regs->imask) | bit_mask, &regs->imask);
+	else
+		iowrite32be(ioread32be(&regs->imask) & ~bit_mask, &regs->imask);
+
+	return 0;
+}
+
+int tgec_init(struct fman_mac *tgec)
+{
+	struct tgec_cfg *cfg;
+	enet_addr_t eth_addr;
+	int err;
+
+	if (is_init_done(tgec->cfg))
+		return -EINVAL;
+
+	if (DEFAULT_RESET_ON_INIT &&
+	    (fman_reset_mac(tgec->fm, tgec->mac_id) != 0)) {
+		pr_err("Can't reset MAC!\n");
+		return -EINVAL;
+	}
+
+	err = check_init_parameters(tgec);
+	if (err)
+		return err;
+
+	cfg = tgec->cfg;
+
+	MAKE_ENET_ADDR_FROM_UINT64(tgec->addr, eth_addr);
+	set_mac_address(tgec->regs, (u8 *)eth_addr);
+
+	/* interrupts */
+	/* FM_10G_REM_N_LCL_FLT_EX_10GMAC_ERRATA_SW005 Errata workaround */
+	if (tgec->fm_rev_info.major <= 2)
+		tgec->exceptions &= ~(TGEC_IMASK_REM_FAULT |
+				      TGEC_IMASK_LOC_FAULT);
+
+	err = init(tgec->regs, cfg, tgec->exceptions);
+	if (err) {
+		free_init_resources(tgec);
+		pr_err("TGEC version doesn't support this i/f mode\n");
+		return err;
+	}
+
+	/* Max Frame Length */
+	err = fman_set_mac_max_frame(tgec->fm, tgec->mac_id,
+				     cfg->max_frame_length);
+	if (err) {
+		pr_err("Setting max frame length FAILED\n");
+		free_init_resources(tgec);
+		return -EINVAL;
+	}
+
+	/* FM_TX_FIFO_CORRUPTION_ERRATA_10GMAC_A007 Errata workaround */
+	if (tgec->fm_rev_info.major == 2) {
+		struct tgec_regs __iomem *regs = tgec->regs;
+		u32 tmp;
+
+		/* restore the default tx ipg Length */
+		tmp = (ioread32be(&regs->tx_ipg_len) &
+		       ~TGEC_TX_IPG_LENGTH_MASK) | 12;
+
+		iowrite32be(tmp, &regs->tx_ipg_len);
+	}
+
+	tgec->multicast_addr_hash = alloc_hash_table(TGEC_HASH_TABLE_SIZE);
+	if (!tgec->multicast_addr_hash) {
+		free_init_resources(tgec);
+		pr_err("allocation hash table is FAILED\n");
+		return -ENOMEM;
+	}
+
+	tgec->unicast_addr_hash = alloc_hash_table(TGEC_HASH_TABLE_SIZE);
+	if (!tgec->unicast_addr_hash) {
+		free_init_resources(tgec);
+		pr_err("allocation hash table is FAILED\n");
+		return -ENOMEM;
+	}
+
+	fman_register_intr(tgec->fm, FMAN_MOD_MAC, tgec->mac_id,
+			   FMAN_INTR_TYPE_ERR, tgec_err_exception, tgec);
+
+	kfree(cfg);
+	tgec->cfg = NULL;
+
+	return 0;
+}
+
+int tgec_free(struct fman_mac *tgec)
+{
+	free_init_resources(tgec);
+
+	if (tgec->cfg)
+		tgec->cfg = NULL;
+
+	kfree(tgec->cfg);
+	kfree(tgec);
+
+	return 0;
+}
+
+struct fman_mac *tgec_config(struct fman_mac_params *params)
+{
+	struct fman_mac *tgec;
+	struct tgec_cfg *cfg;
+	void __iomem *base_addr;
+
+	base_addr = params->base_addr;
+	/* allocate memory for the UCC GETH data structure. */
+	tgec = kzalloc(sizeof(*tgec), GFP_KERNEL);
+	if (!tgec)
+		return NULL;
+
+	/* allocate memory for the 10G MAC driver parameters data structure. */
+	cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
+	if (!cfg) {
+		tgec_free(tgec);
+		return NULL;
+	}
+
+	/* Plant parameter structure pointer */
+	tgec->cfg = cfg;
+
+	set_dflts(cfg);
+
+	tgec->regs = base_addr;
+	tgec->addr = ENET_ADDR_TO_UINT64(params->addr);
+	tgec->max_speed = params->max_speed;
+	tgec->mac_id = params->mac_id;
+	tgec->exceptions = (TGEC_IMASK_MDIO_SCAN_EVENT	|
+			    TGEC_IMASK_REM_FAULT	|
+			    TGEC_IMASK_LOC_FAULT	|
+			    TGEC_IMASK_TX_ECC_ER	|
+			    TGEC_IMASK_TX_FIFO_UNFL	|
+			    TGEC_IMASK_TX_FIFO_OVFL	|
+			    TGEC_IMASK_TX_ER		|
+			    TGEC_IMASK_RX_FIFO_OVFL	|
+			    TGEC_IMASK_RX_ECC_ER	|
+			    TGEC_IMASK_RX_JAB_FRM	|
+			    TGEC_IMASK_RX_OVRSZ_FRM	|
+			    TGEC_IMASK_RX_RUNT_FRM	|
+			    TGEC_IMASK_RX_FRAG_FRM	|
+			    TGEC_IMASK_RX_CRC_ER	|
+			    TGEC_IMASK_RX_ALIGN_ER);
+	tgec->exception_cb = params->exception_cb;
+	tgec->event_cb = params->event_cb;
+	tgec->dev_id = params->dev_id;
+	tgec->fm = params->fm;
+
+	/* Save FMan revision */
+	fman_get_revision(tgec->fm, &tgec->fm_rev_info);
+
+	return tgec;
+}
diff --git a/drivers/net/ethernet/freescale/fman/fman_tgec.h b/drivers/net/ethernet/freescale/fman/fman_tgec.h
new file mode 100644
index 000000000000..514bba9f47ce
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/fman_tgec.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright 2008-2015 Freescale Semiconductor Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __TGEC_H
+#define __TGEC_H
+
+#include "fman_mac.h"
+
+struct fman_mac *tgec_config(struct fman_mac_params *params);
+int tgec_set_promiscuous(struct fman_mac *tgec, bool new_val);
+int tgec_modify_mac_address(struct fman_mac *tgec, enet_addr_t *enet_addr);
+int tgec_cfg_max_frame_len(struct fman_mac *tgec, u16 new_val);
+int tgec_enable(struct fman_mac *tgec, enum comm_mode mode);
+int tgec_disable(struct fman_mac *tgec, enum comm_mode mode);
+int tgec_init(struct fman_mac *tgec);
+int tgec_free(struct fman_mac *tgec);
+int tgec_accept_rx_pause_frames(struct fman_mac *tgec, bool en);
+int tgec_set_tx_pause_frames(struct fman_mac *tgec, u8 priority,
+			     u16 pause_time, u16 thresh_time);
+int tgec_set_exception(struct fman_mac *tgec,
+		       enum fman_mac_exceptions exception, bool enable);
+int tgec_add_hash_mac_address(struct fman_mac *tgec, enet_addr_t *eth_addr);
+int tgec_del_hash_mac_address(struct fman_mac *tgec, enet_addr_t *eth_addr);
+int tgec_get_version(struct fman_mac *tgec, u32 *mac_version);
+
+#endif /* __TGEC_H */
diff --git a/drivers/net/ethernet/freescale/fman/mac.c b/drivers/net/ethernet/freescale/fman/mac.c
new file mode 100644
index 000000000000..e33d9d24c1db
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/mac.c
@@ -0,0 +1,977 @@
+/* Copyright 2008-2015 Freescale Semiconductor, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *	 notice, this list of conditions and the following disclaimer in the
+ *	 documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor 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.
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/of_net.h>
+#include <linux/of_mdio.h>
+#include <linux/device.h>
+#include <linux/phy.h>
+#include <linux/netdevice.h>
+#include <linux/phy_fixed.h>
+#include <linux/etherdevice.h>
+#include <linux/libfdt_env.h>
+
+#include "mac.h"
+#include "fman_mac.h"
+#include "fman_dtsec.h"
+#include "fman_tgec.h"
+#include "fman_memac.h"
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("FSL FMan MAC API based driver");
+
+struct mac_priv_s {
+	struct device			*dev;
+	void __iomem			*vaddr;
+	u8				cell_index;
+	phy_interface_t			phy_if;
+	struct fman			*fman;
+	struct device_node		*phy_node;
+	struct device_node		*internal_phy_node;
+	/* List of multicast addresses */
+	struct list_head		mc_addr_list;
+	struct platform_device		*eth_dev;
+	struct fixed_phy_status		*fixed_link;
+	u16				speed;
+	u16				max_speed;
+
+	int (*enable)(struct fman_mac *mac_dev, enum comm_mode mode);
+	int (*disable)(struct fman_mac *mac_dev, enum comm_mode mode);
+};
+
+struct mac_address {
+	u8 addr[ETH_ALEN];
+	struct list_head list;
+};
+
+static void mac_exception(void *handle, enum fman_mac_exceptions ex)
+{
+	struct mac_device	*mac_dev;
+	struct mac_priv_s	*priv;
+
+	mac_dev = handle;
+	priv = mac_dev->priv;
+
+	if (ex == FM_MAC_EX_10G_RX_FIFO_OVFL) {
+		/* don't flag RX FIFO after the first */
+		mac_dev->set_exception(mac_dev->fman_mac,
+				       FM_MAC_EX_10G_RX_FIFO_OVFL, false);
+		dev_err(priv->dev, "10G MAC got RX FIFO Error = %x\n", ex);
+	}
+
+	dev_dbg(priv->dev, "%s:%s() -> %d\n", KBUILD_BASENAME ".c",
+		__func__, ex);
+}
+
+static void set_fman_mac_params(struct mac_device *mac_dev,
+				struct fman_mac_params *params)
+{
+	struct mac_priv_s *priv = mac_dev->priv;
+
+	params->base_addr = (typeof(params->base_addr))
+		devm_ioremap(priv->dev, mac_dev->res->start,
+			     resource_size(mac_dev->res));
+	memcpy(&params->addr, mac_dev->addr, sizeof(mac_dev->addr));
+	params->max_speed	= priv->max_speed;
+	params->phy_if		= priv->phy_if;
+	params->basex_if	= false;
+	params->mac_id		= priv->cell_index;
+	params->fm		= (void *)priv->fman;
+	params->exception_cb	= mac_exception;
+	params->event_cb	= mac_exception;
+	params->dev_id		= mac_dev;
+	params->internal_phy_node = priv->internal_phy_node;
+}
+
+static int tgec_initialization(struct mac_device *mac_dev)
+{
+	int err;
+	struct mac_priv_s	*priv;
+	struct fman_mac_params	params;
+	u32			version;
+
+	priv = mac_dev->priv;
+
+	set_fman_mac_params(mac_dev, &params);
+
+	mac_dev->fman_mac = tgec_config(&params);
+	if (!mac_dev->fman_mac) {
+		err = -EINVAL;
+		goto _return;
+	}
+
+	err = tgec_cfg_max_frame_len(mac_dev->fman_mac, fman_get_max_frm());
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = tgec_init(mac_dev->fman_mac);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	/* For 10G MAC, disable Tx ECC exception */
+	err = mac_dev->set_exception(mac_dev->fman_mac,
+				     FM_MAC_EX_10G_TX_ECC_ER, false);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = tgec_get_version(mac_dev->fman_mac, &version);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	dev_info(priv->dev, "FMan XGEC version: 0x%08x\n", version);
+
+	goto _return;
+
+_return_fm_mac_free:
+	tgec_free(mac_dev->fman_mac);
+
+_return:
+	return err;
+}
+
+static int dtsec_initialization(struct mac_device *mac_dev)
+{
+	int			err;
+	struct mac_priv_s	*priv;
+	struct fman_mac_params	params;
+	u32			version;
+
+	priv = mac_dev->priv;
+
+	set_fman_mac_params(mac_dev, &params);
+
+	mac_dev->fman_mac = dtsec_config(&params);
+	if (!mac_dev->fman_mac) {
+		err = -EINVAL;
+		goto _return;
+	}
+
+	err = dtsec_cfg_max_frame_len(mac_dev->fman_mac, fman_get_max_frm());
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = dtsec_cfg_pad_and_crc(mac_dev->fman_mac, true);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = dtsec_init(mac_dev->fman_mac);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	/* For 1G MAC, disable by default the MIB counters overflow interrupt */
+	err = mac_dev->set_exception(mac_dev->fman_mac,
+				     FM_MAC_EX_1G_RX_MIB_CNT_OVFL, false);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = dtsec_get_version(mac_dev->fman_mac, &version);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	dev_info(priv->dev, "FMan dTSEC version: 0x%08x\n", version);
+
+	goto _return;
+
+_return_fm_mac_free:
+	dtsec_free(mac_dev->fman_mac);
+
+_return:
+	return err;
+}
+
+static int memac_initialization(struct mac_device *mac_dev)
+{
+	int			 err;
+	struct mac_priv_s	*priv;
+	struct fman_mac_params	 params;
+
+	priv = mac_dev->priv;
+
+	set_fman_mac_params(mac_dev, &params);
+
+	if (priv->max_speed == SPEED_10000)
+		params.phy_if = PHY_INTERFACE_MODE_XGMII;
+
+	mac_dev->fman_mac = memac_config(&params);
+	if (!mac_dev->fman_mac) {
+		err = -EINVAL;
+		goto _return;
+	}
+
+	err = memac_cfg_max_frame_len(mac_dev->fman_mac, fman_get_max_frm());
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = memac_cfg_reset_on_init(mac_dev->fman_mac, true);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = memac_cfg_fixed_link(mac_dev->fman_mac, priv->fixed_link);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	err = memac_init(mac_dev->fman_mac);
+	if (err < 0)
+		goto _return_fm_mac_free;
+
+	dev_info(priv->dev, "FMan MEMAC\n");
+
+	goto _return;
+
+_return_fm_mac_free:
+	memac_free(mac_dev->fman_mac);
+
+_return:
+	return err;
+}
+
+static int start(struct mac_device *mac_dev)
+{
+	int	 err;
+	struct phy_device *phy_dev = mac_dev->phy_dev;
+	struct mac_priv_s *priv = mac_dev->priv;
+
+	err = priv->enable(mac_dev->fman_mac, COMM_MODE_RX_AND_TX);
+	if (!err && phy_dev)
+		phy_start(phy_dev);
+
+	return err;
+}
+
+static int stop(struct mac_device *mac_dev)
+{
+	struct mac_priv_s *priv = mac_dev->priv;
+
+	if (mac_dev->phy_dev)
+		phy_stop(mac_dev->phy_dev);
+
+	return priv->disable(mac_dev->fman_mac, COMM_MODE_RX_AND_TX);
+}
+
+static int set_multi(struct net_device *net_dev, struct mac_device *mac_dev)
+{
+	struct mac_priv_s	*priv;
+	struct mac_address	*old_addr, *tmp;
+	struct netdev_hw_addr	*ha;
+	int			err;
+	enet_addr_t		*addr;
+
+	priv = mac_dev->priv;
+
+	/* Clear previous address list */
+	list_for_each_entry_safe(old_addr, tmp, &priv->mc_addr_list, list) {
+		addr = (enet_addr_t *)old_addr->addr;
+		err = mac_dev->remove_hash_mac_addr(mac_dev->fman_mac, addr);
+		if (err < 0)
+			return err;
+
+		list_del(&old_addr->list);
+		kfree(old_addr);
+	}
+
+	/* Add all the addresses from the new list */
+	netdev_for_each_mc_addr(ha, net_dev) {
+		addr = (enet_addr_t *)ha->addr;
+		err = mac_dev->add_hash_mac_addr(mac_dev->fman_mac, addr);
+		if (err < 0)
+			return err;
+
+		tmp = kmalloc(sizeof(*tmp), GFP_ATOMIC);
+		if (!tmp)
+			return -ENOMEM;
+
+		ether_addr_copy(tmp->addr, ha->addr);
+		list_add(&tmp->list, &priv->mc_addr_list);
+	}
+	return 0;
+}
+
+/**
+ * fman_set_mac_active_pause
+ * @mac_dev:	A pointer to the MAC device
+ * @rx:		Pause frame setting for RX
+ * @tx:		Pause frame setting for TX
+ *
+ * Set the MAC RX/TX PAUSE frames settings
+ *
+ * Avoid redundant calls to FMD, if the MAC driver already contains the desired
+ * active PAUSE settings. Otherwise, the new active settings should be reflected
+ * in FMan.
+ *
+ * Return: 0 on success; Error code otherwise.
+ */
+int fman_set_mac_active_pause(struct mac_device *mac_dev, bool rx, bool tx)
+{
+	struct fman_mac *fman_mac = mac_dev->fman_mac;
+	int err = 0;
+
+	if (rx != mac_dev->rx_pause_active) {
+		err = mac_dev->set_rx_pause(fman_mac, rx);
+		if (likely(err == 0))
+			mac_dev->rx_pause_active = rx;
+	}
+
+	if (tx != mac_dev->tx_pause_active) {
+		u16 pause_time = (tx ? FSL_FM_PAUSE_TIME_ENABLE :
+					 FSL_FM_PAUSE_TIME_DISABLE);
+
+		err = mac_dev->set_tx_pause(fman_mac, 0, pause_time, 0);
+
+		if (likely(err == 0))
+			mac_dev->tx_pause_active = tx;
+	}
+
+	return err;
+}
+EXPORT_SYMBOL(fman_set_mac_active_pause);
+
+/**
+ * fman_get_pause_cfg
+ * @mac_dev:	A pointer to the MAC device
+ * @rx:		Return value for RX setting
+ * @tx:		Return value for TX setting
+ *
+ * Determine the MAC RX/TX PAUSE frames settings based on PHY
+ * autonegotiation or values set by eththool.
+ *
+ * Return: Pointer to FMan device.
+ */
+void fman_get_pause_cfg(struct mac_device *mac_dev, bool *rx_pause,
+			bool *tx_pause)
+{
+	struct phy_device *phy_dev = mac_dev->phy_dev;
+	u16 lcl_adv, rmt_adv;
+	u8 flowctrl;
+
+	*rx_pause = *tx_pause = false;
+
+	if (!phy_dev->duplex)
+		return;
+
+	/* If PAUSE autonegotiation is disabled, the TX/RX PAUSE settings
+	 * are those set by ethtool.
+	 */
+	if (!mac_dev->autoneg_pause) {
+		*rx_pause = mac_dev->rx_pause_req;
+		*tx_pause = mac_dev->tx_pause_req;
+		return;
+	}
+
+	/* Else if PAUSE autonegotiation is enabled, the TX/RX PAUSE
+	 * settings depend on the result of the link negotiation.
+	 */
+
+	/* get local capabilities */
+	lcl_adv = 0;
+	if (phy_dev->advertising & ADVERTISED_Pause)
+		lcl_adv |= ADVERTISE_PAUSE_CAP;
+	if (phy_dev->advertising & ADVERTISED_Asym_Pause)
+		lcl_adv |= ADVERTISE_PAUSE_ASYM;
+
+	/* get link partner capabilities */
+	rmt_adv = 0;
+	if (phy_dev->pause)
+		rmt_adv |= LPA_PAUSE_CAP;
+	if (phy_dev->asym_pause)
+		rmt_adv |= LPA_PAUSE_ASYM;
+
+	/* Calculate TX/RX settings based on local and peer advertised
+	 * symmetric/asymmetric PAUSE capabilities.
+	 */
+	flowctrl = mii_resolve_flowctrl_fdx(lcl_adv, rmt_adv);
+	if (flowctrl & FLOW_CTRL_RX)
+		*rx_pause = true;
+	if (flowctrl & FLOW_CTRL_TX)
+		*tx_pause = true;
+}
+EXPORT_SYMBOL(fman_get_pause_cfg);
+
+static void adjust_link_void(struct net_device *net_dev)
+{
+}
+
+static void adjust_link_dtsec(struct net_device *net_dev)
+{
+	struct device *dev = net_dev->dev.parent;
+	struct dpaa_eth_data *eth_data = dev->platform_data;
+	struct mac_device *mac_dev = eth_data->mac_dev;
+	struct phy_device *phy_dev = mac_dev->phy_dev;
+	struct fman_mac *fman_mac;
+	bool rx_pause, tx_pause;
+	int err;
+
+	fman_mac = mac_dev->fman_mac;
+	if (!phy_dev->link) {
+		dtsec_restart_autoneg(fman_mac);
+
+		return;
+	}
+
+	dtsec_adjust_link(fman_mac, phy_dev->speed);
+	fman_get_pause_cfg(mac_dev, &rx_pause, &tx_pause);
+	err = fman_set_mac_active_pause(mac_dev, rx_pause, tx_pause);
+	if (err < 0)
+		netdev_err(net_dev, "fman_set_mac_active_pause() = %d\n", err);
+}
+
+static void adjust_link_memac(struct net_device *net_dev)
+{
+	struct device *dev = net_dev->dev.parent;
+	struct dpaa_eth_data *eth_data = dev->platform_data;
+	struct mac_device *mac_dev = eth_data->mac_dev;
+	struct phy_device *phy_dev = mac_dev->phy_dev;
+	struct fman_mac *fman_mac;
+	bool rx_pause, tx_pause;
+	int err;
+
+	fman_mac = mac_dev->fman_mac;
+	memac_adjust_link(fman_mac, phy_dev->speed);
+
+	fman_get_pause_cfg(mac_dev, &rx_pause, &tx_pause);
+	err = fman_set_mac_active_pause(mac_dev, rx_pause, tx_pause);
+	if (err < 0)
+		netdev_err(net_dev, "fman_set_mac_active_pause() = %d\n", err);
+}
+
+/* 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 *))
+{
+	struct phy_device	*phy_dev;
+	struct mac_priv_s	*priv = mac_dev->priv;
+
+	phy_dev = of_phy_connect(net_dev, priv->phy_node, adj_lnk, 0,
+				 priv->phy_if);
+	if (!phy_dev) {
+		netdev_err(net_dev, "Could not connect to PHY\n");
+		return -ENODEV;
+	}
+
+	/* Remove any features not supported by the controller */
+	phy_dev->supported &= mac_dev->if_support;
+	/* Enable the symmetric and asymmetric PAUSE frame advertisements,
+	 * as most of the PHY drivers do not enable them by default.
+	 */
+	phy_dev->supported |= (SUPPORTED_Pause | SUPPORTED_Asym_Pause);
+	phy_dev->advertising = phy_dev->supported;
+
+	mac_dev->phy_dev = phy_dev;
+
+	return 0;
+}
+
+static int 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)
+{
+	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)
+{
+	return init_phy(net_dev, mac_dev, &adjust_link_memac);
+}
+
+static void setup_dtsec(struct mac_device *mac_dev)
+{
+	mac_dev->init_phy		= dtsec_init_phy;
+	mac_dev->init			= dtsec_initialization;
+	mac_dev->set_promisc		= dtsec_set_promiscuous;
+	mac_dev->change_addr		= dtsec_modify_mac_address;
+	mac_dev->add_hash_mac_addr	= dtsec_add_hash_mac_address;
+	mac_dev->remove_hash_mac_addr	= dtsec_del_hash_mac_address;
+	mac_dev->set_tx_pause		= dtsec_set_tx_pause_frames;
+	mac_dev->set_rx_pause		= dtsec_accept_rx_pause_frames;
+	mac_dev->set_exception		= dtsec_set_exception;
+	mac_dev->set_multi		= set_multi;
+	mac_dev->start			= start;
+	mac_dev->stop			= stop;
+
+	mac_dev->priv->enable		= dtsec_enable;
+	mac_dev->priv->disable		= dtsec_disable;
+}
+
+static void setup_tgec(struct mac_device *mac_dev)
+{
+	mac_dev->init_phy		= tgec_init_phy;
+	mac_dev->init			= tgec_initialization;
+	mac_dev->set_promisc		= tgec_set_promiscuous;
+	mac_dev->change_addr		= tgec_modify_mac_address;
+	mac_dev->add_hash_mac_addr	= tgec_add_hash_mac_address;
+	mac_dev->remove_hash_mac_addr	= tgec_del_hash_mac_address;
+	mac_dev->set_tx_pause		= tgec_set_tx_pause_frames;
+	mac_dev->set_rx_pause		= tgec_accept_rx_pause_frames;
+	mac_dev->set_exception		= tgec_set_exception;
+	mac_dev->set_multi		= set_multi;
+	mac_dev->start			= start;
+	mac_dev->stop			= stop;
+
+	mac_dev->priv->enable		= tgec_enable;
+	mac_dev->priv->disable		= tgec_disable;
+}
+
+static void setup_memac(struct mac_device *mac_dev)
+{
+	mac_dev->init_phy		= memac_init_phy;
+	mac_dev->init			= memac_initialization;
+	mac_dev->set_promisc		= memac_set_promiscuous;
+	mac_dev->change_addr		= memac_modify_mac_address;
+	mac_dev->add_hash_mac_addr	= memac_add_hash_mac_address;
+	mac_dev->remove_hash_mac_addr	= memac_del_hash_mac_address;
+	mac_dev->set_tx_pause		= memac_set_tx_pause_frames;
+	mac_dev->set_rx_pause		= memac_accept_rx_pause_frames;
+	mac_dev->set_exception		= memac_set_exception;
+	mac_dev->set_multi		= set_multi;
+	mac_dev->start			= start;
+	mac_dev->stop			= stop;
+
+	mac_dev->priv->enable		= memac_enable;
+	mac_dev->priv->disable		= memac_disable;
+}
+
+#define DTSEC_SUPPORTED \
+	(SUPPORTED_10baseT_Half \
+	| SUPPORTED_10baseT_Full \
+	| SUPPORTED_100baseT_Half \
+	| SUPPORTED_100baseT_Full \
+	| SUPPORTED_Autoneg \
+	| SUPPORTED_Pause \
+	| SUPPORTED_Asym_Pause \
+	| SUPPORTED_MII)
+
+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,
+	[PHY_INTERFACE_MODE_SGMII]		= SPEED_1000,
+	[PHY_INTERFACE_MODE_TBI]		= SPEED_1000,
+	[PHY_INTERFACE_MODE_RMII]		= SPEED_100,
+	[PHY_INTERFACE_MODE_RGMII]		= SPEED_1000,
+	[PHY_INTERFACE_MODE_RGMII_ID]		= SPEED_1000,
+	[PHY_INTERFACE_MODE_RGMII_RXID]	= SPEED_1000,
+	[PHY_INTERFACE_MODE_RGMII_TXID]	= SPEED_1000,
+	[PHY_INTERFACE_MODE_RTBI]		= SPEED_1000,
+	[PHY_INTERFACE_MODE_XGMII]		= SPEED_10000
+};
+
+static struct platform_device *dpaa_eth_add_device(int fman_id,
+						   struct mac_device *mac_dev,
+						   struct device_node *node)
+{
+	struct platform_device *pdev;
+	struct dpaa_eth_data data;
+	struct mac_priv_s	*priv;
+	static int dpaa_eth_dev_cnt;
+	int ret;
+
+	priv = mac_dev->priv;
+
+	data.mac_dev = mac_dev;
+	data.mac_hw_id = priv->cell_index;
+	data.fman_hw_id = fman_id;
+	data.mac_node = node;
+
+	mutex_lock(&eth_lock);
+
+	pdev = platform_device_alloc("dpaa-ethernet", dpaa_eth_dev_cnt);
+	if (!pdev) {
+		ret = -ENOMEM;
+		goto no_mem;
+	}
+
+	ret = platform_device_add_data(pdev, &data, sizeof(data));
+	if (ret)
+		goto err;
+
+	ret = platform_device_add(pdev);
+	if (ret)
+		goto err;
+
+	dpaa_eth_dev_cnt++;
+	mutex_unlock(&eth_lock);
+
+	return pdev;
+
+err:
+	platform_device_put(pdev);
+no_mem:
+	mutex_unlock(&eth_lock);
+
+	return ERR_PTR(ret);
+}
+
+static const struct of_device_id mac_match[] = {
+	{ .compatible	= "fsl,fman-dtsec" },
+	{ .compatible	= "fsl,fman-xgec" },
+	{ .compatible	= "fsl,fman-memac" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, mac_match);
+
+static int mac_probe(struct platform_device *_of_dev)
+{
+	int			 err, i, lenp, nph;
+	struct device		*dev;
+	struct device_node	*mac_node, *dev_node;
+	struct mac_device	*mac_dev;
+	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;
+	u8			fman_id;
+
+	dev = &_of_dev->dev;
+	mac_node = dev->of_node;
+
+	mac_dev = devm_kzalloc(dev, sizeof(*mac_dev), GFP_KERNEL);
+	if (!mac_dev) {
+		err = -ENOMEM;
+		dev_err(dev, "devm_kzalloc() = %d\n", err);
+		goto _return;
+	}
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		err = -ENOMEM;
+		goto _return;
+	}
+
+	/* Save private information */
+	mac_dev->priv = priv;
+	priv->dev = dev;
+
+	if (of_device_is_compatible(mac_node, "fsl,fman-dtsec")) {
+		setup_dtsec(mac_dev);
+		priv->internal_phy_node = of_parse_phandle(mac_node,
+							  "tbi-handle", 0);
+	} else if (of_device_is_compatible(mac_node, "fsl,fman-xgec")) {
+		setup_tgec(mac_dev);
+	} else if (of_device_is_compatible(mac_node, "fsl,fman-memac")) {
+		setup_memac(mac_dev);
+		priv->internal_phy_node = of_parse_phandle(mac_node,
+							  "pcsphy-handle", 0);
+	} else {
+		dev_err(dev, "MAC node (%s) contains unsupported MAC\n",
+			mac_node->full_name);
+		err = -EINVAL;
+		goto _return;
+	}
+
+	/* Register mac_dev */
+	dev_set_drvdata(dev, mac_dev);
+
+	INIT_LIST_HEAD(&priv->mc_addr_list);
+
+	/* Get the FM node */
+	dev_node = of_get_parent(mac_node);
+	if (!dev_node) {
+		dev_err(dev, "of_get_parent(%s) failed\n",
+			mac_node->full_name);
+		err = -EINVAL;
+		goto _return_dev_set_drvdata;
+	}
+
+	of_dev = of_find_device_by_node(dev_node);
+	if (!of_dev) {
+		dev_err(dev, "of_find_device_by_node(%s) failed\n",
+			dev_node->full_name);
+		err = -EINVAL;
+		goto _return_of_node_put;
+	}
+
+	/* 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",
+			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);
+
+	priv->fman = fman_bind(&of_dev->dev);
+	if (!priv->fman) {
+		dev_err(dev, "fman_bind(%s) failed\n", dev_node->full_name);
+		err = -ENODEV;
+		goto _return_of_node_put;
+	}
+
+	of_node_put(dev_node);
+
+	/* Get the address of the memory mapped registers */
+	err = of_address_to_resource(mac_node, 0, &res);
+	if (err < 0) {
+		dev_err(dev, "of_address_to_resource(%s) = %d\n",
+			mac_node->full_name, err);
+		goto _return_dev_set_drvdata;
+	}
+
+	mac_dev->res = __devm_request_region(dev,
+					     fman_get_mem_region(priv->fman),
+					     res.start, res.end + 1 - res.start,
+					     "mac");
+	if (!mac_dev->res) {
+		dev_err(dev, "__devm_request_mem_region(mac) failed\n");
+		err = -EBUSY;
+		goto _return_dev_set_drvdata;
+	}
+
+	priv->vaddr = devm_ioremap(dev, mac_dev->res->start,
+				   mac_dev->res->end + 1 - mac_dev->res->start);
+	if (!priv->vaddr) {
+		dev_err(dev, "devm_ioremap() failed\n");
+		err = -EIO;
+		goto _return_dev_set_drvdata;
+	}
+
+	if (!of_device_is_available(mac_node)) {
+		devm_iounmap(dev, priv->vaddr);
+		__devm_release_region(dev, fman_get_mem_region(priv->fman),
+				      res.start, res.end + 1 - res.start);
+		devm_kfree(dev, mac_dev);
+		dev_set_drvdata(dev, NULL);
+		return -ENODEV;
+	}
+
+	/* 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",
+			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]);
+
+	/* Get the MAC address */
+	mac_addr = of_get_mac_address(mac_node);
+	if (!mac_addr) {
+		dev_err(dev, "of_get_mac_address(%s) failed\n",
+			mac_node->full_name);
+		err = -EINVAL;
+		goto _return_dev_set_drvdata;
+	}
+	memcpy(mac_dev->addr, mac_addr, sizeof(mac_dev->addr));
+
+	/* Get the port handles */
+	nph = of_count_phandle_with_args(mac_node, "fsl,fman-ports", NULL);
+	if (unlikely(nph < 0)) {
+		dev_err(dev, "of_count_phandle_with_args(%s, fsl,fman-ports) failed\n",
+			mac_node->full_name);
+		err = nph;
+		goto _return_dev_set_drvdata;
+	}
+
+	if (nph != ARRAY_SIZE(mac_dev->port)) {
+		dev_err(dev, "Not supported number of fman-ports handles of mac node %s from device tree\n",
+			mac_node->full_name);
+		err = -EINVAL;
+		goto _return_dev_set_drvdata;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(mac_dev->port); i++) {
+		/* Find the port node */
+		dev_node = of_parse_phandle(mac_node, "fsl,fman-ports", i);
+		if (!dev_node) {
+			dev_err(dev, "of_parse_phandle(%s, fsl,fman-ports) failed\n",
+				mac_node->full_name);
+			err = -EINVAL;
+			goto _return_of_node_put;
+		}
+
+		of_dev = of_find_device_by_node(dev_node);
+		if (!of_dev) {
+			dev_err(dev, "of_find_device_by_node(%s) failed\n",
+				dev_node->full_name);
+			err = -EINVAL;
+			goto _return_of_node_put;
+		}
+
+		mac_dev->port[i] = fman_port_bind(&of_dev->dev);
+		if (!mac_dev->port[i]) {
+			dev_err(dev, "dev_get_drvdata(%s) failed\n",
+				dev_node->full_name);
+			err = -EINVAL;
+			goto _return_of_node_put;
+		}
+		of_node_put(dev_node);
+	}
+
+	/* Get the PHY connection type */
+	char_prop = (const char *)of_get_property(mac_node,
+						  "phy-connection-type", NULL);
+	if (!char_prop) {
+		dev_warn(dev,
+			 "of_get_property(%s, phy-connection-type) failed. Defaulting to MII\n",
+			 mac_node->full_name);
+		priv->phy_if = PHY_INTERFACE_MODE_MII;
+	} else {
+		priv->phy_if = str2phy(char_prop);
+	}
+
+	priv->speed		= phy2speed[priv->phy_if];
+	priv->max_speed		= priv->speed;
+	mac_dev->if_support	= DTSEC_SUPPORTED;
+	/* We don't support half-duplex in SGMII mode */
+	if (priv->phy_if == PHY_INTERFACE_MODE_SGMII)
+		mac_dev->if_support &= ~(SUPPORTED_10baseT_Half |
+					SUPPORTED_100baseT_Half);
+
+	/* Gigabit support (no half-duplex) */
+	if (priv->max_speed == 1000)
+		mac_dev->if_support |= SUPPORTED_1000baseT_Full;
+
+	/* The 10G interface only supports one mode */
+	if (priv->phy_if == PHY_INTERFACE_MODE_XGMII)
+		mac_dev->if_support = SUPPORTED_10000baseT_Full;
+
+	/* Get the rest of the PHY information */
+	priv->phy_node = of_parse_phandle(mac_node, "phy-handle", 0);
+	if (!priv->phy_node && of_phy_is_fixed_link(mac_node)) {
+		struct phy_device *phy;
+
+		err = of_phy_register_fixed_link(mac_node);
+		if (err)
+			goto _return_dev_set_drvdata;
+
+		priv->fixed_link = kzalloc(sizeof(*priv->fixed_link),
+					   GFP_KERNEL);
+		if (!priv->fixed_link)
+			goto _return_dev_set_drvdata;
+
+		priv->phy_node = of_node_get(mac_node);
+		phy = of_phy_find_device(priv->phy_node);
+		if (!phy)
+			goto _return_dev_set_drvdata;
+
+		priv->fixed_link->link = phy->link;
+		priv->fixed_link->speed = phy->speed;
+		priv->fixed_link->duplex = phy->duplex;
+		priv->fixed_link->pause = phy->pause;
+		priv->fixed_link->asym_pause = phy->asym_pause;
+	}
+
+	err = mac_dev->init(mac_dev);
+	if (err < 0) {
+		dev_err(dev, "mac_dev->init() = %d\n", err);
+		of_node_put(priv->phy_node);
+		goto _return_dev_set_drvdata;
+	}
+
+	/* pause frame autonegotiation enabled */
+	mac_dev->autoneg_pause = true;
+
+	/* By intializing the values to false, force FMD to enable PAUSE frames
+	 * on RX and TX
+	 */
+	mac_dev->rx_pause_req = true;
+	mac_dev->tx_pause_req = true;
+	mac_dev->rx_pause_active = false;
+	mac_dev->tx_pause_active = false;
+	err = fman_set_mac_active_pause(mac_dev, true, true);
+	if (err < 0)
+		dev_err(dev, "fman_set_mac_active_pause() = %d\n", err);
+
+	dev_info(dev, "FMan MAC address: %02hx:%02hx:%02hx:%02hx:%02hx:%02hx\n",
+		 mac_dev->addr[0], mac_dev->addr[1], mac_dev->addr[2],
+		 mac_dev->addr[3], mac_dev->addr[4], mac_dev->addr[5]);
+
+	priv->eth_dev = dpaa_eth_add_device(fman_id, mac_dev, mac_node);
+	if (IS_ERR(priv->eth_dev)) {
+		dev_err(dev, "failed to add Ethernet platform device for MAC %d\n",
+			priv->cell_index);
+		priv->eth_dev = NULL;
+	}
+
+	goto _return;
+
+_return_of_node_put:
+	of_node_put(dev_node);
+_return_dev_set_drvdata:
+	kfree(priv->fixed_link);
+	dev_set_drvdata(dev, NULL);
+_return:
+	return err;
+}
+
+static struct platform_driver mac_driver = {
+	.driver = {
+		.name		= KBUILD_MODNAME,
+		.of_match_table	= mac_match,
+	},
+	.probe		= mac_probe,
+};
+
+builtin_platform_driver(mac_driver);
diff --git a/drivers/net/ethernet/freescale/fman/mac.h b/drivers/net/ethernet/freescale/fman/mac.h
new file mode 100644
index 000000000000..0211cc9a46d6
--- /dev/null
+++ b/drivers/net/ethernet/freescale/fman/mac.h
@@ -0,0 +1,97 @@
+/* Copyright 2008-2015 Freescale Semiconductor, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *	 notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *	 notice, this list of conditions and the following disclaimer in the
+ *	 documentation and/or other materials provided with the distribution.
+ *     * Neither the name of Freescale Semiconductor 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") as published by the Free Software
+ * Foundation, either version 2 of that License or (at your option) any
+ * later version.
+ *
+ * THIS SOFTWARE IS PROVIDED BY Freescale Semiconductor ``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 Freescale Semiconductor BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __MAC_H
+#define __MAC_H
+
+#include <linux/device.h>
+#include <linux/if_ether.h>
+#include <linux/phy.h>
+#include <linux/list.h>
+
+#include "fman_port.h"
+#include "fman.h"
+#include "fman_mac.h"
+
+struct fman_mac;
+struct mac_priv_s;
+
+struct mac_device {
+	struct resource		*res;
+	u8			 addr[ETH_ALEN];
+	struct fman_port	*port[2];
+	u32			 if_support;
+	struct phy_device	*phy_dev;
+
+	bool autoneg_pause;
+	bool rx_pause_req;
+	bool tx_pause_req;
+	bool rx_pause_active;
+	bool tx_pause_active;
+	bool promisc;
+
+	int (*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);
+	int (*set_promisc)(struct fman_mac *mac_dev, bool enable);
+	int (*change_addr)(struct fman_mac *mac_dev, enet_addr_t *enet_addr);
+	int (*set_multi)(struct net_device *net_dev,
+			 struct mac_device *mac_dev);
+	int (*set_rx_pause)(struct fman_mac *mac_dev, bool en);
+	int (*set_tx_pause)(struct fman_mac *mac_dev, u8 priority,
+			    u16 pause_time, u16 thresh_time);
+	int (*set_exception)(struct fman_mac *mac_dev,
+			     enum fman_mac_exceptions exception, bool enable);
+	int (*add_hash_mac_addr)(struct fman_mac *mac_dev,
+				 enet_addr_t *eth_addr);
+	int (*remove_hash_mac_addr)(struct fman_mac *mac_dev,
+				    enet_addr_t *eth_addr);
+
+	struct fman_mac		*fman_mac;
+	struct mac_priv_s	*priv;
+};
+
+struct dpaa_eth_data {
+	struct device_node *mac_node;
+	struct mac_device *mac_dev;
+	int mac_hw_id;
+	int fman_hw_id;
+};
+
+extern const char	*mac_driver_description;
+
+int fman_set_mac_active_pause(struct mac_device *mac_dev, bool rx, bool tx);
+
+void fman_get_pause_cfg(struct mac_device *mac_dev, bool *rx_pause,
+			bool *tx_pause);
+
+#endif	/* __MAC_H */
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 cf8e54652df9..48a9c176e0d1 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -1050,7 +1050,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_napi_add(ndev, &fep->napi_tx, fs_enet_tx_napi, 2);
+	netif_tx_napi_add(ndev, &fep->napi_tx, fs_enet_tx_napi, 2);
 
 	ndev->ethtool_ops = &fs_ethtool_ops;
 
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
index 52e0091b4fb2..1ba359f17ec6 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
@@ -552,7 +552,7 @@ static void tx_restart(struct net_device *dev)
 	cbd_t __iomem *prev_bd;
 	cbd_t __iomem *last_tx_bd;
 
-	last_tx_bd = fep->tx_bd_base + ((fpi->tx_ring - 1) * sizeof(cbd_t));
+	last_tx_bd = fep->tx_bd_base + (fpi->tx_ring - 1);
 
 	/* get the current bd held in TBPTR  and scan back from this point */
 	recheck_bd = curr_tbptr = (cbd_t __iomem *)
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
index 016743e355de..bade2f8f9b5c 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
@@ -254,7 +254,7 @@ static void restart(struct net_device *dev)
 	int r;
 	u32 addrhi, addrlo;
 
-	struct mii_bus* mii = fep->phydev->bus;
+	struct mii_bus *mii = fep->phydev->mdio.bus;
 	struct fec_info* fec_inf = mii->priv;
 
 	r = whack_reset(fep->fec.fecp);
@@ -363,7 +363,7 @@ static void stop(struct net_device *dev)
 	const struct fs_platform_info *fpi = fep->fpi;
 	struct fec __iomem *fecp = fep->fec.fecp;
 
-	struct fec_info* feci= fep->phydev->bus->priv;
+	struct fec_info *feci = fep->phydev->mdio.bus->priv;
 
 	int i;
 
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
index 68a428de0bc0..1f015edcca22 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-bitbang.c
@@ -172,23 +172,16 @@ static int fs_enet_mdio_probe(struct platform_device *ofdev)
 		goto out_free_bus;
 
 	new_bus->phy_mask = ~0;
-	new_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!new_bus->irq) {
-		ret = -ENOMEM;
-		goto out_unmap_regs;
-	}
 
 	new_bus->parent = &ofdev->dev;
 	platform_set_drvdata(ofdev, new_bus);
 
 	ret = of_mdiobus_register(new_bus, ofdev->dev.of_node);
 	if (ret)
-		goto out_free_irqs;
+		goto out_unmap_regs;
 
 	return 0;
 
-out_free_irqs:
-	kfree(new_bus->irq);
 out_unmap_regs:
 	iounmap(bitbang->dir);
 out_free_bus:
@@ -205,7 +198,6 @@ static int fs_enet_mdio_remove(struct platform_device *ofdev)
 	struct bb_info *bitbang = bus->priv;
 
 	mdiobus_unregister(bus);
-	kfree(bus->irq);
 	free_mdio_bitbang(bus);
 	iounmap(bitbang->dir);
 	kfree(bitbang);
diff --git a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
index 2be383e6d258..a89267b94352 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mii-fec.c
@@ -166,23 +166,16 @@ static int fs_enet_mdio_probe(struct platform_device *ofdev)
 	clrsetbits_be32(&fec->fecp->fec_mii_speed, 0x7E, fec->mii_speed);
 
 	new_bus->phy_mask = ~0;
-	new_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!new_bus->irq) {
-		ret = -ENOMEM;
-		goto out_unmap_regs;
-	}
 
 	new_bus->parent = &ofdev->dev;
 	platform_set_drvdata(ofdev, new_bus);
 
 	ret = of_mdiobus_register(new_bus, ofdev->dev.of_node);
 	if (ret)
-		goto out_free_irqs;
+		goto out_unmap_regs;
 
 	return 0;
 
-out_free_irqs:
-	kfree(new_bus->irq);
 out_unmap_regs:
 	iounmap(fec->fecp);
 out_res:
@@ -200,7 +193,6 @@ static int fs_enet_mdio_remove(struct platform_device *ofdev)
 	struct fec_info *fec = bus->priv;
 
 	mdiobus_unregister(bus);
-	kfree(bus->irq);
 	iounmap(fec->fecp);
 	kfree(fec);
 	mdiobus_free(bus);
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index 40071dad1c57..f3c63dce1e30 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -29,7 +29,7 @@
 
 #include <asm/io.h>
 #if IS_ENABLED(CONFIG_UCC_GETH)
-#include <asm/ucc.h>	/* for ucc_set_qe_mux_mii_mng() */
+#include <soc/fsl/qe/ucc.h>
 #endif
 
 #include "gianfar.h"
@@ -69,7 +69,6 @@ struct fsl_pq_mdio {
 struct fsl_pq_mdio_priv {
 	void __iomem *map;
 	struct fsl_pq_mii __iomem *regs;
-	int irqs[PHY_MAX_ADDR];
 };
 
 /*
@@ -401,7 +400,6 @@ static int fsl_pq_mdio_probe(struct platform_device *pdev)
 	new_bus->read = &fsl_pq_mdio_read;
 	new_bus->write = &fsl_pq_mdio_write;
 	new_bus->reset = &fsl_pq_mdio_reset;
-	new_bus->irq = priv->irqs;
 
 	err = of_address_to_resource(np, 0, &res);
 	if (err < 0) {
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index 3e233d924cce..b9ecf197ad11 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -738,7 +738,6 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 	struct gfar_private *priv = NULL;
 	struct device_node *np = ofdev->dev.of_node;
 	struct device_node *child = NULL;
-	struct property *stash;
 	u32 stash_len = 0;
 	u32 stash_idx = 0;
 	unsigned int num_tx_qs, num_rx_qs;
@@ -854,9 +853,7 @@ static int gfar_of_init(struct platform_device *ofdev, struct net_device **pdev)
 			goto err_grp_init;
 	}
 
-	stash = of_find_property(np, "bd-stash", NULL);
-
-	if (stash) {
+	if (of_property_read_bool(np, "bd-stash")) {
 		priv->device_flags |= FSL_GIANFAR_DEV_HAS_BD_STASHING;
 		priv->bd_stash_en = 1;
 	}
@@ -1114,8 +1111,10 @@ static void __gfar_detect_errata_85xx(struct gfar_private *priv)
 
 	if ((SVR_SOC_VER(svr) == SVR_8548) && (SVR_REV(svr) == 0x20))
 		priv->errata |= GFAR_ERRATA_12;
+	/* P2020/P1010 Rev 1; MPC8548 Rev 2 */
 	if (((SVR_SOC_VER(svr) == SVR_P2020) && (SVR_REV(svr) < 0x20)) ||
-	    ((SVR_SOC_VER(svr) == SVR_P2010) && (SVR_REV(svr) < 0x20)))
+	    ((SVR_SOC_VER(svr) == SVR_P2010) && (SVR_REV(svr) < 0x20)) ||
+	    ((SVR_SOC_VER(svr) == SVR_8548) && (SVR_REV(svr) < 0x31)))
 		priv->errata |= GFAR_ERRATA_76; /* aka eTSEC 20 */
 }
 #endif
@@ -1348,12 +1347,12 @@ static int gfar_probe(struct platform_device *ofdev)
 		if (priv->poll_mode == GFAR_SQ_POLLING) {
 			netif_napi_add(dev, &priv->gfargrp[i].napi_rx,
 				       gfar_poll_rx_sq, GFAR_DEV_WEIGHT);
-			netif_napi_add(dev, &priv->gfargrp[i].napi_tx,
+			netif_tx_napi_add(dev, &priv->gfargrp[i].napi_tx,
 				       gfar_poll_tx_sq, 2);
 		} else {
 			netif_napi_add(dev, &priv->gfargrp[i].napi_rx,
 				       gfar_poll_rx, GFAR_DEV_WEIGHT);
-			netif_napi_add(dev, &priv->gfargrp[i].napi_tx,
+			netif_tx_napi_add(dev, &priv->gfargrp[i].napi_tx,
 				       gfar_poll_tx, 2);
 		}
 	}
@@ -1837,7 +1836,7 @@ static void gfar_configure_serdes(struct net_device *dev)
 	 * several seconds for it to come back.
 	 */
 	if (phy_read(tbiphy, MII_BMSR) & BMSR_LSTATUS) {
-		put_device(&tbiphy->dev);
+		put_device(&tbiphy->mdio.dev);
 		return;
 	}
 
@@ -1852,7 +1851,7 @@ static void gfar_configure_serdes(struct net_device *dev)
 		  BMCR_ANENABLE | BMCR_ANRESTART | BMCR_FULLDPLX |
 		  BMCR_SPEED1000);
 
-	put_device(&tbiphy->dev);
+	put_device(&tbiphy->mdio.dev);
 }
 
 static int __gfar_is_rx_idle(struct gfar_private *priv)
diff --git a/drivers/net/ethernet/freescale/ucc_geth.c b/drivers/net/ethernet/freescale/ucc_geth.c
index 650f7888e32b..5bf1ade28315 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.c
+++ b/drivers/net/ethernet/freescale/ucc_geth.c
@@ -40,10 +40,10 @@
 #include <asm/uaccess.h>
 #include <asm/irq.h>
 #include <asm/io.h>
-#include <asm/immap_qe.h>
-#include <asm/qe.h>
-#include <asm/ucc.h>
-#include <asm/ucc_fast.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+#include <soc/fsl/qe/ucc.h>
+#include <soc/fsl/qe/ucc_fast.h>
 #include <asm/machdep.h>
 
 #include "ucc_geth.h"
@@ -1385,7 +1385,7 @@ static int adjust_enet_interface(struct ucc_geth_private *ugeth)
 		value &= ~0x1000;	/* Turn off autonegotiation */
 		phy_write(tbiphy, ENET_TBI_MII_CR, value);
 
-		put_device(&tbiphy->dev);
+		put_device(&tbiphy->mdio.dev);
 	}
 
 	init_check_frame_length_mode(ug_info->lengthCheckRx, &ug_regs->maccfg2);
@@ -1705,7 +1705,7 @@ static void uec_configure_serdes(struct net_device *dev)
 	 * several seconds for it to come back.
 	 */
 	if (phy_read(tbiphy, ENET_TBI_MII_SR) & TBISR_LSTATUS) {
-		put_device(&tbiphy->dev);
+		put_device(&tbiphy->mdio.dev);
 		return;
 	}
 
@@ -1716,7 +1716,7 @@ static void uec_configure_serdes(struct net_device *dev)
 
 	phy_write(tbiphy, ENET_TBI_MII_CR, TBICR_SETTINGS);
 
-	put_device(&tbiphy->dev);
+	put_device(&tbiphy->mdio.dev);
 }
 
 /* Configure the PHY for dev.
diff --git a/drivers/net/ethernet/freescale/ucc_geth.h b/drivers/net/ethernet/freescale/ucc_geth.h
index 75f337163ce3..5da19b440a6a 100644
--- a/drivers/net/ethernet/freescale/ucc_geth.h
+++ b/drivers/net/ethernet/freescale/ucc_geth.h
@@ -22,11 +22,11 @@
 #include <linux/list.h>
 #include <linux/if_ether.h>
 
-#include <asm/immap_qe.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
 
-#include <asm/ucc.h>
-#include <asm/ucc_fast.h>
+#include <soc/fsl/qe/ucc.h>
+#include <soc/fsl/qe/ucc_fast.h>
 
 #define DRV_DESC "QE UCC Gigabit Ethernet Controller"
 #define DRV_NAME "ucc_geth"
diff --git a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
index a7139f588ad2..678f5018d0be 100644
--- a/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
+++ b/drivers/net/ethernet/fujitsu/fmvj18x_cs.c
@@ -469,8 +469,8 @@ static int fmvj18x_config(struct pcmcia_device *link)
 		    goto failed;
 	    }
 	    /* Read MACID from CIS */
-	    for (i = 5; i < 11; i++)
-		    dev->dev_addr[i] = buf[i];
+	    for (i = 0; i < 6; i++)
+		    dev->dev_addr[i] = buf[i + 5];
 	    kfree(buf);
 	} else {
 	    if (pcmcia_get_mac_from_cis(link, dev))
diff --git a/drivers/net/ethernet/hisilicon/Kconfig b/drivers/net/ethernet/hisilicon/Kconfig
index 74beb1867230..4ccc032633c4 100644
--- a/drivers/net/ethernet/hisilicon/Kconfig
+++ b/drivers/net/ethernet/hisilicon/Kconfig
@@ -25,6 +25,7 @@ config HIX5HD2_GMAC
 
 config HIP04_ETH
 	tristate "HISILICON P04 Ethernet support"
+	depends on HAS_IOMEM	# For MFD_SYSCON
 	select MARVELL_PHY
 	select MFD_SYSCON
 	select HNS_MDIO
diff --git a/drivers/net/ethernet/hisilicon/hip04_eth.c b/drivers/net/ethernet/hisilicon/hip04_eth.c
index 253f8ed0537a..0c4afe95ef54 100644
--- a/drivers/net/ethernet/hisilicon/hip04_eth.c
+++ b/drivers/net/ethernet/hisilicon/hip04_eth.c
@@ -500,8 +500,10 @@ static int hip04_rx_poll(struct napi_struct *napi, int budget)
 	while (cnt && !last) {
 		buf = priv->rx_buf[priv->rx_head];
 		skb = build_skb(buf, priv->rx_buf_size);
-		if (unlikely(!skb))
+		if (unlikely(!skb)) {
 			net_dbg_ratelimited("build_skb failed\n");
+			goto refill;
+		}
 
 		dma_unmap_single(&ndev->dev, priv->rx_phys[priv->rx_head],
 				 RX_BUF_SIZE, DMA_FROM_DEVICE);
@@ -528,6 +530,7 @@ static int hip04_rx_poll(struct napi_struct *napi, int budget)
 			rx++;
 		}
 
+refill:
 		buf = netdev_alloc_frag(priv->rx_buf_size);
 		if (!buf)
 			goto done;
diff --git a/drivers/net/ethernet/hisilicon/hns/hnae.c b/drivers/net/ethernet/hisilicon/hns/hnae.c
index b3645297477e..3bfe36f9405b 100644
--- a/drivers/net/ethernet/hisilicon/hns/hnae.c
+++ b/drivers/net/ethernet/hisilicon/hns/hnae.c
@@ -95,21 +95,17 @@ static struct hnae_buf_ops hnae_bops = {
 static int __ae_match(struct device *dev, const void *data)
 {
 	struct hnae_ae_dev *hdev = cls_to_ae_dev(dev);
-	const char *ae_id = data;
 
-	if (!strncmp(ae_id, hdev->name, AE_NAME_SIZE))
-		return 1;
-
-	return 0;
+	return hdev->dev->of_node == data;
 }
 
-static struct hnae_ae_dev *find_ae(const char *ae_id)
+static struct hnae_ae_dev *find_ae(const struct device_node *ae_node)
 {
 	struct device *dev;
 
-	WARN_ON(!ae_id);
+	WARN_ON(!ae_node);
 
-	dev = class_find_device(hnae_class, NULL, ae_id, __ae_match);
+	dev = class_find_device(hnae_class, NULL, ae_node, __ae_match);
 
 	return dev ? cls_to_ae_dev(dev) : NULL;
 }
@@ -316,7 +312,8 @@ EXPORT_SYMBOL(hnae_reinit_handle);
  * return handle ptr or ERR_PTR
  */
 struct hnae_handle *hnae_get_handle(struct device *owner_dev,
-				    const char *ae_id, u32 port_id,
+				    const struct device_node *ae_node,
+				    u32 port_id,
 				    struct hnae_buf_ops *bops)
 {
 	struct hnae_ae_dev *dev;
@@ -324,7 +321,7 @@ struct hnae_handle *hnae_get_handle(struct device *owner_dev,
 	int i, j;
 	int ret;
 
-	dev = find_ae(ae_id);
+	dev = find_ae(ae_node);
 	if (!dev)
 		return ERR_PTR(-ENODEV);
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hnae.h b/drivers/net/ethernet/hisilicon/hns/hnae.h
index cec95ac8687d..1cbcb9fa3fb5 100644
--- a/drivers/net/ethernet/hisilicon/hns/hnae.h
+++ b/drivers/net/ethernet/hisilicon/hns/hnae.h
@@ -35,7 +35,7 @@
 #include <linux/phy.h>
 #include <linux/types.h>
 
-#define HNAE_DRIVER_VERSION "1.3.0"
+#define HNAE_DRIVER_VERSION "2.0"
 #define HNAE_DRIVER_NAME "hns"
 #define HNAE_COPYRIGHT "Copyright(c) 2015 Huawei Corporation."
 #define HNAE_DRIVER_STRING "Hisilicon Network Subsystem Driver"
@@ -63,6 +63,7 @@ do { \
 
 #define AE_VERSION_1 ('6' << 16 | '6' << 8 | '0')
 #define AE_VERSION_2 ('1' << 24 | '6' << 16 | '1' << 8 | '0')
+#define AE_IS_VER1(ver) ((ver) == AE_VERSION_1)
 #define AE_NAME_SIZE 16
 
 /* some said the RX and TX RCB format should not be the same in the future. But
@@ -144,23 +145,61 @@ enum hnae_led_state {
 #define HNS_RXD_ASID_S 24
 #define HNS_RXD_ASID_M (0xff << HNS_RXD_ASID_S)
 
+#define HNSV2_TXD_BUFNUM_S 0
+#define HNSV2_TXD_BUFNUM_M (0x7 << HNSV2_TXD_BUFNUM_S)
+#define HNSV2_TXD_RI_B   1
+#define HNSV2_TXD_L4CS_B   2
+#define HNSV2_TXD_L3CS_B   3
+#define HNSV2_TXD_FE_B   4
+#define HNSV2_TXD_VLD_B  5
+
+#define HNSV2_TXD_TSE_B   0
+#define HNSV2_TXD_VLAN_EN_B   1
+#define HNSV2_TXD_SNAP_B   2
+#define HNSV2_TXD_IPV6_B   3
+#define HNSV2_TXD_SCTP_B   4
+
 /* hardware spec ring buffer format */
 struct __packed hnae_desc {
 	__le64 addr;
 	union {
 		struct {
-			__le16 asid_bufnum_pid;
+			union {
+				__le16 asid_bufnum_pid;
+				__le16 asid;
+			};
 			__le16 send_size;
-			__le32 flag_ipoffset;
-			__le32 reserved_3[4];
+			union {
+				__le32 flag_ipoffset;
+				struct {
+					__u8 bn_pid;
+					__u8 ra_ri_cs_fe_vld;
+					__u8 ip_offset;
+					__u8 tse_vlan_snap_v6_sctp_nth;
+				};
+			};
+			__le16 mss;
+			__u8 l4_len;
+			__u8 reserved1;
+			__le16 paylen;
+			__u8 vmid;
+			__u8 qid;
+			__le32 reserved2[2];
 		} tx;
 
 		struct {
 			__le32 ipoff_bnum_pid_flag;
 			__le16 pkt_len;
 			__le16 size;
-			__le32 vlan_pri_asid;
-			__le32 reserved_2[3];
+			union {
+				__le32 vlan_pri_asid;
+				struct {
+					__le16 asid;
+					__le16 vlan_cfi_pri;
+				};
+			};
+			__le32 rss_hash;
+			__le32 reserved_1[2];
 		} rx;
 	};
 };
@@ -302,7 +341,8 @@ struct hnae_queue {
 	void __iomem *io_base;
 	phys_addr_t phy_base;
 	struct hnae_ae_dev *dev;	/* the device who use this queue */
-	struct hnae_ring rx_ring, tx_ring;
+	struct hnae_ring rx_ring ____cacheline_internodealigned_in_smp;
+	struct hnae_ring tx_ring ____cacheline_internodealigned_in_smp;
 	struct hnae_handle *handle;
 };
 
@@ -435,6 +475,7 @@ struct hnae_ae_ops {
 	int (*set_mac_addr)(struct hnae_handle *handle, void *p);
 	int (*set_mc_addr)(struct hnae_handle *handle, void *addr);
 	int (*set_mtu)(struct hnae_handle *handle, int new_mtu);
+	void (*set_tso_stats)(struct hnae_handle *handle, int enable);
 	void (*update_stats)(struct hnae_handle *handle,
 			     struct net_device_stats *net_stats);
 	void (*get_stats)(struct hnae_handle *handle, u64 *data);
@@ -446,6 +487,12 @@ struct hnae_ae_ops {
 			  enum hnae_led_state status);
 	void (*get_regs)(struct hnae_handle *handle, void *data);
 	int (*get_regs_len)(struct hnae_handle *handle);
+	u32	(*get_rss_key_size)(struct hnae_handle *handle);
+	u32	(*get_rss_indir_size)(struct hnae_handle *handle);
+	int	(*get_rss)(struct hnae_handle *handle, u32 *indir, u8 *key,
+			   u8 *hfunc);
+	int	(*set_rss)(struct hnae_handle *handle, const u32 *indir,
+			   const u8 *key, const u8 hfunc);
 };
 
 struct hnae_ae_dev {
@@ -477,8 +524,11 @@ struct hnae_handle {
 
 #define ring_to_dev(ring) ((ring)->q->dev->dev)
 
-struct hnae_handle *hnae_get_handle(struct device *owner_dev, const char *ae_id,
-				    u32 port_id, struct hnae_buf_ops *bops);
+struct hnae_handle *hnae_get_handle(struct device *owner_dev,
+				    const struct device_node *ae_node,
+				    u32 port_id,
+				    struct hnae_buf_ops *bops);
+
 void hnae_put_handle(struct hnae_handle *handle);
 int hnae_ae_register(struct hnae_ae_dev *dev, struct module *owner);
 void hnae_ae_unregister(struct hnae_ae_dev *dev);
@@ -551,11 +601,9 @@ static inline void hnae_replace_buffer(struct hnae_ring *ring, int i,
 				       struct hnae_desc_cb *res_cb)
 {
 	struct hnae_buf_ops *bops = ring->q->handle->bops;
-	struct hnae_desc_cb tmp_cb = ring->desc_cb[i];
 
 	bops->unmap_buffer(ring, &ring->desc_cb[i]);
 	ring->desc_cb[i] = *res_cb;
-	*res_cb = tmp_cb;
 	ring->desc[i].addr = (__le64)ring->desc_cb[i].dma;
 	ring->desc[i].rx.ipoff_bnum_pid_flag = 0;
 }
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c b/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
index 1a16c0307b47..d4f92ed322d6 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_ae_adapt.c
@@ -252,7 +252,7 @@ static int hns_ae_set_multicast_one(struct hnae_handle *handle, void *addr)
 	if (mac_cb->mac_type != HNAE_PORT_SERVICE)
 		return 0;
 
-	ret = hns_mac_set_multi(mac_cb, mac_cb->mac_id, mac_addr, ENABLE);
+	ret = hns_mac_set_multi(mac_cb, mac_cb->mac_id, mac_addr, true);
 	if (ret) {
 		dev_err(handle->owner_dev,
 			"mac add mul_mac:%pM port%d  fail, ret = %#x!\n",
@@ -261,7 +261,7 @@ static int hns_ae_set_multicast_one(struct hnae_handle *handle, void *addr)
 	}
 
 	ret = hns_mac_set_multi(mac_cb, DSAF_BASE_INNER_PORT_NUM,
-				mac_addr, ENABLE);
+				mac_addr, true);
 	if (ret)
 		dev_err(handle->owner_dev,
 			"mac add mul_mac:%pM port%d  fail, ret = %#x!\n",
@@ -277,12 +277,19 @@ static int hns_ae_set_mtu(struct hnae_handle *handle, int new_mtu)
 	return hns_mac_set_mtu(mac_cb, new_mtu);
 }
 
+static void hns_ae_set_tso_stats(struct hnae_handle *handle, int enable)
+{
+	struct hns_ppe_cb *ppe_cb = hns_get_ppe_cb(handle);
+
+	hns_ppe_set_tso_enable(ppe_cb, enable);
+}
+
 static int hns_ae_start(struct hnae_handle *handle)
 {
 	int ret;
 	struct hns_mac_cb *mac_cb = hns_get_mac_cb(handle);
 
-	ret = hns_mac_vm_config_bc_en(mac_cb, 0, ENABLE);
+	ret = hns_mac_vm_config_bc_en(mac_cb, 0, true);
 	if (ret)
 		return ret;
 
@@ -309,7 +316,7 @@ void hns_ae_stop(struct hnae_handle *handle)
 
 	hns_ae_ring_enable_all(handle, 0);
 
-	(void)hns_mac_vm_config_bc_en(mac_cb, 0, DISABLE);
+	(void)hns_mac_vm_config_bc_en(mac_cb, 0, false);
 }
 
 static void hns_ae_reset(struct hnae_handle *handle)
@@ -334,12 +341,30 @@ void hns_ae_toggle_ring_irq(struct hnae_ring *ring, u32 mask)
 	else
 		flag = RCB_INT_FLAG_RX;
 
-	hns_rcb_int_clr_hw(ring->q, flag);
 	hns_rcb_int_ctrl_hw(ring->q, flag, mask);
 }
 
+static void hns_aev2_toggle_ring_irq(struct hnae_ring *ring, u32 mask)
+{
+	u32 flag;
+
+	if (is_tx_ring(ring))
+		flag = RCB_INT_FLAG_TX;
+	else
+		flag = RCB_INT_FLAG_RX;
+
+	hns_rcbv2_int_ctrl_hw(ring->q, flag, mask);
+}
+
 static void hns_ae_toggle_queue_status(struct hnae_queue *queue, u32 val)
 {
+	struct dsaf_device *dsaf_dev = hns_ae_get_dsaf_dev(queue->dev);
+
+	if (AE_IS_VER1(dsaf_dev->dsaf_ver))
+		hns_rcb_int_clr_hw(queue, RCB_INT_FLAG_TX | RCB_INT_FLAG_RX);
+	else
+		hns_rcbv2_int_clr_hw(queue, RCB_INT_FLAG_TX | RCB_INT_FLAG_RX);
+
 	hns_rcb_start(queue, val);
 }
 
@@ -650,8 +675,12 @@ static int hns_ae_config_loopback(struct hnae_handle *handle,
 {
 	int ret;
 	struct hnae_vf_cb *vf_cb = hns_ae_get_vf_cb(handle);
+	struct hns_mac_cb *mac_cb = hns_get_mac_cb(handle);
 
 	switch (loop) {
+	case MAC_INTERNALLOOP_PHY:
+		ret = 0;
+		break;
 	case MAC_INTERNALLOOP_SERDES:
 		ret = hns_mac_config_sds_loopback(vf_cb->mac_cb, en);
 		break;
@@ -661,6 +690,10 @@ static int hns_ae_config_loopback(struct hnae_handle *handle,
 	default:
 		ret = -EINVAL;
 	}
+
+	if (!ret)
+		hns_dsaf_set_inner_lb(mac_cb->dsaf_dev, mac_cb->mac_id, en);
+
 	return ret;
 }
 
@@ -730,6 +763,53 @@ int hns_ae_get_regs_len(struct hnae_handle *handle)
 	return total_num;
 }
 
+static u32 hns_ae_get_rss_key_size(struct hnae_handle *handle)
+{
+	return HNS_PPEV2_RSS_KEY_SIZE;
+}
+
+static u32 hns_ae_get_rss_indir_size(struct hnae_handle *handle)
+{
+	return HNS_PPEV2_RSS_IND_TBL_SIZE;
+}
+
+static int hns_ae_get_rss(struct hnae_handle *handle, u32 *indir, u8 *key,
+			  u8 *hfunc)
+{
+	struct hns_ppe_cb *ppe_cb = hns_get_ppe_cb(handle);
+
+	/* currently we support only one type of hash function i.e. Toep hash */
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	/* get the RSS Key required by the user */
+	if (key)
+		memcpy(key, ppe_cb->rss_key, HNS_PPEV2_RSS_KEY_SIZE);
+
+	/* update the current hash->queue mappings from the shadow RSS table */
+	memcpy(indir, ppe_cb->rss_indir_table, HNS_PPEV2_RSS_IND_TBL_SIZE);
+
+	return 0;
+}
+
+static int hns_ae_set_rss(struct hnae_handle *handle, const u32 *indir,
+			  const u8 *key, const u8 hfunc)
+{
+	struct hns_ppe_cb *ppe_cb = hns_get_ppe_cb(handle);
+
+	/* set the RSS Hash Key if specififed by the user */
+	if (key)
+		hns_ppe_set_rss_key(ppe_cb, (int *)key);
+
+	/* update the shadow RSS table with user specified qids */
+	memcpy(ppe_cb->rss_indir_table, indir, HNS_PPEV2_RSS_IND_TBL_SIZE);
+
+	/* now update the hardware */
+	hns_ppe_set_indir_table(ppe_cb, ppe_cb->rss_indir_table);
+
+	return 0;
+}
+
 static struct hnae_ae_ops hns_dsaf_ops = {
 	.get_handle = hns_ae_get_handle,
 	.put_handle = hns_ae_put_handle,
@@ -758,19 +838,38 @@ static struct hnae_ae_ops hns_dsaf_ops = {
 	.set_mc_addr = hns_ae_set_multicast_one,
 	.set_mtu = hns_ae_set_mtu,
 	.update_stats = hns_ae_update_stats,
+	.set_tso_stats = hns_ae_set_tso_stats,
 	.get_stats = hns_ae_get_stats,
 	.get_strings = hns_ae_get_strings,
 	.get_sset_count = hns_ae_get_sset_count,
 	.update_led_status = hns_ae_update_led_status,
 	.set_led_id = hns_ae_cpld_set_led_id,
 	.get_regs = hns_ae_get_regs,
-	.get_regs_len = hns_ae_get_regs_len
+	.get_regs_len = hns_ae_get_regs_len,
+	.get_rss_key_size = hns_ae_get_rss_key_size,
+	.get_rss_indir_size = hns_ae_get_rss_indir_size,
+	.get_rss = hns_ae_get_rss,
+	.set_rss = hns_ae_set_rss
 };
 
 int hns_dsaf_ae_init(struct dsaf_device *dsaf_dev)
 {
 	struct hnae_ae_dev *ae_dev = &dsaf_dev->ae_dev;
+	static atomic_t id = ATOMIC_INIT(-1);
+
+	switch (dsaf_dev->dsaf_ver) {
+	case AE_VERSION_1:
+		hns_dsaf_ops.toggle_ring_irq = hns_ae_toggle_ring_irq;
+		break;
+	case AE_VERSION_2:
+		hns_dsaf_ops.toggle_ring_irq = hns_aev2_toggle_ring_irq;
+		break;
+	default:
+		break;
+	}
 
+	snprintf(ae_dev->name, AE_NAME_SIZE, "%s%d", DSAF_DEVICE_NAME,
+		 (int)atomic_inc_return(&id));
 	ae_dev->ops = &hns_dsaf_ops;
 	ae_dev->dev = dsaf_dev->dev;
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
index 026b38676cba..5ef0e96e918a 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.c
@@ -283,7 +283,7 @@ int hns_mac_change_vf_addr(struct hns_mac_cb *mac_cb,
 }
 
 int hns_mac_set_multi(struct hns_mac_cb *mac_cb,
-		      u32 port_num, char *addr, u8 en)
+		      u32 port_num, char *addr, bool enable)
 {
 	int ret;
 	struct dsaf_device *dsaf_dev = mac_cb->dsaf_dev;
@@ -295,7 +295,7 @@ int hns_mac_set_multi(struct hns_mac_cb *mac_cb,
 		mac_entry.in_port_num = mac_cb->mac_id;
 		mac_entry.port_num = port_num;
 
-		if (en == DISABLE)
+		if (!enable)
 			ret = hns_dsaf_del_mac_mc_port(dsaf_dev, &mac_entry);
 		else
 			ret = hns_dsaf_add_mac_mc_port(dsaf_dev, &mac_entry);
@@ -368,7 +368,7 @@ static void hns_mac_param_get(struct mac_params *param,
  *retuen 0 - success , negative --fail
  */
 static int hns_mac_port_config_bc_en(struct hns_mac_cb *mac_cb,
-				     u32 port_num, u16 vlan_id, u8 en)
+				     u32 port_num, u16 vlan_id, bool enable)
 {
 	int ret;
 	struct dsaf_device *dsaf_dev = mac_cb->dsaf_dev;
@@ -386,7 +386,7 @@ static int hns_mac_port_config_bc_en(struct hns_mac_cb *mac_cb,
 		mac_entry.in_port_num = mac_cb->mac_id;
 		mac_entry.port_num = port_num;
 
-		if (en == DISABLE)
+		if (!enable)
 			ret = hns_dsaf_del_mac_mc_port(dsaf_dev, &mac_entry);
 		else
 			ret = hns_dsaf_add_mac_mc_port(dsaf_dev, &mac_entry);
@@ -403,7 +403,7 @@ static int hns_mac_port_config_bc_en(struct hns_mac_cb *mac_cb,
  *@en:enable
  *retuen 0 - success , negative --fail
  */
-int hns_mac_vm_config_bc_en(struct hns_mac_cb *mac_cb, u32 vmid, u8 en)
+int hns_mac_vm_config_bc_en(struct hns_mac_cb *mac_cb, u32 vmid, bool enable)
 {
 	int ret;
 	struct dsaf_device *dsaf_dev = mac_cb->dsaf_dev;
@@ -427,7 +427,7 @@ int hns_mac_vm_config_bc_en(struct hns_mac_cb *mac_cb, u32 vmid, u8 en)
 			return ret;
 		mac_entry.port_num = port_num;
 
-		if (en == DISABLE)
+		if (!enable)
 			ret = hns_dsaf_del_mac_mc_port(dsaf_dev, &mac_entry);
 		else
 			ret = hns_dsaf_add_mac_mc_port(dsaf_dev, &mac_entry);
@@ -648,7 +648,7 @@ static int hns_mac_init_ex(struct hns_mac_cb *mac_cb)
 
 	hns_mac_adjust_link(mac_cb, mac_cb->speed, !mac_cb->half_duplex);
 
-	ret = hns_mac_port_config_bc_en(mac_cb, mac_cb->mac_id, 0, ENABLE);
+	ret = hns_mac_port_config_bc_en(mac_cb, mac_cb->mac_id, 0, true);
 	if (ret)
 		goto free_mac_drv;
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
index 7da95a7581f9..0b052191d751 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_mac.h
@@ -425,8 +425,8 @@ void mac_adjust_link(struct net_device *net_dev);
 void hns_mac_get_link_status(struct hns_mac_cb *mac_cb,	u32 *link_status);
 int hns_mac_change_vf_addr(struct hns_mac_cb *mac_cb, u32 vmid, char *addr);
 int hns_mac_set_multi(struct hns_mac_cb *mac_cb,
-		      u32 port_num, char *addr, u8 en);
-int hns_mac_vm_config_bc_en(struct hns_mac_cb *mac_cb, u32 vm, u8 en);
+		      u32 port_num, char *addr, bool enable);
+int hns_mac_vm_config_bc_en(struct hns_mac_cb *mac_cb, u32 vm, bool enable);
 void hns_mac_start(struct hns_mac_cb *mac_cb);
 void hns_mac_stop(struct hns_mac_cb *mac_cb);
 int hns_mac_del_mac(struct hns_mac_cb *mac_cb, u32 vfn, char *mac);
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
index b674414a4d72..38fc5be3870c 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.c
@@ -35,21 +35,13 @@ int hns_dsaf_get_cfg(struct dsaf_device *dsaf_dev)
 	int ret, i;
 	u32 desc_num;
 	u32 buf_size;
-	const char *name, *mode_str;
+	const char *mode_str;
 	struct device_node *np = dsaf_dev->dev->of_node;
 
-	if (of_device_is_compatible(np, "hisilicon,hns-dsaf-v2"))
-		dsaf_dev->dsaf_ver = AE_VERSION_2;
-	else
+	if (of_device_is_compatible(np, "hisilicon,hns-dsaf-v1"))
 		dsaf_dev->dsaf_ver = AE_VERSION_1;
-
-	ret = of_property_read_string(np, "dsa_name", &name);
-	if (ret) {
-		dev_err(dsaf_dev->dev, "get dsaf name fail, ret=%d!\n", ret);
-		return ret;
-	}
-	strncpy(dsaf_dev->ae_dev.name, name, AE_NAME_SIZE);
-	dsaf_dev->ae_dev.name[AE_NAME_SIZE - 1] = '\0';
+	else
+		dsaf_dev->dsaf_ver = AE_VERSION_2;
 
 	ret = of_property_read_string(np, "mode", &mode_str);
 	if (ret) {
@@ -238,6 +230,30 @@ static void hns_dsaf_mix_def_qid_cfg(struct dsaf_device *dsaf_dev)
 	}
 }
 
+static void hns_dsaf_inner_qid_cfg(struct dsaf_device *dsaf_dev)
+{
+	u16 max_q_per_vf, max_vfn;
+	u32 q_id, q_num_per_port;
+	u32 mac_id;
+
+	if (AE_IS_VER1(dsaf_dev->dsaf_ver))
+		return;
+
+	hns_rcb_get_queue_mode(dsaf_dev->dsaf_mode,
+			       HNS_DSAF_COMM_SERVICE_NW_IDX,
+			       &max_vfn, &max_q_per_vf);
+	q_num_per_port = max_vfn * max_q_per_vf;
+
+	for (mac_id = 0, q_id = 0; mac_id < DSAF_SERVICE_NW_NUM; mac_id++) {
+		dsaf_set_dev_field(dsaf_dev,
+				   DSAFV2_SERDES_LBK_0_REG + 4 * mac_id,
+				   DSAFV2_SERDES_LBK_QID_M,
+				   DSAFV2_SERDES_LBK_QID_S,
+				   q_id);
+		q_id += q_num_per_port;
+	}
+}
+
 /**
  * hns_dsaf_sw_port_type_cfg - cfg sw type
  * @dsaf_id: dsa fabric id
@@ -274,6 +290,8 @@ static void hns_dsaf_stp_port_type_cfg(struct dsaf_device *dsaf_dev,
 	}
 }
 
+#define HNS_DSAF_SBM_NUM(dev) \
+	(AE_IS_VER1((dev)->dsaf_ver) ? DSAF_SBM_NUM : DSAFV2_SBM_NUM)
 /**
  * hns_dsaf_sbm_cfg - config sbm
  * @dsaf_id: dsa fabric id
@@ -283,7 +301,7 @@ static void hns_dsaf_sbm_cfg(struct dsaf_device *dsaf_dev)
 	u32 o_sbm_cfg;
 	u32 i;
 
-	for (i = 0; i < DSAF_SBM_NUM; i++) {
+	for (i = 0; i < HNS_DSAF_SBM_NUM(dsaf_dev); i++) {
 		o_sbm_cfg = dsaf_read_dev(dsaf_dev,
 					  DSAF_SBM_CFG_REG_0_REG + 0x80 * i);
 		dsaf_set_bit(o_sbm_cfg, DSAF_SBM_CFG_EN_S, 1);
@@ -304,13 +322,19 @@ static int hns_dsaf_sbm_cfg_mib_en(struct dsaf_device *dsaf_dev)
 	u32 reg;
 	u32 read_cnt;
 
-	for (i = 0; i < DSAF_SBM_NUM; i++) {
+	/* validate configure by setting SBM_CFG_MIB_EN bit from 0 to 1. */
+	for (i = 0; i < HNS_DSAF_SBM_NUM(dsaf_dev); i++) {
+		reg = DSAF_SBM_CFG_REG_0_REG + 0x80 * i;
+		dsaf_set_dev_bit(dsaf_dev, reg, DSAF_SBM_CFG_MIB_EN_S, 0);
+	}
+
+	for (i = 0; i < HNS_DSAF_SBM_NUM(dsaf_dev); i++) {
 		reg = DSAF_SBM_CFG_REG_0_REG + 0x80 * i;
 		dsaf_set_dev_bit(dsaf_dev, reg, DSAF_SBM_CFG_MIB_EN_S, 1);
 	}
 
 	/* waitint for all sbm enable finished */
-	for (i = 0; i < DSAF_SBM_NUM; i++) {
+	for (i = 0; i < HNS_DSAF_SBM_NUM(dsaf_dev); i++) {
 		read_cnt = 0;
 		reg = DSAF_SBM_CFG_REG_0_REG + 0x80 * i;
 		do {
@@ -338,83 +362,156 @@ static int hns_dsaf_sbm_cfg_mib_en(struct dsaf_device *dsaf_dev)
  */
 static void hns_dsaf_sbm_bp_wl_cfg(struct dsaf_device *dsaf_dev)
 {
-	u32 o_sbm_bp_cfg0;
-	u32 o_sbm_bp_cfg1;
-	u32 o_sbm_bp_cfg2;
-	u32 o_sbm_bp_cfg3;
+	u32 o_sbm_bp_cfg;
 	u32 reg;
 	u32 i;
 
 	/* XGE */
 	for (i = 0; i < DSAF_XGE_NUM; i++) {
 		reg = DSAF_SBM_BP_CFG_0_XGE_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg0 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg0, DSAF_SBM_CFG0_COM_MAX_BUF_NUM_M,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG0_COM_MAX_BUF_NUM_M,
 			       DSAF_SBM_CFG0_COM_MAX_BUF_NUM_S, 512);
-		dsaf_set_field(o_sbm_bp_cfg0, DSAF_SBM_CFG0_VC0_MAX_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG0_VC0_MAX_BUF_NUM_M,
 			       DSAF_SBM_CFG0_VC0_MAX_BUF_NUM_S, 0);
-		dsaf_set_field(o_sbm_bp_cfg0, DSAF_SBM_CFG0_VC1_MAX_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG0_VC1_MAX_BUF_NUM_M,
 			       DSAF_SBM_CFG0_VC1_MAX_BUF_NUM_S, 0);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg0);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 
 		reg = DSAF_SBM_BP_CFG_1_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg1 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg1, DSAF_SBM_CFG1_TC4_MAX_BUF_NUM_M,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG1_TC4_MAX_BUF_NUM_M,
 			       DSAF_SBM_CFG1_TC4_MAX_BUF_NUM_S, 0);
-		dsaf_set_field(o_sbm_bp_cfg1, DSAF_SBM_CFG1_TC0_MAX_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG1_TC0_MAX_BUF_NUM_M,
 			       DSAF_SBM_CFG1_TC0_MAX_BUF_NUM_S, 0);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg1);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 
 		reg = DSAF_SBM_BP_CFG_2_XGE_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg2 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_SET_BUF_NUM_M,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_SET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_SET_BUF_NUM_S, 104);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_RESET_BUF_NUM_S, 128);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg2);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 
 		reg = DSAF_SBM_BP_CFG_3_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg3 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg3,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg,
 			       DSAF_SBM_CFG3_SET_BUF_NUM_NO_PFC_M,
 			       DSAF_SBM_CFG3_SET_BUF_NUM_NO_PFC_S, 110);
-		dsaf_set_field(o_sbm_bp_cfg3,
+		dsaf_set_field(o_sbm_bp_cfg,
 			       DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_M,
 			       DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_S, 160);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg3);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 
 		/* for no enable pfc mode */
 		reg = DSAF_SBM_BP_CFG_4_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg3 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg3,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg,
 			       DSAF_SBM_CFG3_SET_BUF_NUM_NO_PFC_M,
 			       DSAF_SBM_CFG3_SET_BUF_NUM_NO_PFC_S, 128);
-		dsaf_set_field(o_sbm_bp_cfg3,
+		dsaf_set_field(o_sbm_bp_cfg,
 			       DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_M,
 			       DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_S, 192);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg3);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 	}
 
 	/* PPE */
 	for (i = 0; i < DSAF_COMM_CHN; i++) {
 		reg = DSAF_SBM_BP_CFG_2_PPE_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg2 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_SET_BUF_NUM_M,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_SET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_SET_BUF_NUM_S, 10);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_RESET_BUF_NUM_S, 12);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg2);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 	}
 
 	/* RoCEE */
 	for (i = 0; i < DSAF_COMM_CHN; i++) {
 		reg = DSAF_SBM_BP_CFG_2_ROCEE_REG_0_REG + 0x80 * i;
-		o_sbm_bp_cfg2 = dsaf_read_dev(dsaf_dev, reg);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_SET_BUF_NUM_M,
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_SET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_SET_BUF_NUM_S, 2);
-		dsaf_set_field(o_sbm_bp_cfg2, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
+		dsaf_set_field(o_sbm_bp_cfg, DSAF_SBM_CFG2_RESET_BUF_NUM_M,
 			       DSAF_SBM_CFG2_RESET_BUF_NUM_S, 4);
-		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg2);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+	}
+}
+
+static void hns_dsafv2_sbm_bp_wl_cfg(struct dsaf_device *dsaf_dev)
+{
+	u32 o_sbm_bp_cfg;
+	u32 reg;
+	u32 i;
+
+	/* XGE */
+	for (i = 0; i < DSAFV2_SBM_XGE_CHN; i++) {
+		reg = DSAF_SBM_BP_CFG_0_XGE_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG0_COM_MAX_BUF_NUM_M,
+			       DSAFV2_SBM_CFG0_COM_MAX_BUF_NUM_S, 256);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG0_VC0_MAX_BUF_NUM_M,
+			       DSAFV2_SBM_CFG0_VC0_MAX_BUF_NUM_S, 0);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG0_VC1_MAX_BUF_NUM_M,
+			       DSAFV2_SBM_CFG0_VC1_MAX_BUF_NUM_S, 0);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+
+		reg = DSAF_SBM_BP_CFG_1_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG1_TC4_MAX_BUF_NUM_M,
+			       DSAFV2_SBM_CFG1_TC4_MAX_BUF_NUM_S, 0);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG1_TC0_MAX_BUF_NUM_M,
+			       DSAFV2_SBM_CFG1_TC0_MAX_BUF_NUM_S, 0);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+
+		reg = DSAF_SBM_BP_CFG_2_XGE_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_SET_BUF_NUM_M,
+			       DSAFV2_SBM_CFG2_SET_BUF_NUM_S, 104);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_RESET_BUF_NUM_M,
+			       DSAFV2_SBM_CFG2_RESET_BUF_NUM_S, 128);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+
+		reg = DSAF_SBM_BP_CFG_3_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg,
+			       DSAFV2_SBM_CFG3_SET_BUF_NUM_NO_PFC_M,
+			       DSAFV2_SBM_CFG3_SET_BUF_NUM_NO_PFC_S, 110);
+		dsaf_set_field(o_sbm_bp_cfg,
+			       DSAFV2_SBM_CFG3_RESET_BUF_NUM_NO_PFC_M,
+			       DSAFV2_SBM_CFG3_RESET_BUF_NUM_NO_PFC_S, 160);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+
+		/* for no enable pfc mode */
+		reg = DSAF_SBM_BP_CFG_4_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg,
+			       DSAFV2_SBM_CFG4_SET_BUF_NUM_NO_PFC_M,
+			       DSAFV2_SBM_CFG4_SET_BUF_NUM_NO_PFC_S, 128);
+		dsaf_set_field(o_sbm_bp_cfg,
+			       DSAFV2_SBM_CFG4_RESET_BUF_NUM_NO_PFC_M,
+			       DSAFV2_SBM_CFG4_RESET_BUF_NUM_NO_PFC_S, 192);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+	}
+
+	/* PPE */
+	reg = DSAF_SBM_BP_CFG_2_PPE_REG_0_REG + 0x80 * i;
+	o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+	dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_SET_BUF_NUM_M,
+		       DSAFV2_SBM_CFG2_SET_BUF_NUM_S, 10);
+	dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_RESET_BUF_NUM_M,
+		       DSAFV2_SBM_CFG2_RESET_BUF_NUM_S, 12);
+	dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
+	/* RoCEE */
+	for (i = 0; i < DASFV2_ROCEE_CRD_NUM; i++) {
+		reg = DSAFV2_SBM_BP_CFG_2_ROCEE_REG_0_REG + 0x80 * i;
+		o_sbm_bp_cfg = dsaf_read_dev(dsaf_dev, reg);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_SET_BUF_NUM_M,
+			       DSAFV2_SBM_CFG2_SET_BUF_NUM_S, 2);
+		dsaf_set_field(o_sbm_bp_cfg, DSAFV2_SBM_CFG2_RESET_BUF_NUM_M,
+			       DSAFV2_SBM_CFG2_RESET_BUF_NUM_S, 4);
+		dsaf_write_dev(dsaf_dev, reg, o_sbm_bp_cfg);
 	}
 }
 
@@ -618,6 +715,16 @@ void hns_dsaf_set_promisc_mode(struct dsaf_device *dsaf_dev, u32 en)
 	dsaf_set_dev_bit(dsaf_dev, DSAF_CFG_0_REG, 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
@@ -949,6 +1056,9 @@ static void hns_dsaf_comm_init(struct dsaf_device *dsaf_dev)
 	/* set promisc def queue id */
 	hns_dsaf_mix_def_qid_cfg(dsaf_dev);
 
+	/* set inner loopback queue id */
+	hns_dsaf_inner_qid_cfg(dsaf_dev);
+
 	/* in non switch mode, set all port to access mode */
 	hns_dsaf_sw_port_type_cfg(dsaf_dev, DSAF_SW_PORT_TYPE_NON_VLAN);
 
@@ -985,11 +1095,38 @@ static void hns_dsaf_inode_init(struct dsaf_device *dsaf_dev)
 	else
 		tc_cfg = HNS_DSAF_I8TC_CFG;
 
+	if (AE_IS_VER1(dsaf_dev->dsaf_ver)) {
+		for (i = 0; i < DSAF_INODE_NUM; i++) {
+			reg = DSAF_INODE_IN_PORT_NUM_0_REG + 0x80 * i;
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAF_INODE_IN_PORT_NUM_M,
+					   DSAF_INODE_IN_PORT_NUM_S,
+					   i % DSAF_XGE_NUM);
+		}
+	} else {
+		for (i = 0; i < DSAF_PORT_TYPE_NUM; i++) {
+			reg = DSAF_INODE_IN_PORT_NUM_0_REG + 0x80 * i;
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAF_INODE_IN_PORT_NUM_M,
+					   DSAF_INODE_IN_PORT_NUM_S, 0);
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAFV2_INODE_IN_PORT1_NUM_M,
+					   DSAFV2_INODE_IN_PORT1_NUM_S, 1);
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAFV2_INODE_IN_PORT2_NUM_M,
+					   DSAFV2_INODE_IN_PORT2_NUM_S, 2);
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAFV2_INODE_IN_PORT3_NUM_M,
+					   DSAFV2_INODE_IN_PORT3_NUM_S, 3);
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAFV2_INODE_IN_PORT4_NUM_M,
+					   DSAFV2_INODE_IN_PORT4_NUM_S, 4);
+			dsaf_set_dev_field(dsaf_dev, reg,
+					   DSAFV2_INODE_IN_PORT5_NUM_M,
+					   DSAFV2_INODE_IN_PORT5_NUM_S, 5);
+		}
+	}
 	for (i = 0; i < DSAF_INODE_NUM; i++) {
-		reg = DSAF_INODE_IN_PORT_NUM_0_REG + 0x80 * i;
-		dsaf_set_dev_field(dsaf_dev, reg, DSAF_INODE_IN_PORT_NUM_M,
-				   DSAF_INODE_IN_PORT_NUM_S, i % DSAF_XGE_NUM);
-
 		reg = DSAF_INODE_PRI_TC_CFG_0_REG + 0x80 * i;
 		dsaf_write_dev(dsaf_dev, reg, tc_cfg);
 	}
@@ -1002,10 +1139,17 @@ static void hns_dsaf_inode_init(struct dsaf_device *dsaf_dev)
 static int hns_dsaf_sbm_init(struct dsaf_device *dsaf_dev)
 {
 	u32 flag;
+	u32 finish_msk;
 	u32 cnt = 0;
 	int ret;
 
-	hns_dsaf_sbm_bp_wl_cfg(dsaf_dev);
+	if (AE_IS_VER1(dsaf_dev->dsaf_ver)) {
+		hns_dsaf_sbm_bp_wl_cfg(dsaf_dev);
+		finish_msk = DSAF_SRAM_INIT_OVER_M;
+	} else {
+		hns_dsafv2_sbm_bp_wl_cfg(dsaf_dev);
+		finish_msk = DSAFV2_SRAM_INIT_OVER_M;
+	}
 
 	/* enable sbm chanel, disable sbm chanel shcut function*/
 	hns_dsaf_sbm_cfg(dsaf_dev);
@@ -1024,11 +1168,13 @@ static int hns_dsaf_sbm_init(struct dsaf_device *dsaf_dev)
 
 	do {
 		usleep_range(200, 210);/*udelay(200);*/
-		flag = dsaf_read_dev(dsaf_dev, DSAF_SRAM_INIT_OVER_0_REG);
+		flag = dsaf_get_dev_field(dsaf_dev, DSAF_SRAM_INIT_OVER_0_REG,
+					  finish_msk, DSAF_SRAM_INIT_OVER_S);
 		cnt++;
-	} while (flag != DSAF_SRAM_INIT_FINISH_FLAG && cnt < DSAF_CFG_READ_CNT);
+	} while (flag != (finish_msk >> DSAF_SRAM_INIT_OVER_S) &&
+		 cnt < DSAF_CFG_READ_CNT);
 
-	if (flag != DSAF_SRAM_INIT_FINISH_FLAG) {
+	if (flag != (finish_msk >> DSAF_SRAM_INIT_OVER_S)) {
 		dev_err(dsaf_dev->dev,
 			"hns_dsaf_sbm_init fail %s, flag=%d, cnt=%d\n",
 			dsaf_dev->ae_dev.name, flag, cnt);
@@ -2011,7 +2157,7 @@ void hns_dsaf_get_regs(struct dsaf_device *ddev, u32 port, void *data)
 		DSAF_INODE_VC1_IN_PKT_NUM_0_REG + port * 4);
 
 	/* dsaf inode registers */
-	for (i = 0; i < DSAF_SBM_NUM / DSAF_COMM_CHN; i++) {
+	for (i = 0; i < HNS_DSAF_SBM_NUM(ddev) / DSAF_COMM_CHN; i++) {
 		j = i * DSAF_COMM_CHN + port;
 		p[232 + i] = dsaf_read_dev(ddev,
 				DSAF_SBM_CFG_REG_0_REG + j * 0x80);
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
index b2b93484995c..5fea226efaf3 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_main.h
@@ -18,25 +18,22 @@ struct hns_mac_cb;
 
 #define DSAF_DRV_NAME "hns_dsaf"
 #define DSAF_MOD_VERSION "v1.0"
+#define DSAF_DEVICE_NAME "dsaf"
 
-#define ENABLE		(0x1)
-#define DISABLE		(0x0)
+#define HNS_DSAF_DEBUG_NW_REG_OFFSET 0x100000
 
-#define HNS_DSAF_DEBUG_NW_REG_OFFSET (0x100000)
+#define DSAF_BASE_INNER_PORT_NUM 127/* mac tbl qid*/
 
-#define DSAF_BASE_INNER_PORT_NUM (127)  /* mac tbl qid*/
+#define DSAF_MAX_CHIP_NUM 2  /*max 2 chips */
 
-#define DSAF_MAX_CHIP_NUM (2)  /*max 2 chips */
+#define DSAF_DEFAUTL_QUEUE_NUM_PER_PPE 22
 
-#define DSAF_DEFAUTL_QUEUE_NUM_PER_PPE (22)
+#define HNS_DSAF_MAX_DESC_CNT 1024
+#define HNS_DSAF_MIN_DESC_CNT 16
 
-#define HNS_DSAF_MAX_DESC_CNT (1024)
-#define HNS_DSAF_MIN_DESC_CNT (16)
+#define DSAF_INVALID_ENTRY_IDX 0xffff
 
-#define DSAF_INVALID_ENTRY_IDX (0xffff)
-
-#define DSAF_CFG_READ_CNT   (30)
-#define DSAF_SRAM_INIT_FINISH_FLAG (0xff)
+#define DSAF_CFG_READ_CNT   30
 
 #define MAC_NUM_OCTETS_PER_ADDR 6
 
@@ -274,10 +271,6 @@ struct dsaf_device {
 	struct device *dev;
 	struct hnae_ae_dev ae_dev;
 
-	void *priv;
-
-	int virq[DSAF_IRQ_NUM];
-
 	u8 __iomem *sc_base;
 	u8 __iomem *sds_base;
 	u8 __iomem *ppe_base;
@@ -424,5 +417,6 @@ void hns_dsaf_get_strings(int stringset, u8 *data, int port);
 void hns_dsaf_get_regs(struct dsaf_device *ddev, u32 port, void *data);
 int hns_dsaf_get_regs_count(void);
 void hns_dsaf_set_promisc_mode(struct dsaf_device *dsaf_dev, 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 523e9b83d304..607c3be42241 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_misc.c
@@ -149,7 +149,11 @@ void hns_dsaf_ge_srst_by_port(struct dsaf_device *dsaf_dev, u32 port, u32 val)
 
 	if (port < DSAF_SERVICE_NW_NUM) {
 		reg_val_1  = 0x1 << port;
-		reg_val_2  = 0x1041041 << port;
+		/* there is difference between V1 and V2 in register.*/
+		if (AE_IS_VER1(dsaf_dev->dsaf_ver))
+			reg_val_2  = 0x1041041 << port;
+		else
+			reg_val_2  = 0x2082082 << port;
 
 		if (val == 0) {
 			dsaf_write_reg(dsaf_dev->sc_base,
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.c
index 67f33f185a44..f302ef9073c6 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.c
@@ -19,6 +19,48 @@
 
 #include "hns_dsaf_ppe.h"
 
+void hns_ppe_set_tso_enable(struct hns_ppe_cb *ppe_cb, u32 value)
+{
+	dsaf_set_dev_bit(ppe_cb, PPEV2_CFG_TSO_EN_REG, 0, !!value);
+}
+
+void hns_ppe_set_rss_key(struct hns_ppe_cb *ppe_cb,
+			 const u32 rss_key[HNS_PPEV2_RSS_KEY_NUM])
+{
+	int key_item = 0;
+
+	for (key_item = 0; key_item < HNS_PPEV2_RSS_KEY_NUM; key_item++)
+		dsaf_write_dev(ppe_cb, PPEV2_RSS_KEY_REG + key_item * 0x4,
+			       rss_key[key_item]);
+}
+
+void hns_ppe_set_indir_table(struct hns_ppe_cb *ppe_cb,
+			     const u32 rss_tab[HNS_PPEV2_RSS_IND_TBL_SIZE])
+{
+	int i;
+	int reg_value;
+
+	for (i = 0; i < (HNS_PPEV2_RSS_IND_TBL_SIZE / 4); i++) {
+		reg_value = dsaf_read_dev(ppe_cb,
+					  PPEV2_INDRECTION_TBL_REG + i * 0x4);
+
+		dsaf_set_field(reg_value, PPEV2_CFG_RSS_TBL_4N0_M,
+			       PPEV2_CFG_RSS_TBL_4N0_S,
+			       rss_tab[i * 4 + 0] & 0x1F);
+		dsaf_set_field(reg_value, PPEV2_CFG_RSS_TBL_4N1_M,
+			       PPEV2_CFG_RSS_TBL_4N1_S,
+				rss_tab[i * 4 + 1] & 0x1F);
+		dsaf_set_field(reg_value, PPEV2_CFG_RSS_TBL_4N2_M,
+			       PPEV2_CFG_RSS_TBL_4N2_S,
+				rss_tab[i * 4 + 2] & 0x1F);
+		dsaf_set_field(reg_value, PPEV2_CFG_RSS_TBL_4N3_M,
+			       PPEV2_CFG_RSS_TBL_4N3_S,
+				rss_tab[i * 4 + 3] & 0x1F);
+		dsaf_write_dev(
+			ppe_cb, PPEV2_INDRECTION_TBL_REG + i * 0x4, reg_value);
+	}
+}
+
 static void __iomem *hns_ppe_common_get_ioaddr(
 	struct ppe_common_cb *ppe_common)
 {
@@ -134,6 +176,11 @@ static void hns_ppe_cnt_clr_ce(struct hns_ppe_cb *ppe_cb)
 			 PPE_CNT_CLR_CE_B, 1);
 }
 
+static void hns_ppe_set_vlan_strip(struct hns_ppe_cb *ppe_cb, int en)
+{
+	dsaf_write_dev(ppe_cb, PPEV2_VLAN_STRIP_EN_REG, en);
+}
+
 /**
  * hns_ppe_checksum_hw - set ppe checksum caculate
  * @ppe_device: ppe device
@@ -266,13 +313,17 @@ static void hns_ppe_exc_irq_en(struct hns_ppe_cb *ppe_cb, int en)
 
 /**
  * ppe_init_hw - init ppe
- * @ppe_device: ppe device
+ * @ppe_cb: ppe device
  */
 static void hns_ppe_init_hw(struct hns_ppe_cb *ppe_cb)
 {
 	struct ppe_common_cb *ppe_common_cb = ppe_cb->ppe_common_cb;
 	u32 port = ppe_cb->port;
 	struct dsaf_device *dsaf_dev = ppe_common_cb->dsaf_dev;
+	int i;
+
+	/* get default RSS key */
+	netdev_rss_key_fill(ppe_cb->rss_key, HNS_PPEV2_RSS_KEY_SIZE);
 
 	hns_ppe_srst_by_port(dsaf_dev, port, 0);
 	mdelay(10);
@@ -285,8 +336,21 @@ static void hns_ppe_init_hw(struct hns_ppe_cb *ppe_cb)
 		hns_ppe_set_port_mode(ppe_cb, PPE_MODE_GE);
 	else
 		hns_ppe_set_port_mode(ppe_cb, PPE_MODE_XGE);
+
 	hns_ppe_checksum_hw(ppe_cb, 0xffffffff);
 	hns_ppe_cnt_clr_ce(ppe_cb);
+
+	if (!AE_IS_VER1(dsaf_dev->dsaf_ver)) {
+		hns_ppe_set_vlan_strip(ppe_cb, 0);
+
+		/* set default RSS key in h/w */
+		hns_ppe_set_rss_key(ppe_cb, ppe_cb->rss_key);
+
+		/* Set default indrection table in h/w */
+		for (i = 0; i < HNS_PPEV2_RSS_IND_TBL_SIZE; i++)
+			ppe_cb->rss_indir_table[i] = i;
+		hns_ppe_set_indir_table(ppe_cb, ppe_cb->rss_indir_table);
+	}
 }
 
 /**
@@ -341,13 +405,13 @@ void hns_ppe_reset_common(struct dsaf_device *dsaf_dev, u8 ppe_common_index)
 	if (ret)
 		return;
 
+	for (i = 0; i < ppe_common->ppe_num; i++)
+		hns_ppe_init_hw(&ppe_common->ppe_cb[i]);
+
 	ret = hns_rcb_common_init_hw(dsaf_dev->rcb_common[ppe_common_index]);
 	if (ret)
 		return;
 
-	for (i = 0; i < ppe_common->ppe_num; i++)
-		hns_ppe_init_hw(&ppe_common->ppe_cb[i]);
-
 	hns_rcb_common_init_commit_hw(dsaf_dev->rcb_common[ppe_common_index]);
 }
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.h
index 4894f9a0d39f..0f5cb6962acf 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_ppe.h
@@ -25,15 +25,24 @@
 
 #define ETH_PPE_DUMP_NUM 576
 #define ETH_PPE_STATIC_NUM 12
+
+#define HNS_PPEV2_RSS_IND_TBL_SIZE 256
+#define HNS_PPEV2_RSS_KEY_SIZE 40 /* in bytes or 320 bits */
+#define HNS_PPEV2_RSS_KEY_NUM (HNS_PPEV2_RSS_KEY_SIZE / sizeof(u32))
+
 enum ppe_qid_mode {
-	PPE_QID_MODE0 = 0,	/* fixed queue id mode */
-	PPE_QID_MODE1,		/* switch:128VM non switch:6Port/4VM/4TC */
-	PPE_QID_MODE2,		/* switch:32VM/4TC non switch:6Port/16VM */
-	PPE_QID_MODE3,		/* switch:4TC/8TAG non switch:2Port/64VM */
-	PPE_QID_MODE4,		/* switch:8VM/16TAG non switch:2Port/16VM/4TC */
-	PPE_QID_MODE5,		/* non switch:6Port/16TAG */
-	PPE_QID_MODE6,		/* non switch:6Port/2VM/8TC */
-	PPE_QID_MODE7,		/* non switch:2Port/8VM/8TC */
+	PPE_QID_MODE0 = 0, /* fixed queue id mode */
+	PPE_QID_MODE1,	   /* switch:128VM non switch:6Port/4VM/4TC */
+	PPE_QID_MODE2,	   /* switch:32VM/4TC non switch:6Port/16VM */
+	PPE_QID_MODE3,	   /* switch:4TC/8RSS non switch:2Port/64VM */
+	PPE_QID_MODE4,	   /* switch:8VM/16RSS non switch:2Port/16VM/4TC */
+	PPE_QID_MODE5,	   /* switch:16VM/8TC non switch:6Port/16RSS */
+	PPE_QID_MODE6,	   /* switch:32VM/4RSS non switch:6Port/2VM/8TC */
+	PPE_QID_MODE7,	   /* switch:32RSS non switch:2Port/8VM/8TC */
+	PPE_QID_MODE8,	   /* switch:6VM/4TC/4RSS non switch:2Port/16VM/4RSS */
+	PPE_QID_MODE9,	   /* non switch:2Port/32VM/2RSS */
+	PPE_QID_MODE10,	   /* non switch:2Port/32RSS */
+	PPE_QID_MODE11,	   /* non switch:2Port/4TC/16RSS */
 };
 
 enum ppe_port_mode {
@@ -72,6 +81,8 @@ struct hns_ppe_cb {
 	u8 port;			 /* port id in dsaf  */
 	void __iomem *io_base;
 	int virq;
+	u32 rss_indir_table[HNS_PPEV2_RSS_IND_TBL_SIZE]; /*shadow indir tab */
+	u32 rss_key[HNS_PPEV2_RSS_KEY_NUM]; /* rss hash key */
 };
 
 struct ppe_common_cb {
@@ -102,4 +113,9 @@ void hns_ppe_get_regs(struct hns_ppe_cb *ppe_cb, void *data);
 
 void hns_ppe_get_strings(struct hns_ppe_cb *ppe_cb, int stringset, u8 *data);
 void hns_ppe_get_stats(struct hns_ppe_cb *ppe_cb, u64 *data);
+void hns_ppe_set_tso_enable(struct hns_ppe_cb *ppe_cb, u32 value);
+void hns_ppe_set_rss_key(struct hns_ppe_cb *ppe_cb,
+			 const u32 rss_key[HNS_PPEV2_RSS_KEY_NUM]);
+void hns_ppe_set_indir_table(struct hns_ppe_cb *ppe_cb,
+			     const u32 rss_tab[HNS_PPEV2_RSS_IND_TBL_SIZE]);
 #endif /* _HNS_DSAF_PPE_H */
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
index 4db32c62f062..12188807468c 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.c
@@ -136,19 +136,37 @@ void hns_rcb_int_ctrl_hw(struct hnae_queue *q, u32 flag, u32 mask)
 
 void hns_rcb_int_clr_hw(struct hnae_queue *q, u32 flag)
 {
-	u32 clr = 1;
-
 	if (flag & RCB_INT_FLAG_TX) {
-		dsaf_write_dev(q, RCB_RING_INTSTS_TX_RING_REG, clr);
-		dsaf_write_dev(q, RCB_RING_INTSTS_TX_OVERTIME_REG, clr);
+		dsaf_write_dev(q, RCB_RING_INTSTS_TX_RING_REG, 1);
+		dsaf_write_dev(q, RCB_RING_INTSTS_TX_OVERTIME_REG, 1);
 	}
 
 	if (flag & RCB_INT_FLAG_RX) {
-		dsaf_write_dev(q, RCB_RING_INTSTS_RX_RING_REG, clr);
-		dsaf_write_dev(q, RCB_RING_INTSTS_RX_OVERTIME_REG, clr);
+		dsaf_write_dev(q, RCB_RING_INTSTS_RX_RING_REG, 1);
+		dsaf_write_dev(q, RCB_RING_INTSTS_RX_OVERTIME_REG, 1);
 	}
 }
 
+void hns_rcbv2_int_ctrl_hw(struct hnae_queue *q, u32 flag, u32 mask)
+{
+	u32 int_mask_en = !!mask;
+
+	if (flag & RCB_INT_FLAG_TX)
+		dsaf_write_dev(q, RCB_RING_INTMSK_TXWL_REG, int_mask_en);
+
+	if (flag & RCB_INT_FLAG_RX)
+		dsaf_write_dev(q, RCB_RING_INTMSK_RXWL_REG, int_mask_en);
+}
+
+void hns_rcbv2_int_clr_hw(struct hnae_queue *q, u32 flag)
+{
+	if (flag & RCB_INT_FLAG_TX)
+		dsaf_write_dev(q, RCBV2_TX_RING_INT_STS_REG, 1);
+
+	if (flag & RCB_INT_FLAG_RX)
+		dsaf_write_dev(q, RCBV2_RX_RING_INT_STS_REG, 1);
+}
+
 /**
  *hns_rcb_ring_enable_hw - enable ring
  *@ring: rcb ring
@@ -193,6 +211,7 @@ static void hns_rcb_ring_init(struct ring_pair_cb *ring_pair, int ring_type)
 			       (u32)dma);
 		dsaf_write_dev(q, RCB_RING_RX_RING_BASEADDR_H_REG,
 			       (u32)((dma >> 31) >> 1));
+
 		dsaf_write_dev(q, RCB_RING_RX_RING_BD_LEN_REG,
 			       bd_size_type);
 		dsaf_write_dev(q, RCB_RING_RX_RING_BD_NUM_REG,
@@ -204,6 +223,7 @@ static void hns_rcb_ring_init(struct ring_pair_cb *ring_pair, int ring_type)
 			       (u32)dma);
 		dsaf_write_dev(q, RCB_RING_TX_RING_BASEADDR_H_REG,
 			       (u32)((dma >> 31) >> 1));
+
 		dsaf_write_dev(q, RCB_RING_TX_RING_BD_LEN_REG,
 			       bd_size_type);
 		dsaf_write_dev(q, RCB_RING_TX_RING_BD_NUM_REG,
@@ -232,9 +252,6 @@ void hns_rcb_init_hw(struct ring_pair_cb *ring)
 static void hns_rcb_set_port_desc_cnt(struct rcb_common_cb *rcb_common,
 				      u32 port_idx, u32 desc_cnt)
 {
-	if (port_idx >= HNS_RCB_SERVICE_NW_ENGINE_NUM)
-		port_idx = 0;
-
 	dsaf_write_dev(rcb_common, RCB_CFG_BD_NUM_REG + port_idx * 4,
 		       desc_cnt);
 }
@@ -249,8 +266,6 @@ static int  hns_rcb_set_port_coalesced_frames(struct rcb_common_cb *rcb_common,
 					      u32 port_idx,
 					      u32 coalesced_frames)
 {
-	if (port_idx >= HNS_RCB_SERVICE_NW_ENGINE_NUM)
-		port_idx = 0;
 	if (coalesced_frames >= rcb_common->desc_num ||
 	    coalesced_frames > HNS_RCB_MAX_COALESCED_FRAMES)
 		return -EINVAL;
@@ -354,6 +369,18 @@ int hns_rcb_common_init_hw(struct rcb_common_cb *rcb_common)
 	dsaf_write_dev(rcb_common, RCB_COM_CFG_ENDIAN_REG,
 		       HNS_RCB_COMMON_ENDIAN);
 
+	if (AE_IS_VER1(rcb_common->dsaf_dev->dsaf_ver)) {
+		dsaf_write_dev(rcb_common, RCB_COM_CFG_FNA_REG, 0x0);
+		dsaf_write_dev(rcb_common, RCB_COM_CFG_FA_REG, 0x1);
+	} else {
+		dsaf_set_dev_bit(rcb_common, RCBV2_COM_CFG_USER_REG,
+				 RCB_COM_CFG_FNA_B, false);
+		dsaf_set_dev_bit(rcb_common, RCBV2_COM_CFG_USER_REG,
+				 RCB_COM_CFG_FA_B, true);
+		dsaf_set_dev_bit(rcb_common, RCBV2_COM_CFG_TSO_MODE_REG,
+				 RCB_COM_TSO_MODE_B, HNS_TSO_MODE_8BD_32K);
+	}
+
 	return 0;
 }
 
@@ -387,19 +414,23 @@ static void hns_rcb_ring_get_cfg(struct hnae_queue *q, int ring_type)
 	struct rcb_common_cb *rcb_common;
 	struct ring_pair_cb *ring_pair_cb;
 	u32 buf_size;
-	u16 desc_num;
-	int irq_idx;
+	u16 desc_num, mdnum_ppkt;
+	bool irq_idx, is_ver1;
 
 	ring_pair_cb = container_of(q, struct ring_pair_cb, q);
+	is_ver1 = AE_IS_VER1(ring_pair_cb->rcb_common->dsaf_dev->dsaf_ver);
 	if (ring_type == RX_RING) {
 		ring = &q->rx_ring;
 		ring->io_base = ring_pair_cb->q.io_base;
 		irq_idx = HNS_RCB_IRQ_IDX_RX;
+		mdnum_ppkt = HNS_RCB_RING_MAX_BD_PER_PKT;
 	} else {
 		ring = &q->tx_ring;
 		ring->io_base = (u8 __iomem *)ring_pair_cb->q.io_base +
 			HNS_RCB_TX_REG_OFFSET;
 		irq_idx = HNS_RCB_IRQ_IDX_TX;
+		mdnum_ppkt = is_ver1 ? HNS_RCB_RING_MAX_TXBD_PER_PKT :
+				 HNS_RCBV2_RING_MAX_TXBD_PER_PKT;
 	}
 
 	rcb_common = ring_pair_cb->rcb_common;
@@ -414,7 +445,7 @@ static void hns_rcb_ring_get_cfg(struct hnae_queue *q, int ring_type)
 
 	ring->buf_size = buf_size;
 	ring->desc_num = desc_num;
-	ring->max_desc_num_per_pkt = HNS_RCB_RING_MAX_BD_PER_PKT;
+	ring->max_desc_num_per_pkt = mdnum_ppkt;
 	ring->max_raw_data_sz_per_desc = HNS_RCB_MAX_PKT_SIZE;
 	ring->max_pkt_size = HNS_RCB_MAX_PKT_SIZE;
 	ring->next_to_use = 0;
@@ -445,14 +476,22 @@ static int hns_rcb_get_port(struct rcb_common_cb *rcb_common, int ring_idx)
 	return port;
 }
 
+#define SERVICE_RING_IRQ_IDX(v1) \
+	((v1) ? HNS_SERVICE_RING_IRQ_IDX : HNSV2_SERVICE_RING_IRQ_IDX)
+#define DEBUG_RING_IRQ_IDX(v1) \
+	((v1) ? HNS_DEBUG_RING_IRQ_IDX : HNSV2_DEBUG_RING_IRQ_IDX)
+#define DEBUG_RING_IRQ_OFFSET(v1) \
+	((v1) ? HNS_DEBUG_RING_IRQ_OFFSET : HNSV2_DEBUG_RING_IRQ_OFFSET)
 static int hns_rcb_get_base_irq_idx(struct rcb_common_cb *rcb_common)
 {
 	int comm_index = rcb_common->comm_index;
+	bool is_ver1 = AE_IS_VER1(rcb_common->dsaf_dev->dsaf_ver);
 
 	if (comm_index == HNS_DSAF_COMM_SERVICE_NW_IDX)
-		return HNS_SERVICE_RING_IRQ_IDX;
+		return SERVICE_RING_IRQ_IDX(is_ver1);
 	else
-		return HNS_DEBUG_RING_IRQ_IDX + (comm_index - 1) * 2;
+		return  DEBUG_RING_IRQ_IDX(is_ver1) +
+			(comm_index - 1) * DEBUG_RING_IRQ_OFFSET(is_ver1);
 }
 
 #define RCB_COMM_BASE_TO_RING_BASE(base, ringid)\
@@ -468,6 +507,9 @@ void hns_rcb_get_cfg(struct rcb_common_cb *rcb_common)
 	u32 ring_num = rcb_common->ring_num;
 	int base_irq_idx = hns_rcb_get_base_irq_idx(rcb_common);
 	struct device_node *np = rcb_common->dsaf_dev->dev->of_node;
+	struct platform_device *pdev =
+		to_platform_device(rcb_common->dsaf_dev->dev);
+	bool is_ver1 = AE_IS_VER1(rcb_common->dsaf_dev->dsaf_ver);
 
 	for (i = 0; i < ring_num; i++) {
 		ring_pair_cb = &rcb_common->ring_pair_cb[i];
@@ -477,10 +519,12 @@ void hns_rcb_get_cfg(struct rcb_common_cb *rcb_common)
 		ring_pair_cb->q.io_base =
 			RCB_COMM_BASE_TO_RING_BASE(rcb_common->io_base, i);
 		ring_pair_cb->port_id_in_dsa = hns_rcb_get_port(rcb_common, i);
-		ring_pair_cb->virq[HNS_RCB_IRQ_IDX_TX]
-			= irq_of_parse_and_map(np, base_irq_idx + i * 2);
-		ring_pair_cb->virq[HNS_RCB_IRQ_IDX_RX]
-			= irq_of_parse_and_map(np, base_irq_idx + i * 2 + 1);
+		ring_pair_cb->virq[HNS_RCB_IRQ_IDX_TX] =
+		is_ver1 ? irq_of_parse_and_map(np, base_irq_idx + i * 2) :
+			  platform_get_irq(pdev, base_irq_idx + i * 3 + 1);
+		ring_pair_cb->virq[HNS_RCB_IRQ_IDX_RX] =
+		is_ver1 ? irq_of_parse_and_map(np, base_irq_idx + i * 2 + 1) :
+			  platform_get_irq(pdev, base_irq_idx + i * 3);
 		ring_pair_cb->q.phy_base =
 			RCB_COMM_BASE_TO_RING_BASE(rcb_common->phy_base, i);
 		hns_rcb_ring_pair_get_cfg(ring_pair_cb);
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.h
index 3a2afe2dd8bb..81fe9f849973 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_rcb.h
@@ -26,6 +26,8 @@ struct rcb_common_cb;
 #define HNS_RCB_SERVICE_NW_ENGINE_NUM		DSAF_COMM_CHN
 #define HNS_RCB_DEBUG_NW_ENGINE_NUM		1
 #define HNS_RCB_RING_MAX_BD_PER_PKT		3
+#define HNS_RCB_RING_MAX_TXBD_PER_PKT		3
+#define HNS_RCBV2_RING_MAX_TXBD_PER_PKT		8
 #define HNS_RCB_MAX_PKT_SIZE MAC_MAX_MTU
 
 #define HNS_RCB_RING_MAX_PENDING_BD		1024
@@ -52,6 +54,9 @@ struct rcb_common_cb;
 #define HNS_DUMP_REG_NUM			500
 #define HNS_STATIC_REG_NUM			12
 
+#define HNS_TSO_MODE_8BD_32K			1
+#define HNS_TSO_MDOE_4BD_16K			0
+
 enum rcb_int_flag {
 	RCB_INT_FLAG_TX = 0x1,
 	RCB_INT_FLAG_RX = (0x1 << 1),
@@ -106,13 +111,17 @@ void hns_rcb_common_free_cfg(struct dsaf_device *dsaf_dev, u32 comm_index);
 int hns_rcb_common_init_hw(struct rcb_common_cb *rcb_common);
 void hns_rcb_start(struct hnae_queue *q, u32 val);
 void hns_rcb_get_cfg(struct rcb_common_cb *rcb_common);
-void hns_rcb_common_init_commit_hw(struct rcb_common_cb *rcb_common);
 void hns_rcb_get_queue_mode(enum dsaf_mode dsaf_mode, int comm_index,
 			    u16 *max_vfn, u16 *max_q_per_vf);
 
+void hns_rcb_common_init_commit_hw(struct rcb_common_cb *rcb_common);
+
 void hns_rcb_ring_enable_hw(struct hnae_queue *q, u32 val);
 void hns_rcb_int_clr_hw(struct hnae_queue *q, u32 flag);
 void hns_rcb_int_ctrl_hw(struct hnae_queue *q, u32 flag, u32 enable);
+void hns_rcbv2_int_ctrl_hw(struct hnae_queue *q, u32 flag, u32 mask);
+void hns_rcbv2_int_clr_hw(struct hnae_queue *q, u32 flag);
+
 void hns_rcb_init_hw(struct ring_pair_cb *ring);
 void hns_rcb_reset_ring_hw(struct hnae_queue *q);
 void hns_rcb_wait_fbd_clean(struct hnae_queue **qs, int q_num, u32 flag);
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
index bdbd80423b17..60d695daa471 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_dsaf_reg.h
@@ -10,21 +10,12 @@
 #ifndef _DSAF_REG_H_
 #define _DSAF_REG_H_
 
-#define HNS_GE_FIFO_ERR_INTNUM 8
-#define HNS_XGE_ERR_INTNUM 6
-#define HNS_RCB_COMM_ERR_INTNUM 12
-#define HNS_PPE_TNL_ERR_INTNUM 8
-#define HNS_DSAF_EVENT_INTNUM 21
-#define HNS_DEBUG_RING_INTNUM 4
-#define HNS_SERVICE_RING_INTNUM 256
-
-#define HNS_DEBUG_RING_IRQ_IDX (HNS_GE_FIFO_ERR_INTNUM + HNS_XGE_ERR_INTNUM +\
-		HNS_RCB_COMM_ERR_INTNUM + HNS_PPE_TNL_ERR_INTNUM +\
-		HNS_DSAF_EVENT_INTNUM)
-#define HNS_SERVICE_RING_IRQ_IDX (HNS_DEBUG_RING_IRQ_IDX +\
-		HNS_DEBUG_RING_INTNUM)
-
-#define DSAF_IRQ_NUM 18
+#define HNS_DEBUG_RING_IRQ_IDX 55
+#define HNS_SERVICE_RING_IRQ_IDX 59
+#define HNS_DEBUG_RING_IRQ_OFFSET 2
+#define HNSV2_DEBUG_RING_IRQ_IDX 409
+#define HNSV2_SERVICE_RING_IRQ_IDX 25
+#define HNSV2_DEBUG_RING_IRQ_OFFSET 9
 
 #define DSAF_MAX_PORT_NUM_PER_CHIP 8
 #define DSAF_SERVICE_PORT_NUM_PER_DSAF 6
@@ -39,9 +30,15 @@
 #define DSAF_GE_NUM		((DSAF_SERVICE_NW_NUM) + (DSAF_DEBUG_NW_NUM))
 #define DSAF_PORT_NUM		((DSAF_SERVICE_NW_NUM) + (DSAF_DEBUG_NW_NUM))
 #define DSAF_XGE_NUM		DSAF_SERVICE_NW_NUM
+#define DSAF_PORT_TYPE_NUM 3
 #define DSAF_NODE_NUM		18
 #define DSAF_XOD_BIG_NUM	DSAF_NODE_NUM
 #define DSAF_SBM_NUM		DSAF_NODE_NUM
+#define DSAFV2_SBM_NUM		8
+#define DSAFV2_SBM_XGE_CHN    6
+#define DSAFV2_SBM_PPE_CHN    1
+#define DASFV2_ROCEE_CRD_NUM  8
+
 #define DSAF_VOQ_NUM		DSAF_NODE_NUM
 #define DSAF_INODE_NUM		DSAF_NODE_NUM
 #define DSAF_XOD_NUM		8
@@ -52,56 +49,56 @@
 #define DSAF_TCAM_SUM		512
 #define DSAF_LINE_SUM		(2048 * 14)
 
-#define DSAF_SUB_SC_NT_SRAM_CLK_SEL_REG                0x100
-#define DSAF_SUB_SC_HILINK3_CRG_CTRL0_REG              0x180
-#define DSAF_SUB_SC_HILINK3_CRG_CTRL1_REG              0x184
-#define DSAF_SUB_SC_HILINK3_CRG_CTRL2_REG              0x188
-#define DSAF_SUB_SC_HILINK3_CRG_CTRL3_REG              0x18C
-#define DSAF_SUB_SC_HILINK4_CRG_CTRL0_REG              0x190
-#define DSAF_SUB_SC_HILINK4_CRG_CTRL1_REG              0x194
-#define DSAF_SUB_SC_DSAF_CLK_EN_REG                    0x300
-#define DSAF_SUB_SC_DSAF_CLK_DIS_REG                   0x304
-#define DSAF_SUB_SC_NT_CLK_EN_REG                      0x308
-#define DSAF_SUB_SC_NT_CLK_DIS_REG                     0x30C
-#define DSAF_SUB_SC_XGE_CLK_EN_REG                     0x310
-#define DSAF_SUB_SC_XGE_CLK_DIS_REG                    0x314
-#define DSAF_SUB_SC_GE_CLK_EN_REG                      0x318
-#define DSAF_SUB_SC_GE_CLK_DIS_REG                     0x31C
-#define DSAF_SUB_SC_PPE_CLK_EN_REG                     0x320
-#define DSAF_SUB_SC_PPE_CLK_DIS_REG                    0x324
-#define DSAF_SUB_SC_RCB_PPE_COM_CLK_EN_REG             0x350
-#define DSAF_SUB_SC_RCB_PPE_COM_CLK_DIS_REG            0x354
-#define DSAF_SUB_SC_XBAR_RESET_REQ_REG                 0xA00
-#define DSAF_SUB_SC_XBAR_RESET_DREQ_REG                0xA04
-#define DSAF_SUB_SC_NT_RESET_REQ_REG                   0xA08
-#define DSAF_SUB_SC_NT_RESET_DREQ_REG                  0xA0C
-#define DSAF_SUB_SC_XGE_RESET_REQ_REG                  0xA10
-#define DSAF_SUB_SC_XGE_RESET_DREQ_REG                 0xA14
-#define DSAF_SUB_SC_GE_RESET_REQ0_REG                  0xA18
-#define DSAF_SUB_SC_GE_RESET_DREQ0_REG                 0xA1C
-#define DSAF_SUB_SC_GE_RESET_REQ1_REG                  0xA20
-#define DSAF_SUB_SC_GE_RESET_DREQ1_REG                 0xA24
-#define DSAF_SUB_SC_PPE_RESET_REQ_REG                  0xA48
-#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_LIGHT_MODULE_DETECT_EN_REG         0x2060
-#define DSAF_SUB_SC_TCAM_MBIST_EN_REG                  0x2300
-#define DSAF_SUB_SC_DSAF_CLK_ST_REG                    0x5300
-#define DSAF_SUB_SC_NT_CLK_ST_REG                      0x5304
-#define DSAF_SUB_SC_XGE_CLK_ST_REG                     0x5308
-#define DSAF_SUB_SC_GE_CLK_ST_REG                      0x530C
-#define DSAF_SUB_SC_PPE_CLK_ST_REG                     0x5310
-#define DSAF_SUB_SC_ROCEE_CLK_ST_REG                   0x5314
-#define DSAF_SUB_SC_CPU_CLK_ST_REG                     0x5318
-#define DSAF_SUB_SC_RCB_PPE_COM_CLK_ST_REG             0x5328
-#define DSAF_SUB_SC_XBAR_RESET_ST_REG                  0x5A00
-#define DSAF_SUB_SC_NT_RESET_ST_REG                    0x5A04
-#define DSAF_SUB_SC_XGE_RESET_ST_REG                   0x5A08
-#define DSAF_SUB_SC_GE_RESET_ST0_REG                   0x5A0C
-#define DSAF_SUB_SC_GE_RESET_ST1_REG                   0x5A10
-#define DSAF_SUB_SC_PPE_RESET_ST_REG                   0x5A24
-#define DSAF_SUB_SC_RCB_PPE_COM_RESET_ST_REG           0x5A44
+#define DSAF_SUB_SC_NT_SRAM_CLK_SEL_REG			0x100
+#define DSAF_SUB_SC_HILINK3_CRG_CTRL0_REG		0x180
+#define DSAF_SUB_SC_HILINK3_CRG_CTRL1_REG		0x184
+#define DSAF_SUB_SC_HILINK3_CRG_CTRL2_REG		0x188
+#define DSAF_SUB_SC_HILINK3_CRG_CTRL3_REG		0x18C
+#define DSAF_SUB_SC_HILINK4_CRG_CTRL0_REG		0x190
+#define DSAF_SUB_SC_HILINK4_CRG_CTRL1_REG		0x194
+#define DSAF_SUB_SC_DSAF_CLK_EN_REG			0x300
+#define DSAF_SUB_SC_DSAF_CLK_DIS_REG			0x304
+#define DSAF_SUB_SC_NT_CLK_EN_REG			0x308
+#define DSAF_SUB_SC_NT_CLK_DIS_REG			0x30C
+#define DSAF_SUB_SC_XGE_CLK_EN_REG			0x310
+#define DSAF_SUB_SC_XGE_CLK_DIS_REG			0x314
+#define DSAF_SUB_SC_GE_CLK_EN_REG			0x318
+#define DSAF_SUB_SC_GE_CLK_DIS_REG			0x31C
+#define DSAF_SUB_SC_PPE_CLK_EN_REG			0x320
+#define DSAF_SUB_SC_PPE_CLK_DIS_REG			0x324
+#define DSAF_SUB_SC_RCB_PPE_COM_CLK_EN_REG		0x350
+#define DSAF_SUB_SC_RCB_PPE_COM_CLK_DIS_REG		0x354
+#define DSAF_SUB_SC_XBAR_RESET_REQ_REG			0xA00
+#define DSAF_SUB_SC_XBAR_RESET_DREQ_REG			0xA04
+#define DSAF_SUB_SC_NT_RESET_REQ_REG			0xA08
+#define DSAF_SUB_SC_NT_RESET_DREQ_REG			0xA0C
+#define DSAF_SUB_SC_XGE_RESET_REQ_REG			0xA10
+#define DSAF_SUB_SC_XGE_RESET_DREQ_REG			0xA14
+#define DSAF_SUB_SC_GE_RESET_REQ0_REG			0xA18
+#define DSAF_SUB_SC_GE_RESET_DREQ0_REG			0xA1C
+#define DSAF_SUB_SC_GE_RESET_REQ1_REG			0xA20
+#define DSAF_SUB_SC_GE_RESET_DREQ1_REG			0xA24
+#define DSAF_SUB_SC_PPE_RESET_REQ_REG			0xA48
+#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_LIGHT_MODULE_DETECT_EN_REG		0x2060
+#define DSAF_SUB_SC_TCAM_MBIST_EN_REG			0x2300
+#define DSAF_SUB_SC_DSAF_CLK_ST_REG			0x5300
+#define DSAF_SUB_SC_NT_CLK_ST_REG			0x5304
+#define DSAF_SUB_SC_XGE_CLK_ST_REG			0x5308
+#define DSAF_SUB_SC_GE_CLK_ST_REG			0x530C
+#define DSAF_SUB_SC_PPE_CLK_ST_REG			0x5310
+#define DSAF_SUB_SC_ROCEE_CLK_ST_REG			0x5314
+#define DSAF_SUB_SC_CPU_CLK_ST_REG			0x5318
+#define DSAF_SUB_SC_RCB_PPE_COM_CLK_ST_REG		0x5328
+#define DSAF_SUB_SC_XBAR_RESET_ST_REG			0x5A00
+#define DSAF_SUB_SC_NT_RESET_ST_REG			0x5A04
+#define DSAF_SUB_SC_XGE_RESET_ST_REG			0x5A08
+#define DSAF_SUB_SC_GE_RESET_ST0_REG			0x5A0C
+#define DSAF_SUB_SC_GE_RESET_ST1_REG			0x5A10
+#define DSAF_SUB_SC_PPE_RESET_ST_REG			0x5A24
+#define DSAF_SUB_SC_RCB_PPE_COM_RESET_ST_REG		0x5A44
 
 /*serdes offset**/
 #define HNS_MAC_HILINK3_REG DSAF_SUB_SC_HILINK3_CRG_CTRL0_REG
@@ -137,6 +134,7 @@
 #define DSAF_XGE_INT_STS_0_REG		0x1C0
 #define DSAF_PPE_INT_STS_0_REG		0x1E0
 #define DSAF_ROCEE_INT_STS_0_REG	0x200
+#define DSAFV2_SERDES_LBK_0_REG         0x220
 #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 +176,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 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
 #define DSAF_SBM_BP_CNT_0_0_REG			0x2018
@@ -319,6 +318,8 @@
 #define PPE_CFG_TAG_GEN_REG			0x90
 #define PPE_CFG_PARSE_TAG_REG			0x94
 #define PPE_CFG_PRO_CHECK_EN_REG		0x98
+#define PPEV2_CFG_TSO_EN_REG			0xA0
+#define PPEV2_VLAN_STRIP_EN_REG			0xAC
 #define PPE_INTEN_REG				0x100
 #define PPE_RINT_REG				0x104
 #define PPE_INTSTS_REG				0x108
@@ -351,6 +352,8 @@
 #define PPE_ECO0_REG				0x32C
 #define PPE_ECO1_REG				0x330
 #define PPE_ECO2_REG				0x334
+#define PPEV2_INDRECTION_TBL_REG		0x800
+#define PPEV2_RSS_KEY_REG			0x900
 
 #define RCB_COM_CFG_ENDIAN_REG			0x0
 #define RCB_COM_CFG_SYS_FSH_REG			0xC
@@ -361,6 +364,8 @@
 #define RCB_COM_CFG_FA_REG			0x3C
 #define RCB_COM_CFG_PKT_TC_BP_REG		0x40
 #define RCB_COM_CFG_PPE_TNL_CLKEN_REG		0x44
+#define RCBV2_COM_CFG_USER_REG			0x30
+#define RCBV2_COM_CFG_TSO_MODE_REG		0x50
 
 #define RCB_COM_INTMSK_TX_PKT_REG		0x3A0
 #define RCB_COM_RINT_TX_PKT_REG			0x3A8
@@ -431,8 +436,10 @@
 
 #define RCB_RING_INTMSK_RXWL_REG		0x000A0
 #define RCB_RING_INTSTS_RX_RING_REG		0x000A4
+#define RCBV2_RX_RING_INT_STS_REG		0x000A8
 #define RCB_RING_INTMSK_TXWL_REG		0x000AC
 #define RCB_RING_INTSTS_TX_RING_REG		0x000B0
+#define RCBV2_TX_RING_INT_STS_REG		0x000B4
 #define RCB_RING_INTMSK_RX_OVERTIME_REG		0x000B8
 #define RCB_RING_INTSTS_RX_OVERTIME_REG		0x000BC
 #define RCB_RING_INTMSK_TX_OVERTIME_REG		0x000C4
@@ -678,6 +685,10 @@
 
 #define XGMAC_TRX_CORE_SRST_M			0x2080
 
+#define DSAF_SRAM_INIT_OVER_M 0xff
+#define DSAFV2_SRAM_INIT_OVER_M 0x3ff
+#define DSAF_SRAM_INIT_OVER_S 0
+
 #define DSAF_CFG_EN_S 0
 #define DSAF_CFG_TC_MODE_S 1
 #define DSAF_CFG_CRC_EN_S 2
@@ -685,6 +696,7 @@
 #define DSAF_CFG_MIX_MODE_S 4
 #define DSAF_CFG_STP_MODE_S 5
 #define DSAF_CFG_LOCA_ADDR_EN_S 6
+#define DSAFV2_CFG_VLAN_TAG_MODE_S 17
 
 #define DSAF_CNT_CLR_CE_S 0
 #define DSAF_SNAP_EN_S 1
@@ -707,6 +719,16 @@
 
 #define DSAF_INODE_IN_PORT_NUM_M 7
 #define DSAF_INODE_IN_PORT_NUM_S 0
+#define DSAFV2_INODE_IN_PORT1_NUM_M (7ULL << 3)
+#define DSAFV2_INODE_IN_PORT1_NUM_S 3
+#define DSAFV2_INODE_IN_PORT2_NUM_M (7ULL << 6)
+#define DSAFV2_INODE_IN_PORT2_NUM_S 6
+#define DSAFV2_INODE_IN_PORT3_NUM_M (7ULL << 9)
+#define DSAFV2_INODE_IN_PORT3_NUM_S 9
+#define DSAFV2_INODE_IN_PORT4_NUM_M (7ULL << 12)
+#define DSAFV2_INODE_IN_PORT4_NUM_S 12
+#define DSAFV2_INODE_IN_PORT5_NUM_M (7ULL << 15)
+#define DSAFV2_INODE_IN_PORT5_NUM_S 15
 
 #define HNS_DSAF_I4TC_CFG 0x18688688
 #define HNS_DSAF_I8TC_CFG 0x18FAC688
@@ -738,6 +760,33 @@
 #define DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_S 10
 #define DSAF_SBM_CFG3_RESET_BUF_NUM_NO_PFC_M (((1ULL << 10) - 1) << 10)
 
+#define DSAFV2_SBM_CFG0_VC1_MAX_BUF_NUM_S 0
+#define DSAFV2_SBM_CFG0_VC1_MAX_BUF_NUM_M (((1ULL << 9) - 1) << 0)
+#define DSAFV2_SBM_CFG0_VC0_MAX_BUF_NUM_S 9
+#define DSAFV2_SBM_CFG0_VC0_MAX_BUF_NUM_M (((1ULL << 9) - 1) << 9)
+#define DSAFV2_SBM_CFG0_COM_MAX_BUF_NUM_S 18
+#define DSAFV2_SBM_CFG0_COM_MAX_BUF_NUM_M (((1ULL << 10) - 1) << 18)
+
+#define DSAFV2_SBM_CFG1_TC4_MAX_BUF_NUM_S 0
+#define DSAFV2_SBM_CFG1_TC4_MAX_BUF_NUM_M (((1ULL << 9) - 1) << 0)
+#define DSAFV2_SBM_CFG1_TC0_MAX_BUF_NUM_S 9
+#define DSAFV2_SBM_CFG1_TC0_MAX_BUF_NUM_M (((1ULL << 9) - 1) << 9)
+
+#define DSAFV2_SBM_CFG2_SET_BUF_NUM_S 0
+#define DSAFV2_SBM_CFG2_SET_BUF_NUM_M (((1ULL << 9) - 1) << 0)
+#define DSAFV2_SBM_CFG2_RESET_BUF_NUM_S 9
+#define DSAFV2_SBM_CFG2_RESET_BUF_NUM_M (((1ULL << 9) - 1) << 9)
+
+#define DSAFV2_SBM_CFG3_SET_BUF_NUM_NO_PFC_S 0
+#define DSAFV2_SBM_CFG3_SET_BUF_NUM_NO_PFC_M (((1ULL << 9) - 1) << 0)
+#define DSAFV2_SBM_CFG3_RESET_BUF_NUM_NO_PFC_S 9
+#define DSAFV2_SBM_CFG3_RESET_BUF_NUM_NO_PFC_M (((1ULL << 9) - 1) << 9)
+
+#define DSAFV2_SBM_CFG4_SET_BUF_NUM_NO_PFC_S 0
+#define DSAFV2_SBM_CFG4_SET_BUF_NUM_NO_PFC_M (((1ULL << 9) - 1) << 0)
+#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_TBL_TCAM_ADDR_S 0
 #define DSAF_TBL_TCAM_ADDR_M ((1ULL << 9) - 1)
 
@@ -797,11 +846,30 @@
 #define PPE_CFG_QID_MODE_CF_QID_MODE_S	8
 #define PPE_CFG_QID_MODE_CF_QID_MODE_M	(0x7 << PPE_CFG_QID_MODE_CF_QID_MODE_S)
 
+#define PPEV2_CFG_RSS_TBL_4N0_S	0
+#define PPEV2_CFG_RSS_TBL_4N0_M	(((1UL << 5) - 1) << PPEV2_CFG_RSS_TBL_4N0_S)
+
+#define PPEV2_CFG_RSS_TBL_4N1_S	8
+#define PPEV2_CFG_RSS_TBL_4N1_M	(((1UL << 5) - 1) << PPEV2_CFG_RSS_TBL_4N1_S)
+
+#define PPEV2_CFG_RSS_TBL_4N2_S	16
+#define PPEV2_CFG_RSS_TBL_4N2_M	(((1UL << 5) - 1) << PPEV2_CFG_RSS_TBL_4N2_S)
+
+#define PPEV2_CFG_RSS_TBL_4N3_S	24
+#define PPEV2_CFG_RSS_TBL_4N3_M	(((1UL << 5) - 1) << PPEV2_CFG_RSS_TBL_4N3_S)
+
+#define DSAFV2_SERDES_LBK_EN_B  8
+#define DSAFV2_SERDES_LBK_QID_S 0
+#define DSAFV2_SERDES_LBK_QID_M	(((1UL << 8) - 1) << DSAFV2_SERDES_LBK_QID_S)
+
 #define PPE_CNT_CLR_CE_B	0
 #define PPE_CNT_CLR_SNAP_EN_B	1
 
 #define PPE_COMMON_CNT_CLR_CE_B	0
 #define PPE_COMMON_CNT_CLR_SNAP_EN_B	1
+#define RCB_COM_TSO_MODE_B	0
+#define RCB_COM_CFG_FNA_B	1
+#define RCB_COM_CFG_FA_B	0
 
 #define GMAC_DUPLEX_TYPE_B 0
 
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_enet.c b/drivers/net/ethernet/hisilicon/hns/hns_enet.c
index 08cef0dfb5db..3f77ff77abbc 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_enet.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_enet.c
@@ -33,10 +33,105 @@
 
 #define RCB_IRQ_NOT_INITED 0
 #define RCB_IRQ_INITED 1
+#define HNS_BUFFER_SIZE_2048 2048
+
+#define BD_MAX_SEND_SIZE 8191
+#define SKB_TMP_LEN(SKB) \
+	(((SKB)->transport_header - (SKB)->mac_header) + tcp_hdrlen(SKB))
+
+static void fill_v2_desc(struct hnae_ring *ring, void *priv,
+			 int size, dma_addr_t dma, int frag_end,
+			 int buf_num, enum hns_desc_type type, int mtu)
+{
+	struct hnae_desc *desc = &ring->desc[ring->next_to_use];
+	struct hnae_desc_cb *desc_cb = &ring->desc_cb[ring->next_to_use];
+	struct iphdr *iphdr;
+	struct ipv6hdr *ipv6hdr;
+	struct sk_buff *skb;
+	int skb_tmp_len;
+	__be16 protocol;
+	u8 bn_pid = 0;
+	u8 rrcfv = 0;
+	u8 ip_offset = 0;
+	u8 tvsvsn = 0;
+	u16 mss = 0;
+	u8 l4_len = 0;
+	u16 paylen = 0;
+
+	desc_cb->priv = priv;
+	desc_cb->length = size;
+	desc_cb->dma = dma;
+	desc_cb->type = type;
+
+	desc->addr = cpu_to_le64(dma);
+	desc->tx.send_size = cpu_to_le16((u16)size);
+
+	/*config bd buffer end */
+	hnae_set_bit(rrcfv, HNSV2_TXD_VLD_B, 1);
+	hnae_set_field(bn_pid, HNSV2_TXD_BUFNUM_M, 0, buf_num - 1);
+
+	if (type == DESC_TYPE_SKB) {
+		skb = (struct sk_buff *)priv;
+
+		if (skb->ip_summed == CHECKSUM_PARTIAL) {
+			skb_reset_mac_len(skb);
+			protocol = skb->protocol;
+			ip_offset = ETH_HLEN;
+
+			if (protocol == htons(ETH_P_8021Q)) {
+				ip_offset += VLAN_HLEN;
+				protocol = vlan_get_protocol(skb);
+				skb->protocol = protocol;
+			}
+
+			if (skb->protocol == htons(ETH_P_IP)) {
+				iphdr = ip_hdr(skb);
+				hnae_set_bit(rrcfv, HNSV2_TXD_L3CS_B, 1);
+				hnae_set_bit(rrcfv, HNSV2_TXD_L4CS_B, 1);
+
+				/* check for tcp/udp header */
+				if (iphdr->protocol == IPPROTO_TCP) {
+					hnae_set_bit(tvsvsn,
+						     HNSV2_TXD_TSE_B, 1);
+					skb_tmp_len = SKB_TMP_LEN(skb);
+					l4_len = tcp_hdrlen(skb);
+					mss = mtu - skb_tmp_len - ETH_FCS_LEN;
+					paylen = skb->len - skb_tmp_len;
+				}
+			} else if (skb->protocol == htons(ETH_P_IPV6)) {
+				hnae_set_bit(tvsvsn, HNSV2_TXD_IPV6_B, 1);
+				ipv6hdr = ipv6_hdr(skb);
+				hnae_set_bit(rrcfv, HNSV2_TXD_L4CS_B, 1);
+
+				/* check for tcp/udp header */
+				if (ipv6hdr->nexthdr == IPPROTO_TCP) {
+					hnae_set_bit(tvsvsn,
+						     HNSV2_TXD_TSE_B, 1);
+					skb_tmp_len = SKB_TMP_LEN(skb);
+					l4_len = tcp_hdrlen(skb);
+					mss = mtu - skb_tmp_len - ETH_FCS_LEN;
+					paylen = skb->len - skb_tmp_len;
+				}
+			}
+			desc->tx.ip_offset = ip_offset;
+			desc->tx.tse_vlan_snap_v6_sctp_nth = tvsvsn;
+			desc->tx.mss = cpu_to_le16(mss);
+			desc->tx.l4_len = l4_len;
+			desc->tx.paylen = cpu_to_le16(paylen);
+		}
+	}
+
+	hnae_set_bit(rrcfv, HNSV2_TXD_FE_B, frag_end);
+
+	desc->tx.bn_pid = bn_pid;
+	desc->tx.ra_ri_cs_fe_vld = rrcfv;
+
+	ring_ptr_move_fw(ring, next_to_use);
+}
 
 static void fill_desc(struct hnae_ring *ring, void *priv,
 		      int size, dma_addr_t dma, int frag_end,
-		      int buf_num, enum hns_desc_type type)
+		      int buf_num, enum hns_desc_type type, int mtu)
 {
 	struct hnae_desc *desc = &ring->desc[ring->next_to_use];
 	struct hnae_desc_cb *desc_cb = &ring->desc_cb[ring->next_to_use];
@@ -100,47 +195,129 @@ static void unfill_desc(struct hnae_ring *ring)
 	ring_ptr_move_bw(ring, next_to_use);
 }
 
-int hns_nic_net_xmit_hw(struct net_device *ndev,
-			struct sk_buff *skb,
-			struct hns_nic_ring_data *ring_data)
+static int hns_nic_maybe_stop_tx(
+	struct sk_buff **out_skb, int *bnum, struct hnae_ring *ring)
 {
-	struct hns_nic_priv *priv = netdev_priv(ndev);
-	struct device *dev = priv->dev;
-	struct hnae_ring *ring = ring_data->ring;
-	struct netdev_queue *dev_queue;
-	struct skb_frag_struct *frag;
+	struct sk_buff *skb = *out_skb;
+	struct sk_buff *new_skb = NULL;
 	int buf_num;
-	dma_addr_t dma;
-	int size, next_to_use;
-	int i, j;
-	struct sk_buff *new_skb;
-
-	assert(ring->max_desc_num_per_pkt <= ring->desc_num);
 
 	/* no. of segments (plus a header) */
 	buf_num = skb_shinfo(skb)->nr_frags + 1;
 
 	if (unlikely(buf_num > ring->max_desc_num_per_pkt)) {
-		if (ring_space(ring) < 1) {
-			ring->stats.tx_busy++;
-			goto out_net_tx_busy;
-		}
+		if (ring_space(ring) < 1)
+			return -EBUSY;
 
 		new_skb = skb_copy(skb, GFP_ATOMIC);
-		if (!new_skb) {
-			ring->stats.sw_err_cnt++;
-			netdev_err(ndev, "no memory to xmit!\n");
-			goto out_err_tx_ok;
-		}
+		if (!new_skb)
+			return -ENOMEM;
 
 		dev_kfree_skb_any(skb);
-		skb = new_skb;
+		*out_skb = new_skb;
 		buf_num = 1;
-		assert(skb_shinfo(skb)->nr_frags == 1);
 	} else if (buf_num > ring_space(ring)) {
+		return -EBUSY;
+	}
+
+	*bnum = buf_num;
+	return 0;
+}
+
+static int hns_nic_maybe_stop_tso(
+	struct sk_buff **out_skb, int *bnum, struct hnae_ring *ring)
+{
+	int i;
+	int size;
+	int buf_num;
+	int frag_num;
+	struct sk_buff *skb = *out_skb;
+	struct sk_buff *new_skb = NULL;
+	struct skb_frag_struct *frag;
+
+	size = skb_headlen(skb);
+	buf_num = (size + BD_MAX_SEND_SIZE - 1) / BD_MAX_SEND_SIZE;
+
+	frag_num = skb_shinfo(skb)->nr_frags;
+	for (i = 0; i < frag_num; i++) {
+		frag = &skb_shinfo(skb)->frags[i];
+		size = skb_frag_size(frag);
+		buf_num += (size + BD_MAX_SEND_SIZE - 1) / BD_MAX_SEND_SIZE;
+	}
+
+	if (unlikely(buf_num > ring->max_desc_num_per_pkt)) {
+		buf_num = (skb->len + BD_MAX_SEND_SIZE - 1) / BD_MAX_SEND_SIZE;
+		if (ring_space(ring) < buf_num)
+			return -EBUSY;
+		/* manual split the send packet */
+		new_skb = skb_copy(skb, GFP_ATOMIC);
+		if (!new_skb)
+			return -ENOMEM;
+		dev_kfree_skb_any(skb);
+		*out_skb = new_skb;
+
+	} else if (ring_space(ring) < buf_num) {
+		return -EBUSY;
+	}
+
+	*bnum = buf_num;
+	return 0;
+}
+
+static void fill_tso_desc(struct hnae_ring *ring, void *priv,
+			  int size, dma_addr_t dma, int frag_end,
+			  int buf_num, enum hns_desc_type type, int mtu)
+{
+	int frag_buf_num;
+	int sizeoflast;
+	int k;
+
+	frag_buf_num = (size + BD_MAX_SEND_SIZE - 1) / BD_MAX_SEND_SIZE;
+	sizeoflast = size % BD_MAX_SEND_SIZE;
+	sizeoflast = sizeoflast ? sizeoflast : BD_MAX_SEND_SIZE;
+
+	/* when the frag size is bigger than hardware, split this frag */
+	for (k = 0; k < frag_buf_num; k++)
+		fill_v2_desc(ring, priv,
+			     (k == frag_buf_num - 1) ?
+					sizeoflast : BD_MAX_SEND_SIZE,
+			     dma + BD_MAX_SEND_SIZE * k,
+			     frag_end && (k == frag_buf_num - 1) ? 1 : 0,
+			     buf_num,
+			     (type == DESC_TYPE_SKB && !k) ?
+					DESC_TYPE_SKB : DESC_TYPE_PAGE,
+			     mtu);
+}
+
+int hns_nic_net_xmit_hw(struct net_device *ndev,
+			struct sk_buff *skb,
+			struct hns_nic_ring_data *ring_data)
+{
+	struct hns_nic_priv *priv = netdev_priv(ndev);
+	struct device *dev = priv->dev;
+	struct hnae_ring *ring = ring_data->ring;
+	struct netdev_queue *dev_queue;
+	struct skb_frag_struct *frag;
+	int buf_num;
+	int seg_num;
+	dma_addr_t dma;
+	int size, next_to_use;
+	int i;
+
+	switch (priv->ops.maybe_stop_tx(&skb, &buf_num, ring)) {
+	case -EBUSY:
 		ring->stats.tx_busy++;
 		goto out_net_tx_busy;
+	case -ENOMEM:
+		ring->stats.sw_err_cnt++;
+		netdev_err(ndev, "no memory to xmit!\n");
+		goto out_err_tx_ok;
+	default:
+		break;
 	}
+
+	/* no. of segments (plus a header) */
+	seg_num = skb_shinfo(skb)->nr_frags + 1;
 	next_to_use = ring->next_to_use;
 
 	/* fill the first part */
@@ -151,11 +328,11 @@ int hns_nic_net_xmit_hw(struct net_device *ndev,
 		ring->stats.sw_err_cnt++;
 		goto out_err_tx_ok;
 	}
-	fill_desc(ring, skb, size, dma, buf_num == 1 ? 1 : 0, buf_num,
-		  DESC_TYPE_SKB);
+	priv->ops.fill_desc(ring, skb, size, dma, seg_num == 1 ? 1 : 0,
+			    buf_num, DESC_TYPE_SKB, ndev->mtu);
 
 	/* fill the fragments */
-	for (i = 1; i < buf_num; i++) {
+	for (i = 1; i < seg_num; i++) {
 		frag = &skb_shinfo(skb)->frags[i - 1];
 		size = skb_frag_size(frag);
 		dma = skb_frag_dma_map(dev, frag, 0, size, DMA_TO_DEVICE);
@@ -164,8 +341,9 @@ int hns_nic_net_xmit_hw(struct net_device *ndev,
 			ring->stats.sw_err_cnt++;
 			goto out_map_frag_fail;
 		}
-		fill_desc(ring, skb_frag_page(frag), size, dma,
-			  buf_num - 1 == i ? 1 : 0, buf_num, DESC_TYPE_PAGE);
+		priv->ops.fill_desc(ring, skb_frag_page(frag), size, dma,
+				    seg_num - 1 == i ? 1 : 0, buf_num,
+				    DESC_TYPE_PAGE, ndev->mtu);
 	}
 
 	/*complete translate all packets*/
@@ -182,19 +360,20 @@ int hns_nic_net_xmit_hw(struct net_device *ndev,
 
 out_map_frag_fail:
 
-	for (j = i - 1; j > 0; j--) {
+	while (ring->next_to_use != next_to_use) {
 		unfill_desc(ring);
-		next_to_use = ring->next_to_use;
-		dma_unmap_page(dev, ring->desc_cb[next_to_use].dma,
-			       ring->desc_cb[next_to_use].length,
-			       DMA_TO_DEVICE);
+		if (ring->next_to_use != next_to_use)
+			dma_unmap_page(dev,
+				       ring->desc_cb[ring->next_to_use].dma,
+				       ring->desc_cb[ring->next_to_use].length,
+				       DMA_TO_DEVICE);
+		else
+			dma_unmap_single(dev,
+					 ring->desc_cb[next_to_use].dma,
+					 ring->desc_cb[next_to_use].length,
+					 DMA_TO_DEVICE);
 	}
 
-	unfill_desc(ring);
-	next_to_use = ring->next_to_use;
-	dma_unmap_single(dev, ring->desc_cb[next_to_use].dma,
-			 ring->desc_cb[next_to_use].length, DMA_TO_DEVICE);
-
 out_err_tx_ok:
 
 	dev_kfree_skb_any(skb);
@@ -313,20 +492,67 @@ static unsigned int hns_nic_get_headlen(unsigned char *data, u32 flag,
 		return max_size;
 }
 
-static void
-hns_nic_reuse_page(struct hnae_desc_cb *desc_cb, int tsize, int last_offset)
+static void hns_nic_reuse_page(struct sk_buff *skb, int i,
+			       struct hnae_ring *ring, int pull_len,
+			       struct hnae_desc_cb *desc_cb)
 {
+	struct hnae_desc *desc;
+	int truesize, size;
+	int last_offset;
+	bool twobufs;
+
+	twobufs = ((PAGE_SIZE < 8192) && hnae_buf_size(ring) == HNS_BUFFER_SIZE_2048);
+
+	desc = &ring->desc[ring->next_to_clean];
+	size = le16_to_cpu(desc->rx.size);
+
+	if (twobufs) {
+		truesize = hnae_buf_size(ring);
+	} else {
+		truesize = ALIGN(size, L1_CACHE_BYTES);
+		last_offset = hnae_page_size(ring) - hnae_buf_size(ring);
+	}
+
+	skb_add_rx_frag(skb, i, desc_cb->priv, desc_cb->page_offset + pull_len,
+			size - pull_len, truesize - pull_len);
+
 	 /* avoid re-using remote pages,flag default unreuse */
-	if (likely(page_to_nid(desc_cb->priv) == numa_node_id())) {
-		/* move offset up to the next cache line */
-		desc_cb->page_offset += tsize;
+	if (unlikely(page_to_nid(desc_cb->priv) != numa_node_id()))
+		return;
+
+	if (twobufs) {
+		/* if we are only owner of page we can reuse it */
+		if (likely(page_count(desc_cb->priv) == 1)) {
+			/* flip page offset to other buffer */
+			desc_cb->page_offset ^= truesize;
 
-		if (desc_cb->page_offset <= last_offset) {
 			desc_cb->reuse_flag = 1;
 			/* bump ref count on page before it is given*/
 			get_page(desc_cb->priv);
 		}
+		return;
 	}
+
+	/* move offset up to the next cache line */
+	desc_cb->page_offset += truesize;
+
+	if (desc_cb->page_offset <= last_offset) {
+		desc_cb->reuse_flag = 1;
+		/* bump ref count on page before it is given*/
+		get_page(desc_cb->priv);
+	}
+}
+
+static void get_v2rx_desc_bnum(u32 bnum_flag, int *out_bnum)
+{
+	*out_bnum = hnae_get_field(bnum_flag,
+				   HNS_RXD_BUFNUM_M, HNS_RXD_BUFNUM_S) + 1;
+}
+
+static void get_rx_desc_bnum(u32 bnum_flag, int *out_bnum)
+{
+	*out_bnum = hnae_get_field(bnum_flag,
+				   HNS_RXD_BUFNUM_M, HNS_RXD_BUFNUM_S);
 }
 
 static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
@@ -334,30 +560,42 @@ static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
 {
 	struct hnae_ring *ring = ring_data->ring;
 	struct net_device *ndev = ring_data->napi.dev;
+	struct hns_nic_priv *priv = netdev_priv(ndev);
 	struct sk_buff *skb;
 	struct hnae_desc *desc;
 	struct hnae_desc_cb *desc_cb;
 	unsigned char *va;
-	int bnum, length, size, i, truesize, last_offset;
+	int bnum, length, i;
 	int pull_len;
 	u32 bnum_flag;
 
-	last_offset = hnae_page_size(ring) - hnae_buf_size(ring);
 	desc = &ring->desc[ring->next_to_clean];
 	desc_cb = &ring->desc_cb[ring->next_to_clean];
-	length = le16_to_cpu(desc->rx.pkt_len);
-	bnum_flag = le32_to_cpu(desc->rx.ipoff_bnum_pid_flag);
-	bnum = hnae_get_field(bnum_flag, HNS_RXD_BUFNUM_M, HNS_RXD_BUFNUM_S);
-	*out_bnum = bnum;
+
+	prefetch(desc);
+
 	va = (unsigned char *)desc_cb->buf + desc_cb->page_offset;
 
-	skb = *out_skb = napi_alloc_skb(&ring_data->napi, HNS_RX_HEAD_SIZE);
+	/* prefetch first cache line of first page */
+	prefetch(va);
+#if L1_CACHE_BYTES < 128
+	prefetch(va + L1_CACHE_BYTES);
+#endif
+
+	skb = *out_skb = napi_alloc_skb(&ring_data->napi,
+					HNS_RX_HEAD_SIZE);
 	if (unlikely(!skb)) {
 		netdev_err(ndev, "alloc rx skb fail\n");
 		ring->stats.sw_err_cnt++;
 		return -ENOMEM;
 	}
 
+	prefetchw(skb->data);
+	length = le16_to_cpu(desc->rx.pkt_len);
+	bnum_flag = le32_to_cpu(desc->rx.ipoff_bnum_pid_flag);
+	priv->ops.get_rxd_bnum(bnum_flag, &bnum);
+	*out_bnum = bnum;
+
 	if (length <= HNS_RX_HEAD_SIZE) {
 		memcpy(__skb_put(skb, length), va, ALIGN(length, sizeof(long)));
 
@@ -380,13 +618,7 @@ static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
 		memcpy(__skb_put(skb, pull_len), va,
 		       ALIGN(pull_len, sizeof(long)));
 
-		size = le16_to_cpu(desc->rx.size);
-		truesize = ALIGN(size, L1_CACHE_BYTES);
-		skb_add_rx_frag(skb, 0, desc_cb->priv,
-				desc_cb->page_offset + pull_len,
-				size - pull_len, truesize - pull_len);
-
-		hns_nic_reuse_page(desc_cb, truesize, last_offset);
+		hns_nic_reuse_page(skb, 0, ring, pull_len, desc_cb);
 		ring_ptr_move_fw(ring, next_to_clean);
 
 		if (unlikely(bnum >= (int)MAX_SKB_FRAGS)) { /* check err*/
@@ -396,13 +628,8 @@ static int hns_nic_poll_rx_skb(struct hns_nic_ring_data *ring_data,
 		for (i = 1; i < bnum; i++) {
 			desc = &ring->desc[ring->next_to_clean];
 			desc_cb = &ring->desc_cb[ring->next_to_clean];
-			size = le16_to_cpu(desc->rx.size);
-			truesize = ALIGN(size, L1_CACHE_BYTES);
-			skb_add_rx_frag(skb, i, desc_cb->priv,
-					desc_cb->page_offset,
-					size, truesize);
 
-			hns_nic_reuse_page(desc_cb, truesize, last_offset);
+			hns_nic_reuse_page(skb, i, ring, 0, desc_cb);
 			ring_ptr_move_fw(ring, next_to_clean);
 		}
 	}
@@ -540,20 +767,20 @@ recv:
 	}
 
 	/* make all data has been write before submit */
-	if (clean_count > 0) {
-		hns_nic_alloc_rx_buffers(ring_data, clean_count);
-		clean_count = 0;
-	}
-
 	if (recv_pkts < budget) {
 		ex_num = readl_relaxed(ring->io_base + RCB_REG_FBDNUM);
-		rmb(); /*complete read rx ring bd number*/
-		if (ex_num > 0) {
-			num += ex_num;
+
+		if (ex_num > clean_count) {
+			num += ex_num - clean_count;
+			rmb(); /*complete read rx ring bd number*/
 			goto recv;
 		}
 	}
 
+	/* make all data has been write before submit */
+	if (clean_count > 0)
+		hns_nic_alloc_rx_buffers(ring_data, clean_count);
+
 	return recv_pkts;
 }
 
@@ -642,14 +869,20 @@ static int hns_nic_tx_poll_one(struct hns_nic_ring_data *ring_data,
 
 	bytes = 0;
 	pkts = 0;
-	while (head != ring->next_to_clean)
+	while (head != ring->next_to_clean) {
 		hns_nic_reclaim_one_desc(ring, &bytes, &pkts);
+		/* issue prefetch for next Tx descriptor */
+		prefetch(&ring->desc_cb[ring->next_to_clean]);
+	}
 
 	NETIF_TX_UNLOCK(ndev);
 
 	dev_queue = netdev_get_tx_queue(ndev, ring_data->queue_index);
 	netdev_tx_completed_queue(dev_queue, pkts, bytes);
 
+	if (unlikely(priv->link && !netif_carrier_ok(ndev)))
+		netif_carrier_on(ndev);
+
 	if (unlikely(pkts && netif_carrier_ok(ndev) &&
 		     (ring_space(ring) >= ring->max_desc_num_per_pkt * 2))) {
 		/* Make sure that anybody stopping the queue after this
@@ -716,6 +949,7 @@ static int hns_nic_common_poll(struct napi_struct *napi, int budget)
 			ring_data->ring, 0);
 
 		ring_data->fini_process(ring_data);
+		return 0;
 	}
 
 	return clean_complete;
@@ -848,15 +1082,58 @@ static void hns_nic_ring_close(struct net_device *netdev, int idx)
 	napi_disable(&priv->ring_data[idx].napi);
 }
 
-static int hns_nic_init_irq(struct hns_nic_priv *priv)
+static void hns_set_irq_affinity(struct hns_nic_priv *priv)
 {
 	struct hnae_handle *h = priv->ae_handle;
 	struct hns_nic_ring_data *rd;
 	int i;
-	int ret;
 	int cpu;
 	cpumask_t mask;
 
+	/*diffrent irq banlance for 16core and 32core*/
+	if (h->q_num == num_possible_cpus()) {
+		for (i = 0; i < h->q_num * 2; i++) {
+			rd = &priv->ring_data[i];
+			if (cpu_online(rd->queue_index)) {
+				cpumask_clear(&mask);
+				cpu = rd->queue_index;
+				cpumask_set_cpu(cpu, &mask);
+				(void)irq_set_affinity_hint(rd->ring->irq,
+							    &mask);
+			}
+		}
+	} else {
+		for (i = 0; i < h->q_num; i++) {
+			rd = &priv->ring_data[i];
+			if (cpu_online(rd->queue_index * 2)) {
+				cpumask_clear(&mask);
+				cpu = rd->queue_index * 2;
+				cpumask_set_cpu(cpu, &mask);
+				(void)irq_set_affinity_hint(rd->ring->irq,
+							    &mask);
+			}
+		}
+
+		for (i = h->q_num; i < h->q_num * 2; i++) {
+			rd = &priv->ring_data[i];
+			if (cpu_online(rd->queue_index * 2 + 1)) {
+				cpumask_clear(&mask);
+				cpu = rd->queue_index * 2 + 1;
+				cpumask_set_cpu(cpu, &mask);
+				(void)irq_set_affinity_hint(rd->ring->irq,
+							    &mask);
+			}
+		}
+	}
+}
+
+static int hns_nic_init_irq(struct hns_nic_priv *priv)
+{
+	struct hnae_handle *h = priv->ae_handle;
+	struct hns_nic_ring_data *rd;
+	int i;
+	int ret;
+
 	for (i = 0; i < h->q_num * 2; i++) {
 		rd = &priv->ring_data[i];
 
@@ -878,16 +1155,11 @@ static int hns_nic_init_irq(struct hns_nic_priv *priv)
 		}
 		disable_irq(rd->ring->irq);
 		rd->ring->irq_init_flag = RCB_IRQ_INITED;
-
-		/*set cpu affinity*/
-		if (cpu_online(rd->queue_index)) {
-			cpumask_clear(&mask);
-			cpu = rd->queue_index;
-			cpumask_set_cpu(cpu, &mask);
-			irq_set_affinity_hint(rd->ring->irq, &mask);
-		}
 	}
 
+	/*set cpu affinity*/
+	hns_set_irq_affinity(priv);
+
 	return 0;
 }
 
@@ -1136,6 +1408,51 @@ static int hns_nic_change_mtu(struct net_device *ndev, int new_mtu)
 	return ret;
 }
 
+static int hns_nic_set_features(struct net_device *netdev,
+				netdev_features_t features)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_handle *h = priv->ae_handle;
+
+	switch (priv->enet_ver) {
+	case AE_VERSION_1:
+		if (features & (NETIF_F_TSO | NETIF_F_TSO6))
+			netdev_info(netdev, "enet v1 do not support tso!\n");
+		break;
+	default:
+		if (features & (NETIF_F_TSO | NETIF_F_TSO6)) {
+			priv->ops.fill_desc = fill_tso_desc;
+			priv->ops.maybe_stop_tx = hns_nic_maybe_stop_tso;
+			/* The chip only support 7*4096 */
+			netif_set_gso_max_size(netdev, 7 * 4096);
+			h->dev->ops->set_tso_stats(h, 1);
+		} else {
+			priv->ops.fill_desc = fill_v2_desc;
+			priv->ops.maybe_stop_tx = hns_nic_maybe_stop_tx;
+			h->dev->ops->set_tso_stats(h, 0);
+		}
+		break;
+	}
+	netdev->features = features;
+	return 0;
+}
+
+static netdev_features_t hns_nic_fix_features(
+		struct net_device *netdev, netdev_features_t features)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+
+	switch (priv->enet_ver) {
+	case AE_VERSION_1:
+		features &= ~(NETIF_F_TSO | NETIF_F_TSO6 |
+				NETIF_F_HW_VLAN_CTAG_FILTER);
+		break;
+	default:
+		break;
+	}
+	return features;
+}
+
 /**
  * nic_set_multicast_list - set mutl mac address
  * @netdev: net device
@@ -1231,6 +1548,8 @@ static const struct net_device_ops hns_nic_netdev_ops = {
 	.ndo_set_mac_address = hns_nic_net_set_mac_address,
 	.ndo_change_mtu = hns_nic_change_mtu,
 	.ndo_do_ioctl = hns_nic_do_ioctl,
+	.ndo_set_features = hns_nic_set_features,
+	.ndo_fix_features = hns_nic_fix_features,
 	.ndo_get_stats64 = hns_nic_get_stats64,
 #ifdef CONFIG_NET_POLL_CONTROLLER
 	.ndo_poll_controller = hns_nic_poll_controller,
@@ -1315,22 +1634,26 @@ static void hns_nic_reset_subtask(struct hns_nic_priv *priv)
 		return;
 
 	hns_nic_dump(priv);
-	netdev_info(priv->netdev, "Reset %s port\n",
-		    (type == HNAE_PORT_DEBUG ? "debug" : "business"));
+	netdev_info(priv->netdev, "try to reset %s port!\n",
+		    (type == HNAE_PORT_DEBUG ? "debug" : "service"));
 
 	rtnl_lock();
 	/* put off any impending NetWatchDogTimeout */
 	priv->netdev->trans_start = jiffies;
 
-	if (type == HNAE_PORT_DEBUG)
+	if (type == HNAE_PORT_DEBUG) {
 		hns_nic_net_reinit(priv->netdev);
+	} else {
+		netif_carrier_off(priv->netdev);
+		netif_tx_disable(priv->netdev);
+	}
 	rtnl_unlock();
 }
 
 /* for doing service complete*/
 static void hns_nic_service_event_complete(struct hns_nic_priv *priv)
 {
-	assert(!test_bit(NIC_STATE_SERVICE_SCHED, &priv->state));
+	WARN_ON(!test_bit(NIC_STATE_SERVICE_SCHED, &priv->state));
 
 	smp_mb__before_atomic();
 	clear_bit(NIC_STATE_SERVICE_SCHED, &priv->state);
@@ -1435,8 +1758,9 @@ static void hns_nic_uninit_ring_data(struct hns_nic_priv *priv)
 	for (i = 0; i < h->q_num * 2; i++) {
 		netif_napi_del(&priv->ring_data[i].napi);
 		if (priv->ring_data[i].ring->irq_init_flag == RCB_IRQ_INITED) {
-			irq_set_affinity_hint(priv->ring_data[i].ring->irq,
-					      NULL);
+			(void)irq_set_affinity_hint(
+				priv->ring_data[i].ring->irq,
+				NULL);
 			free_irq(priv->ring_data[i].ring->irq,
 				 &priv->ring_data[i]);
 		}
@@ -1446,6 +1770,31 @@ static void hns_nic_uninit_ring_data(struct hns_nic_priv *priv)
 	kfree(priv->ring_data);
 }
 
+static void hns_nic_set_priv_ops(struct net_device *netdev)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_handle *h = priv->ae_handle;
+
+	if (AE_IS_VER1(priv->enet_ver)) {
+		priv->ops.fill_desc = fill_desc;
+		priv->ops.get_rxd_bnum = get_rx_desc_bnum;
+		priv->ops.maybe_stop_tx = hns_nic_maybe_stop_tx;
+	} else {
+		priv->ops.get_rxd_bnum = get_v2rx_desc_bnum;
+		if ((netdev->features & NETIF_F_TSO) ||
+		    (netdev->features & NETIF_F_TSO6)) {
+			priv->ops.fill_desc = fill_tso_desc;
+			priv->ops.maybe_stop_tx = hns_nic_maybe_stop_tso;
+			/* This chip only support 7*4096 */
+			netif_set_gso_max_size(netdev, 7 * 4096);
+			h->dev->ops->set_tso_stats(h, 1);
+		} else {
+			priv->ops.fill_desc = fill_v2_desc;
+			priv->ops.maybe_stop_tx = hns_nic_maybe_stop_tx;
+		}
+	}
+}
+
 static int hns_nic_try_get_ae(struct net_device *ndev)
 {
 	struct hns_nic_priv *priv = netdev_priv(ndev);
@@ -1453,7 +1802,7 @@ static int hns_nic_try_get_ae(struct net_device *ndev)
 	int ret;
 
 	h = hnae_get_handle(&priv->netdev->dev,
-			    priv->ae_name, priv->port_id, NULL);
+			    priv->ae_node, priv->port_id, NULL);
 	if (IS_ERR_OR_NULL(h)) {
 		ret = PTR_ERR(h);
 		dev_dbg(priv->dev, "has not handle, register notifier!\n");
@@ -1473,6 +1822,8 @@ static int hns_nic_try_get_ae(struct net_device *ndev)
 		goto out_init_ring_data;
 	}
 
+	hns_nic_set_priv_ops(ndev);
+
 	ret = register_netdev(ndev);
 	if (ret) {
 		dev_err(priv->dev, "probe register netdev fail!\n");
@@ -1524,18 +1875,21 @@ static int hns_nic_dev_probe(struct platform_device *pdev)
 	priv->dev = dev;
 	priv->netdev = ndev;
 
-	if (of_device_is_compatible(node, "hisilicon,hns-nic-v2"))
-		priv->enet_ver = AE_VERSION_2;
-	else
+	if (of_device_is_compatible(node, "hisilicon,hns-nic-v1"))
 		priv->enet_ver = AE_VERSION_1;
+	else
+		priv->enet_ver = AE_VERSION_2;
 
-	ret = of_property_read_string(node, "ae-name", &priv->ae_name);
-	if (ret)
-		goto out_read_string_fail;
+	priv->ae_node = (void *)of_parse_phandle(node, "ae-handle", 0);
+	if (IS_ERR_OR_NULL(priv->ae_node)) {
+		ret = PTR_ERR(priv->ae_node);
+		dev_err(dev, "not find ae-handle\n");
+		goto out_read_prop_fail;
+	}
 
 	ret = of_property_read_u32(node, "port-id", &priv->port_id);
 	if (ret)
-		goto out_read_string_fail;
+		goto out_read_prop_fail;
 
 	hns_init_mac_addr(ndev);
 
@@ -1543,6 +1897,7 @@ static int hns_nic_dev_probe(struct platform_device *pdev)
 	ndev->priv_flags |= IFF_UNICAST_FLT;
 	ndev->netdev_ops = &hns_nic_netdev_ops;
 	hns_ethtool_set_ops(ndev);
+
 	ndev->features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
 		NETIF_F_RXCSUM | NETIF_F_SG | NETIF_F_GSO |
 		NETIF_F_GRO;
@@ -1550,6 +1905,17 @@ static int hns_nic_dev_probe(struct platform_device *pdev)
 		NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM | NETIF_F_RXCSUM;
 	ndev->vlan_features |= NETIF_F_SG | NETIF_F_GSO | NETIF_F_GRO;
 
+	switch (priv->enet_ver) {
+	case AE_VERSION_2:
+		ndev->features |= NETIF_F_TSO | NETIF_F_TSO6;
+		ndev->hw_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
+			NETIF_F_RXCSUM | NETIF_F_SG | NETIF_F_GSO |
+			NETIF_F_GRO | NETIF_F_TSO | NETIF_F_TSO6;
+		break;
+	default:
+		break;
+	}
+
 	SET_NETDEV_DEV(ndev, dev);
 
 	if (!dma_set_mask_and_coherent(dev, DMA_BIT_MASK(64)))
@@ -1582,7 +1948,7 @@ static int hns_nic_dev_probe(struct platform_device *pdev)
 
 out_notify_fail:
 	(void)cancel_work_sync(&priv->service_task);
-out_read_string_fail:
+out_read_prop_fail:
 	free_netdev(ndev);
 	return ret;
 }
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_enet.h b/drivers/net/ethernet/hisilicon/hns/hns_enet.h
index dae0ed19ac6d..c68ab3d34fc2 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_enet.h
+++ b/drivers/net/ethernet/hisilicon/hns/hns_enet.h
@@ -40,8 +40,18 @@ struct hns_nic_ring_data {
 	void (*fini_process)(struct hns_nic_ring_data *);
 };
 
+/* compatible the difference between two versions */
+struct hns_nic_ops {
+	void (*fill_desc)(struct hnae_ring *ring, void *priv,
+			  int size, dma_addr_t dma, int frag_end,
+			  int buf_num, enum hns_desc_type type, int mtu);
+	int (*maybe_stop_tx)(struct sk_buff **out_skb,
+			     int *bnum, struct hnae_ring *ring);
+	void (*get_rxd_bnum)(u32 bnum_flag, int *out_bnum);
+};
+
 struct hns_nic_priv {
-	const char *ae_name;
+	const struct device_node *ae_node;
 	u32 enet_ver;
 	u32 port_id;
 	int phy_mode;
@@ -51,6 +61,8 @@ struct hns_nic_priv {
 	struct device *dev;
 	struct hnae_handle *ae_handle;
 
+	struct hns_nic_ops ops;
+
 	/* the cb for nic to manage the ring buffer, the first half of the
 	 * array is for tx_ring and vice versa for the second half
 	 */
diff --git a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
index a0332129970b..3c4a3bc31a89 100644
--- a/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
+++ b/drivers/net/ethernet/hisilicon/hns/hns_ethtool.c
@@ -11,7 +11,6 @@
 #include <linux/interrupt.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
-
 #include "hns_enet.h"
 
 #define HNS_PHY_PAGE_MDIX	0
@@ -72,24 +71,22 @@ static void hns_get_mdix_mode(struct net_device *net_dev,
 	struct hns_nic_priv *priv = netdev_priv(net_dev);
 	struct phy_device *phy_dev = priv->phy;
 
-	if (!phy_dev || !phy_dev->bus) {
+	if (!phy_dev || !phy_dev->mdio.bus) {
 		cmd->eth_tp_mdix_ctrl = ETH_TP_MDI_INVALID;
 		cmd->eth_tp_mdix = ETH_TP_MDI_INVALID;
 		return;
 	}
 
-	(void)mdiobus_write(phy_dev->bus, phy_dev->addr, HNS_PHY_PAGE_REG,
-			    HNS_PHY_PAGE_MDIX);
+	phy_write(phy_dev, HNS_PHY_PAGE_REG, HNS_PHY_PAGE_MDIX);
 
-	retval = mdiobus_read(phy_dev->bus, phy_dev->addr, HNS_PHY_CSC_REG);
+	retval = phy_read(phy_dev, HNS_PHY_CSC_REG);
 	mdix_ctrl = hnae_get_field(retval, PHY_MDIX_CTRL_M, PHY_MDIX_CTRL_S);
 
-	retval = mdiobus_read(phy_dev->bus, phy_dev->addr, HNS_PHY_CSS_REG);
+	retval = phy_read(phy_dev, HNS_PHY_CSS_REG);
 	mdix = hnae_get_bit(retval, PHY_MDIX_STATUS_B);
 	is_resolved = hnae_get_bit(retval, PHY_SPEED_DUP_RESOLVE_B);
 
-	(void)mdiobus_write(phy_dev->bus, phy_dev->addr, HNS_PHY_PAGE_REG,
-			    HNS_PHY_PAGE_COPPER);
+	phy_write(phy_dev, HNS_PHY_PAGE_REG, HNS_PHY_PAGE_COPPER);
 
 	switch (mdix_ctrl) {
 	case 0x0:
@@ -254,53 +251,36 @@ static int hns_nic_config_phy_loopback(struct phy_device *phy_dev, u8 en)
 
 	if (en) {
 		/* speed : 1000M */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    HNS_PHY_PAGE_REG, 2);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    21, 0x1046);
+		phy_write(phy_dev, HNS_PHY_PAGE_REG, 2);
+		phy_write(phy_dev, 21, 0x1046);
 		/* Force Master */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    9, 0x1F00);
+		phy_write(phy_dev, 9, 0x1F00);
 		/* Soft-reset */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    0, 0x9140);
+		phy_write(phy_dev, 0, 0x9140);
 		/* If autoneg disabled,two soft-reset operations */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    0, 0x9140);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    22, 0xFA);
+		phy_write(phy_dev, 0, 0x9140);
+		phy_write(phy_dev, 22, 0xFA);
 
 		/* Default is 0x0400 */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    1, 0x418);
+		phy_write(phy_dev, 1, 0x418);
 
 		/* Force 1000M Link, Default is 0x0200 */
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    7, 0x20C);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    22, 0);
+		phy_write(phy_dev, 7, 0x20C);
+		phy_write(phy_dev, 22, 0);
 
 		/* Enable MAC loop-back */
-		val = (u16)mdiobus_read(phy_dev->bus, phy_dev->addr,
-					COPPER_CONTROL_REG);
+		val = phy_read(phy_dev, COPPER_CONTROL_REG);
 		val |= PHY_LOOP_BACK;
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    COPPER_CONTROL_REG, val);
+		phy_write(phy_dev, COPPER_CONTROL_REG, val);
 	} else {
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    22, 0xFA);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    1, 0x400);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    7, 0x200);
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    22, 0);
-
-		val = (u16)mdiobus_read(phy_dev->bus, phy_dev->addr,
-					COPPER_CONTROL_REG);
+		phy_write(phy_dev, 22, 0xFA);
+		phy_write(phy_dev, 1, 0x400);
+		phy_write(phy_dev, 7, 0x200);
+		phy_write(phy_dev, 22, 0);
+
+		val = phy_read(phy_dev, COPPER_CONTROL_REG);
 		val &= ~PHY_LOOP_BACK;
-		(void)mdiobus_write(phy_dev->bus, phy_dev->addr,
-				    COPPER_CONTROL_REG, val);
+		phy_write(phy_dev, COPPER_CONTROL_REG, val);
 	}
 	return 0;
 }
@@ -315,8 +295,10 @@ static int __lb_setup(struct net_device *ndev,
 
 	switch (loop) {
 	case MAC_INTERNALLOOP_PHY:
-		if ((phy_dev) && (!phy_dev->is_c45))
+		if ((phy_dev) && (!phy_dev->is_c45)) {
 			ret = hns_nic_config_phy_loopback(phy_dev, 0x1);
+			ret |= h->dev->ops->set_loopback(h, loop, 0x1);
+		}
 		break;
 	case MAC_INTERNALLOOP_MAC:
 		if ((h->dev->ops->set_loopback) &&
@@ -396,6 +378,7 @@ static void __lb_other_process(struct hns_nic_ring_data *ring_data,
 			       struct sk_buff *skb)
 {
 	struct net_device *ndev;
+	struct hns_nic_priv *priv;
 	struct hnae_ring *ring;
 	struct netdev_queue *dev_queue;
 	struct sk_buff *new_skb;
@@ -405,8 +388,17 @@ static void __lb_other_process(struct hns_nic_ring_data *ring_data,
 	char buff[33]; /* 32B data and the last character '\0' */
 
 	if (!ring_data) { /* Just for doing create frame*/
+		ndev = skb->dev;
+		priv = netdev_priv(ndev);
+
 		frame_size = skb->len;
 		memset(skb->data, 0xFF, frame_size);
+		if ((!AE_IS_VER1(priv->enet_ver)) &&
+		    (priv->ae_handle->port_type == HNAE_PORT_SERVICE)) {
+			memcpy(skb->data, ndev->dev_addr, 6);
+			skb->data[5] += 0x1f;
+		}
+
 		frame_size &= ~1ul;
 		memset(&skb->data[frame_size / 2], 0xAA, frame_size / 2 - 1);
 		memset(&skb->data[frame_size / 2 + 10], 0xBE,
@@ -506,6 +498,7 @@ static int __lb_run_test(struct net_device *ndev,
 
 	/* place data into test skb */
 	(void)skb_put(skb, size);
+	skb->dev = ndev;
 	__lb_other_process(NULL, skb);
 	skb->queue_mapping = NIC_LB_TEST_RING_ID;
 
@@ -667,6 +660,7 @@ static void hns_nic_get_drvinfo(struct net_device *net_dev,
 	drvinfo->bus_info[ETHTOOL_BUSINFO_LEN - 1] = '\0';
 
 	strncpy(drvinfo->fw_version, "N/A", ETHTOOL_FWVERS_LEN);
+	drvinfo->eedump_len = 0;
 }
 
 /**
@@ -1018,16 +1012,9 @@ int hns_phy_led_set(struct net_device *netdev, int value)
 	struct hns_nic_priv *priv = netdev_priv(netdev);
 	struct phy_device *phy_dev = priv->phy;
 
-	if (!phy_dev->bus) {
-		netdev_err(netdev, "phy_dev->bus is null!\n");
-		return -EINVAL;
-	}
-	retval = mdiobus_write(phy_dev->bus, phy_dev->addr,
-			       HNS_PHY_PAGE_REG, HNS_PHY_PAGE_LED);
-	retval = mdiobus_write(phy_dev->bus, phy_dev->addr, HNS_LED_FC_REG,
-			       value);
-	retval = mdiobus_write(phy_dev->bus, phy_dev->addr,
-			       HNS_PHY_PAGE_REG, HNS_PHY_PAGE_COPPER);
+	retval = phy_write(phy_dev, HNS_PHY_PAGE_REG, HNS_PHY_PAGE_LED);
+	retval = phy_write(phy_dev, HNS_LED_FC_REG, value);
+	retval = phy_write(phy_dev, HNS_PHY_PAGE_REG, HNS_PHY_PAGE_COPPER);
 	if (retval) {
 		netdev_err(netdev, "mdiobus_write fail !\n");
 		return retval;
@@ -1052,19 +1039,15 @@ int hns_set_phys_id(struct net_device *netdev, enum ethtool_phys_id_state state)
 	if (phy_dev)
 		switch (state) {
 		case ETHTOOL_ID_ACTIVE:
-			ret = mdiobus_write(phy_dev->bus, phy_dev->addr,
-					    HNS_PHY_PAGE_REG,
-					    HNS_PHY_PAGE_LED);
+			ret = phy_write(phy_dev, HNS_PHY_PAGE_REG,
+					HNS_PHY_PAGE_LED);
 			if (ret)
 				return ret;
 
-			priv->phy_led_val = (u16)mdiobus_read(phy_dev->bus,
-							      phy_dev->addr,
-							      HNS_LED_FC_REG);
+			priv->phy_led_val = phy_read(phy_dev, HNS_LED_FC_REG);
 
-			ret = mdiobus_write(phy_dev->bus, phy_dev->addr,
-					    HNS_PHY_PAGE_REG,
-					    HNS_PHY_PAGE_COPPER);
+			ret = phy_write(phy_dev, HNS_PHY_PAGE_REG,
+					HNS_PHY_PAGE_COPPER);
 			if (ret)
 				return ret;
 			return 2;
@@ -1079,20 +1062,18 @@ int hns_set_phys_id(struct net_device *netdev, enum ethtool_phys_id_state state)
 				return ret;
 			break;
 		case ETHTOOL_ID_INACTIVE:
-			ret = mdiobus_write(phy_dev->bus, phy_dev->addr,
-					    HNS_PHY_PAGE_REG,
-					    HNS_PHY_PAGE_LED);
+			ret = phy_write(phy_dev, HNS_PHY_PAGE_REG,
+					HNS_PHY_PAGE_LED);
 			if (ret)
 				return ret;
 
-			ret = mdiobus_write(phy_dev->bus, phy_dev->addr,
-					    HNS_LED_FC_REG, priv->phy_led_val);
+			ret = phy_write(phy_dev, HNS_LED_FC_REG,
+					priv->phy_led_val);
 			if (ret)
 				return ret;
 
-			ret = mdiobus_write(phy_dev->bus, phy_dev->addr,
-					    HNS_PHY_PAGE_REG,
-					    HNS_PHY_PAGE_COPPER);
+			ret = phy_write(phy_dev, HNS_PHY_PAGE_REG,
+					HNS_PHY_PAGE_COPPER);
 			if (ret)
 				return ret;
 			break;
@@ -1187,6 +1168,95 @@ static int hns_nic_nway_reset(struct net_device *netdev)
 	return ret;
 }
 
+static u32
+hns_get_rss_key_size(struct net_device *netdev)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_ae_ops *ops;
+	u32 ret;
+
+	if (AE_IS_VER1(priv->enet_ver)) {
+		netdev_err(netdev,
+			   "RSS feature is not supported on this hardware\n");
+		return -EOPNOTSUPP;
+	}
+
+	ops = priv->ae_handle->dev->ops;
+	ret = ops->get_rss_key_size(priv->ae_handle);
+
+	return ret;
+}
+
+static u32
+hns_get_rss_indir_size(struct net_device *netdev)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_ae_ops *ops;
+	u32 ret;
+
+	if (AE_IS_VER1(priv->enet_ver)) {
+		netdev_err(netdev,
+			   "RSS feature is not supported on this hardware\n");
+		return -EOPNOTSUPP;
+	}
+
+	ops = priv->ae_handle->dev->ops;
+	ret = ops->get_rss_indir_size(priv->ae_handle);
+
+	return ret;
+}
+
+static int
+hns_get_rss(struct net_device *netdev, u32 *indir, u8 *key, u8 *hfunc)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_ae_ops *ops;
+	int ret;
+
+	if (AE_IS_VER1(priv->enet_ver)) {
+		netdev_err(netdev,
+			   "RSS feature is not supported on this hardware\n");
+		return -EOPNOTSUPP;
+	}
+
+	ops = priv->ae_handle->dev->ops;
+
+	if (!indir)
+		return 0;
+
+	ret = ops->get_rss(priv->ae_handle, indir, key, hfunc);
+
+	return 0;
+}
+
+static int
+hns_set_rss(struct net_device *netdev, const u32 *indir, const u8 *key,
+	    const u8 hfunc)
+{
+	struct hns_nic_priv *priv = netdev_priv(netdev);
+	struct hnae_ae_ops *ops;
+	int ret;
+
+	if (AE_IS_VER1(priv->enet_ver)) {
+		netdev_err(netdev,
+			   "RSS feature is not supported on this hardware\n");
+		return -EOPNOTSUPP;
+	}
+
+	ops = priv->ae_handle->dev->ops;
+
+	/* currently hfunc can only be Toeplitz hash */
+	if (key ||
+	    (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP))
+		return -EOPNOTSUPP;
+	if (!indir)
+		return 0;
+
+	ret = ops->set_rss(priv->ae_handle, indir, key, hfunc);
+
+	return 0;
+}
+
 static struct ethtool_ops hns_ethtool_ops = {
 	.get_drvinfo = hns_nic_get_drvinfo,
 	.get_link  = hns_nic_get_link,
@@ -1206,6 +1276,10 @@ static struct ethtool_ops hns_ethtool_ops = {
 	.get_regs_len = hns_get_regs_len,
 	.get_regs = hns_get_regs,
 	.nway_reset = hns_nic_nway_reset,
+	.get_rxfh_key_size = hns_get_rss_key_size,
+	.get_rxfh_indir_size = hns_get_rss_indir_size,
+	.get_rxfh = hns_get_rss,
+	.set_rxfh = hns_set_rss,
 };
 
 void hns_ethtool_set_ops(struct net_device *ndev)
diff --git a/drivers/net/ethernet/hisilicon/hns_mdio.c b/drivers/net/ethernet/hisilicon/hns_mdio.c
index 37491c85bc42..765ddb3dcd1a 100644
--- a/drivers/net/ethernet/hisilicon/hns_mdio.c
+++ b/drivers/net/ethernet/hisilicon/hns_mdio.c
@@ -458,16 +458,11 @@ static int hns_mdio_probe(struct platform_device *pdev)
 	}
 
 	mdio_dev->subctrl_vbase =
-		syscon_node_to_regmap(of_parse_phandle(np, "subctrl_vbase", 0));
+		syscon_node_to_regmap(of_parse_phandle(np, "subctrl-vbase", 0));
 	if (IS_ERR(mdio_dev->subctrl_vbase)) {
 		dev_warn(&pdev->dev, "no syscon hisilicon,peri-c-subctrl\n");
 		mdio_dev->subctrl_vbase = NULL;
 	}
-	new_bus->irq = devm_kcalloc(&pdev->dev, PHY_MAX_ADDR,
-				    sizeof(int), GFP_KERNEL);
-	if (!new_bus->irq)
-		return -ENOMEM;
-
 	new_bus->parent = &pdev->dev;
 	platform_set_drvdata(pdev, new_bus);
 
diff --git a/drivers/net/ethernet/hp/hp100.c b/drivers/net/ethernet/hp/hp100.c
index ae6e30d39f0f..3daf2d4a7ca0 100644
--- a/drivers/net/ethernet/hp/hp100.c
+++ b/drivers/net/ethernet/hp/hp100.c
@@ -194,7 +194,6 @@ static const char *hp100_isa_tbl[] = {
 };
 #endif
 
-#ifdef CONFIG_EISA
 static struct eisa_device_id hp100_eisa_tbl[] = {
 	{ "HWPF180" }, /* HP J2577 rev A */
 	{ "HWP1920" }, /* HP 27248B */
@@ -205,9 +204,7 @@ static struct eisa_device_id hp100_eisa_tbl[] = {
 	{ "" }	       /* Mandatory final entry ! */
 };
 MODULE_DEVICE_TABLE(eisa, hp100_eisa_tbl);
-#endif
 
-#ifdef CONFIG_PCI
 static const struct pci_device_id hp100_pci_tbl[] = {
 	{PCI_VENDOR_ID_HP, PCI_DEVICE_ID_HP_J2585A, PCI_ANY_ID, PCI_ANY_ID,},
 	{PCI_VENDOR_ID_HP, PCI_DEVICE_ID_HP_J2585B, PCI_ANY_ID, PCI_ANY_ID,},
@@ -219,7 +216,6 @@ static const struct pci_device_id hp100_pci_tbl[] = {
 	{}			/* Terminating entry */
 };
 MODULE_DEVICE_TABLE(pci, hp100_pci_tbl);
-#endif
 
 static int hp100_rx_ratio = HP100_DEFAULT_RX_RATIO;
 static int hp100_priority_tx = HP100_DEFAULT_PRIORITY_TX;
@@ -2842,8 +2838,7 @@ static void cleanup_dev(struct net_device *d)
 	free_netdev(d);
 }
 
-#ifdef CONFIG_EISA
-static int __init hp100_eisa_probe (struct device *gendev)
+static int hp100_eisa_probe(struct device *gendev)
 {
 	struct net_device *dev = alloc_etherdev(sizeof(struct hp100_private));
 	struct eisa_device *edev = to_eisa_device(gendev);
@@ -2884,9 +2879,7 @@ static struct eisa_driver hp100_eisa_driver = {
 		.remove  = hp100_eisa_remove,
         }
 };
-#endif
 
-#ifdef CONFIG_PCI
 static int hp100_pci_probe(struct pci_dev *pdev,
 			   const struct pci_device_id *ent)
 {
@@ -2955,7 +2948,6 @@ static struct pci_driver hp100_pci_driver = {
 	.probe		= hp100_pci_probe,
 	.remove		= hp100_pci_remove,
 };
-#endif
 
 /*
  *  module section
@@ -3032,23 +3024,17 @@ static int __init hp100_module_init(void)
 	err = hp100_isa_init();
 	if (err && err != -ENODEV)
 		goto out;
-#ifdef CONFIG_EISA
 	err = eisa_driver_register(&hp100_eisa_driver);
 	if (err && err != -ENODEV)
 		goto out2;
-#endif
-#ifdef CONFIG_PCI
 	err = pci_register_driver(&hp100_pci_driver);
 	if (err && err != -ENODEV)
 		goto out3;
-#endif
  out:
 	return err;
  out3:
-#ifdef CONFIG_EISA
 	eisa_driver_unregister (&hp100_eisa_driver);
  out2:
-#endif
 	hp100_isa_cleanup();
 	goto out;
 }
@@ -3057,12 +3043,8 @@ static int __init hp100_module_init(void)
 static void __exit hp100_module_exit(void)
 {
 	hp100_isa_cleanup();
-#ifdef CONFIG_EISA
 	eisa_driver_unregister (&hp100_eisa_driver);
-#endif
-#ifdef CONFIG_PCI
 	pci_unregister_driver (&hp100_pci_driver);
-#endif
 }
 
 module_init(hp100_module_init)
diff --git a/drivers/net/ethernet/ibm/Kconfig b/drivers/net/ethernet/ibm/Kconfig
index 99c1cebd002d..37dceabf8861 100644
--- a/drivers/net/ethernet/ibm/Kconfig
+++ b/drivers/net/ethernet/ibm/Kconfig
@@ -37,4 +37,14 @@ config EHEA
 	  To compile the driver as a module, choose M here. The module
 	  will be called ehea.
 
+config IBMVNIC
+	tristate "IBM Virtual NIC support"
+	depends on PPC_PSERIES
+	---help---
+	  This driver supports Virtual NIC adapters on IBM i and IBM System p
+	  systems.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called ibmvnic.
+
 endif # NET_VENDOR_IBM
diff --git a/drivers/net/ethernet/ibm/Makefile b/drivers/net/ethernet/ibm/Makefile
index 2f04e71a5926..447865c8b632 100644
--- a/drivers/net/ethernet/ibm/Makefile
+++ b/drivers/net/ethernet/ibm/Makefile
@@ -3,5 +3,6 @@
 #
 
 obj-$(CONFIG_IBMVETH) += ibmveth.o
+obj-$(CONFIG_IBMVNIC) += ibmvnic.o
 obj-$(CONFIG_IBM_EMAC) += emac/
 obj-$(CONFIG_EHEA) += ehea/
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index 7af870a3c549..ebe60719e489 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -169,7 +169,7 @@ static int ibmveth_alloc_buffer_pool(struct ibmveth_buff_pool *pool)
 	if (!pool->free_map)
 		return -1;
 
-	pool->dma_addr = kmalloc(sizeof(dma_addr_t) * pool->size, GFP_KERNEL);
+	pool->dma_addr = kcalloc(pool->size, sizeof(dma_addr_t), GFP_KERNEL);
 	if (!pool->dma_addr) {
 		kfree(pool->free_map);
 		pool->free_map = NULL;
@@ -187,8 +187,6 @@ static int ibmveth_alloc_buffer_pool(struct ibmveth_buff_pool *pool)
 		return -1;
 	}
 
-	memset(pool->dma_addr, 0, sizeof(dma_addr_t) * pool->size);
-
 	for (i = 0; i < pool->size; ++i)
 		pool->free_map[i] = i;
 
@@ -763,7 +761,7 @@ static netdev_features_t ibmveth_fix_features(struct net_device *dev,
 	 */
 
 	if (!(features & NETIF_F_RXCSUM))
-		features &= ~NETIF_F_ALL_CSUM;
+		features &= ~NETIF_F_CSUM_MASK;
 
 	return features;
 }
@@ -928,7 +926,8 @@ static int ibmveth_set_features(struct net_device *dev,
 		rc1 = ibmveth_set_csum_offload(dev, rx_csum);
 		if (rc1 && !adapter->rx_csum)
 			dev->features =
-				features & ~(NETIF_F_ALL_CSUM | NETIF_F_RXCSUM);
+				features & ~(NETIF_F_CSUM_MASK |
+					     NETIF_F_RXCSUM);
 	}
 
 	if (large_send != adapter->large_send) {
@@ -1167,7 +1166,10 @@ map_failed:
 	if (!firmware_has_feature(FW_FEATURE_CMO))
 		netdev_err(netdev, "tx: unable to map xmit buffer\n");
 	adapter->tx_map_failed++;
-	skb_linearize(skb);
+	if (skb_linearize(skb)) {
+		netdev->stats.tx_dropped++;
+		goto out;
+	}
 	force_bounce = 1;
 	goto retry_bounce;
 }
diff --git a/drivers/net/ethernet/ibm/ibmvnic.c b/drivers/net/ethernet/ibm/ibmvnic.c
new file mode 100644
index 000000000000..6e9e16eee5d0
--- /dev/null
+++ b/drivers/net/ethernet/ibm/ibmvnic.c
@@ -0,0 +1,3585 @@
+/**************************************************************************/
+/*                                                                        */
+/*  IBM System i and System p Virtual NIC Device Driver                   */
+/*  Copyright (C) 2014 IBM Corp.                                          */
+/*  Santiago Leon (santi_leon@yahoo.com)                                  */
+/*  Thomas Falcon (tlfalcon@linux.vnet.ibm.com)                           */
+/*  John Allen (jallen@linux.vnet.ibm.com)                                */
+/*                                                                        */
+/*  This program is free software; you can redistribute it and/or modify  */
+/*  it under the terms of the GNU General Public License as published by  */
+/*  the Free Software Foundation; either version 2 of the License, or     */
+/*  (at your option) any later version.                                   */
+/*                                                                        */
+/*  This program is distributed in the hope that it will be useful,       */
+/*  but WITHOUT ANY WARRANTY; without even the implied warranty of        */
+/*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         */
+/*  GNU General Public License for more details.                          */
+/*                                                                        */
+/*  You should have received a copy of the GNU General Public License     */
+/*  along with this program.                                              */
+/*                                                                        */
+/* This module contains the implementation of a virtual ethernet device   */
+/* for use with IBM i/p Series LPAR Linux. It utilizes the logical LAN    */
+/* option of the RS/6000 Platform Architecture to interface with virtual  */
+/* ethernet NICs that are presented to the partition by the hypervisor.   */
+/*									   */
+/* Messages are passed between the VNIC driver and the VNIC server using  */
+/* Command/Response Queues (CRQs) and sub CRQs (sCRQs). CRQs are used to  */
+/* issue and receive commands that initiate communication with the server */
+/* on driver initialization. Sub CRQs (sCRQs) are similar to CRQs, but    */
+/* are used by the driver to notify the server that a packet is           */
+/* ready for transmission or that a buffer has been added to receive a    */
+/* packet. Subsequently, sCRQs are used by the server to notify the       */
+/* driver that a packet transmission has been completed or that a packet  */
+/* has been received and placed in a waiting buffer.                      */
+/*                                                                        */
+/* In lieu of a more conventional "on-the-fly" DMA mapping strategy in    */
+/* which skbs are DMA mapped and immediately unmapped when the transmit   */
+/* or receive has been completed, the VNIC driver is required to use      */
+/* "long term mapping". This entails that large, continuous DMA mapped    */
+/* buffers are allocated on driver initialization and these buffers are   */
+/* then continuously reused to pass skbs to and from the VNIC server.     */
+/*                                                                        */
+/**************************************************************************/
+
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/completion.h>
+#include <linux/ioport.h>
+#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/mm.h>
+#include <linux/ethtool.h>
+#include <linux/proc_fs.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/irq.h>
+#include <linux/kthread.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/interrupt.h>
+#include <net/net_namespace.h>
+#include <asm/hvcall.h>
+#include <linux/atomic.h>
+#include <asm/vio.h>
+#include <asm/iommu.h>
+#include <linux/uaccess.h>
+#include <asm/firmware.h>
+#include <linux/seq_file.h>
+
+#include "ibmvnic.h"
+
+static const char ibmvnic_driver_name[] = "ibmvnic";
+static const char ibmvnic_driver_string[] = "IBM System i/p Virtual NIC Driver";
+
+MODULE_AUTHOR("Santiago Leon <santi_leon@yahoo.com>");
+MODULE_DESCRIPTION("IBM System i/p Virtual NIC Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(IBMVNIC_DRIVER_VERSION);
+
+static int ibmvnic_version = IBMVNIC_INITIAL_VERSION;
+static int ibmvnic_remove(struct vio_dev *);
+static void release_sub_crqs(struct ibmvnic_adapter *);
+static int ibmvnic_reset_crq(struct ibmvnic_adapter *);
+static int ibmvnic_send_crq_init(struct ibmvnic_adapter *);
+static int ibmvnic_reenable_crq_queue(struct ibmvnic_adapter *);
+static int ibmvnic_send_crq(struct ibmvnic_adapter *, union ibmvnic_crq *);
+static int send_subcrq(struct ibmvnic_adapter *adapter, u64 remote_handle,
+		       union sub_crq *sub_crq);
+static irqreturn_t ibmvnic_interrupt_rx(int irq, void *instance);
+static int enable_scrq_irq(struct ibmvnic_adapter *,
+			   struct ibmvnic_sub_crq_queue *);
+static int disable_scrq_irq(struct ibmvnic_adapter *,
+			    struct ibmvnic_sub_crq_queue *);
+static int pending_scrq(struct ibmvnic_adapter *,
+			struct ibmvnic_sub_crq_queue *);
+static union sub_crq *ibmvnic_next_scrq(struct ibmvnic_adapter *,
+					struct ibmvnic_sub_crq_queue *);
+static int ibmvnic_poll(struct napi_struct *napi, int data);
+static void send_map_query(struct ibmvnic_adapter *adapter);
+static void send_request_map(struct ibmvnic_adapter *, dma_addr_t, __be32, u8);
+static void send_request_unmap(struct ibmvnic_adapter *, u8);
+
+struct ibmvnic_stat {
+	char name[ETH_GSTRING_LEN];
+	int offset;
+};
+
+#define IBMVNIC_STAT_OFF(stat) (offsetof(struct ibmvnic_adapter, stats) + \
+			     offsetof(struct ibmvnic_statistics, stat))
+#define IBMVNIC_GET_STAT(a, off) (*((u64 *)(((unsigned long)(a)) + off)))
+
+static const struct ibmvnic_stat ibmvnic_stats[] = {
+	{"rx_packets", IBMVNIC_STAT_OFF(rx_packets)},
+	{"rx_bytes", IBMVNIC_STAT_OFF(rx_bytes)},
+	{"tx_packets", IBMVNIC_STAT_OFF(tx_packets)},
+	{"tx_bytes", IBMVNIC_STAT_OFF(tx_bytes)},
+	{"ucast_tx_packets", IBMVNIC_STAT_OFF(ucast_tx_packets)},
+	{"ucast_rx_packets", IBMVNIC_STAT_OFF(ucast_rx_packets)},
+	{"mcast_tx_packets", IBMVNIC_STAT_OFF(mcast_tx_packets)},
+	{"mcast_rx_packets", IBMVNIC_STAT_OFF(mcast_rx_packets)},
+	{"bcast_tx_packets", IBMVNIC_STAT_OFF(bcast_tx_packets)},
+	{"bcast_rx_packets", IBMVNIC_STAT_OFF(bcast_rx_packets)},
+	{"align_errors", IBMVNIC_STAT_OFF(align_errors)},
+	{"fcs_errors", IBMVNIC_STAT_OFF(fcs_errors)},
+	{"single_collision_frames", IBMVNIC_STAT_OFF(single_collision_frames)},
+	{"multi_collision_frames", IBMVNIC_STAT_OFF(multi_collision_frames)},
+	{"sqe_test_errors", IBMVNIC_STAT_OFF(sqe_test_errors)},
+	{"deferred_tx", IBMVNIC_STAT_OFF(deferred_tx)},
+	{"late_collisions", IBMVNIC_STAT_OFF(late_collisions)},
+	{"excess_collisions", IBMVNIC_STAT_OFF(excess_collisions)},
+	{"internal_mac_tx_errors", IBMVNIC_STAT_OFF(internal_mac_tx_errors)},
+	{"carrier_sense", IBMVNIC_STAT_OFF(carrier_sense)},
+	{"too_long_frames", IBMVNIC_STAT_OFF(too_long_frames)},
+	{"internal_mac_rx_errors", IBMVNIC_STAT_OFF(internal_mac_rx_errors)},
+};
+
+static long h_reg_sub_crq(unsigned long unit_address, unsigned long token,
+			  unsigned long length, unsigned long *number,
+			  unsigned long *irq)
+{
+	unsigned long retbuf[PLPAR_HCALL_BUFSIZE];
+	long rc;
+
+	rc = plpar_hcall(H_REG_SUB_CRQ, retbuf, unit_address, token, length);
+	*number = retbuf[0];
+	*irq = retbuf[1];
+
+	return rc;
+}
+
+/* net_device_ops functions */
+
+static void init_rx_pool(struct ibmvnic_adapter *adapter,
+			 struct ibmvnic_rx_pool *rx_pool, int num, int index,
+			 int buff_size, int active)
+{
+	netdev_dbg(adapter->netdev,
+		   "Initializing rx_pool %d, %d buffs, %d bytes each\n",
+		   index, num, buff_size);
+	rx_pool->size = num;
+	rx_pool->index = index;
+	rx_pool->buff_size = buff_size;
+	rx_pool->active = active;
+}
+
+static int alloc_long_term_buff(struct ibmvnic_adapter *adapter,
+				struct ibmvnic_long_term_buff *ltb, int size)
+{
+	struct device *dev = &adapter->vdev->dev;
+
+	ltb->size = size;
+	ltb->buff = dma_alloc_coherent(dev, ltb->size, &ltb->addr,
+				       GFP_KERNEL);
+
+	if (!ltb->buff) {
+		dev_err(dev, "Couldn't alloc long term buffer\n");
+		return -ENOMEM;
+	}
+	ltb->map_id = adapter->map_id;
+	adapter->map_id++;
+	send_request_map(adapter, ltb->addr,
+			 ltb->size, ltb->map_id);
+	init_completion(&adapter->fw_done);
+	wait_for_completion(&adapter->fw_done);
+	return 0;
+}
+
+static void free_long_term_buff(struct ibmvnic_adapter *adapter,
+				struct ibmvnic_long_term_buff *ltb)
+{
+	struct device *dev = &adapter->vdev->dev;
+
+	dma_free_coherent(dev, ltb->size, ltb->buff, ltb->addr);
+	send_request_unmap(adapter, ltb->map_id);
+}
+
+static int alloc_rx_pool(struct ibmvnic_adapter *adapter,
+			 struct ibmvnic_rx_pool *pool)
+{
+	struct device *dev = &adapter->vdev->dev;
+	int i;
+
+	pool->free_map = kcalloc(pool->size, sizeof(int), GFP_KERNEL);
+	if (!pool->free_map)
+		return -ENOMEM;
+
+	pool->rx_buff = kcalloc(pool->size, sizeof(struct ibmvnic_rx_buff),
+				GFP_KERNEL);
+
+	if (!pool->rx_buff) {
+		dev_err(dev, "Couldn't alloc rx buffers\n");
+		kfree(pool->free_map);
+		return -ENOMEM;
+	}
+
+	if (alloc_long_term_buff(adapter, &pool->long_term_buff,
+				 pool->size * pool->buff_size)) {
+		kfree(pool->free_map);
+		kfree(pool->rx_buff);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < pool->size; ++i)
+		pool->free_map[i] = i;
+
+	atomic_set(&pool->available, 0);
+	pool->next_alloc = 0;
+	pool->next_free = 0;
+
+	return 0;
+}
+
+static void replenish_rx_pool(struct ibmvnic_adapter *adapter,
+			      struct ibmvnic_rx_pool *pool)
+{
+	int count = pool->size - atomic_read(&pool->available);
+	struct device *dev = &adapter->vdev->dev;
+	int buffers_added = 0;
+	unsigned long lpar_rc;
+	union sub_crq sub_crq;
+	struct sk_buff *skb;
+	unsigned int offset;
+	dma_addr_t dma_addr;
+	unsigned char *dst;
+	u64 *handle_array;
+	int shift = 0;
+	int index;
+	int i;
+
+	handle_array = (u64 *)((u8 *)(adapter->login_rsp_buf) +
+				      be32_to_cpu(adapter->login_rsp_buf->
+				      off_rxadd_subcrqs));
+
+	for (i = 0; i < count; ++i) {
+		skb = alloc_skb(pool->buff_size, GFP_ATOMIC);
+		if (!skb) {
+			dev_err(dev, "Couldn't replenish rx buff\n");
+			adapter->replenish_no_mem++;
+			break;
+		}
+
+		index = pool->free_map[pool->next_free];
+
+		if (pool->rx_buff[index].skb)
+			dev_err(dev, "Inconsistent free_map!\n");
+
+		/* Copy the skb to the long term mapped DMA buffer */
+		offset = index * pool->buff_size;
+		dst = pool->long_term_buff.buff + offset;
+		memset(dst, 0, pool->buff_size);
+		dma_addr = pool->long_term_buff.addr + offset;
+		pool->rx_buff[index].data = dst;
+
+		pool->free_map[pool->next_free] = IBMVNIC_INVALID_MAP;
+		pool->rx_buff[index].dma = dma_addr;
+		pool->rx_buff[index].skb = skb;
+		pool->rx_buff[index].pool_index = pool->index;
+		pool->rx_buff[index].size = pool->buff_size;
+
+		memset(&sub_crq, 0, sizeof(sub_crq));
+		sub_crq.rx_add.first = IBMVNIC_CRQ_CMD;
+		sub_crq.rx_add.correlator =
+		    cpu_to_be64((u64)&pool->rx_buff[index]);
+		sub_crq.rx_add.ioba = cpu_to_be32(dma_addr);
+		sub_crq.rx_add.map_id = pool->long_term_buff.map_id;
+
+		/* The length field of the sCRQ is defined to be 24 bits so the
+		 * buffer size needs to be left shifted by a byte before it is
+		 * converted to big endian to prevent the last byte from being
+		 * truncated.
+		 */
+#ifdef __LITTLE_ENDIAN__
+		shift = 8;
+#endif
+		sub_crq.rx_add.len = cpu_to_be32(pool->buff_size << shift);
+
+		lpar_rc = send_subcrq(adapter, handle_array[pool->index],
+				      &sub_crq);
+		if (lpar_rc != H_SUCCESS)
+			goto failure;
+
+		buffers_added++;
+		adapter->replenish_add_buff_success++;
+		pool->next_free = (pool->next_free + 1) % pool->size;
+	}
+	atomic_add(buffers_added, &pool->available);
+	return;
+
+failure:
+	dev_info(dev, "replenish pools failure\n");
+	pool->free_map[pool->next_free] = index;
+	pool->rx_buff[index].skb = NULL;
+	if (!dma_mapping_error(dev, dma_addr))
+		dma_unmap_single(dev, dma_addr, pool->buff_size,
+				 DMA_FROM_DEVICE);
+
+	dev_kfree_skb_any(skb);
+	adapter->replenish_add_buff_failure++;
+	atomic_add(buffers_added, &pool->available);
+}
+
+static void replenish_pools(struct ibmvnic_adapter *adapter)
+{
+	int i;
+
+	if (adapter->migrated)
+		return;
+
+	adapter->replenish_task_cycles++;
+	for (i = 0; i < be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	     i++) {
+		if (adapter->rx_pool[i].active)
+			replenish_rx_pool(adapter, &adapter->rx_pool[i]);
+	}
+}
+
+static void free_rx_pool(struct ibmvnic_adapter *adapter,
+			 struct ibmvnic_rx_pool *pool)
+{
+	int i;
+
+	kfree(pool->free_map);
+	pool->free_map = NULL;
+
+	if (!pool->rx_buff)
+		return;
+
+	for (i = 0; i < pool->size; i++) {
+		if (pool->rx_buff[i].skb) {
+			dev_kfree_skb_any(pool->rx_buff[i].skb);
+			pool->rx_buff[i].skb = NULL;
+		}
+	}
+	kfree(pool->rx_buff);
+	pool->rx_buff = NULL;
+}
+
+static int ibmvnic_open(struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_tx_pool *tx_pool;
+	union ibmvnic_crq crq;
+	int rxadd_subcrqs;
+	u64 *size_array;
+	int tx_subcrqs;
+	int i, j;
+
+	rxadd_subcrqs =
+	    be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	tx_subcrqs =
+	    be32_to_cpu(adapter->login_rsp_buf->num_txsubm_subcrqs);
+	size_array = (u64 *)((u8 *)(adapter->login_rsp_buf) +
+				  be32_to_cpu(adapter->login_rsp_buf->
+					      off_rxadd_buff_size));
+	adapter->map_id = 1;
+	adapter->napi = kcalloc(adapter->req_rx_queues,
+				sizeof(struct napi_struct), GFP_KERNEL);
+	if (!adapter->napi)
+		goto alloc_napi_failed;
+	for (i = 0; i < adapter->req_rx_queues; i++) {
+		netif_napi_add(netdev, &adapter->napi[i], ibmvnic_poll,
+			       NAPI_POLL_WEIGHT);
+		napi_enable(&adapter->napi[i]);
+	}
+	adapter->rx_pool =
+	    kcalloc(rxadd_subcrqs, sizeof(struct ibmvnic_rx_pool), GFP_KERNEL);
+
+	if (!adapter->rx_pool)
+		goto rx_pool_arr_alloc_failed;
+	send_map_query(adapter);
+	for (i = 0; i < rxadd_subcrqs; i++) {
+		init_rx_pool(adapter, &adapter->rx_pool[i],
+			     IBMVNIC_BUFFS_PER_POOL, i,
+			     be64_to_cpu(size_array[i]), 1);
+		if (alloc_rx_pool(adapter, &adapter->rx_pool[i])) {
+			dev_err(dev, "Couldn't alloc rx pool\n");
+			goto rx_pool_alloc_failed;
+		}
+	}
+	adapter->tx_pool =
+	    kcalloc(tx_subcrqs, sizeof(struct ibmvnic_tx_pool), GFP_KERNEL);
+
+	if (!adapter->tx_pool)
+		goto tx_pool_arr_alloc_failed;
+	for (i = 0; i < tx_subcrqs; i++) {
+		tx_pool = &adapter->tx_pool[i];
+		tx_pool->tx_buff =
+		    kcalloc(adapter->max_tx_entries_per_subcrq,
+			    sizeof(struct ibmvnic_tx_buff), GFP_KERNEL);
+		if (!tx_pool->tx_buff)
+			goto tx_pool_alloc_failed;
+
+		if (alloc_long_term_buff(adapter, &tx_pool->long_term_buff,
+					 adapter->max_tx_entries_per_subcrq *
+					 adapter->req_mtu))
+			goto tx_ltb_alloc_failed;
+
+		tx_pool->free_map =
+		    kcalloc(adapter->max_tx_entries_per_subcrq,
+			    sizeof(int), GFP_KERNEL);
+		if (!tx_pool->free_map)
+			goto tx_fm_alloc_failed;
+
+		for (j = 0; j < adapter->max_tx_entries_per_subcrq; j++)
+			tx_pool->free_map[j] = j;
+
+		tx_pool->consumer_index = 0;
+		tx_pool->producer_index = 0;
+	}
+	adapter->bounce_buffer_size =
+	    (netdev->mtu + ETH_HLEN - 1) / PAGE_SIZE + 1;
+	adapter->bounce_buffer = kmalloc(adapter->bounce_buffer_size,
+					 GFP_KERNEL);
+	if (!adapter->bounce_buffer)
+		goto bounce_alloc_failed;
+
+	adapter->bounce_buffer_dma = dma_map_single(dev, adapter->bounce_buffer,
+						    adapter->bounce_buffer_size,
+						    DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, adapter->bounce_buffer_dma)) {
+		dev_err(dev, "Couldn't map tx bounce buffer\n");
+		goto bounce_map_failed;
+	}
+	replenish_pools(adapter);
+
+	/* We're ready to receive frames, enable the sub-crq interrupts and
+	 * set the logical link state to up
+	 */
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		enable_scrq_irq(adapter, adapter->rx_scrq[i]);
+
+	for (i = 0; i < adapter->req_tx_queues; i++)
+		enable_scrq_irq(adapter, adapter->tx_scrq[i]);
+
+	memset(&crq, 0, sizeof(crq));
+	crq.logical_link_state.first = IBMVNIC_CRQ_CMD;
+	crq.logical_link_state.cmd = LOGICAL_LINK_STATE;
+	crq.logical_link_state.link_state = IBMVNIC_LOGICAL_LNK_UP;
+	ibmvnic_send_crq(adapter, &crq);
+
+	netif_start_queue(netdev);
+	return 0;
+
+bounce_map_failed:
+	kfree(adapter->bounce_buffer);
+bounce_alloc_failed:
+	i = tx_subcrqs - 1;
+	kfree(adapter->tx_pool[i].free_map);
+tx_fm_alloc_failed:
+	free_long_term_buff(adapter, &adapter->tx_pool[i].long_term_buff);
+tx_ltb_alloc_failed:
+	kfree(adapter->tx_pool[i].tx_buff);
+tx_pool_alloc_failed:
+	for (j = 0; j < i; j++) {
+		kfree(adapter->tx_pool[j].tx_buff);
+		free_long_term_buff(adapter,
+				    &adapter->tx_pool[j].long_term_buff);
+		kfree(adapter->tx_pool[j].free_map);
+	}
+	kfree(adapter->tx_pool);
+	adapter->tx_pool = NULL;
+tx_pool_arr_alloc_failed:
+	i = rxadd_subcrqs;
+rx_pool_alloc_failed:
+	for (j = 0; j < i; j++) {
+		free_rx_pool(adapter, &adapter->rx_pool[j]);
+		free_long_term_buff(adapter,
+				    &adapter->rx_pool[j].long_term_buff);
+	}
+	kfree(adapter->rx_pool);
+	adapter->rx_pool = NULL;
+rx_pool_arr_alloc_failed:
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		napi_enable(&adapter->napi[i]);
+alloc_napi_failed:
+	return -ENOMEM;
+}
+
+static int ibmvnic_close(struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	struct device *dev = &adapter->vdev->dev;
+	union ibmvnic_crq crq;
+	int i;
+
+	adapter->closing = true;
+
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		napi_disable(&adapter->napi[i]);
+
+	netif_stop_queue(netdev);
+
+	if (adapter->bounce_buffer) {
+		if (!dma_mapping_error(dev, adapter->bounce_buffer_dma)) {
+			dma_unmap_single(&adapter->vdev->dev,
+					 adapter->bounce_buffer_dma,
+					 adapter->bounce_buffer_size,
+					 DMA_BIDIRECTIONAL);
+			adapter->bounce_buffer_dma = DMA_ERROR_CODE;
+		}
+		kfree(adapter->bounce_buffer);
+		adapter->bounce_buffer = NULL;
+	}
+
+	memset(&crq, 0, sizeof(crq));
+	crq.logical_link_state.first = IBMVNIC_CRQ_CMD;
+	crq.logical_link_state.cmd = LOGICAL_LINK_STATE;
+	crq.logical_link_state.link_state = IBMVNIC_LOGICAL_LNK_DN;
+	ibmvnic_send_crq(adapter, &crq);
+
+	for (i = 0; i < be32_to_cpu(adapter->login_rsp_buf->num_txsubm_subcrqs);
+	     i++) {
+		kfree(adapter->tx_pool[i].tx_buff);
+		free_long_term_buff(adapter,
+				    &adapter->tx_pool[i].long_term_buff);
+		kfree(adapter->tx_pool[i].free_map);
+	}
+	kfree(adapter->tx_pool);
+	adapter->tx_pool = NULL;
+
+	for (i = 0; i < be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	     i++) {
+		free_rx_pool(adapter, &adapter->rx_pool[i]);
+		free_long_term_buff(adapter,
+				    &adapter->rx_pool[i].long_term_buff);
+	}
+	kfree(adapter->rx_pool);
+	adapter->rx_pool = NULL;
+
+	adapter->closing = false;
+
+	return 0;
+}
+
+static int ibmvnic_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	int queue_num = skb_get_queue_mapping(skb);
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_tx_buff *tx_buff = NULL;
+	struct ibmvnic_tx_pool *tx_pool;
+	unsigned int tx_send_failed = 0;
+	unsigned int tx_map_failed = 0;
+	unsigned int tx_dropped = 0;
+	unsigned int tx_packets = 0;
+	unsigned int tx_bytes = 0;
+	dma_addr_t data_dma_addr;
+	struct netdev_queue *txq;
+	bool used_bounce = false;
+	unsigned long lpar_rc;
+	union sub_crq tx_crq;
+	unsigned int offset;
+	unsigned char *dst;
+	u64 *handle_array;
+	int index = 0;
+	int ret = 0;
+
+	tx_pool = &adapter->tx_pool[queue_num];
+	txq = netdev_get_tx_queue(netdev, skb_get_queue_mapping(skb));
+	handle_array = (u64 *)((u8 *)(adapter->login_rsp_buf) +
+				   be32_to_cpu(adapter->login_rsp_buf->
+					       off_txsubm_subcrqs));
+	if (adapter->migrated) {
+		tx_send_failed++;
+		tx_dropped++;
+		ret = NETDEV_TX_BUSY;
+		goto out;
+	}
+
+	index = tx_pool->free_map[tx_pool->consumer_index];
+	offset = index * adapter->req_mtu;
+	dst = tx_pool->long_term_buff.buff + offset;
+	memset(dst, 0, adapter->req_mtu);
+	skb_copy_from_linear_data(skb, dst, skb->len);
+	data_dma_addr = tx_pool->long_term_buff.addr + offset;
+
+	tx_pool->consumer_index =
+	    (tx_pool->consumer_index + 1) %
+		adapter->max_tx_entries_per_subcrq;
+
+	tx_buff = &tx_pool->tx_buff[index];
+	tx_buff->skb = skb;
+	tx_buff->data_dma[0] = data_dma_addr;
+	tx_buff->data_len[0] = skb->len;
+	tx_buff->index = index;
+	tx_buff->pool_index = queue_num;
+	tx_buff->last_frag = true;
+	tx_buff->used_bounce = used_bounce;
+
+	memset(&tx_crq, 0, sizeof(tx_crq));
+	tx_crq.v1.first = IBMVNIC_CRQ_CMD;
+	tx_crq.v1.type = IBMVNIC_TX_DESC;
+	tx_crq.v1.n_crq_elem = 1;
+	tx_crq.v1.n_sge = 1;
+	tx_crq.v1.flags1 = IBMVNIC_TX_COMP_NEEDED;
+	tx_crq.v1.correlator = cpu_to_be32(index);
+	tx_crq.v1.dma_reg = cpu_to_be16(tx_pool->long_term_buff.map_id);
+	tx_crq.v1.sge_len = cpu_to_be32(skb->len);
+	tx_crq.v1.ioba = cpu_to_be64(data_dma_addr);
+
+	if (adapter->vlan_header_insertion) {
+		tx_crq.v1.flags2 |= IBMVNIC_TX_VLAN_INSERT;
+		tx_crq.v1.vlan_id = cpu_to_be16(skb->vlan_tci);
+	}
+
+	if (skb->protocol == htons(ETH_P_IP)) {
+		if (ip_hdr(skb)->version == 4)
+			tx_crq.v1.flags1 |= IBMVNIC_TX_PROT_IPV4;
+		else if (ip_hdr(skb)->version == 6)
+			tx_crq.v1.flags1 |= IBMVNIC_TX_PROT_IPV6;
+
+		if (ip_hdr(skb)->protocol == IPPROTO_TCP)
+			tx_crq.v1.flags1 |= IBMVNIC_TX_PROT_TCP;
+		else if (ip_hdr(skb)->protocol != IPPROTO_TCP)
+			tx_crq.v1.flags1 |= IBMVNIC_TX_PROT_UDP;
+	}
+
+	if (skb->ip_summed == CHECKSUM_PARTIAL)
+		tx_crq.v1.flags1 |= IBMVNIC_TX_CHKSUM_OFFLOAD;
+
+	lpar_rc = send_subcrq(adapter, handle_array[0], &tx_crq);
+
+	if (lpar_rc != H_SUCCESS) {
+		dev_err(dev, "tx failed with code %ld\n", lpar_rc);
+
+		if (tx_pool->consumer_index == 0)
+			tx_pool->consumer_index =
+				adapter->max_tx_entries_per_subcrq - 1;
+		else
+			tx_pool->consumer_index--;
+
+		tx_send_failed++;
+		tx_dropped++;
+		ret = NETDEV_TX_BUSY;
+		goto out;
+	}
+	tx_packets++;
+	tx_bytes += skb->len;
+	txq->trans_start = jiffies;
+	ret = NETDEV_TX_OK;
+
+out:
+	netdev->stats.tx_dropped += tx_dropped;
+	netdev->stats.tx_bytes += tx_bytes;
+	netdev->stats.tx_packets += tx_packets;
+	adapter->tx_send_failed += tx_send_failed;
+	adapter->tx_map_failed += tx_map_failed;
+
+	return ret;
+}
+
+static void ibmvnic_set_multi(struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	struct netdev_hw_addr *ha;
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_capability.first = IBMVNIC_CRQ_CMD;
+	crq.request_capability.cmd = REQUEST_CAPABILITY;
+
+	if (netdev->flags & IFF_PROMISC) {
+		if (!adapter->promisc_supported)
+			return;
+	} else {
+		if (netdev->flags & IFF_ALLMULTI) {
+			/* Accept all multicast */
+			memset(&crq, 0, sizeof(crq));
+			crq.multicast_ctrl.first = IBMVNIC_CRQ_CMD;
+			crq.multicast_ctrl.cmd = MULTICAST_CTRL;
+			crq.multicast_ctrl.flags = IBMVNIC_ENABLE_ALL;
+			ibmvnic_send_crq(adapter, &crq);
+		} else if (netdev_mc_empty(netdev)) {
+			/* Reject all multicast */
+			memset(&crq, 0, sizeof(crq));
+			crq.multicast_ctrl.first = IBMVNIC_CRQ_CMD;
+			crq.multicast_ctrl.cmd = MULTICAST_CTRL;
+			crq.multicast_ctrl.flags = IBMVNIC_DISABLE_ALL;
+			ibmvnic_send_crq(adapter, &crq);
+		} else {
+			/* Accept one or more multicast(s) */
+			netdev_for_each_mc_addr(ha, netdev) {
+				memset(&crq, 0, sizeof(crq));
+				crq.multicast_ctrl.first = IBMVNIC_CRQ_CMD;
+				crq.multicast_ctrl.cmd = MULTICAST_CTRL;
+				crq.multicast_ctrl.flags = IBMVNIC_ENABLE_MC;
+				ether_addr_copy(&crq.multicast_ctrl.mac_addr[0],
+						ha->addr);
+				ibmvnic_send_crq(adapter, &crq);
+			}
+		}
+	}
+}
+
+static int ibmvnic_set_mac(struct net_device *netdev, void *p)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	struct sockaddr *addr = p;
+	union ibmvnic_crq crq;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.change_mac_addr.first = IBMVNIC_CRQ_CMD;
+	crq.change_mac_addr.cmd = CHANGE_MAC_ADDR;
+	ether_addr_copy(&crq.change_mac_addr.mac_addr[0], addr->sa_data);
+	ibmvnic_send_crq(adapter, &crq);
+	/* netdev->dev_addr is changed in handle_change_mac_rsp function */
+	return 0;
+}
+
+static int ibmvnic_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+
+	if (new_mtu > adapter->req_mtu || new_mtu < adapter->min_mtu)
+		return -EINVAL;
+
+	netdev->mtu = new_mtu;
+	return 0;
+}
+
+static void ibmvnic_tx_timeout(struct net_device *dev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(dev);
+	int rc;
+
+	/* Adapter timed out, resetting it */
+	release_sub_crqs(adapter);
+	rc = ibmvnic_reset_crq(adapter);
+	if (rc)
+		dev_err(&adapter->vdev->dev, "Adapter timeout, reset failed\n");
+	else
+		ibmvnic_send_crq_init(adapter);
+}
+
+static void remove_buff_from_pool(struct ibmvnic_adapter *adapter,
+				  struct ibmvnic_rx_buff *rx_buff)
+{
+	struct ibmvnic_rx_pool *pool = &adapter->rx_pool[rx_buff->pool_index];
+
+	rx_buff->skb = NULL;
+
+	pool->free_map[pool->next_alloc] = (int)(rx_buff - pool->rx_buff);
+	pool->next_alloc = (pool->next_alloc + 1) % pool->size;
+
+	atomic_dec(&pool->available);
+}
+
+static int ibmvnic_poll(struct napi_struct *napi, int budget)
+{
+	struct net_device *netdev = napi->dev;
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	int scrq_num = (int)(napi - adapter->napi);
+	int frames_processed = 0;
+restart_poll:
+	while (frames_processed < budget) {
+		struct sk_buff *skb;
+		struct ibmvnic_rx_buff *rx_buff;
+		union sub_crq *next;
+		u32 length;
+		u16 offset;
+		u8 flags = 0;
+
+		if (!pending_scrq(adapter, adapter->rx_scrq[scrq_num]))
+			break;
+		next = ibmvnic_next_scrq(adapter, adapter->rx_scrq[scrq_num]);
+		rx_buff =
+		    (struct ibmvnic_rx_buff *)be64_to_cpu(next->
+							  rx_comp.correlator);
+		/* do error checking */
+		if (next->rx_comp.rc) {
+			netdev_err(netdev, "rx error %x\n", next->rx_comp.rc);
+			/* free the entry */
+			next->rx_comp.first = 0;
+			remove_buff_from_pool(adapter, rx_buff);
+			break;
+		}
+
+		length = be32_to_cpu(next->rx_comp.len);
+		offset = be16_to_cpu(next->rx_comp.off_frame_data);
+		flags = next->rx_comp.flags;
+		skb = rx_buff->skb;
+		skb_copy_to_linear_data(skb, rx_buff->data + offset,
+					length);
+		skb->vlan_tci = be16_to_cpu(next->rx_comp.vlan_tci);
+		/* free the entry */
+		next->rx_comp.first = 0;
+		remove_buff_from_pool(adapter, rx_buff);
+
+		skb_put(skb, length);
+		skb->protocol = eth_type_trans(skb, netdev);
+
+		if (flags & IBMVNIC_IP_CHKSUM_GOOD &&
+		    flags & IBMVNIC_TCP_UDP_CHKSUM_GOOD) {
+			skb->ip_summed = CHECKSUM_UNNECESSARY;
+		}
+
+		length = skb->len;
+		napi_gro_receive(napi, skb); /* send it up */
+		netdev->stats.rx_packets++;
+		netdev->stats.rx_bytes += length;
+		frames_processed++;
+	}
+	replenish_pools(adapter);
+
+	if (frames_processed < budget) {
+		enable_scrq_irq(adapter, adapter->rx_scrq[scrq_num]);
+		napi_complete(napi);
+		if (pending_scrq(adapter, adapter->rx_scrq[scrq_num]) &&
+		    napi_reschedule(napi)) {
+			disable_scrq_irq(adapter, adapter->rx_scrq[scrq_num]);
+			goto restart_poll;
+		}
+	}
+	return frames_processed;
+}
+
+#ifdef CONFIG_NET_POLL_CONTROLLER
+static void ibmvnic_netpoll_controller(struct net_device *dev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(dev);
+	int i;
+
+	replenish_pools(netdev_priv(dev));
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		ibmvnic_interrupt_rx(adapter->rx_scrq[i]->irq,
+				     adapter->rx_scrq[i]);
+}
+#endif
+
+static const struct net_device_ops ibmvnic_netdev_ops = {
+	.ndo_open		= ibmvnic_open,
+	.ndo_stop		= ibmvnic_close,
+	.ndo_start_xmit		= ibmvnic_xmit,
+	.ndo_set_rx_mode	= ibmvnic_set_multi,
+	.ndo_set_mac_address	= ibmvnic_set_mac,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_change_mtu		= ibmvnic_change_mtu,
+	.ndo_tx_timeout		= ibmvnic_tx_timeout,
+#ifdef CONFIG_NET_POLL_CONTROLLER
+	.ndo_poll_controller	= ibmvnic_netpoll_controller,
+#endif
+};
+
+/* ethtool functions */
+
+static int ibmvnic_get_settings(struct net_device *netdev,
+				struct ethtool_cmd *cmd)
+{
+	cmd->supported = (SUPPORTED_1000baseT_Full | SUPPORTED_Autoneg |
+			  SUPPORTED_FIBRE);
+	cmd->advertising = (ADVERTISED_1000baseT_Full | ADVERTISED_Autoneg |
+			    ADVERTISED_FIBRE);
+	ethtool_cmd_speed_set(cmd, SPEED_1000);
+	cmd->duplex = DUPLEX_FULL;
+	cmd->port = PORT_FIBRE;
+	cmd->phy_address = 0;
+	cmd->transceiver = XCVR_INTERNAL;
+	cmd->autoneg = AUTONEG_ENABLE;
+	cmd->maxtxpkt = 0;
+	cmd->maxrxpkt = 1;
+	return 0;
+}
+
+static void ibmvnic_get_drvinfo(struct net_device *dev,
+				struct ethtool_drvinfo *info)
+{
+	strlcpy(info->driver, ibmvnic_driver_name, sizeof(info->driver));
+	strlcpy(info->version, IBMVNIC_DRIVER_VERSION, sizeof(info->version));
+}
+
+static u32 ibmvnic_get_msglevel(struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+
+	return adapter->msg_enable;
+}
+
+static void ibmvnic_set_msglevel(struct net_device *netdev, u32 data)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+
+	adapter->msg_enable = data;
+}
+
+static u32 ibmvnic_get_link(struct net_device *netdev)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+
+	/* Don't need to send a query because we request a logical link up at
+	 * init and then we wait for link state indications
+	 */
+	return adapter->logical_link_state;
+}
+
+static void ibmvnic_get_ringparam(struct net_device *netdev,
+				  struct ethtool_ringparam *ring)
+{
+	ring->rx_max_pending = 0;
+	ring->tx_max_pending = 0;
+	ring->rx_mini_max_pending = 0;
+	ring->rx_jumbo_max_pending = 0;
+	ring->rx_pending = 0;
+	ring->tx_pending = 0;
+	ring->rx_mini_pending = 0;
+	ring->rx_jumbo_pending = 0;
+}
+
+static void ibmvnic_get_strings(struct net_device *dev, u32 stringset, u8 *data)
+{
+	int i;
+
+	if (stringset != ETH_SS_STATS)
+		return;
+
+	for (i = 0; i < ARRAY_SIZE(ibmvnic_stats); i++, data += ETH_GSTRING_LEN)
+		memcpy(data, ibmvnic_stats[i].name, ETH_GSTRING_LEN);
+}
+
+static int ibmvnic_get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return ARRAY_SIZE(ibmvnic_stats);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void ibmvnic_get_ethtool_stats(struct net_device *dev,
+				      struct ethtool_stats *stats, u64 *data)
+{
+	struct ibmvnic_adapter *adapter = netdev_priv(dev);
+	union ibmvnic_crq crq;
+	int i;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_statistics.first = IBMVNIC_CRQ_CMD;
+	crq.request_statistics.cmd = REQUEST_STATISTICS;
+	crq.request_statistics.ioba = cpu_to_be32(adapter->stats_token);
+	crq.request_statistics.len =
+	    cpu_to_be32(sizeof(struct ibmvnic_statistics));
+	ibmvnic_send_crq(adapter, &crq);
+
+	/* Wait for data to be written */
+	init_completion(&adapter->stats_done);
+	wait_for_completion(&adapter->stats_done);
+
+	for (i = 0; i < ARRAY_SIZE(ibmvnic_stats); i++)
+		data[i] = IBMVNIC_GET_STAT(adapter, ibmvnic_stats[i].offset);
+}
+
+static const struct ethtool_ops ibmvnic_ethtool_ops = {
+	.get_settings		= ibmvnic_get_settings,
+	.get_drvinfo		= ibmvnic_get_drvinfo,
+	.get_msglevel		= ibmvnic_get_msglevel,
+	.set_msglevel		= ibmvnic_set_msglevel,
+	.get_link		= ibmvnic_get_link,
+	.get_ringparam		= ibmvnic_get_ringparam,
+	.get_strings            = ibmvnic_get_strings,
+	.get_sset_count         = ibmvnic_get_sset_count,
+	.get_ethtool_stats	= ibmvnic_get_ethtool_stats,
+};
+
+/* Routines for managing CRQs/sCRQs  */
+
+static void release_sub_crq_queue(struct ibmvnic_adapter *adapter,
+				  struct ibmvnic_sub_crq_queue *scrq)
+{
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	netdev_dbg(adapter->netdev, "Releasing sub-CRQ\n");
+
+	/* Close the sub-crqs */
+	do {
+		rc = plpar_hcall_norets(H_FREE_SUB_CRQ,
+					adapter->vdev->unit_address,
+					scrq->crq_num);
+	} while (rc == H_BUSY || H_IS_LONG_BUSY(rc));
+
+	dma_unmap_single(dev, scrq->msg_token, 4 * PAGE_SIZE,
+			 DMA_BIDIRECTIONAL);
+	free_pages((unsigned long)scrq->msgs, 2);
+	kfree(scrq);
+}
+
+static struct ibmvnic_sub_crq_queue *init_sub_crq_queue(struct ibmvnic_adapter
+							*adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_sub_crq_queue *scrq;
+	int rc;
+
+	scrq = kmalloc(sizeof(*scrq), GFP_ATOMIC);
+	if (!scrq)
+		return NULL;
+
+	scrq->msgs = (union sub_crq *)__get_free_pages(GFP_KERNEL, 2);
+	memset(scrq->msgs, 0, 4 * PAGE_SIZE);
+	if (!scrq->msgs) {
+		dev_warn(dev, "Couldn't allocate crq queue messages page\n");
+		goto zero_page_failed;
+	}
+
+	scrq->msg_token = dma_map_single(dev, scrq->msgs, 4 * PAGE_SIZE,
+					 DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, scrq->msg_token)) {
+		dev_warn(dev, "Couldn't map crq queue messages page\n");
+		goto map_failed;
+	}
+
+	rc = h_reg_sub_crq(adapter->vdev->unit_address, scrq->msg_token,
+			   4 * PAGE_SIZE, &scrq->crq_num, &scrq->hw_irq);
+
+	if (rc == H_RESOURCE)
+		rc = ibmvnic_reset_crq(adapter);
+
+	if (rc == H_CLOSED) {
+		dev_warn(dev, "Partner adapter not ready, waiting.\n");
+	} else if (rc) {
+		dev_warn(dev, "Error %d registering sub-crq\n", rc);
+		goto reg_failed;
+	}
+
+	scrq->irq = irq_create_mapping(NULL, scrq->hw_irq);
+	if (scrq->irq == NO_IRQ) {
+		dev_err(dev, "Error mapping irq\n");
+		goto map_irq_failed;
+	}
+
+	scrq->adapter = adapter;
+	scrq->size = 4 * PAGE_SIZE / sizeof(*scrq->msgs);
+	scrq->cur = 0;
+	scrq->rx_skb_top = NULL;
+	spin_lock_init(&scrq->lock);
+
+	netdev_dbg(adapter->netdev,
+		   "sub-crq initialized, num %lx, hw_irq=%lx, irq=%x\n",
+		   scrq->crq_num, scrq->hw_irq, scrq->irq);
+
+	return scrq;
+
+map_irq_failed:
+	do {
+		rc = plpar_hcall_norets(H_FREE_SUB_CRQ,
+					adapter->vdev->unit_address,
+					scrq->crq_num);
+	} while (rc == H_BUSY || H_IS_LONG_BUSY(rc));
+reg_failed:
+	dma_unmap_single(dev, scrq->msg_token, 4 * PAGE_SIZE,
+			 DMA_BIDIRECTIONAL);
+map_failed:
+	free_pages((unsigned long)scrq->msgs, 2);
+zero_page_failed:
+	kfree(scrq);
+
+	return NULL;
+}
+
+static void release_sub_crqs(struct ibmvnic_adapter *adapter)
+{
+	int i;
+
+	if (adapter->tx_scrq) {
+		for (i = 0; i < adapter->req_tx_queues; i++)
+			if (adapter->tx_scrq[i]) {
+				free_irq(adapter->tx_scrq[i]->irq,
+					 adapter->tx_scrq[i]);
+				release_sub_crq_queue(adapter,
+						      adapter->tx_scrq[i]);
+			}
+		adapter->tx_scrq = NULL;
+	}
+
+	if (adapter->rx_scrq) {
+		for (i = 0; i < adapter->req_rx_queues; i++)
+			if (adapter->rx_scrq[i]) {
+				free_irq(adapter->rx_scrq[i]->irq,
+					 adapter->rx_scrq[i]);
+				release_sub_crq_queue(adapter,
+						      adapter->rx_scrq[i]);
+			}
+		adapter->rx_scrq = NULL;
+	}
+
+	adapter->requested_caps = 0;
+}
+
+static int disable_scrq_irq(struct ibmvnic_adapter *adapter,
+			    struct ibmvnic_sub_crq_queue *scrq)
+{
+	struct device *dev = &adapter->vdev->dev;
+	unsigned long rc;
+
+	rc = plpar_hcall_norets(H_VIOCTL, adapter->vdev->unit_address,
+				H_DISABLE_VIO_INTERRUPT, scrq->hw_irq, 0, 0);
+	if (rc)
+		dev_err(dev, "Couldn't disable scrq irq 0x%lx. rc=%ld\n",
+			scrq->hw_irq, rc);
+	return rc;
+}
+
+static int enable_scrq_irq(struct ibmvnic_adapter *adapter,
+			   struct ibmvnic_sub_crq_queue *scrq)
+{
+	struct device *dev = &adapter->vdev->dev;
+	unsigned long rc;
+
+	if (scrq->hw_irq > 0x100000000ULL) {
+		dev_err(dev, "bad hw_irq = %lx\n", scrq->hw_irq);
+		return 1;
+	}
+
+	rc = plpar_hcall_norets(H_VIOCTL, adapter->vdev->unit_address,
+				H_ENABLE_VIO_INTERRUPT, scrq->hw_irq, 0, 0);
+	if (rc)
+		dev_err(dev, "Couldn't enable scrq irq 0x%lx. rc=%ld\n",
+			scrq->hw_irq, rc);
+	return rc;
+}
+
+static int ibmvnic_complete_tx(struct ibmvnic_adapter *adapter,
+			       struct ibmvnic_sub_crq_queue *scrq)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_tx_buff *txbuff;
+	union sub_crq *next;
+	int index;
+	int i, j;
+
+restart_loop:
+	while (pending_scrq(adapter, scrq)) {
+		unsigned int pool = scrq->pool_index;
+
+		next = ibmvnic_next_scrq(adapter, scrq);
+		for (i = 0; i < next->tx_comp.num_comps; i++) {
+			if (next->tx_comp.rcs[i]) {
+				dev_err(dev, "tx error %x\n",
+					next->tx_comp.rcs[i]);
+				continue;
+			}
+			index = be32_to_cpu(next->tx_comp.correlators[i]);
+			txbuff = &adapter->tx_pool[pool].tx_buff[index];
+
+			for (j = 0; j < IBMVNIC_MAX_FRAGS_PER_CRQ; j++) {
+				if (!txbuff->data_dma[j])
+					continue;
+
+				txbuff->data_dma[j] = 0;
+				txbuff->used_bounce = false;
+			}
+
+			if (txbuff->last_frag)
+				dev_kfree_skb_any(txbuff->skb);
+
+			adapter->tx_pool[pool].free_map[adapter->tx_pool[pool].
+						     producer_index] = index;
+			adapter->tx_pool[pool].producer_index =
+			    (adapter->tx_pool[pool].producer_index + 1) %
+			    adapter->max_tx_entries_per_subcrq;
+		}
+		/* remove tx_comp scrq*/
+		next->tx_comp.first = 0;
+	}
+
+	enable_scrq_irq(adapter, scrq);
+
+	if (pending_scrq(adapter, scrq)) {
+		disable_scrq_irq(adapter, scrq);
+		goto restart_loop;
+	}
+
+	return 0;
+}
+
+static irqreturn_t ibmvnic_interrupt_tx(int irq, void *instance)
+{
+	struct ibmvnic_sub_crq_queue *scrq = instance;
+	struct ibmvnic_adapter *adapter = scrq->adapter;
+
+	disable_scrq_irq(adapter, scrq);
+	ibmvnic_complete_tx(adapter, scrq);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ibmvnic_interrupt_rx(int irq, void *instance)
+{
+	struct ibmvnic_sub_crq_queue *scrq = instance;
+	struct ibmvnic_adapter *adapter = scrq->adapter;
+
+	if (napi_schedule_prep(&adapter->napi[scrq->scrq_num])) {
+		disable_scrq_irq(adapter, scrq);
+		__napi_schedule(&adapter->napi[scrq->scrq_num]);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void init_sub_crqs(struct ibmvnic_adapter *adapter, int retry)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_sub_crq_queue **allqueues;
+	int registered_queues = 0;
+	union ibmvnic_crq crq;
+	int total_queues;
+	int more = 0;
+	int i, j;
+	int rc;
+
+	if (!retry) {
+		/* Sub-CRQ entries are 32 byte long */
+		int entries_page = 4 * PAGE_SIZE / (sizeof(u64) * 4);
+
+		if (adapter->min_tx_entries_per_subcrq > entries_page ||
+		    adapter->min_rx_add_entries_per_subcrq > entries_page) {
+			dev_err(dev, "Fatal, invalid entries per sub-crq\n");
+			goto allqueues_failed;
+		}
+
+		/* Get the minimum between the queried max and the entries
+		 * that fit in our PAGE_SIZE
+		 */
+		adapter->req_tx_entries_per_subcrq =
+		    adapter->max_tx_entries_per_subcrq > entries_page ?
+		    entries_page : adapter->max_tx_entries_per_subcrq;
+		adapter->req_rx_add_entries_per_subcrq =
+		    adapter->max_rx_add_entries_per_subcrq > entries_page ?
+		    entries_page : adapter->max_rx_add_entries_per_subcrq;
+
+		/* Choosing the maximum number of queues supported by firmware*/
+		adapter->req_tx_queues = adapter->min_tx_queues;
+		adapter->req_rx_queues = adapter->min_rx_queues;
+		adapter->req_rx_add_queues = adapter->min_rx_add_queues;
+
+		adapter->req_mtu = adapter->max_mtu;
+	}
+
+	total_queues = adapter->req_tx_queues + adapter->req_rx_queues;
+
+	allqueues = kcalloc(total_queues, sizeof(*allqueues), GFP_ATOMIC);
+	if (!allqueues)
+		goto allqueues_failed;
+
+	for (i = 0; i < total_queues; i++) {
+		allqueues[i] = init_sub_crq_queue(adapter);
+		if (!allqueues[i]) {
+			dev_warn(dev, "Couldn't allocate all sub-crqs\n");
+			break;
+		}
+		registered_queues++;
+	}
+
+	/* Make sure we were able to register the minimum number of queues */
+	if (registered_queues <
+	    adapter->min_tx_queues + adapter->min_rx_queues) {
+		dev_err(dev, "Fatal: Couldn't init  min number of sub-crqs\n");
+		goto tx_failed;
+	}
+
+	/* Distribute the failed allocated queues*/
+	for (i = 0; i < total_queues - registered_queues + more ; i++) {
+		netdev_dbg(adapter->netdev, "Reducing number of queues\n");
+		switch (i % 3) {
+		case 0:
+			if (adapter->req_rx_queues > adapter->min_rx_queues)
+				adapter->req_rx_queues--;
+			else
+				more++;
+			break;
+		case 1:
+			if (adapter->req_tx_queues > adapter->min_tx_queues)
+				adapter->req_tx_queues--;
+			else
+				more++;
+			break;
+		}
+	}
+
+	adapter->tx_scrq = kcalloc(adapter->req_tx_queues,
+				   sizeof(*adapter->tx_scrq), GFP_ATOMIC);
+	if (!adapter->tx_scrq)
+		goto tx_failed;
+
+	for (i = 0; i < adapter->req_tx_queues; i++) {
+		adapter->tx_scrq[i] = allqueues[i];
+		adapter->tx_scrq[i]->pool_index = i;
+		rc = request_irq(adapter->tx_scrq[i]->irq, ibmvnic_interrupt_tx,
+				 0, "ibmvnic_tx", adapter->tx_scrq[i]);
+		if (rc) {
+			dev_err(dev, "Couldn't register tx irq 0x%x. rc=%d\n",
+				adapter->tx_scrq[i]->irq, rc);
+			goto req_tx_irq_failed;
+		}
+	}
+
+	adapter->rx_scrq = kcalloc(adapter->req_rx_queues,
+				   sizeof(*adapter->rx_scrq), GFP_ATOMIC);
+	if (!adapter->rx_scrq)
+		goto rx_failed;
+
+	for (i = 0; i < adapter->req_rx_queues; i++) {
+		adapter->rx_scrq[i] = allqueues[i + adapter->req_tx_queues];
+		adapter->rx_scrq[i]->scrq_num = i;
+		rc = request_irq(adapter->rx_scrq[i]->irq, ibmvnic_interrupt_rx,
+				 0, "ibmvnic_rx", adapter->rx_scrq[i]);
+		if (rc) {
+			dev_err(dev, "Couldn't register rx irq 0x%x. rc=%d\n",
+				adapter->rx_scrq[i]->irq, rc);
+			goto req_rx_irq_failed;
+		}
+	}
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_capability.first = IBMVNIC_CRQ_CMD;
+	crq.request_capability.cmd = REQUEST_CAPABILITY;
+
+	crq.request_capability.capability = cpu_to_be16(REQ_TX_QUEUES);
+	crq.request_capability.number = cpu_to_be64(adapter->req_tx_queues);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.request_capability.capability = cpu_to_be16(REQ_RX_QUEUES);
+	crq.request_capability.number = cpu_to_be64(adapter->req_rx_queues);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.request_capability.capability = cpu_to_be16(REQ_RX_ADD_QUEUES);
+	crq.request_capability.number = cpu_to_be64(adapter->req_rx_add_queues);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.request_capability.capability =
+	    cpu_to_be16(REQ_TX_ENTRIES_PER_SUBCRQ);
+	crq.request_capability.number =
+	    cpu_to_be64(adapter->req_tx_entries_per_subcrq);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.request_capability.capability =
+	    cpu_to_be16(REQ_RX_ADD_ENTRIES_PER_SUBCRQ);
+	crq.request_capability.number =
+	    cpu_to_be64(adapter->req_rx_add_entries_per_subcrq);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.request_capability.capability = cpu_to_be16(REQ_MTU);
+	crq.request_capability.number = cpu_to_be64(adapter->req_mtu);
+	ibmvnic_send_crq(adapter, &crq);
+
+	if (adapter->netdev->flags & IFF_PROMISC) {
+		if (adapter->promisc_supported) {
+			crq.request_capability.capability =
+			    cpu_to_be16(PROMISC_REQUESTED);
+			crq.request_capability.number = cpu_to_be64(1);
+			ibmvnic_send_crq(adapter, &crq);
+		}
+	} else {
+		crq.request_capability.capability =
+		    cpu_to_be16(PROMISC_REQUESTED);
+		crq.request_capability.number = cpu_to_be64(0);
+		ibmvnic_send_crq(adapter, &crq);
+	}
+
+	kfree(allqueues);
+
+	return;
+
+req_rx_irq_failed:
+	for (j = 0; j < i; j++)
+		free_irq(adapter->rx_scrq[j]->irq, adapter->rx_scrq[j]);
+	i = adapter->req_tx_queues;
+req_tx_irq_failed:
+	for (j = 0; j < i; j++)
+		free_irq(adapter->tx_scrq[j]->irq, adapter->tx_scrq[j]);
+	kfree(adapter->rx_scrq);
+	adapter->rx_scrq = NULL;
+rx_failed:
+	kfree(adapter->tx_scrq);
+	adapter->tx_scrq = NULL;
+tx_failed:
+	for (i = 0; i < registered_queues; i++)
+		release_sub_crq_queue(adapter, allqueues[i]);
+	kfree(allqueues);
+allqueues_failed:
+	ibmvnic_remove(adapter->vdev);
+}
+
+static int pending_scrq(struct ibmvnic_adapter *adapter,
+			struct ibmvnic_sub_crq_queue *scrq)
+{
+	union sub_crq *entry = &scrq->msgs[scrq->cur];
+
+	if (entry->generic.first & IBMVNIC_CRQ_CMD_RSP || adapter->closing)
+		return 1;
+	else
+		return 0;
+}
+
+static union sub_crq *ibmvnic_next_scrq(struct ibmvnic_adapter *adapter,
+					struct ibmvnic_sub_crq_queue *scrq)
+{
+	union sub_crq *entry;
+	unsigned long flags;
+
+	spin_lock_irqsave(&scrq->lock, flags);
+	entry = &scrq->msgs[scrq->cur];
+	if (entry->generic.first & IBMVNIC_CRQ_CMD_RSP) {
+		if (++scrq->cur == scrq->size)
+			scrq->cur = 0;
+	} else {
+		entry = NULL;
+	}
+	spin_unlock_irqrestore(&scrq->lock, flags);
+
+	return entry;
+}
+
+static union ibmvnic_crq *ibmvnic_next_crq(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_crq_queue *queue = &adapter->crq;
+	union ibmvnic_crq *crq;
+
+	crq = &queue->msgs[queue->cur];
+	if (crq->generic.first & IBMVNIC_CRQ_CMD_RSP) {
+		if (++queue->cur == queue->size)
+			queue->cur = 0;
+	} else {
+		crq = NULL;
+	}
+
+	return crq;
+}
+
+static int send_subcrq(struct ibmvnic_adapter *adapter, u64 remote_handle,
+		       union sub_crq *sub_crq)
+{
+	unsigned int ua = adapter->vdev->unit_address;
+	struct device *dev = &adapter->vdev->dev;
+	u64 *u64_crq = (u64 *)sub_crq;
+	int rc;
+
+	netdev_dbg(adapter->netdev,
+		   "Sending sCRQ %016lx: %016lx %016lx %016lx %016lx\n",
+		   (unsigned long int)cpu_to_be64(remote_handle),
+		   (unsigned long int)cpu_to_be64(u64_crq[0]),
+		   (unsigned long int)cpu_to_be64(u64_crq[1]),
+		   (unsigned long int)cpu_to_be64(u64_crq[2]),
+		   (unsigned long int)cpu_to_be64(u64_crq[3]));
+
+	/* Make sure the hypervisor sees the complete request */
+	mb();
+
+	rc = plpar_hcall_norets(H_SEND_SUB_CRQ, ua,
+				cpu_to_be64(remote_handle),
+				cpu_to_be64(u64_crq[0]),
+				cpu_to_be64(u64_crq[1]),
+				cpu_to_be64(u64_crq[2]),
+				cpu_to_be64(u64_crq[3]));
+
+	if (rc) {
+		if (rc == H_CLOSED)
+			dev_warn(dev, "CRQ Queue closed\n");
+		dev_err(dev, "Send error (rc=%d)\n", rc);
+	}
+
+	return rc;
+}
+
+static int ibmvnic_send_crq(struct ibmvnic_adapter *adapter,
+			    union ibmvnic_crq *crq)
+{
+	unsigned int ua = adapter->vdev->unit_address;
+	struct device *dev = &adapter->vdev->dev;
+	u64 *u64_crq = (u64 *)crq;
+	int rc;
+
+	netdev_dbg(adapter->netdev, "Sending CRQ: %016lx %016lx\n",
+		   (unsigned long int)cpu_to_be64(u64_crq[0]),
+		   (unsigned long int)cpu_to_be64(u64_crq[1]));
+
+	/* Make sure the hypervisor sees the complete request */
+	mb();
+
+	rc = plpar_hcall_norets(H_SEND_CRQ, ua,
+				cpu_to_be64(u64_crq[0]),
+				cpu_to_be64(u64_crq[1]));
+
+	if (rc) {
+		if (rc == H_CLOSED)
+			dev_warn(dev, "CRQ Queue closed\n");
+		dev_warn(dev, "Send error (rc=%d)\n", rc);
+	}
+
+	return rc;
+}
+
+static int ibmvnic_send_crq_init(struct ibmvnic_adapter *adapter)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.generic.first = IBMVNIC_CRQ_INIT_CMD;
+	crq.generic.cmd = IBMVNIC_CRQ_INIT;
+	netdev_dbg(adapter->netdev, "Sending CRQ init\n");
+
+	return ibmvnic_send_crq(adapter, &crq);
+}
+
+static int ibmvnic_send_crq_init_complete(struct ibmvnic_adapter *adapter)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.generic.first = IBMVNIC_CRQ_INIT_CMD;
+	crq.generic.cmd = IBMVNIC_CRQ_INIT_COMPLETE;
+	netdev_dbg(adapter->netdev, "Sending CRQ init complete\n");
+
+	return ibmvnic_send_crq(adapter, &crq);
+}
+
+static int send_version_xchg(struct ibmvnic_adapter *adapter)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.version_exchange.first = IBMVNIC_CRQ_CMD;
+	crq.version_exchange.cmd = VERSION_EXCHANGE;
+	crq.version_exchange.version = cpu_to_be16(ibmvnic_version);
+
+	return ibmvnic_send_crq(adapter, &crq);
+}
+
+static void send_login(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_login_rsp_buffer *login_rsp_buffer;
+	struct ibmvnic_login_buffer *login_buffer;
+	struct ibmvnic_inflight_cmd *inflight_cmd;
+	struct device *dev = &adapter->vdev->dev;
+	dma_addr_t rsp_buffer_token;
+	dma_addr_t buffer_token;
+	size_t rsp_buffer_size;
+	union ibmvnic_crq crq;
+	unsigned long flags;
+	size_t buffer_size;
+	__be64 *tx_list_p;
+	__be64 *rx_list_p;
+	int i;
+
+	buffer_size =
+	    sizeof(struct ibmvnic_login_buffer) +
+	    sizeof(u64) * (adapter->req_tx_queues + adapter->req_rx_queues);
+
+	login_buffer = kmalloc(buffer_size, GFP_ATOMIC);
+	if (!login_buffer)
+		goto buf_alloc_failed;
+
+	buffer_token = dma_map_single(dev, login_buffer, buffer_size,
+				      DMA_TO_DEVICE);
+	if (dma_mapping_error(dev, buffer_token)) {
+		dev_err(dev, "Couldn't map login buffer\n");
+		goto buf_map_failed;
+	}
+
+	rsp_buffer_size =
+	    sizeof(struct ibmvnic_login_rsp_buffer) +
+	    sizeof(u64) * (adapter->req_tx_queues +
+			   adapter->req_rx_queues *
+			   adapter->req_rx_add_queues + adapter->
+			   req_rx_add_queues) +
+	    sizeof(u8) * (IBMVNIC_TX_DESC_VERSIONS);
+
+	login_rsp_buffer = kmalloc(rsp_buffer_size, GFP_ATOMIC);
+	if (!login_rsp_buffer)
+		goto buf_rsp_alloc_failed;
+
+	rsp_buffer_token = dma_map_single(dev, login_rsp_buffer,
+					  rsp_buffer_size, DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, rsp_buffer_token)) {
+		dev_err(dev, "Couldn't map login rsp buffer\n");
+		goto buf_rsp_map_failed;
+	}
+	inflight_cmd = kmalloc(sizeof(*inflight_cmd), GFP_ATOMIC);
+	if (!inflight_cmd) {
+		dev_err(dev, "Couldn't allocate inflight_cmd\n");
+		goto inflight_alloc_failed;
+	}
+	adapter->login_buf = login_buffer;
+	adapter->login_buf_token = buffer_token;
+	adapter->login_buf_sz = buffer_size;
+	adapter->login_rsp_buf = login_rsp_buffer;
+	adapter->login_rsp_buf_token = rsp_buffer_token;
+	adapter->login_rsp_buf_sz = rsp_buffer_size;
+
+	login_buffer->len = cpu_to_be32(buffer_size);
+	login_buffer->version = cpu_to_be32(INITIAL_VERSION_LB);
+	login_buffer->num_txcomp_subcrqs = cpu_to_be32(adapter->req_tx_queues);
+	login_buffer->off_txcomp_subcrqs =
+	    cpu_to_be32(sizeof(struct ibmvnic_login_buffer));
+	login_buffer->num_rxcomp_subcrqs = cpu_to_be32(adapter->req_rx_queues);
+	login_buffer->off_rxcomp_subcrqs =
+	    cpu_to_be32(sizeof(struct ibmvnic_login_buffer) +
+			sizeof(u64) * adapter->req_tx_queues);
+	login_buffer->login_rsp_ioba = cpu_to_be32(rsp_buffer_token);
+	login_buffer->login_rsp_len = cpu_to_be32(rsp_buffer_size);
+
+	tx_list_p = (__be64 *)((char *)login_buffer +
+				      sizeof(struct ibmvnic_login_buffer));
+	rx_list_p = (__be64 *)((char *)login_buffer +
+				      sizeof(struct ibmvnic_login_buffer) +
+				      sizeof(u64) * adapter->req_tx_queues);
+
+	for (i = 0; i < adapter->req_tx_queues; i++) {
+		if (adapter->tx_scrq[i]) {
+			tx_list_p[i] = cpu_to_be64(adapter->tx_scrq[i]->
+						   crq_num);
+		}
+	}
+
+	for (i = 0; i < adapter->req_rx_queues; i++) {
+		if (adapter->rx_scrq[i]) {
+			rx_list_p[i] = cpu_to_be64(adapter->rx_scrq[i]->
+						   crq_num);
+		}
+	}
+
+	netdev_dbg(adapter->netdev, "Login Buffer:\n");
+	for (i = 0; i < (adapter->login_buf_sz - 1) / 8 + 1; i++) {
+		netdev_dbg(adapter->netdev, "%016lx\n",
+			   ((unsigned long int *)(adapter->login_buf))[i]);
+	}
+
+	memset(&crq, 0, sizeof(crq));
+	crq.login.first = IBMVNIC_CRQ_CMD;
+	crq.login.cmd = LOGIN;
+	crq.login.ioba = cpu_to_be32(buffer_token);
+	crq.login.len = cpu_to_be32(buffer_size);
+
+	memcpy(&inflight_cmd->crq, &crq, sizeof(crq));
+
+	spin_lock_irqsave(&adapter->inflight_lock, flags);
+	list_add_tail(&inflight_cmd->list, &adapter->inflight);
+	spin_unlock_irqrestore(&adapter->inflight_lock, flags);
+
+	ibmvnic_send_crq(adapter, &crq);
+
+	return;
+
+inflight_alloc_failed:
+	dma_unmap_single(dev, rsp_buffer_token, rsp_buffer_size,
+			 DMA_FROM_DEVICE);
+buf_rsp_map_failed:
+	kfree(login_rsp_buffer);
+buf_rsp_alloc_failed:
+	dma_unmap_single(dev, buffer_token, buffer_size, DMA_TO_DEVICE);
+buf_map_failed:
+	kfree(login_buffer);
+buf_alloc_failed:
+	return;
+}
+
+static void send_request_map(struct ibmvnic_adapter *adapter, dma_addr_t addr,
+			     u32 len, u8 map_id)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_map.first = IBMVNIC_CRQ_CMD;
+	crq.request_map.cmd = REQUEST_MAP;
+	crq.request_map.map_id = map_id;
+	crq.request_map.ioba = cpu_to_be32(addr);
+	crq.request_map.len = cpu_to_be32(len);
+	ibmvnic_send_crq(adapter, &crq);
+}
+
+static void send_request_unmap(struct ibmvnic_adapter *adapter, u8 map_id)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_unmap.first = IBMVNIC_CRQ_CMD;
+	crq.request_unmap.cmd = REQUEST_UNMAP;
+	crq.request_unmap.map_id = map_id;
+	ibmvnic_send_crq(adapter, &crq);
+}
+
+static void send_map_query(struct ibmvnic_adapter *adapter)
+{
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.query_map.first = IBMVNIC_CRQ_CMD;
+	crq.query_map.cmd = QUERY_MAP;
+	ibmvnic_send_crq(adapter, &crq);
+}
+
+/* Send a series of CRQs requesting various capabilities of the VNIC server */
+static void send_cap_queries(struct ibmvnic_adapter *adapter)
+{
+	union ibmvnic_crq crq;
+
+	atomic_set(&adapter->running_cap_queries, 0);
+	memset(&crq, 0, sizeof(crq));
+	crq.query_capability.first = IBMVNIC_CRQ_CMD;
+	crq.query_capability.cmd = QUERY_CAPABILITY;
+
+	crq.query_capability.capability = cpu_to_be16(MIN_TX_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MIN_RX_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MIN_RX_ADD_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_TX_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_RX_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_RX_ADD_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+	    cpu_to_be16(MIN_TX_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+	    cpu_to_be16(MIN_RX_ADD_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+	    cpu_to_be16(MAX_TX_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+	    cpu_to_be16(MAX_RX_ADD_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(TCP_IP_OFFLOAD);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(PROMISC_SUPPORTED);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MIN_MTU);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_MTU);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_MULTICAST_FILTERS);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(VLAN_HEADER_INSERTION);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(MAX_TX_SG_ENTRIES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(RX_SG_SUPPORTED);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(OPT_TX_COMP_SUB_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(OPT_RX_COMP_QUEUES);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+			cpu_to_be16(OPT_RX_BUFADD_Q_PER_RX_COMP_Q);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+			cpu_to_be16(OPT_TX_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability =
+			cpu_to_be16(OPT_RXBA_ENTRIES_PER_SUBCRQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+
+	crq.query_capability.capability = cpu_to_be16(TX_RX_DESC_REQ);
+	atomic_inc(&adapter->running_cap_queries);
+	ibmvnic_send_crq(adapter, &crq);
+}
+
+static void handle_query_ip_offload_rsp(struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_query_ip_offload_buffer *buf = &adapter->ip_offload_buf;
+	union ibmvnic_crq crq;
+	int i;
+
+	dma_unmap_single(dev, adapter->ip_offload_tok,
+			 sizeof(adapter->ip_offload_buf), DMA_FROM_DEVICE);
+
+	netdev_dbg(adapter->netdev, "Query IP Offload Buffer:\n");
+	for (i = 0; i < (sizeof(adapter->ip_offload_buf) - 1) / 8 + 1; i++)
+		netdev_dbg(adapter->netdev, "%016lx\n",
+			   ((unsigned long int *)(buf))[i]);
+
+	netdev_dbg(adapter->netdev, "ipv4_chksum = %d\n", buf->ipv4_chksum);
+	netdev_dbg(adapter->netdev, "ipv6_chksum = %d\n", buf->ipv6_chksum);
+	netdev_dbg(adapter->netdev, "tcp_ipv4_chksum = %d\n",
+		   buf->tcp_ipv4_chksum);
+	netdev_dbg(adapter->netdev, "tcp_ipv6_chksum = %d\n",
+		   buf->tcp_ipv6_chksum);
+	netdev_dbg(adapter->netdev, "udp_ipv4_chksum = %d\n",
+		   buf->udp_ipv4_chksum);
+	netdev_dbg(adapter->netdev, "udp_ipv6_chksum = %d\n",
+		   buf->udp_ipv6_chksum);
+	netdev_dbg(adapter->netdev, "large_tx_ipv4 = %d\n",
+		   buf->large_tx_ipv4);
+	netdev_dbg(adapter->netdev, "large_tx_ipv6 = %d\n",
+		   buf->large_tx_ipv6);
+	netdev_dbg(adapter->netdev, "large_rx_ipv4 = %d\n",
+		   buf->large_rx_ipv4);
+	netdev_dbg(adapter->netdev, "large_rx_ipv6 = %d\n",
+		   buf->large_rx_ipv6);
+	netdev_dbg(adapter->netdev, "max_ipv4_hdr_sz = %d\n",
+		   buf->max_ipv4_header_size);
+	netdev_dbg(adapter->netdev, "max_ipv6_hdr_sz = %d\n",
+		   buf->max_ipv6_header_size);
+	netdev_dbg(adapter->netdev, "max_tcp_hdr_size = %d\n",
+		   buf->max_tcp_header_size);
+	netdev_dbg(adapter->netdev, "max_udp_hdr_size = %d\n",
+		   buf->max_udp_header_size);
+	netdev_dbg(adapter->netdev, "max_large_tx_size = %d\n",
+		   buf->max_large_tx_size);
+	netdev_dbg(adapter->netdev, "max_large_rx_size = %d\n",
+		   buf->max_large_rx_size);
+	netdev_dbg(adapter->netdev, "ipv6_ext_hdr = %d\n",
+		   buf->ipv6_extension_header);
+	netdev_dbg(adapter->netdev, "tcp_pseudosum_req = %d\n",
+		   buf->tcp_pseudosum_req);
+	netdev_dbg(adapter->netdev, "num_ipv6_ext_hd = %d\n",
+		   buf->num_ipv6_ext_headers);
+	netdev_dbg(adapter->netdev, "off_ipv6_ext_hd = %d\n",
+		   buf->off_ipv6_ext_headers);
+
+	adapter->ip_offload_ctrl_tok =
+	    dma_map_single(dev, &adapter->ip_offload_ctrl,
+			   sizeof(adapter->ip_offload_ctrl), DMA_TO_DEVICE);
+
+	if (dma_mapping_error(dev, adapter->ip_offload_ctrl_tok)) {
+		dev_err(dev, "Couldn't map ip offload control buffer\n");
+		return;
+	}
+
+	adapter->ip_offload_ctrl.version = cpu_to_be32(INITIAL_VERSION_IOB);
+	adapter->ip_offload_ctrl.tcp_ipv4_chksum = buf->tcp_ipv4_chksum;
+	adapter->ip_offload_ctrl.udp_ipv4_chksum = buf->udp_ipv4_chksum;
+	adapter->ip_offload_ctrl.tcp_ipv6_chksum = buf->tcp_ipv6_chksum;
+	adapter->ip_offload_ctrl.udp_ipv6_chksum = buf->udp_ipv6_chksum;
+
+	/* large_tx/rx disabled for now, additional features needed */
+	adapter->ip_offload_ctrl.large_tx_ipv4 = 0;
+	adapter->ip_offload_ctrl.large_tx_ipv6 = 0;
+	adapter->ip_offload_ctrl.large_rx_ipv4 = 0;
+	adapter->ip_offload_ctrl.large_rx_ipv6 = 0;
+
+	adapter->netdev->features = NETIF_F_GSO;
+
+	if (buf->tcp_ipv4_chksum || buf->udp_ipv4_chksum)
+		adapter->netdev->features |= NETIF_F_IP_CSUM;
+
+	if (buf->tcp_ipv6_chksum || buf->udp_ipv6_chksum)
+		adapter->netdev->features |= NETIF_F_IPV6_CSUM;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ip_offload.first = IBMVNIC_CRQ_CMD;
+	crq.control_ip_offload.cmd = CONTROL_IP_OFFLOAD;
+	crq.control_ip_offload.len =
+	    cpu_to_be32(sizeof(adapter->ip_offload_ctrl));
+	crq.control_ip_offload.ioba = cpu_to_be32(adapter->ip_offload_ctrl_tok);
+	ibmvnic_send_crq(adapter, &crq);
+}
+
+static void handle_error_info_rsp(union ibmvnic_crq *crq,
+				  struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_error_buff *error_buff;
+	unsigned long flags;
+	bool found = false;
+	int i;
+
+	if (!crq->request_error_rsp.rc.code) {
+		dev_info(dev, "Request Error Rsp returned with rc=%x\n",
+			 crq->request_error_rsp.rc.code);
+		return;
+	}
+
+	spin_lock_irqsave(&adapter->error_list_lock, flags);
+	list_for_each_entry(error_buff, &adapter->errors, list)
+		if (error_buff->error_id == crq->request_error_rsp.error_id) {
+			found = true;
+			list_del(&error_buff->list);
+			break;
+		}
+	spin_unlock_irqrestore(&adapter->error_list_lock, flags);
+
+	if (!found) {
+		dev_err(dev, "Couldn't find error id %x\n",
+			crq->request_error_rsp.error_id);
+		return;
+	}
+
+	dev_err(dev, "Detailed info for error id %x:",
+		crq->request_error_rsp.error_id);
+
+	for (i = 0; i < error_buff->len; i++) {
+		pr_cont("%02x", (int)error_buff->buff[i]);
+		if (i % 8 == 7)
+			pr_cont(" ");
+	}
+	pr_cont("\n");
+
+	dma_unmap_single(dev, error_buff->dma, error_buff->len,
+			 DMA_FROM_DEVICE);
+	kfree(error_buff->buff);
+	kfree(error_buff);
+}
+
+static void handle_dump_size_rsp(union ibmvnic_crq *crq,
+				 struct ibmvnic_adapter *adapter)
+{
+	int len = be32_to_cpu(crq->request_dump_size_rsp.len);
+	struct ibmvnic_inflight_cmd *inflight_cmd;
+	struct device *dev = &adapter->vdev->dev;
+	union ibmvnic_crq newcrq;
+	unsigned long flags;
+
+	/* allocate and map buffer */
+	adapter->dump_data = kmalloc(len, GFP_KERNEL);
+	if (!adapter->dump_data) {
+		complete(&adapter->fw_done);
+		return;
+	}
+
+	adapter->dump_data_token = dma_map_single(dev, adapter->dump_data, len,
+						  DMA_FROM_DEVICE);
+
+	if (dma_mapping_error(dev, adapter->dump_data_token)) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			dev_err(dev, "Couldn't map dump data\n");
+		kfree(adapter->dump_data);
+		complete(&adapter->fw_done);
+		return;
+	}
+
+	inflight_cmd = kmalloc(sizeof(*inflight_cmd), GFP_ATOMIC);
+	if (!inflight_cmd) {
+		dma_unmap_single(dev, adapter->dump_data_token, len,
+				 DMA_FROM_DEVICE);
+		kfree(adapter->dump_data);
+		complete(&adapter->fw_done);
+		return;
+	}
+
+	memset(&newcrq, 0, sizeof(newcrq));
+	newcrq.request_dump.first = IBMVNIC_CRQ_CMD;
+	newcrq.request_dump.cmd = REQUEST_DUMP;
+	newcrq.request_dump.ioba = cpu_to_be32(adapter->dump_data_token);
+	newcrq.request_dump.len = cpu_to_be32(adapter->dump_data_size);
+
+	memcpy(&inflight_cmd->crq, &newcrq, sizeof(newcrq));
+
+	spin_lock_irqsave(&adapter->inflight_lock, flags);
+	list_add_tail(&inflight_cmd->list, &adapter->inflight);
+	spin_unlock_irqrestore(&adapter->inflight_lock, flags);
+
+	ibmvnic_send_crq(adapter, &newcrq);
+}
+
+static void handle_error_indication(union ibmvnic_crq *crq,
+				    struct ibmvnic_adapter *adapter)
+{
+	int detail_len = be32_to_cpu(crq->error_indication.detail_error_sz);
+	struct ibmvnic_inflight_cmd *inflight_cmd;
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_error_buff *error_buff;
+	union ibmvnic_crq new_crq;
+	unsigned long flags;
+
+	dev_err(dev, "Firmware reports %serror id %x, cause %d\n",
+		crq->error_indication.
+		    flags & IBMVNIC_FATAL_ERROR ? "FATAL " : "",
+		crq->error_indication.error_id,
+		crq->error_indication.error_cause);
+
+	error_buff = kmalloc(sizeof(*error_buff), GFP_ATOMIC);
+	if (!error_buff)
+		return;
+
+	error_buff->buff = kmalloc(detail_len, GFP_ATOMIC);
+	if (!error_buff->buff) {
+		kfree(error_buff);
+		return;
+	}
+
+	error_buff->dma = dma_map_single(dev, error_buff->buff, detail_len,
+					 DMA_FROM_DEVICE);
+	if (dma_mapping_error(dev, error_buff->dma)) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			dev_err(dev, "Couldn't map error buffer\n");
+		kfree(error_buff->buff);
+		kfree(error_buff);
+		return;
+	}
+
+	inflight_cmd = kmalloc(sizeof(*inflight_cmd), GFP_ATOMIC);
+	if (!inflight_cmd) {
+		dma_unmap_single(dev, error_buff->dma, detail_len,
+				 DMA_FROM_DEVICE);
+		kfree(error_buff->buff);
+		kfree(error_buff);
+		return;
+	}
+
+	error_buff->len = detail_len;
+	error_buff->error_id = crq->error_indication.error_id;
+
+	spin_lock_irqsave(&adapter->error_list_lock, flags);
+	list_add_tail(&error_buff->list, &adapter->errors);
+	spin_unlock_irqrestore(&adapter->error_list_lock, flags);
+
+	memset(&new_crq, 0, sizeof(new_crq));
+	new_crq.request_error_info.first = IBMVNIC_CRQ_CMD;
+	new_crq.request_error_info.cmd = REQUEST_ERROR_INFO;
+	new_crq.request_error_info.ioba = cpu_to_be32(error_buff->dma);
+	new_crq.request_error_info.len = cpu_to_be32(detail_len);
+	new_crq.request_error_info.error_id = crq->error_indication.error_id;
+
+	memcpy(&inflight_cmd->crq, &crq, sizeof(crq));
+
+	spin_lock_irqsave(&adapter->inflight_lock, flags);
+	list_add_tail(&inflight_cmd->list, &adapter->inflight);
+	spin_unlock_irqrestore(&adapter->inflight_lock, flags);
+
+	ibmvnic_send_crq(adapter, &new_crq);
+}
+
+static void handle_change_mac_rsp(union ibmvnic_crq *crq,
+				  struct ibmvnic_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	rc = crq->change_mac_addr_rsp.rc.code;
+	if (rc) {
+		dev_err(dev, "Error %ld in CHANGE_MAC_ADDR_RSP\n", rc);
+		return;
+	}
+	memcpy(netdev->dev_addr, &crq->change_mac_addr_rsp.mac_addr[0],
+	       ETH_ALEN);
+}
+
+static void handle_request_cap_rsp(union ibmvnic_crq *crq,
+				   struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	u64 *req_value;
+	char *name;
+
+	switch (be16_to_cpu(crq->request_capability_rsp.capability)) {
+	case REQ_TX_QUEUES:
+		req_value = &adapter->req_tx_queues;
+		name = "tx";
+		break;
+	case REQ_RX_QUEUES:
+		req_value = &adapter->req_rx_queues;
+		name = "rx";
+		break;
+	case REQ_RX_ADD_QUEUES:
+		req_value = &adapter->req_rx_add_queues;
+		name = "rx_add";
+		break;
+	case REQ_TX_ENTRIES_PER_SUBCRQ:
+		req_value = &adapter->req_tx_entries_per_subcrq;
+		name = "tx_entries_per_subcrq";
+		break;
+	case REQ_RX_ADD_ENTRIES_PER_SUBCRQ:
+		req_value = &adapter->req_rx_add_entries_per_subcrq;
+		name = "rx_add_entries_per_subcrq";
+		break;
+	case REQ_MTU:
+		req_value = &adapter->req_mtu;
+		name = "mtu";
+		break;
+	case PROMISC_REQUESTED:
+		req_value = &adapter->promisc;
+		name = "promisc";
+		break;
+	default:
+		dev_err(dev, "Got invalid cap request rsp %d\n",
+			crq->request_capability.capability);
+		return;
+	}
+
+	switch (crq->request_capability_rsp.rc.code) {
+	case SUCCESS:
+		break;
+	case PARTIALSUCCESS:
+		dev_info(dev, "req=%lld, rsp=%ld in %s queue, retrying.\n",
+			 *req_value,
+			 (long int)be32_to_cpu(crq->request_capability_rsp.
+					       number), name);
+		release_sub_crqs(adapter);
+		*req_value = be32_to_cpu(crq->request_capability_rsp.number);
+		complete(&adapter->init_done);
+		return;
+	default:
+		dev_err(dev, "Error %d in request cap rsp\n",
+			crq->request_capability_rsp.rc.code);
+		return;
+	}
+
+	/* Done receiving requested capabilities, query IP offload support */
+	if (++adapter->requested_caps == 7) {
+		union ibmvnic_crq newcrq;
+		int buf_sz = sizeof(struct ibmvnic_query_ip_offload_buffer);
+		struct ibmvnic_query_ip_offload_buffer *ip_offload_buf =
+		    &adapter->ip_offload_buf;
+
+		adapter->ip_offload_tok = dma_map_single(dev, ip_offload_buf,
+							 buf_sz,
+							 DMA_FROM_DEVICE);
+
+		if (dma_mapping_error(dev, adapter->ip_offload_tok)) {
+			if (!firmware_has_feature(FW_FEATURE_CMO))
+				dev_err(dev, "Couldn't map offload buffer\n");
+			return;
+		}
+
+		memset(&newcrq, 0, sizeof(newcrq));
+		newcrq.query_ip_offload.first = IBMVNIC_CRQ_CMD;
+		newcrq.query_ip_offload.cmd = QUERY_IP_OFFLOAD;
+		newcrq.query_ip_offload.len = cpu_to_be32(buf_sz);
+		newcrq.query_ip_offload.ioba =
+		    cpu_to_be32(adapter->ip_offload_tok);
+
+		ibmvnic_send_crq(adapter, &newcrq);
+	}
+}
+
+static int handle_login_rsp(union ibmvnic_crq *login_rsp_crq,
+			    struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_login_rsp_buffer *login_rsp = adapter->login_rsp_buf;
+	struct ibmvnic_login_buffer *login = adapter->login_buf;
+	union ibmvnic_crq crq;
+	int i;
+
+	dma_unmap_single(dev, adapter->login_buf_token, adapter->login_buf_sz,
+			 DMA_BIDIRECTIONAL);
+	dma_unmap_single(dev, adapter->login_rsp_buf_token,
+			 adapter->login_rsp_buf_sz, DMA_BIDIRECTIONAL);
+
+	netdev_dbg(adapter->netdev, "Login Response Buffer:\n");
+	for (i = 0; i < (adapter->login_rsp_buf_sz - 1) / 8 + 1; i++) {
+		netdev_dbg(adapter->netdev, "%016lx\n",
+			   ((unsigned long int *)(adapter->login_rsp_buf))[i]);
+	}
+
+	/* Sanity checks */
+	if (login->num_txcomp_subcrqs != login_rsp->num_txsubm_subcrqs ||
+	    (be32_to_cpu(login->num_rxcomp_subcrqs) *
+	     adapter->req_rx_add_queues !=
+	     be32_to_cpu(login_rsp->num_rxadd_subcrqs))) {
+		dev_err(dev, "FATAL: Inconsistent login and login rsp\n");
+		ibmvnic_remove(adapter->vdev);
+		return -EIO;
+	}
+	complete(&adapter->init_done);
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_ras_comp_num.first = IBMVNIC_CRQ_CMD;
+	crq.request_ras_comp_num.cmd = REQUEST_RAS_COMP_NUM;
+	ibmvnic_send_crq(adapter, &crq);
+
+	return 0;
+}
+
+static void handle_request_map_rsp(union ibmvnic_crq *crq,
+				   struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	u8 map_id = crq->request_map_rsp.map_id;
+	int tx_subcrqs;
+	int rx_subcrqs;
+	long rc;
+	int i;
+
+	tx_subcrqs = be32_to_cpu(adapter->login_rsp_buf->num_txsubm_subcrqs);
+	rx_subcrqs = be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+
+	rc = crq->request_map_rsp.rc.code;
+	if (rc) {
+		dev_err(dev, "Error %ld in REQUEST_MAP_RSP\n", rc);
+		adapter->map_id--;
+		/* need to find and zero tx/rx_pool map_id */
+		for (i = 0; i < tx_subcrqs; i++) {
+			if (adapter->tx_pool[i].long_term_buff.map_id == map_id)
+				adapter->tx_pool[i].long_term_buff.map_id = 0;
+		}
+		for (i = 0; i < rx_subcrqs; i++) {
+			if (adapter->rx_pool[i].long_term_buff.map_id == map_id)
+				adapter->rx_pool[i].long_term_buff.map_id = 0;
+		}
+	}
+	complete(&adapter->fw_done);
+}
+
+static void handle_request_unmap_rsp(union ibmvnic_crq *crq,
+				     struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	rc = crq->request_unmap_rsp.rc.code;
+	if (rc)
+		dev_err(dev, "Error %ld in REQUEST_UNMAP_RSP\n", rc);
+}
+
+static void handle_query_map_rsp(union ibmvnic_crq *crq,
+				 struct ibmvnic_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	rc = crq->query_map_rsp.rc.code;
+	if (rc) {
+		dev_err(dev, "Error %ld in QUERY_MAP_RSP\n", rc);
+		return;
+	}
+	netdev_dbg(netdev, "page_size = %d\ntot_pages = %d\nfree_pages = %d\n",
+		   crq->query_map_rsp.page_size, crq->query_map_rsp.tot_pages,
+		   crq->query_map_rsp.free_pages);
+}
+
+static void handle_query_cap_rsp(union ibmvnic_crq *crq,
+				 struct ibmvnic_adapter *adapter)
+{
+	struct net_device *netdev = adapter->netdev;
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	atomic_dec(&adapter->running_cap_queries);
+	netdev_dbg(netdev, "Outstanding queries: %d\n",
+		   atomic_read(&adapter->running_cap_queries));
+	rc = crq->query_capability.rc.code;
+	if (rc) {
+		dev_err(dev, "Error %ld in QUERY_CAP_RSP\n", rc);
+		goto out;
+	}
+
+	switch (be16_to_cpu(crq->query_capability.capability)) {
+	case MIN_TX_QUEUES:
+		adapter->min_tx_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_tx_queues = %lld\n",
+			   adapter->min_tx_queues);
+		break;
+	case MIN_RX_QUEUES:
+		adapter->min_rx_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_rx_queues = %lld\n",
+			   adapter->min_rx_queues);
+		break;
+	case MIN_RX_ADD_QUEUES:
+		adapter->min_rx_add_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_rx_add_queues = %lld\n",
+			   adapter->min_rx_add_queues);
+		break;
+	case MAX_TX_QUEUES:
+		adapter->max_tx_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_tx_queues = %lld\n",
+			   adapter->max_tx_queues);
+		break;
+	case MAX_RX_QUEUES:
+		adapter->max_rx_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_rx_queues = %lld\n",
+			   adapter->max_rx_queues);
+		break;
+	case MAX_RX_ADD_QUEUES:
+		adapter->max_rx_add_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_rx_add_queues = %lld\n",
+			   adapter->max_rx_add_queues);
+		break;
+	case MIN_TX_ENTRIES_PER_SUBCRQ:
+		adapter->min_tx_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_tx_entries_per_subcrq = %lld\n",
+			   adapter->min_tx_entries_per_subcrq);
+		break;
+	case MIN_RX_ADD_ENTRIES_PER_SUBCRQ:
+		adapter->min_rx_add_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_rx_add_entrs_per_subcrq = %lld\n",
+			   adapter->min_rx_add_entries_per_subcrq);
+		break;
+	case MAX_TX_ENTRIES_PER_SUBCRQ:
+		adapter->max_tx_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_tx_entries_per_subcrq = %lld\n",
+			   adapter->max_tx_entries_per_subcrq);
+		break;
+	case MAX_RX_ADD_ENTRIES_PER_SUBCRQ:
+		adapter->max_rx_add_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_rx_add_entrs_per_subcrq = %lld\n",
+			   adapter->max_rx_add_entries_per_subcrq);
+		break;
+	case TCP_IP_OFFLOAD:
+		adapter->tcp_ip_offload =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "tcp_ip_offload = %lld\n",
+			   adapter->tcp_ip_offload);
+		break;
+	case PROMISC_SUPPORTED:
+		adapter->promisc_supported =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "promisc_supported = %lld\n",
+			   adapter->promisc_supported);
+		break;
+	case MIN_MTU:
+		adapter->min_mtu = be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "min_mtu = %lld\n", adapter->min_mtu);
+		break;
+	case MAX_MTU:
+		adapter->max_mtu = be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_mtu = %lld\n", adapter->max_mtu);
+		break;
+	case MAX_MULTICAST_FILTERS:
+		adapter->max_multicast_filters =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_multicast_filters = %lld\n",
+			   adapter->max_multicast_filters);
+		break;
+	case VLAN_HEADER_INSERTION:
+		adapter->vlan_header_insertion =
+		    be64_to_cpu(crq->query_capability.number);
+		if (adapter->vlan_header_insertion)
+			netdev->features |= NETIF_F_HW_VLAN_STAG_TX;
+		netdev_dbg(netdev, "vlan_header_insertion = %lld\n",
+			   adapter->vlan_header_insertion);
+		break;
+	case MAX_TX_SG_ENTRIES:
+		adapter->max_tx_sg_entries =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "max_tx_sg_entries = %lld\n",
+			   adapter->max_tx_sg_entries);
+		break;
+	case RX_SG_SUPPORTED:
+		adapter->rx_sg_supported =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "rx_sg_supported = %lld\n",
+			   adapter->rx_sg_supported);
+		break;
+	case OPT_TX_COMP_SUB_QUEUES:
+		adapter->opt_tx_comp_sub_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "opt_tx_comp_sub_queues = %lld\n",
+			   adapter->opt_tx_comp_sub_queues);
+		break;
+	case OPT_RX_COMP_QUEUES:
+		adapter->opt_rx_comp_queues =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "opt_rx_comp_queues = %lld\n",
+			   adapter->opt_rx_comp_queues);
+		break;
+	case OPT_RX_BUFADD_Q_PER_RX_COMP_Q:
+		adapter->opt_rx_bufadd_q_per_rx_comp_q =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "opt_rx_bufadd_q_per_rx_comp_q = %lld\n",
+			   adapter->opt_rx_bufadd_q_per_rx_comp_q);
+		break;
+	case OPT_TX_ENTRIES_PER_SUBCRQ:
+		adapter->opt_tx_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "opt_tx_entries_per_subcrq = %lld\n",
+			   adapter->opt_tx_entries_per_subcrq);
+		break;
+	case OPT_RXBA_ENTRIES_PER_SUBCRQ:
+		adapter->opt_rxba_entries_per_subcrq =
+		    be64_to_cpu(crq->query_capability.number);
+		netdev_dbg(netdev, "opt_rxba_entries_per_subcrq = %lld\n",
+			   adapter->opt_rxba_entries_per_subcrq);
+		break;
+	case TX_RX_DESC_REQ:
+		adapter->tx_rx_desc_req = crq->query_capability.number;
+		netdev_dbg(netdev, "tx_rx_desc_req = %llx\n",
+			   adapter->tx_rx_desc_req);
+		break;
+
+	default:
+		netdev_err(netdev, "Got invalid cap rsp %d\n",
+			   crq->query_capability.capability);
+	}
+
+out:
+	if (atomic_read(&adapter->running_cap_queries) == 0)
+		complete(&adapter->init_done);
+		/* We're done querying the capabilities, initialize sub-crqs */
+}
+
+static void handle_control_ras_rsp(union ibmvnic_crq *crq,
+				   struct ibmvnic_adapter *adapter)
+{
+	u8 correlator = crq->control_ras_rsp.correlator;
+	struct device *dev = &adapter->vdev->dev;
+	bool found = false;
+	int i;
+
+	if (crq->control_ras_rsp.rc.code) {
+		dev_warn(dev, "Control ras failed rc=%d\n",
+			 crq->control_ras_rsp.rc.code);
+		return;
+	}
+
+	for (i = 0; i < adapter->ras_comp_num; i++) {
+		if (adapter->ras_comps[i].correlator == correlator) {
+			found = true;
+			break;
+		}
+	}
+
+	if (!found) {
+		dev_warn(dev, "Correlator not found on control_ras_rsp\n");
+		return;
+	}
+
+	switch (crq->control_ras_rsp.op) {
+	case IBMVNIC_TRACE_LEVEL:
+		adapter->ras_comps[i].trace_level = crq->control_ras.level;
+		break;
+	case IBMVNIC_ERROR_LEVEL:
+		adapter->ras_comps[i].error_check_level =
+		    crq->control_ras.level;
+		break;
+	case IBMVNIC_TRACE_PAUSE:
+		adapter->ras_comp_int[i].paused = 1;
+		break;
+	case IBMVNIC_TRACE_RESUME:
+		adapter->ras_comp_int[i].paused = 0;
+		break;
+	case IBMVNIC_TRACE_ON:
+		adapter->ras_comps[i].trace_on = 1;
+		break;
+	case IBMVNIC_TRACE_OFF:
+		adapter->ras_comps[i].trace_on = 0;
+		break;
+	case IBMVNIC_CHG_TRACE_BUFF_SZ:
+		/* trace_buff_sz is 3 bytes, stuff it into an int */
+		((u8 *)(&adapter->ras_comps[i].trace_buff_size))[0] = 0;
+		((u8 *)(&adapter->ras_comps[i].trace_buff_size))[1] =
+		    crq->control_ras_rsp.trace_buff_sz[0];
+		((u8 *)(&adapter->ras_comps[i].trace_buff_size))[2] =
+		    crq->control_ras_rsp.trace_buff_sz[1];
+		((u8 *)(&adapter->ras_comps[i].trace_buff_size))[3] =
+		    crq->control_ras_rsp.trace_buff_sz[2];
+		break;
+	default:
+		dev_err(dev, "invalid op %d on control_ras_rsp",
+			crq->control_ras_rsp.op);
+	}
+}
+
+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)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_fw_trace_entry *trace;
+	int num = ras_comp_int->num;
+	union ibmvnic_crq crq;
+	dma_addr_t trace_tok;
+
+	if (*ppos >= be32_to_cpu(adapter->ras_comps[num].trace_buff_size))
+		return 0;
+
+	trace =
+	    dma_alloc_coherent(dev,
+			       be32_to_cpu(adapter->ras_comps[num].
+					   trace_buff_size), &trace_tok,
+			       GFP_KERNEL);
+	if (!trace) {
+		dev_err(dev, "Couldn't alloc trace buffer\n");
+		return 0;
+	}
+
+	memset(&crq, 0, sizeof(crq));
+	crq.collect_fw_trace.first = IBMVNIC_CRQ_CMD;
+	crq.collect_fw_trace.cmd = COLLECT_FW_TRACE;
+	crq.collect_fw_trace.correlator = adapter->ras_comps[num].correlator;
+	crq.collect_fw_trace.ioba = cpu_to_be32(trace_tok);
+	crq.collect_fw_trace.len = adapter->ras_comps[num].trace_buff_size;
+	ibmvnic_send_crq(adapter, &crq);
+
+	init_completion(&adapter->fw_done);
+	wait_for_completion(&adapter->fw_done);
+
+	if (*ppos + len > be32_to_cpu(adapter->ras_comps[num].trace_buff_size))
+		len =
+		    be32_to_cpu(adapter->ras_comps[num].trace_buff_size) -
+		    *ppos;
+
+	copy_to_user(user_buf, &((u8 *)trace)[*ppos], len);
+
+	dma_free_coherent(dev,
+			  be32_to_cpu(adapter->ras_comps[num].trace_buff_size),
+			  trace, trace_tok);
+	*ppos += len;
+	return len;
+}
+
+static const struct file_operations trace_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= trace_read,
+};
+
+static ssize_t paused_read(struct file *file, char __user *user_buf, size_t len,
+			   loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	char buff[5]; /*  1 or 0 plus \n and \0 */
+	int size;
+
+	size = sprintf(buff, "%d\n", adapter->ras_comp_int[num].paused);
+
+	if (*ppos >= size)
+		return 0;
+
+	copy_to_user(user_buf, buff, size);
+	*ppos += size;
+	return size;
+}
+
+static ssize_t paused_write(struct file *file, const char __user *user_buf,
+			    size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	union ibmvnic_crq crq;
+	unsigned long val;
+	char buff[9]; /* decimal max int plus \n and \0 */
+
+	copy_from_user(buff, user_buf, sizeof(buff));
+	val = kstrtoul(buff, 10, NULL);
+
+	adapter->ras_comp_int[num].paused = val ? 1 : 0;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ras.first = IBMVNIC_CRQ_CMD;
+	crq.control_ras.cmd = CONTROL_RAS;
+	crq.control_ras.correlator = adapter->ras_comps[num].correlator;
+	crq.control_ras.op = val ? IBMVNIC_TRACE_PAUSE : IBMVNIC_TRACE_RESUME;
+	ibmvnic_send_crq(adapter, &crq);
+
+	return len;
+}
+
+static const struct file_operations paused_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= paused_read,
+	.write		= paused_write,
+};
+
+static ssize_t tracing_read(struct file *file, char __user *user_buf,
+			    size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	char buff[5]; /*  1 or 0 plus \n and \0 */
+	int size;
+
+	size = sprintf(buff, "%d\n", adapter->ras_comps[num].trace_on);
+
+	if (*ppos >= size)
+		return 0;
+
+	copy_to_user(user_buf, buff, size);
+	*ppos += size;
+	return size;
+}
+
+static ssize_t tracing_write(struct file *file, const char __user *user_buf,
+			     size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	union ibmvnic_crq crq;
+	unsigned long val;
+	char buff[9]; /* decimal max int plus \n and \0 */
+
+	copy_from_user(buff, user_buf, sizeof(buff));
+	val = kstrtoul(buff, 10, NULL);
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ras.first = IBMVNIC_CRQ_CMD;
+	crq.control_ras.cmd = CONTROL_RAS;
+	crq.control_ras.correlator = adapter->ras_comps[num].correlator;
+	crq.control_ras.op = val ? IBMVNIC_TRACE_ON : IBMVNIC_TRACE_OFF;
+
+	return len;
+}
+
+static const struct file_operations tracing_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= tracing_read,
+	.write		= tracing_write,
+};
+
+static ssize_t error_level_read(struct file *file, char __user *user_buf,
+				size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	char buff[5]; /* decimal max char plus \n and \0 */
+	int size;
+
+	size = sprintf(buff, "%d\n", adapter->ras_comps[num].error_check_level);
+
+	if (*ppos >= size)
+		return 0;
+
+	copy_to_user(user_buf, buff, size);
+	*ppos += size;
+	return size;
+}
+
+static ssize_t error_level_write(struct file *file, const char __user *user_buf,
+				 size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	union ibmvnic_crq crq;
+	unsigned long val;
+	char buff[9]; /* decimal max int plus \n and \0 */
+
+	copy_from_user(buff, user_buf, sizeof(buff));
+	val = kstrtoul(buff, 10, NULL);
+
+	if (val > 9)
+		val = 9;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ras.first = IBMVNIC_CRQ_CMD;
+	crq.control_ras.cmd = CONTROL_RAS;
+	crq.control_ras.correlator = adapter->ras_comps[num].correlator;
+	crq.control_ras.op = IBMVNIC_ERROR_LEVEL;
+	crq.control_ras.level = val;
+	ibmvnic_send_crq(adapter, &crq);
+
+	return len;
+}
+
+static const struct file_operations error_level_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= error_level_read,
+	.write		= error_level_write,
+};
+
+static ssize_t trace_level_read(struct file *file, char __user *user_buf,
+				size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	char buff[5]; /* decimal max char plus \n and \0 */
+	int size;
+
+	size = sprintf(buff, "%d\n", adapter->ras_comps[num].trace_level);
+	if (*ppos >= size)
+		return 0;
+
+	copy_to_user(user_buf, buff, size);
+	*ppos += size;
+	return size;
+}
+
+static ssize_t trace_level_write(struct file *file, const char __user *user_buf,
+				 size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	union ibmvnic_crq crq;
+	unsigned long val;
+	char buff[9]; /* decimal max int plus \n and \0 */
+
+	copy_from_user(buff, user_buf, sizeof(buff));
+	val = kstrtoul(buff, 10, NULL);
+	if (val > 9)
+		val = 9;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ras.first = IBMVNIC_CRQ_CMD;
+	crq.control_ras.cmd = CONTROL_RAS;
+	crq.control_ras.correlator =
+	    adapter->ras_comps[ras_comp_int->num].correlator;
+	crq.control_ras.op = IBMVNIC_TRACE_LEVEL;
+	crq.control_ras.level = val;
+	ibmvnic_send_crq(adapter, &crq);
+
+	return len;
+}
+
+static const struct file_operations trace_level_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= trace_level_read,
+	.write		= trace_level_write,
+};
+
+static ssize_t trace_buff_size_read(struct file *file, char __user *user_buf,
+				    size_t len, loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	int num = ras_comp_int->num;
+	char buff[9]; /* decimal max int plus \n and \0 */
+	int size;
+
+	size = sprintf(buff, "%d\n", adapter->ras_comps[num].trace_buff_size);
+	if (*ppos >= size)
+		return 0;
+
+	copy_to_user(user_buf, buff, size);
+	*ppos += size;
+	return size;
+}
+
+static ssize_t trace_buff_size_write(struct file *file,
+				     const char __user *user_buf, size_t len,
+				     loff_t *ppos)
+{
+	struct ibmvnic_fw_comp_internal *ras_comp_int = file->private_data;
+	struct ibmvnic_adapter *adapter = ras_comp_int->adapter;
+	union ibmvnic_crq crq;
+	unsigned long val;
+	char buff[9]; /* decimal max int plus \n and \0 */
+
+	copy_from_user(buff, user_buf, sizeof(buff));
+	val = kstrtoul(buff, 10, NULL);
+
+	memset(&crq, 0, sizeof(crq));
+	crq.control_ras.first = IBMVNIC_CRQ_CMD;
+	crq.control_ras.cmd = CONTROL_RAS;
+	crq.control_ras.correlator =
+	    adapter->ras_comps[ras_comp_int->num].correlator;
+	crq.control_ras.op = IBMVNIC_CHG_TRACE_BUFF_SZ;
+	/* trace_buff_sz is 3 bytes, stuff an int into it */
+	crq.control_ras.trace_buff_sz[0] = ((u8 *)(&val))[5];
+	crq.control_ras.trace_buff_sz[1] = ((u8 *)(&val))[6];
+	crq.control_ras.trace_buff_sz[2] = ((u8 *)(&val))[7];
+	ibmvnic_send_crq(adapter, &crq);
+
+	return len;
+}
+
+static const struct file_operations trace_size_ops = {
+	.owner		= THIS_MODULE,
+	.open		= ibmvnic_fw_comp_open,
+	.read		= trace_buff_size_read,
+	.write		= trace_buff_size_write,
+};
+
+static void handle_request_ras_comps_rsp(union ibmvnic_crq *crq,
+					 struct ibmvnic_adapter *adapter)
+{
+	struct device *dev = &adapter->vdev->dev;
+	struct dentry *dir_ent;
+	struct dentry *ent;
+	int i;
+
+	debugfs_remove_recursive(adapter->ras_comps_ent);
+
+	adapter->ras_comps_ent = debugfs_create_dir("ras_comps",
+						    adapter->debugfs_dir);
+	if (!adapter->ras_comps_ent || IS_ERR(adapter->ras_comps_ent)) {
+		dev_info(dev, "debugfs create ras_comps dir failed\n");
+		return;
+	}
+
+	for (i = 0; i < adapter->ras_comp_num; i++) {
+		dir_ent = debugfs_create_dir(adapter->ras_comps[i].name,
+					     adapter->ras_comps_ent);
+		if (!dir_ent || IS_ERR(dir_ent)) {
+			dev_info(dev, "debugfs create %s dir failed\n",
+				 adapter->ras_comps[i].name);
+			continue;
+		}
+
+		adapter->ras_comp_int[i].adapter = adapter;
+		adapter->ras_comp_int[i].num = i;
+		adapter->ras_comp_int[i].desc_blob.data =
+		    &adapter->ras_comps[i].description;
+		adapter->ras_comp_int[i].desc_blob.size =
+		    sizeof(adapter->ras_comps[i].description);
+
+		/* Don't need to remember the dentry's because the debugfs dir
+		 * gets removed recursively
+		 */
+		ent = debugfs_create_blob("description", S_IRUGO, dir_ent,
+					  &adapter->ras_comp_int[i].desc_blob);
+		ent = debugfs_create_file("trace_buf_size", S_IRUGO | S_IWUSR,
+					  dir_ent, &adapter->ras_comp_int[i],
+					  &trace_size_ops);
+		ent = debugfs_create_file("trace_level",
+					  S_IRUGO |
+					  (adapter->ras_comps[i].trace_level !=
+					   0xFF  ? S_IWUSR : 0),
+					   dir_ent, &adapter->ras_comp_int[i],
+					   &trace_level_ops);
+		ent = debugfs_create_file("error_level",
+					  S_IRUGO |
+					  (adapter->
+					   ras_comps[i].error_check_level !=
+					   0xFF ? S_IWUSR : 0),
+					  dir_ent, &adapter->ras_comp_int[i],
+					  &trace_level_ops);
+		ent = debugfs_create_file("tracing", S_IRUGO | S_IWUSR,
+					  dir_ent, &adapter->ras_comp_int[i],
+					  &tracing_ops);
+		ent = debugfs_create_file("paused", S_IRUGO | S_IWUSR,
+					  dir_ent, &adapter->ras_comp_int[i],
+					  &paused_ops);
+		ent = debugfs_create_file("trace", S_IRUGO, dir_ent,
+					  &adapter->ras_comp_int[i],
+					  &trace_ops);
+	}
+}
+
+static void handle_request_ras_comp_num_rsp(union ibmvnic_crq *crq,
+					    struct ibmvnic_adapter *adapter)
+{
+	int len = adapter->ras_comp_num * sizeof(struct ibmvnic_fw_component);
+	struct device *dev = &adapter->vdev->dev;
+	union ibmvnic_crq newcrq;
+
+	adapter->ras_comps = dma_alloc_coherent(dev, len,
+						&adapter->ras_comps_tok,
+						GFP_KERNEL);
+	if (!adapter->ras_comps) {
+		if (!firmware_has_feature(FW_FEATURE_CMO))
+			dev_err(dev, "Couldn't alloc fw comps buffer\n");
+		return;
+	}
+
+	adapter->ras_comp_int = kmalloc(adapter->ras_comp_num *
+					sizeof(struct ibmvnic_fw_comp_internal),
+					GFP_KERNEL);
+	if (!adapter->ras_comp_int)
+		dma_free_coherent(dev, len, adapter->ras_comps,
+				  adapter->ras_comps_tok);
+
+	memset(&newcrq, 0, sizeof(newcrq));
+	newcrq.request_ras_comps.first = IBMVNIC_CRQ_CMD;
+	newcrq.request_ras_comps.cmd = REQUEST_RAS_COMPS;
+	newcrq.request_ras_comps.ioba = cpu_to_be32(adapter->ras_comps_tok);
+	newcrq.request_ras_comps.len = cpu_to_be32(len);
+	ibmvnic_send_crq(adapter, &newcrq);
+}
+
+static void ibmvnic_free_inflight(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_inflight_cmd *inflight_cmd;
+	struct device *dev = &adapter->vdev->dev;
+	struct ibmvnic_error_buff *error_buff;
+	unsigned long flags;
+	unsigned long flags2;
+
+	spin_lock_irqsave(&adapter->inflight_lock, flags);
+	list_for_each_entry(inflight_cmd, &adapter->inflight, list) {
+		switch (inflight_cmd->crq.generic.cmd) {
+		case LOGIN:
+			dma_unmap_single(dev, adapter->login_buf_token,
+					 adapter->login_buf_sz,
+					 DMA_BIDIRECTIONAL);
+			dma_unmap_single(dev, adapter->login_rsp_buf_token,
+					 adapter->login_rsp_buf_sz,
+					 DMA_BIDIRECTIONAL);
+			kfree(adapter->login_rsp_buf);
+			kfree(adapter->login_buf);
+			break;
+		case REQUEST_DUMP:
+			complete(&adapter->fw_done);
+			break;
+		case REQUEST_ERROR_INFO:
+			spin_lock_irqsave(&adapter->error_list_lock, flags2);
+			list_for_each_entry(error_buff, &adapter->errors,
+					    list) {
+				dma_unmap_single(dev, error_buff->dma,
+						 error_buff->len,
+						 DMA_FROM_DEVICE);
+				kfree(error_buff->buff);
+				list_del(&error_buff->list);
+				kfree(error_buff);
+			}
+			spin_unlock_irqrestore(&adapter->error_list_lock,
+					       flags2);
+			break;
+		}
+		list_del(&inflight_cmd->list);
+		kfree(inflight_cmd);
+	}
+	spin_unlock_irqrestore(&adapter->inflight_lock, flags);
+}
+
+static void ibmvnic_handle_crq(union ibmvnic_crq *crq,
+			       struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_generic_crq *gen_crq = &crq->generic;
+	struct net_device *netdev = adapter->netdev;
+	struct device *dev = &adapter->vdev->dev;
+	long rc;
+
+	netdev_dbg(netdev, "Handling CRQ: %016lx %016lx\n",
+		   ((unsigned long int *)crq)[0],
+		   ((unsigned long int *)crq)[1]);
+	switch (gen_crq->first) {
+	case IBMVNIC_CRQ_INIT_RSP:
+		switch (gen_crq->cmd) {
+		case IBMVNIC_CRQ_INIT:
+			dev_info(dev, "Partner initialized\n");
+			/* Send back a response */
+			rc = ibmvnic_send_crq_init_complete(adapter);
+			if (rc == 0)
+				send_version_xchg(adapter);
+			else
+				dev_err(dev, "Can't send initrsp rc=%ld\n", rc);
+			break;
+		case IBMVNIC_CRQ_INIT_COMPLETE:
+			dev_info(dev, "Partner initialization complete\n");
+			send_version_xchg(adapter);
+			break;
+		default:
+			dev_err(dev, "Unknown crq cmd: %d\n", gen_crq->cmd);
+		}
+		return;
+	case IBMVNIC_CRQ_XPORT_EVENT:
+		if (gen_crq->cmd == IBMVNIC_PARTITION_MIGRATED) {
+			dev_info(dev, "Re-enabling adapter\n");
+			adapter->migrated = true;
+			ibmvnic_free_inflight(adapter);
+			release_sub_crqs(adapter);
+			rc = ibmvnic_reenable_crq_queue(adapter);
+			if (rc)
+				dev_err(dev, "Error after enable rc=%ld\n", rc);
+			adapter->migrated = false;
+			rc = ibmvnic_send_crq_init(adapter);
+			if (rc)
+				dev_err(dev, "Error sending init rc=%ld\n", rc);
+		} else {
+			/* The adapter lost the connection */
+			dev_err(dev, "Virtual Adapter failed (rc=%d)\n",
+				gen_crq->cmd);
+			ibmvnic_free_inflight(adapter);
+			release_sub_crqs(adapter);
+		}
+		return;
+	case IBMVNIC_CRQ_CMD_RSP:
+		break;
+	default:
+		dev_err(dev, "Got an invalid msg type 0x%02x\n",
+			gen_crq->first);
+		return;
+	}
+
+	switch (gen_crq->cmd) {
+	case VERSION_EXCHANGE_RSP:
+		rc = crq->version_exchange_rsp.rc.code;
+		if (rc) {
+			dev_err(dev, "Error %ld in VERSION_EXCHG_RSP\n", rc);
+			break;
+		}
+		dev_info(dev, "Partner protocol version is %d\n",
+			 crq->version_exchange_rsp.version);
+		if (be16_to_cpu(crq->version_exchange_rsp.version) <
+		    ibmvnic_version)
+			ibmvnic_version =
+			    be16_to_cpu(crq->version_exchange_rsp.version);
+		send_cap_queries(adapter);
+		break;
+	case QUERY_CAPABILITY_RSP:
+		handle_query_cap_rsp(crq, adapter);
+		break;
+	case QUERY_MAP_RSP:
+		handle_query_map_rsp(crq, adapter);
+		break;
+	case REQUEST_MAP_RSP:
+		handle_request_map_rsp(crq, adapter);
+		break;
+	case REQUEST_UNMAP_RSP:
+		handle_request_unmap_rsp(crq, adapter);
+		break;
+	case REQUEST_CAPABILITY_RSP:
+		handle_request_cap_rsp(crq, adapter);
+		break;
+	case LOGIN_RSP:
+		netdev_dbg(netdev, "Got Login Response\n");
+		handle_login_rsp(crq, adapter);
+		break;
+	case LOGICAL_LINK_STATE_RSP:
+		netdev_dbg(netdev, "Got Logical Link State Response\n");
+		adapter->logical_link_state =
+		    crq->logical_link_state_rsp.link_state;
+		break;
+	case LINK_STATE_INDICATION:
+		netdev_dbg(netdev, "Got Logical Link State Indication\n");
+		adapter->phys_link_state =
+		    crq->link_state_indication.phys_link_state;
+		adapter->logical_link_state =
+		    crq->link_state_indication.logical_link_state;
+		break;
+	case CHANGE_MAC_ADDR_RSP:
+		netdev_dbg(netdev, "Got MAC address change Response\n");
+		handle_change_mac_rsp(crq, adapter);
+		break;
+	case ERROR_INDICATION:
+		netdev_dbg(netdev, "Got Error Indication\n");
+		handle_error_indication(crq, adapter);
+		break;
+	case REQUEST_ERROR_RSP:
+		netdev_dbg(netdev, "Got Error Detail Response\n");
+		handle_error_info_rsp(crq, adapter);
+		break;
+	case REQUEST_STATISTICS_RSP:
+		netdev_dbg(netdev, "Got Statistics Response\n");
+		complete(&adapter->stats_done);
+		break;
+	case REQUEST_DUMP_SIZE_RSP:
+		netdev_dbg(netdev, "Got Request Dump Size Response\n");
+		handle_dump_size_rsp(crq, adapter);
+		break;
+	case REQUEST_DUMP_RSP:
+		netdev_dbg(netdev, "Got Request Dump Response\n");
+		complete(&adapter->fw_done);
+		break;
+	case QUERY_IP_OFFLOAD_RSP:
+		netdev_dbg(netdev, "Got Query IP offload Response\n");
+		handle_query_ip_offload_rsp(adapter);
+		break;
+	case MULTICAST_CTRL_RSP:
+		netdev_dbg(netdev, "Got multicast control Response\n");
+		break;
+	case CONTROL_IP_OFFLOAD_RSP:
+		netdev_dbg(netdev, "Got Control IP offload Response\n");
+		dma_unmap_single(dev, adapter->ip_offload_ctrl_tok,
+				 sizeof(adapter->ip_offload_ctrl),
+				 DMA_TO_DEVICE);
+		/* We're done with the queries, perform the login */
+		send_login(adapter);
+		break;
+	case REQUEST_RAS_COMP_NUM_RSP:
+		netdev_dbg(netdev, "Got Request RAS Comp Num Response\n");
+		if (crq->request_ras_comp_num_rsp.rc.code == 10) {
+			netdev_dbg(netdev, "Request RAS Comp Num not supported\n");
+			break;
+		}
+		adapter->ras_comp_num =
+		    be32_to_cpu(crq->request_ras_comp_num_rsp.num_components);
+		handle_request_ras_comp_num_rsp(crq, adapter);
+		break;
+	case REQUEST_RAS_COMPS_RSP:
+		netdev_dbg(netdev, "Got Request RAS Comps Response\n");
+		handle_request_ras_comps_rsp(crq, adapter);
+		break;
+	case CONTROL_RAS_RSP:
+		netdev_dbg(netdev, "Got Control RAS Response\n");
+		handle_control_ras_rsp(crq, adapter);
+		break;
+	case COLLECT_FW_TRACE_RSP:
+		netdev_dbg(netdev, "Got Collect firmware trace Response\n");
+		complete(&adapter->fw_done);
+		break;
+	default:
+		netdev_err(netdev, "Got an invalid cmd type 0x%02x\n",
+			   gen_crq->cmd);
+	}
+}
+
+static irqreturn_t ibmvnic_interrupt(int irq, void *instance)
+{
+	struct ibmvnic_adapter *adapter = instance;
+	struct ibmvnic_crq_queue *queue = &adapter->crq;
+	struct vio_dev *vdev = adapter->vdev;
+	union ibmvnic_crq *crq;
+	unsigned long flags;
+	bool done = false;
+
+	spin_lock_irqsave(&queue->lock, flags);
+	vio_disable_interrupts(vdev);
+	while (!done) {
+		/* Pull all the valid messages off the CRQ */
+		while ((crq = ibmvnic_next_crq(adapter)) != NULL) {
+			ibmvnic_handle_crq(crq, adapter);
+			crq->generic.first = 0;
+		}
+		vio_enable_interrupts(vdev);
+		crq = ibmvnic_next_crq(adapter);
+		if (crq) {
+			vio_disable_interrupts(vdev);
+			ibmvnic_handle_crq(crq, adapter);
+			crq->generic.first = 0;
+		} else {
+			done = true;
+		}
+	}
+	spin_unlock_irqrestore(&queue->lock, flags);
+	return IRQ_HANDLED;
+}
+
+static int ibmvnic_reenable_crq_queue(struct ibmvnic_adapter *adapter)
+{
+	struct vio_dev *vdev = adapter->vdev;
+	int rc;
+
+	do {
+		rc = plpar_hcall_norets(H_ENABLE_CRQ, vdev->unit_address);
+	} while (rc == H_IN_PROGRESS || rc == H_BUSY || H_IS_LONG_BUSY(rc));
+
+	if (rc)
+		dev_err(&vdev->dev, "Error enabling adapter (rc=%d)\n", rc);
+
+	return rc;
+}
+
+static int ibmvnic_reset_crq(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_crq_queue *crq = &adapter->crq;
+	struct device *dev = &adapter->vdev->dev;
+	struct vio_dev *vdev = adapter->vdev;
+	int rc;
+
+	/* Close the CRQ */
+	do {
+		rc = plpar_hcall_norets(H_FREE_CRQ, vdev->unit_address);
+	} while (rc == H_BUSY || H_IS_LONG_BUSY(rc));
+
+	/* Clean out the queue */
+	memset(crq->msgs, 0, PAGE_SIZE);
+	crq->cur = 0;
+
+	/* And re-open it again */
+	rc = plpar_hcall_norets(H_REG_CRQ, vdev->unit_address,
+				crq->msg_token, PAGE_SIZE);
+
+	if (rc == H_CLOSED)
+		/* Adapter is good, but other end is not ready */
+		dev_warn(dev, "Partner adapter not ready\n");
+	else if (rc != 0)
+		dev_warn(dev, "Couldn't register crq (rc=%d)\n", rc);
+
+	return rc;
+}
+
+static void ibmvnic_release_crq_queue(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_crq_queue *crq = &adapter->crq;
+	struct vio_dev *vdev = adapter->vdev;
+	long rc;
+
+	netdev_dbg(adapter->netdev, "Releasing CRQ\n");
+	free_irq(vdev->irq, adapter);
+	do {
+		rc = plpar_hcall_norets(H_FREE_CRQ, vdev->unit_address);
+	} while (rc == H_BUSY || H_IS_LONG_BUSY(rc));
+
+	dma_unmap_single(&vdev->dev, crq->msg_token, PAGE_SIZE,
+			 DMA_BIDIRECTIONAL);
+	free_page((unsigned long)crq->msgs);
+}
+
+static int ibmvnic_init_crq_queue(struct ibmvnic_adapter *adapter)
+{
+	struct ibmvnic_crq_queue *crq = &adapter->crq;
+	struct device *dev = &adapter->vdev->dev;
+	struct vio_dev *vdev = adapter->vdev;
+	int rc, retrc = -ENOMEM;
+
+	crq->msgs = (union ibmvnic_crq *)get_zeroed_page(GFP_KERNEL);
+	/* Should we allocate more than one page? */
+
+	if (!crq->msgs)
+		return -ENOMEM;
+
+	crq->size = PAGE_SIZE / sizeof(*crq->msgs);
+	crq->msg_token = dma_map_single(dev, crq->msgs, PAGE_SIZE,
+					DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(dev, crq->msg_token))
+		goto map_failed;
+
+	rc = plpar_hcall_norets(H_REG_CRQ, vdev->unit_address,
+				crq->msg_token, PAGE_SIZE);
+
+	if (rc == H_RESOURCE)
+		/* maybe kexecing and resource is busy. try a reset */
+		rc = ibmvnic_reset_crq(adapter);
+	retrc = rc;
+
+	if (rc == H_CLOSED) {
+		dev_warn(dev, "Partner adapter not ready\n");
+	} else if (rc) {
+		dev_warn(dev, "Error %d opening adapter\n", rc);
+		goto reg_crq_failed;
+	}
+
+	retrc = 0;
+
+	netdev_dbg(adapter->netdev, "registering irq 0x%x\n", vdev->irq);
+	rc = request_irq(vdev->irq, ibmvnic_interrupt, 0, IBMVNIC_NAME,
+			 adapter);
+	if (rc) {
+		dev_err(dev, "Couldn't register irq 0x%x. rc=%d\n",
+			vdev->irq, rc);
+		goto req_irq_failed;
+	}
+
+	rc = vio_enable_interrupts(vdev);
+	if (rc) {
+		dev_err(dev, "Error %d enabling interrupts\n", rc);
+		goto req_irq_failed;
+	}
+
+	crq->cur = 0;
+	spin_lock_init(&crq->lock);
+
+	return retrc;
+
+req_irq_failed:
+	do {
+		rc = plpar_hcall_norets(H_FREE_CRQ, vdev->unit_address);
+	} while (rc == H_BUSY || H_IS_LONG_BUSY(rc));
+reg_crq_failed:
+	dma_unmap_single(dev, crq->msg_token, PAGE_SIZE, DMA_BIDIRECTIONAL);
+map_failed:
+	free_page((unsigned long)crq->msgs);
+	return retrc;
+}
+
+/* debugfs for dump */
+static int ibmvnic_dump_show(struct seq_file *seq, void *v)
+{
+	struct net_device *netdev = seq->private;
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	struct device *dev = &adapter->vdev->dev;
+	union ibmvnic_crq crq;
+
+	memset(&crq, 0, sizeof(crq));
+	crq.request_dump_size.first = IBMVNIC_CRQ_CMD;
+	crq.request_dump_size.cmd = REQUEST_DUMP_SIZE;
+	ibmvnic_send_crq(adapter, &crq);
+
+	init_completion(&adapter->fw_done);
+	wait_for_completion(&adapter->fw_done);
+
+	seq_write(seq, adapter->dump_data, adapter->dump_data_size);
+
+	dma_unmap_single(dev, adapter->dump_data_token, adapter->dump_data_size,
+			 DMA_BIDIRECTIONAL);
+
+	kfree(adapter->dump_data);
+
+	return 0;
+}
+
+static int ibmvnic_dump_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, ibmvnic_dump_show, inode->i_private);
+}
+
+static const struct file_operations ibmvnic_dump_ops = {
+	.owner          = THIS_MODULE,
+	.open           = ibmvnic_dump_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static int ibmvnic_probe(struct vio_dev *dev, const struct vio_device_id *id)
+{
+	struct ibmvnic_adapter *adapter;
+	struct net_device *netdev;
+	unsigned char *mac_addr_p;
+	struct dentry *ent;
+	char buf[16]; /* debugfs name buf */
+	int rc;
+
+	dev_dbg(&dev->dev, "entering ibmvnic_probe for UA 0x%x\n",
+		dev->unit_address);
+
+	mac_addr_p = (unsigned char *)vio_get_attribute(dev,
+							VETH_MAC_ADDR, NULL);
+	if (!mac_addr_p) {
+		dev_err(&dev->dev,
+			"(%s:%3.3d) ERROR: Can't find MAC_ADDR attribute\n",
+			__FILE__, __LINE__);
+		return 0;
+	}
+
+	netdev = alloc_etherdev_mq(sizeof(struct ibmvnic_adapter),
+				   IBMVNIC_MAX_TX_QUEUES);
+	if (!netdev)
+		return -ENOMEM;
+
+	adapter = netdev_priv(netdev);
+	dev_set_drvdata(&dev->dev, netdev);
+	adapter->vdev = dev;
+	adapter->netdev = netdev;
+
+	ether_addr_copy(adapter->mac_addr, mac_addr_p);
+	ether_addr_copy(netdev->dev_addr, adapter->mac_addr);
+	netdev->irq = dev->irq;
+	netdev->netdev_ops = &ibmvnic_netdev_ops;
+	netdev->ethtool_ops = &ibmvnic_ethtool_ops;
+	SET_NETDEV_DEV(netdev, &dev->dev);
+
+	spin_lock_init(&adapter->stats_lock);
+
+	rc = ibmvnic_init_crq_queue(adapter);
+	if (rc) {
+		dev_err(&dev->dev, "Couldn't initialize crq. rc=%d\n", rc);
+		goto free_netdev;
+	}
+
+	INIT_LIST_HEAD(&adapter->errors);
+	INIT_LIST_HEAD(&adapter->inflight);
+	spin_lock_init(&adapter->error_list_lock);
+	spin_lock_init(&adapter->inflight_lock);
+
+	adapter->stats_token = dma_map_single(&dev->dev, &adapter->stats,
+					      sizeof(struct ibmvnic_statistics),
+					      DMA_FROM_DEVICE);
+	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");
+		goto free_crq;
+	}
+
+	snprintf(buf, sizeof(buf), "ibmvnic_%x", dev->unit_address);
+	ent = debugfs_create_dir(buf, NULL);
+	if (!ent || IS_ERR(ent)) {
+		dev_info(&dev->dev, "debugfs create directory failed\n");
+		adapter->debugfs_dir = NULL;
+	} else {
+		adapter->debugfs_dir = ent;
+		ent = debugfs_create_file("dump", S_IRUGO, adapter->debugfs_dir,
+					  netdev, &ibmvnic_dump_ops);
+		if (!ent || IS_ERR(ent)) {
+			dev_info(&dev->dev,
+				 "debugfs create dump file failed\n");
+			adapter->debugfs_dump = NULL;
+		} else {
+			adapter->debugfs_dump = ent;
+		}
+	}
+	ibmvnic_send_crq_init(adapter);
+
+	init_completion(&adapter->init_done);
+	wait_for_completion(&adapter->init_done);
+
+	/* needed to pull init_sub_crqs outside of an interrupt context
+	 * because it creates IRQ mappings for the subCRQ queues, causing
+	 * a kernel warning
+	 */
+	init_sub_crqs(adapter, 0);
+
+	reinit_completion(&adapter->init_done);
+	wait_for_completion(&adapter->init_done);
+
+	/* if init_sub_crqs is partially successful, retry */
+	while (!adapter->tx_scrq || !adapter->rx_scrq) {
+		init_sub_crqs(adapter, 1);
+
+		reinit_completion(&adapter->init_done);
+		wait_for_completion(&adapter->init_done);
+	}
+
+	netdev->real_num_tx_queues = adapter->req_tx_queues;
+
+	rc = register_netdev(netdev);
+	if (rc) {
+		dev_err(&dev->dev, "failed to register netdev rc=%d\n", rc);
+		goto free_debugfs;
+	}
+	dev_info(&dev->dev, "ibmvnic registered\n");
+
+	return 0;
+
+free_debugfs:
+	if (adapter->debugfs_dir && !IS_ERR(adapter->debugfs_dir))
+		debugfs_remove_recursive(adapter->debugfs_dir);
+free_crq:
+	ibmvnic_release_crq_queue(adapter);
+free_netdev:
+	free_netdev(netdev);
+	return rc;
+}
+
+static int ibmvnic_remove(struct vio_dev *dev)
+{
+	struct net_device *netdev = dev_get_drvdata(&dev->dev);
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+
+	unregister_netdev(netdev);
+
+	release_sub_crqs(adapter);
+
+	ibmvnic_release_crq_queue(adapter);
+
+	if (adapter->debugfs_dir && !IS_ERR(adapter->debugfs_dir))
+		debugfs_remove_recursive(adapter->debugfs_dir);
+
+	if (adapter->ras_comps)
+		dma_free_coherent(&dev->dev,
+				  adapter->ras_comp_num *
+				  sizeof(struct ibmvnic_fw_component),
+				  adapter->ras_comps, adapter->ras_comps_tok);
+
+	kfree(adapter->ras_comp_int);
+
+	free_netdev(netdev);
+	dev_set_drvdata(&dev->dev, NULL);
+
+	return 0;
+}
+
+static unsigned long ibmvnic_get_desired_dma(struct vio_dev *vdev)
+{
+	struct net_device *netdev = dev_get_drvdata(&vdev->dev);
+	struct ibmvnic_adapter *adapter;
+	struct iommu_table *tbl;
+	unsigned long ret = 0;
+	int i;
+
+	tbl = get_iommu_table_base(&vdev->dev);
+
+	/* netdev inits at probe time along with the structures we need below*/
+	if (!netdev)
+		return IOMMU_PAGE_ALIGN(IBMVNIC_IO_ENTITLEMENT_DEFAULT, tbl);
+
+	adapter = netdev_priv(netdev);
+
+	ret += PAGE_SIZE; /* the crq message queue */
+	ret += adapter->bounce_buffer_size;
+	ret += IOMMU_PAGE_ALIGN(sizeof(struct ibmvnic_statistics), tbl);
+
+	for (i = 0; i < adapter->req_tx_queues + adapter->req_rx_queues; i++)
+		ret += 4 * PAGE_SIZE; /* the scrq message queue */
+
+	for (i = 0; i < be32_to_cpu(adapter->login_rsp_buf->num_rxadd_subcrqs);
+	     i++)
+		ret += adapter->rx_pool[i].size *
+		    IOMMU_PAGE_ALIGN(adapter->rx_pool[i].buff_size, tbl);
+
+	return ret;
+}
+
+static int ibmvnic_resume(struct device *dev)
+{
+	struct net_device *netdev = dev_get_drvdata(dev);
+	struct ibmvnic_adapter *adapter = netdev_priv(netdev);
+	int i;
+
+	/* kick the interrupt handlers just in case we lost an interrupt */
+	for (i = 0; i < adapter->req_rx_queues; i++)
+		ibmvnic_interrupt_rx(adapter->rx_scrq[i]->irq,
+				     adapter->rx_scrq[i]);
+
+	return 0;
+}
+
+static struct vio_device_id ibmvnic_device_table[] = {
+	{"network", "IBM,vnic"},
+	{"", "" }
+};
+MODULE_DEVICE_TABLE(vio, ibmvnic_device_table);
+
+static const struct dev_pm_ops ibmvnic_pm_ops = {
+	.resume = ibmvnic_resume
+};
+
+static struct vio_driver ibmvnic_driver = {
+	.id_table       = ibmvnic_device_table,
+	.probe          = ibmvnic_probe,
+	.remove         = ibmvnic_remove,
+	.get_desired_dma = ibmvnic_get_desired_dma,
+	.name		= ibmvnic_driver_name,
+	.pm		= &ibmvnic_pm_ops,
+};
+
+/* module functions */
+static int __init ibmvnic_module_init(void)
+{
+	pr_info("%s: %s %s\n", ibmvnic_driver_name, ibmvnic_driver_string,
+		IBMVNIC_DRIVER_VERSION);
+
+	return vio_register_driver(&ibmvnic_driver);
+}
+
+static void __exit ibmvnic_module_exit(void)
+{
+	vio_unregister_driver(&ibmvnic_driver);
+}
+
+module_init(ibmvnic_module_init);
+module_exit(ibmvnic_module_exit);
diff --git a/drivers/net/ethernet/ibm/ibmvnic.h b/drivers/net/ethernet/ibm/ibmvnic.h
new file mode 100644
index 000000000000..1a9993cc79b5
--- /dev/null
+++ b/drivers/net/ethernet/ibm/ibmvnic.h
@@ -0,0 +1,1044 @@
+/**************************************************************************/
+/*                                                                        */
+/*  IBM System i and System p Virtual NIC Device Driver                   */
+/*  Copyright (C) 2014 IBM Corp.                                          */
+/*  Santiago Leon (santi_leon@yahoo.com)                                  */
+/*  Thomas Falcon (tlfalcon@linux.vnet.ibm.com)                           */
+/*  John Allen (jallen@linux.vnet.ibm.com)                                */
+/*                                                                        */
+/*  This program is free software; you can redistribute it and/or modify  */
+/*  it under the terms of the GNU General Public License as published by  */
+/*  the Free Software Foundation; either version 2 of the License, or     */
+/*  (at your option) any later version.                                   */
+/*                                                                        */
+/*  This program is distributed in the hope that it will be useful,       */
+/*  but WITHOUT ANY WARRANTY; without even the implied warranty of        */
+/*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         */
+/*  GNU General Public License for more details.                          */
+/*                                                                        */
+/*  You should have received a copy of the GNU General Public License     */
+/*  along with this program.                                              */
+/*                                                                        */
+/* This module contains the implementation of a virtual ethernet device   */
+/* for use with IBM i/pSeries LPAR Linux.  It utilizes the logical LAN    */
+/* option of the RS/6000 Platform Architecture to interface with virtual */
+/* ethernet NICs that are presented to the partition by the hypervisor.   */
+/*                                                                        */
+/**************************************************************************/
+
+#define IBMVNIC_NAME		"ibmvnic"
+#define IBMVNIC_DRIVER_VERSION	"1.0"
+#define IBMVNIC_INVALID_MAP	-1
+#define IBMVNIC_STATS_TIMEOUT	1
+/* basic structures plus 100 2k buffers */
+#define IBMVNIC_IO_ENTITLEMENT_DEFAULT	610305
+
+/* Initial module_parameters */
+#define IBMVNIC_RX_WEIGHT		16
+/* when changing this, update IBMVNIC_IO_ENTITLEMENT_DEFAULT */
+#define IBMVNIC_BUFFS_PER_POOL	100
+#define IBMVNIC_MAX_TX_QUEUES	5
+
+struct ibmvnic_login_buffer {
+	__be32 len;
+	__be32 version;
+#define INITIAL_VERSION_LB 1
+	__be32 num_txcomp_subcrqs;
+	__be32 off_txcomp_subcrqs;
+	__be32 num_rxcomp_subcrqs;
+	__be32 off_rxcomp_subcrqs;
+	__be32 login_rsp_ioba;
+	__be32 login_rsp_len;
+} __packed __aligned(8);
+
+struct ibmvnic_login_rsp_buffer {
+	__be32 len;
+	__be32 version;
+#define INITIAL_VERSION_LRB 1
+	__be32 num_txsubm_subcrqs;
+	__be32 off_txsubm_subcrqs;
+	__be32 num_rxadd_subcrqs;
+	__be32 off_rxadd_subcrqs;
+	__be32 off_rxadd_buff_size;
+	__be32 num_supp_tx_desc;
+	__be32 off_supp_tx_desc;
+} __packed __aligned(8);
+
+struct ibmvnic_query_ip_offload_buffer {
+	__be32 len;
+	__be32 version;
+#define INITIAL_VERSION_IOB 1
+	u8 ipv4_chksum;
+	u8 ipv6_chksum;
+	u8 tcp_ipv4_chksum;
+	u8 tcp_ipv6_chksum;
+	u8 udp_ipv4_chksum;
+	u8 udp_ipv6_chksum;
+	u8 large_tx_ipv4;
+	u8 large_tx_ipv6;
+	u8 large_rx_ipv4;
+	u8 large_rx_ipv6;
+	u8 reserved1[14];
+	__be16 max_ipv4_header_size;
+	__be16 max_ipv6_header_size;
+	__be16 max_tcp_header_size;
+	__be16 max_udp_header_size;
+	__be32 max_large_tx_size;
+	__be32 max_large_rx_size;
+	u8 reserved2[16];
+	u8 ipv6_extension_header;
+#define IPV6_EH_NOT_SUPPORTED	0x00
+#define IPV6_EH_SUPPORTED_LIM	0x01
+#define IPV6_EH_SUPPORTED	0xFF
+	u8 tcp_pseudosum_req;
+#define TCP_PS_NOT_REQUIRED	0x00
+#define TCP_PS_REQUIRED		0x01
+	u8 reserved3[30];
+	__be16 num_ipv6_ext_headers;
+	__be32 off_ipv6_ext_headers;
+	u8 reserved4[154];
+} __packed __aligned(8);
+
+struct ibmvnic_control_ip_offload_buffer {
+	__be32 len;
+	__be32 version;
+#define INITIAL_VERSION_IOB 1
+	u8 ipv4_chksum;
+	u8 ipv6_chksum;
+	u8 tcp_ipv4_chksum;
+	u8 tcp_ipv6_chksum;
+	u8 udp_ipv4_chksum;
+	u8 udp_ipv6_chksum;
+	u8 large_tx_ipv4;
+	u8 large_tx_ipv6;
+	u8 bad_packet_rx;
+	u8 large_rx_ipv4;
+	u8 large_rx_ipv6;
+	u8 reserved4[111];
+} __packed __aligned(8);
+
+struct ibmvnic_fw_component {
+	u8 name[48];
+	__be32 trace_buff_size;
+	u8 correlator;
+	u8 trace_level;
+	u8 parent_correlator;
+	u8 error_check_level;
+	u8 trace_on;
+	u8 reserved[7];
+	u8 description[192];
+} __packed __aligned(8);
+
+struct ibmvnic_fw_trace_entry {
+	__be32 trace_id;
+	u8 num_valid_data;
+	u8 reserved[3];
+	__be64 pmc_registers;
+	__be64 timebase;
+	__be64 trace_data[5];
+} __packed __aligned(8);
+
+struct ibmvnic_statistics {
+	__be32 version;
+	__be32 promiscuous;
+	__be64 rx_packets;
+	__be64 rx_bytes;
+	__be64 tx_packets;
+	__be64 tx_bytes;
+	__be64 ucast_tx_packets;
+	__be64 ucast_rx_packets;
+	__be64 mcast_tx_packets;
+	__be64 mcast_rx_packets;
+	__be64 bcast_tx_packets;
+	__be64 bcast_rx_packets;
+	__be64 align_errors;
+	__be64 fcs_errors;
+	__be64 single_collision_frames;
+	__be64 multi_collision_frames;
+	__be64 sqe_test_errors;
+	__be64 deferred_tx;
+	__be64 late_collisions;
+	__be64 excess_collisions;
+	__be64 internal_mac_tx_errors;
+	__be64 carrier_sense;
+	__be64 too_long_frames;
+	__be64 internal_mac_rx_errors;
+	u8 reserved[72];
+} __packed __aligned(8);
+
+struct ibmvnic_acl_buffer {
+	__be32 len;
+	__be32 version;
+#define INITIAL_VERSION_IOB 1
+	u8 mac_acls_restrict;
+	u8 vlan_acls_restrict;
+	u8 reserved1[22];
+	__be32 num_mac_addrs;
+	__be32 offset_mac_addrs;
+	__be32 num_vlan_ids;
+	__be32 offset_vlan_ids;
+	u8 reserved2[80];
+} __packed __aligned(8);
+
+/* descriptors have been changed, how should this be defined?  1? 4? */
+
+#define IBMVNIC_TX_DESC_VERSIONS 3
+
+/* is this still needed? */
+struct ibmvnic_tx_comp_desc {
+	u8 first;
+	u8 num_comps;
+	__be16 rcs[5];
+	__be32 correlators[5];
+} __packed __aligned(8);
+
+/* some flags that included in v0 descriptor, which is gone
+ * only used for IBMVNIC_TCP_CHKSUM and IBMVNIC_UDP_CHKSUM
+ * and only in some offload_flags variable that doesn't seem
+ * to be used anywhere, can probably be removed?
+ */
+
+#define IBMVNIC_TCP_CHKSUM		0x20
+#define IBMVNIC_UDP_CHKSUM		0x08
+
+#define IBMVNIC_MAX_FRAGS_PER_CRQ 3
+
+struct ibmvnic_tx_desc {
+	u8 first;
+	u8 type;
+
+#define IBMVNIC_TX_DESC 0x10
+	u8 n_crq_elem;
+	u8 n_sge;
+	u8 flags1;
+#define IBMVNIC_TX_COMP_NEEDED		0x80
+#define IBMVNIC_TX_CHKSUM_OFFLOAD	0x40
+#define IBMVNIC_TX_LSO			0x20
+#define IBMVNIC_TX_PROT_TCP		0x10
+#define IBMVNIC_TX_PROT_UDP		0x08
+#define IBMVNIC_TX_PROT_IPV4		0x04
+#define IBMVNIC_TX_PROT_IPV6		0x02
+#define IBMVNIC_TX_VLAN_PRESENT		0x01
+	u8 flags2;
+#define IBMVNIC_TX_VLAN_INSERT		0x80
+	__be16 mss;
+	u8 reserved[4];
+	__be32 correlator;
+	__be16 vlan_id;
+	__be16 dma_reg;
+	__be32 sge_len;
+	__be64 ioba;
+} __packed __aligned(8);
+
+struct ibmvnic_hdr_desc {
+	u8 first;
+	u8 type;
+#define IBMVNIC_HDR_DESC		0x11
+	u8 len;
+	u8 l2_len;
+	__be16 l3_len;
+	u8 l4_len;
+	u8 flag;
+	u8 data[24];
+} __packed __aligned(8);
+
+struct ibmvnic_hdr_ext_desc {
+	u8 first;
+	u8 type;
+#define IBMVNIC_HDR_EXT_DESC		0x12
+	u8 len;
+	u8 data[29];
+} __packed __aligned(8);
+
+struct ibmvnic_sge_desc {
+	u8 first;
+	u8 type;
+#define IBMVNIC_SGE_DESC		0x30
+	__be16 sge1_dma_reg;
+	__be32 sge1_len;
+	__be64 sge1_ioba;
+	__be16 reserved;
+	__be16 sge2_dma_reg;
+	__be32 sge2_len;
+	__be64 sge2_ioba;
+} __packed __aligned(8);
+
+struct ibmvnic_rx_comp_desc {
+	u8 first;
+	u8 flags;
+#define IBMVNIC_IP_CHKSUM_GOOD		0x80
+#define IBMVNIC_TCP_UDP_CHKSUM_GOOD	0x40
+#define IBMVNIC_END_FRAME			0x20
+#define IBMVNIC_EXACT_MC			0x10
+#define IBMVNIC_VLAN_STRIPPED			0x08
+	__be16 off_frame_data;
+	__be32 len;
+	__be64 correlator;
+	__be16 vlan_tci;
+	__be16 rc;
+	u8 reserved[12];
+} __packed __aligned(8);
+
+struct ibmvnic_generic_scrq {
+	u8 first;
+	u8 reserved[31];
+} __packed __aligned(8);
+
+struct ibmvnic_rx_buff_add_desc {
+	u8 first;
+	u8 reserved[7];
+	__be64 correlator;
+	__be32 ioba;
+	u8 map_id;
+	__be32 len:24;
+	u8 reserved2[8];
+} __packed __aligned(8);
+
+struct ibmvnic_rc {
+	u8 code; /* one of enum ibmvnic_rc_codes */
+	u8 detailed_data[3];
+} __packed __aligned(4);
+
+struct ibmvnic_generic_crq {
+	u8 first;
+	u8 cmd;
+	u8 params[10];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_version_exchange {
+	u8 first;
+	u8 cmd;
+	__be16 version;
+#define IBMVNIC_INITIAL_VERSION 1
+	u8 reserved[8];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_capability {
+	u8 first;
+	u8 cmd;
+	__be16 capability; /* one of ibmvnic_capabilities */
+	__be64 number;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_login {
+	u8 first;
+	u8 cmd;
+	u8 reserved[6];
+	__be32 ioba;
+	__be32 len;
+} __packed __aligned(8);
+
+struct ibmvnic_phys_parms {
+	u8 first;
+	u8 cmd;
+	u8 flags1;
+#define IBMVNIC_EXTERNAL_LOOPBACK	0x80
+#define IBMVNIC_INTERNAL_LOOPBACK	0x40
+#define IBMVNIC_PROMISC		0x20
+#define IBMVNIC_PHYS_LINK_ACTIVE	0x10
+#define IBMVNIC_AUTONEG_DUPLEX	0x08
+#define IBMVNIC_FULL_DUPLEX	0x04
+#define IBMVNIC_HALF_DUPLEX	0x02
+#define IBMVNIC_CAN_CHG_PHYS_PARMS	0x01
+	u8 flags2;
+#define IBMVNIC_LOGICAL_LNK_ACTIVE 0x80
+	__be32 speed;
+#define IBMVNIC_AUTONEG		0x80
+#define IBMVNIC_10MBPS		0x40
+#define IBMVNIC_100MBPS		0x20
+#define IBMVNIC_1GBPS		0x10
+#define IBMVNIC_10GBPS		0x08
+	__be32 mtu;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_logical_link_state {
+	u8 first;
+	u8 cmd;
+	u8 link_state;
+#define IBMVNIC_LOGICAL_LNK_DN 0x00
+#define IBMVNIC_LOGICAL_LNK_UP 0x01
+#define IBMVNIC_LOGICAL_LNK_QUERY 0xff
+	u8 reserved[9];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_query_ip_offload {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 len;
+	__be32 ioba;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_control_ip_offload {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 ioba;
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_dump_size {
+	u8 first;
+	u8 cmd;
+	u8 reserved[6];
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_dump {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	__be32 ioba;
+	__be32 len;
+	u8 reserved2[4];
+} __packed __aligned(8);
+
+struct ibmvnic_request_dump_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved[6];
+	__be32 dumped_len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_ras_comp_num {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	__be32 num_components;
+	u8 reserved2[4];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_ras_comps {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 ioba;
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_control_ras {
+	u8 first;
+	u8 cmd;
+	u8 correlator;
+	u8 level;
+	u8 op;
+#define IBMVNIC_TRACE_LEVEL	1
+#define IBMVNIC_ERROR_LEVEL	2
+#define IBMVNIC_TRACE_PAUSE	3
+#define IBMVNIC_TRACE_RESUME	4
+#define IBMVNIC_TRACE_ON		5
+#define IBMVNIC_TRACE_OFF		6
+#define IBMVNIC_CHG_TRACE_BUFF_SZ	7
+	u8 trace_buff_sz[3];
+	u8 reserved[4];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_collect_fw_trace {
+	u8 first;
+	u8 cmd;
+	u8 correlator;
+	u8 reserved;
+	__be32 ioba;
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_statistics {
+	u8 first;
+	u8 cmd;
+	u8 flags;
+#define IBMVNIC_PHYSICAL_PORT	0x80
+	u8 reserved1;
+	__be32 ioba;
+	__be32 len;
+	u8 reserved[4];
+} __packed __aligned(8);
+
+struct ibmvnic_request_debug_stats {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 ioba;
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_error_indication {
+	u8 first;
+	u8 cmd;
+	u8 flags;
+#define IBMVNIC_FATAL_ERROR	0x80
+	u8 reserved1;
+	__be32 error_id;
+	__be32 detail_error_sz;
+	__be16 error_cause;
+	u8 reserved2[2];
+} __packed __aligned(8);
+
+struct ibmvnic_request_error_info {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 ioba;
+	__be32 len;
+	__be32 error_id;
+} __packed __aligned(8);
+
+struct ibmvnic_request_error_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be32 error_id;
+	__be32 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_link_state_indication {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	u8 phys_link_state;
+	u8 logical_link_state;
+	u8 reserved2[10];
+} __packed __aligned(8);
+
+struct ibmvnic_change_mac_addr {
+	u8 first;
+	u8 cmd;
+	u8 mac_addr[6];
+	struct ibmvnic_rc rc;
+	u8 reserved[4];
+} __packed __aligned(8);
+
+struct ibmvnic_multicast_ctrl {
+	u8 first;
+	u8 cmd;
+	u8 mac_addr[6];
+	u8 flags;
+#define IBMVNIC_ENABLE_MC		0x80
+#define IBMVNIC_DISABLE_MC		0x40
+#define IBMVNIC_ENABLE_ALL		0x20
+#define IBMVNIC_DISABLE_ALL	0x10
+	u8 reserved1;
+	__be16 reserved2; /* was num_enabled_mc_addr; */
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_get_vpd_size_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved[2];
+	__be64 len;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_get_vpd {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	__be32 ioba;
+	__be32 len;
+	u8 reserved[4];
+} __packed __aligned(8);
+
+struct ibmvnic_acl_change_indication {
+	u8 first;
+	u8 cmd;
+	__be16 change_type;
+#define IBMVNIC_MAC_ACL 0
+#define IBMVNIC_VLAN_ACL 1
+	u8 reserved[12];
+} __packed __aligned(8);
+
+struct ibmvnic_acl_query {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	__be32 ioba;
+	__be32 len;
+	u8 reserved2[4];
+} __packed __aligned(8);
+
+struct ibmvnic_tune {
+	u8 first;
+	u8 cmd;
+	u8 reserved1[2];
+	__be32 ioba;
+	__be32 len;
+	u8 reserved2[4];
+} __packed __aligned(8);
+
+struct ibmvnic_request_map {
+	u8 first;
+	u8 cmd;
+	u8 reserved1;
+	u8 map_id;
+	__be32 ioba;
+	__be32 len;
+	u8 reserved2[4];
+} __packed __aligned(8);
+
+struct ibmvnic_request_map_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved1;
+	u8 map_id;
+	u8 reserved2[4];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_request_unmap {
+	u8 first;
+	u8 cmd;
+	u8 reserved1;
+	u8 map_id;
+	u8 reserved2[12];
+} __packed __aligned(8);
+
+struct ibmvnic_request_unmap_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved1;
+	u8 map_id;
+	u8 reserved2[8];
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+struct ibmvnic_query_map {
+	u8 first;
+	u8 cmd;
+	u8 reserved[14];
+} __packed __aligned(8);
+
+struct ibmvnic_query_map_rsp {
+	u8 first;
+	u8 cmd;
+	u8 reserved;
+	u8 page_size;
+	__be32 tot_pages;
+	__be32 free_pages;
+	struct ibmvnic_rc rc;
+} __packed __aligned(8);
+
+union ibmvnic_crq {
+	struct ibmvnic_generic_crq generic;
+	struct ibmvnic_version_exchange version_exchange;
+	struct ibmvnic_version_exchange version_exchange_rsp;
+	struct ibmvnic_capability query_capability;
+	struct ibmvnic_capability query_capability_rsp;
+	struct ibmvnic_capability request_capability;
+	struct ibmvnic_capability request_capability_rsp;
+	struct ibmvnic_login login;
+	struct ibmvnic_generic_crq login_rsp;
+	struct ibmvnic_phys_parms query_phys_parms;
+	struct ibmvnic_phys_parms query_phys_parms_rsp;
+	struct ibmvnic_phys_parms query_phys_capabilities;
+	struct ibmvnic_phys_parms query_phys_capabilities_rsp;
+	struct ibmvnic_phys_parms set_phys_parms;
+	struct ibmvnic_phys_parms set_phys_parms_rsp;
+	struct ibmvnic_logical_link_state logical_link_state;
+	struct ibmvnic_logical_link_state logical_link_state_rsp;
+	struct ibmvnic_query_ip_offload query_ip_offload;
+	struct ibmvnic_query_ip_offload query_ip_offload_rsp;
+	struct ibmvnic_control_ip_offload control_ip_offload;
+	struct ibmvnic_control_ip_offload control_ip_offload_rsp;
+	struct ibmvnic_request_dump_size request_dump_size;
+	struct ibmvnic_request_dump_size request_dump_size_rsp;
+	struct ibmvnic_request_dump request_dump;
+	struct ibmvnic_request_dump_rsp request_dump_rsp;
+	struct ibmvnic_request_ras_comp_num request_ras_comp_num;
+	struct ibmvnic_request_ras_comp_num request_ras_comp_num_rsp;
+	struct ibmvnic_request_ras_comps request_ras_comps;
+	struct ibmvnic_request_ras_comps request_ras_comps_rsp;
+	struct ibmvnic_control_ras control_ras;
+	struct ibmvnic_control_ras control_ras_rsp;
+	struct ibmvnic_collect_fw_trace collect_fw_trace;
+	struct ibmvnic_collect_fw_trace collect_fw_trace_rsp;
+	struct ibmvnic_request_statistics request_statistics;
+	struct ibmvnic_generic_crq request_statistics_rsp;
+	struct ibmvnic_request_debug_stats request_debug_stats;
+	struct ibmvnic_request_debug_stats request_debug_stats_rsp;
+	struct ibmvnic_error_indication error_indication;
+	struct ibmvnic_request_error_info request_error_info;
+	struct ibmvnic_request_error_rsp request_error_rsp;
+	struct ibmvnic_link_state_indication link_state_indication;
+	struct ibmvnic_change_mac_addr change_mac_addr;
+	struct ibmvnic_change_mac_addr change_mac_addr_rsp;
+	struct ibmvnic_multicast_ctrl multicast_ctrl;
+	struct ibmvnic_multicast_ctrl multicast_ctrl_rsp;
+	struct ibmvnic_generic_crq get_vpd_size;
+	struct ibmvnic_get_vpd_size_rsp get_vpd_size_rsp;
+	struct ibmvnic_get_vpd get_vpd;
+	struct ibmvnic_generic_crq get_vpd_rsp;
+	struct ibmvnic_acl_change_indication acl_change_indication;
+	struct ibmvnic_acl_query acl_query;
+	struct ibmvnic_generic_crq acl_query_rsp;
+	struct ibmvnic_tune tune;
+	struct ibmvnic_generic_crq tune_rsp;
+	struct ibmvnic_request_map request_map;
+	struct ibmvnic_request_map_rsp request_map_rsp;
+	struct ibmvnic_request_unmap request_unmap;
+	struct ibmvnic_request_unmap_rsp request_unmap_rsp;
+	struct ibmvnic_query_map query_map;
+	struct ibmvnic_query_map_rsp query_map_rsp;
+};
+
+enum ibmvnic_rc_codes {
+	SUCCESS = 0,
+	PARTIALSUCCESS = 1,
+	PERMISSION = 2,
+	NOMEMORY = 3,
+	PARAMETER = 4,
+	UNKNOWNCOMMAND = 5,
+	ABORTED = 6,
+	INVALIDSTATE = 7,
+	INVALIDIOBA = 8,
+	INVALIDLENGTH = 9,
+	UNSUPPORTEDOPTION = 10,
+};
+
+enum ibmvnic_capabilities {
+	MIN_TX_QUEUES = 1,
+	MIN_RX_QUEUES = 2,
+	MIN_RX_ADD_QUEUES = 3,
+	MAX_TX_QUEUES = 4,
+	MAX_RX_QUEUES = 5,
+	MAX_RX_ADD_QUEUES = 6,
+	REQ_TX_QUEUES = 7,
+	REQ_RX_QUEUES = 8,
+	REQ_RX_ADD_QUEUES = 9,
+	MIN_TX_ENTRIES_PER_SUBCRQ = 10,
+	MIN_RX_ADD_ENTRIES_PER_SUBCRQ = 11,
+	MAX_TX_ENTRIES_PER_SUBCRQ = 12,
+	MAX_RX_ADD_ENTRIES_PER_SUBCRQ = 13,
+	REQ_TX_ENTRIES_PER_SUBCRQ = 14,
+	REQ_RX_ADD_ENTRIES_PER_SUBCRQ = 15,
+	TCP_IP_OFFLOAD = 16,
+	PROMISC_REQUESTED = 17,
+	PROMISC_SUPPORTED = 18,
+	MIN_MTU = 19,
+	MAX_MTU = 20,
+	REQ_MTU = 21,
+	MAX_MULTICAST_FILTERS = 22,
+	VLAN_HEADER_INSERTION = 23,
+	MAX_TX_SG_ENTRIES = 25,
+	RX_SG_SUPPORTED = 26,
+	RX_SG_REQUESTED = 27,
+	OPT_TX_COMP_SUB_QUEUES = 28,
+	OPT_RX_COMP_QUEUES = 29,
+	OPT_RX_BUFADD_Q_PER_RX_COMP_Q = 30,
+	OPT_TX_ENTRIES_PER_SUBCRQ = 31,
+	OPT_RXBA_ENTRIES_PER_SUBCRQ = 32,
+	TX_RX_DESC_REQ = 33,
+};
+
+enum ibmvnic_error_cause {
+	ADAPTER_PROBLEM = 0,
+	BUS_PROBLEM = 1,
+	FW_PROBLEM = 2,
+	DD_PROBLEM = 3,
+	EEH_RECOVERY = 4,
+	FW_UPDATED = 5,
+	LOW_MEMORY = 6,
+};
+
+enum ibmvnic_commands {
+	VERSION_EXCHANGE = 0x01,
+	VERSION_EXCHANGE_RSP = 0x81,
+	QUERY_CAPABILITY = 0x02,
+	QUERY_CAPABILITY_RSP = 0x82,
+	REQUEST_CAPABILITY = 0x03,
+	REQUEST_CAPABILITY_RSP = 0x83,
+	LOGIN = 0x04,
+	LOGIN_RSP = 0x84,
+	QUERY_PHYS_PARMS = 0x05,
+	QUERY_PHYS_PARMS_RSP = 0x85,
+	QUERY_PHYS_CAPABILITIES = 0x06,
+	QUERY_PHYS_CAPABILITIES_RSP = 0x86,
+	SET_PHYS_PARMS = 0x07,
+	SET_PHYS_PARMS_RSP = 0x87,
+	ERROR_INDICATION = 0x08,
+	REQUEST_ERROR_INFO = 0x09,
+	REQUEST_ERROR_RSP = 0x89,
+	REQUEST_DUMP_SIZE = 0x0A,
+	REQUEST_DUMP_SIZE_RSP = 0x8A,
+	REQUEST_DUMP = 0x0B,
+	REQUEST_DUMP_RSP = 0x8B,
+	LOGICAL_LINK_STATE = 0x0C,
+	LOGICAL_LINK_STATE_RSP = 0x8C,
+	REQUEST_STATISTICS = 0x0D,
+	REQUEST_STATISTICS_RSP = 0x8D,
+	REQUEST_RAS_COMP_NUM = 0x0E,
+	REQUEST_RAS_COMP_NUM_RSP = 0x8E,
+	REQUEST_RAS_COMPS = 0x0F,
+	REQUEST_RAS_COMPS_RSP = 0x8F,
+	CONTROL_RAS = 0x10,
+	CONTROL_RAS_RSP = 0x90,
+	COLLECT_FW_TRACE = 0x11,
+	COLLECT_FW_TRACE_RSP = 0x91,
+	LINK_STATE_INDICATION = 0x12,
+	CHANGE_MAC_ADDR = 0x13,
+	CHANGE_MAC_ADDR_RSP = 0x93,
+	MULTICAST_CTRL = 0x14,
+	MULTICAST_CTRL_RSP = 0x94,
+	GET_VPD_SIZE = 0x15,
+	GET_VPD_SIZE_RSP = 0x95,
+	GET_VPD = 0x16,
+	GET_VPD_RSP = 0x96,
+	TUNE = 0x17,
+	TUNE_RSP = 0x97,
+	QUERY_IP_OFFLOAD = 0x18,
+	QUERY_IP_OFFLOAD_RSP = 0x98,
+	CONTROL_IP_OFFLOAD = 0x19,
+	CONTROL_IP_OFFLOAD_RSP = 0x99,
+	ACL_CHANGE_INDICATION = 0x1A,
+	ACL_QUERY = 0x1B,
+	ACL_QUERY_RSP = 0x9B,
+	REQUEST_DEBUG_STATS = 0x1C,
+	REQUEST_DEBUG_STATS_RSP = 0x9C,
+	QUERY_MAP = 0x1D,
+	QUERY_MAP_RSP = 0x9D,
+	REQUEST_MAP = 0x1E,
+	REQUEST_MAP_RSP = 0x9E,
+	REQUEST_UNMAP = 0x1F,
+	REQUEST_UNMAP_RSP = 0x9F,
+	VLAN_CTRL = 0x20,
+	VLAN_CTRL_RSP = 0xA0,
+};
+
+enum ibmvnic_crq_type {
+	IBMVNIC_CRQ_CMD			= 0x80,
+	IBMVNIC_CRQ_CMD_RSP		= 0x80,
+	IBMVNIC_CRQ_INIT_CMD		= 0xC0,
+	IBMVNIC_CRQ_INIT_RSP		= 0xC0,
+	IBMVNIC_CRQ_XPORT_EVENT		= 0xFF,
+};
+
+enum ibmvfc_crq_format {
+	IBMVNIC_CRQ_INIT                 = 0x01,
+	IBMVNIC_CRQ_INIT_COMPLETE        = 0x02,
+	IBMVNIC_PARTITION_MIGRATED       = 0x06,
+};
+
+struct ibmvnic_crq_queue {
+	union ibmvnic_crq *msgs;
+	int size, cur;
+	dma_addr_t msg_token;
+	spinlock_t lock;
+};
+
+union sub_crq {
+	struct ibmvnic_generic_scrq generic;
+	struct ibmvnic_tx_comp_desc tx_comp;
+	struct ibmvnic_tx_desc v1;
+	struct ibmvnic_hdr_desc hdr;
+	struct ibmvnic_hdr_ext_desc hdr_ext;
+	struct ibmvnic_sge_desc sge;
+	struct ibmvnic_rx_comp_desc rx_comp;
+	struct ibmvnic_rx_buff_add_desc rx_add;
+};
+
+struct ibmvnic_sub_crq_queue {
+	union sub_crq *msgs;
+	int size, cur;
+	dma_addr_t msg_token;
+	unsigned long crq_num;
+	unsigned long hw_irq;
+	unsigned int irq;
+	unsigned int pool_index;
+	int scrq_num;
+	spinlock_t lock;
+	struct sk_buff *rx_skb_top;
+	struct ibmvnic_adapter *adapter;
+};
+
+struct ibmvnic_long_term_buff {
+	unsigned char *buff;
+	dma_addr_t addr;
+	u64 size;
+	u8 map_id;
+};
+
+struct ibmvnic_tx_buff {
+	struct sk_buff *skb;
+	dma_addr_t data_dma[IBMVNIC_MAX_FRAGS_PER_CRQ];
+	unsigned int data_len[IBMVNIC_MAX_FRAGS_PER_CRQ];
+	int index;
+	int pool_index;
+	bool last_frag;
+	bool used_bounce;
+};
+
+struct ibmvnic_tx_pool {
+	struct ibmvnic_tx_buff *tx_buff;
+	int *free_map;
+	int consumer_index;
+	int producer_index;
+	wait_queue_head_t ibmvnic_tx_comp_q;
+	struct task_struct *work_thread;
+	struct ibmvnic_long_term_buff long_term_buff;
+};
+
+struct ibmvnic_rx_buff {
+	struct sk_buff *skb;
+	dma_addr_t dma;
+	unsigned char *data;
+	int size;
+	int pool_index;
+};
+
+struct ibmvnic_rx_pool {
+	struct ibmvnic_rx_buff *rx_buff;
+	int size;
+	int index;
+	int buff_size;
+	atomic_t available;
+	int *free_map;
+	int next_free;
+	int next_alloc;
+	int active;
+	struct ibmvnic_long_term_buff long_term_buff;
+};
+
+struct ibmvnic_error_buff {
+	char *buff;
+	dma_addr_t dma;
+	int len;
+	struct list_head list;
+	__be32 error_id;
+};
+
+struct ibmvnic_fw_comp_internal {
+	struct ibmvnic_adapter *adapter;
+	int num;
+	struct debugfs_blob_wrapper desc_blob;
+	int paused;
+};
+
+struct ibmvnic_inflight_cmd {
+	union ibmvnic_crq crq;
+	struct list_head list;
+};
+
+struct ibmvnic_adapter {
+	struct vio_dev *vdev;
+	struct net_device *netdev;
+	struct ibmvnic_crq_queue crq;
+	u8 mac_addr[ETH_ALEN];
+	struct ibmvnic_query_ip_offload_buffer ip_offload_buf;
+	dma_addr_t ip_offload_tok;
+	struct ibmvnic_control_ip_offload_buffer ip_offload_ctrl;
+	dma_addr_t ip_offload_ctrl_tok;
+	bool migrated;
+	u32 msg_enable;
+	void *bounce_buffer;
+	int bounce_buffer_size;
+	dma_addr_t bounce_buffer_dma;
+
+	/* Statistics */
+	struct net_device_stats net_stats;
+	struct ibmvnic_statistics stats;
+	dma_addr_t stats_token;
+	struct completion stats_done;
+	spinlock_t stats_lock;
+	int replenish_no_mem;
+	int replenish_add_buff_success;
+	int replenish_add_buff_failure;
+	int replenish_task_cycles;
+	int tx_send_failed;
+	int tx_map_failed;
+
+	int phys_link_state;
+	int logical_link_state;
+
+	/* login data */
+	struct ibmvnic_login_buffer *login_buf;
+	dma_addr_t login_buf_token;
+	int login_buf_sz;
+
+	struct ibmvnic_login_rsp_buffer *login_rsp_buf;
+	dma_addr_t login_rsp_buf_token;
+	int login_rsp_buf_sz;
+
+	atomic_t running_cap_queries;
+
+	struct ibmvnic_sub_crq_queue **tx_scrq;
+	struct ibmvnic_sub_crq_queue **rx_scrq;
+	int requested_caps;
+
+	/* rx structs */
+	struct napi_struct *napi;
+	struct ibmvnic_rx_pool *rx_pool;
+	u64 promisc;
+
+	struct ibmvnic_tx_pool *tx_pool;
+	bool closing;
+	struct completion init_done;
+
+	struct list_head errors;
+	spinlock_t error_list_lock;
+
+	/* debugfs */
+	struct dentry *debugfs_dir;
+	struct dentry *debugfs_dump;
+	struct completion fw_done;
+	char *dump_data;
+	dma_addr_t dump_data_token;
+	int dump_data_size;
+	int ras_comp_num;
+	struct ibmvnic_fw_component *ras_comps;
+	struct ibmvnic_fw_comp_internal *ras_comp_int;
+	dma_addr_t ras_comps_tok;
+	struct dentry *ras_comps_ent;
+
+	/* in-flight commands that allocate and/or map memory*/
+	struct list_head inflight;
+	spinlock_t inflight_lock;
+
+	/* partner capabilities */
+	u64 min_tx_queues;
+	u64 min_rx_queues;
+	u64 min_rx_add_queues;
+	u64 max_tx_queues;
+	u64 max_rx_queues;
+	u64 max_rx_add_queues;
+	u64 req_tx_queues;
+	u64 req_rx_queues;
+	u64 req_rx_add_queues;
+	u64 min_tx_entries_per_subcrq;
+	u64 min_rx_add_entries_per_subcrq;
+	u64 max_tx_entries_per_subcrq;
+	u64 max_rx_add_entries_per_subcrq;
+	u64 req_tx_entries_per_subcrq;
+	u64 req_rx_add_entries_per_subcrq;
+	u64 tcp_ip_offload;
+	u64 promisc_requested;
+	u64 promisc_supported;
+	u64 min_mtu;
+	u64 max_mtu;
+	u64 req_mtu;
+	u64 max_multicast_filters;
+	u64 vlan_header_insertion;
+	u64 max_tx_sg_entries;
+	u64 rx_sg_supported;
+	u64 rx_sg_requested;
+	u64 opt_tx_comp_sub_queues;
+	u64 opt_rx_comp_queues;
+	u64 opt_rx_bufadd_q_per_rx_comp_q;
+	u64 opt_tx_entries_per_subcrq;
+	u64 opt_rxba_entries_per_subcrq;
+	__be64 tx_rx_desc_req;
+	u8 map_id;
+};
diff --git a/drivers/net/ethernet/intel/Kconfig b/drivers/net/ethernet/intel/Kconfig
index 4163b16489b3..fa593dd3efe1 100644
--- a/drivers/net/ethernet/intel/Kconfig
+++ b/drivers/net/ethernet/intel/Kconfig
@@ -280,6 +280,16 @@ config I40E_VXLAN
 	  Say Y here if you want to use Virtual eXtensible Local Area Network
 	  (VXLAN) in the driver.
 
+config I40E_GENEVE
+	bool "Generic Network Virtualization Encapsulation (GENEVE) Support"
+	depends on I40E && GENEVE && !(I40E=y && GENEVE=m)
+	default n
+	---help---
+	  This allows one to create GENEVE virtual interfaces that provide
+	  Layer 2 Networks over Layer 3 Networks. GENEVE is often used
+	  to tunnel virtual network infrastructure in virtualized environments.
+	  Say Y here if you want to use GENEVE in the driver.
+
 config I40E_DCB
 	bool "Data Center Bridging (DCB) Support"
 	default n
diff --git a/drivers/net/ethernet/intel/e1000/e1000.h b/drivers/net/ethernet/intel/e1000/e1000.h
index 69707108d23c..98fe5a2cd6e3 100644
--- a/drivers/net/ethernet/intel/e1000/e1000.h
+++ b/drivers/net/ethernet/intel/e1000/e1000.h
@@ -213,8 +213,11 @@ struct e1000_rx_ring {
 };
 
 #define E1000_DESC_UNUSED(R)						\
-	((((R)->next_to_clean > (R)->next_to_use)			\
-	  ? 0 : (R)->count) + (R)->next_to_clean - (R)->next_to_use - 1)
+({									\
+	unsigned int clean = smp_load_acquire(&(R)->next_to_clean);	\
+	unsigned int use = READ_ONCE((R)->next_to_use);			\
+	(clean > use ? 0 : (R)->count) + clean - use - 1;		\
+})
 
 #define E1000_RX_DESC_EXT(R, i)						\
 	(&(((union e1000_rx_desc_extended *)((R).desc))[i]))
diff --git a/drivers/net/ethernet/intel/e1000/e1000_hw.c b/drivers/net/ethernet/intel/e1000/e1000_hw.c
index b1af0d613caa..8172cf08cc33 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_hw.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_hw.c
@@ -1,5 +1,5 @@
 /*******************************************************************************
-
+*
   Intel PRO/1000 Linux driver
   Copyright(c) 1999 - 2006 Intel Corporation.
 
@@ -106,7 +106,7 @@ u16 e1000_igp_cable_length_table[IGP01E1000_AGC_LENGTH_TABLE_SIZE] = {
 	    120, 120
 };
 
-static DEFINE_SPINLOCK(e1000_eeprom_lock);
+static DEFINE_MUTEX(e1000_eeprom_lock);
 static DEFINE_SPINLOCK(e1000_phy_lock);
 
 /**
@@ -624,8 +624,8 @@ s32 e1000_init_hw(struct e1000_hw *hw)
 		/* Workaround for PCI-X problem when BIOS sets MMRBC
 		 * incorrectly.
 		 */
-		if (hw->bus_type == e1000_bus_type_pcix
-		    && e1000_pcix_get_mmrbc(hw) > 2048)
+		if (hw->bus_type == e1000_bus_type_pcix &&
+		    e1000_pcix_get_mmrbc(hw) > 2048)
 			e1000_pcix_set_mmrbc(hw, 2048);
 		break;
 	}
@@ -683,10 +683,9 @@ static s32 e1000_adjust_serdes_amplitude(struct e1000_hw *hw)
 	}
 
 	ret_val = e1000_read_eeprom(hw, EEPROM_SERDES_AMPLITUDE, 1,
-	                            &eeprom_data);
-	if (ret_val) {
+				    &eeprom_data);
+	if (ret_val)
 		return ret_val;
-	}
 
 	if (eeprom_data != EEPROM_RESERVED_WORD) {
 		/* Adjust SERDES output amplitude only. */
@@ -1074,8 +1073,8 @@ static s32 e1000_copper_link_preconfig(struct e1000_hw *hw)
 
 	if (hw->mac_type <= e1000_82543 ||
 	    hw->mac_type == e1000_82541 || hw->mac_type == e1000_82547 ||
-	    hw->mac_type == e1000_82541_rev_2
-	    || hw->mac_type == e1000_82547_rev_2)
+	    hw->mac_type == e1000_82541_rev_2 ||
+	    hw->mac_type == e1000_82547_rev_2)
 		hw->phy_reset_disable = false;
 
 	return E1000_SUCCESS;
@@ -1652,7 +1651,7 @@ s32 e1000_phy_setup_autoneg(struct e1000_hw *hw)
 		mii_1000t_ctrl_reg = 0;
 	} else {
 		ret_val = e1000_write_phy_reg(hw, PHY_1000T_CTRL,
-		                              mii_1000t_ctrl_reg);
+					      mii_1000t_ctrl_reg);
 		if (ret_val)
 			return ret_val;
 	}
@@ -1881,10 +1880,11 @@ static s32 e1000_phy_force_speed_duplex(struct e1000_hw *hw)
 		if (ret_val)
 			return ret_val;
 
-		if ((hw->mac_type == e1000_82544 || hw->mac_type == e1000_82543)
-		    && (!hw->autoneg)
-		    && (hw->forced_speed_duplex == e1000_10_full
-			|| hw->forced_speed_duplex == e1000_10_half)) {
+		if ((hw->mac_type == e1000_82544 ||
+		     hw->mac_type == e1000_82543) &&
+		    (!hw->autoneg) &&
+		    (hw->forced_speed_duplex == e1000_10_full ||
+		     hw->forced_speed_duplex == e1000_10_half)) {
 			ret_val = e1000_polarity_reversal_workaround(hw);
 			if (ret_val)
 				return ret_val;
@@ -2084,11 +2084,12 @@ static s32 e1000_config_fc_after_link_up(struct e1000_hw *hw)
 	 * so we had to force link.  In this case, we need to force the
 	 * configuration of the MAC to match the "fc" parameter.
 	 */
-	if (((hw->media_type == e1000_media_type_fiber) && (hw->autoneg_failed))
-	    || ((hw->media_type == e1000_media_type_internal_serdes)
-		&& (hw->autoneg_failed))
-	    || ((hw->media_type == e1000_media_type_copper)
-		&& (!hw->autoneg))) {
+	if (((hw->media_type == e1000_media_type_fiber) &&
+	     (hw->autoneg_failed)) ||
+	    ((hw->media_type == e1000_media_type_internal_serdes) &&
+	     (hw->autoneg_failed)) ||
+	    ((hw->media_type == e1000_media_type_copper) &&
+	     (!hw->autoneg))) {
 		ret_val = e1000_force_mac_fc(hw);
 		if (ret_val) {
 			e_dbg("Error forcing flow control settings\n");
@@ -2193,8 +2194,7 @@ static s32 e1000_config_fc_after_link_up(struct e1000_hw *hw)
 			else if (!(mii_nway_adv_reg & NWAY_AR_PAUSE) &&
 				 (mii_nway_adv_reg & NWAY_AR_ASM_DIR) &&
 				 (mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
-				 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR))
-			{
+				 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 				hw->fc = E1000_FC_TX_PAUSE;
 				e_dbg
 				    ("Flow Control = TX PAUSE frames only.\n");
@@ -2210,8 +2210,7 @@ static s32 e1000_config_fc_after_link_up(struct e1000_hw *hw)
 			else if ((mii_nway_adv_reg & NWAY_AR_PAUSE) &&
 				 (mii_nway_adv_reg & NWAY_AR_ASM_DIR) &&
 				 !(mii_nway_lp_ability_reg & NWAY_LPAR_PAUSE) &&
-				 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR))
-			{
+				 (mii_nway_lp_ability_reg & NWAY_LPAR_ASM_DIR)) {
 				hw->fc = E1000_FC_RX_PAUSE;
 				e_dbg
 				    ("Flow Control = RX PAUSE frames only.\n");
@@ -2460,10 +2459,11 @@ s32 e1000_check_for_link(struct e1000_hw *hw)
 			 * happen due to the execution of this workaround.
 			 */
 
-			if ((hw->mac_type == e1000_82544
-			     || hw->mac_type == e1000_82543) && (!hw->autoneg)
-			    && (hw->forced_speed_duplex == e1000_10_full
-				|| hw->forced_speed_duplex == e1000_10_half)) {
+			if ((hw->mac_type == e1000_82544 ||
+			     hw->mac_type == e1000_82543) &&
+			    (!hw->autoneg) &&
+			    (hw->forced_speed_duplex == e1000_10_full ||
+			     hw->forced_speed_duplex == e1000_10_half)) {
 				ew32(IMC, 0xffffffff);
 				ret_val =
 				    e1000_polarity_reversal_workaround(hw);
@@ -2528,8 +2528,10 @@ s32 e1000_check_for_link(struct e1000_hw *hw)
 		 */
 		if (hw->tbi_compatibility_en) {
 			u16 speed, duplex;
+
 			ret_val =
 			    e1000_get_speed_and_duplex(hw, &speed, &duplex);
+
 			if (ret_val) {
 				e_dbg
 				    ("Error getting link speed and duplex\n");
@@ -2628,10 +2630,10 @@ s32 e1000_get_speed_and_duplex(struct e1000_hw *hw, u16 *speed, u16 *duplex)
 			    e1000_read_phy_reg(hw, PHY_LP_ABILITY, &phy_data);
 			if (ret_val)
 				return ret_val;
-			if ((*speed == SPEED_100
-			     && !(phy_data & NWAY_LPAR_100TX_FD_CAPS))
-			    || (*speed == SPEED_10
-				&& !(phy_data & NWAY_LPAR_10T_FD_CAPS)))
+			if ((*speed == SPEED_100 &&
+			     !(phy_data & NWAY_LPAR_100TX_FD_CAPS)) ||
+			    (*speed == SPEED_10 &&
+			     !(phy_data & NWAY_LPAR_10T_FD_CAPS)))
 				*duplex = HALF_DUPLEX;
 		}
 	}
@@ -2664,9 +2666,9 @@ static s32 e1000_wait_autoneg(struct e1000_hw *hw)
 		ret_val = e1000_read_phy_reg(hw, PHY_STATUS, &phy_data);
 		if (ret_val)
 			return ret_val;
-		if (phy_data & MII_SR_AUTONEG_COMPLETE) {
+		if (phy_data & MII_SR_AUTONEG_COMPLETE)
 			return E1000_SUCCESS;
-		}
+
 		msleep(100);
 	}
 	return E1000_SUCCESS;
@@ -2803,11 +2805,11 @@ static u16 e1000_shift_in_mdi_bits(struct e1000_hw *hw)
 	return data;
 }
 
-
 /**
  * e1000_read_phy_reg - read a phy register
  * @hw: Struct containing variables accessed by shared code
  * @reg_addr: address of the PHY register to read
+ * @phy_data: pointer to the value on the PHY register
  *
  * Reads the value from a PHY register, if the value is on a specific non zero
  * page, sets the page first.
@@ -2823,14 +2825,13 @@ s32 e1000_read_phy_reg(struct e1000_hw *hw, u32 reg_addr, u16 *phy_data)
 	    (reg_addr > MAX_PHY_MULTI_PAGE_REG)) {
 		ret_val = e1000_write_phy_reg_ex(hw, IGP01E1000_PHY_PAGE_SELECT,
 						 (u16) reg_addr);
-		if (ret_val) {
-			spin_unlock_irqrestore(&e1000_phy_lock, flags);
-			return ret_val;
-		}
+		if (ret_val)
+			goto out;
 	}
 
 	ret_val = e1000_read_phy_reg_ex(hw, MAX_PHY_REG_ADDRESS & reg_addr,
 					phy_data);
+out:
 	spin_unlock_irqrestore(&e1000_phy_lock, flags);
 
 	return ret_val;
@@ -2881,7 +2882,7 @@ static s32 e1000_read_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
 				e_dbg("MDI Read Error\n");
 				return -E1000_ERR_PHY;
 			}
-			*phy_data = (u16) mdic;
+			*phy_data = (u16)mdic;
 		} else {
 			mdic = ((reg_addr << E1000_MDIC_REG_SHIFT) |
 				(phy_addr << E1000_MDIC_PHY_SHIFT) |
@@ -2906,7 +2907,7 @@ static s32 e1000_read_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
 				e_dbg("MDI Error\n");
 				return -E1000_ERR_PHY;
 			}
-			*phy_data = (u16) mdic;
+			*phy_data = (u16)mdic;
 		}
 	} else {
 		/* We must first send a preamble through the MDIO pin to signal
@@ -2960,7 +2961,7 @@ s32 e1000_write_phy_reg(struct e1000_hw *hw, u32 reg_addr, u16 phy_data)
 	if ((hw->phy_type == e1000_phy_igp) &&
 	    (reg_addr > MAX_PHY_MULTI_PAGE_REG)) {
 		ret_val = e1000_write_phy_reg_ex(hw, IGP01E1000_PHY_PAGE_SELECT,
-						 (u16) reg_addr);
+						 (u16)reg_addr);
 		if (ret_val) {
 			spin_unlock_irqrestore(&e1000_phy_lock, flags);
 			return ret_val;
@@ -2993,7 +2994,7 @@ static s32 e1000_write_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
 		 * the desired data.
 		 */
 		if (hw->mac_type == e1000_ce4100) {
-			mdic = (((u32) phy_data) |
+			mdic = (((u32)phy_data) |
 				(reg_addr << E1000_MDIC_REG_SHIFT) |
 				(phy_addr << E1000_MDIC_PHY_SHIFT) |
 				(INTEL_CE_GBE_MDIC_OP_WRITE) |
@@ -3015,7 +3016,7 @@ static s32 e1000_write_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
 				return -E1000_ERR_PHY;
 			}
 		} else {
-			mdic = (((u32) phy_data) |
+			mdic = (((u32)phy_data) |
 				(reg_addr << E1000_MDIC_REG_SHIFT) |
 				(phy_addr << E1000_MDIC_PHY_SHIFT) |
 				(E1000_MDIC_OP_WRITE));
@@ -3053,7 +3054,7 @@ static s32 e1000_write_phy_reg_ex(struct e1000_hw *hw, u32 reg_addr,
 		mdic = ((PHY_TURNAROUND) | (reg_addr << 2) | (phy_addr << 7) |
 			(PHY_OP_WRITE << 12) | (PHY_SOF << 14));
 		mdic <<= 16;
-		mdic |= (u32) phy_data;
+		mdic |= (u32)phy_data;
 
 		e1000_shift_out_mdi_bits(hw, mdic, 32);
 	}
@@ -3176,14 +3177,14 @@ static s32 e1000_detect_gig_phy(struct e1000_hw *hw)
 	if (ret_val)
 		return ret_val;
 
-	hw->phy_id = (u32) (phy_id_high << 16);
+	hw->phy_id = (u32)(phy_id_high << 16);
 	udelay(20);
 	ret_val = e1000_read_phy_reg(hw, PHY_ID2, &phy_id_low);
 	if (ret_val)
 		return ret_val;
 
-	hw->phy_id |= (u32) (phy_id_low & PHY_REVISION_MASK);
-	hw->phy_revision = (u32) phy_id_low & ~PHY_REVISION_MASK;
+	hw->phy_id |= (u32)(phy_id_low & PHY_REVISION_MASK);
+	hw->phy_revision = (u32)phy_id_low & ~PHY_REVISION_MASK;
 
 	switch (hw->mac_type) {
 	case e1000_82543:
@@ -3401,7 +3402,6 @@ static s32 e1000_phy_m88_get_info(struct e1000_hw *hw,
 		phy_info->remote_rx = ((phy_data & SR_1000T_REMOTE_RX_STATUS) >>
 				       SR_1000T_REMOTE_RX_STATUS_SHIFT) ?
 		    e1000_1000t_rx_status_ok : e1000_1000t_rx_status_not_ok;
-
 	}
 
 	return E1000_SUCCESS;
@@ -3449,7 +3449,7 @@ s32 e1000_phy_get_info(struct e1000_hw *hw, struct e1000_phy_info *phy_info)
 	if (hw->phy_type == e1000_phy_igp)
 		return e1000_phy_igp_get_info(hw, phy_info);
 	else if ((hw->phy_type == e1000_phy_8211) ||
-	         (hw->phy_type == e1000_phy_8201))
+		 (hw->phy_type == e1000_phy_8201))
 		return E1000_SUCCESS;
 	else
 		return e1000_phy_m88_get_info(hw, phy_info);
@@ -3611,11 +3611,11 @@ static void e1000_shift_out_ee_bits(struct e1000_hw *hw, u16 data, u16 count)
 	 */
 	mask = 0x01 << (count - 1);
 	eecd = er32(EECD);
-	if (eeprom->type == e1000_eeprom_microwire) {
+	if (eeprom->type == e1000_eeprom_microwire)
 		eecd &= ~E1000_EECD_DO;
-	} else if (eeprom->type == e1000_eeprom_spi) {
+	else if (eeprom->type == e1000_eeprom_spi)
 		eecd |= E1000_EECD_DO;
-	}
+
 	do {
 		/* A "1" is shifted out to the EEPROM by setting bit "DI" to a
 		 * "1", and then raising and then lowering the clock (the SK bit
@@ -3851,7 +3851,7 @@ static s32 e1000_spi_eeprom_ready(struct e1000_hw *hw)
 	do {
 		e1000_shift_out_ee_bits(hw, EEPROM_RDSR_OPCODE_SPI,
 					hw->eeprom.opcode_bits);
-		spi_stat_reg = (u8) e1000_shift_in_ee_bits(hw, 8);
+		spi_stat_reg = (u8)e1000_shift_in_ee_bits(hw, 8);
 		if (!(spi_stat_reg & EEPROM_STATUS_RDY_SPI))
 			break;
 
@@ -3882,9 +3882,10 @@ static s32 e1000_spi_eeprom_ready(struct e1000_hw *hw)
 s32 e1000_read_eeprom(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 {
 	s32 ret;
-	spin_lock(&e1000_eeprom_lock);
+
+	mutex_lock(&e1000_eeprom_lock);
 	ret = e1000_do_read_eeprom(hw, offset, words, data);
-	spin_unlock(&e1000_eeprom_lock);
+	mutex_unlock(&e1000_eeprom_lock);
 	return ret;
 }
 
@@ -3896,15 +3897,16 @@ static s32 e1000_do_read_eeprom(struct e1000_hw *hw, u16 offset, u16 words,
 
 	if (hw->mac_type == e1000_ce4100) {
 		GBE_CONFIG_FLASH_READ(GBE_CONFIG_BASE_VIRT, offset, words,
-		                      data);
+				      data);
 		return E1000_SUCCESS;
 	}
 
 	/* A check for invalid values:  offset too large, too many words, and
 	 * not enough words.
 	 */
-	if ((offset >= eeprom->word_size)
-	    || (words > eeprom->word_size - offset) || (words == 0)) {
+	if ((offset >= eeprom->word_size) ||
+	    (words > eeprom->word_size - offset) ||
+	    (words == 0)) {
 		e_dbg("\"words\" parameter out of bounds. Words = %d,"
 		      "size = %d\n", offset, eeprom->word_size);
 		return -E1000_ERR_EEPROM;
@@ -3940,7 +3942,7 @@ static s32 e1000_do_read_eeprom(struct e1000_hw *hw, u16 offset, u16 words,
 
 		/* Send the READ command (opcode + addr)  */
 		e1000_shift_out_ee_bits(hw, read_opcode, eeprom->opcode_bits);
-		e1000_shift_out_ee_bits(hw, (u16) (offset * 2),
+		e1000_shift_out_ee_bits(hw, (u16)(offset * 2),
 					eeprom->address_bits);
 
 		/* Read the data.  The address of the eeprom internally
@@ -3960,7 +3962,7 @@ static s32 e1000_do_read_eeprom(struct e1000_hw *hw, u16 offset, u16 words,
 			e1000_shift_out_ee_bits(hw,
 						EEPROM_READ_OPCODE_MICROWIRE,
 						eeprom->opcode_bits);
-			e1000_shift_out_ee_bits(hw, (u16) (offset + i),
+			e1000_shift_out_ee_bits(hw, (u16)(offset + i),
 						eeprom->address_bits);
 
 			/* Read the data.  For microwire, each word requires the
@@ -3968,6 +3970,7 @@ static s32 e1000_do_read_eeprom(struct e1000_hw *hw, u16 offset, u16 words,
 			 */
 			data[i] = e1000_shift_in_ee_bits(hw, 16);
 			e1000_standby_eeprom(hw);
+			cond_resched();
 		}
 	}
 
@@ -4004,7 +4007,7 @@ s32 e1000_validate_eeprom_checksum(struct e1000_hw *hw)
 		return E1000_SUCCESS;
 
 #endif
-	if (checksum == (u16) EEPROM_SUM)
+	if (checksum == (u16)EEPROM_SUM)
 		return E1000_SUCCESS;
 	else {
 		e_dbg("EEPROM Checksum Invalid\n");
@@ -4031,7 +4034,7 @@ s32 e1000_update_eeprom_checksum(struct e1000_hw *hw)
 		}
 		checksum += eeprom_data;
 	}
-	checksum = (u16) EEPROM_SUM - checksum;
+	checksum = (u16)EEPROM_SUM - checksum;
 	if (e1000_write_eeprom(hw, EEPROM_CHECKSUM_REG, 1, &checksum) < 0) {
 		e_dbg("EEPROM Write Error\n");
 		return -E1000_ERR_EEPROM;
@@ -4052,9 +4055,10 @@ s32 e1000_update_eeprom_checksum(struct e1000_hw *hw)
 s32 e1000_write_eeprom(struct e1000_hw *hw, u16 offset, u16 words, u16 *data)
 {
 	s32 ret;
-	spin_lock(&e1000_eeprom_lock);
+
+	mutex_lock(&e1000_eeprom_lock);
 	ret = e1000_do_write_eeprom(hw, offset, words, data);
-	spin_unlock(&e1000_eeprom_lock);
+	mutex_unlock(&e1000_eeprom_lock);
 	return ret;
 }
 
@@ -4066,15 +4070,16 @@ static s32 e1000_do_write_eeprom(struct e1000_hw *hw, u16 offset, u16 words,
 
 	if (hw->mac_type == e1000_ce4100) {
 		GBE_CONFIG_FLASH_WRITE(GBE_CONFIG_BASE_VIRT, offset, words,
-		                       data);
+				       data);
 		return E1000_SUCCESS;
 	}
 
 	/* A check for invalid values:  offset too large, too many words, and
 	 * not enough words.
 	 */
-	if ((offset >= eeprom->word_size)
-	    || (words > eeprom->word_size - offset) || (words == 0)) {
+	if ((offset >= eeprom->word_size) ||
+	    (words > eeprom->word_size - offset) ||
+	    (words == 0)) {
 		e_dbg("\"words\" parameter out of bounds\n");
 		return -E1000_ERR_EEPROM;
 	}
@@ -4116,6 +4121,7 @@ static s32 e1000_write_eeprom_spi(struct e1000_hw *hw, u16 offset, u16 words,
 			return -E1000_ERR_EEPROM;
 
 		e1000_standby_eeprom(hw);
+		cond_resched();
 
 		/*  Send the WRITE ENABLE command (8 bit opcode )  */
 		e1000_shift_out_ee_bits(hw, EEPROM_WREN_OPCODE_SPI,
@@ -4132,7 +4138,7 @@ static s32 e1000_write_eeprom_spi(struct e1000_hw *hw, u16 offset, u16 words,
 		/* Send the Write command (8-bit opcode + addr) */
 		e1000_shift_out_ee_bits(hw, write_opcode, eeprom->opcode_bits);
 
-		e1000_shift_out_ee_bits(hw, (u16) ((offset + widx) * 2),
+		e1000_shift_out_ee_bits(hw, (u16)((offset + widx) * 2),
 					eeprom->address_bits);
 
 		/* Send the data */
@@ -4142,6 +4148,7 @@ static s32 e1000_write_eeprom_spi(struct e1000_hw *hw, u16 offset, u16 words,
 		 */
 		while (widx < words) {
 			u16 word_out = data[widx];
+
 			word_out = (word_out >> 8) | (word_out << 8);
 			e1000_shift_out_ee_bits(hw, word_out, 16);
 			widx++;
@@ -4183,9 +4190,9 @@ static s32 e1000_write_eeprom_microwire(struct e1000_hw *hw, u16 offset,
 	 * EEPROM into write/erase mode.
 	 */
 	e1000_shift_out_ee_bits(hw, EEPROM_EWEN_OPCODE_MICROWIRE,
-				(u16) (eeprom->opcode_bits + 2));
+				(u16)(eeprom->opcode_bits + 2));
 
-	e1000_shift_out_ee_bits(hw, 0, (u16) (eeprom->address_bits - 2));
+	e1000_shift_out_ee_bits(hw, 0, (u16)(eeprom->address_bits - 2));
 
 	/* Prepare the EEPROM */
 	e1000_standby_eeprom(hw);
@@ -4195,7 +4202,7 @@ static s32 e1000_write_eeprom_microwire(struct e1000_hw *hw, u16 offset,
 		e1000_shift_out_ee_bits(hw, EEPROM_WRITE_OPCODE_MICROWIRE,
 					eeprom->opcode_bits);
 
-		e1000_shift_out_ee_bits(hw, (u16) (offset + words_written),
+		e1000_shift_out_ee_bits(hw, (u16)(offset + words_written),
 					eeprom->address_bits);
 
 		/* Send the data */
@@ -4224,6 +4231,7 @@ static s32 e1000_write_eeprom_microwire(struct e1000_hw *hw, u16 offset,
 
 		/* Recover from write */
 		e1000_standby_eeprom(hw);
+		cond_resched();
 
 		words_written++;
 	}
@@ -4235,9 +4243,9 @@ static s32 e1000_write_eeprom_microwire(struct e1000_hw *hw, u16 offset,
 	 * EEPROM out of write/erase mode.
 	 */
 	e1000_shift_out_ee_bits(hw, EEPROM_EWDS_OPCODE_MICROWIRE,
-				(u16) (eeprom->opcode_bits + 2));
+				(u16)(eeprom->opcode_bits + 2));
 
-	e1000_shift_out_ee_bits(hw, 0, (u16) (eeprom->address_bits - 2));
+	e1000_shift_out_ee_bits(hw, 0, (u16)(eeprom->address_bits - 2));
 
 	return E1000_SUCCESS;
 }
@@ -4260,8 +4268,8 @@ s32 e1000_read_mac_addr(struct e1000_hw *hw)
 			e_dbg("EEPROM Read Error\n");
 			return -E1000_ERR_EEPROM;
 		}
-		hw->perm_mac_addr[i] = (u8) (eeprom_data & 0x00FF);
-		hw->perm_mac_addr[i + 1] = (u8) (eeprom_data >> 8);
+		hw->perm_mac_addr[i] = (u8)(eeprom_data & 0x00FF);
+		hw->perm_mac_addr[i + 1] = (u8)(eeprom_data >> 8);
 	}
 
 	switch (hw->mac_type) {
@@ -4328,19 +4336,19 @@ u32 e1000_hash_mc_addr(struct e1000_hw *hw, u8 *mc_addr)
 		 */
 	case 0:
 		/* [47:36] i.e. 0x563 for above example address */
-		hash_value = ((mc_addr[4] >> 4) | (((u16) mc_addr[5]) << 4));
+		hash_value = ((mc_addr[4] >> 4) | (((u16)mc_addr[5]) << 4));
 		break;
 	case 1:
 		/* [46:35] i.e. 0xAC6 for above example address */
-		hash_value = ((mc_addr[4] >> 3) | (((u16) mc_addr[5]) << 5));
+		hash_value = ((mc_addr[4] >> 3) | (((u16)mc_addr[5]) << 5));
 		break;
 	case 2:
 		/* [45:34] i.e. 0x5D8 for above example address */
-		hash_value = ((mc_addr[4] >> 2) | (((u16) mc_addr[5]) << 6));
+		hash_value = ((mc_addr[4] >> 2) | (((u16)mc_addr[5]) << 6));
 		break;
 	case 3:
 		/* [43:32] i.e. 0x634 for above example address */
-		hash_value = ((mc_addr[4]) | (((u16) mc_addr[5]) << 8));
+		hash_value = ((mc_addr[4]) | (((u16)mc_addr[5]) << 8));
 		break;
 	}
 
@@ -4361,9 +4369,9 @@ void e1000_rar_set(struct e1000_hw *hw, u8 *addr, u32 index)
 	/* HW expects these in little endian so we reverse the byte order
 	 * from network order (big endian) to little endian
 	 */
-	rar_low = ((u32) addr[0] | ((u32) addr[1] << 8) |
-		   ((u32) addr[2] << 16) | ((u32) addr[3] << 24));
-	rar_high = ((u32) addr[4] | ((u32) addr[5] << 8));
+	rar_low = ((u32)addr[0] | ((u32)addr[1] << 8) |
+		   ((u32)addr[2] << 16) | ((u32)addr[3] << 24));
+	rar_high = ((u32)addr[4] | ((u32)addr[5] << 8));
 
 	/* Disable Rx and flush all Rx frames before enabling RSS to avoid Rx
 	 * unit hang.
@@ -4537,7 +4545,7 @@ s32 e1000_setup_led(struct e1000_hw *hw)
 		if (ret_val)
 			return ret_val;
 		ret_val = e1000_write_phy_reg(hw, IGP01E1000_GMII_FIFO,
-					      (u16) (hw->phy_spd_default &
+					      (u16)(hw->phy_spd_default &
 						     ~IGP01E1000_GMII_SPD));
 		if (ret_val)
 			return ret_val;
@@ -4802,7 +4810,7 @@ void e1000_reset_adaptive(struct e1000_hw *hw)
 void e1000_update_adaptive(struct e1000_hw *hw)
 {
 	if (hw->adaptive_ifs) {
-		if ((hw->collision_delta *hw->ifs_ratio) > hw->tx_packet_delta) {
+		if ((hw->collision_delta * hw->ifs_ratio) > hw->tx_packet_delta) {
 			if (hw->tx_packet_delta > MIN_NUM_XMITS) {
 				hw->in_ifs_mode = true;
 				if (hw->current_ifs_val < hw->ifs_max_val) {
@@ -4816,8 +4824,8 @@ void e1000_update_adaptive(struct e1000_hw *hw)
 				}
 			}
 		} else {
-			if (hw->in_ifs_mode
-			    && (hw->tx_packet_delta <= MIN_NUM_XMITS)) {
+			if (hw->in_ifs_mode &&
+			    (hw->tx_packet_delta <= MIN_NUM_XMITS)) {
 				hw->current_ifs_val = 0;
 				hw->in_ifs_mode = false;
 				ew32(AIT, 0);
@@ -4922,7 +4930,6 @@ static s32 e1000_get_cable_length(struct e1000_hw *hw, u16 *min_length,
 
 	/* Use old method for Phy older than IGP */
 	if (hw->phy_type == e1000_phy_m88) {
-
 		ret_val = e1000_read_phy_reg(hw, M88E1000_PHY_SPEC_STATUS,
 					     &phy_data);
 		if (ret_val)
@@ -4966,7 +4973,6 @@ static s32 e1000_get_cable_length(struct e1000_hw *hw, u16 *min_length,
 		};
 		/* Read the AGC registers for all channels */
 		for (i = 0; i < IGP01E1000_PHY_CHANNEL_NUM; i++) {
-
 			ret_val =
 			    e1000_read_phy_reg(hw, agc_reg_array[i], &phy_data);
 			if (ret_val)
@@ -4976,8 +4982,8 @@ static s32 e1000_get_cable_length(struct e1000_hw *hw, u16 *min_length,
 
 			/* Value bound check. */
 			if ((cur_agc_value >=
-			     IGP01E1000_AGC_LENGTH_TABLE_SIZE - 1)
-			    || (cur_agc_value == 0))
+			     IGP01E1000_AGC_LENGTH_TABLE_SIZE - 1) ||
+			    (cur_agc_value == 0))
 				return -E1000_ERR_PHY;
 
 			agc_value += cur_agc_value;
@@ -5054,7 +5060,6 @@ static s32 e1000_check_polarity(struct e1000_hw *hw,
 		 */
 		if ((phy_data & IGP01E1000_PSSR_SPEED_MASK) ==
 		    IGP01E1000_PSSR_SPEED_1000MBPS) {
-
 			/* Read the GIG initialization PCS register (0x00B4) */
 			ret_val =
 			    e1000_read_phy_reg(hw, IGP01E1000_PHY_PCS_INIT_REG,
@@ -5175,8 +5180,8 @@ static s32 e1000_1000Mb_check_cable_length(struct e1000_hw *hw)
 				hw->ffe_config_state = e1000_ffe_config_active;
 
 				ret_val = e1000_write_phy_reg(hw,
-					      IGP01E1000_PHY_DSP_FFE,
-					      IGP01E1000_PHY_DSP_FFE_CM_CP);
+							      IGP01E1000_PHY_DSP_FFE,
+							      IGP01E1000_PHY_DSP_FFE_CM_CP);
 				if (ret_val)
 					return ret_val;
 				break;
@@ -5243,7 +5248,7 @@ static s32 e1000_config_dsp_after_link_change(struct e1000_hw *hw, bool link_up)
 			msleep(20);
 
 			ret_val = e1000_write_phy_reg(hw, 0x0000,
-						    IGP01E1000_IEEE_FORCE_GIGA);
+						      IGP01E1000_IEEE_FORCE_GIGA);
 			if (ret_val)
 				return ret_val;
 			for (i = 0; i < IGP01E1000_PHY_CHANNEL_NUM; i++) {
@@ -5264,7 +5269,7 @@ static s32 e1000_config_dsp_after_link_change(struct e1000_hw *hw, bool link_up)
 			}
 
 			ret_val = e1000_write_phy_reg(hw, 0x0000,
-					IGP01E1000_IEEE_RESTART_AUTONEG);
+						      IGP01E1000_IEEE_RESTART_AUTONEG);
 			if (ret_val)
 				return ret_val;
 
@@ -5299,7 +5304,7 @@ static s32 e1000_config_dsp_after_link_change(struct e1000_hw *hw, bool link_up)
 			msleep(20);
 
 			ret_val = e1000_write_phy_reg(hw, 0x0000,
-						    IGP01E1000_IEEE_FORCE_GIGA);
+						      IGP01E1000_IEEE_FORCE_GIGA);
 			if (ret_val)
 				return ret_val;
 			ret_val =
@@ -5309,7 +5314,7 @@ static s32 e1000_config_dsp_after_link_change(struct e1000_hw *hw, bool link_up)
 				return ret_val;
 
 			ret_val = e1000_write_phy_reg(hw, 0x0000,
-					IGP01E1000_IEEE_RESTART_AUTONEG);
+						      IGP01E1000_IEEE_RESTART_AUTONEG);
 			if (ret_val)
 				return ret_val;
 
@@ -5346,9 +5351,8 @@ static s32 e1000_set_phy_mode(struct e1000_hw *hw)
 		ret_val =
 		    e1000_read_eeprom(hw, EEPROM_PHY_CLASS_WORD, 1,
 				      &eeprom_data);
-		if (ret_val) {
+		if (ret_val)
 			return ret_val;
-		}
 
 		if ((eeprom_data != EEPROM_RESERVED_WORD) &&
 		    (eeprom_data & EEPROM_PHY_CLASS_A)) {
@@ -5395,8 +5399,8 @@ static s32 e1000_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 	 * from the lowest speeds starting from 10Mbps. The capability is used
 	 * for Dx transitions and states
 	 */
-	if (hw->mac_type == e1000_82541_rev_2
-	    || hw->mac_type == e1000_82547_rev_2) {
+	if (hw->mac_type == e1000_82541_rev_2 ||
+	    hw->mac_type == e1000_82547_rev_2) {
 		ret_val =
 		    e1000_read_phy_reg(hw, IGP01E1000_GMII_FIFO, &phy_data);
 		if (ret_val)
@@ -5446,11 +5450,9 @@ static s32 e1000_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 			if (ret_val)
 				return ret_val;
 		}
-	} else if ((hw->autoneg_advertised == AUTONEG_ADVERTISE_SPEED_DEFAULT)
-		   || (hw->autoneg_advertised == AUTONEG_ADVERTISE_10_ALL)
-		   || (hw->autoneg_advertised ==
-		       AUTONEG_ADVERTISE_10_100_ALL)) {
-
+	} else if ((hw->autoneg_advertised == AUTONEG_ADVERTISE_SPEED_DEFAULT) ||
+		   (hw->autoneg_advertised == AUTONEG_ADVERTISE_10_ALL) ||
+		   (hw->autoneg_advertised == AUTONEG_ADVERTISE_10_100_ALL)) {
 		if (hw->mac_type == e1000_82541_rev_2 ||
 		    hw->mac_type == e1000_82547_rev_2) {
 			phy_data |= IGP01E1000_GMII_FLEX_SPD;
@@ -5474,7 +5476,6 @@ static s32 e1000_set_d3_lplu_state(struct e1000_hw *hw, bool active)
 					phy_data);
 		if (ret_val)
 			return ret_val;
-
 	}
 	return E1000_SUCCESS;
 }
@@ -5542,7 +5543,6 @@ static s32 e1000_set_vco_speed(struct e1000_hw *hw)
 	return E1000_SUCCESS;
 }
 
-
 /**
  * e1000_enable_mng_pass_thru - check for bmc pass through
  * @hw: Struct containing variables accessed by shared code
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index fd7be860c201..3fc7bde699ba 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -99,13 +99,13 @@ int e1000_setup_all_rx_resources(struct e1000_adapter *adapter);
 void e1000_free_all_tx_resources(struct e1000_adapter *adapter);
 void e1000_free_all_rx_resources(struct e1000_adapter *adapter);
 static int e1000_setup_tx_resources(struct e1000_adapter *adapter,
-                             struct e1000_tx_ring *txdr);
+				    struct e1000_tx_ring *txdr);
 static int e1000_setup_rx_resources(struct e1000_adapter *adapter,
-                             struct e1000_rx_ring *rxdr);
+				    struct e1000_rx_ring *rxdr);
 static void e1000_free_tx_resources(struct e1000_adapter *adapter,
-                             struct e1000_tx_ring *tx_ring);
+				    struct e1000_tx_ring *tx_ring);
 static void e1000_free_rx_resources(struct e1000_adapter *adapter,
-                             struct e1000_rx_ring *rx_ring);
+				    struct e1000_rx_ring *rx_ring);
 void e1000_update_stats(struct e1000_adapter *adapter);
 
 static int e1000_init_module(void);
@@ -122,16 +122,16 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter);
 static void e1000_clean_all_tx_rings(struct e1000_adapter *adapter);
 static void e1000_clean_all_rx_rings(struct e1000_adapter *adapter);
 static void e1000_clean_tx_ring(struct e1000_adapter *adapter,
-                                struct e1000_tx_ring *tx_ring);
+				struct e1000_tx_ring *tx_ring);
 static void e1000_clean_rx_ring(struct e1000_adapter *adapter,
-                                struct e1000_rx_ring *rx_ring);
+				struct e1000_rx_ring *rx_ring);
 static void e1000_set_rx_mode(struct net_device *netdev);
 static void e1000_update_phy_info_task(struct work_struct *work);
 static void e1000_watchdog(struct work_struct *work);
 static void e1000_82547_tx_fifo_stall_task(struct work_struct *work);
 static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 				    struct net_device *netdev);
-static struct net_device_stats * e1000_get_stats(struct net_device *netdev);
+static struct net_device_stats *e1000_get_stats(struct net_device *netdev);
 static int e1000_change_mtu(struct net_device *netdev, int new_mtu);
 static int e1000_set_mac(struct net_device *netdev, void *p);
 static irqreturn_t e1000_intr(int irq, void *data);
@@ -164,7 +164,7 @@ static void e1000_tx_timeout(struct net_device *dev);
 static void e1000_reset_task(struct work_struct *work);
 static void e1000_smartspeed(struct e1000_adapter *adapter);
 static int e1000_82547_fifo_workaround(struct e1000_adapter *adapter,
-                                       struct sk_buff *skb);
+				       struct sk_buff *skb);
 
 static bool e1000_vlan_used(struct e1000_adapter *adapter);
 static void e1000_vlan_mode(struct net_device *netdev,
@@ -195,7 +195,7 @@ MODULE_PARM_DESC(copybreak,
 	"Maximum size of packet that is copied to a new buffer on receive");
 
 static pci_ers_result_t e1000_io_error_detected(struct pci_dev *pdev,
-                     pci_channel_state_t state);
+						pci_channel_state_t state);
 static pci_ers_result_t e1000_io_slot_reset(struct pci_dev *pdev);
 static void e1000_io_resume(struct pci_dev *pdev);
 
@@ -287,7 +287,7 @@ static int e1000_request_irq(struct e1000_adapter *adapter)
 	int err;
 
 	err = request_irq(adapter->pdev->irq, handler, irq_flags, netdev->name,
-	                  netdev);
+			  netdev);
 	if (err) {
 		e_err(probe, "Unable to allocate interrupt Error: %d\n", err);
 	}
@@ -636,8 +636,8 @@ void e1000_reset(struct e1000_adapter *adapter)
 		 * but don't include ethernet FCS because hardware appends it
 		 */
 		min_tx_space = (hw->max_frame_size +
-		                sizeof(struct e1000_tx_desc) -
-		                ETH_FCS_LEN) * 2;
+				sizeof(struct e1000_tx_desc) -
+				ETH_FCS_LEN) * 2;
 		min_tx_space = ALIGN(min_tx_space, 1024);
 		min_tx_space >>= 10;
 		/* software strips receive CRC, so leave room for it */
@@ -943,8 +943,8 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct e1000_adapter *adapter;
 	struct e1000_hw *hw;
 
-	static int cards_found = 0;
-	static int global_quad_port_a = 0; /* global ksp3 port a indication */
+	static int cards_found;
+	static int global_quad_port_a; /* global ksp3 port a indication */
 	int i, err, pci_using_dac;
 	u16 eeprom_data = 0;
 	u16 tmp = 0;
@@ -1046,7 +1046,7 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (hw->mac_type == e1000_ce4100) {
 		hw->ce4100_gbe_mdio_base_virt =
 					ioremap(pci_resource_start(pdev, BAR_1),
-		                                pci_resource_len(pdev, BAR_1));
+						pci_resource_len(pdev, BAR_1));
 
 		if (!hw->ce4100_gbe_mdio_base_virt)
 			goto err_mdio_ioremap;
@@ -1148,7 +1148,7 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		break;
 	case e1000_82546:
 	case e1000_82546_rev_3:
-		if (er32(STATUS) & E1000_STATUS_FUNC_1){
+		if (er32(STATUS) & E1000_STATUS_FUNC_1) {
 			e1000_read_eeprom(hw,
 				EEPROM_INIT_CONTROL3_PORT_B, 1, &eeprom_data);
 			break;
@@ -1199,13 +1199,13 @@ static int e1000_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		for (i = 0; i < 32; i++) {
 			hw->phy_addr = i;
 			e1000_read_phy_reg(hw, PHY_ID2, &tmp);
-			if (tmp == 0 || tmp == 0xFF) {
-				if (i == 31)
-					goto err_eeprom;
-				continue;
-			} else
+
+			if (tmp != 0 && tmp != 0xFF)
 				break;
 		}
+
+		if (i >= 32)
+			goto err_eeprom;
 	}
 
 	/* reset the hardware with the new settings */
@@ -1263,7 +1263,7 @@ err_pci_reg:
  * @pdev: PCI device information struct
  *
  * e1000_remove is called by the PCI subsystem to alert the driver
- * that it should release a PCI device.  The could be caused by a
+ * that it should release a PCI device. That could be caused by a
  * Hot-Plug event, or because the driver is going to be removed from
  * memory.
  **/
@@ -1334,12 +1334,12 @@ static int e1000_sw_init(struct e1000_adapter *adapter)
 static int e1000_alloc_queues(struct e1000_adapter *adapter)
 {
 	adapter->tx_ring = kcalloc(adapter->num_tx_queues,
-	                           sizeof(struct e1000_tx_ring), GFP_KERNEL);
+				   sizeof(struct e1000_tx_ring), GFP_KERNEL);
 	if (!adapter->tx_ring)
 		return -ENOMEM;
 
 	adapter->rx_ring = kcalloc(adapter->num_rx_queues,
-	                           sizeof(struct e1000_rx_ring), GFP_KERNEL);
+				   sizeof(struct e1000_rx_ring), GFP_KERNEL);
 	if (!adapter->rx_ring) {
 		kfree(adapter->tx_ring);
 		return -ENOMEM;
@@ -1811,20 +1811,20 @@ static void e1000_setup_rctl(struct e1000_adapter *adapter)
 	rctl &= ~E1000_RCTL_SZ_4096;
 	rctl |= E1000_RCTL_BSEX;
 	switch (adapter->rx_buffer_len) {
-		case E1000_RXBUFFER_2048:
-		default:
-			rctl |= E1000_RCTL_SZ_2048;
-			rctl &= ~E1000_RCTL_BSEX;
-			break;
-		case E1000_RXBUFFER_4096:
-			rctl |= E1000_RCTL_SZ_4096;
-			break;
-		case E1000_RXBUFFER_8192:
-			rctl |= E1000_RCTL_SZ_8192;
-			break;
-		case E1000_RXBUFFER_16384:
-			rctl |= E1000_RCTL_SZ_16384;
-			break;
+	case E1000_RXBUFFER_2048:
+	default:
+		rctl |= E1000_RCTL_SZ_2048;
+		rctl &= ~E1000_RCTL_BSEX;
+		break;
+	case E1000_RXBUFFER_4096:
+		rctl |= E1000_RCTL_SZ_4096;
+		break;
+	case E1000_RXBUFFER_8192:
+		rctl |= E1000_RCTL_SZ_8192;
+		break;
+	case E1000_RXBUFFER_16384:
+		rctl |= E1000_RCTL_SZ_16384;
+		break;
 	}
 
 	/* This is useful for sniffing bad packets. */
@@ -1861,12 +1861,12 @@ static void e1000_configure_rx(struct e1000_adapter *adapter)
 
 	if (adapter->netdev->mtu > ETH_DATA_LEN) {
 		rdlen = adapter->rx_ring[0].count *
-		        sizeof(struct e1000_rx_desc);
+			sizeof(struct e1000_rx_desc);
 		adapter->clean_rx = e1000_clean_jumbo_rx_irq;
 		adapter->alloc_rx_buf = e1000_alloc_jumbo_rx_buffers;
 	} else {
 		rdlen = adapter->rx_ring[0].count *
-		        sizeof(struct e1000_rx_desc);
+			sizeof(struct e1000_rx_desc);
 		adapter->clean_rx = e1000_clean_rx_irq;
 		adapter->alloc_rx_buf = e1000_alloc_rx_buffers;
 	}
@@ -2761,7 +2761,9 @@ static int e1000_tso(struct e1000_adapter *adapter,
 		buffer_info->time_stamp = jiffies;
 		buffer_info->next_to_watch = i;
 
-		if (++i == tx_ring->count) i = 0;
+		if (++i == tx_ring->count)
+			i = 0;
+
 		tx_ring->next_to_use = i;
 
 		return true;
@@ -2816,7 +2818,9 @@ static bool e1000_tx_csum(struct e1000_adapter *adapter,
 	buffer_info->time_stamp = jiffies;
 	buffer_info->next_to_watch = i;
 
-	if (unlikely(++i == tx_ring->count)) i = 0;
+	if (unlikely(++i == tx_ring->count))
+		i = 0;
+
 	tx_ring->next_to_use = i;
 
 	return true;
@@ -2865,8 +2869,8 @@ static int e1000_tx_map(struct e1000_adapter *adapter,
 		 * packet is smaller than 2048 - 16 - 16 (or 2016) bytes
 		 */
 		if (unlikely((hw->bus_type == e1000_bus_type_pcix) &&
-		                (size > 2015) && count == 0))
-		        size = 2015;
+			     (size > 2015) && count == 0))
+			size = 2015;
 
 		/* Workaround for potential 82544 hang in PCI-X.  Avoid
 		 * terminating buffers within evenly-aligned dwords.
@@ -2963,7 +2967,7 @@ dma_error:
 		count--;
 
 	while (count--) {
-		if (i==0)
+		if (i == 0)
 			i += tx_ring->count;
 		i--;
 		buffer_info = &tx_ring->buffer_info[i];
@@ -3013,7 +3017,8 @@ static void e1000_tx_queue(struct e1000_adapter *adapter,
 		tx_desc->lower.data =
 			cpu_to_le32(txd_lower | buffer_info->length);
 		tx_desc->upper.data = cpu_to_le32(txd_upper);
-		if (unlikely(++i == tx_ring->count)) i = 0;
+		if (unlikely(++i == tx_ring->count))
+			i = 0;
 	}
 
 	tx_desc->lower.data |= cpu_to_le32(adapter->txd_cmd);
@@ -3101,7 +3106,7 @@ static int e1000_maybe_stop_tx(struct net_device *netdev,
 	return __e1000_maybe_stop_tx(netdev, size);
 }
 
-#define TXD_USE_COUNT(S, X) (((S) >> (X)) + 1 )
+#define TXD_USE_COUNT(S, X) (((S) >> (X)) + 1)
 static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 				    struct net_device *netdev)
 {
@@ -3841,7 +3846,7 @@ static bool e1000_clean_tx_irq(struct e1000_adapter *adapter,
 	struct e1000_tx_buffer *buffer_info;
 	unsigned int i, eop;
 	unsigned int count = 0;
-	unsigned int total_tx_bytes=0, total_tx_packets=0;
+	unsigned int total_tx_bytes = 0, total_tx_packets = 0;
 	unsigned int bytes_compl = 0, pkts_compl = 0;
 
 	i = tx_ring->next_to_clean;
@@ -3869,14 +3874,18 @@ static bool e1000_clean_tx_irq(struct e1000_adapter *adapter,
 			e1000_unmap_and_free_tx_resource(adapter, buffer_info);
 			tx_desc->upper.data = 0;
 
-			if (unlikely(++i == tx_ring->count)) i = 0;
+			if (unlikely(++i == tx_ring->count))
+				i = 0;
 		}
 
 		eop = tx_ring->buffer_info[i].next_to_watch;
 		eop_desc = E1000_TX_DESC(*tx_ring, eop);
 	}
 
-	tx_ring->next_to_clean = i;
+	/* Synchronize with E1000_DESC_UNUSED called from e1000_xmit_frame,
+	 * which will reuse the cleaned buffers.
+	 */
+	smp_store_release(&tx_ring->next_to_clean, i);
 
 	netdev_completed_queue(netdev, pkts_compl, bytes_compl);
 
@@ -3954,9 +3963,11 @@ static void e1000_rx_checksum(struct e1000_adapter *adapter, u32 status_err,
 	skb_checksum_none_assert(skb);
 
 	/* 82543 or newer only */
-	if (unlikely(hw->mac_type < e1000_82543)) return;
+	if (unlikely(hw->mac_type < e1000_82543))
+		return;
 	/* Ignore Checksum bit is set */
-	if (unlikely(status & E1000_RXD_STAT_IXSM)) return;
+	if (unlikely(status & E1000_RXD_STAT_IXSM))
+		return;
 	/* TCP/UDP checksum error bit is set */
 	if (unlikely(errors & E1000_RXD_ERR_TCPE)) {
 		/* let the stack verify checksum errors */
@@ -4136,7 +4147,7 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 	unsigned int i;
 	int cleaned_count = 0;
 	bool cleaned = false;
-	unsigned int total_rx_bytes=0, total_rx_packets=0;
+	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 
 	i = rx_ring->next_to_clean;
 	rx_desc = E1000_RX_DESC(*rx_ring, i);
@@ -4153,7 +4164,9 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 
 		status = rx_desc->status;
 
-		if (++i == rx_ring->count) i = 0;
+		if (++i == rx_ring->count)
+			i = 0;
+
 		next_rxd = E1000_RX_DESC(*rx_ring, i);
 		prefetch(next_rxd);
 
@@ -4356,7 +4369,7 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 	unsigned int i;
 	int cleaned_count = 0;
 	bool cleaned = false;
-	unsigned int total_rx_bytes=0, total_rx_packets=0;
+	unsigned int total_rx_bytes = 0, total_rx_packets = 0;
 
 	i = rx_ring->next_to_clean;
 	rx_desc = E1000_RX_DESC(*rx_ring, i);
@@ -4395,7 +4408,9 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 			buffer_info->rxbuf.data = NULL;
 		}
 
-		if (++i == rx_ring->count) i = 0;
+		if (++i == rx_ring->count)
+			i = 0;
+
 		next_rxd = E1000_RX_DESC(*rx_ring, i);
 		prefetch(next_rxd);
 
@@ -4683,9 +4698,11 @@ static void e1000_smartspeed(struct e1000_adapter *adapter)
 		 * we assume back-to-back
 		 */
 		e1000_read_phy_reg(hw, PHY_1000T_STATUS, &phy_status);
-		if (!(phy_status & SR_1000T_MS_CONFIG_FAULT)) return;
+		if (!(phy_status & SR_1000T_MS_CONFIG_FAULT))
+			return;
 		e1000_read_phy_reg(hw, PHY_1000T_STATUS, &phy_status);
-		if (!(phy_status & SR_1000T_MS_CONFIG_FAULT)) return;
+		if (!(phy_status & SR_1000T_MS_CONFIG_FAULT))
+			return;
 		e1000_read_phy_reg(hw, PHY_1000T_CTRL, &phy_ctrl);
 		if (phy_ctrl & CR_1000T_MS_ENABLE) {
 			phy_ctrl &= ~CR_1000T_MS_ENABLE;
diff --git a/drivers/net/ethernet/intel/e1000e/defines.h b/drivers/net/ethernet/intel/e1000e/defines.h
index 133d4074dbe4..f7c7804d79e5 100644
--- a/drivers/net/ethernet/intel/e1000e/defines.h
+++ b/drivers/net/ethernet/intel/e1000e/defines.h
@@ -441,12 +441,13 @@
 #define E1000_IMS_RXQ1      E1000_ICR_RXQ1      /* Rx Queue 1 Interrupt */
 #define E1000_IMS_TXQ0      E1000_ICR_TXQ0      /* Tx Queue 0 Interrupt */
 #define E1000_IMS_TXQ1      E1000_ICR_TXQ1      /* Tx Queue 1 Interrupt */
-#define E1000_IMS_OTHER     E1000_ICR_OTHER     /* Other Interrupts */
+#define E1000_IMS_OTHER     E1000_ICR_OTHER     /* Other Interrupt */
 
 /* Interrupt Cause Set */
 #define E1000_ICS_LSC       E1000_ICR_LSC       /* Link Status Change */
 #define E1000_ICS_RXSEQ     E1000_ICR_RXSEQ     /* Rx sequence error */
 #define E1000_ICS_RXDMT0    E1000_ICR_RXDMT0    /* Rx desc min. threshold */
+#define E1000_ICS_OTHER     E1000_ICR_OTHER     /* Other Interrupt */
 
 /* Transmit Descriptor Control */
 #define E1000_TXDCTL_PTHRESH 0x0000003F /* TXDCTL Prefetch Threshold */
diff --git a/drivers/net/ethernet/intel/e1000e/e1000.h b/drivers/net/ethernet/intel/e1000e/e1000.h
index 0b748d1959d9..1dc293bad87b 100644
--- a/drivers/net/ethernet/intel/e1000e/e1000.h
+++ b/drivers/net/ethernet/intel/e1000e/e1000.h
@@ -480,7 +480,7 @@ extern const char e1000e_driver_version[];
 void e1000e_check_options(struct e1000_adapter *adapter);
 void e1000e_set_ethtool_ops(struct net_device *netdev);
 
-int e1000e_up(struct e1000_adapter *adapter);
+void e1000e_up(struct e1000_adapter *adapter);
 void e1000e_down(struct e1000_adapter *adapter, bool reset);
 void e1000e_reinit_locked(struct e1000_adapter *adapter);
 void e1000e_reset(struct e1000_adapter *adapter);
diff --git a/drivers/net/ethernet/intel/e1000e/hw.h b/drivers/net/ethernet/intel/e1000e/hw.h
index c9da4654e9ca..b3949d5bef5c 100644
--- a/drivers/net/ethernet/intel/e1000e/hw.h
+++ b/drivers/net/ethernet/intel/e1000e/hw.h
@@ -91,6 +91,7 @@ struct e1000_hw;
 #define E1000_DEV_ID_PCH_SPT_I219_V		0x1570	/* SPT PCH */
 #define E1000_DEV_ID_PCH_SPT_I219_LM2		0x15B7	/* SPT-H PCH */
 #define E1000_DEV_ID_PCH_SPT_I219_V2		0x15B8	/* SPT-H PCH */
+#define E1000_DEV_ID_PCH_LBG_I219_LM3		0x15B9	/* LBG PCH */
 
 #define E1000_REVISION_4	4
 
diff --git a/drivers/net/ethernet/intel/e1000e/ich8lan.c b/drivers/net/ethernet/intel/e1000e/ich8lan.c
index 91a5a0ae9cd7..a049e30639a1 100644
--- a/drivers/net/ethernet/intel/e1000e/ich8lan.c
+++ b/drivers/net/ethernet/intel/e1000e/ich8lan.c
@@ -1984,7 +1984,7 @@ static s32 e1000_check_reset_block_ich8lan(struct e1000_hw *hw)
 	int i = 0;
 
 	while ((blocked = !(er32(FWSM) & E1000_ICH_FWSM_RSPCIPHY)) &&
-	       (i++ < 10))
+	       (i++ < 30))
 		usleep_range(10000, 20000);
 	return blocked ? E1000_BLK_PHY_RESET : 0;
 }
@@ -3093,24 +3093,45 @@ static s32 e1000_valid_nvm_bank_detect_ich8lan(struct e1000_hw *hw, u32 *bank)
 	struct e1000_nvm_info *nvm = &hw->nvm;
 	u32 bank1_offset = nvm->flash_bank_size * sizeof(u16);
 	u32 act_offset = E1000_ICH_NVM_SIG_WORD * 2 + 1;
+	u32 nvm_dword = 0;
 	u8 sig_byte = 0;
 	s32 ret_val;
 
 	switch (hw->mac.type) {
-		/* In SPT, read from the CTRL_EXT reg instead of
-		 * accessing the sector valid bits from the nvm
-		 */
 	case e1000_pch_spt:
-		*bank = er32(CTRL_EXT)
-		    & E1000_CTRL_EXT_NVMVS;
-		if ((*bank == 0) || (*bank == 1)) {
-			e_dbg("ERROR: No valid NVM bank present\n");
-			return -E1000_ERR_NVM;
-		} else {
-			*bank = *bank - 2;
+		bank1_offset = nvm->flash_bank_size;
+		act_offset = E1000_ICH_NVM_SIG_WORD;
+
+		/* set bank to 0 in case flash read fails */
+		*bank = 0;
+
+		/* Check bank 0 */
+		ret_val = e1000_read_flash_dword_ich8lan(hw, act_offset,
+							 &nvm_dword);
+		if (ret_val)
+			return ret_val;
+		sig_byte = (u8)((nvm_dword & 0xFF00) >> 8);
+		if ((sig_byte & E1000_ICH_NVM_VALID_SIG_MASK) ==
+		    E1000_ICH_NVM_SIG_VALUE) {
+			*bank = 0;
 			return 0;
 		}
-		break;
+
+		/* Check bank 1 */
+		ret_val = e1000_read_flash_dword_ich8lan(hw, act_offset +
+							 bank1_offset,
+							 &nvm_dword);
+		if (ret_val)
+			return ret_val;
+		sig_byte = (u8)((nvm_dword & 0xFF00) >> 8);
+		if ((sig_byte & E1000_ICH_NVM_VALID_SIG_MASK) ==
+		    E1000_ICH_NVM_SIG_VALUE) {
+			*bank = 1;
+			return 0;
+		}
+
+		e_dbg("ERROR: No valid NVM bank present\n");
+		return -E1000_ERR_NVM;
 	case e1000_ich8lan:
 	case e1000_ich9lan:
 		eecd = er32(EECD);
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 0a854a47d31a..c71ba1bfc1ec 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -1905,30 +1905,15 @@ static irqreturn_t e1000_msix_other(int __always_unused irq, void *data)
 	struct net_device *netdev = data;
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
-	u32 icr = er32(ICR);
-
-	if (!(icr & E1000_ICR_INT_ASSERTED)) {
-		if (!test_bit(__E1000_DOWN, &adapter->state))
-			ew32(IMS, E1000_IMS_OTHER);
-		return IRQ_NONE;
-	}
 
-	if (icr & adapter->eiac_mask)
-		ew32(ICS, (icr & adapter->eiac_mask));
+	hw->mac.get_link_status = true;
 
-	if (icr & E1000_ICR_OTHER) {
-		if (!(icr & E1000_ICR_LSC))
-			goto no_link_interrupt;
-		hw->mac.get_link_status = true;
-		/* guard against interrupt when we're going down */
-		if (!test_bit(__E1000_DOWN, &adapter->state))
-			mod_timer(&adapter->watchdog_timer, jiffies + 1);
+	/* guard against interrupt when we're going down */
+	if (!test_bit(__E1000_DOWN, &adapter->state)) {
+		mod_timer(&adapter->watchdog_timer, jiffies + 1);
+		ew32(IMS, E1000_IMS_OTHER);
 	}
 
-no_link_interrupt:
-	if (!test_bit(__E1000_DOWN, &adapter->state))
-		ew32(IMS, E1000_IMS_LSC | E1000_IMS_OTHER);
-
 	return IRQ_HANDLED;
 }
 
@@ -1946,6 +1931,9 @@ static irqreturn_t e1000_intr_msix_tx(int __always_unused irq, void *data)
 		/* Ring was not completely cleaned, so fire another interrupt */
 		ew32(ICS, tx_ring->ims_val);
 
+	if (!test_bit(__E1000_DOWN, &adapter->state))
+		ew32(IMS, adapter->tx_ring->ims_val);
+
 	return IRQ_HANDLED;
 }
 
@@ -1959,8 +1947,10 @@ static irqreturn_t e1000_intr_msix_rx(int __always_unused irq, void *data)
 	 * previous interrupt.
 	 */
 	if (rx_ring->set_itr) {
-		writel(1000000000 / (rx_ring->itr_val * 256),
-		       rx_ring->itr_register);
+		u32 itr = rx_ring->itr_val ?
+			  1000000000 / (rx_ring->itr_val * 256) : 0;
+
+		writel(itr, rx_ring->itr_register);
 		rx_ring->set_itr = 0;
 	}
 
@@ -2025,6 +2015,7 @@ static void e1000_configure_msix(struct e1000_adapter *adapter)
 		       hw->hw_addr + E1000_EITR_82574(vector));
 	else
 		writel(1, hw->hw_addr + E1000_EITR_82574(vector));
+	adapter->eiac_mask |= E1000_IMS_OTHER;
 
 	/* Cause Tx interrupts on every write back */
 	ivar |= (1 << 31);
@@ -2032,12 +2023,8 @@ static void e1000_configure_msix(struct e1000_adapter *adapter)
 	ew32(IVAR, ivar);
 
 	/* enable MSI-X PBA support */
-	ctrl_ext = er32(CTRL_EXT);
-	ctrl_ext |= E1000_CTRL_EXT_PBA_CLR;
-
-	/* Auto-Mask Other interrupts upon ICR read */
-	ew32(IAM, ~E1000_EIAC_MASK_82574 | E1000_IMS_OTHER);
-	ctrl_ext |= E1000_CTRL_EXT_EIAME;
+	ctrl_ext = er32(CTRL_EXT) & ~E1000_CTRL_EXT_IAME;
+	ctrl_ext |= E1000_CTRL_EXT_PBA_CLR | E1000_CTRL_EXT_EIAME;
 	ew32(CTRL_EXT, ctrl_ext);
 	e1e_flush();
 }
@@ -2253,7 +2240,7 @@ static void e1000_irq_enable(struct e1000_adapter *adapter)
 
 	if (adapter->msix_entries) {
 		ew32(EIAC_82574, adapter->eiac_mask & E1000_EIAC_MASK_82574);
-		ew32(IMS, adapter->eiac_mask | E1000_IMS_OTHER | E1000_IMS_LSC);
+		ew32(IMS, adapter->eiac_mask | E1000_IMS_LSC);
 	} else if ((hw->mac.type == e1000_pch_lpt) ||
 		   (hw->mac.type == e1000_pch_spt)) {
 		ew32(IMS, IMS_ENABLE_MASK | E1000_IMS_ECCER);
@@ -4144,10 +4131,24 @@ void e1000e_reset(struct e1000_adapter *adapter)
 
 }
 
-int e1000e_up(struct e1000_adapter *adapter)
+/**
+ * e1000e_trigger_lsc - trigger an LSC interrupt
+ * @adapter: 
+ *
+ * Fire a link status change interrupt to start the watchdog.
+ **/
+static void e1000e_trigger_lsc(struct e1000_adapter *adapter)
 {
 	struct e1000_hw *hw = &adapter->hw;
 
+	if (adapter->msix_entries)
+		ew32(ICS, E1000_ICS_OTHER);
+	else
+		ew32(ICS, E1000_ICS_LSC);
+}
+
+void e1000e_up(struct e1000_adapter *adapter)
+{
 	/* hardware has been reset, we need to reload some things */
 	e1000_configure(adapter);
 
@@ -4159,13 +4160,7 @@ int e1000e_up(struct e1000_adapter *adapter)
 
 	netif_start_queue(adapter->netdev);
 
-	/* fire a link change interrupt to start the watchdog */
-	if (adapter->msix_entries)
-		ew32(ICS, E1000_ICS_LSC | E1000_ICR_OTHER);
-	else
-		ew32(ICS, E1000_ICS_LSC);
-
-	return 0;
+	e1000e_trigger_lsc(adapter);
 }
 
 static void e1000e_flush_descriptors(struct e1000_adapter *adapter)
@@ -4590,11 +4585,7 @@ static int e1000_open(struct net_device *netdev)
 	hw->mac.get_link_status = true;
 	pm_runtime_put(&pdev->dev);
 
-	/* fire a link status change interrupt to start the watchdog */
-	if (adapter->msix_entries)
-		ew32(ICS, E1000_ICS_LSC | E1000_ICR_OTHER);
-	else
-		ew32(ICS, E1000_ICS_LSC);
+	e1000e_trigger_lsc(adapter);
 
 	return 0;
 
@@ -6631,7 +6622,7 @@ static int e1000e_pm_runtime_resume(struct device *dev)
 		return rc;
 
 	if (netdev->flags & IFF_UP)
-		rc = e1000e_up(adapter);
+		e1000e_up(adapter);
 
 	return rc;
 }
@@ -6822,13 +6813,8 @@ static void e1000_io_resume(struct pci_dev *pdev)
 
 	e1000_init_manageability_pt(adapter);
 
-	if (netif_running(netdev)) {
-		if (e1000e_up(adapter)) {
-			dev_err(&pdev->dev,
-				"can't bring device back up after reset\n");
-			return;
-		}
-	}
+	if (netif_running(netdev))
+		e1000e_up(adapter);
 
 	netif_device_attach(netdev);
 
@@ -7465,6 +7451,7 @@ static const struct pci_device_id e1000_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_V), board_pch_spt },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_LM2), board_pch_spt },
 	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_SPT_I219_V2), board_pch_spt },
+	{ PCI_VDEVICE(INTEL, E1000_DEV_ID_PCH_LBG_I219_LM3), board_pch_spt },
 
 	{ 0, 0, 0, 0, 0, 0, 0 }	/* terminate list */
 };
@@ -7504,14 +7491,11 @@ static struct pci_driver e1000_driver = {
  **/
 static int __init e1000_init_module(void)
 {
-	int ret;
-
 	pr_info("Intel(R) PRO/1000 Network Driver - %s\n",
 		e1000e_driver_version);
 	pr_info("Copyright(c) 1999 - 2015 Intel Corporation.\n");
-	ret = pci_register_driver(&e1000_driver);
 
-	return ret;
+	return pci_register_driver(&e1000_driver);
 }
 module_init(e1000_init_module);
 
diff --git a/drivers/net/ethernet/intel/fm10k/Makefile b/drivers/net/ethernet/intel/fm10k/Makefile
index 08859dd220a8..b006ff66d028 100644
--- a/drivers/net/ethernet/intel/fm10k/Makefile
+++ b/drivers/net/ethernet/intel/fm10k/Makefile
@@ -1,7 +1,7 @@
 ################################################################################
 #
 # Intel Ethernet Switch Host Interface Driver
-# Copyright(c) 2013 - 2014 Intel Corporation.
+# Copyright(c) 2013 - 2015 Intel Corporation.
 #
 # This program is free software; you can redistribute it and/or modify it
 # under the terms and conditions of the GNU General Public License,
@@ -27,7 +27,17 @@
 
 obj-$(CONFIG_FM10K) += fm10k.o
 
-fm10k-objs := fm10k_main.o fm10k_common.o fm10k_pci.o \
-	      fm10k_netdev.o fm10k_ethtool.o fm10k_pf.o fm10k_vf.o \
-	      fm10k_mbx.o fm10k_iov.o fm10k_tlv.o \
-	      fm10k_debugfs.o fm10k_ptp.o fm10k_dcbnl.o
+fm10k-y := fm10k_main.o \
+	   fm10k_common.o \
+	   fm10k_pci.o \
+	   fm10k_ptp.o \
+	   fm10k_netdev.o \
+	   fm10k_ethtool.o \
+	   fm10k_pf.o \
+	   fm10k_vf.o \
+	   fm10k_mbx.o \
+	   fm10k_iov.o \
+	   fm10k_tlv.o
+
+fm10k-$(CONFIG_DEBUG_FS) += fm10k_debugfs.o
+fm10k-$(CONFIG_DCB) += fm10k_dcbnl.o
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k.h b/drivers/net/ethernet/intel/fm10k/fm10k.h
index 14440200499b..b34bb008b104 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k.h
@@ -23,6 +23,7 @@
 
 #include <linux/types.h>
 #include <linux/etherdevice.h>
+#include <linux/cpumask.h>
 #include <linux/rtnetlink.h>
 #include <linux/if_vlan.h>
 #include <linux/pci.h>
@@ -33,7 +34,7 @@
 #include "fm10k_pf.h"
 #include "fm10k_vf.h"
 
-#define FM10K_MAX_JUMBO_FRAME_SIZE	15358	/* Maximum supported size 15K */
+#define FM10K_MAX_JUMBO_FRAME_SIZE	15342	/* Maximum supported size 15K */
 
 #define MAX_QUEUES	FM10K_MAX_QUEUES_PF
 
@@ -66,6 +67,7 @@ struct fm10k_l2_accel {
 enum fm10k_ring_state_t {
 	__FM10K_TX_DETECT_HANG,
 	__FM10K_HANG_CHECK_ARMED,
+	__FM10K_TX_XPS_INIT_DONE,
 };
 
 #define check_for_tx_hang(ring) \
@@ -138,7 +140,7 @@ struct fm10k_ring {
 					 * different for DCB and RSS modes
 					 */
 	u8 qos_pc;			/* priority class of queue */
-	u16 vid;			/* default vlan ID of queue */
+	u16 vid;			/* default VLAN ID of queue */
 	u16 count;			/* amount of descriptors */
 
 	u16 next_to_alloc;
@@ -164,14 +166,20 @@ struct fm10k_ring_container {
 	unsigned int total_packets;	/* total packets processed this int */
 	u16 work_limit;			/* total work allowed per interrupt */
 	u16 itr;			/* interrupt throttle rate value */
+	u8 itr_scale;			/* ITR adjustment based on PCI speed */
 	u8 count;			/* total number of rings in vector */
 };
 
 #define FM10K_ITR_MAX		0x0FFF	/* maximum value for ITR */
 #define FM10K_ITR_10K		100	/* 100us */
 #define FM10K_ITR_20K		50	/* 50us */
+#define FM10K_ITR_40K		25	/* 25us */
 #define FM10K_ITR_ADAPTIVE	0x8000	/* adaptive interrupt moderation flag */
 
+#define ITR_IS_ADAPTIVE(itr) (!!(itr & FM10K_ITR_ADAPTIVE))
+
+#define FM10K_TX_ITR_DEFAULT	FM10K_ITR_40K
+#define FM10K_RX_ITR_DEFAULT	FM10K_ITR_20K
 #define FM10K_ITR_ENABLE	(FM10K_ITR_AUTOMASK | FM10K_ITR_MASK_CLEAR)
 
 static inline struct netdev_queue *txring_txq(const struct fm10k_ring *ring)
@@ -203,6 +211,7 @@ struct fm10k_q_vector {
 	struct fm10k_ring_container rx, tx;
 
 	struct napi_struct napi;
+	cpumask_t affinity_mask;
 	char name[IFNAMSIZ + 9];
 
 #ifdef CONFIG_DEBUG_FS
@@ -413,7 +422,7 @@ static inline u16 fm10k_desc_unused(struct fm10k_ring *ring)
 	 (&(((union fm10k_rx_desc *)((R)->desc))[i]))
 
 #define FM10K_MAX_TXD_PWR	14
-#define FM10K_MAX_DATA_PER_TXD	(1 << FM10K_MAX_TXD_PWR)
+#define FM10K_MAX_DATA_PER_TXD	BIT(FM10K_MAX_TXD_PWR)
 
 /* Tx Descriptors needed, worst case */
 #define TXD_USE_COUNT(S)	DIV_ROUND_UP((S), FM10K_MAX_DATA_PER_TXD)
@@ -434,7 +443,7 @@ union fm10k_ftag_info {
 	struct {
 		/* dglort and sglort combined into a single 32bit desc read */
 		__le32 glort;
-		/* upper 16 bits of vlan are reserved 0 for swpri_type_user */
+		/* upper 16 bits of VLAN are reserved 0 for swpri_type_user */
 		__le32 vlan;
 	} d;
 	struct {
@@ -484,7 +493,7 @@ void fm10k_netpoll(struct net_device *netdev);
 #endif
 
 /* Netdev */
-struct net_device *fm10k_alloc_netdev(void);
+struct net_device *fm10k_alloc_netdev(const struct fm10k_info *info);
 int fm10k_setup_rx_resources(struct fm10k_ring *);
 int fm10k_setup_tx_resources(struct fm10k_ring *);
 void fm10k_free_rx_resources(struct fm10k_ring *);
@@ -551,5 +560,9 @@ int fm10k_get_ts_config(struct net_device *netdev, struct ifreq *ifr);
 int fm10k_set_ts_config(struct net_device *netdev, struct ifreq *ifr);
 
 /* DCB */
+#ifdef CONFIG_DCB
 void fm10k_dcbnl_set_ops(struct net_device *dev);
+#else
+static inline void fm10k_dcbnl_set_ops(struct net_device *dev) {}
+#endif
 #endif /* _FM10K_H_ */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
index 5c7a4d7662d8..2be4361839db 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
@@ -20,7 +20,6 @@
 
 #include "fm10k.h"
 
-#ifdef CONFIG_DCB
 /**
  * fm10k_dcbnl_ieee_getets - get the ETS configuration for the device
  * @dev: netdev interface for the device
@@ -155,7 +154,6 @@ static const struct dcbnl_rtnl_ops fm10k_dcbnl_ops = {
 	.setdcbx	= fm10k_dcbnl_setdcbx,
 };
 
-#endif /* CONFIG_DCB */
 /**
  * fm10k_dcbnl_set_ops - Configures dcbnl ops pointer for netdev
  * @dev: netdev interface for the device
@@ -164,11 +162,9 @@ static const struct dcbnl_rtnl_ops fm10k_dcbnl_ops = {
  **/
 void fm10k_dcbnl_set_ops(struct net_device *dev)
 {
-#ifdef CONFIG_DCB
 	struct fm10k_intfc *interface = netdev_priv(dev);
 	struct fm10k_hw *hw = &interface->hw;
 
 	if (hw->mac.type == fm10k_mac_pf)
 		dev->dcbnl_ops = &fm10k_dcbnl_ops;
-#endif /* CONFIG_DCB */
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
index 5304bc1fbecd..5d6137faf7d1 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
@@ -18,8 +18,6 @@
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  */
 
-#ifdef CONFIG_DEBUG_FS
-
 #include "fm10k.h"
 
 #include <linux/debugfs.h>
@@ -258,5 +256,3 @@ void fm10k_dbg_exit(void)
 	debugfs_remove_recursive(dbg_root);
 	dbg_root = NULL;
 }
-
-#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
index 2ce0eba5e040..2f6a05b57228 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
@@ -111,12 +111,14 @@ static const struct fm10k_stats fm10k_gstrings_pf_stats[] = {
 
 static const struct fm10k_stats fm10k_gstrings_mbx_stats[] = {
 	FM10K_MBX_STAT("mbx_tx_busy", tx_busy),
-	FM10K_MBX_STAT("mbx_tx_oversized", tx_dropped),
+	FM10K_MBX_STAT("mbx_tx_dropped", tx_dropped),
 	FM10K_MBX_STAT("mbx_tx_messages", tx_messages),
 	FM10K_MBX_STAT("mbx_tx_dwords", tx_dwords),
+	FM10K_MBX_STAT("mbx_tx_mbmem_pulled", tx_mbmem_pulled),
 	FM10K_MBX_STAT("mbx_rx_messages", rx_messages),
 	FM10K_MBX_STAT("mbx_rx_dwords", rx_dwords),
 	FM10K_MBX_STAT("mbx_rx_parse_err", rx_parse_err),
+	FM10K_MBX_STAT("mbx_rx_mbmem_pushed", rx_mbmem_pushed),
 };
 
 #define FM10K_GLOBAL_STATS_LEN ARRAY_SIZE(fm10k_gstrings_global_stats)
@@ -125,7 +127,7 @@ static const struct fm10k_stats fm10k_gstrings_mbx_stats[] = {
 #define FM10K_MBX_STATS_LEN ARRAY_SIZE(fm10k_gstrings_mbx_stats)
 
 #define FM10K_QUEUE_STATS_LEN(_n) \
-	( (_n) * 2 * (sizeof(struct fm10k_queue_stats) / sizeof(u64)))
+	((_n) * 2 * (sizeof(struct fm10k_queue_stats) / sizeof(u64)))
 
 #define FM10K_STATIC_STATS_LEN (FM10K_GLOBAL_STATS_LEN + \
 				FM10K_NETDEV_STATS_LEN + \
@@ -257,7 +259,8 @@ static int fm10k_get_sset_count(struct net_device *dev, int sset)
 			stats_len += FM10K_DEBUG_STATS_LEN;
 
 			if (iov_data)
-				stats_len += FM10K_MBX_STATS_LEN * iov_data->num_vfs;
+				stats_len += FM10K_MBX_STATS_LEN *
+					iov_data->num_vfs;
 		}
 
 		return stats_len;
@@ -296,14 +299,16 @@ static void fm10k_get_ethtool_stats(struct net_device *netdev,
 
 	if (interface->flags & FM10K_FLAG_DEBUG_STATS) {
 		for (i = 0; i < FM10K_DEBUG_STATS_LEN; i++) {
-			p = (char *)interface + fm10k_gstrings_debug_stats[i].stat_offset;
+			p = (char *)interface +
+				fm10k_gstrings_debug_stats[i].stat_offset;
 			*(data++) = (fm10k_gstrings_debug_stats[i].sizeof_stat ==
 				     sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 		}
 	}
 
 	for (i = 0; i < FM10K_MBX_STATS_LEN; i++) {
-		p = (char *)&interface->hw.mbx + fm10k_gstrings_mbx_stats[i].stat_offset;
+		p = (char *)&interface->hw.mbx +
+			fm10k_gstrings_mbx_stats[i].stat_offset;
 		*(data++) = (fm10k_gstrings_mbx_stats[i].sizeof_stat ==
 			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 	}
@@ -320,6 +325,7 @@ static void fm10k_get_ethtool_stats(struct net_device *netdev,
 	if ((interface->flags & FM10K_FLAG_DEBUG_STATS) && iov_data) {
 		for (i = 0; i < iov_data->num_vfs; i++) {
 			struct fm10k_vf_info *vf_info;
+
 			vf_info = &iov_data->vf_info[i];
 
 			/* skip stats if we don't have a vf info */
@@ -329,7 +335,8 @@ static void fm10k_get_ethtool_stats(struct net_device *netdev,
 			}
 
 			for (j = 0; j < FM10K_MBX_STATS_LEN; j++) {
-				p = (char *)&vf_info->mbx + fm10k_gstrings_mbx_stats[j].stat_offset;
+				p = (char *)&vf_info->mbx +
+					fm10k_gstrings_mbx_stats[j].stat_offset;
 				*(data++) = (fm10k_gstrings_mbx_stats[j].sizeof_stat ==
 					     sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
 			}
@@ -699,12 +706,10 @@ static int fm10k_get_coalesce(struct net_device *dev,
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
 
-	ec->use_adaptive_tx_coalesce =
-		!!(interface->tx_itr & FM10K_ITR_ADAPTIVE);
+	ec->use_adaptive_tx_coalesce = ITR_IS_ADAPTIVE(interface->tx_itr);
 	ec->tx_coalesce_usecs = interface->tx_itr & ~FM10K_ITR_ADAPTIVE;
 
-	ec->use_adaptive_rx_coalesce =
-		!!(interface->rx_itr & FM10K_ITR_ADAPTIVE);
+	ec->use_adaptive_rx_coalesce = ITR_IS_ADAPTIVE(interface->rx_itr);
 	ec->rx_coalesce_usecs = interface->rx_itr & ~FM10K_ITR_ADAPTIVE;
 
 	return 0;
@@ -729,10 +734,10 @@ static int fm10k_set_coalesce(struct net_device *dev,
 
 	/* set initial values for adaptive ITR */
 	if (ec->use_adaptive_tx_coalesce)
-		tx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_10K;
+		tx_itr = FM10K_ITR_ADAPTIVE | FM10K_TX_ITR_DEFAULT;
 
 	if (ec->use_adaptive_rx_coalesce)
-		rx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_20K;
+		rx_itr = FM10K_ITR_ADAPTIVE | FM10K_RX_ITR_DEFAULT;
 
 	/* update interface */
 	interface->tx_itr = tx_itr;
@@ -1020,7 +1025,6 @@ static int fm10k_set_priv_flags(struct net_device *netdev, u32 priv_flags)
 	return 0;
 }
 
-
 static u32 fm10k_get_reta_size(struct net_device __always_unused *netdev)
 {
 	return FM10K_RETA_SIZE * FM10K_RETA_ENTRIES_PER_REG;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_main.c b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
index e76a44cf330c..b243c3cbe68f 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_main.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
@@ -28,7 +28,7 @@
 
 #include "fm10k.h"
 
-#define DRV_VERSION	"0.15.2-k"
+#define DRV_VERSION	"0.19.3-k"
 const char fm10k_driver_version[] = DRV_VERSION;
 char fm10k_driver_name[] = "fm10k";
 static const char fm10k_driver_string[] =
@@ -42,7 +42,7 @@ MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
 /* single workqueue for entire fm10k driver */
-struct workqueue_struct *fm10k_workqueue = NULL;
+struct workqueue_struct *fm10k_workqueue;
 
 /**
  * fm10k_init_module - Driver Registration Routine
@@ -56,8 +56,7 @@ static int __init fm10k_init_module(void)
 	pr_info("%s\n", fm10k_copyright);
 
 	/* create driver workqueue */
-	if (!fm10k_workqueue)
-		fm10k_workqueue = create_workqueue("fm10k");
+	fm10k_workqueue = create_workqueue("fm10k");
 
 	fm10k_dbg_init();
 
@@ -80,7 +79,6 @@ static void __exit fm10k_exit_module(void)
 	/* destroy driver workqueue */
 	flush_workqueue(fm10k_workqueue);
 	destroy_workqueue(fm10k_workqueue);
-	fm10k_workqueue = NULL;
 }
 module_exit(fm10k_exit_module);
 
@@ -917,7 +915,7 @@ static u8 fm10k_tx_desc_flags(struct sk_buff *skb, u32 tx_flags)
 	/* set timestamping bits */
 	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
 	    likely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
-			desc_flags |= FM10K_TXD_FLAG_TIME;
+		desc_flags |= FM10K_TXD_FLAG_TIME;
 
 	/* set checksum offload bits */
 	desc_flags |= FM10K_SET_FLAG(tx_flags, FM10K_TX_FLAGS_CSUM,
@@ -1094,11 +1092,11 @@ dma_error:
 netdev_tx_t fm10k_xmit_frame_ring(struct sk_buff *skb,
 				  struct fm10k_ring *tx_ring)
 {
+	u16 count = TXD_USE_COUNT(skb_headlen(skb));
 	struct fm10k_tx_buffer *first;
-	int tso;
-	u32 tx_flags = 0;
 	unsigned short f;
-	u16 count = TXD_USE_COUNT(skb_headlen(skb));
+	u32 tx_flags = 0;
+	int tso;
 
 	/* need: 1 descriptor per page * PAGE_SIZE/FM10K_MAX_DATA_PER_TXD,
 	 *       + 1 desc for skb_headlen/FM10K_MAX_DATA_PER_TXD,
@@ -1363,10 +1361,10 @@ static bool fm10k_clean_tx_irq(struct fm10k_q_vector *q_vector,
  **/
 static void fm10k_update_itr(struct fm10k_ring_container *ring_container)
 {
-	unsigned int avg_wire_size, packets;
+	unsigned int avg_wire_size, packets, itr_round;
 
 	/* Only update ITR if we are using adaptive setting */
-	if (!(ring_container->itr & FM10K_ITR_ADAPTIVE))
+	if (!ITR_IS_ADAPTIVE(ring_container->itr))
 		goto clear_counts;
 
 	packets = ring_container->total_packets;
@@ -1375,18 +1373,44 @@ static void fm10k_update_itr(struct fm10k_ring_container *ring_container)
 
 	avg_wire_size = ring_container->total_bytes / packets;
 
-	/* Add 24 bytes to size to account for CRC, preamble, and gap */
-	avg_wire_size += 24;
-
-	/* Don't starve jumbo frames */
-	if (avg_wire_size > 3000)
-		avg_wire_size = 3000;
+	/* The following is a crude approximation of:
+	 *  wmem_default / (size + overhead) = desired_pkts_per_int
+	 *  rate / bits_per_byte / (size + ethernet overhead) = pkt_rate
+	 *  (desired_pkt_rate / pkt_rate) * usecs_per_sec = ITR value
+	 *
+	 * Assuming wmem_default is 212992 and overhead is 640 bytes per
+	 * packet, (256 skb, 64 headroom, 320 shared info), we can reduce the
+	 * formula down to
+	 *
+	 *  (34 * (size + 24)) / (size + 640) = ITR
+	 *
+	 * We first do some math on the packet size and then finally bitshift
+	 * by 8 after rounding up. We also have to account for PCIe link speed
+	 * difference as ITR scales based on this.
+	 */
+	if (avg_wire_size <= 360) {
+		/* Start at 250K ints/sec and gradually drop to 77K ints/sec */
+		avg_wire_size *= 8;
+		avg_wire_size += 376;
+	} else if (avg_wire_size <= 1152) {
+		/* 77K ints/sec to 45K ints/sec */
+		avg_wire_size *= 3;
+		avg_wire_size += 2176;
+	} else if (avg_wire_size <= 1920) {
+		/* 45K ints/sec to 38K ints/sec */
+		avg_wire_size += 4480;
+	} else {
+		/* plateau at a limit of 38K ints/sec */
+		avg_wire_size = 6656;
+	}
 
-	/* Give a little boost to mid-size frames */
-	if ((avg_wire_size > 300) && (avg_wire_size < 1200))
-		avg_wire_size /= 3;
-	else
-		avg_wire_size /= 2;
+	/* Perform final bitshift for division after rounding up to ensure
+	 * 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;
+	avg_wire_size += (1 << itr_round) - 1;
+	avg_wire_size >>= itr_round;
 
 	/* write back value and retain adaptive flag */
 	ring_container->itr = avg_wire_size | FM10K_ITR_ADAPTIVE;
@@ -1428,11 +1452,15 @@ static int fm10k_poll(struct napi_struct *napi, int budget)
 	fm10k_for_each_ring(ring, q_vector->tx)
 		clean_complete &= fm10k_clean_tx_irq(q_vector, ring);
 
+	/* Handle case where we are called by netpoll with a budget of 0 */
+	if (budget <= 0)
+		return budget;
+
 	/* attempt to distribute budget to each queue fairly, but don't
 	 * allow the budget to go below 1 because we'll exit polling
 	 */
 	if (q_vector->rx.count > 1)
-		per_ring_budget = max(budget/q_vector->rx.count, 1);
+		per_ring_budget = max(budget / q_vector->rx.count, 1);
 	else
 		per_ring_budget = budget;
 
@@ -1600,6 +1628,7 @@ static int fm10k_alloc_q_vector(struct fm10k_intfc *interface,
 	q_vector->tx.ring = ring;
 	q_vector->tx.work_limit = FM10K_DEFAULT_TX_WORK;
 	q_vector->tx.itr = interface->tx_itr;
+	q_vector->tx.itr_scale = interface->hw.mac.itr_scale;
 	q_vector->tx.count = txr_count;
 
 	while (txr_count) {
@@ -1628,6 +1657,7 @@ static int fm10k_alloc_q_vector(struct fm10k_intfc *interface,
 	/* save Rx ring container info */
 	q_vector->rx.ring = ring;
 	q_vector->rx.itr = interface->rx_itr;
+	q_vector->rx.itr_scale = interface->hw.mac.itr_scale;
 	q_vector->rx.count = rxr_count;
 
 	while (rxr_count) {
@@ -1966,8 +1996,10 @@ int fm10k_init_queueing_scheme(struct fm10k_intfc *interface)
 
 	/* Allocate memory for queues */
 	err = fm10k_alloc_q_vectors(interface);
-	if (err)
+	if (err) {
+		fm10k_reset_msix_capability(interface);
 		return err;
+	}
 
 	/* Map rings to devices, and map devices to physical queues */
 	fm10k_assign_rings(interface);
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
index af09a1b272e6..98202c3d591c 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
@@ -57,7 +57,7 @@ static u16 fm10k_fifo_unused(struct fm10k_mbx_fifo *fifo)
 }
 
 /**
- *  fm10k_fifo_empty - Test to verify if fifo is empty
+ *  fm10k_fifo_empty - Test to verify if FIFO is empty
  *  @fifo: pointer to FIFO
  *
  *  This function returns true if the FIFO is empty, else false
@@ -72,7 +72,7 @@ static bool fm10k_fifo_empty(struct fm10k_mbx_fifo *fifo)
  *  @fifo: pointer to FIFO
  *  @offset: offset to add to head
  *
- *  This function returns the indices into the fifo based on head + offset
+ *  This function returns the indices into the FIFO based on head + offset
  **/
 static u16 fm10k_fifo_head_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
 {
@@ -84,7 +84,7 @@ static u16 fm10k_fifo_head_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
  *  @fifo: pointer to FIFO
  *  @offset: offset to add to tail
  *
- *  This function returns the indices into the fifo based on tail + offset
+ *  This function returns the indices into the FIFO based on tail + offset
  **/
 static u16 fm10k_fifo_tail_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
 {
@@ -160,7 +160,7 @@ static u16 fm10k_mbx_index_len(struct fm10k_mbx_info *mbx, u16 head, u16 tail)
 /**
  *  fm10k_mbx_tail_add - Determine new tail value with added offset
  *  @mbx: pointer to mailbox
- *  @offset: length to add to head offset
+ *  @offset: length to add to tail offset
  *
  *  This function takes the local tail index and recomputes it for
  *  a given length added as an offset.
@@ -176,7 +176,7 @@ static u16 fm10k_mbx_tail_add(struct fm10k_mbx_info *mbx, u16 offset)
 /**
  *  fm10k_mbx_tail_sub - Determine new tail value with subtracted offset
  *  @mbx: pointer to mailbox
- *  @offset: length to add to head offset
+ *  @offset: length to add to tail offset
  *
  *  This function takes the local tail index and recomputes it for
  *  a given length added as an offset.
@@ -240,7 +240,7 @@ static u16 fm10k_mbx_pushed_tail_len(struct fm10k_mbx_info *mbx)
 }
 
 /**
- *  fm10k_fifo_write_copy - pulls data off of msg and places it in fifo
+ *  fm10k_fifo_write_copy - pulls data off of msg and places it in FIFO
  *  @fifo: pointer to FIFO
  *  @msg: message array to populate
  *  @tail_offset: additional offset to add to tail pointer
@@ -336,6 +336,7 @@ static u16 fm10k_mbx_validate_msg_size(struct fm10k_mbx_info *mbx, u16 len)
 
 /**
  *  fm10k_mbx_write_copy - pulls data off of Tx FIFO and places it in mbmem
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
  *  This function will take a section of the Tx FIFO and copy it into the
@@ -375,6 +376,8 @@ static void fm10k_mbx_write_copy(struct fm10k_hw *hw,
 			if (!tail)
 				tail++;
 
+			mbx->tx_mbmem_pulled++;
+
 			/* write message to hardware FIFO */
 			fm10k_write_reg(hw, mbmem + tail++, *(head++));
 		} while (--len && --end);
@@ -459,6 +462,8 @@ static void fm10k_mbx_read_copy(struct fm10k_hw *hw,
 			if (!head)
 				head++;
 
+			mbx->rx_mbmem_pushed++;
+
 			/* read message from hardware FIFO */
 			*(tail++) = fm10k_read_reg(hw, mbmem + head++);
 		} while (--len && --end);
@@ -707,7 +712,7 @@ static bool fm10k_mbx_tx_complete(struct fm10k_mbx_info *mbx)
  *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
- *  This function dequeues messages and hands them off to the tlv parser.
+ *  This function dequeues messages and hands them off to the TLV parser.
  *  It will return the number of messages processed when called.
  **/
 static u16 fm10k_mbx_dequeue_rx(struct fm10k_hw *hw,
@@ -899,7 +904,7 @@ static void fm10k_mbx_create_disconnect_hdr(struct fm10k_mbx_info *mbx)
 }
 
 /**
- *  fm10k_mbx_create_fake_disconnect_hdr - Generate a false disconnect mailbox header
+ *  fm10k_mbx_create_fake_disconnect_hdr - Generate a false disconnect mbox hdr
  *  @mbx: pointer to mailbox
  *
  *  This function creates a fake disconnect header for loading into remote
@@ -920,7 +925,7 @@ static void fm10k_mbx_create_fake_disconnect_hdr(struct fm10k_mbx_info *mbx)
 }
 
 /**
- *  fm10k_mbx_create_error_msg - Generate a error message
+ *  fm10k_mbx_create_error_msg - Generate an error message
  *  @mbx: pointer to mailbox
  *  @err: local error encountered
  *
@@ -953,7 +958,6 @@ static void fm10k_mbx_create_error_msg(struct fm10k_mbx_info *mbx, s32 err)
 /**
  *  fm10k_mbx_validate_msg_hdr - Validate common fields in the message header
  *  @mbx: pointer to mailbox
- *  @msg: message array to read
  *
  *  This function will parse up the fields in the mailbox header and return
  *  an error if the header contains any of a number of invalid configurations
@@ -1017,11 +1021,12 @@ static s32 fm10k_mbx_validate_msg_hdr(struct fm10k_mbx_info *mbx)
 
 /**
  *  fm10k_mbx_create_reply - Generate reply based on state and remote head
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *  @head: acknowledgement number
  *
  *  This function will generate an outgoing message based on the current
- *  mailbox state and the remote fifo head.  It will return the length
+ *  mailbox state and the remote FIFO head.  It will return the length
  *  of the outgoing message excluding header on success, and a negative value
  *  on error.
  **/
@@ -1147,8 +1152,8 @@ static void fm10k_mbx_connect_reset(struct fm10k_mbx_info *mbx)
 
 /**
  *  fm10k_mbx_process_connect - Process connect header
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
- *  @msg: message array to process
  *
  *  This function will read an incoming connect header and reply with the
  *  appropriate message.  It will return a value indicating the number of
@@ -1194,6 +1199,7 @@ static s32 fm10k_mbx_process_connect(struct fm10k_hw *hw,
 
 /**
  *  fm10k_mbx_process_data - Process data header
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
  *  This function will read an incoming data header and reply with the
@@ -1235,6 +1241,7 @@ static s32 fm10k_mbx_process_data(struct fm10k_hw *hw,
 
 /**
  *  fm10k_mbx_process_disconnect - Process disconnect header
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
  *  This function will read an incoming disconnect header and reply with the
@@ -1287,6 +1294,7 @@ static s32 fm10k_mbx_process_disconnect(struct fm10k_hw *hw,
 
 /**
  *  fm10k_mbx_process_error - Process error header
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
  *  This function will read an incoming error header and reply with the
@@ -1556,7 +1564,7 @@ static s32 fm10k_mbx_register_handlers(struct fm10k_mbx_info *mbx,
  *  @id: ID reference for PF as it supports up to 64 PF/VF mailboxes
  *
  *  This function initializes the mailbox for use.  It will split the
- *  buffer provided an use that th populate both the Tx and Rx FIFO by
+ *  buffer provided and use that to populate both the Tx and Rx FIFO by
  *  evenly splitting it.  In order to allow for easy masking of head/tail
  *  the value reported in size must be a power of 2 and is reported in
  *  DWORDs, not bytes.  Any invalid values will cause the mailbox to return
@@ -1633,7 +1641,7 @@ s32 fm10k_pfvf_mbx_init(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx,
  *  fm10k_sm_mbx_create_data_hdr - Generate a mailbox header for local FIFO
  *  @mbx: pointer to mailbox
  *
- *  This function returns a connection mailbox header
+ *  This function returns a data mailbox header
  **/
 static void fm10k_sm_mbx_create_data_hdr(struct fm10k_mbx_info *mbx)
 {
@@ -1726,8 +1734,6 @@ static s32 fm10k_sm_mbx_connect(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
 	fm10k_sm_mbx_create_connect_hdr(mbx, 0);
 	fm10k_mbx_write(hw, mbx);
 
-	/* enable interrupt and notify other party of new message */
-
 	return 0;
 }
 
@@ -1771,7 +1777,7 @@ static void fm10k_sm_mbx_disconnect(struct fm10k_hw *hw,
 }
 
 /**
- *  fm10k_mbx_validate_fifo_hdr - Validate fields in the remote FIFO header
+ *  fm10k_sm_mbx_validate_fifo_hdr - Validate fields in the remote FIFO header
  *  @mbx: pointer to mailbox
  *
  *  This function will parse up the fields in the mailbox header and return
@@ -1849,7 +1855,7 @@ static void fm10k_sm_mbx_process_error(struct fm10k_mbx_info *mbx)
 }
 
 /**
- *  fm10k_sm_mbx_create_error_message - Process an error in FIFO hdr
+ *  fm10k_sm_mbx_create_error_msg - Process an error in FIFO header
  *  @mbx: pointer to mailbox
  *  @err: local error encountered
  *
@@ -1879,6 +1885,7 @@ static void fm10k_sm_mbx_create_error_msg(struct fm10k_mbx_info *mbx, s32 err)
  *  fm10k_sm_mbx_receive - Take message from Rx mailbox FIFO and put it in Rx
  *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
+ *  @tail: tail index of message
  *
  *  This function will dequeue one message from the Rx switch manager mailbox
  *  FIFO and place it in the Rx mailbox FIFO for processing by software.
@@ -1918,6 +1925,7 @@ static s32 fm10k_sm_mbx_receive(struct fm10k_hw *hw,
  *  fm10k_sm_mbx_transmit - Take message from Tx and put it in Tx mailbox FIFO
  *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
+ *  @head: head index of message
  *
  *  This function will dequeue one message from the Tx mailbox FIFO and place
  *  it in the Tx switch manager mailbox FIFO for processing by hardware.
@@ -1957,11 +1965,12 @@ static void fm10k_sm_mbx_transmit(struct fm10k_hw *hw,
 
 /**
  *  fm10k_sm_mbx_create_reply - Generate reply based on state and remote head
+ *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *  @head: acknowledgement number
  *
  *  This function will generate an outgoing message based on the current
- *  mailbox state and the remote fifo head.  It will return the length
+ *  mailbox state and the remote FIFO head.  It will return the length
  *  of the outgoing message excluding header on success, and a negative value
  *  on error.
  **/
@@ -2073,7 +2082,7 @@ send_reply:
 }
 
 /**
- *  fm10k_sm_mbx_process - Process mailbox switch mailbox interrupt
+ *  fm10k_sm_mbx_process - Process switch manager mailbox interrupt
  *  @hw: pointer to hardware structure
  *  @mbx: pointer to mailbox
  *
@@ -2129,13 +2138,19 @@ fifo_err:
  *  @mbx: pointer to mailbox
  *  @msg_data: handlers for mailbox events
  *
- *  This function for now is used to stub out the PF/SM mailbox
+ *  This function initializes the PF/SM mailbox for use.  It will split the
+ *  buffer provided and use that to populate both the Tx and Rx FIFO by
+ *  evenly splitting it.  In order to allow for easy masking of head/tail
+ *  the value reported in size must be a power of 2 and is reported in
+ *  DWORDs, not bytes.  Any invalid values will cause the mailbox to return
+ *  error.
  **/
 s32 fm10k_sm_mbx_init(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx,
 		      const struct fm10k_msg_data *msg_data)
 {
 	mbx->mbx_reg = FM10K_GMBX;
 	mbx->mbmem_reg = FM10K_MBMEM_PF(0);
+
 	/* start out in closed state */
 	mbx->state = FM10K_STATE_CLOSED;
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h
index 0419a7f0035e..245a0a3dc32e 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -128,11 +128,11 @@ enum fm10k_mbx_state {
  *		The maximum message size is provided during connect to avoid
  *		jamming the mailbox with messages that do not fit.
  * Err_no: Error number - Applies only to error headers
- *		The error number provides a indication of the type of error
+ *		The error number provides an indication of the type of error
  *		experienced.
  */
 
-/* macros for retriving and setting header values */
+/* macros for retrieving and setting header values */
 #define FM10K_MSG_HDR_MASK(name) \
 	((0x1u << FM10K_MSG_##name##_SIZE) - 1)
 #define FM10K_MSG_HDR_FIELD_SET(value, name) \
@@ -291,8 +291,10 @@ struct fm10k_mbx_info {
 	u64 tx_dropped;
 	u64 tx_messages;
 	u64 tx_dwords;
+	u64 tx_mbmem_pulled;
 	u64 rx_messages;
 	u64 rx_dwords;
+	u64 rx_mbmem_pushed;
 	u64 rx_parse_err;
 
 	/* Buffer to store messages */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
index 7781e80896a6..662569d5b7c0 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -20,7 +20,7 @@
 
 #include "fm10k.h"
 #include <linux/vmalloc.h>
-#if IS_ENABLED(CONFIG_FM10K_VXLAN)
+#ifdef CONFIG_FM10K_VXLAN
 #include <net/vxlan.h>
 #endif /* CONFIG_FM10K_VXLAN */
 
@@ -556,11 +556,11 @@ int fm10k_open(struct net_device *netdev)
 	if (err)
 		goto err_set_queues;
 
-#if IS_ENABLED(CONFIG_FM10K_VXLAN)
+#ifdef CONFIG_FM10K_VXLAN
 	/* update VXLAN port configuration */
 	vxlan_get_rx_port(netdev);
-
 #endif
+
 	fm10k_up(interface);
 
 	return 0;
@@ -608,7 +608,7 @@ static netdev_tx_t fm10k_xmit_frame(struct sk_buff *skb, struct net_device *dev)
 	unsigned int r_idx = skb->queue_mapping;
 	int err;
 
-	if ((skb->protocol ==  htons(ETH_P_8021Q)) &&
+	if ((skb->protocol == htons(ETH_P_8021Q)) &&
 	    !skb_vlan_tag_present(skb)) {
 		/* FM10K only supports hardware tagging, any tags in frame
 		 * are considered 2nd level or "outer" tags
@@ -632,7 +632,7 @@ static netdev_tx_t fm10k_xmit_frame(struct sk_buff *skb, struct net_device *dev)
 			return NETDEV_TX_OK;
 		}
 
-		/* locate vlan header */
+		/* locate VLAN header */
 		vhdr = (struct vlan_hdr *)(skb->data + ETH_HLEN);
 
 		/* pull the 2 key pieces of data out of it */
@@ -705,7 +705,7 @@ static void fm10k_tx_timeout(struct net_device *netdev)
 	} else {
 		netif_info(interface, drv, netdev,
 			   "Fake Tx hang detected with timeout of %d seconds\n",
-			   netdev->watchdog_timeo/HZ);
+			   netdev->watchdog_timeo / HZ);
 
 		/* fake Tx hang - increase the kernel timeout */
 		if (netdev->watchdog_timeo < TX_TIMEO_LIMIT)
@@ -778,7 +778,7 @@ static int fm10k_update_vid(struct net_device *netdev, u16 vid, bool set)
 	if (!set)
 		clear_bit(vid, interface->active_vlans);
 
-	/* disable the default VID on ring if we have an active VLAN */
+	/* disable the default VLAN ID on ring if we have an active VLAN */
 	for (i = 0; i < interface->num_rx_queues; i++) {
 		struct fm10k_ring *rx_ring = interface->rx_ring[i];
 		u16 rx_vid = rx_ring->vid & (VLAN_N_VID - 1);
@@ -789,7 +789,9 @@ static int fm10k_update_vid(struct net_device *netdev, u16 vid, bool set)
 			rx_ring->vid &= ~FM10K_VLAN_CLEAR;
 	}
 
-	/* Do not remove default VID related entries from VLAN and MAC tables */
+	/* Do not remove default VLAN ID related entries from VLAN and MAC
+	 * tables
+	 */
 	if (!set && vid == hw->mac.default_vid)
 		return 0;
 
@@ -814,7 +816,7 @@ static int fm10k_update_vid(struct net_device *netdev, u16 vid, bool set)
 	if (err)
 		goto err_out;
 
-	/* set vid prior to syncing/unsyncing the VLAN */
+	/* set VLAN ID prior to syncing/unsyncing the VLAN */
 	interface->vid = vid + (set ? VLAN_N_VID : 0);
 
 	/* Update the unicast and multicast address list to add/drop VLAN */
@@ -1151,6 +1153,7 @@ static struct rtnl_link_stats64 *fm10k_get_stats64(struct net_device *netdev,
 int fm10k_setup_tc(struct net_device *dev, u8 tc)
 {
 	struct fm10k_intfc *interface = netdev_priv(dev);
+	int err;
 
 	/* Currently only the PF supports priority classes */
 	if (tc && (interface->hw.mac.type != fm10k_mac_pf))
@@ -1175,17 +1178,30 @@ int fm10k_setup_tc(struct net_device *dev, u8 tc)
 	netdev_reset_tc(dev);
 	netdev_set_num_tc(dev, tc);
 
-	fm10k_init_queueing_scheme(interface);
+	err = fm10k_init_queueing_scheme(interface);
+	if (err)
+		goto err_queueing_scheme;
 
-	fm10k_mbx_request_irq(interface);
+	err = fm10k_mbx_request_irq(interface);
+	if (err)
+		goto err_mbx_irq;
 
-	if (netif_running(dev))
-		fm10k_open(dev);
+	err = netif_running(dev) ? fm10k_open(dev) : 0;
+	if (err)
+		goto err_open;
 
 	/* flag to indicate SWPRI has yet to be updated */
 	interface->flags |= FM10K_FLAG_SWPRI_CONFIG;
 
 	return 0;
+err_open:
+	fm10k_mbx_free_irq(interface);
+err_mbx_irq:
+	fm10k_clear_queueing_scheme(interface);
+err_queueing_scheme:
+	netif_device_detach(dev);
+
+	return err;
 }
 
 static int fm10k_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
@@ -1355,7 +1371,7 @@ static netdev_features_t fm10k_features_check(struct sk_buff *skb,
 	if (!skb->encapsulation || fm10k_tx_encap_offload(skb))
 		return features;
 
-	return features & ~(NETIF_F_ALL_CSUM | NETIF_F_GSO_MASK);
+	return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
 }
 
 static const struct net_device_ops fm10k_netdev_ops = {
@@ -1388,8 +1404,9 @@ static const struct net_device_ops fm10k_netdev_ops = {
 
 #define DEFAULT_DEBUG_LEVEL_SHIFT 3
 
-struct net_device *fm10k_alloc_netdev(void)
+struct net_device *fm10k_alloc_netdev(const struct fm10k_info *info)
 {
+	netdev_features_t hw_features;
 	struct fm10k_intfc *interface;
 	struct net_device *dev;
 
@@ -1412,27 +1429,31 @@ struct net_device *fm10k_alloc_netdev(void)
 			 NETIF_F_TSO |
 			 NETIF_F_TSO6 |
 			 NETIF_F_TSO_ECN |
-			 NETIF_F_GSO_UDP_TUNNEL |
 			 NETIF_F_RXHASH |
 			 NETIF_F_RXCSUM;
 
+	/* Only the PF can support VXLAN and NVGRE tunnel offloads */
+	if (info->mac == fm10k_mac_pf) {
+		dev->hw_enc_features = NETIF_F_IP_CSUM |
+				       NETIF_F_TSO |
+				       NETIF_F_TSO6 |
+				       NETIF_F_TSO_ECN |
+				       NETIF_F_GSO_UDP_TUNNEL |
+				       NETIF_F_IPV6_CSUM |
+				       NETIF_F_SG;
+
+		dev->features |= NETIF_F_GSO_UDP_TUNNEL;
+	}
+
 	/* all features defined to this point should be changeable */
-	dev->hw_features |= dev->features;
+	hw_features = dev->features;
 
 	/* allow user to enable L2 forwarding acceleration */
-	dev->hw_features |= NETIF_F_HW_L2FW_DOFFLOAD;
+	hw_features |= NETIF_F_HW_L2FW_DOFFLOAD;
 
 	/* configure VLAN features */
 	dev->vlan_features |= dev->features;
 
-	/* configure tunnel offloads */
-	dev->hw_enc_features |= NETIF_F_IP_CSUM |
-				NETIF_F_TSO |
-				NETIF_F_TSO6 |
-				NETIF_F_TSO_ECN |
-				NETIF_F_GSO_UDP_TUNNEL |
-				NETIF_F_IPV6_CSUM;
-
 	/* we want to leave these both on as we cannot disable VLAN tag
 	 * insertion or stripping on the hardware since it is contained
 	 * in the FTAG and not in the frame itself.
@@ -1443,5 +1464,7 @@ struct net_device *fm10k_alloc_netdev(void)
 
 	dev->priv_flags |= IFF_UNICAST_FLT;
 
+	dev->hw_features |= hw_features;
+
 	return dev;
 }
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
index 74be792f3f1b..4eb7a6fa6b0d 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
@@ -159,16 +159,40 @@ static void fm10k_reinit(struct fm10k_intfc *interface)
 
 	fm10k_mbx_free_irq(interface);
 
+	/* free interrupts */
+	fm10k_clear_queueing_scheme(interface);
+
 	/* delay any future reset requests */
 	interface->last_reset = jiffies + (10 * HZ);
 
 	/* reset and initialize the hardware so it is in a known state */
-	err = hw->mac.ops.reset_hw(hw) ? : hw->mac.ops.init_hw(hw);
-	if (err)
+	err = hw->mac.ops.reset_hw(hw);
+	if (err) {
+		dev_err(&interface->pdev->dev, "reset_hw failed: %d\n", err);
+		goto reinit_err;
+	}
+
+	err = hw->mac.ops.init_hw(hw);
+	if (err) {
 		dev_err(&interface->pdev->dev, "init_hw failed: %d\n", err);
+		goto reinit_err;
+	}
+
+	err = fm10k_init_queueing_scheme(interface);
+	if (err) {
+		dev_err(&interface->pdev->dev,
+			"init_queueing_scheme failed: %d\n", err);
+		goto reinit_err;
+	}
 
 	/* reassociate interrupts */
-	fm10k_mbx_request_irq(interface);
+	err = fm10k_mbx_request_irq(interface);
+	if (err)
+		goto err_mbx_irq;
+
+	err = fm10k_hw_ready(interface);
+	if (err)
+		goto err_open;
 
 	/* update hardware address for VFs if perm_addr has changed */
 	if (hw->mac.type == fm10k_mac_vf) {
@@ -188,14 +212,27 @@ static void fm10k_reinit(struct fm10k_intfc *interface)
 	/* reset clock */
 	fm10k_ts_reset(interface);
 
-	if (netif_running(netdev))
-		fm10k_open(netdev);
+	err = netif_running(netdev) ? fm10k_open(netdev) : 0;
+	if (err)
+		goto err_open;
 
 	fm10k_iov_resume(interface->pdev);
 
 	rtnl_unlock();
 
 	clear_bit(__FM10K_RESETTING, &interface->state);
+
+	return;
+err_open:
+	fm10k_mbx_free_irq(interface);
+err_mbx_irq:
+	fm10k_clear_queueing_scheme(interface);
+reinit_err:
+	netif_device_detach(netdev);
+
+	rtnl_unlock();
+
+	clear_bit(__FM10K_RESETTING, &interface->state);
 }
 
 static void fm10k_reset_subtask(struct fm10k_intfc *interface)
@@ -563,7 +600,7 @@ static void fm10k_configure_tx_ring(struct fm10k_intfc *interface,
 	/* store tail pointer */
 	ring->tail = &interface->uc_addr[FM10K_TDT(reg_idx)];
 
-	/* reset ntu and ntc to place SW in sync with hardwdare */
+	/* reset ntu and ntc to place SW in sync with hardware */
 	ring->next_to_clean = 0;
 	ring->next_to_use = 0;
 
@@ -579,6 +616,13 @@ static void fm10k_configure_tx_ring(struct fm10k_intfc *interface,
 	fm10k_write_reg(hw, FM10K_PFVTCTL(reg_idx),
 			FM10K_PFVTCTL_FTAG_DESC_ENABLE);
 
+	/* Initialize XPS */
+	if (!test_and_set_bit(__FM10K_TX_XPS_INIT_DONE, &ring->state) &&
+	    ring->q_vector)
+		netif_set_xps_queue(ring->netdev,
+				    &ring->q_vector->affinity_mask,
+				    ring->queue_index);
+
 	/* enable queue */
 	fm10k_write_reg(hw, FM10K_TXDCTL(reg_idx), txdctl);
 }
@@ -669,7 +713,7 @@ static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
 	/* store tail pointer */
 	ring->tail = &interface->uc_addr[FM10K_RDT(reg_idx)];
 
-	/* reset ntu and ntc to place SW in sync with hardwdare */
+	/* reset ntu and ntc to place SW in sync with hardware */
 	ring->next_to_clean = 0;
 	ring->next_to_use = 0;
 	ring->next_to_alloc = 0;
@@ -694,7 +738,7 @@ static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
 	/* assign default VLAN to queue */
 	ring->vid = hw->mac.default_vid;
 
-	/* if we have an active VLAN, disable default VID */
+	/* if we have an active VLAN, disable default VLAN ID */
 	if (test_bit(hw->mac.default_vid, interface->active_vlans))
 		ring->vid |= FM10K_VLAN_CLEAR;
 
@@ -846,7 +890,7 @@ static irqreturn_t fm10k_msix_clean_rings(int __always_unused irq, void *data)
 	struct fm10k_q_vector *q_vector = data;
 
 	if (q_vector->rx.count || q_vector->tx.count)
-		napi_schedule(&q_vector->napi);
+		napi_schedule_irqoff(&q_vector->napi);
 
 	return IRQ_HANDLED;
 }
@@ -859,7 +903,8 @@ static irqreturn_t fm10k_msix_mbx_vf(int __always_unused irq, void *data)
 
 	/* re-enable mailbox interrupt and indicate 20us delay */
 	fm10k_write_reg(hw, FM10K_VFITR(FM10K_MBX_VECTOR),
-			FM10K_ITR_ENABLE | FM10K_MBX_INT_DELAY);
+			FM10K_ITR_ENABLE | (FM10K_MBX_INT_DELAY >>
+					    hw->mac.itr_scale));
 
 	/* service upstream mailbox */
 	if (fm10k_mbx_trylock(interface)) {
@@ -867,7 +912,7 @@ static irqreturn_t fm10k_msix_mbx_vf(int __always_unused irq, void *data)
 		fm10k_mbx_unlock(interface);
 	}
 
-	hw->mac.get_host_state = 1;
+	hw->mac.get_host_state = true;
 	fm10k_service_event_schedule(interface);
 
 	return IRQ_HANDLED;
@@ -897,7 +942,7 @@ void fm10k_netpoll(struct net_device *netdev)
 #endif
 #define FM10K_ERR_MSG(type) case (type): error = #type; break
 static void fm10k_handle_fault(struct fm10k_intfc *interface, int type,
-			      struct fm10k_fault *fault)
+			       struct fm10k_fault *fault)
 {
 	struct pci_dev *pdev = interface->pdev;
 	struct fm10k_hw *hw = &interface->hw;
@@ -1083,14 +1128,15 @@ static irqreturn_t fm10k_msix_mbx_pf(int __always_unused irq, void *data)
 	}
 
 	/* we should validate host state after interrupt event */
-	hw->mac.get_host_state = 1;
+	hw->mac.get_host_state = true;
 
 	/* validate host state, and handle VF mailboxes in the service task */
 	fm10k_service_event_schedule(interface);
 
 	/* re-enable mailbox interrupt and indicate 20us delay */
 	fm10k_write_reg(hw, FM10K_ITR(FM10K_MBX_VECTOR),
-			FM10K_ITR_ENABLE | FM10K_MBX_INT_DELAY);
+			FM10K_ITR_ENABLE | (FM10K_MBX_INT_DELAY >>
+					    hw->mac.itr_scale));
 
 	return IRQ_HANDLED;
 }
@@ -1101,6 +1147,10 @@ void fm10k_mbx_free_irq(struct fm10k_intfc *interface)
 	struct fm10k_hw *hw = &interface->hw;
 	int itr_reg;
 
+	/* no mailbox IRQ to free if MSI-X is not enabled */
+	if (!interface->msix_entries)
+		return;
+
 	/* disconnect the mailbox */
 	hw->mbx.ops.disconnect(hw, &hw->mbx);
 
@@ -1141,7 +1191,7 @@ static s32 fm10k_mbx_mac_addr(struct fm10k_hw *hw, u32 **results,
 
 	/* MAC was changed so we need reset */
 	if (is_valid_ether_addr(hw->mac.perm_addr) &&
-	    memcmp(hw->mac.perm_addr, hw->mac.addr, ETH_ALEN))
+	    !ether_addr_equal(hw->mac.perm_addr, hw->mac.addr))
 		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
 
 	/* VLAN override was changed, or default VLAN changed */
@@ -1269,7 +1319,7 @@ static s32 fm10k_update_pvid(struct fm10k_hw *hw, u32 **results,
 	if (!fm10k_glort_valid_pf(hw, glort))
 		return FM10K_ERR_PARAM;
 
-	/* verify VID is valid */
+	/* verify VLAN ID is valid */
 	if (pvid >= FM10K_VLAN_TABLE_VID_MAX)
 		return FM10K_ERR_PARAM;
 
@@ -1388,14 +1438,14 @@ static int fm10k_mbx_request_irq_pf(struct fm10k_intfc *interface)
 	}
 
 	/* Enable interrupts w/ no moderation for "other" interrupts */
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_PCIeFault), other_itr);
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_SwitchUpDown), other_itr);
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_SRAM), other_itr);
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_MaxHoldTime), other_itr);
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_VFLR), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_pcie_fault), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_switch_up_down), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_sram), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_max_hold_time), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_vflr), other_itr);
 
 	/* Enable interrupts w/ moderation for mailbox */
-	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_Mailbox), mbx_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_mailbox), mbx_itr);
 
 	/* Enable individual interrupt causes */
 	fm10k_write_reg(hw, FM10K_EIMR, FM10K_EIMR_ENABLE(PCA_FAULT) |
@@ -1423,10 +1473,15 @@ int fm10k_mbx_request_irq(struct fm10k_intfc *interface)
 		err = fm10k_mbx_request_irq_pf(interface);
 	else
 		err = fm10k_mbx_request_irq_vf(interface);
+	if (err)
+		return err;
 
 	/* connect mailbox */
-	if (!err)
-		err = hw->mbx.ops.connect(hw, &hw->mbx);
+	err = hw->mbx.ops.connect(hw, &hw->mbx);
+
+	/* if the mailbox failed to connect, then free IRQ */
+	if (err)
+		fm10k_mbx_free_irq(interface);
 
 	return err;
 }
@@ -1455,8 +1510,10 @@ void fm10k_qv_free_irq(struct fm10k_intfc *interface)
 		if (!q_vector->tx.count && !q_vector->rx.count)
 			continue;
 
-		/* disable interrupts */
+		/* clear the affinity_mask in the IRQ descriptor */
+		irq_set_affinity_hint(entry->vector, NULL);
 
+		/* disable interrupts */
 		writel(FM10K_ITR_MASK_SET, q_vector->itr);
 
 		free_irq(entry->vector, q_vector);
@@ -1514,6 +1571,9 @@ int fm10k_qv_request_irq(struct fm10k_intfc *interface)
 			goto err_out;
 		}
 
+		/* assign the mask for this irq */
+		irq_set_affinity_hint(entry->vector, &q_vector->affinity_mask);
+
 		/* Enable q_vector */
 		writel(FM10K_ITR_ENABLE, q_vector->itr);
 
@@ -1534,8 +1594,10 @@ err_out:
 		if (!q_vector->tx.count && !q_vector->rx.count)
 			continue;
 
-		/* disable interrupts */
+		/* clear the affinity_mask in the IRQ descriptor */
+		irq_set_affinity_hint(entry->vector, NULL);
 
+		/* disable interrupts */
 		writel(FM10K_ITR_MASK_SET, q_vector->itr);
 
 		free_irq(entry->vector, q_vector);
@@ -1573,7 +1635,7 @@ void fm10k_up(struct fm10k_intfc *interface)
 	netif_tx_start_all_queues(interface->netdev);
 
 	/* kick off the service timer now */
-	hw->mac.get_host_state = 1;
+	hw->mac.get_host_state = true;
 	mod_timer(&interface->service_timer, jiffies);
 }
 
@@ -1684,7 +1746,13 @@ static int fm10k_sw_init(struct fm10k_intfc *interface,
 	interface->last_reset = jiffies + (10 * HZ);
 
 	/* reset and initialize the hardware so it is in a known state */
-	err = hw->mac.ops.reset_hw(hw) ? : hw->mac.ops.init_hw(hw);
+	err = hw->mac.ops.reset_hw(hw);
+	if (err) {
+		dev_err(&pdev->dev, "reset_hw failed: %d\n", err);
+		return err;
+	}
+
+	err = hw->mac.ops.init_hw(hw);
 	if (err) {
 		dev_err(&pdev->dev, "init_hw failed: %d\n", err);
 		return err;
@@ -1722,13 +1790,6 @@ static int fm10k_sw_init(struct fm10k_intfc *interface,
 					     pci_resource_len(pdev, 4));
 	hw->sw_addr = interface->sw_addr;
 
-	/* Only the PF can support VXLAN and NVGRE offloads */
-	if (hw->mac.type != fm10k_mac_pf) {
-		netdev->hw_enc_features = 0;
-		netdev->features &= ~NETIF_F_GSO_UDP_TUNNEL;
-		netdev->hw_features &= ~NETIF_F_GSO_UDP_TUNNEL;
-	}
-
 	/* initialize DCBNL interface */
 	fm10k_dcbnl_set_ops(netdev);
 
@@ -1749,8 +1810,8 @@ static int fm10k_sw_init(struct fm10k_intfc *interface,
 	interface->rx_ring_count = FM10K_DEFAULT_RXD;
 
 	/* set default interrupt moderation */
-	interface->tx_itr = FM10K_ITR_10K;
-	interface->rx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_20K;
+	interface->tx_itr = FM10K_TX_ITR_DEFAULT;
+	interface->rx_itr = FM10K_ITR_ADAPTIVE | FM10K_RX_ITR_DEFAULT;
 
 	/* initialize vxlan_port list */
 	INIT_LIST_HEAD(&interface->vxlan_port);
@@ -1835,17 +1896,18 @@ static void fm10k_slot_warn(struct fm10k_intfc *interface)
 		return;
 	}
 
-	if (max_gts < expected_gts) {
-		dev_warn(&interface->pdev->dev,
-			 "This device requires %dGT/s of bandwidth for optimal performance.\n",
-			 expected_gts);
-		dev_warn(&interface->pdev->dev,
-			 "A %sslot with x%d lanes is suggested.\n",
-			 (hw->bus_caps.speed == fm10k_bus_speed_2500 ? "2.5GT/s " :
-			  hw->bus_caps.speed == fm10k_bus_speed_5000 ? "5.0GT/s " :
-			  hw->bus_caps.speed == fm10k_bus_speed_8000 ? "8.0GT/s " : ""),
-			 hw->bus_caps.width);
-	}
+	if (max_gts >= expected_gts)
+		return;
+
+	dev_warn(&interface->pdev->dev,
+		 "This device requires %dGT/s of bandwidth for optimal performance.\n",
+		 expected_gts);
+	dev_warn(&interface->pdev->dev,
+		 "A %sslot with x%d lanes is suggested.\n",
+		 (hw->bus_caps.speed == fm10k_bus_speed_2500 ? "2.5GT/s " :
+		  hw->bus_caps.speed == fm10k_bus_speed_5000 ? "5.0GT/s " :
+		  hw->bus_caps.speed == fm10k_bus_speed_8000 ? "8.0GT/s " : ""),
+		 hw->bus_caps.width);
 }
 
 /**
@@ -1859,8 +1921,7 @@ static void fm10k_slot_warn(struct fm10k_intfc *interface)
  * The OS initialization, configuring of the interface private structure,
  * and a hardware reset occur.
  **/
-static int fm10k_probe(struct pci_dev *pdev,
-		       const struct pci_device_id *ent)
+static int fm10k_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct net_device *netdev;
 	struct fm10k_intfc *interface;
@@ -1894,7 +1955,7 @@ static int fm10k_probe(struct pci_dev *pdev,
 	pci_set_master(pdev);
 	pci_save_state(pdev);
 
-	netdev = fm10k_alloc_netdev();
+	netdev = fm10k_alloc_netdev(fm10k_info_tbl[ent->driver_data]);
 	if (!netdev) {
 		err = -ENOMEM;
 		goto err_alloc_netdev;
@@ -2071,8 +2132,10 @@ static int fm10k_resume(struct pci_dev *pdev)
 
 	/* reset hardware to known state */
 	err = hw->mac.ops.init_hw(&interface->hw);
-	if (err)
+	if (err) {
+		dev_err(&pdev->dev, "init_hw failed: %d\n", err);
 		return err;
+	}
 
 	/* reset statistics starting values */
 	hw->mac.ops.rebind_hw_stats(hw, &interface->stats);
@@ -2083,16 +2146,22 @@ static int fm10k_resume(struct pci_dev *pdev)
 	rtnl_lock();
 
 	err = fm10k_init_queueing_scheme(interface);
-	if (!err) {
-		fm10k_mbx_request_irq(interface);
-		if (netif_running(netdev))
-			err = fm10k_open(netdev);
-	}
+	if (err)
+		goto err_queueing_scheme;
 
-	rtnl_unlock();
+	err = fm10k_mbx_request_irq(interface);
+	if (err)
+		goto err_mbx_irq;
 
+	err = fm10k_hw_ready(interface);
 	if (err)
-		return err;
+		goto err_open;
+
+	err = netif_running(netdev) ? fm10k_open(netdev) : 0;
+	if (err)
+		goto err_open;
+
+	rtnl_unlock();
 
 	/* assume host is not ready, to prevent race with watchdog in case we
 	 * actually don't have connection to the switch
@@ -2110,6 +2179,14 @@ static int fm10k_resume(struct pci_dev *pdev)
 	netif_device_attach(netdev);
 
 	return 0;
+err_open:
+	fm10k_mbx_free_irq(interface);
+err_mbx_irq:
+	fm10k_clear_queueing_scheme(interface);
+err_queueing_scheme:
+	rtnl_unlock();
+
+	return err;
 }
 
 /**
@@ -2185,6 +2262,9 @@ static pci_ers_result_t fm10k_io_error_detected(struct pci_dev *pdev,
 	if (netif_running(netdev))
 		fm10k_close(netdev);
 
+	/* free interrupts */
+	fm10k_clear_queueing_scheme(interface);
+
 	fm10k_mbx_free_irq(interface);
 
 	pci_disable_device(pdev);
@@ -2248,11 +2328,22 @@ static void fm10k_io_resume(struct pci_dev *pdev)
 	int err = 0;
 
 	/* reset hardware to known state */
-	hw->mac.ops.init_hw(&interface->hw);
+	err = hw->mac.ops.init_hw(&interface->hw);
+	if (err) {
+		dev_err(&pdev->dev, "init_hw failed: %d\n", err);
+		return;
+	}
 
 	/* reset statistics starting values */
 	hw->mac.ops.rebind_hw_stats(hw, &interface->stats);
 
+	err = fm10k_init_queueing_scheme(interface);
+	if (err) {
+		dev_err(&interface->pdev->dev,
+			"init_queueing_scheme failed: %d\n", err);
+		return;
+	}
+
 	/* reassociate interrupts */
 	fm10k_mbx_request_irq(interface);
 
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
index 8c0bdc4e4edd..62ccebc5f728 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -150,19 +150,26 @@ static s32 fm10k_init_hw_pf(struct fm10k_hw *hw)
 				FM10K_TPH_RXCTRL_HDR_WROEN);
 	}
 
-	/* set max hold interval to align with 1.024 usec in all modes */
+	/* set max hold interval to align with 1.024 usec in all modes and
+	 * store ITR scale
+	 */
 	switch (hw->bus.speed) {
 	case fm10k_bus_speed_2500:
 		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN1;
+		hw->mac.itr_scale = FM10K_TDLEN_ITR_SCALE_GEN1;
 		break;
 	case fm10k_bus_speed_5000:
 		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN2;
+		hw->mac.itr_scale = FM10K_TDLEN_ITR_SCALE_GEN2;
 		break;
 	case fm10k_bus_speed_8000:
 		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN3;
+		hw->mac.itr_scale = FM10K_TDLEN_ITR_SCALE_GEN3;
 		break;
 	default:
 		dma_ctrl = 0;
+		/* just in case, assume Gen3 ITR scale */
+		hw->mac.itr_scale = FM10K_TDLEN_ITR_SCALE_GEN3;
 		break;
 	}
 
@@ -259,7 +266,6 @@ static s32 fm10k_read_mac_addr_pf(struct fm10k_hw *hw)
 {
 	u8 perm_addr[ETH_ALEN];
 	u32 serial_num;
-	int i;
 
 	serial_num = fm10k_read_reg(hw, FM10K_SM_AREA(1));
 
@@ -281,10 +287,8 @@ static s32 fm10k_read_mac_addr_pf(struct fm10k_hw *hw)
 	perm_addr[4] = (u8)(serial_num >> 8);
 	perm_addr[5] = (u8)(serial_num);
 
-	for (i = 0; i < ETH_ALEN; i++) {
-		hw->mac.perm_addr[i] = perm_addr[i];
-		hw->mac.addr[i] = perm_addr[i];
-	}
+	ether_addr_copy(hw->mac.perm_addr, perm_addr);
+	ether_addr_copy(hw->mac.addr, perm_addr);
 
 	return 0;
 }
@@ -325,7 +329,7 @@ static s32 fm10k_update_xc_addr_pf(struct fm10k_hw *hw, u16 glort,
 	/* clear set bit from VLAN ID */
 	vid &= ~FM10K_VLAN_CLEAR;
 
-	/* if glort or vlan are not valid return error */
+	/* if glort or VLAN are not valid return error */
 	if (!fm10k_glort_valid_pf(hw, glort) || vid >= FM10K_VLAN_TABLE_VID_MAX)
 		return FM10K_ERR_PARAM;
 
@@ -334,8 +338,8 @@ static s32 fm10k_update_xc_addr_pf(struct fm10k_hw *hw, u16 glort,
 						 ((u32)mac[3] << 16) |
 						 ((u32)mac[4] << 8) |
 						 ((u32)mac[5]));
-	mac_update.mac_upper = cpu_to_le16(((u32)mac[0] << 8) |
-						 ((u32)mac[1]));
+	mac_update.mac_upper = cpu_to_le16(((u16)mac[0] << 8) |
+					   ((u16)mac[1]));
 	mac_update.vlan = cpu_to_le16(vid);
 	mac_update.glort = cpu_to_le16(glort);
 	mac_update.action = add ? 0 : 1;
@@ -410,6 +414,7 @@ static s32 fm10k_update_xcast_mode_pf(struct fm10k_hw *hw, u16 glort, u8 mode)
 
 	if (mode > FM10K_XCAST_MODE_NONE)
 		return FM10K_ERR_PARAM;
+
 	/* if glort is not valid return error */
 	if (!fm10k_glort_valid_pf(hw, glort))
 		return FM10K_ERR_PARAM;
@@ -903,6 +908,13 @@ static s32 fm10k_iov_assign_default_mac_vlan_pf(struct fm10k_hw *hw,
 	fm10k_write_reg(hw, FM10K_TDBAL(vf_q_idx), tdbal);
 	fm10k_write_reg(hw, FM10K_TDBAH(vf_q_idx), tdbah);
 
+	/* Provide the VF the ITR scale, using software-defined fields in TDLEN
+	 * to pass the information during VF initialization. See definition of
+	 * FM10K_TDLEN_ITR_SCALE_SHIFT for more details.
+	 */
+	fm10k_write_reg(hw, FM10K_TDLEN(vf_q_idx), hw->mac.itr_scale <<
+						   FM10K_TDLEN_ITR_SCALE_SHIFT);
+
 err_out:
 	/* configure Queue control register */
 	txqctl = ((u32)vf_vid << FM10K_TXQCTL_VID_SHIFT) &
@@ -910,7 +922,7 @@ err_out:
 	txqctl |= (vf_idx << FM10K_TXQCTL_TC_SHIFT) |
 		  FM10K_TXQCTL_VF | vf_idx;
 
-	/* assign VID */
+	/* assign VLAN ID */
 	for (i = 0; i < queues_per_pool; i++)
 		fm10k_write_reg(hw, FM10K_TXQCTL(vf_q_idx + i), txqctl);
 
@@ -1035,6 +1047,12 @@ static s32 fm10k_iov_reset_resources_pf(struct fm10k_hw *hw,
 	for (i = queues_per_pool; i--;) {
 		fm10k_write_reg(hw, FM10K_TDBAL(vf_q_idx + i), tdbal);
 		fm10k_write_reg(hw, FM10K_TDBAH(vf_q_idx + i), tdbah);
+		/* See definition of FM10K_TDLEN_ITR_SCALE_SHIFT for an
+		 * explanation of how TDLEN is used.
+		 */
+		fm10k_write_reg(hw, FM10K_TDLEN(vf_q_idx + i),
+				hw->mac.itr_scale <<
+				FM10K_TDLEN_ITR_SCALE_SHIFT);
 		fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx + i), vf_q_idx + i);
 		fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx + i), vf_q_idx + i);
 	}
@@ -1155,14 +1173,14 @@ s32 fm10k_iov_msg_msix_pf(struct fm10k_hw *hw, u32 **results,
 }
 
 /**
- * fm10k_iov_select_vid - Select correct default VID
+ * fm10k_iov_select_vid - Select correct default VLAN ID
  * @hw: Pointer to hardware structure
- * @vid: VID to correct
+ * @vid: VLAN ID to correct
  *
- * Will report an error if VID is out of range. For VID = 0, it will return
- * either the pf_vid or sw_vid depending on which one is set.
+ * Will report an error if the VLAN ID is out of range. For VID = 0, it will
+ * return either the pf_vid or sw_vid depending on which one is set.
  */
-static inline s32 fm10k_iov_select_vid(struct fm10k_vf_info *vf_info, u16 vid)
+static s32 fm10k_iov_select_vid(struct fm10k_vf_info *vf_info, u16 vid)
 {
 	if (!vid)
 		return vf_info->pf_vid ? vf_info->pf_vid : vf_info->sw_vid;
@@ -1212,11 +1230,11 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 		set = !(vid & FM10K_VLAN_CLEAR);
 		vid &= ~FM10K_VLAN_CLEAR;
 
-		err = fm10k_iov_select_vid(vf_info, vid);
+		err = fm10k_iov_select_vid(vf_info, (u16)vid);
 		if (err < 0)
 			return err;
-		else
-			vid = err;
+
+		vid = err;
 
 		/* update VSI info for VF in regards to VLAN table */
 		err = hw->mac.ops.update_vlan(hw, vid, vf_info->vsi, set);
@@ -1232,7 +1250,7 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 
 		/* block attempts to set MAC for a locked device */
 		if (is_valid_ether_addr(vf_info->mac) &&
-		    memcmp(mac, vf_info->mac, ETH_ALEN))
+		    !ether_addr_equal(mac, vf_info->mac))
 			return FM10K_ERR_PARAM;
 
 		set = !(vlan & FM10K_VLAN_CLEAR);
@@ -1241,8 +1259,8 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 		err = fm10k_iov_select_vid(vf_info, vlan);
 		if (err < 0)
 			return err;
-		else
-			vlan = err;
+
+		vlan = (u16)err;
 
 		/* notify switch of request for new unicast address */
 		err = hw->mac.ops.update_uc_addr(hw, vf_info->glort,
@@ -1267,8 +1285,8 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
 		err = fm10k_iov_select_vid(vf_info, vlan);
 		if (err < 0)
 			return err;
-		else
-			vlan = err;
+
+		vlan = (u16)err;
 
 		/* notify switch of request for new multicast address */
 		err = hw->mac.ops.update_mc_addr(hw, vf_info->glort,
@@ -1396,14 +1414,6 @@ s32 fm10k_iov_msg_lport_state_pf(struct fm10k_hw *hw, u32 **results,
 	return err;
 }
 
-const struct fm10k_msg_data fm10k_iov_msg_data_pf[] = {
-	FM10K_TLV_MSG_TEST_HANDLER(fm10k_tlv_msg_test),
-	FM10K_VF_MSG_MSIX_HANDLER(fm10k_iov_msg_msix_pf),
-	FM10K_VF_MSG_MAC_VLAN_HANDLER(fm10k_iov_msg_mac_vlan_pf),
-	FM10K_VF_MSG_LPORT_STATE_HANDLER(fm10k_iov_msg_lport_state_pf),
-	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
-};
-
 /**
  *  fm10k_update_stats_hw_pf - Updates hardware related statistics of PF
  *  @hw: pointer to hardware structure
@@ -1431,9 +1441,10 @@ static void fm10k_update_hw_stats_pf(struct fm10k_hw *hw,
 		xec = fm10k_read_hw_stats_32b(hw, FM10K_STATS_XEC, &stats->xec);
 		vlan_drop = fm10k_read_hw_stats_32b(hw, FM10K_STATS_VLAN_DROP,
 						    &stats->vlan_drop);
-		loopback_drop = fm10k_read_hw_stats_32b(hw,
-							FM10K_STATS_LOOPBACK_DROP,
-							&stats->loopback_drop);
+		loopback_drop =
+			fm10k_read_hw_stats_32b(hw,
+						FM10K_STATS_LOOPBACK_DROP,
+						&stats->loopback_drop);
 		nodesc_drop = fm10k_read_hw_stats_32b(hw,
 						      FM10K_STATS_NODESC_DROP,
 						      &stats->nodesc_drop);
@@ -1678,8 +1689,8 @@ const struct fm10k_tlv_attr fm10k_update_pvid_msg_attr[] = {
  *
  *  This handler configures the default VLAN for the PF
  **/
-s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *hw, u32 **results,
-			     struct fm10k_mbx_info *mbx)
+static s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *hw, u32 **results,
+				    struct fm10k_mbx_info *mbx)
 {
 	u16 glort, pvid;
 	u32 pvid_update;
@@ -1698,7 +1709,7 @@ s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *hw, u32 **results,
 	if (!fm10k_glort_valid_pf(hw, glort))
 		return FM10K_ERR_PARAM;
 
-	/* verify VID is valid */
+	/* verify VLAN ID is valid */
 	if (pvid >= FM10K_VLAN_TABLE_VID_MAX)
 		return FM10K_ERR_PARAM;
 
@@ -1855,39 +1866,39 @@ static const struct fm10k_msg_data fm10k_msg_data_pf[] = {
 	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
 };
 
-static struct fm10k_mac_ops mac_ops_pf = {
-	.get_bus_info		= &fm10k_get_bus_info_generic,
-	.reset_hw		= &fm10k_reset_hw_pf,
-	.init_hw		= &fm10k_init_hw_pf,
-	.start_hw		= &fm10k_start_hw_generic,
-	.stop_hw		= &fm10k_stop_hw_generic,
-	.update_vlan		= &fm10k_update_vlan_pf,
-	.read_mac_addr		= &fm10k_read_mac_addr_pf,
-	.update_uc_addr		= &fm10k_update_uc_addr_pf,
-	.update_mc_addr		= &fm10k_update_mc_addr_pf,
-	.update_xcast_mode	= &fm10k_update_xcast_mode_pf,
-	.update_int_moderator	= &fm10k_update_int_moderator_pf,
-	.update_lport_state	= &fm10k_update_lport_state_pf,
-	.update_hw_stats	= &fm10k_update_hw_stats_pf,
-	.rebind_hw_stats	= &fm10k_rebind_hw_stats_pf,
-	.configure_dglort_map	= &fm10k_configure_dglort_map_pf,
-	.set_dma_mask		= &fm10k_set_dma_mask_pf,
-	.get_fault		= &fm10k_get_fault_pf,
-	.get_host_state		= &fm10k_get_host_state_pf,
-	.adjust_systime		= &fm10k_adjust_systime_pf,
-	.read_systime		= &fm10k_read_systime_pf,
+static const struct fm10k_mac_ops mac_ops_pf = {
+	.get_bus_info		= fm10k_get_bus_info_generic,
+	.reset_hw		= fm10k_reset_hw_pf,
+	.init_hw		= fm10k_init_hw_pf,
+	.start_hw		= fm10k_start_hw_generic,
+	.stop_hw		= fm10k_stop_hw_generic,
+	.update_vlan		= fm10k_update_vlan_pf,
+	.read_mac_addr		= fm10k_read_mac_addr_pf,
+	.update_uc_addr		= fm10k_update_uc_addr_pf,
+	.update_mc_addr		= fm10k_update_mc_addr_pf,
+	.update_xcast_mode	= fm10k_update_xcast_mode_pf,
+	.update_int_moderator	= fm10k_update_int_moderator_pf,
+	.update_lport_state	= fm10k_update_lport_state_pf,
+	.update_hw_stats	= fm10k_update_hw_stats_pf,
+	.rebind_hw_stats	= fm10k_rebind_hw_stats_pf,
+	.configure_dglort_map	= fm10k_configure_dglort_map_pf,
+	.set_dma_mask		= fm10k_set_dma_mask_pf,
+	.get_fault		= fm10k_get_fault_pf,
+	.get_host_state		= fm10k_get_host_state_pf,
+	.adjust_systime		= fm10k_adjust_systime_pf,
+	.read_systime		= fm10k_read_systime_pf,
 };
 
-static struct fm10k_iov_ops iov_ops_pf = {
-	.assign_resources		= &fm10k_iov_assign_resources_pf,
-	.configure_tc			= &fm10k_iov_configure_tc_pf,
-	.assign_int_moderator		= &fm10k_iov_assign_int_moderator_pf,
+static const struct fm10k_iov_ops iov_ops_pf = {
+	.assign_resources		= fm10k_iov_assign_resources_pf,
+	.configure_tc			= fm10k_iov_configure_tc_pf,
+	.assign_int_moderator		= fm10k_iov_assign_int_moderator_pf,
 	.assign_default_mac_vlan	= fm10k_iov_assign_default_mac_vlan_pf,
-	.reset_resources		= &fm10k_iov_reset_resources_pf,
-	.set_lport			= &fm10k_iov_set_lport_pf,
-	.reset_lport			= &fm10k_iov_reset_lport_pf,
-	.update_stats			= &fm10k_iov_update_stats_pf,
-	.report_timestamp		= &fm10k_iov_report_timestamp_pf,
+	.reset_resources		= fm10k_iov_reset_resources_pf,
+	.set_lport			= fm10k_iov_set_lport_pf,
+	.reset_lport			= fm10k_iov_reset_lport_pf,
+	.update_stats			= fm10k_iov_update_stats_pf,
+	.report_timestamp		= fm10k_iov_report_timestamp_pf,
 };
 
 static s32 fm10k_get_invariants_pf(struct fm10k_hw *hw)
@@ -1897,9 +1908,9 @@ static s32 fm10k_get_invariants_pf(struct fm10k_hw *hw)
 	return fm10k_sm_mbx_init(hw, &hw->mbx, fm10k_msg_data_pf);
 }
 
-struct fm10k_info fm10k_pf_info = {
+const struct fm10k_info fm10k_pf_info = {
 	.mac		= fm10k_mac_pf,
-	.get_invariants	= &fm10k_get_invariants_pf,
+	.get_invariants	= fm10k_get_invariants_pf,
 	.mac_ops	= &mac_ops_pf,
 	.iov_ops	= &iov_ops_pf,
 };
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.h b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
index 40a0dbc62a04..b2d96b45ca3c 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -74,6 +74,11 @@ enum fm10k_pf_tlv_attr_id_v1 {
 #define FM10K_MSG_UPDATE_PVID_PVID_SHIFT	16
 #define FM10K_MSG_UPDATE_PVID_PVID_SIZE		16
 
+/* The following data structures are overlayed directly onto TLV mailbox
+ * messages, and must not break 4 byte alignment. Ensure the structures line
+ * up correctly as per their TLV definition.
+ */
+
 struct fm10k_mac_update {
 	__le32	mac_lower;
 	__le16	mac_upper;
@@ -81,34 +86,32 @@ struct fm10k_mac_update {
 	__le16	glort;
 	u8	flags;
 	u8	action;
-} __packed;
+} __aligned(4) __packed;
 
 struct fm10k_global_table_data {
 	__le32	used;
 	__le32	avail;
-} __packed;
+} __aligned(4) __packed;
 
 struct fm10k_swapi_error {
 	__le32				status;
 	struct fm10k_global_table_data	mac;
 	struct fm10k_global_table_data	nexthop;
 	struct fm10k_global_table_data	ffu;
-} __packed;
+} __aligned(4) __packed;
 
 struct fm10k_swapi_1588_timestamp {
 	__le64 egress;
 	__le64 ingress;
 	__le16 dglort;
 	__le16 sglort;
-} __packed;
+} __aligned(4) __packed;
 
 s32 fm10k_msg_lport_map_pf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
 extern const struct fm10k_tlv_attr fm10k_lport_map_msg_attr[];
 #define FM10K_PF_MSG_LPORT_MAP_HANDLER(func) \
 	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_LPORT_MAP, \
 			  fm10k_lport_map_msg_attr, func)
-s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *, u32 **,
-			     struct fm10k_mbx_info *);
 extern const struct fm10k_tlv_attr fm10k_update_pvid_msg_attr[];
 #define FM10K_PF_MSG_UPDATE_PVID_HANDLER(func) \
 	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_UPDATE_PVID, \
@@ -129,7 +132,6 @@ s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *, u32 **,
 			      struct fm10k_mbx_info *);
 s32 fm10k_iov_msg_lport_state_pf(struct fm10k_hw *, u32 **,
 				 struct fm10k_mbx_info *);
-extern const struct fm10k_msg_data fm10k_iov_msg_data_pf[];
 
-extern struct fm10k_info fm10k_pf_info;
+extern const struct fm10k_info fm10k_pf_info;
 #endif /* _FM10K_PF_H */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
index 9b29d7b0377a..ab01bb30752f 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -48,8 +48,8 @@ s32 fm10k_tlv_msg_init(u32 *msg, u16 msg_id)
  *  the attribute buffer.  It will return success if provided with a valid
  *  pointers.
  **/
-s32 fm10k_tlv_attr_put_null_string(u32 *msg, u16 attr_id,
-				   const unsigned char *string)
+static s32 fm10k_tlv_attr_put_null_string(u32 *msg, u16 attr_id,
+					  const unsigned char *string)
 {
 	u32 attr_data = 0, len = 0;
 	u32 *attr;
@@ -98,7 +98,7 @@ s32 fm10k_tlv_attr_put_null_string(u32 *msg, u16 attr_id,
  *  it in the array pointed by by string.  It will return success if provided
  *  with a valid pointers.
  **/
-s32 fm10k_tlv_attr_get_null_string(u32 *attr, unsigned char *string)
+static s32 fm10k_tlv_attr_get_null_string(u32 *attr, unsigned char *string)
 {
 	u32 len;
 
@@ -353,7 +353,7 @@ s32 fm10k_tlv_attr_get_le_struct(u32 *attr, void *le_struct, u32 len)
  *  function will return NULL on failure, and a pointer to the start
  *  of the nested attributes on success.
  **/
-u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
+static u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
 {
 	u32 *attr;
 
@@ -370,7 +370,7 @@ u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
 }
 
 /**
- *  fm10k_tlv_attr_nest_start - Start a set of nested attributes
+ *  fm10k_tlv_attr_nest_stop - Stop a set of nested attributes
  *  @msg: Pointer to message block
  *
  *  This function closes off an existing set of nested attributes.  The
@@ -378,7 +378,7 @@ u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
  *  the case of a nest within the nest this would be the outer nest pointer.
  *  This function will return success provided all pointers are valid.
  **/
-s32 fm10k_tlv_attr_nest_stop(u32 *msg)
+static s32 fm10k_tlv_attr_nest_stop(u32 *msg)
 {
 	u32 *attr;
 	u32 len;
@@ -483,8 +483,8 @@ static s32 fm10k_tlv_attr_validate(u32 *attr,
  *  FM10K_NOT_IMPLEMENTED for any attribute that is outside of the array
  *  and 0 on success.
  **/
-s32 fm10k_tlv_attr_parse(u32 *attr, u32 **results,
-			 const struct fm10k_tlv_attr *tlv_attr)
+static s32 fm10k_tlv_attr_parse(u32 *attr, u32 **results,
+				const struct fm10k_tlv_attr *tlv_attr)
 {
 	u32 i, attr_id, offset = 0;
 	s32 err = 0;
@@ -755,7 +755,7 @@ parse_nested:
 		err = fm10k_tlv_attr_get_mac_vlan(
 					results[FM10K_TEST_MSG_MAC_ADDR],
 					result_mac, &result_vlan);
-		if (!err && memcmp(test_mac, result_mac, ETH_ALEN))
+		if (!err && !ether_addr_equal(test_mac, result_mac))
 			err = FM10K_ERR_INVALID_VALUE;
 		if (!err && test_vlan != result_vlan)
 			err = FM10K_ERR_INVALID_VALUE;
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h
index 7e045e8bf1eb..e1845e0a17d8 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -38,9 +38,9 @@ struct fm10k_msg_data;
  * mailbox size we will provide a message with the above header and it
  * will be segmented and transported to the mailbox to the other side where
  * it is reassembled.  It contains the following fields:
- * Len: Length of the message in bytes excluding the message header
+ * Length: Length of the message in bytes excluding the message header
  * Flags: TBD
- * Rule: These will be the message/argument types we pass
+ * Type/ID: These will be the message/argument types we pass
  */
 /* message data header */
 #define FM10K_TLV_ID_SHIFT		0
@@ -106,8 +106,6 @@ struct fm10k_msg_data {
 #define FM10K_MSG_HANDLER(id, attr, func) { id, attr, func }
 
 s32 fm10k_tlv_msg_init(u32 *, u16);
-s32 fm10k_tlv_attr_put_null_string(u32 *, u16, const unsigned char *);
-s32 fm10k_tlv_attr_get_null_string(u32 *, unsigned char *);
 s32 fm10k_tlv_attr_put_mac_vlan(u32 *, u16, const u8 *, u16);
 s32 fm10k_tlv_attr_get_mac_vlan(u32 *, u8 *, u16 *);
 s32 fm10k_tlv_attr_put_bool(u32 *, u16);
@@ -147,9 +145,6 @@ s32 fm10k_tlv_attr_get_value(u32 *, void *, u32);
 		fm10k_tlv_attr_get_value(attr, ptr, sizeof(s64))
 s32 fm10k_tlv_attr_put_le_struct(u32 *, u16, const void *, u32);
 s32 fm10k_tlv_attr_get_le_struct(u32 *, void *, u32);
-u32 *fm10k_tlv_attr_nest_start(u32 *, u16);
-s32 fm10k_tlv_attr_nest_stop(u32 *);
-s32 fm10k_tlv_attr_parse(u32 *, u32 **, const struct fm10k_tlv_attr *);
 s32 fm10k_tlv_msg_parse(struct fm10k_hw *, u32 *, struct fm10k_mbx_info *,
 			const struct fm10k_msg_data *);
 s32 fm10k_tlv_msg_error(struct fm10k_hw *hw, u32 **results,
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_type.h b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
index 318a212f0a78..854ebb1906bf 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_type.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
@@ -1,5 +1,5 @@
 /* Intel Ethernet Switch Host Interface Driver
- * Copyright(c) 2013 - 2014 Intel Corporation.
+ * Copyright(c) 2013 - 2015 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -77,6 +77,7 @@ struct fm10k_hw;
 #define FM10K_PCIE_SRIOV_CTRL_VFARI		0x10
 
 #define FM10K_ERR_PARAM				-2
+#define FM10K_ERR_NO_RESOURCES			-3
 #define FM10K_ERR_REQUESTS_PENDING		-4
 #define FM10K_ERR_RESET_REQUESTED		-5
 #define FM10K_ERR_DMA_PENDING			-6
@@ -271,6 +272,20 @@ struct fm10k_hw;
 #define FM10K_TDBAL(_n)		((0x40 * (_n)) + 0x8000)
 #define FM10K_TDBAH(_n)		((0x40 * (_n)) + 0x8001)
 #define FM10K_TDLEN(_n)		((0x40 * (_n)) + 0x8002)
+/* When fist initialized, VFs need to know the Interrupt Throttle Rate (ITR)
+ * scale which is based on the PCIe speed but the speed information in the PCI
+ * configuration space may not be accurate. The PF already knows the ITR scale
+ * but there is no defined method to pass that information from the PF to the
+ * VF. This is accomplished during VF initialization by temporarily co-opting
+ * the yet-to-be-used TDLEN register to have the PF store the ITR shift for
+ * the VF to retrieve before the VF needs to use the TDLEN register for its
+ * intended purpose, i.e. before the Tx resources are allocated.
+ */
+#define FM10K_TDLEN_ITR_SCALE_SHIFT		9
+#define FM10K_TDLEN_ITR_SCALE_MASK		0x00000E00
+#define FM10K_TDLEN_ITR_SCALE_GEN1		2
+#define FM10K_TDLEN_ITR_SCALE_GEN2		1
+#define FM10K_TDLEN_ITR_SCALE_GEN3		0
 #define FM10K_TPH_TXCTRL(_n)	((0x40 * (_n)) + 0x8003)
 #define FM10K_TPH_TXCTRL_DESC_TPHEN		0x00000020
 #define FM10K_TPH_TXCTRL_DESC_RROEN		0x00000200
@@ -339,7 +354,7 @@ struct fm10k_hw;
 #define FM10K_VLAN_TABLE_VID_MAX		4096
 #define FM10K_VLAN_TABLE_VSI_MAX		64
 #define FM10K_VLAN_LENGTH_SHIFT			16
-#define FM10K_VLAN_CLEAR			(1 << 15)
+#define FM10K_VLAN_CLEAR			BIT(15)
 #define FM10K_VLAN_ALL \
 	((FM10K_VLAN_TABLE_VID_MAX - 1) << FM10K_VLAN_LENGTH_SHIFT)
 
@@ -373,13 +388,13 @@ struct fm10k_hw;
 #define FM10K_SW_SYSTIME_PULSE(_n)	((_n) + 0x02252)
 
 enum fm10k_int_source {
-	fm10k_int_Mailbox	= 0,
-	fm10k_int_PCIeFault	= 1,
-	fm10k_int_SwitchUpDown	= 2,
-	fm10k_int_SwitchEvent	= 3,
-	fm10k_int_SRAM		= 4,
-	fm10k_int_VFLR		= 5,
-	fm10k_int_MaxHoldTime	= 6,
+	fm10k_int_mailbox		= 0,
+	fm10k_int_pcie_fault		= 1,
+	fm10k_int_switch_up_down	= 2,
+	fm10k_int_switch_event		= 3,
+	fm10k_int_sram			= 4,
+	fm10k_int_vflr			= 5,
+	fm10k_int_max_hold_time		= 6,
 	fm10k_int_sources_max_pf
 };
 
@@ -535,7 +550,6 @@ struct fm10k_mac_ops {
 				    struct fm10k_dglort_cfg *);
 	void (*set_dma_mask)(struct fm10k_hw *, u64);
 	s32 (*get_fault)(struct fm10k_hw *, int, struct fm10k_fault *);
-	void (*request_lport_map)(struct fm10k_hw *);
 	s32 (*adjust_systime)(struct fm10k_hw *, s32 ppb);
 	u64 (*read_systime)(struct fm10k_hw *);
 };
@@ -559,6 +573,7 @@ struct fm10k_mac_info {
 	bool get_host_state;
 	bool tx_ready;
 	u32 dglort_map;
+	u8 itr_scale;
 };
 
 struct fm10k_swapi_table_info {
@@ -644,10 +659,10 @@ enum fm10k_devices {
 };
 
 struct fm10k_info {
-	enum fm10k_mac_type	mac;
-	s32			(*get_invariants)(struct fm10k_hw *);
-	struct fm10k_mac_ops	*mac_ops;
-	struct fm10k_iov_ops	*iov_ops;
+	enum fm10k_mac_type		mac;
+	s32				(*get_invariants)(struct fm10k_hw *);
+	const struct fm10k_mac_ops	*mac_ops;
+	const struct fm10k_iov_ops	*iov_ops;
 };
 
 struct fm10k_hw {
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_vf.c b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
index 36c8b0aa08fd..91f8d7311f3b 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
@@ -28,7 +28,7 @@
 static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
 {
 	u8 *perm_addr = hw->mac.perm_addr;
-	u32 bal = 0, bah = 0;
+	u32 bal = 0, bah = 0, tdlen;
 	s32 err;
 	u16 i;
 
@@ -48,6 +48,9 @@ static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
 		       ((u32)perm_addr[2]);
 	}
 
+	/* restore default itr_scale for next VF initialization */
+	tdlen = hw->mac.itr_scale << FM10K_TDLEN_ITR_SCALE_SHIFT;
+
 	/* The queues have already been disabled so we just need to
 	 * update their base address registers
 	 */
@@ -56,6 +59,12 @@ static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
 		fm10k_write_reg(hw, FM10K_TDBAH(i), bah);
 		fm10k_write_reg(hw, FM10K_RDBAL(i), bal);
 		fm10k_write_reg(hw, FM10K_RDBAH(i), bah);
+		/* Restore ITR scale in software-defined mechanism in TDLEN
+		 * for next VF initialization. See definition of
+		 * FM10K_TDLEN_ITR_SCALE_SHIFT for more details on the use of
+		 * TDLEN here.
+		 */
+		fm10k_write_reg(hw, FM10K_TDLEN(i), tdlen);
 	}
 
 	return 0;
@@ -103,7 +112,14 @@ static s32 fm10k_init_hw_vf(struct fm10k_hw *hw)
 	s32 err;
 	u16 i;
 
-	/* assume we always have at least 1 queue */
+	/* verify we have at least 1 queue */
+	if (!~fm10k_read_reg(hw, FM10K_TXQCTL(0)) ||
+	    !~fm10k_read_reg(hw, FM10K_RXQCTL(0))) {
+		err = FM10K_ERR_NO_RESOURCES;
+		goto reset_max_queues;
+	}
+
+	/* determine how many queues we have */
 	for (i = 1; tqdloc0 && (i < FM10K_MAX_QUEUES_POOL); i++) {
 		/* verify the Descriptor cache offsets are increasing */
 		tqdloc = ~fm10k_read_reg(hw, FM10K_TQDLOC(i));
@@ -119,16 +135,28 @@ static s32 fm10k_init_hw_vf(struct fm10k_hw *hw)
 	/* shut down queues we own and reset DMA configuration */
 	err = fm10k_disable_queues_generic(hw, i);
 	if (err)
-		return err;
+		goto reset_max_queues;
 
 	/* record maximum queue count */
 	hw->mac.max_queues = i;
 
-	/* fetch default VLAN */
+	/* fetch default VLAN and ITR scale */
 	hw->mac.default_vid = (fm10k_read_reg(hw, FM10K_TXQCTL(0)) &
 			       FM10K_TXQCTL_VID_MASK) >> FM10K_TXQCTL_VID_SHIFT;
+	/* Read the ITR scale from TDLEN. See the definition of
+	 * FM10K_TDLEN_ITR_SCALE_SHIFT for more information about how TDLEN is
+	 * used here.
+	 */
+	hw->mac.itr_scale = (fm10k_read_reg(hw, FM10K_TDLEN(0)) &
+			     FM10K_TDLEN_ITR_SCALE_MASK) >>
+			    FM10K_TDLEN_ITR_SCALE_SHIFT;
 
 	return 0;
+
+reset_max_queues:
+	hw->mac.max_queues = 0;
+
+	return err;
 }
 
 /* This structure defines the attibutes to be parsed below */
@@ -270,7 +298,7 @@ static s32 fm10k_update_uc_addr_vf(struct fm10k_hw *hw, u16 glort,
 
 	/* verify we are not locked down on the MAC address */
 	if (is_valid_ether_addr(hw->mac.perm_addr) &&
-	    memcmp(hw->mac.perm_addr, mac, ETH_ALEN))
+	    !ether_addr_equal(hw->mac.perm_addr, mac))
 		return FM10K_ERR_PARAM;
 
 	/* add bit to notify us if this is a set or clear operation */
@@ -414,6 +442,7 @@ static s32 fm10k_update_xcast_mode_vf(struct fm10k_hw *hw, u16 glort, u8 mode)
 
 	if (mode > FM10K_XCAST_MODE_NONE)
 		return FM10K_ERR_PARAM;
+
 	/* generate message requesting to change xcast mode */
 	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_LPORT_STATE);
 	fm10k_tlv_attr_put_u8(msg, FM10K_LPORT_STATE_MSG_XCAST_MODE, mode);
@@ -533,25 +562,25 @@ static const struct fm10k_msg_data fm10k_msg_data_vf[] = {
 	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
 };
 
-static struct fm10k_mac_ops mac_ops_vf = {
-	.get_bus_info		= &fm10k_get_bus_info_generic,
-	.reset_hw		= &fm10k_reset_hw_vf,
-	.init_hw		= &fm10k_init_hw_vf,
-	.start_hw		= &fm10k_start_hw_generic,
-	.stop_hw		= &fm10k_stop_hw_vf,
-	.update_vlan		= &fm10k_update_vlan_vf,
-	.read_mac_addr		= &fm10k_read_mac_addr_vf,
-	.update_uc_addr		= &fm10k_update_uc_addr_vf,
-	.update_mc_addr		= &fm10k_update_mc_addr_vf,
-	.update_xcast_mode	= &fm10k_update_xcast_mode_vf,
-	.update_int_moderator	= &fm10k_update_int_moderator_vf,
-	.update_lport_state	= &fm10k_update_lport_state_vf,
-	.update_hw_stats	= &fm10k_update_hw_stats_vf,
-	.rebind_hw_stats	= &fm10k_rebind_hw_stats_vf,
-	.configure_dglort_map	= &fm10k_configure_dglort_map_vf,
-	.get_host_state		= &fm10k_get_host_state_generic,
-	.adjust_systime		= &fm10k_adjust_systime_vf,
-	.read_systime		= &fm10k_read_systime_vf,
+static const struct fm10k_mac_ops mac_ops_vf = {
+	.get_bus_info		= fm10k_get_bus_info_generic,
+	.reset_hw		= fm10k_reset_hw_vf,
+	.init_hw		= fm10k_init_hw_vf,
+	.start_hw		= fm10k_start_hw_generic,
+	.stop_hw		= fm10k_stop_hw_vf,
+	.update_vlan		= fm10k_update_vlan_vf,
+	.read_mac_addr		= fm10k_read_mac_addr_vf,
+	.update_uc_addr		= fm10k_update_uc_addr_vf,
+	.update_mc_addr		= fm10k_update_mc_addr_vf,
+	.update_xcast_mode	= fm10k_update_xcast_mode_vf,
+	.update_int_moderator	= fm10k_update_int_moderator_vf,
+	.update_lport_state	= fm10k_update_lport_state_vf,
+	.update_hw_stats	= fm10k_update_hw_stats_vf,
+	.rebind_hw_stats	= fm10k_rebind_hw_stats_vf,
+	.configure_dglort_map	= fm10k_configure_dglort_map_vf,
+	.get_host_state		= fm10k_get_host_state_generic,
+	.adjust_systime		= fm10k_adjust_systime_vf,
+	.read_systime		= fm10k_read_systime_vf,
 };
 
 static s32 fm10k_get_invariants_vf(struct fm10k_hw *hw)
@@ -561,8 +590,8 @@ static s32 fm10k_get_invariants_vf(struct fm10k_hw *hw)
 	return fm10k_pfvf_mbx_init(hw, &hw->mbx, fm10k_msg_data_vf, 0);
 }
 
-struct fm10k_info fm10k_vf_info = {
+const struct fm10k_info fm10k_vf_info = {
 	.mac		= fm10k_mac_vf,
-	.get_invariants	= &fm10k_get_invariants_vf,
+	.get_invariants	= fm10k_get_invariants_vf,
 	.mac_ops	= &mac_ops_vf,
 };
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_vf.h b/drivers/net/ethernet/intel/fm10k/fm10k_vf.h
index 06a99d794c99..c4439f1313a0 100644
--- a/drivers/net/ethernet/intel/fm10k/fm10k_vf.h
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_vf.h
@@ -74,5 +74,5 @@ extern const struct fm10k_tlv_attr fm10k_1588_msg_attr[];
 #define FM10K_VF_MSG_1588_HANDLER(func) \
 	FM10K_MSG_HANDLER(FM10K_VF_MSG_ID_1588, fm10k_1588_msg_attr, func)
 
-extern struct fm10k_info fm10k_vf_info;
+extern const struct fm10k_info fm10k_vf_info;
 #endif /* _FM10K_VF_H */
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index 4dd3e26129b4..68f2204ec6f3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -42,7 +42,6 @@
 #include <linux/string.h>
 #include <linux/in.h>
 #include <linux/ip.h>
-#include <linux/tcp.h>
 #include <linux/sctp.h>
 #include <linux/pkt_sched.h>
 #include <linux/ipv6.h>
@@ -104,6 +103,7 @@
 #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_PS		BIT(4)
 
 #define I40E_NVM_VERSION_LO_SHIFT  0
 #define I40E_NVM_VERSION_LO_MASK   (0xff << I40E_NVM_VERSION_LO_SHIFT)
@@ -187,6 +187,7 @@ struct i40e_lump_tracking {
 #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)
 
 enum i40e_fd_stat_idx {
 	I40E_FD_STAT_ATR,
@@ -244,6 +245,11 @@ struct i40e_tc_configuration {
 	struct i40e_tc_info tc_info[I40E_MAX_TRAFFIC_CLASS];
 };
 
+struct i40e_udp_port_config {
+	__be16 index;
+	u8 type;
+};
+
 /* struct that defines the Ethernet device */
 struct i40e_pf {
 	struct pci_dev *pdev;
@@ -265,7 +271,7 @@ struct i40e_pf {
 	u16 num_lan_qps;           /* num lan queues this PF has set up */
 	u16 num_lan_msix;          /* num queue vectors for the base PF vsi */
 	int queues_left;           /* queues left unclaimed */
-	u16 rss_size;              /* num queues in the RSS array */
+	u16 alloc_rss_size;        /* allocated RSS queues */
 	u16 rss_size_max;          /* HW defined max RSS queues */
 	u16 fdir_pf_filter_count;  /* num of guaranteed filters for this PF */
 	u16 num_alloc_vsi;         /* num VSIs this driver supports */
@@ -280,11 +286,9 @@ struct i40e_pf {
 	u32 fd_atr_cnt;
 	u32 fd_tcp_rule;
 
-#ifdef CONFIG_I40E_VXLAN
-	__be16  vxlan_ports[I40E_MAX_PF_UDP_OFFLOAD_PORTS];
-	u16 pending_vxlan_bitmap;
+	struct i40e_udp_port_config udp_ports[I40E_MAX_PF_UDP_OFFLOAD_PORTS];
+	u16 pending_udp_bitmap;
 
-#endif
 	enum i40e_interrupt_policy int_policy;
 	u16 rx_itr_default;
 	u16 tx_itr_default;
@@ -321,9 +325,7 @@ struct i40e_pf {
 #define I40E_FLAG_FD_ATR_ENABLED		BIT_ULL(22)
 #define I40E_FLAG_PTP				BIT_ULL(25)
 #define I40E_FLAG_MFP_ENABLED			BIT_ULL(26)
-#ifdef CONFIG_I40E_VXLAN
-#define I40E_FLAG_VXLAN_FILTER_SYNC		BIT_ULL(27)
-#endif
+#define I40E_FLAG_UDP_FILTER_SYNC		BIT_ULL(27)
 #define I40E_FLAG_PORT_ID_VALID			BIT_ULL(28)
 #define I40E_FLAG_DCB_CAPABLE			BIT_ULL(29)
 #define I40E_FLAG_RSS_AQ_CAPABLE		BIT_ULL(31)
@@ -335,7 +337,9 @@ struct i40e_pf {
 #define I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE	BIT_ULL(38)
 #define I40E_FLAG_LINK_POLLING_ENABLED		BIT_ULL(39)
 #define I40E_FLAG_VEB_MODE_ENABLED		BIT_ULL(40)
+#define I40E_FLAG_GENEVE_OFFLOAD_CAPABLE	BIT_ULL(41)
 #define I40E_FLAG_NO_PCI_LINK_CHECK		BIT_ULL(42)
+#define I40E_FLAG_PF_MAC			BIT_ULL(50)
 
 	/* tracks features that get auto disabled by errors */
 	u64 auto_disable_flags;
@@ -412,7 +416,7 @@ struct i40e_pf {
 	u32 rx_hwtstamp_cleared;
 	bool ptp_tx;
 	bool ptp_rx;
-	u16 rss_table_size;
+	u16 rss_table_size; /* HW RSS table size */
 	/* These are only valid in NPAR modes */
 	u32 npar_max_bw;
 	u32 npar_min_bw;
@@ -487,6 +491,7 @@ struct i40e_vsi {
 	u32 tx_restart;
 	u32 tx_busy;
 	u64 tx_linearize;
+	u64 tx_force_wb;
 	u32 rx_buf_failed;
 	u32 rx_page_failed;
 
@@ -504,8 +509,10 @@ struct i40e_vsi {
 	u16 tx_itr_setting;
 	u16 int_rate_limit;  /* value in usecs */
 
-	u16 rss_table_size;
-	u16 rss_size;
+	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_hdr_len;
@@ -575,6 +582,9 @@ struct i40e_q_vector {
 
 	u8 num_ringpairs;	/* total number of ring pairs in vector */
 
+#define I40E_Q_VECTOR_HUNG_DETECT 0 /* Bit Index for hung detection logic */
+	unsigned long hung_detected; /* Set/Reset for hung_detection logic */
+
 	cpumask_t affinity_mask;
 	struct rcu_head rcu;	/* to avoid race with update stats on free */
 	char name[I40E_INT_NAME_STR_LEN];
@@ -602,8 +612,8 @@ static inline char *i40e_nvm_version_str(struct i40e_hw *hw)
 
 	full_ver = hw->nvm.oem_ver;
 	ver = (u8)(full_ver >> I40E_OEM_VER_SHIFT);
-	build = (u16)((full_ver >> I40E_OEM_VER_BUILD_SHIFT)
-		 & I40E_OEM_VER_BUILD_MASK);
+	build = (u16)((full_ver >> I40E_OEM_VER_BUILD_SHIFT) &
+		 I40E_OEM_VER_BUILD_MASK);
 	patch = (u8)(full_ver & I40E_OEM_VER_PATCH_MASK);
 
 	snprintf(buf, sizeof(buf),
@@ -668,6 +678,8 @@ extern const char i40e_driver_name[];
 extern const char i40e_driver_version_str[];
 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);
 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);
@@ -691,7 +703,7 @@ struct i40e_mac_filter *i40e_add_filter(struct i40e_vsi *vsi,
 					bool is_vf, bool is_netdev);
 void i40e_del_filter(struct i40e_vsi *vsi, u8 *macaddr, s16 vlan,
 		     bool is_vf, bool is_netdev);
-int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl);
+int i40e_sync_vsi_filters(struct i40e_vsi *vsi);
 struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 				u16 uplink, u32 param1);
 int i40e_vsi_release(struct i40e_vsi *vsi);
@@ -709,7 +721,7 @@ struct i40e_veb *i40e_veb_setup(struct i40e_pf *pf, u16 flags, u16 uplink_seid,
 void i40e_veb_release(struct i40e_veb *veb);
 
 int i40e_veb_config_tc(struct i40e_veb *veb, u8 enabled_tc);
-i40e_status i40e_vsi_add_pvid(struct i40e_vsi *vsi, u16 vid);
+int i40e_vsi_add_pvid(struct i40e_vsi *vsi, u16 vid);
 void i40e_vsi_remove_pvid(struct i40e_vsi *vsi);
 void i40e_vsi_reset_stats(struct i40e_vsi *vsi);
 void i40e_pf_reset_stats(struct i40e_pf *pf);
@@ -767,6 +779,8 @@ int i40e_vsi_add_vlan(struct i40e_vsi *vsi, s16 vid);
 int i40e_vsi_kill_vlan(struct i40e_vsi *vsi, s16 vid);
 struct i40e_mac_filter *i40e_put_mac_in_vlan(struct i40e_vsi *vsi, u8 *macaddr,
 					     bool is_vf, bool is_netdev);
+int i40e_del_mac_all_vlan(struct i40e_vsi *vsi, u8 *macaddr,
+			  bool is_vf, bool is_netdev);
 bool i40e_is_vsi_in_vlan(struct i40e_vsi *vsi);
 struct i40e_mac_filter *i40e_find_mac(struct i40e_vsi *vsi, u8 *macaddr,
 				      bool is_vf, bool is_netdev);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
index 6584b6cd73fd..b22012a446a6 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq_cmd.h
@@ -227,6 +227,7 @@ enum i40e_admin_queue_opc {
 	i40e_aqc_opc_nvm_update			= 0x0703,
 	i40e_aqc_opc_nvm_config_read		= 0x0704,
 	i40e_aqc_opc_nvm_config_write		= 0x0705,
+	i40e_aqc_opc_oem_post_update		= 0x0720,
 
 	/* virtualization commands */
 	i40e_aqc_opc_send_msg_to_pf		= 0x0801,
@@ -1891,6 +1892,26 @@ struct i40e_aqc_nvm_config_data_immediate_field {
 
 I40E_CHECK_STRUCT_LEN(0xc, i40e_aqc_nvm_config_data_immediate_field);
 
+/* OEM Post Update (indirect 0x0720)
+ * no command data struct used
+ */
+struct i40e_aqc_nvm_oem_post_update {
+#define I40E_AQ_NVM_OEM_POST_UPDATE_EXTERNAL_DATA	0x01
+	u8 sel_data;
+	u8 reserved[7];
+};
+
+I40E_CHECK_STRUCT_LEN(0x8, i40e_aqc_nvm_oem_post_update);
+
+struct i40e_aqc_nvm_oem_post_update_buffer {
+	u8 str_len;
+	u8 dev_addr;
+	__le16 eeprom_addr;
+	u8 data[36];
+};
+
+I40E_CHECK_STRUCT_LEN(0x28, i40e_aqc_nvm_oem_post_update_buffer);
+
 /* Send to PF command (indirect 0x0801) id is only used by PF
  * Send to VF command (indirect 0x0802) id is only used by PF
  * Send to Peer PF command (indirect 0x0803)
@@ -2403,4 +2424,4 @@ struct i40e_aqc_debug_modify_internals {
 
 I40E_CHECK_CMD_LENGTH(i40e_aqc_debug_modify_internals);
 
-#endif
+#endif /* _I40E_ADMINQ_CMD_H_ */
diff --git a/drivers/net/ethernet/intel/i40e/i40e_common.c b/drivers/net/ethernet/intel/i40e/i40e_common.c
index 2d74c6e4d7b6..6a034ddac36a 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_common.c
@@ -44,7 +44,6 @@ static i40e_status i40e_set_mac_type(struct i40e_hw *hw)
 		switch (hw->device_id) {
 		case I40E_DEV_ID_SFP_XL710:
 		case I40E_DEV_ID_QEMU:
-		case I40E_DEV_ID_KX_A:
 		case I40E_DEV_ID_KX_B:
 		case I40E_DEV_ID_KX_C:
 		case I40E_DEV_ID_QSFP_A:
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index d4b7af9a2fc8..10744a698d6f 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -103,8 +103,8 @@ static ssize_t i40e_dbg_dump_read(struct file *filp, char __user *buffer,
 	len = min_t(int, count, (i40e_dbg_dump_data_len - *ppos));
 
 	bytes_not_copied = copy_to_user(buffer, &i40e_dbg_dump_buf[*ppos], len);
-	if (bytes_not_copied < 0)
-		return bytes_not_copied;
+	if (bytes_not_copied)
+		return -EFAULT;
 
 	*ppos += len;
 	return len;
@@ -353,8 +353,8 @@ static ssize_t i40e_dbg_command_read(struct file *filp, char __user *buffer,
 	bytes_not_copied = copy_to_user(buffer, buf, len);
 	kfree(buf);
 
-	if (bytes_not_copied < 0)
-		return bytes_not_copied;
+	if (bytes_not_copied)
+		return -EFAULT;
 
 	*ppos = len;
 	return len;
@@ -981,12 +981,10 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 	if (!cmd_buf)
 		return count;
 	bytes_not_copied = copy_from_user(cmd_buf, buffer, count);
-	if (bytes_not_copied < 0) {
+	if (bytes_not_copied) {
 		kfree(cmd_buf);
-		return bytes_not_copied;
+		return -EFAULT;
 	}
-	if (bytes_not_copied > 0)
-		count -= bytes_not_copied;
 	cmd_buf[count] = '\0';
 
 	cmd_buf_tmp = strchr(cmd_buf, '\n');
@@ -1140,7 +1138,7 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 		spin_lock_bh(&vsi->mac_filter_list_lock);
 		f = i40e_add_filter(vsi, ma, vlan, false, false);
 		spin_unlock_bh(&vsi->mac_filter_list_lock);
-		ret = i40e_sync_vsi_filters(vsi, true);
+		ret = i40e_sync_vsi_filters(vsi);
 		if (f && !ret)
 			dev_info(&pf->pdev->dev,
 				 "add macaddr: %pM vlan=%d added to VSI %d\n",
@@ -1179,7 +1177,7 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 		spin_lock_bh(&vsi->mac_filter_list_lock);
 		i40e_del_filter(vsi, ma, vlan, false, false);
 		spin_unlock_bh(&vsi->mac_filter_list_lock);
-		ret = i40e_sync_vsi_filters(vsi, true);
+		ret = i40e_sync_vsi_filters(vsi);
 		if (!ret)
 			dev_info(&pf->pdev->dev,
 				 "del macaddr: %pM vlan=%d removed from VSI %d\n",
@@ -2034,8 +2032,8 @@ static ssize_t i40e_dbg_netdev_ops_read(struct file *filp, char __user *buffer,
 	bytes_not_copied = copy_to_user(buffer, buf, len);
 	kfree(buf);
 
-	if (bytes_not_copied < 0)
-		return bytes_not_copied;
+	if (bytes_not_copied)
+		return -EFAULT;
 
 	*ppos = len;
 	return len;
@@ -2068,10 +2066,8 @@ static ssize_t i40e_dbg_netdev_ops_write(struct file *filp,
 	memset(i40e_dbg_netdev_ops_buf, 0, sizeof(i40e_dbg_netdev_ops_buf));
 	bytes_not_copied = copy_from_user(i40e_dbg_netdev_ops_buf,
 					  buffer, count);
-	if (bytes_not_copied < 0)
-		return bytes_not_copied;
-	else if (bytes_not_copied > 0)
-		count -= bytes_not_copied;
+	if (bytes_not_copied)
+		return -EFAULT;
 	i40e_dbg_netdev_ops_buf[count] = '\0';
 
 	buf_tmp = strchr(i40e_dbg_netdev_ops_buf, '\n');
diff --git a/drivers/net/ethernet/intel/i40e/i40e_devids.h b/drivers/net/ethernet/intel/i40e/i40e_devids.h
index c601ca4a610c..448ef4c17efb 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_devids.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_devids.h
@@ -30,7 +30,6 @@
 /* Device IDs */
 #define I40E_DEV_ID_SFP_XL710		0x1572
 #define I40E_DEV_ID_QEMU		0x1574
-#define I40E_DEV_ID_KX_A		0x157F
 #define I40E_DEV_ID_KX_B		0x1580
 #define I40E_DEV_ID_KX_C		0x1581
 #define I40E_DEV_ID_QSFP_A		0x1583
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index 3f385ffe420f..29d5833e24a3 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -88,6 +88,7 @@ static const struct i40e_stats i40e_gstrings_misc_stats[] = {
 	I40E_VSI_STAT("tx_broadcast", eth_stats.tx_broadcast),
 	I40E_VSI_STAT("rx_unknown_protocol", eth_stats.rx_unknown_protocol),
 	I40E_VSI_STAT("tx_linearize", tx_linearize),
+	I40E_VSI_STAT("tx_force_wb", tx_force_wb),
 };
 
 /* These PF_STATs might look like duplicates of some NETDEV_STATs,
@@ -230,6 +231,7 @@ static const char i40e_priv_flags_strings[][ETH_GSTRING_LEN] = {
 	"LinkPolling",
 	"flow-director-atr",
 	"veb-stats",
+	"packet-split",
 };
 
 #define I40E_PRIV_FLAGS_STR_LEN ARRAY_SIZE(i40e_priv_flags_strings)
@@ -2110,7 +2112,7 @@ static int i40e_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd,
 
 	switch (cmd->cmd) {
 	case ETHTOOL_GRXRINGS:
-		cmd->data = vsi->alloc_queue_pairs;
+		cmd->data = vsi->num_queue_pairs;
 		ret = 0;
 		break;
 	case ETHTOOL_GRXFH:
@@ -2583,7 +2585,6 @@ static int i40e_set_channels(struct net_device *dev,
 		return -EINVAL;
 }
 
-#define I40E_HLUT_ARRAY_SIZE ((I40E_PFQF_HLUT_MAX_INDEX + 1) * 4)
 /**
  * i40e_get_rxfh_key_size - get the RSS hash key size
  * @netdev: network interface device structure
@@ -2611,10 +2612,9 @@ static int i40e_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_vsi *vsi = np->vsi;
-	struct i40e_pf *pf = vsi->back;
-	struct i40e_hw *hw = &pf->hw;
-	u32 reg_val;
-	int i, j;
+	u8 *lut, *seed = NULL;
+	int ret;
+	u16 i;
 
 	if (hfunc)
 		*hfunc = ETH_RSS_HASH_TOP;
@@ -2622,24 +2622,20 @@ static int i40e_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 	if (!indir)
 		return 0;
 
-	for (i = 0, j = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++) {
-		reg_val = rd32(hw, I40E_PFQF_HLUT(i));
-		indir[j++] = reg_val & 0xff;
-		indir[j++] = (reg_val >> 8) & 0xff;
-		indir[j++] = (reg_val >> 16) & 0xff;
-		indir[j++] = (reg_val >> 24) & 0xff;
-	}
+	seed = key;
+	lut = kzalloc(I40E_HLUT_ARRAY_SIZE, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
+	ret = i40e_get_rss(vsi, seed, lut, I40E_HLUT_ARRAY_SIZE);
+	if (ret)
+		goto out;
+	for (i = 0; i < I40E_HLUT_ARRAY_SIZE; i++)
+		indir[i] = (u32)(lut[i]);
 
-	if (key) {
-		for (i = 0, j = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++) {
-			reg_val = rd32(hw, I40E_PFQF_HKEY(i));
-			key[j++] = (u8)(reg_val & 0xff);
-			key[j++] = (u8)((reg_val >> 8) & 0xff);
-			key[j++] = (u8)((reg_val >> 16) & 0xff);
-			key[j++] = (u8)((reg_val >> 24) & 0xff);
-		}
-	}
-	return 0;
+out:
+	kfree(lut);
+
+	return ret;
 }
 
 /**
@@ -2656,10 +2652,8 @@ 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;
-	struct i40e_hw *hw = &pf->hw;
-	u32 reg_val;
-	int i, j;
+	u8 *seed = NULL;
+	u16 i;
 
 	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP)
 		return -EOPNOTSUPP;
@@ -2667,24 +2661,28 @@ static int i40e_set_rxfh(struct net_device *netdev, const u32 *indir,
 	if (!indir)
 		return 0;
 
-	for (i = 0, j = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++) {
-		reg_val = indir[j++];
-		reg_val |= indir[j++] << 8;
-		reg_val |= indir[j++] << 16;
-		reg_val |= indir[j++] << 24;
-		wr32(hw, I40E_PFQF_HLUT(i), reg_val);
-	}
-
 	if (key) {
-		for (i = 0, j = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++) {
-			reg_val = key[j++];
-			reg_val |= key[j++] << 8;
-			reg_val |= key[j++] << 16;
-			reg_val |= key[j++] << 24;
-			wr32(hw, I40E_PFQF_HKEY(i), reg_val);
+		if (!vsi->rss_hkey_user) {
+			vsi->rss_hkey_user = kzalloc(I40E_HKEY_ARRAY_SIZE,
+						     GFP_KERNEL);
+			if (!vsi->rss_hkey_user)
+				return -ENOMEM;
 		}
+		memcpy(vsi->rss_hkey_user, key, I40E_HKEY_ARRAY_SIZE);
+		seed = vsi->rss_hkey_user;
 	}
-	return 0;
+	if (!vsi->rss_lut_user) {
+		vsi->rss_lut_user = kzalloc(I40E_HLUT_ARRAY_SIZE, GFP_KERNEL);
+		if (!vsi->rss_lut_user)
+			return -ENOMEM;
+	}
+
+	/* 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]);
+
+	return i40e_config_rss(vsi, seed, vsi->rss_lut_user,
+			       I40E_HLUT_ARRAY_SIZE);
 }
 
 /**
@@ -2712,6 +2710,8 @@ static u32 i40e_get_priv_flags(struct net_device *dev)
 		I40E_PRIV_FLAGS_FD_ATR : 0;
 	ret_flags |= pf->flags & I40E_FLAG_VEB_STATS_ENABLED ?
 		I40E_PRIV_FLAGS_VEB_STATS : 0;
+	ret_flags |= pf->flags & I40E_FLAG_RX_PS_ENABLED ?
+		I40E_PRIV_FLAGS_PS : 0;
 
 	return ret_flags;
 }
@@ -2726,6 +2726,26 @@ static int i40e_set_priv_flags(struct net_device *dev, u32 flags)
 	struct i40e_netdev_priv *np = netdev_priv(dev);
 	struct i40e_vsi *vsi = np->vsi;
 	struct i40e_pf *pf = vsi->back;
+	bool reset_required = false;
+
+	/* NOTE: MFP is not settable */
+
+	/* allow the user to control the method of receive
+	 * buffer DMA, whether the packet is split at header
+	 * boundaries into two separate buffers.  In some cases
+	 * one routine or the other will perform better.
+	 */
+	if ((flags & I40E_PRIV_FLAGS_PS) &&
+	    !(pf->flags & I40E_FLAG_RX_PS_ENABLED)) {
+		pf->flags |= I40E_FLAG_RX_PS_ENABLED;
+		pf->flags &= ~I40E_FLAG_RX_1BUF_ENABLED;
+		reset_required = true;
+	} else if (!(flags & I40E_PRIV_FLAGS_PS) &&
+		   (pf->flags & I40E_FLAG_RX_PS_ENABLED)) {
+		pf->flags &= ~I40E_FLAG_RX_PS_ENABLED;
+		pf->flags |= I40E_FLAG_RX_1BUF_ENABLED;
+		reset_required = true;
+	}
 
 	if (flags & I40E_PRIV_FLAGS_LINKPOLL_FLAG)
 		pf->flags |= I40E_FLAG_LINK_POLLING_ENABLED;
@@ -2748,6 +2768,10 @@ static int i40e_set_priv_flags(struct net_device *dev, u32 flags)
 	else
 		pf->flags &= ~I40E_FLAG_VEB_STATS_ENABLED;
 
+	/* if needed, issue reset to cause things to take effect */
+	if (reset_required)
+		i40e_do_reset(pf, BIT(__I40E_PF_RESET_REQUESTED));
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
index fe5d9bf3ed6d..579a46ca82df 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_fcoe.c
@@ -1544,8 +1544,6 @@ void i40e_fcoe_vsi_setup(struct i40e_pf *pf)
 	if (!(pf->flags & I40E_FLAG_FCOE_ENABLED))
 		return;
 
-	BUG_ON(!pf->vsi[pf->lan_vsi]);
-
 	for (i = 0; i < pf->num_alloc_vsi; i++) {
 		vsi = pf->vsi[i];
 		if (vsi && vsi->type == I40E_VSI_FCOE) {
diff --git a/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c b/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
index 79ae7beeafe5..daa9204426d4 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_lan_hmc.c
@@ -762,7 +762,7 @@ static void i40e_write_byte(u8 *hmc_bits,
 
 	/* prepare the bits and mask */
 	shift_width = ce_info->lsb % 8;
-	mask = BIT(ce_info->width) - 1;
+	mask = (u8)(BIT(ce_info->width) - 1);
 
 	src_byte = *from;
 	src_byte &= mask;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index 4a9873ec28c7..8f3b53e0dc46 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -24,12 +24,24 @@
  *
  ******************************************************************************/
 
+#include <linux/etherdevice.h>
+#include <linux/of_net.h>
+#include <linux/pci.h>
+
+#ifdef CONFIG_SPARC
+#include <asm/idprom.h>
+#include <asm/prom.h>
+#endif
+
 /* Local includes */
 #include "i40e.h"
 #include "i40e_diag.h"
-#ifdef CONFIG_I40E_VXLAN
+#if IS_ENABLED(CONFIG_VXLAN)
 #include <net/vxlan.h>
 #endif
+#if IS_ENABLED(CONFIG_GENEVE)
+#include <net/geneve.h>
+#endif
 
 const char i40e_driver_name[] = "i40e";
 static const char i40e_driver_string[] =
@@ -38,8 +50,8 @@ static const char i40e_driver_string[] =
 #define DRV_KERN "-k"
 
 #define DRV_VERSION_MAJOR 1
-#define DRV_VERSION_MINOR 3
-#define DRV_VERSION_BUILD 46
+#define DRV_VERSION_MINOR 4
+#define DRV_VERSION_BUILD 8
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD)    DRV_KERN
@@ -55,6 +67,8 @@ 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);
 
@@ -68,7 +82,6 @@ static int i40e_veb_get_bw_info(struct i40e_veb *veb);
 static const struct pci_device_id i40e_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_SFP_XL710), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_QEMU), 0},
-	{PCI_VDEVICE(INTEL, I40E_DEV_ID_KX_A), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_KX_B), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_KX_C), 0},
 	{PCI_VDEVICE(INTEL, I40E_DEV_ID_QSFP_A), 0},
@@ -790,75 +803,6 @@ static void i40e_update_fcoe_stats(struct i40e_vsi *vsi)
 
 #endif
 /**
- * i40e_update_link_xoff_rx - Update XOFF received in link flow control mode
- * @pf: the corresponding PF
- *
- * Update the Rx XOFF counter (PAUSE frames) in link flow control mode
- **/
-static void i40e_update_link_xoff_rx(struct i40e_pf *pf)
-{
-	struct i40e_hw_port_stats *osd = &pf->stats_offsets;
-	struct i40e_hw_port_stats *nsd = &pf->stats;
-	struct i40e_hw *hw = &pf->hw;
-	u64 xoff = 0;
-
-	if ((hw->fc.current_mode != I40E_FC_FULL) &&
-	    (hw->fc.current_mode != I40E_FC_RX_PAUSE))
-		return;
-
-	xoff = nsd->link_xoff_rx;
-	i40e_stat_update32(hw, I40E_GLPRT_LXOFFRXC(hw->port),
-			   pf->stat_offsets_loaded,
-			   &osd->link_xoff_rx, &nsd->link_xoff_rx);
-
-	/* No new LFC xoff rx */
-	if (!(nsd->link_xoff_rx - xoff))
-		return;
-
-}
-
-/**
- * i40e_update_prio_xoff_rx - Update XOFF received in PFC mode
- * @pf: the corresponding PF
- *
- * Update the Rx XOFF counter (PAUSE frames) in PFC mode
- **/
-static void i40e_update_prio_xoff_rx(struct i40e_pf *pf)
-{
-	struct i40e_hw_port_stats *osd = &pf->stats_offsets;
-	struct i40e_hw_port_stats *nsd = &pf->stats;
-	bool xoff[I40E_MAX_TRAFFIC_CLASS] = {false};
-	struct i40e_dcbx_config *dcb_cfg;
-	struct i40e_hw *hw = &pf->hw;
-	u16 i;
-	u8 tc;
-
-	dcb_cfg = &hw->local_dcbx_config;
-
-	/* Collect Link XOFF stats when PFC is disabled */
-	if (!dcb_cfg->pfc.pfcenable) {
-		i40e_update_link_xoff_rx(pf);
-		return;
-	}
-
-	for (i = 0; i < I40E_MAX_USER_PRIORITY; i++) {
-		u64 prio_xoff = nsd->priority_xoff_rx[i];
-
-		i40e_stat_update32(hw, I40E_GLPRT_PXOFFRXC(hw->port, i),
-				   pf->stat_offsets_loaded,
-				   &osd->priority_xoff_rx[i],
-				   &nsd->priority_xoff_rx[i]);
-
-		/* No new PFC xoff rx */
-		if (!(nsd->priority_xoff_rx[i] - prio_xoff))
-			continue;
-		/* Get the TC for given priority */
-		tc = dcb_cfg->etscfg.prioritytable[i];
-		xoff[tc] = true;
-	}
-}
-
-/**
  * i40e_update_vsi_stats - Update the vsi statistics counters.
  * @vsi: the VSI to be updated
  *
@@ -881,6 +825,7 @@ static void i40e_update_vsi_stats(struct i40e_vsi *vsi)
 	u64 bytes, packets;
 	unsigned int start;
 	u64 tx_linearize;
+	u64 tx_force_wb;
 	u64 rx_p, rx_b;
 	u64 tx_p, tx_b;
 	u16 q;
@@ -899,7 +844,7 @@ static void i40e_update_vsi_stats(struct i40e_vsi *vsi)
 	 */
 	rx_b = rx_p = 0;
 	tx_b = tx_p = 0;
-	tx_restart = tx_busy = tx_linearize = 0;
+	tx_restart = tx_busy = tx_linearize = tx_force_wb = 0;
 	rx_page = 0;
 	rx_buf = 0;
 	rcu_read_lock();
@@ -917,6 +862,7 @@ static void i40e_update_vsi_stats(struct i40e_vsi *vsi)
 		tx_restart += p->tx_stats.restart_queue;
 		tx_busy += p->tx_stats.tx_busy;
 		tx_linearize += p->tx_stats.tx_linearize;
+		tx_force_wb += p->tx_stats.tx_force_wb;
 
 		/* Rx queue is part of the same block as Tx queue */
 		p = &p[1];
@@ -934,6 +880,7 @@ static void i40e_update_vsi_stats(struct i40e_vsi *vsi)
 	vsi->tx_restart = tx_restart;
 	vsi->tx_busy = tx_busy;
 	vsi->tx_linearize = tx_linearize;
+	vsi->tx_force_wb = tx_force_wb;
 	vsi->rx_page_failed = rx_page;
 	vsi->rx_buf_failed = rx_buf;
 
@@ -1049,12 +996,18 @@ static void i40e_update_pf_stats(struct i40e_pf *pf)
 	i40e_stat_update32(hw, I40E_GLPRT_LXONTXC(hw->port),
 			   pf->stat_offsets_loaded,
 			   &osd->link_xon_tx, &nsd->link_xon_tx);
-	i40e_update_prio_xoff_rx(pf);  /* handles I40E_GLPRT_LXOFFRXC */
+	i40e_stat_update32(hw, I40E_GLPRT_LXOFFRXC(hw->port),
+			   pf->stat_offsets_loaded,
+			   &osd->link_xoff_rx, &nsd->link_xoff_rx);
 	i40e_stat_update32(hw, I40E_GLPRT_LXOFFTXC(hw->port),
 			   pf->stat_offsets_loaded,
 			   &osd->link_xoff_tx, &nsd->link_xoff_tx);
 
 	for (i = 0; i < 8; i++) {
+		i40e_stat_update32(hw, I40E_GLPRT_PXOFFRXC(hw->port, i),
+				   pf->stat_offsets_loaded,
+				   &osd->priority_xoff_rx[i],
+				   &nsd->priority_xoff_rx[i]);
 		i40e_stat_update32(hw, I40E_GLPRT_PXONRXC(hw->port, i),
 				   pf->stat_offsets_loaded,
 				   &osd->priority_xon_rx[i],
@@ -1317,6 +1270,42 @@ struct i40e_mac_filter *i40e_put_mac_in_vlan(struct i40e_vsi *vsi, u8 *macaddr,
 }
 
 /**
+ * i40e_del_mac_all_vlan - Remove a MAC filter from all VLANS
+ * @vsi: the VSI to be searched
+ * @macaddr: the mac address to be removed
+ * @is_vf: true if it is a VF
+ * @is_netdev: true if it is a netdev
+ *
+ * Removes a given MAC address from a VSI, regardless of VLAN
+ *
+ * Returns 0 for success, or error
+ **/
+int i40e_del_mac_all_vlan(struct i40e_vsi *vsi, u8 *macaddr,
+			  bool is_vf, bool is_netdev)
+{
+	struct i40e_mac_filter *f = NULL;
+	int changed = 0;
+
+	WARN(!spin_is_locked(&vsi->mac_filter_list_lock),
+	     "Missing mac_filter_list_lock\n");
+	list_for_each_entry(f, &vsi->mac_filter_list, list) {
+		if ((ether_addr_equal(macaddr, f->macaddr)) &&
+		    (is_vf == f->is_vf) &&
+		    (is_netdev == f->is_netdev)) {
+			f->counter--;
+			f->changed = true;
+			changed = 1;
+		}
+	}
+	if (changed) {
+		vsi->flags |= I40E_VSI_FLAG_FILTER_CHANGED;
+		vsi->back->flags |= I40E_FLAG_FILTER_SYNC;
+		return 0;
+	}
+	return -ENOENT;
+}
+
+/**
  * i40e_rm_default_mac_filter - Remove the default MAC filter set by NVM
  * @vsi: the PF Main VSI - inappropriate for any other VSI
  * @macaddr: the MAC address
@@ -1547,10 +1536,9 @@ static int i40e_set_mac(struct net_device *netdev, void *p)
 		spin_unlock_bh(&vsi->mac_filter_list_lock);
 	}
 
-	i40e_sync_vsi_filters(vsi, false);
 	ether_addr_copy(netdev->dev_addr, addr->sa_data);
 
-	return 0;
+	return i40e_sync_vsi_filters(vsi);
 }
 
 /**
@@ -1590,7 +1578,7 @@ static void i40e_vsi_setup_queue_map(struct i40e_vsi *vsi,
 	if (enabled_tc && (vsi->back->flags & I40E_FLAG_DCB_ENABLED)) {
 		/* Find numtc from enabled TC bitmap */
 		for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
-			if (enabled_tc & BIT_ULL(i)) /* TC is enabled */
+			if (enabled_tc & BIT(i)) /* TC is enabled */
 				numtc++;
 		}
 		if (!numtc) {
@@ -1619,13 +1607,14 @@ static void i40e_vsi_setup_queue_map(struct i40e_vsi *vsi,
 	/* Setup queue offset/count for all TCs for given VSI */
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
 		/* See if the given TC is enabled for the given VSI */
-		if (vsi->tc_config.enabled_tc & BIT_ULL(i)) {
+		if (vsi->tc_config.enabled_tc & BIT(i)) {
 			/* TC is enabled */
 			int pow, num_qps;
 
 			switch (vsi->type) {
 			case I40E_VSI_MAIN:
-				qcount = min_t(int, pf->rss_size, num_tc_qps);
+				qcount = min_t(int, pf->alloc_rss_size,
+					       num_tc_qps);
 				break;
 #ifdef I40E_FCOE
 			case I40E_VSI_FCOE:
@@ -1851,13 +1840,12 @@ static void i40e_cleanup_add_list(struct list_head *add_list)
 /**
  * i40e_sync_vsi_filters - Update the VSI filter list to the HW
  * @vsi: ptr to the VSI
- * @grab_rtnl: whether RTNL needs to be grabbed
  *
  * Push any outstanding VSI filter changes through the AdminQ.
  *
  * Returns 0 or error value
  **/
-int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
+int i40e_sync_vsi_filters(struct i40e_vsi *vsi)
 {
 	struct list_head tmp_del_list, tmp_add_list;
 	struct i40e_mac_filter *f, *ftmp, *fclone;
@@ -1865,8 +1853,9 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 	bool add_happened = false;
 	int filter_list_len = 0;
 	u32 changed_flags = 0;
+	i40e_status aq_ret = 0;
 	bool err_cond = false;
-	i40e_status ret = 0;
+	int retval = 0;
 	struct i40e_pf *pf;
 	int num_add = 0;
 	int num_del = 0;
@@ -1929,17 +1918,22 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 		}
 		spin_unlock_bh(&vsi->mac_filter_list_lock);
 
-		if (err_cond)
+		if (err_cond) {
 			i40e_cleanup_add_list(&tmp_add_list);
+			retval = -ENOMEM;
+			goto out;
+		}
 	}
 
 	/* Now process 'del_list' outside the lock */
 	if (!list_empty(&tmp_del_list)) {
+		int del_list_size;
+
 		filter_list_len = pf->hw.aq.asq_buf_size /
 			    sizeof(struct i40e_aqc_remove_macvlan_element_data);
-		del_list = kcalloc(filter_list_len,
-			    sizeof(struct i40e_aqc_remove_macvlan_element_data),
-			    GFP_KERNEL);
+		del_list_size = filter_list_len *
+			    sizeof(struct i40e_aqc_remove_macvlan_element_data);
+		del_list = kzalloc(del_list_size, GFP_KERNEL);
 		if (!del_list) {
 			i40e_cleanup_add_list(&tmp_add_list);
 
@@ -1948,7 +1942,8 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 			i40e_undo_del_filter_entries(vsi, &tmp_del_list);
 			i40e_undo_add_filter_entries(vsi);
 			spin_unlock_bh(&vsi->mac_filter_list_lock);
-			return -ENOMEM;
+			retval = -ENOMEM;
+			goto out;
 		}
 
 		list_for_each_entry_safe(f, ftmp, &tmp_del_list, list) {
@@ -1966,18 +1961,22 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 
 			/* flush a full buffer */
 			if (num_del == filter_list_len) {
-				ret = i40e_aq_remove_macvlan(&pf->hw,
-						  vsi->seid, del_list, num_del,
-						  NULL);
+				aq_ret = i40e_aq_remove_macvlan(&pf->hw,
+								vsi->seid,
+								del_list,
+								num_del,
+								NULL);
 				aq_err = pf->hw.aq.asq_last_status;
 				num_del = 0;
-				memset(del_list, 0, sizeof(*del_list));
+				memset(del_list, 0, del_list_size);
 
-				if (ret && aq_err != I40E_AQ_RC_ENOENT)
+				if (aq_ret && aq_err != I40E_AQ_RC_ENOENT) {
+					retval = -EIO;
 					dev_err(&pf->pdev->dev,
 						"ignoring delete macvlan error, err %s, aq_err %s while flushing a full buffer\n",
-						i40e_stat_str(&pf->hw, ret),
+						i40e_stat_str(&pf->hw, aq_ret),
 						i40e_aq_str(&pf->hw, aq_err));
+				}
 			}
 			/* Release memory for MAC filter entries which were
 			 * synced up with HW.
@@ -1987,15 +1986,16 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 		}
 
 		if (num_del) {
-			ret = i40e_aq_remove_macvlan(&pf->hw, vsi->seid,
-						     del_list, num_del, NULL);
+			aq_ret = i40e_aq_remove_macvlan(&pf->hw, vsi->seid,
+							del_list, num_del,
+							NULL);
 			aq_err = pf->hw.aq.asq_last_status;
 			num_del = 0;
 
-			if (ret && aq_err != I40E_AQ_RC_ENOENT)
+			if (aq_ret && aq_err != I40E_AQ_RC_ENOENT)
 				dev_info(&pf->pdev->dev,
 					 "ignoring delete macvlan error, err %s aq_err %s\n",
-					 i40e_stat_str(&pf->hw, ret),
+					 i40e_stat_str(&pf->hw, aq_ret),
 					 i40e_aq_str(&pf->hw, aq_err));
 		}
 
@@ -2004,13 +2004,14 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 	}
 
 	if (!list_empty(&tmp_add_list)) {
+		int add_list_size;
 
 		/* do all the adds now */
 		filter_list_len = pf->hw.aq.asq_buf_size /
 			       sizeof(struct i40e_aqc_add_macvlan_element_data),
-		add_list = kcalloc(filter_list_len,
-			       sizeof(struct i40e_aqc_add_macvlan_element_data),
-			       GFP_KERNEL);
+		add_list_size = filter_list_len *
+			       sizeof(struct i40e_aqc_add_macvlan_element_data);
+		add_list = kzalloc(add_list_size, GFP_KERNEL);
 		if (!add_list) {
 			/* Purge element from temporary lists */
 			i40e_cleanup_add_list(&tmp_add_list);
@@ -2019,7 +2020,8 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 			spin_lock_bh(&vsi->mac_filter_list_lock);
 			i40e_undo_add_filter_entries(vsi);
 			spin_unlock_bh(&vsi->mac_filter_list_lock);
-			return -ENOMEM;
+			retval = -ENOMEM;
+			goto out;
 		}
 
 		list_for_each_entry_safe(f, ftmp, &tmp_add_list, list) {
@@ -2040,15 +2042,15 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 
 			/* flush a full buffer */
 			if (num_add == filter_list_len) {
-				ret = i40e_aq_add_macvlan(&pf->hw, vsi->seid,
-							  add_list, num_add,
-							  NULL);
+				aq_ret = i40e_aq_add_macvlan(&pf->hw, vsi->seid,
+							     add_list, num_add,
+							     NULL);
 				aq_err = pf->hw.aq.asq_last_status;
 				num_add = 0;
 
-				if (ret)
+				if (aq_ret)
 					break;
-				memset(add_list, 0, sizeof(*add_list));
+				memset(add_list, 0, add_list_size);
 			}
 			/* Entries from tmp_add_list were cloned from MAC
 			 * filter list, hence clean those cloned entries
@@ -2058,18 +2060,19 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 		}
 
 		if (num_add) {
-			ret = i40e_aq_add_macvlan(&pf->hw, vsi->seid,
-						  add_list, num_add, NULL);
+			aq_ret = i40e_aq_add_macvlan(&pf->hw, vsi->seid,
+						     add_list, num_add, NULL);
 			aq_err = pf->hw.aq.asq_last_status;
 			num_add = 0;
 		}
 		kfree(add_list);
 		add_list = NULL;
 
-		if (add_happened && ret && aq_err != I40E_AQ_RC_EINVAL) {
+		if (add_happened && aq_ret && aq_err != I40E_AQ_RC_EINVAL) {
+			retval = i40e_aq_rc_to_posix(aq_ret, aq_err);
 			dev_info(&pf->pdev->dev,
 				 "add filter failed, err %s aq_err %s\n",
-				 i40e_stat_str(&pf->hw, ret),
+				 i40e_stat_str(&pf->hw, aq_ret),
 				 i40e_aq_str(&pf->hw, aq_err));
 			if ((pf->hw.aq.asq_last_status == I40E_AQ_RC_ENOSPC) &&
 			    !test_bit(__I40E_FILTER_OVERFLOW_PROMISC,
@@ -2087,16 +2090,19 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 		bool cur_multipromisc;
 
 		cur_multipromisc = !!(vsi->current_netdev_flags & IFF_ALLMULTI);
-		ret = i40e_aq_set_vsi_multicast_promiscuous(&vsi->back->hw,
-							    vsi->seid,
-							    cur_multipromisc,
-							    NULL);
-		if (ret)
+		aq_ret = i40e_aq_set_vsi_multicast_promiscuous(&vsi->back->hw,
+							       vsi->seid,
+							       cur_multipromisc,
+							       NULL);
+		if (aq_ret) {
+			retval = i40e_aq_rc_to_posix(aq_ret,
+						     pf->hw.aq.asq_last_status);
 			dev_info(&pf->pdev->dev,
 				 "set multi promisc failed, err %s aq_err %s\n",
-				 i40e_stat_str(&pf->hw, ret),
+				 i40e_stat_str(&pf->hw, aq_ret),
 				 i40e_aq_str(&pf->hw,
 					     pf->hw.aq.asq_last_status));
+		}
 	}
 	if ((changed_flags & IFF_PROMISC) || promisc_forced_on) {
 		bool cur_promisc;
@@ -2112,44 +2118,50 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi, bool grab_rtnl)
 			 */
 			if (pf->cur_promisc != cur_promisc) {
 				pf->cur_promisc = cur_promisc;
-				if (grab_rtnl)
-					i40e_do_reset_safe(pf,
-						BIT(__I40E_PF_RESET_REQUESTED));
-				else
-					i40e_do_reset(pf,
-						BIT(__I40E_PF_RESET_REQUESTED));
+				set_bit(__I40E_PF_RESET_REQUESTED, &pf->state);
 			}
 		} else {
-			ret = i40e_aq_set_vsi_unicast_promiscuous(
+			aq_ret = i40e_aq_set_vsi_unicast_promiscuous(
 							  &vsi->back->hw,
 							  vsi->seid,
 							  cur_promisc, NULL);
-			if (ret)
+			if (aq_ret) {
+				retval =
+				i40e_aq_rc_to_posix(aq_ret,
+						    pf->hw.aq.asq_last_status);
 				dev_info(&pf->pdev->dev,
 					 "set unicast promisc failed, err %d, aq_err %d\n",
-					 ret, pf->hw.aq.asq_last_status);
-			ret = i40e_aq_set_vsi_multicast_promiscuous(
+					 aq_ret, pf->hw.aq.asq_last_status);
+			}
+			aq_ret = i40e_aq_set_vsi_multicast_promiscuous(
 							  &vsi->back->hw,
 							  vsi->seid,
 							  cur_promisc, NULL);
-			if (ret)
+			if (aq_ret) {
+				retval =
+				i40e_aq_rc_to_posix(aq_ret,
+						    pf->hw.aq.asq_last_status);
 				dev_info(&pf->pdev->dev,
 					 "set multicast promisc failed, err %d, aq_err %d\n",
-					 ret, pf->hw.aq.asq_last_status);
+					 aq_ret, pf->hw.aq.asq_last_status);
+			}
 		}
-		ret = i40e_aq_set_vsi_broadcast(&vsi->back->hw,
-						vsi->seid,
-						cur_promisc, NULL);
-		if (ret)
+		aq_ret = i40e_aq_set_vsi_broadcast(&vsi->back->hw,
+						   vsi->seid,
+						   cur_promisc, NULL);
+		if (aq_ret) {
+			retval = i40e_aq_rc_to_posix(aq_ret,
+						     pf->hw.aq.asq_last_status);
 			dev_info(&pf->pdev->dev,
 				 "set brdcast promisc failed, err %s, aq_err %s\n",
-				 i40e_stat_str(&pf->hw, ret),
+				 i40e_stat_str(&pf->hw, aq_ret),
 				 i40e_aq_str(&pf->hw,
 					     pf->hw.aq.asq_last_status));
+		}
 	}
-
+out:
 	clear_bit(__I40E_CONFIG_BUSY, &vsi->state);
-	return 0;
+	return retval;
 }
 
 /**
@@ -2166,8 +2178,15 @@ static void i40e_sync_filters_subtask(struct i40e_pf *pf)
 
 	for (v = 0; v < pf->num_alloc_vsi; v++) {
 		if (pf->vsi[v] &&
-		    (pf->vsi[v]->flags & I40E_VSI_FLAG_FILTER_CHANGED))
-			i40e_sync_vsi_filters(pf->vsi[v], true);
+		    (pf->vsi[v]->flags & I40E_VSI_FLAG_FILTER_CHANGED)) {
+			int ret = i40e_sync_vsi_filters(pf->vsi[v]);
+
+			if (ret) {
+				/* come back and try again later */
+				pf->flags |= I40E_FLAG_FILTER_SYNC;
+				break;
+			}
+		}
 	}
 }
 
@@ -2377,16 +2396,13 @@ int i40e_vsi_add_vlan(struct i40e_vsi *vsi, s16 vid)
 		}
 	}
 
-	/* Make sure to release before sync_vsi_filter because that
-	 * function will lock/unlock as necessary
-	 */
 	spin_unlock_bh(&vsi->mac_filter_list_lock);
 
-	if (test_bit(__I40E_DOWN, &vsi->back->state) ||
-	    test_bit(__I40E_RESET_RECOVERY_PENDING, &vsi->back->state))
-		return 0;
-
-	return i40e_sync_vsi_filters(vsi, false);
+	/* schedule our worker thread which will take care of
+	 * applying the new filter changes
+	 */
+	i40e_service_event_schedule(vsi->back);
+	return 0;
 }
 
 /**
@@ -2459,16 +2475,13 @@ int i40e_vsi_kill_vlan(struct i40e_vsi *vsi, s16 vid)
 		}
 	}
 
-	/* Make sure to release before sync_vsi_filter because that
-	 * function with lock/unlock as necessary
-	 */
 	spin_unlock_bh(&vsi->mac_filter_list_lock);
 
-	if (test_bit(__I40E_DOWN, &vsi->back->state) ||
-	    test_bit(__I40E_RESET_RECOVERY_PENDING, &vsi->back->state))
-		return 0;
-
-	return i40e_sync_vsi_filters(vsi, false);
+	/* schedule our worker thread which will take care of
+	 * applying the new filter changes
+	 */
+	i40e_service_event_schedule(vsi->back);
+	return 0;
 }
 
 /**
@@ -2711,6 +2724,11 @@ static void i40e_config_xps_tx_ring(struct i40e_ring *ring)
 		netif_set_xps_queue(ring->netdev, mask, ring->queue_index);
 		free_cpumask_var(mask);
 	}
+
+	/* schedule our worker thread which will take care of
+	 * applying the new filter changes
+	 */
+	i40e_service_event_schedule(vsi->back);
 }
 
 /**
@@ -4360,17 +4378,41 @@ static void i40e_detect_recover_hung_queue(int q_idx, struct i40e_vsi *vsi)
 	else
 		val = rd32(&pf->hw, I40E_PFINT_DYN_CTL0);
 
+	/* Bail out if interrupts are disabled because napi_poll
+	 * execution in-progress or will get scheduled soon.
+	 * napi_poll cleans TX and RX queues and updates 'next_to_clean'.
+	 */
+	if (!(val & I40E_PFINT_DYN_CTLN_INTENA_MASK))
+		return;
+
 	head = i40e_get_head(tx_ring);
 
 	tx_pending = i40e_get_tx_pending(tx_ring);
 
-	/* Interrupts are disabled and TX pending is non-zero,
-	 * trigger the SW interrupt (don't wait). Worst case
-	 * there will be one extra interrupt which may result
-	 * into not cleaning any queues because queues are cleaned.
+	/* HW is done executing descriptors, updated HEAD write back,
+	 * but SW hasn't processed those descriptors. If interrupt is
+	 * not generated from this point ON, it could result into
+	 * dev_watchdog detecting timeout on those netdev_queue,
+	 * hence proactively trigger SW interrupt.
 	 */
-	if (tx_pending && (!(val & I40E_PFINT_DYN_CTLN_INTENA_MASK)))
-		i40e_force_wb(vsi, tx_ring->q_vector);
+	if (tx_pending) {
+		/* NAPI Poll didn't run and clear since it was set */
+		if (test_and_clear_bit(I40E_Q_VECTOR_HUNG_DETECT,
+				       &tx_ring->q_vector->hung_detected)) {
+			netdev_info(vsi->netdev, "VSI_seid %d, Hung TX queue %d, tx_pending: %d, NTC:0x%x, HWB: 0x%x, NTU: 0x%x, TAIL: 0x%x\n",
+				    vsi->seid, q_idx, tx_pending,
+				    tx_ring->next_to_clean, head,
+				    tx_ring->next_to_use,
+				    readl(tx_ring->tail));
+			netdev_info(vsi->netdev, "VSI_seid %d, Issuing force_wb for TX queue %d, Interrupt Reg: 0x%x\n",
+				    vsi->seid, q_idx, val);
+			i40e_force_wb(vsi, tx_ring->q_vector);
+		} else {
+			/* First Chance - detected possible hung */
+			set_bit(I40E_Q_VECTOR_HUNG_DETECT,
+				&tx_ring->q_vector->hung_detected);
+		}
+	}
 }
 
 /**
@@ -4441,7 +4483,7 @@ static u8 i40e_get_iscsi_tc_map(struct i40e_pf *pf)
 		if (app.selector == I40E_APP_SEL_TCPIP &&
 		    app.protocolid == I40E_APP_PROTOID_ISCSI) {
 			tc = dcbcfg->etscfg.prioritytable[app.priority];
-			enabled_tc |= BIT_ULL(tc);
+			enabled_tc |= BIT(tc);
 			break;
 		}
 	}
@@ -4525,7 +4567,7 @@ static u8 i40e_pf_get_num_tc(struct i40e_pf *pf)
 	/* At least have TC0 */
 	enabled_tc = (enabled_tc ? enabled_tc : 0x1);
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
-		if (enabled_tc & BIT_ULL(i))
+		if (enabled_tc & BIT(i))
 			num_tc++;
 	}
 	return num_tc;
@@ -4547,7 +4589,7 @@ static u8 i40e_pf_get_default_tc(struct i40e_pf *pf)
 
 	/* Find the first enabled TC */
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
-		if (enabled_tc & BIT_ULL(i))
+		if (enabled_tc & BIT(i))
 			break;
 	}
 
@@ -4707,7 +4749,7 @@ static void i40e_vsi_config_netdev_tc(struct i40e_vsi *vsi, u8 enabled_tc)
 		 * will set the numtc for netdev as 2 that will be
 		 * referenced by the netdev layer as TC 0 and 1.
 		 */
-		if (vsi->tc_config.enabled_tc & BIT_ULL(i))
+		if (vsi->tc_config.enabled_tc & BIT(i))
 			netdev_set_tc_queue(netdev,
 					vsi->tc_config.tc_info[i].netdev_tc,
 					vsi->tc_config.tc_info[i].qcount,
@@ -4769,7 +4811,7 @@ static int i40e_vsi_config_tc(struct i40e_vsi *vsi, u8 enabled_tc)
 
 	/* Enable ETS TCs with equal BW Share for now across all VSIs */
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
-		if (enabled_tc & BIT_ULL(i))
+		if (enabled_tc & BIT(i))
 			bw_share[i] = 1;
 	}
 
@@ -4843,7 +4885,7 @@ int i40e_veb_config_tc(struct i40e_veb *veb, u8 enabled_tc)
 
 	/* Enable ETS TCs with equal BW Share for now */
 	for (i = 0; i < I40E_MAX_TRAFFIC_CLASS; i++) {
-		if (enabled_tc & BIT_ULL(i))
+		if (enabled_tc & BIT(i))
 			bw_data.tc_bw_share_credits[i] = 1;
 	}
 
@@ -5240,7 +5282,7 @@ static int i40e_setup_tc(struct net_device *netdev, u8 tc)
 
 	/* Generate TC map for number of tc requested */
 	for (i = 0; i < tc; i++)
-		enabled_tc |= BIT_ULL(i);
+		enabled_tc |= BIT(i);
 
 	/* Requesting same TC configuration as already enabled */
 	if (enabled_tc == vsi->tc_config.enabled_tc)
@@ -5305,6 +5347,9 @@ int i40e_open(struct net_device *netdev)
 #ifdef CONFIG_I40E_VXLAN
 	vxlan_get_rx_port(netdev);
 #endif
+#ifdef CONFIG_I40E_GENEVE
+	geneve_get_rx_port(netdev);
+#endif
 
 	return 0;
 }
@@ -5738,7 +5783,7 @@ static void i40e_handle_lan_overflow_event(struct i40e_pf *pf,
  **/
 static void i40e_service_event_complete(struct i40e_pf *pf)
 {
-	BUG_ON(!test_bit(__I40E_SERVICE_SCHED, &pf->state));
+	WARN_ON(!test_bit(__I40E_SERVICE_SCHED, &pf->state));
 
 	/* flush memory to make sure state is correct before next watchog */
 	smp_mb__before_atomic();
@@ -6013,6 +6058,9 @@ static void i40e_link_event(struct i40e_pf *pf)
 	i40e_status status;
 	bool new_link, old_link;
 
+	/* save off old link status information */
+	pf->hw.phy.link_info_old = pf->hw.phy.link_info;
+
 	/* set this to force the get_link_status call to refresh state */
 	pf->hw.phy.get_link_info = true;
 
@@ -6101,23 +6149,23 @@ static void i40e_reset_subtask(struct i40e_pf *pf)
 
 	rtnl_lock();
 	if (test_bit(__I40E_REINIT_REQUESTED, &pf->state)) {
-		reset_flags |= BIT_ULL(__I40E_REINIT_REQUESTED);
+		reset_flags |= BIT(__I40E_REINIT_REQUESTED);
 		clear_bit(__I40E_REINIT_REQUESTED, &pf->state);
 	}
 	if (test_bit(__I40E_PF_RESET_REQUESTED, &pf->state)) {
-		reset_flags |= BIT_ULL(__I40E_PF_RESET_REQUESTED);
+		reset_flags |= BIT(__I40E_PF_RESET_REQUESTED);
 		clear_bit(__I40E_PF_RESET_REQUESTED, &pf->state);
 	}
 	if (test_bit(__I40E_CORE_RESET_REQUESTED, &pf->state)) {
-		reset_flags |= BIT_ULL(__I40E_CORE_RESET_REQUESTED);
+		reset_flags |= BIT(__I40E_CORE_RESET_REQUESTED);
 		clear_bit(__I40E_CORE_RESET_REQUESTED, &pf->state);
 	}
 	if (test_bit(__I40E_GLOBAL_RESET_REQUESTED, &pf->state)) {
-		reset_flags |= BIT_ULL(__I40E_GLOBAL_RESET_REQUESTED);
+		reset_flags |= BIT(__I40E_GLOBAL_RESET_REQUESTED);
 		clear_bit(__I40E_GLOBAL_RESET_REQUESTED, &pf->state);
 	}
 	if (test_bit(__I40E_DOWN_REQUESTED, &pf->state)) {
-		reset_flags |= BIT_ULL(__I40E_DOWN_REQUESTED);
+		reset_flags |= BIT(__I40E_DOWN_REQUESTED);
 		clear_bit(__I40E_DOWN_REQUESTED, &pf->state);
 	}
 
@@ -6147,13 +6195,9 @@ unlock:
 static void i40e_handle_link_event(struct i40e_pf *pf,
 				   struct i40e_arq_event_info *e)
 {
-	struct i40e_hw *hw = &pf->hw;
 	struct i40e_aqc_get_link_status *status =
 		(struct i40e_aqc_get_link_status *)&e->desc.params.raw;
 
-	/* save off old link status information */
-	hw->phy.link_info_old = hw->phy.link_info;
-
 	/* Do a new status request to re-enable LSE reporting
 	 * and load new status information into the hw struct
 	 * This completely ignores any state information
@@ -6192,15 +6236,18 @@ static void i40e_clean_adminq_subtask(struct i40e_pf *pf)
 	val = rd32(&pf->hw, pf->hw.aq.arq.len);
 	oldval = val;
 	if (val & I40E_PF_ARQLEN_ARQVFE_MASK) {
-		dev_info(&pf->pdev->dev, "ARQ VF Error detected\n");
+		if (hw->debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ARQ VF Error detected\n");
 		val &= ~I40E_PF_ARQLEN_ARQVFE_MASK;
 	}
 	if (val & I40E_PF_ARQLEN_ARQOVFL_MASK) {
-		dev_info(&pf->pdev->dev, "ARQ Overflow Error detected\n");
+		if (hw->debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ARQ Overflow Error detected\n");
 		val &= ~I40E_PF_ARQLEN_ARQOVFL_MASK;
 	}
 	if (val & I40E_PF_ARQLEN_ARQCRIT_MASK) {
-		dev_info(&pf->pdev->dev, "ARQ Critical Error detected\n");
+		if (hw->debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ARQ Critical Error detected\n");
 		val &= ~I40E_PF_ARQLEN_ARQCRIT_MASK;
 	}
 	if (oldval != val)
@@ -6209,15 +6256,18 @@ static void i40e_clean_adminq_subtask(struct i40e_pf *pf)
 	val = rd32(&pf->hw, pf->hw.aq.asq.len);
 	oldval = val;
 	if (val & I40E_PF_ATQLEN_ATQVFE_MASK) {
-		dev_info(&pf->pdev->dev, "ASQ VF Error detected\n");
+		if (pf->hw.debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ASQ VF Error detected\n");
 		val &= ~I40E_PF_ATQLEN_ATQVFE_MASK;
 	}
 	if (val & I40E_PF_ATQLEN_ATQOVFL_MASK) {
-		dev_info(&pf->pdev->dev, "ASQ Overflow Error detected\n");
+		if (pf->hw.debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ASQ Overflow Error detected\n");
 		val &= ~I40E_PF_ATQLEN_ATQOVFL_MASK;
 	}
 	if (val & I40E_PF_ATQLEN_ATQCRIT_MASK) {
-		dev_info(&pf->pdev->dev, "ASQ Critical Error detected\n");
+		if (pf->hw.debug_mask & I40E_DEBUG_AQ)
+			dev_info(&pf->pdev->dev, "ASQ Critical Error detected\n");
 		val &= ~I40E_PF_ATQLEN_ATQCRIT_MASK;
 	}
 	if (oldval != val)
@@ -6268,6 +6318,7 @@ static void i40e_clean_adminq_subtask(struct i40e_pf *pf)
 			break;
 		case i40e_aqc_opc_nvm_erase:
 		case i40e_aqc_opc_nvm_update:
+		case i40e_aqc_opc_oem_post_update:
 			i40e_debug(&pf->hw, I40E_DEBUG_NVM, "ARQ NVM operation completed\n");
 			break;
 		default:
@@ -6685,6 +6736,7 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 	struct i40e_hw *hw = &pf->hw;
 	u8 set_fc_aq_fail = 0;
 	i40e_status ret;
+	u32 val;
 	u32 v;
 
 	/* Now we wait for GRST to settle out.
@@ -6823,6 +6875,20 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 		}
 	}
 
+	/* Reconfigure hardware for allowing smaller MSS in the case
+	 * of TSO, so that we avoid the MDD being fired and causing
+	 * a reset in the case of small MSS+TSO.
+	 */
+#define I40E_REG_MSS          0x000E64DC
+#define I40E_REG_MSS_MIN_MASK 0x3FF0000
+#define I40E_64BYTE_MSS       0x400000
+	val = rd32(hw, I40E_REG_MSS);
+	if ((val & I40E_REG_MSS_MIN_MASK) > I40E_64BYTE_MSS) {
+		val &= ~I40E_REG_MSS_MIN_MASK;
+		val |= I40E_64BYTE_MSS;
+		wr32(hw, I40E_REG_MSS, val);
+	}
+
 	if (((pf->hw.aq.fw_maj_ver == 4) && (pf->hw.aq.fw_min_ver < 33)) ||
 	    (pf->hw.aq.fw_maj_ver < 4)) {
 		msleep(75);
@@ -6984,30 +7050,30 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 	i40e_flush(hw);
 }
 
-#ifdef CONFIG_I40E_VXLAN
 /**
- * i40e_sync_vxlan_filters_subtask - Sync the VSI filter list with HW
+ * i40e_sync_udp_filters_subtask - Sync the VSI filter list with HW
  * @pf: board private structure
  **/
-static void i40e_sync_vxlan_filters_subtask(struct i40e_pf *pf)
+static void i40e_sync_udp_filters_subtask(struct i40e_pf *pf)
 {
+#if IS_ENABLED(CONFIG_VXLAN) || IS_ENABLED(CONFIG_GENEVE)
 	struct i40e_hw *hw = &pf->hw;
 	i40e_status ret;
 	__be16 port;
 	int i;
 
-	if (!(pf->flags & I40E_FLAG_VXLAN_FILTER_SYNC))
+	if (!(pf->flags & I40E_FLAG_UDP_FILTER_SYNC))
 		return;
 
-	pf->flags &= ~I40E_FLAG_VXLAN_FILTER_SYNC;
+	pf->flags &= ~I40E_FLAG_UDP_FILTER_SYNC;
 
 	for (i = 0; i < I40E_MAX_PF_UDP_OFFLOAD_PORTS; i++) {
-		if (pf->pending_vxlan_bitmap & BIT_ULL(i)) {
-			pf->pending_vxlan_bitmap &= ~BIT_ULL(i);
-			port = pf->vxlan_ports[i];
+		if (pf->pending_udp_bitmap & BIT_ULL(i)) {
+			pf->pending_udp_bitmap &= ~BIT_ULL(i);
+			port = pf->udp_ports[i].index;
 			if (port)
 				ret = i40e_aq_add_udp_tunnel(hw, ntohs(port),
-						     I40E_AQC_TUNNEL_TYPE_VXLAN,
+						     pf->udp_ports[i].type,
 						     NULL, NULL);
 			else
 				ret = i40e_aq_del_udp_tunnel(hw, i, NULL);
@@ -7020,13 +7086,13 @@ static void i40e_sync_vxlan_filters_subtask(struct i40e_pf *pf)
 					 i40e_stat_str(&pf->hw, ret),
 					 i40e_aq_str(&pf->hw,
 						    pf->hw.aq.asq_last_status));
-				pf->vxlan_ports[i] = 0;
+				pf->udp_ports[i].index = 0;
 			}
 		}
 	}
+#endif
 }
 
-#endif
 /**
  * i40e_service_task - Run the driver's async subtasks
  * @work: pointer to work_struct containing our data
@@ -7051,9 +7117,7 @@ static void i40e_service_task(struct work_struct *work)
 	i40e_watchdog_subtask(pf);
 	i40e_fdir_reinit_subtask(pf);
 	i40e_sync_filters_subtask(pf);
-#ifdef CONFIG_I40E_VXLAN
-	i40e_sync_vxlan_filters_subtask(pf);
-#endif
+	i40e_sync_udp_filters_subtask(pf);
 	i40e_clean_adminq_subtask(pf);
 
 	i40e_service_event_complete(pf);
@@ -7282,6 +7346,23 @@ static void i40e_vsi_free_arrays(struct i40e_vsi *vsi, bool free_qvectors)
 }
 
 /**
+ * i40e_clear_rss_config_user - clear the user configured RSS hash keys
+ * and lookup table
+ * @vsi: Pointer to VSI structure
+ */
+static void i40e_clear_rss_config_user(struct i40e_vsi *vsi)
+{
+	if (!vsi)
+		return;
+
+	kfree(vsi->rss_hkey_user);
+	vsi->rss_hkey_user = NULL;
+
+	kfree(vsi->rss_lut_user);
+	vsi->rss_lut_user = NULL;
+}
+
+/**
  * i40e_vsi_clear - Deallocate the VSI provided
  * @vsi: the VSI being un-configured
  **/
@@ -7318,6 +7399,7 @@ static int i40e_vsi_clear(struct i40e_vsi *vsi)
 	i40e_put_lump(pf->irq_pile, vsi->base_vector, vsi->idx);
 
 	i40e_vsi_free_arrays(vsi, true);
+	i40e_clear_rss_config_user(vsi);
 
 	pf->vsi[vsi->idx] = NULL;
 	if (vsi->idx < pf->next_vsi)
@@ -7780,7 +7862,8 @@ static int i40e_setup_misc_vector(struct i40e_pf *pf)
  * @vsi: vsi structure
  * @seed: RSS hash seed
  **/
-static int i40e_config_rss_aq(struct i40e_vsi *vsi, const u8 *seed)
+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;
@@ -7833,43 +7916,57 @@ 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;
 
-	netdev_rss_key_fill((void *)seed, I40E_HKEY_ARRAY_SIZE);
-	vsi->rss_size = min_t(int, pf->rss_size, vsi->num_queue_pairs);
+	if (!(pf->flags & I40E_FLAG_RSS_AQ_CAPABLE))
+		return 0;
 
-	if (pf->flags & I40E_FLAG_RSS_AQ_CAPABLE)
-		return i40e_config_rss_aq(vsi, seed);
+	lut = kzalloc(vsi->rss_table_size, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
 
-	return 0;
+	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);
+
+	return ret;
 }
 
 /**
- * i40e_config_rss_reg - Prepare for RSS if used
- * @pf: board private structure
+ * i40e_config_rss_reg - Configure RSS keys and lut by writing registers
+ * @vsi: Pointer to vsi structure
  * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Returns 0 on success, negative on failure
  **/
-static int i40e_config_rss_reg(struct i40e_pf *pf, const u8 *seed)
+static int i40e_config_rss_reg(struct i40e_vsi *vsi, const u8 *seed,
+			       const u8 *lut, u16 lut_size)
 {
-	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
+	struct i40e_pf *pf = vsi->back;
 	struct i40e_hw *hw = &pf->hw;
-	u32 *seed_dw = (u32 *)seed;
-	u32 current_queue = 0;
-	u32 lut = 0;
-	int i, j;
+	u8 i;
 
 	/* Fill out hash function seed */
-	for (i = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++)
-		wr32(hw, I40E_PFQF_HKEY(i), seed_dw[i]);
+	if (seed) {
+		u32 *seed_dw = (u32 *)seed;
 
-	for (i = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++) {
-		lut = 0;
-		for (j = 0; j < 4; j++) {
-			if (current_queue == vsi->rss_size)
-				current_queue = 0;
-			lut |= ((current_queue) << (8 * j));
-			current_queue++;
-		}
-		wr32(&pf->hw, I40E_PFQF_HLUT(i), lut);
+		for (i = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++)
+			wr32(hw, I40E_PFQF_HKEY(i), seed_dw[i]);
+	}
+
+	if (lut) {
+		u32 *lut_dw = (u32 *)lut;
+
+		if (lut_size != I40E_HLUT_ARRAY_SIZE)
+			return -EINVAL;
+
+		for (i = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++)
+			wr32(hw, I40E_PFQF_HLUT(i), lut_dw[i]);
 	}
 	i40e_flush(hw);
 
@@ -7877,18 +7974,101 @@ static int i40e_config_rss_reg(struct i40e_pf *pf, const u8 *seed)
 }
 
 /**
- * i40e_config_rss - Prepare for RSS if used
+ * i40e_get_rss_reg - Get the RSS keys and lut by reading registers
+ * @vsi: Pointer to VSI structure
+ * @seed: Buffer to store the keys
+ * @lut: Buffer to store the lookup table entries
+ * @lut_size: Size of buffer to store the lookup table entries
+ *
+ * Returns 0 on success, negative on failure
+ */
+static int i40e_get_rss_reg(struct i40e_vsi *vsi, u8 *seed,
+			    u8 *lut, u16 lut_size)
+{
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_hw *hw = &pf->hw;
+	u16 i;
+
+	if (seed) {
+		u32 *seed_dw = (u32 *)seed;
+
+		for (i = 0; i <= I40E_PFQF_HKEY_MAX_INDEX; i++)
+			seed_dw[i] = rd32(hw, I40E_PFQF_HKEY(i));
+	}
+	if (lut) {
+		u32 *lut_dw = (u32 *)lut;
+
+		if (lut_size != I40E_HLUT_ARRAY_SIZE)
+			return -EINVAL;
+		for (i = 0; i <= I40E_PFQF_HLUT_MAX_INDEX; i++)
+			lut_dw[i] = rd32(hw, I40E_PFQF_HLUT(i));
+	}
+
+	return 0;
+}
+
+/**
+ * i40e_config_rss - Configure RSS keys and lut
+ * @vsi: Pointer to VSI structure
+ * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Returns 0 on success, negative on failure
+ */
+int i40e_config_rss(struct i40e_vsi *vsi, u8 *seed, u8 *lut, u16 lut_size)
+{
+	struct i40e_pf *pf = vsi->back;
+
+	if (pf->flags & I40E_FLAG_RSS_AQ_CAPABLE)
+		return i40e_config_rss_aq(vsi, seed, lut, lut_size);
+	else
+		return i40e_config_rss_reg(vsi, seed, lut, lut_size);
+}
+
+/**
+ * i40e_get_rss - Get RSS keys and lut
+ * @vsi: Pointer to VSI structure
+ * @seed: Buffer to store the keys
+ * @lut: Buffer to store the lookup table entries
+ * lut_size: Size of buffer to store the lookup table entries
+ *
+ * Returns 0 on success, negative on failure
+ */
+int i40e_get_rss(struct i40e_vsi *vsi, u8 *seed, u8 *lut, u16 lut_size)
+{
+	return i40e_get_rss_reg(vsi, seed, lut, lut_size);
+}
+
+/**
+ * i40e_fill_rss_lut - Fill the RSS lookup table with default values
+ * @pf: Pointer to board private structure
+ * @lut: Lookup table
+ * @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)
+{
+	u16 i;
+
+	for (i = 0; i < rss_table_size; i++)
+		lut[i] = i % rss_size;
+}
+
+/**
+ * i40e_pf_config_rss - Prepare for RSS if used
  * @pf: board private structure
  **/
-static int i40e_config_rss(struct i40e_pf *pf)
+static int i40e_pf_config_rss(struct i40e_pf *pf)
 {
 	struct i40e_vsi *vsi = pf->vsi[pf->lan_vsi];
 	u8 seed[I40E_HKEY_ARRAY_SIZE];
+	u8 *lut;
 	struct i40e_hw *hw = &pf->hw;
 	u32 reg_val;
 	u64 hena;
-
-	netdev_rss_key_fill((void *)seed, I40E_HKEY_ARRAY_SIZE);
+	int ret;
 
 	/* By default we enable TCP/UDP with IPv4/IPv6 ptypes */
 	hena = (u64)rd32(hw, I40E_PFQF_HENA(0)) |
@@ -7898,8 +8078,6 @@ static int i40e_config_rss(struct i40e_pf *pf)
 	wr32(hw, I40E_PFQF_HENA(0), (u32)hena);
 	wr32(hw, I40E_PFQF_HENA(1), (u32)(hena >> 32));
 
-	vsi->rss_size = min_t(int, pf->rss_size, vsi->num_queue_pairs);
-
 	/* Determine the RSS table size based on the hardware capabilities */
 	reg_val = rd32(hw, I40E_PFQF_CTL_0);
 	reg_val = (pf->rss_table_size == 512) ?
@@ -7907,10 +8085,32 @@ static int i40e_config_rss(struct i40e_pf *pf)
 			(reg_val & ~I40E_PFQF_CTL_0_HASHLUTSIZE_512);
 	wr32(hw, I40E_PFQF_CTL_0, reg_val);
 
-	if (pf->flags & I40E_FLAG_RSS_AQ_CAPABLE)
-		return i40e_config_rss_aq(pf->vsi[pf->lan_vsi], seed);
+	/* Determine the RSS size of the VSI */
+	if (!vsi->rss_size)
+		vsi->rss_size = min_t(int, pf->alloc_rss_size,
+				      vsi->num_queue_pairs);
+
+	lut = kzalloc(vsi->rss_table_size, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
+
+	/* Use user configured lut if there is one, otherwise use default */
+	if (vsi->rss_lut_user)
+		memcpy(lut, vsi->rss_lut_user, vsi->rss_table_size);
 	else
-		return i40e_config_rss_reg(pf, seed);
+		i40e_fill_rss_lut(pf, lut, vsi->rss_table_size, vsi->rss_size);
+
+	/* Use user configured hash key if there is one, otherwise
+	 * use default.
+	 */
+	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(vsi, seed, lut, vsi->rss_table_size);
+	kfree(lut);
+
+	return ret;
 }
 
 /**
@@ -7935,13 +8135,28 @@ int i40e_reconfig_rss_queues(struct i40e_pf *pf, int queue_count)
 		vsi->req_queue_pairs = queue_count;
 		i40e_prep_for_reset(pf);
 
-		pf->rss_size = new_rss_size;
+		pf->alloc_rss_size = new_rss_size;
 
 		i40e_reset_and_rebuild(pf, true);
-		i40e_config_rss(pf);
+
+		/* Discard the user configured hash keys and lut, if less
+		 * queues are enabled.
+		 */
+		if (queue_count < vsi->rss_size) {
+			i40e_clear_rss_config_user(vsi);
+			dev_dbg(&pf->pdev->dev,
+				"discard user configured hash keys and lut\n");
+		}
+
+		/* Reset vsi->rss_size, as number of enabled queues changed */
+		vsi->rss_size = min_t(int, pf->alloc_rss_size,
+				      vsi->num_queue_pairs);
+
+		i40e_pf_config_rss(pf);
 	}
-	dev_info(&pf->pdev->dev, "RSS count:  %d\n", pf->rss_size);
-	return pf->rss_size;
+	dev_info(&pf->pdev->dev, "RSS count/HW max RSS count:  %d/%d\n",
+		 pf->alloc_rss_size, pf->rss_size_max);
+	return pf->alloc_rss_size;
 }
 
 /**
@@ -8112,13 +8327,14 @@ static int i40e_sw_init(struct i40e_pf *pf)
 	 * maximum might end up larger than the available queues
 	 */
 	pf->rss_size_max = BIT(pf->hw.func_caps.rss_table_entry_width);
-	pf->rss_size = 1;
+	pf->alloc_rss_size = 1;
 	pf->rss_table_size = pf->hw.func_caps.rss_table_size;
 	pf->rss_size_max = min_t(int, pf->rss_size_max,
 				 pf->hw.func_caps.num_tx_qp);
 	if (pf->hw.func_caps.rss) {
 		pf->flags |= I40E_FLAG_RSS_ENABLED;
-		pf->rss_size = min_t(int, pf->rss_size_max, num_online_cpus());
+		pf->alloc_rss_size = min_t(int, pf->rss_size_max,
+					   num_online_cpus());
 	}
 
 	/* MFP mode enabled */
@@ -8176,7 +8392,8 @@ static int i40e_sw_init(struct i40e_pf *pf)
 			     I40E_FLAG_HW_ATR_EVICT_CAPABLE |
 			     I40E_FLAG_OUTER_UDP_CSUM_CAPABLE |
 			     I40E_FLAG_WB_ON_ITR_CAPABLE |
-			     I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE;
+			     I40E_FLAG_MULTIPLE_TCP_UDP_RSS_PCTYPE |
+			     I40E_FLAG_GENEVE_OFFLOAD_CAPABLE;
 	}
 	pf->eeprom_version = 0xDEAD;
 	pf->lan_veb = I40E_NO_VEB;
@@ -8275,26 +8492,29 @@ static int i40e_set_features(struct net_device *netdev,
 	return 0;
 }
 
-#ifdef CONFIG_I40E_VXLAN
+#if IS_ENABLED(CONFIG_VXLAN) || IS_ENABLED(CONFIG_GENEVE)
 /**
- * i40e_get_vxlan_port_idx - Lookup a possibly offloaded for Rx UDP port
+ * i40e_get_udp_port_idx - Lookup a possibly offloaded for Rx UDP port
  * @pf: board private structure
  * @port: The UDP port to look up
  *
  * Returns the index number or I40E_MAX_PF_UDP_OFFLOAD_PORTS if port not found
  **/
-static u8 i40e_get_vxlan_port_idx(struct i40e_pf *pf, __be16 port)
+static u8 i40e_get_udp_port_idx(struct i40e_pf *pf, __be16 port)
 {
 	u8 i;
 
 	for (i = 0; i < I40E_MAX_PF_UDP_OFFLOAD_PORTS; i++) {
-		if (pf->vxlan_ports[i] == port)
+		if (pf->udp_ports[i].index == port)
 			return i;
 	}
 
 	return i;
 }
 
+#endif
+
+#if IS_ENABLED(CONFIG_VXLAN)
 /**
  * i40e_add_vxlan_port - Get notifications about VXLAN ports that come up
  * @netdev: This physical port's netdev
@@ -8313,7 +8533,7 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 	if (sa_family == AF_INET6)
 		return;
 
-	idx = i40e_get_vxlan_port_idx(pf, port);
+	idx = i40e_get_udp_port_idx(pf, port);
 
 	/* Check if port already exists */
 	if (idx < I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
@@ -8323,7 +8543,7 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 	}
 
 	/* Now check if there is space to add the new port */
-	next_idx = i40e_get_vxlan_port_idx(pf, 0);
+	next_idx = i40e_get_udp_port_idx(pf, 0);
 
 	if (next_idx == I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
 		netdev_info(netdev, "maximum number of vxlan UDP ports reached, not adding port %d\n",
@@ -8332,9 +8552,10 @@ static void i40e_add_vxlan_port(struct net_device *netdev,
 	}
 
 	/* New port: add it and mark its index in the bitmap */
-	pf->vxlan_ports[next_idx] = port;
-	pf->pending_vxlan_bitmap |= BIT_ULL(next_idx);
-	pf->flags |= I40E_FLAG_VXLAN_FILTER_SYNC;
+	pf->udp_ports[next_idx].index = port;
+	pf->udp_ports[next_idx].type = I40E_AQC_TUNNEL_TYPE_VXLAN;
+	pf->pending_udp_bitmap |= BIT_ULL(next_idx);
+	pf->flags |= I40E_FLAG_UDP_FILTER_SYNC;
 }
 
 /**
@@ -8354,23 +8575,106 @@ static void i40e_del_vxlan_port(struct net_device *netdev,
 	if (sa_family == AF_INET6)
 		return;
 
-	idx = i40e_get_vxlan_port_idx(pf, port);
+	idx = i40e_get_udp_port_idx(pf, port);
 
 	/* Check if port already exists */
 	if (idx < I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
 		/* if port exists, set it to 0 (mark for deletion)
 		 * and make it pending
 		 */
-		pf->vxlan_ports[idx] = 0;
-		pf->pending_vxlan_bitmap |= BIT_ULL(idx);
-		pf->flags |= I40E_FLAG_VXLAN_FILTER_SYNC;
+		pf->udp_ports[idx].index = 0;
+		pf->pending_udp_bitmap |= BIT_ULL(idx);
+		pf->flags |= I40E_FLAG_UDP_FILTER_SYNC;
 	} else {
 		netdev_warn(netdev, "vxlan port %d was not found, not deleting\n",
 			    ntohs(port));
 	}
 }
+#endif
+
+#if IS_ENABLED(CONFIG_GENEVE)
+/**
+ * i40e_add_geneve_port - Get notifications about GENEVE ports that come up
+ * @netdev: This physical port's netdev
+ * @sa_family: Socket Family that GENEVE is notifying us about
+ * @port: New UDP port number that GENEVE started listening to
+ **/
+static void i40e_add_geneve_port(struct net_device *netdev,
+				 sa_family_t sa_family, __be16 port)
+{
+	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	u8 next_idx;
+	u8 idx;
+
+	if (sa_family == AF_INET6)
+		return;
+
+	idx = i40e_get_udp_port_idx(pf, port);
+
+	/* Check if port already exists */
+	if (idx < I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
+		netdev_info(netdev, "udp port %d already offloaded\n",
+			    ntohs(port));
+		return;
+	}
+
+	/* Now check if there is space to add the new port */
+	next_idx = i40e_get_udp_port_idx(pf, 0);
+
+	if (next_idx == I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
+		netdev_info(netdev, "maximum number of UDP ports reached, not adding port %d\n",
+			    ntohs(port));
+		return;
+	}
+
+	/* New port: add it and mark its index in the bitmap */
+	pf->udp_ports[next_idx].index = port;
+	pf->udp_ports[next_idx].type = I40E_AQC_TUNNEL_TYPE_NGE;
+	pf->pending_udp_bitmap |= BIT_ULL(next_idx);
+	pf->flags |= I40E_FLAG_UDP_FILTER_SYNC;
+
+	dev_info(&pf->pdev->dev, "adding geneve port %d\n", ntohs(port));
+}
+
+/**
+ * i40e_del_geneve_port - Get notifications about GENEVE ports that go away
+ * @netdev: This physical port's netdev
+ * @sa_family: Socket Family that GENEVE is notifying us about
+ * @port: UDP port number that GENEVE stopped listening to
+ **/
+static void i40e_del_geneve_port(struct net_device *netdev,
+				 sa_family_t sa_family, __be16 port)
+{
+	struct i40e_netdev_priv *np = netdev_priv(netdev);
+	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	u8 idx;
 
+	if (sa_family == AF_INET6)
+		return;
+
+	idx = i40e_get_udp_port_idx(pf, port);
+
+	/* Check if port already exists */
+	if (idx < I40E_MAX_PF_UDP_OFFLOAD_PORTS) {
+		/* if port exists, set it to 0 (mark for deletion)
+		 * and make it pending
+		 */
+		pf->udp_ports[idx].index = 0;
+		pf->pending_udp_bitmap |= BIT_ULL(idx);
+		pf->flags |= I40E_FLAG_UDP_FILTER_SYNC;
+
+		dev_info(&pf->pdev->dev, "deleting geneve port %d\n",
+			 ntohs(port));
+	} else {
+		netdev_warn(netdev, "geneve port %d was not found, not deleting\n",
+			    ntohs(port));
+	}
+}
 #endif
+
 static int i40e_get_phys_port_id(struct net_device *netdev,
 				 struct netdev_phys_item_id *ppid)
 {
@@ -8548,7 +8852,10 @@ static int i40e_ndo_bridge_getlink(struct sk_buff *skb, u32 pid, u32 seq,
 				       nlflags, 0, 0, filter_mask, NULL);
 }
 
-#define I40E_MAX_TUNNEL_HDR_LEN 80
+/* Hardware supports L4 tunnel length of 128B (=2^7) which includes
+ * inner mac plus all inner ethertypes.
+ */
+#define I40E_MAX_TUNNEL_HDR_LEN 128
 /**
  * i40e_features_check - Validate encapsulated packet conforms to limits
  * @skb: skb buff
@@ -8560,9 +8867,9 @@ static netdev_features_t i40e_features_check(struct sk_buff *skb,
 					     netdev_features_t features)
 {
 	if (skb->encapsulation &&
-	    (skb_inner_mac_header(skb) - skb_transport_header(skb) >
+	    ((skb_inner_network_header(skb) - skb_transport_header(skb)) >
 	     I40E_MAX_TUNNEL_HDR_LEN))
-		return features & ~(NETIF_F_ALL_CSUM | NETIF_F_GSO_MASK);
+		return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
 
 	return features;
 }
@@ -8595,10 +8902,14 @@ static const struct net_device_ops i40e_netdev_ops = {
 	.ndo_get_vf_config	= i40e_ndo_get_vf_config,
 	.ndo_set_vf_link_state	= i40e_ndo_set_vf_link_state,
 	.ndo_set_vf_spoofchk	= i40e_ndo_set_vf_spoofchk,
-#ifdef CONFIG_I40E_VXLAN
+#if IS_ENABLED(CONFIG_VXLAN)
 	.ndo_add_vxlan_port	= i40e_add_vxlan_port,
 	.ndo_del_vxlan_port	= i40e_del_vxlan_port,
 #endif
+#if IS_ENABLED(CONFIG_GENEVE)
+	.ndo_add_geneve_port	= i40e_add_geneve_port,
+	.ndo_del_geneve_port	= i40e_del_geneve_port,
+#endif
 	.ndo_get_phys_port_id	= i40e_get_phys_port_id,
 	.ndo_fdb_add		= i40e_ndo_fdb_add,
 	.ndo_features_check	= i40e_features_check,
@@ -8632,13 +8943,14 @@ static int i40e_config_netdev(struct i40e_vsi *vsi)
 	np->vsi = vsi;
 
 	netdev->hw_enc_features |= NETIF_F_IP_CSUM	 |
+				  NETIF_F_RXCSUM	 |
 				  NETIF_F_GSO_UDP_TUNNEL |
 				  NETIF_F_GSO_GRE	 |
 				  NETIF_F_TSO;
 
 	netdev->features = NETIF_F_SG		       |
 			   NETIF_F_IP_CSUM	       |
-			   NETIF_F_SCTP_CSUM	       |
+			   NETIF_F_SCTP_CRC	       |
 			   NETIF_F_HIGHDMA	       |
 			   NETIF_F_GSO_UDP_TUNNEL      |
 			   NETIF_F_GSO_GRE	       |
@@ -9051,7 +9363,7 @@ int i40e_vsi_release(struct i40e_vsi *vsi)
 				f->is_vf, f->is_netdev);
 	spin_unlock_bh(&vsi->mac_filter_list_lock);
 
-	i40e_sync_vsi_filters(vsi, false);
+	i40e_sync_vsi_filters(vsi);
 
 	i40e_vsi_delete(vsi);
 	i40e_vsi_free_q_vectors(vsi);
@@ -9213,6 +9525,44 @@ err_vsi:
 }
 
 /**
+ * i40e_macaddr_init - explicitly write the mac address filters.
+ *
+ * @vsi: pointer to the vsi.
+ * @macaddr: the MAC address
+ *
+ * This is needed when the macaddr has been obtained by other
+ * means than the default, e.g., from Open Firmware or IDPROM.
+ * Returns 0 on success, negative on failure
+ **/
+static int i40e_macaddr_init(struct i40e_vsi *vsi, u8 *macaddr)
+{
+	int ret;
+	struct i40e_aqc_add_macvlan_element_data element;
+
+	ret = i40e_aq_mac_address_write(&vsi->back->hw,
+					I40E_AQC_WRITE_TYPE_LAA_WOL,
+					macaddr, NULL);
+	if (ret) {
+		dev_info(&vsi->back->pdev->dev,
+			 "Addr change for VSI failed: %d\n", ret);
+		return -EADDRNOTAVAIL;
+	}
+
+	memset(&element, 0, sizeof(element));
+	ether_addr_copy(element.mac_addr, macaddr);
+	element.flags = cpu_to_le16(I40E_AQC_MACVLAN_ADD_PERFECT_MATCH);
+	ret = i40e_aq_add_macvlan(&vsi->back->hw, vsi->seid, &element, 1, NULL);
+	if (ret) {
+		dev_info(&vsi->back->pdev->dev,
+			 "add filter failed err %s aq_err %s\n",
+			 i40e_stat_str(&vsi->back->hw, ret),
+			 i40e_aq_str(&vsi->back->hw,
+				     vsi->back->hw.aq.asq_last_status));
+	}
+	return ret;
+}
+
+/**
  * i40e_vsi_setup - Set up a VSI by a given type
  * @pf: board private structure
  * @type: VSI type
@@ -9336,6 +9686,17 @@ struct i40e_vsi *i40e_vsi_setup(struct i40e_pf *pf, u8 type,
 	switch (vsi->type) {
 	/* setup the netdev if needed */
 	case I40E_VSI_MAIN:
+		/* Apply relevant filters if a platform-specific mac
+		 * address was selected.
+		 */
+		if (!!(pf->flags & I40E_FLAG_PF_MAC)) {
+			ret = i40e_macaddr_init(vsi, pf->hw.mac.addr);
+			if (ret) {
+				dev_warn(&pf->pdev->dev,
+					 "could not set up macaddr; err %d\n",
+					 ret);
+			}
+		}
 	case I40E_VSI_VMDQ2:
 	case I40E_VSI_FCOE:
 		ret = i40e_config_netdev(vsi);
@@ -9947,7 +10308,7 @@ static int i40e_setup_pf_switch(struct i40e_pf *pf, bool reinit)
 	 * the hash
 	 */
 	if ((pf->flags & I40E_FLAG_RSS_ENABLED))
-		i40e_config_rss(pf);
+		i40e_pf_config_rss(pf);
 
 	/* fill in link information and enable LSE reporting */
 	i40e_update_link_info(&pf->hw);
@@ -9985,7 +10346,7 @@ static void i40e_determine_queue_usage(struct i40e_pf *pf)
 	    !(pf->flags & I40E_FLAG_MSIX_ENABLED)) {
 		/* one qp for PF, no queues for anything else */
 		queues_left = 0;
-		pf->rss_size = pf->num_lan_qps = 1;
+		pf->alloc_rss_size = pf->num_lan_qps = 1;
 
 		/* make sure all the fancies are disabled */
 		pf->flags &= ~(I40E_FLAG_RSS_ENABLED	|
@@ -10002,7 +10363,7 @@ static void i40e_determine_queue_usage(struct i40e_pf *pf)
 				  I40E_FLAG_FD_ATR_ENABLED |
 				  I40E_FLAG_DCB_CAPABLE))) {
 		/* one qp for PF */
-		pf->rss_size = pf->num_lan_qps = 1;
+		pf->alloc_rss_size = pf->num_lan_qps = 1;
 		queues_left -= pf->num_lan_qps;
 
 		pf->flags &= ~(I40E_FLAG_RSS_ENABLED	|
@@ -10072,8 +10433,9 @@ static void i40e_determine_queue_usage(struct i40e_pf *pf)
 		"qs_avail=%d FD SB=%d lan_qs=%d lan_tc0=%d vf=%d*%d vmdq=%d*%d, remaining=%d\n",
 		pf->hw.func_caps.num_tx_qp,
 		!!(pf->flags & I40E_FLAG_FD_SB_ENABLED),
-		pf->num_lan_qps, pf->rss_size, pf->num_req_vfs, pf->num_vf_qps,
-		pf->num_vmdq_vsis, pf->num_vmdq_qps, queues_left);
+		pf->num_lan_qps, pf->alloc_rss_size, pf->num_req_vfs,
+		pf->num_vf_qps, pf->num_vmdq_vsis, pf->num_vmdq_qps,
+		queues_left);
 #ifdef I40E_FCOE
 	dev_dbg(&pf->pdev->dev, "fcoe queues = %d\n", pf->num_fcoe_qps);
 #endif
@@ -10111,55 +10473,86 @@ static int i40e_setup_pf_filter_control(struct i40e_pf *pf)
 }
 
 #define INFO_STRING_LEN 255
+#define REMAIN(__x) (INFO_STRING_LEN - (__x))
 static void i40e_print_features(struct i40e_pf *pf)
 {
 	struct i40e_hw *hw = &pf->hw;
-	char *buf, *string;
+	char *buf;
+	int i;
 
-	string = kzalloc(INFO_STRING_LEN, GFP_KERNEL);
-	if (!string) {
-		dev_err(&pf->pdev->dev, "Features string allocation failed\n");
+	buf = kmalloc(INFO_STRING_LEN, GFP_KERNEL);
+	if (!buf)
 		return;
-	}
 
-	buf = string;
-
-	buf += sprintf(string, "Features: PF-id[%d] ", hw->pf_id);
+	i = snprintf(buf, INFO_STRING_LEN, "Features: PF-id[%d]", hw->pf_id);
 #ifdef CONFIG_PCI_IOV
-	buf += sprintf(buf, "VFs: %d ", pf->num_req_vfs);
+	i += snprintf(&buf[i], REMAIN(i), " VFs: %d", pf->num_req_vfs);
 #endif
-	buf += sprintf(buf, "VSIs: %d QP: %d RX: %s ",
-		       pf->hw.func_caps.num_vsis,
-		       pf->vsi[pf->lan_vsi]->num_queue_pairs,
-		       pf->flags & I40E_FLAG_RX_PS_ENABLED ? "PS" : "1BUF");
+	i += snprintf(&buf[i], REMAIN(i), " VSIs: %d QP: %d RX: %s",
+		      pf->hw.func_caps.num_vsis,
+		      pf->vsi[pf->lan_vsi]->num_queue_pairs,
+		      pf->flags & I40E_FLAG_RX_PS_ENABLED ? "PS" : "1BUF");
 
 	if (pf->flags & I40E_FLAG_RSS_ENABLED)
-		buf += sprintf(buf, "RSS ");
+		i += snprintf(&buf[i], REMAIN(i), " RSS");
 	if (pf->flags & I40E_FLAG_FD_ATR_ENABLED)
-		buf += sprintf(buf, "FD_ATR ");
+		i += snprintf(&buf[i], REMAIN(i), " FD_ATR");
 	if (pf->flags & I40E_FLAG_FD_SB_ENABLED) {
-		buf += sprintf(buf, "FD_SB ");
-		buf += sprintf(buf, "NTUPLE ");
+		i += snprintf(&buf[i], REMAIN(i), " FD_SB");
+		i += snprintf(&buf[i], REMAIN(i), " NTUPLE");
 	}
 	if (pf->flags & I40E_FLAG_DCB_CAPABLE)
-		buf += sprintf(buf, "DCB ");
+		i += snprintf(&buf[i], REMAIN(i), " DCB");
 #if IS_ENABLED(CONFIG_VXLAN)
-	buf += sprintf(buf, "VxLAN ");
+	i += snprintf(&buf[i], REMAIN(i), " VxLAN");
+#endif
+#if IS_ENABLED(CONFIG_GENEVE)
+	i += snprintf(&buf[i], REMAIN(i), " Geneve");
 #endif
 	if (pf->flags & I40E_FLAG_PTP)
-		buf += sprintf(buf, "PTP ");
+		i += snprintf(&buf[i], REMAIN(i), " PTP");
 #ifdef I40E_FCOE
 	if (pf->flags & I40E_FLAG_FCOE_ENABLED)
-		buf += sprintf(buf, "FCOE ");
+		i += snprintf(&buf[i], REMAIN(i), " FCOE");
 #endif
 	if (pf->flags & I40E_FLAG_VEB_MODE_ENABLED)
-		buf += sprintf(buf, "VEB ");
+		i += snprintf(&buf[i], REMAIN(i), " VEB");
 	else
-		buf += sprintf(buf, "VEPA ");
+		i += snprintf(&buf[i], REMAIN(i), " VEPA");
 
-	BUG_ON(buf > (string + INFO_STRING_LEN));
-	dev_info(&pf->pdev->dev, "%s\n", string);
-	kfree(string);
+	dev_info(&pf->pdev->dev, "%s\n", buf);
+	kfree(buf);
+	WARN_ON(i > INFO_STRING_LEN);
+}
+
+/**
+ * i40e_get_platform_mac_addr - get platform-specific MAC address
+ *
+ * @pdev: PCI device information struct
+ * @pf: board private structure
+ *
+ * Look up the MAC address in Open Firmware  on systems that support it,
+ * and use IDPROM on SPARC if no OF address is found. On return, the
+ * I40E_FLAG_PF_MAC will be wset in pf->flags if a platform-specific value
+ * has been selected.
+ **/
+static void i40e_get_platform_mac_addr(struct pci_dev *pdev, struct i40e_pf *pf)
+{
+	struct device_node *dp = pci_device_to_OF_node(pdev);
+	const unsigned char *addr;
+	u8 *mac_addr = pf->hw.mac.addr;
+
+	pf->flags &= ~I40E_FLAG_PF_MAC;
+	addr = of_get_mac_address(dp);
+	if (addr) {
+		ether_addr_copy(mac_addr, addr);
+		pf->flags |= I40E_FLAG_PF_MAC;
+#ifdef CONFIG_SPARC
+	} else {
+		ether_addr_copy(mac_addr, idprom->id_ethaddr);
+		pf->flags |= I40E_FLAG_PF_MAC;
+#endif /* CONFIG_SPARC */
+	}
 }
 
 /**
@@ -10183,6 +10576,7 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	u16 link_status;
 	int err;
 	u32 len;
+	u32 val;
 	u32 i;
 	u8 set_fc_aq_fail;
 
@@ -10302,6 +10696,16 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	mutex_init(&hw->aq.arq_mutex);
 
 	err = i40e_init_adminq(hw);
+	if (err) {
+		if (err == I40E_ERR_FIRMWARE_API_VERSION)
+			dev_info(&pdev->dev,
+				 "The driver for the device stopped because the NVM image is newer than expected. You must install the most recent version of the network driver.\n");
+		else
+			dev_info(&pdev->dev,
+				 "The driver for the device stopped because the device firmware failed to init. Try updating your NVM image.\n");
+
+		goto err_pf_reset;
+	}
 
 	/* provide nvm, fw, api versions */
 	dev_info(&pdev->dev, "fw %d.%d.%05d api %d.%d nvm %s\n",
@@ -10309,12 +10713,6 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		 hw->aq.api_maj_ver, hw->aq.api_min_ver,
 		 i40e_nvm_version_str(hw));
 
-	if (err) {
-		dev_info(&pdev->dev,
-			 "The driver for the device stopped because the NVM image is newer than expected. You must install the most recent version of the network driver.\n");
-		goto err_pf_reset;
-	}
-
 	if (hw->aq.api_maj_ver == I40E_FW_API_VERSION_MAJOR &&
 	    hw->aq.api_min_ver > I40E_FW_API_VERSION_MINOR)
 		dev_info(&pdev->dev,
@@ -10367,6 +10765,8 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	i40e_get_mac_addr(hw, hw->mac.addr);
+	/* allow a platform config to override the HW addr */
+	i40e_get_platform_mac_addr(pdev, pf);
 	if (!is_valid_ether_addr(hw->mac.addr)) {
 		dev_info(&pdev->dev, "invalid MAC address %pM\n", hw->mac.addr);
 		err = -EIO;
@@ -10411,7 +10811,7 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* NVM bit on means WoL disabled for the port */
 	i40e_read_nvm_word(hw, I40E_SR_NVM_WAKE_ON_LAN, &wol_nvm_bits);
-	if ((1 << hw->port) & wol_nvm_bits || hw->partition_id != 1)
+	if (BIT (hw->port) & wol_nvm_bits || hw->partition_id != 1)
 		pf->wol_en = false;
 	else
 		pf->wol_en = true;
@@ -10493,6 +10893,17 @@ static int i40e_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			 i40e_stat_str(&pf->hw, err),
 			 i40e_aq_str(&pf->hw, pf->hw.aq.asq_last_status));
 
+	/* Reconfigure hardware for allowing smaller MSS in the case
+	 * of TSO, so that we avoid the MDD being fired and causing
+	 * a reset in the case of small MSS+TSO.
+	 */
+	val = rd32(hw, I40E_REG_MSS);
+	if ((val & I40E_REG_MSS_MIN_MASK) > I40E_64BYTE_MSS) {
+		val &= ~I40E_REG_MSS_MIN_MASK;
+		val |= I40E_64BYTE_MSS;
+		wr32(hw, I40E_REG_MSS, val);
+	}
+
 	if (((pf->hw.aq.fw_maj_ver == 4) && (pf->hw.aq.fw_min_ver < 33)) ||
 	    (pf->hw.aq.fw_maj_ver < 4)) {
 		msleep(75);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index 635b3ac17877..47bd8b3145a7 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -235,6 +235,9 @@ static int i40e_add_del_fdir_udpv4(struct i40e_vsi *vsi,
 				 "Filter deleted for PCTYPE %d loc = %d\n",
 				 fd_data->pctype, fd_data->fd_id);
 	}
+	if (err)
+		kfree(raw_packet);
+
 	return err ? -EOPNOTSUPP : 0;
 }
 
@@ -312,6 +315,9 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 				 fd_data->pctype, fd_data->fd_id);
 	}
 
+	if (err)
+		kfree(raw_packet);
+
 	return err ? -EOPNOTSUPP : 0;
 }
 
@@ -322,7 +328,7 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
  * @fd_data: the flow director data required for the FDir descriptor
  * @add: true adds a filter, false removes it
  *
- * Always returns -EOPNOTSUPP
+ * Returns 0 if the filters were successfully added or removed
  **/
 static int i40e_add_del_fdir_sctpv4(struct i40e_vsi *vsi,
 				    struct i40e_fdir_filter *fd_data,
@@ -387,6 +393,9 @@ static int i40e_add_del_fdir_ipv4(struct i40e_vsi *vsi,
 		}
 	}
 
+	if (err)
+		kfree(raw_packet);
+
 	return err ? -EOPNOTSUPP : 0;
 }
 
@@ -506,9 +515,6 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 				pf->auto_disable_flags |=
 							I40E_FLAG_FD_SB_ENABLED;
 			}
-		} else {
-			dev_info(&pdev->dev,
-				"FD filter programming failed due to incorrect filter parameters\n");
 		}
 	} else if (error == BIT(I40E_RX_PROG_STATUS_DESC_NO_FD_ENTRY_SHIFT)) {
 		if (I40E_DEBUG_FD & pf->hw.debug_mask)
@@ -526,11 +532,7 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 					    struct i40e_tx_buffer *tx_buffer)
 {
 	if (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);
-
+		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),
@@ -542,6 +544,10 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 			       dma_unmap_len(tx_buffer, len),
 			       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);
@@ -1374,7 +1380,7 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	if (rx_error & BIT(I40E_RX_DESC_ERROR_PPRS_SHIFT))
 		return;
 
-	/* If VXLAN traffic has an outer UDPv4 checksum we need to check
+	/* If VXLAN/GENEVE traffic has an outer UDPv4 checksum we need to check
 	 * it in the driver, hardware does not do it for us.
 	 * Since L3L4P bit was set we assume a valid IHL value (>=5)
 	 * so the total length of IPv4 header is IHL*4 bytes
@@ -1416,31 +1422,12 @@ checksum_fail:
 }
 
 /**
- * i40e_rx_hash - returns the hash value from the Rx descriptor
- * @ring: descriptor ring
- * @rx_desc: specific descriptor
- **/
-static inline u32 i40e_rx_hash(struct i40e_ring *ring,
-			       union i40e_rx_desc *rx_desc)
-{
-	const __le64 rss_mask =
-		cpu_to_le64((u64)I40E_RX_DESC_FLTSTAT_RSS_HASH <<
-			    I40E_RX_DESC_STATUS_FLTSTAT_SHIFT);
-
-	if ((ring->netdev->features & NETIF_F_RXHASH) &&
-	    (rx_desc->wb.qword1.status_error_len & rss_mask) == rss_mask)
-		return le32_to_cpu(rx_desc->wb.qword0.hi_dword.rss);
-	else
-		return 0;
-}
-
-/**
- * i40e_ptype_to_hash - get a hash type
+ * i40e_ptype_to_htype - get a hash type
  * @ptype: the ptype value from the descriptor
  *
  * Returns a hash type to be used by skb_set_hash
  **/
-static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
+static inline enum pkt_hash_types i40e_ptype_to_htype(u8 ptype)
 {
 	struct i40e_rx_ptype_decoded decoded = decode_rx_desc_ptype(ptype);
 
@@ -1458,6 +1445,30 @@ static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
 }
 
 /**
+ * i40e_rx_hash - set the hash value in the skb
+ * @ring: descriptor ring
+ * @rx_desc: specific descriptor
+ **/
+static inline void i40e_rx_hash(struct i40e_ring *ring,
+				union i40e_rx_desc *rx_desc,
+				struct sk_buff *skb,
+				u8 rx_ptype)
+{
+	u32 hash;
+	const __le64 rss_mask  =
+		cpu_to_le64((u64)I40E_RX_DESC_FLTSTAT_RSS_HASH <<
+			    I40E_RX_DESC_STATUS_FLTSTAT_SHIFT);
+
+	if (ring->netdev->features & NETIF_F_RXHASH)
+		return;
+
+	if ((rx_desc->wb.qword1.status_error_len & rss_mask) == rss_mask) {
+		hash = le32_to_cpu(rx_desc->wb.qword0.hi_dword.rss);
+		skb_set_hash(skb, hash, i40e_ptype_to_htype(rx_ptype));
+	}
+}
+
+/**
  * i40e_clean_rx_irq_ps - Reclaim resources after receive; packet split
  * @rx_ring:  rx ring to clean
  * @budget:   how many cleans we're allowed
@@ -1606,8 +1617,8 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 
-		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
-			     i40e_ptype_to_hash(rx_ptype));
+		i40e_rx_hash(rx_ring, rx_desc, skb, rx_ptype);
+
 		if (unlikely(rx_status & I40E_RXD_QW1_STATUS_TSYNVALID_MASK)) {
 			i40e_ptp_rx_hwtstamp(vsi->back, skb, (rx_status &
 					   I40E_RXD_QW1_STATUS_TSYNINDX_MASK) >>
@@ -1632,7 +1643,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 #endif
-		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
 		rx_desc->wb.qword1.status_error_len = 0;
@@ -1736,8 +1746,7 @@ static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 
-		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
-			     i40e_ptype_to_hash(rx_ptype));
+		i40e_rx_hash(rx_ring, rx_desc, skb, rx_ptype);
 		if (unlikely(rx_status & I40E_RXD_QW1_STATUS_TSYNVALID_MASK)) {
 			i40e_ptp_rx_hwtstamp(vsi->back, skb, (rx_status &
 					   I40E_RXD_QW1_STATUS_TSYNINDX_MASK) >>
@@ -1864,7 +1873,6 @@ enable_int:
 		q_vector->itr_countdown--;
 	else
 		q_vector->itr_countdown = ITR_COUNTDOWN_START;
-
 }
 
 /**
@@ -1892,12 +1900,14 @@ int i40e_napi_poll(struct napi_struct *napi, int budget)
 		return 0;
 	}
 
+	/* Clear hung_detected bit */
+	clear_bit(I40E_Q_VECTOR_HUNG_DETECT, &q_vector->hung_detected);
 	/* Since the actual Tx work is minimal, we can give the Tx a larger
 	 * budget and be more aggressive about cleaning up the Tx descriptors.
 	 */
 	i40e_for_each_ring(ring, q_vector->tx) {
 		clean_complete &= i40e_clean_tx_irq(ring, vsi->work_limit);
-		arm_wb |= ring->arm_wb;
+		arm_wb = arm_wb || ring->arm_wb;
 		ring->arm_wb = false;
 	}
 
@@ -1926,8 +1936,10 @@ int i40e_napi_poll(struct napi_struct *napi, int budget)
 	/* If work not completed, return budget and polling will return */
 	if (!clean_complete) {
 tx_only:
-		if (arm_wb)
+		if (arm_wb) {
+			q_vector->tx.ring[0].tx_stats.tx_force_wb++;
 			i40e_force_wb(vsi, q_vector);
+		}
 		return budget;
 	}
 
@@ -1993,7 +2005,7 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	if (!(tx_flags & (I40E_TX_FLAGS_IPV4 | I40E_TX_FLAGS_IPV6)))
 		return;
 
-	if (!(tx_flags & I40E_TX_FLAGS_VXLAN_TUNNEL)) {
+	if (!(tx_flags & I40E_TX_FLAGS_UDP_TUNNEL)) {
 		/* snag network header to get L4 type and address */
 		hdr.network = skb_network_header(skb);
 
@@ -2078,7 +2090,7 @@ static void i40e_atr(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		     I40E_TXD_FLTR_QW1_FD_STATUS_SHIFT;
 
 	dtype_cmd |= I40E_TXD_FLTR_QW1_CNT_ENA_MASK;
-	if (!(tx_flags & I40E_TX_FLAGS_VXLAN_TUNNEL))
+	if (!(tx_flags & I40E_TX_FLAGS_UDP_TUNNEL))
 		dtype_cmd |=
 			((u32)I40E_FD_ATR_STAT_IDX(pf->hw.pf_id) <<
 			I40E_TXD_FLTR_QW1_CNTINDEX_SHIFT) &
@@ -2187,14 +2199,12 @@ out:
  * @tx_ring:  ptr to the ring to send
  * @skb:      ptr to the skb we're sending
  * @hdr_len:  ptr to the size of the packet header
- * @cd_type_cmd_tso_mss: ptr to u64 object
- * @cd_tunneling: ptr to context descriptor bits
+ * @cd_type_cmd_tso_mss: Quad Word 1
  *
  * Returns 0 if no TSO can happen, 1 if tso is going, or error
  **/
 static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss,
-		    u32 *cd_tunneling)
+		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss)
 {
 	u32 cd_cmd, cd_tso_len, cd_mss;
 	struct ipv6hdr *ipv6h;
@@ -2247,7 +2257,7 @@ static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
  * @tx_ring:  ptr to the ring to send
  * @skb:      ptr to the skb we're sending
  * @tx_flags: the collected send information
- * @cd_type_cmd_tso_mss: ptr to u64 object
+ * @cd_type_cmd_tso_mss: Quad Word 1
  *
  * Returns 0 if no Tx timestamp can happen and 1 if the timestamp will happen
  **/
@@ -2303,8 +2313,8 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 *tx_flags,
 	struct iphdr *this_ip_hdr;
 	u32 network_hdr_len;
 	u8 l4_hdr = 0;
-	struct udphdr *oudph;
-	struct iphdr *oiph;
+	struct udphdr *oudph = NULL;
+	struct iphdr *oiph = NULL;
 	u32 l4_tunnel = 0;
 
 	if (skb->encapsulation) {
@@ -2313,7 +2323,7 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 *tx_flags,
 			oudph = udp_hdr(skb);
 			oiph = ip_hdr(skb);
 			l4_tunnel = I40E_TXD_CTX_UDP_TUNNELING;
-			*tx_flags |= I40E_TX_FLAGS_VXLAN_TUNNEL;
+			*tx_flags |= I40E_TX_FLAGS_UDP_TUNNEL;
 			break;
 		case IPPROTO_GRE:
 			l4_tunnel = I40E_TXD_CTX_GRE_TUNNELING;
@@ -2807,6 +2817,9 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	int tsyn;
 	int tso;
 
+	/* prefetch the data, we'll need it later */
+	prefetch(skb->data);
+
 	if (0 == i40e_xmit_descriptor_count(skb, tx_ring))
 		return NETDEV_TX_BUSY;
 
@@ -2826,8 +2839,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (protocol == htons(ETH_P_IPV6))
 		tx_flags |= I40E_TX_FLAGS_IPV6;
 
-	tso = i40e_tso(tx_ring, skb, &hdr_len,
-		       &cd_type_cmd_tso_mss, &cd_tunneling);
+	tso = i40e_tso(tx_ring, skb, &hdr_len, &cd_type_cmd_tso_mss);
 
 	if (tso < 0)
 		goto out_drop;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index 6779fb771d6a..3f081e25e097 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -163,7 +163,7 @@ enum i40e_dyn_idx_t {
 #define I40E_TX_FLAGS_FSO		BIT(7)
 #define I40E_TX_FLAGS_TSYN		BIT(8)
 #define I40E_TX_FLAGS_FD_SB		BIT(9)
-#define I40E_TX_FLAGS_VXLAN_TUNNEL	BIT(10)
+#define I40E_TX_FLAGS_UDP_TUNNEL	BIT(10)
 #define I40E_TX_FLAGS_VLAN_MASK		0xffff0000
 #define I40E_TX_FLAGS_VLAN_PRIO_MASK	0xe0000000
 #define I40E_TX_FLAGS_VLAN_PRIO_SHIFT	29
@@ -202,6 +202,7 @@ struct i40e_tx_queue_stats {
 	u64 tx_busy;
 	u64 tx_done_old;
 	u64 tx_linearize;
+	u64 tx_force_wb;
 };
 
 struct i40e_rx_queue_stats {
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
index ae879826084b..3226946bf3d4 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl.h
@@ -153,6 +153,7 @@ struct i40e_virtchnl_vsi_resource {
 #define I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR	0x00000020
 #define I40E_VIRTCHNL_VF_OFFLOAD_VLAN		0x00010000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
+#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
 
 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 44462b40f2d7..63e62f9aec6e 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -290,8 +290,8 @@ static void i40e_config_irq_link_list(struct i40e_vf *vf, u16 vsi_id,
 	next_q = find_first_bit(&linklistmap,
 				(I40E_MAX_VSI_QP *
 				 I40E_VIRTCHNL_SUPPORTED_QTYPES));
-	vsi_queue_id = next_q/I40E_VIRTCHNL_SUPPORTED_QTYPES;
-	qtype = next_q%I40E_VIRTCHNL_SUPPORTED_QTYPES;
+	vsi_queue_id = next_q / I40E_VIRTCHNL_SUPPORTED_QTYPES;
+	qtype = next_q % I40E_VIRTCHNL_SUPPORTED_QTYPES;
 	pf_queue_id = i40e_vc_get_pf_queue_id(vf, vsi_id, vsi_queue_id);
 	reg = ((qtype << I40E_VPINT_LNKLSTN_FIRSTQ_TYPE_SHIFT) | pf_queue_id);
 
@@ -549,12 +549,15 @@ static int i40e_alloc_vsi_res(struct i40e_vf *vf, enum i40e_vsi_type type)
 			i40e_vsi_add_pvid(vsi, vf->port_vlan_id);
 
 		spin_lock_bh(&vsi->mac_filter_list_lock);
-		f = i40e_add_filter(vsi, vf->default_lan_addr.addr,
-				    vf->port_vlan_id ? vf->port_vlan_id : -1,
-				    true, false);
-		if (!f)
-			dev_info(&pf->pdev->dev,
-				 "Could not allocate VF MAC addr\n");
+		if (is_valid_ether_addr(vf->default_lan_addr.addr)) {
+			f = i40e_add_filter(vsi, vf->default_lan_addr.addr,
+				       vf->port_vlan_id ? vf->port_vlan_id : -1,
+				       true, false);
+			if (!f)
+				dev_info(&pf->pdev->dev,
+					 "Could not add MAC filter %pM for VF %d\n",
+					vf->default_lan_addr.addr, vf->vf_id);
+		}
 		f = i40e_add_filter(vsi, brdcast,
 				    vf->port_vlan_id ? vf->port_vlan_id : -1,
 				    true, false);
@@ -565,7 +568,7 @@ static int i40e_alloc_vsi_res(struct i40e_vf *vf, enum i40e_vsi_type type)
 	}
 
 	/* program mac filter */
-	ret = i40e_sync_vsi_filters(vsi, false);
+	ret = i40e_sync_vsi_filters(vsi);
 	if (ret)
 		dev_err(&pf->pdev->dev, "Unable to program ucast filters\n");
 
@@ -1094,8 +1097,8 @@ static int i40e_vc_send_msg_to_vf(struct i40e_vf *vf, u32 v_opcode,
 	/* single place to detect unsuccessful return values */
 	if (v_retval) {
 		vf->num_invalid_msgs++;
-		dev_err(&pf->pdev->dev, "Failed opcode %d Error: %d\n",
-			v_opcode, v_retval);
+		dev_err(&pf->pdev->dev, "VF %d failed opcode %d, error: %d\n",
+			vf->vf_id, v_opcode, v_retval);
 		if (vf->num_invalid_msgs >
 		    I40E_DEFAULT_NUM_INVALID_MSGS_ALLOWED) {
 			dev_err(&pf->pdev->dev,
@@ -1623,7 +1626,8 @@ static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 		if (!f) {
 			dev_err(&pf->pdev->dev,
-				"Unable to add VF MAC filter\n");
+				"Unable to add MAC filter %pM for VF %d\n",
+				 al->list[i].addr, vf->vf_id);
 			ret = I40E_ERR_PARAM;
 			spin_unlock_bh(&vsi->mac_filter_list_lock);
 			goto error_param;
@@ -1632,8 +1636,10 @@ static int i40e_vc_add_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	spin_unlock_bh(&vsi->mac_filter_list_lock);
 
 	/* program the updated filter list */
-	if (i40e_sync_vsi_filters(vsi, false))
-		dev_err(&pf->pdev->dev, "Unable to program VF MAC filters\n");
+	ret = i40e_sync_vsi_filters(vsi);
+	if (ret)
+		dev_err(&pf->pdev->dev, "Unable to program VF %d MAC filters, error %d\n",
+			vf->vf_id, ret);
 
 error_param:
 	/* send the response to the VF */
@@ -1669,8 +1675,8 @@ static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	for (i = 0; i < al->num_elements; i++) {
 		if (is_broadcast_ether_addr(al->list[i].addr) ||
 		    is_zero_ether_addr(al->list[i].addr)) {
-			dev_err(&pf->pdev->dev, "invalid VF MAC addr %pM\n",
-				al->list[i].addr);
+			dev_err(&pf->pdev->dev, "Invalid MAC addr %pM for VF %d\n",
+				al->list[i].addr, vf->vf_id);
 			ret = I40E_ERR_INVALID_MAC_ADDR;
 			goto error_param;
 		}
@@ -1680,13 +1686,19 @@ static int i40e_vc_del_mac_addr_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	spin_lock_bh(&vsi->mac_filter_list_lock);
 	/* delete addresses from the list */
 	for (i = 0; i < al->num_elements; i++)
-		i40e_del_filter(vsi, al->list[i].addr,
-				I40E_VLAN_ANY, true, false);
+		if (i40e_del_mac_all_vlan(vsi, al->list[i].addr, true, false)) {
+			ret = I40E_ERR_INVALID_MAC_ADDR;
+			spin_unlock_bh(&vsi->mac_filter_list_lock);
+			goto error_param;
+		}
+
 	spin_unlock_bh(&vsi->mac_filter_list_lock);
 
 	/* program the updated filter list */
-	if (i40e_sync_vsi_filters(vsi, false))
-		dev_err(&pf->pdev->dev, "Unable to program VF MAC filters\n");
+	ret = i40e_sync_vsi_filters(vsi);
+	if (ret)
+		dev_err(&pf->pdev->dev, "Unable to program VF %d MAC filters, error %d\n",
+			vf->vf_id, ret);
 
 error_param:
 	/* send the response to the VF */
@@ -1740,8 +1752,8 @@ static int i40e_vc_add_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 		if (ret)
 			dev_err(&pf->pdev->dev,
-				"Unable to add VF vlan filter %d, error %d\n",
-				vfl->vlan_id[i], ret);
+				"Unable to add VLAN filter %d for VF %d, error %d\n",
+				vfl->vlan_id[i], vf->vf_id, ret);
 	}
 
 error_param:
@@ -1792,8 +1804,8 @@ static int i40e_vc_remove_vlan_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 
 		if (ret)
 			dev_err(&pf->pdev->dev,
-				"Unable to delete VF vlan filter %d, error %d\n",
-				vfl->vlan_id[i], ret);
+				"Unable to delete VLAN filter %d for VF %d, error %d\n",
+				vfl->vlan_id[i], vf->vf_id, ret);
 	}
 
 error_param:
@@ -2066,15 +2078,15 @@ int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 	vf = &(pf->vf[vf_id]);
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
-		dev_err(&pf->pdev->dev,
-			"Uninitialized VF %d\n", vf_id);
-		ret = -EINVAL;
+		dev_err(&pf->pdev->dev, "VF %d still in reset. Try again.\n",
+			vf_id);
+		ret = -EAGAIN;
 		goto error_param;
 	}
 
-	if (!is_valid_ether_addr(mac)) {
+	if (is_multicast_ether_addr(mac)) {
 		dev_err(&pf->pdev->dev,
-			"Invalid VF ethernet address\n");
+			"Invalid Ethernet address %pM for VF %d\n", mac, vf_id);
 		ret = -EINVAL;
 		goto error_param;
 	}
@@ -2085,9 +2097,10 @@ int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 	spin_lock_bh(&vsi->mac_filter_list_lock);
 
 	/* delete the temporary mac address */
-	i40e_del_filter(vsi, vf->default_lan_addr.addr,
-			vf->port_vlan_id ? vf->port_vlan_id : -1,
-			true, false);
+	if (!is_zero_ether_addr(vf->default_lan_addr.addr))
+		i40e_del_filter(vsi, vf->default_lan_addr.addr,
+				vf->port_vlan_id ? vf->port_vlan_id : -1,
+				true, false);
 
 	/* Delete all the filters for this VSI - we're going to kill it
 	 * anyway.
@@ -2099,7 +2112,7 @@ int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 
 	dev_info(&pf->pdev->dev, "Setting MAC %pM on VF %d\n", mac, vf_id);
 	/* program mac filter */
-	if (i40e_sync_vsi_filters(vsi, false)) {
+	if (i40e_sync_vsi_filters(vsi)) {
 		dev_err(&pf->pdev->dev, "Unable to program ucast filters\n");
 		ret = -EIO;
 		goto error_param;
@@ -2150,8 +2163,9 @@ int i40e_ndo_set_vf_port_vlan(struct net_device *netdev,
 	vf = &(pf->vf[vf_id]);
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
-		dev_err(&pf->pdev->dev, "Uninitialized VF %d\n", vf_id);
-		ret = -EINVAL;
+		dev_err(&pf->pdev->dev, "VF %d still in reset. Try again.\n",
+			vf_id);
+		ret = -EAGAIN;
 		goto error_pvid;
 	}
 
@@ -2270,8 +2284,9 @@ int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 	vf = &(pf->vf[vf_id]);
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
-		dev_err(&pf->pdev->dev, "Uninitialized VF %d.\n", vf_id);
-		ret = -EINVAL;
+		dev_err(&pf->pdev->dev, "VF %d still in reset. Try again.\n",
+			vf_id);
+		ret = -EAGAIN;
 		goto error;
 	}
 
@@ -2344,8 +2359,9 @@ int i40e_ndo_get_vf_config(struct net_device *netdev,
 	/* first vsi is always the LAN vsi */
 	vsi = pf->vsi[vf->lan_vsi_idx];
 	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
-		dev_err(&pf->pdev->dev, "Uninitialized VF %d\n", vf_id);
-		ret = -EINVAL;
+		dev_err(&pf->pdev->dev, "VF %d still in reset. Try again.\n",
+			vf_id);
+		ret = -EAGAIN;
 		goto error_param;
 	}
 
@@ -2460,6 +2476,12 @@ int i40e_ndo_set_vf_spoofchk(struct net_device *netdev, int vf_id, bool enable)
 	}
 
 	vf = &(pf->vf[vf_id]);
+	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states)) {
+		dev_err(&pf->pdev->dev, "VF %d still in reset. Try again.\n",
+			vf_id);
+		ret = -EAGAIN;
+		goto out;
+	}
 
 	if (enable == vf->spoofchk)
 		goto out;
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
index fcb9ef34cc7a..f5b2b369dc7c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_adminq_cmd.h
@@ -227,6 +227,7 @@ enum i40e_admin_queue_opc {
 	i40e_aqc_opc_nvm_update			= 0x0703,
 	i40e_aqc_opc_nvm_config_read		= 0x0704,
 	i40e_aqc_opc_nvm_config_write		= 0x0705,
+	i40e_aqc_opc_oem_post_update		= 0x0720,
 
 	/* virtualization commands */
 	i40e_aqc_opc_send_msg_to_pf		= 0x0801,
@@ -1888,6 +1889,26 @@ struct i40e_aqc_nvm_config_data_immediate_field {
 
 I40E_CHECK_STRUCT_LEN(0xc, i40e_aqc_nvm_config_data_immediate_field);
 
+/* OEM Post Update (indirect 0x0720)
+ * no command data struct used
+ */
+ struct i40e_aqc_nvm_oem_post_update {
+#define I40E_AQ_NVM_OEM_POST_UPDATE_EXTERNAL_DATA	0x01
+	u8 sel_data;
+	u8 reserved[7];
+};
+
+I40E_CHECK_STRUCT_LEN(0x8, i40e_aqc_nvm_oem_post_update);
+
+struct i40e_aqc_nvm_oem_post_update_buffer {
+	u8 str_len;
+	u8 dev_addr;
+	__le16 eeprom_addr;
+	u8 data[36];
+};
+
+I40E_CHECK_STRUCT_LEN(0x28, i40e_aqc_nvm_oem_post_update_buffer);
+
 /* Send to PF command (indirect 0x0801) id is only used by PF
  * Send to VF command (indirect 0x0802) id is only used by PF
  * Send to Peer PF command (indirect 0x0803)
@@ -2311,4 +2332,4 @@ struct i40e_aqc_debug_modify_internals {
 
 I40E_CHECK_CMD_LENGTH(i40e_aqc_debug_modify_internals);
 
-#endif
+#endif /* _I40E_ADMINQ_CMD_H_ */
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 72b1942a94aa..938783e0baac 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -44,7 +44,6 @@ i40e_status i40e_set_mac_type(struct i40e_hw *hw)
 		switch (hw->device_id) {
 		case I40E_DEV_ID_SFP_XL710:
 		case I40E_DEV_ID_QEMU:
-		case I40E_DEV_ID_KX_A:
 		case I40E_DEV_ID_KX_B:
 		case I40E_DEV_ID_KX_C:
 		case I40E_DEV_ID_QSFP_A:
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_devids.h b/drivers/net/ethernet/intel/i40evf/i40e_devids.h
index e6a39c9862e8..ca8b58c3d1f5 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_devids.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_devids.h
@@ -30,7 +30,6 @@
 /* Device IDs */
 #define I40E_DEV_ID_SFP_XL710		0x1572
 #define I40E_DEV_ID_QEMU		0x1574
-#define I40E_DEV_ID_KX_A		0x157F
 #define I40E_DEV_ID_KX_B		0x1580
 #define I40E_DEV_ID_KX_C		0x1581
 #define I40E_DEV_ID_QSFP_A		0x1583
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index 47e9a90d6b10..7a00657dacda 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -51,11 +51,7 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 					    struct i40e_tx_buffer *tx_buffer)
 {
 	if (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);
-
+		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),
@@ -67,6 +63,10 @@ static void i40e_unmap_and_free_tx_resource(struct i40e_ring *ring,
 			       dma_unmap_len(tx_buffer, len),
 			       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);
@@ -127,17 +127,24 @@ void i40evf_free_tx_resources(struct i40e_ring *tx_ring)
 }
 
 /**
- * i40e_get_head - Retrieve head from head writeback
- * @tx_ring:  tx ring to fetch head of
+ * i40evf_get_tx_pending - how many Tx descriptors not processed
+ * @tx_ring: the ring of descriptors
  *
- * Returns value of Tx ring head based on value stored
- * in head write-back location
+ * Since there is no access to the ring head register
+ * in XL710, we need to use our local copies
  **/
-static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
+u32 i40evf_get_tx_pending(struct i40e_ring *ring)
 {
-	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
+	u32 head, tail;
+
+	head = i40e_get_head(ring);
+	tail = readl(ring->tail);
 
-	return le32_to_cpu(*(volatile __le32 *)head);
+	if (head != tail)
+		return (head < tail) ?
+			tail - head : (tail + ring->count - head);
+
+	return 0;
 }
 
 #define WB_STRIDE 0x3
@@ -245,16 +252,6 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 	tx_ring->q_vector->tx.total_bytes += total_bytes;
 	tx_ring->q_vector->tx.total_packets += total_packets;
 
-	/* check to see if there are any non-cache aligned descriptors
-	 * waiting to be written back, and kick the hardware to force
-	 * them to be written back in case of napi polling
-	 */
-	if (budget &&
-	    !((i & WB_STRIDE) == WB_STRIDE) &&
-	    !test_bit(__I40E_DOWN, &tx_ring->vsi->state) &&
-	    (I40E_DESC_UNUSED(tx_ring) != tx_ring->count))
-		tx_ring->arm_wb = true;
-
 	netdev_tx_completed_queue(netdev_get_tx_queue(tx_ring->netdev,
 						      tx_ring->queue_index),
 				  total_packets, total_bytes);
@@ -414,7 +411,7 @@ static bool i40e_set_new_dynamic_itr(struct i40e_ring_container *rc)
 	return false;
 }
 
-/*
+/**
  * i40evf_setup_tx_descriptors - Allocate the Tx descriptors
  * @tx_ring: the tx ring to set up
  *
@@ -889,31 +886,12 @@ checksum_fail:
 }
 
 /**
- * i40e_rx_hash - returns the hash value from the Rx descriptor
- * @ring: descriptor ring
- * @rx_desc: specific descriptor
- **/
-static inline u32 i40e_rx_hash(struct i40e_ring *ring,
-			       union i40e_rx_desc *rx_desc)
-{
-	const __le64 rss_mask =
-		cpu_to_le64((u64)I40E_RX_DESC_FLTSTAT_RSS_HASH <<
-			    I40E_RX_DESC_STATUS_FLTSTAT_SHIFT);
-
-	if ((ring->netdev->features & NETIF_F_RXHASH) &&
-	    (rx_desc->wb.qword1.status_error_len & rss_mask) == rss_mask)
-		return le32_to_cpu(rx_desc->wb.qword0.hi_dword.rss);
-	else
-		return 0;
-}
-
-/**
- * i40e_ptype_to_hash - get a hash type
+ * i40e_ptype_to_htype - get a hash type
  * @ptype: the ptype value from the descriptor
  *
  * Returns a hash type to be used by skb_set_hash
  **/
-static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
+static inline enum pkt_hash_types i40e_ptype_to_htype(u8 ptype)
 {
 	struct i40e_rx_ptype_decoded decoded = decode_rx_desc_ptype(ptype);
 
@@ -931,6 +909,30 @@ static inline enum pkt_hash_types i40e_ptype_to_hash(u8 ptype)
 }
 
 /**
+ * i40e_rx_hash - set the hash value in the skb
+ * @ring: descriptor ring
+ * @rx_desc: specific descriptor
+ **/
+static inline void i40e_rx_hash(struct i40e_ring *ring,
+				union i40e_rx_desc *rx_desc,
+				struct sk_buff *skb,
+				u8 rx_ptype)
+{
+	u32 hash;
+	const __le64 rss_mask  =
+		cpu_to_le64((u64)I40E_RX_DESC_FLTSTAT_RSS_HASH <<
+			    I40E_RX_DESC_STATUS_FLTSTAT_SHIFT);
+
+	if (ring->netdev->features & NETIF_F_RXHASH)
+		return;
+
+	if ((rx_desc->wb.qword1.status_error_len & rss_mask) == rss_mask) {
+		hash = le32_to_cpu(rx_desc->wb.qword0.hi_dword.rss);
+		skb_set_hash(skb, hash, i40e_ptype_to_htype(rx_ptype));
+	}
+}
+
+/**
  * i40e_clean_rx_irq_ps - Reclaim resources after receive; packet split
  * @rx_ring:  rx ring to clean
  * @budget:   how many cleans we're allowed
@@ -1071,8 +1073,8 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 
-		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
-			     i40e_ptype_to_hash(rx_ptype));
+		i40e_rx_hash(rx_ring, rx_desc, skb, rx_ptype);
+
 		/* probably a little skewed due to removing CRC */
 		total_rx_bytes += skb->len;
 		total_rx_packets++;
@@ -1090,7 +1092,6 @@ static int i40e_clean_rx_irq_ps(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 #endif
-		skb_mark_napi_id(skb, &rx_ring->q_vector->napi);
 		i40e_receive_skb(rx_ring, skb, vlan_tag);
 
 		rx_desc->wb.qword1.status_error_len = 0;
@@ -1189,8 +1190,7 @@ static int i40e_clean_rx_irq_1buf(struct i40e_ring *rx_ring, int budget)
 			continue;
 		}
 
-		skb_set_hash(skb, i40e_rx_hash(rx_ring, rx_desc),
-			     i40e_ptype_to_hash(rx_ptype));
+		i40e_rx_hash(rx_ring, rx_desc, skb, rx_ptype);
 		/* probably a little skewed due to removing CRC */
 		total_rx_bytes += skb->len;
 		total_rx_packets++;
@@ -1263,10 +1263,12 @@ static inline void i40e_update_enable_itr(struct i40e_vsi *vsi,
 		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)) {
 		tx = i40e_set_new_dynamic_itr(&q_vector->tx);
 		txval = i40e_buildreg_itr(I40E_TX_ITR, q_vector->tx.itr);
 	}
+
 	if (rx || tx) {
 		/* get the higher of the two ITR adjustments and
 		 * use the same value for both ITR registers
@@ -1302,7 +1304,6 @@ enable_int:
 		q_vector->itr_countdown--;
 	else
 		q_vector->itr_countdown = ITR_COUNTDOWN_START;
-
 }
 
 /**
@@ -1335,7 +1336,7 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
 	 */
 	i40e_for_each_ring(ring, q_vector->tx) {
 		clean_complete &= i40e_clean_tx_irq(ring, vsi->work_limit);
-		arm_wb |= ring->arm_wb;
+		arm_wb = arm_wb || ring->arm_wb;
 		ring->arm_wb = false;
 	}
 
@@ -1364,8 +1365,10 @@ int i40evf_napi_poll(struct napi_struct *napi, int budget)
 	/* If work not completed, return budget and polling will return */
 	if (!clean_complete) {
 tx_only:
-		if (arm_wb)
+		if (arm_wb) {
+			q_vector->tx.ring[0].tx_stats.tx_force_wb++;
 			i40evf_force_wb(vsi, q_vector);
+		}
 		return budget;
 	}
 
@@ -1437,13 +1440,12 @@ out:
  * @tx_ring:  ptr to the ring to send
  * @skb:      ptr to the skb we're sending
  * @hdr_len:  ptr to the size of the packet header
- * @cd_tunneling: ptr to context descriptor bits
+ * @cd_type_cmd_tso_mss: Quad Word 1
  *
  * Returns 0 if no TSO can happen, 1 if tso is going, or error
  **/
 static int i40e_tso(struct i40e_ring *tx_ring, struct sk_buff *skb,
-		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss,
-		    u32 *cd_tunneling)
+		    u8 *hdr_len, u64 *cd_type_cmd_tso_mss)
 {
 	u32 cd_cmd, cd_tso_len, cd_mss;
 	struct ipv6hdr *ipv6h;
@@ -1555,7 +1557,6 @@ static void i40e_tx_enable_csum(struct sk_buff *skb, u32 *tx_flags,
 			*tx_flags |= I40E_TX_FLAGS_IPV6;
 		}
 
-
 		if ((tx_ring->flags & I40E_TXR_FLAGS_OUTER_UDP_CSUM) &&
 		    (l4_tunnel == I40E_TXD_CTX_UDP_TUNNELING)        &&
 		    (*cd_tunneling & I40E_TXD_CTX_QW0_EXT_IP_MASK)) {
@@ -1654,7 +1655,7 @@ static void i40e_create_tx_ctx(struct i40e_ring *tx_ring,
 	context_desc->type_cmd_tso_mss = cpu_to_le64(cd_type_cmd_tso_mss);
 }
 
- /**
+/**
  * i40e_chk_linearize - Check if there are more than 8 fragments per packet
  * @skb:      send buffer
  * @tx_flags: collected send information
@@ -1770,6 +1771,9 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 	u32 td_tag = 0;
 	dma_addr_t dma;
 	u16 gso_segs;
+	u16 desc_count = 0;
+	bool tail_bump = true;
+	bool do_rs = false;
 
 	if (tx_flags & I40E_TX_FLAGS_HW_VLAN) {
 		td_cmd |= I40E_TX_DESC_CMD_IL2TAG1;
@@ -1810,6 +1814,8 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 			tx_desc++;
 			i++;
+			desc_count++;
+
 			if (i == tx_ring->count) {
 				tx_desc = I40E_TX_DESC(tx_ring, 0);
 				i = 0;
@@ -1829,6 +1835,8 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 		tx_desc++;
 		i++;
+		desc_count++;
+
 		if (i == tx_ring->count) {
 			tx_desc = I40E_TX_DESC(tx_ring, 0);
 			i = 0;
@@ -1843,35 +1851,6 @@ static inline void i40evf_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 		tx_bi = &tx_ring->tx_bi[i];
 	}
 
-	/* Place RS bit on last descriptor of any packet that spans across the
-	 * 4th descriptor (WB_STRIDE aka 0x3) in a 64B cacheline.
-	 */
-#define WB_STRIDE 0x3
-	if (((i & WB_STRIDE) != WB_STRIDE) &&
-	    (first <= &tx_ring->tx_bi[i]) &&
-	    (first >= &tx_ring->tx_bi[i & ~WB_STRIDE])) {
-		tx_desc->cmd_type_offset_bsz =
-			build_ctob(td_cmd, td_offset, size, td_tag) |
-			cpu_to_le64((u64)I40E_TX_DESC_CMD_EOP <<
-					 I40E_TXD_QW1_CMD_SHIFT);
-	} else {
-		tx_desc->cmd_type_offset_bsz =
-			build_ctob(td_cmd, td_offset, size, td_tag) |
-			cpu_to_le64((u64)I40E_TXD_CMD <<
-					 I40E_TXD_QW1_CMD_SHIFT);
-	}
-
-	netdev_tx_sent_queue(netdev_get_tx_queue(tx_ring->netdev,
-						 tx_ring->queue_index),
-			     first->bytecount);
-
-	/* Force memory writes to complete before letting h/w
-	 * know there are new descriptors to fetch.  (Only
-	 * applicable for weak-ordered memory model archs,
-	 * such as IA-64).
-	 */
-	wmb();
-
 	/* set next_to_watch value indicating a packet is present */
 	first->next_to_watch = tx_desc;
 
@@ -1881,15 +1860,72 @@ 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);
 	i40evf_maybe_stop_tx(tx_ring, DESC_NEEDED);
+
+	/* Algorithm to optimize tail and RS bit setting:
+	 * if xmit_more is supported
+	 *	if xmit_more is true
+	 *		do not update tail and do not mark RS bit.
+	 *	if xmit_more is false and last xmit_more was false
+	 *		if every packet spanned less than 4 desc
+	 *			then set RS bit on 4th packet and update tail
+	 *			on every packet
+	 *		else
+	 *			update tail and set RS bit on every packet.
+	 *	if xmit_more is false and last_xmit_more was true
+	 *		update tail and set RS bit.
+	 *
+	 * Optimization: wmb to be issued only in case of tail update.
+	 * Also optimize the Descriptor WB path for RS bit with the same
+	 * algorithm.
+	 *
+	 * Note: If there are less than 4 packets
+	 * pending and interrupts were disabled the service task will
+	 * trigger a force WB.
+	 */
+	if (skb->xmit_more  &&
+	    !netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
+						    tx_ring->queue_index))) {
+		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)) &&
+		   (!(tx_ring->flags & I40E_TXR_FLAGS_LAST_XMIT_MORE_SET)) &&
+		   (tx_ring->packet_stride < WB_STRIDE) &&
+		   (desc_count < WB_STRIDE)) {
+		tx_ring->packet_stride++;
+	} else {
+		tx_ring->packet_stride = 0;
+		tx_ring->flags &= ~I40E_TXR_FLAGS_LAST_XMIT_MORE_SET;
+		do_rs = true;
+	}
+	if (do_rs)
+		tx_ring->packet_stride = 0;
+
+	tx_desc->cmd_type_offset_bsz =
+			build_ctob(td_cmd, td_offset, size, td_tag) |
+			cpu_to_le64((u64)(do_rs ? I40E_TXD_CMD :
+						  I40E_TX_DESC_CMD_EOP) <<
+						  I40E_TXD_QW1_CMD_SHIFT);
+
 	/* notify HW of packet */
-	if (!skb->xmit_more ||
-	    netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
-						   tx_ring->queue_index)))
-		writel(i, tx_ring->tail);
-	else
+	if (!tail_bump)
 		prefetchw(tx_desc + 1);
 
+	if (tail_bump) {
+		/* Force memory writes to complete before letting h/w
+		 * know there are new descriptors to fetch.  (Only
+		 * applicable for weak-ordered memory model archs,
+		 * such as IA-64).
+		 */
+		wmb();
+		writel(i, tx_ring->tail);
+	}
+
 	return;
 
 dma_error:
@@ -1961,6 +1997,9 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	u8 hdr_len = 0;
 	int tso;
 
+	/* prefetch the data, we'll need it later */
+	prefetch(skb->data);
+
 	if (0 == i40evf_xmit_descriptor_count(skb, tx_ring))
 		return NETDEV_TX_BUSY;
 
@@ -1980,8 +2019,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	else if (protocol == htons(ETH_P_IPV6))
 		tx_flags |= I40E_TX_FLAGS_IPV6;
 
-	tso = i40e_tso(tx_ring, skb, &hdr_len,
-		       &cd_type_cmd_tso_mss, &cd_tunneling);
+	tso = i40e_tso(tx_ring, skb, &hdr_len, &cd_type_cmd_tso_mss);
 
 	if (tso < 0)
 		goto out_drop;
@@ -2029,7 +2067,7 @@ out_drop:
 netdev_tx_t i40evf_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
 {
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
-	struct i40e_ring *tx_ring = adapter->tx_rings[skb->queue_mapping];
+	struct i40e_ring *tx_ring = &adapter->tx_rings[skb->queue_mapping];
 
 	/* hardware can't handle really short frames, hardware padding works
 	 * beyond this point
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index ebc1bf77f036..e29bb3e86cfd 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -201,6 +201,7 @@ struct i40e_tx_queue_stats {
 	u64 tx_busy;
 	u64 tx_done_old;
 	u64 tx_linearize;
+	u64 tx_force_wb;
 };
 
 struct i40e_rx_queue_stats {
@@ -267,6 +268,8 @@ struct i40e_ring {
 
 	bool ring_active;		/* is ring online or not */
 	bool arm_wb;		/* do something to arm write back */
+	u8 packet_stride;
+#define I40E_TXR_FLAGS_LAST_XMIT_MORE_SET BIT(2)
 
 	u16 flags;
 #define I40E_TXR_FLAGS_WB_ON_ITR	BIT(0)
@@ -321,4 +324,19 @@ int i40evf_setup_rx_descriptors(struct i40e_ring *rx_ring);
 void i40evf_free_tx_resources(struct i40e_ring *tx_ring);
 void i40evf_free_rx_resources(struct i40e_ring *rx_ring);
 int i40evf_napi_poll(struct napi_struct *napi, int budget);
+u32 i40evf_get_tx_pending(struct i40e_ring *ring);
+
+/**
+ * i40e_get_head - Retrieve head from head writeback
+ * @tx_ring: Tx ring to fetch head of
+ *
+ * Returns value of Tx ring head based on value stored
+ * in head write-back location
+ **/
+static inline u32 i40e_get_head(struct i40e_ring *tx_ring)
+{
+	void *head = (struct i40e_tx_desc *)tx_ring->desc + tx_ring->count;
+
+	return le32_to_cpu(*(volatile __le32 *)head);
+}
 #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 9f7b279b9d9c..3b9d2037456c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_virtchnl.h
@@ -153,6 +153,7 @@ struct i40e_virtchnl_vsi_resource {
 #define I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR	0x00000020
 #define I40E_VIRTCHNL_VF_OFFLOAD_VLAN		0x00010000
 #define I40E_VIRTCHNL_VF_OFFLOAD_RX_POLLING	0x00020000
+#define I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2	0x00040000
 
 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 22fc3d49c4b9..be1b72b93888 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf.h
+++ b/drivers/net/ethernet/intel/i40evf/i40evf.h
@@ -67,6 +67,8 @@ struct i40e_vsi {
 	u16 rx_itr_setting;
 	u16 tx_itr_setting;
 	u16 qs_handle;
+	u8 *rss_hkey_user; /* User configured hash keys */
+	u8 *rss_lut_user;  /* User configured lookup table entries */
 };
 
 /* How many Rx Buffers do we bundle into one write to the hardware ? */
@@ -95,10 +97,10 @@ struct i40e_vsi {
 #define I40E_TX_DESC(R, i) (&(((struct i40e_tx_desc *)((R)->desc))[i]))
 #define I40E_TX_CTXTDESC(R, i) \
 	(&(((struct i40e_tx_context_desc *)((R)->desc))[i]))
-#define MAX_RX_QUEUES 8
-#define MAX_TX_QUEUES MAX_RX_QUEUES
+#define MAX_QUEUES 16
 
 #define I40EVF_HKEY_ARRAY_SIZE ((I40E_VFQF_HKEY_MAX_INDEX + 1) * 4)
+#define I40EVF_HLUT_ARRAY_SIZE ((I40E_VFQF_HLUT_MAX_INDEX + 1) * 4)
 
 /* MAX_MSIX_Q_VECTORS of these are allocated,
  * but we only use one per queue-specific vector.
@@ -142,9 +144,6 @@ struct i40e_q_vector {
 #define OTHER_VECTOR 1
 #define NONQ_VECS (OTHER_VECTOR)
 
-#define MAX_MSIX_Q_VECTORS 4
-#define MAX_MSIX_COUNT 5
-
 #define MIN_MSIX_Q_VECTORS 1
 #define MIN_MSIX_COUNT (MIN_MSIX_Q_VECTORS + NONQ_VECS)
 
@@ -190,19 +189,19 @@ struct i40evf_adapter {
 	struct work_struct reset_task;
 	struct work_struct adminq_task;
 	struct delayed_work init_task;
-	struct i40e_q_vector *q_vector[MAX_MSIX_Q_VECTORS];
+	struct i40e_q_vector *q_vectors;
 	struct list_head vlan_filter_list;
 	char misc_vector_name[IFNAMSIZ + 9];
 	int num_active_queues;
 
 	/* TX */
-	struct i40e_ring *tx_rings[I40E_MAX_VSI_QP];
+	struct i40e_ring *tx_rings;
 	u32 tx_timeout_count;
 	struct list_head mac_filter_list;
 	u32 tx_desc_count;
 
 	/* RX */
-	struct i40e_ring *rx_rings[I40E_MAX_VSI_QP];
+	struct i40e_ring *rx_rings;
 	u64 hw_csum_rx_error;
 	u32 rx_desc_count;
 	int num_msix_vectors;
@@ -313,4 +312,8 @@ void i40evf_request_reset(struct i40evf_adapter *adapter);
 void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 				enum i40e_virtchnl_ops v_opcode,
 				i40e_status v_retval, u8 *msg, u16 msglen);
+int i40evf_config_rss(struct i40e_vsi *vsi, const u8 *seed, u8 *lut,
+		      u16 lut_size);
+int i40evf_get_rss(struct i40e_vsi *vsi, const u8 *seed, u8 *lut,
+		   u16 lut_size);
 #endif /* _I40EVF_H_ */
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
index 4790437a50ac..a4c9feb589e7 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_ethtool.c
@@ -121,12 +121,12 @@ static void i40evf_get_ethtool_stats(struct net_device *netdev,
 		data[i] =  *(u64 *)p;
 	}
 	for (j = 0; j < adapter->num_active_queues; j++) {
-		data[i++] = adapter->tx_rings[j]->stats.packets;
-		data[i++] = adapter->tx_rings[j]->stats.bytes;
+		data[i++] = adapter->tx_rings[j].stats.packets;
+		data[i++] = adapter->tx_rings[j].stats.bytes;
 	}
 	for (j = 0; j < adapter->num_active_queues; j++) {
-		data[i++] = adapter->rx_rings[j]->stats.packets;
-		data[i++] = adapter->rx_rings[j]->stats.bytes;
+		data[i++] = adapter->rx_rings[j].stats.packets;
+		data[i++] = adapter->rx_rings[j].stats.bytes;
 	}
 }
 
@@ -351,7 +351,7 @@ static int i40evf_set_coalesce(struct net_device *netdev,
 		vsi->tx_itr_setting &= ~I40E_ITR_DYNAMIC;
 
 	for (i = 0; i < adapter->num_msix_vectors - NONQ_VECS; i++) {
-		q_vector = adapter->q_vector[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);
@@ -634,25 +634,34 @@ static int i40evf_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 			   u8 *hfunc)
 {
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
-	struct i40e_hw *hw = &adapter->hw;
-	u32 hlut_val;
-	int i, j;
+	struct i40e_vsi *vsi = &adapter->vsi;
+	u8 *seed = NULL, *lut;
+	int ret;
+	u16 i;
 
 	if (hfunc)
 		*hfunc = ETH_RSS_HASH_TOP;
 	if (!indir)
 		return 0;
 
-	if (indir) {
-		for (i = 0, j = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
-			hlut_val = rd32(hw, I40E_VFQF_HLUT(i));
-			indir[j++] = hlut_val & 0xff;
-			indir[j++] = (hlut_val >> 8) & 0xff;
-			indir[j++] = (hlut_val >> 16) & 0xff;
-			indir[j++] = (hlut_val >> 24) & 0xff;
-		}
-	}
-	return 0;
+	seed = key;
+
+	lut = kzalloc(I40EVF_HLUT_ARRAY_SIZE, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
+
+	ret = i40evf_get_rss(vsi, seed, lut, I40EVF_HLUT_ARRAY_SIZE);
+	if (ret)
+		goto out;
+
+	/* Each 32 bits pointed by 'indir' is stored with a lut entry */
+	for (i = 0; i < I40EVF_HLUT_ARRAY_SIZE; i++)
+		indir[i] = (u32)lut[i];
+
+out:
+	kfree(lut);
+
+	return ret;
 }
 
 /**
@@ -668,9 +677,9 @@ static int i40evf_set_rxfh(struct net_device *netdev, const u32 *indir,
 			   const u8 *key, const u8 hfunc)
 {
 	struct i40evf_adapter *adapter = netdev_priv(netdev);
-	struct i40e_hw *hw = &adapter->hw;
-	u32 hlut_val;
-	int i, j;
+	struct i40e_vsi *vsi = &adapter->vsi;
+	u8 *seed = NULL;
+	u16 i;
 
 	/* We do not allow change in unsupported parameters */
 	if (key ||
@@ -679,15 +688,29 @@ static int i40evf_set_rxfh(struct net_device *netdev, const u32 *indir,
 	if (!indir)
 		return 0;
 
-	for (i = 0, j = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
-		hlut_val = indir[j++];
-		hlut_val |= indir[j++] << 8;
-		hlut_val |= indir[j++] << 16;
-		hlut_val |= indir[j++] << 24;
-		wr32(hw, I40E_VFQF_HLUT(i), hlut_val);
+	if (key) {
+		if (!vsi->rss_hkey_user) {
+			vsi->rss_hkey_user = kzalloc(I40EVF_HKEY_ARRAY_SIZE,
+						     GFP_KERNEL);
+			if (!vsi->rss_hkey_user)
+				return -ENOMEM;
+		}
+		memcpy(vsi->rss_hkey_user, key, I40EVF_HKEY_ARRAY_SIZE);
+		seed = vsi->rss_hkey_user;
+	}
+	if (!vsi->rss_lut_user) {
+		vsi->rss_lut_user = kzalloc(I40EVF_HLUT_ARRAY_SIZE,
+					    GFP_KERNEL);
+		if (!vsi->rss_lut_user)
+			return -ENOMEM;
 	}
 
-	return 0;
+	/* Each 32 bits pointed by 'indir' is stored with a lut entry */
+	for (i = 0; i < I40EVF_HLUT_ARRAY_SIZE; i++)
+		vsi->rss_lut_user[i] = (u8)(indir[i]);
+
+	return i40evf_config_rss(vsi, seed, vsi->rss_lut_user,
+				 I40EVF_HLUT_ARRAY_SIZE);
 }
 
 static const struct ethtool_ops i40evf_ethtool_ops = {
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index 99d2cffae0cd..94da913b151d 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -34,7 +34,15 @@ char i40evf_driver_name[] = "i40evf";
 static const char i40evf_driver_string[] =
 	"Intel(R) XL710/X710 Virtual Function Network Driver";
 
-#define DRV_VERSION "1.3.33"
+#define DRV_KERN "-k"
+
+#define DRV_VERSION_MAJOR 1
+#define DRV_VERSION_MINOR 4
+#define DRV_VERSION_BUILD 4
+#define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
+	     __stringify(DRV_VERSION_MINOR) "." \
+	     __stringify(DRV_VERSION_BUILD) \
+	     DRV_KERN
 const char i40evf_driver_version[] = DRV_VERSION;
 static const char i40evf_copyright[] =
 	"Copyright (c) 2013 - 2015 Intel Corporation.";
@@ -259,7 +267,7 @@ static void i40evf_fire_sw_int(struct i40evf_adapter *adapter, u32 mask)
 {
 	struct i40e_hw *hw = &adapter->hw;
 	int i;
-	uint32_t dyn_ctl;
+	u32 dyn_ctl;
 
 	if (mask & 1) {
 		dyn_ctl = rd32(hw, I40E_VFINT_DYN_CTL01);
@@ -307,10 +315,9 @@ static irqreturn_t i40evf_msix_aq(int irq, void *data)
 	struct i40e_hw *hw = &adapter->hw;
 	u32 val;
 
-	/* handle non-queue interrupts */
-	rd32(hw, I40E_VFINT_ICR01);
-	rd32(hw, I40E_VFINT_ICR0_ENA1);
-
+	/* handle non-queue interrupts, these reads clear the registers */
+	val = rd32(hw, I40E_VFINT_ICR01);
+	val = rd32(hw, I40E_VFINT_ICR0_ENA1);
 
 	val = rd32(hw, I40E_VFINT_DYN_CTL01) |
 	      I40E_VFINT_DYN_CTL01_CLEARPBA_MASK;
@@ -348,8 +355,8 @@ static irqreturn_t i40evf_msix_clean_rings(int irq, void *data)
 static void
 i40evf_map_vector_to_rxq(struct i40evf_adapter *adapter, int v_idx, int r_idx)
 {
-	struct i40e_q_vector *q_vector = adapter->q_vector[v_idx];
-	struct i40e_ring *rx_ring = adapter->rx_rings[r_idx];
+	struct i40e_q_vector *q_vector = &adapter->q_vectors[v_idx];
+	struct i40e_ring *rx_ring = &adapter->rx_rings[r_idx];
 
 	rx_ring->q_vector = q_vector;
 	rx_ring->next = q_vector->rx.ring;
@@ -369,8 +376,8 @@ i40evf_map_vector_to_rxq(struct i40evf_adapter *adapter, int v_idx, int r_idx)
 static void
 i40evf_map_vector_to_txq(struct i40evf_adapter *adapter, int v_idx, int t_idx)
 {
-	struct i40e_q_vector *q_vector = adapter->q_vector[v_idx];
-	struct i40e_ring *tx_ring = adapter->tx_rings[t_idx];
+	struct i40e_q_vector *q_vector = &adapter->q_vectors[v_idx];
+	struct i40e_ring *tx_ring = &adapter->tx_rings[t_idx];
 
 	tx_ring->q_vector = q_vector;
 	tx_ring->next = q_vector->tx.ring;
@@ -465,7 +472,7 @@ static void i40evf_netpoll(struct net_device *netdev)
 		return;
 
 	for (i = 0; i < q_vectors; i++)
-		i40evf_msix_clean_rings(0, adapter->q_vector[i]);
+		i40evf_msix_clean_rings(0, &adapter->q_vectors[i]);
 }
 
 #endif
@@ -487,7 +494,7 @@ i40evf_request_traffic_irqs(struct i40evf_adapter *adapter, char *basename)
 	q_vectors = adapter->num_msix_vectors - NONQ_VECS;
 
 	for (vector = 0; vector < q_vectors; vector++) {
-		struct i40e_q_vector *q_vector = adapter->q_vector[vector];
+		struct i40e_q_vector *q_vector = &adapter->q_vectors[vector];
 
 		if (q_vector->tx.ring && q_vector->rx.ring) {
 			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
@@ -532,7 +539,7 @@ free_queue_irqs:
 			adapter->msix_entries[vector + NONQ_VECS].vector,
 			NULL);
 		free_irq(adapter->msix_entries[vector + NONQ_VECS].vector,
-			 adapter->q_vector[vector]);
+			 &adapter->q_vectors[vector]);
 	}
 	return err;
 }
@@ -582,7 +589,7 @@ static void i40evf_free_traffic_irqs(struct i40evf_adapter *adapter)
 		irq_set_affinity_hint(adapter->msix_entries[i+1].vector,
 				      NULL);
 		free_irq(adapter->msix_entries[i+1].vector,
-			 adapter->q_vector[i]);
+			 &adapter->q_vectors[i]);
 	}
 }
 
@@ -611,7 +618,7 @@ static void i40evf_configure_tx(struct i40evf_adapter *adapter)
 	int i;
 
 	for (i = 0; i < adapter->num_active_queues; i++)
-		adapter->tx_rings[i]->tail = hw->hw_addr + I40E_QTX_TAIL1(i);
+		adapter->tx_rings[i].tail = hw->hw_addr + I40E_QTX_TAIL1(i);
 }
 
 /**
@@ -656,8 +663,8 @@ static void i40evf_configure_rx(struct i40evf_adapter *adapter)
 	}
 
 	for (i = 0; i < adapter->num_active_queues; i++) {
-		adapter->rx_rings[i]->tail = hw->hw_addr + I40E_QRX_TAIL1(i);
-		adapter->rx_rings[i]->rx_buf_len = rx_buf_len;
+		adapter->rx_rings[i].tail = hw->hw_addr + I40E_QRX_TAIL1(i);
+		adapter->rx_rings[i].rx_buf_len = rx_buf_len;
 	}
 }
 
@@ -954,7 +961,7 @@ static void i40evf_napi_enable_all(struct i40evf_adapter *adapter)
 	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
 		struct napi_struct *napi;
 
-		q_vector = adapter->q_vector[q_idx];
+		q_vector = &adapter->q_vectors[q_idx];
 		napi = &q_vector->napi;
 		napi_enable(napi);
 	}
@@ -971,7 +978,7 @@ static void i40evf_napi_disable_all(struct i40evf_adapter *adapter)
 	int q_vectors = adapter->num_msix_vectors - NONQ_VECS;
 
 	for (q_idx = 0; q_idx < q_vectors; q_idx++) {
-		q_vector = adapter->q_vector[q_idx];
+		q_vector = &adapter->q_vectors[q_idx];
 		napi_disable(&q_vector->napi);
 	}
 }
@@ -992,7 +999,7 @@ static void i40evf_configure(struct i40evf_adapter *adapter)
 	adapter->aq_required |= I40EVF_FLAG_AQ_CONFIGURE_QUEUES;
 
 	for (i = 0; i < adapter->num_active_queues; i++) {
-		struct i40e_ring *ring = adapter->rx_rings[i];
+		struct i40e_ring *ring = &adapter->rx_rings[i];
 
 		i40evf_alloc_rx_buffers_1buf(ring, ring->count);
 		ring->next_to_use = ring->count - 1;
@@ -1112,16 +1119,10 @@ i40evf_acquire_msix_vectors(struct i40evf_adapter *adapter, int vectors)
  **/
 static void i40evf_free_queues(struct i40evf_adapter *adapter)
 {
-	int i;
-
 	if (!adapter->vsi_res)
 		return;
-	for (i = 0; i < adapter->num_active_queues; i++) {
-		if (adapter->tx_rings[i])
-			kfree_rcu(adapter->tx_rings[i], rcu);
-		adapter->tx_rings[i] = NULL;
-		adapter->rx_rings[i] = NULL;
-	}
+	kfree(adapter->tx_rings);
+	kfree(adapter->rx_rings);
 }
 
 /**
@@ -1136,13 +1137,20 @@ static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 {
 	int i;
 
+	adapter->tx_rings = kcalloc(adapter->num_active_queues,
+				    sizeof(struct i40e_ring), GFP_KERNEL);
+	if (!adapter->tx_rings)
+		goto err_out;
+	adapter->rx_rings = kcalloc(adapter->num_active_queues,
+				    sizeof(struct i40e_ring), GFP_KERNEL);
+	if (!adapter->rx_rings)
+		goto err_out;
+
 	for (i = 0; i < adapter->num_active_queues; i++) {
 		struct i40e_ring *tx_ring;
 		struct i40e_ring *rx_ring;
 
-		tx_ring = kzalloc(sizeof(*tx_ring) * 2, GFP_KERNEL);
-		if (!tx_ring)
-			goto err_out;
+		tx_ring = &adapter->tx_rings[i];
 
 		tx_ring->queue_index = i;
 		tx_ring->netdev = adapter->netdev;
@@ -1150,14 +1158,12 @@ static int i40evf_alloc_queues(struct i40evf_adapter *adapter)
 		tx_ring->count = adapter->tx_desc_count;
 		if (adapter->flags & I40E_FLAG_WB_ON_ITR_CAPABLE)
 			tx_ring->flags |= I40E_TXR_FLAGS_WB_ON_ITR;
-		adapter->tx_rings[i] = tx_ring;
 
-		rx_ring = &tx_ring[1];
+		rx_ring = &adapter->rx_rings[i];
 		rx_ring->queue_index = i;
 		rx_ring->netdev = adapter->netdev;
 		rx_ring->dev = &adapter->pdev->dev;
 		rx_ring->count = adapter->rx_desc_count;
-		adapter->rx_rings[i] = rx_ring;
 	}
 
 	return 0;
@@ -1207,115 +1213,273 @@ static int i40evf_set_interrupt_capability(struct i40evf_adapter *adapter)
 	err = i40evf_acquire_msix_vectors(adapter, v_budget);
 
 out:
-	adapter->netdev->real_num_tx_queues = pairs;
+	netif_set_real_num_rx_queues(adapter->netdev, pairs);
+	netif_set_real_num_tx_queues(adapter->netdev, pairs);
 	return err;
 }
 
 /**
- * i40e_configure_rss_aq - Prepare for RSS using AQ commands
+ * i40e_config_rss_aq - Prepare for RSS using AQ commands
  * @vsi: vsi structure
  * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Return 0 on success, negative on failure
  **/
-static void i40evf_configure_rss_aq(struct i40e_vsi *vsi, const u8 *seed)
+static int i40evf_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 i40evf_adapter *adapter = vsi->back;
 	struct i40e_hw *hw = &adapter->hw;
-	int ret = 0, i;
-	u8 *rss_lut;
+	int ret = 0;
 
 	if (!vsi->id)
-		return;
+		return -EINVAL;
 
 	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
-		dev_err(&adapter->pdev->dev, "Cannot confiure RSS, command %d pending\n",
+		dev_err(&adapter->pdev->dev, "Cannot configure RSS, command %d pending\n",
 			adapter->current_op);
-		return;
+		return -EBUSY;
 	}
 
-	memset(&rss_key, 0, sizeof(rss_key));
-	memcpy(&rss_key, seed, sizeof(rss_key));
+	if (seed) {
+		struct i40e_aqc_get_set_rss_key_data *rss_key =
+			(struct i40e_aqc_get_set_rss_key_data *)seed;
+		ret = i40evf_aq_set_rss_key(hw, vsi->id, rss_key);
+		if (ret) {
+			dev_err(&adapter->pdev->dev, "Cannot set RSS key, err %s aq_err %s\n",
+				i40evf_stat_str(hw, ret),
+				i40evf_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
+		}
+	}
 
-	rss_lut = kzalloc(((I40E_VFQF_HLUT_MAX_INDEX + 1) * 4), GFP_KERNEL);
-	if (!rss_lut)
-		return;
+	if (lut) {
+		ret = i40evf_aq_set_rss_lut(hw, vsi->id, false, lut, lut_size);
+		if (ret) {
+			dev_err(&adapter->pdev->dev,
+				"Cannot set RSS lut, err %s aq_err %s\n",
+				i40evf_stat_str(hw, ret),
+				i40evf_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
+		}
+	}
 
-	/* Populate the LUT with max no. PF queues in round robin fashion */
-	for (i = 0; i <= (I40E_VFQF_HLUT_MAX_INDEX * 4); i++)
-		rss_lut[i] = i % adapter->num_active_queues;
+	return ret;
+}
 
-	ret = i40evf_aq_set_rss_key(hw, vsi->id, &rss_key);
-	if (ret) {
-		dev_err(&adapter->pdev->dev,
-			"Cannot set RSS key, err %s aq_err %s\n",
-			i40evf_stat_str(hw, ret),
-			i40evf_aq_str(hw, hw->aq.asq_last_status));
-		return;
+/**
+ * i40evf_config_rss_reg - Configure RSS keys and lut by writing registers
+ * @vsi: Pointer to vsi structure
+ * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Returns 0 on success, negative on failure
+ **/
+static int i40evf_config_rss_reg(struct i40e_vsi *vsi, const u8 *seed,
+				 const u8 *lut, u16 lut_size)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+	struct i40e_hw *hw = &adapter->hw;
+	u16 i;
+
+	if (seed) {
+		u32 *seed_dw = (u32 *)seed;
+
+		for (i = 0; i <= I40E_VFQF_HKEY_MAX_INDEX; i++)
+			wr32(hw, I40E_VFQF_HKEY(i), seed_dw[i]);
 	}
 
-	ret = i40evf_aq_set_rss_lut(hw, vsi->id, false, rss_lut,
-				    (I40E_VFQF_HLUT_MAX_INDEX + 1) * 4);
-	if (ret)
-		dev_err(&adapter->pdev->dev,
-			"Cannot set RSS lut, err %s aq_err %s\n",
-			i40evf_stat_str(hw, ret),
-			i40evf_aq_str(hw, hw->aq.asq_last_status));
+	if (lut) {
+		u32 *lut_dw = (u32 *)lut;
+
+		if (lut_size != I40EVF_HLUT_ARRAY_SIZE)
+			return -EINVAL;
+
+		for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++)
+			wr32(hw, I40E_VFQF_HLUT(i), lut_dw[i]);
+	}
+	i40e_flush(hw);
+
+	return 0;
 }
 
 /**
- * i40e_configure_rss_reg - Prepare for RSS if used
- * @adapter: board private structure
- * @seed: RSS hash seed
+ *  * i40evf_get_rss_aq - Get RSS keys and lut by using AQ commands
+ *  @vsi: Pointer to vsi structure
+ *  @seed: RSS hash seed
+ *  @lut: Lookup table
+ *  @lut_size: Lookup table size
+ *
+ *  Return 0 on success, negative on failure
  **/
-static void i40evf_configure_rss_reg(struct i40evf_adapter *adapter,
-				     const u8 *seed)
+static int i40evf_get_rss_aq(struct i40e_vsi *vsi, const u8 *seed,
+			     u8 *lut, u16 lut_size)
 {
+	struct i40evf_adapter *adapter = vsi->back;
 	struct i40e_hw *hw = &adapter->hw;
-	u32 *seed_dw = (u32 *)seed;
-	u32 cqueue = 0;
-	u32 lut = 0;
-	int i, j;
+	int ret = 0;
 
-	/* Fill out hash function seed */
-	for (i = 0; i <= I40E_VFQF_HKEY_MAX_INDEX; i++)
-		wr32(hw, I40E_VFQF_HKEY(i), seed_dw[i]);
-
-	/* Populate the LUT with max no. PF queues in round robin fashion */
-	for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++) {
-		lut = 0;
-		for (j = 0; j < 4; j++) {
-			if (cqueue == adapter->num_active_queues)
-				cqueue = 0;
-			lut |= ((cqueue) << (8 * j));
-			cqueue++;
+	if (seed) {
+		ret = i40evf_aq_get_rss_key(hw, vsi->id,
+			(struct i40e_aqc_get_set_rss_key_data *)seed);
+		if (ret) {
+			dev_err(&adapter->pdev->dev,
+				"Cannot get RSS key, err %s aq_err %s\n",
+				i40evf_stat_str(hw, ret),
+				i40evf_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
 		}
-		wr32(hw, I40E_VFQF_HLUT(i), lut);
 	}
-	i40e_flush(hw);
+
+	if (lut) {
+		ret = i40evf_aq_get_rss_lut(hw, vsi->id, seed, lut, lut_size);
+		if (ret) {
+			dev_err(&adapter->pdev->dev,
+				"Cannot get RSS lut, err %s aq_err %s\n",
+				i40evf_stat_str(hw, ret),
+				i40evf_aq_str(hw, hw->aq.asq_last_status));
+			return ret;
+		}
+	}
+
+	return ret;
 }
 
 /**
- * i40evf_configure_rss - Prepare for RSS
+ *  * i40evf_get_rss_reg - Get RSS keys and lut by reading registers
+ *  @vsi: Pointer to vsi structure
+ *  @seed: RSS hash seed
+ *  @lut: Lookup table
+ *  @lut_size: Lookup table size
+ *
+ *  Returns 0 on success, negative on failure
+ **/
+static int i40evf_get_rss_reg(struct i40e_vsi *vsi, const u8 *seed,
+			      const u8 *lut, u16 lut_size)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+	struct i40e_hw *hw = &adapter->hw;
+	u16 i;
+
+	if (seed) {
+		u32 *seed_dw = (u32 *)seed;
+
+		for (i = 0; i <= I40E_VFQF_HKEY_MAX_INDEX; i++)
+			seed_dw[i] = rd32(hw, I40E_VFQF_HKEY(i));
+	}
+
+	if (lut) {
+		u32 *lut_dw = (u32 *)lut;
+
+		if (lut_size != I40EVF_HLUT_ARRAY_SIZE)
+			return -EINVAL;
+
+		for (i = 0; i <= I40E_VFQF_HLUT_MAX_INDEX; i++)
+			lut_dw[i] = rd32(hw, I40E_VFQF_HLUT(i));
+	}
+
+	return 0;
+}
+
+/**
+ * i40evf_config_rss - Configure RSS keys and lut
+ * @vsi: Pointer to vsi structure
+ * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Returns 0 on success, negative on failure
+ **/
+int i40evf_config_rss(struct i40e_vsi *vsi, const u8 *seed,
+		      u8 *lut, u16 lut_size)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+
+	if (RSS_AQ(adapter))
+		return i40evf_config_rss_aq(vsi, seed, lut, lut_size);
+	else
+		return i40evf_config_rss_reg(vsi, seed, lut, lut_size);
+}
+
+/**
+ * i40evf_get_rss - Get RSS keys and lut
+ * @vsi: Pointer to vsi structure
+ * @seed: RSS hash seed
+ * @lut: Lookup table
+ * @lut_size: Lookup table size
+ *
+ * Returns 0 on success, negative on failure
+ **/
+int i40evf_get_rss(struct i40e_vsi *vsi, const u8 *seed, u8 *lut, u16 lut_size)
+{
+	struct i40evf_adapter *adapter = vsi->back;
+
+	if (RSS_AQ(adapter))
+		return i40evf_get_rss_aq(vsi, seed, lut, lut_size);
+	else
+		return i40evf_get_rss_reg(vsi, seed, lut, lut_size);
+}
+
+/**
+ * i40evf_fill_rss_lut - Fill the lut with default values
+ * @lut: Lookup table to be filled with
+ * @rss_table_size: Lookup table size
+ * @rss_size: Range of queue number for hashing
+ **/
+static void i40evf_fill_rss_lut(u8 *lut, u16 rss_table_size, u16 rss_size)
+{
+	u16 i;
+
+	for (i = 0; i < rss_table_size; i++)
+		lut[i] = i % rss_size;
+}
+
+/**
+ * i40evf_init_rss - Prepare for RSS
  * @adapter: board private structure
+ *
+ * Return 0 on success, negative on failure
  **/
-static void i40evf_configure_rss(struct i40evf_adapter *adapter)
+static int i40evf_init_rss(struct i40evf_adapter *adapter)
 {
+	struct i40e_vsi *vsi = &adapter->vsi;
 	struct i40e_hw *hw = &adapter->hw;
 	u8 seed[I40EVF_HKEY_ARRAY_SIZE];
 	u64 hena;
-
-	netdev_rss_key_fill((void *)seed, I40EVF_HKEY_ARRAY_SIZE);
+	u8 *lut;
+	int ret;
 
 	/* Enable PCTYPES for RSS, TCP/UDP with IPv4/IPv6 */
 	hena = I40E_DEFAULT_RSS_HENA;
 	wr32(hw, I40E_VFQF_HENA(0), (u32)hena);
 	wr32(hw, I40E_VFQF_HENA(1), (u32)(hena >> 32));
 
-	if (RSS_AQ(adapter))
-		i40evf_configure_rss_aq(&adapter->vsi, seed);
+	lut = kzalloc(I40EVF_HLUT_ARRAY_SIZE, GFP_KERNEL);
+	if (!lut)
+		return -ENOMEM;
+
+	/* Use user configured lut if there is one, otherwise use default */
+	if (vsi->rss_lut_user)
+		memcpy(lut, vsi->rss_lut_user, I40EVF_HLUT_ARRAY_SIZE);
+	else
+		i40evf_fill_rss_lut(lut, I40EVF_HLUT_ARRAY_SIZE,
+				    adapter->num_active_queues);
+
+	/* Use user configured hash key if there is one, otherwise
+	 * user default.
+	 */
+	if (vsi->rss_hkey_user)
+		memcpy(seed, vsi->rss_hkey_user, I40EVF_HKEY_ARRAY_SIZE);
 	else
-		i40evf_configure_rss_reg(adapter, seed);
+		netdev_rss_key_fill((void *)seed, I40EVF_HKEY_ARRAY_SIZE);
+	ret = i40evf_config_rss(vsi, seed, lut, I40EVF_HLUT_ARRAY_SIZE);
+	kfree(lut);
+
+	return ret;
 }
 
 /**
@@ -1327,21 +1491,22 @@ static void i40evf_configure_rss(struct i40evf_adapter *adapter)
  **/
 static int i40evf_alloc_q_vectors(struct i40evf_adapter *adapter)
 {
-	int q_idx, num_q_vectors;
+	int q_idx = 0, num_q_vectors;
 	struct i40e_q_vector *q_vector;
 
 	num_q_vectors = adapter->num_msix_vectors - NONQ_VECS;
+	adapter->q_vectors = kcalloc(num_q_vectors, sizeof(*q_vector),
+				     GFP_KERNEL);
+	if (!adapter->q_vectors)
+		goto err_out;
 
 	for (q_idx = 0; q_idx < num_q_vectors; q_idx++) {
-		q_vector = kzalloc(sizeof(*q_vector), GFP_KERNEL);
-		if (!q_vector)
-			goto err_out;
+		q_vector = &adapter->q_vectors[q_idx];
 		q_vector->adapter = adapter;
 		q_vector->vsi = &adapter->vsi;
 		q_vector->v_idx = q_idx;
 		netif_napi_add(adapter->netdev, &q_vector->napi,
 			       i40evf_napi_poll, NAPI_POLL_WEIGHT);
-		adapter->q_vector[q_idx] = q_vector;
 	}
 
 	return 0;
@@ -1349,11 +1514,10 @@ static int i40evf_alloc_q_vectors(struct i40evf_adapter *adapter)
 err_out:
 	while (q_idx) {
 		q_idx--;
-		q_vector = adapter->q_vector[q_idx];
+		q_vector = &adapter->q_vectors[q_idx];
 		netif_napi_del(&q_vector->napi);
-		kfree(q_vector);
-		adapter->q_vector[q_idx] = NULL;
 	}
+	kfree(adapter->q_vectors);
 	return -ENOMEM;
 }
 
@@ -1374,13 +1538,11 @@ static void i40evf_free_q_vectors(struct i40evf_adapter *adapter)
 	napi_vectors = adapter->num_active_queues;
 
 	for (q_idx = 0; q_idx < num_q_vectors; q_idx++) {
-		struct i40e_q_vector *q_vector = adapter->q_vector[q_idx];
-
-		adapter->q_vector[q_idx] = NULL;
+		struct i40e_q_vector *q_vector = &adapter->q_vectors[q_idx];
 		if (q_idx < napi_vectors)
 			netif_napi_del(&q_vector->napi);
-		kfree(q_vector);
 	}
+	kfree(adapter->q_vectors);
 }
 
 /**
@@ -1439,6 +1601,22 @@ err_set_interrupt:
 }
 
 /**
+ * i40evf_clear_rss_config_user - Clear user configurations of RSS
+ * @vsi: Pointer to VSI structure
+ **/
+static void i40evf_clear_rss_config_user(struct i40e_vsi *vsi)
+{
+	if (!vsi)
+		return;
+
+	kfree(vsi->rss_hkey_user);
+	vsi->rss_hkey_user = NULL;
+
+	kfree(vsi->rss_lut_user);
+	vsi->rss_lut_user = NULL;
+}
+
+/**
  * i40evf_watchdog_timer - Periodic call-back timer
  * @data: pointer to adapter disguised as unsigned long
  **/
@@ -1565,7 +1743,7 @@ static void i40evf_watchdog_task(struct work_struct *work)
 		 * PF, so we don't have to set current_op as we will
 		 * not get a response through the ARQ.
 		 */
-		i40evf_configure_rss(adapter);
+		i40evf_init_rss(adapter);
 		adapter->aq_required &= ~I40EVF_FLAG_AQ_CONFIGURE_RSS;
 		goto watchdog_done;
 	}
@@ -1864,9 +2042,12 @@ void i40evf_free_all_tx_resources(struct i40evf_adapter *adapter)
 {
 	int i;
 
+	if (!adapter->tx_rings)
+		return;
+
 	for (i = 0; i < adapter->num_active_queues; i++)
-		if (adapter->tx_rings[i]->desc)
-			i40evf_free_tx_resources(adapter->tx_rings[i]);
+		if (adapter->tx_rings[i].desc)
+			i40evf_free_tx_resources(&adapter->tx_rings[i]);
 }
 
 /**
@@ -1884,8 +2065,8 @@ static int i40evf_setup_all_tx_resources(struct i40evf_adapter *adapter)
 	int i, err = 0;
 
 	for (i = 0; i < adapter->num_active_queues; i++) {
-		adapter->tx_rings[i]->count = adapter->tx_desc_count;
-		err = i40evf_setup_tx_descriptors(adapter->tx_rings[i]);
+		adapter->tx_rings[i].count = adapter->tx_desc_count;
+		err = i40evf_setup_tx_descriptors(&adapter->tx_rings[i]);
 		if (!err)
 			continue;
 		dev_err(&adapter->pdev->dev,
@@ -1911,8 +2092,8 @@ static int i40evf_setup_all_rx_resources(struct i40evf_adapter *adapter)
 	int i, err = 0;
 
 	for (i = 0; i < adapter->num_active_queues; i++) {
-		adapter->rx_rings[i]->count = adapter->rx_desc_count;
-		err = i40evf_setup_rx_descriptors(adapter->rx_rings[i]);
+		adapter->rx_rings[i].count = adapter->rx_desc_count;
+		err = i40evf_setup_rx_descriptors(&adapter->rx_rings[i]);
 		if (!err)
 			continue;
 		dev_err(&adapter->pdev->dev,
@@ -1932,9 +2113,12 @@ void i40evf_free_all_rx_resources(struct i40evf_adapter *adapter)
 {
 	int i;
 
+	if (!adapter->rx_rings)
+		return;
+
 	for (i = 0; i < adapter->num_active_queues; i++)
-		if (adapter->rx_rings[i]->desc)
-			i40evf_free_rx_resources(adapter->rx_rings[i]);
+		if (adapter->rx_rings[i].desc)
+			i40evf_free_rx_resources(&adapter->rx_rings[i]);
 }
 
 /**
@@ -2137,7 +2321,7 @@ int i40evf_process_config(struct i40evf_adapter *adapter)
 	netdev->features |= NETIF_F_HIGHDMA |
 			    NETIF_F_SG |
 			    NETIF_F_IP_CSUM |
-			    NETIF_F_SCTP_CSUM |
+			    NETIF_F_SCTP_CRC |
 			    NETIF_F_IPV6_CSUM |
 			    NETIF_F_TSO |
 			    NETIF_F_TSO6 |
@@ -2263,6 +2447,14 @@ static void i40evf_init_task(struct work_struct *work)
 		if (err == I40E_ERR_ADMIN_QUEUE_NO_WORK) {
 			err = i40evf_send_vf_config_msg(adapter);
 			goto err;
+		} else if (err == I40E_ERR_PARAM) {
+			/* We only get ERR_PARAM if the device is in a very bad
+			 * state or if we've been disabled for previous bad
+			 * behavior. Either way, we're done now.
+			 */
+			i40evf_shutdown_adminq(hw);
+			dev_err(&pdev->dev, "Unable to get VF config due to PF error condition, not retrying\n");
+			return;
 		}
 		if (err) {
 			dev_err(&pdev->dev, "Unable to get VF config (%d)\n",
@@ -2313,7 +2505,7 @@ static void i40evf_init_task(struct work_struct *work)
 		    I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR)
 		adapter->flags |= I40EVF_FLAG_WB_ON_ITR_CAPABLE;
 	if (!RSS_AQ(adapter))
-		i40evf_configure_rss(adapter);
+		i40evf_init_rss(adapter);
 	err = i40evf_request_misc_irq(adapter);
 	if (err)
 		goto err_sw_init;
@@ -2334,7 +2526,6 @@ static void i40evf_init_task(struct work_struct *work)
 	if (netdev->features & NETIF_F_GRO)
 		dev_info(&pdev->dev, "GRO is enabled\n");
 
-	dev_info(&pdev->dev, "%s\n", i40evf_driver_string);
 	adapter->state = __I40EVF_DOWN;
 	set_bit(__I40E_DOWN, &adapter->vsi.state);
 	i40evf_misc_irq_enable(adapter);
@@ -2343,7 +2534,7 @@ static void i40evf_init_task(struct work_struct *work)
 		adapter->aq_required |= I40EVF_FLAG_AQ_CONFIGURE_RSS;
 		mod_timer_pending(&adapter->watchdog_timer, jiffies + 1);
 	} else {
-		i40evf_configure_rss(adapter);
+		i40evf_init_rss(adapter);
 	}
 	return;
 restart:
@@ -2438,8 +2629,7 @@ static int i40evf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_master(pdev);
 
-	netdev = alloc_etherdev_mq(sizeof(struct i40evf_adapter),
-				   MAX_TX_QUEUES);
+	netdev = alloc_etherdev_mq(sizeof(struct i40evf_adapter), MAX_QUEUES);
 	if (!netdev) {
 		err = -ENOMEM;
 		goto err_alloc_etherdev;
@@ -2632,6 +2822,9 @@ static void i40evf_remove(struct pci_dev *pdev)
 
 	flush_scheduled_work();
 
+	/* Clear user configurations for RSS */
+	i40evf_clear_rss_config_user(&adapter->vsi);
+
 	if (hw->aq.asq.count)
 		i40evf_shutdown_adminq(hw);
 
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
index 32e620e1eb5c..c1c526283757 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_virtchnl.c
@@ -157,7 +157,9 @@ int i40evf_send_vf_config_msg(struct i40evf_adapter *adapter)
 	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_AQ |
 	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_REG |
 	       I40E_VIRTCHNL_VF_OFFLOAD_VLAN |
-	       I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR;
+	       I40E_VIRTCHNL_VF_OFFLOAD_WB_ON_ITR |
+	       I40E_VIRTCHNL_VF_OFFLOAD_RSS_PCTYPE_V2;
+
 	adapter->current_op = I40E_VIRTCHNL_OP_GET_VF_RESOURCES;
 	adapter->aq_required &= ~I40EVF_FLAG_AQ_GET_CONFIG;
 	if (PF_IS_V11(adapter))
@@ -242,7 +244,7 @@ void i40evf_configure_queues(struct i40evf_adapter *adapter)
 	adapter->current_op = I40E_VIRTCHNL_OP_CONFIG_VSI_QUEUES;
 	len = sizeof(struct i40e_virtchnl_vsi_queue_config_info) +
 		       (sizeof(struct i40e_virtchnl_queue_pair_info) * pairs);
-	vqci = kzalloc(len, GFP_ATOMIC);
+	vqci = kzalloc(len, GFP_KERNEL);
 	if (!vqci)
 		return;
 
@@ -255,19 +257,19 @@ void i40evf_configure_queues(struct i40evf_adapter *adapter)
 	for (i = 0; i < pairs; i++) {
 		vqpi->txq.vsi_id = vqci->vsi_id;
 		vqpi->txq.queue_id = i;
-		vqpi->txq.ring_len = adapter->tx_rings[i]->count;
-		vqpi->txq.dma_ring_addr = adapter->tx_rings[i]->dma;
+		vqpi->txq.ring_len = adapter->tx_rings[i].count;
+		vqpi->txq.dma_ring_addr = adapter->tx_rings[i].dma;
 		vqpi->txq.headwb_enabled = 1;
 		vqpi->txq.dma_headwb_addr = vqpi->txq.dma_ring_addr +
 		    (vqpi->txq.ring_len * sizeof(struct i40e_tx_desc));
 
 		vqpi->rxq.vsi_id = vqci->vsi_id;
 		vqpi->rxq.queue_id = i;
-		vqpi->rxq.ring_len = adapter->rx_rings[i]->count;
-		vqpi->rxq.dma_ring_addr = adapter->rx_rings[i]->dma;
+		vqpi->rxq.ring_len = adapter->rx_rings[i].count;
+		vqpi->rxq.dma_ring_addr = adapter->rx_rings[i].dma;
 		vqpi->rxq.max_pkt_size = adapter->netdev->mtu
 					+ ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN;
-		vqpi->rxq.databuffer_size = adapter->rx_rings[i]->rx_buf_len;
+		vqpi->rxq.databuffer_size = adapter->rx_rings[i].rx_buf_len;
 		vqpi++;
 	}
 
@@ -353,14 +355,14 @@ void i40evf_map_queues(struct i40evf_adapter *adapter)
 	len = sizeof(struct i40e_virtchnl_irq_map_info) +
 	      (adapter->num_msix_vectors *
 		sizeof(struct i40e_virtchnl_vector_map));
-	vimi = kzalloc(len, GFP_ATOMIC);
+	vimi = kzalloc(len, GFP_KERNEL);
 	if (!vimi)
 		return;
 
 	vimi->num_vectors = adapter->num_msix_vectors;
 	/* Queue vectors first */
 	for (v_idx = 0; v_idx < q_vectors; v_idx++) {
-		q_vector = adapter->q_vector[v_idx];
+		q_vector = adapter->q_vectors + v_idx;
 		vimi->vecmap[v_idx].vsi_id = adapter->vsi_res->vsi_id;
 		vimi->vecmap[v_idx].vector_id = v_idx + NONQ_VECS;
 		vimi->vecmap[v_idx].txq_map = q_vector->ring_mask;
@@ -391,6 +393,7 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 	struct i40e_virtchnl_ether_addr_list *veal;
 	int len, i = 0, count = 0;
 	struct i40evf_mac_filter *f;
+	bool more = false;
 
 	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
@@ -415,10 +418,12 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
 			 sizeof(struct i40e_virtchnl_ether_addr_list)) /
 			sizeof(struct i40e_virtchnl_ether_addr);
-		len = I40EVF_MAX_AQ_BUF_SIZE;
+		len = sizeof(struct i40e_virtchnl_ether_addr_list) +
+		      (count * sizeof(struct i40e_virtchnl_ether_addr));
+		more = true;
 	}
 
-	veal = kzalloc(len, GFP_ATOMIC);
+	veal = kzalloc(len, GFP_KERNEL);
 	if (!veal)
 		return;
 
@@ -431,7 +436,8 @@ void i40evf_add_ether_addrs(struct i40evf_adapter *adapter)
 			f->add = false;
 		}
 	}
-	adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_MAC_FILTER;
+	if (!more)
+		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_MAC_FILTER;
 	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS,
 			   (u8 *)veal, len);
 	kfree(veal);
@@ -450,6 +456,7 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 	struct i40e_virtchnl_ether_addr_list *veal;
 	struct i40evf_mac_filter *f, *ftmp;
 	int len, i = 0, count = 0;
+	bool more = false;
 
 	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
@@ -474,9 +481,11 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
 			 sizeof(struct i40e_virtchnl_ether_addr_list)) /
 			sizeof(struct i40e_virtchnl_ether_addr);
-		len = I40EVF_MAX_AQ_BUF_SIZE;
+		len = sizeof(struct i40e_virtchnl_ether_addr_list) +
+		      (count * sizeof(struct i40e_virtchnl_ether_addr));
+		more = true;
 	}
-	veal = kzalloc(len, GFP_ATOMIC);
+	veal = kzalloc(len, GFP_KERNEL);
 	if (!veal)
 		return;
 
@@ -490,7 +499,8 @@ void i40evf_del_ether_addrs(struct i40evf_adapter *adapter)
 			kfree(f);
 		}
 	}
-	adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_MAC_FILTER;
+	if (!more)
+		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_MAC_FILTER;
 	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS,
 			   (u8 *)veal, len);
 	kfree(veal);
@@ -509,6 +519,7 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
 	struct i40e_virtchnl_vlan_filter_list *vvfl;
 	int len, i = 0, count = 0;
 	struct i40evf_vlan_filter *f;
+	bool more = false;
 
 	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
@@ -534,9 +545,11 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
 			 sizeof(struct i40e_virtchnl_vlan_filter_list)) /
 			sizeof(u16);
-		len = I40EVF_MAX_AQ_BUF_SIZE;
+		len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+		      (count * sizeof(u16));
+		more = true;
 	}
-	vvfl = kzalloc(len, GFP_ATOMIC);
+	vvfl = kzalloc(len, GFP_KERNEL);
 	if (!vvfl)
 		return;
 
@@ -549,7 +562,8 @@ void i40evf_add_vlans(struct i40evf_adapter *adapter)
 			f->add = false;
 		}
 	}
-	adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
+	if (!more)
+		adapter->aq_required &= ~I40EVF_FLAG_AQ_ADD_VLAN_FILTER;
 	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_ADD_VLAN, (u8 *)vvfl, len);
 	kfree(vvfl);
 }
@@ -567,6 +581,7 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
 	struct i40e_virtchnl_vlan_filter_list *vvfl;
 	struct i40evf_vlan_filter *f, *ftmp;
 	int len, i = 0, count = 0;
+	bool more = false;
 
 	if (adapter->current_op != I40E_VIRTCHNL_OP_UNKNOWN) {
 		/* bail because we already have a command pending */
@@ -592,9 +607,11 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
 		count = (I40EVF_MAX_AQ_BUF_SIZE -
 			 sizeof(struct i40e_virtchnl_vlan_filter_list)) /
 			sizeof(u16);
-		len = I40EVF_MAX_AQ_BUF_SIZE;
+		len = sizeof(struct i40e_virtchnl_vlan_filter_list) +
+		      (count * sizeof(u16));
+		more = true;
 	}
-	vvfl = kzalloc(len, GFP_ATOMIC);
+	vvfl = kzalloc(len, GFP_KERNEL);
 	if (!vvfl)
 		return;
 
@@ -608,7 +625,8 @@ void i40evf_del_vlans(struct i40evf_adapter *adapter)
 			kfree(f);
 		}
 	}
-	adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
+	if (!more)
+		adapter->aq_required &= ~I40EVF_FLAG_AQ_DEL_VLAN_FILTER;
 	i40evf_send_pf_msg(adapter, I40E_VIRTCHNL_OP_DEL_VLAN, (u8 *)vvfl, len);
 	kfree(vvfl);
 }
@@ -724,9 +742,29 @@ void i40evf_virtchnl_completion(struct i40evf_adapter *adapter,
 		return;
 	}
 	if (v_retval) {
-		dev_err(&adapter->pdev->dev, "PF returned error %d (%s) to our request %d\n",
-			v_retval, i40evf_stat_str(&adapter->hw, v_retval),
-			v_opcode);
+		switch (v_opcode) {
+		case I40E_VIRTCHNL_OP_ADD_VLAN:
+			dev_err(&adapter->pdev->dev, "Failed to add VLAN filter, error %s\n",
+				i40evf_stat_str(&adapter->hw, v_retval));
+			break;
+		case I40E_VIRTCHNL_OP_ADD_ETHER_ADDRESS:
+			dev_err(&adapter->pdev->dev, "Failed to add MAC filter, error %s\n",
+				i40evf_stat_str(&adapter->hw, v_retval));
+			break;
+		case I40E_VIRTCHNL_OP_DEL_VLAN:
+			dev_err(&adapter->pdev->dev, "Failed to delete VLAN filter, error %s\n",
+				i40evf_stat_str(&adapter->hw, v_retval));
+			break;
+		case I40E_VIRTCHNL_OP_DEL_ETHER_ADDRESS:
+			dev_err(&adapter->pdev->dev, "Failed to delete MAC filter, error %s\n",
+				i40evf_stat_str(&adapter->hw, v_retval));
+			break;
+		default:
+			dev_err(&adapter->pdev->dev, "PF returned error %d (%s) to our request %d\n",
+				v_retval,
+				i40evf_stat_str(&adapter->hw, v_retval),
+				v_opcode);
+		}
 	}
 	switch (v_opcode) {
 	case I40E_VIRTCHNL_OP_GET_STATS: {
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index 7a73510e547c..adb33e2a0137 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -45,8 +45,6 @@ static s32  igb_get_cfg_done_82575(struct e1000_hw *);
 static s32  igb_init_hw_82575(struct e1000_hw *);
 static s32  igb_phy_hw_reset_sgmii_82575(struct e1000_hw *);
 static s32  igb_read_phy_reg_sgmii_82575(struct e1000_hw *, u32, u16 *);
-static s32  igb_read_phy_reg_82580(struct e1000_hw *, u32, u16 *);
-static s32  igb_write_phy_reg_82580(struct e1000_hw *, u32, u16);
 static s32  igb_reset_hw_82575(struct e1000_hw *);
 static s32  igb_reset_hw_82580(struct e1000_hw *);
 static s32  igb_set_d0_lplu_state_82575(struct e1000_hw *, bool);
@@ -205,13 +203,10 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 		case e1000_82580:
 		case e1000_i350:
 		case e1000_i354:
-			phy->ops.read_reg = igb_read_phy_reg_82580;
-			phy->ops.write_reg = igb_write_phy_reg_82580;
-			break;
 		case e1000_i210:
 		case e1000_i211:
-			phy->ops.read_reg = igb_read_phy_reg_gs40g;
-			phy->ops.write_reg = igb_write_phy_reg_gs40g;
+			phy->ops.read_reg = igb_read_phy_reg_82580;
+			phy->ops.write_reg = igb_write_phy_reg_82580;
 			break;
 		default:
 			phy->ops.read_reg = igb_read_phy_reg_igp;
@@ -272,6 +267,11 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 			if (ret_val)
 				goto out;
 		}
+		if (phy->id == M88E1543_E_PHY_ID) {
+			ret_val = igb_initialize_M88E1543_phy(hw);
+			if (ret_val)
+				goto out;
+		}
 		break;
 	case IGP03E1000_E_PHY_ID:
 		phy->type = e1000_phy_igp_3;
@@ -294,6 +294,7 @@ static s32 igb_init_phy_params_82575(struct e1000_hw *hw)
 	case I210_I_PHY_ID:
 		phy->type		= e1000_phy_i210;
 		phy->ops.check_polarity	= igb_check_polarity_m88;
+		phy->ops.get_cfg_done	= igb_get_cfg_done_i210;
 		phy->ops.get_phy_info	= igb_get_phy_info_m88;
 		phy->ops.get_cable_length = igb_get_cable_length_m88_gen2;
 		phy->ops.set_d0_lplu_state = igb_set_d0_lplu_state_82580;
@@ -925,6 +926,8 @@ static s32 igb_phy_hw_reset_sgmii_82575(struct e1000_hw *hw)
 
 	if (phy->id == M88E1512_E_PHY_ID)
 		ret_val = igb_initialize_M88E1512_phy(hw);
+	if (phy->id == M88E1543_E_PHY_ID)
+		ret_val = igb_initialize_M88E1543_phy(hw);
 out:
 	return ret_val;
 }
@@ -2145,7 +2148,7 @@ void igb_vmdq_set_replication_pf(struct e1000_hw *hw, bool enable)
  *  Reads the MDI control register in the PHY at offset and stores the
  *  information read to data.
  **/
-static s32 igb_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data)
+s32 igb_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data)
 {
 	s32 ret_val;
 
@@ -2169,7 +2172,7 @@ out:
  *
  *  Writes data to MDI control register in the PHY at offset.
  **/
-static s32 igb_write_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 data)
+s32 igb_write_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 data)
 {
 	s32 ret_val;
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_defines.h b/drivers/net/ethernet/intel/igb/e1000_defines.h
index b1915043bc0c..c3c598c347a9 100644
--- a/drivers/net/ethernet/intel/igb/e1000_defines.h
+++ b/drivers/net/ethernet/intel/igb/e1000_defines.h
@@ -927,7 +927,10 @@
 
 /* Intel i347-AT4 Registers */
 
-#define I347AT4_PCDL                   0x10 /* PHY Cable Diagnostics Length */
+#define I347AT4_PCDL0                  0x10 /* Pair 0 PHY Cable Diagnostics Length */
+#define I347AT4_PCDL1                  0x11 /* Pair 1 PHY Cable Diagnostics Length */
+#define I347AT4_PCDL2                  0x12 /* Pair 2 PHY Cable Diagnostics Length */
+#define I347AT4_PCDL3                  0x13 /* Pair 3 PHY Cable Diagnostics Length */
 #define I347AT4_PCDC                   0x15 /* PHY Cable Diagnostics Control */
 #define I347AT4_PAGE_SELECT            0x16
 
@@ -990,6 +993,7 @@
 #define E1000_M88E1543_PAGE_ADDR	0x16       /* Page Offset Register */
 #define E1000_M88E1543_EEE_CTRL_1	0x0
 #define E1000_M88E1543_EEE_CTRL_1_MS	0x0001     /* EEE Master/Slave */
+#define E1000_M88E1543_FIBER_CTRL	0x0
 #define E1000_EEE_ADV_DEV_I354		7
 #define E1000_EEE_ADV_ADDR_I354		60
 #define E1000_EEE_ADV_100_SUPPORTED	(1 << 1)   /* 100BaseTx EEE Supported */
diff --git a/drivers/net/ethernet/intel/igb/e1000_hw.h b/drivers/net/ethernet/intel/igb/e1000_hw.h
index 2003b3756ba2..4034207eb5cc 100644
--- a/drivers/net/ethernet/intel/igb/e1000_hw.h
+++ b/drivers/net/ethernet/intel/igb/e1000_hw.h
@@ -441,6 +441,7 @@ struct e1000_phy_info {
 	u16 cable_length;
 	u16 max_cable_length;
 	u16 min_cable_length;
+	u16 pair_length[4];
 
 	u8 mdix;
 
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.c b/drivers/net/ethernet/intel/igb/e1000_i210.c
index 65d931669f81..8aa798737d4d 100644
--- a/drivers/net/ethernet/intel/igb/e1000_i210.c
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.c
@@ -861,10 +861,10 @@ s32 igb_pll_workaround_i210(struct e1000_hw *hw)
 	if (ret_val)
 		nvm_word = E1000_INVM_DEFAULT_AL;
 	tmp_nvm = nvm_word | E1000_INVM_PLL_WO_VAL;
+	igb_write_phy_reg_82580(hw, I347AT4_PAGE_SELECT, E1000_PHY_PLL_FREQ_PAGE);
 	for (i = 0; i < E1000_MAX_PLL_TRIES; i++) {
 		/* check current state directly from internal PHY */
-		igb_read_phy_reg_gs40g(hw, (E1000_PHY_PLL_FREQ_PAGE |
-					 E1000_PHY_PLL_FREQ_REG), &phy_word);
+		igb_read_phy_reg_82580(hw, E1000_PHY_PLL_FREQ_REG, &phy_word);
 		if ((phy_word & E1000_PHY_PLL_UNCONF)
 		    != E1000_PHY_PLL_UNCONF) {
 			ret_val = 0;
@@ -896,7 +896,35 @@ s32 igb_pll_workaround_i210(struct e1000_hw *hw)
 		/* restore WUC register */
 		wr32(E1000_WUC, wuc);
 	}
+	igb_write_phy_reg_82580(hw, I347AT4_PAGE_SELECT, 0);
 	/* restore MDICNFG setting */
 	wr32(E1000_MDICNFG, mdicnfg);
 	return ret_val;
 }
+
+/**
+ *  igb_get_cfg_done_i210 - Read config done bit
+ *  @hw: pointer to the HW structure
+ *
+ *  Read the management control register for the config done bit for
+ *  completion status.  NOTE: silicon which is EEPROM-less will fail trying
+ *  to read the config done bit, so an error is *ONLY* logged and returns
+ *  0.  If we were to return with error, EEPROM-less silicon
+ *  would not be able to be reset or change link.
+ **/
+s32 igb_get_cfg_done_i210(struct e1000_hw *hw)
+{
+	s32 timeout = PHY_CFG_TIMEOUT;
+	u32 mask = E1000_NVM_CFG_DONE_PORT_0;
+
+	while (timeout) {
+		if (rd32(E1000_EEMNGCTL_I210) & mask)
+			break;
+		usleep_range(1000, 2000);
+		timeout--;
+	}
+	if (!timeout)
+		hw_dbg("MNG configuration cycle has not completed.\n");
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/intel/igb/e1000_i210.h b/drivers/net/ethernet/intel/igb/e1000_i210.h
index 3442b6357d01..b2964a2a60b1 100644
--- a/drivers/net/ethernet/intel/igb/e1000_i210.h
+++ b/drivers/net/ethernet/intel/igb/e1000_i210.h
@@ -34,6 +34,7 @@ s32 igb_write_xmdio_reg(struct e1000_hw *hw, u16 addr, u8 dev_addr, u16 data);
 s32 igb_init_nvm_params_i210(struct e1000_hw *hw);
 bool igb_get_flash_presence_i210(struct e1000_hw *hw);
 s32 igb_pll_workaround_i210(struct e1000_hw *hw);
+s32 igb_get_cfg_done_i210(struct e1000_hw *hw);
 
 #define E1000_STM_OPCODE		0xDB00
 #define E1000_EEPROM_FLASH_SIZE_WORD	0x11
@@ -84,7 +85,7 @@ enum E1000_INVM_STRUCTURE_TYPE {
 #define E1000_PCI_PMCSR_D3		0x03
 #define E1000_MAX_PLL_TRIES		5
 #define E1000_PHY_PLL_UNCONF		0xFF
-#define E1000_PHY_PLL_FREQ_PAGE		0xFC0000
+#define E1000_PHY_PLL_FREQ_PAGE		0xFC
 #define E1000_PHY_PLL_FREQ_REG		0x000E
 #define E1000_INVM_DEFAULT_AL		0x202F
 #define E1000_INVM_AUTOLOAD		0x0A
diff --git a/drivers/net/ethernet/intel/igb/e1000_phy.c b/drivers/net/ethernet/intel/igb/e1000_phy.c
index 23ec28f43f6d..5b54254aed4f 100644
--- a/drivers/net/ethernet/intel/igb/e1000_phy.c
+++ b/drivers/net/ethernet/intel/igb/e1000_phy.c
@@ -1717,59 +1717,76 @@ s32 igb_get_cable_length_m88_gen2(struct e1000_hw *hw)
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val;
 	u16 phy_data, phy_data2, index, default_page, is_cm;
+	int len_tot = 0;
+	u16 len_min;
+	u16 len_max;
 
 	switch (hw->phy.id) {
+	case M88E1543_E_PHY_ID:
+	case M88E1512_E_PHY_ID:
+	case I347AT4_E_PHY_ID:
 	case I210_I_PHY_ID:
-		/* Get cable length from PHY Cable Diagnostics Control Reg */
-		ret_val = phy->ops.read_reg(hw, (0x7 << GS40G_PAGE_SHIFT) +
-					    (I347AT4_PCDL + phy->addr),
-					    &phy_data);
+		/* Remember the original page select and set it to 7 */
+		ret_val = phy->ops.read_reg(hw, I347AT4_PAGE_SELECT,
+					    &default_page);
 		if (ret_val)
-			return ret_val;
+			goto out;
+
+		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT, 0x07);
+		if (ret_val)
+			goto out;
 
 		/* Check if the unit of cable length is meters or cm */
-		ret_val = phy->ops.read_reg(hw, (0x7 << GS40G_PAGE_SHIFT) +
-					    I347AT4_PCDC, &phy_data2);
+		ret_val = phy->ops.read_reg(hw, I347AT4_PCDC, &phy_data2);
 		if (ret_val)
-			return ret_val;
+			goto out;
 
 		is_cm = !(phy_data2 & I347AT4_PCDC_CABLE_LENGTH_UNIT);
 
-		/* Populate the phy structure with cable length in meters */
-		phy->min_cable_length = phy_data / (is_cm ? 100 : 1);
-		phy->max_cable_length = phy_data / (is_cm ? 100 : 1);
-		phy->cable_length = phy_data / (is_cm ? 100 : 1);
-		break;
-	case M88E1543_E_PHY_ID:
-	case M88E1512_E_PHY_ID:
-	case I347AT4_E_PHY_ID:
-		/* Remember the original page select and set it to 7 */
-		ret_val = phy->ops.read_reg(hw, I347AT4_PAGE_SELECT,
-					    &default_page);
+		/* Get cable length from Pair 0 length Regs */
+		ret_val = phy->ops.read_reg(hw, I347AT4_PCDL0, &phy_data);
 		if (ret_val)
 			goto out;
 
-		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT, 0x07);
+		phy->pair_length[0] = phy_data / (is_cm ? 100 : 1);
+		len_tot = phy->pair_length[0];
+		len_min = phy->pair_length[0];
+		len_max = phy->pair_length[0];
+
+		/* Get cable length from Pair 1 length Regs */
+		ret_val = phy->ops.read_reg(hw, I347AT4_PCDL1, &phy_data);
 		if (ret_val)
 			goto out;
 
-		/* Get cable length from PHY Cable Diagnostics Control Reg */
-		ret_val = phy->ops.read_reg(hw, (I347AT4_PCDL + phy->addr),
-					    &phy_data);
+		phy->pair_length[1] = phy_data / (is_cm ? 100 : 1);
+		len_tot += phy->pair_length[1];
+		len_min = min(len_min, phy->pair_length[1]);
+		len_max = max(len_max, phy->pair_length[1]);
+
+		/* Get cable length from Pair 2 length Regs */
+		ret_val = phy->ops.read_reg(hw, I347AT4_PCDL2, &phy_data);
 		if (ret_val)
 			goto out;
 
-		/* Check if the unit of cable length is meters or cm */
-		ret_val = phy->ops.read_reg(hw, I347AT4_PCDC, &phy_data2);
+		phy->pair_length[2] = phy_data / (is_cm ? 100 : 1);
+		len_tot += phy->pair_length[2];
+		len_min = min(len_min, phy->pair_length[2]);
+		len_max = max(len_max, phy->pair_length[2]);
+
+		/* Get cable length from Pair 3 length Regs */
+		ret_val = phy->ops.read_reg(hw, I347AT4_PCDL3, &phy_data);
 		if (ret_val)
 			goto out;
 
-		is_cm = !(phy_data2 & I347AT4_PCDC_CABLE_LENGTH_UNIT);
+		phy->pair_length[3] = phy_data / (is_cm ? 100 : 1);
+		len_tot += phy->pair_length[3];
+		len_min = min(len_min, phy->pair_length[3]);
+		len_max = max(len_max, phy->pair_length[3]);
 
 		/* Populate the phy structure with cable length in meters */
-		phy->min_cable_length = phy_data / (is_cm ? 100 : 1);
-		phy->max_cable_length = phy_data / (is_cm ? 100 : 1);
-		phy->cable_length = phy_data / (is_cm ? 100 : 1);
+		phy->min_cable_length = len_min;
+		phy->max_cable_length = len_max;
+		phy->cable_length = len_tot / 4;
 
 		/* Reset the page selec to its original value */
 		ret_val = phy->ops.write_reg(hw, I347AT4_PAGE_SELECT,
@@ -2278,6 +2295,100 @@ out:
 }
 
 /**
+ *  igb_initialize_M88E1543_phy - Initialize M88E1512 PHY
+ *  @hw: pointer to the HW structure
+ *
+ *  Initialize Marvell 1543 to work correctly with Avoton.
+ **/
+s32 igb_initialize_M88E1543_phy(struct e1000_hw *hw)
+{
+	struct e1000_phy_info *phy = &hw->phy;
+	s32 ret_val = 0;
+
+	/* Switch to PHY page 0xFF. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x00FF);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0x214B);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2144);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0x0C28);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2146);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0xB233);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x214D);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_2, 0xDC0C);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_1, 0x2159);
+	if (ret_val)
+		goto out;
+
+	/* Switch to PHY page 0xFB. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x00FB);
+	if (ret_val)
+		goto out;
+
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_CFG_REG_3, 0x0C0D);
+	if (ret_val)
+		goto out;
+
+	/* Switch to PHY page 0x12. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x12);
+	if (ret_val)
+		goto out;
+
+	/* Change mode to SGMII-to-Copper */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1512_MODE, 0x8001);
+	if (ret_val)
+		goto out;
+
+	/* Switch to PHY page 1. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0x1);
+	if (ret_val)
+		goto out;
+
+	/* Change mode to 1000BASE-X/SGMII and autoneg enable */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_FIBER_CTRL, 0x9140);
+	if (ret_val)
+		goto out;
+
+	/* Return the PHY to page 0. */
+	ret_val = phy->ops.write_reg(hw, E1000_M88E1543_PAGE_ADDR, 0);
+	if (ret_val)
+		goto out;
+
+	ret_val = igb_phy_sw_reset(hw);
+	if (ret_val) {
+		hw_dbg("Error committing the PHY changes\n");
+		return ret_val;
+	}
+
+	/* msec_delay(1000); */
+	usleep_range(1000, 2000);
+out:
+	return ret_val;
+}
+
+/**
  * igb_power_up_phy_copper - Restore copper link in case of PHY power down
  * @hw: pointer to the HW structure
  *
@@ -2494,66 +2605,6 @@ out:
 }
 
 /**
- *  igb_write_phy_reg_gs40g - Write GS40G PHY register
- *  @hw: pointer to the HW structure
- *  @offset: lower half is register offset to write to
- *     upper half is page to use.
- *  @data: data to write at register offset
- *
- *  Acquires semaphore, if necessary, then writes the data to PHY register
- *  at the offset.  Release any acquired semaphores before exiting.
- **/
-s32 igb_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data)
-{
-	s32 ret_val;
-	u16 page = offset >> GS40G_PAGE_SHIFT;
-
-	offset = offset & GS40G_OFFSET_MASK;
-	ret_val = hw->phy.ops.acquire(hw);
-	if (ret_val)
-		return ret_val;
-
-	ret_val = igb_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
-	if (ret_val)
-		goto release;
-	ret_val = igb_write_phy_reg_mdic(hw, offset, data);
-
-release:
-	hw->phy.ops.release(hw);
-	return ret_val;
-}
-
-/**
- *  igb_read_phy_reg_gs40g - Read GS40G  PHY register
- *  @hw: pointer to the HW structure
- *  @offset: lower half is register offset to read to
- *     upper half is page to use.
- *  @data: data to read at register offset
- *
- *  Acquires semaphore, if necessary, then reads the data in the PHY register
- *  at the offset.  Release any acquired semaphores before exiting.
- **/
-s32 igb_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data)
-{
-	s32 ret_val;
-	u16 page = offset >> GS40G_PAGE_SHIFT;
-
-	offset = offset & GS40G_OFFSET_MASK;
-	ret_val = hw->phy.ops.acquire(hw);
-	if (ret_val)
-		return ret_val;
-
-	ret_val = igb_write_phy_reg_mdic(hw, GS40G_PAGE_SELECT, page);
-	if (ret_val)
-		goto release;
-	ret_val = igb_read_phy_reg_mdic(hw, offset, data);
-
-release:
-	hw->phy.ops.release(hw);
-	return ret_val;
-}
-
-/**
  *  igb_set_master_slave_mode - Setup PHY for Master/slave mode
  *  @hw: pointer to the HW structure
  *
diff --git a/drivers/net/ethernet/intel/igb/e1000_phy.h b/drivers/net/ethernet/intel/igb/e1000_phy.h
index 24d55edbb0e3..969a6ddafa3b 100644
--- a/drivers/net/ethernet/intel/igb/e1000_phy.h
+++ b/drivers/net/ethernet/intel/igb/e1000_phy.h
@@ -62,6 +62,7 @@ void igb_power_up_phy_copper(struct e1000_hw *hw);
 void igb_power_down_phy_copper(struct e1000_hw *hw);
 s32  igb_phy_init_script_igp3(struct e1000_hw *hw);
 s32  igb_initialize_M88E1512_phy(struct e1000_hw *hw);
+s32  igb_initialize_M88E1543_phy(struct e1000_hw *hw);
 s32  igb_read_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 *data);
 s32  igb_write_phy_reg_mdic(struct e1000_hw *hw, u32 offset, u16 data);
 s32  igb_read_phy_reg_i2c(struct e1000_hw *hw, u32 offset, u16 *data);
@@ -71,8 +72,8 @@ s32  igb_copper_link_setup_82580(struct e1000_hw *hw);
 s32  igb_get_phy_info_82580(struct e1000_hw *hw);
 s32  igb_phy_force_speed_duplex_82580(struct e1000_hw *hw);
 s32  igb_get_cable_length_82580(struct e1000_hw *hw);
-s32  igb_read_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 *data);
-s32  igb_write_phy_reg_gs40g(struct e1000_hw *hw, u32 offset, u16 data);
+s32  igb_read_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 *data);
+s32  igb_write_phy_reg_82580(struct e1000_hw *hw, u32 offset, u16 data);
 s32  igb_check_polarity_m88(struct e1000_hw *hw);
 
 /* IGP01E1000 Specific Registers */
@@ -143,17 +144,6 @@ s32  igb_check_polarity_m88(struct e1000_hw *hw);
 
 #define E1000_CABLE_LENGTH_UNDEFINED      0xFF
 
-/* GS40G - I210 PHY defines */
-#define GS40G_PAGE_SELECT		0x16
-#define GS40G_PAGE_SHIFT		16
-#define GS40G_OFFSET_MASK		0xFFFF
-#define GS40G_PAGE_2			0x20000
-#define GS40G_MAC_REG2			0x15
-#define GS40G_MAC_LB			0x4140
-#define GS40G_MAC_SPEED_1G		0X0006
-#define GS40G_COPPER_SPEC		0x0010
-#define GS40G_LINE_LB			0x4000
-
 /* SFP modules ID memory locations */
 #define E1000_SFF_IDENTIFIER_OFFSET	0x00
 #define E1000_SFF_IDENTIFIER_SFF	0x02
diff --git a/drivers/net/ethernet/intel/igb/e1000_regs.h b/drivers/net/ethernet/intel/igb/e1000_regs.h
index 4af2870e49f8..21d9d02885cb 100644
--- a/drivers/net/ethernet/intel/igb/e1000_regs.h
+++ b/drivers/net/ethernet/intel/igb/e1000_regs.h
@@ -66,6 +66,7 @@
 #define E1000_PBA      0x01000  /* Packet Buffer Allocation - RW */
 #define E1000_PBS      0x01008  /* Packet Buffer Size */
 #define E1000_EEMNGCTL 0x01010  /* MNG EEprom Control */
+#define E1000_EEMNGCTL_I210 0x12030  /* MNG EEprom Control */
 #define E1000_EEARBC_I210 0x12024  /* EEPROM Auto Read Bus Control */
 #define E1000_EEWR     0x0102C  /* EEPROM Write Register - RW */
 #define E1000_I2CCMD   0x01028  /* SFPI2C Command Register - RW */
@@ -385,8 +386,7 @@ do { \
 #define array_wr32(reg, offset, value) \
 	wr32((reg) + ((offset) << 2), (value))
 
-#define array_rd32(reg, offset) \
-	(readl(hw->hw_addr + reg + ((offset) << 2)))
+#define array_rd32(reg, offset) (igb_rd32(hw, reg + ((offset) << 2)))
 
 /* DMA Coalescing registers */
 #define E1000_PCIEMISC	0x05BB8 /* PCIE misc config register */
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 1a2f1cc44b28..e3cb93bdb21a 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -389,6 +389,8 @@ struct igb_adapter {
 	u16 link_speed;
 	u16 link_duplex;
 
+	u8 __iomem *io_addr; /* Mainly for iounmap use */
+
 	struct work_struct reset_task;
 	struct work_struct watchdog_task;
 	bool fc_autoneg;
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index 2529bc625de4..1d329f1d047b 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -127,10 +127,20 @@ static const struct igb_stats igb_gstrings_net_stats[] = {
 #define IGB_STATS_LEN \
 	(IGB_GLOBAL_STATS_LEN + IGB_NETDEV_STATS_LEN + IGB_QUEUE_STATS_LEN)
 
+enum igb_diagnostics_results {
+	TEST_REG = 0,
+	TEST_EEP,
+	TEST_IRQ,
+	TEST_LOOP,
+	TEST_LINK
+};
+
 static const char igb_gstrings_test[][ETH_GSTRING_LEN] = {
-	"Register test  (offline)", "Eeprom test    (offline)",
-	"Interrupt test (offline)", "Loopback test  (offline)",
-	"Link test   (on/offline)"
+	[TEST_REG]  = "Register test  (offline)",
+	[TEST_EEP]  = "Eeprom test    (offline)",
+	[TEST_IRQ]  = "Interrupt test (offline)",
+	[TEST_LOOP] = "Loopback test  (offline)",
+	[TEST_LINK] = "Link test   (on/offline)"
 };
 #define IGB_TEST_LEN (sizeof(igb_gstrings_test) / ETH_GSTRING_LEN)
 
@@ -2002,7 +2012,7 @@ static void igb_diag_test(struct net_device *netdev,
 		/* Link test performed before hardware reset so autoneg doesn't
 		 * interfere with test result
 		 */
-		if (igb_link_test(adapter, &data[4]))
+		if (igb_link_test(adapter, &data[TEST_LINK]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		if (if_running)
@@ -2011,21 +2021,21 @@ static void igb_diag_test(struct net_device *netdev,
 		else
 			igb_reset(adapter);
 
-		if (igb_reg_test(adapter, &data[0]))
+		if (igb_reg_test(adapter, &data[TEST_REG]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		igb_reset(adapter);
-		if (igb_eeprom_test(adapter, &data[1]))
+		if (igb_eeprom_test(adapter, &data[TEST_EEP]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		igb_reset(adapter);
-		if (igb_intr_test(adapter, &data[2]))
+		if (igb_intr_test(adapter, &data[TEST_IRQ]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		igb_reset(adapter);
 		/* power up link for loopback test */
 		igb_power_up_link(adapter);
-		if (igb_loopback_test(adapter, &data[3]))
+		if (igb_loopback_test(adapter, &data[TEST_LOOP]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 
 		/* restore speed, duplex, autoneg settings */
@@ -2045,16 +2055,16 @@ static void igb_diag_test(struct net_device *netdev,
 		dev_info(&adapter->pdev->dev, "online testing starting\n");
 
 		/* PHY is powered down when interface is down */
-		if (if_running && igb_link_test(adapter, &data[4]))
+		if (if_running && igb_link_test(adapter, &data[TEST_LINK]))
 			eth_test->flags |= ETH_TEST_FL_FAILED;
 		else
-			data[4] = 0;
+			data[TEST_LINK] = 0;
 
 		/* Online tests aren't run; pass by default */
-		data[0] = 0;
-		data[1] = 0;
-		data[2] = 0;
-		data[3] = 0;
+		data[TEST_REG] = 0;
+		data[TEST_EEP] = 0;
+		data[TEST_IRQ] = 0;
+		data[TEST_LOOP] = 0;
 
 		clear_bit(__IGB_TESTING, &adapter->state);
 	}
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index ea7b09887245..31e5f3942839 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -946,7 +946,6 @@ static void igb_configure_msix(struct igb_adapter *adapter)
 static int igb_request_msix(struct igb_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	struct e1000_hw *hw = &adapter->hw;
 	int i, err = 0, vector = 0, free_vector = 0;
 
 	err = request_irq(adapter->msix_entries[vector].vector,
@@ -959,7 +958,7 @@ static int igb_request_msix(struct igb_adapter *adapter)
 
 		vector++;
 
-		q_vector->itr_register = hw->hw_addr + E1000_EITR(vector);
+		q_vector->itr_register = adapter->io_addr + E1000_EITR(vector);
 
 		if (q_vector->rx.ring && q_vector->tx.ring)
 			sprintf(q_vector->name, "%s-TxRx-%u", netdev->name,
@@ -1230,7 +1229,7 @@ static int igb_alloc_q_vector(struct igb_adapter *adapter,
 	q_vector->tx.work_limit = adapter->tx_work_limit;
 
 	/* initialize ITR configuration */
-	q_vector->itr_register = adapter->hw.hw_addr + E1000_EITR(0);
+	q_vector->itr_register = adapter->io_addr + E1000_EITR(0);
 	q_vector->itr_val = IGB_START_ITR;
 
 	/* initialize pointer to rings */
@@ -2294,9 +2293,11 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter->msg_enable = netif_msg_init(debug, DEFAULT_MSG_ENABLE);
 
 	err = -EIO;
-	hw->hw_addr = pci_iomap(pdev, 0, 0);
-	if (!hw->hw_addr)
+	adapter->io_addr = pci_iomap(pdev, 0, 0);
+	if (!adapter->io_addr)
 		goto err_ioremap;
+	/* hw->hw_addr can be altered, we'll use adapter->io_addr for unmap */
+	hw->hw_addr = adapter->io_addr;
 
 	netdev->netdev_ops = &igb_netdev_ops;
 	igb_set_ethtool_ops(netdev);
@@ -2378,8 +2379,8 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	if (hw->mac.type >= e1000_82576) {
-		netdev->hw_features |= NETIF_F_SCTP_CSUM;
-		netdev->features |= NETIF_F_SCTP_CSUM;
+		netdev->hw_features |= NETIF_F_SCTP_CRC;
+		netdev->features |= NETIF_F_SCTP_CRC;
 	}
 
 	netdev->priv_flags |= IFF_UNICAST_FLT;
@@ -2656,7 +2657,7 @@ err_sw_init:
 #ifdef CONFIG_PCI_IOV
 	igb_disable_sriov(pdev);
 #endif
-	pci_iounmap(pdev, hw->hw_addr);
+	pci_iounmap(pdev, adapter->io_addr);
 err_ioremap:
 	free_netdev(netdev);
 err_alloc_etherdev:
@@ -2823,7 +2824,7 @@ static void igb_remove(struct pci_dev *pdev)
 
 	igb_clear_interrupt_scheme(adapter);
 
-	pci_iounmap(pdev, hw->hw_addr);
+	pci_iounmap(pdev, adapter->io_addr);
 	if (hw->flash_address)
 		iounmap(hw->flash_address);
 	pci_release_selected_regions(pdev,
@@ -2856,6 +2857,13 @@ static void igb_probe_vfs(struct igb_adapter *adapter)
 	if ((hw->mac.type == e1000_i210) || (hw->mac.type == e1000_i211))
 		return;
 
+	/* Of the below we really only want the effect of getting
+	 * IGB_FLAG_HAS_MSIX set (if available), without which
+	 * igb_enable_sriov() has no effect.
+	 */
+	igb_set_interrupt_capability(adapter, true);
+	igb_reset_interrupt_capability(adapter);
+
 	pci_sriov_set_totalvfs(pdev, 7);
 	igb_enable_sriov(pdev, max_vfs);
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index 1d2174526a4c..4b9156cd8b93 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -139,6 +139,7 @@ enum ixgbe_tx_flags {
 #define IXGBE_X540_VF_DEVICE_ID         0x1515
 
 struct vf_data_storage {
+	struct pci_dev *vfdev;
 	unsigned char vf_mac_addresses[ETH_ALEN];
 	u16 vf_mc_hashes[IXGBE_MAX_VF_MC_ENTRIES];
 	u16 num_vf_mc_hashes;
@@ -224,6 +225,8 @@ struct ixgbe_rx_queue_stats {
 	u64 csum_err;
 };
 
+#define IXGBE_TS_HDR_LEN 8
+
 enum ixgbe_ring_state_t {
 	__IXGBE_TX_FDIR_INIT_DONE,
 	__IXGBE_TX_XPS_INIT_DONE,
@@ -282,6 +285,8 @@ struct ixgbe_ring {
 	u16 next_to_use;
 	u16 next_to_clean;
 
+	unsigned long last_rx_timestamp;
+
 	union {
 		u16 next_to_alloc;
 		struct {
@@ -312,7 +317,7 @@ enum ixgbe_ring_f_enum {
 };
 
 #define IXGBE_MAX_RSS_INDICES		16
-#define IXGBE_MAX_RSS_INDICES_X550	64
+#define IXGBE_MAX_RSS_INDICES_X550	63
 #define IXGBE_MAX_VMDQ_INDICES		64
 #define IXGBE_MAX_FDIR_INDICES		63	/* based on q_vector limit */
 #define IXGBE_MAX_FCOE_INDICES		8
@@ -587,9 +592,10 @@ static inline u16 ixgbe_desc_unused(struct ixgbe_ring *ring)
 
 struct ixgbe_mac_addr {
 	u8 addr[ETH_ALEN];
-	u16 queue;
+	u16 pool;
 	u16 state; /* bitmask */
 };
+
 #define IXGBE_MAC_STATE_DEFAULT		0x1
 #define IXGBE_MAC_STATE_MODIFIED	0x2
 #define IXGBE_MAC_STATE_IN_USE		0x4
@@ -639,6 +645,8 @@ struct ixgbe_adapter {
 #define IXGBE_FLAG_SRIOV_CAPABLE                (u32)(1 << 22)
 #define IXGBE_FLAG_SRIOV_ENABLED                (u32)(1 << 23)
 #define IXGBE_FLAG_VXLAN_OFFLOAD_CAPABLE	BIT(24)
+#define IXGBE_FLAG_RX_HWTSTAMP_ENABLED		BIT(25)
+#define IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER	BIT(26)
 
 	u32 flags2;
 #define IXGBE_FLAG2_RSC_CAPABLE                 (u32)(1 << 0)
@@ -656,6 +664,7 @@ struct ixgbe_adapter {
 #ifdef CONFIG_IXGBE_VXLAN
 #define IXGBE_FLAG2_VXLAN_REREG_NEEDED		BIT(12)
 #endif
+#define IXGBE_FLAG2_VLAN_PROMISC		BIT(13)
 
 	/* Tx fast path data */
 	int num_tx_queues;
@@ -755,9 +764,12 @@ struct ixgbe_adapter {
 	unsigned long last_rx_ptp_check;
 	unsigned long last_rx_timestamp;
 	spinlock_t tmreg_lock;
-	struct cyclecounter cc;
-	struct timecounter tc;
+	struct cyclecounter hw_cc;
+	struct timecounter hw_tc;
 	u32 base_incval;
+	u32 tx_hwtstamp_timeouts;
+	u32 rx_hwtstamp_cleared;
+	void (*ptp_setup_sdp)(struct ixgbe_adapter *);
 
 	/* SR-IOV */
 	DECLARE_BITMAP(active_vfs, IXGBE_MAX_VF_FUNCTIONS);
@@ -883,9 +895,10 @@ int ixgbe_wol_supported(struct ixgbe_adapter *adapter, u16 device_id,
 void ixgbe_full_sync_mac_table(struct ixgbe_adapter *adapter);
 #endif
 int ixgbe_add_mac_filter(struct ixgbe_adapter *adapter,
-			 u8 *addr, u16 queue);
+			 const u8 *addr, u16 queue);
 int ixgbe_del_mac_filter(struct ixgbe_adapter *adapter,
-			 u8 *addr, u16 queue);
+			 const u8 *addr, u16 queue);
+void ixgbe_update_pf_promisc_vlvf(struct ixgbe_adapter *adapter, u32 vid);
 void ixgbe_clear_interrupt_scheme(struct ixgbe_adapter *adapter);
 netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *, struct ixgbe_adapter *,
 				  struct ixgbe_ring *);
@@ -968,12 +981,33 @@ void ixgbe_ptp_suspend(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_stop(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_overflow_check(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter);
-void ixgbe_ptp_rx_hwtstamp(struct ixgbe_adapter *adapter, struct sk_buff *skb);
+void ixgbe_ptp_rx_pktstamp(struct ixgbe_q_vector *, struct sk_buff *);
+void ixgbe_ptp_rx_rgtstamp(struct ixgbe_q_vector *, struct sk_buff *skb);
+static inline void ixgbe_ptp_rx_hwtstamp(struct ixgbe_ring *rx_ring,
+					 union ixgbe_adv_rx_desc *rx_desc,
+					 struct sk_buff *skb)
+{
+	if (unlikely(ixgbe_test_staterr(rx_desc, IXGBE_RXD_STAT_TSIP))) {
+		ixgbe_ptp_rx_pktstamp(rx_ring->q_vector, skb);
+		return;
+	}
+
+	if (unlikely(!ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS)))
+		return;
+
+	ixgbe_ptp_rx_rgtstamp(rx_ring->q_vector, skb);
+
+	/* Update the last_rx_timestamp timer in order to enable watchdog check
+	 * for error case of latched timestamp on a dropped packet.
+	 */
+	rx_ring->last_rx_timestamp = jiffies;
+}
+
 int ixgbe_ptp_set_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr);
 int ixgbe_ptp_get_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr);
 void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter);
 void ixgbe_ptp_reset(struct ixgbe_adapter *adapter);
-void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter, u32 eicr);
+void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter);
 #ifdef CONFIG_PCI_IOV
 void ixgbe_sriov_reinit(struct ixgbe_adapter *adapter);
 #endif
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
index 65db69b862fb..d8a9fb8a59e2 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82598.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 2015 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -765,13 +765,14 @@ mac_reset_top:
 	ctrl = IXGBE_READ_REG(hw, IXGBE_CTRL) | IXGBE_CTRL_RST;
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, ctrl);
 	IXGBE_WRITE_FLUSH(hw);
+	usleep_range(1000, 1200);
 
 	/* Poll for reset bit to self-clear indicating reset is complete */
 	for (i = 0; i < 10; i++) {
-		udelay(1);
 		ctrl = IXGBE_READ_REG(hw, IXGBE_CTRL);
 		if (!(ctrl & IXGBE_CTRL_RST))
 			break;
+		udelay(1);
 	}
 	if (ctrl & IXGBE_CTRL_RST) {
 		status = IXGBE_ERR_RESET_FAILED;
@@ -879,11 +880,12 @@ static s32 ixgbe_clear_vmdq_82598(struct ixgbe_hw *hw, u32 rar, u32 vmdq)
  *  @vlan: VLAN id to write to VLAN filter
  *  @vind: VMDq output index that maps queue to VLAN id in VFTA
  *  @vlan_on: boolean flag to turn on/off VLAN in VFTA
+ *  @vlvf_bypass: boolean flag - unused
  *
  *  Turn on/off specified VLAN in the VLAN filter table.
  **/
 static s32 ixgbe_set_vfta_82598(struct ixgbe_hw *hw, u32 vlan, u32 vind,
-				bool vlan_on)
+				bool vlan_on, bool vlvf_bypass)
 {
 	u32 regindex;
 	u32 bitindex;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
index a39afcf03e2c..fa8d4f40ac2a 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_82599.c
@@ -990,13 +990,14 @@ mac_reset_top:
 	ctrl |= IXGBE_READ_REG(hw, IXGBE_CTRL);
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, ctrl);
 	IXGBE_WRITE_FLUSH(hw);
+	usleep_range(1000, 1200);
 
 	/* Poll for reset bit to self-clear indicating reset is complete */
 	for (i = 0; i < 10; i++) {
-		udelay(1);
 		ctrl = IXGBE_READ_REG(hw, IXGBE_CTRL);
 		if (!(ctrl & IXGBE_CTRL_RST_MASK))
 			break;
+		udelay(1);
 	}
 
 	if (ctrl & IXGBE_CTRL_RST_MASK) {
@@ -1082,12 +1083,16 @@ mac_reset_top:
 
 	/* Add the SAN MAC address to the RAR only if it's a valid address */
 	if (is_valid_ether_addr(hw->mac.san_addr)) {
-		hw->mac.ops.set_rar(hw, hw->mac.num_rar_entries - 1,
-				    hw->mac.san_addr, 0, IXGBE_RAH_AV);
-
 		/* Save the SAN MAC RAR index */
 		hw->mac.san_mac_rar_index = hw->mac.num_rar_entries - 1;
 
+		hw->mac.ops.set_rar(hw, hw->mac.san_mac_rar_index,
+				    hw->mac.san_addr, 0, IXGBE_RAH_AV);
+
+		/* clear VMDq pool/queue selection for this RAR */
+		hw->mac.ops.clear_vmdq(hw, hw->mac.san_mac_rar_index,
+				       IXGBE_CLEAR_VMDQ_ALL);
+
 		/* Reserve the last RAR for the SAN MAC address */
 		hw->mac.num_rar_entries--;
 	}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
index ce61b36b94f1..64045053e874 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 2015 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -1884,10 +1884,11 @@ s32 ixgbe_init_rx_addrs_generic(struct ixgbe_hw *hw)
 		hw_dbg(hw, " New MAC Addr =%pM\n", hw->mac.addr);
 
 		hw->mac.ops.set_rar(hw, 0, hw->mac.addr, 0, IXGBE_RAH_AV);
-
-		/*  clear VMDq pool/queue selection for RAR 0 */
-		hw->mac.ops.clear_vmdq(hw, 0, IXGBE_CLEAR_VMDQ_ALL);
 	}
+
+	/*  clear VMDq pool/queue selection for RAR 0 */
+	hw->mac.ops.clear_vmdq(hw, 0, IXGBE_CLEAR_VMDQ_ALL);
+
 	hw->addr_ctrl.overflow_promisc = 0;
 
 	hw->addr_ctrl.rar_used_count = 1;
@@ -2454,6 +2455,17 @@ static s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw)
 	/* Always set this bit to ensure any future transactions are blocked */
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, IXGBE_CTRL_GIO_DIS);
 
+	/* Poll for bit to read as set */
+	for (i = 0; i < IXGBE_PCI_MASTER_DISABLE_TIMEOUT; i++) {
+		if (IXGBE_READ_REG(hw, IXGBE_CTRL) & IXGBE_CTRL_GIO_DIS)
+			break;
+		usleep_range(100, 120);
+	}
+	if (i >= IXGBE_PCI_MASTER_DISABLE_TIMEOUT) {
+		hw_dbg(hw, "GIO disable did not set - requesting resets\n");
+		goto gio_disable_fail;
+	}
+
 	/* Exit if master requests are blocked */
 	if (!(IXGBE_READ_REG(hw, IXGBE_STATUS) & IXGBE_STATUS_GIO) ||
 	    ixgbe_removed(hw->hw_addr))
@@ -2475,6 +2487,7 @@ static s32 ixgbe_disable_pcie_master(struct ixgbe_hw *hw)
 	 * again to clear out any effects they may have had on our device.
 	 */
 	hw_dbg(hw, "GIO Master Disable bit didn't clear - requesting resets\n");
+gio_disable_fail:
 	hw->mac.flags |= IXGBE_FLAGS_DOUBLE_RESET_REQUIRED;
 
 	if (hw->mac.type >= ixgbe_mac_X550)
@@ -2987,43 +3000,44 @@ s32 ixgbe_init_uta_tables_generic(struct ixgbe_hw *hw)
  *  return the VLVF index where this VLAN id should be placed
  *
  **/
-static s32 ixgbe_find_vlvf_slot(struct ixgbe_hw *hw, u32 vlan)
+static s32 ixgbe_find_vlvf_slot(struct ixgbe_hw *hw, u32 vlan, bool vlvf_bypass)
 {
-	u32 bits = 0;
-	u32 first_empty_slot = 0;
-	s32 regindex;
+	s32 regindex, first_empty_slot;
+	u32 bits;
 
 	/* short cut the special case */
 	if (vlan == 0)
 		return 0;
 
-	/*
-	  * Search for the vlan id in the VLVF entries. Save off the first empty
-	  * slot found along the way
-	  */
-	for (regindex = 1; regindex < IXGBE_VLVF_ENTRIES; regindex++) {
+	/* if vlvf_bypass is set we don't want to use an empty slot, we
+	 * will simply bypass the VLVF if there are no entries present in the
+	 * VLVF that contain our VLAN
+	 */
+	first_empty_slot = vlvf_bypass ? IXGBE_ERR_NO_SPACE : 0;
+
+	/* add VLAN enable bit for comparison */
+	vlan |= IXGBE_VLVF_VIEN;
+
+	/* Search for the vlan id in the VLVF entries. Save off the first empty
+	 * slot found along the way.
+	 *
+	 * pre-decrement loop covering (IXGBE_VLVF_ENTRIES - 1) .. 1
+	 */
+	for (regindex = IXGBE_VLVF_ENTRIES; --regindex;) {
 		bits = IXGBE_READ_REG(hw, IXGBE_VLVF(regindex));
-		if (!bits && !(first_empty_slot))
+		if (bits == vlan)
+			return regindex;
+		if (!first_empty_slot && !bits)
 			first_empty_slot = regindex;
-		else if ((bits & 0x0FFF) == vlan)
-			break;
 	}
 
-	/*
-	  * If regindex is less than IXGBE_VLVF_ENTRIES, then we found the vlan
-	  * in the VLVF. Else use the first empty VLVF register for this
-	  * vlan id.
-	  */
-	if (regindex >= IXGBE_VLVF_ENTRIES) {
-		if (first_empty_slot)
-			regindex = first_empty_slot;
-		else {
-			hw_dbg(hw, "No space in VLVF.\n");
-			regindex = IXGBE_ERR_NO_SPACE;
-		}
-	}
+	/* If we are here then we didn't find the VLAN.  Return first empty
+	 * slot we found during our search, else error.
+	 */
+	if (!first_empty_slot)
+		hw_dbg(hw, "No space in VLVF.\n");
 
-	return regindex;
+	return first_empty_slot ? : IXGBE_ERR_NO_SPACE;
 }
 
 /**
@@ -3032,21 +3046,17 @@ static s32 ixgbe_find_vlvf_slot(struct ixgbe_hw *hw, u32 vlan)
  *  @vlan: VLAN id to write to VLAN filter
  *  @vind: VMDq output index that maps queue to VLAN id in VFVFB
  *  @vlan_on: boolean flag to turn on/off VLAN in VFVF
+ *  @vlvf_bypass: boolean flag indicating updating default pool is okay
  *
  *  Turn on/off specified VLAN in the VLAN filter table.
  **/
 s32 ixgbe_set_vfta_generic(struct ixgbe_hw *hw, u32 vlan, u32 vind,
-			   bool vlan_on)
+			   bool vlan_on, bool vlvf_bypass)
 {
-	s32 regindex;
-	u32 bitindex;
-	u32 vfta;
-	u32 bits;
-	u32 vt;
-	u32 targetbit;
-	bool vfta_changed = false;
+	u32 regidx, vfta_delta, vfta, bits;
+	s32 vlvf_index;
 
-	if (vlan > 4095)
+	if ((vlan > 4095) || (vind > 63))
 		return IXGBE_ERR_PARAM;
 
 	/*
@@ -3061,22 +3071,16 @@ s32 ixgbe_set_vfta_generic(struct ixgbe_hw *hw, u32 vlan, u32 vind,
 	 *    bits[11-5]: which register
 	 *    bits[4-0]:  which bit in the register
 	 */
-	regindex = (vlan >> 5) & 0x7F;
-	bitindex = vlan & 0x1F;
-	targetbit = (1 << bitindex);
-	vfta = IXGBE_READ_REG(hw, IXGBE_VFTA(regindex));
-
-	if (vlan_on) {
-		if (!(vfta & targetbit)) {
-			vfta |= targetbit;
-			vfta_changed = true;
-		}
-	} else {
-		if ((vfta & targetbit)) {
-			vfta &= ~targetbit;
-			vfta_changed = true;
-		}
-	}
+	regidx = vlan / 32;
+	vfta_delta = 1 << (vlan % 32);
+	vfta = IXGBE_READ_REG(hw, IXGBE_VFTA(regidx));
+
+	/* vfta_delta represents the difference between the current value
+	 * of vfta and the value we want in the register.  Since the diff
+	 * is an XOR mask we can just update vfta using an XOR.
+	 */
+	vfta_delta &= vlan_on ? ~vfta : vfta;
+	vfta ^= vfta_delta;
 
 	/* Part 2
 	 * If VT Mode is set
@@ -3086,85 +3090,67 @@ s32 ixgbe_set_vfta_generic(struct ixgbe_hw *hw, u32 vlan, u32 vind,
 	 *   Or !vlan_on
 	 *     clear the pool bit and possibly the vind
 	 */
-	vt = IXGBE_READ_REG(hw, IXGBE_VT_CTL);
-	if (vt & IXGBE_VT_CTL_VT_ENABLE) {
-		s32 vlvf_index;
-
-		vlvf_index = ixgbe_find_vlvf_slot(hw, vlan);
-		if (vlvf_index < 0)
-			return vlvf_index;
-
-		if (vlan_on) {
-			/* set the pool bit */
-			if (vind < 32) {
-				bits = IXGBE_READ_REG(hw,
-						IXGBE_VLVFB(vlvf_index*2));
-				bits |= (1 << vind);
-				IXGBE_WRITE_REG(hw,
-						IXGBE_VLVFB(vlvf_index*2),
-						bits);
-			} else {
-				bits = IXGBE_READ_REG(hw,
-						IXGBE_VLVFB((vlvf_index*2)+1));
-				bits |= (1 << (vind-32));
-				IXGBE_WRITE_REG(hw,
-						IXGBE_VLVFB((vlvf_index*2)+1),
-						bits);
-			}
-		} else {
-			/* clear the pool bit */
-			if (vind < 32) {
-				bits = IXGBE_READ_REG(hw,
-						IXGBE_VLVFB(vlvf_index*2));
-				bits &= ~(1 << vind);
-				IXGBE_WRITE_REG(hw,
-						IXGBE_VLVFB(vlvf_index*2),
-						bits);
-				bits |= IXGBE_READ_REG(hw,
-						IXGBE_VLVFB((vlvf_index*2)+1));
-			} else {
-				bits = IXGBE_READ_REG(hw,
-						IXGBE_VLVFB((vlvf_index*2)+1));
-				bits &= ~(1 << (vind-32));
-				IXGBE_WRITE_REG(hw,
-						IXGBE_VLVFB((vlvf_index*2)+1),
-						bits);
-				bits |= IXGBE_READ_REG(hw,
-						IXGBE_VLVFB(vlvf_index*2));
-			}
-		}
+	if (!(IXGBE_READ_REG(hw, IXGBE_VT_CTL) & IXGBE_VT_CTL_VT_ENABLE))
+		goto vfta_update;
+
+	vlvf_index = ixgbe_find_vlvf_slot(hw, vlan, vlvf_bypass);
+	if (vlvf_index < 0) {
+		if (vlvf_bypass)
+			goto vfta_update;
+		return vlvf_index;
+	}
 
-		/*
-		 * If there are still bits set in the VLVFB registers
-		 * for the VLAN ID indicated we need to see if the
-		 * caller is requesting that we clear the VFTA entry bit.
-		 * If the caller has requested that we clear the VFTA
-		 * entry bit but there are still pools/VFs using this VLAN
-		 * ID entry then ignore the request.  We're not worried
-		 * about the case where we're turning the VFTA VLAN ID
-		 * entry bit on, only when requested to turn it off as
-		 * there may be multiple pools and/or VFs using the
-		 * VLAN ID entry.  In that case we cannot clear the
-		 * VFTA bit until all pools/VFs using that VLAN ID have also
-		 * been cleared.  This will be indicated by "bits" being
-		 * zero.
+	bits = IXGBE_READ_REG(hw, IXGBE_VLVFB(vlvf_index * 2 + vind / 32));
+
+	/* set the pool bit */
+	bits |= 1 << (vind % 32);
+	if (vlan_on)
+		goto vlvf_update;
+
+	/* clear the pool bit */
+	bits ^= 1 << (vind % 32);
+
+	if (!bits &&
+	    !IXGBE_READ_REG(hw, IXGBE_VLVFB(vlvf_index * 2 + 1 - vind / 32))) {
+		/* Clear VFTA first, then disable VLVF.  Otherwise
+		 * we run the risk of stray packets leaking into
+		 * the PF via the default pool
 		 */
-		if (bits) {
-			IXGBE_WRITE_REG(hw, IXGBE_VLVF(vlvf_index),
-					(IXGBE_VLVF_VIEN | vlan));
-			if (!vlan_on) {
-				/* someone wants to clear the vfta entry
-				 * but some pools/VFs are still using it.
-				 * Ignore it. */
-				vfta_changed = false;
-			}
-		} else {
-			IXGBE_WRITE_REG(hw, IXGBE_VLVF(vlvf_index), 0);
-		}
+		if (vfta_delta)
+			IXGBE_WRITE_REG(hw, IXGBE_VFTA(regidx), vfta);
+
+		/* disable VLVF and clear remaining bit from pool */
+		IXGBE_WRITE_REG(hw, IXGBE_VLVF(vlvf_index), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(vlvf_index * 2 + vind / 32), 0);
+
+		return 0;
 	}
 
-	if (vfta_changed)
-		IXGBE_WRITE_REG(hw, IXGBE_VFTA(regindex), vfta);
+	/* If there are still bits set in the VLVFB registers
+	 * for the VLAN ID indicated we need to see if the
+	 * caller is requesting that we clear the VFTA entry bit.
+	 * If the caller has requested that we clear the VFTA
+	 * entry bit but there are still pools/VFs using this VLAN
+	 * ID entry then ignore the request.  We're not worried
+	 * about the case where we're turning the VFTA VLAN ID
+	 * entry bit on, only when requested to turn it off as
+	 * there may be multiple pools and/or VFs using the
+	 * VLAN ID entry.  In that case we cannot clear the
+	 * VFTA bit until all pools/VFs using that VLAN ID have also
+	 * been cleared.  This will be indicated by "bits" being
+	 * zero.
+	 */
+	vfta_delta = 0;
+
+vlvf_update:
+	/* record pool change and enable VLAN ID if not already enabled */
+	IXGBE_WRITE_REG(hw, IXGBE_VLVFB(vlvf_index * 2 + vind / 32), bits);
+	IXGBE_WRITE_REG(hw, IXGBE_VLVF(vlvf_index), IXGBE_VLVF_VIEN | vlan);
+
+vfta_update:
+	/* Update VFTA now that we are ready for traffic */
+	if (vfta_delta)
+		IXGBE_WRITE_REG(hw, IXGBE_VFTA(regidx), vfta);
 
 	return 0;
 }
@@ -3184,8 +3170,8 @@ s32 ixgbe_clear_vfta_generic(struct ixgbe_hw *hw)
 
 	for (offset = 0; offset < IXGBE_VLVF_ENTRIES; offset++) {
 		IXGBE_WRITE_REG(hw, IXGBE_VLVF(offset), 0);
-		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(offset*2), 0);
-		IXGBE_WRITE_REG(hw, IXGBE_VLVFB((offset*2)+1), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(offset * 2), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(offset * 2 + 1), 0);
 	}
 
 	return 0;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
index a0044e4a8b90..2b9563137fd8 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_common.h
@@ -92,7 +92,7 @@ s32 ixgbe_set_vmdq_san_mac_generic(struct ixgbe_hw *hw, u32 vmdq);
 s32 ixgbe_clear_vmdq_generic(struct ixgbe_hw *hw, u32 rar, u32 vmdq);
 s32 ixgbe_init_uta_tables_generic(struct ixgbe_hw *hw);
 s32 ixgbe_set_vfta_generic(struct ixgbe_hw *hw, u32 vlan,
-			   u32 vind, bool vlan_on);
+			   u32 vind, bool vlan_on, bool vlvf_bypass);
 s32 ixgbe_clear_vfta_generic(struct ixgbe_hw *hw);
 s32 ixgbe_check_mac_link_generic(struct ixgbe_hw *hw,
 				 ixgbe_link_speed *speed,
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
index a507a6fe3624..02c7333a9c83 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb.c
@@ -139,6 +139,11 @@ s32 ixgbe_dcb_calculate_tc_credits(struct ixgbe_hw *hw,
 		/* Calculate credit refill ratio using multiplier */
 		credit_refill = min(link_percentage * min_multiplier,
 				    MAX_CREDIT_REFILL);
+
+		/* Refill at least minimum credit */
+		if (credit_refill < min_credit)
+			credit_refill = min_credit;
+
 		p->data_credits_refill = (u16)credit_refill;
 
 		/* Calculate maximum credit for the TC */
@@ -149,7 +154,7 @@ s32 ixgbe_dcb_calculate_tc_credits(struct ixgbe_hw *hw,
 		 * of a TC is too small, the maximum credit may not be
 		 * enough to send out a jumbo frame in data plane arbitration.
 		 */
-		if (credit_max && (credit_max < min_credit))
+		if (credit_max < min_credit)
 			credit_max = min_credit;
 
 		if (direction == DCB_TX_CONFIG) {
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
index 23277ab153b6..b5cc989a3d23 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_dcb_82599.c
@@ -223,13 +223,13 @@ s32 ixgbe_dcb_config_pfc_82599(struct ixgbe_hw *hw, u8 pfc_en, u8 *prio_tc)
 	reg |= IXGBE_MFLCN_DPF;
 
 	/*
-	 * X540 supports per TC Rx priority flow control.  So
-	 * clear all TCs and only enable those that should be
+	 * X540 & X550 supports per TC Rx priority flow control.
+	 * So clear all TCs and only enable those that should be
 	 * enabled.
 	 */
 	reg &= ~(IXGBE_MFLCN_RPFCE_MASK | IXGBE_MFLCN_RFCE);
 
-	if (hw->mac.type == ixgbe_mac_X540)
+	if (hw->mac.type >= ixgbe_mac_X540)
 		reg |= pfc_en << IXGBE_MFLCN_RPFCE_SHIFT;
 
 	if (pfc_en)
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index d681273bd39d..bea96b3bc90c 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -151,47 +151,70 @@ static const char ixgbe_gstrings_test[][ETH_GSTRING_LEN] = {
 };
 #define IXGBE_TEST_LEN sizeof(ixgbe_gstrings_test) / ETH_GSTRING_LEN
 
+/* currently supported speeds for 10G */
+#define ADVRTSD_MSK_10G (SUPPORTED_10000baseT_Full | \
+			 SUPPORTED_10000baseKX4_Full | \
+			 SUPPORTED_10000baseKR_Full)
+
+#define ixgbe_isbackplane(type) ((type) == ixgbe_media_type_backplane)
+
+static u32 ixgbe_get_supported_10gtypes(struct ixgbe_hw *hw)
+{
+	if (!ixgbe_isbackplane(hw->phy.media_type))
+		return SUPPORTED_10000baseT_Full;
+
+	switch (hw->device_id) {
+	case IXGBE_DEV_ID_82598:
+	case IXGBE_DEV_ID_82599_KX4:
+	case IXGBE_DEV_ID_82599_KX4_MEZZ:
+	case IXGBE_DEV_ID_X550EM_X_KX4:
+		return SUPPORTED_10000baseKX4_Full;
+	case IXGBE_DEV_ID_82598_BX:
+	case IXGBE_DEV_ID_82599_KR:
+	case IXGBE_DEV_ID_X550EM_X_KR:
+		return SUPPORTED_10000baseKR_Full;
+	default:
+		return SUPPORTED_10000baseKX4_Full |
+		       SUPPORTED_10000baseKR_Full;
+	}
+}
+
 static int ixgbe_get_settings(struct net_device *netdev,
 			      struct ethtool_cmd *ecmd)
 {
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 	ixgbe_link_speed supported_link;
-	u32 link_speed = 0;
 	bool autoneg = false;
-	bool link_up;
 
 	hw->mac.ops.get_link_capabilities(hw, &supported_link, &autoneg);
 
 	/* set the supported link speeds */
 	if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
-		ecmd->supported |= SUPPORTED_10000baseT_Full;
-	if (supported_link & IXGBE_LINK_SPEED_2_5GB_FULL)
-		ecmd->supported |= SUPPORTED_2500baseX_Full;
+		ecmd->supported |= ixgbe_get_supported_10gtypes(hw);
 	if (supported_link & IXGBE_LINK_SPEED_1GB_FULL)
 		ecmd->supported |= SUPPORTED_1000baseT_Full;
 	if (supported_link & IXGBE_LINK_SPEED_100_FULL)
-		ecmd->supported |= SUPPORTED_100baseT_Full;
+		ecmd->supported |= ixgbe_isbackplane(hw->phy.media_type) ?
+				   SUPPORTED_1000baseKX_Full :
+				   SUPPORTED_1000baseT_Full;
 
+	/* default advertised speed if phy.autoneg_advertised isn't set */
+	ecmd->advertising = ecmd->supported;
 	/* set the advertised speeds */
 	if (hw->phy.autoneg_advertised) {
+		ecmd->advertising = 0;
 		if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_100_FULL)
 			ecmd->advertising |= ADVERTISED_100baseT_Full;
 		if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_10GB_FULL)
-			ecmd->advertising |= ADVERTISED_10000baseT_Full;
-		if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_2_5GB_FULL)
-			ecmd->advertising |= ADVERTISED_2500baseX_Full;
-		if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_1GB_FULL)
-			ecmd->advertising |= ADVERTISED_1000baseT_Full;
+			ecmd->advertising |= ecmd->supported & ADVRTSD_MSK_10G;
+		if (hw->phy.autoneg_advertised & IXGBE_LINK_SPEED_1GB_FULL) {
+			if (ecmd->supported & SUPPORTED_1000baseKX_Full)
+				ecmd->advertising |= ADVERTISED_1000baseKX_Full;
+			else
+				ecmd->advertising |= ADVERTISED_1000baseT_Full;
+		}
 	} else {
-		/* default modes in case phy.autoneg_advertised isn't set */
-		if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
-			ecmd->advertising |= ADVERTISED_10000baseT_Full;
-		if (supported_link & IXGBE_LINK_SPEED_1GB_FULL)
-			ecmd->advertising |= ADVERTISED_1000baseT_Full;
-		if (supported_link & IXGBE_LINK_SPEED_100_FULL)
-			ecmd->advertising |= ADVERTISED_100baseT_Full;
-
 		if (hw->phy.multispeed_fiber && !autoneg) {
 			if (supported_link & IXGBE_LINK_SPEED_10GB_FULL)
 				ecmd->advertising = ADVERTISED_10000baseT_Full;
@@ -229,6 +252,10 @@ static int ixgbe_get_settings(struct net_device *netdev,
 	case ixgbe_phy_sfp_avago:
 	case ixgbe_phy_sfp_intel:
 	case ixgbe_phy_sfp_unknown:
+	case ixgbe_phy_qsfp_passive_unknown:
+	case ixgbe_phy_qsfp_active_unknown:
+	case ixgbe_phy_qsfp_intel:
+	case ixgbe_phy_qsfp_unknown:
 		/* SFP+ devices, further checking needed */
 		switch (adapter->hw.phy.sfp_type) {
 		case ixgbe_sfp_type_da_cu:
@@ -284,9 +311,8 @@ static int ixgbe_get_settings(struct net_device *netdev,
 		break;
 	}
 
-	hw->mac.ops.check_link(hw, &link_speed, &link_up, false);
-	if (link_up) {
-		switch (link_speed) {
+	if (netif_carrier_ok(netdev)) {
+		switch (adapter->link_speed) {
 		case IXGBE_LINK_SPEED_10GB_FULL:
 			ethtool_cmd_speed_set(ecmd, SPEED_10000);
 			break;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
index 631c603fc966..2a653ec954f5 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_fcoe.c
@@ -77,7 +77,7 @@ int ixgbe_fcoe_ddp_put(struct net_device *netdev, u16 xid)
 	if (!netdev)
 		return 0;
 
-	if (xid >= IXGBE_FCOE_DDP_MAX)
+	if (xid >= netdev->fcoe_ddp_xid)
 		return 0;
 
 	adapter = netdev_priv(netdev);
@@ -177,7 +177,7 @@ static int ixgbe_fcoe_ddp_setup(struct net_device *netdev, u16 xid,
 		return 0;
 
 	adapter = netdev_priv(netdev);
-	if (xid >= IXGBE_FCOE_DDP_MAX) {
+	if (xid >= netdev->fcoe_ddp_xid) {
 		e_warn(drv, "xid=0x%x out-of-range\n", xid);
 		return 0;
 	}
@@ -517,6 +517,7 @@ int ixgbe_fso(struct ixgbe_ring *tx_ring,
 	u32 vlan_macip_lens;
 	u32 fcoe_sof_eof = 0;
 	u32 mss_l4len_idx;
+	u32 type_tucmd = IXGBE_ADVTXT_TUCMD_FCOE;
 	u8 sof, eof;
 
 	if (skb_is_gso(skb) && (skb_shinfo(skb)->gso_type != SKB_GSO_FCOE)) {
@@ -593,6 +594,8 @@ int ixgbe_fso(struct ixgbe_ring *tx_ring,
 					       skb_shinfo(skb)->gso_size);
 		first->bytecount += (first->gso_segs - 1) * *hdr_len;
 		first->tx_flags |= IXGBE_TX_FLAGS_TSO;
+		/* Hardware expects L4T to be RSV for FCoE TSO */
+		type_tucmd |= IXGBE_ADVTXD_TUCMD_L4T_RSV;
 	}
 
 	/* set flag indicating FCOE to ixgbe_tx_map call */
@@ -610,7 +613,7 @@ int ixgbe_fso(struct ixgbe_ring *tx_ring,
 
 	/* write context desc */
 	ixgbe_tx_ctxtdesc(tx_ring, vlan_macip_lens, fcoe_sof_eof,
-			  IXGBE_ADVTXT_TUCMD_FCOE, mss_l4len_idx);
+			  type_tucmd, mss_l4len_idx);
 
 	return 0;
 }
@@ -620,8 +623,7 @@ static void ixgbe_fcoe_dma_pool_free(struct ixgbe_fcoe *fcoe, unsigned int cpu)
 	struct ixgbe_fcoe_ddp_pool *ddp_pool;
 
 	ddp_pool = per_cpu_ptr(fcoe->ddp_pool, cpu);
-	if (ddp_pool->pool)
-		dma_pool_destroy(ddp_pool->pool);
+	dma_pool_destroy(ddp_pool->pool);
 	ddp_pool->pool = NULL;
 }
 
@@ -997,8 +999,7 @@ int ixgbe_fcoe_get_hbainfo(struct net_device *netdev,
 		return -EINVAL;
 
 	/* Don't return information on unsupported devices */
-	if (hw->mac.type != ixgbe_mac_82599EB &&
-	    hw->mac.type != ixgbe_mac_X540)
+	if (!(adapter->flags & IXGBE_FLAG_FCOE_ENABLED))
 		return -EINVAL;
 
 	/* Manufacturer */
@@ -1044,6 +1045,10 @@ int ixgbe_fcoe_get_hbainfo(struct net_device *netdev,
 		snprintf(info->model,
 			 sizeof(info->model),
 			 "Intel 82599");
+	} else if (hw->mac.type == ixgbe_mac_X550) {
+		snprintf(info->model,
+			 sizeof(info->model),
+			 "Intel X550");
 	} else {
 		snprintf(info->model,
 			 sizeof(info->model),
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
index f3168bcc7d87..e771e764daa3 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
@@ -844,7 +844,6 @@ static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter,
 	/* initialize NAPI */
 	netif_napi_add(adapter->netdev, &q_vector->napi,
 		       ixgbe_poll, 64);
-	napi_hash_add(&q_vector->napi);
 
 #ifdef CONFIG_NET_RX_BUSY_POLL
 	/* initialize busy poll */
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index aed8d029b23d..c4003a88bbf6 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -65,9 +65,6 @@
 #include "ixgbe_common.h"
 #include "ixgbe_dcb_82599.h"
 #include "ixgbe_sriov.h"
-#ifdef CONFIG_IXGBE_VXLAN
-#include <net/vxlan.h>
-#endif
 
 char ixgbe_driver_name[] = "ixgbe";
 static const char ixgbe_driver_string[] =
@@ -175,6 +172,8 @@ MODULE_DESCRIPTION("Intel(R) 10 Gigabit PCI Express Network Driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_VERSION);
 
+static struct workqueue_struct *ixgbe_wq;
+
 static bool ixgbe_check_cfg_remove(struct ixgbe_hw *hw, struct pci_dev *pdev);
 
 static int ixgbe_read_pci_cfg_word_parent(struct ixgbe_adapter *adapter,
@@ -316,7 +315,7 @@ static void ixgbe_service_event_schedule(struct ixgbe_adapter *adapter)
 	if (!test_bit(__IXGBE_DOWN, &adapter->state) &&
 	    !test_bit(__IXGBE_REMOVING, &adapter->state) &&
 	    !test_and_set_bit(__IXGBE_SERVICE_SCHED, &adapter->state))
-		schedule_work(&adapter->service_task);
+		queue_work(ixgbe_wq, &adapter->service_task);
 }
 
 static void ixgbe_remove_adapter(struct ixgbe_hw *hw)
@@ -1484,7 +1483,7 @@ static inline void ixgbe_rx_checksum(struct ixgbe_ring *ring,
 			return;
 
 		if (ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_ERR_OUTERIPER)) {
-			ring->rx_stats.csum_err++;
+			skb->ip_summed = CHECKSUM_NONE;
 			return;
 		}
 		/* If we checked the outer header let the stack know */
@@ -1635,6 +1634,7 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 				     struct sk_buff *skb)
 {
 	struct net_device *dev = rx_ring->netdev;
+	u32 flags = rx_ring->q_vector->adapter->flags;
 
 	ixgbe_update_rsc_stats(rx_ring, skb);
 
@@ -1642,8 +1642,8 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 
 	ixgbe_rx_checksum(rx_ring, rx_desc, skb);
 
-	if (unlikely(ixgbe_test_staterr(rx_desc, IXGBE_RXDADV_STAT_TS)))
-		ixgbe_ptp_rx_hwtstamp(rx_ring->q_vector->adapter, skb);
+	if (unlikely(flags & IXGBE_FLAG_RX_HWTSTAMP_ENABLED))
+		ixgbe_ptp_rx_hwtstamp(rx_ring, rx_desc, skb);
 
 	if ((dev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
 	    ixgbe_test_staterr(rx_desc, IXGBE_RXD_STAT_VP)) {
@@ -1659,6 +1659,7 @@ static void ixgbe_process_skb_fields(struct ixgbe_ring *rx_ring,
 static void ixgbe_rx_skb(struct ixgbe_q_vector *q_vector,
 			 struct sk_buff *skb)
 {
+	skb_mark_napi_id(skb, &q_vector->napi);
 	if (ixgbe_qv_busy_polling(q_vector))
 		netif_receive_skb(skb);
 	else
@@ -2123,7 +2124,6 @@ static int ixgbe_clean_rx_irq(struct ixgbe_q_vector *q_vector,
 		}
 
 #endif /* IXGBE_FCOE */
-		skb_mark_napi_id(skb, &q_vector->napi);
 		ixgbe_rx_skb(q_vector, skb);
 
 		/* update budget accounting */
@@ -2741,7 +2741,7 @@ static irqreturn_t ixgbe_msix_other(int irq, void *data)
 	ixgbe_check_fan_failure(adapter, eicr);
 
 	if (unlikely(eicr & IXGBE_EICR_TIMESYNC))
-		ixgbe_ptp_check_pps_event(adapter, eicr);
+		ixgbe_ptp_check_pps_event(adapter);
 
 	/* re-enable the original interrupt state, no lsc, no queues */
 	if (!test_bit(__IXGBE_DOWN, &adapter->state))
@@ -2757,7 +2757,7 @@ static irqreturn_t ixgbe_msix_clean_rings(int irq, void *data)
 	/* EIAM disabled interrupts (on this vector) for us */
 
 	if (q_vector->rx.ring || q_vector->tx.ring)
-		napi_schedule(&q_vector->napi);
+		napi_schedule_irqoff(&q_vector->napi);
 
 	return IRQ_HANDLED;
 }
@@ -2786,7 +2786,8 @@ int ixgbe_poll(struct napi_struct *napi, int budget)
 	ixgbe_for_each_ring(ring, q_vector->tx)
 		clean_complete &= !!ixgbe_clean_tx_irq(q_vector, ring);
 
-	if (!ixgbe_qv_lock_napi(q_vector))
+	/* Exit if we are called by netpoll or busy polling is active */
+	if ((budget <= 0) || !ixgbe_qv_lock_napi(q_vector))
 		return budget;
 
 	/* attempt to distribute budget to each queue fairly, but don't allow
@@ -2947,10 +2948,10 @@ static irqreturn_t ixgbe_intr(int irq, void *data)
 
 	ixgbe_check_fan_failure(adapter, eicr);
 	if (unlikely(eicr & IXGBE_EICR_TIMESYNC))
-		ixgbe_ptp_check_pps_event(adapter, eicr);
+		ixgbe_ptp_check_pps_event(adapter);
 
 	/* would disable interrupts here but EIAM disabled it */
-	napi_schedule(&q_vector->napi);
+	napi_schedule_irqoff(&q_vector->napi);
 
 	/*
 	 * re-enable link(maybe) and non-queue interrupts, no flush.
@@ -3315,8 +3316,7 @@ static void ixgbe_configure_srrctl(struct ixgbe_adapter *adapter,
 }
 
 /**
- * Return a number of entries in the RSS indirection table
- *
+ * ixgbe_rss_indir_tbl_entries - Return RSS indirection table entries
  * @adapter: device handle
  *
  *  - 82598/82599/X540:     128
@@ -3334,8 +3334,7 @@ u32 ixgbe_rss_indir_tbl_entries(struct ixgbe_adapter *adapter)
 }
 
 /**
- * Write the RETA table to HW
- *
+ * ixgbe_store_reta - Write the RETA table to HW
  * @adapter: device handle
  *
  * Write the RSS redirection table stored in adapter.rss_indir_tbl[] to HW.
@@ -3374,8 +3373,7 @@ void ixgbe_store_reta(struct ixgbe_adapter *adapter)
 }
 
 /**
- * Write the RETA table to HW (for x550 devices in SRIOV mode)
- *
+ * ixgbe_store_vfreta - Write the RETA table to HW (x550 devices in SRIOV mode)
  * @adapter: device handle
  *
  * Write the RSS redirection table stored in adapter.rss_indir_tbl[] to HW.
@@ -3621,6 +3619,9 @@ void ixgbe_configure_rx_ring(struct ixgbe_adapter *adapter,
 	IXGBE_WRITE_REG(hw, IXGBE_RDBAH(reg_idx), (rdba >> 32));
 	IXGBE_WRITE_REG(hw, IXGBE_RDLEN(reg_idx),
 			ring->count * sizeof(union ixgbe_adv_rx_desc));
+	/* Force flushing of IXGBE_RDLEN to prevent MDD */
+	IXGBE_WRITE_FLUSH(hw);
+
 	IXGBE_WRITE_REG(hw, IXGBE_RDH(reg_idx), 0);
 	IXGBE_WRITE_REG(hw, IXGBE_RDT(reg_idx), 0);
 	ring->tail = adapter->io_addr + IXGBE_RDT(reg_idx);
@@ -3704,6 +3705,9 @@ static void ixgbe_configure_virtualization(struct ixgbe_adapter *adapter)
 	/* Map PF MAC address in RAR Entry 0 to first pool following VFs */
 	hw->mac.ops.set_vmdq(hw, 0, VMDQ_P(0));
 
+	/* clear VLAN promisc flag so VFTA will be updated if necessary */
+	adapter->flags2 &= ~IXGBE_FLAG2_VLAN_PROMISC;
+
 	/*
 	 * Set up VF register offsets for selected VT Mode,
 	 * i.e. 32 or 64 VFs for SR-IOV
@@ -3901,12 +3905,56 @@ static int ixgbe_vlan_rx_add_vid(struct net_device *netdev,
 	struct ixgbe_hw *hw = &adapter->hw;
 
 	/* add VID to filter table */
-	hw->mac.ops.set_vfta(&adapter->hw, vid, VMDQ_P(0), true);
+	hw->mac.ops.set_vfta(&adapter->hw, vid, VMDQ_P(0), true, true);
 	set_bit(vid, adapter->active_vlans);
 
 	return 0;
 }
 
+static int ixgbe_find_vlvf_entry(struct ixgbe_hw *hw, u32 vlan)
+{
+	u32 vlvf;
+	int idx;
+
+	/* short cut the special case */
+	if (vlan == 0)
+		return 0;
+
+	/* Search for the vlan id in the VLVF entries */
+	for (idx = IXGBE_VLVF_ENTRIES; --idx;) {
+		vlvf = IXGBE_READ_REG(hw, IXGBE_VLVF(idx));
+		if ((vlvf & VLAN_VID_MASK) == vlan)
+			break;
+	}
+
+	return idx;
+}
+
+void ixgbe_update_pf_promisc_vlvf(struct ixgbe_adapter *adapter, u32 vid)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 bits, word;
+	int idx;
+
+	idx = ixgbe_find_vlvf_entry(hw, vid);
+	if (!idx)
+		return;
+
+	/* See if any other pools are set for this VLAN filter
+	 * entry other than the PF.
+	 */
+	word = idx * 2 + (VMDQ_P(0) / 32);
+	bits = ~(1 << (VMDQ_P(0)) % 32);
+	bits &= IXGBE_READ_REG(hw, IXGBE_VLVFB(word));
+
+	/* Disable the filter so this falls into the default pool. */
+	if (!bits && !IXGBE_READ_REG(hw, IXGBE_VLVFB(word ^ 1))) {
+		if (!(adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC))
+			IXGBE_WRITE_REG(hw, IXGBE_VLVFB(word), 0);
+		IXGBE_WRITE_REG(hw, IXGBE_VLVF(idx), 0);
+	}
+}
+
 static int ixgbe_vlan_rx_kill_vid(struct net_device *netdev,
 				  __be16 proto, u16 vid)
 {
@@ -3914,7 +3962,11 @@ static int ixgbe_vlan_rx_kill_vid(struct net_device *netdev,
 	struct ixgbe_hw *hw = &adapter->hw;
 
 	/* remove VID from filter table */
-	hw->mac.ops.set_vfta(&adapter->hw, vid, VMDQ_P(0), false);
+	if (adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC)
+		ixgbe_update_pf_promisc_vlvf(adapter, vid);
+	else
+		hw->mac.ops.set_vfta(hw, vid, VMDQ_P(0), false, true);
+
 	clear_bit(vid, adapter->active_vlans);
 
 	return 0;
@@ -3992,6 +4044,129 @@ static void ixgbe_vlan_strip_enable(struct ixgbe_adapter *adapter)
 	}
 }
 
+static void ixgbe_vlan_promisc_enable(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 vlnctrl, i;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X540:
+	case ixgbe_mac_X550:
+	case ixgbe_mac_X550EM_x:
+	default:
+		if (adapter->flags & IXGBE_FLAG_VMDQ_ENABLED)
+			break;
+		/* fall through */
+	case ixgbe_mac_82598EB:
+		/* legacy case, we can just disable VLAN filtering */
+		vlnctrl = IXGBE_READ_REG(hw, IXGBE_VLNCTRL);
+		vlnctrl &= ~(IXGBE_VLNCTRL_VFE | IXGBE_VLNCTRL_CFIEN);
+		IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
+		return;
+	}
+
+	/* We are already in VLAN promisc, nothing to do */
+	if (adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC)
+		return;
+
+	/* Set flag so we don't redo unnecessary work */
+	adapter->flags2 |= IXGBE_FLAG2_VLAN_PROMISC;
+
+	/* Add PF to all active pools */
+	for (i = IXGBE_VLVF_ENTRIES; --i;) {
+		u32 reg_offset = IXGBE_VLVFB(i * 2 + VMDQ_P(0) / 32);
+		u32 vlvfb = IXGBE_READ_REG(hw, reg_offset);
+
+		vlvfb |= 1 << (VMDQ_P(0) % 32);
+		IXGBE_WRITE_REG(hw, reg_offset, vlvfb);
+	}
+
+	/* Set all bits in the VLAN filter table array */
+	for (i = hw->mac.vft_size; i--;)
+		IXGBE_WRITE_REG(hw, IXGBE_VFTA(i), ~0U);
+}
+
+#define VFTA_BLOCK_SIZE 8
+static void ixgbe_scrub_vfta(struct ixgbe_adapter *adapter, u32 vfta_offset)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 vfta[VFTA_BLOCK_SIZE] = { 0 };
+	u32 vid_start = vfta_offset * 32;
+	u32 vid_end = vid_start + (VFTA_BLOCK_SIZE * 32);
+	u32 i, vid, word, bits;
+
+	for (i = IXGBE_VLVF_ENTRIES; --i;) {
+		u32 vlvf = IXGBE_READ_REG(hw, IXGBE_VLVF(i));
+
+		/* pull VLAN ID from VLVF */
+		vid = vlvf & VLAN_VID_MASK;
+
+		/* only concern outselves with a certain range */
+		if (vid < vid_start || vid >= vid_end)
+			continue;
+
+		if (vlvf) {
+			/* record VLAN ID in VFTA */
+			vfta[(vid - vid_start) / 32] |= 1 << (vid % 32);
+
+			/* if PF is part of this then continue */
+			if (test_bit(vid, adapter->active_vlans))
+				continue;
+		}
+
+		/* remove PF from the pool */
+		word = i * 2 + VMDQ_P(0) / 32;
+		bits = ~(1 << (VMDQ_P(0) % 32));
+		bits &= IXGBE_READ_REG(hw, IXGBE_VLVFB(word));
+		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(word), bits);
+	}
+
+	/* extract values from active_vlans and write back to VFTA */
+	for (i = VFTA_BLOCK_SIZE; i--;) {
+		vid = (vfta_offset + i) * 32;
+		word = vid / BITS_PER_LONG;
+		bits = vid % BITS_PER_LONG;
+
+		vfta[i] |= adapter->active_vlans[word] >> bits;
+
+		IXGBE_WRITE_REG(hw, IXGBE_VFTA(vfta_offset + i), vfta[i]);
+	}
+}
+
+static void ixgbe_vlan_promisc_disable(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 vlnctrl, i;
+
+	switch (hw->mac.type) {
+	case ixgbe_mac_82599EB:
+	case ixgbe_mac_X540:
+	case ixgbe_mac_X550:
+	case ixgbe_mac_X550EM_x:
+	default:
+		if (adapter->flags & IXGBE_FLAG_VMDQ_ENABLED)
+			break;
+		/* fall through */
+	case ixgbe_mac_82598EB:
+		vlnctrl = IXGBE_READ_REG(hw, IXGBE_VLNCTRL);
+		vlnctrl &= ~IXGBE_VLNCTRL_CFIEN;
+		vlnctrl |= IXGBE_VLNCTRL_VFE;
+		IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
+		return;
+	}
+
+	/* We are not in VLAN promisc, nothing to do */
+	if (!(adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC))
+		return;
+
+	/* Set flag so we don't redo unnecessary work */
+	adapter->flags2 &= ~IXGBE_FLAG2_VLAN_PROMISC;
+
+	for (i = 0; i < hw->mac.vft_size; i += VFTA_BLOCK_SIZE)
+		ixgbe_scrub_vfta(adapter, i);
+}
+
 static void ixgbe_restore_vlan(struct ixgbe_adapter *adapter)
 {
 	u16 vid;
@@ -4034,124 +4209,156 @@ static int ixgbe_write_mc_addr_list(struct net_device *netdev)
 #ifdef CONFIG_PCI_IOV
 void ixgbe_full_sync_mac_table(struct ixgbe_adapter *adapter)
 {
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
 	int i;
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		if (adapter->mac_table[i].state & IXGBE_MAC_STATE_IN_USE)
-			hw->mac.ops.set_rar(hw, i, adapter->mac_table[i].addr,
-					    adapter->mac_table[i].queue,
+
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		mac_table->state &= ~IXGBE_MAC_STATE_MODIFIED;
+
+		if (mac_table->state & IXGBE_MAC_STATE_IN_USE)
+			hw->mac.ops.set_rar(hw, i,
+					    mac_table->addr,
+					    mac_table->pool,
 					    IXGBE_RAH_AV);
 		else
 			hw->mac.ops.clear_rar(hw, i);
-
-		adapter->mac_table[i].state &= ~(IXGBE_MAC_STATE_MODIFIED);
 	}
 }
-#endif
 
+#endif
 static void ixgbe_sync_mac_table(struct ixgbe_adapter *adapter)
 {
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
 	int i;
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		if (adapter->mac_table[i].state & IXGBE_MAC_STATE_MODIFIED) {
-			if (adapter->mac_table[i].state &
-			    IXGBE_MAC_STATE_IN_USE)
-				hw->mac.ops.set_rar(hw, i,
-						adapter->mac_table[i].addr,
-						adapter->mac_table[i].queue,
-						IXGBE_RAH_AV);
-			else
-				hw->mac.ops.clear_rar(hw, i);
 
-			adapter->mac_table[i].state &=
-						~(IXGBE_MAC_STATE_MODIFIED);
-		}
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		if (!(mac_table->state & IXGBE_MAC_STATE_MODIFIED))
+			continue;
+
+		mac_table->state &= ~IXGBE_MAC_STATE_MODIFIED;
+
+		if (mac_table->state & IXGBE_MAC_STATE_IN_USE)
+			hw->mac.ops.set_rar(hw, i,
+					    mac_table->addr,
+					    mac_table->pool,
+					    IXGBE_RAH_AV);
+		else
+			hw->mac.ops.clear_rar(hw, i);
 	}
 }
 
 static void ixgbe_flush_sw_mac_table(struct ixgbe_adapter *adapter)
 {
-	int i;
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
+	int i;
 
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		adapter->mac_table[i].state |= IXGBE_MAC_STATE_MODIFIED;
-		adapter->mac_table[i].state &= ~IXGBE_MAC_STATE_IN_USE;
-		eth_zero_addr(adapter->mac_table[i].addr);
-		adapter->mac_table[i].queue = 0;
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		mac_table->state |= IXGBE_MAC_STATE_MODIFIED;
+		mac_table->state &= ~IXGBE_MAC_STATE_IN_USE;
 	}
+
 	ixgbe_sync_mac_table(adapter);
 }
 
-static int ixgbe_available_rars(struct ixgbe_adapter *adapter)
+static int ixgbe_available_rars(struct ixgbe_adapter *adapter, u16 pool)
 {
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
 	int i, count = 0;
 
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		if (adapter->mac_table[i].state == 0)
-			count++;
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		/* do not count default RAR as available */
+		if (mac_table->state & IXGBE_MAC_STATE_DEFAULT)
+			continue;
+
+		/* only count unused and addresses that belong to us */
+		if (mac_table->state & IXGBE_MAC_STATE_IN_USE) {
+			if (mac_table->pool != pool)
+				continue;
+		}
+
+		count++;
 	}
+
 	return count;
 }
 
 /* this function destroys the first RAR entry */
-static void ixgbe_mac_set_default_filter(struct ixgbe_adapter *adapter,
-					 u8 *addr)
+static void ixgbe_mac_set_default_filter(struct ixgbe_adapter *adapter)
 {
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
 
-	memcpy(&adapter->mac_table[0].addr, addr, ETH_ALEN);
-	adapter->mac_table[0].queue = VMDQ_P(0);
-	adapter->mac_table[0].state = (IXGBE_MAC_STATE_DEFAULT |
-				       IXGBE_MAC_STATE_IN_USE);
-	hw->mac.ops.set_rar(hw, 0, adapter->mac_table[0].addr,
-			    adapter->mac_table[0].queue,
+	memcpy(&mac_table->addr, hw->mac.addr, ETH_ALEN);
+	mac_table->pool = VMDQ_P(0);
+
+	mac_table->state = IXGBE_MAC_STATE_DEFAULT | IXGBE_MAC_STATE_IN_USE;
+
+	hw->mac.ops.set_rar(hw, 0, mac_table->addr, mac_table->pool,
 			    IXGBE_RAH_AV);
 }
 
-int ixgbe_add_mac_filter(struct ixgbe_adapter *adapter, u8 *addr, u16 queue)
+int ixgbe_add_mac_filter(struct ixgbe_adapter *adapter,
+			 const u8 *addr, u16 pool)
 {
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
 	int i;
 
 	if (is_zero_ether_addr(addr))
 		return -EINVAL;
 
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		if (adapter->mac_table[i].state & IXGBE_MAC_STATE_IN_USE)
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		if (mac_table->state & IXGBE_MAC_STATE_IN_USE)
 			continue;
-		adapter->mac_table[i].state |= (IXGBE_MAC_STATE_MODIFIED |
-						IXGBE_MAC_STATE_IN_USE);
-		ether_addr_copy(adapter->mac_table[i].addr, addr);
-		adapter->mac_table[i].queue = queue;
+
+		ether_addr_copy(mac_table->addr, addr);
+		mac_table->pool = pool;
+
+		mac_table->state |= IXGBE_MAC_STATE_MODIFIED |
+				    IXGBE_MAC_STATE_IN_USE;
+
 		ixgbe_sync_mac_table(adapter);
+
 		return i;
 	}
+
 	return -ENOMEM;
 }
 
-int ixgbe_del_mac_filter(struct ixgbe_adapter *adapter, u8 *addr, u16 queue)
+int ixgbe_del_mac_filter(struct ixgbe_adapter *adapter,
+			 const u8 *addr, u16 pool)
 {
-	/* search table for addr, if found, set to 0 and sync */
-	int i;
+	struct ixgbe_mac_addr *mac_table = &adapter->mac_table[0];
 	struct ixgbe_hw *hw = &adapter->hw;
+	int i;
 
 	if (is_zero_ether_addr(addr))
 		return -EINVAL;
 
-	for (i = 0; i < hw->mac.num_rar_entries; i++) {
-		if (ether_addr_equal(addr, adapter->mac_table[i].addr) &&
-		    adapter->mac_table[i].queue == queue) {
-			adapter->mac_table[i].state |= IXGBE_MAC_STATE_MODIFIED;
-			adapter->mac_table[i].state &= ~IXGBE_MAC_STATE_IN_USE;
-			eth_zero_addr(adapter->mac_table[i].addr);
-			adapter->mac_table[i].queue = 0;
-			ixgbe_sync_mac_table(adapter);
-			return 0;
-		}
+	/* search table for addr, if found clear IN_USE flag and sync */
+	for (i = 0; i < hw->mac.num_rar_entries; i++, mac_table++) {
+		/* we can only delete an entry if it is in use */
+		if (!(mac_table->state & IXGBE_MAC_STATE_IN_USE))
+			continue;
+		/* we only care about entries that belong to the given pool */
+		if (mac_table->pool != pool)
+			continue;
+		/* we only care about a specific MAC address */
+		if (!ether_addr_equal(addr, mac_table->addr))
+			continue;
+
+		mac_table->state |= IXGBE_MAC_STATE_MODIFIED;
+		mac_table->state &= ~IXGBE_MAC_STATE_IN_USE;
+
+		ixgbe_sync_mac_table(adapter);
+
+		return 0;
 	}
+
 	return -ENOMEM;
 }
 /**
@@ -4169,7 +4376,7 @@ static int ixgbe_write_uc_addr_list(struct net_device *netdev, int vfn)
 	int count = 0;
 
 	/* return ENOMEM indicating insufficient memory for addresses */
-	if (netdev_uc_count(netdev) > ixgbe_available_rars(adapter))
+	if (netdev_uc_count(netdev) > ixgbe_available_rars(adapter, vfn))
 		return -ENOMEM;
 
 	if (!netdev_uc_empty(netdev)) {
@@ -4183,6 +4390,25 @@ static int ixgbe_write_uc_addr_list(struct net_device *netdev, int vfn)
 	return count;
 }
 
+static int ixgbe_uc_sync(struct net_device *netdev, const unsigned char *addr)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+	int ret;
+
+	ret = ixgbe_add_mac_filter(adapter, addr, VMDQ_P(0));
+
+	return min_t(int, ret, 0);
+}
+
+static int ixgbe_uc_unsync(struct net_device *netdev, const unsigned char *addr)
+{
+	struct ixgbe_adapter *adapter = netdev_priv(netdev);
+
+	ixgbe_del_mac_filter(adapter, addr, VMDQ_P(0));
+
+	return 0;
+}
+
 /**
  * ixgbe_set_rx_mode - Unicast, Multicast and Promiscuous mode set
  * @netdev: network interface device structure
@@ -4197,12 +4423,10 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 fctrl, vmolr = IXGBE_VMOLR_BAM | IXGBE_VMOLR_AUPE;
-	u32 vlnctrl;
 	int count;
 
 	/* Check for Promiscuous and All Multicast modes */
 	fctrl = IXGBE_READ_REG(hw, IXGBE_FCTRL);
-	vlnctrl = IXGBE_READ_REG(hw, IXGBE_VLNCTRL);
 
 	/* set all bits that we expect to always be set */
 	fctrl &= ~IXGBE_FCTRL_SBP; /* disable store-bad-packets */
@@ -4212,25 +4436,18 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 
 	/* clear the bits we are changing the status of */
 	fctrl &= ~(IXGBE_FCTRL_UPE | IXGBE_FCTRL_MPE);
-	vlnctrl &= ~(IXGBE_VLNCTRL_VFE | IXGBE_VLNCTRL_CFIEN);
 	if (netdev->flags & IFF_PROMISC) {
 		hw->addr_ctrl.user_set_promisc = true;
 		fctrl |= (IXGBE_FCTRL_UPE | IXGBE_FCTRL_MPE);
 		vmolr |= IXGBE_VMOLR_MPE;
-		/* Only disable hardware filter vlans in promiscuous mode
-		 * if SR-IOV and VMDQ are disabled - otherwise ensure
-		 * that hardware VLAN filters remain enabled.
-		 */
-		if (adapter->flags & (IXGBE_FLAG_VMDQ_ENABLED |
-				      IXGBE_FLAG_SRIOV_ENABLED))
-			vlnctrl |= (IXGBE_VLNCTRL_VFE | IXGBE_VLNCTRL_CFIEN);
+		ixgbe_vlan_promisc_enable(adapter);
 	} else {
 		if (netdev->flags & IFF_ALLMULTI) {
 			fctrl |= IXGBE_FCTRL_MPE;
 			vmolr |= IXGBE_VMOLR_MPE;
 		}
-		vlnctrl |= IXGBE_VLNCTRL_VFE;
 		hw->addr_ctrl.user_set_promisc = false;
+		ixgbe_vlan_promisc_disable(adapter);
 	}
 
 	/*
@@ -4238,8 +4455,7 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 	 * sufficient space to store all the addresses then enable
 	 * unicast promiscuous mode
 	 */
-	count = ixgbe_write_uc_addr_list(netdev, VMDQ_P(0));
-	if (count < 0) {
+	if (__dev_uc_sync(netdev, ixgbe_uc_sync, ixgbe_uc_unsync)) {
 		fctrl |= IXGBE_FCTRL_UPE;
 		vmolr |= IXGBE_VMOLR_ROPE;
 	}
@@ -4275,7 +4491,6 @@ void ixgbe_set_rx_mode(struct net_device *netdev)
 		/* NOTE:  VLAN filtering is disabled by setting PROMISC */
 	}
 
-	IXGBE_WRITE_REG(hw, IXGBE_VLNCTRL, vlnctrl);
 	IXGBE_WRITE_REG(hw, IXGBE_FCTRL, fctrl);
 
 	if (netdev->features & NETIF_F_HW_VLAN_CTAG_RX)
@@ -5042,7 +5257,6 @@ void ixgbe_reset(struct ixgbe_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	int err;
-	u8 old_addr[ETH_ALEN];
 
 	if (ixgbe_removed(hw->hw_addr))
 		return;
@@ -5078,10 +5292,13 @@ void ixgbe_reset(struct ixgbe_adapter *adapter)
 	}
 
 	clear_bit(__IXGBE_IN_SFP_INIT, &adapter->state);
-	/* do not flush user set addresses */
-	memcpy(old_addr, &adapter->mac_table[0].addr, netdev->addr_len);
+
+	/* flush entries out of MAC table */
 	ixgbe_flush_sw_mac_table(adapter);
-	ixgbe_mac_set_default_filter(adapter, old_addr);
+	__dev_uc_unsync(netdev, NULL);
+
+	/* do not flush user set addresses */
+	ixgbe_mac_set_default_filter(adapter);
 
 	/* update SAN MAC vmdq pool selection */
 	if (hw->mac.san_mac_rar_index)
@@ -5331,6 +5548,8 @@ static int ixgbe_sw_init(struct ixgbe_adapter *adapter)
 	adapter->mac_table = kzalloc(sizeof(struct ixgbe_mac_addr) *
 				     hw->mac.num_rar_entries,
 				     GFP_ATOMIC);
+	if (!adapter->mac_table)
+		return -ENOMEM;
 
 	/* Set MAC specific capability flags and exceptions */
 	switch (hw->mac.type) {
@@ -6616,10 +6835,8 @@ static void ixgbe_check_for_bad_vf(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
-	struct pci_dev *vfdev;
+	unsigned int vf;
 	u32 gpc;
-	int pos;
-	unsigned short vf_id;
 
 	if (!(netif_carrier_ok(adapter->netdev)))
 		return;
@@ -6636,26 +6853,17 @@ static void ixgbe_check_for_bad_vf(struct ixgbe_adapter *adapter)
 	if (!pdev)
 		return;
 
-	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_SRIOV);
-	if (!pos)
-		return;
-
-	/* get the device ID for the VF */
-	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_DID, &vf_id);
-
 	/* check status reg for all VFs owned by this PF */
-	vfdev = pci_get_device(pdev->vendor, vf_id, NULL);
-	while (vfdev) {
-		if (vfdev->is_virtfn && (vfdev->physfn == pdev)) {
-			u16 status_reg;
-
-			pci_read_config_word(vfdev, PCI_STATUS, &status_reg);
-			if (status_reg & PCI_STATUS_REC_MASTER_ABORT)
-				/* issue VFLR */
-				ixgbe_issue_vf_flr(adapter, vfdev);
-		}
+	for (vf = 0; vf < adapter->num_vfs; ++vf) {
+		struct pci_dev *vfdev = adapter->vfinfo[vf].vfdev;
+		u16 status_reg;
 
-		vfdev = pci_get_device(pdev->vendor, vf_id, vfdev);
+		if (!vfdev)
+			continue;
+		pci_read_config_word(vfdev, PCI_STATUS, &status_reg);
+		if (status_reg != IXGBE_FAILED_READ_CFG_WORD &&
+		    status_reg & PCI_STATUS_REC_MASTER_ABORT)
+			ixgbe_issue_vf_flr(adapter, vfdev);
 	}
 }
 
@@ -7024,6 +7232,7 @@ static void ixgbe_tx_csum(struct ixgbe_ring *tx_ring,
 			struct tcphdr *tcphdr;
 			u8 *raw;
 		} transport_hdr;
+		__be16 frag_off;
 
 		if (skb->encapsulation) {
 			network_hdr.raw = skb_inner_network_header(skb);
@@ -7047,13 +7256,17 @@ static void ixgbe_tx_csum(struct ixgbe_ring *tx_ring,
 		case 6:
 			vlan_macip_lens |= transport_hdr.raw - network_hdr.raw;
 			l4_hdr = network_hdr.ipv6->nexthdr;
+			if (likely((transport_hdr.raw - network_hdr.raw) ==
+				   sizeof(struct ipv6hdr)))
+				break;
+			ipv6_skip_exthdr(skb, network_hdr.raw - skb->data +
+					      sizeof(struct ipv6hdr),
+					 &l4_hdr, &frag_off);
+			if (unlikely(frag_off))
+				l4_hdr = NEXTHDR_FRAGMENT;
 			break;
 		default:
-			if (unlikely(net_ratelimit())) {
-				dev_warn(tx_ring->dev,
-					 "partial checksum but version=%d\n",
-					 network_hdr.ipv4->version);
-			}
+			break;
 		}
 
 		switch (l4_hdr) {
@@ -7074,16 +7287,18 @@ static void ixgbe_tx_csum(struct ixgbe_ring *tx_ring,
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-				 "partial checksum but l4 proto=%x!\n",
-				 l4_hdr);
+					 "partial checksum, version=%d, l4 proto=%x\n",
+					 network_hdr.ipv4->version, l4_hdr);
 			}
-			break;
+			skb_checksum_help(skb);
+			goto no_csum;
 		}
 
 		/* update TX checksum flag */
 		first->tx_flags |= IXGBE_TX_FLAGS_CSUM;
 	}
 
+no_csum:
 	/* vlan_macip_lens: MACLEN, VLAN tag */
 	vlan_macip_lens |= first->tx_flags & IXGBE_TX_FLAGS_VLAN_MASK;
 
@@ -7358,7 +7573,9 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 	/* snag network header to get L4 type and address */
 	skb = first->skb;
 	hdr.network = skb_network_header(skb);
-	if (skb->encapsulation) {
+	if (!skb->encapsulation) {
+		th = tcp_hdr(skb);
+	} else {
 #ifdef CONFIG_IXGBE_VXLAN
 		struct ixgbe_adapter *adapter = q_vector->adapter;
 
@@ -7377,14 +7594,34 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 #else
 		return;
 #endif /* CONFIG_IXGBE_VXLAN */
-	} else {
-		/* Currently only IPv4/IPv6 with TCP is supported */
-		if ((first->protocol != htons(ETH_P_IPV6) ||
-		     hdr.ipv6->nexthdr != IPPROTO_TCP) &&
-		    (first->protocol != htons(ETH_P_IP) ||
-		     hdr.ipv4->protocol != IPPROTO_TCP))
+	}
+
+	/* Currently only IPv4/IPv6 with TCP is supported */
+	switch (hdr.ipv4->version) {
+	case IPVERSION:
+		if (hdr.ipv4->protocol != IPPROTO_TCP)
 			return;
-		th = tcp_hdr(skb);
+		break;
+	case 6:
+		if (likely((unsigned char *)th - hdr.network ==
+			   sizeof(struct ipv6hdr))) {
+			if (hdr.ipv6->nexthdr != IPPROTO_TCP)
+				return;
+		} else {
+			__be16 frag_off;
+			u8 l4_hdr;
+
+			ipv6_skip_exthdr(skb, hdr.network - skb->data +
+					      sizeof(struct ipv6hdr),
+					 &l4_hdr, &frag_off);
+			if (unlikely(frag_off))
+				return;
+			if (l4_hdr != IPPROTO_TCP)
+				return;
+		}
+		break;
+	default:
+		return;
 	}
 
 	/* skip this packet since it is invalid or the socket is closing */
@@ -7419,10 +7656,12 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 		common.port.src ^= th->dest ^ first->protocol;
 	common.port.dst ^= th->source;
 
-	if (first->protocol == htons(ETH_P_IP)) {
+	switch (hdr.ipv4->version) {
+	case IPVERSION:
 		input.formatted.flow_type = IXGBE_ATR_FLOW_TYPE_TCPV4;
 		common.ip ^= hdr.ipv4->saddr ^ hdr.ipv4->daddr;
-	} else {
+		break;
+	case 6:
 		input.formatted.flow_type = IXGBE_ATR_FLOW_TYPE_TCPV6;
 		common.ip ^= hdr.ipv6->saddr.s6_addr32[0] ^
 			     hdr.ipv6->saddr.s6_addr32[1] ^
@@ -7432,6 +7671,9 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 			     hdr.ipv6->daddr.s6_addr32[1] ^
 			     hdr.ipv6->daddr.s6_addr32[2] ^
 			     hdr.ipv6->daddr.s6_addr32[3];
+		break;
+	default:
+		break;
 	}
 
 #ifdef CONFIG_IXGBE_VXLAN
@@ -7659,17 +7901,16 @@ static int ixgbe_set_mac(struct net_device *netdev, void *p)
 	struct ixgbe_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct sockaddr *addr = p;
-	int ret;
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	ixgbe_del_mac_filter(adapter, hw->mac.addr, VMDQ_P(0));
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
 	memcpy(hw->mac.addr, addr->sa_data, netdev->addr_len);
 
-	ret = ixgbe_add_mac_filter(adapter, hw->mac.addr, VMDQ_P(0));
-	return ret > 0 ? 0 : ret;
+	ixgbe_mac_set_default_filter(adapter);
+
+	return 0;
 }
 
 static int
@@ -8155,7 +8396,10 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 {
 	/* guarantee we can provide a unique filter for the unicast address */
 	if (is_unicast_ether_addr(addr) || is_link_local_ether_addr(addr)) {
-		if (IXGBE_MAX_PF_MACVLANS <= netdev_uc_count(dev))
+		struct ixgbe_adapter *adapter = netdev_priv(dev);
+		u16 pool = VMDQ_P(0);
+
+		if (netdev_uc_count(dev) >= ixgbe_available_rars(adapter, pool))
 			return -ENOMEM;
 	}
 
@@ -8387,7 +8631,7 @@ ixgbe_features_check(struct sk_buff *skb, struct net_device *dev,
 
 	if (unlikely(skb_inner_mac_header(skb) - skb_transport_header(skb) >
 		     IXGBE_MAX_TUNNEL_HDR_LEN))
-		return features & ~NETIF_F_ALL_CSUM;
+		return features & ~NETIF_F_CSUM_MASK;
 
 	return features;
 }
@@ -8784,8 +9028,8 @@ skip_sriov:
 	case ixgbe_mac_X540:
 	case ixgbe_mac_X550:
 	case ixgbe_mac_X550EM_x:
-		netdev->features |= NETIF_F_SCTP_CSUM;
-		netdev->hw_features |= NETIF_F_SCTP_CSUM |
+		netdev->features |= NETIF_F_SCTP_CRC;
+		netdev->hw_features |= NETIF_F_SCTP_CRC |
 				       NETIF_F_NTUPLE;
 		break;
 	default:
@@ -8801,8 +9045,7 @@ skip_sriov:
 	netdev->vlan_features |= NETIF_F_IPV6_CSUM;
 	netdev->vlan_features |= NETIF_F_SG;
 
-	netdev->hw_enc_features |= NETIF_F_SG | NETIF_F_IP_CSUM |
-				   NETIF_F_IPV6_CSUM;
+	netdev->hw_enc_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
 
 	netdev->priv_flags |= IFF_UNICAST_FLT;
 	netdev->priv_flags |= IFF_SUPP_NOFCS;
@@ -8811,9 +9054,7 @@ skip_sriov:
 	switch (adapter->hw.mac.type) {
 	case ixgbe_mac_X550:
 	case ixgbe_mac_X550EM_x:
-		netdev->hw_enc_features |= NETIF_F_RXCSUM |
-					   NETIF_F_IP_CSUM |
-					   NETIF_F_IPV6_CSUM;
+		netdev->hw_enc_features |= NETIF_F_RXCSUM;
 		break;
 	default:
 		break;
@@ -8873,7 +9114,7 @@ skip_sriov:
 		goto err_sw_init;
 	}
 
-	ixgbe_mac_set_default_filter(adapter, hw->mac.perm_addr);
+	ixgbe_mac_set_default_filter(adapter);
 
 	setup_timer(&adapter->service_timer, &ixgbe_service_timer,
 		    (unsigned long) adapter);
@@ -9328,6 +9569,12 @@ static int __init ixgbe_init_module(void)
 	pr_info("%s - version %s\n", ixgbe_driver_string, ixgbe_driver_version);
 	pr_info("%s\n", ixgbe_copyright);
 
+	ixgbe_wq = create_singlethread_workqueue(ixgbe_driver_name);
+	if (!ixgbe_wq) {
+		pr_err("%s: Failed to create workqueue\n", ixgbe_driver_name);
+		return -ENOMEM;
+	}
+
 	ixgbe_dbg_init();
 
 	ret = pci_register_driver(&ixgbe_driver);
@@ -9359,6 +9606,10 @@ static void __exit ixgbe_exit_module(void)
 	pci_unregister_driver(&ixgbe_driver);
 
 	ixgbe_dbg_exit();
+	if (ixgbe_wq) {
+		destroy_workqueue(ixgbe_wq);
+		ixgbe_wq = NULL;
+	}
 }
 
 #ifdef CONFIG_IXGBE_DCA
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index fb8673d63806..db0731e05401 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -2393,6 +2393,9 @@ s32 ixgbe_set_copper_phy_power(struct ixgbe_hw *hw, bool on)
 	if (hw->mac.ops.get_media_type(hw) != ixgbe_media_type_copper)
 		return 0;
 
+	if (!on && ixgbe_mng_present(hw))
+		return 0;
+
 	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_VENDOR_SPECIFIC_1_CONTROL,
 				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
 				      &reg);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
index e5ba04025e2b..ef1504d41890 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ptp.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2013 Intel Corporation.
+  Copyright(c) 1999 - 2015 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -27,6 +27,7 @@
 *******************************************************************************/
 #include "ixgbe.h"
 #include <linux/ptp_classify.h>
+#include <linux/clocksource.h>
 
 /*
  * The 82599 and the X540 do not have true 64bit nanosecond scale
@@ -93,7 +94,6 @@
 
 #define IXGBE_INCVAL_SHIFT_82599 7
 #define IXGBE_INCPER_SHIFT_82599 24
-#define IXGBE_MAX_TIMEADJ_VALUE  0x7FFFFFFFFFFFFFFFULL
 
 #define IXGBE_OVERFLOW_PERIOD    (HZ * 30)
 #define IXGBE_PTP_TX_TIMEOUT     (HZ * 15)
@@ -104,8 +104,68 @@
  */
 #define IXGBE_PTP_PPS_HALF_SECOND 500000000ULL
 
+/* In contrast, the X550 controller has two registers, SYSTIMEH and SYSTIMEL
+ * which contain measurements of seconds and nanoseconds respectively. This
+ * matches the standard linux representation of time in the kernel. In addition,
+ * the X550 also has a SYSTIMER register which represents residue, or
+ * subnanosecond overflow adjustments. To control clock adjustment, the TIMINCA
+ * register is used, but it is unlike the X540 and 82599 devices. TIMINCA
+ * represents units of 2^-32 nanoseconds, and uses 31 bits for this, with the
+ * high bit representing whether the adjustent is positive or negative. Every
+ * clock cycle, the X550 will add 12.5 ns + TIMINCA which can result in a range
+ * of 12 to 13 nanoseconds adjustment. Unlike the 82599 and X540 devices, the
+ * X550's clock for purposes of SYSTIME generation is constant and not dependent
+ * on the link speed.
+ *
+ *           SYSTIMEH           SYSTIMEL        SYSTIMER
+ *       +--------------+  +--------------+  +-------------+
+ * X550  |      32      |  |      32      |  |     32      |
+ *       *--------------+  +--------------+  +-------------+
+ *       \____seconds___/   \_nanoseconds_/  \__2^-32 ns__/
+ *
+ * This results in a full 96 bits to represent the clock, with 32 bits for
+ * seconds, 32 bits for nanoseconds (largest value is 0d999999999 or just under
+ * 1 second) and an additional 32 bits to measure sub nanosecond adjustments for
+ * underflow of adjustments.
+ *
+ * The 32 bits of seconds for the X550 overflows every
+ *   2^32 / ( 365.25 * 24 * 60 * 60 ) = ~136 years.
+ *
+ * In order to adjust the clock frequency for the X550, the TIMINCA register is
+ * provided. This register represents a + or minus nearly 0.5 ns adjustment to
+ * the base frequency. It is measured in 2^-32 ns units, with the high bit being
+ * the sign bit. This register enables software to calculate frequency
+ * adjustments and apply them directly to the clock rate.
+ *
+ * The math for converting ppb into TIMINCA values is fairly straightforward.
+ *   TIMINCA value = ( Base_Frequency * ppb ) / 1000000000ULL
+ *
+ * This assumes that ppb is never high enough to create a value bigger than
+ * TIMINCA's 31 bits can store. This is ensured by the stack. Calculating this
+ * value is also simple.
+ *   Max ppb = ( Max Adjustment / Base Frequency ) / 1000000000ULL
+ *
+ * For the X550, the Max adjustment is +/- 0.5 ns, and the base frequency is
+ * 12.5 nanoseconds. This means that the Max ppb is 39999999
+ *   Note: We subtract one in order to ensure no overflow, because the TIMINCA
+ *         register can only hold slightly under 0.5 nanoseconds.
+ *
+ * Because TIMINCA is measured in 2^-32 ns units, we have to convert 12.5 ns
+ * into 2^-32 units, which is
+ *
+ *  12.5 * 2^32 = C80000000
+ *
+ * Some revisions of hardware have a faster base frequency than the registers
+ * were defined for. To fix this, we use a timecounter structure with the
+ * proper mult and shift to convert the cycles into nanoseconds of time.
+ */
+#define IXGBE_X550_BASE_PERIOD 0xC80000000ULL
+#define INCVALUE_MASK	0x7FFFFFFF
+#define ISGN		0x80000000
+#define MAX_TIMADJ	0x7FFFFFFF
+
 /**
- * ixgbe_ptp_setup_sdp
+ * ixgbe_ptp_setup_sdp_x540
  * @hw: the hardware private structure
  *
  * this function enables or disables the clock out feature on SDP0 for
@@ -116,83 +176,116 @@
  * aligns the start of the PPS signal to that value. The shift is
  * necessary because it can change based on the link speed.
  */
-static void ixgbe_ptp_setup_sdp(struct ixgbe_adapter *adapter)
+static void ixgbe_ptp_setup_sdp_x540(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
-	int shift = adapter->cc.shift;
+	int shift = adapter->hw_cc.shift;
 	u32 esdp, tsauxc, clktiml, clktimh, trgttiml, trgttimh, rem;
 	u64 ns = 0, clock_edge = 0;
 
-	if ((adapter->flags2 & IXGBE_FLAG2_PTP_PPS_ENABLED) &&
-	    (hw->mac.type == ixgbe_mac_X540)) {
+	/* disable the pin first */
+	IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, 0x0);
+	IXGBE_WRITE_FLUSH(hw);
 
-		/* disable the pin first */
-		IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, 0x0);
-		IXGBE_WRITE_FLUSH(hw);
+	if (!(adapter->flags2 & IXGBE_FLAG2_PTP_PPS_ENABLED))
+		return;
 
-		esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
+	esdp = IXGBE_READ_REG(hw, IXGBE_ESDP);
 
-		/*
-		 * enable the SDP0 pin as output, and connected to the
-		 * native function for Timesync (ClockOut)
-		 */
-		esdp |= (IXGBE_ESDP_SDP0_DIR |
-			 IXGBE_ESDP_SDP0_NATIVE);
+	/* enable the SDP0 pin as output, and connected to the
+	 * native function for Timesync (ClockOut)
+	 */
+	esdp |= IXGBE_ESDP_SDP0_DIR |
+		IXGBE_ESDP_SDP0_NATIVE;
 
-		/*
-		 * enable the Clock Out feature on SDP0, and allow
-		 * interrupts to occur when the pin changes
-		 */
-		tsauxc = (IXGBE_TSAUXC_EN_CLK |
-			  IXGBE_TSAUXC_SYNCLK |
-			  IXGBE_TSAUXC_SDP0_INT);
+	/* enable the Clock Out feature on SDP0, and allow
+	 * interrupts to occur when the pin changes
+	 */
+	tsauxc = IXGBE_TSAUXC_EN_CLK |
+		 IXGBE_TSAUXC_SYNCLK |
+		 IXGBE_TSAUXC_SDP0_INT;
 
-		/* clock period (or pulse length) */
-		clktiml = (u32)(IXGBE_PTP_PPS_HALF_SECOND << shift);
-		clktimh = (u32)((IXGBE_PTP_PPS_HALF_SECOND << shift) >> 32);
+	/* clock period (or pulse length) */
+	clktiml = (u32)(IXGBE_PTP_PPS_HALF_SECOND << shift);
+	clktimh = (u32)((IXGBE_PTP_PPS_HALF_SECOND << shift) >> 32);
 
-		/*
-		 * Account for the cyclecounter wrap-around value by
-		 * using the converted ns value of the current time to
-		 * check for when the next aligned second would occur.
-		 */
-		clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIML);
-		clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIMH) << 32;
-		ns = timecounter_cyc2time(&adapter->tc, clock_edge);
+	/* Account for the cyclecounter wrap-around value by
+	 * using the converted ns value of the current time to
+	 * check for when the next aligned second would occur.
+	 */
+	clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIML);
+	clock_edge |= (u64)IXGBE_READ_REG(hw, IXGBE_SYSTIMH) << 32;
+	ns = timecounter_cyc2time(&adapter->hw_tc, clock_edge);
 
-		div_u64_rem(ns, IXGBE_PTP_PPS_HALF_SECOND, &rem);
-		clock_edge += ((IXGBE_PTP_PPS_HALF_SECOND - (u64)rem) << shift);
+	div_u64_rem(ns, IXGBE_PTP_PPS_HALF_SECOND, &rem);
+	clock_edge += ((IXGBE_PTP_PPS_HALF_SECOND - (u64)rem) << shift);
 
-		/* specify the initial clock start time */
-		trgttiml = (u32)clock_edge;
-		trgttimh = (u32)(clock_edge >> 32);
+	/* specify the initial clock start time */
+	trgttiml = (u32)clock_edge;
+	trgttimh = (u32)(clock_edge >> 32);
 
-		IXGBE_WRITE_REG(hw, IXGBE_CLKTIML, clktiml);
-		IXGBE_WRITE_REG(hw, IXGBE_CLKTIMH, clktimh);
-		IXGBE_WRITE_REG(hw, IXGBE_TRGTTIML0, trgttiml);
-		IXGBE_WRITE_REG(hw, IXGBE_TRGTTIMH0, trgttimh);
+	IXGBE_WRITE_REG(hw, IXGBE_CLKTIML, clktiml);
+	IXGBE_WRITE_REG(hw, IXGBE_CLKTIMH, clktimh);
+	IXGBE_WRITE_REG(hw, IXGBE_TRGTTIML0, trgttiml);
+	IXGBE_WRITE_REG(hw, IXGBE_TRGTTIMH0, trgttimh);
 
-		IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
-		IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, tsauxc);
-	} else {
-		IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, 0x0);
-	}
+	IXGBE_WRITE_REG(hw, IXGBE_ESDP, esdp);
+	IXGBE_WRITE_REG(hw, IXGBE_TSAUXC, tsauxc);
 
 	IXGBE_WRITE_FLUSH(hw);
 }
 
 /**
- * ixgbe_ptp_read - read raw cycle counter (to be used by time counter)
+ * ixgbe_ptp_read_X550 - read cycle counter value
+ * @hw_cc: cyclecounter structure
+ *
+ * This function reads SYSTIME registers. It is called by the cyclecounter
+ * structure to convert from internal representation into nanoseconds. We need
+ * this for X550 since some skews do not have expected clock frequency and
+ * result of SYSTIME is 32bits of "billions of cycles" and 32 bits of
+ * "cycles", rather than seconds and nanoseconds.
+ */
+static cycle_t ixgbe_ptp_read_X550(const struct cyclecounter *hw_cc)
+{
+	struct ixgbe_adapter *adapter =
+			container_of(hw_cc, struct ixgbe_adapter, hw_cc);
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct timespec64 ts;
+
+	/* storage is 32 bits of 'billions of cycles' and 32 bits of 'cycles'.
+	 * Some revisions of hardware run at a higher frequency and so the
+	 * cycles are not guaranteed to be nanoseconds. The timespec64 created
+	 * here is used for its math/conversions but does not necessarily
+	 * represent nominal time.
+	 *
+	 * It should be noted that this cyclecounter will overflow at a
+	 * non-bitmask field since we have to convert our billions of cycles
+	 * into an actual cycles count. This results in some possible weird
+	 * situations at high cycle counter stamps. However given that 32 bits
+	 * of "seconds" is ~138 years this isn't a problem. Even at the
+	 * increased frequency of some revisions, this is still ~103 years.
+	 * Since the SYSTIME values start at 0 and we never write them, it is
+	 * highly unlikely for the cyclecounter to overflow in practice.
+	 */
+	IXGBE_READ_REG(hw, IXGBE_SYSTIMR);
+	ts.tv_nsec = IXGBE_READ_REG(hw, IXGBE_SYSTIML);
+	ts.tv_sec = IXGBE_READ_REG(hw, IXGBE_SYSTIMH);
+
+	return (u64)timespec64_to_ns(&ts);
+}
+
+/**
+ * ixgbe_ptp_read_82599 - read raw cycle counter (to be used by time counter)
  * @cc: the cyclecounter structure
  *
  * this function reads the cyclecounter registers and is called by the
  * cyclecounter structure used to construct a ns counter from the
  * arbitrary fixed point registers
  */
-static cycle_t ixgbe_ptp_read(const struct cyclecounter *cc)
+static cycle_t ixgbe_ptp_read_82599(const struct cyclecounter *cc)
 {
 	struct ixgbe_adapter *adapter =
-		container_of(cc, struct ixgbe_adapter, cc);
+		container_of(cc, struct ixgbe_adapter, hw_cc);
 	struct ixgbe_hw *hw = &adapter->hw;
 	u64 stamp = 0;
 
@@ -203,20 +296,79 @@ static cycle_t ixgbe_ptp_read(const struct cyclecounter *cc)
 }
 
 /**
- * ixgbe_ptp_adjfreq
+ * ixgbe_ptp_convert_to_hwtstamp - convert register value to hw timestamp
+ * @adapter: private adapter structure
+ * @hwtstamp: stack timestamp structure
+ * @systim: unsigned 64bit system time value
+ *
+ * We need to convert the adapter's RX/TXSTMP registers into a hwtstamp value
+ * which can be used by the stack's ptp functions.
+ *
+ * The lock is used to protect consistency of the cyclecounter and the SYSTIME
+ * registers. However, it does not need to protect against the Rx or Tx
+ * timestamp registers, as there can't be a new timestamp until the old one is
+ * unlatched by reading.
+ *
+ * In addition to the timestamp in hardware, some controllers need a software
+ * overflow cyclecounter, and this function takes this into account as well.
+ **/
+static void ixgbe_ptp_convert_to_hwtstamp(struct ixgbe_adapter *adapter,
+					  struct skb_shared_hwtstamps *hwtstamp,
+					  u64 timestamp)
+{
+	unsigned long flags;
+	struct timespec64 systime;
+	u64 ns;
+
+	memset(hwtstamp, 0, sizeof(*hwtstamp));
+
+	switch (adapter->hw.mac.type) {
+	/* X550 and later hardware supposedly represent time using a seconds
+	 * and nanoseconds counter, instead of raw 64bits nanoseconds. We need
+	 * to convert the timestamp into cycles before it can be fed to the
+	 * cyclecounter. We need an actual cyclecounter because some revisions
+	 * of hardware run at a higher frequency and thus the counter does
+	 * not represent seconds/nanoseconds. Instead it can be thought of as
+	 * cycles and billions of cycles.
+	 */
+	case ixgbe_mac_X550:
+	case ixgbe_mac_X550EM_x:
+		/* Upper 32 bits represent billions of cycles, lower 32 bits
+		 * represent cycles. However, we use timespec64_to_ns for the
+		 * correct math even though the units haven't been corrected
+		 * yet.
+		 */
+		systime.tv_sec = timestamp >> 32;
+		systime.tv_nsec = timestamp & 0xFFFFFFFF;
+
+		timestamp = timespec64_to_ns(&systime);
+		break;
+	default:
+		break;
+	}
+
+	spin_lock_irqsave(&adapter->tmreg_lock, flags);
+	ns = timecounter_cyc2time(&adapter->hw_tc, timestamp);
+	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
+
+	hwtstamp->hwtstamp = ns_to_ktime(ns);
+}
+
+/**
+ * ixgbe_ptp_adjfreq_82599
  * @ptp: the ptp clock structure
  * @ppb: parts per billion adjustment from base
  *
  * adjust the frequency of the ptp cycle counter by the
  * indicated ppb from the base frequency.
  */
-static int ixgbe_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+static int ixgbe_ptp_adjfreq_82599(struct ptp_clock_info *ptp, s32 ppb)
 {
 	struct ixgbe_adapter *adapter =
 		container_of(ptp, struct ixgbe_adapter, ptp_caps);
 	struct ixgbe_hw *hw = &adapter->hw;
-	u64 freq;
-	u32 diff, incval;
+	u64 freq, incval;
+	u32 diff;
 	int neg_adj = 0;
 
 	if (ppb < 0) {
@@ -235,12 +387,16 @@ static int ixgbe_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 
 	switch (hw->mac.type) {
 	case ixgbe_mac_X540:
-		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, incval);
+		if (incval > 0xFFFFFFFFULL)
+			e_dev_warn("PTP ppb adjusted SYSTIME rate overflowed!\n");
+		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, (u32)incval);
 		break;
 	case ixgbe_mac_82599EB:
+		if (incval > 0x00FFFFFFULL)
+			e_dev_warn("PTP ppb adjusted SYSTIME rate overflowed!\n");
 		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA,
 				(1 << IXGBE_INCPER_SHIFT_82599) |
-				incval);
+				((u32)incval & 0x00FFFFFFUL));
 		break;
 	default:
 		break;
@@ -250,6 +406,43 @@ static int ixgbe_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 }
 
 /**
+ * ixgbe_ptp_adjfreq_X550
+ * @ptp: the ptp clock structure
+ * @ppb: parts per billion adjustment from base
+ *
+ * adjust the frequency of the SYSTIME registers by the indicated ppb from base
+ * frequency
+ */
+static int ixgbe_ptp_adjfreq_X550(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct ixgbe_adapter *adapter =
+			container_of(ptp, struct ixgbe_adapter, ptp_caps);
+	struct ixgbe_hw *hw = &adapter->hw;
+	int neg_adj = 0;
+	u64 rate = IXGBE_X550_BASE_PERIOD;
+	u32 inca;
+
+	if (ppb < 0) {
+		neg_adj = 1;
+		ppb = -ppb;
+	}
+	rate *= ppb;
+	rate = div_u64(rate, 1000000000ULL);
+
+	/* warn if rate is too large */
+	if (rate >= INCVALUE_MASK)
+		e_dev_warn("PTP ppb adjusted SYSTIME rate overflowed!\n");
+
+	inca = rate & INCVALUE_MASK;
+	if (neg_adj)
+		inca |= ISGN;
+
+	IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, inca);
+
+	return 0;
+}
+
+/**
  * ixgbe_ptp_adjtime
  * @ptp: the ptp clock structure
  * @delta: offset to adjust the cycle counter by
@@ -263,10 +456,11 @@ static int ixgbe_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	unsigned long flags;
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-	timecounter_adjtime(&adapter->tc, delta);
+	timecounter_adjtime(&adapter->hw_tc, delta);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
-	ixgbe_ptp_setup_sdp(adapter);
+	if (adapter->ptp_setup_sdp)
+		adapter->ptp_setup_sdp(adapter);
 
 	return 0;
 }
@@ -283,11 +477,11 @@ static int ixgbe_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	struct ixgbe_adapter *adapter =
 		container_of(ptp, struct ixgbe_adapter, ptp_caps);
-	u64 ns;
 	unsigned long flags;
+	u64 ns;
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-	ns = timecounter_read(&adapter->tc);
+	ns = timecounter_read(&adapter->hw_tc);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
 	*ts = ns_to_timespec64(ns);
@@ -308,17 +502,16 @@ static int ixgbe_ptp_settime(struct ptp_clock_info *ptp,
 {
 	struct ixgbe_adapter *adapter =
 		container_of(ptp, struct ixgbe_adapter, ptp_caps);
-	u64 ns;
 	unsigned long flags;
-
-	ns = timespec64_to_ns(ts);
+	u64 ns = timespec64_to_ns(ts);
 
 	/* reset the timecounter */
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-	timecounter_init(&adapter->tc, &adapter->cc, ns);
+	timecounter_init(&adapter->hw_tc, &adapter->hw_cc, ns);
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
-	ixgbe_ptp_setup_sdp(adapter);
+	if (adapter->ptp_setup_sdp)
+		adapter->ptp_setup_sdp(adapter);
 	return 0;
 }
 
@@ -343,33 +536,26 @@ static int ixgbe_ptp_feature_enable(struct ptp_clock_info *ptp,
 	 * event when the clock SDP triggers. Clear mask when PPS is
 	 * disabled
 	 */
-	if (rq->type == PTP_CLK_REQ_PPS) {
-		switch (adapter->hw.mac.type) {
-		case ixgbe_mac_X540:
-			if (on)
-				adapter->flags2 |= IXGBE_FLAG2_PTP_PPS_ENABLED;
-			else
-				adapter->flags2 &= ~IXGBE_FLAG2_PTP_PPS_ENABLED;
-
-			ixgbe_ptp_setup_sdp(adapter);
-			return 0;
-		default:
-			break;
-		}
-	}
+	if (rq->type != PTP_CLK_REQ_PPS || !adapter->ptp_setup_sdp)
+		return -ENOTSUPP;
+
+	if (on)
+		adapter->flags2 |= IXGBE_FLAG2_PTP_PPS_ENABLED;
+	else
+		adapter->flags2 &= ~IXGBE_FLAG2_PTP_PPS_ENABLED;
 
-	return -ENOTSUPP;
+	adapter->ptp_setup_sdp(adapter);
+	return 0;
 }
 
 /**
  * ixgbe_ptp_check_pps_event
  * @adapter: the private adapter structure
- * @eicr: the interrupt cause register value
  *
  * This function is called by the interrupt routine when checking for
  * interrupts. It will check and handle a pps event.
  */
-void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter, u32 eicr)
+void ixgbe_ptp_check_pps_event(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct ptp_clock_event event;
@@ -425,7 +611,9 @@ void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
+	struct ixgbe_ring *rx_ring;
 	unsigned long rx_event;
+	int n;
 
 	/* if we don't have a valid timestamp in the registers, just update the
 	 * timeout counter and exit
@@ -437,19 +625,43 @@ void ixgbe_ptp_rx_hang(struct ixgbe_adapter *adapter)
 
 	/* determine the most recent watchdog or rx_timestamp event */
 	rx_event = adapter->last_rx_ptp_check;
-	if (time_after(adapter->last_rx_timestamp, rx_event))
-		rx_event = adapter->last_rx_timestamp;
+	for (n = 0; n < adapter->num_rx_queues; n++) {
+		rx_ring = adapter->rx_ring[n];
+		if (time_after(rx_ring->last_rx_timestamp, rx_event))
+			rx_event = rx_ring->last_rx_timestamp;
+	}
 
 	/* only need to read the high RXSTMP register to clear the lock */
-	if (time_is_before_jiffies(rx_event + 5*HZ)) {
+	if (time_is_before_jiffies(rx_event + 5 * HZ)) {
 		IXGBE_READ_REG(hw, IXGBE_RXSTMPH);
 		adapter->last_rx_ptp_check = jiffies;
 
+		adapter->rx_hwtstamp_cleared++;
 		e_warn(drv, "clearing RX Timestamp hang\n");
 	}
 }
 
 /**
+ * ixgbe_ptp_clear_tx_timestamp - utility function to clear Tx timestamp state
+ * @adapter: the private adapter structure
+ *
+ * This function should be called whenever the state related to a Tx timestamp
+ * needs to be cleared. This helps ensure that all related bits are reset for
+ * the next Tx timestamp event.
+ */
+static void ixgbe_ptp_clear_tx_timestamp(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+
+	IXGBE_READ_REG(hw, IXGBE_TXSTMPH);
+	if (adapter->ptp_tx_skb) {
+		dev_kfree_skb_any(adapter->ptp_tx_skb);
+		adapter->ptp_tx_skb = NULL;
+	}
+	clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
+}
+
+/**
  * ixgbe_ptp_tx_hwtstamp - utility function which checks for TX time stamp
  * @adapter: the private adapter struct
  *
@@ -461,23 +673,15 @@ static void ixgbe_ptp_tx_hwtstamp(struct ixgbe_adapter *adapter)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct skb_shared_hwtstamps shhwtstamps;
-	u64 regval = 0, ns;
-	unsigned long flags;
+	u64 regval = 0;
 
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPL);
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_TXSTMPH) << 32;
 
-	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-	ns = timecounter_cyc2time(&adapter->tc, regval);
-	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
-
-	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
-	shhwtstamps.hwtstamp = ns_to_ktime(ns);
+	ixgbe_ptp_convert_to_hwtstamp(adapter, &shhwtstamps, regval);
 	skb_tstamp_tx(adapter->ptp_tx_skb, &shhwtstamps);
 
-	dev_kfree_skb_any(adapter->ptp_tx_skb);
-	adapter->ptp_tx_skb = NULL;
-	clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
+	ixgbe_ptp_clear_tx_timestamp(adapter);
 }
 
 /**
@@ -497,38 +701,85 @@ static void ixgbe_ptp_tx_hwtstamp_work(struct work_struct *work)
 					      IXGBE_PTP_TX_TIMEOUT);
 	u32 tsynctxctl;
 
-	if (timeout) {
-		dev_kfree_skb_any(adapter->ptp_tx_skb);
-		adapter->ptp_tx_skb = NULL;
-		clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
-		e_warn(drv, "clearing Tx Timestamp hang\n");
+	/* we have to have a valid skb to poll for a timestamp */
+	if (!adapter->ptp_tx_skb) {
+		ixgbe_ptp_clear_tx_timestamp(adapter);
 		return;
 	}
 
+	/* stop polling once we have a valid timestamp */
 	tsynctxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCTXCTL);
-	if (tsynctxctl & IXGBE_TSYNCTXCTL_VALID)
+	if (tsynctxctl & IXGBE_TSYNCTXCTL_VALID) {
 		ixgbe_ptp_tx_hwtstamp(adapter);
-	else
+		return;
+	}
+
+	if (timeout) {
+		ixgbe_ptp_clear_tx_timestamp(adapter);
+		adapter->tx_hwtstamp_timeouts++;
+		e_warn(drv, "clearing Tx Timestamp hang\n");
+	} else {
 		/* reschedule to keep checking if it's not available yet */
 		schedule_work(&adapter->ptp_tx_work);
+	}
 }
 
 /**
- * ixgbe_ptp_rx_hwtstamp - utility function which checks for RX time stamp
- * @adapter: pointer to adapter struct
+ * ixgbe_ptp_rx_pktstamp - utility function to get RX time stamp from buffer
+ * @q_vector: structure containing interrupt and ring information
+ * @skb: the packet
+ *
+ * This function will be called by the Rx routine of the timestamp for this
+ * packet is stored in the buffer. The value is stored in little endian format
+ * starting at the end of the packet data.
+ */
+void ixgbe_ptp_rx_pktstamp(struct ixgbe_q_vector *q_vector,
+			   struct sk_buff *skb)
+{
+	__le64 regval;
+
+	/* copy the bits out of the skb, and then trim the skb length */
+	skb_copy_bits(skb, skb->len - IXGBE_TS_HDR_LEN, &regval,
+		      IXGBE_TS_HDR_LEN);
+	__pskb_trim(skb, skb->len - IXGBE_TS_HDR_LEN);
+
+	/* The timestamp is recorded in little endian format, and is stored at
+	 * the end of the packet.
+	 *
+	 * DWORD: N              N + 1      N + 2
+	 * Field: End of Packet  SYSTIMH    SYSTIML
+	 */
+	ixgbe_ptp_convert_to_hwtstamp(q_vector->adapter, skb_hwtstamps(skb),
+				      le64_to_cpu(regval));
+}
+
+/**
+ * ixgbe_ptp_rx_rgtstamp - utility function which checks for RX time stamp
+ * @q_vector: structure containing interrupt and ring information
  * @skb: particular skb to send timestamp with
  *
  * if the timestamp is valid, we convert it into the timecounter ns
  * value, then store that result into the shhwtstamps structure which
  * is passed up the network stack
  */
-void ixgbe_ptp_rx_hwtstamp(struct ixgbe_adapter *adapter, struct sk_buff *skb)
+void ixgbe_ptp_rx_rgtstamp(struct ixgbe_q_vector *q_vector,
+			   struct sk_buff *skb)
 {
-	struct ixgbe_hw *hw = &adapter->hw;
-	struct skb_shared_hwtstamps *shhwtstamps;
-	u64 regval = 0, ns;
+	struct ixgbe_adapter *adapter;
+	struct ixgbe_hw *hw;
+	u64 regval = 0;
 	u32 tsyncrxctl;
-	unsigned long flags;
+
+	/* we cannot process timestamps on a ring without a q_vector */
+	if (!q_vector || !q_vector->adapter)
+		return;
+
+	adapter = q_vector->adapter;
+	hw = &adapter->hw;
+
+	/* Read the tsyncrxctl register afterwards in order to prevent taking an
+	 * I/O hit on every packet.
+	 */
 
 	tsyncrxctl = IXGBE_READ_REG(hw, IXGBE_TSYNCRXCTL);
 	if (!(tsyncrxctl & IXGBE_TSYNCRXCTL_VALID))
@@ -537,17 +788,7 @@ void ixgbe_ptp_rx_hwtstamp(struct ixgbe_adapter *adapter, struct sk_buff *skb)
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPL);
 	regval |= (u64)IXGBE_READ_REG(hw, IXGBE_RXSTMPH) << 32;
 
-	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-	ns = timecounter_cyc2time(&adapter->tc, regval);
-	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
-
-	shhwtstamps = skb_hwtstamps(skb);
-	shhwtstamps->hwtstamp = ns_to_ktime(ns);
-
-	/* Update the last_rx_timestamp timer in order to enable watchdog check
-	 * for error case of latched timestamp on a dropped packet.
-	 */
-	adapter->last_rx_timestamp = jiffies;
+	ixgbe_ptp_convert_to_hwtstamp(adapter, skb_hwtstamps(skb), regval);
 }
 
 int ixgbe_ptp_get_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr)
@@ -610,14 +851,20 @@ static int ixgbe_ptp_set_timestamp_mode(struct ixgbe_adapter *adapter,
 	case HWTSTAMP_FILTER_NONE:
 		tsync_rx_ctl = 0;
 		tsync_rx_mtrl = 0;
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L4_V1;
 		tsync_rx_mtrl |= IXGBE_RXMTRL_V1_SYNC_MSG;
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_L4_V1;
 		tsync_rx_mtrl |= IXGBE_RXMTRL_V1_DELAY_REQ_MSG;
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		break;
 	case HWTSTAMP_FILTER_PTP_V2_EVENT:
 	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
@@ -631,9 +878,21 @@ static int ixgbe_ptp_set_timestamp_mode(struct ixgbe_adapter *adapter,
 		tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_EVENT_V2;
 		is_l2 = true;
 		config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		break;
 	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 	case HWTSTAMP_FILTER_ALL:
+		/* The X550 controller is capable of timestamping all packets,
+		 * which allows it to accept any filter.
+		 */
+		if (hw->mac.type >= ixgbe_mac_X550) {
+			tsync_rx_ctl |= IXGBE_TSYNCRXCTL_TYPE_ALL;
+			config->rx_filter = HWTSTAMP_FILTER_ALL;
+			adapter->flags |= IXGBE_FLAG_RX_HWTSTAMP_ENABLED;
+			break;
+		}
+		/* fall through */
 	default:
 		/*
 		 * register RXMTRL must be set in order to do V1 packets,
@@ -641,16 +900,46 @@ static int ixgbe_ptp_set_timestamp_mode(struct ixgbe_adapter *adapter,
 		 * Delay_Req messages and hardware does not support
 		 * timestamping all packets => return error
 		 */
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		config->rx_filter = HWTSTAMP_FILTER_NONE;
 		return -ERANGE;
 	}
 
 	if (hw->mac.type == ixgbe_mac_82598EB) {
+		adapter->flags &= ~(IXGBE_FLAG_RX_HWTSTAMP_ENABLED |
+				    IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER);
 		if (tsync_rx_ctl | tsync_tx_ctl)
 			return -ERANGE;
 		return 0;
 	}
 
+	/* Per-packet timestamping only works if the filter is set to all
+	 * packets. Since this is desired, always timestamp all packets as long
+	 * as any Rx filter was configured.
+	 */
+	switch (hw->mac.type) {
+	case ixgbe_mac_X550:
+	case ixgbe_mac_X550EM_x:
+		/* enable timestamping all packets only if at least some
+		 * packets were requested. Otherwise, play nice and disable
+		 * timestamping
+		 */
+		if (config->rx_filter == HWTSTAMP_FILTER_NONE)
+			break;
+
+		tsync_rx_ctl = IXGBE_TSYNCRXCTL_ENABLED |
+			       IXGBE_TSYNCRXCTL_TYPE_ALL |
+			       IXGBE_TSYNCRXCTL_TSIP_UT_EN;
+		config->rx_filter = HWTSTAMP_FILTER_ALL;
+		adapter->flags |= IXGBE_FLAG_RX_HWTSTAMP_ENABLED;
+		adapter->flags &= ~IXGBE_FLAG_RX_HWTSTAMP_IN_REGISTER;
+		is_l2 = true;
+		break;
+	default:
+		break;
+	}
+
 	/* define ethertype filter for timestamping L2 packets */
 	if (is_l2)
 		IXGBE_WRITE_REG(hw, IXGBE_ETQF(IXGBE_ETQF_FILTER_1588),
@@ -678,8 +967,8 @@ static int ixgbe_ptp_set_timestamp_mode(struct ixgbe_adapter *adapter,
 	IXGBE_WRITE_FLUSH(hw);
 
 	/* clear TX/RX time stamp registers, just to be sure */
-	regval = IXGBE_READ_REG(hw, IXGBE_TXSTMPH);
-	regval = IXGBE_READ_REG(hw, IXGBE_RXSTMPH);
+	ixgbe_ptp_clear_tx_timestamp(adapter);
+	IXGBE_READ_REG(hw, IXGBE_RXSTMPH);
 
 	return 0;
 }
@@ -712,23 +1001,9 @@ int ixgbe_ptp_set_ts_config(struct ixgbe_adapter *adapter, struct ifreq *ifr)
 		-EFAULT : 0;
 }
 
-/**
- * ixgbe_ptp_start_cyclecounter - create the cycle counter from hw
- * @adapter: pointer to the adapter structure
- *
- * This function should be called to set the proper values for the TIMINCA
- * register and tell the cyclecounter structure what the tick rate of SYSTIME
- * is. It does not directly modify SYSTIME registers or the timecounter
- * structure. It should be called whenever a new TIMINCA value is necessary,
- * such as during initialization or when the link speed changes.
- */
-void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
+static void ixgbe_ptp_link_speed_adjust(struct ixgbe_adapter *adapter,
+					u32 *shift, u32 *incval)
 {
-	struct ixgbe_hw *hw = &adapter->hw;
-	u32 incval = 0;
-	u32 shift = 0;
-	unsigned long flags;
-
 	/**
 	 * Scale the NIC cycle counter by a large factor so that
 	 * relatively small corrections to the frequency can be added
@@ -745,36 +1020,98 @@ void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
 	 */
 	switch (adapter->link_speed) {
 	case IXGBE_LINK_SPEED_100_FULL:
-		incval = IXGBE_INCVAL_100;
-		shift = IXGBE_INCVAL_SHIFT_100;
+		*shift = IXGBE_INCVAL_SHIFT_100;
+		*incval = IXGBE_INCVAL_100;
 		break;
 	case IXGBE_LINK_SPEED_1GB_FULL:
-		incval = IXGBE_INCVAL_1GB;
-		shift = IXGBE_INCVAL_SHIFT_1GB;
+		*shift = IXGBE_INCVAL_SHIFT_1GB;
+		*incval = IXGBE_INCVAL_1GB;
 		break;
 	case IXGBE_LINK_SPEED_10GB_FULL:
 	default:
-		incval = IXGBE_INCVAL_10GB;
-		shift = IXGBE_INCVAL_SHIFT_10GB;
+		*shift = IXGBE_INCVAL_SHIFT_10GB;
+		*incval = IXGBE_INCVAL_10GB;
 		break;
 	}
+}
 
-	/**
-	 * Modify the calculated values to fit within the correct
-	 * number of bits specified by the hardware. The 82599 doesn't
-	 * have the same space as the X540, so bitshift the calculated
-	 * values to fit.
+/**
+ * ixgbe_ptp_start_cyclecounter - create the cycle counter from hw
+ * @adapter: pointer to the adapter structure
+ *
+ * This function should be called to set the proper values for the TIMINCA
+ * register and tell the cyclecounter structure what the tick rate of SYSTIME
+ * is. It does not directly modify SYSTIME registers or the timecounter
+ * structure. It should be called whenever a new TIMINCA value is necessary,
+ * such as during initialization or when the link speed changes.
+ */
+void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct cyclecounter cc;
+	unsigned long flags;
+	u32 incval = 0;
+	u32 tsauxc = 0;
+	u32 fuse0 = 0;
+
+	/* For some of the boards below this mask is technically incorrect.
+	 * The timestamp mask overflows at approximately 61bits. However the
+	 * particular hardware does not overflow on an even bitmask value.
+	 * Instead, it overflows due to conversion of upper 32bits billions of
+	 * cycles. Timecounters are not really intended for this purpose so
+	 * they do not properly function if the overflow point isn't 2^N-1.
+	 * However, the actual SYSTIME values in question take ~138 years to
+	 * overflow. In practice this means they won't actually overflow. A
+	 * proper fix to this problem would require modification of the
+	 * timecounter delta calculations.
 	 */
+	cc.mask = CLOCKSOURCE_MASK(64);
+	cc.mult = 1;
+	cc.shift = 0;
+
 	switch (hw->mac.type) {
+	case ixgbe_mac_X550EM_x:
+		/* SYSTIME assumes X550EM_x board frequency is 300Mhz, and is
+		 * designed to represent seconds and nanoseconds when this is
+		 * the case. However, some revisions of hardware have a 400Mhz
+		 * clock and we have to compensate for this frequency
+		 * variation using corrected mult and shift values.
+		 */
+		fuse0 = IXGBE_READ_REG(hw, IXGBE_FUSES0_GROUP(0));
+		if (!(fuse0 & IXGBE_FUSES0_300MHZ)) {
+			cc.mult = 3;
+			cc.shift = 2;
+		}
+		/* fallthrough */
+	case ixgbe_mac_X550:
+		cc.read = ixgbe_ptp_read_X550;
+
+		/* enable SYSTIME counter */
+		IXGBE_WRITE_REG(hw, IXGBE_SYSTIMR, 0);
+		IXGBE_WRITE_REG(hw, IXGBE_SYSTIML, 0);
+		IXGBE_WRITE_REG(hw, IXGBE_SYSTIMH, 0);
+		tsauxc = IXGBE_READ_REG(hw, IXGBE_TSAUXC);
+		IXGBE_WRITE_REG(hw, IXGBE_TSAUXC,
+				tsauxc & ~IXGBE_TSAUXC_DISABLE_SYSTIME);
+		IXGBE_WRITE_REG(hw, IXGBE_TSIM, IXGBE_TSIM_TXTS);
+		IXGBE_WRITE_REG(hw, IXGBE_EIMS, IXGBE_EIMS_TIMESYNC);
+
+		IXGBE_WRITE_FLUSH(hw);
+		break;
 	case ixgbe_mac_X540:
+		cc.read = ixgbe_ptp_read_82599;
+
+		ixgbe_ptp_link_speed_adjust(adapter, &cc.shift, &incval);
 		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA, incval);
 		break;
 	case ixgbe_mac_82599EB:
+		cc.read = ixgbe_ptp_read_82599;
+
+		ixgbe_ptp_link_speed_adjust(adapter, &cc.shift, &incval);
 		incval >>= IXGBE_INCVAL_SHIFT_82599;
-		shift -= IXGBE_INCVAL_SHIFT_82599;
+		cc.shift -= IXGBE_INCVAL_SHIFT_82599;
 		IXGBE_WRITE_REG(hw, IXGBE_TIMINCA,
-				(1 << IXGBE_INCPER_SHIFT_82599) |
-				incval);
+				(1 << IXGBE_INCPER_SHIFT_82599) | incval);
 		break;
 	default:
 		/* other devices aren't supported */
@@ -787,13 +1124,7 @@ void ixgbe_ptp_start_cyclecounter(struct ixgbe_adapter *adapter)
 
 	/* need lock to prevent incorrect read while modifying cyclecounter */
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-
-	memset(&adapter->cc, 0, sizeof(adapter->cc));
-	adapter->cc.read = ixgbe_ptp_read;
-	adapter->cc.mask = CYCLECOUNTER_MASK(64);
-	adapter->cc.shift = shift;
-	adapter->cc.mult = 1;
-
+	memcpy(&adapter->hw_cc, &cc, sizeof(adapter->hw_cc));
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 }
 
@@ -814,29 +1145,27 @@ void ixgbe_ptp_reset(struct ixgbe_adapter *adapter)
 	struct ixgbe_hw *hw = &adapter->hw;
 	unsigned long flags;
 
-	/* set SYSTIME registers to 0 just in case */
-	IXGBE_WRITE_REG(hw, IXGBE_SYSTIML, 0x00000000);
-	IXGBE_WRITE_REG(hw, IXGBE_SYSTIMH, 0x00000000);
-	IXGBE_WRITE_FLUSH(hw);
-
 	/* reset the hardware timestamping mode */
 	ixgbe_ptp_set_timestamp_mode(adapter, &adapter->tstamp_config);
 
+	/* 82598 does not support PTP */
+	if (hw->mac.type == ixgbe_mac_82598EB)
+		return;
+
 	ixgbe_ptp_start_cyclecounter(adapter);
 
 	spin_lock_irqsave(&adapter->tmreg_lock, flags);
-
-	/* reset the ns time counter */
-	timecounter_init(&adapter->tc, &adapter->cc,
+	timecounter_init(&adapter->hw_tc, &adapter->hw_cc,
 			 ktime_to_ns(ktime_get_real()));
-
 	spin_unlock_irqrestore(&adapter->tmreg_lock, flags);
 
-	/*
-	 * Now that the shift has been calculated and the systime
+	adapter->last_overflow_check = jiffies;
+
+	/* Now that the shift has been calculated and the systime
 	 * registers reset, (re-)enable the Clock out feature
 	 */
-	ixgbe_ptp_setup_sdp(adapter);
+	if (adapter->ptp_setup_sdp)
+		adapter->ptp_setup_sdp(adapter);
 }
 
 /**
@@ -845,11 +1174,11 @@ void ixgbe_ptp_reset(struct ixgbe_adapter *adapter)
  *
  * This function performs setup of the user entry point function table and
  * initializes the PTP clock device, which is used to access the clock-like
- * features of the PTP core. It will be called by ixgbe_ptp_init, only if
- * there isn't already a clock device (such as after a suspend/resume cycle,
- * where the clock device wasn't destroyed).
+ * features of the PTP core. It will be called by ixgbe_ptp_init, and may
+ * reuse a previously initialized clock (such as during a suspend/resume
+ * cycle).
  */
-static int ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
+static long ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
 	long err;
@@ -869,11 +1198,12 @@ static int ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 		adapter->ptp_caps.n_ext_ts = 0;
 		adapter->ptp_caps.n_per_out = 0;
 		adapter->ptp_caps.pps = 1;
-		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
+		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq_82599;
 		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
 		adapter->ptp_caps.gettime64 = ixgbe_ptp_gettime;
 		adapter->ptp_caps.settime64 = ixgbe_ptp_settime;
 		adapter->ptp_caps.enable = ixgbe_ptp_feature_enable;
+		adapter->ptp_setup_sdp = ixgbe_ptp_setup_sdp_x540;
 		break;
 	case ixgbe_mac_82599EB:
 		snprintf(adapter->ptp_caps.name,
@@ -885,14 +1215,31 @@ static int ixgbe_ptp_create_clock(struct ixgbe_adapter *adapter)
 		adapter->ptp_caps.n_ext_ts = 0;
 		adapter->ptp_caps.n_per_out = 0;
 		adapter->ptp_caps.pps = 0;
-		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq;
+		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq_82599;
+		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
+		adapter->ptp_caps.gettime64 = ixgbe_ptp_gettime;
+		adapter->ptp_caps.settime64 = ixgbe_ptp_settime;
+		adapter->ptp_caps.enable = ixgbe_ptp_feature_enable;
+		break;
+	case ixgbe_mac_X550:
+	case ixgbe_mac_X550EM_x:
+		snprintf(adapter->ptp_caps.name, 16, "%s", netdev->name);
+		adapter->ptp_caps.owner = THIS_MODULE;
+		adapter->ptp_caps.max_adj = 30000000;
+		adapter->ptp_caps.n_alarm = 0;
+		adapter->ptp_caps.n_ext_ts = 0;
+		adapter->ptp_caps.n_per_out = 0;
+		adapter->ptp_caps.pps = 0;
+		adapter->ptp_caps.adjfreq = ixgbe_ptp_adjfreq_X550;
 		adapter->ptp_caps.adjtime = ixgbe_ptp_adjtime;
 		adapter->ptp_caps.gettime64 = ixgbe_ptp_gettime;
 		adapter->ptp_caps.settime64 = ixgbe_ptp_settime;
 		adapter->ptp_caps.enable = ixgbe_ptp_feature_enable;
+		adapter->ptp_setup_sdp = NULL;
 		break;
 	default:
 		adapter->ptp_clock = NULL;
+		adapter->ptp_setup_sdp = NULL;
 		return -EOPNOTSUPP;
 	}
 
@@ -961,18 +1308,13 @@ void ixgbe_ptp_suspend(struct ixgbe_adapter *adapter)
 	if (!test_and_clear_bit(__IXGBE_PTP_RUNNING, &adapter->state))
 		return;
 
-	/* since this might be called in suspend, we don't clear the state,
-	 * but simply reset the auxiliary PPS signal control register
-	 */
-	IXGBE_WRITE_REG(&adapter->hw, IXGBE_TSAUXC, 0x0);
+	adapter->flags2 &= ~IXGBE_FLAG2_PTP_PPS_ENABLED;
+	if (adapter->ptp_setup_sdp)
+		adapter->ptp_setup_sdp(adapter);
 
 	/* ensure that we cancel any pending PTP Tx work item in progress */
 	cancel_work_sync(&adapter->ptp_tx_work);
-	if (adapter->ptp_tx_skb) {
-		dev_kfree_skb_any(adapter->ptp_tx_skb);
-		adapter->ptp_tx_skb = NULL;
-		clear_bit_unlock(__IXGBE_PTP_TX_IN_PROGRESS, &adapter->state);
-	}
+	ixgbe_ptp_clear_tx_timestamp(adapter);
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index fcd8b27a0ccb..8025a3f93598 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -1,7 +1,7 @@
 /*******************************************************************************
 
   Intel 10 Gigabit PCI Express Linux driver
-  Copyright(c) 1999 - 2014 Intel Corporation.
+  Copyright(c) 1999 - 2015 Intel Corporation.
 
   This program is free software; you can redistribute it and/or modify it
   under the terms and conditions of the GNU General Public License,
@@ -130,6 +130,38 @@ static int __ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 	return -ENOMEM;
 }
 
+/**
+ * ixgbe_get_vfs - Find and take references to all vf devices
+ * @adapter: Pointer to adapter struct
+ */
+static void ixgbe_get_vfs(struct ixgbe_adapter *adapter)
+{
+	struct pci_dev *pdev = adapter->pdev;
+	u16 vendor = pdev->vendor;
+	struct pci_dev *vfdev;
+	int vf = 0;
+	u16 vf_id;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_SRIOV);
+	if (!pos)
+		return;
+	pci_read_config_word(pdev, pos + PCI_SRIOV_VF_DID, &vf_id);
+
+	vfdev = pci_get_device(vendor, vf_id, NULL);
+	for (; vfdev; vfdev = pci_get_device(vendor, vf_id, vfdev)) {
+		if (!vfdev->is_virtfn)
+			continue;
+		if (vfdev->physfn != pdev)
+			continue;
+		if (vf >= adapter->num_vfs)
+			continue;
+		pci_dev_get(vfdev);
+		adapter->vfinfo[vf].vfdev = vfdev;
+		++vf;
+	}
+}
+
 /* Note this function is called when the user wants to enable SR-IOV
  * VFs using the now deprecated module parameter
  */
@@ -170,8 +202,10 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 		}
 	}
 
-	if (!__ixgbe_enable_sriov(adapter))
+	if (!__ixgbe_enable_sriov(adapter)) {
+		ixgbe_get_vfs(adapter);
 		return;
+	}
 
 	/* If we have gotten to this point then there is no memory available
 	 * to manage the VF devices - print message and bail.
@@ -184,6 +218,7 @@ void ixgbe_enable_sriov(struct ixgbe_adapter *adapter)
 #endif /* #ifdef CONFIG_PCI_IOV */
 int ixgbe_disable_sriov(struct ixgbe_adapter *adapter)
 {
+	unsigned int num_vfs = adapter->num_vfs, vf;
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 gpie;
 	u32 vmdctl;
@@ -192,6 +227,16 @@ int ixgbe_disable_sriov(struct ixgbe_adapter *adapter)
 	/* set num VFs to 0 to prevent access to vfinfo */
 	adapter->num_vfs = 0;
 
+	/* put the reference to all of the vf devices */
+	for (vf = 0; vf < num_vfs; ++vf) {
+		struct pci_dev *vfdev = adapter->vfinfo[vf].vfdev;
+
+		if (!vfdev)
+			continue;
+		adapter->vfinfo[vf].vfdev = NULL;
+		pci_dev_put(vfdev);
+	}
+
 	/* free VF control structures */
 	kfree(adapter->vfinfo);
 	adapter->vfinfo = NULL;
@@ -289,6 +334,7 @@ static int ixgbe_pci_sriov_enable(struct pci_dev *dev, int num_vfs)
 		e_dev_warn("Failed to enable PCI sriov: %d\n", err);
 		return err;
 	}
+	ixgbe_get_vfs(adapter);
 	ixgbe_sriov_reinit(adapter);
 
 	return num_vfs;
@@ -406,11 +452,34 @@ void ixgbe_restore_vf_multicasts(struct ixgbe_adapter *adapter)
 static int ixgbe_set_vf_vlan(struct ixgbe_adapter *adapter, int add, int vid,
 			     u32 vf)
 {
-	/* VLAN 0 is a special case, don't allow it to be removed */
-	if (!vid && !add)
-		return 0;
+	struct ixgbe_hw *hw = &adapter->hw;
+	int err;
+
+	/* If VLAN overlaps with one the PF is currently monitoring make
+	 * sure that we are able to allocate a VLVF entry.  This may be
+	 * redundant but it guarantees PF will maintain visibility to
+	 * the VLAN.
+	 */
+	if (add && test_bit(vid, adapter->active_vlans)) {
+		err = hw->mac.ops.set_vfta(hw, vid, VMDQ_P(0), true, false);
+		if (err)
+			return err;
+	}
+
+	err = hw->mac.ops.set_vfta(hw, vid, vf, !!add, false);
 
-	return adapter->hw.mac.ops.set_vfta(&adapter->hw, vid, vf, (bool)add);
+	if (add && !err)
+		return err;
+
+	/* If we failed to add the VF VLAN or we are removing the VF VLAN
+	 * we may need to drop the PF pool bit in order to allow us to free
+	 * up the VLVF resources.
+	 */
+	if (test_bit(vid, adapter->active_vlans) ||
+	    (adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC))
+		ixgbe_update_pf_promisc_vlvf(adapter, vid);
+
+	return err;
 }
 
 static s32 ixgbe_set_vf_lpe(struct ixgbe_adapter *adapter, u32 *msgbuf, u32 vf)
@@ -516,13 +585,75 @@ static void ixgbe_clear_vmvir(struct ixgbe_adapter *adapter, u32 vf)
 
 	IXGBE_WRITE_REG(hw, IXGBE_VMVIR(vf), 0);
 }
+
+static void ixgbe_clear_vf_vlans(struct ixgbe_adapter *adapter, u32 vf)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	u32 i;
+
+	/* post increment loop, covers VLVF_ENTRIES - 1 to 0 */
+	for (i = IXGBE_VLVF_ENTRIES; i--;) {
+		u32 bits[2], vlvfb, vid, vfta, vlvf;
+		u32 word = i * 2 + vf / 32;
+		u32 mask = 1 << (vf % 32);
+
+		vlvfb = IXGBE_READ_REG(hw, IXGBE_VLVFB(word));
+
+		/* if our bit isn't set we can skip it */
+		if (!(vlvfb & mask))
+			continue;
+
+		/* clear our bit from vlvfb */
+		vlvfb ^= mask;
+
+		/* create 64b mask to chedk to see if we should clear VLVF */
+		bits[word % 2] = vlvfb;
+		bits[~word % 2] = IXGBE_READ_REG(hw, IXGBE_VLVFB(word ^ 1));
+
+		/* if promisc is enabled, PF will be present, leave VFTA */
+		if (adapter->flags2 & IXGBE_FLAG2_VLAN_PROMISC) {
+			bits[VMDQ_P(0) / 32] &= ~(1 << (VMDQ_P(0) % 32));
+
+			if (bits[0] || bits[1])
+				goto update_vlvfb;
+			goto update_vlvf;
+		}
+
+		/* if other pools are present, just remove ourselves */
+		if (bits[0] || bits[1])
+			goto update_vlvfb;
+
+		/* if we cannot determine VLAN just remove ourselves */
+		vlvf = IXGBE_READ_REG(hw, IXGBE_VLVF(i));
+		if (!vlvf)
+			goto update_vlvfb;
+
+		vid = vlvf & VLAN_VID_MASK;
+		mask = 1 << (vid % 32);
+
+		/* clear bit from VFTA */
+		vfta = IXGBE_READ_REG(hw, IXGBE_VFTA(vid / 32));
+		if (vfta & mask)
+			IXGBE_WRITE_REG(hw, IXGBE_VFTA(vid / 32), vfta ^ mask);
+update_vlvf:
+		/* clear POOL selection enable */
+		IXGBE_WRITE_REG(hw, IXGBE_VLVF(i), 0);
+update_vlvfb:
+		/* clear pool bits */
+		IXGBE_WRITE_REG(hw, IXGBE_VLVFB(word), vlvfb);
+	}
+}
+
 static inline void ixgbe_vf_reset_event(struct ixgbe_adapter *adapter, u32 vf)
 {
 	struct ixgbe_hw *hw = &adapter->hw;
 	struct vf_data_storage *vfinfo = &adapter->vfinfo[vf];
 	u8 num_tcs = netdev_get_num_tc(adapter->netdev);
 
-	/* add PF assigned VLAN or VLAN 0 */
+	/* remove VLAN filters beloning to this VF */
+	ixgbe_clear_vf_vlans(adapter, vf);
+
+	/* add back PF assigned VLAN or VLAN 0 */
 	ixgbe_set_vf_vlan(adapter, true, vfinfo->pf_vlan, vf);
 
 	/* reset offloads to defaults */
@@ -768,40 +899,14 @@ static int ixgbe_set_vf_mac_addr(struct ixgbe_adapter *adapter,
 	return ixgbe_set_vf_mac(adapter, vf, new_mac) < 0;
 }
 
-static int ixgbe_find_vlvf_entry(struct ixgbe_hw *hw, u32 vlan)
-{
-	u32 vlvf;
-	s32 regindex;
-
-	/* short cut the special case */
-	if (vlan == 0)
-		return 0;
-
-	/* Search for the vlan id in the VLVF entries */
-	for (regindex = 1; regindex < IXGBE_VLVF_ENTRIES; regindex++) {
-		vlvf = IXGBE_READ_REG(hw, IXGBE_VLVF(regindex));
-		if ((vlvf & VLAN_VID_MASK) == vlan)
-			break;
-	}
-
-	/* Return a negative value if not found */
-	if (regindex >= IXGBE_VLVF_ENTRIES)
-		regindex = -1;
-
-	return regindex;
-}
-
 static int ixgbe_set_vf_vlan_msg(struct ixgbe_adapter *adapter,
 				 u32 *msgbuf, u32 vf)
 {
+	u32 add = (msgbuf[0] & IXGBE_VT_MSGINFO_MASK) >> IXGBE_VT_MSGINFO_SHIFT;
+	u32 vid = (msgbuf[1] & IXGBE_VLVF_VLANID_MASK);
+	u8 tcs = netdev_get_num_tc(adapter->netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	int add = (msgbuf[0] & IXGBE_VT_MSGINFO_MASK) >> IXGBE_VT_MSGINFO_SHIFT;
-	int vid = (msgbuf[1] & IXGBE_VLVF_VLANID_MASK);
 	int err;
-	s32 reg_ndx;
-	u32 vlvf;
-	u32 bits;
-	u8 tcs = netdev_get_num_tc(adapter->netdev);
 
 	if (adapter->vfinfo[vf].pf_vlan || tcs) {
 		e_warn(drv,
@@ -811,54 +916,23 @@ static int ixgbe_set_vf_vlan_msg(struct ixgbe_adapter *adapter,
 		return -1;
 	}
 
-	if (add)
-		adapter->vfinfo[vf].vlan_count++;
-	else if (adapter->vfinfo[vf].vlan_count)
-		adapter->vfinfo[vf].vlan_count--;
-
-	/* in case of promiscuous mode any VLAN filter set for a VF must
-	 * also have the PF pool added to it.
-	 */
-	if (add && adapter->netdev->flags & IFF_PROMISC)
-		err = ixgbe_set_vf_vlan(adapter, add, vid, VMDQ_P(0));
+	/* VLAN 0 is a special case, don't allow it to be removed */
+	if (!vid && !add)
+		return 0;
 
 	err = ixgbe_set_vf_vlan(adapter, add, vid, vf);
-	if (!err && adapter->vfinfo[vf].spoofchk_enabled)
-		hw->mac.ops.set_vlan_anti_spoofing(hw, true, vf);
+	if (err)
+		return err;
 
-	/* Go through all the checks to see if the VLAN filter should
-	 * be wiped completely.
-	 */
-	if (!add && adapter->netdev->flags & IFF_PROMISC) {
-		reg_ndx = ixgbe_find_vlvf_entry(hw, vid);
-		if (reg_ndx < 0)
-			return err;
-		vlvf = IXGBE_READ_REG(hw, IXGBE_VLVF(reg_ndx));
-		/* See if any other pools are set for this VLAN filter
-		 * entry other than the PF.
-		 */
-		if (VMDQ_P(0) < 32) {
-			bits = IXGBE_READ_REG(hw, IXGBE_VLVFB(reg_ndx * 2));
-			bits &= ~(1 << VMDQ_P(0));
-			bits |= IXGBE_READ_REG(hw,
-					       IXGBE_VLVFB(reg_ndx * 2) + 1);
-		} else {
-			bits = IXGBE_READ_REG(hw,
-					      IXGBE_VLVFB(reg_ndx * 2) + 1);
-			bits &= ~(1 << (VMDQ_P(0) - 32));
-			bits |= IXGBE_READ_REG(hw, IXGBE_VLVFB(reg_ndx * 2));
-		}
+	if (adapter->vfinfo[vf].spoofchk_enabled)
+		hw->mac.ops.set_vlan_anti_spoofing(hw, true, vf);
 
-		/* If the filter was removed then ensure PF pool bit
-		 * is cleared if the PF only added itself to the pool
-		 * because the PF is in promiscuous mode.
-		 */
-		if ((vlvf & VLAN_VID_MASK) == vid &&
-		    !test_bit(vid, adapter->active_vlans) && !bits)
-			ixgbe_set_vf_vlan(adapter, add, vid, VMDQ_P(0));
-	}
+	if (add)
+		adapter->vfinfo[vf].vlan_count++;
+	else if (adapter->vfinfo[vf].vlan_count)
+		adapter->vfinfo[vf].vlan_count--;
 
-	return err;
+	return 0;
 }
 
 static int ixgbe_set_vf_macvlan_msg(struct ixgbe_adapter *adapter,
@@ -1239,6 +1313,9 @@ static int ixgbe_enable_port_vlan(struct ixgbe_adapter *adapter, int vf,
 	if (err)
 		goto out;
 
+	/* Revoke tagless access via VLAN 0 */
+	ixgbe_set_vf_vlan(adapter, false, 0, vf);
+
 	ixgbe_set_vmvir(adapter, vlan, qos, vf);
 	ixgbe_set_vmolr(hw, vf, false);
 	if (adapter->vfinfo[vf].spoofchk_enabled)
@@ -1272,6 +1349,8 @@ static int ixgbe_disable_port_vlan(struct ixgbe_adapter *adapter, int vf)
 
 	err = ixgbe_set_vf_vlan(adapter, false,
 				adapter->vfinfo[vf].pf_vlan, vf);
+	/* Restore tagless access via VLAN 0 */
+	ixgbe_set_vf_vlan(adapter, true, 0, vf);
 	ixgbe_clear_vmvir(adapter, vf);
 	ixgbe_set_vmolr(hw, vf, true);
 	hw->mac.ops.set_vlan_anti_spoofing(hw, false, vf);
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index 995f03107eac..bf7367a08716 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -1020,6 +1020,7 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_TXSTMPH    0x08C08 /* Tx timestamp value High - RO */
 #define IXGBE_SYSTIML    0x08C0C /* System time register Low - RO */
 #define IXGBE_SYSTIMH    0x08C10 /* System time register High - RO */
+#define IXGBE_SYSTIMR    0x08C58 /* System time register Residue - RO */
 #define IXGBE_TIMINCA    0x08C14 /* Increment attributes register - RW */
 #define IXGBE_TIMADJL    0x08C18 /* Time Adjustment Offset register Low - RW */
 #define IXGBE_TIMADJH    0x08C1C /* Time Adjustment Offset register High - RW */
@@ -1036,6 +1037,7 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_AUXSTMPH0  0x08C40 /* Auxiliary Time Stamp 0 register High - RO */
 #define IXGBE_AUXSTMPL1  0x08C44 /* Auxiliary Time Stamp 1 register Low - RO */
 #define IXGBE_AUXSTMPH1  0x08C48 /* Auxiliary Time Stamp 1 register High - RO */
+#define IXGBE_TSIM       0x08C68 /* TimeSync Interrupt Mask Register - RW */
 
 /* Diagnostic Registers */
 #define IXGBE_RDSTATCTL   0x02C20
@@ -1345,7 +1347,10 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_MASK	0xFF01 /* int chip-wide mask */
 #define IXGBE_MDIO_GLOBAL_INT_CHIP_VEN_FLAG	0xFC01 /* int chip-wide mask */
 #define IXGBE_MDIO_GLOBAL_ALARM_1		0xCC00 /* Global alarm 1 */
+#define IXGBE_MDIO_GLOBAL_ALM_1_DEV_FAULT	0x0010 /* device fault */
 #define IXGBE_MDIO_GLOBAL_ALM_1_HI_TMP_FAIL	0x4000 /* high temp failure */
+#define IXGBE_MDIO_GLOBAL_FAULT_MSG		0xC850 /* global fault msg */
+#define IXGBE_MDIO_GLOBAL_FAULT_MSG_HI_TMP	0x8007 /* high temp failure */
 #define IXGBE_MDIO_GLOBAL_INT_MASK		0xD400 /* Global int mask */
 /* autoneg vendor alarm int enable */
 #define IXGBE_MDIO_GLOBAL_AN_VEN_ALM_INT_EN	0x1000
@@ -1353,6 +1358,7 @@ struct ixgbe_thermal_sensor_data {
 #define IXGBE_MDIO_GLOBAL_VEN_ALM_INT_EN	0x1 /* vendor alarm int enable */
 #define IXGBE_MDIO_GLOBAL_STD_ALM2_INT		0x200 /* vendor alarm2 int mask */
 #define IXGBE_MDIO_GLOBAL_INT_HI_TEMP_EN	0x4000 /* int high temp enable */
+#define IXGBE_MDIO_GLOBAL_INT_DEV_FAULT_EN	0x0010 /*int dev fault enable */
 
 #define IXGBE_MDIO_PMA_PMD_SDA_SCL_ADDR	0xC30A /* PHY_XS SDA/SCL Addr Reg */
 #define IXGBE_MDIO_PMA_PMD_SDA_SCL_DATA	0xC30B /* PHY_XS SDA/SCL Data Reg */
@@ -2209,6 +2215,7 @@ enum {
 #define IXGBE_TSAUXC_EN_CLK   0x00000004
 #define IXGBE_TSAUXC_SYNCLK   0x00000008
 #define IXGBE_TSAUXC_SDP0_INT 0x00000040
+#define IXGBE_TSAUXC_DISABLE_SYSTIME	0x80000000
 
 #define IXGBE_TSYNCTXCTL_VALID		0x00000001 /* Tx timestamp valid */
 #define IXGBE_TSYNCTXCTL_ENABLED	0x00000010 /* Tx timestamping enabled */
@@ -2218,8 +2225,12 @@ enum {
 #define IXGBE_TSYNCRXCTL_TYPE_L2_V2	0x00
 #define IXGBE_TSYNCRXCTL_TYPE_L4_V1	0x02
 #define IXGBE_TSYNCRXCTL_TYPE_L2_L4_V2	0x04
+#define IXGBE_TSYNCRXCTL_TYPE_ALL	0x08
 #define IXGBE_TSYNCRXCTL_TYPE_EVENT_V2	0x0A
 #define IXGBE_TSYNCRXCTL_ENABLED	0x00000010 /* Rx Timestamping enabled */
+#define IXGBE_TSYNCRXCTL_TSIP_UT_EN	0x00800000 /* Rx Timestamp in Packet */
+
+#define IXGBE_TSIM_TXTS			0x00000002
 
 #define IXGBE_RXMTRL_V1_CTRLT_MASK	0x000000FF
 #define IXGBE_RXMTRL_V1_SYNC_MSG	0x00
@@ -2332,6 +2343,7 @@ enum {
 #define IXGBE_RXD_STAT_UDPV     0x400   /* Valid UDP checksum */
 #define IXGBE_RXD_STAT_DYNINT   0x800   /* Pkt caused INT via DYNINT */
 #define IXGBE_RXD_STAT_LLINT    0x800   /* Pkt caused Low Latency Interrupt */
+#define IXGBE_RXD_STAT_TSIP     0x08000 /* Time Stamp in packet buffer */
 #define IXGBE_RXD_STAT_TS       0x10000 /* Time Stamp */
 #define IXGBE_RXD_STAT_SECP     0x20000 /* Security Processing */
 #define IXGBE_RXD_STAT_LB       0x40000 /* Loopback Status */
@@ -2768,6 +2780,7 @@ struct ixgbe_adv_tx_context_desc {
 #define IXGBE_ADVTXD_TUCMD_L4T_UDP   0x00000000  /* L4 Packet TYPE of UDP */
 #define IXGBE_ADVTXD_TUCMD_L4T_TCP   0x00000800  /* L4 Packet TYPE of TCP */
 #define IXGBE_ADVTXD_TUCMD_L4T_SCTP  0x00001000  /* L4 Packet TYPE of SCTP */
+#define IXGBE_ADVTXD_TUCMD_L4T_RSV     0x00001800 /* RSV L4 Packet TYPE */
 #define IXGBE_ADVTXD_TUCMD_MKRREQ    0x00002000 /*Req requires Markers and CRC*/
 #define IXGBE_ADVTXD_POPTS_IPSEC      0x00000400 /* IPSec offload request */
 #define IXGBE_ADVTXD_TUCMD_IPSEC_TYPE_ESP 0x00002000 /* IPSec Type ESP */
@@ -3288,7 +3301,7 @@ struct ixgbe_mac_operations {
 	s32 (*enable_mc)(struct ixgbe_hw *);
 	s32 (*disable_mc)(struct ixgbe_hw *);
 	s32 (*clear_vfta)(struct ixgbe_hw *);
-	s32 (*set_vfta)(struct ixgbe_hw *, u32, u32, bool);
+	s32 (*set_vfta)(struct ixgbe_hw *, u32, u32, bool, bool);
 	s32 (*init_uta_tables)(struct ixgbe_hw *);
 	void (*set_mac_anti_spoofing)(struct ixgbe_hw *, bool, int);
 	void (*set_vlan_anti_spoofing)(struct ixgbe_hw *, bool, int);
@@ -3508,7 +3521,7 @@ struct ixgbe_info {
 
 #define IXGBE_FUSES0_GROUP(_i)		(0x11158 + ((_i) * 4))
 #define IXGBE_FUSES0_300MHZ		BIT(5)
-#define IXGBE_FUSES0_REV1		BIT(6)
+#define IXGBE_FUSES0_REV_MASK		(3 << 6)
 
 #define IXGBE_KRM_PORT_CAR_GEN_CTRL(P)	((P) ? 0x8010 : 0x4010)
 #define IXGBE_KRM_LINK_CTRL_1(P)	((P) ? 0x820C : 0x420C)
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
index c1d4584f6469..2358c1b7d586 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x540.c
@@ -57,8 +57,7 @@ s32 ixgbe_get_invariants_X540(struct ixgbe_hw *hw)
 	struct ixgbe_phy_info *phy = &hw->phy;
 
 	/* set_phy_power was set by default to NULL */
-	if (!ixgbe_mng_present(hw))
-		phy->ops.set_phy_power = ixgbe_set_copper_phy_power;
+	phy->ops.set_phy_power = ixgbe_set_copper_phy_power;
 
 	mac->mcft_size = IXGBE_X540_MC_TBL_SIZE;
 	mac->vft_size = IXGBE_X540_VFT_TBL_SIZE;
@@ -110,13 +109,14 @@ mac_reset_top:
 	ctrl |= IXGBE_READ_REG(hw, IXGBE_CTRL);
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, ctrl);
 	IXGBE_WRITE_FLUSH(hw);
+	usleep_range(1000, 1200);
 
 	/* Poll for reset bit to self-clear indicating reset is complete */
 	for (i = 0; i < 10; i++) {
-		udelay(1);
 		ctrl = IXGBE_READ_REG(hw, IXGBE_CTRL);
 		if (!(ctrl & IXGBE_CTRL_RST_MASK))
 			break;
+		udelay(1);
 	}
 
 	if (ctrl & IXGBE_CTRL_RST_MASK) {
@@ -154,12 +154,16 @@ mac_reset_top:
 
 	/* Add the SAN MAC address to the RAR only if it's a valid address */
 	if (is_valid_ether_addr(hw->mac.san_addr)) {
-		hw->mac.ops.set_rar(hw, hw->mac.num_rar_entries - 1,
-				    hw->mac.san_addr, 0, IXGBE_RAH_AV);
-
 		/* Save the SAN MAC RAR index */
 		hw->mac.san_mac_rar_index = hw->mac.num_rar_entries - 1;
 
+		hw->mac.ops.set_rar(hw, hw->mac.san_mac_rar_index,
+				    hw->mac.san_addr, 0, IXGBE_RAH_AV);
+
+		/* clear VMDq pool/queue selection for this RAR */
+		hw->mac.ops.clear_vmdq(hw, hw->mac.san_mac_rar_index,
+				       IXGBE_CLEAR_VMDQ_ALL);
+
 		/* Reserve the last RAR for the SAN MAC address */
 		hw->mac.num_rar_entries--;
 	}
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
index ebe0ac950b14..87aca3f7c3de 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_x550.c
@@ -26,6 +26,8 @@
 #include "ixgbe_common.h"
 #include "ixgbe_phy.h"
 
+static s32 ixgbe_setup_kr_speed_x550em(struct ixgbe_hw *, ixgbe_link_speed);
+
 static s32 ixgbe_get_invariants_X550_x(struct ixgbe_hw *hw)
 {
 	struct ixgbe_mac_info *mac = &hw->mac;
@@ -85,79 +87,6 @@ static s32 ixgbe_write_cs4227(struct ixgbe_hw *hw, u16 reg, u16 value)
 }
 
 /**
- * ixgbe_check_cs4227_reg - Perform diag on a CS4227 register
- * @hw: pointer to hardware structure
- * @reg: the register to check
- *
- * Performs a diagnostic on a register in the CS4227 chip. Returns an error
- * if it is not operating correctly.
- * This function assumes that the caller has acquired the proper semaphore.
- */
-static s32 ixgbe_check_cs4227_reg(struct ixgbe_hw *hw, u16 reg)
-{
-	s32 status;
-	u32 retry;
-	u16 reg_val;
-
-	reg_val = (IXGBE_CS4227_EDC_MODE_DIAG << 1) | 1;
-	status = ixgbe_write_cs4227(hw, reg, reg_val);
-	if (status)
-		return status;
-	for (retry = 0; retry < IXGBE_CS4227_RETRIES; retry++) {
-		msleep(IXGBE_CS4227_CHECK_DELAY);
-		reg_val = 0xFFFF;
-		ixgbe_read_cs4227(hw, reg, &reg_val);
-		if (!reg_val)
-			break;
-	}
-	if (reg_val) {
-		hw_err(hw, "CS4227 reg 0x%04X failed diagnostic\n", reg);
-		return status;
-	}
-
-	return 0;
-}
-
-/**
- * ixgbe_get_cs4227_status - Return CS4227 status
- * @hw: pointer to hardware structure
- *
- * Performs a diagnostic on the CS4227 chip. Returns an error if it is
- * not operating correctly.
- * This function assumes that the caller has acquired the proper semaphore.
- */
-static s32 ixgbe_get_cs4227_status(struct ixgbe_hw *hw)
-{
-	s32 status;
-	u16 value = 0;
-
-	/* Exit if the diagnostic has already been performed. */
-	status = ixgbe_read_cs4227(hw, IXGBE_CS4227_SCRATCH, &value);
-	if (status)
-		return status;
-	if (value == IXGBE_CS4227_RESET_COMPLETE)
-		return 0;
-
-	/* Check port 0. */
-	status = ixgbe_check_cs4227_reg(hw, IXGBE_CS4227_LINE_SPARE24_LSB);
-	if (status)
-		return status;
-
-	status = ixgbe_check_cs4227_reg(hw, IXGBE_CS4227_HOST_SPARE24_LSB);
-	if (status)
-		return status;
-
-	/* Check port 1. */
-	status = ixgbe_check_cs4227_reg(hw, IXGBE_CS4227_LINE_SPARE24_LSB +
-					(1 << 12));
-	if (status)
-		return status;
-
-	return ixgbe_check_cs4227_reg(hw, IXGBE_CS4227_HOST_SPARE24_LSB +
-				      (1 << 12));
-}
-
-/**
  * ixgbe_read_pe - Read register from port expander
  * @hw: pointer to hardware structure
  * @reg: register number to read
@@ -326,13 +255,6 @@ static void ixgbe_check_cs4227(struct ixgbe_hw *hw)
 		return;
 	}
 
-	/* Is the CS4227 working correctly? */
-	status = ixgbe_get_cs4227_status(hw);
-	if (status) {
-		hw_err(hw, "CS4227 status failed: %d", status);
-		goto out;
-	}
-
 	/* Record completion for next time. */
 	status = ixgbe_write_cs4227(hw, IXGBE_CS4227_SCRATCH,
 				    IXGBE_CS4227_RESET_COMPLETE);
@@ -1257,31 +1179,71 @@ ixgbe_setup_mac_link_sfp_x550em(struct ixgbe_hw *hw,
 	if (status)
 		return status;
 
-	/* Configure CS4227 LINE side to 10G SR. */
-	slice = IXGBE_CS4227_LINE_SPARE22_MSB + (hw->bus.lan_id << 12);
-	value = IXGBE_CS4227_SPEED_10G;
-	status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227, slice,
-						  value);
-
-	/* Configure CS4227 for HOST connection rate then type. */
-	slice = IXGBE_CS4227_HOST_SPARE22_MSB + (hw->bus.lan_id << 12);
-	value = speed & IXGBE_LINK_SPEED_10GB_FULL ?
-		IXGBE_CS4227_SPEED_10G : IXGBE_CS4227_SPEED_1G;
-	status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227, slice,
-						  value);
+	if (!(hw->phy.nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE)) {
+		/* Configure CS4227 LINE side to 10G SR. */
+		slice = IXGBE_CS4227_LINE_SPARE22_MSB + (hw->bus.lan_id << 12);
+		value = IXGBE_CS4227_SPEED_10G;
+		status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227,
+							  slice, value);
+		if (status)
+			goto i2c_err;
 
-	slice = IXGBE_CS4227_HOST_SPARE24_LSB + (hw->bus.lan_id << 12);
-	if (setup_linear)
-		value = (IXGBE_CS4227_EDC_MODE_CX1 << 1) | 1;
-	else
+		slice = IXGBE_CS4227_LINE_SPARE24_LSB + (hw->bus.lan_id << 12);
 		value = (IXGBE_CS4227_EDC_MODE_SR << 1) | 1;
-	status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227, slice,
-						  value);
+		status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227,
+							  slice, value);
+		if (status)
+			goto i2c_err;
+
+		/* Configure CS4227 for HOST connection rate then type. */
+		slice = IXGBE_CS4227_HOST_SPARE22_MSB + (hw->bus.lan_id << 12);
+		value = speed & IXGBE_LINK_SPEED_10GB_FULL ?
+			IXGBE_CS4227_SPEED_10G : IXGBE_CS4227_SPEED_1G;
+		status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227,
+							  slice, value);
+		if (status)
+			goto i2c_err;
 
-	/* If internal link mode is XFI, then setup XFI internal link. */
-	if (!(hw->phy.nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE))
+		slice = IXGBE_CS4227_HOST_SPARE24_LSB + (hw->bus.lan_id << 12);
+		if (setup_linear)
+			value = (IXGBE_CS4227_EDC_MODE_CX1 << 1) | 1;
+		else
+			value = (IXGBE_CS4227_EDC_MODE_SR << 1) | 1;
+		status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227,
+							  slice, value);
+		if (status)
+			goto i2c_err;
+
+		/* Setup XFI internal link. */
 		status = ixgbe_setup_ixfi_x550em(hw, &speed);
+		if (status) {
+			hw_dbg(hw, "setup_ixfi failed with %d\n", status);
+			return status;
+		}
+	} else {
+		/* Configure internal PHY for KR/KX. */
+		status = ixgbe_setup_kr_speed_x550em(hw, speed);
+		if (status) {
+			hw_dbg(hw, "setup_kr_speed failed with %d\n", status);
+			return status;
+		}
 
+		/* Configure CS4227 LINE side to proper mode. */
+		slice = IXGBE_CS4227_LINE_SPARE24_LSB + (hw->bus.lan_id << 12);
+		if (setup_linear)
+			value = (IXGBE_CS4227_EDC_MODE_CX1 << 1) | 1;
+		else
+			value = (IXGBE_CS4227_EDC_MODE_SR << 1) | 1;
+		status = ixgbe_write_i2c_combined_generic(hw, IXGBE_CS4227,
+							  slice, value);
+		if (status)
+			goto i2c_err;
+	}
+
+	return 0;
+
+i2c_err:
+	hw_dbg(hw, "combined i2c access failed with %d\n", status);
 	return status;
 }
 
@@ -1482,7 +1444,7 @@ static s32 ixgbe_get_lasi_ext_t_x550em(struct ixgbe_hw *hw, bool *lsc)
 				IXGBE_MDIO_GLOBAL_ALARM_1_INT)))
 		return status;
 
-	/* High temperature failure alarm triggered */
+	/* Global alarm triggered */
 	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_ALARM_1,
 				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
 				      &reg);
@@ -1496,6 +1458,21 @@ static s32 ixgbe_get_lasi_ext_t_x550em(struct ixgbe_hw *hw, bool *lsc)
 		ixgbe_set_copper_phy_power(hw, false);
 		return IXGBE_ERR_OVERTEMP;
 	}
+	if (reg & IXGBE_MDIO_GLOBAL_ALM_1_DEV_FAULT) {
+		/*  device fault alarm triggered */
+		status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_FAULT_MSG,
+					  IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
+					  &reg);
+		if (status)
+			return status;
+
+		/* if device fault was due to high temp alarm handle and exit */
+		if (reg == IXGBE_MDIO_GLOBAL_FAULT_MSG_HI_TMP) {
+			/* power down the PHY in case the PHY FW didn't */
+			ixgbe_set_copper_phy_power(hw, false);
+			return IXGBE_ERR_OVERTEMP;
+		}
+	}
 
 	/* Vendor alarm 2 triggered */
 	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_CHIP_STD_INT_FLAG,
@@ -1549,14 +1526,15 @@ static s32 ixgbe_enable_lasi_ext_t_x550em(struct ixgbe_hw *hw)
 	if (status)
 		return status;
 
-	/* Enables high temperature failure alarm */
+	/* Enable high temperature failure and global fault alarms */
 	status = hw->phy.ops.read_reg(hw, IXGBE_MDIO_GLOBAL_INT_MASK,
 				      IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
 				      &reg);
 	if (status)
 		return status;
 
-	reg |= IXGBE_MDIO_GLOBAL_INT_HI_TEMP_EN;
+	reg |= (IXGBE_MDIO_GLOBAL_INT_HI_TEMP_EN |
+		IXGBE_MDIO_GLOBAL_INT_DEV_FAULT_EN);
 
 	status = hw->phy.ops.write_reg(hw, IXGBE_MDIO_GLOBAL_INT_MASK,
 				       IXGBE_MDIO_VENDOR_SPECIFIC_1_DEV_TYPE,
@@ -1765,6 +1743,12 @@ static s32 ixgbe_setup_internal_phy_t_x550em(struct ixgbe_hw *hw)
 	if (hw->mac.ops.get_media_type(hw) != ixgbe_media_type_copper)
 		return IXGBE_ERR_CONFIG;
 
+	if (hw->phy.nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE) {
+		speed = IXGBE_LINK_SPEED_10GB_FULL |
+			IXGBE_LINK_SPEED_1GB_FULL;
+		return ixgbe_setup_kr_speed_x550em(hw, speed);
+	}
+
 	/* If link is not up, then there is no setup necessary so return  */
 	status = ixgbe_ext_phy_t_x550em_get_link(hw, &link_up);
 	if (status)
@@ -1873,10 +1857,6 @@ static s32 ixgbe_enter_lplu_t_x550em(struct ixgbe_hw *hw)
 	u32 save_autoneg;
 	bool link_up;
 
-	/* SW LPLU not required on later HW revisions. */
-	if (IXGBE_FUSES0_REV1 & IXGBE_READ_REG(hw, IXGBE_FUSES0_GROUP(0)))
-		return 0;
-
 	/* If blocked by MNG FW, then don't restart AN */
 	if (ixgbe_check_reset_blocked(hw))
 		return 0;
@@ -1969,7 +1949,6 @@ static s32 ixgbe_enter_lplu_t_x550em(struct ixgbe_hw *hw)
 static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 {
 	struct ixgbe_phy_info *phy = &hw->phy;
-	ixgbe_link_speed speed;
 	s32 ret_val;
 
 	hw->mac.ops.set_lan_id(hw);
@@ -1982,13 +1961,6 @@ static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 		 * to determine internal PHY mode.
 		 */
 		phy->nw_mng_if_sel = IXGBE_READ_REG(hw, IXGBE_NW_MNG_IF_SEL);
-
-		/* If internal PHY mode is KR, then initialize KR link */
-		if (phy->nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE) {
-			speed = IXGBE_LINK_SPEED_10GB_FULL |
-				IXGBE_LINK_SPEED_1GB_FULL;
-			ret_val = ixgbe_setup_kr_speed_x550em(hw, speed);
-		}
 	}
 
 	/* Identify the PHY or SFP module */
@@ -2020,18 +1992,13 @@ static s32 ixgbe_init_phy_ops_X550em(struct ixgbe_hw *hw)
 		/* If internal link mode is XFI, then setup iXFI internal link,
 		 * else setup KR now.
 		 */
-		if (!(phy->nw_mng_if_sel & IXGBE_NW_MNG_IF_SEL_INT_PHY_MODE)) {
-			phy->ops.setup_internal_link =
-					ixgbe_setup_internal_phy_t_x550em;
-		} else {
-			speed = IXGBE_LINK_SPEED_10GB_FULL |
-				IXGBE_LINK_SPEED_1GB_FULL;
-			ret_val = ixgbe_setup_kr_speed_x550em(hw, speed);
-		}
+		phy->ops.setup_internal_link =
+					      ixgbe_setup_internal_phy_t_x550em;
 
 		/* setup SW LPLU only for first revision */
-		if (!(IXGBE_FUSES0_REV1 & IXGBE_READ_REG(hw,
-							IXGBE_FUSES0_GROUP(0))))
+		if (hw->mac.type == ixgbe_mac_X550EM_x &&
+		    !(IXGBE_READ_REG(hw, IXGBE_FUSES0_GROUP(0)) &
+		      IXGBE_FUSES0_REV_MASK))
 			phy->ops.enter_lplu = ixgbe_enter_lplu_t_x550em;
 
 		phy->ops.handle_lasi = ixgbe_handle_lasi_ext_t_x550em;
@@ -2176,13 +2143,14 @@ mac_reset_top:
 	ctrl |= IXGBE_READ_REG(hw, IXGBE_CTRL);
 	IXGBE_WRITE_REG(hw, IXGBE_CTRL, ctrl);
 	IXGBE_WRITE_FLUSH(hw);
+	usleep_range(1000, 1200);
 
 	/* Poll for reset bit to self-clear meaning reset is complete */
 	for (i = 0; i < 10; i++) {
-		udelay(1);
 		ctrl = IXGBE_READ_REG(hw, IXGBE_CTRL);
 		if (!(ctrl & IXGBE_CTRL_RST_MASK))
 			break;
+		udelay(1);
 	}
 
 	if (ctrl & IXGBE_CTRL_RST_MASK) {
diff --git a/drivers/net/ethernet/intel/ixgbevf/ethtool.c b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
index d3e5f5b37999..c48aef613b0a 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
@@ -774,7 +774,7 @@ static int ixgbevf_set_coalesce(struct net_device *netdev,
 		adapter->tx_itr_setting = ec->tx_coalesce_usecs;
 
 	if (adapter->tx_itr_setting == 1)
-		tx_itr_param = IXGBE_10K_ITR;
+		tx_itr_param = IXGBE_12K_ITR;
 	else
 		tx_itr_param = adapter->tx_itr_setting;
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index ec3147279621..68ec7daa04fd 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -326,8 +326,7 @@ static inline bool ixgbevf_qv_disable(struct ixgbevf_q_vector *q_vector)
 #define IXGBE_MIN_RSC_ITR	24
 #define IXGBE_100K_ITR		40
 #define IXGBE_20K_ITR		200
-#define IXGBE_10K_ITR		400
-#define IXGBE_8K_ITR		500
+#define IXGBE_12K_ITR		336
 
 /* Helper macros to switch between ints/sec and what the register uses.
  * And yes, it's the same math going both ways.  The lowest value
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index 592ff237d692..3558f019b631 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -59,7 +59,7 @@ static const char ixgbevf_driver_string[] =
 #define DRV_VERSION "2.12.1-k"
 const char ixgbevf_driver_version[] = DRV_VERSION;
 static char ixgbevf_copyright[] =
-	"Copyright (c) 2009 - 2012 Intel Corporation.";
+	"Copyright (c) 2009 - 2015 Intel Corporation.";
 
 static const struct ixgbevf_info *ixgbevf_info_tbl[] = {
 	[board_82599_vf] = &ixgbevf_82599_vf_info,
@@ -96,12 +96,14 @@ static int debug = -1;
 module_param(debug, int, 0);
 MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
 
+static struct workqueue_struct *ixgbevf_wq;
+
 static void ixgbevf_service_event_schedule(struct ixgbevf_adapter *adapter)
 {
 	if (!test_bit(__IXGBEVF_DOWN, &adapter->state) &&
 	    !test_bit(__IXGBEVF_REMOVING, &adapter->state) &&
 	    !test_and_set_bit(__IXGBEVF_SERVICE_SCHED, &adapter->state))
-		schedule_work(&adapter->service_task);
+		queue_work(ixgbevf_wq, &adapter->service_task);
 }
 
 static void ixgbevf_service_event_complete(struct ixgbevf_adapter *adapter)
@@ -1014,6 +1016,8 @@ static int ixgbevf_poll(struct napi_struct *napi, int budget)
 	ixgbevf_for_each_ring(ring, q_vector->tx)
 		clean_complete &= ixgbevf_clean_tx_irq(q_vector, ring);
 
+	if (budget <= 0)
+		return budget;
 #ifdef CONFIG_NET_RX_BUSY_POLL
 	if (!ixgbevf_qv_lock_napi(q_vector))
 		return budget;
@@ -1043,7 +1047,7 @@ static int ixgbevf_poll(struct napi_struct *napi, int budget)
 		return budget;
 	/* all work done, exit the polling mode */
 	napi_complete_done(napi, work_done);
-	if (adapter->rx_itr_setting & 1)
+	if (adapter->rx_itr_setting == 1)
 		ixgbevf_set_itr(q_vector);
 	if (!test_bit(__IXGBEVF_DOWN, &adapter->state) &&
 	    !test_bit(__IXGBEVF_REMOVING, &adapter->state))
@@ -1138,7 +1142,7 @@ static void ixgbevf_configure_msix(struct ixgbevf_adapter *adapter)
 		if (q_vector->tx.ring && !q_vector->rx.ring) {
 			/* Tx only vector */
 			if (adapter->tx_itr_setting == 1)
-				q_vector->itr = IXGBE_10K_ITR;
+				q_vector->itr = IXGBE_12K_ITR;
 			else
 				q_vector->itr = adapter->tx_itr_setting;
 		} else {
@@ -1196,7 +1200,7 @@ static void ixgbevf_update_itr(struct ixgbevf_q_vector *q_vector,
 	/* simple throttle rate management
 	 *    0-20MB/s lowest (100000 ints/s)
 	 *   20-100MB/s low   (20000 ints/s)
-	 *  100-1249MB/s bulk (8000 ints/s)
+	 *  100-1249MB/s bulk (12000 ints/s)
 	 */
 	/* what was last interrupt timeslice? */
 	timepassed_us = q_vector->itr >> 2;
@@ -1246,8 +1250,9 @@ static void ixgbevf_set_itr(struct ixgbevf_q_vector *q_vector)
 		new_itr = IXGBE_20K_ITR;
 		break;
 	case bulk_latency:
+		new_itr = IXGBE_12K_ITR;
+		break;
 	default:
-		new_itr = IXGBE_8K_ITR;
 		break;
 	}
 
@@ -1288,7 +1293,7 @@ static irqreturn_t ixgbevf_msix_clean_rings(int irq, void *data)
 
 	/* EIAM disabled interrupts (on this vector) for us */
 	if (q_vector->rx.ring || q_vector->tx.ring)
-		napi_schedule(&q_vector->napi);
+		napi_schedule_irqoff(&q_vector->napi);
 
 	return IRQ_HANDLED;
 }
@@ -1332,7 +1337,6 @@ static int ixgbevf_map_rings_to_vectors(struct ixgbevf_adapter *adapter)
 	int txr_remaining = adapter->num_tx_queues;
 	int i, j;
 	int rqpv, tqpv;
-	int err = 0;
 
 	q_vectors = adapter->num_msix_vectors - NON_Q_VECTORS;
 
@@ -1345,7 +1349,7 @@ static int ixgbevf_map_rings_to_vectors(struct ixgbevf_adapter *adapter)
 
 		for (; txr_idx < txr_remaining; v_start++, txr_idx++)
 			map_vector_to_txq(adapter, v_start, txr_idx);
-		goto out;
+		return 0;
 	}
 
 	/* If we don't have enough vectors for a 1-to-1
@@ -1370,8 +1374,7 @@ static int ixgbevf_map_rings_to_vectors(struct ixgbevf_adapter *adapter)
 		}
 	}
 
-out:
-	return err;
+	return 0;
 }
 
 /**
@@ -1469,9 +1472,7 @@ static inline void ixgbevf_reset_q_vectors(struct ixgbevf_adapter *adapter)
  **/
 static int ixgbevf_request_irq(struct ixgbevf_adapter *adapter)
 {
-	int err = 0;
-
-	err = ixgbevf_request_msix_irqs(adapter);
+	int err = ixgbevf_request_msix_irqs(adapter);
 
 	if (err)
 		hw_dbg(&adapter->hw, "request_irq failed, Error %d\n", err);
@@ -1830,7 +1831,7 @@ static int ixgbevf_vlan_rx_kill_vid(struct net_device *netdev,
 {
 	struct ixgbevf_adapter *adapter = netdev_priv(netdev);
 	struct ixgbe_hw *hw = &adapter->hw;
-	int err = -EOPNOTSUPP;
+	int err;
 
 	spin_lock_bh(&adapter->mbx_lock);
 
@@ -2046,7 +2047,7 @@ static void ixgbevf_negotiate_api(struct ixgbevf_adapter *adapter)
 		      ixgbe_mbox_api_11,
 		      ixgbe_mbox_api_10,
 		      ixgbe_mbox_api_unknown };
-	int err = 0, idx = 0;
+	int err, idx = 0;
 
 	spin_lock_bh(&adapter->mbx_lock);
 
@@ -2260,10 +2261,8 @@ void ixgbevf_reset(struct ixgbevf_adapter *adapter)
 	}
 
 	if (is_valid_ether_addr(adapter->hw.mac.addr)) {
-		memcpy(netdev->dev_addr, adapter->hw.mac.addr,
-		       netdev->addr_len);
-		memcpy(netdev->perm_addr, adapter->hw.mac.addr,
-		       netdev->addr_len);
+		ether_addr_copy(netdev->dev_addr, adapter->hw.mac.addr);
+		ether_addr_copy(netdev->perm_addr, adapter->hw.mac.addr);
 	}
 
 	adapter->last_reset = jiffies;
@@ -2421,7 +2420,7 @@ err_allocation:
 static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 {
 	struct net_device *netdev = adapter->netdev;
-	int err = 0;
+	int err;
 	int vector, v_budget;
 
 	/* It's easy to be greedy for MSI-X vectors, but it really
@@ -2439,26 +2438,21 @@ static int ixgbevf_set_interrupt_capability(struct ixgbevf_adapter *adapter)
 	 */
 	adapter->msix_entries = kcalloc(v_budget,
 					sizeof(struct msix_entry), GFP_KERNEL);
-	if (!adapter->msix_entries) {
-		err = -ENOMEM;
-		goto out;
-	}
+	if (!adapter->msix_entries)
+		return -ENOMEM;
 
 	for (vector = 0; vector < v_budget; vector++)
 		adapter->msix_entries[vector].entry = vector;
 
 	err = ixgbevf_acquire_msix_vectors(adapter, v_budget);
 	if (err)
-		goto out;
+		return err;
 
 	err = netif_set_real_num_tx_queues(netdev, adapter->num_tx_queues);
 	if (err)
-		goto out;
-
-	err = netif_set_real_num_rx_queues(netdev, adapter->num_rx_queues);
+		return err;
 
-out:
-	return err;
+	return netif_set_real_num_rx_queues(netdev, adapter->num_rx_queues);
 }
 
 /**
@@ -2483,9 +2477,6 @@ static int ixgbevf_alloc_q_vectors(struct ixgbevf_adapter *adapter)
 		q_vector->v_idx = q_idx;
 		netif_napi_add(adapter->netdev, &q_vector->napi,
 			       ixgbevf_poll, 64);
-#ifdef CONFIG_NET_RX_BUSY_POLL
-		napi_hash_add(&q_vector->napi);
-#endif
 		adapter->q_vector[q_idx] = q_vector;
 	}
 
@@ -2662,13 +2653,14 @@ static int ixgbevf_sw_init(struct ixgbevf_adapter *adapter)
 		else if (is_zero_ether_addr(adapter->hw.mac.addr))
 			dev_info(&pdev->dev,
 				 "MAC address not assigned by administrator.\n");
-		memcpy(netdev->dev_addr, hw->mac.addr, netdev->addr_len);
+		ether_addr_copy(netdev->dev_addr, hw->mac.addr);
 	}
 
 	if (!is_valid_ether_addr(netdev->dev_addr)) {
 		dev_info(&pdev->dev, "Assigning random MAC address\n");
 		eth_hw_addr_random(netdev);
-		memcpy(hw->mac.addr, netdev->dev_addr, netdev->addr_len);
+		ether_addr_copy(hw->mac.addr, netdev->dev_addr);
+		ether_addr_copy(hw->mac.perm_addr, netdev->dev_addr);
 	}
 
 	/* Enable dynamic interrupt throttling rates */
@@ -3355,6 +3347,7 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		u8 l4_hdr = 0;
+		__be16 frag_off;
 
 		switch (first->protocol) {
 		case htons(ETH_P_IP):
@@ -3365,13 +3358,16 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 		case htons(ETH_P_IPV6):
 			vlan_macip_lens |= skb_network_header_len(skb);
 			l4_hdr = ipv6_hdr(skb)->nexthdr;
+			if (likely(skb_network_header_len(skb) ==
+				   sizeof(struct ipv6hdr)))
+				break;
+			ipv6_skip_exthdr(skb, skb_network_offset(skb) +
+					      sizeof(struct ipv6hdr),
+					 &l4_hdr, &frag_off);
+			if (unlikely(frag_off))
+				l4_hdr = NEXTHDR_FRAGMENT;
 			break;
 		default:
-			if (unlikely(net_ratelimit())) {
-				dev_warn(tx_ring->dev,
-					 "partial checksum but proto=%x!\n",
-					 first->protocol);
-			}
 			break;
 		}
 
@@ -3393,16 +3389,18 @@ static void ixgbevf_tx_csum(struct ixgbevf_ring *tx_ring,
 		default:
 			if (unlikely(net_ratelimit())) {
 				dev_warn(tx_ring->dev,
-					 "partial checksum but l4 proto=%x!\n",
-					 l4_hdr);
+					 "partial checksum, l3 proto=%x, l4 proto=%x\n",
+					 first->protocol, l4_hdr);
 			}
-			break;
+			skb_checksum_help(skb);
+			goto no_csum;
 		}
 
 		/* update TX checksum flag */
 		first->tx_flags |= IXGBE_TX_FLAGS_CSUM;
 	}
 
+no_csum:
 	/* vlan_macip_lens: MACLEN, VLAN tag */
 	vlan_macip_lens |= skb_network_offset(skb) << IXGBE_ADVTXD_MACLEN_SHIFT;
 	vlan_macip_lens |= first->tx_flags & IXGBE_TX_FLAGS_VLAN_MASK;
@@ -3698,8 +3696,8 @@ static int ixgbevf_set_mac(struct net_device *netdev, void *p)
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
-	memcpy(hw->mac.addr, addr->sa_data, netdev->addr_len);
+	ether_addr_copy(netdev->dev_addr, addr->sa_data);
+	ether_addr_copy(hw->mac.addr, addr->sa_data);
 
 	spin_lock_bh(&adapter->mbx_lock);
 
@@ -4248,15 +4246,17 @@ static struct pci_driver ixgbevf_driver = {
  **/
 static int __init ixgbevf_init_module(void)
 {
-	int ret;
-
 	pr_info("%s - version %s\n", ixgbevf_driver_string,
 		ixgbevf_driver_version);
 
 	pr_info("%s\n", ixgbevf_copyright);
+	ixgbevf_wq = create_singlethread_workqueue(ixgbevf_driver_name);
+	if (!ixgbevf_wq) {
+		pr_err("%s: Failed to create workqueue\n", ixgbevf_driver_name);
+		return -ENOMEM;
+	}
 
-	ret = pci_register_driver(&ixgbevf_driver);
-	return ret;
+	return pci_register_driver(&ixgbevf_driver);
 }
 
 module_init(ixgbevf_init_module);
@@ -4270,6 +4270,10 @@ module_init(ixgbevf_init_module);
 static void __exit ixgbevf_exit_module(void)
 {
 	pci_unregister_driver(&ixgbevf_driver);
+	if (ixgbevf_wq) {
+		destroy_workqueue(ixgbevf_wq);
+		ixgbevf_wq = NULL;
+	}
 }
 
 #ifdef DEBUG
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index 427f3605cbfc..61a98f4c5746 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -117,7 +117,9 @@ static s32 ixgbevf_reset_hw_vf(struct ixgbe_hw *hw)
 	    msgbuf[0] != (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_NACK))
 		return IXGBE_ERR_INVALID_MAC_ADDR;
 
-	ether_addr_copy(hw->mac.perm_addr, addr);
+	if (msgbuf[0] == (IXGBE_VF_RESET | IXGBE_VT_MSGTYPE_ACK))
+		ether_addr_copy(hw->mac.perm_addr, addr);
+
 	hw->mac.mc_filter_type = msgbuf[IXGBE_VF_MC_TYPE_WORD];
 
 	return 0;
diff --git a/drivers/net/ethernet/jme.c b/drivers/net/ethernet/jme.c
index 060dd3922974..3ddf657bc10b 100644
--- a/drivers/net/ethernet/jme.c
+++ b/drivers/net/ethernet/jme.c
@@ -270,11 +270,17 @@ jme_reset_mac_processor(struct jme_adapter *jme)
 }
 
 static inline void
-jme_clear_pm(struct jme_adapter *jme)
+jme_clear_pm_enable_wol(struct jme_adapter *jme)
 {
 	jwrite32(jme, JME_PMCS, PMCS_STMASK | jme->reg_pmcs);
 }
 
+static inline void
+jme_clear_pm_disable_wol(struct jme_adapter *jme)
+{
+	jwrite32(jme, JME_PMCS, PMCS_STMASK);
+}
+
 static int
 jme_reload_eeprom(struct jme_adapter *jme)
 {
@@ -1853,7 +1859,7 @@ jme_open(struct net_device *netdev)
 	struct jme_adapter *jme = netdev_priv(netdev);
 	int rc;
 
-	jme_clear_pm(jme);
+	jme_clear_pm_disable_wol(jme);
 	JME_NAPI_ENABLE(jme);
 
 	tasklet_init(&jme->linkch_task, jme_link_change_tasklet,
@@ -1925,11 +1931,11 @@ jme_wait_link(struct jme_adapter *jme)
 static void
 jme_powersave_phy(struct jme_adapter *jme)
 {
-	if (jme->reg_pmcs) {
+	if (jme->reg_pmcs && device_may_wakeup(&jme->pdev->dev)) {
 		jme_set_100m_half(jme);
 		if (jme->reg_pmcs & (PMCS_LFEN | PMCS_LREN))
 			jme_wait_link(jme);
-		jme_clear_pm(jme);
+		jme_clear_pm_enable_wol(jme);
 	} else {
 		jme_phy_off(jme);
 	}
@@ -2646,9 +2652,6 @@ jme_set_wol(struct net_device *netdev,
 	if (wol->wolopts & WAKE_MAGIC)
 		jme->reg_pmcs |= PMCS_MFEN;
 
-	jwrite32(jme, JME_PMCS, jme->reg_pmcs);
-	device_set_wakeup_enable(&jme->pdev->dev, !!(jme->reg_pmcs));
-
 	return 0;
 }
 
@@ -2753,7 +2756,7 @@ static netdev_features_t
 jme_fix_features(struct net_device *netdev, netdev_features_t features)
 {
 	if (netdev->mtu > 1900)
-		features &= ~(NETIF_F_ALL_TSO | NETIF_F_ALL_CSUM);
+		features &= ~(NETIF_F_ALL_TSO | NETIF_F_CSUM_MASK);
 	return features;
 }
 
@@ -3172,8 +3175,8 @@ jme_init_one(struct pci_dev *pdev,
 	jme->mii_if.mdio_read = jme_mdio_read;
 	jme->mii_if.mdio_write = jme_mdio_write;
 
-	jme_clear_pm(jme);
-	device_set_wakeup_enable(&pdev->dev, true);
+	jme_clear_pm_disable_wol(jme);
+	device_init_wakeup(&pdev->dev, true);
 
 	jme_set_phyfifo_5level(jme);
 	jme->pcirev = pdev->revision;
@@ -3304,7 +3307,7 @@ jme_resume(struct device *dev)
 	if (!netif_running(netdev))
 		return 0;
 
-	jme_clear_pm(jme);
+	jme_clear_pm_disable_wol(jme);
 	jme_phy_on(jme);
 	if (test_bit(JME_FLAG_SSET, &jme->flags))
 		jme_set_settings(netdev, &jme->old_ecmd);
@@ -3312,13 +3315,14 @@ jme_resume(struct device *dev)
 		jme_reset_phy_processor(jme);
 	jme_phy_calibration(jme);
 	jme_phy_setEA(jme);
-	jme_start_irq(jme);
 	netif_device_attach(netdev);
 
 	atomic_inc(&jme->link_changing);
 
 	jme_reset_link(jme);
 
+	jme_start_irq(jme);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/lantiq_etop.c b/drivers/net/ethernet/lantiq_etop.c
index 581928c068f2..b630ef1e9646 100644
--- a/drivers/net/ethernet/lantiq_etop.c
+++ b/drivers/net/ethernet/lantiq_etop.c
@@ -375,22 +375,16 @@ static int
 ltq_etop_mdio_probe(struct net_device *dev)
 {
 	struct ltq_etop_priv *priv = netdev_priv(dev);
-	struct phy_device *phydev = NULL;
-	int phy_addr;
+	struct phy_device *phydev;
 
-	for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
-		if (priv->mii_bus->phy_map[phy_addr]) {
-			phydev = priv->mii_bus->phy_map[phy_addr];
-			break;
-		}
-	}
+	phydev = phy_find_first(priv->mii_bus);
 
 	if (!phydev) {
 		netdev_err(dev, "no PHY found\n");
 		return -ENODEV;
 	}
 
-	phydev = phy_connect(dev, dev_name(&phydev->dev),
+	phydev = phy_connect(dev, phydev_name(phydev),
 			     &ltq_etop_mdio_link, priv->pldata->mii_mode);
 
 	if (IS_ERR(phydev)) {
@@ -408,9 +402,7 @@ ltq_etop_mdio_probe(struct net_device *dev)
 
 	phydev->advertising = phydev->supported;
 	priv->phydev = phydev;
-	pr_info("%s: attached PHY [%s] (phy_addr=%s, irq=%d)\n",
-	       dev->name, phydev->drv->name,
-	       dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
 
 	return 0;
 }
@@ -435,18 +427,9 @@ ltq_etop_mdio_init(struct net_device *dev)
 	priv->mii_bus->name = "ltq_mii";
 	snprintf(priv->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		priv->pdev->name, priv->pdev->id);
-	priv->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!priv->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; ++i)
-		priv->mii_bus->irq[i] = PHY_POLL;
-
 	if (mdiobus_register(priv->mii_bus)) {
 		err = -ENXIO;
-		goto err_out_free_mdio_irq;
+		goto err_out_free_mdiobus;
 	}
 
 	if (ltq_etop_mdio_probe(dev)) {
@@ -457,8 +440,6 @@ ltq_etop_mdio_init(struct net_device *dev)
 
 err_out_unregister_bus:
 	mdiobus_unregister(priv->mii_bus);
-err_out_free_mdio_irq:
-	kfree(priv->mii_bus->irq);
 err_out_free_mdiobus:
 	mdiobus_free(priv->mii_bus);
 err_out:
@@ -472,7 +453,6 @@ ltq_etop_mdio_cleanup(struct net_device *dev)
 
 	phy_disconnect(priv->phydev);
 	mdiobus_unregister(priv->mii_bus);
-	kfree(priv->mii_bus->irq);
 	mdiobus_free(priv->mii_bus);
 }
 
diff --git a/drivers/net/ethernet/marvell/mv643xx_eth.c b/drivers/net/ethernet/marvell/mv643xx_eth.c
index 4182290fdbcf..55831188bc32 100644
--- a/drivers/net/ethernet/marvell/mv643xx_eth.c
+++ b/drivers/net/ethernet/marvell/mv643xx_eth.c
@@ -762,10 +762,10 @@ txq_put_data_tso(struct net_device *dev, struct tx_queue *txq,
 
 	if (length <= 8 && (uintptr_t)data & 0x7) {
 		/* Copy unaligned small data fragment to TSO header data area */
-		memcpy(txq->tso_hdrs + txq->tx_curr_desc * TSO_HEADER_SIZE,
+		memcpy(txq->tso_hdrs + tx_index * TSO_HEADER_SIZE,
 		       data, length);
 		desc->buf_ptr = txq->tso_hdrs_dma
-			+ txq->tx_curr_desc * TSO_HEADER_SIZE;
+			+ tx_index * TSO_HEADER_SIZE;
 	} else {
 		/* Alignment is okay, map buffer and hand off to hardware */
 		txq->tx_desc_mapping[tx_index] = DESC_DMA_MAP_SINGLE;
@@ -3133,7 +3133,7 @@ static int mv643xx_eth_probe(struct platform_device *pdev)
 		if (!mp->phy)
 			err = -ENODEV;
 		else
-			phy_addr_set(mp, mp->phy->addr);
+			phy_addr_set(mp, mp->phy->mdio.addr);
 	} else if (pd->phy_addr != MV643XX_ETH_PHY_NONE) {
 		mp->phy = phy_scan(mp, pd->phy_addr);
 
@@ -3257,25 +3257,20 @@ static struct platform_driver mv643xx_eth_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&mv643xx_eth_shared_driver,
+	&mv643xx_eth_driver,
+};
+
 static int __init mv643xx_eth_init_module(void)
 {
-	int rc;
-
-	rc = platform_driver_register(&mv643xx_eth_shared_driver);
-	if (!rc) {
-		rc = platform_driver_register(&mv643xx_eth_driver);
-		if (rc)
-			platform_driver_unregister(&mv643xx_eth_shared_driver);
-	}
-
-	return rc;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_init(mv643xx_eth_init_module);
 
 static void __exit mv643xx_eth_cleanup_module(void)
 {
-	platform_driver_unregister(&mv643xx_eth_driver);
-	platform_driver_unregister(&mv643xx_eth_shared_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(mv643xx_eth_cleanup_module);
 
diff --git a/drivers/net/ethernet/marvell/mvmdio.c b/drivers/net/ethernet/marvell/mvmdio.c
index fc2fb25343f4..8982c882af1b 100644
--- a/drivers/net/ethernet/marvell/mvmdio.c
+++ b/drivers/net/ethernet/marvell/mvmdio.c
@@ -187,7 +187,7 @@ static int orion_mdio_probe(struct platform_device *pdev)
 	struct resource *r;
 	struct mii_bus *bus;
 	struct orion_mdio_dev *dev;
-	int i, ret;
+	int ret;
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!r) {
@@ -207,14 +207,6 @@ static int orion_mdio_probe(struct platform_device *pdev)
 		 dev_name(&pdev->dev));
 	bus->parent = &pdev->dev;
 
-	bus->irq = devm_kmalloc_array(&pdev->dev, PHY_MAX_ADDR, sizeof(int),
-				      GFP_KERNEL);
-	if (!bus->irq)
-		return -ENOMEM;
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		bus->irq[i] = PHY_POLL;
-
 	dev = bus->priv;
 	dev->regs = devm_ioremap(&pdev->dev, r->start, resource_size(r));
 	if (!dev->regs) {
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index ed622fa29dfa..b0ae69f84493 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -11,28 +11,28 @@
  * warranty of any kind, whether express or implied.
  */
 
-#include <linux/kernel.h>
-#include <linux/netdevice.h>
+#include <linux/clk.h>
+#include <linux/cpu.h>
 #include <linux/etherdevice.h>
-#include <linux/platform_device.h>
-#include <linux/skbuff.h>
+#include <linux/if_vlan.h>
 #include <linux/inetdevice.h>
-#include <linux/mbus.h>
-#include <linux/module.h>
 #include <linux/interrupt.h>
-#include <linux/if_vlan.h>
-#include <net/ip.h>
-#include <net/ipv6.h>
 #include <linux/io.h>
-#include <net/tso.h>
+#include <linux/kernel.h>
+#include <linux/mbus.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_mdio.h>
 #include <linux/of_net.h>
-#include <linux/of_address.h>
 #include <linux/phy.h>
-#include <linux/clk.h>
-#include <linux/cpu.h>
+#include <linux/platform_device.h>
+#include <linux/skbuff.h>
+#include <net/ip.h>
+#include <net/ipv6.h>
+#include <net/tso.h>
 
 /* Registers */
 #define MVNETA_RXQ_CONFIG_REG(q)                (0x1400 + ((q) << 2))
@@ -110,9 +110,17 @@
 #define MVNETA_CPU_MAP(cpu)                      (0x2540 + ((cpu) << 2))
 #define      MVNETA_CPU_RXQ_ACCESS_ALL_MASK      0x000000ff
 #define      MVNETA_CPU_TXQ_ACCESS_ALL_MASK      0x0000ff00
+#define      MVNETA_CPU_RXQ_ACCESS(rxq)		 BIT(rxq)
+#define      MVNETA_CPU_TXQ_ACCESS(txq)		 BIT(txq + 8)
 #define MVNETA_RXQ_TIME_COAL_REG(q)              (0x2580 + ((q) << 2))
 
-/* Exception Interrupt Port/Queue Cause register */
+/* Exception Interrupt Port/Queue Cause register
+ *
+ * Their behavior depend of the mapping done using the PCPX2Q
+ * registers. For a given CPU if the bit associated to a queue is not
+ * set, then for the register a read from this CPU will always return
+ * 0 and a write won't do anything
+ */
 
 #define MVNETA_INTR_NEW_CAUSE                    0x25a0
 #define MVNETA_INTR_NEW_MASK                     0x25a4
@@ -254,6 +262,11 @@
 
 #define MVNETA_TX_MTU_MAX		0x3ffff
 
+/* The RSS lookup table actually has 256 entries but we do not use
+ * them yet
+ */
+#define MVNETA_RSS_LU_TABLE_SIZE	1
+
 /* TSO header size */
 #define TSO_HEADER_SIZE 128
 
@@ -356,9 +369,17 @@ struct mvneta_port {
 	struct mvneta_tx_queue *txqs;
 	struct net_device *dev;
 	struct notifier_block cpu_notifier;
+	int rxq_def;
+	/* Protect the access to the percpu interrupt registers,
+	 * ensuring that the configuration remains coherent.
+	 */
+	spinlock_t lock;
+	bool is_stopped;
 
 	/* Core clock */
 	struct clk *clk;
+	/* AXI clock */
+	struct clk *clk_bus;
 	u8 mcast_count[256];
 	u16 tx_ring_size;
 	u16 rx_ring_size;
@@ -371,9 +392,11 @@ struct mvneta_port {
 	unsigned int duplex;
 	unsigned int speed;
 	unsigned int tx_csum_limit;
-	int use_inband_status:1;
+	unsigned int use_inband_status:1;
 
 	u64 ethtool_stats[ARRAY_SIZE(mvneta_statistics)];
+
+	u32 indir[MVNETA_RSS_LU_TABLE_SIZE];
 };
 
 /* The mvneta_tx_desc and mvneta_rx_desc structures describe the
@@ -499,6 +522,9 @@ struct mvneta_tx_queue {
 
 	/* DMA address of TSO headers */
 	dma_addr_t tso_hdrs_phys;
+
+	/* Affinity mask for CPUs*/
+	cpumask_t affinity_mask;
 };
 
 struct mvneta_rx_queue {
@@ -819,7 +845,13 @@ static void mvneta_port_up(struct mvneta_port *pp)
 	mvreg_write(pp, MVNETA_TXQ_CMD, q_map);
 
 	/* Enable all initialized RXQs. */
-	mvreg_write(pp, MVNETA_RXQ_CMD, BIT(rxq_def));
+	for (queue = 0; queue < rxq_number; queue++) {
+		struct mvneta_rx_queue *rxq = &pp->rxqs[queue];
+
+		if (rxq->descs != NULL)
+			q_map |= (1 << queue);
+	}
+	mvreg_write(pp, MVNETA_RXQ_CMD, q_map);
 }
 
 /* Stop the Ethernet port activity */
@@ -973,6 +1005,81 @@ static void mvneta_set_other_mcast_table(struct mvneta_port *pp, int queue)
 		mvreg_write(pp, MVNETA_DA_FILT_OTH_MCAST + offset, val);
 }
 
+static void mvneta_set_autoneg(struct mvneta_port *pp, int enable)
+{
+	u32 val;
+
+	if (enable) {
+		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
+		val &= ~(MVNETA_GMAC_FORCE_LINK_PASS |
+			 MVNETA_GMAC_FORCE_LINK_DOWN |
+			 MVNETA_GMAC_AN_FLOW_CTRL_EN);
+		val |= MVNETA_GMAC_INBAND_AN_ENABLE |
+		       MVNETA_GMAC_AN_SPEED_EN |
+		       MVNETA_GMAC_AN_DUPLEX_EN;
+		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
+
+		val = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
+		val |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, val);
+
+		val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
+		val |= MVNETA_GMAC2_INBAND_AN_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
+	} else {
+		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
+		val &= ~(MVNETA_GMAC_INBAND_AN_ENABLE |
+		       MVNETA_GMAC_AN_SPEED_EN |
+		       MVNETA_GMAC_AN_DUPLEX_EN);
+		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
+
+		val = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
+		val &= ~MVNETA_GMAC_1MS_CLOCK_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, val);
+
+		val = mvreg_read(pp, MVNETA_GMAC_CTRL_2);
+		val &= ~MVNETA_GMAC2_INBAND_AN_ENABLE;
+		mvreg_write(pp, MVNETA_GMAC_CTRL_2, val);
+	}
+}
+
+static void mvneta_percpu_unmask_interrupt(void *arg)
+{
+	struct mvneta_port *pp = arg;
+
+	/* All the queue are unmasked, but actually only the ones
+	 * mapped to this CPU will be unmasked
+	 */
+	mvreg_write(pp, MVNETA_INTR_NEW_MASK,
+		    MVNETA_RX_INTR_MASK_ALL |
+		    MVNETA_TX_INTR_MASK_ALL |
+		    MVNETA_MISCINTR_INTR_MASK);
+}
+
+static void mvneta_percpu_mask_interrupt(void *arg)
+{
+	struct mvneta_port *pp = arg;
+
+	/* All the queue are masked, but actually only the ones
+	 * mapped to this CPU will be masked
+	 */
+	mvreg_write(pp, MVNETA_INTR_NEW_MASK, 0);
+	mvreg_write(pp, MVNETA_INTR_OLD_MASK, 0);
+	mvreg_write(pp, MVNETA_INTR_MISC_MASK, 0);
+}
+
+static void mvneta_percpu_clear_intr_cause(void *arg)
+{
+	struct mvneta_port *pp = arg;
+
+	/* All the queue are cleared, but actually only the ones
+	 * mapped to this CPU will be cleared
+	 */
+	mvreg_write(pp, MVNETA_INTR_NEW_CAUSE, 0);
+	mvreg_write(pp, MVNETA_INTR_MISC_CAUSE, 0);
+	mvreg_write(pp, MVNETA_INTR_OLD_CAUSE, 0);
+}
+
 /* This method sets defaults to the NETA port:
  *	Clears interrupt Cause and Mask registers.
  *	Clears all MAC tables.
@@ -987,28 +1094,45 @@ static void mvneta_defaults_set(struct mvneta_port *pp)
 	int cpu;
 	int queue;
 	u32 val;
+	int max_cpu = num_present_cpus();
 
 	/* Clear all Cause registers */
-	mvreg_write(pp, MVNETA_INTR_NEW_CAUSE, 0);
-	mvreg_write(pp, MVNETA_INTR_OLD_CAUSE, 0);
-	mvreg_write(pp, MVNETA_INTR_MISC_CAUSE, 0);
+	on_each_cpu(mvneta_percpu_clear_intr_cause, pp, true);
 
 	/* Mask all interrupts */
-	mvreg_write(pp, MVNETA_INTR_NEW_MASK, 0);
-	mvreg_write(pp, MVNETA_INTR_OLD_MASK, 0);
-	mvreg_write(pp, MVNETA_INTR_MISC_MASK, 0);
+	on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
 	mvreg_write(pp, MVNETA_INTR_ENABLE, 0);
 
 	/* Enable MBUS Retry bit16 */
 	mvreg_write(pp, MVNETA_MBUS_RETRY, 0x20);
 
-	/* Set CPU queue access map - all CPUs have access to all RX
-	 * queues and to all TX queues
+	/* Set CPU queue access map. CPUs are assigned to the RX and
+	 * TX queues modulo their number. If there is only one TX
+	 * queue then it is assigned to the CPU associated to the
+	 * default RX queue.
 	 */
-	for_each_present_cpu(cpu)
-		mvreg_write(pp, MVNETA_CPU_MAP(cpu),
-			    (MVNETA_CPU_RXQ_ACCESS_ALL_MASK |
-			     MVNETA_CPU_TXQ_ACCESS_ALL_MASK));
+	for_each_present_cpu(cpu) {
+		int rxq_map = 0, txq_map = 0;
+		int rxq, txq;
+
+		for (rxq = 0; rxq < rxq_number; rxq++)
+			if ((rxq % max_cpu) == cpu)
+				rxq_map |= MVNETA_CPU_RXQ_ACCESS(rxq);
+
+		for (txq = 0; txq < txq_number; txq++)
+			if ((txq % max_cpu) == cpu)
+				txq_map |= MVNETA_CPU_TXQ_ACCESS(txq);
+
+		/* With only one TX queue we configure a special case
+		 * which will allow to get all the irq on a single
+		 * CPU
+		 */
+		if (txq_number == 1)
+			txq_map = (cpu == pp->rxq_def) ?
+				MVNETA_CPU_TXQ_ACCESS(1) : 0;
+
+		mvreg_write(pp, MVNETA_CPU_MAP(cpu), rxq_map | txq_map);
+	}
 
 	/* Reset RX and TX DMAs */
 	mvreg_write(pp, MVNETA_PORT_RX_RESET, MVNETA_PORT_RX_DMA_RESET);
@@ -1029,7 +1153,7 @@ static void mvneta_defaults_set(struct mvneta_port *pp)
 	mvreg_write(pp, MVNETA_ACC_MODE, val);
 
 	/* Update val of portCfg register accordingly with all RxQueue types */
-	val = MVNETA_PORT_CONFIG_DEFL_VALUE(rxq_def);
+	val = MVNETA_PORT_CONFIG_DEFL_VALUE(pp->rxq_def);
 	mvreg_write(pp, MVNETA_PORT_CONFIG, val);
 
 	val = 0;
@@ -1058,26 +1182,7 @@ static void mvneta_defaults_set(struct mvneta_port *pp)
 	val &= ~MVNETA_PHY_POLLING_ENABLE;
 	mvreg_write(pp, MVNETA_UNIT_CONTROL, val);
 
-	if (pp->use_inband_status) {
-		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-		val &= ~(MVNETA_GMAC_FORCE_LINK_PASS |
-			 MVNETA_GMAC_FORCE_LINK_DOWN |
-			 MVNETA_GMAC_AN_FLOW_CTRL_EN);
-		val |= MVNETA_GMAC_INBAND_AN_ENABLE |
-		       MVNETA_GMAC_AN_SPEED_EN |
-		       MVNETA_GMAC_AN_DUPLEX_EN;
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
-		val = mvreg_read(pp, MVNETA_GMAC_CLOCK_DIVIDER);
-		val |= MVNETA_GMAC_1MS_CLOCK_ENABLE;
-		mvreg_write(pp, MVNETA_GMAC_CLOCK_DIVIDER, val);
-	} else {
-		val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
-		val &= ~(MVNETA_GMAC_INBAND_AN_ENABLE |
-		       MVNETA_GMAC_AN_SPEED_EN |
-		       MVNETA_GMAC_AN_DUPLEX_EN);
-		mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
-	}
-
+	mvneta_set_autoneg(pp, pp->use_inband_status);
 	mvneta_set_ucast_table(pp, -1);
 	mvneta_set_special_mcast_table(pp, -1);
 	mvneta_set_other_mcast_table(pp, -1);
@@ -2082,19 +2187,19 @@ static void mvneta_set_rx_mode(struct net_device *dev)
 	if (dev->flags & IFF_PROMISC) {
 		/* Accept all: Multicast + Unicast */
 		mvneta_rx_unicast_promisc_set(pp, 1);
-		mvneta_set_ucast_table(pp, rxq_def);
-		mvneta_set_special_mcast_table(pp, rxq_def);
-		mvneta_set_other_mcast_table(pp, rxq_def);
+		mvneta_set_ucast_table(pp, pp->rxq_def);
+		mvneta_set_special_mcast_table(pp, pp->rxq_def);
+		mvneta_set_other_mcast_table(pp, pp->rxq_def);
 	} else {
 		/* Accept single Unicast */
 		mvneta_rx_unicast_promisc_set(pp, 0);
 		mvneta_set_ucast_table(pp, -1);
-		mvneta_mac_addr_set(pp, dev->dev_addr, rxq_def);
+		mvneta_mac_addr_set(pp, dev->dev_addr, pp->rxq_def);
 
 		if (dev->flags & IFF_ALLMULTI) {
 			/* Accept all multicast */
-			mvneta_set_special_mcast_table(pp, rxq_def);
-			mvneta_set_other_mcast_table(pp, rxq_def);
+			mvneta_set_special_mcast_table(pp, pp->rxq_def);
+			mvneta_set_other_mcast_table(pp, pp->rxq_def);
 		} else {
 			/* Accept only initialized multicast */
 			mvneta_set_special_mcast_table(pp, -1);
@@ -2103,7 +2208,7 @@ static void mvneta_set_rx_mode(struct net_device *dev)
 			if (!netdev_mc_empty(dev)) {
 				netdev_for_each_mc_addr(ha, dev) {
 					mvneta_mcast_addr_set(pp, ha->addr,
-							      rxq_def);
+							      pp->rxq_def);
 				}
 			}
 		}
@@ -2154,6 +2259,7 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 {
 	int rx_done = 0;
 	u32 cause_rx_tx;
+	int rx_queue;
 	struct mvneta_port *pp = netdev_priv(napi->dev);
 	struct mvneta_pcpu_port *port = this_cpu_ptr(pp->ports);
 
@@ -2185,8 +2291,15 @@ static int mvneta_poll(struct napi_struct *napi, int budget)
 	/* For the case where the last mvneta_poll did not process all
 	 * RX packets
 	 */
+	rx_queue = fls(((cause_rx_tx >> 8) & 0xff));
+
 	cause_rx_tx |= port->cause_rx_tx;
-	rx_done = mvneta_rx(pp, budget, &pp->rxqs[rxq_def]);
+
+	if (rx_queue) {
+		rx_queue = rx_queue - 1;
+		rx_done = mvneta_rx(pp, budget, &pp->rxqs[rx_queue]);
+	}
+
 	budget -= rx_done;
 
 	if (budget > 0) {
@@ -2303,6 +2416,8 @@ static void mvneta_rxq_deinit(struct mvneta_port *pp,
 static int mvneta_txq_init(struct mvneta_port *pp,
 			   struct mvneta_tx_queue *txq)
 {
+	int cpu;
+
 	txq->size = pp->tx_ring_size;
 
 	/* A queue must always have room for at least one skb.
@@ -2355,6 +2470,14 @@ static int mvneta_txq_init(struct mvneta_port *pp,
 	}
 	mvneta_tx_done_pkts_coal_set(pp, txq, txq->done_pkts_coal);
 
+	/* Setup XPS mapping */
+	if (txq_number > 1)
+		cpu = txq->id % num_present_cpus();
+	else
+		cpu = pp->rxq_def % num_present_cpus();
+	cpumask_set_cpu(cpu, &txq->affinity_mask);
+	netif_set_xps_queue(pp->dev, &txq->affinity_mask, txq->id);
+
 	return 0;
 }
 
@@ -2399,19 +2522,27 @@ static void mvneta_cleanup_txqs(struct mvneta_port *pp)
 /* Cleanup all Rx queues */
 static void mvneta_cleanup_rxqs(struct mvneta_port *pp)
 {
-	mvneta_rxq_deinit(pp, &pp->rxqs[rxq_def]);
+	int queue;
+
+	for (queue = 0; queue < txq_number; queue++)
+		mvneta_rxq_deinit(pp, &pp->rxqs[queue]);
 }
 
 
 /* Init all Rx queues */
 static int mvneta_setup_rxqs(struct mvneta_port *pp)
 {
-	int err = mvneta_rxq_init(pp, &pp->rxqs[rxq_def]);
-	if (err) {
-		netdev_err(pp->dev, "%s: can't create rxq=%d\n",
-			   __func__, rxq_def);
-		mvneta_cleanup_rxqs(pp);
-		return err;
+	int queue;
+
+	for (queue = 0; queue < rxq_number; queue++) {
+		int err = mvneta_rxq_init(pp, &pp->rxqs[queue]);
+
+		if (err) {
+			netdev_err(pp->dev, "%s: can't create rxq=%d\n",
+				   __func__, queue);
+			mvneta_cleanup_rxqs(pp);
+			return err;
+		}
 	}
 
 	return 0;
@@ -2437,7 +2568,7 @@ static int mvneta_setup_txqs(struct mvneta_port *pp)
 
 static void mvneta_start_dev(struct mvneta_port *pp)
 {
-	unsigned int cpu;
+	int cpu;
 
 	mvneta_max_rx_size_set(pp, pp->pkt_size);
 	mvneta_txq_max_tx_size_set(pp, pp->pkt_size);
@@ -2446,17 +2577,15 @@ static void mvneta_start_dev(struct mvneta_port *pp)
 	mvneta_port_enable(pp);
 
 	/* Enable polling on the port */
-	for_each_present_cpu(cpu) {
+	for_each_online_cpu(cpu) {
 		struct mvneta_pcpu_port *port = per_cpu_ptr(pp->ports, cpu);
 
 		napi_enable(&port->napi);
 	}
 
-	/* Unmask interrupts */
-	mvreg_write(pp, MVNETA_INTR_NEW_MASK,
-		    MVNETA_RX_INTR_MASK(rxq_number) |
-		    MVNETA_TX_INTR_MASK(txq_number) |
-		    MVNETA_MISCINTR_INTR_MASK);
+	/* Unmask interrupts. It has to be done from each CPU */
+	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 |
@@ -2472,7 +2601,7 @@ static void mvneta_stop_dev(struct mvneta_port *pp)
 
 	phy_stop(pp->phy_dev);
 
-	for_each_present_cpu(cpu) {
+	for_each_online_cpu(cpu) {
 		struct mvneta_pcpu_port *port = per_cpu_ptr(pp->ports, cpu);
 
 		napi_disable(&port->napi);
@@ -2487,13 +2616,10 @@ static void mvneta_stop_dev(struct mvneta_port *pp)
 	mvneta_port_disable(pp);
 
 	/* Clear all ethernet port interrupts */
-	mvreg_write(pp, MVNETA_INTR_MISC_CAUSE, 0);
-	mvreg_write(pp, MVNETA_INTR_OLD_CAUSE, 0);
+	on_each_cpu(mvneta_percpu_clear_intr_cause, pp, true);
 
 	/* Mask all ethernet port interrupts */
-	mvreg_write(pp, MVNETA_INTR_NEW_MASK, 0);
-	mvreg_write(pp, MVNETA_INTR_OLD_MASK, 0);
-	mvreg_write(pp, MVNETA_INTR_MISC_MASK, 0);
+	on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
 
 	mvneta_tx_reset(pp);
 	mvneta_rx_reset(pp);
@@ -2615,7 +2741,7 @@ static int mvneta_set_mac_addr(struct net_device *dev, void *addr)
 	mvneta_mac_addr_set(pp, dev->dev_addr, -1);
 
 	/* Set new addr in hw */
-	mvneta_mac_addr_set(pp, sockaddr->sa_data, rxq_def);
+	mvneta_mac_addr_set(pp, sockaddr->sa_data, pp->rxq_def);
 
 	eth_commit_mac_addr_change(dev, addr);
 	return 0;
@@ -2730,24 +2856,56 @@ static void mvneta_percpu_disable(void *arg)
 	disable_percpu_irq(pp->dev->irq);
 }
 
+/* Electing a CPU must be done in an atomic way: it should be done
+ * after or before the removal/insertion of a CPU and this function is
+ * not reentrant.
+ */
 static void mvneta_percpu_elect(struct mvneta_port *pp)
 {
-	int online_cpu_idx, cpu, i = 0;
+	int elected_cpu = 0, max_cpu, cpu, i = 0;
+
+	/* Use the cpu associated to the rxq when it is online, in all
+	 * the other cases, use the cpu 0 which can't be offline.
+	 */
+	if (cpu_online(pp->rxq_def))
+		elected_cpu = pp->rxq_def;
 
-	online_cpu_idx = rxq_def % num_online_cpus();
+	max_cpu = num_present_cpus();
 
 	for_each_online_cpu(cpu) {
-		if (i == online_cpu_idx)
-			/* Enable per-CPU interrupt on the one CPU we
-			 * just elected
+		int rxq_map = 0, txq_map = 0;
+		int rxq;
+
+		for (rxq = 0; rxq < rxq_number; rxq++)
+			if ((rxq % max_cpu) == cpu)
+				rxq_map |= MVNETA_CPU_RXQ_ACCESS(rxq);
+
+		if (cpu == elected_cpu)
+			/* Map the default receive queue queue to the
+			 * elected CPU
 			 */
-			smp_call_function_single(cpu, mvneta_percpu_enable,
-						pp, true);
+			rxq_map |= MVNETA_CPU_RXQ_ACCESS(pp->rxq_def);
+
+		/* We update the TX queue map only if we have one
+		 * queue. In this case we associate the TX queue to
+		 * the CPU bound to the default RX queue
+		 */
+		if (txq_number == 1)
+			txq_map = (cpu == elected_cpu) ?
+				MVNETA_CPU_TXQ_ACCESS(1) : 0;
 		else
-			/* Disable per-CPU interrupt on all the other CPU */
-			smp_call_function_single(cpu, mvneta_percpu_disable,
-						pp, true);
+			txq_map = mvreg_read(pp, MVNETA_CPU_MAP(cpu)) &
+				MVNETA_CPU_TXQ_ACCESS_ALL_MASK;
+
+		mvreg_write(pp, MVNETA_CPU_MAP(cpu), rxq_map | txq_map);
+
+		/* Update the interrupt mask on each CPU according the
+		 * new mapping
+		 */
+		smp_call_function_single(cpu, mvneta_percpu_unmask_interrupt,
+					 pp, true);
 		i++;
+
 	}
 };
 
@@ -2762,6 +2920,14 @@ static int mvneta_percpu_notifier(struct notifier_block *nfb,
 	switch (action) {
 	case CPU_ONLINE:
 	case CPU_ONLINE_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
@@ -2777,34 +2943,40 @@ static int mvneta_percpu_notifier(struct notifier_block *nfb,
 		}
 
 		/* Mask all ethernet port interrupts */
-		mvreg_write(pp, MVNETA_INTR_NEW_MASK, 0);
-		mvreg_write(pp, MVNETA_INTR_OLD_MASK, 0);
-		mvreg_write(pp, MVNETA_INTR_MISC_MASK, 0);
+		on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
 		napi_enable(&port->napi);
 
+
+		/* Enable per-CPU interrupts on the CPU that is
+		 * brought up.
+		 */
+		smp_call_function_single(cpu, mvneta_percpu_enable,
+					 pp, true);
+
 		/* Enable per-CPU interrupt on the one CPU we care
 		 * about.
 		 */
 		mvneta_percpu_elect(pp);
 
 		/* Unmask all ethernet port interrupts */
-		mvreg_write(pp, MVNETA_INTR_NEW_MASK,
-			MVNETA_RX_INTR_MASK(rxq_number) |
-			MVNETA_TX_INTR_MASK(txq_number) |
-			MVNETA_MISCINTR_INTR_MASK);
+		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 */
-		mvreg_write(pp, MVNETA_INTR_NEW_MASK, 0);
-		mvreg_write(pp, MVNETA_INTR_OLD_MASK, 0);
-		mvreg_write(pp, MVNETA_INTR_MISC_MASK, 0);
+		on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
+		spin_unlock(&pp->lock);
 
 		napi_synchronize(&port->napi);
 		napi_disable(&port->napi);
@@ -2818,12 +2990,11 @@ static int mvneta_percpu_notifier(struct notifier_block *nfb,
 	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 */
-		mvreg_write(pp, MVNETA_INTR_NEW_MASK,
-			MVNETA_RX_INTR_MASK(rxq_number) |
-			MVNETA_TX_INTR_MASK(txq_number) |
-			MVNETA_MISCINTR_INTR_MASK);
+		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 |
@@ -2860,17 +3031,12 @@ static int mvneta_open(struct net_device *dev)
 		goto err_cleanup_txqs;
 	}
 
-	/* Even though the documentation says that request_percpu_irq
-	 * doesn't enable the interrupts automatically, it actually
-	 * does so on the local CPU.
-	 *
-	 * Make sure it's disabled.
+	/* Enable per-CPU interrupt on all the CPU to handle our RX
+	 * queue interrupts
 	 */
-	mvneta_percpu_disable(pp);
-
-	/* Elect a CPU to handle our RX queue interrupt */
-	mvneta_percpu_elect(pp);
+	on_each_cpu(mvneta_percpu_enable, pp, true);
 
+	pp->is_stopped = false;
 	/* Register a CPU notifier to handle the case where our CPU
 	 * might be taken offline.
 	 */
@@ -2902,13 +3068,20 @@ err_cleanup_rxqs:
 static int mvneta_stop(struct net_device *dev)
 {
 	struct mvneta_port *pp = netdev_priv(dev);
-	int cpu;
 
+	/* Inform that we are stopping so we don't want to setup the
+	 * driver for new CPUs in the notifiers
+	 */
+	spin_lock(&pp->lock);
+	pp->is_stopped = true;
 	mvneta_stop_dev(pp);
 	mvneta_mdio_remove(pp);
 	unregister_cpu_notifier(&pp->cpu_notifier);
-	for_each_present_cpu(cpu)
-		smp_call_function_single(cpu, mvneta_percpu_disable, pp, true);
+	/* Now that the notifier are unregistered, we can release le
+	 * lock
+	 */
+	spin_unlock(&pp->lock);
+	on_each_cpu(mvneta_percpu_disable, pp, true);
 	free_percpu_irq(dev->irq, pp->ports);
 	mvneta_cleanup_rxqs(pp);
 	mvneta_cleanup_txqs(pp);
@@ -2943,10 +3116,43 @@ int mvneta_ethtool_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 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;
 
-	if (!pp->phy_dev)
+	if (!phydev)
 		return -ENODEV;
 
+	if ((cmd->autoneg == AUTONEG_ENABLE) != pp->use_inband_status) {
+		u32 val;
+
+		mvneta_set_autoneg(pp, cmd->autoneg == AUTONEG_ENABLE);
+
+		if (cmd->autoneg == AUTONEG_DISABLE) {
+			val = mvreg_read(pp, MVNETA_GMAC_AUTONEG_CONFIG);
+			val &= ~(MVNETA_GMAC_CONFIG_MII_SPEED |
+				 MVNETA_GMAC_CONFIG_GMII_SPEED |
+				 MVNETA_GMAC_CONFIG_FULL_DUPLEX);
+
+			if (phydev->duplex)
+				val |= MVNETA_GMAC_CONFIG_FULL_DUPLEX;
+
+			if (phydev->speed == SPEED_1000)
+				val |= MVNETA_GMAC_CONFIG_GMII_SPEED;
+			else if (phydev->speed == SPEED_100)
+				val |= MVNETA_GMAC_CONFIG_MII_SPEED;
+
+			mvreg_write(pp, MVNETA_GMAC_AUTONEG_CONFIG, val);
+		}
+
+		pp->use_inband_status = (cmd->autoneg == AUTONEG_ENABLE);
+		netdev_info(pp->dev, "autoneg status set to %i\n",
+			    pp->use_inband_status);
+
+		if (netif_running(dev)) {
+			mvneta_port_down(pp);
+			mvneta_port_up(pp);
+		}
+	}
+
 	return phy_ethtool_sset(pp->phy_dev, cmd);
 }
 
@@ -3056,26 +3262,25 @@ static void mvneta_ethtool_update_stats(struct mvneta_port *pp)
 	const struct mvneta_statistic *s;
 	void __iomem *base = pp->base;
 	u32 high, low, val;
+	u64 val64;
 	int i;
 
 	for (i = 0, s = mvneta_statistics;
 	     s < mvneta_statistics + ARRAY_SIZE(mvneta_statistics);
 	     s++, i++) {
-		val = 0;
-
 		switch (s->type) {
 		case T_REG_32:
 			val = readl_relaxed(base + s->offset);
+			pp->ethtool_stats[i] += val;
 			break;
 		case T_REG_64:
 			/* Docs say to read low 32-bit then high */
 			low = readl_relaxed(base + s->offset);
 			high = readl_relaxed(base + s->offset + 4);
-			val = (u64)high << 32 | low;
+			val64 = (u64)high << 32 | low;
+			pp->ethtool_stats[i] += val64;
 			break;
 		}
-
-		pp->ethtool_stats[i] += val;
 	}
 }
 
@@ -3098,6 +3303,106 @@ static int mvneta_ethtool_get_sset_count(struct net_device *dev, int sset)
 	return -EOPNOTSUPP;
 }
 
+static u32 mvneta_ethtool_get_rxfh_indir_size(struct net_device *dev)
+{
+	return MVNETA_RSS_LU_TABLE_SIZE;
+}
+
+static int mvneta_ethtool_get_rxnfc(struct net_device *dev,
+				    struct ethtool_rxnfc *info,
+				    u32 *rules __always_unused)
+{
+	switch (info->cmd) {
+	case ETHTOOL_GRXRINGS:
+		info->data =  rxq_number;
+		return 0;
+	case ETHTOOL_GRXFH:
+		return -EOPNOTSUPP;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int  mvneta_config_rss(struct mvneta_port *pp)
+{
+	int cpu;
+	u32 val;
+
+	netif_tx_stop_all_queues(pp->dev);
+
+	on_each_cpu(mvneta_percpu_mask_interrupt, pp, true);
+
+	/* We have to synchronise on the napi of each CPU */
+	for_each_online_cpu(cpu) {
+		struct mvneta_pcpu_port *pcpu_port =
+			per_cpu_ptr(pp->ports, cpu);
+
+		napi_synchronize(&pcpu_port->napi);
+		napi_disable(&pcpu_port->napi);
+	}
+
+	pp->rxq_def = pp->indir[0];
+
+	/* Update unicast mapping */
+	mvneta_set_rx_mode(pp->dev);
+
+	/* Update val of portCfg register accordingly with all RxQueue types */
+	val = MVNETA_PORT_CONFIG_DEFL_VALUE(pp->rxq_def);
+	mvreg_write(pp, MVNETA_PORT_CONFIG, val);
+
+	/* Update the elected CPU matching the new rxq_def */
+	spin_lock(&pp->lock);
+	mvneta_percpu_elect(pp);
+	spin_unlock(&pp->lock);
+
+	/* We have to synchronise on the napi of each CPU */
+	for_each_online_cpu(cpu) {
+		struct mvneta_pcpu_port *pcpu_port =
+			per_cpu_ptr(pp->ports, cpu);
+
+		napi_enable(&pcpu_port->napi);
+	}
+
+	netif_tx_start_all_queues(pp->dev);
+
+	return 0;
+}
+
+static int mvneta_ethtool_set_rxfh(struct net_device *dev, const u32 *indir,
+				   const u8 *key, const u8 hfunc)
+{
+	struct mvneta_port *pp = netdev_priv(dev);
+	/* We require at least one supported parameter to be changed
+	 * and no change in any of the unsupported parameters
+	 */
+	if (key ||
+	    (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_TOP))
+		return -EOPNOTSUPP;
+
+	if (!indir)
+		return 0;
+
+	memcpy(pp->indir, indir, MVNETA_RSS_LU_TABLE_SIZE);
+
+	return mvneta_config_rss(pp);
+}
+
+static int mvneta_ethtool_get_rxfh(struct net_device *dev, u32 *indir, u8 *key,
+				   u8 *hfunc)
+{
+	struct mvneta_port *pp = netdev_priv(dev);
+
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	if (!indir)
+		return 0;
+
+	memcpy(indir, pp->indir, MVNETA_RSS_LU_TABLE_SIZE);
+
+	return 0;
+}
+
 static const struct net_device_ops mvneta_netdev_ops = {
 	.ndo_open            = mvneta_open,
 	.ndo_stop            = mvneta_stop,
@@ -3122,6 +3427,10 @@ const struct ethtool_ops mvneta_eth_tool_ops = {
 	.get_strings	= mvneta_ethtool_get_strings,
 	.get_ethtool_stats = mvneta_ethtool_get_stats,
 	.get_sset_count	= mvneta_ethtool_get_sset_count,
+	.get_rxfh_indir_size = mvneta_ethtool_get_rxfh_indir_size,
+	.get_rxnfc	= mvneta_ethtool_get_rxnfc,
+	.get_rxfh	= mvneta_ethtool_get_rxfh,
+	.set_rxfh	= mvneta_ethtool_set_rxfh,
 };
 
 /* Initialize hw */
@@ -3230,9 +3539,6 @@ static int mvneta_port_power_up(struct mvneta_port *pp, int phy_mode)
 		return -EINVAL;
 	}
 
-	if (pp->use_inband_status)
-		ctrl |= MVNETA_GMAC2_INBAND_AN_ENABLE;
-
 	/* Cancel Port Reset */
 	ctrl &= ~MVNETA_GMAC2_PORT_RESET;
 	mvreg_write(pp, MVNETA_GMAC_CTRL_2, ctrl);
@@ -3314,7 +3620,13 @@ static int mvneta_probe(struct platform_device *pdev)
 				 strcmp(managed, "in-band-status") == 0);
 	pp->cpu_notifier.notifier_call = mvneta_percpu_notifier;
 
-	pp->clk = devm_clk_get(&pdev->dev, NULL);
+	pp->rxq_def = rxq_def;
+
+	pp->indir[0] = rxq_def;
+
+	pp->clk = devm_clk_get(&pdev->dev, "core");
+	if (IS_ERR(pp->clk))
+		pp->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(pp->clk)) {
 		err = PTR_ERR(pp->clk);
 		goto err_put_phy_node;
@@ -3322,6 +3634,10 @@ static int mvneta_probe(struct platform_device *pdev)
 
 	clk_prepare_enable(pp->clk);
 
+	pp->clk_bus = devm_clk_get(&pdev->dev, "bus");
+	if (!IS_ERR(pp->clk_bus))
+		clk_prepare_enable(pp->clk_bus);
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	pp->base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(pp->base)) {
@@ -3423,7 +3739,7 @@ static int mvneta_probe(struct platform_device *pdev)
 
 		mvneta_fixed_link_update(pp, phy);
 
-		put_device(&phy->dev);
+		put_device(&phy->mdio.dev);
 	}
 
 	return 0;
@@ -3433,6 +3749,7 @@ err_free_stats:
 err_free_ports:
 	free_percpu(pp->ports);
 err_clk:
+	clk_disable_unprepare(pp->clk_bus);
 	clk_disable_unprepare(pp->clk);
 err_put_phy_node:
 	of_node_put(phy_node);
@@ -3450,6 +3767,7 @@ static int mvneta_remove(struct platform_device *pdev)
 	struct mvneta_port *pp = netdev_priv(dev);
 
 	unregister_netdev(dev);
+	clk_disable_unprepare(pp->clk_bus);
 	clk_disable_unprepare(pp->clk);
 	free_percpu(pp->ports);
 	free_percpu(pp->stats);
diff --git a/drivers/net/ethernet/marvell/mvpp2.c b/drivers/net/ethernet/marvell/mvpp2.c
index a4beccf1fd46..c797971aefab 100644
--- a/drivers/net/ethernet/marvell/mvpp2.c
+++ b/drivers/net/ethernet/marvell/mvpp2.c
@@ -3061,7 +3061,7 @@ static int mvpp2_prs_mac_da_accept(struct mvpp2 *priv, int port,
 
 		pe = kzalloc(sizeof(*pe), GFP_KERNEL);
 		if (!pe)
-			return -1;
+			return -ENOMEM;
 		mvpp2_prs_tcam_lu_set(pe, MVPP2_PRS_LU_MAC);
 		pe->index = tid;
 
@@ -3077,7 +3077,7 @@ static int mvpp2_prs_mac_da_accept(struct mvpp2 *priv, int port,
 	if (pmap == 0) {
 		if (add) {
 			kfree(pe);
-			return -1;
+			return -EINVAL;
 		}
 		mvpp2_prs_hw_inv(priv, pe->index);
 		priv->prs_shadow[pe->index].valid = false;
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index 5606a043063e..ec0a22119e09 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -4380,7 +4380,7 @@ static netdev_features_t sky2_fix_features(struct net_device *dev,
 	 */
 	if (dev->mtu > ETH_DATA_LEN && hw->chip_id == CHIP_ID_YUKON_EC_U) {
 		netdev_info(dev, "checksum offload not possible with jumbo frames\n");
-		features &= ~(NETIF_F_TSO|NETIF_F_SG|NETIF_F_ALL_CSUM);
+		features &= ~(NETIF_F_TSO | NETIF_F_SG | NETIF_F_CSUM_MASK);
 	}
 
 	/* Some hardware requires receive checksum for RSS to work. */
diff --git a/drivers/net/ethernet/mellanox/mlx4/catas.c b/drivers/net/ethernet/mellanox/mlx4/catas.c
index 715de8affcc9..c7e939945259 100644
--- a/drivers/net/ethernet/mellanox/mlx4/catas.c
+++ b/drivers/net/ethernet/mellanox/mlx4/catas.c
@@ -182,10 +182,17 @@ void mlx4_enter_error_state(struct mlx4_dev_persistent *persist)
 		err = mlx4_reset_slave(dev);
 	else
 		err = mlx4_reset_master(dev);
-	BUG_ON(err != 0);
 
+	if (!err) {
+		mlx4_err(dev, "device was reset successfully\n");
+	} else {
+		/* EEH could have disabled the PCI channel during reset. That's
+		 * recoverable and the PCI error flow will handle it.
+		 */
+		if (!pci_channel_offline(dev->persist->pdev))
+			BUG_ON(1);
+	}
 	dev->persist->state |= MLX4_DEVICE_STATE_INTERNAL_ERROR;
-	mlx4_err(dev, "device was reset successfully\n");
 	mutex_unlock(&persist->device_state_mutex);
 
 	/* At that step HW was already reset, now notify clients */
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index d48d5793407d..e94ca1c3fc7c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -2429,7 +2429,7 @@ err_thread:
 	flush_workqueue(priv->mfunc.master.comm_wq);
 	destroy_workqueue(priv->mfunc.master.comm_wq);
 err_slaves:
-	while (--i) {
+	while (i--) {
 		for (port = 1; port <= MLX4_MAX_PORTS; port++)
 			kfree(priv->mfunc.master.slave_state[i].vlan_filter[port]);
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/cq.c b/drivers/net/ethernet/mellanox/mlx4/cq.c
index 3348e646db70..a849da92f857 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cq.c
@@ -318,7 +318,9 @@ int mlx4_cq_alloc(struct mlx4_dev *dev, int nent,
 	if (timestamp_en)
 		cq_context->flags  |= cpu_to_be32(1 << 19);
 
-	cq_context->logsize_usrpage = cpu_to_be32((ilog2(nent) << 24) | uar->index);
+	cq_context->logsize_usrpage =
+		cpu_to_be32((ilog2(nent) << 24) |
+			    mlx4_to_hw_uar_index(dev, uar->index));
 	cq_context->comp_eqn	    = priv->eq_table.eq[MLX4_CQ_TO_EQ_VECTOR(vector)].eqn;
 	cq_context->log_page_size   = mtt->page_shift - MLX4_ICM_PAGE_SHIFT;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_clock.c b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
index 038f9ce391e6..1494997c4f7e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_clock.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_clock.c
@@ -236,6 +236,24 @@ static const struct ptp_clock_info mlx4_en_ptp_clock_info = {
 	.enable		= mlx4_en_phc_enable,
 };
 
+#define MLX4_EN_WRAP_AROUND_SEC	10ULL
+
+/* This function calculates the max shift that enables the user range
+ * of MLX4_EN_WRAP_AROUND_SEC values in the cycles register.
+ */
+static u32 freq_to_shift(u16 freq)
+{
+	u32 freq_khz = freq * 1000;
+	u64 max_val_cycles = freq_khz * 1000 * MLX4_EN_WRAP_AROUND_SEC;
+	u64 max_val_cycles_rounded = is_power_of_2(max_val_cycles + 1) ?
+		max_val_cycles : roundup_pow_of_two(max_val_cycles) - 1;
+	/* calculate max possible multiplier in order to fit in 64bit */
+	u64 max_mul = div_u64(0xffffffffffffffffULL, max_val_cycles_rounded);
+
+	/* This comes from the reverse of clocksource_khz2mult */
+	return ilog2(div_u64(max_mul * freq_khz, 1000000));
+}
+
 void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
 {
 	struct mlx4_dev *dev = mdev->dev;
@@ -254,12 +272,7 @@ void mlx4_en_init_timestamp(struct mlx4_en_dev *mdev)
 	memset(&mdev->cycles, 0, sizeof(mdev->cycles));
 	mdev->cycles.read = mlx4_en_read_clock;
 	mdev->cycles.mask = CLOCKSOURCE_MASK(48);
-	/* Using shift to make calculation more accurate. Since current HW
-	 * clock frequency is 427 MHz, and cycles are given using a 48 bits
-	 * register, the biggest shift when calculating using u64, is 14
-	 * (max_cycles * multiplier < 2^64)
-	 */
-	mdev->cycles.shift = 14;
+	mdev->cycles.shift = freq_to_shift(dev->caps.hca_core_clock);
 	mdev->cycles.mult =
 		clocksource_khz2mult(1000 * dev->caps.hca_core_clock, mdev->cycles.shift);
 	mdev->nominal_c_mult = mdev->cycles.mult;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_cq.c b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
index eb8a4988de63..af975a2b74c6 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_cq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_cq.c
@@ -155,13 +155,11 @@ int mlx4_en_activate_cq(struct mlx4_en_priv *priv, struct mlx4_en_cq *cq,
 	cq->mcq.comp  = cq->is_tx ? mlx4_en_tx_irq : mlx4_en_rx_irq;
 	cq->mcq.event = mlx4_en_cq_event;
 
-	if (cq->is_tx) {
-		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_tx_cq,
-			       NAPI_POLL_WEIGHT);
-	} else {
+	if (cq->is_tx)
+		netif_tx_napi_add(cq->dev, &cq->napi, mlx4_en_poll_tx_cq,
+				  NAPI_POLL_WEIGHT);
+	else
 		netif_napi_add(cq->dev, &cq->napi, mlx4_en_poll_rx_cq, 64);
-		napi_hash_add(&cq->napi);
-	}
 
 	napi_enable(&cq->napi);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index ddb5541882f5..dd84cabb2a51 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -337,11 +337,7 @@ static int mlx4_en_get_sset_count(struct net_device *dev, int sset)
 	case ETH_SS_STATS:
 		return bitmap_iterator_count(&it) +
 			(priv->tx_ring_num * 2) +
-#ifdef CONFIG_NET_RX_BUSY_POLL
-			(priv->rx_ring_num * 5);
-#else
 			(priv->rx_ring_num * 2);
-#endif
 	case ETH_SS_TEST:
 		return MLX4_EN_NUM_SELF_TEST - !(priv->mdev->dev->caps.flags
 					& MLX4_DEV_CAP_FLAG_UC_LOOPBACK) * 2;
@@ -408,11 +404,6 @@ static void mlx4_en_get_ethtool_stats(struct net_device *dev,
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		data[index++] = priv->rx_ring[i]->packets;
 		data[index++] = priv->rx_ring[i]->bytes;
-#ifdef CONFIG_NET_RX_BUSY_POLL
-		data[index++] = priv->rx_ring[i]->yields;
-		data[index++] = priv->rx_ring[i]->misses;
-		data[index++] = priv->rx_ring[i]->cleaned;
-#endif
 	}
 	spin_unlock_bh(&priv->stats_lock);
 
@@ -486,14 +477,6 @@ static void mlx4_en_get_strings(struct net_device *dev,
 				"rx%d_packets", i);
 			sprintf(data + (index++) * ETH_GSTRING_LEN,
 				"rx%d_bytes", i);
-#ifdef CONFIG_NET_RX_BUSY_POLL
-			sprintf(data + (index++) * ETH_GSTRING_LEN,
-				"rx%d_napi_yield", i);
-			sprintf(data + (index++) * ETH_GSTRING_LEN,
-				"rx%d_misses", i);
-			sprintf(data + (index++) * ETH_GSTRING_LEN,
-				"rx%d_cleaned", i);
-#endif
 		}
 		break;
 	case ETH_SS_PRIV_FLAGS:
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index 7869f97de5da..21e2c0960271 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -69,34 +69,6 @@ int mlx4_en_setup_tc(struct net_device *dev, u8 up)
 	return 0;
 }
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-/* must be called with local_bh_disable()d */
-static int mlx4_en_low_latency_recv(struct napi_struct *napi)
-{
-	struct mlx4_en_cq *cq = container_of(napi, struct mlx4_en_cq, napi);
-	struct net_device *dev = cq->dev;
-	struct mlx4_en_priv *priv = netdev_priv(dev);
-	struct mlx4_en_rx_ring *rx_ring = priv->rx_ring[cq->ring];
-	int done;
-
-	if (!priv->port_up)
-		return LL_FLUSH_FAILED;
-
-	if (!mlx4_en_cq_lock_poll(cq))
-		return LL_FLUSH_BUSY;
-
-	done = mlx4_en_process_rx_cq(dev, cq, 4);
-	if (likely(done))
-		rx_ring->cleaned += done;
-	else
-		rx_ring->misses++;
-
-	mlx4_en_cq_unlock_poll(cq);
-
-	return done;
-}
-#endif	/* CONFIG_NET_RX_BUSY_POLL */
-
 #ifdef CONFIG_RFS_ACCEL
 
 struct mlx4_en_filter {
@@ -1561,8 +1533,6 @@ int mlx4_en_start_port(struct net_device *dev)
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		cq = priv->rx_cq[i];
 
-		mlx4_en_cq_init_lock(cq);
-
 		err = mlx4_en_init_affinity_hint(priv, i);
 		if (err) {
 			en_err(priv, "Failed preparing IRQ affinity hint\n");
@@ -1859,13 +1829,6 @@ void mlx4_en_stop_port(struct net_device *dev, int detach)
 	for (i = 0; i < priv->rx_ring_num; i++) {
 		struct mlx4_en_cq *cq = priv->rx_cq[i];
 
-		local_bh_disable();
-		while (!mlx4_en_cq_lock_napi(cq)) {
-			pr_info("CQ %d locked\n", i);
-			mdelay(1);
-		}
-		local_bh_enable();
-
 		napi_synchronize(&cq->napi);
 		mlx4_en_deactivate_rx_ring(priv, priv->rx_ring[i]);
 		mlx4_en_deactivate_cq(priv, cq);
@@ -2282,7 +2245,7 @@ static int mlx4_en_set_vf_mac(struct net_device *dev, int queue, u8 *mac)
 	struct mlx4_en_dev *mdev = en_priv->mdev;
 	u64 mac_u64 = mlx4_mac_to_u64(mac);
 
-	if (!is_valid_ether_addr(mac))
+	if (is_multicast_ether_addr(mac))
 		return -EINVAL;
 
 	return mlx4_set_vf_mac(mdev->dev, en_priv->port, queue, mac_u64);
@@ -2381,8 +2344,6 @@ out:
 	/* set offloads */
 	priv->dev->hw_enc_features |= NETIF_F_IP_CSUM | NETIF_F_RXCSUM |
 				      NETIF_F_TSO | NETIF_F_GSO_UDP_TUNNEL;
-	priv->dev->hw_features |= NETIF_F_GSO_UDP_TUNNEL;
-	priv->dev->features    |= NETIF_F_GSO_UDP_TUNNEL;
 }
 
 static void mlx4_en_del_vxlan_offloads(struct work_struct *work)
@@ -2393,8 +2354,6 @@ static void mlx4_en_del_vxlan_offloads(struct work_struct *work)
 	/* unset offloads */
 	priv->dev->hw_enc_features &= ~(NETIF_F_IP_CSUM | NETIF_F_RXCSUM |
 				      NETIF_F_TSO | NETIF_F_GSO_UDP_TUNNEL);
-	priv->dev->hw_features &= ~NETIF_F_GSO_UDP_TUNNEL;
-	priv->dev->features    &= ~NETIF_F_GSO_UDP_TUNNEL;
 
 	ret = mlx4_SET_PORT_VXLAN(priv->mdev->dev, priv->port,
 				  VXLAN_STEER_BY_OUTER_MAC, 0);
@@ -2507,9 +2466,6 @@ static const struct net_device_ops mlx4_netdev_ops = {
 #ifdef CONFIG_RFS_ACCEL
 	.ndo_rx_flow_steer	= mlx4_en_filter_rfs,
 #endif
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	.ndo_busy_poll		= mlx4_en_low_latency_recv,
-#endif
 	.ndo_get_phys_port_id	= mlx4_en_get_phys_port_id,
 #ifdef CONFIG_MLX4_EN_VXLAN
 	.ndo_add_vxlan_port	= mlx4_en_add_vxlan_port,
@@ -3020,6 +2976,11 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 		priv->rss_hash_fn = ETH_RSS_HASH_TOP;
 	}
 
+	if (mdev->dev->caps.tunnel_offload_mode == MLX4_TUNNEL_OFFLOAD_MODE_VXLAN) {
+		dev->hw_features |= NETIF_F_GSO_UDP_TUNNEL;
+		dev->features    |= NETIF_F_GSO_UDP_TUNNEL;
+	}
+
 	mdev->pndev[port] = dev;
 	mdev->upper[port] = NULL;
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_port.c b/drivers/net/ethernet/mellanox/mlx4/en_port.c
index ee99e67187f5..3904b5fc0b7c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_port.c
@@ -238,11 +238,11 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 	stats->collisions = 0;
 	stats->rx_dropped = be32_to_cpu(mlx4_en_stats->RDROP);
 	stats->rx_length_errors = be32_to_cpu(mlx4_en_stats->RdropLength);
-	stats->rx_over_errors = be32_to_cpu(mlx4_en_stats->RdropOvflw);
+	stats->rx_over_errors = 0;
 	stats->rx_crc_errors = be32_to_cpu(mlx4_en_stats->RCRC);
 	stats->rx_frame_errors = 0;
 	stats->rx_fifo_errors = be32_to_cpu(mlx4_en_stats->RdropOvflw);
-	stats->rx_missed_errors = be32_to_cpu(mlx4_en_stats->RdropOvflw);
+	stats->rx_missed_errors = 0;
 	stats->tx_aborted_errors = 0;
 	stats->tx_carrier_errors = 0;
 	stats->tx_fifo_errors = 0;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_resources.c b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
index 12aab5a659d3..02e925d6f734 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_resources.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_resources.c
@@ -58,7 +58,8 @@ void mlx4_en_fill_qp_context(struct mlx4_en_priv *priv, int size, int stride,
 	} else {
 		context->sq_size_stride = ilog2(TXBB_SIZE) - 4;
 	}
-	context->usr_page = cpu_to_be32(mdev->priv_uar.index);
+	context->usr_page = cpu_to_be32(mlx4_to_hw_uar_index(mdev->dev,
+					mdev->priv_uar.index));
 	context->local_qpn = cpu_to_be32(qpn);
 	context->pri_path.ackto = 1 & 0x07;
 	context->pri_path.sched_queue = 0x83 | (priv->port - 1) << 6;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index e7a5000aa12c..41440b2b20a3 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -873,10 +873,8 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		 * - TCP/IP (v4)
 		 * - without IP options
 		 * - not an IP fragment
-		 * - no LLS polling in progress
 		 */
-		if (!mlx4_en_cq_busy_polling(cq) &&
-		    (dev->features & NETIF_F_GRO)) {
+		if (dev->features & NETIF_F_GRO) {
 			struct sk_buff *gro_skb = napi_get_frags(&cq->napi);
 			if (!gro_skb)
 				goto next;
@@ -927,7 +925,6 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 						PKT_HASH_TYPE_L3);
 
 			skb_record_rx_queue(gro_skb, cq->ring);
-			skb_mark_napi_id(gro_skb, &cq->napi);
 
 			if (ring->hwtstamp_rx_filter == HWTSTAMP_FILTER_ALL) {
 				timestamp = mlx4_en_get_cqe_ts(cqe);
@@ -990,13 +987,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 					       timestamp);
 		}
 
-		skb_mark_napi_id(skb, &cq->napi);
-
-		if (!mlx4_en_cq_busy_polling(cq))
-			napi_gro_receive(&cq->napi, skb);
-		else
-			netif_receive_skb(skb);
-
+		napi_gro_receive(&cq->napi, skb);
 next:
 		for (nr = 0; nr < priv->num_frags; nr++)
 			mlx4_en_free_frag(priv, frags, nr);
@@ -1038,13 +1029,8 @@ int mlx4_en_poll_rx_cq(struct napi_struct *napi, int budget)
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	int done;
 
-	if (!mlx4_en_cq_lock_napi(cq))
-		return budget;
-
 	done = mlx4_en_process_rx_cq(dev, cq, budget);
 
-	mlx4_en_cq_unlock_napi(cq);
-
 	/* If we used up all the quota - we're probably not done yet... */
 	if (done == budget) {
 		const struct cpumask *aff;
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index 4421bf5463f6..e0946ab22010 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -213,7 +213,9 @@ int mlx4_en_activate_tx_ring(struct mlx4_en_priv *priv,
 	mlx4_en_fill_qp_context(priv, ring->size, ring->stride, 1, 0, ring->qpn,
 				ring->cqn, user_prio, &ring->context);
 	if (ring->bf_alloced)
-		ring->context.usr_page = cpu_to_be32(ring->bf.uar->index);
+		ring->context.usr_page =
+			cpu_to_be32(mlx4_to_hw_uar_index(mdev->dev,
+							 ring->bf.uar->index));
 
 	err = mlx4_qp_to_ready(mdev->dev, &ring->wqres.mtt, &ring->context,
 			       &ring->qp, &ring->qp_state);
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 603d1c3d3b2e..f613977455e0 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -151,6 +151,17 @@ void mlx4_gen_slave_eqe(struct work_struct *work)
 	      eqe = next_slave_event_eqe(slave_eq)) {
 		slave = eqe->slave_id;
 
+		if (eqe->type == MLX4_EVENT_TYPE_PORT_CHANGE &&
+		    eqe->subtype == MLX4_PORT_CHANGE_SUBTYPE_DOWN &&
+		    mlx4_is_bonded(dev)) {
+			struct mlx4_port_cap port_cap;
+
+			if (!mlx4_QUERY_PORT(dev, 1, &port_cap) && port_cap.link_state)
+				goto consume;
+
+			if (!mlx4_QUERY_PORT(dev, 2, &port_cap) && port_cap.link_state)
+				goto consume;
+		}
 		/* All active slaves need to receive the event */
 		if (slave == ALL_SLAVES) {
 			for (i = 0; i <= dev->persist->num_vfs; i++) {
@@ -174,6 +185,7 @@ void mlx4_gen_slave_eqe(struct work_struct *work)
 				mlx4_warn(dev, "Failed to generate event for slave %d\n",
 					  slave);
 		}
+consume:
 		++slave_eq->cons;
 	}
 }
@@ -594,7 +606,9 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 					break;
 				for (i = 0; i < dev->persist->num_vfs + 1;
 				     i++) {
-					if (!test_bit(i, slaves_port.slaves))
+					int reported_port = mlx4_is_bonded(dev) ? 1 : mlx4_phys_to_slave_port(dev, i, port);
+
+					if (!test_bit(i, slaves_port.slaves) && !mlx4_is_bonded(dev))
 						continue;
 					if (dev->caps.port_type[port] == MLX4_PORT_TYPE_ETH) {
 						if (i == mlx4_master_func_num(dev))
@@ -606,7 +620,7 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 							eqe->event.port_change.port =
 								cpu_to_be32(
 								(be32_to_cpu(eqe->event.port_change.port) & 0xFFFFFFF)
-								| (mlx4_phys_to_slave_port(dev, i, port) << 28));
+								| (reported_port << 28));
 							mlx4_slave_event(dev, i, eqe);
 						}
 					} else {  /* IB port */
@@ -636,7 +650,9 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 					for (i = 0;
 					     i < dev->persist->num_vfs + 1;
 					     i++) {
-						if (!test_bit(i, slaves_port.slaves))
+						int reported_port = mlx4_is_bonded(dev) ? 1 : mlx4_phys_to_slave_port(dev, i, port);
+
+						if (!test_bit(i, slaves_port.slaves) && !mlx4_is_bonded(dev))
 							continue;
 						if (i == mlx4_master_func_num(dev))
 							continue;
@@ -645,7 +661,7 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 							eqe->event.port_change.port =
 								cpu_to_be32(
 								(be32_to_cpu(eqe->event.port_change.port) & 0xFFFFFFF)
-								| (mlx4_phys_to_slave_port(dev, i, port) << 28));
+								| (reported_port << 28));
 							mlx4_slave_event(dev, i, eqe);
 						}
 					}
@@ -924,9 +940,10 @@ static void __iomem *mlx4_get_eq_uar(struct mlx4_dev *dev, struct mlx4_eq *eq)
 
 	if (!priv->eq_table.uar_map[index]) {
 		priv->eq_table.uar_map[index] =
-			ioremap(pci_resource_start(dev->persist->pdev, 2) +
-				((eq->eqn / 4) << PAGE_SHIFT),
-				PAGE_SIZE);
+			ioremap(
+				pci_resource_start(dev->persist->pdev, 2) +
+				((eq->eqn / 4) << (dev->uar_page_shift)),
+				(1 << (dev->uar_page_shift)));
 		if (!priv->eq_table.uar_map[index]) {
 			mlx4_err(dev, "Couldn't map EQ doorbell for EQN 0x%06x\n",
 				 eq->eqn);
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index 90db94e83fde..d66c690a8597 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -157,6 +157,7 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 		[29] = "802.1ad offload support",
 		[31] = "Modifying loopback source checks using UPDATE_QP support",
 		[32] = "Loopback source checks support",
+		[33] = "RoCEv2 support"
 	};
 	int i;
 
@@ -626,6 +627,8 @@ out:
 	return err;
 }
 
+static void disable_unsupported_roce_caps(void *buf);
+
 int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 {
 	struct mlx4_cmd_mailbox *mailbox;
@@ -738,6 +741,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	if (err)
 		goto out;
 
+	if (mlx4_is_mfunc(dev))
+		disable_unsupported_roce_caps(outbox);
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_RSVD_QP_OFFSET);
 	dev_cap->reserved_qps = 1 << (field & 0xf);
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_MAX_QP_OFFSET);
@@ -905,6 +910,8 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_EQE_STRIDE;
 	MLX4_GET(dev_cap->bmme_flags, outbox,
 		 QUERY_DEV_CAP_BMME_FLAGS_OFFSET);
+	if (dev_cap->bmme_flags & MLX4_FLAG_ROCE_V1_V2)
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_ROCE_V1_V2;
 	if (dev_cap->bmme_flags & MLX4_FLAG_PORT_REMAP)
 		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_PORT_REMAP;
 	MLX4_GET(field, outbox, QUERY_DEV_CAP_CONFIG_DEV_OFFSET);
@@ -1104,6 +1111,7 @@ int mlx4_QUERY_PORT(struct mlx4_dev *dev, int port, struct mlx4_port_cap *port_c
 			goto out;
 
 		MLX4_GET(field, outbox, QUERY_PORT_SUPPORTED_TYPE_OFFSET);
+		port_cap->link_state = (field & 0x80) >> 7;
 		port_cap->supported_port_types = field & 3;
 		port_cap->suggested_type = (field >> 3) & 1;
 		port_cap->default_sense = (field >> 4) & 1;
@@ -1160,6 +1168,7 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	if (err)
 		return err;
 
+	disable_unsupported_roce_caps(outbox->buf);
 	/* add port mng change event capability and disable mw type 1
 	 * unconditionally to slaves
 	 */
@@ -1257,6 +1266,21 @@ int mlx4_QUERY_DEV_CAP_wrapper(struct mlx4_dev *dev, int slave,
 	return 0;
 }
 
+static void disable_unsupported_roce_caps(void *buf)
+{
+	u32 flags;
+
+	MLX4_GET(flags, buf, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
+	flags &= ~(1UL << 31);
+	MLX4_PUT(buf, flags, QUERY_DEV_CAP_EXT_FLAGS_OFFSET);
+	MLX4_GET(flags, buf, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
+	flags &= ~(1UL << 24);
+	MLX4_PUT(buf, flags, QUERY_DEV_CAP_EXT_2_FLAGS_OFFSET);
+	MLX4_GET(flags, buf, QUERY_DEV_CAP_BMME_FLAGS_OFFSET);
+	flags &= ~(MLX4_FLAG_ROCE_V1_V2);
+	MLX4_PUT(buf, flags, QUERY_DEV_CAP_BMME_FLAGS_OFFSET);
+}
+
 int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			    struct mlx4_vhcr *vhcr,
 			    struct mlx4_cmd_mailbox *inbox,
@@ -1310,6 +1334,15 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 			port_type |= MLX4_PORT_LINK_UP_MASK;
 		else if (IFLA_VF_LINK_STATE_DISABLE == admin_link_state)
 			port_type &= ~MLX4_PORT_LINK_UP_MASK;
+		else if (IFLA_VF_LINK_STATE_AUTO == admin_link_state && mlx4_is_bonded(dev)) {
+			int other_port = (port == 1) ? 2 : 1;
+			struct mlx4_port_cap port_cap;
+
+			err = mlx4_QUERY_PORT(dev, other_port, &port_cap);
+			if (err)
+				goto out;
+			port_type |= (port_cap.link_state << 7);
+		}
 
 		MLX4_PUT(outbox->buf, port_type,
 			 QUERY_PORT_SUPPORTED_TYPE_OFFSET);
@@ -1325,7 +1358,7 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 		MLX4_PUT(outbox->buf, short_field,
 			 QUERY_PORT_CUR_MAX_PKEY_OFFSET);
 	}
-
+out:
 	return err;
 }
 
@@ -2229,7 +2262,8 @@ struct mlx4_config_dev {
 	__be32	rsvd1[3];
 	__be16	vxlan_udp_dport;
 	__be16	rsvd2;
-	__be32	rsvd3;
+	__be16  roce_v2_entropy;
+	__be16  roce_v2_udp_dport;
 	__be32	roce_flags;
 	__be32	rsvd4[25];
 	__be16	rsvd5;
@@ -2238,6 +2272,7 @@ struct mlx4_config_dev {
 };
 
 #define MLX4_VXLAN_UDP_DPORT (1 << 0)
+#define MLX4_ROCE_V2_UDP_DPORT BIT(3)
 #define MLX4_DISABLE_RX_PORT BIT(18)
 
 static int mlx4_CONFIG_DEV_set(struct mlx4_dev *dev, struct mlx4_config_dev *config_dev)
@@ -2355,6 +2390,18 @@ int mlx4_disable_rx_port_check(struct mlx4_dev *dev, bool dis)
 	return mlx4_CONFIG_DEV_set(dev, &config_dev);
 }
 
+int mlx4_config_roce_v2_port(struct mlx4_dev *dev, u16 udp_port)
+{
+	struct mlx4_config_dev config_dev;
+
+	memset(&config_dev, 0, sizeof(config_dev));
+	config_dev.update_flags    = cpu_to_be32(MLX4_ROCE_V2_UDP_DPORT);
+	config_dev.roce_v2_udp_dport = cpu_to_be16(udp_port);
+
+	return mlx4_CONFIG_DEV_set(dev, &config_dev);
+}
+EXPORT_SYMBOL_GPL(mlx4_config_roce_v2_port);
+
 int mlx4_virt2phy_port_map(struct mlx4_dev *dev, u32 port1, u32 port2)
 {
 	struct mlx4_cmd_mailbox *mailbox;
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 08de5555c2f4..7ea258af636a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -44,6 +44,7 @@ struct mlx4_mod_stat_cfg {
 };
 
 struct mlx4_port_cap {
+	u8  link_state;
 	u8  supported_port_types;
 	u8  suggested_type;
 	u8  default_sense;
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 31c491e02e69..f8674ae62752 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -168,6 +168,20 @@ struct mlx4_port_config {
 
 static atomic_t pf_loading = ATOMIC_INIT(0);
 
+static inline void mlx4_set_num_reserved_uars(struct mlx4_dev *dev,
+					      struct mlx4_dev_cap *dev_cap)
+{
+	/* The reserved_uars is calculated by system page size unit.
+	 * Therefore, adjustment is added when the uar page size is less
+	 * than the system page size
+	 */
+	dev->caps.reserved_uars	=
+		max_t(int,
+		      mlx4_get_num_reserved_uar(dev),
+		      dev_cap->reserved_uars /
+			(1 << (PAGE_SHIFT - dev->uar_page_shift)));
+}
+
 int mlx4_check_port_params(struct mlx4_dev *dev,
 			   enum mlx4_port_type *port_type)
 {
@@ -386,8 +400,6 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.reserved_mtts      = dev_cap->reserved_mtts;
 	dev->caps.reserved_mrws	     = dev_cap->reserved_mrws;
 
-	/* The first 128 UARs are used for EQ doorbells */
-	dev->caps.reserved_uars	     = max_t(int, 128, dev_cap->reserved_uars);
 	dev->caps.reserved_pds	     = dev_cap->reserved_pds;
 	dev->caps.reserved_xrcds     = (dev->caps.flags & MLX4_DEV_CAP_FLAG_XRC) ?
 					dev_cap->reserved_xrcds : 0;
@@ -405,6 +417,15 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev->caps.max_gso_sz	     = dev_cap->max_gso_sz;
 	dev->caps.max_rss_tbl_sz     = dev_cap->max_rss_tbl_sz;
 
+	/* Save uar page shift */
+	if (!mlx4_is_slave(dev)) {
+		/* Virtual PCI function needs to determine UAR page size from
+		 * firmware. Only master PCI function can set the uar page size
+		 */
+		dev->uar_page_shift = DEFAULT_UAR_PAGE_SHIFT;
+		mlx4_set_num_reserved_uars(dev, dev_cap);
+	}
+
 	if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_PHV_EN) {
 		struct mlx4_init_hca_param hca_param;
 
@@ -815,16 +836,25 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 		return -ENODEV;
 	}
 
-	/* slave gets uar page size from QUERY_HCA fw command */
-	dev->caps.uar_page_size = 1 << (hca_param.uar_page_sz + 12);
+	/* Set uar_page_shift for VF */
+	dev->uar_page_shift = hca_param.uar_page_sz + 12;
 
-	/* TODO: relax this assumption */
-	if (dev->caps.uar_page_size != PAGE_SIZE) {
-		mlx4_err(dev, "UAR size:%d != kernel PAGE_SIZE of %ld\n",
-			 dev->caps.uar_page_size, PAGE_SIZE);
-		return -ENODEV;
+	/* Make sure the master uar page size is valid */
+	if (dev->uar_page_shift > PAGE_SHIFT) {
+		mlx4_err(dev,
+			 "Invalid configuration: uar page size is larger than system page size\n");
+		return  -ENODEV;
 	}
 
+	/* Set reserved_uars based on the uar_page_shift */
+	mlx4_set_num_reserved_uars(dev, &dev_cap);
+
+	/* Although uar page size in FW differs from system page size,
+	 * upper software layers (mlx4_ib, mlx4_en and part of mlx4_core)
+	 * still works with assumption that uar page size == system page size
+	 */
+	dev->caps.uar_page_size = PAGE_SIZE;
+
 	memset(&func_cap, 0, sizeof(func_cap));
 	err = mlx4_QUERY_FUNC_CAP(dev, 0, &func_cap);
 	if (err) {
@@ -1221,6 +1251,84 @@ err_set_port:
 	return err ? err : count;
 }
 
+/* bond for multi-function device */
+#define MAX_MF_BOND_ALLOWED_SLAVES 63
+static int mlx4_mf_bond(struct mlx4_dev *dev)
+{
+	int err = 0;
+	int nvfs;
+	struct mlx4_slaves_pport slaves_port1;
+	struct mlx4_slaves_pport slaves_port2;
+	DECLARE_BITMAP(slaves_port_1_2, MLX4_MFUNC_MAX);
+
+	slaves_port1 = mlx4_phys_to_slaves_pport(dev, 1);
+	slaves_port2 = mlx4_phys_to_slaves_pport(dev, 2);
+	bitmap_and(slaves_port_1_2,
+		   slaves_port1.slaves, slaves_port2.slaves,
+		   dev->persist->num_vfs + 1);
+
+	/* only single port vfs are allowed */
+	if (bitmap_weight(slaves_port_1_2, dev->persist->num_vfs + 1) > 1) {
+		mlx4_warn(dev, "HA mode unsupported for dual ported VFs\n");
+		return -EINVAL;
+	}
+
+	/* number of virtual functions is number of total functions minus one
+	 * physical function for each port.
+	 */
+	nvfs = bitmap_weight(slaves_port1.slaves, dev->persist->num_vfs + 1) +
+		bitmap_weight(slaves_port2.slaves, dev->persist->num_vfs + 1) - 2;
+
+	/* limit on maximum allowed VFs */
+	if (nvfs > MAX_MF_BOND_ALLOWED_SLAVES) {
+		mlx4_warn(dev, "HA mode is not supported for %d VFs (max %d are allowed)\n",
+			  nvfs, MAX_MF_BOND_ALLOWED_SLAVES);
+		return -EINVAL;
+	}
+
+	if (dev->caps.steering_mode != MLX4_STEERING_MODE_DEVICE_MANAGED) {
+		mlx4_warn(dev, "HA mode unsupported for NON DMFS steering\n");
+		return -EINVAL;
+	}
+
+	err = mlx4_bond_mac_table(dev);
+	if (err)
+		return err;
+	err = mlx4_bond_vlan_table(dev);
+	if (err)
+		goto err1;
+	err = mlx4_bond_fs_rules(dev);
+	if (err)
+		goto err2;
+
+	return 0;
+err2:
+	(void)mlx4_unbond_vlan_table(dev);
+err1:
+	(void)mlx4_unbond_mac_table(dev);
+	return err;
+}
+
+static int mlx4_mf_unbond(struct mlx4_dev *dev)
+{
+	int ret, ret1;
+
+	ret = mlx4_unbond_fs_rules(dev);
+	if (ret)
+		mlx4_warn(dev, "multifunction unbond for flow rules failedi (%d)\n", ret);
+	ret1 = mlx4_unbond_mac_table(dev);
+	if (ret1) {
+		mlx4_warn(dev, "multifunction unbond for MAC table failed (%d)\n", ret1);
+		ret = ret1;
+	}
+	ret1 = mlx4_unbond_vlan_table(dev);
+	if (ret1) {
+		mlx4_warn(dev, "multifunction unbond for VLAN table failed (%d)\n", ret1);
+		ret = ret1;
+	}
+	return ret;
+}
+
 int mlx4_bond(struct mlx4_dev *dev)
 {
 	int ret = 0;
@@ -1228,16 +1336,23 @@ int mlx4_bond(struct mlx4_dev *dev)
 
 	mutex_lock(&priv->bond_mutex);
 
-	if (!mlx4_is_bonded(dev))
+	if (!mlx4_is_bonded(dev)) {
 		ret = mlx4_do_bond(dev, true);
-	else
-		ret = 0;
+		if (ret)
+			mlx4_err(dev, "Failed to bond device: %d\n", ret);
+		if (!ret && mlx4_is_master(dev)) {
+			ret = mlx4_mf_bond(dev);
+			if (ret) {
+				mlx4_err(dev, "bond for multifunction failed\n");
+				mlx4_do_bond(dev, false);
+			}
+		}
+	}
 
 	mutex_unlock(&priv->bond_mutex);
-	if (ret)
-		mlx4_err(dev, "Failed to bond device: %d\n", ret);
-	else
+	if (!ret)
 		mlx4_dbg(dev, "Device is bonded\n");
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(mlx4_bond);
@@ -1249,14 +1364,24 @@ int mlx4_unbond(struct mlx4_dev *dev)
 
 	mutex_lock(&priv->bond_mutex);
 
-	if (mlx4_is_bonded(dev))
+	if (mlx4_is_bonded(dev)) {
+		int ret2 = 0;
+
 		ret = mlx4_do_bond(dev, false);
+		if (ret)
+			mlx4_err(dev, "Failed to unbond device: %d\n", ret);
+		if (mlx4_is_master(dev))
+			ret2 = mlx4_mf_unbond(dev);
+		if (ret2) {
+			mlx4_warn(dev, "Failed to unbond device for multifunction (%d)\n", ret2);
+			ret = ret2;
+		}
+	}
 
 	mutex_unlock(&priv->bond_mutex);
-	if (ret)
-		mlx4_err(dev, "Failed to unbond device: %d\n", ret);
-	else
+	if (!ret)
 		mlx4_dbg(dev, "Device is unbonded\n");
+
 	return ret;
 }
 EXPORT_SYMBOL_GPL(mlx4_unbond);
@@ -2092,8 +2217,12 @@ static int mlx4_init_hca(struct mlx4_dev *dev)
 
 		dev->caps.max_fmr_maps = (1 << (32 - ilog2(dev->caps.num_mpts))) - 1;
 
-		init_hca.log_uar_sz = ilog2(dev->caps.num_uars);
-		init_hca.uar_page_sz = PAGE_SHIFT - 12;
+		/* Always set UAR page size 4KB, set log_uar_sz accordingly */
+		init_hca.log_uar_sz = ilog2(dev->caps.num_uars) +
+				      PAGE_SHIFT -
+				      DEFAULT_UAR_PAGE_SHIFT;
+		init_hca.uar_page_sz = DEFAULT_UAR_PAGE_SHIFT - 12;
+
 		init_hca.mw_enabled = 0;
 		if (dev->caps.flags & MLX4_DEV_CAP_FLAG_MEM_WINDOW ||
 		    dev->caps.bmme_flags & MLX4_BMME_FLAG_TYPE_2_WIN)
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index e1cf9036af22..7baef52db6b7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -736,6 +736,7 @@ struct mlx4_catas_err {
 struct mlx4_mac_table {
 	__be64			entries[MLX4_MAX_MAC_NUM];
 	int			refs[MLX4_MAX_MAC_NUM];
+	bool			is_dup[MLX4_MAX_MAC_NUM];
 	struct mutex		mutex;
 	int			total;
 	int			max;
@@ -758,6 +759,7 @@ struct mlx4_roce_gid_table {
 struct mlx4_vlan_table {
 	__be32			entries[MLX4_MAX_VLAN_NUM];
 	int			refs[MLX4_MAX_VLAN_NUM];
+	int			is_dup[MLX4_MAX_VLAN_NUM];
 	struct mutex		mutex;
 	int			total;
 	int			max;
@@ -778,7 +780,10 @@ struct mlx4_set_port_general_context {
 	u16 reserved1;
 	u8 v_ignore_fcs;
 	u8 flags;
-	u8 ignore_fcs;
+	union {
+		u8 ignore_fcs;
+		u8 roce_mode;
+	};
 	u8 reserved2;
 	__be16 mtu;
 	u8 pptx;
@@ -1225,6 +1230,10 @@ void mlx4_init_roce_gid_table(struct mlx4_dev *dev,
 			      struct mlx4_roce_gid_table *table);
 void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan);
 int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan, int *index);
+int mlx4_bond_vlan_table(struct mlx4_dev *dev);
+int mlx4_unbond_vlan_table(struct mlx4_dev *dev);
+int mlx4_bond_mac_table(struct mlx4_dev *dev);
+int mlx4_unbond_mac_table(struct mlx4_dev *dev);
 
 int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz);
 /* resource tracker functions*/
@@ -1385,6 +1394,8 @@ int mlx4_get_slave_num_gids(struct mlx4_dev *dev, int slave, int port);
 int mlx4_get_vf_indx(struct mlx4_dev *dev, int slave);
 int mlx4_config_mad_demux(struct mlx4_dev *dev);
 int mlx4_do_bond(struct mlx4_dev *dev, bool enable);
+int mlx4_bond_fs_rules(struct mlx4_dev *dev);
+int mlx4_unbond_fs_rules(struct mlx4_dev *dev);
 
 enum mlx4_zone_flags {
 	MLX4_ZONE_ALLOW_ALLOC_FROM_LOWER_PRIO	= 1UL << 0,
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index c41f15102ae0..35de7d2e6b34 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -320,11 +320,6 @@ struct mlx4_en_rx_ring {
 	void *rx_info;
 	unsigned long bytes;
 	unsigned long packets;
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	unsigned long yields;
-	unsigned long misses;
-	unsigned long cleaned;
-#endif
 	unsigned long csum_ok;
 	unsigned long csum_none;
 	unsigned long csum_complete;
@@ -347,18 +342,6 @@ struct mlx4_en_cq {
 	struct mlx4_cqe *buf;
 #define MLX4_EN_OPCODE_ERROR	0x1e
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-	unsigned int state;
-#define MLX4_EN_CQ_STATE_IDLE        0
-#define MLX4_EN_CQ_STATE_NAPI     1    /* NAPI owns this CQ */
-#define MLX4_EN_CQ_STATE_POLL     2    /* poll owns this CQ */
-#define MLX4_CQ_LOCKED (MLX4_EN_CQ_STATE_NAPI | MLX4_EN_CQ_STATE_POLL)
-#define MLX4_EN_CQ_STATE_NAPI_YIELD  4    /* NAPI yielded this CQ */
-#define MLX4_EN_CQ_STATE_POLL_YIELD  8    /* poll yielded this CQ */
-#define CQ_YIELD (MLX4_EN_CQ_STATE_NAPI_YIELD | MLX4_EN_CQ_STATE_POLL_YIELD)
-#define CQ_USER_PEND (MLX4_EN_CQ_STATE_POLL | MLX4_EN_CQ_STATE_POLL_YIELD)
-	spinlock_t poll_lock; /* protects from LLS/napi conflicts */
-#endif  /* CONFIG_NET_RX_BUSY_POLL */
 	struct irq_desc *irq_desc;
 };
 
@@ -622,115 +605,6 @@ static inline struct mlx4_cqe *mlx4_en_get_cqe(void *buf, int idx, int cqe_sz)
 	return buf + idx * cqe_sz;
 }
 
-#ifdef CONFIG_NET_RX_BUSY_POLL
-static inline void mlx4_en_cq_init_lock(struct mlx4_en_cq *cq)
-{
-	spin_lock_init(&cq->poll_lock);
-	cq->state = MLX4_EN_CQ_STATE_IDLE;
-}
-
-/* called from the device poll rutine to get ownership of a cq */
-static inline bool mlx4_en_cq_lock_napi(struct mlx4_en_cq *cq)
-{
-	int rc = true;
-	spin_lock(&cq->poll_lock);
-	if (cq->state & MLX4_CQ_LOCKED) {
-		WARN_ON(cq->state & MLX4_EN_CQ_STATE_NAPI);
-		cq->state |= MLX4_EN_CQ_STATE_NAPI_YIELD;
-		rc = false;
-	} else
-		/* we don't care if someone yielded */
-		cq->state = MLX4_EN_CQ_STATE_NAPI;
-	spin_unlock(&cq->poll_lock);
-	return rc;
-}
-
-/* returns true is someone tried to get the cq while napi had it */
-static inline bool mlx4_en_cq_unlock_napi(struct mlx4_en_cq *cq)
-{
-	int rc = false;
-	spin_lock(&cq->poll_lock);
-	WARN_ON(cq->state & (MLX4_EN_CQ_STATE_POLL |
-			       MLX4_EN_CQ_STATE_NAPI_YIELD));
-
-	if (cq->state & MLX4_EN_CQ_STATE_POLL_YIELD)
-		rc = true;
-	cq->state = MLX4_EN_CQ_STATE_IDLE;
-	spin_unlock(&cq->poll_lock);
-	return rc;
-}
-
-/* called from mlx4_en_low_latency_poll() */
-static inline bool mlx4_en_cq_lock_poll(struct mlx4_en_cq *cq)
-{
-	int rc = true;
-	spin_lock_bh(&cq->poll_lock);
-	if ((cq->state & MLX4_CQ_LOCKED)) {
-		struct net_device *dev = cq->dev;
-		struct mlx4_en_priv *priv = netdev_priv(dev);
-		struct mlx4_en_rx_ring *rx_ring = priv->rx_ring[cq->ring];
-
-		cq->state |= MLX4_EN_CQ_STATE_POLL_YIELD;
-		rc = false;
-		rx_ring->yields++;
-	} else
-		/* preserve yield marks */
-		cq->state |= MLX4_EN_CQ_STATE_POLL;
-	spin_unlock_bh(&cq->poll_lock);
-	return rc;
-}
-
-/* returns true if someone tried to get the cq while it was locked */
-static inline bool mlx4_en_cq_unlock_poll(struct mlx4_en_cq *cq)
-{
-	int rc = false;
-	spin_lock_bh(&cq->poll_lock);
-	WARN_ON(cq->state & (MLX4_EN_CQ_STATE_NAPI));
-
-	if (cq->state & MLX4_EN_CQ_STATE_POLL_YIELD)
-		rc = true;
-	cq->state = MLX4_EN_CQ_STATE_IDLE;
-	spin_unlock_bh(&cq->poll_lock);
-	return rc;
-}
-
-/* true if a socket is polling, even if it did not get the lock */
-static inline bool mlx4_en_cq_busy_polling(struct mlx4_en_cq *cq)
-{
-	WARN_ON(!(cq->state & MLX4_CQ_LOCKED));
-	return cq->state & CQ_USER_PEND;
-}
-#else
-static inline void mlx4_en_cq_init_lock(struct mlx4_en_cq *cq)
-{
-}
-
-static inline bool mlx4_en_cq_lock_napi(struct mlx4_en_cq *cq)
-{
-	return true;
-}
-
-static inline bool mlx4_en_cq_unlock_napi(struct mlx4_en_cq *cq)
-{
-	return false;
-}
-
-static inline bool mlx4_en_cq_lock_poll(struct mlx4_en_cq *cq)
-{
-	return false;
-}
-
-static inline bool mlx4_en_cq_unlock_poll(struct mlx4_en_cq *cq)
-{
-	return false;
-}
-
-static inline bool mlx4_en_cq_busy_polling(struct mlx4_en_cq *cq)
-{
-	return false;
-}
-#endif /* CONFIG_NET_RX_BUSY_POLL */
-
 #define MLX4_EN_WOL_DO_MODIFY (1ULL << 63)
 
 void mlx4_en_update_loopback_state(struct net_device *dev,
diff --git a/drivers/net/ethernet/mellanox/mlx4/pd.c b/drivers/net/ethernet/mellanox/mlx4/pd.c
index 609c59dc854e..b3cc3ab63799 100644
--- a/drivers/net/ethernet/mellanox/mlx4/pd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/pd.c
@@ -269,9 +269,15 @@ EXPORT_SYMBOL_GPL(mlx4_bf_free);
 
 int mlx4_init_uar_table(struct mlx4_dev *dev)
 {
-	if (dev->caps.num_uars <= 128) {
-		mlx4_err(dev, "Only %d UAR pages (need more than 128)\n",
-			 dev->caps.num_uars);
+	int num_reserved_uar = mlx4_get_num_reserved_uar(dev);
+
+	mlx4_dbg(dev, "uar_page_shift = %d", dev->uar_page_shift);
+	mlx4_dbg(dev, "Effective reserved_uars=%d", dev->caps.reserved_uars);
+
+	if (dev->caps.num_uars <= num_reserved_uar) {
+		mlx4_err(
+			dev, "Only %d UAR pages (need more than %d)\n",
+			dev->caps.num_uars, num_reserved_uar);
 		mlx4_err(dev, "Increase firmware log2_uar_bar_megabytes?\n");
 		return -ENODEV;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index c2b21313dba7..211c65087997 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -61,6 +61,7 @@ void mlx4_init_mac_table(struct mlx4_dev *dev, struct mlx4_mac_table *table)
 	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
 		table->entries[i] = 0;
 		table->refs[i]	 = 0;
+		table->is_dup[i] = false;
 	}
 	table->max   = 1 << dev->caps.log_num_macs;
 	table->total = 0;
@@ -74,6 +75,7 @@ void mlx4_init_vlan_table(struct mlx4_dev *dev, struct mlx4_vlan_table *table)
 	for (i = 0; i < MLX4_MAX_VLAN_NUM; i++) {
 		table->entries[i] = 0;
 		table->refs[i]	 = 0;
+		table->is_dup[i] = false;
 	}
 	table->max   = (1 << dev->caps.log_num_vlans) - MLX4_VLAN_REGULAR;
 	table->total = 0;
@@ -159,21 +161,94 @@ int mlx4_find_cached_mac(struct mlx4_dev *dev, u8 port, u64 mac, int *idx)
 }
 EXPORT_SYMBOL_GPL(mlx4_find_cached_mac);
 
+static bool mlx4_need_mf_bond(struct mlx4_dev *dev)
+{
+	int i, num_eth_ports = 0;
+
+	if (!mlx4_is_mfunc(dev))
+		return false;
+	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_ETH)
+		++num_eth_ports;
+
+	return (num_eth_ports ==  2) ? true : false;
+}
+
 int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 {
 	struct mlx4_port_info *info = &mlx4_priv(dev)->port[port];
 	struct mlx4_mac_table *table = &info->mac_table;
 	int i, err = 0;
 	int free = -1;
+	int free_for_dup = -1;
+	bool dup = mlx4_is_mf_bonded(dev);
+	u8 dup_port = (port == 1) ? 2 : 1;
+	struct mlx4_mac_table *dup_table = &mlx4_priv(dev)->port[dup_port].mac_table;
+	bool need_mf_bond = mlx4_need_mf_bond(dev);
+	bool can_mf_bond = true;
+
+	mlx4_dbg(dev, "Registering MAC: 0x%llx for port %d %s duplicate\n",
+		 (unsigned long long)mac, port,
+		 dup ? "with" : "without");
+
+	if (need_mf_bond) {
+		if (port == 1) {
+			mutex_lock(&table->mutex);
+			mutex_lock_nested(&dup_table->mutex, SINGLE_DEPTH_NESTING);
+		} else {
+			mutex_lock(&dup_table->mutex);
+			mutex_lock_nested(&table->mutex, SINGLE_DEPTH_NESTING);
+		}
+	} else {
+		mutex_lock(&table->mutex);
+	}
+
+	if (need_mf_bond) {
+		int index_at_port = -1;
+		int index_at_dup_port = -1;
 
-	mlx4_dbg(dev, "Registering MAC: 0x%llx for port %d\n",
-		 (unsigned long long) mac, port);
+		for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
+			if (((MLX4_MAC_MASK & mac) == (MLX4_MAC_MASK & be64_to_cpu(table->entries[i]))))
+				index_at_port = i;
+			if (((MLX4_MAC_MASK & mac) == (MLX4_MAC_MASK & be64_to_cpu(dup_table->entries[i]))))
+				index_at_dup_port = i;
+		}
+
+		/* check that same mac is not in the tables at different indices */
+		if ((index_at_port != index_at_dup_port) &&
+		    (index_at_port >= 0) &&
+		    (index_at_dup_port >= 0))
+			can_mf_bond = false;
+
+		/* If the mac is already in the primary table, the slot must be
+		 * available in the duplicate table as well.
+		 */
+		if (index_at_port >= 0 && index_at_dup_port < 0 &&
+		    dup_table->refs[index_at_port]) {
+			can_mf_bond = false;
+		}
+		/* If the mac is already in the duplicate table, check that the
+		 * corresponding index is not occupied in the primary table, or
+		 * the primary table already contains the mac at the same index.
+		 * Otherwise, you cannot bond (primary contains a different mac
+		 * at that index).
+		 */
+		if (index_at_dup_port >= 0) {
+			if (!table->refs[index_at_dup_port] ||
+			    ((MLX4_MAC_MASK & mac) == (MLX4_MAC_MASK & be64_to_cpu(table->entries[index_at_dup_port]))))
+				free_for_dup = index_at_dup_port;
+			else
+				can_mf_bond = false;
+		}
+	}
 
-	mutex_lock(&table->mutex);
 	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
 		if (!table->refs[i]) {
 			if (free < 0)
 				free = i;
+			if (free_for_dup < 0 && need_mf_bond && can_mf_bond) {
+				if (!dup_table->refs[i])
+					free_for_dup = i;
+			}
 			continue;
 		}
 
@@ -182,10 +257,30 @@ int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 			/* MAC already registered, increment ref count */
 			err = i;
 			++table->refs[i];
+			if (dup) {
+				u64 dup_mac = MLX4_MAC_MASK & be64_to_cpu(dup_table->entries[i]);
+
+				if (dup_mac != mac || !dup_table->is_dup[i]) {
+					mlx4_warn(dev, "register mac: expect duplicate mac 0x%llx on port %d index %d\n",
+						  mac, dup_port, i);
+				}
+			}
 			goto out;
 		}
 	}
 
+	if (need_mf_bond && (free_for_dup < 0)) {
+		if (dup) {
+			mlx4_warn(dev, "Fail to allocate duplicate MAC table entry\n");
+			mlx4_warn(dev, "High Availability for virtual functions may not work as expected\n");
+			dup = false;
+		}
+		can_mf_bond = false;
+	}
+
+	if (need_mf_bond && can_mf_bond)
+		free = free_for_dup;
+
 	mlx4_dbg(dev, "Free MAC index is %d\n", free);
 
 	if (table->total == table->max) {
@@ -205,10 +300,35 @@ int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 		goto out;
 	}
 	table->refs[free] = 1;
-	err = free;
+	table->is_dup[free] = false;
 	++table->total;
+	if (dup) {
+		dup_table->refs[free] = 0;
+		dup_table->is_dup[free] = true;
+		dup_table->entries[free] = cpu_to_be64(mac | MLX4_MAC_VALID);
+
+		err = mlx4_set_port_mac_table(dev, dup_port, dup_table->entries);
+		if (unlikely(err)) {
+			mlx4_warn(dev, "Failed adding duplicate mac: 0x%llx\n", mac);
+			dup_table->is_dup[free] = false;
+			dup_table->entries[free] = 0;
+			goto out;
+		}
+		++dup_table->total;
+	}
+	err = free;
 out:
-	mutex_unlock(&table->mutex);
+	if (need_mf_bond) {
+		if (port == 2) {
+			mutex_unlock(&table->mutex);
+			mutex_unlock(&dup_table->mutex);
+		} else {
+			mutex_unlock(&dup_table->mutex);
+			mutex_unlock(&table->mutex);
+		}
+	} else {
+		mutex_unlock(&table->mutex);
+	}
 	return err;
 }
 EXPORT_SYMBOL_GPL(__mlx4_register_mac);
@@ -255,6 +375,9 @@ void __mlx4_unregister_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 	struct mlx4_port_info *info;
 	struct mlx4_mac_table *table;
 	int index;
+	bool dup = mlx4_is_mf_bonded(dev);
+	u8 dup_port = (port == 1) ? 2 : 1;
+	struct mlx4_mac_table *dup_table = &mlx4_priv(dev)->port[dup_port].mac_table;
 
 	if (port < 1 || port > dev->caps.num_ports) {
 		mlx4_warn(dev, "invalid port number (%d), aborting...\n", port);
@@ -262,22 +385,59 @@ void __mlx4_unregister_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 	}
 	info = &mlx4_priv(dev)->port[port];
 	table = &info->mac_table;
-	mutex_lock(&table->mutex);
+
+	if (dup) {
+		if (port == 1) {
+			mutex_lock(&table->mutex);
+			mutex_lock_nested(&dup_table->mutex, SINGLE_DEPTH_NESTING);
+		} else {
+			mutex_lock(&dup_table->mutex);
+			mutex_lock_nested(&table->mutex, SINGLE_DEPTH_NESTING);
+		}
+	} else {
+		mutex_lock(&table->mutex);
+	}
+
 	index = find_index(dev, table, mac);
 
 	if (validate_index(dev, table, index))
 		goto out;
-	if (--table->refs[index]) {
+
+	if (--table->refs[index] || table->is_dup[index]) {
 		mlx4_dbg(dev, "Have more references for index %d, no need to modify mac table\n",
 			 index);
+		if (!table->refs[index])
+			dup_table->is_dup[index] = false;
 		goto out;
 	}
 
 	table->entries[index] = 0;
-	mlx4_set_port_mac_table(dev, port, table->entries);
+	if (mlx4_set_port_mac_table(dev, port, table->entries))
+		mlx4_warn(dev, "Fail to set mac in port %d during unregister\n", port);
 	--table->total;
+
+	if (dup) {
+		dup_table->is_dup[index] = false;
+		if (dup_table->refs[index])
+			goto out;
+		dup_table->entries[index] = 0;
+		if (mlx4_set_port_mac_table(dev, dup_port, dup_table->entries))
+			mlx4_warn(dev, "Fail to set mac in duplicate port %d during unregister\n", dup_port);
+
+		--table->total;
+	}
 out:
-	mutex_unlock(&table->mutex);
+	if (dup) {
+		if (port == 2) {
+			mutex_unlock(&table->mutex);
+			mutex_unlock(&dup_table->mutex);
+		} else {
+			mutex_unlock(&dup_table->mutex);
+			mutex_unlock(&table->mutex);
+		}
+	} else {
+		mutex_unlock(&table->mutex);
+	}
 }
 EXPORT_SYMBOL_GPL(__mlx4_unregister_mac);
 
@@ -311,9 +471,22 @@ int __mlx4_replace_mac(struct mlx4_dev *dev, u8 port, int qpn, u64 new_mac)
 	struct mlx4_mac_table *table = &info->mac_table;
 	int index = qpn - info->base_qpn;
 	int err = 0;
+	bool dup = mlx4_is_mf_bonded(dev);
+	u8 dup_port = (port == 1) ? 2 : 1;
+	struct mlx4_mac_table *dup_table = &mlx4_priv(dev)->port[dup_port].mac_table;
 
 	/* CX1 doesn't support multi-functions */
-	mutex_lock(&table->mutex);
+	if (dup) {
+		if (port == 1) {
+			mutex_lock(&table->mutex);
+			mutex_lock_nested(&dup_table->mutex, SINGLE_DEPTH_NESTING);
+		} else {
+			mutex_lock(&dup_table->mutex);
+			mutex_lock_nested(&table->mutex, SINGLE_DEPTH_NESTING);
+		}
+	} else {
+		mutex_lock(&table->mutex);
+	}
 
 	err = validate_index(dev, table, index);
 	if (err)
@@ -326,9 +499,30 @@ int __mlx4_replace_mac(struct mlx4_dev *dev, u8 port, int qpn, u64 new_mac)
 		mlx4_err(dev, "Failed adding MAC: 0x%llx\n",
 			 (unsigned long long) new_mac);
 		table->entries[index] = 0;
+	} else {
+		if (dup) {
+			dup_table->entries[index] = cpu_to_be64(new_mac | MLX4_MAC_VALID);
+
+			err = mlx4_set_port_mac_table(dev, dup_port, dup_table->entries);
+			if (unlikely(err)) {
+				mlx4_err(dev, "Failed adding duplicate MAC: 0x%llx\n",
+					 (unsigned long long)new_mac);
+				dup_table->entries[index] = 0;
+			}
+		}
 	}
 out:
-	mutex_unlock(&table->mutex);
+	if (dup) {
+		if (port == 2) {
+			mutex_unlock(&table->mutex);
+			mutex_unlock(&dup_table->mutex);
+		} else {
+			mutex_unlock(&dup_table->mutex);
+			mutex_unlock(&table->mutex);
+		}
+	} else {
+		mutex_unlock(&table->mutex);
+	}
 	return err;
 }
 EXPORT_SYMBOL_GPL(__mlx4_replace_mac);
@@ -380,8 +574,28 @@ int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
 	struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;
 	int i, err = 0;
 	int free = -1;
-
-	mutex_lock(&table->mutex);
+	int free_for_dup = -1;
+	bool dup = mlx4_is_mf_bonded(dev);
+	u8 dup_port = (port == 1) ? 2 : 1;
+	struct mlx4_vlan_table *dup_table = &mlx4_priv(dev)->port[dup_port].vlan_table;
+	bool need_mf_bond = mlx4_need_mf_bond(dev);
+	bool can_mf_bond = true;
+
+	mlx4_dbg(dev, "Registering VLAN: %d for port %d %s duplicate\n",
+		 vlan, port,
+		 dup ? "with" : "without");
+
+	if (need_mf_bond) {
+		if (port == 1) {
+			mutex_lock(&table->mutex);
+			mutex_lock_nested(&dup_table->mutex, SINGLE_DEPTH_NESTING);
+		} else {
+			mutex_lock(&dup_table->mutex);
+			mutex_lock_nested(&table->mutex, SINGLE_DEPTH_NESTING);
+		}
+	} else {
+		mutex_lock(&table->mutex);
+	}
 
 	if (table->total == table->max) {
 		/* No free vlan entries */
@@ -389,22 +603,85 @@ int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
 		goto out;
 	}
 
+	if (need_mf_bond) {
+		int index_at_port = -1;
+		int index_at_dup_port = -1;
+
+		for (i = MLX4_VLAN_REGULAR; i < MLX4_MAX_VLAN_NUM; i++) {
+			if ((vlan == (MLX4_VLAN_MASK & be32_to_cpu(table->entries[i]))))
+				index_at_port = i;
+			if ((vlan == (MLX4_VLAN_MASK & be32_to_cpu(dup_table->entries[i]))))
+				index_at_dup_port = i;
+		}
+		/* check that same vlan is not in the tables at different indices */
+		if ((index_at_port != index_at_dup_port) &&
+		    (index_at_port >= 0) &&
+		    (index_at_dup_port >= 0))
+			can_mf_bond = false;
+
+		/* If the vlan is already in the primary table, the slot must be
+		 * available in the duplicate table as well.
+		 */
+		if (index_at_port >= 0 && index_at_dup_port < 0 &&
+		    dup_table->refs[index_at_port]) {
+			can_mf_bond = false;
+		}
+		/* If the vlan is already in the duplicate table, check that the
+		 * corresponding index is not occupied in the primary table, or
+		 * the primary table already contains the vlan at the same index.
+		 * Otherwise, you cannot bond (primary contains a different vlan
+		 * at that index).
+		 */
+		if (index_at_dup_port >= 0) {
+			if (!table->refs[index_at_dup_port] ||
+			    (vlan == (MLX4_VLAN_MASK & be32_to_cpu(dup_table->entries[index_at_dup_port]))))
+				free_for_dup = index_at_dup_port;
+			else
+				can_mf_bond = false;
+		}
+	}
+
 	for (i = MLX4_VLAN_REGULAR; i < MLX4_MAX_VLAN_NUM; i++) {
-		if (free < 0 && (table->refs[i] == 0)) {
-			free = i;
-			continue;
+		if (!table->refs[i]) {
+			if (free < 0)
+				free = i;
+			if (free_for_dup < 0 && need_mf_bond && can_mf_bond) {
+				if (!dup_table->refs[i])
+					free_for_dup = i;
+			}
 		}
 
-		if (table->refs[i] &&
+		if ((table->refs[i] || table->is_dup[i]) &&
 		    (vlan == (MLX4_VLAN_MASK &
 			      be32_to_cpu(table->entries[i])))) {
 			/* Vlan already registered, increase references count */
+			mlx4_dbg(dev, "vlan %u is already registered.\n", vlan);
 			*index = i;
 			++table->refs[i];
+			if (dup) {
+				u16 dup_vlan = MLX4_VLAN_MASK & be32_to_cpu(dup_table->entries[i]);
+
+				if (dup_vlan != vlan || !dup_table->is_dup[i]) {
+					mlx4_warn(dev, "register vlan: expected duplicate vlan %u on port %d index %d\n",
+						  vlan, dup_port, i);
+				}
+			}
 			goto out;
 		}
 	}
 
+	if (need_mf_bond && (free_for_dup < 0)) {
+		if (dup) {
+			mlx4_warn(dev, "Fail to allocate duplicate VLAN table entry\n");
+			mlx4_warn(dev, "High Availability for virtual functions may not work as expected\n");
+			dup = false;
+		}
+		can_mf_bond = false;
+	}
+
+	if (need_mf_bond && can_mf_bond)
+		free = free_for_dup;
+
 	if (free < 0) {
 		err = -ENOMEM;
 		goto out;
@@ -412,6 +689,7 @@ int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
 
 	/* Register new VLAN */
 	table->refs[free] = 1;
+	table->is_dup[free] = false;
 	table->entries[free] = cpu_to_be32(vlan | MLX4_VLAN_VALID);
 
 	err = mlx4_set_port_vlan_table(dev, port, table->entries);
@@ -421,11 +699,35 @@ int __mlx4_register_vlan(struct mlx4_dev *dev, u8 port, u16 vlan,
 		table->entries[free] = 0;
 		goto out;
 	}
+	++table->total;
+	if (dup) {
+		dup_table->refs[free] = 0;
+		dup_table->is_dup[free] = true;
+		dup_table->entries[free] = cpu_to_be32(vlan | MLX4_VLAN_VALID);
+
+		err = mlx4_set_port_vlan_table(dev, dup_port, dup_table->entries);
+		if (unlikely(err)) {
+			mlx4_warn(dev, "Failed adding duplicate vlan: %u\n", vlan);
+			dup_table->is_dup[free] = false;
+			dup_table->entries[free] = 0;
+			goto out;
+		}
+		++dup_table->total;
+	}
 
 	*index = free;
-	++table->total;
 out:
-	mutex_unlock(&table->mutex);
+	if (need_mf_bond) {
+		if (port == 2) {
+			mutex_unlock(&table->mutex);
+			mutex_unlock(&dup_table->mutex);
+		} else {
+			mutex_unlock(&dup_table->mutex);
+			mutex_unlock(&table->mutex);
+		}
+	} else {
+		mutex_unlock(&table->mutex);
+	}
 	return err;
 }
 
@@ -455,8 +757,22 @@ void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan)
 {
 	struct mlx4_vlan_table *table = &mlx4_priv(dev)->port[port].vlan_table;
 	int index;
+	bool dup = mlx4_is_mf_bonded(dev);
+	u8 dup_port = (port == 1) ? 2 : 1;
+	struct mlx4_vlan_table *dup_table = &mlx4_priv(dev)->port[dup_port].vlan_table;
+
+	if (dup) {
+		if (port == 1) {
+			mutex_lock(&table->mutex);
+			mutex_lock_nested(&dup_table->mutex, SINGLE_DEPTH_NESTING);
+		} else {
+			mutex_lock(&dup_table->mutex);
+			mutex_lock_nested(&table->mutex, SINGLE_DEPTH_NESTING);
+		}
+	} else {
+		mutex_lock(&table->mutex);
+	}
 
-	mutex_lock(&table->mutex);
 	if (mlx4_find_cached_vlan(dev, port, vlan, &index)) {
 		mlx4_warn(dev, "vlan 0x%x is not in the vlan table\n", vlan);
 		goto out;
@@ -467,16 +783,38 @@ void __mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan)
 		goto out;
 	}
 
-	if (--table->refs[index]) {
+	if (--table->refs[index] || table->is_dup[index]) {
 		mlx4_dbg(dev, "Have %d more references for index %d, no need to modify vlan table\n",
 			 table->refs[index], index);
+		if (!table->refs[index])
+			dup_table->is_dup[index] = false;
 		goto out;
 	}
 	table->entries[index] = 0;
-	mlx4_set_port_vlan_table(dev, port, table->entries);
+	if (mlx4_set_port_vlan_table(dev, port, table->entries))
+		mlx4_warn(dev, "Fail to set vlan in port %d during unregister\n", port);
 	--table->total;
+	if (dup) {
+		dup_table->is_dup[index] = false;
+		if (dup_table->refs[index])
+			goto out;
+		dup_table->entries[index] = 0;
+		if (mlx4_set_port_vlan_table(dev, dup_port, dup_table->entries))
+			mlx4_warn(dev, "Fail to set vlan in duplicate port %d during unregister\n", dup_port);
+		--dup_table->total;
+	}
 out:
-	mutex_unlock(&table->mutex);
+	if (dup) {
+		if (port == 2) {
+			mutex_unlock(&table->mutex);
+			mutex_unlock(&dup_table->mutex);
+		} else {
+			mutex_unlock(&dup_table->mutex);
+			mutex_unlock(&table->mutex);
+		}
+	} else {
+		mutex_unlock(&table->mutex);
+	}
 }
 
 void mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan)
@@ -495,6 +833,220 @@ void mlx4_unregister_vlan(struct mlx4_dev *dev, u8 port, u16 vlan)
 }
 EXPORT_SYMBOL_GPL(mlx4_unregister_vlan);
 
+int mlx4_bond_mac_table(struct mlx4_dev *dev)
+{
+	struct mlx4_mac_table *t1 = &mlx4_priv(dev)->port[1].mac_table;
+	struct mlx4_mac_table *t2 = &mlx4_priv(dev)->port[2].mac_table;
+	int ret = 0;
+	int i;
+	bool update1 = false;
+	bool update2 = false;
+
+	mutex_lock(&t1->mutex);
+	mutex_lock(&t2->mutex);
+	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
+		if ((t1->entries[i] != t2->entries[i]) &&
+		    t1->entries[i] && t2->entries[i]) {
+			mlx4_warn(dev, "can't duplicate entry %d in mac table\n", i);
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+
+	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
+		if (t1->entries[i] && !t2->entries[i]) {
+			t2->entries[i] = t1->entries[i];
+			t2->is_dup[i] = true;
+			update2 = true;
+		} else if (!t1->entries[i] && t2->entries[i]) {
+			t1->entries[i] = t2->entries[i];
+			t1->is_dup[i] = true;
+			update1 = true;
+		} else if (t1->entries[i] && t2->entries[i]) {
+			t1->is_dup[i] = true;
+			t2->is_dup[i] = true;
+		}
+	}
+
+	if (update1) {
+		ret = mlx4_set_port_mac_table(dev, 1, t1->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to set MAC table for port 1 (%d)\n", ret);
+	}
+	if (!ret && update2) {
+		ret = mlx4_set_port_mac_table(dev, 2, t2->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to set MAC table for port 2 (%d)\n", ret);
+	}
+
+	if (ret)
+		mlx4_warn(dev, "failed to create mirror MAC tables\n");
+unlock:
+	mutex_unlock(&t2->mutex);
+	mutex_unlock(&t1->mutex);
+	return ret;
+}
+
+int mlx4_unbond_mac_table(struct mlx4_dev *dev)
+{
+	struct mlx4_mac_table *t1 = &mlx4_priv(dev)->port[1].mac_table;
+	struct mlx4_mac_table *t2 = &mlx4_priv(dev)->port[2].mac_table;
+	int ret = 0;
+	int ret1;
+	int i;
+	bool update1 = false;
+	bool update2 = false;
+
+	mutex_lock(&t1->mutex);
+	mutex_lock(&t2->mutex);
+	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
+		if (t1->entries[i] != t2->entries[i]) {
+			mlx4_warn(dev, "mac table is in an unexpected state when trying to unbond\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+
+	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
+		if (!t1->entries[i])
+			continue;
+		t1->is_dup[i] = false;
+		if (!t1->refs[i]) {
+			t1->entries[i] = 0;
+			update1 = true;
+		}
+		t2->is_dup[i] = false;
+		if (!t2->refs[i]) {
+			t2->entries[i] = 0;
+			update2 = true;
+		}
+	}
+
+	if (update1) {
+		ret = mlx4_set_port_mac_table(dev, 1, t1->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to unmirror MAC tables for port 1(%d)\n", ret);
+	}
+	if (update2) {
+		ret1 = mlx4_set_port_mac_table(dev, 2, t2->entries);
+		if (ret1) {
+			mlx4_warn(dev, "failed to unmirror MAC tables for port 2(%d)\n", ret1);
+			ret = ret1;
+		}
+	}
+unlock:
+	mutex_unlock(&t2->mutex);
+	mutex_unlock(&t1->mutex);
+	return ret;
+}
+
+int mlx4_bond_vlan_table(struct mlx4_dev *dev)
+{
+	struct mlx4_vlan_table *t1 = &mlx4_priv(dev)->port[1].vlan_table;
+	struct mlx4_vlan_table *t2 = &mlx4_priv(dev)->port[2].vlan_table;
+	int ret = 0;
+	int i;
+	bool update1 = false;
+	bool update2 = false;
+
+	mutex_lock(&t1->mutex);
+	mutex_lock(&t2->mutex);
+	for (i = 0; i < MLX4_MAX_VLAN_NUM; i++) {
+		if ((t1->entries[i] != t2->entries[i]) &&
+		    t1->entries[i] && t2->entries[i]) {
+			mlx4_warn(dev, "can't duplicate entry %d in vlan table\n", i);
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+
+	for (i = 0; i < MLX4_MAX_VLAN_NUM; i++) {
+		if (t1->entries[i] && !t2->entries[i]) {
+			t2->entries[i] = t1->entries[i];
+			t2->is_dup[i] = true;
+			update2 = true;
+		} else if (!t1->entries[i] && t2->entries[i]) {
+			t1->entries[i] = t2->entries[i];
+			t1->is_dup[i] = true;
+			update1 = true;
+		} else if (t1->entries[i] && t2->entries[i]) {
+			t1->is_dup[i] = true;
+			t2->is_dup[i] = true;
+		}
+	}
+
+	if (update1) {
+		ret = mlx4_set_port_vlan_table(dev, 1, t1->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to set VLAN table for port 1 (%d)\n", ret);
+	}
+	if (!ret && update2) {
+		ret = mlx4_set_port_vlan_table(dev, 2, t2->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to set VLAN table for port 2 (%d)\n", ret);
+	}
+
+	if (ret)
+		mlx4_warn(dev, "failed to create mirror VLAN tables\n");
+unlock:
+	mutex_unlock(&t2->mutex);
+	mutex_unlock(&t1->mutex);
+	return ret;
+}
+
+int mlx4_unbond_vlan_table(struct mlx4_dev *dev)
+{
+	struct mlx4_vlan_table *t1 = &mlx4_priv(dev)->port[1].vlan_table;
+	struct mlx4_vlan_table *t2 = &mlx4_priv(dev)->port[2].vlan_table;
+	int ret = 0;
+	int ret1;
+	int i;
+	bool update1 = false;
+	bool update2 = false;
+
+	mutex_lock(&t1->mutex);
+	mutex_lock(&t2->mutex);
+	for (i = 0; i < MLX4_MAX_VLAN_NUM; i++) {
+		if (t1->entries[i] != t2->entries[i]) {
+			mlx4_warn(dev, "vlan table is in an unexpected state when trying to unbond\n");
+			ret = -EINVAL;
+			goto unlock;
+		}
+	}
+
+	for (i = 0; i < MLX4_MAX_VLAN_NUM; i++) {
+		if (!t1->entries[i])
+			continue;
+		t1->is_dup[i] = false;
+		if (!t1->refs[i]) {
+			t1->entries[i] = 0;
+			update1 = true;
+		}
+		t2->is_dup[i] = false;
+		if (!t2->refs[i]) {
+			t2->entries[i] = 0;
+			update2 = true;
+		}
+	}
+
+	if (update1) {
+		ret = mlx4_set_port_vlan_table(dev, 1, t1->entries);
+		if (ret)
+			mlx4_warn(dev, "failed to unmirror VLAN tables for port 1(%d)\n", ret);
+	}
+	if (update2) {
+		ret1 = mlx4_set_port_vlan_table(dev, 2, t2->entries);
+		if (ret1) {
+			mlx4_warn(dev, "failed to unmirror VLAN tables for port 2(%d)\n", ret1);
+			ret = ret1;
+		}
+	}
+unlock:
+	mutex_unlock(&t2->mutex);
+	mutex_unlock(&t1->mutex);
+	return ret;
+}
+
 int mlx4_get_port_ib_caps(struct mlx4_dev *dev, u8 port, __be32 *caps)
 {
 	struct mlx4_cmd_mailbox *inmailbox, *outmailbox;
@@ -968,6 +1520,8 @@ int mlx4_SET_PORT(struct mlx4_dev *dev, u8 port, int pkey_tbl_sz)
 	return err;
 }
 
+#define SET_PORT_ROCE_2_FLAGS          0x10
+#define MLX4_SET_PORT_ROCE_V1_V2       0x2
 int mlx4_SET_PORT_general(struct mlx4_dev *dev, u8 port, int mtu,
 			  u8 pptx, u8 pfctx, u8 pprx, u8 pfcrx)
 {
@@ -987,6 +1541,11 @@ int mlx4_SET_PORT_general(struct mlx4_dev *dev, u8 port, int mtu,
 	context->pprx = (pprx * (!pfcrx)) << 7;
 	context->pfcrx = pfcrx;
 
+	if (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2) {
+		context->flags |= SET_PORT_ROCE_2_FLAGS;
+		context->roce_mode |=
+			MLX4_SET_PORT_ROCE_V1_V2 << 4;
+	}
 	in_mod = MLX4_SET_PORT_GENERAL << 8 | port;
 	err = mlx4_cmd(dev, mailbox->dma, in_mod, MLX4_SET_PORT_ETH_OPCODE,
 		       MLX4_CMD_SET_PORT, MLX4_CMD_TIME_CLASS_B,
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index 168823dde79f..d1cd9c32a9ae 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -167,6 +167,12 @@ static int __mlx4_qp_modify(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 		context->log_page_size   = mtt->page_shift - MLX4_ICM_PAGE_SHIFT;
 	}
 
+	if ((cur_state == MLX4_QP_STATE_RTR) &&
+	    (new_state == MLX4_QP_STATE_RTS) &&
+	    dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_ROCE_V1_V2)
+		context->roce_entropy =
+			cpu_to_be16(mlx4_qp_roce_entropy(dev, qp->qpn));
+
 	*(__be32 *) mailbox->buf = cpu_to_be32(optpar);
 	memcpy(mailbox->buf + 8, context, sizeof *context);
 
@@ -921,3 +927,23 @@ int mlx4_qp_to_ready(struct mlx4_dev *dev, struct mlx4_mtt *mtt,
 	return 0;
 }
 EXPORT_SYMBOL_GPL(mlx4_qp_to_ready);
+
+u16 mlx4_qp_roce_entropy(struct mlx4_dev *dev, u32 qpn)
+{
+	struct mlx4_qp_context context;
+	struct mlx4_qp qp;
+	int err;
+
+	qp.qpn = qpn;
+	err = mlx4_qp_query(dev, &qp, &context);
+	if (!err) {
+		u32 dest_qpn = be32_to_cpu(context.remote_qpn) & 0xffffff;
+		u16 folded_dst = folded_qp(dest_qpn);
+		u16 folded_src = folded_qp(qpn);
+
+		return (dest_qpn != qpn) ?
+			((folded_dst ^ folded_src) | 0xC000) :
+			folded_src | 0xC000;
+	}
+	return 0xdead;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index cad6c44df91c..25ce1b030a00 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -222,6 +222,13 @@ enum res_fs_rule_states {
 struct res_fs_rule {
 	struct res_common	com;
 	int			qpn;
+	/* VF DMFS mbox with port flipped */
+	void			*mirr_mbox;
+	/* > 0 --> apply mirror when getting into HA mode      */
+	/* = 0 --> un-apply mirror when getting out of HA mode */
+	u32			mirr_mbox_size;
+	struct list_head	mirr_list;
+	u64			mirr_rule_id;
 };
 
 static void *res_tracker_lookup(struct rb_root *root, u64 res_id)
@@ -908,11 +915,13 @@ static int handle_existing_counter(struct mlx4_dev *dev, u8 slave, int port,
 
 	spin_lock_irq(mlx4_tlock(dev));
 	r = find_res(dev, counter_index, RES_COUNTER);
-	if (!r || r->owner != slave)
+	if (!r || r->owner != slave) {
 		ret = -EINVAL;
-	counter = container_of(r, struct res_counter, com);
-	if (!counter->port)
-		counter->port = port;
+	} else {
+		counter = container_of(r, struct res_counter, com);
+		if (!counter->port)
+			counter->port = port;
+	}
 
 	spin_unlock_irq(mlx4_tlock(dev));
 	return ret;
@@ -4284,6 +4293,22 @@ err_mac:
 	return err;
 }
 
+static u32 qp_attach_mbox_size(void *mbox)
+{
+	u32 size = sizeof(struct mlx4_net_trans_rule_hw_ctrl);
+	struct _rule_hw  *rule_header;
+
+	rule_header = (struct _rule_hw *)(mbox + size);
+
+	while (rule_header->size) {
+		size += rule_header->size * sizeof(u32);
+		rule_header += 1;
+	}
+	return size;
+}
+
+static int mlx4_do_mirror_rule(struct mlx4_dev *dev, struct res_fs_rule *fs_rule);
+
 int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 					 struct mlx4_vhcr *vhcr,
 					 struct mlx4_cmd_mailbox *inbox,
@@ -4300,6 +4325,8 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	struct mlx4_net_trans_rule_hw_ctrl *ctrl;
 	struct _rule_hw  *rule_header;
 	int header_id;
+	struct res_fs_rule *rrule;
+	u32 mbox_size;
 
 	if (dev->caps.steering_mode !=
 	    MLX4_STEERING_MODE_DEVICE_MANAGED)
@@ -4329,7 +4356,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	case MLX4_NET_TRANS_RULE_ID_ETH:
 		if (validate_eth_header_mac(slave, rule_header, rlist)) {
 			err = -EINVAL;
-			goto err_put;
+			goto err_put_qp;
 		}
 		break;
 	case MLX4_NET_TRANS_RULE_ID_IB:
@@ -4340,7 +4367,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 		pr_warn("Can't attach FS rule without L2 headers, adding L2 header\n");
 		if (add_eth_header(dev, slave, inbox, rlist, header_id)) {
 			err = -EINVAL;
-			goto err_put;
+			goto err_put_qp;
 		}
 		vhcr->in_modifier +=
 			sizeof(struct mlx4_net_trans_rule_hw_eth) >> 2;
@@ -4348,7 +4375,7 @@ int mlx4_QP_FLOW_STEERING_ATTACH_wrapper(struct mlx4_dev *dev, int slave,
 	default:
 		pr_err("Corrupted mailbox\n");
 		err = -EINVAL;
-		goto err_put;
+		goto err_put_qp;
 	}
 
 execute:
@@ -4357,23 +4384,69 @@ execute:
 			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
 			   MLX4_CMD_NATIVE);
 	if (err)
-		goto err_put;
+		goto err_put_qp;
+
 
 	err = add_res_range(dev, slave, vhcr->out_param, 1, RES_FS_RULE, qpn);
 	if (err) {
 		mlx4_err(dev, "Fail to add flow steering resources\n");
-		/* detach rule*/
+		goto err_detach;
+	}
+
+	err = get_res(dev, slave, vhcr->out_param, RES_FS_RULE, &rrule);
+	if (err)
+		goto err_detach;
+
+	mbox_size = qp_attach_mbox_size(inbox->buf);
+	rrule->mirr_mbox = kmalloc(mbox_size, GFP_KERNEL);
+	if (!rrule->mirr_mbox) {
+		err = -ENOMEM;
+		goto err_put_rule;
+	}
+	rrule->mirr_mbox_size = mbox_size;
+	rrule->mirr_rule_id = 0;
+	memcpy(rrule->mirr_mbox, inbox->buf, mbox_size);
+
+	/* set different port */
+	ctrl = (struct mlx4_net_trans_rule_hw_ctrl *)rrule->mirr_mbox;
+	if (ctrl->port == 1)
+		ctrl->port = 2;
+	else
+		ctrl->port = 1;
+
+	if (mlx4_is_bonded(dev))
+		mlx4_do_mirror_rule(dev, rrule);
+
+	atomic_inc(&rqp->ref_count);
+
+err_put_rule:
+	put_res(dev, slave, vhcr->out_param, RES_FS_RULE);
+err_detach:
+	/* detach rule on error */
+	if (err)
 		mlx4_cmd(dev, vhcr->out_param, 0, 0,
 			 MLX4_QP_FLOW_STEERING_DETACH, MLX4_CMD_TIME_CLASS_A,
 			 MLX4_CMD_NATIVE);
-		goto err_put;
-	}
-	atomic_inc(&rqp->ref_count);
-err_put:
+err_put_qp:
 	put_res(dev, slave, qpn, RES_QP);
 	return err;
 }
 
+static int mlx4_undo_mirror_rule(struct mlx4_dev *dev, struct res_fs_rule *fs_rule)
+{
+	int err;
+
+	err = rem_res_range(dev, fs_rule->com.owner, fs_rule->com.res_id, 1, RES_FS_RULE, 0);
+	if (err) {
+		mlx4_err(dev, "Fail to remove flow steering resources\n");
+		return err;
+	}
+
+	mlx4_cmd(dev, fs_rule->com.res_id, 0, 0, MLX4_QP_FLOW_STEERING_DETACH,
+		 MLX4_CMD_TIME_CLASS_A, MLX4_CMD_NATIVE);
+	return 0;
+}
+
 int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
 					 struct mlx4_vhcr *vhcr,
 					 struct mlx4_cmd_mailbox *inbox,
@@ -4383,6 +4456,7 @@ int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
 	int err;
 	struct res_qp *rqp;
 	struct res_fs_rule *rrule;
+	u64 mirr_reg_id;
 
 	if (dev->caps.steering_mode !=
 	    MLX4_STEERING_MODE_DEVICE_MANAGED)
@@ -4391,12 +4465,30 @@ int mlx4_QP_FLOW_STEERING_DETACH_wrapper(struct mlx4_dev *dev, int slave,
 	err = get_res(dev, slave, vhcr->in_param, RES_FS_RULE, &rrule);
 	if (err)
 		return err;
+
+	if (!rrule->mirr_mbox) {
+		mlx4_err(dev, "Mirror rules cannot be removed explicitly\n");
+		put_res(dev, slave, vhcr->in_param, RES_FS_RULE);
+		return -EINVAL;
+	}
+	mirr_reg_id = rrule->mirr_rule_id;
+	kfree(rrule->mirr_mbox);
+
 	/* Release the rule form busy state before removal */
 	put_res(dev, slave, vhcr->in_param, RES_FS_RULE);
 	err = get_res(dev, slave, rrule->qpn, RES_QP, &rqp);
 	if (err)
 		return err;
 
+	if (mirr_reg_id && mlx4_is_bonded(dev)) {
+		err = get_res(dev, slave, mirr_reg_id, RES_FS_RULE, &rrule);
+		if (err) {
+			mlx4_err(dev, "Fail to get resource of mirror rule\n");
+		} else {
+			put_res(dev, slave, mirr_reg_id, RES_FS_RULE);
+			mlx4_undo_mirror_rule(dev, rrule);
+		}
+	}
 	err = rem_res_range(dev, slave, vhcr->in_param, 1, RES_FS_RULE, 0);
 	if (err) {
 		mlx4_err(dev, "Fail to remove flow steering resources\n");
@@ -4834,6 +4926,91 @@ static void rem_slave_mtts(struct mlx4_dev *dev, int slave)
 	spin_unlock_irq(mlx4_tlock(dev));
 }
 
+static int mlx4_do_mirror_rule(struct mlx4_dev *dev, struct res_fs_rule *fs_rule)
+{
+	struct mlx4_cmd_mailbox *mailbox;
+	int err;
+	struct res_fs_rule *mirr_rule;
+	u64 reg_id;
+
+	mailbox = mlx4_alloc_cmd_mailbox(dev);
+	if (IS_ERR(mailbox))
+		return PTR_ERR(mailbox);
+
+	if (!fs_rule->mirr_mbox) {
+		mlx4_err(dev, "rule mirroring mailbox is null\n");
+		return -EINVAL;
+	}
+	memcpy(mailbox->buf, fs_rule->mirr_mbox, fs_rule->mirr_mbox_size);
+	err = mlx4_cmd_imm(dev, mailbox->dma, &reg_id, fs_rule->mirr_mbox_size >> 2, 0,
+			   MLX4_QP_FLOW_STEERING_ATTACH, MLX4_CMD_TIME_CLASS_A,
+			   MLX4_CMD_NATIVE);
+	mlx4_free_cmd_mailbox(dev, mailbox);
+
+	if (err)
+		goto err;
+
+	err = add_res_range(dev, fs_rule->com.owner, reg_id, 1, RES_FS_RULE, fs_rule->qpn);
+	if (err)
+		goto err_detach;
+
+	err = get_res(dev, fs_rule->com.owner, reg_id, RES_FS_RULE, &mirr_rule);
+	if (err)
+		goto err_rem;
+
+	fs_rule->mirr_rule_id = reg_id;
+	mirr_rule->mirr_rule_id = 0;
+	mirr_rule->mirr_mbox_size = 0;
+	mirr_rule->mirr_mbox = NULL;
+	put_res(dev, fs_rule->com.owner, reg_id, RES_FS_RULE);
+
+	return 0;
+err_rem:
+	rem_res_range(dev, fs_rule->com.owner, reg_id, 1, RES_FS_RULE, 0);
+err_detach:
+	mlx4_cmd(dev, reg_id, 0, 0, MLX4_QP_FLOW_STEERING_DETACH,
+		 MLX4_CMD_TIME_CLASS_A, MLX4_CMD_NATIVE);
+err:
+	return err;
+}
+
+static int mlx4_mirror_fs_rules(struct mlx4_dev *dev, bool bond)
+{
+	struct mlx4_priv *priv = mlx4_priv(dev);
+	struct mlx4_resource_tracker *tracker =
+		&priv->mfunc.master.res_tracker;
+	struct rb_root *root = &tracker->res_tree[RES_FS_RULE];
+	struct rb_node *p;
+	struct res_fs_rule *fs_rule;
+	int err = 0;
+	LIST_HEAD(mirr_list);
+
+	for (p = rb_first(root); p; p = rb_next(p)) {
+		fs_rule = rb_entry(p, struct res_fs_rule, com.node);
+		if ((bond && fs_rule->mirr_mbox_size) ||
+		    (!bond && !fs_rule->mirr_mbox_size))
+			list_add_tail(&fs_rule->mirr_list, &mirr_list);
+	}
+
+	list_for_each_entry(fs_rule, &mirr_list, mirr_list) {
+		if (bond)
+			err += mlx4_do_mirror_rule(dev, fs_rule);
+		else
+			err += mlx4_undo_mirror_rule(dev, fs_rule);
+	}
+	return err;
+}
+
+int mlx4_bond_fs_rules(struct mlx4_dev *dev)
+{
+	return mlx4_mirror_fs_rules(dev, true);
+}
+
+int mlx4_unbond_fs_rules(struct mlx4_dev *dev)
+{
+	return mlx4_mirror_fs_rules(dev, false);
+}
+
 static void rem_slave_fs_rule(struct mlx4_dev *dev, int slave)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
index 158c88c69ef9..c503ea05e742 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Kconfig
@@ -13,6 +13,7 @@ config MLX5_CORE
 config MLX5_CORE_EN
 	bool "Mellanox Technologies ConnectX-4 Ethernet support"
 	depends on NETDEVICES && ETHERNET && PCI && MLX5_CORE
+	select PTP_1588_CLOCK
 	default n
 	---help---
 	  Ethernet support in Mellanox Technologies ConnectX-4 NIC.
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/Makefile b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
index 26a68b8af2c5..01c0256effb8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/Makefile
+++ b/drivers/net/ethernet/mellanox/mlx5/core/Makefile
@@ -2,7 +2,7 @@ obj-$(CONFIG_MLX5_CORE)		+= mlx5_core.o
 
 mlx5_core-y :=	main.o cmd.o debugfs.o fw.o eq.o uar.o pagealloc.o \
 		health.o mcg.o cq.o srq.o alloc.o qp.o port.o mr.o pd.o   \
-		mad.o transobj.o vport.o
-mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o flow_table.o \
-		en_main.o en_flow_table.o en_ethtool.o en_tx.o en_rx.o \
-		en_txrx.o
+		mad.o transobj.o vport.o sriov.o fs_cmd.o fs_core.o
+mlx5_core-$(CONFIG_MLX5_CORE_EN) += wq.o eswitch.o \
+		en_main.o en_fs.o en_ethtool.o en_tx.o en_rx.o \
+		en_txrx.o en_clock.o
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en.h b/drivers/net/ethernet/mellanox/mlx5/core/en.h
index 22e72bf1ae48..5b1753233c5d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en.h
@@ -32,12 +32,15 @@
 
 #include <linux/if_vlan.h>
 #include <linux/etherdevice.h>
+#include <linux/timecounter.h>
+#include <linux/net_tstamp.h>
+#include <linux/ptp_clock_kernel.h>
 #include <linux/mlx5/driver.h>
 #include <linux/mlx5/qp.h>
 #include <linux/mlx5/cq.h>
 #include <linux/mlx5/vport.h>
+#include <linux/mlx5/transobj.h>
 #include "wq.h"
-#include "transobj.h"
 #include "mlx5_core.h"
 
 #define MLX5E_MAX_NUM_TC	8
@@ -64,6 +67,8 @@
 #define MLX5E_UPDATE_STATS_INTERVAL    200 /* msecs */
 #define MLX5E_SQ_BF_BUDGET             16
 
+#define MLX5E_NUM_MAIN_GROUPS 9
+
 static const char vport_strings[][ETH_GSTRING_LEN] = {
 	/* vport statistics */
 	"rx_packets",
@@ -218,6 +223,7 @@ struct mlx5e_pport_stats {
 
 static const char rq_stats_strings[][ETH_GSTRING_LEN] = {
 	"packets",
+	"bytes",
 	"csum_none",
 	"csum_sw",
 	"lro_packets",
@@ -227,16 +233,18 @@ static const char rq_stats_strings[][ETH_GSTRING_LEN] = {
 
 struct mlx5e_rq_stats {
 	u64 packets;
+	u64 bytes;
 	u64 csum_none;
 	u64 csum_sw;
 	u64 lro_packets;
 	u64 lro_bytes;
 	u64 wqe_err;
-#define NUM_RQ_STATS 6
+#define NUM_RQ_STATS 7
 };
 
 static const char sq_stats_strings[][ETH_GSTRING_LEN] = {
 	"packets",
+	"bytes",
 	"tso_packets",
 	"tso_bytes",
 	"csum_offload_none",
@@ -248,6 +256,7 @@ static const char sq_stats_strings[][ETH_GSTRING_LEN] = {
 
 struct mlx5e_sq_stats {
 	u64 packets;
+	u64 bytes;
 	u64 tso_packets;
 	u64 tso_bytes;
 	u64 csum_offload_none;
@@ -255,7 +264,7 @@ struct mlx5e_sq_stats {
 	u64 wake;
 	u64 dropped;
 	u64 nop;
-#define NUM_SQ_STATS 8
+#define NUM_SQ_STATS 9
 };
 
 struct mlx5e_stats {
@@ -282,18 +291,26 @@ struct mlx5e_params {
 	u32 indirection_rqt[MLX5E_INDIR_RQT_SIZE];
 };
 
-enum {
-	MLX5E_RQ_STATE_POST_WQES_ENABLE,
+struct mlx5e_tstamp {
+	rwlock_t                   lock;
+	struct cyclecounter        cycles;
+	struct timecounter         clock;
+	struct hwtstamp_config     hwtstamp_config;
+	u32                        nominal_c_mult;
+	unsigned long              overflow_period;
+	struct delayed_work        overflow_work;
+	struct mlx5_core_dev      *mdev;
+	struct ptp_clock          *ptp;
+	struct ptp_clock_info      ptp_info;
 };
 
-enum cq_flags {
-	MLX5E_CQ_HAS_CQES = 1,
+enum {
+	MLX5E_RQ_STATE_POST_WQES_ENABLE,
 };
 
 struct mlx5e_cq {
 	/* data path - accessed per cqe */
 	struct mlx5_cqwq           wq;
-	unsigned long              flags;
 
 	/* data path - accessed per napi poll */
 	struct napi_struct        *napi;
@@ -313,6 +330,7 @@ struct mlx5e_rq {
 
 	struct device         *pdev;
 	struct net_device     *netdev;
+	struct mlx5e_tstamp   *tstamp;
 	struct mlx5e_rq_stats  stats;
 	struct mlx5e_cq        cq;
 
@@ -326,14 +344,12 @@ struct mlx5e_rq {
 	struct mlx5e_priv     *priv;
 } ____cacheline_aligned_in_smp;
 
-struct mlx5e_tx_skb_cb {
+struct mlx5e_tx_wqe_info {
 	u32 num_bytes;
 	u8  num_wqebbs;
 	u8  num_dma;
 };
 
-#define MLX5E_TX_SKB_CB(__skb) ((struct mlx5e_tx_skb_cb *)__skb->cb)
-
 enum mlx5e_dma_map_type {
 	MLX5E_DMA_MAP_SINGLE,
 	MLX5E_DMA_MAP_PAGE
@@ -369,6 +385,7 @@ struct mlx5e_sq {
 	/* 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;
 
 	/* read only */
 	struct mlx5_wq_cyc         wq;
@@ -381,6 +398,7 @@ struct mlx5e_sq {
 	u16                        max_inline;
 	u16                        edge;
 	struct device             *pdev;
+	struct mlx5e_tstamp       *tstamp;
 	__be32                     mkey_be;
 	unsigned long              state;
 
@@ -433,6 +451,8 @@ enum mlx5e_traffic_types {
 	MLX5E_NUM_TT,
 };
 
+#define IS_HASHING_TT(tt) (tt != MLX5E_TT_ANY)
+
 enum mlx5e_rqt_ix {
 	MLX5E_INDIRECTION_RQT,
 	MLX5E_SINGLE_RQ_RQT,
@@ -442,7 +462,7 @@ enum mlx5e_rqt_ix {
 struct mlx5e_eth_addr_info {
 	u8  addr[ETH_ALEN + 2];
 	u32 tt_vec;
-	u32 ft_ix[MLX5E_NUM_TT]; /* flow table index per traffic type */
+	struct mlx5_flow_rule *ft_rule[MLX5E_NUM_TT];
 };
 
 #define MLX5E_ETH_ADDR_HASH_SIZE (1 << BITS_PER_BYTE)
@@ -465,15 +485,23 @@ enum {
 };
 
 struct mlx5e_vlan_db {
-	u32           active_vlans_ft_ix[VLAN_N_VID];
-	u32           untagged_rule_ft_ix;
-	u32           any_vlan_rule_ft_ix;
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+	struct mlx5_flow_rule	*active_vlans_rule[VLAN_N_VID];
+	struct mlx5_flow_rule	*untagged_rule;
+	struct mlx5_flow_rule	*any_vlan_rule;
 	bool          filter_disabled;
 };
 
 struct mlx5e_flow_table {
-	void *vlan;
-	void *main;
+	int num_groups;
+	struct mlx5_flow_table		*t;
+	struct mlx5_flow_group		**g;
+};
+
+struct mlx5e_flow_tables {
+	struct mlx5_flow_namespace	*ns;
+	struct mlx5e_flow_table		vlan;
+	struct mlx5e_flow_table		main;
 };
 
 struct mlx5e_priv {
@@ -496,7 +524,7 @@ struct mlx5e_priv {
 	u32                        rqtn[MLX5E_NUM_RQT];
 	u32                        tirn[MLX5E_NUM_TT];
 
-	struct mlx5e_flow_table    ft;
+	struct mlx5e_flow_tables   fts;
 	struct mlx5e_eth_addr_db   eth_addr;
 	struct mlx5e_vlan_db       vlan;
 
@@ -509,6 +537,7 @@ struct mlx5e_priv {
 	struct mlx5_core_dev      *mdev;
 	struct net_device         *netdev;
 	struct mlx5e_stats         stats;
+	struct mlx5e_tstamp        tstamp;
 };
 
 #define MLX5E_NET_IP_ALIGN 2
@@ -564,7 +593,7 @@ void mlx5e_completion_event(struct mlx5_core_cq *mcq);
 void mlx5e_cq_error_event(struct mlx5_core_cq *mcq, enum mlx5_event event);
 int mlx5e_napi_poll(struct napi_struct *napi, int budget);
 bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq);
-bool mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget);
+int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget);
 bool mlx5e_post_rx_wqes(struct mlx5e_rq *rq);
 struct mlx5_cqe64 *mlx5e_get_cqe(struct mlx5e_cq *cq);
 
@@ -575,6 +604,13 @@ void mlx5e_destroy_flow_tables(struct mlx5e_priv *priv);
 void mlx5e_init_eth_addr(struct mlx5e_priv *priv);
 void mlx5e_set_rx_mode_work(struct work_struct *work);
 
+void mlx5e_fill_hwstamp(struct mlx5e_tstamp *clock, u64 timestamp,
+			struct skb_shared_hwtstamps *hwts);
+void mlx5e_timestamp_init(struct mlx5e_priv *priv);
+void mlx5e_timestamp_cleanup(struct mlx5e_priv *priv);
+int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr);
+int mlx5e_hwstamp_get(struct net_device *dev, struct ifreq *ifr);
+
 int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
 			  u16 vid);
 int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
@@ -583,9 +619,12 @@ void mlx5e_enable_vlan_filter(struct mlx5e_priv *priv);
 void mlx5e_disable_vlan_filter(struct mlx5e_priv *priv);
 
 int mlx5e_redirect_rqt(struct mlx5e_priv *priv, enum mlx5e_rqt_ix rqt_ix);
+void mlx5e_build_tir_ctx_hash(void *tirc, struct mlx5e_priv *priv);
 
 int mlx5e_open_locked(struct net_device *netdev);
 int mlx5e_close_locked(struct net_device *netdev);
+void mlx5e_build_default_indir_rqt(u32 *indirection_rqt, int len,
+				   int num_channels);
 
 static inline void mlx5e_tx_notify_hw(struct mlx5e_sq *sq,
 				      struct mlx5e_tx_wqe *wqe, int bf_sz)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
new file mode 100644
index 000000000000..2018eebe1531
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_clock.c
@@ -0,0 +1,292 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/clocksource.h>
+#include "en.h"
+
+enum {
+	MLX5E_CYCLES_SHIFT	= 23
+};
+
+void mlx5e_fill_hwstamp(struct mlx5e_tstamp *tstamp, u64 timestamp,
+			struct skb_shared_hwtstamps *hwts)
+{
+	u64 nsec;
+
+	read_lock(&tstamp->lock);
+	nsec = timecounter_cyc2time(&tstamp->clock, timestamp);
+	read_unlock(&tstamp->lock);
+
+	hwts->hwtstamp = ns_to_ktime(nsec);
+}
+
+static cycle_t mlx5e_read_internal_timer(const struct cyclecounter *cc)
+{
+	struct mlx5e_tstamp *tstamp = container_of(cc, struct mlx5e_tstamp,
+						   cycles);
+
+	return mlx5_read_internal_timer(tstamp->mdev) & cc->mask;
+}
+
+static void mlx5e_timestamp_overflow(struct work_struct *work)
+{
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct mlx5e_tstamp *tstamp = container_of(dwork, struct mlx5e_tstamp,
+						   overflow_work);
+	unsigned long flags;
+
+	write_lock_irqsave(&tstamp->lock, flags);
+	timecounter_read(&tstamp->clock);
+	write_unlock_irqrestore(&tstamp->lock, flags);
+	schedule_delayed_work(&tstamp->overflow_work, tstamp->overflow_period);
+}
+
+int mlx5e_hwstamp_set(struct net_device *dev, struct ifreq *ifr)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct hwtstamp_config config;
+
+	if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz))
+		return -EOPNOTSUPP;
+
+	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	/* TX HW timestamp */
+	switch (config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+	case HWTSTAMP_TX_ON:
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	/* RX HW timestamp */
+	switch (config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		config.rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	memcpy(&priv->tstamp.hwtstamp_config, &config, sizeof(config));
+
+	return copy_to_user(ifr->ifr_data, &config,
+			    sizeof(config)) ? -EFAULT : 0;
+}
+
+int mlx5e_hwstamp_get(struct net_device *dev, struct ifreq *ifr)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct hwtstamp_config *cfg = &priv->tstamp.hwtstamp_config;
+
+	if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz))
+		return -EOPNOTSUPP;
+
+	return copy_to_user(ifr->ifr_data, cfg, sizeof(*cfg)) ? -EFAULT : 0;
+}
+
+static int mlx5e_ptp_settime(struct ptp_clock_info *ptp,
+			     const struct timespec64 *ts)
+{
+	struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp,
+						   ptp_info);
+	u64 ns = timespec64_to_ns(ts);
+	unsigned long flags;
+
+	write_lock_irqsave(&tstamp->lock, flags);
+	timecounter_init(&tstamp->clock, &tstamp->cycles, ns);
+	write_unlock_irqrestore(&tstamp->lock, flags);
+
+	return 0;
+}
+
+static int mlx5e_ptp_gettime(struct ptp_clock_info *ptp,
+			     struct timespec64 *ts)
+{
+	struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp,
+						   ptp_info);
+	u64 ns;
+	unsigned long flags;
+
+	write_lock_irqsave(&tstamp->lock, flags);
+	ns = timecounter_read(&tstamp->clock);
+	write_unlock_irqrestore(&tstamp->lock, flags);
+
+	*ts = ns_to_timespec64(ns);
+
+	return 0;
+}
+
+static int mlx5e_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp,
+						   ptp_info);
+	unsigned long flags;
+
+	write_lock_irqsave(&tstamp->lock, flags);
+	timecounter_adjtime(&tstamp->clock, delta);
+	write_unlock_irqrestore(&tstamp->lock, flags);
+
+	return 0;
+}
+
+static int mlx5e_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta)
+{
+	u64 adj;
+	u32 diff;
+	unsigned long flags;
+	int neg_adj = 0;
+	struct mlx5e_tstamp *tstamp = container_of(ptp, struct mlx5e_tstamp,
+						  ptp_info);
+
+	if (delta < 0) {
+		neg_adj = 1;
+		delta = -delta;
+	}
+
+	adj = tstamp->nominal_c_mult;
+	adj *= delta;
+	diff = div_u64(adj, 1000000000ULL);
+
+	write_lock_irqsave(&tstamp->lock, flags);
+	timecounter_read(&tstamp->clock);
+	tstamp->cycles.mult = neg_adj ? tstamp->nominal_c_mult - diff :
+					tstamp->nominal_c_mult + diff;
+	write_unlock_irqrestore(&tstamp->lock, flags);
+
+	return 0;
+}
+
+static const struct ptp_clock_info mlx5e_ptp_clock_info = {
+	.owner		= THIS_MODULE,
+	.max_adj	= 100000000,
+	.n_alarm	= 0,
+	.n_ext_ts	= 0,
+	.n_per_out	= 0,
+	.n_pins		= 0,
+	.pps		= 0,
+	.adjfreq	= mlx5e_ptp_adjfreq,
+	.adjtime	= mlx5e_ptp_adjtime,
+	.gettime64	= mlx5e_ptp_gettime,
+	.settime64	= mlx5e_ptp_settime,
+	.enable		= NULL,
+};
+
+static void mlx5e_timestamp_init_config(struct mlx5e_tstamp *tstamp)
+{
+	tstamp->hwtstamp_config.tx_type = HWTSTAMP_TX_OFF;
+	tstamp->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_NONE;
+}
+
+void mlx5e_timestamp_init(struct mlx5e_priv *priv)
+{
+	struct mlx5e_tstamp *tstamp = &priv->tstamp;
+	u64 ns;
+	u64 frac = 0;
+	u32 dev_freq;
+
+	mlx5e_timestamp_init_config(tstamp);
+	dev_freq = MLX5_CAP_GEN(priv->mdev, device_frequency_khz);
+	if (!dev_freq) {
+		mlx5_core_warn(priv->mdev, "invalid device_frequency_khz, aborting HW clock init\n");
+		return;
+	}
+	rwlock_init(&tstamp->lock);
+	tstamp->cycles.read = mlx5e_read_internal_timer;
+	tstamp->cycles.shift = MLX5E_CYCLES_SHIFT;
+	tstamp->cycles.mult = clocksource_khz2mult(dev_freq,
+						   tstamp->cycles.shift);
+	tstamp->nominal_c_mult = tstamp->cycles.mult;
+	tstamp->cycles.mask = CLOCKSOURCE_MASK(41);
+	tstamp->mdev = priv->mdev;
+
+	timecounter_init(&tstamp->clock, &tstamp->cycles,
+			 ktime_to_ns(ktime_get_real()));
+
+	/* Calculate period in seconds to call the overflow watchdog - to make
+	 * sure counter is checked at least once every wrap around.
+	 */
+	ns = cyclecounter_cyc2ns(&tstamp->cycles, tstamp->cycles.mask,
+				 frac, &frac);
+	do_div(ns, NSEC_PER_SEC / 2 / HZ);
+	tstamp->overflow_period = ns;
+
+	INIT_DELAYED_WORK(&tstamp->overflow_work, mlx5e_timestamp_overflow);
+	if (tstamp->overflow_period)
+		schedule_delayed_work(&tstamp->overflow_work, 0);
+	else
+		mlx5_core_warn(priv->mdev, "invalid overflow period, overflow_work is not scheduled\n");
+
+	/* Configure the PHC */
+	tstamp->ptp_info = mlx5e_ptp_clock_info;
+	snprintf(tstamp->ptp_info.name, 16, "mlx5 ptp");
+
+	tstamp->ptp = ptp_clock_register(&tstamp->ptp_info,
+					 &priv->mdev->pdev->dev);
+	if (IS_ERR_OR_NULL(tstamp->ptp)) {
+		mlx5_core_warn(priv->mdev, "ptp_clock_register failed %ld\n",
+			       PTR_ERR(tstamp->ptp));
+		tstamp->ptp = NULL;
+	}
+}
+
+void mlx5e_timestamp_cleanup(struct mlx5e_priv *priv)
+{
+	struct mlx5e_tstamp *tstamp = &priv->tstamp;
+
+	if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz))
+		return;
+
+	if (priv->tstamp.ptp) {
+		ptp_clock_unregister(priv->tstamp.ptp);
+		priv->tstamp.ptp = NULL;
+	}
+
+	cancel_delayed_work_sync(&tstamp->overflow_work);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
index 2e022e900939..5abeb00fceb8 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_ethtool.c
@@ -385,6 +385,8 @@ static int mlx5e_set_channels(struct net_device *dev,
 		mlx5e_close_locked(dev);
 
 	priv->params.num_channels = count;
+	mlx5e_build_default_indir_rqt(priv->params.indirection_rqt,
+				      MLX5E_INDIR_RQT_SIZE, count);
 
 	if (was_opened)
 		err = mlx5e_open_locked(dev);
@@ -703,18 +705,36 @@ static int mlx5e_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
 	return 0;
 }
 
+static void mlx5e_modify_tirs_hash(struct mlx5e_priv *priv, void *in, int inlen)
+{
+	struct mlx5_core_dev *mdev = priv->mdev;
+	void *tirc = MLX5_ADDR_OF(modify_tir_in, in, ctx);
+	int i;
+
+	MLX5_SET(modify_tir_in, in, bitmask.hash, 1);
+	mlx5e_build_tir_ctx_hash(tirc, priv);
+
+	for (i = 0; i < MLX5E_NUM_TT; i++)
+		if (IS_HASHING_TT(i))
+			mlx5_core_modify_tir(mdev, priv->tirn[i], in, inlen);
+}
+
 static int mlx5e_set_rxfh(struct net_device *dev, const u32 *indir,
 			  const u8 *key, const u8 hfunc)
 {
 	struct mlx5e_priv *priv = netdev_priv(dev);
-	bool close_open;
-	int err = 0;
+	int inlen = MLX5_ST_SZ_BYTES(modify_tir_in);
+	void *in;
 
 	if ((hfunc != ETH_RSS_HASH_NO_CHANGE) &&
 	    (hfunc != ETH_RSS_HASH_XOR) &&
 	    (hfunc != ETH_RSS_HASH_TOP))
 		return -EINVAL;
 
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
 	mutex_lock(&priv->state_lock);
 
 	if (indir) {
@@ -723,11 +743,6 @@ static int mlx5e_set_rxfh(struct net_device *dev, const u32 *indir,
 		mlx5e_redirect_rqt(priv, MLX5E_INDIRECTION_RQT);
 	}
 
-	close_open = (key || (hfunc != ETH_RSS_HASH_NO_CHANGE)) &&
-		     test_bit(MLX5E_STATE_OPENED, &priv->state);
-	if (close_open)
-		mlx5e_close_locked(dev);
-
 	if (key)
 		memcpy(priv->params.toeplitz_hash_key, key,
 		       sizeof(priv->params.toeplitz_hash_key));
@@ -735,12 +750,13 @@ static int mlx5e_set_rxfh(struct net_device *dev, const u32 *indir,
 	if (hfunc != ETH_RSS_HASH_NO_CHANGE)
 		priv->params.rss_hfunc = hfunc;
 
-	if (close_open)
-		err = mlx5e_open_locked(priv->netdev);
+	mlx5e_modify_tirs_hash(priv, in, inlen);
 
 	mutex_unlock(&priv->state_lock);
 
-	return err;
+	kvfree(in);
+
+	return 0;
 }
 
 static int mlx5e_get_rxnfc(struct net_device *netdev,
@@ -855,6 +871,35 @@ static int mlx5e_set_pauseparam(struct net_device *netdev,
 	return err;
 }
 
+static int mlx5e_get_ts_info(struct net_device *dev,
+			     struct ethtool_ts_info *info)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	int ret;
+
+	ret = ethtool_op_get_ts_info(dev, info);
+	if (ret)
+		return ret;
+
+	info->phc_index = priv->tstamp.ptp ?
+			  ptp_clock_index(priv->tstamp.ptp) : -1;
+
+	if (!MLX5_CAP_GEN(priv->mdev, device_frequency_khz))
+		return 0;
+
+	info->so_timestamping |= SOF_TIMESTAMPING_TX_HARDWARE |
+				 SOF_TIMESTAMPING_RX_HARDWARE |
+				 SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	info->tx_types = (BIT(1) << HWTSTAMP_TX_OFF) |
+			 (BIT(1) << HWTSTAMP_TX_ON);
+
+	info->rx_filters = (BIT(1) << HWTSTAMP_FILTER_NONE) |
+			   (BIT(1) << HWTSTAMP_FILTER_ALL);
+
+	return 0;
+}
+
 const struct ethtool_ops mlx5e_ethtool_ops = {
 	.get_drvinfo       = mlx5e_get_drvinfo,
 	.get_link          = ethtool_op_get_link,
@@ -878,4 +923,5 @@ const struct ethtool_ops mlx5e_ethtool_ops = {
 	.set_tunable       = mlx5e_set_tunable,
 	.get_pauseparam    = mlx5e_get_pauseparam,
 	.set_pauseparam    = mlx5e_set_pauseparam,
+	.get_ts_info       = mlx5e_get_ts_info,
 };
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c b/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c
deleted file mode 100644
index 22d603f78273..000000000000
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_flow_table.c
+++ /dev/null
@@ -1,907 +0,0 @@
-/*
- * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <linux/list.h>
-#include <linux/ip.h>
-#include <linux/ipv6.h>
-#include <linux/tcp.h>
-#include <linux/mlx5/flow_table.h>
-#include "en.h"
-
-enum {
-	MLX5E_FULLMATCH = 0,
-	MLX5E_ALLMULTI  = 1,
-	MLX5E_PROMISC   = 2,
-};
-
-enum {
-	MLX5E_UC        = 0,
-	MLX5E_MC_IPV4   = 1,
-	MLX5E_MC_IPV6   = 2,
-	MLX5E_MC_OTHER  = 3,
-};
-
-enum {
-	MLX5E_ACTION_NONE = 0,
-	MLX5E_ACTION_ADD  = 1,
-	MLX5E_ACTION_DEL  = 2,
-};
-
-struct mlx5e_eth_addr_hash_node {
-	struct hlist_node          hlist;
-	u8                         action;
-	struct mlx5e_eth_addr_info ai;
-};
-
-static inline int mlx5e_hash_eth_addr(u8 *addr)
-{
-	return addr[5];
-}
-
-static void mlx5e_add_eth_addr_to_hash(struct hlist_head *hash, u8 *addr)
-{
-	struct mlx5e_eth_addr_hash_node *hn;
-	int ix = mlx5e_hash_eth_addr(addr);
-	int found = 0;
-
-	hlist_for_each_entry(hn, &hash[ix], hlist)
-		if (ether_addr_equal_64bits(hn->ai.addr, addr)) {
-			found = 1;
-			break;
-		}
-
-	if (found) {
-		hn->action = MLX5E_ACTION_NONE;
-		return;
-	}
-
-	hn = kzalloc(sizeof(*hn), GFP_ATOMIC);
-	if (!hn)
-		return;
-
-	ether_addr_copy(hn->ai.addr, addr);
-	hn->action = MLX5E_ACTION_ADD;
-
-	hlist_add_head(&hn->hlist, &hash[ix]);
-}
-
-static void mlx5e_del_eth_addr_from_hash(struct mlx5e_eth_addr_hash_node *hn)
-{
-	hlist_del(&hn->hlist);
-	kfree(hn);
-}
-
-static void mlx5e_del_eth_addr_from_flow_table(struct mlx5e_priv *priv,
-					       struct mlx5e_eth_addr_info *ai)
-{
-	void *ft = priv->ft.main;
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_ESP))
-		mlx5_del_flow_table_entry(ft,
-					  ai->ft_ix[MLX5E_TT_IPV6_IPSEC_ESP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_ESP))
-		mlx5_del_flow_table_entry(ft,
-					  ai->ft_ix[MLX5E_TT_IPV4_IPSEC_ESP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_AH))
-		mlx5_del_flow_table_entry(ft,
-					  ai->ft_ix[MLX5E_TT_IPV6_IPSEC_AH]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_AH))
-		mlx5_del_flow_table_entry(ft,
-					  ai->ft_ix[MLX5E_TT_IPV4_IPSEC_AH]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_TCP))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6_TCP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_TCP))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4_TCP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_UDP))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6_UDP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_UDP))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4_UDP]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV6))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV6]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_IPV4))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_IPV4]);
-
-	if (ai->tt_vec & BIT(MLX5E_TT_ANY))
-		mlx5_del_flow_table_entry(ft, ai->ft_ix[MLX5E_TT_ANY]);
-}
-
-static int mlx5e_get_eth_addr_type(u8 *addr)
-{
-	if (is_unicast_ether_addr(addr))
-		return MLX5E_UC;
-
-	if ((addr[0] == 0x01) &&
-	    (addr[1] == 0x00) &&
-	    (addr[2] == 0x5e) &&
-	   !(addr[3] &  0x80))
-		return MLX5E_MC_IPV4;
-
-	if ((addr[0] == 0x33) &&
-	    (addr[1] == 0x33))
-		return MLX5E_MC_IPV6;
-
-	return MLX5E_MC_OTHER;
-}
-
-static u32 mlx5e_get_tt_vec(struct mlx5e_eth_addr_info *ai, int type)
-{
-	int eth_addr_type;
-	u32 ret;
-
-	switch (type) {
-	case MLX5E_FULLMATCH:
-		eth_addr_type = mlx5e_get_eth_addr_type(ai->addr);
-		switch (eth_addr_type) {
-		case MLX5E_UC:
-			ret =
-				BIT(MLX5E_TT_IPV4_TCP)       |
-				BIT(MLX5E_TT_IPV6_TCP)       |
-				BIT(MLX5E_TT_IPV4_UDP)       |
-				BIT(MLX5E_TT_IPV6_UDP)       |
-				BIT(MLX5E_TT_IPV4_IPSEC_AH)  |
-				BIT(MLX5E_TT_IPV6_IPSEC_AH)  |
-				BIT(MLX5E_TT_IPV4_IPSEC_ESP) |
-				BIT(MLX5E_TT_IPV6_IPSEC_ESP) |
-				BIT(MLX5E_TT_IPV4)           |
-				BIT(MLX5E_TT_IPV6)           |
-				BIT(MLX5E_TT_ANY)            |
-				0;
-			break;
-
-		case MLX5E_MC_IPV4:
-			ret =
-				BIT(MLX5E_TT_IPV4_UDP)       |
-				BIT(MLX5E_TT_IPV4)           |
-				0;
-			break;
-
-		case MLX5E_MC_IPV6:
-			ret =
-				BIT(MLX5E_TT_IPV6_UDP)       |
-				BIT(MLX5E_TT_IPV6)           |
-				0;
-			break;
-
-		case MLX5E_MC_OTHER:
-			ret =
-				BIT(MLX5E_TT_ANY)            |
-				0;
-			break;
-		}
-
-		break;
-
-	case MLX5E_ALLMULTI:
-		ret =
-			BIT(MLX5E_TT_IPV4_UDP) |
-			BIT(MLX5E_TT_IPV6_UDP) |
-			BIT(MLX5E_TT_IPV4)     |
-			BIT(MLX5E_TT_IPV6)     |
-			BIT(MLX5E_TT_ANY)      |
-			0;
-		break;
-
-	default: /* MLX5E_PROMISC */
-		ret =
-			BIT(MLX5E_TT_IPV4_TCP)       |
-			BIT(MLX5E_TT_IPV6_TCP)       |
-			BIT(MLX5E_TT_IPV4_UDP)       |
-			BIT(MLX5E_TT_IPV6_UDP)       |
-			BIT(MLX5E_TT_IPV4_IPSEC_AH)  |
-			BIT(MLX5E_TT_IPV6_IPSEC_AH)  |
-			BIT(MLX5E_TT_IPV4_IPSEC_ESP) |
-			BIT(MLX5E_TT_IPV6_IPSEC_ESP) |
-			BIT(MLX5E_TT_IPV4)           |
-			BIT(MLX5E_TT_IPV6)           |
-			BIT(MLX5E_TT_ANY)            |
-			0;
-		break;
-	}
-
-	return ret;
-}
-
-static int __mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
-				     struct mlx5e_eth_addr_info *ai, int type,
-				     void *flow_context, void *match_criteria)
-{
-	u8 match_criteria_enable = 0;
-	void *match_value;
-	void *dest;
-	u8   *dmac;
-	u8   *match_criteria_dmac;
-	void *ft   = priv->ft.main;
-	u32  *tirn = priv->tirn;
-	u32  *ft_ix;
-	u32  tt_vec;
-	int  err;
-
-	match_value = MLX5_ADDR_OF(flow_context, flow_context, match_value);
-	dmac = MLX5_ADDR_OF(fte_match_param, match_value,
-			    outer_headers.dmac_47_16);
-	match_criteria_dmac = MLX5_ADDR_OF(fte_match_param, match_criteria,
-					   outer_headers.dmac_47_16);
-	dest = MLX5_ADDR_OF(flow_context, flow_context, destination);
-
-	MLX5_SET(flow_context, flow_context, action,
-		 MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
-	MLX5_SET(flow_context, flow_context, destination_list_size, 1);
-	MLX5_SET(dest_format_struct, dest, destination_type,
-		 MLX5_FLOW_CONTEXT_DEST_TYPE_TIR);
-
-	switch (type) {
-	case MLX5E_FULLMATCH:
-		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-		memset(match_criteria_dmac, 0xff, ETH_ALEN);
-		ether_addr_copy(dmac, ai->addr);
-		break;
-
-	case MLX5E_ALLMULTI:
-		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-		match_criteria_dmac[0] = 0x01;
-		dmac[0] = 0x01;
-		break;
-
-	case MLX5E_PROMISC:
-		break;
-	}
-
-	tt_vec = mlx5e_get_tt_vec(ai, type);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_ANY];
-	if (tt_vec & BIT(MLX5E_TT_ANY)) {
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_ANY]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_ANY);
-	}
-
-	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
-			 outer_headers.ethertype);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV4];
-	if (tt_vec & BIT(MLX5E_TT_IPV4)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IP);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV4]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV4);
-	}
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV6];
-	if (tt_vec & BIT(MLX5E_TT_IPV6)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IPV6);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV6]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV6);
-	}
-
-	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
-			 outer_headers.ip_protocol);
-	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
-		 IPPROTO_UDP);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV4_UDP];
-	if (tt_vec & BIT(MLX5E_TT_IPV4_UDP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IP);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV4_UDP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV4_UDP);
-	}
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV6_UDP];
-	if (tt_vec & BIT(MLX5E_TT_IPV6_UDP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IPV6);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV6_UDP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV6_UDP);
-	}
-
-	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
-		 IPPROTO_TCP);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV4_TCP];
-	if (tt_vec & BIT(MLX5E_TT_IPV4_TCP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IP);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV4_TCP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV4_TCP);
-	}
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV6_TCP];
-	if (tt_vec & BIT(MLX5E_TT_IPV6_TCP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IPV6);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV6_TCP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV6_TCP);
-	}
-
-	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
-		 IPPROTO_AH);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV4_IPSEC_AH];
-	if (tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_AH)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IP);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV4_IPSEC_AH]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV4_IPSEC_AH);
-	}
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV6_IPSEC_AH];
-	if (tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_AH)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IPV6);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV6_IPSEC_AH]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV6_IPSEC_AH);
-	}
-
-	MLX5_SET(fte_match_param, match_value, outer_headers.ip_protocol,
-		 IPPROTO_ESP);
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV4_IPSEC_ESP];
-	if (tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_ESP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IP);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV4_IPSEC_ESP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV4_IPSEC_ESP);
-	}
-
-	ft_ix = &ai->ft_ix[MLX5E_TT_IPV6_IPSEC_ESP];
-	if (tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_ESP)) {
-		MLX5_SET(fte_match_param, match_value, outer_headers.ethertype,
-			 ETH_P_IPV6);
-		MLX5_SET(dest_format_struct, dest, destination_id,
-			 tirn[MLX5E_TT_IPV6_IPSEC_ESP]);
-		err = mlx5_add_flow_table_entry(ft, match_criteria_enable,
-						match_criteria, flow_context,
-						ft_ix);
-		if (err)
-			goto err_del_ai;
-
-		ai->tt_vec |= BIT(MLX5E_TT_IPV6_IPSEC_ESP);
-	}
-
-	return 0;
-
-err_del_ai:
-	mlx5e_del_eth_addr_from_flow_table(priv, ai);
-
-	return err;
-}
-
-static int mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
-				   struct mlx5e_eth_addr_info *ai, int type)
-{
-	u32 *flow_context;
-	u32 *match_criteria;
-	int err;
-
-	flow_context   = mlx5_vzalloc(MLX5_ST_SZ_BYTES(flow_context) +
-				      MLX5_ST_SZ_BYTES(dest_format_struct));
-	match_criteria = mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
-	if (!flow_context || !match_criteria) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
-		err = -ENOMEM;
-		goto add_eth_addr_rule_out;
-	}
-
-	err = __mlx5e_add_eth_addr_rule(priv, ai, type, flow_context,
-					match_criteria);
-	if (err)
-		netdev_err(priv->netdev, "%s: failed\n", __func__);
-
-add_eth_addr_rule_out:
-	kvfree(match_criteria);
-	kvfree(flow_context);
-	return err;
-}
-
-enum mlx5e_vlan_rule_type {
-	MLX5E_VLAN_RULE_TYPE_UNTAGGED,
-	MLX5E_VLAN_RULE_TYPE_ANY_VID,
-	MLX5E_VLAN_RULE_TYPE_MATCH_VID,
-};
-
-static int mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
-			       enum mlx5e_vlan_rule_type rule_type, u16 vid)
-{
-	u8 match_criteria_enable = 0;
-	u32 *flow_context;
-	void *match_value;
-	void *dest;
-	u32 *match_criteria;
-	u32 *ft_ix;
-	int err;
-
-	flow_context   = mlx5_vzalloc(MLX5_ST_SZ_BYTES(flow_context) +
-				      MLX5_ST_SZ_BYTES(dest_format_struct));
-	match_criteria = mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
-	if (!flow_context || !match_criteria) {
-		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
-		err = -ENOMEM;
-		goto add_vlan_rule_out;
-	}
-	match_value = MLX5_ADDR_OF(flow_context, flow_context, match_value);
-	dest = MLX5_ADDR_OF(flow_context, flow_context, destination);
-
-	MLX5_SET(flow_context, flow_context, action,
-		 MLX5_FLOW_CONTEXT_ACTION_FWD_DEST);
-	MLX5_SET(flow_context, flow_context, destination_list_size, 1);
-	MLX5_SET(dest_format_struct, dest, destination_type,
-		 MLX5_FLOW_CONTEXT_DEST_TYPE_FLOW_TABLE);
-	MLX5_SET(dest_format_struct, dest, destination_id,
-		 mlx5_get_flow_table_id(priv->ft.main));
-
-	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, match_criteria,
-			 outer_headers.vlan_tag);
-
-	switch (rule_type) {
-	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
-		ft_ix = &priv->vlan.untagged_rule_ft_ix;
-		break;
-	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
-		ft_ix = &priv->vlan.any_vlan_rule_ft_ix;
-		MLX5_SET(fte_match_param, match_value, outer_headers.vlan_tag,
-			 1);
-		break;
-	default: /* MLX5E_VLAN_RULE_TYPE_MATCH_VID */
-		ft_ix = &priv->vlan.active_vlans_ft_ix[vid];
-		MLX5_SET(fte_match_param, match_value, outer_headers.vlan_tag,
-			 1);
-		MLX5_SET_TO_ONES(fte_match_param, match_criteria,
-				 outer_headers.first_vid);
-		MLX5_SET(fte_match_param, match_value, outer_headers.first_vid,
-			 vid);
-		break;
-	}
-
-	err = mlx5_add_flow_table_entry(priv->ft.vlan, match_criteria_enable,
-					match_criteria, flow_context, ft_ix);
-	if (err)
-		netdev_err(priv->netdev, "%s: failed\n", __func__);
-
-add_vlan_rule_out:
-	kvfree(match_criteria);
-	kvfree(flow_context);
-	return err;
-}
-
-static void mlx5e_del_vlan_rule(struct mlx5e_priv *priv,
-				enum mlx5e_vlan_rule_type rule_type, u16 vid)
-{
-	switch (rule_type) {
-	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
-		mlx5_del_flow_table_entry(priv->ft.vlan,
-					  priv->vlan.untagged_rule_ft_ix);
-		break;
-	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
-		mlx5_del_flow_table_entry(priv->ft.vlan,
-					  priv->vlan.any_vlan_rule_ft_ix);
-		break;
-	case MLX5E_VLAN_RULE_TYPE_MATCH_VID:
-		mlx5_del_flow_table_entry(priv->ft.vlan,
-					  priv->vlan.active_vlans_ft_ix[vid]);
-		break;
-	}
-}
-
-void mlx5e_enable_vlan_filter(struct mlx5e_priv *priv)
-{
-	if (!priv->vlan.filter_disabled)
-		return;
-
-	priv->vlan.filter_disabled = false;
-	if (priv->netdev->flags & IFF_PROMISC)
-		return;
-	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
-}
-
-void mlx5e_disable_vlan_filter(struct mlx5e_priv *priv)
-{
-	if (priv->vlan.filter_disabled)
-		return;
-
-	priv->vlan.filter_disabled = true;
-	if (priv->netdev->flags & IFF_PROMISC)
-		return;
-	mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
-}
-
-int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
-			  u16 vid)
-{
-	struct mlx5e_priv *priv = netdev_priv(dev);
-
-	return mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
-}
-
-int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
-			   u16 vid)
-{
-	struct mlx5e_priv *priv = netdev_priv(dev);
-
-	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
-
-	return 0;
-}
-
-#define mlx5e_for_each_hash_node(hn, tmp, hash, i) \
-	for (i = 0; i < MLX5E_ETH_ADDR_HASH_SIZE; i++) \
-		hlist_for_each_entry_safe(hn, tmp, &hash[i], hlist)
-
-static void mlx5e_execute_action(struct mlx5e_priv *priv,
-				 struct mlx5e_eth_addr_hash_node *hn)
-{
-	switch (hn->action) {
-	case MLX5E_ACTION_ADD:
-		mlx5e_add_eth_addr_rule(priv, &hn->ai, MLX5E_FULLMATCH);
-		hn->action = MLX5E_ACTION_NONE;
-		break;
-
-	case MLX5E_ACTION_DEL:
-		mlx5e_del_eth_addr_from_flow_table(priv, &hn->ai);
-		mlx5e_del_eth_addr_from_hash(hn);
-		break;
-	}
-}
-
-static void mlx5e_sync_netdev_addr(struct mlx5e_priv *priv)
-{
-	struct net_device *netdev = priv->netdev;
-	struct netdev_hw_addr *ha;
-
-	netif_addr_lock_bh(netdev);
-
-	mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc,
-				   priv->netdev->dev_addr);
-
-	netdev_for_each_uc_addr(ha, netdev)
-		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc, ha->addr);
-
-	netdev_for_each_mc_addr(ha, netdev)
-		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_mc, ha->addr);
-
-	netif_addr_unlock_bh(netdev);
-}
-
-static void mlx5e_apply_netdev_addr(struct mlx5e_priv *priv)
-{
-	struct mlx5e_eth_addr_hash_node *hn;
-	struct hlist_node *tmp;
-	int i;
-
-	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
-		mlx5e_execute_action(priv, hn);
-
-	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
-		mlx5e_execute_action(priv, hn);
-}
-
-static void mlx5e_handle_netdev_addr(struct mlx5e_priv *priv)
-{
-	struct mlx5e_eth_addr_hash_node *hn;
-	struct hlist_node *tmp;
-	int i;
-
-	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
-		hn->action = MLX5E_ACTION_DEL;
-	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
-		hn->action = MLX5E_ACTION_DEL;
-
-	if (!test_bit(MLX5E_STATE_DESTROYING, &priv->state))
-		mlx5e_sync_netdev_addr(priv);
-
-	mlx5e_apply_netdev_addr(priv);
-}
-
-void mlx5e_set_rx_mode_work(struct work_struct *work)
-{
-	struct mlx5e_priv *priv = container_of(work, struct mlx5e_priv,
-					       set_rx_mode_work);
-
-	struct mlx5e_eth_addr_db *ea = &priv->eth_addr;
-	struct net_device *ndev = priv->netdev;
-
-	bool rx_mode_enable   = !test_bit(MLX5E_STATE_DESTROYING, &priv->state);
-	bool promisc_enabled   = rx_mode_enable && (ndev->flags & IFF_PROMISC);
-	bool allmulti_enabled  = rx_mode_enable && (ndev->flags & IFF_ALLMULTI);
-	bool broadcast_enabled = rx_mode_enable;
-
-	bool enable_promisc    = !ea->promisc_enabled   &&  promisc_enabled;
-	bool disable_promisc   =  ea->promisc_enabled   && !promisc_enabled;
-	bool enable_allmulti   = !ea->allmulti_enabled  &&  allmulti_enabled;
-	bool disable_allmulti  =  ea->allmulti_enabled  && !allmulti_enabled;
-	bool enable_broadcast  = !ea->broadcast_enabled &&  broadcast_enabled;
-	bool disable_broadcast =  ea->broadcast_enabled && !broadcast_enabled;
-
-	if (enable_promisc) {
-		mlx5e_add_eth_addr_rule(priv, &ea->promisc, MLX5E_PROMISC);
-		if (!priv->vlan.filter_disabled)
-			mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
-					    0);
-	}
-	if (enable_allmulti)
-		mlx5e_add_eth_addr_rule(priv, &ea->allmulti, MLX5E_ALLMULTI);
-	if (enable_broadcast)
-		mlx5e_add_eth_addr_rule(priv, &ea->broadcast, MLX5E_FULLMATCH);
-
-	mlx5e_handle_netdev_addr(priv);
-
-	if (disable_broadcast)
-		mlx5e_del_eth_addr_from_flow_table(priv, &ea->broadcast);
-	if (disable_allmulti)
-		mlx5e_del_eth_addr_from_flow_table(priv, &ea->allmulti);
-	if (disable_promisc) {
-		if (!priv->vlan.filter_disabled)
-			mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
-					    0);
-		mlx5e_del_eth_addr_from_flow_table(priv, &ea->promisc);
-	}
-
-	ea->promisc_enabled   = promisc_enabled;
-	ea->allmulti_enabled  = allmulti_enabled;
-	ea->broadcast_enabled = broadcast_enabled;
-}
-
-void mlx5e_init_eth_addr(struct mlx5e_priv *priv)
-{
-	ether_addr_copy(priv->eth_addr.broadcast.addr, priv->netdev->broadcast);
-}
-
-static int mlx5e_create_main_flow_table(struct mlx5e_priv *priv)
-{
-	struct mlx5_flow_table_group *g;
-	u8 *dmac;
-
-	g = kcalloc(9, sizeof(*g), GFP_KERNEL);
-	if (!g)
-		return -ENOMEM;
-
-	g[0].log_sz = 3;
-	g[0].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
-			 outer_headers.ethertype);
-	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
-			 outer_headers.ip_protocol);
-
-	g[1].log_sz = 1;
-	g[1].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, g[1].match_criteria,
-			 outer_headers.ethertype);
-
-	g[2].log_sz = 0;
-
-	g[3].log_sz = 14;
-	g[3].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[3].match_criteria,
-			    outer_headers.dmac_47_16);
-	memset(dmac, 0xff, ETH_ALEN);
-	MLX5_SET_TO_ONES(fte_match_param, g[3].match_criteria,
-			 outer_headers.ethertype);
-	MLX5_SET_TO_ONES(fte_match_param, g[3].match_criteria,
-			 outer_headers.ip_protocol);
-
-	g[4].log_sz = 13;
-	g[4].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[4].match_criteria,
-			    outer_headers.dmac_47_16);
-	memset(dmac, 0xff, ETH_ALEN);
-	MLX5_SET_TO_ONES(fte_match_param, g[4].match_criteria,
-			 outer_headers.ethertype);
-
-	g[5].log_sz = 11;
-	g[5].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[5].match_criteria,
-			    outer_headers.dmac_47_16);
-	memset(dmac, 0xff, ETH_ALEN);
-
-	g[6].log_sz = 2;
-	g[6].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[6].match_criteria,
-			    outer_headers.dmac_47_16);
-	dmac[0] = 0x01;
-	MLX5_SET_TO_ONES(fte_match_param, g[6].match_criteria,
-			 outer_headers.ethertype);
-	MLX5_SET_TO_ONES(fte_match_param, g[6].match_criteria,
-			 outer_headers.ip_protocol);
-
-	g[7].log_sz = 1;
-	g[7].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[7].match_criteria,
-			    outer_headers.dmac_47_16);
-	dmac[0] = 0x01;
-	MLX5_SET_TO_ONES(fte_match_param, g[7].match_criteria,
-			 outer_headers.ethertype);
-
-	g[8].log_sz = 0;
-	g[8].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	dmac = MLX5_ADDR_OF(fte_match_param, g[8].match_criteria,
-			    outer_headers.dmac_47_16);
-	dmac[0] = 0x01;
-	priv->ft.main = mlx5_create_flow_table(priv->mdev, 1,
-					       MLX5_FLOW_TABLE_TYPE_NIC_RCV,
-					       9, g);
-	kfree(g);
-
-	return priv->ft.main ? 0 : -ENOMEM;
-}
-
-static void mlx5e_destroy_main_flow_table(struct mlx5e_priv *priv)
-{
-	mlx5_destroy_flow_table(priv->ft.main);
-}
-
-static int mlx5e_create_vlan_flow_table(struct mlx5e_priv *priv)
-{
-	struct mlx5_flow_table_group *g;
-
-	g = kcalloc(2, sizeof(*g), GFP_KERNEL);
-	if (!g)
-		return -ENOMEM;
-
-	g[0].log_sz = 12;
-	g[0].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
-			 outer_headers.vlan_tag);
-	MLX5_SET_TO_ONES(fte_match_param, g[0].match_criteria,
-			 outer_headers.first_vid);
-
-	/* untagged + any vlan id */
-	g[1].log_sz = 1;
-	g[1].match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
-	MLX5_SET_TO_ONES(fte_match_param, g[1].match_criteria,
-			 outer_headers.vlan_tag);
-
-	priv->ft.vlan = mlx5_create_flow_table(priv->mdev, 0,
-					       MLX5_FLOW_TABLE_TYPE_NIC_RCV,
-					       2, g);
-
-	kfree(g);
-	return priv->ft.vlan ? 0 : -ENOMEM;
-}
-
-static void mlx5e_destroy_vlan_flow_table(struct mlx5e_priv *priv)
-{
-	mlx5_destroy_flow_table(priv->ft.vlan);
-}
-
-int mlx5e_create_flow_tables(struct mlx5e_priv *priv)
-{
-	int err;
-
-	err = mlx5e_create_main_flow_table(priv);
-	if (err)
-		return err;
-
-	err = mlx5e_create_vlan_flow_table(priv);
-	if (err)
-		goto err_destroy_main_flow_table;
-
-	err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
-	if (err)
-		goto err_destroy_vlan_flow_table;
-
-	return 0;
-
-err_destroy_vlan_flow_table:
-	mlx5e_destroy_vlan_flow_table(priv);
-
-err_destroy_main_flow_table:
-	mlx5e_destroy_main_flow_table(priv);
-
-	return err;
-}
-
-void mlx5e_destroy_flow_tables(struct mlx5e_priv *priv)
-{
-	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
-	mlx5e_destroy_vlan_flow_table(priv);
-	mlx5e_destroy_main_flow_table(priv);
-}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
new file mode 100644
index 000000000000..80d81abc4820
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_fs.c
@@ -0,0 +1,1224 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/list.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/tcp.h>
+#include <linux/mlx5/fs.h>
+#include "en.h"
+
+#define MLX5_SET_CFG(p, f, v) MLX5_SET(create_flow_group_in, p, f, v)
+
+enum {
+	MLX5E_FULLMATCH = 0,
+	MLX5E_ALLMULTI  = 1,
+	MLX5E_PROMISC   = 2,
+};
+
+enum {
+	MLX5E_UC        = 0,
+	MLX5E_MC_IPV4   = 1,
+	MLX5E_MC_IPV6   = 2,
+	MLX5E_MC_OTHER  = 3,
+};
+
+enum {
+	MLX5E_ACTION_NONE = 0,
+	MLX5E_ACTION_ADD  = 1,
+	MLX5E_ACTION_DEL  = 2,
+};
+
+struct mlx5e_eth_addr_hash_node {
+	struct hlist_node          hlist;
+	u8                         action;
+	struct mlx5e_eth_addr_info ai;
+};
+
+static inline int mlx5e_hash_eth_addr(u8 *addr)
+{
+	return addr[5];
+}
+
+static void mlx5e_add_eth_addr_to_hash(struct hlist_head *hash, u8 *addr)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	int ix = mlx5e_hash_eth_addr(addr);
+	int found = 0;
+
+	hlist_for_each_entry(hn, &hash[ix], hlist)
+		if (ether_addr_equal_64bits(hn->ai.addr, addr)) {
+			found = 1;
+			break;
+		}
+
+	if (found) {
+		hn->action = MLX5E_ACTION_NONE;
+		return;
+	}
+
+	hn = kzalloc(sizeof(*hn), GFP_ATOMIC);
+	if (!hn)
+		return;
+
+	ether_addr_copy(hn->ai.addr, addr);
+	hn->action = MLX5E_ACTION_ADD;
+
+	hlist_add_head(&hn->hlist, &hash[ix]);
+}
+
+static void mlx5e_del_eth_addr_from_hash(struct mlx5e_eth_addr_hash_node *hn)
+{
+	hlist_del(&hn->hlist);
+	kfree(hn);
+}
+
+static void mlx5e_del_eth_addr_from_flow_table(struct mlx5e_priv *priv,
+					       struct mlx5e_eth_addr_info *ai)
+{
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_ESP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV6_IPSEC_ESP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_ESP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV4_IPSEC_ESP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_AH))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV6_IPSEC_AH]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_AH))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV4_IPSEC_AH]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_TCP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV6_TCP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_TCP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV4_TCP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV6_UDP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV6_UDP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV4_UDP))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV4_UDP]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV6))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV6]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_IPV4))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_IPV4]);
+
+	if (ai->tt_vec & BIT(MLX5E_TT_ANY))
+		mlx5_del_flow_rule(ai->ft_rule[MLX5E_TT_ANY]);
+}
+
+static int mlx5e_get_eth_addr_type(u8 *addr)
+{
+	if (is_unicast_ether_addr(addr))
+		return MLX5E_UC;
+
+	if ((addr[0] == 0x01) &&
+	    (addr[1] == 0x00) &&
+	    (addr[2] == 0x5e) &&
+	   !(addr[3] &  0x80))
+		return MLX5E_MC_IPV4;
+
+	if ((addr[0] == 0x33) &&
+	    (addr[1] == 0x33))
+		return MLX5E_MC_IPV6;
+
+	return MLX5E_MC_OTHER;
+}
+
+static u32 mlx5e_get_tt_vec(struct mlx5e_eth_addr_info *ai, int type)
+{
+	int eth_addr_type;
+	u32 ret;
+
+	switch (type) {
+	case MLX5E_FULLMATCH:
+		eth_addr_type = mlx5e_get_eth_addr_type(ai->addr);
+		switch (eth_addr_type) {
+		case MLX5E_UC:
+			ret =
+				BIT(MLX5E_TT_IPV4_TCP)       |
+				BIT(MLX5E_TT_IPV6_TCP)       |
+				BIT(MLX5E_TT_IPV4_UDP)       |
+				BIT(MLX5E_TT_IPV6_UDP)       |
+				BIT(MLX5E_TT_IPV4_IPSEC_AH)  |
+				BIT(MLX5E_TT_IPV6_IPSEC_AH)  |
+				BIT(MLX5E_TT_IPV4_IPSEC_ESP) |
+				BIT(MLX5E_TT_IPV6_IPSEC_ESP) |
+				BIT(MLX5E_TT_IPV4)           |
+				BIT(MLX5E_TT_IPV6)           |
+				BIT(MLX5E_TT_ANY)            |
+				0;
+			break;
+
+		case MLX5E_MC_IPV4:
+			ret =
+				BIT(MLX5E_TT_IPV4_UDP)       |
+				BIT(MLX5E_TT_IPV4)           |
+				0;
+			break;
+
+		case MLX5E_MC_IPV6:
+			ret =
+				BIT(MLX5E_TT_IPV6_UDP)       |
+				BIT(MLX5E_TT_IPV6)           |
+				0;
+			break;
+
+		case MLX5E_MC_OTHER:
+			ret =
+				BIT(MLX5E_TT_ANY)            |
+				0;
+			break;
+		}
+
+		break;
+
+	case MLX5E_ALLMULTI:
+		ret =
+			BIT(MLX5E_TT_IPV4_UDP) |
+			BIT(MLX5E_TT_IPV6_UDP) |
+			BIT(MLX5E_TT_IPV4)     |
+			BIT(MLX5E_TT_IPV6)     |
+			BIT(MLX5E_TT_ANY)      |
+			0;
+		break;
+
+	default: /* MLX5E_PROMISC */
+		ret =
+			BIT(MLX5E_TT_IPV4_TCP)       |
+			BIT(MLX5E_TT_IPV6_TCP)       |
+			BIT(MLX5E_TT_IPV4_UDP)       |
+			BIT(MLX5E_TT_IPV6_UDP)       |
+			BIT(MLX5E_TT_IPV4_IPSEC_AH)  |
+			BIT(MLX5E_TT_IPV6_IPSEC_AH)  |
+			BIT(MLX5E_TT_IPV4_IPSEC_ESP) |
+			BIT(MLX5E_TT_IPV6_IPSEC_ESP) |
+			BIT(MLX5E_TT_IPV4)           |
+			BIT(MLX5E_TT_IPV6)           |
+			BIT(MLX5E_TT_ANY)            |
+			0;
+		break;
+	}
+
+	return ret;
+}
+
+static int __mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
+				     struct mlx5e_eth_addr_info *ai,
+				     int type, u32 *mc, u32 *mv)
+{
+	struct mlx5_flow_destination dest;
+	u8 match_criteria_enable = 0;
+	struct mlx5_flow_rule **rule_p;
+	struct mlx5_flow_table *ft = priv->fts.main.t;
+	u8 *mc_dmac = MLX5_ADDR_OF(fte_match_param, mc,
+				   outer_headers.dmac_47_16);
+	u8 *mv_dmac = MLX5_ADDR_OF(fte_match_param, mv,
+				   outer_headers.dmac_47_16);
+	u32 *tirn = priv->tirn;
+	u32 tt_vec;
+	int err = 0;
+
+	dest.type = MLX5_FLOW_DESTINATION_TYPE_TIR;
+
+	switch (type) {
+	case MLX5E_FULLMATCH:
+		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+		eth_broadcast_addr(mc_dmac);
+		ether_addr_copy(mv_dmac, ai->addr);
+		break;
+
+	case MLX5E_ALLMULTI:
+		match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+		mc_dmac[0] = 0x01;
+		mv_dmac[0] = 0x01;
+		break;
+
+	case MLX5E_PROMISC:
+		break;
+	}
+
+	tt_vec = mlx5e_get_tt_vec(ai, type);
+
+	if (tt_vec & BIT(MLX5E_TT_ANY)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_ANY];
+		dest.tir_num = tirn[MLX5E_TT_ANY];
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_ANY);
+	}
+
+	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+
+	if (tt_vec & BIT(MLX5E_TT_IPV4)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV4];
+		dest.tir_num = tirn[MLX5E_TT_IPV4];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IP);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV4);
+	}
+
+	if (tt_vec & BIT(MLX5E_TT_IPV6)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV6];
+		dest.tir_num = tirn[MLX5E_TT_IPV6];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV6);
+	}
+
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ip_protocol);
+	MLX5_SET(fte_match_param, mv, outer_headers.ip_protocol, IPPROTO_UDP);
+
+	if (tt_vec & BIT(MLX5E_TT_IPV4_UDP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV4_UDP];
+		dest.tir_num = tirn[MLX5E_TT_IPV4_UDP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IP);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV4_UDP);
+	}
+
+	if (tt_vec & BIT(MLX5E_TT_IPV6_UDP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV6_UDP];
+		dest.tir_num = tirn[MLX5E_TT_IPV6_UDP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV6_UDP);
+	}
+
+	MLX5_SET(fte_match_param, mv, outer_headers.ip_protocol, IPPROTO_TCP);
+
+	if (tt_vec & BIT(MLX5E_TT_IPV4_TCP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV4_TCP];
+		dest.tir_num = tirn[MLX5E_TT_IPV4_TCP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IP);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV4_TCP);
+	}
+
+	if (tt_vec & BIT(MLX5E_TT_IPV6_TCP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV6_TCP];
+		dest.tir_num = tirn[MLX5E_TT_IPV6_TCP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+
+		ai->tt_vec |= BIT(MLX5E_TT_IPV6_TCP);
+	}
+
+	MLX5_SET(fte_match_param, mv, outer_headers.ip_protocol, IPPROTO_AH);
+
+	if (tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_AH)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV4_IPSEC_AH];
+		dest.tir_num = tirn[MLX5E_TT_IPV4_IPSEC_AH];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IP);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV4_IPSEC_AH);
+	}
+
+	if (tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_AH)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV6_IPSEC_AH];
+		dest.tir_num = tirn[MLX5E_TT_IPV6_IPSEC_AH];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV6_IPSEC_AH);
+	}
+
+	MLX5_SET(fte_match_param, mv, outer_headers.ip_protocol, IPPROTO_ESP);
+
+	if (tt_vec & BIT(MLX5E_TT_IPV4_IPSEC_ESP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV4_IPSEC_ESP];
+		dest.tir_num = tirn[MLX5E_TT_IPV4_IPSEC_ESP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IP);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV4_IPSEC_ESP);
+	}
+
+	if (tt_vec & BIT(MLX5E_TT_IPV6_IPSEC_ESP)) {
+		rule_p = &ai->ft_rule[MLX5E_TT_IPV6_IPSEC_ESP];
+		dest.tir_num = tirn[MLX5E_TT_IPV6_IPSEC_ESP];
+		MLX5_SET(fte_match_param, mv, outer_headers.ethertype,
+			 ETH_P_IPV6);
+		*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+					     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+					     MLX5_FS_DEFAULT_FLOW_TAG, &dest);
+		if (IS_ERR_OR_NULL(*rule_p))
+			goto err_del_ai;
+		ai->tt_vec |= BIT(MLX5E_TT_IPV6_IPSEC_ESP);
+	}
+
+	return 0;
+
+err_del_ai:
+	err = PTR_ERR(*rule_p);
+	*rule_p = NULL;
+	mlx5e_del_eth_addr_from_flow_table(priv, ai);
+
+	return err;
+}
+
+static int mlx5e_add_eth_addr_rule(struct mlx5e_priv *priv,
+				   struct mlx5e_eth_addr_info *ai, int type)
+{
+	u32 *match_criteria;
+	u32 *match_value;
+	int err = 0;
+
+	match_value	= mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	match_criteria	= mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	if (!match_value || !match_criteria) {
+		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+		err = -ENOMEM;
+		goto add_eth_addr_rule_out;
+	}
+
+	err = __mlx5e_add_eth_addr_rule(priv, ai, type, match_criteria,
+					match_value);
+
+add_eth_addr_rule_out:
+	kvfree(match_criteria);
+	kvfree(match_value);
+
+	return err;
+}
+
+static int mlx5e_vport_context_update_vlans(struct mlx5e_priv *priv)
+{
+	struct net_device *ndev = priv->netdev;
+	int max_list_size;
+	int list_size;
+	u16 *vlans;
+	int vlan;
+	int err;
+	int i;
+
+	list_size = 0;
+	for_each_set_bit(vlan, priv->vlan.active_vlans, VLAN_N_VID)
+		list_size++;
+
+	max_list_size = 1 << MLX5_CAP_GEN(priv->mdev, log_max_vlan_list);
+
+	if (list_size > max_list_size) {
+		netdev_warn(ndev,
+			    "netdev vlans list size (%d) > (%d) max vport list size, some vlans will be dropped\n",
+			    list_size, max_list_size);
+		list_size = max_list_size;
+	}
+
+	vlans = kcalloc(list_size, sizeof(*vlans), GFP_KERNEL);
+	if (!vlans)
+		return -ENOMEM;
+
+	i = 0;
+	for_each_set_bit(vlan, priv->vlan.active_vlans, VLAN_N_VID) {
+		if (i >= list_size)
+			break;
+		vlans[i++] = vlan;
+	}
+
+	err = mlx5_modify_nic_vport_vlans(priv->mdev, vlans, list_size);
+	if (err)
+		netdev_err(ndev, "Failed to modify vport vlans list err(%d)\n",
+			   err);
+
+	kfree(vlans);
+	return err;
+}
+
+enum mlx5e_vlan_rule_type {
+	MLX5E_VLAN_RULE_TYPE_UNTAGGED,
+	MLX5E_VLAN_RULE_TYPE_ANY_VID,
+	MLX5E_VLAN_RULE_TYPE_MATCH_VID,
+};
+
+static int __mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
+				 enum mlx5e_vlan_rule_type rule_type,
+				 u16 vid, u32 *mc, u32 *mv)
+{
+	struct mlx5_flow_table *ft = priv->fts.vlan.t;
+	struct mlx5_flow_destination dest;
+	u8 match_criteria_enable = 0;
+	struct mlx5_flow_rule **rule_p;
+	int err = 0;
+
+	dest.type = MLX5_FLOW_DESTINATION_TYPE_FLOW_TABLE;
+	dest.ft = priv->fts.main.t;
+
+	match_criteria_enable = MLX5_MATCH_OUTER_HEADERS;
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.vlan_tag);
+
+	switch (rule_type) {
+	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
+		rule_p = &priv->vlan.untagged_rule;
+		break;
+	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
+		rule_p = &priv->vlan.any_vlan_rule;
+		MLX5_SET(fte_match_param, mv, outer_headers.vlan_tag, 1);
+		break;
+	default: /* MLX5E_VLAN_RULE_TYPE_MATCH_VID */
+		rule_p = &priv->vlan.active_vlans_rule[vid];
+		MLX5_SET(fte_match_param, mv, outer_headers.vlan_tag, 1);
+		MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.first_vid);
+		MLX5_SET(fte_match_param, mv, outer_headers.first_vid, vid);
+		break;
+	}
+
+	*rule_p = mlx5_add_flow_rule(ft, match_criteria_enable, mc, mv,
+				     MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+				     MLX5_FS_DEFAULT_FLOW_TAG,
+				     &dest);
+
+	if (IS_ERR(*rule_p)) {
+		err = PTR_ERR(*rule_p);
+		*rule_p = NULL;
+		netdev_err(priv->netdev, "%s: add rule failed\n", __func__);
+	}
+
+	return err;
+}
+
+static int mlx5e_add_vlan_rule(struct mlx5e_priv *priv,
+			       enum mlx5e_vlan_rule_type rule_type, u16 vid)
+{
+	u32 *match_criteria;
+	u32 *match_value;
+	int err = 0;
+
+	match_value	= mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	match_criteria	= mlx5_vzalloc(MLX5_ST_SZ_BYTES(fte_match_param));
+	if (!match_value || !match_criteria) {
+		netdev_err(priv->netdev, "%s: alloc failed\n", __func__);
+		err = -ENOMEM;
+		goto add_vlan_rule_out;
+	}
+
+	if (rule_type == MLX5E_VLAN_RULE_TYPE_MATCH_VID)
+		mlx5e_vport_context_update_vlans(priv);
+
+	err = __mlx5e_add_vlan_rule(priv, rule_type, vid, match_criteria,
+				    match_value);
+
+add_vlan_rule_out:
+	kvfree(match_criteria);
+	kvfree(match_value);
+
+	return err;
+}
+
+static void mlx5e_del_vlan_rule(struct mlx5e_priv *priv,
+				enum mlx5e_vlan_rule_type rule_type, u16 vid)
+{
+	switch (rule_type) {
+	case MLX5E_VLAN_RULE_TYPE_UNTAGGED:
+		if (priv->vlan.untagged_rule) {
+			mlx5_del_flow_rule(priv->vlan.untagged_rule);
+			priv->vlan.untagged_rule = NULL;
+		}
+		break;
+	case MLX5E_VLAN_RULE_TYPE_ANY_VID:
+		if (priv->vlan.any_vlan_rule) {
+			mlx5_del_flow_rule(priv->vlan.any_vlan_rule);
+			priv->vlan.any_vlan_rule = NULL;
+		}
+		break;
+	case MLX5E_VLAN_RULE_TYPE_MATCH_VID:
+		mlx5e_vport_context_update_vlans(priv);
+		if (priv->vlan.active_vlans_rule[vid]) {
+			mlx5_del_flow_rule(priv->vlan.active_vlans_rule[vid]);
+			priv->vlan.active_vlans_rule[vid] = NULL;
+		}
+		mlx5e_vport_context_update_vlans(priv);
+		break;
+	}
+}
+
+void mlx5e_enable_vlan_filter(struct mlx5e_priv *priv)
+{
+	if (!priv->vlan.filter_disabled)
+		return;
+
+	priv->vlan.filter_disabled = false;
+	if (priv->netdev->flags & IFF_PROMISC)
+		return;
+	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
+}
+
+void mlx5e_disable_vlan_filter(struct mlx5e_priv *priv)
+{
+	if (priv->vlan.filter_disabled)
+		return;
+
+	priv->vlan.filter_disabled = true;
+	if (priv->netdev->flags & IFF_PROMISC)
+		return;
+	mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID, 0);
+}
+
+int mlx5e_vlan_rx_add_vid(struct net_device *dev, __always_unused __be16 proto,
+			  u16 vid)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	set_bit(vid, priv->vlan.active_vlans);
+
+	return mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
+}
+
+int mlx5e_vlan_rx_kill_vid(struct net_device *dev, __always_unused __be16 proto,
+			   u16 vid)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+
+	clear_bit(vid, priv->vlan.active_vlans);
+
+	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_MATCH_VID, vid);
+
+	return 0;
+}
+
+#define mlx5e_for_each_hash_node(hn, tmp, hash, i) \
+	for (i = 0; i < MLX5E_ETH_ADDR_HASH_SIZE; i++) \
+		hlist_for_each_entry_safe(hn, tmp, &hash[i], hlist)
+
+static void mlx5e_execute_action(struct mlx5e_priv *priv,
+				 struct mlx5e_eth_addr_hash_node *hn)
+{
+	switch (hn->action) {
+	case MLX5E_ACTION_ADD:
+		mlx5e_add_eth_addr_rule(priv, &hn->ai, MLX5E_FULLMATCH);
+		hn->action = MLX5E_ACTION_NONE;
+		break;
+
+	case MLX5E_ACTION_DEL:
+		mlx5e_del_eth_addr_from_flow_table(priv, &hn->ai);
+		mlx5e_del_eth_addr_from_hash(hn);
+		break;
+	}
+}
+
+static void mlx5e_sync_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct net_device *netdev = priv->netdev;
+	struct netdev_hw_addr *ha;
+
+	netif_addr_lock_bh(netdev);
+
+	mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc,
+				   priv->netdev->dev_addr);
+
+	netdev_for_each_uc_addr(ha, netdev)
+		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_uc, ha->addr);
+
+	netdev_for_each_mc_addr(ha, netdev)
+		mlx5e_add_eth_addr_to_hash(priv->eth_addr.netdev_mc, ha->addr);
+
+	netif_addr_unlock_bh(netdev);
+}
+
+static void mlx5e_fill_addr_array(struct mlx5e_priv *priv, int list_type,
+				  u8 addr_array[][ETH_ALEN], int size)
+{
+	bool is_uc = (list_type == MLX5_NVPRT_LIST_TYPE_UC);
+	struct net_device *ndev = priv->netdev;
+	struct mlx5e_eth_addr_hash_node *hn;
+	struct hlist_head *addr_list;
+	struct hlist_node *tmp;
+	int i = 0;
+	int hi;
+
+	addr_list = is_uc ? priv->eth_addr.netdev_uc : priv->eth_addr.netdev_mc;
+
+	if (is_uc) /* Make sure our own address is pushed first */
+		ether_addr_copy(addr_array[i++], ndev->dev_addr);
+	else if (priv->eth_addr.broadcast_enabled)
+		ether_addr_copy(addr_array[i++], ndev->broadcast);
+
+	mlx5e_for_each_hash_node(hn, tmp, addr_list, hi) {
+		if (ether_addr_equal(ndev->dev_addr, hn->ai.addr))
+			continue;
+		if (i >= size)
+			break;
+		ether_addr_copy(addr_array[i++], hn->ai.addr);
+	}
+}
+
+static void mlx5e_vport_context_update_addr_list(struct mlx5e_priv *priv,
+						 int list_type)
+{
+	bool is_uc = (list_type == MLX5_NVPRT_LIST_TYPE_UC);
+	struct mlx5e_eth_addr_hash_node *hn;
+	u8 (*addr_array)[ETH_ALEN] = NULL;
+	struct hlist_head *addr_list;
+	struct hlist_node *tmp;
+	int max_size;
+	int size;
+	int err;
+	int hi;
+
+	size = is_uc ? 0 : (priv->eth_addr.broadcast_enabled ? 1 : 0);
+	max_size = is_uc ?
+		1 << MLX5_CAP_GEN(priv->mdev, log_max_current_uc_list) :
+		1 << MLX5_CAP_GEN(priv->mdev, log_max_current_mc_list);
+
+	addr_list = is_uc ? priv->eth_addr.netdev_uc : priv->eth_addr.netdev_mc;
+	mlx5e_for_each_hash_node(hn, tmp, addr_list, hi)
+		size++;
+
+	if (size > max_size) {
+		netdev_warn(priv->netdev,
+			    "netdev %s list size (%d) > (%d) max vport list size, some addresses will be dropped\n",
+			    is_uc ? "UC" : "MC", size, max_size);
+		size = max_size;
+	}
+
+	if (size) {
+		addr_array = kcalloc(size, ETH_ALEN, GFP_KERNEL);
+		if (!addr_array) {
+			err = -ENOMEM;
+			goto out;
+		}
+		mlx5e_fill_addr_array(priv, list_type, addr_array, size);
+	}
+
+	err = mlx5_modify_nic_vport_mac_list(priv->mdev, list_type, addr_array, size);
+out:
+	if (err)
+		netdev_err(priv->netdev,
+			   "Failed to modify vport %s list err(%d)\n",
+			   is_uc ? "UC" : "MC", err);
+	kfree(addr_array);
+}
+
+static void mlx5e_vport_context_update(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_db *ea = &priv->eth_addr;
+
+	mlx5e_vport_context_update_addr_list(priv, MLX5_NVPRT_LIST_TYPE_UC);
+	mlx5e_vport_context_update_addr_list(priv, MLX5_NVPRT_LIST_TYPE_MC);
+	mlx5_modify_nic_vport_promisc(priv->mdev, 0,
+				      ea->allmulti_enabled,
+				      ea->promisc_enabled);
+}
+
+static void mlx5e_apply_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	struct hlist_node *tmp;
+	int i;
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
+		mlx5e_execute_action(priv, hn);
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
+		mlx5e_execute_action(priv, hn);
+}
+
+static void mlx5e_handle_netdev_addr(struct mlx5e_priv *priv)
+{
+	struct mlx5e_eth_addr_hash_node *hn;
+	struct hlist_node *tmp;
+	int i;
+
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_uc, i)
+		hn->action = MLX5E_ACTION_DEL;
+	mlx5e_for_each_hash_node(hn, tmp, priv->eth_addr.netdev_mc, i)
+		hn->action = MLX5E_ACTION_DEL;
+
+	if (!test_bit(MLX5E_STATE_DESTROYING, &priv->state))
+		mlx5e_sync_netdev_addr(priv);
+
+	mlx5e_apply_netdev_addr(priv);
+}
+
+void mlx5e_set_rx_mode_work(struct work_struct *work)
+{
+	struct mlx5e_priv *priv = container_of(work, struct mlx5e_priv,
+					       set_rx_mode_work);
+
+	struct mlx5e_eth_addr_db *ea = &priv->eth_addr;
+	struct net_device *ndev = priv->netdev;
+
+	bool rx_mode_enable   = !test_bit(MLX5E_STATE_DESTROYING, &priv->state);
+	bool promisc_enabled   = rx_mode_enable && (ndev->flags & IFF_PROMISC);
+	bool allmulti_enabled  = rx_mode_enable && (ndev->flags & IFF_ALLMULTI);
+	bool broadcast_enabled = rx_mode_enable;
+
+	bool enable_promisc    = !ea->promisc_enabled   &&  promisc_enabled;
+	bool disable_promisc   =  ea->promisc_enabled   && !promisc_enabled;
+	bool enable_allmulti   = !ea->allmulti_enabled  &&  allmulti_enabled;
+	bool disable_allmulti  =  ea->allmulti_enabled  && !allmulti_enabled;
+	bool enable_broadcast  = !ea->broadcast_enabled &&  broadcast_enabled;
+	bool disable_broadcast =  ea->broadcast_enabled && !broadcast_enabled;
+
+	if (enable_promisc) {
+		mlx5e_add_eth_addr_rule(priv, &ea->promisc, MLX5E_PROMISC);
+		if (!priv->vlan.filter_disabled)
+			mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
+					    0);
+	}
+	if (enable_allmulti)
+		mlx5e_add_eth_addr_rule(priv, &ea->allmulti, MLX5E_ALLMULTI);
+	if (enable_broadcast)
+		mlx5e_add_eth_addr_rule(priv, &ea->broadcast, MLX5E_FULLMATCH);
+
+	mlx5e_handle_netdev_addr(priv);
+
+	if (disable_broadcast)
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->broadcast);
+	if (disable_allmulti)
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->allmulti);
+	if (disable_promisc) {
+		if (!priv->vlan.filter_disabled)
+			mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_ANY_VID,
+					    0);
+		mlx5e_del_eth_addr_from_flow_table(priv, &ea->promisc);
+	}
+
+	ea->promisc_enabled   = promisc_enabled;
+	ea->allmulti_enabled  = allmulti_enabled;
+	ea->broadcast_enabled = broadcast_enabled;
+
+	mlx5e_vport_context_update(priv);
+}
+
+static void mlx5e_destroy_groups(struct mlx5e_flow_table *ft)
+{
+	int i;
+
+	for (i = ft->num_groups - 1; i >= 0; i--) {
+		if (!IS_ERR_OR_NULL(ft->g[i]))
+			mlx5_destroy_flow_group(ft->g[i]);
+		ft->g[i] = NULL;
+	}
+	ft->num_groups = 0;
+}
+
+void mlx5e_init_eth_addr(struct mlx5e_priv *priv)
+{
+	ether_addr_copy(priv->eth_addr.broadcast.addr, priv->netdev->broadcast);
+}
+
+#define MLX5E_MAIN_GROUP0_SIZE	BIT(3)
+#define MLX5E_MAIN_GROUP1_SIZE	BIT(1)
+#define MLX5E_MAIN_GROUP2_SIZE	BIT(0)
+#define MLX5E_MAIN_GROUP3_SIZE	BIT(14)
+#define MLX5E_MAIN_GROUP4_SIZE	BIT(13)
+#define MLX5E_MAIN_GROUP5_SIZE	BIT(11)
+#define MLX5E_MAIN_GROUP6_SIZE	BIT(2)
+#define MLX5E_MAIN_GROUP7_SIZE	BIT(1)
+#define MLX5E_MAIN_GROUP8_SIZE	BIT(0)
+#define MLX5E_MAIN_TABLE_SIZE	(MLX5E_MAIN_GROUP0_SIZE +\
+				 MLX5E_MAIN_GROUP1_SIZE +\
+				 MLX5E_MAIN_GROUP2_SIZE +\
+				 MLX5E_MAIN_GROUP3_SIZE +\
+				 MLX5E_MAIN_GROUP4_SIZE +\
+				 MLX5E_MAIN_GROUP5_SIZE +\
+				 MLX5E_MAIN_GROUP6_SIZE +\
+				 MLX5E_MAIN_GROUP7_SIZE +\
+				 MLX5E_MAIN_GROUP8_SIZE)
+
+static int __mlx5e_create_main_groups(struct mlx5e_flow_table *ft, u32 *in,
+				      int inlen)
+{
+	u8 *mc = MLX5_ADDR_OF(create_flow_group_in, in, match_criteria);
+	u8 *dmac = MLX5_ADDR_OF(create_flow_group_in, in,
+				match_criteria.outer_headers.dmac_47_16);
+	int err;
+	int ix = 0;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ip_protocol);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP0_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP1_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP2_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ip_protocol);
+	eth_broadcast_addr(dmac);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP3_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	eth_broadcast_addr(dmac);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP4_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	eth_broadcast_addr(dmac);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP5_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ip_protocol);
+	dmac[0] = 0x01;
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP6_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.ethertype);
+	dmac[0] = 0x01;
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP7_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	dmac[0] = 0x01;
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_MAIN_GROUP8_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	return 0;
+
+err_destroy_groups:
+	err = PTR_ERR(ft->g[ft->num_groups]);
+	ft->g[ft->num_groups] = NULL;
+	mlx5e_destroy_groups(ft);
+
+	return err;
+}
+
+static int mlx5e_create_main_groups(struct mlx5e_flow_table *ft)
+{
+	u32 *in;
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	err = __mlx5e_create_main_groups(ft, in, inlen);
+
+	kvfree(in);
+	return err;
+}
+
+static int mlx5e_create_main_flow_table(struct mlx5e_priv *priv)
+{
+	struct mlx5e_flow_table *ft = &priv->fts.main;
+	int err;
+
+	ft->num_groups = 0;
+	ft->t = mlx5_create_flow_table(priv->fts.ns, 0, MLX5E_MAIN_TABLE_SIZE);
+
+	if (IS_ERR(ft->t)) {
+		err = PTR_ERR(ft->t);
+		ft->t = NULL;
+		return err;
+	}
+	ft->g = kcalloc(MLX5E_NUM_MAIN_GROUPS, sizeof(*ft->g), GFP_KERNEL);
+	if (!ft->g) {
+		err = -ENOMEM;
+		goto err_destroy_main_flow_table;
+	}
+
+	err = mlx5e_create_main_groups(ft);
+	if (err)
+		goto err_free_g;
+	return 0;
+
+err_free_g:
+	kfree(ft->g);
+
+err_destroy_main_flow_table:
+	mlx5_destroy_flow_table(ft->t);
+	ft->t = NULL;
+
+	return err;
+}
+
+static void mlx5e_destroy_flow_table(struct mlx5e_flow_table *ft)
+{
+	mlx5e_destroy_groups(ft);
+	kfree(ft->g);
+	mlx5_destroy_flow_table(ft->t);
+	ft->t = NULL;
+}
+
+static void mlx5e_destroy_main_flow_table(struct mlx5e_priv *priv)
+{
+	mlx5e_destroy_flow_table(&priv->fts.main);
+}
+
+#define MLX5E_NUM_VLAN_GROUPS	2
+#define MLX5E_VLAN_GROUP0_SIZE	BIT(12)
+#define MLX5E_VLAN_GROUP1_SIZE	BIT(1)
+#define MLX5E_VLAN_TABLE_SIZE	(MLX5E_VLAN_GROUP0_SIZE +\
+				 MLX5E_VLAN_GROUP1_SIZE)
+
+static int __mlx5e_create_vlan_groups(struct mlx5e_flow_table *ft, u32 *in,
+				      int inlen)
+{
+	int err;
+	int ix = 0;
+	u8 *mc = MLX5_ADDR_OF(create_flow_group_in, in, match_criteria);
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.vlan_tag);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.first_vid);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_VLAN_GROUP0_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	memset(in, 0, inlen);
+	MLX5_SET_CFG(in, match_criteria_enable, MLX5_MATCH_OUTER_HEADERS);
+	MLX5_SET_TO_ONES(fte_match_param, mc, outer_headers.vlan_tag);
+	MLX5_SET_CFG(in, start_flow_index, ix);
+	ix += MLX5E_VLAN_GROUP1_SIZE;
+	MLX5_SET_CFG(in, end_flow_index, ix - 1);
+	ft->g[ft->num_groups] = mlx5_create_flow_group(ft->t, in);
+	if (IS_ERR(ft->g[ft->num_groups]))
+		goto err_destroy_groups;
+	ft->num_groups++;
+
+	return 0;
+
+err_destroy_groups:
+	err = PTR_ERR(ft->g[ft->num_groups]);
+	ft->g[ft->num_groups] = NULL;
+	mlx5e_destroy_groups(ft);
+
+	return err;
+}
+
+static int mlx5e_create_vlan_groups(struct mlx5e_flow_table *ft)
+{
+	u32 *in;
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return -ENOMEM;
+
+	err = __mlx5e_create_vlan_groups(ft, in, inlen);
+
+	kvfree(in);
+	return err;
+}
+
+static int mlx5e_create_vlan_flow_table(struct mlx5e_priv *priv)
+{
+	struct mlx5e_flow_table *ft = &priv->fts.vlan;
+	int err;
+
+	ft->num_groups = 0;
+	ft->t = mlx5_create_flow_table(priv->fts.ns, 0, MLX5E_VLAN_TABLE_SIZE);
+
+	if (IS_ERR(ft->t)) {
+		err = PTR_ERR(ft->t);
+		ft->t = NULL;
+		return err;
+	}
+	ft->g = kcalloc(MLX5E_NUM_VLAN_GROUPS, sizeof(*ft->g), GFP_KERNEL);
+	if (!ft->g) {
+		err = -ENOMEM;
+		goto err_destroy_vlan_flow_table;
+	}
+
+	err = mlx5e_create_vlan_groups(ft);
+	if (err)
+		goto err_free_g;
+
+	return 0;
+
+err_free_g:
+	kfree(ft->g);
+
+err_destroy_vlan_flow_table:
+	mlx5_destroy_flow_table(ft->t);
+	ft->t = NULL;
+
+	return err;
+}
+
+static void mlx5e_destroy_vlan_flow_table(struct mlx5e_priv *priv)
+{
+	mlx5e_destroy_flow_table(&priv->fts.vlan);
+}
+
+int mlx5e_create_flow_tables(struct mlx5e_priv *priv)
+{
+	int err;
+
+	priv->fts.ns = mlx5_get_flow_namespace(priv->mdev,
+					       MLX5_FLOW_NAMESPACE_KERNEL);
+
+	if (!priv->fts.ns)
+		return -EINVAL;
+
+	err = mlx5e_create_vlan_flow_table(priv);
+	if (err)
+		return err;
+
+	err = mlx5e_create_main_flow_table(priv);
+	if (err)
+		goto err_destroy_vlan_flow_table;
+
+	err = mlx5e_add_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+	if (err)
+		goto err_destroy_main_flow_table;
+
+	return 0;
+
+err_destroy_main_flow_table:
+	mlx5e_destroy_main_flow_table(priv);
+err_destroy_vlan_flow_table:
+	mlx5e_destroy_vlan_flow_table(priv);
+
+	return err;
+}
+
+void mlx5e_destroy_flow_tables(struct mlx5e_priv *priv)
+{
+	mlx5e_del_vlan_rule(priv, MLX5E_VLAN_RULE_TYPE_UNTAGGED, 0);
+	mlx5e_destroy_main_flow_table(priv);
+	mlx5e_destroy_vlan_flow_table(priv);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
index 1e52db32c73d..402994bf7e16 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_main.c
@@ -30,8 +30,9 @@
  * SOFTWARE.
  */
 
-#include <linux/mlx5/flow_table.h>
+#include <linux/mlx5/fs.h>
 #include "en.h"
+#include "eswitch.h"
 
 struct mlx5e_rq_param {
 	u32                        rqc[MLX5_ST_SZ_DW(rqc)];
@@ -63,7 +64,7 @@ static void mlx5e_update_carrier(struct mlx5e_priv *priv)
 	u8 port_state;
 
 	port_state = mlx5_query_vport_state(mdev,
-		MLX5_QUERY_VPORT_STATE_IN_OP_MOD_VNIC_VPORT);
+		MLX5_QUERY_VPORT_STATE_IN_OP_MOD_VNIC_VPORT, 0);
 
 	if (port_state == VPORT_STATE_UP)
 		netif_carrier_on(priv->netdev);
@@ -140,6 +141,10 @@ void mlx5e_update_stats(struct mlx5e_priv *priv)
 		return;
 
 	/* Collect firts the SW counters and then HW for consistency */
+	s->rx_packets		= 0;
+	s->rx_bytes		= 0;
+	s->tx_packets		= 0;
+	s->tx_bytes		= 0;
 	s->tso_packets		= 0;
 	s->tso_bytes		= 0;
 	s->tx_queue_stopped	= 0;
@@ -154,6 +159,8 @@ void mlx5e_update_stats(struct mlx5e_priv *priv)
 	for (i = 0; i < priv->params.num_channels; i++) {
 		rq_stats = &priv->channel[i]->rq.stats;
 
+		s->rx_packets	+= rq_stats->packets;
+		s->rx_bytes	+= rq_stats->bytes;
 		s->lro_packets	+= rq_stats->lro_packets;
 		s->lro_bytes	+= rq_stats->lro_bytes;
 		s->rx_csum_none	+= rq_stats->csum_none;
@@ -163,6 +170,8 @@ void mlx5e_update_stats(struct mlx5e_priv *priv)
 		for (j = 0; j < priv->params.num_tc; j++) {
 			sq_stats = &priv->channel[i]->sq[j].stats;
 
+			s->tx_packets		+= sq_stats->packets;
+			s->tx_bytes		+= sq_stats->bytes;
 			s->tso_packets		+= sq_stats->tso_packets;
 			s->tso_bytes		+= sq_stats->tso_bytes;
 			s->tx_queue_stopped	+= sq_stats->stopped;
@@ -224,23 +233,6 @@ void mlx5e_update_stats(struct mlx5e_priv *priv)
 	s->tx_broadcast_bytes   =
 		MLX5_GET_CTR(out, transmitted_eth_broadcast.octets);
 
-	s->rx_packets =
-		s->rx_unicast_packets +
-		s->rx_multicast_packets +
-		s->rx_broadcast_packets;
-	s->rx_bytes =
-		s->rx_unicast_bytes +
-		s->rx_multicast_bytes +
-		s->rx_broadcast_bytes;
-	s->tx_packets =
-		s->tx_unicast_packets +
-		s->tx_multicast_packets +
-		s->tx_broadcast_packets;
-	s->tx_bytes =
-		s->tx_unicast_bytes +
-		s->tx_multicast_bytes +
-		s->tx_broadcast_bytes;
-
 	/* Update calculated offload counters */
 	s->tx_csum_offload = s->tx_packets - tx_offload_none;
 	s->rx_csum_good    = s->rx_packets - s->rx_csum_none -
@@ -350,6 +342,7 @@ static int mlx5e_create_rq(struct mlx5e_channel *c,
 
 	rq->pdev    = c->pdev;
 	rq->netdev  = c->netdev;
+	rq->tstamp  = &priv->tstamp;
 	rq->channel = c;
 	rq->ix      = c->ix;
 	rq->priv    = c->priv;
@@ -506,6 +499,7 @@ static void mlx5e_close_rq(struct mlx5e_rq *rq)
 
 static void mlx5e_free_sq_db(struct mlx5e_sq *sq)
 {
+	kfree(sq->wqe_info);
 	kfree(sq->dma_fifo);
 	kfree(sq->skb);
 }
@@ -518,8 +512,10 @@ static int mlx5e_alloc_sq_db(struct mlx5e_sq *sq, int numa)
 	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);
 
-	if (!sq->skb || !sq->dma_fifo) {
+	if (!sq->skb || !sq->dma_fifo || !sq->wqe_info) {
 		mlx5e_free_sq_db(sq);
 		return -ENOMEM;
 	}
@@ -567,6 +563,7 @@ static int mlx5e_create_sq(struct mlx5e_channel *c,
 	sq->txq = netdev_get_tx_queue(priv->netdev, txq_ix);
 
 	sq->pdev      = c->pdev;
+	sq->tstamp    = &priv->tstamp;
 	sq->mkey_be   = c->mkey_be;
 	sq->channel   = c;
 	sq->tc        = tc;
@@ -746,7 +743,7 @@ static int mlx5e_create_cq(struct mlx5e_channel *c,
 	struct mlx5_core_dev *mdev = priv->mdev;
 	struct mlx5_core_cq *mcq = &cq->mcq;
 	int eqn_not_used;
-	int irqn;
+	unsigned int irqn;
 	int err;
 	u32 i;
 
@@ -800,7 +797,7 @@ static int mlx5e_enable_cq(struct mlx5e_cq *cq, struct mlx5e_cq_param *param)
 	void *in;
 	void *cqc;
 	int inlen;
-	int irqn_not_used;
+	unsigned int irqn_not_used;
 	int eqn;
 	int err;
 
@@ -1020,6 +1017,7 @@ err_close_tx_cqs:
 
 err_napi_del:
 	netif_napi_del(&c->napi);
+	napi_hash_del(&c->napi);
 	kfree(c);
 
 	return err;
@@ -1033,6 +1031,10 @@ static void mlx5e_close_channel(struct mlx5e_channel *c)
 	mlx5e_close_cq(&c->rq.cq);
 	mlx5e_close_tx_cqs(c);
 	netif_napi_del(&c->napi);
+
+	napi_hash_del(&c->napi);
+	synchronize_rcu();
+
 	kfree(c);
 }
 
@@ -1188,7 +1190,6 @@ static void mlx5e_fill_indir_rqt_rqns(struct mlx5e_priv *priv, void *rqtc)
 			ix = mlx5e_bits_invert(i, MLX5E_LOG_INDIR_RQT_SIZE);
 
 		ix = priv->params.indirection_rqt[ix];
-		ix = ix % priv->params.num_channels;
 		MLX5_SET(rqtc, rqtc, rq_num[i],
 			 test_bit(MLX5E_STATE_OPENED, &priv->state) ?
 			 priv->channel[ix]->rq.rqn :
@@ -1306,7 +1307,22 @@ static void mlx5e_build_tir_ctx_lro(void *tirc, struct mlx5e_priv *priv)
 			      lro_timer_supported_periods[2]));
 }
 
-static int mlx5e_modify_tir_lro(struct mlx5e_priv *priv, int tt)
+void mlx5e_build_tir_ctx_hash(void *tirc, struct mlx5e_priv *priv)
+{
+	MLX5_SET(tirc, tirc, rx_hash_fn,
+		 mlx5e_rx_hash_fn(priv->params.rss_hfunc));
+	if (priv->params.rss_hfunc == ETH_RSS_HASH_TOP) {
+		void *rss_key = MLX5_ADDR_OF(tirc, tirc,
+					     rx_hash_toeplitz_key);
+		size_t len = MLX5_FLD_SZ_BYTES(tirc,
+					       rx_hash_toeplitz_key);
+
+		MLX5_SET(tirc, tirc, rx_hash_symmetric, 1);
+		memcpy(rss_key, priv->params.toeplitz_hash_key, len);
+	}
+}
+
+static int mlx5e_modify_tirs_lro(struct mlx5e_priv *priv)
 {
 	struct mlx5_core_dev *mdev = priv->mdev;
 
@@ -1314,6 +1330,7 @@ static int mlx5e_modify_tir_lro(struct mlx5e_priv *priv, int tt)
 	void *tirc;
 	int inlen;
 	int err;
+	int tt;
 
 	inlen = MLX5_ST_SZ_BYTES(modify_tir_in);
 	in = mlx5_vzalloc(inlen);
@@ -1325,7 +1342,11 @@ static int mlx5e_modify_tir_lro(struct mlx5e_priv *priv, int tt)
 
 	mlx5e_build_tir_ctx_lro(tirc, priv);
 
-	err = mlx5_core_modify_tir(mdev, priv->tirn[tt], in, inlen);
+	for (tt = 0; tt < MLX5E_NUM_TT; tt++) {
+		err = mlx5_core_modify_tir(mdev, priv->tirn[tt], in, inlen);
+		if (err)
+			break;
+	}
 
 	kvfree(in);
 
@@ -1421,6 +1442,7 @@ int mlx5e_open_locked(struct net_device *netdev)
 
 	mlx5e_update_carrier(priv);
 	mlx5e_redirect_rqts(priv);
+	mlx5e_timestamp_init(priv);
 
 	schedule_delayed_work(&priv->update_stats_work, 0);
 
@@ -1457,6 +1479,7 @@ int mlx5e_close_locked(struct net_device *netdev)
 
 	clear_bit(MLX5E_STATE_OPENED, &priv->state);
 
+	mlx5e_timestamp_cleanup(priv);
 	mlx5e_redirect_rqts(priv);
 	netif_carrier_off(priv->netdev);
 	mlx5e_close_channels(priv);
@@ -1504,7 +1527,7 @@ static int mlx5e_create_drop_cq(struct mlx5e_priv *priv,
 	struct mlx5_core_dev *mdev = priv->mdev;
 	struct mlx5_core_cq *mcq = &cq->mcq;
 	int eqn_not_used;
-	int irqn;
+	unsigned int irqn;
 	int err;
 
 	err = mlx5_cqwq_create(mdev, &param->wq, param->cqc, &cq->wq,
@@ -1659,17 +1682,7 @@ static void mlx5e_build_tir_ctx(struct mlx5e_priv *priv, u32 *tirc, int tt)
 	default:
 		MLX5_SET(tirc, tirc, indirect_table,
 			 priv->rqtn[MLX5E_INDIRECTION_RQT]);
-		MLX5_SET(tirc, tirc, rx_hash_fn,
-			 mlx5e_rx_hash_fn(priv->params.rss_hfunc));
-		if (priv->params.rss_hfunc == ETH_RSS_HASH_TOP) {
-			void *rss_key = MLX5_ADDR_OF(tirc, tirc,
-						     rx_hash_toeplitz_key);
-			size_t len = MLX5_FLD_SZ_BYTES(tirc,
-						       rx_hash_toeplitz_key);
-
-			MLX5_SET(tirc, tirc, rx_hash_symmetric, 1);
-			memcpy(rss_key, priv->params.toeplitz_hash_key, len);
-		}
+		mlx5e_build_tir_ctx_hash(tirc, priv);
 		break;
 	}
 
@@ -1872,8 +1885,10 @@ static int mlx5e_set_features(struct net_device *netdev,
 			mlx5e_close_locked(priv->netdev);
 
 		priv->params.lro_en = !!(features & NETIF_F_LRO);
-		mlx5e_modify_tir_lro(priv, MLX5E_TT_IPV4_TCP);
-		mlx5e_modify_tir_lro(priv, MLX5E_TT_IPV6_TCP);
+		err = mlx5e_modify_tirs_lro(priv);
+		if (err)
+			mlx5_core_warn(priv->mdev, "lro modify failed, %d\n",
+				       err);
 
 		if (was_opened)
 			err = mlx5e_open_locked(priv->netdev);
@@ -1926,17 +1941,122 @@ static int mlx5e_change_mtu(struct net_device *netdev, int new_mtu)
 	return err;
 }
 
-static struct net_device_ops mlx5e_netdev_ops = {
+static int mlx5e_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
+{
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return mlx5e_hwstamp_set(dev, ifr);
+	case SIOCGHWTSTAMP:
+		return mlx5e_hwstamp_get(dev, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int mlx5e_set_vf_mac(struct net_device *dev, int vf, u8 *mac)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	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)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	return mlx5_eswitch_set_vport_vlan(mdev->priv.eswitch, vf + 1,
+					   vlan, qos);
+}
+
+static int mlx5_vport_link2ifla(u8 esw_link)
+{
+	switch (esw_link) {
+	case MLX5_ESW_VPORT_ADMIN_STATE_DOWN:
+		return IFLA_VF_LINK_STATE_DISABLE;
+	case MLX5_ESW_VPORT_ADMIN_STATE_UP:
+		return IFLA_VF_LINK_STATE_ENABLE;
+	}
+	return IFLA_VF_LINK_STATE_AUTO;
+}
+
+static int mlx5_ifla_link2vport(u8 ifla_link)
+{
+	switch (ifla_link) {
+	case IFLA_VF_LINK_STATE_DISABLE:
+		return MLX5_ESW_VPORT_ADMIN_STATE_DOWN;
+	case IFLA_VF_LINK_STATE_ENABLE:
+		return MLX5_ESW_VPORT_ADMIN_STATE_UP;
+	}
+	return MLX5_ESW_VPORT_ADMIN_STATE_AUTO;
+}
+
+static int mlx5e_set_vf_link_state(struct net_device *dev, int vf,
+				   int link_state)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	return mlx5_eswitch_set_vport_state(mdev->priv.eswitch, vf + 1,
+					    mlx5_ifla_link2vport(link_state));
+}
+
+static int mlx5e_get_vf_config(struct net_device *dev,
+			       int vf, struct ifla_vf_info *ivi)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+	int err;
+
+	err = mlx5_eswitch_get_vport_config(mdev->priv.eswitch, vf + 1, ivi);
+	if (err)
+		return err;
+	ivi->linkstate = mlx5_vport_link2ifla(ivi->linkstate);
+	return 0;
+}
+
+static int mlx5e_get_vf_stats(struct net_device *dev,
+			      int vf, struct ifla_vf_stats *vf_stats)
+{
+	struct mlx5e_priv *priv = netdev_priv(dev);
+	struct mlx5_core_dev *mdev = priv->mdev;
+
+	return mlx5_eswitch_get_vport_stats(mdev->priv.eswitch, vf + 1,
+					    vf_stats);
+}
+
+static const struct net_device_ops mlx5e_netdev_ops_basic = {
 	.ndo_open                = mlx5e_open,
 	.ndo_stop                = mlx5e_close,
 	.ndo_start_xmit          = mlx5e_xmit,
 	.ndo_get_stats64         = mlx5e_get_stats,
 	.ndo_set_rx_mode         = mlx5e_set_rx_mode,
 	.ndo_set_mac_address     = mlx5e_set_mac,
-	.ndo_vlan_rx_add_vid	 = mlx5e_vlan_rx_add_vid,
-	.ndo_vlan_rx_kill_vid	 = mlx5e_vlan_rx_kill_vid,
+	.ndo_vlan_rx_add_vid     = mlx5e_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid    = mlx5e_vlan_rx_kill_vid,
 	.ndo_set_features        = mlx5e_set_features,
-	.ndo_change_mtu		 = mlx5e_change_mtu,
+	.ndo_change_mtu          = mlx5e_change_mtu,
+	.ndo_do_ioctl            = mlx5e_ioctl,
+};
+
+static const struct net_device_ops mlx5e_netdev_ops_sriov = {
+	.ndo_open                = mlx5e_open,
+	.ndo_stop                = mlx5e_close,
+	.ndo_start_xmit          = mlx5e_xmit,
+	.ndo_get_stats64         = mlx5e_get_stats,
+	.ndo_set_rx_mode         = mlx5e_set_rx_mode,
+	.ndo_set_mac_address     = mlx5e_set_mac,
+	.ndo_vlan_rx_add_vid     = mlx5e_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid    = mlx5e_vlan_rx_kill_vid,
+	.ndo_set_features        = mlx5e_set_features,
+	.ndo_change_mtu          = mlx5e_change_mtu,
+	.ndo_do_ioctl            = mlx5e_ioctl,
+	.ndo_set_vf_mac          = mlx5e_set_vf_mac,
+	.ndo_set_vf_vlan         = mlx5e_set_vf_vlan,
+	.ndo_get_vf_config       = mlx5e_get_vf_config,
+	.ndo_set_vf_link_state   = mlx5e_set_vf_link_state,
+	.ndo_get_vf_stats        = mlx5e_get_vf_stats,
 };
 
 static int mlx5e_check_required_hca_cap(struct mlx5_core_dev *mdev)
@@ -1971,12 +2091,20 @@ u16 mlx5e_get_max_inline_cap(struct mlx5_core_dev *mdev)
 	       2 /*sizeof(mlx5e_tx_wqe.inline_hdr_start)*/;
 }
 
+void mlx5e_build_default_indir_rqt(u32 *indirection_rqt, int len,
+				   int num_channels)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		indirection_rqt[i] = i % num_channels;
+}
+
 static void mlx5e_build_netdev_priv(struct mlx5_core_dev *mdev,
 				    struct net_device *netdev,
 				    int num_channels)
 {
 	struct mlx5e_priv *priv = netdev_priv(netdev);
-	int i;
 
 	priv->params.log_sq_size           =
 		MLX5E_PARAMS_DEFAULT_LOG_SQ_SIZE;
@@ -2000,8 +2128,8 @@ static void mlx5e_build_netdev_priv(struct mlx5_core_dev *mdev,
 	netdev_rss_key_fill(priv->params.toeplitz_hash_key,
 			    sizeof(priv->params.toeplitz_hash_key));
 
-	for (i = 0; i < MLX5E_INDIR_RQT_SIZE; i++)
-		priv->params.indirection_rqt[i] = i % num_channels;
+	mlx5e_build_default_indir_rqt(priv->params.indirection_rqt,
+				      MLX5E_INDIR_RQT_SIZE, num_channels);
 
 	priv->params.lro_wqe_sz            =
 		MLX5E_PARAMS_DEFAULT_LRO_WQE_SZ;
@@ -2023,7 +2151,12 @@ static void mlx5e_set_netdev_dev_addr(struct net_device *netdev)
 {
 	struct mlx5e_priv *priv = netdev_priv(netdev);
 
-	mlx5_query_nic_vport_mac_address(priv->mdev, netdev->dev_addr);
+	mlx5_query_nic_vport_mac_address(priv->mdev, 0, netdev->dev_addr);
+	if (is_zero_ether_addr(netdev->dev_addr) &&
+	    !MLX5_CAP_GEN(priv->mdev, vport_group_manager)) {
+		eth_hw_addr_random(netdev);
+		mlx5_core_info(priv->mdev, "Assigned random MAC address %pM\n", netdev->dev_addr);
+	}
 }
 
 static void mlx5e_build_netdev(struct net_device *netdev)
@@ -2033,10 +2166,11 @@ static void mlx5e_build_netdev(struct net_device *netdev)
 
 	SET_NETDEV_DEV(netdev, &mdev->pdev->dev);
 
-	if (priv->params.num_tc > 1)
-		mlx5e_netdev_ops.ndo_select_queue = mlx5e_select_queue;
+	if (MLX5_CAP_GEN(mdev, vport_group_manager))
+		netdev->netdev_ops = &mlx5e_netdev_ops_sriov;
+	else
+		netdev->netdev_ops = &mlx5e_netdev_ops_basic;
 
-	netdev->netdev_ops        = &mlx5e_netdev_ops;
 	netdev->watchdog_timeo    = 15 * HZ;
 
 	netdev->ethtool_ops	  = &mlx5e_ethtool_ops;
@@ -2129,7 +2263,7 @@ static void *mlx5e_create_netdev(struct mlx5_core_dev *mdev)
 		goto err_unmap_free_uar;
 	}
 
-	err = mlx5_alloc_transport_domain(mdev, &priv->tdn);
+	err = mlx5_core_alloc_transport_domain(mdev, &priv->tdn);
 	if (err) {
 		mlx5_core_err(mdev, "alloc td failed, %d\n", err);
 		goto err_dealloc_pd;
@@ -2212,7 +2346,7 @@ err_destroy_mkey:
 	mlx5_core_destroy_mkey(mdev, &priv->mr);
 
 err_dealloc_transport_domain:
-	mlx5_dealloc_transport_domain(mdev, priv->tdn);
+	mlx5_core_dealloc_transport_domain(mdev, priv->tdn);
 
 err_dealloc_pd:
 	mlx5_core_dealloc_pd(mdev, priv->pdn);
@@ -2244,7 +2378,7 @@ static void mlx5e_destroy_netdev(struct mlx5_core_dev *mdev, void *vpriv)
 	mlx5e_close_drop_rq(priv);
 	mlx5e_destroy_tises(priv);
 	mlx5_core_destroy_mkey(priv->mdev, &priv->mr);
-	mlx5_dealloc_transport_domain(priv->mdev, priv->tdn);
+	mlx5_core_dealloc_transport_domain(priv->mdev, priv->tdn);
 	mlx5_core_dealloc_pd(priv->mdev, priv->pdn);
 	mlx5_unmap_free_uar(priv->mdev, &priv->cq_uar);
 	free_netdev(netdev);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
index cf0098596e85..59658b9d05d1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_rx.c
@@ -33,8 +33,14 @@
 #include <linux/ip.h>
 #include <linux/ipv6.h>
 #include <linux/tcp.h>
+#include <net/busy_poll.h>
 #include "en.h"
 
+static inline bool mlx5e_rx_hw_stamp(struct mlx5e_tstamp *tstamp)
+{
+	return tstamp->hwtstamp_config.rx_filter == HWTSTAMP_FILTER_ALL;
+}
+
 static inline int mlx5e_alloc_rx_wqe(struct mlx5e_rq *rq,
 				     struct mlx5e_rx_wqe *wqe, u16 ix)
 {
@@ -189,6 +195,7 @@ static inline void mlx5e_build_rx_skb(struct mlx5_cqe64 *cqe,
 {
 	struct net_device *netdev = rq->netdev;
 	u32 cqe_bcnt = be32_to_cpu(cqe->byte_cnt);
+	struct mlx5e_tstamp *tstamp = rq->tstamp;
 	int lro_num_seg;
 
 	skb_put(skb, cqe_bcnt);
@@ -201,6 +208,9 @@ static inline void mlx5e_build_rx_skb(struct mlx5_cqe64 *cqe,
 		rq->stats.lro_bytes += cqe_bcnt;
 	}
 
+	if (unlikely(mlx5e_rx_hw_stamp(tstamp)))
+		mlx5e_fill_hwstamp(tstamp, get_cqe_ts(cqe), skb_hwtstamps(skb));
+
 	mlx5e_handle_csum(netdev, cqe, rq, skb);
 
 	skb->protocol = eth_type_trans(skb, netdev);
@@ -215,16 +225,12 @@ static inline void mlx5e_build_rx_skb(struct mlx5_cqe64 *cqe,
 				       be16_to_cpu(cqe->vlan_info));
 }
 
-bool mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
+int mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
 {
 	struct mlx5e_rq *rq = container_of(cq, struct mlx5e_rq, cq);
-	int i;
+	int work_done;
 
-	/* avoid accessing cq (dma coherent memory) if not needed */
-	if (!test_and_clear_bit(MLX5E_CQ_HAS_CQES, &cq->flags))
-		return false;
-
-	for (i = 0; i < budget; i++) {
+	for (work_done = 0; work_done < budget; work_done++) {
 		struct mlx5e_rx_wqe *wqe;
 		struct mlx5_cqe64 *cqe;
 		struct sk_buff *skb;
@@ -257,6 +263,7 @@ bool mlx5e_poll_rx_cq(struct mlx5e_cq *cq, int budget)
 
 		mlx5e_build_rx_skb(cqe, rq, skb);
 		rq->stats.packets++;
+		rq->stats.bytes += be32_to_cpu(cqe->byte_cnt);
 		napi_gro_receive(cq->napi, skb);
 
 wq_ll_pop:
@@ -269,10 +276,5 @@ wq_ll_pop:
 	/* ensure cq space is freed before enabling more cqes */
 	wmb();
 
-	if (i == budget) {
-		set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
-		return true;
-	}
-
-	return false;
+	return work_done;
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
index 1341b1d3c421..bb4eeeb007de 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_tx.c
@@ -92,11 +92,11 @@ static inline struct mlx5e_sq_dma *mlx5e_dma_get(struct mlx5e_sq *sq, u32 i)
 	return &sq->dma_fifo[i & sq->dma_fifo_mask];
 }
 
-static void mlx5e_dma_unmap_wqe_err(struct mlx5e_sq *sq, struct sk_buff *skb)
+static void mlx5e_dma_unmap_wqe_err(struct mlx5e_sq *sq, u8 num_dma)
 {
 	int i;
 
-	for (i = 0; i < MLX5E_TX_SKB_CB(skb)->num_dma; i++) {
+	for (i = 0; i < num_dma; i++) {
 		struct mlx5e_sq_dma *last_pushed_dma =
 			mlx5e_dma_get(sq, --sq->dma_fifo_pc);
 
@@ -139,19 +139,28 @@ static inline u16 mlx5e_get_inline_hdr_size(struct mlx5e_sq *sq,
 	return MLX5E_MIN_INLINE;
 }
 
-static inline void mlx5e_insert_vlan(void *start, struct sk_buff *skb, u16 ihs)
+static inline void mlx5e_tx_skb_pull_inline(unsigned char **skb_data,
+					    unsigned int *skb_len,
+					    unsigned int len)
+{
+	*skb_len -= len;
+	*skb_data += len;
+}
+
+static inline void mlx5e_insert_vlan(void *start, struct sk_buff *skb, u16 ihs,
+				     unsigned char **skb_data,
+				     unsigned int *skb_len)
 {
 	struct vlan_ethhdr *vhdr = (struct vlan_ethhdr *)start;
 	int cpy1_sz = 2 * ETH_ALEN;
 	int cpy2_sz = ihs - cpy1_sz;
 
-	skb_copy_from_linear_data(skb, vhdr, cpy1_sz);
-	skb_pull_inline(skb, cpy1_sz);
+	memcpy(vhdr, *skb_data, cpy1_sz);
+	mlx5e_tx_skb_pull_inline(skb_data, skb_len, cpy1_sz);
 	vhdr->h_vlan_proto = skb->vlan_proto;
 	vhdr->h_vlan_TCI = cpu_to_be16(skb_vlan_tag_get(skb));
-	skb_copy_from_linear_data(skb, &vhdr->h_vlan_encapsulated_proto,
-				  cpy2_sz);
-	skb_pull_inline(skb, cpy2_sz);
+	memcpy(&vhdr->h_vlan_encapsulated_proto, *skb_data, cpy2_sz);
+	mlx5e_tx_skb_pull_inline(skb_data, skb_len, cpy2_sz);
 }
 
 static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
@@ -160,13 +169,17 @@ 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 mlx5_wqe_ctrl_seg *cseg = &wqe->ctrl;
 	struct mlx5_wqe_eth_seg  *eseg = &wqe->eth;
 	struct mlx5_wqe_data_seg *dseg;
 
+	unsigned char *skb_data = skb->data;
+	unsigned int skb_len = skb->len;
 	u8  opcode = MLX5_OPCODE_SEND;
 	dma_addr_t dma_addr = 0;
+	unsigned int num_bytes;
 	bool bf = false;
 	u16 headlen;
 	u16 ds_cnt;
@@ -192,8 +205,7 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 		opcode       = MLX5_OPCODE_LSO;
 		ihs          = skb_transport_offset(skb) + tcp_hdrlen(skb);
 		payload_len  = skb->len - ihs;
-		MLX5E_TX_SKB_CB(skb)->num_bytes = skb->len +
-					(skb_shinfo(skb)->gso_segs - 1) * ihs;
+		num_bytes = skb->len + (skb_shinfo(skb)->gso_segs - 1) * ihs;
 		sq->stats.tso_packets++;
 		sq->stats.tso_bytes += payload_len;
 	} else {
@@ -201,16 +213,18 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 		     !skb->xmit_more &&
 		     !skb_shinfo(skb)->nr_frags;
 		ihs = mlx5e_get_inline_hdr_size(sq, skb, bf);
-		MLX5E_TX_SKB_CB(skb)->num_bytes = max_t(unsigned int, skb->len,
-							ETH_ZLEN);
+		num_bytes = max_t(unsigned int, skb->len, ETH_ZLEN);
 	}
 
+	wi->num_bytes = num_bytes;
+
 	if (skb_vlan_tag_present(skb)) {
-		mlx5e_insert_vlan(eseg->inline_hdr_start, skb, ihs);
+		mlx5e_insert_vlan(eseg->inline_hdr_start, skb, ihs, &skb_data,
+				  &skb_len);
 		ihs += VLAN_HLEN;
 	} else {
-		skb_copy_from_linear_data(skb, eseg->inline_hdr_start, ihs);
-		skb_pull_inline(skb, ihs);
+		memcpy(eseg->inline_hdr_start, skb_data, ihs);
+		mlx5e_tx_skb_pull_inline(&skb_data, &skb_len, ihs);
 	}
 
 	eseg->inline_hdr_sz = cpu_to_be16(ihs);
@@ -220,11 +234,11 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 			       MLX5_SEND_WQE_DS);
 	dseg    = (struct mlx5_wqe_data_seg *)cseg + ds_cnt;
 
-	MLX5E_TX_SKB_CB(skb)->num_dma = 0;
+	wi->num_dma = 0;
 
-	headlen = skb_headlen(skb);
+	headlen = skb_len - skb->data_len;
 	if (headlen) {
-		dma_addr = dma_map_single(sq->pdev, skb->data, headlen,
+		dma_addr = dma_map_single(sq->pdev, skb_data, headlen,
 					  DMA_TO_DEVICE);
 		if (unlikely(dma_mapping_error(sq->pdev, dma_addr)))
 			goto dma_unmap_wqe_err;
@@ -234,7 +248,7 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 		dseg->byte_count = cpu_to_be32(headlen);
 
 		mlx5e_dma_push(sq, dma_addr, headlen, MLX5E_DMA_MAP_SINGLE);
-		MLX5E_TX_SKB_CB(skb)->num_dma++;
+		wi->num_dma++;
 
 		dseg++;
 	}
@@ -253,23 +267,25 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 		dseg->byte_count = cpu_to_be32(fsz);
 
 		mlx5e_dma_push(sq, dma_addr, fsz, MLX5E_DMA_MAP_PAGE);
-		MLX5E_TX_SKB_CB(skb)->num_dma++;
+		wi->num_dma++;
 
 		dseg++;
 	}
 
-	ds_cnt += MLX5E_TX_SKB_CB(skb)->num_dma;
+	ds_cnt += wi->num_dma;
 
 	cseg->opmod_idx_opcode = cpu_to_be32((sq->pc << 8) | opcode);
 	cseg->qpn_ds           = cpu_to_be32((sq->sqn << 8) | ds_cnt);
 
 	sq->skb[pi] = skb;
 
-	MLX5E_TX_SKB_CB(skb)->num_wqebbs = DIV_ROUND_UP(ds_cnt,
-							MLX5_SEND_WQEBB_NUM_DS);
-	sq->pc += MLX5E_TX_SKB_CB(skb)->num_wqebbs;
+	wi->num_wqebbs = DIV_ROUND_UP(ds_cnt, MLX5_SEND_WQEBB_NUM_DS);
+	sq->pc += wi->num_wqebbs;
+
+	netdev_tx_sent_queue(sq->txq, wi->num_bytes);
 
-	netdev_tx_sent_queue(sq->txq, MLX5E_TX_SKB_CB(skb)->num_bytes);
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP))
+		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 
 	if (unlikely(!mlx5e_sq_has_room_for(sq, MLX5E_SQ_STOP_ROOM))) {
 		netif_tx_stop_queue(sq->txq);
@@ -280,7 +296,7 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 		int bf_sz = 0;
 
 		if (bf && sq->uar_bf_map)
-			bf_sz = MLX5E_TX_SKB_CB(skb)->num_wqebbs << 3;
+			bf_sz = wi->num_wqebbs << 3;
 
 		cseg->fm_ce_se = MLX5_WQE_CTRL_CQ_UPDATE;
 		mlx5e_tx_notify_hw(sq, wqe, bf_sz);
@@ -293,11 +309,12 @@ static netdev_tx_t mlx5e_sq_xmit(struct mlx5e_sq *sq, struct sk_buff *skb)
 	sq->bf_budget = bf ? sq->bf_budget - 1 : 0;
 
 	sq->stats.packets++;
+	sq->stats.bytes += num_bytes;
 	return NETDEV_TX_OK;
 
 dma_unmap_wqe_err:
 	sq->stats.dropped++;
-	mlx5e_dma_unmap_wqe_err(sq, skb);
+	mlx5e_dma_unmap_wqe_err(sq, wi->num_dma);
 
 	dev_kfree_skb_any(skb);
 
@@ -321,10 +338,6 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 	u16 sqcc;
 	int i;
 
-	/* avoid accessing cq (dma coherent memory) if not needed */
-	if (!test_and_clear_bit(MLX5E_CQ_HAS_CQES, &cq->flags))
-		return false;
-
 	sq = container_of(cq, struct mlx5e_sq, cq);
 
 	npkts = 0;
@@ -352,6 +365,7 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 		wqe_counter = be16_to_cpu(cqe->wqe_counter);
 
 		do {
+			struct mlx5e_tx_wqe_info *wi;
 			struct sk_buff *skb;
 			u16 ci;
 			int j;
@@ -360,6 +374,7 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 
 			ci = sqcc & sq->wq.sz_m1;
 			skb = sq->skb[ci];
+			wi = &sq->wqe_info[ci];
 
 			if (unlikely(!skb)) { /* nop */
 				sq->stats.nop++;
@@ -367,7 +382,16 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 				continue;
 			}
 
-			for (j = 0; j < MLX5E_TX_SKB_CB(skb)->num_dma; j++) {
+			if (unlikely(skb_shinfo(skb)->tx_flags &
+				     SKBTX_HW_TSTAMP)) {
+				struct skb_shared_hwtstamps hwts = {};
+
+				mlx5e_fill_hwstamp(sq->tstamp,
+						   get_cqe_ts(cqe), &hwts);
+				skb_tstamp_tx(skb, &hwts);
+			}
+
+			for (j = 0; j < wi->num_dma; j++) {
 				struct mlx5e_sq_dma *dma =
 					mlx5e_dma_get(sq, dma_fifo_cc++);
 
@@ -375,8 +399,8 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 			}
 
 			npkts++;
-			nbytes += MLX5E_TX_SKB_CB(skb)->num_bytes;
-			sqcc += MLX5E_TX_SKB_CB(skb)->num_wqebbs;
+			nbytes += wi->num_bytes;
+			sqcc += wi->num_wqebbs;
 			dev_kfree_skb(skb);
 		} while (!last_wqe);
 	}
@@ -397,10 +421,6 @@ bool mlx5e_poll_tx_cq(struct mlx5e_cq *cq)
 				netif_tx_wake_queue(sq->txq);
 				sq->stats.wake++;
 	}
-	if (i == MLX5E_TX_CQ_POLL_BUDGET) {
-		set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
-		return true;
-	}
 
-	return false;
+	return (i == MLX5E_TX_CQ_POLL_BUDGET);
 }
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
index 2c7cb6755d1d..66d51a77609e 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/en_txrx.c
@@ -54,6 +54,7 @@ int mlx5e_napi_poll(struct napi_struct *napi, int budget)
 	struct mlx5e_channel *c = container_of(napi, struct mlx5e_channel,
 					       napi);
 	bool busy = false;
+	int work_done;
 	int i;
 
 	clear_bit(MLX5E_CHANNEL_NAPI_SCHED, &c->flags);
@@ -61,33 +62,32 @@ int mlx5e_napi_poll(struct napi_struct *napi, int budget)
 	for (i = 0; i < c->num_tc; i++)
 		busy |= mlx5e_poll_tx_cq(&c->sq[i].cq);
 
-	busy |= mlx5e_poll_rx_cq(&c->rq.cq, budget);
-
+	work_done = mlx5e_poll_rx_cq(&c->rq.cq, budget);
+	busy |= work_done == budget;
 	busy |= mlx5e_post_rx_wqes(&c->rq);
 
 	if (busy)
 		return budget;
 
-	napi_complete(napi);
+	napi_complete_done(napi, work_done);
 
 	/* avoid losing completion event during/after polling cqs */
 	if (test_bit(MLX5E_CHANNEL_NAPI_SCHED, &c->flags)) {
 		napi_schedule(napi);
-		return 0;
+		return work_done;
 	}
 
 	for (i = 0; i < c->num_tc; i++)
 		mlx5e_cq_arm(&c->sq[i].cq);
 	mlx5e_cq_arm(&c->rq.cq);
 
-	return 0;
+	return work_done;
 }
 
 void mlx5e_completion_event(struct mlx5_core_cq *mcq)
 {
 	struct mlx5e_cq *cq = container_of(mcq, struct mlx5e_cq, mcq);
 
-	set_bit(MLX5E_CQ_HAS_CQES, &cq->flags);
 	set_bit(MLX5E_CHANNEL_NAPI_SCHED, &cq->channel->flags);
 	barrier();
 	napi_schedule(cq->napi);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index 713ead583347..647a3ca2c2a9 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -35,6 +35,9 @@
 #include <linux/mlx5/driver.h>
 #include <linux/mlx5/cmd.h>
 #include "mlx5_core.h"
+#ifdef CONFIG_MLX5_CORE_EN
+#include "eswitch.h"
+#endif
 
 enum {
 	MLX5_EQE_SIZE		= sizeof(struct mlx5_eqe),
@@ -227,6 +230,7 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 		case MLX5_EVENT_TYPE_WQ_INVAL_REQ_ERROR:
 		case MLX5_EVENT_TYPE_WQ_ACCESS_ERROR:
 			rsn = be32_to_cpu(eqe->data.qp_srq.qp_srq_n) & 0xffffff;
+			rsn |= (eqe->data.qp_srq.type << MLX5_USER_INDEX_LEN);
 			mlx5_core_dbg(dev, "event %s(%d) arrived on resource 0x%x\n",
 				      eqe_type_str(eqe->type), eqe->type, rsn);
 			mlx5_rsc_event(dev, rsn, eqe->type);
@@ -287,6 +291,11 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 			break;
 #endif
 
+#ifdef CONFIG_MLX5_CORE_EN
+		case MLX5_EVENT_TYPE_NIC_VPORT_CHANGE:
+			mlx5_eswitch_vport_event(dev->priv.eswitch, eqe);
+			break;
+#endif
 		default:
 			mlx5_core_warn(dev, "Unhandled event 0x%x on EQ 0x%x\n",
 				       eqe->type, eq->eqn);
@@ -459,6 +468,11 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 	if (MLX5_CAP_GEN(dev, pg))
 		async_event_mask |= (1ull << MLX5_EVENT_TYPE_PAGE_FAULT);
 
+	if (MLX5_CAP_GEN(dev, port_type) == MLX5_CAP_PORT_TYPE_ETH &&
+	    MLX5_CAP_GEN(dev, vport_group_manager) &&
+	    mlx5_core_is_pf(dev))
+		async_event_mask |= (1ull << MLX5_EVENT_TYPE_NIC_VPORT_CHANGE);
+
 	err = mlx5_create_map_eq(dev, &table->cmd_eq, MLX5_EQ_VEC_CMD,
 				 MLX5_NUM_CMD_EQE, 1ull << MLX5_EVENT_TYPE_CMD,
 				 "mlx5_cmd_eq", &dev->priv.uuari.uars[0]);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
new file mode 100644
index 000000000000..bc3d9f8a75c1
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.c
@@ -0,0 +1,1097 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/mlx5_ifc.h>
+#include <linux/mlx5/vport.h>
+#include <linux/mlx5/fs.h>
+#include "mlx5_core.h"
+#include "eswitch.h"
+
+#define UPLINK_VPORT 0xFFFF
+
+#define MLX5_DEBUG_ESWITCH_MASK BIT(3)
+
+#define esw_info(dev, format, ...)				\
+	pr_info("(%s): E-Switch: " format, (dev)->priv.name, ##__VA_ARGS__)
+
+#define esw_warn(dev, format, ...)				\
+	pr_warn("(%s): E-Switch: " format, (dev)->priv.name, ##__VA_ARGS__)
+
+#define esw_debug(dev, format, ...)				\
+	mlx5_core_dbg_mask(dev, MLX5_DEBUG_ESWITCH_MASK, format, ##__VA_ARGS__)
+
+enum {
+	MLX5_ACTION_NONE = 0,
+	MLX5_ACTION_ADD  = 1,
+	MLX5_ACTION_DEL  = 2,
+};
+
+/* E-Switch UC L2 table hash node */
+struct esw_uc_addr {
+	struct l2addr_node node;
+	u32                table_index;
+	u32                vport;
+};
+
+/* E-Switch MC FDB table hash node */
+struct esw_mc_addr { /* SRIOV only */
+	struct l2addr_node     node;
+	struct mlx5_flow_rule *uplink_rule; /* Forward to uplink rule */
+	u32                    refcnt;
+};
+
+/* Vport UC/MC hash node */
+struct vport_addr {
+	struct l2addr_node     node;
+	u8                     action;
+	u32                    vport;
+	struct mlx5_flow_rule *flow_rule; /* SRIOV only */
+};
+
+enum {
+	UC_ADDR_CHANGE = BIT(0),
+	MC_ADDR_CHANGE = BIT(1),
+};
+
+/* Vport context events */
+#define SRIOV_VPORT_EVENTS (UC_ADDR_CHANGE | \
+			    MC_ADDR_CHANGE)
+
+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)];
+	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);
+	MLX5_SET(modify_nic_vport_context_in, in, field_select.change_event, 1);
+	MLX5_SET(modify_nic_vport_context_in, in, vport_number, vport);
+	if (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, arm_change_event, 1);
+
+	if (events_mask & UC_ADDR_CHANGE)
+		MLX5_SET(nic_vport_context, nic_vport_ctx,
+			 event_on_uc_address_change, 1);
+	if (events_mask & MC_ADDR_CHANGE)
+		MLX5_SET(nic_vport_context, nic_vport_ctx,
+			 event_on_mc_address_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;
+}
+
+/* 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));
+
+	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));
+}
+
+static int modify_esw_vport_cvlan(struct mlx5_core_dev *dev, u32 vport,
+				  u16 vlan, u8 qos, bool set)
+{
+	u32 in[MLX5_ST_SZ_DW(modify_esw_vport_context_in)];
+
+	memset(in, 0, sizeof(in));
+
+	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);
+
+	if (set) {
+		MLX5_SET(modify_esw_vport_context_in, in,
+			 esw_vport_context.vport_cvlan_strip, 1);
+		/* 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,
+			 esw_vport_context.cvlan_id, vlan);
+	}
+
+	MLX5_SET(modify_esw_vport_context_in, in,
+		 field_select.vport_cvlan_strip, 1);
+	MLX5_SET(modify_esw_vport_context_in, in,
+		 field_select.vport_cvlan_insert, 1);
+
+	return modify_esw_vport_context_cmd(dev, vport, in, sizeof(in));
+}
+
+/* HW L2 Table (MPFS) management */
+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)];
+	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);
+	MLX5_SET(set_l2_table_entry_in, in, vlan_valid, vlan_valid);
+	MLX5_SET(set_l2_table_entry_in, in, vlan, vlan);
+
+	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));
+}
+
+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));
+
+	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));
+}
+
+static int alloc_l2_table_index(struct mlx5_l2_table *l2_table, u32 *ix)
+{
+	int err = 0;
+
+	*ix = find_first_zero_bit(l2_table->bitmap, l2_table->size);
+	if (*ix >= l2_table->size)
+		err = -ENOSPC;
+	else
+		__set_bit(*ix, l2_table->bitmap);
+
+	return err;
+}
+
+static void free_l2_table_index(struct mlx5_l2_table *l2_table, u32 ix)
+{
+	__clear_bit(ix, l2_table->bitmap);
+}
+
+static int set_l2_table_entry(struct mlx5_core_dev *dev, u8 *mac,
+			      u8 vlan_valid, u16 vlan,
+			      u32 *index)
+{
+	struct mlx5_l2_table *l2_table = &dev->priv.eswitch->l2_table;
+	int err;
+
+	err = alloc_l2_table_index(l2_table, index);
+	if (err)
+		return err;
+
+	err = set_l2_table_entry_cmd(dev, *index, mac, vlan_valid, vlan);
+	if (err)
+		free_l2_table_index(l2_table, *index);
+
+	return err;
+}
+
+static void del_l2_table_entry(struct mlx5_core_dev *dev, u32 index)
+{
+	struct mlx5_l2_table *l2_table = &dev->priv.eswitch->l2_table;
+
+	del_l2_table_entry_cmd(dev, index);
+	free_l2_table_index(l2_table, index);
+}
+
+/* E-Switch FDB */
+static struct mlx5_flow_rule *
+esw_fdb_set_vport_rule(struct mlx5_eswitch *esw, u8 mac[ETH_ALEN], u32 vport)
+{
+	int match_header = MLX5_MATCH_OUTER_HEADERS;
+	struct mlx5_flow_destination dest;
+	struct mlx5_flow_rule *flow_rule = NULL;
+	u32 *match_v;
+	u32 *match_c;
+	u8 *dmac_v;
+	u8 *dmac_c;
+
+	match_v = kzalloc(MLX5_ST_SZ_BYTES(fte_match_param), GFP_KERNEL);
+	match_c = kzalloc(MLX5_ST_SZ_BYTES(fte_match_param), GFP_KERNEL);
+	if (!match_v || !match_c) {
+		pr_warn("FDB: Failed to alloc match parameters\n");
+		goto out;
+	}
+	dmac_v = MLX5_ADDR_OF(fte_match_param, match_v,
+			      outer_headers.dmac_47_16);
+	dmac_c = MLX5_ADDR_OF(fte_match_param, match_c,
+			      outer_headers.dmac_47_16);
+
+	ether_addr_copy(dmac_v, mac);
+	/* Match criteria mask */
+	memset(dmac_c, 0xff, 6);
+
+	dest.type = MLX5_FLOW_DESTINATION_TYPE_VPORT;
+	dest.vport_num = vport;
+
+	esw_debug(esw->dev,
+		  "\tFDB add rule dmac_v(%pM) dmac_c(%pM) -> vport(%d)\n",
+		  dmac_v, dmac_c, vport);
+	flow_rule =
+		mlx5_add_flow_rule(esw->fdb_table.fdb,
+				   match_header,
+				   match_c,
+				   match_v,
+				   MLX5_FLOW_CONTEXT_ACTION_FWD_DEST,
+				   0, &dest);
+	if (IS_ERR_OR_NULL(flow_rule)) {
+		pr_warn(
+			"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;
+	}
+out:
+	kfree(match_v);
+	kfree(match_c);
+	return flow_rule;
+}
+
+static int esw_create_fdb_table(struct mlx5_eswitch *esw, int nvports)
+{
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	struct mlx5_core_dev *dev = esw->dev;
+	struct mlx5_flow_namespace *root_ns;
+	struct mlx5_flow_table *fdb;
+	struct mlx5_flow_group *g;
+	void *match_criteria;
+	int table_size;
+	u32 *flow_group_in;
+	u8 *dmac;
+	int err = 0;
+
+	esw_debug(dev, "Create FDB log_max_size(%d)\n",
+		  MLX5_CAP_ESW_FLOWTABLE_FDB(dev, log_max_ft_size));
+
+	root_ns = mlx5_get_flow_namespace(dev, MLX5_FLOW_NAMESPACE_FDB);
+	if (!root_ns) {
+		esw_warn(dev, "Failed to get FDB flow namespace\n");
+		return -ENOMEM;
+	}
+
+	flow_group_in = mlx5_vzalloc(inlen);
+	if (!flow_group_in)
+		return -ENOMEM;
+	memset(flow_group_in, 0, inlen);
+
+	table_size = BIT(MLX5_CAP_ESW_FLOWTABLE_FDB(dev, log_max_ft_size));
+	fdb = mlx5_create_flow_table(root_ns, 0, table_size);
+	if (IS_ERR_OR_NULL(fdb)) {
+		err = PTR_ERR(fdb);
+		esw_warn(dev, "Failed to create FDB Table err %d\n", err);
+		goto out;
+	}
+
+	MLX5_SET(create_flow_group_in, flow_group_in, match_criteria_enable,
+		 MLX5_MATCH_OUTER_HEADERS);
+	match_criteria = MLX5_ADDR_OF(create_flow_group_in, flow_group_in, match_criteria);
+	dmac = MLX5_ADDR_OF(fte_match_param, match_criteria, outer_headers.dmac_47_16);
+	MLX5_SET(create_flow_group_in, flow_group_in, start_flow_index, 0);
+	MLX5_SET(create_flow_group_in, flow_group_in, end_flow_index, table_size - 1);
+	eth_broadcast_addr(dmac);
+
+	g = mlx5_create_flow_group(fdb, flow_group_in);
+	if (IS_ERR_OR_NULL(g)) {
+		err = PTR_ERR(g);
+		esw_warn(dev, "Failed to create flow group err(%d)\n", err);
+		goto out;
+	}
+
+	esw->fdb_table.addr_grp = g;
+	esw->fdb_table.fdb = fdb;
+out:
+	kfree(flow_group_in);
+	if (err && !IS_ERR_OR_NULL(fdb))
+		mlx5_destroy_flow_table(fdb);
+	return err;
+}
+
+static void esw_destroy_fdb_table(struct mlx5_eswitch *esw)
+{
+	if (!esw->fdb_table.fdb)
+		return;
+
+	esw_debug(esw->dev, "Destroy FDB Table\n");
+	mlx5_destroy_flow_group(esw->fdb_table.addr_grp);
+	mlx5_destroy_flow_table(esw->fdb_table.fdb);
+	esw->fdb_table.fdb = NULL;
+	esw->fdb_table.addr_grp = NULL;
+}
+
+/* E-Switch vport UC/MC lists management */
+typedef int (*vport_addr_action)(struct mlx5_eswitch *esw,
+				 struct vport_addr *vaddr);
+
+static int esw_add_uc_addr(struct mlx5_eswitch *esw, struct vport_addr *vaddr)
+{
+	struct hlist_head *hash = esw->l2_table.l2_hash;
+	struct esw_uc_addr *esw_uc;
+	u8 *mac = vaddr->node.addr;
+	u32 vport = vaddr->vport;
+	int err;
+
+	esw_uc = l2addr_hash_find(hash, mac, struct esw_uc_addr);
+	if (esw_uc) {
+		esw_warn(esw->dev,
+			 "Failed to set L2 mac(%pM) for vport(%d), mac is already in use by vport(%d)\n",
+			 mac, vport, esw_uc->vport);
+		return -EEXIST;
+	}
+
+	esw_uc = l2addr_hash_add(hash, mac, struct esw_uc_addr, GFP_KERNEL);
+	if (!esw_uc)
+		return -ENOMEM;
+	esw_uc->vport = vport;
+
+	err = set_l2_table_entry(esw->dev, mac, 0, 0, &esw_uc->table_index);
+	if (err)
+		goto abort;
+
+	if (esw->fdb_table.fdb) /* SRIOV is enabled: Forward UC MAC to vport */
+		vaddr->flow_rule = esw_fdb_set_vport_rule(esw, mac, vport);
+
+	esw_debug(esw->dev, "\tADDED UC MAC: vport[%d] %pM index:%d fr(%p)\n",
+		  vport, mac, esw_uc->table_index, vaddr->flow_rule);
+	return err;
+abort:
+	l2addr_hash_del(esw_uc);
+	return err;
+}
+
+static int esw_del_uc_addr(struct mlx5_eswitch *esw, struct vport_addr *vaddr)
+{
+	struct hlist_head *hash = esw->l2_table.l2_hash;
+	struct esw_uc_addr *esw_uc;
+	u8 *mac = vaddr->node.addr;
+	u32 vport = vaddr->vport;
+
+	esw_uc = l2addr_hash_find(hash, mac, struct esw_uc_addr);
+	if (!esw_uc || esw_uc->vport != vport) {
+		esw_debug(esw->dev,
+			  "MAC(%pM) doesn't belong to vport (%d)\n",
+			  mac, vport);
+		return -EINVAL;
+	}
+	esw_debug(esw->dev, "\tDELETE UC MAC: vport[%d] %pM index:%d fr(%p)\n",
+		  vport, mac, esw_uc->table_index, vaddr->flow_rule);
+
+	del_l2_table_entry(esw->dev, esw_uc->table_index);
+
+	if (vaddr->flow_rule)
+		mlx5_del_flow_rule(vaddr->flow_rule);
+	vaddr->flow_rule = NULL;
+
+	l2addr_hash_del(esw_uc);
+	return 0;
+}
+
+static int esw_add_mc_addr(struct mlx5_eswitch *esw, struct vport_addr *vaddr)
+{
+	struct hlist_head *hash = esw->mc_table;
+	struct esw_mc_addr *esw_mc;
+	u8 *mac = vaddr->node.addr;
+	u32 vport = vaddr->vport;
+
+	if (!esw->fdb_table.fdb)
+		return 0;
+
+	esw_mc = l2addr_hash_find(hash, mac, struct esw_mc_addr);
+	if (esw_mc)
+		goto add;
+
+	esw_mc = l2addr_hash_add(hash, mac, struct esw_mc_addr, GFP_KERNEL);
+	if (!esw_mc)
+		return -ENOMEM;
+
+	esw_mc->uplink_rule = /* Forward MC MAC to Uplink */
+		esw_fdb_set_vport_rule(esw, mac, UPLINK_VPORT);
+add:
+	esw_mc->refcnt++;
+	/* Forward MC MAC to vport */
+	vaddr->flow_rule = esw_fdb_set_vport_rule(esw, mac, vport);
+	esw_debug(esw->dev,
+		  "\tADDED MC MAC: vport[%d] %pM fr(%p) refcnt(%d) uplinkfr(%p)\n",
+		  vport, mac, vaddr->flow_rule,
+		  esw_mc->refcnt, esw_mc->uplink_rule);
+	return 0;
+}
+
+static int esw_del_mc_addr(struct mlx5_eswitch *esw, struct vport_addr *vaddr)
+{
+	struct hlist_head *hash = esw->mc_table;
+	struct esw_mc_addr *esw_mc;
+	u8 *mac = vaddr->node.addr;
+	u32 vport = vaddr->vport;
+
+	if (!esw->fdb_table.fdb)
+		return 0;
+
+	esw_mc = l2addr_hash_find(hash, mac, struct esw_mc_addr);
+	if (!esw_mc) {
+		esw_warn(esw->dev,
+			 "Failed to find eswitch MC addr for MAC(%pM) vport(%d)",
+			 mac, vport);
+		return -EINVAL;
+	}
+	esw_debug(esw->dev,
+		  "\tDELETE MC MAC: vport[%d] %pM fr(%p) refcnt(%d) uplinkfr(%p)\n",
+		  vport, mac, vaddr->flow_rule, esw_mc->refcnt,
+		  esw_mc->uplink_rule);
+
+	if (vaddr->flow_rule)
+		mlx5_del_flow_rule(vaddr->flow_rule);
+	vaddr->flow_rule = NULL;
+
+	if (--esw_mc->refcnt)
+		return 0;
+
+	if (esw_mc->uplink_rule)
+		mlx5_del_flow_rule(esw_mc->uplink_rule);
+
+	l2addr_hash_del(esw_mc);
+	return 0;
+}
+
+/* Apply vport UC/MC list to HW l2 table and FDB table */
+static void esw_apply_vport_addr_list(struct mlx5_eswitch *esw,
+				      u32 vport_num, int list_type)
+{
+	struct mlx5_vport *vport = &esw->vports[vport_num];
+	bool is_uc = list_type == MLX5_NVPRT_LIST_TYPE_UC;
+	vport_addr_action vport_addr_add;
+	vport_addr_action vport_addr_del;
+	struct vport_addr *addr;
+	struct l2addr_node *node;
+	struct hlist_head *hash;
+	struct hlist_node *tmp;
+	int hi;
+
+	vport_addr_add = is_uc ? esw_add_uc_addr :
+				 esw_add_mc_addr;
+	vport_addr_del = is_uc ? esw_del_uc_addr :
+				 esw_del_mc_addr;
+
+	hash = is_uc ? vport->uc_list : vport->mc_list;
+	for_each_l2hash_node(node, tmp, hash, hi) {
+		addr = container_of(node, struct vport_addr, node);
+		switch (addr->action) {
+		case MLX5_ACTION_ADD:
+			vport_addr_add(esw, addr);
+			addr->action = MLX5_ACTION_NONE;
+			break;
+		case MLX5_ACTION_DEL:
+			vport_addr_del(esw, addr);
+			l2addr_hash_del(addr);
+			break;
+		}
+	}
+}
+
+/* Sync vport UC/MC list from vport context */
+static void esw_update_vport_addr_list(struct mlx5_eswitch *esw,
+				       u32 vport_num, int list_type)
+{
+	struct mlx5_vport *vport = &esw->vports[vport_num];
+	bool is_uc = list_type == MLX5_NVPRT_LIST_TYPE_UC;
+	u8 (*mac_list)[ETH_ALEN];
+	struct l2addr_node *node;
+	struct vport_addr *addr;
+	struct hlist_head *hash;
+	struct hlist_node *tmp;
+	int size;
+	int err;
+	int hi;
+	int i;
+
+	size = is_uc ? MLX5_MAX_UC_PER_VPORT(esw->dev) :
+		       MLX5_MAX_MC_PER_VPORT(esw->dev);
+
+	mac_list = kcalloc(size, ETH_ALEN, GFP_KERNEL);
+	if (!mac_list)
+		return;
+
+	hash = is_uc ? vport->uc_list : vport->mc_list;
+
+	for_each_l2hash_node(node, tmp, hash, hi) {
+		addr = container_of(node, struct vport_addr, node);
+		addr->action = MLX5_ACTION_DEL;
+	}
+
+	err = mlx5_query_nic_vport_mac_list(esw->dev, vport_num, list_type,
+					    mac_list, &size);
+	if (err)
+		return;
+	esw_debug(esw->dev, "vport[%d] context update %s list size (%d)\n",
+		  vport_num, is_uc ? "UC" : "MC", size);
+
+	for (i = 0; i < size; i++) {
+		if (is_uc && !is_valid_ether_addr(mac_list[i]))
+			continue;
+
+		if (!is_uc && !is_multicast_ether_addr(mac_list[i]))
+			continue;
+
+		addr = l2addr_hash_find(hash, mac_list[i], struct vport_addr);
+		if (addr) {
+			addr->action = MLX5_ACTION_NONE;
+			continue;
+		}
+
+		addr = l2addr_hash_add(hash, mac_list[i], struct vport_addr,
+				       GFP_KERNEL);
+		if (!addr) {
+			esw_warn(esw->dev,
+				 "Failed to add MAC(%pM) to vport[%d] DB\n",
+				 mac_list[i], vport_num);
+			continue;
+		}
+		addr->vport = vport_num;
+		addr->action = MLX5_ACTION_ADD;
+	}
+	kfree(mac_list);
+}
+
+static void esw_vport_change_handler(struct work_struct *work)
+{
+	struct mlx5_vport *vport =
+		container_of(work, struct mlx5_vport, vport_change_handler);
+	struct mlx5_core_dev *dev = vport->dev;
+	struct mlx5_eswitch *esw = dev->priv.eswitch;
+	u8 mac[ETH_ALEN];
+
+	mlx5_query_nic_vport_mac_address(dev, vport->vport, mac);
+	esw_debug(dev, "vport[%d] Context Changed: perm mac: %pM\n",
+		  vport->vport, mac);
+
+	if (vport->enabled_events & UC_ADDR_CHANGE) {
+		esw_update_vport_addr_list(esw, vport->vport,
+					   MLX5_NVPRT_LIST_TYPE_UC);
+		esw_apply_vport_addr_list(esw, vport->vport,
+					  MLX5_NVPRT_LIST_TYPE_UC);
+	}
+
+	if (vport->enabled_events & MC_ADDR_CHANGE) {
+		esw_update_vport_addr_list(esw, vport->vport,
+					   MLX5_NVPRT_LIST_TYPE_MC);
+		esw_apply_vport_addr_list(esw, vport->vport,
+					  MLX5_NVPRT_LIST_TYPE_MC);
+	}
+
+	esw_debug(esw->dev, "vport[%d] Context Changed: Done\n", vport->vport);
+	if (vport->enabled)
+		arm_vport_context_events_cmd(dev, vport->vport,
+					     vport->enabled_events);
+}
+
+static void esw_enable_vport(struct mlx5_eswitch *esw, int vport_num,
+			     int enable_events)
+{
+	struct mlx5_vport *vport = &esw->vports[vport_num];
+	unsigned long flags;
+
+	WARN_ON(vport->enabled);
+
+	esw_debug(esw->dev, "Enabling VPORT(%d)\n", vport_num);
+	mlx5_modify_vport_admin_state(esw->dev,
+				      MLX5_QUERY_VPORT_STATE_IN_OP_MOD_ESW_VPORT,
+				      vport_num,
+				      MLX5_ESW_VPORT_ADMIN_STATE_AUTO);
+
+	/* Sync with current vport context */
+	vport->enabled_events = enable_events;
+	esw_vport_change_handler(&vport->vport_change_handler);
+
+	spin_lock_irqsave(&vport->lock, flags);
+	vport->enabled = true;
+	spin_unlock_irqrestore(&vport->lock, flags);
+
+	arm_vport_context_events_cmd(esw->dev, vport_num, enable_events);
+
+	esw->enabled_vports++;
+	esw_debug(esw->dev, "Enabled VPORT(%d)\n", vport_num);
+}
+
+static void esw_cleanup_vport(struct mlx5_eswitch *esw, u16 vport_num)
+{
+	struct mlx5_vport *vport = &esw->vports[vport_num];
+	struct l2addr_node *node;
+	struct vport_addr *addr;
+	struct hlist_node *tmp;
+	int hi;
+
+	for_each_l2hash_node(node, tmp, vport->uc_list, hi) {
+		addr = container_of(node, struct vport_addr, node);
+		addr->action = MLX5_ACTION_DEL;
+	}
+	esw_apply_vport_addr_list(esw, vport_num, MLX5_NVPRT_LIST_TYPE_UC);
+
+	for_each_l2hash_node(node, tmp, vport->mc_list, hi) {
+		addr = container_of(node, struct vport_addr, node);
+		addr->action = MLX5_ACTION_DEL;
+	}
+	esw_apply_vport_addr_list(esw, vport_num, MLX5_NVPRT_LIST_TYPE_MC);
+}
+
+static void esw_disable_vport(struct mlx5_eswitch *esw, int vport_num)
+{
+	struct mlx5_vport *vport = &esw->vports[vport_num];
+	unsigned long flags;
+
+	if (!vport->enabled)
+		return;
+
+	esw_debug(esw->dev, "Disabling vport(%d)\n", vport_num);
+	/* Mark this vport as disabled to discard new events */
+	spin_lock_irqsave(&vport->lock, flags);
+	vport->enabled = false;
+	vport->enabled_events = 0;
+	spin_unlock_irqrestore(&vport->lock, flags);
+
+	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 */
+	arm_vport_context_events_cmd(esw->dev, vport->vport, 0);
+	/* We don't assume VFs will cleanup after themselves */
+	esw_cleanup_vport(esw, vport_num);
+	esw->enabled_vports--;
+}
+
+/* Public E-Switch API */
+int mlx5_eswitch_enable_sriov(struct mlx5_eswitch *esw, int nvfs)
+{
+	int err;
+	int i;
+
+	if (!esw || !MLX5_CAP_GEN(esw->dev, vport_group_manager) ||
+	    MLX5_CAP_GEN(esw->dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return 0;
+
+	if (!MLX5_CAP_GEN(esw->dev, eswitch_flow_table) ||
+	    !MLX5_CAP_ESW_FLOWTABLE_FDB(esw->dev, ft_support)) {
+		esw_warn(esw->dev, "E-Switch FDB is not supported, aborting ...\n");
+		return -ENOTSUPP;
+	}
+
+	esw_info(esw->dev, "E-Switch enable SRIOV: nvfs(%d)\n", nvfs);
+
+	esw_disable_vport(esw, 0);
+
+	err = esw_create_fdb_table(esw, nvfs + 1);
+	if (err)
+		goto abort;
+
+	for (i = 0; i <= nvfs; i++)
+		esw_enable_vport(esw, i, SRIOV_VPORT_EVENTS);
+
+	esw_info(esw->dev, "SRIOV enabled: active vports(%d)\n",
+		 esw->enabled_vports);
+	return 0;
+
+abort:
+	esw_enable_vport(esw, 0, UC_ADDR_CHANGE);
+	return err;
+}
+
+void mlx5_eswitch_disable_sriov(struct mlx5_eswitch *esw)
+{
+	int i;
+
+	if (!esw || !MLX5_CAP_GEN(esw->dev, vport_group_manager) ||
+	    MLX5_CAP_GEN(esw->dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return;
+
+	esw_info(esw->dev, "disable SRIOV: active vports(%d)\n",
+		 esw->enabled_vports);
+
+	for (i = 0; i < esw->total_vports; i++)
+		esw_disable_vport(esw, i);
+
+	esw_destroy_fdb_table(esw);
+
+	/* VPORT 0 (PF) must be enabled back with non-sriov configuration */
+	esw_enable_vport(esw, 0, UC_ADDR_CHANGE);
+}
+
+int mlx5_eswitch_init(struct mlx5_core_dev *dev)
+{
+	int l2_table_size = 1 << MLX5_CAP_GEN(dev, log_max_l2_table);
+	int total_vports = 1 + pci_sriov_get_totalvfs(dev->pdev);
+	struct mlx5_eswitch *esw;
+	int vport_num;
+	int err;
+
+	if (!MLX5_CAP_GEN(dev, vport_group_manager) ||
+	    MLX5_CAP_GEN(dev, port_type) != MLX5_CAP_PORT_TYPE_ETH)
+		return 0;
+
+	esw_info(dev,
+		 "Total vports %d, l2 table size(%d), per vport: max uc(%d) max mc(%d)\n",
+		 total_vports, l2_table_size,
+		 MLX5_MAX_UC_PER_VPORT(dev),
+		 MLX5_MAX_MC_PER_VPORT(dev));
+
+	esw = kzalloc(sizeof(*esw), GFP_KERNEL);
+	if (!esw)
+		return -ENOMEM;
+
+	esw->dev = dev;
+
+	esw->l2_table.bitmap = kcalloc(BITS_TO_LONGS(l2_table_size),
+				   sizeof(uintptr_t), GFP_KERNEL);
+	if (!esw->l2_table.bitmap) {
+		err = -ENOMEM;
+		goto abort;
+	}
+	esw->l2_table.size = l2_table_size;
+
+	esw->work_queue = create_singlethread_workqueue("mlx5_esw_wq");
+	if (!esw->work_queue) {
+		err = -ENOMEM;
+		goto abort;
+	}
+
+	esw->vports = kcalloc(total_vports, sizeof(struct mlx5_vport),
+			      GFP_KERNEL);
+	if (!esw->vports) {
+		err = -ENOMEM;
+		goto abort;
+	}
+
+	for (vport_num = 0; vport_num < total_vports; vport_num++) {
+		struct mlx5_vport *vport = &esw->vports[vport_num];
+
+		vport->vport = vport_num;
+		vport->dev = dev;
+		INIT_WORK(&vport->vport_change_handler,
+			  esw_vport_change_handler);
+		spin_lock_init(&vport->lock);
+	}
+
+	esw->total_vports = total_vports;
+	esw->enabled_vports = 0;
+
+	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)
+		destroy_workqueue(esw->work_queue);
+	kfree(esw->l2_table.bitmap);
+	kfree(esw->vports);
+	kfree(esw);
+	return err;
+}
+
+void mlx5_eswitch_cleanup(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_info(esw->dev, "cleanup\n");
+	esw_disable_vport(esw, 0);
+
+	esw->dev->priv.eswitch = NULL;
+	destroy_workqueue(esw->work_queue);
+	kfree(esw->l2_table.bitmap);
+	kfree(esw->vports);
+	kfree(esw);
+}
+
+void mlx5_eswitch_vport_event(struct mlx5_eswitch *esw, struct mlx5_eqe *eqe)
+{
+	struct mlx5_eqe_vport_change *vc_eqe = &eqe->data.vport_change;
+	u16 vport_num = be16_to_cpu(vc_eqe->vport_num);
+	struct mlx5_vport *vport;
+
+	if (!esw) {
+		pr_warn("MLX5 E-Switch: vport %d got an event while eswitch is not initialized\n",
+			vport_num);
+		return;
+	}
+
+	vport = &esw->vports[vport_num];
+	spin_lock(&vport->lock);
+	if (vport->enabled)
+		queue_work(esw->work_queue, &vport->vport_change_handler);
+	spin_unlock(&vport->lock);
+}
+
+/* Vport Administration */
+#define ESW_ALLOWED(esw) \
+	(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)
+
+int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
+			       int vport, u8 mac[ETH_ALEN])
+{
+	int err = 0;
+
+	if (!ESW_ALLOWED(esw))
+		return -EPERM;
+	if (!LEGAL_VPORT(esw, vport))
+		return -EINVAL;
+
+	err = mlx5_modify_nic_vport_mac_address(esw->dev, vport, mac);
+	if (err) {
+		mlx5_core_warn(esw->dev,
+			       "Failed to mlx5_modify_nic_vport_mac vport(%d) err=(%d)\n",
+			       vport, err);
+		return err;
+	}
+
+	return err;
+}
+
+int mlx5_eswitch_set_vport_state(struct mlx5_eswitch *esw,
+				 int vport, int link_state)
+{
+	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);
+}
+
+int mlx5_eswitch_get_vport_config(struct mlx5_eswitch *esw,
+				  int vport, struct ifla_vf_info *ivi)
+{
+	u16 vlan;
+	u8 qos;
+
+	if (!ESW_ALLOWED(esw))
+		return -EPERM;
+	if (!LEGAL_VPORT(esw, vport))
+		return -EINVAL;
+
+	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 = 0;
+
+	return 0;
+}
+
+int mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
+				int vport, u16 vlan, u8 qos)
+{
+	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;
+
+	return modify_esw_vport_cvlan(esw->dev, vport, vlan, qos, set);
+}
+
+int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
+				 int vport,
+				 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)];
+	int err = 0;
+	u32 *out;
+
+	if (!ESW_ALLOWED(esw))
+		return -EPERM;
+	if (!LEGAL_VPORT(esw, vport))
+		return -EINVAL;
+
+	out = mlx5_vzalloc(outlen);
+	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);
+	MLX5_SET(query_vport_counter_in, in, vport_number, vport);
+	if (vport)
+		MLX5_SET(query_vport_counter_in, in, other_vport, 1);
+
+	memset(out, 0, outlen);
+	err = mlx5_cmd_exec(esw->dev, in, sizeof(in), out, outlen);
+	if (err)
+		goto free_out;
+
+	#define MLX5_GET_CTR(p, x) \
+		MLX5_GET64(query_vport_counter_out, p, x)
+
+	memset(vf_stats, 0, sizeof(*vf_stats));
+	vf_stats->rx_packets =
+		MLX5_GET_CTR(out, received_eth_unicast.packets) +
+		MLX5_GET_CTR(out, received_eth_multicast.packets) +
+		MLX5_GET_CTR(out, received_eth_broadcast.packets);
+
+	vf_stats->rx_bytes =
+		MLX5_GET_CTR(out, received_eth_unicast.octets) +
+		MLX5_GET_CTR(out, received_eth_multicast.octets) +
+		MLX5_GET_CTR(out, received_eth_broadcast.octets);
+
+	vf_stats->tx_packets =
+		MLX5_GET_CTR(out, transmitted_eth_unicast.packets) +
+		MLX5_GET_CTR(out, transmitted_eth_multicast.packets) +
+		MLX5_GET_CTR(out, transmitted_eth_broadcast.packets);
+
+	vf_stats->tx_bytes =
+		MLX5_GET_CTR(out, transmitted_eth_unicast.octets) +
+		MLX5_GET_CTR(out, transmitted_eth_multicast.octets) +
+		MLX5_GET_CTR(out, transmitted_eth_broadcast.octets);
+
+	vf_stats->multicast =
+		MLX5_GET_CTR(out, received_eth_multicast.packets);
+
+	vf_stats->broadcast =
+		MLX5_GET_CTR(out, received_eth_broadcast.packets);
+
+free_out:
+	kvfree(out);
+	return err;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
new file mode 100644
index 000000000000..3416a428f70f
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eswitch.h
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies, Ltd.  All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef __MLX5_ESWITCH_H__
+#define __MLX5_ESWITCH_H__
+
+#include <linux/if_ether.h>
+#include <linux/if_link.h>
+#include <linux/mlx5/device.h>
+
+#define MLX5_MAX_UC_PER_VPORT(dev) \
+	(1 << MLX5_CAP_GEN(dev, log_max_current_uc_list))
+
+#define MLX5_MAX_MC_PER_VPORT(dev) \
+	(1 << MLX5_CAP_GEN(dev, log_max_current_mc_list))
+
+#define MLX5_L2_ADDR_HASH_SIZE (BIT(BITS_PER_BYTE))
+#define MLX5_L2_ADDR_HASH(addr) (addr[5])
+
+/* L2 -mac address based- hash helpers */
+struct l2addr_node {
+	struct hlist_node hlist;
+	u8                addr[ETH_ALEN];
+};
+
+#define for_each_l2hash_node(hn, tmp, hash, i) \
+	for (i = 0; i < MLX5_L2_ADDR_HASH_SIZE; i++) \
+		hlist_for_each_entry_safe(hn, tmp, &hash[i], hlist)
+
+#define l2addr_hash_find(hash, mac, type) ({                \
+	int ix = MLX5_L2_ADDR_HASH(mac);                    \
+	bool found = false;                                 \
+	type *ptr = NULL;                                   \
+							    \
+	hlist_for_each_entry(ptr, &hash[ix], node.hlist)    \
+		if (ether_addr_equal(ptr->node.addr, mac)) {\
+			found = true;                       \
+			break;                              \
+		}                                           \
+	if (!found)                                         \
+		ptr = NULL;                                 \
+	ptr;                                                \
+})
+
+#define l2addr_hash_add(hash, mac, type, gfp) ({            \
+	int ix = MLX5_L2_ADDR_HASH(mac);                    \
+	type *ptr = NULL;                                   \
+							    \
+	ptr = kzalloc(sizeof(type), gfp);                   \
+	if (ptr) {                                          \
+		ether_addr_copy(ptr->node.addr, mac);       \
+		hlist_add_head(&ptr->node.hlist, &hash[ix]);\
+	}                                                   \
+	ptr;                                                \
+})
+
+#define l2addr_hash_del(ptr) ({                             \
+	hlist_del(&ptr->node.hlist);                        \
+	kfree(ptr);                                         \
+})
+
+struct mlx5_vport {
+	struct mlx5_core_dev    *dev;
+	int                     vport;
+	struct hlist_head       uc_list[MLX5_L2_ADDR_HASH_SIZE];
+	struct hlist_head       mc_list[MLX5_L2_ADDR_HASH_SIZE];
+	struct work_struct      vport_change_handler;
+
+	/* This spinlock protects access to vport data, between
+	 * "esw_vport_disable" and ongoing interrupt "mlx5_eswitch_vport_event"
+	 * once vport marked as disabled new interrupts are discarded.
+	 */
+	spinlock_t              lock; /* vport events sync */
+	bool                    enabled;
+	u16                     enabled_events;
+};
+
+struct mlx5_l2_table {
+	struct hlist_head l2_hash[MLX5_L2_ADDR_HASH_SIZE];
+	u32                  size;
+	unsigned long        *bitmap;
+};
+
+struct mlx5_eswitch_fdb {
+	void *fdb;
+	struct mlx5_flow_group *addr_grp;
+};
+
+struct mlx5_eswitch {
+	struct mlx5_core_dev    *dev;
+	struct mlx5_l2_table    l2_table;
+	struct mlx5_eswitch_fdb fdb_table;
+	struct hlist_head       mc_table[MLX5_L2_ADDR_HASH_SIZE];
+	struct workqueue_struct *work_queue;
+	struct mlx5_vport       *vports;
+	int                     total_vports;
+	int                     enabled_vports;
+};
+
+/* E-Switch API */
+int mlx5_eswitch_init(struct mlx5_core_dev *dev);
+void mlx5_eswitch_cleanup(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);
+void mlx5_eswitch_disable_sriov(struct mlx5_eswitch *esw);
+int mlx5_eswitch_set_vport_mac(struct mlx5_eswitch *esw,
+			       int vport, u8 mac[ETH_ALEN]);
+int mlx5_eswitch_set_vport_state(struct mlx5_eswitch *esw,
+				 int vport, int link_state);
+int mlx5_eswitch_set_vport_vlan(struct mlx5_eswitch *esw,
+				int vport, u16 vlan, u8 qos);
+int mlx5_eswitch_get_vport_config(struct mlx5_eswitch *esw,
+				  int vport, struct ifla_vf_info *ivi);
+int mlx5_eswitch_get_vport_stats(struct mlx5_eswitch *esw,
+				 int vport,
+				 struct ifla_vf_stats *vf_stats);
+
+#endif /* __MLX5_ESWITCH_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c b/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c
deleted file mode 100644
index ca90b9bc3b95..000000000000
--- a/drivers/net/ethernet/mellanox/mlx5/core/flow_table.c
+++ /dev/null
@@ -1,422 +0,0 @@
-/*
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <linux/export.h>
-#include <linux/mlx5/driver.h>
-#include <linux/mlx5/flow_table.h>
-#include "mlx5_core.h"
-
-struct mlx5_ftg {
-	struct mlx5_flow_table_group    g;
-	u32				id;
-	u32				start_ix;
-};
-
-struct mlx5_flow_table {
-	struct mlx5_core_dev	*dev;
-	u8			level;
-	u8			type;
-	u32			id;
-	struct mutex		mutex; /* sync bitmap alloc */
-	u16			num_groups;
-	struct mlx5_ftg		*group;
-	unsigned long		*bitmap;
-	u32			size;
-};
-
-static int mlx5_set_flow_entry_cmd(struct mlx5_flow_table *ft, u32 group_ix,
-				   u32 flow_index, void *flow_context)
-{
-	u32 out[MLX5_ST_SZ_DW(set_fte_out)];
-	u32 *in;
-	void *in_flow_context;
-	int fcdls =
-		MLX5_GET(flow_context, flow_context, destination_list_size) *
-		MLX5_ST_SZ_BYTES(dest_format_struct);
-	int inlen = MLX5_ST_SZ_BYTES(set_fte_in) + fcdls;
-	int err;
-
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_warn(ft->dev, "failed to allocate inbox\n");
-		return -ENOMEM;
-	}
-
-	MLX5_SET(set_fte_in, in, table_type, ft->type);
-	MLX5_SET(set_fte_in, in, table_id,   ft->id);
-	MLX5_SET(set_fte_in, in, flow_index, flow_index);
-	MLX5_SET(set_fte_in, in, opcode, MLX5_CMD_OP_SET_FLOW_TABLE_ENTRY);
-
-	in_flow_context = MLX5_ADDR_OF(set_fte_in, in, flow_context);
-	memcpy(in_flow_context, flow_context,
-	       MLX5_ST_SZ_BYTES(flow_context) + fcdls);
-
-	MLX5_SET(flow_context, in_flow_context, group_id,
-		 ft->group[group_ix].id);
-
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec_check_status(ft->dev, in, inlen, out,
-					 sizeof(out));
-	kvfree(in);
-
-	return err;
-}
-
-static void mlx5_del_flow_entry_cmd(struct mlx5_flow_table *ft, u32 flow_index)
-{
-	u32 in[MLX5_ST_SZ_DW(delete_fte_in)];
-	u32 out[MLX5_ST_SZ_DW(delete_fte_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
-#define MLX5_SET_DFTEI(p, x, v) MLX5_SET(delete_fte_in, p, x, v)
-	MLX5_SET_DFTEI(in, table_type, ft->type);
-	MLX5_SET_DFTEI(in, table_id,   ft->id);
-	MLX5_SET_DFTEI(in, flow_index, flow_index);
-	MLX5_SET_DFTEI(in, opcode,     MLX5_CMD_OP_DELETE_FLOW_TABLE_ENTRY);
-
-	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
-}
-
-static void mlx5_destroy_flow_group_cmd(struct mlx5_flow_table *ft, int i)
-{
-	u32 in[MLX5_ST_SZ_DW(destroy_flow_group_in)];
-	u32 out[MLX5_ST_SZ_DW(destroy_flow_group_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
-#define MLX5_SET_DFGI(p, x, v) MLX5_SET(destroy_flow_group_in, p, x, v)
-	MLX5_SET_DFGI(in, table_type, ft->type);
-	MLX5_SET_DFGI(in, table_id,   ft->id);
-	MLX5_SET_DFGI(in, opcode, MLX5_CMD_OP_DESTROY_FLOW_GROUP);
-	MLX5_SET_DFGI(in, group_id, ft->group[i].id);
-	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
-}
-
-static int mlx5_create_flow_group_cmd(struct mlx5_flow_table *ft, int i)
-{
-	u32 out[MLX5_ST_SZ_DW(create_flow_group_out)];
-	u32 *in;
-	void *in_match_criteria;
-	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
-	struct mlx5_flow_table_group *g = &ft->group[i].g;
-	u32 start_ix = ft->group[i].start_ix;
-	u32 end_ix = start_ix + (1 << g->log_sz) - 1;
-	int err;
-
-	in = mlx5_vzalloc(inlen);
-	if (!in) {
-		mlx5_core_warn(ft->dev, "failed to allocate inbox\n");
-		return -ENOMEM;
-	}
-	in_match_criteria = MLX5_ADDR_OF(create_flow_group_in, in,
-					 match_criteria);
-
-	memset(out, 0, sizeof(out));
-
-#define MLX5_SET_CFGI(p, x, v) MLX5_SET(create_flow_group_in, p, x, v)
-	MLX5_SET_CFGI(in, table_type,            ft->type);
-	MLX5_SET_CFGI(in, table_id,              ft->id);
-	MLX5_SET_CFGI(in, opcode,                MLX5_CMD_OP_CREATE_FLOW_GROUP);
-	MLX5_SET_CFGI(in, start_flow_index,      start_ix);
-	MLX5_SET_CFGI(in, end_flow_index,        end_ix);
-	MLX5_SET_CFGI(in, match_criteria_enable, g->match_criteria_enable);
-
-	memcpy(in_match_criteria, g->match_criteria,
-	       MLX5_ST_SZ_BYTES(fte_match_param));
-
-	err = mlx5_cmd_exec_check_status(ft->dev, in, inlen, out,
-					 sizeof(out));
-	if (!err)
-		ft->group[i].id = MLX5_GET(create_flow_group_out, out,
-					   group_id);
-
-	kvfree(in);
-
-	return err;
-}
-
-static void mlx5_destroy_flow_table_groups(struct mlx5_flow_table *ft)
-{
-	int i;
-
-	for (i = 0; i < ft->num_groups; i++)
-		mlx5_destroy_flow_group_cmd(ft, i);
-}
-
-static int mlx5_create_flow_table_groups(struct mlx5_flow_table *ft)
-{
-	int err;
-	int i;
-
-	for (i = 0; i < ft->num_groups; i++) {
-		err = mlx5_create_flow_group_cmd(ft, i);
-		if (err)
-			goto err_destroy_flow_table_groups;
-	}
-
-	return 0;
-
-err_destroy_flow_table_groups:
-	for (i--; i >= 0; i--)
-		mlx5_destroy_flow_group_cmd(ft, i);
-
-	return err;
-}
-
-static int mlx5_create_flow_table_cmd(struct mlx5_flow_table *ft)
-{
-	u32 in[MLX5_ST_SZ_DW(create_flow_table_in)];
-	u32 out[MLX5_ST_SZ_DW(create_flow_table_out)];
-	int err;
-
-	memset(in, 0, sizeof(in));
-
-	MLX5_SET(create_flow_table_in, in, table_type, ft->type);
-	MLX5_SET(create_flow_table_in, in, level,      ft->level);
-	MLX5_SET(create_flow_table_in, in, log_size,   order_base_2(ft->size));
-
-	MLX5_SET(create_flow_table_in, in, opcode,
-		 MLX5_CMD_OP_CREATE_FLOW_TABLE);
-
-	memset(out, 0, sizeof(out));
-	err = mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out,
-					 sizeof(out));
-	if (err)
-		return err;
-
-	ft->id = MLX5_GET(create_flow_table_out, out, table_id);
-
-	return 0;
-}
-
-static void mlx5_destroy_flow_table_cmd(struct mlx5_flow_table *ft)
-{
-	u32 in[MLX5_ST_SZ_DW(destroy_flow_table_in)];
-	u32 out[MLX5_ST_SZ_DW(destroy_flow_table_out)];
-
-	memset(in, 0, sizeof(in));
-	memset(out, 0, sizeof(out));
-
-#define MLX5_SET_DFTI(p, x, v) MLX5_SET(destroy_flow_table_in, p, x, v)
-	MLX5_SET_DFTI(in, table_type, ft->type);
-	MLX5_SET_DFTI(in, table_id,   ft->id);
-	MLX5_SET_DFTI(in, opcode, MLX5_CMD_OP_DESTROY_FLOW_TABLE);
-
-	mlx5_cmd_exec_check_status(ft->dev, in, sizeof(in), out, sizeof(out));
-}
-
-static int mlx5_find_group(struct mlx5_flow_table *ft, u8 match_criteria_enable,
-			   u32 *match_criteria, int *group_ix)
-{
-	void *mc_outer = MLX5_ADDR_OF(fte_match_param, match_criteria,
-				      outer_headers);
-	void *mc_misc  = MLX5_ADDR_OF(fte_match_param, match_criteria,
-				      misc_parameters);
-	void *mc_inner = MLX5_ADDR_OF(fte_match_param, match_criteria,
-				      inner_headers);
-	int mc_outer_sz = MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4);
-	int mc_misc_sz  = MLX5_ST_SZ_BYTES(fte_match_set_misc);
-	int mc_inner_sz = MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4);
-	int i;
-
-	for (i = 0; i < ft->num_groups; i++) {
-		struct mlx5_flow_table_group *g = &ft->group[i].g;
-		void *gmc_outer = MLX5_ADDR_OF(fte_match_param,
-					       g->match_criteria,
-					       outer_headers);
-		void *gmc_misc  = MLX5_ADDR_OF(fte_match_param,
-					       g->match_criteria,
-					       misc_parameters);
-		void *gmc_inner = MLX5_ADDR_OF(fte_match_param,
-					       g->match_criteria,
-					       inner_headers);
-
-		if (g->match_criteria_enable != match_criteria_enable)
-			continue;
-
-		if (match_criteria_enable & MLX5_MATCH_OUTER_HEADERS)
-			if (memcmp(mc_outer, gmc_outer, mc_outer_sz))
-				continue;
-
-		if (match_criteria_enable & MLX5_MATCH_MISC_PARAMETERS)
-			if (memcmp(mc_misc, gmc_misc, mc_misc_sz))
-				continue;
-
-		if (match_criteria_enable & MLX5_MATCH_INNER_HEADERS)
-			if (memcmp(mc_inner, gmc_inner, mc_inner_sz))
-				continue;
-
-		*group_ix = i;
-		return 0;
-	}
-
-	return -EINVAL;
-}
-
-static int alloc_flow_index(struct mlx5_flow_table *ft, int group_ix, u32 *ix)
-{
-	struct mlx5_ftg *g = &ft->group[group_ix];
-	int err = 0;
-
-	mutex_lock(&ft->mutex);
-
-	*ix = find_next_zero_bit(ft->bitmap, ft->size, g->start_ix);
-	if (*ix >= (g->start_ix + (1 << g->g.log_sz)))
-		err = -ENOSPC;
-	else
-		__set_bit(*ix, ft->bitmap);
-
-	mutex_unlock(&ft->mutex);
-
-	return err;
-}
-
-static void mlx5_free_flow_index(struct mlx5_flow_table *ft, u32 ix)
-{
-	__clear_bit(ix, ft->bitmap);
-}
-
-int mlx5_add_flow_table_entry(void *flow_table, u8 match_criteria_enable,
-			      void *match_criteria, void *flow_context,
-			      u32 *flow_index)
-{
-	struct mlx5_flow_table *ft = flow_table;
-	int group_ix;
-	int err;
-
-	err = mlx5_find_group(ft, match_criteria_enable, match_criteria,
-			      &group_ix);
-	if (err) {
-		mlx5_core_warn(ft->dev, "mlx5_find_group failed\n");
-		return err;
-	}
-
-	err = alloc_flow_index(ft, group_ix, flow_index);
-	if (err) {
-		mlx5_core_warn(ft->dev, "alloc_flow_index failed\n");
-		return err;
-	}
-
-	return mlx5_set_flow_entry_cmd(ft, group_ix, *flow_index, flow_context);
-}
-EXPORT_SYMBOL(mlx5_add_flow_table_entry);
-
-void mlx5_del_flow_table_entry(void *flow_table, u32 flow_index)
-{
-	struct mlx5_flow_table *ft = flow_table;
-
-	mlx5_del_flow_entry_cmd(ft, flow_index);
-	mlx5_free_flow_index(ft, flow_index);
-}
-EXPORT_SYMBOL(mlx5_del_flow_table_entry);
-
-void *mlx5_create_flow_table(struct mlx5_core_dev *dev, u8 level, u8 table_type,
-			     u16 num_groups,
-			     struct mlx5_flow_table_group *group)
-{
-	struct mlx5_flow_table *ft;
-	u32 start_ix = 0;
-	u32 ft_size = 0;
-	void *gr;
-	void *bm;
-	int err;
-	int i;
-
-	for (i = 0; i < num_groups; i++)
-		ft_size += (1 << group[i].log_sz);
-
-	ft = kzalloc(sizeof(*ft), GFP_KERNEL);
-	gr = kcalloc(num_groups, sizeof(struct mlx5_ftg), GFP_KERNEL);
-	bm = kcalloc(BITS_TO_LONGS(ft_size), sizeof(uintptr_t), GFP_KERNEL);
-	if (!ft || !gr || !bm)
-		goto err_free_ft;
-
-	ft->group	= gr;
-	ft->bitmap	= bm;
-	ft->num_groups	= num_groups;
-	ft->level	= level;
-	ft->type	= table_type;
-	ft->size	= ft_size;
-	ft->dev		= dev;
-	mutex_init(&ft->mutex);
-
-	for (i = 0; i < ft->num_groups; i++) {
-		memcpy(&ft->group[i].g, &group[i], sizeof(*group));
-		ft->group[i].start_ix = start_ix;
-		start_ix += 1 << group[i].log_sz;
-	}
-
-	err = mlx5_create_flow_table_cmd(ft);
-	if (err)
-		goto err_free_ft;
-
-	err = mlx5_create_flow_table_groups(ft);
-	if (err)
-		goto err_destroy_flow_table_cmd;
-
-	return ft;
-
-err_destroy_flow_table_cmd:
-	mlx5_destroy_flow_table_cmd(ft);
-
-err_free_ft:
-	mlx5_core_warn(dev, "failed to alloc flow table\n");
-	kfree(bm);
-	kfree(gr);
-	kfree(ft);
-
-	return NULL;
-}
-EXPORT_SYMBOL(mlx5_create_flow_table);
-
-void mlx5_destroy_flow_table(void *flow_table)
-{
-	struct mlx5_flow_table *ft = flow_table;
-
-	mlx5_destroy_flow_table_groups(ft);
-	mlx5_destroy_flow_table_cmd(ft);
-	kfree(ft->bitmap);
-	kfree(ft->group);
-	kfree(ft);
-}
-EXPORT_SYMBOL(mlx5_destroy_flow_table);
-
-u32 mlx5_get_flow_table_id(void *flow_table)
-{
-	struct mlx5_flow_table *ft = flow_table;
-
-	return ft->id;
-}
-EXPORT_SYMBOL(mlx5_get_flow_table_id);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
new file mode 100644
index 000000000000..a9894d2e8e26
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.c
@@ -0,0 +1,289 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/mlx5/driver.h>
+#include <linux/mlx5/device.h>
+#include <linux/mlx5/mlx5_ifc.h>
+
+#include "fs_core.h"
+#include "fs_cmd.h"
+#include "mlx5_core.h"
+
+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));
+
+	MLX5_SET(set_flow_table_root_in, in, opcode,
+		 MLX5_CMD_OP_SET_FLOW_TABLE_ROOT);
+	MLX5_SET(set_flow_table_root_in, in, table_type, ft->type);
+	MLX5_SET(set_flow_table_root_in, in, table_id, ft->id);
+
+	memset(out, 0, sizeof(out));
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					  sizeof(out));
+}
+
+int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
+			       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)];
+	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);
+
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					 sizeof(out));
+
+	if (!err)
+		*table_id = MLX5_GET(create_flow_table_out, out,
+				     table_id);
+	return err;
+}
+
+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));
+
+	MLX5_SET(destroy_flow_table_in, in, opcode,
+		 MLX5_CMD_OP_DESTROY_FLOW_TABLE);
+	MLX5_SET(destroy_flow_table_in, in, table_type, ft->type);
+	MLX5_SET(destroy_flow_table_in, in, table_id, ft->id);
+
+	return mlx5_cmd_exec_check_status(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));
+
+	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);
+	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));
+}
+
+int mlx5_cmd_create_flow_group(struct mlx5_core_dev *dev,
+			       struct mlx5_flow_table *ft,
+			       u32 *in,
+			       unsigned int *group_id)
+{
+	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);
+	MLX5_SET(create_flow_group_in, in, table_id, ft->id);
+
+	err = mlx5_cmd_exec_check_status(dev, in,
+					 inlen, out,
+					 sizeof(out));
+	if (!err)
+		*group_id = MLX5_GET(create_flow_group_out, out,
+				     group_id);
+
+	return err;
+}
+
+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));
+
+	MLX5_SET(destroy_flow_group_in, in, opcode,
+		 MLX5_CMD_OP_DESTROY_FLOW_GROUP);
+	MLX5_SET(destroy_flow_group_in, in, table_type, ft->type);
+	MLX5_SET(destroy_flow_group_in, in, table_id, ft->id);
+	MLX5_SET(destroy_flow_group_in, in, group_id, group_id);
+
+	return mlx5_cmd_exec_check_status(dev, in, sizeof(in), out,
+					  sizeof(out));
+}
+
+static int mlx5_cmd_set_fte(struct mlx5_core_dev *dev,
+			    int opmod, int modify_mask,
+			    struct mlx5_flow_table *ft,
+			    unsigned group_id,
+			    struct fs_fte *fte)
+{
+	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)];
+	struct mlx5_flow_rule *dst;
+	void *in_flow_context;
+	void *in_match_value;
+	void *in_dests;
+	u32 *in;
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_warn(dev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+
+	MLX5_SET(set_fte_in, in, opcode, MLX5_CMD_OP_SET_FLOW_TABLE_ENTRY);
+	MLX5_SET(set_fte_in, in, op_mod, opmod);
+	MLX5_SET(set_fte_in, in, modify_enable_mask, modify_mask);
+	MLX5_SET(set_fte_in, in, table_type, ft->type);
+	MLX5_SET(set_fte_in, in, table_id,   ft->id);
+	MLX5_SET(set_fte_in, in, flow_index, fte->index);
+
+	in_flow_context = MLX5_ADDR_OF(set_fte_in, in, flow_context);
+	MLX5_SET(flow_context, in_flow_context, group_id, group_id);
+	MLX5_SET(flow_context, in_flow_context, flow_tag, fte->flow_tag);
+	MLX5_SET(flow_context, in_flow_context, action, fte->action);
+	MLX5_SET(flow_context, in_flow_context, destination_list_size,
+		 fte->dests_size);
+	in_match_value = MLX5_ADDR_OF(flow_context, in_flow_context,
+				      match_value);
+	memcpy(in_match_value, &fte->val, MLX5_ST_SZ_BYTES(fte_match_param));
+
+	in_dests = MLX5_ADDR_OF(flow_context, in_flow_context, destination);
+	list_for_each_entry(dst, &fte->node.children, node.list) {
+		unsigned int id;
+
+		MLX5_SET(dest_format_struct, in_dests, destination_type,
+			 dst->dest_attr.type);
+		if (dst->dest_attr.type ==
+		    MLX5_FLOW_DESTINATION_TYPE_FLOW_TABLE)
+			id = dst->dest_attr.ft->id;
+		else
+			id = dst->dest_attr.tir_num;
+		MLX5_SET(dest_format_struct, in_dests, destination_id, id);
+		in_dests += MLX5_ST_SZ_BYTES(dest_format_struct);
+	}
+	memset(out, 0, sizeof(out));
+	err = mlx5_cmd_exec_check_status(dev, in, inlen, out,
+					 sizeof(out));
+	kvfree(in);
+
+	return err;
+}
+
+int mlx5_cmd_create_fte(struct mlx5_core_dev *dev,
+			struct mlx5_flow_table *ft,
+			unsigned group_id,
+			struct fs_fte *fte)
+{
+	return	mlx5_cmd_set_fte(dev, 0, 0, ft, group_id, fte);
+}
+
+int mlx5_cmd_update_fte(struct mlx5_core_dev *dev,
+			struct mlx5_flow_table *ft,
+			unsigned group_id,
+			struct fs_fte *fte)
+{
+	int opmod;
+	int modify_mask;
+	int atomic_mod_cap = MLX5_CAP_FLOWTABLE(dev,
+						flow_table_properties_nic_receive.
+						flow_modify_en);
+	if (!atomic_mod_cap)
+		return -ENOTSUPP;
+	opmod = 1;
+	modify_mask = 1 <<
+		MLX5_SET_FTE_MODIFY_ENABLE_MASK_DESTINATION_LIST;
+
+	return	mlx5_cmd_set_fte(dev, opmod, modify_mask, ft, group_id, fte);
+}
+
+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));
+
+	MLX5_SET(delete_fte_in, in, opcode, MLX5_CMD_OP_DELETE_FLOW_TABLE_ENTRY);
+	MLX5_SET(delete_fte_in, in, table_type, ft->type);
+	MLX5_SET(delete_fte_in, in, table_id, ft->id);
+	MLX5_SET(delete_fte_in, in, flow_index, index);
+
+	err =  mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
+
+	return err;
+}
diff --git a/drivers/staging/rdma/amso1100/c2_user.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
index 7e9e7ad65467..9814d4784803 100644
--- a/drivers/staging/rdma/amso1100/c2_user.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_cmd.h
@@ -1,7 +1,5 @@
 /*
- * 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.
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
  *
  * This software is available to you under a choice of one of two
  * licenses.  You may choose to be licensed under the terms of the GNU
@@ -30,53 +28,45 @@
  * ACTION 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 C2_USER_H
-#define C2_USER_H
+#ifndef _MLX5_FS_CMD_
+#define _MLX5_FS_CMD_
 
-#include <linux/types.h>
+int mlx5_cmd_create_flow_table(struct mlx5_core_dev *dev,
+			       enum fs_flow_table_type type, unsigned int level,
+			       unsigned int log_size, struct mlx5_flow_table
+			       *next_ft, unsigned int *table_id);
 
-/*
- * 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.
- */
+int mlx5_cmd_destroy_flow_table(struct mlx5_core_dev *dev,
+				struct mlx5_flow_table *ft);
+
+int mlx5_cmd_modify_flow_table(struct mlx5_core_dev *dev,
+			       struct mlx5_flow_table *ft,
+			       struct mlx5_flow_table *next_ft);
 
-struct c2_alloc_ucontext_resp {
-	__u32 qp_tab_size;
-	__u32 uarc_size;
-};
+int mlx5_cmd_create_flow_group(struct mlx5_core_dev *dev,
+			       struct mlx5_flow_table *ft,
+			       u32 *in, unsigned int *group_id);
 
-struct c2_alloc_pd_resp {
-	__u32 pdn;
-	__u32 reserved;
-};
+int mlx5_cmd_destroy_flow_group(struct mlx5_core_dev *dev,
+				struct mlx5_flow_table *ft,
+				unsigned int group_id);
 
-struct c2_create_cq {
-	__u32 lkey;
-	__u32 pdn;
-	__u64 arm_db_page;
-	__u64 set_db_page;
-	__u32 arm_db_index;
-	__u32 set_db_index;
-};
+int mlx5_cmd_create_fte(struct mlx5_core_dev *dev,
+			struct mlx5_flow_table *ft,
+			unsigned group_id,
+			struct fs_fte *fte);
 
-struct c2_create_cq_resp {
-	__u32 cqn;
-	__u32 reserved;
-};
+int mlx5_cmd_update_fte(struct mlx5_core_dev *dev,
+			struct mlx5_flow_table *ft,
+			unsigned group_id,
+			struct fs_fte *fte);
 
-struct c2_create_qp {
-	__u32 lkey;
-	__u32 reserved;
-	__u64 sq_db_page;
-	__u64 rq_db_page;
-	__u32 sq_db_index;
-	__u32 rq_db_index;
-};
+int mlx5_cmd_delete_fte(struct mlx5_core_dev *dev,
+			struct mlx5_flow_table *ft,
+			unsigned int index);
 
-#endif				/* C2_USER_H */
+int mlx5_cmd_update_root_ft(struct mlx5_core_dev *dev,
+			    struct mlx5_flow_table *ft);
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
new file mode 100644
index 000000000000..6f68dba8d7ed
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.c
@@ -0,0 +1,1514 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/mutex.h>
+#include <linux/mlx5/driver.h>
+
+#include "mlx5_core.h"
+#include "fs_core.h"
+#include "fs_cmd.h"
+
+#define INIT_TREE_NODE_ARRAY_SIZE(...)	(sizeof((struct init_tree_node[]){__VA_ARGS__}) /\
+					 sizeof(struct init_tree_node))
+
+#define ADD_PRIO(num_prios_val, min_level_val, max_ft_val, caps_val,\
+		 ...) {.type = FS_TYPE_PRIO,\
+	.min_ft_level = min_level_val,\
+	.max_ft = max_ft_val,\
+	.num_leaf_prios = num_prios_val,\
+	.caps = caps_val,\
+	.children = (struct init_tree_node[]) {__VA_ARGS__},\
+	.ar_size = INIT_TREE_NODE_ARRAY_SIZE(__VA_ARGS__) \
+}
+
+#define ADD_MULTIPLE_PRIO(num_prios_val, max_ft_val, ...)\
+	ADD_PRIO(num_prios_val, 0, max_ft_val, {},\
+		 __VA_ARGS__)\
+
+#define ADD_NS(...) {.type = FS_TYPE_NAMESPACE,\
+	.children = (struct init_tree_node[]) {__VA_ARGS__},\
+	.ar_size = INIT_TREE_NODE_ARRAY_SIZE(__VA_ARGS__) \
+}
+
+#define INIT_CAPS_ARRAY_SIZE(...) (sizeof((long[]){__VA_ARGS__}) /\
+				   sizeof(long))
+
+#define FS_CAP(cap) (__mlx5_bit_off(flow_table_nic_cap, cap))
+
+#define FS_REQUIRED_CAPS(...) {.arr_sz = INIT_CAPS_ARRAY_SIZE(__VA_ARGS__), \
+			       .caps = (long[]) {__VA_ARGS__} }
+
+#define LEFTOVERS_MAX_FT 1
+#define LEFTOVERS_NUM_PRIOS 1
+#define BY_PASS_PRIO_MAX_FT 1
+#define BY_PASS_MIN_LEVEL (KENREL_MIN_LEVEL + MLX5_BY_PASS_NUM_PRIOS +\
+			   LEFTOVERS_MAX_FT)
+
+#define KERNEL_MAX_FT 2
+#define KERNEL_NUM_PRIOS 1
+#define KENREL_MIN_LEVEL 2
+
+struct node_caps {
+	size_t	arr_sz;
+	long	*caps;
+};
+static struct init_tree_node {
+	enum fs_node_type	type;
+	struct init_tree_node *children;
+	int ar_size;
+	struct node_caps caps;
+	int min_ft_level;
+	int num_leaf_prios;
+	int prio;
+	int max_ft;
+} root_fs = {
+	.type = FS_TYPE_NAMESPACE,
+	.ar_size = 3,
+	.children = (struct init_tree_node[]) {
+		ADD_PRIO(0, BY_PASS_MIN_LEVEL, 0,
+			 FS_REQUIRED_CAPS(FS_CAP(flow_table_properties_nic_receive.flow_modify_en),
+					  FS_CAP(flow_table_properties_nic_receive.modify_root),
+					  FS_CAP(flow_table_properties_nic_receive.identified_miss_table_mode),
+					  FS_CAP(flow_table_properties_nic_receive.flow_table_modify)),
+			 ADD_NS(ADD_MULTIPLE_PRIO(MLX5_BY_PASS_NUM_PRIOS, BY_PASS_PRIO_MAX_FT))),
+		ADD_PRIO(0, KENREL_MIN_LEVEL, 0, {},
+			 ADD_NS(ADD_MULTIPLE_PRIO(KERNEL_NUM_PRIOS, KERNEL_MAX_FT))),
+		ADD_PRIO(0, BY_PASS_MIN_LEVEL, 0,
+			 FS_REQUIRED_CAPS(FS_CAP(flow_table_properties_nic_receive.flow_modify_en),
+					  FS_CAP(flow_table_properties_nic_receive.modify_root),
+					  FS_CAP(flow_table_properties_nic_receive.identified_miss_table_mode),
+					  FS_CAP(flow_table_properties_nic_receive.flow_table_modify)),
+			 ADD_NS(ADD_MULTIPLE_PRIO(LEFTOVERS_NUM_PRIOS, LEFTOVERS_MAX_FT))),
+	}
+};
+
+enum fs_i_mutex_lock_class {
+	FS_MUTEX_GRANDPARENT,
+	FS_MUTEX_PARENT,
+	FS_MUTEX_CHILD
+};
+
+static void del_rule(struct fs_node *node);
+static void del_flow_table(struct fs_node *node);
+static void del_flow_group(struct fs_node *node);
+static void del_fte(struct fs_node *node);
+
+static void tree_init_node(struct fs_node *node,
+			   unsigned int refcount,
+			   void (*remove_func)(struct fs_node *))
+{
+	atomic_set(&node->refcount, refcount);
+	INIT_LIST_HEAD(&node->list);
+	INIT_LIST_HEAD(&node->children);
+	mutex_init(&node->lock);
+	node->remove_func = remove_func;
+}
+
+static void tree_add_node(struct fs_node *node, struct fs_node *parent)
+{
+	if (parent)
+		atomic_inc(&parent->refcount);
+	node->parent = parent;
+
+	/* Parent is the root */
+	if (!parent)
+		node->root = node;
+	else
+		node->root = parent->root;
+}
+
+static void tree_get_node(struct fs_node *node)
+{
+	atomic_inc(&node->refcount);
+}
+
+static void nested_lock_ref_node(struct fs_node *node,
+				 enum fs_i_mutex_lock_class class)
+{
+	if (node) {
+		mutex_lock_nested(&node->lock, class);
+		atomic_inc(&node->refcount);
+	}
+}
+
+static void lock_ref_node(struct fs_node *node)
+{
+	if (node) {
+		mutex_lock(&node->lock);
+		atomic_inc(&node->refcount);
+	}
+}
+
+static void unlock_ref_node(struct fs_node *node)
+{
+	if (node) {
+		atomic_dec(&node->refcount);
+		mutex_unlock(&node->lock);
+	}
+}
+
+static void tree_put_node(struct fs_node *node)
+{
+	struct fs_node *parent_node = node->parent;
+
+	lock_ref_node(parent_node);
+	if (atomic_dec_and_test(&node->refcount)) {
+		if (parent_node)
+			list_del_init(&node->list);
+		if (node->remove_func)
+			node->remove_func(node);
+		kfree(node);
+		node = NULL;
+	}
+	unlock_ref_node(parent_node);
+	if (!node && parent_node)
+		tree_put_node(parent_node);
+}
+
+static int tree_remove_node(struct fs_node *node)
+{
+	if (atomic_read(&node->refcount) > 1)
+		return -EPERM;
+	tree_put_node(node);
+	return 0;
+}
+
+static struct fs_prio *find_prio(struct mlx5_flow_namespace *ns,
+				 unsigned int prio)
+{
+	struct fs_prio *iter_prio;
+
+	fs_for_each_prio(iter_prio, ns) {
+		if (iter_prio->prio == prio)
+			return iter_prio;
+	}
+
+	return NULL;
+}
+
+static unsigned int find_next_free_level(struct fs_prio *prio)
+{
+	if (!list_empty(&prio->node.children)) {
+		struct mlx5_flow_table *ft;
+
+		ft = list_last_entry(&prio->node.children,
+				     struct mlx5_flow_table,
+				     node.list);
+		return ft->level + 1;
+	}
+	return prio->start_level;
+}
+
+static bool masked_memcmp(void *mask, void *val1, void *val2, size_t size)
+{
+	unsigned int i;
+
+	for (i = 0; i < size; i++, mask++, val1++, val2++)
+		if ((*((u8 *)val1) & (*(u8 *)mask)) !=
+		    ((*(u8 *)val2) & (*(u8 *)mask)))
+			return false;
+
+	return true;
+}
+
+static bool compare_match_value(struct mlx5_flow_group_mask *mask,
+				void *fte_param1, void *fte_param2)
+{
+	if (mask->match_criteria_enable &
+	    1 << MLX5_CREATE_FLOW_GROUP_IN_MATCH_CRITERIA_ENABLE_OUTER_HEADERS) {
+		void *fte_match1 = MLX5_ADDR_OF(fte_match_param,
+						fte_param1, outer_headers);
+		void *fte_match2 = MLX5_ADDR_OF(fte_match_param,
+						fte_param2, outer_headers);
+		void *fte_mask = MLX5_ADDR_OF(fte_match_param,
+					      mask->match_criteria, outer_headers);
+
+		if (!masked_memcmp(fte_mask, fte_match1, fte_match2,
+				   MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4)))
+			return false;
+	}
+
+	if (mask->match_criteria_enable &
+	    1 << MLX5_CREATE_FLOW_GROUP_IN_MATCH_CRITERIA_ENABLE_MISC_PARAMETERS) {
+		void *fte_match1 = MLX5_ADDR_OF(fte_match_param,
+						fte_param1, misc_parameters);
+		void *fte_match2 = MLX5_ADDR_OF(fte_match_param,
+						fte_param2, misc_parameters);
+		void *fte_mask = MLX5_ADDR_OF(fte_match_param,
+					  mask->match_criteria, misc_parameters);
+
+		if (!masked_memcmp(fte_mask, fte_match1, fte_match2,
+				   MLX5_ST_SZ_BYTES(fte_match_set_misc)))
+			return false;
+	}
+
+	if (mask->match_criteria_enable &
+	    1 << MLX5_CREATE_FLOW_GROUP_IN_MATCH_CRITERIA_ENABLE_INNER_HEADERS) {
+		void *fte_match1 = MLX5_ADDR_OF(fte_match_param,
+						fte_param1, inner_headers);
+		void *fte_match2 = MLX5_ADDR_OF(fte_match_param,
+						fte_param2, inner_headers);
+		void *fte_mask = MLX5_ADDR_OF(fte_match_param,
+					  mask->match_criteria, inner_headers);
+
+		if (!masked_memcmp(fte_mask, fte_match1, fte_match2,
+				   MLX5_ST_SZ_BYTES(fte_match_set_lyr_2_4)))
+			return false;
+	}
+	return true;
+}
+
+static bool compare_match_criteria(u8 match_criteria_enable1,
+				   u8 match_criteria_enable2,
+				   void *mask1, void *mask2)
+{
+	return match_criteria_enable1 == match_criteria_enable2 &&
+		!memcmp(mask1, mask2, MLX5_ST_SZ_BYTES(fte_match_param));
+}
+
+static struct mlx5_flow_root_namespace *find_root(struct fs_node *node)
+{
+	struct fs_node *root;
+	struct mlx5_flow_namespace *ns;
+
+	root = node->root;
+
+	if (WARN_ON(root->type != FS_TYPE_NAMESPACE)) {
+		pr_warn("mlx5: flow steering node is not in tree or garbaged\n");
+		return NULL;
+	}
+
+	ns = container_of(root, struct mlx5_flow_namespace, node);
+	return container_of(ns, struct mlx5_flow_root_namespace, ns);
+}
+
+static inline struct mlx5_core_dev *get_dev(struct fs_node *node)
+{
+	struct mlx5_flow_root_namespace *root = find_root(node);
+
+	if (root)
+		return root->dev;
+	return NULL;
+}
+
+static void del_flow_table(struct fs_node *node)
+{
+	struct mlx5_flow_table *ft;
+	struct mlx5_core_dev *dev;
+	struct fs_prio *prio;
+	int err;
+
+	fs_get_obj(ft, node);
+	dev = get_dev(&ft->node);
+
+	err = mlx5_cmd_destroy_flow_table(dev, ft);
+	if (err)
+		pr_warn("flow steering can't destroy ft\n");
+	fs_get_obj(prio, ft->node.parent);
+	prio->num_ft--;
+}
+
+static void del_rule(struct fs_node *node)
+{
+	struct mlx5_flow_rule *rule;
+	struct mlx5_flow_table *ft;
+	struct mlx5_flow_group *fg;
+	struct fs_fte *fte;
+	u32	*match_value;
+	struct mlx5_core_dev *dev = get_dev(node);
+	int match_len = MLX5_ST_SZ_BYTES(fte_match_param);
+	int err;
+
+	match_value = mlx5_vzalloc(match_len);
+	if (!match_value) {
+		pr_warn("failed to allocate inbox\n");
+		return;
+	}
+
+	fs_get_obj(rule, node);
+	fs_get_obj(fte, rule->node.parent);
+	fs_get_obj(fg, fte->node.parent);
+	memcpy(match_value, fte->val, sizeof(fte->val));
+	fs_get_obj(ft, fg->node.parent);
+	list_del(&rule->node.list);
+	fte->dests_size--;
+	if (fte->dests_size) {
+		err = mlx5_cmd_update_fte(dev, ft,
+					  fg->id, fte);
+		if (err)
+			pr_warn("%s can't del rule fg id=%d fte_index=%d\n",
+				__func__, fg->id, fte->index);
+	}
+	kvfree(match_value);
+}
+
+static void del_fte(struct fs_node *node)
+{
+	struct mlx5_flow_table *ft;
+	struct mlx5_flow_group *fg;
+	struct mlx5_core_dev *dev;
+	struct fs_fte *fte;
+	int err;
+
+	fs_get_obj(fte, node);
+	fs_get_obj(fg, fte->node.parent);
+	fs_get_obj(ft, fg->node.parent);
+
+	dev = get_dev(&ft->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);
+
+	fte->status = 0;
+	fg->num_ftes--;
+}
+
+static void del_flow_group(struct fs_node *node)
+{
+	struct mlx5_flow_group *fg;
+	struct mlx5_flow_table *ft;
+	struct mlx5_core_dev *dev;
+
+	fs_get_obj(fg, node);
+	fs_get_obj(ft, fg->node.parent);
+	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);
+}
+
+static struct fs_fte *alloc_fte(u8 action,
+				u32 flow_tag,
+				u32 *match_value,
+				unsigned int index)
+{
+	struct fs_fte *fte;
+
+	fte = kzalloc(sizeof(*fte), GFP_KERNEL);
+	if (!fte)
+		return ERR_PTR(-ENOMEM);
+
+	memcpy(fte->val, match_value, sizeof(fte->val));
+	fte->node.type =  FS_TYPE_FLOW_ENTRY;
+	fte->flow_tag = flow_tag;
+	fte->index = index;
+	fte->action = action;
+
+	return fte;
+}
+
+static struct mlx5_flow_group *alloc_flow_group(u32 *create_fg_in)
+{
+	struct mlx5_flow_group *fg;
+	void *match_criteria = MLX5_ADDR_OF(create_flow_group_in,
+					    create_fg_in, match_criteria);
+	u8 match_criteria_enable = MLX5_GET(create_flow_group_in,
+					    create_fg_in,
+					    match_criteria_enable);
+	fg = kzalloc(sizeof(*fg), GFP_KERNEL);
+	if (!fg)
+		return ERR_PTR(-ENOMEM);
+
+	fg->mask.match_criteria_enable = match_criteria_enable;
+	memcpy(&fg->mask.match_criteria, match_criteria,
+	       sizeof(fg->mask.match_criteria));
+	fg->node.type =  FS_TYPE_FLOW_GROUP;
+	fg->start_index = MLX5_GET(create_flow_group_in, create_fg_in,
+				   start_flow_index);
+	fg->max_ftes = MLX5_GET(create_flow_group_in, create_fg_in,
+				end_flow_index) - fg->start_index + 1;
+	return fg;
+}
+
+static struct mlx5_flow_table *alloc_flow_table(int level, int max_fte,
+						enum fs_flow_table_type table_type)
+{
+	struct mlx5_flow_table *ft;
+
+	ft  = kzalloc(sizeof(*ft), GFP_KERNEL);
+	if (!ft)
+		return NULL;
+
+	ft->level = level;
+	ft->node.type = FS_TYPE_FLOW_TABLE;
+	ft->type = table_type;
+	ft->max_fte = max_fte;
+
+	return ft;
+}
+
+/* If reverse is false, then we search for the first flow table in the
+ * root sub-tree from start(closest from right), else we search for the
+ * last flow table in the root sub-tree till start(closest from left).
+ */
+static struct mlx5_flow_table *find_closest_ft_recursive(struct fs_node  *root,
+							 struct list_head *start,
+							 bool reverse)
+{
+#define list_advance_entry(pos, reverse)		\
+	((reverse) ? list_prev_entry(pos, list) : list_next_entry(pos, list))
+
+#define list_for_each_advance_continue(pos, head, reverse)	\
+	for (pos = list_advance_entry(pos, reverse);		\
+	     &pos->list != (head);				\
+	     pos = list_advance_entry(pos, reverse))
+
+	struct fs_node *iter = list_entry(start, struct fs_node, list);
+	struct mlx5_flow_table *ft = NULL;
+
+	if (!root)
+		return NULL;
+
+	list_for_each_advance_continue(iter, &root->children, reverse) {
+		if (iter->type == FS_TYPE_FLOW_TABLE) {
+			fs_get_obj(ft, iter);
+			return ft;
+		}
+		ft = find_closest_ft_recursive(iter, &iter->children, reverse);
+		if (ft)
+			return ft;
+	}
+
+	return ft;
+}
+
+/* If reverse if false then return the first flow table in next priority of
+ * prio in the tree, else return the last flow table in the previous priority
+ * of prio in the tree.
+ */
+static struct mlx5_flow_table *find_closest_ft(struct fs_prio *prio, bool reverse)
+{
+	struct mlx5_flow_table *ft = NULL;
+	struct fs_node *curr_node;
+	struct fs_node *parent;
+
+	parent = prio->node.parent;
+	curr_node = &prio->node;
+	while (!ft && parent) {
+		ft = find_closest_ft_recursive(parent, &curr_node->list, reverse);
+		curr_node = parent;
+		parent = curr_node->parent;
+	}
+	return ft;
+}
+
+/* Assuming all the tree is locked by mutex chain lock */
+static struct mlx5_flow_table *find_next_chained_ft(struct fs_prio *prio)
+{
+	return find_closest_ft(prio, false);
+}
+
+/* Assuming all the tree is locked by mutex chain lock */
+static struct mlx5_flow_table *find_prev_chained_ft(struct fs_prio *prio)
+{
+	return find_closest_ft(prio, true);
+}
+
+static int connect_fts_in_prio(struct mlx5_core_dev *dev,
+			       struct fs_prio *prio,
+			       struct mlx5_flow_table *ft)
+{
+	struct mlx5_flow_table *iter;
+	int i = 0;
+	int err;
+
+	fs_for_each_ft(iter, prio) {
+		i++;
+		err = mlx5_cmd_modify_flow_table(dev,
+						 iter,
+						 ft);
+		if (err) {
+			mlx5_core_warn(dev, "Failed to modify flow table %d\n",
+				       iter->id);
+			/* The driver is out of sync with the FW */
+			if (i > 1)
+				WARN_ON(true);
+			return err;
+		}
+	}
+	return 0;
+}
+
+/* Connect flow tables from previous priority of prio to ft */
+static int connect_prev_fts(struct mlx5_core_dev *dev,
+			    struct mlx5_flow_table *ft,
+			    struct fs_prio *prio)
+{
+	struct mlx5_flow_table *prev_ft;
+
+	prev_ft = find_prev_chained_ft(prio);
+	if (prev_ft) {
+		struct fs_prio *prev_prio;
+
+		fs_get_obj(prev_prio, prev_ft->node.parent);
+		return connect_fts_in_prio(dev, prev_prio, ft);
+	}
+	return 0;
+}
+
+static int update_root_ft_create(struct mlx5_flow_table *ft, struct fs_prio
+				 *prio)
+{
+	struct mlx5_flow_root_namespace *root = find_root(&prio->node);
+	int min_level = INT_MAX;
+	int err;
+
+	if (root->root_ft)
+		min_level = root->root_ft->level;
+
+	if (ft->level >= min_level)
+		return 0;
+
+	err = mlx5_cmd_update_root_ft(root->dev, ft);
+	if (err)
+		mlx5_core_warn(root->dev, "Update root flow table of id=%u failed\n",
+			       ft->id);
+	else
+		root->root_ft = ft;
+
+	return err;
+}
+
+static int connect_flow_table(struct mlx5_core_dev *dev, struct mlx5_flow_table *ft,
+			      struct fs_prio *prio)
+{
+	int err = 0;
+
+	/* Connect_prev_fts and update_root_ft_create are mutually exclusive */
+
+	if (list_empty(&prio->node.children)) {
+		err = connect_prev_fts(dev, ft, prio);
+		if (err)
+			return err;
+	}
+
+	if (MLX5_CAP_FLOWTABLE(dev,
+			       flow_table_properties_nic_receive.modify_root))
+		err = update_root_ft_create(ft, prio);
+	return err;
+}
+
+struct mlx5_flow_table *mlx5_create_flow_table(struct mlx5_flow_namespace *ns,
+					       int prio,
+					       int max_fte)
+{
+	struct mlx5_flow_table *next_ft = NULL;
+	struct mlx5_flow_table *ft;
+	int err;
+	int log_table_sz;
+	struct mlx5_flow_root_namespace *root =
+		find_root(&ns->node);
+	struct fs_prio *fs_prio = NULL;
+
+	if (!root) {
+		pr_err("mlx5: flow steering failed to find root of namespace\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	mutex_lock(&root->chain_lock);
+	fs_prio = find_prio(ns, prio);
+	if (!fs_prio) {
+		err = -EINVAL;
+		goto unlock_root;
+	}
+	if (fs_prio->num_ft == fs_prio->max_ft) {
+		err = -ENOSPC;
+		goto unlock_root;
+	}
+
+	ft = alloc_flow_table(find_next_free_level(fs_prio),
+			      roundup_pow_of_two(max_fte),
+			      root->table_type);
+	if (!ft) {
+		err = -ENOMEM;
+		goto unlock_root;
+	}
+
+	tree_init_node(&ft->node, 1, del_flow_table);
+	log_table_sz = ilog2(ft->max_fte);
+	next_ft = find_next_chained_ft(fs_prio);
+	err = mlx5_cmd_create_flow_table(root->dev, ft->type, ft->level,
+					 log_table_sz, next_ft, &ft->id);
+	if (err)
+		goto free_ft;
+
+	err = connect_flow_table(root->dev, ft, fs_prio);
+	if (err)
+		goto destroy_ft;
+	lock_ref_node(&fs_prio->node);
+	tree_add_node(&ft->node, &fs_prio->node);
+	list_add_tail(&ft->node.list, &fs_prio->node.children);
+	fs_prio->num_ft++;
+	unlock_ref_node(&fs_prio->node);
+	mutex_unlock(&root->chain_lock);
+	return ft;
+destroy_ft:
+	mlx5_cmd_destroy_flow_table(root->dev, ft);
+free_ft:
+	kfree(ft);
+unlock_root:
+	mutex_unlock(&root->chain_lock);
+	return ERR_PTR(err);
+}
+
+struct mlx5_flow_table *mlx5_create_auto_grouped_flow_table(struct mlx5_flow_namespace *ns,
+							    int prio,
+							    int num_flow_table_entries,
+							    int max_num_groups)
+{
+	struct mlx5_flow_table *ft;
+
+	if (max_num_groups > num_flow_table_entries)
+		return ERR_PTR(-EINVAL);
+
+	ft = mlx5_create_flow_table(ns, prio, num_flow_table_entries);
+	if (IS_ERR(ft))
+		return ft;
+
+	ft->autogroup.active = true;
+	ft->autogroup.required_groups = max_num_groups;
+
+	return ft;
+}
+EXPORT_SYMBOL(mlx5_create_auto_grouped_flow_table);
+
+/* Flow table should be locked */
+static struct mlx5_flow_group *create_flow_group_common(struct mlx5_flow_table *ft,
+							u32 *fg_in,
+							struct list_head
+							*prev_fg,
+							bool is_auto_fg)
+{
+	struct mlx5_flow_group *fg;
+	struct mlx5_core_dev *dev = get_dev(&ft->node);
+	int err;
+
+	if (!dev)
+		return ERR_PTR(-ENODEV);
+
+	fg = alloc_flow_group(fg_in);
+	if (IS_ERR(fg))
+		return fg;
+
+	err = mlx5_cmd_create_flow_group(dev, ft, fg_in, &fg->id);
+	if (err) {
+		kfree(fg);
+		return ERR_PTR(err);
+	}
+
+	if (ft->autogroup.active)
+		ft->autogroup.num_groups++;
+	/* Add node to tree */
+	tree_init_node(&fg->node, !is_auto_fg, del_flow_group);
+	tree_add_node(&fg->node, &ft->node);
+	/* Add node to group list */
+	list_add(&fg->node.list, ft->node.children.prev);
+
+	return fg;
+}
+
+struct mlx5_flow_group *mlx5_create_flow_group(struct mlx5_flow_table *ft,
+					       u32 *fg_in)
+{
+	struct mlx5_flow_group *fg;
+
+	if (ft->autogroup.active)
+		return ERR_PTR(-EPERM);
+
+	lock_ref_node(&ft->node);
+	fg = create_flow_group_common(ft, fg_in, &ft->node.children, false);
+	unlock_ref_node(&ft->node);
+
+	return fg;
+}
+
+static struct mlx5_flow_rule *alloc_rule(struct mlx5_flow_destination *dest)
+{
+	struct mlx5_flow_rule *rule;
+
+	rule = kzalloc(sizeof(*rule), GFP_KERNEL);
+	if (!rule)
+		return NULL;
+
+	rule->node.type = FS_TYPE_FLOW_DEST;
+	memcpy(&rule->dest_attr, dest, sizeof(*dest));
+
+	return rule;
+}
+
+/* fte should not be deleted while calling this function */
+static struct mlx5_flow_rule *add_rule_fte(struct fs_fte *fte,
+					   struct mlx5_flow_group *fg,
+					   struct mlx5_flow_destination *dest)
+{
+	struct mlx5_flow_table *ft;
+	struct mlx5_flow_rule *rule;
+	int err;
+
+	rule = alloc_rule(dest);
+	if (!rule)
+		return ERR_PTR(-ENOMEM);
+
+	fs_get_obj(ft, fg->node.parent);
+	/* Add dest to dests list- added as first element after the head */
+	tree_init_node(&rule->node, 1, del_rule);
+	list_add_tail(&rule->node.list, &fte->node.children);
+	fte->dests_size++;
+	if (fte->dests_size == 1)
+		err = mlx5_cmd_create_fte(get_dev(&ft->node),
+					  ft, fg->id, fte);
+	else
+		err = mlx5_cmd_update_fte(get_dev(&ft->node),
+					  ft, fg->id, fte);
+	if (err)
+		goto free_rule;
+
+	fte->status |= FS_FTE_STATUS_EXISTING;
+
+	return rule;
+
+free_rule:
+	list_del(&rule->node.list);
+	kfree(rule);
+	fte->dests_size--;
+	return ERR_PTR(err);
+}
+
+/* Assumed fg is locked */
+static unsigned int get_free_fte_index(struct mlx5_flow_group *fg,
+				       struct list_head **prev)
+{
+	struct fs_fte *fte;
+	unsigned int start = fg->start_index;
+
+	if (prev)
+		*prev = &fg->node.children;
+
+	/* assumed list is sorted by index */
+	fs_for_each_fte(fte, fg) {
+		if (fte->index != start)
+			return start;
+		start++;
+		if (prev)
+			*prev = &fte->node.list;
+	}
+
+	return start;
+}
+
+/* prev is output, prev->next = new_fte */
+static struct fs_fte *create_fte(struct mlx5_flow_group *fg,
+				 u32 *match_value,
+				 u8 action,
+				 u32 flow_tag,
+				 struct list_head **prev)
+{
+	struct fs_fte *fte;
+	int index;
+
+	index = get_free_fte_index(fg, prev);
+	fte = alloc_fte(action, flow_tag, match_value, index);
+	if (IS_ERR(fte))
+		return fte;
+
+	return fte;
+}
+
+static struct mlx5_flow_group *create_autogroup(struct mlx5_flow_table *ft,
+						u8 match_criteria_enable,
+						u32 *match_criteria)
+{
+	int inlen = MLX5_ST_SZ_BYTES(create_flow_group_in);
+	struct list_head *prev = &ft->node.children;
+	unsigned int candidate_index = 0;
+	struct mlx5_flow_group *fg;
+	void *match_criteria_addr;
+	unsigned int group_size = 0;
+	u32 *in;
+
+	if (!ft->autogroup.active)
+		return ERR_PTR(-ENOENT);
+
+	in = mlx5_vzalloc(inlen);
+	if (!in)
+		return ERR_PTR(-ENOMEM);
+
+	if (ft->autogroup.num_groups < ft->autogroup.required_groups)
+		/* We save place for flow groups in addition to max types */
+		group_size = ft->max_fte / (ft->autogroup.required_groups + 1);
+
+	/*  ft->max_fte == ft->autogroup.max_types */
+	if (group_size == 0)
+		group_size = 1;
+
+	/* sorted by start_index */
+	fs_for_each_fg(fg, ft) {
+		if (candidate_index + group_size > fg->start_index)
+			candidate_index = fg->start_index + fg->max_ftes;
+		else
+			break;
+		prev = &fg->node.list;
+	}
+
+	if (candidate_index + group_size > ft->max_fte) {
+		fg = ERR_PTR(-ENOSPC);
+		goto out;
+	}
+
+	MLX5_SET(create_flow_group_in, in, match_criteria_enable,
+		 match_criteria_enable);
+	MLX5_SET(create_flow_group_in, in, start_flow_index, candidate_index);
+	MLX5_SET(create_flow_group_in, in, end_flow_index,   candidate_index +
+		 group_size - 1);
+	match_criteria_addr = MLX5_ADDR_OF(create_flow_group_in,
+					   in, match_criteria);
+	memcpy(match_criteria_addr, match_criteria,
+	       MLX5_ST_SZ_BYTES(fte_match_param));
+
+	fg = create_flow_group_common(ft, in, prev, true);
+out:
+	kvfree(in);
+	return fg;
+}
+
+static struct mlx5_flow_rule *add_rule_fg(struct mlx5_flow_group *fg,
+					  u32 *match_value,
+					  u8 action,
+					  u32 flow_tag,
+					  struct mlx5_flow_destination *dest)
+{
+	struct fs_fte *fte;
+	struct mlx5_flow_rule *rule;
+	struct mlx5_flow_table *ft;
+	struct list_head *prev;
+
+	nested_lock_ref_node(&fg->node, FS_MUTEX_PARENT);
+	fs_for_each_fte(fte, fg) {
+		nested_lock_ref_node(&fte->node, FS_MUTEX_CHILD);
+		if (compare_match_value(&fg->mask, match_value, &fte->val) &&
+		    action == fte->action && flow_tag == fte->flow_tag) {
+			rule = add_rule_fte(fte, fg, dest);
+			unlock_ref_node(&fte->node);
+			if (IS_ERR(rule))
+				goto unlock_fg;
+			else
+				goto add_rule;
+		}
+		unlock_ref_node(&fte->node);
+	}
+	fs_get_obj(ft, fg->node.parent);
+	if (fg->num_ftes >= fg->max_ftes) {
+		rule = ERR_PTR(-ENOSPC);
+		goto unlock_fg;
+	}
+
+	fte = create_fte(fg, match_value, action, flow_tag, &prev);
+	if (IS_ERR(fte)) {
+		rule = (void *)fte;
+		goto unlock_fg;
+	}
+	tree_init_node(&fte->node, 0, del_fte);
+	rule = add_rule_fte(fte, fg, dest);
+	if (IS_ERR(rule)) {
+		kfree(fte);
+		goto unlock_fg;
+	}
+
+	fg->num_ftes++;
+
+	tree_add_node(&fte->node, &fg->node);
+	list_add(&fte->node.list, prev);
+add_rule:
+	tree_add_node(&rule->node, &fte->node);
+unlock_fg:
+	unlock_ref_node(&fg->node);
+	return rule;
+}
+
+static struct mlx5_flow_rule *add_rule_to_auto_fg(struct mlx5_flow_table *ft,
+						  u8 match_criteria_enable,
+						  u32 *match_criteria,
+						  u32 *match_value,
+						  u8 action,
+						  u32 flow_tag,
+						  struct mlx5_flow_destination *dest)
+{
+	struct mlx5_flow_rule *rule;
+	struct mlx5_flow_group *g;
+
+	g = create_autogroup(ft, match_criteria_enable, match_criteria);
+	if (IS_ERR(g))
+		return (void *)g;
+
+	rule = add_rule_fg(g, match_value,
+			   action, flow_tag, dest);
+	if (IS_ERR(rule)) {
+		/* Remove assumes refcount > 0 and autogroup creates a group
+		 * with a refcount = 0.
+		 */
+		tree_get_node(&g->node);
+		tree_remove_node(&g->node);
+	}
+	return rule;
+}
+
+struct mlx5_flow_rule *
+mlx5_add_flow_rule(struct mlx5_flow_table *ft,
+		   u8 match_criteria_enable,
+		   u32 *match_criteria,
+		   u32 *match_value,
+		   u32 action,
+		   u32 flow_tag,
+		   struct mlx5_flow_destination *dest)
+{
+	struct mlx5_flow_group *g;
+	struct mlx5_flow_rule *rule;
+
+	nested_lock_ref_node(&ft->node, FS_MUTEX_GRANDPARENT);
+	fs_for_each_fg(g, ft)
+		if (compare_match_criteria(g->mask.match_criteria_enable,
+					   match_criteria_enable,
+					   g->mask.match_criteria,
+					   match_criteria)) {
+			rule = add_rule_fg(g, match_value,
+					   action, flow_tag, dest);
+			if (!IS_ERR(rule) || PTR_ERR(rule) != -ENOSPC)
+				goto unlock;
+		}
+
+	rule = add_rule_to_auto_fg(ft, match_criteria_enable, match_criteria,
+				   match_value, action, flow_tag, dest);
+unlock:
+	unlock_ref_node(&ft->node);
+	return rule;
+}
+EXPORT_SYMBOL(mlx5_add_flow_rule);
+
+void mlx5_del_flow_rule(struct mlx5_flow_rule *rule)
+{
+	tree_remove_node(&rule->node);
+}
+EXPORT_SYMBOL(mlx5_del_flow_rule);
+
+/* Assuming prio->node.children(flow tables) is sorted by level */
+static struct mlx5_flow_table *find_next_ft(struct mlx5_flow_table *ft)
+{
+	struct fs_prio *prio;
+
+	fs_get_obj(prio, ft->node.parent);
+
+	if (!list_is_last(&ft->node.list, &prio->node.children))
+		return list_next_entry(ft, node.list);
+	return find_next_chained_ft(prio);
+}
+
+static int update_root_ft_destroy(struct mlx5_flow_table *ft)
+{
+	struct mlx5_flow_root_namespace *root = find_root(&ft->node);
+	struct mlx5_flow_table *new_root_ft = NULL;
+
+	if (root->root_ft != ft)
+		return 0;
+
+	new_root_ft = find_next_ft(ft);
+	if (new_root_ft) {
+		int err = mlx5_cmd_update_root_ft(root->dev, new_root_ft);
+
+		if (err) {
+			mlx5_core_warn(root->dev, "Update root flow table of id=%u failed\n",
+				       ft->id);
+			return err;
+		}
+		root->root_ft = new_root_ft;
+	}
+	return 0;
+}
+
+/* Connect flow table from previous priority to
+ * the next flow table.
+ */
+static int disconnect_flow_table(struct mlx5_flow_table *ft)
+{
+	struct mlx5_core_dev *dev = get_dev(&ft->node);
+	struct mlx5_flow_table *next_ft;
+	struct fs_prio *prio;
+	int err = 0;
+
+	err = update_root_ft_destroy(ft);
+	if (err)
+		return err;
+
+	fs_get_obj(prio, ft->node.parent);
+	if  (!(list_first_entry(&prio->node.children,
+				struct mlx5_flow_table,
+				node.list) == ft))
+		return 0;
+
+	next_ft = find_next_chained_ft(prio);
+	err = connect_prev_fts(dev, next_ft, prio);
+	if (err)
+		mlx5_core_warn(dev, "Failed to disconnect flow table %d\n",
+			       ft->id);
+	return err;
+}
+
+int mlx5_destroy_flow_table(struct mlx5_flow_table *ft)
+{
+	struct mlx5_flow_root_namespace *root = find_root(&ft->node);
+	int err = 0;
+
+	mutex_lock(&root->chain_lock);
+	err = disconnect_flow_table(ft);
+	if (err) {
+		mutex_unlock(&root->chain_lock);
+		return err;
+	}
+	if (tree_remove_node(&ft->node))
+		mlx5_core_warn(get_dev(&ft->node), "Flow table %d wasn't destroyed, refcount > 1\n",
+			       ft->id);
+	mutex_unlock(&root->chain_lock);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx5_destroy_flow_table);
+
+void mlx5_destroy_flow_group(struct mlx5_flow_group *fg)
+{
+	if (tree_remove_node(&fg->node))
+		mlx5_core_warn(get_dev(&fg->node), "Flow group %d wasn't destroyed, refcount > 1\n",
+			       fg->id);
+}
+
+struct mlx5_flow_namespace *mlx5_get_flow_namespace(struct mlx5_core_dev *dev,
+						    enum mlx5_flow_namespace_type type)
+{
+	struct mlx5_flow_root_namespace *root_ns = dev->priv.root_ns;
+	int prio;
+	static struct fs_prio *fs_prio;
+	struct mlx5_flow_namespace *ns;
+
+	if (!root_ns)
+		return NULL;
+
+	switch (type) {
+	case MLX5_FLOW_NAMESPACE_BYPASS:
+	case MLX5_FLOW_NAMESPACE_KERNEL:
+	case MLX5_FLOW_NAMESPACE_LEFTOVERS:
+		prio = type;
+		break;
+	case MLX5_FLOW_NAMESPACE_FDB:
+		if (dev->priv.fdb_root_ns)
+			return &dev->priv.fdb_root_ns->ns;
+		else
+			return NULL;
+	default:
+		return NULL;
+	}
+
+	fs_prio = find_prio(&root_ns->ns, prio);
+	if (!fs_prio)
+		return NULL;
+
+	ns = list_first_entry(&fs_prio->node.children,
+			      typeof(*ns),
+			      node.list);
+
+	return ns;
+}
+EXPORT_SYMBOL(mlx5_get_flow_namespace);
+
+static struct fs_prio *fs_create_prio(struct mlx5_flow_namespace *ns,
+				      unsigned prio, int max_ft)
+{
+	struct fs_prio *fs_prio;
+
+	fs_prio = kzalloc(sizeof(*fs_prio), GFP_KERNEL);
+	if (!fs_prio)
+		return ERR_PTR(-ENOMEM);
+
+	fs_prio->node.type = FS_TYPE_PRIO;
+	tree_init_node(&fs_prio->node, 1, NULL);
+	tree_add_node(&fs_prio->node, &ns->node);
+	fs_prio->max_ft = max_ft;
+	fs_prio->prio = prio;
+	list_add_tail(&fs_prio->node.list, &ns->node.children);
+
+	return fs_prio;
+}
+
+static struct mlx5_flow_namespace *fs_init_namespace(struct mlx5_flow_namespace
+						     *ns)
+{
+	ns->node.type = FS_TYPE_NAMESPACE;
+
+	return ns;
+}
+
+static struct mlx5_flow_namespace *fs_create_namespace(struct fs_prio *prio)
+{
+	struct mlx5_flow_namespace	*ns;
+
+	ns = kzalloc(sizeof(*ns), GFP_KERNEL);
+	if (!ns)
+		return ERR_PTR(-ENOMEM);
+
+	fs_init_namespace(ns);
+	tree_init_node(&ns->node, 1, NULL);
+	tree_add_node(&ns->node, &prio->node);
+	list_add_tail(&ns->node.list, &prio->node.children);
+
+	return ns;
+}
+
+static int create_leaf_prios(struct mlx5_flow_namespace *ns, struct init_tree_node
+			     *prio_metadata)
+{
+	struct fs_prio *fs_prio;
+	int i;
+
+	for (i = 0; i < prio_metadata->num_leaf_prios; i++) {
+		fs_prio = fs_create_prio(ns, i, prio_metadata->max_ft);
+		if (IS_ERR(fs_prio))
+			return PTR_ERR(fs_prio);
+	}
+	return 0;
+}
+
+#define FLOW_TABLE_BIT_SZ 1
+#define GET_FLOW_TABLE_CAP(dev, offset) \
+	((be32_to_cpu(*((__be32 *)(dev->hca_caps_cur[MLX5_CAP_FLOW_TABLE]) +	\
+			offset / 32)) >>					\
+	  (32 - FLOW_TABLE_BIT_SZ - (offset & 0x1f))) & FLOW_TABLE_BIT_SZ)
+static bool has_required_caps(struct mlx5_core_dev *dev, struct node_caps *caps)
+{
+	int i;
+
+	for (i = 0; i < caps->arr_sz; i++) {
+		if (!GET_FLOW_TABLE_CAP(dev, caps->caps[i]))
+			return false;
+	}
+	return true;
+}
+
+static int init_root_tree_recursive(struct mlx5_core_dev *dev,
+				    struct init_tree_node *init_node,
+				    struct fs_node *fs_parent_node,
+				    struct init_tree_node *init_parent_node,
+				    int index)
+{
+	int max_ft_level = MLX5_CAP_FLOWTABLE(dev,
+					      flow_table_properties_nic_receive.
+					      max_ft_level);
+	struct mlx5_flow_namespace *fs_ns;
+	struct fs_prio *fs_prio;
+	struct fs_node *base;
+	int i;
+	int err;
+
+	if (init_node->type == FS_TYPE_PRIO) {
+		if ((init_node->min_ft_level > max_ft_level) ||
+		    !has_required_caps(dev, &init_node->caps))
+			return 0;
+
+		fs_get_obj(fs_ns, fs_parent_node);
+		if (init_node->num_leaf_prios)
+			return create_leaf_prios(fs_ns, init_node);
+		fs_prio = fs_create_prio(fs_ns, index, init_node->max_ft);
+		if (IS_ERR(fs_prio))
+			return PTR_ERR(fs_prio);
+		base = &fs_prio->node;
+	} else if (init_node->type == FS_TYPE_NAMESPACE) {
+		fs_get_obj(fs_prio, fs_parent_node);
+		fs_ns = fs_create_namespace(fs_prio);
+		if (IS_ERR(fs_ns))
+			return PTR_ERR(fs_ns);
+		base = &fs_ns->node;
+	} else {
+		return -EINVAL;
+	}
+	for (i = 0; i < init_node->ar_size; i++) {
+		err = init_root_tree_recursive(dev, &init_node->children[i],
+					       base, init_node, i);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int init_root_tree(struct mlx5_core_dev *dev,
+			  struct init_tree_node *init_node,
+			  struct fs_node *fs_parent_node)
+{
+	int i;
+	struct mlx5_flow_namespace *fs_ns;
+	int err;
+
+	fs_get_obj(fs_ns, fs_parent_node);
+	for (i = 0; i < init_node->ar_size; i++) {
+		err = init_root_tree_recursive(dev, &init_node->children[i],
+					       &fs_ns->node,
+					       init_node, i);
+		if (err)
+			return err;
+	}
+	return 0;
+}
+
+static struct mlx5_flow_root_namespace *create_root_ns(struct mlx5_core_dev *dev,
+						       enum fs_flow_table_type
+						       table_type)
+{
+	struct mlx5_flow_root_namespace *root_ns;
+	struct mlx5_flow_namespace *ns;
+
+	/* Create the root namespace */
+	root_ns = mlx5_vzalloc(sizeof(*root_ns));
+	if (!root_ns)
+		return NULL;
+
+	root_ns->dev = dev;
+	root_ns->table_type = table_type;
+
+	ns = &root_ns->ns;
+	fs_init_namespace(ns);
+	mutex_init(&root_ns->chain_lock);
+	tree_init_node(&ns->node, 1, NULL);
+	tree_add_node(&ns->node, NULL);
+
+	return root_ns;
+}
+
+static void set_prio_attrs_in_prio(struct fs_prio *prio, int acc_level);
+
+static int set_prio_attrs_in_ns(struct mlx5_flow_namespace *ns, int acc_level)
+{
+	struct fs_prio *prio;
+
+	fs_for_each_prio(prio, ns) {
+		 /* This updates prio start_level and max_ft */
+		set_prio_attrs_in_prio(prio, acc_level);
+		acc_level += prio->max_ft;
+	}
+	return acc_level;
+}
+
+static void set_prio_attrs_in_prio(struct fs_prio *prio, int acc_level)
+{
+	struct mlx5_flow_namespace *ns;
+	int acc_level_ns = acc_level;
+
+	prio->start_level = acc_level;
+	fs_for_each_ns(ns, prio)
+		/* This updates start_level and max_ft of ns's priority descendants */
+		acc_level_ns = set_prio_attrs_in_ns(ns, acc_level);
+	if (!prio->max_ft)
+		prio->max_ft = acc_level_ns - prio->start_level;
+	WARN_ON(prio->max_ft < acc_level_ns - prio->start_level);
+}
+
+static void set_prio_attrs(struct mlx5_flow_root_namespace *root_ns)
+{
+	struct mlx5_flow_namespace *ns = &root_ns->ns;
+	struct fs_prio *prio;
+	int start_level = 0;
+
+	fs_for_each_prio(prio, ns) {
+		set_prio_attrs_in_prio(prio, start_level);
+		start_level += prio->max_ft;
+	}
+}
+
+static int init_root_ns(struct mlx5_core_dev *dev)
+{
+
+	dev->priv.root_ns = create_root_ns(dev, FS_FT_NIC_RX);
+	if (IS_ERR_OR_NULL(dev->priv.root_ns))
+		goto cleanup;
+
+	if (init_root_tree(dev, &root_fs, &dev->priv.root_ns->ns.node))
+		goto cleanup;
+
+	set_prio_attrs(dev->priv.root_ns);
+
+	return 0;
+
+cleanup:
+	mlx5_cleanup_fs(dev);
+	return -ENOMEM;
+}
+
+static void cleanup_single_prio_root_ns(struct mlx5_core_dev *dev,
+					struct mlx5_flow_root_namespace *root_ns)
+{
+	struct fs_node *prio;
+
+	if (!root_ns)
+		return;
+
+	if (!list_empty(&root_ns->ns.node.children)) {
+		prio = list_first_entry(&root_ns->ns.node.children,
+					struct fs_node,
+				 list);
+		if (tree_remove_node(prio))
+			mlx5_core_warn(dev,
+				       "Flow steering priority wasn't destroyed, refcount > 1\n");
+	}
+	if (tree_remove_node(&root_ns->ns.node))
+		mlx5_core_warn(dev,
+			       "Flow steering namespace wasn't destroyed, refcount > 1\n");
+	root_ns = NULL;
+}
+
+static void cleanup_root_ns(struct mlx5_core_dev *dev)
+{
+	struct mlx5_flow_root_namespace *root_ns = dev->priv.root_ns;
+	struct fs_prio *iter_prio;
+
+	if (!MLX5_CAP_GEN(dev, nic_flow_table))
+		return;
+
+	if (!root_ns)
+		return;
+
+	/* stage 1 */
+	fs_for_each_prio(iter_prio, &root_ns->ns) {
+		struct fs_node *node;
+		struct mlx5_flow_namespace *iter_ns;
+
+		fs_for_each_ns_or_ft(node, iter_prio) {
+			if (node->type == FS_TYPE_FLOW_TABLE)
+				continue;
+			fs_get_obj(iter_ns, node);
+			while (!list_empty(&iter_ns->node.children)) {
+				struct fs_prio *obj_iter_prio2;
+				struct fs_node *iter_prio2 =
+					list_first_entry(&iter_ns->node.children,
+							 struct fs_node,
+							 list);
+
+				fs_get_obj(obj_iter_prio2, iter_prio2);
+				if (tree_remove_node(iter_prio2)) {
+					mlx5_core_warn(dev,
+						       "Priority %d wasn't destroyed, refcount > 1\n",
+						       obj_iter_prio2->prio);
+					return;
+				}
+			}
+		}
+	}
+
+	/* stage 2 */
+	fs_for_each_prio(iter_prio, &root_ns->ns) {
+		while (!list_empty(&iter_prio->node.children)) {
+			struct fs_node *iter_ns =
+				list_first_entry(&iter_prio->node.children,
+						 struct fs_node,
+						 list);
+			if (tree_remove_node(iter_ns)) {
+				mlx5_core_warn(dev,
+					       "Namespace wasn't destroyed, refcount > 1\n");
+				return;
+			}
+		}
+	}
+
+	/* stage 3 */
+	while (!list_empty(&root_ns->ns.node.children)) {
+		struct fs_prio *obj_prio_node;
+		struct fs_node *prio_node =
+			list_first_entry(&root_ns->ns.node.children,
+					 struct fs_node,
+					 list);
+
+		fs_get_obj(obj_prio_node, prio_node);
+		if (tree_remove_node(prio_node)) {
+			mlx5_core_warn(dev,
+				       "Priority %d wasn't destroyed, refcount > 1\n",
+				       obj_prio_node->prio);
+			return;
+		}
+	}
+
+	if (tree_remove_node(&root_ns->ns.node)) {
+		mlx5_core_warn(dev,
+			       "root namespace wasn't destroyed, refcount > 1\n");
+		return;
+	}
+
+	dev->priv.root_ns = NULL;
+}
+
+void mlx5_cleanup_fs(struct mlx5_core_dev *dev)
+{
+	cleanup_root_ns(dev);
+	cleanup_single_prio_root_ns(dev, dev->priv.fdb_root_ns);
+}
+
+static int init_fdb_root_ns(struct mlx5_core_dev *dev)
+{
+	struct fs_prio *prio;
+
+	dev->priv.fdb_root_ns = create_root_ns(dev, FS_FT_FDB);
+	if (!dev->priv.fdb_root_ns)
+		return -ENOMEM;
+
+	/* Create single prio */
+	prio = fs_create_prio(&dev->priv.fdb_root_ns->ns, 0, 1);
+	if (IS_ERR(prio)) {
+		cleanup_single_prio_root_ns(dev, dev->priv.fdb_root_ns);
+		return PTR_ERR(prio);
+	} else {
+		return 0;
+	}
+}
+
+int mlx5_init_fs(struct mlx5_core_dev *dev)
+{
+	int err = 0;
+
+	if (MLX5_CAP_GEN(dev, nic_flow_table)) {
+		err = init_root_ns(dev);
+		if (err)
+			return err;
+	}
+	if (MLX5_CAP_GEN(dev, eswitch_flow_table)) {
+		err = init_fdb_root_ns(dev);
+		if (err)
+			cleanup_root_ns(dev);
+	}
+
+	return err;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
new file mode 100644
index 000000000000..00245fd7e4bc
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fs_core.h
@@ -0,0 +1,169 @@
+/*
+ * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
+ *
+ * This software is available to you under a choice of one of two
+ * licenses.  You may choose to be licensed under the terms of the GNU
+ * General Public License (GPL) Version 2, available from the file
+ * COPYING in the main directory of this source tree, or the
+ * OpenIB.org BSD license below:
+ *
+ *     Redistribution and use in source and binary forms, with or
+ *     without modification, are permitted provided that the following
+ *     conditions are met:
+ *
+ *      - Redistributions of source code must retain the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer.
+ *
+ *      - Redistributions in binary form must reproduce the above
+ *        copyright notice, this list of conditions and the following
+ *        disclaimer in the documentation and/or other materials
+ *        provided with the distribution.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _MLX5_FS_CORE_
+#define _MLX5_FS_CORE_
+
+#include <linux/mlx5/fs.h>
+
+enum fs_node_type {
+	FS_TYPE_NAMESPACE,
+	FS_TYPE_PRIO,
+	FS_TYPE_FLOW_TABLE,
+	FS_TYPE_FLOW_GROUP,
+	FS_TYPE_FLOW_ENTRY,
+	FS_TYPE_FLOW_DEST
+};
+
+enum fs_flow_table_type {
+	FS_FT_NIC_RX	 = 0x0,
+	FS_FT_FDB	 = 0X4,
+};
+
+enum fs_fte_status {
+	FS_FTE_STATUS_EXISTING = 1UL << 0,
+};
+
+struct fs_node {
+	struct list_head	list;
+	struct list_head	children;
+	enum fs_node_type	type;
+	struct fs_node		*parent;
+	struct fs_node		*root;
+	/* lock the node for writing and traversing */
+	struct mutex		lock;
+	atomic_t		refcount;
+	void			(*remove_func)(struct fs_node *);
+};
+
+struct mlx5_flow_rule {
+	struct fs_node				node;
+	struct mlx5_flow_destination		dest_attr;
+};
+
+/* Type of children is mlx5_flow_group */
+struct mlx5_flow_table {
+	struct fs_node			node;
+	u32				id;
+	unsigned int			max_fte;
+	unsigned int			level;
+	enum fs_flow_table_type		type;
+	struct {
+		bool			active;
+		unsigned int		required_groups;
+		unsigned int		num_groups;
+	} autogroup;
+};
+
+/* Type of children is mlx5_flow_rule */
+struct fs_fte {
+	struct fs_node			node;
+	u32				val[MLX5_ST_SZ_DW(fte_match_param)];
+	u32				dests_size;
+	u32				flow_tag;
+	u32				index;
+	u32				action;
+	enum fs_fte_status		status;
+};
+
+/* Type of children is mlx5_flow_table/namespace */
+struct fs_prio {
+	struct fs_node			node;
+	unsigned int			max_ft;
+	unsigned int			start_level;
+	unsigned int			prio;
+	unsigned int			num_ft;
+};
+
+/* Type of children is fs_prio */
+struct mlx5_flow_namespace {
+	/* parent == NULL => root ns */
+	struct	fs_node			node;
+};
+
+struct mlx5_flow_group_mask {
+	u8	match_criteria_enable;
+	u32	match_criteria[MLX5_ST_SZ_DW(fte_match_param)];
+};
+
+/* Type of children is fs_fte */
+struct mlx5_flow_group {
+	struct fs_node			node;
+	struct mlx5_flow_group_mask	mask;
+	u32				start_index;
+	u32				max_ftes;
+	u32				num_ftes;
+	u32				id;
+};
+
+struct mlx5_flow_root_namespace {
+	struct mlx5_flow_namespace	ns;
+	enum   fs_flow_table_type	table_type;
+	struct mlx5_core_dev		*dev;
+	struct mlx5_flow_table		*root_ft;
+	/* Should be held when chaining flow tables */
+	struct mutex			chain_lock;
+};
+
+int mlx5_init_fs(struct mlx5_core_dev *dev);
+void mlx5_cleanup_fs(struct mlx5_core_dev *dev);
+
+#define fs_get_obj(v, _node)  {v = container_of((_node), typeof(*v), node); }
+
+#define fs_list_for_each_entry(pos, root)		\
+	list_for_each_entry(pos, root, node.list)
+
+#define fs_for_each_ns_or_ft_reverse(pos, prio)				\
+	list_for_each_entry_reverse(pos, &(prio)->node.children, list)
+
+#define fs_for_each_ns_or_ft(pos, prio)					\
+	list_for_each_entry(pos, (&(prio)->node.children), list)
+
+#define fs_for_each_prio(pos, ns)			\
+	fs_list_for_each_entry(pos, &(ns)->node.children)
+
+#define fs_for_each_ns(pos, prio)			\
+	fs_list_for_each_entry(pos, &(prio)->node.children)
+
+#define fs_for_each_ft(pos, prio)			\
+	fs_list_for_each_entry(pos, &(prio)->node.children)
+
+#define fs_for_each_fg(pos, ft)			\
+	fs_list_for_each_entry(pos, &(ft)->node.children)
+
+#define fs_for_each_fte(pos, fg)			\
+	fs_list_for_each_entry(pos, &(fg)->node.children)
+
+#define fs_for_each_dst(pos, fte)			\
+	fs_list_for_each_entry(pos, &(fte)->node.children)
+
+#endif
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index 9335e5ae18cc..aa1ab4702385 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -160,6 +160,30 @@ int mlx5_query_hca_caps(struct mlx5_core_dev *dev)
 		if (err)
 			return err;
 	}
+
+	if (MLX5_CAP_GEN(dev, vport_group_manager) &&
+	    MLX5_CAP_GEN(dev, eswitch_flow_table)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ESWITCH_FLOW_TABLE,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ESWITCH_FLOW_TABLE,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
+
+	if (MLX5_CAP_GEN(dev, eswitch_flow_table)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ESWITCH,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ESWITCH,
+					 HCA_CAP_OPMOD_GET_MAX);
+		if (err)
+			return err;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index 4ac8d4cc4973..1545a944c309 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -49,6 +49,10 @@
 #include <linux/delay.h>
 #include <linux/mlx5/mlx5_ifc.h>
 #include "mlx5_core.h"
+#include "fs_core.h"
+#ifdef CONFIG_MLX5_CORE_EN
+#include "eswitch.h"
+#endif
 
 MODULE_AUTHOR("Eli Cohen <eli@mellanox.com>");
 MODULE_DESCRIPTION("Mellanox Connect-IB, ConnectX-4 core driver");
@@ -74,6 +78,11 @@ struct mlx5_device_context {
 	void		       *context;
 };
 
+enum {
+	MLX5_ATOMIC_REQ_MODE_BE = 0x0,
+	MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS = 0x1,
+};
+
 static struct mlx5_profile profile[] = {
 	[0] = {
 		.mask           = 0,
@@ -383,7 +392,7 @@ query_ex:
 	return err;
 }
 
-static int set_caps(struct mlx5_core_dev *dev, void *in, int in_sz)
+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;
@@ -391,6 +400,7 @@ static int set_caps(struct mlx5_core_dev *dev, void *in, int in_sz)
 	memset(out, 0, sizeof(out));
 
 	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;
@@ -400,6 +410,46 @@ static int set_caps(struct mlx5_core_dev *dev, void *in, int in_sz)
 	return err;
 }
 
+static int handle_hca_cap_atomic(struct mlx5_core_dev *dev)
+{
+	void *set_ctx;
+	void *set_hca_cap;
+	int set_sz = MLX5_ST_SZ_BYTES(set_hca_cap_in);
+	int req_endianness;
+	int err;
+
+	if (MLX5_CAP_GEN(dev, atomic)) {
+		err = mlx5_core_get_caps(dev, MLX5_CAP_ATOMIC,
+					 HCA_CAP_OPMOD_GET_CUR);
+		if (err)
+			return err;
+	} else {
+		return 0;
+	}
+
+	req_endianness =
+		MLX5_CAP_ATOMIC(dev,
+				supported_atomic_req_8B_endianess_mode_1);
+
+	if (req_endianness != MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS)
+		return 0;
+
+	set_ctx = kzalloc(set_sz, GFP_KERNEL);
+	if (!set_ctx)
+		return -ENOMEM;
+
+	set_hca_cap = MLX5_ADDR_OF(set_hca_cap_in, set_ctx, capability);
+
+	/* Set requestor to host endianness */
+	MLX5_SET(atomic_caps, set_hca_cap, atomic_req_8B_endianess_mode,
+		 MLX5_ATOMIC_REQ_MODE_HOST_ENDIANNESS);
+
+	err = set_caps(dev, set_ctx, set_sz, MLX5_SET_HCA_CAP_OP_MOD_ATOMIC);
+
+	kfree(set_ctx);
+	return err;
+}
+
 static int handle_hca_cap(struct mlx5_core_dev *dev)
 {
 	void *set_ctx = NULL;
@@ -441,7 +491,8 @@ static int handle_hca_cap(struct mlx5_core_dev *dev)
 
 	MLX5_SET(cmd_hca_cap, set_hca_cap, log_uar_page_sz, PAGE_SHIFT - 12);
 
-	err = set_caps(dev, set_ctx, set_sz);
+	err = set_caps(dev, set_ctx, set_sz,
+		       MLX5_SET_HCA_CAP_OP_MOD_GENERAL_DEVICE);
 
 query_ex:
 	kfree(set_ctx);
@@ -454,6 +505,9 @@ static int set_hca_ctrl(struct mlx5_core_dev *dev)
 	struct mlx5_reg_host_endianess he_out;
 	int err;
 
+	if (!mlx5_core_is_pf(dev))
+		return 0;
+
 	memset(&he_in, 0, sizeof(he_in));
 	he_in.he = MLX5_SET_HOST_ENDIANNESS;
 	err = mlx5_core_access_reg(dev, &he_in,  sizeof(he_in),
@@ -462,42 +516,52 @@ static int set_hca_ctrl(struct mlx5_core_dev *dev)
 	return err;
 }
 
-static int mlx5_core_enable_hca(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;
-	struct mlx5_enable_hca_mbox_in in;
-	struct mlx5_enable_hca_mbox_out out;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_ENABLE_HCA);
+	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;
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
-
-	return 0;
+	return mlx5_cmd_status_to_err_v2(out);
 }
 
-static int mlx5_core_disable_hca(struct mlx5_core_dev *dev)
+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;
-	struct mlx5_disable_hca_mbox_in in;
-	struct mlx5_disable_hca_mbox_out out;
 
-	memset(&in, 0, sizeof(in));
-	memset(&out, 0, sizeof(out));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_DISABLE_HCA);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), &out, sizeof(out));
+	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;
 
-	if (out.hdr.status)
-		return mlx5_cmd_status_to_err(&out.hdr);
+	return mlx5_cmd_status_to_err_v2(out);
+}
 
-	return 0;
+cycle_t mlx5_read_internal_timer(struct mlx5_core_dev *dev)
+{
+	u32 timer_h, timer_h1, timer_l;
+
+	timer_h = ioread32be(&dev->iseg->internal_timer_h);
+	timer_l = ioread32be(&dev->iseg->internal_timer_l);
+	timer_h1 = ioread32be(&dev->iseg->internal_timer_h);
+	if (timer_h != timer_h1) /* wrap around */
+		timer_l = ioread32be(&dev->iseg->internal_timer_l);
+
+	return (cycle_t)timer_l | (cycle_t)timer_h1 << 32;
 }
 
 static int mlx5_irq_set_affinity_hint(struct mlx5_core_dev *mdev, int i)
@@ -568,7 +632,8 @@ static void mlx5_irq_clear_affinity_hints(struct mlx5_core_dev *mdev)
 		mlx5_irq_clear_affinity_hint(mdev, i);
 }
 
-int mlx5_vector2eqn(struct mlx5_core_dev *dev, int vector, int *eqn, int *irqn)
+int mlx5_vector2eqn(struct mlx5_core_dev *dev, int vector, int *eqn,
+		    unsigned int *irqn)
 {
 	struct mlx5_eq_table *table = &dev->priv.eq_table;
 	struct mlx5_eq *eq, *n;
@@ -649,7 +714,6 @@ clean:
 	return err;
 }
 
-#ifdef CONFIG_MLX5_CORE_EN
 static int mlx5_core_set_issi(struct mlx5_core_dev *dev)
 {
 	u32 query_in[MLX5_ST_SZ_DW(query_issi_in)];
@@ -702,7 +766,6 @@ static int mlx5_core_set_issi(struct mlx5_core_dev *dev)
 
 	return -ENOTSUPP;
 }
-#endif
 
 static int map_bf_area(struct mlx5_core_dev *dev)
 {
@@ -942,19 +1005,17 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 
 	mlx5_pagealloc_init(dev);
 
-	err = mlx5_core_enable_hca(dev);
+	err = mlx5_core_enable_hca(dev, 0);
 	if (err) {
 		dev_err(&pdev->dev, "enable hca failed\n");
 		goto err_pagealloc_cleanup;
 	}
 
-#ifdef CONFIG_MLX5_CORE_EN
 	err = mlx5_core_set_issi(dev);
 	if (err) {
 		dev_err(&pdev->dev, "failed to set issi\n");
 		goto err_disable_hca;
 	}
-#endif
 
 	err = mlx5_satisfy_startup_pages(dev, 1);
 	if (err) {
@@ -974,6 +1035,12 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 		goto reclaim_boot_pages;
 	}
 
+	err = handle_hca_cap_atomic(dev);
+	if (err) {
+		dev_err(&pdev->dev, "handle_hca_cap_atomic failed\n");
+		goto reclaim_boot_pages;
+	}
+
 	err = mlx5_satisfy_startup_pages(dev, 0);
 	if (err) {
 		dev_err(&pdev->dev, "failed to allocate init pages\n");
@@ -1052,6 +1119,25 @@ static int mlx5_load_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 	mlx5_init_srq_table(dev);
 	mlx5_init_mr_table(dev);
 
+	err = mlx5_init_fs(dev);
+	if (err) {
+		dev_err(&pdev->dev, "Failed to init flow steering\n");
+		goto err_fs;
+	}
+#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;
+	}
+#endif
+
+	err = mlx5_sriov_init(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);
@@ -1068,7 +1154,16 @@ out:
 
 	return 0;
 
+err_sriov:
+	if (mlx5_sriov_cleanup(dev))
+		dev_err(&dev->pdev->dev, "sriov cleanup failed\n");
+
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_cleanup(dev->priv.eswitch);
+#endif
 err_reg_dev:
+	mlx5_cleanup_fs(dev);
+err_fs:
 	mlx5_cleanup_mr_table(dev);
 	mlx5_cleanup_srq_table(dev);
 	mlx5_cleanup_qp_table(dev);
@@ -1106,7 +1201,7 @@ reclaim_boot_pages:
 	mlx5_reclaim_startup_pages(dev);
 
 err_disable_hca:
-	mlx5_core_disable_hca(dev);
+	mlx5_core_disable_hca(dev, 0);
 
 err_pagealloc_cleanup:
 	mlx5_pagealloc_cleanup(dev);
@@ -1123,6 +1218,13 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 {
 	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 (dev->interface_state == MLX5_INTERFACE_STATE_DOWN) {
 		dev_warn(&dev->pdev->dev, "%s: interface is down, NOP\n",
@@ -1130,6 +1232,11 @@ static int mlx5_unload_one(struct mlx5_core_dev *dev, struct mlx5_priv *priv)
 		goto out;
 	}
 	mlx5_unregister_device(dev);
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_cleanup(dev->priv.eswitch);
+#endif
+
+	mlx5_cleanup_fs(dev);
 	mlx5_cleanup_mr_table(dev);
 	mlx5_cleanup_srq_table(dev);
 	mlx5_cleanup_qp_table(dev);
@@ -1149,7 +1256,7 @@ 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);
+	mlx5_core_disable_hca(dev, 0);
 	mlx5_pagealloc_cleanup(dev);
 	mlx5_cmd_cleanup(dev);
 
@@ -1195,6 +1302,7 @@ static int init_one(struct pci_dev *pdev,
 		return -ENOMEM;
 	}
 	priv = &dev->priv;
+	priv->pci_dev_data = id->driver_data;
 
 	pci_set_drvdata(pdev, dev);
 
@@ -1365,12 +1473,12 @@ static const struct pci_error_handlers mlx5_err_handler = {
 };
 
 static const struct pci_device_id mlx5_core_pci_table[] = {
-	{ PCI_VDEVICE(MELLANOX, 0x1011) }, /* Connect-IB */
-	{ PCI_VDEVICE(MELLANOX, 0x1012) }, /* Connect-IB VF */
-	{ PCI_VDEVICE(MELLANOX, 0x1013) }, /* ConnectX-4 */
-	{ PCI_VDEVICE(MELLANOX, 0x1014) }, /* ConnectX-4 VF */
-	{ PCI_VDEVICE(MELLANOX, 0x1015) }, /* ConnectX-4LX */
-	{ PCI_VDEVICE(MELLANOX, 0x1016) }, /* ConnectX-4LX VF */
+	{ PCI_VDEVICE(MELLANOX, 0x1011) },			/* Connect-IB */
+	{ PCI_VDEVICE(MELLANOX, 0x1012), MLX5_PCI_DEV_IS_VF},	/* Connect-IB VF */
+	{ PCI_VDEVICE(MELLANOX, 0x1013) },			/* ConnectX-4 */
+	{ PCI_VDEVICE(MELLANOX, 0x1014), MLX5_PCI_DEV_IS_VF},	/* ConnectX-4 VF */
+	{ PCI_VDEVICE(MELLANOX, 0x1015) },			/* ConnectX-4LX */
+	{ PCI_VDEVICE(MELLANOX, 0x1016), MLX5_PCI_DEV_IS_VF},	/* ConnectX-4LX VF */
 	{ 0, }
 };
 
@@ -1381,7 +1489,8 @@ static struct pci_driver mlx5_core_driver = {
 	.id_table       = mlx5_core_pci_table,
 	.probe          = init_one,
 	.remove         = remove_one,
-	.err_handler	= &mlx5_err_handler
+	.err_handler	= &mlx5_err_handler,
+	.sriov_configure   = mlx5_core_sriov_configure,
 };
 
 static int __init init(void)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
index cee5b7a839bc..0336847ec9a1 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
+++ b/drivers/net/ethernet/mellanox/mlx5/core/mlx5_core.h
@@ -36,6 +36,7 @@
 #include <linux/types.h>
 #include <linux/kernel.h>
 #include <linux/sched.h>
+#include <linux/if_link.h>
 
 #define DRIVER_NAME "mlx5_core"
 #define DRIVER_VERSION "3.0-1"
@@ -64,6 +65,9 @@ do {									\
 		(__dev)->priv.name, __func__, __LINE__, current->pid,	\
 		##__VA_ARGS__)
 
+#define mlx5_core_info(__dev, format, ...)				\
+	dev_info(&(__dev)->pdev->dev, format, ##__VA_ARGS__)
+
 enum {
 	MLX5_CMD_DATA, /* print command payload only */
 	MLX5_CMD_TIME, /* print command execution time */
@@ -90,6 +94,11 @@ 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_core_sriov_configure(struct pci_dev *dev, int num_vfs);
+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);
+cycle_t mlx5_read_internal_timer(struct mlx5_core_dev *dev);
 
 void mlx5e_init(void);
 void mlx5e_cleanup(void);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
index 4d3377b12657..9eeee0545f1c 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/pagealloc.c
@@ -33,6 +33,7 @@
 #include <linux/highmem.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/delay.h>
 #include <linux/mlx5/driver.h>
 #include <linux/mlx5/cmd.h>
 #include "mlx5_core.h"
@@ -95,6 +96,7 @@ struct mlx5_manage_pages_outbox {
 
 enum {
 	MAX_RECLAIM_TIME_MSECS	= 5000,
+	MAX_RECLAIM_VFS_PAGES_TIME_MSECS = 2 * 1000 * 60,
 };
 
 enum {
@@ -352,6 +354,10 @@ retry:
 		goto out_4k;
 	}
 
+	dev->priv.fw_pages += npages;
+	if (func_id)
+		dev->priv.vfs_pages += npages;
+
 	mlx5_core_dbg(dev, "err %d\n", err);
 
 	kvfree(in);
@@ -405,6 +411,12 @@ static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 	}
 
 	num_claimed = be32_to_cpu(out->num_entries);
+	if (num_claimed > npages) {
+		mlx5_core_warn(dev, "fw returned %d, driver asked %d => corruption\n",
+			       num_claimed, npages);
+		err = -EINVAL;
+		goto out_free;
+	}
 	if (nclaimed)
 		*nclaimed = num_claimed;
 
@@ -412,6 +424,9 @@ static int reclaim_pages(struct mlx5_core_dev *dev, u32 func_id, int npages,
 		addr = be64_to_cpu(out->pas[i]);
 		free_4k(dev, addr);
 	}
+	dev->priv.fw_pages -= num_claimed;
+	if (func_id)
+		dev->priv.vfs_pages -= num_claimed;
 
 out_free:
 	kvfree(out);
@@ -548,3 +563,26 @@ void mlx5_pagealloc_stop(struct mlx5_core_dev *dev)
 {
 	destroy_workqueue(dev->priv.pg_wq);
 }
+
+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;
+
+	mlx5_core_dbg(dev, "Waiting for %d pages from %s\n", prev_vfs_pages,
+		      dev->priv.name);
+	while (dev->priv.vfs_pages) {
+		if (time_after(jiffies, end)) {
+			mlx5_core_warn(dev, "aborting while there are %d pending pages\n", dev->priv.vfs_pages);
+			return -ETIMEDOUT;
+		}
+		if (dev->priv.vfs_pages < prev_vfs_pages) {
+			end = jiffies + msecs_to_jiffies(MAX_RECLAIM_VFS_PAGES_TIME_MSECS);
+			prev_vfs_pages = dev->priv.vfs_pages;
+		}
+		msleep(50);
+	}
+
+	mlx5_core_dbg(dev, "All pages received from %s\n", dev->priv.name);
+	return 0;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index 30e2ba3f5f16..def289375ecb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -36,6 +36,7 @@
 #include <linux/mlx5/cmd.h>
 #include <linux/mlx5/qp.h>
 #include <linux/mlx5/driver.h>
+#include <linux/mlx5/transobj.h>
 
 #include "mlx5_core.h"
 
@@ -67,6 +68,52 @@ void mlx5_core_put_rsc(struct mlx5_core_rsc_common *common)
 		complete(&common->free);
 }
 
+static u64 qp_allowed_event_types(void)
+{
+	u64 mask;
+
+	mask = BIT(MLX5_EVENT_TYPE_PATH_MIG) |
+	       BIT(MLX5_EVENT_TYPE_COMM_EST) |
+	       BIT(MLX5_EVENT_TYPE_SQ_DRAINED) |
+	       BIT(MLX5_EVENT_TYPE_SRQ_LAST_WQE) |
+	       BIT(MLX5_EVENT_TYPE_WQ_CATAS_ERROR) |
+	       BIT(MLX5_EVENT_TYPE_PATH_MIG_FAILED) |
+	       BIT(MLX5_EVENT_TYPE_WQ_INVAL_REQ_ERROR) |
+	       BIT(MLX5_EVENT_TYPE_WQ_ACCESS_ERROR);
+
+	return mask;
+}
+
+static u64 rq_allowed_event_types(void)
+{
+	u64 mask;
+
+	mask = BIT(MLX5_EVENT_TYPE_SRQ_LAST_WQE) |
+	       BIT(MLX5_EVENT_TYPE_WQ_CATAS_ERROR);
+
+	return mask;
+}
+
+static u64 sq_allowed_event_types(void)
+{
+	return BIT(MLX5_EVENT_TYPE_WQ_CATAS_ERROR);
+}
+
+static bool is_event_type_allowed(int rsc_type, int event_type)
+{
+	switch (rsc_type) {
+	case MLX5_EVENT_QUEUE_TYPE_QP:
+		return BIT(event_type) & qp_allowed_event_types();
+	case MLX5_EVENT_QUEUE_TYPE_RQ:
+		return BIT(event_type) & rq_allowed_event_types();
+	case MLX5_EVENT_QUEUE_TYPE_SQ:
+		return BIT(event_type) & sq_allowed_event_types();
+	default:
+		WARN(1, "Event arrived for unknown resource type");
+		return false;
+	}
+}
+
 void mlx5_rsc_event(struct mlx5_core_dev *dev, u32 rsn, int event_type)
 {
 	struct mlx5_core_rsc_common *common = mlx5_get_rsc(dev, rsn);
@@ -75,8 +122,16 @@ void mlx5_rsc_event(struct mlx5_core_dev *dev, u32 rsn, int event_type)
 	if (!common)
 		return;
 
+	if (!is_event_type_allowed((rsn >> MLX5_USER_INDEX_LEN), event_type)) {
+		mlx5_core_warn(dev, "event 0x%.2x is not allowed on resource 0x%.8x\n",
+			       event_type, rsn);
+		return;
+	}
+
 	switch (common->res) {
 	case MLX5_RES_QP:
+	case MLX5_RES_RQ:
+	case MLX5_RES_SQ:
 		qp = (struct mlx5_core_qp *)common;
 		qp->event(qp, event_type);
 		break;
@@ -177,27 +232,56 @@ void mlx5_eq_pagefault(struct mlx5_core_dev *dev, struct mlx5_eqe *eqe)
 }
 #endif
 
+static int create_qprqsq_common(struct mlx5_core_dev *dev,
+				struct mlx5_core_qp *qp,
+				int rsc_type)
+{
+	struct mlx5_qp_table *table = &dev->priv.qp_table;
+	int err;
+
+	qp->common.res = rsc_type;
+	spin_lock_irq(&table->lock);
+	err = radix_tree_insert(&table->tree,
+				qp->qpn | (rsc_type << MLX5_USER_INDEX_LEN),
+				qp);
+	spin_unlock_irq(&table->lock);
+	if (err)
+		return err;
+
+	atomic_set(&qp->common.refcount, 1);
+	init_completion(&qp->common.free);
+	qp->pid = current->pid;
+
+	return 0;
+}
+
+static void destroy_qprqsq_common(struct mlx5_core_dev *dev,
+				  struct mlx5_core_qp *qp)
+{
+	struct mlx5_qp_table *table = &dev->priv.qp_table;
+	unsigned long flags;
+
+	spin_lock_irqsave(&table->lock, flags);
+	radix_tree_delete(&table->tree,
+			  qp->qpn | (qp->common.res << MLX5_USER_INDEX_LEN));
+	spin_unlock_irqrestore(&table->lock, flags);
+	mlx5_core_put_rsc((struct mlx5_core_rsc_common *)qp);
+	wait_for_completion(&qp->common.free);
+}
+
 int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 			struct mlx5_core_qp *qp,
 			struct mlx5_create_qp_mbox_in *in,
 			int inlen)
 {
-	struct mlx5_qp_table *table = &dev->priv.qp_table;
 	struct mlx5_create_qp_mbox_out out;
 	struct mlx5_destroy_qp_mbox_in din;
 	struct mlx5_destroy_qp_mbox_out dout;
 	int err;
-	void *qpc;
 
 	memset(&out, 0, sizeof(out));
 	in->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_CREATE_QP);
 
-	if (dev->issi) {
-		qpc = MLX5_ADDR_OF(create_qp_in, in, qpc);
-		/* 0xffffff means we ask to work with cqe version 0 */
-		MLX5_SET(qpc, qpc, user_index, 0xffffff);
-	}
-
 	err = mlx5_cmd_exec(dev, in, inlen, &out, sizeof(out));
 	if (err) {
 		mlx5_core_warn(dev, "ret %d\n", err);
@@ -213,24 +297,16 @@ int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 	qp->qpn = be32_to_cpu(out.qpn) & 0xffffff;
 	mlx5_core_dbg(dev, "qpn = 0x%x\n", qp->qpn);
 
-	qp->common.res = MLX5_RES_QP;
-	spin_lock_irq(&table->lock);
-	err = radix_tree_insert(&table->tree, qp->qpn, qp);
-	spin_unlock_irq(&table->lock);
-	if (err) {
-		mlx5_core_warn(dev, "err %d\n", err);
+	err = create_qprqsq_common(dev, qp, MLX5_RES_QP);
+	if (err)
 		goto err_cmd;
-	}
 
 	err = mlx5_debug_qp_add(dev, qp);
 	if (err)
 		mlx5_core_dbg(dev, "failed adding QP 0x%x to debug file system\n",
 			      qp->qpn);
 
-	qp->pid = current->pid;
-	atomic_set(&qp->common.refcount, 1);
 	atomic_inc(&dev->num_qps);
-	init_completion(&qp->common.free);
 
 	return 0;
 
@@ -250,18 +326,11 @@ int mlx5_core_destroy_qp(struct mlx5_core_dev *dev,
 {
 	struct mlx5_destroy_qp_mbox_in in;
 	struct mlx5_destroy_qp_mbox_out out;
-	struct mlx5_qp_table *table = &dev->priv.qp_table;
-	unsigned long flags;
 	int err;
 
 	mlx5_debug_qp_remove(dev, qp);
 
-	spin_lock_irqsave(&table->lock, flags);
-	radix_tree_delete(&table->tree, qp->qpn);
-	spin_unlock_irqrestore(&table->lock, flags);
-
-	mlx5_core_put_rsc((struct mlx5_core_rsc_common *)qp);
-	wait_for_completion(&qp->common.free);
+	destroy_qprqsq_common(dev, qp);
 
 	memset(&in, 0, sizeof(in));
 	memset(&out, 0, sizeof(out));
@@ -279,59 +348,15 @@ int mlx5_core_destroy_qp(struct mlx5_core_dev *dev,
 }
 EXPORT_SYMBOL_GPL(mlx5_core_destroy_qp);
 
-int mlx5_core_qp_modify(struct mlx5_core_dev *dev, enum mlx5_qp_state cur_state,
-			enum mlx5_qp_state new_state,
+int mlx5_core_qp_modify(struct mlx5_core_dev *dev, u16 operation,
 			struct mlx5_modify_qp_mbox_in *in, int sqd_event,
 			struct mlx5_core_qp *qp)
 {
-	static const u16 optab[MLX5_QP_NUM_STATE][MLX5_QP_NUM_STATE] = {
-		[MLX5_QP_STATE_RST] = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_INIT]	= MLX5_CMD_OP_RST2INIT_QP,
-		},
-		[MLX5_QP_STATE_INIT]  = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_INIT]	= MLX5_CMD_OP_INIT2INIT_QP,
-			[MLX5_QP_STATE_RTR]	= MLX5_CMD_OP_INIT2RTR_QP,
-		},
-		[MLX5_QP_STATE_RTR]   = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_RTR2RTS_QP,
-		},
-		[MLX5_QP_STATE_RTS]   = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_RTS2RTS_QP,
-		},
-		[MLX5_QP_STATE_SQD] = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-		},
-		[MLX5_QP_STATE_SQER] = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_SQERR2RTS_QP,
-		},
-		[MLX5_QP_STATE_ERR] = {
-			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
-			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-		}
-	};
-
 	struct mlx5_modify_qp_mbox_out out;
 	int err = 0;
-	u16 op;
-
-	if (cur_state >= MLX5_QP_NUM_STATE || new_state >= MLX5_QP_NUM_STATE ||
-	    !optab[cur_state][new_state])
-		return -EINVAL;
 
 	memset(&out, 0, sizeof(out));
-	op = optab[cur_state][new_state];
-	in->hdr.opcode = cpu_to_be16(op);
+	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));
 	if (err)
@@ -449,3 +474,67 @@ int mlx5_core_page_fault_resume(struct mlx5_core_dev *dev, u32 qpn,
 }
 EXPORT_SYMBOL_GPL(mlx5_core_page_fault_resume);
 #endif
+
+int mlx5_core_create_rq_tracked(struct mlx5_core_dev *dev, u32 *in, int inlen,
+				struct mlx5_core_qp *rq)
+{
+	int err;
+	u32 rqn;
+
+	err = mlx5_core_create_rq(dev, in, inlen, &rqn);
+	if (err)
+		return err;
+
+	rq->qpn = rqn;
+	err = create_qprqsq_common(dev, rq, MLX5_RES_RQ);
+	if (err)
+		goto err_destroy_rq;
+
+	return 0;
+
+err_destroy_rq:
+	mlx5_core_destroy_rq(dev, rq->qpn);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx5_core_create_rq_tracked);
+
+void mlx5_core_destroy_rq_tracked(struct mlx5_core_dev *dev,
+				  struct mlx5_core_qp *rq)
+{
+	destroy_qprqsq_common(dev, rq);
+	mlx5_core_destroy_rq(dev, rq->qpn);
+}
+EXPORT_SYMBOL(mlx5_core_destroy_rq_tracked);
+
+int mlx5_core_create_sq_tracked(struct mlx5_core_dev *dev, u32 *in, int inlen,
+				struct mlx5_core_qp *sq)
+{
+	int err;
+	u32 sqn;
+
+	err = mlx5_core_create_sq(dev, in, inlen, &sqn);
+	if (err)
+		return err;
+
+	sq->qpn = sqn;
+	err = create_qprqsq_common(dev, sq, MLX5_RES_SQ);
+	if (err)
+		goto err_destroy_sq;
+
+	return 0;
+
+err_destroy_sq:
+	mlx5_core_destroy_sq(dev, sq->qpn);
+
+	return err;
+}
+EXPORT_SYMBOL(mlx5_core_create_sq_tracked);
+
+void mlx5_core_destroy_sq_tracked(struct mlx5_core_dev *dev,
+				  struct mlx5_core_qp *sq)
+{
+	destroy_qprqsq_common(dev, sq);
+	mlx5_core_destroy_sq(dev, sq->qpn);
+}
+EXPORT_SYMBOL(mlx5_core_destroy_sq_tracked);
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/sriov.c b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
new file mode 100644
index 000000000000..7b24386794f9
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlx5/core/sriov.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2014, Mellanox Technologies 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/pci.h>
+#include <linux/mlx5/driver.h>
+#include "mlx5_core.h"
+#ifdef CONFIG_MLX5_CORE_EN
+#include "eswitch.h"
+#endif
+
+static void enable_vfs(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 (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);
+		}
+	}
+}
+
+static void disable_vfs(struct mlx5_core_dev *dev, int num_vfs)
+{
+	struct mlx5_core_sriov *sriov = &dev->priv.sriov;
+	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;
+		}
+	}
+}
+
+static int mlx5_core_create_vfs(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);
+
+	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;
+	}
+
+	return 0;
+
+ex:
+	disable_vfs(dev, num_vfs);
+	return err;
+}
+
+static int mlx5_core_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;
+
+	kfree(sriov->vfs_ctx);
+	sriov->vfs_ctx = kcalloc(num_vfs, sizeof(*sriov->vfs_ctx), GFP_ATOMIC);
+	if (!sriov->vfs_ctx)
+		return -ENOMEM;
+
+	sriov->enabled_vfs = num_vfs;
+	err = mlx5_core_create_vfs(pdev, num_vfs);
+	if (err) {
+		kfree(sriov->vfs_ctx);
+		sriov->vfs_ctx = NULL;
+		return err;
+	}
+
+	return 0;
+}
+
+static void mlx5_core_init_vfs(struct mlx5_core_dev *dev, int num_vfs)
+{
+	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");
+
+	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;
+
+	mlx5_core_dbg(dev, "requsted 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;
+	}
+
+	err = mlx5_core_sriov_enable(pdev, num_vfs);
+	if (err) {
+		dev_warn(&pdev->dev, "mlx5_core_sriov_enable failed %d\n", err);
+		return err;
+	}
+
+	mlx5_core_init_vfs(dev, num_vfs);
+#ifdef CONFIG_MLX5_CORE_EN
+	mlx5_eswitch_enable_sriov(dev->priv.eswitch, num_vfs);
+#endif
+
+	return num_vfs;
+}
+
+static int sync_required(struct pci_dev *pdev)
+{
+	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;
+	}
+
+	return 0;
+}
+
+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;
+
+	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);
+	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);
+#endif
+
+	enable_vfs(dev, cur_vfs);
+
+	return 0;
+}
+
+int mlx5_sriov_cleanup(struct mlx5_core_dev *dev)
+{
+	struct pci_dev *pdev = dev->pdev;
+	int err;
+
+	if (!mlx5_core_is_pf(dev))
+		return 0;
+
+	err = mlx5_core_sriov_configure(pdev, 0);
+	if (err)
+		return err;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/srq.c b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
index ffada801976b..04bc522605a0 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/srq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/srq.c
@@ -37,7 +37,7 @@
 #include <linux/mlx5/srq.h>
 #include <rdma/ib_verbs.h>
 #include "mlx5_core.h"
-#include "transobj.h"
+#include <linux/mlx5/transobj.h>
 
 void mlx5_srq_event(struct mlx5_core_dev *dev, u32 srqn, int event_type)
 {
@@ -241,8 +241,6 @@ static int create_xrc_srq_cmd(struct mlx5_core_dev *dev,
 
 	memcpy(xrc_srqc, srqc, MLX5_ST_SZ_BYTES(srqc));
 	memcpy(pas, in->pas, pas_size);
-	/* 0xffffff means we ask to work with cqe version 0 */
-	MLX5_SET(xrc_srqc,	    xrc_srqc,  user_index, 0xffffff);
 	MLX5_SET(create_xrc_srq_in, create_in, opcode,
 		 MLX5_CMD_OP_CREATE_XRC_SRQ);
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
index d7068f54e800..03a5093ffeb7 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/transobj.c
@@ -32,9 +32,9 @@
 
 #include <linux/mlx5/driver.h>
 #include "mlx5_core.h"
-#include "transobj.h"
+#include <linux/mlx5/transobj.h>
 
-int mlx5_alloc_transport_domain(struct mlx5_core_dev *dev, u32 *tdn)
+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)];
@@ -53,8 +53,9 @@ int mlx5_alloc_transport_domain(struct mlx5_core_dev *dev, u32 *tdn)
 
 	return err;
 }
+EXPORT_SYMBOL(mlx5_core_alloc_transport_domain);
 
-void mlx5_dealloc_transport_domain(struct mlx5_core_dev *dev, u32 tdn)
+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)];
@@ -68,6 +69,7 @@ void mlx5_dealloc_transport_domain(struct mlx5_core_dev *dev, u32 tdn)
 
 	mlx5_cmd_exec_check_status(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)
 {
@@ -94,6 +96,7 @@ int mlx5_core_modify_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *in, int inlen)
 	memset(out, 0, sizeof(out));
 	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
 }
+EXPORT_SYMBOL(mlx5_core_modify_rq);
 
 void mlx5_core_destroy_rq(struct mlx5_core_dev *dev, u32 rqn)
 {
@@ -108,6 +111,18 @@ void mlx5_core_destroy_rq(struct mlx5_core_dev *dev, u32 rqn)
 	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
 }
 
+int mlx5_core_query_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *out)
+{
+	u32 in[MLX5_ST_SZ_DW(query_rq_in)] = {0};
+	int outlen = MLX5_ST_SZ_BYTES(query_rq_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);
+}
+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)];
@@ -133,6 +148,7 @@ int mlx5_core_modify_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *in, int inlen)
 	memset(out, 0, sizeof(out));
 	return mlx5_cmd_exec_check_status(dev, in, inlen, out, sizeof(out));
 }
+EXPORT_SYMBOL(mlx5_core_modify_sq);
 
 void mlx5_core_destroy_sq(struct mlx5_core_dev *dev, u32 sqn)
 {
@@ -147,6 +163,18 @@ void mlx5_core_destroy_sq(struct mlx5_core_dev *dev, u32 sqn)
 	mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, sizeof(out));
 }
 
+int mlx5_core_query_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *out)
+{
+	u32 in[MLX5_ST_SZ_DW(query_sq_in)] = {0};
+	int outlen = MLX5_ST_SZ_BYTES(query_sq_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);
+}
+EXPORT_SYMBOL(mlx5_core_query_sq);
+
 int mlx5_core_create_tir(struct mlx5_core_dev *dev, u32 *in, int inlen,
 			 u32 *tirn)
 {
@@ -162,6 +190,7 @@ int mlx5_core_create_tir(struct mlx5_core_dev *dev, u32 *in, int inlen,
 
 	return err;
 }
+EXPORT_SYMBOL(mlx5_core_create_tir);
 
 int mlx5_core_modify_tir(struct mlx5_core_dev *dev, u32 tirn, u32 *in,
 			 int inlen)
@@ -187,6 +216,7 @@ void mlx5_core_destroy_tir(struct mlx5_core_dev *dev, u32 tirn)
 
 	mlx5_cmd_exec_check_status(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)
@@ -203,6 +233,19 @@ int mlx5_core_create_tis(struct mlx5_core_dev *dev, u32 *in, int inlen,
 
 	return err;
 }
+EXPORT_SYMBOL(mlx5_core_create_tis);
+
+int mlx5_core_modify_tis(struct mlx5_core_dev *dev, u32 tisn, u32 *in,
+			 int inlen)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_tis_out)] = {0};
+
+	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));
+}
+EXPORT_SYMBOL(mlx5_core_modify_tis);
 
 void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn)
 {
@@ -216,6 +259,7 @@ void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn)
 
 	mlx5_cmd_exec_check_status(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)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/transobj.h b/drivers/net/ethernet/mellanox/mlx5/core/transobj.h
deleted file mode 100644
index 74cae51436e4..000000000000
--- a/drivers/net/ethernet/mellanox/mlx5/core/transobj.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * Copyright (c) 2013-2015, Mellanox Technologies, Ltd.  All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#ifndef __TRANSOBJ_H__
-#define __TRANSOBJ_H__
-
-int mlx5_alloc_transport_domain(struct mlx5_core_dev *dev, u32 *tdn);
-void mlx5_dealloc_transport_domain(struct mlx5_core_dev *dev, u32 tdn);
-int mlx5_core_create_rq(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			u32 *rqn);
-int mlx5_core_modify_rq(struct mlx5_core_dev *dev, u32 rqn, u32 *in, int inlen);
-void mlx5_core_destroy_rq(struct mlx5_core_dev *dev, u32 rqn);
-int mlx5_core_create_sq(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			u32 *sqn);
-int mlx5_core_modify_sq(struct mlx5_core_dev *dev, u32 sqn, u32 *in, int inlen);
-void mlx5_core_destroy_sq(struct mlx5_core_dev *dev, u32 sqn);
-int mlx5_core_create_tir(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			 u32 *tirn);
-int mlx5_core_modify_tir(struct mlx5_core_dev *dev, u32 tirn, u32 *in,
-			 int inlen);
-void mlx5_core_destroy_tir(struct mlx5_core_dev *dev, u32 tirn);
-int mlx5_core_create_tis(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			 u32 *tisn);
-void mlx5_core_destroy_tis(struct mlx5_core_dev *dev, u32 tisn);
-int mlx5_core_create_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			 u32 *rmpn);
-int mlx5_core_modify_rmp(struct mlx5_core_dev *dev, u32 *in, int inlen);
-int mlx5_core_destroy_rmp(struct mlx5_core_dev *dev, u32 rmpn);
-int mlx5_core_query_rmp(struct mlx5_core_dev *dev, u32 rmpn, u32 *out);
-int mlx5_core_arm_rmp(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm);
-int mlx5_core_create_xsrq(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			  u32 *rmpn);
-int mlx5_core_destroy_xsrq(struct mlx5_core_dev *dev, u32 rmpn);
-int mlx5_core_query_xsrq(struct mlx5_core_dev *dev, u32 rmpn, u32 *out);
-int mlx5_core_arm_xsrq(struct mlx5_core_dev *dev, u32 rmpn, u16 lwm);
-
-int mlx5_core_create_rqt(struct mlx5_core_dev *dev, u32 *in, int inlen,
-			 u32 *rqtn);
-int mlx5_core_modify_rqt(struct mlx5_core_dev *dev, u32 rqtn, u32 *in,
-			 int inlen);
-void mlx5_core_destroy_rqt(struct mlx5_core_dev *dev, u32 rqtn);
-
-#endif /* __TRANSOBJ_H__ */
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/vport.c b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
index b94177ebcf3a..c7398b95aecd 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/vport.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/vport.c
@@ -36,54 +36,458 @@
 #include <linux/mlx5/vport.h>
 #include "mlx5_core.h"
 
-u8 mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod)
+static int _mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod,
+				   u16 vport, u32 *out, int outlen)
 {
-	u32 in[MLX5_ST_SZ_DW(query_vport_state_in)];
-	u32 out[MLX5_ST_SZ_DW(query_vport_state_out)];
 	int err;
+	u32 in[MLX5_ST_SZ_DW(query_vport_state_in)];
 
 	memset(in, 0, sizeof(in));
 
 	MLX5_SET(query_vport_state_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_VPORT_STATE);
 	MLX5_SET(query_vport_state_in, in, op_mod, opmod);
+	MLX5_SET(query_vport_state_in, in, vport_number, vport);
+	if (vport)
+		MLX5_SET(query_vport_state_in, in, other_vport, 1);
 
-	err = mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out,
-					 sizeof(out));
+	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;
+}
+
+u8 mlx5_query_vport_state(struct mlx5_core_dev *mdev, u8 opmod, u16 vport)
+{
+	u32 out[MLX5_ST_SZ_DW(query_vport_state_out)] = {0};
+
+	_mlx5_query_vport_state(mdev, opmod, vport, out, sizeof(out));
+
 	return MLX5_GET(query_vport_state_out, out, state);
 }
-EXPORT_SYMBOL(mlx5_query_vport_state);
+EXPORT_SYMBOL_GPL(mlx5_query_vport_state);
+
+u8 mlx5_query_vport_admin_state(struct mlx5_core_dev *mdev, u8 opmod, u16 vport)
+{
+	u32 out[MLX5_ST_SZ_DW(query_vport_state_out)] = {0};
+
+	_mlx5_query_vport_state(mdev, opmod, vport, out, sizeof(out));
+
+	return MLX5_GET(query_vport_state_out, out, admin_state);
+}
+EXPORT_SYMBOL_GPL(mlx5_query_vport_admin_state);
 
-void mlx5_query_nic_vport_mac_address(struct mlx5_core_dev *mdev, u8 *addr)
+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));
+
+	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;
+}
+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));
+
+	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);
+}
+
+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)];
+
+	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));
+}
+
+int mlx5_query_nic_vport_mac_address(struct mlx5_core_dev *mdev,
+				     u16 vport, u8 *addr)
 {
-	u32  in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
 	u32 *out;
 	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
 	u8 *out_addr;
+	int err;
 
 	out = mlx5_vzalloc(outlen);
 	if (!out)
-		return;
+		return -ENOMEM;
 
 	out_addr = MLX5_ADDR_OF(query_nic_vport_context_out, out,
 				nic_vport_context.permanent_address);
 
+	err = mlx5_query_nic_vport_context(mdev, vport, out, outlen);
+	if (!err)
+		ether_addr_copy(addr, &out_addr[2]);
+
+	kvfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_mac_address);
+
+int mlx5_modify_nic_vport_mac_address(struct mlx5_core_dev *mdev,
+				      u16 vport, u8 *addr)
+{
+	void *in;
+	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
+	int err;
+	void *nic_vport_ctx;
+	u8 *perm_mac;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_warn(mdev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 field_select.permanent_address, 1);
+	MLX5_SET(modify_nic_vport_context_in, in, vport_number, vport);
+
+	if (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);
+	perm_mac = MLX5_ADDR_OF(nic_vport_context, nic_vport_ctx,
+				permanent_address);
+
+	ether_addr_copy(&perm_mac[2], addr);
+
+	err = mlx5_modify_nic_vport_context(mdev, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_modify_nic_vport_mac_address);
+
+int mlx5_query_nic_vport_mac_list(struct mlx5_core_dev *dev,
+				  u32 vport,
+				  enum mlx5_list_type list_type,
+				  u8 addr_list[][ETH_ALEN],
+				  int *list_size)
+{
+	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
+	void *nic_vport_ctx;
+	int max_list_size;
+	int req_list_size;
+	int out_sz;
+	void *out;
+	int err;
+	int i;
+
+	req_list_size = *list_size;
+
+	max_list_size = list_type == MLX5_NVPRT_LIST_TYPE_UC ?
+		1 << MLX5_CAP_GEN(dev, log_max_current_uc_list) :
+		1 << MLX5_CAP_GEN(dev, log_max_current_mc_list);
+
+	if (req_list_size > max_list_size) {
+		mlx5_core_warn(dev, "Requested list size (%d) > (%d) max_list_size\n",
+			       req_list_size, max_list_size);
+		req_list_size = max_list_size;
+	}
+
+	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;
 
 	MLX5_SET(query_nic_vport_context_in, in, opcode,
 		 MLX5_CMD_OP_QUERY_NIC_VPORT_CONTEXT);
+	MLX5_SET(query_nic_vport_context_in, in, allowed_list_type, list_type);
+	MLX5_SET(query_nic_vport_context_in, in, vport_number, vport);
 
-	memset(out, 0, outlen);
-	mlx5_cmd_exec_check_status(mdev, in, sizeof(in), out, outlen);
+	if (vport)
+		MLX5_SET(query_nic_vport_context_in, in, other_vport, 1);
 
-	ether_addr_copy(addr, &out_addr[2]);
+	err = mlx5_cmd_exec_check_status(dev, in, sizeof(in), out, out_sz);
+	if (err)
+		goto out;
 
-	kvfree(out);
+	nic_vport_ctx = MLX5_ADDR_OF(query_nic_vport_context_out, out,
+				     nic_vport_context);
+	req_list_size = MLX5_GET(nic_vport_context, nic_vport_ctx,
+				 allowed_list_size);
+
+	*list_size = req_list_size;
+	for (i = 0; i < req_list_size; i++) {
+		u8 *mac_addr = MLX5_ADDR_OF(nic_vport_context,
+					nic_vport_ctx,
+					current_uc_mac_address[i]) + 2;
+		ether_addr_copy(addr_list[i], mac_addr);
+	}
+out:
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_mac_list);
+
+int mlx5_modify_nic_vport_mac_list(struct mlx5_core_dev *dev,
+				   enum mlx5_list_type list_type,
+				   u8 addr_list[][ETH_ALEN],
+				   int list_size)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)];
+	void *nic_vport_ctx;
+	int max_list_size;
+	int in_sz;
+	void *in;
+	int err;
+	int i;
+
+	max_list_size = list_type == MLX5_NVPRT_LIST_TYPE_UC ?
+		 1 << MLX5_CAP_GEN(dev, log_max_current_uc_list) :
+		 1 << MLX5_CAP_GEN(dev, log_max_current_mc_list);
+
+	if (list_size > max_list_size)
+		return -ENOSPC;
+
+	in_sz = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in) +
+		list_size * MLX5_ST_SZ_BYTES(mac_address_layout);
+
+	memset(out, 0, sizeof(out));
+	in = kzalloc(in_sz, GFP_KERNEL);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(modify_nic_vport_context_in, in, opcode,
+		 MLX5_CMD_OP_MODIFY_NIC_VPORT_CONTEXT);
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 field_select.addresses_list, 1);
+
+	nic_vport_ctx = MLX5_ADDR_OF(modify_nic_vport_context_in, in,
+				     nic_vport_context);
+
+	MLX5_SET(nic_vport_context, nic_vport_ctx,
+		 allowed_list_type, list_type);
+	MLX5_SET(nic_vport_context, nic_vport_ctx,
+		 allowed_list_size, list_size);
+
+	for (i = 0; i < list_size; i++) {
+		u8 *curr_mac = MLX5_ADDR_OF(nic_vport_context,
+					    nic_vport_ctx,
+					    current_uc_mac_address[i]) + 2;
+		ether_addr_copy(curr_mac, addr_list[i]);
+	}
+
+	err = mlx5_cmd_exec_check_status(dev, in, in_sz, out, sizeof(out));
+	kfree(in);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_modify_nic_vport_mac_list);
+
+int mlx5_query_nic_vport_vlans(struct mlx5_core_dev *dev,
+			       u32 vport,
+			       u16 vlans[],
+			       int *size)
+{
+	u32 in[MLX5_ST_SZ_DW(query_nic_vport_context_in)];
+	void *nic_vport_ctx;
+	int req_list_size;
+	int max_list_size;
+	int out_sz;
+	void *out;
+	int err;
+	int i;
+
+	req_list_size = *size;
+	max_list_size = 1 << MLX5_CAP_GEN(dev, log_max_vlan_list);
+	if (req_list_size > max_list_size) {
+		mlx5_core_warn(dev, "Requested list size (%d) > (%d) max list size\n",
+			       req_list_size, max_list_size);
+		req_list_size = max_list_size;
+	}
+
+	out_sz = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in) +
+			req_list_size * MLX5_ST_SZ_BYTES(vlan_layout);
+
+	memset(in, 0, sizeof(in));
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+
+	MLX5_SET(query_nic_vport_context_in, in, opcode,
+		 MLX5_CMD_OP_QUERY_NIC_VPORT_CONTEXT);
+	MLX5_SET(query_nic_vport_context_in, in, allowed_list_type,
+		 MLX5_NVPRT_LIST_TYPE_VLAN);
+	MLX5_SET(query_nic_vport_context_in, in, vport_number, vport);
+
+	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);
+	if (err)
+		goto out;
+
+	nic_vport_ctx = MLX5_ADDR_OF(query_nic_vport_context_out, out,
+				     nic_vport_context);
+	req_list_size = MLX5_GET(nic_vport_context, nic_vport_ctx,
+				 allowed_list_size);
+
+	*size = req_list_size;
+	for (i = 0; i < req_list_size; i++) {
+		void *vlan_addr = MLX5_ADDR_OF(nic_vport_context,
+					       nic_vport_ctx,
+					       current_uc_mac_address[i]);
+		vlans[i] = MLX5_GET(vlan_layout, vlan_addr, vlan);
+	}
+out:
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_vlans);
+
+int mlx5_modify_nic_vport_vlans(struct mlx5_core_dev *dev,
+				u16 vlans[],
+				int list_size)
+{
+	u32 out[MLX5_ST_SZ_DW(modify_nic_vport_context_out)];
+	void *nic_vport_ctx;
+	int max_list_size;
+	int in_sz;
+	void *in;
+	int err;
+	int i;
+
+	max_list_size = 1 << MLX5_CAP_GEN(dev, log_max_vlan_list);
+
+	if (list_size > max_list_size)
+		return -ENOSPC;
+
+	in_sz = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in) +
+		list_size * MLX5_ST_SZ_BYTES(vlan_layout);
+
+	memset(out, 0, sizeof(out));
+	in = kzalloc(in_sz, GFP_KERNEL);
+	if (!in)
+		return -ENOMEM;
+
+	MLX5_SET(modify_nic_vport_context_in, in, opcode,
+		 MLX5_CMD_OP_MODIFY_NIC_VPORT_CONTEXT);
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 field_select.addresses_list, 1);
+
+	nic_vport_ctx = MLX5_ADDR_OF(modify_nic_vport_context_in, in,
+				     nic_vport_context);
+
+	MLX5_SET(nic_vport_context, nic_vport_ctx,
+		 allowed_list_type, MLX5_NVPRT_LIST_TYPE_VLAN);
+	MLX5_SET(nic_vport_context, nic_vport_ctx,
+		 allowed_list_size, list_size);
+
+	for (i = 0; i < list_size; i++) {
+		void *vlan_addr = MLX5_ADDR_OF(nic_vport_context,
+					       nic_vport_ctx,
+					       current_uc_mac_address[i]);
+		MLX5_SET(vlan_layout, vlan_addr, vlan, vlans[i]);
+	}
+
+	err = mlx5_cmd_exec_check_status(dev, in, in_sz, out, sizeof(out));
+	kfree(in);
+	return err;
 }
-EXPORT_SYMBOL(mlx5_query_nic_vport_mac_address);
+EXPORT_SYMBOL_GPL(mlx5_modify_nic_vport_vlans);
+
+int mlx5_query_nic_vport_system_image_guid(struct mlx5_core_dev *mdev,
+					   u64 *system_image_guid)
+{
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
+
+	out = mlx5_vzalloc(outlen);
+	if (!out)
+		return -ENOMEM;
+
+	mlx5_query_nic_vport_context(mdev, 0, out, outlen);
+
+	*system_image_guid = MLX5_GET64(query_nic_vport_context_out, out,
+					nic_vport_context.system_image_guid);
+
+	kfree(out);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_system_image_guid);
+
+int mlx5_query_nic_vport_node_guid(struct mlx5_core_dev *mdev, u64 *node_guid)
+{
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
+
+	out = mlx5_vzalloc(outlen);
+	if (!out)
+		return -ENOMEM;
+
+	mlx5_query_nic_vport_context(mdev, 0, out, outlen);
+
+	*node_guid = MLX5_GET64(query_nic_vport_context_out, out,
+				nic_vport_context.node_guid);
+
+	kfree(out);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_node_guid);
+
+int mlx5_query_nic_vport_qkey_viol_cntr(struct mlx5_core_dev *mdev,
+					u16 *qkey_viol_cntr)
+{
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
+
+	out = mlx5_vzalloc(outlen);
+	if (!out)
+		return -ENOMEM;
+
+	mlx5_query_nic_vport_context(mdev, 0, out, outlen);
+
+	*qkey_viol_cntr = MLX5_GET(query_nic_vport_context_out, out,
+				   nic_vport_context.qkey_violation_counter);
+
+	kfree(out);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_qkey_viol_cntr);
 
 int mlx5_query_hca_vport_gid(struct mlx5_core_dev *dev, u8 other_vport,
 			     u8 port_num, u16  vf_num, u16 gid_index,
@@ -343,3 +747,106 @@ int mlx5_query_hca_vport_node_guid(struct mlx5_core_dev *dev,
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx5_query_hca_vport_node_guid);
+
+int mlx5_query_nic_vport_promisc(struct mlx5_core_dev *mdev,
+				 u32 vport,
+				 int *promisc_uc,
+				 int *promisc_mc,
+				 int *promisc_all)
+{
+	u32 *out;
+	int outlen = MLX5_ST_SZ_BYTES(query_nic_vport_context_out);
+	int err;
+
+	out = kzalloc(outlen, GFP_KERNEL);
+	if (!out)
+		return -ENOMEM;
+
+	err = mlx5_query_nic_vport_context(mdev, vport, out, outlen);
+	if (err)
+		goto out;
+
+	*promisc_uc = MLX5_GET(query_nic_vport_context_out, out,
+			       nic_vport_context.promisc_uc);
+	*promisc_mc = MLX5_GET(query_nic_vport_context_out, out,
+			       nic_vport_context.promisc_mc);
+	*promisc_all = MLX5_GET(query_nic_vport_context_out, out,
+				nic_vport_context.promisc_all);
+
+out:
+	kfree(out);
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_query_nic_vport_promisc);
+
+int mlx5_modify_nic_vport_promisc(struct mlx5_core_dev *mdev,
+				  int promisc_uc,
+				  int promisc_mc,
+				  int promisc_all)
+{
+	void *in;
+	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_err(mdev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+
+	MLX5_SET(modify_nic_vport_context_in, in, field_select.promisc, 1);
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 nic_vport_context.promisc_uc, promisc_uc);
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 nic_vport_context.promisc_mc, promisc_mc);
+	MLX5_SET(modify_nic_vport_context_in, in,
+		 nic_vport_context.promisc_all, promisc_all);
+
+	err = mlx5_modify_nic_vport_context(mdev, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(mlx5_modify_nic_vport_promisc);
+
+enum mlx5_vport_roce_state {
+	MLX5_VPORT_ROCE_DISABLED = 0,
+	MLX5_VPORT_ROCE_ENABLED  = 1,
+};
+
+static int mlx5_nic_vport_update_roce_state(struct mlx5_core_dev *mdev,
+					    enum mlx5_vport_roce_state state)
+{
+	void *in;
+	int inlen = MLX5_ST_SZ_BYTES(modify_nic_vport_context_in);
+	int err;
+
+	in = mlx5_vzalloc(inlen);
+	if (!in) {
+		mlx5_core_warn(mdev, "failed to allocate inbox\n");
+		return -ENOMEM;
+	}
+
+	MLX5_SET(modify_nic_vport_context_in, in, field_select.roce_en, 1);
+	MLX5_SET(modify_nic_vport_context_in, in, nic_vport_context.roce_en,
+		 state);
+
+	err = mlx5_modify_nic_vport_context(mdev, in, inlen);
+
+	kvfree(in);
+
+	return err;
+}
+
+int mlx5_nic_vport_enable_roce(struct mlx5_core_dev *mdev)
+{
+	return mlx5_nic_vport_update_roce_state(mdev, MLX5_VPORT_ROCE_ENABLED);
+}
+EXPORT_SYMBOL_GPL(mlx5_nic_vport_enable_roce);
+
+int mlx5_nic_vport_disable_roce(struct mlx5_core_dev *mdev)
+{
+	return mlx5_nic_vport_update_roce_state(mdev, MLX5_VPORT_ROCE_DISABLED);
+}
+EXPORT_SYMBOL_GPL(mlx5_nic_vport_disable_roce);
diff --git a/drivers/net/ethernet/mellanox/mlxsw/Kconfig b/drivers/net/ethernet/mellanox/mlxsw/Kconfig
index e36e12219c9b..ce26adcb4988 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/Kconfig
+++ b/drivers/net/ethernet/mellanox/mlxsw/Kconfig
@@ -10,6 +10,14 @@ config MLXSW_CORE
 	  To compile this driver as a module, choose M here: the
 	  module will be called mlxsw_core.
 
+config MLXSW_CORE_HWMON
+	bool "HWMON support for Mellanox Technologies Switch ASICs"
+	depends on MLXSW_CORE && HWMON
+	depends on !(MLXSW_CORE=y && HWMON=m)
+	default y
+	---help---
+	  Say Y here if you want to expose HWMON interface on mlxsw devices.
+
 config MLXSW_PCI
 	tristate "PCI bus implementation for Mellanox Technologies Switch ASICs"
 	depends on PCI && HAS_DMA && HAS_IOMEM && MLXSW_CORE
@@ -33,7 +41,7 @@ config MLXSW_SWITCHX2
 
 config MLXSW_SPECTRUM
 	tristate "Mellanox Technologies Spectrum support"
-	depends on MLXSW_CORE && NET_SWITCHDEV
+	depends on MLXSW_CORE && NET_SWITCHDEV && VLAN_8021Q
 	default m
 	---help---
 	  This driver supports Mellanox Technologies Spectrum Ethernet
diff --git a/drivers/net/ethernet/mellanox/mlxsw/Makefile b/drivers/net/ethernet/mellanox/mlxsw/Makefile
index af015818fd19..584cac444852 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/Makefile
+++ b/drivers/net/ethernet/mellanox/mlxsw/Makefile
@@ -1,5 +1,6 @@
 obj-$(CONFIG_MLXSW_CORE)	+= mlxsw_core.o
 mlxsw_core-objs			:= core.o
+mlxsw_core-$(CONFIG_MLXSW_CORE_HWMON) += core_hwmon.o
 obj-$(CONFIG_MLXSW_PCI)		+= mlxsw_pci.o
 mlxsw_pci-objs			:= pci.o
 obj-$(CONFIG_MLXSW_SWITCHX2)	+= mlxsw_switchx2.o
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.c b/drivers/net/ethernet/mellanox/mlxsw/core.c
index 97f0d93caf99..22379eb8e924 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.c
@@ -105,6 +105,10 @@ struct mlxsw_core {
 		struct debugfs_blob_wrapper vsd_blob;
 		struct debugfs_blob_wrapper psid_blob;
 	} dbg;
+	struct {
+		u8 *mapping; /* lag_id+port_index to local_port mapping */
+	} lag;
+	struct mlxsw_hwmon *hwmon;
 	unsigned long driver_priv[0];
 	/* driver_priv has to be always the last item */
 };
@@ -814,6 +818,17 @@ 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;
+		mlxsw_core->lag.mapping = kzalloc(alloc_size, GFP_KERNEL);
+		if (!mlxsw_core->lag.mapping) {
+			err = -ENOMEM;
+			goto err_alloc_lag_mapping;
+		}
+	}
+
 	err = mlxsw_bus->init(bus_priv, mlxsw_core, mlxsw_driver->profile);
 	if (err)
 		goto err_bus_init;
@@ -822,6 +837,10 @@ int mlxsw_core_bus_device_register(const struct mlxsw_bus_info *mlxsw_bus_info,
 	if (err)
 		goto err_emad_init;
 
+	err = mlxsw_hwmon_init(mlxsw_core, mlxsw_bus_info, &mlxsw_core->hwmon);
+	if (err)
+		goto err_hwmon_init;
+
 	err = mlxsw_driver->init(mlxsw_core->driver_priv, mlxsw_core,
 				 mlxsw_bus_info);
 	if (err)
@@ -836,10 +855,13 @@ int mlxsw_core_bus_device_register(const struct mlxsw_bus_info *mlxsw_bus_info,
 err_debugfs_init:
 	mlxsw_core->driver->fini(mlxsw_core->driver_priv);
 err_driver_init:
+err_hwmon_init:
 	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:
 	free_percpu(mlxsw_core->pcpu_stats);
 err_alloc_stats:
 	kfree(mlxsw_core);
@@ -857,6 +879,7 @@ void mlxsw_core_bus_device_unregister(struct mlxsw_core *mlxsw_core)
 	mlxsw_core->driver->fini(mlxsw_core->driver_priv);
 	mlxsw_emad_fini(mlxsw_core);
 	mlxsw_core->bus->fini(mlxsw_core->bus_priv);
+	kfree(mlxsw_core->lag.mapping);
 	free_percpu(mlxsw_core->pcpu_stats);
 	kfree(mlxsw_core);
 	mlxsw_core_driver_put(device_kind);
@@ -1188,11 +1211,25 @@ void mlxsw_core_skb_receive(struct mlxsw_core *mlxsw_core, struct sk_buff *skb,
 	struct mlxsw_rx_listener_item *rxl_item;
 	const struct mlxsw_rx_listener *rxl;
 	struct mlxsw_core_pcpu_stats *pcpu_stats;
-	u8 local_port = rx_info->sys_port;
+	u8 local_port;
 	bool found = false;
 
-	dev_dbg_ratelimited(mlxsw_core->bus_info->dev, "%s: sys_port = %d, trap_id = 0x%x\n",
-			    __func__, rx_info->sys_port, rx_info->trap_id);
+	if (rx_info->is_lag) {
+		dev_dbg_ratelimited(mlxsw_core->bus_info->dev, "%s: lag_id = %d, lag_port_index = 0x%x\n",
+				    __func__, rx_info->u.lag_id,
+				    rx_info->trap_id);
+		/* Upper layer does not care if the skb came from LAG or not,
+		 * so just get the local_port for the lag port and push it up.
+		 */
+		local_port = mlxsw_core_lag_mapping_get(mlxsw_core,
+							rx_info->u.lag_id,
+							rx_info->lag_port_index);
+	} else {
+		local_port = rx_info->u.sys_port;
+	}
+
+	dev_dbg_ratelimited(mlxsw_core->bus_info->dev, "%s: local_port = %d, trap_id = 0x%x\n",
+			    __func__, local_port, rx_info->trap_id);
 
 	if ((rx_info->trap_id >= MLXSW_TRAP_ID_MAX) ||
 	    (local_port >= MLXSW_PORT_MAX_PORTS))
@@ -1236,6 +1273,48 @@ drop:
 }
 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 +
+	       port_index;
+}
+
+void mlxsw_core_lag_mapping_set(struct mlxsw_core *mlxsw_core,
+				u16 lag_id, u8 port_index, u8 local_port)
+{
+	int index = mlxsw_core_lag_mapping_index(mlxsw_core,
+						 lag_id, port_index);
+
+	mlxsw_core->lag.mapping[index] = local_port;
+}
+EXPORT_SYMBOL(mlxsw_core_lag_mapping_set);
+
+u8 mlxsw_core_lag_mapping_get(struct mlxsw_core *mlxsw_core,
+			      u16 lag_id, u8 port_index)
+{
+	int index = mlxsw_core_lag_mapping_index(mlxsw_core,
+						 lag_id, port_index);
+
+	return mlxsw_core->lag.mapping[index];
+}
+EXPORT_SYMBOL(mlxsw_core_lag_mapping_get);
+
+void mlxsw_core_lag_mapping_clear(struct mlxsw_core *mlxsw_core,
+				  u16 lag_id, u8 local_port)
+{
+	int i;
+
+	for (i = 0; i < mlxsw_core->driver->profile->max_port_per_lag; i++) {
+		int index = mlxsw_core_lag_mapping_index(mlxsw_core,
+							 lag_id, i);
+
+		if (mlxsw_core->lag.mapping[index] == local_port)
+			mlxsw_core->lag.mapping[index] = 0;
+	}
+}
+EXPORT_SYMBOL(mlxsw_core_lag_mapping_clear);
+
 int mlxsw_cmd_exec(struct mlxsw_core *mlxsw_core, u16 opcode, u8 opcode_mod,
 		   u32 in_mod, bool out_mbox_direct,
 		   char *in_mbox, size_t in_mbox_size,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core.h b/drivers/net/ethernet/mellanox/mlxsw/core.h
index 807827350a89..a01723600f0a 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/core.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/core.h
@@ -112,13 +112,25 @@ int mlxsw_reg_write(struct mlxsw_core *mlxsw_core,
 		    const struct mlxsw_reg_info *reg, char *payload);
 
 struct mlxsw_rx_info {
-	u16 sys_port;
+	bool is_lag;
+	union {
+		u16 sys_port;
+		u16 lag_id;
+	} u;
+	u8 lag_port_index;
 	int trap_id;
 };
 
 void mlxsw_core_skb_receive(struct mlxsw_core *mlxsw_core, struct sk_buff *skb,
 			    struct mlxsw_rx_info *rx_info);
 
+void mlxsw_core_lag_mapping_set(struct mlxsw_core *mlxsw_core,
+				u16 lag_id, u8 port_index, u8 local_port);
+u8 mlxsw_core_lag_mapping_get(struct mlxsw_core *mlxsw_core,
+			      u16 lag_id, u8 port_index);
+void mlxsw_core_lag_mapping_clear(struct mlxsw_core *mlxsw_core,
+				  u16 lag_id, u8 local_port);
+
 #define MLXSW_CONFIG_PROFILE_SWID_COUNT 8
 
 struct mlxsw_swid_config {
@@ -209,4 +221,24 @@ struct mlxsw_bus_info {
 	u8 psid[MLXSW_CMD_BOARDINFO_PSID_LEN];
 };
 
+struct mlxsw_hwmon;
+
+#ifdef CONFIG_MLXSW_CORE_HWMON
+
+int mlxsw_hwmon_init(struct mlxsw_core *mlxsw_core,
+		     const struct mlxsw_bus_info *mlxsw_bus_info,
+		     struct mlxsw_hwmon **p_hwmon);
+void mlxsw_hwmon_fini(struct mlxsw_hwmon *mlxsw_hwmon);
+
+#else
+
+static inline int mlxsw_hwmon_init(struct mlxsw_core *mlxsw_core,
+				   const struct mlxsw_bus_info *mlxsw_bus_info,
+				   struct mlxsw_hwmon **p_hwmon)
+{
+	return 0;
+}
+
+#endif
+
 #endif
diff --git a/drivers/net/ethernet/mellanox/mlxsw/core_hwmon.c b/drivers/net/ethernet/mellanox/mlxsw/core_hwmon.c
new file mode 100644
index 000000000000..1ac8bf187168
--- /dev/null
+++ b/drivers/net/ethernet/mellanox/mlxsw/core_hwmon.c
@@ -0,0 +1,372 @@
+/*
+ * drivers/net/ethernet/mellanox/mlxsw/core_hwmon.c
+ * Copyright (c) 2015 Mellanox Technologies. All rights reserved.
+ * Copyright (c) 2015 Jiri Pirko <jiri@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/kernel.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/sysfs.h>
+#include <linux/hwmon.h>
+#include <linux/err.h>
+
+#include "core.h"
+
+#define MLXSW_HWMON_TEMP_SENSOR_MAX_COUNT 127
+#define MLXSW_HWMON_ATTR_COUNT (MLXSW_HWMON_TEMP_SENSOR_MAX_COUNT * 4 + \
+				MLXSW_MFCR_TACHOS_MAX + MLXSW_MFCR_PWMS_MAX)
+
+struct mlxsw_hwmon_attr {
+	struct device_attribute dev_attr;
+	struct mlxsw_hwmon *hwmon;
+	unsigned int type_index;
+	char name[32];
+};
+
+struct mlxsw_hwmon {
+	struct mlxsw_core *core;
+	const struct mlxsw_bus_info *bus_info;
+	struct device *hwmon_dev;
+	struct attribute_group group;
+	const struct attribute_group *groups[2];
+	struct attribute *attrs[MLXSW_HWMON_ATTR_COUNT + 1];
+	struct mlxsw_hwmon_attr hwmon_attrs[MLXSW_HWMON_ATTR_COUNT];
+	unsigned int attrs_count;
+};
+
+static ssize_t mlxsw_hwmon_temp_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mtmp_pl[MLXSW_REG_MTMP_LEN];
+	unsigned int temp;
+	int err;
+
+	mlxsw_reg_mtmp_pack(mtmp_pl, mlwsw_hwmon_attr->type_index,
+			    false, false);
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mtmp), mtmp_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to query temp sensor\n");
+		return err;
+	}
+	mlxsw_reg_mtmp_unpack(mtmp_pl, &temp, NULL, NULL);
+	return sprintf(buf, "%u\n", temp);
+}
+
+static ssize_t mlxsw_hwmon_temp_max_show(struct device *dev,
+					 struct device_attribute *attr,
+					 char *buf)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mtmp_pl[MLXSW_REG_MTMP_LEN];
+	unsigned int temp_max;
+	int err;
+
+	mlxsw_reg_mtmp_pack(mtmp_pl, mlwsw_hwmon_attr->type_index,
+			    false, false);
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mtmp), mtmp_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to query temp sensor\n");
+		return err;
+	}
+	mlxsw_reg_mtmp_unpack(mtmp_pl, NULL, &temp_max, NULL);
+	return sprintf(buf, "%u\n", temp_max);
+}
+
+static ssize_t mlxsw_hwmon_temp_rst_store(struct device *dev,
+					  struct device_attribute *attr,
+					  const char *buf, size_t len)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mtmp_pl[MLXSW_REG_MTMP_LEN];
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err)
+		return err;
+	if (val != 1)
+		return -EINVAL;
+
+	mlxsw_reg_mtmp_pack(mtmp_pl, mlwsw_hwmon_attr->type_index, true, true);
+	err = mlxsw_reg_write(mlxsw_hwmon->core, MLXSW_REG(mtmp), mtmp_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to reset temp sensor history\n");
+		return err;
+	}
+	return len;
+}
+
+static ssize_t mlxsw_hwmon_fan_rpm_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mfsm_pl[MLXSW_REG_MFSM_LEN];
+	int err;
+
+	mlxsw_reg_mfsm_pack(mfsm_pl, mlwsw_hwmon_attr->type_index);
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mfsm), mfsm_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to query fan\n");
+		return err;
+	}
+	return sprintf(buf, "%u\n", mlxsw_reg_mfsm_rpm_get(mfsm_pl));
+}
+
+static ssize_t mlxsw_hwmon_pwm_show(struct device *dev,
+				    struct device_attribute *attr,
+				    char *buf)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mfsc_pl[MLXSW_REG_MFSC_LEN];
+	int err;
+
+	mlxsw_reg_mfsc_pack(mfsc_pl, mlwsw_hwmon_attr->type_index, 0);
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mfsc), mfsc_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to query PWM\n");
+		return err;
+	}
+	return sprintf(buf, "%u\n",
+		       mlxsw_reg_mfsc_pwm_duty_cycle_get(mfsc_pl));
+}
+
+static ssize_t mlxsw_hwmon_pwm_store(struct device *dev,
+				     struct device_attribute *attr,
+				     const char *buf, size_t len)
+{
+	struct mlxsw_hwmon_attr *mlwsw_hwmon_attr =
+			container_of(attr, struct mlxsw_hwmon_attr, dev_attr);
+	struct mlxsw_hwmon *mlxsw_hwmon = mlwsw_hwmon_attr->hwmon;
+	char mfsc_pl[MLXSW_REG_MFSC_LEN];
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err)
+		return err;
+	if (val > 255)
+		return -EINVAL;
+
+	mlxsw_reg_mfsc_pack(mfsc_pl, mlwsw_hwmon_attr->type_index, val);
+	err = mlxsw_reg_write(mlxsw_hwmon->core, MLXSW_REG(mfsc), mfsc_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to write PWM\n");
+		return err;
+	}
+	return len;
+}
+
+enum mlxsw_hwmon_attr_type {
+	MLXSW_HWMON_ATTR_TYPE_TEMP,
+	MLXSW_HWMON_ATTR_TYPE_TEMP_MAX,
+	MLXSW_HWMON_ATTR_TYPE_TEMP_RST,
+	MLXSW_HWMON_ATTR_TYPE_FAN_RPM,
+	MLXSW_HWMON_ATTR_TYPE_PWM,
+};
+
+static void mlxsw_hwmon_attr_add(struct mlxsw_hwmon *mlxsw_hwmon,
+				 enum mlxsw_hwmon_attr_type attr_type,
+				 unsigned int type_index, unsigned int num) {
+	struct mlxsw_hwmon_attr *mlxsw_hwmon_attr;
+	unsigned int attr_index;
+
+	attr_index = mlxsw_hwmon->attrs_count;
+	mlxsw_hwmon_attr = &mlxsw_hwmon->hwmon_attrs[attr_index];
+
+	switch (attr_type) {
+	case MLXSW_HWMON_ATTR_TYPE_TEMP:
+		mlxsw_hwmon_attr->dev_attr.show = mlxsw_hwmon_temp_show;
+		mlxsw_hwmon_attr->dev_attr.attr.mode = S_IRUGO;
+		snprintf(mlxsw_hwmon_attr->name, sizeof(mlxsw_hwmon_attr->name),
+			 "temp%u_input", num + 1);
+		break;
+	case MLXSW_HWMON_ATTR_TYPE_TEMP_MAX:
+		mlxsw_hwmon_attr->dev_attr.show = mlxsw_hwmon_temp_max_show;
+		mlxsw_hwmon_attr->dev_attr.attr.mode = S_IRUGO;
+		snprintf(mlxsw_hwmon_attr->name, sizeof(mlxsw_hwmon_attr->name),
+			 "temp%u_highest", num + 1);
+		break;
+	case MLXSW_HWMON_ATTR_TYPE_TEMP_RST:
+		mlxsw_hwmon_attr->dev_attr.store = mlxsw_hwmon_temp_rst_store;
+		mlxsw_hwmon_attr->dev_attr.attr.mode = S_IWUSR;
+		snprintf(mlxsw_hwmon_attr->name, sizeof(mlxsw_hwmon_attr->name),
+			 "temp%u_reset_history", num + 1);
+		break;
+	case MLXSW_HWMON_ATTR_TYPE_FAN_RPM:
+		mlxsw_hwmon_attr->dev_attr.show = mlxsw_hwmon_fan_rpm_show;
+		mlxsw_hwmon_attr->dev_attr.attr.mode = S_IRUGO;
+		snprintf(mlxsw_hwmon_attr->name, sizeof(mlxsw_hwmon_attr->name),
+			 "fan%u_input", num + 1);
+		break;
+	case MLXSW_HWMON_ATTR_TYPE_PWM:
+		mlxsw_hwmon_attr->dev_attr.show = mlxsw_hwmon_pwm_show;
+		mlxsw_hwmon_attr->dev_attr.store = mlxsw_hwmon_pwm_store;
+		mlxsw_hwmon_attr->dev_attr.attr.mode = S_IWUSR | S_IRUGO;
+		snprintf(mlxsw_hwmon_attr->name, sizeof(mlxsw_hwmon_attr->name),
+			 "pwm%u", num + 1);
+		break;
+	default:
+		WARN_ON(1);
+	}
+
+	mlxsw_hwmon_attr->type_index = type_index;
+	mlxsw_hwmon_attr->hwmon = mlxsw_hwmon;
+	mlxsw_hwmon_attr->dev_attr.attr.name = mlxsw_hwmon_attr->name;
+	sysfs_attr_init(&mlxsw_hwmon_attr->dev_attr.attr);
+
+	mlxsw_hwmon->attrs[attr_index] = &mlxsw_hwmon_attr->dev_attr.attr;
+	mlxsw_hwmon->attrs_count++;
+}
+
+static int mlxsw_hwmon_temp_init(struct mlxsw_hwmon *mlxsw_hwmon)
+{
+	char mtcap_pl[MLXSW_REG_MTCAP_LEN];
+	char mtmp_pl[MLXSW_REG_MTMP_LEN];
+	u8 sensor_count;
+	int i;
+	int err;
+
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mtcap), mtcap_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to get number of temp sensors\n");
+		return err;
+	}
+	sensor_count = mlxsw_reg_mtcap_sensor_count_get(mtcap_pl);
+	for (i = 0; i < sensor_count; i++) {
+		mlxsw_reg_mtmp_pack(mtmp_pl, i, true, true);
+		err = mlxsw_reg_write(mlxsw_hwmon->core,
+				      MLXSW_REG(mtmp), mtmp_pl);
+		if (err) {
+			dev_err(mlxsw_hwmon->bus_info->dev, "Failed to setup temp sensor number %d\n",
+				i);
+			return err;
+		}
+		mlxsw_hwmon_attr_add(mlxsw_hwmon,
+				     MLXSW_HWMON_ATTR_TYPE_TEMP, i, i);
+		mlxsw_hwmon_attr_add(mlxsw_hwmon,
+				     MLXSW_HWMON_ATTR_TYPE_TEMP_MAX, i, i);
+		mlxsw_hwmon_attr_add(mlxsw_hwmon,
+				     MLXSW_HWMON_ATTR_TYPE_TEMP_RST, i, i);
+	}
+	return 0;
+}
+
+static int mlxsw_hwmon_fans_init(struct mlxsw_hwmon *mlxsw_hwmon)
+{
+	char mfcr_pl[MLXSW_REG_MFCR_LEN];
+	enum mlxsw_reg_mfcr_pwm_frequency freq;
+	unsigned int type_index;
+	unsigned int num;
+	u16 tacho_active;
+	u8 pwm_active;
+	int err;
+
+	err = mlxsw_reg_query(mlxsw_hwmon->core, MLXSW_REG(mfcr), mfcr_pl);
+	if (err) {
+		dev_err(mlxsw_hwmon->bus_info->dev, "Failed to get to probe PWMs and Tachometers\n");
+		return err;
+	}
+	mlxsw_reg_mfcr_unpack(mfcr_pl, &freq, &tacho_active, &pwm_active);
+	num = 0;
+	for (type_index = 0; type_index < MLXSW_MFCR_TACHOS_MAX; type_index++) {
+		if (tacho_active & BIT(type_index))
+			mlxsw_hwmon_attr_add(mlxsw_hwmon,
+					     MLXSW_HWMON_ATTR_TYPE_FAN_RPM,
+					     type_index, num++);
+	}
+	num = 0;
+	for (type_index = 0; type_index < MLXSW_MFCR_PWMS_MAX; type_index++) {
+		if (pwm_active & BIT(type_index))
+			mlxsw_hwmon_attr_add(mlxsw_hwmon,
+					     MLXSW_HWMON_ATTR_TYPE_PWM,
+					     type_index, num++);
+	}
+	return 0;
+}
+
+int mlxsw_hwmon_init(struct mlxsw_core *mlxsw_core,
+		     const struct mlxsw_bus_info *mlxsw_bus_info,
+		     struct mlxsw_hwmon **p_hwmon)
+{
+	struct mlxsw_hwmon *mlxsw_hwmon;
+	struct device *hwmon_dev;
+	int err;
+
+	mlxsw_hwmon = devm_kzalloc(mlxsw_bus_info->dev, sizeof(*mlxsw_hwmon),
+				   GFP_KERNEL);
+	if (!mlxsw_hwmon)
+		return -ENOMEM;
+	mlxsw_hwmon->core = mlxsw_core;
+	mlxsw_hwmon->bus_info = mlxsw_bus_info;
+
+	err = mlxsw_hwmon_temp_init(mlxsw_hwmon);
+	if (err)
+		goto err_temp_init;
+
+	err = mlxsw_hwmon_fans_init(mlxsw_hwmon);
+	if (err)
+		goto err_fans_init;
+
+	mlxsw_hwmon->groups[0] = &mlxsw_hwmon->group;
+	mlxsw_hwmon->group.attrs = mlxsw_hwmon->attrs;
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(mlxsw_bus_info->dev,
+							   "mlxsw",
+							   mlxsw_hwmon,
+							   mlxsw_hwmon->groups);
+	if (IS_ERR(hwmon_dev)) {
+		err = PTR_ERR(hwmon_dev);
+		goto err_hwmon_register;
+	}
+
+	mlxsw_hwmon->hwmon_dev = hwmon_dev;
+	*p_hwmon = mlxsw_hwmon;
+	return 0;
+
+err_hwmon_register:
+err_fans_init:
+err_temp_init:
+	return err;
+}
diff --git a/drivers/net/ethernet/mellanox/mlxsw/pci.c b/drivers/net/ethernet/mellanox/mlxsw/pci.c
index de69e719dc9d..7992c553c1f5 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/pci.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/pci.c
@@ -215,7 +215,7 @@ mlxsw_pci_queue_elem_info_producer_get(struct mlxsw_pci_queue *q)
 {
 	int index = q->producer_counter & (q->count - 1);
 
-	if ((q->producer_counter - q->consumer_counter) == q->count)
+	if ((u16) (q->producer_counter - q->consumer_counter) == q->count)
 		return NULL;
 	return mlxsw_pci_queue_elem_info_get(q, index);
 }
@@ -384,7 +384,7 @@ static int mlxsw_pci_sdq_init(struct mlxsw_pci *mlxsw_pci, char *mbox,
 
 	/* Set CQ of same number of this SDQ. */
 	mlxsw_cmd_mbox_sw2hw_dq_cq_set(mbox, q->num);
-	mlxsw_cmd_mbox_sw2hw_dq_sdq_tclass_set(mbox, 7);
+	mlxsw_cmd_mbox_sw2hw_dq_sdq_tclass_set(mbox, 3);
 	mlxsw_cmd_mbox_sw2hw_dq_log2_dq_sz_set(mbox, 3); /* 8 pages */
 	for (i = 0; i < MLXSW_PCI_AQ_PAGES; i++) {
 		dma_addr_t mapaddr = __mlxsw_pci_queue_page_get(q, i);
@@ -686,11 +686,15 @@ static void mlxsw_pci_cqe_rdq_handle(struct mlxsw_pci *mlxsw_pci,
 	if (q->consumer_counter++ != consumer_counter_limit)
 		dev_dbg_ratelimited(&pdev->dev, "Consumer counter does not match limit in RDQ\n");
 
-	/* We do not support lag now */
-	if (mlxsw_pci_cqe_lag_get(cqe))
-		goto drop;
+	if (mlxsw_pci_cqe_lag_get(cqe)) {
+		rx_info.is_lag = true;
+		rx_info.u.lag_id = mlxsw_pci_cqe_lag_id_get(cqe);
+		rx_info.lag_port_index = mlxsw_pci_cqe_lag_port_index_get(cqe);
+	} else {
+		rx_info.is_lag = false;
+		rx_info.u.sys_port = mlxsw_pci_cqe_system_port_get(cqe);
+	}
 
-	rx_info.sys_port = mlxsw_pci_cqe_system_port_get(cqe);
 	rx_info.trap_id = mlxsw_pci_cqe_trap_id_get(cqe);
 
 	byte_count = mlxsw_pci_cqe_byte_count_get(cqe);
@@ -699,7 +703,6 @@ static void mlxsw_pci_cqe_rdq_handle(struct mlxsw_pci *mlxsw_pci,
 	skb_put(skb, byte_count);
 	mlxsw_core_skb_receive(mlxsw_pci->core, skb, &rx_info);
 
-put_new_skb:
 	memset(wqe, 0, q->elem_size);
 	err = mlxsw_pci_rdq_skb_alloc(mlxsw_pci, elem_info);
 	if (err)
@@ -708,10 +711,6 @@ put_new_skb:
 	q->producer_counter++;
 	mlxsw_pci_queue_doorbell_producer_ring(mlxsw_pci, q);
 	return;
-
-drop:
-	dev_kfree_skb_any(skb);
-	goto put_new_skb;
 }
 
 static char *mlxsw_pci_cq_sw_cqe_get(struct mlxsw_pci_queue *q)
diff --git a/drivers/net/ethernet/mellanox/mlxsw/pci.h b/drivers/net/ethernet/mellanox/mlxsw/pci.h
index 142f33d978c5..912106054ff2 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/pci.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/pci.h
@@ -129,13 +129,15 @@ MLXSW_ITEM64_INDEXED(pci, wqe, address, 0x08, 0, 64, 0x8, 0x0, false);
  */
 MLXSW_ITEM32(pci, cqe, lag, 0x00, 23, 1);
 
-/* pci_cqe_system_port
+/* pci_cqe_system_port/lag_id
  * When lag=0: System port on which the packet was received
  * When lag=1:
  * bits [15:4] LAG ID on which the packet was received
  * bits [3:0] sub_port on which the packet was received
  */
 MLXSW_ITEM32(pci, cqe, system_port, 0x00, 0, 16);
+MLXSW_ITEM32(pci, cqe, lag_id, 0x00, 4, 12);
+MLXSW_ITEM32(pci, cqe, lag_port_index, 0x00, 0, 4);
 
 /* pci_cqe_wqe_counter
  * WQE count of the WQEs completed on the associated dqn
diff --git a/drivers/net/ethernet/mellanox/mlxsw/port.h b/drivers/net/ethernet/mellanox/mlxsw/port.h
index 726f5435b32f..ae65b9940aed 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/port.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/port.h
@@ -49,7 +49,7 @@
 #define MLXSW_PORT_MID			0xd000
 
 #define MLXSW_PORT_MAX_PHY_PORTS	0x40
-#define MLXSW_PORT_MAX_PORTS		MLXSW_PORT_MAX_PHY_PORTS
+#define MLXSW_PORT_MAX_PORTS		(MLXSW_PORT_MAX_PHY_PORTS + 1)
 
 #define MLXSW_PORT_DEVID_BITS_OFFSET	10
 #define MLXSW_PORT_PHY_BITS_OFFSET	4
diff --git a/drivers/net/ethernet/mellanox/mlxsw/reg.h b/drivers/net/ethernet/mellanox/mlxsw/reg.h
index 236fb5d2ad69..ffe4c0305733 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/reg.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/reg.h
@@ -99,6 +99,55 @@ static const struct mlxsw_reg_info mlxsw_reg_spad = {
  */
 MLXSW_ITEM_BUF(reg, spad, base_mac, 0x02, 6);
 
+/* SMID - Switch Multicast ID
+ * --------------------------
+ * The MID record maps from a MID (Multicast ID), which is a unique identifier
+ * of the multicast group within the stacking domain, into a list of local
+ * ports into which the packet is replicated.
+ */
+#define MLXSW_REG_SMID_ID 0x2007
+#define MLXSW_REG_SMID_LEN 0x240
+
+static const struct mlxsw_reg_info mlxsw_reg_smid = {
+	.id = MLXSW_REG_SMID_ID,
+	.len = MLXSW_REG_SMID_LEN,
+};
+
+/* reg_smid_swid
+ * Switch partition ID.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, smid, swid, 0x00, 24, 8);
+
+/* reg_smid_mid
+ * Multicast identifier - global identifier that represents the multicast group
+ * across all devices.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, smid, mid, 0x00, 0, 16);
+
+/* reg_smid_port
+ * Local port memebership (1 bit per port).
+ * Access: RW
+ */
+MLXSW_ITEM_BIT_ARRAY(reg, smid, port, 0x20, 0x20, 1);
+
+/* reg_smid_port_mask
+ * Local port mask (1 bit per port).
+ * Access: W
+ */
+MLXSW_ITEM_BIT_ARRAY(reg, smid, port_mask, 0x220, 0x20, 1);
+
+static inline void mlxsw_reg_smid_pack(char *payload, u16 mid,
+				       u8 port, bool set)
+{
+	MLXSW_REG_ZERO(smid, payload);
+	mlxsw_reg_smid_swid_set(payload, 0);
+	mlxsw_reg_smid_mid_set(payload, mid);
+	mlxsw_reg_smid_port_set(payload, port, set);
+	mlxsw_reg_smid_port_mask_set(payload, port, 1);
+}
+
 /* SSPR - Switch System Port Record Register
  * -----------------------------------------
  * Configures the system port to local port mapping.
@@ -286,6 +335,8 @@ MLXSW_ITEM32_INDEXED(reg, sfd, rec_swid, MLXSW_REG_SFD_BASE_LEN, 24, 8,
 
 enum mlxsw_reg_sfd_rec_type {
 	MLXSW_REG_SFD_REC_TYPE_UNICAST = 0x0,
+	MLXSW_REG_SFD_REC_TYPE_UNICAST_LAG = 0x1,
+	MLXSW_REG_SFD_REC_TYPE_MULTICAST = 0x2,
 };
 
 /* reg_sfd_rec_type
@@ -376,36 +427,144 @@ MLXSW_ITEM32_INDEXED(reg, sfd, uc_fid_vid, MLXSW_REG_SFD_BASE_LEN, 0, 16,
 MLXSW_ITEM32_INDEXED(reg, sfd, uc_system_port, MLXSW_REG_SFD_BASE_LEN, 0, 16,
 		     MLXSW_REG_SFD_REC_LEN, 0x0C, false);
 
-static inline void mlxsw_reg_sfd_uc_pack(char *payload, int rec_index,
-					 enum mlxsw_reg_sfd_rec_policy policy,
-					 const char *mac, u16 vid,
-					 enum mlxsw_reg_sfd_rec_action action,
-					 u8 local_port)
+static inline void mlxsw_reg_sfd_rec_pack(char *payload, int rec_index,
+					  enum mlxsw_reg_sfd_rec_type rec_type,
+					  const char *mac,
+					  enum mlxsw_reg_sfd_rec_action action)
 {
 	u8 num_rec = mlxsw_reg_sfd_num_rec_get(payload);
 
 	if (rec_index >= num_rec)
 		mlxsw_reg_sfd_num_rec_set(payload, rec_index + 1);
 	mlxsw_reg_sfd_rec_swid_set(payload, rec_index, 0);
-	mlxsw_reg_sfd_rec_type_set(payload, rec_index,
-				   MLXSW_REG_SFD_REC_TYPE_UNICAST);
-	mlxsw_reg_sfd_rec_policy_set(payload, rec_index, policy);
+	mlxsw_reg_sfd_rec_type_set(payload, rec_index, rec_type);
 	mlxsw_reg_sfd_rec_mac_memcpy_to(payload, rec_index, mac);
-	mlxsw_reg_sfd_uc_sub_port_set(payload, rec_index, 0);
-	mlxsw_reg_sfd_uc_fid_vid_set(payload, rec_index, vid);
 	mlxsw_reg_sfd_rec_action_set(payload, rec_index, action);
+}
+
+static inline void mlxsw_reg_sfd_uc_pack(char *payload, int rec_index,
+					 enum mlxsw_reg_sfd_rec_policy policy,
+					 const char *mac, u16 fid_vid,
+					 enum mlxsw_reg_sfd_rec_action action,
+					 u8 local_port)
+{
+	mlxsw_reg_sfd_rec_pack(payload, rec_index,
+			       MLXSW_REG_SFD_REC_TYPE_UNICAST, mac, action);
+	mlxsw_reg_sfd_rec_policy_set(payload, rec_index, policy);
+	mlxsw_reg_sfd_uc_sub_port_set(payload, rec_index, 0);
+	mlxsw_reg_sfd_uc_fid_vid_set(payload, rec_index, fid_vid);
 	mlxsw_reg_sfd_uc_system_port_set(payload, rec_index, local_port);
 }
 
 static inline void mlxsw_reg_sfd_uc_unpack(char *payload, int rec_index,
-					   char *mac, u16 *p_vid,
+					   char *mac, u16 *p_fid_vid,
 					   u8 *p_local_port)
 {
 	mlxsw_reg_sfd_rec_mac_memcpy_from(payload, rec_index, mac);
-	*p_vid = mlxsw_reg_sfd_uc_fid_vid_get(payload, rec_index);
+	*p_fid_vid = mlxsw_reg_sfd_uc_fid_vid_get(payload, rec_index);
 	*p_local_port = mlxsw_reg_sfd_uc_system_port_get(payload, rec_index);
 }
 
+/* reg_sfd_uc_lag_sub_port
+ * LAG sub port.
+ * Must be 0 if multichannel VEPA is not enabled.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, uc_lag_sub_port, MLXSW_REG_SFD_BASE_LEN, 16, 8,
+		     MLXSW_REG_SFD_REC_LEN, 0x08, false);
+
+/* reg_sfd_uc_lag_fid_vid
+ * Filtering ID or VLAN ID
+ * For SwitchX and SwitchX-2:
+ * - Dynamic entries (policy 2,3) use FID
+ * - Static entries (policy 0) use VID
+ * - When independent learning is configured, VID=FID
+ * For Spectrum: use FID for both Dynamic and Static entries.
+ * VID should not be used.
+ * Access: Index
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, uc_lag_fid_vid, MLXSW_REG_SFD_BASE_LEN, 0, 16,
+		     MLXSW_REG_SFD_REC_LEN, 0x08, false);
+
+/* reg_sfd_uc_lag_lag_vid
+ * Indicates VID in case of vFIDs. Reserved for FIDs.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, uc_lag_lag_vid, MLXSW_REG_SFD_BASE_LEN, 16, 12,
+		     MLXSW_REG_SFD_REC_LEN, 0x0C, false);
+
+/* reg_sfd_uc_lag_lag_id
+ * LAG Identifier - pointer into the LAG descriptor table.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, uc_lag_lag_id, MLXSW_REG_SFD_BASE_LEN, 0, 10,
+		     MLXSW_REG_SFD_REC_LEN, 0x0C, false);
+
+static inline void
+mlxsw_reg_sfd_uc_lag_pack(char *payload, int rec_index,
+			  enum mlxsw_reg_sfd_rec_policy policy,
+			  const char *mac, u16 fid_vid,
+			  enum mlxsw_reg_sfd_rec_action action, u16 lag_vid,
+			  u16 lag_id)
+{
+	mlxsw_reg_sfd_rec_pack(payload, rec_index,
+			       MLXSW_REG_SFD_REC_TYPE_UNICAST_LAG,
+			       mac, action);
+	mlxsw_reg_sfd_rec_policy_set(payload, rec_index, policy);
+	mlxsw_reg_sfd_uc_lag_sub_port_set(payload, rec_index, 0);
+	mlxsw_reg_sfd_uc_lag_fid_vid_set(payload, rec_index, fid_vid);
+	mlxsw_reg_sfd_uc_lag_lag_vid_set(payload, rec_index, lag_vid);
+	mlxsw_reg_sfd_uc_lag_lag_id_set(payload, rec_index, lag_id);
+}
+
+static inline void mlxsw_reg_sfd_uc_lag_unpack(char *payload, int rec_index,
+					       char *mac, u16 *p_vid,
+					       u16 *p_lag_id)
+{
+	mlxsw_reg_sfd_rec_mac_memcpy_from(payload, rec_index, mac);
+	*p_vid = mlxsw_reg_sfd_uc_lag_fid_vid_get(payload, rec_index);
+	*p_lag_id = mlxsw_reg_sfd_uc_lag_lag_id_get(payload, rec_index);
+}
+
+/* reg_sfd_mc_pgi
+ *
+ * Multicast port group index - index into the port group table.
+ * Value 0x1FFF indicates the pgi should point to the MID entry.
+ * For Spectrum this value must be set to 0x1FFF
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, mc_pgi, MLXSW_REG_SFD_BASE_LEN, 16, 13,
+		     MLXSW_REG_SFD_REC_LEN, 0x08, false);
+
+/* reg_sfd_mc_fid_vid
+ *
+ * Filtering ID or VLAN ID
+ * Access: Index
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, mc_fid_vid, MLXSW_REG_SFD_BASE_LEN, 0, 16,
+		     MLXSW_REG_SFD_REC_LEN, 0x08, false);
+
+/* reg_sfd_mc_mid
+ *
+ * Multicast identifier - global identifier that represents the multicast
+ * group across all devices.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sfd, mc_mid, MLXSW_REG_SFD_BASE_LEN, 0, 16,
+		     MLXSW_REG_SFD_REC_LEN, 0x0C, false);
+
+static inline void
+mlxsw_reg_sfd_mc_pack(char *payload, int rec_index,
+		      const char *mac, u16 fid_vid,
+		      enum mlxsw_reg_sfd_rec_action action, u16 mid)
+{
+	mlxsw_reg_sfd_rec_pack(payload, rec_index,
+			       MLXSW_REG_SFD_REC_TYPE_MULTICAST, mac, action);
+	mlxsw_reg_sfd_mc_pgi_set(payload, rec_index, 0x1FFF);
+	mlxsw_reg_sfd_mc_fid_vid_set(payload, rec_index, fid_vid);
+	mlxsw_reg_sfd_mc_mid_set(payload, rec_index, mid);
+}
+
 /* SFN - Switch FDB Notification Register
  * -------------------------------------------
  * The switch provides notifications on newly learned FDB entries and
@@ -456,8 +615,12 @@ MLXSW_ITEM32_INDEXED(reg, sfn, rec_swid, MLXSW_REG_SFN_BASE_LEN, 24, 8,
 enum mlxsw_reg_sfn_rec_type {
 	/* MAC addresses learned on a regular port. */
 	MLXSW_REG_SFN_REC_TYPE_LEARNED_MAC = 0x5,
-	/* Aged-out MAC address on a regular port */
+	/* MAC addresses learned on a LAG port. */
+	MLXSW_REG_SFN_REC_TYPE_LEARNED_MAC_LAG = 0x6,
+	/* Aged-out MAC address on a regular port. */
 	MLXSW_REG_SFN_REC_TYPE_AGED_OUT_MAC = 0x7,
+	/* Aged-out MAC address on a LAG port. */
+	MLXSW_REG_SFN_REC_TYPE_AGED_OUT_MAC_LAG = 0x8,
 };
 
 /* reg_sfn_rec_type
@@ -505,6 +668,22 @@ static inline void mlxsw_reg_sfn_mac_unpack(char *payload, int rec_index,
 	*p_local_port = mlxsw_reg_sfn_mac_system_port_get(payload, rec_index);
 }
 
+/* reg_sfn_mac_lag_lag_id
+ * LAG ID (pointer into the LAG descriptor table).
+ * Access: RO
+ */
+MLXSW_ITEM32_INDEXED(reg, sfn, mac_lag_lag_id, MLXSW_REG_SFN_BASE_LEN, 0, 10,
+		     MLXSW_REG_SFN_REC_LEN, 0x0C, false);
+
+static inline void mlxsw_reg_sfn_mac_lag_unpack(char *payload, int rec_index,
+						char *mac, u16 *p_vid,
+						u16 *p_lag_id)
+{
+	mlxsw_reg_sfn_rec_mac_memcpy_from(payload, rec_index, mac);
+	*p_vid = mlxsw_reg_sfn_mac_fid_get(payload, rec_index);
+	*p_lag_id = mlxsw_reg_sfn_mac_lag_lag_id_get(payload, rec_index);
+}
+
 /* SPMS - Switch Port MSTP/RSTP State Register
  * -------------------------------------------
  * Configures the spanning tree state of a physical port.
@@ -694,6 +873,62 @@ static inline void mlxsw_reg_spvm_pack(char *payload, u8 local_port,
 	}
 }
 
+/* SPAFT - Switch Port Acceptable Frame Types
+ * ------------------------------------------
+ * The Switch Port Acceptable Frame Types register configures the frame
+ * admittance of the port.
+ */
+#define MLXSW_REG_SPAFT_ID 0x2010
+#define MLXSW_REG_SPAFT_LEN 0x08
+
+static const struct mlxsw_reg_info mlxsw_reg_spaft = {
+	.id = MLXSW_REG_SPAFT_ID,
+	.len = MLXSW_REG_SPAFT_LEN,
+};
+
+/* reg_spaft_local_port
+ * Local port number.
+ * Access: Index
+ *
+ * Note: CPU port is not supported (all tag types are allowed).
+ */
+MLXSW_ITEM32(reg, spaft, local_port, 0x00, 16, 8);
+
+/* reg_spaft_sub_port
+ * Virtual port within the physical port.
+ * Should be set to 0 when virtual ports are not enabled on the port.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, spaft, sub_port, 0x00, 8, 8);
+
+/* reg_spaft_allow_untagged
+ * When set, untagged frames on the ingress are allowed (default).
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, spaft, allow_untagged, 0x04, 31, 1);
+
+/* reg_spaft_allow_prio_tagged
+ * When set, priority tagged frames on the ingress are allowed (default).
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, spaft, allow_prio_tagged, 0x04, 30, 1);
+
+/* reg_spaft_allow_tagged
+ * When set, tagged frames on the ingress are allowed (default).
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, spaft, allow_tagged, 0x04, 29, 1);
+
+static inline void mlxsw_reg_spaft_pack(char *payload, u8 local_port,
+					bool allow_untagged)
+{
+	MLXSW_REG_ZERO(spaft, payload);
+	mlxsw_reg_spaft_local_port_set(payload, local_port);
+	mlxsw_reg_spaft_allow_untagged_set(payload, allow_untagged);
+	mlxsw_reg_spaft_allow_prio_tagged_set(payload, true);
+	mlxsw_reg_spaft_allow_tagged_set(payload, true);
+}
+
 /* SFGC - Switch Flooding Group Configuration
  * ------------------------------------------
  * The following register controls the association of flooding tables and MIDs
@@ -865,6 +1100,379 @@ static inline void mlxsw_reg_sftr_pack(char *payload,
 	mlxsw_reg_sftr_port_mask_set(payload, port, 1);
 }
 
+/* SFDF - Switch Filtering DB Flush
+ * --------------------------------
+ * The switch filtering DB flush register is used to flush the FDB.
+ * Note that FDB notifications are flushed as well.
+ */
+#define MLXSW_REG_SFDF_ID 0x2013
+#define MLXSW_REG_SFDF_LEN 0x14
+
+static const struct mlxsw_reg_info mlxsw_reg_sfdf = {
+	.id = MLXSW_REG_SFDF_ID,
+	.len = MLXSW_REG_SFDF_LEN,
+};
+
+/* reg_sfdf_swid
+ * Switch partition ID.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, sfdf, swid, 0x00, 24, 8);
+
+enum mlxsw_reg_sfdf_flush_type {
+	MLXSW_REG_SFDF_FLUSH_PER_SWID,
+	MLXSW_REG_SFDF_FLUSH_PER_FID,
+	MLXSW_REG_SFDF_FLUSH_PER_PORT,
+	MLXSW_REG_SFDF_FLUSH_PER_PORT_AND_FID,
+	MLXSW_REG_SFDF_FLUSH_PER_LAG,
+	MLXSW_REG_SFDF_FLUSH_PER_LAG_AND_FID,
+};
+
+/* reg_sfdf_flush_type
+ * Flush type.
+ * 0 - All SWID dynamic entries are flushed.
+ * 1 - All FID dynamic entries are flushed.
+ * 2 - All dynamic entries pointing to port are flushed.
+ * 3 - All FID dynamic entries pointing to port are flushed.
+ * 4 - All dynamic entries pointing to LAG are flushed.
+ * 5 - All FID dynamic entries pointing to LAG are flushed.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, flush_type, 0x04, 28, 4);
+
+/* reg_sfdf_flush_static
+ * Static.
+ * 0 - Flush only dynamic entries.
+ * 1 - Flush both dynamic and static entries.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, flush_static, 0x04, 24, 1);
+
+static inline void mlxsw_reg_sfdf_pack(char *payload,
+				       enum mlxsw_reg_sfdf_flush_type type)
+{
+	MLXSW_REG_ZERO(sfdf, payload);
+	mlxsw_reg_sfdf_flush_type_set(payload, type);
+	mlxsw_reg_sfdf_flush_static_set(payload, true);
+}
+
+/* reg_sfdf_fid
+ * FID to flush.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, fid, 0x0C, 0, 16);
+
+/* reg_sfdf_system_port
+ * Port to flush.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, system_port, 0x0C, 0, 16);
+
+/* reg_sfdf_port_fid_system_port
+ * Port to flush, pointed to by FID.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, port_fid_system_port, 0x08, 0, 16);
+
+/* reg_sfdf_lag_id
+ * LAG ID to flush.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, lag_id, 0x0C, 0, 10);
+
+/* reg_sfdf_lag_fid_lag_id
+ * LAG ID to flush, pointed to by FID.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sfdf, lag_fid_lag_id, 0x08, 0, 10);
+
+/* SLDR - Switch LAG Descriptor Register
+ * -----------------------------------------
+ * The switch LAG descriptor register is populated by LAG descriptors.
+ * Each LAG descriptor is indexed by lag_id. The LAG ID runs from 0 to
+ * max_lag-1.
+ */
+#define MLXSW_REG_SLDR_ID 0x2014
+#define MLXSW_REG_SLDR_LEN 0x0C /* counting in only one port in list */
+
+static const struct mlxsw_reg_info mlxsw_reg_sldr = {
+	.id = MLXSW_REG_SLDR_ID,
+	.len = MLXSW_REG_SLDR_LEN,
+};
+
+enum mlxsw_reg_sldr_op {
+	/* Indicates a creation of a new LAG-ID, lag_id must be valid */
+	MLXSW_REG_SLDR_OP_LAG_CREATE,
+	MLXSW_REG_SLDR_OP_LAG_DESTROY,
+	/* Ports that appear in the list have the Distributor enabled */
+	MLXSW_REG_SLDR_OP_LAG_ADD_PORT_LIST,
+	/* Removes ports from the disributor list */
+	MLXSW_REG_SLDR_OP_LAG_REMOVE_PORT_LIST,
+};
+
+/* reg_sldr_op
+ * Operation.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sldr, op, 0x00, 29, 3);
+
+/* reg_sldr_lag_id
+ * LAG identifier. The lag_id is the index into the LAG descriptor table.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, sldr, lag_id, 0x00, 0, 10);
+
+static inline void mlxsw_reg_sldr_lag_create_pack(char *payload, u8 lag_id)
+{
+	MLXSW_REG_ZERO(sldr, payload);
+	mlxsw_reg_sldr_op_set(payload, MLXSW_REG_SLDR_OP_LAG_CREATE);
+	mlxsw_reg_sldr_lag_id_set(payload, lag_id);
+}
+
+static inline void mlxsw_reg_sldr_lag_destroy_pack(char *payload, u8 lag_id)
+{
+	MLXSW_REG_ZERO(sldr, payload);
+	mlxsw_reg_sldr_op_set(payload, MLXSW_REG_SLDR_OP_LAG_DESTROY);
+	mlxsw_reg_sldr_lag_id_set(payload, lag_id);
+}
+
+/* reg_sldr_num_ports
+ * The number of member ports of the LAG.
+ * Reserved for Create / Destroy operations
+ * For Add / Remove operations - indicates the number of ports in the list.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, sldr, num_ports, 0x04, 24, 8);
+
+/* reg_sldr_system_port
+ * System port.
+ * Access: RW
+ */
+MLXSW_ITEM32_INDEXED(reg, sldr, system_port, 0x08, 0, 16, 4, 0, false);
+
+static inline void mlxsw_reg_sldr_lag_add_port_pack(char *payload, u8 lag_id,
+						    u8 local_port)
+{
+	MLXSW_REG_ZERO(sldr, payload);
+	mlxsw_reg_sldr_op_set(payload, MLXSW_REG_SLDR_OP_LAG_ADD_PORT_LIST);
+	mlxsw_reg_sldr_lag_id_set(payload, lag_id);
+	mlxsw_reg_sldr_num_ports_set(payload, 1);
+	mlxsw_reg_sldr_system_port_set(payload, 0, local_port);
+}
+
+static inline void mlxsw_reg_sldr_lag_remove_port_pack(char *payload, u8 lag_id,
+						       u8 local_port)
+{
+	MLXSW_REG_ZERO(sldr, payload);
+	mlxsw_reg_sldr_op_set(payload, MLXSW_REG_SLDR_OP_LAG_REMOVE_PORT_LIST);
+	mlxsw_reg_sldr_lag_id_set(payload, lag_id);
+	mlxsw_reg_sldr_num_ports_set(payload, 1);
+	mlxsw_reg_sldr_system_port_set(payload, 0, local_port);
+}
+
+/* SLCR - Switch LAG Configuration 2 Register
+ * -------------------------------------------
+ * The Switch LAG Configuration register is used for configuring the
+ * LAG properties of the switch.
+ */
+#define MLXSW_REG_SLCR_ID 0x2015
+#define MLXSW_REG_SLCR_LEN 0x10
+
+static const struct mlxsw_reg_info mlxsw_reg_slcr = {
+	.id = MLXSW_REG_SLCR_ID,
+	.len = MLXSW_REG_SLCR_LEN,
+};
+
+enum mlxsw_reg_slcr_pp {
+	/* Global Configuration (for all ports) */
+	MLXSW_REG_SLCR_PP_GLOBAL,
+	/* Per port configuration, based on local_port field */
+	MLXSW_REG_SLCR_PP_PER_PORT,
+};
+
+/* reg_slcr_pp
+ * Per Port Configuration
+ * Note: Reading at Global mode results in reading port 1 configuration.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, slcr, pp, 0x00, 24, 1);
+
+/* reg_slcr_local_port
+ * Local port number
+ * Supported from CPU port
+ * Not supported from router port
+ * Reserved when pp = Global Configuration
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, slcr, local_port, 0x00, 16, 8);
+
+enum mlxsw_reg_slcr_type {
+	MLXSW_REG_SLCR_TYPE_CRC, /* default */
+	MLXSW_REG_SLCR_TYPE_XOR,
+	MLXSW_REG_SLCR_TYPE_RANDOM,
+};
+
+/* reg_slcr_type
+ * Hash type
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, slcr, type, 0x00, 0, 4);
+
+/* Ingress port */
+#define MLXSW_REG_SLCR_LAG_HASH_IN_PORT		BIT(0)
+/* SMAC - for IPv4 and IPv6 packets */
+#define MLXSW_REG_SLCR_LAG_HASH_SMAC_IP		BIT(1)
+/* SMAC - for non-IP packets */
+#define MLXSW_REG_SLCR_LAG_HASH_SMAC_NONIP	BIT(2)
+#define MLXSW_REG_SLCR_LAG_HASH_SMAC \
+	(MLXSW_REG_SLCR_LAG_HASH_SMAC_IP | \
+	 MLXSW_REG_SLCR_LAG_HASH_SMAC_NONIP)
+/* DMAC - for IPv4 and IPv6 packets */
+#define MLXSW_REG_SLCR_LAG_HASH_DMAC_IP		BIT(3)
+/* DMAC - for non-IP packets */
+#define MLXSW_REG_SLCR_LAG_HASH_DMAC_NONIP	BIT(4)
+#define MLXSW_REG_SLCR_LAG_HASH_DMAC \
+	(MLXSW_REG_SLCR_LAG_HASH_DMAC_IP | \
+	 MLXSW_REG_SLCR_LAG_HASH_DMAC_NONIP)
+/* Ethertype - for IPv4 and IPv6 packets */
+#define MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE_IP	BIT(5)
+/* Ethertype - for non-IP packets */
+#define MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE_NONIP	BIT(6)
+#define MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE \
+	(MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE_IP | \
+	 MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE_NONIP)
+/* VLAN ID - for IPv4 and IPv6 packets */
+#define MLXSW_REG_SLCR_LAG_HASH_VLANID_IP	BIT(7)
+/* VLAN ID - for non-IP packets */
+#define MLXSW_REG_SLCR_LAG_HASH_VLANID_NONIP	BIT(8)
+#define MLXSW_REG_SLCR_LAG_HASH_VLANID \
+	(MLXSW_REG_SLCR_LAG_HASH_VLANID_IP | \
+	 MLXSW_REG_SLCR_LAG_HASH_VLANID_NONIP)
+/* Source IP address (can be IPv4 or IPv6) */
+#define MLXSW_REG_SLCR_LAG_HASH_SIP		BIT(9)
+/* Destination IP address (can be IPv4 or IPv6) */
+#define MLXSW_REG_SLCR_LAG_HASH_DIP		BIT(10)
+/* TCP/UDP source port */
+#define MLXSW_REG_SLCR_LAG_HASH_SPORT		BIT(11)
+/* TCP/UDP destination port*/
+#define MLXSW_REG_SLCR_LAG_HASH_DPORT		BIT(12)
+/* IPv4 Protocol/IPv6 Next Header */
+#define MLXSW_REG_SLCR_LAG_HASH_IPPROTO		BIT(13)
+/* IPv6 Flow label */
+#define MLXSW_REG_SLCR_LAG_HASH_FLOWLABEL	BIT(14)
+/* SID - FCoE source ID */
+#define MLXSW_REG_SLCR_LAG_HASH_FCOE_SID	BIT(15)
+/* DID - FCoE destination ID */
+#define MLXSW_REG_SLCR_LAG_HASH_FCOE_DID	BIT(16)
+/* OXID - FCoE originator exchange ID */
+#define MLXSW_REG_SLCR_LAG_HASH_FCOE_OXID	BIT(17)
+/* Destination QP number - for RoCE packets */
+#define MLXSW_REG_SLCR_LAG_HASH_ROCE_DQP	BIT(19)
+
+/* reg_slcr_lag_hash
+ * LAG hashing configuration. This is a bitmask, in which each set
+ * bit includes the corresponding item in the LAG hash calculation.
+ * The default lag_hash contains SMAC, DMAC, VLANID and
+ * Ethertype (for all packet types).
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, slcr, lag_hash, 0x04, 0, 20);
+
+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_lag_hash_set(payload, lag_hash);
+}
+
+/* SLCOR - Switch LAG Collector Register
+ * -------------------------------------
+ * The Switch LAG Collector register controls the Local Port membership
+ * in a LAG and enablement of the collector.
+ */
+#define MLXSW_REG_SLCOR_ID 0x2016
+#define MLXSW_REG_SLCOR_LEN 0x10
+
+static const struct mlxsw_reg_info mlxsw_reg_slcor = {
+	.id = MLXSW_REG_SLCOR_ID,
+	.len = MLXSW_REG_SLCOR_LEN,
+};
+
+enum mlxsw_reg_slcor_col {
+	/* Port is added with collector disabled */
+	MLXSW_REG_SLCOR_COL_LAG_ADD_PORT,
+	MLXSW_REG_SLCOR_COL_LAG_COLLECTOR_ENABLED,
+	MLXSW_REG_SLCOR_COL_LAG_COLLECTOR_DISABLED,
+	MLXSW_REG_SLCOR_COL_LAG_REMOVE_PORT,
+};
+
+/* reg_slcor_col
+ * Collector configuration
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, slcor, col, 0x00, 30, 2);
+
+/* reg_slcor_local_port
+ * Local port number
+ * Not supported for CPU port
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, slcor, local_port, 0x00, 16, 8);
+
+/* reg_slcor_lag_id
+ * LAG Identifier. Index into the LAG descriptor table.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, slcor, lag_id, 0x00, 0, 10);
+
+/* reg_slcor_port_index
+ * Port index in the LAG list. Only valid on Add Port to LAG col.
+ * Valid range is from 0 to cap_max_lag_members-1
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, slcor, port_index, 0x04, 0, 10);
+
+static inline void mlxsw_reg_slcor_pack(char *payload,
+					u8 local_port, u16 lag_id,
+					enum mlxsw_reg_slcor_col col)
+{
+	MLXSW_REG_ZERO(slcor, payload);
+	mlxsw_reg_slcor_col_set(payload, col);
+	mlxsw_reg_slcor_local_port_set(payload, local_port);
+	mlxsw_reg_slcor_lag_id_set(payload, lag_id);
+}
+
+static inline void mlxsw_reg_slcor_port_add_pack(char *payload,
+						 u8 local_port, u16 lag_id,
+						 u8 port_index)
+{
+	mlxsw_reg_slcor_pack(payload, local_port, lag_id,
+			     MLXSW_REG_SLCOR_COL_LAG_ADD_PORT);
+	mlxsw_reg_slcor_port_index_set(payload, port_index);
+}
+
+static inline void mlxsw_reg_slcor_port_remove_pack(char *payload,
+						    u8 local_port, u16 lag_id)
+{
+	mlxsw_reg_slcor_pack(payload, local_port, lag_id,
+			     MLXSW_REG_SLCOR_COL_LAG_REMOVE_PORT);
+}
+
+static inline void mlxsw_reg_slcor_col_enable_pack(char *payload,
+						   u8 local_port, u16 lag_id)
+{
+	mlxsw_reg_slcor_pack(payload, local_port, lag_id,
+			     MLXSW_REG_SLCOR_COL_LAG_COLLECTOR_ENABLED);
+}
+
+static inline void mlxsw_reg_slcor_col_disable_pack(char *payload,
+						    u8 local_port, u16 lag_id)
+{
+	mlxsw_reg_slcor_pack(payload, local_port, lag_id,
+			     MLXSW_REG_SLCOR_COL_LAG_COLLECTOR_ENABLED);
+}
+
 /* SPMLR - Switch Port MAC Learning Register
  * -----------------------------------------
  * Controls the Switch MAC learning policy per port.
@@ -1235,20 +1843,20 @@ MLXSW_ITEM32(reg, pmlp, width, 0x00, 0, 8);
  * Module number.
  * Access: RW
  */
-MLXSW_ITEM32_INDEXED(reg, pmlp, module, 0x04, 0, 8, 0x04, 0, false);
+MLXSW_ITEM32_INDEXED(reg, pmlp, module, 0x04, 0, 8, 0x04, 0x00, false);
 
 /* reg_pmlp_tx_lane
  * Tx Lane. When rxtx field is cleared, this field is used for Rx as well.
  * Access: RW
  */
-MLXSW_ITEM32_INDEXED(reg, pmlp, tx_lane, 0x04, 16, 2, 0x04, 16, false);
+MLXSW_ITEM32_INDEXED(reg, pmlp, tx_lane, 0x04, 16, 2, 0x04, 0x00, false);
 
 /* reg_pmlp_rx_lane
  * Rx Lane. When rxtx field is cleared, this field is ignored and Rx lane is
  * equal to Tx lane.
  * Access: RW
  */
-MLXSW_ITEM32_INDEXED(reg, pmlp, rx_lane, 0x04, 24, 2, 0x04, 24, false);
+MLXSW_ITEM32_INDEXED(reg, pmlp, rx_lane, 0x04, 24, 2, 0x04, 0x00, false);
 
 static inline void mlxsw_reg_pmlp_pack(char *payload, u8 local_port)
 {
@@ -2087,6 +2695,284 @@ static inline void mlxsw_reg_hpkt_pack(char *payload, u8 action, u16 trap_id)
 	mlxsw_reg_hpkt_ctrl_set(payload, MLXSW_REG_HPKT_CTRL_PACKET_DEFAULT);
 }
 
+/* MFCR - Management Fan Control Register
+ * --------------------------------------
+ * This register controls the settings of the Fan Speed PWM mechanism.
+ */
+#define MLXSW_REG_MFCR_ID 0x9001
+#define MLXSW_REG_MFCR_LEN 0x08
+
+static const struct mlxsw_reg_info mlxsw_reg_mfcr = {
+	.id = MLXSW_REG_MFCR_ID,
+	.len = MLXSW_REG_MFCR_LEN,
+};
+
+enum mlxsw_reg_mfcr_pwm_frequency {
+	MLXSW_REG_MFCR_PWM_FEQ_11HZ = 0x00,
+	MLXSW_REG_MFCR_PWM_FEQ_14_7HZ = 0x01,
+	MLXSW_REG_MFCR_PWM_FEQ_22_1HZ = 0x02,
+	MLXSW_REG_MFCR_PWM_FEQ_1_4KHZ = 0x40,
+	MLXSW_REG_MFCR_PWM_FEQ_5KHZ = 0x41,
+	MLXSW_REG_MFCR_PWM_FEQ_20KHZ = 0x42,
+	MLXSW_REG_MFCR_PWM_FEQ_22_5KHZ = 0x43,
+	MLXSW_REG_MFCR_PWM_FEQ_25KHZ = 0x44,
+};
+
+/* reg_mfcr_pwm_frequency
+ * Controls the frequency of the PWM signal.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mfcr, pwm_frequency, 0x00, 0, 6);
+
+#define MLXSW_MFCR_TACHOS_MAX 10
+
+/* reg_mfcr_tacho_active
+ * Indicates which of the tachometer is active (bit per tachometer).
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mfcr, tacho_active, 0x04, 16, MLXSW_MFCR_TACHOS_MAX);
+
+#define MLXSW_MFCR_PWMS_MAX 5
+
+/* reg_mfcr_pwm_active
+ * Indicates which of the PWM control is active (bit per PWM).
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mfcr, pwm_active, 0x04, 0, MLXSW_MFCR_PWMS_MAX);
+
+static inline void
+mlxsw_reg_mfcr_pack(char *payload,
+		    enum mlxsw_reg_mfcr_pwm_frequency pwm_frequency)
+{
+	MLXSW_REG_ZERO(mfcr, payload);
+	mlxsw_reg_mfcr_pwm_frequency_set(payload, pwm_frequency);
+}
+
+static inline void
+mlxsw_reg_mfcr_unpack(char *payload,
+		      enum mlxsw_reg_mfcr_pwm_frequency *p_pwm_frequency,
+		      u16 *p_tacho_active, u8 *p_pwm_active)
+{
+	*p_pwm_frequency = mlxsw_reg_mfcr_pwm_frequency_get(payload);
+	*p_tacho_active = mlxsw_reg_mfcr_tacho_active_get(payload);
+	*p_pwm_active = mlxsw_reg_mfcr_pwm_active_get(payload);
+}
+
+/* MFSC - Management Fan Speed Control Register
+ * --------------------------------------------
+ * This register controls the settings of the Fan Speed PWM mechanism.
+ */
+#define MLXSW_REG_MFSC_ID 0x9002
+#define MLXSW_REG_MFSC_LEN 0x08
+
+static const struct mlxsw_reg_info mlxsw_reg_mfsc = {
+	.id = MLXSW_REG_MFSC_ID,
+	.len = MLXSW_REG_MFSC_LEN,
+};
+
+/* reg_mfsc_pwm
+ * Fan pwm to control / monitor.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mfsc, pwm, 0x00, 24, 3);
+
+/* reg_mfsc_pwm_duty_cycle
+ * Controls the duty cycle of the PWM. Value range from 0..255 to
+ * represent duty cycle of 0%...100%.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mfsc, pwm_duty_cycle, 0x04, 0, 8);
+
+static inline void mlxsw_reg_mfsc_pack(char *payload, u8 pwm,
+				       u8 pwm_duty_cycle)
+{
+	MLXSW_REG_ZERO(mfsc, payload);
+	mlxsw_reg_mfsc_pwm_set(payload, pwm);
+	mlxsw_reg_mfsc_pwm_duty_cycle_set(payload, pwm_duty_cycle);
+}
+
+/* MFSM - Management Fan Speed Measurement
+ * ---------------------------------------
+ * This register controls the settings of the Tacho measurements and
+ * enables reading the Tachometer measurements.
+ */
+#define MLXSW_REG_MFSM_ID 0x9003
+#define MLXSW_REG_MFSM_LEN 0x08
+
+static const struct mlxsw_reg_info mlxsw_reg_mfsm = {
+	.id = MLXSW_REG_MFSM_ID,
+	.len = MLXSW_REG_MFSM_LEN,
+};
+
+/* reg_mfsm_tacho
+ * Fan tachometer index.
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mfsm, tacho, 0x00, 24, 4);
+
+/* reg_mfsm_rpm
+ * Fan speed (round per minute).
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mfsm, rpm, 0x04, 0, 16);
+
+static inline void mlxsw_reg_mfsm_pack(char *payload, u8 tacho)
+{
+	MLXSW_REG_ZERO(mfsm, payload);
+	mlxsw_reg_mfsm_tacho_set(payload, tacho);
+}
+
+/* MTCAP - Management Temperature Capabilities
+ * -------------------------------------------
+ * This register exposes the capabilities of the device and
+ * system temperature sensing.
+ */
+#define MLXSW_REG_MTCAP_ID 0x9009
+#define MLXSW_REG_MTCAP_LEN 0x08
+
+static const struct mlxsw_reg_info mlxsw_reg_mtcap = {
+	.id = MLXSW_REG_MTCAP_ID,
+	.len = MLXSW_REG_MTCAP_LEN,
+};
+
+/* reg_mtcap_sensor_count
+ * Number of sensors supported by the device.
+ * This includes the QSFP module sensors (if exists in the QSFP module).
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mtcap, sensor_count, 0x00, 0, 7);
+
+/* MTMP - Management Temperature
+ * -----------------------------
+ * This register controls the settings of the temperature measurements
+ * and enables reading the temperature measurements. Note that temperature
+ * is in 0.125 degrees Celsius.
+ */
+#define MLXSW_REG_MTMP_ID 0x900A
+#define MLXSW_REG_MTMP_LEN 0x20
+
+static const struct mlxsw_reg_info mlxsw_reg_mtmp = {
+	.id = MLXSW_REG_MTMP_ID,
+	.len = MLXSW_REG_MTMP_LEN,
+};
+
+/* reg_mtmp_sensor_index
+ * Sensors index to access.
+ * 64-127 of sensor_index are mapped to the SFP+/QSFP modules sequentially
+ * (module 0 is mapped to sensor_index 64).
+ * Access: Index
+ */
+MLXSW_ITEM32(reg, mtmp, sensor_index, 0x00, 0, 7);
+
+/* Convert to milli degrees Celsius */
+#define MLXSW_REG_MTMP_TEMP_TO_MC(val) (val * 125)
+
+/* reg_mtmp_temperature
+ * Temperature reading from the sensor. Reading is in 0.125 Celsius
+ * degrees units.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mtmp, temperature, 0x04, 0, 16);
+
+/* reg_mtmp_mte
+ * Max Temperature Enable - enables measuring the max temperature on a sensor.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mtmp, mte, 0x08, 31, 1);
+
+/* reg_mtmp_mtr
+ * Max Temperature Reset - clears the value of the max temperature register.
+ * Access: WO
+ */
+MLXSW_ITEM32(reg, mtmp, mtr, 0x08, 30, 1);
+
+/* reg_mtmp_max_temperature
+ * The highest measured temperature from the sensor.
+ * When the bit mte is cleared, the field max_temperature is reserved.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mtmp, max_temperature, 0x08, 0, 16);
+
+#define MLXSW_REG_MTMP_SENSOR_NAME_SIZE 8
+
+/* reg_mtmp_sensor_name
+ * Sensor Name
+ * Access: RO
+ */
+MLXSW_ITEM_BUF(reg, mtmp, sensor_name, 0x18, MLXSW_REG_MTMP_SENSOR_NAME_SIZE);
+
+static inline void mlxsw_reg_mtmp_pack(char *payload, u8 sensor_index,
+				       bool max_temp_enable,
+				       bool max_temp_reset)
+{
+	MLXSW_REG_ZERO(mtmp, payload);
+	mlxsw_reg_mtmp_sensor_index_set(payload, sensor_index);
+	mlxsw_reg_mtmp_mte_set(payload, max_temp_enable);
+	mlxsw_reg_mtmp_mtr_set(payload, max_temp_reset);
+}
+
+static inline void mlxsw_reg_mtmp_unpack(char *payload, unsigned int *p_temp,
+					 unsigned int *p_max_temp,
+					 char *sensor_name)
+{
+	u16 temp;
+
+	if (p_temp) {
+		temp = mlxsw_reg_mtmp_temperature_get(payload);
+		*p_temp = MLXSW_REG_MTMP_TEMP_TO_MC(temp);
+	}
+	if (p_max_temp) {
+		temp = mlxsw_reg_mtmp_max_temperature_get(payload);
+		*p_max_temp = MLXSW_REG_MTMP_TEMP_TO_MC(temp);
+	}
+	if (sensor_name)
+		mlxsw_reg_mtmp_sensor_name_memcpy_from(payload, sensor_name);
+}
+
+/* MLCR - Management LED Control Register
+ * --------------------------------------
+ * Controls the system LEDs.
+ */
+#define MLXSW_REG_MLCR_ID 0x902B
+#define MLXSW_REG_MLCR_LEN 0x0C
+
+static const struct mlxsw_reg_info mlxsw_reg_mlcr = {
+	.id = MLXSW_REG_MLCR_ID,
+	.len = MLXSW_REG_MLCR_LEN,
+};
+
+/* reg_mlcr_local_port
+ * Local port number.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mlcr, local_port, 0x00, 16, 8);
+
+#define MLXSW_REG_MLCR_DURATION_MAX 0xFFFF
+
+/* reg_mlcr_beacon_duration
+ * Duration of the beacon to be active, in seconds.
+ * 0x0 - Will turn off the beacon.
+ * 0xFFFF - Will turn on the beacon until explicitly turned off.
+ * Access: RW
+ */
+MLXSW_ITEM32(reg, mlcr, beacon_duration, 0x04, 0, 16);
+
+/* reg_mlcr_beacon_remain
+ * Remaining duration of the beacon, in seconds.
+ * 0xFFFF indicates an infinite amount of time.
+ * Access: RO
+ */
+MLXSW_ITEM32(reg, mlcr, beacon_remain, 0x08, 0, 16);
+
+static inline void mlxsw_reg_mlcr_pack(char *payload, u8 local_port,
+				       bool active)
+{
+	MLXSW_REG_ZERO(mlcr, payload);
+	mlxsw_reg_mlcr_local_port_set(payload, local_port);
+	mlxsw_reg_mlcr_beacon_duration_set(payload, active ?
+					   MLXSW_REG_MLCR_DURATION_MAX : 0);
+}
+
 /* SBPR - Shared Buffer Pools Register
  * -----------------------------------
  * The SBPR configures and retrieves the shared buffer pools and configuration.
@@ -2357,6 +3243,8 @@ static inline const char *mlxsw_reg_id_str(u16 reg_id)
 		return "SGCR";
 	case MLXSW_REG_SPAD_ID:
 		return "SPAD";
+	case MLXSW_REG_SMID_ID:
+		return "SMID";
 	case MLXSW_REG_SSPR_ID:
 		return "SSPR";
 	case MLXSW_REG_SFDAT_ID:
@@ -2371,10 +3259,20 @@ static inline const char *mlxsw_reg_id_str(u16 reg_id)
 		return "SPVID";
 	case MLXSW_REG_SPVM_ID:
 		return "SPVM";
+	case MLXSW_REG_SPAFT_ID:
+		return "SPAFT";
 	case MLXSW_REG_SFGC_ID:
 		return "SFGC";
 	case MLXSW_REG_SFTR_ID:
 		return "SFTR";
+	case MLXSW_REG_SFDF_ID:
+		return "SFDF";
+	case MLXSW_REG_SLDR_ID:
+		return "SLDR";
+	case MLXSW_REG_SLCR_ID:
+		return "SLCR";
+	case MLXSW_REG_SLCOR_ID:
+		return "SLCOR";
 	case MLXSW_REG_SPMLR_ID:
 		return "SPMLR";
 	case MLXSW_REG_SVFA_ID:
@@ -2405,6 +3303,18 @@ static inline const char *mlxsw_reg_id_str(u16 reg_id)
 		return "HTGT";
 	case MLXSW_REG_HPKT_ID:
 		return "HPKT";
+	case MLXSW_REG_MFCR_ID:
+		return "MFCR";
+	case MLXSW_REG_MFSC_ID:
+		return "MFSC";
+	case MLXSW_REG_MFSM_ID:
+		return "MFSM";
+	case MLXSW_REG_MTCAP_ID:
+		return "MTCAP";
+	case MLXSW_REG_MTMP_ID:
+		return "MTMP";
+	case MLXSW_REG_MLCR_ID:
+		return "MLCR";
 	case MLXSW_REG_SBPR_ID:
 		return "SBPR";
 	case MLXSW_REG_SBCM_ID:
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
index 3be4a2355ead..a94daa8c346c 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.c
@@ -48,6 +48,7 @@
 #include <linux/workqueue.h>
 #include <linux/jiffies.h>
 #include <linux/bitops.h>
+#include <linux/list.h>
 #include <net/switchdev.h>
 #include <generated/utsrelease.h>
 
@@ -186,33 +187,6 @@ static int mlxsw_sp_port_oper_status_get(struct mlxsw_sp_port *mlxsw_sp_port,
 	return 0;
 }
 
-static int mlxsw_sp_vfid_create(struct mlxsw_sp *mlxsw_sp, u16 vfid)
-{
-	char sfmr_pl[MLXSW_REG_SFMR_LEN];
-	int err;
-
-	mlxsw_reg_sfmr_pack(sfmr_pl, MLXSW_REG_SFMR_OP_CREATE_FID,
-			    MLXSW_SP_VFID_BASE + vfid, 0);
-	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
-
-	if (err)
-		return err;
-
-	set_bit(vfid, mlxsw_sp->active_vfids);
-	return 0;
-}
-
-static void mlxsw_sp_vfid_destroy(struct mlxsw_sp *mlxsw_sp, u16 vfid)
-{
-	char sfmr_pl[MLXSW_REG_SFMR_LEN];
-
-	clear_bit(vfid, mlxsw_sp->active_vfids);
-
-	mlxsw_reg_sfmr_pack(sfmr_pl, MLXSW_REG_SFMR_OP_DESTROY_FID,
-			    MLXSW_SP_VFID_BASE + vfid, 0);
-	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
-}
-
 static int mlxsw_sp_port_dev_addr_set(struct mlxsw_sp_port *mlxsw_sp_port,
 				      unsigned char *addr)
 {
@@ -417,6 +391,10 @@ static netdev_tx_t mlxsw_sp_port_xmit(struct sk_buff *skb,
 	return NETDEV_TX_OK;
 }
 
+static void mlxsw_sp_set_rx_mode(struct net_device *dev)
+{
+}
+
 static int mlxsw_sp_port_set_mac_address(struct net_device *dev, void *p)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
@@ -545,12 +523,132 @@ static int mlxsw_sp_port_vlan_mode_trans(struct mlxsw_sp_port *mlxsw_sp_port)
 	return 0;
 }
 
+static struct mlxsw_sp_vfid *
+mlxsw_sp_vfid_find(const struct mlxsw_sp *mlxsw_sp, u16 vid)
+{
+	struct mlxsw_sp_vfid *vfid;
+
+	list_for_each_entry(vfid, &mlxsw_sp->port_vfids.list, list) {
+		if (vfid->vid == vid)
+			return vfid;
+	}
+
+	return NULL;
+}
+
+static u16 mlxsw_sp_avail_vfid_get(const struct mlxsw_sp *mlxsw_sp)
+{
+	return find_first_zero_bit(mlxsw_sp->port_vfids.mapped,
+				   MLXSW_SP_VFID_PORT_MAX);
+}
+
+static int __mlxsw_sp_vfid_create(struct mlxsw_sp *mlxsw_sp, u16 vfid)
+{
+	u16 fid = mlxsw_sp_vfid_to_fid(vfid);
+	char sfmr_pl[MLXSW_REG_SFMR_LEN];
+
+	mlxsw_reg_sfmr_pack(sfmr_pl, MLXSW_REG_SFMR_OP_CREATE_FID, fid, 0);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
+}
+
+static void __mlxsw_sp_vfid_destroy(struct mlxsw_sp *mlxsw_sp, u16 vfid)
+{
+	u16 fid = mlxsw_sp_vfid_to_fid(vfid);
+	char sfmr_pl[MLXSW_REG_SFMR_LEN];
+
+	mlxsw_reg_sfmr_pack(sfmr_pl, MLXSW_REG_SFMR_OP_DESTROY_FID, fid, 0);
+	mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfmr), sfmr_pl);
+}
+
+static struct mlxsw_sp_vfid *mlxsw_sp_vfid_create(struct mlxsw_sp *mlxsw_sp,
+						  u16 vid)
+{
+	struct device *dev = mlxsw_sp->bus_info->dev;
+	struct mlxsw_sp_vfid *vfid;
+	u16 n_vfid;
+	int err;
+
+	n_vfid = mlxsw_sp_avail_vfid_get(mlxsw_sp);
+	if (n_vfid == MLXSW_SP_VFID_PORT_MAX) {
+		dev_err(dev, "No available vFIDs\n");
+		return ERR_PTR(-ERANGE);
+	}
+
+	err = __mlxsw_sp_vfid_create(mlxsw_sp, n_vfid);
+	if (err) {
+		dev_err(dev, "Failed to create vFID=%d\n", n_vfid);
+		return ERR_PTR(err);
+	}
+
+	vfid = kzalloc(sizeof(*vfid), GFP_KERNEL);
+	if (!vfid)
+		goto err_allocate_vfid;
+
+	vfid->vfid = n_vfid;
+	vfid->vid = vid;
+
+	list_add(&vfid->list, &mlxsw_sp->port_vfids.list);
+	set_bit(n_vfid, mlxsw_sp->port_vfids.mapped);
+
+	return vfid;
+
+err_allocate_vfid:
+	__mlxsw_sp_vfid_destroy(mlxsw_sp, n_vfid);
+	return ERR_PTR(-ENOMEM);
+}
+
+static void mlxsw_sp_vfid_destroy(struct mlxsw_sp *mlxsw_sp,
+				  struct mlxsw_sp_vfid *vfid)
+{
+	clear_bit(vfid->vfid, mlxsw_sp->port_vfids.mapped);
+	list_del(&vfid->list);
+
+	__mlxsw_sp_vfid_destroy(mlxsw_sp, vfid->vfid);
+
+	kfree(vfid);
+}
+
+static struct mlxsw_sp_port *
+mlxsw_sp_port_vport_create(struct mlxsw_sp_port *mlxsw_sp_port,
+			   struct mlxsw_sp_vfid *vfid)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+
+	mlxsw_sp_vport = kzalloc(sizeof(*mlxsw_sp_vport), GFP_KERNEL);
+	if (!mlxsw_sp_vport)
+		return NULL;
+
+	/* dev will be set correctly after the VLAN device is linked
+	 * with the real device. In case of bridge SELF invocation, dev
+	 * will remain as is.
+	 */
+	mlxsw_sp_vport->dev = mlxsw_sp_port->dev;
+	mlxsw_sp_vport->mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	mlxsw_sp_vport->local_port = mlxsw_sp_port->local_port;
+	mlxsw_sp_vport->stp_state = BR_STATE_FORWARDING;
+	mlxsw_sp_vport->lagged = mlxsw_sp_port->lagged;
+	mlxsw_sp_vport->lag_id = mlxsw_sp_port->lag_id;
+	mlxsw_sp_vport->vport.vfid = vfid;
+	mlxsw_sp_vport->vport.vid = vfid->vid;
+
+	list_add(&mlxsw_sp_vport->vport.list, &mlxsw_sp_port->vports_list);
+
+	return mlxsw_sp_vport;
+}
+
+static void mlxsw_sp_port_vport_destroy(struct mlxsw_sp_port *mlxsw_sp_vport)
+{
+	list_del(&mlxsw_sp_vport->vport.list);
+	kfree(mlxsw_sp_vport);
+}
+
 int mlxsw_sp_port_add_vid(struct net_device *dev, __be16 __always_unused proto,
 			  u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	char *sftr_pl;
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct mlxsw_sp_vfid *vfid;
 	int err;
 
 	/* VLAN 0 is added to HW filter when device goes up, but it is
@@ -559,100 +657,105 @@ int mlxsw_sp_port_add_vid(struct net_device *dev, __be16 __always_unused proto,
 	if (!vid)
 		return 0;
 
-	if (test_bit(vid, mlxsw_sp_port->active_vfids)) {
+	if (mlxsw_sp_port_vport_find(mlxsw_sp_port, vid)) {
 		netdev_warn(dev, "VID=%d already configured\n", vid);
 		return 0;
 	}
 
-	if (!test_bit(vid, mlxsw_sp->active_vfids)) {
-		err = mlxsw_sp_vfid_create(mlxsw_sp, vid);
-		if (err) {
-			netdev_err(dev, "Failed to create vFID=%d\n",
-				   MLXSW_SP_VFID_BASE + vid);
-			return err;
+	vfid = mlxsw_sp_vfid_find(mlxsw_sp, vid);
+	if (!vfid) {
+		vfid = mlxsw_sp_vfid_create(mlxsw_sp, vid);
+		if (IS_ERR(vfid)) {
+			netdev_err(dev, "Failed to create vFID for VID=%d\n",
+				   vid);
+			return PTR_ERR(vfid);
 		}
+	}
 
-		sftr_pl = kmalloc(MLXSW_REG_SFTR_LEN, GFP_KERNEL);
-		if (!sftr_pl) {
-			err = -ENOMEM;
-			goto err_flood_table_alloc;
-		}
-		mlxsw_reg_sftr_pack(sftr_pl, 0, vid,
-				    MLXSW_REG_SFGC_TABLE_TYPE_FID, 0,
-				    MLXSW_PORT_CPU_PORT, true);
-		err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sftr), sftr_pl);
-		kfree(sftr_pl);
+	mlxsw_sp_vport = mlxsw_sp_port_vport_create(mlxsw_sp_port, vfid);
+	if (!mlxsw_sp_vport) {
+		netdev_err(dev, "Failed to create vPort for VID=%d\n", vid);
+		err = -ENOMEM;
+		goto err_port_vport_create;
+	}
+
+	if (!vfid->nr_vports) {
+		err = mlxsw_sp_vport_flood_set(mlxsw_sp_vport, vfid->vfid,
+					       true, false);
 		if (err) {
-			netdev_err(dev, "Failed to configure flood table\n");
-			goto err_flood_table_config;
+			netdev_err(dev, "Failed to setup flooding for vFID=%d\n",
+				   vfid->vfid);
+			goto err_vport_flood_set;
 		}
 	}
 
-	/* In case we fail in the following steps, we intentionally do not
-	 * destroy the associated vFID.
-	 */
-
 	/* When adding the first VLAN interface on a bridged port we need to
 	 * transition all the active 802.1Q bridge VLANs to use explicit
 	 * {Port, VID} to FID mappings and set the port's mode to Virtual mode.
 	 */
-	if (!mlxsw_sp_port->nr_vfids) {
+	if (list_is_singular(&mlxsw_sp_port->vports_list)) {
 		err = mlxsw_sp_port_vp_mode_trans(mlxsw_sp_port);
 		if (err) {
 			netdev_err(dev, "Failed to set to Virtual mode\n");
-			return err;
+			goto err_port_vp_mode_trans;
 		}
 	}
 
-	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port,
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
 					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
-					   true, MLXSW_SP_VFID_BASE + vid, vid);
+					   true,
+					   mlxsw_sp_vfid_to_fid(vfid->vfid),
+					   vid);
 	if (err) {
 		netdev_err(dev, "Failed to map {Port, VID=%d} to vFID=%d\n",
-			   vid, MLXSW_SP_VFID_BASE + vid);
+			   vid, vfid->vfid);
 		goto err_port_vid_to_fid_set;
 	}
 
-	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, false);
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, false);
 	if (err) {
 		netdev_err(dev, "Failed to disable learning for VID=%d\n", vid);
 		goto err_port_vid_learning_set;
 	}
 
-	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, true, false);
+	err = mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, true, false);
 	if (err) {
 		netdev_err(dev, "Failed to set VLAN membership for VID=%d\n",
 			   vid);
 		goto err_port_add_vid;
 	}
 
-	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_port, vid,
+	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_vport, vid,
 					  MLXSW_REG_SPMS_STATE_FORWARDING);
 	if (err) {
 		netdev_err(dev, "Failed to set STP state for VID=%d\n", vid);
 		goto err_port_stp_state_set;
 	}
 
-	mlxsw_sp_port->nr_vfids++;
-	set_bit(vid, mlxsw_sp_port->active_vfids);
+	vfid->nr_vports++;
 
 	return 0;
 
-err_flood_table_config:
-err_flood_table_alloc:
-	mlxsw_sp_vfid_destroy(mlxsw_sp, vid);
-	return err;
-
 err_port_stp_state_set:
-	mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+	mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, false, false);
 err_port_add_vid:
-	mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, true);
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
 err_port_vid_learning_set:
-	mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port,
+	mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
 				     MLXSW_REG_SVFA_MT_PORT_VID_TO_FID, false,
-				     MLXSW_SP_VFID_BASE + vid, vid);
+				     mlxsw_sp_vfid_to_fid(vfid->vfid), vid);
 err_port_vid_to_fid_set:
-	mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
+	if (list_is_singular(&mlxsw_sp_port->vports_list))
+		mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
+err_port_vp_mode_trans:
+	if (!vfid->nr_vports)
+		mlxsw_sp_vport_flood_set(mlxsw_sp_vport, vfid->vfid, false,
+					 false);
+err_vport_flood_set:
+	mlxsw_sp_port_vport_destroy(mlxsw_sp_vport);
+err_port_vport_create:
+	if (!vfid->nr_vports)
+		mlxsw_sp_vfid_destroy(mlxsw_sp, vfid);
 	return err;
 }
 
@@ -660,6 +763,8 @@ int mlxsw_sp_port_kill_vid(struct net_device *dev,
 			   __be16 __always_unused proto, u16 vid)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct mlxsw_sp_vfid *vfid;
 	int err;
 
 	/* VLAN 0 is removed from HW filter when device goes down, but
@@ -668,38 +773,42 @@ int mlxsw_sp_port_kill_vid(struct net_device *dev,
 	if (!vid)
 		return 0;
 
-	if (!test_bit(vid, mlxsw_sp_port->active_vfids)) {
+	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
+	if (!mlxsw_sp_vport) {
 		netdev_warn(dev, "VID=%d does not exist\n", vid);
 		return 0;
 	}
 
-	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_port, vid,
+	vfid = mlxsw_sp_vport->vport.vfid;
+
+	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_vport, vid,
 					  MLXSW_REG_SPMS_STATE_DISCARDING);
 	if (err) {
 		netdev_err(dev, "Failed to set STP state for VID=%d\n", vid);
 		return err;
 	}
 
-	err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid, false, false);
+	err = mlxsw_sp_port_vlan_set(mlxsw_sp_vport, vid, vid, false, false);
 	if (err) {
 		netdev_err(dev, "Failed to set VLAN membership for VID=%d\n",
 			   vid);
 		return err;
 	}
 
-	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_port, vid, true);
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
 	if (err) {
 		netdev_err(dev, "Failed to enable learning for VID=%d\n", vid);
 		return err;
 	}
 
-	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_port,
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
 					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
-					   false, MLXSW_SP_VFID_BASE + vid,
+					   false,
+					   mlxsw_sp_vfid_to_fid(vfid->vfid),
 					   vid);
 	if (err) {
 		netdev_err(dev, "Failed to invalidate {Port, VID=%d} to vFID=%d mapping\n",
-			   vid, MLXSW_SP_VFID_BASE + vid);
+			   vid, vfid->vfid);
 		return err;
 	}
 
@@ -707,7 +816,7 @@ int mlxsw_sp_port_kill_vid(struct net_device *dev,
 	 * transition all active 802.1Q bridge VLANs to use VID to FID
 	 * mappings and set port's mode to VLAN mode.
 	 */
-	if (mlxsw_sp_port->nr_vfids == 1) {
+	if (list_is_singular(&mlxsw_sp_port->vports_list)) {
 		err = mlxsw_sp_port_vlan_mode_trans(mlxsw_sp_port);
 		if (err) {
 			netdev_err(dev, "Failed to set to VLAN mode\n");
@@ -715,8 +824,12 @@ int mlxsw_sp_port_kill_vid(struct net_device *dev,
 		}
 	}
 
-	mlxsw_sp_port->nr_vfids--;
-	clear_bit(vid, mlxsw_sp_port->active_vfids);
+	vfid->nr_vports--;
+	mlxsw_sp_port_vport_destroy(mlxsw_sp_vport);
+
+	/* Destroy the vFID if no vPorts are assigned to it anymore. */
+	if (!vfid->nr_vports)
+		mlxsw_sp_vfid_destroy(mlxsw_sp_port->mlxsw_sp, vfid);
 
 	return 0;
 }
@@ -725,6 +838,7 @@ static const struct net_device_ops mlxsw_sp_port_netdev_ops = {
 	.ndo_open		= mlxsw_sp_port_open,
 	.ndo_stop		= mlxsw_sp_port_stop,
 	.ndo_start_xmit		= mlxsw_sp_port_xmit,
+	.ndo_set_rx_mode	= mlxsw_sp_set_rx_mode,
 	.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,
@@ -859,6 +973,29 @@ static void mlxsw_sp_port_get_strings(struct net_device *dev,
 	}
 }
 
+static int mlxsw_sp_port_set_phys_id(struct net_device *dev,
+				     enum ethtool_phys_id_state state)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char mlcr_pl[MLXSW_REG_MLCR_LEN];
+	bool active;
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		active = true;
+		break;
+	case ETHTOOL_ID_INACTIVE:
+		active = false;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	mlxsw_reg_mlcr_pack(mlcr_pl, mlxsw_sp_port->local_port, active);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mlcr), mlcr_pl);
+}
+
 static void mlxsw_sp_port_get_stats(struct net_device *dev,
 				    struct ethtool_stats *stats, u64 *data)
 {
@@ -1205,6 +1342,7 @@ static const struct ethtool_ops mlxsw_sp_port_ethtool_ops = {
 	.get_drvinfo		= mlxsw_sp_port_get_drvinfo,
 	.get_link		= ethtool_op_get_link,
 	.get_strings		= mlxsw_sp_port_get_strings,
+	.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,
@@ -1216,6 +1354,7 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 	struct mlxsw_sp_port *mlxsw_sp_port;
 	struct net_device *dev;
 	bool usable;
+	size_t bytes;
 	int err;
 
 	dev = alloc_etherdev(sizeof(struct mlxsw_sp_port));
@@ -1225,10 +1364,18 @@ static int mlxsw_sp_port_create(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 	mlxsw_sp_port->dev = dev;
 	mlxsw_sp_port->mlxsw_sp = mlxsw_sp;
 	mlxsw_sp_port->local_port = local_port;
-	mlxsw_sp_port->learning = 1;
-	mlxsw_sp_port->learning_sync = 1;
-	mlxsw_sp_port->uc_flood = 1;
-	mlxsw_sp_port->pvid = 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) {
+		err = -ENOMEM;
+		goto err_port_active_vlans_alloc;
+	}
+	mlxsw_sp_port->untagged_vlans = kzalloc(bytes, GFP_KERNEL);
+	if (!mlxsw_sp_port->untagged_vlans) {
+		err = -ENOMEM;
+		goto err_port_untagged_vlans_alloc;
+	}
+	INIT_LIST_HEAD(&mlxsw_sp_port->vports_list);
 
 	mlxsw_sp_port->pcpu_stats =
 		netdev_alloc_pcpu_stats(struct mlxsw_sp_port_pcpu_stats);
@@ -1330,16 +1477,29 @@ err_port_module_check:
 err_dev_addr_init:
 	free_percpu(mlxsw_sp_port->pcpu_stats);
 err_alloc_stats:
+	kfree(mlxsw_sp_port->untagged_vlans);
+err_port_untagged_vlans_alloc:
+	kfree(mlxsw_sp_port->active_vlans);
+err_port_active_vlans_alloc:
 	free_netdev(dev);
 	return err;
 }
 
-static void mlxsw_sp_vfids_fini(struct mlxsw_sp *mlxsw_sp)
+static void mlxsw_sp_port_vports_fini(struct mlxsw_sp_port *mlxsw_sp_port)
 {
-	u16 vfid;
+	struct net_device *dev = mlxsw_sp_port->dev;
+	struct mlxsw_sp_port *mlxsw_sp_vport, *tmp;
 
-	for_each_set_bit(vfid, mlxsw_sp->active_vfids, VLAN_N_VID)
-		mlxsw_sp_vfid_destroy(mlxsw_sp, vfid);
+	list_for_each_entry_safe(mlxsw_sp_vport, tmp,
+				 &mlxsw_sp_port->vports_list, vport.list) {
+		u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
+
+		/* vPorts created for VLAN devices should already be gone
+		 * by now, since we unregistered the port netdev.
+		 */
+		WARN_ON(is_vlan_dev(mlxsw_sp_vport->dev));
+		mlxsw_sp_port_kill_vid(dev, 0, vid);
+	}
 }
 
 static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
@@ -1348,10 +1508,12 @@ static void mlxsw_sp_port_remove(struct mlxsw_sp *mlxsw_sp, u8 local_port)
 
 	if (!mlxsw_sp_port)
 		return;
-	mlxsw_sp_port_kill_vid(mlxsw_sp_port->dev, 0, 1);
 	unregister_netdev(mlxsw_sp_port->dev); /* This calls ndo_stop */
+	mlxsw_sp_port_vports_fini(mlxsw_sp_port);
 	mlxsw_sp_port_switchdev_fini(mlxsw_sp_port);
 	free_percpu(mlxsw_sp_port->pcpu_stats);
+	kfree(mlxsw_sp_port->untagged_vlans);
+	kfree(mlxsw_sp_port->active_vlans);
 	free_netdev(mlxsw_sp_port->dev);
 }
 
@@ -1633,16 +1795,15 @@ static int __mlxsw_sp_flood_init(struct mlxsw_core *mlxsw_core,
 	enum mlxsw_sp_flood_table flood_table;
 	char sfgc_pl[MLXSW_REG_SFGC_LEN];
 
-	if (bridge_type == MLXSW_REG_SFGC_BRIDGE_TYPE_VFID) {
+	if (bridge_type == MLXSW_REG_SFGC_BRIDGE_TYPE_VFID)
 		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID;
-		flood_table = 0;
-	} else {
+	else
 		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST;
-		if (type == MLXSW_REG_SFGC_TYPE_UNKNOWN_UNICAST)
-			flood_table = MLXSW_SP_FLOOD_TABLE_UC;
-		else
-			flood_table = MLXSW_SP_FLOOD_TABLE_BM;
-	}
+
+	if (type == MLXSW_REG_SFGC_TYPE_UNKNOWN_UNICAST)
+		flood_table = MLXSW_SP_FLOOD_TABLE_UC;
+	else
+		flood_table = MLXSW_SP_FLOOD_TABLE_BM;
 
 	mlxsw_reg_sfgc_pack(sfgc_pl, type, bridge_type, table_type,
 			    flood_table);
@@ -1653,9 +1814,6 @@ static int mlxsw_sp_flood_init(struct mlxsw_sp *mlxsw_sp)
 {
 	int type, err;
 
-	/* For non-offloaded netdevs, flood all traffic types to CPU
-	 * port.
-	 */
 	for (type = 0; type < MLXSW_REG_SFGC_TYPE_MAX; type++) {
 		if (type == MLXSW_REG_SFGC_TYPE_RESERVED)
 			continue;
@@ -1664,15 +1822,6 @@ static int mlxsw_sp_flood_init(struct mlxsw_sp *mlxsw_sp)
 					    MLXSW_REG_SFGC_BRIDGE_TYPE_VFID);
 		if (err)
 			return err;
-	}
-
-	/* For bridged ports, use one flooding table for unknown unicast
-	 * traffic and a second table for unregistered multicast and
-	 * broadcast.
-	 */
-	for (type = 0; type < MLXSW_REG_SFGC_TYPE_MAX; type++) {
-		if (type == MLXSW_REG_SFGC_TYPE_RESERVED)
-			continue;
 
 		err = __mlxsw_sp_flood_init(mlxsw_sp->core, type,
 					    MLXSW_REG_SFGC_BRIDGE_TYPE_1Q_FID);
@@ -1683,6 +1832,22 @@ static int mlxsw_sp_flood_init(struct mlxsw_sp *mlxsw_sp)
 	return 0;
 }
 
+static int mlxsw_sp_lag_init(struct mlxsw_sp *mlxsw_sp)
+{
+	char slcr_pl[MLXSW_REG_SLCR_LEN];
+
+	mlxsw_reg_slcr_pack(slcr_pl, MLXSW_REG_SLCR_LAG_HASH_SMAC |
+				     MLXSW_REG_SLCR_LAG_HASH_DMAC |
+				     MLXSW_REG_SLCR_LAG_HASH_ETHERTYPE |
+				     MLXSW_REG_SLCR_LAG_HASH_VLANID |
+				     MLXSW_REG_SLCR_LAG_HASH_SIP |
+				     MLXSW_REG_SLCR_LAG_HASH_DIP |
+				     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);
+}
+
 static int mlxsw_sp_init(void *priv, struct mlxsw_core *mlxsw_core,
 			 const struct mlxsw_bus_info *mlxsw_bus_info)
 {
@@ -1691,6 +1856,9 @@ static int mlxsw_sp_init(void *priv, struct mlxsw_core *mlxsw_core,
 
 	mlxsw_sp->core = mlxsw_core;
 	mlxsw_sp->bus_info = mlxsw_bus_info;
+	INIT_LIST_HEAD(&mlxsw_sp->port_vfids.list);
+	INIT_LIST_HEAD(&mlxsw_sp->br_vfids.list);
+	INIT_LIST_HEAD(&mlxsw_sp->br_mids.list);
 
 	err = mlxsw_sp_base_mac_get(mlxsw_sp);
 	if (err) {
@@ -1701,7 +1869,7 @@ static int mlxsw_sp_init(void *priv, struct mlxsw_core *mlxsw_core,
 	err = mlxsw_sp_ports_create(mlxsw_sp);
 	if (err) {
 		dev_err(mlxsw_sp->bus_info->dev, "Failed to create ports\n");
-		goto err_ports_create;
+		return err;
 	}
 
 	err = mlxsw_sp_event_register(mlxsw_sp, MLXSW_TRAP_ID_PUDE);
@@ -1728,6 +1896,12 @@ static int mlxsw_sp_init(void *priv, struct mlxsw_core *mlxsw_core,
 		goto err_buffers_init;
 	}
 
+	err = mlxsw_sp_lag_init(mlxsw_sp);
+	if (err) {
+		dev_err(mlxsw_sp->bus_info->dev, "Failed to initialize LAG\n");
+		goto err_lag_init;
+	}
+
 	err = mlxsw_sp_switchdev_init(mlxsw_sp);
 	if (err) {
 		dev_err(mlxsw_sp->bus_info->dev, "Failed to initialize switchdev\n");
@@ -1737,6 +1911,7 @@ static int mlxsw_sp_init(void *priv, struct mlxsw_core *mlxsw_core,
 	return 0;
 
 err_switchdev_init:
+err_lag_init:
 err_buffers_init:
 err_flood_init:
 	mlxsw_sp_traps_fini(mlxsw_sp);
@@ -1744,8 +1919,6 @@ err_rx_listener_register:
 	mlxsw_sp_event_unregister(mlxsw_sp, MLXSW_TRAP_ID_PUDE);
 err_event_register:
 	mlxsw_sp_ports_remove(mlxsw_sp);
-err_ports_create:
-	mlxsw_sp_vfids_fini(mlxsw_sp);
 	return err;
 }
 
@@ -1757,18 +1930,17 @@ static void mlxsw_sp_fini(void *priv)
 	mlxsw_sp_traps_fini(mlxsw_sp);
 	mlxsw_sp_event_unregister(mlxsw_sp, MLXSW_TRAP_ID_PUDE);
 	mlxsw_sp_ports_remove(mlxsw_sp);
-	mlxsw_sp_vfids_fini(mlxsw_sp);
 }
 
 static struct mlxsw_config_profile mlxsw_sp_config_profile = {
 	.used_max_vepa_channels		= 1,
 	.max_vepa_channels		= 0,
 	.used_max_lag			= 1,
-	.max_lag			= 64,
+	.max_lag			= MLXSW_SP_LAG_MAX,
 	.used_max_port_per_lag		= 1,
-	.max_port_per_lag		= 16,
+	.max_port_per_lag		= MLXSW_SP_PORT_PER_LAG_MAX,
 	.used_max_mid			= 1,
-	.max_mid			= 7000,
+	.max_mid			= MLXSW_SP_MID_MAX,
 	.used_max_pgt			= 1,
 	.max_pgt			= 0,
 	.used_max_system_port		= 1,
@@ -1782,8 +1954,8 @@ static struct mlxsw_config_profile mlxsw_sp_config_profile = {
 	.flood_mode			= 3,
 	.max_fid_offset_flood_tables	= 2,
 	.fid_offset_flood_table_size	= VLAN_N_VID - 1,
-	.max_fid_flood_tables		= 1,
-	.fid_flood_table_size		= VLAN_N_VID,
+	.max_fid_flood_tables		= 2,
+	.fid_flood_table_size		= MLXSW_SP_VFID_MAX,
 	.used_max_ib_mc			= 1,
 	.max_ib_mc			= 0,
 	.used_max_pkey			= 1,
@@ -1807,6 +1979,115 @@ static struct mlxsw_driver mlxsw_sp_driver = {
 	.profile		= &mlxsw_sp_config_profile,
 };
 
+static int
+mlxsw_sp_port_fdb_flush_by_port(const struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sfdf_pl[MLXSW_REG_SFDF_LEN];
+
+	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_PORT);
+	mlxsw_reg_sfdf_system_port_set(sfdf_pl, mlxsw_sp_port->local_port);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
+}
+
+static int
+mlxsw_sp_port_fdb_flush_by_port_fid(const struct mlxsw_sp_port *mlxsw_sp_port,
+				    u16 fid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sfdf_pl[MLXSW_REG_SFDF_LEN];
+
+	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_PORT_AND_FID);
+	mlxsw_reg_sfdf_fid_set(sfdf_pl, fid);
+	mlxsw_reg_sfdf_port_fid_system_port_set(sfdf_pl,
+						mlxsw_sp_port->local_port);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
+}
+
+static int
+mlxsw_sp_port_fdb_flush_by_lag_id(const struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sfdf_pl[MLXSW_REG_SFDF_LEN];
+
+	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_LAG);
+	mlxsw_reg_sfdf_lag_id_set(sfdf_pl, mlxsw_sp_port->lag_id);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
+}
+
+static int
+mlxsw_sp_port_fdb_flush_by_lag_id_fid(const struct mlxsw_sp_port *mlxsw_sp_port,
+				      u16 fid)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sfdf_pl[MLXSW_REG_SFDF_LEN];
+
+	mlxsw_reg_sfdf_pack(sfdf_pl, MLXSW_REG_SFDF_FLUSH_PER_LAG_AND_FID);
+	mlxsw_reg_sfdf_fid_set(sfdf_pl, fid);
+	mlxsw_reg_sfdf_lag_fid_lag_id_set(sfdf_pl, mlxsw_sp_port->lag_id);
+
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfdf), sfdf_pl);
+}
+
+static int
+__mlxsw_sp_port_fdb_flush(const struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	int err, last_err = 0;
+	u16 vid;
+
+	for (vid = 1; vid < VLAN_N_VID - 1; vid++) {
+		err = mlxsw_sp_port_fdb_flush_by_port_fid(mlxsw_sp_port, vid);
+		if (err)
+			last_err = err;
+	}
+
+	return last_err;
+}
+
+static int
+__mlxsw_sp_port_fdb_flush_lagged(const struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	int err, last_err = 0;
+	u16 vid;
+
+	for (vid = 1; vid < VLAN_N_VID - 1; vid++) {
+		err = mlxsw_sp_port_fdb_flush_by_lag_id_fid(mlxsw_sp_port, vid);
+		if (err)
+			last_err = err;
+	}
+
+	return last_err;
+}
+
+static int mlxsw_sp_port_fdb_flush(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	if (!list_empty(&mlxsw_sp_port->vports_list))
+		if (mlxsw_sp_port->lagged)
+			return __mlxsw_sp_port_fdb_flush_lagged(mlxsw_sp_port);
+		else
+			return __mlxsw_sp_port_fdb_flush(mlxsw_sp_port);
+	else
+		if (mlxsw_sp_port->lagged)
+			return mlxsw_sp_port_fdb_flush_by_lag_id(mlxsw_sp_port);
+		else
+			return mlxsw_sp_port_fdb_flush_by_port(mlxsw_sp_port);
+}
+
+static int mlxsw_sp_vport_fdb_flush(struct mlxsw_sp_port *mlxsw_sp_vport)
+{
+	u16 vfid = mlxsw_sp_vport_vfid_get(mlxsw_sp_vport);
+	u16 fid = mlxsw_sp_vfid_to_fid(vfid);
+
+	if (mlxsw_sp_vport->lagged)
+		return mlxsw_sp_port_fdb_flush_by_lag_id_fid(mlxsw_sp_vport,
+							     fid);
+	else
+		return mlxsw_sp_port_fdb_flush_by_port_fid(mlxsw_sp_vport, fid);
+}
+
 static bool mlxsw_sp_port_dev_check(const struct net_device *dev)
 {
 	return dev->netdev_ops == &mlxsw_sp_port_netdev_ops;
@@ -1824,24 +2105,35 @@ static int mlxsw_sp_port_bridge_join(struct mlxsw_sp_port *mlxsw_sp_port)
 	 */
 	err = mlxsw_sp_port_kill_vid(dev, 0, 1);
 	if (err)
-		netdev_err(dev, "Failed to remove VID 1\n");
+		return err;
 
-	return err;
+	mlxsw_sp_port->learning = 1;
+	mlxsw_sp_port->learning_sync = 1;
+	mlxsw_sp_port->uc_flood = 1;
+	mlxsw_sp_port->bridged = 1;
+
+	return 0;
 }
 
-static int mlxsw_sp_port_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_port)
+static int mlxsw_sp_port_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_port,
+				      bool flush_fdb)
 {
 	struct net_device *dev = mlxsw_sp_port->dev;
-	int err;
+
+	if (flush_fdb && mlxsw_sp_port_fdb_flush(mlxsw_sp_port))
+		netdev_err(mlxsw_sp_port->dev, "Failed to flush FDB\n");
+
+	mlxsw_sp_port_pvid_set(mlxsw_sp_port, 1);
+
+	mlxsw_sp_port->learning = 0;
+	mlxsw_sp_port->learning_sync = 0;
+	mlxsw_sp_port->uc_flood = 0;
+	mlxsw_sp_port->bridged = 0;
 
 	/* Add implicit VLAN interface in the device, so that untagged
 	 * packets will be classified to the default vFID.
 	 */
-	err = mlxsw_sp_port_add_vid(dev, 0, 1);
-	if (err)
-		netdev_err(dev, "Failed to add VID 1\n");
-
-	return err;
+	return mlxsw_sp_port_add_vid(dev, 0, 1);
 }
 
 static bool mlxsw_sp_master_bridge_check(struct mlxsw_sp *mlxsw_sp,
@@ -1865,19 +2157,318 @@ static void mlxsw_sp_master_bridge_dec(struct mlxsw_sp *mlxsw_sp,
 		mlxsw_sp->master_bridge.dev = NULL;
 }
 
-static int mlxsw_sp_netdevice_event(struct notifier_block *unused,
-				    unsigned long event, void *ptr)
+static int mlxsw_sp_lag_create(struct mlxsw_sp *mlxsw_sp, u16 lag_id)
+{
+	char sldr_pl[MLXSW_REG_SLDR_LEN];
+
+	mlxsw_reg_sldr_lag_create_pack(sldr_pl, lag_id);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sldr), sldr_pl);
+}
+
+static int mlxsw_sp_lag_destroy(struct mlxsw_sp *mlxsw_sp, u16 lag_id)
+{
+	char sldr_pl[MLXSW_REG_SLDR_LEN];
+
+	mlxsw_reg_sldr_lag_destroy_pack(sldr_pl, lag_id);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sldr), sldr_pl);
+}
+
+static int mlxsw_sp_lag_col_port_add(struct mlxsw_sp_port *mlxsw_sp_port,
+				     u16 lag_id, u8 port_index)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char slcor_pl[MLXSW_REG_SLCOR_LEN];
+
+	mlxsw_reg_slcor_port_add_pack(slcor_pl, mlxsw_sp_port->local_port,
+				      lag_id, port_index);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcor), slcor_pl);
+}
+
+static int mlxsw_sp_lag_col_port_remove(struct mlxsw_sp_port *mlxsw_sp_port,
+					u16 lag_id)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char slcor_pl[MLXSW_REG_SLCOR_LEN];
+
+	mlxsw_reg_slcor_port_remove_pack(slcor_pl, mlxsw_sp_port->local_port,
+					 lag_id);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcor), slcor_pl);
+}
+
+static int mlxsw_sp_lag_col_port_enable(struct mlxsw_sp_port *mlxsw_sp_port,
+					u16 lag_id)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char slcor_pl[MLXSW_REG_SLCOR_LEN];
+
+	mlxsw_reg_slcor_col_enable_pack(slcor_pl, mlxsw_sp_port->local_port,
+					lag_id);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcor), slcor_pl);
+}
+
+static int mlxsw_sp_lag_col_port_disable(struct mlxsw_sp_port *mlxsw_sp_port,
+					 u16 lag_id)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char slcor_pl[MLXSW_REG_SLCOR_LEN];
+
+	mlxsw_reg_slcor_col_disable_pack(slcor_pl, mlxsw_sp_port->local_port,
+					 lag_id);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(slcor), slcor_pl);
+}
+
+static int mlxsw_sp_lag_index_get(struct mlxsw_sp *mlxsw_sp,
+				  struct net_device *lag_dev,
+				  u16 *p_lag_id)
+{
+	struct mlxsw_sp_upper *lag;
+	int free_lag_id = -1;
+	int i;
+
+	for (i = 0; i < MLXSW_SP_LAG_MAX; i++) {
+		lag = mlxsw_sp_lag_get(mlxsw_sp, i);
+		if (lag->ref_count) {
+			if (lag->dev == lag_dev) {
+				*p_lag_id = i;
+				return 0;
+			}
+		} else if (free_lag_id < 0) {
+			free_lag_id = i;
+		}
+	}
+	if (free_lag_id < 0)
+		return -EBUSY;
+	*p_lag_id = free_lag_id;
+	return 0;
+}
+
+static bool
+mlxsw_sp_master_lag_check(struct mlxsw_sp *mlxsw_sp,
+			  struct net_device *lag_dev,
+			  struct netdev_lag_upper_info *lag_upper_info)
+{
+	u16 lag_id;
+
+	if (mlxsw_sp_lag_index_get(mlxsw_sp, lag_dev, &lag_id) != 0)
+		return false;
+	if (lag_upper_info->tx_type != NETDEV_LAG_TX_TYPE_HASH)
+		return false;
+	return true;
+}
+
+static int mlxsw_sp_port_lag_index_get(struct mlxsw_sp *mlxsw_sp,
+				       u16 lag_id, u8 *p_port_index)
+{
+	int i;
+
+	for (i = 0; i < MLXSW_SP_PORT_PER_LAG_MAX; i++) {
+		if (!mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i)) {
+			*p_port_index = i;
+			return 0;
+		}
+	}
+	return -EBUSY;
+}
+
+static int mlxsw_sp_port_lag_join(struct mlxsw_sp_port *mlxsw_sp_port,
+				  struct net_device *lag_dev)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_upper *lag;
+	u16 lag_id;
+	u8 port_index;
+	int err;
+
+	err = mlxsw_sp_lag_index_get(mlxsw_sp, lag_dev, &lag_id);
+	if (err)
+		return err;
+	lag = mlxsw_sp_lag_get(mlxsw_sp, lag_id);
+	if (!lag->ref_count) {
+		err = mlxsw_sp_lag_create(mlxsw_sp, lag_id);
+		if (err)
+			return err;
+		lag->dev = lag_dev;
+	}
+
+	err = mlxsw_sp_port_lag_index_get(mlxsw_sp, lag_id, &port_index);
+	if (err)
+		return err;
+	err = mlxsw_sp_lag_col_port_add(mlxsw_sp_port, lag_id, port_index);
+	if (err)
+		goto err_col_port_add;
+	err = mlxsw_sp_lag_col_port_enable(mlxsw_sp_port, lag_id);
+	if (err)
+		goto err_col_port_enable;
+
+	mlxsw_core_lag_mapping_set(mlxsw_sp->core, lag_id, port_index,
+				   mlxsw_sp_port->local_port);
+	mlxsw_sp_port->lag_id = lag_id;
+	mlxsw_sp_port->lagged = 1;
+	lag->ref_count++;
+	return 0;
+
+err_col_port_add:
+	if (!lag->ref_count)
+		mlxsw_sp_lag_destroy(mlxsw_sp, lag_id);
+err_col_port_enable:
+	mlxsw_sp_lag_col_port_remove(mlxsw_sp_port, lag_id);
+	return err;
+}
+
+static int mlxsw_sp_vport_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_vport,
+				       struct net_device *br_dev,
+				       bool flush_fdb);
+
+static int mlxsw_sp_port_lag_leave(struct mlxsw_sp_port *mlxsw_sp_port,
+				   struct net_device *lag_dev)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct mlxsw_sp_upper *lag;
+	u16 lag_id = mlxsw_sp_port->lag_id;
+	int err;
+
+	if (!mlxsw_sp_port->lagged)
+		return 0;
+	lag = mlxsw_sp_lag_get(mlxsw_sp, lag_id);
+	WARN_ON(lag->ref_count == 0);
+
+	err = mlxsw_sp_lag_col_port_disable(mlxsw_sp_port, lag_id);
+	if (err)
+		return err;
+	err = mlxsw_sp_lag_col_port_remove(mlxsw_sp_port, lag_id);
+	if (err)
+		return err;
+
+	/* In case we leave a LAG device that has bridges built on top,
+	 * then their teardown sequence is never issued and we need to
+	 * invoke the necessary cleanup routines ourselves.
+	 */
+	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
+			    vport.list) {
+		struct net_device *br_dev;
+
+		if (!mlxsw_sp_vport->bridged)
+			continue;
+
+		br_dev = mlxsw_sp_vport_br_get(mlxsw_sp_vport);
+		mlxsw_sp_vport_bridge_leave(mlxsw_sp_vport, br_dev, false);
+	}
+
+	if (mlxsw_sp_port->bridged) {
+		mlxsw_sp_port_active_vlans_del(mlxsw_sp_port);
+		mlxsw_sp_port_bridge_leave(mlxsw_sp_port, false);
+		mlxsw_sp_master_bridge_dec(mlxsw_sp, NULL);
+	}
+
+	if (lag->ref_count == 1) {
+		if (mlxsw_sp_port_fdb_flush_by_lag_id(mlxsw_sp_port))
+			netdev_err(mlxsw_sp_port->dev, "Failed to flush FDB\n");
+		err = mlxsw_sp_lag_destroy(mlxsw_sp, lag_id);
+		if (err)
+			return err;
+	}
+
+	mlxsw_core_lag_mapping_clear(mlxsw_sp->core, lag_id,
+				     mlxsw_sp_port->local_port);
+	mlxsw_sp_port->lagged = 0;
+	lag->ref_count--;
+	return 0;
+}
+
+static int mlxsw_sp_lag_dist_port_add(struct mlxsw_sp_port *mlxsw_sp_port,
+				      u16 lag_id)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sldr_pl[MLXSW_REG_SLDR_LEN];
+
+	mlxsw_reg_sldr_lag_add_port_pack(sldr_pl, lag_id,
+					 mlxsw_sp_port->local_port);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sldr), sldr_pl);
+}
+
+static int mlxsw_sp_lag_dist_port_remove(struct mlxsw_sp_port *mlxsw_sp_port,
+					 u16 lag_id)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char sldr_pl[MLXSW_REG_SLDR_LEN];
+
+	mlxsw_reg_sldr_lag_remove_port_pack(sldr_pl, lag_id,
+					    mlxsw_sp_port->local_port);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sldr), sldr_pl);
+}
+
+static int mlxsw_sp_port_lag_tx_en_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				       bool lag_tx_enabled)
+{
+	if (lag_tx_enabled)
+		return mlxsw_sp_lag_dist_port_add(mlxsw_sp_port,
+						  mlxsw_sp_port->lag_id);
+	else
+		return mlxsw_sp_lag_dist_port_remove(mlxsw_sp_port,
+						     mlxsw_sp_port->lag_id);
+}
+
+static int mlxsw_sp_port_lag_changed(struct mlxsw_sp_port *mlxsw_sp_port,
+				     struct netdev_lag_lower_state_info *info)
+{
+	return mlxsw_sp_port_lag_tx_en_set(mlxsw_sp_port, info->tx_enabled);
+}
+
+static int mlxsw_sp_port_vlan_link(struct mlxsw_sp_port *mlxsw_sp_port,
+				   struct net_device *vlan_dev)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	u16 vid = vlan_dev_vlan_id(vlan_dev);
+
+	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
+	if (!mlxsw_sp_vport) {
+		WARN_ON(!mlxsw_sp_vport);
+		return -EINVAL;
+	}
+
+	mlxsw_sp_vport->dev = vlan_dev;
+
+	return 0;
+}
+
+static int mlxsw_sp_port_vlan_unlink(struct mlxsw_sp_port *mlxsw_sp_port,
+				     struct net_device *vlan_dev)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	u16 vid = vlan_dev_vlan_id(vlan_dev);
+
+	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
+	if (!mlxsw_sp_vport) {
+		WARN_ON(!mlxsw_sp_vport);
+		return -EINVAL;
+	}
+
+	/* When removing a VLAN device while still bridged we should first
+	 * remove it from the bridge, as we receive the bridge's notification
+	 * when the vPort is already gone.
+	 */
+	if (mlxsw_sp_vport->bridged) {
+		struct net_device *br_dev;
+
+		br_dev = mlxsw_sp_vport_br_get(mlxsw_sp_vport);
+		mlxsw_sp_vport_bridge_leave(mlxsw_sp_vport, br_dev, true);
+	}
+
+	mlxsw_sp_vport->dev = mlxsw_sp_port->dev;
+
+	return 0;
+}
+
+static int mlxsw_sp_netdevice_port_upper_event(struct net_device *dev,
+					       unsigned long event, void *ptr)
 {
-	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
 	struct netdev_notifier_changeupper_info *info;
 	struct mlxsw_sp_port *mlxsw_sp_port;
 	struct net_device *upper_dev;
 	struct mlxsw_sp *mlxsw_sp;
 	int err;
 
-	if (!mlxsw_sp_port_dev_check(dev))
-		return NOTIFY_DONE;
-
 	mlxsw_sp_port = netdev_priv(dev);
 	mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	info = ptr;
@@ -1885,28 +2476,67 @@ static int mlxsw_sp_netdevice_event(struct notifier_block *unused,
 	switch (event) {
 	case NETDEV_PRECHANGEUPPER:
 		upper_dev = info->upper_dev;
+		if (!info->master || !info->linking)
+			break;
 		/* HW limitation forbids to put ports to multiple bridges. */
-		if (info->master && info->linking &&
-		    netif_is_bridge_master(upper_dev) &&
+		if (netif_is_bridge_master(upper_dev) &&
 		    !mlxsw_sp_master_bridge_check(mlxsw_sp, upper_dev))
 			return NOTIFY_BAD;
+		if (netif_is_lag_master(upper_dev) &&
+		    !mlxsw_sp_master_lag_check(mlxsw_sp, upper_dev,
+					       info->upper_info))
+			return NOTIFY_BAD;
 		break;
 	case NETDEV_CHANGEUPPER:
 		upper_dev = info->upper_dev;
-		if (info->master &&
-		    netif_is_bridge_master(upper_dev)) {
+		if (is_vlan_dev(upper_dev)) {
+			if (info->linking) {
+				err = mlxsw_sp_port_vlan_link(mlxsw_sp_port,
+							      upper_dev);
+				if (err) {
+					netdev_err(dev, "Failed to link VLAN device\n");
+					return NOTIFY_BAD;
+				}
+			} else {
+				err = mlxsw_sp_port_vlan_unlink(mlxsw_sp_port,
+								upper_dev);
+				if (err) {
+					netdev_err(dev, "Failed to unlink VLAN device\n");
+					return NOTIFY_BAD;
+				}
+			}
+		} else if (netif_is_bridge_master(upper_dev)) {
 			if (info->linking) {
 				err = mlxsw_sp_port_bridge_join(mlxsw_sp_port);
-				if (err)
+				if (err) {
 					netdev_err(dev, "Failed to join bridge\n");
+					return NOTIFY_BAD;
+				}
 				mlxsw_sp_master_bridge_inc(mlxsw_sp, upper_dev);
-				mlxsw_sp_port->bridged = 1;
 			} else {
-				err = mlxsw_sp_port_bridge_leave(mlxsw_sp_port);
-				if (err)
-					netdev_err(dev, "Failed to leave bridge\n");
-				mlxsw_sp_port->bridged = 0;
+				err = mlxsw_sp_port_bridge_leave(mlxsw_sp_port,
+								 true);
 				mlxsw_sp_master_bridge_dec(mlxsw_sp, upper_dev);
+				if (err) {
+					netdev_err(dev, "Failed to leave bridge\n");
+					return NOTIFY_BAD;
+				}
+			}
+		} else if (netif_is_lag_master(upper_dev)) {
+			if (info->linking) {
+				err = mlxsw_sp_port_lag_join(mlxsw_sp_port,
+							     upper_dev);
+				if (err) {
+					netdev_err(dev, "Failed to join link aggregation\n");
+					return NOTIFY_BAD;
+				}
+			} else {
+				err = mlxsw_sp_port_lag_leave(mlxsw_sp_port,
+							      upper_dev);
+				if (err) {
+					netdev_err(dev, "Failed to leave link aggregation\n");
+					return NOTIFY_BAD;
+				}
 			}
 		}
 		break;
@@ -1915,6 +2545,455 @@ static int mlxsw_sp_netdevice_event(struct notifier_block *unused,
 	return NOTIFY_DONE;
 }
 
+static int mlxsw_sp_netdevice_port_lower_event(struct net_device *dev,
+					       unsigned long event, void *ptr)
+{
+	struct netdev_notifier_changelowerstate_info *info;
+	struct mlxsw_sp_port *mlxsw_sp_port;
+	int err;
+
+	mlxsw_sp_port = netdev_priv(dev);
+	info = ptr;
+
+	switch (event) {
+	case NETDEV_CHANGELOWERSTATE:
+		if (netif_is_lag_port(dev) && mlxsw_sp_port->lagged) {
+			err = mlxsw_sp_port_lag_changed(mlxsw_sp_port,
+							info->lower_state_info);
+			if (err)
+				netdev_err(dev, "Failed to reflect link aggregation lower state change\n");
+		}
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int mlxsw_sp_netdevice_port_event(struct net_device *dev,
+					 unsigned long event, void *ptr)
+{
+	switch (event) {
+	case NETDEV_PRECHANGEUPPER:
+	case NETDEV_CHANGEUPPER:
+		return mlxsw_sp_netdevice_port_upper_event(dev, event, ptr);
+	case NETDEV_CHANGELOWERSTATE:
+		return mlxsw_sp_netdevice_port_lower_event(dev, event, ptr);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int mlxsw_sp_netdevice_lag_event(struct net_device *lag_dev,
+					unsigned long event, void *ptr)
+{
+	struct net_device *dev;
+	struct list_head *iter;
+	int ret;
+
+	netdev_for_each_lower_dev(lag_dev, dev, iter) {
+		if (mlxsw_sp_port_dev_check(dev)) {
+			ret = mlxsw_sp_netdevice_port_event(dev, event, ptr);
+			if (ret == NOTIFY_BAD)
+				return ret;
+		}
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct mlxsw_sp_vfid *
+mlxsw_sp_br_vfid_find(const struct mlxsw_sp *mlxsw_sp,
+		      const struct net_device *br_dev)
+{
+	struct mlxsw_sp_vfid *vfid;
+
+	list_for_each_entry(vfid, &mlxsw_sp->br_vfids.list, list) {
+		if (vfid->br_dev == br_dev)
+			return vfid;
+	}
+
+	return NULL;
+}
+
+static u16 mlxsw_sp_vfid_to_br_vfid(u16 vfid)
+{
+	return vfid - MLXSW_SP_VFID_PORT_MAX;
+}
+
+static u16 mlxsw_sp_br_vfid_to_vfid(u16 br_vfid)
+{
+	return MLXSW_SP_VFID_PORT_MAX + br_vfid;
+}
+
+static u16 mlxsw_sp_avail_br_vfid_get(const struct mlxsw_sp *mlxsw_sp)
+{
+	return find_first_zero_bit(mlxsw_sp->br_vfids.mapped,
+				   MLXSW_SP_VFID_BR_MAX);
+}
+
+static struct mlxsw_sp_vfid *mlxsw_sp_br_vfid_create(struct mlxsw_sp *mlxsw_sp,
+						     struct net_device *br_dev)
+{
+	struct device *dev = mlxsw_sp->bus_info->dev;
+	struct mlxsw_sp_vfid *vfid;
+	u16 n_vfid;
+	int err;
+
+	n_vfid = mlxsw_sp_br_vfid_to_vfid(mlxsw_sp_avail_br_vfid_get(mlxsw_sp));
+	if (n_vfid == MLXSW_SP_VFID_MAX) {
+		dev_err(dev, "No available vFIDs\n");
+		return ERR_PTR(-ERANGE);
+	}
+
+	err = __mlxsw_sp_vfid_create(mlxsw_sp, n_vfid);
+	if (err) {
+		dev_err(dev, "Failed to create vFID=%d\n", n_vfid);
+		return ERR_PTR(err);
+	}
+
+	vfid = kzalloc(sizeof(*vfid), GFP_KERNEL);
+	if (!vfid)
+		goto err_allocate_vfid;
+
+	vfid->vfid = n_vfid;
+	vfid->br_dev = br_dev;
+
+	list_add(&vfid->list, &mlxsw_sp->br_vfids.list);
+	set_bit(mlxsw_sp_vfid_to_br_vfid(n_vfid), mlxsw_sp->br_vfids.mapped);
+
+	return vfid;
+
+err_allocate_vfid:
+	__mlxsw_sp_vfid_destroy(mlxsw_sp, n_vfid);
+	return ERR_PTR(-ENOMEM);
+}
+
+static void mlxsw_sp_br_vfid_destroy(struct mlxsw_sp *mlxsw_sp,
+				     struct mlxsw_sp_vfid *vfid)
+{
+	u16 br_vfid = mlxsw_sp_vfid_to_br_vfid(vfid->vfid);
+
+	clear_bit(br_vfid, mlxsw_sp->br_vfids.mapped);
+	list_del(&vfid->list);
+
+	__mlxsw_sp_vfid_destroy(mlxsw_sp, vfid->vfid);
+
+	kfree(vfid);
+}
+
+static int mlxsw_sp_vport_bridge_leave(struct mlxsw_sp_port *mlxsw_sp_vport,
+				       struct net_device *br_dev,
+				       bool flush_fdb)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
+	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
+	struct net_device *dev = mlxsw_sp_vport->dev;
+	struct mlxsw_sp_vfid *vfid, *new_vfid;
+	int err;
+
+	vfid = mlxsw_sp_br_vfid_find(mlxsw_sp, br_dev);
+	if (!vfid) {
+		WARN_ON(!vfid);
+		return -EINVAL;
+	}
+
+	/* We need a vFID to go back to after leaving the bridge's vFID. */
+	new_vfid = mlxsw_sp_vfid_find(mlxsw_sp, vid);
+	if (!new_vfid) {
+		new_vfid = mlxsw_sp_vfid_create(mlxsw_sp, vid);
+		if (IS_ERR(new_vfid)) {
+			netdev_err(dev, "Failed to create vFID for VID=%d\n",
+				   vid);
+			return PTR_ERR(new_vfid);
+		}
+	}
+
+	/* Invalidate existing {Port, VID} to vFID mapping and create a new
+	 * one for the new vFID.
+	 */
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
+					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
+					   false,
+					   mlxsw_sp_vfid_to_fid(vfid->vfid),
+					   vid);
+	if (err) {
+		netdev_err(dev, "Failed to invalidate {Port, VID} to vFID=%d mapping\n",
+			   vfid->vfid);
+		goto err_port_vid_to_fid_invalidate;
+	}
+
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
+					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
+					   true,
+					   mlxsw_sp_vfid_to_fid(new_vfid->vfid),
+					   vid);
+	if (err) {
+		netdev_err(dev, "Failed to map {Port, VID} to vFID=%d\n",
+			   new_vfid->vfid);
+		goto err_port_vid_to_fid_validate;
+	}
+
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, false);
+	if (err) {
+		netdev_err(dev, "Failed to disable learning\n");
+		goto err_port_vid_learning_set;
+	}
+
+	err = mlxsw_sp_vport_flood_set(mlxsw_sp_vport, vfid->vfid, false,
+				       false);
+	if (err) {
+		netdev_err(dev, "Failed clear to clear flooding\n");
+		goto err_vport_flood_set;
+	}
+
+	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_vport, vid,
+					  MLXSW_REG_SPMS_STATE_FORWARDING);
+	if (err) {
+		netdev_err(dev, "Failed to set STP state\n");
+		goto err_port_stp_state_set;
+	}
+
+	if (flush_fdb && mlxsw_sp_vport_fdb_flush(mlxsw_sp_vport))
+		netdev_err(dev, "Failed to flush FDB\n");
+
+	/* Switch between the vFIDs and destroy the old one if needed. */
+	new_vfid->nr_vports++;
+	mlxsw_sp_vport->vport.vfid = new_vfid;
+	vfid->nr_vports--;
+	if (!vfid->nr_vports)
+		mlxsw_sp_br_vfid_destroy(mlxsw_sp, vfid);
+
+	mlxsw_sp_vport->learning = 0;
+	mlxsw_sp_vport->learning_sync = 0;
+	mlxsw_sp_vport->uc_flood = 0;
+	mlxsw_sp_vport->bridged = 0;
+
+	return 0;
+
+err_port_stp_state_set:
+err_vport_flood_set:
+err_port_vid_learning_set:
+err_port_vid_to_fid_validate:
+err_port_vid_to_fid_invalidate:
+	/* Rollback vFID only if new. */
+	if (!new_vfid->nr_vports)
+		mlxsw_sp_vfid_destroy(mlxsw_sp, new_vfid);
+	return err;
+}
+
+static int mlxsw_sp_vport_bridge_join(struct mlxsw_sp_port *mlxsw_sp_vport,
+				      struct net_device *br_dev)
+{
+	struct mlxsw_sp_vfid *old_vfid = mlxsw_sp_vport->vport.vfid;
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_vport->mlxsw_sp;
+	u16 vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
+	struct net_device *dev = mlxsw_sp_vport->dev;
+	struct mlxsw_sp_vfid *vfid;
+	int err;
+
+	vfid = mlxsw_sp_br_vfid_find(mlxsw_sp, br_dev);
+	if (!vfid) {
+		vfid = mlxsw_sp_br_vfid_create(mlxsw_sp, br_dev);
+		if (IS_ERR(vfid)) {
+			netdev_err(dev, "Failed to create bridge vFID\n");
+			return PTR_ERR(vfid);
+		}
+	}
+
+	err = mlxsw_sp_vport_flood_set(mlxsw_sp_vport, vfid->vfid, true, false);
+	if (err) {
+		netdev_err(dev, "Failed to setup flooding for vFID=%d\n",
+			   vfid->vfid);
+		goto err_port_flood_set;
+	}
+
+	err = mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, true);
+	if (err) {
+		netdev_err(dev, "Failed to enable learning\n");
+		goto err_port_vid_learning_set;
+	}
+
+	/* We need to invalidate existing {Port, VID} to vFID mapping and
+	 * create a new one for the bridge's vFID.
+	 */
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
+					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
+					   false,
+					   mlxsw_sp_vfid_to_fid(old_vfid->vfid),
+					   vid);
+	if (err) {
+		netdev_err(dev, "Failed to invalidate {Port, VID} to vFID=%d mapping\n",
+			   old_vfid->vfid);
+		goto err_port_vid_to_fid_invalidate;
+	}
+
+	err = mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
+					   MLXSW_REG_SVFA_MT_PORT_VID_TO_FID,
+					   true,
+					   mlxsw_sp_vfid_to_fid(vfid->vfid),
+					   vid);
+	if (err) {
+		netdev_err(dev, "Failed to map {Port, VID} to vFID=%d\n",
+			   vfid->vfid);
+		goto err_port_vid_to_fid_validate;
+	}
+
+	/* Switch between the vFIDs and destroy the old one if needed. */
+	vfid->nr_vports++;
+	mlxsw_sp_vport->vport.vfid = vfid;
+	old_vfid->nr_vports--;
+	if (!old_vfid->nr_vports)
+		mlxsw_sp_vfid_destroy(mlxsw_sp, old_vfid);
+
+	mlxsw_sp_vport->learning = 1;
+	mlxsw_sp_vport->learning_sync = 1;
+	mlxsw_sp_vport->uc_flood = 1;
+	mlxsw_sp_vport->bridged = 1;
+
+	return 0;
+
+err_port_vid_to_fid_validate:
+	mlxsw_sp_port_vid_to_fid_set(mlxsw_sp_vport,
+				     MLXSW_REG_SVFA_MT_PORT_VID_TO_FID, false,
+				     mlxsw_sp_vfid_to_fid(old_vfid->vfid), vid);
+err_port_vid_to_fid_invalidate:
+	mlxsw_sp_port_vid_learning_set(mlxsw_sp_vport, vid, false);
+err_port_vid_learning_set:
+	mlxsw_sp_vport_flood_set(mlxsw_sp_vport, vfid->vfid, false, false);
+err_port_flood_set:
+	if (!vfid->nr_vports)
+		mlxsw_sp_br_vfid_destroy(mlxsw_sp, vfid);
+	return err;
+}
+
+static bool
+mlxsw_sp_port_master_bridge_check(const struct mlxsw_sp_port *mlxsw_sp_port,
+				  const struct net_device *br_dev)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+
+	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
+			    vport.list) {
+		if (mlxsw_sp_vport_br_get(mlxsw_sp_vport) == br_dev)
+			return false;
+	}
+
+	return true;
+}
+
+static int mlxsw_sp_netdevice_vport_event(struct net_device *dev,
+					  unsigned long event, void *ptr,
+					  u16 vid)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
+	struct netdev_notifier_changeupper_info *info = ptr;
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	struct net_device *upper_dev;
+	int err;
+
+	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
+
+	switch (event) {
+	case NETDEV_PRECHANGEUPPER:
+		upper_dev = info->upper_dev;
+		if (!info->master || !info->linking)
+			break;
+		if (!netif_is_bridge_master(upper_dev))
+			return NOTIFY_BAD;
+		/* We can't have multiple VLAN interfaces configured on
+		 * the same port and being members in the same bridge.
+		 */
+		if (!mlxsw_sp_port_master_bridge_check(mlxsw_sp_port,
+						       upper_dev))
+			return NOTIFY_BAD;
+		break;
+	case NETDEV_CHANGEUPPER:
+		upper_dev = info->upper_dev;
+		if (!info->master)
+			break;
+		if (info->linking) {
+			if (!mlxsw_sp_vport) {
+				WARN_ON(!mlxsw_sp_vport);
+				return NOTIFY_BAD;
+			}
+			err = mlxsw_sp_vport_bridge_join(mlxsw_sp_vport,
+							 upper_dev);
+			if (err) {
+				netdev_err(dev, "Failed to join bridge\n");
+				return NOTIFY_BAD;
+			}
+		} else {
+			/* We ignore bridge's unlinking notifications if vPort
+			 * is gone, since we already left the bridge when the
+			 * VLAN device was unlinked from the real device.
+			 */
+			if (!mlxsw_sp_vport)
+				return NOTIFY_DONE;
+			err = mlxsw_sp_vport_bridge_leave(mlxsw_sp_vport,
+							  upper_dev, true);
+			if (err) {
+				netdev_err(dev, "Failed to leave bridge\n");
+				return NOTIFY_BAD;
+			}
+		}
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int mlxsw_sp_netdevice_lag_vport_event(struct net_device *lag_dev,
+					      unsigned long event, void *ptr,
+					      u16 vid)
+{
+	struct net_device *dev;
+	struct list_head *iter;
+	int ret;
+
+	netdev_for_each_lower_dev(lag_dev, dev, iter) {
+		if (mlxsw_sp_port_dev_check(dev)) {
+			ret = mlxsw_sp_netdevice_vport_event(dev, event, ptr,
+							     vid);
+			if (ret == NOTIFY_BAD)
+				return ret;
+		}
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int mlxsw_sp_netdevice_vlan_event(struct net_device *vlan_dev,
+					 unsigned long event, void *ptr)
+{
+	struct net_device *real_dev = vlan_dev_real_dev(vlan_dev);
+	u16 vid = vlan_dev_vlan_id(vlan_dev);
+
+	if (mlxsw_sp_port_dev_check(real_dev))
+		return mlxsw_sp_netdevice_vport_event(real_dev, event, ptr,
+						      vid);
+	else if (netif_is_lag_master(real_dev))
+		return mlxsw_sp_netdevice_lag_vport_event(real_dev, event, ptr,
+							  vid);
+
+	return NOTIFY_DONE;
+}
+
+static int mlxsw_sp_netdevice_event(struct notifier_block *unused,
+				    unsigned long event, void *ptr)
+{
+	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
+
+	if (mlxsw_sp_port_dev_check(dev))
+		return mlxsw_sp_netdevice_port_event(dev, event, ptr);
+
+	if (netif_is_lag_master(dev))
+		return mlxsw_sp_netdevice_lag_event(dev, event, ptr);
+
+	if (is_vlan_dev(dev))
+		return mlxsw_sp_netdevice_vlan_event(dev, event, ptr);
+
+	return NOTIFY_DONE;
+}
+
 static struct notifier_block mlxsw_sp_netdevice_nb __read_mostly = {
 	.notifier_call = mlxsw_sp_netdevice_event,
 };
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
index 4365c8bccc6d..3b89ed2f3c76 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum.h
@@ -41,16 +41,73 @@
 #include <linux/netdevice.h>
 #include <linux/bitops.h>
 #include <linux/if_vlan.h>
+#include <linux/list.h>
 #include <net/switchdev.h>
 
+#include "port.h"
 #include "core.h"
 
 #define MLXSW_SP_VFID_BASE VLAN_N_VID
+#define MLXSW_SP_VFID_PORT_MAX 512	/* Non-bridged VLAN interfaces */
+#define MLXSW_SP_VFID_BR_MAX 8192	/* Bridged VLAN interfaces */
+#define MLXSW_SP_VFID_MAX (MLXSW_SP_VFID_PORT_MAX + MLXSW_SP_VFID_BR_MAX)
+
+#define MLXSW_SP_LAG_MAX 64
+#define MLXSW_SP_PORT_PER_LAG_MAX 16
+
+#define MLXSW_SP_MID_MAX 7000
 
 struct mlxsw_sp_port;
 
+struct mlxsw_sp_upper {
+	struct net_device *dev;
+	unsigned int ref_count;
+};
+
+struct mlxsw_sp_vfid {
+	struct list_head list;
+	u16 nr_vports;
+	u16 vfid;	/* Starting at 0 */
+	struct net_device *br_dev;
+	u16 vid;
+};
+
+struct mlxsw_sp_mid {
+	struct list_head list;
+	unsigned char addr[ETH_ALEN];
+	u16 vid;
+	u16 mid;
+	unsigned int ref_count;
+};
+
+static inline u16 mlxsw_sp_vfid_to_fid(u16 vfid)
+{
+	return MLXSW_SP_VFID_BASE + vfid;
+}
+
+static inline u16 mlxsw_sp_fid_to_vfid(u16 fid)
+{
+	return fid - MLXSW_SP_VFID_BASE;
+}
+
+static inline bool mlxsw_sp_fid_is_vfid(u16 fid)
+{
+	return fid >= MLXSW_SP_VFID_BASE;
+}
+
 struct mlxsw_sp {
-	unsigned long active_vfids[BITS_TO_LONGS(VLAN_N_VID)];
+	struct {
+		struct list_head list;
+		unsigned long mapped[BITS_TO_LONGS(MLXSW_SP_VFID_PORT_MAX)];
+	} port_vfids;
+	struct {
+		struct list_head list;
+		unsigned long mapped[BITS_TO_LONGS(MLXSW_SP_VFID_BR_MAX)];
+	} br_vfids;
+	struct {
+		struct list_head list;
+		unsigned long mapped[BITS_TO_LONGS(MLXSW_SP_MID_MAX)];
+	} br_mids;
 	unsigned long active_fids[BITS_TO_LONGS(VLAN_N_VID)];
 	struct mlxsw_sp_port **ports;
 	struct mlxsw_core *core;
@@ -63,12 +120,16 @@ struct mlxsw_sp {
 	} fdb_notify;
 #define MLXSW_SP_DEFAULT_AGEING_TIME 300
 	u32 ageing_time;
-	struct {
-		struct net_device *dev;
-		unsigned int ref_count;
-	} master_bridge;
+	struct mlxsw_sp_upper master_bridge;
+	struct mlxsw_sp_upper lags[MLXSW_SP_LAG_MAX];
 };
 
+static inline struct mlxsw_sp_upper *
+mlxsw_sp_lag_get(struct mlxsw_sp *mlxsw_sp, u16 lag_id)
+{
+	return &mlxsw_sp->lags[lag_id];
+}
+
 struct mlxsw_sp_port_pcpu_stats {
 	u64			rx_packets;
 	u64			rx_bytes;
@@ -87,15 +148,87 @@ struct mlxsw_sp_port {
 	u8 learning:1,
 	   learning_sync:1,
 	   uc_flood:1,
-	   bridged:1;
+	   bridged:1,
+	   lagged:1;
 	u16 pvid;
+	u16 lag_id;
+	struct {
+		struct list_head list;
+		struct mlxsw_sp_vfid *vfid;
+		u16 vid;
+	} vport;
 	/* 802.1Q bridge VLANs */
-	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+	unsigned long *active_vlans;
+	unsigned long *untagged_vlans;
 	/* VLAN interfaces */
-	unsigned long active_vfids[BITS_TO_LONGS(VLAN_N_VID)];
-	u16 nr_vfids;
+	struct list_head vports_list;
 };
 
+static inline struct mlxsw_sp_port *
+mlxsw_sp_port_lagged_get(struct mlxsw_sp *mlxsw_sp, u16 lag_id, u8 port_index)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port;
+	u8 local_port;
+
+	local_port = mlxsw_core_lag_mapping_get(mlxsw_sp->core,
+						lag_id, port_index);
+	mlxsw_sp_port = mlxsw_sp->ports[local_port];
+	return mlxsw_sp_port && mlxsw_sp_port->lagged ? mlxsw_sp_port : NULL;
+}
+
+static inline bool
+mlxsw_sp_port_is_vport(const struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	return mlxsw_sp_port->vport.vfid;
+}
+
+static inline struct net_device *
+mlxsw_sp_vport_br_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
+{
+	return mlxsw_sp_vport->vport.vfid->br_dev;
+}
+
+static inline u16
+mlxsw_sp_vport_vid_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
+{
+	return mlxsw_sp_vport->vport.vid;
+}
+
+static inline u16
+mlxsw_sp_vport_vfid_get(const struct mlxsw_sp_port *mlxsw_sp_vport)
+{
+	return mlxsw_sp_vport->vport.vfid->vfid;
+}
+
+static inline struct mlxsw_sp_port *
+mlxsw_sp_port_vport_find(const struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+
+	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
+			    vport.list) {
+		if (mlxsw_sp_vport_vid_get(mlxsw_sp_vport) == vid)
+			return mlxsw_sp_vport;
+	}
+
+	return NULL;
+}
+
+static inline struct mlxsw_sp_port *
+mlxsw_sp_port_vport_find_by_vfid(const struct mlxsw_sp_port *mlxsw_sp_port,
+				 u16 vfid)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+
+	list_for_each_entry(mlxsw_sp_vport, &mlxsw_sp_port->vports_list,
+			    vport.list) {
+		if (mlxsw_sp_vport_vfid_get(mlxsw_sp_vport) == vfid)
+			return mlxsw_sp_vport;
+	}
+
+	return NULL;
+}
+
 enum mlxsw_sp_flood_table {
 	MLXSW_SP_FLOOD_TABLE_UC,
 	MLXSW_SP_FLOOD_TABLE_BM,
@@ -118,5 +251,9 @@ int mlxsw_sp_port_add_vid(struct net_device *dev, __be16 __always_unused proto,
 			  u16 vid);
 int mlxsw_sp_port_kill_vid(struct net_device *dev,
 			   __be16 __always_unused proto, u16 vid);
+int mlxsw_sp_vport_flood_set(struct mlxsw_sp_port *mlxsw_sp_vport, u16 vfid,
+			     bool set, bool only_uc);
+void mlxsw_sp_port_active_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port);
+int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid);
 
 #endif
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
index 617fb22b5d81..7b56098acc58 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_switchdev.c
@@ -45,18 +45,57 @@
 #include <linux/if_bridge.h>
 #include <linux/workqueue.h>
 #include <linux/jiffies.h>
+#include <linux/rtnetlink.h>
 #include <net/switchdev.h>
 
 #include "spectrum.h"
 #include "core.h"
 #include "reg.h"
 
+static u16 mlxsw_sp_port_vid_to_fid_get(struct mlxsw_sp_port *mlxsw_sp_port,
+					u16 vid)
+{
+	u16 fid = vid;
+
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		u16 vfid = mlxsw_sp_vport_vfid_get(mlxsw_sp_port);
+
+		fid = mlxsw_sp_vfid_to_fid(vfid);
+	}
+
+	if (!fid)
+		fid = mlxsw_sp_port->pvid;
+
+	return fid;
+}
+
+static struct mlxsw_sp_port *
+mlxsw_sp_port_orig_get(struct net_device *dev,
+		       struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	struct mlxsw_sp_port *mlxsw_sp_vport;
+	u16 vid;
+
+	if (!is_vlan_dev(dev))
+		return mlxsw_sp_port;
+
+	vid = vlan_dev_vlan_id(dev);
+	mlxsw_sp_vport = mlxsw_sp_port_vport_find(mlxsw_sp_port, vid);
+	WARN_ON(!mlxsw_sp_vport);
+
+	return mlxsw_sp_vport;
+}
+
 static int mlxsw_sp_port_attr_get(struct net_device *dev,
 				  struct switchdev_attr *attr)
 {
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 
+	mlxsw_sp_port = mlxsw_sp_port_orig_get(attr->orig_dev, mlxsw_sp_port);
+	if (!mlxsw_sp_port)
+		return -EINVAL;
+
 	switch (attr->id) {
 	case SWITCHDEV_ATTR_ID_PORT_PARENT_ID:
 		attr->u.ppid.id_len = sizeof(mlxsw_sp->base_mac);
@@ -86,14 +125,14 @@ static int mlxsw_sp_port_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	int err;
 
 	switch (state) {
-	case BR_STATE_DISABLED: /* fall-through */
 	case BR_STATE_FORWARDING:
 		spms_state = MLXSW_REG_SPMS_STATE_FORWARDING;
 		break;
-	case BR_STATE_LISTENING: /* fall-through */
 	case BR_STATE_LEARNING:
 		spms_state = MLXSW_REG_SPMS_STATE_LEARNING;
 		break;
+	case BR_STATE_LISTENING: /* fall-through */
+	case BR_STATE_DISABLED: /* fall-through */
 	case BR_STATE_BLOCKING:
 		spms_state = MLXSW_REG_SPMS_STATE_DISCARDING;
 		break;
@@ -105,8 +144,14 @@ static int mlxsw_sp_port_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	if (!spms_pl)
 		return -ENOMEM;
 	mlxsw_reg_spms_pack(spms_pl, mlxsw_sp_port->local_port);
-	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
+
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
 		mlxsw_reg_spms_vid_pack(spms_pl, vid, spms_state);
+	} else {
+		for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
+			mlxsw_reg_spms_vid_pack(spms_pl, vid, spms_state);
+	}
 
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spms), spms_pl);
 	kfree(spms_pl);
@@ -124,22 +169,38 @@ static int mlxsw_sp_port_attr_stp_state_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return mlxsw_sp_port_stp_state_set(mlxsw_sp_port, state);
 }
 
+static bool mlxsw_sp_vfid_is_vport_br(u16 vfid)
+{
+	return vfid >= MLXSW_SP_VFID_PORT_MAX;
+}
+
 static int __mlxsw_sp_port_flood_set(struct mlxsw_sp_port *mlxsw_sp_port,
-				     u16 fid_begin, u16 fid_end, bool set,
+				     u16 idx_begin, u16 idx_end, bool set,
 				     bool only_uc)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
-	u16 range = fid_end - fid_begin + 1;
+	u16 local_port = mlxsw_sp_port->local_port;
+	enum mlxsw_flood_table_type table_type;
+	u16 range = idx_end - idx_begin + 1;
 	char *sftr_pl;
 	int err;
 
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID;
+		if (mlxsw_sp_vfid_is_vport_br(idx_begin))
+			local_port = mlxsw_sp_port->local_port;
+		else
+			local_port = MLXSW_PORT_CPU_PORT;
+	} else {
+		table_type = MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST;
+	}
+
 	sftr_pl = kmalloc(MLXSW_REG_SFTR_LEN, GFP_KERNEL);
 	if (!sftr_pl)
 		return -ENOMEM;
 
-	mlxsw_reg_sftr_pack(sftr_pl, MLXSW_SP_FLOOD_TABLE_UC, fid_begin,
-			    MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST, range,
-			    mlxsw_sp_port->local_port, set);
+	mlxsw_reg_sftr_pack(sftr_pl, MLXSW_SP_FLOOD_TABLE_UC, idx_begin,
+			    table_type, range, local_port, set);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sftr), sftr_pl);
 	if (err)
 		goto buffer_out;
@@ -150,9 +211,8 @@ static int __mlxsw_sp_port_flood_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	if (only_uc)
 		goto buffer_out;
 
-	mlxsw_reg_sftr_pack(sftr_pl, MLXSW_SP_FLOOD_TABLE_BM, fid_begin,
-			    MLXSW_REG_SFGC_TABLE_TYPE_FID_OFFEST, range,
-			    mlxsw_sp_port->local_port, set);
+	mlxsw_reg_sftr_pack(sftr_pl, MLXSW_SP_FLOOD_TABLE_BM, idx_begin,
+			    table_type, range, local_port, set);
 	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sftr), sftr_pl);
 
 buffer_out:
@@ -167,6 +227,13 @@ static int mlxsw_sp_port_uc_flood_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	u16 vid, last_visited_vid;
 	int err;
 
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		u16 vfid = mlxsw_sp_vport_vfid_get(mlxsw_sp_port);
+
+		return  __mlxsw_sp_port_flood_set(mlxsw_sp_port, vfid, vfid,
+						  set, true);
+	}
+
 	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
 		err = __mlxsw_sp_port_flood_set(mlxsw_sp_port, vid, vid, set,
 						true);
@@ -185,6 +252,16 @@ err_port_flood_set:
 	return err;
 }
 
+int mlxsw_sp_vport_flood_set(struct mlxsw_sp_port *mlxsw_sp_vport, u16 vfid,
+			     bool set, bool only_uc)
+{
+	/* In case of vFIDs, index into the flooding table is relative to
+	 * the start of the vFIDs range.
+	 */
+	return __mlxsw_sp_port_flood_set(mlxsw_sp_vport, vfid, vfid, set,
+					 only_uc);
+}
+
 static int mlxsw_sp_port_attr_br_flags_set(struct mlxsw_sp_port *mlxsw_sp_port,
 					   struct switchdev_trans *trans,
 					   unsigned long brport_flags)
@@ -193,6 +270,9 @@ static int mlxsw_sp_port_attr_br_flags_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	bool set;
 	int err;
 
+	if (!mlxsw_sp_port->bridged)
+		return -EINVAL;
+
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
@@ -237,6 +317,22 @@ static int mlxsw_sp_port_attr_br_ageing_set(struct mlxsw_sp_port *mlxsw_sp_port,
 	return mlxsw_sp_ageing_set(mlxsw_sp, ageing_time);
 }
 
+static int mlxsw_sp_port_attr_br_vlan_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					  struct switchdev_trans *trans,
+					  struct net_device *orig_dev,
+					  bool vlan_enabled)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+
+	/* SWITCHDEV_TRANS_PREPARE phase */
+	if ((!vlan_enabled) && (mlxsw_sp->master_bridge.dev == orig_dev)) {
+		netdev_err(mlxsw_sp_port->dev, "Bridge must be vlan-aware\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int mlxsw_sp_port_attr_set(struct net_device *dev,
 				  const struct switchdev_attr *attr,
 				  struct switchdev_trans *trans)
@@ -244,6 +340,10 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
+	mlxsw_sp_port = mlxsw_sp_port_orig_get(attr->orig_dev, mlxsw_sp_port);
+	if (!mlxsw_sp_port)
+		return -EINVAL;
+
 	switch (attr->id) {
 	case SWITCHDEV_ATTR_ID_PORT_STP_STATE:
 		err = mlxsw_sp_port_attr_stp_state_set(mlxsw_sp_port, trans,
@@ -257,6 +357,11 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 		err = mlxsw_sp_port_attr_br_ageing_set(mlxsw_sp_port, trans,
 						       attr->u.ageing_time);
 		break;
+	case SWITCHDEV_ATTR_ID_BRIDGE_VLAN_FILTERING:
+		err = mlxsw_sp_port_attr_br_vlan_set(mlxsw_sp_port, trans,
+						     attr->orig_dev,
+						     attr->u.vlan_filtering);
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -265,7 +370,8 @@ static int mlxsw_sp_port_attr_set(struct net_device *dev,
 	return err;
 }
 
-static int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+static int __mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				    u16 vid)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	char spvid_pl[MLXSW_REG_SPVID_LEN];
@@ -274,6 +380,53 @@ static int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
 	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spvid), spvid_pl);
 }
 
+static int mlxsw_sp_port_allow_untagged_set(struct mlxsw_sp_port *mlxsw_sp_port,
+					    bool allow)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char spaft_pl[MLXSW_REG_SPAFT_LEN];
+
+	mlxsw_reg_spaft_pack(spaft_pl, mlxsw_sp_port->local_port, allow);
+	return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(spaft), spaft_pl);
+}
+
+int mlxsw_sp_port_pvid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 vid)
+{
+	struct net_device *dev = mlxsw_sp_port->dev;
+	int err;
+
+	if (!vid) {
+		err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port, false);
+		if (err) {
+			netdev_err(dev, "Failed to disallow untagged traffic\n");
+			return err;
+		}
+	} else {
+		err = __mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid);
+		if (err) {
+			netdev_err(dev, "Failed to set PVID\n");
+			return err;
+		}
+
+		/* Only allow if not already allowed. */
+		if (!mlxsw_sp_port->pvid) {
+			err = mlxsw_sp_port_allow_untagged_set(mlxsw_sp_port,
+							       true);
+			if (err) {
+				netdev_err(dev, "Failed to allow untagged traffic\n");
+				goto err_port_allow_untagged_set;
+			}
+		}
+	}
+
+	mlxsw_sp_port->pvid = vid;
+	return 0;
+
+err_port_allow_untagged_set:
+	__mlxsw_sp_port_pvid_set(mlxsw_sp_port, mlxsw_sp_port->pvid);
+	return err;
+}
+
 static int mlxsw_sp_fid_create(struct mlxsw_sp *mlxsw_sp, u16 fid)
 {
 	char sfmr_pl[MLXSW_REG_SFMR_LEN];
@@ -304,7 +457,7 @@ static int mlxsw_sp_port_fid_map(struct mlxsw_sp_port *mlxsw_sp_port, u16 fid)
 {
 	enum mlxsw_reg_svfa_mt mt;
 
-	if (mlxsw_sp_port->nr_vfids)
+	if (!list_empty(&mlxsw_sp_port->vports_list))
 		mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
 	else
 		mt = MLXSW_REG_SVFA_MT_VID_TO_FID;
@@ -316,7 +469,7 @@ static int mlxsw_sp_port_fid_unmap(struct mlxsw_sp_port *mlxsw_sp_port, u16 fid)
 {
 	enum mlxsw_reg_svfa_mt mt;
 
-	if (!mlxsw_sp_port->nr_vfids)
+	if (list_empty(&mlxsw_sp_port->vports_list))
 		return 0;
 
 	mt = MLXSW_REG_SVFA_MT_PORT_VID_TO_FID;
@@ -342,14 +495,35 @@ err_port_add_vid:
 	return err;
 }
 
+static int __mlxsw_sp_port_vlans_set(struct mlxsw_sp_port *mlxsw_sp_port,
+				     u16 vid_begin, u16 vid_end, bool is_member,
+				     bool untagged)
+{
+	u16 vid, vid_e;
+	int err;
+
+	for (vid = vid_begin; vid <= vid_end;
+	     vid += MLXSW_REG_SPVM_REC_MAX_COUNT) {
+		vid_e = min((u16) (vid + MLXSW_REG_SPVM_REC_MAX_COUNT - 1),
+			    vid_end);
+
+		err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid_e,
+					     is_member, untagged);
+		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)
 {
 	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 	struct net_device *dev = mlxsw_sp_port->dev;
+	u16 vid, last_visited_vid, old_pvid;
 	enum mlxsw_reg_svfa_mt mt;
-	u16 vid, vid_e;
 	int err;
 
 	/* In case this is invoked with BRIDGE_FLAGS_SELF and port is
@@ -377,15 +551,18 @@ static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 			if (err) {
 				netdev_err(dev, "Failed to create FID=VID=%d mapping\n",
 					   vid);
-				return err;
+				goto err_port_vid_to_fid_set;
 			}
 		}
+	}
 
-		/* Set FID mapping according to port's mode */
+	/* Set FID mapping according to port's mode */
+	for (vid = vid_begin; vid <= vid_end; vid++) {
 		err = mlxsw_sp_port_fid_map(mlxsw_sp_port, vid);
 		if (err) {
 			netdev_err(dev, "Failed to map FID=%d", vid);
-			return err;
+			last_visited_vid = --vid;
+			goto err_port_fid_map;
 		}
 	}
 
@@ -393,83 +570,138 @@ static int __mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 					true, false);
 	if (err) {
 		netdev_err(dev, "Failed to configure flooding\n");
-		return err;
+		goto err_port_flood_set;
 	}
 
-	for (vid = vid_begin; vid <= vid_end;
-	     vid += MLXSW_REG_SPVM_REC_MAX_COUNT) {
-		vid_e = min((u16) (vid + MLXSW_REG_SPVM_REC_MAX_COUNT - 1),
-			    vid_end);
+	err = __mlxsw_sp_port_vlans_set(mlxsw_sp_port, vid_begin, vid_end,
+					true, flag_untagged);
+	if (err) {
+		netdev_err(dev, "Unable to add VIDs %d-%d\n", vid_begin,
+			   vid_end);
+		goto err_port_vlans_set;
+	}
 
-		err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid_e, true,
-					     flag_untagged);
+	old_pvid = mlxsw_sp_port->pvid;
+	if (flag_pvid && old_pvid != vid_begin) {
+		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid_begin);
 		if (err) {
-			netdev_err(mlxsw_sp_port->dev, "Unable to add VIDs %d-%d\n",
-				   vid, vid_e);
-			return err;
+			netdev_err(dev, "Unable to add PVID %d\n", vid_begin);
+			goto err_port_pvid_set;
 		}
-	}
-
-	vid = vid_begin;
-	if (flag_pvid && mlxsw_sp_port->pvid != vid) {
-		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, vid);
+	} else if (!flag_pvid && old_pvid >= vid_begin && old_pvid <= vid_end) {
+		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port, 0);
 		if (err) {
-			netdev_err(mlxsw_sp_port->dev, "Unable to add PVID %d\n",
-				   vid);
-			return err;
+			netdev_err(dev, "Unable to del PVID\n");
+			goto err_port_pvid_set;
 		}
-		mlxsw_sp_port->pvid = vid;
 	}
 
 	/* Changing activity bits only if HW operation succeded */
-	for (vid = vid_begin; vid <= vid_end; vid++)
+	for (vid = vid_begin; vid <= vid_end; vid++) {
 		set_bit(vid, mlxsw_sp_port->active_vlans);
+		if (flag_untagged)
+			set_bit(vid, mlxsw_sp_port->untagged_vlans);
+		else
+			clear_bit(vid, mlxsw_sp_port->untagged_vlans);
+	}
 
-	return mlxsw_sp_port_stp_state_set(mlxsw_sp_port,
-					   mlxsw_sp_port->stp_state);
+	/* STP state change must be done after we set active VLANs */
+	err = mlxsw_sp_port_stp_state_set(mlxsw_sp_port,
+					  mlxsw_sp_port->stp_state);
+	if (err) {
+		netdev_err(dev, "Failed to set STP state\n");
+		goto err_port_stp_state_set;
+	}
+
+	return 0;
+
+err_port_vid_to_fid_set:
+	mlxsw_sp_fid_destroy(mlxsw_sp, vid);
+	return err;
+
+err_port_stp_state_set:
+	for (vid = vid_begin; vid <= vid_end; vid++)
+		clear_bit(vid, mlxsw_sp_port->active_vlans);
+	if (old_pvid != mlxsw_sp_port->pvid)
+		mlxsw_sp_port_pvid_set(mlxsw_sp_port, old_pvid);
+err_port_pvid_set:
+	__mlxsw_sp_port_vlans_set(mlxsw_sp_port, vid_begin, vid_end, false,
+				  false);
+err_port_vlans_set:
+	__mlxsw_sp_port_flood_set(mlxsw_sp_port, vid_begin, vid_end, false,
+				  false);
+err_port_flood_set:
+	last_visited_vid = vid_end;
+err_port_fid_map:
+	for (vid = last_visited_vid; vid >= vid_begin; vid--)
+		mlxsw_sp_port_fid_unmap(mlxsw_sp_port, vid);
+	return err;
 }
 
 static int mlxsw_sp_port_vlans_add(struct mlxsw_sp_port *mlxsw_sp_port,
 				   const struct switchdev_obj_port_vlan *vlan,
 				   struct switchdev_trans *trans)
 {
-	bool untagged_flag = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
-	bool pvid_flag = vlan->flags & BRIDGE_VLAN_INFO_PVID;
+	bool flag_untagged = vlan->flags & BRIDGE_VLAN_INFO_UNTAGGED;
+	bool flag_pvid = vlan->flags & BRIDGE_VLAN_INFO_PVID;
 
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
 	return __mlxsw_sp_port_vlans_add(mlxsw_sp_port,
 					 vlan->vid_begin, vlan->vid_end,
-					 untagged_flag, pvid_flag);
+					 flag_untagged, flag_pvid);
+}
+
+static enum mlxsw_reg_sfd_rec_policy mlxsw_sp_sfd_rec_policy(bool dynamic)
+{
+	return dynamic ? MLXSW_REG_SFD_REC_POLICY_DYNAMIC_ENTRY_INGRESS :
+			 MLXSW_REG_SFD_REC_POLICY_STATIC_ENTRY;
+}
+
+static enum mlxsw_reg_sfd_op mlxsw_sp_sfd_op(bool adding)
+{
+	return adding ? MLXSW_REG_SFD_OP_WRITE_EDIT :
+			MLXSW_REG_SFD_OP_WRITE_REMOVE;
 }
 
-static int mlxsw_sp_port_fdb_op(struct mlxsw_sp_port *mlxsw_sp_port,
-				const char *mac, u16 vid, bool adding,
-				bool dynamic)
+static int mlxsw_sp_port_fdb_uc_op(struct mlxsw_sp *mlxsw_sp, u8 local_port,
+				   const char *mac, u16 fid, bool adding,
+				   bool dynamic)
 {
-	enum mlxsw_reg_sfd_rec_policy policy;
-	enum mlxsw_reg_sfd_op op;
 	char *sfd_pl;
 	int err;
 
-	if (!vid)
-		vid = mlxsw_sp_port->pvid;
+	sfd_pl = kmalloc(MLXSW_REG_SFD_LEN, GFP_KERNEL);
+	if (!sfd_pl)
+		return -ENOMEM;
+
+	mlxsw_reg_sfd_pack(sfd_pl, mlxsw_sp_sfd_op(adding), 0);
+	mlxsw_reg_sfd_uc_pack(sfd_pl, 0, mlxsw_sp_sfd_rec_policy(dynamic),
+			      mac, fid, MLXSW_REG_SFD_REC_ACTION_NOP,
+			      local_port);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfd), sfd_pl);
+	kfree(sfd_pl);
+
+	return err;
+}
+
+static int mlxsw_sp_port_fdb_uc_lag_op(struct mlxsw_sp *mlxsw_sp, u16 lag_id,
+				       const char *mac, u16 fid, u16 lag_vid,
+				       bool adding, bool dynamic)
+{
+	char *sfd_pl;
+	int err;
 
 	sfd_pl = kmalloc(MLXSW_REG_SFD_LEN, GFP_KERNEL);
 	if (!sfd_pl)
 		return -ENOMEM;
 
-	policy = dynamic ? MLXSW_REG_SFD_REC_POLICY_DYNAMIC_ENTRY_INGRESS :
-			   MLXSW_REG_SFD_REC_POLICY_STATIC_ENTRY;
-	op = adding ? MLXSW_REG_SFD_OP_WRITE_EDIT :
-		      MLXSW_REG_SFD_OP_WRITE_REMOVE;
-	mlxsw_reg_sfd_pack(sfd_pl, op, 0);
-	mlxsw_reg_sfd_uc_pack(sfd_pl, 0, policy,
-			      mac, vid, MLXSW_REG_SFD_REC_ACTION_NOP,
-			      mlxsw_sp_port->local_port);
-	err = mlxsw_reg_write(mlxsw_sp_port->mlxsw_sp->core, MLXSW_REG(sfd),
-			      sfd_pl);
+	mlxsw_reg_sfd_pack(sfd_pl, mlxsw_sp_sfd_op(adding), 0);
+	mlxsw_reg_sfd_uc_lag_pack(sfd_pl, 0, mlxsw_sp_sfd_rec_policy(dynamic),
+				  mac, fid, MLXSW_REG_SFD_REC_ACTION_NOP,
+				  lag_vid, lag_id);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfd), sfd_pl);
 	kfree(sfd_pl);
 
 	return err;
@@ -480,11 +712,162 @@ mlxsw_sp_port_fdb_static_add(struct mlxsw_sp_port *mlxsw_sp_port,
 			     const struct switchdev_obj_port_fdb *fdb,
 			     struct switchdev_trans *trans)
 {
+	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, fdb->vid);
+	u16 lag_vid = 0;
+
 	if (switchdev_trans_ph_prepare(trans))
 		return 0;
 
-	return mlxsw_sp_port_fdb_op(mlxsw_sp_port, fdb->addr, fdb->vid,
-				    true, false);
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+	}
+
+	if (!mlxsw_sp_port->lagged)
+		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp_port->mlxsw_sp,
+					       mlxsw_sp_port->local_port,
+					       fdb->addr, fid, true, false);
+	else
+		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
+						   mlxsw_sp_port->lag_id,
+						   fdb->addr, fid, lag_vid,
+						   true, false);
+}
+
+static int mlxsw_sp_port_mdb_op(struct mlxsw_sp *mlxsw_sp, const char *addr,
+				u16 fid, u16 mid, bool adding)
+{
+	char *sfd_pl;
+	int err;
+
+	sfd_pl = kmalloc(MLXSW_REG_SFD_LEN, GFP_KERNEL);
+	if (!sfd_pl)
+		return -ENOMEM;
+
+	mlxsw_reg_sfd_pack(sfd_pl, mlxsw_sp_sfd_op(adding), 0);
+	mlxsw_reg_sfd_mc_pack(sfd_pl, 0, addr, fid,
+			      MLXSW_REG_SFD_REC_ACTION_NOP, mid);
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(sfd), sfd_pl);
+	kfree(sfd_pl);
+	return err;
+}
+
+static int mlxsw_sp_port_smid_set(struct mlxsw_sp_port *mlxsw_sp_port, u16 mid,
+				  bool add, bool clear_all_ports)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	char *smid_pl;
+	int err, i;
+
+	smid_pl = kmalloc(MLXSW_REG_SMID_LEN, GFP_KERNEL);
+	if (!smid_pl)
+		return -ENOMEM;
+
+	mlxsw_reg_smid_pack(smid_pl, mid, mlxsw_sp_port->local_port, add);
+	if (clear_all_ports) {
+		for (i = 1; i < MLXSW_PORT_MAX_PORTS; i++)
+			if (mlxsw_sp->ports[i])
+				mlxsw_reg_smid_port_mask_set(smid_pl, i, 1);
+	}
+	err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(smid), smid_pl);
+	kfree(smid_pl);
+	return err;
+}
+
+static struct mlxsw_sp_mid *__mlxsw_sp_mc_get(struct mlxsw_sp *mlxsw_sp,
+					      const unsigned char *addr,
+					      u16 vid)
+{
+	struct mlxsw_sp_mid *mid;
+
+	list_for_each_entry(mid, &mlxsw_sp->br_mids.list, list) {
+		if (ether_addr_equal(mid->addr, addr) && mid->vid == vid)
+			return mid;
+	}
+	return NULL;
+}
+
+static struct mlxsw_sp_mid *__mlxsw_sp_mc_alloc(struct mlxsw_sp *mlxsw_sp,
+						const unsigned char *addr,
+						u16 vid)
+{
+	struct mlxsw_sp_mid *mid;
+	u16 mid_idx;
+
+	mid_idx = find_first_zero_bit(mlxsw_sp->br_mids.mapped,
+				      MLXSW_SP_MID_MAX);
+	if (mid_idx == MLXSW_SP_MID_MAX)
+		return NULL;
+
+	mid = kzalloc(sizeof(*mid), GFP_KERNEL);
+	if (!mid)
+		return NULL;
+
+	set_bit(mid_idx, mlxsw_sp->br_mids.mapped);
+	ether_addr_copy(mid->addr, addr);
+	mid->vid = vid;
+	mid->mid = mid_idx;
+	mid->ref_count = 0;
+	list_add_tail(&mid->list, &mlxsw_sp->br_mids.list);
+
+	return mid;
+}
+
+static int __mlxsw_sp_mc_dec_ref(struct mlxsw_sp *mlxsw_sp,
+				 struct mlxsw_sp_mid *mid)
+{
+	if (--mid->ref_count == 0) {
+		list_del(&mid->list);
+		clear_bit(mid->mid, mlxsw_sp->br_mids.mapped);
+		kfree(mid);
+		return 1;
+	}
+	return 0;
+}
+
+static int mlxsw_sp_port_mdb_add(struct mlxsw_sp_port *mlxsw_sp_port,
+				 const struct switchdev_obj_port_mdb *mdb,
+				 struct switchdev_trans *trans)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *dev = mlxsw_sp_port->dev;
+	struct mlxsw_sp_mid *mid;
+	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, mdb->vid);
+	int err = 0;
+
+	if (switchdev_trans_ph_prepare(trans))
+		return 0;
+
+	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, mdb->vid);
+	if (!mid) {
+		mid = __mlxsw_sp_mc_alloc(mlxsw_sp, mdb->addr, mdb->vid);
+		if (!mid) {
+			netdev_err(dev, "Unable to allocate MC group\n");
+			return -ENOMEM;
+		}
+	}
+	mid->ref_count++;
+
+	err = mlxsw_sp_port_smid_set(mlxsw_sp_port, mid->mid, true,
+				     mid->ref_count == 1);
+	if (err) {
+		netdev_err(dev, "Unable to set SMID\n");
+		goto err_out;
+	}
+
+	if (mid->ref_count == 1) {
+		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid, mid->mid,
+					   true);
+		if (err) {
+			netdev_err(dev, "Unable to set MC SFD\n");
+			goto err_out;
+		}
+	}
+
+	return 0;
+
+err_out:
+	__mlxsw_sp_mc_dec_ref(mlxsw_sp, mid);
+	return err;
 }
 
 static int mlxsw_sp_port_obj_add(struct net_device *dev,
@@ -494,8 +877,15 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
+	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
+	if (!mlxsw_sp_port)
+		return -EINVAL;
+
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
+		if (mlxsw_sp_port_is_vport(mlxsw_sp_port))
+			return 0;
+
 		err = mlxsw_sp_port_vlans_add(mlxsw_sp_port,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj),
 					      trans);
@@ -505,6 +895,11 @@ static int mlxsw_sp_port_obj_add(struct net_device *dev,
 						   SWITCHDEV_OBJ_PORT_FDB(obj),
 						   trans);
 		break;
+	case SWITCHDEV_OBJ_ID_PORT_MDB:
+		err = mlxsw_sp_port_mdb_add(mlxsw_sp_port,
+					    SWITCHDEV_OBJ_PORT_MDB(obj),
+					    trans);
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -532,7 +927,7 @@ static int __mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
 				     u16 vid_begin, u16 vid_end, bool init)
 {
 	struct net_device *dev = mlxsw_sp_port->dev;
-	u16 vid, vid_e;
+	u16 vid, pvid;
 	int err;
 
 	/* In case this is invoked with BRIDGE_FLAGS_SELF and port is
@@ -542,35 +937,26 @@ static int __mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
 	if (!init && !mlxsw_sp_port->bridged)
 		return mlxsw_sp_port_kill_vids(dev, vid_begin, vid_end);
 
-	for (vid = vid_begin; vid <= vid_end;
-	     vid += MLXSW_REG_SPVM_REC_MAX_COUNT) {
-		vid_e = min((u16) (vid + MLXSW_REG_SPVM_REC_MAX_COUNT - 1),
-			    vid_end);
-		err = mlxsw_sp_port_vlan_set(mlxsw_sp_port, vid, vid_e, false,
-					     false);
-		if (err) {
-			netdev_err(mlxsw_sp_port->dev, "Unable to del VIDs %d-%d\n",
-				   vid, vid_e);
-			return err;
-		}
+	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;
 	}
 
-	if ((mlxsw_sp_port->pvid >= vid_begin) &&
-	    (mlxsw_sp_port->pvid <= vid_end)) {
-		/* Default VLAN is always 1 */
-		mlxsw_sp_port->pvid = 1;
-		err = mlxsw_sp_port_pvid_set(mlxsw_sp_port,
-					     mlxsw_sp_port->pvid);
+	if (init)
+		goto out;
+
+	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(mlxsw_sp_port->dev, "Unable to del PVID %d\n",
-				   vid);
+			netdev_err(dev, "Unable to del PVID %d\n", pvid);
 			return err;
 		}
 	}
 
-	if (init)
-		goto out;
-
 	err = __mlxsw_sp_port_flood_set(mlxsw_sp_port, vid_begin, vid_end,
 					false, false);
 	if (err) {
@@ -602,12 +988,66 @@ static int mlxsw_sp_port_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port,
 					 vlan->vid_begin, vlan->vid_end, false);
 }
 
+void mlxsw_sp_port_active_vlans_del(struct mlxsw_sp_port *mlxsw_sp_port)
+{
+	u16 vid;
+
+	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID)
+		__mlxsw_sp_port_vlans_del(mlxsw_sp_port, vid, vid, false);
+}
+
 static int
 mlxsw_sp_port_fdb_static_del(struct mlxsw_sp_port *mlxsw_sp_port,
 			     const struct switchdev_obj_port_fdb *fdb)
 {
-	return mlxsw_sp_port_fdb_op(mlxsw_sp_port, fdb->addr, fdb->vid,
-				    false, false);
+	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, fdb->vid);
+	u16 lag_vid = 0;
+
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+	}
+
+	if (!mlxsw_sp_port->lagged)
+		return mlxsw_sp_port_fdb_uc_op(mlxsw_sp_port->mlxsw_sp,
+					       mlxsw_sp_port->local_port,
+					       fdb->addr, fid,
+					       false, false);
+	else
+		return mlxsw_sp_port_fdb_uc_lag_op(mlxsw_sp_port->mlxsw_sp,
+						   mlxsw_sp_port->lag_id,
+						   fdb->addr, fid, lag_vid,
+						   false, false);
+}
+
+static int mlxsw_sp_port_mdb_del(struct mlxsw_sp_port *mlxsw_sp_port,
+				 const struct switchdev_obj_port_mdb *mdb)
+{
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct net_device *dev = mlxsw_sp_port->dev;
+	struct mlxsw_sp_mid *mid;
+	u16 fid = mlxsw_sp_port_vid_to_fid_get(mlxsw_sp_port, mdb->vid);
+	u16 mid_idx;
+	int err = 0;
+
+	mid = __mlxsw_sp_mc_get(mlxsw_sp, mdb->addr, mdb->vid);
+	if (!mid) {
+		netdev_err(dev, "Unable to remove port from MC DB\n");
+		return -EINVAL;
+	}
+
+	err = mlxsw_sp_port_smid_set(mlxsw_sp_port, mid->mid, false, false);
+	if (err)
+		netdev_err(dev, "Unable to remove port from SMID\n");
+
+	mid_idx = mid->mid;
+	if (__mlxsw_sp_mc_dec_ref(mlxsw_sp, mid)) {
+		err = mlxsw_sp_port_mdb_op(mlxsw_sp, mdb->addr, fid, mid_idx,
+					   false);
+		if (err)
+			netdev_err(dev, "Unable to remove MC SFD\n");
+	}
+
+	return err;
 }
 
 static int mlxsw_sp_port_obj_del(struct net_device *dev,
@@ -616,8 +1056,15 @@ static int mlxsw_sp_port_obj_del(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
+	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
+	if (!mlxsw_sp_port)
+		return -EINVAL;
+
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
+		if (mlxsw_sp_port_is_vport(mlxsw_sp_port))
+			return 0;
+
 		err = mlxsw_sp_port_vlans_del(mlxsw_sp_port,
 					      SWITCHDEV_OBJ_PORT_VLAN(obj));
 		break;
@@ -625,6 +1072,10 @@ static int mlxsw_sp_port_obj_del(struct net_device *dev,
 		err = mlxsw_sp_port_fdb_static_del(mlxsw_sp_port,
 						   SWITCHDEV_OBJ_PORT_FDB(obj));
 		break;
+	case SWITCHDEV_OBJ_ID_PORT_MDB:
+		err = mlxsw_sp_port_mdb_del(mlxsw_sp_port,
+					    SWITCHDEV_OBJ_PORT_MDB(obj));
+		break;
 	default:
 		err = -EOPNOTSUPP;
 		break;
@@ -633,14 +1084,33 @@ static int mlxsw_sp_port_obj_del(struct net_device *dev,
 	return err;
 }
 
+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;
+	int i;
+
+	for (i = 0; i < MLXSW_SP_PORT_PER_LAG_MAX; i++) {
+		mlxsw_sp_port = mlxsw_sp_port_lagged_get(mlxsw_sp, lag_id, i);
+		if (mlxsw_sp_port)
+			return mlxsw_sp_port;
+	}
+	return NULL;
+}
+
 static int mlxsw_sp_port_fdb_dump(struct mlxsw_sp_port *mlxsw_sp_port,
 				  struct switchdev_obj_port_fdb *fdb,
-				  switchdev_obj_dump_cb_t *cb)
+				  switchdev_obj_dump_cb_t *cb,
+				  struct net_device *orig_dev)
 {
+	struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
+	struct mlxsw_sp_port *tmp;
+	u16 vport_fid = 0;
 	char *sfd_pl;
 	char mac[ETH_ALEN];
-	u16 vid;
+	u16 fid;
 	u8 local_port;
+	u16 lag_id;
 	u8 num_rec;
 	int stored_err = 0;
 	int i;
@@ -650,11 +1120,17 @@ static int mlxsw_sp_port_fdb_dump(struct mlxsw_sp_port *mlxsw_sp_port,
 	if (!sfd_pl)
 		return -ENOMEM;
 
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		u16 tmp;
+
+		tmp = mlxsw_sp_vport_vfid_get(mlxsw_sp_port);
+		vport_fid = mlxsw_sp_vfid_to_fid(tmp);
+	}
+
 	mlxsw_reg_sfd_pack(sfd_pl, MLXSW_REG_SFD_OP_QUERY_DUMP, 0);
 	do {
 		mlxsw_reg_sfd_num_rec_set(sfd_pl, MLXSW_REG_SFD_REC_MAX_COUNT);
-		err = mlxsw_reg_query(mlxsw_sp_port->mlxsw_sp->core,
-				      MLXSW_REG(sfd), sfd_pl);
+		err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(sfd), sfd_pl);
 		if (err)
 			goto out;
 
@@ -669,16 +1145,49 @@ static int mlxsw_sp_port_fdb_dump(struct mlxsw_sp_port *mlxsw_sp_port,
 		for (i = 0; i < num_rec; i++) {
 			switch (mlxsw_reg_sfd_rec_type_get(sfd_pl, i)) {
 			case MLXSW_REG_SFD_REC_TYPE_UNICAST:
-				mlxsw_reg_sfd_uc_unpack(sfd_pl, i, mac, &vid,
+				mlxsw_reg_sfd_uc_unpack(sfd_pl, i, mac, &fid,
 							&local_port);
 				if (local_port == mlxsw_sp_port->local_port) {
+					if (vport_fid && vport_fid == fid)
+						fdb->vid = 0;
+					else if (!vport_fid &&
+						 !mlxsw_sp_fid_is_vfid(fid))
+						fdb->vid = fid;
+					else
+						continue;
 					ether_addr_copy(fdb->addr, mac);
 					fdb->ndm_state = NUD_REACHABLE;
-					fdb->vid = vid;
 					err = cb(&fdb->obj);
 					if (err)
 						stored_err = err;
 				}
+				break;
+			case MLXSW_REG_SFD_REC_TYPE_UNICAST_LAG:
+				mlxsw_reg_sfd_uc_lag_unpack(sfd_pl, i,
+							    mac, &fid, &lag_id);
+				tmp = mlxsw_sp_lag_rep_port(mlxsw_sp, lag_id);
+				if (tmp && tmp->local_port ==
+				    mlxsw_sp_port->local_port) {
+					/* LAG records can only point to LAG
+					 * devices or VLAN devices on top.
+					 */
+					if (!netif_is_lag_master(orig_dev) &&
+					    !is_vlan_dev(orig_dev))
+						continue;
+					if (vport_fid && vport_fid == fid)
+						fdb->vid = 0;
+					else if (!vport_fid &&
+						 !mlxsw_sp_fid_is_vfid(fid))
+						fdb->vid = fid;
+					else
+						continue;
+					ether_addr_copy(fdb->addr, mac);
+					fdb->ndm_state = NUD_REACHABLE;
+					err = cb(&fdb->obj);
+					if (err)
+						stored_err = err;
+				}
+				break;
 			}
 		}
 	} while (num_rec == MLXSW_REG_SFD_REC_MAX_COUNT);
@@ -695,10 +1204,19 @@ static int mlxsw_sp_port_vlan_dump(struct mlxsw_sp_port *mlxsw_sp_port,
 	u16 vid;
 	int err = 0;
 
+	if (mlxsw_sp_port_is_vport(mlxsw_sp_port)) {
+		vlan->flags = 0;
+		vlan->vid_begin = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+		vlan->vid_end = mlxsw_sp_vport_vid_get(mlxsw_sp_port);
+		return cb(&vlan->obj);
+	}
+
 	for_each_set_bit(vid, mlxsw_sp_port->active_vlans, VLAN_N_VID) {
 		vlan->flags = 0;
 		if (vid == mlxsw_sp_port->pvid)
 			vlan->flags |= BRIDGE_VLAN_INFO_PVID;
+		if (test_bit(vid, mlxsw_sp_port->untagged_vlans))
+			vlan->flags |= BRIDGE_VLAN_INFO_UNTAGGED;
 		vlan->vid_begin = vid;
 		vlan->vid_end = vid;
 		err = cb(&vlan->obj);
@@ -715,6 +1233,10 @@ static int mlxsw_sp_port_obj_dump(struct net_device *dev,
 	struct mlxsw_sp_port *mlxsw_sp_port = netdev_priv(dev);
 	int err = 0;
 
+	mlxsw_sp_port = mlxsw_sp_port_orig_get(obj->orig_dev, mlxsw_sp_port);
+	if (!mlxsw_sp_port)
+		return -EINVAL;
+
 	switch (obj->id) {
 	case SWITCHDEV_OBJ_ID_PORT_VLAN:
 		err = mlxsw_sp_port_vlan_dump(mlxsw_sp_port,
@@ -722,7 +1244,8 @@ static int mlxsw_sp_port_obj_dump(struct net_device *dev,
 		break;
 	case SWITCHDEV_OBJ_ID_PORT_FDB:
 		err = mlxsw_sp_port_fdb_dump(mlxsw_sp_port,
-					     SWITCHDEV_OBJ_PORT_FDB(obj), cb);
+					     SWITCHDEV_OBJ_PORT_FDB(obj), cb,
+					     obj->orig_dev);
 		break;
 	default:
 		err = -EOPNOTSUPP;
@@ -740,6 +1263,21 @@ static const struct switchdev_ops mlxsw_sp_port_switchdev_ops = {
 	.switchdev_port_obj_dump	= mlxsw_sp_port_obj_dump,
 };
 
+static void mlxsw_sp_fdb_call_notifiers(bool learning_sync, bool adding,
+					char *mac, u16 vid,
+					struct net_device *dev)
+{
+	struct switchdev_notifier_fdb_info info;
+	unsigned long notifier_type;
+
+	if (learning_sync) {
+		info.addr = mac;
+		info.vid = vid;
+		notifier_type = adding ? SWITCHDEV_FDB_ADD : SWITCHDEV_FDB_DEL;
+		call_switchdev_notifiers(notifier_type, dev, &info.info);
+	}
+}
+
 static void mlxsw_sp_fdb_notify_mac_process(struct mlxsw_sp *mlxsw_sp,
 					    char *sfn_pl, int rec_index,
 					    bool adding)
@@ -747,34 +1285,119 @@ static void mlxsw_sp_fdb_notify_mac_process(struct mlxsw_sp *mlxsw_sp,
 	struct mlxsw_sp_port *mlxsw_sp_port;
 	char mac[ETH_ALEN];
 	u8 local_port;
-	u16 vid;
+	u16 vid, fid;
+	bool do_notification = true;
 	int err;
 
-	mlxsw_reg_sfn_mac_unpack(sfn_pl, rec_index, mac, &vid, &local_port);
+	mlxsw_reg_sfn_mac_unpack(sfn_pl, rec_index, mac, &fid, &local_port);
 	mlxsw_sp_port = mlxsw_sp->ports[local_port];
 	if (!mlxsw_sp_port) {
 		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Incorrect local port in FDB notification\n");
-		return;
+		goto just_remove;
+	}
+
+	if (mlxsw_sp_fid_is_vfid(fid)) {
+		u16 vfid = mlxsw_sp_fid_to_vfid(fid);
+		struct mlxsw_sp_port *mlxsw_sp_vport;
+
+		mlxsw_sp_vport = mlxsw_sp_port_vport_find_by_vfid(mlxsw_sp_port,
+								  vfid);
+		if (!mlxsw_sp_vport) {
+			netdev_err(mlxsw_sp_port->dev, "Failed to find a matching vPort following FDB notification\n");
+			goto just_remove;
+		}
+		vid = 0;
+		/* Override the physical port with the vPort. */
+		mlxsw_sp_port = mlxsw_sp_vport;
+	} else {
+		vid = fid;
 	}
 
-	err = mlxsw_sp_port_fdb_op(mlxsw_sp_port, mac, vid,
-				   adding && mlxsw_sp_port->learning, true);
+	adding = adding && mlxsw_sp_port->learning;
+
+do_fdb_op:
+	err = mlxsw_sp_port_fdb_uc_op(mlxsw_sp, local_port, mac, fid,
+				      adding, true);
 	if (err) {
 		if (net_ratelimit())
 			netdev_err(mlxsw_sp_port->dev, "Failed to set FDB entry\n");
 		return;
 	}
 
-	if (mlxsw_sp_port->learning && mlxsw_sp_port->learning_sync) {
-		struct switchdev_notifier_fdb_info info;
-		unsigned long notifier_type;
+	if (!do_notification)
+		return;
+	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync,
+				    adding, mac, vid, mlxsw_sp_port->dev);
+	return;
+
+just_remove:
+	adding = false;
+	do_notification = false;
+	goto do_fdb_op;
+}
 
-		info.addr = mac;
-		info.vid = vid;
-		notifier_type = adding ? SWITCHDEV_FDB_ADD : SWITCHDEV_FDB_DEL;
-		call_switchdev_notifiers(notifier_type, mlxsw_sp_port->dev,
-					 &info.info);
+static void mlxsw_sp_fdb_notify_mac_lag_process(struct mlxsw_sp *mlxsw_sp,
+						char *sfn_pl, int rec_index,
+						bool adding)
+{
+	struct mlxsw_sp_port *mlxsw_sp_port;
+	struct net_device *dev;
+	char mac[ETH_ALEN];
+	u16 lag_vid = 0;
+	u16 lag_id;
+	u16 vid, fid;
+	bool do_notification = true;
+	int err;
+
+	mlxsw_reg_sfn_mac_lag_unpack(sfn_pl, rec_index, mac, &fid, &lag_id);
+	mlxsw_sp_port = mlxsw_sp_lag_rep_port(mlxsw_sp, lag_id);
+	if (!mlxsw_sp_port) {
+		dev_err_ratelimited(mlxsw_sp->bus_info->dev, "Cannot find port representor for LAG\n");
+		goto just_remove;
+	}
+
+	if (mlxsw_sp_fid_is_vfid(fid)) {
+		u16 vfid = mlxsw_sp_fid_to_vfid(fid);
+		struct mlxsw_sp_port *mlxsw_sp_vport;
+
+		mlxsw_sp_vport = mlxsw_sp_port_vport_find_by_vfid(mlxsw_sp_port,
+								  vfid);
+		if (!mlxsw_sp_vport) {
+			netdev_err(mlxsw_sp_port->dev, "Failed to find a matching vPort following FDB notification\n");
+			goto just_remove;
+		}
+
+		lag_vid = mlxsw_sp_vport_vid_get(mlxsw_sp_vport);
+		dev = mlxsw_sp_vport->dev;
+		vid = 0;
+		/* Override the physical port with the vPort. */
+		mlxsw_sp_port = mlxsw_sp_vport;
+	} else {
+		dev = mlxsw_sp_lag_get(mlxsw_sp, lag_id)->dev;
+		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);
+	if (err) {
+		if (net_ratelimit())
+			netdev_err(mlxsw_sp_port->dev, "Failed to set FDB entry\n");
+		return;
+	}
+
+	if (!do_notification)
+		return;
+	mlxsw_sp_fdb_call_notifiers(mlxsw_sp_port->learning_sync, adding, mac,
+				    vid, dev);
+	return;
+
+just_remove:
+	adding = false;
+	do_notification = false;
+	goto do_fdb_op;
 }
 
 static void mlxsw_sp_fdb_notify_rec_process(struct mlxsw_sp *mlxsw_sp,
@@ -789,6 +1412,14 @@ static void mlxsw_sp_fdb_notify_rec_process(struct mlxsw_sp *mlxsw_sp,
 		mlxsw_sp_fdb_notify_mac_process(mlxsw_sp, sfn_pl,
 						rec_index, false);
 		break;
+	case MLXSW_REG_SFN_REC_TYPE_LEARNED_MAC_LAG:
+		mlxsw_sp_fdb_notify_mac_lag_process(mlxsw_sp, sfn_pl,
+						    rec_index, true);
+		break;
+	case MLXSW_REG_SFN_REC_TYPE_AGED_OUT_MAC_LAG:
+		mlxsw_sp_fdb_notify_mac_lag_process(mlxsw_sp, sfn_pl,
+						    rec_index, false);
+		break;
 	}
 }
 
@@ -812,6 +1443,7 @@ 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);
@@ -824,6 +1456,7 @@ static void mlxsw_sp_fdb_notify_work(struct work_struct *work)
 			mlxsw_sp_fdb_notify_rec_process(mlxsw_sp, sfn_pl, i);
 
 	} while (num_rec);
+	rtnl_unlock();
 
 	kfree(sfn_pl);
 	mlxsw_sp_fdb_notify_work_schedule(mlxsw_sp);
@@ -877,7 +1510,8 @@ int mlxsw_sp_port_vlan_init(struct mlxsw_sp_port *mlxsw_sp_port)
 	 * with VID 1.
 	 */
 	mlxsw_sp_port->pvid = 1;
-	err = __mlxsw_sp_port_vlans_del(mlxsw_sp_port, 0, VLAN_N_VID, true);
+	err = __mlxsw_sp_port_vlans_del(mlxsw_sp_port, 0, VLAN_N_VID - 1,
+					true);
 	if (err) {
 		netdev_err(dev, "Unable to init VLANs\n");
 		return err;
diff --git a/drivers/net/ethernet/microchip/encx24j600.c b/drivers/net/ethernet/microchip/encx24j600.c
index 2056b719c262..7df318346b05 100644
--- a/drivers/net/ethernet/microchip/encx24j600.c
+++ b/drivers/net/ethernet/microchip/encx24j600.c
@@ -600,22 +600,11 @@ static void encx24j600_set_rxfilter_mode(struct encx24j600_priv *priv)
 
 static int encx24j600_hw_init(struct encx24j600_priv *priv)
 {
-	struct net_device *dev = priv->ndev;
 	int ret = 0;
-	u16 eidled;
 	u16 macon2;
 
 	priv->hw_enabled = false;
 
-	eidled = encx24j600_read_reg(priv, EIDLED);
-	if (((eidled & DEVID_MASK) >> DEVID_SHIFT) != ENCX24J600_DEV_ID) {
-		ret = -EINVAL;
-		goto err_out;
-	}
-
-	netif_info(priv, drv, dev, "Silicon rev ID: 0x%02x\n",
-		   (eidled & REVID_MASK) >> REVID_SHIFT);
-
 	/* PHY Leds: link status,
 	 * LEDA: Link State + collision events
 	 * LEDB: Link State + transmit/receive events
@@ -655,7 +644,6 @@ static int encx24j600_hw_init(struct encx24j600_priv *priv)
 	if (netif_msg_hw(priv))
 		encx24j600_dump_config(priv, "Hw is initialized");
 
-err_out:
 	return ret;
 }
 
@@ -1004,6 +992,7 @@ static int encx24j600_spi_probe(struct spi_device *spi)
 
 	struct net_device *ndev;
 	struct encx24j600_priv *priv;
+	u16 eidled;
 
 	ndev = alloc_etherdev(sizeof(struct encx24j600_priv));
 
@@ -1072,10 +1061,21 @@ static int encx24j600_spi_probe(struct spi_device *spi)
 		goto out_free;
 	}
 
+	eidled = encx24j600_read_reg(priv, EIDLED);
+	if (((eidled & DEVID_MASK) >> DEVID_SHIFT) != ENCX24J600_DEV_ID) {
+		ret = -EINVAL;
+		goto out_unregister;
+	}
+
+	netif_info(priv, probe, ndev, "Silicon rev ID: 0x%02x\n",
+		   (eidled & REVID_MASK) >> REVID_SHIFT);
+
 	netif_info(priv, drv, priv->ndev, "MAC address %pM\n", ndev->dev_addr);
 
 	return ret;
 
+out_unregister:
+	unregister_netdev(priv->ndev);
 out_free:
 	free_netdev(ndev);
 
diff --git a/drivers/net/ethernet/moxa/moxart_ether.c b/drivers/net/ethernet/moxa/moxart_ether.c
index a10c928bbd6b..3e67f451f2ab 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.c
+++ b/drivers/net/ethernet/moxa/moxart_ether.c
@@ -28,6 +28,16 @@
 
 #include "moxart_ether.h"
 
+static inline void moxart_desc_write(u32 data, u32 *desc)
+{
+	*desc = cpu_to_le32(data);
+}
+
+static inline u32 moxart_desc_read(u32 *desc)
+{
+	return le32_to_cpu(*desc);
+}
+
 static inline void moxart_emac_write(struct net_device *ndev,
 				     unsigned int reg, unsigned long value)
 {
@@ -112,7 +122,7 @@ static void moxart_mac_enable(struct net_device *ndev)
 static void moxart_mac_setup_desc_ring(struct net_device *ndev)
 {
 	struct moxart_mac_priv_t *priv = netdev_priv(ndev);
-	void __iomem *desc;
+	void *desc;
 	int i;
 
 	for (i = 0; i < TX_DESC_NUM; i++) {
@@ -121,7 +131,7 @@ static void moxart_mac_setup_desc_ring(struct net_device *ndev)
 
 		priv->tx_buf[i] = priv->tx_buf_base + priv->tx_buf_size * i;
 	}
-	writel(TX_DESC1_END, desc + TX_REG_OFFSET_DESC1);
+	moxart_desc_write(TX_DESC1_END, desc + TX_REG_OFFSET_DESC1);
 
 	priv->tx_head = 0;
 	priv->tx_tail = 0;
@@ -129,8 +139,8 @@ static void moxart_mac_setup_desc_ring(struct net_device *ndev)
 	for (i = 0; i < RX_DESC_NUM; i++) {
 		desc = priv->rx_desc_base + i * RX_REG_DESC_SIZE;
 		memset(desc, 0, RX_REG_DESC_SIZE);
-		writel(RX_DESC0_DMA_OWN, desc + RX_REG_OFFSET_DESC0);
-		writel(RX_BUF_SIZE & RX_DESC1_BUF_SIZE_MASK,
+		moxart_desc_write(RX_DESC0_DMA_OWN, desc + RX_REG_OFFSET_DESC0);
+		moxart_desc_write(RX_BUF_SIZE & RX_DESC1_BUF_SIZE_MASK,
 		       desc + RX_REG_OFFSET_DESC1);
 
 		priv->rx_buf[i] = priv->rx_buf_base + priv->rx_buf_size * i;
@@ -141,12 +151,12 @@ static void moxart_mac_setup_desc_ring(struct net_device *ndev)
 		if (dma_mapping_error(&ndev->dev, priv->rx_mapping[i]))
 			netdev_err(ndev, "DMA mapping error\n");
 
-		writel(priv->rx_mapping[i],
+		moxart_desc_write(priv->rx_mapping[i],
 		       desc + RX_REG_OFFSET_DESC2 + RX_DESC2_ADDRESS_PHYS);
-		writel(priv->rx_buf[i],
+		moxart_desc_write((uintptr_t)priv->rx_buf[i],
 		       desc + RX_REG_OFFSET_DESC2 + RX_DESC2_ADDRESS_VIRT);
 	}
-	writel(RX_DESC1_END, desc + RX_REG_OFFSET_DESC1);
+	moxart_desc_write(RX_DESC1_END, desc + RX_REG_OFFSET_DESC1);
 
 	priv->rx_head = 0;
 
@@ -201,14 +211,15 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 						      napi);
 	struct net_device *ndev = priv->ndev;
 	struct sk_buff *skb;
-	void __iomem *desc;
+	void *desc;
 	unsigned int desc0, len;
 	int rx_head = priv->rx_head;
 	int rx = 0;
 
 	while (rx < budget) {
 		desc = priv->rx_desc_base + (RX_REG_DESC_SIZE * rx_head);
-		desc0 = readl(desc + RX_REG_OFFSET_DESC0);
+		desc0 = moxart_desc_read(desc + RX_REG_OFFSET_DESC0);
+		rmb(); /* ensure desc0 is up to date */
 
 		if (desc0 & RX_DESC0_DMA_OWN)
 			break;
@@ -250,7 +261,8 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 			priv->stats.multicast++;
 
 rx_next:
-		writel(RX_DESC0_DMA_OWN, desc + RX_REG_OFFSET_DESC0);
+		wmb(); /* prevent setting ownership back too early */
+		moxart_desc_write(RX_DESC0_DMA_OWN, desc + RX_REG_OFFSET_DESC0);
 
 		rx_head = RX_NEXT(rx_head);
 		priv->rx_head = rx_head;
@@ -310,7 +322,7 @@ static irqreturn_t moxart_mac_interrupt(int irq, void *dev_id)
 static int moxart_mac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct moxart_mac_priv_t *priv = netdev_priv(ndev);
-	void __iomem *desc;
+	void *desc;
 	unsigned int len;
 	unsigned int tx_head = priv->tx_head;
 	u32 txdes1;
@@ -319,11 +331,12 @@ static int moxart_mac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	desc = priv->tx_desc_base + (TX_REG_DESC_SIZE * tx_head);
 
 	spin_lock_irq(&priv->txlock);
-	if (readl(desc + TX_REG_OFFSET_DESC0) & TX_DESC0_DMA_OWN) {
+	if (moxart_desc_read(desc + TX_REG_OFFSET_DESC0) & TX_DESC0_DMA_OWN) {
 		net_dbg_ratelimited("no TX space for packet\n");
 		priv->stats.tx_dropped++;
 		goto out_unlock;
 	}
+	rmb(); /* ensure data is only read that had TX_DESC0_DMA_OWN cleared */
 
 	len = skb->len > TX_BUF_SIZE ? TX_BUF_SIZE : skb->len;
 
@@ -337,9 +350,9 @@ static int moxart_mac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	priv->tx_len[tx_head] = len;
 	priv->tx_skb[tx_head] = skb;
 
-	writel(priv->tx_mapping[tx_head],
+	moxart_desc_write(priv->tx_mapping[tx_head],
 	       desc + TX_REG_OFFSET_DESC2 + TX_DESC2_ADDRESS_PHYS);
-	writel(skb->data,
+	moxart_desc_write((uintptr_t)skb->data,
 	       desc + TX_REG_OFFSET_DESC2 + TX_DESC2_ADDRESS_VIRT);
 
 	if (skb->len < ETH_ZLEN) {
@@ -354,8 +367,9 @@ static int moxart_mac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	txdes1 = TX_DESC1_LTS | TX_DESC1_FTS | (len & TX_DESC1_BUF_SIZE_MASK);
 	if (tx_head == TX_DESC_NUM_MASK)
 		txdes1 |= TX_DESC1_END;
-	writel(txdes1, desc + TX_REG_OFFSET_DESC1);
-	writel(TX_DESC0_DMA_OWN, desc + TX_REG_OFFSET_DESC0);
+	moxart_desc_write(txdes1, desc + TX_REG_OFFSET_DESC1);
+	wmb(); /* flush descriptor before transferring ownership */
+	moxart_desc_write(TX_DESC0_DMA_OWN, desc + TX_REG_OFFSET_DESC0);
 
 	/* start to send packet */
 	writel(0xffffffff, priv->base + REG_TX_POLL_DEMAND);
@@ -460,9 +474,9 @@ static int moxart_mac_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ndev->base_addr = res->start;
 	priv->base = devm_ioremap_resource(p_dev, res);
-	ret = IS_ERR(priv->base);
-	if (ret) {
+	if (IS_ERR(priv->base)) {
 		dev_err(p_dev, "devm_ioremap_resource failed\n");
+		ret = PTR_ERR(priv->base);
 		goto init_fail;
 	}
 
diff --git a/drivers/net/ethernet/moxa/moxart_ether.h b/drivers/net/ethernet/moxa/moxart_ether.h
index 2be9280d608c..93a9563ac7c6 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.h
+++ b/drivers/net/ethernet/moxa/moxart_ether.h
@@ -300,7 +300,7 @@ struct moxart_mac_priv_t {
 
 	dma_addr_t rx_base;
 	dma_addr_t rx_mapping[RX_DESC_NUM];
-	void __iomem *rx_desc_base;
+	void *rx_desc_base;
 	unsigned char *rx_buf_base;
 	unsigned char *rx_buf[RX_DESC_NUM];
 	unsigned int rx_head;
@@ -308,7 +308,7 @@ struct moxart_mac_priv_t {
 
 	dma_addr_t tx_base;
 	dma_addr_t tx_mapping[TX_DESC_NUM];
-	void __iomem *tx_desc_base;
+	void *tx_desc_base;
 	unsigned char *tx_buf_base;
 	unsigned char *tx_buf[RX_DESC_NUM];
 	unsigned int tx_head;
diff --git a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
index 83651ac8ddb9..270c9eeb7ab6 100644
--- a/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
+++ b/drivers/net/ethernet/myricom/myri10ge/myri10ge.c
@@ -1488,7 +1488,6 @@ myri10ge_rx_done(struct myri10ge_slice_state *ss, int len, __wsum csum)
 	}
 	myri10ge_vlan_rx(mgp->dev, va, skb);
 	skb_record_rx_queue(skb, ss - &mgp->ss[0]);
-	skb_mark_napi_id(skb, &ss->napi);
 
 	if (polling) {
 		int hlen;
@@ -1506,6 +1505,7 @@ myri10ge_rx_done(struct myri10ge_slice_state *ss, int len, __wsum csum)
 		skb->data_len -= hlen;
 		skb->tail += hlen;
 		skb->protocol = eth_type_trans(skb, dev);
+		skb_mark_napi_id(skb, &ss->napi);
 		netif_receive_skb(skb);
 	}
 	else
@@ -3814,7 +3814,6 @@ static int myri10ge_alloc_slices(struct myri10ge_priv *mgp)
 		ss->dev = mgp->dev;
 		netif_napi_add(ss->dev, &ss->napi, myri10ge_poll,
 			       myri10ge_napi_weight);
-		napi_hash_add(&ss->napi);
 	}
 	return 0;
 abort:
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index 50d5604833ed..e0993eba5df3 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -2223,8 +2223,6 @@ static irqreturn_t vxge_isr_napi(int irq, void *dev_id)
 	return IRQ_NONE;
 }
 
-#ifdef CONFIG_PCI_MSI
-
 static irqreturn_t vxge_tx_msix_handle(int irq, void *dev_id)
 {
 	struct vxge_fifo *fifo = (struct vxge_fifo *)dev_id;
@@ -2442,16 +2440,13 @@ static void vxge_rem_msix_isr(struct vxgedev *vdev)
 	if (vdev->config.intr_type == MSI_X)
 		pci_disable_msix(vdev->pdev);
 }
-#endif
 
 static void vxge_rem_isr(struct vxgedev *vdev)
 {
-#ifdef CONFIG_PCI_MSI
-	if (vdev->config.intr_type == MSI_X) {
+	if (IS_ENABLED(CONFIG_PCI_MSI) &&
+	    vdev->config.intr_type == MSI_X) {
 		vxge_rem_msix_isr(vdev);
-	} else
-#endif
-	if (vdev->config.intr_type == INTA) {
+	} else if (vdev->config.intr_type == INTA) {
 			synchronize_irq(vdev->pdev->irq);
 			free_irq(vdev->pdev->irq, vdev);
 	}
@@ -2460,11 +2455,10 @@ static void vxge_rem_isr(struct vxgedev *vdev)
 static int vxge_add_isr(struct vxgedev *vdev)
 {
 	int ret = 0;
-#ifdef CONFIG_PCI_MSI
 	int vp_idx = 0, intr_idx = 0, intr_cnt = 0, msix_idx = 0, irq_req = 0;
 	int pci_fun = PCI_FUNC(vdev->pdev->devfn);
 
-	if (vdev->config.intr_type == MSI_X)
+	if (IS_ENABLED(CONFIG_PCI_MSI) && vdev->config.intr_type == MSI_X)
 		ret = vxge_enable_msix(vdev);
 
 	if (ret) {
@@ -2475,7 +2469,7 @@ static int vxge_add_isr(struct vxgedev *vdev)
 		vdev->config.intr_type = INTA;
 	}
 
-	if (vdev->config.intr_type == MSI_X) {
+	if (IS_ENABLED(CONFIG_PCI_MSI) && vdev->config.intr_type == MSI_X) {
 		for (intr_idx = 0;
 		     intr_idx < (vdev->no_of_vpath *
 			VXGE_HW_VPATH_MSIX_ACTIVE); intr_idx++) {
@@ -2576,9 +2570,8 @@ static int vxge_add_isr(struct vxgedev *vdev)
 		vdev->vxge_entries[intr_cnt].in_use = 1;
 		vdev->vxge_entries[intr_cnt].arg = &vdev->vpaths[0];
 	}
-INTA_MODE:
-#endif
 
+INTA_MODE:
 	if (vdev->config.intr_type == INTA) {
 		snprintf(vdev->desc[0], VXGE_INTR_STRLEN,
 			"%s:vxge:INTA", vdev->ndev->name);
@@ -3889,12 +3882,12 @@ static void vxge_device_config_init(struct vxge_hw_device_config *device_config,
 	if (max_mac_vpath > VXGE_MAX_MAC_ADDR_COUNT)
 		max_mac_vpath = VXGE_MAX_MAC_ADDR_COUNT;
 
-#ifndef CONFIG_PCI_MSI
-	vxge_debug_init(VXGE_ERR,
-		"%s: This Kernel does not support "
-		"MSI-X. Defaulting to INTA", VXGE_DRIVER_NAME);
-	*intr_type = INTA;
-#endif
+	if (!IS_ENABLED(CONFIG_PCI_MSI)) {
+		vxge_debug_init(VXGE_ERR,
+			"%s: This Kernel does not support "
+			"MSI-X. Defaulting to INTA", VXGE_DRIVER_NAME);
+		*intr_type = INTA;
+	}
 
 	/* Configure whether MSI-X or IRQL. */
 	switch (*intr_type) {
diff --git a/drivers/net/ethernet/netronome/Kconfig b/drivers/net/ethernet/netronome/Kconfig
new file mode 100644
index 000000000000..9508ad782c30
--- /dev/null
+++ b/drivers/net/ethernet/netronome/Kconfig
@@ -0,0 +1,36 @@
+#
+# Netronome device configuration
+#
+
+config NET_VENDOR_NETRONOME
+	bool "Netronome(R) devices"
+	default y
+	---help---
+	  If you have a Netronome(R) network (Ethernet) card or device, 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 Netronome(R) cards. If you say Y, you will be
+	  asked for your specific card in the following questions.
+
+if NET_VENDOR_NETRONOME
+
+config NFP_NETVF
+	tristate "Netronome(R) NFP4000/NFP6000 VF NIC driver"
+	depends on PCI && PCI_MSI
+	depends on VXLAN || VXLAN=n
+	---help---
+	  This driver supports SR-IOV virtual functions of
+	  the Netronome(R) NFP4000/NFP6000 cards working as
+	  a advanced Ethernet NIC.
+
+config NFP_NET_DEBUG
+	bool "Debug support for Netronome(R) NFP3200/NFP6000 NIC drivers"
+	depends on NFP_NET || NFP_NETVF
+	---help---
+	  Enable extra sanity checks and debugfs support in
+	  Netronome(R) NFP3200/NFP6000 NIC PF and VF drivers.
+	  Note: selecting this option may adversely impact
+		performance.
+
+endif
diff --git a/drivers/net/ethernet/netronome/Makefile b/drivers/net/ethernet/netronome/Makefile
new file mode 100644
index 000000000000..dcb7b383f634
--- /dev/null
+++ b/drivers/net/ethernet/netronome/Makefile
@@ -0,0 +1,5 @@
+#
+# Makefile for the Netronome network device drivers
+#
+
+obj-$(CONFIG_NFP_NETVF) += nfp/
diff --git a/drivers/net/ethernet/netronome/nfp/Makefile b/drivers/net/ethernet/netronome/nfp/Makefile
new file mode 100644
index 000000000000..68178819ff12
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/Makefile
@@ -0,0 +1,8 @@
+obj-$(CONFIG_NFP_NETVF)	+= nfp_netvf.o
+
+nfp_netvf-objs := \
+	    nfp_net_common.o \
+	    nfp_net_ethtool.o \
+	    nfp_netvf_main.o
+
+nfp_netvf-$(CONFIG_NFP_NET_DEBUG) += nfp_net_debugfs.o
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net.h b/drivers/net/ethernet/netronome/nfp/nfp_net.h
new file mode 100644
index 000000000000..ab264e1bccd0
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net.h
@@ -0,0 +1,748 @@
+/*
+ * Copyright (C) 2015 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.h
+ * Declarations for Netronome network device driver.
+ * Authors: Jakub Kicinski <jakub.kicinski@netronome.com>
+ *          Jason McMullan <jason.mcmullan@netronome.com>
+ *          Rolf Neugebauer <rolf.neugebauer@netronome.com>
+ */
+
+#ifndef _NFP_NET_H_
+#define _NFP_NET_H_
+
+#include <linux/interrupt.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <asm-generic/io-64-nonatomic-hi-lo.h>
+
+#include "nfp_net_ctrl.h"
+
+#define nn_err(nn, fmt, args...)  netdev_err((nn)->netdev, fmt, ## args)
+#define nn_warn(nn, fmt, args...) netdev_warn((nn)->netdev, fmt, ## args)
+#define nn_info(nn, fmt, args...) netdev_info((nn)->netdev, fmt, ## args)
+#define nn_dbg(nn, fmt, args...)  netdev_dbg((nn)->netdev, fmt, ## args)
+#define nn_warn_ratelimit(nn, fmt, args...)				\
+	do {								\
+		if (unlikely(net_ratelimit()))				\
+			netdev_warn((nn)->netdev, fmt, ## args);	\
+	} while (0)
+
+/* Max time to wait for NFP to respond on updates (in ms) */
+#define NFP_NET_POLL_TIMEOUT	5000
+
+/* Bar allocation */
+#define NFP_NET_CRTL_BAR	0
+#define NFP_NET_Q0_BAR		2
+#define NFP_NET_Q1_BAR		4	/* OBSOLETE */
+
+/* Max bits in DMA address */
+#define NFP_NET_MAX_DMA_BITS	40
+
+/* Default size for MTU and freelist buffer sizes */
+#define NFP_NET_DEFAULT_MTU		1500
+#define NFP_NET_DEFAULT_RX_BUFSZ	2048
+
+/* Maximum number of bytes prepended to a packet */
+#define NFP_NET_MAX_PREPEND		64
+
+/* Interrupt definitions */
+#define NFP_NET_NON_Q_VECTORS		2
+#define NFP_NET_IRQ_LSC_IDX		0
+#define NFP_NET_IRQ_EXN_IDX		1
+
+/* Queue/Ring definitions */
+#define NFP_NET_MAX_TX_RINGS	64	/* Max. # of Tx rings per device */
+#define NFP_NET_MAX_RX_RINGS	64	/* Max. # of Rx rings per device */
+
+#define NFP_NET_MIN_TX_DESCS	256	/* Min. # of Tx descs per ring */
+#define NFP_NET_MIN_RX_DESCS	256	/* Min. # of Rx descs per ring */
+#define NFP_NET_MAX_TX_DESCS	(256 * 1024) /* Max. # of Tx descs per ring */
+#define NFP_NET_MAX_RX_DESCS	(256 * 1024) /* Max. # of Rx descs per ring */
+
+#define NFP_NET_TX_DESCS_DEFAULT 4096	/* Default # of Tx descs per ring */
+#define NFP_NET_RX_DESCS_DEFAULT 4096	/* Default # of Rx descs per ring */
+
+#define NFP_NET_FL_BATCH	16	/* Add freelist in this Batch size */
+
+/* Offload definitions */
+#define NFP_NET_N_VXLAN_PORTS	(NFP_NET_CFG_VXLAN_SZ / sizeof(__be16))
+
+/* Forward declarations */
+struct nfp_net;
+struct nfp_net_r_vector;
+
+/* Convenience macro for writing dma address into RX/TX descriptors */
+#define nfp_desc_set_dma_addr(desc, dma_addr)				\
+	do {								\
+		__typeof(desc) __d = (desc);				\
+		dma_addr_t __addr = (dma_addr);				\
+									\
+		__d->dma_addr_lo = cpu_to_le32(lower_32_bits(__addr));	\
+		__d->dma_addr_hi = upper_32_bits(__addr) & 0xff;	\
+	} while (0)
+
+/* TX descriptor format */
+
+#define PCIE_DESC_TX_EOP		BIT(7)
+#define PCIE_DESC_TX_OFFSET_MASK	GENMASK(6, 0)
+#define PCIE_DESC_TX_MSS_MASK		GENMASK(13, 0)
+
+/* Flags in the host TX descriptor */
+#define PCIE_DESC_TX_CSUM		BIT(7)
+#define PCIE_DESC_TX_IP4_CSUM		BIT(6)
+#define PCIE_DESC_TX_TCP_CSUM		BIT(5)
+#define PCIE_DESC_TX_UDP_CSUM		BIT(4)
+#define PCIE_DESC_TX_VLAN		BIT(3)
+#define PCIE_DESC_TX_LSO		BIT(2)
+#define PCIE_DESC_TX_ENCAP		BIT(1)
+#define PCIE_DESC_TX_O_IP4_CSUM	BIT(0)
+
+struct nfp_net_tx_desc {
+	union {
+		struct {
+			u8 dma_addr_hi; /* High bits of host buf address */
+			__le16 dma_len;	/* Length to DMA for this desc */
+			u8 offset_eop;	/* Offset in buf where pkt starts +
+					 * highest bit is eop flag.
+					 */
+			__le32 dma_addr_lo; /* Low 32bit of host buf addr */
+
+			__le16 mss;	/* MSS to be used for LSO */
+			u8 l4_offset;	/* LSO, where the L4 data starts */
+			u8 flags;	/* TX Flags, see @PCIE_DESC_TX_* */
+
+			__le16 vlan;	/* VLAN tag to add if indicated */
+			__le16 data_len; /* Length of frame + meta data */
+		} __packed;
+		__le32 vals[4];
+	};
+};
+
+/**
+ * struct nfp_net_tx_buf - software TX buffer descriptor
+ * @skb:	sk_buff associated with this buffer
+ * @dma_addr:	DMA mapping address of the buffer
+ * @fidx:	Fragment index (-1 for the head and [0..nr_frags-1] for frags)
+ * @pkt_cnt:	Number of packets to be produced out of the skb associated
+ *		with this buffer (valid only on the head's buffer).
+ *		Will be 1 for all non-TSO packets.
+ * @real_len:	Number of bytes which to be produced out of the skb (valid only
+ *		on the head's buffer). Equal to skb->len for non-TSO packets.
+ */
+struct nfp_net_tx_buf {
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+	short int fidx;
+	u16 pkt_cnt;
+	u32 real_len;
+};
+
+/**
+ * struct nfp_net_tx_ring - TX ring structure
+ * @r_vec:      Back pointer to ring vector structure
+ * @idx:        Ring index from Linux's perspective
+ * @qcidx:      Queue Controller Peripheral (QCP) queue index for the TX queue
+ * @qcp_q:      Pointer to base of the QCP TX queue
+ * @cnt:        Size of the queue in number of descriptors
+ * @wr_p:       TX ring write pointer (free running)
+ * @rd_p:       TX ring read pointer (free running)
+ * @qcp_rd_p:   Local copy of QCP TX queue read pointer
+ * @wr_ptr_add:	Accumulated number of buffers to add to QCP write pointer
+ *		(used for .xmit_more delayed kick)
+ * @txbufs:     Array of transmitted TX buffers, to free on transmit
+ * @txds:       Virtual address of TX ring in host memory
+ * @dma:        DMA address of the TX ring
+ * @size:       Size, in bytes, of the TX ring (needed to free)
+ */
+struct nfp_net_tx_ring {
+	struct nfp_net_r_vector *r_vec;
+
+	u32 idx;
+	int qcidx;
+	u8 __iomem *qcp_q;
+
+	u32 cnt;
+	u32 wr_p;
+	u32 rd_p;
+	u32 qcp_rd_p;
+
+	u32 wr_ptr_add;
+
+	struct nfp_net_tx_buf *txbufs;
+	struct nfp_net_tx_desc *txds;
+
+	dma_addr_t dma;
+	unsigned int size;
+} ____cacheline_aligned;
+
+/* RX and freelist descriptor format */
+
+#define PCIE_DESC_RX_DD			BIT(7)
+#define PCIE_DESC_RX_META_LEN_MASK	GENMASK(6, 0)
+
+/* Flags in the RX descriptor */
+#define PCIE_DESC_RX_RSS		cpu_to_le16(BIT(15))
+#define PCIE_DESC_RX_I_IP4_CSUM		cpu_to_le16(BIT(14))
+#define PCIE_DESC_RX_I_IP4_CSUM_OK	cpu_to_le16(BIT(13))
+#define PCIE_DESC_RX_I_TCP_CSUM		cpu_to_le16(BIT(12))
+#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_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))
+#define PCIE_DESC_RX_TCP_CSUM		cpu_to_le16(BIT(4))
+#define PCIE_DESC_RX_TCP_CSUM_OK	cpu_to_le16(BIT(3))
+#define PCIE_DESC_RX_UDP_CSUM		cpu_to_le16(BIT(2))
+#define PCIE_DESC_RX_UDP_CSUM_OK	cpu_to_le16(BIT(1))
+#define PCIE_DESC_RX_VLAN		cpu_to_le16(BIT(0))
+
+#define PCIE_DESC_RX_CSUM_ALL		(PCIE_DESC_RX_IP4_CSUM |	\
+					 PCIE_DESC_RX_TCP_CSUM |	\
+					 PCIE_DESC_RX_UDP_CSUM |	\
+					 PCIE_DESC_RX_I_IP4_CSUM |	\
+					 PCIE_DESC_RX_I_TCP_CSUM |	\
+					 PCIE_DESC_RX_I_UDP_CSUM)
+#define PCIE_DESC_RX_CSUM_OK_SHIFT	1
+#define __PCIE_DESC_RX_CSUM_ALL		le16_to_cpu(PCIE_DESC_RX_CSUM_ALL)
+#define __PCIE_DESC_RX_CSUM_ALL_OK	(__PCIE_DESC_RX_CSUM_ALL >>	\
+					 PCIE_DESC_RX_CSUM_OK_SHIFT)
+
+struct nfp_net_rx_desc {
+	union {
+		struct {
+			u8 dma_addr_hi;	/* High bits of the buf address */
+			__le16 reserved; /* Must be zero */
+			u8 meta_len_dd; /* Must be zero */
+
+			__le32 dma_addr_lo; /* Low bits of the buffer address */
+		} __packed fld;
+
+		struct {
+			__le16 data_len; /* Length of the frame + meta data */
+			u8 reserved;
+			u8 meta_len_dd;	/* Length of meta data prepended +
+					 * descriptor done flag.
+					 */
+
+			__le16 flags;	/* RX flags. See @PCIE_DESC_RX_* */
+			__le16 vlan;	/* VLAN if stripped */
+		} __packed rxd;
+
+		__le32 vals[2];
+	};
+};
+
+struct nfp_net_rx_hash {
+	__be32 hash_type;
+	__be32 hash;
+};
+
+/**
+ * struct nfp_net_rx_buf - software RX buffer descriptor
+ * @skb:	sk_buff associated with this buffer
+ * @dma_addr:	DMA mapping address of the buffer
+ */
+struct nfp_net_rx_buf {
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+};
+
+/**
+ * struct nfp_net_rx_ring - RX ring structure
+ * @r_vec:      Back pointer to ring vector structure
+ * @cnt:        Size of the queue in number of descriptors
+ * @wr_p:       FL/RX ring write pointer (free running)
+ * @rd_p:       FL/RX ring read pointer (free running)
+ * @idx:        Ring index from Linux's perspective
+ * @fl_qcidx:   Queue Controller Peripheral (QCP) queue index for the freelist
+ * @rx_qcidx:   Queue Controller Peripheral (QCP) queue index for the RX queue
+ * @qcp_fl:     Pointer to base of the QCP freelist queue
+ * @qcp_rx:     Pointer to base of the QCP RX queue
+ * @wr_ptr_add: Accumulated number of buffers to add to QCP write pointer
+ *              (used for free list batching)
+ * @rxbufs:     Array of transmitted FL/RX buffers
+ * @rxds:       Virtual address of FL/RX ring in host memory
+ * @dma:        DMA address of the FL/RX ring
+ * @size:       Size, in bytes, of the FL/RX ring (needed to free)
+ */
+struct nfp_net_rx_ring {
+	struct nfp_net_r_vector *r_vec;
+
+	u32 cnt;
+	u32 wr_p;
+	u32 rd_p;
+
+	u16 idx;
+	u16 wr_ptr_add;
+
+	int fl_qcidx;
+	int rx_qcidx;
+	u8 __iomem *qcp_fl;
+	u8 __iomem *qcp_rx;
+
+	struct nfp_net_rx_buf *rxbufs;
+	struct nfp_net_rx_desc *rxds;
+
+	dma_addr_t dma;
+	unsigned int size;
+} ____cacheline_aligned;
+
+/**
+ * struct nfp_net_r_vector - Per ring interrupt vector configuration
+ * @nfp_net:        Backpointer to nfp_net structure
+ * @napi:           NAPI structure for this ring vec
+ * @tx_ring:        Pointer to TX ring
+ * @rx_ring:        Pointer to RX ring
+ * @irq_idx:        Index into MSI-X table
+ * @rx_sync:	    Seqlock for atomic updates of RX stats
+ * @rx_pkts:        Number of received packets
+ * @rx_bytes:	    Number of received bytes
+ * @rx_drops:	    Number of packets dropped on RX due to lack of resources
+ * @hw_csum_rx_ok:  Counter of packets where the HW checksum was OK
+ * @hw_csum_rx_inner_ok: Counter of packets where the inner HW checksum was OK
+ * @hw_csum_rx_error:	 Counter of packets with bad checksums
+ * @tx_sync:	    Seqlock for atomic updates of TX stats
+ * @tx_pkts:	    Number of Transmitted packets
+ * @tx_bytes:	    Number of Transmitted bytes
+ * @hw_csum_tx:	    Counter of packets with TX checksum offload requested
+ * @hw_csum_tx_inner:	 Counter of inner TX checksum offload requests
+ * @tx_gather:	    Counter of packets with Gather DMA
+ * @tx_lso:	    Counter of LSO packets sent
+ * @tx_errors:	    How many TX errors were encountered
+ * @tx_busy:        How often was TX busy (no space)?
+ * @handler:        Interrupt handler for this ring vector
+ * @name:           Name of the interrupt vector
+ * @affinity_mask:  SMP affinity mask for this vector
+ *
+ * This structure ties RX and TX rings to interrupt vectors and a NAPI
+ * context. This currently only supports one RX and TX ring per
+ * interrupt vector but might be extended in the future to allow
+ * association of multiple rings per vector.
+ */
+struct nfp_net_r_vector {
+	struct nfp_net *nfp_net;
+	struct napi_struct napi;
+
+	struct nfp_net_tx_ring *tx_ring;
+	struct nfp_net_rx_ring *rx_ring;
+
+	int irq_idx;
+
+	struct u64_stats_sync rx_sync;
+	u64 rx_pkts;
+	u64 rx_bytes;
+	u64 rx_drops;
+	u64 hw_csum_rx_ok;
+	u64 hw_csum_rx_inner_ok;
+	u64 hw_csum_rx_error;
+
+	struct u64_stats_sync tx_sync;
+	u64 tx_pkts;
+	u64 tx_bytes;
+	u64 hw_csum_tx;
+	u64 hw_csum_tx_inner;
+	u64 tx_gather;
+	u64 tx_lso;
+	u64 tx_errors;
+	u64 tx_busy;
+
+	irq_handler_t handler;
+	char name[IFNAMSIZ + 8];
+	cpumask_t affinity_mask;
+} ____cacheline_aligned;
+
+/* Firmware version as it is written in the 32bit value in the BAR */
+struct nfp_net_fw_version {
+	u8 minor;
+	u8 major;
+	u8 class;
+	u8 resv;
+} __packed;
+
+static inline bool nfp_net_fw_ver_eq(struct nfp_net_fw_version *fw_ver,
+				     u8 resv, u8 class, u8 major, u8 minor)
+{
+	return fw_ver->resv == resv &&
+	       fw_ver->class == class &&
+	       fw_ver->major == major &&
+	       fw_ver->minor == minor;
+}
+
+/**
+ * struct nfp_net - NFP network device structure
+ * @pdev:               Backpointer to PCI device
+ * @netdev:             Backpointer to net_device structure
+ * @nfp_fallback:       Is the driver used in fallback mode?
+ * @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?
+ * @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
+ * @cpp:                Pointer to the CPP handle
+ * @nfp_dev_cpp:        Pointer to the NFP Device handle
+ * @ctrl_area:          Pointer to the CPP area for the control BAR
+ * @tx_area:            Pointer to the CPP area for the TX queues
+ * @rx_area:            Pointer to the CPP area for the FL/RX queues
+ * @fw_ver:             Firmware version
+ * @cap:                Capabilities advertised by the Firmware
+ * @max_mtu:            Maximum support MTU advertised by the Firmware
+ * @rss_cfg:            RSS configuration
+ * @rss_key:            RSS secret key
+ * @rss_itbl:           RSS indirection table
+ * @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
+ * @num_rx_rings:       Currently configured number of RX rings
+ * @txd_cnt:            Size of the TX ring in number of descriptors
+ * @rxd_cnt:            Size of the RX ring in number of descriptors
+ * @tx_rings:           Array of pre-allocated TX ring structures
+ * @rx_rings:           Array of pre-allocated RX ring structures
+ * @num_irqs:	        Number of allocated interrupt vectors
+ * @num_r_vecs:         Number of used ring vectors
+ * @r_vecs:             Pre-allocated array of ring vectors
+ * @irq_entries:        Pre-allocated array of MSI-X entries
+ * @lsc_handler:        Handler for Link State Change interrupt
+ * @lsc_name:           Name for Link State Change interrupt
+ * @exn_handler:        Handler for Exception interrupt
+ * @exn_name:           Name for Exception interrupt
+ * @shared_handler:     Handler for shared interrupts
+ * @shared_name:        Name for shared interrupt
+ * @me_freq_mhz:        ME clock_freq (MHz)
+ * @reconfig_lock:	Protects HW reconfiguration request regs/machinery
+ * @link_up:            Is the link up?
+ * @link_status_lock:	Protects @link_up and ensures atomicity with BAR reading
+ * @rx_coalesce_usecs:      RX interrupt moderation usecs delay parameter
+ * @rx_coalesce_max_frames: RX interrupt moderation frame count parameter
+ * @tx_coalesce_usecs:      TX interrupt moderation usecs delay parameter
+ * @tx_coalesce_max_frames: TX interrupt moderation frame count parameter
+ * @vxlan_ports:	VXLAN ports for RX inner csum offload communicated to HW
+ * @vxlan_usecnt:	IPv4/IPv6 VXLAN port use counts
+ * @qcp_cfg:            Pointer to QCP queue used for configuration notification
+ * @ctrl_bar:           Pointer to mapped control BAR
+ * @tx_bar:             Pointer to mapped TX queues
+ * @rx_bar:             Pointer to mapped FL/RX queues
+ * @debugfs_dir:	Device directory in debugfs
+ */
+struct nfp_net {
+	struct pci_dev *pdev;
+	struct net_device *netdev;
+
+	unsigned nfp_fallback:1;
+	unsigned is_vf:1;
+	unsigned is_nfp3200:1;
+	unsigned fw_loaded:1;
+
+	u32 ctrl;
+	u32 fl_bufsz;
+
+	u32 rx_offset;
+
+#ifdef CONFIG_PCI_IOV
+	unsigned int num_vfs;
+	struct vf_data_storage *vfinfo;
+	int vf_rate_link_speed;
+#endif
+
+	struct nfp_cpp *cpp;
+	struct platform_device *nfp_dev_cpp;
+	struct nfp_cpp_area *ctrl_area;
+	struct nfp_cpp_area *tx_area;
+	struct nfp_cpp_area *rx_area;
+
+	struct nfp_net_fw_version fw_ver;
+	u32 cap;
+	u32 max_mtu;
+
+	u32 rss_cfg;
+	u8 rss_key[NFP_NET_CFG_RSS_KEY_SZ];
+	u8 rss_itbl[NFP_NET_CFG_RSS_ITBL_SZ];
+
+	int max_tx_rings;
+	int max_rx_rings;
+
+	int num_tx_rings;
+	int num_rx_rings;
+
+	int stride_tx;
+	int stride_rx;
+
+	int txd_cnt;
+	int rxd_cnt;
+
+	struct nfp_net_tx_ring tx_rings[NFP_NET_MAX_TX_RINGS];
+	struct nfp_net_rx_ring rx_rings[NFP_NET_MAX_RX_RINGS];
+
+	u8 num_irqs;
+	u8 num_r_vecs;
+	struct nfp_net_r_vector r_vecs[NFP_NET_MAX_TX_RINGS];
+	struct msix_entry irq_entries[NFP_NET_NON_Q_VECTORS +
+				      NFP_NET_MAX_TX_RINGS];
+
+	irq_handler_t lsc_handler;
+	char lsc_name[IFNAMSIZ + 8];
+
+	irq_handler_t exn_handler;
+	char exn_name[IFNAMSIZ + 8];
+
+	irq_handler_t shared_handler;
+	char shared_name[IFNAMSIZ + 8];
+
+	u32 me_freq_mhz;
+
+	bool link_up;
+	spinlock_t link_status_lock;
+
+	spinlock_t reconfig_lock;
+
+	u32 rx_coalesce_usecs;
+	u32 rx_coalesce_max_frames;
+	u32 tx_coalesce_usecs;
+	u32 tx_coalesce_max_frames;
+
+	__be16 vxlan_ports[NFP_NET_N_VXLAN_PORTS];
+	u8 vxlan_usecnt[NFP_NET_N_VXLAN_PORTS];
+
+	u8 __iomem *qcp_cfg;
+
+	u8 __iomem *ctrl_bar;
+	u8 __iomem *q_bar;
+	u8 __iomem *tx_bar;
+	u8 __iomem *rx_bar;
+
+	struct dentry *debugfs_dir;
+};
+
+/* Functions to read/write from/to a BAR
+ * Performs any endian conversion necessary.
+ */
+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 */
+
+static inline u32 nn_readl(struct nfp_net *nn, int off)
+{
+	return readl(nn->ctrl_bar + off);
+}
+
+static inline void nn_writel(struct nfp_net *nn, int off, u32 val)
+{
+	writel(val, nn->ctrl_bar + off);
+}
+
+static inline u64 nn_readq(struct nfp_net *nn, int off)
+{
+	return readq(nn->ctrl_bar + off);
+}
+
+static inline void nn_writeq(struct nfp_net *nn, int off, u64 val)
+{
+	writeq(val, nn->ctrl_bar + off);
+}
+
+/* Flush posted PCI writes by reading something without side effects */
+static inline void nn_pci_flush(struct nfp_net *nn)
+{
+	nn_readl(nn, NFP_NET_CFG_VERSION);
+}
+
+/* Queue Controller Peripheral access functions and definitions.
+ *
+ * Some of the BARs of the NFP are mapped to portions of the Queue
+ * Controller Peripheral (QCP) address space on the NFP.  A QCP queue
+ * has a read and a write pointer (as well as a size and flags,
+ * indicating overflow etc).  The QCP offers a number of different
+ * operation on queue pointers, but here we only offer function to
+ * either add to a pointer or to read the pointer value.
+ */
+#define NFP_QCP_QUEUE_ADDR_SZ			0x800
+#define NFP_QCP_QUEUE_OFF(_x)			((_x) * NFP_QCP_QUEUE_ADDR_SZ)
+#define NFP_QCP_QUEUE_ADD_RPTR			0x0000
+#define NFP_QCP_QUEUE_ADD_WPTR			0x0004
+#define NFP_QCP_QUEUE_STS_LO			0x0008
+#define NFP_QCP_QUEUE_STS_LO_READPTR_mask	0x3ffff
+#define NFP_QCP_QUEUE_STS_HI			0x000c
+#define NFP_QCP_QUEUE_STS_HI_WRITEPTR_mask	0x3ffff
+
+/* The offset of a QCP queues in the PCIe Target (same on NFP3200 and NFP6000 */
+#define NFP_PCIE_QUEUE(_q) (0x80000 + (NFP_QCP_QUEUE_ADDR_SZ * ((_q) & 0xff)))
+
+/* nfp_qcp_ptr - Read or Write Pointer of a queue */
+enum nfp_qcp_ptr {
+	NFP_QCP_READ_PTR = 0,
+	NFP_QCP_WRITE_PTR
+};
+
+/* There appear to be an *undocumented* upper limit on the value which
+ * one can add to a queue and that value is either 0x3f or 0x7f.  We
+ * go with 0x3f as a conservative measure.
+ */
+#define NFP_QCP_MAX_ADD				0x3f
+
+static inline void _nfp_qcp_ptr_add(u8 __iomem *q,
+				    enum nfp_qcp_ptr ptr, u32 val)
+{
+	u32 off;
+
+	if (ptr == NFP_QCP_READ_PTR)
+		off = NFP_QCP_QUEUE_ADD_RPTR;
+	else
+		off = NFP_QCP_QUEUE_ADD_WPTR;
+
+	while (val > NFP_QCP_MAX_ADD) {
+		writel(NFP_QCP_MAX_ADD, q + off);
+		val -= NFP_QCP_MAX_ADD;
+	}
+
+	writel(val, q + off);
+}
+
+/**
+ * nfp_qcp_rd_ptr_add() - Add the value to the read pointer of a queue
+ *
+ * @q:   Base address for queue structure
+ * @val: Value to add to the queue pointer
+ *
+ * If @val is greater than @NFP_QCP_MAX_ADD multiple writes are performed.
+ */
+static inline void nfp_qcp_rd_ptr_add(u8 __iomem *q, u32 val)
+{
+	_nfp_qcp_ptr_add(q, NFP_QCP_READ_PTR, val);
+}
+
+/**
+ * nfp_qcp_wr_ptr_add() - Add the value to the write pointer of a queue
+ *
+ * @q:   Base address for queue structure
+ * @val: Value to add to the queue pointer
+ *
+ * If @val is greater than @NFP_QCP_MAX_ADD multiple writes are performed.
+ */
+static inline void nfp_qcp_wr_ptr_add(u8 __iomem *q, u32 val)
+{
+	_nfp_qcp_ptr_add(q, NFP_QCP_WRITE_PTR, val);
+}
+
+static inline u32 _nfp_qcp_read(u8 __iomem *q, enum nfp_qcp_ptr ptr)
+{
+	u32 off;
+	u32 val;
+
+	if (ptr == NFP_QCP_READ_PTR)
+		off = NFP_QCP_QUEUE_STS_LO;
+	else
+		off = NFP_QCP_QUEUE_STS_HI;
+
+	val = readl(q + off);
+
+	if (ptr == NFP_QCP_READ_PTR)
+		return val & NFP_QCP_QUEUE_STS_LO_READPTR_mask;
+	else
+		return val & NFP_QCP_QUEUE_STS_HI_WRITEPTR_mask;
+}
+
+/**
+ * nfp_qcp_rd_ptr_read() - Read the current read pointer value for a queue
+ * @q:  Base address for queue structure
+ *
+ * Return: Value read.
+ */
+static inline u32 nfp_qcp_rd_ptr_read(u8 __iomem *q)
+{
+	return _nfp_qcp_read(q, NFP_QCP_READ_PTR);
+}
+
+/**
+ * nfp_qcp_wr_ptr_read() - Read the current write pointer value for a queue
+ * @q:  Base address for queue structure
+ *
+ * Return: Value read.
+ */
+static inline u32 nfp_qcp_wr_ptr_read(u8 __iomem *q)
+{
+	return _nfp_qcp_read(q, NFP_QCP_WRITE_PTR);
+}
+
+/* Globals */
+extern const char nfp_net_driver_name[];
+extern const char nfp_net_driver_version[];
+
+/* Prototypes */
+void nfp_net_get_fw_version(struct nfp_net_fw_version *fw_ver,
+			    void __iomem *ctrl_bar);
+
+struct nfp_net *nfp_net_netdev_alloc(struct pci_dev *pdev,
+				     int max_tx_rings, int max_rx_rings);
+void nfp_net_netdev_free(struct nfp_net *nn);
+int nfp_net_netdev_init(struct net_device *netdev);
+void nfp_net_netdev_clean(struct net_device *netdev);
+void nfp_net_set_ethtool_ops(struct net_device *netdev);
+void nfp_net_info(struct nfp_net *nn);
+int nfp_net_reconfig(struct nfp_net *nn, u32 update);
+void nfp_net_rss_write_itbl(struct nfp_net *nn);
+void nfp_net_rss_write_key(struct nfp_net *nn);
+void nfp_net_coalesce_write_cfg(struct nfp_net *nn);
+int nfp_net_irqs_alloc(struct nfp_net *nn);
+void nfp_net_irqs_disable(struct nfp_net *nn);
+
+#ifdef CONFIG_NFP_NET_DEBUG
+void nfp_net_debugfs_create(void);
+void nfp_net_debugfs_destroy(void);
+void nfp_net_debugfs_adapter_add(struct nfp_net *nn);
+void nfp_net_debugfs_adapter_del(struct nfp_net *nn);
+#else
+static inline void nfp_net_debugfs_create(void)
+{
+}
+
+static inline void nfp_net_debugfs_destroy(void)
+{
+}
+
+static inline void nfp_net_debugfs_adapter_add(struct nfp_net *nn)
+{
+}
+
+static inline void nfp_net_debugfs_adapter_del(struct nfp_net *nn)
+{
+}
+#endif /* CONFIG_NFP_NET_DEBUG */
+
+#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
new file mode 100644
index 000000000000..43c618bafdb6
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_common.c
@@ -0,0 +1,2435 @@
+/*
+ * Copyright (C) 2015 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_common.c
+ * Netronome network device driver: Common functions between PF and VF
+ * Authors: Jakub Kicinski <jakub.kicinski@netronome.com>
+ *          Jason McMullan <jason.mcmullan@netronome.com>
+ *          Rolf Neugebauer <rolf.neugebauer@netronome.com>
+ *          Brad Petrus <brad.petrus@netronome.com>
+ *          Chris Telfer <chris.telfer@netronome.com>
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/interrupt.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/pci.h>
+#include <linux/pci_regs.h>
+#include <linux/msi.h>
+#include <linux/ethtool.h>
+#include <linux/log2.h>
+#include <linux/if_vlan.h>
+#include <linux/random.h>
+
+#include <linux/ktime.h>
+
+#include <net/vxlan.h>
+
+#include "nfp_net_ctrl.h"
+#include "nfp_net.h"
+
+/**
+ * nfp_net_get_fw_version() - Read and parse the FW version
+ * @fw_ver:	Output fw_version structure to read to
+ * @ctrl_bar:	Mapped address of the control BAR
+ */
+void nfp_net_get_fw_version(struct nfp_net_fw_version *fw_ver,
+			    void __iomem *ctrl_bar)
+{
+	u32 reg;
+
+	reg = readl(ctrl_bar + NFP_NET_CFG_VERSION);
+	put_unaligned_le32(reg, fw_ver);
+}
+
+/**
+ * nfp_net_reconfig() - Reconfigure the firmware
+ * @nn:      NFP Net device to reconfigure
+ * @update:  The value for the update field in the BAR config
+ *
+ * Write the update word to the BAR and ping the reconfig queue.  The
+ * poll until the firmware has acknowledged the update by zeroing the
+ * update word.
+ *
+ * Return: Negative errno on error, 0 on success
+ */
+int nfp_net_reconfig(struct nfp_net *nn, u32 update)
+{
+	int cnt, ret = 0;
+	u32 new;
+
+	spin_lock_bh(&nn->reconfig_lock);
+
+	nn_writel(nn, NFP_NET_CFG_UPDATE, update);
+	/* ensure update is written before pinging HW */
+	nn_pci_flush(nn);
+	nfp_qcp_wr_ptr_add(nn->qcp_cfg, 1);
+
+	/* Poll update field, waiting for NFP to ack the config */
+	for (cnt = 0; ; cnt++) {
+		new = nn_readl(nn, NFP_NET_CFG_UPDATE);
+		if (new == 0)
+			break;
+		if (new & NFP_NET_CFG_UPDATE_ERR) {
+			nn_err(nn, "Reconfig error: 0x%08x\n", new);
+			ret = -EIO;
+			break;
+		} else if (cnt >= NFP_NET_POLL_TIMEOUT) {
+			nn_err(nn, "Reconfig timeout for 0x%08x after %dms\n",
+			       update, cnt);
+			ret = -EIO;
+			break;
+		}
+		mdelay(1);
+	}
+
+	spin_unlock_bh(&nn->reconfig_lock);
+	return ret;
+}
+
+/* Interrupt configuration and handling
+ */
+
+/**
+ * nfp_net_irq_unmask_msix() - Unmask MSI-X after automasking
+ * @nn:       NFP Network structure
+ * @entry_nr: MSI-X table entry
+ *
+ * Clear the MSI-X table mask bit for the given entry bypassing Linux irq
+ * handling subsystem.  Use *only* to reenable automasked vectors.
+ */
+static void nfp_net_irq_unmask_msix(struct nfp_net *nn, unsigned int entry_nr)
+{
+	struct list_head *msi_head = &nn->pdev->dev.msi_list;
+	struct msi_desc *entry;
+	u32 off;
+
+	/* All MSI-Xs have the same mask_base */
+	entry = list_first_entry(msi_head, struct msi_desc, list);
+
+	off = (PCI_MSIX_ENTRY_SIZE * entry_nr) +
+		PCI_MSIX_ENTRY_VECTOR_CTRL;
+	writel(0, entry->mask_base + off);
+	readl(entry->mask_base);
+}
+
+/**
+ * nfp_net_irq_unmask() - Unmask automasked interrupt
+ * @nn:       NFP Network structure
+ * @entry_nr: MSI-X table entry
+ *
+ * If MSI-X auto-masking is enabled clear the mask bit, otherwise
+ * clear the ICR for the entry.
+ */
+static void nfp_net_irq_unmask(struct nfp_net *nn, unsigned int entry_nr)
+{
+	if (nn->ctrl & NFP_NET_CFG_CTRL_MSIXAUTO) {
+		nfp_net_irq_unmask_msix(nn, entry_nr);
+		return;
+	}
+
+	nn_writeb(nn, NFP_NET_CFG_ICR(entry_nr), NFP_NET_CFG_ICR_UNMASKED);
+	nn_pci_flush(nn);
+}
+
+/**
+ * nfp_net_msix_alloc() - Try to allocate MSI-X irqs
+ * @nn:       NFP Network structure
+ * @nr_vecs:  Number of MSI-X vectors to allocate
+ *
+ * For MSI-X we want at least NFP_NET_NON_Q_VECTORS + 1 vectors.
+ *
+ * Return: Number of MSI-X vectors obtained or 0 on error.
+ */
+static int nfp_net_msix_alloc(struct nfp_net *nn, int nr_vecs)
+{
+	struct pci_dev *pdev = nn->pdev;
+	int nvecs;
+	int i;
+
+	for (i = 0; i < nr_vecs; i++)
+		nn->irq_entries[i].entry = i;
+
+	nvecs = pci_enable_msix_range(pdev, nn->irq_entries,
+				      NFP_NET_NON_Q_VECTORS + 1, nr_vecs);
+	if (nvecs < 0) {
+		nn_warn(nn, "Failed to enable MSI-X. Wanted %d-%d (err=%d)\n",
+			NFP_NET_NON_Q_VECTORS + 1, nr_vecs, nvecs);
+		return 0;
+	}
+
+	return nvecs;
+}
+
+/**
+ * nfp_net_irqs_wanted() - Work out how many interrupt vectors we want
+ * @nn:       NFP Network structure
+ *
+ * We want a vector per CPU (or ring), whatever is smaller plus
+ * NFP_NET_NON_Q_VECTORS for LSC etc.
+ *
+ * Return: Number of interrupts wanted
+ */
+static int nfp_net_irqs_wanted(struct nfp_net *nn)
+{
+	int ncpus;
+	int vecs;
+
+	ncpus = num_online_cpus();
+
+	vecs = max_t(int, nn->num_tx_rings, nn->num_rx_rings);
+	vecs = min_t(int, vecs, ncpus);
+
+	return vecs + NFP_NET_NON_Q_VECTORS;
+}
+
+/**
+ * nfp_net_irqs_alloc() - allocates MSI-X irqs
+ * @nn:       NFP Network structure
+ *
+ * Return: Number of irqs obtained or 0 on error.
+ */
+int nfp_net_irqs_alloc(struct nfp_net *nn)
+{
+	int wanted_irqs;
+
+	wanted_irqs = nfp_net_irqs_wanted(nn);
+
+	nn->num_irqs = nfp_net_msix_alloc(nn, wanted_irqs);
+	if (nn->num_irqs == 0) {
+		nn_err(nn, "Failed to allocate MSI-X IRQs\n");
+		return 0;
+	}
+
+	nn->num_r_vecs = nn->num_irqs - NFP_NET_NON_Q_VECTORS;
+
+	if (nn->num_irqs < wanted_irqs)
+		nn_warn(nn, "Unable to allocate %d vectors. Got %d instead\n",
+			wanted_irqs, nn->num_irqs);
+
+	return nn->num_irqs;
+}
+
+/**
+ * nfp_net_irqs_disable() - Disable interrupts
+ * @nn:       NFP Network structure
+ *
+ * Undoes what @nfp_net_irqs_alloc() does.
+ */
+void nfp_net_irqs_disable(struct nfp_net *nn)
+{
+	pci_disable_msix(nn->pdev);
+}
+
+/**
+ * nfp_net_irq_rxtx() - Interrupt service routine for RX/TX rings.
+ * @irq:      Interrupt
+ * @data:     Opaque data structure
+ *
+ * Return: Indicate if the interrupt has been handled.
+ */
+static irqreturn_t nfp_net_irq_rxtx(int irq, void *data)
+{
+	struct nfp_net_r_vector *r_vec = data;
+
+	napi_schedule_irqoff(&r_vec->napi);
+
+	/* The FW auto-masks any interrupt, either via the MASK bit in
+	 * the MSI-X table or via the per entry ICR field.  So there
+	 * is no need to disable interrupts here.
+	 */
+	return IRQ_HANDLED;
+}
+
+/**
+ * nfp_net_read_link_status() - Reread link status from control BAR
+ * @nn:       NFP Network structure
+ */
+static void nfp_net_read_link_status(struct nfp_net *nn)
+{
+	unsigned long flags;
+	bool link_up;
+	u32 sts;
+
+	spin_lock_irqsave(&nn->link_status_lock, flags);
+
+	sts = nn_readl(nn, NFP_NET_CFG_STS);
+	link_up = !!(sts & NFP_NET_CFG_STS_LINK);
+
+	if (nn->link_up == link_up)
+		goto out;
+
+	nn->link_up = link_up;
+
+	if (nn->link_up) {
+		netif_carrier_on(nn->netdev);
+		netdev_info(nn->netdev, "NIC Link is Up\n");
+	} else {
+		netif_carrier_off(nn->netdev);
+		netdev_info(nn->netdev, "NIC Link is Down\n");
+	}
+out:
+	spin_unlock_irqrestore(&nn->link_status_lock, flags);
+}
+
+/**
+ * nfp_net_irq_lsc() - Interrupt service routine for link state changes
+ * @irq:      Interrupt
+ * @data:     Opaque data structure
+ *
+ * Return: Indicate if the interrupt has been handled.
+ */
+static irqreturn_t nfp_net_irq_lsc(int irq, void *data)
+{
+	struct nfp_net *nn = data;
+
+	nfp_net_read_link_status(nn);
+
+	nfp_net_irq_unmask(nn, NFP_NET_IRQ_LSC_IDX);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * nfp_net_irq_exn() - Interrupt service routine for exceptions
+ * @irq:      Interrupt
+ * @data:     Opaque data structure
+ *
+ * Return: Indicate if the interrupt has been handled.
+ */
+static irqreturn_t nfp_net_irq_exn(int irq, void *data)
+{
+	struct nfp_net *nn = data;
+
+	nn_err(nn, "%s: UNIMPLEMENTED.\n", __func__);
+	/* XXX TO BE IMPLEMENTED */
+	return IRQ_HANDLED;
+}
+
+/**
+ * nfp_net_tx_ring_init() - Fill in the boilerplate for a TX ring
+ * @tx_ring:  TX ring structure
+ */
+static void nfp_net_tx_ring_init(struct nfp_net_tx_ring *tx_ring)
+{
+	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+
+	tx_ring->qcidx = tx_ring->idx * nn->stride_tx;
+	tx_ring->qcp_q = nn->tx_bar + NFP_QCP_QUEUE_OFF(tx_ring->qcidx);
+}
+
+/**
+ * nfp_net_rx_ring_init() - Fill in the boilerplate for a RX ring
+ * @rx_ring:  RX ring structure
+ */
+static void nfp_net_rx_ring_init(struct nfp_net_rx_ring *rx_ring)
+{
+	struct nfp_net_r_vector *r_vec = rx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+
+	rx_ring->fl_qcidx = rx_ring->idx * nn->stride_rx;
+	rx_ring->rx_qcidx = rx_ring->fl_qcidx + (nn->stride_rx - 1);
+
+	rx_ring->qcp_fl = nn->rx_bar + NFP_QCP_QUEUE_OFF(rx_ring->fl_qcidx);
+	rx_ring->qcp_rx = nn->rx_bar + NFP_QCP_QUEUE_OFF(rx_ring->rx_qcidx);
+}
+
+/**
+ * nfp_net_irqs_assign() - Assign IRQs and setup rvecs.
+ * @netdev:   netdev structure
+ */
+static void nfp_net_irqs_assign(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	struct nfp_net_r_vector *r_vec;
+	int r;
+
+	/* Assumes nn->num_tx_rings == nn->num_rx_rings */
+	if (nn->num_tx_rings > nn->num_r_vecs) {
+		nn_warn(nn, "More rings (%d) than vectors (%d).\n",
+			nn->num_tx_rings, nn->num_r_vecs);
+		nn->num_tx_rings = nn->num_r_vecs;
+		nn->num_rx_rings = nn->num_r_vecs;
+	}
+
+	nn->lsc_handler = nfp_net_irq_lsc;
+	nn->exn_handler = nfp_net_irq_exn;
+
+	for (r = 0; r < nn->num_r_vecs; r++) {
+		r_vec = &nn->r_vecs[r];
+		r_vec->nfp_net = nn;
+		r_vec->handler = nfp_net_irq_rxtx;
+		r_vec->irq_idx = NFP_NET_NON_Q_VECTORS + r;
+
+		cpumask_set_cpu(r, &r_vec->affinity_mask);
+
+		r_vec->tx_ring = &nn->tx_rings[r];
+		nn->tx_rings[r].idx = r;
+		nn->tx_rings[r].r_vec = r_vec;
+		nfp_net_tx_ring_init(r_vec->tx_ring);
+
+		r_vec->rx_ring = &nn->rx_rings[r];
+		nn->rx_rings[r].idx = r;
+		nn->rx_rings[r].r_vec = r_vec;
+		nfp_net_rx_ring_init(r_vec->rx_ring);
+	}
+}
+
+/**
+ * nfp_net_aux_irq_request() - Request an auxiliary interrupt (LSC or EXN)
+ * @nn:		NFP Network structure
+ * @ctrl_offset: Control BAR offset where IRQ configuration should be written
+ * @format:	printf-style format to construct the interrupt name
+ * @name:	Pointer to allocated space for interrupt name
+ * @name_sz:	Size of space for interrupt name
+ * @vector_idx:	Index of MSI-X vector used for this interrupt
+ * @handler:	IRQ handler to register for this interrupt
+ */
+static int
+nfp_net_aux_irq_request(struct nfp_net *nn, u32 ctrl_offset,
+			const char *format, char *name, size_t name_sz,
+			unsigned int vector_idx, irq_handler_t handler)
+{
+	struct msix_entry *entry;
+	int err;
+
+	entry = &nn->irq_entries[vector_idx];
+
+	snprintf(name, name_sz, format, netdev_name(nn->netdev));
+	err = request_irq(entry->vector, handler, 0, name, nn);
+	if (err) {
+		nn_err(nn, "Failed to request IRQ %d (err=%d).\n",
+		       entry->vector, err);
+		return err;
+	}
+	nn_writeb(nn, ctrl_offset, vector_idx);
+
+	return 0;
+}
+
+/**
+ * nfp_net_aux_irq_free() - Free an auxiliary interrupt (LSC or EXN)
+ * @nn:		NFP Network structure
+ * @ctrl_offset: Control BAR offset where IRQ configuration should be written
+ * @vector_idx:	Index of MSI-X vector used for this interrupt
+ */
+static void nfp_net_aux_irq_free(struct nfp_net *nn, u32 ctrl_offset,
+				 unsigned int vector_idx)
+{
+	nn_writeb(nn, ctrl_offset, 0xff);
+	free_irq(nn->irq_entries[vector_idx].vector, nn);
+}
+
+/* Transmit
+ *
+ * One queue controller peripheral queue is used for transmit.  The
+ * driver en-queues packets for transmit by advancing the write
+ * pointer.  The device indicates that packets have transmitted by
+ * advancing the read pointer.  The driver maintains a local copy of
+ * the read and write pointer in @struct nfp_net_tx_ring.  The driver
+ * keeps @wr_p in sync with the queue controller write pointer and can
+ * determine how many packets have been transmitted by comparing its
+ * copy of the read pointer @rd_p with the read pointer maintained by
+ * the queue controller peripheral.
+ */
+
+/**
+ * nfp_net_tx_full() - Check if the TX ring is full
+ * @tx_ring: TX ring to check
+ * @dcnt:    Number of descriptors that need to be enqueued (must be >= 1)
+ *
+ * This function checks, based on the *host copy* of read/write
+ * pointer if a given TX ring is full.  The real TX queue may have
+ * some newly made available slots.
+ *
+ * Return: True if the ring is full.
+ */
+static inline int nfp_net_tx_full(struct nfp_net_tx_ring *tx_ring, int dcnt)
+{
+	return (tx_ring->wr_p - tx_ring->rd_p) >= (tx_ring->cnt - dcnt);
+}
+
+/* Wrappers for deciding when to stop and restart TX queues */
+static int nfp_net_tx_ring_should_wake(struct nfp_net_tx_ring *tx_ring)
+{
+	return !nfp_net_tx_full(tx_ring, MAX_SKB_FRAGS * 4);
+}
+
+static int nfp_net_tx_ring_should_stop(struct nfp_net_tx_ring *tx_ring)
+{
+	return nfp_net_tx_full(tx_ring, MAX_SKB_FRAGS + 1);
+}
+
+/**
+ * nfp_net_tx_ring_stop() - stop tx ring
+ * @nd_q:    netdev queue
+ * @tx_ring: driver tx queue structure
+ *
+ * Safely stop TX ring.  Remember that while we are running .start_xmit()
+ * someone else may be cleaning the TX ring completions so we need to be
+ * extra careful here.
+ */
+static void nfp_net_tx_ring_stop(struct netdev_queue *nd_q,
+				 struct nfp_net_tx_ring *tx_ring)
+{
+	netif_tx_stop_queue(nd_q);
+
+	/* We can race with the TX completion out of NAPI so recheck */
+	smp_mb();
+	if (unlikely(nfp_net_tx_ring_should_wake(tx_ring)))
+		netif_tx_start_queue(nd_q);
+}
+
+/**
+ * nfp_net_tx_tso() - Set up Tx descriptor for LSO
+ * @nn:  NFP Net device
+ * @r_vec: per-ring structure
+ * @txbuf: Pointer to driver soft TX descriptor
+ * @txd: Pointer to HW TX descriptor
+ * @skb: Pointer to SKB
+ *
+ * Set up Tx descriptor for LSO, do nothing for non-LSO skbs.
+ * Return error on packet header greater than maximum supported LSO header size.
+ */
+static void nfp_net_tx_tso(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
+			   struct nfp_net_tx_buf *txbuf,
+			   struct nfp_net_tx_desc *txd, struct sk_buff *skb)
+{
+	u32 hdrlen;
+	u16 mss;
+
+	if (!skb_is_gso(skb))
+		return;
+
+	if (!skb->encapsulation)
+		hdrlen = skb_transport_offset(skb) + tcp_hdrlen(skb);
+	else
+		hdrlen = skb_inner_transport_header(skb) - skb->data +
+			inner_tcp_hdrlen(skb);
+
+	txbuf->pkt_cnt = skb_shinfo(skb)->gso_segs;
+	txbuf->real_len += hdrlen * (txbuf->pkt_cnt - 1);
+
+	mss = skb_shinfo(skb)->gso_size & PCIE_DESC_TX_MSS_MASK;
+	txd->l4_offset = hdrlen;
+	txd->mss = cpu_to_le16(mss);
+	txd->flags |= PCIE_DESC_TX_LSO;
+
+	u64_stats_update_begin(&r_vec->tx_sync);
+	r_vec->tx_lso++;
+	u64_stats_update_end(&r_vec->tx_sync);
+}
+
+/**
+ * nfp_net_tx_csum() - Set TX CSUM offload flags in TX descriptor
+ * @nn:  NFP Net device
+ * @r_vec: per-ring structure
+ * @txbuf: Pointer to driver soft TX descriptor
+ * @txd: Pointer to TX descriptor
+ * @skb: Pointer to SKB
+ *
+ * This function sets the TX checksum flags in the TX descriptor based
+ * on the configuration and the protocol of the packet to be transmitted.
+ */
+static void nfp_net_tx_csum(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
+			    struct nfp_net_tx_buf *txbuf,
+			    struct nfp_net_tx_desc *txd, struct sk_buff *skb)
+{
+	struct ipv6hdr *ipv6h;
+	struct iphdr *iph;
+	u8 l4_hdr;
+
+	if (!(nn->ctrl & NFP_NET_CFG_CTRL_TXCSUM))
+		return;
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		return;
+
+	txd->flags |= PCIE_DESC_TX_CSUM;
+	if (skb->encapsulation)
+		txd->flags |= PCIE_DESC_TX_ENCAP;
+
+	iph = skb->encapsulation ? inner_ip_hdr(skb) : ip_hdr(skb);
+	ipv6h = skb->encapsulation ? inner_ipv6_hdr(skb) : ipv6_hdr(skb);
+
+	if (iph->version == 4) {
+		txd->flags |= PCIE_DESC_TX_IP4_CSUM;
+		l4_hdr = iph->protocol;
+	} else if (ipv6h->version == 6) {
+		l4_hdr = ipv6h->nexthdr;
+	} else {
+		nn_warn_ratelimit(nn, "partial checksum but ipv=%x!\n",
+				  iph->version);
+		return;
+	}
+
+	switch (l4_hdr) {
+	case IPPROTO_TCP:
+		txd->flags |= PCIE_DESC_TX_TCP_CSUM;
+		break;
+	case IPPROTO_UDP:
+		txd->flags |= PCIE_DESC_TX_UDP_CSUM;
+		break;
+	default:
+		nn_warn_ratelimit(nn, "partial checksum but l4 proto=%x!\n",
+				  l4_hdr);
+		return;
+	}
+
+	u64_stats_update_begin(&r_vec->tx_sync);
+	if (skb->encapsulation)
+		r_vec->hw_csum_tx_inner += txbuf->pkt_cnt;
+	else
+		r_vec->hw_csum_tx += txbuf->pkt_cnt;
+	u64_stats_update_end(&r_vec->tx_sync);
+}
+
+/**
+ * nfp_net_tx() - Main transmit entry point
+ * @skb:    SKB to transmit
+ * @netdev: netdev structure
+ *
+ * Return: NETDEV_TX_OK on success.
+ */
+static int nfp_net_tx(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	const struct skb_frag_struct *frag;
+	struct nfp_net_r_vector *r_vec;
+	struct nfp_net_tx_desc *txd, txdg;
+	struct nfp_net_tx_buf *txbuf;
+	struct nfp_net_tx_ring *tx_ring;
+	struct netdev_queue *nd_q;
+	dma_addr_t dma_addr;
+	unsigned int fsize;
+	int f, nr_frags;
+	int wr_idx;
+	u16 qidx;
+
+	qidx = skb_get_queue_mapping(skb);
+	tx_ring = &nn->tx_rings[qidx];
+	r_vec = tx_ring->r_vec;
+	nd_q = netdev_get_tx_queue(nn->netdev, qidx);
+
+	nr_frags = skb_shinfo(skb)->nr_frags;
+
+	if (unlikely(nfp_net_tx_full(tx_ring, nr_frags + 1))) {
+		nn_warn_ratelimit(nn, "TX ring %d busy. wrp=%u rdp=%u\n",
+				  qidx, tx_ring->wr_p, tx_ring->rd_p);
+		netif_tx_stop_queue(nd_q);
+		u64_stats_update_begin(&r_vec->tx_sync);
+		r_vec->tx_busy++;
+		u64_stats_update_end(&r_vec->tx_sync);
+		return NETDEV_TX_BUSY;
+	}
+
+	/* Start with the head skbuf */
+	dma_addr = dma_map_single(&nn->pdev->dev, skb->data, skb_headlen(skb),
+				  DMA_TO_DEVICE);
+	if (dma_mapping_error(&nn->pdev->dev, dma_addr))
+		goto err_free;
+
+	wr_idx = tx_ring->wr_p % tx_ring->cnt;
+
+	/* Stash the soft descriptor of the head then initialize it */
+	txbuf = &tx_ring->txbufs[wr_idx];
+	txbuf->skb = skb;
+	txbuf->dma_addr = dma_addr;
+	txbuf->fidx = -1;
+	txbuf->pkt_cnt = 1;
+	txbuf->real_len = skb->len;
+
+	/* Build TX descriptor */
+	txd = &tx_ring->txds[wr_idx];
+	txd->offset_eop = (nr_frags == 0) ? PCIE_DESC_TX_EOP : 0;
+	txd->dma_len = cpu_to_le16(skb_headlen(skb));
+	nfp_desc_set_dma_addr(txd, dma_addr);
+	txd->data_len = cpu_to_le16(skb->len);
+
+	txd->flags = 0;
+	txd->mss = 0;
+	txd->l4_offset = 0;
+
+	nfp_net_tx_tso(nn, r_vec, txbuf, txd, skb);
+
+	nfp_net_tx_csum(nn, r_vec, txbuf, txd, skb);
+
+	if (skb_vlan_tag_present(skb) && nn->ctrl & NFP_NET_CFG_CTRL_TXVLAN) {
+		txd->flags |= PCIE_DESC_TX_VLAN;
+		txd->vlan = cpu_to_le16(skb_vlan_tag_get(skb));
+	}
+
+	/* Gather DMA */
+	if (nr_frags > 0) {
+		/* all descs must match except for in addr, length and eop */
+		txdg = *txd;
+
+		for (f = 0; f < nr_frags; f++) {
+			frag = &skb_shinfo(skb)->frags[f];
+			fsize = skb_frag_size(frag);
+
+			dma_addr = skb_frag_dma_map(&nn->pdev->dev, frag, 0,
+						    fsize, DMA_TO_DEVICE);
+			if (dma_mapping_error(&nn->pdev->dev, dma_addr))
+				goto err_unmap;
+
+			wr_idx = (wr_idx + 1) % tx_ring->cnt;
+			tx_ring->txbufs[wr_idx].skb = skb;
+			tx_ring->txbufs[wr_idx].dma_addr = dma_addr;
+			tx_ring->txbufs[wr_idx].fidx = f;
+
+			txd = &tx_ring->txds[wr_idx];
+			*txd = txdg;
+			txd->dma_len = cpu_to_le16(fsize);
+			nfp_desc_set_dma_addr(txd, dma_addr);
+			txd->offset_eop =
+				(f == nr_frags - 1) ? PCIE_DESC_TX_EOP : 0;
+		}
+
+		u64_stats_update_begin(&r_vec->tx_sync);
+		r_vec->tx_gather++;
+		u64_stats_update_end(&r_vec->tx_sync);
+	}
+
+	netdev_tx_sent_queue(nd_q, txbuf->real_len);
+
+	tx_ring->wr_p += nr_frags + 1;
+	if (nfp_net_tx_ring_should_stop(tx_ring))
+		nfp_net_tx_ring_stop(nd_q, tx_ring);
+
+	tx_ring->wr_ptr_add += nr_frags + 1;
+	if (!skb->xmit_more || netif_xmit_stopped(nd_q)) {
+		/* force memory write before we let HW know */
+		wmb();
+		nfp_qcp_wr_ptr_add(tx_ring->qcp_q, tx_ring->wr_ptr_add);
+		tx_ring->wr_ptr_add = 0;
+	}
+
+	skb_tx_timestamp(skb);
+
+	return NETDEV_TX_OK;
+
+err_unmap:
+	--f;
+	while (f >= 0) {
+		frag = &skb_shinfo(skb)->frags[f];
+		dma_unmap_page(&nn->pdev->dev,
+			       tx_ring->txbufs[wr_idx].dma_addr,
+			       skb_frag_size(frag), DMA_TO_DEVICE);
+		tx_ring->txbufs[wr_idx].skb = NULL;
+		tx_ring->txbufs[wr_idx].dma_addr = 0;
+		tx_ring->txbufs[wr_idx].fidx = -2;
+		wr_idx = wr_idx - 1;
+		if (wr_idx < 0)
+			wr_idx += tx_ring->cnt;
+	}
+	dma_unmap_single(&nn->pdev->dev, tx_ring->txbufs[wr_idx].dma_addr,
+			 skb_headlen(skb), DMA_TO_DEVICE);
+	tx_ring->txbufs[wr_idx].skb = NULL;
+	tx_ring->txbufs[wr_idx].dma_addr = 0;
+	tx_ring->txbufs[wr_idx].fidx = -2;
+err_free:
+	nn_warn_ratelimit(nn, "Failed to map DMA TX buffer\n");
+	u64_stats_update_begin(&r_vec->tx_sync);
+	r_vec->tx_errors++;
+	u64_stats_update_end(&r_vec->tx_sync);
+	dev_kfree_skb_any(skb);
+	return NETDEV_TX_OK;
+}
+
+/**
+ * nfp_net_tx_complete() - Handled completed TX packets
+ * @tx_ring:   TX ring structure
+ *
+ * Return: Number of completed TX descriptors
+ */
+static void nfp_net_tx_complete(struct nfp_net_tx_ring *tx_ring)
+{
+	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	const struct skb_frag_struct *frag;
+	struct netdev_queue *nd_q;
+	u32 done_pkts = 0, done_bytes = 0;
+	struct sk_buff *skb;
+	int todo, nr_frags;
+	u32 qcp_rd_p;
+	int fidx;
+	int idx;
+
+	/* Work out how many descriptors have been transmitted */
+	qcp_rd_p = nfp_qcp_rd_ptr_read(tx_ring->qcp_q);
+
+	if (qcp_rd_p == tx_ring->qcp_rd_p)
+		return;
+
+	if (qcp_rd_p > tx_ring->qcp_rd_p)
+		todo = qcp_rd_p - tx_ring->qcp_rd_p;
+	else
+		todo = qcp_rd_p + tx_ring->cnt - tx_ring->qcp_rd_p;
+
+	while (todo--) {
+		idx = tx_ring->rd_p % tx_ring->cnt;
+		tx_ring->rd_p++;
+
+		skb = tx_ring->txbufs[idx].skb;
+		if (!skb)
+			continue;
+
+		nr_frags = skb_shinfo(skb)->nr_frags;
+		fidx = tx_ring->txbufs[idx].fidx;
+
+		if (fidx == -1) {
+			/* unmap head */
+			dma_unmap_single(&nn->pdev->dev,
+					 tx_ring->txbufs[idx].dma_addr,
+					 skb_headlen(skb), DMA_TO_DEVICE);
+
+			done_pkts += tx_ring->txbufs[idx].pkt_cnt;
+			done_bytes += tx_ring->txbufs[idx].real_len;
+		} else {
+			/* unmap fragment */
+			frag = &skb_shinfo(skb)->frags[fidx];
+			dma_unmap_page(&nn->pdev->dev,
+				       tx_ring->txbufs[idx].dma_addr,
+				       skb_frag_size(frag), DMA_TO_DEVICE);
+		}
+
+		/* check for last gather fragment */
+		if (fidx == nr_frags - 1)
+			dev_kfree_skb_any(skb);
+
+		tx_ring->txbufs[idx].dma_addr = 0;
+		tx_ring->txbufs[idx].skb = NULL;
+		tx_ring->txbufs[idx].fidx = -2;
+	}
+
+	tx_ring->qcp_rd_p = qcp_rd_p;
+
+	u64_stats_update_begin(&r_vec->tx_sync);
+	r_vec->tx_bytes += done_bytes;
+	r_vec->tx_pkts += done_pkts;
+	u64_stats_update_end(&r_vec->tx_sync);
+
+	nd_q = netdev_get_tx_queue(nn->netdev, tx_ring->idx);
+	netdev_tx_completed_queue(nd_q, done_pkts, done_bytes);
+	if (nfp_net_tx_ring_should_wake(tx_ring)) {
+		/* Make sure TX thread will see updated tx_ring->rd_p */
+		smp_mb();
+
+		if (unlikely(netif_tx_queue_stopped(nd_q)))
+			netif_tx_wake_queue(nd_q);
+	}
+
+	WARN_ONCE(tx_ring->wr_p - tx_ring->rd_p > tx_ring->cnt,
+		  "TX ring corruption rd_p=%u wr_p=%u cnt=%u\n",
+		  tx_ring->rd_p, tx_ring->wr_p, tx_ring->cnt);
+}
+
+/**
+ * nfp_net_tx_flush() - Free any untransmitted buffers currently on the TX ring
+ * @tx_ring:     TX ring structure
+ *
+ * Assumes that the device is stopped
+ */
+static void nfp_net_tx_flush(struct nfp_net_tx_ring *tx_ring)
+{
+	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+	const struct skb_frag_struct *frag;
+	struct netdev_queue *nd_q;
+	struct sk_buff *skb;
+	int nr_frags;
+	int fidx;
+	int idx;
+
+	while (tx_ring->rd_p != tx_ring->wr_p) {
+		idx = tx_ring->rd_p % tx_ring->cnt;
+
+		skb = tx_ring->txbufs[idx].skb;
+		if (skb) {
+			nr_frags = skb_shinfo(skb)->nr_frags;
+			fidx = tx_ring->txbufs[idx].fidx;
+
+			if (fidx == -1) {
+				/* unmap head */
+				dma_unmap_single(&pdev->dev,
+						 tx_ring->txbufs[idx].dma_addr,
+						 skb_headlen(skb),
+						 DMA_TO_DEVICE);
+			} else {
+				/* unmap fragment */
+				frag = &skb_shinfo(skb)->frags[fidx];
+				dma_unmap_page(&pdev->dev,
+					       tx_ring->txbufs[idx].dma_addr,
+					       skb_frag_size(frag),
+					       DMA_TO_DEVICE);
+			}
+
+			/* check for last gather fragment */
+			if (fidx == nr_frags - 1)
+				dev_kfree_skb_any(skb);
+
+			tx_ring->txbufs[idx].dma_addr = 0;
+			tx_ring->txbufs[idx].skb = NULL;
+			tx_ring->txbufs[idx].fidx = -2;
+		}
+
+		memset(&tx_ring->txds[idx], 0, sizeof(tx_ring->txds[idx]));
+
+		tx_ring->qcp_rd_p++;
+		tx_ring->rd_p++;
+	}
+
+	nd_q = netdev_get_tx_queue(nn->netdev, tx_ring->idx);
+	netdev_tx_reset_queue(nd_q);
+}
+
+static void nfp_net_tx_timeout(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int i;
+
+	for (i = 0; i < nn->num_tx_rings; i++) {
+		if (!netif_tx_queue_stopped(netdev_get_tx_queue(netdev, i)))
+			continue;
+		nn_warn(nn, "TX timeout on ring: %d\n", i);
+	}
+	nn_warn(nn, "TX watchdog timeout\n");
+}
+
+/* Receive processing
+ */
+
+/**
+ * nfp_net_rx_space() - return the number of free slots on the RX ring
+ * @rx_ring:   RX ring structure
+ *
+ * Make sure we leave at least one slot free.
+ *
+ * Return: True if there is space on the RX ring
+ */
+static inline int nfp_net_rx_space(struct nfp_net_rx_ring *rx_ring)
+{
+	return (rx_ring->cnt - 1) - (rx_ring->wr_p - rx_ring->rd_p);
+}
+
+/**
+ * nfp_net_rx_alloc_one() - Allocate and map skb for RX
+ * @rx_ring:	RX ring structure of the skb
+ * @dma_addr:	Pointer to storage for DMA address (output param)
+ *
+ * This function will allcate a new skb, map it for DMA.
+ *
+ * Return: allocated skb or NULL on failure.
+ */
+static struct sk_buff *
+nfp_net_rx_alloc_one(struct nfp_net_rx_ring *rx_ring, dma_addr_t *dma_addr)
+{
+	struct nfp_net *nn = rx_ring->r_vec->nfp_net;
+	struct sk_buff *skb;
+
+	skb = netdev_alloc_skb(nn->netdev, nn->fl_bufsz);
+	if (!skb) {
+		nn_warn_ratelimit(nn, "Failed to alloc receive SKB\n");
+		return NULL;
+	}
+
+	*dma_addr = dma_map_single(&nn->pdev->dev, skb->data,
+				  nn->fl_bufsz, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&nn->pdev->dev, *dma_addr)) {
+		dev_kfree_skb_any(skb);
+		nn_warn_ratelimit(nn, "Failed to map DMA RX buffer\n");
+		return NULL;
+	}
+
+	return skb;
+}
+
+/**
+ * nfp_net_rx_give_one() - Put mapped skb on the software and hardware rings
+ * @rx_ring:	RX ring structure
+ * @skb:	Skb to put on rings
+ * @dma_addr:	DMA address of skb mapping
+ */
+static void nfp_net_rx_give_one(struct nfp_net_rx_ring *rx_ring,
+				struct sk_buff *skb, dma_addr_t dma_addr)
+{
+	unsigned int wr_idx;
+
+	wr_idx = rx_ring->wr_p % rx_ring->cnt;
+
+	/* Stash SKB and DMA address away */
+	rx_ring->rxbufs[wr_idx].skb = skb;
+	rx_ring->rxbufs[wr_idx].dma_addr = dma_addr;
+
+	/* Fill freelist descriptor */
+	rx_ring->rxds[wr_idx].fld.reserved = 0;
+	rx_ring->rxds[wr_idx].fld.meta_len_dd = 0;
+	nfp_desc_set_dma_addr(&rx_ring->rxds[wr_idx].fld, dma_addr);
+
+	rx_ring->wr_p++;
+	rx_ring->wr_ptr_add++;
+	if (rx_ring->wr_ptr_add >= NFP_NET_FL_BATCH) {
+		/* Update write pointer of the freelist queue. Make
+		 * sure all writes are flushed before telling the hardware.
+		 */
+		wmb();
+		nfp_qcp_wr_ptr_add(rx_ring->qcp_fl, rx_ring->wr_ptr_add);
+		rx_ring->wr_ptr_add = 0;
+	}
+}
+
+/**
+ * nfp_net_rx_flush() - Free any buffers currently on the RX ring
+ * @rx_ring:  RX ring to remove buffers from
+ *
+ * Assumes that the device is stopped
+ */
+static void nfp_net_rx_flush(struct nfp_net_rx_ring *rx_ring)
+{
+	struct nfp_net *nn = rx_ring->r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+	int idx;
+
+	while (rx_ring->rd_p != rx_ring->wr_p) {
+		idx = rx_ring->rd_p % rx_ring->cnt;
+
+		if (rx_ring->rxbufs[idx].skb) {
+			dma_unmap_single(&pdev->dev,
+					 rx_ring->rxbufs[idx].dma_addr,
+					 nn->fl_bufsz, DMA_FROM_DEVICE);
+			dev_kfree_skb_any(rx_ring->rxbufs[idx].skb);
+			rx_ring->rxbufs[idx].dma_addr = 0;
+			rx_ring->rxbufs[idx].skb = NULL;
+		}
+
+		memset(&rx_ring->rxds[idx], 0, sizeof(rx_ring->rxds[idx]));
+
+		rx_ring->rd_p++;
+	}
+}
+
+/**
+ * nfp_net_rx_fill_freelist() - Attempt filling freelist with RX buffers
+ * @rx_ring: RX ring to fill
+ *
+ * Try to fill as many buffers as possible into freelist.  Return
+ * number of buffers added.
+ *
+ * Return: Number of freelist buffers added.
+ */
+static int nfp_net_rx_fill_freelist(struct nfp_net_rx_ring *rx_ring)
+{
+	struct sk_buff *skb;
+	dma_addr_t dma_addr;
+
+	while (nfp_net_rx_space(rx_ring)) {
+		skb = nfp_net_rx_alloc_one(rx_ring, &dma_addr);
+		if (!skb) {
+			nfp_net_rx_flush(rx_ring);
+			return -ENOMEM;
+		}
+		nfp_net_rx_give_one(rx_ring, skb, dma_addr);
+	}
+
+	return 0;
+}
+
+/**
+ * nfp_net_rx_csum_has_errors() - group check if rxd has any csum errors
+ * @flags: RX descriptor flags field in CPU byte order
+ */
+static int nfp_net_rx_csum_has_errors(u16 flags)
+{
+	u16 csum_all_checked, csum_all_ok;
+
+	csum_all_checked = flags & __PCIE_DESC_RX_CSUM_ALL;
+	csum_all_ok = flags & __PCIE_DESC_RX_CSUM_ALL_OK;
+
+	return csum_all_checked != (csum_all_ok << PCIE_DESC_RX_CSUM_OK_SHIFT);
+}
+
+/**
+ * nfp_net_rx_csum() - set SKB checksum field based on RX descriptor flags
+ * @nn:  NFP Net device
+ * @r_vec: per-ring structure
+ * @rxd: Pointer to RX descriptor
+ * @skb: Pointer to SKB
+ */
+static void nfp_net_rx_csum(struct nfp_net *nn, struct nfp_net_r_vector *r_vec,
+			    struct nfp_net_rx_desc *rxd, struct sk_buff *skb)
+{
+	skb_checksum_none_assert(skb);
+
+	if (!(nn->netdev->features & NETIF_F_RXCSUM))
+		return;
+
+	if (nfp_net_rx_csum_has_errors(le16_to_cpu(rxd->rxd.flags))) {
+		u64_stats_update_begin(&r_vec->rx_sync);
+		r_vec->hw_csum_rx_error++;
+		u64_stats_update_end(&r_vec->rx_sync);
+		return;
+	}
+
+	/* Assume that the firmware will never report inner CSUM_OK unless outer
+	 * L4 headers were successfully parsed. FW will always report zero UDP
+	 * checksum as CSUM_OK.
+	 */
+	if (rxd->rxd.flags & PCIE_DESC_RX_TCP_CSUM_OK ||
+	    rxd->rxd.flags & PCIE_DESC_RX_UDP_CSUM_OK) {
+		__skb_incr_checksum_unnecessary(skb);
+		u64_stats_update_begin(&r_vec->rx_sync);
+		r_vec->hw_csum_rx_ok++;
+		u64_stats_update_end(&r_vec->rx_sync);
+	}
+
+	if (rxd->rxd.flags & PCIE_DESC_RX_I_TCP_CSUM_OK ||
+	    rxd->rxd.flags & PCIE_DESC_RX_I_UDP_CSUM_OK) {
+		__skb_incr_checksum_unnecessary(skb);
+		u64_stats_update_begin(&r_vec->rx_sync);
+		r_vec->hw_csum_rx_inner_ok++;
+		u64_stats_update_end(&r_vec->rx_sync);
+	}
+}
+
+/**
+ * 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)
+{
+	struct nfp_net_rx_hash *rx_hash;
+
+	if (!(rxd->rxd.flags & PCIE_DESC_RX_RSS) ||
+	    !(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)) {
+	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);
+		break;
+	default:
+		skb_set_hash(skb, be32_to_cpu(rx_hash->hash), PKT_HASH_TYPE_L4);
+		break;
+	}
+}
+
+/**
+ * nfp_net_rx() - receive up to @budget packets on @rx_ring
+ * @rx_ring:   RX ring to receive from
+ * @budget:    NAPI budget
+ *
+ * Note, this function is separated out from the napi poll function to
+ * more cleanly separate packet receive code from other bookkeeping
+ * functions performed in the napi poll function.
+ *
+ * There are differences between the NFP-3200 firmware and the
+ * NFP-6000 firmware.  The NFP-3200 firmware uses a dedicated RX queue
+ * to indicate that new packets have arrived.  The NFP-6000 does not
+ * have this queue and uses the DD bit in the RX descriptor. This
+ * method cannot be used on the NFP-3200 as it causes a race
+ * condition: The RX ring write pointer on the NFP-3200 is updated
+ * after packets (and descriptors) have been DMAed.  If the DD bit is
+ * used and subsequently the read pointer is updated this may lead to
+ * the RX queue to underflow (if the firmware has not yet update the
+ * write pointer).  Therefore we use slightly ugly conditional code
+ * below to handle the differences.  We may, in the future update the
+ * NFP-3200 firmware to behave the same as the firmware on the
+ * NFP-6000.
+ *
+ * Return: Number of packets received.
+ */
+static int nfp_net_rx(struct nfp_net_rx_ring *rx_ring, int budget)
+{
+	struct nfp_net_r_vector *r_vec = rx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	unsigned int data_len, meta_len;
+	int avail = 0, pkts_polled = 0;
+	struct sk_buff *skb, *new_skb;
+	struct nfp_net_rx_desc *rxd;
+	dma_addr_t new_dma_addr;
+	u32 qcp_wr_p;
+	int idx;
+
+	if (nn->is_nfp3200) {
+		/* Work out how many packets arrived */
+		qcp_wr_p = nfp_qcp_wr_ptr_read(rx_ring->qcp_rx);
+		idx = rx_ring->rd_p % rx_ring->cnt;
+
+		if (qcp_wr_p == idx)
+			/* No new packets */
+			return 0;
+
+		if (qcp_wr_p > idx)
+			avail = qcp_wr_p - idx;
+		else
+			avail = qcp_wr_p + rx_ring->cnt - idx;
+	} else {
+		avail = budget + 1;
+	}
+
+	while (avail > 0 && pkts_polled < budget) {
+		idx = rx_ring->rd_p % rx_ring->cnt;
+
+		rxd = &rx_ring->rxds[idx];
+		if (!(rxd->rxd.meta_len_dd & PCIE_DESC_RX_DD)) {
+			if (nn->is_nfp3200)
+				nn_dbg(nn, "RX descriptor not valid (DD)%d:%u rxd[0]=%#x rxd[1]=%#x\n",
+				       rx_ring->idx, idx,
+				       rxd->vals[0], rxd->vals[1]);
+			break;
+		}
+		/* Memory barrier to ensure that we won't do other reads
+		 * before the DD bit.
+		 */
+		dma_rmb();
+
+		rx_ring->rd_p++;
+		pkts_polled++;
+		avail--;
+
+		skb = rx_ring->rxbufs[idx].skb;
+
+		new_skb = nfp_net_rx_alloc_one(rx_ring, &new_dma_addr);
+		if (!new_skb) {
+			nfp_net_rx_give_one(rx_ring, rx_ring->rxbufs[idx].skb,
+					    rx_ring->rxbufs[idx].dma_addr);
+			u64_stats_update_begin(&r_vec->rx_sync);
+			r_vec->rx_drops++;
+			u64_stats_update_end(&r_vec->rx_sync);
+			continue;
+		}
+
+		dma_unmap_single(&nn->pdev->dev,
+				 rx_ring->rxbufs[idx].dma_addr,
+				 nn->fl_bufsz, DMA_FROM_DEVICE);
+
+		nfp_net_rx_give_one(rx_ring, new_skb, new_dma_addr);
+
+		meta_len = rxd->rxd.meta_len_dd & PCIE_DESC_RX_META_LEN_MASK;
+		data_len = le16_to_cpu(rxd->rxd.data_len);
+
+		if (WARN_ON_ONCE(data_len > nn->fl_bufsz)) {
+			dev_kfree_skb_any(skb);
+			continue;
+		}
+
+		if (nn->rx_offset == NFP_NET_CFG_RX_OFFSET_DYNAMIC) {
+			/* The packet data starts after the metadata */
+			skb_reserve(skb, meta_len);
+		} else {
+			/* The packet data starts at a fixed offset */
+			skb_reserve(skb, nn->rx_offset);
+		}
+
+		/* Adjust the SKB for the dynamic meta data pre-pended */
+		skb_put(skb, data_len - meta_len);
+
+		nfp_net_set_hash(nn->netdev, skb, rxd);
+
+		/* Pad small frames to minimum */
+		if (skb_put_padto(skb, 60))
+			break;
+
+		/* 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);
+
+		skb_record_rx_queue(skb, rx_ring->idx);
+		skb->protocol = eth_type_trans(skb, nn->netdev);
+
+		nfp_net_rx_csum(nn, r_vec, rxd, skb);
+
+		if (rxd->rxd.flags & PCIE_DESC_RX_VLAN)
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+					       le16_to_cpu(rxd->rxd.vlan));
+
+		napi_gro_receive(&rx_ring->r_vec->napi, skb);
+	}
+
+	if (nn->is_nfp3200)
+		nfp_qcp_rd_ptr_add(rx_ring->qcp_rx, pkts_polled);
+
+	return pkts_polled;
+}
+
+/**
+ * nfp_net_poll() - napi poll function
+ * @napi:    NAPI structure
+ * @budget:  NAPI budget
+ *
+ * Return: number of packets polled.
+ */
+static int nfp_net_poll(struct napi_struct *napi, int budget)
+{
+	struct nfp_net_r_vector *r_vec =
+		container_of(napi, struct nfp_net_r_vector, napi);
+	struct nfp_net_rx_ring *rx_ring = r_vec->rx_ring;
+	struct nfp_net_tx_ring *tx_ring = r_vec->tx_ring;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct netdev_queue *txq;
+	unsigned int pkts_polled;
+
+	tx_ring = &nn->tx_rings[rx_ring->idx];
+	txq = netdev_get_tx_queue(nn->netdev, tx_ring->idx);
+	nfp_net_tx_complete(tx_ring);
+
+	pkts_polled = nfp_net_rx(rx_ring, budget);
+
+	if (pkts_polled < budget) {
+		napi_complete_done(napi, pkts_polled);
+		nfp_net_irq_unmask(nn, r_vec->irq_idx);
+	}
+
+	return pkts_polled;
+}
+
+/* Setup and Configuration
+ */
+
+/**
+ * nfp_net_tx_ring_free() - Free resources allocated to a TX ring
+ * @tx_ring:   TX ring to free
+ */
+static void nfp_net_tx_ring_free(struct nfp_net_tx_ring *tx_ring)
+{
+	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+
+	nn_writeq(nn, NFP_NET_CFG_TXR_ADDR(tx_ring->idx), 0);
+	nn_writeb(nn, NFP_NET_CFG_TXR_SZ(tx_ring->idx), 0);
+	nn_writeb(nn, NFP_NET_CFG_TXR_VEC(tx_ring->idx), 0);
+
+	kfree(tx_ring->txbufs);
+
+	if (tx_ring->txds)
+		dma_free_coherent(&pdev->dev, tx_ring->size,
+				  tx_ring->txds, tx_ring->dma);
+
+	tx_ring->cnt = 0;
+	tx_ring->wr_p = 0;
+	tx_ring->rd_p = 0;
+	tx_ring->qcp_rd_p = 0;
+	tx_ring->wr_ptr_add = 0;
+
+	tx_ring->txbufs = NULL;
+	tx_ring->txds = NULL;
+	tx_ring->dma = 0;
+	tx_ring->size = 0;
+}
+
+/**
+ * nfp_net_tx_ring_alloc() - Allocate resource for a TX ring
+ * @tx_ring:   TX Ring structure to allocate
+ *
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int nfp_net_tx_ring_alloc(struct nfp_net_tx_ring *tx_ring)
+{
+	struct nfp_net_r_vector *r_vec = tx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+	int sz;
+
+	tx_ring->cnt = nn->txd_cnt;
+
+	tx_ring->size = sizeof(*tx_ring->txds) * tx_ring->cnt;
+	tx_ring->txds = dma_zalloc_coherent(&pdev->dev, tx_ring->size,
+					    &tx_ring->dma, GFP_KERNEL);
+	if (!tx_ring->txds)
+		goto err_alloc;
+
+	sz = sizeof(*tx_ring->txbufs) * tx_ring->cnt;
+	tx_ring->txbufs = kzalloc(sz, GFP_KERNEL);
+	if (!tx_ring->txbufs)
+		goto err_alloc;
+
+	/* Write the DMA address, size and MSI-X info to the device */
+	nn_writeq(nn, NFP_NET_CFG_TXR_ADDR(tx_ring->idx), tx_ring->dma);
+	nn_writeb(nn, NFP_NET_CFG_TXR_SZ(tx_ring->idx), ilog2(tx_ring->cnt));
+	nn_writeb(nn, NFP_NET_CFG_TXR_VEC(tx_ring->idx), r_vec->irq_idx);
+
+	netif_set_xps_queue(nn->netdev, &r_vec->affinity_mask, tx_ring->idx);
+
+	nn_dbg(nn, "TxQ%02d: QCidx=%02d cnt=%d dma=%#llx host=%p\n",
+	       tx_ring->idx, tx_ring->qcidx,
+	       tx_ring->cnt, (unsigned long long)tx_ring->dma, tx_ring->txds);
+
+	return 0;
+
+err_alloc:
+	nfp_net_tx_ring_free(tx_ring);
+	return -ENOMEM;
+}
+
+/**
+ * nfp_net_rx_ring_free() - Free resources allocated to a RX ring
+ * @rx_ring:  RX ring to free
+ */
+static void nfp_net_rx_ring_free(struct nfp_net_rx_ring *rx_ring)
+{
+	struct nfp_net_r_vector *r_vec = rx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+
+	nn_writeq(nn, NFP_NET_CFG_RXR_ADDR(rx_ring->idx), 0);
+	nn_writeb(nn, NFP_NET_CFG_RXR_SZ(rx_ring->idx), 0);
+	nn_writeb(nn, NFP_NET_CFG_RXR_VEC(rx_ring->idx), 0);
+
+	kfree(rx_ring->rxbufs);
+
+	if (rx_ring->rxds)
+		dma_free_coherent(&pdev->dev, rx_ring->size,
+				  rx_ring->rxds, rx_ring->dma);
+
+	rx_ring->cnt = 0;
+	rx_ring->wr_p = 0;
+	rx_ring->rd_p = 0;
+	rx_ring->wr_ptr_add = 0;
+
+	rx_ring->rxbufs = NULL;
+	rx_ring->rxds = NULL;
+	rx_ring->dma = 0;
+	rx_ring->size = 0;
+}
+
+/**
+ * nfp_net_rx_ring_alloc() - Allocate resource for a RX ring
+ * @rx_ring:  RX ring to allocate
+ *
+ * Return: 0 on success, negative errno otherwise.
+ */
+static int nfp_net_rx_ring_alloc(struct nfp_net_rx_ring *rx_ring)
+{
+	struct nfp_net_r_vector *r_vec = rx_ring->r_vec;
+	struct nfp_net *nn = r_vec->nfp_net;
+	struct pci_dev *pdev = nn->pdev;
+	int sz;
+
+	rx_ring->cnt = nn->rxd_cnt;
+
+	rx_ring->size = sizeof(*rx_ring->rxds) * rx_ring->cnt;
+	rx_ring->rxds = dma_zalloc_coherent(&pdev->dev, rx_ring->size,
+					    &rx_ring->dma, GFP_KERNEL);
+	if (!rx_ring->rxds)
+		goto err_alloc;
+
+	sz = sizeof(*rx_ring->rxbufs) * rx_ring->cnt;
+	rx_ring->rxbufs = kzalloc(sz, GFP_KERNEL);
+	if (!rx_ring->rxbufs)
+		goto err_alloc;
+
+	/* Write the DMA address, size and MSI-X info to the device */
+	nn_writeq(nn, NFP_NET_CFG_RXR_ADDR(rx_ring->idx), rx_ring->dma);
+	nn_writeb(nn, NFP_NET_CFG_RXR_SZ(rx_ring->idx), ilog2(rx_ring->cnt));
+	nn_writeb(nn, NFP_NET_CFG_RXR_VEC(rx_ring->idx), r_vec->irq_idx);
+
+	nn_dbg(nn, "RxQ%02d: FlQCidx=%02d RxQCidx=%02d cnt=%d dma=%#llx host=%p\n",
+	       rx_ring->idx, rx_ring->fl_qcidx, rx_ring->rx_qcidx,
+	       rx_ring->cnt, (unsigned long long)rx_ring->dma, rx_ring->rxds);
+
+	return 0;
+
+err_alloc:
+	nfp_net_rx_ring_free(rx_ring);
+	return -ENOMEM;
+}
+
+static void __nfp_net_free_rings(struct nfp_net *nn, unsigned int n_free)
+{
+	struct nfp_net_r_vector *r_vec;
+	struct msix_entry *entry;
+
+	while (n_free--) {
+		r_vec = &nn->r_vecs[n_free];
+		entry = &nn->irq_entries[r_vec->irq_idx];
+
+		nfp_net_rx_ring_free(r_vec->rx_ring);
+		nfp_net_tx_ring_free(r_vec->tx_ring);
+
+		irq_set_affinity_hint(entry->vector, NULL);
+		free_irq(entry->vector, r_vec);
+
+		netif_napi_del(&r_vec->napi);
+	}
+}
+
+/**
+ * nfp_net_free_rings() - Free all ring resources
+ * @nn:      NFP Net device to reconfigure
+ */
+static void nfp_net_free_rings(struct nfp_net *nn)
+{
+	__nfp_net_free_rings(nn, nn->num_r_vecs);
+}
+
+/**
+ * nfp_net_alloc_rings() - Allocate resources for RX and TX rings
+ * @nn:      NFP Net device to reconfigure
+ *
+ * Return: 0 on success or negative errno on error.
+ */
+static int nfp_net_alloc_rings(struct nfp_net *nn)
+{
+	struct nfp_net_r_vector *r_vec;
+	struct msix_entry *entry;
+	int err;
+	int r;
+
+	for (r = 0; r < nn->num_r_vecs; r++) {
+		r_vec = &nn->r_vecs[r];
+		entry = &nn->irq_entries[r_vec->irq_idx];
+
+		/* Setup NAPI */
+		netif_napi_add(nn->netdev, &r_vec->napi,
+			       nfp_net_poll, NAPI_POLL_WEIGHT);
+
+		snprintf(r_vec->name, sizeof(r_vec->name),
+			 "%s-rxtx-%d", nn->netdev->name, r);
+		err = request_irq(entry->vector, r_vec->handler, 0,
+				  r_vec->name, r_vec);
+		if (err) {
+			nn_dbg(nn, "Error requesting IRQ %d\n", entry->vector);
+			goto err_napi_del;
+		}
+
+		irq_set_affinity_hint(entry->vector, &r_vec->affinity_mask);
+
+		nn_dbg(nn, "RV%02d: irq=%03d/%03d\n",
+		       r, entry->vector, entry->entry);
+
+		/* Allocate TX ring resources */
+		err = nfp_net_tx_ring_alloc(r_vec->tx_ring);
+		if (err)
+			goto err_free_irq;
+
+		/* Allocate RX ring resources */
+		err = nfp_net_rx_ring_alloc(r_vec->rx_ring);
+		if (err)
+			goto err_free_tx;
+	}
+
+	return 0;
+
+err_free_tx:
+	nfp_net_tx_ring_free(r_vec->tx_ring);
+err_free_irq:
+	irq_set_affinity_hint(entry->vector, NULL);
+	free_irq(entry->vector, r_vec);
+err_napi_del:
+	netif_napi_del(&r_vec->napi);
+	__nfp_net_free_rings(nn, r);
+	return err;
+}
+
+/**
+ * nfp_net_rss_write_itbl() - Write RSS indirection table to device
+ * @nn:      NFP Net device to reconfigure
+ */
+void nfp_net_rss_write_itbl(struct nfp_net *nn)
+{
+	int i;
+
+	for (i = 0; i < NFP_NET_CFG_RSS_ITBL_SZ; i += 4)
+		nn_writel(nn, NFP_NET_CFG_RSS_ITBL + i,
+			  get_unaligned_le32(nn->rss_itbl + i));
+}
+
+/**
+ * nfp_net_rss_write_key() - Write RSS hash key to device
+ * @nn:      NFP Net device to reconfigure
+ */
+void nfp_net_rss_write_key(struct nfp_net *nn)
+{
+	int i;
+
+	for (i = 0; i < NFP_NET_CFG_RSS_KEY_SZ; i += 4)
+		nn_writel(nn, NFP_NET_CFG_RSS_KEY + i,
+			  get_unaligned_le32(nn->rss_key + i));
+}
+
+/**
+ * nfp_net_coalesce_write_cfg() - Write irq coalescence configuration to HW
+ * @nn:      NFP Net device to reconfigure
+ */
+void nfp_net_coalesce_write_cfg(struct nfp_net *nn)
+{
+	u8 i;
+	u32 factor;
+	u32 value;
+
+	/* Compute factor used to convert coalesce '_usecs' parameters to
+	 * ME timestamp ticks.  There are 16 ME clock cycles for each timestamp
+	 * count.
+	 */
+	factor = nn->me_freq_mhz / 16;
+
+	/* copy RX interrupt coalesce parameters */
+	value = (nn->rx_coalesce_max_frames << 16) |
+		(factor * nn->rx_coalesce_usecs);
+	for (i = 0; i < nn->num_r_vecs; i++)
+		nn_writel(nn, NFP_NET_CFG_RXR_IRQ_MOD(i), value);
+
+	/* copy TX interrupt coalesce parameters */
+	value = (nn->tx_coalesce_max_frames << 16) |
+		(factor * nn->tx_coalesce_usecs);
+	for (i = 0; i < nn->num_r_vecs; i++)
+		nn_writel(nn, NFP_NET_CFG_TXR_IRQ_MOD(i), value);
+}
+
+/**
+ * nfp_net_write_mac_addr() - Write mac address to device registers
+ * @nn:      NFP Net device to reconfigure
+ * @mac:     Six-byte MAC address to be written
+ *
+ * We do a bit of byte swapping dance because firmware is LE.
+ */
+static void nfp_net_write_mac_addr(struct nfp_net *nn, const u8 *mac)
+{
+	nn_writel(nn, NFP_NET_CFG_MACADDR + 0,
+		  get_unaligned_be32(nn->netdev->dev_addr));
+	/* We can't do writew for NFP-3200 compatibility */
+	nn_writel(nn, NFP_NET_CFG_MACADDR + 4,
+		  get_unaligned_be16(nn->netdev->dev_addr + 4) << 16);
+}
+
+/**
+ * nfp_net_clear_config_and_disable() - Clear control BAR and disable NFP
+ * @nn:      NFP Net device to reconfigure
+ */
+static void nfp_net_clear_config_and_disable(struct nfp_net *nn)
+{
+	u32 new_ctrl, update;
+	int err;
+
+	new_ctrl = nn->ctrl;
+	new_ctrl &= ~NFP_NET_CFG_CTRL_ENABLE;
+	update = NFP_NET_CFG_UPDATE_GEN;
+	update |= NFP_NET_CFG_UPDATE_MSIX;
+	update |= NFP_NET_CFG_UPDATE_RING;
+
+	if (nn->cap & NFP_NET_CFG_CTRL_RINGCFG)
+		new_ctrl &= ~NFP_NET_CFG_CTRL_RINGCFG;
+
+	nn_writeq(nn, NFP_NET_CFG_TXRS_ENABLE, 0);
+	nn_writeq(nn, NFP_NET_CFG_RXRS_ENABLE, 0);
+
+	nn_writel(nn, NFP_NET_CFG_CTRL, new_ctrl);
+	err = nfp_net_reconfig(nn, update);
+	if (err) {
+		nn_err(nn, "Could not disable device: %d\n", err);
+		return;
+	}
+
+	nn->ctrl = new_ctrl;
+}
+
+/**
+ * nfp_net_start_vec() - Start ring vector
+ * @nn:      NFP Net device structure
+ * @r_vec:   Ring vector to be started
+ */
+static int nfp_net_start_vec(struct nfp_net *nn, struct nfp_net_r_vector *r_vec)
+{
+	unsigned int irq_vec;
+	int err = 0;
+
+	irq_vec = nn->irq_entries[r_vec->irq_idx].vector;
+
+	disable_irq(irq_vec);
+
+	err = nfp_net_rx_fill_freelist(r_vec->rx_ring);
+	if (err) {
+		nn_err(nn, "RV%02d: couldn't allocate enough buffers\n",
+		       r_vec->irq_idx);
+		goto out;
+	}
+
+	napi_enable(&r_vec->napi);
+out:
+	enable_irq(irq_vec);
+
+	return err;
+}
+
+static int nfp_net_netdev_open(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int err, r;
+	u32 update = 0;
+	u32 new_ctrl;
+
+	if (nn->ctrl & NFP_NET_CFG_CTRL_ENABLE) {
+		nn_err(nn, "Dev is already enabled: 0x%08x\n", nn->ctrl);
+		return -EBUSY;
+	}
+
+	new_ctrl = nn->ctrl;
+
+	/* Step 1: Allocate resources for rings and the like
+	 * - Request interrupts
+	 * - Allocate RX and TX ring resources
+	 * - Setup initial RSS table
+	 */
+	err = nfp_net_aux_irq_request(nn, NFP_NET_CFG_EXN, "%s-exn",
+				      nn->exn_name, sizeof(nn->exn_name),
+				      NFP_NET_IRQ_EXN_IDX, nn->exn_handler);
+	if (err)
+		return err;
+
+	err = nfp_net_alloc_rings(nn);
+	if (err)
+		goto err_free_exn;
+
+	err = netif_set_real_num_tx_queues(netdev, nn->num_tx_rings);
+	if (err)
+		goto err_free_rings;
+
+	err = netif_set_real_num_rx_queues(netdev, nn->num_rx_rings);
+	if (err)
+		goto err_free_rings;
+
+	if (nn->cap & NFP_NET_CFG_CTRL_RSS) {
+		nfp_net_rss_write_key(nn);
+		nfp_net_rss_write_itbl(nn);
+		nn_writel(nn, NFP_NET_CFG_RSS_CTRL, nn->rss_cfg);
+		update |= NFP_NET_CFG_UPDATE_RSS;
+	}
+
+	if (nn->cap & NFP_NET_CFG_CTRL_IRQMOD) {
+		nfp_net_coalesce_write_cfg(nn);
+
+		new_ctrl |= NFP_NET_CFG_CTRL_IRQMOD;
+		update |= NFP_NET_CFG_UPDATE_IRQMOD;
+	}
+
+	/* Step 2: Configure the NFP
+	 * - Enable rings from 0 to tx_rings/rx_rings - 1.
+	 * - Write MAC address (in case it changed)
+	 * - Set the MTU
+	 * - Set the Freelist buffer size
+	 * - Enable the FW
+	 */
+	nn_writeq(nn, NFP_NET_CFG_TXRS_ENABLE, nn->num_tx_rings == 64 ?
+		  0xffffffffffffffffULL : ((u64)1 << nn->num_tx_rings) - 1);
+
+	nn_writeq(nn, NFP_NET_CFG_RXRS_ENABLE, nn->num_rx_rings == 64 ?
+		  0xffffffffffffffffULL : ((u64)1 << nn->num_rx_rings) - 1);
+
+	nfp_net_write_mac_addr(nn, netdev->dev_addr);
+
+	nn_writel(nn, NFP_NET_CFG_MTU, netdev->mtu);
+	nn_writel(nn, NFP_NET_CFG_FLBUFSZ, nn->fl_bufsz);
+
+	/* Enable device */
+	new_ctrl |= NFP_NET_CFG_CTRL_ENABLE;
+	update |= NFP_NET_CFG_UPDATE_GEN;
+	update |= NFP_NET_CFG_UPDATE_MSIX;
+	update |= NFP_NET_CFG_UPDATE_RING;
+	if (nn->cap & NFP_NET_CFG_CTRL_RINGCFG)
+		new_ctrl |= NFP_NET_CFG_CTRL_RINGCFG;
+
+	nn_writel(nn, NFP_NET_CFG_CTRL, new_ctrl);
+	err = nfp_net_reconfig(nn, update);
+	if (err)
+		goto err_clear_config;
+
+	nn->ctrl = new_ctrl;
+
+	/* Since reconfiguration requests while NFP is down are ignored we
+	 * have to wipe the entire VXLAN configuration and reinitialize it.
+	 */
+	if (nn->ctrl & NFP_NET_CFG_CTRL_VXLAN) {
+		memset(&nn->vxlan_ports, 0, sizeof(nn->vxlan_ports));
+		memset(&nn->vxlan_usecnt, 0, sizeof(nn->vxlan_usecnt));
+		vxlan_get_rx_port(netdev);
+	}
+
+	/* Step 3: Enable for kernel
+	 * - put some freelist descriptors on each RX ring
+	 * - enable NAPI on each ring
+	 * - enable all TX queues
+	 * - set link state
+	 */
+	for (r = 0; r < nn->num_r_vecs; r++) {
+		err = nfp_net_start_vec(nn, &nn->r_vecs[r]);
+		if (err)
+			goto err_disable_napi;
+	}
+
+	netif_tx_wake_all_queues(netdev);
+
+	err = nfp_net_aux_irq_request(nn, NFP_NET_CFG_LSC, "%s-lsc",
+				      nn->lsc_name, sizeof(nn->lsc_name),
+				      NFP_NET_IRQ_LSC_IDX, nn->lsc_handler);
+	if (err)
+		goto err_stop_tx;
+	nfp_net_read_link_status(nn);
+
+	return 0;
+
+err_stop_tx:
+	netif_tx_disable(netdev);
+	for (r = 0; r < nn->num_r_vecs; r++)
+		nfp_net_tx_flush(nn->r_vecs[r].tx_ring);
+err_disable_napi:
+	while (r--) {
+		napi_disable(&nn->r_vecs[r].napi);
+		nfp_net_rx_flush(nn->r_vecs[r].rx_ring);
+	}
+err_clear_config:
+	nfp_net_clear_config_and_disable(nn);
+err_free_rings:
+	nfp_net_free_rings(nn);
+err_free_exn:
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
+	return err;
+}
+
+/**
+ * nfp_net_netdev_close() - Called when the device is downed
+ * @netdev:      netdev structure
+ */
+static int nfp_net_netdev_close(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int r;
+
+	if (!(nn->ctrl & NFP_NET_CFG_CTRL_ENABLE)) {
+		nn_err(nn, "Dev is not up: 0x%08x\n", nn->ctrl);
+		return 0;
+	}
+
+	/* Step 1: Disable RX and TX rings from the Linux kernel perspective
+	 */
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_LSC, NFP_NET_IRQ_LSC_IDX);
+	netif_carrier_off(netdev);
+	nn->link_up = false;
+
+	for (r = 0; r < nn->num_r_vecs; r++)
+		napi_disable(&nn->r_vecs[r].napi);
+
+	netif_tx_disable(netdev);
+
+	/* Step 2: Tell NFP
+	 */
+	nfp_net_clear_config_and_disable(nn);
+
+	/* Step 3: Free resources
+	 */
+	for (r = 0; r < nn->num_r_vecs; r++) {
+		nfp_net_rx_flush(nn->r_vecs[r].rx_ring);
+		nfp_net_tx_flush(nn->r_vecs[r].tx_ring);
+	}
+
+	nfp_net_free_rings(nn);
+	nfp_net_aux_irq_free(nn, NFP_NET_CFG_EXN, NFP_NET_IRQ_EXN_IDX);
+
+	nn_dbg(nn, "%s down", netdev->name);
+	return 0;
+}
+
+static void nfp_net_set_rx_mode(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	u32 new_ctrl;
+
+	new_ctrl = nn->ctrl;
+
+	if (netdev->flags & IFF_PROMISC) {
+		if (nn->cap & NFP_NET_CFG_CTRL_PROMISC)
+			new_ctrl |= NFP_NET_CFG_CTRL_PROMISC;
+		else
+			nn_warn(nn, "FW does not support promiscuous mode\n");
+	} else {
+		new_ctrl &= ~NFP_NET_CFG_CTRL_PROMISC;
+	}
+
+	if (new_ctrl == nn->ctrl)
+		return;
+
+	nn_writel(nn, NFP_NET_CFG_CTRL, new_ctrl);
+	if (nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_GEN))
+		return;
+
+	nn->ctrl = new_ctrl;
+}
+
+static int nfp_net_change_mtu(struct net_device *netdev, int new_mtu)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	u32 tmp;
+
+	nn_dbg(nn, "New MTU = %d\n", new_mtu);
+
+	if (new_mtu < 68 || new_mtu > nn->max_mtu) {
+		nn_err(nn, "New MTU (%d) is not valid\n", new_mtu);
+		return -EINVAL;
+	}
+
+	netdev->mtu = new_mtu;
+
+	/* Freelist buffer size rounded up to the nearest 1K */
+	tmp = new_mtu + ETH_HLEN + VLAN_HLEN + NFP_NET_MAX_PREPEND;
+	nn->fl_bufsz = roundup(tmp, 1024);
+
+	/* restart if running */
+	if (netif_running(netdev)) {
+		nfp_net_netdev_close(netdev);
+		nfp_net_netdev_open(netdev);
+	}
+
+	return 0;
+}
+
+static struct rtnl_link_stats64 *nfp_net_stat64(struct net_device *netdev,
+						struct rtnl_link_stats64 *stats)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int r;
+
+	for (r = 0; r < nn->num_r_vecs; r++) {
+		struct nfp_net_r_vector *r_vec = &nn->r_vecs[r];
+		u64 data[3];
+		unsigned int start;
+
+		do {
+			start = u64_stats_fetch_begin(&r_vec->rx_sync);
+			data[0] = r_vec->rx_pkts;
+			data[1] = r_vec->rx_bytes;
+			data[2] = r_vec->rx_drops;
+		} while (u64_stats_fetch_retry(&r_vec->rx_sync, start));
+		stats->rx_packets += data[0];
+		stats->rx_bytes += data[1];
+		stats->rx_dropped += data[2];
+
+		do {
+			start = u64_stats_fetch_begin(&r_vec->tx_sync);
+			data[0] = r_vec->tx_pkts;
+			data[1] = r_vec->tx_bytes;
+			data[2] = r_vec->tx_errors;
+		} while (u64_stats_fetch_retry(&r_vec->tx_sync, start));
+		stats->tx_packets += data[0];
+		stats->tx_bytes += data[1];
+		stats->tx_errors += data[2];
+	}
+
+	return stats;
+}
+
+static int nfp_net_set_features(struct net_device *netdev,
+				netdev_features_t features)
+{
+	netdev_features_t changed = netdev->features ^ features;
+	struct nfp_net *nn = netdev_priv(netdev);
+	u32 new_ctrl;
+	int err;
+
+	/* Assume this is not called with features we have not advertised */
+
+	new_ctrl = nn->ctrl;
+
+	if (changed & NETIF_F_RXCSUM) {
+		if (features & NETIF_F_RXCSUM)
+			new_ctrl |= NFP_NET_CFG_CTRL_RXCSUM;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_RXCSUM;
+	}
+
+	if (changed & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM)) {
+		if (features & (NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM))
+			new_ctrl |= NFP_NET_CFG_CTRL_TXCSUM;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_TXCSUM;
+	}
+
+	if (changed & (NETIF_F_TSO | NETIF_F_TSO6)) {
+		if (features & (NETIF_F_TSO | NETIF_F_TSO6))
+			new_ctrl |= NFP_NET_CFG_CTRL_LSO;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_LSO;
+	}
+
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX) {
+		if (features & NETIF_F_HW_VLAN_CTAG_RX)
+			new_ctrl |= NFP_NET_CFG_CTRL_RXVLAN;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_RXVLAN;
+	}
+
+	if (changed & NETIF_F_HW_VLAN_CTAG_TX) {
+		if (features & NETIF_F_HW_VLAN_CTAG_TX)
+			new_ctrl |= NFP_NET_CFG_CTRL_TXVLAN;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_TXVLAN;
+	}
+
+	if (changed & NETIF_F_SG) {
+		if (features & NETIF_F_SG)
+			new_ctrl |= NFP_NET_CFG_CTRL_GATHER;
+		else
+			new_ctrl &= ~NFP_NET_CFG_CTRL_GATHER;
+	}
+
+	nn_dbg(nn, "Feature change 0x%llx -> 0x%llx (changed=0x%llx)\n",
+	       netdev->features, features, changed);
+
+	if (new_ctrl == nn->ctrl)
+		return 0;
+
+	nn_dbg(nn, "NIC ctrl: 0x%x -> 0x%x\n", nn->ctrl, new_ctrl);
+	nn_writel(nn, NFP_NET_CFG_CTRL, new_ctrl);
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_GEN);
+	if (err)
+		return err;
+
+	nn->ctrl = new_ctrl;
+
+	return 0;
+}
+
+static netdev_features_t
+nfp_net_features_check(struct sk_buff *skb, struct net_device *dev,
+		       netdev_features_t features)
+{
+	u8 l4_hdr;
+
+	/* We can't do TSO over double tagged packets (802.1AD) */
+	features &= vlan_features_check(skb, features);
+
+	if (!skb->encapsulation)
+		return features;
+
+	/* Ensure that inner L4 header offset fits into TX descriptor field */
+	if (skb_is_gso(skb)) {
+		u32 hdrlen;
+
+		hdrlen = skb_inner_transport_header(skb) - skb->data +
+			inner_tcp_hdrlen(skb);
+
+		if (unlikely(hdrlen > NFP_NET_LSO_MAX_HDR_SZ))
+			features &= ~NETIF_F_GSO_MASK;
+	}
+
+	/* VXLAN/GRE check */
+	switch (vlan_get_protocol(skb)) {
+	case htons(ETH_P_IP):
+		l4_hdr = ip_hdr(skb)->protocol;
+		break;
+	case htons(ETH_P_IPV6):
+		l4_hdr = ipv6_hdr(skb)->nexthdr;
+		break;
+	default:
+		return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
+	}
+
+	if (skb->inner_protocol_type != ENCAP_TYPE_ETHER ||
+	    skb->inner_protocol != htons(ETH_P_TEB) ||
+	    (l4_hdr != IPPROTO_UDP && l4_hdr != IPPROTO_GRE) ||
+	    (l4_hdr == IPPROTO_UDP &&
+	     (skb_inner_mac_header(skb) - skb_transport_header(skb) !=
+	      sizeof(struct udphdr) + sizeof(struct vxlanhdr))))
+		return features & ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
+
+	return features;
+}
+
+/**
+ * nfp_net_set_vxlan_port() - set vxlan port in SW and reconfigure HW
+ * @nn:   NFP Net device to reconfigure
+ * @idx:  Index into the port table where new port should be written
+ * @port: UDP port to configure (pass zero to remove VXLAN port)
+ */
+static void nfp_net_set_vxlan_port(struct nfp_net *nn, int idx, __be16 port)
+{
+	int i;
+
+	nn->vxlan_ports[idx] = port;
+
+	if (!(nn->ctrl & NFP_NET_CFG_CTRL_VXLAN))
+		return;
+
+	BUILD_BUG_ON(NFP_NET_N_VXLAN_PORTS & 1);
+	for (i = 0; i < NFP_NET_N_VXLAN_PORTS; i += 2)
+		nn_writel(nn, NFP_NET_CFG_VXLAN_PORT + i * sizeof(port),
+			  be16_to_cpu(nn->vxlan_ports[i + 1]) << 16 |
+			  be16_to_cpu(nn->vxlan_ports[i]));
+
+	nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_VXLAN);
+}
+
+/**
+ * nfp_net_find_vxlan_idx() - find table entry of the port or a free one
+ * @nn:   NFP Network structure
+ * @port: UDP port to look for
+ *
+ * Return: if the port is already in the table -- it's position;
+ *	   if the port is not in the table -- free position to use;
+ *	   if the table is full -- -ENOSPC.
+ */
+static int nfp_net_find_vxlan_idx(struct nfp_net *nn, __be16 port)
+{
+	int i, free_idx = -ENOSPC;
+
+	for (i = 0; i < NFP_NET_N_VXLAN_PORTS; i++) {
+		if (nn->vxlan_ports[i] == port)
+			return i;
+		if (!nn->vxlan_usecnt[i])
+			free_idx = i;
+	}
+
+	return free_idx;
+}
+
+static void nfp_net_add_vxlan_port(struct net_device *netdev,
+				   sa_family_t sa_family, __be16 port)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int idx;
+
+	idx = nfp_net_find_vxlan_idx(nn, port);
+	if (idx == -ENOSPC)
+		return;
+
+	if (!nn->vxlan_usecnt[idx]++)
+		nfp_net_set_vxlan_port(nn, idx, port);
+}
+
+static void nfp_net_del_vxlan_port(struct net_device *netdev,
+				   sa_family_t sa_family, __be16 port)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int idx;
+
+	idx = nfp_net_find_vxlan_idx(nn, port);
+	if (!nn->vxlan_usecnt[idx] || idx == -ENOSPC)
+		return;
+
+	if (!--nn->vxlan_usecnt[idx])
+		nfp_net_set_vxlan_port(nn, idx, 0);
+}
+
+static const struct net_device_ops nfp_net_netdev_ops = {
+	.ndo_open		= nfp_net_netdev_open,
+	.ndo_stop		= nfp_net_netdev_close,
+	.ndo_start_xmit		= nfp_net_tx,
+	.ndo_get_stats64	= nfp_net_stat64,
+	.ndo_tx_timeout		= nfp_net_tx_timeout,
+	.ndo_set_rx_mode	= nfp_net_set_rx_mode,
+	.ndo_change_mtu		= nfp_net_change_mtu,
+	.ndo_set_mac_address	= eth_mac_addr,
+	.ndo_set_features	= nfp_net_set_features,
+	.ndo_features_check	= nfp_net_features_check,
+	.ndo_add_vxlan_port     = nfp_net_add_vxlan_port,
+	.ndo_del_vxlan_port     = nfp_net_del_vxlan_port,
+};
+
+/**
+ * nfp_net_info() - Print general info about the NIC
+ * @nn:      NFP Net device to reconfigure
+ */
+void nfp_net_info(struct nfp_net *nn)
+{
+	nn_info(nn, "Netronome %s %sNetdev: TxQs=%d/%d RxQs=%d/%d\n",
+		nn->is_nfp3200 ? "NFP-32xx" : "NFP-6xxx",
+		nn->is_vf ? "VF " : "",
+		nn->num_tx_rings, nn->max_tx_rings,
+		nn->num_rx_rings, nn->max_rx_rings);
+	nn_info(nn, "VER: %d.%d.%d.%d, Maximum supported MTU: %d\n",
+		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->cap,
+		nn->cap & NFP_NET_CFG_CTRL_PROMISC  ? "PROMISC "  : "",
+		nn->cap & NFP_NET_CFG_CTRL_L2BC     ? "L2BCFILT " : "",
+		nn->cap & NFP_NET_CFG_CTRL_L2MC     ? "L2MCFILT " : "",
+		nn->cap & NFP_NET_CFG_CTRL_RXCSUM   ? "RXCSUM "   : "",
+		nn->cap & NFP_NET_CFG_CTRL_TXCSUM   ? "TXCSUM "   : "",
+		nn->cap & NFP_NET_CFG_CTRL_RXVLAN   ? "RXVLAN "   : "",
+		nn->cap & NFP_NET_CFG_CTRL_TXVLAN   ? "TXVLAN "   : "",
+		nn->cap & NFP_NET_CFG_CTRL_SCATTER  ? "SCATTER "  : "",
+		nn->cap & NFP_NET_CFG_CTRL_GATHER   ? "GATHER "   : "",
+		nn->cap & NFP_NET_CFG_CTRL_LSO      ? "TSO "      : "",
+		nn->cap & NFP_NET_CFG_CTRL_RSS      ? "RSS "      : "",
+		nn->cap & NFP_NET_CFG_CTRL_L2SWITCH ? "L2SWITCH " : "",
+		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 "	  : "");
+}
+
+/**
+ * nfp_net_netdev_alloc() - Allocate netdev and related structure
+ * @pdev:         PCI device
+ * @max_tx_rings: Maximum number of TX rings supported by device
+ * @max_rx_rings: Maximum number of RX rings supported by device
+ *
+ * This function allocates a netdev device and fills in the initial
+ * part of the @struct nfp_net structure.
+ *
+ * Return: NFP Net device structure, or ERR_PTR on error.
+ */
+struct nfp_net *nfp_net_netdev_alloc(struct pci_dev *pdev,
+				     int max_tx_rings, int max_rx_rings)
+{
+	struct net_device *netdev;
+	struct nfp_net *nn;
+	int nqs;
+
+	netdev = alloc_etherdev_mqs(sizeof(struct nfp_net),
+				    max_tx_rings, max_rx_rings);
+	if (!netdev)
+		return ERR_PTR(-ENOMEM);
+
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+	nn = netdev_priv(netdev);
+
+	nn->netdev = netdev;
+	nn->pdev = pdev;
+
+	nn->max_tx_rings = max_tx_rings;
+	nn->max_rx_rings = max_rx_rings;
+
+	nqs = netif_get_num_default_rss_queues();
+	nn->num_tx_rings = min_t(int, nqs, max_tx_rings);
+	nn->num_rx_rings = min_t(int, nqs, max_rx_rings);
+
+	nn->txd_cnt = NFP_NET_TX_DESCS_DEFAULT;
+	nn->rxd_cnt = NFP_NET_RX_DESCS_DEFAULT;
+
+	spin_lock_init(&nn->reconfig_lock);
+	spin_lock_init(&nn->link_status_lock);
+
+	return nn;
+}
+
+/**
+ * nfp_net_netdev_free() - Undo what @nfp_net_netdev_alloc() did
+ * @nn:      NFP Net device to reconfigure
+ */
+void nfp_net_netdev_free(struct nfp_net *nn)
+{
+	free_netdev(nn->netdev);
+}
+
+/**
+ * nfp_net_rss_init() - Set the initial RSS parameters
+ * @nn:	     NFP Net device to reconfigure
+ */
+static void nfp_net_rss_init(struct nfp_net *nn)
+{
+	int i;
+
+	netdev_rss_key_fill(nn->rss_key, NFP_NET_CFG_RSS_KEY_SZ);
+
+	for (i = 0; i < sizeof(nn->rss_itbl); i++)
+		nn->rss_itbl[i] =
+			ethtool_rxfh_indir_default(i, nn->num_rx_rings);
+
+	/* Enable IPv4/IPv6 TCP by default */
+	nn->rss_cfg = NFP_NET_CFG_RSS_IPV4_TCP |
+		      NFP_NET_CFG_RSS_IPV6_TCP |
+		      NFP_NET_CFG_RSS_TOEPLITZ |
+		      NFP_NET_CFG_RSS_MASK;
+}
+
+/**
+ * nfp_net_irqmod_init() - Set the initial IRQ moderation parameters
+ * @nn:	     NFP Net device to reconfigure
+ */
+static void nfp_net_irqmod_init(struct nfp_net *nn)
+{
+	nn->rx_coalesce_usecs      = 50;
+	nn->rx_coalesce_max_frames = 64;
+	nn->tx_coalesce_usecs      = 50;
+	nn->tx_coalesce_max_frames = 64;
+}
+
+/**
+ * nfp_net_netdev_init() - Initialise/finalise the netdev structure
+ * @netdev:      netdev structure
+ *
+ * Return: 0 on success or negative errno on error.
+ */
+int nfp_net_netdev_init(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int err;
+
+	/* Get some of the read-only fields from the BAR */
+	nn->cap = nn_readl(nn, NFP_NET_CFG_CAP);
+	nn->max_mtu = nn_readl(nn, NFP_NET_CFG_MAX_MTU);
+
+	nfp_net_write_mac_addr(nn, nn->netdev->dev_addr);
+
+	/* Set default MTU and Freelist buffer size */
+	if (nn->max_mtu < NFP_NET_DEFAULT_MTU)
+		netdev->mtu = nn->max_mtu;
+	else
+		netdev->mtu = NFP_NET_DEFAULT_MTU;
+	nn->fl_bufsz = NFP_NET_DEFAULT_RX_BUFSZ;
+
+	/* Advertise/enable offloads based on capabilities
+	 *
+	 * Note: netdev->features show the currently enabled features
+	 * and netdev->hw_features advertises which features are
+	 * supported.  By default we enable most features.
+	 */
+	netdev->hw_features = NETIF_F_HIGHDMA;
+	if (nn->cap & NFP_NET_CFG_CTRL_RXCSUM) {
+		netdev->hw_features |= NETIF_F_RXCSUM;
+		nn->ctrl |= NFP_NET_CFG_CTRL_RXCSUM;
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_TXCSUM) {
+		netdev->hw_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
+		nn->ctrl |= NFP_NET_CFG_CTRL_TXCSUM;
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_GATHER) {
+		netdev->hw_features |= NETIF_F_SG;
+		nn->ctrl |= NFP_NET_CFG_CTRL_GATHER;
+	}
+	if ((nn->cap & NFP_NET_CFG_CTRL_LSO) && nn->fw_ver.major > 2) {
+		netdev->hw_features |= NETIF_F_TSO | NETIF_F_TSO6;
+		nn->ctrl |= NFP_NET_CFG_CTRL_LSO;
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_RSS) {
+		netdev->hw_features |= NETIF_F_RXHASH;
+		nfp_net_rss_init(nn);
+		nn->ctrl |= NFP_NET_CFG_CTRL_RSS;
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_VXLAN &&
+	    nn->cap & NFP_NET_CFG_CTRL_NVGRE) {
+		if (nn->cap & NFP_NET_CFG_CTRL_LSO)
+			netdev->hw_features |= NETIF_F_GSO_GRE |
+					       NETIF_F_GSO_UDP_TUNNEL;
+		nn->ctrl |= NFP_NET_CFG_CTRL_VXLAN | NFP_NET_CFG_CTRL_NVGRE;
+
+		netdev->hw_enc_features = netdev->hw_features;
+	}
+
+	netdev->vlan_features = netdev->hw_features;
+
+	if (nn->cap & NFP_NET_CFG_CTRL_RXVLAN) {
+		netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_RX;
+		nn->ctrl |= NFP_NET_CFG_CTRL_RXVLAN;
+	}
+	if (nn->cap & NFP_NET_CFG_CTRL_TXVLAN) {
+		netdev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX;
+		nn->ctrl |= NFP_NET_CFG_CTRL_TXVLAN;
+	}
+
+	netdev->features = netdev->hw_features;
+
+	/* Advertise but disable TSO by default. */
+	netdev->features &= ~(NETIF_F_TSO | NETIF_F_TSO6);
+
+	/* Allow L2 Broadcast and Multicast through by default, if supported */
+	if (nn->cap & NFP_NET_CFG_CTRL_L2BC)
+		nn->ctrl |= NFP_NET_CFG_CTRL_L2BC;
+	if (nn->cap & NFP_NET_CFG_CTRL_L2MC)
+		nn->ctrl |= NFP_NET_CFG_CTRL_L2MC;
+
+	/* Allow IRQ moderation, if supported */
+	if (nn->cap & NFP_NET_CFG_CTRL_IRQMOD) {
+		nfp_net_irqmod_init(nn);
+		nn->ctrl |= NFP_NET_CFG_CTRL_IRQMOD;
+	}
+
+	/* On NFP-3200 enable MSI-X auto-masking, if supported and the
+	 * interrupts are not shared.
+	 */
+	if (nn->is_nfp3200 && nn->cap & NFP_NET_CFG_CTRL_MSIXAUTO)
+		nn->ctrl |= NFP_NET_CFG_CTRL_MSIXAUTO;
+
+	/* On NFP4000/NFP6000, determine RX packet/metadata boundary offset */
+	if (nn->fw_ver.major >= 2)
+		nn->rx_offset = nn_readl(nn, NFP_NET_CFG_RX_OFFSET);
+	else
+		nn->rx_offset = NFP_NET_RX_OFFSET;
+
+	/* Stash the re-configuration queue away.  First odd queue in TX Bar */
+	nn->qcp_cfg = nn->tx_bar + NFP_QCP_QUEUE_ADDR_SZ;
+
+	/* Make sure the FW knows the netdev is supposed to be disabled here */
+	nn_writel(nn, NFP_NET_CFG_CTRL, 0);
+	nn_writeq(nn, NFP_NET_CFG_TXRS_ENABLE, 0);
+	nn_writeq(nn, NFP_NET_CFG_RXRS_ENABLE, 0);
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_RING |
+				   NFP_NET_CFG_UPDATE_GEN);
+	if (err)
+		return err;
+
+	/* Finalise the netdev setup */
+	ether_setup(netdev);
+	netdev->netdev_ops = &nfp_net_netdev_ops;
+	netdev->watchdog_timeo = msecs_to_jiffies(5 * 1000);
+	netif_carrier_off(netdev);
+
+	nfp_net_set_ethtool_ops(netdev);
+	nfp_net_irqs_assign(netdev);
+
+	return register_netdev(netdev);
+}
+
+/**
+ * nfp_net_netdev_clean() - Undo what nfp_net_netdev_init() did.
+ * @netdev:      netdev structure
+ */
+void nfp_net_netdev_clean(struct net_device *netdev)
+{
+	unregister_netdev(netdev);
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
new file mode 100644
index 000000000000..8692003aeed8
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ctrl.h
@@ -0,0 +1,323 @@
+/*
+ * Copyright (C) 2015 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_ctrl.h
+ * Netronome network device driver: Control BAR layout
+ * Authors: Jakub Kicinski <jakub.kicinski@netronome.com>
+ *          Jason McMullan <jason.mcmullan@netronome.com>
+ *          Rolf Neugebauer <rolf.neugebauer@netronome.com>
+ *          Brad Petrus <brad.petrus@netronome.com>
+ */
+
+#ifndef _NFP_NET_CTRL_H_
+#define _NFP_NET_CTRL_H_
+
+/* IMPORTANT: This header file is shared with the FW,
+ *	      no OS specific constructs, please!
+ */
+
+/**
+ * Configuration BAR size.
+ *
+ * The configuration BAR is 8K in size, but on the NFP6000, due to
+ * THB-350, 32k needs to be reserved.
+ */
+#define NFP_NET_CFG_BAR_SZ              (32 * 1024)
+
+/**
+ * Offset in Freelist buffer where packet starts on RX
+ */
+#define NFP_NET_RX_OFFSET               32
+
+/**
+ * Maximum header size supported for LSO frames
+ */
+#define NFP_NET_LSO_MAX_HDR_SZ		255
+
+/**
+ * Hash type pre-pended when a RSS hash was computed
+ */
+#define NFP_NET_RSS_NONE                0
+#define NFP_NET_RSS_IPV4                1
+#define NFP_NET_RSS_IPV6                2
+#define NFP_NET_RSS_IPV6_EX             3
+#define NFP_NET_RSS_IPV4_TCP            4
+#define NFP_NET_RSS_IPV6_TCP            5
+#define NFP_NET_RSS_IPV6_EX_TCP         6
+#define NFP_NET_RSS_IPV4_UDP            7
+#define NFP_NET_RSS_IPV6_UDP            8
+#define NFP_NET_RSS_IPV6_EX_UDP         9
+
+/**
+ * @NFP_NET_TXR_MAX:         Maximum number of TX rings
+ * @NFP_NET_TXR_MASK:        Mask for TX rings
+ * @NFP_NET_RXR_MAX:         Maximum number of RX rings
+ * @NFP_NET_RXR_MASK:        Mask for RX rings
+ */
+#define NFP_NET_TXR_MAX                 64
+#define NFP_NET_TXR_MASK                (NFP_NET_TXR_MAX - 1)
+#define NFP_NET_RXR_MAX                 64
+#define NFP_NET_RXR_MASK                (NFP_NET_RXR_MAX - 1)
+
+/**
+ * Read/Write config words (0x0000 - 0x002c)
+ * @NFP_NET_CFG_CTRL:        Global control
+ * @NFP_NET_CFG_UPDATE:      Indicate which fields are updated
+ * @NFP_NET_CFG_TXRS_ENABLE: Bitmask of enabled TX rings
+ * @NFP_NET_CFG_RXRS_ENABLE: Bitmask of enabled RX rings
+ * @NFP_NET_CFG_MTU:         Set MTU size
+ * @NFP_NET_CFG_FLBUFSZ:     Set freelist buffer size (must be larger than MTU)
+ * @NFP_NET_CFG_EXN:         MSI-X table entry for exceptions
+ * @NFP_NET_CFG_LSC:         MSI-X table entry for link state changes
+ * @NFP_NET_CFG_MACADDR:     MAC address
+ *
+ * TODO:
+ * - define Error details in UPDATE
+ */
+#define NFP_NET_CFG_CTRL                0x0000
+#define   NFP_NET_CFG_CTRL_ENABLE         (0x1 <<  0) /* Global enable */
+#define   NFP_NET_CFG_CTRL_PROMISC        (0x1 <<  1) /* Enable Promisc mode */
+#define   NFP_NET_CFG_CTRL_L2BC           (0x1 <<  2) /* Allow L2 Broadcast */
+#define   NFP_NET_CFG_CTRL_L2MC           (0x1 <<  3) /* Allow L2 Multicast */
+#define   NFP_NET_CFG_CTRL_RXCSUM         (0x1 <<  4) /* Enable RX Checksum */
+#define   NFP_NET_CFG_CTRL_TXCSUM         (0x1 <<  5) /* Enable TX Checksum */
+#define   NFP_NET_CFG_CTRL_RXVLAN         (0x1 <<  6) /* Enable VLAN strip */
+#define   NFP_NET_CFG_CTRL_TXVLAN         (0x1 <<  7) /* Enable VLAN insert */
+#define   NFP_NET_CFG_CTRL_SCATTER        (0x1 <<  8) /* Scatter DMA */
+#define   NFP_NET_CFG_CTRL_GATHER         (0x1 <<  9) /* Gather DMA */
+#define   NFP_NET_CFG_CTRL_LSO            (0x1 << 10) /* LSO/TSO */
+#define   NFP_NET_CFG_CTRL_RINGCFG        (0x1 << 16) /* Ring runtime changes */
+#define   NFP_NET_CFG_CTRL_RSS            (0x1 << 17) /* RSS */
+#define   NFP_NET_CFG_CTRL_IRQMOD         (0x1 << 18) /* Interrupt moderation */
+#define   NFP_NET_CFG_CTRL_RINGPRIO       (0x1 << 19) /* Ring priorities */
+#define   NFP_NET_CFG_CTRL_MSIXAUTO       (0x1 << 20) /* MSI-X auto-masking */
+#define   NFP_NET_CFG_CTRL_TXRWB          (0x1 << 21) /* Write-back of TX ring*/
+#define   NFP_NET_CFG_CTRL_L2SWITCH       (0x1 << 22) /* L2 Switch */
+#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_UPDATE              0x0004
+#define   NFP_NET_CFG_UPDATE_GEN          (0x1 <<  0) /* General update */
+#define   NFP_NET_CFG_UPDATE_RING         (0x1 <<  1) /* Ring config change */
+#define   NFP_NET_CFG_UPDATE_RSS          (0x1 <<  2) /* RSS config change */
+#define   NFP_NET_CFG_UPDATE_TXRPRIO      (0x1 <<  3) /* TX Ring prio change */
+#define   NFP_NET_CFG_UPDATE_RXRPRIO      (0x1 <<  4) /* RX Ring prio change */
+#define   NFP_NET_CFG_UPDATE_MSIX         (0x1 <<  5) /* MSI-X change */
+#define   NFP_NET_CFG_UPDATE_L2SWITCH     (0x1 <<  6) /* Switch changes */
+#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_ERR          (0x1 << 31) /* A error occurred */
+#define NFP_NET_CFG_TXRS_ENABLE         0x0008
+#define NFP_NET_CFG_RXRS_ENABLE         0x0010
+#define NFP_NET_CFG_MTU                 0x0018
+#define NFP_NET_CFG_FLBUFSZ             0x001c
+#define NFP_NET_CFG_EXN                 0x001f
+#define NFP_NET_CFG_LSC                 0x0020
+#define NFP_NET_CFG_MACADDR             0x0024
+
+/**
+ * Read-only words (0x0030 - 0x0050):
+ * @NFP_NET_CFG_VERSION:     Firmware version number
+ * @NFP_NET_CFG_STS:         Status
+ * @NFP_NET_CFG_CAP:         Capabilities (same bits as @NFP_NET_CFG_CTRL)
+ * @NFP_NET_MAX_TXRINGS:     Maximum number of TX rings
+ * @NFP_NET_MAX_RXRINGS:     Maximum number of RX rings
+ * @NFP_NET_MAX_MTU:         Maximum support MTU
+ * @NFP_NET_CFG_START_TXQ:   Start Queue Control Queue to use for TX (PF only)
+ * @NFP_NET_CFG_START_RXQ:   Start Queue Control Queue to use for RX (PF only)
+ *
+ * TODO:
+ * - define more STS bits
+ */
+#define NFP_NET_CFG_VERSION             0x0030
+#define   NFP_NET_CFG_VERSION_RESERVED_MASK	(0xff << 24)
+#define   NFP_NET_CFG_VERSION_CLASS_MASK  (0xff << 16)
+#define   NFP_NET_CFG_VERSION_CLASS(x)    (((x) & 0xff) << 16)
+#define   NFP_NET_CFG_VERSION_CLASS_GENERIC	0
+#define   NFP_NET_CFG_VERSION_MAJOR_MASK  (0xff <<  8)
+#define   NFP_NET_CFG_VERSION_MAJOR(x)    (((x) & 0xff) <<  8)
+#define   NFP_NET_CFG_VERSION_MINOR_MASK  (0xff <<  0)
+#define   NFP_NET_CFG_VERSION_MINOR(x)    (((x) & 0xff) <<  0)
+#define NFP_NET_CFG_STS                 0x0034
+#define   NFP_NET_CFG_STS_LINK            (0x1 << 0) /* Link up or down */
+#define NFP_NET_CFG_CAP                 0x0038
+#define NFP_NET_CFG_MAX_TXRINGS         0x003c
+#define NFP_NET_CFG_MAX_RXRINGS         0x0040
+#define NFP_NET_CFG_MAX_MTU             0x0044
+/* Next two words are being used by VFs for solving THB350 issue */
+#define NFP_NET_CFG_START_TXQ           0x0048
+#define NFP_NET_CFG_START_RXQ           0x004c
+
+/**
+ * NFP-3200 workaround (0x0050 - 0x0058)
+ * @NFP_NET_CFG_SPARE_ADDR:  DMA address for ME code to use (e.g. YDS-155 fix)
+ */
+#define NFP_NET_CFG_SPARE_ADDR          0x0050
+/**
+ * NFP6000/NFP4000 - Prepend configuration
+ */
+#define NFP_NET_CFG_RX_OFFSET		0x0050
+#define NFP_NET_CFG_RX_OFFSET_DYNAMIC		0	/* Prepend mode */
+
+/**
+ * NFP6000/NFP4000 - VXLAN/UDP encap configuration
+ * @NFP_NET_CFG_VXLAN_PORT:	Base address of table of tunnels' UDP dst ports
+ * @NFP_NET_CFG_VXLAN_SZ:	Size of the UDP port table in bytes
+ */
+#define NFP_NET_CFG_VXLAN_PORT		0x0060
+#define NFP_NET_CFG_VXLAN_SZ		  0x0008
+
+/**
+ * 64B reserved for future use (0x0080 - 0x00c0)
+ */
+#define NFP_NET_CFG_RESERVED            0x0080
+#define NFP_NET_CFG_RESERVED_SZ         0x0040
+
+/**
+ * RSS configuration (0x0100 - 0x01ac):
+ * Used only when NFP_NET_CFG_CTRL_RSS is enabled
+ * @NFP_NET_CFG_RSS_CFG:     RSS configuration word
+ * @NFP_NET_CFG_RSS_KEY:     RSS "secret" key
+ * @NFP_NET_CFG_RSS_ITBL:    RSS indirection table
+ */
+#define NFP_NET_CFG_RSS_BASE            0x0100
+#define NFP_NET_CFG_RSS_CTRL            NFP_NET_CFG_RSS_BASE
+#define   NFP_NET_CFG_RSS_MASK            (0x7f)
+#define   NFP_NET_CFG_RSS_MASK_of(_x)     ((_x) & 0x7f)
+#define   NFP_NET_CFG_RSS_IPV4            (1 <<  8) /* RSS for IPv4 */
+#define   NFP_NET_CFG_RSS_IPV6            (1 <<  9) /* RSS for IPv6 */
+#define   NFP_NET_CFG_RSS_IPV4_TCP        (1 << 10) /* RSS for IPv4/TCP */
+#define   NFP_NET_CFG_RSS_IPV4_UDP        (1 << 11) /* RSS for IPv4/UDP */
+#define   NFP_NET_CFG_RSS_IPV6_TCP        (1 << 12) /* RSS for IPv6/TCP */
+#define   NFP_NET_CFG_RSS_IPV6_UDP        (1 << 13) /* RSS for IPv6/UDP */
+#define   NFP_NET_CFG_RSS_TOEPLITZ        (1 << 24) /* Use Toeplitz hash */
+#define NFP_NET_CFG_RSS_KEY             (NFP_NET_CFG_RSS_BASE + 0x4)
+#define NFP_NET_CFG_RSS_KEY_SZ          0x28
+#define NFP_NET_CFG_RSS_ITBL            (NFP_NET_CFG_RSS_BASE + 0x4 + \
+					 NFP_NET_CFG_RSS_KEY_SZ)
+#define NFP_NET_CFG_RSS_ITBL_SZ         0x80
+
+/**
+ * TX ring configuration (0x200 - 0x800)
+ * @NFP_NET_CFG_TXR_BASE:    Base offset for TX ring configuration
+ * @NFP_NET_CFG_TXR_ADDR:    Per TX ring DMA address (8B entries)
+ * @NFP_NET_CFG_TXR_WB_ADDR: Per TX ring write back DMA address (8B entries)
+ * @NFP_NET_CFG_TXR_SZ:      Per TX ring ring size (1B entries)
+ * @NFP_NET_CFG_TXR_VEC:     Per TX ring MSI-X table entry (1B entries)
+ * @NFP_NET_CFG_TXR_PRIO:    Per TX ring priority (1B entries)
+ * @NFP_NET_CFG_TXR_IRQ_MOD: Per TX ring interrupt moderation packet
+ */
+#define NFP_NET_CFG_TXR_BASE            0x0200
+#define NFP_NET_CFG_TXR_ADDR(_x)        (NFP_NET_CFG_TXR_BASE + ((_x) * 0x8))
+#define NFP_NET_CFG_TXR_WB_ADDR(_x)     (NFP_NET_CFG_TXR_BASE + 0x200 + \
+					 ((_x) * 0x8))
+#define NFP_NET_CFG_TXR_SZ(_x)          (NFP_NET_CFG_TXR_BASE + 0x400 + (_x))
+#define NFP_NET_CFG_TXR_VEC(_x)         (NFP_NET_CFG_TXR_BASE + 0x440 + (_x))
+#define NFP_NET_CFG_TXR_PRIO(_x)        (NFP_NET_CFG_TXR_BASE + 0x480 + (_x))
+#define NFP_NET_CFG_TXR_IRQ_MOD(_x)	(NFP_NET_CFG_TXR_BASE + 0x500 + \
+					 ((_x) * 0x4))
+
+/**
+ * RX ring configuration (0x0800 - 0x0c00)
+ * @NFP_NET_CFG_RXR_BASE:    Base offset for RX ring configuration
+ * @NFP_NET_CFG_RXR_ADDR:    Per RX ring DMA address (8B entries)
+ * @NFP_NET_CFG_RXR_SZ:      Per RX ring ring size (1B entries)
+ * @NFP_NET_CFG_RXR_VEC:     Per RX ring MSI-X table entry (1B entries)
+ * @NFP_NET_CFG_RXR_PRIO:    Per RX ring priority (1B entries)
+ * @NFP_NET_CFG_RXR_IRQ_MOD: Per RX ring interrupt moderation (4B entries)
+ */
+#define NFP_NET_CFG_RXR_BASE            0x0800
+#define NFP_NET_CFG_RXR_ADDR(_x)        (NFP_NET_CFG_RXR_BASE + ((_x) * 0x8))
+#define NFP_NET_CFG_RXR_SZ(_x)          (NFP_NET_CFG_RXR_BASE + 0x200 + (_x))
+#define NFP_NET_CFG_RXR_VEC(_x)         (NFP_NET_CFG_RXR_BASE + 0x240 + (_x))
+#define NFP_NET_CFG_RXR_PRIO(_x)        (NFP_NET_CFG_RXR_BASE + 0x280 + (_x))
+#define NFP_NET_CFG_RXR_IRQ_MOD(_x)	(NFP_NET_CFG_RXR_BASE + 0x300 + \
+					 ((_x) * 0x4))
+
+/**
+ * Interrupt Control/Cause registers (0x0c00 - 0x0d00)
+ * These registers are only used when MSI-X auto-masking is not
+ * enabled (@NFP_NET_CFG_CTRL_MSIXAUTO not set).  The array is index
+ * by MSI-X entry and are 1B in size.  If an entry is zero, the
+ * corresponding entry is enabled.  If the FW generates an interrupt,
+ * it writes a cause into the corresponding field.  This also masks
+ * the MSI-X entry and the host driver must clear the register to
+ * re-enable the interrupt.
+ */
+#define NFP_NET_CFG_ICR_BASE            0x0c00
+#define NFP_NET_CFG_ICR(_x)             (NFP_NET_CFG_ICR_BASE + (_x))
+#define   NFP_NET_CFG_ICR_UNMASKED      0x0
+#define   NFP_NET_CFG_ICR_RXTX          0x1
+#define   NFP_NET_CFG_ICR_LSC           0x2
+
+/**
+ * General device stats (0x0d00 - 0x0d90)
+ * all counters are 64bit.
+ */
+#define NFP_NET_CFG_STATS_BASE          0x0d00
+#define NFP_NET_CFG_STATS_RX_DISCARDS   (NFP_NET_CFG_STATS_BASE + 0x00)
+#define NFP_NET_CFG_STATS_RX_ERRORS     (NFP_NET_CFG_STATS_BASE + 0x08)
+#define NFP_NET_CFG_STATS_RX_OCTETS     (NFP_NET_CFG_STATS_BASE + 0x10)
+#define NFP_NET_CFG_STATS_RX_UC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x18)
+#define NFP_NET_CFG_STATS_RX_MC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x20)
+#define NFP_NET_CFG_STATS_RX_BC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x28)
+#define NFP_NET_CFG_STATS_RX_FRAMES     (NFP_NET_CFG_STATS_BASE + 0x30)
+#define NFP_NET_CFG_STATS_RX_MC_FRAMES  (NFP_NET_CFG_STATS_BASE + 0x38)
+#define NFP_NET_CFG_STATS_RX_BC_FRAMES  (NFP_NET_CFG_STATS_BASE + 0x40)
+
+#define NFP_NET_CFG_STATS_TX_DISCARDS   (NFP_NET_CFG_STATS_BASE + 0x48)
+#define NFP_NET_CFG_STATS_TX_ERRORS     (NFP_NET_CFG_STATS_BASE + 0x50)
+#define NFP_NET_CFG_STATS_TX_OCTETS     (NFP_NET_CFG_STATS_BASE + 0x58)
+#define NFP_NET_CFG_STATS_TX_UC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x60)
+#define NFP_NET_CFG_STATS_TX_MC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x68)
+#define NFP_NET_CFG_STATS_TX_BC_OCTETS  (NFP_NET_CFG_STATS_BASE + 0x70)
+#define NFP_NET_CFG_STATS_TX_FRAMES     (NFP_NET_CFG_STATS_BASE + 0x78)
+#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)
+
+/**
+ * Per ring stats (0x1000 - 0x1800)
+ * options, 64bit per entry
+ * @NFP_NET_CFG_TXR_STATS:   TX ring statistics (Packet and Byte count)
+ * @NFP_NET_CFG_RXR_STATS:   RX ring statistics (Packet and Byte count)
+ */
+#define NFP_NET_CFG_TXR_STATS_BASE      0x1000
+#define NFP_NET_CFG_TXR_STATS(_x)       (NFP_NET_CFG_TXR_STATS_BASE + \
+					 ((_x) * 0x10))
+#define NFP_NET_CFG_RXR_STATS_BASE      0x1400
+#define NFP_NET_CFG_RXR_STATS(_x)       (NFP_NET_CFG_RXR_STATS_BASE + \
+					 ((_x) * 0x10))
+
+#endif /* _NFP_NET_CTRL_H_ */
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
new file mode 100644
index 000000000000..4c97c713121c
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_debugfs.c
@@ -0,0 +1,235 @@
+/*
+ * Copyright (C) 2015 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.
+ */
+#include <linux/debugfs.h>
+#include <linux/module.h>
+#include <linux/rtnetlink.h>
+
+#include "nfp_net.h"
+
+static struct dentry *nfp_dir;
+
+static int nfp_net_debugfs_rx_q_read(struct seq_file *file, void *data)
+{
+	struct nfp_net_rx_ring *rx_ring = file->private;
+	int fl_rd_p, fl_wr_p, rx_rd_p, rx_wr_p, rxd_cnt;
+	struct nfp_net_rx_desc *rxd;
+	struct sk_buff *skb;
+	struct nfp_net *nn;
+	int i;
+
+	rtnl_lock();
+
+	if (!rx_ring->r_vec || !rx_ring->r_vec->nfp_net)
+		goto out;
+	nn = rx_ring->r_vec->nfp_net;
+	if (!netif_running(nn->netdev))
+		goto out;
+
+	rxd_cnt = rx_ring->cnt;
+
+	fl_rd_p = nfp_qcp_rd_ptr_read(rx_ring->qcp_fl);
+	fl_wr_p = nfp_qcp_wr_ptr_read(rx_ring->qcp_fl);
+	rx_rd_p = nfp_qcp_rd_ptr_read(rx_ring->qcp_rx);
+	rx_wr_p = nfp_qcp_wr_ptr_read(rx_ring->qcp_rx);
+
+	seq_printf(file, "RX[%02d]: H_RD=%d H_WR=%d FL_RD=%d FL_WR=%d RX_RD=%d RX_WR=%d\n",
+		   rx_ring->idx, rx_ring->rd_p, rx_ring->wr_p,
+		   fl_rd_p, fl_wr_p, rx_rd_p, rx_wr_p);
+
+	for (i = 0; i < rxd_cnt; i++) {
+		rxd = &rx_ring->rxds[i];
+		seq_printf(file, "%04d: 0x%08x 0x%08x", i,
+			   rxd->vals[0], rxd->vals[1]);
+
+		skb = READ_ONCE(rx_ring->rxbufs[i].skb);
+		if (skb)
+			seq_printf(file, " skb->head=%p skb->data=%p",
+				   skb->head, skb->data);
+
+		if (rx_ring->rxbufs[i].dma_addr)
+			seq_printf(file, " dma_addr=%pad",
+				   &rx_ring->rxbufs[i].dma_addr);
+
+		if (i == rx_ring->rd_p % rxd_cnt)
+			seq_puts(file, " H_RD ");
+		if (i == rx_ring->wr_p % rxd_cnt)
+			seq_puts(file, " H_WR ");
+		if (i == fl_rd_p % rxd_cnt)
+			seq_puts(file, " FL_RD");
+		if (i == fl_wr_p % rxd_cnt)
+			seq_puts(file, " FL_WR");
+		if (i == rx_rd_p % rxd_cnt)
+			seq_puts(file, " RX_RD");
+		if (i == rx_wr_p % rxd_cnt)
+			seq_puts(file, " RX_WR");
+
+		seq_putc(file, '\n');
+	}
+out:
+	rtnl_unlock();
+	return 0;
+}
+
+static int nfp_net_debugfs_rx_q_open(struct inode *inode, struct file *f)
+{
+	return single_open(f, nfp_net_debugfs_rx_q_read, inode->i_private);
+}
+
+static const struct file_operations nfp_rx_q_fops = {
+	.owner = THIS_MODULE,
+	.open = nfp_net_debugfs_rx_q_open,
+	.release = single_release,
+	.read = seq_read,
+	.llseek = seq_lseek
+};
+
+static int nfp_net_debugfs_tx_q_read(struct seq_file *file, void *data)
+{
+	struct nfp_net_tx_ring *tx_ring = file->private;
+	struct nfp_net_tx_desc *txd;
+	int d_rd_p, d_wr_p, txd_cnt;
+	struct sk_buff *skb;
+	struct nfp_net *nn;
+	int i;
+
+	rtnl_lock();
+
+	if (!tx_ring->r_vec || !tx_ring->r_vec->nfp_net)
+		goto out;
+	nn = tx_ring->r_vec->nfp_net;
+	if (!netif_running(nn->netdev))
+		goto out;
+
+	txd_cnt = tx_ring->cnt;
+
+	d_rd_p = nfp_qcp_rd_ptr_read(tx_ring->qcp_q);
+	d_wr_p = nfp_qcp_wr_ptr_read(tx_ring->qcp_q);
+
+	seq_printf(file, "TX[%02d]: H_RD=%d H_WR=%d D_RD=%d D_WR=%d\n",
+		   tx_ring->idx, tx_ring->rd_p, tx_ring->wr_p, d_rd_p, d_wr_p);
+
+	for (i = 0; i < txd_cnt; i++) {
+		txd = &tx_ring->txds[i];
+		seq_printf(file, "%04d: 0x%08x 0x%08x 0x%08x 0x%08x", i,
+			   txd->vals[0], txd->vals[1],
+			   txd->vals[2], txd->vals[3]);
+
+		skb = READ_ONCE(tx_ring->txbufs[i].skb);
+		if (skb)
+			seq_printf(file, " skb->head=%p skb->data=%p",
+				   skb->head, skb->data);
+		if (tx_ring->txbufs[i].dma_addr)
+			seq_printf(file, " dma_addr=%pad",
+				   &tx_ring->txbufs[i].dma_addr);
+
+		if (i == tx_ring->rd_p % txd_cnt)
+			seq_puts(file, " H_RD");
+		if (i == tx_ring->wr_p % txd_cnt)
+			seq_puts(file, " H_WR");
+		if (i == d_rd_p % txd_cnt)
+			seq_puts(file, " D_RD");
+		if (i == d_wr_p % txd_cnt)
+			seq_puts(file, " D_WR");
+
+		seq_putc(file, '\n');
+	}
+out:
+	rtnl_unlock();
+	return 0;
+}
+
+static int nfp_net_debugfs_tx_q_open(struct inode *inode, struct file *f)
+{
+	return single_open(f, nfp_net_debugfs_tx_q_read, inode->i_private);
+}
+
+static const struct file_operations nfp_tx_q_fops = {
+	.owner = THIS_MODULE,
+	.open = nfp_net_debugfs_tx_q_open,
+	.release = single_release,
+	.read = seq_read,
+	.llseek = seq_lseek
+};
+
+void nfp_net_debugfs_adapter_add(struct nfp_net *nn)
+{
+	static struct dentry *queues, *tx, *rx;
+	char int_name[16];
+	int i;
+
+	if (IS_ERR_OR_NULL(nfp_dir))
+		return;
+
+	nn->debugfs_dir = debugfs_create_dir(pci_name(nn->pdev), nfp_dir);
+	if (IS_ERR_OR_NULL(nn->debugfs_dir))
+		return;
+
+	/* Create queue debugging sub-tree */
+	queues = debugfs_create_dir("queue", nn->debugfs_dir);
+	if (IS_ERR_OR_NULL(nn->debugfs_dir))
+		return;
+
+	rx = debugfs_create_dir("rx", queues);
+	tx = debugfs_create_dir("tx", queues);
+	if (IS_ERR_OR_NULL(rx) || IS_ERR_OR_NULL(tx))
+		return;
+
+	for (i = 0; i < nn->num_rx_rings; i++) {
+		sprintf(int_name, "%d", i);
+		debugfs_create_file(int_name, S_IRUSR, rx,
+				    &nn->rx_rings[i], &nfp_rx_q_fops);
+	}
+
+	for (i = 0; i < nn->num_tx_rings; i++) {
+		sprintf(int_name, "%d", i);
+		debugfs_create_file(int_name, S_IRUSR, tx,
+				    &nn->tx_rings[i], &nfp_tx_q_fops);
+	}
+}
+
+void nfp_net_debugfs_adapter_del(struct nfp_net *nn)
+{
+	debugfs_remove_recursive(nn->debugfs_dir);
+	nn->debugfs_dir = NULL;
+}
+
+void nfp_net_debugfs_create(void)
+{
+	nfp_dir = debugfs_create_dir("nfp_net", NULL);
+}
+
+void nfp_net_debugfs_destroy(void)
+{
+	debugfs_remove_recursive(nfp_dir);
+	nfp_dir = NULL;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
new file mode 100644
index 000000000000..9a4084a68db5
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_net_ethtool.c
@@ -0,0 +1,640 @@
+/*
+ * Copyright (C) 2015 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_ethtool.c
+ * Netronome network device driver: ethtool support
+ * Authors: Jakub Kicinski <jakub.kicinski@netronome.com>
+ *          Jason McMullan <jason.mcmullan@netronome.com>
+ *          Rolf Neugebauer <rolf.neugebauer@netronome.com>
+ *          Brad Petrus <brad.petrus@netronome.com>
+ */
+
+#include <linux/version.h>
+#include <linux/kernel.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/interrupt.h>
+#include <linux/pci.h>
+#include <linux/ethtool.h>
+
+#include "nfp_net_ctrl.h"
+#include "nfp_net.h"
+
+/* Support for stats. Returns netdev, driver, and device stats */
+enum { NETDEV_ET_STATS, NFP_NET_DRV_ET_STATS, NFP_NET_DEV_ET_STATS };
+struct _nfp_net_et_stats {
+	char name[ETH_GSTRING_LEN];
+	int type;
+	int sz;
+	int off;
+};
+
+#define NN_ET_NETDEV_STAT(m) NETDEV_ET_STATS,			\
+		FIELD_SIZEOF(struct net_device_stats, m),	\
+		offsetof(struct net_device_stats, m)
+/* For stats in the control BAR (other than Q stats) */
+#define NN_ET_DEV_STAT(m) NFP_NET_DEV_ET_STATS,			\
+		sizeof(u64),					\
+		(m)
+static const struct _nfp_net_et_stats nfp_net_et_stats[] = {
+	/* netdev stats */
+	{"rx_packets", NN_ET_NETDEV_STAT(rx_packets)},
+	{"tx_packets", NN_ET_NETDEV_STAT(tx_packets)},
+	{"rx_bytes", NN_ET_NETDEV_STAT(rx_bytes)},
+	{"tx_bytes", NN_ET_NETDEV_STAT(tx_bytes)},
+	{"rx_errors", NN_ET_NETDEV_STAT(rx_errors)},
+	{"tx_errors", NN_ET_NETDEV_STAT(tx_errors)},
+	{"rx_dropped", NN_ET_NETDEV_STAT(rx_dropped)},
+	{"tx_dropped", NN_ET_NETDEV_STAT(tx_dropped)},
+	{"multicast", NN_ET_NETDEV_STAT(multicast)},
+	{"collisions", NN_ET_NETDEV_STAT(collisions)},
+	{"rx_over_errors", NN_ET_NETDEV_STAT(rx_over_errors)},
+	{"rx_crc_errors", NN_ET_NETDEV_STAT(rx_crc_errors)},
+	{"rx_frame_errors", NN_ET_NETDEV_STAT(rx_frame_errors)},
+	{"rx_fifo_errors", NN_ET_NETDEV_STAT(rx_fifo_errors)},
+	{"rx_missed_errors", NN_ET_NETDEV_STAT(rx_missed_errors)},
+	{"tx_aborted_errors", NN_ET_NETDEV_STAT(tx_aborted_errors)},
+	{"tx_carrier_errors", NN_ET_NETDEV_STAT(tx_carrier_errors)},
+	{"tx_fifo_errors", NN_ET_NETDEV_STAT(tx_fifo_errors)},
+	/* Stats from the device */
+	{"dev_rx_discards", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_DISCARDS)},
+	{"dev_rx_errors", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_ERRORS)},
+	{"dev_rx_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_OCTETS)},
+	{"dev_rx_uc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_UC_OCTETS)},
+	{"dev_rx_mc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_MC_OCTETS)},
+	{"dev_rx_bc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_BC_OCTETS)},
+	{"dev_rx_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_FRAMES)},
+	{"dev_rx_mc_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_MC_FRAMES)},
+	{"dev_rx_bc_pkts", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_RX_BC_FRAMES)},
+
+	{"dev_tx_discards", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_DISCARDS)},
+	{"dev_tx_errors", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_ERRORS)},
+	{"dev_tx_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_OCTETS)},
+	{"dev_tx_uc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_UC_OCTETS)},
+	{"dev_tx_mc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_MC_OCTETS)},
+	{"dev_tx_bc_bytes", NN_ET_DEV_STAT(NFP_NET_CFG_STATS_TX_BC_OCTETS)},
+	{"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)},
+};
+
+#define NN_ET_GLOBAL_STATS_LEN ARRAY_SIZE(nfp_net_et_stats)
+#define NN_ET_RVEC_STATS_LEN (nn->num_r_vecs * 3)
+#define NN_ET_RVEC_GATHER_STATS 7
+#define NN_ET_QUEUE_STATS_LEN ((nn->num_tx_rings + nn->num_rx_rings) * 2)
+#define NN_ET_STATS_LEN (NN_ET_GLOBAL_STATS_LEN + NN_ET_RVEC_GATHER_STATS + \
+			 NN_ET_RVEC_STATS_LEN + NN_ET_QUEUE_STATS_LEN)
+
+static void nfp_net_get_drvinfo(struct net_device *netdev,
+				struct ethtool_drvinfo *drvinfo)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	strlcpy(drvinfo->driver, nfp_net_driver_name, sizeof(drvinfo->driver));
+	strlcpy(drvinfo->version, nfp_net_driver_version,
+		sizeof(drvinfo->version));
+
+	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version),
+		 "%d.%d.%d.%d",
+		 nn->fw_ver.resv, nn->fw_ver.class,
+		 nn->fw_ver.major, nn->fw_ver.minor);
+	strlcpy(drvinfo->bus_info, pci_name(nn->pdev),
+		sizeof(drvinfo->bus_info));
+
+	drvinfo->n_stats = NN_ET_STATS_LEN;
+	drvinfo->regdump_len = NFP_NET_CFG_BAR_SZ;
+}
+
+static void nfp_net_get_ringparam(struct net_device *netdev,
+				  struct ethtool_ringparam *ring)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	ring->rx_max_pending = NFP_NET_MAX_RX_DESCS;
+	ring->tx_max_pending = NFP_NET_MAX_TX_DESCS;
+	ring->rx_pending = nn->rxd_cnt;
+	ring->tx_pending = nn->txd_cnt;
+}
+
+static int nfp_net_set_ringparam(struct net_device *netdev,
+				 struct ethtool_ringparam *ring)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	u32 rxd_cnt, txd_cnt;
+
+	if (netif_running(netdev)) {
+		/* Some NIC drivers allow reconfiguration on the fly,
+		 * some down the interface, change and then up it
+		 * again.  For now we don't allow changes when the
+		 * device is up.
+		 */
+		nn_warn(nn, "Can't change rings while device is up\n");
+		return -EBUSY;
+	}
+
+	/* We don't have separate queues/rings for small/large frames. */
+	if (ring->rx_mini_pending || ring->rx_jumbo_pending)
+		return -EINVAL;
+
+	/* Round up to supported values */
+	rxd_cnt = roundup_pow_of_two(ring->rx_pending);
+	rxd_cnt = max_t(u32, rxd_cnt, NFP_NET_MIN_RX_DESCS);
+	rxd_cnt = min_t(u32, rxd_cnt, NFP_NET_MAX_RX_DESCS);
+
+	txd_cnt = roundup_pow_of_two(ring->tx_pending);
+	txd_cnt = max_t(u32, txd_cnt, NFP_NET_MIN_TX_DESCS);
+	txd_cnt = min_t(u32, txd_cnt, NFP_NET_MAX_TX_DESCS);
+
+	if (nn->rxd_cnt != rxd_cnt || nn->txd_cnt != txd_cnt)
+		nn_dbg(nn, "Change ring size: RxQ %u->%u, TxQ %u->%u\n",
+		       nn->rxd_cnt, rxd_cnt, nn->txd_cnt, txd_cnt);
+
+	nn->rxd_cnt = rxd_cnt;
+	nn->txd_cnt = txd_cnt;
+
+	return 0;
+}
+
+static void nfp_net_get_strings(struct net_device *netdev,
+				u32 stringset, u8 *data)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	u8 *p = data;
+	int i;
+
+	switch (stringset) {
+	case ETH_SS_STATS:
+		for (i = 0; i < NN_ET_GLOBAL_STATS_LEN; i++) {
+			memcpy(p, nfp_net_et_stats[i].name, ETH_GSTRING_LEN);
+			p += ETH_GSTRING_LEN;
+		}
+		for (i = 0; i < nn->num_r_vecs; i++) {
+			sprintf(p, "rvec_%u_rx_pkts", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "rvec_%u_tx_pkts", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "rvec_%u_tx_busy", i);
+			p += ETH_GSTRING_LEN;
+		}
+		strncpy(p, "hw_rx_csum_ok", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "hw_rx_csum_inner_ok", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "hw_rx_csum_err", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "hw_tx_csum", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "hw_tx_inner_csum", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "tx_gather", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		strncpy(p, "tx_lso", ETH_GSTRING_LEN);
+		p += ETH_GSTRING_LEN;
+		for (i = 0; i < nn->num_tx_rings; i++) {
+			sprintf(p, "txq_%u_pkts", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "txq_%u_bytes", i);
+			p += ETH_GSTRING_LEN;
+		}
+		for (i = 0; i < nn->num_rx_rings; i++) {
+			sprintf(p, "rxq_%u_pkts", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "rxq_%u_bytes", i);
+			p += ETH_GSTRING_LEN;
+		}
+		break;
+	}
+}
+
+static void nfp_net_get_stats(struct net_device *netdev,
+			      struct ethtool_stats *stats, u64 *data)
+{
+	u64 gathered_stats[NN_ET_RVEC_GATHER_STATS] = {};
+	struct nfp_net *nn = netdev_priv(netdev);
+	struct rtnl_link_stats64 *netdev_stats;
+	struct rtnl_link_stats64 temp = {};
+	u64 tmp[NN_ET_RVEC_GATHER_STATS];
+	u8 __iomem *io_p;
+	int i, j, k;
+	u8 *p;
+
+	netdev_stats = dev_get_stats(netdev, &temp);
+
+	for (i = 0; i < NN_ET_GLOBAL_STATS_LEN; i++) {
+		switch (nfp_net_et_stats[i].type) {
+		case NETDEV_ET_STATS:
+			p = (char *)netdev_stats + nfp_net_et_stats[i].off;
+			data[i] = nfp_net_et_stats[i].sz == sizeof(u64) ?
+				*(u64 *)p : *(u32 *)p;
+			break;
+
+		case NFP_NET_DEV_ET_STATS:
+			io_p = nn->ctrl_bar + nfp_net_et_stats[i].off;
+			data[i] = readq(io_p);
+			break;
+		}
+	}
+	for (j = 0; j < nn->num_r_vecs; j++) {
+		unsigned int start;
+
+		do {
+			start = u64_stats_fetch_begin(&nn->r_vecs[j].rx_sync);
+			data[i++] = nn->r_vecs[j].rx_pkts;
+			tmp[0] = nn->r_vecs[j].hw_csum_rx_ok;
+			tmp[1] = nn->r_vecs[j].hw_csum_rx_inner_ok;
+			tmp[2] = nn->r_vecs[j].hw_csum_rx_error;
+		} while (u64_stats_fetch_retry(&nn->r_vecs[j].rx_sync, start));
+
+		do {
+			start = u64_stats_fetch_begin(&nn->r_vecs[j].tx_sync);
+			data[i++] = nn->r_vecs[j].tx_pkts;
+			data[i++] = nn->r_vecs[j].tx_busy;
+			tmp[3] = nn->r_vecs[j].hw_csum_tx;
+			tmp[4] = nn->r_vecs[j].hw_csum_tx_inner;
+			tmp[5] = nn->r_vecs[j].tx_gather;
+			tmp[6] = nn->r_vecs[j].tx_lso;
+		} while (u64_stats_fetch_retry(&nn->r_vecs[j].tx_sync, start));
+
+		for (k = 0; k < NN_ET_RVEC_GATHER_STATS; k++)
+			gathered_stats[k] += tmp[k];
+	}
+	for (j = 0; j < NN_ET_RVEC_GATHER_STATS; j++)
+		data[i++] = gathered_stats[j];
+	for (j = 0; j < nn->num_tx_rings; j++) {
+		io_p = nn->ctrl_bar + NFP_NET_CFG_TXR_STATS(j);
+		data[i++] = readq(io_p);
+		io_p = nn->ctrl_bar + NFP_NET_CFG_TXR_STATS(j) + 8;
+		data[i++] = readq(io_p);
+	}
+	for (j = 0; j < nn->num_rx_rings; j++) {
+		io_p = nn->ctrl_bar + NFP_NET_CFG_RXR_STATS(j);
+		data[i++] = readq(io_p);
+		io_p = nn->ctrl_bar + NFP_NET_CFG_RXR_STATS(j) + 8;
+		data[i++] = readq(io_p);
+	}
+}
+
+static int nfp_net_get_sset_count(struct net_device *netdev, int sset)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	switch (sset) {
+	case ETH_SS_STATS:
+		return NN_ET_STATS_LEN;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+/* RX network flow classification (RSS, filters, etc)
+ */
+static u32 ethtool_flow_to_nfp_flag(u32 flow_type)
+{
+	static const u32 xlate_ethtool_to_nfp[IPV6_FLOW + 1] = {
+		[TCP_V4_FLOW]	= NFP_NET_CFG_RSS_IPV4_TCP,
+		[TCP_V6_FLOW]	= NFP_NET_CFG_RSS_IPV6_TCP,
+		[UDP_V4_FLOW]	= NFP_NET_CFG_RSS_IPV4_UDP,
+		[UDP_V6_FLOW]	= NFP_NET_CFG_RSS_IPV6_UDP,
+		[IPV4_FLOW]	= NFP_NET_CFG_RSS_IPV4,
+		[IPV6_FLOW]	= NFP_NET_CFG_RSS_IPV6,
+	};
+
+	if (flow_type >= ARRAY_SIZE(xlate_ethtool_to_nfp))
+		return 0;
+
+	return xlate_ethtool_to_nfp[flow_type];
+}
+
+static int nfp_net_get_rss_hash_opts(struct nfp_net *nn,
+				     struct ethtool_rxnfc *cmd)
+{
+	u32 nfp_rss_flag;
+
+	cmd->data = 0;
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+		return -EOPNOTSUPP;
+
+	nfp_rss_flag = ethtool_flow_to_nfp_flag(cmd->flow_type);
+	if (!nfp_rss_flag)
+		return -EINVAL;
+
+	cmd->data |= RXH_IP_SRC | RXH_IP_DST;
+	if (nn->rss_cfg & nfp_rss_flag)
+		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+
+	return 0;
+}
+
+static int nfp_net_get_rxnfc(struct net_device *netdev,
+			     struct ethtool_rxnfc *cmd, u32 *rule_locs)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXRINGS:
+		cmd->data = nn->num_rx_rings;
+		return 0;
+	case ETHTOOL_GRXFH:
+		return nfp_net_get_rss_hash_opts(nn, cmd);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static int nfp_net_set_rss_hash_opt(struct nfp_net *nn,
+				    struct ethtool_rxnfc *nfc)
+{
+	u32 new_rss_cfg = nn->rss_cfg;
+	u32 nfp_rss_flag;
+	int err;
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+		return -EOPNOTSUPP;
+
+	/* RSS only supports IP SA/DA and L4 src/dst ports  */
+	if (nfc->data & ~(RXH_IP_SRC | RXH_IP_DST |
+			  RXH_L4_B_0_1 | RXH_L4_B_2_3))
+		return -EINVAL;
+
+	/* We need at least the IP SA/DA fields for hashing */
+	if (!(nfc->data & RXH_IP_SRC) ||
+	    !(nfc->data & RXH_IP_DST))
+		return -EINVAL;
+
+	nfp_rss_flag = ethtool_flow_to_nfp_flag(nfc->flow_type);
+	if (!nfp_rss_flag)
+		return -EINVAL;
+
+	switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+	case 0:
+		new_rss_cfg &= ~nfp_rss_flag;
+		break;
+	case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+		new_rss_cfg |= nfp_rss_flag;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	new_rss_cfg |= NFP_NET_CFG_RSS_TOEPLITZ;
+	new_rss_cfg |= NFP_NET_CFG_RSS_MASK;
+
+	if (new_rss_cfg == nn->rss_cfg)
+		return 0;
+
+	writel(new_rss_cfg, nn->ctrl_bar + NFP_NET_CFG_RSS_CTRL);
+	err = nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_RSS);
+	if (err)
+		return err;
+
+	nn->rss_cfg = new_rss_cfg;
+
+	nn_dbg(nn, "Changed RSS config to 0x%x\n", nn->rss_cfg);
+	return 0;
+}
+
+static int nfp_net_set_rxnfc(struct net_device *netdev,
+			     struct ethtool_rxnfc *cmd)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXFH:
+		return nfp_net_set_rss_hash_opt(nn, cmd);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static u32 nfp_net_get_rxfh_indir_size(struct net_device *netdev)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+		return 0;
+
+	return ARRAY_SIZE(nn->rss_itbl);
+}
+
+static u32 nfp_net_get_rxfh_key_size(struct net_device *netdev)
+{
+	return NFP_NET_CFG_RSS_KEY_SZ;
+}
+
+static int nfp_net_get_rxfh(struct net_device *netdev, u32 *indir, u8 *key,
+			    u8 *hfunc)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int i;
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS))
+		return -EOPNOTSUPP;
+
+	if (indir)
+		for (i = 0; i < ARRAY_SIZE(nn->rss_itbl); i++)
+			indir[i] = nn->rss_itbl[i];
+	if (key)
+		memcpy(key, nn->rss_key, NFP_NET_CFG_RSS_KEY_SZ);
+	if (hfunc)
+		*hfunc = ETH_RSS_HASH_TOP;
+
+	return 0;
+}
+
+static int nfp_net_set_rxfh(struct net_device *netdev,
+			    const u32 *indir, const u8 *key,
+			    const u8 hfunc)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	int i;
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_RSS) ||
+	    !(hfunc == ETH_RSS_HASH_NO_CHANGE || hfunc == ETH_RSS_HASH_TOP))
+		return -EOPNOTSUPP;
+
+	if (!key && !indir)
+		return 0;
+
+	if (key) {
+		memcpy(nn->rss_key, key, NFP_NET_CFG_RSS_KEY_SZ);
+		nfp_net_rss_write_key(nn);
+	}
+	if (indir) {
+		for (i = 0; i < ARRAY_SIZE(nn->rss_itbl); i++)
+			nn->rss_itbl[i] = indir[i];
+
+		nfp_net_rss_write_itbl(nn);
+	}
+
+	return nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_RSS);
+}
+
+/* Dump BAR registers
+ */
+static int nfp_net_get_regs_len(struct net_device *netdev)
+{
+	return NFP_NET_CFG_BAR_SZ;
+}
+
+static void nfp_net_get_regs(struct net_device *netdev,
+			     struct ethtool_regs *regs, void *p)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	u32 *regs_buf = p;
+	int i;
+
+	regs->version = nn_readl(nn, NFP_NET_CFG_VERSION);
+
+	for (i = 0; i < NFP_NET_CFG_BAR_SZ / sizeof(u32); i++)
+		regs_buf[i] = readl(nn->ctrl_bar + (i * sizeof(u32)));
+}
+
+static int nfp_net_get_coalesce(struct net_device *netdev,
+				struct ethtool_coalesce *ec)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_IRQMOD))
+		return -EINVAL;
+
+	ec->rx_coalesce_usecs       = nn->rx_coalesce_usecs;
+	ec->rx_max_coalesced_frames = nn->rx_coalesce_max_frames;
+	ec->tx_coalesce_usecs       = nn->tx_coalesce_usecs;
+	ec->tx_max_coalesced_frames = nn->tx_coalesce_max_frames;
+
+	return 0;
+}
+
+static int nfp_net_set_coalesce(struct net_device *netdev,
+				struct ethtool_coalesce *ec)
+{
+	struct nfp_net *nn = netdev_priv(netdev);
+	unsigned int factor;
+
+	if (ec->rx_coalesce_usecs_irq ||
+	    ec->rx_max_coalesced_frames_irq ||
+	    ec->tx_coalesce_usecs_irq ||
+	    ec->tx_max_coalesced_frames_irq ||
+	    ec->stats_block_coalesce_usecs ||
+	    ec->use_adaptive_rx_coalesce ||
+	    ec->use_adaptive_tx_coalesce ||
+	    ec->pkt_rate_low ||
+	    ec->rx_coalesce_usecs_low ||
+	    ec->rx_max_coalesced_frames_low ||
+	    ec->tx_coalesce_usecs_low ||
+	    ec->tx_max_coalesced_frames_low ||
+	    ec->pkt_rate_high ||
+	    ec->rx_coalesce_usecs_high ||
+	    ec->rx_max_coalesced_frames_high ||
+	    ec->tx_coalesce_usecs_high ||
+	    ec->tx_max_coalesced_frames_high ||
+	    ec->rate_sample_interval)
+		return -ENOTSUPP;
+
+	/* Compute factor used to convert coalesce '_usecs' parameters to
+	 * ME timestamp ticks.  There are 16 ME clock cycles for each timestamp
+	 * count.
+	 */
+	factor = nn->me_freq_mhz / 16;
+
+	/* Each pair of (usecs, max_frames) fields specifies that interrupts
+	 * should be coalesced until
+	 *      (usecs > 0 && time_since_first_completion >= usecs) ||
+	 *      (max_frames > 0 && completed_frames >= max_frames)
+	 *
+	 * It is illegal to set both usecs and max_frames to zero as this would
+	 * cause interrupts to never be generated.  To disable coalescing, set
+	 * usecs = 0 and max_frames = 1.
+	 *
+	 * Some implementations ignore the value of max_frames and use the
+	 * condition time_since_first_completion >= usecs
+	 */
+
+	if (!(nn->cap & NFP_NET_CFG_CTRL_IRQMOD))
+		return -EINVAL;
+
+	/* ensure valid configuration */
+	if (!ec->rx_coalesce_usecs && !ec->rx_max_coalesced_frames)
+		return -EINVAL;
+
+	if (!ec->tx_coalesce_usecs && !ec->tx_max_coalesced_frames)
+		return -EINVAL;
+
+	if (ec->rx_coalesce_usecs * factor >= ((1 << 16) - 1))
+		return -EINVAL;
+
+	if (ec->tx_coalesce_usecs * factor >= ((1 << 16) - 1))
+		return -EINVAL;
+
+	if (ec->rx_max_coalesced_frames >= ((1 << 16) - 1))
+		return -EINVAL;
+
+	if (ec->tx_max_coalesced_frames >= ((1 << 16) - 1))
+		return -EINVAL;
+
+	/* configuration is valid */
+	nn->rx_coalesce_usecs      = ec->rx_coalesce_usecs;
+	nn->rx_coalesce_max_frames = ec->rx_max_coalesced_frames;
+	nn->tx_coalesce_usecs      = ec->tx_coalesce_usecs;
+	nn->tx_coalesce_max_frames = ec->tx_max_coalesced_frames;
+
+	/* write configuration to device */
+	nfp_net_coalesce_write_cfg(nn);
+	return nfp_net_reconfig(nn, NFP_NET_CFG_UPDATE_IRQMOD);
+}
+
+static const struct ethtool_ops nfp_net_ethtool_ops = {
+	.get_drvinfo		= nfp_net_get_drvinfo,
+	.get_ringparam		= nfp_net_get_ringparam,
+	.set_ringparam		= nfp_net_set_ringparam,
+	.get_strings		= nfp_net_get_strings,
+	.get_ethtool_stats	= nfp_net_get_stats,
+	.get_sset_count		= nfp_net_get_sset_count,
+	.get_rxnfc		= nfp_net_get_rxnfc,
+	.set_rxnfc		= nfp_net_set_rxnfc,
+	.get_rxfh_indir_size	= nfp_net_get_rxfh_indir_size,
+	.get_rxfh_key_size	= nfp_net_get_rxfh_key_size,
+	.get_rxfh		= nfp_net_get_rxfh,
+	.set_rxfh		= nfp_net_set_rxfh,
+	.get_regs_len		= nfp_net_get_regs_len,
+	.get_regs		= nfp_net_get_regs,
+	.get_coalesce           = nfp_net_get_coalesce,
+	.set_coalesce           = nfp_net_set_coalesce,
+};
+
+void nfp_net_set_ethtool_ops(struct net_device *netdev)
+{
+	netdev->ethtool_ops = &nfp_net_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
new file mode 100644
index 000000000000..e2b22b8a20f1
--- /dev/null
+++ b/drivers/net/ethernet/netronome/nfp/nfp_netvf_main.c
@@ -0,0 +1,385 @@
+/*
+ * Copyright (C) 2015 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_netvf_main.c
+ * Netronome virtual function network device driver: Main entry point
+ * Author: Jason McMullan <jason.mcmullan@netronome.com>
+ *         Rolf Neugebauer <rolf.neugebauer@netronome.com>
+ */
+
+#include <linux/version.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/etherdevice.h>
+
+#include "nfp_net_ctrl.h"
+#include "nfp_net.h"
+
+const char nfp_net_driver_name[] = "nfp_netvf";
+const char nfp_net_driver_version[] = "0.1";
+#define PCI_DEVICE_NFP6000VF		0x6003
+static const struct pci_device_id nfp_netvf_pci_device_ids[] = {
+	{ PCI_VENDOR_ID_NETRONOME, PCI_DEVICE_NFP6000VF,
+	  PCI_VENDOR_ID_NETRONOME, PCI_ANY_ID,
+	  PCI_ANY_ID, 0,
+	},
+	{ 0, } /* Required last entry. */
+};
+MODULE_DEVICE_TABLE(pci, nfp_netvf_pci_device_ids);
+
+static void nfp_netvf_get_mac_addr(struct nfp_net *nn)
+{
+	u8 mac_addr[ETH_ALEN];
+
+	put_unaligned_be32(nn_readl(nn, NFP_NET_CFG_MACADDR + 0), &mac_addr[0]);
+	/* We can't do readw for NFP-3200 compatibility */
+	put_unaligned_be16(nn_readl(nn, NFP_NET_CFG_MACADDR + 4) >> 16,
+			   &mac_addr[4]);
+
+	if (!is_valid_ether_addr(mac_addr)) {
+		eth_hw_addr_random(nn->netdev);
+		return;
+	}
+
+	ether_addr_copy(nn->netdev->dev_addr, mac_addr);
+	ether_addr_copy(nn->netdev->perm_addr, mac_addr);
+}
+
+static int nfp_netvf_pci_probe(struct pci_dev *pdev,
+			       const struct pci_device_id *pci_id)
+{
+	struct nfp_net_fw_version fw_ver;
+	int max_tx_rings, max_rx_rings;
+	u32 tx_bar_off, rx_bar_off;
+	u32 tx_bar_sz, rx_bar_sz;
+	int tx_bar_no, rx_bar_no;
+	u8 __iomem *ctrl_bar;
+	struct nfp_net *nn;
+	int is_nfp3200;
+	u32 startq;
+	int stride;
+	int err;
+
+	err = pci_enable_device_mem(pdev);
+	if (err)
+		return err;
+
+	err = pci_request_regions(pdev, nfp_net_driver_name);
+	if (err) {
+		dev_err(&pdev->dev, "Unable to allocate device memory.\n");
+		goto err_pci_disable;
+	}
+
+	switch (pdev->device) {
+	case PCI_DEVICE_NFP6000VF:
+		is_nfp3200 = 0;
+		break;
+	default:
+		err = -ENODEV;
+		goto err_pci_regions;
+	}
+
+	pci_set_master(pdev);
+
+	err = dma_set_mask_and_coherent(&pdev->dev,
+					DMA_BIT_MASK(NFP_NET_MAX_DMA_BITS));
+	if (err)
+		goto err_pci_regions;
+
+	/* Map the Control BAR.
+	 *
+	 * Irrespective of the advertised BAR size we only map the
+	 * first NFP_NET_CFG_BAR_SZ of the BAR.  This keeps the code
+	 * the identical for PF and VF drivers.
+	 */
+	ctrl_bar = ioremap_nocache(pci_resource_start(pdev, NFP_NET_CRTL_BAR),
+				   NFP_NET_CFG_BAR_SZ);
+	if (!ctrl_bar) {
+		dev_err(&pdev->dev,
+			"Failed to map resource %d\n", NFP_NET_CRTL_BAR);
+		err = -EIO;
+		goto err_pci_regions;
+	}
+
+	nfp_net_get_fw_version(&fw_ver, ctrl_bar);
+	if (fw_ver.class != NFP_NET_CFG_VERSION_CLASS_GENERIC) {
+		dev_err(&pdev->dev, "Unknown Firmware ABI %d.%d.%d.%d\n",
+			fw_ver.resv, fw_ver.class, fw_ver.major, fw_ver.minor);
+		err = -EINVAL;
+		goto err_ctrl_unmap;
+	}
+
+	/* Determine stride */
+	if (nfp_net_fw_ver_eq(&fw_ver, 0, 0, 0, 0) ||
+	    nfp_net_fw_ver_eq(&fw_ver, 0, 0, 0, 1) ||
+	    nfp_net_fw_ver_eq(&fw_ver, 0, 0, 0x12, 0x48)) {
+		stride = 2;
+		tx_bar_no = NFP_NET_Q0_BAR;
+		rx_bar_no = NFP_NET_Q1_BAR;
+		dev_warn(&pdev->dev, "OBSOLETE Firmware detected - VF isolation not available\n");
+	} else {
+		switch (fw_ver.major) {
+		case 1 ... 3:
+			if (is_nfp3200) {
+				stride = 2;
+				tx_bar_no = NFP_NET_Q0_BAR;
+				rx_bar_no = NFP_NET_Q1_BAR;
+			} else {
+				stride = 4;
+				tx_bar_no = NFP_NET_Q0_BAR;
+				rx_bar_no = tx_bar_no;
+			}
+			break;
+		default:
+			dev_err(&pdev->dev, "Unsupported Firmware ABI %d.%d.%d.%d\n",
+				fw_ver.resv, fw_ver.class,
+				fw_ver.major, fw_ver.minor);
+			err = -EINVAL;
+			goto err_ctrl_unmap;
+		}
+	}
+
+	/* Find out how many rings are supported */
+	max_tx_rings = readl(ctrl_bar + NFP_NET_CFG_MAX_TXRINGS);
+	max_rx_rings = readl(ctrl_bar + NFP_NET_CFG_MAX_RXRINGS);
+
+	tx_bar_sz = NFP_QCP_QUEUE_ADDR_SZ * max_tx_rings * stride;
+	rx_bar_sz = NFP_QCP_QUEUE_ADDR_SZ * max_rx_rings * stride;
+
+	/* Sanity checks */
+	if (tx_bar_sz > pci_resource_len(pdev, tx_bar_no)) {
+		dev_err(&pdev->dev,
+			"TX BAR too small for number of TX rings. Adjusting\n");
+		tx_bar_sz = pci_resource_len(pdev, tx_bar_no);
+		max_tx_rings = (tx_bar_sz / NFP_QCP_QUEUE_ADDR_SZ) / 2;
+	}
+	if (rx_bar_sz > pci_resource_len(pdev, rx_bar_no)) {
+		dev_err(&pdev->dev,
+			"RX BAR too small for number of RX rings. Adjusting\n");
+		rx_bar_sz = pci_resource_len(pdev, rx_bar_no);
+		max_rx_rings = (rx_bar_sz / NFP_QCP_QUEUE_ADDR_SZ) / 2;
+	}
+
+	/* XXX Implement a workaround for THB-350 here.  Ideally, we
+	 * have a different PCI ID for A rev VFs.
+	 */
+	switch (pdev->device) {
+	case PCI_DEVICE_NFP6000VF:
+		startq = readl(ctrl_bar + NFP_NET_CFG_START_TXQ);
+		tx_bar_off = NFP_PCIE_QUEUE(startq);
+		startq = readl(ctrl_bar + NFP_NET_CFG_START_RXQ);
+		rx_bar_off = NFP_PCIE_QUEUE(startq);
+		break;
+	default:
+		err = -ENODEV;
+		goto err_ctrl_unmap;
+	}
+
+	/* Allocate and initialise the netdev */
+	nn = nfp_net_netdev_alloc(pdev, max_tx_rings, max_rx_rings);
+	if (IS_ERR(nn)) {
+		err = PTR_ERR(nn);
+		goto err_ctrl_unmap;
+	}
+
+	nn->fw_ver = fw_ver;
+	nn->ctrl_bar = ctrl_bar;
+	nn->is_vf = 1;
+	nn->is_nfp3200 = is_nfp3200;
+	nn->stride_tx = stride;
+	nn->stride_rx = stride;
+
+	if (rx_bar_no == tx_bar_no) {
+		u32 bar_off, bar_sz;
+		resource_size_t map_addr;
+
+		/* Make a single overlapping BAR mapping */
+		if (tx_bar_off < rx_bar_off)
+			bar_off = tx_bar_off;
+		else
+			bar_off = rx_bar_off;
+
+		if ((tx_bar_off + tx_bar_sz) > (rx_bar_off + rx_bar_sz))
+			bar_sz = (tx_bar_off + tx_bar_sz) - bar_off;
+		else
+			bar_sz = (rx_bar_off + rx_bar_sz) - bar_off;
+
+		map_addr = pci_resource_start(pdev, tx_bar_no) + bar_off;
+		nn->q_bar = ioremap_nocache(map_addr, bar_sz);
+		if (!nn->q_bar) {
+			nn_err(nn, "Failed to map resource %d\n", tx_bar_no);
+			err = -EIO;
+			goto err_netdev_free;
+		}
+
+		/* TX queues */
+		nn->tx_bar = nn->q_bar + (tx_bar_off - bar_off);
+		/* RX queues */
+		nn->rx_bar = nn->q_bar + (rx_bar_off - bar_off);
+	} else {
+		resource_size_t map_addr;
+
+		/* TX queues */
+		map_addr = pci_resource_start(pdev, tx_bar_no) + tx_bar_off;
+		nn->tx_bar = ioremap_nocache(map_addr, tx_bar_sz);
+		if (!nn->tx_bar) {
+			nn_err(nn, "Failed to map resource %d\n", tx_bar_no);
+			err = -EIO;
+			goto err_netdev_free;
+		}
+
+		/* RX queues */
+		map_addr = pci_resource_start(pdev, rx_bar_no) + rx_bar_off;
+		nn->rx_bar = ioremap_nocache(map_addr, rx_bar_sz);
+		if (!nn->rx_bar) {
+			nn_err(nn, "Failed to map resource %d\n", rx_bar_no);
+			err = -EIO;
+			goto err_unmap_tx;
+		}
+	}
+
+	nfp_netvf_get_mac_addr(nn);
+
+	err = nfp_net_irqs_alloc(nn);
+	if (!err) {
+		nn_warn(nn, "Unable to allocate MSI-X Vectors. Exiting\n");
+		err = -EIO;
+		goto err_unmap_rx;
+	}
+
+	/* Get ME clock frequency from ctrl BAR
+	 * XXX for now frequency is hardcoded until we figure out how
+	 * to get the value from nfp-hwinfo into ctrl bar
+	 */
+	nn->me_freq_mhz = 1200;
+
+	err = nfp_net_netdev_init(nn->netdev);
+	if (err)
+		goto err_irqs_disable;
+
+	pci_set_drvdata(pdev, nn);
+
+	nfp_net_info(nn);
+	nfp_net_debugfs_adapter_add(nn);
+
+	return 0;
+
+err_irqs_disable:
+	nfp_net_irqs_disable(nn);
+err_unmap_rx:
+	if (!nn->q_bar)
+		iounmap(nn->rx_bar);
+err_unmap_tx:
+	if (!nn->q_bar)
+		iounmap(nn->tx_bar);
+	else
+		iounmap(nn->q_bar);
+err_netdev_free:
+	pci_set_drvdata(pdev, NULL);
+	nfp_net_netdev_free(nn);
+err_ctrl_unmap:
+	iounmap(ctrl_bar);
+err_pci_regions:
+	pci_release_regions(pdev);
+err_pci_disable:
+	pci_disable_device(pdev);
+	return err;
+}
+
+static void nfp_netvf_pci_remove(struct pci_dev *pdev)
+{
+	struct nfp_net *nn = pci_get_drvdata(pdev);
+
+	/* Note, the order is slightly different from above as we need
+	 * to keep the nn pointer around till we have freed everything.
+	 */
+	nfp_net_debugfs_adapter_del(nn);
+
+	nfp_net_netdev_clean(nn->netdev);
+
+	nfp_net_irqs_disable(nn);
+
+	if (!nn->q_bar) {
+		iounmap(nn->rx_bar);
+		iounmap(nn->tx_bar);
+	} else {
+		iounmap(nn->q_bar);
+	}
+	iounmap(nn->ctrl_bar);
+
+	pci_set_drvdata(pdev, NULL);
+
+	nfp_net_netdev_free(nn);
+
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+}
+
+static struct pci_driver nfp_netvf_pci_driver = {
+	.name        = nfp_net_driver_name,
+	.id_table    = nfp_netvf_pci_device_ids,
+	.probe       = nfp_netvf_pci_probe,
+	.remove      = nfp_netvf_pci_remove,
+};
+
+static int __init nfp_netvf_init(void)
+{
+	int err;
+
+	pr_info("%s: NFP VF Network driver, Copyright (C) 2014-2015 Netronome Systems\n",
+		nfp_net_driver_name);
+
+	nfp_net_debugfs_create();
+	err = pci_register_driver(&nfp_netvf_pci_driver);
+	if (err) {
+		nfp_net_debugfs_destroy();
+		return err;
+	}
+
+	return 0;
+}
+
+static void __exit nfp_netvf_exit(void)
+{
+	pci_unregister_driver(&nfp_netvf_pci_driver);
+	nfp_net_debugfs_destroy();
+}
+
+module_init(nfp_netvf_init);
+module_exit(nfp_netvf_exit);
+
+MODULE_AUTHOR("Netronome Systems <oss-drivers@netronome.com>");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("NFP VF network device driver");
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 057665180f13..b1ce7aaa8f8b 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -797,7 +797,7 @@ static int lpc_mii_probe(struct net_device *ndev)
 		netdev_info(ndev, "using MII interface\n");
 	else
 		netdev_info(ndev, "using RMII interface\n");
-	phydev = phy_connect(ndev, dev_name(&phydev->dev),
+	phydev = phy_connect(ndev, phydev_name(phydev),
 			     &lpc_handle_link_change,
 			     lpc_phy_interface_mode(&pldat->pdev->dev));
 
@@ -816,15 +816,14 @@ static int lpc_mii_probe(struct net_device *ndev)
 	pldat->duplex = -1;
 	pldat->phy_dev = phydev;
 
-	netdev_info(ndev,
-		"attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
+
 	return 0;
 }
 
 static int lpc_mii_init(struct netdata_local *pldat)
 {
-	int err = -ENXIO, i;
+	int err = -ENXIO;
 
 	pldat->mii_bus = mdiobus_alloc();
 	if (!pldat->mii_bus) {
@@ -851,19 +850,10 @@ static int lpc_mii_init(struct netdata_local *pldat)
 	pldat->mii_bus->priv = pldat;
 	pldat->mii_bus->parent = &pldat->pdev->dev;
 
-	pldat->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!pldat->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_1;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		pldat->mii_bus->irq[i] = PHY_POLL;
-
 	platform_set_drvdata(pldat->pdev, pldat->mii_bus);
 
 	if (mdiobus_register(pldat->mii_bus))
-		goto err_out_free_mdio_irq;
+		goto err_out_unregister_bus;
 
 	if (lpc_mii_probe(pldat->ndev) != 0)
 		goto err_out_unregister_bus;
@@ -872,9 +862,6 @@ static int lpc_mii_init(struct netdata_local *pldat)
 
 err_out_unregister_bus:
 	mdiobus_unregister(pldat->mii_bus);
-err_out_free_mdio_irq:
-	kfree(pldat->mii_bus->irq);
-err_out_1:
 	mdiobus_free(pldat->mii_bus);
 err_out:
 	return err;
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
index 08d4be616064..e097e6baaac4 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/pch_gbe_param.c
@@ -500,7 +500,7 @@ void pch_gbe_check_options(struct pch_gbe_adapter *adapter)
 		val = XsumTX;
 		pch_gbe_validate_option(&val, &opt, adapter);
 		if (!val)
-			dev->features &= ~NETIF_F_ALL_CSUM;
+			dev->features &= ~NETIF_F_CSUM_MASK;
 	}
 	{ /* Flow Control */
 		static const struct pch_gbe_option opt = {
diff --git a/drivers/net/ethernet/qlogic/qed/qed_hsi.h b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
index b2f8e854dfd1..264e954675d1 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_hsi.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_hsi.h
@@ -3993,6 +3993,8 @@ struct public_drv_mb {
 #define DRV_MSG_CODE_PHY_CORE_WRITE             0x000e0000
 #define DRV_MSG_CODE_SET_VERSION                0x000f0000
 
+#define DRV_MSG_CODE_SET_LED_MODE               0x00200000
+
 #define DRV_MSG_SEQ_NUMBER_MASK                 0x0000ffff
 
 	u32 drv_mb_param;
@@ -4044,6 +4046,10 @@ struct public_drv_mb {
 #define DRV_MB_PARAM_CFG_VF_MSIX_SB_NUM_SHIFT   8
 #define DRV_MB_PARAM_CFG_VF_MSIX_SB_NUM_MASK    0x0000FF00
 
+#define DRV_MB_PARAM_SET_LED_MODE_OPER          0x0
+#define DRV_MB_PARAM_SET_LED_MODE_ON            0x1
+#define DRV_MB_PARAM_SET_LED_MODE_OFF           0x2
+
 	u32 fw_mb_header;
 #define FW_MSG_CODE_MASK                        0xffff0000
 #define FW_MSG_CODE_DRV_LOAD_ENGINE             0x10100000
diff --git a/drivers/net/ethernet/qlogic/qed/qed_main.c b/drivers/net/ethernet/qlogic/qed/qed_main.c
index 174f7341c5c3..9d76ce249277 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_main.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_main.c
@@ -1115,6 +1115,23 @@ static int qed_drain(struct qed_dev *cdev)
 	return 0;
 }
 
+static int qed_set_led(struct qed_dev *cdev, enum qed_led_mode mode)
+{
+	struct qed_hwfn *hwfn = QED_LEADING_HWFN(cdev);
+	struct qed_ptt *ptt;
+	int status = 0;
+
+	ptt = qed_ptt_acquire(hwfn);
+	if (!ptt)
+		return -EAGAIN;
+
+	status = qed_mcp_set_led(hwfn, ptt, mode);
+
+	qed_ptt_release(hwfn, ptt);
+
+	return status;
+}
+
 const struct qed_common_ops qed_common_ops_pass = {
 	.probe = &qed_probe,
 	.remove = &qed_remove,
@@ -1135,6 +1152,7 @@ const struct qed_common_ops qed_common_ops_pass = {
 	.update_msglvl = &qed_init_dp,
 	.chain_alloc = &qed_chain_alloc,
 	.chain_free = &qed_chain_free,
+	.set_led = &qed_set_led,
 };
 
 u32 qed_get_protocol_version(enum qed_protocol protocol)
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.c b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
index 20d048cdcb88..ba1b1f1ef789 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.c
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.c
@@ -858,3 +858,30 @@ qed_mcp_send_drv_version(struct qed_hwfn *p_hwfn,
 
 	return 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;
+
+	switch (mode) {
+	case QED_LED_MODE_ON:
+		drv_mb_param = DRV_MB_PARAM_SET_LED_MODE_ON;
+		break;
+	case QED_LED_MODE_OFF:
+		drv_mb_param = DRV_MB_PARAM_SET_LED_MODE_OFF;
+		break;
+	case QED_LED_MODE_RESTORE:
+		drv_mb_param = DRV_MB_PARAM_SET_LED_MODE_OPER;
+		break;
+	default:
+		DP_NOTICE(p_hwfn, "Invalid LED mode %d\n", mode);
+		return -EINVAL;
+	}
+
+	rc = qed_mcp_cmd(p_hwfn, p_ptt, DRV_MSG_CODE_SET_LED_MODE,
+			 drv_mb_param, &resp, &param);
+
+	return rc;
+}
diff --git a/drivers/net/ethernet/qlogic/qed/qed_mcp.h b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
index dbaae586b4a7..506197d5c3dd 100644
--- a/drivers/net/ethernet/qlogic/qed/qed_mcp.h
+++ b/drivers/net/ethernet/qlogic/qed/qed_mcp.h
@@ -224,6 +224,19 @@ qed_mcp_send_drv_version(struct qed_hwfn *p_hwfn,
 			 struct qed_ptt *p_ptt,
 			 struct qed_mcp_drv_version *p_ver);
 
+/**
+ * @brief Set LED status
+ *
+ *  @param p_hwfn
+ *  @param p_ptt
+ *  @param mode - LED mode
+ *
+ * @return int - 0 - operation was successful.
+ */
+int qed_mcp_set_led(struct qed_hwfn *p_hwfn,
+		    struct qed_ptt *p_ptt,
+		    enum qed_led_mode mode);
+
 /* Using hwfn number (and not pf_num) is required since in CMT mode,
  * same pf_num may be used by two different hwfn
  * TODO - this shouldn't really be in .h file, but until all fields
diff --git a/drivers/net/ethernet/qlogic/qede/qede.h b/drivers/net/ethernet/qlogic/qede/qede.h
index ea00d5f3bab4..7c6caf7f6612 100644
--- a/drivers/net/ethernet/qlogic/qede/qede.h
+++ b/drivers/net/ethernet/qlogic/qede/qede.h
@@ -116,6 +116,7 @@ struct qede_dev {
 				 (edev)->dev_info.num_tc)
 
 	struct qede_fastpath		*fp_array;
+	u16				req_rss;
 	u16				num_rss;
 	u8				num_tc;
 #define QEDE_RSS_CNT(edev)		((edev)->num_rss)
@@ -269,13 +270,13 @@ int qede_change_mtu(struct net_device *dev, int new_mtu);
 void qede_fill_by_demand_stats(struct qede_dev *edev);
 
 #define RX_RING_SIZE_POW	13
-#define RX_RING_SIZE		BIT(RX_RING_SIZE_POW)
+#define RX_RING_SIZE		((u16)BIT(RX_RING_SIZE_POW))
 #define NUM_RX_BDS_MAX		(RX_RING_SIZE - 1)
 #define NUM_RX_BDS_MIN		128
 #define NUM_RX_BDS_DEF		NUM_RX_BDS_MAX
 
 #define TX_RING_SIZE_POW	13
-#define TX_RING_SIZE		BIT(TX_RING_SIZE_POW)
+#define TX_RING_SIZE		((u16)BIT(TX_RING_SIZE_POW))
 #define NUM_TX_BDS_MAX		(TX_RING_SIZE - 1)
 #define NUM_TX_BDS_MIN		128
 #define NUM_TX_BDS_DEF		NUM_TX_BDS_MAX
diff --git a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
index 3a362476a22c..e442b85c9a5e 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_ethtool.c
@@ -322,6 +322,30 @@ static void qede_set_msglevel(struct net_device *ndev, u32 level)
 					 dp_module, dp_level);
 }
 
+static int qede_nway_reset(struct net_device *dev)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+	struct qed_link_output current_link;
+	struct qed_link_params link_params;
+
+	if (!netif_running(dev))
+		return 0;
+
+	memset(&current_link, 0, sizeof(current_link));
+	edev->ops->common->get_link(edev->cdev, &current_link);
+	if (!current_link.link_up)
+		return 0;
+
+	/* Toggle the link */
+	memset(&link_params, 0, sizeof(link_params));
+	link_params.link_up = false;
+	edev->ops->common->set_link(edev->cdev, &link_params);
+	link_params.link_up = true;
+	edev->ops->common->set_link(edev->cdev, &link_params);
+
+	return 0;
+}
+
 static u32 qede_get_link(struct net_device *dev)
 {
 	struct qede_dev *edev = netdev_priv(dev);
@@ -333,6 +357,106 @@ static u32 qede_get_link(struct net_device *dev)
 	return current_link.link_up;
 }
 
+static void qede_get_ringparam(struct net_device *dev,
+			       struct ethtool_ringparam *ering)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+
+	ering->rx_max_pending = NUM_RX_BDS_MAX;
+	ering->rx_pending = edev->q_num_rx_buffers;
+	ering->tx_max_pending = NUM_TX_BDS_MAX;
+	ering->tx_pending = edev->q_num_tx_buffers;
+}
+
+static int qede_set_ringparam(struct net_device *dev,
+			      struct ethtool_ringparam *ering)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+
+	DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+		   "Set ring params command parameters: rx_pending = %d, tx_pending = %d\n",
+		   ering->rx_pending, ering->tx_pending);
+
+	/* Validate legality of configuration */
+	if (ering->rx_pending > NUM_RX_BDS_MAX ||
+	    ering->rx_pending < NUM_RX_BDS_MIN ||
+	    ering->tx_pending > NUM_TX_BDS_MAX ||
+	    ering->tx_pending < NUM_TX_BDS_MIN) {
+		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+			   "Can only support Rx Buffer size [0%08x,...,0x%08x] and Tx Buffer size [0x%08x,...,0x%08x]\n",
+			   NUM_RX_BDS_MIN, NUM_RX_BDS_MAX,
+			   NUM_TX_BDS_MIN, NUM_TX_BDS_MAX);
+		return -EINVAL;
+	}
+
+	/* Change ring size and re-load */
+	edev->q_num_rx_buffers = ering->rx_pending;
+	edev->q_num_tx_buffers = ering->tx_pending;
+
+	if (netif_running(edev->ndev))
+		qede_reload(edev, NULL, NULL);
+
+	return 0;
+}
+
+static void qede_get_pauseparam(struct net_device *dev,
+				struct ethtool_pauseparam *epause)
+{
+	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);
+
+	if (current_link.pause_config & QED_LINK_PAUSE_AUTONEG_ENABLE)
+		epause->autoneg = true;
+	if (current_link.pause_config & QED_LINK_PAUSE_RX_ENABLE)
+		epause->rx_pause = true;
+	if (current_link.pause_config & QED_LINK_PAUSE_TX_ENABLE)
+		epause->tx_pause = true;
+
+	DP_VERBOSE(edev, QED_MSG_DEBUG,
+		   "ethtool_pauseparam: cmd %d  autoneg %d  rx_pause %d  tx_pause %d\n",
+		   epause->cmd, epause->autoneg, epause->rx_pause,
+		   epause->tx_pause);
+}
+
+static int qede_set_pauseparam(struct net_device *dev,
+			       struct ethtool_pauseparam *epause)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+	struct qed_link_params params;
+	struct qed_link_output current_link;
+
+	if (!edev->dev_info.common.is_mf) {
+		DP_INFO(edev,
+			"Pause parameters can not be updated in non-default mode\n");
+		return -EOPNOTSUPP;
+	}
+
+	memset(&current_link, 0, sizeof(current_link));
+	edev->ops->common->get_link(edev->cdev, &current_link);
+
+	memset(&params, 0, sizeof(params));
+	params.override_flags |= QED_LINK_OVERRIDE_PAUSE_CONFIG;
+	if (epause->autoneg) {
+		if (!(current_link.supported_caps & SUPPORTED_Autoneg)) {
+			DP_INFO(edev, "autoneg not supported\n");
+			return -EINVAL;
+		}
+		params.pause_config |= QED_LINK_PAUSE_AUTONEG_ENABLE;
+	}
+	if (epause->rx_pause)
+		params.pause_config |= QED_LINK_PAUSE_RX_ENABLE;
+	if (epause->tx_pause)
+		params.pause_config |= QED_LINK_PAUSE_TX_ENABLE;
+
+	params.link_up = true;
+	edev->ops->common->set_link(edev->cdev, &params);
+
+	return 0;
+}
+
 static void qede_update_mtu(struct qede_dev *edev, union qede_reload_args *args)
 {
 	edev->ndev->mtu = args->mtu;
@@ -366,17 +490,104 @@ int qede_change_mtu(struct net_device *ndev, int new_mtu)
 	return 0;
 }
 
+static void qede_get_channels(struct net_device *dev,
+			      struct ethtool_channels *channels)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+
+	channels->max_combined = QEDE_MAX_RSS_CNT(edev);
+	channels->combined_count = QEDE_RSS_CNT(edev);
+}
+
+static int qede_set_channels(struct net_device *dev,
+			     struct ethtool_channels *channels)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+
+	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))) {
+		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+			   "command parameters not supported\n");
+		return -EINVAL;
+	}
+
+	/* Check if there was a change in the active parameters */
+	if (channels->combined_count == QEDE_RSS_CNT(edev)) {
+		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) {
+		DP_VERBOSE(edev, (NETIF_MSG_IFUP | NETIF_MSG_IFDOWN),
+			   "Number of channels must be divisable 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;
+	if (netif_running(dev))
+		qede_reload(edev, NULL, NULL);
+
+	return 0;
+}
+
+static int qede_set_phys_id(struct net_device *dev,
+			    enum ethtool_phys_id_state state)
+{
+	struct qede_dev *edev = netdev_priv(dev);
+	u8 led_state = 0;
+
+	switch (state) {
+	case ETHTOOL_ID_ACTIVE:
+		return 1;	/* cycle on/off once per second */
+
+	case ETHTOOL_ID_ON:
+		led_state = QED_LED_MODE_ON;
+		break;
+
+	case ETHTOOL_ID_OFF:
+		led_state = QED_LED_MODE_OFF;
+		break;
+
+	case ETHTOOL_ID_INACTIVE:
+		led_state = QED_LED_MODE_RESTORE;
+		break;
+	}
+
+	edev->ops->common->set_led(edev->cdev, led_state);
+
+	return 0;
+}
+
 static const struct ethtool_ops qede_ethtool_ops = {
 	.get_settings = qede_get_settings,
 	.set_settings = qede_set_settings,
 	.get_drvinfo = qede_get_drvinfo,
 	.get_msglevel = qede_get_msglevel,
 	.set_msglevel = qede_set_msglevel,
+	.nway_reset = qede_nway_reset,
 	.get_link = qede_get_link,
+	.get_ringparam = qede_get_ringparam,
+	.set_ringparam = qede_set_ringparam,
+	.get_pauseparam = qede_get_pauseparam,
+	.set_pauseparam = qede_set_pauseparam,
 	.get_strings = qede_get_strings,
+	.set_phys_id = qede_set_phys_id,
 	.get_ethtool_stats = qede_get_ethtool_stats,
 	.get_sset_count = qede_get_sset_count,
 
+	.get_channels = qede_get_channels,
+	.set_channels = qede_set_channels,
 };
 
 void qede_set_ethtool_ops(struct net_device *dev)
diff --git a/drivers/net/ethernet/qlogic/qede/qede_main.c b/drivers/net/ethernet/qlogic/qede/qede_main.c
index f4657a2e730a..6237f10b5119 100644
--- a/drivers/net/ethernet/qlogic/qede/qede_main.c
+++ b/drivers/net/ethernet/qlogic/qede/qede_main.c
@@ -1502,8 +1502,11 @@ static int qede_set_num_queues(struct qede_dev *edev)
 	u16 rss_num;
 
 	/* Setup queues according to possible resources*/
-	rss_num = netif_get_num_default_rss_queues() *
-		  edev->dev_info.common.num_hwfns;
+	if (edev->req_rss)
+		rss_num = edev->req_rss;
+	else
+		rss_num = netif_get_num_default_rss_queues() *
+			  edev->dev_info.common.num_hwfns;
 
 	rss_num = min_t(u16, QEDE_MAX_RSS_CNT(edev), rss_num);
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
index a72bcddf160a..4b76c69fe86d 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.c
@@ -167,7 +167,7 @@ struct qlcnic_dcb_cfg {
 	u32 version;
 };
 
-static struct qlcnic_dcb_ops qlcnic_83xx_dcb_ops = {
+static const struct qlcnic_dcb_ops qlcnic_83xx_dcb_ops = {
 	.init_dcbnl_ops		= __qlcnic_init_dcbnl_ops,
 	.free			= __qlcnic_dcb_free,
 	.attach			= __qlcnic_dcb_attach,
@@ -180,7 +180,7 @@ static struct qlcnic_dcb_ops qlcnic_83xx_dcb_ops = {
 	.aen_handler		= qlcnic_83xx_dcb_aen_handler,
 };
 
-static struct qlcnic_dcb_ops qlcnic_82xx_dcb_ops = {
+static const struct qlcnic_dcb_ops qlcnic_82xx_dcb_ops = {
 	.init_dcbnl_ops		= __qlcnic_init_dcbnl_ops,
 	.free			= __qlcnic_dcb_free,
 	.attach			= __qlcnic_dcb_attach,
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.h
index 3cf4a10fbe1e..9777e5713525 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_dcb.h
@@ -37,7 +37,7 @@ struct qlcnic_dcb {
 	struct qlcnic_adapter		*adapter;
 	struct delayed_work		aen_work;
 	struct workqueue_struct		*wq;
-	struct qlcnic_dcb_ops		*ops;
+	const struct qlcnic_dcb_ops	*ops;
 	struct qlcnic_dcb_cfg		*cfg;
 	unsigned long			state;
 };
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
index d4b5085a21fa..7bd6f25b4625 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
@@ -1604,7 +1604,7 @@ int qlcnic_82xx_napi_add(struct qlcnic_adapter *adapter,
 	if (qlcnic_check_multi_tx(adapter) && !adapter->ahw->diag_test) {
 		for (ring = 0; ring < adapter->drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
-			netif_napi_add(netdev, &tx_ring->napi, qlcnic_tx_poll,
+			netif_tx_napi_add(netdev, &tx_ring->napi, qlcnic_tx_poll,
 				       NAPI_POLL_WEIGHT);
 		}
 	}
@@ -2135,7 +2135,7 @@ int qlcnic_83xx_napi_add(struct qlcnic_adapter *adapter,
 	    !(adapter->flags & QLCNIC_TX_INTR_SHARED)) {
 		for (ring = 0; ring < adapter->drv_tx_rings; ring++) {
 			tx_ring = &adapter->tx_ring[ring];
-			netif_napi_add(netdev, &tx_ring->napi,
+			netif_tx_napi_add(netdev, &tx_ring->napi,
 				       qlcnic_83xx_msix_tx_poll,
 				       NAPI_POLL_WEIGHT);
 		}
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.c b/drivers/net/ethernet/qualcomm/qca_spi.c
index 689a4a5c8dcf..1ef03939d25f 100644
--- a/drivers/net/ethernet/qualcomm/qca_spi.c
+++ b/drivers/net/ethernet/qualcomm/qca_spi.c
@@ -811,7 +811,7 @@ qcaspi_netdev_setup(struct net_device *dev)
 	dev->netdev_ops = &qcaspi_netdev_ops;
 	qcaspi_set_ethtool_ops(dev);
 	dev->watchdog_timeo = QCASPI_TX_TIMEOUT;
-	dev->flags = IFF_MULTICAST;
+	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
 	dev->tx_queue_len = 100;
 
 	qca = netdev_priv(dev);
diff --git a/drivers/net/ethernet/rdc/r6040.c b/drivers/net/ethernet/rdc/r6040.c
index 9a37247cf4b8..6b541e57c96a 100644
--- a/drivers/net/ethernet/rdc/r6040.c
+++ b/drivers/net/ethernet/rdc/r6040.c
@@ -1039,7 +1039,7 @@ static int r6040_mii_probe(struct net_device *dev)
 		return -ENODEV;
 	}
 
-	phydev = phy_connect(dev, dev_name(&phydev->dev), &r6040_adjust_link,
+	phydev = phy_connect(dev, phydev_name(phydev), &r6040_adjust_link,
 			     PHY_INTERFACE_MODE_MII);
 
 	if (IS_ERR(phydev)) {
@@ -1061,9 +1061,7 @@ static int r6040_mii_probe(struct net_device *dev)
 	lp->old_link = 0;
 	lp->old_duplex = -1;
 
-	dev_info(&lp->pdev->dev, "attached PHY driver [%s] "
-		"(mii_bus:phy_addr=%s)\n",
-		phydev->drv->name, dev_name(&phydev->dev));
+	phy_attached_info(phydev);
 
 	return 0;
 }
@@ -1077,7 +1075,6 @@ static int r6040_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	static int card_idx = -1;
 	int bar = 0;
 	u16 *adrp;
-	int i;
 
 	pr_info("%s\n", version);
 
@@ -1189,19 +1186,11 @@ static int r6040_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	lp->mii_bus->name = "r6040_eth_mii";
 	snprintf(lp->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		dev_name(&pdev->dev), card_idx);
-	lp->mii_bus->irq = kmalloc_array(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
-	if (!lp->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_mdio;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		lp->mii_bus->irq[i] = PHY_POLL;
 
 	err = mdiobus_register(lp->mii_bus);
 	if (err) {
 		dev_err(&pdev->dev, "failed to register MII bus\n");
-		goto err_out_mdio_irq;
+		goto err_out_mdio;
 	}
 
 	err = r6040_mii_probe(dev);
@@ -1220,8 +1209,6 @@ static int r6040_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 err_out_mdio_unregister:
 	mdiobus_unregister(lp->mii_bus);
-err_out_mdio_irq:
-	kfree(lp->mii_bus->irq);
 err_out_mdio:
 	mdiobus_free(lp->mii_bus);
 err_out_unmap:
@@ -1244,7 +1231,6 @@ static void r6040_remove_one(struct pci_dev *pdev)
 
 	unregister_netdev(dev);
 	mdiobus_unregister(lp->mii_bus);
-	kfree(lp->mii_bus->irq);
 	mdiobus_free(lp->mii_bus);
 	netif_napi_del(&lp->napi);
 	pci_iounmap(pdev, lp->base);
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index 79ef799f88ab..dd2cf3738b73 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -3894,7 +3894,7 @@ static void rtl8168h_1_hw_phy_config(struct rtl8169_private *tp)
 
 	/* disable phy pfm mode */
 	rtl_writephy(tp, 0x1f, 0x0a44);
-	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0080);
+	rtl_w0w1_phy(tp, 0x11, 0x0000, 0x0080);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* Check ALDPS bit, disable it if enabled */
@@ -3947,7 +3947,7 @@ static void rtl8168h_2_hw_phy_config(struct rtl8169_private *tp)
 	data = (ioffset_p3<<12)|(ioffset_p2<<8)|(ioffset_p1<<4)|(ioffset_p0);
 
 	if ((ioffset_p3 != 0x0f) || (ioffset_p2 != 0x0f) ||
-	    (ioffset_p1 != 0x0f) || (ioffset_p0 == 0x0f)) {
+	    (ioffset_p1 != 0x0f) || (ioffset_p0 != 0x0f)) {
 		rtl_writephy(tp, 0x1f, 0x0bcf);
 		rtl_writephy(tp, 0x16, data);
 		rtl_writephy(tp, 0x1f, 0x0000);
@@ -3967,7 +3967,7 @@ static void rtl8168h_2_hw_phy_config(struct rtl8169_private *tp)
 
 	/* disable phy pfm mode */
 	rtl_writephy(tp, 0x1f, 0x0a44);
-	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0080);
+	rtl_w0w1_phy(tp, 0x11, 0x0000, 0x0080);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* Check ALDPS bit, disable it if enabled */
@@ -4933,8 +4933,6 @@ static void rtl_init_rxcfg(struct rtl8169_private *tp)
 		RTL_W32(RxConfig, RX128_INT_EN | RX_MULTI_EN | RX_DMA_BURST);
 		break;
 	case RTL_GIGA_MAC_VER_40:
-		RTL_W32(RxConfig, RX128_INT_EN | RX_MULTI_EN | RX_DMA_BURST | RX_EARLY_OFF);
-		break;
 	case RTL_GIGA_MAC_VER_41:
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
@@ -4943,8 +4941,6 @@ static void rtl_init_rxcfg(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_46:
 	case RTL_GIGA_MAC_VER_47:
 	case RTL_GIGA_MAC_VER_48:
-		RTL_W32(RxConfig, RX128_INT_EN | RX_DMA_BURST | RX_EARLY_OFF);
-		break;
 	case RTL_GIGA_MAC_VER_49:
 	case RTL_GIGA_MAC_VER_50:
 	case RTL_GIGA_MAC_VER_51:
@@ -5818,11 +5814,10 @@ static void rtl_hw_start_8168d_4(struct rtl8169_private *tp)
 	void __iomem *ioaddr = tp->mmio_addr;
 	struct pci_dev *pdev = tp->pci_dev;
 	static const struct ephy_info e_info_8168d_4[] = {
-		{ 0x0b, ~0,	0x48 },
-		{ 0x19, 0x20,	0x50 },
-		{ 0x0c, ~0,	0x20 }
+		{ 0x0b, 0x0000,	0x0048 },
+		{ 0x19, 0x0020,	0x0050 },
+		{ 0x0c, 0x0100,	0x0020 }
 	};
-	int i;
 
 	rtl_csi_access_enable_1(tp);
 
@@ -5830,13 +5825,7 @@ static void rtl_hw_start_8168d_4(struct rtl8169_private *tp)
 
 	RTL_W8(MaxTxPacketSize, TxPacketMax);
 
-	for (i = 0; i < ARRAY_SIZE(e_info_8168d_4); i++) {
-		const struct ephy_info *e = e_info_8168d_4 + i;
-		u16 w;
-
-		w = rtl_ephy_read(tp, e->offset);
-		rtl_ephy_write(tp, 0x03, (w & e->mask) | e->bits);
-	}
+	rtl_ephy_init(tp, e_info_8168d_4, ARRAY_SIZE(e_info_8168d_4));
 
 	rtl_enable_clock_request(pdev);
 }
@@ -6127,7 +6116,7 @@ static void rtl_hw_start_8168h_1(struct rtl8169_private *tp)
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
 
 	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
-	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+	RTL_W8(MISC_1, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
 
 	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~TX_10M_PS_EN);
 
@@ -6136,7 +6125,7 @@ static void rtl_hw_start_8168h_1(struct rtl8169_private *tp)
 	rtl_pcie_state_l2l3_enable(tp, false);
 
 	rtl_writephy(tp, 0x1f, 0x0c42);
-	rg_saw_cnt = rtl_readphy(tp, 0x13);
+	rg_saw_cnt = (rtl_readphy(tp, 0x13) & 0x3fff);
 	rtl_writephy(tp, 0x1f, 0x0000);
 	if (rg_saw_cnt > 0) {
 		u16 sw_cnt_1ms_ini;
@@ -6144,28 +6133,28 @@ static void rtl_hw_start_8168h_1(struct rtl8169_private *tp)
 		sw_cnt_1ms_ini = 16000000/rg_saw_cnt;
 		sw_cnt_1ms_ini &= 0x0fff;
 		data = r8168_mac_ocp_read(tp, 0xd412);
-		data &= 0x0fff;
+		data &= ~0x0fff;
 		data |= sw_cnt_1ms_ini;
 		r8168_mac_ocp_write(tp, 0xd412, data);
 	}
 
 	data = r8168_mac_ocp_read(tp, 0xe056);
-	data &= 0xf0;
-	data |= 0x07;
+	data &= ~0xf0;
+	data |= 0x70;
 	r8168_mac_ocp_write(tp, 0xe056, data);
 
 	data = r8168_mac_ocp_read(tp, 0xe052);
-	data &= 0x8008;
-	data |= 0x6000;
+	data &= ~0x6000;
+	data |= 0x8008;
 	r8168_mac_ocp_write(tp, 0xe052, data);
 
 	data = r8168_mac_ocp_read(tp, 0xe0d6);
-	data &= 0x01ff;
+	data &= ~0x01ff;
 	data |= 0x017f;
 	r8168_mac_ocp_write(tp, 0xe0d6, data);
 
 	data = r8168_mac_ocp_read(tp, 0xd420);
-	data &= 0x0fff;
+	data &= ~0x0fff;
 	data |= 0x047f;
 	r8168_mac_ocp_write(tp, 0xd420, data);
 
@@ -6252,7 +6241,7 @@ static void rtl_hw_start_8168ep_2(struct rtl8169_private *tp)
 	rtl_hw_start_8168ep(tp);
 
 	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
-	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+	RTL_W8(MISC_1, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
 }
 
 static void rtl_hw_start_8168ep_3(struct rtl8169_private *tp)
@@ -6274,7 +6263,7 @@ static void rtl_hw_start_8168ep_3(struct rtl8169_private *tp)
 	rtl_hw_start_8168ep(tp);
 
 	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
-	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+	RTL_W8(MISC_1, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
 
 	data = r8168_mac_ocp_read(tp, 0xd3e2);
 	data &= 0xf000;
@@ -7737,10 +7726,13 @@ rtl8169_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
 	struct rtl8169_counters *counters = tp->counters;
 	unsigned int start;
 
-	if (netif_running(dev))
+	pm_runtime_get_noresume(&pdev->dev);
+
+	if (netif_running(dev) && pm_runtime_active(&pdev->dev))
 		rtl8169_rx_missed(dev, ioaddr);
 
 	do {
@@ -7768,7 +7760,8 @@ rtl8169_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
 	 * Fetch additonal counter values missing in stats collected by driver
 	 * from tally counters.
 	 */
-	rtl8169_update_counters(dev);
+	if (pm_runtime_active(&pdev->dev))
+		rtl8169_update_counters(dev);
 
 	/*
 	 * Subtract values fetched during initalization.
@@ -7781,6 +7774,8 @@ rtl8169_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
 	stats->tx_aborted_errors = le16_to_cpu(counters->tx_aborted) -
 		le16_to_cpu(tp->tc_offset.tx_aborted);
 
+	pm_runtime_put_noidle(&pdev->dev);
+
 	return stats;
 }
 
@@ -7860,6 +7855,10 @@ static int rtl8169_runtime_suspend(struct device *device)
 
 	rtl8169_net_suspend(dev);
 
+	/* Update counters before going runtime suspend */
+	rtl8169_rx_missed(dev, tp->mmio_addr);
+	rtl8169_update_counters(dev);
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/renesas/ravb.h b/drivers/net/ethernet/renesas/ravb.h
index 0623fff932e4..9fbe92ac225b 100644
--- a/drivers/net/ethernet/renesas/ravb.h
+++ b/drivers/net/ethernet/renesas/ravb.h
@@ -206,6 +206,7 @@ enum CCC_BIT {
 	CCC_OPC_RESET	= 0x00000000,
 	CCC_OPC_CONFIG	= 0x00000001,
 	CCC_OPC_OPERATION = 0x00000002,
+	CCC_GAC		= 0x00000080,
 	CCC_DTSR	= 0x00000100,
 	CCC_CSEL	= 0x00030000,
 	CCC_CSEL_HPB	= 0x00010000,
@@ -576,6 +577,9 @@ enum GTI_BIT {
 	GTI_TIV		= 0x0FFFFFFF,
 };
 
+#define GTI_TIV_MAX	GTI_TIV
+#define GTI_TIV_MIN	0x20
+
 /* GIC */
 enum GIC_BIT {
 	GIC_PTCE	= 0x00000001,	/* Undocumented? */
diff --git a/drivers/net/ethernet/renesas/ravb_main.c b/drivers/net/ethernet/renesas/ravb_main.c
index 467d41698fd5..86449c357168 100644
--- a/drivers/net/ethernet/renesas/ravb_main.c
+++ b/drivers/net/ethernet/renesas/ravb_main.c
@@ -32,6 +32,8 @@
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 
+#include <asm/div64.h>
+
 #include "ravb.h"
 
 #define RAVB_DEF_MSG_ENABLE \
@@ -113,12 +115,15 @@ static void ravb_read_mac_address(struct net_device *ndev, const u8 *mac)
 	if (mac) {
 		ether_addr_copy(ndev->dev_addr, mac);
 	} else {
-		ndev->dev_addr[0] = (ravb_read(ndev, MAHR) >> 24);
-		ndev->dev_addr[1] = (ravb_read(ndev, MAHR) >> 16) & 0xFF;
-		ndev->dev_addr[2] = (ravb_read(ndev, MAHR) >> 8) & 0xFF;
-		ndev->dev_addr[3] = (ravb_read(ndev, MAHR) >> 0) & 0xFF;
-		ndev->dev_addr[4] = (ravb_read(ndev, MALR) >> 8) & 0xFF;
-		ndev->dev_addr[5] = (ravb_read(ndev, MALR) >> 0) & 0xFF;
+		u32 mahr = ravb_read(ndev, MAHR);
+		u32 malr = ravb_read(ndev, MALR);
+
+		ndev->dev_addr[0] = (mahr >> 24) & 0xFF;
+		ndev->dev_addr[1] = (mahr >> 16) & 0xFF;
+		ndev->dev_addr[2] = (mahr >>  8) & 0xFF;
+		ndev->dev_addr[3] = (mahr >>  0) & 0xFF;
+		ndev->dev_addr[4] = (malr >>  8) & 0xFF;
+		ndev->dev_addr[5] = (malr >>  0) & 0xFF;
 	}
 }
 
@@ -338,16 +343,13 @@ error:
 static void ravb_emac_init(struct net_device *ndev)
 {
 	struct ravb_private *priv = netdev_priv(ndev);
-	u32 ecmr;
 
 	/* Receive frame limit set register */
 	ravb_write(ndev, ndev->mtu + ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN, RFLR);
 
 	/* PAUSE prohibition */
-	ecmr =  ravb_read(ndev, ECMR);
-	ecmr &= ECMR_DM;
-	ecmr |= ECMR_ZPF | (priv->duplex ? ECMR_DM : 0) | ECMR_TE | ECMR_RE;
-	ravb_write(ndev, ecmr, ECMR);
+	ravb_write(ndev, ECMR_ZPF | (priv->duplex ? ECMR_DM : 0) |
+		   ECMR_TE | ECMR_RE, ECMR);
 
 	ravb_set_rate(ndev);
 
@@ -405,9 +407,11 @@ static int ravb_dmac_init(struct net_device *ndev)
 	/* Timestamp enable */
 	ravb_write(ndev, TCCR_TFEN, TCCR);
 
-	/* Interrupt enable: */
+	/* Interrupt init: */
 	/* Frame receive */
 	ravb_write(ndev, RIC0_FRE0 | RIC0_FRE1, RIC0);
+	/* Disable FIFO full warning */
+	ravb_write(ndev, 0, RIC1);
 	/* Receive FIFO full error, descriptor empty */
 	ravb_write(ndev, RIC2_QFE0 | RIC2_QFE1 | RIC2_RFFE, RIC2);
 	/* Frame transmitted, timestamp FIFO updated */
@@ -875,6 +879,7 @@ static int ravb_phy_init(struct net_device *ndev)
 	struct ravb_private *priv = netdev_priv(ndev);
 	struct phy_device *phydev;
 	struct device_node *pn;
+	int err;
 
 	priv->link = 0;
 	priv->speed = 0;
@@ -882,6 +887,17 @@ static int ravb_phy_init(struct net_device *ndev)
 
 	/* Try connecting to PHY */
 	pn = of_parse_phandle(np, "phy-handle", 0);
+	if (!pn) {
+		/* In the case of a fixed PHY, the DT node associated
+		 * to the PHY is the Ethernet MAC DT node.
+		 */
+		if (of_phy_is_fixed_link(np)) {
+			err = of_phy_register_fixed_link(np);
+			if (err)
+				return err;
+		}
+		pn = of_node_get(np);
+	}
 	phydev = of_phy_connect(ndev, pn, ravb_adjust_link, 0,
 				priv->phy_interface);
 	if (!phydev) {
@@ -908,8 +924,7 @@ static int ravb_phy_init(struct net_device *ndev)
 	/* 10BASE is not supported */
 	phydev->supported &= ~PHY_10BT_FEATURES;
 
-	netdev_info(ndev, "attached PHY %d (IRQ %d) to driver %s\n",
-		    phydev->addr, phydev->irq, phydev->drv->name);
+	phy_attached_info(phydev);
 
 	priv->phydev = phydev;
 
@@ -1124,7 +1139,8 @@ static int ravb_set_ringparam(struct net_device *ndev,
 	if (netif_running(ndev)) {
 		netif_device_detach(ndev);
 		/* Stop PTP Clock driver */
-		ravb_ptp_stop(ndev);
+		if (priv->chip_id == RCAR_GEN2)
+			ravb_ptp_stop(ndev);
 		/* Wait for DMA stopping */
 		error = ravb_stop_dma(ndev);
 		if (error) {
@@ -1155,7 +1171,8 @@ static int ravb_set_ringparam(struct net_device *ndev,
 		ravb_emac_init(ndev);
 
 		/* Initialise PTP Clock driver */
-		ravb_ptp_init(ndev, priv->pdev);
+		if (priv->chip_id == RCAR_GEN2)
+			ravb_ptp_init(ndev, priv->pdev);
 
 		netif_device_attach(ndev);
 	}
@@ -1232,7 +1249,8 @@ static int ravb_open(struct net_device *ndev)
 	ravb_emac_init(ndev);
 
 	/* Initialise PTP Clock driver */
-	ravb_ptp_init(ndev, priv->pdev);
+	if (priv->chip_id == RCAR_GEN2)
+		ravb_ptp_init(ndev, priv->pdev);
 
 	netif_tx_start_all_queues(ndev);
 
@@ -1245,7 +1263,8 @@ static int ravb_open(struct net_device *ndev)
 
 out_ptp_stop:
 	/* Stop PTP Clock driver */
-	ravb_ptp_stop(ndev);
+	if (priv->chip_id == RCAR_GEN2)
+		ravb_ptp_stop(ndev);
 out_free_irq2:
 	if (priv->chip_id == RCAR_GEN3)
 		free_irq(priv->emac_irq, ndev);
@@ -1281,7 +1300,8 @@ static void ravb_tx_timeout_work(struct work_struct *work)
 	netif_tx_stop_all_queues(ndev);
 
 	/* Stop PTP Clock driver */
-	ravb_ptp_stop(ndev);
+	if (priv->chip_id == RCAR_GEN2)
+		ravb_ptp_stop(ndev);
 
 	/* Wait for DMA stopping */
 	ravb_stop_dma(ndev);
@@ -1294,7 +1314,8 @@ static void ravb_tx_timeout_work(struct work_struct *work)
 	ravb_emac_init(ndev);
 
 	/* Initialise PTP Clock driver */
-	ravb_ptp_init(ndev, priv->pdev);
+	if (priv->chip_id == RCAR_GEN2)
+		ravb_ptp_init(ndev, priv->pdev);
 
 	netif_tx_start_all_queues(ndev);
 }
@@ -1474,12 +1495,12 @@ static int ravb_close(struct net_device *ndev)
 
 	/* Disable interrupts by clearing the interrupt masks. */
 	ravb_write(ndev, 0, RIC0);
-	ravb_write(ndev, 0, RIC1);
 	ravb_write(ndev, 0, RIC2);
 	ravb_write(ndev, 0, TIC);
 
 	/* Stop PTP Clock driver */
-	ravb_ptp_stop(ndev);
+	if (priv->chip_id == RCAR_GEN2)
+		ravb_ptp_stop(ndev);
 
 	/* Set the config mode to stop the AVB-DMAC's processes */
 	if (ravb_stop_dma(ndev) < 0)
@@ -1659,15 +1680,48 @@ static int ravb_mdio_release(struct ravb_private *priv)
 static const struct of_device_id ravb_match_table[] = {
 	{ .compatible = "renesas,etheravb-r8a7790", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,etheravb-r8a7794", .data = (void *)RCAR_GEN2 },
+	{ .compatible = "renesas,etheravb-rcar-gen2", .data = (void *)RCAR_GEN2 },
 	{ .compatible = "renesas,etheravb-r8a7795", .data = (void *)RCAR_GEN3 },
+	{ .compatible = "renesas,etheravb-rcar-gen3", .data = (void *)RCAR_GEN3 },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, ravb_match_table);
 
+static int ravb_set_gti(struct net_device *ndev)
+{
+
+	struct device *dev = ndev->dev.parent;
+	struct device_node *np = dev->of_node;
+	unsigned long rate;
+	struct clk *clk;
+	uint64_t inc;
+
+	clk = of_clk_get(np, 0);
+	if (IS_ERR(clk)) {
+		dev_err(dev, "could not get clock\n");
+		return PTR_ERR(clk);
+	}
+
+	rate = clk_get_rate(clk);
+	clk_put(clk);
+
+	inc = 1000000000ULL << 20;
+	do_div(inc, rate);
+
+	if (inc < GTI_TIV_MIN || inc > GTI_TIV_MAX) {
+		dev_err(dev, "gti.tiv increment 0x%llx is outside the range 0x%x - 0x%x\n",
+			inc, GTI_TIV_MIN, GTI_TIV_MAX);
+		return -EINVAL;
+	}
+
+	ravb_write(ndev, inc, GTI);
+
+	return 0;
+}
+
 static int ravb_probe(struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	const struct of_device_id *match;
 	struct ravb_private *priv;
 	enum ravb_chip_id chip_id;
 	struct net_device *ndev;
@@ -1699,8 +1753,7 @@ static int ravb_probe(struct platform_device *pdev)
 	ndev->base_addr = res->start;
 	ndev->dma = -1;
 
-	match = of_match_device(of_match_ptr(ravb_match_table), &pdev->dev);
-	chip_id = (enum ravb_chip_id)match->data;
+	chip_id = (enum ravb_chip_id)of_device_get_match_data(&pdev->dev);
 
 	if (chip_id == RCAR_GEN3)
 		irq = platform_get_irq_byname(pdev, "ch22");
@@ -1752,15 +1805,21 @@ static int ravb_probe(struct platform_device *pdev)
 	ndev->ethtool_ops = &ravb_ethtool_ops;
 
 	/* Set AVB config mode */
-	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) | CCC_OPC_CONFIG,
-		   CCC);
-
-	/* Set CSEL value */
-	ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_CSEL) | CCC_CSEL_HPB,
-		   CCC);
+	if (chip_id == RCAR_GEN2) {
+		ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) |
+			   CCC_OPC_CONFIG, CCC);
+		/* Set CSEL value */
+		ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_CSEL) |
+			   CCC_CSEL_HPB, CCC);
+	} else {
+		ravb_write(ndev, (ravb_read(ndev, CCC) & ~CCC_OPC) |
+			   CCC_OPC_CONFIG | CCC_GAC | CCC_CSEL_HPB, CCC);
+	}
 
 	/* Set GTI value */
-	ravb_write(ndev, ((1000 << 20) / 130) & GTI_TIV, GTI);
+	error = ravb_set_gti(ndev);
+	if (error)
+		goto out_release;
 
 	/* Request GTI loading */
 	ravb_write(ndev, ravb_read(ndev, GCCR) | GCCR_LTI, GCCR);
@@ -1783,6 +1842,10 @@ static int ravb_probe(struct platform_device *pdev)
 	/* Initialise HW timestamp list */
 	INIT_LIST_HEAD(&priv->ts_skb_list);
 
+	/* Initialise PTP Clock driver */
+	if (chip_id != RCAR_GEN2)
+		ravb_ptp_init(ndev, pdev);
+
 	/* Debug message level */
 	priv->msg_enable = RAVB_DEF_MSG_ENABLE;
 
@@ -1824,6 +1887,10 @@ out_napi_del:
 out_dma_free:
 	dma_free_coherent(ndev->dev.parent, priv->desc_bat_size, priv->desc_bat,
 			  priv->desc_bat_dma);
+
+	/* Stop PTP Clock driver */
+	if (chip_id != RCAR_GEN2)
+		ravb_ptp_stop(ndev);
 out_release:
 	if (ndev)
 		free_netdev(ndev);
@@ -1838,6 +1905,10 @@ static int ravb_remove(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct ravb_private *priv = netdev_priv(ndev);
 
+	/* Stop PTP Clock driver */
+	if (priv->chip_id != RCAR_GEN2)
+		ravb_ptp_stop(ndev);
+
 	dma_free_coherent(ndev->dev.parent, priv->desc_bat_size, priv->desc_bat,
 			  priv->desc_bat_dma);
 	/* Set reset mode */
diff --git a/drivers/net/ethernet/renesas/sh_eth.c b/drivers/net/ethernet/renesas/sh_eth.c
index 6a8fc0f341ff..738449992876 100644
--- a/drivers/net/ethernet/renesas/sh_eth.c
+++ b/drivers/net/ethernet/renesas/sh_eth.c
@@ -473,6 +473,109 @@ static void sh_eth_set_duplex(struct net_device *ndev)
 		sh_eth_write(ndev, sh_eth_read(ndev, ECMR) & ~ECMR_DM, ECMR);
 }
 
+static void sh_eth_chip_reset(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	/* reset device */
+	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
+	mdelay(1);
+}
+
+static void sh_eth_set_rate_gether(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	switch (mdp->speed) {
+	case 10: /* 10BASE */
+		sh_eth_write(ndev, GECMR_10, GECMR);
+		break;
+	case 100:/* 100BASE */
+		sh_eth_write(ndev, GECMR_100, GECMR);
+		break;
+	case 1000: /* 1000BASE */
+		sh_eth_write(ndev, GECMR_1000, GECMR);
+		break;
+	default:
+		break;
+	}
+}
+
+#ifdef CONFIG_OF
+/* R7S72100 */
+static struct sh_eth_cpu_data r7s72100_data = {
+	.chip_reset	= sh_eth_chip_reset,
+	.set_duplex	= sh_eth_set_duplex,
+
+	.register_type	= SH_ETH_REG_FAST_RZ,
+
+	.ecsr_value	= ECSR_ICD,
+	.ecsipr_value	= ECSIPR_ICDIP,
+	.eesipr_value	= 0xff7f009f,
+
+	.tx_check	= EESR_TC1 | EESR_FTC,
+	.eesr_err_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_RABT |
+			  EESR_RFE | EESR_RDE | EESR_RFRMER | EESR_TFE |
+			  EESR_TDE | EESR_ECI,
+	.fdr_value	= 0x0000070f,
+
+	.no_psr		= 1,
+	.apr		= 1,
+	.mpr		= 1,
+	.tpauser	= 1,
+	.hw_swap	= 1,
+	.rpadir		= 1,
+	.rpadir_value   = 2 << 16,
+	.no_trimd	= 1,
+	.no_ade		= 1,
+	.hw_crc		= 1,
+	.tsu		= 1,
+	.shift_rd0	= 1,
+};
+
+static void sh_eth_chip_reset_r8a7740(struct net_device *ndev)
+{
+	struct sh_eth_private *mdp = netdev_priv(ndev);
+
+	/* reset device */
+	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
+	mdelay(1);
+
+	sh_eth_select_mii(ndev);
+}
+
+/* R8A7740 */
+static struct sh_eth_cpu_data r8a7740_data = {
+	.chip_reset	= sh_eth_chip_reset_r8a7740,
+	.set_duplex	= sh_eth_set_duplex,
+	.set_rate	= sh_eth_set_rate_gether,
+
+	.register_type	= SH_ETH_REG_GIGABIT,
+
+	.ecsr_value	= ECSR_ICD | ECSR_MPD,
+	.ecsipr_value	= ECSIPR_LCHNGIP | ECSIPR_ICDIP | ECSIPR_MPDIP,
+	.eesipr_value	= DMAC_M_RFRMER | DMAC_M_ECI | 0x003fffff,
+
+	.tx_check	= EESR_TC1 | EESR_FTC,
+	.eesr_err_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_RABT |
+			  EESR_RFE | EESR_RDE | EESR_RFRMER | EESR_TFE |
+			  EESR_TDE | EESR_ECI,
+	.fdr_value	= 0x0000070f,
+
+	.apr		= 1,
+	.mpr		= 1,
+	.tpauser	= 1,
+	.bculr		= 1,
+	.hw_swap	= 1,
+	.rpadir		= 1,
+	.rpadir_value   = 2 << 16,
+	.no_trimd	= 1,
+	.no_ade		= 1,
+	.tsu		= 1,
+	.select_mii	= 1,
+	.shift_rd0	= 1,
+};
+
 /* There is CPU dependent code */
 static void sh_eth_set_rate_r8a777x(struct net_device *ndev)
 {
@@ -538,6 +641,7 @@ static struct sh_eth_cpu_data r8a779x_data = {
 	.hw_swap	= 1,
 	.rmiimode	= 1,
 };
+#endif /* CONFIG_OF */
 
 static void sh_eth_set_rate_sh7724(struct net_device *ndev)
 {
@@ -695,34 +799,6 @@ static struct sh_eth_cpu_data sh7757_data_giga = {
 	.tsu		= 1,
 };
 
-static void sh_eth_chip_reset(struct net_device *ndev)
-{
-	struct sh_eth_private *mdp = netdev_priv(ndev);
-
-	/* reset device */
-	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
-	mdelay(1);
-}
-
-static void sh_eth_set_rate_gether(struct net_device *ndev)
-{
-	struct sh_eth_private *mdp = netdev_priv(ndev);
-
-	switch (mdp->speed) {
-	case 10: /* 10BASE */
-		sh_eth_write(ndev, GECMR_10, GECMR);
-		break;
-	case 100:/* 100BASE */
-		sh_eth_write(ndev, GECMR_100, GECMR);
-		break;
-	case 1000: /* 1000BASE */
-		sh_eth_write(ndev, GECMR_1000, GECMR);
-		break;
-	default:
-		break;
-	}
-}
-
 /* SH7734 */
 static struct sh_eth_cpu_data sh7734_data = {
 	.chip_reset	= sh_eth_chip_reset,
@@ -780,80 +856,6 @@ static struct sh_eth_cpu_data sh7763_data = {
 	.irq_flags	= IRQF_SHARED,
 };
 
-static void sh_eth_chip_reset_r8a7740(struct net_device *ndev)
-{
-	struct sh_eth_private *mdp = netdev_priv(ndev);
-
-	/* reset device */
-	sh_eth_tsu_write(mdp, ARSTR_ARSTR, ARSTR);
-	mdelay(1);
-
-	sh_eth_select_mii(ndev);
-}
-
-/* R8A7740 */
-static struct sh_eth_cpu_data r8a7740_data = {
-	.chip_reset	= sh_eth_chip_reset_r8a7740,
-	.set_duplex	= sh_eth_set_duplex,
-	.set_rate	= sh_eth_set_rate_gether,
-
-	.register_type	= SH_ETH_REG_GIGABIT,
-
-	.ecsr_value	= ECSR_ICD | ECSR_MPD,
-	.ecsipr_value	= ECSIPR_LCHNGIP | ECSIPR_ICDIP | ECSIPR_MPDIP,
-	.eesipr_value	= DMAC_M_RFRMER | DMAC_M_ECI | 0x003fffff,
-
-	.tx_check	= EESR_TC1 | EESR_FTC,
-	.eesr_err_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_RABT |
-			  EESR_RFE | EESR_RDE | EESR_RFRMER | EESR_TFE |
-			  EESR_TDE | EESR_ECI,
-	.fdr_value	= 0x0000070f,
-
-	.apr		= 1,
-	.mpr		= 1,
-	.tpauser	= 1,
-	.bculr		= 1,
-	.hw_swap	= 1,
-	.rpadir		= 1,
-	.rpadir_value   = 2 << 16,
-	.no_trimd	= 1,
-	.no_ade		= 1,
-	.tsu		= 1,
-	.select_mii	= 1,
-	.shift_rd0	= 1,
-};
-
-/* R7S72100 */
-static struct sh_eth_cpu_data r7s72100_data = {
-	.chip_reset	= sh_eth_chip_reset,
-	.set_duplex	= sh_eth_set_duplex,
-
-	.register_type	= SH_ETH_REG_FAST_RZ,
-
-	.ecsr_value	= ECSR_ICD,
-	.ecsipr_value	= ECSIPR_ICDIP,
-	.eesipr_value	= 0xff7f009f,
-
-	.tx_check	= EESR_TC1 | EESR_FTC,
-	.eesr_err_check	= EESR_TWB1 | EESR_TWB | EESR_TABT | EESR_RABT |
-			  EESR_RFE | EESR_RDE | EESR_RFRMER | EESR_TFE |
-			  EESR_TDE | EESR_ECI,
-	.fdr_value	= 0x0000070f,
-
-	.no_psr		= 1,
-	.apr		= 1,
-	.mpr		= 1,
-	.tpauser	= 1,
-	.hw_swap	= 1,
-	.rpadir		= 1,
-	.rpadir_value   = 2 << 16,
-	.no_trimd	= 1,
-	.no_ade		= 1,
-	.hw_crc		= 1,
-	.tsu		= 1,
-	.shift_rd0	= 1,
-};
-
 static struct sh_eth_cpu_data sh7619_data = {
 	.register_type	= SH_ETH_REG_FAST_SH3_SH2,
 
@@ -965,30 +967,6 @@ static void sh_eth_set_receive_align(struct sk_buff *skb)
 		skb_reserve(skb, SH_ETH_RX_ALIGN - reserve);
 }
 
-
-/* CPU <-> EDMAC endian convert */
-static inline __u32 cpu_to_edmac(struct sh_eth_private *mdp, u32 x)
-{
-	switch (mdp->edmac_endian) {
-	case EDMAC_LITTLE_ENDIAN:
-		return cpu_to_le32(x);
-	case EDMAC_BIG_ENDIAN:
-		return cpu_to_be32(x);
-	}
-	return x;
-}
-
-static inline __u32 edmac_to_cpu(struct sh_eth_private *mdp, u32 x)
-{
-	switch (mdp->edmac_endian) {
-	case EDMAC_LITTLE_ENDIAN:
-		return le32_to_cpu(x);
-	case EDMAC_BIG_ENDIAN:
-		return be32_to_cpu(x);
-	}
-	return x;
-}
-
 /* Program the hardware MAC address from dev->dev_addr. */
 static void update_mac_address(struct net_device *ndev)
 {
@@ -1011,12 +989,15 @@ static void read_mac_address(struct net_device *ndev, unsigned char *mac)
 	if (mac[0] || mac[1] || mac[2] || mac[3] || mac[4] || mac[5]) {
 		memcpy(ndev->dev_addr, mac, ETH_ALEN);
 	} else {
-		ndev->dev_addr[0] = (sh_eth_read(ndev, MAHR) >> 24);
-		ndev->dev_addr[1] = (sh_eth_read(ndev, MAHR) >> 16) & 0xFF;
-		ndev->dev_addr[2] = (sh_eth_read(ndev, MAHR) >> 8) & 0xFF;
-		ndev->dev_addr[3] = (sh_eth_read(ndev, MAHR) & 0xFF);
-		ndev->dev_addr[4] = (sh_eth_read(ndev, MALR) >> 8) & 0xFF;
-		ndev->dev_addr[5] = (sh_eth_read(ndev, MALR) & 0xFF);
+		u32 mahr = sh_eth_read(ndev, MAHR);
+		u32 malr = sh_eth_read(ndev, MALR);
+
+		ndev->dev_addr[0] = (mahr >> 24) & 0xFF;
+		ndev->dev_addr[1] = (mahr >> 16) & 0xFF;
+		ndev->dev_addr[2] = (mahr >>  8) & 0xFF;
+		ndev->dev_addr[3] = (mahr >>  0) & 0xFF;
+		ndev->dev_addr[4] = (malr >>  8) & 0xFF;
+		ndev->dev_addr[5] = (malr >>  0) & 0xFF;
 	}
 }
 
@@ -1032,56 +1013,34 @@ struct bb_info {
 	void (*set_gate)(void *addr);
 	struct mdiobb_ctrl ctrl;
 	void *addr;
-	u32 mmd_msk;/* MMD */
-	u32 mdo_msk;
-	u32 mdi_msk;
-	u32 mdc_msk;
 };
 
-/* PHY bit set */
-static void bb_set(void *addr, u32 msk)
+static void sh_mdio_ctrl(struct mdiobb_ctrl *ctrl, u32 mask, int set)
 {
-	iowrite32(ioread32(addr) | msk, addr);
-}
+	struct bb_info *bitbang = container_of(ctrl, struct bb_info, ctrl);
+	u32 pir;
 
-/* PHY bit clear */
-static void bb_clr(void *addr, u32 msk)
-{
-	iowrite32((ioread32(addr) & ~msk), addr);
-}
+	if (bitbang->set_gate)
+		bitbang->set_gate(bitbang->addr);
 
-/* PHY bit read */
-static int bb_read(void *addr, u32 msk)
-{
-	return (ioread32(addr) & msk) != 0;
+	pir = ioread32(bitbang->addr);
+	if (set)
+		pir |=  mask;
+	else
+		pir &= ~mask;
+	iowrite32(pir, bitbang->addr);
 }
 
 /* Data I/O pin control */
 static void sh_mmd_ctrl(struct mdiobb_ctrl *ctrl, int bit)
 {
-	struct bb_info *bitbang = container_of(ctrl, struct bb_info, ctrl);
-
-	if (bitbang->set_gate)
-		bitbang->set_gate(bitbang->addr);
-
-	if (bit)
-		bb_set(bitbang->addr, bitbang->mmd_msk);
-	else
-		bb_clr(bitbang->addr, bitbang->mmd_msk);
+	sh_mdio_ctrl(ctrl, PIR_MMD, bit);
 }
 
 /* Set bit data*/
 static void sh_set_mdio(struct mdiobb_ctrl *ctrl, int bit)
 {
-	struct bb_info *bitbang = container_of(ctrl, struct bb_info, ctrl);
-
-	if (bitbang->set_gate)
-		bitbang->set_gate(bitbang->addr);
-
-	if (bit)
-		bb_set(bitbang->addr, bitbang->mdo_msk);
-	else
-		bb_clr(bitbang->addr, bitbang->mdo_msk);
+	sh_mdio_ctrl(ctrl, PIR_MDO, bit);
 }
 
 /* Get bit data*/
@@ -1092,21 +1051,13 @@ static int sh_get_mdio(struct mdiobb_ctrl *ctrl)
 	if (bitbang->set_gate)
 		bitbang->set_gate(bitbang->addr);
 
-	return bb_read(bitbang->addr, bitbang->mdi_msk);
+	return (ioread32(bitbang->addr) & PIR_MDI) != 0;
 }
 
 /* MDC pin control */
 static void sh_mdc_ctrl(struct mdiobb_ctrl *ctrl, int bit)
 {
-	struct bb_info *bitbang = container_of(ctrl, struct bb_info, ctrl);
-
-	if (bitbang->set_gate)
-		bitbang->set_gate(bitbang->addr);
-
-	if (bit)
-		bb_set(bitbang->addr, bitbang->mdc_msk);
-	else
-		bb_clr(bitbang->addr, bitbang->mdc_msk);
+	sh_mdio_ctrl(ctrl, PIR_MDC, bit);
 }
 
 /* mdio bus control struct */
@@ -1189,7 +1140,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 		rxdesc = &mdp->rx_ring[i];
 		/* The size of the buffer is a multiple of 32 bytes. */
 		buf_len = ALIGN(mdp->rx_buf_sz, 32);
-		rxdesc->len = cpu_to_edmac(mdp, buf_len << 16);
+		rxdesc->len = cpu_to_le32(buf_len << 16);
 		dma_addr = dma_map_single(&ndev->dev, skb->data, buf_len,
 					  DMA_FROM_DEVICE);
 		if (dma_mapping_error(&ndev->dev, dma_addr)) {
@@ -1197,8 +1148,8 @@ static void sh_eth_ring_format(struct net_device *ndev)
 			break;
 		}
 		mdp->rx_skbuff[i] = skb;
-		rxdesc->addr = cpu_to_edmac(mdp, dma_addr);
-		rxdesc->status = cpu_to_edmac(mdp, RD_RACT | RD_RFP);
+		rxdesc->addr = cpu_to_le32(dma_addr);
+		rxdesc->status = cpu_to_le32(RD_RACT | RD_RFP);
 
 		/* Rx descriptor address set */
 		if (i == 0) {
@@ -1212,7 +1163,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	mdp->dirty_rx = (u32) (i - mdp->num_rx_ring);
 
 	/* Mark the last entry as wrapping the ring. */
-	rxdesc->status |= cpu_to_edmac(mdp, RD_RDLE);
+	rxdesc->status |= cpu_to_le32(RD_RDLE);
 
 	memset(mdp->tx_ring, 0, tx_ringsize);
 
@@ -1220,8 +1171,8 @@ static void sh_eth_ring_format(struct net_device *ndev)
 	for (i = 0; i < mdp->num_tx_ring; i++) {
 		mdp->tx_skbuff[i] = NULL;
 		txdesc = &mdp->tx_ring[i];
-		txdesc->status = cpu_to_edmac(mdp, TD_TFP);
-		txdesc->len = cpu_to_edmac(mdp, 0);
+		txdesc->status = cpu_to_le32(TD_TFP);
+		txdesc->len = cpu_to_le32(0);
 		if (i == 0) {
 			/* Tx descriptor address set */
 			sh_eth_write(ndev, mdp->tx_desc_dma, TDLAR);
@@ -1231,7 +1182,7 @@ static void sh_eth_ring_format(struct net_device *ndev)
 		}
 	}
 
-	txdesc->status |= cpu_to_edmac(mdp, TD_TDLE);
+	txdesc->status |= cpu_to_le32(TD_TDLE);
 }
 
 /* Get skb and descriptor buffer */
@@ -1289,7 +1240,6 @@ static int sh_eth_dev_init(struct net_device *ndev, bool start)
 {
 	int ret = 0;
 	struct sh_eth_private *mdp = netdev_priv(ndev);
-	u32 val;
 
 	/* Soft Reset */
 	ret = sh_eth_reset(ndev);
@@ -1342,10 +1292,8 @@ static int sh_eth_dev_init(struct net_device *ndev, bool start)
 	}
 
 	/* PAUSE Prohibition */
-	val = (sh_eth_read(ndev, ECMR) & ECMR_DM) |
-		ECMR_ZPF | (mdp->duplex ? ECMR_DM : 0) | ECMR_TE | ECMR_RE;
-
-	sh_eth_write(ndev, val, ECMR);
+	sh_eth_write(ndev, ECMR_ZPF | (mdp->duplex ? ECMR_DM : 0) |
+		     ECMR_TE | ECMR_RE, ECMR);
 
 	if (mdp->cd->set_rate)
 		mdp->cd->set_rate(ndev);
@@ -1387,7 +1335,7 @@ static void sh_eth_dev_exit(struct net_device *ndev)
 	 * packet boundary if it's currently running
 	 */
 	for (i = 0; i < mdp->num_tx_ring; i++)
-		mdp->tx_ring[i].status &= ~cpu_to_edmac(mdp, TD_TACT);
+		mdp->tx_ring[i].status &= ~cpu_to_le32(TD_TACT);
 
 	/* Disable TX FIFO egress to MAC */
 	sh_eth_rcv_snd_disable(ndev);
@@ -1419,29 +1367,28 @@ static int sh_eth_txfree(struct net_device *ndev)
 	for (; mdp->cur_tx - mdp->dirty_tx > 0; mdp->dirty_tx++) {
 		entry = mdp->dirty_tx % mdp->num_tx_ring;
 		txdesc = &mdp->tx_ring[entry];
-		if (txdesc->status & cpu_to_edmac(mdp, TD_TACT))
+		if (txdesc->status & cpu_to_le32(TD_TACT))
 			break;
 		/* TACT bit must be checked before all the following reads */
 		dma_rmb();
 		netif_info(mdp, tx_done, ndev,
 			   "tx entry %d status 0x%08x\n",
-			   entry, edmac_to_cpu(mdp, txdesc->status));
+			   entry, le32_to_cpu(txdesc->status));
 		/* Free the original skb. */
 		if (mdp->tx_skbuff[entry]) {
-			dma_unmap_single(&ndev->dev,
-					 edmac_to_cpu(mdp, txdesc->addr),
-					 edmac_to_cpu(mdp, txdesc->len) >> 16,
+			dma_unmap_single(&ndev->dev, le32_to_cpu(txdesc->addr),
+					 le32_to_cpu(txdesc->len) >> 16,
 					 DMA_TO_DEVICE);
 			dev_kfree_skb_irq(mdp->tx_skbuff[entry]);
 			mdp->tx_skbuff[entry] = NULL;
 			free_num++;
 		}
-		txdesc->status = cpu_to_edmac(mdp, TD_TFP);
+		txdesc->status = cpu_to_le32(TD_TFP);
 		if (entry >= mdp->num_tx_ring - 1)
-			txdesc->status |= cpu_to_edmac(mdp, TD_TDLE);
+			txdesc->status |= cpu_to_le32(TD_TDLE);
 
 		ndev->stats.tx_packets++;
-		ndev->stats.tx_bytes += edmac_to_cpu(mdp, txdesc->len) >> 16;
+		ndev->stats.tx_bytes += le32_to_cpu(txdesc->len) >> 16;
 	}
 	return free_num;
 }
@@ -1465,11 +1412,11 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 	boguscnt = min(boguscnt, *quota);
 	limit = boguscnt;
 	rxdesc = &mdp->rx_ring[entry];
-	while (!(rxdesc->status & cpu_to_edmac(mdp, RD_RACT))) {
+	while (!(rxdesc->status & cpu_to_le32(RD_RACT))) {
 		/* RACT bit must be checked before all the following reads */
 		dma_rmb();
-		desc_status = edmac_to_cpu(mdp, rxdesc->status);
-		pkt_len = edmac_to_cpu(mdp, rxdesc->len) & RD_RFL;
+		desc_status = le32_to_cpu(rxdesc->status);
+		pkt_len = le32_to_cpu(rxdesc->len) & RD_RFL;
 
 		if (--boguscnt < 0)
 			break;
@@ -1507,7 +1454,7 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 			if (desc_status & RD_RFS10)
 				ndev->stats.rx_over_errors++;
 		} else	if (skb) {
-			dma_addr = edmac_to_cpu(mdp, rxdesc->addr);
+			dma_addr = le32_to_cpu(rxdesc->addr);
 			if (!mdp->cd->hw_swap)
 				sh_eth_soft_swap(
 					phys_to_virt(ALIGN(dma_addr, 4)),
@@ -1536,7 +1483,7 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 		rxdesc = &mdp->rx_ring[entry];
 		/* The size of the buffer is 32 byte boundary. */
 		buf_len = ALIGN(mdp->rx_buf_sz, 32);
-		rxdesc->len = cpu_to_edmac(mdp, buf_len << 16);
+		rxdesc->len = cpu_to_le32(buf_len << 16);
 
 		if (mdp->rx_skbuff[entry] == NULL) {
 			skb = netdev_alloc_skb(ndev, skbuff_size);
@@ -1552,15 +1499,14 @@ static int sh_eth_rx(struct net_device *ndev, u32 intr_status, int *quota)
 			mdp->rx_skbuff[entry] = skb;
 
 			skb_checksum_none_assert(skb);
-			rxdesc->addr = cpu_to_edmac(mdp, dma_addr);
+			rxdesc->addr = cpu_to_le32(dma_addr);
 		}
 		dma_wmb(); /* RACT bit must be set after all the above writes */
 		if (entry >= mdp->num_rx_ring - 1)
 			rxdesc->status |=
-				cpu_to_edmac(mdp, RD_RACT | RD_RFP | RD_RDLE);
+				cpu_to_le32(RD_RACT | RD_RFP | RD_RDLE);
 		else
-			rxdesc->status |=
-				cpu_to_edmac(mdp, RD_RACT | RD_RFP);
+			rxdesc->status |= cpu_to_le32(RD_RACT | RD_RFP);
 	}
 
 	/* Restart Rx engine if stopped. */
@@ -1877,8 +1823,7 @@ static int sh_eth_phy_init(struct net_device *ndev)
 		return PTR_ERR(phydev);
 	}
 
-	netdev_info(ndev, "attached PHY %d (IRQ %d) to driver %s\n",
-		    phydev->addr, phydev->irq, phydev->drv->name);
+	phy_attached_info(phydev);
 
 	mdp->phydev = phydev;
 
@@ -2360,8 +2305,8 @@ static void sh_eth_tx_timeout(struct net_device *ndev)
 	/* Free all the skbuffs in the Rx queue. */
 	for (i = 0; i < mdp->num_rx_ring; i++) {
 		rxdesc = &mdp->rx_ring[i];
-		rxdesc->status = cpu_to_edmac(mdp, 0);
-		rxdesc->addr = cpu_to_edmac(mdp, 0xBADF00D0);
+		rxdesc->status = cpu_to_le32(0);
+		rxdesc->addr = cpu_to_le32(0xBADF00D0);
 		dev_kfree_skb(mdp->rx_skbuff[i]);
 		mdp->rx_skbuff[i] = NULL;
 	}
@@ -2409,14 +2354,14 @@ static int sh_eth_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		kfree_skb(skb);
 		return NETDEV_TX_OK;
 	}
-	txdesc->addr = cpu_to_edmac(mdp, dma_addr);
-	txdesc->len  = cpu_to_edmac(mdp, skb->len << 16);
+	txdesc->addr = cpu_to_le32(dma_addr);
+	txdesc->len  = cpu_to_le32(skb->len << 16);
 
 	dma_wmb(); /* TACT bit must be set after all the above writes */
 	if (entry >= mdp->num_tx_ring - 1)
-		txdesc->status |= cpu_to_edmac(mdp, TD_TACT | TD_TDLE);
+		txdesc->status |= cpu_to_le32(TD_TACT | TD_TDLE);
 	else
-		txdesc->status |= cpu_to_edmac(mdp, TD_TACT);
+		txdesc->status |= cpu_to_le32(TD_TACT);
 
 	mdp->cur_tx++;
 
@@ -2911,7 +2856,7 @@ static int sh_mdio_release(struct sh_eth_private *mdp)
 static int sh_mdio_init(struct sh_eth_private *mdp,
 			struct sh_eth_plat_data *pd)
 {
-	int ret, i;
+	int ret;
 	struct bb_info *bitbang;
 	struct platform_device *pdev = mdp->pdev;
 	struct device *dev = &mdp->pdev->dev;
@@ -2924,10 +2869,6 @@ static int sh_mdio_init(struct sh_eth_private *mdp,
 	/* bitbang init */
 	bitbang->addr = mdp->addr + mdp->reg_offset[PIR];
 	bitbang->set_gate = pd->set_mdio_gate;
-	bitbang->mdi_msk = PIR_MDI;
-	bitbang->mdo_msk = PIR_MDO;
-	bitbang->mmd_msk = PIR_MMD;
-	bitbang->mdc_msk = PIR_MDC;
 	bitbang->ctrl.ops = &bb_ops;
 
 	/* MII controller setting */
@@ -2941,20 +2882,10 @@ static int sh_mdio_init(struct sh_eth_private *mdp,
 	snprintf(mdp->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		 pdev->name, pdev->id);
 
-	/* PHY IRQ */
-	mdp->mii_bus->irq = devm_kmalloc_array(dev, PHY_MAX_ADDR, sizeof(int),
-					       GFP_KERNEL);
-	if (!mdp->mii_bus->irq) {
-		ret = -ENOMEM;
-		goto out_free_bus;
-	}
-
 	/* register MDIO bus */
 	if (dev->of_node) {
 		ret = of_mdiobus_register(mdp->mii_bus, dev->of_node);
 	} else {
-		for (i = 0; i < PHY_MAX_ADDR; i++)
-			mdp->mii_bus->irq[i] = PHY_POLL;
 		if (pd->phy_irq > 0)
 			mdp->mii_bus->irq[pd->phy] = pd->phy_irq;
 
@@ -3126,21 +3057,15 @@ static int sh_eth_drv_probe(struct platform_device *pdev)
 	/* get PHY ID */
 	mdp->phy_id = pd->phy;
 	mdp->phy_interface = pd->phy_interface;
-	/* EDMAC endian */
-	mdp->edmac_endian = pd->edmac_endian;
 	mdp->no_ether_link = pd->no_ether_link;
 	mdp->ether_link_active_low = pd->ether_link_active_low;
 
 	/* set cpu data */
-	if (id) {
+	if (id)
 		mdp->cd = (struct sh_eth_cpu_data *)id->driver_data;
-	} else	{
-		const struct of_device_id *match;
+	else
+		mdp->cd = (struct sh_eth_cpu_data *)of_device_get_match_data(&pdev->dev);
 
-		match = of_match_device(of_match_ptr(sh_eth_match_table),
-					&pdev->dev);
-		mdp->cd = (struct sh_eth_cpu_data *)match->data;
-	}
 	mdp->reg_offset = sh_eth_get_register_offset(mdp->cd->register_type);
 	if (!mdp->reg_offset) {
 		dev_err(&pdev->dev, "Unknown register type (%d)\n",
@@ -3307,13 +3232,6 @@ static struct platform_device_id sh_eth_id_table[] = {
 	{ "sh7757-ether", (kernel_ulong_t)&sh7757_data },
 	{ "sh7757-gether", (kernel_ulong_t)&sh7757_data_giga },
 	{ "sh7763-gether", (kernel_ulong_t)&sh7763_data },
-	{ "r7s72100-ether", (kernel_ulong_t)&r7s72100_data },
-	{ "r8a7740-gether", (kernel_ulong_t)&r8a7740_data },
-	{ "r8a777x-ether", (kernel_ulong_t)&r8a777x_data },
-	{ "r8a7790-ether", (kernel_ulong_t)&r8a779x_data },
-	{ "r8a7791-ether", (kernel_ulong_t)&r8a779x_data },
-	{ "r8a7793-ether", (kernel_ulong_t)&r8a779x_data },
-	{ "r8a7794-ether", (kernel_ulong_t)&r8a779x_data },
 	{ }
 };
 MODULE_DEVICE_TABLE(platform, sh_eth_id_table);
diff --git a/drivers/net/ethernet/renesas/sh_eth.h b/drivers/net/ethernet/renesas/sh_eth.h
index 72fcfc924589..8fa4ef3a7fdd 100644
--- a/drivers/net/ethernet/renesas/sh_eth.h
+++ b/drivers/net/ethernet/renesas/sh_eth.h
@@ -513,7 +513,6 @@ struct sh_eth_private {
 	u32 cur_rx, dirty_rx;		/* Producer/consumer ring indices */
 	u32 cur_tx, dirty_tx;
 	u32 rx_buf_sz;			/* Based on MTU+slack. */
-	int edmac_endian;
 	struct napi_struct napi;
 	bool irq_enabled;
 	/* MII transceiver section. */
diff --git a/drivers/net/ethernet/rocker/rocker.c b/drivers/net/ethernet/rocker/rocker.c
index e9f2349e98bc..166a7fc87e2f 100644
--- a/drivers/net/ethernet/rocker/rocker.c
+++ b/drivers/net/ethernet/rocker/rocker.c
@@ -3531,12 +3531,14 @@ static void rocker_port_fdb_learn_work(struct work_struct *work)
 	info.addr = lw->addr;
 	info.vid = lw->vid;
 
+	rtnl_lock();
 	if (learned && removing)
 		call_switchdev_notifiers(SWITCHDEV_FDB_DEL,
 					 lw->rocker_port->dev, &info.info);
 	else if (learned && !removing)
 		call_switchdev_notifiers(SWITCHDEV_FDB_ADD,
 					 lw->rocker_port->dev, &info.info);
+	rtnl_unlock();
 
 	rocker_port_kfree(lw->trans, work);
 }
@@ -4998,7 +5000,7 @@ static int rocker_probe_port(struct rocker *rocker, unsigned int port_number)
 	dev->netdev_ops = &rocker_port_netdev_ops;
 	dev->ethtool_ops = &rocker_port_ethtool_ops;
 	dev->switchdev_ops = &rocker_port_switchdev_ops;
-	netif_napi_add(dev, &rocker_port->napi_tx, rocker_port_poll_tx,
+	netif_tx_napi_add(dev, &rocker_port->napi_tx, rocker_port_poll_tx,
 		       NAPI_POLL_WEIGHT);
 	netif_napi_add(dev, &rocker_port->napi_rx, rocker_port_poll_rx,
 		       NAPI_POLL_WEIGHT);
diff --git a/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c b/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
index 43ccb4a6de15..467ff7033606 100644
--- a/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
+++ b/drivers/net/ethernet/samsung/sxgbe/sxgbe_mdio.c
@@ -180,7 +180,7 @@ int sxgbe_mdio_register(struct net_device *ndev)
 	}
 
 	for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
-		struct phy_device *phy = mdio_bus->phy_map[phy_addr];
+		struct phy_device *phy = mdiobus_get_phy(mdio_bus, phy_addr);
 
 		if (phy) {
 			char irq_num[4];
@@ -216,7 +216,7 @@ int sxgbe_mdio_register(struct net_device *ndev)
 			}
 			netdev_info(ndev, "PHY ID %08x at %d IRQ %s (%s)%s\n",
 				    phy->phy_id, phy_addr, irq_str,
-				    dev_name(&phy->dev), act ? " active" : "");
+				    phydev_name(phy), act ? " active" : "");
 			phy_found = true;
 		}
 	}
diff --git a/drivers/net/ethernet/sfc/ef10.c b/drivers/net/ethernet/sfc/ef10.c
index e6a084a6be12..98d33d462c6c 100644
--- a/drivers/net/ethernet/sfc/ef10.c
+++ b/drivers/net/ethernet/sfc/ef10.c
@@ -181,13 +181,6 @@ static int efx_ef10_init_datapath_caps(struct efx_nic *efx)
 		MCDI_WORD(outbuf, GET_CAPABILITIES_OUT_TX_DPCPU_FW_ID);
 
 	if (!(nic_data->datapath_caps &
-	      (1 << MC_CMD_GET_CAPABILITIES_OUT_TX_TSO_LBN))) {
-		netif_err(efx, drv, efx->net_dev,
-			  "current firmware does not support TSO\n");
-		return -ENODEV;
-	}
-
-	if (!(nic_data->datapath_caps &
 	      (1 << MC_CMD_GET_CAPABILITIES_OUT_RX_PREFIX_LEN_14_LBN))) {
 		netif_err(efx, probe, efx->net_dev,
 			  "current firmware does not support an RX prefix\n");
@@ -493,10 +486,17 @@ static int efx_ef10_alloc_piobufs(struct efx_nic *efx, unsigned int n)
 	BUILD_BUG_ON(MC_CMD_ALLOC_PIOBUF_IN_LEN != 0);
 
 	for (i = 0; i < n; i++) {
-		rc = efx_mcdi_rpc(efx, MC_CMD_ALLOC_PIOBUF, NULL, 0,
-				  outbuf, sizeof(outbuf), &outlen);
-		if (rc)
+		rc = efx_mcdi_rpc_quiet(efx, MC_CMD_ALLOC_PIOBUF, NULL, 0,
+					outbuf, sizeof(outbuf), &outlen);
+		if (rc) {
+			/* Don't display the MC error if we didn't have space
+			 * for a VF.
+			 */
+			if (!(efx_ef10_is_vf(efx) && rc == -ENOSPC))
+				efx_mcdi_display_error(efx, MC_CMD_ALLOC_PIOBUF,
+						       0, outbuf, outlen, rc);
 			break;
+		}
 		if (outlen < MC_CMD_ALLOC_PIOBUF_OUT_LEN) {
 			rc = -EIO;
 			break;
@@ -1797,6 +1797,12 @@ static void efx_ef10_tx_init(struct efx_tx_queue *tx_queue)
 			     ESF_DZ_TX_OPTION_UDP_TCP_CSUM, csum_offload,
 			     ESF_DZ_TX_OPTION_IP_CSUM, csum_offload);
 	tx_queue->write_count = 1;
+
+	if (nic_data->datapath_caps &
+	    (1 << MC_CMD_GET_CAPABILITIES_OUT_TX_TSO_LBN)) {
+		tx_queue->tso_version = 1;
+	}
+
 	wmb();
 	efx_ef10_push_tx_desc(tx_queue, txd);
 
@@ -2375,8 +2381,19 @@ static int efx_ef10_ev_init(struct efx_channel *channel)
 				    1 << MC_CMD_WORKAROUND_EXT_OUT_FLR_DONE_LBN) {
 					netif_info(efx, drv, efx->net_dev,
 						   "other functions on NIC have been reset\n");
-					/* MC's boot count has incremented */
-					++nic_data->warm_boot_count;
+
+					/* With MCFW v4.6.x and earlier, the
+					 * boot count will have incremented,
+					 * so re-read the warm_boot_count
+					 * value now to ensure this function
+					 * doesn't think it has changed next
+					 * time it checks.
+					 */
+					rc = efx_ef10_get_warm_boot_count(efx);
+					if (rc >= 0) {
+						nic_data->warm_boot_count = rc;
+						rc = 0;
+					}
 				}
 				nic_data->workaround_26807 = true;
 			} else if (rc == -EPERM) {
@@ -3823,13 +3840,12 @@ static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 			       MC_CMD_FILTER_OP_IN_OP_UNSUBSCRIBE);
 		MCDI_SET_QWORD(inbuf, FILTER_OP_IN_HANDLE,
 			       table->entry[filter_idx].handle);
-		rc = efx_mcdi_rpc(efx, MC_CMD_FILTER_OP, inbuf, sizeof(inbuf),
-				  NULL, 0, NULL);
+		rc = efx_mcdi_rpc_quiet(efx, MC_CMD_FILTER_OP, inbuf,
+					sizeof(inbuf), NULL, 0, NULL);
 		if (rc)
-			netdev_WARN(efx->net_dev,
-				    "filter_idx=%#x handle=%#llx\n",
-				    filter_idx,
-				    table->entry[filter_idx].handle);
+			netif_info(efx, drv, efx->net_dev,
+				   "%s: filter %04x remove failed\n",
+				   __func__, filter_idx);
 		kfree(spec);
 	}
 
@@ -3838,11 +3854,14 @@ static void efx_ef10_filter_table_remove(struct efx_nic *efx)
 }
 
 #define EFX_EF10_FILTER_DO_MARK_OLD(id) \
-		if (id != EFX_EF10_FILTER_ID_INVALID) { \
-			filter_idx = efx_ef10_filter_get_unsafe_id(efx, id); \
-			WARN_ON(!table->entry[filter_idx].spec); \
-			table->entry[filter_idx].spec |= EFX_EF10_FILTER_FLAG_AUTO_OLD; \
-		}
+	if (id != EFX_EF10_FILTER_ID_INVALID) { \
+		filter_idx = efx_ef10_filter_get_unsafe_id(efx, id); \
+		if (!table->entry[filter_idx].spec) \
+			netif_dbg(efx, drv, efx->net_dev, \
+				  "%s: marked null spec old %04x:%04x\n", \
+				  __func__, id, filter_idx); \
+		table->entry[filter_idx].spec |= EFX_EF10_FILTER_FLAG_AUTO_OLD;\
+	}
 static void efx_ef10_filter_mark_old(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
@@ -4016,9 +4035,10 @@ static int efx_ef10_filter_insert_def(struct efx_nic *efx, bool multicast,
 
 	rc = efx_ef10_filter_insert(efx, &spec, true);
 	if (rc < 0) {
-		netif_warn(efx, drv, efx->net_dev,
-			   "%scast mismatch filter insert failed rc=%d\n",
-			   multicast ? "Multi" : "Uni", rc);
+		netif_printk(efx, drv, rc == -EPERM ? KERN_DEBUG : KERN_WARNING,
+			     efx->net_dev,
+			     "%scast mismatch filter insert failed rc=%d\n",
+			     multicast ? "Multi" : "Uni", rc);
 	} else if (multicast) {
 		table->mcdef_id = efx_ef10_filter_get_unsafe_id(efx, rc);
 		if (!nic_data->workaround_26807) {
@@ -4060,19 +4080,31 @@ static int efx_ef10_filter_insert_def(struct efx_nic *efx, bool multicast,
 static void efx_ef10_filter_remove_old(struct efx_nic *efx)
 {
 	struct efx_ef10_filter_table *table = efx->filter_state;
-	bool remove_failed = false;
+	int remove_failed = 0;
+	int remove_noent = 0;
+	int rc;
 	int i;
 
 	for (i = 0; i < HUNT_FILTER_TBL_ROWS; i++) {
 		if (ACCESS_ONCE(table->entry[i].spec) &
 		    EFX_EF10_FILTER_FLAG_AUTO_OLD) {
-			if (efx_ef10_filter_remove_internal(
-				    efx, 1U << EFX_FILTER_PRI_AUTO,
-				    i, true) < 0)
-				remove_failed = true;
+			rc = efx_ef10_filter_remove_internal(efx,
+					1U << EFX_FILTER_PRI_AUTO, i, true);
+			if (rc == -ENOENT)
+				remove_noent++;
+			else if (rc)
+				remove_failed++;
 		}
 	}
-	WARN_ON(remove_failed);
+
+	if (remove_failed)
+		netif_info(efx, drv, efx->net_dev,
+			   "%s: failed to remove %d filters\n",
+			   __func__, remove_failed);
+	if (remove_noent)
+		netif_info(efx, drv, efx->net_dev,
+			   "%s: failed to remove %d non-existent filters\n",
+			   __func__, remove_noent);
 }
 
 static int efx_ef10_vport_set_mac_address(struct efx_nic *efx)
diff --git a/drivers/net/ethernet/sfc/efx.c b/drivers/net/ethernet/sfc/efx.c
index a3c42a376741..0705ec869487 100644
--- a/drivers/net/ethernet/sfc/efx.c
+++ b/drivers/net/ethernet/sfc/efx.c
@@ -2059,7 +2059,6 @@ static void efx_init_napi_channel(struct efx_channel *channel)
 	channel->napi_dev = efx->net_dev;
 	netif_napi_add(channel->napi_dev, &channel->napi_str,
 		       efx_poll, napi_weight);
-	napi_hash_add(&channel->napi_str);
 	efx_channel_busy_poll_init(channel);
 }
 
@@ -2785,6 +2784,12 @@ static const struct pci_device_id efx_pci_table[] = {
 	 .driver_data = (unsigned long) &efx_hunt_a0_vf_nic_type},
 	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x0923),  /* SFC9140 PF */
 	 .driver_data = (unsigned long) &efx_hunt_a0_nic_type},
+	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x1923),  /* SFC9140 VF */
+	 .driver_data = (unsigned long) &efx_hunt_a0_vf_nic_type},
+	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x0a03),  /* SFC9220 PF */
+	 .driver_data = (unsigned long) &efx_hunt_a0_nic_type},
+	{PCI_DEVICE(PCI_VENDOR_ID_SOLARFLARE, 0x1a03),  /* SFC9220 VF */
+	 .driver_data = (unsigned long) &efx_hunt_a0_vf_nic_type},
 	{0}			/* end of list */
 };
 
@@ -3123,10 +3128,10 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 	net_dev->features |= (efx->type->offload_features | NETIF_F_SG |
 			      NETIF_F_HIGHDMA | NETIF_F_TSO |
 			      NETIF_F_RXCSUM);
-	if (efx->type->offload_features & NETIF_F_V6_CSUM)
+	if (efx->type->offload_features & (NETIF_F_IPV6_CSUM | NETIF_F_HW_CSUM))
 		net_dev->features |= NETIF_F_TSO6;
 	/* Mask for features that also apply to VLAN devices */
-	net_dev->vlan_features |= (NETIF_F_ALL_CSUM | NETIF_F_SG |
+	net_dev->vlan_features |= (NETIF_F_HW_CSUM | NETIF_F_SG |
 				   NETIF_F_HIGHDMA | NETIF_F_ALL_TSO |
 				   NETIF_F_RXCSUM);
 	/* All offloads can be toggled */
@@ -3169,14 +3174,15 @@ static int efx_pci_probe(struct pci_dev *pci_dev,
 	rtnl_lock();
 	rc = efx_mtd_probe(efx);
 	rtnl_unlock();
-	if (rc)
+	if (rc && rc != -EPERM)
 		netif_warn(efx, probe, efx->net_dev,
 			   "failed to create MTDs (%d)\n", rc);
 
 	rc = pci_enable_pcie_error_reporting(pci_dev);
 	if (rc && rc != -EINVAL)
-		netif_warn(efx, probe, efx->net_dev,
-			   "pci_enable_pcie_error_reporting failed (%d)\n", rc);
+		netif_notice(efx, probe, efx->net_dev,
+			     "PCIE error reporting unavailable (%d).\n",
+			     rc);
 
 	return 0;
 
diff --git a/drivers/net/ethernet/sfc/mcdi.c b/drivers/net/ethernet/sfc/mcdi.c
index 41fb6b60a3f0..d28e7dd8fa3c 100644
--- a/drivers/net/ethernet/sfc/mcdi.c
+++ b/drivers/net/ethernet/sfc/mcdi.c
@@ -82,6 +82,7 @@ int efx_mcdi_init(struct efx_nic *efx)
 	mcdi->logging_enabled = mcdi_logging_default;
 #endif
 	init_waitqueue_head(&mcdi->wq);
+	init_waitqueue_head(&mcdi->proxy_rx_wq);
 	spin_lock_init(&mcdi->iface_lock);
 	mcdi->state = MCDI_STATE_QUIESCENT;
 	mcdi->mode = MCDI_MODE_POLL;
@@ -315,6 +316,7 @@ static void efx_mcdi_read_response_header(struct efx_nic *efx)
 	}
 #endif
 
+	mcdi->resprc_raw = 0;
 	if (error && mcdi->resp_data_len == 0) {
 		netif_err(efx, hw, efx->net_dev, "MC rebooted\n");
 		mcdi->resprc = -EIO;
@@ -325,8 +327,8 @@ static void efx_mcdi_read_response_header(struct efx_nic *efx)
 		mcdi->resprc = -EIO;
 	} else if (error) {
 		efx->type->mcdi_read_response(efx, &hdr, mcdi->resp_hdr_len, 4);
-		mcdi->resprc =
-			efx_mcdi_errno(EFX_DWORD_FIELD(hdr, EFX_DWORD_0));
+		mcdi->resprc_raw = EFX_DWORD_FIELD(hdr, EFX_DWORD_0);
+		mcdi->resprc = efx_mcdi_errno(mcdi->resprc_raw);
 	} else {
 		mcdi->resprc = 0;
 	}
@@ -621,9 +623,30 @@ efx_mcdi_check_supported(struct efx_nic *efx, unsigned int cmd, size_t inlen)
 	return 0;
 }
 
-static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
+static bool efx_mcdi_get_proxy_handle(struct efx_nic *efx,
+				      size_t hdr_len, size_t data_len,
+				      u32 *proxy_handle)
+{
+	MCDI_DECLARE_BUF_ERR(testbuf);
+	const size_t buflen = sizeof(testbuf);
+
+	if (!proxy_handle || data_len < buflen)
+		return false;
+
+	efx->type->mcdi_read_response(efx, testbuf, hdr_len, buflen);
+	if (MCDI_DWORD(testbuf, ERR_CODE) == MC_CMD_ERR_PROXY_PENDING) {
+		*proxy_handle = MCDI_DWORD(testbuf, ERR_PROXY_PENDING_HANDLE);
+		return true;
+	}
+
+	return false;
+}
+
+static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned int cmd,
+				size_t inlen,
 				efx_dword_t *outbuf, size_t outlen,
-				size_t *outlen_actual, bool quiet)
+				size_t *outlen_actual, bool quiet,
+				u32 *proxy_handle, int *raw_rc)
 {
 	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
 	MCDI_DECLARE_BUF_ERR(errbuf);
@@ -657,6 +680,9 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 		spin_unlock_bh(&mcdi->iface_lock);
 	}
 
+	if (proxy_handle)
+		*proxy_handle = 0;
+
 	if (rc != 0) {
 		if (outlen_actual)
 			*outlen_actual = 0;
@@ -669,6 +695,8 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 		 * acquiring the iface_lock. */
 		spin_lock_bh(&mcdi->iface_lock);
 		rc = mcdi->resprc;
+		if (raw_rc)
+			*raw_rc = mcdi->resprc_raw;
 		hdr_len = mcdi->resp_hdr_len;
 		data_len = mcdi->resp_data_len;
 		err_len = min(sizeof(errbuf), data_len);
@@ -689,6 +717,12 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 			netif_err(efx, hw, efx->net_dev, "MC fatal error %d\n",
 				  -rc);
 			efx_schedule_reset(efx, RESET_TYPE_MC_FAILURE);
+		} else if (proxy_handle && (rc == -EPROTO) &&
+			   efx_mcdi_get_proxy_handle(efx, hdr_len, data_len,
+						     proxy_handle)) {
+			mcdi->proxy_rx_status = 0;
+			mcdi->proxy_rx_handle = 0;
+			mcdi->state = MCDI_STATE_PROXY_WAIT;
 		} else if (rc && !quiet) {
 			efx_mcdi_display_error(efx, cmd, inlen, errbuf, err_len,
 					       rc);
@@ -701,34 +735,195 @@ static int _efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 		}
 	}
 
-	efx_mcdi_release(mcdi);
+	if (!proxy_handle || !*proxy_handle)
+		efx_mcdi_release(mcdi);
 	return rc;
 }
 
-static int _efx_mcdi_rpc(struct efx_nic *efx, unsigned cmd,
+static void efx_mcdi_proxy_abort(struct efx_mcdi_iface *mcdi)
+{
+	if (mcdi->state == MCDI_STATE_PROXY_WAIT) {
+		/* Interrupt the proxy wait. */
+		mcdi->proxy_rx_status = -EINTR;
+		wake_up(&mcdi->proxy_rx_wq);
+	}
+}
+
+static void efx_mcdi_ev_proxy_response(struct efx_nic *efx,
+				       u32 handle, int status)
+{
+	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+
+	WARN_ON(mcdi->state != MCDI_STATE_PROXY_WAIT);
+
+	mcdi->proxy_rx_status = efx_mcdi_errno(status);
+	/* Ensure the status is written before we update the handle, since the
+	 * latter is used to check if we've finished.
+	 */
+	wmb();
+	mcdi->proxy_rx_handle = handle;
+	wake_up(&mcdi->proxy_rx_wq);
+}
+
+static int efx_mcdi_proxy_wait(struct efx_nic *efx, u32 handle, bool quiet)
+{
+	struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+	int rc;
+
+	/* Wait for a proxy event, or timeout. */
+	rc = wait_event_timeout(mcdi->proxy_rx_wq,
+				mcdi->proxy_rx_handle != 0 ||
+				mcdi->proxy_rx_status == -EINTR,
+				MCDI_RPC_TIMEOUT);
+
+	if (rc <= 0) {
+		netif_dbg(efx, hw, efx->net_dev,
+			  "MCDI proxy timeout %d\n", handle);
+		return -ETIMEDOUT;
+	} else if (mcdi->proxy_rx_handle != handle) {
+		netif_warn(efx, hw, efx->net_dev,
+			   "MCDI proxy unexpected handle %d (expected %d)\n",
+			   mcdi->proxy_rx_handle, handle);
+		return -EINVAL;
+	}
+
+	return mcdi->proxy_rx_status;
+}
+
+static int _efx_mcdi_rpc(struct efx_nic *efx, unsigned int cmd,
 			 const efx_dword_t *inbuf, size_t inlen,
 			 efx_dword_t *outbuf, size_t outlen,
-			 size_t *outlen_actual, bool quiet)
+			 size_t *outlen_actual, bool quiet, int *raw_rc)
 {
+	u32 proxy_handle = 0; /* Zero is an invalid proxy handle. */
 	int rc;
 
+	if (inbuf && inlen && (inbuf == outbuf)) {
+		/* The input buffer can't be aliased with the output. */
+		WARN_ON(1);
+		return -EINVAL;
+	}
+
 	rc = efx_mcdi_rpc_start(efx, cmd, inbuf, inlen);
-	if (rc) {
-		if (outlen_actual)
-			*outlen_actual = 0;
+	if (rc)
 		return rc;
+
+	rc = _efx_mcdi_rpc_finish(efx, cmd, inlen, outbuf, outlen,
+				  outlen_actual, quiet, &proxy_handle, raw_rc);
+
+	if (proxy_handle) {
+		/* Handle proxy authorisation. This allows approval of MCDI
+		 * operations to be delegated to the admin function, allowing
+		 * fine control over (eg) multicast subscriptions.
+		 */
+		struct efx_mcdi_iface *mcdi = efx_mcdi(efx);
+
+		netif_dbg(efx, hw, efx->net_dev,
+			  "MCDI waiting for proxy auth %d\n",
+			  proxy_handle);
+		rc = efx_mcdi_proxy_wait(efx, proxy_handle, quiet);
+
+		if (rc == 0) {
+			netif_dbg(efx, hw, efx->net_dev,
+				  "MCDI proxy retry %d\n", proxy_handle);
+
+			/* We now retry the original request. */
+			mcdi->state = MCDI_STATE_RUNNING_SYNC;
+			efx_mcdi_send_request(efx, cmd, inbuf, inlen);
+
+			rc = _efx_mcdi_rpc_finish(efx, cmd, inlen,
+						  outbuf, outlen, outlen_actual,
+						  quiet, NULL, raw_rc);
+		} else {
+			netif_printk(efx, hw,
+				     rc == -EPERM ? KERN_DEBUG : KERN_ERR,
+				     efx->net_dev,
+				     "MC command 0x%x failed after proxy auth rc=%d\n",
+				     cmd, rc);
+
+			if (rc == -EINTR || rc == -EIO)
+				efx_schedule_reset(efx, RESET_TYPE_MC_FAILURE);
+			efx_mcdi_release(mcdi);
+		}
 	}
-	return _efx_mcdi_rpc_finish(efx, cmd, inlen, outbuf, outlen,
-				    outlen_actual, quiet);
+
+	return rc;
 }
 
+static int _efx_mcdi_rpc_evb_retry(struct efx_nic *efx, unsigned cmd,
+				   const efx_dword_t *inbuf, size_t inlen,
+				   efx_dword_t *outbuf, size_t outlen,
+				   size_t *outlen_actual, bool quiet)
+{
+	int raw_rc = 0;
+	int rc;
+
+	rc = _efx_mcdi_rpc(efx, cmd, inbuf, inlen,
+			   outbuf, outlen, outlen_actual, true, &raw_rc);
+
+	if ((rc == -EPROTO) && (raw_rc == MC_CMD_ERR_NO_EVB_PORT) &&
+	    efx->type->is_vf) {
+		/* If the EVB port isn't available within a VF this may
+		 * mean the PF is still bringing the switch up. We should
+		 * retry our request shortly.
+		 */
+		unsigned long abort_time = jiffies + MCDI_RPC_TIMEOUT;
+		unsigned int delay_us = 10000;
+
+		netif_dbg(efx, hw, efx->net_dev,
+			  "%s: NO_EVB_PORT; will retry request\n",
+			  __func__);
+
+		do {
+			usleep_range(delay_us, delay_us + 10000);
+			rc = _efx_mcdi_rpc(efx, cmd, inbuf, inlen,
+					   outbuf, outlen, outlen_actual,
+					   true, &raw_rc);
+			if (delay_us < 100000)
+				delay_us <<= 1;
+		} while ((rc == -EPROTO) &&
+			 (raw_rc == MC_CMD_ERR_NO_EVB_PORT) &&
+			 time_before(jiffies, abort_time));
+	}
+
+	if (rc && !quiet && !(cmd == MC_CMD_REBOOT && rc == -EIO))
+		efx_mcdi_display_error(efx, cmd, inlen,
+				       outbuf, outlen, rc);
+
+	return rc;
+}
+
+/**
+ * efx_mcdi_rpc - Issue an MCDI command and wait for completion
+ * @efx: NIC through which to issue the command
+ * @cmd: Command type number
+ * @inbuf: Command parameters
+ * @inlen: Length of command parameters, in bytes.  Must be a multiple
+ *	of 4 and no greater than %MCDI_CTL_SDU_LEN_MAX_V1.
+ * @outbuf: Response buffer.  May be %NULL if @outlen is 0.
+ * @outlen: Length of response buffer, in bytes.  If the actual
+ *	response is longer than @outlen & ~3, it will be truncated
+ *	to that length.
+ * @outlen_actual: Pointer through which to return the actual response
+ *	length.  May be %NULL if this is not needed.
+ *
+ * This function may sleep and therefore must be called in an appropriate
+ * context.
+ *
+ * Return: A negative error code, or zero if successful.  The error
+ *	code may come from the MCDI response or may indicate a failure
+ *	to communicate with the MC.  In the former case, the response
+ *	will still be copied to @outbuf and *@outlen_actual will be
+ *	set accordingly.  In the latter case, *@outlen_actual will be
+ *	set to zero.
+ */
 int efx_mcdi_rpc(struct efx_nic *efx, unsigned cmd,
 		 const efx_dword_t *inbuf, size_t inlen,
 		 efx_dword_t *outbuf, size_t outlen,
 		 size_t *outlen_actual)
 {
-	return _efx_mcdi_rpc(efx, cmd, inbuf, inlen, outbuf, outlen,
-			     outlen_actual, false);
+	return _efx_mcdi_rpc_evb_retry(efx, cmd, inbuf, inlen, outbuf, outlen,
+				       outlen_actual, false);
 }
 
 /* Normally, on receiving an error code in the MCDI response,
@@ -744,8 +939,8 @@ int efx_mcdi_rpc_quiet(struct efx_nic *efx, unsigned cmd,
 		       efx_dword_t *outbuf, size_t outlen,
 		       size_t *outlen_actual)
 {
-	return _efx_mcdi_rpc(efx, cmd, inbuf, inlen, outbuf, outlen,
-			     outlen_actual, true);
+	return _efx_mcdi_rpc_evb_retry(efx, cmd, inbuf, inlen, outbuf, outlen,
+				       outlen_actual, true);
 }
 
 int efx_mcdi_rpc_start(struct efx_nic *efx, unsigned cmd,
@@ -866,7 +1061,7 @@ int efx_mcdi_rpc_finish(struct efx_nic *efx, unsigned cmd, size_t inlen,
 			size_t *outlen_actual)
 {
 	return _efx_mcdi_rpc_finish(efx, cmd, inlen, outbuf, outlen,
-				    outlen_actual, false);
+				    outlen_actual, false, NULL, NULL);
 }
 
 int efx_mcdi_rpc_finish_quiet(struct efx_nic *efx, unsigned cmd, size_t inlen,
@@ -874,7 +1069,7 @@ int efx_mcdi_rpc_finish_quiet(struct efx_nic *efx, unsigned cmd, size_t inlen,
 			      size_t *outlen_actual)
 {
 	return _efx_mcdi_rpc_finish(efx, cmd, inlen, outbuf, outlen,
-				    outlen_actual, true);
+				    outlen_actual, true, NULL, NULL);
 }
 
 void efx_mcdi_display_error(struct efx_nic *efx, unsigned cmd,
@@ -887,9 +1082,10 @@ void efx_mcdi_display_error(struct efx_nic *efx, unsigned cmd,
 		code = MCDI_DWORD(outbuf, ERR_CODE);
 	if (outlen >= MC_CMD_ERR_ARG_OFST + 4)
 		err_arg = MCDI_DWORD(outbuf, ERR_ARG);
-	netif_err(efx, hw, efx->net_dev,
-		  "MC command 0x%x inlen %d failed rc=%d (raw=%d) arg=%d\n",
-		  cmd, (int)inlen, rc, code, err_arg);
+	netif_printk(efx, hw, rc == -EPERM ? KERN_DEBUG : KERN_ERR,
+		     efx->net_dev,
+		     "MC command 0x%x inlen %zu failed rc=%d (raw=%d) arg=%d\n",
+		     cmd, inlen, rc, code, err_arg);
 }
 
 /* Switch to polled MCDI completions.  This can be called in various
@@ -1014,8 +1210,13 @@ static void efx_mcdi_ev_death(struct efx_nic *efx, int rc)
 	 * receiving a REBOOT event after posting the MCDI
 	 * request. Did the mc reboot before or after the copyout? The
 	 * best we can do always is just return failure.
+	 *
+	 * If there is an outstanding proxy response expected it is not going
+	 * to arrive. We should thus abort it.
 	 */
 	spin_lock(&mcdi->iface_lock);
+	efx_mcdi_proxy_abort(mcdi);
+
 	if (efx_mcdi_complete_sync(mcdi)) {
 		if (mcdi->mode == MCDI_MODE_EVENTS) {
 			mcdi->resprc = rc;
@@ -1063,6 +1264,8 @@ static void efx_mcdi_ev_bist(struct efx_nic *efx)
 
 	spin_lock(&mcdi->iface_lock);
 	efx->mc_bist_for_other_fn = true;
+	efx_mcdi_proxy_abort(mcdi);
+
 	if (efx_mcdi_complete_sync(mcdi)) {
 		if (mcdi->mode == MCDI_MODE_EVENTS) {
 			mcdi->resprc = -EIO;
@@ -1171,6 +1374,11 @@ void efx_mcdi_process_event(struct efx_channel *channel,
 			  EFX_QWORD_VAL(*event));
 		efx_schedule_reset(efx, RESET_TYPE_DMA_ERROR);
 		break;
+	case MCDI_EVENT_CODE_PROXY_RESPONSE:
+		efx_mcdi_ev_proxy_response(efx,
+				MCDI_EVENT_FIELD(*event, PROXY_RESPONSE_HANDLE),
+				MCDI_EVENT_FIELD(*event, PROXY_RESPONSE_RC));
+		break;
 	default:
 		netif_err(efx, hw, efx->net_dev, "Unknown MCDI event 0x%x\n",
 			  code);
diff --git a/drivers/net/ethernet/sfc/mcdi.h b/drivers/net/ethernet/sfc/mcdi.h
index 025d504c472b..c9aeb0701c9a 100644
--- a/drivers/net/ethernet/sfc/mcdi.h
+++ b/drivers/net/ethernet/sfc/mcdi.h
@@ -17,6 +17,8 @@
  * @MCDI_STATE_RUNNING_SYNC: There is a synchronous MCDI request pending.
  *	Only the thread that moved into this state is allowed to move out of it.
  * @MCDI_STATE_RUNNING_ASYNC: There is an asynchronous MCDI request pending.
+ * @MCDI_STATE_PROXY_WAIT: An MCDI request has completed with a response that
+ *	indicates we must wait for a proxy try again message.
  * @MCDI_STATE_COMPLETED: An MCDI request has completed, but the owning thread
  *	has not yet consumed the result. For all other threads, equivalent to
  *	%MCDI_STATE_RUNNING.
@@ -25,6 +27,7 @@ enum efx_mcdi_state {
 	MCDI_STATE_QUIESCENT,
 	MCDI_STATE_RUNNING_SYNC,
 	MCDI_STATE_RUNNING_ASYNC,
+	MCDI_STATE_PROXY_WAIT,
 	MCDI_STATE_COMPLETED,
 };
 
@@ -60,6 +63,9 @@ enum efx_mcdi_mode {
  * @async_timer: Timer for asynchronous request timeout
  * @logging_buffer: buffer that may be used to build MCDI tracing messages
  * @logging_enabled: whether to trace MCDI
+ * @proxy_rx_handle: Most recently received proxy authorisation handle
+ * @proxy_rx_status: Status of most recent proxy authorisation
+ * @proxy_rx_wq: Wait queue for updates to proxy_rx_handle
  */
 struct efx_mcdi_iface {
 	struct efx_nic *efx;
@@ -71,6 +77,7 @@ struct efx_mcdi_iface {
 	unsigned int credits;
 	unsigned int seqno;
 	int resprc;
+	int resprc_raw;
 	size_t resp_hdr_len;
 	size_t resp_data_len;
 	spinlock_t async_lock;
@@ -80,6 +87,9 @@ struct efx_mcdi_iface {
 	char *logging_buffer;
 	bool logging_enabled;
 #endif
+	unsigned int proxy_rx_handle;
+	int proxy_rx_status;
+	wait_queue_head_t proxy_rx_wq;
 };
 
 struct efx_mcdi_mon {
diff --git a/drivers/net/ethernet/sfc/net_driver.h b/drivers/net/ethernet/sfc/net_driver.h
index a8ddd122f685..38c422321cda 100644
--- a/drivers/net/ethernet/sfc/net_driver.h
+++ b/drivers/net/ethernet/sfc/net_driver.h
@@ -182,6 +182,7 @@ struct efx_tx_buffer {
  *
  * @efx: The associated Efx NIC
  * @queue: DMA queue number
+ * @tso_version: Version of TSO in use for this queue.
  * @channel: The associated channel
  * @core_txq: The networking core TX queue structure
  * @buffer: The software buffer ring
@@ -228,6 +229,7 @@ struct efx_tx_queue {
 	/* Members which don't change on the fast path */
 	struct efx_nic *efx ____cacheline_aligned_in_smp;
 	unsigned queue;
+	unsigned int tso_version;
 	struct efx_channel *channel;
 	struct netdev_queue *core_txq;
 	struct efx_tx_buffer *buffer;
@@ -1502,8 +1504,9 @@ static inline struct efx_rx_buffer *efx_rx_buffer(struct efx_rx_queue *rx_queue,
  * same cycle, the XMAC can miss the IPG altogether.  We work around
  * this by adding a further 16 bytes.
  */
+#define EFX_FRAME_PAD	16
 #define EFX_MAX_FRAME_LEN(mtu) \
-	((((mtu) + ETH_HLEN + VLAN_HLEN + 4/* FCS */ + 7) & ~7) + 16)
+	(ALIGN(((mtu) + ETH_HLEN + VLAN_HLEN + ETH_FCS_LEN + EFX_FRAME_PAD), 8))
 
 static inline bool efx_xmit_with_hwtstamp(struct sk_buff *skb)
 {
diff --git a/drivers/net/ethernet/sfc/rx.c b/drivers/net/ethernet/sfc/rx.c
index 809ea4610a77..8956995b2fe7 100644
--- a/drivers/net/ethernet/sfc/rx.c
+++ b/drivers/net/ethernet/sfc/rx.c
@@ -463,7 +463,6 @@ efx_rx_packet_gro(struct efx_channel *channel, struct efx_rx_buffer *rx_buf,
 
 	skb_record_rx_queue(skb, channel->rx_queue.core_index);
 
-	skb_mark_napi_id(skb, &channel->napi_str);
 	gro_result = napi_gro_frags(napi);
 	if (gro_result != GRO_DROP)
 		channel->irq_mod_score += 2;
diff --git a/drivers/net/ethernet/sfc/tx.c b/drivers/net/ethernet/sfc/tx.c
index 67f6afaa022f..f7a0ec1bca97 100644
--- a/drivers/net/ethernet/sfc/tx.c
+++ b/drivers/net/ethernet/sfc/tx.c
@@ -1010,13 +1010,17 @@ static void efx_enqueue_unwind(struct efx_tx_queue *tx_queue,
 
 /* Parse the SKB header and initialise state. */
 static int tso_start(struct tso_state *st, struct efx_nic *efx,
+		     struct efx_tx_queue *tx_queue,
 		     const struct sk_buff *skb)
 {
-	bool use_opt_desc = efx_nic_rev(efx) >= EFX_REV_HUNT_A0;
 	struct device *dma_dev = &efx->pci_dev->dev;
 	unsigned int header_len, in_len;
+	bool use_opt_desc = false;
 	dma_addr_t dma_addr;
 
+	if (tx_queue->tso_version == 1)
+		use_opt_desc = true;
+
 	st->ip_off = skb_network_header(skb) - skb->data;
 	st->tcp_off = skb_transport_header(skb) - skb->data;
 	header_len = st->tcp_off + (tcp_hdr(skb)->doff << 2u);
@@ -1271,7 +1275,7 @@ static int efx_enqueue_skb_tso(struct efx_tx_queue *tx_queue,
 	/* Find the packet protocol and sanity-check it */
 	state.protocol = efx_tso_check_protocol(skb);
 
-	rc = tso_start(&state, efx, skb);
+	rc = tso_start(&state, efx, tx_queue, skb);
 	if (rc)
 		goto mem_err;
 
diff --git a/drivers/net/ethernet/smsc/Kconfig b/drivers/net/ethernet/smsc/Kconfig
index eb9230e2092f..63aca9f847e1 100644
--- a/drivers/net/ethernet/smsc/Kconfig
+++ b/drivers/net/ethernet/smsc/Kconfig
@@ -7,7 +7,7 @@ config NET_VENDOR_SMSC
 	default y
 	depends on ARM || ARM64 || ATARI_ETHERNAT || BLACKFIN || COLDFIRE || \
 		   ISA || M32R || MAC || MIPS || MN10300 || NIOS2 || PCI || \
-		   PCMCIA || SUPERH || XTENSA
+		   PCMCIA || SUPERH || XTENSA || H8300
 	---help---
 	  If you have a network (Ethernet) card belonging to this class, say Y.
 
@@ -38,7 +38,7 @@ config SMC91X
 	select MII
 	depends on !OF || GPIOLIB
 	depends on ARM || ARM64 || ATARI_ETHERNAT || BLACKFIN || COLDFIRE || \
-		   M32R || MIPS || MN10300 || NIOS2 || SUPERH || XTENSA
+		   M32R || MIPS || MN10300 || NIOS2 || SUPERH || XTENSA || H8300
 	---help---
 	  This is a driver for SMC's 91x series of Ethernet chipsets,
 	  including the SMC91C94 and the SMC91C111. Say Y if you want it
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index 0e2fc1a844ab..db7db8ac4ca3 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -2342,8 +2342,8 @@ static int smc_drv_probe(struct platform_device *pdev)
 	}
 
 	ndev->irq = platform_get_irq(pdev, 0);
-	if (ndev->irq <= 0) {
-		ret = -ENODEV;
+	if (ndev->irq < 0) {
+		ret = ndev->irq;
 		goto out_release_io;
 	}
 	/*
diff --git a/drivers/net/ethernet/smsc/smc91x.h b/drivers/net/ethernet/smsc/smc91x.h
index a3c129e1e40a..1a55c7976df0 100644
--- a/drivers/net/ethernet/smsc/smc91x.h
+++ b/drivers/net/ethernet/smsc/smc91x.h
@@ -172,6 +172,17 @@ static inline void mcf_outsw(void *a, unsigned char *p, int l)
 
 #define SMC_IRQ_FLAGS		0
 
+#elif defined(CONFIG_H8300)
+#define SMC_CAN_USE_8BIT	1
+#define SMC_CAN_USE_16BIT	0
+#define SMC_CAN_USE_32BIT	0
+#define SMC_NOWAIT		0
+
+#define SMC_inb(a, r)		ioread8((a) + (r))
+#define SMC_outb(v, a, r)	iowrite8(v, (a) + (r))
+#define SMC_insb(a, r, p, l)	ioread8_rep((a) + (r), p, l)
+#define SMC_outsb(a, r, p, l)	iowrite8_rep((a) + (r), p, l)
+
 #else
 
 /*
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index 219a99b7a631..8af25563f627 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -864,8 +864,8 @@ static int smsc911x_phy_loopbacktest(struct net_device *dev)
 
 	for (i = 0; i < 10; i++) {
 		/* Set PHY to 10/FD, no ANEG, and loopback mode */
-		smsc911x_mii_write(phy_dev->bus, phy_dev->addr,	MII_BMCR,
-			BMCR_LOOPBACK | BMCR_FULLDPLX);
+		smsc911x_mii_write(phy_dev->mdio.bus, phy_dev->mdio.addr,
+				   MII_BMCR, BMCR_LOOPBACK | BMCR_FULLDPLX);
 
 		/* Enable MAC tx/rx, FD */
 		spin_lock_irqsave(&pdata->mac_lock, flags);
@@ -893,7 +893,7 @@ static int smsc911x_phy_loopbacktest(struct net_device *dev)
 	spin_unlock_irqrestore(&pdata->mac_lock, flags);
 
 	/* Cancel PHY loopback mode */
-	smsc911x_mii_write(phy_dev->bus, phy_dev->addr, MII_BMCR, 0);
+	smsc911x_mii_write(phy_dev->mdio.bus, phy_dev->mdio.addr, MII_BMCR, 0);
 
 	smsc911x_reg_write(pdata, TX_CFG, 0);
 	smsc911x_reg_write(pdata, RX_CFG, 0);
@@ -1021,7 +1021,7 @@ static int smsc911x_mii_probe(struct net_device *dev)
 	}
 
 	SMSC_TRACE(pdata, probe, "PHY: addr %d, phy_id 0x%08X",
-		   phydev->addr, phydev->phy_id);
+		   phydev->mdio.addr, phydev->phy_id);
 
 	ret = phy_connect_direct(dev, phydev, &smsc911x_phy_adjust_link,
 				 pdata->config.phy_interface);
@@ -1031,9 +1031,7 @@ static int smsc911x_mii_probe(struct net_device *dev)
 		return ret;
 	}
 
-	netdev_info(dev,
-		    "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		    phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
+	phy_attached_info(phydev);
 
 	/* mask with MAC supported features */
 	phydev->supported &= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
@@ -1061,7 +1059,7 @@ static int smsc911x_mii_init(struct platform_device *pdev,
 			     struct net_device *dev)
 {
 	struct smsc911x_data *pdata = netdev_priv(dev);
-	int err = -ENXIO, i;
+	int err = -ENXIO;
 
 	pdata->mii_bus = mdiobus_alloc();
 	if (!pdata->mii_bus) {
@@ -1075,9 +1073,7 @@ static int smsc911x_mii_init(struct platform_device *pdev,
 	pdata->mii_bus->priv = pdata;
 	pdata->mii_bus->read = smsc911x_mii_read;
 	pdata->mii_bus->write = smsc911x_mii_write;
-	pdata->mii_bus->irq = pdata->phy_irq;
-	for (i = 0; i < PHY_MAX_ADDR; ++i)
-		pdata->mii_bus->irq[i] = PHY_POLL;
+	memcpy(pdata->mii_bus->irq, pdata->phy_irq, sizeof(pdata->mii_bus));
 
 	pdata->mii_bus->parent = &pdev->dev;
 
@@ -1992,7 +1988,8 @@ smsc911x_ethtool_getregs(struct net_device *dev, struct ethtool_regs *regs,
 	}
 
 	for (i = 0; i <= 31; i++)
-		data[j++] = smsc911x_mii_read(phy_dev->bus, phy_dev->addr, i);
+		data[j++] = smsc911x_mii_read(phy_dev->mdio.bus,
+					      phy_dev->mdio.addr, i);
 }
 
 static void smsc911x_eeprom_enable_access(struct smsc911x_data *pdata)
diff --git a/drivers/net/ethernet/smsc/smsc9420.c b/drivers/net/ethernet/smsc/smsc9420.c
index 4a90cdae5444..8594b9e8b28b 100644
--- a/drivers/net/ethernet/smsc/smsc9420.c
+++ b/drivers/net/ethernet/smsc/smsc9420.c
@@ -78,7 +78,6 @@ struct smsc9420_pdata {
 
 	struct phy_device *phy_dev;
 	struct mii_bus *mii_bus;
-	int phy_irq[PHY_MAX_ADDR];
 	int last_duplex;
 	int last_carrier;
 };
@@ -316,7 +315,8 @@ smsc9420_ethtool_getregs(struct net_device *dev, struct ethtool_regs *regs,
 		return;
 
 	for (i = 0; i <= 31; i++)
-		data[j++] = smsc9420_mii_read(phy_dev->bus, phy_dev->addr, i);
+		data[j++] = smsc9420_mii_read(phy_dev->mdio.bus,
+					      phy_dev->mdio.addr, i);
 }
 
 static void smsc9420_eeprom_enable_access(struct smsc9420_pdata *pd)
@@ -1158,16 +1158,13 @@ static int smsc9420_mii_probe(struct net_device *dev)
 	BUG_ON(pd->phy_dev);
 
 	/* Device only supports internal PHY at address 1 */
-	if (!pd->mii_bus->phy_map[1]) {
+	phydev = mdiobus_get_phy(pd->mii_bus, 1);
+	if (!phydev) {
 		netdev_err(dev, "no PHY found at address 1\n");
 		return -ENODEV;
 	}
 
-	phydev = pd->mii_bus->phy_map[1];
-	netif_info(pd, probe, pd->dev, "PHY addr %d, phy_id 0x%08X\n",
-		   phydev->addr, phydev->phy_id);
-
-	phydev = phy_connect(dev, dev_name(&phydev->dev),
+	phydev = phy_connect(dev, phydev_name(phydev),
 			     smsc9420_phy_adjust_link, PHY_INTERFACE_MODE_MII);
 
 	if (IS_ERR(phydev)) {
@@ -1175,14 +1172,13 @@ static int smsc9420_mii_probe(struct net_device *dev)
 		return PTR_ERR(phydev);
 	}
 
-	netdev_info(dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
-		    phydev->drv->name, dev_name(&phydev->dev), phydev->irq);
-
 	/* mask with MAC supported features */
 	phydev->supported &= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
 			      SUPPORTED_Asym_Pause);
 	phydev->advertising = phydev->supported;
 
+	phy_attached_info(phydev);
+
 	pd->phy_dev = phydev;
 	pd->last_duplex = -1;
 	pd->last_carrier = -1;
@@ -1193,7 +1189,7 @@ static int smsc9420_mii_probe(struct net_device *dev)
 static int smsc9420_mii_init(struct net_device *dev)
 {
 	struct smsc9420_pdata *pd = netdev_priv(dev);
-	int err = -ENXIO, i;
+	int err = -ENXIO;
 
 	pd->mii_bus = mdiobus_alloc();
 	if (!pd->mii_bus) {
@@ -1206,9 +1202,6 @@ static int smsc9420_mii_init(struct net_device *dev)
 	pd->mii_bus->priv = pd;
 	pd->mii_bus->read = smsc9420_mii_read;
 	pd->mii_bus->write = smsc9420_mii_write;
-	pd->mii_bus->irq = pd->phy_irq;
-	for (i = 0; i < PHY_MAX_ADDR; ++i)
-		pd->mii_bus->irq[i] = PHY_POLL;
 
 	/* Mask all PHYs except ID 1 (internal) */
 	pd->mii_bus->phy_mask = ~(1 << 1);
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index 623c6ed8764a..1e19c8fd8b82 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -137,6 +137,31 @@ struct stmmac_extra_stats {
 	unsigned long pcs_link;
 	unsigned long pcs_duplex;
 	unsigned long pcs_speed;
+	/* debug register */
+	unsigned long mtl_tx_status_fifo_full;
+	unsigned long mtl_tx_fifo_not_empty;
+	unsigned long mmtl_fifo_ctrl;
+	unsigned long mtl_tx_fifo_read_ctrl_write;
+	unsigned long mtl_tx_fifo_read_ctrl_wait;
+	unsigned long mtl_tx_fifo_read_ctrl_read;
+	unsigned long mtl_tx_fifo_read_ctrl_idle;
+	unsigned long mac_tx_in_pause;
+	unsigned long mac_tx_frame_ctrl_xfer;
+	unsigned long mac_tx_frame_ctrl_idle;
+	unsigned long mac_tx_frame_ctrl_wait;
+	unsigned long mac_tx_frame_ctrl_pause;
+	unsigned long mac_gmii_tx_proto_engine;
+	unsigned long mtl_rx_fifo_fill_level_full;
+	unsigned long mtl_rx_fifo_fill_above_thresh;
+	unsigned long mtl_rx_fifo_fill_below_thresh;
+	unsigned long mtl_rx_fifo_fill_level_empty;
+	unsigned long mtl_rx_fifo_read_ctrl_flush;
+	unsigned long mtl_rx_fifo_read_ctrl_read_data;
+	unsigned long mtl_rx_fifo_read_ctrl_status;
+	unsigned long mtl_rx_fifo_read_ctrl_idle;
+	unsigned long mtl_rx_fifo_ctrl_active;
+	unsigned long mac_rx_frame_ctrl_fifo;
+	unsigned long mac_gmii_rx_proto_engine;
 };
 
 /* CSR Frequency Access Defines*/
@@ -408,12 +433,13 @@ struct stmmac_ops {
 	void (*set_eee_pls)(struct mac_device_info *hw, int link);
 	void (*ctrl_ane)(struct mac_device_info *hw, bool restart);
 	void (*get_adv)(struct mac_device_info *hw, struct rgmii_adv *adv);
+	void (*debug)(void __iomem *ioaddr, struct stmmac_extra_stats *x);
 };
 
 /* PTP and HW Timer helpers */
 struct stmmac_hwtimestamp {
 	void (*config_hw_tstamping) (void __iomem *ioaddr, u32 data);
-	void (*config_sub_second_increment) (void __iomem *ioaddr);
+	u32 (*config_sub_second_increment) (void __iomem *ioaddr, u32 clk_rate);
 	int (*init_systime) (void __iomem *ioaddr, u32 sec, u32 nsec);
 	int (*config_addend) (void __iomem *ioaddr, u32 addend);
 	int (*adjust_systime) (void __iomem *ioaddr, u32 sec, u32 nsec,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
index 82de68b1a452..36d3355f2fb0 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-ipq806x.c
@@ -198,19 +198,19 @@ static int ipq806x_gmac_set_speed(struct ipq806x_gmac *gmac, unsigned int speed)
 	return 0;
 }
 
-static void *ipq806x_gmac_of_parse(struct ipq806x_gmac *gmac)
+static int ipq806x_gmac_of_parse(struct ipq806x_gmac *gmac)
 {
 	struct device *dev = &gmac->pdev->dev;
 
 	gmac->phy_mode = of_get_phy_mode(dev->of_node);
 	if (gmac->phy_mode < 0) {
 		dev_err(dev, "missing phy mode property\n");
-		return ERR_PTR(-EINVAL);
+		return -EINVAL;
 	}
 
 	if (of_property_read_u32(dev->of_node, "qcom,id", &gmac->id) < 0) {
 		dev_err(dev, "missing qcom id property\n");
-		return ERR_PTR(-EINVAL);
+		return -EINVAL;
 	}
 
 	/* The GMACs are called 1 to 4 in the documentation, but to simplify the
@@ -219,13 +219,13 @@ static void *ipq806x_gmac_of_parse(struct ipq806x_gmac *gmac)
 	 */
 	if (gmac->id < 0 || gmac->id > 3) {
 		dev_err(dev, "invalid gmac id\n");
-		return ERR_PTR(-EINVAL);
+		return -EINVAL;
 	}
 
 	gmac->core_clk = devm_clk_get(dev, "stmmaceth");
 	if (IS_ERR(gmac->core_clk)) {
 		dev_err(dev, "missing stmmaceth clk property\n");
-		return gmac->core_clk;
+		return PTR_ERR(gmac->core_clk);
 	}
 	clk_set_rate(gmac->core_clk, 266000000);
 
@@ -234,18 +234,16 @@ static void *ipq806x_gmac_of_parse(struct ipq806x_gmac *gmac)
 							   "qcom,nss-common");
 	if (IS_ERR(gmac->nss_common)) {
 		dev_err(dev, "missing nss-common node\n");
-		return gmac->nss_common;
+		return PTR_ERR(gmac->nss_common);
 	}
 
 	/* Setup the register map for the qsgmii csr registers */
 	gmac->qsgmii_csr = syscon_regmap_lookup_by_phandle(dev->of_node,
 							   "qcom,qsgmii-csr");
-	if (IS_ERR(gmac->qsgmii_csr)) {
+	if (IS_ERR(gmac->qsgmii_csr))
 		dev_err(dev, "missing qsgmii-csr node\n");
-		return gmac->qsgmii_csr;
-	}
 
-	return NULL;
+	return PTR_ERR_OR_ZERO(gmac->qsgmii_csr);
 }
 
 static void ipq806x_gmac_fix_mac_speed(void *priv, unsigned int speed)
@@ -262,7 +260,7 @@ static int ipq806x_gmac_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct ipq806x_gmac *gmac;
 	int val;
-	void *err;
+	int err;
 
 	val = stmmac_get_platform_resources(pdev, &stmmac_res);
 	if (val)
@@ -279,9 +277,9 @@ static int ipq806x_gmac_probe(struct platform_device *pdev)
 	gmac->pdev = pdev;
 
 	err = ipq806x_gmac_of_parse(gmac);
-	if (IS_ERR(err)) {
+	if (err) {
 		dev_err(dev, "device tree parsing error\n");
-		return PTR_ERR(err);
+		return err;
 	}
 
 	regmap_write(gmac->qsgmii_csr, QSGMII_PCS_CAL_LCKDT_CTL,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
index 401383b252a8..f0d797ab74d8 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
@@ -32,6 +32,7 @@
 #define SYSMGR_EMACGRP_CTRL_PHYSEL_ENUM_RMII 0x2
 #define SYSMGR_EMACGRP_CTRL_PHYSEL_WIDTH 2
 #define SYSMGR_EMACGRP_CTRL_PHYSEL_MASK 0x00000003
+#define SYSMGR_EMACGRP_CTRL_PTP_REF_CLK_MASK 0x00000010
 
 #define EMAC_SPLITTER_CTRL_REG			0x0
 #define EMAC_SPLITTER_CTRL_SPEED_MASK		0x3
@@ -47,6 +48,7 @@ struct socfpga_dwmac {
 	struct regmap *sys_mgr_base_addr;
 	struct reset_control *stmmac_rst;
 	void __iomem *splitter_base;
+	bool f2h_ptp_ref_clk;
 };
 
 static void socfpga_dwmac_fix_mac_speed(void *priv, unsigned int speed)
@@ -116,6 +118,8 @@ static int socfpga_dwmac_parse_data(struct socfpga_dwmac *dwmac, struct device *
 		return -EINVAL;
 	}
 
+	dwmac->f2h_ptp_ref_clk = of_property_read_bool(np, "altr,f2h_ptp_ref_clk");
+
 	np_splitter = of_parse_phandle(np, "altr,emac-splitter", 0);
 	if (np_splitter) {
 		if (of_address_to_resource(np_splitter, 0, &res_splitter)) {
@@ -171,6 +175,11 @@ static int socfpga_dwmac_setup(struct socfpga_dwmac *dwmac)
 	ctrl &= ~(SYSMGR_EMACGRP_CTRL_PHYSEL_MASK << reg_shift);
 	ctrl |= val << reg_shift;
 
+	if (dwmac->f2h_ptp_ref_clk)
+		ctrl |= SYSMGR_EMACGRP_CTRL_PTP_REF_CLK_MASK << (reg_shift / 2);
+	else
+		ctrl &= ~(SYSMGR_EMACGRP_CTRL_PTP_REF_CLK_MASK << (reg_shift / 2));
+
 	regmap_write(sys_mgr_base_addr, reg_offset, ctrl);
 	return 0;
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index b3fe0575ff6b..8831a053ac13 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -34,6 +34,7 @@
 #define GMAC_FLOW_CTRL		0x00000018	/* Flow Control */
 #define GMAC_VLAN_TAG		0x0000001c	/* VLAN Tag */
 #define GMAC_VERSION		0x00000020	/* GMAC CORE Version */
+#define GMAC_DEBUG		0x00000024	/* GMAC debug register */
 #define GMAC_WAKEUP_FILTER	0x00000028	/* Wake-up Frame Filter */
 
 #define GMAC_INT_STATUS		0x00000038	/* interrupt status register */
@@ -177,6 +178,47 @@ enum inter_frame_gap {
 #define GMAC_FLOW_CTRL_TFE	0x00000002	/* Tx Flow Control Enable */
 #define GMAC_FLOW_CTRL_FCB_BPA	0x00000001	/* Flow Control Busy ... */
 
+/* DEBUG Register defines */
+/* MTL TxStatus FIFO */
+#define GMAC_DEBUG_TXSTSFSTS	BIT(25)	/* MTL TxStatus FIFO Full Status */
+#define GMAC_DEBUG_TXFSTS	BIT(24) /* MTL Tx FIFO Not Empty Status */
+#define GMAC_DEBUG_TWCSTS	BIT(22) /* MTL Tx FIFO Write Controller */
+/* MTL Tx FIFO Read Controller Status */
+#define GMAC_DEBUG_TRCSTS_MASK	GENMASK(21, 20)
+#define GMAC_DEBUG_TRCSTS_SHIFT	20
+#define GMAC_DEBUG_TRCSTS_IDLE	0
+#define GMAC_DEBUG_TRCSTS_READ	1
+#define GMAC_DEBUG_TRCSTS_TXW	2
+#define GMAC_DEBUG_TRCSTS_WRITE	3
+#define GMAC_DEBUG_TXPAUSED	BIT(19) /* MAC Transmitter in PAUSE */
+/* MAC Transmit Frame Controller Status */
+#define GMAC_DEBUG_TFCSTS_MASK	GENMASK(18, 17)
+#define GMAC_DEBUG_TFCSTS_SHIFT	17
+#define GMAC_DEBUG_TFCSTS_IDLE	0
+#define GMAC_DEBUG_TFCSTS_WAIT	1
+#define GMAC_DEBUG_TFCSTS_GEN_PAUSE	2
+#define GMAC_DEBUG_TFCSTS_XFER	3
+/* MAC GMII or MII Transmit Protocol Engine Status */
+#define GMAC_DEBUG_TPESTS	BIT(16)
+#define GMAC_DEBUG_RXFSTS_MASK	GENMASK(9, 8) /* MTL Rx FIFO Fill-level */
+#define GMAC_DEBUG_RXFSTS_SHIFT	8
+#define GMAC_DEBUG_RXFSTS_EMPTY	0
+#define GMAC_DEBUG_RXFSTS_BT	1
+#define GMAC_DEBUG_RXFSTS_AT	2
+#define GMAC_DEBUG_RXFSTS_FULL	3
+#define GMAC_DEBUG_RRCSTS_MASK	GENMASK(6, 5) /* MTL Rx FIFO Read Controller */
+#define GMAC_DEBUG_RRCSTS_SHIFT	5
+#define GMAC_DEBUG_RRCSTS_IDLE	0
+#define GMAC_DEBUG_RRCSTS_RDATA	1
+#define GMAC_DEBUG_RRCSTS_RSTAT	2
+#define GMAC_DEBUG_RRCSTS_FLUSH	3
+#define GMAC_DEBUG_RWCSTS	BIT(4) /* MTL Rx FIFO Write Controller Active */
+/* MAC Receive Frame Controller FIFO Status */
+#define GMAC_DEBUG_RFCFCSTS_MASK	GENMASK(2, 1)
+#define GMAC_DEBUG_RFCFCSTS_SHIFT	1
+/* MAC GMII or MII Receive Protocol Engine Status */
+#define GMAC_DEBUG_RPESTS	BIT(0)
+
 /*--- DMA BLOCK defines ---*/
 /* DMA Bus Mode register defines */
 #define DMA_BUS_MODE_SFT_RESET	0x00000001	/* Software Reset */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index 371a669d69fd..c2941172f6d1 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -397,6 +397,80 @@ static void dwmac1000_get_adv(struct mac_device_info *hw, struct rgmii_adv *adv)
 	adv->lp_pause = (value & GMAC_ANE_PSE) >> GMAC_ANE_PSE_SHIFT;
 }
 
+static void dwmac1000_debug(void __iomem *ioaddr, struct stmmac_extra_stats *x)
+{
+	u32 value = readl(ioaddr + GMAC_DEBUG);
+
+	if (value & GMAC_DEBUG_TXSTSFSTS)
+		x->mtl_tx_status_fifo_full++;
+	if (value & GMAC_DEBUG_TXFSTS)
+		x->mtl_tx_fifo_not_empty++;
+	if (value & GMAC_DEBUG_TWCSTS)
+		x->mmtl_fifo_ctrl++;
+	if (value & GMAC_DEBUG_TRCSTS_MASK) {
+		u32 trcsts = (value & GMAC_DEBUG_TRCSTS_MASK)
+			     >> GMAC_DEBUG_TRCSTS_SHIFT;
+		if (trcsts == GMAC_DEBUG_TRCSTS_WRITE)
+			x->mtl_tx_fifo_read_ctrl_write++;
+		else if (trcsts == GMAC_DEBUG_TRCSTS_TXW)
+			x->mtl_tx_fifo_read_ctrl_wait++;
+		else if (trcsts == GMAC_DEBUG_TRCSTS_READ)
+			x->mtl_tx_fifo_read_ctrl_read++;
+		else
+			x->mtl_tx_fifo_read_ctrl_idle++;
+	}
+	if (value & GMAC_DEBUG_TXPAUSED)
+		x->mac_tx_in_pause++;
+	if (value & GMAC_DEBUG_TFCSTS_MASK) {
+		u32 tfcsts = (value & GMAC_DEBUG_TFCSTS_MASK)
+			      >> GMAC_DEBUG_TFCSTS_SHIFT;
+
+		if (tfcsts == GMAC_DEBUG_TFCSTS_XFER)
+			x->mac_tx_frame_ctrl_xfer++;
+		else if (tfcsts == GMAC_DEBUG_TFCSTS_GEN_PAUSE)
+			x->mac_tx_frame_ctrl_pause++;
+		else if (tfcsts == GMAC_DEBUG_TFCSTS_WAIT)
+			x->mac_tx_frame_ctrl_wait++;
+		else
+			x->mac_tx_frame_ctrl_idle++;
+	}
+	if (value & GMAC_DEBUG_TPESTS)
+		x->mac_gmii_tx_proto_engine++;
+	if (value & GMAC_DEBUG_RXFSTS_MASK) {
+		u32 rxfsts = (value & GMAC_DEBUG_RXFSTS_MASK)
+			     >> GMAC_DEBUG_RRCSTS_SHIFT;
+
+		if (rxfsts == GMAC_DEBUG_RXFSTS_FULL)
+			x->mtl_rx_fifo_fill_level_full++;
+		else if (rxfsts == GMAC_DEBUG_RXFSTS_AT)
+			x->mtl_rx_fifo_fill_above_thresh++;
+		else if (rxfsts == GMAC_DEBUG_RXFSTS_BT)
+			x->mtl_rx_fifo_fill_below_thresh++;
+		else
+			x->mtl_rx_fifo_fill_level_empty++;
+	}
+	if (value & GMAC_DEBUG_RRCSTS_MASK) {
+		u32 rrcsts = (value & GMAC_DEBUG_RRCSTS_MASK) >>
+			     GMAC_DEBUG_RRCSTS_SHIFT;
+
+		if (rrcsts == GMAC_DEBUG_RRCSTS_FLUSH)
+			x->mtl_rx_fifo_read_ctrl_flush++;
+		else if (rrcsts == GMAC_DEBUG_RRCSTS_RSTAT)
+			x->mtl_rx_fifo_read_ctrl_read_data++;
+		else if (rrcsts == GMAC_DEBUG_RRCSTS_RDATA)
+			x->mtl_rx_fifo_read_ctrl_status++;
+		else
+			x->mtl_rx_fifo_read_ctrl_idle++;
+	}
+	if (value & GMAC_DEBUG_RWCSTS)
+		x->mtl_rx_fifo_ctrl_active++;
+	if (value & GMAC_DEBUG_RFCFCSTS_MASK)
+		x->mac_rx_frame_ctrl_fifo = (value & GMAC_DEBUG_RFCFCSTS_MASK)
+					    >> GMAC_DEBUG_RFCFCSTS_SHIFT;
+	if (value & GMAC_DEBUG_RPESTS)
+		x->mac_gmii_rx_proto_engine++;
+}
+
 static const struct stmmac_ops dwmac1000_ops = {
 	.core_init = dwmac1000_core_init,
 	.rx_ipc = dwmac1000_rx_ipc_enable,
@@ -413,6 +487,7 @@ static const struct stmmac_ops dwmac1000_ops = {
 	.set_eee_pls = dwmac1000_set_eee_pls,
 	.ctrl_ane = dwmac1000_ctrl_ane,
 	.get_adv = dwmac1000_get_adv,
+	.debug = dwmac1000_debug,
 };
 
 struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr, int mcbins,
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index 2e51b816a7e8..4c6486cc80fb 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -136,6 +136,31 @@ static const struct stmmac_stats stmmac_gstrings_stats[] = {
 	STMMAC_STAT(irq_pcs_ane_n),
 	STMMAC_STAT(irq_pcs_link_n),
 	STMMAC_STAT(irq_rgmii_n),
+	/* DEBUG */
+	STMMAC_STAT(mtl_tx_status_fifo_full),
+	STMMAC_STAT(mtl_tx_fifo_not_empty),
+	STMMAC_STAT(mmtl_fifo_ctrl),
+	STMMAC_STAT(mtl_tx_fifo_read_ctrl_write),
+	STMMAC_STAT(mtl_tx_fifo_read_ctrl_wait),
+	STMMAC_STAT(mtl_tx_fifo_read_ctrl_read),
+	STMMAC_STAT(mtl_tx_fifo_read_ctrl_idle),
+	STMMAC_STAT(mac_tx_in_pause),
+	STMMAC_STAT(mac_tx_frame_ctrl_xfer),
+	STMMAC_STAT(mac_tx_frame_ctrl_idle),
+	STMMAC_STAT(mac_tx_frame_ctrl_wait),
+	STMMAC_STAT(mac_tx_frame_ctrl_pause),
+	STMMAC_STAT(mac_gmii_tx_proto_engine),
+	STMMAC_STAT(mtl_rx_fifo_fill_level_full),
+	STMMAC_STAT(mtl_rx_fifo_fill_above_thresh),
+	STMMAC_STAT(mtl_rx_fifo_fill_below_thresh),
+	STMMAC_STAT(mtl_rx_fifo_fill_level_empty),
+	STMMAC_STAT(mtl_rx_fifo_read_ctrl_flush),
+	STMMAC_STAT(mtl_rx_fifo_read_ctrl_read_data),
+	STMMAC_STAT(mtl_rx_fifo_read_ctrl_status),
+	STMMAC_STAT(mtl_rx_fifo_read_ctrl_idle),
+	STMMAC_STAT(mtl_rx_fifo_ctrl_active),
+	STMMAC_STAT(mac_rx_frame_ctrl_fifo),
+	STMMAC_STAT(mac_gmii_rx_proto_engine),
 };
 #define STMMAC_STATS_LEN ARRAY_SIZE(stmmac_gstrings_stats)
 
@@ -497,6 +522,11 @@ static void stmmac_get_ethtool_stats(struct net_device *dev,
 			if (val)
 				priv->xstats.phy_eee_wakeup_error_n = val;
 		}
+
+		if ((priv->hw->mac->debug) &&
+		    (priv->synopsys_id >= DWMAC_CORE_3_50))
+			priv->hw->mac->debug(priv->ioaddr,
+					     (void *)&priv->xstats);
 	}
 	for (i = 0; i < STMMAC_STATS_LEN; i++) {
 		char *p = (char *)priv + stmmac_gstrings_stats[i].stat_offset;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
index 76ad214b4036..a77f68918010 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c
@@ -33,22 +33,25 @@ static void stmmac_config_hw_tstamping(void __iomem *ioaddr, u32 data)
 	writel(data, ioaddr + PTP_TCR);
 }
 
-static void stmmac_config_sub_second_increment(void __iomem *ioaddr)
+static u32 stmmac_config_sub_second_increment(void __iomem *ioaddr,
+					      u32 ptp_clock)
 {
 	u32 value = readl(ioaddr + PTP_TCR);
 	unsigned long data;
 
 	/* Convert the ptp_clock to nano second
-	 * formula = (1/ptp_clock) * 1000000000
+	 * formula = (2/ptp_clock) * 1000000000
 	 * where, ptp_clock = 50MHz.
 	 */
-	data = (1000000000ULL / 50000000);
+	data = (2000000000ULL / ptp_clock);
 
 	/* 0.465ns accuracy */
 	if (!(value & PTP_TCR_TSCTRLSSR))
 		data = (data * 1000) / 465;
 
 	writel(data, ioaddr + PTP_SSIR);
+
+	return data;
 }
 
 static int stmmac_init_systime(void __iomem *ioaddr, u32 sec, u32 nsec)
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index a5b869eb4678..c21015b68097 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -53,6 +53,7 @@
 #include "stmmac.h"
 #include <linux/reset.h>
 #include <linux/of_mdio.h>
+#include "dwmac1000.h"
 
 #define STMMAC_ALIGN(x)	L1_CACHE_ALIGN(x)
 
@@ -185,7 +186,7 @@ static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 			priv->clk_csr = STMMAC_CSR_100_150M;
 		else if ((clk_rate >= CSR_F_150M) && (clk_rate < CSR_F_250M))
 			priv->clk_csr = STMMAC_CSR_150_250M;
-		else if ((clk_rate >= CSR_F_250M) && (clk_rate <= CSR_F_300M))
+		else if ((clk_rate >= CSR_F_250M) && (clk_rate < CSR_F_300M))
 			priv->clk_csr = STMMAC_CSR_250_300M;
 	}
 }
@@ -435,6 +436,7 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 	u32 ts_master_en = 0;
 	u32 ts_event_en = 0;
 	u32 value = 0;
+	u32 sec_inc;
 
 	if (!(priv->dma_cap.time_stamp || priv->adv_ts)) {
 		netdev_alert(priv->dev, "No support for HW time stamping\n");
@@ -598,24 +600,19 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 			 tstamp_all | ptp_v2 | ptp_over_ethernet |
 			 ptp_over_ipv6_udp | ptp_over_ipv4_udp | ts_event_en |
 			 ts_master_en | snap_type_sel);
-
 		priv->hw->ptp->config_hw_tstamping(priv->ioaddr, value);
 
 		/* program Sub Second Increment reg */
-		priv->hw->ptp->config_sub_second_increment(priv->ioaddr);
+		sec_inc = priv->hw->ptp->config_sub_second_increment(
+			priv->ioaddr, priv->clk_ptp_rate);
+		temp = div_u64(1000000000ULL, sec_inc);
 
 		/* calculate default added value:
 		 * formula is :
 		 * addend = (2^32)/freq_div_ratio;
-		 * where, freq_div_ratio = clk_ptp_ref_i/50MHz
-		 * hence, addend = ((2^32) * 50MHz)/clk_ptp_ref_i;
-		 * NOTE: clk_ptp_ref_i should be >= 50MHz to
-		 *       achieve 20ns accuracy.
-		 *
-		 * 2^x * y == (y << x), hence
-		 * 2^32 * 50000000 ==> (50000000 << 32)
+		 * where, freq_div_ratio = 1e9ns/sec_inc
 		 */
-		temp = (u64) (50000000ULL << 32);
+		temp = (u64)(temp << 32);
 		priv->default_addend = div_u64(temp, priv->clk_ptp_rate);
 		priv->hw->ptp->config_addend(priv->ioaddr,
 					     priv->default_addend);
@@ -2402,7 +2399,7 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 		features &= ~NETIF_F_RXCSUM;
 
 	if (!priv->plat->tx_coe)
-		features &= ~NETIF_F_ALL_CSUM;
+		features &= ~NETIF_F_CSUM_MASK;
 
 	/* Some GMAC devices have a bugged Jumbo frame support that
 	 * needs to have the Tx COE disabled for oversized frames
@@ -2410,7 +2407,7 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 	 * the TX csum insertionin the TDES and not use SF.
 	 */
 	if (priv->plat->bugged_jumbo && (dev->mtu > ETH_DATA_LEN))
-		features &= ~NETIF_F_ALL_CSUM;
+		features &= ~NETIF_F_CSUM_MASK;
 
 	return features;
 }
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
index bba670c42e37..efb54f356a67 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
@@ -29,7 +29,7 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/of_gpio.h>
-
+#include <linux/of_mdio.h>
 #include <asm/io.h>
 
 #include "stmmac.h"
@@ -196,25 +196,28 @@ int stmmac_mdio_register(struct net_device *ndev)
 {
 	int err = 0;
 	struct mii_bus *new_bus;
-	int *irqlist;
 	struct stmmac_priv *priv = netdev_priv(ndev);
 	struct stmmac_mdio_bus_data *mdio_bus_data = priv->plat->mdio_bus_data;
 	int addr, found;
+	struct device_node *mdio_node = priv->plat->mdio_node;
 
 	if (!mdio_bus_data)
 		return 0;
 
+	if (IS_ENABLED(CONFIG_OF)) {
+		if (mdio_node) {
+			netdev_dbg(ndev, "FOUND MDIO subnode\n");
+		} else {
+			netdev_warn(ndev, "No MDIO subnode found\n");
+		}
+	}
+
 	new_bus = mdiobus_alloc();
 	if (new_bus == NULL)
 		return -ENOMEM;
 
-	if (mdio_bus_data->irqs) {
-		irqlist = mdio_bus_data->irqs;
-	} else {
-		for (addr = 0; addr < PHY_MAX_ADDR; addr++)
-			priv->mii_irq[addr] = PHY_POLL;
-		irqlist = priv->mii_irq;
-	}
+	if (mdio_bus_data->irqs)
+		memcpy(new_bus->irq, mdio_bus_data, sizeof(new_bus->irq));
 
 #ifdef CONFIG_OF
 	if (priv->device->of_node)
@@ -228,10 +231,13 @@ int stmmac_mdio_register(struct net_device *ndev)
 	snprintf(new_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 		 new_bus->name, priv->plat->bus_id);
 	new_bus->priv = ndev;
-	new_bus->irq = irqlist;
 	new_bus->phy_mask = mdio_bus_data->phy_mask;
 	new_bus->parent = priv->device;
-	err = mdiobus_register(new_bus);
+
+	if (mdio_node)
+		err = of_mdiobus_register(new_bus, mdio_node);
+	else
+		err = mdiobus_register(new_bus);
 	if (err != 0) {
 		pr_err("%s: Cannot register as MDIO bus\n", new_bus->name);
 		goto bus_register_fail;
@@ -239,7 +245,7 @@ int stmmac_mdio_register(struct net_device *ndev)
 
 	found = 0;
 	for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
-		struct phy_device *phydev = new_bus->phy_map[addr];
+		struct phy_device *phydev = mdiobus_get_phy(new_bus, addr);
 		if (phydev) {
 			int act = 0;
 			char irq_num[4];
@@ -251,7 +257,8 @@ int stmmac_mdio_register(struct net_device *ndev)
 			 */
 			if ((mdio_bus_data->irqs == NULL) &&
 			    (mdio_bus_data->probed_phy_irq > 0)) {
-				irqlist[addr] = mdio_bus_data->probed_phy_irq;
+				new_bus->irq[addr] =
+					mdio_bus_data->probed_phy_irq;
 				phydev->irq = mdio_bus_data->probed_phy_irq;
 			}
 
@@ -278,13 +285,13 @@ int stmmac_mdio_register(struct net_device *ndev)
 			}
 			pr_info("%s: PHY ID %08x at %d IRQ %s (%s)%s\n",
 				ndev->name, phydev->phy_id, addr,
-				irq_str, dev_name(&phydev->dev),
+				irq_str, phydev_name(phydev),
 				act ? " active" : "");
 			found = 1;
 		}
 	}
 
-	if (!found) {
+	if (!found && !mdio_node) {
 		pr_warn("%s: No PHY found\n", ndev->name);
 		mdiobus_unregister(new_bus);
 		mdiobus_free(new_bus);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index d02691ba3d7f..4514ba73d961 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -110,6 +110,7 @@ stmmac_probe_config_dt(struct platform_device *pdev, const char **mac)
 	struct device_node *np = pdev->dev.of_node;
 	struct plat_stmmacenet_data *plat;
 	struct stmmac_dma_cfg *dma_cfg;
+	struct device_node *child_node = NULL;
 
 	plat = devm_kzalloc(&pdev->dev, sizeof(*plat), GFP_KERNEL);
 	if (!plat)
@@ -140,13 +141,19 @@ stmmac_probe_config_dt(struct platform_device *pdev, const char **mac)
 		plat->phy_node = of_node_get(np);
 	}
 
+	for_each_child_of_node(np, child_node)
+		if (of_device_is_compatible(child_node,	"snps,dwmac-mdio")) {
+			plat->mdio_node = child_node;
+			break;
+		}
+
 	/* "snps,phy-addr" is not a standard property. Mark it as deprecated
 	 * and warn of its use. Remove this when phy node support is added.
 	 */
 	if (of_property_read_u32(np, "snps,phy-addr", &plat->phy_addr) == 0)
 		dev_warn(&pdev->dev, "snps,phy-addr property is deprecated\n");
 
-	if (plat->phy_node || plat->phy_bus_name)
+	if ((plat->phy_node && !of_phy_is_fixed_link(np)) || !plat->mdio_node)
 		plat->mdio_bus_data = NULL;
 	else
 		plat->mdio_bus_data =
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index cc106d892e29..23fa29877f5b 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -389,17 +389,27 @@ static int vnet_rx_one(struct vnet_port *port, struct vio_net_desc *desc)
 	if (vio_version_after_eq(&port->vio, 1, 8)) {
 		struct vio_net_dext *dext = vio_net_ext(desc);
 
+		skb_reset_network_header(skb);
+
 		if (dext->flags & VNET_PKT_HCK_IPV4_HDRCKSUM) {
 			if (skb->protocol == ETH_P_IP) {
-				struct iphdr *iph = (struct iphdr *)skb->data;
+				struct iphdr *iph = ip_hdr(skb);
 
 				iph->check = 0;
 				ip_send_check(iph);
 			}
 		}
 		if ((dext->flags & VNET_PKT_HCK_FULLCKSUM) &&
-		    skb->ip_summed == CHECKSUM_NONE)
-			vnet_fullcsum(skb);
+		    skb->ip_summed == CHECKSUM_NONE) {
+			if (skb->protocol == htons(ETH_P_IP)) {
+				struct iphdr *iph = ip_hdr(skb);
+				int ihl = iph->ihl * 4;
+
+				skb_reset_transport_header(skb);
+				skb_set_transport_header(skb, ihl);
+				vnet_fullcsum(skb);
+			}
+		}
 		if (dext->flags & VNET_PKT_HCK_IPV4_HDRCKSUM_OK) {
 			skb->ip_summed = CHECKSUM_PARTIAL;
 			skb->csum_level = 0;
diff --git a/drivers/net/ethernet/synopsys/dwc_eth_qos.c b/drivers/net/ethernet/synopsys/dwc_eth_qos.c
index 9066d7a8483c..af11ed1e0bcc 100644
--- a/drivers/net/ethernet/synopsys/dwc_eth_qos.c
+++ b/drivers/net/ethernet/synopsys/dwc_eth_qos.c
@@ -426,7 +426,7 @@
 #define DWC_MMC_RXOCTETCOUNT_GB          0x0784
 #define DWC_MMC_RXPACKETCOUNT_GB         0x0780
 
-static int debug = 3;
+static int debug = -1;
 module_param(debug, int, 0);
 MODULE_PARM_DESC(debug, "DWC_eth_qos debug level (0=none,...,16=all)");
 
@@ -650,6 +650,11 @@ struct net_local {
 	u32 mmc_tx_counters_mask;
 
 	struct dwceqos_flowcontrol flowcontrol;
+
+	/* Tracks the intermediate state of phy started but hardware
+	 * init not finished yet.
+	 */
+	bool phy_defer;
 };
 
 static void dwceqos_read_mmc_counters(struct net_local *lp, u32 rx_mask,
@@ -901,6 +906,9 @@ static void dwceqos_adjust_link(struct net_device *ndev)
 	struct phy_device *phydev = lp->phy_dev;
 	int status_change = 0;
 
+	if (lp->phy_defer)
+		return;
+
 	if (phydev->link) {
 		if ((lp->speed != phydev->speed) ||
 		    (lp->duplex != phydev->duplex)) {
@@ -972,9 +980,7 @@ static int dwceqos_mii_probe(struct net_device *ndev)
 	}
 
 	if (netif_msg_probe(lp))
-		netdev_dbg(lp->ndev,
-			   "phydev %p, phydev->phy_id 0xa%x, phydev->addr 0x%x\n",
-			   phydev, phydev->phy_id, phydev->addr);
+		phy_attached_info(phydev);
 
 	phydev->supported &= PHY_GBIT_FEATURES;
 
@@ -983,14 +989,6 @@ static int dwceqos_mii_probe(struct net_device *ndev)
 	lp->duplex  = DUPLEX_UNKNOWN;
 	lp->phy_dev = phydev;
 
-	if (netif_msg_probe(lp)) {
-		netdev_dbg(lp->ndev, "phy_addr 0x%x, phy_id 0x%08x\n",
-			   lp->phy_dev->addr, lp->phy_dev->phy_id);
-
-		netdev_dbg(lp->ndev, "attach [%s] phy driver\n",
-			   lp->phy_dev->drv->name);
-	}
-
 	return 0;
 }
 
@@ -1123,7 +1121,7 @@ static int dwceqos_descriptor_init(struct net_local *lp)
 	/* Allocate DMA descriptors */
 	size = DWCEQOS_RX_DCNT * sizeof(struct dwceqos_dma_desc);
 	lp->rx_descs = dma_alloc_coherent(lp->ndev->dev.parent, size,
-			&lp->rx_descs_addr, 0);
+			&lp->rx_descs_addr, GFP_KERNEL);
 	if (!lp->rx_descs)
 		goto err_out;
 	lp->rx_descs_tail_addr = lp->rx_descs_addr +
@@ -1131,7 +1129,7 @@ static int dwceqos_descriptor_init(struct net_local *lp)
 
 	size = DWCEQOS_TX_DCNT * sizeof(struct dwceqos_dma_desc);
 	lp->tx_descs = dma_alloc_coherent(lp->ndev->dev.parent, size,
-			&lp->tx_descs_addr, 0);
+			&lp->tx_descs_addr, GFP_KERNEL);
 	if (!lp->tx_descs)
 		goto err_out;
 	lp->tx_descs_tail_addr = lp->tx_descs_addr +
@@ -1230,7 +1228,7 @@ static void dwceqos_enable_mmc_interrupt(struct net_local *lp)
 
 static int dwceqos_mii_init(struct net_local *lp)
 {
-	int ret = -ENXIO, i;
+	int ret = -ENXIO;
 	struct resource res;
 	struct device_node *mdionode;
 
@@ -1251,24 +1249,14 @@ static int dwceqos_mii_init(struct net_local *lp)
 	lp->mii_bus->priv = lp;
 	lp->mii_bus->parent = &lp->ndev->dev;
 
-	lp->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!lp->mii_bus->irq) {
-		ret = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		lp->mii_bus->irq[i] = PHY_POLL;
 	of_address_to_resource(lp->pdev->dev.of_node, 0, &res);
 	snprintf(lp->mii_bus->id, MII_BUS_ID_SIZE, "%.8llx",
 		 (unsigned long long)res.start);
 	if (of_mdiobus_register(lp->mii_bus, mdionode))
-		goto err_out_free_mdio_irq;
+		goto err_out_free_mdiobus;
 
 	return 0;
 
-err_out_free_mdio_irq:
-	kfree(lp->mii_bus->irq);
 err_out_free_mdiobus:
 	mdiobus_free(lp->mii_bus);
 err_out:
@@ -1655,6 +1643,12 @@ static void dwceqos_init_hw(struct net_local *lp)
 	regval = dwceqos_read(lp, REG_DWCEQOS_MAC_CFG);
 	dwceqos_write(lp, REG_DWCEQOS_MAC_CFG,
 		      regval | DWCEQOS_MAC_CFG_TE | DWCEQOS_MAC_CFG_RE);
+
+	lp->phy_defer = false;
+	mutex_lock(&lp->phy_dev->lock);
+	phy_read_status(lp->phy_dev);
+	dwceqos_adjust_link(lp->ndev);
+	mutex_unlock(&lp->phy_dev->lock);
 }
 
 static void dwceqos_tx_reclaim(unsigned long data)
@@ -1900,9 +1894,13 @@ static int dwceqos_open(struct net_device *ndev)
 	}
 	netdev_reset_queue(ndev);
 
-	napi_enable(&lp->napi);
+	/* The dwceqos reset state machine requires all phy clocks to complete,
+	 * hence the unusual init order with phy_start first.
+	 */
+	lp->phy_defer = true;
 	phy_start(lp->phy_dev);
 	dwceqos_init_hw(lp);
+	napi_enable(&lp->napi);
 
 	netif_start_queue(ndev);
 	tasklet_enable(&lp->tx_bdreclaim_tasklet);
@@ -1935,18 +1933,19 @@ static int dwceqos_stop(struct net_device *ndev)
 {
 	struct net_local *lp = netdev_priv(ndev);
 
-	phy_stop(lp->phy_dev);
-
 	tasklet_disable(&lp->tx_bdreclaim_tasklet);
-	netif_stop_queue(ndev);
 	napi_disable(&lp->napi);
 
-	dwceqos_drain_dma(lp);
+	/* Stop all tx before we drain the tx dma. */
+	netif_tx_lock_bh(lp->ndev);
+	netif_stop_queue(ndev);
+	netif_tx_unlock_bh(lp->ndev);
 
-	netif_tx_lock(lp->ndev);
+	dwceqos_drain_dma(lp);
 	dwceqos_reset_hw(lp);
+	phy_stop(lp->phy_dev);
+
 	dwceqos_descriptor_free(lp);
-	netif_tx_unlock(lp->ndev);
 
 	return 0;
 }
@@ -2107,7 +2106,7 @@ static int dwceqos_tx_frags(struct sk_buff *skb, struct net_local *lp,
 			dd = &lp->tx_descs[lp->tx_next];
 
 			/* Set DMA Descriptor fields */
-			dd->des0 = dma_handle;
+			dd->des0 = dma_handle + consumed_size;
 			dd->des1 = 0;
 			dd->des2 = dma_size;
 
@@ -2198,12 +2197,10 @@ static int dwceqos_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		((trans.initial_descriptor + trans.nr_descriptors) %
 		 DWCEQOS_TX_DCNT));
 
-	dwceqos_tx_finalize(skb, lp, &trans);
-
-	netdev_sent_queue(ndev, skb->len);
-
 	spin_lock_bh(&lp->tx_lock);
 	lp->tx_free -= trans.nr_descriptors;
+	dwceqos_tx_finalize(skb, lp, &trans);
+	netdev_sent_queue(ndev, skb->len);
 	spin_unlock_bh(&lp->tx_lock);
 
 	ndev->trans_start = jiffies;
@@ -2987,7 +2984,6 @@ static int dwceqos_remove(struct platform_device *pdev)
 		if (lp->phy_dev)
 			phy_disconnect(lp->phy_dev);
 		mdiobus_unregister(lp->mii_bus);
-		kfree(lp->mii_bus->irq);
 		mdiobus_free(lp->mii_bus);
 
 		unregister_netdev(ndev);
diff --git a/drivers/net/ethernet/ti/cpmac.c b/drivers/net/ethernet/ti/cpmac.c
index 77d26fe286c0..7eef45e6d70a 100644
--- a/drivers/net/ethernet/ti/cpmac.c
+++ b/drivers/net/ethernet/ti/cpmac.c
@@ -316,8 +316,6 @@ static int cpmac_mdio_reset(struct mii_bus *bus)
 	return 0;
 }
 
-static int mii_irqs[PHY_MAX_ADDR] = { PHY_POLL, };
-
 static struct mii_bus *cpmac_mii;
 
 static void cpmac_set_multicast_list(struct net_device *dev)
@@ -1118,7 +1116,7 @@ static int cpmac_probe(struct platform_device *pdev)
 		for (phy_id = 0; phy_id < PHY_MAX_ADDR; phy_id++) {
 			if (!(pdata->phy_mask & (1 << phy_id)))
 				continue;
-			if (!cpmac_mii->phy_map[phy_id])
+			if (!mdiobus_get_phy(cpmac_mii, phy_id))
 				continue;
 			strncpy(mdio_bus_id, cpmac_mii->id, MII_BUS_ID_SIZE);
 			break;
@@ -1226,7 +1224,6 @@ int cpmac_init(void)
 	cpmac_mii->read = cpmac_mdio_read;
 	cpmac_mii->write = cpmac_mdio_write;
 	cpmac_mii->reset = cpmac_mdio_reset;
-	cpmac_mii->irq = mii_irqs;
 
 	cpmac_mii->priv = ioremap(AR7_REGS_MDIO, 256);
 
diff --git a/drivers/net/ethernet/ti/cpsw-phy-sel.c b/drivers/net/ethernet/ti/cpsw-phy-sel.c
index e9cc61e1ec74..c3e85acfdc70 100644
--- a/drivers/net/ethernet/ti/cpsw-phy-sel.c
+++ b/drivers/net/ethernet/ti/cpsw-phy-sel.c
@@ -63,8 +63,12 @@ static void cpsw_gmii_sel_am3352(struct cpsw_phy_sel_priv *priv,
 		mode = AM33XX_GMII_SEL_MODE_RGMII;
 		break;
 
-	case PHY_INTERFACE_MODE_MII:
 	default:
+		dev_warn(priv->dev,
+			 "Unsupported PHY mode: \"%s\". Defaulting to MII.\n",
+			phy_modes(phy_mode));
+		/* fallthrough */
+	case PHY_INTERFACE_MODE_MII:
 		mode = AM33XX_GMII_SEL_MODE_MII;
 		break;
 	};
@@ -106,8 +110,12 @@ static void cpsw_gmii_sel_dra7xx(struct cpsw_phy_sel_priv *priv,
 		mode = AM33XX_GMII_SEL_MODE_RGMII;
 		break;
 
-	case PHY_INTERFACE_MODE_MII:
 	default:
+		dev_warn(priv->dev,
+			 "Unsupported PHY mode: \"%s\". Defaulting to MII.\n",
+			phy_modes(phy_mode));
+		/* fallthrough */
+	case PHY_INTERFACE_MODE_MII:
 		mode = AM33XX_GMII_SEL_MODE_MII;
 		break;
 	};
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index fc958067d10a..42fdfd4d9d4f 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -1159,8 +1159,8 @@ static void cpsw_slave_open(struct cpsw_slave *slave, struct cpsw_priv *priv)
 			slave->data->phy_id, slave->slave_num);
 		slave->phy = NULL;
 	} else {
-		dev_info(priv->dev, "phy found : id is : 0x%x\n",
-			 slave->phy->phy_id);
+		phy_attached_info(slave->phy);
+
 		phy_start(slave->phy);
 
 		/* Configure GMII_SEL register */
@@ -2050,7 +2050,8 @@ static int cpsw_probe_dt(struct cpsw_priv *priv,
 			if (!phy_dev)
 				return -ENODEV;
 			snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
-				 PHY_ID_FMT, phy_dev->bus->id, phy_dev->addr);
+				 PHY_ID_FMT, phy_dev->mdio.bus->id,
+				 phy_dev->mdio.addr);
 		} else if (parp) {
 			u32 phyid;
 			struct device_node *mdio_node;
@@ -2482,7 +2483,7 @@ static int cpsw_probe(struct platform_device *pdev)
 	ndev->netdev_ops = &cpsw_netdev_ops;
 	ndev->ethtool_ops = &cpsw_ethtool_ops;
 	netif_napi_add(ndev, &priv->napi_rx, cpsw_rx_poll, CPSW_POLL_WEIGHT);
-	netif_napi_add(ndev, &priv->napi_tx, cpsw_tx_poll, CPSW_POLL_WEIGHT);
+	netif_tx_napi_add(ndev, &priv->napi_tx, cpsw_tx_poll, CPSW_POLL_WEIGHT);
 
 	/* register the network device */
 	SET_NETDEV_DEV(ndev, &pdev->dev);
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index 657b65bf5cac..18bf3a8fdc50 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -82,7 +82,7 @@ struct cpdma_desc {
 
 struct cpdma_desc_pool {
 	phys_addr_t		phys;
-	u32			hw_addr;
+	dma_addr_t		hw_addr;
 	void __iomem		*iomap;		/* ioremap map */
 	void			*cpumap;	/* dma_alloc map */
 	int			desc_size, mem_size;
@@ -152,7 +152,7 @@ struct cpdma_chan {
  * abstract out these details
  */
 static struct cpdma_desc_pool *
-cpdma_desc_pool_create(struct device *dev, u32 phys, u32 hw_addr,
+cpdma_desc_pool_create(struct device *dev, u32 phys, dma_addr_t hw_addr,
 				int size, int align)
 {
 	int bitmap_size;
@@ -176,13 +176,13 @@ cpdma_desc_pool_create(struct device *dev, u32 phys, u32 hw_addr,
 
 	if (phys) {
 		pool->phys  = phys;
-		pool->iomap = ioremap(phys, size);
+		pool->iomap = ioremap(phys, size); /* should be memremap? */
 		pool->hw_addr = hw_addr;
 	} else {
-		pool->cpumap = dma_alloc_coherent(dev, size, &pool->phys,
+		pool->cpumap = dma_alloc_coherent(dev, size, &pool->hw_addr,
 						  GFP_KERNEL);
-		pool->iomap = pool->cpumap;
-		pool->hw_addr = pool->phys;
+		pool->iomap = (void __iomem __force *)pool->cpumap;
+		pool->phys = pool->hw_addr; /* assumes no IOMMU, don't use this value */
 	}
 
 	if (pool->iomap)
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index 33bd3b902304..5d9abedd6b75 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -1644,10 +1644,7 @@ static int emac_dev_open(struct net_device *ndev)
 		priv->speed = 0;
 		priv->duplex = ~0;
 
-		dev_info(emac_dev, "attached PHY driver [%s] "
-			"(mii_bus:phy_addr=%s, id=%x)\n",
-			priv->phydev->drv->name, dev_name(&priv->phydev->dev),
-			priv->phydev->phy_id);
+		phy_attached_info(priv->phydev);
 	}
 
 	if (!priv->phydev) {
diff --git a/drivers/net/ethernet/ti/davinci_mdio.c b/drivers/net/ethernet/ti/davinci_mdio.c
index c00084d689f3..4e7c9b9b042a 100644
--- a/drivers/net/ethernet/ti/davinci_mdio.c
+++ b/drivers/net/ethernet/ti/davinci_mdio.c
@@ -393,10 +393,10 @@ static int davinci_mdio_probe(struct platform_device *pdev)
 
 	/* scan and dump the bus */
 	for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
-		phy = data->bus->phy_map[addr];
+		phy = mdiobus_get_phy(data->bus, addr);
 		if (phy) {
 			dev_info(dev, "phy[%d]: device %s, driver %s\n",
-				 phy->addr, dev_name(&phy->dev),
+				 phy->mdio.addr, phydev_name(phy),
 				 phy->drv ? phy->drv->name : "unknown");
 		}
 	}
diff --git a/drivers/net/ethernet/ti/netcp.h b/drivers/net/ethernet/ti/netcp.h
index bb1bb72121c0..17a26a429b71 100644
--- a/drivers/net/ethernet/ti/netcp.h
+++ b/drivers/net/ethernet/ti/netcp.h
@@ -113,7 +113,7 @@ struct netcp_intf {
 #define	NETCP_PSDATA_LEN		KNAV_DMA_NUM_PS_WORDS
 struct netcp_packet {
 	struct sk_buff		*skb;
-	u32			*epib;
+	__le32			*epib;
 	u32			*psdata;
 	unsigned int		psdata_len;
 	struct netcp_intf	*netcp;
diff --git a/drivers/net/ethernet/ti/netcp_core.c b/drivers/net/ethernet/ti/netcp_core.c
index 37b9b39192ec..029841f98c32 100644
--- a/drivers/net/ethernet/ti/netcp_core.c
+++ b/drivers/net/ethernet/ti/netcp_core.c
@@ -109,69 +109,81 @@ module_param(netcp_debug_level, int, 0);
 MODULE_PARM_DESC(netcp_debug_level, "Netcp debug level (NETIF_MSG bits) (0=none,...,16=all)");
 
 /* Helper functions - Get/Set */
-static void get_pkt_info(u32 *buff, u32 *buff_len, u32 *ndesc,
+static void get_pkt_info(dma_addr_t *buff, u32 *buff_len, dma_addr_t *ndesc,
 			 struct knav_dma_desc *desc)
 {
-	*buff_len = desc->buff_len;
-	*buff = desc->buff;
-	*ndesc = desc->next_desc;
+	*buff_len = le32_to_cpu(desc->buff_len);
+	*buff = le32_to_cpu(desc->buff);
+	*ndesc = le32_to_cpu(desc->next_desc);
 }
 
-static void get_pad_info(u32 *pad0, u32 *pad1, struct knav_dma_desc *desc)
+static u32 get_sw_data(int index, struct knav_dma_desc *desc)
 {
-	*pad0 = desc->pad[0];
-	*pad1 = desc->pad[1];
+	/* No Endian conversion needed as this data is untouched by hw */
+	return desc->sw_data[index];
 }
 
-static void get_org_pkt_info(u32 *buff, u32 *buff_len,
+/* use these macros to get sw data */
+#define GET_SW_DATA0(desc) get_sw_data(0, desc)
+#define GET_SW_DATA1(desc) get_sw_data(1, desc)
+#define GET_SW_DATA2(desc) get_sw_data(2, desc)
+#define GET_SW_DATA3(desc) get_sw_data(3, desc)
+
+static void get_org_pkt_info(dma_addr_t *buff, u32 *buff_len,
 			     struct knav_dma_desc *desc)
 {
-	*buff = desc->orig_buff;
-	*buff_len = desc->orig_len;
+	*buff = le32_to_cpu(desc->orig_buff);
+	*buff_len = le32_to_cpu(desc->orig_len);
 }
 
-static void get_words(u32 *words, int num_words, u32 *desc)
+static void get_words(dma_addr_t *words, int num_words, __le32 *desc)
 {
 	int i;
 
 	for (i = 0; i < num_words; i++)
-		words[i] = desc[i];
+		words[i] = le32_to_cpu(desc[i]);
 }
 
-static void set_pkt_info(u32 buff, u32 buff_len, u32 ndesc,
+static void set_pkt_info(dma_addr_t buff, u32 buff_len, u32 ndesc,
 			 struct knav_dma_desc *desc)
 {
-	desc->buff_len = buff_len;
-	desc->buff = buff;
-	desc->next_desc = ndesc;
+	desc->buff_len = cpu_to_le32(buff_len);
+	desc->buff = cpu_to_le32(buff);
+	desc->next_desc = cpu_to_le32(ndesc);
 }
 
 static void set_desc_info(u32 desc_info, u32 pkt_info,
 			  struct knav_dma_desc *desc)
 {
-	desc->desc_info = desc_info;
-	desc->packet_info = pkt_info;
+	desc->desc_info = cpu_to_le32(desc_info);
+	desc->packet_info = cpu_to_le32(pkt_info);
 }
 
-static void set_pad_info(u32 pad0, u32 pad1, struct knav_dma_desc *desc)
+static void set_sw_data(int index, u32 data, struct knav_dma_desc *desc)
 {
-	desc->pad[0] = pad0;
-	desc->pad[1] = pad1;
+	/* No Endian conversion needed as this data is untouched by hw */
+	desc->sw_data[index] = data;
 }
 
-static void set_org_pkt_info(u32 buff, u32 buff_len,
+/* use these macros to set sw data */
+#define SET_SW_DATA0(data, desc) set_sw_data(0, data, desc)
+#define SET_SW_DATA1(data, desc) set_sw_data(1, data, desc)
+#define SET_SW_DATA2(data, desc) set_sw_data(2, data, desc)
+#define SET_SW_DATA3(data, desc) set_sw_data(3, data, desc)
+
+static void set_org_pkt_info(dma_addr_t buff, u32 buff_len,
 			     struct knav_dma_desc *desc)
 {
-	desc->orig_buff = buff;
-	desc->orig_len = buff_len;
+	desc->orig_buff = cpu_to_le32(buff);
+	desc->orig_len = cpu_to_le32(buff_len);
 }
 
-static void set_words(u32 *words, int num_words, u32 *desc)
+static void set_words(u32 *words, int num_words, __le32 *desc)
 {
 	int i;
 
 	for (i = 0; i < num_words; i++)
-		desc[i] = words[i];
+		desc[i] = cpu_to_le32(words[i]);
 }
 
 /* Read the e-fuse value as 32 bit values to be endian independent */
@@ -581,13 +593,21 @@ static void netcp_free_rx_desc_chain(struct netcp_intf *netcp,
 			break;
 		}
 		get_pkt_info(&dma_buf, &tmp, &dma_desc, ndesc);
-		get_pad_info((u32 *)&buf_ptr, &tmp, ndesc);
+		/* warning!!!! We are retrieving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		buf_ptr = (void *)GET_SW_DATA0(ndesc);
+		buf_len = (int)GET_SW_DATA1(desc);
 		dma_unmap_page(netcp->dev, dma_buf, PAGE_SIZE, DMA_FROM_DEVICE);
 		__free_page(buf_ptr);
 		knav_pool_desc_put(netcp->rx_pool, desc);
 	}
+	/* warning!!!! We are retrieving the virtual ptr in the sw_data
+	 * field as a 32bit value. Will not work on 64bit machines
+	 */
+	buf_ptr = (void *)GET_SW_DATA0(desc);
+	buf_len = (int)GET_SW_DATA1(desc);
 
-	get_pad_info((u32 *)&buf_ptr, &buf_len, desc);
 	if (buf_ptr)
 		netcp_frag_free(buf_len <= PAGE_SIZE, buf_ptr);
 	knav_pool_desc_put(netcp->rx_pool, desc);
@@ -626,7 +646,6 @@ static int netcp_process_one_rx_packet(struct netcp_intf *netcp)
 	struct netcp_packet p_info;
 	struct sk_buff *skb;
 	void *org_buf_ptr;
-	u32 tmp;
 
 	dma_desc = knav_queue_pop(netcp->rx_queue, &dma_sz);
 	if (!dma_desc)
@@ -639,7 +658,11 @@ static int netcp_process_one_rx_packet(struct netcp_intf *netcp)
 	}
 
 	get_pkt_info(&dma_buff, &buf_len, &dma_desc, desc);
-	get_pad_info((u32 *)&org_buf_ptr, &org_buf_len, desc);
+	/* warning!!!! We are retrieving the virtual ptr in the sw_data
+	 * field as a 32bit value. Will not work on 64bit machines
+	 */
+	org_buf_ptr = (void *)GET_SW_DATA0(desc);
+	org_buf_len = (int)GET_SW_DATA1(desc);
 
 	if (unlikely(!org_buf_ptr)) {
 		dev_err(netcp->ndev_dev, "NULL bufptr in desc\n");
@@ -672,14 +695,17 @@ static int netcp_process_one_rx_packet(struct netcp_intf *netcp)
 		}
 
 		get_pkt_info(&dma_buff, &buf_len, &dma_desc, ndesc);
-		get_pad_info((u32 *)&page, &tmp, ndesc);
+		/* warning!!!! We are retrieving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		page = (struct page *)GET_SW_DATA0(desc);
 
 		if (likely(dma_buff && buf_len && page)) {
 			dma_unmap_page(netcp->dev, dma_buff, PAGE_SIZE,
 				       DMA_FROM_DEVICE);
 		} else {
-			dev_err(netcp->ndev_dev, "Bad Rx desc dma_buff(%p), len(%d), page(%p)\n",
-				(void *)dma_buff, buf_len, page);
+			dev_err(netcp->ndev_dev, "Bad Rx desc dma_buff(%pad), len(%d), page(%p)\n",
+				&dma_buff, buf_len, page);
 			goto free_desc;
 		}
 
@@ -750,7 +776,6 @@ static void netcp_free_rx_buf(struct netcp_intf *netcp, int fdq)
 	unsigned int buf_len, dma_sz;
 	dma_addr_t dma;
 	void *buf_ptr;
-	u32 tmp;
 
 	/* Allocate descriptor */
 	while ((dma = knav_queue_pop(netcp->rx_fdq[fdq], &dma_sz))) {
@@ -761,7 +786,10 @@ static void netcp_free_rx_buf(struct netcp_intf *netcp, int fdq)
 		}
 
 		get_org_pkt_info(&dma, &buf_len, desc);
-		get_pad_info((u32 *)&buf_ptr, &tmp, desc);
+		/* warning!!!! We are retrieving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		buf_ptr = (void *)GET_SW_DATA0(desc);
 
 		if (unlikely(!dma)) {
 			dev_err(netcp->ndev_dev, "NULL orig_buff in desc\n");
@@ -813,7 +841,7 @@ static int netcp_allocate_rx_buf(struct netcp_intf *netcp, int fdq)
 	struct page *page;
 	dma_addr_t dma;
 	void *bufptr;
-	u32 pad[2];
+	u32 sw_data[2];
 
 	/* Allocate descriptor */
 	hwdesc = knav_pool_desc_get(netcp->rx_pool);
@@ -830,7 +858,7 @@ static int netcp_allocate_rx_buf(struct netcp_intf *netcp, int fdq)
 				SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
 
 		bufptr = netdev_alloc_frag(primary_buf_len);
-		pad[1] = primary_buf_len;
+		sw_data[1] = primary_buf_len;
 
 		if (unlikely(!bufptr)) {
 			dev_warn_ratelimited(netcp->ndev_dev,
@@ -842,8 +870,10 @@ static int netcp_allocate_rx_buf(struct netcp_intf *netcp, int fdq)
 		if (unlikely(dma_mapping_error(netcp->dev, dma)))
 			goto fail;
 
-		pad[0] = (u32)bufptr;
-
+		/* warning!!!! We are saving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		sw_data[0] = (u32)bufptr;
 	} else {
 		/* Allocate a secondary receive queue entry */
 		page = alloc_page(GFP_ATOMIC | GFP_DMA | __GFP_COLD);
@@ -853,8 +883,11 @@ static int netcp_allocate_rx_buf(struct netcp_intf *netcp, int fdq)
 		}
 		buf_len = PAGE_SIZE;
 		dma = dma_map_page(netcp->dev, page, 0, buf_len, DMA_TO_DEVICE);
-		pad[0] = (u32)page;
-		pad[1] = 0;
+		/* warning!!!! We are saving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		sw_data[0] = (u32)page;
+		sw_data[1] = 0;
 	}
 
 	desc_info =  KNAV_DMA_DESC_PS_INFO_IN_DESC;
@@ -864,7 +897,8 @@ static int netcp_allocate_rx_buf(struct netcp_intf *netcp, int fdq)
 	pkt_info |= (netcp->rx_queue_id & KNAV_DMA_DESC_RETQ_MASK) <<
 		    KNAV_DMA_DESC_RETQ_SHIFT;
 	set_org_pkt_info(dma, buf_len, hwdesc);
-	set_pad_info(pad[0], pad[1], hwdesc);
+	SET_SW_DATA0(sw_data[0], hwdesc);
+	SET_SW_DATA1(sw_data[1], hwdesc);
 	set_desc_info(desc_info, pkt_info, hwdesc);
 
 	/* Push to FDQs */
@@ -935,8 +969,8 @@ static void netcp_free_tx_desc_chain(struct netcp_intf *netcp,
 			dma_unmap_single(netcp->dev, dma_buf, buf_len,
 					 DMA_TO_DEVICE);
 		else
-			dev_warn(netcp->ndev_dev, "bad Tx desc buf(%p), len(%d)\n",
-				 (void *)dma_buf, buf_len);
+			dev_warn(netcp->ndev_dev, "bad Tx desc buf(%pad), len(%d)\n",
+				 &dma_buf, buf_len);
 
 		knav_pool_desc_put(netcp->tx_pool, ndesc);
 		ndesc = NULL;
@@ -957,7 +991,6 @@ static int netcp_process_tx_compl_packets(struct netcp_intf *netcp,
 	unsigned int dma_sz;
 	dma_addr_t dma;
 	int pkts = 0;
-	u32 tmp;
 
 	while (budget--) {
 		dma = knav_queue_pop(netcp->tx_compl_q, &dma_sz);
@@ -970,7 +1003,10 @@ static int netcp_process_tx_compl_packets(struct netcp_intf *netcp,
 			continue;
 		}
 
-		get_pad_info((u32 *)&skb, &tmp, desc);
+		/* warning!!!! We are retrieving the virtual ptr in the sw_data
+		 * field as a 32bit value. Will not work on 64bit machines
+		 */
+		skb = (struct sk_buff *)GET_SW_DATA0(desc);
 		netcp_free_tx_desc_chain(netcp, desc, dma_sz);
 		if (!skb) {
 			dev_err(netcp->ndev_dev, "No skb in Tx desc\n");
@@ -1059,6 +1095,7 @@ netcp_tx_map_skb(struct sk_buff *skb, struct netcp_intf *netcp)
 		u32 page_offset = frag->page_offset;
 		u32 buf_len = skb_frag_size(frag);
 		dma_addr_t desc_dma;
+		u32 desc_dma_32;
 		u32 pkt_info;
 
 		dma_addr = dma_map_page(dev, page, page_offset, buf_len,
@@ -1075,13 +1112,13 @@ netcp_tx_map_skb(struct sk_buff *skb, struct netcp_intf *netcp)
 			goto free_descs;
 		}
 
-		desc_dma = knav_pool_desc_virt_to_dma(netcp->tx_pool,
-						      (void *)ndesc);
+		desc_dma = knav_pool_desc_virt_to_dma(netcp->tx_pool, ndesc);
 		pkt_info =
 			(netcp->tx_compl_qid & KNAV_DMA_DESC_RETQ_MASK) <<
 				KNAV_DMA_DESC_RETQ_SHIFT;
 		set_pkt_info(dma_addr, buf_len, 0, ndesc);
-		set_words(&desc_dma, 1, &pdesc->next_desc);
+		desc_dma_32 = (u32)desc_dma;
+		set_words(&desc_dma_32, 1, &pdesc->next_desc);
 		pkt_len += buf_len;
 		if (pdesc != desc)
 			knav_pool_desc_map(netcp->tx_pool, pdesc,
@@ -1129,8 +1166,8 @@ static int netcp_tx_submit_skb(struct netcp_intf *netcp,
 	p_info.ts_context = NULL;
 	p_info.txtstamp_complete = NULL;
 	p_info.epib = desc->epib;
-	p_info.psdata = desc->psdata;
-	memset(p_info.epib, 0, KNAV_DMA_NUM_EPIB_WORDS * sizeof(u32));
+	p_info.psdata = (u32 __force *)desc->psdata;
+	memset(p_info.epib, 0, KNAV_DMA_NUM_EPIB_WORDS * sizeof(__le32));
 
 	/* Find out where to inject the packet for transmission */
 	list_for_each_entry(tx_hook, &netcp->txhook_list_head, list) {
@@ -1154,11 +1191,12 @@ static int netcp_tx_submit_skb(struct netcp_intf *netcp,
 
 	/* update descriptor */
 	if (p_info.psdata_len) {
-		u32 *psdata = p_info.psdata;
+		/* psdata points to both native-endian and device-endian data */
+		__le32 *psdata = (void __force *)p_info.psdata;
 
 		memmove(p_info.psdata, p_info.psdata + p_info.psdata_len,
 			p_info.psdata_len);
-		set_words(psdata, p_info.psdata_len, psdata);
+		set_words(p_info.psdata, p_info.psdata_len, psdata);
 		tmp |= (p_info.psdata_len & KNAV_DMA_DESC_PSLEN_MASK) <<
 			KNAV_DMA_DESC_PSLEN_SHIFT;
 	}
@@ -1173,11 +1211,14 @@ static int netcp_tx_submit_skb(struct netcp_intf *netcp,
 	}
 
 	set_words(&tmp, 1, &desc->packet_info);
-	set_words((u32 *)&skb, 1, &desc->pad[0]);
+	/* warning!!!! We are saving the virtual ptr in the sw_data
+	 * field as a 32bit value. Will not work on 64bit machines
+	 */
+	SET_SW_DATA0((u32)skb, desc);
 
 	if (tx_pipe->flags & SWITCH_TO_PORT_IN_TAGINFO) {
 		tmp = tx_pipe->switch_to_port;
-		set_words((u32 *)&tmp, 1, &desc->tag_info);
+		set_words(&tmp, 1, &desc->tag_info);
 	}
 
 	/* submit packet descriptor */
@@ -1990,7 +2031,7 @@ static int netcp_create_interface(struct netcp_device *netcp_device,
 
 	/* NAPI register */
 	netif_napi_add(ndev, &netcp->rx_napi, netcp_rx_poll, NETCP_NAPI_WEIGHT);
-	netif_napi_add(ndev, &netcp->tx_napi, netcp_tx_poll, NETCP_NAPI_WEIGHT);
+	netif_tx_napi_add(ndev, &netcp->tx_napi, netcp_tx_poll, NETCP_NAPI_WEIGHT);
 
 	/* Register the network device */
 	ndev->dev_id		= 0;
diff --git a/drivers/net/ethernet/ti/netcp_ethss.c b/drivers/net/ethernet/ti/netcp_ethss.c
index 4e70e7586a09..d543298d6750 100644
--- a/drivers/net/ethernet/ti/netcp_ethss.c
+++ b/drivers/net/ethernet/ti/netcp_ethss.c
@@ -2178,7 +2178,7 @@ static int gbe_slave_open(struct gbe_intf *gbe_intf)
 			return -ENODEV;
 		}
 		dev_dbg(priv->dev, "phy found: id is: 0x%s\n",
-			dev_name(&slave->phy->dev));
+			phydev_name(slave->phy));
 		phy_start(slave->phy);
 		phy_read_status(slave->phy);
 	}
@@ -2681,7 +2681,7 @@ static void init_secondary_ports(struct gbe_priv *gbe_dev,
 			slave->phy = NULL;
 		} else {
 			dev_dbg(dev, "phy found: id is: 0x%s\n",
-				dev_name(&slave->phy->dev));
+				phydev_name(slave->phy));
 			phy_start(slave->phy);
 			phy_read_status(slave->phy);
 		}
diff --git a/drivers/net/ethernet/tile/tilepro.c b/drivers/net/ethernet/tile/tilepro.c
index 6f0a4495c7f3..298e059d0498 100644
--- a/drivers/net/ethernet/tile/tilepro.c
+++ b/drivers/net/ethernet/tile/tilepro.c
@@ -1349,8 +1349,7 @@ static int tile_net_open_inner(struct net_device *dev)
  */
 static void tile_net_open_retry(struct work_struct *w)
 {
-	struct delayed_work *dw =
-		container_of(w, struct delayed_work, work);
+	struct delayed_work *dw = to_delayed_work(w);
 
 	struct tile_net_priv *priv =
 		container_of(dw, struct tile_net_priv, retry_work);
diff --git a/drivers/net/ethernet/toshiba/tc35815.c b/drivers/net/ethernet/toshiba/tc35815.c
index 45ac38d29ed8..54874783476a 100644
--- a/drivers/net/ethernet/toshiba/tc35815.c
+++ b/drivers/net/ethernet/toshiba/tc35815.c
@@ -608,40 +608,25 @@ static void tc_handle_link_change(struct net_device *dev)
 static int tc_mii_probe(struct net_device *dev)
 {
 	struct tc35815_local *lp = netdev_priv(dev);
-	struct phy_device *phydev = NULL;
-	int phy_addr;
+	struct phy_device *phydev;
 	u32 dropmask;
 
-	/* find the first phy */
-	for (phy_addr = 0; phy_addr < PHY_MAX_ADDR; phy_addr++) {
-		if (lp->mii_bus->phy_map[phy_addr]) {
-			if (phydev) {
-				printk(KERN_ERR "%s: multiple PHYs found\n",
-				       dev->name);
-				return -EINVAL;
-			}
-			phydev = lp->mii_bus->phy_map[phy_addr];
-			break;
-		}
-	}
-
+	phydev = phy_find_first(lp->mii_bus);
 	if (!phydev) {
 		printk(KERN_ERR "%s: no PHY found\n", dev->name);
 		return -ENODEV;
 	}
 
 	/* attach the mac to the phy */
-	phydev = phy_connect(dev, dev_name(&phydev->dev),
+	phydev = phy_connect(dev, phydev_name(phydev),
 			     &tc_handle_link_change,
 			     lp->chiptype == TC35815_TX4939 ? PHY_INTERFACE_MODE_RMII : PHY_INTERFACE_MODE_MII);
 	if (IS_ERR(phydev)) {
 		printk(KERN_ERR "%s: Could not attach to PHY\n", dev->name);
 		return PTR_ERR(phydev);
 	}
-	printk(KERN_INFO "%s: attached PHY driver [%s] "
-		"(mii_bus:phy_addr=%s, id=%x)\n",
-		dev->name, phydev->drv->name, dev_name(&phydev->dev),
-		phydev->phy_id);
+
+	phy_attached_info(phydev);
 
 	/* mask with MAC supported features */
 	phydev->supported &= PHY_BASIC_FEATURES;
@@ -669,7 +654,6 @@ static int tc_mii_init(struct net_device *dev)
 {
 	struct tc35815_local *lp = netdev_priv(dev);
 	int err;
-	int i;
 
 	lp->mii_bus = mdiobus_alloc();
 	if (lp->mii_bus == NULL) {
@@ -684,18 +668,9 @@ static int tc_mii_init(struct net_device *dev)
 		 (lp->pci_dev->bus->number << 8) | lp->pci_dev->devfn);
 	lp->mii_bus->priv = dev;
 	lp->mii_bus->parent = &lp->pci_dev->dev;
-	lp->mii_bus->irq = kmalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!lp->mii_bus->irq) {
-		err = -ENOMEM;
-		goto err_out_free_mii_bus;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		lp->mii_bus->irq[i] = PHY_POLL;
-
 	err = mdiobus_register(lp->mii_bus);
 	if (err)
-		goto err_out_free_mdio_irq;
+		goto err_out_free_mii_bus;
 	err = tc_mii_probe(dev);
 	if (err)
 		goto err_out_unregister_bus;
@@ -703,8 +678,6 @@ static int tc_mii_init(struct net_device *dev)
 
 err_out_unregister_bus:
 	mdiobus_unregister(lp->mii_bus);
-err_out_free_mdio_irq:
-	kfree(lp->mii_bus->irq);
 err_out_free_mii_bus:
 	mdiobus_free(lp->mii_bus);
 err_out:
@@ -882,7 +855,6 @@ static void tc35815_remove_one(struct pci_dev *pdev)
 
 	phy_disconnect(lp->phy_dev);
 	mdiobus_unregister(lp->mii_bus);
-	kfree(lp->mii_bus->irq);
 	mdiobus_free(lp->mii_bus);
 	unregister_netdev(dev);
 	free_netdev(dev);
diff --git a/drivers/net/ethernet/xilinx/ll_temac.h b/drivers/net/ethernet/xilinx/ll_temac.h
index 522abe2ff25a..902457e43628 100644
--- a/drivers/net/ethernet/xilinx/ll_temac.h
+++ b/drivers/net/ethernet/xilinx/ll_temac.h
@@ -337,7 +337,6 @@ struct temac_local {
 
 	/* MDIO bus data */
 	struct mii_bus *mii_bus;	/* MII bus reference */
-	int mdio_irqs[PHY_MAX_ADDR];	/* IRQs table for MDIO bus */
 
 	/* IO registers, dma functions and IRQs */
 	void __iomem *regs;
diff --git a/drivers/net/ethernet/xilinx/ll_temac_mdio.c b/drivers/net/ethernet/xilinx/ll_temac_mdio.c
index 415de1eaf641..7714aff78b7d 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_mdio.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_mdio.c
@@ -92,7 +92,6 @@ int temac_mdio_setup(struct temac_local *lp, struct device_node *np)
 	bus->read = temac_mdio_read;
 	bus->write = temac_mdio_write;
 	bus->parent = lp->dev;
-	bus->irq = lp->mdio_irqs; /* preallocated IRQ table */
 
 	lp->mii_bus = bus;
 
@@ -114,7 +113,6 @@ int temac_mdio_setup(struct temac_local *lp, struct device_node *np)
 void temac_mdio_teardown(struct temac_local *lp)
 {
 	mdiobus_unregister(lp->mii_bus);
-	kfree(lp->mii_bus->irq);
 	mdiobus_free(lp->mii_bus);
 	lp->mii_bus = NULL;
 }
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet.h b/drivers/net/ethernet/xilinx/xilinx_axienet.h
index 7cb9abac95c8..9ead4e269409 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet.h
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet.h
@@ -385,7 +385,6 @@ struct axidma_bd {
  * @phy_dev:	Pointer to PHY device structure attached to the axienet_local
  * @phy_node:	Pointer to device node structure
  * @mii_bus:	Pointer to MII bus structure
- * @mdio_irqs:	IRQs table for MDIO bus required in mii_bus structure
  * @regs:	Base address for the axienet_local device address space
  * @dma_regs:	Base address for the axidma device address space
  * @dma_err_tasklet: Tasklet structure to process Axi DMA errors
@@ -426,7 +425,6 @@ struct axienet_local {
 
 	/* MDIO bus data */
 	struct mii_bus *mii_bus;	/* MII bus reference */
-	int mdio_irqs[PHY_MAX_ADDR];	/* IRQs table for MDIO bus */
 
 	/* IO registers, dma functions and IRQs */
 	void __iomem *regs;
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c b/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
index 507bbb0355c2..63307ea97846 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_mdio.c
@@ -212,7 +212,6 @@ issue:
 	bus->read = axienet_mdio_read;
 	bus->write = axienet_mdio_write;
 	bus->parent = lp->dev;
-	bus->irq = lp->mdio_irqs; /* preallocated IRQ table */
 	lp->mii_bus = bus;
 
 	ret = of_mdiobus_register(bus, np1);
@@ -232,7 +231,6 @@ issue:
 void axienet_mdio_teardown(struct axienet_local *lp)
 {
 	mdiobus_unregister(lp->mii_bus);
-	kfree(lp->mii_bus->irq);
 	mdiobus_free(lp->mii_bus);
 	lp->mii_bus = NULL;
 }
diff --git a/drivers/net/ethernet/xilinx/xilinx_emaclite.c b/drivers/net/ethernet/xilinx/xilinx_emaclite.c
index cf468c87ce57..e324b3092380 100644
--- a/drivers/net/ethernet/xilinx/xilinx_emaclite.c
+++ b/drivers/net/ethernet/xilinx/xilinx_emaclite.c
@@ -114,7 +114,6 @@
  * @phy_dev:		pointer to the PHY device
  * @phy_node:		pointer to the PHY device node
  * @mii_bus:		pointer to the MII bus
- * @mdio_irqs:		IRQs table for MDIO bus
  * @last_link:		last link status
  * @has_mdio:		indicates whether MDIO is included in the HW
  */
@@ -135,7 +134,6 @@ struct net_local {
 	struct device_node *phy_node;
 
 	struct mii_bus *mii_bus;
-	int mdio_irqs[PHY_MAX_ADDR];
 
 	int last_link;
 	bool has_mdio;
@@ -829,7 +827,7 @@ static int xemaclite_mdio_setup(struct net_local *lp, struct device *dev)
 			dev_info(dev,
 				 "MDIO of the phy is not registered yet\n");
 		else
-			put_device(&phydev->dev);
+			put_device(&phydev->mdio.dev);
 		return 0;
 	}
 
@@ -852,7 +850,6 @@ static int xemaclite_mdio_setup(struct net_local *lp, struct device *dev)
 	bus->read = xemaclite_mdio_read;
 	bus->write = xemaclite_mdio_write;
 	bus->parent = dev;
-	bus->irq = lp->mdio_irqs; /* preallocated IRQ table */
 
 	lp->mii_bus = bus;
 
@@ -1196,7 +1193,6 @@ static int xemaclite_of_remove(struct platform_device *of_dev)
 	/* Un-register the mii_bus, if configured */
 	if (lp->has_mdio) {
 		mdiobus_unregister(lp->mii_bus);
-		kfree(lp->mii_bus->irq);
 		mdiobus_free(lp->mii_bus);
 		lp->mii_bus = NULL;
 	}
diff --git a/drivers/net/fddi/defxx.c b/drivers/net/fddi/defxx.c
index 7f975a2c8990..b0de8ecd7fe8 100644
--- a/drivers/net/fddi/defxx.c
+++ b/drivers/net/fddi/defxx.c
@@ -533,8 +533,8 @@ static int dfx_register(struct device *bdev)
 	const char *print_name = dev_name(bdev);
 	struct net_device *dev;
 	DFX_board_t	  *bp;			/* board pointer */
-	resource_size_t bar_start[3];		/* pointers to ports */
-	resource_size_t bar_len[3];		/* resource length */
+	resource_size_t bar_start[3] = {0};	/* pointers to ports */
+	resource_size_t bar_len[3] = {0};	/* resource length */
 	int alloc_size;				/* total buffer size used */
 	struct resource *region;
 	int err = 0;
@@ -3697,8 +3697,8 @@ static void dfx_unregister(struct device *bdev)
 	int dfx_bus_pci = dev_is_pci(bdev);
 	int dfx_bus_tc = DFX_BUS_TC(bdev);
 	int dfx_use_mmio = DFX_MMIO || dfx_bus_tc;
-	resource_size_t bar_start[3];		/* pointers to ports */
-	resource_size_t bar_len[3];		/* resource lengths */
+	resource_size_t bar_start[3] = {0};	/* pointers to ports */
+	resource_size_t bar_len[3] = {0};	/* resource lengths */
 	int		alloc_size;		/* total buffer size used */
 
 	unregister_netdev(dev);
diff --git a/drivers/net/geneve.c b/drivers/net/geneve.c
index 58efdec12f30..0bf7edd99573 100644
--- a/drivers/net/geneve.c
+++ b/drivers/net/geneve.c
@@ -71,8 +71,14 @@ struct geneve_dev {
 	__be16		   dst_port;
 	bool		   collect_md;
 	struct gro_cells   gro_cells;
+	u32		   flags;
 };
 
+/* Geneve device flags */
+#define GENEVE_F_UDP_CSUM		BIT(0)
+#define GENEVE_F_UDP_ZERO_CSUM6_TX	BIT(1)
+#define GENEVE_F_UDP_ZERO_CSUM6_RX	BIT(2)
+
 struct geneve_sock {
 	bool			collect_md;
 	struct list_head	list;
@@ -81,6 +87,7 @@ struct geneve_sock {
 	int			refcnt;
 	struct udp_offload	udp_offloads;
 	struct hlist_head	vni_list[VNI_HASH_SIZE];
+	u32			flags;
 };
 
 static inline __u32 geneve_net_vni_hash(u8 vni[3])
@@ -343,7 +350,7 @@ error:
 }
 
 static struct socket *geneve_create_sock(struct net *net, bool ipv6,
-					 __be16 port)
+					 __be16 port, u32 flags)
 {
 	struct socket *sock;
 	struct udp_port_cfg udp_conf;
@@ -354,6 +361,8 @@ static struct socket *geneve_create_sock(struct net *net, bool ipv6,
 	if (ipv6) {
 		udp_conf.family = AF_INET6;
 		udp_conf.ipv6_v6only = 1;
+		udp_conf.use_udp6_rx_checksums =
+		    !(flags & GENEVE_F_UDP_ZERO_CSUM6_RX);
 	} else {
 		udp_conf.family = AF_INET;
 		udp_conf.local_ip.s_addr = htonl(INADDR_ANY);
@@ -371,16 +380,27 @@ static struct socket *geneve_create_sock(struct net *net, bool ipv6,
 
 static void geneve_notify_add_rx_port(struct geneve_sock *gs)
 {
+	struct net_device *dev;
 	struct sock *sk = gs->sock->sk;
+	struct net *net = sock_net(sk);
 	sa_family_t sa_family = sk->sk_family;
+	__be16 port = inet_sk(sk)->inet_sport;
 	int err;
 
 	if (sa_family == AF_INET) {
-		err = udp_add_offload(&gs->udp_offloads);
+		err = udp_add_offload(sock_net(sk), &gs->udp_offloads);
 		if (err)
 			pr_warn("geneve: udp_add_offload failed with status %d\n",
 				err);
 	}
+
+	rcu_read_lock();
+	for_each_netdev_rcu(net, dev) {
+		if (dev->netdev_ops->ndo_add_geneve_port)
+			dev->netdev_ops->ndo_add_geneve_port(dev, sa_family,
+							     port);
+	}
+	rcu_read_unlock();
 }
 
 static int geneve_hlen(struct genevehdr *gh)
@@ -480,7 +500,7 @@ static int geneve_gro_complete(struct sk_buff *skb, int nhoff,
 
 /* Create new listen socket if needed */
 static struct geneve_sock *geneve_socket_create(struct net *net, __be16 port,
-						bool ipv6)
+						bool ipv6, u32 flags)
 {
 	struct geneve_net *gn = net_generic(net, geneve_net_id);
 	struct geneve_sock *gs;
@@ -492,7 +512,7 @@ static struct geneve_sock *geneve_socket_create(struct net *net, __be16 port,
 	if (!gs)
 		return ERR_PTR(-ENOMEM);
 
-	sock = geneve_create_sock(net, ipv6, port);
+	sock = geneve_create_sock(net, ipv6, port, flags);
 	if (IS_ERR(sock)) {
 		kfree(gs);
 		return ERR_CAST(sock);
@@ -521,8 +541,20 @@ static struct geneve_sock *geneve_socket_create(struct net *net, __be16 port,
 
 static void geneve_notify_del_rx_port(struct geneve_sock *gs)
 {
+	struct net_device *dev;
 	struct sock *sk = gs->sock->sk;
+	struct net *net = sock_net(sk);
 	sa_family_t sa_family = sk->sk_family;
+	__be16 port = inet_sk(sk)->inet_sport;
+
+	rcu_read_lock();
+	for_each_netdev_rcu(net, dev) {
+		if (dev->netdev_ops->ndo_del_geneve_port)
+			dev->netdev_ops->ndo_del_geneve_port(dev, sa_family,
+							     port);
+	}
+
+	rcu_read_unlock();
 
 	if (sa_family == AF_INET)
 		udp_del_offload(&gs->udp_offloads);
@@ -575,12 +607,13 @@ static int geneve_sock_add(struct geneve_dev *geneve, bool ipv6)
 		goto out;
 	}
 
-	gs = geneve_socket_create(net, geneve->dst_port, ipv6);
+	gs = geneve_socket_create(net, geneve->dst_port, ipv6, geneve->flags);
 	if (IS_ERR(gs))
 		return PTR_ERR(gs);
 
 out:
 	gs->collect_md = geneve->collect_md;
+	gs->flags = geneve->flags;
 #if IS_ENABLED(CONFIG_IPV6)
 	if (ipv6)
 		geneve->sock6 = gs;
@@ -642,11 +675,12 @@ static void geneve_build_header(struct genevehdr *geneveh,
 
 static int geneve_build_skb(struct rtable *rt, struct sk_buff *skb,
 			    __be16 tun_flags, u8 vni[3], u8 opt_len, u8 *opt,
-			    bool csum, bool xnet)
+			    u32 flags, bool xnet)
 {
 	struct genevehdr *gnvh;
 	int min_headroom;
 	int err;
+	bool udp_sum = !!(flags & GENEVE_F_UDP_CSUM);
 
 	skb_scrub_packet(skb, xnet);
 
@@ -658,7 +692,7 @@ static int geneve_build_skb(struct rtable *rt, struct sk_buff *skb,
 		goto free_rt;
 	}
 
-	skb = udp_tunnel_handle_offloads(skb, csum);
+	skb = udp_tunnel_handle_offloads(skb, udp_sum);
 	if (IS_ERR(skb)) {
 		err = PTR_ERR(skb);
 		goto free_rt;
@@ -678,11 +712,12 @@ free_rt:
 #if IS_ENABLED(CONFIG_IPV6)
 static int geneve6_build_skb(struct dst_entry *dst, struct sk_buff *skb,
 			     __be16 tun_flags, u8 vni[3], u8 opt_len, u8 *opt,
-			     bool csum, bool xnet)
+			     u32 flags, bool xnet)
 {
 	struct genevehdr *gnvh;
 	int min_headroom;
 	int err;
+	bool udp_sum = !(flags & GENEVE_F_UDP_ZERO_CSUM6_TX);
 
 	skb_scrub_packet(skb, xnet);
 
@@ -694,7 +729,7 @@ static int geneve6_build_skb(struct dst_entry *dst, struct sk_buff *skb,
 		goto free_dst;
 	}
 
-	skb = udp_tunnel_handle_offloads(skb, csum);
+	skb = udp_tunnel_handle_offloads(skb, udp_sum);
 	if (IS_ERR(skb)) {
 		err = PTR_ERR(skb);
 		goto free_dst;
@@ -824,9 +859,9 @@ static netdev_tx_t geneve_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 	struct flowi4 fl4;
 	__u8 tos, ttl;
 	__be16 sport;
-	bool udp_csum;
 	__be16 df;
 	bool xnet = !net_eq(geneve->net, dev_net(geneve->dev));
+	u32 flags = geneve->flags;
 
 	if (geneve->collect_md) {
 		if (unlikely(!info || !(info->mode & IP_TUNNEL_INFO_TX))) {
@@ -857,9 +892,13 @@ static netdev_tx_t geneve_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 		if (key->tun_flags & TUNNEL_GENEVE_OPT)
 			opts = ip_tunnel_info_opts(info);
 
-		udp_csum = !!(key->tun_flags & TUNNEL_CSUM);
+		if (key->tun_flags & TUNNEL_CSUM)
+			flags |= GENEVE_F_UDP_CSUM;
+		else
+			flags &= ~GENEVE_F_UDP_CSUM;
+
 		err = geneve_build_skb(rt, skb, key->tun_flags, vni,
-				       info->options_len, opts, udp_csum, xnet);
+				       info->options_len, opts, flags, xnet);
 		if (unlikely(err))
 			goto err;
 
@@ -867,9 +906,8 @@ static netdev_tx_t geneve_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 		ttl = key->ttl;
 		df = key->tun_flags & TUNNEL_DONT_FRAGMENT ? htons(IP_DF) : 0;
 	} else {
-		udp_csum = false;
 		err = geneve_build_skb(rt, skb, 0, geneve->vni,
-				       0, NULL, udp_csum, xnet);
+				       0, NULL, flags, xnet);
 		if (unlikely(err))
 			goto err;
 
@@ -880,12 +918,11 @@ static netdev_tx_t geneve_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 		ttl = ttl ? : ip4_dst_hoplimit(&rt->dst);
 		df = 0;
 	}
-	err = udp_tunnel_xmit_skb(rt, gs4->sock->sk, skb, fl4.saddr, fl4.daddr,
-				  tos, ttl, df, sport, geneve->dst_port,
-				  !net_eq(geneve->net, dev_net(geneve->dev)),
-				  !udp_csum);
+	udp_tunnel_xmit_skb(rt, gs4->sock->sk, skb, fl4.saddr, fl4.daddr,
+			    tos, ttl, df, sport, geneve->dst_port,
+			    !net_eq(geneve->net, dev_net(geneve->dev)),
+			    !(flags & GENEVE_F_UDP_CSUM));
 
-	iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
 	return NETDEV_TX_OK;
 
 tx_error:
@@ -912,8 +949,8 @@ static netdev_tx_t geneve6_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 	struct flowi6 fl6;
 	__u8 prio, ttl;
 	__be16 sport;
-	bool udp_csum;
 	bool xnet = !net_eq(geneve->net, dev_net(geneve->dev));
+	u32 flags = geneve->flags;
 
 	if (geneve->collect_md) {
 		if (unlikely(!info || !(info->mode & IP_TUNNEL_INFO_TX))) {
@@ -942,19 +979,22 @@ static netdev_tx_t geneve6_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 		if (key->tun_flags & TUNNEL_GENEVE_OPT)
 			opts = ip_tunnel_info_opts(info);
 
-		udp_csum = !!(key->tun_flags & TUNNEL_CSUM);
+		if (key->tun_flags & TUNNEL_CSUM)
+			flags &= ~GENEVE_F_UDP_ZERO_CSUM6_TX;
+		else
+			flags |= GENEVE_F_UDP_ZERO_CSUM6_TX;
+
 		err = geneve6_build_skb(dst, skb, key->tun_flags, vni,
 					info->options_len, opts,
-					udp_csum, xnet);
+					flags, xnet);
 		if (unlikely(err))
 			goto err;
 
 		prio = ip_tunnel_ecn_encap(key->tos, iip, skb);
 		ttl = key->ttl;
 	} else {
-		udp_csum = false;
 		err = geneve6_build_skb(dst, skb, 0, geneve->vni,
-					0, NULL, udp_csum, xnet);
+					0, NULL, flags, xnet);
 		if (unlikely(err))
 			goto err;
 
@@ -964,9 +1004,10 @@ static netdev_tx_t geneve6_xmit_skb(struct sk_buff *skb, struct net_device *dev,
 			ttl = 1;
 		ttl = ttl ? : ip6_dst_hoplimit(dst);
 	}
-	err = udp_tunnel6_xmit_skb(dst, gs6->sock->sk, skb, dev,
-				   &fl6.saddr, &fl6.daddr, prio, ttl,
-				   sport, geneve->dst_port, !udp_csum);
+	udp_tunnel6_xmit_skb(dst, gs6->sock->sk, skb, dev,
+			     &fl6.saddr, &fl6.daddr, prio, ttl,
+			     sport, geneve->dst_port,
+			     !!(flags & GENEVE_F_UDP_ZERO_CSUM6_TX));
 	return NETDEV_TX_OK;
 
 tx_error:
@@ -998,6 +1039,34 @@ static netdev_tx_t geneve_xmit(struct sk_buff *skb, struct net_device *dev)
 	return geneve_xmit_skb(skb, dev, info);
 }
 
+static int __geneve_change_mtu(struct net_device *dev, int new_mtu, bool strict)
+{
+	/* The max_mtu calculation does not take account of GENEVE
+	 * options, to avoid excluding potentially valid
+	 * configurations.
+	 */
+	int max_mtu = IP_MAX_MTU - GENEVE_BASE_HLEN - sizeof(struct iphdr)
+		- dev->hard_header_len;
+
+	if (new_mtu < 68)
+		return -EINVAL;
+
+	if (new_mtu > max_mtu) {
+		if (strict)
+			return -EINVAL;
+
+		new_mtu = max_mtu;
+	}
+
+	dev->mtu = new_mtu;
+	return 0;
+}
+
+static int geneve_change_mtu(struct net_device *dev, int new_mtu)
+{
+	return __geneve_change_mtu(dev, new_mtu, true);
+}
+
 static int geneve_fill_metadata_dst(struct net_device *dev, struct sk_buff *skb)
 {
 	struct ip_tunnel_info *info = skb_tunnel_info(skb);
@@ -1042,7 +1111,7 @@ static const struct net_device_ops geneve_netdev_ops = {
 	.ndo_stop		= geneve_stop,
 	.ndo_start_xmit		= geneve_xmit,
 	.ndo_get_stats64	= ip_tunnel_get_stats64,
-	.ndo_change_mtu		= eth_change_mtu,
+	.ndo_change_mtu		= geneve_change_mtu,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_fill_metadata_dst	= geneve_fill_metadata_dst,
@@ -1065,6 +1134,30 @@ static struct device_type geneve_type = {
 	.name = "geneve",
 };
 
+/* Calls the ndo_add_geneve_port of the caller in order to
+ * supply the listening GENEVE udp ports. Callers are expected
+ * to implement the ndo_add_geneve_port.
+ */
+void geneve_get_rx_port(struct net_device *dev)
+{
+	struct net *net = dev_net(dev);
+	struct geneve_net *gn = net_generic(net, geneve_net_id);
+	struct geneve_sock *gs;
+	sa_family_t sa_family;
+	struct sock *sk;
+	__be16 port;
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(gs, &gn->sock_list, list) {
+		sk = gs->sock->sk;
+		sa_family = sk->sk_family;
+		port = inet_sk(sk)->inet_sport;
+		dev->netdev_ops->ndo_add_geneve_port(dev, sa_family, port);
+	}
+	rcu_read_unlock();
+}
+EXPORT_SYMBOL_GPL(geneve_get_rx_port);
+
 /* Initialize the device structure. */
 static void geneve_setup(struct net_device *dev)
 {
@@ -1085,6 +1178,7 @@ static void geneve_setup(struct net_device *dev)
 	dev->hw_features |= NETIF_F_GSO_SOFTWARE;
 
 	netif_keep_dst(dev);
+	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
 	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE | IFF_NO_QUEUE;
 	eth_hw_addr_random(dev);
 }
@@ -1097,6 +1191,9 @@ static const struct nla_policy geneve_policy[IFLA_GENEVE_MAX + 1] = {
 	[IFLA_GENEVE_TOS]		= { .type = NLA_U8 },
 	[IFLA_GENEVE_PORT]		= { .type = NLA_U16 },
 	[IFLA_GENEVE_COLLECT_METADATA]	= { .type = NLA_FLAG },
+	[IFLA_GENEVE_UDP_CSUM]		= { .type = NLA_U8 },
+	[IFLA_GENEVE_UDP_ZERO_CSUM6_TX]	= { .type = NLA_U8 },
+	[IFLA_GENEVE_UDP_ZERO_CSUM6_RX]	= { .type = NLA_U8 },
 };
 
 static int geneve_validate(struct nlattr *tb[], struct nlattr *data[])
@@ -1150,7 +1247,7 @@ static struct geneve_dev *geneve_find_dev(struct geneve_net *gn,
 static int geneve_configure(struct net *net, struct net_device *dev,
 			    union geneve_addr *remote,
 			    __u32 vni, __u8 ttl, __u8 tos, __be16 dst_port,
-			    bool metadata)
+			    bool metadata, u32 flags)
 {
 	struct geneve_net *gn = net_generic(net, geneve_net_id);
 	struct geneve_dev *t, *geneve = netdev_priv(dev);
@@ -1181,6 +1278,7 @@ static int geneve_configure(struct net *net, struct net_device *dev,
 	geneve->tos = tos;
 	geneve->dst_port = dst_port;
 	geneve->collect_md = metadata;
+	geneve->flags = flags;
 
 	t = geneve_find_dev(gn, dst_port, remote, geneve->vni,
 			    &tun_on_same_port, &tun_collect_md);
@@ -1219,6 +1317,7 @@ static int geneve_newlink(struct net *net, struct net_device *dev,
 	bool metadata = false;
 	union geneve_addr remote = geneve_remote_unspec;
 	__u32 vni = 0;
+	u32 flags = 0;
 
 	if (data[IFLA_GENEVE_REMOTE] && data[IFLA_GENEVE_REMOTE6])
 		return -EINVAL;
@@ -1259,8 +1358,20 @@ static int geneve_newlink(struct net *net, struct net_device *dev,
 	if (data[IFLA_GENEVE_COLLECT_METADATA])
 		metadata = true;
 
+	if (data[IFLA_GENEVE_UDP_CSUM] &&
+	    nla_get_u8(data[IFLA_GENEVE_UDP_CSUM]))
+		flags |= GENEVE_F_UDP_CSUM;
+
+	if (data[IFLA_GENEVE_UDP_ZERO_CSUM6_TX] &&
+	    nla_get_u8(data[IFLA_GENEVE_UDP_ZERO_CSUM6_TX]))
+		flags |= GENEVE_F_UDP_ZERO_CSUM6_TX;
+
+	if (data[IFLA_GENEVE_UDP_ZERO_CSUM6_RX] &&
+	    nla_get_u8(data[IFLA_GENEVE_UDP_ZERO_CSUM6_RX]))
+		flags |= GENEVE_F_UDP_ZERO_CSUM6_RX;
+
 	return geneve_configure(net, dev, &remote, vni, ttl, tos, dst_port,
-				metadata);
+				metadata, flags);
 }
 
 static void geneve_dellink(struct net_device *dev, struct list_head *head)
@@ -1279,6 +1390,9 @@ static size_t geneve_get_size(const struct net_device *dev)
 		nla_total_size(sizeof(__u8)) +  /* IFLA_GENEVE_TOS */
 		nla_total_size(sizeof(__be16)) +  /* IFLA_GENEVE_PORT */
 		nla_total_size(0) +	 /* IFLA_GENEVE_COLLECT_METADATA */
+		nla_total_size(sizeof(__u8)) + /* IFLA_GENEVE_UDP_CSUM */
+		nla_total_size(sizeof(__u8)) + /* IFLA_GENEVE_UDP_ZERO_CSUM6_TX */
+		nla_total_size(sizeof(__u8)) + /* IFLA_GENEVE_UDP_ZERO_CSUM6_RX */
 		0;
 }
 
@@ -1315,6 +1429,14 @@ static int geneve_fill_info(struct sk_buff *skb, const struct net_device *dev)
 			goto nla_put_failure;
 	}
 
+	if (nla_put_u8(skb, IFLA_GENEVE_UDP_CSUM,
+		       !!(geneve->flags & GENEVE_F_UDP_CSUM)) ||
+	    nla_put_u8(skb, IFLA_GENEVE_UDP_ZERO_CSUM6_TX,
+		       !!(geneve->flags & GENEVE_F_UDP_ZERO_CSUM6_TX)) ||
+	    nla_put_u8(skb, IFLA_GENEVE_UDP_ZERO_CSUM6_RX,
+		       !!(geneve->flags & GENEVE_F_UDP_ZERO_CSUM6_RX)))
+		goto nla_put_failure;
+
 	return 0;
 
 nla_put_failure:
@@ -1348,12 +1470,23 @@ struct net_device *geneve_dev_create_fb(struct net *net, const char *name,
 		return dev;
 
 	err = geneve_configure(net, dev, &geneve_remote_unspec,
-			       0, 0, 0, htons(dst_port), true);
-	if (err) {
-		free_netdev(dev);
-		return ERR_PTR(err);
-	}
+			       0, 0, 0, htons(dst_port), true,
+			       GENEVE_F_UDP_ZERO_CSUM6_RX);
+	if (err)
+		goto err;
+
+	/* openvswitch users expect packet sizes to be unrestricted,
+	 * so set the largest MTU we can.
+	 */
+	err = __geneve_change_mtu(dev, IP_MAX_MTU, false);
+	if (err)
+		goto err;
+
 	return dev;
+
+ err:
+	free_netdev(dev);
+	return ERR_PTR(err);
 }
 EXPORT_SYMBOL_GPL(geneve_dev_create_fb);
 
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index 5fa98f599b3d..fcb92c0d0eb9 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -124,37 +124,22 @@ struct ndis_tcp_ip_checksum_info;
 /*
  * Represent netvsc packet which contains 1 RNDIS and 1 ethernet frame
  * within the RNDIS
+ *
+ * The size of this structure is less than 48 bytes and we can now
+ * place this structure in the skb->cb field.
  */
 struct hv_netvsc_packet {
 	/* Bookkeeping stuff */
-	u32 status;
-
-	bool is_data_pkt;
-	bool xmit_more; /* from skb */
-	bool cp_partial; /* partial copy into send buffer */
+	u8 cp_partial; /* partial copy into send buffer */
 
-	u16 vlan_tci;
+	u8 rmsg_size; /* RNDIS header and PPI size */
+	u8 rmsg_pgcnt; /* page count of RNDIS header and PPI */
+	u8 page_buf_cnt;
 
 	u16 q_idx;
-	struct vmbus_channel *channel;
-
-	u64 send_completion_tid;
-	void *send_completion_ctx;
-	void (*send_completion)(void *context);
-
 	u32 send_buf_index;
 
-	/* This points to the memory after page_buf */
-	struct rndis_message *rndis_msg;
-
-	u32 rmsg_size; /* RNDIS header and PPI size */
-	u32 rmsg_pgcnt; /* page count of RNDIS header and PPI */
-
 	u32 total_data_buflen;
-	/* Points to the send/receive buffer where the ethernet frame is */
-	void *data;
-	u32 page_buf_cnt;
-	struct hv_page_buffer *page_buf;
 };
 
 struct netvsc_device_info {
@@ -177,7 +162,6 @@ struct rndis_device {
 
 	enum rndis_device_state state;
 	bool link_state;
-	bool link_change;
 	atomic_t new_req_id;
 
 	spinlock_t request_lock;
@@ -188,16 +172,22 @@ struct rndis_device {
 
 
 /* Interface */
+struct rndis_message;
 int netvsc_device_add(struct hv_device *device, void *additional_info);
 int netvsc_device_remove(struct hv_device *device);
 int netvsc_send(struct hv_device *device,
-		struct hv_netvsc_packet *packet);
+		struct hv_netvsc_packet *packet,
+		struct rndis_message *rndis_msg,
+		struct hv_page_buffer **page_buffer,
+		struct sk_buff *skb);
 void netvsc_linkstatus_callback(struct hv_device *device_obj,
 				struct rndis_message *resp);
-void netvsc_xmit_completion(void *context);
 int netvsc_recv_callback(struct hv_device *device_obj,
 			struct hv_netvsc_packet *packet,
-			struct ndis_tcp_ip_checksum_info *csum_info);
+			void **data,
+			struct ndis_tcp_ip_checksum_info *csum_info,
+			struct vmbus_channel *channel,
+			u16 vlan_tci);
 void netvsc_channel_cb(void *context);
 int rndis_filter_open(struct hv_device *dev);
 int rndis_filter_close(struct hv_device *dev);
@@ -205,12 +195,13 @@ int rndis_filter_device_add(struct hv_device *dev,
 			void *additional_info);
 void rndis_filter_device_remove(struct hv_device *dev);
 int rndis_filter_receive(struct hv_device *dev,
-			struct hv_netvsc_packet *pkt);
+			struct hv_netvsc_packet *pkt,
+			void **data,
+			struct vmbus_channel *channel);
 
 int rndis_filter_set_packet_filter(struct rndis_device *dev, u32 new_filter);
 int rndis_filter_set_device_mac(struct hv_device *hdev, char *mac);
 
-
 #define NVSP_INVALID_PROTOCOL_VERSION	((u32)0xFFFFFFFF)
 
 #define NVSP_PROTOCOL_VERSION_1		2
@@ -633,7 +624,7 @@ struct nvsp_message {
 #define RNDIS_PKT_ALIGN_DEFAULT 8
 
 struct multi_send_data {
-	spinlock_t lock; /* protect struct multi_send_data */
+	struct sk_buff *skb; /* skb containing the pkt */
 	struct hv_netvsc_packet *pkt; /* netvsc pkt pending */
 	u32 count; /* counter of batched packets */
 };
@@ -644,11 +635,24 @@ struct netvsc_stats {
 	struct u64_stats_sync syncp;
 };
 
+struct netvsc_reconfig {
+	struct list_head list;
+	u32 event;
+};
+
 /* The context of the netvsc device  */
 struct net_device_context {
 	/* point back to our device context */
 	struct hv_device *device_ctx;
+	/* reconfigure work */
 	struct delayed_work dwork;
+	/* last reconfig time */
+	unsigned long last_reconfig;
+	/* reconfig events */
+	struct list_head reconfig_events;
+	/* list protection */
+	spinlock_t lock;
+
 	struct work_struct work;
 	u32 msg_enable; /* debug level */
 
@@ -1260,5 +1264,4 @@ struct rndis_message {
 #define TRANSPORT_INFO_IPV6_TCP ((INFO_IPV6 << 16) | INFO_TCP)
 #define TRANSPORT_INFO_IPV6_UDP ((INFO_IPV6 << 16) | INFO_UDP)
 
-
 #endif /* _HYPERV_NET_H */
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 51e4c0fd0a74..ec313fc08d82 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -38,7 +38,6 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 {
 	struct netvsc_device *net_device;
 	struct net_device *ndev = hv_get_drvdata(device);
-	int i;
 
 	net_device = kzalloc(sizeof(struct netvsc_device), GFP_KERNEL);
 	if (!net_device)
@@ -58,9 +57,6 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 	net_device->max_pkt = RNDIS_MAX_PKT_DEFAULT;
 	net_device->pkt_align = RNDIS_PKT_ALIGN_DEFAULT;
 
-	for (i = 0; i < num_online_cpus(); i++)
-		spin_lock_init(&net_device->msd[i].lock);
-
 	hv_set_drvdata(device, net_device);
 	return net_device;
 }
@@ -610,6 +606,7 @@ static inline void netvsc_free_send_slot(struct netvsc_device *net_device,
 }
 
 static void netvsc_send_completion(struct netvsc_device *net_device,
+				   struct vmbus_channel *incoming_channel,
 				   struct hv_device *device,
 				   struct vmpacket_descriptor *packet)
 {
@@ -617,6 +614,7 @@ static void netvsc_send_completion(struct netvsc_device *net_device,
 	struct hv_netvsc_packet *nvsc_packet;
 	struct net_device *ndev;
 	u32 send_index;
+	struct sk_buff *skb;
 
 	ndev = net_device->ndev;
 
@@ -642,18 +640,17 @@ static void netvsc_send_completion(struct netvsc_device *net_device,
 		int queue_sends;
 
 		/* Get the send context */
-		nvsc_packet = (struct hv_netvsc_packet *)(unsigned long)
-			packet->trans_id;
+		skb = (struct sk_buff *)(unsigned long)packet->trans_id;
 
 		/* Notify the layer above us */
-		if (nvsc_packet) {
+		if (skb) {
+			nvsc_packet = (struct hv_netvsc_packet *) skb->cb;
 			send_index = nvsc_packet->send_buf_index;
 			if (send_index != NETVSC_INVALID_INDEX)
 				netvsc_free_send_slot(net_device, send_index);
 			q_idx = nvsc_packet->q_idx;
-			channel = nvsc_packet->channel;
-			nvsc_packet->send_completion(nvsc_packet->
-						     send_completion_ctx);
+			channel = incoming_channel;
+			dev_kfree_skb_any(skb);
 		}
 
 		num_outstanding_sends =
@@ -705,12 +702,17 @@ static u32 netvsc_get_next_send_section(struct netvsc_device *net_device)
 static u32 netvsc_copy_to_send_buf(struct netvsc_device *net_device,
 				   unsigned int section_index,
 				   u32 pend_size,
-				   struct hv_netvsc_packet *packet)
+				   struct hv_netvsc_packet *packet,
+				   struct rndis_message *rndis_msg,
+				   struct hv_page_buffer **pb,
+				   struct sk_buff *skb)
 {
 	char *start = net_device->send_buf;
 	char *dest = start + (section_index * net_device->send_section_size)
 		     + pend_size;
 	int i;
+	bool is_data_pkt = (skb != NULL) ? true : false;
+	bool xmit_more = (skb != NULL) ? skb->xmit_more : false;
 	u32 msg_size = 0;
 	u32 padding = 0;
 	u32 remain = packet->total_data_buflen % net_device->pkt_align;
@@ -718,17 +720,17 @@ static u32 netvsc_copy_to_send_buf(struct netvsc_device *net_device,
 		packet->page_buf_cnt;
 
 	/* Add padding */
-	if (packet->is_data_pkt && packet->xmit_more && remain &&
+	if (is_data_pkt && xmit_more && remain &&
 	    !packet->cp_partial) {
 		padding = net_device->pkt_align - remain;
-		packet->rndis_msg->msg_len += padding;
+		rndis_msg->msg_len += padding;
 		packet->total_data_buflen += padding;
 	}
 
 	for (i = 0; i < page_count; i++) {
-		char *src = phys_to_virt(packet->page_buf[i].pfn << PAGE_SHIFT);
-		u32 offset = packet->page_buf[i].offset;
-		u32 len = packet->page_buf[i].len;
+		char *src = phys_to_virt((*pb)[i].pfn << PAGE_SHIFT);
+		u32 offset = (*pb)[i].offset;
+		u32 len = (*pb)[i].len;
 
 		memcpy(dest, (src + offset), len);
 		msg_size += len;
@@ -745,19 +747,22 @@ static u32 netvsc_copy_to_send_buf(struct netvsc_device *net_device,
 
 static inline int netvsc_send_pkt(
 	struct hv_netvsc_packet *packet,
-	struct netvsc_device *net_device)
+	struct netvsc_device *net_device,
+	struct hv_page_buffer **pb,
+	struct sk_buff *skb)
 {
 	struct nvsp_message nvmsg;
-	struct vmbus_channel *out_channel = packet->channel;
 	u16 q_idx = packet->q_idx;
+	struct vmbus_channel *out_channel = net_device->chn_table[q_idx];
 	struct net_device *ndev = net_device->ndev;
 	u64 req_id;
 	int ret;
 	struct hv_page_buffer *pgbuf;
 	u32 ring_avail = hv_ringbuf_avail_percent(&out_channel->outbound);
+	bool xmit_more = (skb != NULL) ? skb->xmit_more : false;
 
 	nvmsg.hdr.msg_type = NVSP_MSG1_TYPE_SEND_RNDIS_PKT;
-	if (packet->is_data_pkt) {
+	if (skb != NULL) {
 		/* 0 is RMC_DATA; */
 		nvmsg.msg.v1_msg.send_rndis_pkt.channel_type = 0;
 	} else {
@@ -773,10 +778,7 @@ static inline int netvsc_send_pkt(
 		nvmsg.msg.v1_msg.send_rndis_pkt.send_buf_section_size =
 			packet->total_data_buflen;
 
-	if (packet->send_completion)
-		req_id = (ulong)packet;
-	else
-		req_id = 0;
+	req_id = (ulong)skb;
 
 	if (out_channel->rescind)
 		return -ENODEV;
@@ -789,11 +791,11 @@ static inline int netvsc_send_pkt(
 	 * unnecessarily.
 	 */
 	if (ring_avail < (RING_AVAIL_PERCENT_LOWATER + 1))
-		packet->xmit_more = false;
+		xmit_more = false;
 
 	if (packet->page_buf_cnt) {
-		pgbuf = packet->cp_partial ? packet->page_buf +
-			packet->rmsg_pgcnt : packet->page_buf;
+		pgbuf = packet->cp_partial ? (*pb) +
+			packet->rmsg_pgcnt : (*pb);
 		ret = vmbus_sendpacket_pagebuffer_ctl(out_channel,
 						      pgbuf,
 						      packet->page_buf_cnt,
@@ -801,14 +803,14 @@ static inline int netvsc_send_pkt(
 						      sizeof(struct nvsp_message),
 						      req_id,
 						      VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED,
-						      !packet->xmit_more);
+						      !xmit_more);
 	} else {
 		ret = vmbus_sendpacket_ctl(out_channel, &nvmsg,
 					   sizeof(struct nvsp_message),
 					   req_id,
 					   VM_PKT_DATA_INBAND,
 					   VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED,
-					   !packet->xmit_more);
+					   !xmit_more);
 	}
 
 	if (ret == 0) {
@@ -839,8 +841,23 @@ static inline int netvsc_send_pkt(
 	return ret;
 }
 
+/* Move packet out of multi send data (msd), and clear msd */
+static inline void move_pkt_msd(struct hv_netvsc_packet **msd_send,
+				struct sk_buff **msd_skb,
+				struct multi_send_data *msdp)
+{
+	*msd_skb = msdp->skb;
+	*msd_send = msdp->pkt;
+	msdp->skb = NULL;
+	msdp->pkt = NULL;
+	msdp->count = 0;
+}
+
 int netvsc_send(struct hv_device *device,
-		struct hv_netvsc_packet *packet)
+		struct hv_netvsc_packet *packet,
+		struct rndis_message *rndis_msg,
+		struct hv_page_buffer **pb,
+		struct sk_buff *skb)
 {
 	struct netvsc_device *net_device;
 	int ret = 0, m_ret = 0;
@@ -848,33 +865,36 @@ int netvsc_send(struct hv_device *device,
 	u16 q_idx = packet->q_idx;
 	u32 pktlen = packet->total_data_buflen, msd_len = 0;
 	unsigned int section_index = NETVSC_INVALID_INDEX;
-	unsigned long flag;
 	struct multi_send_data *msdp;
 	struct hv_netvsc_packet *msd_send = NULL, *cur_send = NULL;
+	struct sk_buff *msd_skb = NULL;
 	bool try_batch;
+	bool xmit_more = (skb != NULL) ? skb->xmit_more : false;
 
 	net_device = get_outbound_net_device(device);
 	if (!net_device)
 		return -ENODEV;
 
 	out_channel = net_device->chn_table[q_idx];
-	if (!out_channel) {
-		out_channel = device->channel;
-		q_idx = 0;
-		packet->q_idx = 0;
-	}
-	packet->channel = out_channel;
+
 	packet->send_buf_index = NETVSC_INVALID_INDEX;
 	packet->cp_partial = false;
 
+	/* Send control message directly without accessing msd (Multi-Send
+	 * Data) field which may be changed during data packet processing.
+	 */
+	if (!skb) {
+		cur_send = packet;
+		goto send_now;
+	}
+
 	msdp = &net_device->msd[q_idx];
 
 	/* batch packets in send buffer if possible */
-	spin_lock_irqsave(&msdp->lock, flag);
 	if (msdp->pkt)
 		msd_len = msdp->pkt->total_data_buflen;
 
-	try_batch = packet->is_data_pkt && msd_len > 0 && msdp->count <
+	try_batch = (skb != NULL) && msd_len > 0 && msdp->count <
 		    net_device->max_pkt;
 
 	if (try_batch && msd_len + pktlen + net_device->pkt_align <
@@ -886,21 +906,19 @@ int netvsc_send(struct hv_device *device,
 		section_index = msdp->pkt->send_buf_index;
 		packet->cp_partial = true;
 
-	} else if (packet->is_data_pkt && pktlen + net_device->pkt_align <
+	} else if ((skb != NULL) && pktlen + net_device->pkt_align <
 		   net_device->send_section_size) {
 		section_index = netvsc_get_next_send_section(net_device);
 		if (section_index != NETVSC_INVALID_INDEX) {
-				msd_send = msdp->pkt;
-				msdp->pkt = NULL;
-				msdp->count = 0;
-				msd_len = 0;
+			move_pkt_msd(&msd_send, &msd_skb, msdp);
+			msd_len = 0;
 		}
 	}
 
 	if (section_index != NETVSC_INVALID_INDEX) {
 		netvsc_copy_to_send_buf(net_device,
 					section_index, msd_len,
-					packet);
+					packet, rndis_msg, pb, skb);
 
 		packet->send_buf_index = section_index;
 
@@ -912,38 +930,37 @@ int netvsc_send(struct hv_device *device,
 			packet->total_data_buflen += msd_len;
 		}
 
-		if (msdp->pkt)
-			netvsc_xmit_completion(msdp->pkt);
+		if (msdp->skb)
+			dev_kfree_skb_any(msdp->skb);
 
-		if (packet->xmit_more && !packet->cp_partial) {
+		if (xmit_more && !packet->cp_partial) {
+			msdp->skb = skb;
 			msdp->pkt = packet;
 			msdp->count++;
 		} else {
 			cur_send = packet;
+			msdp->skb = NULL;
 			msdp->pkt = NULL;
 			msdp->count = 0;
 		}
 	} else {
-		msd_send = msdp->pkt;
-		msdp->pkt = NULL;
-		msdp->count = 0;
+		move_pkt_msd(&msd_send, &msd_skb, msdp);
 		cur_send = packet;
 	}
 
-	spin_unlock_irqrestore(&msdp->lock, flag);
-
 	if (msd_send) {
-		m_ret = netvsc_send_pkt(msd_send, net_device);
+		m_ret = netvsc_send_pkt(msd_send, net_device, NULL, msd_skb);
 
 		if (m_ret != 0) {
 			netvsc_free_send_slot(net_device,
 					      msd_send->send_buf_index);
-			netvsc_xmit_completion(msd_send);
+			dev_kfree_skb_any(msd_skb);
 		}
 	}
 
+send_now:
 	if (cur_send)
-		ret = netvsc_send_pkt(cur_send, net_device);
+		ret = netvsc_send_pkt(cur_send, net_device, pb, skb);
 
 	if (ret != 0 && section_index != NETVSC_INVALID_INDEX)
 		netvsc_free_send_slot(net_device, section_index);
@@ -1009,6 +1026,7 @@ static void netvsc_receive(struct netvsc_device *net_device,
 	int i;
 	int count = 0;
 	struct net_device *ndev;
+	void *data;
 
 	ndev = net_device->ndev;
 
@@ -1043,22 +1061,19 @@ static void netvsc_receive(struct netvsc_device *net_device,
 	}
 
 	count = vmxferpage_packet->range_cnt;
-	netvsc_packet->channel = channel;
 
 	/* Each range represents 1 RNDIS pkt that contains 1 ethernet frame */
 	for (i = 0; i < count; i++) {
 		/* Initialize the netvsc packet */
-		netvsc_packet->status = NVSP_STAT_SUCCESS;
-		netvsc_packet->data = (void *)((unsigned long)net_device->
+		data = (void *)((unsigned long)net_device->
 			recv_buf + vmxferpage_packet->ranges[i].byte_offset);
 		netvsc_packet->total_data_buflen =
 					vmxferpage_packet->ranges[i].byte_count;
 
 		/* Pass it to the upper layer */
-		rndis_filter_receive(device, netvsc_packet);
+		status = rndis_filter_receive(device, netvsc_packet, &data,
+					      channel);
 
-		if (netvsc_packet->status != NVSP_STAT_SUCCESS)
-			status = NVSP_STAT_FAIL;
 	}
 
 	netvsc_send_recv_completion(device, channel, net_device,
@@ -1150,6 +1165,7 @@ void netvsc_channel_cb(void *context)
 				switch (desc->type) {
 				case VM_PKT_COMP:
 					netvsc_send_completion(net_device,
+								channel,
 								device, desc);
 					break;
 
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index 409b48e1e589..98e34fee45c7 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -42,6 +42,7 @@
 
 
 #define RING_SIZE_MIN 64
+#define LINKCHANGE_INT (2 * HZ)
 static int ring_size = 128;
 module_param(ring_size, int, S_IRUGO);
 MODULE_PARM_DESC(ring_size, "Ring buffer size (# of pages)");
@@ -195,65 +196,6 @@ static void *init_ppi_data(struct rndis_message *msg, u32 ppi_size,
 	return ppi;
 }
 
-union sub_key {
-	u64 k;
-	struct {
-		u8 pad[3];
-		u8 kb;
-		u32 ka;
-	};
-};
-
-/* Toeplitz hash function
- * data: network byte order
- * return: host byte order
- */
-static u32 comp_hash(u8 *key, int klen, void *data, int dlen)
-{
-	union sub_key subk;
-	int k_next = 4;
-	u8 dt;
-	int i, j;
-	u32 ret = 0;
-
-	subk.k = 0;
-	subk.ka = ntohl(*(u32 *)key);
-
-	for (i = 0; i < dlen; i++) {
-		subk.kb = key[k_next];
-		k_next = (k_next + 1) % klen;
-		dt = ((u8 *)data)[i];
-		for (j = 0; j < 8; j++) {
-			if (dt & 0x80)
-				ret ^= subk.ka;
-			dt <<= 1;
-			subk.k <<= 1;
-		}
-	}
-
-	return ret;
-}
-
-static bool netvsc_set_hash(u32 *hash, struct sk_buff *skb)
-{
-	struct flow_keys flow;
-	int data_len;
-
-	if (!skb_flow_dissect_flow_keys(skb, &flow, 0) ||
-	    !(flow.basic.n_proto == htons(ETH_P_IP) ||
-	      flow.basic.n_proto == htons(ETH_P_IPV6)))
-		return false;
-
-	if (flow.basic.ip_proto == IPPROTO_TCP)
-		data_len = 12;
-	else
-		data_len = 8;
-
-	*hash = comp_hash(netvsc_hash_key, HASH_KEYLEN, &flow, data_len);
-
-	return true;
-}
-
 static u16 netvsc_select_queue(struct net_device *ndev, struct sk_buff *skb,
 			void *accel_priv, select_queue_fallback_t fallback)
 {
@@ -266,23 +208,14 @@ static u16 netvsc_select_queue(struct net_device *ndev, struct sk_buff *skb,
 	if (nvsc_dev == NULL || ndev->real_num_tx_queues <= 1)
 		return 0;
 
-	if (netvsc_set_hash(&hash, skb)) {
-		q_idx = nvsc_dev->send_table[hash % VRSS_SEND_TAB_SIZE] %
-			ndev->real_num_tx_queues;
-		skb_set_hash(skb, hash, PKT_HASH_TYPE_L3);
-	}
-
-	return q_idx;
-}
+	hash = skb_get_hash(skb);
+	q_idx = nvsc_dev->send_table[hash % VRSS_SEND_TAB_SIZE] %
+		ndev->real_num_tx_queues;
 
-void netvsc_xmit_completion(void *context)
-{
-	struct hv_netvsc_packet *packet = (struct hv_netvsc_packet *)context;
-	struct sk_buff *skb = (struct sk_buff *)
-		(unsigned long)packet->send_completion_tid;
+	if (!nvsc_dev->chn_table[q_idx])
+		q_idx = 0;
 
-	if (skb)
-		dev_kfree_skb_any(skb);
+	return q_idx;
 }
 
 static u32 fill_pg_buf(struct page *page, u32 offset, u32 len,
@@ -320,9 +253,10 @@ static u32 fill_pg_buf(struct page *page, u32 offset, u32 len,
 }
 
 static u32 init_page_array(void *hdr, u32 len, struct sk_buff *skb,
-			   struct hv_netvsc_packet *packet)
+			   struct hv_netvsc_packet *packet,
+			   struct hv_page_buffer **page_buf)
 {
-	struct hv_page_buffer *pb = packet->page_buf;
+	struct hv_page_buffer *pb = *page_buf;
 	u32 slots_used = 0;
 	char *data = skb->data;
 	int frags = skb_shinfo(skb)->nr_frags;
@@ -432,8 +366,8 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	u32 net_trans_info;
 	u32 hash;
 	u32 skb_length;
-	u32 pkt_sz;
 	struct hv_page_buffer page_buf[MAX_PAGE_BUFFER_COUNT];
+	struct hv_page_buffer *pb = page_buf;
 	struct netvsc_stats *tx_stats = this_cpu_ptr(net_device_ctx->tx_stats);
 
 	/* We will atmost need two pages to describe the rndis
@@ -460,42 +394,34 @@ check_size:
 		goto check_size;
 	}
 
-	pkt_sz = sizeof(struct hv_netvsc_packet) + RNDIS_AND_PPI_SIZE;
-
-	ret = skb_cow_head(skb, pkt_sz);
+	/*
+	 * Place the rndis header in the skb head room and
+	 * the skb->cb will be used for hv_netvsc_packet
+	 * structure.
+	 */
+	ret = skb_cow_head(skb, RNDIS_AND_PPI_SIZE);
 	if (ret) {
 		netdev_err(net, "unable to alloc hv_netvsc_packet\n");
 		ret = -ENOMEM;
 		goto drop;
 	}
-	/* Use the headroom for building up the packet */
-	packet = (struct hv_netvsc_packet *)skb->head;
-
-	packet->status = 0;
-	packet->xmit_more = skb->xmit_more;
+	/* Use the skb control buffer for building up the packet */
+	BUILD_BUG_ON(sizeof(struct hv_netvsc_packet) >
+			FIELD_SIZEOF(struct sk_buff, cb));
+	packet = (struct hv_netvsc_packet *)skb->cb;
 
-	packet->vlan_tci = skb->vlan_tci;
-	packet->page_buf = page_buf;
 
 	packet->q_idx = skb_get_queue_mapping(skb);
 
-	packet->is_data_pkt = true;
 	packet->total_data_buflen = skb->len;
 
-	packet->rndis_msg = (struct rndis_message *)((unsigned long)packet +
-				sizeof(struct hv_netvsc_packet));
+	rndis_msg = (struct rndis_message *)skb->head;
 
-	memset(packet->rndis_msg, 0, RNDIS_AND_PPI_SIZE);
+	memset(rndis_msg, 0, RNDIS_AND_PPI_SIZE);
 
-	/* Set the completion routine */
-	packet->send_completion = netvsc_xmit_completion;
-	packet->send_completion_ctx = packet;
-	packet->send_completion_tid = (unsigned long)skb;
-
-	isvlan = packet->vlan_tci & VLAN_TAG_PRESENT;
+	isvlan = skb->vlan_tci & VLAN_TAG_PRESENT;
 
 	/* Add the rndis header */
-	rndis_msg = packet->rndis_msg;
 	rndis_msg->ndis_msg_type = RNDIS_MSG_PACKET;
 	rndis_msg->msg_len = packet->total_data_buflen;
 	rndis_pkt = &rndis_msg->msg.pkt;
@@ -521,8 +447,8 @@ check_size:
 					IEEE_8021Q_INFO);
 		vlan = (struct ndis_pkt_8021q_info *)((void *)ppi +
 						ppi->ppi_offset);
-		vlan->vlanid = packet->vlan_tci & VLAN_VID_MASK;
-		vlan->pri = (packet->vlan_tci & VLAN_PRIO_MASK) >>
+		vlan->vlanid = skb->vlan_tci & VLAN_VID_MASK;
+		vlan->pri = (skb->vlan_tci & VLAN_PRIO_MASK) >>
 				VLAN_PRIO_SHIFT;
 	}
 
@@ -617,9 +543,10 @@ do_send:
 	rndis_msg->msg_len += rndis_msg_size;
 	packet->total_data_buflen = rndis_msg->msg_len;
 	packet->page_buf_cnt = init_page_array(rndis_msg, rndis_msg_size,
-					       skb, packet);
+					       skb, packet, &pb);
 
-	ret = netvsc_send(net_device_ctx->device_ctx, packet);
+	ret = netvsc_send(net_device_ctx->device_ctx, packet,
+			  rndis_msg, &pb, skb);
 
 drop:
 	if (ret == 0) {
@@ -647,37 +574,33 @@ void netvsc_linkstatus_callback(struct hv_device *device_obj,
 	struct net_device *net;
 	struct net_device_context *ndev_ctx;
 	struct netvsc_device *net_device;
-	struct rndis_device *rdev;
+	struct netvsc_reconfig *event;
+	unsigned long flags;
 
-	net_device = hv_get_drvdata(device_obj);
-	rdev = net_device->extension;
-
-	switch (indicate->status) {
-	case RNDIS_STATUS_MEDIA_CONNECT:
-		rdev->link_state = false;
-		break;
-	case RNDIS_STATUS_MEDIA_DISCONNECT:
-		rdev->link_state = true;
-		break;
-	case RNDIS_STATUS_NETWORK_CHANGE:
-		rdev->link_change = true;
-		break;
-	default:
+	/* Handle link change statuses only */
+	if (indicate->status != RNDIS_STATUS_NETWORK_CHANGE &&
+	    indicate->status != RNDIS_STATUS_MEDIA_CONNECT &&
+	    indicate->status != RNDIS_STATUS_MEDIA_DISCONNECT)
 		return;
-	}
 
+	net_device = hv_get_drvdata(device_obj);
 	net = net_device->ndev;
 
 	if (!net || net->reg_state != NETREG_REGISTERED)
 		return;
 
 	ndev_ctx = netdev_priv(net);
-	if (!rdev->link_state) {
-		schedule_delayed_work(&ndev_ctx->dwork, 0);
-		schedule_delayed_work(&ndev_ctx->dwork, msecs_to_jiffies(20));
-	} else {
-		schedule_delayed_work(&ndev_ctx->dwork, 0);
-	}
+
+	event = kzalloc(sizeof(*event), GFP_ATOMIC);
+	if (!event)
+		return;
+	event->event = indicate->status;
+
+	spin_lock_irqsave(&ndev_ctx->lock, flags);
+	list_add_tail(&event->list, &ndev_ctx->reconfig_events);
+	spin_unlock_irqrestore(&ndev_ctx->lock, flags);
+
+	schedule_delayed_work(&ndev_ctx->dwork, 0);
 }
 
 /*
@@ -686,7 +609,10 @@ void netvsc_linkstatus_callback(struct hv_device *device_obj,
  */
 int netvsc_recv_callback(struct hv_device *device_obj,
 				struct hv_netvsc_packet *packet,
-				struct ndis_tcp_ip_checksum_info *csum_info)
+				void **data,
+				struct ndis_tcp_ip_checksum_info *csum_info,
+				struct vmbus_channel *channel,
+				u16 vlan_tci)
 {
 	struct net_device *net;
 	struct net_device_context *net_device_ctx;
@@ -695,8 +621,7 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 
 	net = ((struct netvsc_device *)hv_get_drvdata(device_obj))->ndev;
 	if (!net || net->reg_state != NETREG_REGISTERED) {
-		packet->status = NVSP_STAT_FAIL;
-		return 0;
+		return NVSP_STAT_FAIL;
 	}
 	net_device_ctx = netdev_priv(net);
 	rx_stats = this_cpu_ptr(net_device_ctx->rx_stats);
@@ -705,15 +630,14 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 	skb = netdev_alloc_skb_ip_align(net, packet->total_data_buflen);
 	if (unlikely(!skb)) {
 		++net->stats.rx_dropped;
-		packet->status = NVSP_STAT_FAIL;
-		return 0;
+		return NVSP_STAT_FAIL;
 	}
 
 	/*
 	 * Copy to skb. This copy is needed here since the memory pointed by
 	 * hv_netvsc_packet cannot be deallocated
 	 */
-	memcpy(skb_put(skb, packet->total_data_buflen), packet->data,
+	memcpy(skb_put(skb, packet->total_data_buflen), *data,
 		packet->total_data_buflen);
 
 	skb->protocol = eth_type_trans(skb, net);
@@ -728,11 +652,11 @@ int netvsc_recv_callback(struct hv_device *device_obj,
 			skb->ip_summed = CHECKSUM_NONE;
 	}
 
-	if (packet->vlan_tci & VLAN_TAG_PRESENT)
+	if (vlan_tci & VLAN_TAG_PRESENT)
 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
-				       packet->vlan_tci);
+				       vlan_tci);
 
-	skb_record_rx_queue(skb, packet->channel->
+	skb_record_rx_queue(skb, channel->
 			    offermsg.offer.sub_channel_index);
 
 	u64_stats_update_begin(&rx_stats->syncp);
@@ -1009,12 +933,9 @@ static const struct net_device_ops device_ops = {
 };
 
 /*
- * Send GARP packet to network peers after migrations.
- * After Quick Migration, the network is not immediately operational in the
- * current context when receiving RNDIS_STATUS_MEDIA_CONNECT event. So, add
- * another netif_notify_peers() into a delayed work, otherwise GARP packet
- * will not be sent after quick migration, and cause network disconnection.
- * Also, we update the carrier status here.
+ * Handle link status changes. For RNDIS_STATUS_NETWORK_CHANGE emulate link
+ * down/up sequence. In case of RNDIS_STATUS_MEDIA_CONNECT when carrier is
+ * present send GARP packet to network peers with netif_notify_peers().
  */
 static void netvsc_link_change(struct work_struct *w)
 {
@@ -1022,36 +943,89 @@ static void netvsc_link_change(struct work_struct *w)
 	struct net_device *net;
 	struct netvsc_device *net_device;
 	struct rndis_device *rdev;
-	bool notify, refresh = false;
-	char *argv[] = { "/etc/init.d/network", "restart", NULL };
-	char *envp[] = { "HOME=/", "PATH=/sbin:/usr/sbin:/bin:/usr/bin", NULL };
-
-	rtnl_lock();
+	struct netvsc_reconfig *event = NULL;
+	bool notify = false, reschedule = false;
+	unsigned long flags, next_reconfig, delay;
 
 	ndev_ctx = container_of(w, struct net_device_context, dwork.work);
 	net_device = hv_get_drvdata(ndev_ctx->device_ctx);
 	rdev = net_device->extension;
 	net = net_device->ndev;
 
-	if (rdev->link_state) {
-		netif_carrier_off(net);
-		notify = false;
-	} else {
-		netif_carrier_on(net);
-		notify = true;
-		if (rdev->link_change) {
-			rdev->link_change = false;
-			refresh = true;
+	next_reconfig = ndev_ctx->last_reconfig + LINKCHANGE_INT;
+	if (time_is_after_jiffies(next_reconfig)) {
+		/* link_watch only sends one notification with current state
+		 * per second, avoid doing reconfig more frequently. Handle
+		 * wrap around.
+		 */
+		delay = next_reconfig - jiffies;
+		delay = delay < LINKCHANGE_INT ? delay : LINKCHANGE_INT;
+		schedule_delayed_work(&ndev_ctx->dwork, delay);
+		return;
+	}
+	ndev_ctx->last_reconfig = jiffies;
+
+	spin_lock_irqsave(&ndev_ctx->lock, flags);
+	if (!list_empty(&ndev_ctx->reconfig_events)) {
+		event = list_first_entry(&ndev_ctx->reconfig_events,
+					 struct netvsc_reconfig, list);
+		list_del(&event->list);
+		reschedule = !list_empty(&ndev_ctx->reconfig_events);
+	}
+	spin_unlock_irqrestore(&ndev_ctx->lock, flags);
+
+	if (!event)
+		return;
+
+	rtnl_lock();
+
+	switch (event->event) {
+		/* Only the following events are possible due to the check in
+		 * netvsc_linkstatus_callback()
+		 */
+	case RNDIS_STATUS_MEDIA_CONNECT:
+		if (rdev->link_state) {
+			rdev->link_state = false;
+			netif_carrier_on(net);
+			netif_tx_wake_all_queues(net);
+		} else {
+			notify = true;
+		}
+		kfree(event);
+		break;
+	case RNDIS_STATUS_MEDIA_DISCONNECT:
+		if (!rdev->link_state) {
+			rdev->link_state = true;
+			netif_carrier_off(net);
+			netif_tx_stop_all_queues(net);
+		}
+		kfree(event);
+		break;
+	case RNDIS_STATUS_NETWORK_CHANGE:
+		/* Only makes sense if carrier is present */
+		if (!rdev->link_state) {
+			rdev->link_state = true;
+			netif_carrier_off(net);
+			netif_tx_stop_all_queues(net);
+			event->event = RNDIS_STATUS_MEDIA_CONNECT;
+			spin_lock_irqsave(&ndev_ctx->lock, flags);
+			list_add_tail(&event->list, &ndev_ctx->reconfig_events);
+			spin_unlock_irqrestore(&ndev_ctx->lock, flags);
+			reschedule = true;
 		}
+		break;
 	}
 
 	rtnl_unlock();
 
-	if (refresh)
-		call_usermodehelper(argv[0], argv, envp, UMH_WAIT_EXEC);
-
 	if (notify)
 		netdev_notify_peers(net);
+
+	/* link_watch only sends one notification with current state per
+	 * second, handle next reconfig event in 2 seconds.
+	 */
+	if (reschedule)
+		schedule_delayed_work(&ndev_ctx->dwork, LINKCHANGE_INT);
 }
 
 static void netvsc_free_netdev(struct net_device *netdev)
@@ -1071,16 +1045,12 @@ static int netvsc_probe(struct hv_device *dev,
 	struct netvsc_device_info device_info;
 	struct netvsc_device *nvdev;
 	int ret;
-	u32 max_needed_headroom;
 
 	net = alloc_etherdev_mq(sizeof(struct net_device_context),
 				num_online_cpus());
 	if (!net)
 		return -ENOMEM;
 
-	max_needed_headroom = sizeof(struct hv_netvsc_packet) +
-			      RNDIS_AND_PPI_SIZE;
-
 	netif_carrier_off(net);
 
 	net_device_ctx = netdev_priv(net);
@@ -1106,6 +1076,9 @@ static int netvsc_probe(struct hv_device *dev,
 	INIT_DELAYED_WORK(&net_device_ctx->dwork, netvsc_link_change);
 	INIT_WORK(&net_device_ctx->work, do_set_multicast);
 
+	spin_lock_init(&net_device_ctx->lock);
+	INIT_LIST_HEAD(&net_device_ctx->reconfig_events);
+
 	net->netdev_ops = &device_ops;
 
 	net->hw_features = NETIF_F_RXCSUM | NETIF_F_SG | NETIF_F_IP_CSUM |
@@ -1116,12 +1089,8 @@ static int netvsc_probe(struct hv_device *dev,
 	net->ethtool_ops = &ethtool_ops;
 	SET_NETDEV_DEV(net, &dev->device);
 
-	/*
-	 * Request additional head room in the skb.
-	 * We will use this space to build the rndis
-	 * heaser and other state we need to maintain.
-	 */
-	net->needed_headroom = max_needed_headroom;
+	/* We always need headroom for rndis header */
+	net->needed_headroom = RNDIS_AND_PPI_SIZE;
 
 	/* Notify the netvsc driver of the new device */
 	memset(&device_info, 0, sizeof(device_info));
@@ -1145,8 +1114,6 @@ static int netvsc_probe(struct hv_device *dev,
 		pr_err("Unable to register netdev.\n");
 		rndis_filter_device_remove(dev);
 		netvsc_free_netdev(net);
-	} else {
-		schedule_delayed_work(&net_device_ctx->dwork, 0);
 	}
 
 	return ret;
diff --git a/drivers/net/hyperv/rndis_filter.c b/drivers/net/hyperv/rndis_filter.c
index 5931a799aa17..a37bbda37ffa 100644
--- a/drivers/net/hyperv/rndis_filter.c
+++ b/drivers/net/hyperv/rndis_filter.c
@@ -210,37 +210,33 @@ static int rndis_filter_send_request(struct rndis_device *dev,
 	int ret;
 	struct hv_netvsc_packet *packet;
 	struct hv_page_buffer page_buf[2];
+	struct hv_page_buffer *pb = page_buf;
 
 	/* Setup the packet to send it */
 	packet = &req->pkt;
 
-	packet->is_data_pkt = false;
 	packet->total_data_buflen = req->request_msg.msg_len;
 	packet->page_buf_cnt = 1;
-	packet->page_buf = page_buf;
 
-	packet->page_buf[0].pfn = virt_to_phys(&req->request_msg) >>
+	pb[0].pfn = virt_to_phys(&req->request_msg) >>
 					PAGE_SHIFT;
-	packet->page_buf[0].len = req->request_msg.msg_len;
-	packet->page_buf[0].offset =
+	pb[0].len = req->request_msg.msg_len;
+	pb[0].offset =
 		(unsigned long)&req->request_msg & (PAGE_SIZE - 1);
 
 	/* Add one page_buf when request_msg crossing page boundary */
-	if (packet->page_buf[0].offset + packet->page_buf[0].len > PAGE_SIZE) {
+	if (pb[0].offset + pb[0].len > PAGE_SIZE) {
 		packet->page_buf_cnt++;
-		packet->page_buf[0].len = PAGE_SIZE -
-			packet->page_buf[0].offset;
-		packet->page_buf[1].pfn = virt_to_phys((void *)&req->request_msg
-			+ packet->page_buf[0].len) >> PAGE_SHIFT;
-		packet->page_buf[1].offset = 0;
-		packet->page_buf[1].len = req->request_msg.msg_len -
-			packet->page_buf[0].len;
+		pb[0].len = PAGE_SIZE -
+			pb[0].offset;
+		pb[1].pfn = virt_to_phys((void *)&req->request_msg
+			+ pb[0].len) >> PAGE_SHIFT;
+		pb[1].offset = 0;
+		pb[1].len = req->request_msg.msg_len -
+			pb[0].len;
 	}
 
-	packet->send_completion = NULL;
-	packet->xmit_more = false;
-
-	ret = netvsc_send(dev->net_dev->dev, packet);
+	ret = netvsc_send(dev->net_dev->dev, packet, NULL, &pb, NULL);
 	return ret;
 }
 
@@ -348,14 +344,17 @@ static inline void *rndis_get_ppi(struct rndis_packet *rpkt, u32 type)
 	return NULL;
 }
 
-static void rndis_filter_receive_data(struct rndis_device *dev,
+static int rndis_filter_receive_data(struct rndis_device *dev,
 				   struct rndis_message *msg,
-				   struct hv_netvsc_packet *pkt)
+				   struct hv_netvsc_packet *pkt,
+				   void **data,
+				   struct vmbus_channel *channel)
 {
 	struct rndis_packet *rndis_pkt;
 	u32 data_offset;
 	struct ndis_pkt_8021q_info *vlan;
 	struct ndis_tcp_ip_checksum_info *csum_info;
+	u16 vlan_tci = 0;
 
 	rndis_pkt = &msg->msg.pkt;
 
@@ -373,7 +372,7 @@ static void rndis_filter_receive_data(struct rndis_device *dev,
 			   "overflow detected (got %u, min %u)"
 			   "...dropping this message!\n",
 			   pkt->total_data_buflen, rndis_pkt->data_len);
-		return;
+		return NVSP_STAT_FAIL;
 	}
 
 	/*
@@ -382,22 +381,23 @@ static void rndis_filter_receive_data(struct rndis_device *dev,
 	 * the data packet to the stack, without the rndis trailer padding
 	 */
 	pkt->total_data_buflen = rndis_pkt->data_len;
-	pkt->data = (void *)((unsigned long)pkt->data + data_offset);
+	*data = (void *)((unsigned long)(*data) + data_offset);
 
 	vlan = rndis_get_ppi(rndis_pkt, IEEE_8021Q_INFO);
 	if (vlan) {
-		pkt->vlan_tci = VLAN_TAG_PRESENT | vlan->vlanid |
+		vlan_tci = VLAN_TAG_PRESENT | vlan->vlanid |
 			(vlan->pri << VLAN_PRIO_SHIFT);
-	} else {
-		pkt->vlan_tci = 0;
 	}
 
 	csum_info = rndis_get_ppi(rndis_pkt, TCPIP_CHKSUM_PKTINFO);
-	netvsc_recv_callback(dev->net_dev->dev, pkt, csum_info);
+	return netvsc_recv_callback(dev->net_dev->dev, pkt, data,
+				    csum_info, channel, vlan_tci);
 }
 
 int rndis_filter_receive(struct hv_device *dev,
-				struct hv_netvsc_packet	*pkt)
+				struct hv_netvsc_packet	*pkt,
+				void **data,
+				struct vmbus_channel *channel)
 {
 	struct netvsc_device *net_dev = hv_get_drvdata(dev);
 	struct rndis_device *rndis_dev;
@@ -406,7 +406,7 @@ int rndis_filter_receive(struct hv_device *dev,
 	int ret = 0;
 
 	if (!net_dev) {
-		ret = -EINVAL;
+		ret = NVSP_STAT_FAIL;
 		goto exit;
 	}
 
@@ -416,7 +416,7 @@ int rndis_filter_receive(struct hv_device *dev,
 	if (!net_dev->extension) {
 		netdev_err(ndev, "got rndis message but no rndis device - "
 			  "dropping this message!\n");
-		ret = -ENODEV;
+		ret = NVSP_STAT_FAIL;
 		goto exit;
 	}
 
@@ -424,11 +424,11 @@ int rndis_filter_receive(struct hv_device *dev,
 	if (rndis_dev->state == RNDIS_DEV_UNINITIALIZED) {
 		netdev_err(ndev, "got rndis message but rndis device "
 			   "uninitialized...dropping this message!\n");
-		ret = -ENODEV;
+		ret = NVSP_STAT_FAIL;
 		goto exit;
 	}
 
-	rndis_msg = pkt->data;
+	rndis_msg = *data;
 
 	if (netif_msg_rx_err(net_dev->nd_ctx))
 		dump_rndis_message(dev, rndis_msg);
@@ -436,7 +436,8 @@ int rndis_filter_receive(struct hv_device *dev,
 	switch (rndis_msg->ndis_msg_type) {
 	case RNDIS_MSG_PACKET:
 		/* data msg */
-		rndis_filter_receive_data(rndis_dev, rndis_msg, pkt);
+		ret = rndis_filter_receive_data(rndis_dev, rndis_msg, pkt,
+						data, channel);
 		break;
 
 	case RNDIS_MSG_INIT_C:
@@ -459,9 +460,6 @@ int rndis_filter_receive(struct hv_device *dev,
 	}
 
 exit:
-	if (ret != 0)
-		pkt->status = NVSP_STAT_FAIL;
-
 	return ret;
 }
 
diff --git a/drivers/net/ieee802154/Kconfig b/drivers/net/ieee802154/Kconfig
index ce5f1a21e6d7..3057a8df4ce9 100644
--- a/drivers/net/ieee802154/Kconfig
+++ b/drivers/net/ieee802154/Kconfig
@@ -71,3 +71,14 @@ config IEEE802154_ATUSB
 
 	  This driver can also be built as a module. To do so say M here.
 	  The module will be called 'atusb'.
+
+config IEEE802154_ADF7242
+       tristate "ADF7242 transceiver driver"
+       depends on IEEE802154_DRIVERS && MAC802154
+       depends on SPI
+	---help---
+	  Say Y here to enable the ADF7242 SPI 802.15.4 wireless
+	  controller.
+
+	  This driver can also be built as a module. To do so, say M here.
+	  the module will be called 'adf7242'.
diff --git a/drivers/net/ieee802154/Makefile b/drivers/net/ieee802154/Makefile
index cf1d2a6db023..3a923d339497 100644
--- a/drivers/net/ieee802154/Makefile
+++ b/drivers/net/ieee802154/Makefile
@@ -3,3 +3,4 @@ obj-$(CONFIG_IEEE802154_AT86RF230) += at86rf230.o
 obj-$(CONFIG_IEEE802154_MRF24J40) += mrf24j40.o
 obj-$(CONFIG_IEEE802154_CC2520) += cc2520.o
 obj-$(CONFIG_IEEE802154_ATUSB) += atusb.o
+obj-$(CONFIG_IEEE802154_ADF7242) += adf7242.o
diff --git a/drivers/net/ieee802154/adf7242.c b/drivers/net/ieee802154/adf7242.c
new file mode 100644
index 000000000000..89154c079788
--- /dev/null
+++ b/drivers/net/ieee802154/adf7242.c
@@ -0,0 +1,1285 @@
+/*
+ * Analog Devices ADF7242 Low-Power IEEE 802.15.4 Transceiver
+ *
+ * Copyright 2009-2015 Analog Devices Inc.
+ *
+ * Licensed under the GPL-2 or later.
+ *
+ * http://www.analog.com/ADF7242
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/spinlock.h>
+#include <linux/firmware.h>
+#include <linux/spi/spi.h>
+#include <linux/skbuff.h>
+#include <linux/of.h>
+#include <linux/irq.h>
+#include <linux/delay.h>
+#include <linux/debugfs.h>
+#include <linux/bitops.h>
+#include <linux/ieee802154.h>
+#include <net/mac802154.h>
+#include <net/cfg802154.h>
+
+#define FIRMWARE "adf7242_firmware.bin"
+#define MAX_POLL_LOOPS 200
+
+/* All Registers */
+
+#define REG_EXT_CTRL	0x100	/* RW External LNA/PA and internal PA control */
+#define REG_TX_FSK_TEST 0x101	/* RW TX FSK test mode configuration */
+#define REG_CCA1	0x105	/* RW RSSI threshold for CCA */
+#define REG_CCA2	0x106	/* RW CCA mode configuration */
+#define REG_BUFFERCFG	0x107	/* RW RX_BUFFER overwrite control */
+#define REG_PKT_CFG	0x108	/* RW FCS evaluation configuration */
+#define REG_DELAYCFG0	0x109	/* RW RC_RX command to SFD or sync word delay */
+#define REG_DELAYCFG1	0x10A	/* RW RC_TX command to TX state */
+#define REG_DELAYCFG2	0x10B	/* RW Mac delay extension */
+#define REG_SYNC_WORD0	0x10C	/* RW sync word bits [7:0] of [23:0]  */
+#define REG_SYNC_WORD1	0x10D	/* RW sync word bits [15:8] of [23:0]  */
+#define REG_SYNC_WORD2	0x10E	/* RW sync word bits [23:16] of [23:0]	*/
+#define REG_SYNC_CONFIG	0x10F	/* RW sync word configuration */
+#define REG_RC_CFG	0x13E	/* RW RX / TX packet configuration */
+#define REG_RC_VAR44	0x13F	/* RW RESERVED */
+#define REG_CH_FREQ0	0x300	/* RW Channel Frequency Settings - Low */
+#define REG_CH_FREQ1	0x301	/* RW Channel Frequency Settings - Middle */
+#define REG_CH_FREQ2	0x302	/* RW Channel Frequency Settings - High */
+#define REG_TX_FD	0x304	/* RW TX Frequency Deviation Register */
+#define REG_DM_CFG0	0x305	/* RW RX Discriminator BW Register */
+#define REG_TX_M	0x306	/* RW TX Mode Register */
+#define REG_RX_M	0x307	/* RW RX Mode Register */
+#define REG_RRB		0x30C	/* R RSSI Readback Register */
+#define REG_LRB		0x30D	/* R Link Quality Readback Register */
+#define REG_DR0		0x30E	/* RW bits [15:8] of [15:0] data rate setting */
+#define REG_DR1		0x30F	/* RW bits [7:0] of [15:0] data rate setting */
+#define REG_PRAMPG	0x313	/* RW RESERVED */
+#define REG_TXPB	0x314	/* RW TX Packet Storage Base Address */
+#define REG_RXPB	0x315	/* RW RX Packet Storage Base Address */
+#define REG_TMR_CFG0	0x316	/* RW Wake up Timer Conf Register - High */
+#define REG_TMR_CFG1	0x317	/* RW Wake up Timer Conf Register - Low */
+#define REG_TMR_RLD0	0x318	/* RW Wake up Timer Value Register - High */
+#define REG_TMR_RLD1	0x319	/* RW Wake up Timer Value Register - Low  */
+#define REG_TMR_CTRL	0x31A	/* RW Wake up Timer Timeout flag */
+#define REG_PD_AUX	0x31E	/* RW Battmon enable */
+#define REG_GP_CFG	0x32C	/* RW GPIO Configuration */
+#define REG_GP_OUT	0x32D	/* RW GPIO Configuration */
+#define REG_GP_IN	0x32E	/* R GPIO Configuration */
+#define REG_SYNT	0x335	/* RW bandwidth calibration timers */
+#define REG_CAL_CFG	0x33D	/* RW Calibration Settings */
+#define REG_PA_BIAS	0x36E	/* RW PA BIAS */
+#define REG_SYNT_CAL	0x371	/* RW Oscillator and Doubler Configuration */
+#define REG_IIRF_CFG	0x389	/* RW BB Filter Decimation Rate */
+#define REG_CDR_CFG	0x38A	/* RW CDR kVCO */
+#define REG_DM_CFG1	0x38B	/* RW Postdemodulator Filter */
+#define REG_AGCSTAT	0x38E	/* R RXBB Ref Osc Calibration Engine Readback */
+#define REG_RXCAL0	0x395	/* RW RX BB filter tuning, LSB */
+#define REG_RXCAL1	0x396	/* RW RX BB filter tuning, MSB */
+#define REG_RXFE_CFG	0x39B	/* RW RXBB Ref Osc & RXFE Calibration */
+#define REG_PA_RR	0x3A7	/* RW Set PA ramp rate */
+#define REG_PA_CFG	0x3A8	/* RW PA enable */
+#define REG_EXTPA_CFG	0x3A9	/* RW External PA BIAS DAC */
+#define REG_EXTPA_MSC	0x3AA	/* RW PA Bias Mode */
+#define REG_ADC_RBK	0x3AE	/* R Readback temp */
+#define REG_AGC_CFG1	0x3B2	/* RW GC Parameters */
+#define REG_AGC_MAX	0x3B4	/* RW Slew rate	 */
+#define REG_AGC_CFG2	0x3B6	/* RW RSSI Parameters */
+#define REG_AGC_CFG3	0x3B7	/* RW RSSI Parameters */
+#define REG_AGC_CFG4	0x3B8	/* RW RSSI Parameters */
+#define REG_AGC_CFG5	0x3B9	/* RW RSSI & NDEC Parameters */
+#define REG_AGC_CFG6	0x3BA	/* RW NDEC Parameters */
+#define REG_OCL_CFG1	0x3C4	/* RW OCL System Parameters */
+#define REG_IRQ1_EN0	0x3C7	/* RW Interrupt Mask set bits for IRQ1 */
+#define REG_IRQ1_EN1	0x3C8	/* RW Interrupt Mask set bits for IRQ1 */
+#define REG_IRQ2_EN0	0x3C9	/* RW Interrupt Mask set bits for IRQ2 */
+#define REG_IRQ2_EN1	0x3CA	/* RW Interrupt Mask set bits for IRQ2 */
+#define REG_IRQ1_SRC0	0x3CB	/* RW Interrupt Source bits for IRQ */
+#define REG_IRQ1_SRC1	0x3CC	/* RW Interrupt Source bits for IRQ */
+#define REG_OCL_BW0	0x3D2	/* RW OCL System Parameters */
+#define REG_OCL_BW1	0x3D3	/* RW OCL System Parameters */
+#define REG_OCL_BW2	0x3D4	/* RW OCL System Parameters */
+#define REG_OCL_BW3	0x3D5	/* RW OCL System Parameters */
+#define REG_OCL_BW4	0x3D6	/* RW OCL System Parameters */
+#define REG_OCL_BWS	0x3D7	/* RW OCL System Parameters */
+#define REG_OCL_CFG13	0x3E0	/* RW OCL System Parameters */
+#define REG_GP_DRV	0x3E3	/* RW I/O pads Configuration and bg trim */
+#define REG_BM_CFG	0x3E6	/* RW Batt. Monitor Threshold Voltage setting */
+#define REG_SFD_15_4	0x3F4	/* RW Option to set non standard SFD */
+#define REG_AFC_CFG	0x3F7	/* RW AFC mode and polarity */
+#define REG_AFC_KI_KP	0x3F8	/* RW AFC ki and kp */
+#define REG_AFC_RANGE	0x3F9	/* RW AFC range */
+#define REG_AFC_READ	0x3FA	/* RW Readback frequency error */
+
+/* REG_EXTPA_MSC */
+#define PA_PWR(x)		(((x) & 0xF) << 4)
+#define EXTPA_BIAS_SRC		BIT(3)
+#define EXTPA_BIAS_MODE(x)	(((x) & 0x7) << 0)
+
+/* REG_PA_CFG */
+#define PA_BRIDGE_DBIAS(x)	(((x) & 0x1F) << 0)
+#define PA_DBIAS_HIGH_POWER	21
+#define PA_DBIAS_LOW_POWER	13
+
+/* REG_PA_BIAS */
+#define PA_BIAS_CTRL(x)		(((x) & 0x1F) << 1)
+#define REG_PA_BIAS_DFL		BIT(0)
+#define PA_BIAS_HIGH_POWER	63
+#define PA_BIAS_LOW_POWER	55
+
+#define REG_PAN_ID0		0x112
+#define REG_PAN_ID1		0x113
+#define REG_SHORT_ADDR_0	0x114
+#define REG_SHORT_ADDR_1	0x115
+#define REG_IEEE_ADDR_0		0x116
+#define REG_IEEE_ADDR_1		0x117
+#define REG_IEEE_ADDR_2		0x118
+#define REG_IEEE_ADDR_3		0x119
+#define REG_IEEE_ADDR_4		0x11A
+#define REG_IEEE_ADDR_5		0x11B
+#define REG_IEEE_ADDR_6		0x11C
+#define REG_IEEE_ADDR_7		0x11D
+#define REG_FFILT_CFG		0x11E
+#define REG_AUTO_CFG		0x11F
+#define REG_AUTO_TX1		0x120
+#define REG_AUTO_TX2		0x121
+#define REG_AUTO_STATUS		0x122
+
+/* REG_FFILT_CFG */
+#define ACCEPT_BEACON_FRAMES   BIT(0)
+#define ACCEPT_DATA_FRAMES     BIT(1)
+#define ACCEPT_ACK_FRAMES      BIT(2)
+#define ACCEPT_MACCMD_FRAMES   BIT(3)
+#define ACCEPT_RESERVED_FRAMES BIT(4)
+#define ACCEPT_ALL_ADDRESS     BIT(5)
+
+/* REG_AUTO_CFG */
+#define AUTO_ACK_FRAMEPEND     BIT(0)
+#define IS_PANCOORD	       BIT(1)
+#define RX_AUTO_ACK_EN	       BIT(3)
+#define CSMA_CA_RX_TURNAROUND  BIT(4)
+
+/* REG_AUTO_TX1 */
+#define MAX_FRAME_RETRIES(x)   ((x) & 0xF)
+#define MAX_CCA_RETRIES(x)     (((x) & 0x7) << 4)
+
+/* REG_AUTO_TX2 */
+#define CSMA_MAX_BE(x)	       ((x) & 0xF)
+#define CSMA_MIN_BE(x)	       (((x) & 0xF) << 4)
+
+#define CMD_SPI_NOP		0xFF /* No operation. Use for dummy writes */
+#define CMD_SPI_PKT_WR		0x10 /* Write telegram to the Packet RAM
+				      * starting from the TX packet base address
+				      * pointer tx_packet_base
+				      */
+#define CMD_SPI_PKT_RD		0x30 /* Read telegram from the Packet RAM
+				      * starting from RX packet base address
+				      * pointer rxpb.rx_packet_base
+				      */
+#define CMD_SPI_MEM_WR(x)	(0x18 + (x >> 8)) /* Write data to MCR or
+						   * Packet RAM sequentially
+						   */
+#define CMD_SPI_MEM_RD(x)	(0x38 + (x >> 8)) /* Read data from MCR or
+						   * Packet RAM sequentially
+						   */
+#define CMD_SPI_MEMR_WR(x)	(0x08 + (x >> 8)) /* Write data to MCR or Packet
+						   * RAM as random block
+						   */
+#define CMD_SPI_MEMR_RD(x)	(0x28 + (x >> 8)) /* Read data from MCR or
+						   * Packet RAM random block
+						   */
+#define CMD_SPI_PRAM_WR		0x1E /* Write data sequentially to current
+				      * PRAM page selected
+				      */
+#define CMD_SPI_PRAM_RD		0x3E /* Read data sequentially from current
+				      * PRAM page selected
+				      */
+#define CMD_RC_SLEEP		0xB1 /* Invoke transition of radio controller
+				      * into SLEEP state
+				      */
+#define CMD_RC_IDLE		0xB2 /* Invoke transition of radio controller
+				      * into IDLE state
+				      */
+#define CMD_RC_PHY_RDY		0xB3 /* Invoke transition of radio controller
+				      * into PHY_RDY state
+				      */
+#define CMD_RC_RX		0xB4 /* Invoke transition of radio controller
+				      * into RX state
+				      */
+#define CMD_RC_TX		0xB5 /* Invoke transition of radio controller
+				      * into TX state
+				      */
+#define CMD_RC_MEAS		0xB6 /* Invoke transition of radio controller
+				      * into MEAS state
+				      */
+#define CMD_RC_CCA		0xB7 /* Invoke Clear channel assessment */
+#define CMD_RC_CSMACA		0xC1 /* initiates CSMA-CA channel access
+				      * sequence and frame transmission
+				      */
+#define CMD_RC_PC_RESET		0xC7 /* Program counter reset */
+#define CMD_RC_RESET		0xC8 /* Resets the ADF7242 and puts it in
+				      * the sleep state
+				      */
+#define CMD_RC_PC_RESET_NO_WAIT (CMD_RC_PC_RESET | BIT(31))
+
+/* STATUS */
+
+#define STAT_SPI_READY		BIT(7)
+#define STAT_IRQ_STATUS		BIT(6)
+#define STAT_RC_READY		BIT(5)
+#define STAT_CCA_RESULT		BIT(4)
+#define RC_STATUS_IDLE		1
+#define RC_STATUS_MEAS		2
+#define RC_STATUS_PHY_RDY	3
+#define RC_STATUS_RX		4
+#define RC_STATUS_TX		5
+#define RC_STATUS_MASK		0xF
+
+/* AUTO_STATUS */
+
+#define SUCCESS			0
+#define SUCCESS_DATPEND		1
+#define FAILURE_CSMACA		2
+#define FAILURE_NOACK		3
+#define AUTO_STATUS_MASK	0x3
+
+#define PRAM_PAGESIZE		256
+
+/* IRQ1 */
+
+#define IRQ_CCA_COMPLETE	BIT(0)
+#define IRQ_SFD_RX		BIT(1)
+#define IRQ_SFD_TX		BIT(2)
+#define IRQ_RX_PKT_RCVD		BIT(3)
+#define IRQ_TX_PKT_SENT		BIT(4)
+#define IRQ_FRAME_VALID		BIT(5)
+#define IRQ_ADDRESS_VALID	BIT(6)
+#define IRQ_CSMA_CA		BIT(7)
+
+#define AUTO_TX_TURNAROUND	BIT(3)
+#define ADDON_EN		BIT(4)
+
+#define FLAG_XMIT		0
+#define FLAG_START		1
+
+#define ADF7242_REPORT_CSMA_CA_STAT 0 /* framework doesn't handle yet */
+
+struct adf7242_local {
+	struct spi_device *spi;
+	struct completion tx_complete;
+	struct ieee802154_hw *hw;
+	struct mutex bmux; /* protect SPI messages */
+	struct spi_message stat_msg;
+	struct spi_transfer stat_xfer;
+	struct dentry *debugfs_root;
+	unsigned long flags;
+	int tx_stat;
+	bool promiscuous;
+	s8 rssi;
+	u8 max_frame_retries;
+	u8 max_cca_retries;
+	u8 max_be;
+	u8 min_be;
+
+	/* DMA (thus cache coherency maintenance) requires the
+	 * transfer buffers to live in their own cache lines.
+	 */
+
+	u8 buf[3] ____cacheline_aligned;
+	u8 buf_reg_tx[3];
+	u8 buf_read_tx[4];
+	u8 buf_read_rx[4];
+	u8 buf_stat_rx;
+	u8 buf_stat_tx;
+	u8 buf_cmd;
+};
+
+static int adf7242_soft_reset(struct adf7242_local *lp, int line);
+
+static int adf7242_status(struct adf7242_local *lp, u8 *stat)
+{
+	int status;
+
+	mutex_lock(&lp->bmux);
+	status = spi_sync(lp->spi, &lp->stat_msg);
+	*stat = lp->buf_stat_rx;
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int adf7242_wait_status(struct adf7242_local *lp, unsigned status,
+			       unsigned mask, int line)
+{
+	int cnt = 0, ret = 0;
+	u8 stat;
+
+	do {
+		adf7242_status(lp, &stat);
+		cnt++;
+	} while (((stat & mask) != status) && (cnt < MAX_POLL_LOOPS));
+
+	if (cnt >= MAX_POLL_LOOPS) {
+		ret = -ETIMEDOUT;
+
+		if (!(stat & STAT_RC_READY)) {
+			adf7242_soft_reset(lp, line);
+			adf7242_status(lp, &stat);
+
+			if ((stat & mask) == status)
+				ret = 0;
+		}
+
+		if (ret < 0)
+			dev_warn(&lp->spi->dev,
+				 "%s:line %d Timeout status 0x%x (%d)\n",
+				 __func__, line, stat, cnt);
+	}
+
+	dev_vdbg(&lp->spi->dev, "%s : loops=%d line %d\n", __func__, cnt, line);
+
+	return ret;
+}
+
+static int adf7242_wait_ready(struct adf7242_local *lp, int line)
+{
+	return adf7242_wait_status(lp, STAT_RC_READY | STAT_SPI_READY,
+				   STAT_RC_READY | STAT_SPI_READY, line);
+}
+
+static int adf7242_write_fbuf(struct adf7242_local *lp, u8 *data, u8 len)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer_head = {
+		.len = 2,
+		.tx_buf = buf,
+
+	};
+	struct spi_transfer xfer_buf = {
+		.len = len,
+		.tx_buf = data,
+	};
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head, &msg);
+	spi_message_add_tail(&xfer_buf, &msg);
+
+	adf7242_wait_ready(lp, __LINE__);
+
+	mutex_lock(&lp->bmux);
+	buf[0] = CMD_SPI_PKT_WR;
+	buf[1] = len + 2;
+
+	status = spi_sync(lp->spi, &msg);
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int adf7242_read_fbuf(struct adf7242_local *lp,
+			     u8 *data, size_t len, bool packet_read)
+{
+	u8 *buf = lp->buf;
+	int status;
+	struct spi_message msg;
+	struct spi_transfer xfer_head = {
+		.len = 3,
+		.tx_buf = buf,
+		.rx_buf = buf,
+	};
+	struct spi_transfer xfer_buf = {
+		.len = len,
+		.rx_buf = data,
+	};
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head, &msg);
+	spi_message_add_tail(&xfer_buf, &msg);
+
+	adf7242_wait_ready(lp, __LINE__);
+
+	mutex_lock(&lp->bmux);
+	if (packet_read) {
+		buf[0] = CMD_SPI_PKT_RD;
+		buf[1] = CMD_SPI_NOP;
+		buf[2] = 0;	/* PHR */
+	} else {
+		buf[0] = CMD_SPI_PRAM_RD;
+		buf[1] = 0;
+		buf[2] = CMD_SPI_NOP;
+	}
+
+	status = spi_sync(lp->spi, &msg);
+
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int adf7242_read_reg(struct adf7242_local *lp, u16 addr, u8 *data)
+{
+	int status;
+	struct spi_message msg;
+
+	struct spi_transfer xfer = {
+		.len = 4,
+		.tx_buf = lp->buf_read_tx,
+		.rx_buf = lp->buf_read_rx,
+	};
+
+	adf7242_wait_ready(lp, __LINE__);
+
+	mutex_lock(&lp->bmux);
+	lp->buf_read_tx[0] = CMD_SPI_MEM_RD(addr);
+	lp->buf_read_tx[1] = addr;
+	lp->buf_read_tx[2] = CMD_SPI_NOP;
+	lp->buf_read_tx[3] = CMD_SPI_NOP;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer, &msg);
+
+	status = spi_sync(lp->spi, &msg);
+	if (msg.status)
+		status = msg.status;
+
+	if (!status)
+		*data = lp->buf_read_rx[3];
+
+	mutex_unlock(&lp->bmux);
+
+	dev_vdbg(&lp->spi->dev, "%s : REG 0x%X, VAL 0x%X\n", __func__,
+		 addr, *data);
+
+	return status;
+}
+
+static int adf7242_write_reg(struct adf7242_local *lp, u16 addr, u8 data)
+{
+	int status;
+
+	adf7242_wait_ready(lp, __LINE__);
+
+	mutex_lock(&lp->bmux);
+	lp->buf_reg_tx[0] = CMD_SPI_MEM_WR(addr);
+	lp->buf_reg_tx[1] = addr;
+	lp->buf_reg_tx[2] = data;
+	status = spi_write(lp->spi, lp->buf_reg_tx, 3);
+	mutex_unlock(&lp->bmux);
+
+	dev_vdbg(&lp->spi->dev, "%s : REG 0x%X, VAL 0x%X\n",
+		 __func__, addr, data);
+
+	return status;
+}
+
+static int adf7242_cmd(struct adf7242_local *lp, unsigned cmd)
+{
+	int status;
+
+	dev_vdbg(&lp->spi->dev, "%s : CMD=0x%X\n", __func__, cmd);
+
+	if (cmd != CMD_RC_PC_RESET_NO_WAIT)
+		adf7242_wait_ready(lp, __LINE__);
+
+	mutex_lock(&lp->bmux);
+	lp->buf_cmd = cmd;
+	status = spi_write(lp->spi, &lp->buf_cmd, 1);
+	mutex_unlock(&lp->bmux);
+
+	return status;
+}
+
+static int adf7242_upload_firmware(struct adf7242_local *lp, u8 *data, u16 len)
+{
+	struct spi_message msg;
+	struct spi_transfer xfer_buf = { };
+	int status, i, page = 0;
+	u8 *buf = lp->buf;
+
+	struct spi_transfer xfer_head = {
+		.len = 2,
+		.tx_buf = buf,
+	};
+
+	buf[0] = CMD_SPI_PRAM_WR;
+	buf[1] = 0;
+
+	spi_message_init(&msg);
+	spi_message_add_tail(&xfer_head, &msg);
+	spi_message_add_tail(&xfer_buf, &msg);
+
+	for (i = len; i >= 0; i -= PRAM_PAGESIZE) {
+		adf7242_write_reg(lp, REG_PRAMPG, page);
+
+		xfer_buf.len = (i >= PRAM_PAGESIZE) ? PRAM_PAGESIZE : i;
+		xfer_buf.tx_buf = &data[page * PRAM_PAGESIZE];
+
+		mutex_lock(&lp->bmux);
+		status = spi_sync(lp->spi, &msg);
+		mutex_unlock(&lp->bmux);
+		page++;
+	}
+
+	return status;
+}
+
+static int adf7242_verify_firmware(struct adf7242_local *lp,
+				   const u8 *data, size_t len)
+{
+#ifdef DEBUG
+	int i, j;
+	unsigned int page;
+	u8 *buf = kmalloc(PRAM_PAGESIZE, GFP_KERNEL);
+
+	if (!buf)
+		return -ENOMEM;
+
+	for (page = 0, i = len; i >= 0; i -= PRAM_PAGESIZE, page++) {
+		size_t nb = (i >= PRAM_PAGESIZE) ? PRAM_PAGESIZE : i;
+
+		adf7242_write_reg(lp, REG_PRAMPG, page);
+		adf7242_read_fbuf(lp, buf, nb, false);
+
+		for (j = 0; j < nb; j++) {
+			if (buf[j] != data[page * PRAM_PAGESIZE + j]) {
+				kfree(buf);
+				return -EIO;
+			}
+		}
+	}
+	kfree(buf);
+#endif
+	return 0;
+}
+
+static int adf7242_set_txpower(struct ieee802154_hw *hw, int mbm)
+{
+	struct adf7242_local *lp = hw->priv;
+	u8 pwr, bias_ctrl, dbias, tmp;
+	int db = mbm / 100;
+
+	dev_vdbg(&lp->spi->dev, "%s : Power %d dB\n", __func__, db);
+
+	if (db > 5 || db < -26)
+		return -EINVAL;
+
+	db = DIV_ROUND_CLOSEST(db + 29, 2);
+
+	if (db > 15) {
+		dbias = PA_DBIAS_HIGH_POWER;
+		bias_ctrl = PA_BIAS_HIGH_POWER;
+	} else {
+		dbias = PA_DBIAS_LOW_POWER;
+		bias_ctrl = PA_BIAS_LOW_POWER;
+	}
+
+	pwr = clamp_t(u8, db, 3, 15);
+
+	adf7242_read_reg(lp, REG_PA_CFG, &tmp);
+	tmp &= ~PA_BRIDGE_DBIAS(~0);
+	tmp |= PA_BRIDGE_DBIAS(dbias);
+	adf7242_write_reg(lp, REG_PA_CFG, tmp);
+
+	adf7242_read_reg(lp, REG_PA_BIAS, &tmp);
+	tmp &= ~PA_BIAS_CTRL(~0);
+	tmp |= PA_BIAS_CTRL(bias_ctrl);
+	adf7242_write_reg(lp, REG_PA_BIAS, tmp);
+
+	adf7242_read_reg(lp, REG_EXTPA_MSC, &tmp);
+	tmp &= ~PA_PWR(~0);
+	tmp |= PA_PWR(pwr);
+
+	return adf7242_write_reg(lp, REG_EXTPA_MSC, tmp);
+}
+
+static int adf7242_set_csma_params(struct ieee802154_hw *hw, u8 min_be,
+				   u8 max_be, u8 retries)
+{
+	struct adf7242_local *lp = hw->priv;
+	int ret;
+
+	dev_vdbg(&lp->spi->dev, "%s : min_be=%d max_be=%d retries=%d\n",
+		 __func__, min_be, max_be, retries);
+
+	if (min_be > max_be || max_be > 8 || retries > 5)
+		return -EINVAL;
+
+	ret = adf7242_write_reg(lp, REG_AUTO_TX1,
+				MAX_FRAME_RETRIES(lp->max_frame_retries) |
+				MAX_CCA_RETRIES(retries));
+	if (ret)
+		return ret;
+
+	lp->max_cca_retries = retries;
+	lp->max_be = max_be;
+	lp->min_be = min_be;
+
+	return adf7242_write_reg(lp, REG_AUTO_TX2, CSMA_MAX_BE(max_be) |
+			CSMA_MIN_BE(min_be));
+}
+
+static int adf7242_set_frame_retries(struct ieee802154_hw *hw, s8 retries)
+{
+	struct adf7242_local *lp = hw->priv;
+	int ret = 0;
+
+	dev_vdbg(&lp->spi->dev, "%s : Retries = %d\n", __func__, retries);
+
+	if (retries < -1 || retries > 15)
+		return -EINVAL;
+
+	if (retries >= 0)
+		ret = adf7242_write_reg(lp, REG_AUTO_TX1,
+					MAX_FRAME_RETRIES(retries) |
+					MAX_CCA_RETRIES(lp->max_cca_retries));
+
+	lp->max_frame_retries = retries;
+
+	return ret;
+}
+
+static int adf7242_ed(struct ieee802154_hw *hw, u8 *level)
+{
+	struct adf7242_local *lp = hw->priv;
+
+	*level = lp->rssi;
+
+	dev_vdbg(&lp->spi->dev, "%s :Exit level=%d\n",
+		 __func__, *level);
+
+	return 0;
+}
+
+static int adf7242_start(struct ieee802154_hw *hw)
+{
+	struct adf7242_local *lp = hw->priv;
+
+	adf7242_cmd(lp, CMD_RC_PHY_RDY);
+	adf7242_write_reg(lp, REG_IRQ1_SRC1, 0xFF);
+	enable_irq(lp->spi->irq);
+	set_bit(FLAG_START, &lp->flags);
+
+	return adf7242_cmd(lp, CMD_RC_RX);
+}
+
+static void adf7242_stop(struct ieee802154_hw *hw)
+{
+	struct adf7242_local *lp = hw->priv;
+
+	adf7242_cmd(lp, CMD_RC_IDLE);
+	clear_bit(FLAG_START, &lp->flags);
+	disable_irq(lp->spi->irq);
+	adf7242_write_reg(lp, REG_IRQ1_SRC1, 0xFF);
+}
+
+static int adf7242_channel(struct ieee802154_hw *hw, u8 page, u8 channel)
+{
+	struct adf7242_local *lp = hw->priv;
+	unsigned long freq;
+
+	dev_dbg(&lp->spi->dev, "%s :Channel=%d\n", __func__, channel);
+
+	might_sleep();
+
+	WARN_ON(page != 0);
+	WARN_ON(channel < 11);
+	WARN_ON(channel > 26);
+
+	freq = (2405 + 5 * (channel - 11)) * 100;
+	adf7242_cmd(lp, CMD_RC_PHY_RDY);
+
+	adf7242_write_reg(lp, REG_CH_FREQ0, freq);
+	adf7242_write_reg(lp, REG_CH_FREQ1, freq >> 8);
+	adf7242_write_reg(lp, REG_CH_FREQ2, freq >> 16);
+
+	return adf7242_cmd(lp, CMD_RC_RX);
+}
+
+static int adf7242_set_hw_addr_filt(struct ieee802154_hw *hw,
+				    struct ieee802154_hw_addr_filt *filt,
+				    unsigned long changed)
+{
+	struct adf7242_local *lp = hw->priv;
+	u8 reg;
+
+	dev_dbg(&lp->spi->dev, "%s :Changed=0x%lX\n", __func__, changed);
+
+	might_sleep();
+
+	if (changed & IEEE802154_AFILT_IEEEADDR_CHANGED) {
+		u8 addr[8], i;
+
+		memcpy(addr, &filt->ieee_addr, 8);
+
+		for (i = 0; i < 8; i++)
+			adf7242_write_reg(lp, REG_IEEE_ADDR_0 + i, addr[i]);
+	}
+
+	if (changed & IEEE802154_AFILT_SADDR_CHANGED) {
+		u16 saddr = le16_to_cpu(filt->short_addr);
+
+		adf7242_write_reg(lp, REG_SHORT_ADDR_0, saddr);
+		adf7242_write_reg(lp, REG_SHORT_ADDR_1, saddr >> 8);
+	}
+
+	if (changed & IEEE802154_AFILT_PANID_CHANGED) {
+		u16 pan_id = le16_to_cpu(filt->pan_id);
+
+		adf7242_write_reg(lp, REG_PAN_ID0, pan_id);
+		adf7242_write_reg(lp, REG_PAN_ID1, pan_id >> 8);
+	}
+
+	if (changed & IEEE802154_AFILT_PANC_CHANGED) {
+		adf7242_read_reg(lp, REG_AUTO_CFG, &reg);
+		if (filt->pan_coord)
+			reg |= IS_PANCOORD;
+		else
+			reg &= ~IS_PANCOORD;
+		adf7242_write_reg(lp, REG_AUTO_CFG, reg);
+	}
+
+	return 0;
+}
+
+static int adf7242_set_promiscuous_mode(struct ieee802154_hw *hw, bool on)
+{
+	struct adf7242_local *lp = hw->priv;
+
+	dev_dbg(&lp->spi->dev, "%s : mode %d\n", __func__, on);
+
+	lp->promiscuous = on;
+
+	if (on) {
+		adf7242_write_reg(lp, REG_AUTO_CFG, 0);
+		return adf7242_write_reg(lp, REG_FFILT_CFG,
+				  ACCEPT_BEACON_FRAMES |
+				  ACCEPT_DATA_FRAMES |
+				  ACCEPT_MACCMD_FRAMES |
+				  ACCEPT_ALL_ADDRESS |
+				  ACCEPT_ACK_FRAMES |
+				  ACCEPT_RESERVED_FRAMES);
+	} else {
+		adf7242_write_reg(lp, REG_FFILT_CFG,
+				  ACCEPT_BEACON_FRAMES |
+				  ACCEPT_DATA_FRAMES |
+				  ACCEPT_MACCMD_FRAMES |
+				  ACCEPT_RESERVED_FRAMES);
+
+		return adf7242_write_reg(lp, REG_AUTO_CFG, RX_AUTO_ACK_EN);
+	}
+}
+
+static int adf7242_set_cca_ed_level(struct ieee802154_hw *hw, s32 mbm)
+{
+	struct adf7242_local *lp = hw->priv;
+	s8 level = clamp_t(s8, mbm / 100, S8_MIN, S8_MAX);
+
+	dev_dbg(&lp->spi->dev, "%s : level %d\n", __func__, level);
+
+	return adf7242_write_reg(lp, REG_CCA1, level);
+}
+
+static int adf7242_xmit(struct ieee802154_hw *hw, struct sk_buff *skb)
+{
+	struct adf7242_local *lp = hw->priv;
+	int ret;
+
+	set_bit(FLAG_XMIT, &lp->flags);
+	reinit_completion(&lp->tx_complete);
+	adf7242_cmd(lp, CMD_RC_PHY_RDY);
+
+	ret = adf7242_write_fbuf(lp, skb->data, skb->len);
+	if (ret)
+		goto err;
+
+	ret = adf7242_cmd(lp, CMD_RC_CSMACA);
+	if (ret)
+		goto err;
+
+	ret = wait_for_completion_interruptible_timeout(&lp->tx_complete,
+							HZ / 10);
+	if (ret < 0)
+		goto err;
+	if (ret == 0) {
+		dev_dbg(&lp->spi->dev, "Timeout waiting for TX interrupt\n");
+		ret = -ETIMEDOUT;
+		goto err;
+	}
+
+	if (lp->tx_stat != SUCCESS) {
+		dev_dbg(&lp->spi->dev,
+			"Error xmit: Retry count exceeded Status=0x%x\n",
+			lp->tx_stat);
+		ret = -ECOMM;
+	} else {
+		ret = 0;
+	}
+
+err:
+	clear_bit(FLAG_XMIT, &lp->flags);
+	adf7242_cmd(lp, CMD_RC_RX);
+
+	return ret;
+}
+
+static int adf7242_rx(struct adf7242_local *lp)
+{
+	struct sk_buff *skb;
+	size_t len;
+	int ret;
+	u8 lqi, len_u8, *data;
+
+	adf7242_read_reg(lp, 0, &len_u8);
+
+	len = len_u8;
+
+	if (!ieee802154_is_valid_psdu_len(len)) {
+		dev_dbg(&lp->spi->dev,
+			"corrupted frame received len %d\n", (int)len);
+		len = IEEE802154_MTU;
+	}
+
+	skb = dev_alloc_skb(len);
+	if (!skb) {
+		adf7242_cmd(lp, CMD_RC_RX);
+		return -ENOMEM;
+	}
+
+	data = skb_put(skb, len);
+	ret = adf7242_read_fbuf(lp, data, len, true);
+	if (ret < 0) {
+		kfree_skb(skb);
+		adf7242_cmd(lp, CMD_RC_RX);
+		return ret;
+	}
+
+	lqi = data[len - 2];
+	lp->rssi = data[len - 1];
+
+	adf7242_cmd(lp, CMD_RC_RX);
+
+	skb_trim(skb, len - 2);	/* Don't put RSSI/LQI or CRC into the frame */
+
+	ieee802154_rx_irqsafe(lp->hw, skb, lqi);
+
+	dev_dbg(&lp->spi->dev, "%s: ret=%d len=%d lqi=%d rssi=%d\n",
+		__func__, ret, (int)len, (int)lqi, lp->rssi);
+
+	return 0;
+}
+
+static struct ieee802154_ops adf7242_ops = {
+	.owner = THIS_MODULE,
+	.xmit_sync = adf7242_xmit,
+	.ed = adf7242_ed,
+	.set_channel = adf7242_channel,
+	.set_hw_addr_filt = adf7242_set_hw_addr_filt,
+	.start = adf7242_start,
+	.stop = adf7242_stop,
+	.set_csma_params = adf7242_set_csma_params,
+	.set_frame_retries = adf7242_set_frame_retries,
+	.set_txpower = adf7242_set_txpower,
+	.set_promiscuous_mode = adf7242_set_promiscuous_mode,
+	.set_cca_ed_level = adf7242_set_cca_ed_level,
+};
+
+static void adf7242_debug(u8 irq1)
+{
+#ifdef DEBUG
+	u8 stat;
+
+	adf7242_status(lp, &stat);
+
+	dev_dbg(&lp->spi->dev, "%s IRQ1 = %X:\n%s%s%s%s%s%s%s%s\n",
+		__func__, irq1,
+		irq1 & IRQ_CCA_COMPLETE ? "IRQ_CCA_COMPLETE\n" : "",
+		irq1 & IRQ_SFD_RX ? "IRQ_SFD_RX\n" : "",
+		irq1 & IRQ_SFD_TX ? "IRQ_SFD_TX\n" : "",
+		irq1 & IRQ_RX_PKT_RCVD ? "IRQ_RX_PKT_RCVD\n" : "",
+		irq1 & IRQ_TX_PKT_SENT ? "IRQ_TX_PKT_SENT\n" : "",
+		irq1 & IRQ_CSMA_CA ? "IRQ_CSMA_CA\n" : "",
+		irq1 & IRQ_FRAME_VALID ? "IRQ_FRAME_VALID\n" : "",
+		irq1 & IRQ_ADDRESS_VALID ? "IRQ_ADDRESS_VALID\n" : "");
+
+	dev_dbg(&lp->spi->dev, "%s STATUS = %X:\n%s\n%s%s%s%s%s\n",
+		__func__, stat,
+		stat & STAT_RC_READY ? "RC_READY" : "RC_BUSY",
+		(stat & 0xf) == RC_STATUS_IDLE ? "RC_STATUS_IDLE" : "",
+		(stat & 0xf) == RC_STATUS_MEAS ? "RC_STATUS_MEAS" : "",
+		(stat & 0xf) == RC_STATUS_PHY_RDY ? "RC_STATUS_PHY_RDY" : "",
+		(stat & 0xf) == RC_STATUS_RX ? "RC_STATUS_RX" : "",
+		(stat & 0xf) == RC_STATUS_TX ? "RC_STATUS_TX" : "");
+	}
+#endif
+}
+
+static irqreturn_t adf7242_isr(int irq, void *data)
+{
+	struct adf7242_local *lp = data;
+	unsigned xmit;
+	u8 irq1;
+
+	adf7242_wait_status(lp, RC_STATUS_PHY_RDY, RC_STATUS_MASK, __LINE__);
+
+	adf7242_read_reg(lp, REG_IRQ1_SRC1, &irq1);
+	adf7242_write_reg(lp, REG_IRQ1_SRC1, irq1);
+
+	if (!(irq1 & (IRQ_RX_PKT_RCVD | IRQ_CSMA_CA)))
+		dev_err(&lp->spi->dev, "%s :ERROR IRQ1 = 0x%X\n",
+			__func__, irq1);
+
+	adf7242_debug(irq1);
+
+	xmit = test_bit(FLAG_XMIT, &lp->flags);
+
+	if (xmit && (irq1 & IRQ_CSMA_CA)) {
+		if (ADF7242_REPORT_CSMA_CA_STAT) {
+			u8 astat;
+
+			adf7242_read_reg(lp, REG_AUTO_STATUS, &astat);
+			astat &= AUTO_STATUS_MASK;
+
+			dev_dbg(&lp->spi->dev, "AUTO_STATUS = %X:\n%s%s%s%s\n",
+				astat,
+				astat == SUCCESS ? "SUCCESS" : "",
+				astat ==
+				SUCCESS_DATPEND ? "SUCCESS_DATPEND" : "",
+				astat == FAILURE_CSMACA ? "FAILURE_CSMACA" : "",
+				astat == FAILURE_NOACK ? "FAILURE_NOACK" : "");
+
+			/* save CSMA-CA completion status */
+			lp->tx_stat = astat;
+		} else {
+			lp->tx_stat = SUCCESS;
+		}
+		complete(&lp->tx_complete);
+	} else if (!xmit && (irq1 & IRQ_RX_PKT_RCVD) &&
+		   (irq1 & IRQ_FRAME_VALID)) {
+		adf7242_rx(lp);
+	} else if (!xmit && test_bit(FLAG_START, &lp->flags)) {
+		/* Invalid packet received - drop it and restart */
+		dev_dbg(&lp->spi->dev, "%s:%d : ERROR IRQ1 = 0x%X\n",
+			__func__, __LINE__, irq1);
+		adf7242_cmd(lp, CMD_RC_PHY_RDY);
+		adf7242_write_reg(lp, REG_IRQ1_SRC1, 0xFF);
+		adf7242_cmd(lp, CMD_RC_RX);
+	} else {
+		/* This can only be xmit without IRQ, likely a RX packet.
+		 * we get an TX IRQ shortly - do nothing or let the xmit
+		 * timeout handle this
+		 */
+		dev_dbg(&lp->spi->dev, "%s:%d : ERROR IRQ1 = 0x%X, xmit %d\n",
+			__func__, __LINE__, irq1, xmit);
+		complete(&lp->tx_complete);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int adf7242_soft_reset(struct adf7242_local *lp, int line)
+{
+	dev_warn(&lp->spi->dev, "%s (line %d)\n", __func__, line);
+
+	if (test_bit(FLAG_START, &lp->flags))
+		disable_irq_nosync(lp->spi->irq);
+
+	adf7242_cmd(lp, CMD_RC_PC_RESET_NO_WAIT);
+	usleep_range(200, 250);
+	adf7242_write_reg(lp, REG_PKT_CFG, ADDON_EN | BIT(2));
+	adf7242_cmd(lp, CMD_RC_PHY_RDY);
+	adf7242_set_promiscuous_mode(lp->hw, lp->promiscuous);
+	adf7242_set_csma_params(lp->hw, lp->min_be, lp->max_be,
+				lp->max_cca_retries);
+	adf7242_write_reg(lp, REG_IRQ1_SRC1, 0xFF);
+
+	if (test_bit(FLAG_START, &lp->flags)) {
+		enable_irq(lp->spi->irq);
+		return adf7242_cmd(lp, CMD_RC_RX);
+	}
+
+	return 0;
+}
+
+static int adf7242_hw_init(struct adf7242_local *lp)
+{
+	int ret;
+	const struct firmware *fw;
+
+	adf7242_cmd(lp, CMD_RC_RESET);
+	adf7242_cmd(lp, CMD_RC_IDLE);
+
+	/* get ADF7242 addon firmware
+	 * build this driver as module
+	 * and place under /lib/firmware/adf7242_firmware.bin
+	 * or compile firmware into the kernel.
+	 */
+	ret = request_firmware(&fw, FIRMWARE, &lp->spi->dev);
+	if (ret) {
+		dev_err(&lp->spi->dev,
+			"request_firmware() failed with %d\n", ret);
+		return ret;
+	}
+
+	ret = adf7242_upload_firmware(lp, (u8 *)fw->data, fw->size);
+	if (ret) {
+		dev_err(&lp->spi->dev,
+			"upload firmware failed with %d\n", ret);
+		return ret;
+	}
+
+	ret = adf7242_verify_firmware(lp, (u8 *)fw->data, fw->size);
+	if (ret) {
+		dev_err(&lp->spi->dev,
+			"verify firmware failed with %d\n", ret);
+		return ret;
+	}
+
+	adf7242_cmd(lp, CMD_RC_PC_RESET);
+
+	release_firmware(fw);
+
+	adf7242_write_reg(lp, REG_FFILT_CFG,
+			  ACCEPT_BEACON_FRAMES |
+			  ACCEPT_DATA_FRAMES |
+			  ACCEPT_MACCMD_FRAMES |
+			  ACCEPT_RESERVED_FRAMES);
+
+	adf7242_write_reg(lp, REG_AUTO_CFG, RX_AUTO_ACK_EN);
+
+	adf7242_write_reg(lp, REG_PKT_CFG, ADDON_EN | BIT(2));
+
+	adf7242_write_reg(lp, REG_EXTPA_MSC, 0xF1);
+	adf7242_write_reg(lp, REG_RXFE_CFG, 0x1D);
+
+	adf7242_write_reg(lp, REG_IRQ1_EN0, 0);
+	adf7242_write_reg(lp, REG_IRQ1_EN1, IRQ_RX_PKT_RCVD | IRQ_CSMA_CA);
+
+	adf7242_write_reg(lp, REG_IRQ1_SRC1, 0xFF);
+	adf7242_write_reg(lp, REG_IRQ1_SRC0, 0xFF);
+
+	adf7242_cmd(lp, CMD_RC_IDLE);
+
+	return 0;
+}
+
+static int adf7242_stats_show(struct seq_file *file, void *offset)
+{
+	struct adf7242_local *lp = spi_get_drvdata(file->private);
+	u8 stat, irq1;
+
+	adf7242_status(lp, &stat);
+	adf7242_read_reg(lp, REG_IRQ1_SRC1, &irq1);
+
+	seq_printf(file, "IRQ1 = %X:\n%s%s%s%s%s%s%s%s\n", irq1,
+		   irq1 & IRQ_CCA_COMPLETE ? "IRQ_CCA_COMPLETE\n" : "",
+		   irq1 & IRQ_SFD_RX ? "IRQ_SFD_RX\n" : "",
+		   irq1 & IRQ_SFD_TX ? "IRQ_SFD_TX\n" : "",
+		   irq1 & IRQ_RX_PKT_RCVD ? "IRQ_RX_PKT_RCVD\n" : "",
+		   irq1 & IRQ_TX_PKT_SENT ? "IRQ_TX_PKT_SENT\n" : "",
+		   irq1 & IRQ_CSMA_CA ? "IRQ_CSMA_CA\n" : "",
+		   irq1 & IRQ_FRAME_VALID ? "IRQ_FRAME_VALID\n" : "",
+		   irq1 & IRQ_ADDRESS_VALID ? "IRQ_ADDRESS_VALID\n" : "");
+
+	seq_printf(file, "STATUS = %X:\n%s\n%s%s%s%s%s\n", stat,
+		   stat & STAT_RC_READY ? "RC_READY" : "RC_BUSY",
+		   (stat & 0xf) == RC_STATUS_IDLE ? "RC_STATUS_IDLE" : "",
+		   (stat & 0xf) == RC_STATUS_MEAS ? "RC_STATUS_MEAS" : "",
+		   (stat & 0xf) == RC_STATUS_PHY_RDY ? "RC_STATUS_PHY_RDY" : "",
+		   (stat & 0xf) == RC_STATUS_RX ? "RC_STATUS_RX" : "",
+		   (stat & 0xf) == RC_STATUS_TX ? "RC_STATUS_TX" : "");
+
+	seq_printf(file, "RSSI = %d\n", lp->rssi);
+
+	return 0;
+}
+
+static int adf7242_debugfs_init(struct adf7242_local *lp)
+{
+	char debugfs_dir_name[DNAME_INLINE_LEN + 1] = "adf7242-";
+	struct dentry *stats;
+
+	strncat(debugfs_dir_name, dev_name(&lp->spi->dev), DNAME_INLINE_LEN);
+
+	lp->debugfs_root = debugfs_create_dir(debugfs_dir_name, NULL);
+	if (IS_ERR_OR_NULL(lp->debugfs_root))
+		return PTR_ERR_OR_ZERO(lp->debugfs_root);
+
+	stats = debugfs_create_devm_seqfile(&lp->spi->dev, "status",
+					    lp->debugfs_root,
+					    adf7242_stats_show);
+	return PTR_ERR_OR_ZERO(stats);
+
+	return 0;
+}
+
+static const s32 adf7242_powers[] = {
+	500, 400, 300, 200, 100, 0, -100, -200, -300, -400, -500, -600, -700,
+	-800, -900, -1000, -1100, -1200, -1300, -1400, -1500, -1600, -1700,
+	-1800, -1900, -2000, -2100, -2200, -2300, -2400, -2500, -2600,
+};
+
+static const s32 adf7242_ed_levels[] = {
+	-9000, -8900, -8800, -8700, -8600, -8500, -8400, -8300, -8200, -8100,
+	-8000, -7900, -7800, -7700, -7600, -7500, -7400, -7300, -7200, -7100,
+	-7000, -6900, -6800, -6700, -6600, -6500, -6400, -6300, -6200, -6100,
+	-6000, -5900, -5800, -5700, -5600, -5500, -5400, -5300, -5200, -5100,
+	-5000, -4900, -4800, -4700, -4600, -4500, -4400, -4300, -4200, -4100,
+	-4000, -3900, -3800, -3700, -3600, -3500, -3400, -3200, -3100, -3000
+};
+
+static int adf7242_probe(struct spi_device *spi)
+{
+	struct ieee802154_hw *hw;
+	struct adf7242_local *lp;
+	int ret, irq_type;
+
+	if (!spi->irq) {
+		dev_err(&spi->dev, "no IRQ specified\n");
+		return -EINVAL;
+	}
+
+	hw = ieee802154_alloc_hw(sizeof(*lp), &adf7242_ops);
+	if (!hw)
+		return -ENOMEM;
+
+	lp = hw->priv;
+	lp->hw = hw;
+	lp->spi = spi;
+
+	hw->priv = lp;
+	hw->parent = &spi->dev;
+	hw->extra_tx_headroom = 0;
+
+	/* We support only 2.4 Ghz */
+	hw->phy->supported.channels[0] = 0x7FFF800;
+
+	hw->flags = IEEE802154_HW_OMIT_CKSUM |
+		    IEEE802154_HW_CSMA_PARAMS |
+		    IEEE802154_HW_FRAME_RETRIES | IEEE802154_HW_AFILT |
+		    IEEE802154_HW_PROMISCUOUS;
+
+	hw->phy->flags = WPAN_PHY_FLAG_TXPOWER |
+			 WPAN_PHY_FLAG_CCA_ED_LEVEL |
+			 WPAN_PHY_FLAG_CCA_MODE;
+
+	hw->phy->supported.cca_modes = BIT(NL802154_CCA_ENERGY);
+
+	hw->phy->supported.cca_ed_levels = adf7242_ed_levels;
+	hw->phy->supported.cca_ed_levels_size = ARRAY_SIZE(adf7242_ed_levels);
+
+	hw->phy->cca.mode = NL802154_CCA_ENERGY;
+
+	hw->phy->supported.tx_powers = adf7242_powers;
+	hw->phy->supported.tx_powers_size = ARRAY_SIZE(adf7242_powers);
+
+	hw->phy->supported.min_minbe = 0;
+	hw->phy->supported.max_minbe = 8;
+
+	hw->phy->supported.min_maxbe = 3;
+	hw->phy->supported.max_maxbe = 8;
+
+	hw->phy->supported.min_frame_retries = 0;
+	hw->phy->supported.max_frame_retries = 15;
+
+	hw->phy->supported.min_csma_backoffs = 0;
+	hw->phy->supported.max_csma_backoffs = 5;
+
+	ieee802154_random_extended_addr(&hw->phy->perm_extended_addr);
+
+	mutex_init(&lp->bmux);
+	init_completion(&lp->tx_complete);
+
+	/* Setup Status Message */
+	lp->stat_xfer.len = 1;
+	lp->stat_xfer.tx_buf = &lp->buf_stat_tx;
+	lp->stat_xfer.rx_buf = &lp->buf_stat_rx;
+	lp->buf_stat_tx = CMD_SPI_NOP;
+
+	spi_message_init(&lp->stat_msg);
+	spi_message_add_tail(&lp->stat_xfer, &lp->stat_msg);
+
+	spi_set_drvdata(spi, lp);
+
+	ret = adf7242_hw_init(lp);
+	if (ret)
+		goto err_hw_init;
+
+	irq_type = irq_get_trigger_type(spi->irq);
+	if (!irq_type)
+		irq_type = IRQF_TRIGGER_HIGH;
+
+	ret = devm_request_threaded_irq(&spi->dev, spi->irq, NULL, adf7242_isr,
+					irq_type | IRQF_ONESHOT,
+					dev_name(&spi->dev), lp);
+	if (ret)
+		goto err_hw_init;
+
+	disable_irq(spi->irq);
+
+	ret = ieee802154_register_hw(lp->hw);
+	if (ret)
+		goto err_hw_init;
+
+	dev_set_drvdata(&spi->dev, lp);
+
+	adf7242_debugfs_init(lp);
+
+	dev_info(&spi->dev, "mac802154 IRQ-%d registered\n", spi->irq);
+
+	return ret;
+
+err_hw_init:
+	mutex_destroy(&lp->bmux);
+	ieee802154_free_hw(lp->hw);
+
+	return ret;
+}
+
+static int adf7242_remove(struct spi_device *spi)
+{
+	struct adf7242_local *lp = spi_get_drvdata(spi);
+
+	if (!IS_ERR_OR_NULL(lp->debugfs_root))
+		debugfs_remove_recursive(lp->debugfs_root);
+
+	ieee802154_unregister_hw(lp->hw);
+	mutex_destroy(&lp->bmux);
+	ieee802154_free_hw(lp->hw);
+
+	return 0;
+}
+
+static const struct of_device_id adf7242_of_match[] = {
+	{ .compatible = "adi,adf7242", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, adf7242_of_match);
+
+static const struct spi_device_id adf7242_device_id[] = {
+	{ .name = "adf7242", },
+	{ },
+};
+MODULE_DEVICE_TABLE(spi, adf7242_device_id);
+
+static struct spi_driver adf7242_driver = {
+	.id_table = adf7242_device_id,
+	.driver = {
+		   .of_match_table = of_match_ptr(adf7242_of_match),
+		   .name = "adf7242",
+		   .owner = THIS_MODULE,
+		   },
+	.probe = adf7242_probe,
+	.remove = adf7242_remove,
+};
+
+module_spi_driver(adf7242_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("ADF7242 IEEE802.15.4 Transceiver Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ieee802154/atusb.c b/drivers/net/ieee802154/atusb.c
index 199a94a9c8bc..b1cd865ade2e 100644
--- a/drivers/net/ieee802154/atusb.c
+++ b/drivers/net/ieee802154/atusb.c
@@ -310,8 +310,7 @@ static void atusb_free_urbs(struct atusb *atusb)
 		urb = usb_get_from_anchor(&atusb->idle_urbs);
 		if (!urb)
 			break;
-		if (urb->context)
-			kfree_skb(urb->context);
+		kfree_skb(urb->context);
 		usb_free_urb(urb);
 	}
 }
diff --git a/drivers/net/ieee802154/cc2520.c b/drivers/net/ieee802154/cc2520.c
index e65b60591317..d50add705a79 100644
--- a/drivers/net/ieee802154/cc2520.c
+++ b/drivers/net/ieee802154/cc2520.c
@@ -21,6 +21,8 @@
 #include <linux/skbuff.h>
 #include <linux/of_gpio.h>
 #include <linux/ieee802154.h>
+#include <linux/crc-ccitt.h>
+#include <asm/unaligned.h>
 
 #include <net/mac802154.h>
 #include <net/cfg802154.h>
@@ -189,6 +191,18 @@
 #define	CC2520_RXFIFOCNT		0x3E
 #define	CC2520_TXFIFOCNT		0x3F
 
+/* CC2520_FRMFILT0 */
+#define FRMFILT0_FRAME_FILTER_EN	BIT(0)
+#define FRMFILT0_PAN_COORDINATOR	BIT(1)
+
+/* CC2520_FRMCTRL0 */
+#define FRMCTRL0_AUTOACK		BIT(5)
+#define FRMCTRL0_AUTOCRC		BIT(6)
+
+/* CC2520_FRMCTRL1 */
+#define FRMCTRL1_SET_RXENMASK_ON_TX	BIT(0)
+#define FRMCTRL1_IGNORE_TX_UNDERF	BIT(1)
+
 /* Driver private information */
 struct cc2520_private {
 	struct spi_device *spi;		/* SPI device structure */
@@ -201,6 +215,7 @@ struct cc2520_private {
 	struct work_struct fifop_irqwork;/* Workqueue for FIFOP */
 	spinlock_t lock;		/* Lock for is_tx*/
 	struct completion tx_complete;	/* Work completion for Tx */
+	bool promiscuous;               /* Flag for promiscuous mode */
 };
 
 /* Generic Functions */
@@ -367,14 +382,14 @@ cc2520_read_register(struct cc2520_private *priv, u8 reg, u8 *data)
 }
 
 static int
-cc2520_write_txfifo(struct cc2520_private *priv, u8 *data, u8 len)
+cc2520_write_txfifo(struct cc2520_private *priv, u8 pkt_len, u8 *data, u8 len)
 {
 	int status;
 
 	/* length byte must include FCS even
 	 * if it is calculated in the hardware
 	 */
-	int len_byte = len + 2;
+	int len_byte = pkt_len;
 
 	struct spi_message msg;
 
@@ -414,7 +429,7 @@ cc2520_write_txfifo(struct cc2520_private *priv, u8 *data, u8 len)
 }
 
 static int
-cc2520_read_rxfifo(struct cc2520_private *priv, u8 *data, u8 len, u8 *lqi)
+cc2520_read_rxfifo(struct cc2520_private *priv, u8 *data, u8 len)
 {
 	int status;
 	struct spi_message msg;
@@ -470,12 +485,25 @@ cc2520_tx(struct ieee802154_hw *hw, struct sk_buff *skb)
 	unsigned long flags;
 	int rc;
 	u8 status = 0;
+	u8 pkt_len;
+
+	/* In promiscuous mode we disable AUTOCRC so we can get the raw CRC
+	 * values on RX. This means we need to manually add the CRC on TX.
+	 */
+	if (priv->promiscuous) {
+		u16 crc = crc_ccitt(0, skb->data, skb->len);
+
+		put_unaligned_le16(crc, skb_put(skb, 2));
+		pkt_len = skb->len;
+	} else {
+		pkt_len = skb->len + 2;
+	}
 
 	rc = cc2520_cmd_strobe(priv, CC2520_CMD_SFLUSHTX);
 	if (rc)
 		goto err_tx;
 
-	rc = cc2520_write_txfifo(priv, skb->data, skb->len);
+	rc = cc2520_write_txfifo(priv, pkt_len, skb->data, skb->len);
 	if (rc)
 		goto err_tx;
 
@@ -518,22 +546,62 @@ static int cc2520_rx(struct cc2520_private *priv)
 	u8 len = 0, lqi = 0, bytes = 1;
 	struct sk_buff *skb;
 
-	cc2520_read_rxfifo(priv, &len, bytes, &lqi);
+	/* Read single length byte from the radio. */
+	cc2520_read_rxfifo(priv, &len, bytes);
 
-	if (len < 2 || len > IEEE802154_MTU)
-		return -EINVAL;
+	if (!ieee802154_is_valid_psdu_len(len)) {
+		/* Corrupted frame received, clear frame buffer by
+		 * reading entire buffer.
+		 */
+		dev_dbg(&priv->spi->dev, "corrupted frame received\n");
+		len = IEEE802154_MTU;
+	}
 
 	skb = dev_alloc_skb(len);
 	if (!skb)
 		return -ENOMEM;
 
-	if (cc2520_read_rxfifo(priv, skb_put(skb, len), len, &lqi)) {
+	if (cc2520_read_rxfifo(priv, skb_put(skb, len), len)) {
 		dev_dbg(&priv->spi->dev, "frame reception failed\n");
 		kfree_skb(skb);
 		return -EINVAL;
 	}
 
-	skb_trim(skb, skb->len - 2);
+	/* In promiscuous mode, we configure the radio to include the
+	 * CRC (AUTOCRC==0) and we pass on the packet unconditionally. If not
+	 * in promiscuous mode, we check the CRC here, but leave the
+	 * RSSI/LQI/CRC_OK bytes as they will get removed in the mac layer.
+	 */
+	if (!priv->promiscuous) {
+		bool crc_ok;
+
+		/* Check if the CRC is valid. With AUTOCRC set, the most
+		 * significant bit of the last byte returned from the CC2520
+		 * is CRC_OK flag. See section 20.3.4 of the datasheet.
+		 */
+		crc_ok = skb->data[len - 1] & BIT(7);
+
+		/* If we failed CRC drop the packet in the driver layer. */
+		if (!crc_ok) {
+			dev_dbg(&priv->spi->dev, "CRC check failed\n");
+			kfree_skb(skb);
+			return -EINVAL;
+		}
+
+		/* To calculate LQI, the lower 7 bits of the last byte (the
+		 * correlation value provided by the radio) must be scaled to
+		 * the range 0-255. According to section 20.6, the correlation
+		 * value ranges from 50-110. Ideally this would be calibrated
+		 * per hardware design, but we use roughly the datasheet values
+		 * to get close enough while avoiding floating point.
+		 */
+		lqi = skb->data[len - 1] & 0x7f;
+		if (lqi < 50)
+			lqi = 50;
+		else if (lqi > 113)
+			lqi = 113;
+		lqi = (lqi - 50) * 4;
+	}
 
 	ieee802154_rx_irqsafe(priv->hw, skb, lqi);
 
@@ -619,14 +687,19 @@ cc2520_filter(struct ieee802154_hw *hw,
 	}
 
 	if (changed & IEEE802154_AFILT_PANC_CHANGED) {
+		u8 frmfilt0;
+
 		dev_vdbg(&priv->spi->dev,
 			 "cc2520_filter called for panc change\n");
+
+		cc2520_read_register(priv, CC2520_FRMFILT0, &frmfilt0);
+
 		if (filt->pan_coord)
-			ret = cc2520_write_register(priv, CC2520_FRMFILT0,
-						    0x02);
+			frmfilt0 |= FRMFILT0_PAN_COORDINATOR;
 		else
-			ret = cc2520_write_register(priv, CC2520_FRMFILT0,
-						    0x00);
+			frmfilt0 &= ~FRMFILT0_PAN_COORDINATOR;
+
+		ret = cc2520_write_register(priv, CC2520_FRMFILT0, frmfilt0);
 	}
 
 	return ret;
@@ -723,6 +796,30 @@ cc2520_set_txpower(struct ieee802154_hw *hw, s32 mbm)
 	return cc2520_cc2591_set_tx_power(priv, mbm);
 }
 
+static int
+cc2520_set_promiscuous_mode(struct ieee802154_hw *hw, bool on)
+{
+	struct cc2520_private *priv = hw->priv;
+	u8 frmfilt0;
+
+	dev_dbg(&priv->spi->dev, "%s : mode %d\n", __func__, on);
+
+	priv->promiscuous = on;
+
+	cc2520_read_register(priv, CC2520_FRMFILT0, &frmfilt0);
+
+	if (on) {
+		/* Disable automatic ACK, automatic CRC, and frame filtering. */
+		cc2520_write_register(priv, CC2520_FRMCTRL0, 0);
+		frmfilt0 &= ~FRMFILT0_FRAME_FILTER_EN;
+	} else {
+		cc2520_write_register(priv, CC2520_FRMCTRL0, FRMCTRL0_AUTOACK |
+							     FRMCTRL0_AUTOCRC);
+		frmfilt0 |= FRMFILT0_FRAME_FILTER_EN;
+	}
+	return cc2520_write_register(priv, CC2520_FRMFILT0, frmfilt0);
+}
+
 static const struct ieee802154_ops cc2520_ops = {
 	.owner = THIS_MODULE,
 	.start = cc2520_start,
@@ -732,6 +829,7 @@ static const struct ieee802154_ops cc2520_ops = {
 	.set_channel = cc2520_set_channel,
 	.set_hw_addr_filt = cc2520_filter,
 	.set_txpower = cc2520_set_txpower,
+	.set_promiscuous_mode = cc2520_set_promiscuous_mode,
 };
 
 static int cc2520_register(struct cc2520_private *priv)
@@ -749,7 +847,8 @@ static int cc2520_register(struct cc2520_private *priv)
 
 	/* We do support only 2.4 Ghz */
 	priv->hw->phy->supported.channels[0] = 0x7FFF800;
-	priv->hw->flags = IEEE802154_HW_OMIT_CKSUM | IEEE802154_HW_AFILT;
+	priv->hw->flags = IEEE802154_HW_TX_OMIT_CKSUM | IEEE802154_HW_AFILT |
+			  IEEE802154_HW_PROMISCUOUS;
 
 	priv->hw->phy->flags = WPAN_PHY_FLAG_TXPOWER;
 
@@ -919,6 +1018,11 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 	}
 
 	/* Registers default value: section 28.1 in Datasheet */
+
+	/* Set the CCA threshold to -50 dBm. This seems to have been copied
+	 * from the TinyOS CC2520 driver and is much higher than the -84 dBm
+	 * threshold suggested in the datasheet.
+	 */
 	ret = cc2520_write_register(priv, CC2520_CCACTRL0, 0x1A);
 	if (ret)
 		goto err_ret;
@@ -955,15 +1059,10 @@ static int cc2520_hw_init(struct cc2520_private *priv)
 	if (ret)
 		goto err_ret;
 
-	ret = cc2520_write_register(priv, CC2520_FRMCTRL0, 0x60);
-	if (ret)
-		goto err_ret;
-
-	ret = cc2520_write_register(priv, CC2520_FRMCTRL1, 0x03);
-	if (ret)
-		goto err_ret;
-
-	ret = cc2520_write_register(priv, CC2520_FRMFILT0, 0x00);
+	/* Configure registers correctly for this driver. */
+	ret = cc2520_write_register(priv, CC2520_FRMCTRL1,
+				    FRMCTRL1_SET_RXENMASK_ON_TX |
+				    FRMCTRL1_IGNORE_TX_UNDERF);
 	if (ret)
 		goto err_ret;
 
diff --git a/drivers/net/ipvlan/ipvlan_main.c b/drivers/net/ipvlan/ipvlan_main.c
index a9268db4e349..f94392d07126 100644
--- a/drivers/net/ipvlan/ipvlan_main.c
+++ b/drivers/net/ipvlan/ipvlan_main.c
@@ -88,7 +88,7 @@ static struct lock_class_key ipvlan_netdev_xmit_lock_key;
 static struct lock_class_key ipvlan_netdev_addr_lock_key;
 
 #define IPVLAN_FEATURES \
-	(NETIF_F_SG | NETIF_F_ALL_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
+	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
 	 NETIF_F_GSO | NETIF_F_TSO | NETIF_F_UFO | NETIF_F_GSO_ROBUST | \
 	 NETIF_F_TSO_ECN | NETIF_F_TSO6 | NETIF_F_GRO | NETIF_F_RXCSUM | \
 	 NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_VLAN_STAG_FILTER)
diff --git a/drivers/net/irda/bfin_sir.h b/drivers/net/irda/bfin_sir.h
index 29cbde8501ed..d47cf14bb4a5 100644
--- a/drivers/net/irda/bfin_sir.h
+++ b/drivers/net/irda/bfin_sir.h
@@ -82,9 +82,6 @@ struct bfin_sir_self {
 
 #define DRIVER_NAME "bfin_sir"
 
-#define port_membase(port)     (((struct bfin_sir_port *)(port))->membase)
-#define get_lsr_cache(port)    (((struct bfin_sir_port *)(port))->lsr)
-#define put_lsr_cache(port, v) (((struct bfin_sir_port *)(port))->lsr = (v))
 #include <asm/bfin_serial.h>
 
 static const unsigned short per[][4] = {
diff --git a/drivers/net/irda/toim3232-sir.c b/drivers/net/irda/toim3232-sir.c
index 6d2f55959c49..b977d6d33e74 100644
--- a/drivers/net/irda/toim3232-sir.c
+++ b/drivers/net/irda/toim3232-sir.c
@@ -130,16 +130,6 @@ static int toim3232delay = 150;	/* default is 150 ms */
 module_param(toim3232delay, int, 0);
 MODULE_PARM_DESC(toim3232delay, "toim3232 dongle write complete delay");
 
-#if 0
-static int toim3232flipdtr = 0;	/* default is DTR high to reset */
-module_param(toim3232flipdtr, int, 0);
-MODULE_PARM_DESC(toim3232flipdtr, "toim3232 dongle invert DTR (Reset)");
-
-static int toim3232fliprts = 0;	/* default is RTS high for baud change */
-module_param(toim3232fliptrs, int, 0);
-MODULE_PARM_DESC(toim3232fliprts, "toim3232 dongle invert RTS (BR/D)");
-#endif
-
 static int toim3232_open(struct sir_dev *);
 static int toim3232_close(struct sir_dev *);
 static int toim3232_change_speed(struct sir_dev *, unsigned);
diff --git a/drivers/net/loopback.c b/drivers/net/loopback.c
index dc7d970bd1c0..a400288cb37b 100644
--- a/drivers/net/loopback.c
+++ b/drivers/net/loopback.c
@@ -175,7 +175,7 @@ static void loopback_setup(struct net_device *dev)
 		| NETIF_F_UFO
 		| NETIF_F_HW_CSUM
 		| NETIF_F_RXCSUM
-		| NETIF_F_SCTP_CSUM
+		| NETIF_F_SCTP_CRC
 		| NETIF_F_HIGHDMA
 		| NETIF_F_LLTX
 		| NETIF_F_NETNS_LOCAL
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index 06c8bfeaccd6..94e688805dd2 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -758,11 +758,11 @@ static struct lock_class_key macvlan_netdev_xmit_lock_key;
 static struct lock_class_key macvlan_netdev_addr_lock_key;
 
 #define ALWAYS_ON_FEATURES \
-	(NETIF_F_SG | NETIF_F_GEN_CSUM | NETIF_F_GSO_SOFTWARE | NETIF_F_LLTX | \
+	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_GSO_SOFTWARE | NETIF_F_LLTX | \
 	 NETIF_F_GSO_ROBUST)
 
 #define MACVLAN_FEATURES \
-	(NETIF_F_SG | NETIF_F_ALL_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
+	(NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_HIGHDMA | NETIF_F_FRAGLIST | \
 	 NETIF_F_GSO | NETIF_F_TSO | NETIF_F_UFO | NETIF_F_LRO | \
 	 NETIF_F_TSO_ECN | NETIF_F_TSO6 | NETIF_F_GRO | NETIF_F_RXCSUM | \
 	 NETIF_F_HW_VLAN_CTAG_FILTER | NETIF_F_HW_VLAN_STAG_FILTER)
@@ -1323,6 +1323,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 
 	list_add_tail_rcu(&vlan->list, &port->vlans);
 	netif_stacked_transfer_operstate(lowerdev, dev);
+	linkwatch_fire_event(dev);
 
 	return 0;
 
@@ -1522,6 +1523,7 @@ static int macvlan_device_event(struct notifier_block *unused,
 	port = macvlan_port_get_rtnl(dev);
 
 	switch (event) {
+	case NETDEV_UP:
 	case NETDEV_CHANGE:
 		list_for_each_entry(vlan, &port->vlans, list)
 			netif_stacked_transfer_operstate(vlan->lowerdev,
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 0fc521941c71..d636d051fac8 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -388,7 +388,7 @@ static rx_handler_result_t macvtap_handle_frame(struct sk_buff **pskb)
 		 *        check, we either support them all or none.
 		 */
 		if (skb->ip_summed == CHECKSUM_PARTIAL &&
-		    !(features & NETIF_F_ALL_CSUM) &&
+		    !(features & NETIF_F_CSUM_MASK) &&
 		    skb_checksum_help(skb))
 			goto drop;
 		skb_queue_tail(&q->sk.sk_receive_queue, skb);
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 60994a83a0d6..f0a77020037a 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -186,6 +186,7 @@ config MDIO_GPIO
 config MDIO_OCTEON
 	tristate "Support for MDIO buses on Octeon and ThunderX SOCs"
 	depends on 64BIT
+	depends on HAS_IOMEM
 	help
 
 	  This module provides a driver for the Octeon and ThunderX MDIO
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index f31a4e25cf15..680e88f9915a 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -1,6 +1,6 @@
 # Makefile for Linux PHY drivers
 
-libphy-objs			:= phy.o phy_device.o mdio_bus.o
+libphy-objs			:= phy.o phy_device.o mdio_bus.o mdio_device.o
 
 obj-$(CONFIG_PHYLIB)		+= libphy.o
 obj-$(CONFIG_AQUANTIA_PHY)	+= aquantia.o
diff --git a/drivers/net/phy/amd.c b/drivers/net/phy/amd.c
index 65a488f82eb8..18141c022b13 100644
--- a/drivers/net/phy/amd.c
+++ b/drivers/net/phy/amd.c
@@ -72,7 +72,6 @@ static struct phy_driver am79c_driver[] = { {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= am79c_ack_interrupt,
 	.config_intr	= am79c_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(am79c_driver);
diff --git a/drivers/net/phy/aquantia.c b/drivers/net/phy/aquantia.c
index f1936b7a7af6..09b0b0aa8d68 100644
--- a/drivers/net/phy/aquantia.c
+++ b/drivers/net/phy/aquantia.c
@@ -128,7 +128,6 @@ static struct phy_driver aquantia_driver[] = {
 	.config_intr	= aquantia_config_intr,
 	.ack_interrupt	= aquantia_ack_interrupt,
 	.read_status	= aquantia_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 },
 {
 	.phy_id		= PHY_ID_AQ2104,
@@ -141,7 +140,6 @@ static struct phy_driver aquantia_driver[] = {
 	.config_intr	= aquantia_config_intr,
 	.ack_interrupt	= aquantia_ack_interrupt,
 	.read_status	= aquantia_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 },
 {
 	.phy_id		= PHY_ID_AQR105,
@@ -154,7 +152,6 @@ static struct phy_driver aquantia_driver[] = {
 	.config_intr	= aquantia_config_intr,
 	.ack_interrupt	= aquantia_ack_interrupt,
 	.read_status	= aquantia_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 },
 {
 	.phy_id		= PHY_ID_AQR405,
@@ -167,7 +164,6 @@ static struct phy_driver aquantia_driver[] = {
 	.config_intr	= aquantia_config_intr,
 	.ack_interrupt	= aquantia_ack_interrupt,
 	.read_status	= aquantia_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 },
 };
 
diff --git a/drivers/net/phy/at803x.c b/drivers/net/phy/at803x.c
index 2d020a3ec0b5..2174ec937b4d 100644
--- a/drivers/net/phy/at803x.c
+++ b/drivers/net/phy/at803x.c
@@ -20,10 +20,21 @@
 #include <linux/gpio/consumer.h>
 
 #define AT803X_INTR_ENABLE			0x12
+#define AT803X_INTR_ENABLE_AUTONEG_ERR		BIT(15)
+#define AT803X_INTR_ENABLE_SPEED_CHANGED	BIT(14)
+#define AT803X_INTR_ENABLE_DUPLEX_CHANGED	BIT(13)
+#define AT803X_INTR_ENABLE_PAGE_RECEIVED	BIT(12)
+#define AT803X_INTR_ENABLE_LINK_FAIL		BIT(11)
+#define AT803X_INTR_ENABLE_LINK_SUCCESS		BIT(10)
+#define AT803X_INTR_ENABLE_WIRESPEED_DOWNGRADE	BIT(5)
+#define AT803X_INTR_ENABLE_POLARITY_CHANGED	BIT(1)
+#define AT803X_INTR_ENABLE_WOL			BIT(0)
+
 #define AT803X_INTR_STATUS			0x13
+
 #define AT803X_SMART_SPEED			0x14
 #define AT803X_LED_CONTROL			0x18
-#define AT803X_WOL_ENABLE			0x01
+
 #define AT803X_DEVICE_ADDR			0x03
 #define AT803X_LOC_MAC_ADDR_0_15_OFFSET		0x804C
 #define AT803X_LOC_MAC_ADDR_16_31_OFFSET	0x804B
@@ -31,13 +42,15 @@
 #define AT803X_MMD_ACCESS_CONTROL		0x0D
 #define AT803X_MMD_ACCESS_CONTROL_DATA		0x0E
 #define AT803X_FUNC_DATA			0x4003
-#define AT803X_INER				0x0012
-#define AT803X_INER_INIT			0xec00
-#define AT803X_INSR				0x0013
+
 #define AT803X_DEBUG_ADDR			0x1D
 #define AT803X_DEBUG_DATA			0x1E
-#define AT803X_DEBUG_SYSTEM_MODE_CTRL		0x05
-#define AT803X_DEBUG_RGMII_TX_CLK_DLY		BIT(8)
+
+#define AT803X_DEBUG_REG_0			0x00
+#define AT803X_DEBUG_RX_CLK_DLY_EN		BIT(15)
+
+#define AT803X_DEBUG_REG_5			0x05
+#define AT803X_DEBUG_TX_CLK_DLY_EN		BIT(8)
 
 #define ATH8030_PHY_ID 0x004dd076
 #define ATH8031_PHY_ID 0x004dd074
@@ -61,6 +74,46 @@ struct at803x_context {
 	u16 led_control;
 };
 
+static int at803x_debug_reg_read(struct phy_device *phydev, u16 reg)
+{
+	int ret;
+
+	ret = phy_write(phydev, AT803X_DEBUG_ADDR, reg);
+	if (ret < 0)
+		return ret;
+
+	return phy_read(phydev, AT803X_DEBUG_DATA);
+}
+
+static int at803x_debug_reg_mask(struct phy_device *phydev, u16 reg,
+				 u16 clear, u16 set)
+{
+	u16 val;
+	int ret;
+
+	ret = at803x_debug_reg_read(phydev, reg);
+	if (ret < 0)
+		return ret;
+
+	val = ret & 0xffff;
+	val &= ~clear;
+	val |= set;
+
+	return phy_write(phydev, AT803X_DEBUG_DATA, val);
+}
+
+static inline int at803x_enable_rx_delay(struct phy_device *phydev)
+{
+	return at803x_debug_reg_mask(phydev, AT803X_DEBUG_REG_0, 0,
+					AT803X_DEBUG_RX_CLK_DLY_EN);
+}
+
+static inline int at803x_enable_tx_delay(struct phy_device *phydev)
+{
+	return at803x_debug_reg_mask(phydev, AT803X_DEBUG_REG_5, 0,
+					AT803X_DEBUG_TX_CLK_DLY_EN);
+}
+
 /* save relevant PHY registers to private copy */
 static void at803x_context_save(struct phy_device *phydev,
 				struct at803x_context *context)
@@ -119,14 +172,14 @@ static int at803x_set_wol(struct phy_device *phydev,
 		}
 
 		value = phy_read(phydev, AT803X_INTR_ENABLE);
-		value |= AT803X_WOL_ENABLE;
+		value |= AT803X_INTR_ENABLE_WOL;
 		ret = phy_write(phydev, AT803X_INTR_ENABLE, value);
 		if (ret)
 			return ret;
 		value = phy_read(phydev, AT803X_INTR_STATUS);
 	} else {
 		value = phy_read(phydev, AT803X_INTR_ENABLE);
-		value &= (~AT803X_WOL_ENABLE);
+		value &= (~AT803X_INTR_ENABLE_WOL);
 		ret = phy_write(phydev, AT803X_INTR_ENABLE, value);
 		if (ret)
 			return ret;
@@ -145,7 +198,7 @@ static void at803x_get_wol(struct phy_device *phydev,
 	wol->wolopts = 0;
 
 	value = phy_read(phydev, AT803X_INTR_ENABLE);
-	if (value & AT803X_WOL_ENABLE)
+	if (value & AT803X_INTR_ENABLE_WOL)
 		wol->wolopts |= WAKE_MAGIC;
 }
 
@@ -157,7 +210,7 @@ static int at803x_suspend(struct phy_device *phydev)
 	mutex_lock(&phydev->lock);
 
 	value = phy_read(phydev, AT803X_INTR_ENABLE);
-	wol_enabled = value & AT803X_WOL_ENABLE;
+	wol_enabled = value & AT803X_INTR_ENABLE_WOL;
 
 	value = phy_read(phydev, MII_BMCR);
 
@@ -190,7 +243,7 @@ static int at803x_resume(struct phy_device *phydev)
 
 static int at803x_probe(struct phy_device *phydev)
 {
-	struct device *dev = &phydev->dev;
+	struct device *dev = &phydev->mdio.dev;
 	struct at803x_priv *priv;
 	struct gpio_desc *gpiod_reset;
 
@@ -217,14 +270,17 @@ static int at803x_config_init(struct phy_device *phydev)
 	if (ret < 0)
 		return ret;
 
-	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID) {
-		ret = phy_write(phydev, AT803X_DEBUG_ADDR,
-				AT803X_DEBUG_SYSTEM_MODE_CTRL);
-		if (ret)
+	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_RXID ||
+			phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
+		ret = at803x_enable_rx_delay(phydev);
+		if (ret < 0)
 			return ret;
-		ret = phy_write(phydev, AT803X_DEBUG_DATA,
-				AT803X_DEBUG_RGMII_TX_CLK_DLY);
-		if (ret)
+	}
+
+	if (phydev->interface == PHY_INTERFACE_MODE_RGMII_TXID ||
+			phydev->interface == PHY_INTERFACE_MODE_RGMII_ID) {
+		ret = at803x_enable_tx_delay(phydev);
+		if (ret < 0)
 			return ret;
 	}
 
@@ -235,7 +291,7 @@ static int at803x_ack_interrupt(struct phy_device *phydev)
 {
 	int err;
 
-	err = phy_read(phydev, AT803X_INSR);
+	err = phy_read(phydev, AT803X_INTR_STATUS);
 
 	return (err < 0) ? err : 0;
 }
@@ -245,13 +301,19 @@ static int at803x_config_intr(struct phy_device *phydev)
 	int err;
 	int value;
 
-	value = phy_read(phydev, AT803X_INER);
+	value = phy_read(phydev, AT803X_INTR_ENABLE);
+
+	if (phydev->interrupts == PHY_INTERRUPT_ENABLED) {
+		value |= AT803X_INTR_ENABLE_AUTONEG_ERR;
+		value |= AT803X_INTR_ENABLE_SPEED_CHANGED;
+		value |= AT803X_INTR_ENABLE_DUPLEX_CHANGED;
+		value |= AT803X_INTR_ENABLE_LINK_FAIL;
+		value |= AT803X_INTR_ENABLE_LINK_SUCCESS;
 
-	if (phydev->interrupts == PHY_INTERRUPT_ENABLED)
-		err = phy_write(phydev, AT803X_INER,
-				value | AT803X_INER_INIT);
+		err = phy_write(phydev, AT803X_INTR_ENABLE, value);
+	}
 	else
-		err = phy_write(phydev, AT803X_INER, 0);
+		err = phy_write(phydev, AT803X_INTR_ENABLE, 0);
 
 	return err;
 }
@@ -281,8 +343,8 @@ static void at803x_link_change_notify(struct phy_device *phydev)
 
 				at803x_context_restore(phydev, &context);
 
-				dev_dbg(&phydev->dev, "%s(): phy was reset\n",
-					__func__);
+				phydev_dbg(phydev, "%s(): phy was reset\n",
+					   __func__);
 				priv->phy_reset = true;
 			}
 		} else {
@@ -310,9 +372,6 @@ static struct phy_driver at803x_driver[] = {
 	.read_status		= genphy_read_status,
 	.ack_interrupt		= at803x_ack_interrupt,
 	.config_intr		= at803x_config_intr,
-	.driver			= {
-		.owner = THIS_MODULE,
-	},
 }, {
 	/* ATHEROS 8030 */
 	.phy_id			= ATH8030_PHY_ID,
@@ -325,15 +384,12 @@ static struct phy_driver at803x_driver[] = {
 	.get_wol		= at803x_get_wol,
 	.suspend		= at803x_suspend,
 	.resume			= at803x_resume,
-	.features		= PHY_GBIT_FEATURES,
+	.features		= PHY_BASIC_FEATURES,
 	.flags			= PHY_HAS_INTERRUPT,
 	.config_aneg		= genphy_config_aneg,
 	.read_status		= genphy_read_status,
 	.ack_interrupt		= at803x_ack_interrupt,
 	.config_intr		= at803x_config_intr,
-	.driver			= {
-		.owner = THIS_MODULE,
-	},
 }, {
 	/* ATHEROS 8031 */
 	.phy_id			= ATH8031_PHY_ID,
@@ -352,9 +408,6 @@ static struct phy_driver at803x_driver[] = {
 	.read_status		= genphy_read_status,
 	.ack_interrupt		= &at803x_ack_interrupt,
 	.config_intr		= &at803x_config_intr,
-	.driver			= {
-		.owner = THIS_MODULE,
-	},
 } };
 
 module_phy_driver(at803x_driver);
diff --git a/drivers/net/phy/bcm-phy-lib.c b/drivers/net/phy/bcm-phy-lib.c
index ddb377e53633..df0416db0b88 100644
--- a/drivers/net/phy/bcm-phy-lib.c
+++ b/drivers/net/phy/bcm-phy-lib.c
@@ -184,25 +184,25 @@ int bcm_phy_enable_eee(struct phy_device *phydev)
 
 	/* Enable EEE at PHY level */
 	val = phy_read_mmd_indirect(phydev, BRCM_CL45VEN_EEE_CONTROL,
-				    MDIO_MMD_AN, phydev->addr);
+				    MDIO_MMD_AN);
 	if (val < 0)
 		return val;
 
 	val |= LPI_FEATURE_EN | LPI_FEATURE_EN_DIG1000X;
 
 	phy_write_mmd_indirect(phydev, BRCM_CL45VEN_EEE_CONTROL,
-			       MDIO_MMD_AN,  phydev->addr, (u32)val);
+			       MDIO_MMD_AN, (u32)val);
 
 	/* Advertise EEE */
 	val = phy_read_mmd_indirect(phydev, BCM_CL45VEN_EEE_ADV,
-				    MDIO_MMD_AN, phydev->addr);
+				    MDIO_MMD_AN);
 	if (val < 0)
 		return val;
 
 	val |= (MDIO_AN_EEE_ADV_100TX | MDIO_AN_EEE_ADV_1000T);
 
 	phy_write_mmd_indirect(phydev, BCM_CL45VEN_EEE_ADV,
-			       MDIO_MMD_AN,  phydev->addr, (u32)val);
+			       MDIO_MMD_AN, (u32)val);
 
 	return 0;
 }
diff --git a/drivers/net/phy/bcm63xx.c b/drivers/net/phy/bcm63xx.c
index 86b28052bf06..e741bf614c4e 100644
--- a/drivers/net/phy/bcm63xx.c
+++ b/drivers/net/phy/bcm63xx.c
@@ -56,7 +56,6 @@ static struct phy_driver bcm63xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	/* same phy as above, with just a different OUI */
 	.phy_id		= 0x002bdc00,
@@ -69,7 +68,6 @@ static struct phy_driver bcm63xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 } };
 
 module_phy_driver(bcm63xx_driver);
diff --git a/drivers/net/phy/bcm7xxx.c b/drivers/net/phy/bcm7xxx.c
index 03d4809a9126..db507e3bcab9 100644
--- a/drivers/net/phy/bcm7xxx.c
+++ b/drivers/net/phy/bcm7xxx.c
@@ -170,7 +170,7 @@ static int bcm7xxx_28nm_config_init(struct phy_device *phydev)
 	int ret = 0;
 
 	pr_info_once("%s: %s PHY revision: 0x%02x, patch: %d\n",
-		     dev_name(&phydev->dev), phydev->drv->name, rev, patch);
+		     phydev_name(phydev), phydev->drv->name, rev, patch);
 
 	/* Dummy read to a register to workaround an issue upon reset where the
 	 * internal inverter may not allow the first MDIO transaction to pass
@@ -250,10 +250,6 @@ static int bcm7xxx_config_init(struct phy_device *phydev)
 	phy_write(phydev, MII_BCM7XXX_AUX_MODE, MII_BCM7XX_64CLK_MDIO);
 	phy_read(phydev, MII_BCM7XXX_AUX_MODE);
 
-	/* Workaround only required for 100Mbits/sec capable PHYs */
-	if (phydev->supported & PHY_GBIT_FEATURES)
-		return 0;
-
 	/* set shadow mode 2 */
 	ret = phy_set_clr_bits(phydev, MII_BCM7XXX_TEST,
 			MII_BCM7XXX_SHD_MODE_2, MII_BCM7XXX_SHD_MODE_2);
@@ -270,7 +266,7 @@ static int bcm7xxx_config_init(struct phy_device *phydev)
 	phy_write(phydev, MII_BCM7XXX_100TX_FALSE_CAR, 0x7555);
 
 	/* reset shadow mode 2 */
-	ret = phy_set_clr_bits(phydev, MII_BCM7XXX_TEST, MII_BCM7XXX_SHD_MODE_2, 0);
+	ret = phy_set_clr_bits(phydev, MII_BCM7XXX_TEST, 0, MII_BCM7XXX_SHD_MODE_2);
 	if (ret < 0)
 		return ret;
 
@@ -307,11 +303,6 @@ static int bcm7xxx_suspend(struct phy_device *phydev)
 	return 0;
 }
 
-static int bcm7xxx_dummy_config_init(struct phy_device *phydev)
-{
-	return 0;
-}
-
 #define BCM7XXX_28NM_GPHY(_oui, _name)					\
 {									\
 	.phy_id		= (_oui),					\
@@ -324,7 +315,6 @@ static int bcm7xxx_dummy_config_init(struct phy_device *phydev)
 	.config_aneg	= genphy_config_aneg,				\
 	.read_status	= genphy_read_status,				\
 	.resume		= bcm7xxx_28nm_resume,				\
-	.driver		= { .owner = THIS_MODULE },			\
 }
 
 static struct phy_driver bcm7xxx_driver[] = {
@@ -338,7 +328,7 @@ static struct phy_driver bcm7xxx_driver[] = {
 	.phy_id         = PHY_ID_BCM7425,
 	.phy_id_mask    = 0xfffffff0,
 	.name           = "Broadcom BCM7425",
-	.features       = PHY_GBIT_FEATURES |
+	.features       = PHY_BASIC_FEATURES |
 			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
 	.flags          = PHY_IS_INTERNAL,
 	.config_init    = bcm7xxx_config_init,
@@ -346,12 +336,11 @@ static struct phy_driver bcm7xxx_driver[] = {
 	.read_status    = genphy_read_status,
 	.suspend        = bcm7xxx_suspend,
 	.resume         = bcm7xxx_config_init,
-	.driver         = { .owner = THIS_MODULE },
 }, {
 	.phy_id         = PHY_ID_BCM7429,
 	.phy_id_mask    = 0xfffffff0,
 	.name           = "Broadcom BCM7429",
-	.features       = PHY_GBIT_FEATURES |
+	.features       = PHY_BASIC_FEATURES |
 			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
 	.flags          = PHY_IS_INTERNAL,
 	.config_init    = bcm7xxx_config_init,
@@ -359,33 +348,18 @@ static struct phy_driver bcm7xxx_driver[] = {
 	.read_status    = genphy_read_status,
 	.suspend        = bcm7xxx_suspend,
 	.resume         = bcm7xxx_config_init,
-	.driver         = { .owner = THIS_MODULE },
-}, {
-	.phy_id		= PHY_BCM_OUI_4,
-	.phy_id_mask	= 0xffff0000,
-	.name		= "Broadcom BCM7XXX 40nm",
-	.features	= PHY_GBIT_FEATURES |
-			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_config_init,
-	.driver		= { .owner = THIS_MODULE },
 }, {
-	.phy_id		= PHY_BCM_OUI_5,
-	.phy_id_mask	= 0xffffff00,
-	.name		= "Broadcom BCM7XXX 65nm",
-	.features	= PHY_BASIC_FEATURES |
+	.phy_id         = PHY_ID_BCM7435,
+	.phy_id_mask    = 0xfffffff0,
+	.name           = "Broadcom BCM7435",
+	.features       = PHY_BASIC_FEATURES |
 			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_dummy_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_config_init,
-	.driver		= { .owner = THIS_MODULE },
+	.flags          = PHY_IS_INTERNAL,
+	.config_init    = bcm7xxx_config_init,
+	.config_aneg    = genphy_config_aneg,
+	.read_status    = genphy_read_status,
+	.suspend        = bcm7xxx_suspend,
+	.resume         = bcm7xxx_config_init,
 } };
 
 static struct mdio_device_id __maybe_unused bcm7xxx_tbl[] = {
@@ -395,9 +369,8 @@ static struct mdio_device_id __maybe_unused bcm7xxx_tbl[] = {
 	{ PHY_ID_BCM7425, 0xfffffff0, },
 	{ PHY_ID_BCM7429, 0xfffffff0, },
 	{ PHY_ID_BCM7439, 0xfffffff0, },
+	{ PHY_ID_BCM7435, 0xfffffff0, },
 	{ PHY_ID_BCM7445, 0xfffffff0, },
-	{ PHY_BCM_OUI_4, 0xffff0000 },
-	{ PHY_BCM_OUI_5, 0xffffff00 },
 	{ }
 };
 
diff --git a/drivers/net/phy/bcm87xx.c b/drivers/net/phy/bcm87xx.c
index 1eca20452f03..f7ebdcff53e4 100644
--- a/drivers/net/phy/bcm87xx.c
+++ b/drivers/net/phy/bcm87xx.c
@@ -40,10 +40,10 @@ static int bcm87xx_of_reg_init(struct phy_device *phydev)
 	const __be32 *paddr_end;
 	int len, ret;
 
-	if (!phydev->dev.of_node)
+	if (!phydev->mdio.dev.of_node)
 		return 0;
 
-	paddr = of_get_property(phydev->dev.of_node,
+	paddr = of_get_property(phydev->mdio.dev.of_node,
 				"broadcom,c45-reg-init", &len);
 	if (!paddr)
 		return 0;
@@ -163,8 +163,9 @@ static int bcm87xx_did_interrupt(struct phy_device *phydev)
 	reg = phy_read(phydev, BCM87XX_LASI_STATUS);
 
 	if (reg < 0) {
-		dev_err(&phydev->dev,
-			"Error: Read of BCM87XX_LASI_STATUS failed: %d\n", reg);
+		phydev_err(phydev,
+			   "Error: Read of BCM87XX_LASI_STATUS failed: %d\n",
+			   reg);
 		return 0;
 	}
 	return (reg & 1) != 0;
@@ -200,7 +201,6 @@ static struct phy_driver bcm87xx_driver[] = {
 	.config_intr	= bcm87xx_config_intr,
 	.did_interrupt	= bcm87xx_did_interrupt,
 	.match_phy_device = bcm8706_match_phy_device,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM8727,
 	.phy_id_mask	= 0xffffffff,
@@ -213,7 +213,6 @@ static struct phy_driver bcm87xx_driver[] = {
 	.config_intr	= bcm87xx_config_intr,
 	.did_interrupt	= bcm87xx_did_interrupt,
 	.match_phy_device = bcm8727_match_phy_device,
-	.driver		= { .owner = THIS_MODULE },
 } };
 
 module_phy_driver(bcm87xx_driver);
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 3ce5d9514623..870327efccf7 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -460,7 +460,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5421,
 	.phy_id_mask	= 0xfffffff0,
@@ -473,7 +472,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5461,
 	.phy_id_mask	= 0xfffffff0,
@@ -486,7 +484,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM54616S,
 	.phy_id_mask	= 0xfffffff0,
@@ -499,7 +496,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5464,
 	.phy_id_mask	= 0xfffffff0,
@@ -512,7 +508,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5481,
 	.phy_id_mask	= 0xfffffff0,
@@ -525,7 +520,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5482,
 	.phy_id_mask	= 0xfffffff0,
@@ -538,7 +532,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= bcm5482_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM50610,
 	.phy_id_mask	= 0xfffffff0,
@@ -551,7 +544,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM50610M,
 	.phy_id_mask	= 0xfffffff0,
@@ -564,7 +556,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM57780,
 	.phy_id_mask	= 0xfffffff0,
@@ -577,7 +568,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= bcm_phy_ack_intr,
 	.config_intr	= bcm_phy_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCMAC131,
 	.phy_id_mask	= 0xfffffff0,
@@ -590,7 +580,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= brcm_fet_ack_interrupt,
 	.config_intr	= brcm_fet_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_ID_BCM5241,
 	.phy_id_mask	= 0xfffffff0,
@@ -603,7 +592,6 @@ static struct phy_driver broadcom_drivers[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= brcm_fet_ack_interrupt,
 	.config_intr	= brcm_fet_config_intr,
-	.driver		= { .owner = THIS_MODULE },
 } };
 
 module_phy_driver(broadcom_drivers);
diff --git a/drivers/net/phy/cicada.c b/drivers/net/phy/cicada.c
index 27f5464899d4..d339c1afea77 100644
--- a/drivers/net/phy/cicada.c
+++ b/drivers/net/phy/cicada.c
@@ -114,7 +114,6 @@ static struct phy_driver cis820x_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &cis820x_ack_interrupt,
 	.config_intr	= &cis820x_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x000fc440,
 	.name		= "Cicada Cis8204",
@@ -126,7 +125,6 @@ static struct phy_driver cis820x_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &cis820x_ack_interrupt,
 	.config_intr	= &cis820x_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(cis820x_driver);
diff --git a/drivers/net/phy/davicom.c b/drivers/net/phy/davicom.c
index 2a328703b4ae..36e3e2033eca 100644
--- a/drivers/net/phy/davicom.c
+++ b/drivers/net/phy/davicom.c
@@ -156,7 +156,6 @@ static struct phy_driver dm91xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= dm9161_ack_interrupt,
 	.config_intr	= dm9161_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x0181b8b0,
 	.name		= "Davicom DM9161B/C",
@@ -168,7 +167,6 @@ static struct phy_driver dm91xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= dm9161_ack_interrupt,
 	.config_intr	= dm9161_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x0181b8a0,
 	.name		= "Davicom DM9161A",
@@ -180,7 +178,6 @@ static struct phy_driver dm91xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= dm9161_ack_interrupt,
 	.config_intr	= dm9161_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x00181b80,
 	.name		= "Davicom DM9131",
@@ -191,7 +188,6 @@ static struct phy_driver dm91xx_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= dm9161_ack_interrupt,
 	.config_intr	= dm9161_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(dm91xx_driver);
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index 47b711739ba9..7a240fce3a7e 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -220,9 +220,10 @@ static void rx_timestamp_work(struct work_struct *work);
 
 #define BROADCAST_ADDR 31
 
-static inline int broadcast_write(struct mii_bus *bus, u32 regnum, u16 val)
+static inline int broadcast_write(struct phy_device *phydev, u32 regnum,
+				  u16 val)
 {
-	return mdiobus_write(bus, BROADCAST_ADDR, regnum, val);
+	return mdiobus_write(phydev->mdio.bus, BROADCAST_ADDR, regnum, val);
 }
 
 /* Caller must hold extreg_lock. */
@@ -232,7 +233,7 @@ static int ext_read(struct phy_device *phydev, int page, u32 regnum)
 	int val;
 
 	if (dp83640->clock->page != page) {
-		broadcast_write(phydev->bus, PAGESEL, page);
+		broadcast_write(phydev, PAGESEL, page);
 		dp83640->clock->page = page;
 	}
 	val = phy_read(phydev, regnum);
@@ -247,11 +248,11 @@ static void ext_write(int broadcast, struct phy_device *phydev,
 	struct dp83640_private *dp83640 = phydev->priv;
 
 	if (dp83640->clock->page != page) {
-		broadcast_write(phydev->bus, PAGESEL, page);
+		broadcast_write(phydev, PAGESEL, page);
 		dp83640->clock->page = page;
 	}
 	if (broadcast)
-		broadcast_write(phydev->bus, regnum, val);
+		broadcast_write(phydev, regnum, val);
 	else
 		phy_write(phydev, regnum, val);
 }
@@ -845,6 +846,11 @@ static void decode_rxts(struct dp83640_private *dp83640,
 	struct skb_shared_hwtstamps *shhwtstamps = NULL;
 	struct sk_buff *skb;
 	unsigned long flags;
+	u8 overflow;
+
+	overflow = (phy_rxts->ns_hi >> 14) & 0x3;
+	if (overflow)
+		pr_debug("rx timestamp queue overflow, count %d\n", overflow);
 
 	spin_lock_irqsave(&dp83640->rx_lock, flags);
 
@@ -887,6 +893,7 @@ static void decode_txts(struct dp83640_private *dp83640,
 	struct skb_shared_hwtstamps shhwtstamps;
 	struct sk_buff *skb;
 	u64 ns;
+	u8 overflow;
 
 	/* We must already have the skb that triggered this. */
 
@@ -896,6 +903,17 @@ static void decode_txts(struct dp83640_private *dp83640,
 		pr_debug("have timestamp but tx_queue empty\n");
 		return;
 	}
+
+	overflow = (phy_txts->ns_hi >> 14) & 0x3;
+	if (overflow) {
+		pr_debug("tx timestamp queue overflow, count %d\n", overflow);
+		while (skb) {
+			skb_complete_tx_timestamp(skb, NULL);
+			skb = skb_dequeue(&dp83640->tx_queue);
+		}
+		return;
+	}
+
 	ns = phy2txts(phy_txts);
 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
@@ -1039,7 +1057,7 @@ static int choose_this_phy(struct dp83640_clock *clock,
 	if (chosen_phy == -1 && !clock->chosen)
 		return 1;
 
-	if (chosen_phy == phydev->addr)
+	if (chosen_phy == phydev->mdio.addr)
 		return 1;
 
 	return 0;
@@ -1103,10 +1121,10 @@ static int dp83640_probe(struct phy_device *phydev)
 	struct dp83640_private *dp83640;
 	int err = -ENOMEM, i;
 
-	if (phydev->addr == BROADCAST_ADDR)
+	if (phydev->mdio.addr == BROADCAST_ADDR)
 		return 0;
 
-	clock = dp83640_clock_get_bus(phydev->bus);
+	clock = dp83640_clock_get_bus(phydev->mdio.bus);
 	if (!clock)
 		goto no_clock;
 
@@ -1132,7 +1150,8 @@ static int dp83640_probe(struct phy_device *phydev)
 
 	if (choose_this_phy(clock, phydev)) {
 		clock->chosen = dp83640;
-		clock->ptp_clock = ptp_clock_register(&clock->caps, &phydev->dev);
+		clock->ptp_clock = ptp_clock_register(&clock->caps,
+						      &phydev->mdio.dev);
 		if (IS_ERR(clock->ptp_clock)) {
 			err = PTR_ERR(clock->ptp_clock);
 			goto no_register;
@@ -1158,7 +1177,7 @@ static void dp83640_remove(struct phy_device *phydev)
 	struct list_head *this, *next;
 	struct dp83640_private *tmp, *dp83640 = phydev->priv;
 
-	if (phydev->addr == BROADCAST_ADDR)
+	if (phydev->mdio.addr == BROADCAST_ADDR)
 		return;
 
 	enable_status_frames(phydev, false);
@@ -1490,12 +1509,11 @@ static struct phy_driver dp83640_driver = {
 	.hwtstamp	= dp83640_hwtstamp,
 	.rxtstamp	= dp83640_rxtstamp,
 	.txtstamp	= dp83640_txtstamp,
-	.driver		= {.owner = THIS_MODULE,}
 };
 
 static int __init dp83640_init(void)
 {
-	return phy_driver_register(&dp83640_driver);
+	return phy_driver_register(&dp83640_driver, THIS_MODULE);
 }
 
 static void __exit dp83640_exit(void)
diff --git a/drivers/net/phy/dp83848.c b/drivers/net/phy/dp83848.c
index 5ce9bef54468..5e14e629c597 100644
--- a/drivers/net/phy/dp83848.c
+++ b/drivers/net/phy/dp83848.c
@@ -88,8 +88,6 @@ static struct phy_driver dp83848_driver[] = {
 		/* IRQ related */
 		.ack_interrupt	= dp83848_ack_interrupt,
 		.config_intr	= dp83848_config_intr,
-
-		.driver		= { .owner = THIS_MODULE, },
 	},
 };
 module_phy_driver(dp83848_driver);
diff --git a/drivers/net/phy/dp83867.c b/drivers/net/phy/dp83867.c
index 32f10662f4ac..2afa61b51d41 100644
--- a/drivers/net/phy/dp83867.c
+++ b/drivers/net/phy/dp83867.c
@@ -103,14 +103,11 @@ static int dp83867_config_intr(struct phy_device *phydev)
 static int dp83867_of_init(struct phy_device *phydev)
 {
 	struct dp83867_private *dp83867 = phydev->priv;
-	struct device *dev = &phydev->dev;
+	struct device *dev = &phydev->mdio.dev;
 	struct device_node *of_node = dev->of_node;
 	int ret;
 
-	if (!of_node && dev->parent->of_node)
-		of_node = dev->parent->of_node;
-
-	if (!phydev->dev.of_node)
+	if (!of_node)
 		return -ENODEV;
 
 	ret = of_property_read_u32(of_node, "ti,rx-internal-delay",
@@ -140,7 +137,7 @@ static int dp83867_config_init(struct phy_device *phydev)
 	u16 val, delay;
 
 	if (!phydev->priv) {
-		dp83867 = devm_kzalloc(&phydev->dev, sizeof(*dp83867),
+		dp83867 = devm_kzalloc(&phydev->mdio.dev, sizeof(*dp83867),
 				       GFP_KERNEL);
 		if (!dp83867)
 			return -ENOMEM;
@@ -163,7 +160,7 @@ static int dp83867_config_init(struct phy_device *phydev)
 	if ((phydev->interface >= PHY_INTERFACE_MODE_RGMII_ID) &&
 	    (phydev->interface <= PHY_INTERFACE_MODE_RGMII_RXID)) {
 		val = phy_read_mmd_indirect(phydev, DP83867_RGMIICTL,
-					    DP83867_DEVADDR, phydev->addr);
+					    DP83867_DEVADDR);
 
 		if (phydev->interface == PHY_INTERFACE_MODE_RGMII_ID)
 			val |= (DP83867_RGMII_TX_CLK_DELAY_EN | DP83867_RGMII_RX_CLK_DELAY_EN);
@@ -175,13 +172,13 @@ static int dp83867_config_init(struct phy_device *phydev)
 			val |= DP83867_RGMII_RX_CLK_DELAY_EN;
 
 		phy_write_mmd_indirect(phydev, DP83867_RGMIICTL,
-				       DP83867_DEVADDR, phydev->addr, val);
+				       DP83867_DEVADDR, val);
 
 		delay = (dp83867->rx_id_delay |
 			(dp83867->tx_id_delay << DP83867_RGMII_TX_CLK_DELAY_SHIFT));
 
 		phy_write_mmd_indirect(phydev, DP83867_RGMIIDCTL,
-				       DP83867_DEVADDR, phydev->addr, delay);
+				       DP83867_DEVADDR, delay);
 	}
 
 	return 0;
@@ -217,8 +214,6 @@ static struct phy_driver dp83867_driver[] = {
 		.read_status	= genphy_read_status,
 		.suspend	= genphy_suspend,
 		.resume		= genphy_resume,
-
-		.driver		= {.owner = THIS_MODULE,}
 	},
 };
 module_phy_driver(dp83867_driver);
diff --git a/drivers/net/phy/et1011c.c b/drivers/net/phy/et1011c.c
index a907743816a8..a9a4edfa23c8 100644
--- a/drivers/net/phy/et1011c.c
+++ b/drivers/net/phy/et1011c.c
@@ -95,7 +95,6 @@ static struct phy_driver et1011c_driver[] = { {
 	.flags		= PHY_POLL,
 	.config_aneg	= et1011c_config_aneg,
 	.read_status	= et1011c_read_status,
-	.driver 	= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(et1011c_driver);
diff --git a/drivers/net/phy/fixed_phy.c b/drivers/net/phy/fixed_phy.c
index e23bf5b90e17..ab9c473d75ea 100644
--- a/drivers/net/phy/fixed_phy.c
+++ b/drivers/net/phy/fixed_phy.c
@@ -27,7 +27,6 @@
 #define MII_REGS_NUM 29
 
 struct fixed_mdio_bus {
-	int irqs[PHY_MAX_ADDR];
 	struct mii_bus *mii_bus;
 	struct list_head phys;
 };
@@ -198,11 +197,11 @@ int fixed_phy_set_link_update(struct phy_device *phydev,
 	struct fixed_mdio_bus *fmb = &platform_fmb;
 	struct fixed_phy *fp;
 
-	if (!phydev || !phydev->bus)
+	if (!phydev || !phydev->mdio.bus)
 		return -EINVAL;
 
 	list_for_each_entry(fp, &fmb->phys, node) {
-		if (fp->addr == phydev->addr) {
+		if (fp->addr == phydev->mdio.addr) {
 			fp->link_update = link_update;
 			fp->phydev = phydev;
 			return 0;
@@ -220,11 +219,11 @@ int fixed_phy_update_state(struct phy_device *phydev,
 	struct fixed_mdio_bus *fmb = &platform_fmb;
 	struct fixed_phy *fp;
 
-	if (!phydev || phydev->bus != fmb->mii_bus)
+	if (!phydev || phydev->mdio.bus != fmb->mii_bus)
 		return -EINVAL;
 
 	list_for_each_entry(fp, &fmb->phys, node) {
-		if (fp->addr == phydev->addr) {
+		if (fp->addr == phydev->mdio.addr) {
 #define _UPD(x) if (changed->x) \
 	fp->status.x = status->x
 			_UPD(link);
@@ -256,7 +255,7 @@ int fixed_phy_add(unsigned int irq, int phy_addr,
 
 	memset(fp->regs, 0xFF,  sizeof(fp->regs[0]) * MII_REGS_NUM);
 
-	fmb->irqs[phy_addr] = irq;
+	fmb->mii_bus->irq[phy_addr] = irq;
 
 	fp->addr = phy_addr;
 	fp->status = *status;
@@ -345,7 +344,7 @@ struct phy_device *fixed_phy_register(unsigned int irq,
 	}
 
 	of_node_get(np);
-	phy->dev.of_node = np;
+	phy->mdio.dev.of_node = np;
 	phy->is_pseudo_fixed_link = true;
 
 	switch (status->speed) {
@@ -395,7 +394,6 @@ static int __init fixed_mdio_bus_init(void)
 	fmb->mii_bus->parent = &pdev->dev;
 	fmb->mii_bus->read = &fixed_mdio_read;
 	fmb->mii_bus->write = &fixed_mdio_write;
-	fmb->mii_bus->irq = fmb->irqs;
 
 	ret = mdiobus_register(fmb->mii_bus);
 	if (ret)
diff --git a/drivers/net/phy/icplus.c b/drivers/net/phy/icplus.c
index 0dbc445a5fa0..e5f251b91578 100644
--- a/drivers/net/phy/icplus.c
+++ b/drivers/net/phy/icplus.c
@@ -53,43 +53,43 @@ static int ip175c_config_init(struct phy_device *phydev)
 	if (full_reset_performed == 0) {
 
 		/* master reset */
-		err = mdiobus_write(phydev->bus, 30, 0, 0x175c);
+		err = mdiobus_write(phydev->mdio.bus, 30, 0, 0x175c);
 		if (err < 0)
 			return err;
 
 		/* ensure no bus delays overlap reset period */
-		err = mdiobus_read(phydev->bus, 30, 0);
+		err = mdiobus_read(phydev->mdio.bus, 30, 0);
 
 		/* data sheet specifies reset period is 2 msec */
 		mdelay(2);
 
 		/* enable IP175C mode */
-		err = mdiobus_write(phydev->bus, 29, 31, 0x175c);
+		err = mdiobus_write(phydev->mdio.bus, 29, 31, 0x175c);
 		if (err < 0)
 			return err;
 
 		/* Set MII0 speed and duplex (in PHY mode) */
-		err = mdiobus_write(phydev->bus, 29, 22, 0x420);
+		err = mdiobus_write(phydev->mdio.bus, 29, 22, 0x420);
 		if (err < 0)
 			return err;
 
 		/* reset switch ports */
 		for (i = 0; i < 5; i++) {
-			err = mdiobus_write(phydev->bus, i,
+			err = mdiobus_write(phydev->mdio.bus, i,
 					    MII_BMCR, BMCR_RESET);
 			if (err < 0)
 				return err;
 		}
 
 		for (i = 0; i < 5; i++)
-			err = mdiobus_read(phydev->bus, i, MII_BMCR);
+			err = mdiobus_read(phydev->mdio.bus, i, MII_BMCR);
 
 		mdelay(2);
 
 		full_reset_performed = 1;
 	}
 
-	if (phydev->addr != 4) {
+	if (phydev->mdio.addr != 4) {
 		phydev->state = PHY_RUNNING;
 		phydev->speed = SPEED_100;
 		phydev->duplex = DUPLEX_FULL;
@@ -184,7 +184,7 @@ static int ip101a_g_config_init(struct phy_device *phydev)
 
 static int ip175c_read_status(struct phy_device *phydev)
 {
-	if (phydev->addr == 4) /* WAN port */
+	if (phydev->mdio.addr == 4) /* WAN port */
 		genphy_read_status(phydev);
 	else
 		/* Don't need to read status for switch ports */
@@ -195,7 +195,7 @@ static int ip175c_read_status(struct phy_device *phydev)
 
 static int ip175c_config_aneg(struct phy_device *phydev)
 {
-	if (phydev->addr == 4) /* WAN port */
+	if (phydev->mdio.addr == 4) /* WAN port */
 		genphy_config_aneg(phydev);
 
 	return 0;
@@ -221,7 +221,6 @@ static struct phy_driver icplus_driver[] = {
 	.read_status	= &ip175c_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x02430d90,
 	.name		= "ICPlus IP1001",
@@ -233,7 +232,6 @@ static struct phy_driver icplus_driver[] = {
 	.read_status	= &genphy_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x02430c54,
 	.name		= "ICPlus IP101A/G",
@@ -247,7 +245,6 @@ static struct phy_driver icplus_driver[] = {
 	.read_status	= &genphy_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(icplus_driver);
diff --git a/drivers/net/phy/lxt.c b/drivers/net/phy/lxt.c
index a3a5a703635b..f6078376ef50 100644
--- a/drivers/net/phy/lxt.c
+++ b/drivers/net/phy/lxt.c
@@ -278,7 +278,6 @@ static struct phy_driver lxt97x_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= lxt970_ack_interrupt,
 	.config_intr	= lxt970_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x001378e0,
 	.name		= "LXT971",
@@ -289,7 +288,6 @@ static struct phy_driver lxt97x_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= lxt971_ack_interrupt,
 	.config_intr	= lxt971_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x00137a10,
 	.name		= "LXT973-A2",
@@ -299,7 +297,6 @@ static struct phy_driver lxt97x_driver[] = {
 	.probe		= lxt973_probe,
 	.config_aneg	= lxt973_config_aneg,
 	.read_status	= lxt973a2_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= 0x00137a10,
 	.name		= "LXT973",
@@ -309,7 +306,6 @@ static struct phy_driver lxt97x_driver[] = {
 	.probe		= lxt973_probe,
 	.config_aneg	= lxt973_config_aneg,
 	.read_status	= genphy_read_status,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(lxt97x_driver);
diff --git a/drivers/net/phy/marvell.c b/drivers/net/phy/marvell.c
index 0240552b50f3..ab1d0fcaf1d9 100644
--- a/drivers/net/phy/marvell.c
+++ b/drivers/net/phy/marvell.c
@@ -137,6 +137,22 @@ MODULE_DESCRIPTION("Marvell PHY driver");
 MODULE_AUTHOR("Andy Fleming");
 MODULE_LICENSE("GPL");
 
+struct marvell_hw_stat {
+	const char *string;
+	u8 page;
+	u8 reg;
+	u8 bits;
+};
+
+static struct marvell_hw_stat marvell_hw_stats[] = {
+	{ "phy_receive_errors", 0, 21, 16},
+	{ "phy_idle_errors", 0, 10, 8 },
+};
+
+struct marvell_priv {
+	u64 stats[ARRAY_SIZE(marvell_hw_stats)];
+};
+
 static int marvell_ack_interrupt(struct phy_device *phydev)
 {
 	int err;
@@ -284,10 +300,11 @@ static int marvell_of_reg_init(struct phy_device *phydev)
 	const __be32 *paddr;
 	int len, i, saved_page, current_page, page_changed, ret;
 
-	if (!phydev->dev.of_node)
+	if (!phydev->mdio.dev.of_node)
 		return 0;
 
-	paddr = of_get_property(phydev->dev.of_node, "marvell,reg-init", &len);
+	paddr = of_get_property(phydev->mdio.dev.of_node,
+				"marvell,reg-init", &len);
 	if (!paddr || len < (4 * sizeof(*paddr)))
 		return 0;
 
@@ -429,6 +446,12 @@ static int m88e1510_config_aneg(struct phy_device *phydev)
 	if (err < 0)
 		return err;
 
+	return 0;
+}
+
+static int marvell_config_init(struct phy_device *phydev)
+{
+	/* Set registers from marvell,reg-init DT property */
 	return marvell_of_reg_init(phydev);
 }
 
@@ -478,7 +501,7 @@ static int m88e1116r_config_init(struct phy_device *phydev)
 
 	mdelay(500);
 
-	return 0;
+	return marvell_config_init(phydev);
 }
 
 static int m88e3016_config_init(struct phy_device *phydev)
@@ -497,7 +520,7 @@ static int m88e3016_config_init(struct phy_device *phydev)
 	if (reg < 0)
 		return reg;
 
-	return 0;
+	return marvell_config_init(phydev);
 }
 
 static int m88e1111_config_init(struct phy_device *phydev)
@@ -986,20 +1009,91 @@ static int m88e1318_set_wol(struct phy_device *phydev, struct ethtool_wolinfo *w
 	return 0;
 }
 
+static int marvell_get_sset_count(struct phy_device *phydev)
+{
+	return ARRAY_SIZE(marvell_hw_stats);
+}
+
+static void marvell_get_strings(struct phy_device *phydev, u8 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(marvell_hw_stats); i++) {
+		memcpy(data + i * ETH_GSTRING_LEN,
+		       marvell_hw_stats[i].string, ETH_GSTRING_LEN);
+	}
+}
+
+#ifndef UINT64_MAX
+#define UINT64_MAX              (u64)(~((u64)0))
+#endif
+static u64 marvell_get_stat(struct phy_device *phydev, int i)
+{
+	struct marvell_hw_stat stat = marvell_hw_stats[i];
+	struct marvell_priv *priv = phydev->priv;
+	int err, oldpage;
+	u64 val;
+
+	oldpage = phy_read(phydev, MII_MARVELL_PHY_PAGE);
+	err = phy_write(phydev, MII_MARVELL_PHY_PAGE,
+			stat.page);
+	if (err < 0)
+		return UINT64_MAX;
+
+	val = phy_read(phydev, stat.reg);
+	if (val < 0) {
+		val = UINT64_MAX;
+	} else {
+		val = val & ((1 << stat.bits) - 1);
+		priv->stats[i] += val;
+		val = priv->stats[i];
+	}
+
+	phy_write(phydev, MII_MARVELL_PHY_PAGE, oldpage);
+
+	return val;
+}
+
+static void marvell_get_stats(struct phy_device *phydev,
+			      struct ethtool_stats *stats, u64 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(marvell_hw_stats); i++)
+		data[i] = marvell_get_stat(phydev, i);
+}
+
+static int marvell_probe(struct phy_device *phydev)
+{
+	struct marvell_priv *priv;
+
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	phydev->priv = priv;
+
+	return 0;
+}
+
 static struct phy_driver marvell_drivers[] = {
 	{
 		.phy_id = MARVELL_PHY_ID_88E1101,
 		.phy_id_mask = MARVELL_PHY_ID_MASK,
 		.name = "Marvell 88E1101",
 		.features = PHY_GBIT_FEATURES,
+		.probe = marvell_probe,
 		.flags = PHY_HAS_INTERRUPT,
+		.config_init = &marvell_config_init,
 		.config_aneg = &marvell_config_aneg,
 		.read_status = &genphy_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1112,
@@ -1007,6 +1101,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1112",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1111_config_init,
 		.config_aneg = &marvell_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1014,7 +1109,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1111,
@@ -1022,6 +1119,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1111",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1111_config_init,
 		.config_aneg = &marvell_config_aneg,
 		.read_status = &marvell_read_status,
@@ -1029,7 +1127,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1118,
@@ -1037,6 +1137,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1118",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1118_config_init,
 		.config_aneg = &m88e1118_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1044,7 +1145,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = {.owner = THIS_MODULE,},
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1121R,
@@ -1052,6 +1155,8 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1121R",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
+		.config_init = &marvell_config_init,
 		.config_aneg = &m88e1121_config_aneg,
 		.read_status = &marvell_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
@@ -1059,7 +1164,9 @@ static struct phy_driver marvell_drivers[] = {
 		.did_interrupt = &m88e1121_did_interrupt,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1318S,
@@ -1067,6 +1174,8 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1318S",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
+		.config_init = &marvell_config_init,
 		.config_aneg = &m88e1318_config_aneg,
 		.read_status = &marvell_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
@@ -1076,7 +1185,9 @@ static struct phy_driver marvell_drivers[] = {
 		.set_wol = &m88e1318_set_wol,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1145,
@@ -1084,6 +1195,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1145",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1145_config_init,
 		.config_aneg = &marvell_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1091,7 +1203,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1149R,
@@ -1099,6 +1213,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1149R",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1149_config_init,
 		.config_aneg = &m88e1118_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1106,7 +1221,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1240,
@@ -1114,6 +1231,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1240",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1111_config_init,
 		.config_aneg = &marvell_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1121,7 +1239,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1116R,
@@ -1129,6 +1249,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1116R",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_init = &m88e1116r_config_init,
 		.config_aneg = &genphy_config_aneg,
 		.read_status = &genphy_read_status,
@@ -1136,7 +1257,9 @@ static struct phy_driver marvell_drivers[] = {
 		.config_intr = &marvell_config_intr,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1510,
@@ -1144,6 +1267,8 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1510",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
+		.config_init = &marvell_config_init,
 		.config_aneg = &m88e1510_config_aneg,
 		.read_status = &marvell_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
@@ -1151,7 +1276,9 @@ static struct phy_driver marvell_drivers[] = {
 		.did_interrupt = &m88e1121_did_interrupt,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E1540,
@@ -1159,6 +1286,8 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E1540",
 		.features = PHY_GBIT_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
+		.config_init = &marvell_config_init,
 		.config_aneg = &m88e1510_config_aneg,
 		.read_status = &marvell_read_status,
 		.ack_interrupt = &marvell_ack_interrupt,
@@ -1166,7 +1295,9 @@ static struct phy_driver marvell_drivers[] = {
 		.did_interrupt = &m88e1121_did_interrupt,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 	{
 		.phy_id = MARVELL_PHY_ID_88E3016,
@@ -1174,6 +1305,7 @@ static struct phy_driver marvell_drivers[] = {
 		.name = "Marvell 88E3016",
 		.features = PHY_BASIC_FEATURES,
 		.flags = PHY_HAS_INTERRUPT,
+		.probe = marvell_probe,
 		.config_aneg = &genphy_config_aneg,
 		.config_init = &m88e3016_config_init,
 		.aneg_done = &marvell_aneg_done,
@@ -1183,7 +1315,9 @@ static struct phy_driver marvell_drivers[] = {
 		.did_interrupt = &m88e1121_did_interrupt,
 		.resume = &genphy_resume,
 		.suspend = &genphy_suspend,
-		.driver = { .owner = THIS_MODULE },
+		.get_sset_count = marvell_get_sset_count,
+		.get_strings = marvell_get_strings,
+		.get_stats = marvell_get_stats,
 	},
 };
 
diff --git a/drivers/net/phy/mdio-bcm-unimac.c b/drivers/net/phy/mdio-bcm-unimac.c
index 4bde5e728fe0..8c73b2e771dd 100644
--- a/drivers/net/phy/mdio-bcm-unimac.c
+++ b/drivers/net/phy/mdio-bcm-unimac.c
@@ -200,16 +200,10 @@ static int unimac_mdio_probe(struct platform_device *pdev)
 	bus->reset = unimac_mdio_reset;
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s", pdev->name);
 
-	bus->irq = kcalloc(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
-	if (!bus->irq) {
-		ret = -ENOMEM;
-		goto out_mdio_free;
-	}
-
 	ret = of_mdiobus_register(bus, np);
 	if (ret) {
 		dev_err(&pdev->dev, "MDIO bus registration failed\n");
-		goto out_mdio_irq;
+		goto out_mdio_free;
 	}
 
 	platform_set_drvdata(pdev, priv);
@@ -218,8 +212,6 @@ static int unimac_mdio_probe(struct platform_device *pdev)
 
 	return 0;
 
-out_mdio_irq:
-	kfree(bus->irq);
 out_mdio_free:
 	mdiobus_free(bus);
 	return ret;
@@ -230,7 +222,6 @@ static int unimac_mdio_remove(struct platform_device *pdev)
 	struct unimac_mdio_priv *priv = platform_get_drvdata(pdev);
 
 	mdiobus_unregister(priv->mii_bus);
-	kfree(priv->mii_bus->irq);
 	mdiobus_free(priv->mii_bus);
 
 	return 0;
diff --git a/drivers/net/phy/mdio-gpio.c b/drivers/net/phy/mdio-gpio.c
index 95f51d7267b3..27ab63064f95 100644
--- a/drivers/net/phy/mdio-gpio.c
+++ b/drivers/net/phy/mdio-gpio.c
@@ -159,7 +159,7 @@ static struct mii_bus *mdio_gpio_bus_init(struct device *dev,
 
 	new_bus->phy_mask = pdata->phy_mask;
 	new_bus->phy_ignore_ta_mask = pdata->phy_ignore_ta_mask;
-	new_bus->irq = pdata->irqs;
+	memcpy(new_bus->irq, pdata->irqs, sizeof(new_bus->irq));
 	new_bus->parent = dev;
 
 	if (new_bus->phy_mask == ~0)
diff --git a/drivers/net/phy/mdio-moxart.c b/drivers/net/phy/mdio-moxart.c
index f1fc51f655d9..5bb56d126693 100644
--- a/drivers/net/phy/mdio-moxart.c
+++ b/drivers/net/phy/mdio-moxart.c
@@ -130,13 +130,6 @@ static int moxart_mdio_probe(struct platform_device *pdev)
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-%d-mii", pdev->name, pdev->id);
 	bus->parent = &pdev->dev;
 
-	bus->irq = devm_kzalloc(&pdev->dev, sizeof(int) * PHY_MAX_ADDR,
-			GFP_KERNEL);
-	if (!bus->irq) {
-		ret = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
 	/* Setting PHY_IGNORE_INTERRUPT here even if it has no effect,
 	 * of_mdiobus_register() sets these PHY_POLL.
 	 * Ideally, the interrupt from MAC controller could be used to
diff --git a/drivers/net/phy/mdio-mux.c b/drivers/net/phy/mdio-mux.c
index 7f8e7662e28c..308ade0eb1b6 100644
--- a/drivers/net/phy/mdio-mux.c
+++ b/drivers/net/phy/mdio-mux.c
@@ -34,7 +34,6 @@ struct mdio_mux_child_bus {
 	struct mdio_mux_parent_bus *parent;
 	struct mdio_mux_child_bus *next;
 	int bus_number;
-	int phy_irq[PHY_MAX_ADDR];
 };
 
 /*
@@ -157,7 +156,7 @@ int mdio_mux_init(struct device *dev,
 			break;
 		}
 		cb->mii_bus->priv = cb;
-		cb->mii_bus->irq = cb->phy_irq;
+
 		cb->mii_bus->name = "mdio_mux";
 		snprintf(cb->mii_bus->id, MII_BUS_ID_SIZE, "%x.%x",
 			 pb->parent_id, v);
diff --git a/drivers/net/phy/mdio-octeon.c b/drivers/net/phy/mdio-octeon.c
index fcf4e4df7cc8..47d4f2f263d1 100644
--- a/drivers/net/phy/mdio-octeon.c
+++ b/drivers/net/phy/mdio-octeon.c
@@ -113,7 +113,6 @@ struct octeon_mdiobus {
 	resource_size_t mdio_phys;
 	resource_size_t regsize;
 	enum octeon_mdiobus_mode mode;
-	int phy_irq[PHY_MAX_ADDR];
 };
 
 #ifdef CONFIG_CAVIUM_OCTEON_SOC
@@ -268,12 +267,13 @@ static int octeon_mdiobus_write(struct mii_bus *bus, int phy_id,
 static int octeon_mdiobus_probe(struct platform_device *pdev)
 {
 	struct octeon_mdiobus *bus;
+	struct mii_bus *mii_bus;
 	struct resource *res_mem;
 	union cvmx_smix_en smi_en;
 	int err = -ENOENT;
 
-	bus = devm_kzalloc(&pdev->dev, sizeof(*bus), GFP_KERNEL);
-	if (!bus)
+	mii_bus = devm_mdiobus_alloc_size(&pdev->dev, sizeof(*bus));
+	if (!mii_bus)
 		return -ENOMEM;
 
 	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -282,6 +282,8 @@ static int octeon_mdiobus_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
+	bus = mii_bus->priv;
+	bus->mii_bus = mii_bus;
 	bus->mdio_phys = res_mem->start;
 	bus->regsize = resource_size(res_mem);
 
@@ -298,16 +300,11 @@ static int octeon_mdiobus_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	bus->mii_bus = mdiobus_alloc();
-	if (!bus->mii_bus)
-		goto fail;
-
 	smi_en.u64 = 0;
 	smi_en.s.en = 1;
 	oct_mdio_writeq(smi_en.u64, bus->register_base + SMI_EN);
 
 	bus->mii_bus->priv = bus;
-	bus->mii_bus->irq = bus->phy_irq;
 	bus->mii_bus->name = "mdio-octeon";
 	snprintf(bus->mii_bus->id, MII_BUS_ID_SIZE, "%llx", bus->register_base);
 	bus->mii_bus->parent = &pdev->dev;
@@ -326,7 +323,6 @@ static int octeon_mdiobus_probe(struct platform_device *pdev)
 	return 0;
 fail_register:
 	mdiobus_free(bus->mii_bus);
-fail:
 	smi_en.u64 = 0;
 	oct_mdio_writeq(smi_en.u64, bus->register_base + SMI_EN);
 	return err;
diff --git a/drivers/net/phy/mdio-sun4i.c b/drivers/net/phy/mdio-sun4i.c
index 15bc7f9ea224..f70522c35163 100644
--- a/drivers/net/phy/mdio-sun4i.c
+++ b/drivers/net/phy/mdio-sun4i.c
@@ -96,7 +96,7 @@ static int sun4i_mdio_probe(struct platform_device *pdev)
 	struct mii_bus *bus;
 	struct sun4i_mdio_data *data;
 	struct resource *res;
-	int ret, i;
+	int ret;
 
 	bus = mdiobus_alloc_size(sizeof(*data));
 	if (!bus)
@@ -108,16 +108,6 @@ static int sun4i_mdio_probe(struct platform_device *pdev)
 	snprintf(bus->id, MII_BUS_ID_SIZE, "%s-mii", dev_name(&pdev->dev));
 	bus->parent = &pdev->dev;
 
-	bus->irq = devm_kzalloc(&pdev->dev, sizeof(int) * PHY_MAX_ADDR,
-			GFP_KERNEL);
-	if (!bus->irq) {
-		ret = -ENOMEM;
-		goto err_out_free_mdiobus;
-	}
-
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		bus->irq[i] = PHY_POLL;
-
 	data = bus->priv;
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	data->membase = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 88cb4592b6fb..0cba64f1ecf4 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -38,6 +38,48 @@
 
 #include <asm/irq.h>
 
+int mdiobus_register_device(struct mdio_device *mdiodev)
+{
+	if (mdiodev->bus->mdio_map[mdiodev->addr])
+		return -EBUSY;
+
+	mdiodev->bus->mdio_map[mdiodev->addr] = mdiodev;
+
+	return 0;
+}
+EXPORT_SYMBOL(mdiobus_register_device);
+
+int mdiobus_unregister_device(struct mdio_device *mdiodev)
+{
+	if (mdiodev->bus->mdio_map[mdiodev->addr] != mdiodev)
+		return -EINVAL;
+
+	mdiodev->bus->mdio_map[mdiodev->addr] = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL(mdiobus_unregister_device);
+
+struct phy_device *mdiobus_get_phy(struct mii_bus *bus, int addr)
+{
+	struct mdio_device *mdiodev = bus->mdio_map[addr];
+
+	if (!mdiodev)
+		return NULL;
+
+	if (!(mdiodev->flags & MDIO_DEVICE_FLAG_PHY))
+		return NULL;
+
+	return container_of(mdiodev, struct phy_device, mdio);
+}
+EXPORT_SYMBOL(mdiobus_get_phy);
+
+bool mdiobus_is_registered_device(struct mii_bus *bus, int addr)
+{
+	return bus->mdio_map[addr];
+}
+EXPORT_SYMBOL(mdiobus_is_registered_device);
+
 /**
  * mdiobus_alloc_size - allocate a mii_bus structure
  * @size: extra amount of memory to allocate for private storage.
@@ -51,6 +93,7 @@ struct mii_bus *mdiobus_alloc_size(size_t size)
 	struct mii_bus *bus;
 	size_t aligned_size = ALIGN(sizeof(*bus), NETDEV_ALIGN);
 	size_t alloc_size;
+	int i;
 
 	/* If we alloc extra space, it should be aligned */
 	if (size)
@@ -59,11 +102,16 @@ struct mii_bus *mdiobus_alloc_size(size_t size)
 		alloc_size = sizeof(*bus);
 
 	bus = kzalloc(alloc_size, GFP_KERNEL);
-	if (bus) {
-		bus->state = MDIOBUS_ALLOCATED;
-		if (size)
-			bus->priv = (void *)bus + aligned_size;
-	}
+	if (!bus)
+		return NULL;
+
+	bus->state = MDIOBUS_ALLOCATED;
+	if (size)
+		bus->priv = (void *)bus + aligned_size;
+
+	/* Initialise the interrupts to polling */
+	for (i = 0; i < PHY_MAX_ADDR; i++)
+		bus->irq[i] = PHY_POLL;
 
 	return bus;
 }
@@ -190,47 +238,48 @@ struct mii_bus *of_mdio_find_bus(struct device_node *mdio_bus_np)
 }
 EXPORT_SYMBOL(of_mdio_find_bus);
 
-/* Walk the list of subnodes of a mdio bus and look for a node that matches the
- * phy's address with its 'reg' property. If found, set the of_node pointer for
- * the phy. This allows auto-probed pyh devices to be supplied with information
- * passed in via DT.
+/* Walk the list of subnodes of a mdio bus and look for a node that
+ * matches the mdio device's address with its 'reg' property. If
+ * found, set the of_node pointer for the mdio device. This allows
+ * auto-probed phy devices to be supplied with information passed in
+ * via DT.
  */
-static void of_mdiobus_link_phydev(struct mii_bus *mdio,
-				   struct phy_device *phydev)
+static void of_mdiobus_link_mdiodev(struct mii_bus *bus,
+				    struct mdio_device *mdiodev)
 {
-	struct device *dev = &phydev->dev;
+	struct device *dev = &mdiodev->dev;
 	struct device_node *child;
 
-	if (dev->of_node || !mdio->dev.of_node)
+	if (dev->of_node || !bus->dev.of_node)
 		return;
 
-	for_each_available_child_of_node(mdio->dev.of_node, child) {
+	for_each_available_child_of_node(bus->dev.of_node, child) {
 		int addr;
 		int ret;
 
 		ret = of_property_read_u32(child, "reg", &addr);
 		if (ret < 0) {
-			dev_err(dev, "%s has invalid PHY address\n",
+			dev_err(dev, "%s has invalid MDIO address\n",
 				child->full_name);
 			continue;
 		}
 
-		/* A PHY must have a reg property in the range [0-31] */
+		/* A MDIO device must have a reg property in the range [0-31] */
 		if (addr >= PHY_MAX_ADDR) {
-			dev_err(dev, "%s PHY address %i is too large\n",
+			dev_err(dev, "%s MDIO address %i is too large\n",
 				child->full_name, addr);
 			continue;
 		}
 
-		if (addr == phydev->addr) {
+		if (addr == mdiodev->addr) {
 			dev->of_node = child;
 			return;
 		}
 	}
 }
 #else /* !IS_ENABLED(CONFIG_OF_MDIO) */
-static inline void of_mdiobus_link_phydev(struct mii_bus *mdio,
-					  struct phy_device *phydev)
+static inline void of_mdiobus_link_mdiodev(struct mii_bus *mdio,
+					   struct mdio_device *mdiodev)
 {
 }
 #endif
@@ -243,12 +292,15 @@ static inline void of_mdiobus_link_phydev(struct mii_bus *mdio,
  * Description: Called by a bus driver to bring up all the PHYs
  *   on a given bus, and attach them to the bus. Drivers should use
  *   mdiobus_register() rather than __mdiobus_register() unless they
- *   need to pass a specific owner module.
+ *   need to pass a specific owner module. MDIO devices which are not
+ *   PHYs will not be brought up by this function. They are expected to
+ *   to be explicitly listed in DT and instantiated by of_mdiobus_register().
  *
  * Returns 0 on success or < 0 on error.
  */
 int __mdiobus_register(struct mii_bus *bus, struct module *owner)
 {
+	struct mdio_device *mdiodev;
 	int i, err;
 
 	if (NULL == bus || NULL == bus->name ||
@@ -294,11 +346,12 @@ int __mdiobus_register(struct mii_bus *bus, struct module *owner)
 
 error:
 	while (--i >= 0) {
-		struct phy_device *phydev = bus->phy_map[i];
-		if (phydev) {
-			phy_device_remove(phydev);
-			phy_device_free(phydev);
-		}
+		mdiodev = bus->mdio_map[i];
+		if (!mdiodev)
+			continue;
+
+		mdiodev->device_remove(mdiodev);
+		mdiodev->device_free(mdiodev);
 	}
 	device_del(&bus->dev);
 	return err;
@@ -307,17 +360,19 @@ EXPORT_SYMBOL(__mdiobus_register);
 
 void mdiobus_unregister(struct mii_bus *bus)
 {
+	struct mdio_device *mdiodev;
 	int i;
 
 	BUG_ON(bus->state != MDIOBUS_REGISTERED);
 	bus->state = MDIOBUS_UNREGISTERED;
 
 	for (i = 0; i < PHY_MAX_ADDR; i++) {
-		struct phy_device *phydev = bus->phy_map[i];
-		if (phydev) {
-			phy_device_remove(phydev);
-			phy_device_free(phydev);
-		}
+		mdiodev = bus->mdio_map[i];
+		if (!mdiodev)
+			continue;
+
+		mdiodev->device_remove(mdiodev);
+		mdiodev->device_free(mdiodev);
 	}
 	device_del(&bus->dev);
 }
@@ -346,6 +401,18 @@ void mdiobus_free(struct mii_bus *bus)
 }
 EXPORT_SYMBOL(mdiobus_free);
 
+/**
+ * mdiobus_scan - scan a bus for MDIO devices.
+ * @bus: mii_bus to scan
+ * @addr: address on bus to scan
+ *
+ * This function scans the MDIO bus, looking for devices which can be
+ * identified using a vendor/product ID in registers 2 and 3. Not all
+ * MDIO devices have such registers, but PHY devices typically
+ * do. Hence this function assumes anything found is a PHY, or can be
+ * treated as a PHY. Other MDIO devices, such as switches, will
+ * probably not be found during the scan.
+ */
 struct phy_device *mdiobus_scan(struct mii_bus *bus, int addr)
 {
 	struct phy_device *phydev;
@@ -359,7 +426,7 @@ struct phy_device *mdiobus_scan(struct mii_bus *bus, int addr)
 	 * For DT, see if the auto-probed phy has a correspoding child
 	 * in the bus node, and set the of_node pointer in this case.
 	 */
-	of_mdiobus_link_phydev(bus, phydev);
+	of_mdiobus_link_mdiodev(bus, &phydev->mdio);
 
 	err = phy_device_register(phydev);
 	if (err) {
@@ -476,133 +543,56 @@ int mdiobus_write(struct mii_bus *bus, int addr, u32 regnum, u16 val)
 EXPORT_SYMBOL(mdiobus_write);
 
 /**
- * mdio_bus_match - determine if given PHY driver supports the given PHY device
- * @dev: target PHY device
- * @drv: given PHY driver
+ * mdio_bus_match - determine if given MDIO driver supports the given
+ *		    MDIO device
+ * @dev: target MDIO device
+ * @drv: given MDIO driver
  *
- * Description: Given a PHY device, and a PHY driver, return 1 if
- *   the driver supports the device.  Otherwise, return 0.
+ * Description: Given a MDIO device, and a MDIO driver, return 1 if
+ *   the driver supports the device.  Otherwise, return 0. This may
+ *   require calling the devices own match function, since different classes
+ *   of MDIO devices have different match criteria.
  */
 static int mdio_bus_match(struct device *dev, struct device_driver *drv)
 {
-	struct phy_device *phydev = to_phy_device(dev);
-	struct phy_driver *phydrv = to_phy_driver(drv);
-	const int num_ids = ARRAY_SIZE(phydev->c45_ids.device_ids);
-	int i;
+	struct mdio_device *mdio = to_mdio_device(dev);
 
 	if (of_driver_match_device(dev, drv))
 		return 1;
 
-	if (phydrv->match_phy_device)
-		return phydrv->match_phy_device(phydev);
-
-	if (phydev->is_c45) {
-		for (i = 1; i < num_ids; i++) {
-			if (!(phydev->c45_ids.devices_in_package & (1 << i)))
-				continue;
+	if (mdio->bus_match)
+		return mdio->bus_match(dev, drv);
 
-			if ((phydrv->phy_id & phydrv->phy_id_mask) ==
-			    (phydev->c45_ids.device_ids[i] &
-			     phydrv->phy_id_mask))
-				return 1;
-		}
-		return 0;
-	} else {
-		return (phydrv->phy_id & phydrv->phy_id_mask) ==
-			(phydev->phy_id & phydrv->phy_id_mask);
-	}
+	return 0;
 }
 
 #ifdef CONFIG_PM
-
-static bool mdio_bus_phy_may_suspend(struct phy_device *phydev)
-{
-	struct device_driver *drv = phydev->dev.driver;
-	struct phy_driver *phydrv = to_phy_driver(drv);
-	struct net_device *netdev = phydev->attached_dev;
-
-	if (!drv || !phydrv->suspend)
-		return false;
-
-	/* PHY not attached? May suspend if the PHY has not already been
-	 * suspended as part of a prior call to phy_disconnect() ->
-	 * phy_detach() -> phy_suspend() because the parent netdev might be the
-	 * MDIO bus driver and clock gated at this point.
-	 */
-	if (!netdev)
-		return !phydev->suspended;
-
-	/* Don't suspend PHY if the attched netdev parent may wakeup.
-	 * The parent may point to a PCI device, as in tg3 driver.
-	 */
-	if (netdev->dev.parent && device_may_wakeup(netdev->dev.parent))
-		return false;
-
-	/* Also don't suspend PHY if the netdev itself may wakeup. This
-	 * is the case for devices w/o underlaying pwr. mgmt. aware bus,
-	 * e.g. SoC devices.
-	 */
-	if (device_may_wakeup(&netdev->dev))
-		return false;
-
-	return true;
-}
-
 static int mdio_bus_suspend(struct device *dev)
 {
-	struct phy_device *phydev = to_phy_device(dev);
+	struct mdio_device *mdio = to_mdio_device(dev);
 
-	/* We must stop the state machine manually, otherwise it stops out of
-	 * control, possibly with the phydev->lock held. Upon resume, netdev
-	 * may call phy routines that try to grab the same lock, and that may
-	 * lead to a deadlock.
-	 */
-	if (phydev->attached_dev && phydev->adjust_link)
-		phy_stop_machine(phydev);
+	if (mdio->pm_ops && mdio->pm_ops->suspend)
+		return mdio->pm_ops->suspend(dev);
 
-	if (!mdio_bus_phy_may_suspend(phydev))
-		return 0;
-
-	return phy_suspend(phydev);
+	return 0;
 }
 
 static int mdio_bus_resume(struct device *dev)
 {
-	struct phy_device *phydev = to_phy_device(dev);
-	int ret;
-
-	if (!mdio_bus_phy_may_suspend(phydev))
-		goto no_resume;
-
-	ret = phy_resume(phydev);
-	if (ret < 0)
-		return ret;
+	struct mdio_device *mdio = to_mdio_device(dev);
 
-no_resume:
-	if (phydev->attached_dev && phydev->adjust_link)
-		phy_start_machine(phydev);
+	if (mdio->pm_ops && mdio->pm_ops->resume)
+		return mdio->pm_ops->resume(dev);
 
 	return 0;
 }
 
 static int mdio_bus_restore(struct device *dev)
 {
-	struct phy_device *phydev = to_phy_device(dev);
-	struct net_device *netdev = phydev->attached_dev;
-	int ret;
-
-	if (!netdev)
-		return 0;
+	struct mdio_device *mdio = to_mdio_device(dev);
 
-	ret = phy_init_hw(phydev);
-	if (ret < 0)
-		return ret;
-
-	/* The PHY needs to renegotiate. */
-	phydev->link = 0;
-	phydev->state = PHY_UP;
-
-	phy_start_machine(phydev);
+	if (mdio->pm_ops && mdio->pm_ops->restore)
+		return mdio->pm_ops->restore(dev);
 
 	return 0;
 }
@@ -623,52 +613,10 @@ static const struct dev_pm_ops mdio_bus_pm_ops = {
 
 #endif /* CONFIG_PM */
 
-static ssize_t
-phy_id_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct phy_device *phydev = to_phy_device(dev);
-
-	return sprintf(buf, "0x%.8lx\n", (unsigned long)phydev->phy_id);
-}
-static DEVICE_ATTR_RO(phy_id);
-
-static ssize_t
-phy_interface_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct phy_device *phydev = to_phy_device(dev);
-	const char *mode = NULL;
-
-	if (phy_is_internal(phydev))
-		mode = "internal";
-	else
-		mode = phy_modes(phydev->interface);
-
-	return sprintf(buf, "%s\n", mode);
-}
-static DEVICE_ATTR_RO(phy_interface);
-
-static ssize_t
-phy_has_fixups_show(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct phy_device *phydev = to_phy_device(dev);
-
-	return sprintf(buf, "%d\n", phydev->has_fixups);
-}
-static DEVICE_ATTR_RO(phy_has_fixups);
-
-static struct attribute *mdio_dev_attrs[] = {
-	&dev_attr_phy_id.attr,
-	&dev_attr_phy_interface.attr,
-	&dev_attr_phy_has_fixups.attr,
-	NULL,
-};
-ATTRIBUTE_GROUPS(mdio_dev);
-
 struct bus_type mdio_bus_type = {
 	.name		= "mdio_bus",
 	.match		= mdio_bus_match,
 	.pm		= MDIO_BUS_PM_OPS,
-	.dev_groups	= mdio_dev_groups,
 };
 EXPORT_SYMBOL(mdio_bus_type);
 
diff --git a/drivers/net/phy/mdio_device.c b/drivers/net/phy/mdio_device.c
new file mode 100644
index 000000000000..9c88e6749b9a
--- /dev/null
+++ b/drivers/net/phy/mdio_device.c
@@ -0,0 +1,171 @@
+/* Framework for MDIO devices, other than PHYs.
+ *
+ * Copyright (c) 2016 Andrew Lunn <andrew@lunn.ch>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mdio.h>
+#include <linux/mii.h>
+#include <linux/module.h>
+#include <linux/phy.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/unistd.h>
+
+void mdio_device_free(struct mdio_device *mdiodev)
+{
+	put_device(&mdiodev->dev);
+}
+EXPORT_SYMBOL(mdio_device_free);
+
+static void mdio_device_release(struct device *dev)
+{
+	kfree(to_mdio_device(dev));
+}
+
+struct mdio_device *mdio_device_create(struct mii_bus *bus, int addr)
+{
+	struct mdio_device *mdiodev;
+
+	/* We allocate the device, and initialize the default values */
+	mdiodev = kzalloc(sizeof(*mdiodev), GFP_KERNEL);
+	if (!mdiodev)
+		return ERR_PTR(-ENOMEM);
+
+	mdiodev->dev.release = mdio_device_release;
+	mdiodev->dev.parent = &bus->dev;
+	mdiodev->dev.bus = &mdio_bus_type;
+	mdiodev->device_free = mdio_device_free;
+	mdiodev->device_remove = mdio_device_remove;
+	mdiodev->bus = bus;
+	mdiodev->addr = addr;
+
+	dev_set_name(&mdiodev->dev, PHY_ID_FMT, bus->id, addr);
+
+	device_initialize(&mdiodev->dev);
+
+	return mdiodev;
+}
+EXPORT_SYMBOL(mdio_device_create);
+
+/**
+ * mdio_device_register - Register the mdio device on the MDIO bus
+ * @mdiodev: mdio_device structure to be added to the MDIO bus
+ */
+int mdio_device_register(struct mdio_device *mdiodev)
+{
+	int err;
+
+	dev_info(&mdiodev->dev, "mdio_device_register\n");
+
+	err = mdiobus_register_device(mdiodev);
+	if (err)
+		return err;
+
+	err = device_add(&mdiodev->dev);
+	if (err) {
+		pr_err("MDIO %d failed to add\n", mdiodev->addr);
+		goto out;
+	}
+
+	return 0;
+
+ out:
+	mdiobus_unregister_device(mdiodev);
+	return err;
+}
+EXPORT_SYMBOL(mdio_device_register);
+
+/**
+ * mdio_device_remove - Remove a previously registered mdio device from the
+ *			MDIO bus
+ * @mdiodev: mdio_device structure to remove
+ *
+ * This doesn't free the mdio_device itself, it merely reverses the effects
+ * of mdio_device_register(). Use mdio_device_free() to free the device
+ * after calling this function.
+ */
+void mdio_device_remove(struct mdio_device *mdiodev)
+{
+	device_del(&mdiodev->dev);
+	mdiobus_unregister_device(mdiodev);
+}
+EXPORT_SYMBOL(mdio_device_remove);
+
+/**
+ * mdio_probe - probe an MDIO device
+ * @dev: device to probe
+ *
+ * Description: Take care of setting up the mdio_device structure
+ * and calling the driver to probe the device.
+ */
+static int mdio_probe(struct device *dev)
+{
+	struct mdio_device *mdiodev = to_mdio_device(dev);
+	struct device_driver *drv = mdiodev->dev.driver;
+	struct mdio_driver *mdiodrv = to_mdio_driver(drv);
+	int err = 0;
+
+	if (mdiodrv->probe)
+		err = mdiodrv->probe(mdiodev);
+
+	return err;
+}
+
+static int mdio_remove(struct device *dev)
+{
+	struct mdio_device *mdiodev = to_mdio_device(dev);
+	struct device_driver *drv = mdiodev->dev.driver;
+	struct mdio_driver *mdiodrv = to_mdio_driver(drv);
+
+	if (mdiodrv->remove)
+		mdiodrv->remove(mdiodev);
+
+	return 0;
+}
+
+/**
+ * mdio_driver_register - register an mdio_driver with the MDIO layer
+ * @new_driver: new mdio_driver to register
+ */
+int mdio_driver_register(struct mdio_driver *drv)
+{
+	struct mdio_driver_common *mdiodrv = &drv->mdiodrv;
+	int retval;
+
+	pr_info("mdio_driver_register: %s\n", mdiodrv->driver.name);
+
+	mdiodrv->driver.bus = &mdio_bus_type;
+	mdiodrv->driver.probe = mdio_probe;
+	mdiodrv->driver.remove = mdio_remove;
+
+	retval = driver_register(&mdiodrv->driver);
+	if (retval) {
+		pr_err("%s: Error %d in registering driver\n",
+		       mdiodrv->driver.name, retval);
+
+		return retval;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(mdio_driver_register);
+
+void mdio_driver_unregister(struct mdio_driver *drv)
+{
+	struct mdio_driver_common *mdiodrv = &drv->mdiodrv;
+
+	driver_unregister(&mdiodrv->driver);
+}
+EXPORT_SYMBOL(mdio_driver_unregister);
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index e13ad6cdcc22..dc85f7095e51 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -73,6 +73,17 @@
 
 #define PS_TO_REG				200
 
+struct kszphy_hw_stat {
+	const char *string;
+	u8 reg;
+	u8 bits;
+};
+
+static struct kszphy_hw_stat kszphy_hw_stats[] = {
+	{ "phy_receive_errors", 21, 16},
+	{ "phy_idle_errors", 10, 8 },
+};
+
 struct kszphy_type {
 	u32 led_mode_reg;
 	u16 interrupt_level_mask;
@@ -86,6 +97,7 @@ struct kszphy_priv {
 	int led_mode;
 	bool rmii_ref_clk_sel;
 	bool rmii_ref_clk_sel_val;
+	u64 stats[ARRAY_SIZE(kszphy_hw_stats)];
 };
 
 static const struct kszphy_type ksz8021_type = {
@@ -212,7 +224,7 @@ static int kszphy_setup_led(struct phy_device *phydev, u32 reg, int val)
 	rc = phy_write(phydev, reg, temp);
 out:
 	if (rc < 0)
-		dev_err(&phydev->dev, "failed to set led mode\n");
+		phydev_err(phydev, "failed to set led mode\n");
 
 	return rc;
 }
@@ -231,7 +243,7 @@ static int kszphy_broadcast_disable(struct phy_device *phydev)
 	ret = phy_write(phydev, MII_KSZPHY_OMSO, ret | KSZPHY_OMSO_B_CAST_OFF);
 out:
 	if (ret)
-		dev_err(&phydev->dev, "failed to disable broadcast address\n");
+		phydev_err(phydev, "failed to disable broadcast address\n");
 
 	return ret;
 }
@@ -251,7 +263,7 @@ static int kszphy_nand_tree_disable(struct phy_device *phydev)
 			ret & ~KSZPHY_OMSO_NAND_TREE_ON);
 out:
 	if (ret)
-		dev_err(&phydev->dev, "failed to disable NAND tree mode\n");
+		phydev_err(phydev, "failed to disable NAND tree mode\n");
 
 	return ret;
 }
@@ -276,7 +288,8 @@ static int kszphy_config_init(struct phy_device *phydev)
 	if (priv->rmii_ref_clk_sel) {
 		ret = kszphy_rmii_clk_sel(phydev, priv->rmii_ref_clk_sel_val);
 		if (ret) {
-			dev_err(&phydev->dev, "failed to set rmii reference clock\n");
+			phydev_err(phydev,
+				   "failed to set rmii reference clock\n");
 			return ret;
 		}
 	}
@@ -284,6 +297,17 @@ static int kszphy_config_init(struct phy_device *phydev)
 	if (priv->led_mode >= 0)
 		kszphy_setup_led(phydev, type->led_mode_reg, priv->led_mode);
 
+	if (phy_interrupt_is_valid(phydev)) {
+		int ctl = phy_read(phydev, MII_BMCR);
+
+		if (ctl < 0)
+			return ctl;
+
+		ret = phy_write(phydev, MII_BMCR, ctl & ~BMCR_ANENABLE);
+		if (ret < 0)
+			return ret;
+	}
+
 	return 0;
 }
 
@@ -337,7 +361,7 @@ static int ksz9021_load_values_from_of(struct phy_device *phydev,
 
 static int ksz9021_config_init(struct phy_device *phydev)
 {
-	const struct device *dev = &phydev->dev;
+	const struct device *dev = &phydev->mdio.dev;
 	const struct device_node *of_node = dev->of_node;
 	const struct device *dev_walker;
 
@@ -345,7 +369,7 @@ static int ksz9021_config_init(struct phy_device *phydev)
 	 * properties in the MAC node. Walk up the tree of devices to
 	 * find a device with an OF node.
 	 */
-	dev_walker = &phydev->dev;
+	dev_walker = &phydev->mdio.dev;
 	do {
 		of_node = dev_walker->of_node;
 		dev_walker = dev_walker->parent;
@@ -458,7 +482,7 @@ static int ksz9031_center_flp_timing(struct phy_device *phydev)
 
 static int ksz9031_config_init(struct phy_device *phydev)
 {
-	const struct device *dev = &phydev->dev;
+	const struct device *dev = &phydev->mdio.dev;
 	const struct device_node *of_node = dev->of_node;
 	static const char *clk_skews[2] = {"rxc-skew-ps", "txc-skew-ps"};
 	static const char *rx_data_skews[4] = {
@@ -470,9 +494,17 @@ static int ksz9031_config_init(struct phy_device *phydev)
 		"txd2-skew-ps", "txd3-skew-ps"
 	};
 	static const char *control_skews[2] = {"txen-skew-ps", "rxdv-skew-ps"};
+	const struct device *dev_walker;
 
-	if (!of_node && dev->parent->of_node)
-		of_node = dev->parent->of_node;
+	/* The Micrel driver has a deprecated option to place phy OF
+	 * properties in the MAC node. Walk up the tree of devices to
+	 * find a device with an OF node.
+	 */
+	dev_walker = &phydev->mdio.dev;
+	do {
+		of_node = dev_walker->of_node;
+		dev_walker = dev_walker->parent;
+	} while (!of_node && dev_walker);
 
 	if (of_node) {
 		ksz9031_of_load_skew_values(phydev, of_node,
@@ -569,15 +601,75 @@ ksz9021_wr_mmd_phyreg(struct phy_device *phydev, int ptrad, int devnum,
 {
 }
 
+static int kszphy_get_sset_count(struct phy_device *phydev)
+{
+	return ARRAY_SIZE(kszphy_hw_stats);
+}
+
+static void kszphy_get_strings(struct phy_device *phydev, u8 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(kszphy_hw_stats); i++) {
+		memcpy(data + i * ETH_GSTRING_LEN,
+		       kszphy_hw_stats[i].string, ETH_GSTRING_LEN);
+	}
+}
+
+#ifndef UINT64_MAX
+#define UINT64_MAX              (u64)(~((u64)0))
+#endif
+static u64 kszphy_get_stat(struct phy_device *phydev, int i)
+{
+	struct kszphy_hw_stat stat = kszphy_hw_stats[i];
+	struct kszphy_priv *priv = phydev->priv;
+	u64 val;
+
+	val = phy_read(phydev, stat.reg);
+	if (val < 0) {
+		val = UINT64_MAX;
+	} else {
+		val = val & ((1 << stat.bits) - 1);
+		priv->stats[i] += val;
+		val = priv->stats[i];
+	}
+
+	return val;
+}
+
+static void kszphy_get_stats(struct phy_device *phydev,
+			     struct ethtool_stats *stats, u64 *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(kszphy_hw_stats); i++)
+		data[i] = kszphy_get_stat(phydev, i);
+}
+
+static int kszphy_resume(struct phy_device *phydev)
+{
+	int value;
+
+	mutex_lock(&phydev->lock);
+
+	value = phy_read(phydev, MII_BMCR);
+	phy_write(phydev, MII_BMCR, value & ~BMCR_PDOWN);
+
+	kszphy_config_intr(phydev);
+	mutex_unlock(&phydev->lock);
+
+	return 0;
+}
+
 static int kszphy_probe(struct phy_device *phydev)
 {
 	const struct kszphy_type *type = phydev->drv->driver_data;
-	const struct device_node *np = phydev->dev.of_node;
+	const struct device_node *np = phydev->mdio.dev.of_node;
 	struct kszphy_priv *priv;
 	struct clk *clk;
 	int ret;
 
-	priv = devm_kzalloc(&phydev->dev, sizeof(*priv), GFP_KERNEL);
+	priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
@@ -592,15 +684,15 @@ static int kszphy_probe(struct phy_device *phydev)
 			priv->led_mode = -1;
 
 		if (priv->led_mode > 3) {
-			dev_err(&phydev->dev, "invalid led mode: 0x%02x\n",
-					priv->led_mode);
+			phydev_err(phydev, "invalid led mode: 0x%02x\n",
+				   priv->led_mode);
 			priv->led_mode = -1;
 		}
 	} else {
 		priv->led_mode = -1;
 	}
 
-	clk = devm_clk_get(&phydev->dev, "rmii-ref");
+	clk = devm_clk_get(&phydev->mdio.dev, "rmii-ref");
 	/* NOTE: clk may be NULL if building without CONFIG_HAVE_CLK */
 	if (!IS_ERR_OR_NULL(clk)) {
 		unsigned long rate = clk_get_rate(clk);
@@ -615,7 +707,8 @@ static int kszphy_probe(struct phy_device *phydev)
 		} else if (rate > 49500000 && rate < 50500000) {
 			priv->rmii_ref_clk_sel_val = !rmii_ref_clk_sel_25_mhz;
 		} else {
-			dev_err(&phydev->dev, "Clock rate out of range: %ld\n", rate);
+			phydev_err(phydev, "Clock rate out of range: %ld\n",
+				   rate);
 			return -EINVAL;
 		}
 	}
@@ -642,9 +735,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8021,
 	.phy_id_mask	= 0x00ffffff,
@@ -659,9 +754,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8031,
 	.phy_id_mask	= 0x00ffffff,
@@ -676,9 +773,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8041,
 	.phy_id_mask	= 0x00fffff0,
@@ -693,9 +792,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8041RNLI,
 	.phy_id_mask	= 0x00fffff0,
@@ -710,9 +811,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8051,
 	.phy_id_mask	= 0x00fffff0,
@@ -727,9 +830,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8001,
 	.name		= "Micrel KSZ8001 or KS8721",
@@ -743,9 +848,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ8081,
 	.name		= "Micrel KSZ8081 or KSZ8091",
@@ -759,9 +866,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
-	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
+	.resume		= kszphy_resume,
 }, {
 	.phy_id		= PHY_ID_KSZ8061,
 	.name		= "Micrel KSZ8061",
@@ -773,9 +882,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_KSZ9021,
 	.phy_id_mask	= 0x000ffffe,
@@ -788,11 +899,13 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
 	.read_mmd_indirect = ksz9021_rd_mmd_phyreg,
 	.write_mmd_indirect = ksz9021_wr_mmd_phyreg,
-	.driver		= { .owner = THIS_MODULE, },
 }, {
 	.phy_id		= PHY_ID_KSZ9031,
 	.phy_id_mask	= 0x00fffff0,
@@ -805,9 +918,11 @@ static struct phy_driver ksphy_driver[] = {
 	.read_status	= ksz9031_read_status,
 	.ack_interrupt	= kszphy_ack_interrupt,
 	.config_intr	= kszphy_config_intr,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE, },
 }, {
 	.phy_id		= PHY_ID_KSZ8873MLL,
 	.phy_id_mask	= 0x00fffff0,
@@ -817,9 +932,11 @@ static struct phy_driver ksphy_driver[] = {
 	.config_init	= kszphy_config_init,
 	.config_aneg	= ksz8873mll_config_aneg,
 	.read_status	= ksz8873mll_read_status,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE, },
 }, {
 	.phy_id		= PHY_ID_KSZ886X,
 	.phy_id_mask	= 0x00fffff0,
@@ -829,9 +946,11 @@ static struct phy_driver ksphy_driver[] = {
 	.config_init	= kszphy_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
+	.get_sset_count = kszphy_get_sset_count,
+	.get_strings	= kszphy_get_strings,
+	.get_stats	= kszphy_get_stats,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE, },
 } };
 
 module_phy_driver(ksphy_driver);
diff --git a/drivers/net/phy/microchip.c b/drivers/net/phy/microchip.c
index c0a20ebd083b..15f820648f82 100644
--- a/drivers/net/phy/microchip.c
+++ b/drivers/net/phy/microchip.c
@@ -68,7 +68,7 @@ int lan88xx_suspend(struct phy_device *phydev)
 
 static int lan88xx_probe(struct phy_device *phydev)
 {
-	struct device *dev = &phydev->dev;
+	struct device *dev = &phydev->mdio.dev;
 	struct lan88xx_priv *priv;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
@@ -78,10 +78,9 @@ static int lan88xx_probe(struct phy_device *phydev)
 	priv->wolopts = 0;
 
 	/* these values can be used to identify internal PHY */
-	priv->chip_id = phy_read_mmd_indirect(phydev, LAN88XX_MMD3_CHIP_ID,
-					      3, phydev->addr);
+	priv->chip_id = phy_read_mmd_indirect(phydev, LAN88XX_MMD3_CHIP_ID, 3);
 	priv->chip_rev = phy_read_mmd_indirect(phydev, LAN88XX_MMD3_CHIP_REV,
-					       3, phydev->addr);
+					       3);
 
 	phydev->priv = priv;
 
@@ -90,7 +89,7 @@ static int lan88xx_probe(struct phy_device *phydev)
 
 static void lan88xx_remove(struct phy_device *phydev)
 {
-	struct device *dev = &phydev->dev;
+	struct device *dev = &phydev->mdio.dev;
 	struct lan88xx_priv *priv = phydev->priv;
 
 	if (priv)
@@ -130,8 +129,6 @@ static struct phy_driver microchip_phy_driver[] = {
 	.suspend	= lan88xx_suspend,
 	.resume		= genphy_resume,
 	.set_wol	= lan88xx_set_wol,
-
-	.driver		= { .owner = THIS_MODULE, }
 } };
 
 module_phy_driver(microchip_phy_driver);
diff --git a/drivers/net/phy/national.c b/drivers/net/phy/national.c
index 0a7b9c7f09a2..2a1b490bc587 100644
--- a/drivers/net/phy/national.c
+++ b/drivers/net/phy/national.c
@@ -140,7 +140,6 @@ static struct phy_driver dp83865_driver[] = { {
 	.read_status = genphy_read_status,
 	.ack_interrupt = ns_ack_interrupt,
 	.config_intr = ns_config_intr,
-	.driver = {.owner = THIS_MODULE,}
 } };
 
 module_phy_driver(dp83865_driver);
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index 47cd306dbb3c..5590b9c182c9 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -319,7 +319,7 @@ int phy_ethtool_sset(struct phy_device *phydev, struct ethtool_cmd *cmd)
 {
 	u32 speed = ethtool_cmd_speed(cmd);
 
-	if (cmd->phy_address != phydev->addr)
+	if (cmd->phy_address != phydev->mdio.addr)
 		return -EINVAL;
 
 	/* We make sure that we don't pass unsupported values in to the PHY */
@@ -375,7 +375,7 @@ int phy_ethtool_gset(struct phy_device *phydev, struct ethtool_cmd *cmd)
 		cmd->port = PORT_BNC;
 	else
 		cmd->port = PORT_MII;
-	cmd->phy_address = phydev->addr;
+	cmd->phy_address = phydev->mdio.addr;
 	cmd->transceiver = phy_is_internal(phydev) ?
 		XCVR_INTERNAL : XCVR_EXTERNAL;
 	cmd->autoneg = phydev->autoneg;
@@ -403,16 +403,17 @@ int phy_mii_ioctl(struct phy_device *phydev, struct ifreq *ifr, int cmd)
 
 	switch (cmd) {
 	case SIOCGMIIPHY:
-		mii_data->phy_id = phydev->addr;
+		mii_data->phy_id = phydev->mdio.addr;
 		/* fall through */
 
 	case SIOCGMIIREG:
-		mii_data->val_out = mdiobus_read(phydev->bus, mii_data->phy_id,
+		mii_data->val_out = mdiobus_read(phydev->mdio.bus,
+						 mii_data->phy_id,
 						 mii_data->reg_num);
 		return 0;
 
 	case SIOCSMIIREG:
-		if (mii_data->phy_id == phydev->addr) {
+		if (mii_data->phy_id == phydev->mdio.addr) {
 			switch (mii_data->reg_num) {
 			case MII_BMCR:
 				if ((val & (BMCR_RESET | BMCR_ANENABLE)) == 0) {
@@ -445,10 +446,10 @@ int phy_mii_ioctl(struct phy_device *phydev, struct ifreq *ifr, int cmd)
 			}
 		}
 
-		mdiobus_write(phydev->bus, mii_data->phy_id,
+		mdiobus_write(phydev->mdio.bus, mii_data->phy_id,
 			      mii_data->reg_num, val);
 
-		if (mii_data->phy_id == phydev->addr &&
+		if (mii_data->phy_id == phydev->mdio.addr &&
 		    mii_data->reg_num == MII_BMCR &&
 		    val & BMCR_RESET)
 			return phy_init_hw(phydev);
@@ -643,7 +644,7 @@ int phy_start_interrupts(struct phy_device *phydev)
 	if (request_irq(phydev->irq, phy_interrupt, 0, "phy_interrupt",
 			phydev) < 0) {
 		pr_warn("%s: Can't get IRQ %d (PHY)\n",
-			phydev->bus->name, phydev->irq);
+			phydev->mdio.bus->name, phydev->irq);
 		phydev->irq = PHY_POLL;
 		return 0;
 	}
@@ -691,25 +692,29 @@ void phy_change(struct work_struct *work)
 	struct phy_device *phydev =
 		container_of(work, struct phy_device, phy_queue);
 
-	if (phydev->drv->did_interrupt &&
-	    !phydev->drv->did_interrupt(phydev))
-		goto ignore;
+	if (phy_interrupt_is_valid(phydev)) {
+		if (phydev->drv->did_interrupt &&
+		    !phydev->drv->did_interrupt(phydev))
+			goto ignore;
 
-	if (phy_disable_interrupts(phydev))
-		goto phy_err;
+		if (phy_disable_interrupts(phydev))
+			goto phy_err;
+	}
 
 	mutex_lock(&phydev->lock);
 	if ((PHY_RUNNING == phydev->state) || (PHY_NOLINK == phydev->state))
 		phydev->state = PHY_CHANGELINK;
 	mutex_unlock(&phydev->lock);
 
-	atomic_dec(&phydev->irq_disable);
-	enable_irq(phydev->irq);
+	if (phy_interrupt_is_valid(phydev)) {
+		atomic_dec(&phydev->irq_disable);
+		enable_irq(phydev->irq);
 
-	/* Reenable interrupts */
-	if (PHY_HALTED != phydev->state &&
-	    phy_config_interrupt(phydev, PHY_INTERRUPT_ENABLED))
-		goto irq_enable_err;
+		/* Reenable interrupts */
+		if (PHY_HALTED != phydev->state &&
+		    phy_config_interrupt(phydev, PHY_INTERRUPT_ENABLED))
+			goto irq_enable_err;
+	}
 
 	/* reschedule state queue work to run as soon as possible */
 	cancel_delayed_work_sync(&phydev->state_queue);
@@ -904,10 +909,10 @@ void phy_state_machine(struct work_struct *work)
 		phydev->adjust_link(phydev->attached_dev);
 		break;
 	case PHY_RUNNING:
-		/* Only register a CHANGE if we are polling or ignoring
-		 * interrupts and link changed since latest checking.
+		/* Only register a CHANGE if we are polling and link changed
+		 * since latest checking.
 		 */
-		if (!phy_interrupt_is_valid(phydev)) {
+		if (phydev->irq == PHY_POLL) {
 			old_link = phydev->link;
 			err = phy_read_status(phydev);
 			if (err)
@@ -995,18 +1000,25 @@ void phy_state_machine(struct work_struct *work)
 	if (err < 0)
 		phy_error(phydev);
 
-	dev_dbg(&phydev->dev, "PHY state change %s -> %s\n",
-		phy_state_to_str(old_state), phy_state_to_str(phydev->state));
+	phydev_dbg(phydev, "PHY state change %s -> %s\n",
+		   phy_state_to_str(old_state),
+		   phy_state_to_str(phydev->state));
 
-	queue_delayed_work(system_power_efficient_wq, &phydev->state_queue,
-			   PHY_STATE_TIME * HZ);
+	/* Only re-schedule a PHY state machine change if we are polling the
+	 * PHY, if PHY_IGNORE_INTERRUPT is set, then we will be moving
+	 * between states from phy_mac_interrupt()
+	 */
+	if (phydev->irq == PHY_POLL)
+		queue_delayed_work(system_power_efficient_wq, &phydev->state_queue,
+				   PHY_STATE_TIME * HZ);
 }
 
 void phy_mac_interrupt(struct phy_device *phydev, int new_link)
 {
-	cancel_work_sync(&phydev->phy_queue);
 	phydev->link = new_link;
-	schedule_work(&phydev->phy_queue);
+
+	/* Trigger a state machine change */
+	queue_work(system_power_efficient_wq, &phydev->phy_queue);
 }
 EXPORT_SYMBOL(phy_mac_interrupt);
 
@@ -1028,7 +1040,6 @@ static inline void mmd_phy_indirect(struct mii_bus *bus, int prtad, int devad,
  * @phydev: The PHY device bus
  * @prtad: MMD Address
  * @devad: MMD DEVAD
- * @addr: PHY address on the MII bus
  *
  * Description: it reads data from the MMD registers (clause 22 to access to
  * clause 45) of the specified phy address.
@@ -1038,14 +1049,14 @@ static inline void mmd_phy_indirect(struct mii_bus *bus, int prtad, int devad,
  * 3) Write reg 13 // MMD Data Command for MMD DEVAD
  * 3) Read  reg 14 // Read MMD data
  */
-int phy_read_mmd_indirect(struct phy_device *phydev, int prtad,
-				 int devad, int addr)
+int phy_read_mmd_indirect(struct phy_device *phydev, int prtad, int devad)
 {
 	struct phy_driver *phydrv = phydev->drv;
+	int addr = phydev->mdio.addr;
 	int value = -1;
 
 	if (!phydrv->read_mmd_indirect) {
-		struct mii_bus *bus = phydev->bus;
+		struct mii_bus *bus = phydev->mdio.bus;
 
 		mutex_lock(&bus->mdio_lock);
 		mmd_phy_indirect(bus, prtad, devad, addr);
@@ -1065,7 +1076,6 @@ EXPORT_SYMBOL(phy_read_mmd_indirect);
  * @phydev: The PHY device
  * @prtad: MMD Address
  * @devad: MMD DEVAD
- * @addr: PHY address on the MII bus
  * @data: data to write in the MMD register
  *
  * Description: Write data from the MMD registers of the specified
@@ -1077,12 +1087,13 @@ EXPORT_SYMBOL(phy_read_mmd_indirect);
  * 3) Write reg 14 // Write MMD data
  */
 void phy_write_mmd_indirect(struct phy_device *phydev, int prtad,
-				   int devad, int addr, u32 data)
+				   int devad, u32 data)
 {
 	struct phy_driver *phydrv = phydev->drv;
+	int addr = phydev->mdio.addr;
 
 	if (!phydrv->write_mmd_indirect) {
-		struct mii_bus *bus = phydev->bus;
+		struct mii_bus *bus = phydev->mdio.bus;
 
 		mutex_lock(&bus->mdio_lock);
 		mmd_phy_indirect(bus, prtad, devad, addr);
@@ -1129,7 +1140,7 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 
 		/* First check if the EEE ability is supported */
 		eee_cap = phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_ABLE,
-						MDIO_MMD_PCS, phydev->addr);
+						MDIO_MMD_PCS);
 		if (eee_cap <= 0)
 			goto eee_exit_err;
 
@@ -1141,12 +1152,12 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 		 * the EEE advertising registers.
 		 */
 		eee_lp = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_LPABLE,
-					       MDIO_MMD_AN, phydev->addr);
+					       MDIO_MMD_AN);
 		if (eee_lp <= 0)
 			goto eee_exit_err;
 
 		eee_adv = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_ADV,
-						MDIO_MMD_AN, phydev->addr);
+						MDIO_MMD_AN);
 		if (eee_adv <= 0)
 			goto eee_exit_err;
 
@@ -1160,15 +1171,13 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 			 * clock while it is signaling LPI.
 			 */
 			int val = phy_read_mmd_indirect(phydev, MDIO_CTRL1,
-							MDIO_MMD_PCS,
-							phydev->addr);
+							MDIO_MMD_PCS);
 			if (val < 0)
 				return val;
 
 			val |= MDIO_PCS_CTRL1_CLKSTOP_EN;
 			phy_write_mmd_indirect(phydev, MDIO_CTRL1,
-					       MDIO_MMD_PCS, phydev->addr,
-					       val);
+					       MDIO_MMD_PCS, val);
 		}
 
 		return 0; /* EEE supported */
@@ -1187,8 +1196,7 @@ EXPORT_SYMBOL(phy_init_eee);
  */
 int phy_get_eee_err(struct phy_device *phydev)
 {
-	return phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_WK_ERR,
-				     MDIO_MMD_PCS, phydev->addr);
+	return phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_WK_ERR, MDIO_MMD_PCS);
 }
 EXPORT_SYMBOL(phy_get_eee_err);
 
@@ -1205,22 +1213,19 @@ int phy_ethtool_get_eee(struct phy_device *phydev, struct ethtool_eee *data)
 	int val;
 
 	/* Get Supported EEE */
-	val = phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_ABLE,
-				    MDIO_MMD_PCS, phydev->addr);
+	val = phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_ABLE, MDIO_MMD_PCS);
 	if (val < 0)
 		return val;
 	data->supported = mmd_eee_cap_to_ethtool_sup_t(val);
 
 	/* Get advertisement EEE */
-	val = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_ADV,
-				    MDIO_MMD_AN, phydev->addr);
+	val = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_ADV, MDIO_MMD_AN);
 	if (val < 0)
 		return val;
 	data->advertised = mmd_eee_adv_to_ethtool_adv_t(val);
 
 	/* Get LP advertisement EEE */
-	val = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_LPABLE,
-				    MDIO_MMD_AN, phydev->addr);
+	val = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_LPABLE, MDIO_MMD_AN);
 	if (val < 0)
 		return val;
 	data->lp_advertised = mmd_eee_adv_to_ethtool_adv_t(val);
@@ -1240,8 +1245,7 @@ int phy_ethtool_set_eee(struct phy_device *phydev, struct ethtool_eee *data)
 {
 	int val = ethtool_adv_to_mmd_eee_adv_t(data->advertised);
 
-	phy_write_mmd_indirect(phydev, MDIO_AN_EEE_ADV, MDIO_MMD_AN,
-			       phydev->addr, val);
+	phy_write_mmd_indirect(phydev, MDIO_AN_EEE_ADV, MDIO_MMD_AN, val);
 
 	return 0;
 }
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index 0bfbabad4431..e551f3a89cfd 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -43,15 +43,31 @@ MODULE_LICENSE("GPL");
 
 void phy_device_free(struct phy_device *phydev)
 {
-	put_device(&phydev->dev);
+	put_device(&phydev->mdio.dev);
 }
 EXPORT_SYMBOL(phy_device_free);
 
+static void phy_mdio_device_free(struct mdio_device *mdiodev)
+{
+	struct phy_device *phydev;
+
+	phydev = container_of(mdiodev, struct phy_device, mdio);
+	phy_device_free(phydev);
+}
+
 static void phy_device_release(struct device *dev)
 {
 	kfree(to_phy_device(dev));
 }
 
+static void phy_mdio_device_remove(struct mdio_device *mdiodev)
+{
+	struct phy_device *phydev;
+
+	phydev = container_of(mdiodev, struct phy_device, mdio);
+	phy_device_remove(phydev);
+}
+
 enum genphy_driver {
 	GENPHY_DRV_1G,
 	GENPHY_DRV_10G,
@@ -63,9 +79,118 @@ static struct phy_driver genphy_driver[GENPHY_DRV_MAX];
 static LIST_HEAD(phy_fixup_list);
 static DEFINE_MUTEX(phy_fixup_lock);
 
+#ifdef CONFIG_PM
+static bool mdio_bus_phy_may_suspend(struct phy_device *phydev)
+{
+	struct device_driver *drv = phydev->mdio.dev.driver;
+	struct phy_driver *phydrv = to_phy_driver(drv);
+	struct net_device *netdev = phydev->attached_dev;
+
+	if (!drv || !phydrv->suspend)
+		return false;
+
+	/* PHY not attached? May suspend if the PHY has not already been
+	 * suspended as part of a prior call to phy_disconnect() ->
+	 * phy_detach() -> phy_suspend() because the parent netdev might be the
+	 * MDIO bus driver and clock gated at this point.
+	 */
+	if (!netdev)
+		return !phydev->suspended;
+
+	/* Don't suspend PHY if the attached netdev parent may wakeup.
+	 * The parent may point to a PCI device, as in tg3 driver.
+	 */
+	if (netdev->dev.parent && device_may_wakeup(netdev->dev.parent))
+		return false;
+
+	/* Also don't suspend PHY if the netdev itself may wakeup. This
+	 * is the case for devices w/o underlaying pwr. mgmt. aware bus,
+	 * e.g. SoC devices.
+	 */
+	if (device_may_wakeup(&netdev->dev))
+		return false;
+
+	return true;
+}
+
+static int mdio_bus_phy_suspend(struct device *dev)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+
+	/* We must stop the state machine manually, otherwise it stops out of
+	 * control, possibly with the phydev->lock held. Upon resume, netdev
+	 * may call phy routines that try to grab the same lock, and that may
+	 * lead to a deadlock.
+	 */
+	if (phydev->attached_dev && phydev->adjust_link)
+		phy_stop_machine(phydev);
+
+	if (!mdio_bus_phy_may_suspend(phydev))
+		return 0;
+
+	return phy_suspend(phydev);
+}
+
+static int mdio_bus_phy_resume(struct device *dev)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	int ret;
+
+	if (!mdio_bus_phy_may_suspend(phydev))
+		goto no_resume;
+
+	ret = phy_resume(phydev);
+	if (ret < 0)
+		return ret;
+
+no_resume:
+	if (phydev->attached_dev && phydev->adjust_link)
+		phy_start_machine(phydev);
+
+	return 0;
+}
+
+static int mdio_bus_phy_restore(struct device *dev)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	struct net_device *netdev = phydev->attached_dev;
+	int ret;
+
+	if (!netdev)
+		return 0;
+
+	ret = phy_init_hw(phydev);
+	if (ret < 0)
+		return ret;
+
+	/* The PHY needs to renegotiate. */
+	phydev->link = 0;
+	phydev->state = PHY_UP;
+
+	phy_start_machine(phydev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops mdio_bus_phy_pm_ops = {
+	.suspend = mdio_bus_phy_suspend,
+	.resume = mdio_bus_phy_resume,
+	.freeze = mdio_bus_phy_suspend,
+	.thaw = mdio_bus_phy_resume,
+	.restore = mdio_bus_phy_restore,
+};
+
+#define MDIO_BUS_PHY_PM_OPS (&mdio_bus_phy_pm_ops)
+
+#else
+
+#define MDIO_BUS_PHY_PM_OPS NULL
+
+#endif /* CONFIG_PM */
+
 /**
  * phy_register_fixup - creates a new phy_fixup and adds it to the list
- * @bus_id: A string which matches phydev->dev.bus_id (or PHY_ANY_ID)
+ * @bus_id: A string which matches phydev->mdio.dev.bus_id (or PHY_ANY_ID)
  * @phy_uid: Used to match against phydev->phy_id (the UID of the PHY)
  *	It can also be PHY_ANY_UID
  * @phy_uid_mask: Applied to phydev->phy_id and fixup->phy_uid before
@@ -114,7 +239,7 @@ EXPORT_SYMBOL(phy_register_fixup_for_id);
  */
 static int phy_needs_fixup(struct phy_device *phydev, struct phy_fixup *fixup)
 {
-	if (strcmp(fixup->bus_id, dev_name(&phydev->dev)) != 0)
+	if (strcmp(fixup->bus_id, phydev_name(phydev)) != 0)
 		if (strcmp(fixup->bus_id, PHY_ANY_ID) != 0)
 			return 0;
 
@@ -148,18 +273,59 @@ static int phy_scan_fixups(struct phy_device *phydev)
 	return 0;
 }
 
+static int phy_bus_match(struct device *dev, struct device_driver *drv)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	struct phy_driver *phydrv = to_phy_driver(drv);
+	const int num_ids = ARRAY_SIZE(phydev->c45_ids.device_ids);
+	int i;
+
+	if (!(phydrv->mdiodrv.flags & MDIO_DEVICE_IS_PHY))
+		return 0;
+
+	if (phydrv->match_phy_device)
+		return phydrv->match_phy_device(phydev);
+
+	if (phydev->is_c45) {
+		for (i = 1; i < num_ids; i++) {
+			if (!(phydev->c45_ids.devices_in_package & (1 << i)))
+				continue;
+
+			if ((phydrv->phy_id & phydrv->phy_id_mask) ==
+			    (phydev->c45_ids.device_ids[i] &
+			     phydrv->phy_id_mask))
+				return 1;
+		}
+		return 0;
+	} else {
+		return (phydrv->phy_id & phydrv->phy_id_mask) ==
+			(phydev->phy_id & phydrv->phy_id_mask);
+	}
+}
+
 struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id,
 				     bool is_c45,
 				     struct phy_c45_device_ids *c45_ids)
 {
 	struct phy_device *dev;
+	struct mdio_device *mdiodev;
 
 	/* We allocate the device, and initialize the default values */
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (!dev)
 		return ERR_PTR(-ENOMEM);
 
-	dev->dev.release = phy_device_release;
+	mdiodev = &dev->mdio;
+	mdiodev->dev.release = phy_device_release;
+	mdiodev->dev.parent = &bus->dev;
+	mdiodev->dev.bus = &mdio_bus_type;
+	mdiodev->bus = bus;
+	mdiodev->pm_ops = MDIO_BUS_PHY_PM_OPS;
+	mdiodev->bus_match = phy_bus_match;
+	mdiodev->addr = addr;
+	mdiodev->flags = MDIO_DEVICE_FLAG_PHY;
+	mdiodev->device_free = phy_mdio_device_free;
+	mdiodev->device_remove = phy_mdio_device_remove;
 
 	dev->speed = 0;
 	dev->duplex = -1;
@@ -171,15 +337,11 @@ struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id,
 	dev->autoneg = AUTONEG_ENABLE;
 
 	dev->is_c45 = is_c45;
-	dev->addr = addr;
 	dev->phy_id = phy_id;
 	if (c45_ids)
 		dev->c45_ids = *c45_ids;
-	dev->bus = bus;
-	dev->dev.parent = &bus->dev;
-	dev->dev.bus = &mdio_bus_type;
-	dev->irq = bus->irq ? bus->irq[addr] : PHY_POLL;
-	dev_set_name(&dev->dev, PHY_ID_FMT, bus->id, addr);
+	dev->irq = bus->irq[addr];
+	dev_set_name(&mdiodev->dev, PHY_ID_FMT, bus->id, addr);
 
 	dev->state = PHY_DOWN;
 
@@ -199,7 +361,7 @@ struct phy_device *phy_device_create(struct mii_bus *bus, int addr, int phy_id,
 	 */
 	request_module(MDIO_MODULE_PREFIX MDIO_ID_FMT, MDIO_ID_ARGS(phy_id));
 
-	device_initialize(&dev->dev);
+	device_initialize(&mdiodev->dev);
 
 	return dev;
 }
@@ -373,6 +535,48 @@ struct phy_device *get_phy_device(struct mii_bus *bus, int addr, bool is_c45)
 }
 EXPORT_SYMBOL(get_phy_device);
 
+static ssize_t
+phy_id_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+
+	return sprintf(buf, "0x%.8lx\n", (unsigned long)phydev->phy_id);
+}
+static DEVICE_ATTR_RO(phy_id);
+
+static ssize_t
+phy_interface_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+	const char *mode = NULL;
+
+	if (phy_is_internal(phydev))
+		mode = "internal";
+	else
+		mode = phy_modes(phydev->interface);
+
+	return sprintf(buf, "%s\n", mode);
+}
+static DEVICE_ATTR_RO(phy_interface);
+
+static ssize_t
+phy_has_fixups_show(struct device *dev, struct device_attribute *attr,
+		    char *buf)
+{
+	struct phy_device *phydev = to_phy_device(dev);
+
+	return sprintf(buf, "%d\n", phydev->has_fixups);
+}
+static DEVICE_ATTR_RO(phy_has_fixups);
+
+static struct attribute *phy_dev_attrs[] = {
+	&dev_attr_phy_id.attr,
+	&dev_attr_phy_interface.attr,
+	&dev_attr_phy_has_fixups.attr,
+	NULL,
+};
+ATTRIBUTE_GROUPS(phy_dev);
+
 /**
  * phy_device_register - Register the phy device on the MDIO bus
  * @phydev: phy_device structure to be added to the MDIO bus
@@ -381,28 +585,29 @@ int phy_device_register(struct phy_device *phydev)
 {
 	int err;
 
-	/* Don't register a phy if one is already registered at this address */
-	if (phydev->bus->phy_map[phydev->addr])
-		return -EINVAL;
-	phydev->bus->phy_map[phydev->addr] = phydev;
+	err = mdiobus_register_device(&phydev->mdio);
+	if (err)
+		return err;
 
 	/* Run all of the fixups for this PHY */
 	err = phy_scan_fixups(phydev);
 	if (err) {
-		pr_err("PHY %d failed to initialize\n", phydev->addr);
+		pr_err("PHY %d failed to initialize\n", phydev->mdio.addr);
 		goto out;
 	}
 
-	err = device_add(&phydev->dev);
+	phydev->mdio.dev.groups = phy_dev_groups;
+
+	err = device_add(&phydev->mdio.dev);
 	if (err) {
-		pr_err("PHY %d failed to add\n", phydev->addr);
+		pr_err("PHY %d failed to add\n", phydev->mdio.addr);
 		goto out;
 	}
 
 	return 0;
 
  out:
-	phydev->bus->phy_map[phydev->addr] = NULL;
+	mdiobus_unregister_device(&phydev->mdio);
 	return err;
 }
 EXPORT_SYMBOL(phy_device_register);
@@ -417,11 +622,8 @@ EXPORT_SYMBOL(phy_device_register);
  */
 void phy_device_remove(struct phy_device *phydev)
 {
-	struct mii_bus *bus = phydev->bus;
-	int addr = phydev->addr;
-
-	device_del(&phydev->dev);
-	bus->phy_map[addr] = NULL;
+	device_del(&phydev->mdio.dev);
+	mdiobus_unregister_device(&phydev->mdio);
 }
 EXPORT_SYMBOL(phy_device_remove);
 
@@ -431,11 +633,13 @@ EXPORT_SYMBOL(phy_device_remove);
  */
 struct phy_device *phy_find_first(struct mii_bus *bus)
 {
+	struct phy_device *phydev;
 	int addr;
 
 	for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
-		if (bus->phy_map[addr])
-			return bus->phy_map[addr];
+		phydev = mdiobus_get_phy(bus, addr);
+		if (phydev)
+			return phydev;
 	}
 	return NULL;
 }
@@ -607,6 +811,33 @@ int phy_init_hw(struct phy_device *phydev)
 }
 EXPORT_SYMBOL(phy_init_hw);
 
+void phy_attached_info(struct phy_device *phydev)
+{
+	phy_attached_print(phydev, NULL);
+}
+EXPORT_SYMBOL(phy_attached_info);
+
+#define ATTACHED_FMT "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)"
+void phy_attached_print(struct phy_device *phydev, const char *fmt, ...)
+{
+	if (!fmt) {
+		dev_info(&phydev->mdio.dev, ATTACHED_FMT "\n",
+			 phydev->drv->name, phydev_name(phydev),
+			 phydev->irq);
+	} else {
+		va_list ap;
+
+		dev_info(&phydev->mdio.dev, ATTACHED_FMT,
+			 phydev->drv->name, phydev_name(phydev),
+			 phydev->irq);
+
+		va_start(ap, fmt);
+		vprintk(fmt, ap);
+		va_end(ap);
+	}
+}
+EXPORT_SYMBOL(phy_attached_print);
+
 /**
  * phy_attach_direct - attach a network device to a given PHY device pointer
  * @dev: network device to attach
@@ -625,8 +856,8 @@ EXPORT_SYMBOL(phy_init_hw);
 int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 		      u32 flags, phy_interface_t interface)
 {
-	struct mii_bus *bus = phydev->bus;
-	struct device *d = &phydev->dev;
+	struct mii_bus *bus = phydev->mdio.bus;
+	struct device *d = &phydev->mdio.dev;
 	int err;
 
 	if (!try_module_get(bus->owner)) {
@@ -641,9 +872,11 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 	 */
 	if (!d->driver) {
 		if (phydev->is_c45)
-			d->driver = &genphy_driver[GENPHY_DRV_10G].driver;
+			d->driver =
+				&genphy_driver[GENPHY_DRV_10G].mdiodrv.driver;
 		else
-			d->driver = &genphy_driver[GENPHY_DRV_1G].driver;
+			d->driver =
+				&genphy_driver[GENPHY_DRV_1G].mdiodrv.driver;
 
 		err = d->driver->probe(d);
 		if (err >= 0)
@@ -668,6 +901,11 @@ int phy_attach_direct(struct net_device *dev, struct phy_device *phydev,
 
 	phydev->state = PHY_READY;
 
+	/* Initial carrier state is off as the phy is about to be
+	 * (re)initialized.
+	 */
+	netif_carrier_off(phydev->attached_dev);
+
 	/* Do initial configuration here, now that
 	 * we have certain key parameters
 	 * (dev_flags and interface)
@@ -744,8 +982,9 @@ void phy_detach(struct phy_device *phydev)
 	 * real driver could be loaded
 	 */
 	for (i = 0; i < ARRAY_SIZE(genphy_driver); i++) {
-		if (phydev->dev.driver == &genphy_driver[i].driver) {
-			device_release_driver(&phydev->dev);
+		if (phydev->mdio.dev.driver ==
+		    &genphy_driver[i].mdiodrv.driver) {
+			device_release_driver(&phydev->mdio.dev);
 			break;
 		}
 	}
@@ -754,16 +993,16 @@ void phy_detach(struct phy_device *phydev)
 	 * The phydev might go away on the put_device() below, so avoid
 	 * a use-after-free bug by reading the underlying bus first.
 	 */
-	bus = phydev->bus;
+	bus = phydev->mdio.bus;
 
-	put_device(&phydev->dev);
+	put_device(&phydev->mdio.dev);
 	module_put(bus->owner);
 }
 EXPORT_SYMBOL(phy_detach);
 
 int phy_suspend(struct phy_device *phydev)
 {
-	struct phy_driver *phydrv = to_phy_driver(phydev->dev.driver);
+	struct phy_driver *phydrv = to_phy_driver(phydev->mdio.dev.driver);
 	struct ethtool_wolinfo wol = { .cmd = ETHTOOL_GWOL };
 	int ret = 0;
 
@@ -786,7 +1025,7 @@ EXPORT_SYMBOL(phy_suspend);
 
 int phy_resume(struct phy_device *phydev)
 {
-	struct phy_driver *phydrv = to_phy_driver(phydev->dev.driver);
+	struct phy_driver *phydrv = to_phy_driver(phydev->mdio.dev.driver);
 	int ret = 0;
 
 	if (phydrv->resume)
@@ -1171,7 +1410,7 @@ int genphy_config_init(struct phy_device *phydev)
 
 	features = (SUPPORTED_TP | SUPPORTED_MII
 			| SUPPORTED_AUI | SUPPORTED_FIBRE |
-			SUPPORTED_BNC);
+			SUPPORTED_BNC | SUPPORTED_Pause | SUPPORTED_Asym_Pause);
 
 	/* Do we support autonegotiation? */
 	val = phy_read(phydev, MII_BMSR);
@@ -1303,7 +1542,7 @@ EXPORT_SYMBOL(phy_set_max_speed);
 
 static void of_set_phy_supported(struct phy_device *phydev)
 {
-	struct device_node *node = phydev->dev.of_node;
+	struct device_node *node = phydev->mdio.dev.of_node;
 	u32 max_speed;
 
 	if (!IS_ENABLED(CONFIG_OF_MDIO))
@@ -1327,7 +1566,7 @@ static void of_set_phy_supported(struct phy_device *phydev)
 static int phy_probe(struct device *dev)
 {
 	struct phy_device *phydev = to_phy_device(dev);
-	struct device_driver *drv = phydev->dev.driver;
+	struct device_driver *drv = phydev->mdio.dev.driver;
 	struct phy_driver *phydrv = to_phy_driver(drv);
 	int err = 0;
 
@@ -1382,17 +1621,20 @@ static int phy_remove(struct device *dev)
 /**
  * phy_driver_register - register a phy_driver with the PHY layer
  * @new_driver: new phy_driver to register
+ * @owner: module owning this PHY
  */
-int phy_driver_register(struct phy_driver *new_driver)
+int phy_driver_register(struct phy_driver *new_driver, struct module *owner)
 {
 	int retval;
 
-	new_driver->driver.name = new_driver->name;
-	new_driver->driver.bus = &mdio_bus_type;
-	new_driver->driver.probe = phy_probe;
-	new_driver->driver.remove = phy_remove;
+	new_driver->mdiodrv.flags |= MDIO_DEVICE_IS_PHY;
+	new_driver->mdiodrv.driver.name = new_driver->name;
+	new_driver->mdiodrv.driver.bus = &mdio_bus_type;
+	new_driver->mdiodrv.driver.probe = phy_probe;
+	new_driver->mdiodrv.driver.remove = phy_remove;
+	new_driver->mdiodrv.driver.owner = owner;
 
-	retval = driver_register(&new_driver->driver);
+	retval = driver_register(&new_driver->mdiodrv.driver);
 	if (retval) {
 		pr_err("%s: Error %d in registering driver\n",
 		       new_driver->name, retval);
@@ -1406,12 +1648,13 @@ int phy_driver_register(struct phy_driver *new_driver)
 }
 EXPORT_SYMBOL(phy_driver_register);
 
-int phy_drivers_register(struct phy_driver *new_driver, int n)
+int phy_drivers_register(struct phy_driver *new_driver, int n,
+			 struct module *owner)
 {
 	int i, ret = 0;
 
 	for (i = 0; i < n; i++) {
-		ret = phy_driver_register(new_driver + i);
+		ret = phy_driver_register(new_driver + i, owner);
 		if (ret) {
 			while (i-- > 0)
 				phy_driver_unregister(new_driver + i);
@@ -1424,7 +1667,7 @@ EXPORT_SYMBOL(phy_drivers_register);
 
 void phy_driver_unregister(struct phy_driver *drv)
 {
-	driver_unregister(&drv->driver);
+	driver_unregister(&drv->mdiodrv.driver);
 }
 EXPORT_SYMBOL(phy_driver_unregister);
 
@@ -1452,7 +1695,6 @@ static struct phy_driver genphy_driver[] = {
 	.read_status	= genphy_read_status,
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-	.driver		= { .owner = THIS_MODULE, },
 }, {
 	.phy_id         = 0xffffffff,
 	.phy_id_mask    = 0xffffffff,
@@ -1464,7 +1706,6 @@ static struct phy_driver genphy_driver[] = {
 	.read_status    = gen10g_read_status,
 	.suspend        = gen10g_suspend,
 	.resume         = gen10g_resume,
-	.driver         = {.owner = THIS_MODULE, },
 } };
 
 static int __init phy_init(void)
@@ -1476,7 +1717,7 @@ static int __init phy_init(void)
 		return rc;
 
 	rc = phy_drivers_register(genphy_driver,
-				  ARRAY_SIZE(genphy_driver));
+				  ARRAY_SIZE(genphy_driver), THIS_MODULE);
 	if (rc)
 		mdio_bus_exit();
 
diff --git a/drivers/net/phy/qsemi.c b/drivers/net/phy/qsemi.c
index be4c6f7c3645..d470db89e8dd 100644
--- a/drivers/net/phy/qsemi.c
+++ b/drivers/net/phy/qsemi.c
@@ -122,7 +122,6 @@ static struct phy_driver qs6612_driver[] = { {
 	.read_status	= genphy_read_status,
 	.ack_interrupt	= qs6612_ack_interrupt,
 	.config_intr	= qs6612_config_intr,
-	.driver 	= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(qs6612_driver);
diff --git a/drivers/net/phy/realtek.c b/drivers/net/phy/realtek.c
index 43ab691362d4..aadd6e9f54ad 100644
--- a/drivers/net/phy/realtek.c
+++ b/drivers/net/phy/realtek.c
@@ -124,7 +124,6 @@ static struct phy_driver realtek_drvs[] = {
 		.flags          = PHY_HAS_INTERRUPT,
 		.config_aneg    = &genphy_config_aneg,
 		.read_status    = &genphy_read_status,
-		.driver         = { .owner = THIS_MODULE,},
 	}, {
 		.phy_id		= 0x001cc912,
 		.name		= "RTL8211B Gigabit Ethernet",
@@ -135,7 +134,6 @@ static struct phy_driver realtek_drvs[] = {
 		.read_status	= &genphy_read_status,
 		.ack_interrupt	= &rtl821x_ack_interrupt,
 		.config_intr	= &rtl8211b_config_intr,
-		.driver		= { .owner = THIS_MODULE,},
 	}, {
 		.phy_id		= 0x001cc914,
 		.name		= "RTL8211DN Gigabit Ethernet",
@@ -148,7 +146,6 @@ static struct phy_driver realtek_drvs[] = {
 		.config_intr	= rtl8211e_config_intr,
 		.suspend	= genphy_suspend,
 		.resume		= genphy_resume,
-		.driver		= { .owner = THIS_MODULE,},
 	}, {
 		.phy_id		= 0x001cc915,
 		.name		= "RTL8211E Gigabit Ethernet",
@@ -161,7 +158,6 @@ static struct phy_driver realtek_drvs[] = {
 		.config_intr	= &rtl8211e_config_intr,
 		.suspend	= genphy_suspend,
 		.resume		= genphy_resume,
-		.driver		= { .owner = THIS_MODULE,},
 	}, {
 		.phy_id		= 0x001cc916,
 		.name		= "RTL8211F Gigabit Ethernet",
@@ -175,7 +171,6 @@ static struct phy_driver realtek_drvs[] = {
 		.config_intr	= &rtl8211f_config_intr,
 		.suspend	= genphy_suspend,
 		.resume		= genphy_resume,
-		.driver		= { .owner = THIS_MODULE },
 	},
 };
 
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index dc2da8770918..2e21e9366f76 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -24,6 +24,10 @@
 #include <linux/netdevice.h>
 #include <linux/smscphy.h>
 
+struct smsc_phy_priv {
+	bool energy_enable;
+};
+
 static int smsc_phy_config_intr(struct phy_device *phydev)
 {
 	int rc = phy_write (phydev, MII_LAN83C185_IM,
@@ -43,19 +47,14 @@ static int smsc_phy_ack_interrupt(struct phy_device *phydev)
 
 static int smsc_phy_config_init(struct phy_device *phydev)
 {
-	int __maybe_unused len;
-	struct device *dev __maybe_unused = &phydev->dev;
-	struct device_node *of_node __maybe_unused = dev->of_node;
+	struct smsc_phy_priv *priv = phydev->priv;
+
 	int rc = phy_read(phydev, MII_LAN83C185_CTRL_STATUS);
-	int enable_energy = 1;
 
 	if (rc < 0)
 		return rc;
 
-	if (of_find_property(of_node, "smsc,disable-energy-detect", &len))
-		enable_energy = 0;
-
-	if (enable_energy) {
+	if (priv->energy_enable) {
 		/* Enable energy detect mode for this SMSC Transceivers */
 		rc = phy_write(phydev, MII_LAN83C185_CTRL_STATUS,
 			       rc | MII_LAN83C185_EDPWRDOWN);
@@ -110,10 +109,13 @@ static int lan911x_config_init(struct phy_device *phydev)
  */
 static int lan87xx_read_status(struct phy_device *phydev)
 {
+	struct smsc_phy_priv *priv = phydev->priv;
+
 	int err = genphy_read_status(phydev);
-	int i;
 
-	if (!phydev->link) {
+	if (!phydev->link && priv->energy_enable) {
+		int i;
+
 		/* Disable EDPD to wake up PHY */
 		int rc = phy_read(phydev, MII_LAN83C185_CTRL_STATUS);
 		if (rc < 0)
@@ -149,6 +151,26 @@ static int lan87xx_read_status(struct phy_device *phydev)
 	return err;
 }
 
+static int smsc_phy_probe(struct phy_device *phydev)
+{
+	struct device *dev = &phydev->mdio.dev;
+	struct device_node *of_node = dev->of_node;
+	struct smsc_phy_priv *priv;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->energy_enable = true;
+
+	if (of_property_read_bool(of_node, "smsc,disable-energy-detect"))
+		priv->energy_enable = false;
+
+	phydev->priv = priv;
+
+	return 0;
+}
+
 static struct phy_driver smsc_phy_driver[] = {
 {
 	.phy_id		= 0x0007c0a0, /* OUI=0x00800f, Model#=0x0a */
@@ -159,6 +181,8 @@ static struct phy_driver smsc_phy_driver[] = {
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
 
+	.probe		= smsc_phy_probe,
+
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -171,8 +195,6 @@ static struct phy_driver smsc_phy_driver[] = {
 
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-
-	.driver		= { .owner = THIS_MODULE, }
 }, {
 	.phy_id		= 0x0007c0b0, /* OUI=0x00800f, Model#=0x0b */
 	.phy_id_mask	= 0xfffffff0,
@@ -182,6 +204,8 @@ static struct phy_driver smsc_phy_driver[] = {
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
 
+	.probe		= smsc_phy_probe,
+
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -194,8 +218,6 @@ static struct phy_driver smsc_phy_driver[] = {
 
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-
-	.driver		= { .owner = THIS_MODULE, }
 }, {
 	.phy_id		= 0x0007c0c0, /* OUI=0x00800f, Model#=0x0c */
 	.phy_id_mask	= 0xfffffff0,
@@ -205,6 +227,8 @@ static struct phy_driver smsc_phy_driver[] = {
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
 
+	.probe		= smsc_phy_probe,
+
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= lan87xx_read_status,
@@ -217,8 +241,6 @@ static struct phy_driver smsc_phy_driver[] = {
 
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-
-	.driver		= { .owner = THIS_MODULE, }
 }, {
 	.phy_id		= 0x0007c0d0, /* OUI=0x00800f, Model#=0x0d */
 	.phy_id_mask	= 0xfffffff0,
@@ -228,6 +250,8 @@ static struct phy_driver smsc_phy_driver[] = {
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
 
+	.probe		= smsc_phy_probe,
+
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -239,8 +263,6 @@ static struct phy_driver smsc_phy_driver[] = {
 
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
-
-	.driver		= { .owner = THIS_MODULE, }
 }, {
 	.phy_id		= 0x0007c0f0, /* OUI=0x00800f, Model#=0x0f */
 	.phy_id_mask	= 0xfffffff0,
@@ -250,6 +272,8 @@ static struct phy_driver smsc_phy_driver[] = {
 				| SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
 
+	.probe		= smsc_phy_probe,
+
 	/* basic functions */
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= lan87xx_read_status,
@@ -262,8 +286,29 @@ static struct phy_driver smsc_phy_driver[] = {
 
 	.suspend	= genphy_suspend,
 	.resume		= genphy_resume,
+}, {
+	.phy_id		= 0x0007c110,
+	.phy_id_mask	= 0xfffffff0,
+	.name		= "SMSC LAN8740",
 
-	.driver		= { .owner = THIS_MODULE, }
+	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause
+				| SUPPORTED_Asym_Pause),
+	.flags		= PHY_HAS_INTERRUPT | PHY_HAS_MAGICANEG,
+
+	.probe		= smsc_phy_probe,
+
+	/* basic functions */
+	.config_aneg	= genphy_config_aneg,
+	.read_status	= lan87xx_read_status,
+	.config_init	= smsc_phy_config_init,
+	.soft_reset	= smsc_phy_reset,
+
+	/* IRQ related */
+	.ack_interrupt	= smsc_phy_ack_interrupt,
+	.config_intr	= smsc_phy_config_intr,
+
+	.suspend	= genphy_suspend,
+	.resume		= genphy_resume,
 } };
 
 module_phy_driver(smsc_phy_driver);
@@ -278,6 +323,7 @@ static struct mdio_device_id __maybe_unused smsc_tbl[] = {
 	{ 0x0007c0c0, 0xfffffff0 },
 	{ 0x0007c0d0, 0xfffffff0 },
 	{ 0x0007c0f0, 0xfffffff0 },
+	{ 0x0007c110, 0xfffffff0 },
 	{ }
 };
 
diff --git a/drivers/net/phy/ste10Xp.c b/drivers/net/phy/ste10Xp.c
index 3fc199b773e6..d00cfb64529e 100644
--- a/drivers/net/phy/ste10Xp.c
+++ b/drivers/net/phy/ste10Xp.c
@@ -95,7 +95,6 @@ static struct phy_driver ste10xp_pdriver[] = {
 	.config_intr = ste10Xp_config_intr,
 	.suspend = genphy_suspend,
 	.resume = genphy_resume,
-	.driver = {.owner = THIS_MODULE,}
 }, {
 	.phy_id = STE100P_PHY_ID,
 	.phy_id_mask = 0xffffffff,
@@ -109,7 +108,6 @@ static struct phy_driver ste10xp_pdriver[] = {
 	.config_intr = ste10Xp_config_intr,
 	.suspend = genphy_suspend,
 	.resume = genphy_resume,
-	.driver = {.owner = THIS_MODULE,}
 } };
 
 module_phy_driver(ste10xp_pdriver);
diff --git a/drivers/net/phy/teranetics.c b/drivers/net/phy/teranetics.c
index 07463fcca212..fb2cef764e9a 100644
--- a/drivers/net/phy/teranetics.c
+++ b/drivers/net/phy/teranetics.c
@@ -108,7 +108,6 @@ static struct phy_driver teranetics_driver[] = {
 	.config_aneg    = teranetics_config_aneg,
 	.read_status	= teranetics_read_status,
 	.match_phy_device = teranetics_match_phy_device,
-	.driver		= { .owner = THIS_MODULE,},
 },
 };
 
diff --git a/drivers/net/phy/vitesse.c b/drivers/net/phy/vitesse.c
index dd295dbaa074..2e37eb337d48 100644
--- a/drivers/net/phy/vitesse.c
+++ b/drivers/net/phy/vitesse.c
@@ -236,7 +236,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status    = &genphy_read_status,
 	.ack_interrupt  = &vsc824x_ack_interrupt,
 	.config_intr    = &vsc82xx_config_intr,
-	.driver         = { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_VSC8244,
 	.name		= "Vitesse VSC8244",
@@ -248,7 +247,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &vsc824x_ack_interrupt,
 	.config_intr	= &vsc82xx_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id		= PHY_ID_VSC8514,
 	.name		= "Vitesse VSC8514",
@@ -260,7 +258,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &vsc824x_ack_interrupt,
 	.config_intr	= &vsc82xx_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	.phy_id         = PHY_ID_VSC8574,
 	.name           = "Vitesse VSC8574",
@@ -272,7 +269,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status    = &genphy_read_status,
 	.ack_interrupt  = &vsc824x_ack_interrupt,
 	.config_intr    = &vsc82xx_config_intr,
-	.driver         = { .owner = THIS_MODULE,},
 }, {
 	.phy_id         = PHY_ID_VSC8601,
 	.name           = "Vitesse VSC8601",
@@ -284,7 +280,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status    = &genphy_read_status,
 	.ack_interrupt  = &vsc824x_ack_interrupt,
 	.config_intr    = &vsc82xx_config_intr,
-	.driver         = { .owner = THIS_MODULE,},
 }, {
 	.phy_id         = PHY_ID_VSC8662,
 	.name           = "Vitesse VSC8662",
@@ -296,7 +291,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status    = &genphy_read_status,
 	.ack_interrupt  = &vsc824x_ack_interrupt,
 	.config_intr    = &vsc82xx_config_intr,
-	.driver         = { .owner = THIS_MODULE,},
 }, {
 	/* Vitesse 8221 */
 	.phy_id		= PHY_ID_VSC8221,
@@ -309,7 +303,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &vsc824x_ack_interrupt,
 	.config_intr	= &vsc82xx_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 }, {
 	/* Vitesse 8211 */
 	.phy_id		= PHY_ID_VSC8211,
@@ -322,7 +315,6 @@ static struct phy_driver vsc82xx_driver[] = {
 	.read_status	= &genphy_read_status,
 	.ack_interrupt	= &vsc824x_ack_interrupt,
 	.config_intr	= &vsc82xx_config_intr,
-	.driver		= { .owner = THIS_MODULE,},
 } };
 
 module_phy_driver(vsc82xx_driver);
diff --git a/drivers/net/plip/plip.c b/drivers/net/plip/plip.c
index 040b8978d6ca..9c4b41a4df7d 100644
--- a/drivers/net/plip/plip.c
+++ b/drivers/net/plip/plip.c
@@ -1249,6 +1249,7 @@ static void plip_attach (struct parport *port)
 	struct net_device *dev;
 	struct net_local *nl;
 	char name[IFNAMSIZ];
+	struct pardev_cb plip_cb;
 
 	if ((parport[0] == -1 && (!timid || !port->devices)) ||
 	    plip_searchfor(parport, port->number)) {
@@ -1273,9 +1274,15 @@ static void plip_attach (struct parport *port)
 
 		nl = netdev_priv(dev);
 		nl->dev = dev;
-		nl->pardev = parport_register_device(port, dev->name, plip_preempt,
-						 plip_wakeup, plip_interrupt,
-						 0, dev);
+
+		memset(&plip_cb, 0, sizeof(plip_cb));
+		plip_cb.private = dev;
+		plip_cb.preempt = plip_preempt;
+		plip_cb.wakeup = plip_wakeup;
+		plip_cb.irq_func = plip_interrupt;
+
+		nl->pardev = parport_register_dev_model(port, dev->name,
+							&plip_cb, unit);
 
 		if (!nl->pardev) {
 			printk(KERN_ERR "%s: parport_register failed\n", name);
@@ -1315,10 +1322,23 @@ static void plip_detach (struct parport *port)
 	/* Nothing to do */
 }
 
+static int plip_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 plip_driver = {
-	.name	= "plip",
-	.attach = plip_attach,
-	.detach = plip_detach
+	.name		= "plip",
+	.probe		= plip_probe,
+	.match_port	= plip_attach,
+	.detach		= plip_detach,
+	.devmodel	= true,
 };
 
 static void __exit plip_cleanup_module (void)
@@ -1326,8 +1346,6 @@ static void __exit plip_cleanup_module (void)
 	struct net_device *dev;
 	int i;
 
-	parport_unregister_driver (&plip_driver);
-
 	for (i=0; i < PLIP_MAX; i++) {
 		if ((dev = dev_plip[i])) {
 			struct net_local *nl = netdev_priv(dev);
@@ -1339,6 +1357,8 @@ static void __exit plip_cleanup_module (void)
 			dev_plip[i] = NULL;
 		}
 	}
+
+	parport_unregister_driver(&plip_driver);
 }
 
 #ifndef MODULE
diff --git a/drivers/net/ppp/ppp_generic.c b/drivers/net/ppp/ppp_generic.c
index 9a863c6a6a33..d61da9ece3ba 100644
--- a/drivers/net/ppp/ppp_generic.c
+++ b/drivers/net/ppp/ppp_generic.c
@@ -443,9 +443,14 @@ static ssize_t ppp_read(struct file *file, char __user *buf,
 			 * network traffic (demand mode).
 			 */
 			struct ppp *ppp = PF_TO_PPP(pf);
+
+			ppp_recv_lock(ppp);
 			if (ppp->n_channels == 0 &&
-			    (ppp->flags & SC_LOOP_TRAFFIC) == 0)
+			    (ppp->flags & SC_LOOP_TRAFFIC) == 0) {
+				ppp_recv_unlock(ppp);
 				break;
+			}
+			ppp_recv_unlock(ppp);
 		}
 		ret = -EAGAIN;
 		if (file->f_flags & O_NONBLOCK)
@@ -532,9 +537,12 @@ static unsigned int ppp_poll(struct file *file, poll_table *wait)
 	else if (pf->kind == INTERFACE) {
 		/* see comment in ppp_read */
 		struct ppp *ppp = PF_TO_PPP(pf);
+
+		ppp_recv_lock(ppp);
 		if (ppp->n_channels == 0 &&
 		    (ppp->flags & SC_LOOP_TRAFFIC) == 0)
 			mask |= POLLIN | POLLRDNORM;
+		ppp_recv_unlock(ppp);
 	}
 
 	return mask;
@@ -1138,9 +1146,15 @@ static const struct net_device_ops ppp_netdev_ops = {
 	.ndo_get_stats64 = ppp_get_stats64,
 };
 
+static struct device_type ppp_type = {
+	.name = "ppp",
+};
+
 static void ppp_setup(struct net_device *dev)
 {
 	dev->netdev_ops = &ppp_netdev_ops;
+	SET_NETDEV_DEVTYPE(dev, &ppp_type);
+
 	dev->hard_header_len = PPP_HDRLEN;
 	dev->mtu = PPP_MRU;
 	dev->addr_len = 0;
@@ -2720,8 +2734,7 @@ static struct ppp *ppp_create_interface(struct net *net, int unit,
 	int ret = -ENOMEM;
 	int i;
 
-	dev = alloc_netdev(sizeof(struct ppp), "", NET_NAME_UNKNOWN,
-			   ppp_setup);
+	dev = alloc_netdev(sizeof(struct ppp), "", NET_NAME_ENUM, ppp_setup);
 	if (!dev)
 		goto out1;
 
@@ -2803,6 +2816,7 @@ static struct ppp *ppp_create_interface(struct net *net, int unit,
 
 out2:
 	mutex_unlock(&pn->all_ppp_mutex);
+	rtnl_unlock();
 	free_netdev(dev);
 out1:
 	*retp = ret;
diff --git a/drivers/net/ppp/pppoe.c b/drivers/net/ppp/pppoe.c
index 0a37f840fcc5..4ddae8118c85 100644
--- a/drivers/net/ppp/pppoe.c
+++ b/drivers/net/ppp/pppoe.c
@@ -311,7 +311,7 @@ static void pppoe_flush_dev(struct net_device *dev)
 			lock_sock(sk);
 
 			if (po->pppoe_dev == dev &&
-			    sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND | PPPOX_ZOMBIE)) {
+			    sk->sk_state & (PPPOX_CONNECTED | PPPOX_BOUND)) {
 				pppox_unbind_sock(sk);
 				sk->sk_state_change(sk);
 				po->pppoe_dev = NULL;
@@ -395,6 +395,8 @@ static int pppoe_rcv_core(struct sock *sk, struct sk_buff *skb)
 
 		if (!__pppoe_xmit(sk_pppox(relay_po), skb))
 			goto abort_put;
+
+		sock_put(sk_pppox(relay_po));
 	} else {
 		if (sock_queue_rcv_skb(sk, skb))
 			goto abort_kfree;
@@ -500,27 +502,9 @@ static int pppoe_disc_rcv(struct sk_buff *skb, struct net_device *dev,
 
 	pn = pppoe_pernet(dev_net(dev));
 	po = get_item(pn, ph->sid, eth_hdr(skb)->h_source, dev->ifindex);
-	if (po) {
-		struct sock *sk = sk_pppox(po);
-
-		bh_lock_sock(sk);
-
-		/* If the user has locked the socket, just ignore
-		 * the packet.  With the way two rcv protocols hook into
-		 * one socket family type, we cannot (easily) distinguish
-		 * what kind of SKB it is during backlog rcv.
-		 */
-		if (sock_owned_by_user(sk) == 0) {
-			/* We're no longer connect at the PPPOE layer,
-			 * and must wait for ppp channel to disconnect us.
-			 */
-			sk->sk_state = PPPOX_ZOMBIE;
-		}
-
-		bh_unlock_sock(sk);
+	if (po)
 		if (!schedule_work(&po->proto.pppoe.padt_work))
-			sock_put(sk);
-	}
+			sock_put(sk_pppox(po));
 
 abort:
 	kfree_skb(skb);
@@ -799,7 +783,7 @@ static int pppoe_ioctl(struct socket *sock, unsigned int cmd,
 		struct pppox_sock *relay_po;
 
 		err = -EBUSY;
-		if (sk->sk_state & (PPPOX_BOUND | PPPOX_ZOMBIE | PPPOX_DEAD))
+		if (sk->sk_state & (PPPOX_BOUND | PPPOX_DEAD))
 			break;
 
 		err = -ENOTCONN;
@@ -1226,4 +1210,4 @@ module_exit(pppoe_exit);
 MODULE_AUTHOR("Michal Ostrowski <mostrows@speakeasy.net>");
 MODULE_DESCRIPTION("PPP over Ethernet driver");
 MODULE_LICENSE("GPL");
-MODULE_ALIAS_NETPROTO(PF_PPPOX);
+MODULE_ALIAS_NET_PF_PROTO(PF_PPPOX, PX_PROTO_OE);
diff --git a/drivers/net/ppp/pppox.c b/drivers/net/ppp/pppox.c
index 0e1b30622477..b9c8be6283d3 100644
--- a/drivers/net/ppp/pppox.c
+++ b/drivers/net/ppp/pppox.c
@@ -58,7 +58,7 @@ void pppox_unbind_sock(struct sock *sk)
 {
 	/* Clear connection to ppp device, if attached. */
 
-	if (sk->sk_state & (PPPOX_BOUND | PPPOX_CONNECTED | PPPOX_ZOMBIE)) {
+	if (sk->sk_state & (PPPOX_BOUND | PPPOX_CONNECTED)) {
 		ppp_unregister_channel(&pppox_sk(sk)->chan);
 		sk->sk_state = PPPOX_DEAD;
 	}
@@ -113,7 +113,7 @@ static int pppox_create(struct net *net, struct socket *sock, int protocol,
 
 	rc = -EPROTONOSUPPORT;
 	if (!pppox_protos[protocol])
-		request_module("pppox-proto-%d", protocol);
+		request_module("net-pf-%d-proto-%d", PF_PPPOX, protocol);
 	if (!pppox_protos[protocol] ||
 	    !try_module_get(pppox_protos[protocol]->owner))
 		goto out;
@@ -147,3 +147,4 @@ module_exit(pppox_exit);
 MODULE_AUTHOR("Michal Ostrowski <mostrows@speakeasy.net>");
 MODULE_DESCRIPTION("PPP over Ethernet driver (generic socket layer)");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS_NETPROTO(PF_PPPOX);
diff --git a/drivers/net/ppp/pptp.c b/drivers/net/ppp/pptp.c
index 597c53e0a2ec..ae0905ed4a32 100644
--- a/drivers/net/ppp/pptp.c
+++ b/drivers/net/ppp/pptp.c
@@ -129,24 +129,27 @@ static int lookup_chan_dst(u16 call_id, __be32 d_addr)
 	return i < MAX_CALLID;
 }
 
-static int add_chan(struct pppox_sock *sock)
+static int add_chan(struct pppox_sock *sock,
+		    struct pptp_addr *sa)
 {
 	static int call_id;
 
 	spin_lock(&chan_lock);
-	if (!sock->proto.pptp.src_addr.call_id)	{
+	if (!sa->call_id)	{
 		call_id = find_next_zero_bit(callid_bitmap, MAX_CALLID, call_id + 1);
 		if (call_id == MAX_CALLID) {
 			call_id = find_next_zero_bit(callid_bitmap, MAX_CALLID, 1);
 			if (call_id == MAX_CALLID)
 				goto out_err;
 		}
-		sock->proto.pptp.src_addr.call_id = call_id;
-	} else if (test_bit(sock->proto.pptp.src_addr.call_id, callid_bitmap))
+		sa->call_id = call_id;
+	} else if (test_bit(sa->call_id, callid_bitmap)) {
 		goto out_err;
+	}
 
-	set_bit(sock->proto.pptp.src_addr.call_id, callid_bitmap);
-	rcu_assign_pointer(callid_sock[sock->proto.pptp.src_addr.call_id], sock);
+	sock->proto.pptp.src_addr = *sa;
+	set_bit(sa->call_id, callid_bitmap);
+	rcu_assign_pointer(callid_sock[sa->call_id], sock);
 	spin_unlock(&chan_lock);
 
 	return 0;
@@ -416,7 +419,6 @@ static int pptp_bind(struct socket *sock, struct sockaddr *uservaddr,
 	struct sock *sk = sock->sk;
 	struct sockaddr_pppox *sp = (struct sockaddr_pppox *) uservaddr;
 	struct pppox_sock *po = pppox_sk(sk);
-	struct pptp_opt *opt = &po->proto.pptp;
 	int error = 0;
 
 	if (sockaddr_len < sizeof(struct sockaddr_pppox))
@@ -424,10 +426,22 @@ static int pptp_bind(struct socket *sock, struct sockaddr *uservaddr,
 
 	lock_sock(sk);
 
-	opt->src_addr = sp->sa_addr.pptp;
-	if (add_chan(po))
+	if (sk->sk_state & PPPOX_DEAD) {
+		error = -EALREADY;
+		goto out;
+	}
+
+	if (sk->sk_state & PPPOX_BOUND) {
 		error = -EBUSY;
+		goto out;
+	}
+
+	if (add_chan(po, &sp->sa_addr.pptp))
+		error = -EBUSY;
+	else
+		sk->sk_state |= PPPOX_BOUND;
 
+out:
 	release_sock(sk);
 	return error;
 }
@@ -498,7 +512,7 @@ static int pptp_connect(struct socket *sock, struct sockaddr *uservaddr,
 	}
 
 	opt->dst_addr = sp->sa_addr.pptp;
-	sk->sk_state = PPPOX_CONNECTED;
+	sk->sk_state |= PPPOX_CONNECTED;
 
  end:
 	release_sock(sk);
@@ -724,3 +738,4 @@ module_exit(pptp_exit_module);
 MODULE_DESCRIPTION("Point-to-Point Tunneling Protocol");
 MODULE_AUTHOR("D. Kozlov (xeb@mail.ru)");
 MODULE_LICENSE("GPL");
+MODULE_ALIAS_NET_PF_PROTO(PF_PPPOX, PX_PROTO_PPTP);
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index 651d35ea22c5..718ceeab4dbc 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -91,10 +91,24 @@ void team_modeop_port_change_dev_addr(struct team *team,
 }
 EXPORT_SYMBOL(team_modeop_port_change_dev_addr);
 
+static void team_lower_state_changed(struct team_port *port)
+{
+	struct netdev_lag_lower_state_info info;
+
+	info.link_up = port->linkup;
+	info.tx_enabled = team_port_enabled(port);
+	netdev_lower_state_changed(port->dev, &info);
+}
+
 static void team_refresh_port_linkup(struct team_port *port)
 {
-	port->linkup = port->user.linkup_enabled ? port->user.linkup :
-						   port->state.linkup;
+	bool new_linkup = port->user.linkup_enabled ? port->user.linkup :
+						      port->state.linkup;
+
+	if (port->linkup != new_linkup) {
+		port->linkup = new_linkup;
+		team_lower_state_changed(port);
+	}
 }
 
 
@@ -932,6 +946,7 @@ static void team_port_enable(struct team *team,
 		team->ops.port_enabled(team, port);
 	team_notify_peers(team);
 	team_mcast_rejoin(team);
+	team_lower_state_changed(port);
 }
 
 static void __reconstruct_port_hlist(struct team *team, int rm_index)
@@ -963,16 +978,21 @@ static void team_port_disable(struct team *team,
 	team_adjust_ops(team);
 	team_notify_peers(team);
 	team_mcast_rejoin(team);
+	team_lower_state_changed(port);
 }
 
-#define TEAM_VLAN_FEATURES (NETIF_F_ALL_CSUM | NETIF_F_SG | \
+#define TEAM_VLAN_FEATURES (NETIF_F_HW_CSUM | NETIF_F_SG | \
 			    NETIF_F_FRAGLIST | NETIF_F_ALL_TSO | \
 			    NETIF_F_HIGHDMA | NETIF_F_LRO)
 
+#define TEAM_ENC_FEATURES	(NETIF_F_HW_CSUM | NETIF_F_SG | \
+				 NETIF_F_RXCSUM | NETIF_F_ALL_TSO)
+
 static void __team_compute_features(struct team *team)
 {
 	struct team_port *port;
 	u32 vlan_features = TEAM_VLAN_FEATURES & NETIF_F_ALL_FOR_ALL;
+	netdev_features_t enc_features  = TEAM_ENC_FEATURES;
 	unsigned short max_hard_header_len = ETH_HLEN;
 	unsigned int dst_release_flag = IFF_XMIT_DST_RELEASE |
 					IFF_XMIT_DST_RELEASE_PERM;
@@ -981,6 +1001,11 @@ static void __team_compute_features(struct team *team)
 		vlan_features = netdev_increment_features(vlan_features,
 					port->dev->vlan_features,
 					TEAM_VLAN_FEATURES);
+		enc_features =
+			netdev_increment_features(enc_features,
+						  port->dev->hw_enc_features,
+						  TEAM_ENC_FEATURES);
+
 
 		dst_release_flag &= port->dev->priv_flags;
 		if (port->dev->hard_header_len > max_hard_header_len)
@@ -988,6 +1013,7 @@ static void __team_compute_features(struct team *team)
 	}
 
 	team->dev->vlan_features = vlan_features;
+	team->dev->hw_enc_features = enc_features | NETIF_F_GSO_ENCAP_ALL;
 	team->dev->hard_header_len = max_hard_header_len;
 
 	team->dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
@@ -1078,23 +1104,24 @@ static void team_port_disable_netpoll(struct team_port *port)
 }
 #endif
 
-static int team_upper_dev_link(struct net_device *dev,
-			       struct net_device *port_dev)
+static int team_upper_dev_link(struct team *team, struct team_port *port)
 {
+	struct netdev_lag_upper_info lag_upper_info;
 	int err;
 
-	err = netdev_master_upper_dev_link(port_dev, dev);
+	lag_upper_info.tx_type = team->mode->lag_tx_type;
+	err = netdev_master_upper_dev_link(port->dev, team->dev, NULL,
+					   &lag_upper_info);
 	if (err)
 		return err;
-	port_dev->priv_flags |= IFF_TEAM_PORT;
+	port->dev->priv_flags |= IFF_TEAM_PORT;
 	return 0;
 }
 
-static void team_upper_dev_unlink(struct net_device *dev,
-				  struct net_device *port_dev)
+static void team_upper_dev_unlink(struct team *team, struct team_port *port)
 {
-	netdev_upper_dev_unlink(port_dev, dev);
-	port_dev->priv_flags &= ~IFF_TEAM_PORT;
+	netdev_upper_dev_unlink(port->dev, team->dev);
+	port->dev->priv_flags &= ~IFF_TEAM_PORT;
 }
 
 static void __team_port_change_port_added(struct team_port *port, bool linkup);
@@ -1194,7 +1221,7 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_handler_register;
 	}
 
-	err = team_upper_dev_link(dev, port_dev);
+	err = team_upper_dev_link(team, port);
 	if (err) {
 		netdev_err(dev, "Device %s failed to set upper link\n",
 			   portname);
@@ -1220,7 +1247,7 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 	return 0;
 
 err_option_port_add:
-	team_upper_dev_unlink(dev, port_dev);
+	team_upper_dev_unlink(team, port);
 
 err_set_upper_link:
 	netdev_rx_handler_unregister(port_dev);
@@ -1264,7 +1291,7 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 
 	team_port_disable(team, port);
 	list_del_rcu(&port->list);
-	team_upper_dev_unlink(dev, port_dev);
+	team_upper_dev_unlink(team, port);
 	netdev_rx_handler_unregister(port_dev);
 	team_port_disable_netpoll(port);
 	vlan_vids_del_by_dev(port_dev, dev);
@@ -1845,10 +1872,10 @@ static int team_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
 	struct team *team = netdev_priv(dev);
 	struct team_port *port;
 
-	rcu_read_lock();
-	list_for_each_entry_rcu(port, &team->port_list, list)
+	mutex_lock(&team->lock);
+	list_for_each_entry(port, &team->port_list, list)
 		vlan_vid_del(port->dev, proto, vid);
-	rcu_read_unlock();
+	mutex_unlock(&team->lock);
 
 	return 0;
 }
@@ -2054,6 +2081,7 @@ static void team_setup(struct net_device *dev)
 	dev->flags |= IFF_MULTICAST;
 	dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
 	dev->priv_flags |= IFF_NO_QUEUE;
+	dev->priv_flags |= IFF_TEAM;
 
 	/*
 	 * Indicate we support unicast address filtering. That way core won't
@@ -2073,7 +2101,7 @@ static void team_setup(struct net_device *dev)
 			   NETIF_F_HW_VLAN_CTAG_RX |
 			   NETIF_F_HW_VLAN_CTAG_FILTER;
 
-	dev->hw_features &= ~(NETIF_F_ALL_CSUM & ~NETIF_F_HW_CSUM);
+	dev->hw_features |= NETIF_F_GSO_ENCAP_ALL;
 	dev->features |= dev->hw_features;
 }
 
@@ -2420,9 +2448,13 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 	struct nlattr *nl_option;
 	LIST_HEAD(opt_inst_list);
 
+	rtnl_lock();
+
 	team = team_nl_team_get(info);
-	if (!team)
-		return -EINVAL;
+	if (!team) {
+		err = -EINVAL;
+		goto rtnl_unlock;
+	}
 
 	err = -EINVAL;
 	if (!info->attrs[TEAM_ATTR_LIST_OPTION]) {
@@ -2549,7 +2581,8 @@ static int team_nl_cmd_options_set(struct sk_buff *skb, struct genl_info *info)
 
 team_put:
 	team_nl_team_put(team);
-
+rtnl_unlock:
+	rtnl_unlock();
 	return err;
 }
 
diff --git a/drivers/net/team/team_mode_activebackup.c b/drivers/net/team/team_mode_activebackup.c
index 40fd3381b693..3f189823ba3b 100644
--- a/drivers/net/team/team_mode_activebackup.c
+++ b/drivers/net/team/team_mode_activebackup.c
@@ -127,6 +127,7 @@ static const struct team_mode ab_mode = {
 	.owner		= THIS_MODULE,
 	.priv_size	= sizeof(struct ab_priv),
 	.ops		= &ab_mode_ops,
+	.lag_tx_type	= NETDEV_LAG_TX_TYPE_ACTIVEBACKUP,
 };
 
 static int __init ab_init_module(void)
diff --git a/drivers/net/team/team_mode_broadcast.c b/drivers/net/team/team_mode_broadcast.c
index c366cd299c06..302ff35b0cbc 100644
--- a/drivers/net/team/team_mode_broadcast.c
+++ b/drivers/net/team/team_mode_broadcast.c
@@ -56,6 +56,7 @@ static const struct team_mode bc_mode = {
 	.kind		= "broadcast",
 	.owner		= THIS_MODULE,
 	.ops		= &bc_mode_ops,
+	.lag_tx_type	= NETDEV_LAG_TX_TYPE_BROADCAST,
 };
 
 static int __init bc_init_module(void)
diff --git a/drivers/net/team/team_mode_loadbalance.c b/drivers/net/team/team_mode_loadbalance.c
index a1536d0d83a9..cdb19b385d42 100644
--- a/drivers/net/team/team_mode_loadbalance.c
+++ b/drivers/net/team/team_mode_loadbalance.c
@@ -661,6 +661,7 @@ static const struct team_mode lb_mode = {
 	.priv_size	= sizeof(struct lb_priv),
 	.port_priv_size	= sizeof(struct lb_port_priv),
 	.ops		= &lb_mode_ops,
+	.lag_tx_type	= NETDEV_LAG_TX_TYPE_HASH,
 };
 
 static int __init lb_init_module(void)
diff --git a/drivers/net/team/team_mode_random.c b/drivers/net/team/team_mode_random.c
index cd2f692b8074..215f845782db 100644
--- a/drivers/net/team/team_mode_random.c
+++ b/drivers/net/team/team_mode_random.c
@@ -46,6 +46,7 @@ static const struct team_mode rnd_mode = {
 	.kind		= "random",
 	.owner		= THIS_MODULE,
 	.ops		= &rnd_mode_ops,
+	.lag_tx_type	= NETDEV_LAG_TX_TYPE_RANDOM,
 };
 
 static int __init rnd_init_module(void)
diff --git a/drivers/net/team/team_mode_roundrobin.c b/drivers/net/team/team_mode_roundrobin.c
index 53665850b59e..0aa234118c03 100644
--- a/drivers/net/team/team_mode_roundrobin.c
+++ b/drivers/net/team/team_mode_roundrobin.c
@@ -58,6 +58,7 @@ static const struct team_mode rr_mode = {
 	.owner		= THIS_MODULE,
 	.priv_size	= sizeof(struct rr_priv),
 	.ops		= &rr_mode_ops,
+	.lag_tx_type	= NETDEV_LAG_TX_TYPE_ROUNDROBIN,
 };
 
 static int __init rr_init_module(void)
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index f0db770e8b2f..88bb8cc3555b 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -1095,6 +1095,9 @@ static ssize_t tun_get_user(struct tun_struct *tun, struct tun_file *tfile,
 	u32 rxhash;
 	ssize_t n;
 
+	if (!(tun->dev->flags & IFF_UP))
+		return -EIO;
+
 	if (!(tun->flags & IFF_NO_PI)) {
 		if (len < sizeof(pi))
 			return -EINVAL;
diff --git a/drivers/net/usb/Kconfig b/drivers/net/usb/Kconfig
index 7f83504dfa69..cdde59089f72 100644
--- a/drivers/net/usb/Kconfig
+++ b/drivers/net/usb/Kconfig
@@ -395,6 +395,10 @@ config USB_NET_RNDIS_HOST
 	  The protocol specification is incomplete, and is controlled by
 	  (and for) Microsoft; it isn't an "Open" ecosystem or market.
 
+config USB_NET_CDC_SUBSET_ENABLE
+	tristate
+	depends on USB_NET_CDC_SUBSET
+
 config USB_NET_CDC_SUBSET
 	tristate "Simple USB Network Links (CDC Ethernet subset)"
 	depends on USB_USBNET
@@ -413,6 +417,7 @@ config USB_NET_CDC_SUBSET
 config USB_ALI_M5632
 	bool "ALi M5632 based 'USB 2.0 Data Link' cables"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	help
 	  Choose this option if you're using a host-to-host cable
 	  based on this design, which supports USB 2.0 high speed.
@@ -420,6 +425,7 @@ config USB_ALI_M5632
 config USB_AN2720
 	bool "AnchorChips 2720 based cables (Xircom PGUNET, ...)"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	help
 	  Choose this option if you're using a host-to-host cable
 	  based on this design.  Note that AnchorChips is now a
@@ -428,6 +434,7 @@ config USB_AN2720
 config USB_BELKIN
 	bool "eTEK based host-to-host cables (Advance, Belkin, ...)"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	default y
 	help
 	  Choose this option if you're using a host-to-host cable
@@ -437,6 +444,7 @@ config USB_BELKIN
 config USB_ARMLINUX
 	bool "Embedded ARM Linux links (iPaq, ...)"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	default y
 	help
 	  Choose this option to support the "usb-eth" networking driver
@@ -454,6 +462,7 @@ config USB_ARMLINUX
 config USB_EPSON2888
 	bool "Epson 2888 based firmware (DEVELOPMENT)"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	help
 	  Choose this option to support the usb networking links used
 	  by some sample firmware from Epson.
@@ -461,6 +470,7 @@ config USB_EPSON2888
 config USB_KC2190
 	bool "KT Technology KC2190 based cables (InstaNet)"
 	depends on USB_NET_CDC_SUBSET
+	select USB_NET_CDC_SUBSET_ENABLE
 	help
 	  Choose this option if you're using a host-to-host cable
 	  with one of these chips.
diff --git a/drivers/net/usb/Makefile b/drivers/net/usb/Makefile
index b5f04068dbe4..37fb46aee341 100644
--- a/drivers/net/usb/Makefile
+++ b/drivers/net/usb/Makefile
@@ -23,7 +23,7 @@ obj-$(CONFIG_USB_NET_GL620A)	+= gl620a.o
 obj-$(CONFIG_USB_NET_NET1080)	+= net1080.o
 obj-$(CONFIG_USB_NET_PLUSB)	+= plusb.o
 obj-$(CONFIG_USB_NET_RNDIS_HOST)	+= rndis_host.o
-obj-$(CONFIG_USB_NET_CDC_SUBSET)	+= cdc_subset.o
+obj-$(CONFIG_USB_NET_CDC_SUBSET_ENABLE)	+= cdc_subset.o
 obj-$(CONFIG_USB_NET_ZAURUS)	+= zaurus.o
 obj-$(CONFIG_USB_NET_MCS7830)	+= mcs7830.o
 obj-$(CONFIG_USB_USBNET)	+= usbnet.o
diff --git a/drivers/net/usb/asix_common.c b/drivers/net/usb/asix_common.c
index bd9acff1eb7b..0c5c22b84da8 100644
--- a/drivers/net/usb/asix_common.c
+++ b/drivers/net/usb/asix_common.c
@@ -118,7 +118,7 @@ int asix_rx_fixup_internal(struct usbnet *dev, struct sk_buff *skb,
 				return 0;
 			}
 			if (size > dev->net->mtu + ETH_HLEN + VLAN_HLEN) {
-				netdev_err(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
+				netdev_dbg(dev->net, "asix_rx_fixup() Bad RX Length %d\n",
 					   size);
 				return 0;
 			}
diff --git a/drivers/net/usb/ax88172a.c b/drivers/net/usb/ax88172a.c
index 5f18fcb8dcc7..cf77f2dffa69 100644
--- a/drivers/net/usb/ax88172a.c
+++ b/drivers/net/usb/ax88172a.c
@@ -98,7 +98,7 @@ static void ax88172a_status(struct usbnet *dev, struct urb *urb)
 static int ax88172a_init_mdio(struct usbnet *dev)
 {
 	struct ax88172a_private *priv = dev->driver_priv;
-	int ret, i;
+	int ret;
 
 	priv->mdio = mdiobus_alloc();
 	if (!priv->mdio) {
@@ -114,25 +114,15 @@ static int ax88172a_init_mdio(struct usbnet *dev)
 	snprintf(priv->mdio->id, MII_BUS_ID_SIZE, "usb-%03d:%03d",
 		 dev->udev->bus->busnum, dev->udev->devnum);
 
-	priv->mdio->irq = kzalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!priv->mdio->irq) {
-		ret = -ENOMEM;
-		goto mfree;
-	}
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		priv->mdio->irq[i] = PHY_POLL;
-
 	ret = mdiobus_register(priv->mdio);
 	if (ret) {
 		netdev_err(dev->net, "Could not register MDIO bus\n");
-		goto ifree;
+		goto mfree;
 	}
 
 	netdev_info(dev->net, "registered mdio bus %s\n", priv->mdio->id);
 	return 0;
 
-ifree:
-	kfree(priv->mdio->irq);
 mfree:
 	mdiobus_free(priv->mdio);
 	return ret;
@@ -144,7 +134,6 @@ static void ax88172a_remove_mdio(struct usbnet *dev)
 
 	netdev_info(dev->net, "deregistering mdio bus %s\n", priv->mdio->id);
 	mdiobus_unregister(priv->mdio);
-	kfree(priv->mdio->irq);
 	mdiobus_free(priv->mdio);
 }
 
diff --git a/drivers/net/usb/cdc_ether.c b/drivers/net/usb/cdc_ether.c
index 3da70bf9936a..7cba2c3759df 100644
--- a/drivers/net/usb/cdc_ether.c
+++ b/drivers/net/usb/cdc_ether.c
@@ -160,6 +160,12 @@ int usbnet_generic_cdc_bind(struct usbnet *dev, struct usb_interface *intf)
 	info->u = header.usb_cdc_union_desc;
 	info->header = header.usb_cdc_header_desc;
 	info->ether = header.usb_cdc_ether_desc;
+	if (!info->u) {
+		if (rndis)
+			goto skip;
+		else /* in that case a quirk is mandatory */
+			goto bad_desc;
+	}
 	/* we need a master/control interface (what we're
 	 * probed with) and a slave/data interface; union
 	 * descriptors sort this all out.
@@ -256,7 +262,7 @@ skip:
 			goto bad_desc;
 		}
 
-	} else if (!info->header || !info->u || (!rndis && !info->ether)) {
+	} else if (!info->header || (!rndis && !info->ether)) {
 		dev_dbg(&intf->dev, "missing cdc %s%s%sdescriptor\n",
 			info->header ? "" : "header ",
 			info->u ? "" : "union ",
diff --git a/drivers/net/usb/cdc_ncm.c b/drivers/net/usb/cdc_ncm.c
index e8a1144c5a8b..86ba30ba35e8 100644
--- a/drivers/net/usb/cdc_ncm.c
+++ b/drivers/net/usb/cdc_ncm.c
@@ -284,6 +284,48 @@ static DEVICE_ATTR(rx_max, S_IRUGO | S_IWUSR, cdc_ncm_show_rx_max, cdc_ncm_store
 static DEVICE_ATTR(tx_max, S_IRUGO | S_IWUSR, cdc_ncm_show_tx_max, cdc_ncm_store_tx_max);
 static DEVICE_ATTR(tx_timer_usecs, S_IRUGO | S_IWUSR, cdc_ncm_show_tx_timer_usecs, cdc_ncm_store_tx_timer_usecs);
 
+static ssize_t ndp_to_end_show(struct device *d, struct device_attribute *attr, char *buf)
+{
+	struct usbnet *dev = netdev_priv(to_net_dev(d));
+	struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)dev->data[0];
+
+	return sprintf(buf, "%c\n", ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END ? 'Y' : 'N');
+}
+
+static ssize_t ndp_to_end_store(struct device *d,  struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct usbnet *dev = netdev_priv(to_net_dev(d));
+	struct cdc_ncm_ctx *ctx = (struct cdc_ncm_ctx *)dev->data[0];
+	bool enable;
+
+	if (strtobool(buf, &enable))
+		return -EINVAL;
+
+	/* no change? */
+	if (enable == (ctx->drvflags & CDC_NCM_FLAG_NDP_TO_END))
+		return len;
+
+	if (enable && !ctx->delayed_ndp16) {
+		ctx->delayed_ndp16 = kzalloc(ctx->max_ndp_size, GFP_KERNEL);
+		if (!ctx->delayed_ndp16)
+			return -ENOMEM;
+	}
+
+	/* flush pending data before changing flag */
+	netif_tx_lock_bh(dev->net);
+	usbnet_start_xmit(NULL, dev->net);
+	spin_lock_bh(&ctx->mtx);
+	if (enable)
+		ctx->drvflags |= CDC_NCM_FLAG_NDP_TO_END;
+	else
+		ctx->drvflags &= ~CDC_NCM_FLAG_NDP_TO_END;
+	spin_unlock_bh(&ctx->mtx);
+	netif_tx_unlock_bh(dev->net);
+
+	return len;
+}
+static DEVICE_ATTR_RW(ndp_to_end);
+
 #define NCM_PARM_ATTR(name, format, tocpu)				\
 static ssize_t cdc_ncm_show_##name(struct device *d, struct device_attribute *attr, char *buf) \
 { \
@@ -306,6 +348,7 @@ NCM_PARM_ATTR(wNtbOutMaxDatagrams, "%u", le16_to_cpu);
 
 static struct attribute *cdc_ncm_sysfs_attrs[] = {
 	&dev_attr_min_tx_pkt.attr,
+	&dev_attr_ndp_to_end.attr,
 	&dev_attr_rx_max.attr,
 	&dev_attr_tx_max.attr,
 	&dev_attr_tx_timer_usecs.attr,
@@ -794,7 +837,11 @@ int cdc_ncm_bind_common(struct usbnet *dev, struct usb_interface *intf, u8 data_
 
 	iface_no = ctx->data->cur_altsetting->desc.bInterfaceNumber;
 
-	/* reset data interface */
+	/* Reset data interface. Some devices will not reset properly
+	 * unless they are configured first.  Toggle the altsetting to
+	 * force a reset
+	 */
+	usb_set_interface(dev->udev, iface_no, data_altsetting);
 	temp = usb_set_interface(dev->udev, iface_no, 0);
 	if (temp) {
 		dev_dbg(&intf->dev, "set interface failed\n");
@@ -941,8 +988,6 @@ EXPORT_SYMBOL_GPL(cdc_ncm_select_altsetting);
 
 static int cdc_ncm_bind(struct usbnet *dev, struct usb_interface *intf)
 {
-	int ret;
-
 	/* MBIM backwards compatible function? */
 	if (cdc_ncm_select_altsetting(intf) != CDC_NCM_COMM_ALTSETTING_NCM)
 		return -ENODEV;
@@ -951,16 +996,7 @@ static int cdc_ncm_bind(struct usbnet *dev, struct usb_interface *intf)
 	 * Additionally, generic NCM devices are assumed to accept arbitrarily
 	 * placed NDP.
 	 */
-	ret = cdc_ncm_bind_common(dev, intf, CDC_NCM_DATA_ALTSETTING_NCM, 0);
-
-	/*
-	 * We should get an event when network connection is "connected" or
-	 * "disconnected". Set network connection in "disconnected" state
-	 * (carrier is OFF) during attach, so the IP network stack does not
-	 * start IPv6 negotiation and more.
-	 */
-	usbnet_link_change(dev, 0, 0);
-	return ret;
+	return cdc_ncm_bind_common(dev, intf, CDC_NCM_DATA_ALTSETTING_NCM, 0);
 }
 
 static void cdc_ncm_align_tail(struct sk_buff *skb, size_t modulus, size_t remainder, size_t max)
@@ -1543,7 +1579,8 @@ static void cdc_ncm_status(struct usbnet *dev, struct urb *urb)
 
 static const struct driver_info cdc_ncm_info = {
 	.description = "CDC NCM",
-	.flags = FLAG_POINTTOPOINT | FLAG_NO_SETINT | FLAG_MULTI_PACKET,
+	.flags = FLAG_POINTTOPOINT | FLAG_NO_SETINT | FLAG_MULTI_PACKET
+			| FLAG_LINK_INTR,
 	.bind = cdc_ncm_bind,
 	.unbind = cdc_ncm_unbind,
 	.manage_power = usbnet_manage_power,
@@ -1556,7 +1593,7 @@ static const struct driver_info cdc_ncm_info = {
 static const struct driver_info wwan_info = {
 	.description = "Mobile Broadband Network Device",
 	.flags = FLAG_POINTTOPOINT | FLAG_NO_SETINT | FLAG_MULTI_PACKET
-			| FLAG_WWAN,
+			| FLAG_LINK_INTR | FLAG_WWAN,
 	.bind = cdc_ncm_bind,
 	.unbind = cdc_ncm_unbind,
 	.manage_power = usbnet_manage_power,
@@ -1569,7 +1606,7 @@ static const struct driver_info wwan_info = {
 static const struct driver_info wwan_noarp_info = {
 	.description = "Mobile Broadband Network Device (NO ARP)",
 	.flags = FLAG_POINTTOPOINT | FLAG_NO_SETINT | FLAG_MULTI_PACKET
-			| FLAG_WWAN | FLAG_NOARP,
+			| FLAG_LINK_INTR | FLAG_WWAN | FLAG_NOARP,
 	.bind = cdc_ncm_bind,
 	.unbind = cdc_ncm_unbind,
 	.manage_power = usbnet_manage_power,
diff --git a/drivers/net/usb/lan78xx.c b/drivers/net/usb/lan78xx.c
index 226668ead0d8..1c299b8a162d 100644
--- a/drivers/net/usb/lan78xx.c
+++ b/drivers/net/usb/lan78xx.c
@@ -36,7 +36,7 @@
 #define DRIVER_AUTHOR	"WOOJUNG HUH <woojung.huh@microchip.com>"
 #define DRIVER_DESC	"LAN78XX USB 3.0 Gigabit Ethernet Devices"
 #define DRIVER_NAME	"lan78xx"
-#define DRIVER_VERSION	"1.0.1"
+#define DRIVER_VERSION	"1.0.2"
 
 #define TX_TIMEOUT_JIFFIES		(5 * HZ)
 #define THROTTLE_JIFFIES		(HZ / 8)
@@ -462,32 +462,53 @@ static int lan78xx_read_raw_eeprom(struct lan78xx_net *dev, u32 offset,
 				   u32 length, u8 *data)
 {
 	u32 val;
+	u32 saved;
 	int i, ret;
+	int retval;
 
-	ret = lan78xx_eeprom_confirm_not_busy(dev);
-	if (ret)
-		return ret;
+	/* depends on chip, some EEPROM pins are muxed with LED function.
+	 * disable & restore LED function to access EEPROM.
+	 */
+	ret = lan78xx_read_reg(dev, HW_CFG, &val);
+	saved = val;
+	if ((dev->devid & ID_REV_CHIP_ID_MASK_) == 0x78000000) {
+		val &= ~(HW_CFG_LED1_EN_ | HW_CFG_LED0_EN_);
+		ret = lan78xx_write_reg(dev, HW_CFG, val);
+	}
+
+	retval = lan78xx_eeprom_confirm_not_busy(dev);
+	if (retval)
+		return retval;
 
 	for (i = 0; i < length; i++) {
 		val = E2P_CMD_EPC_BUSY_ | E2P_CMD_EPC_CMD_READ_;
 		val |= (offset & E2P_CMD_EPC_ADDR_MASK_);
 		ret = lan78xx_write_reg(dev, E2P_CMD, val);
-		if (unlikely(ret < 0))
-			return -EIO;
+		if (unlikely(ret < 0)) {
+			retval = -EIO;
+			goto exit;
+		}
 
-		ret = lan78xx_wait_eeprom(dev);
-		if (ret < 0)
-			return ret;
+		retval = lan78xx_wait_eeprom(dev);
+		if (retval < 0)
+			goto exit;
 
 		ret = lan78xx_read_reg(dev, E2P_DATA, &val);
-		if (unlikely(ret < 0))
-			return -EIO;
+		if (unlikely(ret < 0)) {
+			retval = -EIO;
+			goto exit;
+		}
 
 		data[i] = val & 0xFF;
 		offset++;
 	}
 
-	return 0;
+	retval = 0;
+exit:
+	if ((dev->devid & ID_REV_CHIP_ID_MASK_) == 0x78000000)
+		ret = lan78xx_write_reg(dev, HW_CFG, saved);
+
+	return retval;
 }
 
 static int lan78xx_read_eeprom(struct lan78xx_net *dev, u32 offset,
@@ -509,44 +530,67 @@ static int lan78xx_write_raw_eeprom(struct lan78xx_net *dev, u32 offset,
 				    u32 length, u8 *data)
 {
 	u32 val;
+	u32 saved;
 	int i, ret;
+	int retval;
 
-	ret = lan78xx_eeprom_confirm_not_busy(dev);
-	if (ret)
-		return ret;
+	/* depends on chip, some EEPROM pins are muxed with LED function.
+	 * disable & restore LED function to access EEPROM.
+	 */
+	ret = lan78xx_read_reg(dev, HW_CFG, &val);
+	saved = val;
+	if ((dev->devid & ID_REV_CHIP_ID_MASK_) == 0x78000000) {
+		val &= ~(HW_CFG_LED1_EN_ | HW_CFG_LED0_EN_);
+		ret = lan78xx_write_reg(dev, HW_CFG, val);
+	}
+
+	retval = lan78xx_eeprom_confirm_not_busy(dev);
+	if (retval)
+		goto exit;
 
 	/* Issue write/erase enable command */
 	val = E2P_CMD_EPC_BUSY_ | E2P_CMD_EPC_CMD_EWEN_;
 	ret = lan78xx_write_reg(dev, E2P_CMD, val);
-	if (unlikely(ret < 0))
-		return -EIO;
+	if (unlikely(ret < 0)) {
+		retval = -EIO;
+		goto exit;
+	}
 
-	ret = lan78xx_wait_eeprom(dev);
-	if (ret < 0)
-		return ret;
+	retval = lan78xx_wait_eeprom(dev);
+	if (retval < 0)
+		goto exit;
 
 	for (i = 0; i < length; i++) {
 		/* Fill data register */
 		val = data[i];
 		ret = lan78xx_write_reg(dev, E2P_DATA, val);
-		if (ret < 0)
-			return ret;
+		if (ret < 0) {
+			retval = -EIO;
+			goto exit;
+		}
 
 		/* Send "write" command */
 		val = E2P_CMD_EPC_BUSY_ | E2P_CMD_EPC_CMD_WRITE_;
 		val |= (offset & E2P_CMD_EPC_ADDR_MASK_);
 		ret = lan78xx_write_reg(dev, E2P_CMD, val);
-		if (ret < 0)
-			return ret;
+		if (ret < 0) {
+			retval = -EIO;
+			goto exit;
+		}
 
-		ret = lan78xx_wait_eeprom(dev);
-		if (ret < 0)
-			return ret;
+		retval = lan78xx_wait_eeprom(dev);
+		if (retval < 0)
+			goto exit;
 
 		offset++;
 	}
 
-	return 0;
+	retval = 0;
+exit:
+	if ((dev->devid & ID_REV_CHIP_ID_MASK_) == 0x78000000)
+		ret = lan78xx_write_reg(dev, HW_CFG, saved);
+
+	return retval;
 }
 
 static int lan78xx_read_raw_otp(struct lan78xx_net *dev, u32 offset,
@@ -603,6 +647,59 @@ static int lan78xx_read_raw_otp(struct lan78xx_net *dev, u32 offset,
 	return 0;
 }
 
+static int lan78xx_write_raw_otp(struct lan78xx_net *dev, u32 offset,
+				 u32 length, u8 *data)
+{
+	int i;
+	int ret;
+	u32 buf;
+	unsigned long timeout;
+
+	ret = lan78xx_read_reg(dev, OTP_PWR_DN, &buf);
+
+	if (buf & OTP_PWR_DN_PWRDN_N_) {
+		/* clear it and wait to be cleared */
+		ret = lan78xx_write_reg(dev, OTP_PWR_DN, 0);
+
+		timeout = jiffies + HZ;
+		do {
+			udelay(1);
+			ret = lan78xx_read_reg(dev, OTP_PWR_DN, &buf);
+			if (time_after(jiffies, timeout)) {
+				netdev_warn(dev->net,
+					    "timeout on OTP_PWR_DN completion");
+				return -EIO;
+			}
+		} while (buf & OTP_PWR_DN_PWRDN_N_);
+	}
+
+	/* set to BYTE program mode */
+	ret = lan78xx_write_reg(dev, OTP_PRGM_MODE, OTP_PRGM_MODE_BYTE_);
+
+	for (i = 0; i < length; i++) {
+		ret = lan78xx_write_reg(dev, OTP_ADDR1,
+					((offset + i) >> 8) & OTP_ADDR1_15_11);
+		ret = lan78xx_write_reg(dev, OTP_ADDR2,
+					((offset + i) & OTP_ADDR2_10_3));
+		ret = lan78xx_write_reg(dev, OTP_PRGM_DATA, data[i]);
+		ret = lan78xx_write_reg(dev, OTP_TST_CMD, OTP_TST_CMD_PRGVRFY_);
+		ret = lan78xx_write_reg(dev, OTP_CMD_GO, OTP_CMD_GO_GO_);
+
+		timeout = jiffies + HZ;
+		do {
+			udelay(1);
+			ret = lan78xx_read_reg(dev, OTP_STATUS, &buf);
+			if (time_after(jiffies, timeout)) {
+				netdev_warn(dev->net,
+					    "Timeout on OTP_STATUS completion");
+				return -EIO;
+			}
+		} while (buf & OTP_STATUS_BUSY_);
+	}
+
+	return 0;
+}
+
 static int lan78xx_read_otp(struct lan78xx_net *dev, u32 offset,
 			    u32 length, u8 *data)
 {
@@ -851,7 +948,6 @@ static int lan78xx_link_reset(struct lan78xx_net *dev)
 
 	if (!phydev->link && dev->link_on) {
 		dev->link_on = false;
-		netif_carrier_off(dev->net);
 
 		/* reset MAC */
 		ret = lan78xx_read_reg(dev, MAC_CR, &buf);
@@ -861,6 +957,8 @@ static int lan78xx_link_reset(struct lan78xx_net *dev)
 		ret = lan78xx_write_reg(dev, MAC_CR, buf);
 		if (unlikely(ret < 0))
 			return -EIO;
+
+		phy_mac_interrupt(phydev, 0);
 	} else if (phydev->link && !dev->link_on) {
 		dev->link_on = true;
 
@@ -900,7 +998,7 @@ static int lan78xx_link_reset(struct lan78xx_net *dev)
 			  ethtool_cmd_speed(&ecmd), ecmd.duplex, ladv, radv);
 
 		ret = lan78xx_update_flowcontrol(dev, ecmd.duplex, ladv, radv);
-		netif_carrier_on(dev->net);
+		phy_mac_interrupt(phydev, 1);
 	}
 
 	return ret;
@@ -969,7 +1067,7 @@ static int lan78xx_ethtool_set_eeprom(struct net_device *netdev,
 		 (ee->offset == 0) &&
 		 (ee->len == 512) &&
 		 (data[0] == OTP_INDICATOR_1))
-		return lan78xx_write_raw_eeprom(dev, ee->offset, ee->len, data);
+		return lan78xx_write_raw_otp(dev, ee->offset, ee->len, data);
 
 	return -EINVAL;
 }
@@ -1442,7 +1540,6 @@ done:
 static int lan78xx_mdio_init(struct lan78xx_net *dev)
 {
 	int ret;
-	int i;
 
 	dev->mdiobus = mdiobus_alloc();
 	if (!dev->mdiobus) {
@@ -1458,16 +1555,6 @@ static int lan78xx_mdio_init(struct lan78xx_net *dev)
 	snprintf(dev->mdiobus->id, MII_BUS_ID_SIZE, "usb-%03d:%03d",
 		 dev->udev->bus->busnum, dev->udev->devnum);
 
-	dev->mdiobus->irq = kzalloc(sizeof(int) * PHY_MAX_ADDR, GFP_KERNEL);
-	if (!dev->mdiobus->irq) {
-		ret = -ENOMEM;
-		goto exit1;
-	}
-
-	/* handle our own interrupt */
-	for (i = 0; i < PHY_MAX_ADDR; i++)
-		dev->mdiobus->irq[i] = PHY_IGNORE_INTERRUPT;
-
 	switch (dev->devid & ID_REV_CHIP_ID_MASK_) {
 	case 0x78000000:
 	case 0x78500000:
@@ -1479,13 +1566,11 @@ static int lan78xx_mdio_init(struct lan78xx_net *dev)
 	ret = mdiobus_register(dev->mdiobus);
 	if (ret) {
 		netdev_err(dev->net, "can't register MDIO bus\n");
-		goto exit2;
+		goto exit1;
 	}
 
 	netdev_dbg(dev->net, "registered mdiobus bus %s\n", dev->mdiobus->id);
 	return 0;
-exit2:
-	kfree(dev->mdiobus->irq);
 exit1:
 	mdiobus_free(dev->mdiobus);
 	return ret;
@@ -1494,7 +1579,6 @@ exit1:
 static void lan78xx_remove_mdio(struct lan78xx_net *dev)
 {
 	mdiobus_unregister(dev->mdiobus);
-	kfree(dev->mdiobus->irq);
 	mdiobus_free(dev->mdiobus);
 }
 
@@ -1514,6 +1598,16 @@ static int lan78xx_phy_init(struct lan78xx_net *dev)
 		return -EIO;
 	}
 
+	/* Enable PHY interrupts.
+	 * We handle our own interrupt
+	 */
+	ret = phy_read(phydev, LAN88XX_INT_STS);
+	ret = phy_write(phydev, LAN88XX_INT_MASK,
+			LAN88XX_INT_MASK_MDINTPIN_EN_ |
+			LAN88XX_INT_MASK_LINK_CHANGE_);
+
+	phydev->irq = PHY_IGNORE_INTERRUPT;
+
 	ret = phy_connect_direct(dev->net, phydev,
 				 lan78xx_link_status_change,
 				 PHY_INTERFACE_MODE_GMII);
@@ -1536,14 +1630,6 @@ static int lan78xx_phy_init(struct lan78xx_net *dev)
 			      SUPPORTED_Pause | SUPPORTED_Asym_Pause);
 	genphy_config_aneg(phydev);
 
-	/* Workaround to enable PHY interrupt.
-	 * phy_start_interrupts() is API for requesting and enabling
-	 * PHY interrupt. However, USB-to-Ethernet device can't use
-	 * request_irq() called in phy_start_interrupts().
-	 * Set PHY to PHY_HALTED and call phy_start()
-	 * to make a call to phy_enable_interrupts()
-	 */
-	phy_stop(phydev);
 	phy_start(phydev);
 
 	netif_dbg(dev, ifup, dev->net, "phy initialised successfully");
@@ -2177,7 +2263,9 @@ netdev_tx_t lan78xx_start_xmit(struct sk_buff *skb, struct net_device *net)
 	if (skb2) {
 		skb_queue_tail(&dev->txq_pend, skb2);
 
-		if (skb_queue_len(&dev->txq_pend) > 10)
+		/* throttle TX patch at slower than SUPER SPEED USB */
+		if ((dev->udev->speed < USB_SPEED_SUPER) &&
+		    (skb_queue_len(&dev->txq_pend) > 10))
 			netif_stop_queue(net);
 	} else {
 		netif_dbg(dev, tx_err, dev->net,
diff --git a/drivers/net/usb/qmi_wwan.c b/drivers/net/usb/qmi_wwan.c
index 5fccc5a8153f..a3a4ccf7cf52 100644
--- a/drivers/net/usb/qmi_wwan.c
+++ b/drivers/net/usb/qmi_wwan.c
@@ -14,7 +14,9 @@
 #include <linux/netdevice.h>
 #include <linux/ethtool.h>
 #include <linux/etherdevice.h>
+#include <linux/if_arp.h>
 #include <linux/mii.h>
+#include <linux/rtnetlink.h>
 #include <linux/usb.h>
 #include <linux/usb/cdc.h>
 #include <linux/usb/usbnet.h>
@@ -48,11 +50,100 @@
 struct qmi_wwan_state {
 	struct usb_driver *subdriver;
 	atomic_t pmcount;
-	unsigned long unused;
+	unsigned long flags;
 	struct usb_interface *control;
 	struct usb_interface *data;
 };
 
+enum qmi_wwan_flags {
+	QMI_WWAN_FLAG_RAWIP = 1 << 0,
+};
+
+static void qmi_wwan_netdev_setup(struct net_device *net)
+{
+	struct usbnet *dev = netdev_priv(net);
+	struct qmi_wwan_state *info = (void *)&dev->data;
+
+	if (info->flags & QMI_WWAN_FLAG_RAWIP) {
+		net->header_ops      = NULL;  /* No header */
+		net->type            = ARPHRD_NONE;
+		net->hard_header_len = 0;
+		net->addr_len        = 0;
+		net->flags           = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
+		netdev_dbg(net, "mode: raw IP\n");
+	} else if (!net->header_ops) { /* don't bother if already set */
+		ether_setup(net);
+		netdev_dbg(net, "mode: Ethernet\n");
+	}
+
+	/* recalculate buffers after changing hard_header_len */
+	usbnet_change_mtu(net, net->mtu);
+}
+
+static ssize_t raw_ip_show(struct device *d, struct device_attribute *attr, char *buf)
+{
+	struct usbnet *dev = netdev_priv(to_net_dev(d));
+	struct qmi_wwan_state *info = (void *)&dev->data;
+
+	return sprintf(buf, "%c\n", info->flags & QMI_WWAN_FLAG_RAWIP ? 'Y' : 'N');
+}
+
+static ssize_t raw_ip_store(struct device *d,  struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct usbnet *dev = netdev_priv(to_net_dev(d));
+	struct qmi_wwan_state *info = (void *)&dev->data;
+	bool enable;
+	int ret;
+
+	if (strtobool(buf, &enable))
+		return -EINVAL;
+
+	/* no change? */
+	if (enable == (info->flags & QMI_WWAN_FLAG_RAWIP))
+		return len;
+
+	if (!rtnl_trylock())
+		return restart_syscall();
+
+	/* we don't want to modify a running netdev */
+	if (netif_running(dev->net)) {
+		netdev_err(dev->net, "Cannot change a running device\n");
+		ret = -EBUSY;
+		goto err;
+	}
+
+	/* let other drivers deny the change */
+	ret = call_netdevice_notifiers(NETDEV_PRE_TYPE_CHANGE, dev->net);
+	ret = notifier_to_errno(ret);
+	if (ret) {
+		netdev_err(dev->net, "Type change was refused\n");
+		goto err;
+	}
+
+	if (enable)
+		info->flags |= QMI_WWAN_FLAG_RAWIP;
+	else
+		info->flags &= ~QMI_WWAN_FLAG_RAWIP;
+	qmi_wwan_netdev_setup(dev->net);
+	call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev->net);
+	ret = len;
+err:
+	rtnl_unlock();
+	return ret;
+}
+
+static DEVICE_ATTR_RW(raw_ip);
+
+static struct attribute *qmi_wwan_sysfs_attrs[] = {
+	&dev_attr_raw_ip.attr,
+	NULL,
+};
+
+static struct attribute_group qmi_wwan_sysfs_attr_group = {
+	.name = "qmi",
+	.attrs = qmi_wwan_sysfs_attrs,
+};
+
 /* default ethernet address used by the modem */
 static const u8 default_modem_addr[ETH_ALEN] = {0x02, 0x50, 0xf3};
 
@@ -80,6 +171,8 @@ static const u8 buggy_fw_addr[ETH_ALEN] = {0x00, 0xa0, 0xc6, 0x00, 0x00, 0x00};
  */
 static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 {
+	struct qmi_wwan_state *info = (void *)&dev->data;
+	bool rawip = info->flags & QMI_WWAN_FLAG_RAWIP;
 	__be16 proto;
 
 	/* This check is no longer done by usbnet */
@@ -94,15 +187,25 @@ static int qmi_wwan_rx_fixup(struct usbnet *dev, struct sk_buff *skb)
 		proto = htons(ETH_P_IPV6);
 		break;
 	case 0x00:
+		if (rawip)
+			return 0;
 		if (is_multicast_ether_addr(skb->data))
 			return 1;
 		/* possibly bogus destination - rewrite just in case */
 		skb_reset_mac_header(skb);
 		goto fix_dest;
 	default:
+		if (rawip)
+			return 0;
 		/* pass along other packets without modifications */
 		return 1;
 	}
+	if (rawip) {
+		skb->dev = dev->net; /* normally set by eth_type_trans */
+		skb->protocol = proto;
+		return 1;
+	}
+
 	if (skb_headroom(skb) < ETH_HLEN)
 		return 0;
 	skb_push(skb, ETH_HLEN);
@@ -223,6 +326,20 @@ err:
 	return rv;
 }
 
+/* Send CDC SetControlLineState request, setting or clearing the DTR.
+ * "Required for Autoconnect and 9x30 to wake up" according to the
+ * GobiNet driver. The requirement has been verified on an MDM9230
+ * based Sierra Wireless MC7455
+ */
+static int qmi_wwan_change_dtr(struct usbnet *dev, bool on)
+{
+	u8 intf = dev->intf->cur_altsetting->desc.bInterfaceNumber;
+
+	return usbnet_write_cmd(dev, USB_CDC_REQ_SET_CONTROL_LINE_STATE,
+				USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
+				on ? 0x01 : 0x00, intf, NULL, 0);
+}
+
 static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf)
 {
 	int status = -1;
@@ -257,7 +374,10 @@ static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf)
 				"bogus CDC Union: master=%u, slave=%u\n",
 				cdc_union->bMasterInterface0,
 				cdc_union->bSlaveInterface0);
-			goto err;
+
+			/* ignore and continue... */
+			cdc_union = NULL;
+			info->data = intf;
 		}
 	}
 
@@ -280,6 +400,24 @@ static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf)
 		usb_driver_release_interface(driver, info->data);
 	}
 
+	/* disabling remote wakeup on MDM9x30 devices has the same
+	 * effect as clearing DTR. The device will not respond to QMI
+	 * requests until we set DTR again.  This is similar to a
+	 * QMI_CTL SYNC request, clearing a lot of firmware state
+	 * including the client ID allocations.
+	 *
+	 * Our usage model allows a session to span multiple
+	 * open/close events, so we must prevent the firmware from
+	 * clearing out state the clients might need.
+	 *
+	 * MDM9x30 is the first QMI chipset with USB3 support. Abuse
+	 * this fact to enable the quirk.
+	 */
+	if (le16_to_cpu(dev->udev->descriptor.bcdUSB) >= 0x0201) {
+		qmi_wwan_manage_power(dev, 1);
+		qmi_wwan_change_dtr(dev, true);
+	}
+
 	/* Never use the same address on both ends of the link, even if the
 	 * buggy firmware told us to. Or, if device is assigned the well-known
 	 * buggy firmware MAC address, replace it with a random address,
@@ -294,6 +432,7 @@ static int qmi_wwan_bind(struct usbnet *dev, struct usb_interface *intf)
 		dev->net->dev_addr[0] &= 0xbf;	/* clear "IP" bit */
 	}
 	dev->net->netdev_ops = &qmi_wwan_netdev_ops;
+	dev->net->sysfs_groups[0] = &qmi_wwan_sysfs_attr_group;
 err:
 	return status;
 }
@@ -307,6 +446,12 @@ static void qmi_wwan_unbind(struct usbnet *dev, struct usb_interface *intf)
 	if (info->subdriver && info->subdriver->disconnect)
 		info->subdriver->disconnect(info->control);
 
+	/* disable MDM9x30 quirk */
+	if (le16_to_cpu(dev->udev->descriptor.bcdUSB) >= 0x0201) {
+		qmi_wwan_change_dtr(dev, false);
+		qmi_wwan_manage_power(dev, 0);
+	}
+
 	/* allow user to unbind using either control or data */
 	if (intf == info->control)
 		other = info->data;
@@ -492,6 +637,7 @@ static const struct usb_device_id products[] = {
 
 	/* 3. Combined interface devices matching on interface number */
 	{QMI_FIXED_INTF(0x0408, 0xea42, 4)},	/* Yota / Megafon M100-1 */
+	{QMI_FIXED_INTF(0x05c6, 0x6001, 3)},	/* 4G LTE usb-modem U901 */
 	{QMI_FIXED_INTF(0x05c6, 0x7000, 0)},
 	{QMI_FIXED_INTF(0x05c6, 0x7001, 1)},
 	{QMI_FIXED_INTF(0x05c6, 0x7002, 1)},
@@ -715,10 +861,10 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x1199, 0x9056, 8)},	/* Sierra Wireless Modem */
 	{QMI_FIXED_INTF(0x1199, 0x9057, 8)},
 	{QMI_FIXED_INTF(0x1199, 0x9061, 8)},	/* Sierra Wireless Modem */
-	{QMI_FIXED_INTF(0x1199, 0x9070, 8)},	/* Sierra Wireless MC74xx/EM74xx */
-	{QMI_FIXED_INTF(0x1199, 0x9070, 10)},	/* Sierra Wireless MC74xx/EM74xx */
-	{QMI_FIXED_INTF(0x1199, 0x9071, 8)},	/* Sierra Wireless MC74xx/EM74xx */
-	{QMI_FIXED_INTF(0x1199, 0x9071, 10)},	/* Sierra Wireless MC74xx/EM74xx */
+	{QMI_FIXED_INTF(0x1199, 0x9071, 8)},	/* Sierra Wireless MC74xx */
+	{QMI_FIXED_INTF(0x1199, 0x9071, 10)},	/* Sierra Wireless MC74xx */
+	{QMI_FIXED_INTF(0x1199, 0x9079, 8)},	/* Sierra Wireless EM74xx */
+	{QMI_FIXED_INTF(0x1199, 0x9079, 10)},	/* Sierra Wireless EM74xx */
 	{QMI_FIXED_INTF(0x1bbb, 0x011e, 4)},	/* Telekom Speedstick LTE II (Alcatel One Touch L100V LTE) */
 	{QMI_FIXED_INTF(0x1bbb, 0x0203, 2)},	/* Alcatel L800MA */
 	{QMI_FIXED_INTF(0x2357, 0x0201, 4)},	/* TP-LINK HSUPA Modem MA180 */
@@ -741,8 +887,10 @@ static const struct usb_device_id products[] = {
 	{QMI_FIXED_INTF(0x413c, 0x81a8, 8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
 	{QMI_FIXED_INTF(0x413c, 0x81a9, 8)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 	{QMI_FIXED_INTF(0x413c, 0x81b1, 8)},	/* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card */
+	{QMI_FIXED_INTF(0x413c, 0x81b3, 8)},	/* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card (rev3) */
 	{QMI_FIXED_INTF(0x03f0, 0x4e1d, 8)},	/* HP lt4111 LTE/EV-DO/HSPA+ Gobi 4G Module */
 	{QMI_FIXED_INTF(0x22de, 0x9061, 3)},	/* WeTelecom WPD-600N */
+	{QMI_FIXED_INTF(0x1e0e, 0x9001, 5)},	/* SIMCom 7230E */
 
 	/* 4. Gobi 1000 devices */
 	{QMI_GOBI1K_DEVICE(0x05c6, 0x9212)},	/* Acer Gobi Modem Device */
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index 2fb637ad594a..d1f78c2c97aa 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -25,12 +25,13 @@
 #include <uapi/linux/mdio.h>
 #include <linux/mdio.h>
 #include <linux/usb/cdc.h>
+#include <linux/suspend.h>
 
 /* Information for net-next */
 #define NETNEXT_VERSION		"08"
 
 /* Information for net */
-#define NET_VERSION		"2"
+#define NET_VERSION		"3"
 
 #define DRIVER_VERSION		"v1." NETNEXT_VERSION "." NET_VERSION
 #define DRIVER_AUTHOR "Realtek linux nic maintainers <nic_swsd@realtek.com>"
@@ -604,6 +605,9 @@ struct r8152 {
 	struct delayed_work schedule;
 	struct mii_if_info mii;
 	struct mutex control;	/* use for hw setting */
+#ifdef CONFIG_PM_SLEEP
+	struct notifier_block pm_notifier;
+#endif
 
 	struct rtl_ops {
 		void (*init)(struct r8152 *);
@@ -1938,7 +1942,6 @@ static void _rtl8152_set_rx_mode(struct net_device *netdev)
 	__le32 tmp[2];
 	u32 ocp_data;
 
-	clear_bit(RTL8152_SET_RX_MODE, &tp->flags);
 	netif_stop_queue(netdev);
 	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
 	ocp_data &= ~RCR_ACPT_ALL;
@@ -1986,7 +1989,7 @@ rtl8152_features_check(struct sk_buff *skb, struct net_device *dev,
 	int offset = skb_transport_offset(skb);
 
 	if ((mss || skb->ip_summed == CHECKSUM_PARTIAL) && offset > max_offset)
-		features &= ~(NETIF_F_ALL_CSUM | NETIF_F_GSO_MASK);
+		features &= ~(NETIF_F_CSUM_MASK | NETIF_F_GSO_MASK);
 	else if ((skb->len + sizeof(struct tx_desc)) > agg_buf_sz)
 		features &= ~NETIF_F_GSO_MASK;
 
@@ -2424,8 +2427,6 @@ static void rtl_phy_reset(struct r8152 *tp)
 	u16 data;
 	int i;
 
-	clear_bit(PHY_RESET, &tp->flags);
-
 	data = r8152_mdio_read(tp, MII_BMCR);
 
 	/* don't reset again before the previous one complete */
@@ -2455,23 +2456,23 @@ static void r8153_teredo_off(struct r8152 *tp)
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_TEREDO_TIMER, 0);
 }
 
-static void r8152b_disable_aldps(struct r8152 *tp)
+static void r8152_aldps_en(struct r8152 *tp, bool enable)
 {
-	ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPDNPS | LINKENA | DIS_SDSAVE);
-	msleep(20);
-}
-
-static inline void r8152b_enable_aldps(struct r8152 *tp)
-{
-	ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPWRSAVE | ENPDNPS |
-					    LINKENA | DIS_SDSAVE);
+	if (enable) {
+		ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPWRSAVE | ENPDNPS |
+						    LINKENA | DIS_SDSAVE);
+	} else {
+		ocp_reg_write(tp, OCP_ALDPS_CONFIG, ENPDNPS | LINKENA |
+						    DIS_SDSAVE);
+		msleep(20);
+	}
 }
 
 static void rtl8152_disable(struct r8152 *tp)
 {
-	r8152b_disable_aldps(tp);
+	r8152_aldps_en(tp, false);
 	rtl_disable(tp);
-	r8152b_enable_aldps(tp);
+	r8152_aldps_en(tp, true);
 }
 
 static void r8152b_hw_phy_cfg(struct r8152 *tp)
@@ -2783,30 +2784,26 @@ static void r8153_enter_oob(struct r8152 *tp)
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
 }
 
-static void r8153_disable_aldps(struct r8152 *tp)
-{
-	u16 data;
-
-	data = ocp_reg_read(tp, OCP_POWER_CFG);
-	data &= ~EN_ALDPS;
-	ocp_reg_write(tp, OCP_POWER_CFG, data);
-	msleep(20);
-}
-
-static void r8153_enable_aldps(struct r8152 *tp)
+static void r8153_aldps_en(struct r8152 *tp, bool enable)
 {
 	u16 data;
 
 	data = ocp_reg_read(tp, OCP_POWER_CFG);
-	data |= EN_ALDPS;
-	ocp_reg_write(tp, OCP_POWER_CFG, data);
+	if (enable) {
+		data |= EN_ALDPS;
+		ocp_reg_write(tp, OCP_POWER_CFG, data);
+	} else {
+		data &= ~EN_ALDPS;
+		ocp_reg_write(tp, OCP_POWER_CFG, data);
+		msleep(20);
+	}
 }
 
 static void rtl8153_disable(struct r8152 *tp)
 {
-	r8153_disable_aldps(tp);
+	r8153_aldps_en(tp, false);
 	rtl_disable(tp);
-	r8153_enable_aldps(tp);
+	r8153_aldps_en(tp, true);
 	usb_enable_lpm(tp->udev);
 }
 
@@ -2884,10 +2881,9 @@ static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 	r8152_mdio_write(tp, MII_ADVERTISE, anar);
 	r8152_mdio_write(tp, MII_BMCR, bmcr);
 
-	if (test_bit(PHY_RESET, &tp->flags)) {
+	if (test_and_clear_bit(PHY_RESET, &tp->flags)) {
 		int i;
 
-		clear_bit(PHY_RESET, &tp->flags);
 		for (i = 0; i < 50; i++) {
 			msleep(20);
 			if ((r8152_mdio_read(tp, MII_BMCR) & BMCR_RESET) == 0)
@@ -2896,7 +2892,6 @@ static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 	}
 
 out:
-
 	return ret;
 }
 
@@ -2905,9 +2900,9 @@ static void rtl8152_up(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
-	r8152b_disable_aldps(tp);
+	r8152_aldps_en(tp, false);
 	r8152b_exit_oob(tp);
-	r8152b_enable_aldps(tp);
+	r8152_aldps_en(tp, true);
 }
 
 static void rtl8152_down(struct r8152 *tp)
@@ -2918,9 +2913,9 @@ static void rtl8152_down(struct r8152 *tp)
 	}
 
 	r8152_power_cut_en(tp, false);
-	r8152b_disable_aldps(tp);
+	r8152_aldps_en(tp, false);
 	r8152b_enter_oob(tp);
-	r8152b_enable_aldps(tp);
+	r8152_aldps_en(tp, true);
 }
 
 static void rtl8153_up(struct r8152 *tp)
@@ -2929,9 +2924,9 @@ static void rtl8153_up(struct r8152 *tp)
 		return;
 
 	r8153_u1u2en(tp, false);
-	r8153_disable_aldps(tp);
+	r8153_aldps_en(tp, false);
 	r8153_first_init(tp);
-	r8153_enable_aldps(tp);
+	r8153_aldps_en(tp, true);
 	r8153_u2p3en(tp, true);
 	r8153_u1u2en(tp, true);
 	usb_enable_lpm(tp->udev);
@@ -2947,9 +2942,9 @@ static void rtl8153_down(struct r8152 *tp)
 	r8153_u1u2en(tp, false);
 	r8153_u2p3en(tp, false);
 	r8153_power_cut_en(tp, false);
-	r8153_disable_aldps(tp);
+	r8153_aldps_en(tp, false);
 	r8153_enter_oob(tp);
-	r8153_enable_aldps(tp);
+	r8153_aldps_en(tp, true);
 }
 
 static bool rtl8152_in_nway(struct r8152 *tp)
@@ -2983,7 +2978,6 @@ static void set_carrier(struct r8152 *tp)
 	struct net_device *netdev = tp->netdev;
 	u8 speed;
 
-	clear_bit(RTL8152_LINK_CHG, &tp->flags);
 	speed = rtl8152_get_speed(tp);
 
 	if (speed & LINK_STATUS) {
@@ -3026,20 +3020,18 @@ static void rtl_work_func_t(struct work_struct *work)
 		goto out1;
 	}
 
-	if (test_bit(RTL8152_LINK_CHG, &tp->flags))
+	if (test_and_clear_bit(RTL8152_LINK_CHG, &tp->flags))
 		set_carrier(tp);
 
-	if (test_bit(RTL8152_SET_RX_MODE, &tp->flags))
+	if (test_and_clear_bit(RTL8152_SET_RX_MODE, &tp->flags))
 		_rtl8152_set_rx_mode(tp->netdev);
 
 	/* don't schedule napi before linking */
-	if (test_bit(SCHEDULE_NAPI, &tp->flags) &&
-	    netif_carrier_ok(tp->netdev)) {
-		clear_bit(SCHEDULE_NAPI, &tp->flags);
+	if (test_and_clear_bit(SCHEDULE_NAPI, &tp->flags) &&
+	    netif_carrier_ok(tp->netdev))
 		napi_schedule(&tp->napi);
-	}
 
-	if (test_bit(PHY_RESET, &tp->flags))
+	if (test_and_clear_bit(PHY_RESET, &tp->flags))
 		rtl_phy_reset(tp);
 
 	mutex_unlock(&tp->control);
@@ -3048,6 +3040,33 @@ out1:
 	usb_autopm_put_interface(tp->intf);
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int rtl_notifier(struct notifier_block *nb, unsigned long action,
+			void *data)
+{
+	struct r8152 *tp = container_of(nb, struct r8152, pm_notifier);
+
+	switch (action) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_SUSPEND_PREPARE:
+		usb_autopm_get_interface(tp->intf);
+		break;
+
+	case PM_POST_HIBERNATION:
+	case PM_POST_SUSPEND:
+		usb_autopm_put_interface(tp->intf);
+		break;
+
+	case PM_POST_RESTORE:
+	case PM_RESTORE_PREPARE:
+	default:
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+#endif
+
 static int rtl8152_open(struct net_device *netdev)
 {
 	struct r8152 *tp = netdev_priv(netdev);
@@ -3090,6 +3109,10 @@ static int rtl8152_open(struct net_device *netdev)
 	mutex_unlock(&tp->control);
 
 	usb_autopm_put_interface(tp->intf);
+#ifdef CONFIG_PM_SLEEP
+	tp->pm_notifier.notifier_call = rtl_notifier;
+	register_pm_notifier(&tp->pm_notifier);
+#endif
 
 out:
 	return res;
@@ -3100,6 +3123,9 @@ static int rtl8152_close(struct net_device *netdev)
 	struct r8152 *tp = netdev_priv(netdev);
 	int res = 0;
 
+#ifdef CONFIG_PM_SLEEP
+	unregister_pm_notifier(&tp->pm_notifier);
+#endif
 	napi_disable(&tp->napi);
 	clear_bit(WORK_ENABLE, &tp->flags);
 	usb_kill_urb(tp->intr_urb);
@@ -3238,7 +3264,7 @@ static void r8152b_init(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
-	r8152b_disable_aldps(tp);
+	r8152_aldps_en(tp, false);
 
 	if (tp->version == RTL_VER_01) {
 		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_LED_FEATURE);
@@ -3260,7 +3286,7 @@ static void r8152b_init(struct r8152 *tp)
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_GPHY_INTR_IMR, ocp_data);
 
 	r8152b_enable_eee(tp);
-	r8152b_enable_aldps(tp);
+	r8152_aldps_en(tp, true);
 	r8152b_enable_fc(tp);
 	rtl_tally_reset(tp);
 
@@ -3278,7 +3304,7 @@ static void r8153_init(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
-	r8153_disable_aldps(tp);
+	r8153_aldps_en(tp, false);
 	r8153_u1u2en(tp, false);
 
 	for (i = 0; i < 500; i++) {
@@ -3367,7 +3393,7 @@ static void r8153_init(struct r8152 *tp)
 		       EEE_SPDWN_EN);
 
 	r8153_enable_eee(tp);
-	r8153_enable_aldps(tp);
+	r8153_aldps_en(tp, true);
 	r8152b_enable_fc(tp);
 	rtl_tally_reset(tp);
 	r8153_u2p3en(tp, true);
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 0744bf2ef2d6..10798128c03f 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -324,7 +324,10 @@ void usbnet_skb_return (struct usbnet *dev, struct sk_buff *skb)
 		return;
 	}
 
-	skb->protocol = eth_type_trans (skb, dev->net);
+	/* only update if unset to allow minidriver rx_fixup override */
+	if (skb->protocol == 0)
+		skb->protocol = eth_type_trans (skb, dev->net);
+
 	dev->net->stats.rx_packets++;
 	dev->net->stats.rx_bytes += skb->len;
 
@@ -1766,6 +1769,13 @@ out3:
 	if (info->unbind)
 		info->unbind (dev, udev);
 out1:
+	/* subdrivers must undo all they did in bind() if they
+	 * fail it, but we may fail later and a deferred kevent
+	 * may trigger an error resubmitting itself and, worse,
+	 * schedule a timer. So we kill it all just in case.
+	 */
+	cancel_work_sync(&dev->kevent);
+	del_timer_sync(&dev->delay);
 	free_netdev(net);
 out:
 	return status;
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index f94ab786088f..767ab11a6e9f 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -522,8 +522,6 @@ static void receive_buf(struct virtnet_info *vi, struct receive_queue *rq,
 		skb_shinfo(skb)->gso_segs = 0;
 	}
 
-	skb_mark_napi_id(skb, &rq->napi);
-
 	napi_gro_receive(&rq->napi, skb);
 	return;
 
@@ -1616,7 +1614,6 @@ static int virtnet_alloc_queues(struct virtnet_info *vi)
 		vi->rq[i].pages = NULL;
 		netif_napi_add(vi->dev, &vi->rq[i].napi, virtnet_poll,
 			       napi_weight);
-		napi_hash_add(&vi->rq[i].napi);
 
 		sg_init_table(vi->rq[i].sg, ARRAY_SIZE(vi->rq[i].sg));
 		ewma_pkt_len_init(&vi->rq[i].mrg_avg_pkt_len);
diff --git a/drivers/net/vmxnet3/vmxnet3_defs.h b/drivers/net/vmxnet3/vmxnet3_defs.h
index 221a53025fd0..72ba8ae7f09a 100644
--- a/drivers/net/vmxnet3/vmxnet3_defs.h
+++ b/drivers/net/vmxnet3/vmxnet3_defs.h
@@ -377,7 +377,7 @@ union Vmxnet3_GenericDesc {
 #define VMXNET3_TX_RING_MAX_SIZE   4096
 #define VMXNET3_TC_RING_MAX_SIZE   4096
 #define VMXNET3_RX_RING_MAX_SIZE   4096
-#define VMXNET3_RX_RING2_MAX_SIZE  2048
+#define VMXNET3_RX_RING2_MAX_SIZE  4096
 #define VMXNET3_RC_RING_MAX_SIZE   8192
 
 /* a list of reasons for queue stop */
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index 0cbf520cea77..fc895d0e85d9 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -814,7 +814,7 @@ vmxnet3_tq_init_all(struct vmxnet3_adapter *adapter)
 
 
 /*
- *    parse and copy relevant protocol headers:
+ *    parse relevant protocol headers:
  *      For a tso pkt, relevant headers are L2/3/4 including options
  *      For a pkt requesting csum offloading, they are L2/3 and may include L4
  *      if it's a TCP/UDP pkt
@@ -827,15 +827,14 @@ vmxnet3_tq_init_all(struct vmxnet3_adapter *adapter)
  * Other effects:
  *    1. related *ctx fields are updated.
  *    2. ctx->copy_size is # of bytes copied
- *    3. the portion copied is guaranteed to be in the linear part
+ *    3. the portion to be copied is guaranteed to be in the linear part
  *
  */
 static int
-vmxnet3_parse_and_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
-			   struct vmxnet3_tx_ctx *ctx,
-			   struct vmxnet3_adapter *adapter)
+vmxnet3_parse_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
+		  struct vmxnet3_tx_ctx *ctx,
+		  struct vmxnet3_adapter *adapter)
 {
-	struct Vmxnet3_TxDataDesc *tdd;
 	u8 protocol = 0;
 
 	if (ctx->mss) {	/* TSO */
@@ -892,16 +891,34 @@ vmxnet3_parse_and_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 		return 0;
 	}
 
+	return 1;
+err:
+	return -1;
+}
+
+/*
+ *    copy relevant protocol headers to the transmit ring:
+ *      For a tso pkt, relevant headers are L2/3/4 including options
+ *      For a pkt requesting csum offloading, they are L2/3 and may include L4
+ *      if it's a TCP/UDP pkt
+ *
+ *
+ *    Note that this requires that vmxnet3_parse_hdr be called first to set the
+ *      appropriate bits in ctx first
+ */
+static void
+vmxnet3_copy_hdr(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
+		 struct vmxnet3_tx_ctx *ctx,
+		 struct vmxnet3_adapter *adapter)
+{
+	struct Vmxnet3_TxDataDesc *tdd;
+
 	tdd = tq->data_ring.base + tq->tx_ring.next2fill;
 
 	memcpy(tdd->data, skb->data, ctx->copy_size);
 	netdev_dbg(adapter->netdev,
 		"copy %u bytes to dataRing[%u]\n",
 		ctx->copy_size, tq->tx_ring.next2fill);
-	return 1;
-
-err:
-	return -1;
 }
 
 
@@ -998,22 +1015,7 @@ vmxnet3_tq_xmit(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 		}
 	}
 
-	spin_lock_irqsave(&tq->tx_lock, flags);
-
-	if (count > vmxnet3_cmd_ring_desc_avail(&tq->tx_ring)) {
-		tq->stats.tx_ring_full++;
-		netdev_dbg(adapter->netdev,
-			"tx queue stopped on %s, next2comp %u"
-			" next2fill %u\n", adapter->netdev->name,
-			tq->tx_ring.next2comp, tq->tx_ring.next2fill);
-
-		vmxnet3_tq_stop(tq, adapter);
-		spin_unlock_irqrestore(&tq->tx_lock, flags);
-		return NETDEV_TX_BUSY;
-	}
-
-
-	ret = vmxnet3_parse_and_copy_hdr(skb, tq, &ctx, adapter);
+	ret = vmxnet3_parse_hdr(skb, tq, &ctx, adapter);
 	if (ret >= 0) {
 		BUG_ON(ret <= 0 && ctx.copy_size != 0);
 		/* hdrs parsed, check against other limits */
@@ -1033,9 +1035,26 @@ vmxnet3_tq_xmit(struct sk_buff *skb, struct vmxnet3_tx_queue *tq,
 		}
 	} else {
 		tq->stats.drop_hdr_inspect_err++;
-		goto unlock_drop_pkt;
+		goto drop_pkt;
 	}
 
+	spin_lock_irqsave(&tq->tx_lock, flags);
+
+	if (count > vmxnet3_cmd_ring_desc_avail(&tq->tx_ring)) {
+		tq->stats.tx_ring_full++;
+		netdev_dbg(adapter->netdev,
+			"tx queue stopped on %s, next2comp %u"
+			" next2fill %u\n", adapter->netdev->name,
+			tq->tx_ring.next2comp, tq->tx_ring.next2fill);
+
+		vmxnet3_tq_stop(tq, adapter);
+		spin_unlock_irqrestore(&tq->tx_lock, flags);
+		return NETDEV_TX_BUSY;
+	}
+
+
+	vmxnet3_copy_hdr(skb, tq, &ctx, adapter);
+
 	/* fill tx descs related to addr & len */
 	if (vmxnet3_map_pkt(skb, &ctx, tq, adapter->pdev, adapter))
 		goto unlock_drop_pkt;
diff --git a/drivers/net/vmxnet3/vmxnet3_int.h b/drivers/net/vmxnet3/vmxnet3_int.h
index bdb8a6c0f8aa..729c344e6774 100644
--- a/drivers/net/vmxnet3/vmxnet3_int.h
+++ b/drivers/net/vmxnet3/vmxnet3_int.h
@@ -69,10 +69,10 @@
 /*
  * Version numbers
  */
-#define VMXNET3_DRIVER_VERSION_STRING   "1.4.5.0-k"
+#define VMXNET3_DRIVER_VERSION_STRING   "1.4.6.0-k"
 
 /* a 32-bit int, each byte encode a verion number in VMXNET3_DRIVER_VERSION */
-#define VMXNET3_DRIVER_VERSION_NUM      0x01040500
+#define VMXNET3_DRIVER_VERSION_NUM      0x01040600
 
 #if defined(CONFIG_PCI_MSI)
 	/* RSS only makes sense if MSI-X is supported. */
diff --git a/drivers/net/vrf.c b/drivers/net/vrf.c
index 0a242b200df4..bdcf617a9d52 100644
--- a/drivers/net/vrf.c
+++ b/drivers/net/vrf.c
@@ -46,17 +46,7 @@
 #define vrf_master_get_rcu(dev) \
 	((struct net_device *)rcu_dereference(dev->rx_handler_data))
 
-struct slave {
-	struct list_head        list;
-	struct net_device       *dev;
-};
-
-struct slave_queue {
-	struct list_head        all_slaves;
-};
-
 struct net_vrf {
-	struct slave_queue      queue;
 	struct rtable           *rth;
 	struct rt6_info		*rt6;
 	u32                     tb_id;
@@ -114,20 +104,23 @@ static struct dst_ops vrf_dst_ops = {
 #if IS_ENABLED(CONFIG_IPV6)
 static bool check_ipv6_frame(const struct sk_buff *skb)
 {
-	const struct ipv6hdr *ipv6h = (struct ipv6hdr *)skb->data;
-	size_t hlen = sizeof(*ipv6h);
+	const struct ipv6hdr *ipv6h;
+	struct ipv6hdr _ipv6h;
 	bool rc = true;
 
-	if (skb->len < hlen)
+	ipv6h = skb_header_pointer(skb, 0, sizeof(_ipv6h), &_ipv6h);
+	if (!ipv6h)
 		goto out;
 
 	if (ipv6h->nexthdr == NEXTHDR_ICMP) {
 		const struct icmp6hdr *icmph;
+		struct icmp6hdr _icmph;
 
-		if (skb->len < hlen + sizeof(*icmph))
+		icmph = skb_header_pointer(skb, sizeof(_ipv6h),
+					   sizeof(_icmph), &_icmph);
+		if (!icmph)
 			goto out;
 
-		icmph = (struct icmp6hdr *)(skb->data + sizeof(*ipv6h));
 		switch (icmph->icmp6_type) {
 		case NDISC_ROUTER_SOLICITATION:
 		case NDISC_ROUTER_ADVERTISEMENT:
@@ -621,42 +614,9 @@ static void cycle_netdev(struct net_device *dev)
 	}
 }
 
-static struct slave *__vrf_find_slave_dev(struct slave_queue *queue,
-					  struct net_device *dev)
-{
-	struct list_head *head = &queue->all_slaves;
-	struct slave *slave;
-
-	list_for_each_entry(slave, head, list) {
-		if (slave->dev == dev)
-			return slave;
-	}
-
-	return NULL;
-}
-
-/* inverse of __vrf_insert_slave */
-static void __vrf_remove_slave(struct slave_queue *queue, struct slave *slave)
-{
-	list_del(&slave->list);
-}
-
-static void __vrf_insert_slave(struct slave_queue *queue, struct slave *slave)
-{
-	list_add(&slave->list, &queue->all_slaves);
-}
-
 static int do_vrf_add_slave(struct net_device *dev, struct net_device *port_dev)
 {
-	struct slave *slave = kzalloc(sizeof(*slave), GFP_KERNEL);
-	struct net_vrf *vrf = netdev_priv(dev);
-	struct slave_queue *queue = &vrf->queue;
-	int ret = -ENOMEM;
-
-	if (!slave)
-		goto out_fail;
-
-	slave->dev = port_dev;
+	int ret;
 
 	/* register the packet handler for slave ports */
 	ret = netdev_rx_handler_register(port_dev, vrf_handle_frame, dev);
@@ -667,12 +627,11 @@ static int do_vrf_add_slave(struct net_device *dev, struct net_device *port_dev)
 		goto out_fail;
 	}
 
-	ret = netdev_master_upper_dev_link(port_dev, dev);
+	ret = netdev_master_upper_dev_link(port_dev, dev, NULL, NULL);
 	if (ret < 0)
 		goto out_unregister;
 
 	port_dev->priv_flags |= IFF_L3MDEV_SLAVE;
-	__vrf_insert_slave(queue, slave);
 	cycle_netdev(port_dev);
 
 	return 0;
@@ -680,7 +639,6 @@ static int do_vrf_add_slave(struct net_device *dev, struct net_device *port_dev)
 out_unregister:
 	netdev_rx_handler_unregister(port_dev);
 out_fail:
-	kfree(slave);
 	return ret;
 }
 
@@ -695,10 +653,6 @@ static int vrf_add_slave(struct net_device *dev, struct net_device *port_dev)
 /* inverse of do_vrf_add_slave */
 static int do_vrf_del_slave(struct net_device *dev, struct net_device *port_dev)
 {
-	struct net_vrf *vrf = netdev_priv(dev);
-	struct slave_queue *queue = &vrf->queue;
-	struct slave *slave;
-
 	netdev_upper_dev_unlink(port_dev, dev);
 	port_dev->priv_flags &= ~IFF_L3MDEV_SLAVE;
 
@@ -706,12 +660,6 @@ static int do_vrf_del_slave(struct net_device *dev, struct net_device *port_dev)
 
 	cycle_netdev(port_dev);
 
-	slave = __vrf_find_slave_dev(queue, port_dev);
-	if (slave)
-		__vrf_remove_slave(queue, slave);
-
-	kfree(slave);
-
 	return 0;
 }
 
@@ -723,15 +671,14 @@ static int vrf_del_slave(struct net_device *dev, struct net_device *port_dev)
 static void vrf_dev_uninit(struct net_device *dev)
 {
 	struct net_vrf *vrf = netdev_priv(dev);
-	struct slave_queue *queue = &vrf->queue;
-	struct list_head *head = &queue->all_slaves;
-	struct slave *slave, *next;
+	struct net_device *port_dev;
+	struct list_head *iter;
 
 	vrf_rtable_destroy(vrf);
 	vrf_rt6_destroy(vrf);
 
-	list_for_each_entry_safe(slave, next, head, list)
-		vrf_del_slave(dev, slave->dev);
+	netdev_for_each_lower_dev(dev, port_dev, iter)
+		vrf_del_slave(dev, port_dev);
 
 	free_percpu(dev->dstats);
 	dev->dstats = NULL;
@@ -741,8 +688,6 @@ static int vrf_dev_init(struct net_device *dev)
 {
 	struct net_vrf *vrf = netdev_priv(dev);
 
-	INIT_LIST_HEAD(&vrf->queue.all_slaves);
-
 	dev->dstats = netdev_alloc_pcpu_stats(struct pcpu_dstats);
 	if (!dev->dstats)
 		goto out_nomem;
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index ba363cedef80..1c32bd104797 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -73,7 +73,7 @@ MODULE_PARM_DESC(log_ecn_error, "Log packets received with corrupted ECN");
 static int vxlan_net_id;
 static struct rtnl_link_ops vxlan_link_ops;
 
-static const u8 all_zeros_mac[ETH_ALEN];
+static const u8 all_zeros_mac[ETH_ALEN + 2];
 
 static int vxlan_sock_add(struct vxlan_dev *vxlan);
 
@@ -621,7 +621,7 @@ static void vxlan_notify_add_rx_port(struct vxlan_sock *vs)
 	int err;
 
 	if (sa_family == AF_INET) {
-		err = udp_add_offload(&vs->udp_offloads);
+		err = udp_add_offload(net, &vs->udp_offloads);
 		if (err)
 			pr_warn("vxlan: udp_add_offload failed with status %d\n", err);
 	}
@@ -931,8 +931,10 @@ static int vxlan_fdb_dump(struct sk_buff *skb, struct netlink_callback *cb,
 						     cb->nlh->nlmsg_seq,
 						     RTM_NEWNEIGH,
 						     NLM_F_MULTI, rd);
-				if (err < 0)
+				if (err < 0) {
+					cb->args[1] = err;
 					goto out;
+				}
 skip:
 				++idx;
 			}
@@ -1306,8 +1308,10 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 		gbp = (struct vxlanhdr_gbp *)vxh;
 		md->gbp = ntohs(gbp->policy_id);
 
-		if (tun_dst)
+		if (tun_dst) {
 			tun_dst->u.tun_info.key.tun_flags |= TUNNEL_VXLAN_OPT;
+			tun_dst->u.tun_info.options_len = sizeof(*md);
+		}
 
 		if (gbp->dont_learn)
 			md->gbp |= VXLAN_GBP_DONT_LEARN;
@@ -1841,9 +1845,10 @@ static int vxlan_xmit_skb(struct rtable *rt, struct sock *sk, struct sk_buff *sk
 
 	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 
-	return udp_tunnel_xmit_skb(rt, sk, skb, src, dst, tos,
-				   ttl, df, src_port, dst_port, xnet,
-				   !(vxflags & VXLAN_F_UDP_CSUM));
+	udp_tunnel_xmit_skb(rt, sk, skb, src, dst, tos, ttl, df,
+			    src_port, dst_port, xnet,
+			    !(vxflags & VXLAN_F_UDP_CSUM));
+	return 0;
 }
 
 #if IS_ENABLED(CONFIG_IPV6)
@@ -1984,11 +1989,6 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 				     vxlan->cfg.port_max, true);
 
 	if (info) {
-		if (info->key.tun_flags & TUNNEL_CSUM)
-			flags |= VXLAN_F_UDP_CSUM;
-		else
-			flags &= ~VXLAN_F_UDP_CSUM;
-
 		ttl = info->key.ttl;
 		tos = info->key.tos;
 
@@ -2003,8 +2003,15 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 			goto drop;
 		sk = vxlan->vn4_sock->sock->sk;
 
-		if (info && (info->key.tun_flags & TUNNEL_DONT_FRAGMENT))
-			df = htons(IP_DF);
+		if (info) {
+			if (info->key.tun_flags & TUNNEL_DONT_FRAGMENT)
+				df = htons(IP_DF);
+
+			if (info->key.tun_flags & TUNNEL_CSUM)
+				flags |= VXLAN_F_UDP_CSUM;
+			else
+				flags &= ~VXLAN_F_UDP_CSUM;
+		}
 
 		memset(&fl4, 0, sizeof(fl4));
 		fl4.flowi4_oif = rdst ? rdst->remote_ifindex : 0;
@@ -2056,8 +2063,6 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 			skb = NULL;
 			goto rt_tx_error;
 		}
-
-		iptunnel_xmit_stats(err, &dev->stats, dev->tstats);
 #if IS_ENABLED(CONFIG_IPV6)
 	} else {
 		struct dst_entry *ndst;
@@ -2102,6 +2107,13 @@ static void vxlan_xmit_one(struct sk_buff *skb, struct net_device *dev,
 			return;
 		}
 
+		if (info) {
+			if (info->key.tun_flags & TUNNEL_CSUM)
+				flags &= ~VXLAN_F_UDP_ZERO_CSUM6_TX;
+			else
+				flags |= VXLAN_F_UDP_ZERO_CSUM6_TX;
+		}
+
 		ttl = ttl ? : ip6_dst_hoplimit(ndst);
 		err = vxlan6_xmit_skb(ndst, sk, skb, dev, &saddr, &dst->sin6.sin6_addr,
 				      0, ttl, src_port, dst_port, htonl(vni << 8), md,
@@ -2163,9 +2175,11 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 #endif
 	}
 
-	if (vxlan->flags & VXLAN_F_COLLECT_METADATA &&
-	    info && info->mode & IP_TUNNEL_INFO_TX) {
-		vxlan_xmit_one(skb, dev, NULL, false);
+	if (vxlan->flags & VXLAN_F_COLLECT_METADATA) {
+		if (info && info->mode & IP_TUNNEL_INFO_TX)
+			vxlan_xmit_one(skb, dev, NULL, false);
+		else
+			kfree_skb(skb);
 		return NETDEV_TX_OK;
 	}
 
@@ -2359,29 +2373,43 @@ static void vxlan_set_multicast_list(struct net_device *dev)
 {
 }
 
-static int vxlan_change_mtu(struct net_device *dev, int new_mtu)
+static int __vxlan_change_mtu(struct net_device *dev,
+			      struct net_device *lowerdev,
+			      struct vxlan_rdst *dst, int new_mtu, bool strict)
 {
-	struct vxlan_dev *vxlan = netdev_priv(dev);
-	struct vxlan_rdst *dst = &vxlan->default_dst;
-	struct net_device *lowerdev;
-	int max_mtu;
+	int max_mtu = IP_MAX_MTU;
 
-	lowerdev = __dev_get_by_index(vxlan->net, dst->remote_ifindex);
-	if (lowerdev == NULL)
-		return eth_change_mtu(dev, new_mtu);
+	if (lowerdev)
+		max_mtu = lowerdev->mtu;
 
 	if (dst->remote_ip.sa.sa_family == AF_INET6)
-		max_mtu = lowerdev->mtu - VXLAN6_HEADROOM;
+		max_mtu -= VXLAN6_HEADROOM;
 	else
-		max_mtu = lowerdev->mtu - VXLAN_HEADROOM;
+		max_mtu -= VXLAN_HEADROOM;
 
-	if (new_mtu < 68 || new_mtu > max_mtu)
+	if (new_mtu < 68)
 		return -EINVAL;
 
+	if (new_mtu > max_mtu) {
+		if (strict)
+			return -EINVAL;
+
+		new_mtu = max_mtu;
+	}
+
 	dev->mtu = new_mtu;
 	return 0;
 }
 
+static int vxlan_change_mtu(struct net_device *dev, int new_mtu)
+{
+	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct vxlan_rdst *dst = &vxlan->default_dst;
+	struct net_device *lowerdev = __dev_get_by_index(vxlan->net,
+							 dst->remote_ifindex);
+	return __vxlan_change_mtu(dev, lowerdev, dst, new_mtu, true);
+}
+
 static int egress_ipv4_tun_info(struct net_device *dev, struct sk_buff *skb,
 				struct ip_tunnel_info *info,
 				__be16 sport, __be16 dport)
@@ -2515,6 +2543,7 @@ static void vxlan_setup(struct net_device *dev)
 	dev->hw_features |= NETIF_F_GSO_SOFTWARE;
 	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_STAG_TX;
 	netif_keep_dst(dev);
+	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
 	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE | IFF_NO_QUEUE;
 
 	INIT_LIST_HEAD(&vxlan->next);
@@ -2751,12 +2780,13 @@ static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
 			       struct vxlan_config *conf)
 {
 	struct vxlan_net *vn = net_generic(src_net, vxlan_net_id);
-	struct vxlan_dev *vxlan = netdev_priv(dev);
+	struct vxlan_dev *vxlan = netdev_priv(dev), *tmp;
 	struct vxlan_rdst *dst = &vxlan->default_dst;
 	unsigned short needed_headroom = ETH_HLEN;
 	int err;
 	bool use_ipv6 = false;
 	__be16 default_port = vxlan->cfg.dst_port;
+	struct net_device *lowerdev = NULL;
 
 	vxlan->net = src_net;
 
@@ -2777,9 +2807,7 @@ static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
 	}
 
 	if (conf->remote_ifindex) {
-		struct net_device *lowerdev
-			 = __dev_get_by_index(src_net, conf->remote_ifindex);
-
+		lowerdev = __dev_get_by_index(src_net, conf->remote_ifindex);
 		dst->remote_ifindex = conf->remote_ifindex;
 
 		if (!lowerdev) {
@@ -2803,6 +2831,12 @@ static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
 		needed_headroom = lowerdev->hard_header_len;
 	}
 
+	if (conf->mtu) {
+		err = __vxlan_change_mtu(dev, lowerdev, dst, conf->mtu, false);
+		if (err)
+			return err;
+	}
+
 	if (use_ipv6 || conf->flags & VXLAN_F_COLLECT_METADATA)
 		needed_headroom += VXLAN6_HEADROOM;
 	else
@@ -2817,9 +2851,15 @@ static int vxlan_dev_configure(struct net *src_net, struct net_device *dev,
 	if (!vxlan->cfg.age_interval)
 		vxlan->cfg.age_interval = FDB_AGE_DEFAULT;
 
-	if (vxlan_find_vni(src_net, conf->vni, use_ipv6 ? AF_INET6 : AF_INET,
-			   vxlan->cfg.dst_port, vxlan->flags))
+	list_for_each_entry(tmp, &vn->vxlan_list, next) {
+		if (tmp->cfg.vni == conf->vni &&
+		    (tmp->default_dst.remote_ip.sa.sa_family == AF_INET6 ||
+		     tmp->cfg.saddr.sa.sa_family == AF_INET6) == use_ipv6 &&
+		    tmp->cfg.dst_port == vxlan->cfg.dst_port &&
+		    (tmp->flags & VXLAN_F_RCV_FLAGS) ==
+		    (vxlan->flags & VXLAN_F_RCV_FLAGS))
 		return -EEXIST;
+	}
 
 	dev->ethtool_ops = &vxlan_ethtool_ops;
 
diff --git a/drivers/net/wan/dscc4.c b/drivers/net/wan/dscc4.c
index 7a72407208b1..629225980463 100644
--- a/drivers/net/wan/dscc4.c
+++ b/drivers/net/wan/dscc4.c
@@ -1626,7 +1626,7 @@ try:
 		if (state & Xpr) {
 			void __iomem *scc_addr;
 			unsigned long ring;
-			int i;
+			unsigned int i;
 
 			/*
 			 * - the busy condition happens (sometimes);
diff --git a/drivers/net/wan/hdlc.c b/drivers/net/wan/hdlc.c
index 51f6cee8aab2..9bd4aa8083ce 100644
--- a/drivers/net/wan/hdlc.c
+++ b/drivers/net/wan/hdlc.c
@@ -266,8 +266,8 @@ struct net_device *alloc_hdlcdev(void *priv)
 void unregister_hdlc_device(struct net_device *dev)
 {
 	rtnl_lock();
-	unregister_netdevice(dev);
 	detach_hdlc_protocol(dev);
+	unregister_netdevice(dev);
 	rtnl_unlock();
 }
 
@@ -276,7 +276,11 @@ void unregister_hdlc_device(struct net_device *dev)
 int attach_hdlc_protocol(struct net_device *dev, struct hdlc_proto *proto,
 			 size_t size)
 {
-	detach_hdlc_protocol(dev);
+	int err;
+
+	err = detach_hdlc_protocol(dev);
+	if (err)
+		return err;
 
 	if (!try_module_get(proto->module))
 		return -ENOSYS;
@@ -289,15 +293,24 @@ int attach_hdlc_protocol(struct net_device *dev, struct hdlc_proto *proto,
 		}
 	}
 	dev_to_hdlc(dev)->proto = proto;
+
 	return 0;
 }
 
 
-void detach_hdlc_protocol(struct net_device *dev)
+int detach_hdlc_protocol(struct net_device *dev)
 {
 	hdlc_device *hdlc = dev_to_hdlc(dev);
+	int err;
 
 	if (hdlc->proto) {
+		err = call_netdevice_notifiers(NETDEV_PRE_TYPE_CHANGE, dev);
+		err = notifier_to_errno(err);
+		if (err) {
+			netdev_err(dev, "Refused to change device type\n");
+			return err;
+		}
+
 		if (hdlc->proto->detach)
 			hdlc->proto->detach(dev);
 		module_put(hdlc->proto->module);
@@ -306,6 +319,8 @@ void detach_hdlc_protocol(struct net_device *dev)
 	kfree(hdlc->state);
 	hdlc->state = NULL;
 	hdlc_setup_dev(dev);
+
+	return 0;
 }
 
 
diff --git a/drivers/net/wan/hdlc_cisco.c b/drivers/net/wan/hdlc_cisco.c
index 3f20808b5ff8..a408abc25512 100644
--- a/drivers/net/wan/hdlc_cisco.c
+++ b/drivers/net/wan/hdlc_cisco.c
@@ -378,6 +378,7 @@ static int cisco_ioctl(struct net_device *dev, struct ifreq *ifr)
 		spin_lock_init(&state(hdlc)->lock);
 		dev->header_ops = &cisco_header_ops;
 		dev->type = ARPHRD_CISCO;
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		netif_dormant_on(dev);
 		return 0;
 	}
diff --git a/drivers/net/wan/hdlc_fr.c b/drivers/net/wan/hdlc_fr.c
index 89541cc90e87..b6e0cfb095d3 100644
--- a/drivers/net/wan/hdlc_fr.c
+++ b/drivers/net/wan/hdlc_fr.c
@@ -1240,6 +1240,7 @@ static int fr_ioctl(struct net_device *dev, struct ifreq *ifr)
 		}
 		memcpy(&state(hdlc)->settings, &new_settings, size);
 		dev->type = ARPHRD_FRAD;
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		return 0;
 
 	case IF_PROTO_FR_ADD_PVC:
diff --git a/drivers/net/wan/hdlc_ppp.c b/drivers/net/wan/hdlc_ppp.c
index 0d7645581f91..47fdb87d3567 100644
--- a/drivers/net/wan/hdlc_ppp.c
+++ b/drivers/net/wan/hdlc_ppp.c
@@ -687,6 +687,7 @@ static int ppp_ioctl(struct net_device *dev, struct ifreq *ifr)
 		dev->hard_header_len = sizeof(struct hdlc_header);
 		dev->header_ops = &ppp_header_ops;
 		dev->type = ARPHRD_PPP;
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		netif_dormant_on(dev);
 		return 0;
 	}
diff --git a/drivers/net/wan/hdlc_raw.c b/drivers/net/wan/hdlc_raw.c
index 5dc153e8a29d..4feb45001aac 100644
--- a/drivers/net/wan/hdlc_raw.c
+++ b/drivers/net/wan/hdlc_raw.c
@@ -84,6 +84,7 @@ static int raw_ioctl(struct net_device *dev, struct ifreq *ifr)
 			return result;
 		memcpy(hdlc->state, &new_settings, size);
 		dev->type = ARPHRD_RAWHDLC;
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		netif_dormant_off(dev);
 		return 0;
 	}
diff --git a/drivers/net/wan/hdlc_raw_eth.c b/drivers/net/wan/hdlc_raw_eth.c
index 3ab72b3082de..2f11836078ab 100644
--- a/drivers/net/wan/hdlc_raw_eth.c
+++ b/drivers/net/wan/hdlc_raw_eth.c
@@ -102,6 +102,7 @@ static int raw_eth_ioctl(struct net_device *dev, struct ifreq *ifr)
 		ether_setup(dev);
 		dev->tx_queue_len = old_qlen;
 		eth_hw_addr_random(dev);
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		netif_dormant_off(dev);
 		return 0;
 	}
diff --git a/drivers/net/wan/hdlc_x25.c b/drivers/net/wan/hdlc_x25.c
index a49aec5efd20..e867638067a6 100644
--- a/drivers/net/wan/hdlc_x25.c
+++ b/drivers/net/wan/hdlc_x25.c
@@ -213,6 +213,7 @@ static int x25_ioctl(struct net_device *dev, struct ifreq *ifr)
 		if ((result = attach_hdlc_protocol(dev, &proto, 0)))
 			return result;
 		dev->type = ARPHRD_X25;
+		call_netdevice_notifiers(NETDEV_POST_TYPE_CHANGE, dev);
 		netif_dormant_off(dev);
 		return 0;
 	}
diff --git a/drivers/net/wan/wanxl.c b/drivers/net/wan/wanxl.c
index e73f13857846..a20d688d2595 100644
--- a/drivers/net/wan/wanxl.c
+++ b/drivers/net/wan/wanxl.c
@@ -586,6 +586,7 @@ static int wanxl_pci_init_one(struct pci_dev *pdev,
 	if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(28)) ||
 	    pci_set_dma_mask(pdev, DMA_BIT_MASK(28))) {
 		pr_err("No usable DMA configuration\n");
+		pci_disable_device(pdev);
 		return -EIO;
 	}
 
diff --git a/drivers/net/wan/x25_asy.c b/drivers/net/wan/x25_asy.c
index cd39025d2abf..1bc5e93d2a34 100644
--- a/drivers/net/wan/x25_asy.c
+++ b/drivers/net/wan/x25_asy.c
@@ -571,8 +571,10 @@ static int x25_asy_open_tty(struct tty_struct *tty)
 
 	/* Perform the low-level X.25 async init */
 	err = x25_asy_open(sl->dev);
-	if (err)
+	if (err) {
+		x25_asy_free(sl);
 		return err;
+	}
 	/* Done.  We have linked the TTY line to a channel. */
 	return 0;
 }
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index f9f94229bf1b..8c8edaf1bba6 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -17,6 +17,22 @@ menuconfig WLAN
 
 if WLAN
 
+source "drivers/net/wireless/admtek/Kconfig"
+source "drivers/net/wireless/ath/Kconfig"
+source "drivers/net/wireless/atmel/Kconfig"
+source "drivers/net/wireless/broadcom/Kconfig"
+source "drivers/net/wireless/cisco/Kconfig"
+source "drivers/net/wireless/intel/Kconfig"
+source "drivers/net/wireless/intersil/Kconfig"
+source "drivers/net/wireless/marvell/Kconfig"
+source "drivers/net/wireless/mediatek/Kconfig"
+source "drivers/net/wireless/ralink/Kconfig"
+source "drivers/net/wireless/realtek/Kconfig"
+source "drivers/net/wireless/rsi/Kconfig"
+source "drivers/net/wireless/st/Kconfig"
+source "drivers/net/wireless/ti/Kconfig"
+source "drivers/net/wireless/zydas/Kconfig"
+
 config PCMCIA_RAYCS
 	tristate "Aviator/Raytheon 2.4GHz wireless support"
 	depends on PCMCIA
@@ -32,110 +48,6 @@ config PCMCIA_RAYCS
 	  To compile this driver as a module, choose M here: the module will be
 	  called ray_cs.  If unsure, say N.
 
-config LIBERTAS_THINFIRM
-	tristate "Marvell 8xxx Libertas WLAN driver support with thin firmware"
-	depends on MAC80211
-	select FW_LOADER
-	---help---
-	  A library for Marvell Libertas 8xxx devices using thinfirm.
-
-config LIBERTAS_THINFIRM_DEBUG
-	bool "Enable full debugging output in the Libertas thin firmware module."
-	depends on LIBERTAS_THINFIRM
-	---help---
-	  Debugging support.
-
-config LIBERTAS_THINFIRM_USB
-	tristate "Marvell Libertas 8388 USB 802.11b/g cards with thin firmware"
-	depends on LIBERTAS_THINFIRM && USB
-	---help---
-	  A driver for Marvell Libertas 8388 USB devices using thinfirm.
-
-config AIRO
-	tristate "Cisco/Aironet 34X/35X/4500/4800 ISA and PCI cards"
-	depends on CFG80211 && ISA_DMA_API && (PCI || BROKEN)
-	select WIRELESS_EXT
-	select CRYPTO
-	select WEXT_SPY
-	select WEXT_PRIV
-	---help---
-	  This is the standard Linux driver to support Cisco/Aironet ISA and
-	  PCI 802.11 wireless cards.
-	  It supports the new 802.11b cards from Cisco (Cisco 34X, Cisco 35X
-	  - with or without encryption) as well as card before the Cisco
-	  acquisition (Aironet 4500, Aironet 4800, Aironet 4800B).
-
-	  This driver support both the standard Linux Wireless Extensions
-	  and Cisco proprietary API, so both the Linux Wireless Tools and the
-	  Cisco Linux utilities can be used to configure the card.
-
-	  The driver can be compiled as a module and will be named "airo".
-
-config ATMEL
-      tristate "Atmel at76c50x chipset  802.11b support"
-      depends on CFG80211 && (PCI || PCMCIA)
-      select WIRELESS_EXT
-      select WEXT_PRIV
-      select FW_LOADER
-      select CRC32
-       ---help---
-        A driver 802.11b wireless cards based on the Atmel fast-vnet
-        chips. This driver supports standard Linux wireless extensions.
-
-        Many  cards based on this chipset do not have flash memory
-        and need their firmware loaded at start-up. If yours is
-        one of these, you will need to provide a firmware image
-        to be loaded into the card by the driver. The Atmel
-        firmware package can be downloaded from
-        <http://www.thekelleys.org.uk/atmel>
-
-config PCI_ATMEL
-      tristate "Atmel at76c506 PCI cards"
-      depends on ATMEL && PCI
-       ---help---
-        Enable support for PCI and mini-PCI cards containing the
-        Atmel at76c506 chip.
-
-config PCMCIA_ATMEL
-	tristate "Atmel at76c502/at76c504 PCMCIA cards"
-	depends on ATMEL && PCMCIA
-	select WIRELESS_EXT
-	select FW_LOADER
-	select CRC32
-	---help---
-	  Enable support for PCMCIA cards containing the
-	  Atmel at76c502 and at76c504 chips.
-
-config AT76C50X_USB
-        tristate "Atmel at76c503/at76c505/at76c505a USB cards"
-        depends on MAC80211 && USB
-        select FW_LOADER
-        ---help---
-          Enable support for USB Wireless devices using Atmel at76c503,
-          at76c505 or at76c505a chips.
-
-config AIRO_CS
-	tristate "Cisco/Aironet 34X/35X/4500/4800 PCMCIA cards"
-	depends on CFG80211 && PCMCIA && (BROKEN || !M32R)
-	select WIRELESS_EXT
-	select WEXT_SPY
-	select WEXT_PRIV
-	select CRYPTO
-	select CRYPTO_AES
-	---help---
-	  This is the standard Linux driver to support Cisco/Aironet PCMCIA
-	  802.11 wireless cards.  This driver is the same as the Aironet
-	  driver part of the Linux Pcmcia package.
-	  It supports the new 802.11b cards from Cisco (Cisco 34X, Cisco 35X
-	  - with or without encryption) as well as card before the Cisco
-	  acquisition (Aironet 4500, Aironet 4800, Aironet 4800B). It also
-	  supports OEM of Cisco such as the DELL TrueMobile 4800 and Xircom
-	  802.11b cards.
-
-	  This driver support both the standard Linux Wireless Extensions
-	  and Cisco proprietary API, so both the Linux Wireless Tools and the
-	  Cisco Linux utilities can be used to configure the card.
-
 config PCMCIA_WL3501
 	tristate "Planet WL3501 PCMCIA cards"
 	depends on CFG80211 && PCMCIA
@@ -146,44 +58,18 @@ config PCMCIA_WL3501
 	  It has basic support for Linux wireless extensions and initial
 	  micro support for ethtool.
 
-config PRISM54
-	tristate 'Intersil Prism GT/Duette/Indigo PCI/Cardbus (DEPRECATED)'
-	depends on PCI
-	select WIRELESS_EXT
-	select WEXT_SPY
-	select WEXT_PRIV
-	select FW_LOADER
-	---help---
-	  This enables support for FullMAC PCI/Cardbus prism54 devices. This
-	  driver is now deprecated in favor for the SoftMAC driver, p54pci.
-	  p54pci supports FullMAC PCI/Cardbus devices as well.
-
-	  For more information refer to the p54 wiki:
-
-	  http://wireless.kernel.org/en/users/Drivers/p54
-
-	  Note: You need a motherboard with DMA support to use any of these cards
-
-	  When built as module you get the module prism54
-
-config USB_ZD1201
-	tristate "USB ZD1201 based Wireless device support"
-	depends on CFG80211 && USB
-	select WIRELESS_EXT
-	select WEXT_PRIV
-	select FW_LOADER
+config MAC80211_HWSIM
+	tristate "Simulated radio testing tool for mac80211"
+	depends on MAC80211
 	---help---
-	  Say Y if you want to use wireless LAN adapters based on the ZyDAS
-	  ZD1201 chip.
-
-	  This driver makes the adapter appear as a normal Ethernet interface,
-	  typically on wlan0.
-
-	  The zd1201 device requires external firmware to be loaded.
-	  This can be found at http://linux-lc100020.sourceforge.net/
+	  This driver is a developer testing tool that can be used to test
+	  IEEE 802.11 networking stack (mac80211) functionality. This is not
+	  needed for normal wireless LAN usage and is only for testing. See
+	  Documentation/networking/mac80211_hwsim for more information on how
+	  to use this tool.
 
-	  To compile this driver as a module, choose M here: the
-	  module will be called zd1201.
+	  To compile this driver as a module, choose M here: the module will be
+	  called mac80211_hwsim.  If unsure, say N.
 
 config USB_NET_RNDIS_WLAN
 	tristate "Wireless RNDIS USB support"
@@ -214,76 +100,4 @@ config USB_NET_RNDIS_WLAN
 
 	  If you choose to build a module, it'll be called rndis_wlan.
 
-config ADM8211
-	tristate "ADMtek ADM8211 support"
-	depends on MAC80211 && PCI
-	select CRC32
-	select EEPROM_93CX6
-	---help---
-	  This driver is for ADM8211A, ADM8211B, and ADM8211C based cards.
-	  These are PCI/mini-PCI/Cardbus 802.11b chips found in cards such as:
-
-	  Xterasys Cardbus XN-2411b
-	  Blitz NetWave Point PC
-	  TrendNet 221pc
-	  Belkin F5D6001
-	  SMC 2635W
-	  Linksys WPC11 v1
-	  Fiberline FL-WL-200X
-	  3com Office Connect (3CRSHPW796)
-	  Corega WLPCIB-11
-	  SMC 2602W V2 EU
-	  D-Link DWL-520 Revision C
-
-	  However, some of these cards have been replaced with other chips
-	  like the RTL8180L (Xterasys Cardbus XN-2411b, Belkin F5D6001) or
-	  the Ralink RT2400 (SMC2635W) without a model number change.
-
-	  Thanks to Infineon-ADMtek for their support of this driver.
-
-source "drivers/net/wireless/realtek/rtl818x/Kconfig"
-
-config MAC80211_HWSIM
-	tristate "Simulated radio testing tool for mac80211"
-	depends on MAC80211
-	---help---
-	  This driver is a developer testing tool that can be used to test
-	  IEEE 802.11 networking stack (mac80211) functionality. This is not
-	  needed for normal wireless LAN usage and is only for testing. See
-	  Documentation/networking/mac80211_hwsim for more information on how
-	  to use this tool.
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called mac80211_hwsim.  If unsure, say N.
-
-config MWL8K
-	tristate "Marvell 88W8xxx PCI/PCIe Wireless support"
-	depends on MAC80211 && PCI
-	---help---
-	  This driver supports Marvell TOPDOG 802.11 wireless cards.
-
-	  To compile this driver as a module, choose M here: the module
-	  will be called mwl8k.  If unsure, say N.
-
-source "drivers/net/wireless/ath/Kconfig"
-source "drivers/net/wireless/b43/Kconfig"
-source "drivers/net/wireless/b43legacy/Kconfig"
-source "drivers/net/wireless/brcm80211/Kconfig"
-source "drivers/net/wireless/hostap/Kconfig"
-source "drivers/net/wireless/ipw2x00/Kconfig"
-source "drivers/net/wireless/iwlwifi/Kconfig"
-source "drivers/net/wireless/iwlegacy/Kconfig"
-source "drivers/net/wireless/libertas/Kconfig"
-source "drivers/net/wireless/orinoco/Kconfig"
-source "drivers/net/wireless/p54/Kconfig"
-source "drivers/net/wireless/rt2x00/Kconfig"
-source "drivers/net/wireless/mediatek/Kconfig"
-source "drivers/net/wireless/realtek/rtlwifi/Kconfig"
-source "drivers/net/wireless/realtek/rtl8xxxu/Kconfig"
-source "drivers/net/wireless/ti/Kconfig"
-source "drivers/net/wireless/zd1211rw/Kconfig"
-source "drivers/net/wireless/mwifiex/Kconfig"
-source "drivers/net/wireless/cw1200/Kconfig"
-source "drivers/net/wireless/rsi/Kconfig"
-
 endif # WLAN
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 740fdd353c5d..f00d42953fb8 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -2,27 +2,21 @@
 # Makefile for the Linux Wireless network device drivers.
 #
 
-obj-$(CONFIG_IPW2100) += ipw2x00/
-obj-$(CONFIG_IPW2200) += ipw2x00/
-
-obj-$(CONFIG_HERMES)		+= orinoco/
-
-obj-$(CONFIG_AIRO)		+= airo.o
-obj-$(CONFIG_AIRO_CS)		+= airo_cs.o airo.o
-
-obj-$(CONFIG_ATMEL)             += atmel.o
-obj-$(CONFIG_PCI_ATMEL)         += atmel_pci.o 
-obj-$(CONFIG_PCMCIA_ATMEL)      += atmel_cs.o
-
-obj-$(CONFIG_AT76C50X_USB)      += at76c50x-usb.o
-
-obj-$(CONFIG_PRISM54)		+= prism54/
-
-obj-$(CONFIG_HOSTAP)		+= hostap/
-obj-$(CONFIG_B43)		+= b43/
-obj-$(CONFIG_B43LEGACY)		+= b43legacy/
-obj-$(CONFIG_ZD1211RW)		+= zd1211rw/
-obj-$(CONFIG_WLAN)		+= realtek/
+obj-$(CONFIG_WLAN_VENDOR_ADMTEK) += admtek/
+obj-$(CONFIG_WLAN_VENDOR_ATH) += ath/
+obj-$(CONFIG_WLAN_VENDOR_ATMEL) += atmel/
+obj-$(CONFIG_WLAN_VENDOR_BROADCOM) += broadcom/
+obj-$(CONFIG_WLAN_VENDOR_CISCO) += cisco/
+obj-$(CONFIG_WLAN_VENDOR_INTEL) += intel/
+obj-$(CONFIG_WLAN_VENDOR_INTERSIL) += intersil/
+obj-$(CONFIG_WLAN_VENDOR_MARVELL) += marvell/
+obj-$(CONFIG_WLAN_VENDOR_MEDIATEK) += mediatek/
+obj-$(CONFIG_WLAN_VENDOR_RALINK) += ralink/
+obj-$(CONFIG_WLAN_VENDOR_REALTEK) += realtek/
+obj-$(CONFIG_WLAN_VENDOR_RSI) += rsi/
+obj-$(CONFIG_WLAN_VENDOR_ST) += st/
+obj-$(CONFIG_WLAN_VENDOR_TI) += ti/
+obj-$(CONFIG_WLAN_VENDOR_ZYDAS) += zydas/
 
 # 16-bit wireless PCMCIA client drivers
 obj-$(CONFIG_PCMCIA_RAYCS)	+= ray_cs.o
@@ -30,33 +24,4 @@ obj-$(CONFIG_PCMCIA_WL3501)	+= wl3501_cs.o
 
 obj-$(CONFIG_USB_NET_RNDIS_WLAN)	+= rndis_wlan.o
 
-obj-$(CONFIG_USB_ZD1201)	+= zd1201.o
-obj-$(CONFIG_LIBERTAS)		+= libertas/
-
-obj-$(CONFIG_LIBERTAS_THINFIRM)	+= libertas_tf/
-
-obj-$(CONFIG_ADM8211)	+= adm8211.o
-
-obj-$(CONFIG_MWL8K)	+= mwl8k.o
-
-obj-$(CONFIG_IWLWIFI)	+= iwlwifi/
-obj-$(CONFIG_IWLEGACY)	+= iwlegacy/
-obj-$(CONFIG_RT2X00)	+= rt2x00/
-
-obj-$(CONFIG_WL_MEDIATEK)	+= mediatek/
-
-obj-$(CONFIG_P54_COMMON)	+= p54/
-
-obj-$(CONFIG_ATH_CARDS)		+= ath/
-
 obj-$(CONFIG_MAC80211_HWSIM)	+= mac80211_hwsim.o
-
-obj-$(CONFIG_WL_TI)	+= ti/
-
-obj-$(CONFIG_MWIFIEX)	+= mwifiex/
-
-obj-$(CONFIG_BRCMFMAC)	+= brcm80211/
-obj-$(CONFIG_BRCMSMAC)	+= brcm80211/
-
-obj-$(CONFIG_CW1200)	+= cw1200/
-obj-$(CONFIG_RSI_91X)	+= rsi/
diff --git a/drivers/net/wireless/admtek/Kconfig b/drivers/net/wireless/admtek/Kconfig
new file mode 100644
index 000000000000..d5a2dc728078
--- /dev/null
+++ b/drivers/net/wireless/admtek/Kconfig
@@ -0,0 +1,41 @@
+config WLAN_VENDOR_ADMTEK
+	bool "ADMtek devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_ADMTEK
+
+config ADM8211
+	tristate "ADMtek ADM8211 support"
+	depends on MAC80211 && PCI
+	select CRC32
+	select EEPROM_93CX6
+	---help---
+	  This driver is for ADM8211A, ADM8211B, and ADM8211C based cards.
+	  These are PCI/mini-PCI/Cardbus 802.11b chips found in cards such as:
+
+	  Xterasys Cardbus XN-2411b
+	  Blitz NetWave Point PC
+	  TrendNet 221pc
+	  Belkin F5D6001
+	  SMC 2635W
+	  Linksys WPC11 v1
+	  Fiberline FL-WL-200X
+	  3com Office Connect (3CRSHPW796)
+	  Corega WLPCIB-11
+	  SMC 2602W V2 EU
+	  D-Link DWL-520 Revision C
+
+	  However, some of these cards have been replaced with other chips
+	  like the RTL8180L (Xterasys Cardbus XN-2411b, Belkin F5D6001) or
+	  the Ralink RT2400 (SMC2635W) without a model number change.
+
+	  Thanks to Infineon-ADMtek for their support of this driver.
+
+endif # WLAN_VENDOR_ADMTEK
diff --git a/drivers/net/wireless/admtek/Makefile b/drivers/net/wireless/admtek/Makefile
new file mode 100644
index 000000000000..9cca7e571cdd
--- /dev/null
+++ b/drivers/net/wireless/admtek/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_ADM8211)	+= adm8211.o
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/admtek/adm8211.c
index 15f057ed41ad..15f057ed41ad 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/admtek/adm8211.c
diff --git a/drivers/net/wireless/adm8211.h b/drivers/net/wireless/admtek/adm8211.h
index bbc10b1cde87..bbc10b1cde87 100644
--- a/drivers/net/wireless/adm8211.h
+++ b/drivers/net/wireless/admtek/adm8211.h
diff --git a/drivers/net/wireless/ath/Kconfig b/drivers/net/wireless/ath/Kconfig
index ce7826009eeb..44b2470af81d 100644
--- a/drivers/net/wireless/ath/Kconfig
+++ b/drivers/net/wireless/ath/Kconfig
@@ -1,13 +1,16 @@
 config ATH_COMMON
 	tristate
 
-menuconfig ATH_CARDS
-	tristate "Atheros Wireless Cards"
-	depends on CFG80211 && (!UML || BROKEN)
+config WLAN_VENDOR_ATH
+	bool "Atheros/Qualcomm devices"
+	default y
 	---help---
-	  This will enable the support for the Atheros wireless drivers.
-	  ath5k, ath9k, ath9k_htc and ar9170 drivers share some common code, this option
-	  enables the common ath.ko module which shares common helpers.
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
 
 	  For more information and documentation on this module you can visit:
 
@@ -17,7 +20,7 @@ menuconfig ATH_CARDS
 
 	  http://wireless.kernel.org/en/users/Drivers/Atheros
 
-if ATH_CARDS
+if WLAN_VENDOR_ATH
 
 config ATH_DEBUG
 	bool "Atheros wireless debugging"
diff --git a/drivers/net/wireless/ath/ath10k/Kconfig b/drivers/net/wireless/ath/ath10k/Kconfig
index 72acb822bb11..03aa35f999a1 100644
--- a/drivers/net/wireless/ath/ath10k/Kconfig
+++ b/drivers/net/wireless/ath/ath10k/Kconfig
@@ -2,6 +2,7 @@ config ATH10K
         tristate "Atheros 802.11ac wireless cards support"
         depends on MAC80211 && HAS_DMA
 	select ATH_COMMON
+	select CRC32
         ---help---
           This module adds support for wireless adapters based on
           Atheros IEEE 802.11ac family of chipsets.
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 0947cc271e69..b41eb3f4ee56 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -59,6 +59,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.otp_exe_param = 0,
 		.channel_counters_freq_hz = 88000,
 		.max_probe_resp_desc_thres = 0,
+		.hw_4addr_pad = ATH10K_HW_4ADDR_PAD_AFTER,
 		.fw = {
 			.dir = QCA988X_HW_2_0_FW_DIR,
 			.fw = QCA988X_HW_2_0_FW_FILE,
@@ -95,6 +96,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.otp_exe_param = 0,
 		.channel_counters_freq_hz = 88000,
 		.max_probe_resp_desc_thres = 0,
+		.hw_4addr_pad = ATH10K_HW_4ADDR_PAD_AFTER,
 		.fw = {
 			.dir = QCA6174_HW_2_1_FW_DIR,
 			.fw = QCA6174_HW_2_1_FW_FILE,
@@ -113,6 +115,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.otp_exe_param = 0,
 		.channel_counters_freq_hz = 88000,
 		.max_probe_resp_desc_thres = 0,
+		.hw_4addr_pad = ATH10K_HW_4ADDR_PAD_AFTER,
 		.fw = {
 			.dir = QCA6174_HW_3_0_FW_DIR,
 			.fw = QCA6174_HW_3_0_FW_FILE,
@@ -131,6 +134,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.otp_exe_param = 0,
 		.channel_counters_freq_hz = 88000,
 		.max_probe_resp_desc_thres = 0,
+		.hw_4addr_pad = ATH10K_HW_4ADDR_PAD_AFTER,
 		.fw = {
 			/* uses same binaries as hw3.0 */
 			.dir = QCA6174_HW_3_0_FW_DIR,
@@ -151,6 +155,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 		.continuous_frag_desc = true,
 		.channel_counters_freq_hz = 150000,
 		.max_probe_resp_desc_thres = 24,
+		.hw_4addr_pad = ATH10K_HW_4ADDR_PAD_BEFORE,
 		.fw = {
 			.dir = QCA99X0_HW_2_0_FW_DIR,
 			.fw = QCA99X0_HW_2_0_FW_FILE,
@@ -211,6 +216,7 @@ static const char *const ath10k_core_fw_feature_str[] = {
 	[ATH10K_FW_FEATURE_SUPPORTS_SKIP_CLOCK_INIT] = "skip-clock-init",
 	[ATH10K_FW_FEATURE_RAW_MODE_SUPPORT] = "raw-mode",
 	[ATH10K_FW_FEATURE_SUPPORTS_ADAPTIVE_CCA] = "adaptive-cca",
+	[ATH10K_FW_FEATURE_MFP_SUPPORT] = "mfp",
 };
 
 static unsigned int ath10k_core_get_fw_feature_str(char *buf,
@@ -887,7 +893,7 @@ out:
 	if (!ar->board_data || !ar->board_len) {
 		ath10k_err(ar,
 			   "failed to fetch board data for %s from %s/%s\n",
-			   ar->hw_params.fw.dir, boardname, filename);
+			   boardname, ar->hw_params.fw.dir, filename);
 		ret = -ENODATA;
 		goto err;
 	}
@@ -1790,9 +1796,11 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 		goto err_power_down;
 	}
 
+	ath10k_debug_print_hwfw_info(ar);
+
 	ret = ath10k_core_get_board_id_from_otp(ar);
 	if (ret && ret != -EOPNOTSUPP) {
-		ath10k_err(ar, "failed to get board id from otp for qca99x0: %d\n",
+		ath10k_err(ar, "failed to get board id from otp: %d\n",
 			   ret);
 		return ret;
 	}
@@ -1803,6 +1811,8 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 		goto err_free_firmware_files;
 	}
 
+	ath10k_debug_print_board_info(ar);
+
 	ret = ath10k_core_init_firmware_features(ar);
 	if (ret) {
 		ath10k_err(ar, "fatal problem with firmware features: %d\n",
@@ -1825,7 +1835,7 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 		goto err_unlock;
 	}
 
-	ath10k_print_driver_info(ar);
+	ath10k_debug_print_boot_info(ar);
 	ath10k_core_stop(ar);
 
 	mutex_unlock(&ar->conf_mutex);
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index 858d75f49a9f..7840cf3ef7a6 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -81,26 +81,20 @@ static inline const char *ath10k_bus_str(enum ath10k_bus bus)
 	return "unknown";
 }
 
+enum ath10k_skb_flags {
+	ATH10K_SKB_F_NO_HWCRYPT = BIT(0),
+	ATH10K_SKB_F_DTIM_ZERO = BIT(1),
+	ATH10K_SKB_F_DELIVER_CAB = BIT(2),
+	ATH10K_SKB_F_MGMT = BIT(3),
+	ATH10K_SKB_F_QOS = BIT(4),
+};
+
 struct ath10k_skb_cb {
 	dma_addr_t paddr;
+	u8 flags;
 	u8 eid;
-	u8 vdev_id;
-	enum ath10k_hw_txrx_mode txmode;
-	bool is_protected;
-
-	struct {
-		u8 tid;
-		u16 freq;
-		bool is_offchan;
-		bool nohwcrypt;
-		struct ath10k_htt_txbuf *txbuf;
-		u32 txbuf_paddr;
-	} __packed htt;
-
-	struct {
-		bool dtim_zero;
-		bool deliver_cab;
-	} bcn;
+	u16 msdu_id;
+	struct ieee80211_vif *vif;
 } __packed;
 
 struct ath10k_skb_rxcb {
@@ -151,6 +145,7 @@ struct ath10k_wmi {
 	struct wmi_vdev_param_map *vdev_param;
 	struct wmi_pdev_param_map *pdev_param;
 	const struct wmi_ops *ops;
+	const struct wmi_peer_flags_map *peer_flags;
 
 	u32 num_mem_chunks;
 	u32 rx_decap_mode;
@@ -512,6 +507,9 @@ enum ath10k_fw_features {
 	/* Firmware Supports Adaptive CCA*/
 	ATH10K_FW_FEATURE_SUPPORTS_ADAPTIVE_CCA = 11,
 
+	/* Firmware supports management frame protection */
+	ATH10K_FW_FEATURE_MFP_SUPPORT = 12,
+
 	/* keep last */
 	ATH10K_FW_FEATURE_COUNT,
 };
@@ -534,6 +532,9 @@ enum ath10k_dev_flags {
 
 	/* Disable HW crypto engine */
 	ATH10K_FLAG_HW_CRYPTO_DISABLED,
+
+	/* Bluetooth coexistance enabled */
+	ATH10K_FLAG_BTCOEX,
 };
 
 enum ath10k_cal_mode {
@@ -662,6 +663,9 @@ struct ath10k {
 		 */
 		u32 max_probe_resp_desc_thres;
 
+		/* The padding bytes's location is different on various chips */
+		enum ath10k_hw_4addr_pad hw_4addr_pad;
+
 		struct ath10k_hw_params_fw {
 			const char *dir;
 			const char *fw;
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index 6cc1aa3449c8..2bdf5408b0d9 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -19,6 +19,8 @@
 #include <linux/debugfs.h>
 #include <linux/vmalloc.h>
 #include <linux/utsname.h>
+#include <linux/crc32.h>
+#include <linux/firmware.h>
 
 #include "core.h"
 #include "debug.h"
@@ -122,28 +124,51 @@ void ath10k_info(struct ath10k *ar, const char *fmt, ...)
 }
 EXPORT_SYMBOL(ath10k_info);
 
-void ath10k_print_driver_info(struct ath10k *ar)
+void ath10k_debug_print_hwfw_info(struct ath10k *ar)
 {
 	char fw_features[128] = {};
-	char boardinfo[100];
 
 	ath10k_core_get_fw_features_str(ar, fw_features, sizeof(fw_features));
 
-	if (ar->id.bmi_ids_valid)
-		scnprintf(boardinfo, sizeof(boardinfo), "bmi %d:%d",
-			  ar->id.bmi_chip_id, ar->id.bmi_board_id);
-	else
-		scnprintf(boardinfo, sizeof(boardinfo), "sub %04x:%04x",
-			  ar->id.subsystem_vendor, ar->id.subsystem_device);
-
-	ath10k_info(ar, "%s (0x%08x, 0x%08x %s) fw %s fwapi %d bdapi %d htt-ver %d.%d wmi-op %d htt-op %d cal %s max-sta %d raw %d hwcrypto %d features %s\n",
+	ath10k_info(ar, "%s target 0x%08x chip_id 0x%08x sub %04x:%04x",
 		    ar->hw_params.name,
 		    ar->target_version,
 		    ar->chip_id,
-		    boardinfo,
+		    ar->id.subsystem_vendor, ar->id.subsystem_device);
+
+	ath10k_info(ar, "kconfig debug %d debugfs %d tracing %d dfs %d testmode %d\n",
+		    config_enabled(CONFIG_ATH10K_DEBUG),
+		    config_enabled(CONFIG_ATH10K_DEBUGFS),
+		    config_enabled(CONFIG_ATH10K_TRACING),
+		    config_enabled(CONFIG_ATH10K_DFS_CERTIFIED),
+		    config_enabled(CONFIG_NL80211_TESTMODE));
+
+	ath10k_info(ar, "firmware ver %s api %d features %s crc32 %08x\n",
 		    ar->hw->wiphy->fw_version,
 		    ar->fw_api,
+		    fw_features,
+		    crc32_le(0, ar->firmware->data, ar->firmware->size));
+}
+
+void ath10k_debug_print_board_info(struct ath10k *ar)
+{
+	char boardinfo[100];
+
+	if (ar->id.bmi_ids_valid)
+		scnprintf(boardinfo, sizeof(boardinfo), "%d:%d",
+			  ar->id.bmi_chip_id, ar->id.bmi_board_id);
+	else
+		scnprintf(boardinfo, sizeof(boardinfo), "N/A");
+
+	ath10k_info(ar, "board_file api %d bmi_id %s crc32 %08x",
 		    ar->bd_api,
+		    boardinfo,
+		    crc32_le(0, ar->board->data, ar->board->size));
+}
+
+void ath10k_debug_print_boot_info(struct ath10k *ar)
+{
+	ath10k_info(ar, "htt-ver %d.%d wmi-op %d htt-op %d cal %s max-sta %d raw %d hwcrypto %d\n",
 		    ar->htt.target_version_major,
 		    ar->htt.target_version_minor,
 		    ar->wmi.op_version,
@@ -151,14 +176,14 @@ void ath10k_print_driver_info(struct ath10k *ar)
 		    ath10k_cal_mode_str(ar->cal_mode),
 		    ar->max_num_stations,
 		    test_bit(ATH10K_FLAG_RAW_MODE, &ar->dev_flags),
-		    !test_bit(ATH10K_FLAG_HW_CRYPTO_DISABLED, &ar->dev_flags),
-		    fw_features);
-	ath10k_info(ar, "debug %d debugfs %d tracing %d dfs %d testmode %d\n",
-		    config_enabled(CONFIG_ATH10K_DEBUG),
-		    config_enabled(CONFIG_ATH10K_DEBUGFS),
-		    config_enabled(CONFIG_ATH10K_TRACING),
-		    config_enabled(CONFIG_ATH10K_DFS_CERTIFIED),
-		    config_enabled(CONFIG_NL80211_TESTMODE));
+		    !test_bit(ATH10K_FLAG_HW_CRYPTO_DISABLED, &ar->dev_flags));
+}
+
+void ath10k_print_driver_info(struct ath10k *ar)
+{
+	ath10k_debug_print_hwfw_info(ar);
+	ath10k_debug_print_board_info(ar);
+	ath10k_debug_print_boot_info(ar);
 }
 EXPORT_SYMBOL(ath10k_print_driver_info);
 
@@ -1114,7 +1139,7 @@ static ssize_t ath10k_read_htt_max_amsdu_ampdu(struct file *file,
 {
 	struct ath10k *ar = file->private_data;
 	char buf[64];
-	u8 amsdu = 3, ampdu = 64;
+	u8 amsdu, ampdu;
 	unsigned int len;
 
 	mutex_lock(&ar->conf_mutex);
@@ -2074,6 +2099,121 @@ static const struct file_operations fops_quiet_period = {
 	.open = simple_open
 };
 
+static ssize_t ath10k_write_btcoex(struct file *file,
+				   const char __user *ubuf,
+				   size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	char buf[32];
+	size_t buf_size;
+	bool val;
+
+	buf_size = min(count, (sizeof(buf) - 1));
+	if (copy_from_user(buf, ubuf, buf_size))
+		return -EFAULT;
+
+	buf[buf_size] = '\0';
+
+	if (strtobool(buf, &val) != 0)
+		return -EINVAL;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (!(test_bit(ATH10K_FLAG_BTCOEX, &ar->dev_flags) ^ val))
+		goto exit;
+
+	if (val)
+		set_bit(ATH10K_FLAG_BTCOEX, &ar->dev_flags);
+	else
+		clear_bit(ATH10K_FLAG_BTCOEX, &ar->dev_flags);
+
+	if (ar->state != ATH10K_STATE_ON)
+		goto exit;
+
+	ath10k_info(ar, "restarting firmware due to btcoex change");
+
+	queue_work(ar->workqueue, &ar->restart_work);
+
+exit:
+	mutex_unlock(&ar->conf_mutex);
+
+	return count;
+}
+
+static ssize_t ath10k_read_btcoex(struct file *file, char __user *ubuf,
+				  size_t count, loff_t *ppos)
+{
+	char buf[32];
+	struct ath10k *ar = file->private_data;
+	int len = 0;
+
+	mutex_lock(&ar->conf_mutex);
+	len = scnprintf(buf, sizeof(buf) - len, "%d\n",
+			test_bit(ATH10K_FLAG_BTCOEX, &ar->dev_flags));
+	mutex_unlock(&ar->conf_mutex);
+
+	return simple_read_from_buffer(ubuf, count, ppos, buf, len);
+}
+
+static const struct file_operations fops_btcoex = {
+	.read = ath10k_read_btcoex,
+	.write = ath10k_write_btcoex,
+	.open = simple_open
+};
+
+static ssize_t ath10k_debug_fw_checksums_read(struct file *file,
+					      char __user *user_buf,
+					      size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	unsigned int len = 0, buf_len = 4096;
+	ssize_t ret_cnt;
+	char *buf;
+
+	buf = kzalloc(buf_len, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (len > buf_len)
+		len = buf_len;
+
+	len += scnprintf(buf + len, buf_len - len,
+			 "firmware-N.bin\t\t%08x\n",
+			 crc32_le(0, ar->firmware->data, ar->firmware->size));
+	len += scnprintf(buf + len, buf_len - len,
+			 "athwlan\t\t\t%08x\n",
+			 crc32_le(0, ar->firmware_data, ar->firmware_len));
+	len += scnprintf(buf + len, buf_len - len,
+			 "otp\t\t\t%08x\n",
+			 crc32_le(0, ar->otp_data, ar->otp_len));
+	len += scnprintf(buf + len, buf_len - len,
+			 "codeswap\t\t%08x\n",
+			 crc32_le(0, ar->swap.firmware_codeswap_data,
+				  ar->swap.firmware_codeswap_len));
+	len += scnprintf(buf + len, buf_len - len,
+			 "board-N.bin\t\t%08x\n",
+			 crc32_le(0, ar->board->data, ar->board->size));
+	len += scnprintf(buf + len, buf_len - len,
+			 "board\t\t\t%08x\n",
+			 crc32_le(0, ar->board_data, ar->board_len));
+
+	ret_cnt = simple_read_from_buffer(user_buf, count, ppos, buf, len);
+
+	mutex_unlock(&ar->conf_mutex);
+
+	kfree(buf);
+	return ret_cnt;
+}
+
+static const struct file_operations fops_fw_checksums = {
+	.read = ath10k_debug_fw_checksums_read,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 int ath10k_debug_create(struct ath10k *ar)
 {
 	ar->debug.fw_crash_data = vzalloc(sizeof(*ar->debug.fw_crash_data));
@@ -2123,8 +2263,8 @@ int ath10k_debug_register(struct ath10k *ar)
 	debugfs_create_file("wmi_services", S_IRUSR, ar->debug.debugfs_phy, ar,
 			    &fops_wmi_services);
 
-	debugfs_create_file("simulate_fw_crash", S_IRUSR, ar->debug.debugfs_phy,
-			    ar, &fops_simulate_fw_crash);
+	debugfs_create_file("simulate_fw_crash", S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_simulate_fw_crash);
 
 	debugfs_create_file("fw_crash_dump", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_fw_crash_dump);
@@ -2141,15 +2281,15 @@ int ath10k_debug_register(struct ath10k *ar)
 	debugfs_create_file("chip_id", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_chip_id);
 
-	debugfs_create_file("htt_stats_mask", S_IRUSR, ar->debug.debugfs_phy,
-			    ar, &fops_htt_stats_mask);
+	debugfs_create_file("htt_stats_mask", S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_htt_stats_mask);
 
 	debugfs_create_file("htt_max_amsdu_ampdu", S_IRUSR | S_IWUSR,
 			    ar->debug.debugfs_phy, ar,
 			    &fops_htt_max_amsdu_ampdu);
 
-	debugfs_create_file("fw_dbglog", S_IRUSR, ar->debug.debugfs_phy,
-			    ar, &fops_fw_dbglog);
+	debugfs_create_file("fw_dbglog", S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar, &fops_fw_dbglog);
 
 	debugfs_create_file("cal_data", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_cal_data);
@@ -2183,6 +2323,13 @@ int ath10k_debug_register(struct ath10k *ar)
 	debugfs_create_file("tpc_stats", S_IRUSR,
 			    ar->debug.debugfs_phy, ar, &fops_tpc_stats);
 
+	if (test_bit(WMI_SERVICE_COEX_GPIO, ar->wmi.svc_map))
+		debugfs_create_file("btcoex", S_IRUGO | S_IWUSR,
+				    ar->debug.debugfs_phy, ar, &fops_btcoex);
+
+	debugfs_create_file("fw_checksums", S_IRUSR,
+			    ar->debug.debugfs_phy, ar, &fops_fw_checksums);
+
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index 7de780c4ec8d..814719cf4f22 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -63,6 +63,10 @@ extern unsigned int ath10k_debug_mask;
 __printf(2, 3) void ath10k_info(struct ath10k *ar, const char *fmt, ...);
 __printf(2, 3) void ath10k_err(struct ath10k *ar, const char *fmt, ...);
 __printf(2, 3) void ath10k_warn(struct ath10k *ar, const char *fmt, ...);
+
+void ath10k_debug_print_hwfw_info(struct ath10k *ar);
+void ath10k_debug_print_board_info(struct ath10k *ar);
+void ath10k_debug_print_boot_info(struct ath10k *ar);
 void ath10k_print_driver_info(struct ath10k *ar);
 
 #ifdef CONFIG_ATH10K_DEBUGFS
diff --git a/drivers/net/wireless/ath/ath10k/htt.h b/drivers/net/wireless/ath/ath10k/htt.h
index 2bad50e520b5..47ca048feaf0 100644
--- a/drivers/net/wireless/ath/ath10k/htt.h
+++ b/drivers/net/wireless/ath/ath10k/htt.h
@@ -166,8 +166,13 @@ struct htt_data_tx_desc {
 	__le16 len;
 	__le16 id;
 	__le32 frags_paddr;
-	__le16 peerid;
-	__le16 freq;
+	union {
+		__le32 peerid;
+		struct {
+			__le16 peerid;
+			__le16 freq;
+		} __packed offchan_tx;
+	} __packed;
 	u8 prefetch[0]; /* start of frame, for FW classification engine */
 } __packed;
 
@@ -1597,6 +1602,10 @@ void __ath10k_htt_tx_dec_pending(struct ath10k_htt *htt, bool limit_mgmt_desc);
 int ath10k_htt_tx_alloc_msdu_id(struct ath10k_htt *htt, struct sk_buff *skb);
 void ath10k_htt_tx_free_msdu_id(struct ath10k_htt *htt, u16 msdu_id);
 int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *);
-int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *);
+int ath10k_htt_tx(struct ath10k_htt *htt,
+		  enum ath10k_hw_txrx_mode txmode,
+		  struct sk_buff *msdu);
+void ath10k_htt_rx_pktlog_completion_handler(struct ath10k *ar,
+					     struct sk_buff *skb);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index 6060dda4e910..91afa3ae414c 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -536,7 +536,7 @@ int ath10k_htt_rx_alloc(struct ath10k_htt *htt)
 
 	size = htt->rx_ring.size * sizeof(htt->rx_ring.paddrs_ring);
 
-	vaddr = dma_alloc_coherent(htt->ar->dev, size, &paddr, GFP_DMA);
+	vaddr = dma_alloc_coherent(htt->ar->dev, size, &paddr, GFP_KERNEL);
 	if (!vaddr)
 		goto err_dma_ring;
 
@@ -545,7 +545,7 @@ int ath10k_htt_rx_alloc(struct ath10k_htt *htt)
 
 	vaddr = dma_alloc_coherent(htt->ar->dev,
 				   sizeof(*htt->rx_ring.alloc_idx.vaddr),
-				   &paddr, GFP_DMA);
+				   &paddr, GFP_KERNEL);
 	if (!vaddr)
 		goto err_dma_idx;
 
@@ -674,7 +674,7 @@ static void ath10k_htt_rx_h_rates(struct ath10k *ar,
 		rate &= ~RX_PPDU_START_RATE_FLAG;
 
 		sband = &ar->mac.sbands[status->band];
-		status->rate_idx = ath10k_mac_hw_rate_to_idx(sband, rate);
+		status->rate_idx = ath10k_mac_hw_rate_to_idx(sband, rate, cck);
 		break;
 	case HTT_RX_HT:
 	case HTT_RX_HT_WITH_TXBF:
@@ -1114,7 +1114,20 @@ static void ath10k_htt_rx_h_undecap_nwifi(struct ath10k *ar,
 	 */
 
 	/* pull decapped header and copy SA & DA */
-	hdr = (struct ieee80211_hdr *)msdu->data;
+	if ((ar->hw_params.hw_4addr_pad == ATH10K_HW_4ADDR_PAD_BEFORE) &&
+	    ieee80211_has_a4(((struct ieee80211_hdr *)first_hdr)->frame_control)) {
+		/* The QCA99X0 4 address mode pad 2 bytes at the
+		 * beginning of MSDU
+		 */
+		hdr = (struct ieee80211_hdr *)(msdu->data + 2);
+		/* The skb length need be extended 2 as the 2 bytes at the tail
+		 * be excluded due to the padding
+		 */
+		skb_put(msdu, 2);
+	} else {
+		hdr = (struct ieee80211_hdr *)(msdu->data);
+	}
+
 	hdr_len = ath10k_htt_rx_nwifi_hdrlen(ar, hdr);
 	ether_addr_copy(da, ieee80211_get_DA(hdr));
 	ether_addr_copy(sa, ieee80211_get_SA(hdr));
@@ -2127,6 +2140,18 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 }
 EXPORT_SYMBOL(ath10k_htt_t2h_msg_handler);
 
+void ath10k_htt_rx_pktlog_completion_handler(struct ath10k *ar,
+					     struct sk_buff *skb)
+{
+	struct ath10k_pktlog_10_4_hdr *hdr =
+		(struct ath10k_pktlog_10_4_hdr *)skb->data;
+
+	trace_ath10k_htt_pktlog(ar, hdr->payload,
+				sizeof(*hdr) + __le16_to_cpu(hdr->size));
+	dev_kfree_skb_any(skb);
+}
+EXPORT_SYMBOL(ath10k_htt_rx_pktlog_completion_handler);
+
 static void ath10k_htt_txrx_compl_task(unsigned long ptr)
 {
 	struct ath10k_htt *htt = (struct ath10k_htt *)ptr;
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index 16823970dbfd..b3adadb5f824 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -111,7 +111,7 @@ int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 	size = htt->max_num_pending_tx * sizeof(struct ath10k_htt_txbuf);
 	htt->txbuf.vaddr = dma_alloc_coherent(ar->dev, size,
 						  &htt->txbuf.paddr,
-						  GFP_DMA);
+						  GFP_KERNEL);
 	if (!htt->txbuf.vaddr) {
 		ath10k_err(ar, "failed to alloc tx buffer\n");
 		ret = -ENOMEM;
@@ -124,7 +124,7 @@ int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 	size = htt->max_num_pending_tx * sizeof(struct htt_msdu_ext_desc);
 	htt->frag_desc.vaddr = dma_alloc_coherent(ar->dev, size,
 						  &htt->frag_desc.paddr,
-						  GFP_DMA);
+						  GFP_KERNEL);
 	if (!htt->frag_desc.vaddr) {
 		ath10k_warn(ar, "failed to alloc fragment desc memory\n");
 		ret = -ENOMEM;
@@ -439,6 +439,35 @@ int ath10k_htt_h2t_aggr_cfg_msg(struct ath10k_htt *htt,
 	return 0;
 }
 
+static u8 ath10k_htt_tx_get_vdev_id(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ath10k_skb_cb *cb = ATH10K_SKB_CB(skb);
+	struct ath10k_vif *arvif = (void *)cb->vif->drv_priv;
+
+	if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)
+		return ar->scan.vdev_id;
+	else if (cb->vif)
+		return arvif->vdev_id;
+	else if (ar->monitor_started)
+		return ar->monitor_vdev_id;
+	else
+		return 0;
+}
+
+static u8 ath10k_htt_tx_get_tid(struct sk_buff *skb, bool is_eth)
+{
+	struct ieee80211_hdr *hdr = (void *)skb->data;
+	struct ath10k_skb_cb *cb = ATH10K_SKB_CB(skb);
+
+	if (!is_eth && ieee80211_is_mgmt(hdr->frame_control))
+		return HTT_DATA_TX_EXT_TID_MGMT;
+	else if (cb->flags & ATH10K_SKB_F_QOS)
+		return skb->priority % IEEE80211_QOS_CTL_TID_MASK;
+	else
+		return HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
+}
+
 int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 {
 	struct ath10k *ar = htt->ar;
@@ -446,7 +475,7 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	struct sk_buff *txdesc = NULL;
 	struct htt_cmd *cmd;
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
-	u8 vdev_id = skb_cb->vdev_id;
+	u8 vdev_id = ath10k_htt_tx_get_vdev_id(ar, msdu);
 	int len = 0;
 	int msdu_id = -1;
 	int res;
@@ -477,6 +506,13 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 
 	msdu_id = res;
 
+	if ((ieee80211_is_action(hdr->frame_control) ||
+	     ieee80211_is_deauth(hdr->frame_control) ||
+	     ieee80211_is_disassoc(hdr->frame_control)) &&
+	     ieee80211_has_protected(hdr->frame_control)) {
+		skb_put(msdu, IEEE80211_CCMP_MIC_LEN);
+	}
+
 	txdesc = ath10k_htc_alloc_skb(ar, len);
 	if (!txdesc) {
 		res = -ENOMEM;
@@ -503,8 +539,6 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	memcpy(cmd->mgmt_tx.hdr, msdu->data,
 	       min_t(int, msdu->len, HTT_MGMT_FRM_HDR_DOWNLOAD_LEN));
 
-	skb_cb->htt.txbuf = NULL;
-
 	res = ath10k_htc_send(&htt->ar->htc, htt->eid, txdesc);
 	if (res)
 		goto err_unmap_msdu;
@@ -525,21 +559,27 @@ err:
 	return res;
 }
 
-int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
+int ath10k_htt_tx(struct ath10k_htt *htt, enum ath10k_hw_txrx_mode txmode,
+		  struct sk_buff *msdu)
 {
 	struct ath10k *ar = htt->ar;
 	struct device *dev = ar->dev;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)msdu->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(msdu);
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
 	struct ath10k_hif_sg_item sg_items[2];
+	struct ath10k_htt_txbuf *txbuf;
 	struct htt_data_tx_desc_frag *frags;
-	u8 vdev_id = skb_cb->vdev_id;
-	u8 tid = skb_cb->htt.tid;
+	bool is_eth = (txmode == ATH10K_HW_TXRX_ETHERNET);
+	u8 vdev_id = ath10k_htt_tx_get_vdev_id(ar, msdu);
+	u8 tid = ath10k_htt_tx_get_tid(msdu, is_eth);
 	int prefetch_len;
 	int res;
 	u8 flags0 = 0;
 	u16 msdu_id, flags1 = 0;
+	u16 freq = 0;
 	u32 frags_paddr = 0;
+	u32 txbuf_paddr;
 	struct htt_msdu_ext_desc *ext_desc = NULL;
 	bool limit_mgmt_desc = false;
 	bool is_probe_resp = false;
@@ -567,17 +607,17 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	prefetch_len = min(htt->prefetch_len, msdu->len);
 	prefetch_len = roundup(prefetch_len, 4);
 
-	skb_cb->htt.txbuf = &htt->txbuf.vaddr[msdu_id];
-	skb_cb->htt.txbuf_paddr = htt->txbuf.paddr +
-		(sizeof(struct ath10k_htt_txbuf) * msdu_id);
+	txbuf = &htt->txbuf.vaddr[msdu_id];
+	txbuf_paddr = htt->txbuf.paddr +
+		      (sizeof(struct ath10k_htt_txbuf) * msdu_id);
 
 	if ((ieee80211_is_action(hdr->frame_control) ||
 	     ieee80211_is_deauth(hdr->frame_control) ||
 	     ieee80211_is_disassoc(hdr->frame_control)) &&
 	     ieee80211_has_protected(hdr->frame_control)) {
 		skb_put(msdu, IEEE80211_CCMP_MIC_LEN);
-	} else if (!skb_cb->htt.nohwcrypt &&
-		   skb_cb->txmode == ATH10K_HW_TXRX_RAW &&
+	} else if (!(skb_cb->flags & ATH10K_SKB_F_NO_HWCRYPT) &&
+		   txmode == ATH10K_HW_TXRX_RAW &&
 		   ieee80211_has_protected(hdr->frame_control)) {
 		skb_put(msdu, IEEE80211_CCMP_MIC_LEN);
 	}
@@ -590,7 +630,10 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 		goto err_free_msdu_id;
 	}
 
-	switch (skb_cb->txmode) {
+	if (unlikely(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN))
+		freq = ar->scan.roc_freq;
+
+	switch (txmode) {
 	case ATH10K_HW_TXRX_RAW:
 	case ATH10K_HW_TXRX_NATIVE_WIFI:
 		flags0 |= HTT_DATA_TX_DESC_FLAGS0_MAC_HDR_PRESENT;
@@ -610,16 +653,16 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 			frags_paddr =  htt->frag_desc.paddr +
 				(sizeof(struct htt_msdu_ext_desc) * msdu_id);
 		} else {
-			frags = skb_cb->htt.txbuf->frags;
+			frags = txbuf->frags;
 			frags[0].dword_addr.paddr =
 				__cpu_to_le32(skb_cb->paddr);
 			frags[0].dword_addr.len = __cpu_to_le32(msdu->len);
 			frags[1].dword_addr.paddr = 0;
 			frags[1].dword_addr.len = 0;
 
-			frags_paddr = skb_cb->htt.txbuf_paddr;
+			frags_paddr = txbuf_paddr;
 		}
-		flags0 |= SM(skb_cb->txmode, HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
+		flags0 |= SM(txmode, HTT_DATA_TX_DESC_FLAGS0_PKT_TYPE);
 		break;
 	case ATH10K_HW_TXRX_MGMT:
 		flags0 |= SM(ATH10K_HW_TXRX_MGMT,
@@ -646,17 +689,13 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	 * avoid extra memory allocations, compress data structures and thus
 	 * improve performance. */
 
-	skb_cb->htt.txbuf->htc_hdr.eid = htt->eid;
-	skb_cb->htt.txbuf->htc_hdr.len = __cpu_to_le16(
-			sizeof(skb_cb->htt.txbuf->cmd_hdr) +
-			sizeof(skb_cb->htt.txbuf->cmd_tx) +
-			prefetch_len);
-	skb_cb->htt.txbuf->htc_hdr.flags = 0;
-
-	if (skb_cb->htt.nohwcrypt)
-		flags0 |= HTT_DATA_TX_DESC_FLAGS0_NO_ENCRYPT;
+	txbuf->htc_hdr.eid = htt->eid;
+	txbuf->htc_hdr.len = __cpu_to_le16(sizeof(txbuf->cmd_hdr) +
+					   sizeof(txbuf->cmd_tx) +
+					   prefetch_len);
+	txbuf->htc_hdr.flags = 0;
 
-	if (!skb_cb->is_protected)
+	if (skb_cb->flags & ATH10K_SKB_F_NO_HWCRYPT)
 		flags0 |= HTT_DATA_TX_DESC_FLAGS0_NO_ENCRYPT;
 
 	flags1 |= SM((u16)vdev_id, HTT_DATA_TX_DESC_FLAGS1_VDEV_ID);
@@ -675,20 +714,27 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	 */
 	flags1 |= HTT_DATA_TX_DESC_FLAGS1_POSTPONED;
 
-	skb_cb->htt.txbuf->cmd_hdr.msg_type = HTT_H2T_MSG_TYPE_TX_FRM;
-	skb_cb->htt.txbuf->cmd_tx.flags0 = flags0;
-	skb_cb->htt.txbuf->cmd_tx.flags1 = __cpu_to_le16(flags1);
-	skb_cb->htt.txbuf->cmd_tx.len = __cpu_to_le16(msdu->len);
-	skb_cb->htt.txbuf->cmd_tx.id = __cpu_to_le16(msdu_id);
-	skb_cb->htt.txbuf->cmd_tx.frags_paddr = __cpu_to_le32(frags_paddr);
-	skb_cb->htt.txbuf->cmd_tx.peerid = __cpu_to_le16(HTT_INVALID_PEERID);
-	skb_cb->htt.txbuf->cmd_tx.freq = __cpu_to_le16(skb_cb->htt.freq);
+	txbuf->cmd_hdr.msg_type = HTT_H2T_MSG_TYPE_TX_FRM;
+	txbuf->cmd_tx.flags0 = flags0;
+	txbuf->cmd_tx.flags1 = __cpu_to_le16(flags1);
+	txbuf->cmd_tx.len = __cpu_to_le16(msdu->len);
+	txbuf->cmd_tx.id = __cpu_to_le16(msdu_id);
+	txbuf->cmd_tx.frags_paddr = __cpu_to_le32(frags_paddr);
+	if (ath10k_mac_tx_frm_has_freq(ar)) {
+		txbuf->cmd_tx.offchan_tx.peerid =
+				__cpu_to_le16(HTT_INVALID_PEERID);
+		txbuf->cmd_tx.offchan_tx.freq =
+				__cpu_to_le16(freq);
+	} else {
+		txbuf->cmd_tx.peerid =
+				__cpu_to_le32(HTT_INVALID_PEERID);
+	}
 
 	trace_ath10k_htt_tx(ar, msdu_id, msdu->len, vdev_id, tid);
 	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt tx flags0 %hhu flags1 %hu len %d id %hu frags_paddr %08x, msdu_paddr %08x vdev %hhu tid %hhu freq %hu\n",
 		   flags0, flags1, msdu->len, msdu_id, frags_paddr,
-		   (u32)skb_cb->paddr, vdev_id, tid, skb_cb->htt.freq);
+		   (u32)skb_cb->paddr, vdev_id, tid, freq);
 	ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt tx msdu: ",
 			msdu->data, msdu->len);
 	trace_ath10k_tx_hdr(ar, msdu->data, msdu->len);
@@ -696,12 +742,12 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 
 	sg_items[0].transfer_id = 0;
 	sg_items[0].transfer_context = NULL;
-	sg_items[0].vaddr = &skb_cb->htt.txbuf->htc_hdr;
-	sg_items[0].paddr = skb_cb->htt.txbuf_paddr +
-			    sizeof(skb_cb->htt.txbuf->frags);
-	sg_items[0].len = sizeof(skb_cb->htt.txbuf->htc_hdr) +
-			  sizeof(skb_cb->htt.txbuf->cmd_hdr) +
-			  sizeof(skb_cb->htt.txbuf->cmd_tx);
+	sg_items[0].vaddr = &txbuf->htc_hdr;
+	sg_items[0].paddr = txbuf_paddr +
+			    sizeof(txbuf->frags);
+	sg_items[0].len = sizeof(txbuf->htc_hdr) +
+			  sizeof(txbuf->cmd_hdr) +
+			  sizeof(txbuf->cmd_tx);
 
 	sg_items[1].transfer_id = 0;
 	sg_items[1].transfer_context = NULL;
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 713c2bcea178..0678831e8671 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -286,6 +286,16 @@ struct ath10k_pktlog_hdr {
 	u8 payload[0];
 } __packed;
 
+struct ath10k_pktlog_10_4_hdr {
+	__le16 flags;
+	__le16 missed_cnt;
+	__le16 log_type;
+	__le16 size;
+	__le32 timestamp;
+	__le32 type_specific_data;
+	u8 payload[0];
+} __packed;
+
 enum ath10k_hw_rate_ofdm {
 	ATH10K_HW_RATE_OFDM_48M = 0,
 	ATH10K_HW_RATE_OFDM_24M,
@@ -307,6 +317,11 @@ enum ath10k_hw_rate_cck {
 	ATH10K_HW_RATE_CCK_SP_2M,
 };
 
+enum ath10k_hw_4addr_pad {
+	ATH10K_HW_4ADDR_PAD_AFTER,
+	ATH10K_HW_4ADDR_PAD_BEFORE,
+};
+
 /* Target specific defines for MAIN firmware */
 #define TARGET_NUM_VDEVS			8
 #define TARGET_NUM_PEER_AST			2
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 95a55405ebf0..6146a293601a 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -90,7 +90,7 @@ static u8 ath10k_mac_bitrate_to_rate(int bitrate)
 }
 
 u8 ath10k_mac_hw_rate_to_idx(const struct ieee80211_supported_band *sband,
-			     u8 hw_rate)
+			     u8 hw_rate, bool cck)
 {
 	const struct ieee80211_rate *rate;
 	int i;
@@ -98,6 +98,9 @@ u8 ath10k_mac_hw_rate_to_idx(const struct ieee80211_supported_band *sband,
 	for (i = 0; i < sband->n_bitrates; i++) {
 		rate = &sband->bitrates[i];
 
+		if (ath10k_mac_bitrate_is_cck(rate->bitrate) != cck)
+			continue;
+
 		if (rate->hw_value == hw_rate)
 			return i;
 		else if (rate->flags & IEEE80211_RATE_SHORT_PREAMBLE &&
@@ -247,7 +250,8 @@ static int ath10k_install_peer_wep_keys(struct ath10k_vif *arvif,
 	lockdep_assert_held(&ar->conf_mutex);
 
 	if (WARN_ON(arvif->vif->type != NL80211_IFTYPE_AP &&
-		    arvif->vif->type != NL80211_IFTYPE_ADHOC))
+		    arvif->vif->type != NL80211_IFTYPE_ADHOC &&
+		    arvif->vif->type != NL80211_IFTYPE_MESH_POINT))
 		return -EINVAL;
 
 	spin_lock_bh(&ar->data_lock);
@@ -1960,7 +1964,7 @@ static void ath10k_peer_assoc_h_basic(struct ath10k *ar,
 	ether_addr_copy(arg->addr, sta->addr);
 	arg->vdev_id = arvif->vdev_id;
 	arg->peer_aid = aid;
-	arg->peer_flags |= WMI_PEER_AUTH;
+	arg->peer_flags |= arvif->ar->wmi.peer_flags->auth;
 	arg->peer_listen_intval = ath10k_peer_assoc_h_listen_intval(ar, vif);
 	arg->peer_num_spatial_streams = 1;
 	arg->peer_caps = vif->bss_conf.assoc_capability;
@@ -1968,6 +1972,7 @@ static void ath10k_peer_assoc_h_basic(struct ath10k *ar,
 
 static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 				       struct ieee80211_vif *vif,
+				       struct ieee80211_sta *sta,
 				       struct wmi_peer_assoc_complete_arg *arg)
 {
 	struct ieee80211_bss_conf *info = &vif->bss_conf;
@@ -2002,12 +2007,17 @@ static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 	/* FIXME: base on RSN IE/WPA IE is a correct idea? */
 	if (rsnie || wpaie) {
 		ath10k_dbg(ar, ATH10K_DBG_WMI, "%s: rsn ie found\n", __func__);
-		arg->peer_flags |= WMI_PEER_NEED_PTK_4_WAY;
+		arg->peer_flags |= ar->wmi.peer_flags->need_ptk_4_way;
 	}
 
 	if (wpaie) {
 		ath10k_dbg(ar, ATH10K_DBG_WMI, "%s: wpa ie found\n", __func__);
-		arg->peer_flags |= WMI_PEER_NEED_GTK_2_WAY;
+		arg->peer_flags |= ar->wmi.peer_flags->need_gtk_2_way;
+	}
+
+	if (sta->mfp &&
+	    test_bit(ATH10K_FW_FEATURE_MFP_SUPPORT, ar->fw_features)) {
+		arg->peer_flags |= ar->wmi.peer_flags->pmf;
 	}
 }
 
@@ -2104,7 +2114,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 	    ath10k_peer_assoc_h_vht_masked(vht_mcs_mask))
 		return;
 
-	arg->peer_flags |= WMI_PEER_HT;
+	arg->peer_flags |= ar->wmi.peer_flags->ht;
 	arg->peer_max_mpdu = (1 << (IEEE80211_HT_MAX_AMPDU_FACTOR +
 				    ht_cap->ampdu_factor)) - 1;
 
@@ -2115,10 +2125,10 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 	arg->peer_rate_caps |= WMI_RC_HT_FLAG;
 
 	if (ht_cap->cap & IEEE80211_HT_CAP_LDPC_CODING)
-		arg->peer_flags |= WMI_PEER_LDPC;
+		arg->peer_flags |= ar->wmi.peer_flags->ldbc;
 
 	if (sta->bandwidth >= IEEE80211_STA_RX_BW_40) {
-		arg->peer_flags |= WMI_PEER_40MHZ;
+		arg->peer_flags |= ar->wmi.peer_flags->bw40;
 		arg->peer_rate_caps |= WMI_RC_CW40_FLAG;
 	}
 
@@ -2132,7 +2142,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 
 	if (ht_cap->cap & IEEE80211_HT_CAP_TX_STBC) {
 		arg->peer_rate_caps |= WMI_RC_TX_STBC_FLAG;
-		arg->peer_flags |= WMI_PEER_STBC;
+		arg->peer_flags |= ar->wmi.peer_flags->stbc;
 	}
 
 	if (ht_cap->cap & IEEE80211_HT_CAP_RX_STBC) {
@@ -2140,7 +2150,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		stbc = stbc >> IEEE80211_HT_CAP_RX_STBC_SHIFT;
 		stbc = stbc << WMI_RC_RX_STBC_FLAG_S;
 		arg->peer_rate_caps |= stbc;
-		arg->peer_flags |= WMI_PEER_STBC;
+		arg->peer_flags |= ar->wmi.peer_flags->stbc;
 	}
 
 	if (ht_cap->mcs.rx_mask[1] && ht_cap->mcs.rx_mask[2])
@@ -2321,10 +2331,10 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 	if (ath10k_peer_assoc_h_vht_masked(vht_mcs_mask))
 		return;
 
-	arg->peer_flags |= WMI_PEER_VHT;
+	arg->peer_flags |= ar->wmi.peer_flags->vht;
 
 	if (def.chan->band == IEEE80211_BAND_2GHZ)
-		arg->peer_flags |= WMI_PEER_VHT_2G;
+		arg->peer_flags |= ar->wmi.peer_flags->vht_2g;
 
 	arg->peer_vht_caps = vht_cap->cap;
 
@@ -2341,7 +2351,7 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 					ampdu_factor)) - 1);
 
 	if (sta->bandwidth == IEEE80211_STA_RX_BW_80)
-		arg->peer_flags |= WMI_PEER_80MHZ;
+		arg->peer_flags |= ar->wmi.peer_flags->bw80;
 
 	arg->peer_vht_rates.rx_max_rate =
 		__le16_to_cpu(vht_cap->vht_mcs.rx_highest);
@@ -2366,27 +2376,28 @@ static void ath10k_peer_assoc_h_qos(struct ath10k *ar,
 	switch (arvif->vdev_type) {
 	case WMI_VDEV_TYPE_AP:
 		if (sta->wme)
-			arg->peer_flags |= WMI_PEER_QOS;
+			arg->peer_flags |= arvif->ar->wmi.peer_flags->qos;
 
 		if (sta->wme && sta->uapsd_queues) {
-			arg->peer_flags |= WMI_PEER_APSD;
+			arg->peer_flags |= arvif->ar->wmi.peer_flags->apsd;
 			arg->peer_rate_caps |= WMI_RC_UAPSD_FLAG;
 		}
 		break;
 	case WMI_VDEV_TYPE_STA:
 		if (vif->bss_conf.qos)
-			arg->peer_flags |= WMI_PEER_QOS;
+			arg->peer_flags |= arvif->ar->wmi.peer_flags->qos;
 		break;
 	case WMI_VDEV_TYPE_IBSS:
 		if (sta->wme)
-			arg->peer_flags |= WMI_PEER_QOS;
+			arg->peer_flags |= arvif->ar->wmi.peer_flags->qos;
 		break;
 	default:
 		break;
 	}
 
 	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac peer %pM qos %d\n",
-		   sta->addr, !!(arg->peer_flags & WMI_PEER_QOS));
+		   sta->addr, !!(arg->peer_flags &
+		   arvif->ar->wmi.peer_flags->qos));
 }
 
 static bool ath10k_mac_sta_has_ofdm_only(struct ieee80211_sta *sta)
@@ -2479,7 +2490,7 @@ static int ath10k_peer_assoc_prepare(struct ath10k *ar,
 	memset(arg, 0, sizeof(*arg));
 
 	ath10k_peer_assoc_h_basic(ar, vif, sta, arg);
-	ath10k_peer_assoc_h_crypto(ar, vif, arg);
+	ath10k_peer_assoc_h_crypto(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_rates(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_ht(ar, vif, sta, arg);
 	ath10k_peer_assoc_h_vht(ar, vif, sta, arg);
@@ -3112,35 +3123,11 @@ void ath10k_mac_handle_tx_pause_vdev(struct ath10k *ar, u32 vdev_id,
 	spin_unlock_bh(&ar->htt.tx_lock);
 }
 
-static u8 ath10k_tx_h_get_tid(struct ieee80211_hdr *hdr)
-{
-	if (ieee80211_is_mgmt(hdr->frame_control))
-		return HTT_DATA_TX_EXT_TID_MGMT;
-
-	if (!ieee80211_is_data_qos(hdr->frame_control))
-		return HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
-
-	if (!is_unicast_ether_addr(ieee80211_get_DA(hdr)))
-		return HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
-
-	return ieee80211_get_qos_ctl(hdr)[0] & IEEE80211_QOS_CTL_TID_MASK;
-}
-
-static u8 ath10k_tx_h_get_vdev_id(struct ath10k *ar, struct ieee80211_vif *vif)
-{
-	if (vif)
-		return ath10k_vif_to_arvif(vif)->vdev_id;
-
-	if (ar->monitor_started)
-		return ar->monitor_vdev_id;
-
-	ath10k_warn(ar, "failed to resolve vdev id\n");
-	return 0;
-}
-
 static enum ath10k_hw_txrx_mode
-ath10k_tx_h_get_txmode(struct ath10k *ar, struct ieee80211_vif *vif,
-		       struct ieee80211_sta *sta, struct sk_buff *skb)
+ath10k_mac_tx_h_get_txmode(struct ath10k *ar,
+			   struct ieee80211_vif *vif,
+			   struct ieee80211_sta *sta,
+			   struct sk_buff *skb)
 {
 	const struct ieee80211_hdr *hdr = (void *)skb->data;
 	__le16 fc = hdr->frame_control;
@@ -3190,14 +3177,22 @@ ath10k_tx_h_get_txmode(struct ath10k *ar, struct ieee80211_vif *vif,
 }
 
 static bool ath10k_tx_h_use_hwcrypto(struct ieee80211_vif *vif,
-				     struct sk_buff *skb) {
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+				     struct sk_buff *skb)
+{
+	const struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	const struct ieee80211_hdr *hdr = (void *)skb->data;
 	const u32 mask = IEEE80211_TX_INTFL_DONT_ENCRYPT |
 			 IEEE80211_TX_CTL_INJECTED;
+
+	if (!ieee80211_has_protected(hdr->frame_control))
+		return false;
+
 	if ((info->flags & mask) == mask)
 		return false;
+
 	if (vif)
 		return !ath10k_vif_to_arvif(vif)->nohwcrypt;
+
 	return true;
 }
 
@@ -3224,7 +3219,7 @@ static void ath10k_tx_h_nwifi(struct ieee80211_hw *hw, struct sk_buff *skb)
 	 */
 	hdr = (void *)skb->data;
 	if (ieee80211_is_qos_nullfunc(hdr->frame_control))
-		cb->htt.tid = HTT_DATA_TX_EXT_TID_NON_QOS_MCAST_BCAST;
+		cb->flags &= ~ATH10K_SKB_F_QOS;
 
 	hdr->frame_control &= ~__cpu_to_le16(IEEE80211_STYPE_QOS_DATA);
 }
@@ -3280,7 +3275,7 @@ static void ath10k_tx_h_add_p2p_noa_ie(struct ath10k *ar,
 	}
 }
 
-static bool ath10k_mac_need_offchan_tx_work(struct ath10k *ar)
+bool ath10k_mac_tx_frm_has_freq(struct ath10k *ar)
 {
 	/* FIXME: Not really sure since when the behaviour changed. At some
 	 * point new firmware stopped requiring creation of peer entries for
@@ -3288,8 +3283,9 @@ static bool ath10k_mac_need_offchan_tx_work(struct ath10k *ar)
 	 * tx credit replenishment and reliability). Assuming it's at least 3.4
 	 * because that's when the `freq` was introduced to TX_FRM HTT command.
 	 */
-	return !(ar->htt.target_version_major >= 3 &&
-		 ar->htt.target_version_minor >= 4);
+	return (ar->htt.target_version_major >= 3 &&
+		ar->htt.target_version_minor >= 4 &&
+		ar->htt.op_version == ATH10K_FW_HTT_OP_VERSION_TLV);
 }
 
 static int ath10k_mac_tx_wmi_mgmt(struct ath10k *ar, struct sk_buff *skb)
@@ -3314,24 +3310,24 @@ unlock:
 	return ret;
 }
 
-static void ath10k_mac_tx(struct ath10k *ar, struct sk_buff *skb)
+static void ath10k_mac_tx(struct ath10k *ar, enum ath10k_hw_txrx_mode txmode,
+			  struct sk_buff *skb)
 {
-	struct ath10k_skb_cb *cb = ATH10K_SKB_CB(skb);
 	struct ath10k_htt *htt = &ar->htt;
 	int ret = 0;
 
-	switch (cb->txmode) {
+	switch (txmode) {
 	case ATH10K_HW_TXRX_RAW:
 	case ATH10K_HW_TXRX_NATIVE_WIFI:
 	case ATH10K_HW_TXRX_ETHERNET:
-		ret = ath10k_htt_tx(htt, skb);
+		ret = ath10k_htt_tx(htt, txmode, skb);
 		break;
 	case ATH10K_HW_TXRX_MGMT:
 		if (test_bit(ATH10K_FW_FEATURE_HAS_WMI_MGMT_TX,
 			     ar->fw_features))
 			ret = ath10k_mac_tx_wmi_mgmt(ar, skb);
 		else if (ar->htt.target_version_major >= 3)
-			ret = ath10k_htt_tx(htt, skb);
+			ret = ath10k_htt_tx(htt, txmode, skb);
 		else
 			ret = ath10k_htt_mgmt_tx(htt, skb);
 		break;
@@ -3361,9 +3357,13 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 {
 	struct ath10k *ar = container_of(work, struct ath10k, offchan_tx_work);
 	struct ath10k_peer *peer;
+	struct ath10k_vif *arvif;
 	struct ieee80211_hdr *hdr;
+	struct ieee80211_vif *vif;
+	struct ieee80211_sta *sta;
 	struct sk_buff *skb;
 	const u8 *peer_addr;
+	enum ath10k_hw_txrx_mode txmode;
 	int vdev_id;
 	int ret;
 	unsigned long time_left;
@@ -3388,9 +3388,9 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 
 		hdr = (struct ieee80211_hdr *)skb->data;
 		peer_addr = ieee80211_get_DA(hdr);
-		vdev_id = ATH10K_SKB_CB(skb)->vdev_id;
 
 		spin_lock_bh(&ar->data_lock);
+		vdev_id = ar->scan.vdev_id;
 		peer = ath10k_peer_find(ar, vdev_id, peer_addr);
 		spin_unlock_bh(&ar->data_lock);
 
@@ -3413,7 +3413,22 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 		ar->offchan_tx_skb = skb;
 		spin_unlock_bh(&ar->data_lock);
 
-		ath10k_mac_tx(ar, skb);
+		/* It's safe to access vif and sta - conf_mutex guarantees that
+		 * sta_state() and remove_interface() are locked exclusively
+		 * out wrt to this offchannel worker.
+		 */
+		arvif = ath10k_get_arvif(ar, vdev_id);
+		if (arvif) {
+			vif = arvif->vif;
+			sta = ieee80211_find_sta(vif, peer_addr);
+		} else {
+			vif = NULL;
+			sta = NULL;
+		}
+
+		txmode = ath10k_mac_tx_h_get_txmode(ar, vif, sta, skb);
+
+		ath10k_mac_tx(ar, txmode, skb);
 
 		time_left =
 		wait_for_completion_timeout(&ar->offchan_tx_completed, 3 * HZ);
@@ -3488,6 +3503,7 @@ void __ath10k_scan_finish(struct ath10k *ar)
 	case ATH10K_SCAN_STARTING:
 		ar->scan.state = ATH10K_SCAN_IDLE;
 		ar->scan_channel = NULL;
+		ar->scan.roc_freq = 0;
 		ath10k_offchan_tx_purge(ar);
 		cancel_delayed_work(&ar->scan.timeout);
 		complete_all(&ar->scan.completed);
@@ -3631,25 +3647,32 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 		      struct sk_buff *skb)
 {
 	struct ath10k *ar = hw->priv;
+	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_vif *vif = info->control.vif;
 	struct ieee80211_sta *sta = control->sta;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	__le16 fc = hdr->frame_control;
+	enum ath10k_hw_txrx_mode txmode;
 
 	/* We should disable CCK RATE due to P2P */
 	if (info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
 		ath10k_dbg(ar, ATH10K_DBG_MAC, "IEEE80211_TX_CTL_NO_CCK_RATE\n");
 
-	ATH10K_SKB_CB(skb)->htt.is_offchan = false;
-	ATH10K_SKB_CB(skb)->htt.freq = 0;
-	ATH10K_SKB_CB(skb)->htt.tid = ath10k_tx_h_get_tid(hdr);
-	ATH10K_SKB_CB(skb)->htt.nohwcrypt = !ath10k_tx_h_use_hwcrypto(vif, skb);
-	ATH10K_SKB_CB(skb)->vdev_id = ath10k_tx_h_get_vdev_id(ar, vif);
-	ATH10K_SKB_CB(skb)->txmode = ath10k_tx_h_get_txmode(ar, vif, sta, skb);
-	ATH10K_SKB_CB(skb)->is_protected = ieee80211_has_protected(fc);
+	txmode = ath10k_mac_tx_h_get_txmode(ar, vif, sta, skb);
+
+	skb_cb->flags = 0;
+	if (!ath10k_tx_h_use_hwcrypto(vif, skb))
+		skb_cb->flags |= ATH10K_SKB_F_NO_HWCRYPT;
+
+	if (ieee80211_is_mgmt(hdr->frame_control))
+		skb_cb->flags |= ATH10K_SKB_F_MGMT;
 
-	switch (ATH10K_SKB_CB(skb)->txmode) {
+	if (ieee80211_is_data_qos(hdr->frame_control))
+		skb_cb->flags |= ATH10K_SKB_F_QOS;
+
+	skb_cb->vif = vif;
+
+	switch (txmode) {
 	case ATH10K_HW_TXRX_MGMT:
 	case ATH10K_HW_TXRX_NATIVE_WIFI:
 		ath10k_tx_h_nwifi(hw, skb);
@@ -3668,15 +3691,7 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 	}
 
 	if (info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) {
-		spin_lock_bh(&ar->data_lock);
-		ATH10K_SKB_CB(skb)->htt.freq = ar->scan.roc_freq;
-		ATH10K_SKB_CB(skb)->vdev_id = ar->scan.vdev_id;
-		spin_unlock_bh(&ar->data_lock);
-
-		if (ath10k_mac_need_offchan_tx_work(ar)) {
-			ATH10K_SKB_CB(skb)->htt.freq = 0;
-			ATH10K_SKB_CB(skb)->htt.is_offchan = true;
-
+		if (!ath10k_mac_tx_frm_has_freq(ar)) {
 			ath10k_dbg(ar, ATH10K_DBG_MAC, "queued offchannel skb %p\n",
 				   skb);
 
@@ -3686,7 +3701,7 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 		}
 	}
 
-	ath10k_mac_tx(ar, skb);
+	ath10k_mac_tx(ar, txmode, skb);
 }
 
 /* Must not be called with conf_mutex held as workers can use that also. */
@@ -3845,7 +3860,8 @@ static struct ieee80211_sta_ht_cap ath10k_get_ht_cap(struct ath10k *ar)
 	ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_8;
 	ht_cap.cap |= IEEE80211_HT_CAP_SUP_WIDTH_20_40;
 	ht_cap.cap |= IEEE80211_HT_CAP_DSSSCCK40;
-	ht_cap.cap |= WLAN_HT_CAP_SM_PS_STATIC << IEEE80211_HT_CAP_SM_PS_SHIFT;
+	ht_cap.cap |=
+		WLAN_HT_CAP_SM_PS_DISABLED << IEEE80211_HT_CAP_SM_PS_SHIFT;
 
 	if (ar->ht_cap_info & WMI_HT_CAP_HT20_SGI)
 		ht_cap.cap |= IEEE80211_HT_CAP_SGI_20;
@@ -4350,7 +4366,9 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		arvif->vdev_type = WMI_VDEV_TYPE_IBSS;
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
-		if (!test_bit(ATH10K_FLAG_RAW_MODE, &ar->dev_flags)) {
+		if (test_bit(WMI_SERVICE_MESH, ar->wmi.svc_map)) {
+			arvif->vdev_subtype = WMI_VDEV_SUBTYPE_MESH;
+		} else if (!test_bit(ATH10K_FLAG_RAW_MODE, &ar->dev_flags)) {
 			ret = -EINVAL;
 			ath10k_warn(ar, "must load driver with rawmode=1 to add mesh interfaces\n");
 			goto err;
@@ -6907,35 +6925,39 @@ void ath10k_mac_destroy(struct ath10k *ar)
 
 static const struct ieee80211_iface_limit ath10k_if_limits[] = {
 	{
-	.max	= 8,
-	.types	= BIT(NL80211_IFTYPE_STATION)
-		| BIT(NL80211_IFTYPE_P2P_CLIENT)
+		.max	= 8,
+		.types	= BIT(NL80211_IFTYPE_STATION)
+			| BIT(NL80211_IFTYPE_P2P_CLIENT)
 	},
 	{
-	.max	= 3,
-	.types	= BIT(NL80211_IFTYPE_P2P_GO)
+		.max	= 3,
+		.types	= BIT(NL80211_IFTYPE_P2P_GO)
 	},
 	{
-	.max	= 1,
-	.types	= BIT(NL80211_IFTYPE_P2P_DEVICE)
+		.max	= 1,
+		.types	= BIT(NL80211_IFTYPE_P2P_DEVICE)
 	},
 	{
-	.max	= 7,
-	.types	= BIT(NL80211_IFTYPE_AP)
+		.max	= 7,
+		.types	= BIT(NL80211_IFTYPE_AP)
 #ifdef CONFIG_MAC80211_MESH
-		| BIT(NL80211_IFTYPE_MESH_POINT)
+			| BIT(NL80211_IFTYPE_MESH_POINT)
 #endif
 	},
 };
 
 static const struct ieee80211_iface_limit ath10k_10x_if_limits[] = {
 	{
-	.max	= 8,
-	.types	= BIT(NL80211_IFTYPE_AP)
+		.max	= 8,
+		.types	= BIT(NL80211_IFTYPE_AP)
 #ifdef CONFIG_MAC80211_MESH
-		| BIT(NL80211_IFTYPE_MESH_POINT)
+			| BIT(NL80211_IFTYPE_MESH_POINT)
 #endif
 	},
+	{
+		.max	= 1,
+		.types	= BIT(NL80211_IFTYPE_STATION)
+	},
 };
 
 static const struct ieee80211_iface_combination ath10k_if_comb[] = {
diff --git a/drivers/net/wireless/ath/ath10k/mac.h b/drivers/net/wireless/ath/ath10k/mac.h
index e3cefe4c7cfd..53091588090d 100644
--- a/drivers/net/wireless/ath/ath10k/mac.h
+++ b/drivers/net/wireless/ath/ath10k/mac.h
@@ -66,7 +66,7 @@ void ath10k_mac_handle_tx_pause_vdev(struct ath10k *ar, u32 vdev_id,
 				     enum wmi_tlv_tx_pause_action action);
 
 u8 ath10k_mac_hw_rate_to_idx(const struct ieee80211_supported_band *sband,
-			     u8 hw_rate);
+			     u8 hw_rate, bool cck);
 u8 ath10k_mac_bitrate_to_idx(const struct ieee80211_supported_band *sband,
 			     u32 bitrate);
 
@@ -74,6 +74,7 @@ void ath10k_mac_tx_lock(struct ath10k *ar, int reason);
 void ath10k_mac_tx_unlock(struct ath10k *ar, int reason);
 void ath10k_mac_vif_tx_lock(struct ath10k_vif *arvif, int reason);
 void ath10k_mac_vif_tx_unlock(struct ath10k_vif *arvif, int reason);
+bool ath10k_mac_tx_frm_has_freq(struct ath10k *ar);
 
 static inline struct ath10k_vif *ath10k_vif_to_arvif(struct ieee80211_vif *vif)
 {
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 930785a724e1..ee925c618535 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -108,6 +108,7 @@ static void ath10k_pci_htc_rx_cb(struct ath10k_ce_pipe *ce_state);
 static void ath10k_pci_htt_tx_cb(struct ath10k_ce_pipe *ce_state);
 static void ath10k_pci_htt_rx_cb(struct ath10k_ce_pipe *ce_state);
 static void ath10k_pci_htt_htc_rx_cb(struct ath10k_ce_pipe *ce_state);
+static void ath10k_pci_pktlog_rx_cb(struct ath10k_ce_pipe *ce_state);
 
 static struct ce_attr host_ce_config_wlan[] = {
 	/* CE0: host->target HTC control and raw streams */
@@ -186,6 +187,7 @@ static struct ce_attr host_ce_config_wlan[] = {
 		.src_nentries = 0,
 		.src_sz_max = 2048,
 		.dest_nentries = 128,
+		.recv_cb = ath10k_pci_pktlog_rx_cb,
 	},
 
 	/* CE9 target autonomous qcache memcpy */
@@ -485,6 +487,9 @@ static int ath10k_pci_force_wake(struct ath10k *ar)
 	unsigned long flags;
 	int ret = 0;
 
+	if (ar_pci->pci_ps)
+		return ret;
+
 	spin_lock_irqsave(&ar_pci->ps_lock, flags);
 
 	if (!ar_pci->ps_awake) {
@@ -1215,6 +1220,15 @@ static void ath10k_pci_htt_htc_rx_cb(struct ath10k_ce_pipe *ce_state)
 	ath10k_pci_process_rx_cb(ce_state, ath10k_htc_rx_completion_handler);
 }
 
+/* Called by lower (CE) layer when data is received from the Target.
+ * Only 10.4 firmware uses separate CE to transfer pktlog data.
+ */
+static void ath10k_pci_pktlog_rx_cb(struct ath10k_ce_pipe *ce_state)
+{
+	ath10k_pci_process_rx_cb(ce_state,
+				 ath10k_htt_rx_pktlog_completion_handler);
+}
+
 /* Called by lower (CE) layer when a send to HTT Target completes. */
 static void ath10k_pci_htt_tx_cb(struct ath10k_ce_pipe *ce_state)
 {
@@ -2469,12 +2483,10 @@ static int ath10k_pci_hif_resume(struct ath10k *ar)
 	u32 val;
 	int ret = 0;
 
-	if (ar_pci->pci_ps == 0) {
-		ret = ath10k_pci_force_wake(ar);
-		if (ret) {
-			ath10k_err(ar, "failed to wake up target: %d\n", ret);
-			return ret;
-		}
+	ret = ath10k_pci_force_wake(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to wake up target: %d\n", ret);
+		return ret;
 	}
 
 	/* Suspend/Resume resets the PCI configuration space, so we have to
@@ -2581,13 +2593,10 @@ static irqreturn_t ath10k_pci_interrupt_handler(int irq, void *arg)
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	int ret;
 
-	if (ar_pci->pci_ps == 0) {
-		ret = ath10k_pci_force_wake(ar);
-		if (ret) {
-			ath10k_warn(ar, "failed to wake device up on irq: %d\n",
-				    ret);
-			return IRQ_NONE;
-		}
+	ret = ath10k_pci_force_wake(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to wake device up on irq: %d\n", ret);
+		return IRQ_NONE;
 	}
 
 	if (ar_pci->num_msi_intrs == 0) {
@@ -3060,17 +3069,15 @@ static int ath10k_pci_probe(struct pci_dev *pdev,
 		goto err_sleep;
 	}
 
+	ret = ath10k_pci_force_wake(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to wake up device : %d\n", ret);
+		goto err_free_pipes;
+	}
+
 	ath10k_pci_ce_deinit(ar);
 	ath10k_pci_irq_disable(ar);
 
-	if (ar_pci->pci_ps == 0) {
-		ret = ath10k_pci_force_wake(ar);
-		if (ret) {
-			ath10k_warn(ar, "failed to wake up device : %d\n", ret);
-			goto err_free_pipes;
-		}
-	}
-
 	ret = ath10k_pci_init_irq(ar);
 	if (ret) {
 		ath10k_err(ar, "failed to init irqs: %d\n", ret);
diff --git a/drivers/net/wireless/ath/ath10k/thermal.c b/drivers/net/wireless/ath/ath10k/thermal.c
index 60fe562e3041..444b52c7e4f3 100644
--- a/drivers/net/wireless/ath/ath10k/thermal.c
+++ b/drivers/net/wireless/ath/ath10k/thermal.c
@@ -187,7 +187,7 @@ int ath10k_thermal_register(struct ath10k *ar)
 	/* Do not register hwmon device when temperature reading is not
 	 * supported by firmware
 	 */
-	if (ar->wmi.op_version != ATH10K_FW_WMI_OP_VERSION_10_2_4)
+	if (!(ar->wmi.ops->gen_pdev_get_temperature))
 		return 0;
 
 	/* Avoid linking error on devm_hwmon_device_register_with_groups, I
diff --git a/drivers/net/wireless/ath/ath10k/thermal.h b/drivers/net/wireless/ath/ath10k/thermal.h
index b610ea5caae8..c9223e9e962f 100644
--- a/drivers/net/wireless/ath/ath10k/thermal.h
+++ b/drivers/net/wireless/ath/ath10k/thermal.h
@@ -36,7 +36,7 @@ struct ath10k_thermal {
 	int temperature;
 };
 
-#ifdef CONFIG_THERMAL
+#if IS_REACHABLE(CONFIG_THERMAL)
 int ath10k_thermal_register(struct ath10k *ar);
 void ath10k_thermal_unregister(struct ath10k *ar);
 void ath10k_thermal_event_temperature(struct ath10k *ar, int temperature);
diff --git a/drivers/net/wireless/ath/ath10k/txrx.c b/drivers/net/wireless/ath/ath10k/txrx.c
index 6d1105ab4592..fbfb608e48ab 100644
--- a/drivers/net/wireless/ath/ath10k/txrx.c
+++ b/drivers/net/wireless/ath/ath10k/txrx.c
@@ -23,7 +23,12 @@
 
 static void ath10k_report_offchan_tx(struct ath10k *ar, struct sk_buff *skb)
 {
-	if (!ATH10K_SKB_CB(skb)->htt.is_offchan)
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	if (likely(!(info->flags & IEEE80211_TX_CTL_TX_OFFCHAN)))
+		return;
+
+	if (ath10k_mac_tx_frm_has_freq(ar))
 		return;
 
 	/* If the original wait_for_completion() timed out before
@@ -52,8 +57,6 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 	struct ieee80211_tx_info *info;
 	struct ath10k_skb_cb *skb_cb;
 	struct sk_buff *msdu;
-	struct ieee80211_hdr *hdr;
-	__le16 fc;
 	bool limit_mgmt_desc = false;
 
 	ath10k_dbg(ar, ATH10K_DBG_HTT,
@@ -76,10 +79,9 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 		return;
 	}
 
-	hdr = (struct ieee80211_hdr *)msdu->data;
-	fc = hdr->frame_control;
+	skb_cb = ATH10K_SKB_CB(msdu);
 
-	if (unlikely(ieee80211_is_mgmt(fc)) &&
+	if (unlikely(skb_cb->flags & ATH10K_SKB_F_MGMT) &&
 	    ar->hw_params.max_probe_resp_desc_thres)
 		limit_mgmt_desc = true;
 
@@ -89,7 +91,6 @@ void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 		wake_up(&htt->empty_tx_wq);
 	spin_unlock_bh(&htt->tx_lock);
 
-	skb_cb = ATH10K_SKB_CB(msdu);
 	dma_unmap_single(dev, skb_cb->paddr, msdu->len, DMA_TO_DEVICE);
 
 	ath10k_report_offchan_tx(htt->ar, msdu);
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.c b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
index 6fbd17b69469..3b3a27b859f3 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.c
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.c
@@ -3485,6 +3485,24 @@ static const struct wmi_ops wmi_tlv_ops = {
 	.fw_stats_fill = ath10k_wmi_main_op_fw_stats_fill,
 };
 
+static const struct wmi_peer_flags_map wmi_tlv_peer_flags_map = {
+	.auth = WMI_TLV_PEER_AUTH,
+	.qos = WMI_TLV_PEER_QOS,
+	.need_ptk_4_way = WMI_TLV_PEER_NEED_PTK_4_WAY,
+	.need_gtk_2_way = WMI_TLV_PEER_NEED_GTK_2_WAY,
+	.apsd = WMI_TLV_PEER_APSD,
+	.ht = WMI_TLV_PEER_HT,
+	.bw40 = WMI_TLV_PEER_40MHZ,
+	.stbc = WMI_TLV_PEER_STBC,
+	.ldbc = WMI_TLV_PEER_LDPC,
+	.dyn_mimops = WMI_TLV_PEER_DYN_MIMOPS,
+	.static_mimops = WMI_TLV_PEER_STATIC_MIMOPS,
+	.spatial_mux = WMI_TLV_PEER_SPATIAL_MUX,
+	.vht = WMI_TLV_PEER_VHT,
+	.bw80 = WMI_TLV_PEER_80MHZ,
+	.pmf = WMI_TLV_PEER_PMF,
+};
+
 /************/
 /* TLV init */
 /************/
@@ -3495,4 +3513,5 @@ void ath10k_wmi_tlv_attach(struct ath10k *ar)
 	ar->wmi.vdev_param = &wmi_tlv_vdev_param_map;
 	ar->wmi.pdev_param = &wmi_tlv_pdev_param_map;
 	ar->wmi.ops = &wmi_tlv_ops;
+	ar->wmi.peer_flags = &wmi_tlv_peer_flags_map;
 }
diff --git a/drivers/net/wireless/ath/ath10k/wmi-tlv.h b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
index ad655c44afdb..dd678590531a 100644
--- a/drivers/net/wireless/ath/ath10k/wmi-tlv.h
+++ b/drivers/net/wireless/ath/ath10k/wmi-tlv.h
@@ -527,6 +527,24 @@ enum wmi_tlv_vdev_param {
 	WMI_TLV_VDEV_PARAM_IBSS_PS_1RX_CHAIN_IN_ATIM_WINDOW_ENABLE,
 };
 
+enum wmi_tlv_peer_flags {
+	WMI_TLV_PEER_AUTH = 0x00000001,
+	WMI_TLV_PEER_QOS = 0x00000002,
+	WMI_TLV_PEER_NEED_PTK_4_WAY = 0x00000004,
+	WMI_TLV_PEER_NEED_GTK_2_WAY = 0x00000010,
+	WMI_TLV_PEER_APSD = 0x00000800,
+	WMI_TLV_PEER_HT = 0x00001000,
+	WMI_TLV_PEER_40MHZ = 0x00002000,
+	WMI_TLV_PEER_STBC = 0x00008000,
+	WMI_TLV_PEER_LDPC = 0x00010000,
+	WMI_TLV_PEER_DYN_MIMOPS = 0x00020000,
+	WMI_TLV_PEER_STATIC_MIMOPS = 0x00040000,
+	WMI_TLV_PEER_SPATIAL_MUX = 0x00200000,
+	WMI_TLV_PEER_VHT = 0x02000000,
+	WMI_TLV_PEER_80MHZ = 0x04000000,
+	WMI_TLV_PEER_PMF = 0x08000000,
+};
+
 enum wmi_tlv_tag {
 	WMI_TLV_TAG_LAST_RESERVED = 15,
 
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index 7569db0f69b5..a7c3d299639b 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -1546,6 +1546,61 @@ static struct wmi_pdev_param_map wmi_10_4_pdev_param_map = {
 	.arp_dstaddr = WMI_10_4_PDEV_PARAM_ARP_DSTADDR,
 };
 
+static const struct wmi_peer_flags_map wmi_peer_flags_map = {
+	.auth = WMI_PEER_AUTH,
+	.qos = WMI_PEER_QOS,
+	.need_ptk_4_way = WMI_PEER_NEED_PTK_4_WAY,
+	.need_gtk_2_way = WMI_PEER_NEED_GTK_2_WAY,
+	.apsd = WMI_PEER_APSD,
+	.ht = WMI_PEER_HT,
+	.bw40 = WMI_PEER_40MHZ,
+	.stbc = WMI_PEER_STBC,
+	.ldbc = WMI_PEER_LDPC,
+	.dyn_mimops = WMI_PEER_DYN_MIMOPS,
+	.static_mimops = WMI_PEER_STATIC_MIMOPS,
+	.spatial_mux = WMI_PEER_SPATIAL_MUX,
+	.vht = WMI_PEER_VHT,
+	.bw80 = WMI_PEER_80MHZ,
+	.vht_2g = WMI_PEER_VHT_2G,
+	.pmf = WMI_PEER_PMF,
+};
+
+static const struct wmi_peer_flags_map wmi_10x_peer_flags_map = {
+	.auth = WMI_10X_PEER_AUTH,
+	.qos = WMI_10X_PEER_QOS,
+	.need_ptk_4_way = WMI_10X_PEER_NEED_PTK_4_WAY,
+	.need_gtk_2_way = WMI_10X_PEER_NEED_GTK_2_WAY,
+	.apsd = WMI_10X_PEER_APSD,
+	.ht = WMI_10X_PEER_HT,
+	.bw40 = WMI_10X_PEER_40MHZ,
+	.stbc = WMI_10X_PEER_STBC,
+	.ldbc = WMI_10X_PEER_LDPC,
+	.dyn_mimops = WMI_10X_PEER_DYN_MIMOPS,
+	.static_mimops = WMI_10X_PEER_STATIC_MIMOPS,
+	.spatial_mux = WMI_10X_PEER_SPATIAL_MUX,
+	.vht = WMI_10X_PEER_VHT,
+	.bw80 = WMI_10X_PEER_80MHZ,
+};
+
+static const struct wmi_peer_flags_map wmi_10_2_peer_flags_map = {
+	.auth = WMI_10_2_PEER_AUTH,
+	.qos = WMI_10_2_PEER_QOS,
+	.need_ptk_4_way = WMI_10_2_PEER_NEED_PTK_4_WAY,
+	.need_gtk_2_way = WMI_10_2_PEER_NEED_GTK_2_WAY,
+	.apsd = WMI_10_2_PEER_APSD,
+	.ht = WMI_10_2_PEER_HT,
+	.bw40 = WMI_10_2_PEER_40MHZ,
+	.stbc = WMI_10_2_PEER_STBC,
+	.ldbc = WMI_10_2_PEER_LDPC,
+	.dyn_mimops = WMI_10_2_PEER_DYN_MIMOPS,
+	.static_mimops = WMI_10_2_PEER_STATIC_MIMOPS,
+	.spatial_mux = WMI_10_2_PEER_SPATIAL_MUX,
+	.vht = WMI_10_2_PEER_VHT,
+	.bw80 = WMI_10_2_PEER_80MHZ,
+	.vht_2g = WMI_10_2_PEER_VHT_2G,
+	.pmf = WMI_10_2_PEER_PMF,
+};
+
 void ath10k_wmi_put_wmi_channel(struct wmi_channel *ch,
 				const struct wmi_channel_arg *arg)
 {
@@ -1660,6 +1715,8 @@ static void ath10k_wmi_tx_beacon_nowait(struct ath10k_vif *arvif)
 	struct ath10k *ar = arvif->ar;
 	struct ath10k_skb_cb *cb;
 	struct sk_buff *bcn;
+	bool dtim_zero;
+	bool deliver_cab;
 	int ret;
 
 	spin_lock_bh(&ar->data_lock);
@@ -1679,12 +1736,14 @@ static void ath10k_wmi_tx_beacon_nowait(struct ath10k_vif *arvif)
 		arvif->beacon_state = ATH10K_BEACON_SENDING;
 		spin_unlock_bh(&ar->data_lock);
 
+		dtim_zero = !!(cb->flags & ATH10K_SKB_F_DTIM_ZERO);
+		deliver_cab = !!(cb->flags & ATH10K_SKB_F_DELIVER_CAB);
 		ret = ath10k_wmi_beacon_send_ref_nowait(arvif->ar,
 							arvif->vdev_id,
 							bcn->data, bcn->len,
 							cb->paddr,
-							cb->bcn.dtim_zero,
-							cb->bcn.deliver_cab);
+							dtim_zero,
+							deliver_cab);
 
 		spin_lock_bh(&ar->data_lock);
 
@@ -1755,16 +1814,24 @@ int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id)
 static struct sk_buff *
 ath10k_wmi_op_gen_mgmt_tx(struct ath10k *ar, struct sk_buff *msdu)
 {
+	struct ath10k_skb_cb *cb = ATH10K_SKB_CB(msdu);
+	struct ath10k_vif *arvif = (void *)cb->vif->drv_priv;
 	struct wmi_mgmt_tx_cmd *cmd;
 	struct ieee80211_hdr *hdr;
 	struct sk_buff *skb;
 	int len;
+	u32 vdev_id;
 	u32 buf_len = msdu->len;
 	u16 fc;
 
 	hdr = (struct ieee80211_hdr *)msdu->data;
 	fc = le16_to_cpu(hdr->frame_control);
 
+	if (cb->vif)
+		vdev_id = arvif->vdev_id;
+	else
+		vdev_id = 0;
+
 	if (WARN_ON_ONCE(!ieee80211_is_mgmt(hdr->frame_control)))
 		return ERR_PTR(-EINVAL);
 
@@ -1786,7 +1853,7 @@ ath10k_wmi_op_gen_mgmt_tx(struct ath10k *ar, struct sk_buff *msdu)
 
 	cmd = (struct wmi_mgmt_tx_cmd *)skb->data;
 
-	cmd->hdr.vdev_id = __cpu_to_le32(ATH10K_SKB_CB(msdu)->vdev_id);
+	cmd->hdr.vdev_id = __cpu_to_le32(vdev_id);
 	cmd->hdr.tx_rate = 0;
 	cmd->hdr.tx_power = 0;
 	cmd->hdr.buf_len = __cpu_to_le32(buf_len);
@@ -2204,22 +2271,9 @@ int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 	ath10k_dbg(ar, ATH10K_DBG_MGMT,
 		   "event mgmt rx status %08x\n", rx_status);
 
-	if (test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags)) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (rx_status & WMI_RX_STATUS_ERR_DECRYPT) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (rx_status & WMI_RX_STATUS_ERR_KEY_CACHE_MISS) {
-		dev_kfree_skb(skb);
-		return 0;
-	}
-
-	if (rx_status & WMI_RX_STATUS_ERR_CRC) {
+	if ((test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags)) ||
+	    (rx_status & (WMI_RX_STATUS_ERR_DECRYPT |
+	    WMI_RX_STATUS_ERR_KEY_CACHE_MISS | WMI_RX_STATUS_ERR_CRC))) {
 		dev_kfree_skb(skb);
 		return 0;
 	}
@@ -3115,10 +3169,10 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 	memcpy(tim->virtual_map, arvif->u.ap.tim_bitmap, pvm_len);
 
 	if (tim->dtim_count == 0) {
-		ATH10K_SKB_CB(bcn)->bcn.dtim_zero = true;
+		ATH10K_SKB_CB(bcn)->flags |= ATH10K_SKB_F_DTIM_ZERO;
 
 		if (__le32_to_cpu(tim_info->tim_mcast) == 1)
-			ATH10K_SKB_CB(bcn)->bcn.deliver_cab = true;
+			ATH10K_SKB_CB(bcn)->flags |= ATH10K_SKB_F_DELIVER_CAB;
 	}
 
 	ath10k_dbg(ar, ATH10K_DBG_MGMT, "dtim %d/%d mcast %d pvmlen %d\n",
@@ -4258,34 +4312,58 @@ void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar, struct sk_buff *skb)
 	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_RESUME_REQ_EVENTID\n");
 }
 
-static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
-				     u32 num_units, u32 unit_len)
+static int ath10k_wmi_alloc_chunk(struct ath10k *ar, u32 req_id,
+				  u32 num_units, u32 unit_len)
 {
 	dma_addr_t paddr;
-	u32 pool_size;
+	u32 pool_size = 0;
 	int idx = ar->wmi.num_mem_chunks;
+	void *vaddr = NULL;
 
-	pool_size = num_units * round_up(unit_len, 4);
+	if (ar->wmi.num_mem_chunks == ARRAY_SIZE(ar->wmi.mem_chunks))
+		return -ENOMEM;
 
-	if (!pool_size)
-		return -EINVAL;
+	while (!vaddr && num_units) {
+		pool_size = num_units * round_up(unit_len, 4);
+		if (!pool_size)
+			return -EINVAL;
 
-	ar->wmi.mem_chunks[idx].vaddr = dma_alloc_coherent(ar->dev,
-							   pool_size,
-							   &paddr,
-							   GFP_KERNEL);
-	if (!ar->wmi.mem_chunks[idx].vaddr) {
-		ath10k_warn(ar, "failed to allocate memory chunk\n");
-		return -ENOMEM;
+		vaddr = kzalloc(pool_size, GFP_KERNEL | __GFP_NOWARN);
+		if (!vaddr)
+			num_units /= 2;
 	}
 
-	memset(ar->wmi.mem_chunks[idx].vaddr, 0, pool_size);
+	if (!num_units)
+		return -ENOMEM;
 
+	paddr = dma_map_single(ar->dev, vaddr, pool_size, DMA_TO_DEVICE);
+	if (dma_mapping_error(ar->dev, paddr)) {
+		kfree(vaddr);
+		return -ENOMEM;
+	}
+
+	ar->wmi.mem_chunks[idx].vaddr = vaddr;
 	ar->wmi.mem_chunks[idx].paddr = paddr;
 	ar->wmi.mem_chunks[idx].len = pool_size;
 	ar->wmi.mem_chunks[idx].req_id = req_id;
 	ar->wmi.num_mem_chunks++;
 
+	return num_units;
+}
+
+static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
+				     u32 num_units, u32 unit_len)
+{
+	int ret;
+
+	while (num_units) {
+		ret = ath10k_wmi_alloc_chunk(ar, req_id, num_units, unit_len);
+		if (ret < 0)
+			return ret;
+
+		num_units -= ret;
+	}
+
 	return 0;
 }
 
@@ -5061,6 +5139,9 @@ static void ath10k_wmi_10_4_op_rx(struct ath10k *ar, struct sk_buff *skb)
 	case WMI_10_4_UPDATE_STATS_EVENTID:
 		ath10k_wmi_event_update_stats(ar, skb);
 		break;
+	case WMI_10_4_PDEV_TEMPERATURE_EVENTID:
+		ath10k_wmi_event_temperature(ar, skb);
+		break;
 	default:
 		ath10k_warn(ar, "Unknown eventid: %d\n", id);
 		break;
@@ -5431,8 +5512,11 @@ static struct sk_buff *ath10k_wmi_10_2_op_gen_init(struct ath10k *ar)
 	cmd = (struct wmi_init_cmd_10_2 *)buf->data;
 
 	features = WMI_10_2_RX_BATCH_MODE;
-	if (test_bit(WMI_SERVICE_COEX_GPIO, ar->wmi.svc_map))
+
+	if (test_bit(ATH10K_FLAG_BTCOEX, &ar->dev_flags) &&
+	    test_bit(WMI_SERVICE_COEX_GPIO, ar->wmi.svc_map))
 		features |= WMI_10_2_COEX_GPIO;
+
 	cmd->resource_config.feature_mask = __cpu_to_le32(features);
 
 	memcpy(&cmd->resource_config.common, &config, sizeof(config));
@@ -6328,6 +6412,16 @@ ath10k_wmi_peer_assoc_fill_10_2(struct ath10k *ar, void *buf,
 	cmd->info0 = __cpu_to_le32(info0);
 }
 
+static void
+ath10k_wmi_peer_assoc_fill_10_4(struct ath10k *ar, void *buf,
+				const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct wmi_10_4_peer_assoc_complete_cmd *cmd = buf;
+
+	ath10k_wmi_peer_assoc_fill_10_2(ar, buf, arg);
+	cmd->peer_bw_rxnss_override = 0;
+}
+
 static int
 ath10k_wmi_peer_assoc_check_arg(const struct wmi_peer_assoc_complete_arg *arg)
 {
@@ -6417,6 +6511,31 @@ ath10k_wmi_10_2_op_gen_peer_assoc(struct ath10k *ar,
 }
 
 static struct sk_buff *
+ath10k_wmi_10_4_op_gen_peer_assoc(struct ath10k *ar,
+				  const struct wmi_peer_assoc_complete_arg *arg)
+{
+	size_t len = sizeof(struct wmi_10_4_peer_assoc_complete_cmd);
+	struct sk_buff *skb;
+	int ret;
+
+	ret = ath10k_wmi_peer_assoc_check_arg(arg);
+	if (ret)
+		return ERR_PTR(ret);
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	ath10k_wmi_peer_assoc_fill_10_4(ar, skb->data, arg);
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi peer assoc vdev %d addr %pM (%s)\n",
+		   arg->vdev_id, arg->addr,
+		   arg->peer_reassoc ? "reassociate" : "new");
+	return skb;
+}
+
+static struct sk_buff *
 ath10k_wmi_10_2_op_gen_pdev_get_temperature(struct ath10k *ar)
 {
 	struct sk_buff *skb;
@@ -7536,6 +7655,7 @@ static const struct wmi_ops wmi_10_4_ops = {
 	.gen_peer_delete = ath10k_wmi_op_gen_peer_delete,
 	.gen_peer_flush = ath10k_wmi_op_gen_peer_flush,
 	.gen_peer_set_param = ath10k_wmi_op_gen_peer_set_param,
+	.gen_peer_assoc = ath10k_wmi_10_4_op_gen_peer_assoc,
 	.gen_set_psmode = ath10k_wmi_op_gen_set_psmode,
 	.gen_set_sta_ps = ath10k_wmi_op_gen_set_sta_ps,
 	.gen_set_ap_ps = ath10k_wmi_op_gen_set_ap_ps,
@@ -7555,8 +7675,8 @@ static const struct wmi_ops wmi_10_4_ops = {
 	.fw_stats_fill = ath10k_wmi_10_4_op_fw_stats_fill,
 
 	/* shared with 10.2 */
-	.gen_peer_assoc = ath10k_wmi_10_2_op_gen_peer_assoc,
 	.gen_request_stats = ath10k_wmi_op_gen_request_stats,
+	.gen_pdev_get_temperature = ath10k_wmi_10_2_op_gen_pdev_get_temperature,
 };
 
 int ath10k_wmi_attach(struct ath10k *ar)
@@ -7567,30 +7687,35 @@ int ath10k_wmi_attach(struct ath10k *ar)
 		ar->wmi.cmd = &wmi_10_4_cmd_map;
 		ar->wmi.vdev_param = &wmi_10_4_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10_4_pdev_param_map;
+		ar->wmi.peer_flags = &wmi_10_2_peer_flags_map;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_10_2_4:
 		ar->wmi.cmd = &wmi_10_2_4_cmd_map;
 		ar->wmi.ops = &wmi_10_2_4_ops;
 		ar->wmi.vdev_param = &wmi_10_2_4_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10_2_4_pdev_param_map;
+		ar->wmi.peer_flags = &wmi_10_2_peer_flags_map;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_10_2:
 		ar->wmi.cmd = &wmi_10_2_cmd_map;
 		ar->wmi.ops = &wmi_10_2_ops;
 		ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
+		ar->wmi.peer_flags = &wmi_10_2_peer_flags_map;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_10_1:
 		ar->wmi.cmd = &wmi_10x_cmd_map;
 		ar->wmi.ops = &wmi_10_1_ops;
 		ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
+		ar->wmi.peer_flags = &wmi_10x_peer_flags_map;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_MAIN:
 		ar->wmi.cmd = &wmi_cmd_map;
 		ar->wmi.ops = &wmi_ops;
 		ar->wmi.vdev_param = &wmi_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_pdev_param_map;
+		ar->wmi.peer_flags = &wmi_peer_flags_map;
 		break;
 	case ATH10K_FW_WMI_OP_VERSION_TLV:
 		ath10k_wmi_tlv_attach(ar);
@@ -7616,10 +7741,11 @@ void ath10k_wmi_free_host_mem(struct ath10k *ar)
 
 	/* free the host memory chunks requested by firmware */
 	for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
-		dma_free_coherent(ar->dev,
-				  ar->wmi.mem_chunks[i].len,
-				  ar->wmi.mem_chunks[i].vaddr,
-				  ar->wmi.mem_chunks[i].paddr);
+		dma_unmap_single(ar->dev,
+				 ar->wmi.mem_chunks[i].paddr,
+				 ar->wmi.mem_chunks[i].len,
+				 DMA_TO_DEVICE);
+		kfree(ar->wmi.mem_chunks[i].vaddr);
 	}
 
 	ar->wmi.num_mem_chunks = 0;
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index 72a4ef709577..d85ad7855d20 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -175,6 +175,8 @@ enum wmi_service {
 	WMI_SERVICE_AUX_SPECTRAL_INTF,
 	WMI_SERVICE_AUX_CHAN_LOAD_INTF,
 	WMI_SERVICE_BSS_CHANNEL_INFO_64,
+	WMI_SERVICE_EXT_RES_CFG_SUPPORT,
+	WMI_SERVICE_MESH,
 
 	/* keep last */
 	WMI_SERVICE_MAX,
@@ -206,6 +208,11 @@ enum wmi_10x_service {
 	WMI_10X_SERVICE_SMART_ANTENNA_HW_SUPPORT,
 	WMI_10X_SERVICE_ATF,
 	WMI_10X_SERVICE_COEX_GPIO,
+	WMI_10X_SERVICE_AUX_SPECTRAL_INTF,
+	WMI_10X_SERVICE_AUX_CHAN_LOAD_INTF,
+	WMI_10X_SERVICE_BSS_CHANNEL_INFO_64,
+	WMI_10X_SERVICE_MESH,
+	WMI_10X_SERVICE_EXT_RES_CFG_SUPPORT,
 };
 
 enum wmi_main_service {
@@ -286,6 +293,8 @@ enum wmi_10_4_service {
 	WMI_10_4_SERVICE_AUX_SPECTRAL_INTF,
 	WMI_10_4_SERVICE_AUX_CHAN_LOAD_INTF,
 	WMI_10_4_SERVICE_BSS_CHANNEL_INFO_64,
+	WMI_10_4_SERVICE_EXT_RES_CFG_SUPPORT,
+	WMI_10_4_SERVICE_MESH,
 };
 
 static inline char *wmi_service_name(int service_id)
@@ -375,6 +384,8 @@ static inline char *wmi_service_name(int service_id)
 	SVCSTR(WMI_SERVICE_AUX_SPECTRAL_INTF);
 	SVCSTR(WMI_SERVICE_AUX_CHAN_LOAD_INTF);
 	SVCSTR(WMI_SERVICE_BSS_CHANNEL_INFO_64);
+	SVCSTR(WMI_SERVICE_EXT_RES_CFG_SUPPORT);
+	SVCSTR(WMI_SERVICE_MESH);
 	default:
 		return NULL;
 	}
@@ -442,6 +453,16 @@ static inline void wmi_10x_svc_map(const __le32 *in, unsigned long *out,
 	       WMI_SERVICE_ATF, len);
 	SVCMAP(WMI_10X_SERVICE_COEX_GPIO,
 	       WMI_SERVICE_COEX_GPIO, len);
+	SVCMAP(WMI_10X_SERVICE_AUX_SPECTRAL_INTF,
+	       WMI_SERVICE_AUX_SPECTRAL_INTF, len);
+	SVCMAP(WMI_10X_SERVICE_AUX_CHAN_LOAD_INTF,
+	       WMI_SERVICE_AUX_CHAN_LOAD_INTF, len);
+	SVCMAP(WMI_10X_SERVICE_BSS_CHANNEL_INFO_64,
+	       WMI_SERVICE_BSS_CHANNEL_INFO_64, len);
+	SVCMAP(WMI_10X_SERVICE_MESH,
+	       WMI_SERVICE_MESH, len);
+	SVCMAP(WMI_10X_SERVICE_EXT_RES_CFG_SUPPORT,
+	       WMI_SERVICE_EXT_RES_CFG_SUPPORT, len);
 }
 
 static inline void wmi_main_svc_map(const __le32 *in, unsigned long *out,
@@ -600,6 +621,10 @@ static inline void wmi_10_4_svc_map(const __le32 *in, unsigned long *out,
 	       WMI_SERVICE_AUX_CHAN_LOAD_INTF, len);
 	SVCMAP(WMI_10_4_SERVICE_BSS_CHANNEL_INFO_64,
 	       WMI_SERVICE_BSS_CHANNEL_INFO_64, len);
+	SVCMAP(WMI_10_4_SERVICE_EXT_RES_CFG_SUPPORT,
+	       WMI_SERVICE_EXT_RES_CFG_SUPPORT, len);
+	SVCMAP(WMI_10_4_SERVICE_MESH,
+	       WMI_SERVICE_MESH, len);
 }
 
 #undef SVCMAP
@@ -1576,6 +1601,9 @@ enum wmi_10_4_cmd_id {
 	WMI_10_4_MU_CAL_START_CMDID,
 	WMI_10_4_SET_CCA_PARAMS_CMDID,
 	WMI_10_4_PDEV_BSS_CHAN_INFO_REQUEST_CMDID,
+	WMI_10_4_EXT_RESOURCE_CFG_CMDID,
+	WMI_10_4_VDEV_SET_IE_CMDID,
+	WMI_10_4_SET_LTEU_CONFIG_CMDID,
 	WMI_10_4_PDEV_UTF_CMDID = WMI_10_4_END_CMDID - 1,
 };
 
@@ -1638,6 +1666,7 @@ enum wmi_10_4_event_id {
 	WMI_10_4_PDEV_TEMPERATURE_EVENTID,
 	WMI_10_4_PDEV_NFCAL_POWER_ALL_CHANNELS_EVENTID,
 	WMI_10_4_PDEV_BSS_CHAN_INFO_EVENTID,
+	WMI_10_4_MU_REPORT_EVENTID,
 	WMI_10_4_PDEV_UTF_EVENTID = WMI_10_4_END_EVENTID - 1,
 };
 
@@ -3650,6 +3679,12 @@ enum wmi_10_4_pdev_param {
 	WMI_10_4_PDEV_PARAM_WAPI_MBSSID_OFFSET,
 	WMI_10_4_PDEV_PARAM_ARP_SRCADDR,
 	WMI_10_4_PDEV_PARAM_ARP_DSTADDR,
+	WMI_10_4_PDEV_PARAM_TXPOWER_DECR_DB,
+	WMI_10_4_PDEV_PARAM_RX_BATCHMODE,
+	WMI_10_4_PDEV_PARAM_PACKET_AGGR_DELAY,
+	WMI_10_4_PDEV_PARAM_ATF_OBSS_NOISE_SCH,
+	WMI_10_4_PDEV_PARAM_ATF_OBSS_NOISE_SCALING_FACTOR,
+	WMI_10_4_PDEV_PARAM_CUST_TXPOWER_SCALE,
 };
 
 struct wmi_pdev_set_param_cmd {
@@ -4239,6 +4274,8 @@ enum wmi_vdev_subtype {
 	WMI_VDEV_SUBTYPE_P2P_DEVICE = 1,
 	WMI_VDEV_SUBTYPE_P2P_CLIENT = 2,
 	WMI_VDEV_SUBTYPE_P2P_GO     = 3,
+	WMI_VDEV_SUBTYPE_PROXY_STA	= 4,
+	WMI_VDEV_SUBTYPE_MESH		= 5,
 };
 
 /* values for vdev_subtype */
@@ -5641,21 +5678,79 @@ struct wmi_peer_set_q_empty_callback_cmd {
 	__le32 callback_enable;
 } __packed;
 
-#define WMI_PEER_AUTH           0x00000001
-#define WMI_PEER_QOS            0x00000002
-#define WMI_PEER_NEED_PTK_4_WAY 0x00000004
-#define WMI_PEER_NEED_GTK_2_WAY 0x00000010
-#define WMI_PEER_APSD           0x00000800
-#define WMI_PEER_HT             0x00001000
-#define WMI_PEER_40MHZ          0x00002000
-#define WMI_PEER_STBC           0x00008000
-#define WMI_PEER_LDPC           0x00010000
-#define WMI_PEER_DYN_MIMOPS     0x00020000
-#define WMI_PEER_STATIC_MIMOPS  0x00040000
-#define WMI_PEER_SPATIAL_MUX    0x00200000
-#define WMI_PEER_VHT            0x02000000
-#define WMI_PEER_80MHZ          0x04000000
-#define WMI_PEER_VHT_2G         0x08000000
+struct wmi_peer_flags_map {
+	u32 auth;
+	u32 qos;
+	u32 need_ptk_4_way;
+	u32 need_gtk_2_way;
+	u32 apsd;
+	u32 ht;
+	u32 bw40;
+	u32 stbc;
+	u32 ldbc;
+	u32 dyn_mimops;
+	u32 static_mimops;
+	u32 spatial_mux;
+	u32 vht;
+	u32 bw80;
+	u32 vht_2g;
+	u32 pmf;
+};
+
+enum wmi_peer_flags {
+	WMI_PEER_AUTH = 0x00000001,
+	WMI_PEER_QOS = 0x00000002,
+	WMI_PEER_NEED_PTK_4_WAY = 0x00000004,
+	WMI_PEER_NEED_GTK_2_WAY = 0x00000010,
+	WMI_PEER_APSD = 0x00000800,
+	WMI_PEER_HT = 0x00001000,
+	WMI_PEER_40MHZ = 0x00002000,
+	WMI_PEER_STBC = 0x00008000,
+	WMI_PEER_LDPC = 0x00010000,
+	WMI_PEER_DYN_MIMOPS = 0x00020000,
+	WMI_PEER_STATIC_MIMOPS = 0x00040000,
+	WMI_PEER_SPATIAL_MUX = 0x00200000,
+	WMI_PEER_VHT = 0x02000000,
+	WMI_PEER_80MHZ = 0x04000000,
+	WMI_PEER_VHT_2G = 0x08000000,
+	WMI_PEER_PMF = 0x10000000,
+};
+
+enum wmi_10x_peer_flags {
+	WMI_10X_PEER_AUTH = 0x00000001,
+	WMI_10X_PEER_QOS = 0x00000002,
+	WMI_10X_PEER_NEED_PTK_4_WAY = 0x00000004,
+	WMI_10X_PEER_NEED_GTK_2_WAY = 0x00000010,
+	WMI_10X_PEER_APSD = 0x00000800,
+	WMI_10X_PEER_HT = 0x00001000,
+	WMI_10X_PEER_40MHZ = 0x00002000,
+	WMI_10X_PEER_STBC = 0x00008000,
+	WMI_10X_PEER_LDPC = 0x00010000,
+	WMI_10X_PEER_DYN_MIMOPS = 0x00020000,
+	WMI_10X_PEER_STATIC_MIMOPS = 0x00040000,
+	WMI_10X_PEER_SPATIAL_MUX = 0x00200000,
+	WMI_10X_PEER_VHT = 0x02000000,
+	WMI_10X_PEER_80MHZ = 0x04000000,
+};
+
+enum wmi_10_2_peer_flags {
+	WMI_10_2_PEER_AUTH = 0x00000001,
+	WMI_10_2_PEER_QOS = 0x00000002,
+	WMI_10_2_PEER_NEED_PTK_4_WAY = 0x00000004,
+	WMI_10_2_PEER_NEED_GTK_2_WAY = 0x00000010,
+	WMI_10_2_PEER_APSD = 0x00000800,
+	WMI_10_2_PEER_HT = 0x00001000,
+	WMI_10_2_PEER_40MHZ = 0x00002000,
+	WMI_10_2_PEER_STBC = 0x00008000,
+	WMI_10_2_PEER_LDPC = 0x00010000,
+	WMI_10_2_PEER_DYN_MIMOPS = 0x00020000,
+	WMI_10_2_PEER_STATIC_MIMOPS = 0x00040000,
+	WMI_10_2_PEER_SPATIAL_MUX = 0x00200000,
+	WMI_10_2_PEER_VHT = 0x02000000,
+	WMI_10_2_PEER_80MHZ = 0x04000000,
+	WMI_10_2_PEER_VHT_2G = 0x08000000,
+	WMI_10_2_PEER_PMF = 0x10000000,
+};
 
 /*
  * Peer rate capabilities.
@@ -5721,6 +5816,11 @@ struct wmi_10_2_peer_assoc_complete_cmd {
 	__le32 info0; /* WMI_PEER_ASSOC_INFO0_ */
 } __packed;
 
+struct wmi_10_4_peer_assoc_complete_cmd {
+	struct wmi_10_2_peer_assoc_complete_cmd cmd;
+	__le32 peer_bw_rxnss_override;
+} __packed;
+
 struct wmi_peer_assoc_complete_arg {
 	u8 addr[ETH_ALEN];
 	u32 vdev_id;
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 342563a3706f..3d946d8b2db2 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -767,7 +767,7 @@ ath5k_txbuf_setup(struct ath5k_hw *ah, struct ath5k_buf *bf,
 	if (info->flags & IEEE80211_TX_CTL_NO_ACK)
 		flags |= AR5K_TXDESC_NOACK;
 
-	rc_flags = info->control.rates[0].flags;
+	rc_flags = bf->rates[0].flags;
 
 	hw_rate = ath5k_get_rate_hw_value(ah->hw, info, bf, 0);
 
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 81ac8c59f0ec..7f3f94fbf157 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -3930,8 +3930,8 @@ int ath6kl_cfg80211_init(struct ath6kl *ar)
 		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
 		ath6kl_band_2ghz.ht_cap.mcs.rx_mask[1] = 0xff;
 		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[1] = 0xff;
-		ar->hw.tx_ant = 2;
-		ar->hw.rx_ant = 2;
+		ar->hw.tx_ant = 0x3; /* mask, 2 antenna */
+		ar->hw.rx_ant = 0x3;
 	} else {
 		ath6kl_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
 		ath6kl_band_5ghz.ht_cap.mcs.rx_mask[0] = 0xff;
diff --git a/drivers/net/wireless/ath/ath6kl/htc_mbox.c b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
index fffb65b3e652..65c31da43c47 100644
--- a/drivers/net/wireless/ath/ath6kl/htc_mbox.c
+++ b/drivers/net/wireless/ath/ath6kl/htc_mbox.c
@@ -2222,8 +2222,9 @@ int ath6kl_htc_rxmsg_pending_handler(struct htc_target *target,
 	}
 
 	if (status) {
-		ath6kl_err("failed to get pending recv messages: %d\n",
-			   status);
+		if (status != -ECANCELED)
+			ath6kl_err("failed to get pending recv messages: %d\n",
+				   status);
 
 		/* cleanup any packets in sync completion queue */
 		list_for_each_entry_safe(packets, tmp_pkt, &comp_pktq, list) {
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index 6ae0734f86e0..da557dc742e6 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -954,8 +954,10 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 	snprintf(filename, sizeof(filename), "%s/%s", ar->hw.fw.dir, name);
 
 	ret = request_firmware(&fw, filename, ar->dev);
-	if (ret)
+	if (ret) {
+		ath6kl_err("Failed request firmware, rv: %d\n", ret);
 		return ret;
+	}
 
 	data = fw->data;
 	len = fw->size;
@@ -964,11 +966,15 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 	magic_len = strlen(ATH6KL_FIRMWARE_MAGIC) + 1;
 
 	if (len < magic_len) {
+		ath6kl_err("Magic length is invalid, len: %zd  magic_len: %zd\n",
+			   len, magic_len);
 		ret = -EINVAL;
 		goto out;
 	}
 
 	if (memcmp(data, ATH6KL_FIRMWARE_MAGIC, magic_len) != 0) {
+		ath6kl_err("Magic is invalid, magic_len: %zd\n",
+			   magic_len);
 		ret = -EINVAL;
 		goto out;
 	}
@@ -987,7 +993,12 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 		len -= sizeof(*hdr);
 		data += sizeof(*hdr);
 
+		ath6kl_dbg(ATH6KL_DBG_BOOT, "ie-id: %d  len: %zd (0x%zx)\n",
+			   ie_id, ie_len, ie_len);
+
 		if (len < ie_len) {
+			ath6kl_err("IE len is invalid, len: %zd  ie_len: %zd  ie-id: %d\n",
+				   len, ie_len, ie_id);
 			ret = -EINVAL;
 			goto out;
 		}
@@ -1008,6 +1019,7 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 			ar->fw_otp = kmemdup(data, ie_len, GFP_KERNEL);
 
 			if (ar->fw_otp == NULL) {
+				ath6kl_err("fw_otp cannot be allocated\n");
 				ret = -ENOMEM;
 				goto out;
 			}
@@ -1025,6 +1037,7 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 			ar->fw = vmalloc(ie_len);
 
 			if (ar->fw == NULL) {
+				ath6kl_err("fw storage cannot be allocated, len: %zd\n", ie_len);
 				ret = -ENOMEM;
 				goto out;
 			}
@@ -1039,6 +1052,7 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 			ar->fw_patch = kmemdup(data, ie_len, GFP_KERNEL);
 
 			if (ar->fw_patch == NULL) {
+				ath6kl_err("fw_patch storage cannot be allocated, len: %zd\n", ie_len);
 				ret = -ENOMEM;
 				goto out;
 			}
diff --git a/drivers/net/wireless/ath/ath9k/Kconfig b/drivers/net/wireless/ath/ath9k/Kconfig
index fee0cadb0f5e..40fa915d6f35 100644
--- a/drivers/net/wireless/ath/ath9k/Kconfig
+++ b/drivers/net/wireless/ath/ath9k/Kconfig
@@ -176,3 +176,14 @@ config ATH9K_HTC_DEBUGFS
 	depends on ATH9K_HTC && DEBUG_FS
 	---help---
 	  Say Y, if you need access to ath9k_htc's statistics.
+
+config ATH9K_HWRNG
+	bool "Random number generator support"
+	depends on ATH9K && (HW_RANDOM = y || HW_RANDOM = ATH9K)
+	default y
+	---help---
+	  This option incorporates the ADC register output as a source of
+	  randomness into Linux entropy pool (/dev/urandom and /dev/random)
+
+	  Say Y, feeds the entropy directly from the WiFi driver to the input
+	  pool.
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index ecda613c2d54..76f9dc37500b 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -15,6 +15,7 @@ ath9k-$(CONFIG_ATH9K_DFS_DEBUGFS) += dfs_debug.o
 ath9k-$(CONFIG_ATH9K_DFS_CERTIFIED) += dfs.o
 ath9k-$(CONFIG_ATH9K_TX99) += tx99.o
 ath9k-$(CONFIG_ATH9K_WOW) += wow.o
+ath9k-$(CONFIG_ATH9K_HWRNG) += rng.o
 
 ath9k-$(CONFIG_ATH9K_DEBUGFS) += debug.o
 
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index b42f4a963ef4..5294595da5a7 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -23,6 +23,7 @@
 #include <linux/leds.h>
 #include <linux/completion.h>
 #include <linux/time.h>
+#include <linux/hw_random.h>
 
 #include "common.h"
 #include "debug.h"
@@ -981,6 +982,7 @@ struct ath_softc {
 	struct ath_offchannel offchannel;
 	struct ath_chanctx *next_chan;
 	struct completion go_beacon;
+	struct timespec last_event_time;
 #endif
 
 	unsigned long driver_data;
@@ -1040,6 +1042,11 @@ struct ath_softc {
 	u32 wow_intr_before_sleep;
 	bool force_wow;
 #endif
+
+#ifdef CONFIG_ATH9K_HWRNG
+	u32 rng_last;
+	struct task_struct *rng_task;
+#endif
 };
 
 /********/
@@ -1062,6 +1069,22 @@ static inline int ath9k_tx99_send(struct ath_softc *sc,
 }
 #endif /* CONFIG_ATH9K_TX99 */
 
+/***************************/
+/* Random Number Generator */
+/***************************/
+#ifdef CONFIG_ATH9K_HWRNG
+void ath9k_rng_start(struct ath_softc *sc);
+void ath9k_rng_stop(struct ath_softc *sc);
+#else
+static inline void ath9k_rng_start(struct ath_softc *sc)
+{
+}
+
+static inline void ath9k_rng_stop(struct ath_softc *sc)
+{
+}
+#endif
+
 static inline void ath_read_cachesize(struct ath_common *common, int *csz)
 {
 	common->bus_ops->read_cachesize(common, csz);
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index f50a6bc5d06e..5cf0cd7cb2d1 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -148,7 +148,8 @@ static struct ath_buf *ath9k_beacon_generate(struct ieee80211_hw *hw,
 
 	ath_assign_seq(common, skb);
 
-	if (vif->p2p)
+	/* Always assign NOA attr when MCC enabled */
+	if (ath9k_is_chanctx_enabled())
 		ath9k_beacon_add_noa(sc, avp, skb);
 
 	bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
diff --git a/drivers/net/wireless/ath/ath9k/channel.c b/drivers/net/wireless/ath/ath9k/channel.c
index 90f5773a1a61..50e614b915f1 100644
--- a/drivers/net/wireless/ath/ath9k/channel.c
+++ b/drivers/net/wireless/ath/ath9k/channel.c
@@ -226,6 +226,20 @@ static const char *chanctx_state_string(enum ath_chanctx_state state)
 	}
 }
 
+static const u32 chanctx_event_delta(struct ath_softc *sc)
+{
+	u64 ms;
+	struct timespec ts, *old;
+
+	getrawmonotonic(&ts);
+	old = &sc->last_event_time;
+	ms = ts.tv_sec * 1000 + ts.tv_nsec / 1000000;
+	ms -= old->tv_sec * 1000 + old->tv_nsec / 1000000;
+	sc->last_event_time = ts;
+
+	return (u32)ms;
+}
+
 void ath_chanctx_check_active(struct ath_softc *sc, struct ath_chanctx *ctx)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
@@ -356,14 +370,16 @@ static void ath_chanctx_setup_timer(struct ath_softc *sc, u32 tsf_time)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_hw *ah = sc->sc_ah;
+	unsigned long timeout;
 
 	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, tsf_time, 1000000);
 	tsf_time -= ath9k_hw_gettsf32(ah);
-	tsf_time = msecs_to_jiffies(tsf_time / 1000) + 1;
-	mod_timer(&sc->sched.timer, jiffies + tsf_time);
+	timeout = msecs_to_jiffies(tsf_time / 1000) + 1;
+	mod_timer(&sc->sched.timer, jiffies + timeout);
 
 	ath_dbg(common, CHAN_CTX,
-		"Setup chanctx timer with timeout: %d ms\n", jiffies_to_msecs(tsf_time));
+		"Setup chanctx timer with timeout: %d (%d) ms\n",
+		tsf_time / 1000, jiffies_to_msecs(timeout));
 }
 
 static void ath_chanctx_handle_bmiss(struct ath_softc *sc,
@@ -403,7 +419,7 @@ static void ath_chanctx_offchannel_noa(struct ath_softc *sc,
 	avp->offchannel_duration = sc->sched.offchannel_duration;
 
 	ath_dbg(common, CHAN_CTX,
-		"offchannel noa_duration: %d, noa_start: %d, noa_index: %d\n",
+		"offchannel noa_duration: %d, noa_start: %u, noa_index: %d\n",
 		avp->offchannel_duration,
 		avp->offchannel_start,
 		avp->noa_index);
@@ -443,7 +459,7 @@ static void ath_chanctx_set_periodic_noa(struct ath_softc *sc,
 		avp->periodic_noa = true;
 
 	ath_dbg(common, CHAN_CTX,
-		"noa_duration: %d, noa_start: %d, noa_index: %d, periodic: %d\n",
+		"noa_duration: %d, noa_start: %u, noa_index: %d, periodic: %d\n",
 		avp->noa_duration,
 		avp->noa_start,
 		avp->noa_index,
@@ -464,7 +480,7 @@ static void ath_chanctx_set_oneshot_noa(struct ath_softc *sc,
 	avp->noa_duration = duration + sc->sched.channel_switch_time;
 
 	ath_dbg(common, CHAN_CTX,
-		"oneshot noa_duration: %d, noa_start: %d, noa_index: %d, periodic: %d\n",
+		"oneshot noa_duration: %d, noa_start: %u, noa_index: %d, periodic: %d\n",
 		avp->noa_duration,
 		avp->noa_start,
 		avp->noa_index,
@@ -487,10 +503,11 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 
 	spin_lock_bh(&sc->chan_lock);
 
-	ath_dbg(common, CHAN_CTX, "cur_chan: %d MHz, event: %s, state: %s\n",
+	ath_dbg(common, CHAN_CTX, "cur_chan: %d MHz, event: %s, state: %s, delta: %u ms\n",
 		sc->cur_chan->chandef.center_freq1,
 		chanctx_event_string(ev),
-		chanctx_state_string(sc->sched.state));
+		chanctx_state_string(sc->sched.state),
+		chanctx_event_delta(sc));
 
 	switch (ev) {
 	case ATH_CHANCTX_EVENT_BEACON_PREPARE:
@@ -1099,6 +1116,7 @@ ath_chanctx_send_vif_ps_frame(struct ath_softc *sc, struct ath_vif *avp,
 			nullfunc->frame_control |=
 				cpu_to_le16(IEEE80211_FCTL_PM);
 
+		skb->priority = 7;
 		skb_set_queue_mapping(skb, IEEE80211_AC_VO);
 		if (!ieee80211_tx_prepare_skb(sc->hw, vif, skb, band, &sta)) {
 			dev_kfree_skb_any(skb);
@@ -1401,8 +1419,9 @@ void ath9k_chanctx_wake_queues(struct ath_softc *sc, struct ath_chanctx *ctx)
 
 static void ath9k_update_p2p_ps_timer(struct ath_softc *sc, struct ath_vif *avp)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_hw *ah = sc->sc_ah;
-	s32 tsf, target_tsf;
+	u32 tsf, target_tsf;
 
 	if (!avp || !avp->noa.has_next_tsf)
 		return;
@@ -1414,11 +1433,17 @@ static void ath9k_update_p2p_ps_timer(struct ath_softc *sc, struct ath_vif *avp)
 	target_tsf = avp->noa.next_tsf;
 	if (!avp->noa.absent)
 		target_tsf -= ATH_P2P_PS_STOP_TIME;
+	else
+		target_tsf += ATH_P2P_PS_STOP_TIME;
 
 	if (target_tsf - tsf < ATH_P2P_PS_STOP_TIME)
 		target_tsf = tsf + ATH_P2P_PS_STOP_TIME;
 
-	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, (u32) target_tsf, 1000000);
+	ath_dbg(common, CHAN_CTX, "%s absent %d tsf 0x%08X next_tsf 0x%08X (%dms)\n",
+		__func__, avp->noa.absent, tsf, target_tsf,
+		(target_tsf - tsf) / 1000);
+
+	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, target_tsf, 1000000);
 }
 
 static void ath9k_update_p2p_ps(struct ath_softc *sc, struct ieee80211_vif *vif)
@@ -1433,6 +1458,10 @@ static void ath9k_update_p2p_ps(struct ath_softc *sc, struct ieee80211_vif *vif)
 		return;
 
 	sc->p2p_ps_vif = avp;
+
+	if (sc->ps_flags & PS_BEACON_SYNC)
+		return;
+
 	tsf = ath9k_hw_gettsf32(sc->sc_ah);
 	ieee80211_parse_p2p_noa(&vif->bss_conf.p2p_noa_attr, &avp->noa, tsf);
 	ath9k_update_p2p_ps_timer(sc, avp);
@@ -1495,6 +1524,8 @@ void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
 
 	noa->index = avp->noa_index;
 	noa->oppps_ctwindow = ath9k_get_ctwin(sc, avp);
+	if (noa->oppps_ctwindow)
+		noa->oppps_ctwindow |= BIT(7);
 
 	if (avp->noa_duration) {
 		if (avp->periodic_noa) {
@@ -1536,6 +1567,8 @@ void ath9k_p2p_ps_timer(void *priv)
 	tsf = ath9k_hw_gettsf32(sc->sc_ah);
 	if (!avp->noa.absent)
 		tsf += ATH_P2P_PS_STOP_TIME;
+	else
+		tsf -= ATH_P2P_PS_STOP_TIME;
 
 	if (!avp->noa.has_next_tsf ||
 	    avp->noa.next_tsf - tsf > BIT(31))
@@ -1571,8 +1604,7 @@ void ath9k_p2p_bss_info_changed(struct ath_softc *sc,
 
 	spin_lock_bh(&sc->sc_pcu_lock);
 	spin_lock_irqsave(&sc->sc_pm_lock, flags);
-	if (!(sc->ps_flags & PS_BEACON_SYNC))
-		ath9k_update_p2p_ps(sc, vif);
+	ath9k_update_p2p_ps(sc, vif);
 	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 	spin_unlock_bh(&sc->sc_pcu_lock);
 }
diff --git a/drivers/net/wireless/ath/ath9k/common-beacon.c b/drivers/net/wireless/ath/ath9k/common-beacon.c
index 6ad44470d0f2..01d6d3205a65 100644
--- a/drivers/net/wireless/ath/ath9k/common-beacon.c
+++ b/drivers/net/wireless/ath/ath9k/common-beacon.c
@@ -18,30 +18,16 @@
 
 #define FUDGE 2
 
-/* Calculate the modulo of a 64 bit TSF snapshot with a TU divisor */
-static u32 ath9k_mod_tsf64_tu(u64 tsf, u32 div_tu)
-{
-	u32 tsf_mod, tsf_hi, tsf_lo, mod_hi, mod_lo;
-
-	tsf_mod = tsf & (BIT(10) - 1);
-	tsf_hi = tsf >> 32;
-	tsf_lo = ((u32) tsf) >> 10;
-
-	mod_hi = tsf_hi % div_tu;
-	mod_lo = ((mod_hi << 22) + tsf_lo) % div_tu;
-
-	return (mod_lo << 10) | tsf_mod;
-}
-
 static u32 ath9k_get_next_tbtt(struct ath_hw *ah, u64 tsf,
 			       unsigned int interval)
 {
-	unsigned int offset;
+	unsigned int offset, divisor;
 
 	tsf += TU_TO_USEC(FUDGE + ah->config.sw_beacon_response_time);
-	offset = ath9k_mod_tsf64_tu(tsf, interval);
+	divisor = TU_TO_USEC(interval);
+	div_u64_rem(tsf, divisor, &offset);
 
-	return (u32) tsf + TU_TO_USEC(interval) - offset;
+	return (u32) tsf + divisor - offset;
 }
 
 /*
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.c b/drivers/net/wireless/ath/ath9k/eeprom.c
index cc81482c934d..73fb4232f9f2 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom.c
@@ -138,6 +138,80 @@ bool ath9k_hw_nvram_read(struct ath_hw *ah, u32 off, u16 *data)
 	return ret;
 }
 
+int ath9k_hw_nvram_swap_data(struct ath_hw *ah, bool *swap_needed, int size)
+{
+	u16 magic;
+	u16 *eepdata;
+	int i;
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (!ath9k_hw_nvram_read(ah, AR5416_EEPROM_MAGIC_OFFSET, &magic)) {
+		ath_err(common, "Reading Magic # failed\n");
+		return -EIO;
+	}
+
+	*swap_needed = false;
+	if (swab16(magic) == AR5416_EEPROM_MAGIC) {
+		if (ah->ah_flags & AH_NO_EEP_SWAP) {
+			ath_info(common,
+				 "Ignoring endianness difference in EEPROM magic bytes.\n");
+		} else {
+			*swap_needed = true;
+		}
+	} else if (magic != AR5416_EEPROM_MAGIC) {
+		if (ath9k_hw_use_flash(ah))
+			return 0;
+
+		ath_err(common,
+			"Invalid EEPROM Magic (0x%04x).\n", magic);
+		return -EINVAL;
+	}
+
+	eepdata = (u16 *)(&ah->eeprom);
+
+	if (*swap_needed) {
+		ath_dbg(common, EEPROM,
+			"EEPROM Endianness is not native.. Changing.\n");
+
+		for (i = 0; i < size; i++)
+			eepdata[i] = swab16(eepdata[i]);
+	}
+
+	return 0;
+}
+
+bool ath9k_hw_nvram_validate_checksum(struct ath_hw *ah, int size)
+{
+	u32 i, sum = 0;
+	u16 *eepdata = (u16 *)(&ah->eeprom);
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	for (i = 0; i < size; i++)
+		sum ^= eepdata[i];
+
+	if (sum != 0xffff) {
+		ath_err(common, "Bad EEPROM checksum 0x%x\n", sum);
+		return false;
+	}
+
+	return true;
+}
+
+bool ath9k_hw_nvram_check_version(struct ath_hw *ah, int version, int minrev)
+{
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	if (ah->eep_ops->get_eeprom_ver(ah) != version ||
+	    ah->eep_ops->get_eeprom_rev(ah) < minrev) {
+		ath_err(common, "Bad EEPROM VER 0x%04x or REV 0x%04x\n",
+			ah->eep_ops->get_eeprom_ver(ah),
+			ah->eep_ops->get_eeprom_rev(ah));
+		return false;
+	}
+
+	return true;
+}
+
 void ath9k_hw_fill_vpd_table(u8 pwrMin, u8 pwrMax, u8 *pPwrList,
 			     u8 *pVpdList, u16 numIntercepts,
 			     u8 *pRetVpdList)
diff --git a/drivers/net/wireless/ath/ath9k/eeprom.h b/drivers/net/wireless/ath/ath9k/eeprom.h
index 40d4f62d0f16..4465c6566f20 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom.h
+++ b/drivers/net/wireless/ath/ath9k/eeprom.h
@@ -664,6 +664,9 @@ int16_t ath9k_hw_interpolate(u16 target, u16 srcLeft, u16 srcRight,
 bool ath9k_hw_get_lower_upper_index(u8 target, u8 *pList, u16 listSize,
 				    u16 *indexL, u16 *indexR);
 bool ath9k_hw_nvram_read(struct ath_hw *ah, u32 off, u16 *data);
+int ath9k_hw_nvram_swap_data(struct ath_hw *ah, bool *swap_needed, int size);
+bool ath9k_hw_nvram_validate_checksum(struct ath_hw *ah, int size);
+bool ath9k_hw_nvram_check_version(struct ath_hw *ah, int version, int minrev);
 void ath9k_hw_usb_gen_fill_eeprom(struct ath_hw *ah, u16 *eep_data,
 				  int eep_start_loc, int size);
 void ath9k_hw_fill_vpd_table(u8 pwrMin, u8 pwrMax, u8 *pPwrList,
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_4k.c b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
index 4773da6dc6f2..5da0826bf1be 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_4k.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_4k.c
@@ -177,74 +177,30 @@ static u32 ath9k_hw_4k_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 }
 #endif
 
-
-#undef SIZE_EEPROM_4K
-
 static int ath9k_hw_4k_check_eeprom(struct ath_hw *ah)
 {
-#define EEPROM_4K_SIZE (sizeof(struct ar5416_eeprom_4k) / sizeof(u16))
-	struct ath_common *common = ath9k_hw_common(ah);
 	struct ar5416_eeprom_4k *eep = &ah->eeprom.map4k;
-	u16 *eepdata, temp, magic, magic2;
-	u32 sum = 0, el;
-	bool need_swap = false;
-	int i, addr;
+	u32 el;
+	bool need_swap;
+	int i, err;
 
-
-	if (!ath9k_hw_use_flash(ah)) {
-		if (!ath9k_hw_nvram_read(ah, AR5416_EEPROM_MAGIC_OFFSET,
-					 &magic)) {
-			ath_err(common, "Reading Magic # failed\n");
-			return false;
-		}
-
-		ath_dbg(common, EEPROM, "Read Magic = 0x%04X\n", magic);
-
-		if (magic != AR5416_EEPROM_MAGIC) {
-			magic2 = swab16(magic);
-
-			if (magic2 == AR5416_EEPROM_MAGIC) {
-				need_swap = true;
-				eepdata = (u16 *) (&ah->eeprom);
-
-				for (addr = 0; addr < EEPROM_4K_SIZE; addr++) {
-					temp = swab16(*eepdata);
-					*eepdata = temp;
-					eepdata++;
-				}
-			} else {
-				ath_err(common,
-					"Invalid EEPROM Magic. Endianness mismatch.\n");
-				return -EINVAL;
-			}
-		}
-	}
-
-	ath_dbg(common, EEPROM, "need_swap = %s\n",
-		need_swap ? "True" : "False");
+	err = ath9k_hw_nvram_swap_data(ah, &need_swap, SIZE_EEPROM_4K);
+	if (err)
+		return err;
 
 	if (need_swap)
-		el = swab16(ah->eeprom.map4k.baseEepHeader.length);
-	else
-		el = ah->eeprom.map4k.baseEepHeader.length;
-
-	if (el > sizeof(struct ar5416_eeprom_4k))
-		el = sizeof(struct ar5416_eeprom_4k) / sizeof(u16);
+		el = swab16(eep->baseEepHeader.length);
 	else
-		el = el / sizeof(u16);
+		el = eep->baseEepHeader.length;
 
-	eepdata = (u16 *)(&ah->eeprom);
-
-	for (i = 0; i < el; i++)
-		sum ^= *eepdata++;
+	el = min(el / sizeof(u16), SIZE_EEPROM_4K);
+	if (!ath9k_hw_nvram_validate_checksum(ah, el))
+		return -EINVAL;
 
 	if (need_swap) {
 		u32 integer;
 		u16 word;
 
-		ath_dbg(common, EEPROM,
-			"EEPROM Endianness is not native.. Changing\n");
-
 		word = swab16(eep->baseEepHeader.length);
 		eep->baseEepHeader.length = word;
 
@@ -283,17 +239,15 @@ static int ath9k_hw_4k_check_eeprom(struct ath_hw *ah)
 		}
 	}
 
-	if (sum != 0xffff || ah->eep_ops->get_eeprom_ver(ah) != AR5416_EEP_VER ||
-	    ah->eep_ops->get_eeprom_rev(ah) < AR5416_EEP_NO_BACK_VER) {
-		ath_err(common, "Bad EEPROM checksum 0x%x or revision 0x%04x\n",
-			sum, ah->eep_ops->get_eeprom_ver(ah));
+	if (!ath9k_hw_nvram_check_version(ah, AR5416_EEP_VER,
+	    AR5416_EEP_NO_BACK_VER))
 		return -EINVAL;
-	}
 
 	return 0;
-#undef EEPROM_4K_SIZE
 }
 
+#undef SIZE_EEPROM_4K
+
 static u32 ath9k_hw_4k_get_eeprom(struct ath_hw *ah,
 				  enum eeprom_param param)
 {
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_9287.c b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
index 6ca33dfde1fd..1a019a39eda1 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_9287.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_9287.c
@@ -177,59 +177,24 @@ static u32 ath9k_hw_ar9287_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 
 static int ath9k_hw_ar9287_check_eeprom(struct ath_hw *ah)
 {
-	u32 sum = 0, el, integer;
-	u16 temp, word, magic, magic2, *eepdata;
-	int i, addr;
-	bool need_swap = false;
+	u32 el, integer;
+	u16 word;
+	int i, err;
+	bool need_swap;
 	struct ar9287_eeprom *eep = &ah->eeprom.map9287;
-	struct ath_common *common = ath9k_hw_common(ah);
-
-	if (!ath9k_hw_use_flash(ah)) {
-		if (!ath9k_hw_nvram_read(ah, AR5416_EEPROM_MAGIC_OFFSET,
-					 &magic)) {
-			ath_err(common, "Reading Magic # failed\n");
-			return false;
-		}
-
-		ath_dbg(common, EEPROM, "Read Magic = 0x%04X\n", magic);
-
-		if (magic != AR5416_EEPROM_MAGIC) {
-			magic2 = swab16(magic);
-
-			if (magic2 == AR5416_EEPROM_MAGIC) {
-				need_swap = true;
-				eepdata = (u16 *)(&ah->eeprom);
-
-				for (addr = 0; addr < SIZE_EEPROM_AR9287; addr++) {
-					temp = swab16(*eepdata);
-					*eepdata = temp;
-					eepdata++;
-				}
-			} else {
-				ath_err(common,
-					"Invalid EEPROM Magic. Endianness mismatch.\n");
-				return -EINVAL;
-			}
-		}
-	}
 
-	ath_dbg(common, EEPROM, "need_swap = %s\n",
-		need_swap ? "True" : "False");
+	err = ath9k_hw_nvram_swap_data(ah, &need_swap, SIZE_EEPROM_AR9287);
+	if (err)
+		return err;
 
 	if (need_swap)
-		el = swab16(ah->eeprom.map9287.baseEepHeader.length);
-	else
-		el = ah->eeprom.map9287.baseEepHeader.length;
-
-	if (el > sizeof(struct ar9287_eeprom))
-		el = sizeof(struct ar9287_eeprom) / sizeof(u16);
+		el = swab16(eep->baseEepHeader.length);
 	else
-		el = el / sizeof(u16);
-
-	eepdata = (u16 *)(&ah->eeprom);
+		el = eep->baseEepHeader.length;
 
-	for (i = 0; i < el; i++)
-		sum ^= *eepdata++;
+	el = min(el / sizeof(u16), SIZE_EEPROM_AR9287);
+	if (!ath9k_hw_nvram_validate_checksum(ah, el))
+		return -EINVAL;
 
 	if (need_swap) {
 		word = swab16(eep->baseEepHeader.length);
@@ -270,16 +235,15 @@ static int ath9k_hw_ar9287_check_eeprom(struct ath_hw *ah)
 		}
 	}
 
-	if (sum != 0xffff || ah->eep_ops->get_eeprom_ver(ah) != AR9287_EEP_VER
-	    || ah->eep_ops->get_eeprom_rev(ah) < AR5416_EEP_NO_BACK_VER) {
-		ath_err(common, "Bad EEPROM checksum 0x%x or revision 0x%04x\n",
-			sum, ah->eep_ops->get_eeprom_ver(ah));
+	if (!ath9k_hw_nvram_check_version(ah, AR9287_EEP_VER,
+	    AR5416_EEP_NO_BACK_VER))
 		return -EINVAL;
-	}
 
 	return 0;
 }
 
+#undef SIZE_EEPROM_AR9287
+
 static u32 ath9k_hw_ar9287_get_eeprom(struct ath_hw *ah,
 				      enum eeprom_param param)
 {
diff --git a/drivers/net/wireless/ath/ath9k/eeprom_def.c b/drivers/net/wireless/ath/ath9k/eeprom_def.c
index 056f516bf017..959682f7909c 100644
--- a/drivers/net/wireless/ath/ath9k/eeprom_def.c
+++ b/drivers/net/wireless/ath/ath9k/eeprom_def.c
@@ -126,8 +126,6 @@ static bool ath9k_hw_def_fill_eeprom(struct ath_hw *ah)
 		return __ath9k_hw_def_fill_eeprom(ah);
 }
 
-#undef SIZE_EEPROM_DEF
-
 #if defined(CONFIG_ATH9K_DEBUGFS) || defined(CONFIG_ATH9K_HTC_DEBUGFS)
 static u32 ath9k_def_dump_modal_eeprom(char *buf, u32 len, u32 size,
 				       struct modal_eep_header *modal_hdr)
@@ -257,59 +255,31 @@ static u32 ath9k_hw_def_dump_eeprom(struct ath_hw *ah, bool dump_base_hdr,
 }
 #endif
 
-
 static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 {
 	struct ar5416_eeprom_def *eep = &ah->eeprom.def;
 	struct ath_common *common = ath9k_hw_common(ah);
-	u16 *eepdata, temp, magic;
-	u32 sum = 0, el;
-	bool need_swap = false;
-	int i, addr, size;
-
-	if (!ath9k_hw_nvram_read(ah, AR5416_EEPROM_MAGIC_OFFSET, &magic)) {
-		ath_err(common, "Reading Magic # failed\n");
-		return false;
-	}
-
-	if (swab16(magic) == AR5416_EEPROM_MAGIC &&
-	    !(ah->ah_flags & AH_NO_EEP_SWAP)) {
-		size = sizeof(struct ar5416_eeprom_def);
-		need_swap = true;
-		eepdata = (u16 *) (&ah->eeprom);
-
-		for (addr = 0; addr < size / sizeof(u16); addr++) {
-			temp = swab16(*eepdata);
-			*eepdata = temp;
-			eepdata++;
-		}
-	}
+	u32 el;
+	bool need_swap;
+	int i, err;
 
-	ath_dbg(common, EEPROM, "need_swap = %s\n",
-		need_swap ? "True" : "False");
+	err = ath9k_hw_nvram_swap_data(ah, &need_swap, SIZE_EEPROM_DEF);
+	if (err)
+		return err;
 
 	if (need_swap)
-		el = swab16(ah->eeprom.def.baseEepHeader.length);
+		el = swab16(eep->baseEepHeader.length);
 	else
-		el = ah->eeprom.def.baseEepHeader.length;
+		el = eep->baseEepHeader.length;
 
-	if (el > sizeof(struct ar5416_eeprom_def))
-		el = sizeof(struct ar5416_eeprom_def) / sizeof(u16);
-	else
-		el = el / sizeof(u16);
-
-	eepdata = (u16 *)(&ah->eeprom);
-
-	for (i = 0; i < el; i++)
-		sum ^= *eepdata++;
+	el = min(el / sizeof(u16), SIZE_EEPROM_DEF);
+	if (!ath9k_hw_nvram_validate_checksum(ah, el))
+		return -EINVAL;
 
 	if (need_swap) {
 		u32 integer, j;
 		u16 word;
 
-		ath_dbg(common, EEPROM,
-			"EEPROM Endianness is not native.. Changing.\n");
-
 		word = swab16(eep->baseEepHeader.length);
 		eep->baseEepHeader.length = word;
 
@@ -356,12 +326,9 @@ static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 		}
 	}
 
-	if (sum != 0xffff || ah->eep_ops->get_eeprom_ver(ah) != AR5416_EEP_VER ||
-	    ah->eep_ops->get_eeprom_rev(ah) < AR5416_EEP_NO_BACK_VER) {
-		ath_err(common, "Bad EEPROM checksum 0x%x or revision 0x%04x\n",
-			sum, ah->eep_ops->get_eeprom_ver(ah));
+	if (!ath9k_hw_nvram_check_version(ah, AR5416_EEP_VER,
+	    AR5416_EEP_NO_BACK_VER))
 		return -EINVAL;
-	}
 
 	/* Enable fixup for AR_AN_TOP2 if necessary */
 	if ((ah->hw_version.devid == AR9280_DEVID_PCI) &&
@@ -376,6 +343,8 @@ static int ath9k_hw_def_check_eeprom(struct ath_hw *ah)
 	return 0;
 }
 
+#undef SIZE_EEPROM_DEF
+
 static u32 ath9k_hw_def_get_eeprom(struct ath_hw *ah,
 				   enum eeprom_param param)
 {
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index a680a970b7f7..fe1fd1a5ae15 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -834,7 +834,7 @@ void ath9k_htc_ani_work(struct work_struct *work)
 		if (longcal || shortcal)
 			common->ani.caldone =
 				ath9k_hw_calibrate(ah, ah->curchan,
-						   ah->rxchainmask, longcal);
+						ah->rxchainmask, longcal) > 0;
 
 		ath9k_htc_ps_restore(priv);
 	}
diff --git a/drivers/net/wireless/ath/ath9k/htc_hst.c b/drivers/net/wireless/ath/ath9k/htc_hst.c
index 2294709ee8b0..fd85f996c554 100644
--- a/drivers/net/wireless/ath/ath9k/htc_hst.c
+++ b/drivers/net/wireless/ath/ath9k/htc_hst.c
@@ -414,7 +414,7 @@ void ath9k_htc_rx_msg(struct htc_target *htc_handle,
 		return;
 	}
 
-	if (epid >= ENDPOINT_MAX) {
+	if (epid < 0 || epid >= ENDPOINT_MAX) {
 		if (pipe_id != USB_REG_IN_PIPE)
 			dev_kfree_skb_any(skb);
 		else
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 41382f89abe1..257f46ed4a04 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -2299,10 +2299,10 @@ void ath9k_hw_set_sta_beacon_timers(struct ath_hw *ah,
 	else
 		nextTbtt = bs->bs_nexttbtt;
 
-	ath_dbg(common, BEACON, "next DTIM %d\n", bs->bs_nextdtim);
-	ath_dbg(common, BEACON, "next beacon %d\n", nextTbtt);
-	ath_dbg(common, BEACON, "beacon period %d\n", beaconintval);
-	ath_dbg(common, BEACON, "DTIM period %d\n", dtimperiod);
+	ath_dbg(common, BEACON, "next DTIM %u\n", bs->bs_nextdtim);
+	ath_dbg(common, BEACON, "next beacon %u\n", nextTbtt);
+	ath_dbg(common, BEACON, "beacon period %u\n", beaconintval);
+	ath_dbg(common, BEACON, "DTIM period %u\n", dtimperiod);
 
 	ENABLE_REGWRITE_BUFFER(ah);
 
@@ -2761,9 +2761,6 @@ void ath9k_hw_setrxfilter(struct ath_hw *ah, u32 bits)
 
 	ENABLE_REGWRITE_BUFFER(ah);
 
-	if (AR_SREV_9462(ah) || AR_SREV_9565(ah))
-		bits |= ATH9K_RX_FILTER_CONTROL_WRAPPER;
-
 	REG_WRITE(ah, AR_RX_FILTER, bits);
 
 	phybits = 0;
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index 2e2b92ba96b8..ab7a1ac37849 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -828,6 +828,7 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	ieee80211_hw_set(hw, RX_INCLUDES_FCS);
 	ieee80211_hw_set(hw, HOST_BROADCAST_PS_BUFFERING);
 	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
+	ieee80211_hw_set(hw, SUPPORTS_CLONED_SKBS);
 
 	if (ath9k_ps_enable)
 		ieee80211_hw_set(hw, SUPPORTS_PS);
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index d184e682e636..c1b33fdcca08 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -739,6 +739,8 @@ static int ath9k_start(struct ieee80211_hw *hw)
 
 	ath9k_ps_restore(sc);
 
+	ath9k_rng_start(sc);
+
 	return 0;
 }
 
@@ -828,6 +830,8 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 
 	ath9k_deinit_channel_context(sc);
 
+	ath9k_rng_stop(sc);
+
 	mutex_lock(&sc->mutex);
 
 	ath_cancel_work(sc);
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 994daf6c6297..32160fca876a 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -424,6 +424,9 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	    AR_SREV_9561(sc->sc_ah))
 		rfilt |= ATH9K_RX_FILTER_4ADDRESS;
 
+	if (AR_SREV_9462(sc->sc_ah) || AR_SREV_9565(sc->sc_ah))
+		rfilt |= ATH9K_RX_FILTER_CONTROL_WRAPPER;
+
 	if (ath9k_is_chanctx_enabled() &&
 	    test_bit(ATH_OP_SCANNING, &common->op_flags))
 		rfilt |= ATH9K_RX_FILTER_BEACON;
diff --git a/drivers/net/wireless/ath/ath9k/rng.c b/drivers/net/wireless/ath/ath9k/rng.c
new file mode 100644
index 000000000000..c9cb2aad7b6f
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/rng.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2015 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/hw_random.h>
+#include <linux/kthread.h>
+
+#include "ath9k.h"
+#include "hw.h"
+#include "ar9003_phy.h"
+
+#define ATH9K_RNG_BUF_SIZE	320
+#define ATH9K_RNG_ENTROPY(x)	(((x) * 8 * 320) >> 10) /* quality: 320/1024 */
+
+static int ath9k_rng_data_read(struct ath_softc *sc, u32 *buf, u32 buf_size)
+{
+	int i, j;
+	u32  v1, v2, rng_last = sc->rng_last;
+	struct ath_hw *ah = sc->sc_ah;
+
+	ath9k_ps_wakeup(sc);
+
+	REG_RMW_FIELD(ah, AR_PHY_TEST, AR_PHY_TEST_BBB_OBS_SEL, 1);
+	REG_CLR_BIT(ah, AR_PHY_TEST, AR_PHY_TEST_RX_OBS_SEL_BIT5);
+	REG_RMW_FIELD(ah, AR_PHY_TEST_CTL_STATUS, AR_PHY_TEST_CTL_RX_OBS_SEL, 0);
+
+	for (i = 0, j = 0; i < buf_size; i++) {
+		v1 = REG_READ(ah, AR_PHY_TST_ADC) & 0xffff;
+		v2 = REG_READ(ah, AR_PHY_TST_ADC) & 0xffff;
+
+		/* wait for data ready */
+		if (v1 && v2 && rng_last != v1 && v1 != v2 && v1 != 0xffff &&
+		    v2 != 0xffff)
+			buf[j++] = (v1 << 16) | v2;
+
+		rng_last = v2;
+	}
+
+	ath9k_ps_restore(sc);
+
+	sc->rng_last = rng_last;
+
+	return j << 2;
+}
+
+static int ath9k_rng_kthread(void *data)
+{
+	int bytes_read;
+	struct ath_softc *sc = data;
+	u32 *rng_buf;
+
+	rng_buf = kmalloc_array(ATH9K_RNG_BUF_SIZE, sizeof(u32), GFP_KERNEL);
+	if (!rng_buf)
+		goto out;
+
+	while (!kthread_should_stop()) {
+		bytes_read = ath9k_rng_data_read(sc, rng_buf,
+						 ATH9K_RNG_BUF_SIZE);
+		if (unlikely(!bytes_read)) {
+			msleep_interruptible(10);
+			continue;
+		}
+
+		/* sleep until entropy bits under write_wakeup_threshold */
+		add_hwgenerator_randomness((void *)rng_buf, bytes_read,
+					   ATH9K_RNG_ENTROPY(bytes_read));
+	}
+
+	kfree(rng_buf);
+out:
+	sc->rng_task = NULL;
+
+	return 0;
+}
+
+void ath9k_rng_start(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+
+	if (sc->rng_task)
+		return;
+
+	if (!AR_SREV_9300_20_OR_LATER(ah))
+		return;
+
+	sc->rng_task = kthread_run(ath9k_rng_kthread, sc, "ath9k-hwrng");
+	if (IS_ERR(sc->rng_task))
+		sc->rng_task = NULL;
+}
+
+void ath9k_rng_stop(struct ath_softc *sc)
+{
+	if (sc->rng_task)
+		kthread_stop(sc->rng_task);
+}
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 3e3dac3d7060..fe795fc5288c 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -1473,11 +1473,14 @@ static bool ath_tx_sched_aggr(struct ath_softc *sc, struct ath_txq *txq,
 int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 		      u16 tid, u16 *ssn)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_atx_tid *txtid;
 	struct ath_txq *txq;
 	struct ath_node *an;
 	u8 density;
 
+	ath_dbg(common, XMIT, "%s called\n", __func__);
+
 	an = (struct ath_node *)sta->drv_priv;
 	txtid = ATH_AN_2_TID(an, tid);
 	txq = txtid->txq;
@@ -1512,10 +1515,13 @@ int ath_tx_aggr_start(struct ath_softc *sc, struct ieee80211_sta *sta,
 
 void ath_tx_aggr_stop(struct ath_softc *sc, struct ieee80211_sta *sta, u16 tid)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_node *an = (struct ath_node *)sta->drv_priv;
 	struct ath_atx_tid *txtid = ATH_AN_2_TID(an, tid);
 	struct ath_txq *txq = txtid->txq;
 
+	ath_dbg(common, XMIT, "%s called\n", __func__);
+
 	ath_txq_lock(sc, txq);
 	txtid->active = false;
 	ath_tx_flush_tid(sc, txtid);
@@ -1526,11 +1532,14 @@ void ath_tx_aggr_stop(struct ath_softc *sc, struct ieee80211_sta *sta, u16 tid)
 void ath_tx_aggr_sleep(struct ieee80211_sta *sta, struct ath_softc *sc,
 		       struct ath_node *an)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_atx_tid *tid;
 	struct ath_txq *txq;
 	bool buffered;
 	int tidno;
 
+	ath_dbg(common, XMIT, "%s called\n", __func__);
+
 	for (tidno = 0, tid = &an->tid[tidno];
 	     tidno < IEEE80211_NUM_TIDS; tidno++, tid++) {
 
@@ -1555,10 +1564,13 @@ void ath_tx_aggr_sleep(struct ieee80211_sta *sta, struct ath_softc *sc,
 
 void ath_tx_aggr_wakeup(struct ath_softc *sc, struct ath_node *an)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_atx_tid *tid;
 	struct ath_txq *txq;
 	int tidno;
 
+	ath_dbg(common, XMIT, "%s called\n", __func__);
+
 	for (tidno = 0, tid = &an->tid[tidno];
 	     tidno < IEEE80211_NUM_TIDS; tidno++, tid++) {
 
@@ -1579,10 +1591,13 @@ void ath_tx_aggr_wakeup(struct ath_softc *sc, struct ath_node *an)
 void ath_tx_aggr_resume(struct ath_softc *sc, struct ieee80211_sta *sta,
 			u16 tidno)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_atx_tid *tid;
 	struct ath_node *an;
 	struct ath_txq *txq;
 
+	ath_dbg(common, XMIT, "%s called\n", __func__);
+
 	an = (struct ath_node *)sta->drv_priv;
 	tid = ATH_AN_2_TID(an, tidno);
 	txq = tid->txq;
@@ -2316,6 +2331,12 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 
 	queue = ieee80211_is_data_present(hdr->frame_control);
 
+	/* If chanctx, queue all null frames while NOA could be there */
+	if (ath9k_is_chanctx_enabled() &&
+	    ieee80211_is_nullfunc(hdr->frame_control) &&
+	    !txctl->force_channel)
+		queue = true;
+
 	/* Force queueing of all frames that belong to a virtual interface on
 	 * a different channel context, to ensure that they are sent on the
 	 * correct channel.
@@ -2894,7 +2915,7 @@ int ath9k_tx99_send(struct ath_softc *sc, struct sk_buff *skb,
 		if (skb_headroom(skb) < padsize) {
 			ath_dbg(common, XMIT,
 				"tx99 padding failed\n");
-		return -EINVAL;
+			return -EINVAL;
 		}
 
 		skb_push(skb, padsize);
diff --git a/drivers/net/wireless/ath/wcn36xx/dxe.c b/drivers/net/wireless/ath/wcn36xx/dxe.c
index f8dfa05b290a..8643801f31b6 100644
--- a/drivers/net/wireless/ath/wcn36xx/dxe.c
+++ b/drivers/net/wireless/ath/wcn36xx/dxe.c
@@ -474,36 +474,37 @@ static int wcn36xx_rx_handle_packets(struct wcn36xx *wcn,
 	struct wcn36xx_dxe_desc *dxe = ctl->desc;
 	dma_addr_t  dma_addr;
 	struct sk_buff *skb;
+	int ret = 0, int_mask;
+	u32 value;
+
+	if (ch->ch_type == WCN36XX_DXE_CH_RX_L) {
+		value = WCN36XX_DXE_CTRL_RX_L;
+		int_mask = WCN36XX_DXE_INT_CH1_MASK;
+	} else {
+		value = WCN36XX_DXE_CTRL_RX_H;
+		int_mask = WCN36XX_DXE_INT_CH3_MASK;
+	}
 
 	while (!(dxe->ctrl & WCN36XX_DXE_CTRL_VALID_MASK)) {
 		skb = ctl->skb;
 		dma_addr = dxe->dst_addr_l;
-		wcn36xx_dxe_fill_skb(wcn->dev, ctl);
-
-		switch (ch->ch_type) {
-		case WCN36XX_DXE_CH_RX_L:
-			dxe->ctrl = WCN36XX_DXE_CTRL_RX_L;
-			wcn36xx_dxe_write_register(wcn, WCN36XX_DXE_ENCH_ADDR,
-						   WCN36XX_DXE_INT_CH1_MASK);
-			break;
-		case WCN36XX_DXE_CH_RX_H:
-			dxe->ctrl = WCN36XX_DXE_CTRL_RX_H;
-			wcn36xx_dxe_write_register(wcn, WCN36XX_DXE_ENCH_ADDR,
-						   WCN36XX_DXE_INT_CH3_MASK);
-			break;
-		default:
-			wcn36xx_warn("Unknown channel\n");
-		}
-
-		dma_unmap_single(wcn->dev, dma_addr, WCN36XX_PKT_SIZE,
-				 DMA_FROM_DEVICE);
-		wcn36xx_rx_skb(wcn, skb);
+		ret = wcn36xx_dxe_fill_skb(wcn->dev, ctl);
+		if (0 == ret) {
+			/* new skb allocation ok. Use the new one and queue
+			 * the old one to network system.
+			 */
+			dma_unmap_single(wcn->dev, dma_addr, WCN36XX_PKT_SIZE,
+					DMA_FROM_DEVICE);
+			wcn36xx_rx_skb(wcn, skb);
+		} /* else keep old skb not submitted and use it for rx DMA */
+
+		dxe->ctrl = value;
 		ctl = ctl->next;
 		dxe = ctl->desc;
 	}
+	wcn36xx_dxe_write_register(wcn, WCN36XX_DXE_ENCH_ADDR, int_mask);
 
 	ch->head_blk_ctl = ctl;
-
 	return 0;
 }
 
diff --git a/drivers/net/wireless/ath/wcn36xx/hal.h b/drivers/net/wireless/ath/wcn36xx/hal.h
index a1f1127d7808..b947de0fb2e5 100644
--- a/drivers/net/wireless/ath/wcn36xx/hal.h
+++ b/drivers/net/wireless/ath/wcn36xx/hal.h
@@ -345,6 +345,8 @@ enum wcn36xx_hal_host_msg_type {
 	WCN36XX_HAL_DHCP_START_IND = 189,
 	WCN36XX_HAL_DHCP_STOP_IND = 190,
 
+	WCN36XX_HAL_AVOID_FREQ_RANGE_IND = 233,
+
 	WCN36XX_HAL_MSG_MAX = WCN36XX_HAL_MSG_TYPE_MAX_ENUM_SIZE
 };
 
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.c b/drivers/net/wireless/ath/wcn36xx/smd.c
index c9263e1c75d4..74f56a81ad9a 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.c
+++ b/drivers/net/wireless/ath/wcn36xx/smd.c
@@ -302,6 +302,22 @@ static int wcn36xx_smd_rsp_status_check(void *buf, size_t len)
 	return 0;
 }
 
+static int wcn36xx_smd_rsp_status_check_v2(struct wcn36xx *wcn, void *buf,
+					     size_t len)
+{
+	struct wcn36xx_fw_msg_status_rsp_v2 *rsp;
+
+	if (len < sizeof(struct wcn36xx_hal_msg_header) + sizeof(*rsp))
+		return wcn36xx_smd_rsp_status_check(buf, len);
+
+	rsp = buf + sizeof(struct wcn36xx_hal_msg_header);
+
+	if (WCN36XX_FW_MSG_RESULT_SUCCESS != rsp->status)
+		return rsp->status;
+
+	return 0;
+}
+
 int wcn36xx_smd_load_nv(struct wcn36xx *wcn)
 {
 	struct nv_data *nv_d;
@@ -1582,7 +1598,8 @@ int wcn36xx_smd_remove_bsskey(struct wcn36xx *wcn,
 		wcn36xx_err("Sending hal_remove_bsskey failed\n");
 		goto out;
 	}
-	ret = wcn36xx_smd_rsp_status_check(wcn->hal_buf, wcn->hal_rsp_len);
+	ret = wcn36xx_smd_rsp_status_check_v2(wcn, wcn->hal_buf,
+					      wcn->hal_rsp_len);
 	if (ret) {
 		wcn36xx_err("hal_remove_bsskey response failed err=%d\n", ret);
 		goto out;
@@ -1951,7 +1968,8 @@ int wcn36xx_smd_trigger_ba(struct wcn36xx *wcn, u8 sta_index)
 		wcn36xx_err("Sending hal_trigger_ba failed\n");
 		goto out;
 	}
-	ret = wcn36xx_smd_rsp_status_check(wcn->hal_buf, wcn->hal_rsp_len);
+	ret = wcn36xx_smd_rsp_status_check_v2(wcn, wcn->hal_buf,
+						wcn->hal_rsp_len);
 	if (ret) {
 		wcn36xx_err("hal_trigger_ba response failed err=%d\n", ret);
 		goto out;
@@ -2128,6 +2146,8 @@ static void wcn36xx_smd_rsp_process(struct wcn36xx *wcn, void *buf, size_t len)
 		complete(&wcn->hal_rsp_compl);
 		break;
 
+	case WCN36XX_HAL_COEX_IND:
+	case WCN36XX_HAL_AVOID_FREQ_RANGE_IND:
 	case WCN36XX_HAL_OTA_TX_COMPL_IND:
 	case WCN36XX_HAL_MISSED_BEACON_IND:
 	case WCN36XX_HAL_DELETE_STA_CONTEXT_IND:
@@ -2174,6 +2194,9 @@ static void wcn36xx_ind_smd_work(struct work_struct *work)
 	msg_header = (struct wcn36xx_hal_msg_header *)hal_ind_msg->msg;
 
 	switch (msg_header->msg_type) {
+	case WCN36XX_HAL_COEX_IND:
+	case WCN36XX_HAL_AVOID_FREQ_RANGE_IND:
+		break;
 	case WCN36XX_HAL_OTA_TX_COMPL_IND:
 		wcn36xx_smd_tx_compl_ind(wcn,
 					 hal_ind_msg->msg,
diff --git a/drivers/net/wireless/ath/wcn36xx/smd.h b/drivers/net/wireless/ath/wcn36xx/smd.h
index 008d03423dbf..8361f9e3995b 100644
--- a/drivers/net/wireless/ath/wcn36xx/smd.h
+++ b/drivers/net/wireless/ath/wcn36xx/smd.h
@@ -44,6 +44,15 @@ struct wcn36xx_fw_msg_status_rsp {
 	u32	status;
 } __packed;
 
+/* wcn3620 returns this for tigger_ba */
+
+struct wcn36xx_fw_msg_status_rsp_v2 {
+	u8	bss_id[6];
+	u32	status __packed;
+	u16	count_following_candidates __packed;
+	/* candidate list follows */
+};
+
 struct wcn36xx_hal_ind_msg {
 	struct list_head list;
 	u8 *msg;
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 97bc186f9728..a1d10b85989f 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -580,16 +580,10 @@ static ssize_t wil_write_file_rxon(struct file *file, const char __user *buf,
 	long channel;
 	bool on;
 
-	char *kbuf = kmalloc(len + 1, GFP_KERNEL);
-
-	if (!kbuf)
-		return -ENOMEM;
-	if (copy_from_user(kbuf, buf, len)) {
-		kfree(kbuf);
-		return -EIO;
-	}
+	char *kbuf = memdup_user_nul(buf, len);
 
-	kbuf[len] = '\0';
+	if (IS_ERR(kbuf))
+		return PTR_ERR(kbuf);
 	rc = kstrtol(kbuf, 0, &channel);
 	kfree(kbuf);
 	if (rc)
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index 50c136e843c4..4f2ffa5c6e17 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -394,9 +394,13 @@ static irqreturn_t wil6210_irq_misc_thread(int irq, void *cookie)
 		wil_fw_core_dump(wil);
 		wil_notify_fw_error(wil);
 		isr &= ~ISR_MISC_FW_ERROR;
-		wil_fw_error_recovery(wil);
+		if (wil->platform_ops.notify_crash) {
+			wil_err(wil, "notify platform driver about FW crash");
+			wil->platform_ops.notify_crash(wil->platform_handle);
+		} else {
+			wil_fw_error_recovery(wil);
+		}
 	}
-
 	if (isr & ISR_MISC_MBOX_EVT) {
 		wil_dbg_irq(wil, "MBOX event\n");
 		wmi_recv_cmd(wil);
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index bb69a5949aea..b39f0bfc591e 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -401,20 +401,26 @@ void wil_bcast_fini(struct wil6210_priv *wil)
 
 static void wil_connect_worker(struct work_struct *work)
 {
-	int rc;
+	int rc, cid, ringid;
 	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
 						connect_worker);
 	struct net_device *ndev = wil_to_ndev(wil);
 
-	int cid = wil->pending_connect_cid;
-	int ringid = wil_find_free_vring(wil);
+	mutex_lock(&wil->mutex);
 
+	cid = wil->pending_connect_cid;
 	if (cid < 0) {
 		wil_err(wil, "No connection pending\n");
-		return;
+		goto out;
+	}
+	ringid = wil_find_free_vring(wil);
+	if (ringid < 0) {
+		wil_err(wil, "No free vring found\n");
+		goto out;
 	}
 
-	wil_dbg_wmi(wil, "Configure for connection CID %d\n", cid);
+	wil_dbg_wmi(wil, "Configure for connection CID %d vring %d\n",
+		    cid, ringid);
 
 	rc = wil_vring_init_tx(wil, ringid, 1 << tx_ring_order, cid, 0);
 	wil->pending_connect_cid = -1;
@@ -424,6 +430,8 @@ static void wil_connect_worker(struct work_struct *work)
 	} else {
 		wil_disconnect_cid(wil, cid, WLAN_REASON_UNSPECIFIED, true);
 	}
+out:
+	mutex_unlock(&wil->mutex);
 }
 
 int wil_priv_init(struct wil6210_priv *wil)
@@ -773,8 +781,10 @@ int wil_reset(struct wil6210_priv *wil, bool load_fw)
 	wil6210_disconnect(wil, NULL, WLAN_REASON_DEAUTH_LEAVING, false);
 	wil_bcast_fini(wil);
 
-	/* prevent NAPI from being scheduled */
+	/* prevent NAPI from being scheduled and prevent wmi commands */
+	mutex_lock(&wil->wmi_mutex);
 	bitmap_zero(wil->status, wil_status_last);
+	mutex_unlock(&wil->wmi_mutex);
 
 	if (wil->scan_request) {
 		wil_dbg_misc(wil, "Abort scan_request 0x%p\n",
@@ -977,7 +987,7 @@ int __wil_down(struct wil6210_priv *wil)
 	}
 	mutex_lock(&wil->mutex);
 
-	if (!iter)
+	if (iter < 0)
 		wil_err(wil, "timeout waiting for idle FW/HW\n");
 
 	wil_reset(wil, false);
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index e3b3c8fb4605..56aaa2d4fb0e 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -183,7 +183,7 @@ void *wil_if_alloc(struct device *dev)
 
 	netif_napi_add(ndev, &wil->napi_rx, wil6210_netdev_poll_rx,
 		       WIL6210_NAPI_BUDGET);
-	netif_napi_add(ndev, &wil->napi_tx, wil6210_netdev_poll_tx,
+	netif_tx_napi_add(ndev, &wil->napi_tx, wil6210_netdev_poll_tx,
 		       WIL6210_NAPI_BUDGET);
 
 	netif_tx_stop_all_queues(ndev);
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index 1a3142c332e1..e36f2a0c8cb6 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2015 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -125,11 +125,37 @@ static int wil_if_pcie_disable(struct wil6210_priv *wil)
 	return 0;
 }
 
+static int wil_platform_rop_ramdump(void *wil_handle, void *buf, uint32_t size)
+{
+	struct wil6210_priv *wil = wil_handle;
+
+	if (!wil)
+		return -EINVAL;
+
+	return wil_fw_copy_crash_dump(wil, buf, size);
+}
+
+static int wil_platform_rop_fw_recovery(void *wil_handle)
+{
+	struct wil6210_priv *wil = wil_handle;
+
+	if (!wil)
+		return -EINVAL;
+
+	wil_fw_error_recovery(wil);
+
+	return 0;
+}
+
 static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct wil6210_priv *wil;
 	struct device *dev = &pdev->dev;
 	int rc;
+	const struct wil_platform_rops rops = {
+		.ramdump = wil_platform_rop_ramdump,
+		.fw_recovery = wil_platform_rop_fw_recovery,
+	};
 
 	/* check HW */
 	dev_info(&pdev->dev, WIL_NAME
@@ -154,7 +180,7 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	/* rollback to if_free */
 
 	wil->platform_handle =
-			wil_platform_init(&pdev->dev, &wil->platform_ops);
+		wil_platform_init(&pdev->dev, &wil->platform_ops, &rops, wil);
 	if (!wil->platform_handle) {
 		rc = -ENODEV;
 		wil_err(wil, "wil_platform_init failed\n");
diff --git a/drivers/net/wireless/ath/wil6210/rx_reorder.c b/drivers/net/wireless/ath/wil6210/rx_reorder.c
index e3d1be82f314..32031e7a11d5 100644
--- a/drivers/net/wireless/ath/wil6210/rx_reorder.c
+++ b/drivers/net/wireless/ath/wil6210/rx_reorder.c
@@ -261,9 +261,19 @@ struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
 void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
 			   struct wil_tid_ampdu_rx *r)
 {
+	int i;
+
 	if (!r)
 		return;
-	wil_release_reorder_frames(wil, r, r->head_seq_num + r->buf_size);
+
+	/* Do not pass remaining frames to the network stack - it may be
+	 * not expecting to get any more Rx. Rx from here may lead to
+	 * kernel OOPS since some per-socket accounting info was already
+	 * released.
+	 */
+	for (i = 0; i < r->buf_size; i++)
+		kfree_skb(r->reorder_buf[i]);
+
 	kfree(r->reorder_buf);
 	kfree(r->reorder_time);
 	kfree(r);
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index 3bc9bc0efbac..7887e6cfd817 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -160,6 +160,7 @@ static void wil_vring_free(struct wil6210_priv *wil, struct vring *vring,
 	struct device *dev = wil_to_dev(wil);
 	size_t sz = vring->size * sizeof(vring->va[0]);
 
+	lockdep_assert_held(&wil->mutex);
 	if (tx) {
 		int vring_index = vring - wil->vring_tx;
 
@@ -749,6 +750,7 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 
 	wil_dbg_misc(wil, "%s() max_mpdu_size %d\n", __func__,
 		     cmd.vring_cfg.tx_sw_ring.max_mpdu_size);
+	lockdep_assert_held(&wil->mutex);
 
 	if (vring->va) {
 		wil_err(wil, "Tx ring [%d] already allocated\n", id);
@@ -821,6 +823,7 @@ int wil_vring_init_bcast(struct wil6210_priv *wil, int id, int size)
 
 	wil_dbg_misc(wil, "%s() max_mpdu_size %d\n", __func__,
 		     cmd.vring_cfg.tx_sw_ring.max_mpdu_size);
+	lockdep_assert_held(&wil->mutex);
 
 	if (vring->va) {
 		wil_err(wil, "Tx ring [%d] already allocated\n", id);
@@ -872,7 +875,7 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 	struct vring *vring = &wil->vring_tx[id];
 	struct vring_tx_data *txdata = &wil->vring_tx_data[id];
 
-	WARN_ON(!mutex_is_locked(&wil->mutex));
+	lockdep_assert_held(&wil->mutex);
 
 	if (!vring->va)
 		return;
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index ade5f3b8274b..235e205ce2bc 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -828,6 +828,7 @@ int wil_can_suspend(struct wil6210_priv *wil, bool is_runtime);
 int wil_suspend(struct wil6210_priv *wil, bool is_runtime);
 int wil_resume(struct wil6210_priv *wil, bool is_runtime);
 
+int wil_fw_copy_crash_dump(struct wil6210_priv *wil, void *dest, u32 size);
 void wil_fw_core_dump(struct wil6210_priv *wil);
 
 #endif /* __WIL6210_H__ */
diff --git a/drivers/net/wireless/ath/wil6210/wil_crash_dump.c b/drivers/net/wireless/ath/wil6210/wil_crash_dump.c
index 7e70934990ae..b57d280946e0 100644
--- a/drivers/net/wireless/ath/wil6210/wil_crash_dump.c
+++ b/drivers/net/wireless/ath/wil6210/wil_crash_dump.c
@@ -51,8 +51,7 @@ static int wil_fw_get_crash_dump_bounds(struct wil6210_priv *wil,
 	return 0;
 }
 
-static int wil_fw_copy_crash_dump(struct wil6210_priv *wil, void *dest,
-				  u32 size)
+int wil_fw_copy_crash_dump(struct wil6210_priv *wil, void *dest, u32 size)
 {
 	int i;
 	const struct fw_map *map;
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.c b/drivers/net/wireless/ath/wil6210/wil_platform.c
index 2e831bf20117..4eed05bddb60 100644
--- a/drivers/net/wireless/ath/wil6210/wil_platform.c
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.c
@@ -33,7 +33,8 @@ void wil_platform_modexit(void)
  * It returns a handle which is used with the rest of the API
  *
  */
-void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops)
+void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops,
+			const struct wil_platform_rops *rops, void *wil_handle)
 {
 	void *handle = ops; /* to return some non-NULL for 'void' impl. */
 
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.h b/drivers/net/wireless/ath/wil6210/wil_platform.h
index d7fa19b7886d..9a949d910343 100644
--- a/drivers/net/wireless/ath/wil6210/wil_platform.h
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ * Copyright (c) 2014-2015 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -20,16 +20,48 @@
 struct device;
 
 /**
- * struct wil_platform_ops - wil platform module callbacks
+ * struct wil_platform_ops - wil platform module calls from this
+ * driver to platform driver
  */
 struct wil_platform_ops {
 	int (*bus_request)(void *handle, uint32_t kbps /* KBytes/Sec */);
 	int (*suspend)(void *handle);
 	int (*resume)(void *handle);
 	void (*uninit)(void *handle);
+	int (*notify_crash)(void *handle);
 };
 
-void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops);
+/**
+ * struct wil_platform_rops - wil platform module callbacks from
+ * platform driver to this driver
+ * @ramdump: store a ramdump from the wil firmware. The platform
+ *	driver may add additional data to the ramdump to
+ *	generate the final crash dump.
+ * @fw_recovery: start a firmware recovery process. Called as
+ *      part of a crash recovery process which may include other
+ *      related platform subsystems.
+ */
+struct wil_platform_rops {
+	int (*ramdump)(void *wil_handle, void *buf, uint32_t size);
+	int (*fw_recovery)(void *wil_handle);
+};
+
+/**
+ * wil_platform_init - initialize the platform driver
+ *
+ * @dev - pointer to the wil6210 device
+ * @ops - structure with platform driver operations. Platform
+ *	driver will fill this structure with function pointers.
+ * @rops - structure with callbacks from platform driver to
+ *	this driver. The platform driver copies the structure to
+ *	its own storage. Can be NULL if this driver does not
+ *	support crash recovery.
+ * @wil_handle - context for this driver that will be passed
+ *      when platform driver invokes one of the callbacks in
+ *      rops. May be NULL if rops is NULL.
+ */
+void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops,
+			const struct wil_platform_rops *rops, void *wil_handle);
 
 int __init wil_platform_modinit(void);
 void wil_platform_modexit(void);
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 6ed26baca0e5..e3ea74cdd4aa 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -228,6 +228,10 @@ static int __wmi_send(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len)
 	wil_dbg_wmi(wil, "Head 0x%08x -> 0x%08x\n", r->head, next_head);
 	/* wait till FW finish with previous command */
 	for (retry = 5; retry > 0; retry--) {
+		if (!test_bit(wil_status_fwready, wil->status)) {
+			wil_err(wil, "WMI: cannot send command while FW not ready\n");
+			return -EAGAIN;
+		}
 		r->tail = wil_r(wil, RGF_MBOX +
 				offsetof(struct wil6210_mbox_ctl, tx.tail));
 		if (next_head != r->tail)
diff --git a/drivers/net/wireless/atmel/Kconfig b/drivers/net/wireless/atmel/Kconfig
new file mode 100644
index 000000000000..a43cfd163254
--- /dev/null
+++ b/drivers/net/wireless/atmel/Kconfig
@@ -0,0 +1,57 @@
+config WLAN_VENDOR_ATMEL
+	bool "Atmel devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_ATMEL
+
+config ATMEL
+      tristate "Atmel at76c50x chipset  802.11b support"
+      depends on CFG80211 && (PCI || PCMCIA)
+      select WIRELESS_EXT
+      select WEXT_PRIV
+      select FW_LOADER
+      select CRC32
+       ---help---
+        A driver 802.11b wireless cards based on the Atmel fast-vnet
+        chips. This driver supports standard Linux wireless extensions.
+
+        Many  cards based on this chipset do not have flash memory
+        and need their firmware loaded at start-up. If yours is
+        one of these, you will need to provide a firmware image
+        to be loaded into the card by the driver. The Atmel
+        firmware package can be downloaded from
+        <http://www.thekelleys.org.uk/atmel>
+
+config PCI_ATMEL
+      tristate "Atmel at76c506 PCI cards"
+      depends on ATMEL && PCI
+       ---help---
+        Enable support for PCI and mini-PCI cards containing the
+        Atmel at76c506 chip.
+
+config PCMCIA_ATMEL
+	tristate "Atmel at76c502/at76c504 PCMCIA cards"
+	depends on ATMEL && PCMCIA
+	select WIRELESS_EXT
+	select FW_LOADER
+	select CRC32
+	---help---
+	  Enable support for PCMCIA cards containing the
+	  Atmel at76c502 and at76c504 chips.
+
+config AT76C50X_USB
+        tristate "Atmel at76c503/at76c505/at76c505a USB cards"
+        depends on MAC80211 && USB
+        select FW_LOADER
+        ---help---
+          Enable support for USB Wireless devices using Atmel at76c503,
+          at76c505 or at76c505a chips.
+
+endif # WLAN_VENDOR_ATMEL
diff --git a/drivers/net/wireless/atmel/Makefile b/drivers/net/wireless/atmel/Makefile
new file mode 100644
index 000000000000..e62e345f7af6
--- /dev/null
+++ b/drivers/net/wireless/atmel/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_ATMEL)             += atmel.o
+obj-$(CONFIG_PCI_ATMEL)         += atmel_pci.o 
+obj-$(CONFIG_PCMCIA_ATMEL)      += atmel_cs.o
+
+obj-$(CONFIG_AT76C50X_USB)      += at76c50x-usb.o
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/atmel/at76c50x-usb.c
index dab25136214a..dab25136214a 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/atmel/at76c50x-usb.c
diff --git a/drivers/net/wireless/at76c50x-usb.h b/drivers/net/wireless/atmel/at76c50x-usb.h
index ae03271f878e..ae03271f878e 100644
--- a/drivers/net/wireless/at76c50x-usb.h
+++ b/drivers/net/wireless/atmel/at76c50x-usb.h
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel/atmel.c
index 6a1f03c271c1..6a1f03c271c1 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel/atmel.c
diff --git a/drivers/net/wireless/atmel.h b/drivers/net/wireless/atmel/atmel.h
index 96f7318cbb04..96f7318cbb04 100644
--- a/drivers/net/wireless/atmel.h
+++ b/drivers/net/wireless/atmel/atmel.h
diff --git a/drivers/net/wireless/atmel_cs.c b/drivers/net/wireless/atmel/atmel_cs.c
index 7afc9c5329fb..7afc9c5329fb 100644
--- a/drivers/net/wireless/atmel_cs.c
+++ b/drivers/net/wireless/atmel/atmel_cs.c
diff --git a/drivers/net/wireless/atmel_pci.c b/drivers/net/wireless/atmel/atmel_pci.c
index bcf1f274a251..bcf1f274a251 100644
--- a/drivers/net/wireless/atmel_pci.c
+++ b/drivers/net/wireless/atmel/atmel_pci.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/common.h b/drivers/net/wireless/brcm80211/brcmfmac/common.h
deleted file mode 100644
index 21c7488b4732..000000000000
--- a/drivers/net/wireless/brcm80211/brcmfmac/common.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/* Copyright (c) 2014 Broadcom Corporation
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
- * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
- * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-#ifndef BRCMFMAC_COMMON_H
-#define BRCMFMAC_COMMON_H
-
-extern const u8 ALLFFMAC[ETH_ALEN];
-
-/* Sets dongle media info (drv_version, mac address). */
-int brcmf_c_preinit_dcmds(struct brcmf_if *ifp);
-
-#endif /* BRCMFMAC_COMMON_H */
diff --git a/drivers/net/wireless/broadcom/Kconfig b/drivers/net/wireless/broadcom/Kconfig
new file mode 100644
index 000000000000..d3651ceb5046
--- /dev/null
+++ b/drivers/net/wireless/broadcom/Kconfig
@@ -0,0 +1,18 @@
+config WLAN_VENDOR_BROADCOM
+	bool "Broadcom devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_BROADCOM
+
+source "drivers/net/wireless/broadcom/b43/Kconfig"
+source "drivers/net/wireless/broadcom/b43legacy/Kconfig"
+source "drivers/net/wireless/broadcom/brcm80211/Kconfig"
+
+endif # WLAN_VENDOR_BROADCOM
diff --git a/drivers/net/wireless/broadcom/Makefile b/drivers/net/wireless/broadcom/Makefile
new file mode 100644
index 000000000000..9d5ac95710c3
--- /dev/null
+++ b/drivers/net/wireless/broadcom/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_B43)		+= b43/
+obj-$(CONFIG_B43LEGACY)		+= b43legacy/
+
+obj-$(CONFIG_BRCMFMAC)	+= brcm80211/
+obj-$(CONFIG_BRCMSMAC)	+= brcm80211/
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/broadcom/b43/Kconfig
index fba856032ca5..fba856032ca5 100644
--- a/drivers/net/wireless/b43/Kconfig
+++ b/drivers/net/wireless/broadcom/b43/Kconfig
diff --git a/drivers/net/wireless/b43/Makefile b/drivers/net/wireless/broadcom/b43/Makefile
index ddc4df46656f..ddc4df46656f 100644
--- a/drivers/net/wireless/b43/Makefile
+++ b/drivers/net/wireless/broadcom/b43/Makefile
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/broadcom/b43/b43.h
index 036552439816..036552439816 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/broadcom/b43/b43.h
diff --git a/drivers/net/wireless/b43/bus.c b/drivers/net/wireless/broadcom/b43/bus.c
index 17d16a391fe6..17d16a391fe6 100644
--- a/drivers/net/wireless/b43/bus.c
+++ b/drivers/net/wireless/broadcom/b43/bus.c
diff --git a/drivers/net/wireless/b43/bus.h b/drivers/net/wireless/broadcom/b43/bus.h
index 256c2c17939a..256c2c17939a 100644
--- a/drivers/net/wireless/b43/bus.h
+++ b/drivers/net/wireless/broadcom/b43/bus.h
diff --git a/drivers/net/wireless/b43/debugfs.c b/drivers/net/wireless/broadcom/b43/debugfs.c
index b4bcd94aff6c..b4bcd94aff6c 100644
--- a/drivers/net/wireless/b43/debugfs.c
+++ b/drivers/net/wireless/broadcom/b43/debugfs.c
diff --git a/drivers/net/wireless/b43/debugfs.h b/drivers/net/wireless/broadcom/b43/debugfs.h
index d05377745011..d05377745011 100644
--- a/drivers/net/wireless/b43/debugfs.h
+++ b/drivers/net/wireless/broadcom/b43/debugfs.h
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/broadcom/b43/dma.c
index 6837064908be..6837064908be 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/broadcom/b43/dma.c
diff --git a/drivers/net/wireless/b43/dma.h b/drivers/net/wireless/broadcom/b43/dma.h
index df8c8cdcbdb5..df8c8cdcbdb5 100644
--- a/drivers/net/wireless/b43/dma.h
+++ b/drivers/net/wireless/broadcom/b43/dma.h
diff --git a/drivers/net/wireless/b43/leds.c b/drivers/net/wireless/broadcom/b43/leds.c
index d79ab2a227e1..d79ab2a227e1 100644
--- a/drivers/net/wireless/b43/leds.c
+++ b/drivers/net/wireless/broadcom/b43/leds.c
diff --git a/drivers/net/wireless/b43/leds.h b/drivers/net/wireless/broadcom/b43/leds.h
index 32b66d53cdac..32b66d53cdac 100644
--- a/drivers/net/wireless/b43/leds.h
+++ b/drivers/net/wireless/broadcom/b43/leds.h
diff --git a/drivers/net/wireless/b43/lo.c b/drivers/net/wireless/broadcom/b43/lo.c
index a335f94c72ff..a335f94c72ff 100644
--- a/drivers/net/wireless/b43/lo.c
+++ b/drivers/net/wireless/broadcom/b43/lo.c
diff --git a/drivers/net/wireless/b43/lo.h b/drivers/net/wireless/broadcom/b43/lo.h
index 7b4df3883bc2..7b4df3883bc2 100644
--- a/drivers/net/wireless/b43/lo.h
+++ b/drivers/net/wireless/broadcom/b43/lo.h
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/broadcom/b43/main.c
index ec013fbd6a81..ec013fbd6a81 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/broadcom/b43/main.c
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/broadcom/b43/main.h
index c46430cc725c..c46430cc725c 100644
--- a/drivers/net/wireless/b43/main.h
+++ b/drivers/net/wireless/broadcom/b43/main.h
diff --git a/drivers/net/wireless/b43/phy_a.c b/drivers/net/wireless/broadcom/b43/phy_a.c
index 99c036f5ecb7..99c036f5ecb7 100644
--- a/drivers/net/wireless/b43/phy_a.c
+++ b/drivers/net/wireless/broadcom/b43/phy_a.c
diff --git a/drivers/net/wireless/b43/phy_a.h b/drivers/net/wireless/broadcom/b43/phy_a.h
index f7d0d929a374..f7d0d929a374 100644
--- a/drivers/net/wireless/b43/phy_a.h
+++ b/drivers/net/wireless/broadcom/b43/phy_a.h
diff --git a/drivers/net/wireless/b43/phy_ac.c b/drivers/net/wireless/broadcom/b43/phy_ac.c
index e75633d67938..e75633d67938 100644
--- a/drivers/net/wireless/b43/phy_ac.c
+++ b/drivers/net/wireless/broadcom/b43/phy_ac.c
diff --git a/drivers/net/wireless/b43/phy_ac.h b/drivers/net/wireless/broadcom/b43/phy_ac.h
index d1ca79e0eb24..d1ca79e0eb24 100644
--- a/drivers/net/wireless/b43/phy_ac.h
+++ b/drivers/net/wireless/broadcom/b43/phy_ac.h
diff --git a/drivers/net/wireless/b43/phy_common.c b/drivers/net/wireless/broadcom/b43/phy_common.c
index ec2b9c577b90..ec2b9c577b90 100644
--- a/drivers/net/wireless/b43/phy_common.c
+++ b/drivers/net/wireless/broadcom/b43/phy_common.c
diff --git a/drivers/net/wireless/b43/phy_common.h b/drivers/net/wireless/broadcom/b43/phy_common.h
index 78d86526799e..78d86526799e 100644
--- a/drivers/net/wireless/b43/phy_common.h
+++ b/drivers/net/wireless/broadcom/b43/phy_common.h
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/broadcom/b43/phy_g.c
index 462310e6e88f..462310e6e88f 100644
--- a/drivers/net/wireless/b43/phy_g.c
+++ b/drivers/net/wireless/broadcom/b43/phy_g.c
diff --git a/drivers/net/wireless/b43/phy_g.h b/drivers/net/wireless/broadcom/b43/phy_g.h
index 5413c906a3e7..5413c906a3e7 100644
--- a/drivers/net/wireless/b43/phy_g.h
+++ b/drivers/net/wireless/broadcom/b43/phy_g.h
diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/broadcom/b43/phy_ht.c
index bd68945965d6..bd68945965d6 100644
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/broadcom/b43/phy_ht.c
diff --git a/drivers/net/wireless/b43/phy_ht.h b/drivers/net/wireless/broadcom/b43/phy_ht.h
index c086f56ce478..c086f56ce478 100644
--- a/drivers/net/wireless/b43/phy_ht.h
+++ b/drivers/net/wireless/broadcom/b43/phy_ht.h
diff --git a/drivers/net/wireless/b43/phy_lcn.c b/drivers/net/wireless/broadcom/b43/phy_lcn.c
index 97461ccf3e1e..97461ccf3e1e 100644
--- a/drivers/net/wireless/b43/phy_lcn.c
+++ b/drivers/net/wireless/broadcom/b43/phy_lcn.c
diff --git a/drivers/net/wireless/b43/phy_lcn.h b/drivers/net/wireless/broadcom/b43/phy_lcn.h
index 6a7092e13fff..6a7092e13fff 100644
--- a/drivers/net/wireless/b43/phy_lcn.h
+++ b/drivers/net/wireless/broadcom/b43/phy_lcn.h
diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/broadcom/b43/phy_lp.c
index 058a9f232050..058a9f232050 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/broadcom/b43/phy_lp.c
diff --git a/drivers/net/wireless/b43/phy_lp.h b/drivers/net/wireless/broadcom/b43/phy_lp.h
index 62737f700cbc..62737f700cbc 100644
--- a/drivers/net/wireless/b43/phy_lp.h
+++ b/drivers/net/wireless/broadcom/b43/phy_lp.h
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/broadcom/b43/phy_n.c
index 9f0bcf3b8414..9f0bcf3b8414 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/broadcom/b43/phy_n.c
diff --git a/drivers/net/wireless/b43/phy_n.h b/drivers/net/wireless/broadcom/b43/phy_n.h
index a6da2c31a99c..a6da2c31a99c 100644
--- a/drivers/net/wireless/b43/phy_n.h
+++ b/drivers/net/wireless/broadcom/b43/phy_n.h
diff --git a/drivers/net/wireless/b43/pio.c b/drivers/net/wireless/broadcom/b43/pio.c
index a4ff5e2a42b9..a4ff5e2a42b9 100644
--- a/drivers/net/wireless/b43/pio.c
+++ b/drivers/net/wireless/broadcom/b43/pio.c
diff --git a/drivers/net/wireless/b43/pio.h b/drivers/net/wireless/broadcom/b43/pio.h
index 1e516147424f..1e516147424f 100644
--- a/drivers/net/wireless/b43/pio.h
+++ b/drivers/net/wireless/broadcom/b43/pio.h
diff --git a/drivers/net/wireless/b43/ppr.c b/drivers/net/wireless/broadcom/b43/ppr.c
index 9a770279c415..9a770279c415 100644
--- a/drivers/net/wireless/b43/ppr.c
+++ b/drivers/net/wireless/broadcom/b43/ppr.c
diff --git a/drivers/net/wireless/b43/ppr.h b/drivers/net/wireless/broadcom/b43/ppr.h
index 24d7447e9f01..24d7447e9f01 100644
--- a/drivers/net/wireless/b43/ppr.h
+++ b/drivers/net/wireless/broadcom/b43/ppr.h
diff --git a/drivers/net/wireless/b43/radio_2055.c b/drivers/net/wireless/broadcom/b43/radio_2055.c
index 5289a18ddd8c..5289a18ddd8c 100644
--- a/drivers/net/wireless/b43/radio_2055.c
+++ b/drivers/net/wireless/broadcom/b43/radio_2055.c
diff --git a/drivers/net/wireless/b43/radio_2055.h b/drivers/net/wireless/broadcom/b43/radio_2055.h
index 67f96122f8d8..67f96122f8d8 100644
--- a/drivers/net/wireless/b43/radio_2055.h
+++ b/drivers/net/wireless/broadcom/b43/radio_2055.h
diff --git a/drivers/net/wireless/b43/radio_2056.c b/drivers/net/wireless/broadcom/b43/radio_2056.c
index 2ce25607c60d..2ce25607c60d 100644
--- a/drivers/net/wireless/b43/radio_2056.c
+++ b/drivers/net/wireless/broadcom/b43/radio_2056.c
diff --git a/drivers/net/wireless/b43/radio_2056.h b/drivers/net/wireless/broadcom/b43/radio_2056.h
index 5b86673459fa..5b86673459fa 100644
--- a/drivers/net/wireless/b43/radio_2056.h
+++ b/drivers/net/wireless/broadcom/b43/radio_2056.h
diff --git a/drivers/net/wireless/b43/radio_2057.c b/drivers/net/wireless/broadcom/b43/radio_2057.c
index ff1e026a61a1..ff1e026a61a1 100644
--- a/drivers/net/wireless/b43/radio_2057.c
+++ b/drivers/net/wireless/broadcom/b43/radio_2057.c
diff --git a/drivers/net/wireless/b43/radio_2057.h b/drivers/net/wireless/broadcom/b43/radio_2057.h
index 220d080238ff..220d080238ff 100644
--- a/drivers/net/wireless/b43/radio_2057.h
+++ b/drivers/net/wireless/broadcom/b43/radio_2057.h
diff --git a/drivers/net/wireless/b43/radio_2059.c b/drivers/net/wireless/broadcom/b43/radio_2059.c
index a3cf9efd7e21..a3cf9efd7e21 100644
--- a/drivers/net/wireless/b43/radio_2059.c
+++ b/drivers/net/wireless/broadcom/b43/radio_2059.c
diff --git a/drivers/net/wireless/b43/radio_2059.h b/drivers/net/wireless/broadcom/b43/radio_2059.h
index 9e22fb60588b..9e22fb60588b 100644
--- a/drivers/net/wireless/b43/radio_2059.h
+++ b/drivers/net/wireless/broadcom/b43/radio_2059.h
diff --git a/drivers/net/wireless/b43/rfkill.c b/drivers/net/wireless/broadcom/b43/rfkill.c
index 70c2fcedd1bb..70c2fcedd1bb 100644
--- a/drivers/net/wireless/b43/rfkill.c
+++ b/drivers/net/wireless/broadcom/b43/rfkill.c
diff --git a/drivers/net/wireless/b43/rfkill.h b/drivers/net/wireless/broadcom/b43/rfkill.h
index f046c3ca0519..f046c3ca0519 100644
--- a/drivers/net/wireless/b43/rfkill.h
+++ b/drivers/net/wireless/broadcom/b43/rfkill.h
diff --git a/drivers/net/wireless/b43/sdio.c b/drivers/net/wireless/broadcom/b43/sdio.c
index 59a521800694..59a521800694 100644
--- a/drivers/net/wireless/b43/sdio.c
+++ b/drivers/net/wireless/broadcom/b43/sdio.c
diff --git a/drivers/net/wireless/b43/sdio.h b/drivers/net/wireless/broadcom/b43/sdio.h
index 1e93926f388f..1e93926f388f 100644
--- a/drivers/net/wireless/b43/sdio.h
+++ b/drivers/net/wireless/broadcom/b43/sdio.h
diff --git a/drivers/net/wireless/b43/sysfs.c b/drivers/net/wireless/broadcom/b43/sysfs.c
index 3190493bd07f..3190493bd07f 100644
--- a/drivers/net/wireless/b43/sysfs.c
+++ b/drivers/net/wireless/broadcom/b43/sysfs.c
diff --git a/drivers/net/wireless/b43/sysfs.h b/drivers/net/wireless/broadcom/b43/sysfs.h
index 12bda9ef1a85..12bda9ef1a85 100644
--- a/drivers/net/wireless/b43/sysfs.h
+++ b/drivers/net/wireless/broadcom/b43/sysfs.h
diff --git a/drivers/net/wireless/b43/tables.c b/drivers/net/wireless/broadcom/b43/tables.c
index ea288df8aee9..ea288df8aee9 100644
--- a/drivers/net/wireless/b43/tables.c
+++ b/drivers/net/wireless/broadcom/b43/tables.c
diff --git a/drivers/net/wireless/b43/tables.h b/drivers/net/wireless/broadcom/b43/tables.h
index 80e73c7cbac5..80e73c7cbac5 100644
--- a/drivers/net/wireless/b43/tables.h
+++ b/drivers/net/wireless/broadcom/b43/tables.h
diff --git a/drivers/net/wireless/b43/tables_lpphy.c b/drivers/net/wireless/broadcom/b43/tables_lpphy.c
index cff187c5616d..cff187c5616d 100644
--- a/drivers/net/wireless/b43/tables_lpphy.c
+++ b/drivers/net/wireless/broadcom/b43/tables_lpphy.c
diff --git a/drivers/net/wireless/b43/tables_lpphy.h b/drivers/net/wireless/broadcom/b43/tables_lpphy.h
index 84f1d265f657..84f1d265f657 100644
--- a/drivers/net/wireless/b43/tables_lpphy.h
+++ b/drivers/net/wireless/broadcom/b43/tables_lpphy.h
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/broadcom/b43/tables_nphy.c
index b2f0d245bcf3..b2f0d245bcf3 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/broadcom/b43/tables_nphy.c
diff --git a/drivers/net/wireless/b43/tables_nphy.h b/drivers/net/wireless/broadcom/b43/tables_nphy.h
index b51f386db02f..b51f386db02f 100644
--- a/drivers/net/wireless/b43/tables_nphy.h
+++ b/drivers/net/wireless/broadcom/b43/tables_nphy.h
diff --git a/drivers/net/wireless/b43/tables_phy_ht.c b/drivers/net/wireless/broadcom/b43/tables_phy_ht.c
index 176c49d74ef4..176c49d74ef4 100644
--- a/drivers/net/wireless/b43/tables_phy_ht.c
+++ b/drivers/net/wireless/broadcom/b43/tables_phy_ht.c
diff --git a/drivers/net/wireless/b43/tables_phy_ht.h b/drivers/net/wireless/broadcom/b43/tables_phy_ht.h
index 1b5ef2bc770c..1b5ef2bc770c 100644
--- a/drivers/net/wireless/b43/tables_phy_ht.h
+++ b/drivers/net/wireless/broadcom/b43/tables_phy_ht.h
diff --git a/drivers/net/wireless/b43/tables_phy_lcn.c b/drivers/net/wireless/broadcom/b43/tables_phy_lcn.c
index e347b8d80ea4..e347b8d80ea4 100644
--- a/drivers/net/wireless/b43/tables_phy_lcn.c
+++ b/drivers/net/wireless/broadcom/b43/tables_phy_lcn.c
diff --git a/drivers/net/wireless/b43/tables_phy_lcn.h b/drivers/net/wireless/broadcom/b43/tables_phy_lcn.h
index caff9db6831f..caff9db6831f 100644
--- a/drivers/net/wireless/b43/tables_phy_lcn.h
+++ b/drivers/net/wireless/broadcom/b43/tables_phy_lcn.h
diff --git a/drivers/net/wireless/b43/wa.c b/drivers/net/wireless/broadcom/b43/wa.c
index c218c08fb2f5..c218c08fb2f5 100644
--- a/drivers/net/wireless/b43/wa.c
+++ b/drivers/net/wireless/broadcom/b43/wa.c
diff --git a/drivers/net/wireless/b43/wa.h b/drivers/net/wireless/broadcom/b43/wa.h
index e163c5e56e81..e163c5e56e81 100644
--- a/drivers/net/wireless/b43/wa.h
+++ b/drivers/net/wireless/broadcom/b43/wa.h
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/broadcom/b43/xmit.c
index 426dc13c44cd..426dc13c44cd 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/broadcom/b43/xmit.c
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/broadcom/b43/xmit.h
index ba6115308068..ba6115308068 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/broadcom/b43/xmit.h
diff --git a/drivers/net/wireless/b43legacy/Kconfig b/drivers/net/wireless/broadcom/b43legacy/Kconfig
index 1ffa28835c58..1ffa28835c58 100644
--- a/drivers/net/wireless/b43legacy/Kconfig
+++ b/drivers/net/wireless/broadcom/b43legacy/Kconfig
diff --git a/drivers/net/wireless/b43legacy/Makefile b/drivers/net/wireless/broadcom/b43legacy/Makefile
index 227a77e84362..227a77e84362 100644
--- a/drivers/net/wireless/b43legacy/Makefile
+++ b/drivers/net/wireless/broadcom/b43legacy/Makefile
diff --git a/drivers/net/wireless/b43legacy/b43legacy.h b/drivers/net/wireless/broadcom/b43legacy/b43legacy.h
index 482476fdb1f3..482476fdb1f3 100644
--- a/drivers/net/wireless/b43legacy/b43legacy.h
+++ b/drivers/net/wireless/broadcom/b43legacy/b43legacy.h
diff --git a/drivers/net/wireless/b43legacy/debugfs.c b/drivers/net/wireless/broadcom/b43legacy/debugfs.c
index 090910ea259e..090910ea259e 100644
--- a/drivers/net/wireless/b43legacy/debugfs.c
+++ b/drivers/net/wireless/broadcom/b43legacy/debugfs.c
diff --git a/drivers/net/wireless/b43legacy/debugfs.h b/drivers/net/wireless/broadcom/b43legacy/debugfs.h
index 9ee32158b947..9ee32158b947 100644
--- a/drivers/net/wireless/b43legacy/debugfs.h
+++ b/drivers/net/wireless/broadcom/b43legacy/debugfs.h
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/broadcom/b43legacy/dma.c
index f9dd892b9f27..f9dd892b9f27 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/broadcom/b43legacy/dma.c
diff --git a/drivers/net/wireless/b43legacy/dma.h b/drivers/net/wireless/broadcom/b43legacy/dma.h
index c3282f906bc7..c3282f906bc7 100644
--- a/drivers/net/wireless/b43legacy/dma.h
+++ b/drivers/net/wireless/broadcom/b43legacy/dma.h
diff --git a/drivers/net/wireless/b43legacy/ilt.c b/drivers/net/wireless/broadcom/b43legacy/ilt.c
index ee5682e54204..ee5682e54204 100644
--- a/drivers/net/wireless/b43legacy/ilt.c
+++ b/drivers/net/wireless/broadcom/b43legacy/ilt.c
diff --git a/drivers/net/wireless/b43legacy/ilt.h b/drivers/net/wireless/broadcom/b43legacy/ilt.h
index 48bcf37eccb8..48bcf37eccb8 100644
--- a/drivers/net/wireless/b43legacy/ilt.h
+++ b/drivers/net/wireless/broadcom/b43legacy/ilt.h
diff --git a/drivers/net/wireless/b43legacy/leds.c b/drivers/net/wireless/broadcom/b43legacy/leds.c
index fd4565389c77..fd4565389c77 100644
--- a/drivers/net/wireless/b43legacy/leds.c
+++ b/drivers/net/wireless/broadcom/b43legacy/leds.c
diff --git a/drivers/net/wireless/b43legacy/leds.h b/drivers/net/wireless/broadcom/b43legacy/leds.h
index 9ff6750dc57f..9ff6750dc57f 100644
--- a/drivers/net/wireless/b43legacy/leds.h
+++ b/drivers/net/wireless/broadcom/b43legacy/leds.h
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/broadcom/b43legacy/main.c
index afc1fb3e38df..afc1fb3e38df 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/broadcom/b43legacy/main.c
diff --git a/drivers/net/wireless/b43legacy/main.h b/drivers/net/wireless/broadcom/b43legacy/main.h
index b74a058d7bac..b74a058d7bac 100644
--- a/drivers/net/wireless/b43legacy/main.h
+++ b/drivers/net/wireless/broadcom/b43legacy/main.h
diff --git a/drivers/net/wireless/b43legacy/phy.c b/drivers/net/wireless/broadcom/b43legacy/phy.c
index 995c7d0c212a..995c7d0c212a 100644
--- a/drivers/net/wireless/b43legacy/phy.c
+++ b/drivers/net/wireless/broadcom/b43legacy/phy.c
diff --git a/drivers/net/wireless/b43legacy/phy.h b/drivers/net/wireless/broadcom/b43legacy/phy.h
index 831a7a4760e5..831a7a4760e5 100644
--- a/drivers/net/wireless/b43legacy/phy.h
+++ b/drivers/net/wireless/broadcom/b43legacy/phy.h
diff --git a/drivers/net/wireless/b43legacy/pio.c b/drivers/net/wireless/broadcom/b43legacy/pio.c
index 282eedec675e..282eedec675e 100644
--- a/drivers/net/wireless/b43legacy/pio.c
+++ b/drivers/net/wireless/broadcom/b43legacy/pio.c
diff --git a/drivers/net/wireless/b43legacy/pio.h b/drivers/net/wireless/broadcom/b43legacy/pio.h
index 8e6773ea6e75..8e6773ea6e75 100644
--- a/drivers/net/wireless/b43legacy/pio.h
+++ b/drivers/net/wireless/broadcom/b43legacy/pio.h
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/broadcom/b43legacy/radio.c
index 9501420340a9..9501420340a9 100644
--- a/drivers/net/wireless/b43legacy/radio.c
+++ b/drivers/net/wireless/broadcom/b43legacy/radio.c
diff --git a/drivers/net/wireless/b43legacy/radio.h b/drivers/net/wireless/broadcom/b43legacy/radio.h
index dd2976d1d561..dd2976d1d561 100644
--- a/drivers/net/wireless/b43legacy/radio.h
+++ b/drivers/net/wireless/broadcom/b43legacy/radio.h
diff --git a/drivers/net/wireless/b43legacy/rfkill.c b/drivers/net/wireless/broadcom/b43legacy/rfkill.c
index 7c1bdbc02569..7c1bdbc02569 100644
--- a/drivers/net/wireless/b43legacy/rfkill.c
+++ b/drivers/net/wireless/broadcom/b43legacy/rfkill.c
diff --git a/drivers/net/wireless/b43legacy/rfkill.h b/drivers/net/wireless/broadcom/b43legacy/rfkill.h
index 75585571c544..75585571c544 100644
--- a/drivers/net/wireless/b43legacy/rfkill.h
+++ b/drivers/net/wireless/broadcom/b43legacy/rfkill.h
diff --git a/drivers/net/wireless/b43legacy/sysfs.c b/drivers/net/wireless/broadcom/b43legacy/sysfs.c
index 2a1da15c913b..2a1da15c913b 100644
--- a/drivers/net/wireless/b43legacy/sysfs.c
+++ b/drivers/net/wireless/broadcom/b43legacy/sysfs.c
diff --git a/drivers/net/wireless/b43legacy/sysfs.h b/drivers/net/wireless/broadcom/b43legacy/sysfs.h
index 417d509803c7..417d509803c7 100644
--- a/drivers/net/wireless/b43legacy/sysfs.h
+++ b/drivers/net/wireless/broadcom/b43legacy/sysfs.h
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/broadcom/b43legacy/xmit.c
index 34bf3f0b729f..34bf3f0b729f 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/broadcom/b43legacy/xmit.c
diff --git a/drivers/net/wireless/b43legacy/xmit.h b/drivers/net/wireless/broadcom/b43legacy/xmit.h
index 289db00a4a7b..289db00a4a7b 100644
--- a/drivers/net/wireless/b43legacy/xmit.h
+++ b/drivers/net/wireless/broadcom/b43legacy/xmit.h
diff --git a/drivers/net/wireless/brcm80211/Kconfig b/drivers/net/wireless/broadcom/brcm80211/Kconfig
index ab42b1fea03c..ab42b1fea03c 100644
--- a/drivers/net/wireless/brcm80211/Kconfig
+++ b/drivers/net/wireless/broadcom/brcm80211/Kconfig
diff --git a/drivers/net/wireless/brcm80211/Makefile b/drivers/net/wireless/broadcom/brcm80211/Makefile
index b987920e982e..b987920e982e 100644
--- a/drivers/net/wireless/brcm80211/Makefile
+++ b/drivers/net/wireless/broadcom/brcm80211/Makefile
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/Makefile
index dc4c75083085..9e4b505ca593 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/Makefile
@@ -16,8 +16,8 @@
 # CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
 ccflags-y += \
-	-Idrivers/net/wireless/brcm80211/brcmfmac	\
-	-Idrivers/net/wireless/brcm80211/include
+	-Idrivers/net/wireless/broadcom/brcm80211/brcmfmac	\
+	-Idrivers/net/wireless/broadcom/brcm80211/include
 
 ccflags-y += -D__CHECK_ENDIAN__
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcdc.c
index 288c84e7c56b..6af658e443e4 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcdc.c
@@ -187,7 +187,8 @@ retry:
 		goto retry;
 	if (id != bcdc->reqid) {
 		brcmf_err("%s: unexpected request id %d (expected %d)\n",
-			  brcmf_ifname(drvr, ifidx), id, bcdc->reqid);
+			  brcmf_ifname(brcmf_get_ifp(drvr, ifidx)), id,
+			  bcdc->reqid);
 		ret = -EINVAL;
 		goto done;
 	}
@@ -234,7 +235,8 @@ brcmf_proto_bcdc_set_dcmd(struct brcmf_pub *drvr, int ifidx, uint cmd,
 
 	if (id != bcdc->reqid) {
 		brcmf_err("%s: unexpected request id %d (expected %d)\n",
-			  brcmf_ifname(drvr, ifidx), id, bcdc->reqid);
+			  brcmf_ifname(brcmf_get_ifp(drvr, ifidx)), id,
+			  bcdc->reqid);
 		ret = -EINVAL;
 		goto done;
 	}
@@ -298,13 +300,13 @@ brcmf_proto_bcdc_hdrpull(struct brcmf_pub *drvr, bool do_fws,
 	if (((h->flags & BCDC_FLAG_VER_MASK) >> BCDC_FLAG_VER_SHIFT) !=
 	    BCDC_PROTO_VER) {
 		brcmf_err("%s: non-BCDC packet received, flags 0x%x\n",
-			  brcmf_ifname(drvr, tmp_if->ifidx), h->flags);
+			  brcmf_ifname(tmp_if), h->flags);
 		return -EBADE;
 	}
 
 	if (h->flags & BCDC_FLAG_SUM_GOOD) {
 		brcmf_dbg(BCDC, "%s: BDC rcv, good checksum, flags 0x%x\n",
-			  brcmf_ifname(drvr, tmp_if->ifidx), h->flags);
+			  brcmf_ifname(tmp_if), h->flags);
 		pktbuf->ip_summed = CHECKSUM_UNNECESSARY;
 	}
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcdc.h
index 6003179c0ceb..6003179c0ceb 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcdc.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c
index 410a6645d316..b98db8a0a069 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcmsdh.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bcmsdh.c
@@ -47,6 +47,8 @@
 #include "debug.h"
 #include "sdio.h"
 #include "of.h"
+#include "core.h"
+#include "common.h"
 
 #define SDIOH_API_ACCESS_RETRY_LIMIT	2
 
@@ -57,7 +59,6 @@
 /* Maximum milliseconds to wait for F2 to come up */
 #define SDIO_WAIT_F2RDY	3000
 
-#define BRCMF_DEFAULT_TXGLOM_SIZE	32  /* max tx frames in glom chain */
 #define BRCMF_DEFAULT_RXGLOM_SIZE	32  /* max rx frames in glom chain */
 
 struct brcmf_sdiod_freezer {
@@ -68,10 +69,6 @@ struct brcmf_sdiod_freezer {
 	struct completion resumed;
 };
 
-static int brcmf_sdiod_txglomsz = BRCMF_DEFAULT_TXGLOM_SIZE;
-module_param_named(txglomsz, brcmf_sdiod_txglomsz, int, 0);
-MODULE_PARM_DESC(txglomsz, "maximum tx packet chain size [SDIO]");
-
 static irqreturn_t brcmf_sdiod_oob_irqhandler(int irq, void *dev_id)
 {
 	struct brcmf_bus *bus_if = dev_get_drvdata(dev_id);
@@ -882,15 +879,29 @@ int brcmf_sdiod_abort(struct brcmf_sdio_dev *sdiodev, uint fn)
 	return 0;
 }
 
-static void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev)
+void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev)
 {
+	struct sdio_func *func;
+	struct mmc_host *host;
+	uint max_blocks;
 	uint nents;
 	int err;
 
+	func = sdiodev->func[2];
+	host = func->card->host;
+	sdiodev->sg_support = host->max_segs > 1;
+	max_blocks = min_t(uint, host->max_blk_count, 511u);
+	sdiodev->max_request_size = min_t(uint, host->max_req_size,
+					  max_blocks * func->cur_blksize);
+	sdiodev->max_segment_count = min_t(uint, host->max_segs,
+					   SG_MAX_SINGLE_ALLOC);
+	sdiodev->max_segment_size = host->max_seg_size;
+
 	if (!sdiodev->sg_support)
 		return;
 
-	nents = max_t(uint, BRCMF_DEFAULT_RXGLOM_SIZE, brcmf_sdiod_txglomsz);
+	nents = max_t(uint, BRCMF_DEFAULT_RXGLOM_SIZE,
+		      sdiodev->bus_if->drvr->settings->sdiod_txglomsz);
 	nents += (nents >> 4) + 1;
 
 	WARN_ON(nents > sdiodev->max_segment_count);
@@ -902,7 +913,7 @@ static void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev)
 		sdiodev->sg_support = false;
 	}
 
-	sdiodev->txglomsz = brcmf_sdiod_txglomsz;
+	sdiodev->txglomsz = sdiodev->bus_if->drvr->settings->sdiod_txglomsz;
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -1023,9 +1034,6 @@ static void brcmf_sdiod_host_fixup(struct mmc_host *host)
 
 static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 {
-	struct sdio_func *func;
-	struct mmc_host *host;
-	uint max_blocks;
 	int ret = 0;
 
 	sdiodev->num_funcs = 2;
@@ -1056,26 +1064,6 @@ static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 		goto out;
 	}
 
-	/*
-	 * determine host related variables after brcmf_sdiod_probe()
-	 * as func->cur_blksize is properly set and F2 init has been
-	 * completed successfully.
-	 */
-	func = sdiodev->func[2];
-	host = func->card->host;
-	sdiodev->sg_support = host->max_segs > 1;
-	max_blocks = min_t(uint, host->max_blk_count, 511u);
-	sdiodev->max_request_size = min_t(uint, host->max_req_size,
-					  max_blocks * func->cur_blksize);
-	sdiodev->max_segment_count = min_t(uint, host->max_segs,
-					   SG_MAX_SINGLE_ALLOC);
-	sdiodev->max_segment_size = host->max_seg_size;
-
-	/* allocate scatter-gather table. sg support
-	 * will be disabled upon allocation failure.
-	 */
-	brcmf_sdiod_sgtable_alloc(sdiodev);
-
 	ret = brcmf_sdiod_freezer_attach(sdiodev);
 	if (ret)
 		goto out;
@@ -1086,7 +1074,7 @@ static int brcmf_sdiod_probe(struct brcmf_sdio_dev *sdiodev)
 		ret = -ENODEV;
 		goto out;
 	}
-	brcmf_sdiod_host_fixup(host);
+	brcmf_sdiod_host_fixup(sdiodev->func[2]->card->host);
 out:
 	if (ret)
 		brcmf_sdiod_remove(sdiodev);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c
index 4e33f96b3dd1..14a70d4b4e86 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.c
@@ -29,7 +29,7 @@
 #include "cfg80211.h"
 
 /* T1 start SCO/eSCO priority suppression */
-#define BRCMF_BTCOEX_OPPR_WIN_TIME   2000
+#define BRCMF_BTCOEX_OPPR_WIN_TIME   msecs_to_jiffies(2000)
 
 /* BT registers values during DHCP */
 #define BRCMF_BT_DHCP_REG50 0x8022
@@ -314,8 +314,7 @@ static void brcmf_btcoex_handler(struct work_struct *work)
 		} else {
 			btci->timeout -= BRCMF_BTCOEX_OPPR_WIN_TIME;
 			mod_timer(&btci->timer,
-				  jiffies +
-				  msecs_to_jiffies(BRCMF_BTCOEX_OPPR_WIN_TIME));
+				  jiffies + BRCMF_BTCOEX_OPPR_WIN_TIME);
 		}
 		btci->timer_on = true;
 		break;
@@ -328,12 +327,11 @@ static void brcmf_btcoex_handler(struct work_struct *work)
 
 		/* DHCP is not over yet, start lowering BT priority */
 		brcmf_dbg(INFO, "DHCP T1:%d expired\n",
-			  BRCMF_BTCOEX_OPPR_WIN_TIME);
+			  jiffies_to_msecs(BRCMF_BTCOEX_OPPR_WIN_TIME));
 		brcmf_btcoex_boost_wifi(btci, true);
 
 		btci->bt_state = BRCMF_BT_DHCP_FLAG_FORCE_TIMEOUT;
-		mod_timer(&btci->timer,
-			  jiffies + msecs_to_jiffies(btci->timeout));
+		mod_timer(&btci->timer, jiffies + btci->timeout);
 		btci->timer_on = true;
 		break;
 
@@ -477,7 +475,7 @@ int brcmf_btcoex_set_mode(struct brcmf_cfg80211_vif *vif,
 			return -EBUSY;
 		/* Start BT timer only for SCO connection */
 		if (brcmf_btcoex_is_sco_active(ifp)) {
-			btci->timeout = duration;
+			btci->timeout = msecs_to_jiffies(duration);
 			btci->vif = vif;
 			brcmf_btcoex_dhcp_start(btci);
 		}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.h
index 19647c68aa9e..19647c68aa9e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/btcoex.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/btcoex.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bus.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
index 230cad788ace..36093f93bfbe 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bus.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/bus.h
@@ -137,7 +137,7 @@ struct brcmf_bus {
 	bool always_use_fws_queue;
 	bool wowl_supported;
 
-	struct brcmf_bus_ops *ops;
+	const struct brcmf_bus_ops *ops;
 	struct brcmf_bus_msgbuf *msgbuf;
 };
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
index deb5f78dcacc..7b01e4ddb315 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.c
@@ -91,6 +91,12 @@
 #define BRCMF_SCAN_JOIN_PASSIVE_DWELL_TIME_MS	400
 #define BRCMF_SCAN_JOIN_PROBE_INTERVAL_MS	20
 
+#define BRCMF_SCAN_CHANNEL_TIME		40
+#define BRCMF_SCAN_UNASSOC_TIME		40
+#define BRCMF_SCAN_PASSIVE_TIME		120
+
+#define BRCMF_ND_INFO_TIMEOUT		msecs_to_jiffies(2000)
+
 #define BRCMF_ASSOC_PARAMS_FIXED_SIZE \
 	(sizeof(struct brcmf_assoc_params_le) - sizeof(u16))
 
@@ -232,11 +238,6 @@ struct parsed_vndr_ies {
 	struct parsed_vndr_ie_info ie_info[VNDR_IE_PARSE_LIMIT];
 };
 
-static int brcmf_roamoff;
-module_param_named(roamoff, brcmf_roamoff, int, S_IRUSR);
-MODULE_PARM_DESC(roamoff, "do not use internal roaming engine");
-
-
 static u16 chandef_to_chanspec(struct brcmu_d11inf *d11inf,
 			       struct cfg80211_chan_def *ch)
 {
@@ -392,15 +393,23 @@ static int brcmf_vif_change_validate(struct brcmf_cfg80211_info *cfg,
 {
 	int iftype_num[NUM_NL80211_IFTYPES];
 	struct brcmf_cfg80211_vif *pos;
+	bool check_combos = false;
+	int ret = 0;
 
 	memset(&iftype_num[0], 0, sizeof(iftype_num));
 	list_for_each_entry(pos, &cfg->vif_list, list)
-		if (pos == vif)
+		if (pos == vif) {
 			iftype_num[new_type]++;
-		else
+		} else {
+			/* concurrent interfaces so need check combinations */
+			check_combos = true;
 			iftype_num[pos->wdev.iftype]++;
+		}
 
-	return cfg80211_check_combinations(cfg->wiphy, 1, 0, iftype_num);
+	if (check_combos)
+		ret = cfg80211_check_combinations(cfg->wiphy, 1, 0, iftype_num);
+
+	return ret;
 }
 
 static int brcmf_vif_add_validate(struct brcmf_cfg80211_info *cfg,
@@ -559,7 +568,7 @@ struct wireless_dev *brcmf_ap_add_vif(struct wiphy *wiphy, const char *name,
 
 	/* wait for firmware event */
 	err = brcmf_cfg80211_wait_vif_event_timeout(cfg, BRCMF_E_IF_ADD,
-						    msecs_to_jiffies(1500));
+						    BRCMF_VIF_EVENT_TIMEOUT);
 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
 	if (!err) {
 		brcmf_err("timeout occurred\n");
@@ -776,7 +785,8 @@ brcmf_cfg80211_change_iface(struct wiphy *wiphy, struct net_device *ndev,
 	s32 ap = 0;
 	s32 err = 0;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d, type=%d\n", ifp->bssidx, type);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d, type=%d\n", ifp->bsscfgidx,
+		  type);
 
 	/* WAR: There are a number of p2p interface related problems which
 	 * need to be handled initially (before doing the validate).
@@ -945,7 +955,7 @@ static void brcmf_escan_prep(struct brcmf_cfg80211_info *cfg,
 
 static s32
 brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
-		struct cfg80211_scan_request *request, u16 action)
+		struct cfg80211_scan_request *request)
 {
 	s32 params_size = BRCMF_SCAN_PARAMS_FIXED_SIZE +
 			  offsetof(struct brcmf_escan_params_le, params_le);
@@ -959,7 +969,7 @@ brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
 		params_size += sizeof(u32) * ((request->n_channels + 1) / 2);
 
 		/* Allocate space for populating ssids in struct */
-		params_size += sizeof(struct brcmf_ssid) * request->n_ssids;
+		params_size += sizeof(struct brcmf_ssid_le) * request->n_ssids;
 	}
 
 	params = kzalloc(params_size, GFP_KERNEL);
@@ -970,7 +980,7 @@ brcmf_run_escan(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
 	BUG_ON(params_size + sizeof("escan") >= BRCMF_DCMD_MEDLEN);
 	brcmf_escan_prep(cfg, &params->params_le, request);
 	params->version = cpu_to_le32(BRCMF_ESCAN_REQ_VERSION);
-	params->action = cpu_to_le16(action);
+	params->action = cpu_to_le16(WL_ESCAN_ACTION_START);
 	params->sync_id = cpu_to_le16(0x1234);
 
 	err = brcmf_fil_iovar_data_set(ifp, "escan", params, params_size);
@@ -1012,7 +1022,7 @@ brcmf_do_escan(struct brcmf_cfg80211_info *cfg, struct wiphy *wiphy,
 	results->count = 0;
 	results->buflen = WL_ESCAN_RESULTS_FIXED_SIZE;
 
-	err = escan->run(cfg, ifp, request, WL_ESCAN_ACTION_START);
+	err = escan->run(cfg, ifp, request);
 	if (err)
 		brcmf_scan_config_mpc(ifp, 1);
 	return err;
@@ -1026,11 +1036,11 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 	struct brcmf_if *ifp = vif->ifp;
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct cfg80211_ssid *ssids;
-	struct brcmf_cfg80211_scan_req *sr = &cfg->scan_req_int;
 	u32 passive_scan;
 	bool escan_req;
 	bool spec_scan;
 	s32 err;
+	struct brcmf_ssid_le ssid_le;
 	u32 SSID_len;
 
 	brcmf_dbg(SCAN, "START ESCAN\n");
@@ -1083,13 +1093,13 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 	} else {
 		brcmf_dbg(SCAN, "ssid \"%s\", ssid_len (%d)\n",
 			  ssids->ssid, ssids->ssid_len);
-		memset(&sr->ssid_le, 0, sizeof(sr->ssid_le));
-		SSID_len = min_t(u8, sizeof(sr->ssid_le.SSID), ssids->ssid_len);
-		sr->ssid_le.SSID_len = cpu_to_le32(0);
+		memset(&ssid_le, 0, sizeof(ssid_le));
+		SSID_len = min_t(u8, sizeof(ssid_le.SSID), ssids->ssid_len);
+		ssid_le.SSID_len = cpu_to_le32(0);
 		spec_scan = false;
 		if (SSID_len) {
-			memcpy(sr->ssid_le.SSID, ssids->ssid, SSID_len);
-			sr->ssid_le.SSID_len = cpu_to_le32(SSID_len);
+			memcpy(ssid_le.SSID, ssids->ssid, SSID_len);
+			ssid_le.SSID_len = cpu_to_le32(SSID_len);
 			spec_scan = true;
 		} else
 			brcmf_dbg(SCAN, "Broadcast scan\n");
@@ -1102,12 +1112,12 @@ brcmf_cfg80211_escan(struct wiphy *wiphy, struct brcmf_cfg80211_vif *vif,
 			goto scan_out;
 		}
 		brcmf_scan_config_mpc(ifp, 0);
-		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN,
-					     &sr->ssid_le, sizeof(sr->ssid_le));
+		err = brcmf_fil_cmd_data_set(ifp, BRCMF_C_SCAN, &ssid_le,
+					     sizeof(ssid_le));
 		if (err) {
 			if (err == -EBUSY)
 				brcmf_dbg(INFO, "BUSY: scan for \"%s\" canceled\n",
-					  sr->ssid_le.SSID);
+					  ssid_le.SSID);
 			else
 				brcmf_err("WLC_SCAN error (%d)\n", err);
 
@@ -1260,17 +1270,17 @@ static void brcmf_link_down(struct brcmf_cfg80211_vif *vif, u16 reason)
 
 	brcmf_dbg(TRACE, "Enter\n");
 
-	if (test_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state)) {
+	if (test_and_clear_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state)) {
 		brcmf_dbg(INFO, "Call WLC_DISASSOC to stop excess roaming\n ");
 		err = brcmf_fil_cmd_data_set(vif->ifp,
 					     BRCMF_C_DISASSOC, NULL, 0);
 		if (err) {
 			brcmf_err("WLC_DISASSOC failed (%d)\n", err);
 		}
-		clear_bit(BRCMF_VIF_STATUS_CONNECTED, &vif->sme_state);
-		cfg80211_disconnected(vif->wdev.netdev, reason, NULL, 0,
-				      true, GFP_KERNEL);
-
+		if ((vif->wdev.iftype == NL80211_IFTYPE_STATION) ||
+		    (vif->wdev.iftype == NL80211_IFTYPE_P2P_CLIENT))
+			cfg80211_disconnected(vif->wdev.netdev, reason, NULL, 0,
+					      true, GFP_KERNEL);
 	}
 	clear_bit(BRCMF_VIF_STATUS_CONNECTING, &vif->sme_state);
 	clear_bit(BRCMF_SCAN_STATUS_SUPPRESS, &cfg->scan_status);
@@ -1291,6 +1301,7 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 	s32 wsec = 0;
 	s32 bcnprd;
 	u16 chanspec;
+	u32 ssid_len;
 
 	brcmf_dbg(TRACE, "Enter\n");
 	if (!check_vif_up(ifp->vif))
@@ -1368,17 +1379,15 @@ brcmf_cfg80211_join_ibss(struct wiphy *wiphy, struct net_device *ndev,
 	memset(&join_params, 0, sizeof(struct brcmf_join_params));
 
 	/* SSID */
-	profile->ssid.SSID_len = min_t(u32, params->ssid_len, 32);
-	memcpy(profile->ssid.SSID, params->ssid, profile->ssid.SSID_len);
-	memcpy(join_params.ssid_le.SSID, params->ssid, profile->ssid.SSID_len);
-	join_params.ssid_le.SSID_len = cpu_to_le32(profile->ssid.SSID_len);
+	ssid_len = min_t(u32, params->ssid_len, IEEE80211_MAX_SSID_LEN);
+	memcpy(join_params.ssid_le.SSID, params->ssid, ssid_len);
+	join_params.ssid_le.SSID_len = cpu_to_le32(ssid_len);
 	join_params_size = sizeof(join_params.ssid_le);
 
 	/* BSSID */
 	if (params->bssid) {
 		memcpy(join_params.params_le.bssid, params->bssid, ETH_ALEN);
-		join_params_size = sizeof(join_params.ssid_le) +
-				   BRCMF_ASSOC_PARAMS_FIXED_SIZE;
+		join_params_size += BRCMF_ASSOC_PARAMS_FIXED_SIZE;
 		memcpy(profile->bssid, params->bssid, ETH_ALEN);
 	} else {
 		eth_broadcast_addr(join_params.params_le.bssid);
@@ -1436,10 +1445,16 @@ brcmf_cfg80211_leave_ibss(struct wiphy *wiphy, struct net_device *ndev)
 	struct brcmf_if *ifp = netdev_priv(ndev);
 
 	brcmf_dbg(TRACE, "Enter\n");
-	if (!check_vif_up(ifp->vif))
-		return -EIO;
+	if (!check_vif_up(ifp->vif)) {
+		/* When driver is being unloaded, it can end up here. If an
+		 * error is returned then later on a debug trace in the wireless
+		 * core module will be printed. To avoid this 0 is returned.
+		 */
+		return 0;
+	}
 
 	brcmf_link_down(ifp->vif, WLAN_REASON_DEAUTH_LEAVING);
+	brcmf_net_setcarrier(ifp, false);
 
 	brcmf_dbg(TRACE, "Exit\n");
 
@@ -1728,7 +1743,6 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 {
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_cfg80211_profile *profile = &ifp->vif->profile;
 	struct ieee80211_channel *chan = sme->channel;
 	struct brcmf_join_params join_params;
 	size_t join_params_size;
@@ -1739,6 +1753,7 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	struct brcmf_ext_join_params_le *ext_join_params;
 	u16 chanspec;
 	s32 err = 0;
+	u32 ssid_len;
 
 	brcmf_dbg(TRACE, "Enter\n");
 	if (!check_vif_up(ifp->vif))
@@ -1824,15 +1839,6 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 		goto done;
 	}
 
-	profile->ssid.SSID_len = min_t(u32, (u32)sizeof(profile->ssid.SSID),
-				       (u32)sme->ssid_len);
-	memcpy(&profile->ssid.SSID, sme->ssid, profile->ssid.SSID_len);
-	if (profile->ssid.SSID_len < IEEE80211_MAX_SSID_LEN) {
-		profile->ssid.SSID[profile->ssid.SSID_len] = 0;
-		brcmf_dbg(CONN, "SSID \"%s\", len (%d)\n", profile->ssid.SSID,
-			  profile->ssid.SSID_len);
-	}
-
 	/* Join with specific BSSID and cached SSID
 	 * If SSID is zero join based on BSSID only
 	 */
@@ -1845,9 +1851,12 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 		err = -ENOMEM;
 		goto done;
 	}
-	ext_join_params->ssid_le.SSID_len = cpu_to_le32(profile->ssid.SSID_len);
-	memcpy(&ext_join_params->ssid_le.SSID, sme->ssid,
-	       profile->ssid.SSID_len);
+	ssid_len = min_t(u32, sme->ssid_len, IEEE80211_MAX_SSID_LEN);
+	ext_join_params->ssid_le.SSID_len = cpu_to_le32(ssid_len);
+	memcpy(&ext_join_params->ssid_le.SSID, sme->ssid, ssid_len);
+	if (ssid_len < IEEE80211_MAX_SSID_LEN)
+		brcmf_dbg(CONN, "SSID \"%s\", len (%d)\n",
+			  ext_join_params->ssid_le.SSID, ssid_len);
 
 	/* Set up join scan parameters */
 	ext_join_params->scan_le.scan_type = -1;
@@ -1895,8 +1904,8 @@ brcmf_cfg80211_connect(struct wiphy *wiphy, struct net_device *ndev,
 	memset(&join_params, 0, sizeof(join_params));
 	join_params_size = sizeof(join_params.ssid_le);
 
-	memcpy(&join_params.ssid_le.SSID, sme->ssid, profile->ssid.SSID_len);
-	join_params.ssid_le.SSID_len = cpu_to_le32(profile->ssid.SSID_len);
+	memcpy(&join_params.ssid_le.SSID, sme->ssid, ssid_len);
+	join_params.ssid_le.SSID_len = cpu_to_le32(ssid_len);
 
 	if (sme->bssid)
 		memcpy(join_params.params_le.bssid, sme->bssid, ETH_ALEN);
@@ -2423,6 +2432,54 @@ static void brcmf_fill_bss_param(struct brcmf_if *ifp, struct station_info *si)
 }
 
 static s32
+brcmf_cfg80211_get_station_ibss(struct brcmf_if *ifp,
+				struct station_info *sinfo)
+{
+	struct brcmf_scb_val_le scbval;
+	struct brcmf_pktcnt_le pktcnt;
+	s32 err;
+	u32 rate;
+	u32 rssi;
+
+	/* Get the current tx rate */
+	err = brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_RATE, &rate);
+	if (err < 0) {
+		brcmf_err("BRCMF_C_GET_RATE error (%d)\n", err);
+		return err;
+	}
+	sinfo->filled |= BIT(NL80211_STA_INFO_TX_BITRATE);
+	sinfo->txrate.legacy = rate * 5;
+
+	memset(&scbval, 0, sizeof(scbval));
+	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_RSSI, &scbval,
+				     sizeof(scbval));
+	if (err) {
+		brcmf_err("BRCMF_C_GET_RSSI error (%d)\n", err);
+		return err;
+	}
+	rssi = le32_to_cpu(scbval.val);
+	sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
+	sinfo->signal = rssi;
+
+	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_GET_PKTCNTS, &pktcnt,
+				     sizeof(pktcnt));
+	if (err) {
+		brcmf_err("BRCMF_C_GET_GET_PKTCNTS error (%d)\n", err);
+		return err;
+	}
+	sinfo->filled |= BIT(NL80211_STA_INFO_RX_PACKETS) |
+			 BIT(NL80211_STA_INFO_RX_DROP_MISC) |
+			 BIT(NL80211_STA_INFO_TX_PACKETS) |
+			 BIT(NL80211_STA_INFO_TX_FAILED);
+	sinfo->rx_packets = le32_to_cpu(pktcnt.rx_good_pkt);
+	sinfo->rx_dropped_misc = le32_to_cpu(pktcnt.rx_bad_pkt);
+	sinfo->tx_packets = le32_to_cpu(pktcnt.tx_good_pkt);
+	sinfo->tx_failed  = le32_to_cpu(pktcnt.tx_bad_pkt);
+
+	return 0;
+}
+
+static s32
 brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 			   const u8 *mac, struct station_info *sinfo)
 {
@@ -2439,6 +2496,9 @@ brcmf_cfg80211_get_station(struct wiphy *wiphy, struct net_device *ndev,
 	if (!check_vif_up(ifp->vif))
 		return -EIO;
 
+	if (brcmf_is_ibssmode(ifp->vif))
+		return brcmf_cfg80211_get_station_ibss(ifp, sinfo);
+
 	memset(&sta_info_le, 0, sizeof(sta_info_le));
 	memcpy(&sta_info_le, mac, ETH_ALEN);
 	err = brcmf_fil_iovar_data_get(ifp, "tdls_sta_info",
@@ -2691,8 +2751,8 @@ static s32 brcmf_inform_bss(struct brcmf_cfg80211_info *cfg)
 	return err;
 }
 
-static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
-			  struct net_device *ndev, const u8 *bssid)
+static s32 brcmf_inform_ibss(struct brcmf_cfg80211_info *cfg,
+			     struct net_device *ndev, const u8 *bssid)
 {
 	struct wiphy *wiphy = cfg_to_wiphy(cfg);
 	struct ieee80211_channel *notify_channel;
@@ -2737,6 +2797,7 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 		band = wiphy->bands[IEEE80211_BAND_5GHZ];
 
 	freq = ieee80211_channel_to_frequency(ch.chnum, band->band);
+	cfg->channel = freq;
 	notify_channel = ieee80211_get_channel(wiphy, freq);
 
 	notify_capability = le16_to_cpu(bi->capability);
@@ -2775,9 +2836,7 @@ CleanUp:
 static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg,
 				 struct brcmf_if *ifp)
 {
-	struct brcmf_cfg80211_profile *profile = ndev_to_prof(ifp->ndev);
 	struct brcmf_bss_info_le *bi;
-	struct brcmf_ssid *ssid;
 	const struct brcmf_tlv *tim;
 	u16 beacon_interval;
 	u8 dtim_period;
@@ -2789,8 +2848,6 @@ static s32 brcmf_update_bss_info(struct brcmf_cfg80211_info *cfg,
 	if (brcmf_is_ibssmode(ifp->vif))
 		return err;
 
-	ssid = &profile->ssid;
-
 	*(__le32 *)cfg->extra_buf = cpu_to_le32(WL_EXTRA_BUF_MAX);
 	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BSS_INFO,
 				     cfg->extra_buf, WL_EXTRA_BUF_MAX);
@@ -2921,7 +2978,7 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 	status = e->status;
 
 	if (!test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status)) {
-		brcmf_err("scan not ready, bssidx=%d\n", ifp->bssidx);
+		brcmf_err("scan not ready, bsscfgidx=%d\n", ifp->bsscfgidx);
 		return -EPERM;
 	}
 
@@ -3013,296 +3070,7 @@ static void brcmf_init_escan(struct brcmf_cfg80211_info *cfg)
 		  brcmf_cfg80211_escan_timeout_worker);
 }
 
-static __always_inline void brcmf_delay(u32 ms)
-{
-	if (ms < 1000 / HZ) {
-		cond_resched();
-		mdelay(ms);
-	} else {
-		msleep(ms);
-	}
-}
-
-static s32 brcmf_config_wowl_pattern(struct brcmf_if *ifp, u8 cmd[4],
-				     u8 *pattern, u32 patternsize, u8 *mask,
-				     u32 packet_offset)
-{
-	struct brcmf_fil_wowl_pattern_le *filter;
-	u32 masksize;
-	u32 patternoffset;
-	u8 *buf;
-	u32 bufsize;
-	s32 ret;
-
-	masksize = (patternsize + 7) / 8;
-	patternoffset = sizeof(*filter) - sizeof(filter->cmd) + masksize;
-
-	bufsize = sizeof(*filter) + patternsize + masksize;
-	buf = kzalloc(bufsize, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-	filter = (struct brcmf_fil_wowl_pattern_le *)buf;
-
-	memcpy(filter->cmd, cmd, 4);
-	filter->masksize = cpu_to_le32(masksize);
-	filter->offset = cpu_to_le32(packet_offset);
-	filter->patternoffset = cpu_to_le32(patternoffset);
-	filter->patternsize = cpu_to_le32(patternsize);
-	filter->type = cpu_to_le32(BRCMF_WOWL_PATTERN_TYPE_BITMAP);
-
-	if ((mask) && (masksize))
-		memcpy(buf + sizeof(*filter), mask, masksize);
-	if ((pattern) && (patternsize))
-		memcpy(buf + sizeof(*filter) + masksize, pattern, patternsize);
-
-	ret = brcmf_fil_iovar_data_set(ifp, "wowl_pattern", buf, bufsize);
-
-	kfree(buf);
-	return ret;
-}
-
-static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
-{
-	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-	struct net_device *ndev = cfg_to_ndev(cfg);
-	struct brcmf_if *ifp = netdev_priv(ndev);
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	if (cfg->wowl_enabled) {
-		brcmf_configure_arp_offload(ifp, true);
-		brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM,
-				      cfg->pre_wowl_pmmode);
-		brcmf_fil_iovar_int_set(ifp, "wowl_clear", 0);
-		brcmf_config_wowl_pattern(ifp, "clr", NULL, 0, NULL, 0);
-		cfg->wowl_enabled = false;
-	}
-	return 0;
-}
-
-static void brcmf_configure_wowl(struct brcmf_cfg80211_info *cfg,
-				 struct brcmf_if *ifp,
-				 struct cfg80211_wowlan *wowl)
-{
-	u32 wowl_config;
-	u32 i;
-
-	brcmf_dbg(TRACE, "Suspend, wowl config.\n");
-
-	brcmf_configure_arp_offload(ifp, false);
-	brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_PM, &cfg->pre_wowl_pmmode);
-	brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM, PM_MAX);
-
-	wowl_config = 0;
-	if (wowl->disconnect)
-		wowl_config = BRCMF_WOWL_DIS | BRCMF_WOWL_BCN | BRCMF_WOWL_RETR;
-	if (wowl->magic_pkt)
-		wowl_config |= BRCMF_WOWL_MAGIC;
-	if ((wowl->patterns) && (wowl->n_patterns)) {
-		wowl_config |= BRCMF_WOWL_NET;
-		for (i = 0; i < wowl->n_patterns; i++) {
-			brcmf_config_wowl_pattern(ifp, "add",
-				(u8 *)wowl->patterns[i].pattern,
-				wowl->patterns[i].pattern_len,
-				(u8 *)wowl->patterns[i].mask,
-				wowl->patterns[i].pkt_offset);
-		}
-	}
-	brcmf_fil_iovar_int_set(ifp, "wowl", wowl_config);
-	brcmf_fil_iovar_int_set(ifp, "wowl_activate", 1);
-	brcmf_bus_wowl_config(cfg->pub->bus_if, true);
-	cfg->wowl_enabled = true;
-}
-
-static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
-				  struct cfg80211_wowlan *wowl)
-{
-	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-	struct net_device *ndev = cfg_to_ndev(cfg);
-	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct brcmf_cfg80211_vif *vif;
-
-	brcmf_dbg(TRACE, "Enter\n");
-
-	/* if the primary net_device is not READY there is nothing
-	 * we can do but pray resume goes smoothly.
-	 */
-	if (!check_vif_up(ifp->vif))
-		goto exit;
-
-	/* end any scanning */
-	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
-		brcmf_abort_scanning(cfg);
-
-	if (wowl == NULL) {
-		brcmf_bus_wowl_config(cfg->pub->bus_if, false);
-		list_for_each_entry(vif, &cfg->vif_list, list) {
-			if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state))
-				continue;
-			/* While going to suspend if associated with AP
-			 * disassociate from AP to save power while system is
-			 * in suspended state
-			 */
-			brcmf_link_down(vif, WLAN_REASON_UNSPECIFIED);
-			/* Make sure WPA_Supplicant receives all the event
-			 * generated due to DISASSOC call to the fw to keep
-			 * the state fw and WPA_Supplicant state consistent
-			 */
-			brcmf_delay(500);
-		}
-		/* Configure MPC */
-		brcmf_set_mpc(ifp, 1);
-
-	} else {
-		/* Configure WOWL paramaters */
-		brcmf_configure_wowl(cfg, ifp, wowl);
-	}
-
-exit:
-	brcmf_dbg(TRACE, "Exit\n");
-	/* clear any scanning activity */
-	cfg->scan_status = 0;
-	return 0;
-}
-
-static __used s32
-brcmf_update_pmklist(struct net_device *ndev,
-		     struct brcmf_cfg80211_pmk_list *pmk_list, s32 err)
-{
-	int i, j;
-	u32 pmkid_len;
-
-	pmkid_len = le32_to_cpu(pmk_list->pmkids.npmkid);
-
-	brcmf_dbg(CONN, "No of elements %d\n", pmkid_len);
-	for (i = 0; i < pmkid_len; i++) {
-		brcmf_dbg(CONN, "PMKID[%d]: %pM =\n", i,
-			  &pmk_list->pmkids.pmkid[i].BSSID);
-		for (j = 0; j < WLAN_PMKID_LEN; j++)
-			brcmf_dbg(CONN, "%02x\n",
-				  pmk_list->pmkids.pmkid[i].PMKID[j]);
-	}
-
-	if (!err)
-		brcmf_fil_iovar_data_set(netdev_priv(ndev), "pmkid_info",
-					 (char *)pmk_list, sizeof(*pmk_list));
-
-	return err;
-}
-
-static s32
-brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *ndev,
-			 struct cfg80211_pmksa *pmksa)
-{
-	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct pmkid_list *pmkids = &cfg->pmk_list->pmkids;
-	s32 err = 0;
-	u32 pmkid_len, i;
-
-	brcmf_dbg(TRACE, "Enter\n");
-	if (!check_vif_up(ifp->vif))
-		return -EIO;
-
-	pmkid_len = le32_to_cpu(pmkids->npmkid);
-	for (i = 0; i < pmkid_len; i++)
-		if (!memcmp(pmksa->bssid, pmkids->pmkid[i].BSSID, ETH_ALEN))
-			break;
-	if (i < WL_NUM_PMKIDS_MAX) {
-		memcpy(pmkids->pmkid[i].BSSID, pmksa->bssid, ETH_ALEN);
-		memcpy(pmkids->pmkid[i].PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
-		if (i == pmkid_len) {
-			pmkid_len++;
-			pmkids->npmkid = cpu_to_le32(pmkid_len);
-		}
-	} else
-		err = -EINVAL;
-
-	brcmf_dbg(CONN, "set_pmksa,IW_PMKSA_ADD - PMKID: %pM =\n",
-		  pmkids->pmkid[pmkid_len].BSSID);
-	for (i = 0; i < WLAN_PMKID_LEN; i++)
-		brcmf_dbg(CONN, "%02x\n", pmkids->pmkid[pmkid_len].PMKID[i]);
-
-	err = brcmf_update_pmklist(ndev, cfg->pmk_list, err);
-
-	brcmf_dbg(TRACE, "Exit\n");
-	return err;
-}
-
-static s32
-brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *ndev,
-		      struct cfg80211_pmksa *pmksa)
-{
-	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-	struct brcmf_if *ifp = netdev_priv(ndev);
-	struct pmkid_list pmkid;
-	s32 err = 0;
-	u32 pmkid_len, i;
-
-	brcmf_dbg(TRACE, "Enter\n");
-	if (!check_vif_up(ifp->vif))
-		return -EIO;
-
-	memcpy(&pmkid.pmkid[0].BSSID, pmksa->bssid, ETH_ALEN);
-	memcpy(&pmkid.pmkid[0].PMKID, pmksa->pmkid, WLAN_PMKID_LEN);
-
-	brcmf_dbg(CONN, "del_pmksa,IW_PMKSA_REMOVE - PMKID: %pM =\n",
-		  &pmkid.pmkid[0].BSSID);
-	for (i = 0; i < WLAN_PMKID_LEN; i++)
-		brcmf_dbg(CONN, "%02x\n", pmkid.pmkid[0].PMKID[i]);
-
-	pmkid_len = le32_to_cpu(cfg->pmk_list->pmkids.npmkid);
-	for (i = 0; i < pmkid_len; i++)
-		if (!memcmp
-		    (pmksa->bssid, &cfg->pmk_list->pmkids.pmkid[i].BSSID,
-		     ETH_ALEN))
-			break;
-
-	if ((pmkid_len > 0)
-	    && (i < pmkid_len)) {
-		memset(&cfg->pmk_list->pmkids.pmkid[i], 0,
-		       sizeof(struct pmkid));
-		for (; i < (pmkid_len - 1); i++) {
-			memcpy(&cfg->pmk_list->pmkids.pmkid[i].BSSID,
-			       &cfg->pmk_list->pmkids.pmkid[i + 1].BSSID,
-			       ETH_ALEN);
-			memcpy(&cfg->pmk_list->pmkids.pmkid[i].PMKID,
-			       &cfg->pmk_list->pmkids.pmkid[i + 1].PMKID,
-			       WLAN_PMKID_LEN);
-		}
-		cfg->pmk_list->pmkids.npmkid = cpu_to_le32(pmkid_len - 1);
-	} else
-		err = -EINVAL;
-
-	err = brcmf_update_pmklist(ndev, cfg->pmk_list, err);
-
-	brcmf_dbg(TRACE, "Exit\n");
-	return err;
-
-}
-
-static s32
-brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *ndev)
-{
-	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
-	struct brcmf_if *ifp = netdev_priv(ndev);
-	s32 err = 0;
-
-	brcmf_dbg(TRACE, "Enter\n");
-	if (!check_vif_up(ifp->vif))
-		return -EIO;
-
-	memset(cfg->pmk_list, 0, sizeof(*cfg->pmk_list));
-	err = brcmf_update_pmklist(ndev, cfg->pmk_list, err);
-
-	brcmf_dbg(TRACE, "Exit\n");
-	return err;
-
-}
-
-/*
- * PFN result doesn't have all the info which are
- * required by the supplicant
+/* PFN result doesn't have all the info which are required by the supplicant
  * (For e.g IEs) Do a target Escan so that sched scan results are reported
  * via wl_inform_single_bss in the required format. Escan does require the
  * scan request in the form of cfg80211_scan_request. For timebeing, create
@@ -3336,8 +3104,7 @@ brcmf_notify_sched_scan_results(struct brcmf_if *ifp,
 	result_count = le32_to_cpu(pfn_result->count);
 	status = le32_to_cpu(pfn_result->status);
 
-	/*
-	 * PFN event is limited to fit 512 bytes so we may get
+	/* PFN event is limited to fit 512 bytes so we may get
 	 * multiple NET_FOUND events. For now place a warning here.
 	 */
 	WARN_ON(status != BRCMF_PNO_SCAN_COMPLETE);
@@ -3439,9 +3206,14 @@ static int brcmf_dev_pno_clean(struct net_device *ndev)
 	return ret;
 }
 
-static int brcmf_dev_pno_config(struct net_device *ndev)
+static int brcmf_dev_pno_config(struct brcmf_if *ifp,
+				struct cfg80211_sched_scan_request *request)
 {
 	struct brcmf_pno_param_le pfn_param;
+	struct brcmf_pno_macaddr_le pfn_mac;
+	s32 err;
+	u8 *mac_mask;
+	int i;
 
 	memset(&pfn_param, 0, sizeof(pfn_param));
 	pfn_param.version = cpu_to_le32(BRCMF_PNO_VERSION);
@@ -3454,8 +3226,37 @@ static int brcmf_dev_pno_config(struct net_device *ndev)
 	/* set up pno scan fr */
 	pfn_param.scan_freq = cpu_to_le32(BRCMF_PNO_TIME);
 
-	return brcmf_fil_iovar_data_set(netdev_priv(ndev), "pfn_set",
-					&pfn_param, sizeof(pfn_param));
+	err = brcmf_fil_iovar_data_set(ifp, "pfn_set", &pfn_param,
+				       sizeof(pfn_param));
+	if (err) {
+		brcmf_err("pfn_set failed, err=%d\n", err);
+		return err;
+	}
+
+	/* Find out if mac randomization should be turned on */
+	if (!(request->flags & NL80211_SCAN_FLAG_RANDOM_ADDR))
+		return 0;
+
+	pfn_mac.version = BRCMF_PFN_MACADDR_CFG_VER;
+	pfn_mac.flags = BRCMF_PFN_MAC_OUI_ONLY | BRCMF_PFN_SET_MAC_UNASSOC;
+
+	memcpy(pfn_mac.mac, request->mac_addr, ETH_ALEN);
+	mac_mask = request->mac_addr_mask;
+	for (i = 0; i < ETH_ALEN; i++) {
+		pfn_mac.mac[i] &= mac_mask[i];
+		pfn_mac.mac[i] |= get_random_int() & ~(mac_mask[i]);
+	}
+	/* Clear multi bit */
+	pfn_mac.mac[0] &= 0xFE;
+	/* Set locally administered */
+	pfn_mac.mac[0] |= 0x02;
+
+	err = brcmf_fil_iovar_data_set(ifp, "pfn_macaddr", &pfn_mac,
+				       sizeof(pfn_mac));
+	if (err)
+		brcmf_err("pfn_macaddr failed, err=%d\n", err);
+
+	return err;
 }
 
 static int
@@ -3493,9 +3294,8 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 			brcmf_dbg(SCAN, ">>> Active scan req for ssid (%s)\n",
 				  request->ssids[i].ssid);
 
-			/*
-			 * match_set ssids is a supert set of n_ssid list,
-			 * so we need not add these set seperately.
+			/* match_set ssids is a supert set of n_ssid list,
+			 * so we need not add these set separately.
 			 */
 		}
 	}
@@ -3509,11 +3309,8 @@ brcmf_cfg80211_sched_scan_start(struct wiphy *wiphy,
 		}
 
 		/* configure pno */
-		ret = brcmf_dev_pno_config(ndev);
-		if (ret < 0) {
-			brcmf_err("PNO setup failed!! ret=%d\n", ret);
+		if (brcmf_dev_pno_config(ifp, request))
 			return -EINVAL;
-		}
 
 		/* configure each match set */
 		for (i = 0; i < request->n_match_sets; i++) {
@@ -3563,6 +3360,425 @@ static int brcmf_cfg80211_sched_scan_stop(struct wiphy *wiphy,
 	return 0;
 }
 
+static __always_inline void brcmf_delay(u32 ms)
+{
+	if (ms < 1000 / HZ) {
+		cond_resched();
+		mdelay(ms);
+	} else {
+		msleep(ms);
+	}
+}
+
+static s32 brcmf_config_wowl_pattern(struct brcmf_if *ifp, u8 cmd[4],
+				     u8 *pattern, u32 patternsize, u8 *mask,
+				     u32 packet_offset)
+{
+	struct brcmf_fil_wowl_pattern_le *filter;
+	u32 masksize;
+	u32 patternoffset;
+	u8 *buf;
+	u32 bufsize;
+	s32 ret;
+
+	masksize = (patternsize + 7) / 8;
+	patternoffset = sizeof(*filter) - sizeof(filter->cmd) + masksize;
+
+	bufsize = sizeof(*filter) + patternsize + masksize;
+	buf = kzalloc(bufsize, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+	filter = (struct brcmf_fil_wowl_pattern_le *)buf;
+
+	memcpy(filter->cmd, cmd, 4);
+	filter->masksize = cpu_to_le32(masksize);
+	filter->offset = cpu_to_le32(packet_offset);
+	filter->patternoffset = cpu_to_le32(patternoffset);
+	filter->patternsize = cpu_to_le32(patternsize);
+	filter->type = cpu_to_le32(BRCMF_WOWL_PATTERN_TYPE_BITMAP);
+
+	if ((mask) && (masksize))
+		memcpy(buf + sizeof(*filter), mask, masksize);
+	if ((pattern) && (patternsize))
+		memcpy(buf + sizeof(*filter) + masksize, pattern, patternsize);
+
+	ret = brcmf_fil_iovar_data_set(ifp, "wowl_pattern", buf, bufsize);
+
+	kfree(buf);
+	return ret;
+}
+
+static s32
+brcmf_wowl_nd_results(struct brcmf_if *ifp, const struct brcmf_event_msg *e,
+		      void *data)
+{
+	struct brcmf_cfg80211_info *cfg = ifp->drvr->config;
+	struct brcmf_pno_scanresults_le *pfn_result;
+	struct brcmf_pno_net_info_le *netinfo;
+
+	brcmf_dbg(SCAN, "Enter\n");
+
+	pfn_result = (struct brcmf_pno_scanresults_le *)data;
+
+	if (e->event_code == BRCMF_E_PFN_NET_LOST) {
+		brcmf_dbg(SCAN, "PFN NET LOST event. Ignore\n");
+		return 0;
+	}
+
+	if (le32_to_cpu(pfn_result->count) < 1) {
+		brcmf_err("Invalid result count, expected 1 (%d)\n",
+			  le32_to_cpu(pfn_result->count));
+		return -EINVAL;
+	}
+
+	data += sizeof(struct brcmf_pno_scanresults_le);
+	netinfo = (struct brcmf_pno_net_info_le *)data;
+	memcpy(cfg->wowl.nd->ssid.ssid, netinfo->SSID, netinfo->SSID_len);
+	cfg->wowl.nd->ssid.ssid_len = netinfo->SSID_len;
+	cfg->wowl.nd->n_channels = 1;
+	cfg->wowl.nd->channels[0] =
+		ieee80211_channel_to_frequency(netinfo->channel,
+			netinfo->channel <= CH_MAX_2G_CHANNEL ?
+					NL80211_BAND_2GHZ : NL80211_BAND_5GHZ);
+	cfg->wowl.nd_info->n_matches = 1;
+	cfg->wowl.nd_info->matches[0] = cfg->wowl.nd;
+
+	/* Inform (the resume task) that the net detect information was recvd */
+	cfg->wowl.nd_data_completed = true;
+	wake_up(&cfg->wowl.nd_data_wait);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+
+static void brcmf_report_wowl_wakeind(struct wiphy *wiphy, struct brcmf_if *ifp)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_wowl_wakeind_le wake_ind_le;
+	struct cfg80211_wowlan_wakeup wakeup_data;
+	struct cfg80211_wowlan_wakeup *wakeup;
+	u32 wakeind;
+	s32 err;
+	int timeout;
+
+	err = brcmf_fil_iovar_data_get(ifp, "wowl_wakeind", &wake_ind_le,
+				       sizeof(wake_ind_le));
+	if (err) {
+		brcmf_err("Get wowl_wakeind failed, err = %d\n", err);
+		return;
+	}
+
+	wakeind = le32_to_cpu(wake_ind_le.ucode_wakeind);
+	if (wakeind & (BRCMF_WOWL_MAGIC | BRCMF_WOWL_DIS | BRCMF_WOWL_BCN |
+		       BRCMF_WOWL_RETR | BRCMF_WOWL_NET |
+		       BRCMF_WOWL_PFN_FOUND)) {
+		wakeup = &wakeup_data;
+		memset(&wakeup_data, 0, sizeof(wakeup_data));
+		wakeup_data.pattern_idx = -1;
+
+		if (wakeind & BRCMF_WOWL_MAGIC) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_MAGIC\n");
+			wakeup_data.magic_pkt = true;
+		}
+		if (wakeind & BRCMF_WOWL_DIS) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_DIS\n");
+			wakeup_data.disconnect = true;
+		}
+		if (wakeind & BRCMF_WOWL_BCN) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_BCN\n");
+			wakeup_data.disconnect = true;
+		}
+		if (wakeind & BRCMF_WOWL_RETR) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_RETR\n");
+			wakeup_data.disconnect = true;
+		}
+		if (wakeind & BRCMF_WOWL_NET) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_NET\n");
+			/* For now always map to pattern 0, no API to get
+			 * correct information available at the moment.
+			 */
+			wakeup_data.pattern_idx = 0;
+		}
+		if (wakeind & BRCMF_WOWL_PFN_FOUND) {
+			brcmf_dbg(INFO, "WOWL Wake indicator: BRCMF_WOWL_PFN_FOUND\n");
+			timeout = wait_event_timeout(cfg->wowl.nd_data_wait,
+				cfg->wowl.nd_data_completed,
+				BRCMF_ND_INFO_TIMEOUT);
+			if (!timeout)
+				brcmf_err("No result for wowl net detect\n");
+			else
+				wakeup_data.net_detect = cfg->wowl.nd_info;
+		}
+	} else {
+		wakeup = NULL;
+	}
+	cfg80211_report_wowlan_wakeup(&ifp->vif->wdev, wakeup, GFP_KERNEL);
+}
+
+#else
+
+static void brcmf_report_wowl_wakeind(struct wiphy *wiphy, struct brcmf_if *ifp)
+{
+}
+
+#endif /* CONFIG_PM */
+
+static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct net_device *ndev = cfg_to_ndev(cfg);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	if (cfg->wowl.active) {
+		brcmf_report_wowl_wakeind(wiphy, ifp);
+		brcmf_fil_iovar_int_set(ifp, "wowl_clear", 0);
+		brcmf_config_wowl_pattern(ifp, "clr", NULL, 0, NULL, 0);
+		brcmf_configure_arp_offload(ifp, true);
+		brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM,
+				      cfg->wowl.pre_pmmode);
+		cfg->wowl.active = false;
+		if (cfg->wowl.nd_enabled) {
+			brcmf_cfg80211_sched_scan_stop(cfg->wiphy, ifp->ndev);
+			brcmf_fweh_unregister(cfg->pub, BRCMF_E_PFN_NET_FOUND);
+			brcmf_fweh_register(cfg->pub, BRCMF_E_PFN_NET_FOUND,
+					    brcmf_notify_sched_scan_results);
+			cfg->wowl.nd_enabled = false;
+		}
+	}
+	return 0;
+}
+
+static void brcmf_configure_wowl(struct brcmf_cfg80211_info *cfg,
+				 struct brcmf_if *ifp,
+				 struct cfg80211_wowlan *wowl)
+{
+	u32 wowl_config;
+	u32 i;
+
+	brcmf_dbg(TRACE, "Suspend, wowl config.\n");
+
+	brcmf_configure_arp_offload(ifp, false);
+	brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_PM, &cfg->wowl.pre_pmmode);
+	brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM, PM_MAX);
+
+	wowl_config = 0;
+	if (wowl->disconnect)
+		wowl_config = BRCMF_WOWL_DIS | BRCMF_WOWL_BCN | BRCMF_WOWL_RETR;
+	if (wowl->magic_pkt)
+		wowl_config |= BRCMF_WOWL_MAGIC;
+	if ((wowl->patterns) && (wowl->n_patterns)) {
+		wowl_config |= BRCMF_WOWL_NET;
+		for (i = 0; i < wowl->n_patterns; i++) {
+			brcmf_config_wowl_pattern(ifp, "add",
+				(u8 *)wowl->patterns[i].pattern,
+				wowl->patterns[i].pattern_len,
+				(u8 *)wowl->patterns[i].mask,
+				wowl->patterns[i].pkt_offset);
+		}
+	}
+	if (wowl->nd_config) {
+		brcmf_cfg80211_sched_scan_start(cfg->wiphy, ifp->ndev,
+						wowl->nd_config);
+		wowl_config |= BRCMF_WOWL_PFN_FOUND;
+
+		cfg->wowl.nd_data_completed = false;
+		cfg->wowl.nd_enabled = true;
+		/* Now reroute the event for PFN to the wowl function. */
+		brcmf_fweh_unregister(cfg->pub, BRCMF_E_PFN_NET_FOUND);
+		brcmf_fweh_register(cfg->pub, BRCMF_E_PFN_NET_FOUND,
+				    brcmf_wowl_nd_results);
+	}
+	if (!test_bit(BRCMF_VIF_STATUS_CONNECTED, &ifp->vif->sme_state))
+		wowl_config |= BRCMF_WOWL_UNASSOC;
+
+	brcmf_fil_iovar_data_set(ifp, "wowl_wakeind", "clear", strlen("clear"));
+	brcmf_fil_iovar_int_set(ifp, "wowl", wowl_config);
+	brcmf_fil_iovar_int_set(ifp, "wowl_activate", 1);
+	brcmf_bus_wowl_config(cfg->pub->bus_if, true);
+	cfg->wowl.active = true;
+}
+
+static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
+				  struct cfg80211_wowlan *wowl)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct net_device *ndev = cfg_to_ndev(cfg);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_cfg80211_vif *vif;
+
+	brcmf_dbg(TRACE, "Enter\n");
+
+	/* if the primary net_device is not READY there is nothing
+	 * we can do but pray resume goes smoothly.
+	 */
+	if (!check_vif_up(ifp->vif))
+		goto exit;
+
+	/* Stop scheduled scan */
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_PNO))
+		brcmf_cfg80211_sched_scan_stop(wiphy, ndev);
+
+	/* end any scanning */
+	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
+		brcmf_abort_scanning(cfg);
+
+	if (wowl == NULL) {
+		brcmf_bus_wowl_config(cfg->pub->bus_if, false);
+		list_for_each_entry(vif, &cfg->vif_list, list) {
+			if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state))
+				continue;
+			/* While going to suspend if associated with AP
+			 * disassociate from AP to save power while system is
+			 * in suspended state
+			 */
+			brcmf_link_down(vif, WLAN_REASON_UNSPECIFIED);
+			/* Make sure WPA_Supplicant receives all the event
+			 * generated due to DISASSOC call to the fw to keep
+			 * the state fw and WPA_Supplicant state consistent
+			 */
+			brcmf_delay(500);
+		}
+		/* Configure MPC */
+		brcmf_set_mpc(ifp, 1);
+
+	} else {
+		/* Configure WOWL paramaters */
+		brcmf_configure_wowl(cfg, ifp, wowl);
+	}
+
+exit:
+	brcmf_dbg(TRACE, "Exit\n");
+	/* clear any scanning activity */
+	cfg->scan_status = 0;
+	return 0;
+}
+
+static __used s32
+brcmf_update_pmklist(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp)
+{
+	struct brcmf_pmk_list_le *pmk_list;
+	int i;
+	u32 npmk;
+	s32 err;
+
+	pmk_list = &cfg->pmk_list;
+	npmk = le32_to_cpu(pmk_list->npmk);
+
+	brcmf_dbg(CONN, "No of elements %d\n", npmk);
+	for (i = 0; i < npmk; i++)
+		brcmf_dbg(CONN, "PMK[%d]: %pM\n", i, &pmk_list->pmk[i].bssid);
+
+	err = brcmf_fil_iovar_data_set(ifp, "pmkid_info", pmk_list,
+				       sizeof(*pmk_list));
+
+	return err;
+}
+
+static s32
+brcmf_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *ndev,
+			 struct cfg80211_pmksa *pmksa)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_pmksa *pmk = &cfg->pmk_list.pmk[0];
+	s32 err;
+	u32 npmk, i;
+
+	brcmf_dbg(TRACE, "Enter\n");
+	if (!check_vif_up(ifp->vif))
+		return -EIO;
+
+	npmk = le32_to_cpu(cfg->pmk_list.npmk);
+	for (i = 0; i < npmk; i++)
+		if (!memcmp(pmksa->bssid, pmk[i].bssid, ETH_ALEN))
+			break;
+	if (i < BRCMF_MAXPMKID) {
+		memcpy(pmk[i].bssid, pmksa->bssid, ETH_ALEN);
+		memcpy(pmk[i].pmkid, pmksa->pmkid, WLAN_PMKID_LEN);
+		if (i == npmk) {
+			npmk++;
+			cfg->pmk_list.npmk = cpu_to_le32(npmk);
+		}
+	} else {
+		brcmf_err("Too many PMKSA entries cached %d\n", npmk);
+		return -EINVAL;
+	}
+
+	brcmf_dbg(CONN, "set_pmksa - PMK bssid: %pM =\n", pmk[npmk].bssid);
+	for (i = 0; i < WLAN_PMKID_LEN; i += 4)
+		brcmf_dbg(CONN, "%02x %02x %02x %02x\n", pmk[npmk].pmkid[i],
+			  pmk[npmk].pmkid[i + 1], pmk[npmk].pmkid[i + 2],
+			  pmk[npmk].pmkid[i + 3]);
+
+	err = brcmf_update_pmklist(cfg, ifp);
+
+	brcmf_dbg(TRACE, "Exit\n");
+	return err;
+}
+
+static s32
+brcmf_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *ndev,
+			 struct cfg80211_pmksa *pmksa)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_pmksa *pmk = &cfg->pmk_list.pmk[0];
+	s32 err;
+	u32 npmk, i;
+
+	brcmf_dbg(TRACE, "Enter\n");
+	if (!check_vif_up(ifp->vif))
+		return -EIO;
+
+	brcmf_dbg(CONN, "del_pmksa - PMK bssid = %pM\n", &pmksa->bssid);
+
+	npmk = le32_to_cpu(cfg->pmk_list.npmk);
+	for (i = 0; i < npmk; i++)
+		if (!memcmp(&pmksa->bssid, &pmk[i].bssid, ETH_ALEN))
+			break;
+
+	if ((npmk > 0) && (i < npmk)) {
+		for (; i < (npmk - 1); i++) {
+			memcpy(&pmk[i].bssid, &pmk[i + 1].bssid, ETH_ALEN);
+			memcpy(&pmk[i].pmkid, &pmk[i + 1].pmkid,
+			       WLAN_PMKID_LEN);
+		}
+		memset(&pmk[i], 0, sizeof(*pmk));
+		cfg->pmk_list.npmk = cpu_to_le32(npmk - 1);
+	} else {
+		brcmf_err("Cache entry not found\n");
+		return -EINVAL;
+	}
+
+	err = brcmf_update_pmklist(cfg, ifp);
+
+	brcmf_dbg(TRACE, "Exit\n");
+	return err;
+
+}
+
+static s32
+brcmf_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *ndev)
+{
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+	s32 err;
+
+	brcmf_dbg(TRACE, "Enter\n");
+	if (!check_vif_up(ifp->vif))
+		return -EIO;
+
+	memset(&cfg->pmk_list, 0, sizeof(cfg->pmk_list));
+	err = brcmf_update_pmklist(cfg, ifp);
+
+	brcmf_dbg(TRACE, "Exit\n");
+	return err;
+
+}
+
 static s32 brcmf_configure_opensecurity(struct brcmf_if *ifp)
 {
 	s32 err;
@@ -3877,7 +4093,8 @@ s32 brcmf_vif_set_mgmt_ie(struct brcmf_cfg80211_vif *vif, s32 pktflag,
 	ifp = vif->ifp;
 	saved_ie = &vif->saved_ie;
 
-	brcmf_dbg(TRACE, "bssidx %d, pktflag : 0x%02X\n", ifp->bssidx, pktflag);
+	brcmf_dbg(TRACE, "bsscfgidx %d, pktflag : 0x%02X\n", ifp->bsscfgidx,
+		  pktflag);
 	iovar_ie_buf = kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
 	if (!iovar_ie_buf)
 		return -ENOMEM;
@@ -4183,7 +4400,9 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 			}
 		}
 
-		if (dev_role == NL80211_IFTYPE_AP) {
+		if ((dev_role == NL80211_IFTYPE_AP) &&
+		    ((ifp->ifidx == 0) ||
+		     !brcmf_feat_is_enabled(ifp, BRCMF_FEAT_RSDB))) {
 			err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_DOWN, 1);
 			if (err < 0) {
 				brcmf_err("BRCMF_C_DOWN error %d\n", err);
@@ -4239,7 +4458,7 @@ brcmf_cfg80211_start_ap(struct wiphy *wiphy, struct net_device *ndev,
 			brcmf_err("setting ssid failed %d\n", err);
 			goto exit;
 		}
-		bss_enable.bsscfg_idx = cpu_to_le32(ifp->bssidx);
+		bss_enable.bsscfgidx = cpu_to_le32(ifp->bsscfgidx);
 		bss_enable.enable = cpu_to_le32(1);
 		err = brcmf_fil_iovar_data_set(ifp, "bss", &bss_enable,
 					       sizeof(bss_enable));
@@ -4306,7 +4525,7 @@ static int brcmf_cfg80211_stop_ap(struct wiphy *wiphy, struct net_device *ndev)
 		if (err < 0)
 			brcmf_err("BRCMF_C_UP error %d\n", err);
 	} else {
-		bss_enable.bsscfg_idx = cpu_to_le32(ifp->bssidx);
+		bss_enable.bsscfgidx = cpu_to_le32(ifp->bsscfgidx);
 		bss_enable.enable = cpu_to_le32(0);
 		err = brcmf_fil_iovar_data_set(ifp, "bss", &bss_enable,
 					       sizeof(bss_enable));
@@ -4700,7 +4919,6 @@ struct brcmf_cfg80211_vif *brcmf_alloc_vif(struct brcmf_cfg80211_info *cfg,
 	vif->wdev.iftype = type;
 
 	vif->pm_block = pm_block;
-	vif->roam_off = -1;
 
 	brcmf_init_prof(&vif->profile);
 
@@ -5016,9 +5234,9 @@ brcmf_notify_connect_status(struct brcmf_if *ifp,
 	} else if (brcmf_is_linkup(e)) {
 		brcmf_dbg(CONN, "Linkup\n");
 		if (brcmf_is_ibssmode(ifp->vif)) {
+			brcmf_inform_ibss(cfg, ndev, e->addr);
 			chan = ieee80211_get_channel(cfg->wiphy, cfg->channel);
 			memcpy(profile->bssid, e->addr, ETH_ALEN);
-			wl_inform_ibss(cfg, ndev, e->addr);
 			cfg80211_ibss_joined(ndev, e->addr, chan, GFP_KERNEL);
 			clear_bit(BRCMF_VIF_STATUS_CONNECTING,
 				  &ifp->vif->sme_state);
@@ -5031,12 +5249,13 @@ brcmf_notify_connect_status(struct brcmf_if *ifp,
 		brcmf_dbg(CONN, "Linkdown\n");
 		if (!brcmf_is_ibssmode(ifp->vif)) {
 			brcmf_bss_connect_done(cfg, ndev, e, false);
+			brcmf_link_down(ifp->vif,
+					brcmf_map_fw_linkdown_reason(e));
+			brcmf_init_prof(ndev_to_prof(ndev));
+			if (ndev != cfg_to_ndev(cfg))
+				complete(&cfg->vif_disabled);
+			brcmf_net_setcarrier(ifp, false);
 		}
-		brcmf_link_down(ifp->vif, brcmf_map_fw_linkdown_reason(e));
-		brcmf_init_prof(ndev_to_prof(ndev));
-		if (ndev != cfg_to_ndev(cfg))
-			complete(&cfg->vif_disabled);
-		brcmf_net_setcarrier(ifp, false);
 	} else if (brcmf_is_nonetwork(cfg, e)) {
 		if (brcmf_is_ibssmode(ifp->vif))
 			clear_bit(BRCMF_VIF_STATUS_CONNECTING,
@@ -5092,9 +5311,9 @@ static s32 brcmf_notify_vif_event(struct brcmf_if *ifp,
 	struct brcmf_cfg80211_vif_event *event = &cfg->vif_event;
 	struct brcmf_cfg80211_vif *vif;
 
-	brcmf_dbg(TRACE, "Enter: action %u flags %u ifidx %u bsscfg %u\n",
+	brcmf_dbg(TRACE, "Enter: action %u flags %u ifidx %u bsscfgidx %u\n",
 		  ifevent->action, ifevent->flags, ifevent->ifidx,
-		  ifevent->bssidx);
+		  ifevent->bsscfgidx);
 
 	mutex_lock(&event->vif_event_lock);
 	event->action = ifevent->action;
@@ -5144,7 +5363,6 @@ static void brcmf_init_conf(struct brcmf_cfg80211_conf *conf)
 	conf->rts_threshold = (u32)-1;
 	conf->retry_short = (u32)-1;
 	conf->retry_long = (u32)-1;
-	conf->tx_power = -1;
 }
 
 static void brcmf_register_event_handlers(struct brcmf_cfg80211_info *cfg)
@@ -5191,8 +5409,10 @@ static void brcmf_deinit_priv_mem(struct brcmf_cfg80211_info *cfg)
 	cfg->escan_ioctl_buf = NULL;
 	kfree(cfg->extra_buf);
 	cfg->extra_buf = NULL;
-	kfree(cfg->pmk_list);
-	cfg->pmk_list = NULL;
+	kfree(cfg->wowl.nd);
+	cfg->wowl.nd = NULL;
+	kfree(cfg->wowl.nd_info);
+	cfg->wowl.nd_info = NULL;
 }
 
 static s32 brcmf_init_priv_mem(struct brcmf_cfg80211_info *cfg)
@@ -5206,8 +5426,13 @@ static s32 brcmf_init_priv_mem(struct brcmf_cfg80211_info *cfg)
 	cfg->extra_buf = kzalloc(WL_EXTRA_BUF_MAX, GFP_KERNEL);
 	if (!cfg->extra_buf)
 		goto init_priv_mem_out;
-	cfg->pmk_list = kzalloc(sizeof(*cfg->pmk_list), GFP_KERNEL);
-	if (!cfg->pmk_list)
+	cfg->wowl.nd = kzalloc(sizeof(*cfg->wowl.nd) + sizeof(u32), GFP_KERNEL);
+	if (!cfg->wowl.nd)
+		goto init_priv_mem_out;
+	cfg->wowl.nd_info = kzalloc(sizeof(*cfg->wowl.nd_info) +
+				    sizeof(struct cfg80211_wowlan_nd_match *),
+				    GFP_KERNEL);
+	if (!cfg->wowl.nd_info)
 		goto init_priv_mem_out;
 
 	return 0;
@@ -5250,35 +5475,34 @@ static void init_vif_event(struct brcmf_cfg80211_vif_event *event)
 	mutex_init(&event->vif_event_lock);
 }
 
-static s32
-brcmf_dongle_roam(struct brcmf_if *ifp, u32 bcn_timeout)
+static s32 brcmf_dongle_roam(struct brcmf_if *ifp)
 {
-	s32 err = 0;
+	s32 err;
+	u32 bcn_timeout;
 	__le32 roamtrigger[2];
 	__le32 roam_delta[2];
 
-	/*
-	 * Setup timeout if Beacons are lost and roam is
-	 * off to report link down
-	 */
-	if (brcmf_roamoff) {
-		err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout", bcn_timeout);
-		if (err) {
-			brcmf_err("bcn_timeout error (%d)\n", err);
-			goto dongle_rom_out;
-		}
+	/* Configure beacon timeout value based upon roaming setting */
+	if (ifp->drvr->settings->roamoff)
+		bcn_timeout = BRCMF_DEFAULT_BCN_TIMEOUT_ROAM_OFF;
+	else
+		bcn_timeout = BRCMF_DEFAULT_BCN_TIMEOUT_ROAM_ON;
+	err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout", bcn_timeout);
+	if (err) {
+		brcmf_err("bcn_timeout error (%d)\n", err);
+		goto roam_setup_done;
 	}
 
-	/*
-	 * Enable/Disable built-in roaming to allow supplicant
-	 * to take care of roaming
+	/* Enable/Disable built-in roaming to allow supplicant to take care of
+	 * roaming.
 	 */
 	brcmf_dbg(INFO, "Internal Roaming = %s\n",
-		  brcmf_roamoff ? "Off" : "On");
-	err = brcmf_fil_iovar_int_set(ifp, "roam_off", !!(brcmf_roamoff));
+		  ifp->drvr->settings->roamoff ? "Off" : "On");
+	err = brcmf_fil_iovar_int_set(ifp, "roam_off",
+				      ifp->drvr->settings->roamoff);
 	if (err) {
 		brcmf_err("roam_off error (%d)\n", err);
-		goto dongle_rom_out;
+		goto roam_setup_done;
 	}
 
 	roamtrigger[0] = cpu_to_le32(WL_ROAM_TRIGGER_LEVEL);
@@ -5287,7 +5511,7 @@ brcmf_dongle_roam(struct brcmf_if *ifp, u32 bcn_timeout)
 				     (void *)roamtrigger, sizeof(roamtrigger));
 	if (err) {
 		brcmf_err("WLC_SET_ROAM_TRIGGER error (%d)\n", err);
-		goto dongle_rom_out;
+		goto roam_setup_done;
 	}
 
 	roam_delta[0] = cpu_to_le32(WL_ROAM_DELTA);
@@ -5296,45 +5520,35 @@ brcmf_dongle_roam(struct brcmf_if *ifp, u32 bcn_timeout)
 				     (void *)roam_delta, sizeof(roam_delta));
 	if (err) {
 		brcmf_err("WLC_SET_ROAM_DELTA error (%d)\n", err);
-		goto dongle_rom_out;
+		goto roam_setup_done;
 	}
 
-dongle_rom_out:
+roam_setup_done:
 	return err;
 }
 
 static s32
-brcmf_dongle_scantime(struct brcmf_if *ifp, s32 scan_assoc_time,
-		      s32 scan_unassoc_time, s32 scan_passive_time)
+brcmf_dongle_scantime(struct brcmf_if *ifp)
 {
 	s32 err = 0;
 
 	err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_CHANNEL_TIME,
-				    scan_assoc_time);
+				    BRCMF_SCAN_CHANNEL_TIME);
 	if (err) {
-		if (err == -EOPNOTSUPP)
-			brcmf_dbg(INFO, "Scan assoc time is not supported\n");
-		else
-			brcmf_err("Scan assoc time error (%d)\n", err);
+		brcmf_err("Scan assoc time error (%d)\n", err);
 		goto dongle_scantime_out;
 	}
 	err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_UNASSOC_TIME,
-				    scan_unassoc_time);
+				    BRCMF_SCAN_UNASSOC_TIME);
 	if (err) {
-		if (err == -EOPNOTSUPP)
-			brcmf_dbg(INFO, "Scan unassoc time is not supported\n");
-		else
-			brcmf_err("Scan unassoc time error (%d)\n", err);
+		brcmf_err("Scan unassoc time error (%d)\n", err);
 		goto dongle_scantime_out;
 	}
 
 	err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_SCAN_PASSIVE_TIME,
-				    scan_passive_time);
+				    BRCMF_SCAN_PASSIVE_TIME);
 	if (err) {
-		if (err == -EOPNOTSUPP)
-			brcmf_dbg(INFO, "Scan passive time is not supported\n");
-		else
-			brcmf_err("Scan passive time error (%d)\n", err);
+		brcmf_err("Scan passive time error (%d)\n", err);
 		goto dongle_scantime_out;
 	}
 
@@ -5619,7 +5833,8 @@ static __le16 brcmf_get_mcs_map(u32 nchain, enum ieee80211_vht_mcs_support supp)
 }
 
 static void brcmf_update_vht_cap(struct ieee80211_supported_band *band,
-				 u32 bw_cap[2], u32 nchain)
+				 u32 bw_cap[2], u32 nchain, u32 txstreams,
+				 u32 txbf_bfe_cap, u32 txbf_bfr_cap)
 {
 	__le16 mcs_map;
 
@@ -5638,6 +5853,25 @@ static void brcmf_update_vht_cap(struct ieee80211_supported_band *band,
 	mcs_map = brcmf_get_mcs_map(nchain, IEEE80211_VHT_MCS_SUPPORT_0_9);
 	band->vht_cap.vht_mcs.rx_mcs_map = mcs_map;
 	band->vht_cap.vht_mcs.tx_mcs_map = mcs_map;
+
+	/* Beamforming support information */
+	if (txbf_bfe_cap & BRCMF_TXBF_SU_BFE_CAP)
+		band->vht_cap.cap |= IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE;
+	if (txbf_bfe_cap & BRCMF_TXBF_MU_BFE_CAP)
+		band->vht_cap.cap |= IEEE80211_VHT_CAP_MU_BEAMFORMEE_CAPABLE;
+	if (txbf_bfr_cap & BRCMF_TXBF_SU_BFR_CAP)
+		band->vht_cap.cap |= IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE;
+	if (txbf_bfr_cap & BRCMF_TXBF_MU_BFR_CAP)
+		band->vht_cap.cap |= IEEE80211_VHT_CAP_MU_BEAMFORMER_CAPABLE;
+
+	if ((txbf_bfe_cap || txbf_bfr_cap) && (txstreams > 1)) {
+		band->vht_cap.cap |=
+			(2 << IEEE80211_VHT_CAP_BEAMFORMEE_STS_SHIFT);
+		band->vht_cap.cap |= ((txstreams - 1) <<
+				IEEE80211_VHT_CAP_SOUNDING_DIMENSIONS_SHIFT);
+		band->vht_cap.cap |=
+			IEEE80211_VHT_CAP_VHT_LINK_ADAPTATION_VHT_MRQ_MFB;
+	}
 }
 
 static int brcmf_setup_wiphybands(struct wiphy *wiphy)
@@ -5652,6 +5886,9 @@ static int brcmf_setup_wiphybands(struct wiphy *wiphy)
 	int err;
 	s32 i;
 	struct ieee80211_supported_band *band;
+	u32 txstreams = 0;
+	u32 txbf_bfe_cap = 0;
+	u32 txbf_bfr_cap = 0;
 
 	(void)brcmf_fil_iovar_int_get(ifp, "vhtmode", &vhtmode);
 	err = brcmf_fil_iovar_int_get(ifp, "nmode", &nmode);
@@ -5680,6 +5917,14 @@ static int brcmf_setup_wiphybands(struct wiphy *wiphy)
 		return err;
 	}
 
+	if (vhtmode) {
+		(void)brcmf_fil_iovar_int_get(ifp, "txstreams", &txstreams);
+		(void)brcmf_fil_iovar_int_get(ifp, "txbf_bfe_cap",
+					      &txbf_bfe_cap);
+		(void)brcmf_fil_iovar_int_get(ifp, "txbf_bfr_cap",
+					      &txbf_bfr_cap);
+	}
+
 	wiphy = cfg_to_wiphy(cfg);
 	for (i = 0; i < ARRAY_SIZE(wiphy->bands); i++) {
 		band = wiphy->bands[i];
@@ -5689,7 +5934,8 @@ static int brcmf_setup_wiphybands(struct wiphy *wiphy)
 		if (nmode)
 			brcmf_update_ht_cap(band, bw_cap, nchain);
 		if (vhtmode)
-			brcmf_update_vht_cap(band, bw_cap, nchain);
+			brcmf_update_vht_cap(band, bw_cap, nchain, txstreams,
+					     txbf_bfe_cap, txbf_bfr_cap);
 	}
 
 	return 0;
@@ -5864,7 +6110,7 @@ static void brcmf_wiphy_pno_params(struct wiphy *wiphy)
 }
 
 #ifdef CONFIG_PM
-static const struct wiphy_wowlan_support brcmf_wowlan_support = {
+static struct wiphy_wowlan_support brcmf_wowlan_support = {
 	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
 	.n_patterns = BRCMF_WOWL_MAXPATTERNS,
 	.pattern_max_len = BRCMF_WOWL_MAXPATTERNSIZE,
@@ -5873,10 +6119,23 @@ static const struct wiphy_wowlan_support brcmf_wowlan_support = {
 };
 #endif
 
-static void brcmf_wiphy_wowl_params(struct wiphy *wiphy)
+static void brcmf_wiphy_wowl_params(struct wiphy *wiphy, struct brcmf_if *ifp)
 {
 #ifdef CONFIG_PM
-	/* wowl settings */
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	s32 err;
+	u32 wowl_cap;
+
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_PNO)) {
+		err = brcmf_fil_iovar_int_get(ifp, "wowl_cap", &wowl_cap);
+		if (!err) {
+			if (wowl_cap & BRCMF_WOWL_PFN_FOUND) {
+				brcmf_wowlan_support.flags |=
+							WIPHY_WOWLAN_NET_DETECT;
+				init_waitqueue_head(&cfg->wowl.nd_data_wait);
+			}
+		}
+	}
 	wiphy->wowlan = &brcmf_wowlan_support;
 #endif
 }
@@ -5893,7 +6152,7 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 
 	wiphy->max_scan_ssids = WL_NUM_SCAN_MAX;
 	wiphy->max_scan_ie_len = BRCMF_SCAN_IE_LEN_MAX;
-	wiphy->max_num_pmkids = WL_NUM_PMKIDS_MAX;
+	wiphy->max_num_pmkids = BRCMF_MAXPMKID;
 
 	err = brcmf_setup_ifmodes(wiphy, ifp);
 	if (err)
@@ -5922,9 +6181,10 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 	wiphy->n_cipher_suites = ARRAY_SIZE(__wl_cipher_suites);
 	wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT |
 			WIPHY_FLAG_OFFCHAN_TX |
-			WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
-			WIPHY_FLAG_SUPPORTS_TDLS;
-	if (!brcmf_roamoff)
+			WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_TDLS))
+		wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
+	if (!ifp->drvr->settings->roamoff)
 		wiphy->flags |= WIPHY_FLAG_SUPPORTS_FW_ROAM;
 	wiphy->mgmt_stypes = brcmf_txrx_stypes;
 	wiphy->max_remain_on_channel_duration = 5000;
@@ -5936,8 +6196,7 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 	wiphy->n_vendor_commands = BRCMF_VNDR_CMDS_LAST - 1;
 
 	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_WOWL))
-		brcmf_wiphy_wowl_params(wiphy);
-
+		brcmf_wiphy_wowl_params(wiphy, ifp);
 	err = brcmf_fil_cmd_data_get(ifp, BRCMF_C_GET_BANDLIST, &bandlist,
 				     sizeof(bandlist));
 	if (err) {
@@ -6004,8 +6263,7 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
 	/* make sure RF is ready for work */
 	brcmf_fil_cmd_int_set(ifp, BRCMF_C_UP, 0);
 
-	brcmf_dongle_scantime(ifp, WL_SCAN_CHANNEL_TIME,
-			      WL_SCAN_UNASSOC_TIME, WL_SCAN_PASSIVE_TIME);
+	brcmf_dongle_scantime(ifp);
 
 	power_mode = cfg->pwr_save ? PM_FAST : PM_OFF;
 	err = brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM, power_mode);
@@ -6014,7 +6272,7 @@ static s32 brcmf_config_dongle(struct brcmf_cfg80211_info *cfg)
 	brcmf_dbg(INFO, "power save set to %s\n",
 		  (power_mode ? "enabled" : "disabled"));
 
-	err = brcmf_dongle_roam(ifp, WL_BEACON_TIMEOUT);
+	err = brcmf_dongle_roam(ifp);
 	if (err)
 		goto default_conf_out;
 	err = brcmf_cfg80211_change_iface(wdev->wiphy, ndev, wdev->iftype,
@@ -6316,13 +6574,15 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		goto wiphy_unreg_out;
 	}
 
-	err = brcmf_fil_iovar_int_set(ifp, "tdls_enable", 1);
-	if (err) {
-		brcmf_dbg(INFO, "TDLS not enabled (%d)\n", err);
-		wiphy->flags &= ~WIPHY_FLAG_SUPPORTS_TDLS;
-	} else {
-		brcmf_fweh_register(cfg->pub, BRCMF_E_TDLS_PEER_EVENT,
-				    brcmf_notify_tdls_peer_event);
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_TDLS)) {
+		err = brcmf_fil_iovar_int_set(ifp, "tdls_enable", 1);
+		if (err) {
+			brcmf_dbg(INFO, "TDLS not enabled (%d)\n", err);
+			wiphy->flags &= ~WIPHY_FLAG_SUPPORTS_TDLS;
+		} else {
+			brcmf_fweh_register(cfg->pub, BRCMF_E_TDLS_PEER_EVENT,
+					    brcmf_notify_tdls_peer_event);
+		}
 	}
 
 	/* (re-) activate FWEH event handling */
@@ -6332,6 +6592,15 @@ struct brcmf_cfg80211_info *brcmf_cfg80211_attach(struct brcmf_pub *drvr,
 		goto wiphy_unreg_out;
 	}
 
+	/* Fill in some of the advertised nl80211 supported features */
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_SCAN_RANDOM_MAC)) {
+		wiphy->features |= NL80211_FEATURE_SCHED_SCAN_RANDOM_MAC_ADDR;
+#ifdef CONFIG_PM
+		if (wiphy->wowlan->flags & WIPHY_WOWLAN_NET_DETECT)
+			wiphy->features |= NL80211_FEATURE_ND_RANDOM_MAC_ADDR;
+#endif
+	}
+
 	return cfg;
 
 wiphy_unreg_out:
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
index 6a878c8f883f..40efb539ac26 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/cfg80211.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/cfg80211.h
@@ -21,18 +21,12 @@
 #include <brcmu_d11.h>
 
 #define WL_NUM_SCAN_MAX			10
-#define WL_NUM_PMKIDS_MAX		MAXPMKID
 #define WL_TLV_INFO_MAX			1024
 #define WL_BSS_INFO_MAX			2048
 #define WL_ASSOC_INFO_MAX		512	/* assoc related fil max buf */
 #define WL_EXTRA_BUF_MAX		2048
 #define WL_ROAM_TRIGGER_LEVEL		-75
 #define WL_ROAM_DELTA			20
-#define WL_BEACON_TIMEOUT		3
-
-#define WL_SCAN_CHANNEL_TIME		40
-#define WL_SCAN_UNASSOC_TIME		40
-#define WL_SCAN_PASSIVE_TIME		120
 
 #define WL_ESCAN_BUF_SIZE		(1024 * 64)
 #define WL_ESCAN_TIMER_INTERVAL_MS	10000 /* E-Scan timeout */
@@ -77,6 +71,11 @@
 
 #define BRCMF_MAX_DEFAULT_KEYS		4
 
+/* beacon loss timeout defaults */
+#define BRCMF_DEFAULT_BCN_TIMEOUT_ROAM_ON	2
+#define BRCMF_DEFAULT_BCN_TIMEOUT_ROAM_OFF	4
+
+#define BRCMF_VIF_EVENT_TIMEOUT		msecs_to_jiffies(1500)
 
 /**
  * enum brcmf_scan_status - scan engine status
@@ -97,19 +96,6 @@ struct brcmf_cfg80211_conf {
 	u32 rts_threshold;
 	u32 retry_short;
 	u32 retry_long;
-	s32 tx_power;
-	struct ieee80211_channel channel;
-};
-
-/* basic structure of scan request */
-struct brcmf_cfg80211_scan_req {
-	struct brcmf_ssid_le ssid_le;
-};
-
-/* basic structure of information element */
-struct brcmf_cfg80211_ie {
-	u16 offset;
-	u8 buf[WL_TLV_INFO_MAX];
 };
 
 /* security information with currently associated ap */
@@ -124,13 +110,11 @@ struct brcmf_cfg80211_security {
 /**
  * struct brcmf_cfg80211_profile - profile information.
  *
- * @ssid: ssid of associated/associating ap.
  * @bssid: bssid of joined/joining ibss.
  * @sec: security information.
  * @key: key information
  */
 struct brcmf_cfg80211_profile {
-	struct brcmf_ssid ssid;
 	u8 bssid[ETH_ALEN];
 	struct brcmf_cfg80211_security sec;
 	struct brcmf_wsec_key key[BRCMF_MAX_DEFAULT_KEYS];
@@ -180,7 +164,6 @@ struct vif_saved_ie {
  * @ifp: lower layer interface pointer
  * @wdev: wireless device.
  * @profile: profile information.
- * @roam_off: roaming state.
  * @sme_state: SME state using enum brcmf_vif_status bits.
  * @pm_block: power-management blocked.
  * @list: linked list.
@@ -191,7 +174,6 @@ struct brcmf_cfg80211_vif {
 	struct brcmf_if *ifp;
 	struct wireless_dev wdev;
 	struct brcmf_cfg80211_profile profile;
-	s32 roam_off;
 	unsigned long sme_state;
 	bool pm_block;
 	struct vif_saved_ie saved_ie;
@@ -215,12 +197,6 @@ struct brcmf_cfg80211_assoc_ielen_le {
 	__le32 resp_len;
 };
 
-/* wpa2 pmk list */
-struct brcmf_cfg80211_pmk_list {
-	struct pmkid_list pmkids;
-	struct pmkid foo[MAXPMKID - 1];
-};
-
 /* dongle escan state */
 enum wl_escan_state {
 	WL_ESCAN_STATE_IDLE,
@@ -233,88 +209,7 @@ struct escan_info {
 	struct wiphy *wiphy;
 	struct brcmf_if *ifp;
 	s32 (*run)(struct brcmf_cfg80211_info *cfg, struct brcmf_if *ifp,
-		   struct cfg80211_scan_request *request, u16 action);
-};
-
-/**
- * struct brcmf_pno_param_le - PNO scan configuration parameters
- *
- * @version: PNO parameters version.
- * @scan_freq: scan frequency.
- * @lost_network_timeout: #sec. to declare discovered network as lost.
- * @flags: Bit field to control features of PFN such as sort criteria auto
- *	enable switch and background scan.
- * @rssi_margin: Margin to avoid jitter for choosing a PFN based on RSSI sort
- *	criteria.
- * @bestn: number of best networks in each scan.
- * @mscan: number of scans recorded.
- * @repeat: minimum number of scan intervals before scan frequency changes
- *	in adaptive scan.
- * @exp: exponent of 2 for maximum scan interval.
- * @slow_freq: slow scan period.
- */
-struct brcmf_pno_param_le {
-	__le32 version;
-	__le32 scan_freq;
-	__le32 lost_network_timeout;
-	__le16 flags;
-	__le16 rssi_margin;
-	u8 bestn;
-	u8 mscan;
-	u8 repeat;
-	u8 exp;
-	__le32 slow_freq;
-};
-
-/**
- * struct brcmf_pno_net_param_le - scan parameters per preferred network.
- *
- * @ssid: ssid name and its length.
- * @flags: bit2: hidden.
- * @infra: BSS vs IBSS.
- * @auth: Open vs Closed.
- * @wpa_auth: WPA type.
- * @wsec: wsec value.
- */
-struct brcmf_pno_net_param_le {
-	struct brcmf_ssid_le ssid;
-	__le32 flags;
-	__le32 infra;
-	__le32 auth;
-	__le32 wpa_auth;
-	__le32 wsec;
-};
-
-/**
- * struct brcmf_pno_net_info_le - information per found network.
- *
- * @bssid: BSS network identifier.
- * @channel: channel number only.
- * @SSID_len: length of ssid.
- * @SSID: ssid characters.
- * @RSSI: receive signal strength (in dBm).
- * @timestamp: age in seconds.
- */
-struct brcmf_pno_net_info_le {
-	u8 bssid[ETH_ALEN];
-	u8 channel;
-	u8 SSID_len;
-	u8 SSID[32];
-	__le16	RSSI;
-	__le16	timestamp;
-};
-
-/**
- * struct brcmf_pno_scanresults_le - result returned in PNO NET FOUND event.
- *
- * @version: PNO version identifier.
- * @status: indicates completion status of PNO scan.
- * @count: amount of brcmf_pno_net_info_le entries appended.
- */
-struct brcmf_pno_scanresults_le {
-	__le32 version;
-	__le32 status;
-	__le32 count;
+		   struct cfg80211_scan_request *request);
 };
 
 /**
@@ -334,6 +229,27 @@ struct brcmf_cfg80211_vif_event {
 };
 
 /**
+ * struct brcmf_cfg80211_wowl - wowl related information.
+ *
+ * @active: set on suspend, cleared on resume.
+ * @pre_pmmode: firmware PM mode at entering suspend.
+ * @nd: net dectect data.
+ * @nd_info: helper struct to pass to cfg80211.
+ * @nd_data_wait: wait queue to sync net detect data.
+ * @nd_data_completed: completion for net detect data.
+ * @nd_enabled: net detect enabled.
+ */
+struct brcmf_cfg80211_wowl {
+	bool active;
+	u32 pre_pmmode;
+	struct cfg80211_wowlan_nd_match *nd;
+	struct cfg80211_wowlan_nd_info *nd_info;
+	wait_queue_head_t nd_data_wait;
+	bool nd_data_completed;
+	bool nd_enabled;
+};
+
+/**
  * struct brcmf_cfg80211_info - dongle private data of cfg80211 interface
  *
  * @wiphy: wiphy object for cfg80211 interface.
@@ -343,9 +259,7 @@ struct brcmf_cfg80211_vif_event {
  * @scan_request: cfg80211 scan request object.
  * @usr_sync: mainly for dongle up/down synchronization.
  * @bss_list: bss_list holding scanned ap information.
- * @scan_req_int: internal scan request object.
  * @bss_info: bss information for cfg80211 layer.
- * @ie: information element object for internal purpose.
  * @conn_info: association info.
  * @pmk_list: wpa2 pmk list.
  * @scan_status: scan activity on the dongle.
@@ -368,8 +282,7 @@ struct brcmf_cfg80211_vif_event {
  * @vif_list: linked list of vif instances.
  * @vif_cnt: number of vif instances.
  * @vif_event: vif event signalling.
- * @wowl_enabled; set during suspend, is wowl used.
- * @pre_wowl_pmmode: intermediate storage of pm mode during wowl.
+ * @wowl: wowl related information.
  */
 struct brcmf_cfg80211_info {
 	struct wiphy *wiphy;
@@ -378,11 +291,9 @@ struct brcmf_cfg80211_info {
 	struct brcmf_btcoex_info *btcoex;
 	struct cfg80211_scan_request *scan_request;
 	struct mutex usr_sync;
-	struct brcmf_cfg80211_scan_req scan_req_int;
 	struct wl_cfg80211_bss_info *bss_info;
-	struct brcmf_cfg80211_ie ie;
 	struct brcmf_cfg80211_connect_info conn_info;
-	struct brcmf_cfg80211_pmk_list *pmk_list;
+	struct brcmf_pmk_list_le pmk_list;
 	unsigned long scan_status;
 	struct brcmf_pub *pub;
 	u32 channel;
@@ -403,9 +314,8 @@ struct brcmf_cfg80211_info {
 	struct brcmf_cfg80211_vif_event vif_event;
 	struct completion vif_disabled;
 	struct brcmu_d11inf d11inf;
-	bool wowl_enabled;
-	u32 pre_wowl_pmmode;
 	struct brcmf_assoclist_le assoclist;
+	struct brcmf_cfg80211_wowl wowl;
 };
 
 /**
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/chip.c
index f04833db2fd0..82e4382eb177 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/chip.c
@@ -681,6 +681,7 @@ static u32 brcmf_chip_tcm_rambase(struct brcmf_chip_priv *ci)
 	case BRCM_CC_43569_CHIP_ID:
 	case BRCM_CC_43570_CHIP_ID:
 	case BRCM_CC_4358_CHIP_ID:
+	case BRCM_CC_4359_CHIP_ID:
 	case BRCM_CC_43602_CHIP_ID:
 	case BRCM_CC_4371_CHIP_ID:
 		return 0x180000;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/chip.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/chip.h
index f6b5feea23d2..f6b5feea23d2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/chip.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/chip.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/common.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
index fe54844c75e0..cfee477a6eb1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/common.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.c
@@ -17,6 +17,7 @@
 #include <linux/kernel.h>
 #include <linux/string.h>
 #include <linux/netdevice.h>
+#include <linux/module.h>
 #include <brcmu_wifi.h>
 #include <brcmu_utils.h>
 #include "core.h"
@@ -29,13 +30,53 @@
 
 const u8 ALLFFMAC[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
 
-#define BRCMF_DEFAULT_BCN_TIMEOUT	3
 #define BRCMF_DEFAULT_SCAN_CHANNEL_TIME	40
 #define BRCMF_DEFAULT_SCAN_UNASSOC_TIME	40
 
 /* boost value for RSSI_DELTA in preferred join selection */
 #define BRCMF_JOIN_PREF_RSSI_BOOST	8
 
+#define BRCMF_DEFAULT_TXGLOM_SIZE	32  /* max tx frames in glom chain */
+
+static int brcmf_sdiod_txglomsz = BRCMF_DEFAULT_TXGLOM_SIZE;
+module_param_named(txglomsz, brcmf_sdiod_txglomsz, int, 0);
+MODULE_PARM_DESC(txglomsz, "Maximum tx packet chain size [SDIO]");
+
+/* Debug level configuration. See debug.h for bits, sysfs modifiable */
+int brcmf_msg_level;
+module_param_named(debug, brcmf_msg_level, int, S_IRUSR | S_IWUSR);
+MODULE_PARM_DESC(debug, "Level of debug output");
+
+static int brcmf_p2p_enable;
+module_param_named(p2pon, brcmf_p2p_enable, int, 0);
+MODULE_PARM_DESC(p2pon, "Enable legacy p2p management functionality");
+
+static int brcmf_feature_disable;
+module_param_named(feature_disable, brcmf_feature_disable, int, 0);
+MODULE_PARM_DESC(feature_disable, "Disable features");
+
+static char brcmf_firmware_path[BRCMF_FW_ALTPATH_LEN];
+module_param_string(alternative_fw_path, brcmf_firmware_path,
+		    BRCMF_FW_ALTPATH_LEN, S_IRUSR);
+MODULE_PARM_DESC(alternative_fw_path, "Alternative firmware path");
+
+static int brcmf_fcmode;
+module_param_named(fcmode, brcmf_fcmode, int, 0);
+MODULE_PARM_DESC(fcmode, "Mode of firmware signalled flow control");
+
+static int brcmf_roamoff;
+module_param_named(roamoff, brcmf_roamoff, int, S_IRUSR);
+MODULE_PARM_DESC(roamoff, "Do not use internal roaming engine");
+
+#ifdef DEBUG
+/* always succeed brcmf_bus_start() */
+static int brcmf_ignore_probe_fail;
+module_param_named(ignore_probe_fail, brcmf_ignore_probe_fail, int, 0);
+MODULE_PARM_DESC(ignore_probe_fail, "always succeed probe for debugging");
+#endif
+
+struct brcmf_mp_global_t brcmf_mp_global;
+
 int brcmf_c_preinit_dcmds(struct brcmf_if *ifp)
 {
 	s8 eventmask[BRCMF_EVENTING_MASK_LEN];
@@ -107,26 +148,6 @@ int brcmf_c_preinit_dcmds(struct brcmf_if *ifp)
 		goto done;
 	}
 
-	/*
-	 * Setup timeout if Beacons are lost and roam is off to report
-	 * link down
-	 */
-	err = brcmf_fil_iovar_int_set(ifp, "bcn_timeout",
-				      BRCMF_DEFAULT_BCN_TIMEOUT);
-	if (err) {
-		brcmf_err("bcn_timeout error (%d)\n", err);
-		goto done;
-	}
-
-	/* Enable/Disable build-in roaming to allowed ext supplicant to take
-	 * of romaing
-	 */
-	err = brcmf_fil_iovar_int_set(ifp, "roam_off", 1);
-	if (err) {
-		brcmf_err("roam_off error (%d)\n", err);
-		goto done;
-	}
-
 	/* Setup join_pref to select target by RSSI(with boost on 5GHz) */
 	join_pref_params[0].type = BRCMF_JOIN_PREF_RSSI_DELTA;
 	join_pref_params[0].len = 2;
@@ -174,6 +195,9 @@ int brcmf_c_preinit_dcmds(struct brcmf_if *ifp)
 		goto done;
 	}
 
+	/* Enable tx beamforming, errors can be ignored (not supported) */
+	(void)brcmf_fil_iovar_int_set(ifp, "txbf", 1);
+
 	/* do bus specific preinit here */
 	err = brcmf_bus_preinit(ifp->drvr->bus_if);
 done:
@@ -196,3 +220,34 @@ void __brcmf_dbg(u32 level, const char *func, const char *fmt, ...)
 	va_end(args);
 }
 #endif
+
+void brcmf_mp_attach(void)
+{
+	strlcpy(brcmf_mp_global.firmware_path, brcmf_firmware_path,
+		BRCMF_FW_ALTPATH_LEN);
+}
+
+int brcmf_mp_device_attach(struct brcmf_pub *drvr)
+{
+	drvr->settings = kzalloc(sizeof(*drvr->settings), GFP_ATOMIC);
+	if (!drvr->settings) {
+		brcmf_err("Failed to alloca storage space for settings\n");
+		return -ENOMEM;
+	}
+
+	drvr->settings->sdiod_txglomsz = brcmf_sdiod_txglomsz;
+	drvr->settings->p2p_enable = !!brcmf_p2p_enable;
+	drvr->settings->feature_disable = brcmf_feature_disable;
+	drvr->settings->fcmode = brcmf_fcmode;
+	drvr->settings->roamoff = !!brcmf_roamoff;
+#ifdef DEBUG
+	drvr->settings->ignore_probe_fail = !!brcmf_ignore_probe_fail;
+#endif
+	return 0;
+}
+
+void brcmf_mp_device_detach(struct brcmf_pub *drvr)
+{
+	kfree(drvr->settings);
+}
+
diff --git a/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
new file mode 100644
index 000000000000..3b0a63b98e99
--- /dev/null
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/common.h
@@ -0,0 +1,79 @@
+/* Copyright (c) 2014 Broadcom Corporation
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#ifndef BRCMFMAC_COMMON_H
+#define BRCMFMAC_COMMON_H
+
+extern const u8 ALLFFMAC[ETH_ALEN];
+
+#define BRCMF_FW_ALTPATH_LEN			256
+
+/* Definitions for the module global and device specific settings are defined
+ * here. Two structs are used for them. brcmf_mp_global_t and brcmf_mp_device.
+ * The mp_global is instantiated once in a global struct and gets initialized
+ * by the common_attach function which should be called before any other
+ * (module) initiliazation takes place. The device specific settings is part
+ * of the drvr struct and should be initialized on every brcmf_attach.
+ */
+
+/**
+ * struct brcmf_mp_global_t - Global module paramaters.
+ *
+ * @firmware_path: Alternative firmware path.
+ */
+struct brcmf_mp_global_t {
+	char	firmware_path[BRCMF_FW_ALTPATH_LEN];
+};
+
+extern struct brcmf_mp_global_t brcmf_mp_global;
+
+/**
+ * struct brcmf_mp_device - Device module paramaters.
+ *
+ * @sdiod_txglomsz: SDIO txglom size.
+ * @joinboost_5g_rssi: 5g rssi booost for preferred join selection.
+ * @p2p_enable: Legacy P2P0 enable (old wpa_supplicant).
+ * @feature_disable: Feature_disable bitmask.
+ * @fcmode: FWS flow control.
+ * @roamoff: Firmware roaming off?
+ */
+struct brcmf_mp_device {
+	int	sdiod_txglomsz;
+	int	joinboost_5g_rssi;
+	bool	p2p_enable;
+	int	feature_disable;
+	int	fcmode;
+	bool	roamoff;
+	bool	ignore_probe_fail;
+};
+
+void brcmf_mp_attach(void);
+int brcmf_mp_device_attach(struct brcmf_pub *drvr);
+void brcmf_mp_device_detach(struct brcmf_pub *drvr);
+#ifdef DEBUG
+static inline bool brcmf_ignoring_probe_fail(struct brcmf_pub *drvr)
+{
+	return drvr->settings->ignore_probe_fail;
+}
+#else
+static inline bool brcmf_ignoring_probe_fail(struct brcmf_pub *drvr)
+{
+	return false;
+}
+#endif
+
+/* Sets dongle media info (drv_version, mac address). */
+int brcmf_c_preinit_dcmds(struct brcmf_if *ifp);
+
+#endif /* BRCMFMAC_COMMON_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/commonring.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/commonring.c
index 7b0e52195a85..7b0e52195a85 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/commonring.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/commonring.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/commonring.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/commonring.h
index b85033611c8d..b85033611c8d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/commonring.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/commonring.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
index b5ab98ee1445..ed9998b69709 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.c
@@ -17,6 +17,7 @@
 #include <linux/kernel.h>
 #include <linux/etherdevice.h>
 #include <linux/module.h>
+#include <linux/inetdevice.h>
 #include <net/cfg80211.h>
 #include <net/rtnetlink.h>
 #include <brcmu_utils.h>
@@ -39,7 +40,7 @@ MODULE_AUTHOR("Broadcom Corporation");
 MODULE_DESCRIPTION("Broadcom 802.11 wireless LAN fullmac driver.");
 MODULE_LICENSE("Dual BSD/GPL");
 
-#define MAX_WAIT_FOR_8021X_TX		50	/* msecs */
+#define MAX_WAIT_FOR_8021X_TX			msecs_to_jiffies(50)
 
 /* AMPDU rx reordering definitions */
 #define BRCMF_RXREORDER_FLOWID_OFFSET		0
@@ -56,30 +57,13 @@ MODULE_LICENSE("Dual BSD/GPL");
 
 #define BRCMF_BSSIDX_INVALID			-1
 
-/* Error bits */
-int brcmf_msg_level;
-module_param_named(debug, brcmf_msg_level, int, S_IRUSR | S_IWUSR);
-MODULE_PARM_DESC(debug, "level of debug output");
-
-/* P2P0 enable */
-static int brcmf_p2p_enable;
-module_param_named(p2pon, brcmf_p2p_enable, int, 0);
-MODULE_PARM_DESC(p2pon, "enable legacy p2p management functionality");
-
-char *brcmf_ifname(struct brcmf_pub *drvr, int ifidx)
+char *brcmf_ifname(struct brcmf_if *ifp)
 {
-	if (ifidx < 0 || ifidx >= BRCMF_MAX_IFS) {
-		brcmf_err("ifidx %d out of range\n", ifidx);
-		return "<if_bad>";
-	}
-
-	if (drvr->iflist[ifidx] == NULL) {
-		brcmf_err("null i/f %d\n", ifidx);
+	if (!ifp)
 		return "<if_null>";
-	}
 
-	if (drvr->iflist[ifidx]->ndev)
-		return drvr->iflist[ifidx]->ndev->name;
+	if (ifp->ndev)
+		return ifp->ndev->name;
 
 	return "<if_none>";
 }
@@ -87,7 +71,7 @@ char *brcmf_ifname(struct brcmf_pub *drvr, int ifidx)
 struct brcmf_if *brcmf_get_ifp(struct brcmf_pub *drvr, int ifidx)
 {
 	struct brcmf_if *ifp;
-	s32 bssidx;
+	s32 bsscfgidx;
 
 	if (ifidx < 0 || ifidx >= BRCMF_MAX_IFS) {
 		brcmf_err("ifidx %d out of range\n", ifidx);
@@ -95,9 +79,9 @@ struct brcmf_if *brcmf_get_ifp(struct brcmf_pub *drvr, int ifidx)
 	}
 
 	ifp = NULL;
-	bssidx = drvr->if2bss[ifidx];
-	if (bssidx >= 0)
-		ifp = drvr->iflist[bssidx];
+	bsscfgidx = drvr->if2bss[ifidx];
+	if (bsscfgidx >= 0)
+		ifp = drvr->iflist[bsscfgidx];
 
 	return ifp;
 }
@@ -115,7 +99,7 @@ static void _brcmf_set_multicast_list(struct work_struct *work)
 
 	ifp = container_of(work, struct brcmf_if, multicast_work);
 
-	brcmf_dbg(TRACE, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	ndev = ifp->ndev;
 
@@ -175,7 +159,7 @@ _brcmf_set_mac_address(struct work_struct *work)
 
 	ifp = container_of(work, struct brcmf_if, setmacaddr_work);
 
-	brcmf_dbg(TRACE, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	err = brcmf_fil_iovar_data_set(ifp, "cur_etheraddr", ifp->mac_addr,
 				       ETH_ALEN);
@@ -213,7 +197,7 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 	struct brcmf_pub *drvr = ifp->drvr;
 	struct ethhdr *eh = (struct ethhdr *)(skb->data);
 
-	brcmf_dbg(DATA, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(DATA, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	/* Can the device send data? */
 	if (drvr->bus_if->state != BRCMF_BUS_UP) {
@@ -224,27 +208,19 @@ static netdev_tx_t brcmf_netdev_start_xmit(struct sk_buff *skb,
 		goto done;
 	}
 
-	if (!drvr->iflist[ifp->bssidx]) {
-		brcmf_err("bad ifidx %d\n", ifp->bssidx);
-		netif_stop_queue(ndev);
-		dev_kfree_skb(skb);
-		ret = -ENODEV;
-		goto done;
-	}
-
 	/* Make sure there's enough room for any header */
 	if (skb_headroom(skb) < drvr->hdrlen) {
 		struct sk_buff *skb2;
 
 		brcmf_dbg(INFO, "%s: insufficient headroom\n",
-			  brcmf_ifname(drvr, ifp->bssidx));
+			  brcmf_ifname(ifp));
 		drvr->bus_if->tx_realloc++;
 		skb2 = skb_realloc_headroom(skb, drvr->hdrlen);
 		dev_kfree_skb(skb);
 		skb = skb2;
 		if (skb == NULL) {
 			brcmf_err("%s: skb_realloc_headroom failed\n",
-				  brcmf_ifname(drvr, ifp->bssidx));
+				  brcmf_ifname(ifp));
 			ret = -ENOMEM;
 			goto done;
 		}
@@ -282,8 +258,8 @@ void brcmf_txflowblock_if(struct brcmf_if *ifp,
 	if (!ifp || !ifp->ndev)
 		return;
 
-	brcmf_dbg(TRACE, "enter: idx=%d stop=0x%X reason=%d state=%d\n",
-		  ifp->bssidx, ifp->netif_stop, reason, state);
+	brcmf_dbg(TRACE, "enter: bsscfgidx=%d stop=0x%X reason=%d state=%d\n",
+		  ifp->bsscfgidx, ifp->netif_stop, reason, state);
 
 	spin_lock_irqsave(&ifp->netif_stop_lock, flags);
 	if (state) {
@@ -602,7 +578,7 @@ static struct net_device_stats *brcmf_netdev_get_stats(struct net_device *ndev)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
 
-	brcmf_dbg(TRACE, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	return &ifp->stats;
 }
@@ -631,10 +607,12 @@ static int brcmf_netdev_stop(struct net_device *ndev)
 {
 	struct brcmf_if *ifp = netdev_priv(ndev);
 
-	brcmf_dbg(TRACE, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	brcmf_cfg80211_down(ndev);
 
+	brcmf_fil_iovar_data_set(ifp, "arp_hostip_clear", NULL, 0);
+
 	brcmf_net_setcarrier(ifp, false);
 
 	return 0;
@@ -647,7 +625,7 @@ static int brcmf_netdev_open(struct net_device *ndev)
 	struct brcmf_bus *bus_if = drvr->bus_if;
 	u32 toe_ol;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	/* If bus is not ready, can't continue */
 	if (bus_if->state != BRCMF_BUS_UP) {
@@ -689,7 +667,7 @@ int brcmf_net_attach(struct brcmf_if *ifp, bool rtnl_locked)
 	struct net_device *ndev;
 	s32 err;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d mac=%pM\n", ifp->bssidx,
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d mac=%pM\n", ifp->bsscfgidx,
 		  ifp->mac_addr);
 	ndev = ifp->ndev;
 
@@ -721,7 +699,7 @@ int brcmf_net_attach(struct brcmf_if *ifp, bool rtnl_locked)
 	return 0;
 
 fail:
-	drvr->iflist[ifp->bssidx] = NULL;
+	drvr->iflist[ifp->bsscfgidx] = NULL;
 	ndev->netdev_ops = NULL;
 	free_netdev(ndev);
 	return -EBADE;
@@ -739,7 +717,8 @@ void brcmf_net_setcarrier(struct brcmf_if *ifp, bool on)
 {
 	struct net_device *ndev;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d carrier=%d\n", ifp->bssidx, on);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d carrier=%d\n", ifp->bsscfgidx,
+		  on);
 
 	ndev = ifp->ndev;
 	brcmf_txflowblock_if(ifp, BRCMF_NETIF_STOP_REASON_DISCONNECTED, !on);
@@ -786,7 +765,7 @@ static int brcmf_net_p2p_attach(struct brcmf_if *ifp)
 {
 	struct net_device *ndev;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d mac=%pM\n", ifp->bssidx,
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d mac=%pM\n", ifp->bsscfgidx,
 		  ifp->mac_addr);
 	ndev = ifp->ndev;
 
@@ -805,39 +784,40 @@ static int brcmf_net_p2p_attach(struct brcmf_if *ifp)
 	return 0;
 
 fail:
-	ifp->drvr->iflist[ifp->bssidx] = NULL;
+	ifp->drvr->iflist[ifp->bsscfgidx] = NULL;
 	ndev->netdev_ops = NULL;
 	free_netdev(ndev);
 	return -EBADE;
 }
 
-struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx, s32 ifidx,
+struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bsscfgidx, s32 ifidx,
 			      bool is_p2pdev, char *name, u8 *mac_addr)
 {
 	struct brcmf_if *ifp;
 	struct net_device *ndev;
 
-	brcmf_dbg(TRACE, "Enter, idx=%d, ifidx=%d\n", bssidx, ifidx);
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d, ifidx=%d\n", bsscfgidx, ifidx);
 
-	ifp = drvr->iflist[bssidx];
+	ifp = drvr->iflist[bsscfgidx];
 	/*
 	 * Delete the existing interface before overwriting it
 	 * in case we missed the BRCMF_E_IF_DEL event.
 	 */
 	if (ifp) {
-		brcmf_err("ERROR: netdev:%s already exists\n",
-			  ifp->ndev->name);
 		if (ifidx) {
+			brcmf_err("ERROR: netdev:%s already exists\n",
+				  ifp->ndev->name);
 			netif_stop_queue(ifp->ndev);
 			brcmf_net_detach(ifp->ndev);
-			drvr->iflist[bssidx] = NULL;
+			drvr->iflist[bsscfgidx] = NULL;
 		} else {
-			brcmf_err("ignore IF event\n");
+			brcmf_dbg(INFO, "netdev:%s ignore IF event\n",
+				  ifp->ndev->name);
 			return ERR_PTR(-EINVAL);
 		}
 	}
 
-	if (!brcmf_p2p_enable && is_p2pdev) {
+	if (!drvr->settings->p2p_enable && is_p2pdev) {
 		/* this is P2P_DEVICE interface */
 		brcmf_dbg(INFO, "allocate non-netdev interface\n");
 		ifp = kzalloc(sizeof(*ifp), GFP_KERNEL);
@@ -854,15 +834,15 @@ struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx, s32 ifidx,
 		ndev->destructor = brcmf_cfg80211_free_netdev;
 		ifp = netdev_priv(ndev);
 		ifp->ndev = ndev;
-		/* store mapping ifidx to bssidx */
+		/* store mapping ifidx to bsscfgidx */
 		if (drvr->if2bss[ifidx] == BRCMF_BSSIDX_INVALID)
-			drvr->if2bss[ifidx] = bssidx;
+			drvr->if2bss[ifidx] = bsscfgidx;
 	}
 
 	ifp->drvr = drvr;
-	drvr->iflist[bssidx] = ifp;
+	drvr->iflist[bsscfgidx] = ifp;
 	ifp->ifidx = ifidx;
-	ifp->bssidx = bssidx;
+	ifp->bsscfgidx = bsscfgidx;
 
 	init_waitqueue_head(&ifp->pend_8021x_wait);
 	spin_lock_init(&ifp->netif_stop_lock);
@@ -876,21 +856,22 @@ struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx, s32 ifidx,
 	return ifp;
 }
 
-static void brcmf_del_if(struct brcmf_pub *drvr, s32 bssidx)
+static void brcmf_del_if(struct brcmf_pub *drvr, s32 bsscfgidx)
 {
 	struct brcmf_if *ifp;
 
-	ifp = drvr->iflist[bssidx];
-	drvr->iflist[bssidx] = NULL;
+	ifp = drvr->iflist[bsscfgidx];
+	drvr->iflist[bsscfgidx] = NULL;
 	if (!ifp) {
-		brcmf_err("Null interface, idx=%d\n", bssidx);
+		brcmf_err("Null interface, bsscfgidx=%d\n", bsscfgidx);
 		return;
 	}
-	brcmf_dbg(TRACE, "Enter, idx=%d, ifidx=%d\n", bssidx, ifp->ifidx);
-	if (drvr->if2bss[ifp->ifidx] == bssidx)
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d, ifidx=%d\n", bsscfgidx,
+		  ifp->ifidx);
+	if (drvr->if2bss[ifp->ifidx] == bsscfgidx)
 		drvr->if2bss[ifp->ifidx] = BRCMF_BSSIDX_INVALID;
 	if (ifp->ndev) {
-		if (bssidx == 0) {
+		if (bsscfgidx == 0) {
 			if (ifp->ndev->netdev_ops == &brcmf_netdev_ops_pri) {
 				rtnl_lock();
 				brcmf_netdev_stop(ifp->ndev);
@@ -920,12 +901,12 @@ static void brcmf_del_if(struct brcmf_pub *drvr, s32 bssidx)
 
 void brcmf_remove_interface(struct brcmf_if *ifp)
 {
-	if (!ifp || WARN_ON(ifp->drvr->iflist[ifp->bssidx] != ifp))
+	if (!ifp || WARN_ON(ifp->drvr->iflist[ifp->bsscfgidx] != ifp))
 		return;
-	brcmf_dbg(TRACE, "Enter, bssidx=%d, ifidx=%d\n", ifp->bssidx,
+	brcmf_dbg(TRACE, "Enter, bsscfgidx=%d, ifidx=%d\n", ifp->bsscfgidx,
 		  ifp->ifidx);
 	brcmf_fws_del_interface(ifp);
-	brcmf_del_if(ifp->drvr, ifp->bssidx);
+	brcmf_del_if(ifp->drvr, ifp->bsscfgidx);
 }
 
 int brcmf_get_next_free_bsscfgidx(struct brcmf_pub *drvr)
@@ -940,10 +921,10 @@ int brcmf_get_next_free_bsscfgidx(struct brcmf_pub *drvr)
 	highest = 2;
 	for (ifidx = 0; ifidx < BRCMF_MAX_IFS; ifidx++) {
 		if (drvr->iflist[ifidx]) {
-			if (drvr->iflist[ifidx]->bssidx == bsscfgidx)
+			if (drvr->iflist[ifidx]->bsscfgidx == bsscfgidx)
 				bsscfgidx = highest + 1;
-			else if (drvr->iflist[ifidx]->bssidx > highest)
-				highest = drvr->iflist[ifidx]->bssidx;
+			else if (drvr->iflist[ifidx]->bsscfgidx > highest)
+				highest = drvr->iflist[ifidx]->bsscfgidx;
 		} else {
 			available = true;
 		}
@@ -952,6 +933,98 @@ int brcmf_get_next_free_bsscfgidx(struct brcmf_pub *drvr)
 	return available ? bsscfgidx : -ENOMEM;
 }
 
+#ifdef CONFIG_INET
+#define ARPOL_MAX_ENTRIES	8
+static int brcmf_inetaddr_changed(struct notifier_block *nb,
+				  unsigned long action, void *data)
+{
+	struct brcmf_pub *drvr = container_of(nb, struct brcmf_pub,
+					      inetaddr_notifier);
+	struct in_ifaddr *ifa = data;
+	struct net_device *ndev = ifa->ifa_dev->dev;
+	struct brcmf_if *ifp;
+	int idx, i, ret;
+	u32 val;
+	__be32 addr_table[ARPOL_MAX_ENTRIES] = {0};
+
+	/* Find out if the notification is meant for us */
+	for (idx = 0; idx < BRCMF_MAX_IFS; idx++) {
+		ifp = drvr->iflist[idx];
+		if (ifp && ifp->ndev == ndev)
+			break;
+		if (idx == BRCMF_MAX_IFS - 1)
+			return NOTIFY_DONE;
+	}
+
+	/* check if arp offload is supported */
+	ret = brcmf_fil_iovar_int_get(ifp, "arpoe", &val);
+	if (ret)
+		return NOTIFY_OK;
+
+	/* old version only support primary index */
+	ret = brcmf_fil_iovar_int_get(ifp, "arp_version", &val);
+	if (ret)
+		val = 1;
+	if (val == 1)
+		ifp = drvr->iflist[0];
+
+	/* retrieve the table from firmware */
+	ret = brcmf_fil_iovar_data_get(ifp, "arp_hostip", addr_table,
+				       sizeof(addr_table));
+	if (ret) {
+		brcmf_err("fail to get arp ip table err:%d\n", ret);
+		return NOTIFY_OK;
+	}
+
+	for (i = 0; i < ARPOL_MAX_ENTRIES; i++)
+		if (ifa->ifa_address == addr_table[i])
+			break;
+
+	switch (action) {
+	case NETDEV_UP:
+		if (i == ARPOL_MAX_ENTRIES) {
+			brcmf_dbg(TRACE, "add %pI4 to arp table\n",
+				  &ifa->ifa_address);
+			/* set it directly */
+			ret = brcmf_fil_iovar_data_set(ifp, "arp_hostip",
+				&ifa->ifa_address, sizeof(ifa->ifa_address));
+			if (ret)
+				brcmf_err("add arp ip err %d\n", ret);
+		}
+		break;
+	case NETDEV_DOWN:
+		if (i < ARPOL_MAX_ENTRIES) {
+			addr_table[i] = 0;
+			brcmf_dbg(TRACE, "remove %pI4 from arp table\n",
+				  &ifa->ifa_address);
+			/* clear the table in firmware */
+			ret = brcmf_fil_iovar_data_set(ifp, "arp_hostip_clear",
+						       NULL, 0);
+			if (ret) {
+				brcmf_err("fail to clear arp ip table err:%d\n",
+					  ret);
+				return NOTIFY_OK;
+			}
+			for (i = 0; i < ARPOL_MAX_ENTRIES; i++) {
+				if (addr_table[i] != 0) {
+					brcmf_fil_iovar_data_set(ifp,
+						"arp_hostip", &addr_table[i],
+						sizeof(addr_table[i]));
+					if (ret)
+						brcmf_err("add arp ip err %d\n",
+							  ret);
+				}
+			}
+		}
+		break;
+	default:
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+#endif
+
 int brcmf_attach(struct device *dev)
 {
 	struct brcmf_pub *drvr = NULL;
@@ -975,6 +1048,10 @@ int brcmf_attach(struct device *dev)
 	drvr->bus_if = dev_get_drvdata(dev);
 	drvr->bus_if->drvr = drvr;
 
+	/* Initialize device specific settings */
+	if (brcmf_mp_device_attach(drvr))
+		goto fail;
+
 	/* attach debug facilities */
 	brcmf_debug_attach(drvr);
 
@@ -1067,7 +1144,7 @@ int brcmf_bus_start(struct device *dev)
 	brcmf_fws_add_interface(ifp);
 
 	drvr->config = brcmf_cfg80211_attach(drvr, bus_if->dev,
-					     brcmf_p2p_enable);
+					     drvr->settings->p2p_enable);
 	if (drvr->config == NULL) {
 		ret = -ENOMEM;
 		goto fail;
@@ -1075,11 +1152,20 @@ int brcmf_bus_start(struct device *dev)
 
 	ret = brcmf_net_attach(ifp, false);
 
-	if ((!ret) && (brcmf_p2p_enable)) {
+	if ((!ret) && (drvr->settings->p2p_enable)) {
 		p2p_ifp = drvr->iflist[1];
 		if (p2p_ifp)
 			ret = brcmf_net_p2p_attach(p2p_ifp);
 	}
+
+	if (ret)
+		goto fail;
+
+#ifdef CONFIG_INET
+	drvr->inetaddr_notifier.notifier_call = brcmf_inetaddr_changed;
+	ret = register_inetaddr_notifier(&drvr->inetaddr_notifier);
+#endif
+
 fail:
 	if (ret < 0) {
 		brcmf_err("failed: %d\n", ret);
@@ -1095,6 +1181,10 @@ fail:
 			brcmf_net_detach(ifp->ndev);
 		if (p2p_ifp)
 			brcmf_net_detach(p2p_ifp->ndev);
+		drvr->iflist[0] = NULL;
+		drvr->iflist[1] = NULL;
+		if (brcmf_ignoring_probe_fail(drvr))
+			ret = 0;
 		return ret;
 	}
 	return 0;
@@ -1143,6 +1233,10 @@ void brcmf_detach(struct device *dev)
 	if (drvr == NULL)
 		return;
 
+#ifdef CONFIG_INET
+	unregister_inetaddr_notifier(&drvr->inetaddr_notifier);
+#endif
+
 	/* stop firmware event handling */
 	brcmf_fweh_detach(drvr);
 	if (drvr->config)
@@ -1162,6 +1256,8 @@ void brcmf_detach(struct device *dev)
 
 	brcmf_proto_detach(drvr);
 
+	brcmf_mp_device_detach(drvr);
+
 	brcmf_debug_detach(drvr);
 	bus_if->drvr = NULL;
 	kfree(drvr);
@@ -1186,7 +1282,7 @@ int brcmf_netdev_wait_pend8021x(struct brcmf_if *ifp)
 
 	err = wait_event_timeout(ifp->pend_8021x_wait,
 				 !brcmf_get_pend_8021x_cnt(ifp),
-				 msecs_to_jiffies(MAX_WAIT_FOR_8021X_TX));
+				 MAX_WAIT_FOR_8021X_TX);
 
 	WARN_ON(!err);
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/core.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
index 2f9101b2ad34..8f39435f976f 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/core.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/core.h
@@ -69,8 +69,8 @@ struct brcmf_ampdu_rx_reorder {
 
 /* Forward decls for struct brcmf_pub (see below) */
 struct brcmf_proto;	/* device communication protocol info */
-struct brcmf_cfg80211_dev; /* cfg80211 device info */
-struct brcmf_fws_info; /* firmware signalling info */
+struct brcmf_fws_info;	/* firmware signalling info */
+struct brcmf_mp_device;	/* module paramateres, device specific */
 
 /*
  * struct brcmf_rev_info
@@ -141,6 +141,9 @@ struct brcmf_pub {
 #ifdef DEBUG
 	struct dentry *dbgfs_dir;
 #endif
+
+	struct notifier_block inetaddr_notifier;
+	struct brcmf_mp_device *settings;
 };
 
 /* forward declarations */
@@ -174,7 +177,7 @@ enum brcmf_netif_stop_reason {
  * @multicast_work: worker object for multicast provisioning.
  * @fws_desc: interface specific firmware-signalling descriptor.
  * @ifidx: interface index in device firmware.
- * @bssidx: index of bss associated with this interface.
+ * @bsscfgidx: index of bss associated with this interface.
  * @mac_addr: assigned mac address.
  * @netif_stop: bitmap indicates reason why netif queues are stopped.
  * @netif_stop_lock: spinlock for update netif_stop from multiple sources.
@@ -190,7 +193,7 @@ struct brcmf_if {
 	struct work_struct multicast_work;
 	struct brcmf_fws_mac_descriptor *fws_desc;
 	int ifidx;
-	s32 bssidx;
+	s32 bsscfgidx;
 	u8 mac_addr[ETH_ALEN];
 	u8 netif_stop;
 	spinlock_t netif_stop_lock;
@@ -205,10 +208,10 @@ struct brcmf_skb_reorder_data {
 int brcmf_netdev_wait_pend8021x(struct brcmf_if *ifp);
 
 /* Return pointer to interface name */
-char *brcmf_ifname(struct brcmf_pub *drvr, int idx);
+char *brcmf_ifname(struct brcmf_if *ifp);
 struct brcmf_if *brcmf_get_ifp(struct brcmf_pub *drvr, int ifidx);
 int brcmf_net_attach(struct brcmf_if *ifp, bool rtnl_locked);
-struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bssidx, s32 ifidx,
+struct brcmf_if *brcmf_add_if(struct brcmf_pub *drvr, s32 bsscfgidx, s32 ifidx,
 			      bool is_p2pdev, char *name, u8 *mac_addr);
 void brcmf_remove_interface(struct brcmf_if *ifp);
 int brcmf_get_next_free_bsscfgidx(struct brcmf_pub *drvr);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/debug.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.c
index 1299dccc78b4..e64557c35553 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/debug.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.c
@@ -49,7 +49,7 @@ static int brcmf_debug_psm_watchdog_notify(struct brcmf_if *ifp,
 					   const struct brcmf_event_msg *evtmsg,
 					   void *data)
 {
-	brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "enter: bsscfgidx=%d\n", ifp->bsscfgidx);
 
 	return brcmf_debug_create_memdump(ifp->drvr->bus_if, data,
 					  evtmsg->datalen);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/debug.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h
index d0d9676f7f9d..6687812770cc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/debug.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/debug.h
@@ -17,6 +17,8 @@
 #ifndef BRCMFMAC_DEBUG_H
 #define BRCMFMAC_DEBUG_H
 
+#include <linux/net.h>	/* net_ratelimit() */
+
 /* message levels */
 #define BRCMF_TRACE_VAL		0x00000002
 #define BRCMF_INFO_VAL		0x00000004
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
index 44bb30636690..1ffa95f1b8d2 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.c
@@ -18,18 +18,16 @@
 #include <linux/module.h>
 
 #include <brcm_hw_ids.h>
+#include <brcmu_wifi.h>
 #include "core.h"
 #include "bus.h"
 #include "debug.h"
 #include "fwil.h"
+#include "fwil_types.h"
 #include "feature.h"
+#include "common.h"
 
 
-/* Module param feature_disable (global for all devices) */
-static int brcmf_feature_disable;
-module_param_named(feature_disable, brcmf_feature_disable, int, 0);
-MODULE_PARM_DESC(feature_disable, "Disable features");
-
 /*
  * expand feature list to array of feature strings.
  */
@@ -40,6 +38,17 @@ static const char *brcmf_feat_names[] = {
 };
 #undef BRCMF_FEAT_DEF
 
+struct brcmf_feat_fwcap {
+	enum brcmf_feat_id feature;
+	const char * const fwcap_id;
+};
+
+static const struct brcmf_feat_fwcap brcmf_fwcap_map[] = {
+	{ BRCMF_FEAT_MBSS, "mbss" },
+	{ BRCMF_FEAT_MCHAN, "mchan" },
+	{ BRCMF_FEAT_P2P, "p2p" },
+};
+
 #ifdef DEBUG
 /*
  * expand quirk list to array of quirk strings.
@@ -104,44 +113,53 @@ static void brcmf_feat_iovar_int_get(struct brcmf_if *ifp,
 	}
 }
 
-/**
- * brcmf_feat_iovar_int_set() - determine feature through iovar set.
- *
- * @ifp: interface to query.
- * @id: feature id.
- * @name: iovar name.
- */
-static void brcmf_feat_iovar_int_set(struct brcmf_if *ifp,
-				     enum brcmf_feat_id id, char *name, u32 val)
+static void brcmf_feat_firmware_capabilities(struct brcmf_if *ifp)
 {
-	int err;
-
-	err = brcmf_fil_iovar_int_set(ifp, name, val);
-	if (err == 0) {
-		brcmf_dbg(INFO, "enabling feature: %s\n", brcmf_feat_names[id]);
-		ifp->drvr->feat_flags |= BIT(id);
-	} else {
-		brcmf_dbg(TRACE, "%s feature check failed: %d\n",
-			  brcmf_feat_names[id], err);
+	char caps[256];
+	enum brcmf_feat_id id;
+	int i;
+
+	brcmf_fil_iovar_data_get(ifp, "cap", caps, sizeof(caps));
+	brcmf_dbg(INFO, "[ %s]\n", caps);
+
+	for (i = 0; i < ARRAY_SIZE(brcmf_fwcap_map); i++) {
+		if (strnstr(caps, brcmf_fwcap_map[i].fwcap_id, sizeof(caps))) {
+			id = brcmf_fwcap_map[i].feature;
+			brcmf_dbg(INFO, "enabling feature: %s\n",
+				  brcmf_feat_names[id]);
+			ifp->drvr->feat_flags |= BIT(id);
+		}
 	}
 }
 
 void brcmf_feat_attach(struct brcmf_pub *drvr)
 {
 	struct brcmf_if *ifp = brcmf_get_ifp(drvr, 0);
+	struct brcmf_pno_macaddr_le pfn_mac;
+	s32 err;
+
+	brcmf_feat_firmware_capabilities(ifp);
 
-	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_MCHAN, "mchan");
 	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_PNO, "pfn");
 	if (drvr->bus_if->wowl_supported)
 		brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_WOWL, "wowl");
-	if (drvr->bus_if->chip != BRCM_CC_43362_CHIP_ID)
-		brcmf_feat_iovar_int_set(ifp, BRCMF_FEAT_MBSS, "mbss", 0);
-	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_P2P, "p2p");
-
-	if (brcmf_feature_disable) {
+	/* MBSS does not work for 43362 */
+	if (drvr->bus_if->chip == BRCM_CC_43362_CHIP_ID)
+		ifp->drvr->feat_flags &= ~BIT(BRCMF_FEAT_MBSS);
+	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_RSDB, "rsdb_mode");
+	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_TDLS, "tdls_enable");
+
+	pfn_mac.version = BRCMF_PFN_MACADDR_CFG_VER;
+	err = brcmf_fil_iovar_data_get(ifp, "pfn_macaddr", &pfn_mac,
+				       sizeof(pfn_mac));
+	if (!err)
+		ifp->drvr->feat_flags |= BIT(BRCMF_FEAT_SCAN_RANDOM_MAC);
+
+	if (drvr->settings->feature_disable) {
 		brcmf_dbg(INFO, "Features: 0x%02x, disable: 0x%02x\n",
-			  ifp->drvr->feat_flags, brcmf_feature_disable);
-		ifp->drvr->feat_flags &= ~brcmf_feature_disable;
+			  ifp->drvr->feat_flags,
+			  drvr->settings->feature_disable);
+		ifp->drvr->feat_flags &= ~drvr->settings->feature_disable;
 	}
 
 	/* set chip related quirks */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
index 6b381f799f22..2e2479d41337 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/feature.h
@@ -24,13 +24,20 @@
  * PNO: preferred network offload.
  * WOWL: Wake-On-WLAN.
  * P2P: peer-to-peer
+ * RSDB: Real Simultaneous Dual Band
+ * TDLS: Tunneled Direct Link Setup
+ * SCAN_RANDOM_MAC: Random MAC during (net detect) scheduled scan.
  */
 #define BRCMF_FEAT_LIST \
 	BRCMF_FEAT_DEF(MBSS) \
 	BRCMF_FEAT_DEF(MCHAN) \
 	BRCMF_FEAT_DEF(PNO) \
 	BRCMF_FEAT_DEF(WOWL) \
-	BRCMF_FEAT_DEF(P2P)
+	BRCMF_FEAT_DEF(P2P) \
+	BRCMF_FEAT_DEF(RSDB) \
+	BRCMF_FEAT_DEF(TDLS) \
+	BRCMF_FEAT_DEF(SCAN_RANDOM_MAC)
+
 /*
  * Quirks:
  *
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/firmware.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c
index 4248f3c80e78..1365c12b78fc 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/firmware.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.c
@@ -23,15 +23,13 @@
 
 #include "debug.h"
 #include "firmware.h"
+#include "core.h"
+#include "common.h"
 
 #define BRCMF_FW_MAX_NVRAM_SIZE			64000
 #define BRCMF_FW_NVRAM_DEVPATH_LEN		19	/* devpath0=pcie/1/4/ */
 #define BRCMF_FW_NVRAM_PCIEDEV_LEN		10	/* pcie/1/4/ + \0 */
 
-char brcmf_firmware_path[BRCMF_FW_PATH_LEN];
-module_param_string(alternative_fw_path, brcmf_firmware_path,
-		    BRCMF_FW_PATH_LEN, 0440);
-
 enum nvram_parser_state {
 	IDLE,
 	KEY,
@@ -449,8 +447,7 @@ static void brcmf_fw_request_nvram_done(const struct firmware *fw, void *ctx)
 
 	if (raw_nvram)
 		bcm47xx_nvram_release_contents(data);
-	if (fw)
-		release_firmware(fw);
+	release_firmware(fw);
 	if (!nvram && !(fwctx->flags & BRCMF_FW_REQ_NV_OPTIONAL))
 		goto fail;
 
@@ -540,3 +537,45 @@ int brcmf_fw_get_firmwares(struct device *dev, u16 flags,
 					   0);
 }
 
+int brcmf_fw_map_chip_to_name(u32 chip, u32 chiprev,
+			      struct brcmf_firmware_mapping mapping_table[],
+			      u32 table_size, char fw_name[BRCMF_FW_NAME_LEN],
+			      char nvram_name[BRCMF_FW_NAME_LEN])
+{
+	u32 i;
+	char end;
+
+	for (i = 0; i < table_size; i++) {
+		if (mapping_table[i].chipid == chip &&
+		    mapping_table[i].revmask & BIT(chiprev))
+			break;
+	}
+
+	if (i == table_size) {
+		brcmf_err("Unknown chipid %d [%d]\n", chip, chiprev);
+		return -ENODEV;
+	}
+
+	/* check if firmware path is provided by module parameter */
+	if (brcmf_mp_global.firmware_path[0] != '\0') {
+		strlcpy(fw_name, brcmf_mp_global.firmware_path,
+			BRCMF_FW_NAME_LEN);
+		if ((nvram_name) && (mapping_table[i].nvram))
+			strlcpy(nvram_name, brcmf_mp_global.firmware_path,
+				BRCMF_FW_NAME_LEN);
+
+		end = brcmf_mp_global.firmware_path[
+				strlen(brcmf_mp_global.firmware_path) - 1];
+		if (end != '/') {
+			strlcat(fw_name, "/", BRCMF_FW_NAME_LEN);
+			if ((nvram_name) && (mapping_table[i].nvram))
+				strlcat(nvram_name, "/", BRCMF_FW_NAME_LEN);
+		}
+	}
+	strlcat(fw_name, mapping_table[i].fw, BRCMF_FW_NAME_LEN);
+	if ((nvram_name) && (mapping_table[i].nvram))
+		strlcat(nvram_name, mapping_table[i].nvram, BRCMF_FW_NAME_LEN);
+
+	return 0;
+}
+
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/firmware.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h
index 604dd48ab4e0..d3c9f0d52ae3 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/firmware.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/firmware.h
@@ -21,11 +21,50 @@
 #define BRCMF_FW_REQ_FLAGS		0x00F0
 #define  BRCMF_FW_REQ_NV_OPTIONAL	0x0010
 
-#define	BRCMF_FW_PATH_LEN	256
-#define	BRCMF_FW_NAME_LEN	32
+#define	BRCMF_FW_NAME_LEN		320
 
-extern char brcmf_firmware_path[];
+#define BRCMF_FW_DEFAULT_PATH		"brcm/"
 
+/**
+ * struct brcmf_firmware_mapping - Used to map chipid/revmask to firmware
+ *	filename and nvram filename. Each bus type implementation should create
+ *	a table of firmware mappings (using the macros defined below).
+ *
+ * @chipid: ID of chip.
+ * @revmask: bitmask of revisions, e.g. 0x10 means rev 4 only, 0xf means rev 0-3
+ * @fw: name of the firmware file.
+ * @nvram: name of nvram file.
+ */
+struct brcmf_firmware_mapping {
+	u32 chipid;
+	u32 revmask;
+	const char *fw;
+	const char *nvram;
+};
+
+#define BRCMF_FW_NVRAM_DEF(fw_nvram_name, fw, nvram) \
+static const char BRCM_ ## fw_nvram_name ## _FIRMWARE_NAME[] = \
+	BRCMF_FW_DEFAULT_PATH fw; \
+static const char BRCM_ ## fw_nvram_name ## _NVRAM_NAME[] = \
+	BRCMF_FW_DEFAULT_PATH nvram; \
+MODULE_FIRMWARE(BRCMF_FW_DEFAULT_PATH fw);
+
+#define BRCMF_FW_DEF(fw_name, fw) \
+static const char BRCM_ ## fw_name ## _FIRMWARE_NAME[] = \
+	BRCMF_FW_DEFAULT_PATH fw; \
+MODULE_FIRMWARE(BRCMF_FW_DEFAULT_PATH fw) \
+
+#define BRCMF_FW_NVRAM_ENTRY(chipid, mask, name) \
+	{ chipid, mask, \
+	  BRCM_ ## name ## _FIRMWARE_NAME, BRCM_ ## name ## _NVRAM_NAME }
+
+#define BRCMF_FW_ENTRY(chipid, mask, name) \
+	{ chipid, mask, BRCM_ ## name ## _FIRMWARE_NAME, NULL }
+
+int brcmf_fw_map_chip_to_name(u32 chip, u32 chiprev,
+			      struct brcmf_firmware_mapping mapping_table[],
+			      u32 table_size, char fw_name[BRCMF_FW_NAME_LEN],
+			      char nvram_name[BRCMF_FW_NAME_LEN]);
 void brcmf_fw_nvram_free(void *nvram);
 /*
  * Request firmware(s) asynchronously. When the asynchronous request
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/flowring.c
index 2ca783fa50cf..2ca783fa50cf 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/flowring.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/flowring.h
index 95fd1c9675d1..95fd1c9675d1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/flowring.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.c
index 3878b6f6cfce..7b26fb1b437c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.c
@@ -182,8 +182,8 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 	bool is_p2pdev;
 	int err = 0;
 
-	brcmf_dbg(EVENT, "action: %u idx: %u bsscfg: %u flags: %u role: %u\n",
-		  ifevent->action, ifevent->ifidx, ifevent->bssidx,
+	brcmf_dbg(EVENT, "action: %u ifidx: %u bsscfgidx: %u flags: %u role: %u\n",
+		  ifevent->action, ifevent->ifidx, ifevent->bsscfgidx,
 		  ifevent->flags, ifevent->role);
 
 	/* The P2P Device interface event must not be ignored contrary to what
@@ -204,12 +204,12 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 		return;
 	}
 
-	ifp = drvr->iflist[ifevent->bssidx];
+	ifp = drvr->iflist[ifevent->bsscfgidx];
 
 	if (ifevent->action == BRCMF_E_IF_ADD) {
 		brcmf_dbg(EVENT, "adding %s (%pM)\n", emsg->ifname,
 			  emsg->addr);
-		ifp = brcmf_add_if(drvr, ifevent->bssidx, ifevent->ifidx,
+		ifp = brcmf_add_if(drvr, ifevent->bsscfgidx, ifevent->ifidx,
 				   is_p2pdev, emsg->ifname, emsg->addr);
 		if (IS_ERR(ifp))
 			return;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h
index d9a942842382..5e39e2a9e388 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fweh.h
@@ -219,7 +219,7 @@ struct brcmf_if_event {
 	u8 ifidx;
 	u8 action;
 	u8 flags;
-	u8 bssidx;
+	u8 bsscfgidx;
 	u8 role;
 };
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.c
index dcfa0bb149ce..f6a2df94dba7 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.c
@@ -126,7 +126,8 @@ brcmf_fil_cmd_data(struct brcmf_if *ifp, u32 cmd, void *data, u32 len, bool set)
 
 	brcmf_dbg(FIL, "Failed: %s (%d)\n",
 		  brcmf_fil_get_errstr((u32)(-err)), err);
-	return -EBADE;
+
+	return err;
 }
 
 s32
@@ -293,22 +294,22 @@ brcmf_fil_iovar_int_get(struct brcmf_if *ifp, char *name, u32 *data)
 }
 
 static u32
-brcmf_create_bsscfg(s32 bssidx, char *name, char *data, u32 datalen, char *buf,
-		    u32 buflen)
+brcmf_create_bsscfg(s32 bsscfgidx, char *name, char *data, u32 datalen,
+		    char *buf, u32 buflen)
 {
 	const s8 *prefix = "bsscfg:";
 	s8 *p;
 	u32 prefixlen;
 	u32 namelen;
 	u32 iolen;
-	__le32 bssidx_le;
+	__le32 bsscfgidx_le;
 
-	if (bssidx == 0)
+	if (bsscfgidx == 0)
 		return brcmf_create_iovar(name, data, datalen, buf, buflen);
 
 	prefixlen = strlen(prefix);
 	namelen = strlen(name) + 1; /* lengh of iovar  name + null */
-	iolen = prefixlen + namelen + sizeof(bssidx_le) + datalen;
+	iolen = prefixlen + namelen + sizeof(bsscfgidx_le) + datalen;
 
 	if (buflen < iolen) {
 		brcmf_err("buffer is too short\n");
@@ -326,9 +327,9 @@ brcmf_create_bsscfg(s32 bssidx, char *name, char *data, u32 datalen, char *buf,
 	p += namelen;
 
 	/* bss config index as first data */
-	bssidx_le = cpu_to_le32(bssidx);
-	memcpy(p, &bssidx_le, sizeof(bssidx_le));
-	p += sizeof(bssidx_le);
+	bsscfgidx_le = cpu_to_le32(bsscfgidx);
+	memcpy(p, &bsscfgidx_le, sizeof(bsscfgidx_le));
+	p += sizeof(bsscfgidx_le);
 
 	/* parameter buffer follows */
 	if (datalen)
@@ -347,12 +348,12 @@ brcmf_fil_bsscfg_data_set(struct brcmf_if *ifp, char *name,
 
 	mutex_lock(&drvr->proto_block);
 
-	brcmf_dbg(FIL, "ifidx=%d, bssidx=%d, name=%s, len=%d\n", ifp->ifidx,
-		  ifp->bssidx, name, len);
+	brcmf_dbg(FIL, "ifidx=%d, bsscfgidx=%d, name=%s, len=%d\n", ifp->ifidx,
+		  ifp->bsscfgidx, name, len);
 	brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data,
 			   min_t(uint, len, MAX_HEX_DUMP_LEN), "data\n");
 
-	buflen = brcmf_create_bsscfg(ifp->bssidx, name, data, len,
+	buflen = brcmf_create_bsscfg(ifp->bsscfgidx, name, data, len,
 				     drvr->proto_buf, sizeof(drvr->proto_buf));
 	if (buflen) {
 		err = brcmf_fil_cmd_data(ifp, BRCMF_C_SET_VAR, drvr->proto_buf,
@@ -376,7 +377,7 @@ brcmf_fil_bsscfg_data_get(struct brcmf_if *ifp, char *name,
 
 	mutex_lock(&drvr->proto_block);
 
-	buflen = brcmf_create_bsscfg(ifp->bssidx, name, data, len,
+	buflen = brcmf_create_bsscfg(ifp->bsscfgidx, name, data, len,
 				     drvr->proto_buf, sizeof(drvr->proto_buf));
 	if (buflen) {
 		err = brcmf_fil_cmd_data(ifp, BRCMF_C_GET_VAR, drvr->proto_buf,
@@ -387,8 +388,8 @@ brcmf_fil_bsscfg_data_get(struct brcmf_if *ifp, char *name,
 		err = -EPERM;
 		brcmf_err("Creating bsscfg failed\n");
 	}
-	brcmf_dbg(FIL, "ifidx=%d, bssidx=%d, name=%s, len=%d\n", ifp->ifidx,
-		  ifp->bssidx, name, len);
+	brcmf_dbg(FIL, "ifidx=%d, bsscfgidx=%d, name=%s, len=%d\n", ifp->ifidx,
+		  ifp->bsscfgidx, name, len);
 	brcmf_dbg_hex_dump(BRCMF_FIL_ON(), data,
 			   min_t(uint, len, MAX_HEX_DUMP_LEN), "data\n");
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
index b20fc0f82a48..6b72df17744e 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil.h
@@ -70,6 +70,7 @@
 #define BRCMF_C_SET_WSEC			134
 #define BRCMF_C_GET_PHY_NOISE			135
 #define BRCMF_C_GET_BSS_INFO			136
+#define BRCMF_C_GET_GET_PKTCNTS			137
 #define BRCMF_C_GET_BANDLIST			140
 #define BRCMF_C_SET_SCB_TIMEOUT			158
 #define BRCMF_C_GET_ASSOCLIST			159
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
index daa427b46712..1afc2ad83b6c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwil_types.h
@@ -110,6 +110,8 @@
 #define BRCMF_WOWL_UNASSOC		(1 << 24)
 /* Wakeup if received matched secured pattern: */
 #define BRCMF_WOWL_SECURE		(1 << 25)
+/* Wakeup on finding preferred network */
+#define BRCMF_WOWL_PFN_FOUND		(1 << 26)
 /* Link Down indication in WoWL mode: */
 #define BRCMF_WOWL_LINKDOWN		(1 << 31)
 
@@ -121,6 +123,17 @@
 
 #define BRCMF_MAX_ASSOCLIST		128
 
+#define BRCMF_TXBF_SU_BFE_CAP		BIT(0)
+#define BRCMF_TXBF_MU_BFE_CAP		BIT(1)
+#define BRCMF_TXBF_SU_BFR_CAP		BIT(0)
+#define BRCMF_TXBF_MU_BFR_CAP		BIT(1)
+
+#define	BRCMF_MAXPMKID			16	/* max # PMKID cache entries */
+
+#define BRCMF_PFN_MACADDR_CFG_VER	1
+#define BRCMF_PFN_MAC_OUI_ONLY		BIT(0)
+#define BRCMF_PFN_SET_MAC_UNASSOC	BIT(1)
+
 /* join preference types for join_pref iovar */
 enum brcmf_join_pref_types {
 	BRCMF_JOIN_PREF_RSSI = 1,
@@ -170,7 +183,7 @@ struct brcmf_fil_af_params_le {
 };
 
 struct brcmf_fil_bss_enable_le {
-	__le32 bsscfg_idx;
+	__le32 bsscfgidx;
 	__le32 enable;
 };
 
@@ -282,14 +295,9 @@ struct brcm_rateset_le {
 	u8 rates[BRCMF_MAXRATES_IN_SET];
 };
 
-struct brcmf_ssid {
-	u32 SSID_len;
-	unsigned char SSID[32];
-};
-
 struct brcmf_ssid_le {
 	__le32 SSID_len;
-	unsigned char SSID[32];
+	unsigned char SSID[IEEE80211_MAX_SSID_LEN];
 };
 
 struct brcmf_scan_params_le {
@@ -634,4 +642,149 @@ struct brcmf_assoclist_le {
 	u8 mac[BRCMF_MAX_ASSOCLIST][ETH_ALEN];
 };
 
+/**
+ * struct brcmf_wowl_wakeind_le - Wakeup indicators
+ *	Note: note both fields contain same information.
+ *
+ * @pci_wakeind: Whether PCI PMECSR PMEStatus bit was set.
+ * @ucode_wakeind: What wakeup-event indication was set by ucode
+ */
+struct brcmf_wowl_wakeind_le {
+	__le32 pci_wakeind;
+	__le32 ucode_wakeind;
+};
+
+/**
+ * struct brcmf_pmksa - PMK Security Association
+ *
+ * @bssid: The AP's BSSID.
+ * @pmkid: he PMK material itself.
+ */
+struct brcmf_pmksa {
+	u8 bssid[ETH_ALEN];
+	u8 pmkid[WLAN_PMKID_LEN];
+};
+
+/**
+ * struct brcmf_pmk_list_le - List of pmksa's.
+ *
+ * @npmk: Number of pmksa's.
+ * @pmk: PMK SA information.
+ */
+struct brcmf_pmk_list_le {
+	__le32 npmk;
+	struct brcmf_pmksa pmk[BRCMF_MAXPMKID];
+};
+
+/**
+ * struct brcmf_pno_param_le - PNO scan configuration parameters
+ *
+ * @version: PNO parameters version.
+ * @scan_freq: scan frequency.
+ * @lost_network_timeout: #sec. to declare discovered network as lost.
+ * @flags: Bit field to control features of PFN such as sort criteria auto
+ *	enable switch and background scan.
+ * @rssi_margin: Margin to avoid jitter for choosing a PFN based on RSSI sort
+ *	criteria.
+ * @bestn: number of best networks in each scan.
+ * @mscan: number of scans recorded.
+ * @repeat: minimum number of scan intervals before scan frequency changes
+ *	in adaptive scan.
+ * @exp: exponent of 2 for maximum scan interval.
+ * @slow_freq: slow scan period.
+ */
+struct brcmf_pno_param_le {
+	__le32 version;
+	__le32 scan_freq;
+	__le32 lost_network_timeout;
+	__le16 flags;
+	__le16 rssi_margin;
+	u8 bestn;
+	u8 mscan;
+	u8 repeat;
+	u8 exp;
+	__le32 slow_freq;
+};
+
+/**
+ * struct brcmf_pno_net_param_le - scan parameters per preferred network.
+ *
+ * @ssid: ssid name and its length.
+ * @flags: bit2: hidden.
+ * @infra: BSS vs IBSS.
+ * @auth: Open vs Closed.
+ * @wpa_auth: WPA type.
+ * @wsec: wsec value.
+ */
+struct brcmf_pno_net_param_le {
+	struct brcmf_ssid_le ssid;
+	__le32 flags;
+	__le32 infra;
+	__le32 auth;
+	__le32 wpa_auth;
+	__le32 wsec;
+};
+
+/**
+ * struct brcmf_pno_net_info_le - information per found network.
+ *
+ * @bssid: BSS network identifier.
+ * @channel: channel number only.
+ * @SSID_len: length of ssid.
+ * @SSID: ssid characters.
+ * @RSSI: receive signal strength (in dBm).
+ * @timestamp: age in seconds.
+ */
+struct brcmf_pno_net_info_le {
+	u8 bssid[ETH_ALEN];
+	u8 channel;
+	u8 SSID_len;
+	u8 SSID[32];
+	__le16	RSSI;
+	__le16	timestamp;
+};
+
+/**
+ * struct brcmf_pno_scanresults_le - result returned in PNO NET FOUND event.
+ *
+ * @version: PNO version identifier.
+ * @status: indicates completion status of PNO scan.
+ * @count: amount of brcmf_pno_net_info_le entries appended.
+ */
+struct brcmf_pno_scanresults_le {
+	__le32 version;
+	__le32 status;
+	__le32 count;
+};
+
+/**
+ * struct brcmf_pno_macaddr_le - to configure PNO macaddr randomization.
+ *
+ * @version: PNO version identifier.
+ * @flags: Flags defining how mac addrss should be used.
+ * @mac: MAC address.
+ */
+struct brcmf_pno_macaddr_le {
+	u8 version;
+	u8 flags;
+	u8 mac[ETH_ALEN];
+};
+
+/**
+ * struct brcmf_pktcnt_le - packet counters.
+ *
+ * @rx_good_pkt: packets (MSDUs & MMPDUs) received from this station
+ * @rx_bad_pkt: failed rx packets
+ * @tx_good_pkt: packets (MSDUs & MMPDUs) transmitted to this station
+ * @tx_bad_pkt: failed tx packets
+ * @rx_ocast_good_pkt: unicast packets destined for others
+ */
+struct brcmf_pktcnt_le {
+	__le32 rx_good_pkt;
+	__le32 rx_bad_pkt;
+	__le32 tx_good_pkt;
+	__le32 tx_bad_pkt;
+	__le32 rx_ocast_good_pkt;
+};
+
 #endif /* FWIL_TYPES_H_ */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c
index 086cac3f86d6..f82c9ab5480b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.c
@@ -36,6 +36,7 @@
 #include "p2p.h"
 #include "cfg80211.h"
 #include "proto.h"
+#include "common.h"
 
 /**
  * DOC: Firmware Signalling
@@ -521,10 +522,6 @@ static const int brcmf_fws_prio2fifo[] = {
 	BRCMF_FWS_FIFO_AC_VO
 };
 
-static int fcmode;
-module_param(fcmode, int, S_IRUSR);
-MODULE_PARM_DESC(fcmode, "mode of firmware signalled flow control");
-
 #define BRCMF_FWS_TLV_DEF(name, id, len) \
 	case BRCMF_FWS_TYPE_ ## name: \
 		return len;
@@ -719,7 +716,7 @@ static void brcmf_fws_macdesc_init(struct brcmf_fws_mac_descriptor *desc,
 	desc->state = BRCMF_FWS_STATE_OPEN;
 	desc->requested_credit = 0;
 	desc->requested_packet = 0;
-	/* depending on use may need ifp->bssidx instead */
+	/* depending on use may need ifp->bsscfgidx instead */
 	desc->interface_id = ifidx;
 	desc->ac_bitmap = 0xff; /* update this when handling APSD */
 	if (addr)
@@ -1609,10 +1606,11 @@ static int brcmf_fws_notify_bcmc_credit_support(struct brcmf_if *ifp,
 {
 	struct brcmf_fws_info *fws = ifp->drvr->fws;
 
-	brcmf_fws_lock(fws);
-	if (fws)
+	if (fws) {
+		brcmf_fws_lock(fws);
 		fws->bcmc_credit_check = true;
-	brcmf_fws_unlock(fws);
+		brcmf_fws_unlock(fws);
+	}
 	return 0;
 }
 
@@ -1938,7 +1936,7 @@ void brcmf_fws_reset_interface(struct brcmf_if *ifp)
 {
 	struct brcmf_fws_mac_descriptor *entry = ifp->fws_desc;
 
-	brcmf_dbg(TRACE, "enter: idx=%d\n", ifp->bssidx);
+	brcmf_dbg(TRACE, "enter: bsscfgidx=%d\n", ifp->bsscfgidx);
 	if (!entry)
 		return;
 
@@ -2133,10 +2131,10 @@ int brcmf_fws_init(struct brcmf_pub *drvr)
 
 	/* set linkage back */
 	fws->drvr = drvr;
-	fws->fcmode = fcmode;
+	fws->fcmode = drvr->settings->fcmode;
 
 	if ((drvr->bus_if->always_use_fws_queue == false) &&
-	    (fcmode == BRCMF_FWS_FCMODE_NONE)) {
+	    (fws->fcmode == BRCMF_FWS_FCMODE_NONE)) {
 		fws->avoid_queueing = true;
 		brcmf_dbg(INFO, "FWS queueing will be avoided\n");
 		return 0;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.h
index a36bac17eafd..a36bac17eafd 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/fwsignal.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/msgbuf.c
index 44e618f9d890..c2bdb91746cf 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/msgbuf.c
@@ -34,7 +34,7 @@
 #include "tracepoint.h"
 
 
-#define MSGBUF_IOCTL_RESP_TIMEOUT		2000
+#define MSGBUF_IOCTL_RESP_TIMEOUT		msecs_to_jiffies(2000)
 
 #define MSGBUF_TYPE_GEN_STATUS			0x1
 #define MSGBUF_TYPE_RING_STATUS			0x2
@@ -466,15 +466,14 @@ static int brcmf_msgbuf_ioctl_resp_wait(struct brcmf_msgbuf *msgbuf)
 {
 	return wait_event_timeout(msgbuf->ioctl_resp_wait,
 				  msgbuf->ctl_completed,
-				  msecs_to_jiffies(MSGBUF_IOCTL_RESP_TIMEOUT));
+				  MSGBUF_IOCTL_RESP_TIMEOUT);
 }
 
 
 static void brcmf_msgbuf_ioctl_resp_wake(struct brcmf_msgbuf *msgbuf)
 {
 	msgbuf->ctl_completed = true;
-	if (waitqueue_active(&msgbuf->ioctl_resp_wait))
-		wake_up(&msgbuf->ioctl_resp_wait);
+	wake_up(&msgbuf->ioctl_resp_wait);
 }
 
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/msgbuf.h
index 3d513e407e3d..3d513e407e3d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/msgbuf.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/of.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/of.c
index 03f35e0c52ca..03f35e0c52ca 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/of.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/of.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/of.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/of.h
index 5f7c3550deda..5f7c3550deda 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/of.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/of.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c
index d224b3dd72ed..821b6494f9d1 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.c
@@ -71,10 +71,10 @@
 #define P2P_AF_MED_DWELL_TIME		400
 #define P2P_AF_LONG_DWELL_TIME		1000
 #define P2P_AF_TX_MAX_RETRY		1
-#define P2P_AF_MAX_WAIT_TIME		2000
+#define P2P_AF_MAX_WAIT_TIME		msecs_to_jiffies(2000)
 #define P2P_INVALID_CHANNEL		-1
 #define P2P_CHANNEL_SYNC_RETRY		5
-#define P2P_AF_FRM_SCAN_MAX_WAIT	1500
+#define P2P_AF_FRM_SCAN_MAX_WAIT	msecs_to_jiffies(1500)
 #define P2P_DEFAULT_SLEEP_TIME_VSDB	200
 
 /* WiFi P2P Public Action Frame OUI Subtypes */
@@ -102,6 +102,7 @@
 #define P2PSD_ACTION_ID_GAS_CREQ	0x0c	/* GAS Comback Request AF */
 #define P2PSD_ACTION_ID_GAS_CRESP	0x0d	/* GAS Comback Response AF */
 
+#define BRCMF_P2P_DISABLE_TIMEOUT	msecs_to_jiffies(500)
 /**
  * struct brcmf_p2p_disc_st_le - set discovery state in firmware.
  *
@@ -625,11 +626,10 @@ exit:
  * @num_chans: number of channels to scan.
  * @chanspecs: channel parameters for @num_chans channels.
  * @search_state: P2P discover state to use.
- * @action: scan action to pass to firmware.
  * @bss_type: type of P2P bss.
  */
 static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
-			   u16 chanspecs[], s32 search_state, u16 action,
+			   u16 chanspecs[], s32 search_state,
 			   enum p2p_bss_type bss_type)
 {
 	s32 ret = 0;
@@ -642,7 +642,6 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 	struct brcmf_cfg80211_vif *vif;
 	struct brcmf_p2p_scan_le *p2p_params;
 	struct brcmf_scan_params_le *sparams;
-	struct brcmf_ssid ssid;
 
 	memsize += num_chans * sizeof(__le16);
 	memblk = kzalloc(memsize, GFP_KERNEL);
@@ -655,16 +654,16 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 		ret = -EINVAL;
 		goto exit;
 	}
+	p2p_params = (struct brcmf_p2p_scan_le *)memblk;
+	sparams = &p2p_params->eparams.params_le;
 
 	switch (search_state) {
 	case WL_P2P_DISC_ST_SEARCH:
 		/*
 		 * If we in SEARCH STATE, we don't need to set SSID explictly
-		 * because dongle use P2P WILDCARD internally by default
+		 * because dongle use P2P WILDCARD internally by default, use
+		 * null ssid, which it is already due to kzalloc.
 		 */
-		/* use null ssid */
-		ssid.SSID_len = 0;
-		memset(ssid.SSID, 0, sizeof(ssid.SSID));
 		break;
 	case WL_P2P_DISC_ST_SCAN:
 		/*
@@ -673,8 +672,10 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 		 * P2P WILDCARD because we just do broadcast scan unless
 		 * setting SSID.
 		 */
-		ssid.SSID_len = BRCMF_P2P_WILDCARD_SSID_LEN;
-		memcpy(ssid.SSID, BRCMF_P2P_WILDCARD_SSID, ssid.SSID_len);
+		sparams->ssid_le.SSID_len =
+				cpu_to_le32(BRCMF_P2P_WILDCARD_SSID_LEN);
+		memcpy(sparams->ssid_le.SSID, BRCMF_P2P_WILDCARD_SSID,
+		       BRCMF_P2P_WILDCARD_SSID_LEN);
 		break;
 	default:
 		brcmf_err(" invalid search state %d\n", search_state);
@@ -687,11 +688,9 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 	/*
 	 * set p2p scan parameters.
 	 */
-	p2p_params = (struct brcmf_p2p_scan_le *)memblk;
 	p2p_params->type = 'E';
 
 	/* determine the scan engine parameters */
-	sparams = &p2p_params->eparams.params_le;
 	sparams->bss_type = DOT11_BSSTYPE_ANY;
 	if (p2p->cfg->active_scan)
 		sparams->scan_type = 0;
@@ -699,9 +698,6 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 		sparams->scan_type = 1;
 
 	eth_broadcast_addr(sparams->bssid);
-	if (ssid.SSID_len)
-		memcpy(sparams->ssid_le.SSID, ssid.SSID, ssid.SSID_len);
-	sparams->ssid_le.SSID_len = cpu_to_le32(ssid.SSID_len);
 	sparams->home_time = cpu_to_le32(P2PAPI_SCAN_HOME_TIME_MS);
 
 	/*
@@ -742,7 +738,7 @@ static s32 brcmf_p2p_escan(struct brcmf_p2p_info *p2p, u32 num_chans,
 
 	/* set the escan specific parameters */
 	p2p_params->eparams.version = cpu_to_le32(BRCMF_ESCAN_REQ_VERSION);
-	p2p_params->eparams.action =  cpu_to_le16(action);
+	p2p_params->eparams.action =  cpu_to_le16(WL_ESCAN_ACTION_START);
 	p2p_params->eparams.sync_id = cpu_to_le16(0x1234);
 	/* perform p2p scan on primary device */
 	ret = brcmf_fil_bsscfg_data_set(vif->ifp, "p2p_scan", memblk, memsize);
@@ -766,8 +762,7 @@ exit:
  */
 static s32 brcmf_p2p_run_escan(struct brcmf_cfg80211_info *cfg,
 			       struct brcmf_if *ifp,
-			       struct cfg80211_scan_request *request,
-			       u16 action)
+			       struct cfg80211_scan_request *request)
 {
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
 	s32 err = 0;
@@ -827,7 +822,7 @@ static s32 brcmf_p2p_run_escan(struct brcmf_cfg80211_info *cfg,
 			num_nodfs++;
 		}
 		err = brcmf_p2p_escan(p2p, num_nodfs, chanspecs, search_state,
-				      action, P2PAPI_BSSCFG_DEVICE);
+				      P2PAPI_BSSCFG_DEVICE);
 		kfree(chanspecs);
 	}
 exit:
@@ -1096,8 +1091,7 @@ static s32 brcmf_p2p_act_frm_search(struct brcmf_p2p_info *p2p, u16 channel)
 		default_chan_list[2] = ch.chspec;
 	}
 	err = brcmf_p2p_escan(p2p, channel_cnt, default_chan_list,
-			      WL_P2P_DISC_ST_SEARCH, WL_ESCAN_ACTION_START,
-			      P2PAPI_BSSCFG_DEVICE);
+			      WL_P2P_DISC_ST_SEARCH, P2PAPI_BSSCFG_DEVICE);
 	kfree(default_chan_list);
 exit:
 	return err;
@@ -1521,7 +1515,7 @@ static s32 brcmf_p2p_tx_action_frame(struct brcmf_p2p_info *p2p,
 	p2p->af_tx_sent_jiffies = jiffies;
 
 	timeout = wait_for_completion_timeout(&p2p->send_af_done,
-					msecs_to_jiffies(P2P_AF_MAX_WAIT_TIME));
+					      P2P_AF_MAX_WAIT_TIME);
 
 	if (test_bit(BRCMF_P2P_STATUS_ACTION_TX_COMPLETED, &p2p->status)) {
 		brcmf_dbg(TRACE, "TX action frame operation is success\n");
@@ -1995,7 +1989,7 @@ int brcmf_p2p_ifchange(struct brcmf_cfg80211_info *cfg,
 		return err;
 	}
 	err = brcmf_cfg80211_wait_vif_event_timeout(cfg, BRCMF_E_IF_CHANGE,
-						    msecs_to_jiffies(1500));
+						    BRCMF_VIF_EVENT_TIMEOUT);
 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
 	if (!err)  {
 		brcmf_err("No BRCMF_E_IF_CHANGE event received\n");
@@ -2067,7 +2061,7 @@ static struct wireless_dev *brcmf_p2p_create_p2pdev(struct brcmf_p2p_info *p2p,
 	struct brcmf_if *p2p_ifp;
 	struct brcmf_if *pri_ifp;
 	int err;
-	u32 bssidx;
+	u32 bsscfgidx;
 
 	if (p2p->bss_idx[P2PAPI_BSSCFG_DEVICE].vif)
 		return ERR_PTR(-ENOSPC);
@@ -2097,7 +2091,7 @@ static struct wireless_dev *brcmf_p2p_create_p2pdev(struct brcmf_p2p_info *p2p,
 
 	/* wait for firmware event */
 	err = brcmf_cfg80211_wait_vif_event_timeout(p2p->cfg, BRCMF_E_IF_ADD,
-						    msecs_to_jiffies(1500));
+						    BRCMF_VIF_EVENT_TIMEOUT);
 	brcmf_cfg80211_arm_vif_event(p2p->cfg, NULL);
 	brcmf_fweh_p2pdev_setup(pri_ifp, false);
 	if (!err) {
@@ -2113,13 +2107,13 @@ static struct wireless_dev *brcmf_p2p_create_p2pdev(struct brcmf_p2p_info *p2p,
 	memcpy(&p2p_vif->wdev.address, p2p->dev_addr, sizeof(p2p->dev_addr));
 
 	/* verify bsscfg index for P2P discovery */
-	err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bssidx);
+	err = brcmf_fil_iovar_int_get(pri_ifp, "p2p_dev", &bsscfgidx);
 	if (err < 0) {
 		brcmf_err("retrieving discover bsscfg index failed\n");
 		goto fail;
 	}
 
-	WARN_ON(p2p_ifp->bssidx != bssidx);
+	WARN_ON(p2p_ifp->bsscfgidx != bsscfgidx);
 
 	init_completion(&p2p->send_af_done);
 	INIT_WORK(&p2p->afx_hdl.afx_work, brcmf_p2p_afx_handler);
@@ -2187,7 +2181,7 @@ struct wireless_dev *brcmf_p2p_add_vif(struct wiphy *wiphy, const char *name,
 
 	/* wait for firmware event */
 	err = brcmf_cfg80211_wait_vif_event_timeout(cfg, BRCMF_E_IF_ADD,
-						    msecs_to_jiffies(1500));
+						    BRCMF_VIF_EVENT_TIMEOUT);
 	brcmf_cfg80211_arm_vif_event(cfg, NULL);
 	if (!err) {
 		brcmf_err("timeout occurred\n");
@@ -2237,7 +2231,6 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 	struct brcmf_cfg80211_info *cfg = wiphy_priv(wiphy);
 	struct brcmf_p2p_info *p2p = &cfg->p2p;
 	struct brcmf_cfg80211_vif *vif;
-	unsigned long jiffie_timeout = msecs_to_jiffies(1500);
 	bool wait_for_disable = false;
 	int err;
 
@@ -2270,7 +2263,7 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 
 	if (wait_for_disable)
 		wait_for_completion_timeout(&cfg->vif_disabled,
-					    msecs_to_jiffies(500));
+					    BRCMF_P2P_DISABLE_TIMEOUT);
 
 	err = 0;
 	if (vif->wdev.iftype != NL80211_IFTYPE_P2P_DEVICE) {
@@ -2280,7 +2273,7 @@ int brcmf_p2p_del_vif(struct wiphy *wiphy, struct wireless_dev *wdev)
 	if (!err) {
 		/* wait for firmware event */
 		err = brcmf_cfg80211_wait_vif_event_timeout(cfg, BRCMF_E_IF_DEL,
-							    jiffie_timeout);
+							BRCMF_VIF_EVENT_TIMEOUT);
 		if (!err)
 			err = -EIO;
 		else
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.h
index 5d49059021a9..a3bd18c2360b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/p2p.h
@@ -112,7 +112,6 @@ struct afx_hdl {
  * @int_addr: P2P interface address.
  * @bss_idx: informate for P2P bss types.
  * @listen_timer: timer for @WL_P2P_DISC_ST_LISTEN discover state.
- * @ssid: ssid for P2P GO.
  * @listen_channel: channel for @WL_P2P_DISC_ST_LISTEN discover state.
  * @remain_on_channel: contains copy of struct used by cfg80211.
  * @remain_on_channel_cookie: cookie counter for remain on channel cmd
@@ -133,7 +132,6 @@ struct brcmf_p2p_info {
 	u8 int_addr[ETH_ALEN];
 	struct p2p_bss bss_idx[P2PAPI_BSSCFG_MAX];
 	struct timer_list listen_timer;
-	struct brcmf_ssid ssid;
 	u8 listen_channel;
 	struct ieee80211_channel remain_on_channel;
 	u32 remain_on_channel_cookie;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c
index 83d804221715..0480b70e3eb8 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.c
@@ -44,23 +44,31 @@ enum brcmf_pcie_state {
 	BRCMFMAC_PCIE_STATE_UP
 };
 
-
-#define BRCMF_PCIE_43602_FW_NAME		"brcm/brcmfmac43602-pcie.bin"
-#define BRCMF_PCIE_43602_NVRAM_NAME		"brcm/brcmfmac43602-pcie.txt"
-#define BRCMF_PCIE_4350_FW_NAME			"brcm/brcmfmac4350-pcie.bin"
-#define BRCMF_PCIE_4350_NVRAM_NAME		"brcm/brcmfmac4350-pcie.txt"
-#define BRCMF_PCIE_4356_FW_NAME			"brcm/brcmfmac4356-pcie.bin"
-#define BRCMF_PCIE_4356_NVRAM_NAME		"brcm/brcmfmac4356-pcie.txt"
-#define BRCMF_PCIE_43570_FW_NAME		"brcm/brcmfmac43570-pcie.bin"
-#define BRCMF_PCIE_43570_NVRAM_NAME		"brcm/brcmfmac43570-pcie.txt"
-#define BRCMF_PCIE_4358_FW_NAME			"brcm/brcmfmac4358-pcie.bin"
-#define BRCMF_PCIE_4358_NVRAM_NAME		"brcm/brcmfmac4358-pcie.txt"
-#define BRCMF_PCIE_4365_FW_NAME			"brcm/brcmfmac4365b-pcie.bin"
-#define BRCMF_PCIE_4365_NVRAM_NAME		"brcm/brcmfmac4365b-pcie.txt"
-#define BRCMF_PCIE_4366_FW_NAME			"brcm/brcmfmac4366b-pcie.bin"
-#define BRCMF_PCIE_4366_NVRAM_NAME		"brcm/brcmfmac4366b-pcie.txt"
-#define BRCMF_PCIE_4371_FW_NAME			"brcm/brcmfmac4371-pcie.bin"
-#define BRCMF_PCIE_4371_NVRAM_NAME		"brcm/brcmfmac4371-pcie.txt"
+BRCMF_FW_NVRAM_DEF(43602, "brcmfmac43602-pcie.bin", "brcmfmac43602-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4350, "brcmfmac4350-pcie.bin", "brcmfmac4350-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4350C, "brcmfmac4350c2-pcie.bin", "brcmfmac4350c2-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4356, "brcmfmac4356-pcie.bin", "brcmfmac4356-pcie.txt");
+BRCMF_FW_NVRAM_DEF(43570, "brcmfmac43570-pcie.bin", "brcmfmac43570-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4358, "brcmfmac4358-pcie.bin", "brcmfmac4358-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4359, "brcmfmac4359-pcie.bin", "brcmfmac4359-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4365B, "brcmfmac4365b-pcie.bin", "brcmfmac4365b-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4366B, "brcmfmac4366b-pcie.bin", "brcmfmac4366b-pcie.txt");
+BRCMF_FW_NVRAM_DEF(4371, "brcmfmac4371-pcie.bin", "brcmfmac4371-pcie.txt");
+
+static struct brcmf_firmware_mapping brcmf_pcie_fwnames[] = {
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43602_CHIP_ID, 0xFFFFFFFF, 43602),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4350_CHIP_ID, 0x000000FF, 4350C),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4350_CHIP_ID, 0xFFFFFF00, 4350),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4356_CHIP_ID, 0xFFFFFFFF, 4356),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43567_CHIP_ID, 0xFFFFFFFF, 43570),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43569_CHIP_ID, 0xFFFFFFFF, 43570),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43570_CHIP_ID, 0xFFFFFFFF, 43570),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4358_CHIP_ID, 0xFFFFFFFF, 4358),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4359_CHIP_ID, 0xFFFFFFFF, 4359),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4365_CHIP_ID, 0xFFFFFFFF, 4365B),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4366_CHIP_ID, 0xFFFFFFFF, 4366B),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4371_CHIP_ID, 0xFFFFFFFF, 4371),
+};
 
 #define BRCMF_PCIE_FW_UP_TIMEOUT		2000 /* msec */
 
@@ -183,7 +191,7 @@ enum brcmf_pcie_state {
 #define BRCMF_H2D_HOST_D0_INFORM_IN_USE		0x00000008
 #define BRCMF_H2D_HOST_D0_INFORM		0x00000010
 
-#define BRCMF_PCIE_MBDATA_TIMEOUT		2000
+#define BRCMF_PCIE_MBDATA_TIMEOUT		msecs_to_jiffies(2000)
 
 #define BRCMF_PCIE_CFGREG_STATUS_CMD		0x4
 #define BRCMF_PCIE_CFGREG_PM_CSR		0x4C
@@ -200,24 +208,6 @@ enum brcmf_pcie_state {
 #define BRCMF_PCIE_LINK_STATUS_CTRL_ASPM_ENAB	3
 
 
-MODULE_FIRMWARE(BRCMF_PCIE_43602_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_43602_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4350_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4350_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4356_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4356_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_43570_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_43570_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4358_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4358_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4365_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4365_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4366_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4366_NVRAM_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4371_FW_NAME);
-MODULE_FIRMWARE(BRCMF_PCIE_4371_NVRAM_NAME);
-
-
 struct brcmf_pcie_console {
 	u32 base_addr;
 	u32 buf_addr;
@@ -253,10 +243,9 @@ struct brcmf_pcie_core_info {
 struct brcmf_pciedev_info {
 	enum brcmf_pcie_state state;
 	bool in_irq;
-	bool irq_requested;
 	struct pci_dev *pdev;
-	char fw_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
-	char nvram_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
+	char fw_name[BRCMF_FW_NAME_LEN];
+	char nvram_name[BRCMF_FW_NAME_LEN];
 	void __iomem *regs;
 	void __iomem *tcm;
 	u32 tcm_size;
@@ -622,7 +611,9 @@ static int brcmf_pcie_exit_download_state(struct brcmf_pciedev_info *devinfo,
 		brcmf_chip_resetcore(core, 0, 0, 0);
 	}
 
-	return !brcmf_chip_set_active(devinfo->ci, resetintr);
+	if (!brcmf_chip_set_active(devinfo->ci, resetintr))
+		return -EINVAL;
+	return 0;
 }
 
 
@@ -885,7 +876,6 @@ static int brcmf_pcie_request_irq(struct brcmf_pciedev_info *devinfo)
 
 	brcmf_dbg(PCIE, "Enter\n");
 	/* is it a v1 or v2 implementation */
-	devinfo->irq_requested = false;
 	pci_enable_msi(pdev);
 	if (devinfo->generic_corerev == BRCMF_PCIE_GENREV1) {
 		if (request_threaded_irq(pdev->irq,
@@ -908,7 +898,6 @@ static int brcmf_pcie_request_irq(struct brcmf_pciedev_info *devinfo)
 			return -EIO;
 		}
 	}
-	devinfo->irq_requested = true;
 	devinfo->irq_allocated = true;
 	return 0;
 }
@@ -926,9 +915,6 @@ static void brcmf_pcie_release_irq(struct brcmf_pciedev_info *devinfo)
 	pdev = devinfo->pdev;
 
 	brcmf_pcie_intr_disable(devinfo);
-	if (!devinfo->irq_requested)
-		return;
-	devinfo->irq_requested = false;
 	free_irq(pdev->irq, devinfo);
 	pci_disable_msi(pdev);
 
@@ -1390,10 +1376,6 @@ static void brcmf_pcie_wowl_config(struct device *dev, bool enabled)
 
 	brcmf_dbg(PCIE, "Configuring WOWL, enabled=%d\n", enabled);
 	devinfo->wowl_enabled = enabled;
-	if (enabled)
-		device_set_wakeup_enable(&devinfo->pdev->dev, true);
-	else
-		device_set_wakeup_enable(&devinfo->pdev->dev, false);
 }
 
 
@@ -1419,7 +1401,7 @@ static int brcmf_pcie_get_memdump(struct device *dev, void *data, size_t len)
 }
 
 
-static struct brcmf_bus_ops brcmf_pcie_bus_ops = {
+static const struct brcmf_bus_ops brcmf_pcie_bus_ops = {
 	.txdata = brcmf_pcie_tx,
 	.stop = brcmf_pcie_down,
 	.txctl = brcmf_pcie_tx_ctlpkt,
@@ -1484,80 +1466,6 @@ brcmf_pcie_init_share_ram_info(struct brcmf_pciedev_info *devinfo,
 }
 
 
-static int brcmf_pcie_get_fwnames(struct brcmf_pciedev_info *devinfo)
-{
-	char *fw_name;
-	char *nvram_name;
-	uint fw_len, nv_len;
-	char end;
-
-	brcmf_dbg(PCIE, "Enter, chip 0x%04x chiprev %d\n", devinfo->ci->chip,
-		  devinfo->ci->chiprev);
-
-	switch (devinfo->ci->chip) {
-	case BRCM_CC_43602_CHIP_ID:
-		fw_name = BRCMF_PCIE_43602_FW_NAME;
-		nvram_name = BRCMF_PCIE_43602_NVRAM_NAME;
-		break;
-	case BRCM_CC_4350_CHIP_ID:
-		fw_name = BRCMF_PCIE_4350_FW_NAME;
-		nvram_name = BRCMF_PCIE_4350_NVRAM_NAME;
-		break;
-	case BRCM_CC_4356_CHIP_ID:
-		fw_name = BRCMF_PCIE_4356_FW_NAME;
-		nvram_name = BRCMF_PCIE_4356_NVRAM_NAME;
-		break;
-	case BRCM_CC_43567_CHIP_ID:
-	case BRCM_CC_43569_CHIP_ID:
-	case BRCM_CC_43570_CHIP_ID:
-		fw_name = BRCMF_PCIE_43570_FW_NAME;
-		nvram_name = BRCMF_PCIE_43570_NVRAM_NAME;
-		break;
-	case BRCM_CC_4358_CHIP_ID:
-		fw_name = BRCMF_PCIE_4358_FW_NAME;
-		nvram_name = BRCMF_PCIE_4358_NVRAM_NAME;
-		break;
-	case BRCM_CC_4365_CHIP_ID:
-		fw_name = BRCMF_PCIE_4365_FW_NAME;
-		nvram_name = BRCMF_PCIE_4365_NVRAM_NAME;
-		break;
-	case BRCM_CC_4366_CHIP_ID:
-		fw_name = BRCMF_PCIE_4366_FW_NAME;
-		nvram_name = BRCMF_PCIE_4366_NVRAM_NAME;
-		break;
-	case BRCM_CC_4371_CHIP_ID:
-		fw_name = BRCMF_PCIE_4371_FW_NAME;
-		nvram_name = BRCMF_PCIE_4371_NVRAM_NAME;
-		break;
-	default:
-		brcmf_err("Unsupported chip 0x%04x\n", devinfo->ci->chip);
-		return -ENODEV;
-	}
-
-	fw_len = sizeof(devinfo->fw_name) - 1;
-	nv_len = sizeof(devinfo->nvram_name) - 1;
-	/* check if firmware path is provided by module parameter */
-	if (brcmf_firmware_path[0] != '\0') {
-		strncpy(devinfo->fw_name, brcmf_firmware_path, fw_len);
-		strncpy(devinfo->nvram_name, brcmf_firmware_path, nv_len);
-		fw_len -= strlen(devinfo->fw_name);
-		nv_len -= strlen(devinfo->nvram_name);
-
-		end = brcmf_firmware_path[strlen(brcmf_firmware_path) - 1];
-		if (end != '/') {
-			strncat(devinfo->fw_name, "/", fw_len);
-			strncat(devinfo->nvram_name, "/", nv_len);
-			fw_len--;
-			nv_len--;
-		}
-	}
-	strncat(devinfo->fw_name, fw_name, fw_len);
-	strncat(devinfo->nvram_name, nvram_name, nv_len);
-
-	return 0;
-}
-
-
 static int brcmf_pcie_download_fw_nvram(struct brcmf_pciedev_info *devinfo,
 					const struct firmware *fw, void *nvram,
 					u32 nvram_len)
@@ -1893,7 +1801,10 @@ brcmf_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	bus->wowl_supported = pci_pme_capable(pdev, PCI_D3hot);
 	dev_set_drvdata(&pdev->dev, bus);
 
-	ret = brcmf_pcie_get_fwnames(devinfo);
+	ret = brcmf_fw_map_chip_to_name(devinfo->ci->chip, devinfo->ci->chiprev,
+					brcmf_pcie_fwnames,
+					ARRAY_SIZE(brcmf_pcie_fwnames),
+					devinfo->fw_name, devinfo->nvram_name);
 	if (ret)
 		goto fail_bus;
 
@@ -1959,15 +1870,14 @@ brcmf_pcie_remove(struct pci_dev *pdev)
 #ifdef CONFIG_PM
 
 
-static int brcmf_pcie_suspend(struct pci_dev *pdev, pm_message_t state)
+static int brcmf_pcie_pm_enter_D3(struct device *dev)
 {
 	struct brcmf_pciedev_info *devinfo;
 	struct brcmf_bus *bus;
-	int err;
 
-	brcmf_dbg(PCIE, "Enter, state=%d, pdev=%p\n", state.event, pdev);
+	brcmf_dbg(PCIE, "Enter\n");
 
-	bus = dev_get_drvdata(&pdev->dev);
+	bus = dev_get_drvdata(dev);
 	devinfo = bus->bus_priv.pcie->devinfo;
 
 	brcmf_bus_change_state(bus, BRCMF_BUS_DOWN);
@@ -1975,69 +1885,51 @@ static int brcmf_pcie_suspend(struct pci_dev *pdev, pm_message_t state)
 	devinfo->mbdata_completed = false;
 	brcmf_pcie_send_mb_data(devinfo, BRCMF_H2D_HOST_D3_INFORM);
 
-	wait_event_timeout(devinfo->mbdata_resp_wait,
-			   devinfo->mbdata_completed,
-			   msecs_to_jiffies(BRCMF_PCIE_MBDATA_TIMEOUT));
+	wait_event_timeout(devinfo->mbdata_resp_wait, devinfo->mbdata_completed,
+			   BRCMF_PCIE_MBDATA_TIMEOUT);
 	if (!devinfo->mbdata_completed) {
 		brcmf_err("Timeout on response for entering D3 substate\n");
 		return -EIO;
 	}
-	brcmf_pcie_send_mb_data(devinfo, BRCMF_H2D_HOST_D0_INFORM_IN_USE);
 
-	err = pci_save_state(pdev);
-	if (err)
-		brcmf_err("pci_save_state failed, err=%d\n", err);
-	if ((err) || (!devinfo->wowl_enabled)) {
-		brcmf_chip_detach(devinfo->ci);
-		devinfo->ci = NULL;
-		brcmf_pcie_remove(pdev);
-		return 0;
-	}
+	devinfo->state = BRCMFMAC_PCIE_STATE_DOWN;
 
-	return pci_prepare_to_sleep(pdev);
+	return 0;
 }
 
-static int brcmf_pcie_resume(struct pci_dev *pdev)
+
+static int brcmf_pcie_pm_leave_D3(struct device *dev)
 {
 	struct brcmf_pciedev_info *devinfo;
 	struct brcmf_bus *bus;
+	struct pci_dev *pdev;
 	int err;
 
-	bus = dev_get_drvdata(&pdev->dev);
-	brcmf_dbg(PCIE, "Enter, pdev=%p, bus=%p\n", pdev, bus);
+	brcmf_dbg(PCIE, "Enter\n");
 
-	err = pci_set_power_state(pdev, PCI_D0);
-	if (err) {
-		brcmf_err("pci_set_power_state failed, err=%d\n", err);
-		goto cleanup;
-	}
-	pci_restore_state(pdev);
-	pci_enable_wake(pdev, PCI_D3hot, false);
-	pci_enable_wake(pdev, PCI_D3cold, false);
+	bus = dev_get_drvdata(dev);
+	devinfo = bus->bus_priv.pcie->devinfo;
+	brcmf_dbg(PCIE, "Enter, dev=%p, bus=%p\n", dev, bus);
 
 	/* Check if device is still up and running, if so we are ready */
-	if (bus) {
-		devinfo = bus->bus_priv.pcie->devinfo;
-		if (brcmf_pcie_read_reg32(devinfo,
-					  BRCMF_PCIE_PCIE2REG_INTMASK) != 0) {
-			if (brcmf_pcie_send_mb_data(devinfo,
-						    BRCMF_H2D_HOST_D0_INFORM))
-				goto cleanup;
-			brcmf_dbg(PCIE, "Hot resume, continue....\n");
-			brcmf_pcie_select_core(devinfo, BCMA_CORE_PCIE2);
-			brcmf_bus_change_state(bus, BRCMF_BUS_UP);
-			brcmf_pcie_intr_enable(devinfo);
-			return 0;
-		}
+	if (brcmf_pcie_read_reg32(devinfo, BRCMF_PCIE_PCIE2REG_INTMASK) != 0) {
+		brcmf_dbg(PCIE, "Try to wakeup device....\n");
+		if (brcmf_pcie_send_mb_data(devinfo, BRCMF_H2D_HOST_D0_INFORM))
+			goto cleanup;
+		brcmf_dbg(PCIE, "Hot resume, continue....\n");
+		devinfo->state = BRCMFMAC_PCIE_STATE_UP;
+		brcmf_pcie_select_core(devinfo, BCMA_CORE_PCIE2);
+		brcmf_bus_change_state(bus, BRCMF_BUS_UP);
+		brcmf_pcie_intr_enable(devinfo);
+		return 0;
 	}
 
 cleanup:
-	if (bus) {
-		devinfo = bus->bus_priv.pcie->devinfo;
-		brcmf_chip_detach(devinfo->ci);
-		devinfo->ci = NULL;
-		brcmf_pcie_remove(pdev);
-	}
+	brcmf_chip_detach(devinfo->ci);
+	devinfo->ci = NULL;
+	pdev = devinfo->pdev;
+	brcmf_pcie_remove(pdev);
+
 	err = brcmf_pcie_probe(pdev, NULL);
 	if (err)
 		brcmf_err("probe after resume failed, err=%d\n", err);
@@ -2046,6 +1938,14 @@ cleanup:
 }
 
 
+static const struct dev_pm_ops brcmf_pciedrvr_pm = {
+	.suspend = brcmf_pcie_pm_enter_D3,
+	.resume = brcmf_pcie_pm_leave_D3,
+	.freeze = brcmf_pcie_pm_enter_D3,
+	.restore = brcmf_pcie_pm_leave_D3,
+};
+
+
 #endif /* CONFIG_PM */
 
 
@@ -2058,6 +1958,7 @@ static struct pci_device_id brcmf_pcie_devid_table[] = {
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43567_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43570_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_4358_DEVICE_ID),
+	BRCMF_PCIE_DEVICE(BRCM_PCIE_4359_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_2G_DEVICE_ID),
 	BRCMF_PCIE_DEVICE(BRCM_PCIE_43602_5G_DEVICE_ID),
@@ -2083,9 +1984,8 @@ static struct pci_driver brcmf_pciedrvr = {
 	.probe = brcmf_pcie_probe,
 	.remove = brcmf_pcie_remove,
 #ifdef CONFIG_PM
-	.suspend = brcmf_pcie_suspend,
-	.resume = brcmf_pcie_resume
-#endif /* CONFIG_PM */
+	.driver.pm = &brcmf_pciedrvr_pm,
+#endif
 };
 
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.h
index 6edaaf8ef5ce..6edaaf8ef5ce 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/pcie.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/proto.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/proto.c
index 26b68c367f57..26b68c367f57 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/proto.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/proto.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/proto.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/proto.h
index d55119d36755..d55119d36755 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/proto.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/proto.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
index 7e74ac3ad815..a14d9d9da094 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.c
@@ -45,8 +45,8 @@
 #include "chip.h"
 #include "firmware.h"
 
-#define DCMD_RESP_TIMEOUT	2000	/* In milli second */
-#define CTL_DONE_TIMEOUT	2000	/* In milli second */
+#define DCMD_RESP_TIMEOUT	msecs_to_jiffies(2000)
+#define CTL_DONE_TIMEOUT	msecs_to_jiffies(2000)
 
 #ifdef DEBUG
 
@@ -460,7 +460,6 @@ struct brcmf_sdio {
 
 	struct sk_buff *glomd;	/* Packet containing glomming descriptor */
 	struct sk_buff_head glom; /* Packet list for glommed superframe */
-	uint glomerr;		/* Glom packet read errors */
 
 	u8 *rxbuf;		/* Buffer for receiving control packets */
 	uint rxblen;		/* Allocated length of rxbuf */
@@ -504,8 +503,7 @@ struct brcmf_sdio {
 	struct timer_list timer;
 	struct completion watchdog_wait;
 	struct task_struct *watchdog_tsk;
-	bool wd_timer_valid;
-	uint save_ms;
+	bool wd_active;
 
 	struct workqueue_struct *brcmf_wq;
 	struct work_struct datawork;
@@ -597,136 +595,41 @@ static const struct sdiod_drive_str sdiod_drvstr_tab2_3v3[] = {
 	{4,  0x1}
 };
 
-#define BCM43143_FIRMWARE_NAME		"brcm/brcmfmac43143-sdio.bin"
-#define BCM43143_NVRAM_NAME		"brcm/brcmfmac43143-sdio.txt"
-#define BCM43241B0_FIRMWARE_NAME	"brcm/brcmfmac43241b0-sdio.bin"
-#define BCM43241B0_NVRAM_NAME		"brcm/brcmfmac43241b0-sdio.txt"
-#define BCM43241B4_FIRMWARE_NAME	"brcm/brcmfmac43241b4-sdio.bin"
-#define BCM43241B4_NVRAM_NAME		"brcm/brcmfmac43241b4-sdio.txt"
-#define BCM43241B5_FIRMWARE_NAME	"brcm/brcmfmac43241b5-sdio.bin"
-#define BCM43241B5_NVRAM_NAME		"brcm/brcmfmac43241b5-sdio.txt"
-#define BCM4329_FIRMWARE_NAME		"brcm/brcmfmac4329-sdio.bin"
-#define BCM4329_NVRAM_NAME		"brcm/brcmfmac4329-sdio.txt"
-#define BCM4330_FIRMWARE_NAME		"brcm/brcmfmac4330-sdio.bin"
-#define BCM4330_NVRAM_NAME		"brcm/brcmfmac4330-sdio.txt"
-#define BCM4334_FIRMWARE_NAME		"brcm/brcmfmac4334-sdio.bin"
-#define BCM4334_NVRAM_NAME		"brcm/brcmfmac4334-sdio.txt"
-#define BCM43340_FIRMWARE_NAME		"brcm/brcmfmac43340-sdio.bin"
-#define BCM43340_NVRAM_NAME		"brcm/brcmfmac43340-sdio.txt"
-#define BCM4335_FIRMWARE_NAME		"brcm/brcmfmac4335-sdio.bin"
-#define BCM4335_NVRAM_NAME		"brcm/brcmfmac4335-sdio.txt"
-#define BCM43362_FIRMWARE_NAME		"brcm/brcmfmac43362-sdio.bin"
-#define BCM43362_NVRAM_NAME		"brcm/brcmfmac43362-sdio.txt"
-#define BCM4339_FIRMWARE_NAME		"brcm/brcmfmac4339-sdio.bin"
-#define BCM4339_NVRAM_NAME		"brcm/brcmfmac4339-sdio.txt"
-#define BCM43430_FIRMWARE_NAME		"brcm/brcmfmac43430-sdio.bin"
-#define BCM43430_NVRAM_NAME		"brcm/brcmfmac43430-sdio.txt"
-#define BCM43455_FIRMWARE_NAME		"brcm/brcmfmac43455-sdio.bin"
-#define BCM43455_NVRAM_NAME		"brcm/brcmfmac43455-sdio.txt"
-#define BCM4354_FIRMWARE_NAME		"brcm/brcmfmac4354-sdio.bin"
-#define BCM4354_NVRAM_NAME		"brcm/brcmfmac4354-sdio.txt"
-
-MODULE_FIRMWARE(BCM43143_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43143_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43241B0_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43241B0_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43241B4_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43241B4_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43241B5_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43241B5_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4329_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4329_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4330_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4330_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4334_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4334_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43340_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43340_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4335_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4335_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43362_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43362_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4339_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4339_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43430_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43430_NVRAM_NAME);
-MODULE_FIRMWARE(BCM43455_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM43455_NVRAM_NAME);
-MODULE_FIRMWARE(BCM4354_FIRMWARE_NAME);
-MODULE_FIRMWARE(BCM4354_NVRAM_NAME);
-
-struct brcmf_firmware_names {
-	u32 chipid;
-	u32 revmsk;
-	const char *bin;
-	const char *nv;
+BRCMF_FW_NVRAM_DEF(43143, "brcmfmac43143-sdio.bin", "brcmfmac43143-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43241B0, "brcmfmac43241b0-sdio.bin",
+		   "brcmfmac43241b0-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43241B4, "brcmfmac43241b4-sdio.bin",
+		   "brcmfmac43241b4-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43241B5, "brcmfmac43241b5-sdio.bin",
+		   "brcmfmac43241b5-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4329, "brcmfmac4329-sdio.bin", "brcmfmac4329-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4330, "brcmfmac4330-sdio.bin", "brcmfmac4330-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4334, "brcmfmac4334-sdio.bin", "brcmfmac4334-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43340, "brcmfmac43340-sdio.bin", "brcmfmac43340-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4335, "brcmfmac4335-sdio.bin", "brcmfmac4335-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43362, "brcmfmac43362-sdio.bin", "brcmfmac43362-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4339, "brcmfmac4339-sdio.bin", "brcmfmac4339-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43430, "brcmfmac43430-sdio.bin", "brcmfmac43430-sdio.txt");
+BRCMF_FW_NVRAM_DEF(43455, "brcmfmac43455-sdio.bin", "brcmfmac43455-sdio.txt");
+BRCMF_FW_NVRAM_DEF(4354, "brcmfmac4354-sdio.bin", "brcmfmac4354-sdio.txt");
+
+static struct brcmf_firmware_mapping brcmf_sdio_fwnames[] = {
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43143_CHIP_ID, 0xFFFFFFFF, 43143),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43241_CHIP_ID, 0x0000001F, 43241B0),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43241_CHIP_ID, 0x00000020, 43241B4),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43241_CHIP_ID, 0xFFFFFFC0, 43241B5),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4329_CHIP_ID, 0xFFFFFFFF, 4329),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4330_CHIP_ID, 0xFFFFFFFF, 4330),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4334_CHIP_ID, 0xFFFFFFFF, 4334),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43340_CHIP_ID, 0xFFFFFFFF, 43340),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4335_CHIP_ID, 0xFFFFFFFF, 4335),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43362_CHIP_ID, 0xFFFFFFFE, 43362),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4339_CHIP_ID, 0xFFFFFFFF, 4339),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_43430_CHIP_ID, 0xFFFFFFFF, 43430),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4345_CHIP_ID, 0xFFFFFFC0, 43455),
+	BRCMF_FW_NVRAM_ENTRY(BRCM_CC_4354_CHIP_ID, 0xFFFFFFFF, 4354)
 };
 
-enum brcmf_firmware_type {
-	BRCMF_FIRMWARE_BIN,
-	BRCMF_FIRMWARE_NVRAM
-};
-
-#define BRCMF_FIRMWARE_NVRAM(name) \
-	name ## _FIRMWARE_NAME, name ## _NVRAM_NAME
-
-static const struct brcmf_firmware_names brcmf_fwname_data[] = {
-	{ BRCM_CC_43143_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43143) },
-	{ BRCM_CC_43241_CHIP_ID, 0x0000001F, BRCMF_FIRMWARE_NVRAM(BCM43241B0) },
-	{ BRCM_CC_43241_CHIP_ID, 0x00000020, BRCMF_FIRMWARE_NVRAM(BCM43241B4) },
-	{ BRCM_CC_43241_CHIP_ID, 0xFFFFFFC0, BRCMF_FIRMWARE_NVRAM(BCM43241B5) },
-	{ BRCM_CC_4329_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4329) },
-	{ BRCM_CC_4330_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4330) },
-	{ BRCM_CC_4334_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4334) },
-	{ BRCM_CC_43340_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43340) },
-	{ BRCM_CC_4335_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4335) },
-	{ BRCM_CC_43362_CHIP_ID, 0xFFFFFFFE, BRCMF_FIRMWARE_NVRAM(BCM43362) },
-	{ BRCM_CC_4339_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4339) },
-	{ BRCM_CC_43430_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM43430) },
-	{ BRCM_CC_4345_CHIP_ID, 0xFFFFFFC0, BRCMF_FIRMWARE_NVRAM(BCM43455) },
-	{ BRCM_CC_4354_CHIP_ID, 0xFFFFFFFF, BRCMF_FIRMWARE_NVRAM(BCM4354) }
-};
-
-static int brcmf_sdio_get_fwnames(struct brcmf_chip *ci,
-				  struct brcmf_sdio_dev *sdiodev)
-{
-	int i;
-	char end;
-
-	for (i = 0; i < ARRAY_SIZE(brcmf_fwname_data); i++) {
-		if (brcmf_fwname_data[i].chipid == ci->chip &&
-		    brcmf_fwname_data[i].revmsk & BIT(ci->chiprev))
-			break;
-	}
-
-	if (i == ARRAY_SIZE(brcmf_fwname_data)) {
-		brcmf_err("Unknown chipid %d [%d]\n", ci->chip, ci->chiprev);
-		return -ENODEV;
-	}
-
-	/* check if firmware path is provided by module parameter */
-	if (brcmf_firmware_path[0] != '\0') {
-		strlcpy(sdiodev->fw_name, brcmf_firmware_path,
-			sizeof(sdiodev->fw_name));
-		strlcpy(sdiodev->nvram_name, brcmf_firmware_path,
-			sizeof(sdiodev->nvram_name));
-
-		end = brcmf_firmware_path[strlen(brcmf_firmware_path) - 1];
-		if (end != '/') {
-			strlcat(sdiodev->fw_name, "/",
-				sizeof(sdiodev->fw_name));
-			strlcat(sdiodev->nvram_name, "/",
-				sizeof(sdiodev->nvram_name));
-		}
-	}
-	strlcat(sdiodev->fw_name, brcmf_fwname_data[i].bin,
-		sizeof(sdiodev->fw_name));
-	strlcat(sdiodev->nvram_name, brcmf_fwname_data[i].nv,
-		sizeof(sdiodev->nvram_name));
-
-	return 0;
-}
-
 static void pkt_align(struct sk_buff *p, int len, int align)
 {
 	uint datalign;
@@ -1057,7 +960,7 @@ end:
 			brcmf_sdio_clkctl(bus, CLK_NONE, pendok);
 	} else {
 		brcmf_sdio_clkctl(bus, CLK_AVAIL, pendok);
-		brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
+		brcmf_sdio_wd_timer(bus, true);
 	}
 	bus->sleeping = sleep;
 	brcmf_dbg(SDIO, "new state %s\n",
@@ -1654,20 +1557,15 @@ static u8 brcmf_sdio_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 		sdio_release_host(bus->sdiodev->func[1]);
 		bus->sdcnt.f2rxdata++;
 
-		/* On failure, kill the superframe, allow a couple retries */
+		/* On failure, kill the superframe */
 		if (errcode < 0) {
 			brcmf_err("glom read of %d bytes failed: %d\n",
 				  dlen, errcode);
 
 			sdio_claim_host(bus->sdiodev->func[1]);
-			if (bus->glomerr++ < 3) {
-				brcmf_sdio_rxfail(bus, true, true);
-			} else {
-				bus->glomerr = 0;
-				brcmf_sdio_rxfail(bus, true, false);
-				bus->sdcnt.rxglomfail++;
-				brcmf_sdio_free_glom(bus);
-			}
+			brcmf_sdio_rxfail(bus, true, false);
+			bus->sdcnt.rxglomfail++;
+			brcmf_sdio_free_glom(bus);
 			sdio_release_host(bus->sdiodev->func[1]);
 			return 0;
 		}
@@ -1708,19 +1606,11 @@ static u8 brcmf_sdio_rxglom(struct brcmf_sdio *bus, u8 rxseq)
 		}
 
 		if (errcode) {
-			/* Terminate frame on error, request
-				 a couple retries */
+			/* Terminate frame on error */
 			sdio_claim_host(bus->sdiodev->func[1]);
-			if (bus->glomerr++ < 3) {
-				/* Restore superframe header space */
-				skb_push(pfirst, sfdoff);
-				brcmf_sdio_rxfail(bus, true, true);
-			} else {
-				bus->glomerr = 0;
-				brcmf_sdio_rxfail(bus, true, false);
-				bus->sdcnt.rxglomfail++;
-				brcmf_sdio_free_glom(bus);
-			}
+			brcmf_sdio_rxfail(bus, true, false);
+			bus->sdcnt.rxglomfail++;
+			brcmf_sdio_free_glom(bus);
 			sdio_release_host(bus->sdiodev->func[1]);
 			bus->cur_read.len = 0;
 			return 0;
@@ -1767,7 +1657,7 @@ static int brcmf_sdio_dcmd_resp_wait(struct brcmf_sdio *bus, uint *condition,
 				     bool *pending)
 {
 	DECLARE_WAITQUEUE(wait, current);
-	int timeout = msecs_to_jiffies(DCMD_RESP_TIMEOUT);
+	int timeout = DCMD_RESP_TIMEOUT;
 
 	/* Wait until control frame is available */
 	add_wait_queue(&bus->dcmd_resp_wait, &wait);
@@ -1787,8 +1677,7 @@ static int brcmf_sdio_dcmd_resp_wait(struct brcmf_sdio *bus, uint *condition,
 
 static int brcmf_sdio_dcmd_resp_wake(struct brcmf_sdio *bus)
 {
-	if (waitqueue_active(&bus->dcmd_resp_wait))
-		wake_up_interruptible(&bus->dcmd_resp_wait);
+	wake_up_interruptible(&bus->dcmd_resp_wait);
 
 	return 0;
 }
@@ -2112,8 +2001,7 @@ static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
 static void
 brcmf_sdio_wait_event_wakeup(struct brcmf_sdio *bus)
 {
-	if (waitqueue_active(&bus->ctrl_wait))
-		wake_up_interruptible(&bus->ctrl_wait);
+	wake_up_interruptible(&bus->ctrl_wait);
 	return;
 }
 
@@ -2954,7 +2842,7 @@ brcmf_sdio_bus_txctl(struct device *dev, unsigned char *msg, uint msglen)
 
 	brcmf_sdio_trigger_dpc(bus);
 	wait_event_interruptible_timeout(bus->ctrl_wait, !bus->ctrl_frame_stat,
-					 msecs_to_jiffies(CTL_DONE_TIMEOUT));
+					 CTL_DONE_TIMEOUT);
 	ret = 0;
 	if (bus->ctrl_frame_stat) {
 		sdio_claim_host(bus->sdiodev->func[1]);
@@ -3664,7 +3552,7 @@ static void brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 	/* Poll for console output periodically */
 	if (bus->sdiodev->state == BRCMF_SDIOD_DATA && BRCMF_FWCON_ON() &&
 	    bus->console_interval != 0) {
-		bus->console.count += BRCMF_WD_POLL_MS;
+		bus->console.count += jiffies_to_msecs(BRCMF_WD_POLL);
 		if (bus->console.count >= bus->console_interval) {
 			bus->console.count -= bus->console_interval;
 			sdio_claim_host(bus->sdiodev->func[1]);
@@ -3687,7 +3575,7 @@ static void brcmf_sdio_bus_watchdog(struct brcmf_sdio *bus)
 			if (bus->idlecount > bus->idletime) {
 				brcmf_dbg(SDIO, "idle\n");
 				sdio_claim_host(bus->sdiodev->func[1]);
-				brcmf_sdio_wd_timer(bus, 0);
+				brcmf_sdio_wd_timer(bus, false);
 				bus->idlecount = 0;
 				brcmf_sdio_bus_sleep(bus, true, false);
 				sdio_release_host(bus->sdiodev->func[1]);
@@ -4019,13 +3907,13 @@ brcmf_sdio_watchdog(unsigned long data)
 	if (bus->watchdog_tsk) {
 		complete(&bus->watchdog_wait);
 		/* Reschedule the watchdog */
-		if (bus->wd_timer_valid)
+		if (bus->wd_active)
 			mod_timer(&bus->timer,
-				  jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS));
+				  jiffies + BRCMF_WD_POLL);
 	}
 }
 
-static struct brcmf_bus_ops brcmf_sdio_bus_ops = {
+static const struct brcmf_bus_ops brcmf_sdio_bus_ops = {
 	.stop = brcmf_sdio_bus_stop,
 	.preinit = brcmf_sdio_bus_preinit,
 	.txdata = brcmf_sdio_bus_txdata,
@@ -4061,7 +3949,7 @@ static void brcmf_sdio_firmware_callback(struct device *dev,
 
 	/* Start the watchdog timer */
 	bus->sdcnt.tickcnt = 0;
-	brcmf_sdio_wd_timer(bus, BRCMF_WD_POLL_MS);
+	brcmf_sdio_wd_timer(bus, true);
 
 	sdio_claim_host(sdiodev->func[1]);
 
@@ -4226,6 +4114,11 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 		goto fail;
 	}
 
+	/* allocate scatter-gather table. sg support
+	 * will be disabled upon allocation failure.
+	 */
+	brcmf_sdiod_sgtable_alloc(bus->sdiodev);
+
 	/* Query the F2 block size, set roundup accordingly */
 	bus->blocksize = bus->sdiodev->func[2]->cur_blksize;
 	bus->roundup = min(max_roundup, bus->blocksize);
@@ -4266,7 +4159,10 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev)
 	brcmf_sdio_debugfs_create(bus);
 	brcmf_dbg(INFO, "completed!!\n");
 
-	ret = brcmf_sdio_get_fwnames(bus->ci, sdiodev);
+	ret = brcmf_fw_map_chip_to_name(bus->ci->chip, bus->ci->chiprev,
+					brcmf_sdio_fwnames,
+					ARRAY_SIZE(brcmf_sdio_fwnames),
+					sdiodev->fw_name, sdiodev->nvram_name);
 	if (ret)
 		goto fail;
 
@@ -4303,7 +4199,7 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 		if (bus->ci) {
 			if (bus->sdiodev->state != BRCMF_SDIOD_NOMEDIUM) {
 				sdio_claim_host(bus->sdiodev->func[1]);
-				brcmf_sdio_wd_timer(bus, 0);
+				brcmf_sdio_wd_timer(bus, false);
 				brcmf_sdio_clkctl(bus, CLK_AVAIL, false);
 				/* Leave the device in state where it is
 				 * 'passive'. This is done by resetting all
@@ -4325,13 +4221,12 @@ void brcmf_sdio_remove(struct brcmf_sdio *bus)
 	brcmf_dbg(TRACE, "Disconnected\n");
 }
 
-void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick)
+void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, bool active)
 {
 	/* Totally stop the timer */
-	if (!wdtick && bus->wd_timer_valid) {
+	if (!active && bus->wd_active) {
 		del_timer_sync(&bus->timer);
-		bus->wd_timer_valid = false;
-		bus->save_ms = wdtick;
+		bus->wd_active = false;
 		return;
 	}
 
@@ -4339,27 +4234,18 @@ void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick)
 	if (bus->sdiodev->state != BRCMF_SDIOD_DATA)
 		return;
 
-	if (wdtick) {
-		if (bus->save_ms != BRCMF_WD_POLL_MS) {
-			if (bus->wd_timer_valid)
-				/* Stop timer and restart at new value */
-				del_timer_sync(&bus->timer);
-
+	if (active) {
+		if (!bus->wd_active) {
 			/* Create timer again when watchdog period is
 			   dynamically changed or in the first instance
 			 */
-			bus->timer.expires =
-				jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS);
+			bus->timer.expires = jiffies + BRCMF_WD_POLL;
 			add_timer(&bus->timer);
-
+			bus->wd_active = true;
 		} else {
 			/* Re arm the timer, at last watchdog period */
-			mod_timer(&bus->timer,
-				jiffies + msecs_to_jiffies(BRCMF_WD_POLL_MS));
+			mod_timer(&bus->timer, jiffies + BRCMF_WD_POLL);
 		}
-
-		bus->wd_timer_valid = true;
-		bus->save_ms = wdtick;
 	}
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/sdio.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.h
index 7328478b2d7b..23f223150cef 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/sdio.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/sdio.h
@@ -152,8 +152,8 @@
 /* Packet alignment for most efficient SDIO (can change based on platform) */
 #define BRCMF_SDALIGN	(1 << 6)
 
-/* watchdog polling interval in ms */
-#define BRCMF_WD_POLL_MS	10
+/* watchdog polling interval */
+#define BRCMF_WD_POLL	msecs_to_jiffies(10)
 
 /**
  * enum brcmf_sdiod_state - the state of the bus.
@@ -195,8 +195,8 @@ struct brcmf_sdio_dev {
 	uint max_segment_size;
 	uint txglomsz;
 	struct sg_table sgtable;
-	char fw_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
-	char nvram_name[BRCMF_FW_PATH_LEN + BRCMF_FW_NAME_LEN];
+	char fw_name[BRCMF_FW_NAME_LEN];
+	char nvram_name[BRCMF_FW_NAME_LEN];
 	bool wowl_enabled;
 	enum brcmf_sdiod_state state;
 	struct brcmf_sdiod_freezer *freezer;
@@ -342,6 +342,7 @@ int brcmf_sdiod_ramrw(struct brcmf_sdio_dev *sdiodev, bool write, u32 address,
 
 /* Issue an abort to the specified function */
 int brcmf_sdiod_abort(struct brcmf_sdio_dev *sdiodev, uint fn);
+void brcmf_sdiod_sgtable_alloc(struct brcmf_sdio_dev *sdiodev);
 void brcmf_sdiod_change_state(struct brcmf_sdio_dev *sdiodev,
 			      enum brcmf_sdiod_state state);
 #ifdef CONFIG_PM_SLEEP
@@ -369,7 +370,7 @@ struct brcmf_sdio *brcmf_sdio_probe(struct brcmf_sdio_dev *sdiodev);
 void brcmf_sdio_remove(struct brcmf_sdio *bus);
 void brcmf_sdio_isr(struct brcmf_sdio *bus);
 
-void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, uint wdtick);
+void brcmf_sdio_wd_timer(struct brcmf_sdio *bus, bool active);
 void brcmf_sdio_wowl_config(struct device *dev, bool enabled);
 int brcmf_sdio_sleep(struct brcmf_sdio *bus, bool sleep);
 void brcmf_sdio_trigger_dpc(struct brcmf_sdio *bus);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/tracepoint.c
index a10f35c5eb3d..a10f35c5eb3d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/tracepoint.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/tracepoint.h
index 4d7d51f95716..4d7d51f95716 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/tracepoint.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/tracepoint.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c
index 689e64d004bc..c72b7b352a77 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.c
@@ -29,7 +29,7 @@
 #include "usb.h"
 
 
-#define IOCTL_RESP_TIMEOUT		2000
+#define IOCTL_RESP_TIMEOUT		msecs_to_jiffies(2000)
 
 #define BRCMF_USB_RESET_GETVER_SPINWAIT	100	/* in unit of ms */
 #define BRCMF_USB_RESET_GETVER_LOOP_CNT	10
@@ -43,10 +43,20 @@
 #define BRCMF_USB_CBCTL_READ		1
 #define BRCMF_USB_MAX_PKT_SIZE		1600
 
-#define BRCMF_USB_43143_FW_NAME		"brcm/brcmfmac43143.bin"
-#define BRCMF_USB_43236_FW_NAME		"brcm/brcmfmac43236b.bin"
-#define BRCMF_USB_43242_FW_NAME		"brcm/brcmfmac43242a.bin"
-#define BRCMF_USB_43569_FW_NAME		"brcm/brcmfmac43569.bin"
+BRCMF_FW_DEF(43143, "brcmfmac43143.bin");
+BRCMF_FW_DEF(43236B, "brcmfmac43236b.bin");
+BRCMF_FW_DEF(43242A, "brcmfmac43242a.bin");
+BRCMF_FW_DEF(43569, "brcmfmac43569.bin");
+
+static struct brcmf_firmware_mapping brcmf_usb_fwnames[] = {
+	BRCMF_FW_ENTRY(BRCM_CC_43143_CHIP_ID, 0xFFFFFFFF, 43143),
+	BRCMF_FW_ENTRY(BRCM_CC_43235_CHIP_ID, 0x00000008, 43236B),
+	BRCMF_FW_ENTRY(BRCM_CC_43236_CHIP_ID, 0x00000008, 43236B),
+	BRCMF_FW_ENTRY(BRCM_CC_43238_CHIP_ID, 0x00000008, 43236B),
+	BRCMF_FW_ENTRY(BRCM_CC_43242_CHIP_ID, 0xFFFFFFFF, 43242A),
+	BRCMF_FW_ENTRY(BRCM_CC_43566_CHIP_ID, 0xFFFFFFFF, 43569),
+	BRCMF_FW_ENTRY(BRCM_CC_43569_CHIP_ID, 0xFFFFFFFF, 43569)
+};
 
 #define TRX_MAGIC		0x30524448	/* "HDR0" */
 #define TRX_MAX_OFFSET		3		/* Max number of file offsets */
@@ -139,6 +149,7 @@ struct brcmf_usbdev_info {
 	struct brcmf_usbreq *tx_reqs;
 	struct brcmf_usbreq *rx_reqs;
 
+	char fw_name[BRCMF_FW_NAME_LEN];
 	const u8 *image;	/* buffer for combine fw and nvram */
 	int image_len;
 
@@ -179,14 +190,12 @@ static struct brcmf_usbdev_info *brcmf_usb_get_businfo(struct device *dev)
 static int brcmf_usb_ioctl_resp_wait(struct brcmf_usbdev_info *devinfo)
 {
 	return wait_event_timeout(devinfo->ioctl_resp_wait,
-				  devinfo->ctl_completed,
-				  msecs_to_jiffies(IOCTL_RESP_TIMEOUT));
+				  devinfo->ctl_completed, IOCTL_RESP_TIMEOUT);
 }
 
 static void brcmf_usb_ioctl_resp_wake(struct brcmf_usbdev_info *devinfo)
 {
-	if (waitqueue_active(&devinfo->ioctl_resp_wait))
-		wake_up(&devinfo->ioctl_resp_wait);
+	wake_up(&devinfo->ioctl_resp_wait);
 }
 
 static void
@@ -983,45 +992,15 @@ static int brcmf_usb_dlrun(struct brcmf_usbdev_info *devinfo)
 	return 0;
 }
 
-static bool brcmf_usb_chip_support(int chipid, int chiprev)
-{
-	switch(chipid) {
-	case BRCM_CC_43143_CHIP_ID:
-		return true;
-	case BRCM_CC_43235_CHIP_ID:
-	case BRCM_CC_43236_CHIP_ID:
-	case BRCM_CC_43238_CHIP_ID:
-		return (chiprev == 3);
-	case BRCM_CC_43242_CHIP_ID:
-		return true;
-	case BRCM_CC_43566_CHIP_ID:
-	case BRCM_CC_43569_CHIP_ID:
-		return true;
-	default:
-		break;
-	}
-	return false;
-}
-
 static int
 brcmf_usb_fw_download(struct brcmf_usbdev_info *devinfo)
 {
-	int devid, chiprev;
 	int err;
 
 	brcmf_dbg(USB, "Enter\n");
 	if (devinfo == NULL)
 		return -ENODEV;
 
-	devid = devinfo->bus_pub.devid;
-	chiprev = devinfo->bus_pub.chiprev;
-
-	if (!brcmf_usb_chip_support(devid, chiprev)) {
-		brcmf_err("unsupported chip %d rev %d\n",
-			  devid, chiprev);
-		return -EINVAL;
-	}
-
 	if (!devinfo->image) {
 		brcmf_err("No firmware!\n");
 		return -ENOENT;
@@ -1071,25 +1050,6 @@ static int check_file(const u8 *headers)
 	return -1;
 }
 
-static const char *brcmf_usb_get_fwname(struct brcmf_usbdev_info *devinfo)
-{
-	switch (devinfo->bus_pub.devid) {
-	case BRCM_CC_43143_CHIP_ID:
-		return BRCMF_USB_43143_FW_NAME;
-	case BRCM_CC_43235_CHIP_ID:
-	case BRCM_CC_43236_CHIP_ID:
-	case BRCM_CC_43238_CHIP_ID:
-		return BRCMF_USB_43236_FW_NAME;
-	case BRCM_CC_43242_CHIP_ID:
-		return BRCMF_USB_43242_FW_NAME;
-	case BRCM_CC_43566_CHIP_ID:
-	case BRCM_CC_43569_CHIP_ID:
-		return BRCMF_USB_43569_FW_NAME;
-	default:
-		return NULL;
-	}
-}
-
 
 static
 struct brcmf_usbdev *brcmf_usb_attach(struct brcmf_usbdev_info *devinfo,
@@ -1163,7 +1123,7 @@ static void brcmf_usb_wowl_config(struct device *dev, bool enabled)
 		device_set_wakeup_enable(devinfo->dev, false);
 }
 
-static struct brcmf_bus_ops brcmf_usb_bus_ops = {
+static const struct brcmf_bus_ops brcmf_usb_bus_ops = {
 	.txdata = brcmf_usb_tx,
 	.stop = brcmf_usb_down,
 	.txctl = brcmf_usb_tx_ctlpkt,
@@ -1274,9 +1234,16 @@ static int brcmf_usb_probe_cb(struct brcmf_usbdev_info *devinfo)
 	bus->chip = bus_pub->devid;
 	bus->chiprev = bus_pub->chiprev;
 
+	ret = brcmf_fw_map_chip_to_name(bus_pub->devid, bus_pub->chiprev,
+					brcmf_usb_fwnames,
+					ARRAY_SIZE(brcmf_usb_fwnames),
+					devinfo->fw_name, NULL);
+	if (ret)
+		goto fail;
+
 	/* request firmware here */
-	ret = brcmf_fw_get_firmwares(dev, 0, brcmf_usb_get_fwname(devinfo),
-				     NULL, brcmf_usb_probe_phase2);
+	ret = brcmf_fw_get_firmwares(dev, 0, devinfo->fw_name, NULL,
+				     brcmf_usb_probe_phase2);
 	if (ret) {
 		brcmf_err("firmware request failed: %d\n", ret);
 		goto fail;
@@ -1472,8 +1439,7 @@ static int brcmf_usb_reset_resume(struct usb_interface *intf)
 
 	brcmf_dbg(USB, "Enter\n");
 
-	return brcmf_fw_get_firmwares(&usb->dev, 0,
-				      brcmf_usb_get_fwname(devinfo), NULL,
+	return brcmf_fw_get_firmwares(&usb->dev, 0, devinfo->fw_name, NULL,
 				      brcmf_usb_probe_phase2);
 }
 
@@ -1485,16 +1451,13 @@ static struct usb_device_id brcmf_usb_devid_table[] = {
 	BRCMF_USB_DEVICE(BRCM_USB_43236_DEVICE_ID),
 	BRCMF_USB_DEVICE(BRCM_USB_43242_DEVICE_ID),
 	BRCMF_USB_DEVICE(BRCM_USB_43569_DEVICE_ID),
+	{ USB_DEVICE(BRCM_USB_VENDOR_ID_LG, BRCM_USB_43242_LG_DEVICE_ID) },
 	/* special entry for device with firmware loaded and running */
 	BRCMF_USB_DEVICE(BRCM_USB_BCMFW_DEVICE_ID),
 	{ /* end: all zeroes */ }
 };
 
 MODULE_DEVICE_TABLE(usb, brcmf_usb_devid_table);
-MODULE_FIRMWARE(BRCMF_USB_43143_FW_NAME);
-MODULE_FIRMWARE(BRCMF_USB_43236_FW_NAME);
-MODULE_FIRMWARE(BRCMF_USB_43242_FW_NAME);
-MODULE_FIRMWARE(BRCMF_USB_43569_FW_NAME);
 
 static struct usb_driver brcmf_usbdrvr = {
 	.name = KBUILD_MODNAME,
@@ -1504,7 +1467,6 @@ static struct usb_driver brcmf_usbdrvr = {
 	.suspend = brcmf_usb_suspend,
 	.resume = brcmf_usb_resume,
 	.reset_resume = brcmf_usb_reset_resume,
-	.supports_autosuspend = 1,
 	.disable_hub_initiated_lpm = 1,
 };
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/usb.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.h
index f483a8c9945b..f483a8c9945b 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/usb.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/usb.h
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/vendor.c b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/vendor.c
index 8eff2753abad..8eff2753abad 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/vendor.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/vendor.c
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/vendor.h b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/vendor.h
index 061b7bfa2e1c..061b7bfa2e1c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/vendor.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmfmac/vendor.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/Makefile b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/Makefile
index 32464acccd90..960e6b86bbcb 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/Makefile
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/Makefile
@@ -17,9 +17,9 @@
 
 ccflags-y := \
 	-D__CHECK_ENDIAN__ \
-	-Idrivers/net/wireless/brcm80211/brcmsmac \
-	-Idrivers/net/wireless/brcm80211/brcmsmac/phy \
-	-Idrivers/net/wireless/brcm80211/include
+	-Idrivers/net/wireless/broadcom/brcm80211/brcmsmac \
+	-Idrivers/net/wireless/broadcom/brcm80211/brcmsmac/phy \
+	-Idrivers/net/wireless/broadcom/brcm80211/include
 
 brcmsmac-y := \
 	mac80211_if.o \
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/aiutils.c
index 53365977bfd6..53365977bfd6 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/aiutils.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/aiutils.h
index 2d08c155c23b..2d08c155c23b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/aiutils.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/aiutils.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ampdu.c
index fa391e4eb098..fa391e4eb098 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ampdu.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ampdu.h
index 03bdcf29bd50..03bdcf29bd50 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ampdu.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ampdu.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/antsel.c
index 54c616919590..54c616919590 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/antsel.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/antsel.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/antsel.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/antsel.h
index a3d487ab1964..a3d487ab1964 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/antsel.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/antsel.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac.h
index a0da3248b942..a0da3248b942 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h
index 0e8a69ab909f..0e8a69ab909f 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac_msg.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h
index cf2cc070f1e5..cf2cc070f1e5 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_brcmsmac_tx.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_events.c
index 52fc9eeb5fa5..52fc9eeb5fa5 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_events.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_events.h
index cbf2f06436fc..cbf2f06436fc 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/brcms_trace_events.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/brcms_trace_events.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/channel.c
index 635ae034c7e5..38bd5890bd53 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/channel.c
@@ -177,8 +177,8 @@ static bool brcms_c_country_valid(const char *ccode)
 	 * only allow ascii alpha uppercase for the first 2
 	 * chars.
 	 */
-	if (!((0x80 & ccode[0]) == 0 && ccode[0] >= 0x41 && ccode[0] <= 0x5A &&
-	      (0x80 & ccode[1]) == 0 && ccode[1] >= 0x41 && ccode[1] <= 0x5A))
+	if (!((ccode[0] & 0x80) == 0 && ccode[0] >= 0x41 && ccode[0] <= 0x5A &&
+	      (ccode[1] & 0x80) == 0 && ccode[1] >= 0x41 && ccode[1] <= 0x5A))
 		return false;
 
 	/*
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/channel.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/channel.h
index 39dd3a5b2979..39dd3a5b2979 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/channel.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/channel.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/d11.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/d11.h
index 9035cc4d6ff3..9035cc4d6ff3 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/d11.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/d11.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/debug.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/debug.c
index 7a1fbb2e3a71..7a1fbb2e3a71 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/debug.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/debug.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/debug.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/debug.h
index 822781cf15d4..822781cf15d4 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/debug.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/debug.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/dma.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/dma.c
index 796f5f9d5d5a..796f5f9d5d5a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/dma.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/dma.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/dma.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/dma.h
index ff5b80b09046..ff5b80b09046 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/dma.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/dma.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/led.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/led.c
index 74b17cecb189..74b17cecb189 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/led.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/led.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/led.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/led.h
index 17a0b1f5dbcf..17a0b1f5dbcf 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/led.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/led.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/mac80211_if.c
index bec2dc1ca2e4..bec2dc1ca2e4 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/mac80211_if.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/mac80211_if.h
index 198053dfc310..198053dfc310 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/mac80211_if.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/main.c
index 218cbc8bf3a7..218cbc8bf3a7 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/main.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/main.h
index c4d135cff04a..c4d135cff04a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/main.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_cmn.c
index 1c4e9dd57960..1c4e9dd57960 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_cmn.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_hal.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_hal.h
index 4d3734f48d9c..4d3734f48d9c 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_hal.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_hal.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_int.h
index 4960f7d26804..4960f7d26804 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_int.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_int.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_lcn.c
index 93d4cde0eb31..93d4cde0eb31 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_lcn.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_lcn.h
index f4a8ab09da43..f4a8ab09da43 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_lcn.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_n.c
index 99dac9b8a082..99dac9b8a082 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_n.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_qmath.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_qmath.c
index faf1ebe76068..faf1ebe76068 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_qmath.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_qmath.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_qmath.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_qmath.h
index 20e3783f921b..20e3783f921b 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_qmath.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_qmath.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_radio.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_radio.h
index c3a675455ff5..c3a675455ff5 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_radio.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phy_radio.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phyreg_n.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phyreg_n.h
index a97c3a799479..a97c3a799479 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phyreg_n.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phyreg_n.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_lcn.c
index d7fa312214f3..d7fa312214f3 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_lcn.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_lcn.h
index 489422a36085..489422a36085 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_lcn.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_lcn.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_n.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_n.c
index dbf50ef6cd75..dbf50ef6cd75 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_n.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_n.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_n.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_n.h
index dc8a84e85117..dc8a84e85117 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phytbl_n.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy/phytbl_n.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy_shim.c
index a0de5db0cd64..a0de5db0cd64 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy_shim.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy_shim.h
index dd8774717ade..dd8774717ade 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy_shim.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/phy_shim.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pmu.c
index 71b80381f3ad..71b80381f3ad 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pmu.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pmu.h
index a014bbc4f935..a014bbc4f935 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pmu.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pmu.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pub.h
index 4da38cb4f318..4da38cb4f318 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/pub.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/rate.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/rate.c
index 0a0c0ad4f96f..0a0c0ad4f96f 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/rate.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/rate.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/rate.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/rate.h
index 5bb88b78ed64..5bb88b78ed64 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/rate.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/rate.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/scb.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/scb.h
index 3a3d73699f83..3a3d73699f83 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/scb.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/scb.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/stf.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/stf.c
index dd9162722495..dd9162722495 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/stf.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/stf.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/stf.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/stf.h
index ba9493009a33..ba9493009a33 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/stf.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/stf.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/types.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/types.h
index ae1f3ad40d45..ae1f3ad40d45 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/types.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/types.h
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ucode_loader.c b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ucode_loader.c
index 80e3ccf865e3..80e3ccf865e3 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ucode_loader.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ucode_loader.c
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/ucode_loader.h b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ucode_loader.h
index c87dd89bcb78..c87dd89bcb78 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/ucode_loader.h
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmsmac/ucode_loader.h
diff --git a/drivers/net/wireless/brcm80211/brcmutil/Makefile b/drivers/net/wireless/broadcom/brcm80211/brcmutil/Makefile
index 8a928184016a..256c91f9ac4b 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/Makefile
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmutil/Makefile
@@ -16,8 +16,8 @@
 # CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 
 ccflags-y :=				\
-	-Idrivers/net/wireless/brcm80211/brcmutil \
-	-Idrivers/net/wireless/brcm80211/include
+	-Idrivers/net/wireless/broadcom/brcm80211/brcmutil \
+	-Idrivers/net/wireless/broadcom/brcm80211/include
 
 obj-$(CONFIG_BRCMUTIL)	+= brcmutil.o
 brcmutil-objs	= utils.o d11.o
diff --git a/drivers/net/wireless/brcm80211/brcmutil/d11.c b/drivers/net/wireless/broadcom/brcm80211/brcmutil/d11.c
index 2b2522bdd8eb..2b2522bdd8eb 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/d11.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmutil/d11.c
diff --git a/drivers/net/wireless/brcm80211/brcmutil/utils.c b/drivers/net/wireless/broadcom/brcm80211/brcmutil/utils.c
index 0543607002fd..0543607002fd 100644
--- a/drivers/net/wireless/brcm80211/brcmutil/utils.c
+++ b/drivers/net/wireless/broadcom/brcm80211/brcmutil/utils.c
diff --git a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h b/drivers/net/wireless/broadcom/brcm80211/include/brcm_hw_ids.h
index aa06ea231db3..699f2c2782ee 100644
--- a/drivers/net/wireless/brcm80211/include/brcm_hw_ids.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/brcm_hw_ids.h
@@ -21,6 +21,7 @@
 #include <linux/mmc/sdio_ids.h>
 
 #define BRCM_USB_VENDOR_ID_BROADCOM	0x0a5c
+#define BRCM_USB_VENDOR_ID_LG		0x043e
 #define BRCM_PCIE_VENDOR_ID_BROADCOM	PCI_VENDOR_ID_BROADCOM
 
 /* Chipcommon Core Chip IDs */
@@ -47,6 +48,7 @@
 #define BRCM_CC_43569_CHIP_ID		43569
 #define BRCM_CC_43570_CHIP_ID		43570
 #define BRCM_CC_4358_CHIP_ID		0x4358
+#define BRCM_CC_4359_CHIP_ID		0x4359
 #define BRCM_CC_43602_CHIP_ID		43602
 #define BRCM_CC_4365_CHIP_ID		0x4365
 #define BRCM_CC_4366_CHIP_ID		0x4366
@@ -56,6 +58,7 @@
 #define BRCM_USB_43143_DEVICE_ID	0xbd1e
 #define BRCM_USB_43236_DEVICE_ID	0xbd17
 #define BRCM_USB_43242_DEVICE_ID	0xbd1f
+#define BRCM_USB_43242_LG_DEVICE_ID	0x3101
 #define BRCM_USB_43569_DEVICE_ID	0xbd27
 #define BRCM_USB_BCMFW_DEVICE_ID	0x0bdc
 
@@ -66,6 +69,7 @@
 #define BRCM_PCIE_43567_DEVICE_ID	0x43d3
 #define BRCM_PCIE_43570_DEVICE_ID	0x43d9
 #define BRCM_PCIE_4358_DEVICE_ID	0x43e9
+#define BRCM_PCIE_4359_DEVICE_ID	0x43ef
 #define BRCM_PCIE_43602_DEVICE_ID	0x43ba
 #define BRCM_PCIE_43602_2G_DEVICE_ID	0x43bb
 #define BRCM_PCIE_43602_5G_DEVICE_ID	0x43bc
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_d11.h b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_d11.h
index f9745ea8b3e0..f9745ea8b3e0 100644
--- a/drivers/net/wireless/brcm80211/include/brcmu_d11.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_d11.h
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_utils.h b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_utils.h
index 41969527b459..41969527b459 100644
--- a/drivers/net/wireless/brcm80211/include/brcmu_utils.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_utils.h
diff --git a/drivers/net/wireless/brcm80211/include/brcmu_wifi.h b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_wifi.h
index 76b5d3a86294..3f68dd5ecd11 100644
--- a/drivers/net/wireless/brcm80211/include/brcmu_wifi.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/brcmu_wifi.h
@@ -237,9 +237,6 @@ static inline bool ac_bitmap_tst(u8 bitmap, int prec)
 #define WPA2_AUTH_RESERVED4	0x0400
 #define WPA2_AUTH_RESERVED5	0x0800
 
-/* pmkid */
-#define	MAXPMKID		16
-
 #define DOT11_DEFAULT_RTS_LEN		2347
 #define DOT11_DEFAULT_FRAG_LEN		2346
 
@@ -251,24 +248,4 @@ static inline bool ac_bitmap_tst(u8 bitmap, int prec)
 #define HT_CAP_RX_STBC_NO		0x0
 #define HT_CAP_RX_STBC_ONE_STREAM	0x1
 
-struct pmkid {
-	u8 BSSID[ETH_ALEN];
-	u8 PMKID[WLAN_PMKID_LEN];
-};
-
-struct pmkid_list {
-	__le32 npmkid;
-	struct pmkid pmkid[1];
-};
-
-struct pmkid_cand {
-	u8 BSSID[ETH_ALEN];
-	u8 preauth;
-};
-
-struct pmkid_cand_list {
-	u32 npmkid_cand;
-	struct pmkid_cand pmkid_cand[1];
-};
-
 #endif				/* _BRCMU_WIFI_H_ */
diff --git a/drivers/net/wireless/brcm80211/include/chipcommon.h b/drivers/net/wireless/broadcom/brcm80211/include/chipcommon.h
index e1fd499930a0..e1fd499930a0 100644
--- a/drivers/net/wireless/brcm80211/include/chipcommon.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/chipcommon.h
diff --git a/drivers/net/wireless/brcm80211/include/defs.h b/drivers/net/wireless/broadcom/brcm80211/include/defs.h
index 8d1e85e0ed51..8d1e85e0ed51 100644
--- a/drivers/net/wireless/brcm80211/include/defs.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/defs.h
diff --git a/drivers/net/wireless/brcm80211/include/soc.h b/drivers/net/wireless/broadcom/brcm80211/include/soc.h
index 123cfa854a0d..123cfa854a0d 100644
--- a/drivers/net/wireless/brcm80211/include/soc.h
+++ b/drivers/net/wireless/broadcom/brcm80211/include/soc.h
diff --git a/drivers/net/wireless/cisco/Kconfig b/drivers/net/wireless/cisco/Kconfig
new file mode 100644
index 000000000000..b22567dff893
--- /dev/null
+++ b/drivers/net/wireless/cisco/Kconfig
@@ -0,0 +1,56 @@
+config WLAN_VENDOR_CISCO
+	bool "Cisco devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_CISCO
+
+config AIRO
+	tristate "Cisco/Aironet 34X/35X/4500/4800 ISA and PCI cards"
+	depends on CFG80211 && ISA_DMA_API && (PCI || BROKEN)
+	select WIRELESS_EXT
+	select CRYPTO
+	select WEXT_SPY
+	select WEXT_PRIV
+	---help---
+	  This is the standard Linux driver to support Cisco/Aironet ISA and
+	  PCI 802.11 wireless cards.
+	  It supports the new 802.11b cards from Cisco (Cisco 34X, Cisco 35X
+	  - with or without encryption) as well as card before the Cisco
+	  acquisition (Aironet 4500, Aironet 4800, Aironet 4800B).
+
+	  This driver support both the standard Linux Wireless Extensions
+	  and Cisco proprietary API, so both the Linux Wireless Tools and the
+	  Cisco Linux utilities can be used to configure the card.
+
+	  The driver can be compiled as a module and will be named "airo".
+
+config AIRO_CS
+	tristate "Cisco/Aironet 34X/35X/4500/4800 PCMCIA cards"
+	depends on CFG80211 && PCMCIA && (BROKEN || !M32R)
+	select WIRELESS_EXT
+	select WEXT_SPY
+	select WEXT_PRIV
+	select CRYPTO
+	select CRYPTO_AES
+	---help---
+	  This is the standard Linux driver to support Cisco/Aironet PCMCIA
+	  802.11 wireless cards.  This driver is the same as the Aironet
+	  driver part of the Linux Pcmcia package.
+	  It supports the new 802.11b cards from Cisco (Cisco 34X, Cisco 35X
+	  - with or without encryption) as well as card before the Cisco
+	  acquisition (Aironet 4500, Aironet 4800, Aironet 4800B). It also
+	  supports OEM of Cisco such as the DELL TrueMobile 4800 and Xircom
+	  802.11b cards.
+
+	  This driver support both the standard Linux Wireless Extensions
+	  and Cisco proprietary API, so both the Linux Wireless Tools and the
+	  Cisco Linux utilities can be used to configure the card.
+
+endif # WLAN_VENDOR_CISCO
diff --git a/drivers/net/wireless/cisco/Makefile b/drivers/net/wireless/cisco/Makefile
new file mode 100644
index 000000000000..d4110b19d6ef
--- /dev/null
+++ b/drivers/net/wireless/cisco/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_AIRO)		+= airo.o
+obj-$(CONFIG_AIRO_CS)		+= airo_cs.o airo.o
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/cisco/airo.c
index 17c40f06f13e..d2353f6e5214 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/cisco/airo.c
@@ -5137,21 +5137,9 @@ static void proc_APList_on_close( struct inode *inode, struct file *file ) {
 	memset(APList_rid, 0, sizeof(*APList_rid));
 	APList_rid->len = cpu_to_le16(sizeof(*APList_rid));
 
-	for( i = 0; i < 4 && data->writelen >= (i+1)*6*3; i++ ) {
-		int j;
-		for( j = 0; j < 6*3 && data->wbuffer[j+i*6*3]; j++ ) {
-			switch(j%3) {
-			case 0:
-				APList_rid->ap[i][j/3]=
-					hex_to_bin(data->wbuffer[j+i*6*3])<<4;
-				break;
-			case 1:
-				APList_rid->ap[i][j/3]|=
-					hex_to_bin(data->wbuffer[j+i*6*3]);
-				break;
-			}
-		}
-	}
+	for (i = 0; i < 4 && data->writelen >= (i + 1) * 6 * 3; i++)
+		mac_pton(data->wbuffer + i * 6 * 3, APList_rid->ap[i]);
+
 	disable_MAC(ai, 1);
 	writeAPListRid(ai, APList_rid, 1);
 	enable_MAC(ai, 1);
diff --git a/drivers/net/wireless/airo.h b/drivers/net/wireless/cisco/airo.h
index e480adf86be6..e480adf86be6 100644
--- a/drivers/net/wireless/airo.h
+++ b/drivers/net/wireless/cisco/airo.h
diff --git a/drivers/net/wireless/airo_cs.c b/drivers/net/wireless/cisco/airo_cs.c
index d9ed22b4cc6b..d9ed22b4cc6b 100644
--- a/drivers/net/wireless/airo_cs.c
+++ b/drivers/net/wireless/cisco/airo_cs.c
diff --git a/drivers/net/wireless/intel/Kconfig b/drivers/net/wireless/intel/Kconfig
new file mode 100644
index 000000000000..5b14f2f64a8a
--- /dev/null
+++ b/drivers/net/wireless/intel/Kconfig
@@ -0,0 +1,18 @@
+config WLAN_VENDOR_INTEL
+	bool "Intel devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_INTEL
+
+source "drivers/net/wireless/intel/ipw2x00/Kconfig"
+source "drivers/net/wireless/intel/iwlegacy/Kconfig"
+source "drivers/net/wireless/intel/iwlwifi/Kconfig"
+
+endif # WLAN_VENDOR_INTEL
diff --git a/drivers/net/wireless/intel/Makefile b/drivers/net/wireless/intel/Makefile
new file mode 100644
index 000000000000..c9cbcc85b569
--- /dev/null
+++ b/drivers/net/wireless/intel/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_IPW2100) += ipw2x00/
+obj-$(CONFIG_IPW2200) += ipw2x00/
+
+obj-$(CONFIG_IWLEGACY)	+= iwlegacy/
+
+obj-$(CONFIG_IWLWIFI)	+= iwlwifi/
diff --git a/drivers/net/wireless/ipw2x00/Kconfig b/drivers/net/wireless/intel/ipw2x00/Kconfig
index d6ec44d7a391..d6ec44d7a391 100644
--- a/drivers/net/wireless/ipw2x00/Kconfig
+++ b/drivers/net/wireless/intel/ipw2x00/Kconfig
diff --git a/drivers/net/wireless/ipw2x00/Makefile b/drivers/net/wireless/intel/ipw2x00/Makefile
index aecd2cff462b..aecd2cff462b 100644
--- a/drivers/net/wireless/ipw2x00/Makefile
+++ b/drivers/net/wireless/intel/ipw2x00/Makefile
diff --git a/drivers/net/wireless/ipw2x00/ipw.h b/drivers/net/wireless/intel/ipw2x00/ipw.h
index 4007bf5ed6f3..4007bf5ed6f3 100644
--- a/drivers/net/wireless/ipw2x00/ipw.h
+++ b/drivers/net/wireless/intel/ipw2x00/ipw.h
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/intel/ipw2x00/ipw2100.c
index 36818c7f30b9..f93a7f71c047 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2100.c
@@ -2311,8 +2311,10 @@ static int ipw2100_alloc_skb(struct ipw2100_priv *priv,
 	packet->dma_addr = pci_map_single(priv->pci_dev, packet->skb->data,
 					  sizeof(struct ipw2100_rx),
 					  PCI_DMA_FROMDEVICE);
-	/* NOTE: pci_map_single does not return an error code, and 0 is a valid
-	 *       dma_addr */
+	if (pci_dma_mapping_error(priv->pci_dev, packet->dma_addr)) {
+		dev_kfree_skb(packet->skb);
+		return -ENOMEM;
+	}
 
 	return 0;
 }
@@ -3183,6 +3185,11 @@ static void ipw2100_tx_send_data(struct ipw2100_priv *priv)
 							LIBIPW_3ADDR_LEN,
 							tbd->buf_length,
 							PCI_DMA_TODEVICE);
+			if (pci_dma_mapping_error(priv->pci_dev,
+						  tbd->host_addr)) {
+				IPW_DEBUG_TX("dma mapping error\n");
+				break;
+			}
 
 			IPW_DEBUG_TX("data frag tbd TX%d P=%08x L=%d\n",
 				     txq->next, tbd->host_addr,
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.h b/drivers/net/wireless/intel/ipw2x00/ipw2100.h
index 193947865efd..193947865efd 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.h
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2100.h
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/intel/ipw2x00/ipw2200.c
index ed0adaf1eec4..ed0adaf1eec4 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2200.c
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.h b/drivers/net/wireless/intel/ipw2x00/ipw2200.h
index aa301d1eee3c..aa301d1eee3c 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.h
+++ b/drivers/net/wireless/intel/ipw2x00/ipw2200.h
diff --git a/drivers/net/wireless/ipw2x00/libipw.h b/drivers/net/wireless/intel/ipw2x00/libipw.h
index b0571618c2ed..b0571618c2ed 100644
--- a/drivers/net/wireless/ipw2x00/libipw.h
+++ b/drivers/net/wireless/intel/ipw2x00/libipw.h
diff --git a/drivers/net/wireless/ipw2x00/libipw_geo.c b/drivers/net/wireless/intel/ipw2x00/libipw_geo.c
index 218f2a32de21..218f2a32de21 100644
--- a/drivers/net/wireless/ipw2x00/libipw_geo.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_geo.c
diff --git a/drivers/net/wireless/ipw2x00/libipw_module.c b/drivers/net/wireless/intel/ipw2x00/libipw_module.c
index 60f28740f6af..60f28740f6af 100644
--- a/drivers/net/wireless/ipw2x00/libipw_module.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_module.c
diff --git a/drivers/net/wireless/ipw2x00/libipw_rx.c b/drivers/net/wireless/intel/ipw2x00/libipw_rx.c
index cef7f7d79cd9..cef7f7d79cd9 100644
--- a/drivers/net/wireless/ipw2x00/libipw_rx.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_rx.c
diff --git a/drivers/net/wireless/ipw2x00/libipw_tx.c b/drivers/net/wireless/intel/ipw2x00/libipw_tx.c
index e8c039879b05..e8c039879b05 100644
--- a/drivers/net/wireless/ipw2x00/libipw_tx.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_tx.c
diff --git a/drivers/net/wireless/ipw2x00/libipw_wx.c b/drivers/net/wireless/intel/ipw2x00/libipw_wx.c
index dd29f46d086b..dd29f46d086b 100644
--- a/drivers/net/wireless/ipw2x00/libipw_wx.c
+++ b/drivers/net/wireless/intel/ipw2x00/libipw_wx.c
diff --git a/drivers/net/wireless/iwlegacy/3945-debug.c b/drivers/net/wireless/intel/iwlegacy/3945-debug.c
index c1b4441fb8b2..c1b4441fb8b2 100644
--- a/drivers/net/wireless/iwlegacy/3945-debug.c
+++ b/drivers/net/wireless/intel/iwlegacy/3945-debug.c
diff --git a/drivers/net/wireless/iwlegacy/3945-mac.c b/drivers/net/wireless/intel/iwlegacy/3945-mac.c
index af1b3e6839fa..af1b3e6839fa 100644
--- a/drivers/net/wireless/iwlegacy/3945-mac.c
+++ b/drivers/net/wireless/intel/iwlegacy/3945-mac.c
diff --git a/drivers/net/wireless/iwlegacy/3945-rs.c b/drivers/net/wireless/intel/iwlegacy/3945-rs.c
index 76b0729ade17..76b0729ade17 100644
--- a/drivers/net/wireless/iwlegacy/3945-rs.c
+++ b/drivers/net/wireless/intel/iwlegacy/3945-rs.c
diff --git a/drivers/net/wireless/iwlegacy/3945.c b/drivers/net/wireless/intel/iwlegacy/3945.c
index 93bdf684babe..93bdf684babe 100644
--- a/drivers/net/wireless/iwlegacy/3945.c
+++ b/drivers/net/wireless/intel/iwlegacy/3945.c
diff --git a/drivers/net/wireless/iwlegacy/3945.h b/drivers/net/wireless/intel/iwlegacy/3945.h
index 00030d43a194..00030d43a194 100644
--- a/drivers/net/wireless/iwlegacy/3945.h
+++ b/drivers/net/wireless/intel/iwlegacy/3945.h
diff --git a/drivers/net/wireless/iwlegacy/4965-calib.c b/drivers/net/wireless/intel/iwlegacy/4965-calib.c
index e78bdefb8952..e78bdefb8952 100644
--- a/drivers/net/wireless/iwlegacy/4965-calib.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965-calib.c
diff --git a/drivers/net/wireless/iwlegacy/4965-debug.c b/drivers/net/wireless/intel/iwlegacy/4965-debug.c
index e0597bfdddb8..e0597bfdddb8 100644
--- a/drivers/net/wireless/iwlegacy/4965-debug.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965-debug.c
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/intel/iwlegacy/4965-mac.c
index 6656215a13a9..fd38aa0763e4 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965-mac.c
@@ -6416,7 +6416,7 @@ il4965_hw_detect(struct il_priv *il)
 	D_INFO("HW Revision ID = 0x%X\n", il->rev_id);
 }
 
-static struct il_sensitivity_ranges il4965_sensitivity = {
+static const struct il_sensitivity_ranges il4965_sensitivity = {
 	.min_nrg_cck = 97,
 	.max_nrg_cck = 0,	/* not used, set to 0 */
 
diff --git a/drivers/net/wireless/iwlegacy/4965-rs.c b/drivers/net/wireless/intel/iwlegacy/4965-rs.c
index bac60b2bc3f0..bac60b2bc3f0 100644
--- a/drivers/net/wireless/iwlegacy/4965-rs.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965-rs.c
diff --git a/drivers/net/wireless/iwlegacy/4965.c b/drivers/net/wireless/intel/iwlegacy/4965.c
index fe47db9c20cd..fe47db9c20cd 100644
--- a/drivers/net/wireless/iwlegacy/4965.c
+++ b/drivers/net/wireless/intel/iwlegacy/4965.c
diff --git a/drivers/net/wireless/iwlegacy/4965.h b/drivers/net/wireless/intel/iwlegacy/4965.h
index 8ab8706f9422..8ab8706f9422 100644
--- a/drivers/net/wireless/iwlegacy/4965.h
+++ b/drivers/net/wireless/intel/iwlegacy/4965.h
diff --git a/drivers/net/wireless/iwlegacy/Kconfig b/drivers/net/wireless/intel/iwlegacy/Kconfig
index fb919727b8bb..fb919727b8bb 100644
--- a/drivers/net/wireless/iwlegacy/Kconfig
+++ b/drivers/net/wireless/intel/iwlegacy/Kconfig
diff --git a/drivers/net/wireless/iwlegacy/Makefile b/drivers/net/wireless/intel/iwlegacy/Makefile
index c985a01a0731..c985a01a0731 100644
--- a/drivers/net/wireless/iwlegacy/Makefile
+++ b/drivers/net/wireless/intel/iwlegacy/Makefile
diff --git a/drivers/net/wireless/iwlegacy/commands.h b/drivers/net/wireless/intel/iwlegacy/commands.h
index dd744135c956..dd744135c956 100644
--- a/drivers/net/wireless/iwlegacy/commands.h
+++ b/drivers/net/wireless/intel/iwlegacy/commands.h
diff --git a/drivers/net/wireless/iwlegacy/common.c b/drivers/net/wireless/intel/iwlegacy/common.c
index 887114582583..eb5cb603bc52 100644
--- a/drivers/net/wireless/iwlegacy/common.c
+++ b/drivers/net/wireless/intel/iwlegacy/common.c
@@ -1865,14 +1865,14 @@ il_send_add_sta(struct il_priv *il, struct il_addsta_cmd *sta, u8 flags)
 
 	cmd.len = il->ops->build_addsta_hcmd(sta, data);
 	ret = il_send_cmd(il, &cmd);
-
-	if (ret || (flags & CMD_ASYNC))
+	if (ret)
 		return ret;
+	if (flags & CMD_ASYNC)
+		return 0;
+
+	pkt = (struct il_rx_pkt *)cmd.reply_page;
+	ret = il_process_add_sta_resp(il, sta, pkt, true);
 
-	if (ret == 0) {
-		pkt = (struct il_rx_pkt *)cmd.reply_page;
-		ret = il_process_add_sta_resp(il, sta, pkt, true);
-	}
 	il_free_pages(il, cmd.reply_page);
 
 	return ret;
@@ -3602,7 +3602,7 @@ il_is_ht40_tx_allowed(struct il_priv *il, struct ieee80211_sta_ht_cap *ht_cap)
 }
 EXPORT_SYMBOL(il_is_ht40_tx_allowed);
 
-static u16
+static u16 noinline
 il_adjust_beacon_interval(u16 beacon_val, u16 max_beacon_val)
 {
 	u16 new_val;
diff --git a/drivers/net/wireless/iwlegacy/common.h b/drivers/net/wireless/intel/iwlegacy/common.h
index ce52cf114fde..ce52cf114fde 100644
--- a/drivers/net/wireless/iwlegacy/common.h
+++ b/drivers/net/wireless/intel/iwlegacy/common.h
diff --git a/drivers/net/wireless/iwlegacy/csr.h b/drivers/net/wireless/intel/iwlegacy/csr.h
index 9138e15004fa..9138e15004fa 100644
--- a/drivers/net/wireless/iwlegacy/csr.h
+++ b/drivers/net/wireless/intel/iwlegacy/csr.h
diff --git a/drivers/net/wireless/iwlegacy/debug.c b/drivers/net/wireless/intel/iwlegacy/debug.c
index 908b9f4fef6f..908b9f4fef6f 100644
--- a/drivers/net/wireless/iwlegacy/debug.c
+++ b/drivers/net/wireless/intel/iwlegacy/debug.c
diff --git a/drivers/net/wireless/iwlegacy/iwl-spectrum.h b/drivers/net/wireless/intel/iwlegacy/iwl-spectrum.h
index 85fe48e520f9..85fe48e520f9 100644
--- a/drivers/net/wireless/iwlegacy/iwl-spectrum.h
+++ b/drivers/net/wireless/intel/iwlegacy/iwl-spectrum.h
diff --git a/drivers/net/wireless/iwlegacy/prph.h b/drivers/net/wireless/intel/iwlegacy/prph.h
index ffec4b4a248a..ffec4b4a248a 100644
--- a/drivers/net/wireless/iwlegacy/prph.h
+++ b/drivers/net/wireless/intel/iwlegacy/prph.h
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/intel/iwlwifi/Kconfig
index 6e949df399d6..7438fbeef744 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/intel/iwlwifi/Kconfig
@@ -53,7 +53,6 @@ config IWLWIFI_LEDS
 
 config IWLDVM
 	tristate "Intel Wireless WiFi DVM Firmware support"
-	default IWLWIFI
 	help
 	  This is the driver that supports the DVM firmware. The list
 	  of the devices that use this firmware is available here:
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/intel/iwlwifi/Makefile
index dbfc5b18bcb7..05828c61d1ab 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/intel/iwlwifi/Makefile
@@ -8,7 +8,7 @@ iwlwifi-objs		+= iwl-eeprom-read.o iwl-eeprom-parse.o
 iwlwifi-objs		+= iwl-phy-db.o iwl-nvm-parse.o
 iwlwifi-objs		+= pcie/drv.o pcie/rx.o pcie/tx.o pcie/trans.o
 iwlwifi-$(CONFIG_IWLDVM) += iwl-1000.o iwl-2000.o iwl-5000.o iwl-6000.o
-iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o iwl-8000.o
+iwlwifi-$(CONFIG_IWLMVM) += iwl-7000.o iwl-8000.o iwl-9000.o
 iwlwifi-objs		+= iwl-trans.o
 
 iwlwifi-objs += $(iwlwifi-m)
diff --git a/drivers/net/wireless/iwlwifi/dvm/Makefile b/drivers/net/wireless/intel/iwlwifi/dvm/Makefile
index 4d19685f31c3..4d19685f31c3 100644
--- a/drivers/net/wireless/iwlwifi/dvm/Makefile
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/Makefile
diff --git a/drivers/net/wireless/iwlwifi/dvm/agn.h b/drivers/net/wireless/intel/iwlwifi/dvm/agn.h
index 991def878881..9de277c6c420 100644
--- a/drivers/net/wireless/iwlwifi/dvm/agn.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/agn.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -473,13 +473,4 @@ do {									\
 } while (0)
 #endif				/* CONFIG_IWLWIFI_DEBUG */
 
-extern const char *const iwl_dvm_cmd_strings[REPLY_MAX + 1];
-
-static inline const char *iwl_dvm_get_cmd_string(u8 cmd)
-{
-	const char *s = iwl_dvm_cmd_strings[cmd];
-	if (s)
-		return s;
-	return "UNKNOWN";
-}
 #endif /* __iwl_agn_h__ */
diff --git a/drivers/net/wireless/iwlwifi/dvm/calib.c b/drivers/net/wireless/intel/iwlwifi/dvm/calib.c
index 20e6aa910700..e9cef9de9ed8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/calib.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/calib.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -311,7 +311,7 @@ static int iwl_sens_energy_cck(struct iwl_priv *priv,
 		/* If previous beacon had too many false alarms,
 		 *   give it some extra margin by reducing sensitivity again
 		 *   (but don't go below measured energy of desired Rx) */
-		if (IWL_FA_TOO_MANY == data->nrg_prev_state) {
+		if (data->nrg_prev_state == IWL_FA_TOO_MANY) {
 			IWL_DEBUG_CALIB(priv, "... increasing margin\n");
 			if (data->nrg_th_cck > (max_nrg_cck + NRG_MARGIN))
 				data->nrg_th_cck -= NRG_MARGIN;
@@ -901,7 +901,7 @@ static void iwlagn_gain_computation(struct iwl_priv *priv,
 		/* bound gain by 2 bits value max, 3rd bit is sign */
 		data->delta_gain_code[i] =
 			min(abs(delta_g),
-			(long) CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
+			(s32) CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
 
 		if (delta_g < 0)
 			/*
diff --git a/drivers/net/wireless/iwlwifi/dvm/calib.h b/drivers/net/wireless/intel/iwlwifi/dvm/calib.h
index aeae4e80ea40..099e3ce80ffc 100644
--- a/drivers/net/wireless/iwlwifi/dvm/calib.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/calib.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/dvm/commands.h b/drivers/net/wireless/intel/iwlwifi/dvm/commands.h
index 7a34e4d158d1..2ab2773655a8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/commands.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/commands.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/dvm/debugfs.c b/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c
index b15e44f8d1bd..74c51615244e 100644
--- a/drivers/net/wireless/iwlwifi/dvm/debugfs.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/debugfs.c
@@ -22,7 +22,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 
@@ -32,7 +32,9 @@
 #include <linux/debugfs.h>
 #include <linux/ieee80211.h>
 #include <net/mac80211.h>
+
 #include "iwl-debug.h"
+#include "iwl-trans.h"
 #include "iwl-io.h"
 #include "dev.h"
 #include "agn.h"
@@ -438,7 +440,7 @@ static ssize_t iwl_dbgfs_rx_handlers_read(struct file *file,
 		if (priv->rx_handlers_stats[cnt] > 0)
 			pos += scnprintf(buf + pos, bufsz - pos,
 				"\tRx handler[%36s]:\t\t %u\n",
-				iwl_dvm_get_cmd_string(cnt),
+				iwl_get_cmd_string(priv->trans, (u32)cnt),
 				priv->rx_handlers_stats[cnt]);
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/dev.h b/drivers/net/wireless/intel/iwlwifi/dvm/dev.h
index 0ba3e56d6015..1a7ead753eee 100644
--- a/drivers/net/wireless/iwlwifi/dvm/dev.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/dev.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/devices.c b/drivers/net/wireless/intel/iwlwifi/dvm/devices.c
index 34b41e5f7cfc..cc13c04063a5 100644
--- a/drivers/net/wireless/iwlwifi/dvm/devices.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/devices.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/led.c b/drivers/net/wireless/intel/iwlwifi/dvm/led.c
index ca4d6692cc4e..1aabb5ec096f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/led.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/led.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -134,8 +134,6 @@ static int iwl_led_cmd(struct iwl_priv *priv,
 		on = IWL_LED_SOLID;
 	}
 
-	IWL_DEBUG_LED(priv, "Led blink time compensation=%u\n",
-			priv->cfg->base_params->led_compensation);
 	led_cmd.on = iwl_blink_compensation(priv, on,
 				priv->cfg->base_params->led_compensation);
 	led_cmd.off = iwl_blink_compensation(priv, off,
diff --git a/drivers/net/wireless/iwlwifi/dvm/led.h b/drivers/net/wireless/intel/iwlwifi/dvm/led.h
index 1c6b2252d0f2..75f74edd018f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/led.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/led.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/lib.c b/drivers/net/wireless/intel/iwlwifi/dvm/lib.c
index e18629a16fb0..4841be2aa499 100644
--- a/drivers/net/wireless/iwlwifi/dvm/lib.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/lib.c
@@ -22,7 +22,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -1154,6 +1154,9 @@ int iwlagn_suspend(struct iwl_priv *priv, struct cfg80211_wowlan *wowlan)
 
 	priv->ucode_loaded = false;
 	iwl_trans_stop_device(priv->trans);
+	ret = iwl_trans_start_hw(priv->trans);
+	if (ret)
+		goto out;
 
 	priv->wowlan = true;
 
@@ -1262,7 +1265,7 @@ int iwl_dvm_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 
 	if (test_bit(STATUS_FW_ERROR, &priv->status)) {
 		IWL_ERR(priv, "Command %s failed: FW Error\n",
-			iwl_dvm_get_cmd_string(cmd->id));
+			iwl_get_cmd_string(priv->trans, cmd->id));
 		return -EIO;
 	}
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c
index b3ad34e8bf5a..29ea1c6705b4 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/mac80211.c
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -115,6 +115,9 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
 	ieee80211_hw_set(hw, WANT_MONITOR_VIF);
 
+	if (priv->trans->max_skb_frags)
+		hw->netdev_features = NETIF_F_HIGHDMA | NETIF_F_SG;
+
 	hw->offchannel_tx_hw_queue = IWL_AUX_QUEUE;
 	hw->radiotap_mcs_details |= IEEE80211_RADIOTAP_MCS_HAVE_FMT;
 
@@ -1411,13 +1414,7 @@ static void iwlagn_mac_remove_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&priv->mutex);
 
-	if (WARN_ON(ctx->vif != vif)) {
-		struct iwl_rxon_context *tmp;
-		IWL_ERR(priv, "ctx->vif = %p, vif = %p\n", ctx->vif, vif);
-		for_each_context(priv, tmp)
-			IWL_ERR(priv, "\tID = %d:\tctx = %p\tctx->vif = %p\n",
-				tmp->ctxid, tmp, tmp->vif);
-	}
+	WARN_ON(ctx->vif != vif);
 	ctx->vif = NULL;
 
 	iwl_teardown_interface(priv, vif, false);
diff --git a/drivers/net/wireless/iwlwifi/dvm/main.c b/drivers/net/wireless/intel/iwlwifi/dvm/main.c
index e7616f0ee6e8..f62c2d727ddb 100644
--- a/drivers/net/wireless/iwlwifi/dvm/main.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/main.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -22,7 +23,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -69,6 +70,93 @@ MODULE_DESCRIPTION(DRV_DESCRIPTION);
 MODULE_AUTHOR(DRV_COPYRIGHT " " DRV_AUTHOR);
 MODULE_LICENSE("GPL");
 
+/* Please keep this array *SORTED* by hex value.
+ * Access is done through binary search.
+ * A warning will be triggered on violation.
+ */
+static const struct iwl_hcmd_names iwl_dvm_cmd_names[] = {
+	HCMD_NAME(REPLY_ALIVE),
+	HCMD_NAME(REPLY_ERROR),
+	HCMD_NAME(REPLY_ECHO),
+	HCMD_NAME(REPLY_RXON),
+	HCMD_NAME(REPLY_RXON_ASSOC),
+	HCMD_NAME(REPLY_QOS_PARAM),
+	HCMD_NAME(REPLY_RXON_TIMING),
+	HCMD_NAME(REPLY_ADD_STA),
+	HCMD_NAME(REPLY_REMOVE_STA),
+	HCMD_NAME(REPLY_REMOVE_ALL_STA),
+	HCMD_NAME(REPLY_TX),
+	HCMD_NAME(REPLY_TXFIFO_FLUSH),
+	HCMD_NAME(REPLY_WEPKEY),
+	HCMD_NAME(REPLY_LEDS_CMD),
+	HCMD_NAME(REPLY_TX_LINK_QUALITY_CMD),
+	HCMD_NAME(COEX_PRIORITY_TABLE_CMD),
+	HCMD_NAME(COEX_MEDIUM_NOTIFICATION),
+	HCMD_NAME(COEX_EVENT_CMD),
+	HCMD_NAME(TEMPERATURE_NOTIFICATION),
+	HCMD_NAME(CALIBRATION_CFG_CMD),
+	HCMD_NAME(CALIBRATION_RES_NOTIFICATION),
+	HCMD_NAME(CALIBRATION_COMPLETE_NOTIFICATION),
+	HCMD_NAME(REPLY_QUIET_CMD),
+	HCMD_NAME(REPLY_CHANNEL_SWITCH),
+	HCMD_NAME(CHANNEL_SWITCH_NOTIFICATION),
+	HCMD_NAME(REPLY_SPECTRUM_MEASUREMENT_CMD),
+	HCMD_NAME(SPECTRUM_MEASURE_NOTIFICATION),
+	HCMD_NAME(POWER_TABLE_CMD),
+	HCMD_NAME(PM_SLEEP_NOTIFICATION),
+	HCMD_NAME(PM_DEBUG_STATISTIC_NOTIFIC),
+	HCMD_NAME(REPLY_SCAN_CMD),
+	HCMD_NAME(REPLY_SCAN_ABORT_CMD),
+	HCMD_NAME(SCAN_START_NOTIFICATION),
+	HCMD_NAME(SCAN_RESULTS_NOTIFICATION),
+	HCMD_NAME(SCAN_COMPLETE_NOTIFICATION),
+	HCMD_NAME(BEACON_NOTIFICATION),
+	HCMD_NAME(REPLY_TX_BEACON),
+	HCMD_NAME(WHO_IS_AWAKE_NOTIFICATION),
+	HCMD_NAME(REPLY_TX_POWER_DBM_CMD),
+	HCMD_NAME(QUIET_NOTIFICATION),
+	HCMD_NAME(REPLY_TX_PWR_TABLE_CMD),
+	HCMD_NAME(REPLY_TX_POWER_DBM_CMD_V1),
+	HCMD_NAME(TX_ANT_CONFIGURATION_CMD),
+	HCMD_NAME(MEASURE_ABORT_NOTIFICATION),
+	HCMD_NAME(REPLY_BT_CONFIG),
+	HCMD_NAME(REPLY_STATISTICS_CMD),
+	HCMD_NAME(STATISTICS_NOTIFICATION),
+	HCMD_NAME(REPLY_CARD_STATE_CMD),
+	HCMD_NAME(CARD_STATE_NOTIFICATION),
+	HCMD_NAME(MISSED_BEACONS_NOTIFICATION),
+	HCMD_NAME(REPLY_CT_KILL_CONFIG_CMD),
+	HCMD_NAME(SENSITIVITY_CMD),
+	HCMD_NAME(REPLY_PHY_CALIBRATION_CMD),
+	HCMD_NAME(REPLY_WIPAN_PARAMS),
+	HCMD_NAME(REPLY_WIPAN_RXON),
+	HCMD_NAME(REPLY_WIPAN_RXON_TIMING),
+	HCMD_NAME(REPLY_WIPAN_RXON_ASSOC),
+	HCMD_NAME(REPLY_WIPAN_QOS_PARAM),
+	HCMD_NAME(REPLY_WIPAN_WEPKEY),
+	HCMD_NAME(REPLY_WIPAN_P2P_CHANNEL_SWITCH),
+	HCMD_NAME(REPLY_WIPAN_NOA_NOTIFICATION),
+	HCMD_NAME(REPLY_WIPAN_DEACTIVATION_COMPLETE),
+	HCMD_NAME(REPLY_RX_PHY_CMD),
+	HCMD_NAME(REPLY_RX_MPDU_CMD),
+	HCMD_NAME(REPLY_RX),
+	HCMD_NAME(REPLY_COMPRESSED_BA),
+	HCMD_NAME(REPLY_BT_COEX_PRIO_TABLE),
+	HCMD_NAME(REPLY_BT_COEX_PROT_ENV),
+	HCMD_NAME(REPLY_BT_COEX_PROFILE_NOTIF),
+	HCMD_NAME(REPLY_D3_CONFIG),
+	HCMD_NAME(REPLY_WOWLAN_PATTERNS),
+	HCMD_NAME(REPLY_WOWLAN_WAKEUP_FILTER),
+	HCMD_NAME(REPLY_WOWLAN_TSC_RSC_PARAMS),
+	HCMD_NAME(REPLY_WOWLAN_TKIP_PARAMS),
+	HCMD_NAME(REPLY_WOWLAN_KEK_KCK_MATERIAL),
+	HCMD_NAME(REPLY_WOWLAN_GET_STATUS),
+};
+
+static const struct iwl_hcmd_arr iwl_dvm_groups[] = {
+	[0x0] = HCMD_ARR(iwl_dvm_cmd_names),
+};
+
 static const struct iwl_op_mode_ops iwl_dvm_ops;
 
 void iwl_update_chain_flags(struct iwl_priv *priv)
@@ -341,7 +429,7 @@ static void iwl_print_cont_event_trace(struct iwl_priv *priv, u32 base,
 		ptr = base + (4 * sizeof(u32)) + (start_idx * 3 * sizeof(u32));
 
 	/* Make sure device is powered up for SRAM reads */
-	if (!iwl_trans_grab_nic_access(priv->trans, false, &reg_flags))
+	if (!iwl_trans_grab_nic_access(priv->trans, &reg_flags))
 		return;
 
 	/* Set starting address; reads will auto-increment */
@@ -1227,10 +1315,26 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 	trans_cfg.op_mode = op_mode;
 	trans_cfg.no_reclaim_cmds = no_reclaim_cmds;
 	trans_cfg.n_no_reclaim_cmds = ARRAY_SIZE(no_reclaim_cmds);
-	trans_cfg.rx_buf_size_8k = iwlwifi_mod_params.amsdu_size_8K;
+
+	switch (iwlwifi_mod_params.amsdu_size) {
+	case IWL_AMSDU_4K:
+		trans_cfg.rx_buf_size = IWL_AMSDU_4K;
+		break;
+	case IWL_AMSDU_8K:
+		trans_cfg.rx_buf_size = IWL_AMSDU_8K;
+		break;
+	case IWL_AMSDU_12K:
+	default:
+		trans_cfg.rx_buf_size = IWL_AMSDU_4K;
+		pr_err("Unsupported amsdu_size: %d\n",
+		       iwlwifi_mod_params.amsdu_size);
+	}
+
 	trans_cfg.cmd_q_wdg_timeout = IWL_WATCHDOG_DISABLED;
 
-	trans_cfg.command_names = iwl_dvm_cmd_strings;
+	trans_cfg.command_groups = iwl_dvm_groups;
+	trans_cfg.command_groups_size = ARRAY_SIZE(iwl_dvm_groups);
+
 	trans_cfg.cmd_fifo = IWLAGN_CMD_FIFO_NUM;
 
 	WARN_ON(sizeof(priv->transport_queue_stop) * BITS_PER_BYTE <
@@ -1251,6 +1355,8 @@ static struct iwl_op_mode *iwl_op_mode_dvm_start(struct iwl_trans *trans,
 
 	trans->rx_mpdu_cmd = REPLY_RX_MPDU_CMD;
 	trans->rx_mpdu_cmd_hdr_size = sizeof(struct iwl_rx_mpdu_res_start);
+	trans->command_groups = trans_cfg.command_groups;
+	trans->command_groups_size = trans_cfg.command_groups_size;
 
 	/* At this point both hw and priv are allocated. */
 
@@ -1625,7 +1731,7 @@ static int iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
 	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
 
 	/* Make sure device is powered up for SRAM reads */
-	if (!iwl_trans_grab_nic_access(trans, false, &reg_flags))
+	if (!iwl_trans_grab_nic_access(trans, &reg_flags))
 		return pos;
 
 	/* Set starting address; reads will auto-increment */
diff --git a/drivers/net/wireless/iwlwifi/dvm/power.c b/drivers/net/wireless/intel/iwlwifi/dvm/power.c
index 1513dbc79c14..0ad557c89514 100644
--- a/drivers/net/wireless/iwlwifi/dvm/power.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/power.c
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/power.h b/drivers/net/wireless/intel/iwlwifi/dvm/power.h
index 570d3a5e4670..2fd9b43adafd 100644
--- a/drivers/net/wireless/iwlwifi/dvm/power.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/power.h
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 #ifndef __iwl_power_setting_h__
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.c b/drivers/net/wireless/intel/iwlwifi/dvm/rs.c
index cef921c1a623..ee7505537c96 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rs.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/rs.h b/drivers/net/wireless/intel/iwlwifi/dvm/rs.h
index f6bd25cad203..c5fe44584613 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rs.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rs.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/rx.c b/drivers/net/wireless/intel/iwlwifi/dvm/rx.c
index 4a45b0b594c7..52ab1e012e8f 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rx.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portionhelp of the ieee80211 subsystem header files.
@@ -22,7 +23,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -32,91 +33,13 @@
 #include <linux/sched.h>
 #include <net/mac80211.h>
 #include <asm/unaligned.h>
+
+#include "iwl-trans.h"
 #include "iwl-io.h"
 #include "dev.h"
 #include "calib.h"
 #include "agn.h"
 
-#define IWL_CMD_ENTRY(x) [x] = #x
-
-const char *const iwl_dvm_cmd_strings[REPLY_MAX + 1] = {
-	IWL_CMD_ENTRY(REPLY_ALIVE),
-	IWL_CMD_ENTRY(REPLY_ERROR),
-	IWL_CMD_ENTRY(REPLY_ECHO),
-	IWL_CMD_ENTRY(REPLY_RXON),
-	IWL_CMD_ENTRY(REPLY_RXON_ASSOC),
-	IWL_CMD_ENTRY(REPLY_QOS_PARAM),
-	IWL_CMD_ENTRY(REPLY_RXON_TIMING),
-	IWL_CMD_ENTRY(REPLY_ADD_STA),
-	IWL_CMD_ENTRY(REPLY_REMOVE_STA),
-	IWL_CMD_ENTRY(REPLY_REMOVE_ALL_STA),
-	IWL_CMD_ENTRY(REPLY_TXFIFO_FLUSH),
-	IWL_CMD_ENTRY(REPLY_WEPKEY),
-	IWL_CMD_ENTRY(REPLY_TX),
-	IWL_CMD_ENTRY(REPLY_LEDS_CMD),
-	IWL_CMD_ENTRY(REPLY_TX_LINK_QUALITY_CMD),
-	IWL_CMD_ENTRY(COEX_PRIORITY_TABLE_CMD),
-	IWL_CMD_ENTRY(COEX_MEDIUM_NOTIFICATION),
-	IWL_CMD_ENTRY(COEX_EVENT_CMD),
-	IWL_CMD_ENTRY(REPLY_QUIET_CMD),
-	IWL_CMD_ENTRY(REPLY_CHANNEL_SWITCH),
-	IWL_CMD_ENTRY(CHANNEL_SWITCH_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_SPECTRUM_MEASUREMENT_CMD),
-	IWL_CMD_ENTRY(SPECTRUM_MEASURE_NOTIFICATION),
-	IWL_CMD_ENTRY(POWER_TABLE_CMD),
-	IWL_CMD_ENTRY(PM_SLEEP_NOTIFICATION),
-	IWL_CMD_ENTRY(PM_DEBUG_STATISTIC_NOTIFIC),
-	IWL_CMD_ENTRY(REPLY_SCAN_CMD),
-	IWL_CMD_ENTRY(REPLY_SCAN_ABORT_CMD),
-	IWL_CMD_ENTRY(SCAN_START_NOTIFICATION),
-	IWL_CMD_ENTRY(SCAN_RESULTS_NOTIFICATION),
-	IWL_CMD_ENTRY(SCAN_COMPLETE_NOTIFICATION),
-	IWL_CMD_ENTRY(BEACON_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_TX_BEACON),
-	IWL_CMD_ENTRY(WHO_IS_AWAKE_NOTIFICATION),
-	IWL_CMD_ENTRY(QUIET_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_TX_PWR_TABLE_CMD),
-	IWL_CMD_ENTRY(MEASURE_ABORT_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_BT_CONFIG),
-	IWL_CMD_ENTRY(REPLY_STATISTICS_CMD),
-	IWL_CMD_ENTRY(STATISTICS_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_CARD_STATE_CMD),
-	IWL_CMD_ENTRY(CARD_STATE_NOTIFICATION),
-	IWL_CMD_ENTRY(MISSED_BEACONS_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_CT_KILL_CONFIG_CMD),
-	IWL_CMD_ENTRY(SENSITIVITY_CMD),
-	IWL_CMD_ENTRY(REPLY_PHY_CALIBRATION_CMD),
-	IWL_CMD_ENTRY(REPLY_RX_PHY_CMD),
-	IWL_CMD_ENTRY(REPLY_RX_MPDU_CMD),
-	IWL_CMD_ENTRY(REPLY_COMPRESSED_BA),
-	IWL_CMD_ENTRY(CALIBRATION_CFG_CMD),
-	IWL_CMD_ENTRY(CALIBRATION_RES_NOTIFICATION),
-	IWL_CMD_ENTRY(CALIBRATION_COMPLETE_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_TX_POWER_DBM_CMD),
-	IWL_CMD_ENTRY(TEMPERATURE_NOTIFICATION),
-	IWL_CMD_ENTRY(TX_ANT_CONFIGURATION_CMD),
-	IWL_CMD_ENTRY(REPLY_BT_COEX_PROFILE_NOTIF),
-	IWL_CMD_ENTRY(REPLY_BT_COEX_PRIO_TABLE),
-	IWL_CMD_ENTRY(REPLY_BT_COEX_PROT_ENV),
-	IWL_CMD_ENTRY(REPLY_WIPAN_PARAMS),
-	IWL_CMD_ENTRY(REPLY_WIPAN_RXON),
-	IWL_CMD_ENTRY(REPLY_WIPAN_RXON_TIMING),
-	IWL_CMD_ENTRY(REPLY_WIPAN_RXON_ASSOC),
-	IWL_CMD_ENTRY(REPLY_WIPAN_QOS_PARAM),
-	IWL_CMD_ENTRY(REPLY_WIPAN_WEPKEY),
-	IWL_CMD_ENTRY(REPLY_WIPAN_P2P_CHANNEL_SWITCH),
-	IWL_CMD_ENTRY(REPLY_WIPAN_NOA_NOTIFICATION),
-	IWL_CMD_ENTRY(REPLY_WIPAN_DEACTIVATION_COMPLETE),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_PATTERNS),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_WAKEUP_FILTER),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_TSC_RSC_PARAMS),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_TKIP_PARAMS),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_KEK_KCK_MATERIAL),
-	IWL_CMD_ENTRY(REPLY_WOWLAN_GET_STATUS),
-	IWL_CMD_ENTRY(REPLY_D3_CONFIG),
-};
-#undef IWL_CMD_ENTRY
-
 /******************************************************************************
  *
  * Generic RX handler implementations
@@ -1095,7 +1018,9 @@ void iwl_rx_dispatch(struct iwl_op_mode *op_mode, struct napi_struct *napi,
 	} else {
 		/* No handling needed */
 		IWL_DEBUG_RX(priv, "No handler needed for %s, 0x%02x\n",
-			     iwl_dvm_get_cmd_string(pkt->hdr.cmd),
+			     iwl_get_cmd_string(priv->trans,
+						iwl_cmd_id(pkt->hdr.cmd,
+							   0, 0)),
 			     pkt->hdr.cmd);
 	}
 }
diff --git a/drivers/net/wireless/iwlwifi/dvm/rxon.c b/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c
index 85ceceb34fcc..2d47cb24c48b 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rxon.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/rxon.c
@@ -20,7 +20,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- * Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/scan.c b/drivers/net/wireless/intel/iwlwifi/dvm/scan.c
index 648159495bbc..81a2ddbe9569 100644
--- a/drivers/net/wireless/iwlwifi/dvm/scan.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/scan.c
@@ -22,7 +22,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 #include <linux/slab.h>
diff --git a/drivers/net/wireless/iwlwifi/dvm/sta.c b/drivers/net/wireless/intel/iwlwifi/dvm/sta.c
index 0fa67d3b7235..8e9768a553e4 100644
--- a/drivers/net/wireless/iwlwifi/dvm/sta.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/sta.c
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/dvm/tt.c b/drivers/net/wireless/intel/iwlwifi/dvm/tt.c
index c4736c8834c5..5b73492e7ff7 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tt.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/tt.c
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 
@@ -184,7 +184,7 @@ static void iwl_tt_check_exit_ct_kill(unsigned long data)
 			priv->thermal_throttle.ct_kill_toggle = true;
 		}
 		iwl_read32(priv->trans, CSR_UCODE_DRV_GP1);
-		if (iwl_trans_grab_nic_access(priv->trans, false, &flags))
+		if (iwl_trans_grab_nic_access(priv->trans, &flags))
 			iwl_trans_release_nic_access(priv->trans, &flags);
 
 		/* Reschedule the ct_kill timer to occur in
diff --git a/drivers/net/wireless/iwlwifi/dvm/tt.h b/drivers/net/wireless/intel/iwlwifi/dvm/tt.h
index 507726534b84..d324e9be9cbf 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tt.h
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/tt.h
@@ -22,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *****************************************************************************/
 #ifndef __iwl_tt_setting_h__
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/intel/iwlwifi/dvm/tx.c
index bddd19769035..59e2001c39f8 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/tx.c
@@ -22,7 +22,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -383,6 +383,7 @@ int iwlagn_tx_skb(struct iwl_priv *priv,
 	iwlagn_tx_cmd_build_rate(priv, tx_cmd, info, sta, fc);
 
 	memset(&info->status, 0, sizeof(info->status));
+	memset(info->driver_data, 0, sizeof(info->driver_data));
 
 	info->driver_data[0] = ctx;
 	info->driver_data[1] = dev_cmd;
diff --git a/drivers/net/wireless/iwlwifi/dvm/ucode.c b/drivers/net/wireless/intel/iwlwifi/dvm/ucode.c
index 931a8e4269ef..b662cf35b033 100644
--- a/drivers/net/wireless/iwlwifi/dvm/ucode.c
+++ b/drivers/net/wireless/intel/iwlwifi/dvm/ucode.c
@@ -23,7 +23,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/intel/iwlwifi/iwl-1000.c
index 06f6cc08f451..a90dbab6bbbe 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-1000.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/intel/iwlwifi/iwl-2000.c
index 890b95f497d6..a6da9594c4a5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-2000.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/intel/iwlwifi/iwl-5000.c
index 724194e23414..8b5afdef2d83 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-5000.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/intel/iwlwifi/iwl-6000.c
index 21b2630763dc..0b4ba781b631 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-6000.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/intel/iwlwifi/iwl-7000.c
index d9a4aee246a6..fa41a5e1c890 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-7000.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -70,18 +72,21 @@
 
 /* Highest firmware API version supported */
 #define IWL7260_UCODE_API_MAX	17
-#define IWL7265_UCODE_API_MAX	19
-#define IWL7265D_UCODE_API_MAX	19
+#define IWL7265_UCODE_API_MAX	17
+#define IWL7265D_UCODE_API_MAX	20
+#define IWL3168_UCODE_API_MAX	20
 
 /* Oldest version we won't warn about */
 #define IWL7260_UCODE_API_OK	13
 #define IWL7265_UCODE_API_OK	13
 #define IWL7265D_UCODE_API_OK	13
+#define IWL3168_UCODE_API_OK	20
 
 /* Lowest firmware API version supported */
 #define IWL7260_UCODE_API_MIN	13
 #define IWL7265_UCODE_API_MIN	13
 #define IWL7265D_UCODE_API_MIN	13
+#define IWL3168_UCODE_API_MIN	20
 
 /* NVM versions */
 #define IWL7260_NVM_VERSION		0x0a1d
@@ -90,6 +95,8 @@
 #define IWL3160_TX_POWER_VERSION	0xffff /* meaningless */
 #define IWL3165_NVM_VERSION		0x709
 #define IWL3165_TX_POWER_VERSION	0xffff /* meaningless */
+#define IWL3168_NVM_VERSION		0xd01
+#define IWL3168_TX_POWER_VERSION	0xffff /* meaningless */
 #define IWL7265_NVM_VERSION		0x0a1d
 #define IWL7265_TX_POWER_VERSION	0xffff /* meaningless */
 #define IWL7265D_NVM_VERSION		0x0c11
@@ -107,6 +114,9 @@
 #define IWL3160_FW_PRE "iwlwifi-3160-"
 #define IWL3160_MODULE_FIRMWARE(api) IWL3160_FW_PRE __stringify(api) ".ucode"
 
+#define IWL3168_FW_PRE "iwlwifi-3168-"
+#define IWL3168_MODULE_FIRMWARE(api) IWL3168_FW_PRE __stringify(api) ".ucode"
+
 #define IWL7265_FW_PRE "iwlwifi-7265-"
 #define IWL7265_MODULE_FIRMWARE(api) IWL7265_FW_PRE __stringify(api) ".ucode"
 
@@ -178,6 +188,12 @@ static const struct iwl_ht_params iwl7000_ht_params = {
 	.ucode_api_ok = IWL7265_UCODE_API_OK,			\
 	.ucode_api_min = IWL7265_UCODE_API_MIN
 
+#define IWL_DEVICE_3008						\
+	IWL_DEVICE_7000_COMMON,					\
+	.ucode_api_max = IWL3168_UCODE_API_MAX,			\
+	.ucode_api_ok = IWL3168_UCODE_API_OK,			\
+	.ucode_api_min = IWL3168_UCODE_API_MIN
+
 #define IWL_DEVICE_7005D					\
 	IWL_DEVICE_7000_COMMON,					\
 	.ucode_api_max = IWL7265D_UCODE_API_MAX,		\
@@ -295,6 +311,17 @@ const struct iwl_cfg iwl3165_2ac_cfg = {
 	.dccm_len = IWL7265_DCCM_LEN,
 };
 
+const struct iwl_cfg iwl3168_2ac_cfg = {
+	.name = "Intel(R) Dual Band Wireless AC 3168",
+	.fw_name_pre = IWL3168_FW_PRE,
+	IWL_DEVICE_3008,
+	.ht_params = &iwl7000_ht_params,
+	.nvm_ver = IWL3168_NVM_VERSION,
+	.nvm_calib_ver = IWL3168_TX_POWER_VERSION,
+	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+	.dccm_len = IWL7265_DCCM_LEN,
+};
+
 const struct iwl_cfg iwl7265_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 7265",
 	.fw_name_pre = IWL7265_FW_PRE,
@@ -363,5 +390,6 @@ const struct iwl_cfg iwl7265d_n_cfg = {
 
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
 MODULE_FIRMWARE(IWL3160_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
+MODULE_FIRMWARE(IWL3168_MODULE_FIRMWARE(IWL3168_UCODE_API_OK));
 MODULE_FIRMWARE(IWL7265_MODULE_FIRMWARE(IWL7265_UCODE_API_OK));
 MODULE_FIRMWARE(IWL7265D_MODULE_FIRMWARE(IWL7265D_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/intel/iwlwifi/iwl-8000.c
index 9bcc0bf937d8..bce9b3420a13 100644
--- a/drivers/net/wireless/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-8000.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2014 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -69,13 +70,16 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL8000_UCODE_API_MAX	19
+#define IWL8000_UCODE_API_MAX	20
+#define IWL8265_UCODE_API_MAX	20
 
 /* Oldest version we won't warn about */
 #define IWL8000_UCODE_API_OK	13
+#define IWL8265_UCODE_API_OK	20
 
 /* Lowest firmware API version supported */
 #define IWL8000_UCODE_API_MIN	13
+#define IWL8265_UCODE_API_MIN	20
 
 /* NVM versions */
 #define IWL8000_NVM_VERSION		0x0a1d
@@ -93,6 +97,10 @@
 #define IWL8000_MODULE_FIRMWARE(api) \
 	IWL8000_FW_PRE "-" __stringify(api) ".ucode"
 
+#define IWL8265_FW_PRE "iwlwifi-8265-"
+#define IWL8265_MODULE_FIRMWARE(api) \
+	IWL8265_FW_PRE __stringify(api) ".ucode"
+
 #define NVM_HW_SECTION_NUM_FAMILY_8000		10
 #define DEFAULT_NVM_FILE_FAMILY_8000B		"nvmData-8000B"
 #define DEFAULT_NVM_FILE_FAMILY_8000C		"nvmData-8000C"
@@ -144,17 +152,13 @@ static const struct iwl_tt_params iwl8000_tt_params = {
 	.support_tx_backoff = true,
 };
 
-#define IWL_DEVICE_8000							\
-	.ucode_api_max = IWL8000_UCODE_API_MAX,				\
-	.ucode_api_ok = IWL8000_UCODE_API_OK,				\
-	.ucode_api_min = IWL8000_UCODE_API_MIN,				\
+#define IWL_DEVICE_8000_COMMON						\
 	.device_family = IWL_DEVICE_FAMILY_8000,			\
 	.max_inst_size = IWL60_RTC_INST_SIZE,				\
 	.max_data_size = IWL60_RTC_DATA_SIZE,				\
 	.base_params = &iwl8000_base_params,				\
 	.led_mode = IWL_LED_RF_STATE,					\
 	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000,		\
-	.d0i3 = true,							\
 	.features = NETIF_F_RXCSUM,					\
 	.non_shared_ant = ANT_A,					\
 	.dccm_offset = IWL8260_DCCM_OFFSET,				\
@@ -168,10 +172,28 @@ static const struct iwl_tt_params iwl8000_tt_params = {
 	.thermal_params = &iwl8000_tt_params,				\
 	.apmg_not_supported = true
 
+#define IWL_DEVICE_8000							\
+	IWL_DEVICE_8000_COMMON,						\
+	.ucode_api_max = IWL8000_UCODE_API_MAX,				\
+	.ucode_api_ok = IWL8000_UCODE_API_OK,				\
+	.ucode_api_min = IWL8000_UCODE_API_MIN				\
+
+#define IWL_DEVICE_8260							\
+	IWL_DEVICE_8000_COMMON,						\
+	.ucode_api_max = IWL8000_UCODE_API_MAX,				\
+	.ucode_api_ok = IWL8000_UCODE_API_OK,				\
+	.ucode_api_min = IWL8000_UCODE_API_MIN				\
+
+#define IWL_DEVICE_8265							\
+	IWL_DEVICE_8000_COMMON,						\
+	.ucode_api_max = IWL8265_UCODE_API_MAX,				\
+	.ucode_api_ok = IWL8265_UCODE_API_OK,				\
+	.ucode_api_min = IWL8265_UCODE_API_MIN				\
+
 const struct iwl_cfg iwl8260_2n_cfg = {
 	.name = "Intel(R) Dual Band Wireless N 8260",
 	.fw_name_pre = IWL8000_FW_PRE,
-	IWL_DEVICE_8000,
+	IWL_DEVICE_8260,
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
@@ -180,7 +202,17 @@ const struct iwl_cfg iwl8260_2n_cfg = {
 const struct iwl_cfg iwl8260_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 8260",
 	.fw_name_pre = IWL8000_FW_PRE,
-	IWL_DEVICE_8000,
+	IWL_DEVICE_8260,
+	.ht_params = &iwl8000_ht_params,
+	.nvm_ver = IWL8000_NVM_VERSION,
+	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
+	.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,
+};
+
+const struct iwl_cfg iwl8265_2ac_cfg = {
+	.name = "Intel(R) Dual Band Wireless AC 8265",
+	.fw_name_pre = IWL8265_FW_PRE,
+	IWL_DEVICE_8265,
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
@@ -200,7 +232,7 @@ const struct iwl_cfg iwl4165_2ac_cfg = {
 const struct iwl_cfg iwl8260_2ac_sdio_cfg = {
 	.name = "Intel(R) Dual Band Wireless-AC 8260",
 	.fw_name_pre = IWL8000_FW_PRE,
-	IWL_DEVICE_8000,
+	IWL_DEVICE_8260,
 	.ht_params = &iwl8000_ht_params,
 	.nvm_ver = IWL8000_NVM_VERSION,
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
@@ -227,3 +259,4 @@ const struct iwl_cfg iwl4165_2ac_sdio_cfg = {
 };
 
 MODULE_FIRMWARE(IWL8000_MODULE_FIRMWARE(IWL8000_UCODE_API_OK));
+MODULE_FIRMWARE(IWL8265_MODULE_FIRMWARE(IWL8265_UCODE_API_OK));
diff --git a/drivers/net/wireless/intel/iwlwifi/iwl-9000.c b/drivers/net/wireless/intel/iwlwifi/iwl-9000.c
new file mode 100644
index 000000000000..ecbf4822cd69
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-9000.c
@@ -0,0 +1,163 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Deutschland GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Deutschland GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include <linux/module.h>
+#include <linux/stringify.h>
+#include "iwl-config.h"
+#include "iwl-agn-hw.h"
+
+/* Highest firmware API version supported */
+#define IWL9000_UCODE_API_MAX	20
+
+/* Oldest version we won't warn about */
+#define IWL9000_UCODE_API_OK	13
+
+/* Lowest firmware API version supported */
+#define IWL9000_UCODE_API_MIN	13
+
+/* NVM versions */
+#define IWL9000_NVM_VERSION		0x0a1d
+#define IWL9000_TX_POWER_VERSION	0xffff /* meaningless */
+
+/* Memory offsets and lengths */
+#define IWL9000_DCCM_OFFSET		0x800000
+#define IWL9000_DCCM_LEN		0x18000
+#define IWL9000_DCCM2_OFFSET		0x880000
+#define IWL9000_DCCM2_LEN		0x8000
+#define IWL9000_SMEM_OFFSET		0x400000
+#define IWL9000_SMEM_LEN		0x68000
+
+#define  IWL9000_FW_PRE "iwlwifi-9000-"
+#define IWL9000_MODULE_FIRMWARE(api) \
+	IWL9000_FW_PRE "-" __stringify(api) ".ucode"
+
+#define NVM_HW_SECTION_NUM_FAMILY_9000		10
+
+static const struct iwl_base_params iwl9000_base_params = {
+	.eeprom_size = OTP_LOW_IMAGE_SIZE_FAMILY_9000,
+	.num_of_queues = 31,
+	.pll_cfg_val = 0,
+	.shadow_ram_support = true,
+	.led_compensation = 57,
+	.wd_timeout = IWL_LONG_WD_TIMEOUT,
+	.max_event_log_size = 512,
+	.shadow_reg_enable = true,
+	.pcie_l1_allowed = true,
+};
+
+static const struct iwl_ht_params iwl9000_ht_params = {
+	.stbc = true,
+	.ldpc = true,
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
+};
+
+static const struct iwl_tt_params iwl9000_tt_params = {
+	.ct_kill_entry = 115,
+	.ct_kill_exit = 93,
+	.ct_kill_duration = 5,
+	.dynamic_smps_entry = 111,
+	.dynamic_smps_exit = 107,
+	.tx_protection_entry = 112,
+	.tx_protection_exit = 105,
+	.tx_backoff = {
+		{.temperature = 110, .backoff = 200},
+		{.temperature = 111, .backoff = 600},
+		{.temperature = 112, .backoff = 1200},
+		{.temperature = 113, .backoff = 2000},
+		{.temperature = 114, .backoff = 4000},
+	},
+	.support_ct_kill = true,
+	.support_dynamic_smps = true,
+	.support_tx_protection = true,
+	.support_tx_backoff = true,
+};
+
+#define IWL_DEVICE_9000							\
+	.ucode_api_max = IWL9000_UCODE_API_MAX,				\
+	.ucode_api_ok = IWL9000_UCODE_API_OK,				\
+	.ucode_api_min = IWL9000_UCODE_API_MIN,				\
+	.device_family = IWL_DEVICE_FAMILY_8000,			\
+	.max_inst_size = IWL60_RTC_INST_SIZE,				\
+	.max_data_size = IWL60_RTC_DATA_SIZE,				\
+	.base_params = &iwl9000_base_params,				\
+	.led_mode = IWL_LED_RF_STATE,					\
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_9000,		\
+	.non_shared_ant = ANT_A,					\
+	.dccm_offset = IWL9000_DCCM_OFFSET,				\
+	.dccm_len = IWL9000_DCCM_LEN,					\
+	.dccm2_offset = IWL9000_DCCM2_OFFSET,				\
+	.dccm2_len = IWL9000_DCCM2_LEN,					\
+	.smem_offset = IWL9000_SMEM_OFFSET,				\
+	.smem_len = IWL9000_SMEM_LEN,					\
+	.thermal_params = &iwl9000_tt_params,				\
+	.apmg_not_supported = true
+
+const struct iwl_cfg iwl9260_2ac_cfg = {
+		.name = "Intel(R) Dual Band Wireless AC 9260",
+		.fw_name_pre = IWL9000_FW_PRE,
+		IWL_DEVICE_9000,
+		.ht_params = &iwl9000_ht_params,
+		.nvm_ver = IWL9000_NVM_VERSION,
+		.nvm_calib_ver = IWL9000_TX_POWER_VERSION,
+		.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,
+};
+
+const struct iwl_cfg iwl5165_2ac_cfg = {
+		.name = "Intel(R) Dual Band Wireless AC 5165",
+		.fw_name_pre = IWL9000_FW_PRE,
+		IWL_DEVICE_9000,
+		.ht_params = &iwl9000_ht_params,
+		.nvm_ver = IWL9000_NVM_VERSION,
+		.nvm_calib_ver = IWL9000_TX_POWER_VERSION,
+		.max_ht_ampdu_exponent = IEEE80211_HT_MAX_AMPDU_64K,
+};
+
+MODULE_FIRMWARE(IWL9000_MODULE_FIRMWARE(IWL9000_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h b/drivers/net/wireless/intel/iwlwifi/iwl-agn-hw.h
index 04a483d38659..ee9347a54cdc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-hw.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-agn-hw.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/intel/iwlwifi/iwl-config.h
index 910970858f98..f99048135fb9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-config.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -254,6 +254,7 @@ struct iwl_tt_params {
 #define OTP_LOW_IMAGE_SIZE		(2 * 512 * sizeof(u16)) /* 2 KB */
 #define OTP_LOW_IMAGE_SIZE_FAMILY_7000	(16 * 512 * sizeof(u16)) /* 16 KB */
 #define OTP_LOW_IMAGE_SIZE_FAMILY_8000	(32 * 512 * sizeof(u16)) /* 32 KB */
+#define OTP_LOW_IMAGE_SIZE_FAMILY_9000	OTP_LOW_IMAGE_SIZE_FAMILY_8000
 
 struct iwl_eeprom_params {
 	const u8 regulatory_bands[7];
@@ -295,7 +296,6 @@ struct iwl_pwr_tx_backoff {
  * @high_temp: Is this NIC is designated to be in high temperature.
  * @host_interrupt_operation_mode: device needs host interrupt operation
  *	mode set
- * @d0i3: device uses d0i3 instead of d3
  * @nvm_hw_section_num: the ID of the HW NVM section
  * @features: hw features, any combination of feature_whitelist
  * @pwr_tx_backoffs: translation table between power limits and backoffs
@@ -342,7 +342,6 @@ struct iwl_cfg {
 	const bool internal_wimax_coex;
 	const bool host_interrupt_operation_mode;
 	bool high_temp;
-	bool d0i3;
 	u8   nvm_hw_section_num;
 	bool lp_xtal_workaround;
 	const struct iwl_pwr_tx_backoff *pwr_tx_backoffs;
@@ -421,6 +420,7 @@ extern const struct iwl_cfg iwl3160_2ac_cfg;
 extern const struct iwl_cfg iwl3160_2n_cfg;
 extern const struct iwl_cfg iwl3160_n_cfg;
 extern const struct iwl_cfg iwl3165_2ac_cfg;
+extern const struct iwl_cfg iwl3168_2ac_cfg;
 extern const struct iwl_cfg iwl7265_2ac_cfg;
 extern const struct iwl_cfg iwl7265_2n_cfg;
 extern const struct iwl_cfg iwl7265_n_cfg;
@@ -429,9 +429,12 @@ extern const struct iwl_cfg iwl7265d_2n_cfg;
 extern const struct iwl_cfg iwl7265d_n_cfg;
 extern const struct iwl_cfg iwl8260_2n_cfg;
 extern const struct iwl_cfg iwl8260_2ac_cfg;
+extern const struct iwl_cfg iwl8265_2ac_cfg;
 extern const struct iwl_cfg iwl4165_2ac_cfg;
 extern const struct iwl_cfg iwl8260_2ac_sdio_cfg;
 extern const struct iwl_cfg iwl4165_2ac_sdio_cfg;
+extern const struct iwl_cfg iwl9260_2ac_cfg;
+extern const struct iwl_cfg iwl5165_2ac_cfg;
 #endif /* CONFIG_IWLMVM */
 
 #endif /* __IWL_CONFIG_H__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/intel/iwlwifi/iwl-csr.h
index 543abeaffcf0..163b21bc20cb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-csr.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.c b/drivers/net/wireless/intel/iwlwifi/iwl-debug.c
index 09feff4fa226..b1c3b0d0fcc6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-debug.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/intel/iwlwifi/iwl-debug.h
index 9bb36d79c2bd..110333208450 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-debug.h
@@ -21,7 +21,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -163,7 +163,6 @@ do {                                            			\
 #define IWL_DL_FW		0x00010000
 #define IWL_DL_RF_KILL		0x00020000
 #define IWL_DL_FW_ERRORS	0x00040000
-#define IWL_DL_LED		0x00080000
 /* 0x00F00000 - 0x00100000 */
 #define IWL_DL_RATE		0x00100000
 #define IWL_DL_CALIB		0x00200000
@@ -189,7 +188,6 @@ do {                                            			\
 #define IWL_DEBUG_RX(p, f, a...)	IWL_DEBUG(p, IWL_DL_RX, f, ## a)
 #define IWL_DEBUG_TX(p, f, a...)	IWL_DEBUG(p, IWL_DL_TX, f, ## a)
 #define IWL_DEBUG_ISR(p, f, a...)	IWL_DEBUG(p, IWL_DL_ISR, f, ## a)
-#define IWL_DEBUG_LED(p, f, a...)	IWL_DEBUG(p, IWL_DL_LED, f, ## a)
 #define IWL_DEBUG_WEP(p, f, a...)	IWL_DEBUG(p, IWL_DL_WEP, f, ## a)
 #define IWL_DEBUG_HC(p, f, a...)	IWL_DEBUG(p, IWL_DL_HCMD, f, ## a)
 #define IWL_DEBUG_QUOTA(p, f, a...)	IWL_DEBUG(p, IWL_DL_QUOTA, f, ## a)
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-data.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h
index 71a78cede9b0..d80312b46f16 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-data.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-data.h
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2009 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2015        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -19,7 +20,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -51,6 +52,22 @@ TRACE_EVENT(iwlwifi_dev_tx_data,
 	TP_printk("[%s] TX frame data", __get_str(dev))
 );
 
+TRACE_EVENT(iwlwifi_dev_tx_tso_chunk,
+	TP_PROTO(const struct device *dev,
+		 u8 *data_src, size_t data_len),
+	TP_ARGS(dev, data_src, data_len),
+	TP_STRUCT__entry(
+		DEV_ENTRY
+
+		__dynamic_array(u8, data, data_len)
+	),
+	TP_fast_assign(
+		DEV_ASSIGN;
+		memcpy(__get_dynamic_array(data), data_src, data_len);
+	),
+	TP_printk("[%s] TX frame data", __get_str(dev))
+);
+
 TRACE_EVENT(iwlwifi_dev_rx_data,
 	TP_PROTO(const struct device *dev,
 		 const struct iwl_trans *trans,
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-io.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h
index f62c54485852..27914eedc146 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-io.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-io.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h
index eb4b99a1c8cd..22786d7dc00a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-iwlwifi.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-iwlwifi.h
@@ -20,7 +20,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-msg.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-msg.h
index a3b3c2465f89..5dfc9295a7e0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-msg.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-msg.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace-ucode.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-ucode.h
index 10839fae9cd9..e9b8673dd245 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace-ucode.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace-ucode.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.c b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace.c
index 90987d6f348e..1d9dd153ef1c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace.c
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.h b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace.h
index b87acd6a229b..f4d3cd010087 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-devtrace.h
@@ -19,7 +19,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/intel/iwlwifi/iwl-drv.c
index 463cadfbfccb..ab4c2a0470b2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-drv.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -243,8 +243,10 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
 		char rev_step = 'A' + CSR_HW_REV_STEP(drv->trans->hw_rev);
 
-		snprintf(drv->firmware_name, sizeof(drv->firmware_name),
-			 "%s%c-%s.ucode", name_pre, rev_step, tag);
+		if (rev_step != 'A')
+			snprintf(drv->firmware_name,
+				 sizeof(drv->firmware_name), "%s%c-%s.ucode",
+				 name_pre, rev_step, tag);
 	}
 
 	IWL_DEBUG_INFO(drv, "attempting to load firmware %s'%s'\n",
@@ -451,7 +453,9 @@ static int iwl_set_ucode_api_flags(struct iwl_drv *drv, const u8 *data,
 	int i;
 
 	if (api_index >= DIV_ROUND_UP(NUM_IWL_UCODE_TLV_API, 32)) {
-		IWL_ERR(drv, "api_index larger than supported by driver\n");
+		IWL_ERR(drv,
+			"api flags index %d larger than supported by driver\n",
+			api_index);
 		/* don't return an error so we can load FW that has more bits */
 		return 0;
 	}
@@ -473,7 +477,9 @@ static int iwl_set_ucode_capabilities(struct iwl_drv *drv, const u8 *data,
 	int i;
 
 	if (api_index >= DIV_ROUND_UP(NUM_IWL_UCODE_TLV_CAPA, 32)) {
-		IWL_ERR(drv, "api_index larger than supported by driver\n");
+		IWL_ERR(drv,
+			"capa flags index %d larger than supported by driver\n",
+			api_index);
 		/* don't return an error so we can load FW that has more bits */
 		return 0;
 	}
@@ -590,7 +596,8 @@ static int iwl_parse_v1_v2_firmware(struct iwl_drv *drv,
 static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 				const struct firmware *ucode_raw,
 				struct iwl_firmware_pieces *pieces,
-				struct iwl_ucode_capabilities *capa)
+				struct iwl_ucode_capabilities *capa,
+				bool *usniffer_images)
 {
 	struct iwl_tlv_ucode_header *ucode = (void *)ucode_raw->data;
 	struct iwl_ucode_tlv *tlv;
@@ -603,7 +610,6 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 	char buildstr[25];
 	u32 build, paging_mem_size;
 	int num_of_cpus;
-	bool usniffer_images = false;
 	bool usniffer_req = false;
 	bool gscan_capa = false;
 
@@ -976,7 +982,7 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 			break;
 			}
 		case IWL_UCODE_TLV_SEC_RT_USNIFFER:
-			usniffer_images = true;
+			*usniffer_images = true;
 			iwl_store_ucode_sec(pieces, tlv_data,
 					    IWL_UCODE_REGULAR_USNIFFER,
 					    tlv_len);
@@ -1027,7 +1033,7 @@ static int iwl_parse_tlv_firmware(struct iwl_drv *drv,
 		}
 	}
 
-	if (usniffer_req && !usniffer_images) {
+	if (usniffer_req && !*usniffer_images) {
 		IWL_ERR(drv,
 			"user selected to work with usniffer but usniffer image isn't available in ucode package\n");
 		return -EINVAL;
@@ -1188,6 +1194,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 	u32 api_ver;
 	int i;
 	bool load_module = false;
+	bool usniffer_images = false;
 
 	fw->ucode_capa.max_probe_length = IWL_DEFAULT_MAX_PROBE_LENGTH;
 	fw->ucode_capa.standard_phy_calibration_size =
@@ -1225,7 +1232,7 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 		err = iwl_parse_v1_v2_firmware(drv, ucode_raw, pieces);
 	else
 		err = iwl_parse_tlv_firmware(drv, ucode_raw, pieces,
-					     &fw->ucode_capa);
+					     &fw->ucode_capa, &usniffer_images);
 
 	if (err)
 		goto try_again;
@@ -1323,6 +1330,8 @@ static void iwl_req_fw_callback(const struct firmware *ucode_raw, void *context)
 		sizeof(struct iwl_fw_dbg_trigger_time_event);
 	trigger_tlv_sz[FW_DBG_TRIGGER_BA] =
 		sizeof(struct iwl_fw_dbg_trigger_ba);
+	trigger_tlv_sz[FW_DBG_TRIGGER_TDLS] =
+		sizeof(struct iwl_fw_dbg_trigger_tdls);
 
 	for (i = 0; i < ARRAY_SIZE(drv->fw.dbg_trigger_tlv); i++) {
 		if (pieces->dbg_trigger_tlv[i]) {
@@ -1539,6 +1548,7 @@ struct iwl_mod_params iwlwifi_mod_params = {
 	.bt_coex_active = true,
 	.power_level = IWL_POWER_INDEX_1,
 	.d0i3_disable = true,
+	.d0i3_entry_delay = 1000,
 #ifndef CONFIG_IWLWIFI_UAPSD
 	.uapsd_disable = true,
 #endif /* CONFIG_IWLWIFI_UAPSD */
@@ -1637,9 +1647,9 @@ MODULE_PARM_DESC(swcrypto, "using crypto in software (default 0 [hardware])");
 module_param_named(11n_disable, iwlwifi_mod_params.disable_11n, uint, S_IRUGO);
 MODULE_PARM_DESC(11n_disable,
 	"disable 11n functionality, bitmap: 1: full, 2: disable agg TX, 4: disable agg RX, 8 enable agg TX");
-module_param_named(amsdu_size_8K, iwlwifi_mod_params.amsdu_size_8K,
+module_param_named(amsdu_size, iwlwifi_mod_params.amsdu_size,
 		   int, S_IRUGO);
-MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size (default 0)");
+MODULE_PARM_DESC(amsdu_size, "amsdu size 0:4K 1:8K 2:12K (default 0)");
 module_param_named(fw_restart, iwlwifi_mod_params.restart_fw, bool, S_IRUGO);
 MODULE_PARM_DESC(fw_restart, "restart firmware in case of error (default true)");
 
@@ -1704,3 +1714,7 @@ MODULE_PARM_DESC(power_level,
 module_param_named(fw_monitor, iwlwifi_mod_params.fw_monitor, bool, S_IRUGO);
 MODULE_PARM_DESC(fw_monitor,
 		 "firmware monitor - to debug FW (default: false - needs lots of memory)");
+
+module_param_named(d0i3_timeout, iwlwifi_mod_params.d0i3_entry_delay,
+		   uint, S_IRUGO);
+MODULE_PARM_DESC(d0i3_timeout, "Timeout to D0i3 entry when idle (ms)");
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/intel/iwlwifi/iwl-drv.h
index cda746b33db1..f6eacfdbc265 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-drv.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -69,7 +69,7 @@
 /* for all modules */
 #define DRV_NAME        "iwlwifi"
 #define DRV_COPYRIGHT	"Copyright(c) 2003- 2015 Intel Corporation"
-#define DRV_AUTHOR     "<ilw@linux.intel.com>"
+#define DRV_AUTHOR     "<linuxwifi@intel.com>"
 
 /* radio config bits (actual values from NVM definition) */
 #define NVM_RF_CFG_DASH_MSK(x)   (x & 0x3)         /* bits 0-1   */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.c
index acc3d186c5c1..c15f5be85197 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -454,11 +454,11 @@ static void iwl_eeprom_enhanced_txpower(struct device *dev,
 				 TXP_CHECK_AND_PRINT(COMMON_TYPE),
 				 txp->flags);
 		IWL_DEBUG_EEPROM(dev,
-				 "\t\t chain_A: 0x%02x chain_B: 0X%02x chain_C: 0X%02x\n",
+				 "\t\t chain_A: %d chain_B: %d chain_C: %d\n",
 				 txp->chain_a_max, txp->chain_b_max,
 				 txp->chain_c_max);
 		IWL_DEBUG_EEPROM(dev,
-				 "\t\t MIMO2: 0x%02x MIMO3: 0x%02x High 20_on_40: 0x%02x Low 20_on_40: 0x%02x\n",
+				 "\t\t MIMO2: %d MIMO3: %d High 20_on_40: 0x%02x Low 20_on_40: 0x%02x\n",
 				 txp->mimo2_max, txp->mimo3_max,
 				 ((txp->delta_20_in_40 & 0xf0) >> 4),
 				 (txp->delta_20_in_40 & 0x0f));
@@ -766,7 +766,7 @@ void iwl_init_ht_hw_capab(const struct iwl_cfg *cfg,
 	if (cfg->ht_params->ldpc)
 		ht_info->cap |= IEEE80211_HT_CAP_LDPC_CODING;
 
-	if (iwlwifi_mod_params.amsdu_size_8K)
+	if (iwlwifi_mod_params.amsdu_size >= IWL_AMSDU_8K)
 		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
 
 	ht_info->ampdu_factor = cfg->max_ht_ampdu_exponent;
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h
index 750c8c9ee70d..ad2b834668ff 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-parse.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-read.c
index 219ca8acca62..f2cea1c7befc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-read.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-read.h
index a6d3bdf82cdd..1ed78be06c23 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-read.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-eeprom-read.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/intel/iwlwifi/iwl-fh.h
index d56064861a9c..5cc6be927eab 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fh.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-fh.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/intel/iwlwifi/iwl-fw-error-dump.h
index 9dbe19cbb4dd..a5aaf6853704 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-fw-error-dump.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -88,6 +88,7 @@
  *	&struct iwl_fw_error_dump_rb
  * @IWL_FW_ERROR_PAGING: UMAC's image memory segments which were
  *	paged to the DRAM.
+ * @IWL_FW_ERROR_DUMP_RADIO_REG: Dump the radio registers.
  */
 enum iwl_fw_error_dump_type {
 	/* 0 is deprecated */
@@ -103,6 +104,7 @@ enum iwl_fw_error_dump_type {
 	IWL_FW_ERROR_DUMP_ERROR_INFO = 10,
 	IWL_FW_ERROR_DUMP_RB = 11,
 	IWL_FW_ERROR_DUMP_PAGING = 12,
+	IWL_FW_ERROR_DUMP_RADIO_REG = 13,
 
 	IWL_FW_ERROR_DUMP_MAX,
 };
@@ -288,6 +290,9 @@ iwl_fw_error_next_data(struct iwl_fw_error_dump_data *data)
  * @FW_DBG_TRIGGER_TIME_EVENT: trigger log collection upon time events related
  *	events.
  * @FW_DBG_TRIGGER_BA: trigger log collection upon BlockAck related events.
+ * @FW_DBG_TX_LATENCY: trigger log collection when the tx latency goes above a
+ *	threshold.
+ * @FW_DBG_TDLS: trigger log collection upon TDLS related events.
  */
 enum iwl_fw_dbg_trigger {
 	FW_DBG_TRIGGER_INVALID = 0,
@@ -302,6 +307,8 @@ enum iwl_fw_dbg_trigger {
 	FW_DBG_TRIGGER_TXQ_TIMERS,
 	FW_DBG_TRIGGER_TIME_EVENT,
 	FW_DBG_TRIGGER_BA,
+	FW_DBG_TRIGGER_TX_LATENCY,
+	FW_DBG_TRIGGER_TDLS,
 
 	/* must be last */
 	FW_DBG_TRIGGER_MAX,
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/intel/iwlwifi/iwl-fw-file.h
index 08303db0000f..84f8aeb926c8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-fw-file.h
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -308,6 +310,10 @@ typedef unsigned int __bitwise__ iwl_ucode_tlv_capa_t;
  * @IWL_UCODE_TLV_CAPA_GSCAN_SUPPORT: supports gscan
  * @IWL_UCODE_TLV_CAPA_EXTENDED_DTS_MEASURE: extended DTS measurement
  * @IWL_UCODE_TLV_CAPA_SHORT_PM_TIMEOUTS: supports short PM timeouts
+ * @IWL_UCODE_TLV_CAPA_BT_MPLUT_SUPPORT: supports bt-coex Multi-priority LUT
+ * @IWL_UCODE_TLV_CAPA_BEACON_ANT_SELECTION: firmware will decide on what
+ *	antenna the beacon should be transmitted
+ * @IWL_UCODE_TLV_CAPA_LAR_SUPPORT_V2: support LAR API V2
  *
  * @NUM_IWL_UCODE_TLV_CAPA: number of bits used
  */
@@ -334,6 +340,9 @@ enum iwl_ucode_tlv_capa {
 	IWL_UCODE_TLV_CAPA_GSCAN_SUPPORT		= (__force iwl_ucode_tlv_capa_t)31,
 	IWL_UCODE_TLV_CAPA_EXTENDED_DTS_MEASURE		= (__force iwl_ucode_tlv_capa_t)64,
 	IWL_UCODE_TLV_CAPA_SHORT_PM_TIMEOUTS		= (__force iwl_ucode_tlv_capa_t)65,
+	IWL_UCODE_TLV_CAPA_BT_MPLUT_SUPPORT		= (__force iwl_ucode_tlv_capa_t)67,
+	IWL_UCODE_TLV_CAPA_BEACON_ANT_SELECTION		= (__force iwl_ucode_tlv_capa_t)71,
+	IWL_UCODE_TLV_CAPA_LAR_SUPPORT_V2		= (__force iwl_ucode_tlv_capa_t)73,
 
 	NUM_IWL_UCODE_TLV_CAPA
 #ifdef __CHECKER__
@@ -548,6 +557,8 @@ enum iwl_fw_dbg_trigger_vif_type {
  * @start_conf_id: if mode is %IWL_FW_DBG_TRIGGER_START, this defines what
  *	configuration should be applied when the triggers kicks in.
  * @occurrences: number of occurrences. 0 means the trigger will never fire.
+ * @trig_dis_ms: the time, in milliseconds, after an occurrence of this
+ *	trigger in which another occurrence should be ignored.
  */
 struct iwl_fw_dbg_trigger_tlv {
 	__le32 id;
@@ -557,7 +568,8 @@ struct iwl_fw_dbg_trigger_tlv {
 	u8 mode;
 	u8 start_conf_id;
 	__le16 occurrences;
-	__le32 reserved[2];
+	__le16 trig_dis_ms;
+	__le16 reserved[3];
 
 	u8 data[0];
 } __packed;
@@ -723,6 +735,19 @@ struct iwl_fw_dbg_trigger_ba {
 } __packed;
 
 /**
+ * struct iwl_fw_dbg_trigger_tdls - configures trigger for TDLS events.
+ * @action_bitmap: the TDLS action to trigger the collection upon
+ * @peer_mode: trigger on specific peer or all
+ * @peer: the TDLS peer to trigger the collection on
+ */
+struct iwl_fw_dbg_trigger_tdls {
+	u8 action_bitmap;
+	u8 peer_mode;
+	u8 peer[ETH_ALEN];
+	u8 reserved[4];
+} __packed;
+
+/**
  * struct iwl_fw_dbg_conf_tlv - a TLV that describes a debug configuration.
  * @id: conf id
  * @usniffer: should the uSniffer image be used
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/intel/iwlwifi/iwl-fw.h
index 84ec0cefb62a..85d6d6d55e2f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-fw.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -305,18 +305,4 @@ iwl_fw_dbg_conf_usniffer(const struct iwl_fw *fw, u8 id)
 	return conf_tlv->usniffer;
 }
 
-#define iwl_fw_dbg_trigger_enabled(fw, id) ({			\
-	void *__dbg_trigger = (fw)->dbg_trigger_tlv[(id)];	\
-	unlikely(__dbg_trigger);				\
-})
-
-static inline struct iwl_fw_dbg_trigger_tlv*
-iwl_fw_dbg_get_trigger(const struct iwl_fw *fw, u8 id)
-{
-	if (WARN_ON(id >= ARRAY_SIZE(fw->dbg_trigger_tlv)))
-		return NULL;
-
-	return fw->dbg_trigger_tlv[id];
-}
-
 #endif  /* __iwl_fw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/intel/iwlwifi/iwl-io.c
index 0bd9d4aad0c0..32c8f84ae519 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-io.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * Portions of this file are derived from the ipw3945 project.
  *
@@ -21,7 +22,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -81,7 +82,7 @@ u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg)
 {
 	u32 value = 0x5a5a5a5a;
 	unsigned long flags;
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
 		value = iwl_read32(trans, reg);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
@@ -94,7 +95,7 @@ void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value)
 {
 	unsigned long flags;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
 		iwl_write32(trans, reg, value);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
@@ -117,26 +118,28 @@ int iwl_poll_direct_bit(struct iwl_trans *trans, u32 addr, u32 mask,
 }
 IWL_EXPORT_SYMBOL(iwl_poll_direct_bit);
 
-u32 __iwl_read_prph(struct iwl_trans *trans, u32 ofs)
+u32 iwl_read_prph_no_grab(struct iwl_trans *trans, u32 ofs)
 {
 	u32 val = iwl_trans_read_prph(trans, ofs);
 	trace_iwlwifi_dev_ioread_prph32(trans->dev, ofs, val);
 	return val;
 }
+IWL_EXPORT_SYMBOL(iwl_read_prph_no_grab);
 
-void __iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val)
+void iwl_write_prph_no_grab(struct iwl_trans *trans, u32 ofs, u32 val)
 {
 	trace_iwlwifi_dev_iowrite_prph32(trans->dev, ofs, val);
 	iwl_trans_write_prph(trans, ofs, val);
 }
+IWL_EXPORT_SYMBOL(iwl_write_prph_no_grab);
 
 u32 iwl_read_prph(struct iwl_trans *trans, u32 ofs)
 {
 	unsigned long flags;
 	u32 val = 0x5a5a5a5a;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
-		val = __iwl_read_prph(trans, ofs);
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
+		val = iwl_read_prph_no_grab(trans, ofs);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 	return val;
@@ -147,8 +150,8 @@ void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val)
 {
 	unsigned long flags;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
-		__iwl_write_prph(trans, ofs, val);
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
+		iwl_write_prph_no_grab(trans, ofs, val);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
@@ -173,9 +176,10 @@ void iwl_set_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 {
 	unsigned long flags;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
-		__iwl_write_prph(trans, ofs,
-				 __iwl_read_prph(trans, ofs) | mask);
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
+		iwl_write_prph_no_grab(trans, ofs,
+				       iwl_read_prph_no_grab(trans, ofs) |
+				       mask);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
@@ -186,9 +190,10 @@ void iwl_set_bits_mask_prph(struct iwl_trans *trans, u32 ofs,
 {
 	unsigned long flags;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
-		__iwl_write_prph(trans, ofs,
-				 (__iwl_read_prph(trans, ofs) & mask) | bits);
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
+		iwl_write_prph_no_grab(trans, ofs,
+				       (iwl_read_prph_no_grab(trans, ofs) &
+					mask) | bits);
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
@@ -199,9 +204,9 @@ void iwl_clear_bits_prph(struct iwl_trans *trans, u32 ofs, u32 mask)
 	unsigned long flags;
 	u32 val;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
-		val = __iwl_read_prph(trans, ofs);
-		__iwl_write_prph(trans, ofs, (val & ~mask));
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
+		val = iwl_read_prph_no_grab(trans, ofs);
+		iwl_write_prph_no_grab(trans, ofs, (val & ~mask));
 		iwl_trans_release_nic_access(trans, &flags);
 	}
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/intel/iwlwifi/iwl-io.h
index 501d0560c061..a9bcc788cae1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-io.h
@@ -21,7 +21,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -55,9 +55,9 @@ u32 iwl_read_direct32(struct iwl_trans *trans, u32 reg);
 void iwl_write_direct32(struct iwl_trans *trans, u32 reg, u32 value);
 
 
-u32 __iwl_read_prph(struct iwl_trans *trans, u32 ofs);
+u32 iwl_read_prph_no_grab(struct iwl_trans *trans, u32 ofs);
 u32 iwl_read_prph(struct iwl_trans *trans, u32 ofs);
-void __iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val);
+void iwl_write_prph_no_grab(struct iwl_trans *trans, u32 ofs, u32 val);
 void iwl_write_prph(struct iwl_trans *trans, u32 ofs, u32 val);
 int iwl_poll_prph_bit(struct iwl_trans *trans, u32 addr,
 		      u32 bits, u32 mask, int timeout);
diff --git a/drivers/net/wireless/iwlwifi/iwl-modparams.h b/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h
index ac2b90df8413..fd42f63f5e84 100644
--- a/drivers/net/wireless/iwlwifi/iwl-modparams.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-modparams.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -86,6 +86,12 @@ enum iwl_disable_11n {
 	IWL_ENABLE_HT_TXAGG	 = BIT(3),
 };
 
+enum iwl_amsdu_size {
+	IWL_AMSDU_4K = 0,
+	IWL_AMSDU_8K = 1,
+	IWL_AMSDU_12K = 2,
+};
+
 /**
  * struct iwl_mod_params
  *
@@ -94,7 +100,7 @@ enum iwl_disable_11n {
  * @sw_crypto: using hardware encryption, default = 0
  * @disable_11n: disable 11n capabilities, default = 0,
  *	use IWL_[DIS,EN]ABLE_HT_* constants
- * @amsdu_size_8K: enable 8K amsdu size, default = 0
+ * @amsdu_size: enable 8K amsdu size, default = 4K. enum iwl_amsdu_size.
  * @restart_fw: restart firmware, default = 1
  * @bt_coex_active: enable bt coex, default = true
  * @led_mode: system default, default = 0
@@ -103,13 +109,15 @@ enum iwl_disable_11n {
  * @debug_level: levels are IWL_DL_*
  * @ant_coupling: antenna coupling in dB, default = 0
  * @d0i3_disable: disable d0i3, default = 1,
+ * @d0i3_entry_delay: time to wait after no refs are taken before
+ *	entering D0i3 (in msecs)
  * @lar_disable: disable LAR (regulatory), default = 0
  * @fw_monitor: allow to use firmware monitor
  */
 struct iwl_mod_params {
 	int sw_crypto;
 	unsigned int disable_11n;
-	int amsdu_size_8K;
+	int amsdu_size;
 	bool restart_fw;
 	bool bt_coex_active;
 	int led_mode;
@@ -122,6 +130,7 @@ struct iwl_mod_params {
 	char *nvm_file;
 	bool uapsd_disable;
 	bool d0i3_disable;
+	unsigned int d0i3_entry_delay;
 	bool lar_disable;
 	bool fw_monitor;
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c b/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.c
index 6caf2affbbb5..8aa1f2b7fdfc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h b/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.h
index dbe8234521de..0f9995ed71cd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-notif-wait.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-notif-wait.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c
index d82984912e04..7b89bfc8c8ac 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -379,8 +379,19 @@ static void iwl_init_vht_hw_capab(const struct iwl_cfg *cfg,
 	else
 		vht_cap->cap |= IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN;
 
-	if (iwlwifi_mod_params.amsdu_size_8K)
+	switch (iwlwifi_mod_params.amsdu_size) {
+	case IWL_AMSDU_4K:
+		vht_cap->cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895;
+		break;
+	case IWL_AMSDU_8K:
 		vht_cap->cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991;
+		break;
+	case IWL_AMSDU_12K:
+		vht_cap->cap |= IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454;
+		break;
+	default:
+		break;
+	}
 
 	vht_cap->vht_mcs.rx_mcs_map =
 		cpu_to_le16(IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
@@ -580,15 +591,13 @@ static void iwl_set_hw_address_family_8000(struct device *dev,
 	IWL_ERR_DEV(dev, "mac address is not found\n");
 }
 
-#define IWL_4165_DEVICE_ID 0x5501
-
 struct iwl_nvm_data *
 iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_hw, const __le16 *nvm_sw,
 		   const __le16 *nvm_calib, const __le16 *regulatory,
 		   const __le16 *mac_override, const __le16 *phy_sku,
 		   u8 tx_chains, u8 rx_chains, bool lar_fw_supported,
-		   u32 mac_addr0, u32 mac_addr1, u32 hw_id)
+		   u32 mac_addr0, u32 mac_addr1)
 {
 	struct iwl_nvm_data *data;
 	u32 sku;
@@ -627,17 +636,6 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 				    (sku & NVM_SKU_CAP_11AC_ENABLE);
 	data->sku_cap_mimo_disabled = sku & NVM_SKU_CAP_MIMO_DISABLE;
 
-	/*
-	 * OTP 0x52 bug work around
-	 * define antenna 1x1 according to MIMO disabled
-	 */
-	if (hw_id == IWL_4165_DEVICE_ID && data->sku_cap_mimo_disabled) {
-		data->valid_tx_ant = ANT_B;
-		data->valid_rx_ant = ANT_B;
-		tx_chains = ANT_B;
-		rx_chains = ANT_B;
-	}
-
 	data->n_hw_addrs = iwl_get_n_hw_addrs(cfg, nvm_sw);
 
 	if (cfg->device_family != IWL_DEVICE_FAMILY_8000) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.h
index 9f44d8188c5c..92466ee72806 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-nvm-parse.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -79,7 +79,7 @@ iwl_parse_nvm_data(struct device *dev, const struct iwl_cfg *cfg,
 		   const __le16 *nvm_calib, const __le16 *regulatory,
 		   const __le16 *mac_override, const __le16 *phy_sku,
 		   u8 tx_chains, u8 rx_chains, bool lar_fw_supported,
-		   u32 mac_addr0, u32 mac_addr1, u32 hw_id);
+		   u32 mac_addr0, u32 mac_addr1);
 
 /**
  * iwl_parse_mcc_info - parse MCC (mobile country code) info coming from FW
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/intel/iwlwifi/iwl-op-mode.h
index 2a58d6833224..b49eda8150bb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-op-mode.h
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -123,6 +123,8 @@ struct iwl_cfg;
  *	received on the RSS queue(s). The queue parameter indicates which of the
  *	RSS queues received this frame; it will always be non-zero.
  *	This method must not sleep.
+ * @async_cb: called when an ASYNC command with CMD_WANT_ASYNC_CALLBACK set
+ *	completes. Must be atomic.
  * @queue_full: notifies that a HW queue is full.
  *	Must be atomic and called with BH disabled.
  * @queue_not_full: notifies that a HW queue is not full any more.
@@ -155,6 +157,8 @@ struct iwl_op_mode_ops {
 		   struct iwl_rx_cmd_buffer *rxb);
 	void (*rx_rss)(struct iwl_op_mode *op_mode, struct napi_struct *napi,
 		       struct iwl_rx_cmd_buffer *rxb, unsigned int queue);
+	void (*async_cb)(struct iwl_op_mode *op_mode,
+			 const struct iwl_device_cmd *cmd);
 	void (*queue_full)(struct iwl_op_mode *op_mode, int queue);
 	void (*queue_not_full)(struct iwl_op_mode *op_mode, int queue);
 	bool (*hw_rf_kill)(struct iwl_op_mode *op_mode, bool state);
@@ -203,6 +207,13 @@ static inline void iwl_op_mode_rx_rss(struct iwl_op_mode *op_mode,
 	op_mode->ops->rx_rss(op_mode, napi, rxb, queue);
 }
 
+static inline void iwl_op_mode_async_cb(struct iwl_op_mode *op_mode,
+					const struct iwl_device_cmd *cmd)
+{
+	if (op_mode->ops->async_cb)
+		op_mode->ops->async_cb(op_mode, cmd);
+}
+
 static inline void iwl_op_mode_queue_full(struct iwl_op_mode *op_mode,
 					  int queue)
 {
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.c b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c
index a105455b6a24..4a4dea08751c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-phy-db.h b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.h
index 9ee18d0d2d01..24103877eab0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-phy-db.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-phy-db.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/intel/iwlwifi/iwl-prph.h
index 3ab777f79e4f..5bde23a472b4 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-prph.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -345,6 +345,12 @@ enum secure_load_status_reg {
 #define TXF_READ_MODIFY_DATA		(0xa00448)
 #define TXF_READ_MODIFY_ADDR		(0xa0044c)
 
+/* Radio registers access */
+#define RSP_RADIO_CMD			(0xa02804)
+#define RSP_RADIO_RDDAT			(0xa02814)
+#define RADIO_RSP_ADDR_POS		(6)
+#define RADIO_RSP_RD_CMD		(3)
+
 /* FW monitor */
 #define MON_BUFF_SAMPLE_CTL		(0xa03c00)
 #define MON_BUFF_BASE_ADDR		(0xa03c3c)
diff --git a/drivers/net/wireless/iwlwifi/iwl-scd.h b/drivers/net/wireless/intel/iwlwifi/iwl-scd.h
index f2353ebf2666..99b43da32adf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-scd.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-scd.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.c b/drivers/net/wireless/intel/iwlwifi/iwl-trans.c
index 71610968c365..6069a9ff53fa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.c
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-trans.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -61,7 +61,10 @@
  *
  *****************************************************************************/
 #include <linux/kernel.h>
+#include <linux/bsearch.h>
+
 #include "iwl-trans.h"
+#include "iwl-drv.h"
 
 struct iwl_trans *iwl_trans_alloc(unsigned int priv_size,
 				  struct device *dev,
@@ -112,3 +115,91 @@ void iwl_trans_free(struct iwl_trans *trans)
 	kmem_cache_destroy(trans->dev_cmd_pool);
 	kfree(trans);
 }
+
+int iwl_trans_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
+{
+	int ret;
+
+	if (unlikely(!(cmd->flags & CMD_SEND_IN_RFKILL) &&
+		     test_bit(STATUS_RFKILL, &trans->status)))
+		return -ERFKILL;
+
+	if (unlikely(test_bit(STATUS_FW_ERROR, &trans->status)))
+		return -EIO;
+
+	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE)) {
+		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return -EIO;
+	}
+
+	if (WARN_ON((cmd->flags & CMD_WANT_ASYNC_CALLBACK) &&
+		    !(cmd->flags & CMD_ASYNC)))
+		return -EINVAL;
+
+	if (!(cmd->flags & CMD_ASYNC))
+		lock_map_acquire_read(&trans->sync_cmd_lockdep_map);
+
+	ret = trans->ops->send_cmd(trans, cmd);
+
+	if (!(cmd->flags & CMD_ASYNC))
+		lock_map_release(&trans->sync_cmd_lockdep_map);
+
+	return ret;
+}
+IWL_EXPORT_SYMBOL(iwl_trans_send_cmd);
+
+/* Comparator for struct iwl_hcmd_names.
+ * Used in the binary search over a list of host commands.
+ *
+ * @key: command_id that we're looking for.
+ * @elt: struct iwl_hcmd_names candidate for match.
+ *
+ * @return 0 iff equal.
+ */
+static int iwl_hcmd_names_cmp(const void *key, const void *elt)
+{
+	const struct iwl_hcmd_names *name = elt;
+	u8 cmd1 = *(u8 *)key;
+	u8 cmd2 = name->cmd_id;
+
+	return (cmd1 - cmd2);
+}
+
+const char *iwl_get_cmd_string(struct iwl_trans *trans, u32 id)
+{
+	u8 grp, cmd;
+	struct iwl_hcmd_names *ret;
+	const struct iwl_hcmd_arr *arr;
+	size_t size = sizeof(struct iwl_hcmd_names);
+
+	grp = iwl_cmd_groupid(id);
+	cmd = iwl_cmd_opcode(id);
+
+	if (!trans->command_groups || grp >= trans->command_groups_size ||
+	    !trans->command_groups[grp].arr)
+		return "UNKNOWN";
+
+	arr = &trans->command_groups[grp];
+	ret = bsearch(&cmd, arr->arr, arr->size, size, iwl_hcmd_names_cmp);
+	if (!ret)
+		return "UNKNOWN";
+	return ret->cmd_name;
+}
+IWL_EXPORT_SYMBOL(iwl_get_cmd_string);
+
+int iwl_cmd_groups_verify_sorted(const struct iwl_trans_config *trans)
+{
+	int i, j;
+	const struct iwl_hcmd_arr *arr;
+
+	for (i = 0; i < trans->command_groups_size; i++) {
+		arr = &trans->command_groups[i];
+		if (!arr->arr)
+			continue;
+		for (j = 0; j < arr->size - 1; j++)
+			if (arr->arr[j].cmd_id > arr->arr[j + 1].cmd_id)
+				return -1;
+	}
+	return 0;
+}
+IWL_EXPORT_SYMBOL(iwl_cmd_groups_verify_sorted);
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/intel/iwlwifi/iwl-trans.h
index 6f76525088f0..82fb3a97a46d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/intel/iwlwifi/iwl-trans.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -68,6 +68,7 @@
 #include <linux/ieee80211.h>
 #include <linux/mm.h> /* for page_address */
 #include <linux/lockdep.h>
+#include <linux/kernel.h>
 
 #include "iwl-debug.h"
 #include "iwl-config.h"
@@ -248,6 +249,8 @@ static inline u32 iwl_rx_packet_payload_len(const struct iwl_rx_packet *pkt)
  * @CMD_MAKE_TRANS_IDLE: The command response should mark the trans as idle.
  * @CMD_WAKE_UP_TRANS: The command response should wake up the trans
  *	(i.e. mark it as non-idle).
+ * @CMD_WANT_ASYNC_CALLBACK: the op_mode's async callback function must be
+ *	called after this command completes. Valid only with CMD_ASYNC.
  * @CMD_TB_BITMAP_POS: Position of the first bit for the TB bitmap. We need to
  *	check that we leave enough room for the TBs bitmap which needs 20 bits.
  */
@@ -259,6 +262,7 @@ enum CMD_MODE {
 	CMD_SEND_IN_IDLE	= BIT(4),
 	CMD_MAKE_TRANS_IDLE	= BIT(5),
 	CMD_WAKE_UP_TRANS	= BIT(6),
+	CMD_WANT_ASYNC_CALLBACK	= BIT(7),
 
 	CMD_TB_BITMAP_POS	= 11,
 };
@@ -377,6 +381,11 @@ static inline void iwl_free_rxb(struct iwl_rx_cmd_buffer *r)
 
 #define MAX_NO_RECLAIM_CMDS	6
 
+/*
+ * The first entry in driver_data array in ieee80211_tx_info
+ * that can be used by the transport.
+ */
+#define IWL_TRANS_FIRST_DRIVER_DATA 2
 #define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
 
 /*
@@ -423,6 +432,38 @@ enum iwl_trans_status {
 	STATUS_TRANS_DEAD,
 };
 
+static inline int
+iwl_trans_get_rb_size_order(enum iwl_amsdu_size rb_size)
+{
+	switch (rb_size) {
+	case IWL_AMSDU_4K:
+		return get_order(4 * 1024);
+	case IWL_AMSDU_8K:
+		return get_order(8 * 1024);
+	case IWL_AMSDU_12K:
+		return get_order(12 * 1024);
+	default:
+		WARN_ON(1);
+		return -1;
+	}
+}
+
+struct iwl_hcmd_names {
+	u8 cmd_id;
+	const char *const cmd_name;
+};
+
+#define HCMD_NAME(x)	\
+	{ .cmd_id = x, .cmd_name = #x }
+
+struct iwl_hcmd_arr {
+	const struct iwl_hcmd_names *arr;
+	int size;
+};
+
+#define HCMD_ARR(x)	\
+	{ .arr = x, .size = ARRAY_SIZE(x) }
+
 /**
  * struct iwl_trans_config - transport configuration
  *
@@ -436,14 +477,16 @@ enum iwl_trans_status {
  *	list of such notifications to filter. Max length is
  *	%MAX_NO_RECLAIM_CMDS.
  * @n_no_reclaim_cmds: # of commands in list
- * @rx_buf_size_8k: 8 kB RX buffer size needed for A-MSDUs,
+ * @rx_buf_size: RX buffer size needed for A-MSDUs
  *	if unset 4k will be the RX buffer size
  * @bc_table_dword: set to true if the BC table expects the byte count to be
  *	in DWORD (as opposed to bytes)
  * @scd_set_active: should the transport configure the SCD for HCMD queue
  * @wide_cmd_header: firmware supports wide host command header
- * @command_names: array of command names, must be 256 entries
- *	(one for each command); for debugging only
+ * @sw_csum_tx: transport should compute the TCP checksum
+ * @command_groups: array of command groups, each member is an array of the
+ *	commands in the group; for debugging only
+ * @command_groups_size: number of command groups, to avoid illegal access
  * @sdio_adma_addr: the default address to set for the ADMA in SDIO mode until
  *	we get the ALIVE from the uCode
  */
@@ -456,12 +499,14 @@ struct iwl_trans_config {
 	const u8 *no_reclaim_cmds;
 	unsigned int n_no_reclaim_cmds;
 
-	bool rx_buf_size_8k;
+	enum iwl_amsdu_size rx_buf_size;
 	bool bc_table_dword;
 	bool scd_set_active;
 	bool wide_cmd_header;
-	const char *const *command_names;
-
+	bool sw_csum_tx;
+	const struct iwl_hcmd_arr *command_groups;
+	int command_groups_size;
+ 
 	u32 sdio_adma_addr;
 };
 
@@ -512,7 +557,11 @@ struct iwl_trans_txq_scd_cfg {
  *	If RFkill is asserted in the middle of a SYNC host command, it must
  *	return -ERFKILL straight away.
  *	May sleep only if CMD_ASYNC is not set
- * @tx: send an skb
+ * @tx: send an skb. The transport relies on the op_mode to zero the
+ *	the ieee80211_tx_info->driver_data. If the MPDU is an A-MSDU, all
+ *	the CSUM will be taken care of (TCP CSUM and IP header in case of
+ *	IPv4). If the MPDU is a single MSDU, the op_mode must compute the IP
+ *	header if it is IPv4.
  *	Must be atomic
  * @reclaim: free packet until ssn. Returns a list of freed packets.
  *	Must be atomic
@@ -526,8 +575,11 @@ struct iwl_trans_txq_scd_cfg {
  * @wait_tx_queue_empty: wait until tx queues are empty. May sleep.
  * @freeze_txq_timer: prevents the timer of the queue from firing until the
  *	queue is set to awake. Must be atomic.
- * @dbgfs_register: add the dbgfs files under this directory. Files will be
- *	automatically deleted.
+ * @block_txq_ptrs: stop updating the write pointers of the Tx queues. Note
+ *	that the transport needs to refcount the calls since this function
+ *	will be called several times with block = true, and then the queues
+ *	need to be unblocked only after the same number of calls with
+ *	block = false.
  * @write8: write a u8 to a register at offset ofs from the BAR
  * @write32: write a u32 to a register at offset ofs from the BAR
  * @read32: read a u32 register at offset ofs from the BAR
@@ -583,10 +635,10 @@ struct iwl_trans_ops {
 	void (*txq_disable)(struct iwl_trans *trans, int queue,
 			    bool configure_scd);
 
-	int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
 	int (*wait_tx_queue_empty)(struct iwl_trans *trans, u32 txq_bm);
 	void (*freeze_txq_timer)(struct iwl_trans *trans, unsigned long txqs,
 				 bool freeze);
+	void (*block_txq_ptrs)(struct iwl_trans *trans, bool block);
 
 	void (*write8)(struct iwl_trans *trans, u32 ofs, u8 val);
 	void (*write32)(struct iwl_trans *trans, u32 ofs, u32 val);
@@ -600,8 +652,7 @@ struct iwl_trans_ops {
 	void (*configure)(struct iwl_trans *trans,
 			  const struct iwl_trans_config *trans_cfg);
 	void (*set_pmi)(struct iwl_trans *trans, bool state);
-	bool (*grab_nic_access)(struct iwl_trans *trans, bool silent,
-				unsigned long *flags);
+	bool (*grab_nic_access)(struct iwl_trans *trans, unsigned long *flags);
 	void (*release_nic_access)(struct iwl_trans *trans,
 				   unsigned long *flags);
 	void (*set_bits_mask)(struct iwl_trans *trans, u32 reg, u32 mask,
@@ -612,7 +663,7 @@ struct iwl_trans_ops {
 	void (*resume)(struct iwl_trans *trans);
 
 	struct iwl_trans_dump_data *(*dump_data)(struct iwl_trans *trans,
-						 struct iwl_fw_dbg_trigger_tlv
+						 const struct iwl_fw_dbg_trigger_tlv
 						 *trigger);
 };
 
@@ -628,18 +679,61 @@ enum iwl_trans_state {
 };
 
 /**
- * enum iwl_d0i3_mode - d0i3 mode
+ * DOC: Platform power management
+ *
+ * There are two types of platform power management: system-wide
+ * (WoWLAN) and runtime.
+ *
+ * In system-wide power management the entire platform goes into a low
+ * power state (e.g. idle or suspend to RAM) at the same time and the
+ * device is configured as a wakeup source for the entire platform.
+ * This is usually triggered by userspace activity (e.g. the user
+ * presses the suspend button or a power management daemon decides to
+ * put the platform in low power mode).  The device's behavior in this
+ * mode is dictated by the wake-on-WLAN configuration.
+ *
+ * In runtime power management, only the devices which are themselves
+ * idle enter a low power state.  This is done at runtime, which means
+ * that the entire system is still running normally.  This mode is
+ * usually triggered automatically by the device driver and requires
+ * the ability to enter and exit the low power modes in a very short
+ * time, so there is not much impact in usability.
  *
- * @IWL_D0I3_MODE_OFF - d0i3 is disabled
- * @IWL_D0I3_MODE_ON_IDLE - enter d0i3 when device is idle
- *	(e.g. no active references)
- * @IWL_D0I3_MODE_ON_SUSPEND - enter d0i3 only on suspend
- *	(in case of 'any' trigger)
+ * The terms used for the device's behavior are as follows:
+ *
+ *	- D0: the device is fully powered and the host is awake;
+ *	- D3: the device is in low power mode and only reacts to
+ *		specific events (e.g. magic-packet received or scan
+ *		results found);
+ *	- D0I3: the device is in low power mode and reacts to any
+ *		activity (e.g. RX);
+ *
+ * These terms reflect the power modes in the firmware and are not to
+ * be confused with the physical device power state.  The NIC can be
+ * in D0I3 mode even if, for instance, the PCI device is in D3 state.
+ */
+
+/**
+ * enum iwl_plat_pm_mode - platform power management mode
+ *
+ * This enumeration describes the device's platform power management
+ * behavior when in idle mode (i.e. runtime power management) or when
+ * in system-wide suspend (i.e WoWLAN).
+ *
+ * @IWL_PLAT_PM_MODE_DISABLED: power management is disabled for this
+ *	device.  At runtime, this means that nothing happens and the
+ *	device always remains in active.  In system-wide suspend mode,
+ *	it means that the all connections will be closed automatically
+ *	by mac80211 before the platform is suspended.
+ * @IWL_PLAT_PM_MODE_D3: the device goes into D3 mode (i.e. WoWLAN).
+ *	For runtime power management, this mode is not officially
+ *	supported.
+ * @IWL_PLAT_PM_MODE_D0I3: the device goes into D0I3 mode.
  */
-enum iwl_d0i3_mode {
-	IWL_D0I3_MODE_OFF = 0,
-	IWL_D0I3_MODE_ON_IDLE,
-	IWL_D0I3_MODE_ON_SUSPEND,
+enum iwl_plat_pm_mode {
+	IWL_PLAT_PM_MODE_DISABLED,
+	IWL_PLAT_PM_MODE_D3,
+	IWL_PLAT_PM_MODE_D0I3,
 };
 
 /**
@@ -679,6 +773,12 @@ enum iwl_d0i3_mode {
  *	the opmode.
  * @paging_download_buf: Buffer used for copying all of the pages before
  *	downloading them to the FW. The buffer is allocated in the opmode
+ * @system_pm_mode: the system-wide power management mode in use.
+ *	This mode is set dynamically, depending on the WoWLAN values
+ *	configured from the userspace at runtime.
+ * @runtime_pm_mode: the runtime power management mode in use.  This
+ *	mode is set during the initialization phase and is not
+ *	supposed to change during runtime.
  */
 struct iwl_trans {
 	const struct iwl_trans_ops *ops;
@@ -698,6 +798,9 @@ struct iwl_trans {
 	bool pm_support;
 	bool ltr_enabled;
 
+	const struct iwl_hcmd_arr *command_groups;
+	int command_groups_size;
+
 	u8 num_rx_queues;
 
 	/* The following fields are internal only */
@@ -726,21 +829,24 @@ struct iwl_trans {
 	struct iwl_fw_paging *paging_db;
 	void *paging_download_buf;
 
-	enum iwl_d0i3_mode d0i3_mode;
-
-	bool wowlan_d0i3;
+	enum iwl_plat_pm_mode system_pm_mode;
+	enum iwl_plat_pm_mode runtime_pm_mode;
 
 	/* pointer to trans specific struct */
 	/*Ensure that this pointer will always be aligned to sizeof pointer */
 	char trans_specific[0] __aligned(sizeof(void *));
 };
 
+const char *iwl_get_cmd_string(struct iwl_trans *trans, u32 id);
+int iwl_cmd_groups_verify_sorted(const struct iwl_trans_config *trans);
+
 static inline void iwl_trans_configure(struct iwl_trans *trans,
 				       const struct iwl_trans_config *trans_cfg)
 {
 	trans->op_mode = trans_cfg->op_mode;
 
 	trans->ops->configure(trans, trans_cfg);
+	WARN_ON(iwl_cmd_groups_verify_sorted(trans_cfg));
 }
 
 static inline int _iwl_trans_start_hw(struct iwl_trans *trans, bool low_power)
@@ -860,41 +966,13 @@ static inline void iwl_trans_resume(struct iwl_trans *trans)
 
 static inline struct iwl_trans_dump_data *
 iwl_trans_dump_data(struct iwl_trans *trans,
-		    struct iwl_fw_dbg_trigger_tlv *trigger)
+		    const struct iwl_fw_dbg_trigger_tlv *trigger)
 {
 	if (!trans->ops->dump_data)
 		return NULL;
 	return trans->ops->dump_data(trans, trigger);
 }
 
-static inline int iwl_trans_send_cmd(struct iwl_trans *trans,
-				     struct iwl_host_cmd *cmd)
-{
-	int ret;
-
-	if (unlikely(!(cmd->flags & CMD_SEND_IN_RFKILL) &&
-		     test_bit(STATUS_RFKILL, &trans->status)))
-		return -ERFKILL;
-
-	if (unlikely(test_bit(STATUS_FW_ERROR, &trans->status)))
-		return -EIO;
-
-	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE)) {
-		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
-		return -EIO;
-	}
-
-	if (!(cmd->flags & CMD_ASYNC))
-		lock_map_acquire_read(&trans->sync_cmd_lockdep_map);
-
-	ret = trans->ops->send_cmd(trans, cmd);
-
-	if (!(cmd->flags & CMD_ASYNC))
-		lock_map_release(&trans->sync_cmd_lockdep_map);
-
-	return ret;
-}
-
 static inline struct iwl_device_cmd *
 iwl_trans_alloc_tx_cmd(struct iwl_trans *trans)
 {
@@ -907,6 +985,8 @@ iwl_trans_alloc_tx_cmd(struct iwl_trans *trans)
 			(dev_cmd_ptr + trans->dev_cmd_headroom);
 }
 
+int iwl_trans_send_cmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd);
+
 static inline void iwl_trans_free_tx_cmd(struct iwl_trans *trans,
 					 struct iwl_device_cmd *dev_cmd)
 {
@@ -921,8 +1001,10 @@ static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
 	if (unlikely(test_bit(STATUS_FW_ERROR, &trans->status)))
 		return -EIO;
 
-	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return -EIO;
+	}
 
 	return trans->ops->tx(trans, skb, dev_cmd, queue);
 }
@@ -930,8 +1012,10 @@ static inline int iwl_trans_tx(struct iwl_trans *trans, struct sk_buff *skb,
 static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
 				     int ssn, struct sk_buff_head *skbs)
 {
-	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return;
+	}
 
 	trans->ops->reclaim(trans, queue, ssn, skbs);
 }
@@ -949,8 +1033,10 @@ iwl_trans_txq_enable_cfg(struct iwl_trans *trans, int queue, u16 ssn,
 {
 	might_sleep();
 
-	if (unlikely((trans->state != IWL_TRANS_FW_ALIVE)))
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return;
+	}
 
 	trans->ops->txq_enable(trans, queue, ssn, cfg, queue_wdg_timeout);
 }
@@ -990,26 +1076,36 @@ static inline void iwl_trans_freeze_txq_timer(struct iwl_trans *trans,
 					      unsigned long txqs,
 					      bool freeze)
 {
-	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return;
+	}
 
 	if (trans->ops->freeze_txq_timer)
 		trans->ops->freeze_txq_timer(trans, txqs, freeze);
 }
 
-static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans,
-						u32 txqs)
+static inline void iwl_trans_block_txq_ptrs(struct iwl_trans *trans,
+					    bool block)
 {
-	if (unlikely(trans->state != IWL_TRANS_FW_ALIVE))
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return;
+	}
 
-	return trans->ops->wait_tx_queue_empty(trans, txqs);
+	if (trans->ops->block_txq_ptrs)
+		trans->ops->block_txq_ptrs(trans, block);
 }
 
-static inline int iwl_trans_dbgfs_register(struct iwl_trans *trans,
-					   struct dentry *dir)
+static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans,
+						u32 txqs)
 {
-	return trans->ops->dbgfs_register(trans, dir);
+	if (WARN_ON_ONCE(trans->state != IWL_TRANS_FW_ALIVE)) {
+		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
+		return -EIO;
+	}
+
+	return trans->ops->wait_tx_queue_empty(trans, txqs);
 }
 
 static inline void iwl_trans_write8(struct iwl_trans *trans, u32 ofs, u8 val)
@@ -1085,9 +1181,9 @@ iwl_trans_set_bits_mask(struct iwl_trans *trans, u32 reg, u32 mask, u32 value)
 	trans->ops->set_bits_mask(trans, reg, mask, value);
 }
 
-#define iwl_trans_grab_nic_access(trans, silent, flags)	\
+#define iwl_trans_grab_nic_access(trans, flags)	\
 	__cond_lock(nic_access,				\
-		    likely((trans)->ops->grab_nic_access(trans, silent, flags)))
+		    likely((trans)->ops->grab_nic_access(trans, flags)))
 
 static inline void __releases(nic_access)
 iwl_trans_release_nic_access(struct iwl_trans *trans, unsigned long *flags)
diff --git a/drivers/net/wireless/iwlwifi/mvm/Makefile b/drivers/net/wireless/intel/iwlwifi/mvm/Makefile
index 8c2c3d13b092..23e7e2937566 100644
--- a/drivers/net/wireless/iwlwifi/mvm/Makefile
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/Makefile
@@ -1,12 +1,12 @@
 obj-$(CONFIG_IWLMVM)   += iwlmvm.o
 iwlmvm-y += fw.o mac80211.o nvm.o ops.o phy-ctxt.o mac-ctxt.o
-iwlmvm-y += utils.o rx.o tx.o binding.o quota.o sta.o sf.o
+iwlmvm-y += utils.o rx.o rxmq.o tx.o binding.o quota.o sta.o sf.o
 iwlmvm-y += scan.o time-event.o rs.o
 iwlmvm-y += power.o coex.o coex_legacy.o
 iwlmvm-y += tt.o offloading.o tdls.o
 iwlmvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o debugfs-vif.o
 iwlmvm-$(CONFIG_IWLWIFI_LEDS) += led.o
-iwlmvm-y += tof.o
-iwlmvm-$(CONFIG_PM_SLEEP) += d3.o
+iwlmvm-y += tof.o fw-dbg.o
+iwlmvm-$(CONFIG_PM) += d3.o
 
 ccflags-y += -D__CHECK_ENDIAN__ -I$(src)/../
diff --git a/drivers/net/wireless/iwlwifi/mvm/binding.c b/drivers/net/wireless/intel/iwlwifi/mvm/binding.c
index a1376539d2dc..7cb68f6ed1b0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/binding.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/binding.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/intel/iwlwifi/mvm/coex.c
index e290ac67d975..2e098f8e0f83 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/coex.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -443,11 +443,8 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 	if (iwl_mvm_bt_is_plcr_supported(mvm))
 		bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_CORUN_ENABLED);
 
-	if (IWL_MVM_BT_COEX_MPLUT) {
+	if (iwl_mvm_is_mplut_supported(mvm))
 		bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_MPLUT_ENABLED);
-		bt_cmd.enabled_modules |=
-			cpu_to_le32(BT_COEX_MPLUT_BOOST_ENABLED);
-	}
 
 	bt_cmd.enabled_modules |= cpu_to_le32(BT_COEX_HIGH_BAND_RET);
 
@@ -904,6 +901,7 @@ u8 iwl_mvm_bt_coex_tx_prio(struct iwl_mvm *mvm, struct ieee80211_hdr *hdr,
 			   struct ieee80211_tx_info *info, u8 ac)
 {
 	__le16 fc = hdr->frame_control;
+	bool mplut_enabled = iwl_mvm_is_mplut_supported(mvm);
 
 	if (info->band != IEEE80211_BAND_2GHZ)
 		return 0;
@@ -911,22 +909,27 @@ u8 iwl_mvm_bt_coex_tx_prio(struct iwl_mvm *mvm, struct ieee80211_hdr *hdr,
 	if (unlikely(mvm->bt_tx_prio))
 		return mvm->bt_tx_prio - 1;
 
-	/* High prio packet (wrt. BT coex) if it is EAPOL, MCAST or MGMT */
-	if (info->control.flags & IEEE80211_TX_CTRL_PORT_CTRL_PROTO ||
-	     is_multicast_ether_addr(hdr->addr1) ||
-	     ieee80211_is_ctl(fc) || ieee80211_is_mgmt(fc) ||
-	     ieee80211_is_nullfunc(fc) || ieee80211_is_qos_nullfunc(fc))
-		return 3;
-
-	switch (ac) {
-	case IEEE80211_AC_BE:
-		return 1;
-	case IEEE80211_AC_VO:
+	if (likely(ieee80211_is_data(fc))) {
+		if (likely(ieee80211_is_data_qos(fc))) {
+			switch (ac) {
+			case IEEE80211_AC_BE:
+				return mplut_enabled ? 1 : 0;
+			case IEEE80211_AC_VI:
+				return mplut_enabled ? 2 : 3;
+			case IEEE80211_AC_VO:
+				return 3;
+			default:
+				return 0;
+			}
+		} else if (is_multicast_ether_addr(hdr->addr1)) {
+			return 3;
+		} else
+			return 0;
+	} else if (ieee80211_is_mgmt(fc)) {
+		return ieee80211_is_disassoc(fc) ? 0 : 3;
+	} else if (ieee80211_is_ctl(fc)) {
+		/* ignore cfend and cfendack frames as we never send those */
 		return 3;
-	case IEEE80211_AC_VI:
-		return 2;
-	default:
-		break;
 	}
 
 	return 0;
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c b/drivers/net/wireless/intel/iwlwifi/mvm/coex_legacy.c
index 61c07b05fcaa..015045733444 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/coex_legacy.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/constants.h b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
index 5c21231e195d..b00c03fcd447 100644
--- a/drivers/net/wireless/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/constants.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -106,6 +106,7 @@
 #define IWL_MVM_RS_RSSI_BASED_INIT_RATE         0
 #define IWL_MVM_RS_80_20_FAR_RANGE_TWEAK	1
 #define IWL_MVM_TOF_IS_RESPONDER		0
+#define IWL_MVM_SW_TX_CSUM_OFFLOAD		0
 #define IWL_MVM_RS_NUM_TRY_BEFORE_ANT_TOGGLE    1
 #define IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE      2
 #define IWL_MVM_RS_HT_VHT_RETRIES_PER_RATE_TW   1
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/intel/iwlwifi/mvm/d3.c
index 29ae58ebf223..d3e21d95cece 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/d3.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -104,9 +104,13 @@ void iwl_mvm_ipv6_addr_change(struct ieee80211_hw *hw,
 	struct inet6_ifaddr *ifa;
 	int idx = 0;
 
+	memset(mvmvif->tentative_addrs, 0, sizeof(mvmvif->tentative_addrs));
+
 	read_lock_bh(&idev->lock);
 	list_for_each_entry(ifa, &idev->addr_list, if_list) {
 		mvmvif->target_ipv6_addrs[idx] = ifa->addr;
+		if (ifa->flags & IFA_F_TENTATIVE)
+			__set_bit(idx, mvmvif->tentative_addrs);
 		idx++;
 		if (idx >= IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX)
 			break;
@@ -133,10 +137,32 @@ static void iwl_mvm_convert_p1k(u16 *p1k, __le16 *out)
 		out[i] = cpu_to_le16(p1k[i]);
 }
 
+static const u8 *iwl_mvm_find_max_pn(struct ieee80211_key_conf *key,
+				     struct iwl_mvm_key_pn *ptk_pn,
+				     struct ieee80211_key_seq *seq,
+				     int tid, int queues)
+{
+	const u8 *ret = seq->ccmp.pn;
+	int i;
+
+	/* get the PN from mac80211, used on the default queue */
+	ieee80211_get_key_rx_seq(key, tid, seq);
+
+	/* and use the internal data for the other queues */
+	for (i = 1; i < queues; i++) {
+		const u8 *tmp = ptk_pn->q[i].pn[tid];
+
+		if (memcmp(ret, tmp, IEEE80211_CCMP_PN_LEN) <= 0)
+			ret = tmp;
+	}
+
+	return ret;
+}
+
 struct wowlan_key_data {
 	struct iwl_wowlan_rsc_tsc_params_cmd *rsc_tsc;
 	struct iwl_wowlan_tkip_params_cmd *tkip;
-	bool error, use_rsc_tsc, use_tkip;
+	bool error, use_rsc_tsc, use_tkip, configure_keys;
 	int wep_key_idx;
 };
 
@@ -158,8 +184,6 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 	u16 p1k[IWL_P1K_SIZE];
 	int ret, i;
 
-	mutex_lock(&mvm->mutex);
-
 	switch (key->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104: { /* hack it for now */
@@ -195,20 +219,25 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 			wkc.wep_key.key_offset = data->wep_key_idx;
 		}
 
-		ret = iwl_mvm_send_cmd_pdu(mvm, WEP_KEY, 0, sizeof(wkc), &wkc);
-		data->error = ret != 0;
-
-		mvm->ptk_ivlen = key->iv_len;
-		mvm->ptk_icvlen = key->icv_len;
-		mvm->gtk_ivlen = key->iv_len;
-		mvm->gtk_icvlen = key->icv_len;
+		if (data->configure_keys) {
+			mutex_lock(&mvm->mutex);
+			ret = iwl_mvm_send_cmd_pdu(mvm, WEP_KEY, 0,
+						   sizeof(wkc), &wkc);
+			data->error = ret != 0;
+
+			mvm->ptk_ivlen = key->iv_len;
+			mvm->ptk_icvlen = key->icv_len;
+			mvm->gtk_ivlen = key->iv_len;
+			mvm->gtk_icvlen = key->icv_len;
+			mutex_unlock(&mvm->mutex);
+		}
 
 		/* don't upload key again */
-		goto out_unlock;
+		return;
 	}
 	default:
 		data->error = true;
-		goto out_unlock;
+		return;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
 		/*
 		 * Ignore CMAC keys -- the WoWLAN firmware doesn't support them
@@ -217,7 +246,7 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 		 * IGTK for anything. This means we could spuriously wake up or
 		 * be deauthenticated, but that was considered acceptable.
 		 */
-		goto out_unlock;
+		return;
 	case WLAN_CIPHER_SUITE_TKIP:
 		if (sta) {
 			tkip_sc = data->rsc_tsc->all_tsc_rsc.tkip.unicast_rsc;
@@ -287,46 +316,71 @@ static void iwl_mvm_wowlan_program_keys(struct ieee80211_hw *hw,
 
 		/*
 		 * For non-QoS this relies on the fact that both the uCode and
-		 * mac80211 use TID 0 for checking the IV in the frames.
+		 * mac80211/our RX code use TID 0 for checking the PN.
 		 */
-		for (i = 0; i < IWL_NUM_RSC; i++) {
-			u8 *pn = seq.ccmp.pn;
+		if (sta && iwl_mvm_has_new_rx_api(mvm)) {
+			struct iwl_mvm_sta *mvmsta;
+			struct iwl_mvm_key_pn *ptk_pn;
+			const u8 *pn;
+
+			mvmsta = iwl_mvm_sta_from_mac80211(sta);
+			ptk_pn = rcu_dereference_protected(
+						mvmsta->ptk_pn[key->keyidx],
+						lockdep_is_held(&mvm->mutex));
+			if (WARN_ON(!ptk_pn))
+				break;
 
-			ieee80211_get_key_rx_seq(key, i, &seq);
-			aes_sc[i].pn = cpu_to_le64((u64)pn[5] |
-						   ((u64)pn[4] << 8) |
-						   ((u64)pn[3] << 16) |
-						   ((u64)pn[2] << 24) |
-						   ((u64)pn[1] << 32) |
-						   ((u64)pn[0] << 40));
+			for (i = 0; i < IWL_MAX_TID_COUNT; i++) {
+				pn = iwl_mvm_find_max_pn(key, ptk_pn, &seq, i,
+						mvm->trans->num_rx_queues);
+				aes_sc[i].pn = cpu_to_le64((u64)pn[5] |
+							   ((u64)pn[4] << 8) |
+							   ((u64)pn[3] << 16) |
+							   ((u64)pn[2] << 24) |
+							   ((u64)pn[1] << 32) |
+							   ((u64)pn[0] << 40));
+			}
+		} else {
+			for (i = 0; i < IWL_NUM_RSC; i++) {
+				u8 *pn = seq.ccmp.pn;
+
+				ieee80211_get_key_rx_seq(key, i, &seq);
+				aes_sc[i].pn = cpu_to_le64((u64)pn[5] |
+							   ((u64)pn[4] << 8) |
+							   ((u64)pn[3] << 16) |
+							   ((u64)pn[2] << 24) |
+							   ((u64)pn[1] << 32) |
+							   ((u64)pn[0] << 40));
+			}
 		}
 		data->use_rsc_tsc = true;
 		break;
 	}
 
-	/*
-	 * The D3 firmware hardcodes the key offset 0 as the key it uses
-	 * to transmit packets to the AP, i.e. the PTK.
-	 */
-	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
-		mvm->ptk_ivlen = key->iv_len;
-		mvm->ptk_icvlen = key->icv_len;
-		ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, 0);
-	} else {
+	if (data->configure_keys) {
+		mutex_lock(&mvm->mutex);
 		/*
-		 * firmware only supports TSC/RSC for a single key,
-		 * so if there are multiple keep overwriting them
-		 * with new ones -- this relies on mac80211 doing
-		 * list_add_tail().
+		 * The D3 firmware hardcodes the key offset 0 as the key it
+		 * uses to transmit packets to the AP, i.e. the PTK.
 		 */
-		mvm->gtk_ivlen = key->iv_len;
-		mvm->gtk_icvlen = key->icv_len;
-		ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, 1);
+		if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
+			mvm->ptk_ivlen = key->iv_len;
+			mvm->ptk_icvlen = key->icv_len;
+			ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, 0);
+		} else {
+			/*
+			 * firmware only supports TSC/RSC for a single key,
+			 * so if there are multiple keep overwriting them
+			 * with new ones -- this relies on mac80211 doing
+			 * list_add_tail().
+			 */
+			mvm->gtk_ivlen = key->iv_len;
+			mvm->gtk_icvlen = key->icv_len;
+			ret = iwl_mvm_set_sta_key(mvm, vif, sta, key, 1);
+		}
+		mutex_unlock(&mvm->mutex);
+		data->error = ret != 0;
 	}
-
-	data->error = ret != 0;
-out_unlock:
-	mutex_unlock(&mvm->mutex);
 }
 
 static int iwl_mvm_send_patterns(struct iwl_mvm *mvm,
@@ -771,6 +825,9 @@ static int iwl_mvm_switch_to_d3(struct iwl_mvm *mvm)
 	 */
 	set_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status);
 
+	/* the fw is reset, so all the keys are cleared */
+	memset(mvm->fw_key_table, 0, sizeof(mvm->fw_key_table));
+
 	mvm->ptk_ivlen = 0;
 	mvm->ptk_icvlen = 0;
 	mvm->ptk_ivlen = 0;
@@ -793,6 +850,8 @@ iwl_mvm_get_wowlan_config(struct iwl_mvm *mvm,
 
 	wowlan_config_cmd->is_11n_connection =
 					ap_sta->ht_cap.ht_supported;
+	wowlan_config_cmd->flags = ENABLE_L3_FILTERING |
+		ENABLE_NBNS_FILTERING | ENABLE_DHCP_FILTERING;
 
 	/* Query the last used seqno and set it */
 	ret = iwl_mvm_get_last_nonqos_seq(mvm, vif);
@@ -842,20 +901,127 @@ iwl_mvm_get_wowlan_config(struct iwl_mvm *mvm,
 	return 0;
 }
 
-static int
-iwl_mvm_wowlan_config(struct iwl_mvm *mvm,
-		      struct cfg80211_wowlan *wowlan,
-		      struct iwl_wowlan_config_cmd *wowlan_config_cmd,
-		      struct ieee80211_vif *vif, struct iwl_mvm_vif *mvmvif,
-		      struct ieee80211_sta *ap_sta)
+static void
+iwl_mvm_iter_d0i3_ap_keys(struct iwl_mvm *mvm,
+			  struct ieee80211_vif *vif,
+			  void (*iter)(struct ieee80211_hw *hw,
+				       struct ieee80211_vif *vif,
+				       struct ieee80211_sta *sta,
+				       struct ieee80211_key_conf *key,
+				       void *data),
+			  void *data)
+{
+	struct ieee80211_sta *ap_sta;
+
+	rcu_read_lock();
+
+	ap_sta = rcu_dereference(mvm->fw_id_to_mac_id[mvm->d0i3_ap_sta_id]);
+	if (IS_ERR_OR_NULL(ap_sta))
+		goto out;
+
+	ieee80211_iter_keys_rcu(mvm->hw, vif, iter, data);
+out:
+	rcu_read_unlock();
+}
+
+int iwl_mvm_wowlan_config_key_params(struct iwl_mvm *mvm,
+				     struct ieee80211_vif *vif,
+				     bool d0i3,
+				     u32 cmd_flags)
 {
 	struct iwl_wowlan_kek_kck_material_cmd kek_kck_cmd = {};
 	struct iwl_wowlan_tkip_params_cmd tkip_cmd = {};
 	struct wowlan_key_data key_data = {
+		.configure_keys = !d0i3,
 		.use_rsc_tsc = false,
 		.tkip = &tkip_cmd,
 		.use_tkip = false,
 	};
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	int ret;
+
+	key_data.rsc_tsc = kzalloc(sizeof(*key_data.rsc_tsc), GFP_KERNEL);
+	if (!key_data.rsc_tsc)
+		return -ENOMEM;
+
+	/*
+	 * if we have to configure keys, call ieee80211_iter_keys(),
+	 * as we need non-atomic context in order to take the
+	 * required locks.
+	 * for the d0i3 we can't use ieee80211_iter_keys(), as
+	 * taking (almost) any mutex might result in deadlock.
+	 */
+	if (!d0i3) {
+		/*
+		 * Note that currently we don't propagate cmd_flags
+		 * to the iterator. In case of key_data.configure_keys,
+		 * all the configured commands are SYNC, and
+		 * iwl_mvm_wowlan_program_keys() will take care of
+		 * locking/unlocking mvm->mutex.
+		 */
+		ieee80211_iter_keys(mvm->hw, vif,
+				    iwl_mvm_wowlan_program_keys,
+				    &key_data);
+	} else {
+		iwl_mvm_iter_d0i3_ap_keys(mvm, vif,
+					  iwl_mvm_wowlan_program_keys,
+					  &key_data);
+	}
+
+	if (key_data.error) {
+		ret = -EIO;
+		goto out;
+	}
+
+	if (key_data.use_rsc_tsc) {
+		ret = iwl_mvm_send_cmd_pdu(mvm,
+					   WOWLAN_TSC_RSC_PARAM, cmd_flags,
+					   sizeof(*key_data.rsc_tsc),
+					   key_data.rsc_tsc);
+		if (ret)
+			goto out;
+	}
+
+	if (key_data.use_tkip) {
+		ret = iwl_mvm_send_cmd_pdu(mvm,
+					   WOWLAN_TKIP_PARAM,
+					   cmd_flags, sizeof(tkip_cmd),
+					   &tkip_cmd);
+		if (ret)
+			goto out;
+	}
+
+	/* configure rekey data only if offloaded rekey is supported (d3) */
+	if (mvmvif->rekey_data.valid && !d0i3) {
+		memset(&kek_kck_cmd, 0, sizeof(kek_kck_cmd));
+		memcpy(kek_kck_cmd.kck, mvmvif->rekey_data.kck,
+		       NL80211_KCK_LEN);
+		kek_kck_cmd.kck_len = cpu_to_le16(NL80211_KCK_LEN);
+		memcpy(kek_kck_cmd.kek, mvmvif->rekey_data.kek,
+		       NL80211_KEK_LEN);
+		kek_kck_cmd.kek_len = cpu_to_le16(NL80211_KEK_LEN);
+		kek_kck_cmd.replay_ctr = mvmvif->rekey_data.replay_ctr;
+
+		ret = iwl_mvm_send_cmd_pdu(mvm,
+					   WOWLAN_KEK_KCK_MATERIAL, cmd_flags,
+					   sizeof(kek_kck_cmd),
+					   &kek_kck_cmd);
+		if (ret)
+			goto out;
+	}
+	ret = 0;
+out:
+	kfree(key_data.rsc_tsc);
+	return ret;
+}
+
+static int
+iwl_mvm_wowlan_config(struct iwl_mvm *mvm,
+		      struct cfg80211_wowlan *wowlan,
+		      struct iwl_wowlan_config_cmd *wowlan_config_cmd,
+		      struct ieee80211_vif *vif, struct iwl_mvm_vif *mvmvif,
+		      struct ieee80211_sta *ap_sta)
+{
 	int ret;
 
 	ret = iwl_mvm_switch_to_d3(mvm);
@@ -866,10 +1032,6 @@ iwl_mvm_wowlan_config(struct iwl_mvm *mvm,
 	if (ret)
 		return ret;
 
-	key_data.rsc_tsc = kzalloc(sizeof(*key_data.rsc_tsc), GFP_KERNEL);
-	if (!key_data.rsc_tsc)
-		return -ENOMEM;
-
 	if (!iwlwifi_mod_params.sw_crypto) {
 		/*
 		 * This needs to be unlocked due to lock ordering
@@ -877,74 +1039,28 @@ iwl_mvm_wowlan_config(struct iwl_mvm *mvm,
 		 * that isn't really a problem though.
 		 */
 		mutex_unlock(&mvm->mutex);
-		ieee80211_iter_keys(mvm->hw, vif,
-				    iwl_mvm_wowlan_program_keys,
-				    &key_data);
+		ret = iwl_mvm_wowlan_config_key_params(mvm, vif, false,
+						       CMD_ASYNC);
 		mutex_lock(&mvm->mutex);
-		if (key_data.error) {
-			ret = -EIO;
-			goto out;
-		}
-
-		if (key_data.use_rsc_tsc) {
-			struct iwl_host_cmd rsc_tsc_cmd = {
-				.id = WOWLAN_TSC_RSC_PARAM,
-				.data[0] = key_data.rsc_tsc,
-				.dataflags[0] = IWL_HCMD_DFL_NOCOPY,
-				.len[0] = sizeof(*key_data.rsc_tsc),
-			};
-
-			ret = iwl_mvm_send_cmd(mvm, &rsc_tsc_cmd);
-			if (ret)
-				goto out;
-		}
-
-		if (key_data.use_tkip) {
-			ret = iwl_mvm_send_cmd_pdu(mvm,
-						   WOWLAN_TKIP_PARAM,
-						   0, sizeof(tkip_cmd),
-						   &tkip_cmd);
-			if (ret)
-				goto out;
-		}
-
-		if (mvmvif->rekey_data.valid) {
-			memset(&kek_kck_cmd, 0, sizeof(kek_kck_cmd));
-			memcpy(kek_kck_cmd.kck, mvmvif->rekey_data.kck,
-			       NL80211_KCK_LEN);
-			kek_kck_cmd.kck_len = cpu_to_le16(NL80211_KCK_LEN);
-			memcpy(kek_kck_cmd.kek, mvmvif->rekey_data.kek,
-			       NL80211_KEK_LEN);
-			kek_kck_cmd.kek_len = cpu_to_le16(NL80211_KEK_LEN);
-			kek_kck_cmd.replay_ctr = mvmvif->rekey_data.replay_ctr;
-
-			ret = iwl_mvm_send_cmd_pdu(mvm,
-						   WOWLAN_KEK_KCK_MATERIAL, 0,
-						   sizeof(kek_kck_cmd),
-						   &kek_kck_cmd);
-			if (ret)
-				goto out;
-		}
+		if (ret)
+			return ret;
 	}
 
 	ret = iwl_mvm_send_cmd_pdu(mvm, WOWLAN_CONFIGURATION, 0,
 				   sizeof(*wowlan_config_cmd),
 				   wowlan_config_cmd);
 	if (ret)
-		goto out;
+		return ret;
 
 	ret = iwl_mvm_send_patterns(mvm, wowlan);
 	if (ret)
-		goto out;
+		return ret;
 
-	ret = iwl_mvm_send_proto_offload(mvm, vif, false, 0);
+	ret = iwl_mvm_send_proto_offload(mvm, vif, false, true, 0);
 	if (ret)
-		goto out;
+		return ret;
 
 	ret = iwl_mvm_send_remote_wake_cfg(mvm, vif, wowlan->tcp);
-
-out:
-	kfree(key_data.rsc_tsc);
 	return ret;
 }
 
@@ -1057,13 +1173,13 @@ static int __iwl_mvm_suspend(struct ieee80211_hw *hw,
 
 	if (mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT) {
 		/* if we're not associated, this must be netdetect */
-		if (!wowlan->nd_config && !mvm->nd_config) {
+		if (!wowlan->nd_config) {
 			ret = 1;
 			goto out_noreset;
 		}
 
 		ret = iwl_mvm_netdetect_config(
-			mvm, wowlan, wowlan->nd_config ?: mvm->nd_config, vif);
+			mvm, wowlan, wowlan->nd_config, vif);
 		if (ret)
 			goto out;
 
@@ -1159,19 +1275,20 @@ remove_notif:
 int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_trans *trans = mvm->trans;
 	int ret;
 
 	/* make sure the d0i3 exit work is not pending */
 	flush_work(&mvm->d0i3_exit_work);
 
-	ret = iwl_trans_suspend(mvm->trans);
+	ret = iwl_trans_suspend(trans);
 	if (ret)
 		return ret;
 
-	mvm->trans->wowlan_d0i3 = wowlan->any;
-	if (mvm->trans->wowlan_d0i3) {
-		/* 'any' trigger means d0i3 usage */
-		if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND) {
+	if (wowlan->any) {
+		trans->system_pm_mode = IWL_PLAT_PM_MODE_D0I3;
+
+		if (iwl_mvm_enter_d0i3_on_suspend(mvm)) {
 			ret = iwl_mvm_enter_d0i3_sync(mvm);
 
 			if (ret)
@@ -1182,11 +1299,13 @@ int iwl_mvm_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 		__set_bit(D0I3_DEFER_WAKEUP, &mvm->d0i3_suspend_flags);
 		mutex_unlock(&mvm->d0i3_suspend_mutex);
 
-		iwl_trans_d3_suspend(mvm->trans, false);
+		iwl_trans_d3_suspend(trans, false);
 
 		return 0;
 	}
 
+	trans->system_pm_mode = IWL_PLAT_PM_MODE_D3;
+
 	return __iwl_mvm_suspend(hw, wowlan, false);
 }
 
@@ -1216,6 +1335,8 @@ static void iwl_mvm_report_wakeup_reasons(struct iwl_mvm *mvm,
 		goto report;
 	}
 
+	pm_wakeup_event(mvm->dev, 0);
+
 	if (reasons & IWL_WOWLAN_WAKEUP_BY_MAGIC_PACKET)
 		wakeup.magic_pkt = true;
 
@@ -1351,18 +1472,42 @@ static void iwl_mvm_tkip_sc_to_seq(struct tkip_sc *sc,
 	seq->tkip.iv16 = le16_to_cpu(sc->iv16);
 }
 
-static void iwl_mvm_set_aes_rx_seq(struct aes_sc *scs,
+static void iwl_mvm_set_aes_rx_seq(struct iwl_mvm *mvm, struct aes_sc *scs,
+				   struct ieee80211_sta *sta,
 				   struct ieee80211_key_conf *key)
 {
 	int tid;
 
 	BUILD_BUG_ON(IWL_NUM_RSC != IEEE80211_NUM_TIDS);
 
-	for (tid = 0; tid < IWL_NUM_RSC; tid++) {
-		struct ieee80211_key_seq seq = {};
+	if (sta && iwl_mvm_has_new_rx_api(mvm)) {
+		struct iwl_mvm_sta *mvmsta;
+		struct iwl_mvm_key_pn *ptk_pn;
 
-		iwl_mvm_aes_sc_to_seq(&scs[tid], &seq);
-		ieee80211_set_key_rx_seq(key, tid, &seq);
+		mvmsta = iwl_mvm_sta_from_mac80211(sta);
+
+		ptk_pn = rcu_dereference_protected(mvmsta->ptk_pn[key->keyidx],
+						   lockdep_is_held(&mvm->mutex));
+		if (WARN_ON(!ptk_pn))
+			return;
+
+		for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++) {
+			struct ieee80211_key_seq seq = {};
+			int i;
+
+			iwl_mvm_aes_sc_to_seq(&scs[tid], &seq);
+			ieee80211_set_key_rx_seq(key, tid, &seq);
+			for (i = 1; i < mvm->trans->num_rx_queues; i++)
+				memcpy(ptk_pn->q[i].pn[tid],
+				       seq.ccmp.pn, IEEE80211_CCMP_PN_LEN);
+		}
+	} else {
+		for (tid = 0; tid < IWL_NUM_RSC; tid++) {
+			struct ieee80211_key_seq seq = {};
+
+			iwl_mvm_aes_sc_to_seq(&scs[tid], &seq);
+			ieee80211_set_key_rx_seq(key, tid, &seq);
+		}
 	}
 }
 
@@ -1381,14 +1526,15 @@ static void iwl_mvm_set_tkip_rx_seq(struct tkip_sc *scs,
 	}
 }
 
-static void iwl_mvm_set_key_rx_seq(struct ieee80211_key_conf *key,
+static void iwl_mvm_set_key_rx_seq(struct iwl_mvm *mvm,
+				   struct ieee80211_key_conf *key,
 				   struct iwl_wowlan_status *status)
 {
 	union iwl_all_tsc_rsc *rsc = &status->gtk.rsc.all_tsc_rsc;
 
 	switch (key->cipher) {
 	case WLAN_CIPHER_SUITE_CCMP:
-		iwl_mvm_set_aes_rx_seq(rsc->aes.multicast_rsc, key);
+		iwl_mvm_set_aes_rx_seq(mvm, rsc->aes.multicast_rsc, NULL, key);
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
 		iwl_mvm_set_tkip_rx_seq(rsc->tkip.multicast_rsc, key);
@@ -1399,6 +1545,7 @@ static void iwl_mvm_set_key_rx_seq(struct ieee80211_key_conf *key,
 }
 
 struct iwl_mvm_d3_gtk_iter_data {
+	struct iwl_mvm *mvm;
 	struct iwl_wowlan_status *status;
 	void *last_gtk;
 	u32 cipher;
@@ -1406,7 +1553,7 @@ struct iwl_mvm_d3_gtk_iter_data {
 	int num_keys;
 };
 
-static void iwl_mvm_d3_update_gtks(struct ieee80211_hw *hw,
+static void iwl_mvm_d3_update_keys(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif,
 				   struct ieee80211_sta *sta,
 				   struct ieee80211_key_conf *key,
@@ -1447,7 +1594,8 @@ static void iwl_mvm_d3_update_gtks(struct ieee80211_hw *hw,
 
 		switch (key->cipher) {
 		case WLAN_CIPHER_SUITE_CCMP:
-			iwl_mvm_set_aes_rx_seq(sc->aes.unicast_rsc, key);
+			iwl_mvm_set_aes_rx_seq(data->mvm, sc->aes.unicast_rsc,
+					       sta, key);
 			atomic64_set(&key->tx_pn, le64_to_cpu(sc->aes.tsc.pn));
 			break;
 		case WLAN_CIPHER_SUITE_TKIP:
@@ -1470,7 +1618,7 @@ static void iwl_mvm_d3_update_gtks(struct ieee80211_hw *hw,
 	if (data->status->num_of_gtk_rekeys)
 		ieee80211_remove_key(key);
 	else if (data->last_gtk == key)
-		iwl_mvm_set_key_rx_seq(key, data->status);
+		iwl_mvm_set_key_rx_seq(data->mvm, key, data->status);
 }
 
 static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
@@ -1479,6 +1627,7 @@ static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_d3_gtk_iter_data gtkdata = {
+		.mvm = mvm,
 		.status = status,
 	};
 	u32 disconnection_reasons =
@@ -1494,7 +1643,7 @@ static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
 	/* find last GTK that we used initially, if any */
 	gtkdata.find_phase = true;
 	ieee80211_iter_keys(mvm->hw, vif,
-			    iwl_mvm_d3_update_gtks, &gtkdata);
+			    iwl_mvm_d3_update_keys, &gtkdata);
 	/* not trying to keep connections with MFP/unhandled ciphers */
 	if (gtkdata.unhandled_cipher)
 		return false;
@@ -1509,7 +1658,7 @@ static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
 	 */
 	gtkdata.find_phase = false;
 	ieee80211_iter_keys(mvm->hw, vif,
-			    iwl_mvm_d3_update_gtks, &gtkdata);
+			    iwl_mvm_d3_update_keys, &gtkdata);
 
 	if (status->num_of_gtk_rekeys) {
 		struct ieee80211_key_conf *key;
@@ -1540,7 +1689,7 @@ static bool iwl_mvm_setup_connection_keep(struct iwl_mvm *mvm,
 		key = ieee80211_gtk_rekey_add(vif, &conf.conf);
 		if (IS_ERR(key))
 			return false;
-		iwl_mvm_set_key_rx_seq(key, status);
+		iwl_mvm_set_key_rx_seq(mvm, key, status);
 	}
 
 	if (status->num_of_gtk_rekeys) {
@@ -1689,6 +1838,30 @@ out_unlock:
 	return false;
 }
 
+void iwl_mvm_d0i3_update_keys(struct iwl_mvm *mvm,
+			      struct ieee80211_vif *vif,
+			      struct iwl_wowlan_status *status)
+{
+	struct iwl_mvm_d3_gtk_iter_data gtkdata = {
+		.mvm = mvm,
+		.status = status,
+	};
+
+	/*
+	 * rekey handling requires taking locks that can't be taken now.
+	 * however, d0i3 doesn't offload rekey, so we're fine.
+	 */
+	if (WARN_ON_ONCE(status->num_of_gtk_rekeys))
+		return;
+
+	/* find last GTK that we used initially, if any */
+	gtkdata.find_phase = true;
+	iwl_mvm_iter_d0i3_ap_keys(mvm, vif, iwl_mvm_d3_update_keys, &gtkdata);
+
+	gtkdata.find_phase = false;
+	iwl_mvm_iter_d0i3_ap_keys(mvm, vif, iwl_mvm_d3_update_keys, &gtkdata);
+}
+
 struct iwl_mvm_nd_query_results {
 	u32 matched_profiles;
 	struct iwl_scan_offload_profile_match matches[IWL_SCAN_MAX_PROFILES];
@@ -1947,8 +2120,9 @@ static int iwl_mvm_resume_d0i3(struct iwl_mvm *mvm)
 {
 	bool exit_now;
 	enum iwl_d3_status d3_status;
+	struct iwl_trans *trans = mvm->trans;
 
-	iwl_trans_d3_resume(mvm->trans, &d3_status, false);
+	iwl_trans_d3_resume(trans, &d3_status, false);
 
 	/*
 	 * make sure to clear D0I3_DEFER_WAKEUP before
@@ -1965,9 +2139,9 @@ static int iwl_mvm_resume_d0i3(struct iwl_mvm *mvm)
 		_iwl_mvm_exit_d0i3(mvm);
 	}
 
-	iwl_trans_resume(mvm->trans);
+	iwl_trans_resume(trans);
 
-	if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND) {
+	if (iwl_mvm_enter_d0i3_on_suspend(mvm)) {
 		int ret = iwl_mvm_exit_d0i3(mvm->hw->priv);
 
 		if (ret)
@@ -1983,12 +2157,16 @@ static int iwl_mvm_resume_d0i3(struct iwl_mvm *mvm)
 int iwl_mvm_resume(struct ieee80211_hw *hw)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	int ret;
 
-	/* 'any' trigger means d0i3 was used */
-	if (hw->wiphy->wowlan_config->any)
-		return iwl_mvm_resume_d0i3(mvm);
+	if (mvm->trans->system_pm_mode == IWL_PLAT_PM_MODE_D0I3)
+		ret = iwl_mvm_resume_d0i3(mvm);
 	else
-		return iwl_mvm_resume_d3(mvm);
+		ret = iwl_mvm_resume_d3(mvm);
+
+	mvm->trans->system_pm_mode = IWL_PLAT_PM_MODE_DISABLED;
+
+	return ret;
 }
 
 void iwl_mvm_set_wakeup(struct ieee80211_hw *hw, bool enabled)
@@ -2012,6 +2190,8 @@ static int iwl_mvm_d3_test_open(struct inode *inode, struct file *file)
 	ieee80211_stop_queues(mvm->hw);
 	synchronize_net();
 
+	mvm->trans->system_pm_mode = IWL_PLAT_PM_MODE_D3;
+
 	/* start pseudo D3 */
 	rtnl_lock();
 	err = __iwl_mvm_suspend(mvm->hw, mvm->hw->wiphy->wowlan_config, true);
@@ -2066,9 +2246,13 @@ static int iwl_mvm_d3_test_release(struct inode *inode, struct file *file)
 	int remaining_time = 10;
 
 	mvm->d3_test_active = false;
+
 	rtnl_lock();
 	__iwl_mvm_resume(mvm, true);
 	rtnl_unlock();
+
+	mvm->trans->system_pm_mode = IWL_PLAT_PM_MODE_DISABLED;
+
 	iwl_abort_notification_waits(&mvm->notif_wait);
 	ieee80211_restart_hw(mvm->hw);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c
index 7904b41a04c6..9e0d46368cdd 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs-vif.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c
index 05928fb4021d..90500e2d107b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -65,6 +65,7 @@
 #include <linux/vmalloc.h>
 
 #include "mvm.h"
+#include "fw-dbg.h"
 #include "sta.h"
 #include "iwl-io.h"
 #include "debugfs.h"
@@ -512,6 +513,10 @@ static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
 		pos += scnprintf(buf+pos, bufsz-pos,
 				 "antenna isolation = %d CORUN LUT index = %d\n",
 				 mvm->last_ant_isol, mvm->last_corun_lut);
+		pos += scnprintf(buf + pos, bufsz - pos, "bt_rrc = %d\n",
+				 notif->rrc_enabled);
+		pos += scnprintf(buf + pos, bufsz - pos, "bt_ttc = %d\n",
+				 notif->ttc_enabled);
 	} else {
 		struct iwl_bt_coex_profile_notif *notif =
 			&mvm->last_bt_notif;
@@ -530,8 +535,19 @@ static ssize_t iwl_dbgfs_bt_notif_read(struct file *file, char __user *user_buf,
 		pos += scnprintf(buf+pos, bufsz-pos,
 				 "antenna isolation = %d CORUN LUT index = %d\n",
 				 mvm->last_ant_isol, mvm->last_corun_lut);
+		pos += scnprintf(buf + pos, bufsz - pos, "bt_rrc = %d\n",
+				 (notif->ttc_rrc_status >> 4) & 0xF);
+		pos += scnprintf(buf + pos, bufsz - pos, "bt_ttc = %d\n",
+				 notif->ttc_rrc_status & 0xF);
 	}
 
+	pos += scnprintf(buf + pos, bufsz - pos, "sync_sco = %d\n",
+			 IWL_MVM_BT_COEX_SYNC2SCO);
+	pos += scnprintf(buf + pos, bufsz - pos, "mplut = %d\n",
+			 IWL_MVM_BT_COEX_MPLUT);
+	pos += scnprintf(buf + pos, bufsz - pos, "corunning = %d\n",
+			 IWL_MVM_BT_COEX_CORUNNING);
+
 	mutex_unlock(&mvm->mutex);
 
 	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
@@ -945,6 +961,44 @@ static ssize_t iwl_dbgfs_fw_dbg_conf_read(struct file *file,
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
 
+/*
+ * Enable / Disable continuous recording.
+ * Cause the FW to start continuous recording, by sending the relevant hcmd.
+ * Enable: input of every integer larger than 0, ENABLE_CONT_RECORDING.
+ * Disable: for 0 as input, DISABLE_CONT_RECORDING.
+ */
+static ssize_t iwl_dbgfs_cont_recording_write(struct iwl_mvm *mvm,
+					      char *buf, size_t count,
+					      loff_t *ppos)
+{
+	struct iwl_trans *trans = mvm->trans;
+	const struct iwl_fw_dbg_dest_tlv *dest = trans->dbg_dest_tlv;
+	struct iwl_continuous_record_cmd cont_rec = {};
+	int ret, rec_mode;
+
+	if (!dest)
+		return -EOPNOTSUPP;
+
+	if (dest->monitor_mode != SMEM_MODE ||
+	    trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
+		return -EOPNOTSUPP;
+
+	ret = kstrtouint(buf, 0, &rec_mode);
+	if (ret)
+		return ret;
+
+	cont_rec.record_mode.enable_recording = rec_mode ?
+		cpu_to_le16(ENABLE_CONT_RECORDING) :
+		cpu_to_le16(DISABLE_CONT_RECORDING);
+
+	mutex_lock(&mvm->mutex);
+	ret = iwl_mvm_send_cmd_pdu(mvm, LDBG_CONFIG_CMD, 0,
+				   sizeof(cont_rec), &cont_rec);
+	mutex_unlock(&mvm->mutex);
+
+	return ret ?: count;
+}
+
 static ssize_t iwl_dbgfs_fw_dbg_conf_write(struct iwl_mvm *mvm,
 					   char *buf, size_t count,
 					   loff_t *ppos)
@@ -975,7 +1029,8 @@ static ssize_t iwl_dbgfs_fw_dbg_collect_write(struct iwl_mvm *mvm,
 	if (ret)
 		return ret;
 
-	iwl_mvm_fw_dbg_collect(mvm, FW_DBG_TRIGGER_USER, NULL, 0, NULL);
+	iwl_mvm_fw_dbg_collect(mvm, FW_DBG_TRIGGER_USER, buf,
+			       (count - 1), NULL);
 
 	iwl_mvm_unref(mvm, IWL_MVM_REF_PRPH_WRITE);
 
@@ -1262,6 +1317,7 @@ static ssize_t iwl_dbgfs_d0i3_refs_read(struct file *file,
 	PRINT_MVM_REF(IWL_MVM_REF_EXIT_WORK);
 	PRINT_MVM_REF(IWL_MVM_REF_PROTECT_CSA);
 	PRINT_MVM_REF(IWL_MVM_REF_FW_DBG_COLLECT);
+	PRINT_MVM_REF(IWL_MVM_REF_INIT_UCODE);
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
 }
@@ -1396,7 +1452,8 @@ MVM_DEBUGFS_WRITE_FILE_OPS(bt_force_ant, 10);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(scan_ant_rxchain, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(d0i3_refs, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(fw_dbg_conf, 8);
-MVM_DEBUGFS_WRITE_FILE_OPS(fw_dbg_collect, 8);
+MVM_DEBUGFS_WRITE_FILE_OPS(fw_dbg_collect, 64);
+MVM_DEBUGFS_WRITE_FILE_OPS(cont_recording, 8);
 
 #ifdef CONFIG_IWLWIFI_BCAST_FILTERING
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(bcast_filters, 256);
@@ -1440,6 +1497,7 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	MVM_DEBUGFS_ADD_FILE(fw_dbg_conf, mvm->debugfs_dir, S_IRUSR | S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_dbg_collect, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(send_echo_cmd, mvm->debugfs_dir, S_IWUSR);
+	MVM_DEBUGFS_ADD_FILE(cont_recording, mvm->debugfs_dir, S_IWUSR);
 	if (!debugfs_create_bool("enable_scan_iteration_notif",
 				 S_IRUSR | S_IWUSR,
 				 mvm->debugfs_dir,
@@ -1476,10 +1534,6 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 		goto err;
 #endif
 
-	if (!debugfs_create_u8("low_latency_agg_frame_limit", S_IRUSR | S_IWUSR,
-			       mvm->debugfs_dir,
-			       &mvm->low_latency_agg_frame_limit))
-		goto err;
 	if (!debugfs_create_u8("ps_disabled", S_IRUSR,
 			       mvm->debugfs_dir, &mvm->ps_disabled))
 		goto err;
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.h b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.h
index 8c4190e7e027..ede6ef8d390e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/debugfs.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h
index d398a6102805..2a33b694ba10 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-coex.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h
index 20521bebb0b1..62b9a0a96700 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-d3.h
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -94,10 +95,14 @@ struct iwl_d3_manager_config {
  * enum iwl_d3_proto_offloads - enabled protocol offloads
  * @IWL_D3_PROTO_OFFLOAD_ARP: ARP data is enabled
  * @IWL_D3_PROTO_OFFLOAD_NS: NS (Neighbor Solicitation) is enabled
+ * @IWL_D3_PROTO_IPV4_VALID: IPv4 data is valid
+ * @IWL_D3_PROTO_IPV6_VALID: IPv6 data is valid
  */
 enum iwl_proto_offloads {
 	IWL_D3_PROTO_OFFLOAD_ARP = BIT(0),
 	IWL_D3_PROTO_OFFLOAD_NS = BIT(1),
+	IWL_D3_PROTO_IPV4_VALID = BIT(2),
+	IWL_D3_PROTO_IPV6_VALID = BIT(3),
 };
 
 #define IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V1	2
@@ -241,6 +246,13 @@ enum iwl_wowlan_wakeup_filters {
 	IWL_WOWLAN_WAKEUP_BCN_FILTERING			= BIT(16),
 }; /* WOWLAN_WAKEUP_FILTER_API_E_VER_4 */
 
+enum iwl_wowlan_flags {
+	IS_11W_ASSOC		= BIT(0),
+	ENABLE_L3_FILTERING	= BIT(1),
+	ENABLE_NBNS_FILTERING	= BIT(2),
+	ENABLE_DHCP_FILTERING	= BIT(3),
+};
+
 struct iwl_wowlan_config_cmd {
 	__le32 wakeup_filter;
 	__le16 non_qos_seq;
@@ -248,8 +260,9 @@ struct iwl_wowlan_config_cmd {
 	u8 wowlan_ba_teardown_tids;
 	u8 is_11n_connection;
 	u8 offloading_tid;
-	u8 reserved[3];
-} __packed; /* WOWLAN_CONFIG_API_S_VER_3 */
+	u8 flags;
+	u8 reserved[2];
+} __packed; /* WOWLAN_CONFIG_API_S_VER_4 */
 
 /*
  * WOWLAN_TSC_RSC_PARAMS
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h
index f3f3ee0a766b..95ac59d088b1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-mac.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-mac.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h
index c8f3e2536cbb..65a7c8a4cacf 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-power.h
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h
index 0f1ea80a55ef..ad9cc03e16c4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-rs.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rs.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-rx.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h
index 9b7e49d4620f..fb6d341d6f3d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-rx.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-rx.h
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -68,6 +68,8 @@
 #ifndef __fw_api_rx_h__
 #define __fw_api_rx_h__
 
+/* API for pre-9000 hardware */
+
 #define IWL_RX_INFO_PHY_CNT 8
 #define IWL_RX_INFO_ENERGY_ANT_ABC_IDX 1
 #define IWL_RX_INFO_ENERGY_ANT_A_MSK 0x000000ff
@@ -77,6 +79,11 @@
 #define IWL_RX_INFO_ENERGY_ANT_B_POS 8
 #define IWL_RX_INFO_ENERGY_ANT_C_POS 16
 
+enum iwl_mac_context_info {
+	MAC_CONTEXT_INFO_NONE,
+	MAC_CONTEXT_INFO_GSCAN,
+};
+
 /**
  * struct iwl_rx_phy_info - phy info
  * (REPLY_RX_PHY_CMD = 0xc0)
@@ -95,6 +102,8 @@
  * @frame_time: frame's time on the air, based on byte count and frame rate
  *	calculation
  * @mac_active_msk: what MACs were active when the frame was received
+ * @mac_context_info: additional info on the context in which the frame was
+ *	received as defined in &enum iwl_mac_context_info
  *
  * Before each Rx, the device sends this data. It contains PHY information
  * about the reception of the packet.
@@ -112,7 +121,8 @@ struct iwl_rx_phy_info {
 	__le32 non_cfg_phy[IWL_RX_INFO_PHY_CNT];
 	__le32 rate_n_flags;
 	__le32 byte_count;
-	__le16 mac_active_msk;
+	u8 mac_active_msk;
+	u8 mac_context_info;
 	__le16 frame_time;
 } __packed;
 
@@ -229,10 +239,130 @@ enum iwl_mvm_rx_status {
 	RX_MPDU_RES_STATUS_CSUM_DONE			= BIT(16),
 	RX_MPDU_RES_STATUS_CSUM_OK			= BIT(17),
 	RX_MPDU_RES_STATUS_HASH_INDEX_MSK		= (0x3F0000),
-	RX_MPDU_RES_STATUS_STA_ID_MSK			= (0x1f000000),
+	RX_MDPU_RES_STATUS_STA_ID_SHIFT			= 24,
+	RX_MPDU_RES_STATUS_STA_ID_MSK			= 0x1f << RX_MDPU_RES_STATUS_STA_ID_SHIFT,
 	RX_MPDU_RES_STATUS_RRF_KILL			= BIT(29),
 	RX_MPDU_RES_STATUS_FILTERING_MSK		= (0xc00000),
 	RX_MPDU_RES_STATUS2_FILTERING_MSK		= (0xc0000000),
 };
 
+/* 9000 series API */
+enum iwl_rx_mpdu_mac_flags1 {
+	IWL_RX_MDPU_MFLG1_ADDRTYPE_MASK		= 0x03,
+	IWL_RX_MPDU_MFLG1_MIC_CRC_LEN_MASK	= 0xf0,
+	/* shift should be 4, but the length is measured in 2-byte
+	 * words, so shifting only by 3 gives a byte result
+	 */
+	IWL_RX_MPDU_MFLG1_MIC_CRC_LEN_SHIFT	= 3,
+};
+
+enum iwl_rx_mpdu_mac_flags2 {
+	/* in 2-byte words */
+	IWL_RX_MPDU_MFLG2_HDR_LEN_MASK		= 0x1f,
+	IWL_RX_MPDU_MFLG2_PAD			= 0x20,
+	IWL_RX_MPDU_MFLG2_AMSDU			= 0x40,
+};
+
+enum iwl_rx_mpdu_amsdu_info {
+	IWL_RX_MPDU_AMSDU_SUBFRAME_IDX_MASK	= 0x3f,
+	IWL_RX_MPDU_AMSDU_LAST_SUBFRAME		= 0x40,
+	/* 0x80 bit reserved for now */
+};
+
+enum iwl_rx_l3l4_flags {
+	IWL_RX_L3L4_IP_HDR_CSUM_OK		= BIT(0),
+	IWL_RX_L3L4_TCP_UDP_CSUM_OK		= BIT(1),
+	IWL_RX_L3L4_TCP_FIN_SYN_RST_PSH		= BIT(2),
+	IWL_RX_L3L4_TCP_ACK			= BIT(3),
+	IWL_RX_L3L4_L3_PROTO_MASK		= 0xf << 4,
+	IWL_RX_L3L4_L4_PROTO_MASK		= 0xf << 8,
+	IWL_RX_L3L4_RSS_HASH_MASK		= 0xf << 12,
+};
+
+enum iwl_rx_mpdu_status {
+	IWL_RX_MPDU_STATUS_CRC_OK		= BIT(0),
+	IWL_RX_MPDU_STATUS_OVERRUN_OK		= BIT(1),
+	IWL_RX_MPDU_STATUS_SRC_STA_FOUND	= BIT(2),
+	IWL_RX_MPDU_STATUS_KEY_VALID		= BIT(3),
+	IWL_RX_MPDU_STATUS_KEY_ERROR		= BIT(4),
+	IWL_RX_MPDU_STATUS_ICV_OK		= BIT(5),
+	IWL_RX_MPDU_STATUS_MIC_OK		= BIT(6),
+	/* TODO - verify this is the correct value */
+	IWL_RX_MPDU_RES_STATUS_TTAK_OK		= BIT(7),
+	IWL_RX_MPDU_STATUS_SEC_MASK		= 0x7 << 8,
+	IWL_RX_MPDU_STATUS_SEC_NONE		= 0x0 << 8,
+	IWL_RX_MPDU_STATUS_SEC_WEP		= 0x1 << 8,
+	IWL_RX_MPDU_STATUS_SEC_CCM		= 0x2 << 8,
+	IWL_RX_MPDU_STATUS_SEC_TKIP		= 0x3 << 8,
+	/* TODO - define IWL_RX_MPDU_STATUS_SEC_EXT_ENC - this is a stub */
+	IWL_RX_MPDU_STATUS_SEC_EXT_ENC		= 0x4 << 8,
+	/* TODO - define IWL_RX_MPDU_STATUS_SEC_GCM - this is a stub */
+	IWL_RX_MPDU_STATUS_SEC_GCM		= 0x5 << 8,
+	IWL_RX_MPDU_STATUS_DECRYPTED		= BIT(11),
+	IWL_RX_MPDU_STATUS_WEP_MATCH		= BIT(12),
+	IWL_RX_MPDU_STATUS_EXT_IV_MATCH		= BIT(13),
+	IWL_RX_MPDU_STATUS_KEY_ID_MATCH		= BIT(14),
+	IWL_RX_MPDU_STATUS_KEY_COLOR		= BIT(15),
+};
+
+enum iwl_rx_mpdu_hash_filter {
+	IWL_RX_MPDU_HF_A1_HASH_MASK		= 0x3f,
+	IWL_RX_MPDU_HF_FILTER_STATUS_MASK	= 0xc0,
+};
+
+enum iwl_rx_mpdu_sta_id_flags {
+	IWL_RX_MPDU_SIF_STA_ID_MASK		= 0x1f,
+	IWL_RX_MPDU_SIF_RRF_ABORT		= 0x20,
+	IWL_RX_MPDU_SIF_FILTER_STATUS_MASK	= 0xc0,
+};
+
+#define IWL_RX_REORDER_DATA_INVALID_BAID 0x7f
+
+enum iwl_rx_mpdu_reorder_data {
+	IWL_RX_MPDU_REORDER_NSSN_MASK		= 0x00000fff,
+	IWL_RX_MPDU_REORDER_SN_MASK		= 0x00fff000,
+	IWL_RX_MPDU_REORDER_SN_SHIFT		= 12,
+	IWL_RX_MPDU_REORDER_BAID_MASK		= 0x7f000000,
+	IWL_RX_MPDU_REORDER_BAID_SHIFT		= 24,
+	IWL_RX_MPDU_REORDER_BA_OLD_SN		= 0x80000000,
+};
+
+struct iwl_rx_mpdu_desc {
+	/* DW2 */
+	__le16 mpdu_len;
+	u8 mac_flags1;
+	u8 mac_flags2;
+	/* DW3 */
+	u8 amsdu_info;
+	__le16 reserved_for_software;
+	u8 mac_phy_idx;
+	/* DW4 */
+	__le16 raw_csum; /* alledgedly unreliable */
+	__le16 l3l4_flags;
+	/* DW5 */
+	__le16 status;
+	u8 hash_filter;
+	u8 sta_id_flags;
+	/* DW6 */
+	__le32 reorder_data;
+	/* DW7 */
+	__le32 rss_hash;
+	/* DW8 */
+	__le32 filter_match;
+	/* DW9 */
+	__le32 gp2_on_air_rise;
+	/* DW10 */
+	__le32 rate_n_flags;
+	/* DW11 */
+	u8 energy_a, energy_b, energy_c, channel;
+	/* DW12 & DW13 */
+	__le64 tsf_on_air_rise;
+} __packed;
+
+struct iwl_frame_release {
+	u8 baid;
+	u8 reserved;
+	__le16 nssn;
+};
+
 #endif /* __fw_api_rx_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h
index 3a657e4b60ac..f01dab0d0dac 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-scan.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -285,6 +285,8 @@ struct iwl_scan_channel_opt {
  * @IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED: all passive scans will be fragmented
  * @IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED: insert WFA vendor-specific TPC report
  *	and DS parameter set IEs into probe requests.
+ * @IWL_MVM_LMAC_SCAN_FLAG_EXTENDED_DWELL: use extended dwell time on channels
+ *	1, 6 and 11.
  * @IWL_MVM_LMAC_SCAN_FLAG_MATCH: Send match found notification on matches
  */
 enum iwl_mvm_lmac_scan_flags {
@@ -295,6 +297,7 @@ enum iwl_mvm_lmac_scan_flags {
 	IWL_MVM_LMAC_SCAN_FLAG_MULTIPLE_SSIDS	= BIT(4),
 	IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED	= BIT(5),
 	IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED	= BIT(6),
+	IWL_MVM_LMAC_SCAN_FLAG_EXTENDED_DWELL	= BIT(7),
 	IWL_MVM_LMAC_SCAN_FLAG_MATCH		= BIT(9),
 };
 
@@ -322,6 +325,7 @@ enum iwl_scan_priority_ext {
  * @active-dwell: dwell time for active channels
  * @passive-dwell: dwell time for passive channels
  * @fragmented-dwell: dwell time for fragmented passive scan
+ * @extended_dwell: dwell time for channels 1, 6 and 11 (in certain cases)
  * @reserved2: for alignment and future use
  * @rx_chain_selct: PHY_RX_CHAIN_* flags
  * @scan_flags: &enum iwl_mvm_lmac_scan_flags
@@ -346,7 +350,8 @@ struct iwl_scan_req_lmac {
 	u8 active_dwell;
 	u8 passive_dwell;
 	u8 fragmented_dwell;
-	__le16 reserved2;
+	u8 extended_dwell;
+	u8 reserved2;
 	__le16 rx_chain_select;
 	__le32 scan_flags;
 	__le32 max_out_time;
@@ -490,7 +495,7 @@ enum iwl_channel_flags {
  * @dwell_active:		default dwell time for active scan
  * @dwell_passive:		default dwell time for passive scan
  * @dwell_fragmented:		default dwell time for fragmented scan
- * @reserved:			for future use and alignment
+ * @dwell_extended:		default dwell time for channels 1, 6 and 11
  * @mac_addr:			default mac address to be used in probes
  * @bcast_sta_id:		the index of the station in the fw
  * @channel_flags:		default channel flags - enum iwl_channel_flags
@@ -507,7 +512,7 @@ struct iwl_scan_config {
 	u8 dwell_active;
 	u8 dwell_passive;
 	u8 dwell_fragmented;
-	u8 reserved;
+	u8 dwell_extended;
 	u8 mac_addr[ETH_ALEN];
 	u8 bcast_sta_id;
 	u8 channel_flags;
@@ -543,7 +548,8 @@ enum iwl_umac_scan_general_flags {
 	IWL_UMAC_SCAN_GEN_FLAGS_MULTIPLE_SSID	= BIT(6),
 	IWL_UMAC_SCAN_GEN_FLAGS_FRAGMENTED	= BIT(7),
 	IWL_UMAC_SCAN_GEN_FLAGS_RRM_ENABLED	= BIT(8),
-	IWL_UMAC_SCAN_GEN_FLAGS_MATCH		= BIT(9)
+	IWL_UMAC_SCAN_GEN_FLAGS_MATCH		= BIT(9),
+	IWL_UMAC_SCAN_GEN_FLAGS_EXTENDED_DWELL	= BIT(10),
 };
 
 /**
@@ -597,7 +603,7 @@ struct iwl_scan_req_umac_tail {
  * @uid: scan id, &enum iwl_umac_scan_uid_offsets
  * @ooc_priority: out of channel priority - &enum iwl_scan_priority
  * @general_flags: &enum iwl_umac_scan_general_flags
- * @reserved1: for future use and alignment
+ * @extended_dwell: dwell time for channels 1, 6 and 11
  * @active_dwell: dwell time for active scan
  * @passive_dwell: dwell time for passive scan
  * @fragmented_dwell: dwell time for fragmented passive scan
@@ -606,7 +612,7 @@ struct iwl_scan_req_umac_tail {
  * @scan_priority: scan internal prioritization &enum iwl_scan_priority
  * @channel_flags: &enum iwl_scan_channel_flags
  * @n_channels: num of channels in scan request
- * @reserved2: for future use and alignment
+ * @reserved: for future use and alignment
  * @data: &struct iwl_scan_channel_cfg_umac and
  *	&struct iwl_scan_req_umac_tail
  */
@@ -616,7 +622,7 @@ struct iwl_scan_req_umac {
 	__le32 ooc_priority;
 	/* SCAN_GENERAL_PARAMS_API_S_VER_1 */
 	__le32 general_flags;
-	u8 reserved1;
+	u8 extended_dwell;
 	u8 active_dwell;
 	u8 passive_dwell;
 	u8 fragmented_dwell;
@@ -626,7 +632,7 @@ struct iwl_scan_req_umac {
 	/* SCAN_CHANNEL_PARAMS_API_S_VER_1 */
 	u8 channel_flags;
 	u8 n_channels;
-	__le16 reserved2;
+	__le16 reserved;
 	u8 data[];
 } __packed; /* SCAN_REQUEST_CMD_UMAC_API_S_VER_1 */
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h
index 493a8bdfbc9e..6fca4fb1d306 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-sta.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h
index 0c321f63ee42..438665a54923 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-stats.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-stats.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-tof.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h
index eed6271d01a3..86aa51b2210e 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-tof.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tof.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- * Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h
index 853698ab8b05..ba3f0bbddde8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api-tx.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -510,6 +510,9 @@ struct iwl_mvm_tx_resp {
  * @scd_ssn: the index of the last contiguously sent packet
  * @txed: number of Txed frames in this batch
  * @txed_2_done: number of Acked frames in this batch
+ * @reduced_txp: power reduced according to TPC. This is the actual value and
+ *	not a copy from the LQ command. Thus, if not the first rate was used
+ *	for Tx-ing then this value will be set to 0 by FW.
  */
 struct iwl_mvm_ba_notif {
 	__le32 sta_addr_lo32;
@@ -524,7 +527,8 @@ struct iwl_mvm_ba_notif {
 	__le16 scd_ssn;
 	u8 txed;
 	u8 txed_2_done;
-	__le16 reserved1;
+	u8 reduced_txp;
+	u8 reserved1;
 } __packed;
 
 /*
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h
index 181590fbd3b3..82049bb139c2 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-api.h
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -213,6 +215,7 @@ enum {
 
 	REPLY_RX_PHY_CMD = 0xc0,
 	REPLY_RX_MPDU_CMD = 0xc1,
+	FRAME_RELEASE = 0xc3,
 	BA_NOTIF = 0xc5,
 
 	/* Location Aware Regulatory */
@@ -239,6 +242,7 @@ enum {
 	DTS_MEASUREMENT_NOTIFICATION = 0xdd,
 
 	REPLY_DEBUG_CMD = 0xf0,
+	LDBG_CONFIG_CMD = 0xf6,
 	DEBUG_LOG_MSG = 0xf7,
 
 	BCAST_FILTER_CMD = 0xcf,
@@ -268,6 +272,9 @@ enum {
 	REPLY_MAX = 0xff,
 };
 
+/* Please keep this enum *SORTED* by hex value.
+ * Needed for binary search, otherwise a warning will be triggered.
+ */
 enum iwl_phy_ops_subcmd_ids {
 	CMD_DTS_MEASUREMENT_TRIGGER_WIDE = 0x0,
 	DTS_MEASUREMENT_NOTIF_WIDE = 0xFF,
@@ -275,6 +282,8 @@ enum iwl_phy_ops_subcmd_ids {
 
 /* command groups */
 enum {
+	LEGACY_GROUP = 0x0,
+	LONG_GROUP = 0x1,
 	PHY_OPS_GROUP = 0x4,
 };
 
@@ -426,6 +435,26 @@ struct iwl_fw_get_item_cmd {
 	__le32 item_id;
 } __packed; /* FW_GET_ITEM_CMD_API_S_VER_1 */
 
+#define CONT_REC_COMMAND_SIZE	80
+#define ENABLE_CONT_RECORDING	0x15
+#define DISABLE_CONT_RECORDING	0x16
+
+/*
+ * struct iwl_continuous_record_mode - recording mode
+ */
+struct iwl_continuous_record_mode {
+	__le16 enable_recording;
+} __packed;
+
+/*
+ * struct iwl_continuous_record_cmd - enable/disable continuous recording
+ */
+struct iwl_continuous_record_cmd {
+	struct iwl_continuous_record_mode record_mode;
+	u8 pad[CONT_REC_COMMAND_SIZE -
+		sizeof(struct iwl_continuous_record_mode)];
+} __packed;
+
 struct iwl_fw_get_item_resp {
 	__le32 item_id;
 	__le32 item_byte_cnt;
@@ -1425,6 +1454,22 @@ struct iwl_sf_cfg_cmd {
  ***********************************/
 
 /**
+ * struct iwl_mcc_update_cmd_v1 - Request the device to update geographic
+ * regulatory profile according to the given MCC (Mobile Country Code).
+ * The MCC is two letter-code, ascii upper case[A-Z] or '00' for world domain.
+ * 'ZZ' MCC will be used to switch to NVM default profile; in this case, the
+ * MCC in the cmd response will be the relevant MCC in the NVM.
+ * @mcc: given mobile country code
+ * @source_id: the source from where we got the MCC, see iwl_mcc_source
+ * @reserved: reserved for alignment
+ */
+struct iwl_mcc_update_cmd_v1 {
+	__le16 mcc;
+	u8 source_id;
+	u8 reserved;
+} __packed; /* LAR_UPDATE_MCC_CMD_API_S_VER_1 */
+
+/**
  * struct iwl_mcc_update_cmd - Request the device to update geographic
  * regulatory profile according to the given MCC (Mobile Country Code).
  * The MCC is two letter-code, ascii upper case[A-Z] or '00' for world domain.
@@ -1433,12 +1478,39 @@ struct iwl_sf_cfg_cmd {
  * @mcc: given mobile country code
  * @source_id: the source from where we got the MCC, see iwl_mcc_source
  * @reserved: reserved for alignment
+ * @key: integrity key for MCC API OEM testing
+ * @reserved2: reserved
  */
 struct iwl_mcc_update_cmd {
 	__le16 mcc;
 	u8 source_id;
 	u8 reserved;
-} __packed; /* LAR_UPDATE_MCC_CMD_API_S */
+	__le32 key;
+	__le32 reserved2[5];
+} __packed; /* LAR_UPDATE_MCC_CMD_API_S_VER_2 */
+
+/**
+ * iwl_mcc_update_resp_v1  - response to MCC_UPDATE_CMD.
+ * Contains the new channel control profile map, if changed, and the new MCC
+ * (mobile country code).
+ * The new MCC may be different than what was requested in MCC_UPDATE_CMD.
+ * @status: see &enum iwl_mcc_update_status
+ * @mcc: the new applied MCC
+ * @cap: capabilities for all channels which matches the MCC
+ * @source_id: the MCC source, see iwl_mcc_source
+ * @n_channels: number of channels in @channels_data (may be 14, 39, 50 or 51
+ *		channels, depending on platform)
+ * @channels: channel control data map, DWORD for each channel. Only the first
+ *	16bits are used.
+ */
+struct iwl_mcc_update_resp_v1  {
+	__le32 status;
+	__le16 mcc;
+	u8 cap;
+	u8 source_id;
+	__le32 n_channels;
+	__le32 channels[0];
+} __packed; /* LAR_UPDATE_MCC_CMD_RESP_S_VER_1 */
 
 /**
  * iwl_mcc_update_resp - response to MCC_UPDATE_CMD.
@@ -1449,6 +1521,8 @@ struct iwl_mcc_update_cmd {
  * @mcc: the new applied MCC
  * @cap: capabilities for all channels which matches the MCC
  * @source_id: the MCC source, see iwl_mcc_source
+ * @time: time elapsed from the MCC test start (in 30 seconds TU)
+ * @reserved: reserved.
  * @n_channels: number of channels in @channels_data (may be 14, 39, 50 or 51
  *		channels, depending on platform)
  * @channels: channel control data map, DWORD for each channel. Only the first
@@ -1459,9 +1533,11 @@ struct iwl_mcc_update_resp {
 	__le16 mcc;
 	u8 cap;
 	u8 source_id;
+	__le16 time;
+	__le16 reserved;
 	__le32 n_channels;
 	__le32 channels[0];
-} __packed; /* LAR_UPDATE_MCC_CMD_RESP_S */
+} __packed; /* LAR_UPDATE_MCC_CMD_RESP_S_VER_2 */
 
 /**
  * struct iwl_mcc_chub_notif - chub notifies of mcc change
@@ -1491,6 +1567,9 @@ enum iwl_mcc_update_status {
 	MCC_RESP_NVM_DISABLED,
 	MCC_RESP_ILLEGAL,
 	MCC_RESP_LOW_PRIORITY,
+	MCC_RESP_TEST_MODE_ACTIVE,
+	MCC_RESP_TEST_MODE_NOT_ACTIVE,
+	MCC_RESP_TEST_MODE_DENIAL_OF_SERVICE,
 };
 
 enum iwl_mcc_source {
@@ -1503,7 +1582,9 @@ enum iwl_mcc_source {
 	MCC_SOURCE_RESERVED = 6,
 	MCC_SOURCE_DEFAULT = 7,
 	MCC_SOURCE_UNINITIALIZED = 8,
-	MCC_SOURCE_GET_CURRENT = 0x10
+	MCC_SOURCE_MCC_API = 9,
+	MCC_SOURCE_GET_CURRENT = 0x10,
+	MCC_SOURCE_GETTING_MCC_TEST_MODE = 0x11,
 };
 
 /* DTS measurements */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c
new file mode 100644
index 000000000000..0813f8184e10
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.c
@@ -0,0 +1,817 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program;
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <linuxwifi@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+#include <linux/devcoredump.h>
+
+#include "fw-dbg.h"
+#include "iwl-io.h"
+#include "mvm.h"
+#include "iwl-prph.h"
+#include "iwl-csr.h"
+
+static ssize_t iwl_mvm_read_coredump(char *buffer, loff_t offset, size_t count,
+				     const void *data, size_t datalen)
+{
+	const struct iwl_mvm_dump_ptrs *dump_ptrs = data;
+	ssize_t bytes_read;
+	ssize_t bytes_read_trans;
+
+	if (offset < dump_ptrs->op_mode_len) {
+		bytes_read = min_t(ssize_t, count,
+				   dump_ptrs->op_mode_len - offset);
+		memcpy(buffer, (u8 *)dump_ptrs->op_mode_ptr + offset,
+		       bytes_read);
+		offset += bytes_read;
+		count -= bytes_read;
+
+		if (count == 0)
+			return bytes_read;
+	} else {
+		bytes_read = 0;
+	}
+
+	if (!dump_ptrs->trans_ptr)
+		return bytes_read;
+
+	offset -= dump_ptrs->op_mode_len;
+	bytes_read_trans = min_t(ssize_t, count,
+				 dump_ptrs->trans_ptr->len - offset);
+	memcpy(buffer + bytes_read,
+	       (u8 *)dump_ptrs->trans_ptr->data + offset,
+	       bytes_read_trans);
+
+	return bytes_read + bytes_read_trans;
+}
+
+static void iwl_mvm_free_coredump(const void *data)
+{
+	const struct iwl_mvm_dump_ptrs *fw_error_dump = data;
+
+	vfree(fw_error_dump->op_mode_ptr);
+	vfree(fw_error_dump->trans_ptr);
+	kfree(fw_error_dump);
+}
+
+#define RADIO_REG_MAX_READ 0x2ad
+static void iwl_mvm_read_radio_reg(struct iwl_mvm *mvm,
+				   struct iwl_fw_error_dump_data **dump_data)
+{
+	u8 *pos = (void *)(*dump_data)->data;
+	unsigned long flags;
+	int i;
+
+	if (!iwl_trans_grab_nic_access(mvm->trans, &flags))
+		return;
+
+	(*dump_data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_RADIO_REG);
+	(*dump_data)->len = cpu_to_le32(RADIO_REG_MAX_READ);
+
+	for (i = 0; i < RADIO_REG_MAX_READ; i++) {
+		u32 rd_cmd = RADIO_RSP_RD_CMD;
+
+		rd_cmd |= i << RADIO_RSP_ADDR_POS;
+		iwl_write_prph_no_grab(mvm->trans, RSP_RADIO_CMD, rd_cmd);
+		*pos =  (u8)iwl_read_prph_no_grab(mvm->trans, RSP_RADIO_RDDAT);
+
+		pos++;
+	}
+
+	*dump_data = iwl_fw_error_next_data(*dump_data);
+
+	iwl_trans_release_nic_access(mvm->trans, &flags);
+}
+
+static void iwl_mvm_dump_fifos(struct iwl_mvm *mvm,
+			       struct iwl_fw_error_dump_data **dump_data)
+{
+	struct iwl_fw_error_dump_fifo *fifo_hdr;
+	u32 *fifo_data;
+	u32 fifo_len;
+	unsigned long flags;
+	int i, j;
+
+	if (!iwl_trans_grab_nic_access(mvm->trans, &flags))
+		return;
+
+	/* Pull RXF data from all RXFs */
+	for (i = 0; i < ARRAY_SIZE(mvm->shared_mem_cfg.rxfifo_size); i++) {
+		/*
+		 * Keep aside the additional offset that might be needed for
+		 * next RXF
+		 */
+		u32 offset_diff = RXF_DIFF_FROM_PREV * i;
+
+		fifo_hdr = (void *)(*dump_data)->data;
+		fifo_data = (void *)fifo_hdr->data;
+		fifo_len = mvm->shared_mem_cfg.rxfifo_size[i];
+
+		/* No need to try to read the data if the length is 0 */
+		if (fifo_len == 0)
+			continue;
+
+		/* Add a TLV for the RXF */
+		(*dump_data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_RXF);
+		(*dump_data)->len = cpu_to_le32(fifo_len + sizeof(*fifo_hdr));
+
+		fifo_hdr->fifo_num = cpu_to_le32(i);
+		fifo_hdr->available_bytes =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							RXF_RD_D_SPACE +
+							offset_diff));
+		fifo_hdr->wr_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							RXF_RD_WR_PTR +
+							offset_diff));
+		fifo_hdr->rd_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							RXF_RD_RD_PTR +
+							offset_diff));
+		fifo_hdr->fence_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							RXF_RD_FENCE_PTR +
+							offset_diff));
+		fifo_hdr->fence_mode =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							RXF_SET_FENCE_MODE +
+							offset_diff));
+
+		/* Lock fence */
+		iwl_trans_write_prph(mvm->trans,
+				     RXF_SET_FENCE_MODE + offset_diff, 0x1);
+		/* Set fence pointer to the same place like WR pointer */
+		iwl_trans_write_prph(mvm->trans,
+				     RXF_LD_WR2FENCE + offset_diff, 0x1);
+		/* Set fence offset */
+		iwl_trans_write_prph(mvm->trans,
+				     RXF_LD_FENCE_OFFSET_ADDR + offset_diff,
+				     0x0);
+
+		/* Read FIFO */
+		fifo_len /= sizeof(u32); /* Size in DWORDS */
+		for (j = 0; j < fifo_len; j++)
+			fifo_data[j] = iwl_trans_read_prph(mvm->trans,
+							 RXF_FIFO_RD_FENCE_INC +
+							 offset_diff);
+		*dump_data = iwl_fw_error_next_data(*dump_data);
+	}
+
+	/* Pull TXF data from all TXFs */
+	for (i = 0; i < ARRAY_SIZE(mvm->shared_mem_cfg.txfifo_size); i++) {
+		/* Mark the number of TXF we're pulling now */
+		iwl_trans_write_prph(mvm->trans, TXF_LARC_NUM, i);
+
+		fifo_hdr = (void *)(*dump_data)->data;
+		fifo_data = (void *)fifo_hdr->data;
+		fifo_len = mvm->shared_mem_cfg.txfifo_size[i];
+
+		/* No need to try to read the data if the length is 0 */
+		if (fifo_len == 0)
+			continue;
+
+		/* Add a TLV for the FIFO */
+		(*dump_data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_TXF);
+		(*dump_data)->len = cpu_to_le32(fifo_len + sizeof(*fifo_hdr));
+
+		fifo_hdr->fifo_num = cpu_to_le32(i);
+		fifo_hdr->available_bytes =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							TXF_FIFO_ITEM_CNT));
+		fifo_hdr->wr_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							TXF_WR_PTR));
+		fifo_hdr->rd_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							TXF_RD_PTR));
+		fifo_hdr->fence_ptr =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							TXF_FENCE_PTR));
+		fifo_hdr->fence_mode =
+			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
+							TXF_LOCK_FENCE));
+
+		/* Set the TXF_READ_MODIFY_ADDR to TXF_WR_PTR */
+		iwl_trans_write_prph(mvm->trans, TXF_READ_MODIFY_ADDR,
+				     TXF_WR_PTR);
+
+		/* Dummy-read to advance the read pointer to the head */
+		iwl_trans_read_prph(mvm->trans, TXF_READ_MODIFY_DATA);
+
+		/* Read FIFO */
+		fifo_len /= sizeof(u32); /* Size in DWORDS */
+		for (j = 0; j < fifo_len; j++)
+			fifo_data[j] = iwl_trans_read_prph(mvm->trans,
+							  TXF_READ_MODIFY_DATA);
+		*dump_data = iwl_fw_error_next_data(*dump_data);
+	}
+
+	iwl_trans_release_nic_access(mvm->trans, &flags);
+}
+
+void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm)
+{
+	if (mvm->fw_dump_desc == &iwl_mvm_dump_desc_assert)
+		return;
+
+	kfree(mvm->fw_dump_desc);
+	mvm->fw_dump_desc = NULL;
+}
+
+#define IWL8260_ICCM_OFFSET		0x44000 /* Only for B-step */
+#define IWL8260_ICCM_LEN		0xC000 /* Only for B-step */
+
+static const struct {
+	u32 start, end;
+} iwl_prph_dump_addr[] = {
+	{ .start = 0x00a00000, .end = 0x00a00000 },
+	{ .start = 0x00a0000c, .end = 0x00a00024 },
+	{ .start = 0x00a0002c, .end = 0x00a0003c },
+	{ .start = 0x00a00410, .end = 0x00a00418 },
+	{ .start = 0x00a00420, .end = 0x00a00420 },
+	{ .start = 0x00a00428, .end = 0x00a00428 },
+	{ .start = 0x00a00430, .end = 0x00a0043c },
+	{ .start = 0x00a00444, .end = 0x00a00444 },
+	{ .start = 0x00a004c0, .end = 0x00a004cc },
+	{ .start = 0x00a004d8, .end = 0x00a004d8 },
+	{ .start = 0x00a004e0, .end = 0x00a004f0 },
+	{ .start = 0x00a00840, .end = 0x00a00840 },
+	{ .start = 0x00a00850, .end = 0x00a00858 },
+	{ .start = 0x00a01004, .end = 0x00a01008 },
+	{ .start = 0x00a01010, .end = 0x00a01010 },
+	{ .start = 0x00a01018, .end = 0x00a01018 },
+	{ .start = 0x00a01024, .end = 0x00a01024 },
+	{ .start = 0x00a0102c, .end = 0x00a01034 },
+	{ .start = 0x00a0103c, .end = 0x00a01040 },
+	{ .start = 0x00a01048, .end = 0x00a01094 },
+	{ .start = 0x00a01c00, .end = 0x00a01c20 },
+	{ .start = 0x00a01c58, .end = 0x00a01c58 },
+	{ .start = 0x00a01c7c, .end = 0x00a01c7c },
+	{ .start = 0x00a01c28, .end = 0x00a01c54 },
+	{ .start = 0x00a01c5c, .end = 0x00a01c5c },
+	{ .start = 0x00a01c60, .end = 0x00a01cdc },
+	{ .start = 0x00a01ce0, .end = 0x00a01d0c },
+	{ .start = 0x00a01d18, .end = 0x00a01d20 },
+	{ .start = 0x00a01d2c, .end = 0x00a01d30 },
+	{ .start = 0x00a01d40, .end = 0x00a01d5c },
+	{ .start = 0x00a01d80, .end = 0x00a01d80 },
+	{ .start = 0x00a01d98, .end = 0x00a01d9c },
+	{ .start = 0x00a01da8, .end = 0x00a01da8 },
+	{ .start = 0x00a01db8, .end = 0x00a01df4 },
+	{ .start = 0x00a01dc0, .end = 0x00a01dfc },
+	{ .start = 0x00a01e00, .end = 0x00a01e2c },
+	{ .start = 0x00a01e40, .end = 0x00a01e60 },
+	{ .start = 0x00a01e68, .end = 0x00a01e6c },
+	{ .start = 0x00a01e74, .end = 0x00a01e74 },
+	{ .start = 0x00a01e84, .end = 0x00a01e90 },
+	{ .start = 0x00a01e9c, .end = 0x00a01ec4 },
+	{ .start = 0x00a01ed0, .end = 0x00a01ee0 },
+	{ .start = 0x00a01f00, .end = 0x00a01f1c },
+	{ .start = 0x00a01f44, .end = 0x00a01ffc },
+	{ .start = 0x00a02000, .end = 0x00a02048 },
+	{ .start = 0x00a02068, .end = 0x00a020f0 },
+	{ .start = 0x00a02100, .end = 0x00a02118 },
+	{ .start = 0x00a02140, .end = 0x00a0214c },
+	{ .start = 0x00a02168, .end = 0x00a0218c },
+	{ .start = 0x00a021c0, .end = 0x00a021c0 },
+	{ .start = 0x00a02400, .end = 0x00a02410 },
+	{ .start = 0x00a02418, .end = 0x00a02420 },
+	{ .start = 0x00a02428, .end = 0x00a0242c },
+	{ .start = 0x00a02434, .end = 0x00a02434 },
+	{ .start = 0x00a02440, .end = 0x00a02460 },
+	{ .start = 0x00a02468, .end = 0x00a024b0 },
+	{ .start = 0x00a024c8, .end = 0x00a024cc },
+	{ .start = 0x00a02500, .end = 0x00a02504 },
+	{ .start = 0x00a0250c, .end = 0x00a02510 },
+	{ .start = 0x00a02540, .end = 0x00a02554 },
+	{ .start = 0x00a02580, .end = 0x00a025f4 },
+	{ .start = 0x00a02600, .end = 0x00a0260c },
+	{ .start = 0x00a02648, .end = 0x00a02650 },
+	{ .start = 0x00a02680, .end = 0x00a02680 },
+	{ .start = 0x00a026c0, .end = 0x00a026d0 },
+	{ .start = 0x00a02700, .end = 0x00a0270c },
+	{ .start = 0x00a02804, .end = 0x00a02804 },
+	{ .start = 0x00a02818, .end = 0x00a0281c },
+	{ .start = 0x00a02c00, .end = 0x00a02db4 },
+	{ .start = 0x00a02df4, .end = 0x00a02fb0 },
+	{ .start = 0x00a03000, .end = 0x00a03014 },
+	{ .start = 0x00a0301c, .end = 0x00a0302c },
+	{ .start = 0x00a03034, .end = 0x00a03038 },
+	{ .start = 0x00a03040, .end = 0x00a03048 },
+	{ .start = 0x00a03060, .end = 0x00a03068 },
+	{ .start = 0x00a03070, .end = 0x00a03074 },
+	{ .start = 0x00a0307c, .end = 0x00a0307c },
+	{ .start = 0x00a03080, .end = 0x00a03084 },
+	{ .start = 0x00a0308c, .end = 0x00a03090 },
+	{ .start = 0x00a03098, .end = 0x00a03098 },
+	{ .start = 0x00a030a0, .end = 0x00a030a0 },
+	{ .start = 0x00a030a8, .end = 0x00a030b4 },
+	{ .start = 0x00a030bc, .end = 0x00a030bc },
+	{ .start = 0x00a030c0, .end = 0x00a0312c },
+	{ .start = 0x00a03c00, .end = 0x00a03c5c },
+	{ .start = 0x00a04400, .end = 0x00a04454 },
+	{ .start = 0x00a04460, .end = 0x00a04474 },
+	{ .start = 0x00a044c0, .end = 0x00a044ec },
+	{ .start = 0x00a04500, .end = 0x00a04504 },
+	{ .start = 0x00a04510, .end = 0x00a04538 },
+	{ .start = 0x00a04540, .end = 0x00a04548 },
+	{ .start = 0x00a04560, .end = 0x00a0457c },
+	{ .start = 0x00a04590, .end = 0x00a04598 },
+	{ .start = 0x00a045c0, .end = 0x00a045f4 },
+	{ .start = 0x00a44000, .end = 0x00a7bf80 },
+};
+
+static u32 iwl_dump_prph(struct iwl_trans *trans,
+			 struct iwl_fw_error_dump_data **data)
+{
+	struct iwl_fw_error_dump_prph *prph;
+	unsigned long flags;
+	u32 prph_len = 0, i;
+
+	if (!iwl_trans_grab_nic_access(trans, &flags))
+		return 0;
+
+	for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr); i++) {
+		/* The range includes both boundaries */
+		int num_bytes_in_chunk = iwl_prph_dump_addr[i].end -
+			 iwl_prph_dump_addr[i].start + 4;
+		int reg;
+		__le32 *val;
+
+		prph_len += sizeof(**data) + sizeof(*prph) + num_bytes_in_chunk;
+
+		(*data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_PRPH);
+		(*data)->len = cpu_to_le32(sizeof(*prph) +
+					num_bytes_in_chunk);
+		prph = (void *)(*data)->data;
+		prph->prph_start = cpu_to_le32(iwl_prph_dump_addr[i].start);
+		val = (void *)prph->data;
+
+		for (reg = iwl_prph_dump_addr[i].start;
+		     reg <= iwl_prph_dump_addr[i].end;
+		     reg += 4)
+			*val++ = cpu_to_le32(iwl_read_prph_no_grab(trans,
+								   reg));
+
+		*data = iwl_fw_error_next_data(*data);
+	}
+
+	iwl_trans_release_nic_access(trans, &flags);
+
+	return prph_len;
+}
+
+void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
+{
+	struct iwl_fw_error_dump_file *dump_file;
+	struct iwl_fw_error_dump_data *dump_data;
+	struct iwl_fw_error_dump_info *dump_info;
+	struct iwl_fw_error_dump_mem *dump_mem;
+	struct iwl_fw_error_dump_trigger_desc *dump_trig;
+	struct iwl_mvm_dump_ptrs *fw_error_dump;
+	u32 sram_len, sram_ofs;
+	u32 file_len, fifo_data_len = 0, prph_len = 0, radio_len = 0;
+	u32 smem_len = mvm->cfg->smem_len;
+	u32 sram2_len = mvm->cfg->dccm2_len;
+	bool monitor_dump_only = false;
+	int i;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	/* there's no point in fw dump if the bus is dead */
+	if (test_bit(STATUS_TRANS_DEAD, &mvm->trans->status)) {
+		IWL_ERR(mvm, "Skip fw error dump since bus is dead\n");
+		goto out;
+	}
+
+	if (mvm->fw_dump_trig &&
+	    mvm->fw_dump_trig->mode & IWL_FW_DBG_TRIGGER_MONITOR_ONLY)
+		monitor_dump_only = true;
+
+	fw_error_dump = kzalloc(sizeof(*fw_error_dump), GFP_KERNEL);
+	if (!fw_error_dump)
+		goto out;
+
+	/* SRAM - include stack CCM if driver knows the values for it */
+	if (!mvm->cfg->dccm_offset || !mvm->cfg->dccm_len) {
+		const struct fw_img *img;
+
+		img = &mvm->fw->img[mvm->cur_ucode];
+		sram_ofs = img->sec[IWL_UCODE_SECTION_DATA].offset;
+		sram_len = img->sec[IWL_UCODE_SECTION_DATA].len;
+	} else {
+		sram_ofs = mvm->cfg->dccm_offset;
+		sram_len = mvm->cfg->dccm_len;
+	}
+
+	/* reading RXF/TXF sizes */
+	if (test_bit(STATUS_FW_ERROR, &mvm->trans->status)) {
+		struct iwl_mvm_shared_mem_cfg *mem_cfg = &mvm->shared_mem_cfg;
+
+		fifo_data_len = 0;
+
+		/* Count RXF size */
+		for (i = 0; i < ARRAY_SIZE(mem_cfg->rxfifo_size); i++) {
+			if (!mem_cfg->rxfifo_size[i])
+				continue;
+
+			/* Add header info */
+			fifo_data_len += mem_cfg->rxfifo_size[i] +
+					 sizeof(*dump_data) +
+					 sizeof(struct iwl_fw_error_dump_fifo);
+		}
+
+		for (i = 0; i < ARRAY_SIZE(mem_cfg->txfifo_size); i++) {
+			if (!mem_cfg->txfifo_size[i])
+				continue;
+
+			/* Add header info */
+			fifo_data_len += mem_cfg->txfifo_size[i] +
+					 sizeof(*dump_data) +
+					 sizeof(struct iwl_fw_error_dump_fifo);
+		}
+
+		/* Make room for PRPH registers */
+		for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr); i++) {
+			/* The range includes both boundaries */
+			int num_bytes_in_chunk = iwl_prph_dump_addr[i].end -
+				iwl_prph_dump_addr[i].start + 4;
+
+			prph_len += sizeof(*dump_data) +
+				sizeof(struct iwl_fw_error_dump_prph) +
+				num_bytes_in_chunk;
+		}
+
+		if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000)
+			radio_len = sizeof(*dump_data) + RADIO_REG_MAX_READ;
+	}
+
+	file_len = sizeof(*dump_file) +
+		   sizeof(*dump_data) * 2 +
+		   sram_len + sizeof(*dump_mem) +
+		   fifo_data_len +
+		   prph_len +
+		   radio_len +
+		   sizeof(*dump_info);
+
+	/* Make room for the SMEM, if it exists */
+	if (smem_len)
+		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + smem_len;
+
+	/* Make room for the secondary SRAM, if it exists */
+	if (sram2_len)
+		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + sram2_len;
+
+	/* Make room for fw's virtual image pages, if it exists */
+	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size)
+		file_len += mvm->num_of_paging_blk *
+			(sizeof(*dump_data) +
+			 sizeof(struct iwl_fw_error_dump_paging) +
+			 PAGING_BLOCK_SIZE);
+
+	/* If we only want a monitor dump, reset the file length */
+	if (monitor_dump_only) {
+		file_len = sizeof(*dump_file) + sizeof(*dump_data) +
+			   sizeof(*dump_info);
+	}
+
+	/*
+	 * In 8000 HW family B-step include the ICCM (which resides separately)
+	 */
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP)
+		file_len += sizeof(*dump_data) + sizeof(*dump_mem) +
+			    IWL8260_ICCM_LEN;
+
+	if (mvm->fw_dump_desc)
+		file_len += sizeof(*dump_data) + sizeof(*dump_trig) +
+			    mvm->fw_dump_desc->len;
+
+	dump_file = vzalloc(file_len);
+	if (!dump_file) {
+		kfree(fw_error_dump);
+		goto out;
+	}
+
+	fw_error_dump->op_mode_ptr = dump_file;
+
+	dump_file->barker = cpu_to_le32(IWL_FW_ERROR_DUMP_BARKER);
+	dump_data = (void *)dump_file->data;
+
+	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_DEV_FW_INFO);
+	dump_data->len = cpu_to_le32(sizeof(*dump_info));
+	dump_info = (void *)dump_data->data;
+	dump_info->device_family =
+		mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000 ?
+			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_7) :
+			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_8);
+	dump_info->hw_step = cpu_to_le32(CSR_HW_REV_STEP(mvm->trans->hw_rev));
+	memcpy(dump_info->fw_human_readable, mvm->fw->human_readable,
+	       sizeof(dump_info->fw_human_readable));
+	strncpy(dump_info->dev_human_readable, mvm->cfg->name,
+		sizeof(dump_info->dev_human_readable));
+	strncpy(dump_info->bus_human_readable, mvm->dev->bus->name,
+		sizeof(dump_info->bus_human_readable));
+
+	dump_data = iwl_fw_error_next_data(dump_data);
+	/* We only dump the FIFOs if the FW is in error state */
+	if (test_bit(STATUS_FW_ERROR, &mvm->trans->status)) {
+		iwl_mvm_dump_fifos(mvm, &dump_data);
+		if (radio_len)
+			iwl_mvm_read_radio_reg(mvm, &dump_data);
+	}
+
+	if (mvm->fw_dump_desc) {
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_ERROR_INFO);
+		dump_data->len = cpu_to_le32(sizeof(*dump_trig) +
+					     mvm->fw_dump_desc->len);
+		dump_trig = (void *)dump_data->data;
+		memcpy(dump_trig, &mvm->fw_dump_desc->trig_desc,
+		       sizeof(*dump_trig) + mvm->fw_dump_desc->len);
+
+		dump_data = iwl_fw_error_next_data(dump_data);
+	}
+
+	/* In case we only want monitor dump, skip to dump trasport data */
+	if (monitor_dump_only)
+		goto dump_trans_data;
+
+	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+	dump_data->len = cpu_to_le32(sram_len + sizeof(*dump_mem));
+	dump_mem = (void *)dump_data->data;
+	dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
+	dump_mem->offset = cpu_to_le32(sram_ofs);
+	iwl_trans_read_mem_bytes(mvm->trans, sram_ofs, dump_mem->data,
+				 sram_len);
+
+	if (smem_len) {
+		dump_data = iwl_fw_error_next_data(dump_data);
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+		dump_data->len = cpu_to_le32(smem_len + sizeof(*dump_mem));
+		dump_mem = (void *)dump_data->data;
+		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SMEM);
+		dump_mem->offset = cpu_to_le32(mvm->cfg->smem_offset);
+		iwl_trans_read_mem_bytes(mvm->trans, mvm->cfg->smem_offset,
+					 dump_mem->data, smem_len);
+	}
+
+	if (sram2_len) {
+		dump_data = iwl_fw_error_next_data(dump_data);
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+		dump_data->len = cpu_to_le32(sram2_len + sizeof(*dump_mem));
+		dump_mem = (void *)dump_data->data;
+		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
+		dump_mem->offset = cpu_to_le32(mvm->cfg->dccm2_offset);
+		iwl_trans_read_mem_bytes(mvm->trans, mvm->cfg->dccm2_offset,
+					 dump_mem->data, sram2_len);
+	}
+
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
+	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP) {
+		dump_data = iwl_fw_error_next_data(dump_data);
+		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
+		dump_data->len = cpu_to_le32(IWL8260_ICCM_LEN +
+					     sizeof(*dump_mem));
+		dump_mem = (void *)dump_data->data;
+		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
+		dump_mem->offset = cpu_to_le32(IWL8260_ICCM_OFFSET);
+		iwl_trans_read_mem_bytes(mvm->trans, IWL8260_ICCM_OFFSET,
+					 dump_mem->data, IWL8260_ICCM_LEN);
+	}
+
+	/* Dump fw's virtual image */
+	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size) {
+		u32 i;
+
+		for (i = 1; i < mvm->num_of_paging_blk + 1; i++) {
+			struct iwl_fw_error_dump_paging *paging;
+			struct page *pages =
+				mvm->fw_paging_db[i].fw_paging_block;
+
+			dump_data = iwl_fw_error_next_data(dump_data);
+			dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_PAGING);
+			dump_data->len = cpu_to_le32(sizeof(*paging) +
+						     PAGING_BLOCK_SIZE);
+			paging = (void *)dump_data->data;
+			paging->index = cpu_to_le32(i);
+			memcpy(paging->data, page_address(pages),
+			       PAGING_BLOCK_SIZE);
+		}
+	}
+
+	dump_data = iwl_fw_error_next_data(dump_data);
+	if (prph_len)
+		iwl_dump_prph(mvm->trans, &dump_data);
+
+dump_trans_data:
+	fw_error_dump->trans_ptr = iwl_trans_dump_data(mvm->trans,
+						       mvm->fw_dump_trig);
+	fw_error_dump->op_mode_len = file_len;
+	if (fw_error_dump->trans_ptr)
+		file_len += fw_error_dump->trans_ptr->len;
+	dump_file->file_len = cpu_to_le32(file_len);
+
+	dev_coredumpm(mvm->trans->dev, THIS_MODULE, fw_error_dump, 0,
+		      GFP_KERNEL, iwl_mvm_read_coredump, iwl_mvm_free_coredump);
+
+out:
+	iwl_mvm_free_fw_dump_desc(mvm);
+	mvm->fw_dump_trig = NULL;
+	clear_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status);
+}
+
+const struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert = {
+	.trig_desc = {
+		.type = cpu_to_le32(FW_DBG_TRIGGER_FW_ASSERT),
+	},
+};
+
+int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
+				const struct iwl_mvm_dump_desc *desc,
+				const struct iwl_fw_dbg_trigger_tlv *trigger)
+{
+	unsigned int delay = 0;
+
+	if (trigger)
+		delay = msecs_to_jiffies(le32_to_cpu(trigger->stop_delay));
+
+	if (test_and_set_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status))
+		return -EBUSY;
+
+	if (WARN_ON(mvm->fw_dump_desc))
+		iwl_mvm_free_fw_dump_desc(mvm);
+
+	IWL_WARN(mvm, "Collecting data: trigger %d fired.\n",
+		 le32_to_cpu(desc->trig_desc.type));
+
+	mvm->fw_dump_desc = desc;
+	mvm->fw_dump_trig = trigger;
+
+	queue_delayed_work(system_wq, &mvm->fw_dump_wk, delay);
+
+	return 0;
+}
+
+int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
+			   const char *str, size_t len,
+			   const struct iwl_fw_dbg_trigger_tlv *trigger)
+{
+	struct iwl_mvm_dump_desc *desc;
+
+	desc = kzalloc(sizeof(*desc) + len, GFP_ATOMIC);
+	if (!desc)
+		return -ENOMEM;
+
+	desc->len = len;
+	desc->trig_desc.type = cpu_to_le32(trig);
+	memcpy(desc->trig_desc.data, str, len);
+
+	return iwl_mvm_fw_dbg_collect_desc(mvm, desc, trigger);
+}
+
+int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
+				struct iwl_fw_dbg_trigger_tlv *trigger,
+				const char *fmt, ...)
+{
+	u16 occurrences = le16_to_cpu(trigger->occurrences);
+	int ret, len = 0;
+	char buf[64];
+
+	if (!occurrences)
+		return 0;
+
+	if (fmt) {
+		va_list ap;
+
+		buf[sizeof(buf) - 1] = '\0';
+
+		va_start(ap, fmt);
+		vsnprintf(buf, sizeof(buf), fmt, ap);
+		va_end(ap);
+
+		/* check for truncation */
+		if (WARN_ON_ONCE(buf[sizeof(buf) - 1]))
+			buf[sizeof(buf) - 1] = '\0';
+
+		len = strlen(buf) + 1;
+	}
+
+	ret = iwl_mvm_fw_dbg_collect(mvm, le32_to_cpu(trigger->id), buf, len,
+				     trigger);
+
+	if (ret)
+		return ret;
+
+	trigger->occurrences = cpu_to_le16(occurrences - 1);
+	return 0;
+}
+
+static inline void iwl_mvm_restart_early_start(struct iwl_mvm *mvm)
+{
+	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000)
+		iwl_clear_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x100);
+	else
+		iwl_write_prph(mvm->trans, DBGC_IN_SAMPLE, 1);
+}
+
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 conf_id)
+{
+	u8 *ptr;
+	int ret;
+	int i;
+
+	if (WARN_ONCE(conf_id >= ARRAY_SIZE(mvm->fw->dbg_conf_tlv),
+		      "Invalid configuration %d\n", conf_id))
+		return -EINVAL;
+
+	/* EARLY START - firmware's configuration is hard coded */
+	if ((!mvm->fw->dbg_conf_tlv[conf_id] ||
+	     !mvm->fw->dbg_conf_tlv[conf_id]->num_of_hcmds) &&
+	    conf_id == FW_DBG_START_FROM_ALIVE) {
+		iwl_mvm_restart_early_start(mvm);
+		return 0;
+	}
+
+	if (!mvm->fw->dbg_conf_tlv[conf_id])
+		return -EINVAL;
+
+	if (mvm->fw_dbg_conf != FW_DBG_INVALID)
+		IWL_WARN(mvm, "FW already configured (%d) - re-configuring\n",
+			 mvm->fw_dbg_conf);
+
+	/* Send all HCMDs for configuring the FW debug */
+	ptr = (void *)&mvm->fw->dbg_conf_tlv[conf_id]->hcmd;
+	for (i = 0; i < mvm->fw->dbg_conf_tlv[conf_id]->num_of_hcmds; i++) {
+		struct iwl_fw_dbg_conf_hcmd *cmd = (void *)ptr;
+
+		ret = iwl_mvm_send_cmd_pdu(mvm, cmd->id, 0,
+					   le16_to_cpu(cmd->len), cmd->data);
+		if (ret)
+			return ret;
+
+		ptr += sizeof(*cmd);
+		ptr += le16_to_cpu(cmd->len);
+	}
+
+	mvm->fw_dbg_conf = conf_id;
+	return ret;
+}
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h
new file mode 100644
index 000000000000..f7dff7612c9c
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw-dbg.h
@@ -0,0 +1,174 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program;
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <linuxwifi@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __mvm_fw_dbg_h__
+#define __mvm_fw_dbg_h__
+#include "iwl-fw-file.h"
+#include "iwl-fw-error-dump.h"
+#include "mvm.h"
+
+void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
+void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm);
+int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
+				const struct iwl_mvm_dump_desc *desc,
+				const struct iwl_fw_dbg_trigger_tlv *trigger);
+int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
+			   const char *str, size_t len,
+			   const struct iwl_fw_dbg_trigger_tlv *trigger);
+int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
+				struct iwl_fw_dbg_trigger_tlv *trigger,
+				const char *fmt, ...) __printf(3, 4);
+int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 id);
+
+#define iwl_fw_dbg_trigger_enabled(fw, id) ({			\
+	void *__dbg_trigger = (fw)->dbg_trigger_tlv[(id)];	\
+	unlikely(__dbg_trigger);				\
+})
+
+static inline struct iwl_fw_dbg_trigger_tlv*
+_iwl_fw_dbg_get_trigger(const struct iwl_fw *fw, enum iwl_fw_dbg_trigger id)
+{
+	return fw->dbg_trigger_tlv[id];
+}
+
+#define iwl_fw_dbg_get_trigger(fw, id) ({			\
+	BUILD_BUG_ON(!__builtin_constant_p(id));		\
+	BUILD_BUG_ON((id) >= FW_DBG_TRIGGER_MAX);		\
+	_iwl_fw_dbg_get_trigger((fw), (id));			\
+})
+
+static inline bool
+iwl_fw_dbg_trigger_vif_match(struct iwl_fw_dbg_trigger_tlv *trig,
+			     struct ieee80211_vif *vif)
+{
+	u32 trig_vif = le32_to_cpu(trig->vif_type);
+
+	return trig_vif == IWL_FW_DBG_CONF_VIF_ANY || vif->type == trig_vif;
+}
+
+static inline bool
+iwl_fw_dbg_trigger_stop_conf_match(struct iwl_mvm *mvm,
+				   struct iwl_fw_dbg_trigger_tlv *trig)
+{
+	return ((trig->mode & IWL_FW_DBG_TRIGGER_STOP) &&
+		(mvm->fw_dbg_conf == FW_DBG_INVALID ||
+		(BIT(mvm->fw_dbg_conf) & le32_to_cpu(trig->stop_conf_ids))));
+}
+
+static inline bool
+iwl_fw_dbg_no_trig_window(struct iwl_mvm *mvm,
+			  struct iwl_fw_dbg_trigger_tlv *trig)
+{
+	unsigned long wind_jiff =
+		msecs_to_jiffies(le16_to_cpu(trig->trig_dis_ms));
+	u32 id = le32_to_cpu(trig->id);
+
+	/* If this is the first event checked, jump to update start ts */
+	if (mvm->fw_dbg_non_collect_ts_start[id] &&
+	    (time_after(mvm->fw_dbg_non_collect_ts_start[id] + wind_jiff,
+			jiffies)))
+		return true;
+
+	mvm->fw_dbg_non_collect_ts_start[id] = jiffies;
+	return false;
+}
+
+static inline bool
+iwl_fw_dbg_trigger_check_stop(struct iwl_mvm *mvm,
+			      struct ieee80211_vif *vif,
+			      struct iwl_fw_dbg_trigger_tlv *trig)
+{
+	if (vif && !iwl_fw_dbg_trigger_vif_match(trig, vif))
+		return false;
+
+	if (iwl_fw_dbg_no_trig_window(mvm, trig)) {
+		IWL_WARN(mvm, "Trigger %d occurred while no-collect window.\n",
+			 trig->id);
+		return false;
+	}
+
+	return iwl_fw_dbg_trigger_stop_conf_match(mvm, trig);
+}
+
+static inline void
+_iwl_fw_dbg_trigger_simple_stop(struct iwl_mvm *mvm,
+				struct ieee80211_vif *vif,
+				struct iwl_fw_dbg_trigger_tlv *trigger)
+{
+	if (!trigger)
+		return;
+
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trigger))
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trigger, NULL);
+}
+
+#define iwl_fw_dbg_trigger_simple_stop(mvm, vif, trig)	\
+	_iwl_fw_dbg_trigger_simple_stop((mvm), (vif),	\
+					iwl_fw_dbg_get_trigger((mvm)->fw,\
+							       (trig)))
+
+#endif  /* __mvm_fw_dbg_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/intel/iwlwifi/mvm/fw.c
index d906fa13ba97..0ccc697fef76 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/fw.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -74,6 +74,7 @@
 #include "iwl-eeprom-parse.h"
 
 #include "mvm.h"
+#include "fw-dbg.h"
 #include "iwl-phy-db.h"
 
 #define MVM_UCODE_ALIVE_TIMEOUT	HZ
@@ -106,7 +107,7 @@ static int iwl_send_tx_ant_cfg(struct iwl_mvm *mvm, u8 valid_tx_ant)
 				    sizeof(tx_ant_cmd), &tx_ant_cmd);
 }
 
-static void iwl_free_fw_paging(struct iwl_mvm *mvm)
+void iwl_free_fw_paging(struct iwl_mvm *mvm)
 {
 	int i;
 
@@ -126,6 +127,8 @@ static void iwl_free_fw_paging(struct iwl_mvm *mvm)
 			     get_order(mvm->fw_paging_db[i].fw_paging_size));
 	}
 	kfree(mvm->trans->paging_download_buf);
+	mvm->trans->paging_download_buf = NULL;
+
 	memset(mvm->fw_paging_db, 0, sizeof(mvm->fw_paging_db));
 }
 
@@ -805,137 +808,6 @@ static void iwl_mvm_get_shared_mem_conf(struct iwl_mvm *mvm)
 	iwl_free_resp(&cmd);
 }
 
-int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
-				struct iwl_mvm_dump_desc *desc,
-				struct iwl_fw_dbg_trigger_tlv *trigger)
-{
-	unsigned int delay = 0;
-
-	if (trigger)
-		delay = msecs_to_jiffies(le32_to_cpu(trigger->stop_delay));
-
-	if (test_and_set_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status))
-		return -EBUSY;
-
-	if (WARN_ON(mvm->fw_dump_desc))
-		iwl_mvm_free_fw_dump_desc(mvm);
-
-	IWL_WARN(mvm, "Collecting data: trigger %d fired.\n",
-		 le32_to_cpu(desc->trig_desc.type));
-
-	mvm->fw_dump_desc = desc;
-	mvm->fw_dump_trig = trigger;
-
-	queue_delayed_work(system_wq, &mvm->fw_dump_wk, delay);
-
-	return 0;
-}
-
-int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
-			   const char *str, size_t len,
-			   struct iwl_fw_dbg_trigger_tlv *trigger)
-{
-	struct iwl_mvm_dump_desc *desc;
-
-	desc = kzalloc(sizeof(*desc) + len, GFP_ATOMIC);
-	if (!desc)
-		return -ENOMEM;
-
-	desc->len = len;
-	desc->trig_desc.type = cpu_to_le32(trig);
-	memcpy(desc->trig_desc.data, str, len);
-
-	return iwl_mvm_fw_dbg_collect_desc(mvm, desc, trigger);
-}
-
-int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
-				struct iwl_fw_dbg_trigger_tlv *trigger,
-				const char *fmt, ...)
-{
-	u16 occurrences = le16_to_cpu(trigger->occurrences);
-	int ret, len = 0;
-	char buf[64];
-
-	if (!occurrences)
-		return 0;
-
-	if (fmt) {
-		va_list ap;
-
-		buf[sizeof(buf) - 1] = '\0';
-
-		va_start(ap, fmt);
-		vsnprintf(buf, sizeof(buf), fmt, ap);
-		va_end(ap);
-
-		/* check for truncation */
-		if (WARN_ON_ONCE(buf[sizeof(buf) - 1]))
-			buf[sizeof(buf) - 1] = '\0';
-
-		len = strlen(buf) + 1;
-	}
-
-	ret = iwl_mvm_fw_dbg_collect(mvm, le32_to_cpu(trigger->id), buf, len,
-				     trigger);
-
-	if (ret)
-		return ret;
-
-	trigger->occurrences = cpu_to_le16(occurrences - 1);
-	return 0;
-}
-
-static inline void iwl_mvm_restart_early_start(struct iwl_mvm *mvm)
-{
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000)
-		iwl_clear_bits_prph(mvm->trans, MON_BUFF_SAMPLE_CTL, 0x100);
-	else
-		iwl_write_prph(mvm->trans, DBGC_IN_SAMPLE, 1);
-}
-
-int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 conf_id)
-{
-	u8 *ptr;
-	int ret;
-	int i;
-
-	if (WARN_ONCE(conf_id >= ARRAY_SIZE(mvm->fw->dbg_conf_tlv),
-		      "Invalid configuration %d\n", conf_id))
-		return -EINVAL;
-
-	/* EARLY START - firmware's configuration is hard coded */
-	if ((!mvm->fw->dbg_conf_tlv[conf_id] ||
-	     !mvm->fw->dbg_conf_tlv[conf_id]->num_of_hcmds) &&
-	    conf_id == FW_DBG_START_FROM_ALIVE) {
-		iwl_mvm_restart_early_start(mvm);
-		return 0;
-	}
-
-	if (!mvm->fw->dbg_conf_tlv[conf_id])
-		return -EINVAL;
-
-	if (mvm->fw_dbg_conf != FW_DBG_INVALID)
-		IWL_WARN(mvm, "FW already configured (%d) - re-configuring\n",
-			 mvm->fw_dbg_conf);
-
-	/* Send all HCMDs for configuring the FW debug */
-	ptr = (void *)&mvm->fw->dbg_conf_tlv[conf_id]->hcmd;
-	for (i = 0; i < mvm->fw->dbg_conf_tlv[conf_id]->num_of_hcmds; i++) {
-		struct iwl_fw_dbg_conf_hcmd *cmd = (void *)ptr;
-
-		ret = iwl_mvm_send_cmd_pdu(mvm, cmd->id, 0,
-					   le16_to_cpu(cmd->len), cmd->data);
-		if (ret)
-			return ret;
-
-		ptr += sizeof(*cmd);
-		ptr += le16_to_cpu(cmd->len);
-	}
-
-	mvm->fw_dbg_conf = conf_id;
-	return ret;
-}
-
 static int iwl_mvm_config_ltr(struct iwl_mvm *mvm)
 {
 	struct iwl_ltr_config_cmd cmd = {
@@ -1073,6 +945,7 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	}
 
 	if (fw_has_capa(&mvm->fw->ucode_capa, IWL_UCODE_TLV_CAPA_UMAC_SCAN)) {
+		mvm->scan_type = IWL_SCAN_TYPE_NOT_SET;
 		ret = iwl_mvm_config_scan(mvm);
 		if (ret)
 			goto error;
diff --git a/drivers/net/wireless/iwlwifi/mvm/led.c b/drivers/net/wireless/intel/iwlwifi/mvm/led.c
index e3b3cf4dbd77..1e51fbe95f7c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/led.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/led.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c
index ad7ad720d2e7..bf1e5eb5dbdb 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mac-ctxt.c
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -72,6 +72,7 @@
 #include "fw-api.h"
 #include "mvm.h"
 #include "time-event.h"
+#include "fw-dbg.h"
 
 const u8 iwl_mvm_ac_to_tx_fifo[] = {
 	IWL_MVM_TX_FIFO_VO,
@@ -716,6 +717,8 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 		cpu_to_le32(vif->bss_conf.use_short_slot ?
 			    MAC_FLG_SHORT_SLOT : 0);
 
+	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP);
+
 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
 		u8 txf = iwl_mvm_ac_to_tx_fifo[i];
 
@@ -729,11 +732,26 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 		cmd->ac[txf].fifos_mask = BIT(txf);
 	}
 
-	/* in AP mode, the MCAST FIFO takes the EDCA params from VO */
-	if (vif->type == NL80211_IFTYPE_AP)
+	if (vif->type == NL80211_IFTYPE_AP) {
+		/* in AP mode, the MCAST FIFO takes the EDCA params from VO */
 		cmd->ac[IWL_MVM_TX_FIFO_VO].fifos_mask |=
 			BIT(IWL_MVM_TX_FIFO_MCAST);
 
+		/*
+		 * in AP mode, pass probe requests and beacons from other APs
+		 * (needed for ht protection); when there're no any associated
+		 * station don't ask FW to pass beacons to prevent unnecessary
+		 * wake-ups.
+		 */
+		cmd->filter_flags |= cpu_to_le32(MAC_FILTER_IN_PROBE_REQUEST);
+		if (mvmvif->ap_assoc_sta_count) {
+			cmd->filter_flags |= cpu_to_le32(MAC_FILTER_IN_BEACON);
+			IWL_DEBUG_HC(mvm, "Asking FW to pass beacons\n");
+		} else {
+			IWL_DEBUG_HC(mvm, "No need to receive beacons\n");
+		}
+	}
+
 	if (vif->bss_conf.qos)
 		cmd->qos_flags |= cpu_to_le32(MAC_QOS_FLG_UPDATE_EDCA);
 
@@ -747,8 +765,6 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 		cmd->qos_flags |= cpu_to_le32(MAC_QOS_FLG_TGN);
 	if (ht_enabled)
 		iwl_mvm_mac_ctxt_set_ht_flags(mvm, vif, cmd);
-
-	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP);
 }
 
 static int iwl_mvm_mac_ctxt_send_cmd(struct iwl_mvm *mvm,
@@ -854,11 +870,17 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
 					 u32 action)
 {
 	struct iwl_mac_ctx_cmd cmd = {};
+	u32 tfd_queue_msk = 0;
+	int ret, i;
 
 	WARN_ON(vif->type != NL80211_IFTYPE_MONITOR);
 
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
+	for (i = 0; i < IEEE80211_NUM_ACS; i++)
+		if (vif->hw_queue[i] != IEEE80211_INVAL_HW_QUEUE)
+			tfd_queue_msk |= BIT(vif->hw_queue[i]);
+
 	cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_PROMISC |
 				       MAC_FILTER_IN_CONTROL_AND_MGMT |
 				       MAC_FILTER_IN_BEACON |
@@ -866,6 +888,12 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
 				       MAC_FILTER_IN_CRC32);
 	ieee80211_hw_set(mvm->hw, RX_INCLUDES_FCS);
 
+	/* Allocate sniffer station */
+	ret = iwl_mvm_allocate_int_sta(mvm, &mvm->snif_sta, tfd_queue_msk,
+				       vif->type);
+	if (ret)
+		return ret;
+
 	return iwl_mvm_mac_ctxt_send_cmd(mvm, &cmd);
 }
 
@@ -999,9 +1027,12 @@ static int iwl_mvm_mac_ctxt_send_beacon(struct iwl_mvm *mvm,
 						TX_CMD_FLG_BT_PRIO_POS;
 	beacon_cmd.tx.tx_flags = cpu_to_le32(tx_flags);
 
-	mvm->mgmt_last_antenna_idx =
-		iwl_mvm_next_antenna(mvm, iwl_mvm_get_valid_tx_ant(mvm),
-				     mvm->mgmt_last_antenna_idx);
+	if (!fw_has_capa(&mvm->fw->ucode_capa,
+			 IWL_UCODE_TLV_CAPA_BEACON_ANT_SELECTION)) {
+		mvm->mgmt_last_antenna_idx =
+			iwl_mvm_next_antenna(mvm, iwl_mvm_get_valid_tx_ant(mvm),
+					     mvm->mgmt_last_antenna_idx);
+	}
 
 	beacon_cmd.tx.rate_n_flags =
 		cpu_to_le32(BIT(mvm->mgmt_last_antenna_idx) <<
@@ -1140,7 +1171,6 @@ static int iwl_mvm_mac_ctxt_cmd_ap(struct iwl_mvm *mvm,
 				   struct ieee80211_vif *vif,
 				   u32 action)
 {
-	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mac_ctx_cmd cmd = {};
 
 	WARN_ON(vif->type != NL80211_IFTYPE_AP || vif->p2p);
@@ -1148,19 +1178,6 @@ static int iwl_mvm_mac_ctxt_cmd_ap(struct iwl_mvm *mvm,
 	/* Fill the common data for all mac context types */
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
-	/*
-	 * pass probe requests and beacons from other APs (needed
-	 * for ht protection); when there're no any associated station
-	 * don't ask FW to pass beacons to prevent unnecessary wake-ups.
-	 */
-	cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_PROBE_REQUEST);
-	if (mvmvif->ap_assoc_sta_count) {
-		cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_BEACON);
-		IWL_DEBUG_HC(mvm, "Asking FW to pass beacons\n");
-	} else {
-		IWL_DEBUG_HC(mvm, "No need to receive beacons\n");
-	}
-
 	/* Fill the data specific for ap mode */
 	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.ap,
 				     action == FW_CTXT_ACTION_ADD);
@@ -1180,13 +1197,6 @@ static int iwl_mvm_mac_ctxt_cmd_go(struct iwl_mvm *mvm,
 	/* Fill the common data for all mac context types */
 	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
-	/*
-	 * pass probe requests and beacons from other APs (needed
-	 * for ht protection)
-	 */
-	cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_PROBE_REQUEST |
-					MAC_FILTER_IN_BEACON);
-
 	/* Fill the data specific for GO mode */
 	iwl_mvm_mac_ctxt_cmd_fill_ap(mvm, vif, &cmd.go.ap,
 				     action == FW_CTXT_ACTION_ADD);
@@ -1288,8 +1298,10 @@ int iwl_mvm_mac_ctxt_remove(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	mvmvif->uploaded = false;
 
-	if (vif->type == NL80211_IFTYPE_MONITOR)
+	if (vif->type == NL80211_IFTYPE_MONITOR) {
 		__clear_bit(IEEE80211_HW_RX_INCLUDES_FCS, mvm->hw->flags);
+		iwl_mvm_dealloc_snif_sta(mvm);
+	}
 
 	return 0;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
index e88afac51c5d..d70a1716f3e0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mac80211.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -70,6 +70,7 @@
 #include <linux/ip.h>
 #include <linux/if_arp.h>
 #include <linux/devcoredump.h>
+#include <linux/time.h>
 #include <net/mac80211.h>
 #include <net/ieee80211_radiotap.h>
 #include <net/tcp.h>
@@ -86,6 +87,7 @@
 #include "iwl-prph.h"
 #include "iwl-csr.h"
 #include "iwl-nvm-parse.h"
+#include "fw-dbg.h"
 
 static const struct ieee80211_iface_limit iwl_mvm_limits[] = {
 	{
@@ -436,6 +438,11 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	ieee80211_hw_set(hw, CHANCTX_STA_CSA);
 	ieee80211_hw_set(hw, SUPPORT_FAST_XMIT);
 	ieee80211_hw_set(hw, SUPPORTS_CLONED_SKBS);
+	ieee80211_hw_set(hw, SUPPORTS_AMSDU_IN_AMPDU);
+	ieee80211_hw_set(hw, NEEDS_UNIQUE_STA_ADDR);
+
+	if (mvm->trans->max_skb_frags)
+		hw->netdev_features = NETIF_F_HIGHDMA | NETIF_F_SG;
 
 	hw->queues = mvm->first_agg_queue;
 	hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
@@ -662,6 +669,10 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	if (!iwl_mvm_is_csum_supported(mvm))
 		hw->netdev_features &= ~NETIF_F_RXCSUM;
 
+	if (IWL_MVM_SW_TX_CSUM_OFFLOAD)
+		hw->netdev_features |= NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
+			NETIF_F_TSO | NETIF_F_TSO6;
+
 	ret = ieee80211_register_hw(mvm->hw);
 	if (ret)
 		iwl_mvm_leds_exit(mvm);
@@ -939,431 +950,6 @@ static void iwl_mvm_cleanup_iterator(void *data, u8 *mac,
 	memset(&mvmvif->bf_data, 0, sizeof(mvmvif->bf_data));
 }
 
-static ssize_t iwl_mvm_read_coredump(char *buffer, loff_t offset, size_t count,
-				     const void *data, size_t datalen)
-{
-	const struct iwl_mvm_dump_ptrs *dump_ptrs = data;
-	ssize_t bytes_read;
-	ssize_t bytes_read_trans;
-
-	if (offset < dump_ptrs->op_mode_len) {
-		bytes_read = min_t(ssize_t, count,
-				   dump_ptrs->op_mode_len - offset);
-		memcpy(buffer, (u8 *)dump_ptrs->op_mode_ptr + offset,
-		       bytes_read);
-		offset += bytes_read;
-		count -= bytes_read;
-
-		if (count == 0)
-			return bytes_read;
-	} else {
-		bytes_read = 0;
-	}
-
-	if (!dump_ptrs->trans_ptr)
-		return bytes_read;
-
-	offset -= dump_ptrs->op_mode_len;
-	bytes_read_trans = min_t(ssize_t, count,
-				 dump_ptrs->trans_ptr->len - offset);
-	memcpy(buffer + bytes_read,
-	       (u8 *)dump_ptrs->trans_ptr->data + offset,
-	       bytes_read_trans);
-
-	return bytes_read + bytes_read_trans;
-}
-
-static void iwl_mvm_free_coredump(const void *data)
-{
-	const struct iwl_mvm_dump_ptrs *fw_error_dump = data;
-
-	vfree(fw_error_dump->op_mode_ptr);
-	vfree(fw_error_dump->trans_ptr);
-	kfree(fw_error_dump);
-}
-
-static void iwl_mvm_dump_fifos(struct iwl_mvm *mvm,
-			       struct iwl_fw_error_dump_data **dump_data)
-{
-	struct iwl_fw_error_dump_fifo *fifo_hdr;
-	u32 *fifo_data;
-	u32 fifo_len;
-	unsigned long flags;
-	int i, j;
-
-	if (!iwl_trans_grab_nic_access(mvm->trans, false, &flags))
-		return;
-
-	/* Pull RXF data from all RXFs */
-	for (i = 0; i < ARRAY_SIZE(mvm->shared_mem_cfg.rxfifo_size); i++) {
-		/*
-		 * Keep aside the additional offset that might be needed for
-		 * next RXF
-		 */
-		u32 offset_diff = RXF_DIFF_FROM_PREV * i;
-
-		fifo_hdr = (void *)(*dump_data)->data;
-		fifo_data = (void *)fifo_hdr->data;
-		fifo_len = mvm->shared_mem_cfg.rxfifo_size[i];
-
-		/* No need to try to read the data if the length is 0 */
-		if (fifo_len == 0)
-			continue;
-
-		/* Add a TLV for the RXF */
-		(*dump_data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_RXF);
-		(*dump_data)->len = cpu_to_le32(fifo_len + sizeof(*fifo_hdr));
-
-		fifo_hdr->fifo_num = cpu_to_le32(i);
-		fifo_hdr->available_bytes =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							RXF_RD_D_SPACE +
-							offset_diff));
-		fifo_hdr->wr_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							RXF_RD_WR_PTR +
-							offset_diff));
-		fifo_hdr->rd_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							RXF_RD_RD_PTR +
-							offset_diff));
-		fifo_hdr->fence_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							RXF_RD_FENCE_PTR +
-							offset_diff));
-		fifo_hdr->fence_mode =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							RXF_SET_FENCE_MODE +
-							offset_diff));
-
-		/* Lock fence */
-		iwl_trans_write_prph(mvm->trans,
-				     RXF_SET_FENCE_MODE + offset_diff, 0x1);
-		/* Set fence pointer to the same place like WR pointer */
-		iwl_trans_write_prph(mvm->trans,
-				     RXF_LD_WR2FENCE + offset_diff, 0x1);
-		/* Set fence offset */
-		iwl_trans_write_prph(mvm->trans,
-				     RXF_LD_FENCE_OFFSET_ADDR + offset_diff,
-				     0x0);
-
-		/* Read FIFO */
-		fifo_len /= sizeof(u32); /* Size in DWORDS */
-		for (j = 0; j < fifo_len; j++)
-			fifo_data[j] = iwl_trans_read_prph(mvm->trans,
-							 RXF_FIFO_RD_FENCE_INC +
-							 offset_diff);
-		*dump_data = iwl_fw_error_next_data(*dump_data);
-	}
-
-	/* Pull TXF data from all TXFs */
-	for (i = 0; i < ARRAY_SIZE(mvm->shared_mem_cfg.txfifo_size); i++) {
-		/* Mark the number of TXF we're pulling now */
-		iwl_trans_write_prph(mvm->trans, TXF_LARC_NUM, i);
-
-		fifo_hdr = (void *)(*dump_data)->data;
-		fifo_data = (void *)fifo_hdr->data;
-		fifo_len = mvm->shared_mem_cfg.txfifo_size[i];
-
-		/* No need to try to read the data if the length is 0 */
-		if (fifo_len == 0)
-			continue;
-
-		/* Add a TLV for the FIFO */
-		(*dump_data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_TXF);
-		(*dump_data)->len = cpu_to_le32(fifo_len + sizeof(*fifo_hdr));
-
-		fifo_hdr->fifo_num = cpu_to_le32(i);
-		fifo_hdr->available_bytes =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							TXF_FIFO_ITEM_CNT));
-		fifo_hdr->wr_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							TXF_WR_PTR));
-		fifo_hdr->rd_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							TXF_RD_PTR));
-		fifo_hdr->fence_ptr =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							TXF_FENCE_PTR));
-		fifo_hdr->fence_mode =
-			cpu_to_le32(iwl_trans_read_prph(mvm->trans,
-							TXF_LOCK_FENCE));
-
-		/* Set the TXF_READ_MODIFY_ADDR to TXF_WR_PTR */
-		iwl_trans_write_prph(mvm->trans, TXF_READ_MODIFY_ADDR,
-				     TXF_WR_PTR);
-
-		/* Dummy-read to advance the read pointer to the head */
-		iwl_trans_read_prph(mvm->trans, TXF_READ_MODIFY_DATA);
-
-		/* Read FIFO */
-		fifo_len /= sizeof(u32); /* Size in DWORDS */
-		for (j = 0; j < fifo_len; j++)
-			fifo_data[j] = iwl_trans_read_prph(mvm->trans,
-							  TXF_READ_MODIFY_DATA);
-		*dump_data = iwl_fw_error_next_data(*dump_data);
-	}
-
-	iwl_trans_release_nic_access(mvm->trans, &flags);
-}
-
-void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm)
-{
-	if (mvm->fw_dump_desc == &iwl_mvm_dump_desc_assert ||
-	    !mvm->fw_dump_desc)
-		return;
-
-	kfree(mvm->fw_dump_desc);
-	mvm->fw_dump_desc = NULL;
-}
-
-#define IWL8260_ICCM_OFFSET		0x44000 /* Only for B-step */
-#define IWL8260_ICCM_LEN		0xC000 /* Only for B-step */
-
-void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
-{
-	struct iwl_fw_error_dump_file *dump_file;
-	struct iwl_fw_error_dump_data *dump_data;
-	struct iwl_fw_error_dump_info *dump_info;
-	struct iwl_fw_error_dump_mem *dump_mem;
-	struct iwl_fw_error_dump_trigger_desc *dump_trig;
-	struct iwl_mvm_dump_ptrs *fw_error_dump;
-	u32 sram_len, sram_ofs;
-	u32 file_len, fifo_data_len = 0;
-	u32 smem_len = mvm->cfg->smem_len;
-	u32 sram2_len = mvm->cfg->dccm2_len;
-	bool monitor_dump_only = false;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	/* there's no point in fw dump if the bus is dead */
-	if (test_bit(STATUS_TRANS_DEAD, &mvm->trans->status)) {
-		IWL_ERR(mvm, "Skip fw error dump since bus is dead\n");
-		return;
-	}
-
-	if (mvm->fw_dump_trig &&
-	    mvm->fw_dump_trig->mode & IWL_FW_DBG_TRIGGER_MONITOR_ONLY)
-		monitor_dump_only = true;
-
-	fw_error_dump = kzalloc(sizeof(*fw_error_dump), GFP_KERNEL);
-	if (!fw_error_dump)
-		return;
-
-	/* SRAM - include stack CCM if driver knows the values for it */
-	if (!mvm->cfg->dccm_offset || !mvm->cfg->dccm_len) {
-		const struct fw_img *img;
-
-		img = &mvm->fw->img[mvm->cur_ucode];
-		sram_ofs = img->sec[IWL_UCODE_SECTION_DATA].offset;
-		sram_len = img->sec[IWL_UCODE_SECTION_DATA].len;
-	} else {
-		sram_ofs = mvm->cfg->dccm_offset;
-		sram_len = mvm->cfg->dccm_len;
-	}
-
-	/* reading RXF/TXF sizes */
-	if (test_bit(STATUS_FW_ERROR, &mvm->trans->status)) {
-		struct iwl_mvm_shared_mem_cfg *mem_cfg = &mvm->shared_mem_cfg;
-		int i;
-
-		fifo_data_len = 0;
-
-		/* Count RXF size */
-		for (i = 0; i < ARRAY_SIZE(mem_cfg->rxfifo_size); i++) {
-			if (!mem_cfg->rxfifo_size[i])
-				continue;
-
-			/* Add header info */
-			fifo_data_len += mem_cfg->rxfifo_size[i] +
-					 sizeof(*dump_data) +
-					 sizeof(struct iwl_fw_error_dump_fifo);
-		}
-
-		for (i = 0; i < ARRAY_SIZE(mem_cfg->txfifo_size); i++) {
-			if (!mem_cfg->txfifo_size[i])
-				continue;
-
-			/* Add header info */
-			fifo_data_len += mem_cfg->txfifo_size[i] +
-					 sizeof(*dump_data) +
-					 sizeof(struct iwl_fw_error_dump_fifo);
-		}
-	}
-
-	file_len = sizeof(*dump_file) +
-		   sizeof(*dump_data) * 2 +
-		   sram_len + sizeof(*dump_mem) +
-		   fifo_data_len +
-		   sizeof(*dump_info);
-
-	/* Make room for the SMEM, if it exists */
-	if (smem_len)
-		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + smem_len;
-
-	/* Make room for the secondary SRAM, if it exists */
-	if (sram2_len)
-		file_len += sizeof(*dump_data) + sizeof(*dump_mem) + sram2_len;
-
-	/* Make room for fw's virtual image pages, if it exists */
-	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size)
-		file_len += mvm->num_of_paging_blk *
-			(sizeof(*dump_data) +
-			 sizeof(struct iwl_fw_error_dump_paging) +
-			 PAGING_BLOCK_SIZE);
-
-	/* If we only want a monitor dump, reset the file length */
-	if (monitor_dump_only) {
-		file_len = sizeof(*dump_file) + sizeof(*dump_data) +
-			   sizeof(*dump_info);
-	}
-
-	/*
-	 * In 8000 HW family B-step include the ICCM (which resides separately)
-	 */
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP)
-		file_len += sizeof(*dump_data) + sizeof(*dump_mem) +
-			    IWL8260_ICCM_LEN;
-
-	if (mvm->fw_dump_desc)
-		file_len += sizeof(*dump_data) + sizeof(*dump_trig) +
-			    mvm->fw_dump_desc->len;
-
-	dump_file = vzalloc(file_len);
-	if (!dump_file) {
-		kfree(fw_error_dump);
-		iwl_mvm_free_fw_dump_desc(mvm);
-		return;
-	}
-
-	fw_error_dump->op_mode_ptr = dump_file;
-
-	dump_file->barker = cpu_to_le32(IWL_FW_ERROR_DUMP_BARKER);
-	dump_data = (void *)dump_file->data;
-
-	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_DEV_FW_INFO);
-	dump_data->len = cpu_to_le32(sizeof(*dump_info));
-	dump_info = (void *) dump_data->data;
-	dump_info->device_family =
-		mvm->cfg->device_family == IWL_DEVICE_FAMILY_7000 ?
-			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_7) :
-			cpu_to_le32(IWL_FW_ERROR_DUMP_FAMILY_8);
-	dump_info->hw_step = cpu_to_le32(CSR_HW_REV_STEP(mvm->trans->hw_rev));
-	memcpy(dump_info->fw_human_readable, mvm->fw->human_readable,
-	       sizeof(dump_info->fw_human_readable));
-	strncpy(dump_info->dev_human_readable, mvm->cfg->name,
-		sizeof(dump_info->dev_human_readable));
-	strncpy(dump_info->bus_human_readable, mvm->dev->bus->name,
-		sizeof(dump_info->bus_human_readable));
-
-	dump_data = iwl_fw_error_next_data(dump_data);
-	/* We only dump the FIFOs if the FW is in error state */
-	if (test_bit(STATUS_FW_ERROR, &mvm->trans->status))
-		iwl_mvm_dump_fifos(mvm, &dump_data);
-
-	if (mvm->fw_dump_desc) {
-		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_ERROR_INFO);
-		dump_data->len = cpu_to_le32(sizeof(*dump_trig) +
-					     mvm->fw_dump_desc->len);
-		dump_trig = (void *)dump_data->data;
-		memcpy(dump_trig, &mvm->fw_dump_desc->trig_desc,
-		       sizeof(*dump_trig) + mvm->fw_dump_desc->len);
-
-		/* now we can free this copy */
-		iwl_mvm_free_fw_dump_desc(mvm);
-		dump_data = iwl_fw_error_next_data(dump_data);
-	}
-
-	/* In case we only want monitor dump, skip to dump trasport data */
-	if (monitor_dump_only)
-		goto dump_trans_data;
-
-	dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
-	dump_data->len = cpu_to_le32(sram_len + sizeof(*dump_mem));
-	dump_mem = (void *)dump_data->data;
-	dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
-	dump_mem->offset = cpu_to_le32(sram_ofs);
-	iwl_trans_read_mem_bytes(mvm->trans, sram_ofs, dump_mem->data,
-				 sram_len);
-
-	if (smem_len) {
-		dump_data = iwl_fw_error_next_data(dump_data);
-		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
-		dump_data->len = cpu_to_le32(smem_len + sizeof(*dump_mem));
-		dump_mem = (void *)dump_data->data;
-		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SMEM);
-		dump_mem->offset = cpu_to_le32(mvm->cfg->smem_offset);
-		iwl_trans_read_mem_bytes(mvm->trans, mvm->cfg->smem_offset,
-					 dump_mem->data, smem_len);
-	}
-
-	if (sram2_len) {
-		dump_data = iwl_fw_error_next_data(dump_data);
-		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
-		dump_data->len = cpu_to_le32(sram2_len + sizeof(*dump_mem));
-		dump_mem = (void *)dump_data->data;
-		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
-		dump_mem->offset = cpu_to_le32(mvm->cfg->dccm2_offset);
-		iwl_trans_read_mem_bytes(mvm->trans, mvm->cfg->dccm2_offset,
-					 dump_mem->data, sram2_len);
-	}
-
-	if (mvm->cfg->device_family == IWL_DEVICE_FAMILY_8000 &&
-	    CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_B_STEP) {
-		dump_data = iwl_fw_error_next_data(dump_data);
-		dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM);
-		dump_data->len = cpu_to_le32(IWL8260_ICCM_LEN +
-					     sizeof(*dump_mem));
-		dump_mem = (void *)dump_data->data;
-		dump_mem->type = cpu_to_le32(IWL_FW_ERROR_DUMP_MEM_SRAM);
-		dump_mem->offset = cpu_to_le32(IWL8260_ICCM_OFFSET);
-		iwl_trans_read_mem_bytes(mvm->trans, IWL8260_ICCM_OFFSET,
-					 dump_mem->data, IWL8260_ICCM_LEN);
-	}
-
-	/* Dump fw's virtual image */
-	if (mvm->fw->img[mvm->cur_ucode].paging_mem_size) {
-		u32 i;
-
-		for (i = 1; i < mvm->num_of_paging_blk + 1; i++) {
-			struct iwl_fw_error_dump_paging *paging;
-			struct page *pages =
-				mvm->fw_paging_db[i].fw_paging_block;
-
-			dump_data = iwl_fw_error_next_data(dump_data);
-			dump_data->type = cpu_to_le32(IWL_FW_ERROR_DUMP_PAGING);
-			dump_data->len = cpu_to_le32(sizeof(*paging) +
-						     PAGING_BLOCK_SIZE);
-			paging = (void *)dump_data->data;
-			paging->index = cpu_to_le32(i);
-			memcpy(paging->data, page_address(pages),
-			       PAGING_BLOCK_SIZE);
-		}
-	}
-
-dump_trans_data:
-	fw_error_dump->trans_ptr = iwl_trans_dump_data(mvm->trans,
-						       mvm->fw_dump_trig);
-	fw_error_dump->op_mode_len = file_len;
-	if (fw_error_dump->trans_ptr)
-		file_len += fw_error_dump->trans_ptr->len;
-	dump_file->file_len = cpu_to_le32(file_len);
-
-	dev_coredumpm(mvm->trans->dev, THIS_MODULE, fw_error_dump, 0,
-		      GFP_KERNEL, iwl_mvm_read_coredump, iwl_mvm_free_coredump);
-
-	mvm->fw_dump_trig = NULL;
-	clear_bit(IWL_MVM_STATUS_DUMPING_FW_LOG, &mvm->status);
-}
-
-struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert = {
-	.trig_desc = {
-		.type = cpu_to_le32(FW_DBG_TRIGGER_FW_ASSERT),
-	},
-};
-
 static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 {
 	/* clear the D3 reconfig, we only need it to avoid dumping a
@@ -1387,6 +973,7 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 	mvm->calibrating = false;
 
 	/* just in case one was running */
+	iwl_mvm_cleanup_roc_te(mvm);
 	ieee80211_remain_on_channel_expired(mvm->hw);
 
 	/*
@@ -1399,6 +986,7 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 	mvm->d0i3_ap_sta_id = IWL_MVM_STATION_COUNT;
 
 	iwl_mvm_reset_phy_ctxts(mvm);
+	memset(mvm->fw_key_table, 0, sizeof(mvm->fw_key_table));
 	memset(mvm->sta_drained, 0, sizeof(mvm->sta_drained));
 	memset(mvm->tfd_drained, 0, sizeof(mvm->tfd_drained));
 	memset(&mvm->last_bt_notif, 0, sizeof(mvm->last_bt_notif));
@@ -1427,10 +1015,18 @@ int __iwl_mvm_mac_start(struct iwl_mvm *mvm)
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* Clean up some internal and mac80211 state on restart */
-	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
+	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
+		/* Clean up some internal and mac80211 state on restart */
 		iwl_mvm_restart_cleanup(mvm);
-
+	} else {
+		/* Hold the reference to prevent runtime suspend while
+		 * the start procedure runs.  It's a bit confusing
+		 * that the UCODE_DOWN reference is taken, but it just
+		 * means "UCODE is not UP yet". ( TODO: rename this
+		 * reference).
+		 */
+		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
+	}
 	ret = iwl_mvm_up(mvm);
 
 	if (ret && test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
@@ -1497,15 +1093,13 @@ static void iwl_mvm_restart_complete(struct iwl_mvm *mvm)
 
 static void iwl_mvm_resume_complete(struct iwl_mvm *mvm)
 {
-	if (!iwl_mvm_is_d0i3_supported(mvm))
-		return;
-
-	if (mvm->trans->d0i3_mode == IWL_D0I3_MODE_ON_SUSPEND)
-		if (!wait_event_timeout(mvm->d0i3_exit_waitq,
-					!test_bit(IWL_MVM_STATUS_IN_D0I3,
-						  &mvm->status),
-					HZ))
-			WARN_ONCE(1, "D0i3 exit on resume timed out\n");
+	if (iwl_mvm_is_d0i3_supported(mvm) &&
+	    iwl_mvm_enter_d0i3_on_suspend(mvm))
+		WARN_ONCE(!wait_event_timeout(mvm->d0i3_exit_waitq,
+					      !test_bit(IWL_MVM_STATUS_IN_D0I3,
+							&mvm->status),
+					      HZ),
+			  "D0i3 exit on resume timed out\n");
 }
 
 static void
@@ -1533,14 +1127,6 @@ void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 	 */
 	memset(&mvm->accu_radio_stats, 0, sizeof(mvm->accu_radio_stats));
 
-	/*
-	 * Disallow low power states when the FW is down by taking
-	 * the UCODE_DOWN ref. in case of ongoing hw restart the
-	 * ref is already taken, so don't take it again.
-	 */
-	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
-		iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
-
 	/* async_handlers_wk is now blocked */
 
 	/*
@@ -2152,8 +1738,8 @@ bool iwl_mvm_bcast_filter_build_cmd(struct iwl_mvm *mvm,
 
 	return true;
 }
-static int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
-					  struct ieee80211_vif *vif)
+
+static int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm)
 {
 	struct iwl_bcast_filter_cmd cmd;
 
@@ -2167,8 +1753,7 @@ static int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
 				    sizeof(cmd), &cmd);
 }
 #else
-static inline int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
-						 struct ieee80211_vif *vif)
+static inline int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm)
 {
 	return 0;
 }
@@ -2283,7 +1868,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		}
 
 		iwl_mvm_recalc_multicast(mvm);
-		iwl_mvm_configure_bcast_filter(mvm, vif);
+		iwl_mvm_configure_bcast_filter(mvm);
 
 		/* reset rssi values */
 		mvmvif->bf_data.ave_beacon_signal = 0;
@@ -2291,6 +1876,9 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		iwl_mvm_bt_coex_vif_change(mvm);
 		iwl_mvm_update_smps(mvm, vif, IWL_MVM_SMPS_REQ_TT,
 				    IEEE80211_SMPS_AUTOMATIC);
+		if (fw_has_capa(&mvm->fw->ucode_capa,
+				IWL_UCODE_TLV_CAPA_UMAC_SCAN))
+			iwl_mvm_config_scan(mvm);
 	} else if (changes & BSS_CHANGED_BEACON_INFO) {
 		/*
 		 * We received a beacon _after_ association so
@@ -2331,7 +1919,7 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 
 	if (changes & BSS_CHANGED_ARP_FILTER) {
 		IWL_DEBUG_MAC80211(mvm, "arp filter changed\n");
-		iwl_mvm_configure_bcast_filter(mvm, vif);
+		iwl_mvm_configure_bcast_filter(mvm);
 	}
 }
 
@@ -2661,7 +2249,6 @@ static void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
 				       struct ieee80211_sta *sta)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 
 	/*
@@ -2677,11 +2264,6 @@ static void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
 		rcu_assign_pointer(mvm->fw_id_to_mac_id[mvm_sta->sta_id],
 				   ERR_PTR(-ENOENT));
 
-	if (mvm_sta->vif->type == NL80211_IFTYPE_AP) {
-		mvmvif->ap_assoc_sta_count--;
-		iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
-	}
-
 	mutex_unlock(&mvm->mutex);
 }
 
@@ -2699,6 +2281,34 @@ static void iwl_mvm_check_uapsd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	vif->driver_flags |= IEEE80211_VIF_SUPPORTS_UAPSD;
 }
 
+static void
+iwl_mvm_tdls_check_trigger(struct iwl_mvm *mvm,
+			   struct ieee80211_vif *vif, u8 *peer_addr,
+			   enum nl80211_tdls_operation action)
+{
+	struct iwl_fw_dbg_trigger_tlv *trig;
+	struct iwl_fw_dbg_trigger_tdls *tdls_trig;
+
+	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_TDLS))
+		return;
+
+	trig = iwl_fw_dbg_get_trigger(mvm->fw, FW_DBG_TRIGGER_TDLS);
+	tdls_trig = (void *)trig->data;
+	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trig))
+		return;
+
+	if (!(tdls_trig->action_bitmap & BIT(action)))
+		return;
+
+	if (tdls_trig->peer_mode &&
+	    memcmp(tdls_trig->peer, peer_addr, ETH_ALEN) != 0)
+		return;
+
+	iwl_mvm_fw_dbg_collect_trig(mvm, trig,
+				    "TDLS event occurred, peer %pM, action %d",
+				    peer_addr, action);
+}
+
 static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 				 struct ieee80211_vif *vif,
 				 struct ieee80211_sta *sta,
@@ -2749,8 +2359,11 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 		}
 
 		ret = iwl_mvm_add_sta(mvm, vif, sta);
-		if (sta->tdls && ret == 0)
+		if (sta->tdls && ret == 0) {
 			iwl_mvm_recalc_tdls_state(mvm, vif, true);
+			iwl_mvm_tdls_check_trigger(mvm, vif, sta->addr,
+						   NL80211_TDLS_SETUP);
+		}
 	} else if (old_state == IEEE80211_STA_NONE &&
 		   new_state == IEEE80211_STA_AUTH) {
 		/*
@@ -2762,6 +2375,10 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 		ret = 0;
 	} else if (old_state == IEEE80211_STA_AUTH &&
 		   new_state == IEEE80211_STA_ASSOC) {
+		if (vif->type == NL80211_IFTYPE_AP) {
+			mvmvif->ap_assoc_sta_count++;
+			iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
+		}
 		ret = iwl_mvm_update_sta(mvm, vif, sta);
 		if (ret == 0)
 			iwl_mvm_rs_rate_init(mvm, sta,
@@ -2774,6 +2391,10 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 		if (iwl_mvm_phy_ctx_count(mvm) > 1)
 			iwl_mvm_teardown_tdls_peers(mvm);
 
+		if (sta->tdls)
+			iwl_mvm_tdls_check_trigger(mvm, vif, sta->addr,
+						   NL80211_TDLS_ENABLE_LINK);
+
 		/* enable beacon filtering */
 		WARN_ON(iwl_mvm_enable_beacon_filter(mvm, vif, 0));
 		ret = 0;
@@ -2784,6 +2405,10 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 		ret = 0;
 	} else if (old_state == IEEE80211_STA_ASSOC &&
 		   new_state == IEEE80211_STA_AUTH) {
+		if (vif->type == NL80211_IFTYPE_AP) {
+			mvmvif->ap_assoc_sta_count--;
+			iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
+		}
 		ret = 0;
 	} else if (old_state == IEEE80211_STA_AUTH &&
 		   new_state == IEEE80211_STA_NONE) {
@@ -2791,8 +2416,11 @@ static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 	} else if (old_state == IEEE80211_STA_NONE &&
 		   new_state == IEEE80211_STA_NOTEXIST) {
 		ret = iwl_mvm_rm_sta(mvm, vif, sta);
-		if (sta->tdls)
+		if (sta->tdls) {
 			iwl_mvm_recalc_tdls_state(mvm, vif, false);
+			iwl_mvm_tdls_check_trigger(mvm, vif, sta->addr,
+						   NL80211_TDLS_DISABLE_LINK);
+		}
 	} else {
 		ret = -EIO;
 	}
@@ -2940,6 +2568,9 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 			       struct ieee80211_key_conf *key)
 {
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	struct iwl_mvm_sta *mvmsta;
+	struct iwl_mvm_key_pn *ptk_pn;
+	int keyidx = key->keyidx;
 	int ret;
 	u8 key_offset;
 
@@ -3007,6 +2638,36 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 			break;
 		}
 
+		if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status) &&
+		    sta && iwl_mvm_has_new_rx_api(mvm) &&
+		    key->flags & IEEE80211_KEY_FLAG_PAIRWISE &&
+		    (key->cipher == WLAN_CIPHER_SUITE_CCMP ||
+		     key->cipher == WLAN_CIPHER_SUITE_GCMP)) {
+			struct ieee80211_key_seq seq;
+			int tid, q;
+
+			mvmsta = iwl_mvm_sta_from_mac80211(sta);
+			WARN_ON(rcu_access_pointer(mvmsta->ptk_pn[keyidx]));
+			ptk_pn = kzalloc(sizeof(*ptk_pn) +
+					 mvm->trans->num_rx_queues *
+						sizeof(ptk_pn->q[0]),
+					 GFP_KERNEL);
+			if (!ptk_pn) {
+				ret = -ENOMEM;
+				break;
+			}
+
+			for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++) {
+				ieee80211_get_key_rx_seq(key, tid, &seq);
+				for (q = 0; q < mvm->trans->num_rx_queues; q++)
+					memcpy(ptk_pn->q[q].pn[tid],
+					       seq.ccmp.pn,
+					       IEEE80211_CCMP_PN_LEN);
+			}
+
+			rcu_assign_pointer(mvmsta->ptk_pn[keyidx], ptk_pn);
+		}
+
 		/* in HW restart reuse the index, otherwise request a new one */
 		if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
 			key_offset = key->hw_key_idx;
@@ -3032,6 +2693,19 @@ static int iwl_mvm_mac_set_key(struct ieee80211_hw *hw,
 			break;
 		}
 
+		if (sta && iwl_mvm_has_new_rx_api(mvm) &&
+		    key->flags & IEEE80211_KEY_FLAG_PAIRWISE &&
+		    (key->cipher == WLAN_CIPHER_SUITE_CCMP ||
+		     key->cipher == WLAN_CIPHER_SUITE_GCMP)) {
+			mvmsta = iwl_mvm_sta_from_mac80211(sta);
+			ptk_pn = rcu_dereference_protected(
+						mvmsta->ptk_pn[keyidx],
+						lockdep_is_held(&mvm->mutex));
+			RCU_INIT_POINTER(mvmsta->ptk_pn[keyidx], NULL);
+			if (ptk_pn)
+				kfree_rcu(ptk_pn, rcu_head);
+		}
+
 		IWL_DEBUG_MAC80211(mvm, "disable hwcrypto key\n");
 		ret = iwl_mvm_remove_sta_key(mvm, vif, sta, key);
 		break;
@@ -3092,7 +2766,11 @@ static bool iwl_mvm_rx_aux_roc(struct iwl_notif_wait_data *notif_wait,
 	return true;
 }
 
-#define AUX_ROC_MAX_DELAY_ON_CHANNEL 200
+#define AUX_ROC_MIN_DURATION MSEC_TO_TU(100)
+#define AUX_ROC_MIN_DELAY MSEC_TO_TU(200)
+#define AUX_ROC_MAX_DELAY MSEC_TO_TU(600)
+#define AUX_ROC_SAFETY_BUFFER MSEC_TO_TU(20)
+#define AUX_ROC_MIN_SAFETY_BUFFER MSEC_TO_TU(10)
 static int iwl_mvm_send_aux_roc_cmd(struct iwl_mvm *mvm,
 				    struct ieee80211_channel *channel,
 				    struct ieee80211_vif *vif,
@@ -3103,6 +2781,9 @@ static int iwl_mvm_send_aux_roc_cmd(struct iwl_mvm *mvm,
 	struct iwl_mvm_time_event_data *te_data = &mvmvif->hs_time_event_data;
 	static const u16 time_event_response[] = { HOT_SPOT_CMD };
 	struct iwl_notification_wait wait_time_event;
+	u32 dtim_interval = vif->bss_conf.dtim_period *
+		vif->bss_conf.beacon_int;
+	u32 req_dur, delay;
 	struct iwl_hs20_roc_req aux_roc_req = {
 		.action = cpu_to_le32(FW_CTXT_ACTION_ADD),
 		.id_and_color =
@@ -3115,11 +2796,38 @@ static int iwl_mvm_send_aux_roc_cmd(struct iwl_mvm *mvm,
 		.channel_info.width = PHY_VHT_CHANNEL_MODE20,
 		/* Set the time and duration */
 		.apply_time = cpu_to_le32(iwl_read_prph(mvm->trans, time_reg)),
-		.apply_time_max_delay =
-			cpu_to_le32(MSEC_TO_TU(AUX_ROC_MAX_DELAY_ON_CHANNEL)),
-		.duration = cpu_to_le32(MSEC_TO_TU(duration)),
 	 };
 
+	delay = AUX_ROC_MIN_DELAY;
+	req_dur = MSEC_TO_TU(duration);
+
+	/*
+	 * If we are associated we want the delay time to be at least one
+	 * dtim interval so that the FW can wait until after the DTIM and
+	 * then start the time event, this will potentially allow us to
+	 * remain off-channel for the max duration.
+	 * Since we want to use almost a whole dtim interval we would also
+	 * like the delay to be for 2-3 dtim intervals, in case there are
+	 * other time events with higher priority.
+	 */
+	if (vif->bss_conf.assoc) {
+		delay = min_t(u32, dtim_interval * 3, AUX_ROC_MAX_DELAY);
+		/* We cannot remain off-channel longer than the DTIM interval */
+		if (dtim_interval <= req_dur) {
+			req_dur = dtim_interval - AUX_ROC_SAFETY_BUFFER;
+			if (req_dur <= AUX_ROC_MIN_DURATION)
+				req_dur = dtim_interval -
+					AUX_ROC_MIN_SAFETY_BUFFER;
+		}
+	}
+
+	aux_roc_req.duration = cpu_to_le32(req_dur);
+	aux_roc_req.apply_time_max_delay = cpu_to_le32(delay);
+
+	IWL_DEBUG_TE(mvm,
+		     "ROC: Requesting to remain on channel %u for %ums (requested = %ums, max_delay = %ums, dtim_interval = %ums)\n",
+		     channel->hw_value, req_dur, duration, delay,
+		     dtim_interval);
 	/* Set the node address */
 	memcpy(aux_roc_req.node_addr, vif->addr, ETH_ALEN);
 
@@ -3467,6 +3175,11 @@ static int __iwl_mvm_assign_vif_chanctx(struct iwl_mvm *mvm,
 		ret = iwl_mvm_update_quotas(mvm, false, NULL);
 		if (ret)
 			goto out_remove_binding;
+
+		ret = iwl_mvm_add_snif_sta(mvm, vif);
+		if (ret)
+			goto out_remove_binding;
+
 	}
 
 	/* Handle binding during CSA */
@@ -3540,6 +3253,7 @@ static void __iwl_mvm_unassign_vif_chanctx(struct iwl_mvm *mvm,
 	case NL80211_IFTYPE_MONITOR:
 		mvmvif->monitor_active = false;
 		mvmvif->ps_disabled = false;
+		iwl_mvm_rm_snif_sta(mvm, vif);
 		break;
 	case NL80211_IFTYPE_AP:
 		/* This part is triggered only during CSA */
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
index 4bde2d027dcd..ff7c6df9f941 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/mvm.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -157,7 +157,7 @@ struct iwl_mvm_dump_desc {
 	struct iwl_fw_error_dump_trigger_desc trig_desc;
 };
 
-extern struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert;
+extern const struct iwl_mvm_dump_desc iwl_mvm_dump_desc_assert;
 
 struct iwl_mvm_phy_ctxt {
 	u16 id;
@@ -294,6 +294,7 @@ enum iwl_mvm_ref_type {
 	IWL_MVM_REF_EXIT_WORK,
 	IWL_MVM_REF_PROTECT_CSA,
 	IWL_MVM_REF_FW_DBG_COLLECT,
+	IWL_MVM_REF_INIT_UCODE,
 
 	/* update debugfs.c when changing this */
 
@@ -404,7 +405,7 @@ struct iwl_mvm_vif {
 	 */
 	struct iwl_mvm_phy_ctxt *phy_ctxt;
 
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
 	/* WoWLAN GTK rekey data */
 	struct {
 		u8 kck[NL80211_KCK_LEN], kek[NL80211_KEK_LEN];
@@ -421,6 +422,7 @@ struct iwl_mvm_vif {
 #if IS_ENABLED(CONFIG_IPV6)
 	/* IPv6 addresses for WoWLAN */
 	struct in6_addr target_ipv6_addrs[IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX];
+	unsigned long tentative_addrs[BITS_TO_LONGS(IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_MAX)];
 	int num_target_ipv6_addrs;
 #endif
 
@@ -475,6 +477,14 @@ enum iwl_scan_status {
 	IWL_MVM_SCAN_MASK		= 0xff,
 };
 
+enum iwl_mvm_scan_type {
+	IWL_SCAN_TYPE_NOT_SET,
+	IWL_SCAN_TYPE_UNASSOC,
+	IWL_SCAN_TYPE_WILD,
+	IWL_SCAN_TYPE_MILD,
+	IWL_SCAN_TYPE_FRAGMENTED,
+};
+
 /**
  * struct iwl_nvm_section - describes an NVM section in memory.
  *
@@ -643,10 +653,14 @@ struct iwl_mvm {
 	unsigned int scan_status;
 	void *scan_cmd;
 	struct iwl_mcast_filter_cmd *mcast_filter_cmd;
+	enum iwl_mvm_scan_type scan_type;
 
 	/* max number of simultaneous scans the FW supports */
 	unsigned int max_scans;
 
+	/* ts of the beginning of a non-collect fw dbg data period */
+	unsigned long fw_dbg_non_collect_ts_start[FW_DBG_TRIGGER_MAX - 1];
+
 	/* UMAC scan tracking */
 	u32 scan_uid_status[IWL_MVM_MAX_UMAC_SCANS];
 
@@ -666,6 +680,7 @@ struct iwl_mvm {
 
 	/* Internal station */
 	struct iwl_mvm_int_sta aux_sta;
+	struct iwl_mvm_int_sta snif_sta;
 
 	bool last_ebs_successful;
 
@@ -717,8 +732,8 @@ struct iwl_mvm {
 	s8 restart_fw;
 	u8 fw_dbg_conf;
 	struct delayed_work fw_dump_wk;
-	struct iwl_mvm_dump_desc *fw_dump_desc;
-	struct iwl_fw_dbg_trigger_tlv *fw_dump_trig;
+	const struct iwl_mvm_dump_desc *fw_dump_desc;
+	const struct iwl_fw_dbg_trigger_tlv *fw_dump_trig;
 
 #ifdef CONFIG_IWLWIFI_LEDS
 	struct led_classdev led;
@@ -726,12 +741,11 @@ struct iwl_mvm {
 
 	struct ieee80211_vif *p2p_device_vif;
 
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
 	struct wiphy_wowlan_support wowlan;
 	int gtk_ivlen, gtk_icvlen, ptk_ivlen, ptk_icvlen;
 
 	/* sched scan settings for net detect */
-	struct cfg80211_sched_scan_request *nd_config;
 	struct ieee80211_scan_ies nd_ies;
 	struct cfg80211_match_set *nd_match_sets;
 	int n_nd_match_sets;
@@ -813,8 +827,6 @@ struct iwl_mvm {
 	bool lar_regdom_set;
 	enum iwl_mcc_source mcc_src;
 
-	u8 low_latency_agg_frame_limit;
-
 	/* TDLS channel switch data */
 	struct {
 		struct delayed_work dwork;
@@ -915,11 +927,9 @@ iwl_mvm_sta_from_staid_protected(struct iwl_mvm *mvm, u8 sta_id)
 
 static inline bool iwl_mvm_is_d0i3_supported(struct iwl_mvm *mvm)
 {
-	return mvm->trans->cfg->d0i3 &&
-	       mvm->trans->d0i3_mode != IWL_D0I3_MODE_OFF &&
-	       !iwlwifi_mod_params.d0i3_disable &&
-	       fw_has_capa(&mvm->fw->ucode_capa,
-			   IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
+	return !iwlwifi_mod_params.d0i3_disable &&
+		fw_has_capa(&mvm->fw->ucode_capa,
+			    IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
 }
 
 static inline bool iwl_mvm_is_dqa_supported(struct iwl_mvm *mvm)
@@ -928,6 +938,19 @@ static inline bool iwl_mvm_is_dqa_supported(struct iwl_mvm *mvm)
 			   IWL_UCODE_TLV_CAPA_DQA_SUPPORT);
 }
 
+static inline bool iwl_mvm_enter_d0i3_on_suspend(struct iwl_mvm *mvm)
+{
+	/* For now we only use this mode to differentiate between
+	 * slave transports, which handle D0i3 entry in suspend by
+	 * themselves in conjunction with runtime PM D0i3.  So, this
+	 * function is used to check whether we need to do anything
+	 * when entering suspend or if the transport layer has already
+	 * done it.
+	 */
+	return (mvm->trans->system_pm_mode == IWL_PLAT_PM_MODE_D0I3) &&
+		(mvm->trans->runtime_pm_mode != IWL_PLAT_PM_MODE_D0I3);
+}
+
 static inline bool iwl_mvm_is_lar_supported(struct iwl_mvm *mvm)
 {
 	bool nvm_lar = mvm->nvm_data->lar_enabled;
@@ -975,6 +998,13 @@ static inline bool iwl_mvm_is_csum_supported(struct iwl_mvm *mvm)
 			   IWL_UCODE_TLV_CAPA_CSUM_SUPPORT);
 }
 
+static inline bool iwl_mvm_is_mplut_supported(struct iwl_mvm *mvm)
+{
+	return fw_has_capa(&mvm->fw->ucode_capa,
+			   IWL_UCODE_TLV_CAPA_BT_MPLUT_SUPPORT) &&
+		IWL_MVM_BT_COEX_MPLUT;
+}
+
 static inline bool iwl_mvm_has_new_rx_api(struct iwl_mvm *mvm)
 {
 	/* firmware flag isn't defined yet */
@@ -1108,6 +1138,11 @@ bool iwl_mvm_bcast_filter_build_cmd(struct iwl_mvm *mvm,
 void iwl_mvm_rx_rx_phy_cmd(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb);
 void iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct napi_struct *napi,
 			struct iwl_rx_cmd_buffer *rxb);
+void iwl_mvm_rx_phy_cmd_mq(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb);
+void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
+			struct iwl_rx_cmd_buffer *rxb, int queue);
+void iwl_mvm_rx_frame_release(struct iwl_mvm *mvm,
+			      struct iwl_rx_cmd_buffer *rxb, int queue);
 void iwl_mvm_rx_tx_cmd(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb);
 void iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb);
 void iwl_mvm_rx_ant_coupling_notif(struct iwl_mvm *mvm,
@@ -1190,6 +1225,9 @@ void iwl_mvm_rx_umac_scan_complete_notif(struct iwl_mvm *mvm,
 void iwl_mvm_rx_umac_scan_iter_complete_notif(struct iwl_mvm *mvm,
 					      struct iwl_rx_cmd_buffer *rxb);
 
+/* Paging */
+void iwl_free_fw_paging(struct iwl_mvm *mvm);
+
 /* MVM debugfs */
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir);
@@ -1256,10 +1294,31 @@ void iwl_mvm_ipv6_addr_change(struct ieee80211_hw *hw,
 void iwl_mvm_set_default_unicast_key(struct ieee80211_hw *hw,
 				     struct ieee80211_vif *vif, int idx);
 extern const struct file_operations iwl_dbgfs_d3_test_ops;
-#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
+int iwl_mvm_wowlan_config_key_params(struct iwl_mvm *mvm,
+				     struct ieee80211_vif *vif,
+				     bool host_awake,
+				     u32 cmd_flags);
+void iwl_mvm_d0i3_update_keys(struct iwl_mvm *mvm,
+			      struct ieee80211_vif *vif,
+			      struct iwl_wowlan_status *status);
 void iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm,
 				 struct ieee80211_vif *vif);
 #else
+static inline int iwl_mvm_wowlan_config_key_params(struct iwl_mvm *mvm,
+						   struct ieee80211_vif *vif,
+						   bool host_awake,
+						   u32 cmd_flags)
+{
+	return 0;
+}
+
+static inline void iwl_mvm_d0i3_update_keys(struct iwl_mvm *mvm,
+					    struct ieee80211_vif *vif,
+					    struct iwl_wowlan_status *status)
+{
+}
+
 static inline void
 iwl_mvm_set_last_nonqos_seq(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
@@ -1270,6 +1329,7 @@ void iwl_mvm_set_wowlan_qos_seq(struct iwl_mvm_sta *mvm_ap_sta,
 int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 			       struct ieee80211_vif *vif,
 			       bool disable_offloading,
+			       bool offload_ns,
 			       u32 cmd_flags);
 
 /* D0i3 */
@@ -1376,6 +1436,15 @@ void iwl_mvm_disable_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
 			 u8 tid, u8 flags);
 int iwl_mvm_find_free_queue(struct iwl_mvm *mvm, u8 minq, u8 maxq);
 
+/* Return a bitmask with all the hw supported queues, except for the
+ * command queue, which can't be flushed.
+ */
+static inline u32 iwl_mvm_flushable_queues(struct iwl_mvm *mvm)
+{
+	return ((BIT(mvm->cfg->base_params->num_of_queues) - 1) &
+		~BIT(IWL_MVM_CMD_QUEUE));
+}
+
 static inline
 void iwl_mvm_enable_ac_txq(struct iwl_mvm *mvm, int queue, int mac80211_queue,
 			   u8 fifo, u16 ssn, unsigned int wdg_timeout)
@@ -1468,68 +1537,10 @@ void iwl_mvm_tdls_ch_switch_work(struct work_struct *work);
 struct ieee80211_vif *iwl_mvm_get_bss_vif(struct iwl_mvm *mvm);
 
 void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error);
-void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
-
-int iwl_mvm_start_fw_dbg_conf(struct iwl_mvm *mvm, u8 id);
-int iwl_mvm_fw_dbg_collect(struct iwl_mvm *mvm, enum iwl_fw_dbg_trigger trig,
-			   const char *str, size_t len,
-			   struct iwl_fw_dbg_trigger_tlv *trigger);
-int iwl_mvm_fw_dbg_collect_desc(struct iwl_mvm *mvm,
-				struct iwl_mvm_dump_desc *desc,
-				struct iwl_fw_dbg_trigger_tlv *trigger);
-void iwl_mvm_free_fw_dump_desc(struct iwl_mvm *mvm);
-int iwl_mvm_fw_dbg_collect_trig(struct iwl_mvm *mvm,
-				struct iwl_fw_dbg_trigger_tlv *trigger,
-				const char *fmt, ...) __printf(3, 4);
 unsigned int iwl_mvm_get_wd_timeout(struct iwl_mvm *mvm,
 				    struct ieee80211_vif *vif,
 				    bool tdls, bool cmd_q);
 void iwl_mvm_connection_loss(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			     const char *errmsg);
-static inline bool
-iwl_fw_dbg_trigger_vif_match(struct iwl_fw_dbg_trigger_tlv *trig,
-			     struct ieee80211_vif *vif)
-{
-	u32 trig_vif = le32_to_cpu(trig->vif_type);
-
-	return trig_vif == IWL_FW_DBG_CONF_VIF_ANY || vif->type == trig_vif;
-}
-
-static inline bool
-iwl_fw_dbg_trigger_stop_conf_match(struct iwl_mvm *mvm,
-				   struct iwl_fw_dbg_trigger_tlv *trig)
-{
-	return ((trig->mode & IWL_FW_DBG_TRIGGER_STOP) &&
-		(mvm->fw_dbg_conf == FW_DBG_INVALID ||
-		(BIT(mvm->fw_dbg_conf) & le32_to_cpu(trig->stop_conf_ids))));
-}
-
-static inline bool
-iwl_fw_dbg_trigger_check_stop(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      struct iwl_fw_dbg_trigger_tlv *trig)
-{
-	if (vif && !iwl_fw_dbg_trigger_vif_match(trig, vif))
-		return false;
-
-	return iwl_fw_dbg_trigger_stop_conf_match(mvm, trig);
-}
-
-static inline void
-iwl_fw_dbg_trigger_simple_stop(struct iwl_mvm *mvm,
-			       struct ieee80211_vif *vif,
-			       enum iwl_fw_dbg_trigger trig)
-{
-	struct iwl_fw_dbg_trigger_tlv *trigger;
-
-	if (!iwl_fw_dbg_trigger_enabled(mvm->fw, trig))
-		return;
-
-	trigger = iwl_fw_dbg_get_trigger(mvm->fw, trig);
-	if (!iwl_fw_dbg_trigger_check_stop(mvm, vif, trigger))
-		return;
-
-	iwl_mvm_fw_dbg_collect_trig(mvm, trigger, NULL);
-}
 
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c
index 2ee0f6fe56a1..7a3da2da6fd0 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/nvm.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016        Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -104,13 +106,35 @@ static int iwl_nvm_write_chunk(struct iwl_mvm *mvm, u16 section,
 	struct iwl_host_cmd cmd = {
 		.id = NVM_ACCESS_CMD,
 		.len = { sizeof(struct iwl_nvm_access_cmd), length },
-		.flags = CMD_SEND_IN_RFKILL,
+		.flags = CMD_WANT_SKB | CMD_SEND_IN_RFKILL,
 		.data = { &nvm_access_cmd, data },
 		/* data may come from vmalloc, so use _DUP */
 		.dataflags = { 0, IWL_HCMD_DFL_DUP },
 	};
+	struct iwl_rx_packet *pkt;
+	struct iwl_nvm_access_resp *nvm_resp;
+	int ret;
 
-	return iwl_mvm_send_cmd(mvm, &cmd);
+	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	if (ret)
+		return ret;
+
+	pkt = cmd.resp_pkt;
+	if (!pkt) {
+		IWL_ERR(mvm, "Error in NVM_ACCESS response\n");
+		return -EINVAL;
+	}
+	/* Extract & check NVM write response */
+	nvm_resp = (void *)pkt->data;
+	if (le16_to_cpu(nvm_resp->status) != READ_NVM_CHUNK_SUCCEED) {
+		IWL_ERR(mvm,
+			"NVM access write command failed for section %u (status = 0x%x)\n",
+			section, le16_to_cpu(nvm_resp->status));
+		ret = -EIO;
+	}
+
+	iwl_free_resp(&cmd);
+	return ret;
 }
 
 static int iwl_nvm_read_chunk(struct iwl_mvm *mvm, u16 section,
@@ -210,6 +234,19 @@ static int iwl_nvm_write_section(struct iwl_mvm *mvm, u16 section,
 	return 0;
 }
 
+static void iwl_mvm_nvm_fixups(struct iwl_mvm *mvm, unsigned int section,
+			       u8 *data, unsigned int len)
+{
+#define IWL_4165_DEVICE_ID	0x5501
+#define NVM_SKU_CAP_MIMO_DISABLE BIT(5)
+
+	if (section == NVM_SECTION_TYPE_PHY_SKU &&
+	    mvm->trans->hw_id == IWL_4165_DEVICE_ID && data && len >= 5 &&
+	    (data[4] & NVM_SKU_CAP_MIMO_DISABLE))
+		/* OTP 0x52 bug work around: it's a 1x1 device */
+		data[3] = ANT_B | (ANT_B << 4);
+}
+
 /*
  * Reads an NVM section completely.
  * NICs prior to 7000 family doesn't have a real NVM, but just read
@@ -250,6 +287,8 @@ static int iwl_nvm_read_section(struct iwl_mvm *mvm, u16 section,
 		offset += ret;
 	}
 
+	iwl_mvm_nvm_fixups(mvm, section, data, offset);
+
 	IWL_DEBUG_EEPROM(mvm->trans->dev,
 			 "NVM section %d read completed\n", section);
 	return offset;
@@ -316,8 +355,7 @@ iwl_parse_nvm_sections(struct iwl_mvm *mvm)
 	return iwl_parse_nvm_data(mvm->trans->dev, mvm->cfg, hw, sw, calib,
 				  regulatory, mac_override, phy_sku,
 				  mvm->fw->valid_tx_ant, mvm->fw->valid_rx_ant,
-				  lar_enabled, mac_addr0, mac_addr1,
-				  mvm->trans->hw_id);
+				  lar_enabled, mac_addr0, mac_addr1);
 }
 
 #define MAX_NVM_FILE_LEN	16384
@@ -353,7 +391,8 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 		__le16 word2;
 		u8 data[];
 	} *file_sec;
-	const u8 *eof, *temp;
+	const u8 *eof;
+	u8 *temp;
 	int max_section_size;
 	const __le32 *dword_buff;
 
@@ -483,6 +522,9 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 			ret = -ENOMEM;
 			break;
 		}
+
+		iwl_mvm_nvm_fixups(mvm, section_id, temp, section_size);
+
 		kfree(mvm->nvm_sections[section_id].data);
 		mvm->nvm_sections[section_id].data = temp;
 		mvm->nvm_sections[section_id].length = section_size;
@@ -548,6 +590,9 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 				ret = -ENOMEM;
 				break;
 			}
+
+			iwl_mvm_nvm_fixups(mvm, section, temp, ret);
+
 			mvm->nvm_sections[section].data = temp;
 			mvm->nvm_sections[section].length = ret;
 
@@ -597,7 +642,8 @@ int iwl_nvm_init(struct iwl_mvm *mvm, bool read_nvm_from_nic)
 			else
 				mvm->nvm_file_name = nvm_file_C;
 
-			if (ret == -EFAULT && mvm->nvm_file_name) {
+			if ((ret == -EFAULT || ret == -ENOENT) &&
+			    mvm->nvm_file_name) {
 				/* in case nvm file was failed try again */
 				ret = iwl_mvm_read_external_nvm(mvm);
 				if (ret)
@@ -627,6 +673,7 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 		.source_id = (u8)src_id,
 	};
 	struct iwl_mcc_update_resp *mcc_resp, *resp_cp = NULL;
+	struct iwl_mcc_update_resp_v1 *mcc_resp_v1 = NULL;
 	struct iwl_rx_packet *pkt;
 	struct iwl_host_cmd cmd = {
 		.id = MCC_UPDATE_CMD,
@@ -638,11 +685,15 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 	u32 status;
 	int resp_len, n_channels;
 	u16 mcc;
+	bool resp_v2 = fw_has_capa(&mvm->fw->ucode_capa,
+				   IWL_UCODE_TLV_CAPA_LAR_SUPPORT_V2);
 
 	if (WARN_ON_ONCE(!iwl_mvm_is_lar_supported(mvm)))
 		return ERR_PTR(-EOPNOTSUPP);
 
 	cmd.len[0] = sizeof(struct iwl_mcc_update_cmd);
+	if (!resp_v2)
+		cmd.len[0] = sizeof(struct iwl_mcc_update_cmd_v1);
 
 	IWL_DEBUG_LAR(mvm, "send MCC update to FW with '%c%c' src = %d\n",
 		      alpha2[0], alpha2[1], src_id);
@@ -654,31 +705,50 @@ iwl_mvm_update_mcc(struct iwl_mvm *mvm, const char *alpha2,
 	pkt = cmd.resp_pkt;
 
 	/* Extract MCC response */
-	mcc_resp = (void *)pkt->data;
-	status = le32_to_cpu(mcc_resp->status);
+	if (resp_v2) {
+		mcc_resp = (void *)pkt->data;
+		n_channels =  __le32_to_cpu(mcc_resp->n_channels);
+	} else {
+		mcc_resp_v1 = (void *)pkt->data;
+		n_channels =  __le32_to_cpu(mcc_resp_v1->n_channels);
+	}
+
+	resp_len = sizeof(struct iwl_mcc_update_resp) + n_channels *
+		sizeof(__le32);
+
+	resp_cp = kzalloc(resp_len, GFP_KERNEL);
+	if (!resp_cp) {
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	if (resp_v2) {
+		memcpy(resp_cp, mcc_resp, resp_len);
+	} else {
+		resp_cp->status = mcc_resp_v1->status;
+		resp_cp->mcc = mcc_resp_v1->mcc;
+		resp_cp->cap = mcc_resp_v1->cap;
+		resp_cp->source_id = mcc_resp_v1->source_id;
+		resp_cp->n_channels = mcc_resp_v1->n_channels;
+		memcpy(resp_cp->channels, mcc_resp_v1->channels,
+		       n_channels * sizeof(__le32));
+	}
+
+	status = le32_to_cpu(resp_cp->status);
 
-	mcc = le16_to_cpu(mcc_resp->mcc);
+	mcc = le16_to_cpu(resp_cp->mcc);
 
 	/* W/A for a FW/NVM issue - returns 0x00 for the world domain */
 	if (mcc == 0) {
 		mcc = 0x3030;  /* "00" - world */
-		mcc_resp->mcc = cpu_to_le16(mcc);
+		resp_cp->mcc = cpu_to_le16(mcc);
 	}
 
-	n_channels =  __le32_to_cpu(mcc_resp->n_channels);
 	IWL_DEBUG_LAR(mvm,
 		      "MCC response status: 0x%x. new MCC: 0x%x ('%c%c') change: %d n_chans: %d\n",
 		      status, mcc, mcc >> 8, mcc & 0xff,
 		      !!(status == MCC_RESP_NEW_CHAN_PROFILE), n_channels);
 
-	resp_len = sizeof(*mcc_resp) + n_channels * sizeof(__le32);
-	resp_cp = kmemdup(mcc_resp, resp_len, GFP_KERNEL);
-	if (!resp_cp) {
-		ret = -ENOMEM;
-		goto exit;
-	}
-
-	ret = 0;
 exit:
 	iwl_free_resp(&cmd);
 	if (ret)
diff --git a/drivers/net/wireless/iwlwifi/mvm/offloading.c b/drivers/net/wireless/intel/iwlwifi/mvm/offloading.c
index 68b0169c8892..6338d9cf7070 100644
--- a/drivers/net/wireless/iwlwifi/mvm/offloading.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/offloading.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -64,6 +66,7 @@
  *****************************************************************************/
 #include <net/ipv6.h>
 #include <net/addrconf.h>
+#include <linux/bitops.h>
 #include "mvm.h"
 
 void iwl_mvm_set_wowlan_qos_seq(struct iwl_mvm_sta *mvm_ap_sta,
@@ -86,6 +89,7 @@ void iwl_mvm_set_wowlan_qos_seq(struct iwl_mvm_sta *mvm_ap_sta,
 int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 			       struct ieee80211_vif *vif,
 			       bool disable_offloading,
+			       bool offload_ns,
 			       u32 cmd_flags)
 {
 	union {
@@ -106,6 +110,13 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 #if IS_ENABLED(CONFIG_IPV6)
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	int i;
+	/*
+	 * Skip tentative address when ns offload is enabled to avoid
+	 * violating RFC4862.
+	 * Keep tentative address when ns offload is disabled so the NS packets
+	 * will not be filtered out and will wake up the host.
+	 */
+	bool skip_tentative = offload_ns;
 
 	if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL ||
 	    capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_LARGE) {
@@ -113,6 +124,7 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 		struct iwl_targ_addr *addrs;
 		int n_nsc, n_addrs;
 		int c;
+		int num_skipped = 0;
 
 		if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL) {
 			nsc = cmd.v3s.ns_config;
@@ -126,9 +138,6 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 			n_addrs = IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V3L;
 		}
 
-		if (mvmvif->num_target_ipv6_addrs)
-			enabled |= IWL_D3_PROTO_OFFLOAD_NS;
-
 		/*
 		 * For each address we have (and that will fit) fill a target
 		 * address struct and combine for NS offload structs with the
@@ -140,6 +149,12 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 			struct in6_addr solicited_addr;
 			int j;
 
+			if (skip_tentative &&
+			    test_bit(i, mvmvif->tentative_addrs)) {
+				num_skipped++;
+				continue;
+			}
+
 			addrconf_addr_solict_mult(&mvmvif->target_ipv6_addrs[i],
 						  &solicited_addr);
 			for (j = 0; j < c; j++)
@@ -154,41 +169,64 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 			memcpy(nsc[j].target_mac_addr, vif->addr, ETH_ALEN);
 		}
 
+		if (mvmvif->num_target_ipv6_addrs - num_skipped)
+			enabled |= IWL_D3_PROTO_IPV6_VALID;
+
 		if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL)
-			cmd.v3s.num_valid_ipv6_addrs = cpu_to_le32(i);
+			cmd.v3s.num_valid_ipv6_addrs =
+				cpu_to_le32(i - num_skipped);
 		else
-			cmd.v3l.num_valid_ipv6_addrs = cpu_to_le32(i);
+			cmd.v3l.num_valid_ipv6_addrs =
+				cpu_to_le32(i - num_skipped);
 	} else if (capa_flags & IWL_UCODE_TLV_FLAGS_D3_6_IPV6_ADDRS) {
-		if (mvmvif->num_target_ipv6_addrs) {
-			enabled |= IWL_D3_PROTO_OFFLOAD_NS;
-			memcpy(cmd.v2.ndp_mac_addr, vif->addr, ETH_ALEN);
-		}
+		bool found = false;
 
 		BUILD_BUG_ON(sizeof(cmd.v2.target_ipv6_addr[0]) !=
 			     sizeof(mvmvif->target_ipv6_addrs[0]));
 
 		for (i = 0; i < min(mvmvif->num_target_ipv6_addrs,
-				    IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V2); i++)
+				    IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V2); i++) {
+			if (skip_tentative &&
+			    test_bit(i, mvmvif->tentative_addrs))
+				continue;
+
 			memcpy(cmd.v2.target_ipv6_addr[i],
 			       &mvmvif->target_ipv6_addrs[i],
 			       sizeof(cmd.v2.target_ipv6_addr[i]));
-	} else {
-		if (mvmvif->num_target_ipv6_addrs) {
-			enabled |= IWL_D3_PROTO_OFFLOAD_NS;
-			memcpy(cmd.v1.ndp_mac_addr, vif->addr, ETH_ALEN);
-		}
 
+			found = true;
+		}
+		if (found) {
+			enabled |= IWL_D3_PROTO_IPV6_VALID;
+			memcpy(cmd.v2.ndp_mac_addr, vif->addr, ETH_ALEN);
+		}
+	} else {
+		bool found = false;
 		BUILD_BUG_ON(sizeof(cmd.v1.target_ipv6_addr[0]) !=
 			     sizeof(mvmvif->target_ipv6_addrs[0]));
 
 		for (i = 0; i < min(mvmvif->num_target_ipv6_addrs,
-				    IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V1); i++)
+				    IWL_PROTO_OFFLOAD_NUM_IPV6_ADDRS_V1); i++) {
+			if (skip_tentative &&
+			    test_bit(i, mvmvif->tentative_addrs))
+				continue;
+
 			memcpy(cmd.v1.target_ipv6_addr[i],
 			       &mvmvif->target_ipv6_addrs[i],
 			       sizeof(cmd.v1.target_ipv6_addr[i]));
+
+			found = true;
+		}
+
+		if (found) {
+			enabled |= IWL_D3_PROTO_IPV6_VALID;
+			memcpy(cmd.v1.ndp_mac_addr, vif->addr, ETH_ALEN);
+		}
 	}
-#endif
 
+	if (offload_ns && (enabled & IWL_D3_PROTO_IPV6_VALID))
+		enabled |= IWL_D3_PROTO_OFFLOAD_NS;
+#endif
 	if (capa_flags & IWL_UCODE_TLV_FLAGS_NEW_NSOFFL_SMALL) {
 		common = &cmd.v3s.common;
 		size = sizeof(cmd.v3s);
@@ -204,7 +242,7 @@ int iwl_mvm_send_proto_offload(struct iwl_mvm *mvm,
 	}
 
 	if (vif->bss_conf.arp_addr_cnt) {
-		enabled |= IWL_D3_PROTO_OFFLOAD_ARP;
+		enabled |= IWL_D3_PROTO_OFFLOAD_ARP | IWL_D3_PROTO_IPV4_VALID;
 		common->host_ipv4_addr = vif->bss_conf.arp_addr_list[0];
 		memcpy(common->arp_mac_addr, vif->addr, ETH_ALEN);
 	}
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/intel/iwlwifi/mvm/ops.c
index 13c97f665ba8..e80be9a59520 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/ops.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -82,6 +82,9 @@
 #include "rs.h"
 #include "fw-api-scan.h"
 #include "time-event.h"
+#include "fw-dbg.h"
+#include "fw-api.h"
+#include "fw-api-scan.h"
 
 #define DRV_DESCRIPTION	"The new Intel(R) wireless AGN driver for Linux"
 MODULE_DESCRIPTION(DRV_DESCRIPTION);
@@ -268,102 +271,127 @@ static const struct iwl_rx_handlers iwl_mvm_rx_handlers[] = {
 };
 #undef RX_HANDLER
 #undef RX_HANDLER_GRP
-#define CMD(x) [x] = #x
-
-static const char *const iwl_mvm_cmd_strings[REPLY_MAX + 1] = {
-	CMD(MVM_ALIVE),
-	CMD(REPLY_ERROR),
-	CMD(ECHO_CMD),
-	CMD(INIT_COMPLETE_NOTIF),
-	CMD(PHY_CONTEXT_CMD),
-	CMD(MGMT_MCAST_KEY),
-	CMD(TX_CMD),
-	CMD(TXPATH_FLUSH),
-	CMD(SHARED_MEM_CFG),
-	CMD(MAC_CONTEXT_CMD),
-	CMD(TIME_EVENT_CMD),
-	CMD(TIME_EVENT_NOTIFICATION),
-	CMD(BINDING_CONTEXT_CMD),
-	CMD(TIME_QUOTA_CMD),
-	CMD(NON_QOS_TX_COUNTER_CMD),
-	CMD(DC2DC_CONFIG_CMD),
-	CMD(NVM_ACCESS_CMD),
-	CMD(PHY_CONFIGURATION_CMD),
-	CMD(CALIB_RES_NOTIF_PHY_DB),
-	CMD(SET_CALIB_DEFAULT_CMD),
-	CMD(FW_PAGING_BLOCK_CMD),
-	CMD(ADD_STA_KEY),
-	CMD(ADD_STA),
-	CMD(FW_GET_ITEM_CMD),
-	CMD(REMOVE_STA),
-	CMD(LQ_CMD),
-	CMD(SCAN_OFFLOAD_CONFIG_CMD),
-	CMD(MATCH_FOUND_NOTIFICATION),
-	CMD(SCAN_OFFLOAD_REQUEST_CMD),
-	CMD(SCAN_OFFLOAD_ABORT_CMD),
-	CMD(HOT_SPOT_CMD),
-	CMD(SCAN_OFFLOAD_COMPLETE),
-	CMD(SCAN_OFFLOAD_UPDATE_PROFILES_CMD),
-	CMD(SCAN_ITERATION_COMPLETE),
-	CMD(POWER_TABLE_CMD),
-	CMD(WEP_KEY),
-	CMD(REPLY_RX_PHY_CMD),
-	CMD(REPLY_RX_MPDU_CMD),
-	CMD(BEACON_NOTIFICATION),
-	CMD(BEACON_TEMPLATE_CMD),
-	CMD(STATISTICS_CMD),
-	CMD(STATISTICS_NOTIFICATION),
-	CMD(EOSP_NOTIFICATION),
-	CMD(REDUCE_TX_POWER_CMD),
-	CMD(TX_ANT_CONFIGURATION_CMD),
-	CMD(D3_CONFIG_CMD),
-	CMD(D0I3_END_CMD),
-	CMD(PROT_OFFLOAD_CONFIG_CMD),
-	CMD(OFFLOADS_QUERY_CMD),
-	CMD(REMOTE_WAKE_CONFIG_CMD),
-	CMD(WOWLAN_PATTERNS),
-	CMD(WOWLAN_CONFIGURATION),
-	CMD(WOWLAN_TSC_RSC_PARAM),
-	CMD(WOWLAN_TKIP_PARAM),
-	CMD(WOWLAN_KEK_KCK_MATERIAL),
-	CMD(WOWLAN_GET_STATUSES),
-	CMD(WOWLAN_TX_POWER_PER_DB),
-	CMD(SCAN_OFFLOAD_PROFILES_QUERY_CMD),
-	CMD(SCAN_OFFLOAD_HOTSPOTS_CONFIG_CMD),
-	CMD(SCAN_OFFLOAD_HOTSPOTS_QUERY_CMD),
-	CMD(CARD_STATE_NOTIFICATION),
-	CMD(MISSED_BEACONS_NOTIFICATION),
-	CMD(BT_COEX_PRIO_TABLE),
-	CMD(BT_COEX_PROT_ENV),
-	CMD(BT_PROFILE_NOTIFICATION),
-	CMD(BT_CONFIG),
-	CMD(MCAST_FILTER_CMD),
-	CMD(BCAST_FILTER_CMD),
-	CMD(REPLY_SF_CFG_CMD),
-	CMD(REPLY_BEACON_FILTERING_CMD),
-	CMD(CMD_DTS_MEASUREMENT_TRIGGER),
-	CMD(DTS_MEASUREMENT_NOTIFICATION),
-	CMD(REPLY_THERMAL_MNG_BACKOFF),
-	CMD(MAC_PM_POWER_TABLE),
-	CMD(LTR_CONFIG),
-	CMD(BT_COEX_CI),
-	CMD(BT_COEX_UPDATE_SW_BOOST),
-	CMD(BT_COEX_UPDATE_CORUN_LUT),
-	CMD(BT_COEX_UPDATE_REDUCED_TXP),
-	CMD(PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION),
-	CMD(ANTENNA_COUPLING_NOTIFICATION),
-	CMD(SCD_QUEUE_CFG),
-	CMD(SCAN_CFG_CMD),
-	CMD(SCAN_REQ_UMAC),
-	CMD(SCAN_ABORT_UMAC),
-	CMD(SCAN_COMPLETE_UMAC),
-	CMD(TDLS_CHANNEL_SWITCH_CMD),
-	CMD(TDLS_CHANNEL_SWITCH_NOTIFICATION),
-	CMD(TDLS_CONFIG_CMD),
-	CMD(MCC_UPDATE_CMD),
-	CMD(SCAN_ITERATION_COMPLETE_UMAC),
+
+/* Please keep this array *SORTED* by hex value.
+ * Access is done through binary search
+ */
+static const struct iwl_hcmd_names iwl_mvm_legacy_names[] = {
+	HCMD_NAME(MVM_ALIVE),
+	HCMD_NAME(REPLY_ERROR),
+	HCMD_NAME(ECHO_CMD),
+	HCMD_NAME(INIT_COMPLETE_NOTIF),
+	HCMD_NAME(PHY_CONTEXT_CMD),
+	HCMD_NAME(DBG_CFG),
+	HCMD_NAME(ANTENNA_COUPLING_NOTIFICATION),
+	HCMD_NAME(SCAN_CFG_CMD),
+	HCMD_NAME(SCAN_REQ_UMAC),
+	HCMD_NAME(SCAN_ABORT_UMAC),
+	HCMD_NAME(SCAN_COMPLETE_UMAC),
+	HCMD_NAME(TOF_CMD),
+	HCMD_NAME(TOF_NOTIFICATION),
+	HCMD_NAME(ADD_STA_KEY),
+	HCMD_NAME(ADD_STA),
+	HCMD_NAME(REMOVE_STA),
+	HCMD_NAME(FW_GET_ITEM_CMD),
+	HCMD_NAME(TX_CMD),
+	HCMD_NAME(SCD_QUEUE_CFG),
+	HCMD_NAME(TXPATH_FLUSH),
+	HCMD_NAME(MGMT_MCAST_KEY),
+	HCMD_NAME(WEP_KEY),
+	HCMD_NAME(SHARED_MEM_CFG),
+	HCMD_NAME(TDLS_CHANNEL_SWITCH_CMD),
+	HCMD_NAME(MAC_CONTEXT_CMD),
+	HCMD_NAME(TIME_EVENT_CMD),
+	HCMD_NAME(TIME_EVENT_NOTIFICATION),
+	HCMD_NAME(BINDING_CONTEXT_CMD),
+	HCMD_NAME(TIME_QUOTA_CMD),
+	HCMD_NAME(NON_QOS_TX_COUNTER_CMD),
+	HCMD_NAME(LQ_CMD),
+	HCMD_NAME(FW_PAGING_BLOCK_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_REQUEST_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_ABORT_CMD),
+	HCMD_NAME(HOT_SPOT_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_PROFILES_QUERY_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_HOTSPOTS_CONFIG_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_HOTSPOTS_QUERY_CMD),
+	HCMD_NAME(BT_COEX_UPDATE_SW_BOOST),
+	HCMD_NAME(BT_COEX_UPDATE_CORUN_LUT),
+	HCMD_NAME(BT_COEX_UPDATE_REDUCED_TXP),
+	HCMD_NAME(BT_COEX_CI),
+	HCMD_NAME(PHY_CONFIGURATION_CMD),
+	HCMD_NAME(CALIB_RES_NOTIF_PHY_DB),
+	HCMD_NAME(SCAN_OFFLOAD_COMPLETE),
+	HCMD_NAME(SCAN_OFFLOAD_UPDATE_PROFILES_CMD),
+	HCMD_NAME(SCAN_OFFLOAD_CONFIG_CMD),
+	HCMD_NAME(POWER_TABLE_CMD),
+	HCMD_NAME(PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION),
+	HCMD_NAME(REPLY_THERMAL_MNG_BACKOFF),
+	HCMD_NAME(DC2DC_CONFIG_CMD),
+	HCMD_NAME(NVM_ACCESS_CMD),
+	HCMD_NAME(SET_CALIB_DEFAULT_CMD),
+	HCMD_NAME(BEACON_NOTIFICATION),
+	HCMD_NAME(BEACON_TEMPLATE_CMD),
+	HCMD_NAME(TX_ANT_CONFIGURATION_CMD),
+	HCMD_NAME(BT_CONFIG),
+	HCMD_NAME(STATISTICS_CMD),
+	HCMD_NAME(STATISTICS_NOTIFICATION),
+	HCMD_NAME(EOSP_NOTIFICATION),
+	HCMD_NAME(REDUCE_TX_POWER_CMD),
+	HCMD_NAME(CARD_STATE_CMD),
+	HCMD_NAME(CARD_STATE_NOTIFICATION),
+	HCMD_NAME(MISSED_BEACONS_NOTIFICATION),
+	HCMD_NAME(TDLS_CONFIG_CMD),
+	HCMD_NAME(MAC_PM_POWER_TABLE),
+	HCMD_NAME(TDLS_CHANNEL_SWITCH_NOTIFICATION),
+	HCMD_NAME(MFUART_LOAD_NOTIFICATION),
+	HCMD_NAME(SCAN_ITERATION_COMPLETE_UMAC),
+	HCMD_NAME(REPLY_RX_PHY_CMD),
+	HCMD_NAME(REPLY_RX_MPDU_CMD),
+	HCMD_NAME(BA_NOTIF),
+	HCMD_NAME(MCC_UPDATE_CMD),
+	HCMD_NAME(MCC_CHUB_UPDATE_CMD),
+	HCMD_NAME(MARKER_CMD),
+	HCMD_NAME(BT_COEX_PRIO_TABLE),
+	HCMD_NAME(BT_COEX_PROT_ENV),
+	HCMD_NAME(BT_PROFILE_NOTIFICATION),
+	HCMD_NAME(BCAST_FILTER_CMD),
+	HCMD_NAME(MCAST_FILTER_CMD),
+	HCMD_NAME(REPLY_SF_CFG_CMD),
+	HCMD_NAME(REPLY_BEACON_FILTERING_CMD),
+	HCMD_NAME(D3_CONFIG_CMD),
+	HCMD_NAME(PROT_OFFLOAD_CONFIG_CMD),
+	HCMD_NAME(OFFLOADS_QUERY_CMD),
+	HCMD_NAME(REMOTE_WAKE_CONFIG_CMD),
+	HCMD_NAME(MATCH_FOUND_NOTIFICATION),
+	HCMD_NAME(CMD_DTS_MEASUREMENT_TRIGGER),
+	HCMD_NAME(DTS_MEASUREMENT_NOTIFICATION),
+	HCMD_NAME(WOWLAN_PATTERNS),
+	HCMD_NAME(WOWLAN_CONFIGURATION),
+	HCMD_NAME(WOWLAN_TSC_RSC_PARAM),
+	HCMD_NAME(WOWLAN_TKIP_PARAM),
+	HCMD_NAME(WOWLAN_KEK_KCK_MATERIAL),
+	HCMD_NAME(WOWLAN_GET_STATUSES),
+	HCMD_NAME(WOWLAN_TX_POWER_PER_DB),
+	HCMD_NAME(SCAN_ITERATION_COMPLETE),
+	HCMD_NAME(D0I3_END_CMD),
+	HCMD_NAME(LTR_CONFIG),
+	HCMD_NAME(REPLY_DEBUG_CMD),
+};
+
+/* Please keep this array *SORTED* by hex value.
+ * Access is done through binary search
+ */
+static const struct iwl_hcmd_names iwl_mvm_phy_names[] = {
+	HCMD_NAME(CMD_DTS_MEASUREMENT_TRIGGER_WIDE),
+	HCMD_NAME(DTS_MEASUREMENT_NOTIF_WIDE),
 };
-#undef CMD
+
+static const struct iwl_hcmd_arr iwl_mvm_groups[] = {
+	[LEGACY_GROUP] = HCMD_ARR(iwl_mvm_legacy_names),
+	[LONG_GROUP] = HCMD_ARR(iwl_mvm_legacy_names),
+	[PHY_OPS_GROUP] = HCMD_ARR(iwl_mvm_phy_names),
+};
+
 
 /* this forward declaration can avoid to export the function */
 static void iwl_mvm_async_handlers_wk(struct work_struct *wk);
@@ -452,7 +480,6 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		mvm->first_agg_queue = 12;
 	}
 	mvm->sf_state = SF_UNINIT;
-	mvm->low_latency_agg_frame_limit = 6;
 	mvm->cur_ucode = IWL_UCODE_INIT;
 
 	mutex_init(&mvm->mutex);
@@ -485,20 +512,36 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	trans_cfg.op_mode = op_mode;
 	trans_cfg.no_reclaim_cmds = no_reclaim_cmds;
 	trans_cfg.n_no_reclaim_cmds = ARRAY_SIZE(no_reclaim_cmds);
-	trans_cfg.rx_buf_size_8k = iwlwifi_mod_params.amsdu_size_8K;
+	switch (iwlwifi_mod_params.amsdu_size) {
+	case IWL_AMSDU_4K:
+		trans_cfg.rx_buf_size = IWL_AMSDU_4K;
+		break;
+	case IWL_AMSDU_8K:
+		trans_cfg.rx_buf_size = IWL_AMSDU_8K;
+		break;
+	case IWL_AMSDU_12K:
+		trans_cfg.rx_buf_size = IWL_AMSDU_12K;
+		break;
+	default:
+		pr_err("%s: Unsupported amsdu_size: %d\n", KBUILD_MODNAME,
+		       iwlwifi_mod_params.amsdu_size);
+		trans_cfg.rx_buf_size = IWL_AMSDU_4K;
+	}
 	trans_cfg.wide_cmd_header = fw_has_api(&mvm->fw->ucode_capa,
 					       IWL_UCODE_TLV_API_WIDE_CMD_HDR);
 
 	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_DW_BC_TABLE)
 		trans_cfg.bc_table_dword = true;
 
-	trans_cfg.command_names = iwl_mvm_cmd_strings;
+	trans_cfg.command_groups = iwl_mvm_groups;
+	trans_cfg.command_groups_size = ARRAY_SIZE(iwl_mvm_groups);
 
 	trans_cfg.cmd_queue = IWL_MVM_CMD_QUEUE;
 	trans_cfg.cmd_fifo = IWL_MVM_TX_FIFO_CMD;
 	trans_cfg.scd_set_active = true;
 
 	trans_cfg.sdio_adma_addr = fw->sdio_adma_addr;
+	trans_cfg.sw_csum_tx = IWL_MVM_SW_TX_CSUM_OFFLOAD;
 
 	/* Set a short watchdog for the command queue */
 	trans_cfg.cmd_q_wdg_timeout =
@@ -561,9 +604,11 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 			goto out_free;
 
 		mutex_lock(&mvm->mutex);
+		iwl_mvm_ref(mvm, IWL_MVM_REF_INIT_UCODE);
 		err = iwl_run_init_mvm_ucode(mvm, true);
 		if (!err || !iwlmvm_mod_params.init_dbg)
 			iwl_trans_stop_device(trans);
+		iwl_mvm_unref(mvm, IWL_MVM_REF_INIT_UCODE);
 		mutex_unlock(&mvm->mutex);
 		/* returns 0 if successful, 1 if success but in rfkill */
 		if (err < 0 && !iwlmvm_mod_params.init_dbg) {
@@ -591,8 +636,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 
 	memset(&mvm->rx_stats, 0, sizeof(struct mvm_statistics_rx));
 
-	/* rpm starts with a taken ref. only set the appropriate bit here. */
-	mvm->refs[IWL_MVM_REF_UCODE_DOWN] = 1;
+	/* rpm starts with a taken reference, we can release it now */
+	iwl_trans_unref(mvm->trans);
 
 	iwl_mvm_tof_init(mvm);
 
@@ -628,12 +673,6 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 
 #if defined(CONFIG_PM_SLEEP) && defined(CONFIG_IWLWIFI_DEBUGFS)
 	kfree(mvm->d3_resume_sram);
-	if (mvm->nd_config) {
-		kfree(mvm->nd_config->match_sets);
-		kfree(mvm->nd_config->scan_plans);
-		kfree(mvm->nd_config);
-		mvm->nd_config = NULL;
-	}
 #endif
 
 	iwl_trans_op_mode_leave(mvm->trans);
@@ -645,6 +684,8 @@ static void iwl_op_mode_mvm_stop(struct iwl_op_mode *op_mode)
 	for (i = 0; i < NVM_MAX_NUM_SECTIONS; i++)
 		kfree(mvm->nvm_sections[i].data);
 
+	iwl_free_fw_paging(mvm);
+
 	iwl_mvm_tof_clean(mvm);
 
 	ieee80211_free_hw(mvm->hw);
@@ -783,6 +824,8 @@ static void iwl_mvm_rx(struct iwl_op_mode *op_mode,
 
 	if (likely(pkt->hdr.cmd == REPLY_RX_MPDU_CMD))
 		iwl_mvm_rx_rx_mpdu(mvm, napi, rxb);
+	else if (pkt->hdr.cmd == FRAME_RELEASE)
+		iwl_mvm_rx_frame_release(mvm, rxb, 0);
 	else if (pkt->hdr.cmd == REPLY_RX_PHY_CMD)
 		iwl_mvm_rx_rx_phy_cmd(mvm, rxb);
 	else
@@ -797,9 +840,9 @@ static void iwl_mvm_rx_mq(struct iwl_op_mode *op_mode,
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
 
 	if (likely(pkt->hdr.cmd == REPLY_RX_MPDU_CMD))
-		iwl_mvm_rx_rx_mpdu(mvm, napi, rxb);
+		iwl_mvm_rx_mpdu_mq(mvm, napi, rxb, 0);
 	else if (pkt->hdr.cmd == REPLY_RX_PHY_CMD)
-		iwl_mvm_rx_rx_phy_cmd(mvm, rxb);
+		iwl_mvm_rx_phy_cmd_mq(mvm, rxb);
 	else
 		iwl_mvm_rx_common(mvm, rxb, pkt);
 }
@@ -829,6 +872,18 @@ static void iwl_mvm_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 	}
 }
 
+static void iwl_mvm_async_cb(struct iwl_op_mode *op_mode,
+			     const struct iwl_device_cmd *cmd)
+{
+	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
+
+	/*
+	 * For now, we only set the CMD_WANT_ASYNC_CALLBACK for ADD_STA
+	 * commands that need to block the Tx queues.
+	 */
+	iwl_trans_block_txq_ptrs(mvm->trans, false);
+}
+
 static void iwl_mvm_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
 {
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
@@ -1023,6 +1078,7 @@ static void iwl_mvm_cmd_queue_full(struct iwl_op_mode *op_mode)
 
 struct iwl_d0i3_iter_data {
 	struct iwl_mvm *mvm;
+	struct ieee80211_vif *connected_vif;
 	u8 ap_sta_id;
 	u8 vif_count;
 	u8 offloading_tid;
@@ -1103,7 +1159,8 @@ static void iwl_mvm_enter_d0i3_iterator(void *_data, u8 *mac,
 		data->disable_offloading = true;
 
 	iwl_mvm_update_d0i3_power_mode(mvm, vif, true, flags);
-	iwl_mvm_send_proto_offload(mvm, vif, data->disable_offloading, flags);
+	iwl_mvm_send_proto_offload(mvm, vif, data->disable_offloading,
+				   false, flags);
 
 	/*
 	 * on init/association, mvm already configures POWER_TABLE_CMD
@@ -1113,6 +1170,12 @@ static void iwl_mvm_enter_d0i3_iterator(void *_data, u8 *mac,
 	 */
 	data->ap_sta_id = mvmvif->ap_sta_id;
 	data->vif_count++;
+
+	/*
+	 * no new commands can be sent at this stage, so it's safe
+	 * to save the vif pointer during d0i3 entrance.
+	 */
+	data->connected_vif = vif;
 }
 
 static void iwl_mvm_set_wowlan_data(struct iwl_mvm *mvm,
@@ -1134,7 +1197,8 @@ static void iwl_mvm_set_wowlan_data(struct iwl_mvm *mvm,
 	mvm_ap_sta = iwl_mvm_sta_from_mac80211(ap_sta);
 	cmd->is_11n_connection = ap_sta->ht_cap.ht_supported;
 	cmd->offloading_tid = iter_data->offloading_tid;
-
+	cmd->flags = ENABLE_L3_FILTERING | ENABLE_NBNS_FILTERING |
+		ENABLE_DHCP_FILTERING;
 	/*
 	 * The d0i3 uCode takes care of the nonqos counters,
 	 * so configure only the qos seq ones.
@@ -1165,6 +1229,9 @@ int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
 
 	IWL_DEBUG_RPM(mvm, "MVM entering D0i3\n");
 
+	if (WARN_ON_ONCE(mvm->cur_ucode != IWL_UCODE_REGULAR))
+		return -EINVAL;
+
 	set_bit(IWL_MVM_STATUS_IN_D0I3, &mvm->status);
 
 	/*
@@ -1196,8 +1263,17 @@ int iwl_mvm_enter_d0i3(struct iwl_op_mode *op_mode)
 	/* make sure we have no running tx while configuring the seqno */
 	synchronize_net();
 
+	/* Flush the hw queues, in case something got queued during entry */
+	ret = iwl_mvm_flush_tx_path(mvm, iwl_mvm_flushable_queues(mvm), flags);
+	if (ret)
+		return ret;
+
 	/* configure wowlan configuration only if needed */
 	if (mvm->d0i3_ap_sta_id != IWL_MVM_STATION_COUNT) {
+		iwl_mvm_wowlan_config_key_params(mvm,
+						 d0i3_iter_data.connected_vif,
+						 true, flags);
+
 		iwl_mvm_set_wowlan_data(mvm, &wowlan_config_cmd,
 					&d0i3_iter_data);
 
@@ -1227,25 +1303,30 @@ static void iwl_mvm_exit_d0i3_iterator(void *_data, u8 *mac,
 	iwl_mvm_update_d0i3_power_mode(mvm, vif, false, flags);
 }
 
-struct iwl_mvm_wakeup_reason_iter_data {
+struct iwl_mvm_d0i3_exit_work_iter_data {
 	struct iwl_mvm *mvm;
+	struct iwl_wowlan_status *status;
 	u32 wakeup_reasons;
 };
 
-static void iwl_mvm_d0i3_wakeup_reason_iter(void *_data, u8 *mac,
-					    struct ieee80211_vif *vif)
+static void iwl_mvm_d0i3_exit_work_iter(void *_data, u8 *mac,
+					struct ieee80211_vif *vif)
 {
-	struct iwl_mvm_wakeup_reason_iter_data *data = _data;
+	struct iwl_mvm_d0i3_exit_work_iter_data *data = _data;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	u32 reasons = data->wakeup_reasons;
 
-	if (vif->type == NL80211_IFTYPE_STATION && vif->bss_conf.assoc &&
-	    data->mvm->d0i3_ap_sta_id == mvmvif->ap_sta_id) {
-		if (data->wakeup_reasons &
-		    IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH)
-			iwl_mvm_connection_loss(data->mvm, vif, "D0i3");
-		else
-			ieee80211_beacon_loss(vif);
-	}
+	/* consider only the relevant station interface */
+	if (vif->type != NL80211_IFTYPE_STATION || !vif->bss_conf.assoc ||
+	    data->mvm->d0i3_ap_sta_id != mvmvif->ap_sta_id)
+		return;
+
+	if (reasons & IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH)
+		iwl_mvm_connection_loss(data->mvm, vif, "D0i3");
+	else if (reasons & IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_MISSED_BEACON)
+		ieee80211_beacon_loss(vif);
+	else
+		iwl_mvm_d0i3_update_keys(data->mvm, vif, data->status);
 }
 
 void iwl_mvm_d0i3_enable_tx(struct iwl_mvm *mvm, __le16 *qos_seq)
@@ -1311,9 +1392,13 @@ static void iwl_mvm_d0i3_exit_work(struct work_struct *wk)
 		.id = WOWLAN_GET_STATUSES,
 		.flags = CMD_HIGH_PRIO | CMD_WANT_SKB,
 	};
+	struct iwl_mvm_d0i3_exit_work_iter_data iter_data = {
+		.mvm = mvm,
+	};
+
 	struct iwl_wowlan_status *status;
 	int ret;
-	u32 handled_reasons, wakeup_reasons = 0;
+	u32 wakeup_reasons = 0;
 	__le16 *qos_seq = NULL;
 
 	mutex_lock(&mvm->mutex);
@@ -1330,18 +1415,12 @@ static void iwl_mvm_d0i3_exit_work(struct work_struct *wk)
 
 	IWL_DEBUG_RPM(mvm, "wakeup reasons: 0x%x\n", wakeup_reasons);
 
-	handled_reasons = IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_MISSED_BEACON |
-				IWL_WOWLAN_WAKEUP_BY_DISCONNECTION_ON_DEAUTH;
-	if (wakeup_reasons & handled_reasons) {
-		struct iwl_mvm_wakeup_reason_iter_data data = {
-			.mvm = mvm,
-			.wakeup_reasons = wakeup_reasons,
-		};
-
-		ieee80211_iterate_active_interfaces(
-			mvm->hw, IEEE80211_IFACE_ITER_NORMAL,
-			iwl_mvm_d0i3_wakeup_reason_iter, &data);
-	}
+	iter_data.wakeup_reasons = wakeup_reasons;
+	iter_data.status = status;
+	ieee80211_iterate_active_interfaces(mvm->hw,
+					    IEEE80211_IFACE_ITER_NORMAL,
+					    iwl_mvm_d0i3_exit_work_iter,
+					    &iter_data);
 out:
 	iwl_mvm_d0i3_enable_tx(mvm, qos_seq);
 
@@ -1367,6 +1446,9 @@ int _iwl_mvm_exit_d0i3(struct iwl_mvm *mvm)
 
 	IWL_DEBUG_RPM(mvm, "MVM exiting D0i3\n");
 
+	if (WARN_ON_ONCE(mvm->cur_ucode != IWL_UCODE_REGULAR))
+		return -EINVAL;
+
 	mutex_lock(&mvm->d0i3_suspend_mutex);
 	if (test_bit(D0I3_DEFER_WAKEUP, &mvm->d0i3_suspend_flags)) {
 		IWL_DEBUG_RPM(mvm, "Deferring d0i3 exit until resume\n");
@@ -1399,6 +1481,7 @@ int iwl_mvm_exit_d0i3(struct iwl_op_mode *op_mode)
 
 #define IWL_MVM_COMMON_OPS					\
 	/* these could be differentiated */			\
+	.async_cb = iwl_mvm_async_cb,				\
 	.queue_full = iwl_mvm_stop_sw_queue,			\
 	.queue_not_full = iwl_mvm_wake_sw_queue,		\
 	.hw_rf_kill = iwl_mvm_set_hw_rfkill_state,		\
@@ -1423,8 +1506,12 @@ static void iwl_mvm_rx_mq_rss(struct iwl_op_mode *op_mode,
 			      unsigned int queue)
 {
 	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 
-	iwl_mvm_rx_rx_mpdu(mvm, napi, rxb);
+	if (unlikely(pkt->hdr.cmd == FRAME_RELEASE))
+		iwl_mvm_rx_frame_release(mvm, rxb, queue);
+	else
+		iwl_mvm_rx_mpdu_mq(mvm, napi, rxb, queue);
 }
 
 static const struct iwl_op_mode_ops iwl_mvm_ops_mq = {
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c
index e68a475e3071..6e6a56f2153d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/phy-ctxt.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/intel/iwlwifi/mvm/power.c
index bed9696ee410..9de159f1ef2d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/power.c
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -613,8 +613,6 @@ static void iwl_mvm_power_get_vifs_iterator(void *_data, u8 *mac,
 		break;
 
 	case NL80211_IFTYPE_STATION:
-		/* only a single MAC of the same type */
-		WARN_ON(power_iterator->bss_vif);
 		power_iterator->bss_vif = vif;
 		if (mvmvif->phy_ctxt)
 			if (mvmvif->phy_ctxt->id < MAX_PHYS)
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/intel/iwlwifi/mvm/quota.c
index 509a66d05245..0b762b4f8fad 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/quota.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/intel/iwlwifi/mvm/rs.c
index d1ad10391b47..94caa88df442 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rs.c
@@ -2,6 +2,7 @@
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -20,7 +21,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -552,9 +553,10 @@ static char *rs_pretty_rate(const struct rs_rate *rate)
 	};
 	const char *rate_str;
 
-	if (is_type_legacy(rate->type))
+	if (is_type_legacy(rate->type) && (rate->index <= IWL_RATE_54M_INDEX))
 		rate_str = legacy_rates[rate->index];
-	else if (is_type_ht(rate->type) || is_type_vht(rate->type))
+	else if ((is_type_ht(rate->type) || is_type_vht(rate->type)) &&
+		 (rate->index <= IWL_RATE_MCS_9_INDEX))
 		rate_str = ht_vht_rates[rate->index];
 	else
 		rate_str = "BAD_RATE";
@@ -723,14 +725,28 @@ static int _rs_collect_tx_data(struct iwl_mvm *mvm,
 	return 0;
 }
 
-static int rs_collect_tx_data(struct iwl_mvm *mvm,
-			      struct iwl_lq_sta *lq_sta,
-			      struct iwl_scale_tbl_info *tbl,
-			      int scale_index, int attempts, int successes,
-			      u8 reduced_txp)
+static int rs_collect_tpc_data(struct iwl_mvm *mvm,
+			       struct iwl_lq_sta *lq_sta,
+			       struct iwl_scale_tbl_info *tbl,
+			       int scale_index, int attempts, int successes,
+			       u8 reduced_txp)
+{
+	struct iwl_rate_scale_data *window = NULL;
+
+	if (WARN_ON_ONCE(reduced_txp > TPC_MAX_REDUCTION))
+		return -EINVAL;
+
+	window = &tbl->tpc_win[reduced_txp];
+	return  _rs_collect_tx_data(mvm, tbl, scale_index, attempts, successes,
+				    window);
+}
+
+static int rs_collect_tlc_data(struct iwl_mvm *mvm,
+			       struct iwl_lq_sta *lq_sta,
+			       struct iwl_scale_tbl_info *tbl,
+			       int scale_index, int attempts, int successes)
 {
 	struct iwl_rate_scale_data *window = NULL;
-	int ret;
 
 	if (scale_index < 0 || scale_index >= IWL_RATE_COUNT)
 		return -EINVAL;
@@ -744,16 +760,6 @@ static int rs_collect_tx_data(struct iwl_mvm *mvm,
 
 	/* Select window for current tx bit rate */
 	window = &(tbl->win[scale_index]);
-
-	ret = _rs_collect_tx_data(mvm, tbl, scale_index, attempts, successes,
-				  window);
-	if (ret)
-		return ret;
-
-	if (WARN_ON_ONCE(reduced_txp > TPC_MAX_REDUCTION))
-		return -EINVAL;
-
-	window = &tbl->tpc_win[reduced_txp];
 	return _rs_collect_tx_data(mvm, tbl, scale_index, attempts, successes,
 				   window);
 }
@@ -1300,17 +1306,30 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	 * first index into rate scale table.
 	 */
 	if (info->flags & IEEE80211_TX_STAT_AMPDU) {
-		/* ampdu_ack_len = 0 marks no BA was received. In this case
-		 * treat it as a single frame loss as we don't want the success
-		 * ratio to dip too quickly because a BA wasn't received
+		rs_collect_tpc_data(mvm, lq_sta, curr_tbl, lq_rate.index,
+				    info->status.ampdu_len,
+				    info->status.ampdu_ack_len,
+				    reduced_txp);
+
+		/* ampdu_ack_len = 0 marks no BA was received. For TLC, treat
+		 * it as a single frame loss as we don't want the success ratio
+		 * to dip too quickly because a BA wasn't received.
+		 * For TPC, there's no need for this optimisation since we want
+		 * to recover very quickly from a bad power reduction and,
+		 * therefore we'd like the success ratio to get an immediate hit
+		 * when failing to get a BA, so we'd switch back to a lower or
+		 * zero power reduction. When FW transmits agg with a rate
+		 * different from the initial rate, it will not use reduced txp
+		 * and will send BA notification twice (one empty with reduced
+		 * txp equal to the value from LQ and one with reduced txp 0).
+		 * We need to update counters for each txp level accordingly.
 		 */
 		if (info->status.ampdu_ack_len == 0)
 			info->status.ampdu_len = 1;
 
-		rs_collect_tx_data(mvm, lq_sta, curr_tbl, lq_rate.index,
-				   info->status.ampdu_len,
-				   info->status.ampdu_ack_len,
-				   reduced_txp);
+		rs_collect_tlc_data(mvm, lq_sta, curr_tbl, lq_rate.index,
+				    info->status.ampdu_len,
+				    info->status.ampdu_ack_len);
 
 		/* Update success/fail counts if not searching for new mode */
 		if (lq_sta->rs_state == RS_STATE_STAY_IN_COLUMN) {
@@ -1343,9 +1362,13 @@ void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			else
 				continue;
 
-			rs_collect_tx_data(mvm, lq_sta, tmp_tbl, lq_rate.index,
-					   1, i < retries ? 0 : legacy_success,
-					   reduced_txp);
+			rs_collect_tpc_data(mvm, lq_sta, tmp_tbl,
+					    lq_rate.index, 1,
+					    i < retries ? 0 : legacy_success,
+					    reduced_txp);
+			rs_collect_tlc_data(mvm, lq_sta, tmp_tbl,
+					    lq_rate.index, 1,
+					    i < retries ? 0 : legacy_success);
 		}
 
 		/* Update success/fail counts if not searching for new mode */
@@ -1827,7 +1850,7 @@ static int rs_switch_to_column(struct iwl_mvm *mvm,
 		rate->type = lq_sta->is_vht ? LQ_VHT_MIMO2 : LQ_HT_MIMO2;
 		rate_mask = lq_sta->active_mimo2_rate;
 	} else {
-		WARN_ON_ONCE("Bad column mode");
+		WARN_ONCE(1, "Bad column mode");
 	}
 
 	if (column->mode != RS_LEGACY) {
@@ -2550,6 +2573,8 @@ static const struct rs_init_rate_info rs_optimal_rates_vht_40_80mhz[] = {
 	{ S8_MIN, IWL_RATE_MCS_0_INDEX },
 };
 
+#define IWL_RS_LOW_RSSI_THRESHOLD (-76) /* dBm */
+
 /* Init the optimal rate based on STA caps
  * This combined with rssi is used to report the last tx rate
  * to userspace when we haven't transmitted enough frames.
@@ -2635,11 +2660,13 @@ static struct rs_rate *rs_get_optimal_rate(struct iwl_mvm *mvm,
  * of last Rx
  */
 static void rs_get_initial_rate(struct iwl_mvm *mvm,
+				struct ieee80211_sta *sta,
 				struct iwl_lq_sta *lq_sta,
 				enum ieee80211_band band,
 				struct rs_rate *rate)
 {
 	int i, nentries;
+	unsigned long active_rate;
 	s8 best_rssi = S8_MIN;
 	u8 best_ant = ANT_NONE;
 	u8 valid_tx_ant = iwl_mvm_get_valid_tx_ant(mvm);
@@ -2680,19 +2707,55 @@ static void rs_get_initial_rate(struct iwl_mvm *mvm,
 		nentries = ARRAY_SIZE(rs_optimal_rates_24ghz_legacy);
 	}
 
-	if (IWL_MVM_RS_RSSI_BASED_INIT_RATE) {
-		for (i = 0; i < nentries; i++) {
-			int rate_idx = initial_rates[i].rate_idx;
-			if ((best_rssi >= initial_rates[i].rssi) &&
-			    (BIT(rate_idx) & lq_sta->active_legacy_rate)) {
-				rate->index = rate_idx;
-				break;
-			}
+	if (!IWL_MVM_RS_RSSI_BASED_INIT_RATE)
+		goto out;
+
+	/* Start from a higher rate if the corresponding debug capability
+	 * is enabled. The rate is chosen according to AP capabilities.
+	 * In case of VHT/HT when the rssi is low fallback to the case of
+	 * legacy rates.
+	 */
+	if (sta->vht_cap.vht_supported &&
+	    best_rssi > IWL_RS_LOW_RSSI_THRESHOLD) {
+		if (sta->bandwidth >= IEEE80211_STA_RX_BW_40) {
+			initial_rates = rs_optimal_rates_vht_40_80mhz;
+			nentries = ARRAY_SIZE(rs_optimal_rates_vht_40_80mhz);
+			if (sta->bandwidth >= IEEE80211_STA_RX_BW_80)
+				rate->bw = RATE_MCS_CHAN_WIDTH_80;
+			else
+				rate->bw = RATE_MCS_CHAN_WIDTH_40;
+		} else if (sta->bandwidth == IEEE80211_STA_RX_BW_20) {
+			initial_rates = rs_optimal_rates_vht_20mhz;
+			nentries = ARRAY_SIZE(rs_optimal_rates_vht_20mhz);
+			rate->bw = RATE_MCS_CHAN_WIDTH_20;
+		} else {
+			IWL_ERR(mvm, "Invalid BW %d\n", sta->bandwidth);
+			goto out;
 		}
+		active_rate = lq_sta->active_siso_rate;
+		rate->type = LQ_VHT_SISO;
+	} else if (sta->ht_cap.ht_supported &&
+		   best_rssi > IWL_RS_LOW_RSSI_THRESHOLD) {
+		initial_rates = rs_optimal_rates_ht;
+		nentries = ARRAY_SIZE(rs_optimal_rates_ht);
+		active_rate = lq_sta->active_siso_rate;
+		rate->type = LQ_HT_SISO;
+	} else {
+		active_rate = lq_sta->active_legacy_rate;
 	}
 
-	IWL_DEBUG_RATE(mvm, "rate_idx %d ANT %s\n", rate->index,
-		       rs_pretty_ant(rate->ant));
+	for (i = 0; i < nentries; i++) {
+		int rate_idx = initial_rates[i].rate_idx;
+
+		if ((best_rssi >= initial_rates[i].rssi) &&
+		    (BIT(rate_idx) & active_rate)) {
+			rate->index = rate_idx;
+			break;
+		}
+	}
+
+out:
+	rs_dump_rate(mvm, rate, "INITIAL");
 }
 
 /* Save info about RSSI of last Rx */
@@ -2752,14 +2815,11 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 	tbl = &(lq_sta->lq_info[active_tbl]);
 	rate = &tbl->rate;
 
-	rs_get_initial_rate(mvm, lq_sta, band, rate);
+	rs_get_initial_rate(mvm, sta, lq_sta, band, rate);
 	rs_init_optimal_rate(mvm, sta, lq_sta);
 
 	WARN_ON_ONCE(rate->ant != ANT_A && rate->ant != ANT_B);
-	if (rate->ant == ANT_A)
-		tbl->column = RS_COLUMN_LEGACY_ANT_A;
-	else
-		tbl->column = RS_COLUMN_LEGACY_ANT_B;
+	tbl->column = rs_get_column_from_rate(rate);
 
 	rs_set_expected_tpt_table(lq_sta, tbl);
 	rs_fill_lq_cmd(mvm, sta, lq_sta, rate);
@@ -3454,15 +3514,9 @@ static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 	 * Tx Fifo so that it can start a transaction in the same TxOP. This
 	 * basically allows the firmware to send bursts.
 	 */
-	if (iwl_mvm_vif_low_latency(mvmvif)) {
+	if (iwl_mvm_vif_low_latency(mvmvif))
 		lq_cmd->agg_frame_cnt_limit--;
 
-		if (mvm->low_latency_agg_frame_limit)
-			lq_cmd->agg_frame_cnt_limit =
-				min(lq_cmd->agg_frame_cnt_limit,
-				    mvm->low_latency_agg_frame_limit);
-	}
-
 	if (mvmsta->vif->p2p)
 		lq_cmd->flags |= LQ_FLAG_USE_RTS_MSK;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/intel/iwlwifi/mvm/rs.h
index 81314ad9ebe0..bdb6f2d8d854 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rs.h
@@ -20,7 +20,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/intel/iwlwifi/mvm/rx.c
index 5b58f5320e8d..145ec68ce6f9 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rx.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -61,10 +61,12 @@
  * (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/etherdevice.h>
 #include <linux/skbuff.h>
 #include "iwl-trans.h"
 #include "mvm.h"
 #include "fw-api.h"
+#include "fw-dbg.h"
 
 /*
  * iwl_mvm_rx_rx_phy_cmd - REPLY_RX_PHY_CMD handler
@@ -261,7 +263,7 @@ void iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct napi_struct *napi,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_rx_phy_info *phy_info;
 	struct iwl_rx_mpdu_res_start *rx_res;
-	struct ieee80211_sta *sta;
+	struct ieee80211_sta *sta = NULL;
 	struct sk_buff *skb;
 	u32 len;
 	u32 ampdu_status;
@@ -332,22 +334,33 @@ void iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct napi_struct *napi,
 			      (unsigned long long)rx_status->mactime);
 
 	rcu_read_lock();
-	/*
-	 * We have tx blocked stations (with CS bit). If we heard frames from
-	 * a blocked station on a new channel we can TX to it again.
-	 */
-	if (unlikely(mvm->csa_tx_block_bcn_timeout)) {
-		sta = ieee80211_find_sta(
-			rcu_dereference(mvm->csa_tx_blocked_vif), hdr->addr2);
-		if (sta)
-			iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, false);
+	if (rx_pkt_status & RX_MPDU_RES_STATUS_SRC_STA_FOUND) {
+		u32 id = rx_pkt_status & RX_MPDU_RES_STATUS_STA_ID_MSK;
+
+		id >>= RX_MDPU_RES_STATUS_STA_ID_SHIFT;
+
+		if (!WARN_ON_ONCE(id >= IWL_MVM_STATION_COUNT)) {
+			sta = rcu_dereference(mvm->fw_id_to_mac_id[id]);
+			if (IS_ERR(sta))
+				sta = NULL;
+		}
+	} else if (!is_multicast_ether_addr(hdr->addr2)) {
+		/* This is fine since we prevent two stations with the same
+		 * address from being added.
+		 */
+		sta = ieee80211_find_sta_by_ifaddr(mvm->hw, hdr->addr2, NULL);
 	}
 
-	/* This is fine since we don't support multiple AP interfaces */
-	sta = ieee80211_find_sta_by_ifaddr(mvm->hw, hdr->addr2, NULL);
 	if (sta) {
 		struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 
+		/* We have tx blocked stations (with CS bit). If we heard
+		 * frames from a blocked station on a new channel we can
+		 * TX to it again.
+		 */
+		if (unlikely(mvm->csa_tx_block_bcn_timeout))
+			iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, false);
+
 		rs_update_last_rssi(mvm, &mvmsta->lq_sta, rx_status);
 
 		if (iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_RSSI) &&
@@ -368,11 +381,10 @@ void iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct napi_struct *napi,
 			if (trig_check && rx_status->signal < rssi)
 				iwl_mvm_fw_dbg_collect_trig(mvm, trig, NULL);
 		}
-	}
-
-	if (sta && ieee80211_is_data(hdr->frame_control))
-		iwl_mvm_rx_csum(sta, skb, rx_pkt_status);
 
+		if (ieee80211_is_data(hdr->frame_control))
+			iwl_mvm_rx_csum(sta, skb, rx_pkt_status);
+	}
 	rcu_read_unlock();
 
 	/* set the preamble flag if appropriate */
diff --git a/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c b/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c
new file mode 100644
index 000000000000..0c073e02fd4c
--- /dev/null
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/rxmq.c
@@ -0,0 +1,458 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+#include "iwl-trans.h"
+#include "mvm.h"
+#include "fw-api.h"
+#include "fw-dbg.h"
+
+void iwl_mvm_rx_phy_cmd_mq(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
+{
+	mvm->ampdu_ref++;
+
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+	if (mvm->last_phy_info.phy_flags & cpu_to_le16(RX_RES_PHY_FLAGS_AGG)) {
+		spin_lock(&mvm->drv_stats_lock);
+		mvm->drv_rx_stats.ampdu_count++;
+		spin_unlock(&mvm->drv_stats_lock);
+	}
+#endif
+}
+
+static inline int iwl_mvm_check_pn(struct iwl_mvm *mvm, struct sk_buff *skb,
+				   int queue, struct ieee80211_sta *sta)
+{
+	struct iwl_mvm_sta *mvmsta;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_rx_status *stats = IEEE80211_SKB_RXCB(skb);
+	struct iwl_mvm_key_pn *ptk_pn;
+	u8 tid, keyidx;
+	u8 pn[IEEE80211_CCMP_PN_LEN];
+	u8 *extiv;
+
+	/* do PN checking */
+
+	/* multicast and non-data only arrives on default queue */
+	if (!ieee80211_is_data(hdr->frame_control) ||
+	    is_multicast_ether_addr(hdr->addr1))
+		return 0;
+
+	/* do not check PN for open AP */
+	if (!(stats->flag & RX_FLAG_DECRYPTED))
+		return 0;
+
+	/*
+	 * avoid checking for default queue - we don't want to replicate
+	 * all the logic that's necessary for checking the PN on fragmented
+	 * frames, leave that to mac80211
+	 */
+	if (queue == 0)
+		return 0;
+
+	/* if we are here - this for sure is either CCMP or GCMP */
+	if (IS_ERR_OR_NULL(sta)) {
+		IWL_ERR(mvm,
+			"expected hw-decrypted unicast frame for station\n");
+		return -1;
+	}
+
+	mvmsta = iwl_mvm_sta_from_mac80211(sta);
+
+	extiv = (u8 *)hdr + ieee80211_hdrlen(hdr->frame_control);
+	keyidx = extiv[3] >> 6;
+
+	ptk_pn = rcu_dereference(mvmsta->ptk_pn[keyidx]);
+	if (!ptk_pn)
+		return -1;
+
+	if (ieee80211_is_data_qos(hdr->frame_control))
+		tid = *ieee80211_get_qos_ctl(hdr) & IEEE80211_QOS_CTL_TID_MASK;
+	else
+		tid = 0;
+
+	/* we don't use HCCA/802.11 QoS TSPECs, so drop such frames */
+	if (tid >= IWL_MAX_TID_COUNT)
+		return -1;
+
+	/* load pn */
+	pn[0] = extiv[7];
+	pn[1] = extiv[6];
+	pn[2] = extiv[5];
+	pn[3] = extiv[4];
+	pn[4] = extiv[1];
+	pn[5] = extiv[0];
+
+	if (memcmp(pn, ptk_pn->q[queue].pn[tid],
+		   IEEE80211_CCMP_PN_LEN) <= 0)
+		return -1;
+
+	memcpy(ptk_pn->q[queue].pn[tid], pn, IEEE80211_CCMP_PN_LEN);
+	stats->flag |= RX_FLAG_PN_VALIDATED;
+
+	return 0;
+}
+
+/* iwl_mvm_create_skb Adds the rxb to a new skb */
+static void iwl_mvm_create_skb(struct sk_buff *skb, struct ieee80211_hdr *hdr,
+			       u16 len, u8 crypt_len,
+			       struct iwl_rx_cmd_buffer *rxb)
+{
+	unsigned int hdrlen, fraglen;
+
+	/* If frame is small enough to fit in skb->head, pull it completely.
+	 * If not, only pull ieee80211_hdr (including crypto if present, and
+	 * an additional 8 bytes for SNAP/ethertype, see below) so that
+	 * splice() or TCP coalesce are more efficient.
+	 *
+	 * Since, in addition, ieee80211_data_to_8023() always pull in at
+	 * least 8 bytes (possibly more for mesh) we can do the same here
+	 * to save the cost of doing it later. That still doesn't pull in
+	 * the actual IP header since the typical case has a SNAP header.
+	 * If the latter changes (there are efforts in the standards group
+	 * to do so) we should revisit this and ieee80211_data_to_8023().
+	 */
+	hdrlen = (len <= skb_tailroom(skb)) ? len :
+					      sizeof(*hdr) + crypt_len + 8;
+
+	memcpy(skb_put(skb, hdrlen), hdr, hdrlen);
+	fraglen = len - hdrlen;
+
+	if (fraglen) {
+		int offset = (void *)hdr + hdrlen -
+			     rxb_addr(rxb) + rxb_offset(rxb);
+
+		skb_add_rx_frag(skb, 0, rxb_steal_page(rxb), offset,
+				fraglen, rxb->truesize);
+	}
+}
+
+/* iwl_mvm_pass_packet_to_mac80211 - passes the packet for mac80211 */
+static void iwl_mvm_pass_packet_to_mac80211(struct iwl_mvm *mvm,
+					    struct napi_struct *napi,
+					    struct sk_buff *skb, int queue,
+					    struct ieee80211_sta *sta)
+{
+	if (iwl_mvm_check_pn(mvm, skb, queue, sta))
+		kfree_skb(skb);
+	else
+		ieee80211_rx_napi(mvm->hw, skb, napi);
+}
+
+static void iwl_mvm_get_signal_strength(struct iwl_mvm *mvm,
+					struct iwl_rx_mpdu_desc *desc,
+					struct ieee80211_rx_status *rx_status)
+{
+	int energy_a, energy_b, energy_c, max_energy;
+
+	energy_a = desc->energy_a;
+	energy_a = energy_a ? -energy_a : S8_MIN;
+	energy_b = desc->energy_b;
+	energy_b = energy_b ? -energy_b : S8_MIN;
+	energy_c = desc->energy_c;
+	energy_c = energy_c ? -energy_c : S8_MIN;
+	max_energy = max(energy_a, energy_b);
+	max_energy = max(max_energy, energy_c);
+
+	IWL_DEBUG_STATS(mvm, "energy In A %d B %d C %d , and max %d\n",
+			energy_a, energy_b, energy_c, max_energy);
+
+	rx_status->signal = max_energy;
+	rx_status->chains = 0; /* TODO: phy info */
+	rx_status->chain_signal[0] = energy_a;
+	rx_status->chain_signal[1] = energy_b;
+	rx_status->chain_signal[2] = energy_c;
+}
+
+static int iwl_mvm_rx_crypto(struct iwl_mvm *mvm, struct ieee80211_hdr *hdr,
+			     struct ieee80211_rx_status *stats,
+			     struct iwl_rx_mpdu_desc *desc, int queue,
+			     u8 *crypt_len)
+{
+	u16 status = le16_to_cpu(desc->status);
+
+	if (!ieee80211_has_protected(hdr->frame_control) ||
+	    (status & IWL_RX_MPDU_STATUS_SEC_MASK) ==
+	    IWL_RX_MPDU_STATUS_SEC_NONE)
+		return 0;
+
+	/* TODO: handle packets encrypted with unknown alg */
+
+	switch (status & IWL_RX_MPDU_STATUS_SEC_MASK) {
+	case IWL_RX_MPDU_STATUS_SEC_CCM:
+	case IWL_RX_MPDU_STATUS_SEC_GCM:
+		BUILD_BUG_ON(IEEE80211_CCMP_PN_LEN != IEEE80211_GCMP_PN_LEN);
+		/* alg is CCM: check MIC only */
+		if (!(status & IWL_RX_MPDU_STATUS_MIC_OK))
+			return -1;
+
+		stats->flag |= RX_FLAG_DECRYPTED;
+		*crypt_len = IEEE80211_CCMP_HDR_LEN;
+		return 0;
+	case IWL_RX_MPDU_STATUS_SEC_TKIP:
+		/* Don't drop the frame and decrypt it in SW */
+		if (!(status & IWL_RX_MPDU_RES_STATUS_TTAK_OK))
+			return 0;
+
+		*crypt_len = IEEE80211_TKIP_IV_LEN;
+		/* fall through if TTAK OK */
+	case IWL_RX_MPDU_STATUS_SEC_WEP:
+		if (!(status & IWL_RX_MPDU_STATUS_ICV_OK))
+			return -1;
+
+		stats->flag |= RX_FLAG_DECRYPTED;
+		if ((status & IWL_RX_MPDU_STATUS_SEC_MASK) ==
+				IWL_RX_MPDU_STATUS_SEC_WEP)
+			*crypt_len = IEEE80211_WEP_IV_LEN;
+		return 0;
+	case IWL_RX_MPDU_STATUS_SEC_EXT_ENC:
+		if (!(status & IWL_RX_MPDU_STATUS_MIC_OK))
+			return -1;
+		stats->flag |= RX_FLAG_DECRYPTED;
+		return 0;
+	default:
+		IWL_ERR(mvm, "Unhandled alg: 0x%x\n", status);
+	}
+
+	return 0;
+}
+
+static void iwl_mvm_rx_csum(struct ieee80211_sta *sta,
+			    struct sk_buff *skb,
+			    struct iwl_rx_mpdu_desc *desc)
+{
+	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(mvmsta->vif);
+
+	if (mvmvif->features & NETIF_F_RXCSUM &&
+	    desc->l3l4_flags & cpu_to_le16(IWL_RX_L3L4_IP_HDR_CSUM_OK) &&
+	    desc->l3l4_flags & cpu_to_le16(IWL_RX_L3L4_TCP_UDP_CSUM_OK))
+		skb->ip_summed = CHECKSUM_UNNECESSARY;
+}
+
+void iwl_mvm_rx_mpdu_mq(struct iwl_mvm *mvm, struct napi_struct *napi,
+			struct iwl_rx_cmd_buffer *rxb, int queue)
+{
+	struct ieee80211_rx_status *rx_status;
+	struct iwl_rx_packet *pkt = rxb_addr(rxb);
+	struct iwl_rx_mpdu_desc *desc = (void *)pkt->data;
+	struct ieee80211_hdr *hdr = (void *)(desc + 1);
+	u32 len = le16_to_cpu(desc->mpdu_len);
+	u32 rate_n_flags = le32_to_cpu(desc->rate_n_flags);
+	struct ieee80211_sta *sta = NULL;
+	struct sk_buff *skb;
+	u8 crypt_len = 0;
+
+	/* Dont use dev_alloc_skb(), we'll have enough headroom once
+	 * ieee80211_hdr pulled.
+	 */
+	skb = alloc_skb(128, GFP_ATOMIC);
+	if (!skb) {
+		IWL_ERR(mvm, "alloc_skb failed\n");
+		return;
+	}
+
+	rx_status = IEEE80211_SKB_RXCB(skb);
+
+	if (iwl_mvm_rx_crypto(mvm, hdr, rx_status, desc, queue, &crypt_len)) {
+		kfree_skb(skb);
+		return;
+	}
+
+	/*
+	 * Keep packets with CRC errors (and with overrun) for monitor mode
+	 * (otherwise the firmware discards them) but mark them as bad.
+	 */
+	if (!(desc->status & cpu_to_le16(IWL_RX_MPDU_STATUS_CRC_OK)) ||
+	    !(desc->status & cpu_to_le16(IWL_RX_MPDU_STATUS_OVERRUN_OK))) {
+		IWL_DEBUG_RX(mvm, "Bad CRC or FIFO: 0x%08X.\n",
+			     le16_to_cpu(desc->status));
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+	}
+
+	rx_status->mactime = le64_to_cpu(desc->tsf_on_air_rise);
+	rx_status->device_timestamp = le32_to_cpu(desc->gp2_on_air_rise);
+	rx_status->band = desc->channel > 14 ? IEEE80211_BAND_5GHZ :
+					       IEEE80211_BAND_2GHZ;
+	rx_status->freq = ieee80211_channel_to_frequency(desc->channel,
+							 rx_status->band);
+	iwl_mvm_get_signal_strength(mvm, desc, rx_status);
+
+	rcu_read_lock();
+
+	if (le16_to_cpu(desc->status) & IWL_RX_MPDU_STATUS_SRC_STA_FOUND) {
+		u8 id = desc->sta_id_flags & IWL_RX_MPDU_SIF_STA_ID_MASK;
+
+		if (!WARN_ON_ONCE(id >= IWL_MVM_STATION_COUNT)) {
+			sta = rcu_dereference(mvm->fw_id_to_mac_id[id]);
+			if (IS_ERR(sta))
+				sta = NULL;
+		}
+	} else if (!is_multicast_ether_addr(hdr->addr2)) {
+		/*
+		 * This is fine since we prevent two stations with the same
+		 * address from being added.
+		 */
+		sta = ieee80211_find_sta_by_ifaddr(mvm->hw, hdr->addr2, NULL);
+	}
+
+	if (sta) {
+		struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+
+		/*
+		 * We have tx blocked stations (with CS bit). If we heard
+		 * frames from a blocked station on a new channel we can
+		 * TX to it again.
+		 */
+		if (unlikely(mvm->csa_tx_block_bcn_timeout))
+			iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, false);
+
+		rs_update_last_rssi(mvm, &mvmsta->lq_sta, rx_status);
+
+		if (iwl_fw_dbg_trigger_enabled(mvm->fw, FW_DBG_TRIGGER_RSSI) &&
+		    ieee80211_is_beacon(hdr->frame_control)) {
+			struct iwl_fw_dbg_trigger_tlv *trig;
+			struct iwl_fw_dbg_trigger_low_rssi *rssi_trig;
+			bool trig_check;
+			s32 rssi;
+
+			trig = iwl_fw_dbg_get_trigger(mvm->fw,
+						      FW_DBG_TRIGGER_RSSI);
+			rssi_trig = (void *)trig->data;
+			rssi = le32_to_cpu(rssi_trig->rssi);
+
+			trig_check =
+				iwl_fw_dbg_trigger_check_stop(mvm, mvmsta->vif,
+							      trig);
+			if (trig_check && rx_status->signal < rssi)
+				iwl_mvm_fw_dbg_collect_trig(mvm, trig, NULL);
+		}
+
+		/* TODO: multi queue TCM */
+
+		if (ieee80211_is_data(hdr->frame_control))
+			iwl_mvm_rx_csum(sta, skb, desc);
+	}
+
+	/*
+	 * TODO: PHY info.
+	 * Verify we don't have the information in the MPDU descriptor and
+	 * that it is not needed.
+	 * Make sure for monitor mode that we are on default queue, update
+	 * ampdu_ref and the rest of phy info then
+	 */
+
+	/* Set up the HT phy flags */
+	switch (rate_n_flags & RATE_MCS_CHAN_WIDTH_MSK) {
+	case RATE_MCS_CHAN_WIDTH_20:
+		break;
+	case RATE_MCS_CHAN_WIDTH_40:
+		rx_status->flag |= RX_FLAG_40MHZ;
+		break;
+	case RATE_MCS_CHAN_WIDTH_80:
+		rx_status->vht_flag |= RX_VHT_FLAG_80MHZ;
+		break;
+	case RATE_MCS_CHAN_WIDTH_160:
+		rx_status->vht_flag |= RX_VHT_FLAG_160MHZ;
+		break;
+	}
+	if (rate_n_flags & RATE_MCS_SGI_MSK)
+		rx_status->flag |= RX_FLAG_SHORT_GI;
+	if (rate_n_flags & RATE_HT_MCS_GF_MSK)
+		rx_status->flag |= RX_FLAG_HT_GF;
+	if (rate_n_flags & RATE_MCS_LDPC_MSK)
+		rx_status->flag |= RX_FLAG_LDPC;
+	if (rate_n_flags & RATE_MCS_HT_MSK) {
+		u8 stbc = (rate_n_flags & RATE_MCS_HT_STBC_MSK) >>
+				RATE_MCS_STBC_POS;
+		rx_status->flag |= RX_FLAG_HT;
+		rx_status->rate_idx = rate_n_flags & RATE_HT_MCS_INDEX_MSK;
+		rx_status->flag |= stbc << RX_FLAG_STBC_SHIFT;
+	} else if (rate_n_flags & RATE_MCS_VHT_MSK) {
+		u8 stbc = (rate_n_flags & RATE_MCS_VHT_STBC_MSK) >>
+				RATE_MCS_STBC_POS;
+		rx_status->vht_nss =
+			((rate_n_flags & RATE_VHT_MCS_NSS_MSK) >>
+						RATE_VHT_MCS_NSS_POS) + 1;
+		rx_status->rate_idx = rate_n_flags & RATE_VHT_MCS_RATE_CODE_MSK;
+		rx_status->flag |= RX_FLAG_VHT;
+		rx_status->flag |= stbc << RX_FLAG_STBC_SHIFT;
+		if (rate_n_flags & RATE_MCS_BF_MSK)
+			rx_status->vht_flag |= RX_VHT_FLAG_BF;
+	} else {
+		rx_status->rate_idx =
+			iwl_mvm_legacy_rate_to_mac80211_idx(rate_n_flags,
+							    rx_status->band);
+	}
+
+	/* TODO: PHY info - update ampdu queue statistics (for debugfs) */
+	/* TODO: PHY info - gscan */
+
+	iwl_mvm_create_skb(skb, hdr, len, crypt_len, rxb);
+	iwl_mvm_pass_packet_to_mac80211(mvm, napi, skb, queue, sta);
+	rcu_read_unlock();
+}
+
+void iwl_mvm_rx_frame_release(struct iwl_mvm *mvm,
+			      struct iwl_rx_cmd_buffer *rxb, int queue)
+{
+	/* TODO */
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/intel/iwlwifi/mvm/scan.c
index d6e0c1b5c20c..ea1e177c2ea1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/scan.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -72,13 +72,6 @@
 #define IWL_DENSE_EBS_SCAN_RATIO 5
 #define IWL_SPARSE_EBS_SCAN_RATIO 1
 
-enum iwl_mvm_scan_type {
-	IWL_SCAN_TYPE_UNASSOC,
-	IWL_SCAN_TYPE_WILD,
-	IWL_SCAN_TYPE_MILD,
-	IWL_SCAN_TYPE_FRAGMENTED,
-};
-
 enum iwl_mvm_traffic_load {
 	IWL_MVM_TRAFFIC_LOW,
 	IWL_MVM_TRAFFIC_MEDIUM,
@@ -89,6 +82,7 @@ struct iwl_mvm_scan_timing_params {
 	u32 dwell_active;
 	u32 dwell_passive;
 	u32 dwell_fragmented;
+	u32 dwell_extended;
 	u32 suspend_time;
 	u32 max_out_time;
 };
@@ -98,6 +92,7 @@ static struct iwl_mvm_scan_timing_params scan_timing[] = {
 		.dwell_active = 10,
 		.dwell_passive = 110,
 		.dwell_fragmented = 44,
+		.dwell_extended = 90,
 		.suspend_time = 0,
 		.max_out_time = 0,
 	},
@@ -105,6 +100,7 @@ static struct iwl_mvm_scan_timing_params scan_timing[] = {
 		.dwell_active = 10,
 		.dwell_passive = 110,
 		.dwell_fragmented = 44,
+		.dwell_extended = 90,
 		.suspend_time = 30,
 		.max_out_time = 120,
 	},
@@ -112,6 +108,7 @@ static struct iwl_mvm_scan_timing_params scan_timing[] = {
 		.dwell_active = 10,
 		.dwell_passive = 110,
 		.dwell_fragmented = 44,
+		.dwell_extended = 90,
 		.suspend_time = 120,
 		.max_out_time = 120,
 	},
@@ -206,9 +203,7 @@ static enum iwl_mvm_traffic_load iwl_mvm_get_traffic_load(struct iwl_mvm *mvm)
 }
 
 static enum
-iwl_mvm_scan_type iwl_mvm_get_scan_type(struct iwl_mvm *mvm,
-					struct ieee80211_vif *vif,
-					struct iwl_mvm_scan_params *params)
+iwl_mvm_scan_type iwl_mvm_get_scan_type(struct iwl_mvm *mvm, bool p2p_device)
 {
 	int global_cnt = 0;
 	enum iwl_mvm_traffic_load load;
@@ -224,8 +219,7 @@ iwl_mvm_scan_type iwl_mvm_get_scan_type(struct iwl_mvm *mvm,
 	load = iwl_mvm_get_traffic_load(mvm);
 	low_latency = iwl_mvm_low_latency(mvm);
 
-	if ((load == IWL_MVM_TRAFFIC_HIGH || low_latency) &&
-	    vif->type != NL80211_IFTYPE_P2P_DEVICE &&
+	if ((load == IWL_MVM_TRAFFIC_HIGH || low_latency) && !p2p_device &&
 	    fw_has_api(&mvm->fw->ucode_capa, IWL_UCODE_TLV_API_FRAGMENTED_SCAN))
 		return IWL_SCAN_TYPE_FRAGMENTED;
 
@@ -333,6 +327,13 @@ void iwl_mvm_rx_lmac_scan_complete_notif(struct iwl_mvm *mvm,
 	struct iwl_periodic_scan_complete *scan_notif = (void *)pkt->data;
 	bool aborted = (scan_notif->status == IWL_SCAN_OFFLOAD_ABORTED);
 
+	/* If this happens, the firmware has mistakenly sent an LMAC
+	 * notification during UMAC scans -- warn and ignore it.
+	 */
+	if (WARN_ON_ONCE(fw_has_capa(&mvm->fw->ucode_capa,
+				     IWL_UCODE_TLV_CAPA_UMAC_SCAN)))
+		return;
+
 	/* scan status must be locked for proper checking */
 	lockdep_assert_held(&mvm->mutex);
 
@@ -719,6 +720,7 @@ static void iwl_mvm_scan_lmac_dwell(struct iwl_mvm *mvm,
 	cmd->active_dwell = scan_timing[params->type].dwell_active;
 	cmd->passive_dwell = scan_timing[params->type].dwell_passive;
 	cmd->fragmented_dwell = scan_timing[params->type].dwell_fragmented;
+	cmd->extended_dwell = scan_timing[params->type].dwell_extended;
 	cmd->max_out_time = cpu_to_le32(scan_timing[params->type].max_out_time);
 	cmd->suspend_time = cpu_to_le32(scan_timing[params->type].suspend_time);
 	cmd->scan_prio = iwl_mvm_scan_priority(mvm, IWL_SCAN_PRIORITY_EXT_6);
@@ -752,8 +754,15 @@ static inline bool iwl_mvm_scan_use_ebs(struct iwl_mvm *mvm,
 		vif->type != NL80211_IFTYPE_P2P_DEVICE);
 }
 
+static inline bool iwl_mvm_is_regular_scan(struct iwl_mvm_scan_params *params)
+{
+	return params->n_scan_plans == 1 &&
+		params->scan_plans[0].iterations == 1;
+}
+
 static int iwl_mvm_scan_lmac_flags(struct iwl_mvm *mvm,
-				   struct iwl_mvm_scan_params *params)
+				   struct iwl_mvm_scan_params *params,
+				   struct ieee80211_vif *vif)
 {
 	int flags = 0;
 
@@ -779,6 +788,11 @@ static int iwl_mvm_scan_lmac_flags(struct iwl_mvm *mvm,
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE;
 #endif
 
+	if (iwl_mvm_is_regular_scan(params) &&
+	    vif->type != NL80211_IFTYPE_P2P_DEVICE &&
+	    params->type != IWL_SCAN_TYPE_FRAGMENTED)
+		flags |= IWL_MVM_LMAC_SCAN_FLAG_EXTENDED_DWELL;
+
 	return flags;
 }
 
@@ -807,7 +821,8 @@ static int iwl_mvm_scan_lmac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 	cmd->delay = cpu_to_le32(params->delay);
 
-	cmd->scan_flags = cpu_to_le32(iwl_mvm_scan_lmac_flags(mvm, params));
+	cmd->scan_flags = cpu_to_le32(iwl_mvm_scan_lmac_flags(mvm, params,
+							      vif));
 
 	cmd->flags = iwl_mvm_scan_rxon_flags(params->channels[0]->band);
 	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
@@ -910,10 +925,14 @@ int iwl_mvm_config_scan(struct iwl_mvm *mvm)
 	struct iwl_host_cmd cmd = {
 		.id = iwl_cmd_id(SCAN_CFG_CMD, IWL_ALWAYS_LONG_GROUP, 0),
 	};
+	enum iwl_mvm_scan_type type = iwl_mvm_get_scan_type(mvm, false);
 
 	if (WARN_ON(num_channels > mvm->fw->ucode_capa.n_scan_channels))
 		return -ENOBUFS;
 
+	if (type == mvm->scan_type)
+		return 0;
+
 	cmd_size = sizeof(*scan_config) + mvm->fw->ucode_capa.n_scan_channels;
 
 	scan_config = kzalloc(cmd_size, GFP_KERNEL);
@@ -928,15 +947,20 @@ int iwl_mvm_config_scan(struct iwl_mvm *mvm)
 					 SCAN_CONFIG_FLAG_SET_LEGACY_RATES |
 					 SCAN_CONFIG_FLAG_SET_MAC_ADDR |
 					 SCAN_CONFIG_FLAG_SET_CHANNEL_FLAGS|
-					 SCAN_CONFIG_N_CHANNELS(num_channels));
+					 SCAN_CONFIG_N_CHANNELS(num_channels) |
+					 (type == IWL_SCAN_TYPE_FRAGMENTED ?
+					  SCAN_CONFIG_FLAG_SET_FRAGMENTED :
+					  SCAN_CONFIG_FLAG_CLEAR_FRAGMENTED));
 	scan_config->tx_chains = cpu_to_le32(iwl_mvm_get_valid_tx_ant(mvm));
 	scan_config->rx_chains = cpu_to_le32(iwl_mvm_scan_rx_ant(mvm));
 	scan_config->legacy_rates = iwl_mvm_scan_config_rates(mvm);
-	scan_config->out_of_channel_time = cpu_to_le32(170);
-	scan_config->suspend_time = cpu_to_le32(30);
-	scan_config->dwell_active = 20;
-	scan_config->dwell_passive = 110;
-	scan_config->dwell_fragmented = 20;
+	scan_config->out_of_channel_time =
+		cpu_to_le32(scan_timing[type].max_out_time);
+	scan_config->suspend_time = cpu_to_le32(scan_timing[type].suspend_time);
+	scan_config->dwell_active = scan_timing[type].dwell_active;
+	scan_config->dwell_passive = scan_timing[type].dwell_passive;
+	scan_config->dwell_fragmented = scan_timing[type].dwell_fragmented;
+	scan_config->dwell_extended = scan_timing[type].dwell_extended;
 
 	memcpy(&scan_config->mac_addr, &mvm->addresses[0].addr, ETH_ALEN);
 
@@ -960,6 +984,8 @@ int iwl_mvm_config_scan(struct iwl_mvm *mvm)
 	IWL_DEBUG_SCAN(mvm, "Sending UMAC scan config\n");
 
 	ret = iwl_mvm_send_cmd(mvm, &cmd);
+	if (!ret)
+		mvm->scan_type = type;
 
 	kfree(scan_config);
 	return ret;
@@ -976,16 +1002,11 @@ static int iwl_mvm_scan_uid_by_status(struct iwl_mvm *mvm, int status)
 	return -ENOENT;
 }
 
-static inline bool iwl_mvm_is_regular_scan(struct iwl_mvm_scan_params *params)
-{
-	return params->n_scan_plans == 1 &&
-		params->scan_plans[0].iterations == 1;
-}
-
 static void iwl_mvm_scan_umac_dwell(struct iwl_mvm *mvm,
 				    struct iwl_scan_req_umac *cmd,
 				    struct iwl_mvm_scan_params *params)
 {
+	cmd->extended_dwell = scan_timing[params->type].dwell_extended;
 	cmd->active_dwell = scan_timing[params->type].dwell_active;
 	cmd->passive_dwell = scan_timing[params->type].dwell_passive;
 	cmd->fragmented_dwell = scan_timing[params->type].dwell_fragmented;
@@ -1020,7 +1041,8 @@ iwl_mvm_umac_scan_cfg_channels(struct iwl_mvm *mvm,
 }
 
 static u32 iwl_mvm_scan_umac_flags(struct iwl_mvm *mvm,
-				   struct iwl_mvm_scan_params *params)
+				   struct iwl_mvm_scan_params *params,
+				   struct ieee80211_vif *vif)
 {
 	int flags = 0;
 
@@ -1048,6 +1070,12 @@ static u32 iwl_mvm_scan_umac_flags(struct iwl_mvm *mvm,
 	if (mvm->scan_iter_notif_enabled)
 		flags |= IWL_UMAC_SCAN_GEN_FLAGS_ITER_COMPLETE;
 #endif
+
+	if (iwl_mvm_is_regular_scan(params) &&
+	    vif->type != NL80211_IFTYPE_P2P_DEVICE &&
+	    params->type != IWL_SCAN_TYPE_FRAGMENTED)
+		flags |= IWL_UMAC_SCAN_GEN_FLAGS_EXTENDED_DWELL;
+
 	return flags;
 }
 
@@ -1078,7 +1106,8 @@ static int iwl_mvm_scan_umac(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	mvm->scan_uid_status[uid] = type;
 
 	cmd->uid = cpu_to_le32(uid);
-	cmd->general_flags = cpu_to_le32(iwl_mvm_scan_umac_flags(mvm, params));
+	cmd->general_flags = cpu_to_le32(iwl_mvm_scan_umac_flags(mvm, params,
+								 vif));
 
 	if (type == IWL_MVM_SCAN_SCHED)
 		cmd->flags = cpu_to_le32(IWL_UMAC_SCAN_FLAG_PREEMPTIVE);
@@ -1150,7 +1179,7 @@ static int iwl_mvm_check_running_scans(struct iwl_mvm *mvm, int type)
 	case IWL_MVM_SCAN_SCHED:
 		if (mvm->scan_status & IWL_MVM_SCAN_SCHED_MASK)
 			return -EBUSY;
-		iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_REGULAR, true);
+		return iwl_mvm_scan_stop(mvm, IWL_MVM_SCAN_REGULAR, true);
 	case IWL_MVM_SCAN_NETDETECT:
 		/* No need to stop anything for net-detect since the
 		 * firmware is restarted anyway.  This way, any sched
@@ -1213,7 +1242,9 @@ int iwl_mvm_reg_scan_start(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	params.scan_plans = &scan_plan;
 	params.n_scan_plans = 1;
 
-	params.type = iwl_mvm_get_scan_type(mvm, vif, &params);
+	params.type =
+		iwl_mvm_get_scan_type(mvm,
+				      vif->type == NL80211_IFTYPE_P2P_DEVICE);
 
 	iwl_mvm_build_scan_probe(mvm, vif, ies, &params);
 
@@ -1267,6 +1298,10 @@ int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
 		return -EBUSY;
 	}
 
+	/* we don't support "match all" in the firmware */
+	if (!req->n_match_sets)
+		return -EOPNOTSUPP;
+
 	ret = iwl_mvm_check_running_scans(mvm, type);
 	if (ret)
 		return ret;
@@ -1295,7 +1330,9 @@ int iwl_mvm_sched_scan_start(struct iwl_mvm *mvm,
 	params.n_scan_plans = req->n_scan_plans;
 	params.scan_plans = req->scan_plans;
 
-	params.type = iwl_mvm_get_scan_type(mvm, vif, &params);
+	params.type =
+		iwl_mvm_get_scan_type(mvm,
+				      vif->type == NL80211_IFTYPE_P2P_DEVICE);
 
 	/* In theory, LMAC scans can handle a 32-bit delay, but since
 	 * waiting for over 18 hours to start the scan is a bit silly
diff --git a/drivers/net/wireless/iwlwifi/mvm/sf.c b/drivers/net/wireless/intel/iwlwifi/mvm/sf.c
index b0f59fdd287c..c2def1232a8c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sf.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sf.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
index 2b976b110207..b556e33658d7 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -106,6 +106,7 @@ int iwl_mvm_sta_send_to_fw(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		.add_modify = update ? 1 : 0,
 		.station_flags_msk = cpu_to_le32(STA_FLG_FAT_EN_MSK |
 						 STA_FLG_MIMO_EN_MSK),
+		.tid_disable_tx = cpu_to_le16(mvm_sta->tid_disable_agg),
 	};
 	int ret;
 	u32 status;
@@ -277,11 +278,6 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 	if (sta_id == IWL_MVM_STATION_COUNT)
 		return -ENOSPC;
 
-	if (vif->type == NL80211_IFTYPE_AP) {
-		mvmvif->ap_assoc_sta_count++;
-		iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
-	}
-
 	spin_lock_init(&mvm_sta->lock);
 
 	mvm_sta->sta_id = sta_id;
@@ -580,9 +576,9 @@ int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
 	return ret;
 }
 
-static int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
-				    struct iwl_mvm_int_sta *sta,
-				    u32 qmask, enum nl80211_iftype iftype)
+int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
+			     struct iwl_mvm_int_sta *sta,
+			     u32 qmask, enum nl80211_iftype iftype)
 {
 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
 		sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
@@ -622,6 +618,7 @@ static int iwl_mvm_add_int_sta_common(struct iwl_mvm *mvm,
 							     color));
 
 	cmd.tfd_queue_msk = cpu_to_le32(sta->tfd_queue_msk);
+	cmd.tid_disable_tx = cpu_to_le16(0xffff);
 
 	if (addr)
 		memcpy(cmd.addr, addr, ETH_ALEN);
@@ -671,6 +668,33 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
 	return ret;
 }
 
+int iwl_mvm_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	lockdep_assert_held(&mvm->mutex);
+	return iwl_mvm_add_int_sta_common(mvm, &mvm->snif_sta, vif->addr,
+					 mvmvif->id, 0);
+}
+
+int iwl_mvm_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	int ret;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	ret = iwl_mvm_rm_sta_common(mvm, mvm->snif_sta.sta_id);
+	if (ret)
+		IWL_WARN(mvm, "Failed sending remove station\n");
+
+	return ret;
+}
+
+void iwl_mvm_dealloc_snif_sta(struct iwl_mvm *mvm)
+{
+	iwl_mvm_dealloc_int_sta(mvm, &mvm->snif_sta);
+}
+
 void iwl_mvm_del_aux_sta(struct iwl_mvm *mvm)
 {
 	lockdep_assert_held(&mvm->mutex);
@@ -1196,22 +1220,17 @@ static int iwl_mvm_set_fw_key_idx(struct iwl_mvm *mvm)
 	if (max_offs < 0)
 		return STA_KEY_IDX_INVALID;
 
-	__set_bit(max_offs, mvm->fw_key_table);
-
 	return max_offs;
 }
 
-static u8 iwl_mvm_get_key_sta_id(struct iwl_mvm *mvm,
-				 struct ieee80211_vif *vif,
-				 struct ieee80211_sta *sta)
+static struct iwl_mvm_sta *iwl_mvm_get_key_sta(struct iwl_mvm *mvm,
+					       struct ieee80211_vif *vif,
+					       struct ieee80211_sta *sta)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
-	if (sta) {
-		struct iwl_mvm_sta *mvm_sta = iwl_mvm_sta_from_mac80211(sta);
-
-		return mvm_sta->sta_id;
-	}
+	if (sta)
+		return iwl_mvm_sta_from_mac80211(sta);
 
 	/*
 	 * The device expects GTKs for station interfaces to be
@@ -1230,12 +1249,12 @@ static u8 iwl_mvm_get_key_sta_id(struct iwl_mvm *mvm,
 		 * be the AP ID, and no station was passed by mac80211.
 		 */
 		if (IS_ERR_OR_NULL(sta))
-			return IWL_MVM_STATION_COUNT;
+			return NULL;
 
-		return sta_id;
+		return iwl_mvm_sta_from_mac80211(sta);
 	}
 
-	return IWL_MVM_STATION_COUNT;
+	return NULL;
 }
 
 static int iwl_mvm_send_sta_key(struct iwl_mvm *mvm,
@@ -1452,6 +1471,7 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 			u8 key_offset)
 {
 	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
+	struct iwl_mvm_sta *mvm_sta;
 	u8 sta_id;
 	int ret;
 	static const u8 __maybe_unused zero_addr[ETH_ALEN] = {0};
@@ -1459,11 +1479,12 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 	lockdep_assert_held(&mvm->mutex);
 
 	/* Get the station id from the mvm local station table */
-	sta_id = iwl_mvm_get_key_sta_id(mvm, vif, sta);
-	if (sta_id == IWL_MVM_STATION_COUNT) {
-		IWL_ERR(mvm, "Failed to find station id\n");
+	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
+	if (!mvm_sta) {
+		IWL_ERR(mvm, "Failed to find station\n");
 		return -EINVAL;
 	}
+	sta_id = mvm_sta->sta_id;
 
 	if (keyconf->cipher == WLAN_CIPHER_SUITE_AES_CMAC) {
 		ret = iwl_mvm_send_sta_igtk(mvm, keyconf, sta_id, false);
@@ -1505,10 +1526,8 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 	}
 
 	ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf, key_offset, mcast);
-	if (ret) {
-		__clear_bit(keyconf->hw_key_idx, mvm->fw_key_table);
+	if (ret)
 		goto end;
-	}
 
 	/*
 	 * For WEP, the same key is used for multicast and unicast. Upload it
@@ -1521,11 +1540,13 @@ int iwl_mvm_set_sta_key(struct iwl_mvm *mvm,
 		ret = __iwl_mvm_set_sta_key(mvm, vif, sta, keyconf,
 					    key_offset, !mcast);
 		if (ret) {
-			__clear_bit(keyconf->hw_key_idx, mvm->fw_key_table);
 			__iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
+			goto end;
 		}
 	}
 
+	__set_bit(key_offset, mvm->fw_key_table);
+
 end:
 	IWL_DEBUG_WEP(mvm, "key: cipher=%x len=%d idx=%d sta=%pM ret=%d\n",
 		      keyconf->cipher, keyconf->keylen, keyconf->keyidx,
@@ -1539,13 +1560,14 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
 			   struct ieee80211_key_conf *keyconf)
 {
 	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
-	u8 sta_id;
+	struct iwl_mvm_sta *mvm_sta;
+	u8 sta_id = IWL_MVM_STATION_COUNT;
 	int ret, i;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* Get the station id from the mvm local station table */
-	sta_id = iwl_mvm_get_key_sta_id(mvm, vif, sta);
+	/* Get the station from the mvm local station table */
+	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
 
 	IWL_DEBUG_WEP(mvm, "mvm remove dynamic key: idx=%d sta=%d\n",
 		      keyconf->keyidx, sta_id);
@@ -1566,11 +1588,13 @@ int iwl_mvm_remove_sta_key(struct iwl_mvm *mvm,
 	}
 	mvm->fw_key_deleted[keyconf->hw_key_idx] = 0;
 
-	if (sta_id == IWL_MVM_STATION_COUNT) {
+	if (!mvm_sta) {
 		IWL_DEBUG_WEP(mvm, "station non-existent, early return.\n");
 		return 0;
 	}
 
+	sta_id = mvm_sta->sta_id;
+
 	ret = __iwl_mvm_remove_sta_key(mvm, sta_id, keyconf, mcast);
 	if (ret)
 		return ret;
@@ -1590,24 +1614,13 @@ void iwl_mvm_update_tkip_key(struct iwl_mvm *mvm,
 			     u16 *phase1key)
 {
 	struct iwl_mvm_sta *mvm_sta;
-	u8 sta_id;
 	bool mcast = !(keyconf->flags & IEEE80211_KEY_FLAG_PAIRWISE);
 
 	rcu_read_lock();
 
-	sta_id = iwl_mvm_get_key_sta_id(mvm, vif, sta);
-	if (WARN_ON_ONCE(sta_id == IWL_MVM_STATION_COUNT))
+	mvm_sta = iwl_mvm_get_key_sta(mvm, vif, sta);
+	if (WARN_ON_ONCE(!mvm_sta))
 		goto unlock;
-
-	if (!sta) {
-		sta = rcu_dereference(mvm->fw_id_to_mac_id[sta_id]);
-		if (WARN_ON(IS_ERR_OR_NULL(sta))) {
-			rcu_read_unlock();
-			return;
-		}
-	}
-
-	mvm_sta = iwl_mvm_sta_from_mac80211(sta);
 	iwl_mvm_send_sta_key(mvm, mvm_sta, keyconf, mcast,
 			     iv32, phase1key, CMD_ASYNC, keyconf->hw_key_idx);
 
@@ -1665,6 +1678,7 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 	 */
 	if (agg) {
 		int remaining = cnt;
+		int sleep_tx_count;
 
 		spin_lock_bh(&mvmsta->lock);
 		for_each_set_bit(tid, &_tids, IWL_MAX_TID_COUNT) {
@@ -1689,9 +1703,12 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 			}
 			remaining -= n_queued;
 		}
+		sleep_tx_count = cnt - remaining;
+		if (reason == IEEE80211_FRAME_RELEASE_UAPSD)
+			mvmsta->sleep_tx_count = sleep_tx_count;
 		spin_unlock_bh(&mvmsta->lock);
 
-		cmd.sleep_tx_count = cpu_to_le16(cnt - remaining);
+		cmd.sleep_tx_count = cpu_to_le16(sleep_tx_count);
 		if (WARN_ON(cnt - remaining == 0)) {
 			ieee80211_sta_eosp(sta);
 			return;
@@ -1709,7 +1726,12 @@ void iwl_mvm_sta_modify_sleep_tx_count(struct iwl_mvm *mvm,
 		cmd.sleep_state_flags |= cpu_to_le16(STA_SLEEP_STATE_UAPSD);
 	}
 
-	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA, CMD_ASYNC, sizeof(cmd), &cmd);
+	/* block the Tx queues until the FW updated the sleep Tx count */
+	iwl_trans_block_txq_ptrs(mvm->trans, true);
+
+	ret = iwl_mvm_send_cmd_pdu(mvm, ADD_STA,
+				   CMD_ASYNC | CMD_WANT_ASYNC_CALLBACK,
+				   sizeof(cmd), &cmd);
 	if (ret)
 		IWL_ERR(mvm, "Failed to send ADD_STA command (%d)\n", ret);
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
index 0631cc0a6d3c..39fdf5224e81 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/sta.h
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
+ * Copyright(c) 2015        Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -284,6 +286,13 @@ static inline u16 iwl_mvm_tid_queued(struct iwl_mvm_tid_data *tid_data)
 				tid_data->next_reclaimed);
 }
 
+struct iwl_mvm_key_pn {
+	struct rcu_head rcu_head;
+	struct {
+		u8 pn[IWL_MAX_TID_COUNT][IEEE80211_CCMP_PN_LEN];
+	} ____cacheline_aligned_in_smp q[];
+};
+
 /**
  * struct iwl_mvm_sta - representation of a station in the driver
  * @sta_id: the index of the station in the fw (will be replaced by id_n_color)
@@ -303,6 +312,12 @@ static inline u16 iwl_mvm_tid_queued(struct iwl_mvm_tid_data *tid_data)
  * @tt_tx_protection: is thermal throttling enable Tx protection?
  * @disable_tx: is tx to this STA disabled?
  * @agg_tids: bitmap of tids whose status is operational aggregated (IWL_AGG_ON)
+ * @sleep_tx_count: the number of frames that we told the firmware to let out
+ *	even when that station is asleep. This is useful in case the queue
+ *	gets empty before all the frames were sent, which can happen when
+ *	we are sending frames from an AMPDU queue and there was a hole in
+ *	the BA window. To be used for UAPSD only.
+ * @ptk_pn: per-queue PTK PN data structures
  *
  * When mac80211 creates a station it reserves some space (hw->sta_data_size)
  * in the structure for use by driver. This structure is placed in that
@@ -323,12 +338,15 @@ struct iwl_mvm_sta {
 	struct iwl_lq_sta lq_sta;
 	struct ieee80211_vif *vif;
 
+	struct iwl_mvm_key_pn __rcu *ptk_pn[4];
+
 	/* Temporary, until the new TLC will control the Tx protection */
 	s8 tx_protection;
 	bool tt_tx_protection;
 
 	bool disable_tx;
 	u8 agg_tids;
+	u8 sleep_tx_count;
 };
 
 static inline struct iwl_mvm_sta *
@@ -401,7 +419,13 @@ int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
+			     struct iwl_mvm_int_sta *sta,
+				    u32 qmask, enum nl80211_iftype iftype);
 void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_add_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_rm_snif_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_dealloc_snif_sta(struct iwl_mvm *mvm);
 
 void iwl_mvm_sta_drained_wk(struct work_struct *wk);
 void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
diff --git a/drivers/net/wireless/iwlwifi/mvm/tdls.c b/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c
index fe2fa5650443..18711c5de35a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tdls.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tdls.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/testmode.h b/drivers/net/wireless/intel/iwlwifi/mvm/testmode.h
index 79ab6beb6b26..cbbc16fd006a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/testmode.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/testmode.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c
index 7530eb23035d..924dd6a41626 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -73,6 +73,7 @@
 #include "mvm.h"
 #include "iwl-io.h"
 #include "iwl-prph.h"
+#include "fw-dbg.h"
 
 /*
  * For the high priority TE use a time event type that has similar priority to
@@ -791,11 +792,9 @@ int iwl_mvm_start_p2p_roc(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	return iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
 }
 
-void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
+static struct iwl_mvm_time_event_data *iwl_mvm_get_roc_te(struct iwl_mvm *mvm)
 {
-	struct iwl_mvm_vif *mvmvif = NULL;
 	struct iwl_mvm_time_event_data *te_data;
-	bool is_p2p = false;
 
 	lockdep_assert_held(&mvm->mutex);
 
@@ -809,11 +808,8 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 	 * request
 	 */
 	list_for_each_entry(te_data, &mvm->time_event_list, list) {
-		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE) {
-			mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
-			is_p2p = true;
-			goto remove_te;
-		}
+		if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE)
+			goto out;
 	}
 
 	/* There can only be at most one AUX ROC time event, we just use the
@@ -822,18 +818,35 @@ void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
 	te_data = list_first_entry_or_null(&mvm->aux_roc_te_list,
 					   struct iwl_mvm_time_event_data,
 					   list);
+out:
+	spin_unlock_bh(&mvm->time_event_lock);
+	return te_data;
+}
+
+void iwl_mvm_cleanup_roc_te(struct iwl_mvm *mvm)
+{
+	struct iwl_mvm_time_event_data *te_data;
+	u32 uid;
+
+	te_data = iwl_mvm_get_roc_te(mvm);
 	if (te_data)
-		mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
+		__iwl_mvm_remove_time_event(mvm, te_data, &uid);
+}
 
-remove_te:
-	spin_unlock_bh(&mvm->time_event_lock);
+void iwl_mvm_stop_roc(struct iwl_mvm *mvm)
+{
+	struct iwl_mvm_vif *mvmvif;
+	struct iwl_mvm_time_event_data *te_data;
 
-	if (!mvmvif) {
+	te_data = iwl_mvm_get_roc_te(mvm);
+	if (!te_data) {
 		IWL_WARN(mvm, "No remain on channel event\n");
 		return;
 	}
 
-	if (is_p2p)
+	mvmvif = iwl_mvm_vif_from_mac80211(te_data->vif);
+
+	if (te_data->vif->type == NL80211_IFTYPE_P2P_DEVICE)
 		iwl_mvm_remove_time_event(mvm, mvmvif, te_data);
 	else
 		iwl_mvm_remove_aux_roc_te(mvm, mvmvif, te_data);
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.h b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.h
index cbdf8e52a5f1..99d9a35ad5b1 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/time-event.h
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -215,6 +215,7 @@ void iwl_mvm_remove_time_event(struct iwl_mvm *mvm,
 void iwl_mvm_te_clear_data(struct iwl_mvm *mvm,
 			   struct iwl_mvm_time_event_data *te_data);
 
+void iwl_mvm_cleanup_roc_te(struct iwl_mvm *mvm);
 void iwl_mvm_roc_done_wk(struct work_struct *wk);
 
 /**
diff --git a/drivers/net/wireless/iwlwifi/mvm/tof.c b/drivers/net/wireless/intel/iwlwifi/mvm/tof.c
index 4007f1d421dd..a1947d6f3a2c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tof.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tof.c
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- * Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/tof.h b/drivers/net/wireless/intel/iwlwifi/mvm/tof.h
index 9beebc33cb8d..8c3421c9991d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tof.h
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tof.h
@@ -25,7 +25,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- * Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
diff --git a/drivers/net/wireless/iwlwifi/mvm/tt.c b/drivers/net/wireless/intel/iwlwifi/mvm/tt.c
index cadfc0460597..fb76004eede4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tt.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -120,7 +120,7 @@ static int iwl_mvm_temp_notif_parse(struct iwl_mvm *mvm,
 	int len = iwl_rx_packet_payload_len(pkt);
 	int temp;
 
-	if (WARN_ON_ONCE(len != sizeof(*notif))) {
+	if (WARN_ON_ONCE(len < sizeof(*notif))) {
 		IWL_ERR(mvm, "Invalid DTS_MEASUREMENT_NOTIFICATION\n");
 		return -EINVAL;
 	}
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/intel/iwlwifi/mvm/tx.c
index c652a66be803..a040edc55057 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/tx.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -64,11 +64,13 @@
  *****************************************************************************/
 #include <linux/ieee80211.h>
 #include <linux/etherdevice.h>
+#include <linux/tcp.h>
 
 #include "iwl-trans.h"
 #include "iwl-eeprom-parse.h"
 #include "mvm.h"
 #include "sta.h"
+#include "fw-dbg.h"
 
 static void
 iwl_mvm_bar_check_trigger(struct iwl_mvm *mvm, const u8 *addr,
@@ -344,8 +346,8 @@ iwl_mvm_set_tx_params(struct iwl_mvm *mvm, struct sk_buff *skb,
 	iwl_mvm_set_tx_cmd_rate(mvm, tx_cmd, info, sta, hdr->frame_control);
 
 	memset(&info->status, 0, sizeof(info->status));
+	memset(info->driver_data, 0, sizeof(info->driver_data));
 
-	info->driver_data[0] = NULL;
 	info->driver_data[1] = dev_cmd;
 
 	return dev_cmd;
@@ -421,14 +423,51 @@ int iwl_mvm_tx_skb_non_sta(struct iwl_mvm *mvm, struct sk_buff *skb)
 		return -1;
 	}
 
+	/*
+	 * Increase the pending frames counter, so that later when a reply comes
+	 * in and the counter is decreased - we don't start getting negative
+	 * values.
+	 * Note that we don't need to make sure it isn't agg'd, since we're
+	 * TXing non-sta
+	 */
+	atomic_inc(&mvm->pending_frames[sta_id]);
+
+	return 0;
+}
+
+static int iwl_mvm_tx_tso(struct iwl_mvm *mvm, struct sk_buff *skb_gso,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff_head *mpdus_skb)
+{
+	struct sk_buff *tmp, *next;
+	char cb[sizeof(skb_gso->cb)];
+
+	memcpy(cb, skb_gso->cb, sizeof(cb));
+	next = skb_gso_segment(skb_gso, 0);
+	if (IS_ERR(next))
+		return -EINVAL;
+	else if (next)
+		consume_skb(skb_gso);
+
+	while (next) {
+		tmp = next;
+		next = tmp->next;
+		memcpy(tmp->cb, cb, sizeof(tmp->cb));
+
+		tmp->prev = NULL;
+		tmp->next = NULL;
+
+		__skb_queue_tail(mpdus_skb, tmp);
+	}
+
 	return 0;
 }
 
 /*
  * Sets the fields in the Tx cmd that are crypto related
  */
-int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
-		   struct ieee80211_sta *sta)
+static int iwl_mvm_tx_mpdu(struct iwl_mvm *mvm, struct sk_buff *skb,
+			   struct ieee80211_sta *sta)
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -524,6 +563,51 @@ drop:
 	return -1;
 }
 
+int iwl_mvm_tx_skb(struct iwl_mvm *mvm, struct sk_buff *skb,
+		   struct ieee80211_sta *sta)
+{
+	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	struct sk_buff_head mpdus_skbs;
+	unsigned int payload_len;
+	int ret;
+
+	if (WARN_ON_ONCE(!mvmsta))
+		return -1;
+
+	if (WARN_ON_ONCE(mvmsta->sta_id == IWL_MVM_STATION_COUNT))
+		return -1;
+
+	if (!skb_is_gso(skb))
+		return iwl_mvm_tx_mpdu(mvm, skb, sta);
+
+	payload_len = skb_tail_pointer(skb) - skb_transport_header(skb) -
+		tcp_hdrlen(skb) + skb->data_len;
+
+	if (payload_len <= skb_shinfo(skb)->gso_size)
+		return iwl_mvm_tx_mpdu(mvm, skb, sta);
+
+	__skb_queue_head_init(&mpdus_skbs);
+
+	ret = iwl_mvm_tx_tso(mvm, skb, sta, &mpdus_skbs);
+	if (ret)
+		return ret;
+
+	if (WARN_ON(skb_queue_empty(&mpdus_skbs)))
+		return ret;
+
+	while (!skb_queue_empty(&mpdus_skbs)) {
+		struct sk_buff *skb = __skb_dequeue(&mpdus_skbs);
+
+		ret = iwl_mvm_tx_mpdu(mvm, skb, sta);
+		if (ret) {
+			__skb_queue_purge(&mpdus_skbs);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
 static void iwl_mvm_check_ratid_empty(struct iwl_mvm *mvm,
 				      struct ieee80211_sta *sta, u8 tid)
 {
@@ -787,13 +871,43 @@ static void iwl_mvm_rx_tx_cmd_single(struct iwl_mvm *mvm,
 		if (tid != IWL_TID_NON_QOS) {
 			struct iwl_mvm_tid_data *tid_data =
 				&mvmsta->tid_data[tid];
+			bool send_eosp_ndp = false;
 
 			spin_lock_bh(&mvmsta->lock);
 			tid_data->next_reclaimed = next_reclaimed;
 			IWL_DEBUG_TX_REPLY(mvm, "Next reclaimed packet:%d\n",
 					   next_reclaimed);
 			iwl_mvm_check_ratid_empty(mvm, sta, tid);
+
+			if (mvmsta->sleep_tx_count) {
+				mvmsta->sleep_tx_count--;
+				if (mvmsta->sleep_tx_count &&
+				    !iwl_mvm_tid_queued(tid_data)) {
+					/*
+					 * The number of frames in the queue
+					 * dropped to 0 even if we sent less
+					 * frames than we thought we had on the
+					 * Tx queue.
+					 * This means we had holes in the BA
+					 * window that we just filled, ask
+					 * mac80211 to send EOSP since the
+					 * firmware won't know how to do that.
+					 * Send NDP and the firmware will send
+					 * EOSP notification that will trigger
+					 * a call to ieee80211_sta_eosp().
+					 */
+					send_eosp_ndp = true;
+				}
+			}
+
 			spin_unlock_bh(&mvmsta->lock);
+			if (send_eosp_ndp) {
+				iwl_mvm_sta_modify_sleep_tx_count(mvm, sta,
+					IEEE80211_FRAME_RELEASE_UAPSD,
+					1, tid, false, false);
+				mvmsta->sleep_tx_count = 0;
+				ieee80211_send_eosp_nullfunc(sta, tid);
+			}
 		}
 
 		if (mvmsta->next_status_eosp) {
@@ -924,7 +1038,6 @@ static void iwl_mvm_rx_tx_cmd_agg(struct iwl_mvm *mvm,
 		struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
 		mvmsta->tid_data[tid].rate_n_flags =
 			le32_to_cpu(tx_resp->initial_rate);
-		mvmsta->tid_data[tid].reduced_tpc = tx_resp->reduced_tpc;
 		mvmsta->tid_data[tid].tx_time =
 			le16_to_cpu(tx_resp->wireless_media_time);
 	}
@@ -955,7 +1068,7 @@ static void iwl_mvm_tx_info_from_ba_notif(struct ieee80211_tx_info *info,
 	/* TODO: not accounted if the whole A-MPDU failed */
 	info->status.tx_time = tid_data->tx_time;
 	info->status.status_driver_data[0] =
-		(void *)(uintptr_t)tid_data->reduced_tpc;
+		(void *)(uintptr_t)ba_notif->reduced_txp;
 	info->status.status_driver_data[1] =
 		(void *)(uintptr_t)tid_data->rate_n_flags;
 }
@@ -1028,6 +1141,8 @@ void iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb)
 			   scd_flow, ba_resp_scd_ssn, ba_notif->txed,
 			   ba_notif->txed_2_done);
 
+	IWL_DEBUG_TX_REPLY(mvm, "reduced txp from ba notif %d\n",
+			   ba_notif->reduced_txp);
 	tid_data->next_reclaimed = ba_resp_scd_ssn;
 
 	iwl_mvm_check_ratid_empty(mvm, sta, tid);
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/intel/iwlwifi/mvm/utils.c
index ad0f16909e2e..3a989f5c20db 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/intel/iwlwifi/mvm/utils.c
@@ -27,7 +27,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -68,7 +68,7 @@
 #include "iwl-debug.h"
 #include "iwl-io.h"
 #include "iwl-prph.h"
-
+#include "fw-dbg.h"
 #include "mvm.h"
 #include "fw-api-rs.h"
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/intel/iwlwifi/pcie/drv.c
index 639761fb2bfb..00335ea6b3eb 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/drv.c
@@ -26,7 +26,7 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
@@ -377,6 +377,13 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x3165, 0x8010, iwl3165_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x3165, 0x8110, iwl3165_2ac_cfg)},
 
+/* 3168 Series */
+	{IWL_PCI_DEVICE(0x24FB, 0x2010, iwl3168_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FB, 0x2110, iwl3168_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FB, 0x2050, iwl3168_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FB, 0x2150, iwl3168_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FB, 0x0000, iwl3168_2ac_cfg)},
+
 /* 7265 Series */
 	{IWL_PCI_DEVICE(0x095A, 0x5010, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5110, iwl7265_2ac_cfg)},
@@ -384,6 +391,7 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x095B, 0x5310, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x5302, iwl7265_n_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x5210, iwl7265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x095A, 0x5C10, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5012, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5412, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5410, iwl7265_2ac_cfg)},
@@ -401,10 +409,10 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x095A, 0x900A, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9110, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9112, iwl7265_2ac_cfg)},
-	{IWL_PCI_DEVICE(0x095A, 0x9210, iwl7265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x095B, 0x9210, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095B, 0x9200, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9510, iwl7265_2ac_cfg)},
-	{IWL_PCI_DEVICE(0x095A, 0x9310, iwl7265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x095B, 0x9310, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9410, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5020, iwl7265_2n_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x502A, iwl7265_2n_cfg)},
@@ -467,6 +475,23 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x24F3, 0x0850, iwl8260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x24F3, 0x0950, iwl8260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x24F3, 0x0930, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0000, iwl8265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FD, 0x0010, iwl8265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FD, 0x8010, iwl8265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24FD, 0x0810, iwl8265_2ac_cfg)},
+
+/* 9000 Series */
+	{IWL_PCI_DEVICE(0x9DF0, 0x2A10, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x2010, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0A10, iwl9260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0010, iwl9260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0000, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0310, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0510, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0710, iwl5165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0210, iwl9260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0410, iwl9260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x9DF0, 0x0610, iwl9260_2ac_cfg)},
 #endif /* CONFIG_IWLMVM */
 
 	{0}
@@ -598,7 +623,7 @@ static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	set_dflt_pwr_limit(iwl_trans, pdev);
 
 	/* register transport layer debugfs here */
-	ret = iwl_trans_dbgfs_register(iwl_trans, iwl_trans->dbgfs_dir);
+	ret = iwl_trans_pcie_dbgfs_register(iwl_trans);
 	if (ret)
 		goto out_free_drv;
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/intel/iwlwifi/pcie/internal.h
index feb2f7e81134..73c95594eabe 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/internal.h
@@ -23,7 +23,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -278,6 +278,7 @@ struct iwl_txq {
 	bool frozen;
 	u8 active;
 	bool ampdu;
+	bool block;
 	unsigned long wd_timeout;
 };
 
@@ -288,6 +289,11 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
 	       sizeof(struct iwl_pcie_txq_scratch_buf) * idx;
 }
 
+struct iwl_tso_hdr_page {
+	struct page *page;
+	u8 *pos;
+};
+
 /**
  * struct iwl_trans_pcie - PCIe transport specific data
  * @rxq: all the RX queue data
@@ -302,10 +308,12 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
  * @ucode_write_complete: indicates that the ucode has been copied.
  * @ucode_write_waitq: wait queue for uCode load
  * @cmd_queue - command queue number
- * @rx_buf_size_8k: 8 kB RX buffer size
+ * @rx_buf_size: Rx buffer size
  * @bc_table_dword: true if the BC table expects DWORD (as opposed to bytes)
  * @scd_set_active: should the transport configure the SCD for HCMD queue
  * @wide_cmd_header: true when ucode supports wide command header format
+ * @sw_csum_tx: if true, then the transport will compute the csum of the TXed
+ *	frame.
  * @rx_page_order: page order for receive buffer size
  * @reg_lock: protect hw register access
  * @mutex: to protect stop_device / start_fw / start_hw
@@ -323,6 +331,8 @@ struct iwl_trans_pcie {
 	struct net_device napi_dev;
 	struct napi_struct napi;
 
+	struct __percpu iwl_tso_hdr_page *tso_hdr_page;
+
 	/* INT ICT Table */
 	__le32 *ict_tbl;
 	dma_addr_t ict_tbl_dma;
@@ -356,14 +366,13 @@ struct iwl_trans_pcie {
 	u8 n_no_reclaim_cmds;
 	u8 no_reclaim_cmds[MAX_NO_RECLAIM_CMDS];
 
-	bool rx_buf_size_8k;
+	enum iwl_amsdu_size rx_buf_size;
 	bool bc_table_dword;
 	bool scd_set_active;
 	bool wide_cmd_header;
+	bool sw_csum_tx;
 	u32 rx_page_order;
 
-	const char *const *command_names;
-
 	/*protect hw register */
 	spinlock_t reg_lock;
 	bool cmd_hold_nic_awake;
@@ -378,8 +387,11 @@ struct iwl_trans_pcie {
 	u32 fw_mon_size;
 };
 
-#define IWL_TRANS_GET_PCIE_TRANS(_iwl_trans) \
-	((struct iwl_trans_pcie *) ((_iwl_trans)->trans_specific))
+static inline struct iwl_trans_pcie *
+IWL_TRANS_GET_PCIE_TRANS(struct iwl_trans *trans)
+{
+	return (void *)trans->trans_specific;
+}
 
 static inline struct iwl_trans *
 iwl_trans_pcie_get_trans(struct iwl_trans_pcie *trans_pcie)
@@ -478,6 +490,15 @@ static inline void iwl_enable_interrupts(struct iwl_trans *trans)
 	iwl_write32(trans, CSR_INT_MASK, trans_pcie->inta_mask);
 }
 
+static inline void iwl_enable_fw_load_int(struct iwl_trans *trans)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+
+	IWL_DEBUG_ISR(trans, "Enabling FW load interrupt\n");
+	trans_pcie->inta_mask = CSR_INT_BIT_FH_TX;
+	iwl_write32(trans, CSR_INT_MASK, trans_pcie->inta_mask);
+}
+
 static inline void iwl_enable_rfkill_int(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -523,14 +544,6 @@ static inline u8 get_cmd_index(struct iwl_queue *q, u32 index)
 	return index & (q->n_window - 1);
 }
 
-static inline const char *get_cmd_string(struct iwl_trans_pcie *trans_pcie,
-					 u8 cmd)
-{
-	if (!trans_pcie->command_names || !trans_pcie->command_names[cmd])
-		return "UNKNOWN";
-	return trans_pcie->command_names[cmd];
-}
-
 static inline bool iwl_is_rfkill_set(struct iwl_trans *trans)
 {
 	return !(iwl_read32(trans, CSR_GP_CNTRL) &
@@ -566,4 +579,13 @@ static inline void __iwl_trans_pcie_set_bit(struct iwl_trans *trans,
 
 void iwl_trans_pcie_rf_kill(struct iwl_trans *trans, bool state);
 
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans);
+#else
+static inline int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans)
+{
+	return 0;
+}
+#endif
+
 #endif /* __iwl_trans_int_pcie_h__ */
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/intel/iwlwifi/pcie/rx.c
index e06591f625c4..152cf9ad9566 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/rx.c
@@ -23,7 +23,7 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
@@ -602,10 +602,20 @@ static void iwl_pcie_rx_hw_init(struct iwl_trans *trans, struct iwl_rxq *rxq)
 	u32 rb_size;
 	const u32 rfdnlog = RX_QUEUE_SIZE_LOG; /* 256 RBDs */
 
-	if (trans_pcie->rx_buf_size_8k)
+	switch (trans_pcie->rx_buf_size) {
+	case IWL_AMSDU_4K:
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
+		break;
+	case IWL_AMSDU_8K:
 		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K;
-	else
+		break;
+	case IWL_AMSDU_12K:
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_12K;
+		break;
+	default:
+		WARN_ON(1);
 		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
+	}
 
 	/* Stop Rx DMA */
 	iwl_write_direct32(trans, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
@@ -629,7 +639,7 @@ static void iwl_pcie_rx_hw_init(struct iwl_trans *trans, struct iwl_rxq *rxq)
 	 * FH_RCSR_CHNL0_RX_IGNORE_RXF_EMPTY is set because of HW bug in
 	 *      the credit mechanism in 5000 HW RX FIFO
 	 * Direct rx interrupts to hosts
-	 * Rx buffer size 4 or 8k
+	 * Rx buffer size 4 or 8k or 12k
 	 * RB timeout 0x10
 	 * 256 RBDs
 	 */
@@ -867,7 +877,10 @@ static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 		IWL_DEBUG_RX(trans,
 			     "cmd at offset %d: %s (0x%.2x, seq 0x%x)\n",
 			     rxcb._offset,
-			     get_cmd_string(trans_pcie, pkt->hdr.cmd),
+			     iwl_get_cmd_string(trans,
+						iwl_cmd_id(pkt->hdr.cmd,
+							   pkt->hdr.group_id,
+							   0)),
 			     pkt->hdr.cmd, le16_to_cpu(pkt->hdr.sequence));
 
 		len = iwl_rx_packet_len(pkt);
@@ -986,8 +999,7 @@ restart:
 		rxb = rxq->queue[i];
 		rxq->queue[i] = NULL;
 
-		IWL_DEBUG_RX(trans, "rxbuf: HW = %d, SW = %d (%p)\n",
-			     r, i, rxb);
+		IWL_DEBUG_RX(trans, "rxbuf: HW = %d, SW = %d\n", r, i);
 		iwl_pcie_rx_handle_rb(trans, rxb, emergency);
 
 		i = (i + 1) & RX_QUEUE_MASK;
@@ -1426,9 +1438,11 @@ irqreturn_t iwl_pcie_irq_handler(int irq, void *dev_id)
 			 inta & ~trans_pcie->inta_mask);
 	}
 
-	/* Re-enable all interrupts */
-	/* only Re-enable if disabled by irq */
-	if (test_bit(STATUS_INT_ENABLED, &trans->status))
+	/* we are loading the firmware, enable FH_TX interrupt only */
+	if (handled & CSR_INT_BIT_FH_TX)
+		iwl_enable_fw_load_int(trans);
+	/* only Re-enable all interrupt if disabled by irq */
+	else if (test_bit(STATUS_INT_ENABLED, &trans->status))
 		iwl_enable_interrupts(trans);
 	/* Re-enable RF_KILL if it occurred */
 	else if (handled & CSR_INT_BIT_RF_KILL)
@@ -1481,10 +1495,6 @@ int iwl_pcie_alloc_ict(struct iwl_trans *trans)
 		return -EINVAL;
 	}
 
-	IWL_DEBUG_ISR(trans, "ict dma addr %Lx ict vir addr %p\n",
-		      (unsigned long long)trans_pcie->ict_tbl_dma,
-		      trans_pcie->ict_tbl);
-
 	return 0;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/intel/iwlwifi/pcie/trans.c
index 90283453073c..5a854c609477 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/trans.c
@@ -7,6 +7,7 @@
  *
  * Copyright(c) 2007 - 2015 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 Intel Deutschland GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -26,13 +27,14 @@
  * in the file called COPYING.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2015 Intel Corporation. All rights reserved.
  * Copyright(c) 2013 - 2015 Intel Mobile Communications GmbH
+ * Copyright(c) 2016 Intel Deutschland GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -924,9 +926,16 @@ monitor:
 	if (dest->monitor_mode == EXTERNAL_MODE && trans_pcie->fw_mon_size) {
 		iwl_write_prph(trans, le32_to_cpu(dest->base_reg),
 			       trans_pcie->fw_mon_phys >> dest->base_shift);
-		iwl_write_prph(trans, le32_to_cpu(dest->end_reg),
-			       (trans_pcie->fw_mon_phys +
-				trans_pcie->fw_mon_size) >> dest->end_shift);
+		if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+			iwl_write_prph(trans, le32_to_cpu(dest->end_reg),
+				       (trans_pcie->fw_mon_phys +
+					trans_pcie->fw_mon_size - 256) >>
+						dest->end_shift);
+		else
+			iwl_write_prph(trans, le32_to_cpu(dest->end_reg),
+				       (trans_pcie->fw_mon_phys +
+					trans_pcie->fw_mon_size) >>
+						dest->end_shift);
 	}
 }
 
@@ -1012,82 +1021,6 @@ static int iwl_pcie_load_given_ucode_8000(struct iwl_trans *trans,
 					       &first_ucode_section);
 }
 
-static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
-				   const struct fw_img *fw, bool run_in_rfkill)
-{
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
-	bool hw_rfkill;
-	int ret;
-
-	mutex_lock(&trans_pcie->mutex);
-
-	/* Someone called stop_device, don't try to start_fw */
-	if (trans_pcie->is_down) {
-		IWL_WARN(trans,
-			 "Can't start_fw since the HW hasn't been started\n");
-		ret = EIO;
-		goto out;
-	}
-
-	/* This may fail if AMT took ownership of the device */
-	if (iwl_pcie_prepare_card_hw(trans)) {
-		IWL_WARN(trans, "Exit HW not ready\n");
-		ret = -EIO;
-		goto out;
-	}
-
-	iwl_enable_rfkill_int(trans);
-
-	/* If platform's RF_KILL switch is NOT set to KILL */
-	hw_rfkill = iwl_is_rfkill_set(trans);
-	if (hw_rfkill)
-		set_bit(STATUS_RFKILL, &trans->status);
-	else
-		clear_bit(STATUS_RFKILL, &trans->status);
-	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
-	if (hw_rfkill && !run_in_rfkill) {
-		ret = -ERFKILL;
-		goto out;
-	}
-
-	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
-
-	ret = iwl_pcie_nic_init(trans);
-	if (ret) {
-		IWL_ERR(trans, "Unable to init nic\n");
-		goto out;
-	}
-
-	/* make sure rfkill handshake bits are cleared */
-	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
-	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR,
-		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
-
-	/* clear (again), then enable host interrupts */
-	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
-	iwl_enable_interrupts(trans);
-
-	/* really make sure rfkill handshake bits are cleared */
-	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
-	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
-
-	/* Load the given image to the HW */
-	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
-		ret = iwl_pcie_load_given_ucode_8000(trans, fw);
-	else
-		ret = iwl_pcie_load_given_ucode(trans, fw);
-
-out:
-	mutex_unlock(&trans_pcie->mutex);
-	return ret;
-}
-
-static void iwl_trans_pcie_fw_alive(struct iwl_trans *trans, u32 scd_addr)
-{
-	iwl_pcie_reset_ict(trans);
-	iwl_pcie_tx_start(trans, scd_addr);
-}
-
 static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -1118,7 +1051,8 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	 * already dead.
 	 */
 	if (test_and_clear_bit(STATUS_DEVICE_ENABLED, &trans->status)) {
-		IWL_DEBUG_INFO(trans, "DEVICE_ENABLED bit was set and is now cleared\n");
+		IWL_DEBUG_INFO(trans,
+			       "DEVICE_ENABLED bit was set and is now cleared\n");
 		iwl_pcie_tx_stop(trans);
 		iwl_pcie_rx_stop(trans);
 
@@ -1152,7 +1086,6 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	iwl_disable_interrupts(trans);
 	spin_unlock(&trans_pcie->irq_lock);
 
-
 	/* clear all status bits */
 	clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 	clear_bit(STATUS_INT_ENABLED, &trans->status);
@@ -1185,10 +1118,116 @@ static void _iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 	if (hw_rfkill != was_hw_rfkill)
 		iwl_trans_pcie_rf_kill(trans, hw_rfkill);
 
-	/* re-take ownership to prevent other users from stealing the deivce */
+	/* re-take ownership to prevent other users from stealing the device */
 	iwl_pcie_prepare_card_hw(trans);
 }
 
+static int iwl_trans_pcie_start_fw(struct iwl_trans *trans,
+				   const struct fw_img *fw, bool run_in_rfkill)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	bool hw_rfkill;
+	int ret;
+
+	/* This may fail if AMT took ownership of the device */
+	if (iwl_pcie_prepare_card_hw(trans)) {
+		IWL_WARN(trans, "Exit HW not ready\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	iwl_enable_rfkill_int(trans);
+
+	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
+
+	/*
+	 * We enabled the RF-Kill interrupt and the handler may very
+	 * well be running. Disable the interrupts to make sure no other
+	 * interrupt can be fired.
+	 */
+	iwl_disable_interrupts(trans);
+
+	/* Make sure it finished running */
+	synchronize_irq(trans_pcie->pci_dev->irq);
+
+	mutex_lock(&trans_pcie->mutex);
+
+	/* If platform's RF_KILL switch is NOT set to KILL */
+	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill)
+		set_bit(STATUS_RFKILL, &trans->status);
+	else
+		clear_bit(STATUS_RFKILL, &trans->status);
+	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
+	if (hw_rfkill && !run_in_rfkill) {
+		ret = -ERFKILL;
+		goto out;
+	}
+
+	/* Someone called stop_device, don't try to start_fw */
+	if (trans_pcie->is_down) {
+		IWL_WARN(trans,
+			 "Can't start_fw since the HW hasn't been started\n");
+		ret = -EIO;
+		goto out;
+	}
+
+	/* make sure rfkill handshake bits are cleared */
+	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+	/* clear (again), then enable host interrupts */
+	iwl_write32(trans, CSR_INT, 0xFFFFFFFF);
+
+	ret = iwl_pcie_nic_init(trans);
+	if (ret) {
+		IWL_ERR(trans, "Unable to init nic\n");
+		goto out;
+	}
+
+	/*
+	 * Now, we load the firmware and don't want to be interrupted, even
+	 * by the RF-Kill interrupt (hence mask all the interrupt besides the
+	 * FH_TX interrupt which is needed to load the firmware). If the
+	 * RF-Kill switch is toggled, we will find out after having loaded
+	 * the firmware and return the proper value to the caller.
+	 */
+	iwl_enable_fw_load_int(trans);
+
+	/* really make sure rfkill handshake bits are cleared */
+	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(trans, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* Load the given image to the HW */
+	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
+		ret = iwl_pcie_load_given_ucode_8000(trans, fw);
+	else
+		ret = iwl_pcie_load_given_ucode(trans, fw);
+	iwl_enable_interrupts(trans);
+
+	/* re-check RF-Kill state since we may have missed the interrupt */
+	hw_rfkill = iwl_is_rfkill_set(trans);
+	if (hw_rfkill)
+		set_bit(STATUS_RFKILL, &trans->status);
+	else
+		clear_bit(STATUS_RFKILL, &trans->status);
+
+	iwl_trans_pcie_rf_kill(trans, hw_rfkill);
+	if (hw_rfkill && !run_in_rfkill)
+		ret = -ERFKILL;
+
+out:
+	mutex_unlock(&trans_pcie->mutex);
+	return ret;
+}
+
+static void iwl_trans_pcie_fw_alive(struct iwl_trans *trans, u32 scd_addr)
+{
+	iwl_pcie_reset_ict(trans);
+	iwl_pcie_tx_start(trans, scd_addr);
+}
+
 static void iwl_trans_pcie_stop_device(struct iwl_trans *trans, bool low_power)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -1213,7 +1252,7 @@ static void iwl_trans_pcie_d3_suspend(struct iwl_trans *trans, bool test)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 
-	if (trans->wowlan_d0i3) {
+	if (trans->system_pm_mode == IWL_PLAT_PM_MODE_D0I3) {
 		/* Enable persistence mode to avoid reset */
 		iwl_set_bit(trans, CSR_HW_IF_CONFIG_REG,
 			    CSR_HW_IF_CONFIG_REG_PERSIST_MODE);
@@ -1237,7 +1276,7 @@ static void iwl_trans_pcie_d3_suspend(struct iwl_trans *trans, bool test)
 	iwl_clear_bit(trans, CSR_GP_CNTRL,
 		      CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 
-	if (!trans->wowlan_d0i3) {
+	if (trans->system_pm_mode == IWL_PLAT_PM_MODE_D3) {
 		/*
 		 * reset TX queues -- some of their registers reset during S3
 		 * so if we don't reset everything here the D3 image would try
@@ -1286,7 +1325,7 @@ static int iwl_trans_pcie_d3_resume(struct iwl_trans *trans,
 
 	iwl_pcie_set_pwr(trans, false);
 
-	if (trans->wowlan_d0i3) {
+	if (trans->system_pm_mode == IWL_PLAT_PM_MODE_D0I3) {
 		iwl_clear_bit(trans, CSR_GP_CNTRL,
 			      CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
 	} else {
@@ -1435,16 +1474,17 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 		memcpy(trans_pcie->no_reclaim_cmds, trans_cfg->no_reclaim_cmds,
 		       trans_pcie->n_no_reclaim_cmds * sizeof(u8));
 
-	trans_pcie->rx_buf_size_8k = trans_cfg->rx_buf_size_8k;
-	if (trans_pcie->rx_buf_size_8k)
-		trans_pcie->rx_page_order = get_order(8 * 1024);
-	else
-		trans_pcie->rx_page_order = get_order(4 * 1024);
+	trans_pcie->rx_buf_size = trans_cfg->rx_buf_size;
+	trans_pcie->rx_page_order =
+		iwl_trans_get_rb_size_order(trans_pcie->rx_buf_size);
 
 	trans_pcie->wide_cmd_header = trans_cfg->wide_cmd_header;
-	trans_pcie->command_names = trans_cfg->command_names;
 	trans_pcie->bc_table_dword = trans_cfg->bc_table_dword;
 	trans_pcie->scd_set_active = trans_cfg->scd_set_active;
+	trans_pcie->sw_csum_tx = trans_cfg->sw_csum_tx;
+
+	trans->command_groups = trans_cfg->command_groups;
+	trans->command_groups_size = trans_cfg->command_groups_size;
 
 	/* init ref_count to 1 (should be cleared when ucode is loaded) */
 	trans_pcie->ref_count = 1;
@@ -1464,6 +1504,7 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 void iwl_trans_pcie_free(struct iwl_trans *trans)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int i;
 
 	synchronize_irq(trans_pcie->pci_dev->irq);
 
@@ -1483,6 +1524,15 @@ void iwl_trans_pcie_free(struct iwl_trans *trans)
 
 	iwl_pcie_free_fw_monitor(trans);
 
+	for_each_possible_cpu(i) {
+		struct iwl_tso_hdr_page *p =
+			per_cpu_ptr(trans_pcie->tso_hdr_page, i);
+
+		if (p->page)
+			__free_page(p->page);
+	}
+
+	free_percpu(trans_pcie->tso_hdr_page);
 	iwl_trans_free(trans);
 }
 
@@ -1494,8 +1544,8 @@ static void iwl_trans_pcie_set_pmi(struct iwl_trans *trans, bool state)
 		clear_bit(STATUS_TPOWER_PMI, &trans->status);
 }
 
-static bool iwl_trans_pcie_grab_nic_access(struct iwl_trans *trans, bool silent,
-						unsigned long *flags)
+static bool iwl_trans_pcie_grab_nic_access(struct iwl_trans *trans,
+					   unsigned long *flags)
 {
 	int ret;
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -1536,14 +1586,11 @@ static bool iwl_trans_pcie_grab_nic_access(struct iwl_trans *trans, bool silent,
 			    CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP), 15000);
 	if (unlikely(ret < 0)) {
 		iwl_write32(trans, CSR_RESET, CSR_RESET_REG_FLAG_FORCE_NMI);
-		if (!silent) {
-			u32 val = iwl_read32(trans, CSR_GP_CNTRL);
-			WARN_ONCE(1,
-				  "Timeout waiting for hardware access (CSR_GP_CNTRL 0x%08x)\n",
-				  val);
-			spin_unlock_irqrestore(&trans_pcie->reg_lock, *flags);
-			return false;
-		}
+		WARN_ONCE(1,
+			  "Timeout waiting for hardware access (CSR_GP_CNTRL 0x%08x)\n",
+			  iwl_read32(trans, CSR_GP_CNTRL));
+		spin_unlock_irqrestore(&trans_pcie->reg_lock, *flags);
+		return false;
 	}
 
 out:
@@ -1591,7 +1638,7 @@ static int iwl_trans_pcie_read_mem(struct iwl_trans *trans, u32 addr,
 	int offs, ret = 0;
 	u32 *vals = buf;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
 		iwl_write32(trans, HBUS_TARG_MEM_RADDR, addr);
 		for (offs = 0; offs < dwords; offs++)
 			vals[offs] = iwl_read32(trans, HBUS_TARG_MEM_RDAT);
@@ -1609,7 +1656,7 @@ static int iwl_trans_pcie_write_mem(struct iwl_trans *trans, u32 addr,
 	int offs, ret = 0;
 	const u32 *vals = buf;
 
-	if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+	if (iwl_trans_grab_nic_access(trans, &flags)) {
 		iwl_write32(trans, HBUS_TARG_MEM_WADDR, addr);
 		for (offs = 0; offs < dwords; offs++)
 			iwl_write32(trans, HBUS_TARG_MEM_WDAT,
@@ -1675,6 +1722,33 @@ next_queue:
 	}
 }
 
+static void iwl_trans_pcie_block_txq_ptrs(struct iwl_trans *trans, bool block)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int i;
+
+	for (i = 0; i < trans->cfg->base_params->num_of_queues; i++) {
+		struct iwl_txq *txq = &trans_pcie->txq[i];
+
+		if (i == trans_pcie->cmd_queue)
+			continue;
+
+		spin_lock_bh(&txq->lock);
+
+		if (!block && !(WARN_ON_ONCE(!txq->block))) {
+			txq->block--;
+			if (!txq->block) {
+				iwl_write32(trans, HBUS_TARG_WRPTR,
+					    txq->q.write_ptr | (i << 8));
+			}
+		} else if (block) {
+			txq->block++;
+		}
+
+		spin_unlock_bh(&txq->lock);
+	}
+}
+
 #define IWL_FLUSH_WAIT_MS	2000
 
 static int iwl_trans_pcie_wait_txq_empty(struct iwl_trans *trans, u32 txq_bm)
@@ -2109,13 +2183,11 @@ DEBUGFS_READ_FILE_OPS(rx_queue);
 DEBUGFS_READ_FILE_OPS(tx_queue);
 DEBUGFS_WRITE_FILE_OPS(csr);
 
-/*
- * Create the debugfs files and directories
- *
- */
-static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
-					 struct dentry *dir)
+/* Create the debugfs files and directories */
+int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans)
 {
+	struct dentry *dir = trans->dbgfs_dir;
+
 	DEBUGFS_ADD_FILE(rx_queue, dir, S_IRUSR);
 	DEBUGFS_ADD_FILE(tx_queue, dir, S_IRUSR);
 	DEBUGFS_ADD_FILE(interrupt, dir, S_IWUSR | S_IRUSR);
@@ -2127,12 +2199,6 @@ err:
 	IWL_ERR(trans, "failed to create the trans debugfs entry\n");
 	return -ENOMEM;
 }
-#else
-static int iwl_trans_pcie_dbgfs_register(struct iwl_trans *trans,
-					 struct dentry *dir)
-{
-	return 0;
-}
 #endif /*CONFIG_IWLWIFI_DEBUGFS */
 
 static u32 iwl_trans_pcie_get_cmdlen(struct iwl_tfd *tfd)
@@ -2146,144 +2212,6 @@ static u32 iwl_trans_pcie_get_cmdlen(struct iwl_tfd *tfd)
 	return cmdlen;
 }
 
-static const struct {
-	u32 start, end;
-} iwl_prph_dump_addr[] = {
-	{ .start = 0x00a00000, .end = 0x00a00000 },
-	{ .start = 0x00a0000c, .end = 0x00a00024 },
-	{ .start = 0x00a0002c, .end = 0x00a0003c },
-	{ .start = 0x00a00410, .end = 0x00a00418 },
-	{ .start = 0x00a00420, .end = 0x00a00420 },
-	{ .start = 0x00a00428, .end = 0x00a00428 },
-	{ .start = 0x00a00430, .end = 0x00a0043c },
-	{ .start = 0x00a00444, .end = 0x00a00444 },
-	{ .start = 0x00a004c0, .end = 0x00a004cc },
-	{ .start = 0x00a004d8, .end = 0x00a004d8 },
-	{ .start = 0x00a004e0, .end = 0x00a004f0 },
-	{ .start = 0x00a00840, .end = 0x00a00840 },
-	{ .start = 0x00a00850, .end = 0x00a00858 },
-	{ .start = 0x00a01004, .end = 0x00a01008 },
-	{ .start = 0x00a01010, .end = 0x00a01010 },
-	{ .start = 0x00a01018, .end = 0x00a01018 },
-	{ .start = 0x00a01024, .end = 0x00a01024 },
-	{ .start = 0x00a0102c, .end = 0x00a01034 },
-	{ .start = 0x00a0103c, .end = 0x00a01040 },
-	{ .start = 0x00a01048, .end = 0x00a01094 },
-	{ .start = 0x00a01c00, .end = 0x00a01c20 },
-	{ .start = 0x00a01c58, .end = 0x00a01c58 },
-	{ .start = 0x00a01c7c, .end = 0x00a01c7c },
-	{ .start = 0x00a01c28, .end = 0x00a01c54 },
-	{ .start = 0x00a01c5c, .end = 0x00a01c5c },
-	{ .start = 0x00a01c60, .end = 0x00a01cdc },
-	{ .start = 0x00a01ce0, .end = 0x00a01d0c },
-	{ .start = 0x00a01d18, .end = 0x00a01d20 },
-	{ .start = 0x00a01d2c, .end = 0x00a01d30 },
-	{ .start = 0x00a01d40, .end = 0x00a01d5c },
-	{ .start = 0x00a01d80, .end = 0x00a01d80 },
-	{ .start = 0x00a01d98, .end = 0x00a01d9c },
-	{ .start = 0x00a01da8, .end = 0x00a01da8 },
-	{ .start = 0x00a01db8, .end = 0x00a01df4 },
-	{ .start = 0x00a01dc0, .end = 0x00a01dfc },
-	{ .start = 0x00a01e00, .end = 0x00a01e2c },
-	{ .start = 0x00a01e40, .end = 0x00a01e60 },
-	{ .start = 0x00a01e68, .end = 0x00a01e6c },
-	{ .start = 0x00a01e74, .end = 0x00a01e74 },
-	{ .start = 0x00a01e84, .end = 0x00a01e90 },
-	{ .start = 0x00a01e9c, .end = 0x00a01ec4 },
-	{ .start = 0x00a01ed0, .end = 0x00a01ee0 },
-	{ .start = 0x00a01f00, .end = 0x00a01f1c },
-	{ .start = 0x00a01f44, .end = 0x00a01ffc },
-	{ .start = 0x00a02000, .end = 0x00a02048 },
-	{ .start = 0x00a02068, .end = 0x00a020f0 },
-	{ .start = 0x00a02100, .end = 0x00a02118 },
-	{ .start = 0x00a02140, .end = 0x00a0214c },
-	{ .start = 0x00a02168, .end = 0x00a0218c },
-	{ .start = 0x00a021c0, .end = 0x00a021c0 },
-	{ .start = 0x00a02400, .end = 0x00a02410 },
-	{ .start = 0x00a02418, .end = 0x00a02420 },
-	{ .start = 0x00a02428, .end = 0x00a0242c },
-	{ .start = 0x00a02434, .end = 0x00a02434 },
-	{ .start = 0x00a02440, .end = 0x00a02460 },
-	{ .start = 0x00a02468, .end = 0x00a024b0 },
-	{ .start = 0x00a024c8, .end = 0x00a024cc },
-	{ .start = 0x00a02500, .end = 0x00a02504 },
-	{ .start = 0x00a0250c, .end = 0x00a02510 },
-	{ .start = 0x00a02540, .end = 0x00a02554 },
-	{ .start = 0x00a02580, .end = 0x00a025f4 },
-	{ .start = 0x00a02600, .end = 0x00a0260c },
-	{ .start = 0x00a02648, .end = 0x00a02650 },
-	{ .start = 0x00a02680, .end = 0x00a02680 },
-	{ .start = 0x00a026c0, .end = 0x00a026d0 },
-	{ .start = 0x00a02700, .end = 0x00a0270c },
-	{ .start = 0x00a02804, .end = 0x00a02804 },
-	{ .start = 0x00a02818, .end = 0x00a0281c },
-	{ .start = 0x00a02c00, .end = 0x00a02db4 },
-	{ .start = 0x00a02df4, .end = 0x00a02fb0 },
-	{ .start = 0x00a03000, .end = 0x00a03014 },
-	{ .start = 0x00a0301c, .end = 0x00a0302c },
-	{ .start = 0x00a03034, .end = 0x00a03038 },
-	{ .start = 0x00a03040, .end = 0x00a03048 },
-	{ .start = 0x00a03060, .end = 0x00a03068 },
-	{ .start = 0x00a03070, .end = 0x00a03074 },
-	{ .start = 0x00a0307c, .end = 0x00a0307c },
-	{ .start = 0x00a03080, .end = 0x00a03084 },
-	{ .start = 0x00a0308c, .end = 0x00a03090 },
-	{ .start = 0x00a03098, .end = 0x00a03098 },
-	{ .start = 0x00a030a0, .end = 0x00a030a0 },
-	{ .start = 0x00a030a8, .end = 0x00a030b4 },
-	{ .start = 0x00a030bc, .end = 0x00a030bc },
-	{ .start = 0x00a030c0, .end = 0x00a0312c },
-	{ .start = 0x00a03c00, .end = 0x00a03c5c },
-	{ .start = 0x00a04400, .end = 0x00a04454 },
-	{ .start = 0x00a04460, .end = 0x00a04474 },
-	{ .start = 0x00a044c0, .end = 0x00a044ec },
-	{ .start = 0x00a04500, .end = 0x00a04504 },
-	{ .start = 0x00a04510, .end = 0x00a04538 },
-	{ .start = 0x00a04540, .end = 0x00a04548 },
-	{ .start = 0x00a04560, .end = 0x00a0457c },
-	{ .start = 0x00a04590, .end = 0x00a04598 },
-	{ .start = 0x00a045c0, .end = 0x00a045f4 },
-};
-
-static u32 iwl_trans_pcie_dump_prph(struct iwl_trans *trans,
-				    struct iwl_fw_error_dump_data **data)
-{
-	struct iwl_fw_error_dump_prph *prph;
-	unsigned long flags;
-	u32 prph_len = 0, i;
-
-	if (!iwl_trans_grab_nic_access(trans, false, &flags))
-		return 0;
-
-	for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr); i++) {
-		/* The range includes both boundaries */
-		int num_bytes_in_chunk = iwl_prph_dump_addr[i].end -
-			 iwl_prph_dump_addr[i].start + 4;
-		int reg;
-		__le32 *val;
-
-		prph_len += sizeof(**data) + sizeof(*prph) + num_bytes_in_chunk;
-
-		(*data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_PRPH);
-		(*data)->len = cpu_to_le32(sizeof(*prph) +
-					num_bytes_in_chunk);
-		prph = (void *)(*data)->data;
-		prph->prph_start = cpu_to_le32(iwl_prph_dump_addr[i].start);
-		val = (void *)prph->data;
-
-		for (reg = iwl_prph_dump_addr[i].start;
-		     reg <= iwl_prph_dump_addr[i].end;
-		     reg += 4)
-			*val++ = cpu_to_le32(iwl_trans_pcie_read_prph(trans,
-								      reg));
-		*data = iwl_fw_error_next_data(*data);
-	}
-
-	iwl_trans_release_nic_access(trans, &flags);
-
-	return prph_len;
-}
-
 static u32 iwl_trans_pcie_dump_rbs(struct iwl_trans *trans,
 				   struct iwl_fw_error_dump_data **data,
 				   int allocated_rb_nums)
@@ -2354,7 +2282,7 @@ static u32 iwl_trans_pcie_fh_regs_dump(struct iwl_trans *trans,
 	__le32 *val;
 	int i;
 
-	if (!iwl_trans_grab_nic_access(trans, false, &flags))
+	if (!iwl_trans_grab_nic_access(trans, &flags))
 		return 0;
 
 	(*data)->type = cpu_to_le32(IWL_FW_ERROR_DUMP_FH_REGS);
@@ -2381,13 +2309,14 @@ iwl_trans_pci_dump_marbh_monitor(struct iwl_trans *trans,
 	unsigned long flags;
 	u32 i;
 
-	if (!iwl_trans_grab_nic_access(trans, false, &flags))
+	if (!iwl_trans_grab_nic_access(trans, &flags))
 		return 0;
 
-	__iwl_write_prph(trans, MON_DMARB_RD_CTL_ADDR, 0x1);
+	iwl_write_prph_no_grab(trans, MON_DMARB_RD_CTL_ADDR, 0x1);
 	for (i = 0; i < buf_size_in_dwords; i++)
-		buffer[i] = __iwl_read_prph(trans, MON_DMARB_RD_DATA_ADDR);
-	__iwl_write_prph(trans, MON_DMARB_RD_CTL_ADDR, 0x0);
+		buffer[i] = iwl_read_prph_no_grab(trans,
+				MON_DMARB_RD_DATA_ADDR);
+	iwl_write_prph_no_grab(trans, MON_DMARB_RD_CTL_ADDR, 0x0);
 
 	iwl_trans_release_nic_access(trans, &flags);
 
@@ -2474,7 +2403,7 @@ iwl_trans_pcie_dump_monitor(struct iwl_trans *trans,
 
 static struct iwl_trans_dump_data
 *iwl_trans_pcie_dump_data(struct iwl_trans *trans,
-			  struct iwl_fw_dbg_trigger_tlv *trigger)
+			  const struct iwl_fw_dbg_trigger_tlv *trigger)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	struct iwl_fw_error_dump_data *data;
@@ -2535,16 +2464,6 @@ static struct iwl_trans_dump_data
 	/* CSR registers */
 	len += sizeof(*data) + IWL_CSR_TO_DUMP;
 
-	/* PRPH registers */
-	for (i = 0; i < ARRAY_SIZE(iwl_prph_dump_addr); i++) {
-		/* The range includes both boundaries */
-		int num_bytes_in_chunk = iwl_prph_dump_addr[i].end -
-			iwl_prph_dump_addr[i].start + 4;
-
-		len += sizeof(*data) + sizeof(struct iwl_fw_error_dump_prph) +
-		       num_bytes_in_chunk;
-	}
-
 	/* FH registers */
 	len += sizeof(*data) + (FH_MEM_UPPER_BOUND - FH_MEM_LOWER_BOUND);
 
@@ -2592,7 +2511,6 @@ static struct iwl_trans_dump_data
 	len += sizeof(*data);
 	data = iwl_fw_error_next_data(data);
 
-	len += iwl_trans_pcie_dump_prph(trans, &data);
 	len += iwl_trans_pcie_dump_csr(trans, &data);
 	len += iwl_trans_pcie_fh_regs_dump(trans, &data);
 	if (dump_rbs)
@@ -2623,10 +2541,9 @@ static const struct iwl_trans_ops trans_ops_pcie = {
 	.txq_disable = iwl_trans_pcie_txq_disable,
 	.txq_enable = iwl_trans_pcie_txq_enable,
 
-	.dbgfs_register = iwl_trans_pcie_dbgfs_register,
-
 	.wait_tx_queue_empty = iwl_trans_pcie_wait_txq_empty,
 	.freeze_txq_timer = iwl_trans_pcie_freeze_txq_timer,
+	.block_txq_ptrs = iwl_trans_pcie_block_txq_ptrs,
 
 	.write8 = iwl_trans_pcie_write8,
 	.write32 = iwl_trans_pcie_write32,
@@ -2671,6 +2588,11 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	spin_lock_init(&trans_pcie->ref_lock);
 	mutex_init(&trans_pcie->mutex);
 	init_waitqueue_head(&trans_pcie->ucode_write_waitq);
+	trans_pcie->tso_hdr_page = alloc_percpu(struct iwl_tso_hdr_page);
+	if (!trans_pcie->tso_hdr_page) {
+		ret = -ENOMEM;
+		goto out_no_pci;
+	}
 
 	ret = pci_enable_device(pdev);
 	if (ret)
@@ -2772,13 +2694,13 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 			goto out_pci_disable_msi;
 		}
 
-		if (iwl_trans_grab_nic_access(trans, false, &flags)) {
+		if (iwl_trans_grab_nic_access(trans, &flags)) {
 			u32 hw_step;
 
-			hw_step = __iwl_read_prph(trans, WFPM_CTRL_REG);
+			hw_step = iwl_read_prph_no_grab(trans, WFPM_CTRL_REG);
 			hw_step |= ENABLE_WFPM;
-			__iwl_write_prph(trans, WFPM_CTRL_REG, hw_step);
-			hw_step = __iwl_read_prph(trans, AUX_MISC_REG);
+			iwl_write_prph_no_grab(trans, WFPM_CTRL_REG, hw_step);
+			hw_step = iwl_read_prph_no_grab(trans, AUX_MISC_REG);
 			hw_step = (hw_step >> HW_STEP_LOCATION_BITS) & 0xF;
 			if (hw_step == 0x3)
 				trans->hw_rev = (trans->hw_rev & 0xFFFFFFF3) |
@@ -2807,7 +2729,6 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	}
 
 	trans_pcie->inta_mask = CSR_INI_SET_MASK;
-	trans->d0i3_mode = IWL_D0I3_MODE_ON_SUSPEND;
 
 	return trans;
 
@@ -2820,6 +2741,7 @@ out_pci_release_regions:
 out_pci_disable_device:
 	pci_disable_device(pdev);
 out_no_pci:
+	free_percpu(trans_pcie->tso_hdr_page);
 	iwl_trans_free(trans);
 	return ERR_PTR(ret);
 }
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/intel/iwlwifi/pcie/tx.c
index a8c8a4a7420b..5262028b5505 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/intel/iwlwifi/pcie/tx.c
@@ -23,13 +23,17 @@
  * file called LICENSE.
  *
  * Contact Information:
- *  Intel Linux Wireless <ilw@linux.intel.com>
+ *  Intel Linux Wireless <linuxwifi@intel.com>
  * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
  *
  *****************************************************************************/
 #include <linux/etherdevice.h>
+#include <linux/ieee80211.h>
 #include <linux/slab.h>
 #include <linux/sched.h>
+#include <net/ip6_checksum.h>
+#include <net/tso.h>
+#include <net/ip6_checksum.h>
 
 #include "iwl-debug.h"
 #include "iwl-csr.h"
@@ -318,7 +322,9 @@ static void iwl_pcie_txq_inc_wr_ptr(struct iwl_trans *trans,
 	 * trying to tx (during RFKILL, we're not trying to tx).
 	 */
 	IWL_DEBUG_TX(trans, "Q:%d WR: 0x%x\n", txq_id, txq->q.write_ptr);
-	iwl_write32(trans, HBUS_TARG_WRPTR, txq->q.write_ptr | (txq_id << 8));
+	if (!txq->block)
+		iwl_write32(trans, HBUS_TARG_WRPTR,
+			    txq->q.write_ptr | (txq_id << 8));
 }
 
 void iwl_pcie_txq_check_wrptrs(struct iwl_trans *trans)
@@ -576,6 +582,19 @@ static int iwl_pcie_txq_init(struct iwl_trans *trans, struct iwl_txq *txq,
 	return 0;
 }
 
+static void iwl_pcie_free_tso_page(struct sk_buff *skb)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	if (info->driver_data[IWL_TRANS_FIRST_DRIVER_DATA]) {
+		struct page *page =
+			info->driver_data[IWL_TRANS_FIRST_DRIVER_DATA];
+
+		__free_page(page);
+		info->driver_data[IWL_TRANS_FIRST_DRIVER_DATA] = NULL;
+	}
+}
+
 /*
  * iwl_pcie_txq_unmap -  Unmap any remaining DMA mappings and free skb's
  */
@@ -589,6 +608,15 @@ static void iwl_pcie_txq_unmap(struct iwl_trans *trans, int txq_id)
 	while (q->write_ptr != q->read_ptr) {
 		IWL_DEBUG_TX_REPLY(trans, "Q %d Free %d\n",
 				   txq_id, q->read_ptr);
+
+		if (txq_id != trans_pcie->cmd_queue) {
+			struct sk_buff *skb = txq->entries[q->read_ptr].skb;
+
+			if (WARN_ON_ONCE(!skb))
+				continue;
+
+			iwl_pcie_free_tso_page(skb);
+		}
 		iwl_pcie_txq_free_tfd(trans, txq);
 		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr);
 	}
@@ -742,7 +770,7 @@ static void iwl_pcie_tx_stop_fh(struct iwl_trans *trans)
 
 	spin_lock(&trans_pcie->irq_lock);
 
-	if (!iwl_trans_grab_nic_access(trans, false, &flags))
+	if (!iwl_trans_grab_nic_access(trans, &flags))
 		goto out;
 
 	/* Stop each Tx DMA channel */
@@ -1006,11 +1034,14 @@ void iwl_trans_pcie_reclaim(struct iwl_trans *trans, int txq_id, int ssn,
 	for (;
 	     q->read_ptr != tfd_num;
 	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr)) {
+		struct sk_buff *skb = txq->entries[txq->q.read_ptr].skb;
 
-		if (WARN_ON_ONCE(txq->entries[txq->q.read_ptr].skb == NULL))
+		if (WARN_ON_ONCE(!skb))
 			continue;
 
-		__skb_queue_tail(skbs, txq->entries[txq->q.read_ptr].skb);
+		iwl_pcie_free_tso_page(skb);
+
+		__skb_queue_tail(skbs, skb);
 
 		txq->entries[txq->q.read_ptr].skb = NULL;
 
@@ -1411,7 +1442,8 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 	 */
 	if (WARN(copy_size > TFD_MAX_PAYLOAD_SIZE,
 		 "Command %s (%#x) is too large (%d bytes)\n",
-		 get_cmd_string(trans_pcie, cmd->id), cmd->id, copy_size)) {
+		 iwl_get_cmd_string(trans, cmd->id),
+		 cmd->id, copy_size)) {
 		idx = -EINVAL;
 		goto free_dup_buf;
 	}
@@ -1501,7 +1533,7 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 
 	IWL_DEBUG_HC(trans,
 		     "Sending command %s (%.2x.%.2x), seq: 0x%04X, %d bytes at %d[%d]:%d\n",
-		     get_cmd_string(trans_pcie, out_cmd->hdr.cmd),
+		     iwl_get_cmd_string(trans, cmd->id),
 		     group_id, out_cmd->hdr.cmd,
 		     le16_to_cpu(out_cmd->hdr.sequence),
 		     cmd_size, q->write_ptr, idx, trans_pcie->cmd_queue);
@@ -1591,16 +1623,14 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 /*
  * iwl_pcie_hcmd_complete - Pull unused buffers off the queue and reclaim them
  * @rxb: Rx buffer to reclaim
- *
- * If an Rx buffer has an async callback associated with it the callback
- * will be executed.  The attached skb (if present) will only be freed
- * if the callback returns 1
  */
 void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 			    struct iwl_rx_cmd_buffer *rxb)
 {
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	u8 group_id = iwl_cmd_groupid(pkt->hdr.group_id);
+	u32 cmd_id;
 	int txq_id = SEQ_TO_QUEUE(sequence);
 	int index = SEQ_TO_INDEX(sequence);
 	int cmd_index;
@@ -1626,6 +1656,7 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 	cmd_index = get_cmd_index(&txq->q, index);
 	cmd = txq->entries[cmd_index].cmd;
 	meta = &txq->entries[cmd_index].meta;
+	cmd_id = iwl_cmd_id(cmd->hdr.cmd, group_id, 0);
 
 	iwl_pcie_tfd_unmap(trans, meta, &txq->tfds[index]);
 
@@ -1638,17 +1669,20 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 		meta->source->_rx_page_order = trans_pcie->rx_page_order;
 	}
 
+	if (meta->flags & CMD_WANT_ASYNC_CALLBACK)
+		iwl_op_mode_async_cb(trans->op_mode, cmd);
+
 	iwl_pcie_cmdq_reclaim(trans, txq_id, index);
 
 	if (!(meta->flags & CMD_ASYNC)) {
 		if (!test_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status)) {
 			IWL_WARN(trans,
 				 "HCMD_ACTIVE already clear for command %s\n",
-				 get_cmd_string(trans_pcie, cmd->hdr.cmd));
+				 iwl_get_cmd_string(trans, cmd_id));
 		}
 		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 		IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
-			       get_cmd_string(trans_pcie, cmd->hdr.cmd));
+			       iwl_get_cmd_string(trans, cmd_id));
 		wake_up(&trans_pcie->wait_command_queue);
 	}
 
@@ -1662,7 +1696,6 @@ void iwl_pcie_hcmd_complete(struct iwl_trans *trans,
 static int iwl_pcie_send_hcmd_async(struct iwl_trans *trans,
 				    struct iwl_host_cmd *cmd)
 {
-	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	int ret;
 
 	/* An asynchronous command can not expect an SKB to be set. */
@@ -1673,7 +1706,7 @@ static int iwl_pcie_send_hcmd_async(struct iwl_trans *trans,
 	if (ret < 0) {
 		IWL_ERR(trans,
 			"Error sending %s: enqueue_hcmd failed: %d\n",
-			get_cmd_string(trans_pcie, cmd->id), ret);
+			iwl_get_cmd_string(trans, cmd->id), ret);
 		return ret;
 	}
 	return 0;
@@ -1687,16 +1720,16 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 	int ret;
 
 	IWL_DEBUG_INFO(trans, "Attempting to send sync command %s\n",
-		       get_cmd_string(trans_pcie, cmd->id));
+		       iwl_get_cmd_string(trans, cmd->id));
 
 	if (WARN(test_and_set_bit(STATUS_SYNC_HCMD_ACTIVE,
 				  &trans->status),
 		 "Command %s: a command is already active!\n",
-		 get_cmd_string(trans_pcie, cmd->id)))
+		 iwl_get_cmd_string(trans, cmd->id)))
 		return -EIO;
 
 	IWL_DEBUG_INFO(trans, "Setting HCMD_ACTIVE for command %s\n",
-		       get_cmd_string(trans_pcie, cmd->id));
+		       iwl_get_cmd_string(trans, cmd->id));
 
 	cmd_idx = iwl_pcie_enqueue_hcmd(trans, cmd);
 	if (cmd_idx < 0) {
@@ -1704,7 +1737,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 		IWL_ERR(trans,
 			"Error sending %s: enqueue_hcmd failed: %d\n",
-			get_cmd_string(trans_pcie, cmd->id), ret);
+			iwl_get_cmd_string(trans, cmd->id), ret);
 		return ret;
 	}
 
@@ -1717,7 +1750,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 		struct iwl_queue *q = &txq->q;
 
 		IWL_ERR(trans, "Error sending %s: time out after %dms.\n",
-			get_cmd_string(trans_pcie, cmd->id),
+			iwl_get_cmd_string(trans, cmd->id),
 			jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
 
 		IWL_ERR(trans, "Current CMD queue read_ptr %d write_ptr %d\n",
@@ -1725,7 +1758,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 
 		clear_bit(STATUS_SYNC_HCMD_ACTIVE, &trans->status);
 		IWL_DEBUG_INFO(trans, "Clearing HCMD_ACTIVE for command %s\n",
-			       get_cmd_string(trans_pcie, cmd->id));
+			       iwl_get_cmd_string(trans, cmd->id));
 		ret = -ETIMEDOUT;
 
 		iwl_force_nmi(trans);
@@ -1736,7 +1769,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 
 	if (test_bit(STATUS_FW_ERROR, &trans->status)) {
 		IWL_ERR(trans, "FW error in SYNC CMD %s\n",
-			get_cmd_string(trans_pcie, cmd->id));
+			iwl_get_cmd_string(trans, cmd->id));
 		dump_stack();
 		ret = -EIO;
 		goto cancel;
@@ -1751,7 +1784,7 @@ static int iwl_pcie_send_hcmd_sync(struct iwl_trans *trans,
 
 	if ((cmd->flags & CMD_WANT_SKB) && !cmd->resp_pkt) {
 		IWL_ERR(trans, "Error: Response NULL in '%s'\n",
-			get_cmd_string(trans_pcie, cmd->id));
+			iwl_get_cmd_string(trans, cmd->id));
 		ret = -EIO;
 		goto cancel;
 	}
@@ -1794,6 +1827,305 @@ int iwl_trans_pcie_send_hcmd(struct iwl_trans *trans, struct iwl_host_cmd *cmd)
 	return iwl_pcie_send_hcmd_sync(trans, cmd);
 }
 
+static int iwl_fill_data_tbs(struct iwl_trans *trans, struct sk_buff *skb,
+			     struct iwl_txq *txq, u8 hdr_len,
+			     struct iwl_cmd_meta *out_meta,
+			     struct iwl_device_cmd *dev_cmd, u16 tb1_len)
+{
+	struct iwl_queue *q = &txq->q;
+	u16 tb2_len;
+	int i;
+
+	/*
+	 * Set up TFD's third entry to point directly to remainder
+	 * of skb's head, if any
+	 */
+	tb2_len = skb_headlen(skb) - hdr_len;
+
+	if (tb2_len > 0) {
+		dma_addr_t tb2_phys = dma_map_single(trans->dev,
+						     skb->data + hdr_len,
+						     tb2_len, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(trans->dev, tb2_phys))) {
+			iwl_pcie_tfd_unmap(trans, out_meta,
+					   &txq->tfds[q->write_ptr]);
+			return -EINVAL;
+		}
+		iwl_pcie_txq_build_tfd(trans, txq, tb2_phys, tb2_len, false);
+	}
+
+	/* set up the remaining entries to point to the data */
+	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
+		const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
+		dma_addr_t tb_phys;
+		int tb_idx;
+
+		if (!skb_frag_size(frag))
+			continue;
+
+		tb_phys = skb_frag_dma_map(trans->dev, frag, 0,
+					   skb_frag_size(frag), DMA_TO_DEVICE);
+
+		if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
+			iwl_pcie_tfd_unmap(trans, out_meta,
+					   &txq->tfds[q->write_ptr]);
+			return -EINVAL;
+		}
+		tb_idx = iwl_pcie_txq_build_tfd(trans, txq, tb_phys,
+						skb_frag_size(frag), false);
+
+		out_meta->flags |= BIT(tb_idx + CMD_TB_BITMAP_POS);
+	}
+
+	trace_iwlwifi_dev_tx(trans->dev, skb,
+			     &txq->tfds[txq->q.write_ptr],
+			     sizeof(struct iwl_tfd),
+			     &dev_cmd->hdr, IWL_HCMD_SCRATCHBUF_SIZE + tb1_len,
+			     skb->data + hdr_len, tb2_len);
+	trace_iwlwifi_dev_tx_data(trans->dev, skb,
+				  hdr_len, skb->len - hdr_len);
+	return 0;
+}
+
+#ifdef CONFIG_INET
+static struct iwl_tso_hdr_page *
+get_page_hdr(struct iwl_trans *trans, size_t len)
+{
+	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	struct iwl_tso_hdr_page *p = this_cpu_ptr(trans_pcie->tso_hdr_page);
+
+	if (!p->page)
+		goto alloc;
+
+	/* enough room on this page */
+	if (p->pos + len < (u8 *)page_address(p->page) + PAGE_SIZE)
+		return p;
+
+	/* We don't have enough room on this page, get a new one. */
+	__free_page(p->page);
+
+alloc:
+	p->page = alloc_page(GFP_ATOMIC);
+	if (!p->page)
+		return NULL;
+	p->pos = page_address(p->page);
+	return p;
+}
+
+static void iwl_compute_pseudo_hdr_csum(void *iph, struct tcphdr *tcph,
+					bool ipv6, unsigned int len)
+{
+	if (ipv6) {
+		struct ipv6hdr *iphv6 = iph;
+
+		tcph->check = ~csum_ipv6_magic(&iphv6->saddr, &iphv6->daddr,
+					       len + tcph->doff * 4,
+					       IPPROTO_TCP, 0);
+	} else {
+		struct iphdr *iphv4 = iph;
+
+		ip_send_check(iphv4);
+		tcph->check = ~csum_tcpudp_magic(iphv4->saddr, iphv4->daddr,
+						 len + tcph->doff * 4,
+						 IPPROTO_TCP, 0);
+	}
+}
+
+static int iwl_fill_data_tbs_amsdu(struct iwl_trans *trans, struct sk_buff *skb,
+				   struct iwl_txq *txq, u8 hdr_len,
+				   struct iwl_cmd_meta *out_meta,
+				   struct iwl_device_cmd *dev_cmd, u16 tb1_len)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct iwl_trans_pcie *trans_pcie = txq->trans_pcie;
+	struct ieee80211_hdr *hdr = (void *)skb->data;
+	unsigned int snap_ip_tcp_hdrlen, ip_hdrlen, total_len, hdr_room;
+	unsigned int mss = skb_shinfo(skb)->gso_size;
+	struct iwl_queue *q = &txq->q;
+	u16 length, iv_len, amsdu_pad;
+	u8 *start_hdr;
+	struct iwl_tso_hdr_page *hdr_page;
+	int ret;
+	struct tso_t tso;
+
+	/* if the packet is protected, then it must be CCMP or GCMP */
+	BUILD_BUG_ON(IEEE80211_CCMP_HDR_LEN != IEEE80211_GCMP_HDR_LEN);
+	iv_len = ieee80211_has_protected(hdr->frame_control) ?
+		IEEE80211_CCMP_HDR_LEN : 0;
+
+	trace_iwlwifi_dev_tx(trans->dev, skb,
+			     &txq->tfds[txq->q.write_ptr],
+			     sizeof(struct iwl_tfd),
+			     &dev_cmd->hdr, IWL_HCMD_SCRATCHBUF_SIZE + tb1_len,
+			     NULL, 0);
+
+	ip_hdrlen = skb_transport_header(skb) - skb_network_header(skb);
+	snap_ip_tcp_hdrlen = 8 + ip_hdrlen + tcp_hdrlen(skb);
+	total_len = skb->len - snap_ip_tcp_hdrlen - hdr_len - iv_len;
+	amsdu_pad = 0;
+
+	/* total amount of header we may need for this A-MSDU */
+	hdr_room = DIV_ROUND_UP(total_len, mss) *
+		(3 + snap_ip_tcp_hdrlen + sizeof(struct ethhdr)) + iv_len;
+
+	/* Our device supports 9 segments at most, it will fit in 1 page */
+	hdr_page = get_page_hdr(trans, hdr_room);
+	if (!hdr_page)
+		return -ENOMEM;
+
+	get_page(hdr_page->page);
+	start_hdr = hdr_page->pos;
+	info->driver_data[IWL_TRANS_FIRST_DRIVER_DATA] = hdr_page->page;
+	memcpy(hdr_page->pos, skb->data + hdr_len, iv_len);
+	hdr_page->pos += iv_len;
+
+	/*
+	 * Pull the ieee80211 header + IV to be able to use TSO core,
+	 * we will restore it for the tx_status flow.
+	 */
+	skb_pull(skb, hdr_len + iv_len);
+
+	tso_start(skb, &tso);
+
+	while (total_len) {
+		/* this is the data left for this subframe */
+		unsigned int data_left =
+			min_t(unsigned int, mss, total_len);
+		struct sk_buff *csum_skb = NULL;
+		unsigned int hdr_tb_len;
+		dma_addr_t hdr_tb_phys;
+		struct tcphdr *tcph;
+		u8 *iph;
+
+		total_len -= data_left;
+
+		memset(hdr_page->pos, 0, amsdu_pad);
+		hdr_page->pos += amsdu_pad;
+		amsdu_pad = (4 - (sizeof(struct ethhdr) + snap_ip_tcp_hdrlen +
+				  data_left)) & 0x3;
+		ether_addr_copy(hdr_page->pos, ieee80211_get_DA(hdr));
+		hdr_page->pos += ETH_ALEN;
+		ether_addr_copy(hdr_page->pos, ieee80211_get_SA(hdr));
+		hdr_page->pos += ETH_ALEN;
+
+		length = snap_ip_tcp_hdrlen + data_left;
+		*((__be16 *)hdr_page->pos) = cpu_to_be16(length);
+		hdr_page->pos += sizeof(length);
+
+		/*
+		 * This will copy the SNAP as well which will be considered
+		 * as MAC header.
+		 */
+		tso_build_hdr(skb, hdr_page->pos, &tso, data_left, !total_len);
+		iph = hdr_page->pos + 8;
+		tcph = (void *)(iph + ip_hdrlen);
+
+		/* For testing on current hardware only */
+		if (trans_pcie->sw_csum_tx) {
+			csum_skb = alloc_skb(data_left + tcp_hdrlen(skb),
+					     GFP_ATOMIC);
+			if (!csum_skb) {
+				ret = -ENOMEM;
+				goto out_unmap;
+			}
+
+			iwl_compute_pseudo_hdr_csum(iph, tcph,
+						    skb->protocol ==
+							htons(ETH_P_IPV6),
+						    data_left);
+
+			memcpy(skb_put(csum_skb, tcp_hdrlen(skb)),
+			       tcph, tcp_hdrlen(skb));
+			skb_set_transport_header(csum_skb, 0);
+			csum_skb->csum_start =
+				(unsigned char *)tcp_hdr(csum_skb) -
+						 csum_skb->head;
+		}
+
+		hdr_page->pos += snap_ip_tcp_hdrlen;
+
+		hdr_tb_len = hdr_page->pos - start_hdr;
+		hdr_tb_phys = dma_map_single(trans->dev, start_hdr,
+					     hdr_tb_len, DMA_TO_DEVICE);
+		if (unlikely(dma_mapping_error(trans->dev, hdr_tb_phys))) {
+			dev_kfree_skb(csum_skb);
+			ret = -EINVAL;
+			goto out_unmap;
+		}
+		iwl_pcie_txq_build_tfd(trans, txq, hdr_tb_phys,
+				       hdr_tb_len, false);
+		trace_iwlwifi_dev_tx_tso_chunk(trans->dev, start_hdr,
+					       hdr_tb_len);
+
+		/* prepare the start_hdr for the next subframe */
+		start_hdr = hdr_page->pos;
+
+		/* put the payload */
+		while (data_left) {
+			unsigned int size = min_t(unsigned int, tso.size,
+						  data_left);
+			dma_addr_t tb_phys;
+
+			if (trans_pcie->sw_csum_tx)
+				memcpy(skb_put(csum_skb, size), tso.data, size);
+
+			tb_phys = dma_map_single(trans->dev, tso.data,
+						 size, DMA_TO_DEVICE);
+			if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
+				dev_kfree_skb(csum_skb);
+				ret = -EINVAL;
+				goto out_unmap;
+			}
+
+			iwl_pcie_txq_build_tfd(trans, txq, tb_phys,
+					       size, false);
+			trace_iwlwifi_dev_tx_tso_chunk(trans->dev, tso.data,
+						       size);
+
+			data_left -= size;
+			tso_build_data(skb, &tso, size);
+		}
+
+		/* For testing on early hardware only */
+		if (trans_pcie->sw_csum_tx) {
+			__wsum csum;
+
+			csum = skb_checksum(csum_skb,
+					    skb_checksum_start_offset(csum_skb),
+					    csum_skb->len -
+					    skb_checksum_start_offset(csum_skb),
+					    0);
+			dev_kfree_skb(csum_skb);
+			dma_sync_single_for_cpu(trans->dev, hdr_tb_phys,
+						hdr_tb_len, DMA_TO_DEVICE);
+			tcph->check = csum_fold(csum);
+			dma_sync_single_for_device(trans->dev, hdr_tb_phys,
+						   hdr_tb_len, DMA_TO_DEVICE);
+		}
+	}
+
+	/* re -add the WiFi header and IV */
+	skb_push(skb, hdr_len + iv_len);
+
+	return 0;
+
+out_unmap:
+	iwl_pcie_tfd_unmap(trans, out_meta, &txq->tfds[q->write_ptr]);
+	return ret;
+}
+#else /* CONFIG_INET */
+static int iwl_fill_data_tbs_amsdu(struct iwl_trans *trans, struct sk_buff *skb,
+				   struct iwl_txq *txq, u8 hdr_len,
+				   struct iwl_cmd_meta *out_meta,
+				   struct iwl_device_cmd *dev_cmd, u16 tb1_len)
+{
+	/* No A-MSDU without CONFIG_INET */
+	WARN_ON(1);
+
+	return -1;
+}
+#endif /* CONFIG_INET */
+
 int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 		      struct iwl_device_cmd *dev_cmd, int txq_id)
 {
@@ -1805,12 +2137,11 @@ int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 	struct iwl_queue *q;
 	dma_addr_t tb0_phys, tb1_phys, scratch_phys;
 	void *tb1_addr;
-	u16 len, tb1_len, tb2_len;
+	u16 len, tb1_len;
 	bool wait_write_ptr;
 	__le16 fc;
 	u8 hdr_len;
 	u16 wifi_seq;
-	int i;
 
 	txq = &trans_pcie->txq[txq_id];
 	q = &txq->q;
@@ -1819,6 +2150,19 @@ int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 		      "TX on unused queue %d\n", txq_id))
 		return -EINVAL;
 
+	if (unlikely(trans_pcie->sw_csum_tx &&
+		     skb->ip_summed == CHECKSUM_PARTIAL)) {
+		int offs = skb_checksum_start_offset(skb);
+		int csum_offs = offs + skb->csum_offset;
+		__wsum csum;
+
+		if (skb_ensure_writable(skb, csum_offs + sizeof(__sum16)))
+			return -1;
+
+		csum = skb_checksum(skb, offs, skb->len - offs, 0);
+		*(__sum16 *)(skb->data + csum_offs) = csum_fold(csum);
+	}
+
 	if (skb_is_nonlinear(skb) &&
 	    skb_shinfo(skb)->nr_frags > IWL_PCIE_MAX_FRAGS &&
 	    __skb_linearize(skb))
@@ -1893,57 +2237,20 @@ int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 		goto out_err;
 	iwl_pcie_txq_build_tfd(trans, txq, tb1_phys, tb1_len, false);
 
-	/*
-	 * Set up TFD's third entry to point directly to remainder
-	 * of skb's head, if any
-	 */
-	tb2_len = skb_headlen(skb) - hdr_len;
-	if (tb2_len > 0) {
-		dma_addr_t tb2_phys = dma_map_single(trans->dev,
-						     skb->data + hdr_len,
-						     tb2_len, DMA_TO_DEVICE);
-		if (unlikely(dma_mapping_error(trans->dev, tb2_phys))) {
-			iwl_pcie_tfd_unmap(trans, out_meta,
-					   &txq->tfds[q->write_ptr]);
-			goto out_err;
-		}
-		iwl_pcie_txq_build_tfd(trans, txq, tb2_phys, tb2_len, false);
-	}
-
-	/* set up the remaining entries to point to the data */
-	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
-		const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
-		dma_addr_t tb_phys;
-		int tb_idx;
-
-		if (!skb_frag_size(frag))
-			continue;
-
-		tb_phys = skb_frag_dma_map(trans->dev, frag, 0,
-					   skb_frag_size(frag), DMA_TO_DEVICE);
-
-		if (unlikely(dma_mapping_error(trans->dev, tb_phys))) {
-			iwl_pcie_tfd_unmap(trans, out_meta,
-					   &txq->tfds[q->write_ptr]);
+	if (ieee80211_is_data_qos(fc) &&
+	    (*ieee80211_get_qos_ctl(hdr) & IEEE80211_QOS_CTL_A_MSDU_PRESENT)) {
+		if (unlikely(iwl_fill_data_tbs_amsdu(trans, skb, txq, hdr_len,
+						     out_meta, dev_cmd,
+						     tb1_len)))
 			goto out_err;
-		}
-		tb_idx = iwl_pcie_txq_build_tfd(trans, txq, tb_phys,
-						skb_frag_size(frag), false);
-
-		out_meta->flags |= BIT(tb_idx + CMD_TB_BITMAP_POS);
+	} else if (unlikely(iwl_fill_data_tbs(trans, skb, txq, hdr_len,
+				       out_meta, dev_cmd, tb1_len))) {
+		goto out_err;
 	}
 
 	/* Set up entry for this TFD in Tx byte-count array */
 	iwl_pcie_txq_update_byte_cnt_tbl(trans, txq, le16_to_cpu(tx_cmd->len));
 
-	trace_iwlwifi_dev_tx(trans->dev, skb,
-			     &txq->tfds[txq->q.write_ptr],
-			     sizeof(struct iwl_tfd),
-			     &dev_cmd->hdr, IWL_HCMD_SCRATCHBUF_SIZE + tb1_len,
-			     skb->data + hdr_len, tb2_len);
-	trace_iwlwifi_dev_tx_data(trans->dev, skb,
-				  hdr_len, skb->len - hdr_len);
-
 	wait_write_ptr = ieee80211_has_morefrags(fc);
 
 	/* start timer if queue currently empty */
diff --git a/drivers/net/wireless/intersil/Kconfig b/drivers/net/wireless/intersil/Kconfig
new file mode 100644
index 000000000000..9da136049955
--- /dev/null
+++ b/drivers/net/wireless/intersil/Kconfig
@@ -0,0 +1,38 @@
+config WLAN_VENDOR_INTERSIL
+	bool "Intersil devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_INTERSIL
+
+source "drivers/net/wireless/intersil/hostap/Kconfig"
+source "drivers/net/wireless/intersil/orinoco/Kconfig"
+source "drivers/net/wireless/intersil/p54/Kconfig"
+
+config PRISM54
+	tristate 'Intersil Prism GT/Duette/Indigo PCI/Cardbus (DEPRECATED)'
+	depends on PCI
+	select WIRELESS_EXT
+	select WEXT_SPY
+	select WEXT_PRIV
+	select FW_LOADER
+	---help---
+	  This enables support for FullMAC PCI/Cardbus prism54 devices. This
+	  driver is now deprecated in favor for the SoftMAC driver, p54pci.
+	  p54pci supports FullMAC PCI/Cardbus devices as well.
+
+	  For more information refer to the p54 wiki:
+
+	  http://wireless.kernel.org/en/users/Drivers/p54
+
+	  Note: You need a motherboard with DMA support to use any of these cards
+
+	  When built as module you get the module prism54
+
+endif # WLAN_VENDOR_INTERSIL
diff --git a/drivers/net/wireless/intersil/Makefile b/drivers/net/wireless/intersil/Makefile
new file mode 100644
index 000000000000..9a8cbfee3ea5
--- /dev/null
+++ b/drivers/net/wireless/intersil/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_HOSTAP)		+= hostap/
+obj-$(CONFIG_HERMES)		+= orinoco/
+obj-$(CONFIG_P54_COMMON)	+= p54/
+obj-$(CONFIG_PRISM54)		+= prism54/
diff --git a/drivers/net/wireless/hostap/Kconfig b/drivers/net/wireless/intersil/hostap/Kconfig
index 287d82728bc3..287d82728bc3 100644
--- a/drivers/net/wireless/hostap/Kconfig
+++ b/drivers/net/wireless/intersil/hostap/Kconfig
diff --git a/drivers/net/wireless/hostap/Makefile b/drivers/net/wireless/intersil/hostap/Makefile
index b8e41a702c00..b8e41a702c00 100644
--- a/drivers/net/wireless/hostap/Makefile
+++ b/drivers/net/wireless/intersil/hostap/Makefile
diff --git a/drivers/net/wireless/hostap/hostap.h b/drivers/net/wireless/intersil/hostap/hostap.h
index ce8721fbc10e..ce8721fbc10e 100644
--- a/drivers/net/wireless/hostap/hostap.h
+++ b/drivers/net/wireless/intersil/hostap/hostap.h
diff --git a/drivers/net/wireless/hostap/hostap_80211.h b/drivers/net/wireless/intersil/hostap/hostap_80211.h
index ed98ce7c8f65..ed98ce7c8f65 100644
--- a/drivers/net/wireless/hostap/hostap_80211.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_80211.h
diff --git a/drivers/net/wireless/hostap/hostap_80211_rx.c b/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c
index 599f30f22841..599f30f22841 100644
--- a/drivers/net/wireless/hostap/hostap_80211_rx.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_80211_rx.c
diff --git a/drivers/net/wireless/hostap/hostap_80211_tx.c b/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c
index 055e11d353ca..055e11d353ca 100644
--- a/drivers/net/wireless/hostap/hostap_80211_tx.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_80211_tx.c
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/intersil/hostap/hostap_ap.c
index c995ace153ee..c995ace153ee 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_ap.c
diff --git a/drivers/net/wireless/hostap/hostap_ap.h b/drivers/net/wireless/intersil/hostap/hostap_ap.h
index 334e2d0b8e11..334e2d0b8e11 100644
--- a/drivers/net/wireless/hostap/hostap_ap.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_ap.h
diff --git a/drivers/net/wireless/hostap/hostap_common.h b/drivers/net/wireless/intersil/hostap/hostap_common.h
index 4230102ac9e4..4230102ac9e4 100644
--- a/drivers/net/wireless/hostap/hostap_common.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_common.h
diff --git a/drivers/net/wireless/hostap/hostap_config.h b/drivers/net/wireless/intersil/hostap/hostap_config.h
index 2c8f71f0ed45..2c8f71f0ed45 100644
--- a/drivers/net/wireless/hostap/hostap_config.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_config.h
diff --git a/drivers/net/wireless/hostap/hostap_cs.c b/drivers/net/wireless/intersil/hostap/hostap_cs.c
index 50033aa7c7d5..74f63b7bf7b4 100644
--- a/drivers/net/wireless/hostap/hostap_cs.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_cs.c
@@ -473,7 +473,7 @@ static int prism2_config(struct pcmcia_device *link)
 	struct net_device *dev;
 	struct hostap_interface *iface;
 	local_info_t *local;
-	int ret = 1;
+	int ret;
 	struct hostap_cs_priv *hw_priv;
 	unsigned long flags;
 
@@ -502,8 +502,10 @@ static int prism2_config(struct pcmcia_device *link)
 	/* Need to allocate net_device before requesting IRQ handler */
 	dev = prism2_init_local_data(&prism2_pccard_funcs, 0,
 				     &link->dev);
-	if (dev == NULL)
+	if (!dev) {
+		ret = -ENOMEM;
 		goto failed;
+	}
 	link->priv = dev;
 
 	iface = netdev_priv(dev);
diff --git a/drivers/net/wireless/hostap/hostap_download.c b/drivers/net/wireless/intersil/hostap/hostap_download.c
index 705fe668b969..705fe668b969 100644
--- a/drivers/net/wireless/hostap/hostap_download.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_download.c
diff --git a/drivers/net/wireless/hostap/hostap_hw.c b/drivers/net/wireless/intersil/hostap/hostap_hw.c
index 6df3ee561d52..6df3ee561d52 100644
--- a/drivers/net/wireless/hostap/hostap_hw.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_hw.c
diff --git a/drivers/net/wireless/hostap/hostap_info.c b/drivers/net/wireless/intersil/hostap/hostap_info.c
index 7635ac4f6679..7635ac4f6679 100644
--- a/drivers/net/wireless/hostap/hostap_info.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_info.c
diff --git a/drivers/net/wireless/hostap/hostap_ioctl.c b/drivers/net/wireless/intersil/hostap/hostap_ioctl.c
index 3e5fa7872b64..3e5fa7872b64 100644
--- a/drivers/net/wireless/hostap/hostap_ioctl.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_ioctl.c
diff --git a/drivers/net/wireless/hostap/hostap_main.c b/drivers/net/wireless/intersil/hostap/hostap_main.c
index 80d4228ba754..80d4228ba754 100644
--- a/drivers/net/wireless/hostap/hostap_main.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_main.c
diff --git a/drivers/net/wireless/hostap/hostap_pci.c b/drivers/net/wireless/intersil/hostap/hostap_pci.c
index c864ef4b0015..c864ef4b0015 100644
--- a/drivers/net/wireless/hostap/hostap_pci.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_pci.c
diff --git a/drivers/net/wireless/hostap/hostap_plx.c b/drivers/net/wireless/intersil/hostap/hostap_plx.c
index 4901a99c6c59..4901a99c6c59 100644
--- a/drivers/net/wireless/hostap/hostap_plx.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_plx.c
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/intersil/hostap/hostap_proc.c
index dd84557cf957..dd84557cf957 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/intersil/hostap/hostap_proc.c
diff --git a/drivers/net/wireless/hostap/hostap_wlan.h b/drivers/net/wireless/intersil/hostap/hostap_wlan.h
index ca25283e1c92..ca25283e1c92 100644
--- a/drivers/net/wireless/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/intersil/hostap/hostap_wlan.h
diff --git a/drivers/net/wireless/orinoco/Kconfig b/drivers/net/wireless/intersil/orinoco/Kconfig
index f6fa3f4e294f..f6fa3f4e294f 100644
--- a/drivers/net/wireless/orinoco/Kconfig
+++ b/drivers/net/wireless/intersil/orinoco/Kconfig
diff --git a/drivers/net/wireless/orinoco/Makefile b/drivers/net/wireless/intersil/orinoco/Makefile
index bfdefb85abcd..bfdefb85abcd 100644
--- a/drivers/net/wireless/orinoco/Makefile
+++ b/drivers/net/wireless/intersil/orinoco/Makefile
diff --git a/drivers/net/wireless/orinoco/airport.c b/drivers/net/wireless/intersil/orinoco/airport.c
index 77e6c53040a3..77e6c53040a3 100644
--- a/drivers/net/wireless/orinoco/airport.c
+++ b/drivers/net/wireless/intersil/orinoco/airport.c
diff --git a/drivers/net/wireless/orinoco/cfg.c b/drivers/net/wireless/intersil/orinoco/cfg.c
index 0f6ea316e38e..0f6ea316e38e 100644
--- a/drivers/net/wireless/orinoco/cfg.c
+++ b/drivers/net/wireless/intersil/orinoco/cfg.c
diff --git a/drivers/net/wireless/orinoco/cfg.h b/drivers/net/wireless/intersil/orinoco/cfg.h
index 3ddc96a06cd7..3ddc96a06cd7 100644
--- a/drivers/net/wireless/orinoco/cfg.h
+++ b/drivers/net/wireless/intersil/orinoco/cfg.h
diff --git a/drivers/net/wireless/orinoco/fw.c b/drivers/net/wireless/intersil/orinoco/fw.c
index 400a35217644..400a35217644 100644
--- a/drivers/net/wireless/orinoco/fw.c
+++ b/drivers/net/wireless/intersil/orinoco/fw.c
diff --git a/drivers/net/wireless/orinoco/fw.h b/drivers/net/wireless/intersil/orinoco/fw.h
index aca63e3c4b5b..aca63e3c4b5b 100644
--- a/drivers/net/wireless/orinoco/fw.h
+++ b/drivers/net/wireless/intersil/orinoco/fw.h
diff --git a/drivers/net/wireless/orinoco/hermes.c b/drivers/net/wireless/intersil/orinoco/hermes.c
index 43790fbea0e0..43790fbea0e0 100644
--- a/drivers/net/wireless/orinoco/hermes.c
+++ b/drivers/net/wireless/intersil/orinoco/hermes.c
diff --git a/drivers/net/wireless/orinoco/hermes.h b/drivers/net/wireless/intersil/orinoco/hermes.h
index 28a42448d329..28a42448d329 100644
--- a/drivers/net/wireless/orinoco/hermes.h
+++ b/drivers/net/wireless/intersil/orinoco/hermes.h
diff --git a/drivers/net/wireless/orinoco/hermes_dld.c b/drivers/net/wireless/intersil/orinoco/hermes_dld.c
index 4a10b7aca043..4a10b7aca043 100644
--- a/drivers/net/wireless/orinoco/hermes_dld.c
+++ b/drivers/net/wireless/intersil/orinoco/hermes_dld.c
diff --git a/drivers/net/wireless/orinoco/hermes_dld.h b/drivers/net/wireless/intersil/orinoco/hermes_dld.h
index b5377e232c63..b5377e232c63 100644
--- a/drivers/net/wireless/orinoco/hermes_dld.h
+++ b/drivers/net/wireless/intersil/orinoco/hermes_dld.h
diff --git a/drivers/net/wireless/orinoco/hermes_rid.h b/drivers/net/wireless/intersil/orinoco/hermes_rid.h
index 42eb67dea1df..42eb67dea1df 100644
--- a/drivers/net/wireless/orinoco/hermes_rid.h
+++ b/drivers/net/wireless/intersil/orinoco/hermes_rid.h
diff --git a/drivers/net/wireless/orinoco/hw.c b/drivers/net/wireless/intersil/orinoco/hw.c
index e27e32851f1e..e27e32851f1e 100644
--- a/drivers/net/wireless/orinoco/hw.c
+++ b/drivers/net/wireless/intersil/orinoco/hw.c
diff --git a/drivers/net/wireless/orinoco/hw.h b/drivers/net/wireless/intersil/orinoco/hw.h
index 466d1ede76f1..466d1ede76f1 100644
--- a/drivers/net/wireless/orinoco/hw.h
+++ b/drivers/net/wireless/intersil/orinoco/hw.h
diff --git a/drivers/net/wireless/orinoco/main.c b/drivers/net/wireless/intersil/orinoco/main.c
index 7b5c554323c7..7b5c554323c7 100644
--- a/drivers/net/wireless/orinoco/main.c
+++ b/drivers/net/wireless/intersil/orinoco/main.c
diff --git a/drivers/net/wireless/orinoco/main.h b/drivers/net/wireless/intersil/orinoco/main.h
index 5a8fec26136e..5a8fec26136e 100644
--- a/drivers/net/wireless/orinoco/main.h
+++ b/drivers/net/wireless/intersil/orinoco/main.h
diff --git a/drivers/net/wireless/orinoco/mic.c b/drivers/net/wireless/intersil/orinoco/mic.c
index fce4a843e656..fce4a843e656 100644
--- a/drivers/net/wireless/orinoco/mic.c
+++ b/drivers/net/wireless/intersil/orinoco/mic.c
diff --git a/drivers/net/wireless/orinoco/mic.h b/drivers/net/wireless/intersil/orinoco/mic.h
index 04d05bc566d6..04d05bc566d6 100644
--- a/drivers/net/wireless/orinoco/mic.h
+++ b/drivers/net/wireless/intersil/orinoco/mic.h
diff --git a/drivers/net/wireless/orinoco/orinoco.h b/drivers/net/wireless/intersil/orinoco/orinoco.h
index eebd2be21ee9..eebd2be21ee9 100644
--- a/drivers/net/wireless/orinoco/orinoco.h
+++ b/drivers/net/wireless/intersil/orinoco/orinoco.h
diff --git a/drivers/net/wireless/orinoco/orinoco_cs.c b/drivers/net/wireless/intersil/orinoco/orinoco_cs.c
index a956f965a1e5..a956f965a1e5 100644
--- a/drivers/net/wireless/orinoco/orinoco_cs.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_cs.c
diff --git a/drivers/net/wireless/orinoco/orinoco_nortel.c b/drivers/net/wireless/intersil/orinoco/orinoco_nortel.c
index 048693b6c6c2..048693b6c6c2 100644
--- a/drivers/net/wireless/orinoco/orinoco_nortel.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_nortel.c
diff --git a/drivers/net/wireless/orinoco/orinoco_pci.c b/drivers/net/wireless/intersil/orinoco/orinoco_pci.c
index 4938a2208a37..4938a2208a37 100644
--- a/drivers/net/wireless/orinoco/orinoco_pci.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_pci.c
diff --git a/drivers/net/wireless/orinoco/orinoco_pci.h b/drivers/net/wireless/intersil/orinoco/orinoco_pci.h
index 43f5b9f5a0b0..43f5b9f5a0b0 100644
--- a/drivers/net/wireless/orinoco/orinoco_pci.h
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_pci.h
diff --git a/drivers/net/wireless/orinoco/orinoco_plx.c b/drivers/net/wireless/intersil/orinoco/orinoco_plx.c
index 221352027779..221352027779 100644
--- a/drivers/net/wireless/orinoco/orinoco_plx.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_plx.c
diff --git a/drivers/net/wireless/orinoco/orinoco_tmd.c b/drivers/net/wireless/intersil/orinoco/orinoco_tmd.c
index 20ce569b8a43..20ce569b8a43 100644
--- a/drivers/net/wireless/orinoco/orinoco_tmd.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_tmd.c
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/intersil/orinoco/orinoco_usb.c
index f2cd513d54b2..f2cd513d54b2 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/intersil/orinoco/orinoco_usb.c
diff --git a/drivers/net/wireless/orinoco/scan.c b/drivers/net/wireless/intersil/orinoco/scan.c
index 2c66166add70..2c66166add70 100644
--- a/drivers/net/wireless/orinoco/scan.c
+++ b/drivers/net/wireless/intersil/orinoco/scan.c
diff --git a/drivers/net/wireless/orinoco/scan.h b/drivers/net/wireless/intersil/orinoco/scan.h
index 27281fb0a6dc..27281fb0a6dc 100644
--- a/drivers/net/wireless/orinoco/scan.h
+++ b/drivers/net/wireless/intersil/orinoco/scan.h
diff --git a/drivers/net/wireless/orinoco/spectrum_cs.c b/drivers/net/wireless/intersil/orinoco/spectrum_cs.c
index b60048c95e0a..b60048c95e0a 100644
--- a/drivers/net/wireless/orinoco/spectrum_cs.c
+++ b/drivers/net/wireless/intersil/orinoco/spectrum_cs.c
diff --git a/drivers/net/wireless/orinoco/wext.c b/drivers/net/wireless/intersil/orinoco/wext.c
index 1d4dae422106..1d4dae422106 100644
--- a/drivers/net/wireless/orinoco/wext.c
+++ b/drivers/net/wireless/intersil/orinoco/wext.c
diff --git a/drivers/net/wireless/orinoco/wext.h b/drivers/net/wireless/intersil/orinoco/wext.h
index 1479f4e26dde..1479f4e26dde 100644
--- a/drivers/net/wireless/orinoco/wext.h
+++ b/drivers/net/wireless/intersil/orinoco/wext.h
diff --git a/drivers/net/wireless/p54/Kconfig b/drivers/net/wireless/intersil/p54/Kconfig
index cdafb8c73e82..cdafb8c73e82 100644
--- a/drivers/net/wireless/p54/Kconfig
+++ b/drivers/net/wireless/intersil/p54/Kconfig
diff --git a/drivers/net/wireless/p54/Makefile b/drivers/net/wireless/intersil/p54/Makefile
index b542e68f1781..b542e68f1781 100644
--- a/drivers/net/wireless/p54/Makefile
+++ b/drivers/net/wireless/intersil/p54/Makefile
diff --git a/drivers/net/wireless/p54/eeprom.c b/drivers/net/wireless/intersil/p54/eeprom.c
index 2fe713eda7ad..2fe713eda7ad 100644
--- a/drivers/net/wireless/p54/eeprom.c
+++ b/drivers/net/wireless/intersil/p54/eeprom.c
diff --git a/drivers/net/wireless/p54/eeprom.h b/drivers/net/wireless/intersil/p54/eeprom.h
index 20ebe39a3f4e..20ebe39a3f4e 100644
--- a/drivers/net/wireless/p54/eeprom.h
+++ b/drivers/net/wireless/intersil/p54/eeprom.h
diff --git a/drivers/net/wireless/p54/fwio.c b/drivers/net/wireless/intersil/p54/fwio.c
index 257a9eadd595..257a9eadd595 100644
--- a/drivers/net/wireless/p54/fwio.c
+++ b/drivers/net/wireless/intersil/p54/fwio.c
diff --git a/drivers/net/wireless/p54/led.c b/drivers/net/wireless/intersil/p54/led.c
index 9a8fedd3c0f5..9a8fedd3c0f5 100644
--- a/drivers/net/wireless/p54/led.c
+++ b/drivers/net/wireless/intersil/p54/led.c
diff --git a/drivers/net/wireless/p54/lmac.h b/drivers/net/wireless/intersil/p54/lmac.h
index de1d46bf97df..de1d46bf97df 100644
--- a/drivers/net/wireless/p54/lmac.h
+++ b/drivers/net/wireless/intersil/p54/lmac.h
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/intersil/p54/main.c
index 7805864e76f9..7805864e76f9 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/intersil/p54/main.c
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/intersil/p54/p54.h
index 40b401ed6845..40b401ed6845 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/intersil/p54/p54.h
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/intersil/p54/p54pci.c
index 27a49068d32d..27a49068d32d 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/intersil/p54/p54pci.c
diff --git a/drivers/net/wireless/p54/p54pci.h b/drivers/net/wireless/intersil/p54/p54pci.h
index 68405c142f97..68405c142f97 100644
--- a/drivers/net/wireless/p54/p54pci.h
+++ b/drivers/net/wireless/intersil/p54/p54pci.h
diff --git a/drivers/net/wireless/p54/p54spi.c b/drivers/net/wireless/intersil/p54/p54spi.c
index 7ab2f43ab425..7ab2f43ab425 100644
--- a/drivers/net/wireless/p54/p54spi.c
+++ b/drivers/net/wireless/intersil/p54/p54spi.c
diff --git a/drivers/net/wireless/p54/p54spi.h b/drivers/net/wireless/intersil/p54/p54spi.h
index dfaa62aaeb07..dfaa62aaeb07 100644
--- a/drivers/net/wireless/p54/p54spi.h
+++ b/drivers/net/wireless/intersil/p54/p54spi.h
diff --git a/drivers/net/wireless/p54/p54spi_eeprom.h b/drivers/net/wireless/intersil/p54/p54spi_eeprom.h
index 0b7bfb0adcf2..0b7bfb0adcf2 100644
--- a/drivers/net/wireless/p54/p54spi_eeprom.h
+++ b/drivers/net/wireless/intersil/p54/p54spi_eeprom.h
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/intersil/p54/p54usb.c
index 043bd1c23c19..043bd1c23c19 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/intersil/p54/p54usb.c
diff --git a/drivers/net/wireless/p54/p54usb.h b/drivers/net/wireless/intersil/p54/p54usb.h
index a5f5f0fea3bd..a5f5f0fea3bd 100644
--- a/drivers/net/wireless/p54/p54usb.h
+++ b/drivers/net/wireless/intersil/p54/p54usb.h
diff --git a/drivers/net/wireless/p54/txrx.c b/drivers/net/wireless/intersil/p54/txrx.c
index 24e5ff9a9272..24e5ff9a9272 100644
--- a/drivers/net/wireless/p54/txrx.c
+++ b/drivers/net/wireless/intersil/p54/txrx.c
diff --git a/drivers/net/wireless/prism54/Makefile b/drivers/net/wireless/intersil/prism54/Makefile
index fad305c76737..fad305c76737 100644
--- a/drivers/net/wireless/prism54/Makefile
+++ b/drivers/net/wireless/intersil/prism54/Makefile
diff --git a/drivers/net/wireless/prism54/isl_38xx.c b/drivers/net/wireless/intersil/prism54/isl_38xx.c
index 333c1a2f882e..333c1a2f882e 100644
--- a/drivers/net/wireless/prism54/isl_38xx.c
+++ b/drivers/net/wireless/intersil/prism54/isl_38xx.c
diff --git a/drivers/net/wireless/prism54/isl_38xx.h b/drivers/net/wireless/intersil/prism54/isl_38xx.h
index 547ab885610b..547ab885610b 100644
--- a/drivers/net/wireless/prism54/isl_38xx.h
+++ b/drivers/net/wireless/intersil/prism54/isl_38xx.h
diff --git a/drivers/net/wireless/prism54/isl_ioctl.c b/drivers/net/wireless/intersil/prism54/isl_ioctl.c
index ecbb0546cf3e..48e8a978a832 100644
--- a/drivers/net/wireless/prism54/isl_ioctl.c
+++ b/drivers/net/wireless/intersil/prism54/isl_ioctl.c
@@ -2036,7 +2036,7 @@ format_event(islpci_private *priv, char *dest, const char *str,
 			 mlme->address,
 			 (error ? (mlme->code ? " : REJECTED " : " : ACCEPTED ")
 			  : ""), mlme->code);
-	BUG_ON(n > IW_CUSTOM_MAX);
+	WARN_ON(n >= IW_CUSTOM_MAX);
 	*length = n;
 }
 
diff --git a/drivers/net/wireless/prism54/isl_ioctl.h b/drivers/net/wireless/intersil/prism54/isl_ioctl.h
index 842a2549facc..842a2549facc 100644
--- a/drivers/net/wireless/prism54/isl_ioctl.h
+++ b/drivers/net/wireless/intersil/prism54/isl_ioctl.h
diff --git a/drivers/net/wireless/prism54/isl_oid.h b/drivers/net/wireless/intersil/prism54/isl_oid.h
index 83fec557997e..83fec557997e 100644
--- a/drivers/net/wireless/prism54/isl_oid.h
+++ b/drivers/net/wireless/intersil/prism54/isl_oid.h
diff --git a/drivers/net/wireless/prism54/islpci_dev.c b/drivers/net/wireless/intersil/prism54/islpci_dev.c
index 931cf440ff18..84a42012aeae 100644
--- a/drivers/net/wireless/prism54/islpci_dev.c
+++ b/drivers/net/wireless/intersil/prism54/islpci_dev.c
@@ -707,7 +707,9 @@ islpci_alloc_memory(islpci_private *priv)
 		    pci_map_single(priv->pdev, (void *) skb->data,
 				   MAX_FRAGMENT_SIZE_RX + 2,
 				   PCI_DMA_FROMDEVICE);
-		if (!priv->pci_map_rx_address[counter]) {
+		if (pci_dma_mapping_error(priv->pdev,
+					  priv->pci_map_rx_address[counter])) {
+			priv->pci_map_rx_address[counter] = 0;
 			/* error mapping the buffer to device
 			   accessible memory address */
 			printk(KERN_ERR "failed to map skb DMA'able\n");
diff --git a/drivers/net/wireless/prism54/islpci_dev.h b/drivers/net/wireless/intersil/prism54/islpci_dev.h
index f6f088e05fe4..f6f088e05fe4 100644
--- a/drivers/net/wireless/prism54/islpci_dev.h
+++ b/drivers/net/wireless/intersil/prism54/islpci_dev.h
diff --git a/drivers/net/wireless/prism54/islpci_eth.c b/drivers/net/wireless/intersil/prism54/islpci_eth.c
index 674658f2e6ef..d83f6332019e 100644
--- a/drivers/net/wireless/prism54/islpci_eth.c
+++ b/drivers/net/wireless/intersil/prism54/islpci_eth.c
@@ -190,7 +190,7 @@ islpci_eth_transmit(struct sk_buff *skb, struct net_device *ndev)
 	pci_map_address = pci_map_single(priv->pdev,
 					 (void *) skb->data, skb->len,
 					 PCI_DMA_TODEVICE);
-	if (unlikely(pci_map_address == 0)) {
+	if (pci_dma_mapping_error(priv->pdev, pci_map_address)) {
 		printk(KERN_WARNING "%s: cannot map buffer to PCI\n",
 		       ndev->name);
 		goto drop_free;
@@ -448,7 +448,8 @@ islpci_eth_receive(islpci_private *priv)
 		    pci_map_single(priv->pdev, (void *) skb->data,
 				   MAX_FRAGMENT_SIZE_RX + 2,
 				   PCI_DMA_FROMDEVICE);
-		if (unlikely(!priv->pci_map_rx_address[index])) {
+		if (pci_dma_mapping_error(priv->pdev,
+					  priv->pci_map_rx_address[index])) {
 			/* error mapping the buffer to device accessible memory address */
 			DEBUG(SHOW_ERROR_MESSAGES,
 			      "Error mapping DMA address\n");
diff --git a/drivers/net/wireless/prism54/islpci_eth.h b/drivers/net/wireless/intersil/prism54/islpci_eth.h
index 80f50f1bc6f2..80f50f1bc6f2 100644
--- a/drivers/net/wireless/prism54/islpci_eth.h
+++ b/drivers/net/wireless/intersil/prism54/islpci_eth.h
diff --git a/drivers/net/wireless/prism54/islpci_hotplug.c b/drivers/net/wireless/intersil/prism54/islpci_hotplug.c
index 300c846ea087..300c846ea087 100644
--- a/drivers/net/wireless/prism54/islpci_hotplug.c
+++ b/drivers/net/wireless/intersil/prism54/islpci_hotplug.c
diff --git a/drivers/net/wireless/prism54/islpci_mgt.c b/drivers/net/wireless/intersil/prism54/islpci_mgt.c
index 0de14dfa68cc..53d7a1705e8e 100644
--- a/drivers/net/wireless/prism54/islpci_mgt.c
+++ b/drivers/net/wireless/intersil/prism54/islpci_mgt.c
@@ -130,7 +130,7 @@ islpci_mgmt_rx_fill(struct net_device *ndev)
 			buf->pci_addr = pci_map_single(priv->pdev, buf->mem,
 						       MGMT_FRAME_SIZE,
 						       PCI_DMA_FROMDEVICE);
-			if (!buf->pci_addr) {
+			if (pci_dma_mapping_error(priv->pdev, buf->pci_addr)) {
 				printk(KERN_WARNING
 				       "Failed to make memory DMA'able.\n");
 				return -ENOMEM;
@@ -217,7 +217,7 @@ islpci_mgt_transmit(struct net_device *ndev, int operation, unsigned long oid,
 	err = -ENOMEM;
 	buf.pci_addr = pci_map_single(priv->pdev, buf.mem, frag_len,
 				      PCI_DMA_TODEVICE);
-	if (!buf.pci_addr) {
+	if (pci_dma_mapping_error(priv->pdev, buf.pci_addr)) {
 		printk(KERN_WARNING "%s: cannot map PCI memory for mgmt\n",
 		       ndev->name);
 		goto error_free;
diff --git a/drivers/net/wireless/prism54/islpci_mgt.h b/drivers/net/wireless/intersil/prism54/islpci_mgt.h
index 700c434c8803..700c434c8803 100644
--- a/drivers/net/wireless/prism54/islpci_mgt.h
+++ b/drivers/net/wireless/intersil/prism54/islpci_mgt.h
diff --git a/drivers/net/wireless/prism54/oid_mgt.c b/drivers/net/wireless/intersil/prism54/oid_mgt.c
index 3a8d2dbcfecd..6528ed5b9b1d 100644
--- a/drivers/net/wireless/prism54/oid_mgt.c
+++ b/drivers/net/wireless/intersil/prism54/oid_mgt.c
@@ -424,7 +424,7 @@ mgt_set_request(islpci_private *priv, enum oid_num_t n, int extra, void *data)
 	void *cache, *_data = data;
 	u32 oid;
 
-	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(n >= OID_NUM_LAST);
 	BUG_ON(extra > isl_oid[n].range);
 
 	if (!priv->mib)
@@ -485,7 +485,7 @@ mgt_set_varlen(islpci_private *priv, enum oid_num_t n, void *data, int extra_len
 	int dlen;
 	u32 oid;
 
-	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(n >= OID_NUM_LAST);
 
 	dlen = isl_oid[n].size;
 	oid = isl_oid[n].oid;
@@ -524,7 +524,7 @@ mgt_get_request(islpci_private *priv, enum oid_num_t n, int extra, void *data,
 	void *cache, *_res = NULL;
 	u32 oid;
 
-	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(n >= OID_NUM_LAST);
 	BUG_ON(extra > isl_oid[n].range);
 
 	res->ptr = NULL;
@@ -626,7 +626,7 @@ mgt_commit_list(islpci_private *priv, enum oid_num_t *l, int n)
 void
 mgt_set(islpci_private *priv, enum oid_num_t n, void *data)
 {
-	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(n >= OID_NUM_LAST);
 	BUG_ON(priv->mib[n] == NULL);
 
 	memcpy(priv->mib[n], data, isl_oid[n].size);
@@ -636,7 +636,7 @@ mgt_set(islpci_private *priv, enum oid_num_t n, void *data)
 void
 mgt_get(islpci_private *priv, enum oid_num_t n, void *res)
 {
-	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(n >= OID_NUM_LAST);
 	BUG_ON(priv->mib[n] == NULL);
 	BUG_ON(res == NULL);
 
diff --git a/drivers/net/wireless/prism54/oid_mgt.h b/drivers/net/wireless/intersil/prism54/oid_mgt.h
index cf5141df8474..cf5141df8474 100644
--- a/drivers/net/wireless/prism54/oid_mgt.h
+++ b/drivers/net/wireless/intersil/prism54/oid_mgt.h
diff --git a/drivers/net/wireless/prism54/prismcompat.h b/drivers/net/wireless/intersil/prism54/prismcompat.h
index bc1401eb4b9d..bc1401eb4b9d 100644
--- a/drivers/net/wireless/prism54/prismcompat.h
+++ b/drivers/net/wireless/intersil/prism54/prismcompat.h
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index c00a7daaa4bc..a28414c50edf 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -495,6 +495,9 @@ struct mac80211_hwsim_data {
 	const struct ieee80211_regdomain *regd;
 
 	struct ieee80211_channel *tmp_chan;
+	struct ieee80211_channel *roc_chan;
+	u32 roc_duration;
+	struct delayed_work roc_start;
 	struct delayed_work roc_done;
 	struct delayed_work hw_scan;
 	struct cfg80211_scan_request *hw_scan_request;
@@ -514,6 +517,7 @@ struct mac80211_hwsim_data {
 	bool ps_poll_pending;
 	struct dentry *debugfs;
 
+	uintptr_t pending_cookie;
 	struct sk_buff_head pending;	/* packets pending */
 	/*
 	 * Only radios in the same group can communicate together (the
@@ -810,6 +814,9 @@ static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_skb);
 	struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
 
+	if (WARN_ON(!txrate))
+		return;
+
 	if (!netif_running(hwsim_mon))
 		return;
 
@@ -960,6 +967,7 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 	unsigned int hwsim_flags = 0;
 	int i;
 	struct hwsim_tx_rate tx_attempts[IEEE80211_TX_MAX_RATES];
+	uintptr_t cookie;
 
 	if (data->ps != PS_DISABLED)
 		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
@@ -983,7 +991,8 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		goto nla_put_failure;
 	}
 
-	if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER, ETH_ALEN, hdr->addr2))
+	if (nla_put(skb, HWSIM_ATTR_ADDR_TRANSMITTER,
+		    ETH_ALEN, data->addresses[1].addr))
 		goto nla_put_failure;
 
 	/* We get the skb->data */
@@ -1018,7 +1027,10 @@ static void mac80211_hwsim_tx_frame_nl(struct ieee80211_hw *hw,
 		goto nla_put_failure;
 
 	/* We create a cookie to identify this skb */
-	if (nla_put_u64(skb, HWSIM_ATTR_COOKIE, (unsigned long) my_skb))
+	data->pending_cookie++;
+	cookie = data->pending_cookie;
+	info->rate_driver_data[0] = (void *)cookie;
+	if (nla_put_u64(skb, HWSIM_ATTR_COOKIE, cookie))
 		goto nla_put_failure;
 
 	genlmsg_end(skb, msg_head);
@@ -1247,6 +1259,7 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 {
 	struct mac80211_hwsim_data *data = hw->priv;
 	struct ieee80211_tx_info *txi = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hdr *hdr = (void *)skb->data;
 	struct ieee80211_chanctx_conf *chanctx_conf;
 	struct ieee80211_channel *channel;
 	bool ack;
@@ -1292,6 +1305,22 @@ static void mac80211_hwsim_tx(struct ieee80211_hw *hw,
 				       ARRAY_SIZE(txi->control.rates));
 
 	txi->rate_driver_data[0] = channel;
+
+	if (skb->len >= 24 + 8 &&
+	    ieee80211_is_probe_resp(hdr->frame_control)) {
+		/* fake header transmission time */
+		struct ieee80211_mgmt *mgmt;
+		struct ieee80211_rate *txrate;
+		u64 ts;
+
+		mgmt = (struct ieee80211_mgmt *)skb->data;
+		txrate = ieee80211_get_tx_rate(hw, txi);
+		ts = mac80211_hwsim_get_tsf_raw();
+		mgmt->u.probe_resp.timestamp =
+			cpu_to_le64(ts + data->tsf_offset +
+				    24 * 8 * 10 / txrate->bitrate);
+	}
+
 	mac80211_hwsim_monitor_rx(hw, skb, channel);
 
 	/* wmediumd mode check */
@@ -1871,7 +1900,8 @@ static void hw_scan_work(struct work_struct *work)
 		    req->channels[hwsim->scan_chan_idx]->center_freq);
 
 	hwsim->tmp_chan = req->channels[hwsim->scan_chan_idx];
-	if (hwsim->tmp_chan->flags & IEEE80211_CHAN_NO_IR ||
+	if (hwsim->tmp_chan->flags & (IEEE80211_CHAN_NO_IR |
+				      IEEE80211_CHAN_RADAR) ||
 	    !req->n_ssids) {
 		dwell = 120;
 	} else {
@@ -1987,6 +2017,23 @@ static void mac80211_hwsim_sw_scan_complete(struct ieee80211_hw *hw,
 	mutex_unlock(&hwsim->mutex);
 }
 
+static void hw_roc_start(struct work_struct *work)
+{
+	struct mac80211_hwsim_data *hwsim =
+		container_of(work, struct mac80211_hwsim_data, roc_start.work);
+
+	mutex_lock(&hwsim->mutex);
+
+	wiphy_debug(hwsim->hw->wiphy, "hwsim ROC begins\n");
+	hwsim->tmp_chan = hwsim->roc_chan;
+	ieee80211_ready_on_channel(hwsim->hw);
+
+	ieee80211_queue_delayed_work(hwsim->hw, &hwsim->roc_done,
+				     msecs_to_jiffies(hwsim->roc_duration));
+
+	mutex_unlock(&hwsim->mutex);
+}
+
 static void hw_roc_done(struct work_struct *work)
 {
 	struct mac80211_hwsim_data *hwsim =
@@ -2014,16 +2061,14 @@ static int mac80211_hwsim_roc(struct ieee80211_hw *hw,
 		return -EBUSY;
 	}
 
-	hwsim->tmp_chan = chan;
+	hwsim->roc_chan = chan;
+	hwsim->roc_duration = duration;
 	mutex_unlock(&hwsim->mutex);
 
 	wiphy_debug(hw->wiphy, "hwsim ROC (%d MHz, %d ms)\n",
 		    chan->center_freq, duration);
+	ieee80211_queue_delayed_work(hw, &hwsim->roc_start, HZ/50);
 
-	ieee80211_ready_on_channel(hw);
-
-	ieee80211_queue_delayed_work(hw, &hwsim->roc_done,
-				     msecs_to_jiffies(duration));
 	return 0;
 }
 
@@ -2031,6 +2076,7 @@ static int mac80211_hwsim_croc(struct ieee80211_hw *hw)
 {
 	struct mac80211_hwsim_data *hwsim = hw->priv;
 
+	cancel_delayed_work_sync(&hwsim->roc_start);
 	cancel_delayed_work_sync(&hwsim->roc_done);
 
 	mutex_lock(&hwsim->mutex);
@@ -2375,6 +2421,7 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 		hw->wiphy->n_iface_combinations = ARRAY_SIZE(hwsim_if_comb);
 	}
 
+	INIT_DELAYED_WORK(&data->roc_start, hw_roc_start);
 	INIT_DELAYED_WORK(&data->roc_done, hw_roc_done);
 	INIT_DELAYED_WORK(&data->hw_scan, hw_scan_work);
 
@@ -2411,6 +2458,7 @@ static int mac80211_hwsim_new_radio(struct genl_info *info,
 			       NL80211_FEATURE_STATIC_SMPS |
 			       NL80211_FEATURE_DYNAMIC_SMPS |
 			       NL80211_FEATURE_SCAN_RANDOM_MAC_ADDR;
+	wiphy_ext_feature_set(hw->wiphy, NL80211_EXT_FEATURE_VHT_IBSS);
 
 	/* ask mac80211 to reserve space for magic */
 	hw->vif_data_size = sizeof(struct hwsim_vif_priv);
@@ -2689,7 +2737,7 @@ static struct mac80211_hwsim_data *get_hwsim_data_ref_from_addr(const u8 *addr)
 
 	spin_lock_bh(&hwsim_radio_lock);
 	list_for_each_entry(data, &hwsim_radios, list) {
-		if (mac80211_hwsim_addr_match(data, addr)) {
+		if (memcmp(data->addresses[1].addr, addr, ETH_ALEN) == 0) {
 			_found = true;
 			break;
 		}
@@ -2710,7 +2758,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 	struct mac80211_hwsim_data *data2;
 	struct ieee80211_tx_info *txi;
 	struct hwsim_tx_rate *tx_attempts;
-	unsigned long ret_skb_ptr;
+	u64 ret_skb_cookie;
 	struct sk_buff *skb, *tmp;
 	const u8 *src;
 	unsigned int hwsim_flags;
@@ -2728,7 +2776,7 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 
 	src = (void *)nla_data(info->attrs[HWSIM_ATTR_ADDR_TRANSMITTER]);
 	hwsim_flags = nla_get_u32(info->attrs[HWSIM_ATTR_FLAGS]);
-	ret_skb_ptr = nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
+	ret_skb_cookie = nla_get_u64(info->attrs[HWSIM_ATTR_COOKIE]);
 
 	data2 = get_hwsim_data_ref_from_addr(src);
 	if (!data2)
@@ -2736,7 +2784,12 @@ static int hwsim_tx_info_frame_received_nl(struct sk_buff *skb_2,
 
 	/* look for the skb matching the cookie passed back from user */
 	skb_queue_walk_safe(&data2->pending, skb, tmp) {
-		if ((unsigned long)skb == ret_skb_ptr) {
+		u64 skb_cookie;
+
+		txi = IEEE80211_SKB_CB(skb);
+		skb_cookie = (u64)(uintptr_t)txi->rate_driver_data[0];
+
+		if (skb_cookie == ret_skb_cookie) {
 			skb_unlink(skb, &data2->pending);
 			found = true;
 			break;
@@ -2827,10 +2880,25 @@ static int hwsim_cloned_frame_received_nl(struct sk_buff *skb_2,
 
 	/* A frame is received from user space */
 	memset(&rx_status, 0, sizeof(rx_status));
-	/* TODO: Check ATTR_FREQ if it exists, and maybe throw away off-channel
-	 * packets?
-	 */
-	rx_status.freq = data2->channel->center_freq;
+	if (info->attrs[HWSIM_ATTR_FREQ]) {
+		/* throw away off-channel packets, but allow both the temporary
+		 * ("hw" scan/remain-on-channel) and regular channel, since the
+		 * internal datapath also allows this
+		 */
+		mutex_lock(&data2->mutex);
+		rx_status.freq = nla_get_u32(info->attrs[HWSIM_ATTR_FREQ]);
+
+		if (rx_status.freq != data2->channel->center_freq &&
+		    (!data2->tmp_chan ||
+		     rx_status.freq != data2->tmp_chan->center_freq)) {
+			mutex_unlock(&data2->mutex);
+			goto out;
+		}
+		mutex_unlock(&data2->mutex);
+	} else {
+		rx_status.freq = data2->channel->center_freq;
+	}
+
 	rx_status.band = data2->channel->band;
 	rx_status.rate_idx = nla_get_u32(info->attrs[HWSIM_ATTR_RX_RATE]);
 	rx_status.signal = nla_get_u32(info->attrs[HWSIM_ATTR_SIGNAL]);
diff --git a/drivers/net/wireless/marvell/Kconfig b/drivers/net/wireless/marvell/Kconfig
new file mode 100644
index 000000000000..4938c7ec0009
--- /dev/null
+++ b/drivers/net/wireless/marvell/Kconfig
@@ -0,0 +1,27 @@
+config WLAN_VENDOR_MARVELL
+	bool "Marvell devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_MARVELL
+
+source "drivers/net/wireless/marvell/libertas/Kconfig"
+source "drivers/net/wireless/marvell/libertas_tf/Kconfig"
+source "drivers/net/wireless/marvell/mwifiex/Kconfig"
+
+config MWL8K
+	tristate "Marvell 88W8xxx PCI/PCIe Wireless support"
+	depends on MAC80211 && PCI
+	---help---
+	  This driver supports Marvell TOPDOG 802.11 wireless cards.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called mwl8k.  If unsure, say N.
+
+endif # WLAN_VENDOR_MARVELL
diff --git a/drivers/net/wireless/marvell/Makefile b/drivers/net/wireless/marvell/Makefile
new file mode 100644
index 000000000000..1b0a7d2bc8e6
--- /dev/null
+++ b/drivers/net/wireless/marvell/Makefile
@@ -0,0 +1,6 @@
+obj-$(CONFIG_LIBERTAS)		+= libertas/
+
+obj-$(CONFIG_LIBERTAS_THINFIRM)	+= libertas_tf/
+obj-$(CONFIG_MWIFIEX)	+= mwifiex/
+
+obj-$(CONFIG_MWL8K)	+= mwl8k.o
diff --git a/drivers/net/wireless/libertas/Kconfig b/drivers/net/wireless/marvell/libertas/Kconfig
index e6268ceacbf1..e6268ceacbf1 100644
--- a/drivers/net/wireless/libertas/Kconfig
+++ b/drivers/net/wireless/marvell/libertas/Kconfig
diff --git a/drivers/net/wireless/libertas/LICENSE b/drivers/net/wireless/marvell/libertas/LICENSE
index 8862742213b9..8862742213b9 100644
--- a/drivers/net/wireless/libertas/LICENSE
+++ b/drivers/net/wireless/marvell/libertas/LICENSE
diff --git a/drivers/net/wireless/libertas/Makefile b/drivers/net/wireless/marvell/libertas/Makefile
index eac72f7bd341..eac72f7bd341 100644
--- a/drivers/net/wireless/libertas/Makefile
+++ b/drivers/net/wireless/marvell/libertas/Makefile
diff --git a/drivers/net/wireless/libertas/README b/drivers/net/wireless/marvell/libertas/README
index 1a554a685e91..1a554a685e91 100644
--- a/drivers/net/wireless/libertas/README
+++ b/drivers/net/wireless/marvell/libertas/README
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/marvell/libertas/cfg.c
index 8317afd065b4..86955c416b30 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/marvell/libertas/cfg.c
@@ -1108,7 +1108,7 @@ static int lbs_associate(struct lbs_private *priv,
 	size_t len, resp_ie_len;
 	int status;
 	int ret;
-	u8 *pos = &(cmd->iebuf[0]);
+	u8 *pos;
 	u8 *tmp;
 
 	lbs_deb_enter(LBS_DEB_CFG80211);
@@ -1117,6 +1117,7 @@ static int lbs_associate(struct lbs_private *priv,
 		ret = -ENOMEM;
 		goto done;
 	}
+	pos = &cmd->iebuf[0];
 
 	/*
 	 * cmd              50 00
diff --git a/drivers/net/wireless/libertas/cfg.h b/drivers/net/wireless/marvell/libertas/cfg.h
index acccc2922401..acccc2922401 100644
--- a/drivers/net/wireless/libertas/cfg.h
+++ b/drivers/net/wireless/marvell/libertas/cfg.h
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/marvell/libertas/cmd.c
index 0387a5b380c8..0387a5b380c8 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/marvell/libertas/cmd.c
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/marvell/libertas/cmd.h
index 0c5444b02c64..0c5444b02c64 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/marvell/libertas/cmd.h
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/marvell/libertas/cmdresp.c
index e5442e8956f7..e5442e8956f7 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/marvell/libertas/cmdresp.c
diff --git a/drivers/net/wireless/libertas/debugfs.c b/drivers/net/wireless/marvell/libertas/debugfs.c
index 26cbf1dcc662..faed1823c58e 100644
--- a/drivers/net/wireless/libertas/debugfs.c
+++ b/drivers/net/wireless/marvell/libertas/debugfs.c
@@ -56,19 +56,15 @@ static ssize_t lbs_sleepparams_write(struct file *file,
 				loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t buf_size, ret;
+	ssize_t ret;
 	struct sleep_params sp;
 	int p1, p2, p3, p4, p5, p6;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(user_buf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, user_buf, buf_size)) {
-		ret = -EFAULT;
-		goto out_unlock;
-	}
 	ret = sscanf(buf, "%d %d %d %d %d %d", &p1, &p2, &p3, &p4, &p5, &p6);
 	if (ret != 6) {
 		ret = -EINVAL;
@@ -88,7 +84,7 @@ static ssize_t lbs_sleepparams_write(struct file *file,
 		ret = -EINVAL;
 
 out_unlock:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -125,18 +121,14 @@ static ssize_t lbs_host_sleep_write(struct file *file,
 				loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t buf_size, ret;
+	ssize_t ret;
 	int host_sleep;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(user_buf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, user_buf, buf_size)) {
-		ret = -EFAULT;
-		goto out_unlock;
-	}
 	ret = sscanf(buf, "%d", &host_sleep);
 	if (ret != 1) {
 		ret = -EINVAL;
@@ -162,7 +154,7 @@ static ssize_t lbs_host_sleep_write(struct file *file,
 		ret = count;
 
 out_unlock:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -281,21 +273,15 @@ static ssize_t lbs_threshold_write(uint16_t tlv_type, uint16_t event_mask,
 	struct cmd_ds_802_11_subscribe_event *events;
 	struct mrvl_ie_thresholds *tlv;
 	struct lbs_private *priv = file->private_data;
-	ssize_t buf_size;
 	int value, freq, new_mask;
 	uint16_t curr_mask;
 	char *buf;
 	int ret;
 
-	buf = (char *)get_zeroed_page(GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		ret = -EFAULT;
-		goto out_page;
-	}
 	ret = sscanf(buf, "%d %d %d", &value, &freq, &new_mask);
 	if (ret != 3) {
 		ret = -EINVAL;
@@ -343,7 +329,7 @@ static ssize_t lbs_threshold_write(uint16_t tlv_type, uint16_t event_mask,
  out_events:
 	kfree(events);
  out_page:
-	free_page((unsigned long)buf);
+	kfree(buf);
 	return ret;
 }
 
@@ -472,22 +458,15 @@ static ssize_t lbs_rdmac_write(struct file *file,
 				    size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	priv->mac_offset = simple_strtoul(buf, NULL, 16);
-	res = count;
-out_unlock:
-	free_page(addr);
-	return res;
+	kfree(buf);
+	return count;
 }
 
 static ssize_t lbs_wrmac_write(struct file *file,
@@ -496,18 +475,14 @@ static ssize_t lbs_wrmac_write(struct file *file,
 {
 
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
+	ssize_t res;
 	u32 offset, value;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	res = sscanf(buf, "%x %x", &offset, &value);
 	if (res != 2) {
 		res = -EFAULT;
@@ -520,7 +495,7 @@ static ssize_t lbs_wrmac_write(struct file *file,
 	if (!res)
 		res = count;
 out_unlock:
-	free_page(addr);
+	kfree(buf);
 	return res;
 }
 
@@ -554,22 +529,16 @@ static ssize_t lbs_rdbbp_write(struct file *file,
 				    size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	priv->bbp_offset = simple_strtoul(buf, NULL, 16);
-	res = count;
-out_unlock:
-	free_page(addr);
-	return res;
+	kfree(buf);
+
+	return count;
 }
 
 static ssize_t lbs_wrbbp_write(struct file *file,
@@ -578,18 +547,14 @@ static ssize_t lbs_wrbbp_write(struct file *file,
 {
 
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
+	ssize_t res;
 	u32 offset, value;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	res = sscanf(buf, "%x %x", &offset, &value);
 	if (res != 2) {
 		res = -EFAULT;
@@ -602,7 +567,7 @@ static ssize_t lbs_wrbbp_write(struct file *file,
 	if (!res)
 		res = count;
 out_unlock:
-	free_page(addr);
+	kfree(buf);
 	return res;
 }
 
@@ -636,22 +601,15 @@ static ssize_t lbs_rdrf_write(struct file *file,
 				    size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	priv->rf_offset = simple_strtoul(buf, NULL, 16);
-	res = count;
-out_unlock:
-	free_page(addr);
-	return res;
+	kfree(buf);
+	return count;
 }
 
 static ssize_t lbs_wrrf_write(struct file *file,
@@ -660,18 +618,14 @@ static ssize_t lbs_wrrf_write(struct file *file,
 {
 
 	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
+	ssize_t res;
 	u32 offset, value;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	if (!buf)
-		return -ENOMEM;
+	char *buf;
+
+	buf = memdup_user_nul(userbuf, min(count, len - 1));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
-	buf_size = min(count, len - 1);
-	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
-	}
 	res = sscanf(buf, "%x %x", &offset, &value);
 	if (res != 2) {
 		res = -EFAULT;
@@ -684,7 +638,7 @@ static ssize_t lbs_wrrf_write(struct file *file,
 	if (!res)
 		res = count;
 out_unlock:
-	free_page(addr);
+	kfree(buf);
 	return res;
 }
 
@@ -915,16 +869,9 @@ static ssize_t lbs_debugfs_write(struct file *f, const char __user *buf,
 	if (cnt == 0)
 		return 0;
 
-	pdata = kmalloc(cnt + 1, GFP_KERNEL);
-	if (pdata == NULL)
-		return 0;
-
-	if (copy_from_user(pdata, buf, cnt)) {
-		lbs_deb_debugfs("Copy from user failed\n");
-		kfree(pdata);
-		return 0;
-	}
-	pdata[cnt] = '\0';
+	pdata = memdup_user_nul(buf, cnt);
+	if (IS_ERR(pdata))
+		return PTR_ERR(pdata);
 
 	p0 = pdata;
 	for (i = 0; i < num_of_items; i++) {
diff --git a/drivers/net/wireless/libertas/debugfs.h b/drivers/net/wireless/marvell/libertas/debugfs.h
index f2b9c7ffe0fd..f2b9c7ffe0fd 100644
--- a/drivers/net/wireless/libertas/debugfs.h
+++ b/drivers/net/wireless/marvell/libertas/debugfs.h
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/marvell/libertas/decl.h
index 84a3aa7ac570..84a3aa7ac570 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/marvell/libertas/decl.h
diff --git a/drivers/net/wireless/libertas/defs.h b/drivers/net/wireless/marvell/libertas/defs.h
index 407784aca627..407784aca627 100644
--- a/drivers/net/wireless/libertas/defs.h
+++ b/drivers/net/wireless/marvell/libertas/defs.h
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/marvell/libertas/dev.h
index 6bd1608992b0..6bd1608992b0 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/marvell/libertas/dev.h
diff --git a/drivers/net/wireless/libertas/ethtool.c b/drivers/net/wireless/marvell/libertas/ethtool.c
index f955b2d66ed6..f955b2d66ed6 100644
--- a/drivers/net/wireless/libertas/ethtool.c
+++ b/drivers/net/wireless/marvell/libertas/ethtool.c
diff --git a/drivers/net/wireless/libertas/firmware.c b/drivers/net/wireless/marvell/libertas/firmware.c
index 51b92b5df119..51b92b5df119 100644
--- a/drivers/net/wireless/libertas/firmware.c
+++ b/drivers/net/wireless/marvell/libertas/firmware.c
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/marvell/libertas/host.h
index 96726f79a1dd..96726f79a1dd 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/marvell/libertas/host.h
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/marvell/libertas/if_cs.c
index f499efc6abcf..f499efc6abcf 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/marvell/libertas/if_cs.c
diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/marvell/libertas/if_sdio.c
index 33ceda296c9c..68fd3a9779bd 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/marvell/libertas/if_sdio.c
@@ -228,7 +228,7 @@ static int if_sdio_handle_cmd(struct if_sdio_card *card,
 	memcpy(priv->resp_buf[i], buffer, size);
 	lbs_notify_command_response(priv, i);
 
-	spin_unlock_irqrestore(&card->priv->driver_lock, flags);
+	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 	ret = 0;
 
diff --git a/drivers/net/wireless/libertas/if_sdio.h b/drivers/net/wireless/marvell/libertas/if_sdio.h
index 62fda3592f67..62fda3592f67 100644
--- a/drivers/net/wireless/libertas/if_sdio.h
+++ b/drivers/net/wireless/marvell/libertas/if_sdio.h
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/marvell/libertas/if_spi.c
index 82c0796377aa..82c0796377aa 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/marvell/libertas/if_spi.c
diff --git a/drivers/net/wireless/libertas/if_spi.h b/drivers/net/wireless/marvell/libertas/if_spi.h
index e450e31fd11d..e450e31fd11d 100644
--- a/drivers/net/wireless/libertas/if_spi.h
+++ b/drivers/net/wireless/marvell/libertas/if_spi.h
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/marvell/libertas/if_usb.c
index dff08a2896a3..dff08a2896a3 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/marvell/libertas/if_usb.c
diff --git a/drivers/net/wireless/libertas/if_usb.h b/drivers/net/wireless/marvell/libertas/if_usb.h
index 6e42eac331de..6e42eac331de 100644
--- a/drivers/net/wireless/libertas/if_usb.h
+++ b/drivers/net/wireless/marvell/libertas/if_usb.h
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/marvell/libertas/main.c
index 8079560f4965..8079560f4965 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/marvell/libertas/main.c
diff --git a/drivers/net/wireless/libertas/mesh.c b/drivers/net/wireless/marvell/libertas/mesh.c
index d0c881dd5846..d0c881dd5846 100644
--- a/drivers/net/wireless/libertas/mesh.c
+++ b/drivers/net/wireless/marvell/libertas/mesh.c
diff --git a/drivers/net/wireless/libertas/mesh.h b/drivers/net/wireless/marvell/libertas/mesh.h
index 6603f341c874..6603f341c874 100644
--- a/drivers/net/wireless/libertas/mesh.h
+++ b/drivers/net/wireless/marvell/libertas/mesh.h
diff --git a/drivers/net/wireless/libertas/radiotap.h b/drivers/net/wireless/marvell/libertas/radiotap.h
index b3c8ea6d610e..b3c8ea6d610e 100644
--- a/drivers/net/wireless/libertas/radiotap.h
+++ b/drivers/net/wireless/marvell/libertas/radiotap.h
diff --git a/drivers/net/wireless/libertas/rx.c b/drivers/net/wireless/marvell/libertas/rx.c
index e446fed7b345..e446fed7b345 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/marvell/libertas/rx.c
diff --git a/drivers/net/wireless/libertas/tx.c b/drivers/net/wireless/marvell/libertas/tx.c
index c025f9c18282..c025f9c18282 100644
--- a/drivers/net/wireless/libertas/tx.c
+++ b/drivers/net/wireless/marvell/libertas/tx.c
diff --git a/drivers/net/wireless/libertas/types.h b/drivers/net/wireless/marvell/libertas/types.h
index cf1d9b047ee6..cf1d9b047ee6 100644
--- a/drivers/net/wireless/libertas/types.h
+++ b/drivers/net/wireless/marvell/libertas/types.h
diff --git a/drivers/net/wireless/marvell/libertas_tf/Kconfig b/drivers/net/wireless/marvell/libertas_tf/Kconfig
new file mode 100644
index 000000000000..b5557af90048
--- /dev/null
+++ b/drivers/net/wireless/marvell/libertas_tf/Kconfig
@@ -0,0 +1,18 @@
+config LIBERTAS_THINFIRM
+	tristate "Marvell 8xxx Libertas WLAN driver support with thin firmware"
+	depends on MAC80211
+	select FW_LOADER
+	---help---
+	  A library for Marvell Libertas 8xxx devices using thinfirm.
+
+config LIBERTAS_THINFIRM_DEBUG
+	bool "Enable full debugging output in the Libertas thin firmware module."
+	depends on LIBERTAS_THINFIRM
+	---help---
+	  Debugging support.
+
+config LIBERTAS_THINFIRM_USB
+	tristate "Marvell Libertas 8388 USB 802.11b/g cards with thin firmware"
+	depends on LIBERTAS_THINFIRM && USB
+	---help---
+	  A driver for Marvell Libertas 8388 USB devices using thinfirm.
diff --git a/drivers/net/wireless/libertas_tf/Makefile b/drivers/net/wireless/marvell/libertas_tf/Makefile
index ff5544d6ac9d..ff5544d6ac9d 100644
--- a/drivers/net/wireless/libertas_tf/Makefile
+++ b/drivers/net/wireless/marvell/libertas_tf/Makefile
diff --git a/drivers/net/wireless/libertas_tf/cmd.c b/drivers/net/wireless/marvell/libertas_tf/cmd.c
index 909ac3685010..909ac3685010 100644
--- a/drivers/net/wireless/libertas_tf/cmd.c
+++ b/drivers/net/wireless/marvell/libertas_tf/cmd.c
diff --git a/drivers/net/wireless/libertas_tf/deb_defs.h b/drivers/net/wireless/marvell/libertas_tf/deb_defs.h
index 4bd3dc5adf7c..4bd3dc5adf7c 100644
--- a/drivers/net/wireless/libertas_tf/deb_defs.h
+++ b/drivers/net/wireless/marvell/libertas_tf/deb_defs.h
diff --git a/drivers/net/wireless/libertas_tf/if_usb.c b/drivers/net/wireless/marvell/libertas_tf/if_usb.c
index 799a2efe5793..799a2efe5793 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.c
+++ b/drivers/net/wireless/marvell/libertas_tf/if_usb.c
diff --git a/drivers/net/wireless/libertas_tf/if_usb.h b/drivers/net/wireless/marvell/libertas_tf/if_usb.h
index 6fa5b3f59efe..6fa5b3f59efe 100644
--- a/drivers/net/wireless/libertas_tf/if_usb.h
+++ b/drivers/net/wireless/marvell/libertas_tf/if_usb.h
diff --git a/drivers/net/wireless/libertas_tf/libertas_tf.h b/drivers/net/wireless/marvell/libertas_tf/libertas_tf.h
index ad77b92d0b41..ad77b92d0b41 100644
--- a/drivers/net/wireless/libertas_tf/libertas_tf.h
+++ b/drivers/net/wireless/marvell/libertas_tf/libertas_tf.h
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/marvell/libertas_tf/main.c
index a47f0acc099a..a47f0acc099a 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/marvell/libertas_tf/main.c
diff --git a/drivers/net/wireless/mwifiex/11ac.c b/drivers/net/wireless/marvell/mwifiex/11ac.c
index 59d23fb2365f..59d23fb2365f 100644
--- a/drivers/net/wireless/mwifiex/11ac.c
+++ b/drivers/net/wireless/marvell/mwifiex/11ac.c
diff --git a/drivers/net/wireless/mwifiex/11ac.h b/drivers/net/wireless/marvell/mwifiex/11ac.h
index 1ca92c7a8a4a..1ca92c7a8a4a 100644
--- a/drivers/net/wireless/mwifiex/11ac.h
+++ b/drivers/net/wireless/marvell/mwifiex/11ac.h
diff --git a/drivers/net/wireless/mwifiex/11h.c b/drivers/net/wireless/marvell/mwifiex/11h.c
index 71a1b580796f..71a1b580796f 100644
--- a/drivers/net/wireless/mwifiex/11h.c
+++ b/drivers/net/wireless/marvell/mwifiex/11h.c
diff --git a/drivers/net/wireless/mwifiex/11n.c b/drivers/net/wireless/marvell/mwifiex/11n.c
index c174e79e6df2..c174e79e6df2 100644
--- a/drivers/net/wireless/mwifiex/11n.c
+++ b/drivers/net/wireless/marvell/mwifiex/11n.c
diff --git a/drivers/net/wireless/mwifiex/11n.h b/drivers/net/wireless/marvell/mwifiex/11n.h
index afdd58aa90de..afdd58aa90de 100644
--- a/drivers/net/wireless/mwifiex/11n.h
+++ b/drivers/net/wireless/marvell/mwifiex/11n.h
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.c b/drivers/net/wireless/marvell/mwifiex/11n_aggr.c
index aa498e0d2204..1efef3b8273d 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.c
+++ b/drivers/net/wireless/marvell/mwifiex/11n_aggr.c
@@ -203,8 +203,6 @@ mwifiex_11n_aggregate_pkt(struct mwifiex_private *priv,
 	skb_aggr->priority = skb_src->priority;
 	skb_aggr->tstamp = skb_src->tstamp;
 
-	skb_aggr->tstamp = ktime_get_real();
-
 	do {
 		/* Check if AMSDU can accommodate this MSDU */
 		if (skb_tailroom(skb_aggr) < (skb_src->len + LLC_SNAP_LEN))
diff --git a/drivers/net/wireless/mwifiex/11n_aggr.h b/drivers/net/wireless/marvell/mwifiex/11n_aggr.h
index 0cd2a3eb6c17..0cd2a3eb6c17 100644
--- a/drivers/net/wireless/mwifiex/11n_aggr.h
+++ b/drivers/net/wireless/marvell/mwifiex/11n_aggr.h
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
index b3970a8c9e48..09578c6cde59 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.c
@@ -48,7 +48,17 @@ static int mwifiex_11n_dispatch_amsdu_pkt(struct mwifiex_private *priv,
 					 priv->wdev.iftype, 0, false);
 
 		while (!skb_queue_empty(&list)) {
+			struct rx_packet_hdr *rx_hdr;
+
 			rx_skb = __skb_dequeue(&list);
+			rx_hdr = (struct rx_packet_hdr *)rx_skb->data;
+			if (ISSUPP_TDLS_ENABLED(priv->adapter->fw_cap_info) &&
+			    ntohs(rx_hdr->eth803_hdr.h_proto) == ETH_P_TDLS) {
+				mwifiex_process_tdls_action_frame(priv,
+								  (u8 *)rx_hdr,
+								  skb->len);
+			}
+
 			ret = mwifiex_recv_packet(priv, rx_skb);
 			if (ret == -1)
 				mwifiex_dbg(priv->adapter, ERROR,
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.h b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.h
index 63ecea89b4ab..63ecea89b4ab 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.h
+++ b/drivers/net/wireless/marvell/mwifiex/11n_rxreorder.h
diff --git a/drivers/net/wireless/mwifiex/Kconfig b/drivers/net/wireless/marvell/mwifiex/Kconfig
index 279167ddd293..279167ddd293 100644
--- a/drivers/net/wireless/mwifiex/Kconfig
+++ b/drivers/net/wireless/marvell/mwifiex/Kconfig
diff --git a/drivers/net/wireless/mwifiex/Makefile b/drivers/net/wireless/marvell/mwifiex/Makefile
index fdfd9bf15ed4..fdfd9bf15ed4 100644
--- a/drivers/net/wireless/mwifiex/Makefile
+++ b/drivers/net/wireless/marvell/mwifiex/Makefile
diff --git a/drivers/net/wireless/mwifiex/README b/drivers/net/wireless/marvell/mwifiex/README
index 2f0f9b5609d0..2f0f9b5609d0 100644
--- a/drivers/net/wireless/mwifiex/README
+++ b/drivers/net/wireless/marvell/mwifiex/README
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
index 4073116e6e9f..e7adef72c05f 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/marvell/mwifiex/cfg80211.c
@@ -26,12 +26,10 @@ module_param(reg_alpha2, charp, 0);
 
 static const struct ieee80211_iface_limit mwifiex_ap_sta_limits[] = {
 	{
-		.max = 2, .types = BIT(NL80211_IFTYPE_STATION) |
+		.max = 3, .types = BIT(NL80211_IFTYPE_STATION) |
 				   BIT(NL80211_IFTYPE_P2P_GO) |
-				   BIT(NL80211_IFTYPE_P2P_CLIENT),
-	},
-	{
-		.max = 1, .types = BIT(NL80211_IFTYPE_AP),
+				   BIT(NL80211_IFTYPE_P2P_CLIENT) |
+				   BIT(NL80211_IFTYPE_AP),
 	},
 };
 
@@ -827,18 +825,26 @@ mwifiex_init_new_priv_params(struct mwifiex_private *priv,
 	switch (type) {
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_ADHOC:
+		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
+			 MWIFIEX_BSS_TYPE_STA);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_STA;
 		priv->bss_type = MWIFIEX_BSS_TYPE_STA;
 		break;
 	case NL80211_IFTYPE_P2P_CLIENT:
+		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
+			 MWIFIEX_BSS_TYPE_P2P);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_STA;
 		priv->bss_type = MWIFIEX_BSS_TYPE_P2P;
 		break;
 	case NL80211_IFTYPE_P2P_GO:
+		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
+			 MWIFIEX_BSS_TYPE_P2P);
 		priv->bss_role =  MWIFIEX_BSS_ROLE_UAP;
 		priv->bss_type = MWIFIEX_BSS_TYPE_P2P;
 		break;
 	case NL80211_IFTYPE_AP:
+		priv->bss_num = mwifiex_get_unused_bss_num(adapter,
+			 MWIFIEX_BSS_TYPE_UAP);
 		priv->bss_type = MWIFIEX_BSS_TYPE_UAP;
 		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
 		break;
@@ -1533,6 +1539,7 @@ static const u32 mwifiex_cipher_suites[] = {
 	WLAN_CIPHER_SUITE_WEP104,
 	WLAN_CIPHER_SUITE_TKIP,
 	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_SMS4,
 	WLAN_CIPHER_SUITE_AES_CMAC,
 };
 
@@ -1701,6 +1708,11 @@ mwifiex_cfg80211_del_station(struct wiphy *wiphy, struct net_device *dev,
 	u8 deauth_mac[ETH_ALEN];
 	unsigned long flags;
 
+	if (!priv->bss_started && priv->wdev.cac_started) {
+		mwifiex_dbg(priv->adapter, INFO, "%s: abort CAC!\n", __func__);
+		mwifiex_abort_cac(priv);
+	}
+
 	if (list_empty(&priv->sta_list) || !priv->bss_started)
 		return 0;
 
@@ -2608,7 +2620,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 			return ERR_PTR(-EINVAL);
 		}
 
-		priv = mwifiex_get_unused_priv(adapter);
+		priv = mwifiex_get_unused_priv_by_bss_type(
+						adapter, MWIFIEX_BSS_TYPE_STA);
 		if (!priv) {
 			mwifiex_dbg(adapter, ERROR,
 				    "could not get free private struct\n");
@@ -2627,7 +2640,6 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->frame_type = MWIFIEX_DATA_FRAME_TYPE_ETH_II;
 		priv->bss_priority = 0;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
-		priv->bss_num = adapter->curr_iface_comb.sta_intf;
 
 		break;
 	case NL80211_IFTYPE_AP:
@@ -2638,7 +2650,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 			return ERR_PTR(-EINVAL);
 		}
 
-		priv = mwifiex_get_unused_priv(adapter);
+		priv = mwifiex_get_unused_priv_by_bss_type(
+						adapter, MWIFIEX_BSS_TYPE_UAP);
 		if (!priv) {
 			mwifiex_dbg(adapter, ERROR,
 				    "could not get free private struct\n");
@@ -2653,7 +2666,6 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->bss_priority = 0;
 		priv->bss_role = MWIFIEX_BSS_ROLE_UAP;
 		priv->bss_started = 0;
-		priv->bss_num = adapter->curr_iface_comb.uap_intf;
 		priv->bss_mode = type;
 
 		break;
@@ -2665,7 +2677,8 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 			return ERR_PTR(-EINVAL);
 		}
 
-		priv = mwifiex_get_unused_priv(adapter);
+		priv = mwifiex_get_unused_priv_by_bss_type(
+						adapter, MWIFIEX_BSS_TYPE_P2P);
 		if (!priv) {
 			mwifiex_dbg(adapter, ERROR,
 				    "could not get free private struct\n");
@@ -2689,7 +2702,6 @@ struct wireless_dev *mwifiex_add_virtual_intf(struct wiphy *wiphy,
 		priv->bss_priority = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_role = MWIFIEX_BSS_ROLE_STA;
 		priv->bss_started = 0;
-		priv->bss_num = adapter->curr_iface_comb.p2p_intf;
 
 		if (mwifiex_cfg80211_init_p2p_client(priv)) {
 			memset(&priv->wdev, 0, sizeof(priv->wdev));
@@ -2914,6 +2926,12 @@ mwifiex_is_pattern_supported(struct cfg80211_pkt_pattern *pat, s8 *byte_seq,
 					dont_care_byte = true;
 			}
 
+			/* wildcard bytes record as the offset
+			 * before the valid byte
+			 */
+			if (!valid_byte_cnt && !dont_care_byte)
+				pat->pkt_offset++;
+
 			if (valid_byte_cnt > max_byte_seq)
 				return false;
 		}
@@ -3141,8 +3159,8 @@ static int mwifiex_cfg80211_suspend(struct wiphy *wiphy,
 		memset(&hs_cfg, 0, sizeof(hs_cfg));
 		hs_cfg.is_invoke_hostcmd = false;
 		hs_cfg.conditions = HS_CFG_COND_MAC_EVENT;
-		hs_cfg.gpio = HS_CFG_GPIO_DEF;
-		hs_cfg.gap = HS_CFG_GAP_DEF;
+		hs_cfg.gpio = adapter->hs_cfg.gpio;
+		hs_cfg.gap = adapter->hs_cfg.gap;
 		ret = mwifiex_set_hs_params(priv, HostCmd_ACT_GEN_SET,
 					    MWIFIEX_SYNC_CMD, &hs_cfg);
 		if (ret) {
@@ -3802,6 +3820,10 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 	wiphy->cipher_suites = mwifiex_cipher_suites;
 	wiphy->n_cipher_suites = ARRAY_SIZE(mwifiex_cipher_suites);
 
+	if (adapter->region_code)
+		wiphy->regulatory_flags |= REGULATORY_DISABLE_BEACON_HINTS |
+					   REGULATORY_COUNTRY_IE_IGNORE;
+
 	ether_addr_copy(wiphy->perm_addr, adapter->perm_addr);
 	wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
 	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME |
@@ -3862,11 +3884,15 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 			    "driver hint alpha2: %2.2s\n", reg_alpha2);
 		regulatory_hint(wiphy, reg_alpha2);
 	} else {
-		country_code = mwifiex_11d_code_2_region(adapter->region_code);
-		if (country_code)
-			mwifiex_dbg(adapter, WARN,
-				    "ignoring F/W country code %2.2s\n",
-				    country_code);
+		if (adapter->region_code == 0x00) {
+			mwifiex_dbg(adapter, WARN, "Ignore world regulatory domain\n");
+		} else {
+			country_code =
+				mwifiex_11d_code_2_region(adapter->region_code);
+			if (country_code &&
+			    regulatory_hint(wiphy, country_code))
+				mwifiex_dbg(priv->adapter, ERROR, "regulatory_hint() failed\n");
+		}
 	}
 
 	mwifiex_send_cmd(priv, HostCmd_CMD_802_11_SNMP_MIB,
diff --git a/drivers/net/wireless/mwifiex/cfg80211.h b/drivers/net/wireless/marvell/mwifiex/cfg80211.h
index 908367857d58..908367857d58 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.h
+++ b/drivers/net/wireless/marvell/mwifiex/cfg80211.h
diff --git a/drivers/net/wireless/mwifiex/cfp.c b/drivers/net/wireless/marvell/mwifiex/cfp.c
index 3ddb8ec676ed..09fae27140f7 100644
--- a/drivers/net/wireless/mwifiex/cfp.c
+++ b/drivers/net/wireless/marvell/mwifiex/cfp.c
@@ -66,8 +66,8 @@ static u8 supported_rates_bg[BG_SUPPORTED_RATES] = { 0x02, 0x04, 0x0b, 0x0c,
 					0x12, 0x16, 0x18, 0x24, 0x30, 0x48,
 					0x60, 0x6c, 0 };
 
-u16 region_code_index[MWIFIEX_MAX_REGION_CODE] = { 0x10, 0x20, 0x30,
-						0x32, 0x40, 0x41, 0xff };
+u16 region_code_index[MWIFIEX_MAX_REGION_CODE] = { 0x00, 0x10, 0x20, 0x30,
+						0x31, 0x32, 0x40, 0x41, 0x50 };
 
 static u8 supported_rates_n[N_SUPPORTED_RATES] = { 0x02, 0x04, 0 };
 
@@ -168,7 +168,7 @@ struct region_code_mapping {
 static struct region_code_mapping region_code_mapping_t[] = {
 	{ 0x10, "US " }, /* US FCC */
 	{ 0x20, "CA " }, /* IC Canada */
-	{ 0x30, "EU " }, /* ETSI */
+	{ 0x30, "FR " }, /* France */
 	{ 0x31, "ES " }, /* Spain */
 	{ 0x32, "FR " }, /* France */
 	{ 0x40, "JP " }, /* Japan */
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
index 45ae38e32621..cb25aa7e90db 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/marvell/mwifiex/cmdevt.c
@@ -1637,9 +1637,9 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		if (adapter->region_code == region_code_index[i])
 			break;
 
-	/* If it's unidentified region code, use the default (USA) */
+	/* If it's unidentified region code, use the default (world) */
 	if (i >= MWIFIEX_MAX_REGION_CODE) {
-		adapter->region_code = 0x10;
+		adapter->region_code = 0x00;
 		mwifiex_dbg(adapter, WARN,
 			    "cmd: unknown region code, use default (USA)\n");
 	}
diff --git a/drivers/net/wireless/mwifiex/debugfs.c b/drivers/net/wireless/marvell/mwifiex/debugfs.c
index 9824d8dd2b44..0b9c580af988 100644
--- a/drivers/net/wireless/mwifiex/debugfs.c
+++ b/drivers/net/wireless/marvell/mwifiex/debugfs.c
@@ -447,20 +447,13 @@ static ssize_t
 mwifiex_regrdwr_write(struct file *file,
 		      const char __user *ubuf, size_t count, loff_t *ppos)
 {
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *) addr;
-	size_t buf_size = min_t(size_t, count, PAGE_SIZE - 1);
+	char *buf;
 	int ret;
 	u32 reg_type = 0, reg_offset = 0, reg_value = UINT_MAX;
 
-	if (!buf)
-		return -ENOMEM;
-
-
-	if (copy_from_user(buf, ubuf, buf_size)) {
-		ret = -EFAULT;
-		goto done;
-	}
+	buf = memdup_user_nul(ubuf, min(count, (size_t)(PAGE_SIZE - 1)));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	sscanf(buf, "%u %x %x", &reg_type, &reg_offset, &reg_value);
 
@@ -474,7 +467,7 @@ mwifiex_regrdwr_write(struct file *file,
 		ret = count;
 	}
 done:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -572,17 +565,11 @@ mwifiex_debug_mask_write(struct file *file, const char __user *ubuf,
 	int ret;
 	unsigned long debug_mask;
 	struct mwifiex_private *priv = (void *)file->private_data;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (void *)addr;
-	size_t buf_size = min(count, (size_t)(PAGE_SIZE - 1));
+	char *buf;
 
-	if (!buf)
-		return -ENOMEM;
-
-	if (copy_from_user(buf, ubuf, buf_size)) {
-		ret = -EFAULT;
-		goto done;
-	}
+	buf = memdup_user_nul(ubuf, min(count, (size_t)(PAGE_SIZE - 1)));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	if (kstrtoul(buf, 0, &debug_mask)) {
 		ret = -EINVAL;
@@ -592,7 +579,7 @@ mwifiex_debug_mask_write(struct file *file, const char __user *ubuf,
 	priv->adapter->debug_mask = debug_mask;
 	ret = count;
 done:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -609,17 +596,11 @@ mwifiex_memrw_write(struct file *file, const char __user *ubuf, size_t count,
 	struct mwifiex_ds_mem_rw mem_rw;
 	u16 cmd_action;
 	struct mwifiex_private *priv = (void *)file->private_data;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (void *)addr;
-	size_t buf_size = min(count, (size_t)(PAGE_SIZE - 1));
+	char *buf;
 
-	if (!buf)
-		return -ENOMEM;
-
-	if (copy_from_user(buf, ubuf, buf_size)) {
-		ret = -EFAULT;
-		goto done;
-	}
+	buf = memdup_user_nul(ubuf, min(count, (size_t)(PAGE_SIZE - 1)));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	ret = sscanf(buf, "%c %x %x", &cmd, &mem_rw.addr, &mem_rw.value);
 	if (ret != 3) {
@@ -645,7 +626,7 @@ mwifiex_memrw_write(struct file *file, const char __user *ubuf, size_t count,
 		ret = count;
 
 done:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -686,20 +667,13 @@ static ssize_t
 mwifiex_rdeeprom_write(struct file *file,
 		       const char __user *ubuf, size_t count, loff_t *ppos)
 {
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *) addr;
-	size_t buf_size = min_t(size_t, count, PAGE_SIZE - 1);
+	char *buf;
 	int ret = 0;
 	int offset = -1, bytes = -1;
 
-	if (!buf)
-		return -ENOMEM;
-
-
-	if (copy_from_user(buf, ubuf, buf_size)) {
-		ret = -EFAULT;
-		goto done;
-	}
+	buf = memdup_user_nul(ubuf, min(count, (size_t)(PAGE_SIZE - 1)));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	sscanf(buf, "%d %d", &offset, &bytes);
 
@@ -712,7 +686,7 @@ mwifiex_rdeeprom_write(struct file *file,
 		ret = count;
 	}
 done:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -771,21 +745,15 @@ mwifiex_hscfg_write(struct file *file, const char __user *ubuf,
 		    size_t count, loff_t *ppos)
 {
 	struct mwifiex_private *priv = (void *)file->private_data;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
-	size_t buf_size = min_t(size_t, count, PAGE_SIZE - 1);
+	char *buf;
 	int ret, arg_num;
 	struct mwifiex_ds_hs_cfg hscfg;
 	int conditions = HS_CFG_COND_DEF;
 	u32 gpio = HS_CFG_GPIO_DEF, gap = HS_CFG_GAP_DEF;
 
-	if (!buf)
-		return -ENOMEM;
-
-	if (copy_from_user(buf, ubuf, buf_size)) {
-		ret = -EFAULT;
-		goto done;
-	}
+	buf = memdup_user_nul(ubuf, min(count, (size_t)(PAGE_SIZE - 1)));
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
 
 	arg_num = sscanf(buf, "%d %x %x", &conditions, &gpio, &gap);
 
@@ -823,7 +791,7 @@ mwifiex_hscfg_write(struct file *file, const char __user *ubuf,
 	priv->adapter->hs_enabling = false;
 	ret = count;
 done:
-	free_page(addr);
+	kfree(buf);
 	return ret;
 }
 
@@ -906,6 +874,34 @@ mwifiex_timeshare_coex_write(struct file *file, const char __user *ubuf,
 		return count;
 }
 
+static ssize_t
+mwifiex_reset_write(struct file *file,
+		    const char __user *ubuf, size_t count, loff_t *ppos)
+{
+	struct mwifiex_private *priv = file->private_data;
+	struct mwifiex_adapter *adapter = priv->adapter;
+	char cmd;
+	bool result;
+
+	if (copy_from_user(&cmd, ubuf, sizeof(cmd)))
+		return -EFAULT;
+
+	if (strtobool(&cmd, &result))
+		return -EINVAL;
+
+	if (!result)
+		return -EINVAL;
+
+	if (adapter->if_ops.card_reset) {
+		dev_info(adapter->dev, "Resetting per request\n");
+		adapter->hw_status = MWIFIEX_HW_STATUS_RESET;
+		mwifiex_cancel_all_pending_cmd(adapter);
+		adapter->if_ops.card_reset(adapter);
+	}
+
+	return count;
+}
+
 #define MWIFIEX_DFS_ADD_FILE(name) do {                                 \
 	if (!debugfs_create_file(#name, 0644, priv->dfs_dev_dir,        \
 			priv, &mwifiex_dfs_##name##_fops))              \
@@ -943,6 +939,7 @@ MWIFIEX_DFS_FILE_OPS(hscfg);
 MWIFIEX_DFS_FILE_OPS(histogram);
 MWIFIEX_DFS_FILE_OPS(debug_mask);
 MWIFIEX_DFS_FILE_OPS(timeshare_coex);
+MWIFIEX_DFS_FILE_WRITE_OPS(reset);
 
 /*
  * This function creates the debug FS directory structure and the files.
@@ -970,6 +967,7 @@ mwifiex_dev_debugfs_init(struct mwifiex_private *priv)
 	MWIFIEX_DFS_ADD_FILE(histogram);
 	MWIFIEX_DFS_ADD_FILE(debug_mask);
 	MWIFIEX_DFS_ADD_FILE(timeshare_coex);
+	MWIFIEX_DFS_ADD_FILE(reset);
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/marvell/mwifiex/decl.h
index 098e1f14dc9a..d9c15cd36f12 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/marvell/mwifiex/decl.h
@@ -111,9 +111,9 @@
 /* Rate index for OFDM 0 */
 #define MWIFIEX_RATE_INDEX_OFDM0   4
 
-#define MWIFIEX_MAX_STA_NUM		1
-#define MWIFIEX_MAX_UAP_NUM		1
-#define MWIFIEX_MAX_P2P_NUM		1
+#define MWIFIEX_MAX_STA_NUM		3
+#define MWIFIEX_MAX_UAP_NUM		3
+#define MWIFIEX_MAX_P2P_NUM		3
 
 #define MWIFIEX_A_BAND_START_FREQ	5000
 
diff --git a/drivers/net/wireless/mwifiex/ethtool.c b/drivers/net/wireless/marvell/mwifiex/ethtool.c
index 58400c69ab26..58400c69ab26 100644
--- a/drivers/net/wireless/mwifiex/ethtool.c
+++ b/drivers/net/wireless/marvell/mwifiex/ethtool.c
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/marvell/mwifiex/fw.h
index 1e1e81a0a8d4..ced7af2be29a 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/marvell/mwifiex/fw.h
@@ -537,7 +537,7 @@ enum P2P_MODES {
 #define EVENT_GET_BSS_TYPE(event_cause)         \
 	(((event_cause) >> 24) & 0x00ff)
 
-#define MWIFIEX_MAX_PATTERN_LEN		20
+#define MWIFIEX_MAX_PATTERN_LEN		40
 #define MWIFIEX_MAX_OFFSET_LEN		100
 #define STACK_NBYTES			100
 #define TYPE_DNUM			1
@@ -1092,9 +1092,15 @@ struct host_cmd_ds_802_11_ad_hoc_start {
 	u8 data_rate[HOSTCMD_SUPPORTED_RATES];
 } __packed;
 
-struct host_cmd_ds_802_11_ad_hoc_result {
+struct host_cmd_ds_802_11_ad_hoc_start_result {
 	u8 pad[3];
 	u8 bssid[ETH_ALEN];
+	u8 pad2[2];
+	u8 result;
+} __packed;
+
+struct host_cmd_ds_802_11_ad_hoc_join_result {
+	u8 result;
 } __packed;
 
 struct adhoc_bss_desc {
@@ -2124,7 +2130,8 @@ struct host_cmd_ds_command {
 		struct host_cmd_ds_802_11_associate_rsp associate_rsp;
 		struct host_cmd_ds_802_11_deauthenticate deauth;
 		struct host_cmd_ds_802_11_ad_hoc_start adhoc_start;
-		struct host_cmd_ds_802_11_ad_hoc_result adhoc_result;
+		struct host_cmd_ds_802_11_ad_hoc_start_result start_result;
+		struct host_cmd_ds_802_11_ad_hoc_join_result join_result;
 		struct host_cmd_ds_802_11_ad_hoc_join adhoc_join;
 		struct host_cmd_ds_802_11d_domain_info domain_info;
 		struct host_cmd_ds_802_11d_domain_info_rsp domain_info_resp;
diff --git a/drivers/net/wireless/mwifiex/ie.c b/drivers/net/wireless/marvell/mwifiex/ie.c
index abf52d25b981..c488c3068abc 100644
--- a/drivers/net/wireless/mwifiex/ie.c
+++ b/drivers/net/wireless/marvell/mwifiex/ie.c
@@ -140,7 +140,7 @@ mwifiex_update_autoindex_ies(struct mwifiex_private *priv,
 	if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP)
 		return mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
 					HostCmd_ACT_GEN_SET,
-					UAP_CUSTOM_IE_I, ie_list, false);
+					UAP_CUSTOM_IE_I, ie_list, true);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/marvell/mwifiex/init.c
index de74a7773fb6..6f7876ec31b7 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/marvell/mwifiex/init.c
@@ -95,7 +95,7 @@ int mwifiex_init_priv(struct mwifiex_private *priv)
 	priv->curr_pkt_filter = HostCmd_ACT_MAC_RX_ON | HostCmd_ACT_MAC_TX_ON |
 				HostCmd_ACT_MAC_ETHERNETII_ENABLE;
 
-	priv->beacon_period = 100; /* beacon interval */ ;
+	priv->beacon_period = 100; /* beacon interval */
 	priv->attempted_bss_desc = NULL;
 	memset(&priv->curr_bss_params, 0, sizeof(priv->curr_bss_params));
 	priv->listen_interval = MWIFIEX_DEFAULT_LISTEN_INTERVAL;
diff --git a/drivers/net/wireless/mwifiex/ioctl.h b/drivers/net/wireless/marvell/mwifiex/ioctl.h
index 4f0174c64946..4f0174c64946 100644
--- a/drivers/net/wireless/mwifiex/ioctl.h
+++ b/drivers/net/wireless/marvell/mwifiex/ioctl.h
diff --git a/drivers/net/wireless/mwifiex/join.c b/drivers/net/wireless/marvell/mwifiex/join.c
index 3cda1f956f0b..cc09a81dbf6a 100644
--- a/drivers/net/wireless/mwifiex/join.c
+++ b/drivers/net/wireless/marvell/mwifiex/join.c
@@ -1247,20 +1247,26 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 {
 	int ret = 0;
 	struct mwifiex_adapter *adapter = priv->adapter;
-	struct host_cmd_ds_802_11_ad_hoc_result *adhoc_result;
+	struct host_cmd_ds_802_11_ad_hoc_start_result *start_result =
+				&resp->params.start_result;
+	struct host_cmd_ds_802_11_ad_hoc_join_result *join_result =
+				&resp->params.join_result;
 	struct mwifiex_bssdescriptor *bss_desc;
-	u16 reason_code;
+	u16 cmd = le16_to_cpu(resp->command);
+	u8 result;
 
-	adhoc_result = &resp->params.adhoc_result;
+	if (cmd == HostCmd_CMD_802_11_AD_HOC_START)
+		result = start_result->result;
+	else
+		result = join_result->result;
 
 	bss_desc = priv->attempted_bss_desc;
 
 	/* Join result code 0 --> SUCCESS */
-	reason_code = le16_to_cpu(resp->result);
-	if (reason_code) {
+	if (result) {
 		mwifiex_dbg(priv->adapter, ERROR, "ADHOC_RESP: failed\n");
 		if (priv->media_connected)
-			mwifiex_reset_connect_state(priv, reason_code);
+			mwifiex_reset_connect_state(priv, result);
 
 		memset(&priv->curr_bss_params.bss_descriptor,
 		       0x00, sizeof(struct mwifiex_bssdescriptor));
@@ -1278,7 +1284,7 @@ int mwifiex_ret_802_11_ad_hoc(struct mwifiex_private *priv,
 
 		/* Update the created network descriptor with the new BSSID */
 		memcpy(bss_desc->mac_address,
-		       adhoc_result->bssid, ETH_ALEN);
+		       start_result->bssid, ETH_ALEN);
 
 		priv->adhoc_state = ADHOC_STARTED;
 	} else {
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/marvell/mwifiex/main.c
index 969ca1e1f3e9..79c16de8743e 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/marvell/mwifiex/main.c
@@ -763,7 +763,7 @@ mwifiex_clone_skb_for_tx_status(struct mwifiex_private *priv,
 
 		spin_lock_irqsave(&priv->ack_status_lock, flags);
 		id = idr_alloc(&priv->ack_status_frames, orig_skb,
-			       1, 0xff, GFP_ATOMIC);
+			       1, 0x10, GFP_ATOMIC);
 		spin_unlock_irqrestore(&priv->ack_status_lock, flags);
 
 		if (id >= 0) {
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/marvell/mwifiex/main.h
index 3959f1c97f4e..2f7f478ce04b 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/marvell/mwifiex/main.h
@@ -84,7 +84,7 @@ enum {
 
 #define MWIFIEX_KEY_BUFFER_SIZE			16
 #define MWIFIEX_DEFAULT_LISTEN_INTERVAL 10
-#define MWIFIEX_MAX_REGION_CODE         7
+#define MWIFIEX_MAX_REGION_CODE         9
 
 #define DEFAULT_BCN_AVG_FACTOR          8
 #define DEFAULT_DATA_AVG_FACTOR         8
@@ -564,14 +564,14 @@ struct mwifiex_private {
 	struct mwifiex_wep_key wep_key[NUM_WEP_KEYS];
 	u16 wep_key_curr_index;
 	u8 wpa_ie[256];
-	u8 wpa_ie_len;
+	u16 wpa_ie_len;
 	u8 wpa_is_gtk_set;
 	struct host_cmd_ds_802_11_key_material aes_key;
 	struct host_cmd_ds_802_11_key_material_v2 aes_key_v2;
 	u8 wapi_ie[256];
-	u8 wapi_ie_len;
+	u16 wapi_ie_len;
 	u8 *wps_ie;
-	u8 wps_ie_len;
+	u16 wps_ie_len;
 	u8 wmm_required;
 	u8 wmm_enabled;
 	u8 wmm_qosinfo;
@@ -1273,20 +1273,46 @@ mwifiex_get_priv(struct mwifiex_adapter *adapter,
 }
 
 /*
+ * This function checks available bss_num when adding new interface or
+ * changing interface type.
+ */
+static inline u8
+mwifiex_get_unused_bss_num(struct mwifiex_adapter *adapter, u8 bss_type)
+{
+	u8 i, j;
+	int index[MWIFIEX_MAX_BSS_NUM];
+
+	memset(index, 0, sizeof(index));
+	for (i = 0; i < adapter->priv_num; i++)
+		if (adapter->priv[i]) {
+			if (adapter->priv[i]->bss_type == bss_type &&
+			    !(adapter->priv[i]->bss_mode ==
+			      NL80211_IFTYPE_UNSPECIFIED)) {
+				index[adapter->priv[i]->bss_num] = 1;
+			}
+		}
+	for (j = 0; j < MWIFIEX_MAX_BSS_NUM; j++)
+		if (!index[j])
+			return j;
+	return -1;
+}
+
+/*
  * This function returns the first available unused private structure pointer.
  */
 static inline struct mwifiex_private *
-mwifiex_get_unused_priv(struct mwifiex_adapter *adapter)
+mwifiex_get_unused_priv_by_bss_type(struct mwifiex_adapter *adapter,
+				    u8 bss_type)
 {
-	int i;
+	u8 i;
 
-	for (i = 0; i < adapter->priv_num; i++) {
-		if (adapter->priv[i]) {
-			if (adapter->priv[i]->bss_mode ==
-			    NL80211_IFTYPE_UNSPECIFIED)
-				break;
+	for (i = 0; i < adapter->priv_num; i++)
+		if (adapter->priv[i]->bss_mode ==
+		   NL80211_IFTYPE_UNSPECIFIED) {
+			adapter->priv[i]->bss_num =
+			mwifiex_get_unused_bss_num(adapter, bss_type);
+			break;
 		}
-	}
 
 	return ((i < adapter->priv_num) ? adapter->priv[i] : NULL);
 }
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/marvell/mwifiex/pcie.c
index 21192b6f9c64..6d0dc40e20e5 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/marvell/mwifiex/pcie.c
@@ -2129,14 +2129,14 @@ static irqreturn_t mwifiex_pcie_interrupt(int irq, void *context)
 	struct mwifiex_adapter *adapter;
 
 	if (!pdev) {
-		pr_debug("info: %s: pdev is NULL\n", (u8 *)pdev);
+		pr_err("info: %s: pdev is NULL\n", __func__);
 		goto exit;
 	}
 
 	card = pci_get_drvdata(pdev);
 	if (!card || !card->adapter) {
-		pr_debug("info: %s: card=%p adapter=%p\n", __func__, card,
-			 card ? card->adapter : NULL);
+		pr_err("info: %s: card=%p adapter=%p\n", __func__, card,
+		       card ? card->adapter : NULL);
 		goto exit;
 	}
 	adapter = card->adapter;
@@ -2473,50 +2473,44 @@ static int mwifiex_pcie_init(struct mwifiex_adapter *adapter)
 
 	pci_set_master(pdev);
 
-	mwifiex_dbg(adapter, INFO,
-		    "try set_consistent_dma_mask(32)\n");
+	pr_notice("try set_consistent_dma_mask(32)\n");
 	ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		mwifiex_dbg(adapter, ERROR,
-			    "set_dma_mask(32) failed\n");
+		pr_err("set_dma_mask(32) failed\n");
 		goto err_set_dma_mask;
 	}
 
 	ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		mwifiex_dbg(adapter, ERROR,
-			    "set_consistent_dma_mask(64) failed\n");
+		pr_err("set_consistent_dma_mask(64) failed\n");
 		goto err_set_dma_mask;
 	}
 
 	ret = pci_request_region(pdev, 0, DRV_NAME);
 	if (ret) {
-		mwifiex_dbg(adapter, ERROR,
-			    "req_reg(0) error\n");
+		pr_err("req_reg(0) error\n");
 		goto err_req_region0;
 	}
 	card->pci_mmap = pci_iomap(pdev, 0, 0);
 	if (!card->pci_mmap) {
-		mwifiex_dbg(adapter, ERROR, "iomap(0) error\n");
+		pr_err("iomap(0) error\n");
 		ret = -EIO;
 		goto err_iomap0;
 	}
 	ret = pci_request_region(pdev, 2, DRV_NAME);
 	if (ret) {
-		mwifiex_dbg(adapter, ERROR, "req_reg(2) error\n");
+		pr_err("req_reg(2) error\n");
 		goto err_req_region2;
 	}
 	card->pci_mmap1 = pci_iomap(pdev, 2, 0);
 	if (!card->pci_mmap1) {
-		mwifiex_dbg(adapter, ERROR,
-			    "iomap(2) error\n");
+		pr_err("iomap(2) error\n");
 		ret = -EIO;
 		goto err_iomap2;
 	}
 
-	mwifiex_dbg(adapter, INFO,
-		    "PCI memory map Virt0: %p PCI memory map Virt2: %p\n",
-		    card->pci_mmap, card->pci_mmap1);
+	pr_notice("PCI memory map Virt0: %p PCI memory map Virt2: %p\n",
+		  card->pci_mmap, card->pci_mmap1);
 
 	card->cmdrsp_buf = NULL;
 	ret = mwifiex_pcie_create_txbd_ring(adapter);
@@ -2599,6 +2593,30 @@ static void mwifiex_pcie_cleanup(struct mwifiex_adapter *adapter)
 	kfree(card);
 }
 
+static int mwifiex_pcie_request_irq(struct mwifiex_adapter *adapter)
+{
+	int ret;
+	struct pcie_service_card *card = adapter->card;
+	struct pci_dev *pdev = card->dev;
+
+	if (pci_enable_msi(pdev) != 0)
+		pci_disable_msi(pdev);
+	else
+		card->msi_enable = 1;
+
+	mwifiex_dbg(adapter, INFO, "msi_enable = %d\n", card->msi_enable);
+
+	ret = request_irq(pdev->irq, mwifiex_pcie_interrupt, IRQF_SHARED,
+			  "MRVL_PCIE", pdev);
+	if (ret) {
+		pr_err("request_irq failed: ret=%d\n", ret);
+		adapter->card = NULL;
+		return -1;
+	}
+
+	return 0;
+}
+
 /*
  * This function registers the PCIE device.
  *
@@ -2606,23 +2624,16 @@ static void mwifiex_pcie_cleanup(struct mwifiex_adapter *adapter)
  */
 static int mwifiex_register_dev(struct mwifiex_adapter *adapter)
 {
-	int ret;
 	struct pcie_service_card *card = adapter->card;
 	struct pci_dev *pdev = card->dev;
 
 	/* save adapter pointer in card */
 	card->adapter = adapter;
+	adapter->dev = &pdev->dev;
 
-	ret = request_irq(pdev->irq, mwifiex_pcie_interrupt, IRQF_SHARED,
-			  "MRVL_PCIE", pdev);
-	if (ret) {
-		mwifiex_dbg(adapter, ERROR,
-			    "request_irq failed: ret=%d\n", ret);
-		adapter->card = NULL;
+	if (mwifiex_pcie_request_irq(adapter))
 		return -1;
-	}
 
-	adapter->dev = &pdev->dev;
 	adapter->tx_buf_size = card->pcie.tx_buf_size;
 	adapter->mem_type_mapping_tbl = mem_type_mapping_tbl;
 	adapter->num_mem_types = ARRAY_SIZE(mem_type_mapping_tbl);
diff --git a/drivers/net/wireless/mwifiex/pcie.h b/drivers/net/wireless/marvell/mwifiex/pcie.h
index 48e549c3b285..6fc28737b576 100644
--- a/drivers/net/wireless/mwifiex/pcie.h
+++ b/drivers/net/wireless/marvell/mwifiex/pcie.h
@@ -210,17 +210,17 @@ static const struct mwifiex_pcie_card_reg mwifiex_reg_8997 = {
 	.cmdrsp_addr_lo = PCIE_SCRATCH_4_REG,
 	.cmdrsp_addr_hi = PCIE_SCRATCH_5_REG,
 	.tx_rdptr = 0xC1A4,
-	.tx_wrptr = 0xC1A8,
-	.rx_rdptr = 0xC1A8,
+	.tx_wrptr = 0xC174,
+	.rx_rdptr = 0xC174,
 	.rx_wrptr = 0xC1A4,
 	.evt_rdptr = PCIE_SCRATCH_10_REG,
 	.evt_wrptr = PCIE_SCRATCH_11_REG,
 	.drv_rdy = PCIE_SCRATCH_12_REG,
 	.tx_start_ptr = 16,
 	.tx_mask = 0x0FFF0000,
-	.tx_wrap_mask = 0x01FF0000,
+	.tx_wrap_mask = 0x1FFF0000,
 	.rx_mask = 0x00000FFF,
-	.rx_wrap_mask = 0x000001FF,
+	.rx_wrap_mask = 0x00001FFF,
 	.tx_rollover_ind = BIT(28),
 	.rx_rollover_ind = BIT(12),
 	.evt_rollover_ind = MWIFIEX_BD_FLAG_EVT_ROLLOVER_IND,
@@ -326,6 +326,7 @@ struct pcie_service_card {
 	dma_addr_t sleep_cookie_pbase;
 	void __iomem *pci_mmap;
 	void __iomem *pci_mmap1;
+	int msi_enable;
 };
 
 static inline int
@@ -342,6 +343,7 @@ mwifiex_pcie_txbd_empty(struct pcie_service_card *card, u32 rdptr)
 			return 1;
 		break;
 	case PCIE_DEVICE_ID_MARVELL_88W8897:
+	case PCIE_DEVICE_ID_MARVELL_88W8997:
 		if (((card->txbd_wrptr & reg->tx_mask) ==
 		     (rdptr & reg->tx_mask)) &&
 		    ((card->txbd_wrptr & reg->tx_rollover_ind) ==
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/marvell/mwifiex/scan.c
index c20017ced566..c20017ced566 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/marvell/mwifiex/scan.c
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/marvell/mwifiex/sdio.c
index 78a8474e1a3d..4c8cae682c89 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/marvell/mwifiex/sdio.c
@@ -796,8 +796,8 @@ mwifiex_sdio_interrupt(struct sdio_func *func)
 
 	card = sdio_get_drvdata(func);
 	if (!card || !card->adapter) {
-		pr_debug("int: func=%p card=%p adapter=%p\n",
-			 func, card, card ? card->adapter : NULL);
+		pr_err("int: func=%p card=%p adapter=%p\n",
+		       func, card, card ? card->adapter : NULL);
 		return;
 	}
 	adapter = card->adapter;
@@ -2053,8 +2053,19 @@ static int mwifiex_init_sdio(struct mwifiex_adapter *adapter)
 	/* Allocate skb pointer buffers */
 	card->mpa_rx.skb_arr = kzalloc((sizeof(void *)) *
 				       card->mp_agg_pkt_limit, GFP_KERNEL);
+	if (!card->mpa_rx.skb_arr) {
+		kfree(card->mp_regs);
+		return -ENOMEM;
+	}
+
 	card->mpa_rx.len_arr = kzalloc(sizeof(*card->mpa_rx.len_arr) *
 				       card->mp_agg_pkt_limit, GFP_KERNEL);
+	if (!card->mpa_rx.len_arr) {
+		kfree(card->mp_regs);
+		kfree(card->mpa_rx.skb_arr);
+		return -ENOMEM;
+	}
+
 	ret = mwifiex_alloc_sdio_mpa_buffers(adapter,
 					     card->mp_tx_agg_buf_size,
 					     card->mp_rx_agg_buf_size);
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/marvell/mwifiex/sdio.h
index b9fbc5cf6262..b9fbc5cf6262 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/marvell/mwifiex/sdio.h
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
index e486867a4c67..e486867a4c67 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_cmd.c
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c
index 9ac7aa2431b4..9ac7aa2431b4 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_cmdresp.c
diff --git a/drivers/net/wireless/mwifiex/sta_event.c b/drivers/net/wireless/marvell/mwifiex/sta_event.c
index ff3ee9dfbbd5..ff3ee9dfbbd5 100644
--- a/drivers/net/wireless/mwifiex/sta_event.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_event.c
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/marvell/mwifiex/sta_ioctl.c
index a6c8a4f7bfe9..6a4fc5d183cf 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_ioctl.c
@@ -272,7 +272,8 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 	priv->scan_block = false;
 
 	if (bss) {
-		mwifiex_process_country_ie(priv, bss);
+		if (adapter->region_code == 0x00)
+			mwifiex_process_country_ie(priv, bss);
 
 		/* Allocate and fill new bss descriptor */
 		bss_desc = kzalloc(sizeof(struct mwifiex_bssdescriptor),
@@ -758,7 +759,7 @@ static int mwifiex_set_wpa_ie_helper(struct mwifiex_private *priv,
 			return -1;
 		}
 		memcpy(priv->wpa_ie, ie_data_ptr, ie_len);
-		priv->wpa_ie_len = (u8) ie_len;
+		priv->wpa_ie_len = ie_len;
 		mwifiex_dbg(priv->adapter, CMD,
 			    "cmd: Set Wpa_ie_len=%d IE=%#x\n",
 			    priv->wpa_ie_len, priv->wpa_ie[0]);
@@ -923,9 +924,8 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 		if (encrypt_key->key_disable) {
 			memset(&priv->wep_key[index], 0,
 			       sizeof(struct mwifiex_wep_key));
-			if (wep_key->key_length)
-				goto done;
-			}
+			goto done;
+		}
 
 		if (adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2)
 			enc_key = encrypt_key;
@@ -1293,6 +1293,8 @@ mwifiex_set_gen_ie_helper(struct mwifiex_private *priv, u8 *ie_data_ptr,
 	struct ieee_types_vendor_header *pvendor_ie;
 	const u8 wpa_oui[] = { 0x00, 0x50, 0xf2, 0x01 };
 	const u8 wps_oui[] = { 0x00, 0x50, 0xf2, 0x04 };
+	u16 unparsed_len = ie_len;
+	int find_wpa_ie = 0;
 
 	/* If the passed length is zero, reset the buffer */
 	if (!ie_len) {
@@ -1304,40 +1306,69 @@ mwifiex_set_gen_ie_helper(struct mwifiex_private *priv, u8 *ie_data_ptr,
 		return -1;
 	}
 	pvendor_ie = (struct ieee_types_vendor_header *) ie_data_ptr;
-	/* Test to see if it is a WPA IE, if not, then it is a gen IE */
-	if (((pvendor_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) &&
-	     (!memcmp(pvendor_ie->oui, wpa_oui, sizeof(wpa_oui)))) ||
-	    (pvendor_ie->element_id == WLAN_EID_RSN)) {
 
-		/* IE is a WPA/WPA2 IE so call set_wpa function */
-		ret = mwifiex_set_wpa_ie_helper(priv, ie_data_ptr, ie_len);
-		priv->wps.session_enable = false;
+	while (pvendor_ie) {
+		if (pvendor_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) {
+			/* Test to see if it is a WPA IE, if not, then it is a
+			 * gen IE
+			 */
+			if (!memcmp(pvendor_ie->oui, wpa_oui,
+				    sizeof(wpa_oui))) {
+				find_wpa_ie = 1;
+				break;
+			}
 
-		return ret;
-	} else if (pvendor_ie->element_id == WLAN_EID_BSS_AC_ACCESS_DELAY) {
+			/* Test to see if it is a WPS IE, if so, enable
+			 * wps session flag
+			 */
+			if (!memcmp(pvendor_ie->oui, wps_oui,
+				    sizeof(wps_oui))) {
+				priv->wps.session_enable = true;
+				mwifiex_dbg(priv->adapter, MSG,
+					    "info: WPS Session Enabled.\n");
+				ret = mwifiex_set_wps_ie(priv,
+							 (u8 *)pvendor_ie,
+							 unparsed_len);
+			}
+		}
+
+		if (pvendor_ie->element_id == WLAN_EID_RSN) {
+			find_wpa_ie = 1;
+			break;
+		}
+
+		if (pvendor_ie->element_id == WLAN_EID_BSS_AC_ACCESS_DELAY) {
 		/* IE is a WAPI IE so call set_wapi function */
-		ret = mwifiex_set_wapi_ie(priv, ie_data_ptr, ie_len);
+			ret = mwifiex_set_wapi_ie(priv, (u8 *)pvendor_ie,
+						  unparsed_len);
+			return ret;
+		}
 
+		unparsed_len -= (pvendor_ie->len +
+				 sizeof(struct ieee_types_header));
+
+		if (unparsed_len <= sizeof(struct ieee_types_header))
+			pvendor_ie = NULL;
+		else
+			pvendor_ie = (struct ieee_types_vendor_header *)
+				(((u8 *)pvendor_ie) + pvendor_ie->len +
+				 sizeof(struct ieee_types_header));
+	}
+
+	if (find_wpa_ie) {
+		/* IE is a WPA/WPA2 IE so call set_wpa function */
+		ret = mwifiex_set_wpa_ie_helper(priv, (u8 *)pvendor_ie,
+						unparsed_len);
+		priv->wps.session_enable = false;
 		return ret;
 	}
+
 	/*
 	 * Verify that the passed length is not larger than the
 	 * available space remaining in the buffer
 	 */
 	if (ie_len < (sizeof(priv->gen_ie_buf) - priv->gen_ie_buf_len)) {
 
-		/* Test to see if it is a WPS IE, if so, enable
-		 * wps session flag
-		 */
-		pvendor_ie = (struct ieee_types_vendor_header *) ie_data_ptr;
-		if ((pvendor_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) &&
-		    (!memcmp(pvendor_ie->oui, wps_oui, sizeof(wps_oui)))) {
-			priv->wps.session_enable = true;
-			mwifiex_dbg(priv->adapter, INFO,
-				    "info: WPS Session Enabled.\n");
-			ret = mwifiex_set_wps_ie(priv, ie_data_ptr, ie_len);
-		}
-
 		/* Append the passed data to the end of the
 		   genIeBuffer */
 		memcpy(priv->gen_ie_buf + priv->gen_ie_buf_len, ie_data_ptr,
diff --git a/drivers/net/wireless/mwifiex/sta_rx.c b/drivers/net/wireless/marvell/mwifiex/sta_rx.c
index d4d4cb1ce95b..00fcbda09349 100644
--- a/drivers/net/wireless/mwifiex/sta_rx.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_rx.c
@@ -215,7 +215,7 @@ int mwifiex_process_sta_rx_packet(struct mwifiex_private *priv,
 	if (rx_pkt_type == PKT_TYPE_MGMT) {
 		ret = mwifiex_process_mgmt_packet(priv, skb);
 		if (ret)
-			mwifiex_dbg(adapter, ERROR, "Rx of mgmt packet failed");
+			mwifiex_dbg(adapter, DATA, "Rx of mgmt packet failed");
 		dev_kfree_skb_any(skb);
 		return ret;
 	}
diff --git a/drivers/net/wireless/mwifiex/sta_tx.c b/drivers/net/wireless/marvell/mwifiex/sta_tx.c
index f6683ea6bd5d..f6683ea6bd5d 100644
--- a/drivers/net/wireless/mwifiex/sta_tx.c
+++ b/drivers/net/wireless/marvell/mwifiex/sta_tx.c
diff --git a/drivers/net/wireless/mwifiex/tdls.c b/drivers/net/wireless/marvell/mwifiex/tdls.c
index 9275f9c3f869..9275f9c3f869 100644
--- a/drivers/net/wireless/mwifiex/tdls.c
+++ b/drivers/net/wireless/marvell/mwifiex/tdls.c
diff --git a/drivers/net/wireless/mwifiex/txrx.c b/drivers/net/wireless/marvell/mwifiex/txrx.c
index bf6182b646a5..bf6182b646a5 100644
--- a/drivers/net/wireless/mwifiex/txrx.c
+++ b/drivers/net/wireless/marvell/mwifiex/txrx.c
diff --git a/drivers/net/wireless/mwifiex/uap_cmd.c b/drivers/net/wireless/marvell/mwifiex/uap_cmd.c
index 759a6ada5b0f..e791166d90c4 100644
--- a/drivers/net/wireless/mwifiex/uap_cmd.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_cmd.c
@@ -848,9 +848,9 @@ int mwifiex_config_start_uap(struct mwifiex_private *priv,
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_SYS_CONFIG,
 			     HostCmd_ACT_GEN_SET,
-			     UAP_BSS_PARAMS_I, bss_cfg, false)) {
+			     UAP_BSS_PARAMS_I, bss_cfg, true)) {
 		mwifiex_dbg(priv->adapter, ERROR,
-			    "Failed to set the SSID\n");
+			    "Failed to set AP configuration\n");
 		return -1;
 	}
 
@@ -865,7 +865,7 @@ int mwifiex_config_start_uap(struct mwifiex_private *priv,
 	}
 
 	if (mwifiex_send_cmd(priv, HostCmd_CMD_UAP_BSS_START,
-			     HostCmd_ACT_GEN_SET, 0, NULL, false)) {
+			     HostCmd_ACT_GEN_SET, 0, NULL, true)) {
 		mwifiex_dbg(priv->adapter, ERROR,
 			    "Failed to start the BSS\n");
 		return -1;
diff --git a/drivers/net/wireless/mwifiex/uap_event.c b/drivers/net/wireless/marvell/mwifiex/uap_event.c
index 86ff54296f39..86ff54296f39 100644
--- a/drivers/net/wireless/mwifiex/uap_event.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_event.c
diff --git a/drivers/net/wireless/mwifiex/uap_txrx.c b/drivers/net/wireless/marvell/mwifiex/uap_txrx.c
index 74d5d7238633..52f7981a8afc 100644
--- a/drivers/net/wireless/mwifiex/uap_txrx.c
+++ b/drivers/net/wireless/marvell/mwifiex/uap_txrx.c
@@ -310,8 +310,7 @@ int mwifiex_process_uap_rx_packet(struct mwifiex_private *priv,
 	if (rx_pkt_type == PKT_TYPE_MGMT) {
 		ret = mwifiex_process_mgmt_packet(priv, skb);
 		if (ret)
-			mwifiex_dbg(adapter, ERROR,
-				    "Rx of mgmt packet failed");
+			mwifiex_dbg(adapter, DATA, "Rx of mgmt packet failed");
 		dev_kfree_skb_any(skb);
 		return ret;
 	}
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/marvell/mwifiex/usb.c
index e43aff932360..e43aff932360 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/marvell/mwifiex/usb.c
diff --git a/drivers/net/wireless/mwifiex/usb.h b/drivers/net/wireless/marvell/mwifiex/usb.h
index b4e9246bbcdc..b4e9246bbcdc 100644
--- a/drivers/net/wireless/mwifiex/usb.h
+++ b/drivers/net/wireless/marvell/mwifiex/usb.h
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/marvell/mwifiex/util.c
index 0cec8a64473e..0cec8a64473e 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/marvell/mwifiex/util.c
diff --git a/drivers/net/wireless/mwifiex/util.h b/drivers/net/wireless/marvell/mwifiex/util.h
index b541d66c01eb..b541d66c01eb 100644
--- a/drivers/net/wireless/mwifiex/util.h
+++ b/drivers/net/wireless/marvell/mwifiex/util.h
diff --git a/drivers/net/wireless/mwifiex/wmm.c b/drivers/net/wireless/marvell/mwifiex/wmm.c
index acccd6734e3b..acccd6734e3b 100644
--- a/drivers/net/wireless/mwifiex/wmm.c
+++ b/drivers/net/wireless/marvell/mwifiex/wmm.c
diff --git a/drivers/net/wireless/mwifiex/wmm.h b/drivers/net/wireless/marvell/mwifiex/wmm.h
index 38f09762bd2f..38f09762bd2f 100644
--- a/drivers/net/wireless/mwifiex/wmm.h
+++ b/drivers/net/wireless/marvell/mwifiex/wmm.h
diff --git a/drivers/net/wireless/mwl8k.c b/drivers/net/wireless/marvell/mwl8k.c
index 30e3aaae32e2..30e3aaae32e2 100644
--- a/drivers/net/wireless/mwl8k.c
+++ b/drivers/net/wireless/marvell/mwl8k.c
diff --git a/drivers/net/wireless/mediatek/Kconfig b/drivers/net/wireless/mediatek/Kconfig
index cba300c6b5da..28843fed750a 100644
--- a/drivers/net/wireless/mediatek/Kconfig
+++ b/drivers/net/wireless/mediatek/Kconfig
@@ -1,10 +1,14 @@
-menuconfig WL_MEDIATEK
-	bool "Mediatek Wireless LAN support"
+config WLAN_VENDOR_MEDIATEK
+	bool "MediaTek devices"
+	default y
 	---help---
-	  Enable community drivers for MediaTek WiFi devices.
-	  Those drivers make use of the Linux mac80211 stack.
+	  If you have a wireless card belonging to this class, say Y.
 
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
 
-if WL_MEDIATEK
+if WLAN_VENDOR_MEDIATEK
 source "drivers/net/wireless/mediatek/mt7601u/Kconfig"
-endif # WL_MEDIATEK
+endif # WLAN_VENDOR_MEDIATEK
diff --git a/drivers/net/wireless/ralink/Kconfig b/drivers/net/wireless/ralink/Kconfig
new file mode 100644
index 000000000000..41dbf3130e2b
--- /dev/null
+++ b/drivers/net/wireless/ralink/Kconfig
@@ -0,0 +1,16 @@
+config WLAN_VENDOR_RALINK
+	bool "Ralink devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_RALINK
+
+source "drivers/net/wireless/ralink/rt2x00/Kconfig"
+
+endif # WLAN_VENDOR_RALINK
diff --git a/drivers/net/wireless/ralink/Makefile b/drivers/net/wireless/ralink/Makefile
new file mode 100644
index 000000000000..f84c0a2e4f4d
--- /dev/null
+++ b/drivers/net/wireless/ralink/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_RT2X00)	+= rt2x00/
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/ralink/rt2x00/Kconfig
index de62f5dcb62f..de62f5dcb62f 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/ralink/rt2x00/Kconfig
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/ralink/rt2x00/Makefile
index 24a66015a495..24a66015a495 100644
--- a/drivers/net/wireless/rt2x00/Makefile
+++ b/drivers/net/wireless/ralink/rt2x00/Makefile
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/ralink/rt2x00/rt2400pci.c
index 9a3966cd6fbe..155f343981fe 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2400pci.c
@@ -273,8 +273,10 @@ static void rt2400pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
 	rt2x00mmio_register_write(rt2x00dev, RXCSR0, reg);
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.h b/drivers/net/wireless/ralink/rt2x00/rt2400pci.h
index 0fd3a9d01a60..0fd3a9d01a60 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2400pci.h
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/ralink/rt2x00/rt2500pci.c
index 1a6740b4d396..2553cdd74066 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500pci.c
@@ -274,8 +274,10 @@ static void rt2500pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, RXCSR0_DROP_MCAST,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.h b/drivers/net/wireless/ralink/rt2x00/rt2500pci.h
index 573e87bcc553..573e87bcc553 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500pci.h
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/ralink/rt2x00/rt2500usb.c
index b50d873145d5..2d64611de300 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500usb.c
@@ -229,7 +229,10 @@ static void _rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
 				     const unsigned int offset,
 				     u32 *value)
 {
-	rt2500usb_register_read(rt2x00dev, offset, (u16 *)value);
+	u16 tmp;
+
+	rt2500usb_register_read(rt2x00dev, offset, &tmp);
+	*value = tmp;
 }
 
 static void _rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
@@ -434,8 +437,10 @@ static void rt2500usb_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CONTROL,
 			   !(filter_flags & FIF_CONTROL));
-	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_TODS,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_MULTICAST,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.h b/drivers/net/wireless/ralink/rt2x00/rt2500usb.h
index 78cc035b2d17..78cc035b2d17 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2500usb.h
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/ralink/rt2x00/rt2800.h
index 95c1d7c0a2f3..95c1d7c0a2f3 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800.h
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
index 9733b31a780d..a26afcab03ed 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.c
@@ -1490,7 +1490,8 @@ void rt2800_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_FCSFAIL));
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_PHY_ERROR,
 			   !(filter_flags & FIF_PLCPFAIL));
-	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_NOT_MY_BSSD, 0);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_VER_ERROR, 1);
 	rt2x00_set_field32(&reg, RX_FILTER_CFG_DROP_MULTICAST,
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.h b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
index 440790b92b19..440790b92b19 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800lib.h
diff --git a/drivers/net/wireless/rt2x00/rt2800mmio.c b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c
index de4790b41be7..de4790b41be7 100644
--- a/drivers/net/wireless/rt2x00/rt2800mmio.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.c
diff --git a/drivers/net/wireless/rt2x00/rt2800mmio.h b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.h
index b63312ce3f27..b63312ce3f27 100644
--- a/drivers/net/wireless/rt2x00/rt2800mmio.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800mmio.h
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/ralink/rt2x00/rt2800pci.c
index 0af22573a2eb..0af22573a2eb 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800pci.c
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.h b/drivers/net/wireless/ralink/rt2x00/rt2800pci.h
index 9dfef4607d6b..9dfef4607d6b 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800pci.h
diff --git a/drivers/net/wireless/rt2x00/rt2800soc.c b/drivers/net/wireless/ralink/rt2x00/rt2800soc.c
index a985a5a7945e..a985a5a7945e 100644
--- a/drivers/net/wireless/rt2x00/rt2800soc.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800soc.c
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/ralink/rt2x00/rt2800usb.c
index bf9afbf46c1b..bf9afbf46c1b 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800usb.c
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.h b/drivers/net/wireless/ralink/rt2x00/rt2800usb.h
index ea7cac095997..ea7cac095997 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2800usb.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/ralink/rt2x00/rt2x00.h
index 3282ddb766f4..26427140a963 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00.h
@@ -669,6 +669,7 @@ enum rt2x00_state_flags {
 	CONFIG_POWERSAVING,
 	CONFIG_HT_DISABLED,
 	CONFIG_QOS_DISABLED,
+	CONFIG_MONITORING,
 
 	/*
 	 * Mark we currently are sequentially reading TX_STA_FIFO register
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/ralink/rt2x00/rt2x00config.c
index 7e8bb1198ae9..6a1f508d472f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00config.c
@@ -277,6 +277,11 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 	else
 		clear_bit(CONFIG_POWERSAVING, &rt2x00dev->flags);
 
+	if (conf->flags & IEEE80211_CONF_MONITOR)
+		set_bit(CONFIG_MONITORING, &rt2x00dev->flags);
+	else
+		clear_bit(CONFIG_MONITORING, &rt2x00dev->flags);
+
 	rt2x00dev->curr_band = conf->chandef.chan->band;
 	rt2x00dev->curr_freq = conf->chandef.chan->center_freq;
 	rt2x00dev->tx_power = conf->power_level;
diff --git a/drivers/net/wireless/rt2x00/rt2x00crypto.c b/drivers/net/wireless/ralink/rt2x00/rt2x00crypto.c
index a2fd05ba25ca..a2fd05ba25ca 100644
--- a/drivers/net/wireless/rt2x00/rt2x00crypto.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00crypto.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
index 90fdb02b55e7..90fdb02b55e7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.h b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h
index e65712c235bd..e65712c235bd 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00debug.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/ralink/rt2x00/rt2x00dev.c
index 5639ed816813..5639ed816813 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00dev.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00dump.h b/drivers/net/wireless/ralink/rt2x00/rt2x00dump.h
index 4c0e01b5d515..4c0e01b5d515 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dump.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00dump.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00firmware.c b/drivers/net/wireless/ralink/rt2x00/rt2x00firmware.c
index 5813300f68a2..5813300f68a2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00firmware.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00firmware.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00leds.c b/drivers/net/wireless/ralink/rt2x00/rt2x00leds.c
index c681d04b506c..c681d04b506c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00leds.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00leds.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00leds.h b/drivers/net/wireless/ralink/rt2x00/rt2x00leds.h
index b2c5269570da..b2c5269570da 100644
--- a/drivers/net/wireless/rt2x00/rt2x00leds.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00leds.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/ralink/rt2x00/rt2x00lib.h
index fb7c349ccc9c..fb7c349ccc9c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00lib.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00link.c b/drivers/net/wireless/ralink/rt2x00/rt2x00link.c
index 017188e5a736..017188e5a736 100644
--- a/drivers/net/wireless/rt2x00/rt2x00link.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00link.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/ralink/rt2x00/rt2x00mac.c
index 3c26ee65a415..13da95a24cf7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00mac.c
@@ -385,11 +385,6 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
 			*total_flags |= FIF_PSPOLL;
 	}
 
-	/*
-	 * Check if there is any work left for us.
-	 */
-	if (rt2x00dev->packet_filter == *total_flags)
-		return;
 	rt2x00dev->packet_filter = *total_flags;
 
 	rt2x00dev->ops->lib->config_filter(rt2x00dev, *total_flags);
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.c b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c
index f0178fd4fe5f..f0178fd4fe5f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mmio.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00mmio.h b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h
index 701c3127efb9..701c3127efb9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mmio.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00mmio.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/ralink/rt2x00/rt2x00pci.c
index d93db4b0371b..eb6dbcd4fddf 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00pci.c
@@ -149,6 +149,7 @@ exit_free_device:
 	ieee80211_free_hw(hw);
 
 exit_release_regions:
+	pci_clear_mwi(pci_dev);
 	pci_release_regions(pci_dev);
 
 exit_disable_device:
@@ -173,6 +174,7 @@ void rt2x00pci_remove(struct pci_dev *pci_dev)
 	/*
 	 * Free the PCI device data.
 	 */
+	pci_clear_mwi(pci_dev);
 	pci_disable_device(pci_dev);
 	pci_release_regions(pci_dev);
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/ralink/rt2x00/rt2x00pci.h
index bc0ca5f58f38..bc0ca5f58f38 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00pci.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.c
index 68b620b2462f..68b620b2462f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h
index 2233b911a1d7..2233b911a1d7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00queue.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/ralink/rt2x00/rt2x00reg.h
index 3cc541d13d67..3cc541d13d67 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00reg.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00soc.c b/drivers/net/wireless/ralink/rt2x00/rt2x00soc.c
index 69a0cdadb07f..69a0cdadb07f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00soc.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00soc.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00soc.h b/drivers/net/wireless/ralink/rt2x00/rt2x00soc.h
index 9948d355e9a4..9948d355e9a4 100644
--- a/drivers/net/wireless/rt2x00/rt2x00soc.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00soc.h
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c
index 7627af6098eb..7627af6098eb 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.c
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h
index 569363da00a2..569363da00a2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt2x00usb.h
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/ralink/rt2x00/rt61pci.c
index c0e730ea1b69..24a3436ef952 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt61pci.c
@@ -530,8 +530,10 @@ static void rt61pci_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
 			   !(filter_flags & (FIF_CONTROL | FIF_PSPOLL)));
-	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
diff --git a/drivers/net/wireless/rt2x00/rt61pci.h b/drivers/net/wireless/ralink/rt2x00/rt61pci.h
index 1442075a8382..1442075a8382 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt61pci.h
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/ralink/rt2x00/rt73usb.c
index 7081e13b4fd6..7bbc86931168 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/ralink/rt2x00/rt73usb.c
@@ -480,8 +480,10 @@ static void rt73usb_config_filter(struct rt2x00_dev *rt2x00dev,
 			   !(filter_flags & FIF_PLCPFAIL));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
 			   !(filter_flags & (FIF_CONTROL | FIF_PSPOLL)));
-	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags));
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
+			   !test_bit(CONFIG_MONITORING, &rt2x00dev->flags) &&
 			   !rt2x00dev->intf_ap_count);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.h b/drivers/net/wireless/ralink/rt2x00/rt73usb.h
index 4a4f235466d1..4a4f235466d1 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.h
+++ b/drivers/net/wireless/ralink/rt2x00/rt73usb.h
diff --git a/drivers/net/wireless/realtek/Kconfig b/drivers/net/wireless/realtek/Kconfig
new file mode 100644
index 000000000000..8a8ba2003964
--- /dev/null
+++ b/drivers/net/wireless/realtek/Kconfig
@@ -0,0 +1,18 @@
+config WLAN_VENDOR_REALTEK
+	bool "Realtek devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_REALTEK
+
+source "drivers/net/wireless/realtek/rtl818x/Kconfig"
+source "drivers/net/wireless/realtek/rtlwifi/Kconfig"
+source "drivers/net/wireless/realtek/rtl8xxxu/Kconfig"
+
+endif # WLAN_VENDOR_REALTEK
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
index 53261d6f8578..451456835f87 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8192e2ant.c
@@ -3356,9 +3356,8 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   "Dot11 channel / HsMode(HsChnl)",
 		   wifi_dot11_chnl, bt_hson, wifi_hs_chnl);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
-		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %3ph ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
@@ -3409,17 +3408,9 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 	for (i = 0; i < BT_INFO_SRC_8192E_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "\r\n %-35s = %02x %02x %02x %02x ",
+				   "\r\n %-35s = %7ph(%d)",
 				   GLBtInfoSrc8192e2Ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3]);
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "%02x %02x %02x(%d)",
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h[i],
 				   coex_sta->bt_info_c2h_cnt[i]);
 		}
 	}
@@ -3453,10 +3444,8 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 
 	ps_tdma_case = coex_dm->cur_ps_tdma;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-		   "PS TDMA", coex_dm->ps_tdma_para[0],
-		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   "\r\n %-35s = %5ph case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para,
 		   ps_tdma_case, coex_dm->auto_tdma_adjust);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d ",
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
index c4acd403e5f6..7e239d3cea26 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b1ant.c
@@ -2457,10 +2457,9 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 		   "Dot11 channel / HsChnl(HsMode)",
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %3ph ",
 		   "H2C Wifi inform bt chnl Info",
-		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
-		   coex_dm->wifi_chnl_info[2]);
+		   coex_dm->wifi_chnl_info);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
@@ -2525,15 +2524,9 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   "\r\n %-35s = %7ph(%d)",
 				   GLBtInfoSrc8723b1Ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h[i],
 				   coex_sta->bt_info_c2h_cnt[i]);
 		}
 	}
@@ -2569,10 +2562,8 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 
 		pstdmacase = coex_dm->cur_ps_tdma;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-			   "PS TDMA", coex_dm->ps_tdma_para[0],
-			   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-			   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+			   "\r\n %-35s = %5ph case-%d (auto:%d)",
+			   "PS TDMA", coex_dm->ps_tdma_para,
 			   pstdmacase, coex_dm->auto_tdma_adjust);
 
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d ",
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
index f2b9d11adc9e..c43ab59a690a 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8723b2ant.c
@@ -3215,9 +3215,8 @@ void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   "Dot11 channel / HsChnl(HsMode)",
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
-		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %3ph ",
+		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
@@ -3259,16 +3258,9 @@ void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "\r\n %-35s = %02x %02x %02x "
-				   "%02x %02x %02x %02x(%d)",
+				   "\r\n %-35s = %7ph(%d)",
 				   glbt_info_src_8723b_2ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h[i],
 				   coex_sta->bt_info_c2h_cnt[i]);
 		}
 	}
@@ -3296,10 +3288,8 @@ void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 
 	ps_tdma_case = coex_dm->cur_ps_tdma;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-		   "PS TDMA", coex_dm->ps_tdma_para[0],
-		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
+		   "\r\n %-35s = %5ph case-%d (auto:%d)",
+		   "PS TDMA", coex_dm->ps_tdma_para,
 		   ps_tdma_case, coex_dm->auto_tdma_adjust);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d ",
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
index b72e5377bdbc..9cecf174a37d 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a1ant.c
@@ -2302,10 +2302,9 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		   "\r\n %-35s = %02x %02x %02x ",
+		   "\r\n %-35s = %3ph ",
 		   "H2C Wifi inform bt chnl Info",
-		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
-		   coex_dm->wifi_chnl_info[2]);
+		   coex_dm->wifi_chnl_info);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
@@ -2366,15 +2365,9 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 	for (i = 0; i < BT_INFO_SRC_8821A_1ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   "\r\n %-35s = %7ph(%d)",
 				   glbt_info_src_8821a_1ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h[i],
 				   coex_sta->bt_info_c2h_cnt[i]);
 		}
 	}
@@ -2412,13 +2405,9 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 
 		ps_tdma_case = coex_dm->cur_ps_tdma;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+			   "\r\n %-35s = %5ph case-%d (auto:%d)",
 			   "PS TDMA",
-			   coex_dm->ps_tdma_para[0],
-			   coex_dm->ps_tdma_para[1],
-			   coex_dm->ps_tdma_para[2],
-			   coex_dm->ps_tdma_para[3],
-			   coex_dm->ps_tdma_para[4],
+			   coex_dm->ps_tdma_para,
 			   ps_tdma_case,
 			   coex_dm->auto_tdma_adjust);
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
index cf819f02ed23..044d914291c0 100644
--- a/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
+++ b/drivers/net/wireless/realtek/rtlwifi/btcoexist/halbtc8821a2ant.c
@@ -3393,10 +3393,9 @@ ex_halbtc8821a2ant_display_coex_info(
 		   wifi_dot_11_chnl, bt_hs_on, wifi_hs_chnl);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-		   "\r\n %-35s = %02x %02x %02x ",
+		   "\r\n %-35s = %3ph ",
 		   "H2C Wifi inform bt chnl Info",
-		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
-		   coex_dm->wifi_chnl_info[2]);
+		   coex_dm->wifi_chnl_info);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
@@ -3454,15 +3453,9 @@ ex_halbtc8821a2ant_display_coex_info(
 	for (i = 0; i < BT_INFO_SRC_8821A_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+				   "\r\n %-35s = %7ph(%d)",
 				   glbt_info_src_8821a_2ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
+				   coex_sta->bt_info_c2h[i],
 				   coex_sta->bt_info_c2h_cnt[i]);
 		}
 	}
@@ -3494,11 +3487,9 @@ ex_halbtc8821a2ant_display_coex_info(
 	if (!btcoexist->manual_control) {
 		ps_tdma_case = coex_dm->cur_ps_tdma;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
-			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d",
+			   "\r\n %-35s = %5ph case-%d",
 			   "PS TDMA",
-			   coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
-			   coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
-			   coex_dm->ps_tdma_para[4], ps_tdma_case);
+			   coex_dm->ps_tdma_para, ps_tdma_case);
 
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct",
diff --git a/drivers/net/wireless/realtek/rtlwifi/core.c b/drivers/net/wireless/realtek/rtlwifi/core.c
index c925a4dff599..4ae421ef30d9 100644
--- a/drivers/net/wireless/realtek/rtlwifi/core.c
+++ b/drivers/net/wireless/realtek/rtlwifi/core.c
@@ -1833,8 +1833,7 @@ bool rtl_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb)
 
 	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
+	kfree_skb(pskb);
 
 	/*this is wrong, fill_tx_cmddesc needs update*/
 	pdesc = &ring->desc[0];
diff --git a/drivers/net/wireless/realtek/rtlwifi/pci.c b/drivers/net/wireless/realtek/rtlwifi/pci.c
index f46c9d7f6528..7f471bff435c 100644
--- a/drivers/net/wireless/realtek/rtlwifi/pci.c
+++ b/drivers/net/wireless/realtek/rtlwifi/pci.c
@@ -801,7 +801,9 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 								      hw_queue);
 			if (rx_remained_cnt == 0)
 				return;
-
+			buffer_desc = &rtlpci->rx_ring[rxring_idx].buffer_desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+			pdesc = (struct rtl_rx_desc *)skb->data;
 		} else {	/* rx descriptor */
 			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
 				rtlpci->rx_ring[rxring_idx].idx];
@@ -824,13 +826,6 @@ static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 		new_skb = dev_alloc_skb(rtlpci->rxbuffersize);
 		if (unlikely(!new_skb))
 			goto no_new;
-		if (rtlpriv->use_new_trx_flow) {
-			buffer_desc =
-			  &rtlpci->rx_ring[rxring_idx].buffer_desc
-				[rtlpci->rx_ring[rxring_idx].idx];
-			/*means rx wifi info*/
-			pdesc = (struct rtl_rx_desc *)skb->data;
-		}
 		memset(&rx_status , 0 , sizeof(rx_status));
 		rtlpriv->cfg->ops->query_rx_desc(hw, &stats,
 						 &rx_status, (u8 *)pdesc, skb);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rc.c b/drivers/net/wireless/realtek/rtlwifi/rc.c
index 74c14ce28238..28f7010e7108 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rc.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rc.c
@@ -138,6 +138,11 @@ static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
 		    ((wireless_mode == WIRELESS_MODE_N_5G) ||
 		     (wireless_mode == WIRELESS_MODE_N_24G)))
 			rate->flags |= IEEE80211_TX_RC_MCS;
+		if (sta && sta->vht_cap.vht_supported &&
+		    (wireless_mode == WIRELESS_MODE_AC_5G ||
+		     wireless_mode == WIRELESS_MODE_AC_24G ||
+		     wireless_mode == WIRELESS_MODE_AC_ONLY))
+			rate->flags |= IEEE80211_TX_RC_VHT_MCS;
 	}
 }
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/regd.c b/drivers/net/wireless/realtek/rtlwifi/regd.c
index a62bf0a65c32..5be34118e0af 100644
--- a/drivers/net/wireless/realtek/rtlwifi/regd.c
+++ b/drivers/net/wireless/realtek/rtlwifi/regd.c
@@ -351,7 +351,6 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 	case COUNTRY_CODE_SPAIN:
 	case COUNTRY_CODE_FRANCE:
 	case COUNTRY_CODE_ISRAEL:
-	case COUNTRY_CODE_WORLD_WIDE_13:
 		return &rtl_regdom_12_13;
 	case COUNTRY_CODE_MKK:
 	case COUNTRY_CODE_MKK1:
@@ -360,6 +359,7 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 		return &rtl_regdom_14_60_64;
 	case COUNTRY_CODE_GLOBAL_DOMAIN:
 		return &rtl_regdom_14;
+	case COUNTRY_CODE_WORLD_WIDE_13:
 	case COUNTRY_CODE_WORLD_WIDE_13_5G_ALL:
 		return &rtl_regdom_12_13_5g_all;
 	default:
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
index 11344121c55e..47e32cb0ec1a 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8188ee/sw.c
@@ -88,8 +88,6 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 	u8 tid;
 
 	rtl8188ee_bt_reg_init(hw);
-	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
-
 	rtlpriv->dm.dm_initialgain_enable = 1;
 	rtlpriv->dm.dm_flag = 0;
 	rtlpriv->dm.disable_framebursting = 0;
@@ -138,6 +136,11 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
+	rtlpriv->cfg->mod_params->disable_watchdog =
+		rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
 	if (!rtlpriv->psc.inactiveps)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/sw.c
index de6cb6c3a48c..4780bdc63b2b 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192ce/sw.c
@@ -139,6 +139,8 @@ int rtl92c_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
 	if (!rtlpriv->psc.inactiveps)
 		pr_info("rtl8192ce: Power Save off (module option)\n");
 	if (!rtlpriv->psc.fwctrl_lps)
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/sw.c
index fd4a5353d216..7c6f7f0d18c6 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192cu/sw.c
@@ -65,6 +65,8 @@ static int rtl92cu_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->dm.disable_framebursting = false;
 	rtlpriv->dm.thermalvalue = 0;
 	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
 
 	/* for firmware buf */
 	rtlpriv->rtlhal.pfirmware = vzalloc(0x4000);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/sw.c
index b19d0398215f..c6e09a19de1a 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192de/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192de/sw.c
@@ -376,8 +376,8 @@ module_param_named(swlps, rtl92de_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl92de_mod_params.fwctrl_lps, bool, 0444);
 MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
 MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
-MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
-MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 1)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 0)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8192se/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8192se/sw.c
index e1fd27c888bf..31baca41ac2f 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8192se/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8192se/sw.c
@@ -187,6 +187,8 @@ static int rtl92s_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
 	if (!rtlpriv->psc.inactiveps)
 		pr_info("Power Save off (module option)\n");
 	if (!rtlpriv->psc.fwctrl_lps)
@@ -425,8 +427,8 @@ module_param_named(swlps, rtl92se_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl92se_mod_params.fwctrl_lps, bool, 0444);
 MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
 MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
-MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
-MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 1)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 0)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/sw.c
index 3859b3e3d158..ff49a8c0ff61 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723ae/sw.c
@@ -150,6 +150,11 @@ int rtl8723e_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
+	rtlpriv->cfg->mod_params->disable_watchdog =
+		rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
 	rtlpriv->psc.reg_fwctrl_lps = 3;
@@ -267,6 +272,8 @@ static struct rtl_mod_params rtl8723e_mod_params = {
 	.swctrl_lps = false,
 	.fwctrl_lps = true,
 	.debug = DBG_EMERG,
+	.msi_support = false,
+	.disable_watchdog = false,
 };
 
 static struct rtl_hal_cfg rtl8723e_hal_cfg = {
@@ -383,12 +390,14 @@ module_param_named(debug, rtl8723e_mod_params.debug, int, 0444);
 module_param_named(ips, rtl8723e_mod_params.inactiveps, bool, 0444);
 module_param_named(swlps, rtl8723e_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl8723e_mod_params.fwctrl_lps, bool, 0444);
+module_param_named(msi, rtl8723e_mod_params.msi_support, bool, 0444);
 module_param_named(disable_watchdog, rtl8723e_mod_params.disable_watchdog,
 		   bool, 0444);
 MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
 MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
 MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
 MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 0)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
 MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
index d091f1d5f91e..a78eaeda0008 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723be/sw.c
@@ -93,7 +93,6 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
 	rtl8723be_bt_reg_init(hw);
-	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
 	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
 
 	rtlpriv->dm.dm_initialgain_enable = 1;
@@ -151,6 +150,10 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
 	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		 rtlpriv->cfg->mod_params->sw_crypto;
+	rtlpriv->cfg->mod_params->disable_watchdog =
+		 rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
 	rtlpriv->psc.reg_fwctrl_lps = 3;
@@ -267,6 +270,9 @@ static struct rtl_mod_params rtl8723be_mod_params = {
 	.inactiveps = true,
 	.swctrl_lps = false,
 	.fwctrl_lps = true,
+	.msi_support = false,
+	.disable_watchdog = false,
+	.debug = DBG_EMERG,
 };
 
 static struct rtl_hal_cfg rtl8723be_hal_cfg = {
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8723com/fw_common.c b/drivers/net/wireless/realtek/rtlwifi/rtl8723com/fw_common.c
index a2f5e89bedfe..6e518625edbe 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8723com/fw_common.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8723com/fw_common.c
@@ -318,9 +318,7 @@ bool rtl8723_cmd_send_packet(struct ieee80211_hw *hw,
 	ring = &rtlpci->tx_ring[BEACON_QUEUE];
 
 	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
+	kfree_skb(pskb);
 	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 
 	pdesc = &ring->desc[0];
diff --git a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
index 142bdff4ed60..4159f9b14db6 100644
--- a/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
+++ b/drivers/net/wireless/realtek/rtlwifi/rtl8821ae/sw.c
@@ -95,8 +95,6 @@ int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	rtl8821ae_bt_reg_init(hw);
-	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
-	rtlpci->int_clear = rtlpriv->cfg->mod_params->int_clear;
 	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
 
 	rtlpriv->dm.dm_initialgain_enable = 1;
@@ -168,12 +166,15 @@ int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
 	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
-	rtlpci->msi_support = rtlpriv->cfg->mod_params->int_clear;
+	rtlpci->int_clear = rtlpriv->cfg->mod_params->int_clear;
+	rtlpriv->cfg->mod_params->sw_crypto =
+		rtlpriv->cfg->mod_params->sw_crypto;
+	rtlpriv->cfg->mod_params->disable_watchdog =
+		rtlpriv->cfg->mod_params->disable_watchdog;
 	if (rtlpriv->cfg->mod_params->disable_watchdog)
 		pr_info("watchdog disabled\n");
 	rtlpriv->psc.reg_fwctrl_lps = 3;
 	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
-	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
 
 	/* for ASPM, you can close aspm through
 	 * set const_support_pciaspm = 0
diff --git a/drivers/net/wireless/realtek/rtlwifi/usb.c b/drivers/net/wireless/realtek/rtlwifi/usb.c
index 2721cf89fb16..aac1ed3f7bb4 100644
--- a/drivers/net/wireless/realtek/rtlwifi/usb.c
+++ b/drivers/net/wireless/realtek/rtlwifi/usb.c
@@ -531,6 +531,8 @@ static void _rtl_usb_rx_process_noagg(struct ieee80211_hw *hw,
 			ieee80211_rx(hw, skb);
 		else
 			dev_kfree_skb_any(skb);
+	} else {
+		dev_kfree_skb_any(skb);
 	}
 }
 
diff --git a/drivers/net/wireless/rsi/Kconfig b/drivers/net/wireless/rsi/Kconfig
index 35245f994c10..7c5e4ca4e3d0 100644
--- a/drivers/net/wireless/rsi/Kconfig
+++ b/drivers/net/wireless/rsi/Kconfig
@@ -1,3 +1,16 @@
+config WLAN_VENDOR_RSI
+	bool "Redpine Signals Inc devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_RSI
+
 config RSI_91X
 	tristate "Redpine Signals Inc 91x WLAN driver support"
 	depends on MAC80211
@@ -28,3 +41,5 @@ config RSI_USB
 	---help---
 	  This option enables the USB bus support in rsi drivers.
 	  Select M (recommended), if you have a RSI 1x1 wireless module.
+
+endif # WLAN_VENDOR_RSI
diff --git a/drivers/net/wireless/rsi/rsi_91x_mgmt.c b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
index 8d110fd9eba1..e43b59d5b53b 100644
--- a/drivers/net/wireless/rsi/rsi_91x_mgmt.c
+++ b/drivers/net/wireless/rsi/rsi_91x_mgmt.c
@@ -1023,7 +1023,7 @@ static int rsi_send_auto_rate_request(struct rsi_common *common)
 		return -ENOMEM;
 	}
 
-	selected_rates = kmalloc(2 * RSI_TBL_SZ, GFP_KERNEL);
+	selected_rates = kzalloc(2 * RSI_TBL_SZ, GFP_KERNEL);
 	if (!selected_rates) {
 		rsi_dbg(ERR_ZONE, "%s: Failed in allocation of mem\n",
 			__func__);
@@ -1032,7 +1032,6 @@ static int rsi_send_auto_rate_request(struct rsi_common *common)
 	}
 
 	memset(skb->data, 0, sizeof(struct rsi_auto_rate));
-	memset(selected_rates, 0, 2 * RSI_TBL_SZ);
 
 	auto_rate = (struct rsi_auto_rate *)skb->data;
 
@@ -1227,7 +1226,7 @@ int rsi_send_block_unblock_frame(struct rsi_common *common, bool block_event)
 	mgmt_frame->desc_word[0] = cpu_to_le16(RSI_WIFI_MGMT_Q << 12);
 	mgmt_frame->desc_word[1] = cpu_to_le16(BLOCK_HW_QUEUE);
 
-	if (block_event == true) {
+	if (block_event) {
 		rsi_dbg(INFO_ZONE, "blocking the data qs\n");
 		mgmt_frame->desc_word[4] = cpu_to_le16(0xf);
 	} else {
diff --git a/drivers/net/wireless/st/Kconfig b/drivers/net/wireless/st/Kconfig
new file mode 100644
index 000000000000..969b4f6e53b5
--- /dev/null
+++ b/drivers/net/wireless/st/Kconfig
@@ -0,0 +1,16 @@
+config WLAN_VENDOR_ST
+	bool "STMicroelectronics devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_ST
+
+source "drivers/net/wireless/st/cw1200/Kconfig"
+
+endif # WLAN_VENDOR_ST
diff --git a/drivers/net/wireless/st/Makefile b/drivers/net/wireless/st/Makefile
new file mode 100644
index 000000000000..a60d6350ba46
--- /dev/null
+++ b/drivers/net/wireless/st/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_CW1200)	+= cw1200/
diff --git a/drivers/net/wireless/cw1200/Kconfig b/drivers/net/wireless/st/cw1200/Kconfig
index 0880742eab17..0880742eab17 100644
--- a/drivers/net/wireless/cw1200/Kconfig
+++ b/drivers/net/wireless/st/cw1200/Kconfig
diff --git a/drivers/net/wireless/cw1200/Makefile b/drivers/net/wireless/st/cw1200/Makefile
index b086aac6547a..b086aac6547a 100644
--- a/drivers/net/wireless/cw1200/Makefile
+++ b/drivers/net/wireless/st/cw1200/Makefile
diff --git a/drivers/net/wireless/cw1200/bh.c b/drivers/net/wireless/st/cw1200/bh.c
index 92d299aa257c..92d299aa257c 100644
--- a/drivers/net/wireless/cw1200/bh.c
+++ b/drivers/net/wireless/st/cw1200/bh.c
diff --git a/drivers/net/wireless/cw1200/bh.h b/drivers/net/wireless/st/cw1200/bh.h
index af6a4853728f..af6a4853728f 100644
--- a/drivers/net/wireless/cw1200/bh.h
+++ b/drivers/net/wireless/st/cw1200/bh.h
diff --git a/drivers/net/wireless/cw1200/cw1200.h b/drivers/net/wireless/st/cw1200/cw1200.h
index 1ad7d3602520..1ad7d3602520 100644
--- a/drivers/net/wireless/cw1200/cw1200.h
+++ b/drivers/net/wireless/st/cw1200/cw1200.h
diff --git a/drivers/net/wireless/cw1200/cw1200_sdio.c b/drivers/net/wireless/st/cw1200/cw1200_sdio.c
index d3acc85932a5..d3acc85932a5 100644
--- a/drivers/net/wireless/cw1200/cw1200_sdio.c
+++ b/drivers/net/wireless/st/cw1200/cw1200_sdio.c
diff --git a/drivers/net/wireless/cw1200/cw1200_spi.c b/drivers/net/wireless/st/cw1200/cw1200_spi.c
index a740083634d8..a740083634d8 100644
--- a/drivers/net/wireless/cw1200/cw1200_spi.c
+++ b/drivers/net/wireless/st/cw1200/cw1200_spi.c
diff --git a/drivers/net/wireless/cw1200/debug.c b/drivers/net/wireless/st/cw1200/debug.c
index 34f97c31eecf..34f97c31eecf 100644
--- a/drivers/net/wireless/cw1200/debug.c
+++ b/drivers/net/wireless/st/cw1200/debug.c
diff --git a/drivers/net/wireless/cw1200/debug.h b/drivers/net/wireless/st/cw1200/debug.h
index b525aba53bfc..b525aba53bfc 100644
--- a/drivers/net/wireless/cw1200/debug.h
+++ b/drivers/net/wireless/st/cw1200/debug.h
diff --git a/drivers/net/wireless/cw1200/fwio.c b/drivers/net/wireless/st/cw1200/fwio.c
index 30e7646d04af..30e7646d04af 100644
--- a/drivers/net/wireless/cw1200/fwio.c
+++ b/drivers/net/wireless/st/cw1200/fwio.c
diff --git a/drivers/net/wireless/cw1200/fwio.h b/drivers/net/wireless/st/cw1200/fwio.h
index ea3099362cdf..ea3099362cdf 100644
--- a/drivers/net/wireless/cw1200/fwio.h
+++ b/drivers/net/wireless/st/cw1200/fwio.h
diff --git a/drivers/net/wireless/cw1200/hwbus.h b/drivers/net/wireless/st/cw1200/hwbus.h
index 8b2fc831c3de..8b2fc831c3de 100644
--- a/drivers/net/wireless/cw1200/hwbus.h
+++ b/drivers/net/wireless/st/cw1200/hwbus.h
diff --git a/drivers/net/wireless/cw1200/hwio.c b/drivers/net/wireless/st/cw1200/hwio.c
index ff230b7aeedd..ff230b7aeedd 100644
--- a/drivers/net/wireless/cw1200/hwio.c
+++ b/drivers/net/wireless/st/cw1200/hwio.c
diff --git a/drivers/net/wireless/cw1200/hwio.h b/drivers/net/wireless/st/cw1200/hwio.h
index ddf52669dc5b..ddf52669dc5b 100644
--- a/drivers/net/wireless/cw1200/hwio.h
+++ b/drivers/net/wireless/st/cw1200/hwio.h
diff --git a/drivers/net/wireless/cw1200/main.c b/drivers/net/wireless/st/cw1200/main.c
index 0e51e27d2e3f..0e51e27d2e3f 100644
--- a/drivers/net/wireless/cw1200/main.c
+++ b/drivers/net/wireless/st/cw1200/main.c
diff --git a/drivers/net/wireless/cw1200/pm.c b/drivers/net/wireless/st/cw1200/pm.c
index d2202ae92bdd..d2202ae92bdd 100644
--- a/drivers/net/wireless/cw1200/pm.c
+++ b/drivers/net/wireless/st/cw1200/pm.c
diff --git a/drivers/net/wireless/cw1200/pm.h b/drivers/net/wireless/st/cw1200/pm.h
index 3ed90ff22bb8..3ed90ff22bb8 100644
--- a/drivers/net/wireless/cw1200/pm.h
+++ b/drivers/net/wireless/st/cw1200/pm.h
diff --git a/drivers/net/wireless/cw1200/queue.c b/drivers/net/wireless/st/cw1200/queue.c
index 0ba5ef9b3e7b..0ba5ef9b3e7b 100644
--- a/drivers/net/wireless/cw1200/queue.c
+++ b/drivers/net/wireless/st/cw1200/queue.c
diff --git a/drivers/net/wireless/cw1200/queue.h b/drivers/net/wireless/st/cw1200/queue.h
index 119f9c79c14e..119f9c79c14e 100644
--- a/drivers/net/wireless/cw1200/queue.h
+++ b/drivers/net/wireless/st/cw1200/queue.h
diff --git a/drivers/net/wireless/cw1200/scan.c b/drivers/net/wireless/st/cw1200/scan.c
index bff81b8d4164..bff81b8d4164 100644
--- a/drivers/net/wireless/cw1200/scan.c
+++ b/drivers/net/wireless/st/cw1200/scan.c
diff --git a/drivers/net/wireless/cw1200/scan.h b/drivers/net/wireless/st/cw1200/scan.h
index cc75459e5784..cc75459e5784 100644
--- a/drivers/net/wireless/cw1200/scan.h
+++ b/drivers/net/wireless/st/cw1200/scan.h
diff --git a/drivers/net/wireless/cw1200/sta.c b/drivers/net/wireless/st/cw1200/sta.c
index 95a7fdb3cc1c..06321c799c90 100644
--- a/drivers/net/wireless/cw1200/sta.c
+++ b/drivers/net/wireless/st/cw1200/sta.c
@@ -873,12 +873,6 @@ int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	else
 		val32 = 0; /* disabled */
 
-	if (priv->mode == NL80211_IFTYPE_UNSPECIFIED) {
-		/* device is down, can _not_ set threshold */
-		ret = -ENODEV;
-		goto out;
-	}
-
 	if (priv->rts_threshold == value)
 		goto out;
 
diff --git a/drivers/net/wireless/cw1200/sta.h b/drivers/net/wireless/st/cw1200/sta.h
index bebb3379017f..bebb3379017f 100644
--- a/drivers/net/wireless/cw1200/sta.h
+++ b/drivers/net/wireless/st/cw1200/sta.h
diff --git a/drivers/net/wireless/cw1200/txrx.c b/drivers/net/wireless/st/cw1200/txrx.c
index d28bd49cb5fd..d28bd49cb5fd 100644
--- a/drivers/net/wireless/cw1200/txrx.c
+++ b/drivers/net/wireless/st/cw1200/txrx.c
diff --git a/drivers/net/wireless/cw1200/txrx.h b/drivers/net/wireless/st/cw1200/txrx.h
index 492a4e14213b..492a4e14213b 100644
--- a/drivers/net/wireless/cw1200/txrx.h
+++ b/drivers/net/wireless/st/cw1200/txrx.h
diff --git a/drivers/net/wireless/cw1200/wsm.c b/drivers/net/wireless/st/cw1200/wsm.c
index 9e0ca3048657..9e0ca3048657 100644
--- a/drivers/net/wireless/cw1200/wsm.c
+++ b/drivers/net/wireless/st/cw1200/wsm.c
diff --git a/drivers/net/wireless/cw1200/wsm.h b/drivers/net/wireless/st/cw1200/wsm.h
index 48086e849515..48086e849515 100644
--- a/drivers/net/wireless/cw1200/wsm.h
+++ b/drivers/net/wireless/st/cw1200/wsm.h
diff --git a/drivers/net/wireless/ti/Kconfig b/drivers/net/wireless/ti/Kconfig
index cbe1e7fef61b..92fbd6597e34 100644
--- a/drivers/net/wireless/ti/Kconfig
+++ b/drivers/net/wireless/ti/Kconfig
@@ -1,11 +1,15 @@
-menuconfig WL_TI
-	bool "TI Wireless LAN support"
+config WLAN_VENDOR_TI
+	bool "Texas Instrument devices"
+	default y
 	---help---
-	  This section contains support for all the wireless drivers
-	  for Texas Instruments WLAN chips, such as wl1251 and the wl12xx
-	  family.
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
 
-if WL_TI
+if WLAN_VENDOR_TI
 source "drivers/net/wireless/ti/wl1251/Kconfig"
 source "drivers/net/wireless/ti/wl12xx/Kconfig"
 source "drivers/net/wireless/ti/wl18xx/Kconfig"
@@ -21,4 +25,4 @@ config WILINK_PLATFORM_DATA
 	Small platform data bit needed to pass data to the sdio modules.
 
 
-endif # WL_TI
+endif # WLAN_VENDOR_TI
diff --git a/drivers/net/wireless/ti/wl1251/Kconfig b/drivers/net/wireless/ti/wl1251/Kconfig
index 477a206c098e..7142ccf3a425 100644
--- a/drivers/net/wireless/ti/wl1251/Kconfig
+++ b/drivers/net/wireless/ti/wl1251/Kconfig
@@ -1,4 +1,4 @@
-menuconfig WL1251
+config WL1251
 	tristate "TI wl1251 driver support"
 	depends on MAC80211
 	select FW_LOADER
diff --git a/drivers/net/wireless/ti/wl12xx/conf.h b/drivers/net/wireless/ti/wl12xx/conf.h
index 75e29897a0f5..a606ba9ef041 100644
--- a/drivers/net/wireless/ti/wl12xx/conf.h
+++ b/drivers/net/wireless/ti/wl12xx/conf.h
@@ -47,4 +47,237 @@ struct wl12xx_priv_conf {
 	struct conf_memory_settings mem_wl127x;
 };
 
+enum wl12xx_sg_params {
+	/*
+	* Configure the min and max time BT gains the antenna
+	* in WLAN / BT master basic rate
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_BT_MASTER_MIN_BR = 0,
+	WL12XX_CONF_SG_ACL_BT_MASTER_MAX_BR,
+
+	/*
+	* Configure the min and max time BT gains the antenna
+	* in WLAN / BT slave basic rate
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_BT_SLAVE_MIN_BR,
+	WL12XX_CONF_SG_ACL_BT_SLAVE_MAX_BR,
+
+	/*
+	* Configure the min and max time BT gains the antenna
+	* in WLAN / BT master EDR
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_BT_MASTER_MIN_EDR,
+	WL12XX_CONF_SG_ACL_BT_MASTER_MAX_EDR,
+
+	/*
+	* Configure the min and max time BT gains the antenna
+	* in WLAN / BT slave EDR
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_BT_SLAVE_MIN_EDR,
+	WL12XX_CONF_SG_ACL_BT_SLAVE_MAX_EDR,
+
+	/*
+	* The maximum time WLAN can gain the antenna
+	* in WLAN PSM / BT master/slave BR
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_WLAN_PS_MASTER_BR,
+	WL12XX_CONF_SG_ACL_WLAN_PS_SLAVE_BR,
+
+	/*
+	* The maximum time WLAN can gain the antenna
+	* in WLAN PSM / BT master/slave EDR
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_ACL_WLAN_PS_MASTER_EDR,
+	WL12XX_CONF_SG_ACL_WLAN_PS_SLAVE_EDR,
+
+	/* TODO: explain these values */
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR,
+	WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR,
+
+	WL12XX_CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR,
+	WL12XX_CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR,
+	WL12XX_CONF_SG_ACL_PASSIVE_SCAN_BT_BR,
+	WL12XX_CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR,
+	WL12XX_CONF_SG_ACL_PASSIVE_SCAN_BT_EDR,
+	WL12XX_CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR,
+
+	/*
+	* Compensation percentage of probe requests when scan initiated
+	* during BT voice/ACL link.
+	*
+	* Range: 0 - 255 (%)
+	*/
+	WL12XX_CONF_SG_AUTO_SCAN_PROBE_REQ,
+
+	/*
+	* Compensation percentage of probe requests when active scan initiated
+	* during BT voice
+	*
+	* Range: 0 - 255 (%)
+	*/
+	WL12XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3,
+
+	/*
+	* Compensation percentage of WLAN active scan window if initiated
+	* during BT A2DP
+	*
+	* Range: 0 - 1000 (%)
+	*/
+	WL12XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP,
+
+	/*
+	* Compensation percentage of WLAN passive scan window if initiated
+	* during BT A2DP BR
+	*
+	* Range: 0 - 1000 (%)
+	*/
+	WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_A2DP_BR,
+
+	/*
+	* Compensation percentage of WLAN passive scan window if initiated
+	* during BT A2DP EDR
+	*
+	* Range: 0 - 1000 (%)
+	*/
+	WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_A2DP_EDR,
+
+	/*
+	* Compensation percentage of WLAN passive scan window if initiated
+	* during BT voice
+	*
+	* Range: 0 - 1000 (%)
+	*/
+	WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_HV3,
+
+	/* TODO: explain these values */
+	WL12XX_CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN,
+	WL12XX_CONF_SG_BCN_HV3_COLL_THR_IN_PASSIVE_SCAN,
+	WL12XX_CONF_SG_TX_RX_PROTECT_BW_IN_PASSIVE_SCAN,
+
+	/*
+	* Defines whether the SG will force WLAN host to enter/exit PSM
+	*
+	* Range: 1 - SG can force, 0 - host handles PSM
+	*/
+	WL12XX_CONF_SG_STA_FORCE_PS_IN_BT_SCO,
+
+	/*
+	* Defines antenna configuration (single/dual antenna)
+	*
+	* Range: 0 - single antenna, 1 - dual antenna
+	*/
+	WL12XX_CONF_SG_ANTENNA_CONFIGURATION,
+
+	/*
+	* The threshold (percent) of max consecutive beacon misses before
+	* increasing priority of beacon reception.
+	*
+	* Range: 0 - 100 (%)
+	*/
+	WL12XX_CONF_SG_BEACON_MISS_PERCENT,
+
+	/*
+	* Protection time of the DHCP procedure.
+	*
+	* Range: 0 - 100000 (ms)
+	*/
+	WL12XX_CONF_SG_DHCP_TIME,
+
+	/*
+	* RX guard time before the beginning of a new BT voice frame during
+	* which no new WLAN trigger frame is transmitted.
+	*
+	* Range: 0 - 100000 (us)
+	*/
+	WL12XX_CONF_SG_RXT,
+
+	/*
+	* TX guard time before the beginning of a new BT voice frame during
+	* which no new WLAN frame is transmitted.
+	*
+	* Range: 0 - 100000 (us)
+	*/
+	WL12XX_CONF_SG_TXT,
+
+	/*
+	* Enable adaptive RXT/TXT algorithm. If disabled, the host values
+	* will be utilized.
+	*
+	* Range: 0 - disable, 1 - enable
+	*/
+	WL12XX_CONF_SG_ADAPTIVE_RXT_TXT,
+
+	/* TODO: explain this value */
+	WL12XX_CONF_SG_GENERAL_USAGE_BIT_MAP,
+
+	/*
+	* Number of consecutive BT voice frames not interrupted by WLAN
+	*
+	* Range: 0 - 100
+	*/
+	WL12XX_CONF_SG_HV3_MAX_SERVED,
+
+	/*
+	* The used WLAN legacy service period during active BT ACL link
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_PS_POLL_TIMEOUT,
+
+	/*
+	* The used WLAN UPSD service period during active BT ACL link
+	*
+	* Range: 0 - 255 (ms)
+	*/
+	WL12XX_CONF_SG_UPSD_TIMEOUT,
+
+	WL12XX_CONF_SG_CONSECUTIVE_CTS_THRESHOLD,
+	WL12XX_CONF_SG_STA_RX_WINDOW_AFTER_DTIM,
+	WL12XX_CONF_SG_STA_CONNECTION_PROTECTION_TIME,
+
+	/* AP params */
+	WL12XX_CONF_AP_BEACON_MISS_TX,
+	WL12XX_CONF_AP_RX_WINDOW_AFTER_BEACON,
+	WL12XX_CONF_AP_BEACON_WINDOW_INTERVAL,
+	WL12XX_CONF_AP_CONNECTION_PROTECTION_TIME,
+	WL12XX_CONF_AP_BT_ACL_VAL_BT_SERVE_TIME,
+	WL12XX_CONF_AP_BT_ACL_VAL_WL_SERVE_TIME,
+
+	/* CTS Diluting params */
+	WL12XX_CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH,
+	WL12XX_CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER,
+
+	WL12XX_CONF_SG_TEMP_PARAM_1,
+	WL12XX_CONF_SG_TEMP_PARAM_2,
+	WL12XX_CONF_SG_TEMP_PARAM_3,
+	WL12XX_CONF_SG_TEMP_PARAM_4,
+	WL12XX_CONF_SG_TEMP_PARAM_5,
+	WL12XX_CONF_SG_TEMP_PARAM_6,
+	WL12XX_CONF_SG_TEMP_PARAM_7,
+	WL12XX_CONF_SG_TEMP_PARAM_8,
+	WL12XX_CONF_SG_TEMP_PARAM_9,
+	WL12XX_CONF_SG_TEMP_PARAM_10,
+
+	WL12XX_CONF_SG_PARAMS_MAX,
+	WL12XX_CONF_SG_PARAMS_ALL = 0xff
+};
+
 #endif /* __WL12XX_CONF_H__ */
diff --git a/drivers/net/wireless/ti/wl12xx/main.c b/drivers/net/wireless/ti/wl12xx/main.c
index af0fe2e17151..a0d6cccc56f3 100644
--- a/drivers/net/wireless/ti/wl12xx/main.c
+++ b/drivers/net/wireless/ti/wl12xx/main.c
@@ -39,6 +39,7 @@
 #include "scan.h"
 #include "event.h"
 #include "debugfs.h"
+#include "conf.h"
 
 static char *fref_param;
 static char *tcxo_param;
@@ -46,69 +47,69 @@ static char *tcxo_param;
 static struct wlcore_conf wl12xx_conf = {
 	.sg = {
 		.params = {
-			[CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
-			[CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
-			[CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
+			[WL12XX_CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
+			[WL12XX_CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
+			[WL12XX_CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
+			[WL12XX_CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
+			[WL12XX_CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
+			[WL12XX_CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
+			[WL12XX_CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
+			[WL12XX_CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
+			[WL12XX_CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
+			[WL12XX_CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
+			[WL12XX_CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
+			[WL12XX_CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
+			[WL12XX_CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
+			[WL12XX_CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
+			[WL12XX_CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
+			[WL12XX_CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
+			[WL12XX_CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
+			[WL12XX_CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
+			[WL12XX_CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
 			/* active scan params */
-			[CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
+			[WL12XX_CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
+			[WL12XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
+			[WL12XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
 			/* passive scan params */
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR] = 800,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR] = 200,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
+			[WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_A2DP_BR] = 800,
+			[WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_A2DP_EDR] = 200,
+			[WL12XX_CONF_SG_PASSIVE_SCAN_DUR_FACTOR_HV3] = 200,
 			/* passive scan in dual antenna params */
-			[CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN] = 0,
+			[WL12XX_CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
+			[WL12XX_CONF_SG_BCN_HV3_COLL_THR_IN_PASSIVE_SCAN] = 0,
+			[WL12XX_CONF_SG_TX_RX_PROTECT_BW_IN_PASSIVE_SCAN] = 0,
 			/* general params */
-			[CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
-			[CONF_SG_ANTENNA_CONFIGURATION] = 0,
-			[CONF_SG_BEACON_MISS_PERCENT] = 60,
-			[CONF_SG_DHCP_TIME] = 5000,
-			[CONF_SG_RXT] = 1200,
-			[CONF_SG_TXT] = 1000,
-			[CONF_SG_ADAPTIVE_RXT_TXT] = 1,
-			[CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
-			[CONF_SG_HV3_MAX_SERVED] = 6,
-			[CONF_SG_PS_POLL_TIMEOUT] = 10,
-			[CONF_SG_UPSD_TIMEOUT] = 10,
-			[CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
-			[CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
-			[CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
+			[WL12XX_CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
+			[WL12XX_CONF_SG_ANTENNA_CONFIGURATION] = 0,
+			[WL12XX_CONF_SG_BEACON_MISS_PERCENT] = 60,
+			[WL12XX_CONF_SG_DHCP_TIME] = 5000,
+			[WL12XX_CONF_SG_RXT] = 1200,
+			[WL12XX_CONF_SG_TXT] = 1000,
+			[WL12XX_CONF_SG_ADAPTIVE_RXT_TXT] = 1,
+			[WL12XX_CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
+			[WL12XX_CONF_SG_HV3_MAX_SERVED] = 6,
+			[WL12XX_CONF_SG_PS_POLL_TIMEOUT] = 10,
+			[WL12XX_CONF_SG_UPSD_TIMEOUT] = 10,
+			[WL12XX_CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
+			[WL12XX_CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
+			[WL12XX_CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
 			/* AP params */
-			[CONF_AP_BEACON_MISS_TX] = 3,
-			[CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
-			[CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
-			[CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
-			[CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
-			[CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
+			[WL12XX_CONF_AP_BEACON_MISS_TX] = 3,
+			[WL12XX_CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
+			[WL12XX_CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
+			[WL12XX_CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
+			[WL12XX_CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
+			[WL12XX_CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
 			/* CTS Diluting params */
-			[CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
-			[CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
+			[WL12XX_CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
+			[WL12XX_CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
 		},
 		.state = CONF_SG_PROTECTIVE,
 	},
@@ -1809,6 +1810,7 @@ static int wl12xx_setup(struct wl1271 *wl)
 
 	BUILD_BUG_ON(WL12XX_MAX_LINKS > WLCORE_MAX_LINKS);
 	BUILD_BUG_ON(WL12XX_MAX_AP_STATIONS > WL12XX_MAX_LINKS);
+	BUILD_BUG_ON(WL12XX_CONF_SG_PARAMS_MAX > WLCORE_CONF_SG_PARAMS_MAX);
 
 	wl->rtable = wl12xx_rtable;
 	wl->num_tx_desc = WL12XX_NUM_TX_DESCRIPTORS;
diff --git a/drivers/net/wireless/ti/wl18xx/conf.h b/drivers/net/wireless/ti/wl18xx/conf.h
index 71f1ec448ba5..7aa880f14ccb 100644
--- a/drivers/net/wireless/ti/wl18xx/conf.h
+++ b/drivers/net/wireless/ti/wl18xx/conf.h
@@ -139,4 +139,94 @@ struct wl18xx_priv_conf {
 	struct conf_ap_sleep_settings ap_sleep;
 } __packed;
 
+enum wl18xx_sg_params {
+	WL18XX_CONF_SG_PARAM_0 = 0,
+
+	/* Configuration Parameters */
+	WL18XX_CONF_SG_ANTENNA_CONFIGURATION,
+	WL18XX_CONF_SG_ZIGBEE_COEX,
+	WL18XX_CONF_SG_TIME_SYNC,
+
+	WL18XX_CONF_SG_PARAM_4,
+	WL18XX_CONF_SG_PARAM_5,
+	WL18XX_CONF_SG_PARAM_6,
+	WL18XX_CONF_SG_PARAM_7,
+	WL18XX_CONF_SG_PARAM_8,
+	WL18XX_CONF_SG_PARAM_9,
+	WL18XX_CONF_SG_PARAM_10,
+	WL18XX_CONF_SG_PARAM_11,
+	WL18XX_CONF_SG_PARAM_12,
+	WL18XX_CONF_SG_PARAM_13,
+	WL18XX_CONF_SG_PARAM_14,
+	WL18XX_CONF_SG_PARAM_15,
+	WL18XX_CONF_SG_PARAM_16,
+	WL18XX_CONF_SG_PARAM_17,
+	WL18XX_CONF_SG_PARAM_18,
+	WL18XX_CONF_SG_PARAM_19,
+	WL18XX_CONF_SG_PARAM_20,
+	WL18XX_CONF_SG_PARAM_21,
+	WL18XX_CONF_SG_PARAM_22,
+	WL18XX_CONF_SG_PARAM_23,
+	WL18XX_CONF_SG_PARAM_24,
+	WL18XX_CONF_SG_PARAM_25,
+
+	/* Active Scan Parameters */
+	WL18XX_CONF_SG_AUTO_SCAN_PROBE_REQ,
+	WL18XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3,
+
+	WL18XX_CONF_SG_PARAM_28,
+
+	/* Passive Scan Parameters */
+	WL18XX_CONF_SG_PARAM_29,
+	WL18XX_CONF_SG_PARAM_30,
+	WL18XX_CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3,
+
+	/* Passive Scan in Dual Antenna Parameters */
+	WL18XX_CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN,
+	WL18XX_CONF_SG_BEACON_HV3_COLL_TH_IN_PASSIVE_SCAN,
+	WL18XX_CONF_SG_TX_RX_PROTECT_BW_IN_PASSIVE_SCAN,
+
+	/* General Parameters */
+	WL18XX_CONF_SG_STA_FORCE_PS_IN_BT_SCO,
+	WL18XX_CONF_SG_PARAM_36,
+	WL18XX_CONF_SG_BEACON_MISS_PERCENT,
+	WL18XX_CONF_SG_PARAM_38,
+	WL18XX_CONF_SG_RXT,
+	WL18XX_CONF_SG_UNUSED,
+	WL18XX_CONF_SG_ADAPTIVE_RXT_TXT,
+	WL18XX_CONF_SG_GENERAL_USAGE_BIT_MAP,
+	WL18XX_CONF_SG_HV3_MAX_SERVED,
+	WL18XX_CONF_SG_PARAM_44,
+	WL18XX_CONF_SG_PARAM_45,
+	WL18XX_CONF_SG_CONSECUTIVE_CTS_THRESHOLD,
+	WL18XX_CONF_SG_GEMINI_PARAM_47,
+	WL18XX_CONF_SG_STA_CONNECTION_PROTECTION_TIME,
+
+	/* AP Parameters */
+	WL18XX_CONF_SG_AP_BEACON_MISS_TX,
+	WL18XX_CONF_SG_PARAM_50,
+	WL18XX_CONF_SG_AP_BEACON_WINDOW_INTERVAL,
+	WL18XX_CONF_SG_AP_CONNECTION_PROTECTION_TIME,
+	WL18XX_CONF_SG_PARAM_53,
+	WL18XX_CONF_SG_PARAM_54,
+
+	/* CTS Diluting Parameters */
+	WL18XX_CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH,
+	WL18XX_CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER,
+
+	WL18XX_CONF_SG_TEMP_PARAM_1,
+	WL18XX_CONF_SG_TEMP_PARAM_2,
+	WL18XX_CONF_SG_TEMP_PARAM_3,
+	WL18XX_CONF_SG_TEMP_PARAM_4,
+	WL18XX_CONF_SG_TEMP_PARAM_5,
+	WL18XX_CONF_SG_TEMP_PARAM_6,
+	WL18XX_CONF_SG_TEMP_PARAM_7,
+	WL18XX_CONF_SG_TEMP_PARAM_8,
+	WL18XX_CONF_SG_TEMP_PARAM_9,
+	WL18XX_CONF_SG_TEMP_PARAM_10,
+
+	WL18XX_CONF_SG_PARAMS_MAX,
+	WL18XX_CONF_SG_PARAMS_ALL = 0xff
+};
+
 #endif /* __WL18XX_CONF_H__ */
diff --git a/drivers/net/wireless/ti/wl18xx/event.c b/drivers/net/wireless/ti/wl18xx/event.c
index 09c7e098f460..719907a0a2c2 100644
--- a/drivers/net/wireless/ti/wl18xx/event.c
+++ b/drivers/net/wireless/ti/wl18xx/event.c
@@ -205,6 +205,8 @@ int wl18xx_process_mailbox_events(struct wl1271 *wl)
 						 mbox->sc_ssid,
 						 mbox->sc_pwd_len,
 						 mbox->sc_pwd);
+	if (vector & FW_LOGGER_INDICATION)
+		wlcore_event_fw_logger(wl);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ti/wl18xx/event.h b/drivers/net/wireless/ti/wl18xx/event.h
index f3d4f13379cb..070de1274694 100644
--- a/drivers/net/wireless/ti/wl18xx/event.h
+++ b/drivers/net/wireless/ti/wl18xx/event.h
@@ -41,6 +41,7 @@ enum {
 	SMART_CONFIG_SYNC_EVENT_ID               = BIT(22),
 	SMART_CONFIG_DECODE_EVENT_ID             = BIT(23),
 	TIME_SYNC_EVENT_ID                       = BIT(24),
+	FW_LOGGER_INDICATION			= BIT(25),
 };
 
 enum wl18xx_radar_types {
diff --git a/drivers/net/wireless/ti/wl18xx/main.c b/drivers/net/wireless/ti/wl18xx/main.c
index 50cce42089a5..1bf26cc7374e 100644
--- a/drivers/net/wireless/ti/wl18xx/main.c
+++ b/drivers/net/wireless/ti/wl18xx/main.c
@@ -177,69 +177,80 @@ enum wl18xx_hw_rates {
 static struct wlcore_conf wl18xx_conf = {
 	.sg = {
 		.params = {
-			[CONF_SG_ACL_BT_MASTER_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_SLAVE_MIN_BR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_BR] = 180,
-			[CONF_SG_ACL_BT_MASTER_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_MASTER_MAX_EDR] = 80,
-			[CONF_SG_ACL_BT_SLAVE_MIN_EDR] = 10,
-			[CONF_SG_ACL_BT_SLAVE_MAX_EDR] = 80,
-			[CONF_SG_ACL_WLAN_PS_MASTER_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_BR] = 8,
-			[CONF_SG_ACL_WLAN_PS_MASTER_EDR] = 20,
-			[CONF_SG_ACL_WLAN_PS_SLAVE_EDR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR] = 20,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR] = 16,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR] = 35,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR] = 32,
-			[CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR] = 50,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR] = 28,
-			[CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR] = 50,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR] = 10,
-			[CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR] = 20,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_BR] = 75,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR] = 15,
-			[CONF_SG_ACL_PASSIVE_SCAN_BT_EDR] = 27,
-			[CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR] = 17,
-			/* active scan params */
-			[CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
-			[CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP] = 100,
-			/* passive scan params */
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR] = 800,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR] = 200,
-			[CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
-			/* passive scan in dual antenna params */
-			[CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN] = 0,
-			[CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN] = 0,
-			/* general params */
-			[CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
-			[CONF_SG_ANTENNA_CONFIGURATION] = 0,
-			[CONF_SG_BEACON_MISS_PERCENT] = 60,
-			[CONF_SG_DHCP_TIME] = 5000,
-			[CONF_SG_RXT] = 1200,
-			[CONF_SG_TXT] = 1000,
-			[CONF_SG_ADAPTIVE_RXT_TXT] = 1,
-			[CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
-			[CONF_SG_HV3_MAX_SERVED] = 6,
-			[CONF_SG_PS_POLL_TIMEOUT] = 10,
-			[CONF_SG_UPSD_TIMEOUT] = 10,
-			[CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
-			[CONF_SG_STA_RX_WINDOW_AFTER_DTIM] = 5,
-			[CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 30,
-			/* AP params */
-			[CONF_AP_BEACON_MISS_TX] = 3,
-			[CONF_AP_RX_WINDOW_AFTER_BEACON] = 10,
-			[CONF_AP_BEACON_WINDOW_INTERVAL] = 2,
-			[CONF_AP_CONNECTION_PROTECTION_TIME] = 0,
-			[CONF_AP_BT_ACL_VAL_BT_SERVE_TIME] = 25,
-			[CONF_AP_BT_ACL_VAL_WL_SERVE_TIME] = 25,
-			/* CTS Diluting params */
-			[CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
-			[CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
+			[WL18XX_CONF_SG_PARAM_0] = 0,
+			/* Configuartion Parameters */
+			[WL18XX_CONF_SG_ANTENNA_CONFIGURATION] = 0,
+			[WL18XX_CONF_SG_ZIGBEE_COEX] = 0,
+			[WL18XX_CONF_SG_TIME_SYNC] = 0,
+			[WL18XX_CONF_SG_PARAM_4] = 0,
+			[WL18XX_CONF_SG_PARAM_5] = 0,
+			[WL18XX_CONF_SG_PARAM_6] = 0,
+			[WL18XX_CONF_SG_PARAM_7] = 0,
+			[WL18XX_CONF_SG_PARAM_8] = 0,
+			[WL18XX_CONF_SG_PARAM_9] = 0,
+			[WL18XX_CONF_SG_PARAM_10] = 0,
+			[WL18XX_CONF_SG_PARAM_11] = 0,
+			[WL18XX_CONF_SG_PARAM_12] = 0,
+			[WL18XX_CONF_SG_PARAM_13] = 0,
+			[WL18XX_CONF_SG_PARAM_14] = 0,
+			[WL18XX_CONF_SG_PARAM_15] = 0,
+			[WL18XX_CONF_SG_PARAM_16] = 0,
+			[WL18XX_CONF_SG_PARAM_17] = 0,
+			[WL18XX_CONF_SG_PARAM_18] = 0,
+			[WL18XX_CONF_SG_PARAM_19] = 0,
+			[WL18XX_CONF_SG_PARAM_20] = 0,
+			[WL18XX_CONF_SG_PARAM_21] = 0,
+			[WL18XX_CONF_SG_PARAM_22] = 0,
+			[WL18XX_CONF_SG_PARAM_23] = 0,
+			[WL18XX_CONF_SG_PARAM_24] = 0,
+			[WL18XX_CONF_SG_PARAM_25] = 0,
+			/* Active Scan Parameters */
+			[WL18XX_CONF_SG_AUTO_SCAN_PROBE_REQ] = 170,
+			[WL18XX_CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3] = 50,
+			[WL18XX_CONF_SG_PARAM_28] = 0,
+			/* Passive Scan Parameters */
+			[WL18XX_CONF_SG_PARAM_29] = 0,
+			[WL18XX_CONF_SG_PARAM_30] = 0,
+			[WL18XX_CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3] = 200,
+			/* Passive Scan in Dual Antenna Parameters */
+			[WL18XX_CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN] = 0,
+			[WL18XX_CONF_SG_BEACON_HV3_COLL_TH_IN_PASSIVE_SCAN] = 0,
+			[WL18XX_CONF_SG_TX_RX_PROTECT_BW_IN_PASSIVE_SCAN] = 0,
+			/* General Parameters */
+			[WL18XX_CONF_SG_STA_FORCE_PS_IN_BT_SCO] = 1,
+			[WL18XX_CONF_SG_PARAM_36] = 0,
+			[WL18XX_CONF_SG_BEACON_MISS_PERCENT] = 60,
+			[WL18XX_CONF_SG_PARAM_38] = 0,
+			[WL18XX_CONF_SG_RXT] = 1200,
+			[WL18XX_CONF_SG_UNUSED] = 0,
+			[WL18XX_CONF_SG_ADAPTIVE_RXT_TXT] = 1,
+			[WL18XX_CONF_SG_GENERAL_USAGE_BIT_MAP] = 3,
+			[WL18XX_CONF_SG_HV3_MAX_SERVED] = 6,
+			[WL18XX_CONF_SG_PARAM_44] = 0,
+			[WL18XX_CONF_SG_PARAM_45] = 0,
+			[WL18XX_CONF_SG_CONSECUTIVE_CTS_THRESHOLD] = 2,
+			[WL18XX_CONF_SG_GEMINI_PARAM_47] = 0,
+			[WL18XX_CONF_SG_STA_CONNECTION_PROTECTION_TIME] = 0,
+			/* AP Parameters */
+			[WL18XX_CONF_SG_AP_BEACON_MISS_TX] = 3,
+			[WL18XX_CONF_SG_PARAM_50] = 0,
+			[WL18XX_CONF_SG_AP_BEACON_WINDOW_INTERVAL] = 2,
+			[WL18XX_CONF_SG_AP_CONNECTION_PROTECTION_TIME] = 30,
+			[WL18XX_CONF_SG_PARAM_53] = 0,
+			[WL18XX_CONF_SG_PARAM_54] = 0,
+			/* CTS Diluting Parameters */
+			[WL18XX_CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH] = 0,
+			[WL18XX_CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_1] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_2] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_3] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_4] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_5] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_6] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_7] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_8] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_9] = 0,
+			[WL18XX_CONF_SG_TEMP_PARAM_10] = 0,
 		},
 		.state = CONF_SG_PROTECTIVE,
 	},
@@ -461,7 +472,7 @@ static struct wlcore_conf wl18xx_conf = {
 	},
 	.fwlog = {
 		.mode                         = WL12XX_FWLOG_CONTINUOUS,
-		.mem_blocks                   = 2,
+		.mem_blocks                   = 0,
 		.severity                     = 0,
 		.timestamp                    = WL12XX_FWLOG_TIMESTAMP_DISABLED,
 		.output                       = WL12XX_FWLOG_OUTPUT_DBG_PINS,
@@ -584,7 +595,7 @@ static const struct wlcore_partition_set wl18xx_ptable[PART_TABLE_LEN] = {
 		.mem  = { .start = 0x00A00000, .size  = 0x00012000 },
 		.reg  = { .start = 0x00807000, .size  = 0x00005000 },
 		.mem2 = { .start = 0x00800000, .size  = 0x0000B000 },
-		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
+		.mem3 = { .start = 0x00401594, .size  = 0x00001020 },
 	},
 	[PART_DOWN] = {
 		.mem  = { .start = 0x00000000, .size  = 0x00014000 },
@@ -602,7 +613,7 @@ static const struct wlcore_partition_set wl18xx_ptable[PART_TABLE_LEN] = {
 		.mem  = { .start = 0x00800000, .size  = 0x000050FC },
 		.reg  = { .start = 0x00B00404, .size  = 0x00001000 },
 		.mem2 = { .start = 0x00C00000, .size  = 0x00000400 },
-		.mem3 = { .start = 0x00000000, .size  = 0x00000000 },
+		.mem3 = { .start = 0x00401594, .size  = 0x00001020 },
 	},
 	[PART_PHY_INIT] = {
 		.mem  = { .start = WL18XX_PHY_INIT_MEM_ADDR,
@@ -1029,7 +1040,8 @@ static int wl18xx_boot(struct wl1271 *wl)
 		DFS_CHANNELS_CONFIG_COMPLETE_EVENT |
 		SMART_CONFIG_SYNC_EVENT_ID |
 		SMART_CONFIG_DECODE_EVENT_ID |
-		TIME_SYNC_EVENT_ID;
+		TIME_SYNC_EVENT_ID |
+		FW_LOGGER_INDICATION;
 
 	wl->ap_event_mask = MAX_TX_FAILURE_EVENT_ID;
 
@@ -1895,6 +1907,7 @@ static int wl18xx_setup(struct wl1271 *wl)
 
 	BUILD_BUG_ON(WL18XX_MAX_LINKS > WLCORE_MAX_LINKS);
 	BUILD_BUG_ON(WL18XX_MAX_AP_STATIONS > WL18XX_MAX_LINKS);
+	BUILD_BUG_ON(WL18XX_CONF_SG_PARAMS_MAX > WLCORE_CONF_SG_PARAMS_MAX);
 
 	wl->rtable = wl18xx_rtable;
 	wl->num_tx_desc = WL18XX_NUM_TX_DESCRIPTORS;
diff --git a/drivers/net/wireless/ti/wlcore/Kconfig b/drivers/net/wireless/ti/wlcore/Kconfig
index 7c099542b214..969c9d79bfc8 100644
--- a/drivers/net/wireless/ti/wlcore/Kconfig
+++ b/drivers/net/wireless/ti/wlcore/Kconfig
@@ -1,6 +1,6 @@
 config WLCORE
 	tristate "TI wlcore support"
-	depends on WL_TI && MAC80211
+	depends on MAC80211
 	select FW_LOADER
 	---help---
 	  This module contains the main code for TI WLAN chips.  It abstracts
diff --git a/drivers/net/wireless/ti/wlcore/acx.c b/drivers/net/wireless/ti/wlcore/acx.c
index f28fa3b5029d..26cc23f32241 100644
--- a/drivers/net/wireless/ti/wlcore/acx.c
+++ b/drivers/net/wireless/ti/wlcore/acx.c
@@ -534,9 +534,9 @@ int wl12xx_acx_sg_cfg(struct wl1271 *wl)
 	}
 
 	/* BT-WLAN coext parameters */
-	for (i = 0; i < CONF_SG_PARAMS_MAX; i++)
+	for (i = 0; i < WLCORE_CONF_SG_PARAMS_MAX; i++)
 		param->params[i] = cpu_to_le32(c->params[i]);
-	param->param_idx = CONF_SG_PARAMS_ALL;
+	param->param_idx = WLCORE_CONF_SG_PARAMS_ALL;
 
 	ret = wl1271_cmd_configure(wl, ACX_SG_CFG, param, sizeof(*param));
 	if (ret < 0) {
diff --git a/drivers/net/wireless/ti/wlcore/acx.h b/drivers/net/wireless/ti/wlcore/acx.h
index 954d57ec98f4..0d61fae88dcb 100644
--- a/drivers/net/wireless/ti/wlcore/acx.h
+++ b/drivers/net/wireless/ti/wlcore/acx.h
@@ -300,7 +300,7 @@ struct acx_bt_wlan_coex {
 struct acx_bt_wlan_coex_param {
 	struct acx_header header;
 
-	__le32 params[CONF_SG_PARAMS_MAX];
+	__le32 params[WLCORE_CONF_SG_PARAMS_MAX];
 	u8 param_idx;
 	u8 padding[3];
 } __packed;
diff --git a/drivers/net/wireless/ti/wlcore/cmd.h b/drivers/net/wireless/ti/wlcore/cmd.h
index 8dc46c0a489a..e28e2f2303ce 100644
--- a/drivers/net/wireless/ti/wlcore/cmd.h
+++ b/drivers/net/wireless/ti/wlcore/cmd.h
@@ -626,7 +626,6 @@ struct wl12xx_cmd_remove_peer {
  */
 enum wl12xx_fwlogger_log_mode {
 	WL12XX_FWLOG_CONTINUOUS,
-	WL12XX_FWLOG_ON_DEMAND
 };
 
 /* Include/exclude timestamps from the log messages */
diff --git a/drivers/net/wireless/ti/wlcore/conf.h b/drivers/net/wireless/ti/wlcore/conf.h
index 52a9d1b14020..44d898fe0afc 100644
--- a/drivers/net/wireless/ti/wlcore/conf.h
+++ b/drivers/net/wireless/ti/wlcore/conf.h
@@ -110,242 +110,11 @@ enum {
 	CONF_SG_OPPORTUNISTIC
 };
 
-enum {
-	/*
-	 * Configure the min and max time BT gains the antenna
-	 * in WLAN / BT master basic rate
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_BT_MASTER_MIN_BR = 0,
-	CONF_SG_ACL_BT_MASTER_MAX_BR,
-
-	/*
-	 * Configure the min and max time BT gains the antenna
-	 * in WLAN / BT slave basic rate
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_BT_SLAVE_MIN_BR,
-	CONF_SG_ACL_BT_SLAVE_MAX_BR,
-
-	/*
-	 * Configure the min and max time BT gains the antenna
-	 * in WLAN / BT master EDR
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_BT_MASTER_MIN_EDR,
-	CONF_SG_ACL_BT_MASTER_MAX_EDR,
-
-	/*
-	 * Configure the min and max time BT gains the antenna
-	 * in WLAN / BT slave EDR
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_BT_SLAVE_MIN_EDR,
-	CONF_SG_ACL_BT_SLAVE_MAX_EDR,
-
-	/*
-	 * The maximum time WLAN can gain the antenna
-	 * in WLAN PSM / BT master/slave BR
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_WLAN_PS_MASTER_BR,
-	CONF_SG_ACL_WLAN_PS_SLAVE_BR,
-
-	/*
-	 * The maximum time WLAN can gain the antenna
-	 * in WLAN PSM / BT master/slave EDR
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_ACL_WLAN_PS_MASTER_EDR,
-	CONF_SG_ACL_WLAN_PS_SLAVE_EDR,
-
-	/* TODO: explain these values */
-	CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_BR,
-	CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_BR,
-	CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_BR,
-	CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_BR,
-	CONF_SG_ACL_WLAN_ACTIVE_MASTER_MIN_EDR,
-	CONF_SG_ACL_WLAN_ACTIVE_MASTER_MAX_EDR,
-	CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MIN_EDR,
-	CONF_SG_ACL_WLAN_ACTIVE_SLAVE_MAX_EDR,
-
-	CONF_SG_ACL_ACTIVE_SCAN_WLAN_BR,
-	CONF_SG_ACL_ACTIVE_SCAN_WLAN_EDR,
-	CONF_SG_ACL_PASSIVE_SCAN_BT_BR,
-	CONF_SG_ACL_PASSIVE_SCAN_WLAN_BR,
-	CONF_SG_ACL_PASSIVE_SCAN_BT_EDR,
-	CONF_SG_ACL_PASSIVE_SCAN_WLAN_EDR,
-
-	/*
-	 * Compensation percentage of probe requests when scan initiated
-	 * during BT voice/ACL link.
-	 *
-	 * Range: 0 - 255 (%)
-	 */
-	CONF_SG_AUTO_SCAN_PROBE_REQ,
-
-	/*
-	 * Compensation percentage of probe requests when active scan initiated
-	 * during BT voice
-	 *
-	 * Range: 0 - 255 (%)
-	 */
-	CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_HV3,
-
-	/*
-	 * Compensation percentage of WLAN active scan window if initiated
-	 * during BT A2DP
-	 *
-	 * Range: 0 - 1000 (%)
-	 */
-	CONF_SG_ACTIVE_SCAN_DURATION_FACTOR_A2DP,
-
-	/*
-	 * Compensation percentage of WLAN passive scan window if initiated
-	 * during BT A2DP BR
-	 *
-	 * Range: 0 - 1000 (%)
-	 */
-	CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_BR,
-
-	/*
-	 * Compensation percentage of WLAN passive scan window if initiated
-	 * during BT A2DP EDR
-	 *
-	 * Range: 0 - 1000 (%)
-	 */
-	CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_A2DP_EDR,
-
-	/*
-	 * Compensation percentage of WLAN passive scan window if initiated
-	 * during BT voice
-	 *
-	 * Range: 0 - 1000 (%)
-	 */
-	CONF_SG_PASSIVE_SCAN_DURATION_FACTOR_HV3,
-
-	/* TODO: explain these values */
-	CONF_SG_CONSECUTIVE_HV3_IN_PASSIVE_SCAN,
-	CONF_SG_BCN_HV3_COLLISION_THRESH_IN_PASSIVE_SCAN,
-	CONF_SG_TX_RX_PROTECTION_BWIDTH_IN_PASSIVE_SCAN,
-
-	/*
-	 * Defines whether the SG will force WLAN host to enter/exit PSM
-	 *
-	 * Range: 1 - SG can force, 0 - host handles PSM
-	 */
-	CONF_SG_STA_FORCE_PS_IN_BT_SCO,
-
-	/*
-	 * Defines antenna configuration (single/dual antenna)
-	 *
-	 * Range: 0 - single antenna, 1 - dual antenna
-	 */
-	CONF_SG_ANTENNA_CONFIGURATION,
-
-	/*
-	 * The threshold (percent) of max consecutive beacon misses before
-	 * increasing priority of beacon reception.
-	 *
-	 * Range: 0 - 100 (%)
-	 */
-	CONF_SG_BEACON_MISS_PERCENT,
-
-	/*
-	 * Protection time of the DHCP procedure.
-	 *
-	 * Range: 0 - 100000 (ms)
-	 */
-	CONF_SG_DHCP_TIME,
-
-	/*
-	 * RX guard time before the beginning of a new BT voice frame during
-	 * which no new WLAN trigger frame is transmitted.
-	 *
-	 * Range: 0 - 100000 (us)
-	 */
-	CONF_SG_RXT,
-
-	/*
-	 * TX guard time before the beginning of a new BT voice frame during
-	 * which no new WLAN frame is transmitted.
-	 *
-	 * Range: 0 - 100000 (us)
-	 */
-
-	CONF_SG_TXT,
-
-	/*
-	 * Enable adaptive RXT/TXT algorithm. If disabled, the host values
-	 * will be utilized.
-	 *
-	 * Range: 0 - disable, 1 - enable
-	 */
-	CONF_SG_ADAPTIVE_RXT_TXT,
-
-	/* TODO: explain this value */
-	CONF_SG_GENERAL_USAGE_BIT_MAP,
-
-	/*
-	 * Number of consecutive BT voice frames not interrupted by WLAN
-	 *
-	 * Range: 0 - 100
-	 */
-	CONF_SG_HV3_MAX_SERVED,
-
-	/*
-	 * The used WLAN legacy service period during active BT ACL link
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_PS_POLL_TIMEOUT,
-
-	/*
-	 * The used WLAN UPSD service period during active BT ACL link
-	 *
-	 * Range: 0 - 255 (ms)
-	 */
-	CONF_SG_UPSD_TIMEOUT,
-
-	CONF_SG_CONSECUTIVE_CTS_THRESHOLD,
-	CONF_SG_STA_RX_WINDOW_AFTER_DTIM,
-	CONF_SG_STA_CONNECTION_PROTECTION_TIME,
-
-	/* AP params */
-	CONF_AP_BEACON_MISS_TX,
-	CONF_AP_RX_WINDOW_AFTER_BEACON,
-	CONF_AP_BEACON_WINDOW_INTERVAL,
-	CONF_AP_CONNECTION_PROTECTION_TIME,
-	CONF_AP_BT_ACL_VAL_BT_SERVE_TIME,
-	CONF_AP_BT_ACL_VAL_WL_SERVE_TIME,
-
-	/* CTS Diluting params */
-	CONF_SG_CTS_DILUTED_BAD_RX_PACKETS_TH,
-	CONF_SG_CTS_CHOP_IN_DUAL_ANT_SCO_MASTER,
-
-	CONF_SG_TEMP_PARAM_1,
-	CONF_SG_TEMP_PARAM_2,
-	CONF_SG_TEMP_PARAM_3,
-	CONF_SG_TEMP_PARAM_4,
-	CONF_SG_TEMP_PARAM_5,
-	CONF_SG_TEMP_PARAM_6,
-	CONF_SG_TEMP_PARAM_7,
-	CONF_SG_TEMP_PARAM_8,
-	CONF_SG_TEMP_PARAM_9,
-	CONF_SG_TEMP_PARAM_10,
-
-	CONF_SG_PARAMS_MAX,
-	CONF_SG_PARAMS_ALL = 0xff
-};
+#define WLCORE_CONF_SG_PARAMS_MAX 67
+#define WLCORE_CONF_SG_PARAMS_ALL 0xff
 
 struct conf_sg_settings {
-	u32 params[CONF_SG_PARAMS_MAX];
+	u32 params[WLCORE_CONF_SG_PARAMS_MAX];
 	u8 state;
 } __packed;
 
diff --git a/drivers/net/wireless/ti/wlcore/debugfs.c b/drivers/net/wireless/ti/wlcore/debugfs.c
index eb43f94a1597..7f672f6879d0 100644
--- a/drivers/net/wireless/ti/wlcore/debugfs.c
+++ b/drivers/net/wireless/ti/wlcore/debugfs.c
@@ -1205,26 +1205,11 @@ err_out:
 
 static loff_t dev_mem_seek(struct file *file, loff_t offset, int orig)
 {
-	loff_t ret;
-
 	/* only requests of dword-aligned size and offset are supported */
 	if (offset % 4)
 		return -EINVAL;
 
-	switch (orig) {
-	case SEEK_SET:
-		file->f_pos = offset;
-		ret = file->f_pos;
-		break;
-	case SEEK_CUR:
-		file->f_pos += offset;
-		ret = file->f_pos;
-		break;
-	default:
-		ret = -EINVAL;
-	}
-
-	return ret;
+	return no_seek_end_llseek(file, offset, orig);
 }
 
 static const struct file_operations dev_mem_ops = {
@@ -1234,6 +1219,65 @@ static const struct file_operations dev_mem_ops = {
 	.llseek = dev_mem_seek,
 };
 
+static ssize_t fw_logger_read(struct file *file, char __user *user_buf,
+			      size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+
+	return wl1271_format_buffer(user_buf, count,
+					ppos, "%d\n",
+					wl->conf.fwlog.output);
+}
+
+static ssize_t fw_logger_write(struct file *file,
+			       const char __user *user_buf,
+			       size_t count, loff_t *ppos)
+{
+	struct wl1271 *wl = file->private_data;
+	unsigned long value;
+	int ret;
+
+	ret = kstrtoul_from_user(user_buf, count, 0, &value);
+	if (ret < 0) {
+		wl1271_warning("illegal value in fw_logger");
+		return -EINVAL;
+	}
+
+	if ((value > 2) || (value == 0)) {
+		wl1271_warning("fw_logger value must be 1-UART 2-SDIO");
+		return -ERANGE;
+	}
+
+	if (wl->conf.fwlog.output == 0) {
+		wl1271_warning("iligal opperation - fw logger disabled by default, please change mode via wlconf");
+		return -EINVAL;
+	}
+
+	mutex_lock(&wl->mutex);
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0) {
+		count = ret;
+		goto out;
+	}
+
+	wl->conf.fwlog.output = value;
+
+	ret = wl12xx_cmd_config_fwlog(wl);
+
+	wl1271_ps_elp_sleep(wl);
+
+out:
+	mutex_unlock(&wl->mutex);
+	return count;
+}
+
+static const struct file_operations fw_logger_ops = {
+	.open = simple_open,
+	.read = fw_logger_read,
+	.write = fw_logger_write,
+	.llseek = default_llseek,
+};
+
 static int wl1271_debugfs_add_files(struct wl1271 *wl,
 				    struct dentry *rootdir)
 {
@@ -1260,6 +1304,7 @@ static int wl1271_debugfs_add_files(struct wl1271 *wl,
 	DEBUGFS_ADD(irq_timeout, rootdir);
 	DEBUGFS_ADD(fw_stats_raw, rootdir);
 	DEBUGFS_ADD(sleep_auth, rootdir);
+	DEBUGFS_ADD(fw_logger, rootdir);
 
 	streaming = debugfs_create_dir("rx_streaming", rootdir);
 	if (!streaming || IS_ERR(streaming))
diff --git a/drivers/net/wireless/ti/wlcore/event.c b/drivers/net/wireless/ti/wlcore/event.c
index c42e78955e7b..c96405498bf4 100644
--- a/drivers/net/wireless/ti/wlcore/event.c
+++ b/drivers/net/wireless/ti/wlcore/event.c
@@ -28,6 +28,88 @@
 #include "ps.h"
 #include "scan.h"
 #include "wl12xx_80211.h"
+#include "hw_ops.h"
+
+#define WL18XX_LOGGER_SDIO_BUFF_MAX	(0x1020)
+#define WL18XX_DATA_RAM_BASE_ADDRESS	(0x20000000)
+#define WL18XX_LOGGER_SDIO_BUFF_ADDR	(0x40159c)
+#define WL18XX_LOGGER_BUFF_OFFSET	(sizeof(struct fw_logger_information))
+#define WL18XX_LOGGER_READ_POINT_OFFSET		(12)
+
+int wlcore_event_fw_logger(struct wl1271 *wl)
+{
+	u32 ret;
+	struct fw_logger_information fw_log;
+	u8  *buffer;
+	u32 internal_fw_addrbase = WL18XX_DATA_RAM_BASE_ADDRESS;
+	u32 addr = WL18XX_LOGGER_SDIO_BUFF_ADDR;
+	u32 end_buff_addr = WL18XX_LOGGER_SDIO_BUFF_ADDR +
+				WL18XX_LOGGER_BUFF_OFFSET;
+	u32 available_len;
+	u32 actual_len;
+	u32 clear_addr;
+	size_t len;
+	u32 start_loc;
+
+	buffer = kzalloc(WL18XX_LOGGER_SDIO_BUFF_MAX, GFP_KERNEL);
+	if (!buffer) {
+		wl1271_error("Fail to allocate fw logger memory");
+		fw_log.actual_buff_size = cpu_to_le32(0);
+		goto out;
+	}
+
+	ret = wlcore_read(wl, addr, buffer, WL18XX_LOGGER_SDIO_BUFF_MAX,
+			  false);
+	if (ret < 0) {
+		wl1271_error("Fail to read logger buffer, error_id = %d",
+			     ret);
+		fw_log.actual_buff_size = cpu_to_le32(0);
+		goto free_out;
+	}
+
+	memcpy(&fw_log, buffer, sizeof(fw_log));
+
+	if (le32_to_cpu(fw_log.actual_buff_size) == 0)
+		goto free_out;
+
+	actual_len = le32_to_cpu(fw_log.actual_buff_size);
+	start_loc = (le32_to_cpu(fw_log.buff_read_ptr) -
+			internal_fw_addrbase) - addr;
+	end_buff_addr += le32_to_cpu(fw_log.max_buff_size);
+	available_len = end_buff_addr -
+			(le32_to_cpu(fw_log.buff_read_ptr) -
+				 internal_fw_addrbase);
+	actual_len = min(actual_len, available_len);
+	len = actual_len;
+
+	wl12xx_copy_fwlog(wl, &buffer[start_loc], len);
+	clear_addr = addr + start_loc + le32_to_cpu(fw_log.actual_buff_size) +
+			internal_fw_addrbase;
+
+	len = le32_to_cpu(fw_log.actual_buff_size) - len;
+	if (len) {
+		wl12xx_copy_fwlog(wl,
+				  &buffer[WL18XX_LOGGER_BUFF_OFFSET],
+				  len);
+		clear_addr = addr + WL18XX_LOGGER_BUFF_OFFSET + len +
+				internal_fw_addrbase;
+	}
+
+	/* double check that clear address and write pointer are the same */
+	if (clear_addr != le32_to_cpu(fw_log.buff_write_ptr)) {
+		wl1271_error("Calculate of clear addr Clear = %x, write = %x",
+			     clear_addr, le32_to_cpu(fw_log.buff_write_ptr));
+	}
+
+	/* indicate FW about Clear buffer */
+	ret = wlcore_write32(wl, addr + WL18XX_LOGGER_READ_POINT_OFFSET,
+			     fw_log.buff_write_ptr);
+free_out:
+	kfree(buffer);
+out:
+	return le32_to_cpu(fw_log.actual_buff_size);
+}
+EXPORT_SYMBOL_GPL(wlcore_event_fw_logger);
 
 void wlcore_event_rssi_trigger(struct wl1271 *wl, s8 *metric_arr)
 {
diff --git a/drivers/net/wireless/ti/wlcore/event.h b/drivers/net/wireless/ti/wlcore/event.h
index acc7a59d3828..75e8e98da2fe 100644
--- a/drivers/net/wireless/ti/wlcore/event.h
+++ b/drivers/net/wireless/ti/wlcore/event.h
@@ -64,6 +64,14 @@ enum {
 
 #define NUM_OF_RSSI_SNR_TRIGGERS 8
 
+struct fw_logger_information {
+	__le32 max_buff_size;
+	__le32 actual_buff_size;
+	__le32 num_trace_drop;
+	__le32 buff_read_ptr;
+	__le32 buff_write_ptr;
+} __packed;
+
 struct wl1271;
 
 int wl1271_event_unmask(struct wl1271 *wl);
@@ -84,4 +92,5 @@ void wlcore_event_max_tx_failure(struct wl1271 *wl, unsigned long sta_bitmap);
 void wlcore_event_inactive_sta(struct wl1271 *wl, unsigned long sta_bitmap);
 void wlcore_event_roc_complete(struct wl1271 *wl);
 void wlcore_event_rssi_trigger(struct wl1271 *wl, s8 *metric_arr);
+int  wlcore_event_fw_logger(struct wl1271 *wl);
 #endif
diff --git a/drivers/net/wireless/ti/wlcore/io.c b/drivers/net/wireless/ti/wlcore/io.c
index 68e74eefd296..564ca750c5ee 100644
--- a/drivers/net/wireless/ti/wlcore/io.c
+++ b/drivers/net/wireless/ti/wlcore/io.c
@@ -175,12 +175,13 @@ int wlcore_set_partition(struct wl1271 *wl,
 	if (ret < 0)
 		goto out;
 
-	/*
-	 * We don't need the size of the last partition, as it is
+	/* We don't need the size of the last partition, as it is
 	 * automatically calculated based on the total memory size and
 	 * the sizes of the previous partitions.
 	 */
 	ret = wlcore_raw_write32(wl, HW_PART3_START_ADDR, p->mem3.start);
+	if (ret < 0)
+		goto out;
 
 out:
 	return ret;
diff --git a/drivers/net/wireless/ti/wlcore/io.h b/drivers/net/wireless/ti/wlcore/io.h
index 0305729d0986..10cf3747694d 100644
--- a/drivers/net/wireless/ti/wlcore/io.h
+++ b/drivers/net/wireless/ti/wlcore/io.h
@@ -207,19 +207,23 @@ static inline int __must_check wlcore_write_reg(struct wl1271 *wl, int reg,
 
 static inline void wl1271_power_off(struct wl1271 *wl)
 {
-	int ret;
+	int ret = 0;
 
 	if (!test_bit(WL1271_FLAG_GPIO_POWER, &wl->flags))
 		return;
 
-	ret = wl->if_ops->power(wl->dev, false);
+	if (wl->if_ops->power)
+		ret = wl->if_ops->power(wl->dev, false);
 	if (!ret)
 		clear_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
 }
 
 static inline int wl1271_power_on(struct wl1271 *wl)
 {
-	int ret = wl->if_ops->power(wl->dev, true);
+	int ret = 0;
+
+	if (wl->if_ops->power)
+		ret = wl->if_ops->power(wl->dev, true);
 	if (ret == 0)
 		set_bit(WL1271_FLAG_GPIO_POWER, &wl->flags);
 
diff --git a/drivers/net/wireless/ti/wlcore/main.c b/drivers/net/wireless/ti/wlcore/main.c
index ec7f6af3fab2..d1109c4f0f0d 100644
--- a/drivers/net/wireless/ti/wlcore/main.c
+++ b/drivers/net/wireless/ti/wlcore/main.c
@@ -1,4 +1,3 @@
-
 /*
  * This file is part of wlcore
  *
@@ -303,25 +302,11 @@ out:
 
 static void wlcore_adjust_conf(struct wl1271 *wl)
 {
-	/* Adjust settings according to optional module parameters */
-
-	/* Firmware Logger params */
-	if (fwlog_mem_blocks != -1) {
-		if (fwlog_mem_blocks >= CONF_FWLOG_MIN_MEM_BLOCKS &&
-		    fwlog_mem_blocks <= CONF_FWLOG_MAX_MEM_BLOCKS) {
-			wl->conf.fwlog.mem_blocks = fwlog_mem_blocks;
-		} else {
-			wl1271_error(
-				"Illegal fwlog_mem_blocks=%d using default %d",
-				fwlog_mem_blocks, wl->conf.fwlog.mem_blocks);
-		}
-	}
 
 	if (fwlog_param) {
 		if (!strcmp(fwlog_param, "continuous")) {
 			wl->conf.fwlog.mode = WL12XX_FWLOG_CONTINUOUS;
-		} else if (!strcmp(fwlog_param, "ondemand")) {
-			wl->conf.fwlog.mode = WL12XX_FWLOG_ON_DEMAND;
+			wl->conf.fwlog.output = WL12XX_FWLOG_OUTPUT_HOST;
 		} else if (!strcmp(fwlog_param, "dbgpins")) {
 			wl->conf.fwlog.mode = WL12XX_FWLOG_CONTINUOUS;
 			wl->conf.fwlog.output = WL12XX_FWLOG_OUTPUT_DBG_PINS;
@@ -825,91 +810,32 @@ size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
 
 static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
 {
-	struct wlcore_partition_set part, old_part;
-	u32 addr;
-	u32 offset;
-	u32 end_of_log;
-	u8 *block;
-	int ret;
+	u32 end_of_log = 0;
 
-	if ((wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED) ||
-	    (wl->conf.fwlog.mem_blocks == 0))
+	if (wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED)
 		return;
 
 	wl1271_info("Reading FW panic log");
 
-	block = kmalloc(wl->fw_mem_block_size, GFP_KERNEL);
-	if (!block)
-		return;
-
 	/*
 	 * Make sure the chip is awake and the logger isn't active.
 	 * Do not send a stop fwlog command if the fw is hanged or if
 	 * dbgpins are used (due to some fw bug).
 	 */
 	if (wl1271_ps_elp_wakeup(wl))
-		goto out;
+		return;
 	if (!wl->watchdog_recovery &&
 	    wl->conf.fwlog.output != WL12XX_FWLOG_OUTPUT_DBG_PINS)
 		wl12xx_cmd_stop_fwlog(wl);
 
-	/* Read the first memory block address */
-	ret = wlcore_fw_status(wl, wl->fw_status);
-	if (ret < 0)
-		goto out;
-
-	addr = wl->fw_status->log_start_addr;
-	if (!addr)
-		goto out;
-
-	if (wl->conf.fwlog.mode == WL12XX_FWLOG_CONTINUOUS) {
-		offset = sizeof(addr) + sizeof(struct wl1271_rx_descriptor);
-		end_of_log = wl->fwlog_end;
-	} else {
-		offset = sizeof(addr);
-		end_of_log = addr;
-	}
-
-	old_part = wl->curr_part;
-	memset(&part, 0, sizeof(part));
-
 	/* Traverse the memory blocks linked list */
 	do {
-		part.mem.start = wlcore_hw_convert_hwaddr(wl, addr);
-		part.mem.size  = PAGE_SIZE;
-
-		ret = wlcore_set_partition(wl, &part);
-		if (ret < 0) {
-			wl1271_error("%s: set_partition start=0x%X size=%d",
-				__func__, part.mem.start, part.mem.size);
-			goto out;
+		end_of_log = wlcore_event_fw_logger(wl);
+		if (end_of_log == 0) {
+			msleep(100);
+			end_of_log = wlcore_event_fw_logger(wl);
 		}
-
-		memset(block, 0, wl->fw_mem_block_size);
-		ret = wlcore_read_hwaddr(wl, addr, block,
-					wl->fw_mem_block_size, false);
-
-		if (ret < 0)
-			goto out;
-
-		/*
-		 * Memory blocks are linked to one another. The first 4 bytes
-		 * of each memory block hold the hardware address of the next
-		 * one. The last memory block points to the first one in
-		 * on demand mode and is equal to 0x2000000 in continuous mode.
-		 */
-		addr = le32_to_cpup((__le32 *)block);
-
-		if (!wl12xx_copy_fwlog(wl, block + offset,
-					wl->fw_mem_block_size - offset))
-			break;
-	} while (addr && (addr != end_of_log));
-
-	wake_up_interruptible(&wl->fwlog_waitq);
-
-out:
-	kfree(block);
-	wlcore_set_partition(wl, &old_part);
+	} while (end_of_log != 0);
 }
 
 static void wlcore_save_freed_pkts(struct wl1271 *wl, struct wl12xx_vif *wlvif,
@@ -6291,7 +6217,6 @@ struct ieee80211_hw *wlcore_alloc_hw(size_t priv_size, u32 aggr_buf_size,
 	wl->active_sta_count = 0;
 	wl->active_link_count = 0;
 	wl->fwlog_size = 0;
-	init_waitqueue_head(&wl->fwlog_waitq);
 
 	/* The system link is always allocated */
 	__set_bit(WL12XX_SYSTEM_HLID, wl->links_map);
@@ -6377,7 +6302,6 @@ int wlcore_free_hw(struct wl1271 *wl)
 	/* Unblock any fwlog readers */
 	mutex_lock(&wl->mutex);
 	wl->fwlog_size = -1;
-	wake_up_interruptible_all(&wl->fwlog_waitq);
 	mutex_unlock(&wl->mutex);
 
 	wlcore_sysfs_free(wl);
@@ -6584,7 +6508,7 @@ MODULE_PARM_DESC(debug_level, "wl12xx debugging level");
 
 module_param_named(fwlog, fwlog_param, charp, 0);
 MODULE_PARM_DESC(fwlog,
-		 "FW logger options: continuous, ondemand, dbgpins or disable");
+		 "FW logger options: continuous, dbgpins or disable");
 
 module_param(fwlog_mem_blocks, int, S_IRUSR | S_IWUSR);
 MODULE_PARM_DESC(fwlog_mem_blocks, "fwlog mem_blocks");
diff --git a/drivers/net/wireless/ti/wlcore/rx.c b/drivers/net/wireless/ti/wlcore/rx.c
index 5b2927391d1c..34e7e938ede4 100644
--- a/drivers/net/wireless/ti/wlcore/rx.c
+++ b/drivers/net/wireless/ti/wlcore/rx.c
@@ -149,7 +149,6 @@ static int wl1271_rx_handle_data(struct wl1271 *wl, u8 *data, u32 length,
 	if (desc->packet_class == WL12XX_RX_CLASS_LOGGER) {
 		size_t len = length - sizeof(*desc);
 		wl12xx_copy_fwlog(wl, data + sizeof(*desc), len);
-		wake_up_interruptible(&wl->fwlog_waitq);
 		return 0;
 	}
 
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index 236b41090827..44f059f7f34e 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -73,7 +73,10 @@
  */
 #define SPI_AGGR_BUFFER_SIZE (4 * PAGE_SIZE)
 
-#define WSPI_MAX_NUM_OF_CHUNKS (SPI_AGGR_BUFFER_SIZE / WSPI_MAX_CHUNK_SIZE)
+/* Maximum number of SPI write chunks */
+#define WSPI_MAX_NUM_OF_CHUNKS \
+	((SPI_AGGR_BUFFER_SIZE / WSPI_MAX_CHUNK_SIZE) + 1)
+
 
 struct wl12xx_spi_glue {
 	struct device *dev;
@@ -268,9 +271,10 @@ static int __must_check wl12xx_spi_raw_write(struct device *child, int addr,
 					     void *buf, size_t len, bool fixed)
 {
 	struct wl12xx_spi_glue *glue = dev_get_drvdata(child->parent);
-	struct spi_transfer t[2 * (WSPI_MAX_NUM_OF_CHUNKS + 1)];
+	/* SPI write buffers - 2 for each chunk */
+	struct spi_transfer t[2 * WSPI_MAX_NUM_OF_CHUNKS];
 	struct spi_message m;
-	u32 commands[WSPI_MAX_NUM_OF_CHUNKS];
+	u32 commands[WSPI_MAX_NUM_OF_CHUNKS]; /* 1 command per chunk */
 	u32 *cmd;
 	u32 chunk_len;
 	int i;
diff --git a/drivers/net/wireless/ti/wlcore/sysfs.c b/drivers/net/wireless/ti/wlcore/sysfs.c
index 24dd288d6809..a9218e5b0efc 100644
--- a/drivers/net/wireless/ti/wlcore/sysfs.c
+++ b/drivers/net/wireless/ti/wlcore/sysfs.c
@@ -119,32 +119,6 @@ static ssize_t wl1271_sysfs_read_fwlog(struct file *filp, struct kobject *kobj,
 	if (ret < 0)
 		return -ERESTARTSYS;
 
-	/* Let only one thread read the log at a time, blocking others */
-	while (wl->fwlog_size == 0) {
-		DEFINE_WAIT(wait);
-
-		prepare_to_wait_exclusive(&wl->fwlog_waitq,
-					  &wait,
-					  TASK_INTERRUPTIBLE);
-
-		if (wl->fwlog_size != 0) {
-			finish_wait(&wl->fwlog_waitq, &wait);
-			break;
-		}
-
-		mutex_unlock(&wl->mutex);
-
-		schedule();
-		finish_wait(&wl->fwlog_waitq, &wait);
-
-		if (signal_pending(current))
-			return -ERESTARTSYS;
-
-		ret = mutex_lock_interruptible(&wl->mutex);
-		if (ret < 0)
-			return -ERESTARTSYS;
-	}
-
 	/* Check if the fwlog is still valid */
 	if (wl->fwlog_size < 0) {
 		mutex_unlock(&wl->mutex);
diff --git a/drivers/net/wireless/ti/wlcore/wlcore.h b/drivers/net/wireless/ti/wlcore/wlcore.h
index 906be6aa4eb6..dda01b118c26 100644
--- a/drivers/net/wireless/ti/wlcore/wlcore.h
+++ b/drivers/net/wireless/ti/wlcore/wlcore.h
@@ -310,9 +310,6 @@ struct wl1271 {
 	/* FW memory block size */
 	u32 fw_mem_block_size;
 
-	/* Sysfs FW log entry readers wait queue */
-	wait_queue_head_t fwlog_waitq;
-
 	/* Hardware recovery work */
 	struct work_struct recovery_work;
 	bool watchdog_recovery;
diff --git a/drivers/net/wireless/zydas/Kconfig b/drivers/net/wireless/zydas/Kconfig
new file mode 100644
index 000000000000..a58c0f65e376
--- /dev/null
+++ b/drivers/net/wireless/zydas/Kconfig
@@ -0,0 +1,35 @@
+config WLAN_VENDOR_ZYDAS
+	bool "ZyDAS devices"
+	default y
+	---help---
+	  If you have a wireless card belonging to this class, say Y.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about  cards. If you say Y, you will be asked for
+	  your specific card in the following questions.
+
+if WLAN_VENDOR_ZYDAS
+
+config USB_ZD1201
+	tristate "USB ZD1201 based Wireless device support"
+	depends on CFG80211 && USB
+	select WIRELESS_EXT
+	select WEXT_PRIV
+	select FW_LOADER
+	---help---
+	  Say Y if you want to use wireless LAN adapters based on the ZyDAS
+	  ZD1201 chip.
+
+	  This driver makes the adapter appear as a normal Ethernet interface,
+	  typically on wlan0.
+
+	  The zd1201 device requires external firmware to be loaded.
+	  This can be found at http://linux-lc100020.sourceforge.net/
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called zd1201.
+
+source "drivers/net/wireless/zydas/zd1211rw/Kconfig"
+
+endif # WLAN_VENDOR_ZYDAS
diff --git a/drivers/net/wireless/zydas/Makefile b/drivers/net/wireless/zydas/Makefile
new file mode 100644
index 000000000000..679fbbf3a6cd
--- /dev/null
+++ b/drivers/net/wireless/zydas/Makefile
@@ -0,0 +1,3 @@
+obj-$(CONFIG_ZD1211RW)		+= zd1211rw/
+
+obj-$(CONFIG_USB_ZD1201)	+= zd1201.o
diff --git a/drivers/net/wireless/zd1201.c b/drivers/net/wireless/zydas/zd1201.c
index 6f5c793a7855..6f5c793a7855 100644
--- a/drivers/net/wireless/zd1201.c
+++ b/drivers/net/wireless/zydas/zd1201.c
diff --git a/drivers/net/wireless/zd1201.h b/drivers/net/wireless/zydas/zd1201.h
index dd7ea1f35bef..dd7ea1f35bef 100644
--- a/drivers/net/wireless/zd1201.h
+++ b/drivers/net/wireless/zydas/zd1201.h
diff --git a/drivers/net/wireless/zd1211rw/Kconfig b/drivers/net/wireless/zydas/zd1211rw/Kconfig
index 95920581860a..95920581860a 100644
--- a/drivers/net/wireless/zd1211rw/Kconfig
+++ b/drivers/net/wireless/zydas/zd1211rw/Kconfig
diff --git a/drivers/net/wireless/zd1211rw/Makefile b/drivers/net/wireless/zydas/zd1211rw/Makefile
index 5728a918e508..5728a918e508 100644
--- a/drivers/net/wireless/zd1211rw/Makefile
+++ b/drivers/net/wireless/zydas/zd1211rw/Makefile
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zydas/zd1211rw/zd_chip.c
index 07b94eda9604..07b94eda9604 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_chip.c
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zydas/zd1211rw/zd_chip.h
index b03786c9f3aa..b03786c9f3aa 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_chip.h
diff --git a/drivers/net/wireless/zd1211rw/zd_def.h b/drivers/net/wireless/zydas/zd1211rw/zd_def.h
index 41bd755bc135..41bd755bc135 100644
--- a/drivers/net/wireless/zd1211rw/zd_def.h
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_def.h
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zydas/zd1211rw/zd_mac.c
index e539d9b1b562..e539d9b1b562 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_mac.c
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zydas/zd1211rw/zd_mac.h
index 5a484235308f..5a484235308f 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_mac.h
diff --git a/drivers/net/wireless/zd1211rw/zd_rf.c b/drivers/net/wireless/zydas/zd1211rw/zd_rf.c
index dc179c414518..dc179c414518 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf.c
diff --git a/drivers/net/wireless/zd1211rw/zd_rf.h b/drivers/net/wireless/zydas/zd1211rw/zd_rf.h
index 8f14e25e1041..8f14e25e1041 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf.h
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf.h
diff --git a/drivers/net/wireless/zd1211rw/zd_rf_al2230.c b/drivers/net/wireless/zydas/zd1211rw/zd_rf_al2230.c
index 99aed7d78952..99aed7d78952 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf_al2230.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf_al2230.c
diff --git a/drivers/net/wireless/zd1211rw/zd_rf_al7230b.c b/drivers/net/wireless/zydas/zd1211rw/zd_rf_al7230b.c
index 5fea485be574..5fea485be574 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf_al7230b.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf_al7230b.c
diff --git a/drivers/net/wireless/zd1211rw/zd_rf_rf2959.c b/drivers/net/wireless/zydas/zd1211rw/zd_rf_rf2959.c
index a93f657a41c7..a93f657a41c7 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf_rf2959.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf_rf2959.c
diff --git a/drivers/net/wireless/zd1211rw/zd_rf_uw2453.c b/drivers/net/wireless/zydas/zd1211rw/zd_rf_uw2453.c
index 61b924027356..61b924027356 100644
--- a/drivers/net/wireless/zd1211rw/zd_rf_uw2453.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_rf_uw2453.c
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zydas/zd1211rw/zd_usb.c
index a912dc051111..a912dc051111 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_usb.c
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.h b/drivers/net/wireless/zydas/zd1211rw/zd_usb.h
index a9075f225178..a9075f225178 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.h
+++ b/drivers/net/wireless/zydas/zd1211rw/zd_usb.h
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index e7bd63eb2876..f5231a2dd2ac 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -615,6 +615,7 @@ err_tx_unbind:
 	queue->tx_irq = 0;
 err_unmap:
 	xenvif_unmap_frontend_rings(queue);
+	netif_napi_del(&queue->napi);
 err:
 	module_put(THIS_MODULE);
 	return err;
@@ -684,22 +685,16 @@ void xenvif_deinit_queue(struct xenvif_queue *queue)
 
 void xenvif_free(struct xenvif *vif)
 {
-	struct xenvif_queue *queue = NULL;
+	struct xenvif_queue *queues = vif->queues;
 	unsigned int num_queues = vif->num_queues;
 	unsigned int queue_index;
 
 	unregister_netdev(vif->dev);
-
-	for (queue_index = 0; queue_index < num_queues; ++queue_index) {
-		queue = &vif->queues[queue_index];
-		xenvif_deinit_queue(queue);
-	}
-
-	vfree(vif->queues);
-	vif->queues = NULL;
-	vif->num_queues = 0;
-
 	free_netdev(vif->dev);
 
+	for (queue_index = 0; queue_index < num_queues; ++queue_index)
+		xenvif_deinit_queue(&queues[queue_index]);
+	vfree(queues);
+
 	module_put(THIS_MODULE);
 }
diff --git a/drivers/net/xen-netback/netback.c b/drivers/net/xen-netback/netback.c
index 1049c34e7d43..61b97c34bb3b 100644
--- a/drivers/net/xen-netback/netback.c
+++ b/drivers/net/xen-netback/netback.c
@@ -149,20 +149,19 @@ static inline pending_ring_idx_t pending_index(unsigned i)
 	return i & (MAX_PENDING_REQS-1);
 }
 
-static int xenvif_rx_ring_slots_needed(struct xenvif *vif)
-{
-	if (vif->gso_mask)
-		return DIV_ROUND_UP(vif->dev->gso_max_size, XEN_PAGE_SIZE) + 1;
-	else
-		return DIV_ROUND_UP(vif->dev->mtu, XEN_PAGE_SIZE);
-}
-
 static bool xenvif_rx_ring_slots_available(struct xenvif_queue *queue)
 {
 	RING_IDX prod, cons;
+	struct sk_buff *skb;
 	int needed;
 
-	needed = xenvif_rx_ring_slots_needed(queue->vif);
+	skb = skb_peek(&queue->rx_queue);
+	if (!skb)
+		return false;
+
+	needed = DIV_ROUND_UP(skb->len, XEN_PAGE_SIZE);
+	if (skb_is_gso(skb))
+		needed++;
 
 	do {
 		prod = queue->rx.sring->req_prod;
@@ -2005,8 +2004,7 @@ static bool xenvif_rx_queue_ready(struct xenvif_queue *queue)
 
 static bool xenvif_have_rx_work(struct xenvif_queue *queue)
 {
-	return (!skb_queue_empty(&queue->rx_queue)
-		&& xenvif_rx_ring_slots_available(queue))
+	return xenvif_rx_ring_slots_available(queue)
 		|| (queue->vif->stall_timeout &&
 		    (xenvif_rx_queue_stalled(queue)
 		     || xenvif_rx_queue_ready(queue)))
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index d6abf191122a..96ccd4e943db 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -364,6 +364,7 @@ static void xennet_tx_buf_gc(struct netfront_queue *queue)
 	RING_IDX cons, prod;
 	unsigned short id;
 	struct sk_buff *skb;
+	bool more_to_do;
 
 	BUG_ON(!netif_carrier_ok(queue->info->netdev));
 
@@ -398,18 +399,8 @@ static void xennet_tx_buf_gc(struct netfront_queue *queue)
 
 		queue->tx.rsp_cons = prod;
 
-		/*
-		 * Set a new event, then check for race with update of tx_cons.
-		 * Note that it is essential to schedule a callback, no matter
-		 * how few buffers are pending. Even if there is space in the
-		 * transmit ring, higher layers may be blocked because too much
-		 * data is outstanding: in such cases notification from Xen is
-		 * likely to be the only kick that we'll get.
-		 */
-		queue->tx.sring->rsp_event =
-			prod + ((queue->tx.sring->req_prod - prod) >> 1) + 1;
-		mb();		/* update shared area */
-	} while ((cons == prod) && (prod != queue->tx.sring->rsp_prod));
+		RING_FINAL_CHECK_FOR_RESPONSES(&queue->tx, more_to_do);
+	} while (more_to_do);
 
 	xennet_maybe_wake_tx(queue);
 }
diff --git a/drivers/nfc/Kconfig b/drivers/nfc/Kconfig
index 0d6003dee3af..7437c9dfd8fc 100644
--- a/drivers/nfc/Kconfig
+++ b/drivers/nfc/Kconfig
@@ -76,4 +76,5 @@ source "drivers/nfc/st21nfca/Kconfig"
 source "drivers/nfc/st-nci/Kconfig"
 source "drivers/nfc/nxp-nci/Kconfig"
 source "drivers/nfc/s3fwrn5/Kconfig"
+source "drivers/nfc/st95hf/Kconfig"
 endmenu
diff --git a/drivers/nfc/Makefile b/drivers/nfc/Makefile
index e3621416a48e..0a99e67daa10 100644
--- a/drivers/nfc/Makefile
+++ b/drivers/nfc/Makefile
@@ -16,3 +16,4 @@ obj-$(CONFIG_NFC_ST21NFCA)  	+= st21nfca/
 obj-$(CONFIG_NFC_ST_NCI)	+= st-nci/
 obj-$(CONFIG_NFC_NXP_NCI)	+= nxp-nci/
 obj-$(CONFIG_NFC_S3FWRN5)	+= s3fwrn5/
+obj-$(CONFIG_NFC_ST95HF)	+= st95hf/
diff --git a/drivers/nfc/fdp/i2c.c b/drivers/nfc/fdp/i2c.c
index 532db28145c7..5e797d5c38ed 100644
--- a/drivers/nfc/fdp/i2c.c
+++ b/drivers/nfc/fdp/i2c.c
@@ -298,6 +298,12 @@ static int fdp_nci_i2c_probe(struct i2c_client *client,
 		return -ENODEV;
 	}
 
+	/* Checking if we have an irq */
+	if (client->irq <= 0) {
+		nfc_err(dev, "IRQ not present\n");
+		return -ENODEV;
+	}
+
 	phy = devm_kzalloc(dev, sizeof(struct fdp_i2c_phy),
 			   GFP_KERNEL);
 	if (!phy)
@@ -307,12 +313,6 @@ static int fdp_nci_i2c_probe(struct i2c_client *client,
 	phy->next_read_size = FDP_NCI_I2C_MIN_PAYLOAD;
 	i2c_set_clientdata(client, phy);
 
-	/* Checking if we have an irq */
-	if (client->irq <= 0) {
-		dev_err(dev, "IRQ not present\n");
-		return -ENODEV;
-	}
-
 	r = request_threaded_irq(client->irq, NULL, fdp_nci_i2c_irq_thread_fn,
 				 IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 				 FDP_I2C_DRIVER_NAME, phy);
diff --git a/drivers/nfc/microread/i2c.c b/drivers/nfc/microread/i2c.c
index daf352597ef8..918e8f2eac47 100644
--- a/drivers/nfc/microread/i2c.c
+++ b/drivers/nfc/microread/i2c.c
@@ -50,8 +50,6 @@ struct microread_i2c_phy {
 	struct i2c_client *i2c_dev;
 	struct nfc_hci_dev *hdev;
 
-	int irq;
-
 	int hard_fault;		/*
 				 * < 0 if hardware error occured (e.g. i2c err)
 				 * and prevents normal operation.
diff --git a/drivers/nfc/nfcsim.c b/drivers/nfc/nfcsim.c
index 26ac9e5fa1ab..93aaca586858 100644
--- a/drivers/nfc/nfcsim.c
+++ b/drivers/nfc/nfcsim.c
@@ -32,6 +32,8 @@
 #define NFCSIM_POLL_TARGET	2
 #define NFCSIM_POLL_DUAL	(NFCSIM_POLL_INITIATOR | NFCSIM_POLL_TARGET)
 
+#define RX_DEFAULT_DELAY	5
+
 struct nfcsim {
 	struct nfc_dev *nfc_dev;
 
@@ -51,6 +53,8 @@ struct nfcsim {
 
 	u8 initiator;
 
+	u32 rx_delay;
+
 	data_exchange_cb_t cb;
 	void *cb_context;
 
@@ -320,10 +324,9 @@ static int nfcsim_tx(struct nfc_dev *nfc_dev, struct nfc_target *target,
 	 * If packet transmission occurs immediately between them, we have a
 	 * non-stop flow of several tens of thousands SYMM packets per second
 	 * and a burning cpu.
-	 *
-	 * TODO: Add support for a sysfs entry to control this delay.
 	 */
-	queue_delayed_work(wq, &peer->recv_work, msecs_to_jiffies(5));
+	queue_delayed_work(wq, &peer->recv_work,
+			msecs_to_jiffies(dev->rx_delay));
 
 	mutex_unlock(&peer->lock);
 
@@ -461,6 +464,7 @@ static struct nfcsim *nfcsim_init_dev(void)
 	if (rc)
 		goto free_nfc_dev;
 
+	dev->rx_delay = RX_DEFAULT_DELAY;
 	return dev;
 
 free_nfc_dev:
diff --git a/drivers/nfc/nxp-nci/i2c.c b/drivers/nfc/nxp-nci/i2c.c
index df4333c7ee0f..11520f472f98 100644
--- a/drivers/nfc/nxp-nci/i2c.c
+++ b/drivers/nfc/nxp-nci/i2c.c
@@ -52,7 +52,6 @@ struct nxp_nci_i2c_phy {
 
 	unsigned int gpio_en;
 	unsigned int gpio_fw;
-	unsigned int gpio_irq;
 
 	int hard_fault; /*
 			 * < 0 if hardware error occurred (e.g. i2c err)
@@ -85,7 +84,7 @@ static int nxp_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 		return phy->hard_fault;
 
 	r = i2c_master_send(client, skb->data, skb->len);
-	if (r == -EREMOTEIO) {
+	if (r < 0) {
 		/* Retry, chip was in standby */
 		usleep_range(110000, 120000);
 		r = i2c_master_send(client, skb->data, skb->len);
@@ -264,8 +263,6 @@ exit_irq_none:
 	return IRQ_NONE;
 }
 
-#ifdef CONFIG_OF
-
 static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
 {
 	struct nxp_nci_i2c_phy *phy = i2c_get_clientdata(client);
@@ -294,48 +291,24 @@ static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
 	}
 	phy->gpio_fw = r;
 
-	r = irq_of_parse_and_map(pp, 0);
-	if (r < 0) {
-		nfc_err(&client->dev, "Unable to get irq, error: %d\n", r);
-		return r;
-	}
-	client->irq = r;
-
 	return 0;
 }
 
-#else
-
-static int nxp_nci_i2c_parse_devtree(struct i2c_client *client)
-{
-	return -ENODEV;
-}
-
-#endif
-
 static int nxp_nci_i2c_acpi_config(struct nxp_nci_i2c_phy *phy)
 {
 	struct i2c_client *client = phy->i2c_dev;
-	struct gpio_desc *gpiod_en, *gpiod_fw, *gpiod_irq;
+	struct gpio_desc *gpiod_en, *gpiod_fw;
 
 	gpiod_en = devm_gpiod_get_index(&client->dev, NULL, 2, GPIOD_OUT_LOW);
 	gpiod_fw = devm_gpiod_get_index(&client->dev, NULL, 1, GPIOD_OUT_LOW);
-	gpiod_irq = devm_gpiod_get_index(&client->dev, NULL, 0, GPIOD_IN);
 
-	if (IS_ERR(gpiod_en) || IS_ERR(gpiod_fw) || IS_ERR(gpiod_irq)) {
+	if (IS_ERR(gpiod_en) || IS_ERR(gpiod_fw)) {
 		nfc_err(&client->dev, "No GPIOs\n");
 		return -EINVAL;
 	}
 
-	client->irq = gpiod_to_irq(gpiod_irq);
-	if (client->irq < 0) {
-		nfc_err(&client->dev, "No IRQ\n");
-		return -EINVAL;
-	}
-
 	phy->gpio_en = desc_to_gpio(gpiod_en);
 	phy->gpio_fw = desc_to_gpio(gpiod_fw);
-	phy->gpio_irq = desc_to_gpio(gpiod_irq);
 
 	return 0;
 }
@@ -374,7 +347,6 @@ static int nxp_nci_i2c_probe(struct i2c_client *client,
 	} else if (pdata) {
 		phy->gpio_en = pdata->gpio_en;
 		phy->gpio_fw = pdata->gpio_fw;
-		client->irq = pdata->irq;
 	} else if (ACPI_HANDLE(&client->dev)) {
 		r = nxp_nci_i2c_acpi_config(phy);
 		if (r < 0)
diff --git a/drivers/nfc/pn544/i2c.c b/drivers/nfc/pn544/i2c.c
index fa75c53f3fa5..76c318444304 100644
--- a/drivers/nfc/pn544/i2c.c
+++ b/drivers/nfc/pn544/i2c.c
@@ -166,7 +166,6 @@ struct pn544_i2c_phy {
 	struct nfc_hci_dev *hdev;
 
 	unsigned int gpio_en;
-	unsigned int gpio_irq;
 	unsigned int gpio_fw;
 	unsigned int en_polarity;
 
@@ -879,9 +878,8 @@ static int pn544_hci_i2c_acpi_request_resources(struct i2c_client *client)
 {
 	struct pn544_i2c_phy *phy = i2c_get_clientdata(client);
 	const struct acpi_device_id *id;
-	struct gpio_desc *gpiod_en, *gpiod_irq, *gpiod_fw;
+	struct gpio_desc *gpiod_en, *gpiod_fw;
 	struct device *dev;
-	int ret;
 
 	if (!client)
 		return -EINVAL;
@@ -914,32 +912,9 @@ static int pn544_hci_i2c_acpi_request_resources(struct i2c_client *client)
 
 	phy->gpio_fw = desc_to_gpio(gpiod_fw);
 
-	/* Get IRQ GPIO */
-	gpiod_irq = devm_gpiod_get_index(dev, PN544_GPIO_NAME_IRQ, 0,
-					 GPIOD_IN);
-	if (IS_ERR(gpiod_irq)) {
-		nfc_err(dev, "Unable to get IRQ GPIO\n");
-		return -ENODEV;
-	}
-
-	phy->gpio_irq = desc_to_gpio(gpiod_irq);
-
-	/* Map the pin to an IRQ */
-	ret = gpiod_to_irq(gpiod_irq);
-	if (ret < 0) {
-		nfc_err(dev, "Fail pin IRQ mapping\n");
-		return ret;
-	}
-
-	nfc_info(dev, "GPIO resource, no:%d irq:%d\n",
-		 desc_to_gpio(gpiod_irq), ret);
-	client->irq = ret;
-
 	return 0;
 }
 
-#ifdef CONFIG_OF
-
 static int pn544_hci_i2c_of_request_resources(struct i2c_client *client)
 {
 	struct pn544_i2c_phy *phy = i2c_get_clientdata(client);
@@ -996,15 +971,6 @@ static int pn544_hci_i2c_of_request_resources(struct i2c_client *client)
 		goto err_gpio_fw;
 	}
 
-	/* IRQ */
-	ret = irq_of_parse_and_map(pp, 0);
-	if (ret < 0) {
-		nfc_err(&client->dev,
-			"Unable to get irq, error: %d\n", ret);
-		goto err_gpio_fw;
-	}
-	client->irq = ret;
-
 	return 0;
 
 err_gpio_fw:
@@ -1015,15 +981,6 @@ err_dt:
 	return ret;
 }
 
-#else
-
-static int pn544_hci_i2c_of_request_resources(struct i2c_client *client)
-{
-	return -ENODEV;
-}
-
-#endif
-
 static int pn544_hci_i2c_probe(struct i2c_client *client,
 			       const struct i2c_device_id *id)
 {
@@ -1076,7 +1033,6 @@ static int pn544_hci_i2c_probe(struct i2c_client *client,
 
 		phy->gpio_en = pdata->get_gpio(NFC_GPIO_ENABLE);
 		phy->gpio_fw = pdata->get_gpio(NFC_GPIO_FW_RESET);
-		phy->gpio_irq = pdata->get_gpio(NFC_GPIO_IRQ);
 	/* Using ACPI */
 	} else if (ACPI_HANDLE(&client->dev)) {
 		r = pn544_hci_i2c_acpi_request_resources(client);
diff --git a/drivers/nfc/s3fwrn5/core.c b/drivers/nfc/s3fwrn5/core.c
index 0d866ca295e3..9d9c8d57a042 100644
--- a/drivers/nfc/s3fwrn5/core.c
+++ b/drivers/nfc/s3fwrn5/core.c
@@ -147,7 +147,7 @@ static struct nci_ops s3fwrn5_nci_ops = {
 };
 
 int s3fwrn5_probe(struct nci_dev **ndev, void *phy_id, struct device *pdev,
-	struct s3fwrn5_phy_ops *phy_ops, unsigned int max_payload)
+	const struct s3fwrn5_phy_ops *phy_ops, unsigned int max_payload)
 {
 	struct s3fwrn5_info *info;
 	int ret;
diff --git a/drivers/nfc/s3fwrn5/i2c.c b/drivers/nfc/s3fwrn5/i2c.c
index c61d8a308da4..3ed0adf6479b 100644
--- a/drivers/nfc/s3fwrn5/i2c.c
+++ b/drivers/nfc/s3fwrn5/i2c.c
@@ -125,7 +125,7 @@ static int s3fwrn5_i2c_write(void *phy_id, struct sk_buff *skb)
 	return 0;
 }
 
-static struct s3fwrn5_phy_ops i2c_phy_ops = {
+static const struct s3fwrn5_phy_ops i2c_phy_ops = {
 	.set_wake = s3fwrn5_i2c_set_wake,
 	.set_mode = s3fwrn5_i2c_set_mode,
 	.get_mode = s3fwrn5_i2c_get_mode,
diff --git a/drivers/nfc/s3fwrn5/s3fwrn5.h b/drivers/nfc/s3fwrn5/s3fwrn5.h
index 89210d4828b8..7d5e516036fb 100644
--- a/drivers/nfc/s3fwrn5/s3fwrn5.h
+++ b/drivers/nfc/s3fwrn5/s3fwrn5.h
@@ -44,7 +44,7 @@ struct s3fwrn5_info {
 	void *phy_id;
 	struct device *pdev;
 
-	struct s3fwrn5_phy_ops *phy_ops;
+	const struct s3fwrn5_phy_ops *phy_ops;
 	unsigned int max_payload;
 
 	struct s3fwrn5_fw_info fw_info;
@@ -90,7 +90,7 @@ static inline int s3fwrn5_write(struct s3fwrn5_info *info, struct sk_buff *skb)
 }
 
 int s3fwrn5_probe(struct nci_dev **ndev, void *phy_id, struct device *pdev,
-	struct s3fwrn5_phy_ops *phy_ops, unsigned int max_payload);
+	const struct s3fwrn5_phy_ops *phy_ops, unsigned int max_payload);
 void s3fwrn5_remove(struct nci_dev *ndev);
 
 int s3fwrn5_recv_frame(struct nci_dev *ndev, struct sk_buff *skb,
diff --git a/drivers/nfc/st-nci/Kconfig b/drivers/nfc/st-nci/Kconfig
index e7c6db9c5860..dc9b777d78f6 100644
--- a/drivers/nfc/st-nci/Kconfig
+++ b/drivers/nfc/st-nci/Kconfig
@@ -1,19 +1,14 @@
 config NFC_ST_NCI
-	tristate "STMicroelectronics ST NCI NFC driver"
-	depends on NFC_NCI
-	default n
+	tristate
 	---help---
 	  STMicroelectronics NFC NCI chips core driver. It implements the chipset
 	  NCI logic and hooks into the NFC kernel APIs. Physical layers will
 	  register against it.
 
-	  To compile this driver as a module, choose m here. The module will
-	  be called st-nci.
-	  Say N if unsure.
-
 config NFC_ST_NCI_I2C
-	tristate "NFC ST NCI i2c support"
-	depends on NFC_ST_NCI && I2C
+	tristate "STMicroelectronics ST NCI NFC driver (I2C)"
+	depends on NFC_NCI && I2C
+	select NFC_ST_NCI
 	---help---
 	  This module adds support for an I2C interface to the
 	  STMicroelectronics NFC NCI chips familly.
@@ -23,8 +18,9 @@ config NFC_ST_NCI_I2C
 	  Say N if unsure.
 
 config NFC_ST_NCI_SPI
-	tristate "NFC ST NCI spi support"
-	depends on NFC_ST_NCI && SPI
+	tristate "STMicroelectronics ST NCI NFC driver (SPI)"
+	depends on NFC_NCI && SPI
+	select NFC_ST_NCI
 	---help---
 	  This module adds support for an SPI interface to the
 	  STMicroelectronics NFC NCI chips familly.
diff --git a/drivers/nfc/st-nci/i2c.c b/drivers/nfc/st-nci/i2c.c
index 15e3ce2d274c..8a56b5c6e4c4 100644
--- a/drivers/nfc/st-nci/i2c.c
+++ b/drivers/nfc/st-nci/i2c.c
@@ -20,8 +20,10 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
+#include <linux/acpi.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
@@ -40,11 +42,7 @@
 
 #define ST_NCI_I2C_DRIVER_NAME "st_nci_i2c"
 
-static struct i2c_device_id st_nci_i2c_id_table[] = {
-	{ST_NCI_DRIVER_NAME, 0},
-	{}
-};
-MODULE_DEVICE_TABLE(i2c, st_nci_i2c_id_table);
+#define ST_NCI_GPIO_NAME_RESET "clf_reset"
 
 struct st_nci_i2c_phy {
 	struct i2c_client *i2c_dev;
@@ -210,7 +208,43 @@ static struct nfc_phy_ops i2c_phy_ops = {
 	.disable = st_nci_i2c_disable,
 };
 
-#ifdef CONFIG_OF
+static int st_nci_i2c_acpi_request_resources(struct i2c_client *client)
+{
+	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
+	const struct acpi_device_id *id;
+	struct gpio_desc *gpiod_reset;
+	struct device *dev;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* Match the struct device against a given list of ACPI IDs */
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return -ENODEV;
+
+	/* Get RESET GPIO from ACPI */
+	gpiod_reset = devm_gpiod_get_index(dev, ST_NCI_GPIO_NAME_RESET, 1,
+					   GPIOD_OUT_HIGH);
+	if (IS_ERR(gpiod_reset)) {
+		nfc_err(dev, "Unable to get RESET GPIO\n");
+		return -ENODEV;
+	}
+
+	phy->gpio_reset = desc_to_gpio(gpiod_reset);
+
+	phy->irq_polarity = irq_get_trigger_type(client->irq);
+
+	phy->se_status.is_ese_present =
+				device_property_present(dev, "ese-present");
+	phy->se_status.is_uicc_present =
+				device_property_present(dev, "uicc-present");
+
+	return 0;
+}
+
 static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 {
 	struct st_nci_i2c_phy *phy = i2c_get_clientdata(client);
@@ -232,7 +266,7 @@ static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 
 	/* GPIO request and configuration */
 	r = devm_gpio_request_one(&client->dev, gpio,
-				GPIOF_OUT_INIT_HIGH, "clf_reset");
+				GPIOF_OUT_INIT_HIGH, ST_NCI_GPIO_NAME_RESET);
 	if (r) {
 		nfc_err(&client->dev, "Failed to request reset pin\n");
 		return r;
@@ -248,12 +282,6 @@ static int st_nci_i2c_of_request_resources(struct i2c_client *client)
 
 	return 0;
 }
-#else
-static int st_nci_i2c_of_request_resources(struct i2c_client *client)
-{
-	return -ENODEV;
-}
-#endif
 
 static int st_nci_i2c_request_resources(struct i2c_client *client)
 {
@@ -272,7 +300,8 @@ static int st_nci_i2c_request_resources(struct i2c_client *client)
 	phy->irq_polarity = pdata->irq_polarity;
 
 	r = devm_gpio_request_one(&client->dev,
-			phy->gpio_reset, GPIOF_OUT_INIT_HIGH, "clf_reset");
+			phy->gpio_reset, GPIOF_OUT_INIT_HIGH,
+			ST_NCI_GPIO_NAME_RESET);
 	if (r) {
 		pr_err("%s : reset gpio_request failed\n", __FILE__);
 		return r;
@@ -322,6 +351,12 @@ static int st_nci_i2c_probe(struct i2c_client *client,
 				"Cannot get platform resources\n");
 			return r;
 		}
+	} else if (ACPI_HANDLE(&client->dev)) {
+		r = st_nci_i2c_acpi_request_resources(client);
+		if (r) {
+			nfc_err(&client->dev, "Cannot get ACPI data\n");
+			return r;
+		}
 	} else {
 		nfc_err(&client->dev,
 			"st_nci platform resources not available\n");
@@ -358,7 +393,19 @@ static int st_nci_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_OF
+static struct i2c_device_id st_nci_i2c_id_table[] = {
+	{ST_NCI_DRIVER_NAME, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, st_nci_i2c_id_table);
+
+static const struct acpi_device_id st_nci_i2c_acpi_match[] = {
+	{"SMO2101"},
+	{"SMO2102"},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, st_nci_i2c_acpi_match);
+
 static const struct of_device_id of_st_nci_i2c_match[] = {
 	{ .compatible = "st,st21nfcb-i2c", },
 	{ .compatible = "st,st21nfcb_i2c", },
@@ -366,19 +413,18 @@ static const struct of_device_id of_st_nci_i2c_match[] = {
 	{}
 };
 MODULE_DEVICE_TABLE(of, of_st_nci_i2c_match);
-#endif
 
 static struct i2c_driver st_nci_i2c_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = ST_NCI_I2C_DRIVER_NAME,
 		.of_match_table = of_match_ptr(of_st_nci_i2c_match),
+		.acpi_match_table = ACPI_PTR(st_nci_i2c_acpi_match),
 	},
 	.probe = st_nci_i2c_probe,
 	.id_table = st_nci_i2c_id_table,
 	.remove = st_nci_i2c_remove,
 };
-
 module_i2c_driver(st_nci_i2c_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/nfc/st-nci/ndlc.c b/drivers/nfc/st-nci/ndlc.c
index 0884b11001ef..50880d747b02 100644
--- a/drivers/nfc/st-nci/ndlc.c
+++ b/drivers/nfc/st-nci/ndlc.c
@@ -20,7 +20,6 @@
 #include <net/nfc/nci_core.h>
 
 #include "st-nci.h"
-#include "ndlc.h"
 
 #define NDLC_TIMER_T1		100
 #define NDLC_TIMER_T1_WAIT	400
diff --git a/drivers/nfc/st-nci/se.c b/drivers/nfc/st-nci/se.c
index dbab722a0654..a53e5df803eb 100644
--- a/drivers/nfc/st-nci/se.c
+++ b/drivers/nfc/st-nci/se.c
@@ -331,7 +331,7 @@ static int st_nci_hci_connectivity_event_received(struct nci_dev *ndev,
 
 	switch (event) {
 	case ST_NCI_EVT_CONNECTIVITY:
-
+		r = nfc_se_connectivity(ndev->nfc_dev, host);
 	break;
 	case ST_NCI_EVT_TRANSACTION:
 		/* According to specification etsi 102 622
@@ -392,7 +392,6 @@ void st_nci_hci_event_received(struct nci_dev *ndev, u8 pipe,
 }
 EXPORT_SYMBOL_GPL(st_nci_hci_event_received);
 
-
 void st_nci_hci_cmd_received(struct nci_dev *ndev, u8 pipe, u8 cmd,
 			       struct sk_buff *skb)
 {
diff --git a/drivers/nfc/st-nci/spi.c b/drivers/nfc/st-nci/spi.c
index d6519bb9dba5..821dfa950fa8 100644
--- a/drivers/nfc/st-nci/spi.c
+++ b/drivers/nfc/st-nci/spi.c
@@ -20,8 +20,10 @@
 #include <linux/module.h>
 #include <linux/spi/spi.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
+#include <linux/acpi.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
@@ -34,18 +36,14 @@
 
 /* ndlc header */
 #define ST_NCI_FRAME_HEADROOM	1
-#define ST_NCI_FRAME_TAILROOM 0
+#define ST_NCI_FRAME_TAILROOM	0
 
 #define ST_NCI_SPI_MIN_SIZE 4   /* PCB(1) + NCI Packet header(3) */
 #define ST_NCI_SPI_MAX_SIZE 250 /* req 4.2.1 */
 
 #define ST_NCI_SPI_DRIVER_NAME "st_nci_spi"
 
-static struct spi_device_id st_nci_spi_id_table[] = {
-	{ST_NCI_SPI_DRIVER_NAME, 0},
-	{}
-};
-MODULE_DEVICE_TABLE(spi, st_nci_spi_id_table);
+#define ST_NCI_GPIO_NAME_RESET "clf_reset"
 
 struct st_nci_spi_phy {
 	struct spi_device *spi_dev;
@@ -225,7 +223,43 @@ static struct nfc_phy_ops spi_phy_ops = {
 	.disable = st_nci_spi_disable,
 };
 
-#ifdef CONFIG_OF
+static int st_nci_spi_acpi_request_resources(struct spi_device *spi_dev)
+{
+	struct st_nci_spi_phy *phy = spi_get_drvdata(spi_dev);
+	const struct acpi_device_id *id;
+	struct gpio_desc *gpiod_reset;
+	struct device *dev;
+
+	if (!spi_dev)
+		return -EINVAL;
+
+	dev = &spi_dev->dev;
+
+	/* Match the struct device against a given list of ACPI IDs */
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return -ENODEV;
+
+	/* Get RESET GPIO from ACPI */
+	gpiod_reset = devm_gpiod_get_index(dev, ST_NCI_GPIO_NAME_RESET, 1,
+					   GPIOD_OUT_HIGH);
+	if (IS_ERR(gpiod_reset)) {
+		nfc_err(dev, "Unable to get RESET GPIO\n");
+		return -ENODEV;
+	}
+
+	phy->gpio_reset = desc_to_gpio(gpiod_reset);
+
+	phy->irq_polarity = irq_get_trigger_type(spi_dev->irq);
+
+	phy->se_status.is_ese_present =
+				device_property_present(dev, "ese-present");
+	phy->se_status.is_uicc_present =
+				device_property_present(dev, "uicc-present");
+
+	return 0;
+}
+
 static int st_nci_spi_of_request_resources(struct spi_device *dev)
 {
 	struct st_nci_spi_phy *phy = spi_get_drvdata(dev);
@@ -247,7 +281,7 @@ static int st_nci_spi_of_request_resources(struct spi_device *dev)
 
 	/* GPIO request and configuration */
 	r = devm_gpio_request_one(&dev->dev, gpio,
-				GPIOF_OUT_INIT_HIGH, "clf_reset");
+				GPIOF_OUT_INIT_HIGH, ST_NCI_GPIO_NAME_RESET);
 	if (r) {
 		nfc_err(&dev->dev, "Failed to request reset pin\n");
 		return r;
@@ -263,12 +297,6 @@ static int st_nci_spi_of_request_resources(struct spi_device *dev)
 
 	return 0;
 }
-#else
-static int st_nci_spi_of_request_resources(struct spi_device *dev)
-{
-	return -ENODEV;
-}
-#endif
 
 static int st_nci_spi_request_resources(struct spi_device *dev)
 {
@@ -287,7 +315,8 @@ static int st_nci_spi_request_resources(struct spi_device *dev)
 	phy->irq_polarity = pdata->irq_polarity;
 
 	r = devm_gpio_request_one(&dev->dev,
-			phy->gpio_reset, GPIOF_OUT_INIT_HIGH, "clf_reset");
+			phy->gpio_reset, GPIOF_OUT_INIT_HIGH,
+			ST_NCI_GPIO_NAME_RESET);
 	if (r) {
 		pr_err("%s : reset gpio_request failed\n", __FILE__);
 		return r;
@@ -338,6 +367,12 @@ static int st_nci_spi_probe(struct spi_device *dev)
 				"Cannot get platform resources\n");
 			return r;
 		}
+	} else if (ACPI_HANDLE(&dev->dev)) {
+		r = st_nci_spi_acpi_request_resources(dev);
+		if (r) {
+			nfc_err(&dev->dev, "Cannot get ACPI data\n");
+			return r;
+		}
 	} else {
 		nfc_err(&dev->dev,
 			"st_nci platform resources not available\n");
@@ -374,24 +409,34 @@ static int st_nci_spi_remove(struct spi_device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
+static struct spi_device_id st_nci_spi_id_table[] = {
+	{ST_NCI_SPI_DRIVER_NAME, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(spi, st_nci_spi_id_table);
+
+static const struct acpi_device_id st_nci_spi_acpi_match[] = {
+	{"SMO2101", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, st_nci_spi_acpi_match);
+
 static const struct of_device_id of_st_nci_spi_match[] = {
 	{ .compatible = "st,st21nfcb-spi", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, of_st_nci_spi_match);
-#endif
 
 static struct spi_driver st_nci_spi_driver = {
 	.driver = {
 		.name = ST_NCI_SPI_DRIVER_NAME,
 		.of_match_table = of_match_ptr(of_st_nci_spi_match),
+		.acpi_match_table = ACPI_PTR(st_nci_spi_acpi_match),
 	},
 	.probe = st_nci_spi_probe,
 	.id_table = st_nci_spi_id_table,
 	.remove = st_nci_spi_remove,
 };
-
 module_spi_driver(st_nci_spi_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/nfc/st21nfca/Kconfig b/drivers/nfc/st21nfca/Kconfig
index ee459f066ade..cc3bd5658901 100644
--- a/drivers/nfc/st21nfca/Kconfig
+++ b/drivers/nfc/st21nfca/Kconfig
@@ -1,20 +1,15 @@
 config NFC_ST21NFCA
-	tristate "STMicroelectronics ST21NFCA NFC driver"
-	depends on NFC_HCI
+	tristate
 	select CRC_CCITT
-	default n
 	---help---
 	  STMicroelectronics ST21NFCA core driver. It implements the chipset
 	  HCI logic and hooks into the NFC kernel APIs. Physical layers will
 	  register against it.
 
-	  To compile this driver as a module, choose m here. The module will
-	  be called st21nfca.
-	  Say N if unsure.
-
 config NFC_ST21NFCA_I2C
-	tristate "NFC ST21NFCA i2c support"
-	depends on NFC_ST21NFCA && I2C && NFC_SHDLC
+	tristate "STMicroelectronics ST21NFCA NFC driver (I2C)"
+	depends on NFC_HCI && I2C && NFC_SHDLC
+	select NFC_ST21NFCA
 	---help---
 	  This module adds support for the STMicroelectronics st21nfca i2c interface.
 	  Select this if your platform is using the i2c bus.
diff --git a/drivers/nfc/st21nfca/i2c.c b/drivers/nfc/st21nfca/i2c.c
index a98da33e680a..1f44a151d206 100644
--- a/drivers/nfc/st21nfca/i2c.c
+++ b/drivers/nfc/st21nfca/i2c.c
@@ -21,8 +21,10 @@
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
+#include <linux/acpi.h>
 #include <linux/miscdevice.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
@@ -60,12 +62,7 @@
 
 #define ST21NFCA_HCI_I2C_DRIVER_NAME "st21nfca_hci_i2c"
 
-static struct i2c_device_id st21nfca_hci_i2c_id_table[] = {
-	{ST21NFCA_HCI_DRIVER_NAME, 0},
-	{}
-};
-
-MODULE_DEVICE_TABLE(i2c, st21nfca_hci_i2c_id_table);
+#define ST21NFCA_GPIO_NAME_EN "clf_enable"
 
 struct st21nfca_i2c_phy {
 	struct i2c_client *i2c_dev;
@@ -167,7 +164,6 @@ static void st21nfca_hci_i2c_disable(void *phy_id)
 {
 	struct st21nfca_i2c_phy *phy = phy_id;
 
-	pr_info("\n");
 	gpio_set_value(phy->gpio_ena, 0);
 
 	phy->powered = 0;
@@ -210,7 +206,6 @@ static int st21nfca_hci_i2c_write(void *phy_id, struct sk_buff *skb)
 
 	I2C_DUMP_SKB("st21nfca_hci_i2c_write", skb);
 
-
 	if (phy->hard_fault != 0)
 		return phy->hard_fault;
 
@@ -509,7 +504,41 @@ static struct nfc_phy_ops i2c_phy_ops = {
 	.disable = st21nfca_hci_i2c_disable,
 };
 
-#ifdef CONFIG_OF
+static int st21nfca_hci_i2c_acpi_request_resources(struct i2c_client *client)
+{
+	struct st21nfca_i2c_phy *phy = i2c_get_clientdata(client);
+	const struct acpi_device_id *id;
+	struct gpio_desc *gpiod_ena;
+	struct device *dev;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* Match the struct device against a given list of ACPI IDs */
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return -ENODEV;
+
+	/* Get EN GPIO from ACPI */
+	gpiod_ena = devm_gpiod_get_index(dev, ST21NFCA_GPIO_NAME_EN, 1,
+					 GPIOD_OUT_LOW);
+	if (!IS_ERR(gpiod_ena))
+		phy->gpio_ena = desc_to_gpio(gpiod_ena);
+
+	phy->gpio_ena = desc_to_gpio(gpiod_ena);
+
+	phy->irq_polarity = irq_get_trigger_type(client->irq);
+
+	phy->se_status.is_ese_present =
+				device_property_present(dev, "ese-present");
+	phy->se_status.is_uicc_present =
+				device_property_present(dev, "uicc-present");
+
+	return 0;
+}
+
 static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
 {
 	struct st21nfca_i2c_phy *phy = i2c_get_clientdata(client);
@@ -530,7 +559,7 @@ static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
 
 	/* GPIO request and configuration */
 	r = devm_gpio_request_one(&client->dev, gpio, GPIOF_OUT_INIT_HIGH,
-				  "clf_enable");
+				  ST21NFCA_GPIO_NAME_EN);
 	if (r) {
 		nfc_err(&client->dev, "Failed to request enable pin\n");
 		return r;
@@ -547,12 +576,6 @@ static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
 
 	return 0;
 }
-#else
-static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
-{
-	return -ENODEV;
-}
-#endif
 
 static int st21nfca_hci_i2c_request_resources(struct i2c_client *client)
 {
@@ -572,7 +595,8 @@ static int st21nfca_hci_i2c_request_resources(struct i2c_client *client)
 
 	if (phy->gpio_ena > 0) {
 		r = devm_gpio_request_one(&client->dev, phy->gpio_ena,
-					  GPIOF_OUT_INIT_HIGH, "clf_enable");
+					  GPIOF_OUT_INIT_HIGH,
+					  ST21NFCA_GPIO_NAME_EN);
 		if (r) {
 			pr_err("%s : ena gpio_request failed\n", __FILE__);
 			return r;
@@ -628,6 +652,12 @@ static int st21nfca_hci_i2c_probe(struct i2c_client *client,
 			nfc_err(&client->dev, "Cannot get platform resources\n");
 			return r;
 		}
+	} else if (ACPI_HANDLE(&client->dev)) {
+		r = st21nfca_hci_i2c_acpi_request_resources(client);
+		if (r) {
+			nfc_err(&client->dev, "Cannot get ACPI data\n");
+			return r;
+		}
 	} else {
 		nfc_err(&client->dev, "st21nfca platform resources not available\n");
 		return -ENODEV;
@@ -670,26 +700,36 @@ static int st21nfca_hci_i2c_remove(struct i2c_client *client)
 	return 0;
 }
 
-#ifdef CONFIG_OF
+static struct i2c_device_id st21nfca_hci_i2c_id_table[] = {
+	{ST21NFCA_HCI_DRIVER_NAME, 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, st21nfca_hci_i2c_id_table);
+
+static const struct acpi_device_id st21nfca_hci_i2c_acpi_match[] = {
+	{"SMO2100", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(acpi, st21nfca_hci_i2c_acpi_match);
+
 static const struct of_device_id of_st21nfca_i2c_match[] = {
 	{ .compatible = "st,st21nfca-i2c", },
 	{ .compatible = "st,st21nfca_i2c", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, of_st21nfca_i2c_match);
-#endif
 
 static struct i2c_driver st21nfca_hci_i2c_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = ST21NFCA_HCI_I2C_DRIVER_NAME,
 		.of_match_table = of_match_ptr(of_st21nfca_i2c_match),
+		.acpi_match_table = ACPI_PTR(st21nfca_hci_i2c_acpi_match),
 	},
 	.probe = st21nfca_hci_i2c_probe,
 	.id_table = st21nfca_hci_i2c_id_table,
 	.remove = st21nfca_hci_i2c_remove,
 };
-
 module_i2c_driver(st21nfca_hci_i2c_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/nfc/st21nfca/se.c b/drivers/nfc/st21nfca/se.c
index c79d99b24c96..bd56a16e4007 100644
--- a/drivers/nfc/st21nfca/se.c
+++ b/drivers/nfc/st21nfca/se.c
@@ -312,7 +312,8 @@ int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
 
 	switch (event) {
 	case ST21NFCA_EVT_CONNECTIVITY:
-		break;
+		r = nfc_se_connectivity(hdev->ndev, host);
+	break;
 	case ST21NFCA_EVT_TRANSACTION:
 		/*
 		 * According to specification etsi 102 622
@@ -342,7 +343,7 @@ int st21nfca_connectivity_event_received(struct nfc_hci_dev *hdev, u8 host,
 		       transaction->aid_len + 4, transaction->params_len);
 
 		r = nfc_se_transaction(hdev->ndev, host, transaction);
-		break;
+	break;
 	default:
 		nfc_err(&hdev->ndev->dev, "Unexpected event on connectivity gate\n");
 		return 1;
diff --git a/drivers/nfc/st95hf/Kconfig b/drivers/nfc/st95hf/Kconfig
new file mode 100644
index 000000000000..224f266fdcb6
--- /dev/null
+++ b/drivers/nfc/st95hf/Kconfig
@@ -0,0 +1,10 @@
+config NFC_ST95HF
+	tristate "ST95HF NFC Transceiver driver"
+	depends on SPI && NFC_DIGITAL
+	help
+	This enables the ST NFC driver for ST95HF NFC transceiver.
+	This makes use of SPI framework to communicate with transceiver
+	and registered with NFC digital core to support Linux NFC framework.
+
+	Say Y here to compile support for ST NFC transceiver ST95HF
+	linux driver into the kernel or say M to compile it as module.
diff --git a/drivers/nfc/st95hf/Makefile b/drivers/nfc/st95hf/Makefile
new file mode 100644
index 000000000000..00760b38ab7e
--- /dev/null
+++ b/drivers/nfc/st95hf/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for STMicroelectronics NFC transceiver ST95HF
+#
+
+obj-$(CONFIG_NFC_ST95HF)	+= st95hf.o
+st95hf-objs			:= spi.o core.o
diff --git a/drivers/nfc/st95hf/core.c b/drivers/nfc/st95hf/core.c
new file mode 100644
index 000000000000..c2840e412962
--- /dev/null
+++ b/drivers/nfc/st95hf/core.c
@@ -0,0 +1,1273 @@
+/*
+ * --------------------------------------------------------------------
+ * Driver for ST NFC Transceiver ST95HF
+ * --------------------------------------------------------------------
+ * Copyright (C) 2015 STMicroelectronics Pvt. Ltd. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/nfc.h>
+#include <linux/of_gpio.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/property.h>
+#include <linux/regulator/consumer.h>
+#include <linux/wait.h>
+#include <net/nfc/digital.h>
+#include <net/nfc/nfc.h>
+
+#include "spi.h"
+
+/* supported protocols */
+#define ST95HF_SUPPORTED_PROT		(NFC_PROTO_ISO14443_MASK | \
+					NFC_PROTO_ISO14443_B_MASK | \
+					NFC_PROTO_ISO15693_MASK)
+/* driver capabilities */
+#define ST95HF_CAPABILITIES		NFC_DIGITAL_DRV_CAPS_IN_CRC
+
+/* Command Send Interface */
+/* ST95HF_COMMAND_SEND CMD Ids */
+#define ECHO_CMD			0x55
+#define WRITE_REGISTER_CMD		0x9
+#define PROTOCOL_SELECT_CMD		0x2
+#define SEND_RECEIVE_CMD		0x4
+
+/* Select protocol codes */
+#define ISO15693_PROTOCOL_CODE		0x1
+#define ISO14443A_PROTOCOL_CODE		0x2
+#define ISO14443B_PROTOCOL_CODE		0x3
+
+/*
+ * head room len is 3
+ * 1 byte for control byte
+ * 1 byte for cmd
+ * 1 byte for size
+ */
+#define ST95HF_HEADROOM_LEN		3
+
+/*
+ * tailroom is 1 for ISO14443A
+ * and 0 for ISO14443B/ISO15693,
+ * hence the max value 1 should be
+ * taken.
+ */
+#define ST95HF_TAILROOM_LEN		1
+
+/* Command Response interface */
+#define MAX_RESPONSE_BUFFER_SIZE	280
+#define ECHORESPONSE			0x55
+#define ST95HF_ERR_MASK			0xF
+#define ST95HF_TIMEOUT_ERROR		0x87
+#define ST95HF_NFCA_CRC_ERR_MASK	0x20
+#define ST95HF_NFCB_CRC_ERR_MASK	0x01
+
+/* ST95HF transmission flag values */
+#define TRFLAG_NFCA_SHORT_FRAME		0x07
+#define TRFLAG_NFCA_STD_FRAME		0x08
+#define TRFLAG_NFCA_STD_FRAME_CRC	0x28
+
+/* Misc defs */
+#define HIGH				1
+#define LOW				0
+#define ISO14443A_RATS_REQ		0xE0
+#define RATS_TB1_PRESENT_MASK		0x20
+#define RATS_TA1_PRESENT_MASK		0x10
+#define TB1_FWI_MASK			0xF0
+#define WTX_REQ_FROM_TAG		0xF2
+
+#define MAX_CMD_LEN			0x7
+
+#define MAX_CMD_PARAMS			4
+struct cmd {
+	int cmd_len;
+	unsigned char cmd_id;
+	unsigned char no_cmd_params;
+	unsigned char cmd_params[MAX_CMD_PARAMS];
+	enum req_type req;
+};
+
+struct param_list {
+	int param_offset;
+	int new_param_val;
+};
+
+/*
+ * List of top-level cmds to be used internally by the driver.
+ * All these commands are build on top of ST95HF basic commands
+ * such as SEND_RECEIVE_CMD, PROTOCOL_SELECT_CMD, etc.
+ * These top level cmds are used internally while implementing various ops of
+ * digital layer/driver probe or extending the digital framework layer for
+ * features that are not yet implemented there, for example, WTX cmd handling.
+ */
+enum st95hf_cmd_list {
+	CMD_ECHO,
+	CMD_ISO14443A_CONFIG,
+	CMD_ISO14443A_DEMOGAIN,
+	CMD_ISO14443B_DEMOGAIN,
+	CMD_ISO14443A_PROTOCOL_SELECT,
+	CMD_ISO14443B_PROTOCOL_SELECT,
+	CMD_WTX_RESPONSE,
+	CMD_FIELD_OFF,
+	CMD_ISO15693_PROTOCOL_SELECT,
+};
+
+static const struct cmd cmd_array[] = {
+	[CMD_ECHO] = {
+		.cmd_len = 0x2,
+		.cmd_id = ECHO_CMD,
+		.no_cmd_params = 0,
+		.req = SYNC,
+	},
+	[CMD_ISO14443A_CONFIG] = {
+		.cmd_len = 0x7,
+		.cmd_id = WRITE_REGISTER_CMD,
+		.no_cmd_params = 0x4,
+		.cmd_params = {0x3A, 0x00, 0x5A, 0x04},
+		.req = SYNC,
+	},
+	[CMD_ISO14443A_DEMOGAIN] = {
+		.cmd_len = 0x7,
+		.cmd_id = WRITE_REGISTER_CMD,
+		.no_cmd_params = 0x4,
+		.cmd_params = {0x68, 0x01, 0x01, 0xDF},
+		.req = SYNC,
+	},
+	[CMD_ISO14443B_DEMOGAIN] = {
+		.cmd_len = 0x7,
+		.cmd_id = WRITE_REGISTER_CMD,
+		.no_cmd_params = 0x4,
+		.cmd_params = {0x68, 0x01, 0x01, 0x51},
+		.req = SYNC,
+	},
+	[CMD_ISO14443A_PROTOCOL_SELECT] = {
+		.cmd_len = 0x7,
+		.cmd_id = PROTOCOL_SELECT_CMD,
+		.no_cmd_params = 0x4,
+		.cmd_params = {ISO14443A_PROTOCOL_CODE, 0x00, 0x01, 0xA0},
+		.req = SYNC,
+	},
+	[CMD_ISO14443B_PROTOCOL_SELECT] = {
+		.cmd_len = 0x7,
+		.cmd_id = PROTOCOL_SELECT_CMD,
+		.no_cmd_params = 0x4,
+		.cmd_params = {ISO14443B_PROTOCOL_CODE, 0x01, 0x03, 0xFF},
+		.req = SYNC,
+	},
+	[CMD_WTX_RESPONSE] = {
+		.cmd_len = 0x6,
+		.cmd_id = SEND_RECEIVE_CMD,
+		.no_cmd_params = 0x3,
+		.cmd_params = {0xF2, 0x00, TRFLAG_NFCA_STD_FRAME_CRC},
+		.req = ASYNC,
+	},
+	[CMD_FIELD_OFF] = {
+		.cmd_len = 0x5,
+		.cmd_id = PROTOCOL_SELECT_CMD,
+		.no_cmd_params = 0x2,
+		.cmd_params = {0x0, 0x0},
+		.req = SYNC,
+	},
+	[CMD_ISO15693_PROTOCOL_SELECT] = {
+		.cmd_len = 0x5,
+		.cmd_id = PROTOCOL_SELECT_CMD,
+		.no_cmd_params = 0x2,
+		.cmd_params = {ISO15693_PROTOCOL_CODE, 0x0D},
+		.req = SYNC,
+	},
+};
+
+/* st95_digital_cmd_complete_arg stores client context */
+struct st95_digital_cmd_complete_arg {
+	struct sk_buff *skb_resp;
+	nfc_digital_cmd_complete_t complete_cb;
+	void *cb_usrarg;
+	bool rats;
+};
+
+/*
+ * structure containing ST95HF driver specific data.
+ * @spicontext: structure containing information required
+ *	for spi communication between st95hf and host.
+ * @ddev: nfc digital device object.
+ * @nfcdev: nfc device object.
+ * @enable_gpio: gpio used to enable st95hf transceiver.
+ * @complete_cb_arg: structure to store various context information
+ *	that is passed from nfc requesting thread to the threaded ISR.
+ * @st95hf_supply: regulator "consumer" for NFC device.
+ * @sendrcv_trflag: last byte of frame send by sendrecv command
+ *	of st95hf. This byte contains transmission flag info.
+ * @exchange_lock: semaphore used for signaling the st95hf_remove
+ *	function that the last outstanding async nfc request is finished.
+ * @rm_lock: mutex for ensuring safe access of nfc digital object
+ *	from threaded ISR. Usage of this mutex avoids any race between
+ *	deletion of the object from st95hf_remove() and its access from
+ *	the threaded ISR.
+ * @nfcdev_free: flag to have the state of nfc device object.
+ *	[alive | died]
+ * @current_protocol: current nfc protocol.
+ * @current_rf_tech: current rf technology.
+ * @fwi: frame waiting index, received in reply of RATS according to
+ *	digital protocol.
+ */
+struct st95hf_context {
+	struct st95hf_spi_context spicontext;
+	struct nfc_digital_dev *ddev;
+	struct nfc_dev *nfcdev;
+	unsigned int enable_gpio;
+	struct st95_digital_cmd_complete_arg complete_cb_arg;
+	struct regulator *st95hf_supply;
+	unsigned char sendrcv_trflag;
+	struct semaphore exchange_lock;
+	struct mutex rm_lock;
+	bool nfcdev_free;
+	u8 current_protocol;
+	u8 current_rf_tech;
+	int fwi;
+};
+
+/*
+ * st95hf_send_recv_cmd() is for sending commands to ST95HF
+ * that are described in the cmd_array[]. It can optionally
+ * receive the response if the cmd request is of type
+ * SYNC. For that to happen caller must pass true to recv_res.
+ * For ASYNC request, recv_res is ignored and the
+ * function will never try to receive the response on behalf
+ * of the caller.
+ */
+static int st95hf_send_recv_cmd(struct st95hf_context *st95context,
+				enum st95hf_cmd_list cmd,
+				int no_modif,
+				struct param_list *list_array,
+				bool recv_res)
+{
+	unsigned char spi_cmd_buffer[MAX_CMD_LEN];
+	int i, ret;
+	struct device *dev = &st95context->spicontext.spidev->dev;
+
+	if (cmd_array[cmd].cmd_len > MAX_CMD_LEN)
+		return -EINVAL;
+	if (cmd_array[cmd].no_cmd_params < no_modif)
+		return -EINVAL;
+	if (no_modif && !list_array)
+		return -EINVAL;
+
+	spi_cmd_buffer[0] = ST95HF_COMMAND_SEND;
+	spi_cmd_buffer[1] = cmd_array[cmd].cmd_id;
+	spi_cmd_buffer[2] = cmd_array[cmd].no_cmd_params;
+
+	memcpy(&spi_cmd_buffer[3], cmd_array[cmd].cmd_params,
+	       spi_cmd_buffer[2]);
+
+	for (i = 0; i < no_modif; i++) {
+		if (list_array[i].param_offset >= cmd_array[cmd].no_cmd_params)
+			return -EINVAL;
+		spi_cmd_buffer[3 + list_array[i].param_offset] =
+						list_array[i].new_param_val;
+	}
+
+	ret = st95hf_spi_send(&st95context->spicontext,
+			      spi_cmd_buffer,
+			      cmd_array[cmd].cmd_len,
+			      cmd_array[cmd].req);
+	if (ret) {
+		dev_err(dev, "st95hf_spi_send failed with error %d\n", ret);
+		return ret;
+	}
+
+	if (cmd_array[cmd].req == SYNC && recv_res) {
+		unsigned char st95hf_response_arr[2];
+
+		ret = st95hf_spi_recv_response(&st95context->spicontext,
+					       st95hf_response_arr);
+		if (ret < 0) {
+			dev_err(dev, "spi error from st95hf_spi_recv_response(), err = 0x%x\n",
+				ret);
+			return ret;
+		}
+
+		if (st95hf_response_arr[0]) {
+			dev_err(dev, "st95hf error from st95hf_spi_recv_response(), err = 0x%x\n",
+				st95hf_response_arr[0]);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int st95hf_echo_command(struct st95hf_context *st95context)
+{
+	int result = 0;
+	unsigned char echo_response;
+
+	result = st95hf_send_recv_cmd(st95context, CMD_ECHO, 0, NULL, false);
+	if (result)
+		return result;
+
+	/* If control reached here, response can be taken */
+	result = st95hf_spi_recv_echo_res(&st95context->spicontext,
+					  &echo_response);
+	if (result) {
+		dev_err(&st95context->spicontext.spidev->dev,
+			"err: echo response receieve error = 0x%x\n", result);
+		return result;
+	}
+
+	if (echo_response == ECHORESPONSE)
+		return 0;
+
+	dev_err(&st95context->spicontext.spidev->dev, "err: echo res is 0x%x\n",
+		echo_response);
+
+	return -EIO;
+}
+
+static int secondary_configuration_type4a(struct st95hf_context *stcontext)
+{
+	int result = 0;
+	struct device *dev = &stcontext->nfcdev->dev;
+
+	/* 14443A config setting after select protocol */
+	result = st95hf_send_recv_cmd(stcontext,
+				      CMD_ISO14443A_CONFIG,
+				      0,
+				      NULL,
+				      true);
+	if (result) {
+		dev_err(dev, "type a config cmd, err = 0x%x\n", result);
+		return result;
+	}
+
+	/* 14443A demo gain setting */
+	result = st95hf_send_recv_cmd(stcontext,
+				      CMD_ISO14443A_DEMOGAIN,
+				      0,
+				      NULL,
+				      true);
+	if (result)
+		dev_err(dev, "type a demogain cmd, err = 0x%x\n", result);
+
+	return result;
+}
+
+static int secondary_configuration_type4b(struct st95hf_context *stcontext)
+{
+	int result = 0;
+	struct device *dev = &stcontext->nfcdev->dev;
+
+	result = st95hf_send_recv_cmd(stcontext,
+				      CMD_ISO14443B_DEMOGAIN,
+				      0,
+				      NULL,
+				      true);
+	if (result)
+		dev_err(dev, "type b demogain cmd, err = 0x%x\n", result);
+
+	return result;
+}
+
+static int st95hf_select_protocol(struct st95hf_context *stcontext, int type)
+{
+	int result = 0;
+	struct device *dev;
+
+	dev = &stcontext->nfcdev->dev;
+
+	switch (type) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		stcontext->current_rf_tech = NFC_DIGITAL_RF_TECH_106A;
+		result = st95hf_send_recv_cmd(stcontext,
+					      CMD_ISO14443A_PROTOCOL_SELECT,
+					      0,
+					      NULL,
+					      true);
+		if (result) {
+			dev_err(dev, "protocol sel, err = 0x%x\n",
+				result);
+			return result;
+		}
+
+		/* secondary config. for 14443Type 4A after protocol select */
+		result = secondary_configuration_type4a(stcontext);
+		if (result) {
+			dev_err(dev, "type a secondary config, err = 0x%x\n",
+				result);
+			return result;
+		}
+		break;
+	case NFC_DIGITAL_RF_TECH_106B:
+		stcontext->current_rf_tech = NFC_DIGITAL_RF_TECH_106B;
+		result = st95hf_send_recv_cmd(stcontext,
+					      CMD_ISO14443B_PROTOCOL_SELECT,
+					      0,
+					      NULL,
+					      true);
+		if (result) {
+			dev_err(dev, "protocol sel send, err = 0x%x\n",
+				result);
+			return result;
+		}
+
+		/*
+		 * delay of 5-6 ms is required after select protocol
+		 * command in case of ISO14443 Type B
+		 */
+		usleep_range(50000, 60000);
+
+		/* secondary config. for 14443Type 4B after protocol select */
+		result = secondary_configuration_type4b(stcontext);
+		if (result) {
+			dev_err(dev, "type b secondary config, err = 0x%x\n",
+				result);
+			return result;
+		}
+		break;
+	case NFC_DIGITAL_RF_TECH_ISO15693:
+		stcontext->current_rf_tech = NFC_DIGITAL_RF_TECH_ISO15693;
+		result = st95hf_send_recv_cmd(stcontext,
+					      CMD_ISO15693_PROTOCOL_SELECT,
+					      0,
+					      NULL,
+					      true);
+		if (result) {
+			dev_err(dev, "protocol sel send, err = 0x%x\n",
+				result);
+			return result;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void st95hf_send_st95enable_negativepulse(struct st95hf_context *st95con)
+{
+	/* First make irq_in pin high */
+	gpio_set_value(st95con->enable_gpio, HIGH);
+
+	/* wait for 1 milisecond */
+	usleep_range(1000, 2000);
+
+	/* Make irq_in pin low */
+	gpio_set_value(st95con->enable_gpio, LOW);
+
+	/* wait for minimum interrupt pulse to make st95 active */
+	usleep_range(1000, 2000);
+
+	/* At end make it high */
+	gpio_set_value(st95con->enable_gpio, HIGH);
+}
+
+/*
+ * Send a reset sequence over SPI bus (Reset command + wait 3ms +
+ * negative pulse on st95hf enable gpio
+ */
+static int st95hf_send_spi_reset_sequence(struct st95hf_context *st95context)
+{
+	int result = 0;
+	unsigned char reset_cmd = ST95HF_COMMAND_RESET;
+
+	result = st95hf_spi_send(&st95context->spicontext,
+				 &reset_cmd,
+				 ST95HF_RESET_CMD_LEN,
+				 ASYNC);
+	if (result) {
+		dev_err(&st95context->spicontext.spidev->dev,
+			"spi reset sequence cmd error = %d", result);
+		return result;
+	}
+
+	/* wait for 3 milisecond to complete the controller reset process */
+	usleep_range(3000, 4000);
+
+	/* send negative pulse to make st95hf active */
+	st95hf_send_st95enable_negativepulse(st95context);
+
+	/* wait for 10 milisecond : HFO setup time */
+	usleep_range(10000, 20000);
+
+	return result;
+}
+
+static int st95hf_por_sequence(struct st95hf_context *st95context)
+{
+	int nth_attempt = 1;
+	int result;
+
+	st95hf_send_st95enable_negativepulse(st95context);
+
+	usleep_range(5000, 6000);
+	do {
+		/* send an ECHO command and checks ST95HF response */
+		result = st95hf_echo_command(st95context);
+
+		dev_dbg(&st95context->spicontext.spidev->dev,
+			"response from echo function = 0x%x, attempt = %d\n",
+			result, nth_attempt);
+
+		if (!result)
+			return 0;
+
+		/* send an pulse on IRQ in case of the chip is on sleep state */
+		if (nth_attempt == 2)
+			st95hf_send_st95enable_negativepulse(st95context);
+		else
+			st95hf_send_spi_reset_sequence(st95context);
+
+		/* delay of 50 milisecond */
+		usleep_range(50000, 51000);
+	} while (nth_attempt++ < 3);
+
+	return -ETIMEDOUT;
+}
+
+static int iso14443_config_fdt(struct st95hf_context *st95context, int wtxm)
+{
+	int result = 0;
+	struct device *dev = &st95context->spicontext.spidev->dev;
+	struct nfc_digital_dev *nfcddev = st95context->ddev;
+	unsigned char pp_typeb;
+	struct param_list new_params[2];
+
+	pp_typeb = cmd_array[CMD_ISO14443B_PROTOCOL_SELECT].cmd_params[2];
+
+	if (nfcddev->curr_protocol == NFC_PROTO_ISO14443 &&
+	    st95context->fwi < 4)
+		st95context->fwi = 4;
+
+	new_params[0].param_offset = 2;
+	if (nfcddev->curr_protocol == NFC_PROTO_ISO14443)
+		new_params[0].new_param_val = st95context->fwi;
+	else if (nfcddev->curr_protocol == NFC_PROTO_ISO14443_B)
+		new_params[0].new_param_val = pp_typeb;
+
+	new_params[1].param_offset = 3;
+	new_params[1].new_param_val = wtxm;
+
+	switch (nfcddev->curr_protocol) {
+	case NFC_PROTO_ISO14443:
+		result = st95hf_send_recv_cmd(st95context,
+					      CMD_ISO14443A_PROTOCOL_SELECT,
+					      2,
+					      new_params,
+					      true);
+		if (result) {
+			dev_err(dev, "WTX type a sel proto, err = 0x%x\n",
+				result);
+			return result;
+		}
+
+		/* secondary config. for 14443Type 4A after protocol select */
+		result = secondary_configuration_type4a(st95context);
+		if (result) {
+			dev_err(dev, "WTX type a second. config, err = 0x%x\n",
+				result);
+			return result;
+		}
+		break;
+	case NFC_PROTO_ISO14443_B:
+		result = st95hf_send_recv_cmd(st95context,
+					      CMD_ISO14443B_PROTOCOL_SELECT,
+					      2,
+					      new_params,
+					      true);
+		if (result) {
+			dev_err(dev, "WTX type b sel proto, err = 0x%x\n",
+				result);
+			return result;
+		}
+
+		/* secondary config. for 14443Type 4B after protocol select */
+		result = secondary_configuration_type4b(st95context);
+		if (result) {
+			dev_err(dev, "WTX type b second. config, err = 0x%x\n",
+				result);
+			return result;
+		}
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int st95hf_handle_wtx(struct st95hf_context *stcontext,
+			     bool new_wtx,
+			     int wtx_val)
+{
+	int result = 0;
+	unsigned char val_mm = 0;
+	struct param_list new_params[1];
+	struct nfc_digital_dev *nfcddev = stcontext->ddev;
+	struct device *dev = &stcontext->nfcdev->dev;
+
+	if (new_wtx) {
+		result = iso14443_config_fdt(stcontext, wtx_val & 0x3f);
+		if (result) {
+			dev_err(dev, "Config. setting error on WTX req, err = 0x%x\n",
+				result);
+			return result;
+		}
+
+		/* Send response of wtx with ASYNC as no response expected */
+		new_params[0].param_offset = 1;
+		new_params[0].new_param_val = wtx_val;
+
+		result = st95hf_send_recv_cmd(stcontext,
+					      CMD_WTX_RESPONSE,
+					      1,
+					      new_params,
+					      false);
+		if (result)
+			dev_err(dev, "WTX response send, err = 0x%x\n", result);
+		return result;
+	}
+
+	/* if no new wtx, cofigure with default values */
+	if (nfcddev->curr_protocol == NFC_PROTO_ISO14443)
+		val_mm = cmd_array[CMD_ISO14443A_PROTOCOL_SELECT].cmd_params[3];
+	else if (nfcddev->curr_protocol == NFC_PROTO_ISO14443_B)
+		val_mm = cmd_array[CMD_ISO14443B_PROTOCOL_SELECT].cmd_params[3];
+
+	result = iso14443_config_fdt(stcontext, val_mm);
+	if (result)
+		dev_err(dev, "Default config. setting error after WTX processing, err = 0x%x\n",
+			result);
+
+	return result;
+}
+
+static int st95hf_error_handling(struct st95hf_context *stcontext,
+				 struct sk_buff *skb_resp,
+				 int res_len)
+{
+	int result = 0;
+	unsigned char error_byte;
+	struct device *dev = &stcontext->nfcdev->dev;
+
+	/* First check ST95HF specific error */
+	if (skb_resp->data[0] & ST95HF_ERR_MASK) {
+		if (skb_resp->data[0] == ST95HF_TIMEOUT_ERROR)
+			result = -ETIMEDOUT;
+		else
+			result = -EIO;
+	return  result;
+	}
+
+	/* Check for CRC err only if CRC is present in the tag response */
+	switch (stcontext->current_rf_tech) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		if (stcontext->sendrcv_trflag == TRFLAG_NFCA_STD_FRAME_CRC) {
+			error_byte = skb_resp->data[res_len - 3];
+			if (error_byte & ST95HF_NFCA_CRC_ERR_MASK) {
+				/* CRC error occurred */
+				dev_err(dev, "CRC error, byte received = 0x%x\n",
+					error_byte);
+				result = -EIO;
+			}
+		}
+		break;
+	case NFC_DIGITAL_RF_TECH_106B:
+	case NFC_DIGITAL_RF_TECH_ISO15693:
+		error_byte = skb_resp->data[res_len - 1];
+		if (error_byte & ST95HF_NFCB_CRC_ERR_MASK) {
+			/* CRC error occurred */
+			dev_err(dev, "CRC error, byte received = 0x%x\n",
+				error_byte);
+			result = -EIO;
+		}
+		break;
+	}
+
+	return result;
+}
+
+static int st95hf_response_handler(struct st95hf_context *stcontext,
+				   struct sk_buff *skb_resp,
+				   int res_len)
+{
+	int result = 0;
+	int skb_len;
+	unsigned char val_mm;
+	struct nfc_digital_dev *nfcddev = stcontext->ddev;
+	struct device *dev = &stcontext->nfcdev->dev;
+	struct st95_digital_cmd_complete_arg *cb_arg;
+
+	cb_arg = &stcontext->complete_cb_arg;
+
+	/* Process the response */
+	skb_put(skb_resp, res_len);
+
+	/* Remove st95 header */
+	skb_pull(skb_resp, 2);
+
+	skb_len = skb_resp->len;
+
+	/* check if it is case of RATS request reply & FWI is present */
+	if (nfcddev->curr_protocol == NFC_PROTO_ISO14443 && cb_arg->rats &&
+	    (skb_resp->data[1] & RATS_TB1_PRESENT_MASK)) {
+		if (skb_resp->data[1] & RATS_TA1_PRESENT_MASK)
+			stcontext->fwi =
+				(skb_resp->data[3] & TB1_FWI_MASK) >> 4;
+		else
+			stcontext->fwi =
+				(skb_resp->data[2] & TB1_FWI_MASK) >> 4;
+
+		val_mm = cmd_array[CMD_ISO14443A_PROTOCOL_SELECT].cmd_params[3];
+
+		result = iso14443_config_fdt(stcontext, val_mm);
+		if (result) {
+			dev_err(dev, "error in config_fdt to handle fwi of ATS, error=%d\n",
+				result);
+			return result;
+		}
+	}
+	cb_arg->rats = false;
+
+	/* Remove CRC bytes only if received frames data has an eod (CRC) */
+	switch (stcontext->current_rf_tech) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		if (stcontext->sendrcv_trflag == TRFLAG_NFCA_STD_FRAME_CRC)
+			skb_trim(skb_resp, (skb_len - 5));
+		else
+			skb_trim(skb_resp, (skb_len - 3));
+		break;
+	case NFC_DIGITAL_RF_TECH_106B:
+	case NFC_DIGITAL_RF_TECH_ISO15693:
+		skb_trim(skb_resp, (skb_len - 3));
+		break;
+	}
+
+	return result;
+}
+
+static irqreturn_t st95hf_irq_handler(int irq, void  *st95hfcontext)
+{
+	struct st95hf_context *stcontext  =
+		(struct st95hf_context *)st95hfcontext;
+
+	if (stcontext->spicontext.req_issync) {
+		complete(&stcontext->spicontext.done);
+		stcontext->spicontext.req_issync = false;
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_WAKE_THREAD;
+}
+
+static irqreturn_t st95hf_irq_thread_handler(int irq, void  *st95hfcontext)
+{
+	int result = 0;
+	int res_len;
+	static bool wtx;
+	struct device *dev;
+	struct device *spidevice;
+	struct nfc_digital_dev *nfcddev;
+	struct sk_buff *skb_resp;
+	struct st95hf_context *stcontext  =
+		(struct st95hf_context *)st95hfcontext;
+	struct st95_digital_cmd_complete_arg *cb_arg;
+
+	spidevice = &stcontext->spicontext.spidev->dev;
+
+	/*
+	 * check semaphore, if not down() already, then we don't
+	 * know in which context the ISR is called and surely it
+	 * will be a bug. Note that down() of the semaphore is done
+	 * in the corresponding st95hf_in_send_cmd() and then
+	 * only this ISR should be called. ISR will up() the
+	 * semaphore before leaving. Hence when the ISR is called
+	 * the correct behaviour is down_trylock() should always
+	 * return 1 (indicating semaphore cant be taken and hence no
+	 * change in semaphore count).
+	 * If not, then we up() the semaphore and crash on
+	 * a BUG() !
+	 */
+	if (!down_trylock(&stcontext->exchange_lock)) {
+		up(&stcontext->exchange_lock);
+		WARN(1, "unknown context in ST95HF ISR");
+		return IRQ_NONE;
+	}
+
+	cb_arg = &stcontext->complete_cb_arg;
+	skb_resp = cb_arg->skb_resp;
+
+	mutex_lock(&stcontext->rm_lock);
+	res_len = st95hf_spi_recv_response(&stcontext->spicontext,
+					   skb_resp->data);
+	if (res_len < 0) {
+		dev_err(spidevice, "TISR spi response err = 0x%x\n", res_len);
+		result = res_len;
+		goto end;
+	}
+
+	/* if stcontext->nfcdev_free is true, it means remove already ran */
+	if (stcontext->nfcdev_free) {
+		result = -ENODEV;
+		goto end;
+	}
+
+	dev = &stcontext->nfcdev->dev;
+	nfcddev = stcontext->ddev;
+	if (skb_resp->data[2] == WTX_REQ_FROM_TAG) {
+		/* Request for new FWT from tag */
+		result = st95hf_handle_wtx(stcontext, true, skb_resp->data[3]);
+		if (result)
+			goto end;
+
+		wtx = true;
+		mutex_unlock(&stcontext->rm_lock);
+		return IRQ_HANDLED;
+	}
+
+	result = st95hf_error_handling(stcontext, skb_resp, res_len);
+	if (result)
+		goto end;
+
+	result = st95hf_response_handler(stcontext, skb_resp, res_len);
+	if (result)
+		goto end;
+
+	/*
+	 * If select protocol is done on wtx req. do select protocol
+	 * again with default values
+	 */
+	if (wtx) {
+		wtx = false;
+		result = st95hf_handle_wtx(stcontext, false, 0);
+		if (result)
+			goto end;
+	}
+
+	/* call digital layer callback */
+	cb_arg->complete_cb(stcontext->ddev, cb_arg->cb_usrarg, skb_resp);
+
+	/* up the semaphore before returning */
+	up(&stcontext->exchange_lock);
+	mutex_unlock(&stcontext->rm_lock);
+
+	return IRQ_HANDLED;
+
+end:
+	kfree_skb(skb_resp);
+	wtx = false;
+	cb_arg->rats = false;
+	skb_resp = ERR_PTR(result);
+	/* call of callback with error */
+	cb_arg->complete_cb(stcontext->ddev, cb_arg->cb_usrarg, skb_resp);
+	/* up the semaphore before returning */
+	up(&stcontext->exchange_lock);
+	mutex_unlock(&stcontext->rm_lock);
+	return IRQ_HANDLED;
+}
+
+/* NFC ops functions definition */
+static int st95hf_in_configure_hw(struct nfc_digital_dev *ddev,
+				  int type,
+				  int param)
+{
+	struct st95hf_context *stcontext = nfc_digital_get_drvdata(ddev);
+
+	if (type == NFC_DIGITAL_CONFIG_RF_TECH)
+		return st95hf_select_protocol(stcontext, param);
+
+	if (type == NFC_DIGITAL_CONFIG_FRAMING) {
+		switch (param) {
+		case NFC_DIGITAL_FRAMING_NFCA_SHORT:
+			stcontext->sendrcv_trflag = TRFLAG_NFCA_SHORT_FRAME;
+			break;
+		case NFC_DIGITAL_FRAMING_NFCA_STANDARD:
+			stcontext->sendrcv_trflag = TRFLAG_NFCA_STD_FRAME;
+			break;
+		case NFC_DIGITAL_FRAMING_NFCA_T4T:
+		case NFC_DIGITAL_FRAMING_NFCA_NFC_DEP:
+		case NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A:
+			stcontext->sendrcv_trflag = TRFLAG_NFCA_STD_FRAME_CRC;
+			break;
+		case NFC_DIGITAL_FRAMING_NFCB:
+		case NFC_DIGITAL_FRAMING_ISO15693_INVENTORY:
+		case NFC_DIGITAL_FRAMING_ISO15693_T5T:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+static int rf_off(struct st95hf_context *stcontext)
+{
+	int rc;
+	struct device *dev;
+
+	dev = &stcontext->nfcdev->dev;
+
+	rc = st95hf_send_recv_cmd(stcontext, CMD_FIELD_OFF, 0, NULL, true);
+	if (rc)
+		dev_err(dev, "protocol sel send field off, err = 0x%x\n", rc);
+
+	return rc;
+}
+
+static int st95hf_in_send_cmd(struct nfc_digital_dev *ddev,
+			      struct sk_buff *skb,
+			      u16 timeout,
+			      nfc_digital_cmd_complete_t cb,
+			      void *arg)
+{
+	struct st95hf_context *stcontext = nfc_digital_get_drvdata(ddev);
+	int rc;
+	struct sk_buff *skb_resp;
+	int len_data_to_tag = 0;
+
+	skb_resp = nfc_alloc_recv_skb(MAX_RESPONSE_BUFFER_SIZE, GFP_KERNEL);
+	if (!skb_resp) {
+		rc = -ENOMEM;
+		goto error;
+	}
+
+	switch (stcontext->current_rf_tech) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		len_data_to_tag = skb->len + 1;
+		*skb_put(skb, 1) = stcontext->sendrcv_trflag;
+		break;
+	case NFC_DIGITAL_RF_TECH_106B:
+	case NFC_DIGITAL_RF_TECH_ISO15693:
+		len_data_to_tag = skb->len;
+		break;
+	default:
+		rc = -EINVAL;
+		goto free_skb_resp;
+	}
+
+	skb_push(skb, 3);
+	skb->data[0] = ST95HF_COMMAND_SEND;
+	skb->data[1] = SEND_RECEIVE_CMD;
+	skb->data[2] = len_data_to_tag;
+
+	stcontext->complete_cb_arg.skb_resp = skb_resp;
+	stcontext->complete_cb_arg.cb_usrarg = arg;
+	stcontext->complete_cb_arg.complete_cb = cb;
+
+	if ((skb->data[3] == ISO14443A_RATS_REQ) &&
+	    ddev->curr_protocol == NFC_PROTO_ISO14443)
+		stcontext->complete_cb_arg.rats = true;
+
+	/*
+	 * down the semaphore to indicate to remove func that an
+	 * ISR is pending, note that it will not block here in any case.
+	 * If found blocked, it is a BUG!
+	 */
+	rc = down_killable(&stcontext->exchange_lock);
+	if (rc) {
+		WARN(1, "Semaphore is not found up in st95hf_in_send_cmd\n");
+		return rc;
+	}
+
+	rc = st95hf_spi_send(&stcontext->spicontext, skb->data,
+			     skb->len,
+			     ASYNC);
+	if (rc) {
+		dev_err(&stcontext->nfcdev->dev,
+			"Error %d trying to perform data_exchange", rc);
+		/* up the semaphore since ISR will never come in this case */
+		up(&stcontext->exchange_lock);
+		goto free_skb_resp;
+	}
+
+	kfree_skb(skb);
+
+	return rc;
+
+free_skb_resp:
+	kfree_skb(skb_resp);
+error:
+	return rc;
+}
+
+/* p2p will be supported in a later release ! */
+static int st95hf_tg_configure_hw(struct nfc_digital_dev *ddev,
+				  int type,
+				  int param)
+{
+	return 0;
+}
+
+static int st95hf_tg_send_cmd(struct nfc_digital_dev *ddev,
+			      struct sk_buff *skb,
+			      u16 timeout,
+			      nfc_digital_cmd_complete_t cb,
+			      void *arg)
+{
+	return 0;
+}
+
+static int st95hf_tg_listen(struct nfc_digital_dev *ddev,
+			    u16 timeout,
+			    nfc_digital_cmd_complete_t cb,
+			    void *arg)
+{
+	return 0;
+}
+
+static int st95hf_tg_get_rf_tech(struct nfc_digital_dev *ddev, u8 *rf_tech)
+{
+	return 0;
+}
+
+static int st95hf_switch_rf(struct nfc_digital_dev *ddev, bool on)
+{
+	u8 rf_tech;
+	struct st95hf_context *stcontext = nfc_digital_get_drvdata(ddev);
+
+	rf_tech = ddev->curr_rf_tech;
+
+	if (on)
+		/* switch on RF field */
+		return st95hf_select_protocol(stcontext, rf_tech);
+
+	/* switch OFF RF field */
+	return rf_off(stcontext);
+}
+
+/* TODO st95hf_abort_cmd */
+static void st95hf_abort_cmd(struct nfc_digital_dev *ddev)
+{
+}
+
+static struct nfc_digital_ops st95hf_nfc_digital_ops = {
+	.in_configure_hw = st95hf_in_configure_hw,
+	.in_send_cmd = st95hf_in_send_cmd,
+
+	.tg_listen = st95hf_tg_listen,
+	.tg_configure_hw = st95hf_tg_configure_hw,
+	.tg_send_cmd = st95hf_tg_send_cmd,
+	.tg_get_rf_tech = st95hf_tg_get_rf_tech,
+
+	.switch_rf = st95hf_switch_rf,
+	.abort_cmd = st95hf_abort_cmd,
+};
+
+static const struct spi_device_id st95hf_id[] = {
+	{ "st95hf", 0 },
+	{}
+};
+MODULE_DEVICE_TABLE(spi, st95hf_id);
+
+static int st95hf_probe(struct spi_device *nfc_spi_dev)
+{
+	int ret;
+
+	struct st95hf_context *st95context;
+	struct st95hf_spi_context *spicontext;
+
+	nfc_info(&nfc_spi_dev->dev, "ST95HF driver probe called.\n");
+
+	st95context = devm_kzalloc(&nfc_spi_dev->dev,
+				   sizeof(struct st95hf_context),
+				   GFP_KERNEL);
+	if (!st95context)
+		return -ENOMEM;
+
+	spicontext = &st95context->spicontext;
+
+	spicontext->spidev = nfc_spi_dev;
+
+	st95context->fwi =
+		cmd_array[CMD_ISO14443A_PROTOCOL_SELECT].cmd_params[2];
+
+	if (device_property_present(&nfc_spi_dev->dev, "st95hfvin")) {
+		st95context->st95hf_supply =
+			devm_regulator_get(&nfc_spi_dev->dev,
+					   "st95hfvin");
+		if (IS_ERR(st95context->st95hf_supply)) {
+			dev_err(&nfc_spi_dev->dev, "failed to acquire regulator\n");
+			return PTR_ERR(st95context->st95hf_supply);
+		}
+
+		ret = regulator_enable(st95context->st95hf_supply);
+		if (ret) {
+			dev_err(&nfc_spi_dev->dev, "failed to enable regulator\n");
+			return ret;
+		}
+	}
+
+	init_completion(&spicontext->done);
+	mutex_init(&spicontext->spi_lock);
+
+	/*
+	 * Store spicontext in spi device object for using it in
+	 * remove function
+	 */
+	dev_set_drvdata(&nfc_spi_dev->dev, spicontext);
+
+	st95context->enable_gpio =
+		of_get_named_gpio(nfc_spi_dev->dev.of_node,
+				  "enable-gpio",
+				  0);
+	if (!gpio_is_valid(st95context->enable_gpio)) {
+		dev_err(&nfc_spi_dev->dev, "No valid enable gpio\n");
+		ret = st95context->enable_gpio;
+		goto err_disable_regulator;
+	}
+
+	ret = devm_gpio_request_one(&nfc_spi_dev->dev, st95context->enable_gpio,
+				    GPIOF_DIR_OUT | GPIOF_INIT_HIGH,
+				    "enable_gpio");
+	if (ret)
+		goto err_disable_regulator;
+
+	if (nfc_spi_dev->irq > 0) {
+		if (devm_request_threaded_irq(&nfc_spi_dev->dev,
+					      nfc_spi_dev->irq,
+					      st95hf_irq_handler,
+					      st95hf_irq_thread_handler,
+					      IRQF_TRIGGER_FALLING,
+					      "st95hf",
+					      (void *)st95context) < 0) {
+			dev_err(&nfc_spi_dev->dev, "err: irq request for st95hf is failed\n");
+			ret =  -EINVAL;
+			goto err_disable_regulator;
+		}
+	} else {
+		dev_err(&nfc_spi_dev->dev, "not a valid IRQ associated with ST95HF\n");
+		ret = -EINVAL;
+		goto err_disable_regulator;
+	}
+
+	/*
+	 * First reset SPI to handle warm reset of the system.
+	 * It will put the ST95HF device in Power ON state
+	 * which make the state of device identical to state
+	 * at the time of cold reset of the system.
+	 */
+	ret = st95hf_send_spi_reset_sequence(st95context);
+	if (ret) {
+		dev_err(&nfc_spi_dev->dev, "err: spi_reset_sequence failed\n");
+		goto err_disable_regulator;
+	}
+
+	/* call PowerOnReset sequence of ST95hf to activate it */
+	ret = st95hf_por_sequence(st95context);
+	if (ret) {
+		dev_err(&nfc_spi_dev->dev, "err: por seq failed for st95hf\n");
+		goto err_disable_regulator;
+	}
+
+	/* create NFC dev object and register with NFC Subsystem */
+	st95context->ddev = nfc_digital_allocate_device(&st95hf_nfc_digital_ops,
+							ST95HF_SUPPORTED_PROT,
+							ST95HF_CAPABILITIES,
+							ST95HF_HEADROOM_LEN,
+							ST95HF_TAILROOM_LEN);
+	if (!st95context->ddev) {
+		ret = -ENOMEM;
+		goto err_disable_regulator;
+	}
+
+	st95context->nfcdev = st95context->ddev->nfc_dev;
+	nfc_digital_set_parent_dev(st95context->ddev, &nfc_spi_dev->dev);
+
+	ret =  nfc_digital_register_device(st95context->ddev);
+	if (ret) {
+		dev_err(&st95context->nfcdev->dev, "st95hf registration failed\n");
+		goto err_free_digital_device;
+	}
+
+	/* store st95context in nfc device object */
+	nfc_digital_set_drvdata(st95context->ddev, st95context);
+
+	sema_init(&st95context->exchange_lock, 1);
+	mutex_init(&st95context->rm_lock);
+
+	return ret;
+
+err_free_digital_device:
+	nfc_digital_free_device(st95context->ddev);
+err_disable_regulator:
+	if (st95context->st95hf_supply)
+		regulator_disable(st95context->st95hf_supply);
+
+	return ret;
+}
+
+static int st95hf_remove(struct spi_device *nfc_spi_dev)
+{
+	int result = 0;
+	unsigned char reset_cmd = ST95HF_COMMAND_RESET;
+	struct st95hf_spi_context *spictx = dev_get_drvdata(&nfc_spi_dev->dev);
+
+	struct st95hf_context *stcontext = container_of(spictx,
+							struct st95hf_context,
+							spicontext);
+
+	mutex_lock(&stcontext->rm_lock);
+
+	nfc_digital_unregister_device(stcontext->ddev);
+	nfc_digital_free_device(stcontext->ddev);
+	stcontext->nfcdev_free = true;
+
+	mutex_unlock(&stcontext->rm_lock);
+
+	/* if last in_send_cmd's ISR is pending, wait for it to finish */
+	result = down_killable(&stcontext->exchange_lock);
+	if (result == -EINTR)
+		dev_err(&spictx->spidev->dev, "sleep for semaphore interrupted by signal\n");
+
+	/* next reset the ST95HF controller */
+	result = st95hf_spi_send(&stcontext->spicontext,
+				 &reset_cmd,
+				 ST95HF_RESET_CMD_LEN,
+				 ASYNC);
+	if (result) {
+		dev_err(&spictx->spidev->dev,
+			"ST95HF reset failed in remove() err = %d\n", result);
+		return result;
+	}
+
+	/* wait for 3 ms to complete the controller reset process */
+	usleep_range(3000, 4000);
+
+	/* disable regulator */
+	if (stcontext->st95hf_supply)
+		regulator_disable(stcontext->st95hf_supply);
+
+	return result;
+}
+
+/* Register as SPI protocol driver */
+static struct spi_driver st95hf_driver = {
+	.driver = {
+		.name = "st95hf",
+		.owner = THIS_MODULE,
+	},
+	.id_table = st95hf_id,
+	.probe = st95hf_probe,
+	.remove = st95hf_remove,
+};
+
+module_spi_driver(st95hf_driver);
+
+MODULE_AUTHOR("Shikha Singh <shikha.singh@st.com>");
+MODULE_DESCRIPTION("ST NFC Transceiver ST95HF driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/nfc/st95hf/spi.c b/drivers/nfc/st95hf/spi.c
new file mode 100644
index 000000000000..e2d3bbcc8c34
--- /dev/null
+++ b/drivers/nfc/st95hf/spi.c
@@ -0,0 +1,167 @@
+/*
+ * ----------------------------------------------------------------------------
+ * drivers/nfc/st95hf/spi.c function definitions for SPI communication
+ * ----------------------------------------------------------------------------
+ * Copyright (C) 2015 STMicroelectronics Pvt. Ltd. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "spi.h"
+
+/* Function to send user provided buffer to ST95HF through SPI */
+int st95hf_spi_send(struct st95hf_spi_context *spicontext,
+		    unsigned char *buffertx,
+		    int datalen,
+		    enum req_type reqtype)
+{
+	struct spi_message m;
+	int result = 0;
+	struct spi_device *spidev = spicontext->spidev;
+	struct spi_transfer tx_transfer = {
+		.tx_buf = buffertx,
+		.len = datalen,
+	};
+
+	mutex_lock(&spicontext->spi_lock);
+
+	if (reqtype == SYNC) {
+		spicontext->req_issync = true;
+		reinit_completion(&spicontext->done);
+	} else {
+		spicontext->req_issync = false;
+	}
+
+	spi_message_init(&m);
+	spi_message_add_tail(&tx_transfer, &m);
+
+	result = spi_sync(spidev, &m);
+	if (result) {
+		dev_err(&spidev->dev, "error: sending cmd to st95hf using SPI = %d\n",
+			result);
+		mutex_unlock(&spicontext->spi_lock);
+		return result;
+	}
+
+	/* return for asynchronous or no-wait case */
+	if (reqtype == ASYNC) {
+		mutex_unlock(&spicontext->spi_lock);
+		return 0;
+	}
+
+	result = wait_for_completion_timeout(&spicontext->done,
+					     msecs_to_jiffies(1000));
+	/* check for timeout or success */
+	if (!result) {
+		dev_err(&spidev->dev, "error: response not ready timeout\n");
+		result = -ETIMEDOUT;
+	} else {
+		result = 0;
+	}
+
+	mutex_unlock(&spicontext->spi_lock);
+
+	return result;
+}
+EXPORT_SYMBOL_GPL(st95hf_spi_send);
+
+/* Function to Receive command Response */
+int st95hf_spi_recv_response(struct st95hf_spi_context *spicontext,
+			     unsigned char *receivebuff)
+{
+	int len = 0;
+	struct spi_transfer tx_takedata;
+	struct spi_message m;
+	struct spi_device *spidev = spicontext->spidev;
+	unsigned char readdata_cmd = ST95HF_COMMAND_RECEIVE;
+	struct spi_transfer t[2] = {
+		{.tx_buf = &readdata_cmd, .len = 1,},
+		{.rx_buf = receivebuff, .len = 2, .cs_change = 1,},
+	};
+
+	int ret = 0;
+
+	memset(&tx_takedata, 0x0, sizeof(struct spi_transfer));
+
+	mutex_lock(&spicontext->spi_lock);
+
+	/* First spi transfer to know the length of valid data */
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+
+	ret = spi_sync(spidev, &m);
+	if (ret) {
+		dev_err(&spidev->dev, "spi_recv_resp, data length error = %d\n",
+			ret);
+		mutex_unlock(&spicontext->spi_lock);
+		return ret;
+	}
+
+	/* As 2 bytes are already read */
+	len = 2;
+
+	/* Support of long frame */
+	if (receivebuff[0] & 0x60)
+		len += (((receivebuff[0] & 0x60) >> 5) << 8) | receivebuff[1];
+	else
+		len += receivebuff[1];
+
+	/* Now make a transfer to read only relevant bytes */
+	tx_takedata.rx_buf = &receivebuff[2];
+	tx_takedata.len = len - 2;
+
+	spi_message_init(&m);
+	spi_message_add_tail(&tx_takedata, &m);
+
+	ret = spi_sync(spidev, &m);
+
+	mutex_unlock(&spicontext->spi_lock);
+	if (ret) {
+		dev_err(&spidev->dev, "spi_recv_resp, data read error = %d\n",
+			ret);
+		return ret;
+	}
+
+	return len;
+}
+EXPORT_SYMBOL_GPL(st95hf_spi_recv_response);
+
+int st95hf_spi_recv_echo_res(struct st95hf_spi_context *spicontext,
+			     unsigned char *receivebuff)
+{
+	unsigned char readdata_cmd = ST95HF_COMMAND_RECEIVE;
+	struct spi_transfer t[2] = {
+		{.tx_buf = &readdata_cmd, .len = 1,},
+		{.rx_buf = receivebuff, .len = 1,},
+	};
+	struct spi_message m;
+	struct spi_device *spidev = spicontext->spidev;
+	int ret = 0;
+
+	mutex_lock(&spicontext->spi_lock);
+
+	spi_message_init(&m);
+	spi_message_add_tail(&t[0], &m);
+	spi_message_add_tail(&t[1], &m);
+	ret = spi_sync(spidev, &m);
+
+	mutex_unlock(&spicontext->spi_lock);
+
+	if (ret)
+		dev_err(&spidev->dev, "recv_echo_res, data read error = %d\n",
+			ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(st95hf_spi_recv_echo_res);
diff --git a/drivers/nfc/st95hf/spi.h b/drivers/nfc/st95hf/spi.h
new file mode 100644
index 000000000000..552d220747cd
--- /dev/null
+++ b/drivers/nfc/st95hf/spi.h
@@ -0,0 +1,64 @@
+/*
+ * ---------------------------------------------------------------------------
+ * drivers/nfc/st95hf/spi.h functions declarations for SPI communication
+ * ---------------------------------------------------------------------------
+ * Copyright (C) 2015 STMicroelectronics – All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __LINUX_ST95HF_SPI_H
+#define __LINUX_ST95HF_SPI_H
+
+#include <linux/spi/spi.h>
+
+/* Basic ST95HF SPI CMDs */
+#define ST95HF_COMMAND_SEND	0x0
+#define ST95HF_COMMAND_RESET	0x1
+#define ST95HF_COMMAND_RECEIVE	0x2
+
+#define ST95HF_RESET_CMD_LEN	0x1
+
+/*
+ * structure to contain st95hf spi communication specific information.
+ * @req_issync: true for synchronous calls.
+ * @spidev: st95hf spi device object.
+ * @done: completion structure to wait for st95hf response
+ *	for synchronous calls.
+ * @spi_lock: mutex to allow only one spi transfer at a time.
+ */
+struct st95hf_spi_context {
+	bool req_issync;
+	struct spi_device *spidev;
+	struct completion done;
+	struct mutex spi_lock;
+};
+
+/* flag to differentiate synchronous & asynchronous spi request */
+enum req_type {
+	SYNC,
+	ASYNC,
+};
+
+int st95hf_spi_send(struct st95hf_spi_context *spicontext,
+		    unsigned char *buffertx,
+		    int datalen,
+		    enum req_type reqtype);
+
+int st95hf_spi_recv_response(struct st95hf_spi_context *spicontext,
+			     unsigned char *receivebuff);
+
+int st95hf_spi_recv_echo_res(struct st95hf_spi_context *spicontext,
+			     unsigned char *receivebuff);
+
+#endif
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index f857feb2b573..10842b7051b3 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -2139,7 +2139,7 @@ static int trf7970a_remove(struct spi_device *spi)
 #ifdef CONFIG_PM_SLEEP
 static int trf7970a_suspend(struct device *dev)
 {
-	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct spi_device *spi = to_spi_device(dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 
 	dev_dbg(dev, "Suspend\n");
@@ -2155,7 +2155,7 @@ static int trf7970a_suspend(struct device *dev)
 
 static int trf7970a_resume(struct device *dev)
 {
-	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct spi_device *spi = to_spi_device(dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 	int ret;
 
@@ -2174,7 +2174,7 @@ static int trf7970a_resume(struct device *dev)
 #ifdef CONFIG_PM
 static int trf7970a_pm_runtime_suspend(struct device *dev)
 {
-	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct spi_device *spi = to_spi_device(dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 	int ret;
 
@@ -2191,7 +2191,7 @@ static int trf7970a_pm_runtime_suspend(struct device *dev)
 
 static int trf7970a_pm_runtime_resume(struct device *dev)
 {
-	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct spi_device *spi = to_spi_device(dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 	int ret;
 
diff --git a/drivers/ntb/hw/Kconfig b/drivers/ntb/hw/Kconfig
index 4d5535c4cddf..7116472b4625 100644
--- a/drivers/ntb/hw/Kconfig
+++ b/drivers/ntb/hw/Kconfig
@@ -1 +1,2 @@
+source "drivers/ntb/hw/amd/Kconfig"
 source "drivers/ntb/hw/intel/Kconfig"
diff --git a/drivers/ntb/hw/Makefile b/drivers/ntb/hw/Makefile
index 175d7c92a569..532e0859b4a1 100644
--- a/drivers/ntb/hw/Makefile
+++ b/drivers/ntb/hw/Makefile
@@ -1 +1,2 @@
+obj-$(CONFIG_NTB_AMD)	+= amd/
 obj-$(CONFIG_NTB_INTEL)	+= intel/
diff --git a/drivers/ntb/hw/amd/Kconfig b/drivers/ntb/hw/amd/Kconfig
new file mode 100644
index 000000000000..cfe903cd9514
--- /dev/null
+++ b/drivers/ntb/hw/amd/Kconfig
@@ -0,0 +1,7 @@
+config NTB_AMD
+	tristate "AMD Non-Transparent Bridge support"
+	depends on X86_64
+	help
+	 This driver supports AMD NTB on capable Zeppelin hardware.
+
+	 If unsure, say N.
diff --git a/drivers/ntb/hw/amd/Makefile b/drivers/ntb/hw/amd/Makefile
new file mode 100644
index 000000000000..ad54da917563
--- /dev/null
+++ b/drivers/ntb/hw/amd/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_NTB_AMD) += ntb_hw_amd.o
diff --git a/drivers/ntb/hw/amd/ntb_hw_amd.c b/drivers/ntb/hw/amd/ntb_hw_amd.c
new file mode 100644
index 000000000000..588803ad6847
--- /dev/null
+++ b/drivers/ntb/hw/amd/ntb_hw_amd.c
@@ -0,0 +1,1143 @@
+/*
+ * 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 Advanced Micro Devices, Inc. All Rights Reserved.
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of version 2 of the GNU General Public License as
+ *   published by the Free Software Foundation.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright (C) 2016 Advanced Micro Devices, Inc. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of AMD 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.
+ *
+ * AMD PCIe NTB Linux driver
+ *
+ * Contact Information:
+ * Xiangliang Yu <Xiangliang.Yu@amd.com>
+ */
+
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/pci.h>
+#include <linux/random.h>
+#include <linux/slab.h>
+#include <linux/ntb.h>
+
+#include "ntb_hw_amd.h"
+
+#define NTB_NAME	"ntb_hw_amd"
+#define NTB_DESC	"AMD(R) PCI-E Non-Transparent Bridge Driver"
+#define NTB_VER		"1.0"
+
+MODULE_DESCRIPTION(NTB_DESC);
+MODULE_VERSION(NTB_VER);
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_AUTHOR("AMD Inc.");
+
+static const struct file_operations amd_ntb_debugfs_info;
+static struct dentry *debugfs_dir;
+
+static int ndev_mw_to_bar(struct amd_ntb_dev *ndev, int idx)
+{
+	if (idx < 0 || idx > ndev->mw_count)
+		return -EINVAL;
+
+	return 1 << idx;
+}
+
+static int amd_ntb_mw_count(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->mw_count;
+}
+
+static int amd_ntb_mw_get_range(struct ntb_dev *ntb, int idx,
+				phys_addr_t *base,
+				resource_size_t *size,
+				resource_size_t *align,
+				resource_size_t *align_size)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	int bar;
+
+	bar = ndev_mw_to_bar(ndev, idx);
+	if (bar < 0)
+		return bar;
+
+	if (base)
+		*base = pci_resource_start(ndev->ntb.pdev, bar);
+
+	if (size)
+		*size = pci_resource_len(ndev->ntb.pdev, bar);
+
+	if (align)
+		*align = SZ_4K;
+
+	if (align_size)
+		*align_size = 1;
+
+	return 0;
+}
+
+static int amd_ntb_mw_set_trans(struct ntb_dev *ntb, int idx,
+				dma_addr_t addr, resource_size_t size)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	unsigned long xlat_reg, limit_reg = 0;
+	resource_size_t mw_size;
+	void __iomem *mmio, *peer_mmio;
+	u64 base_addr, limit, reg_val;
+	int bar;
+
+	bar = ndev_mw_to_bar(ndev, idx);
+	if (bar < 0)
+		return bar;
+
+	mw_size = pci_resource_len(ndev->ntb.pdev, bar);
+
+	/* make sure the range fits in the usable mw size */
+	if (size > mw_size)
+		return -EINVAL;
+
+	mmio = ndev->self_mmio;
+	peer_mmio = ndev->peer_mmio;
+
+	base_addr = pci_resource_start(ndev->ntb.pdev, bar);
+
+	if (bar != 1) {
+		xlat_reg = AMD_BAR23XLAT_OFFSET + ((bar - 2) << 3);
+		limit_reg = AMD_BAR23LMT_OFFSET + ((bar - 2) << 3);
+
+		/* Set the limit if supported */
+		limit = base_addr + size;
+
+		/* set and verify setting the translation address */
+		write64(addr, peer_mmio + xlat_reg);
+		reg_val = read64(peer_mmio + xlat_reg);
+		if (reg_val != addr) {
+			write64(0, peer_mmio + xlat_reg);
+			return -EIO;
+		}
+
+		/* set and verify setting the limit */
+		write64(limit, mmio + limit_reg);
+		reg_val = read64(mmio + limit_reg);
+		if (reg_val != limit) {
+			write64(base_addr, mmio + limit_reg);
+			write64(0, peer_mmio + xlat_reg);
+			return -EIO;
+		}
+	} else {
+		xlat_reg = AMD_BAR1XLAT_OFFSET;
+		limit_reg = AMD_BAR1LMT_OFFSET;
+
+		/* split bar addr range must all be 32 bit */
+		if (addr & (~0ull << 32))
+			return -EINVAL;
+		if ((addr + size) & (~0ull << 32))
+			return -EINVAL;
+
+		/* Set the limit if supported */
+		limit = base_addr + size;
+
+		/* set and verify setting the translation address */
+		write64(addr, peer_mmio + xlat_reg);
+		reg_val = read64(peer_mmio + xlat_reg);
+		if (reg_val != addr) {
+			write64(0, peer_mmio + xlat_reg);
+			return -EIO;
+		}
+
+		/* set and verify setting the limit */
+		writel(limit, mmio + limit_reg);
+		reg_val = readl(mmio + limit_reg);
+		if (reg_val != limit) {
+			writel(base_addr, mmio + limit_reg);
+			writel(0, peer_mmio + xlat_reg);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+static int amd_link_is_up(struct amd_ntb_dev *ndev)
+{
+	if (!ndev->peer_sta)
+		return NTB_LNK_STA_ACTIVE(ndev->cntl_sta);
+
+	/* If peer_sta is reset or D0 event, the ISR has
+	 * started a timer to check link status of hardware.
+	 * So here just clear status bit. And if peer_sta is
+	 * D3 or PME_TO, D0/reset event will be happened when
+	 * system wakeup/poweron, so do nothing here.
+	 */
+	if (ndev->peer_sta & AMD_PEER_RESET_EVENT)
+		ndev->peer_sta &= ~AMD_PEER_RESET_EVENT;
+	else if (ndev->peer_sta & AMD_PEER_D0_EVENT)
+		ndev->peer_sta = 0;
+
+	return 0;
+}
+
+static int amd_ntb_link_is_up(struct ntb_dev *ntb,
+			      enum ntb_speed *speed,
+			      enum ntb_width *width)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	int ret = 0;
+
+	if (amd_link_is_up(ndev)) {
+		if (speed)
+			*speed = NTB_LNK_STA_SPEED(ndev->lnk_sta);
+		if (width)
+			*width = NTB_LNK_STA_WIDTH(ndev->lnk_sta);
+
+		dev_dbg(ndev_dev(ndev), "link is up.\n");
+
+		ret = 1;
+	} else {
+		if (speed)
+			*speed = NTB_SPEED_NONE;
+		if (width)
+			*width = NTB_WIDTH_NONE;
+
+		dev_dbg(ndev_dev(ndev), "link is down.\n");
+	}
+
+	return ret;
+}
+
+static int amd_ntb_link_enable(struct ntb_dev *ntb,
+			       enum ntb_speed max_speed,
+			       enum ntb_width max_width)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 ntb_ctl;
+
+	/* Enable event interrupt */
+	ndev->int_mask &= ~AMD_EVENT_INTMASK;
+	writel(ndev->int_mask, mmio + AMD_INTMASK_OFFSET);
+
+	if (ndev->ntb.topo == NTB_TOPO_SEC)
+		return -EINVAL;
+	dev_dbg(ndev_dev(ndev), "Enabling Link.\n");
+
+	ntb_ctl = readl(mmio + AMD_CNTL_OFFSET);
+	ntb_ctl |= (PMM_REG_CTL | SMM_REG_CTL);
+	writel(ntb_ctl, mmio + AMD_CNTL_OFFSET);
+
+	return 0;
+}
+
+static int amd_ntb_link_disable(struct ntb_dev *ntb)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 ntb_ctl;
+
+	/* Disable event interrupt */
+	ndev->int_mask |= AMD_EVENT_INTMASK;
+	writel(ndev->int_mask, mmio + AMD_INTMASK_OFFSET);
+
+	if (ndev->ntb.topo == NTB_TOPO_SEC)
+		return -EINVAL;
+	dev_dbg(ndev_dev(ndev), "Enabling Link.\n");
+
+	ntb_ctl = readl(mmio + AMD_CNTL_OFFSET);
+	ntb_ctl &= ~(PMM_REG_CTL | SMM_REG_CTL);
+	writel(ntb_ctl, mmio + AMD_CNTL_OFFSET);
+
+	return 0;
+}
+
+static u64 amd_ntb_db_valid_mask(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->db_valid_mask;
+}
+
+static int amd_ntb_db_vector_count(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->db_count;
+}
+
+static u64 amd_ntb_db_vector_mask(struct ntb_dev *ntb, int db_vector)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+
+	if (db_vector < 0 || db_vector > ndev->db_count)
+		return 0;
+
+	return ntb_ndev(ntb)->db_valid_mask & (1 << db_vector);
+}
+
+static u64 amd_ntb_db_read(struct ntb_dev *ntb)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+
+	return (u64)readw(mmio + AMD_DBSTAT_OFFSET);
+}
+
+static int amd_ntb_db_clear(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+
+	writew((u16)db_bits, mmio + AMD_DBSTAT_OFFSET);
+
+	return 0;
+}
+
+static int amd_ntb_db_set_mask(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	unsigned long flags;
+
+	if (db_bits & ~ndev->db_valid_mask)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ndev->db_mask_lock, flags);
+	ndev->db_mask |= db_bits;
+	writew((u16)ndev->db_mask, mmio + AMD_DBMASK_OFFSET);
+	spin_unlock_irqrestore(&ndev->db_mask_lock, flags);
+
+	return 0;
+}
+
+static int amd_ntb_db_clear_mask(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	unsigned long flags;
+
+	if (db_bits & ~ndev->db_valid_mask)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ndev->db_mask_lock, flags);
+	ndev->db_mask &= ~db_bits;
+	writew((u16)ndev->db_mask, mmio + AMD_DBMASK_OFFSET);
+	spin_unlock_irqrestore(&ndev->db_mask_lock, flags);
+
+	return 0;
+}
+
+static int amd_ntb_peer_db_addr(struct ntb_dev *ntb,
+				phys_addr_t *db_addr,
+				resource_size_t *db_size)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+
+	if (db_addr)
+		*db_addr = (phys_addr_t)(ndev->peer_mmio + AMD_DBREQ_OFFSET);
+	if (db_size)
+		*db_size = sizeof(u32);
+
+	return 0;
+}
+
+static int amd_ntb_peer_db_set(struct ntb_dev *ntb, u64 db_bits)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+
+	writew((u16)db_bits, mmio + AMD_DBREQ_OFFSET);
+
+	return 0;
+}
+
+static int amd_ntb_spad_count(struct ntb_dev *ntb)
+{
+	return ntb_ndev(ntb)->spad_count;
+}
+
+static u32 amd_ntb_spad_read(struct ntb_dev *ntb, int idx)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 offset;
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return 0;
+
+	offset = ndev->self_spad + (idx << 2);
+	return readl(mmio + AMD_SPAD_OFFSET + offset);
+}
+
+static int amd_ntb_spad_write(struct ntb_dev *ntb,
+			      int idx, u32 val)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 offset;
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	offset = ndev->self_spad + (idx << 2);
+	writel(val, mmio + AMD_SPAD_OFFSET + offset);
+
+	return 0;
+}
+
+static int amd_ntb_peer_spad_addr(struct ntb_dev *ntb, int idx,
+				  phys_addr_t *spad_addr)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	if (spad_addr)
+		*spad_addr = (phys_addr_t)(ndev->self_mmio + AMD_SPAD_OFFSET +
+					   ndev->peer_spad + (idx << 2));
+	return 0;
+}
+
+static u32 amd_ntb_peer_spad_read(struct ntb_dev *ntb, int idx)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 offset;
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	offset = ndev->peer_spad + (idx << 2);
+	return readl(mmio + AMD_SPAD_OFFSET + offset);
+}
+
+static int amd_ntb_peer_spad_write(struct ntb_dev *ntb,
+				   int idx, u32 val)
+{
+	struct amd_ntb_dev *ndev = ntb_ndev(ntb);
+	void __iomem *mmio = ndev->self_mmio;
+	u32 offset;
+
+	if (idx < 0 || idx >= ndev->spad_count)
+		return -EINVAL;
+
+	offset = ndev->peer_spad + (idx << 2);
+	writel(val, mmio + AMD_SPAD_OFFSET + offset);
+
+	return 0;
+}
+
+static const struct ntb_dev_ops amd_ntb_ops = {
+	.mw_count		= amd_ntb_mw_count,
+	.mw_get_range		= amd_ntb_mw_get_range,
+	.mw_set_trans		= amd_ntb_mw_set_trans,
+	.link_is_up		= amd_ntb_link_is_up,
+	.link_enable		= amd_ntb_link_enable,
+	.link_disable		= amd_ntb_link_disable,
+	.db_valid_mask		= amd_ntb_db_valid_mask,
+	.db_vector_count	= amd_ntb_db_vector_count,
+	.db_vector_mask		= amd_ntb_db_vector_mask,
+	.db_read		= amd_ntb_db_read,
+	.db_clear		= amd_ntb_db_clear,
+	.db_set_mask		= amd_ntb_db_set_mask,
+	.db_clear_mask		= amd_ntb_db_clear_mask,
+	.peer_db_addr		= amd_ntb_peer_db_addr,
+	.peer_db_set		= amd_ntb_peer_db_set,
+	.spad_count		= amd_ntb_spad_count,
+	.spad_read		= amd_ntb_spad_read,
+	.spad_write		= amd_ntb_spad_write,
+	.peer_spad_addr		= amd_ntb_peer_spad_addr,
+	.peer_spad_read		= amd_ntb_peer_spad_read,
+	.peer_spad_write	= amd_ntb_peer_spad_write,
+};
+
+static void amd_ack_smu(struct amd_ntb_dev *ndev, u32 bit)
+{
+	void __iomem *mmio = ndev->self_mmio;
+	int reg;
+
+	reg = readl(mmio + AMD_SMUACK_OFFSET);
+	reg |= bit;
+	writel(reg, mmio + AMD_SMUACK_OFFSET);
+
+	ndev->peer_sta |= bit;
+}
+
+static void amd_handle_event(struct amd_ntb_dev *ndev, int vec)
+{
+	void __iomem *mmio = ndev->self_mmio;
+	u32 status;
+
+	status = readl(mmio + AMD_INTSTAT_OFFSET);
+	if (!(status & AMD_EVENT_INTMASK))
+		return;
+
+	dev_dbg(ndev_dev(ndev), "status = 0x%x and vec = %d\n", status, vec);
+
+	status &= AMD_EVENT_INTMASK;
+	switch (status) {
+	case AMD_PEER_FLUSH_EVENT:
+		dev_info(ndev_dev(ndev), "Flush is done.\n");
+		break;
+	case AMD_PEER_RESET_EVENT:
+		amd_ack_smu(ndev, AMD_PEER_RESET_EVENT);
+
+		/* link down first */
+		ntb_link_event(&ndev->ntb);
+		/* polling peer status */
+		schedule_delayed_work(&ndev->hb_timer, AMD_LINK_HB_TIMEOUT);
+
+		break;
+	case AMD_PEER_D3_EVENT:
+	case AMD_PEER_PMETO_EVENT:
+		amd_ack_smu(ndev, status);
+
+		/* link down */
+		ntb_link_event(&ndev->ntb);
+
+		break;
+	case AMD_PEER_D0_EVENT:
+		mmio = ndev->peer_mmio;
+		status = readl(mmio + AMD_PMESTAT_OFFSET);
+		/* check if this is WAKEUP event */
+		if (status & 0x1)
+			dev_info(ndev_dev(ndev), "Wakeup is done.\n");
+
+		amd_ack_smu(ndev, AMD_PEER_D0_EVENT);
+
+		/* start a timer to poll link status */
+		schedule_delayed_work(&ndev->hb_timer,
+				      AMD_LINK_HB_TIMEOUT);
+		break;
+	default:
+		dev_info(ndev_dev(ndev), "event status = 0x%x.\n", status);
+		break;
+	}
+}
+
+static irqreturn_t ndev_interrupt(struct amd_ntb_dev *ndev, int vec)
+{
+	dev_dbg(ndev_dev(ndev), "vec %d\n", vec);
+
+	if (vec > (AMD_DB_CNT - 1) || (ndev->msix_vec_count == 1))
+		amd_handle_event(ndev, vec);
+
+	if (vec < AMD_DB_CNT)
+		ntb_db_event(&ndev->ntb, vec);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t ndev_vec_isr(int irq, void *dev)
+{
+	struct amd_ntb_vec *nvec = dev;
+
+	return ndev_interrupt(nvec->ndev, nvec->num);
+}
+
+static irqreturn_t ndev_irq_isr(int irq, void *dev)
+{
+	struct amd_ntb_dev *ndev = dev;
+
+	return ndev_interrupt(ndev, irq - ndev_pdev(ndev)->irq);
+}
+
+static int ndev_init_isr(struct amd_ntb_dev *ndev,
+			 int msix_min, int msix_max)
+{
+	struct pci_dev *pdev;
+	int rc, i, msix_count, node;
+
+	pdev = ndev_pdev(ndev);
+
+	node = dev_to_node(&pdev->dev);
+
+	ndev->db_mask = ndev->db_valid_mask;
+
+	/* Try to set up msix irq */
+	ndev->vec = kzalloc_node(msix_max * sizeof(*ndev->vec),
+				 GFP_KERNEL, node);
+	if (!ndev->vec)
+		goto err_msix_vec_alloc;
+
+	ndev->msix = kzalloc_node(msix_max * sizeof(*ndev->msix),
+				  GFP_KERNEL, node);
+	if (!ndev->msix)
+		goto err_msix_alloc;
+
+	for (i = 0; i < msix_max; ++i)
+		ndev->msix[i].entry = i;
+
+	msix_count = pci_enable_msix_range(pdev, ndev->msix,
+					   msix_min, msix_max);
+	if (msix_count < 0)
+		goto err_msix_enable;
+
+	/* NOTE: Disable MSIX if msix count is less than 16 because of
+	 * hardware limitation.
+	 */
+	if (msix_count < msix_min) {
+		pci_disable_msix(pdev);
+		goto err_msix_enable;
+	}
+
+	for (i = 0; i < msix_count; ++i) {
+		ndev->vec[i].ndev = ndev;
+		ndev->vec[i].num = i;
+		rc = request_irq(ndev->msix[i].vector, ndev_vec_isr, 0,
+				 "ndev_vec_isr", &ndev->vec[i]);
+		if (rc)
+			goto err_msix_request;
+	}
+
+	dev_dbg(ndev_dev(ndev), "Using msix interrupts\n");
+	ndev->db_count = msix_min;
+	ndev->msix_vec_count = msix_max;
+	return 0;
+
+err_msix_request:
+	while (i-- > 0)
+		free_irq(ndev->msix[i].vector, ndev);
+	pci_disable_msix(pdev);
+err_msix_enable:
+	kfree(ndev->msix);
+err_msix_alloc:
+	kfree(ndev->vec);
+err_msix_vec_alloc:
+	ndev->msix = NULL;
+	ndev->vec = NULL;
+
+	/* Try to set up msi irq */
+	rc = pci_enable_msi(pdev);
+	if (rc)
+		goto err_msi_enable;
+
+	rc = request_irq(pdev->irq, ndev_irq_isr, 0,
+			 "ndev_irq_isr", ndev);
+	if (rc)
+		goto err_msi_request;
+
+	dev_dbg(ndev_dev(ndev), "Using msi interrupts\n");
+	ndev->db_count = 1;
+	ndev->msix_vec_count = 1;
+	return 0;
+
+err_msi_request:
+	pci_disable_msi(pdev);
+err_msi_enable:
+
+	/* Try to set up intx irq */
+	pci_intx(pdev, 1);
+
+	rc = request_irq(pdev->irq, ndev_irq_isr, IRQF_SHARED,
+			 "ndev_irq_isr", ndev);
+	if (rc)
+		goto err_intx_request;
+
+	dev_dbg(ndev_dev(ndev), "Using intx interrupts\n");
+	ndev->db_count = 1;
+	ndev->msix_vec_count = 1;
+	return 0;
+
+err_intx_request:
+	return rc;
+}
+
+static void ndev_deinit_isr(struct amd_ntb_dev *ndev)
+{
+	struct pci_dev *pdev;
+	void __iomem *mmio = ndev->self_mmio;
+	int i;
+
+	pdev = ndev_pdev(ndev);
+
+	/* Mask all doorbell interrupts */
+	ndev->db_mask = ndev->db_valid_mask;
+	writel(ndev->db_mask, mmio + AMD_DBMASK_OFFSET);
+
+	if (ndev->msix) {
+		i = ndev->msix_vec_count;
+		while (i--)
+			free_irq(ndev->msix[i].vector, &ndev->vec[i]);
+		pci_disable_msix(pdev);
+		kfree(ndev->msix);
+		kfree(ndev->vec);
+	} else {
+		free_irq(pdev->irq, ndev);
+		if (pci_dev_msi_enabled(pdev))
+			pci_disable_msi(pdev);
+		else
+			pci_intx(pdev, 0);
+	}
+}
+
+static ssize_t ndev_debugfs_read(struct file *filp, char __user *ubuf,
+				 size_t count, loff_t *offp)
+{
+	struct amd_ntb_dev *ndev;
+	void __iomem *mmio;
+	char *buf;
+	size_t buf_size;
+	ssize_t ret, off;
+	union { u64 v64; u32 v32; u16 v16; } u;
+
+	ndev = filp->private_data;
+	mmio = ndev->self_mmio;
+
+	buf_size = min(count, 0x800ul);
+
+	buf = kmalloc(buf_size, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	off = 0;
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "NTB Device Information:\n");
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Connection Topology -\t%s\n",
+			 ntb_topo_string(ndev->ntb.topo));
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "LNK STA -\t\t%#06x\n", ndev->lnk_sta);
+
+	if (!amd_link_is_up(ndev)) {
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Status -\t\tDown\n");
+	} else {
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Status -\t\tUp\n");
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Speed -\t\tPCI-E Gen %u\n",
+				 NTB_LNK_STA_SPEED(ndev->lnk_sta));
+		off += scnprintf(buf + off, buf_size - off,
+				 "Link Width -\t\tx%u\n",
+				 NTB_LNK_STA_WIDTH(ndev->lnk_sta));
+	}
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Memory Window Count -\t%u\n", ndev->mw_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Scratchpad Count -\t%u\n", ndev->spad_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Count -\t%u\n", ndev->db_count);
+	off += scnprintf(buf + off, buf_size - off,
+			 "MSIX Vector Count -\t%u\n", ndev->msix_vec_count);
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Valid Mask -\t%#llx\n", ndev->db_valid_mask);
+
+	u.v32 = readl(ndev->self_mmio + AMD_DBMASK_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Mask -\t\t\t%#06x\n", u.v32);
+
+	u.v32 = readl(mmio + AMD_DBSTAT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "Doorbell Bell -\t\t\t%#06x\n", u.v32);
+
+	off += scnprintf(buf + off, buf_size - off,
+			 "\nNTB Incoming XLAT:\n");
+
+	u.v64 = read64(mmio + AMD_BAR1XLAT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "XLAT1 -\t\t%#018llx\n", u.v64);
+
+	u.v64 = read64(ndev->self_mmio + AMD_BAR23XLAT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "XLAT23 -\t\t%#018llx\n", u.v64);
+
+	u.v64 = read64(ndev->self_mmio + AMD_BAR45XLAT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "XLAT45 -\t\t%#018llx\n", u.v64);
+
+	u.v32 = readl(mmio + AMD_BAR1LMT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "LMT1 -\t\t\t%#06x\n", u.v32);
+
+	u.v64 = read64(ndev->self_mmio + AMD_BAR23LMT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "LMT23 -\t\t\t%#018llx\n", u.v64);
+
+	u.v64 = read64(ndev->self_mmio + AMD_BAR45LMT_OFFSET);
+	off += scnprintf(buf + off, buf_size - off,
+			 "LMT45 -\t\t\t%#018llx\n", u.v64);
+
+	ret = simple_read_from_buffer(ubuf, count, offp, buf, off);
+	kfree(buf);
+	return ret;
+}
+
+static void ndev_init_debugfs(struct amd_ntb_dev *ndev)
+{
+	if (!debugfs_dir) {
+		ndev->debugfs_dir = NULL;
+		ndev->debugfs_info = NULL;
+	} else {
+		ndev->debugfs_dir =
+			debugfs_create_dir(ndev_name(ndev), debugfs_dir);
+		if (!ndev->debugfs_dir)
+			ndev->debugfs_info = NULL;
+		else
+			ndev->debugfs_info =
+				debugfs_create_file("info", S_IRUSR,
+						    ndev->debugfs_dir, ndev,
+						    &amd_ntb_debugfs_info);
+	}
+}
+
+static void ndev_deinit_debugfs(struct amd_ntb_dev *ndev)
+{
+	debugfs_remove_recursive(ndev->debugfs_dir);
+}
+
+static inline void ndev_init_struct(struct amd_ntb_dev *ndev,
+				    struct pci_dev *pdev)
+{
+	ndev->ntb.pdev = pdev;
+	ndev->ntb.topo = NTB_TOPO_NONE;
+	ndev->ntb.ops = &amd_ntb_ops;
+	ndev->int_mask = AMD_EVENT_INTMASK;
+	spin_lock_init(&ndev->db_mask_lock);
+}
+
+static int amd_poll_link(struct amd_ntb_dev *ndev)
+{
+	void __iomem *mmio = ndev->peer_mmio;
+	u32 reg, stat;
+	int rc;
+
+	reg = readl(mmio + AMD_SIDEINFO_OFFSET);
+	reg &= NTB_LIN_STA_ACTIVE_BIT;
+
+	dev_dbg(ndev_dev(ndev), "%s: reg_val = 0x%x.\n", __func__, reg);
+
+	if (reg == ndev->cntl_sta)
+		return 0;
+
+	ndev->cntl_sta = reg;
+
+	rc = pci_read_config_dword(ndev->ntb.pdev,
+				   AMD_LINK_STATUS_OFFSET, &stat);
+	if (rc)
+		return 0;
+	ndev->lnk_sta = stat;
+
+	return 1;
+}
+
+static void amd_link_hb(struct work_struct *work)
+{
+	struct amd_ntb_dev *ndev = hb_ndev(work);
+
+	if (amd_poll_link(ndev))
+		ntb_link_event(&ndev->ntb);
+
+	if (!amd_link_is_up(ndev))
+		schedule_delayed_work(&ndev->hb_timer, AMD_LINK_HB_TIMEOUT);
+}
+
+static int amd_init_isr(struct amd_ntb_dev *ndev)
+{
+	return ndev_init_isr(ndev, AMD_DB_CNT, AMD_MSIX_VECTOR_CNT);
+}
+
+static void amd_init_side_info(struct amd_ntb_dev *ndev)
+{
+	void __iomem *mmio = ndev->self_mmio;
+	unsigned int reg;
+
+	reg = readl(mmio + AMD_SIDEINFO_OFFSET);
+	if (!(reg & AMD_SIDE_READY)) {
+		reg |= AMD_SIDE_READY;
+		writel(reg, mmio + AMD_SIDEINFO_OFFSET);
+	}
+}
+
+static void amd_deinit_side_info(struct amd_ntb_dev *ndev)
+{
+	void __iomem *mmio = ndev->self_mmio;
+	unsigned int reg;
+
+	reg = readl(mmio + AMD_SIDEINFO_OFFSET);
+	if (reg & AMD_SIDE_READY) {
+		reg &= ~AMD_SIDE_READY;
+		writel(reg, mmio + AMD_SIDEINFO_OFFSET);
+		readl(mmio + AMD_SIDEINFO_OFFSET);
+	}
+}
+
+static int amd_init_ntb(struct amd_ntb_dev *ndev)
+{
+	void __iomem *mmio = ndev->self_mmio;
+
+	ndev->mw_count = AMD_MW_CNT;
+	ndev->spad_count = AMD_SPADS_CNT;
+	ndev->db_count = AMD_DB_CNT;
+
+	switch (ndev->ntb.topo) {
+	case NTB_TOPO_PRI:
+	case NTB_TOPO_SEC:
+		ndev->spad_count >>= 1;
+		if (ndev->ntb.topo == NTB_TOPO_PRI) {
+			ndev->self_spad = 0;
+			ndev->peer_spad = 0x20;
+		} else {
+			ndev->self_spad = 0x20;
+			ndev->peer_spad = 0;
+		}
+
+		INIT_DELAYED_WORK(&ndev->hb_timer, amd_link_hb);
+		schedule_delayed_work(&ndev->hb_timer, AMD_LINK_HB_TIMEOUT);
+
+		break;
+	default:
+		dev_err(ndev_dev(ndev), "AMD NTB does not support B2B mode.\n");
+		return -EINVAL;
+	}
+
+	ndev->db_valid_mask = BIT_ULL(ndev->db_count) - 1;
+
+	/* Mask event interrupts */
+	writel(ndev->int_mask, mmio + AMD_INTMASK_OFFSET);
+
+	return 0;
+}
+
+static enum ntb_topo amd_get_topo(struct amd_ntb_dev *ndev)
+{
+	void __iomem *mmio = ndev->self_mmio;
+	u32 info;
+
+	info = readl(mmio + AMD_SIDEINFO_OFFSET);
+	if (info & AMD_SIDE_MASK)
+		return NTB_TOPO_SEC;
+	else
+		return NTB_TOPO_PRI;
+}
+
+static int amd_init_dev(struct amd_ntb_dev *ndev)
+{
+	struct pci_dev *pdev;
+	int rc = 0;
+
+	pdev = ndev_pdev(ndev);
+
+	ndev->ntb.topo = amd_get_topo(ndev);
+	dev_dbg(ndev_dev(ndev), "AMD NTB topo is %s\n",
+		ntb_topo_string(ndev->ntb.topo));
+
+	rc = amd_init_ntb(ndev);
+	if (rc)
+		return rc;
+
+	rc = amd_init_isr(ndev);
+	if (rc) {
+		dev_err(ndev_dev(ndev), "fail to init isr.\n");
+		return rc;
+	}
+
+	ndev->db_valid_mask = BIT_ULL(ndev->db_count) - 1;
+
+	return 0;
+}
+
+static void amd_deinit_dev(struct amd_ntb_dev *ndev)
+{
+	cancel_delayed_work_sync(&ndev->hb_timer);
+
+	ndev_deinit_isr(ndev);
+}
+
+static int amd_ntb_init_pci(struct amd_ntb_dev *ndev,
+			    struct pci_dev *pdev)
+{
+	int rc;
+
+	pci_set_drvdata(pdev, ndev);
+
+	rc = pci_enable_device(pdev);
+	if (rc)
+		goto err_pci_enable;
+
+	rc = pci_request_regions(pdev, NTB_NAME);
+	if (rc)
+		goto err_pci_regions;
+
+	pci_set_master(pdev);
+
+	rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (rc) {
+		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (rc)
+			goto err_dma_mask;
+		dev_warn(ndev_dev(ndev), "Cannot DMA highmem\n");
+	}
+
+	rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (rc) {
+		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (rc)
+			goto err_dma_mask;
+		dev_warn(ndev_dev(ndev), "Cannot DMA consistent highmem\n");
+	}
+
+	ndev->self_mmio = pci_iomap(pdev, 0, 0);
+	if (!ndev->self_mmio) {
+		rc = -EIO;
+		goto err_dma_mask;
+	}
+	ndev->peer_mmio = ndev->self_mmio + AMD_PEER_OFFSET;
+
+	return 0;
+
+err_dma_mask:
+	pci_clear_master(pdev);
+err_pci_regions:
+	pci_disable_device(pdev);
+err_pci_enable:
+	pci_set_drvdata(pdev, NULL);
+	return rc;
+}
+
+static void amd_ntb_deinit_pci(struct amd_ntb_dev *ndev)
+{
+	struct pci_dev *pdev = ndev_pdev(ndev);
+
+	pci_iounmap(pdev, ndev->self_mmio);
+
+	pci_clear_master(pdev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+}
+
+static int amd_ntb_pci_probe(struct pci_dev *pdev,
+			     const struct pci_device_id *id)
+{
+	struct amd_ntb_dev *ndev;
+	int rc, node;
+
+	node = dev_to_node(&pdev->dev);
+
+	ndev = kzalloc_node(sizeof(*ndev), GFP_KERNEL, node);
+	if (!ndev) {
+		rc = -ENOMEM;
+		goto err_ndev;
+	}
+
+	ndev_init_struct(ndev, pdev);
+
+	rc = amd_ntb_init_pci(ndev, pdev);
+	if (rc)
+		goto err_init_pci;
+
+	rc = amd_init_dev(ndev);
+	if (rc)
+		goto err_init_dev;
+
+	/* write side info */
+	amd_init_side_info(ndev);
+
+	amd_poll_link(ndev);
+
+	ndev_init_debugfs(ndev);
+
+	rc = ntb_register_device(&ndev->ntb);
+	if (rc)
+		goto err_register;
+
+	dev_info(&pdev->dev, "NTB device registered.\n");
+
+	return 0;
+
+err_register:
+	ndev_deinit_debugfs(ndev);
+	amd_deinit_dev(ndev);
+err_init_dev:
+	amd_ntb_deinit_pci(ndev);
+err_init_pci:
+	kfree(ndev);
+err_ndev:
+	return rc;
+}
+
+static void amd_ntb_pci_remove(struct pci_dev *pdev)
+{
+	struct amd_ntb_dev *ndev = pci_get_drvdata(pdev);
+
+	ntb_unregister_device(&ndev->ntb);
+	ndev_deinit_debugfs(ndev);
+	amd_deinit_side_info(ndev);
+	amd_deinit_dev(ndev);
+	amd_ntb_deinit_pci(ndev);
+	kfree(ndev);
+}
+
+static const struct file_operations amd_ntb_debugfs_info = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ndev_debugfs_read,
+};
+
+static const struct pci_device_id amd_ntb_pci_tbl[] = {
+	{PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_NTB)},
+	{0}
+};
+MODULE_DEVICE_TABLE(pci, amd_ntb_pci_tbl);
+
+static struct pci_driver amd_ntb_pci_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= amd_ntb_pci_tbl,
+	.probe		= amd_ntb_pci_probe,
+	.remove		= amd_ntb_pci_remove,
+};
+
+static int __init amd_ntb_pci_driver_init(void)
+{
+	pr_info("%s %s\n", NTB_DESC, NTB_VER);
+
+	if (debugfs_initialized())
+		debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+
+	return pci_register_driver(&amd_ntb_pci_driver);
+}
+module_init(amd_ntb_pci_driver_init);
+
+static void __exit amd_ntb_pci_driver_exit(void)
+{
+	pci_unregister_driver(&amd_ntb_pci_driver);
+	debugfs_remove_recursive(debugfs_dir);
+}
+module_exit(amd_ntb_pci_driver_exit);
diff --git a/drivers/ntb/hw/amd/ntb_hw_amd.h b/drivers/ntb/hw/amd/ntb_hw_amd.h
new file mode 100644
index 000000000000..2eac3cd3e646
--- /dev/null
+++ b/drivers/ntb/hw/amd/ntb_hw_amd.h
@@ -0,0 +1,217 @@
+/*
+ * 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 Advanced Micro Devices, Inc. All Rights Reserved.
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of version 2 of the GNU General Public License as
+ *   published by the Free Software Foundation.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright (C) 2016 Advanced Micro Devices, Inc. All Rights Reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of AMD 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.
+ *
+ * AMD PCIe NTB Linux driver
+ *
+ * Contact Information:
+ * Xiangliang Yu <Xiangliang.Yu@amd.com>
+ */
+
+#ifndef NTB_HW_AMD_H
+#define NTB_HW_AMD_H
+
+#include <linux/ntb.h>
+#include <linux/pci.h>
+
+#define PCI_DEVICE_ID_AMD_NTB	0x145B
+#define AMD_LINK_HB_TIMEOUT	msecs_to_jiffies(1000)
+#define AMD_LINK_STATUS_OFFSET	0x68
+#define NTB_LIN_STA_ACTIVE_BIT	0x00000002
+#define NTB_LNK_STA_SPEED_MASK	0x000F0000
+#define NTB_LNK_STA_WIDTH_MASK	0x03F00000
+#define NTB_LNK_STA_ACTIVE(x)	(!!((x) & NTB_LIN_STA_ACTIVE_BIT))
+#define NTB_LNK_STA_SPEED(x)	(((x) & NTB_LNK_STA_SPEED_MASK) >> 16)
+#define NTB_LNK_STA_WIDTH(x)	(((x) & NTB_LNK_STA_WIDTH_MASK) >> 20)
+
+#ifndef read64
+#ifdef readq
+#define read64 readq
+#else
+#define read64 _read64
+static inline u64 _read64(void __iomem *mmio)
+{
+	u64 low, high;
+
+	low = readl(mmio);
+	high = readl(mmio + sizeof(u32));
+	return low | (high << 32);
+}
+#endif
+#endif
+
+#ifndef write64
+#ifdef writeq
+#define write64 writeq
+#else
+#define write64 _write64
+static inline void _write64(u64 val, void __iomem *mmio)
+{
+	writel(val, mmio);
+	writel(val >> 32, mmio + sizeof(u32));
+}
+#endif
+#endif
+
+enum {
+	/* AMD NTB Capability */
+	AMD_MW_CNT		= 3,
+	AMD_DB_CNT		= 16,
+	AMD_MSIX_VECTOR_CNT	= 24,
+	AMD_SPADS_CNT		= 16,
+
+	/*  AMD NTB register offset */
+	AMD_CNTL_OFFSET		= 0x200,
+
+	/* NTB control register bits */
+	PMM_REG_CTL		= BIT(21),
+	SMM_REG_CTL		= BIT(20),
+	SMM_REG_ACC_PATH	= BIT(18),
+	PMM_REG_ACC_PATH	= BIT(17),
+	NTB_CLK_EN		= BIT(16),
+
+	AMD_STA_OFFSET		= 0x204,
+	AMD_PGSLV_OFFSET	= 0x208,
+	AMD_SPAD_MUX_OFFSET	= 0x20C,
+	AMD_SPAD_OFFSET		= 0x210,
+	AMD_RSMU_HCID		= 0x250,
+	AMD_RSMU_SIID		= 0x254,
+	AMD_PSION_OFFSET	= 0x300,
+	AMD_SSION_OFFSET	= 0x330,
+	AMD_MMINDEX_OFFSET	= 0x400,
+	AMD_MMDATA_OFFSET	= 0x404,
+	AMD_SIDEINFO_OFFSET	= 0x408,
+
+	AMD_SIDE_MASK		= BIT(0),
+	AMD_SIDE_READY		= BIT(1),
+
+	/* limit register */
+	AMD_ROMBARLMT_OFFSET	= 0x410,
+	AMD_BAR1LMT_OFFSET	= 0x414,
+	AMD_BAR23LMT_OFFSET	= 0x418,
+	AMD_BAR45LMT_OFFSET	= 0x420,
+	/* xlat address */
+	AMD_POMBARXLAT_OFFSET	= 0x428,
+	AMD_BAR1XLAT_OFFSET	= 0x430,
+	AMD_BAR23XLAT_OFFSET	= 0x438,
+	AMD_BAR45XLAT_OFFSET	= 0x440,
+	/* doorbell and interrupt */
+	AMD_DBFM_OFFSET		= 0x450,
+	AMD_DBREQ_OFFSET	= 0x454,
+	AMD_MIRRDBSTAT_OFFSET	= 0x458,
+	AMD_DBMASK_OFFSET	= 0x45C,
+	AMD_DBSTAT_OFFSET	= 0x460,
+	AMD_INTMASK_OFFSET	= 0x470,
+	AMD_INTSTAT_OFFSET	= 0x474,
+
+	/* event type */
+	AMD_PEER_FLUSH_EVENT	= BIT(0),
+	AMD_PEER_RESET_EVENT	= BIT(1),
+	AMD_PEER_D3_EVENT	= BIT(2),
+	AMD_PEER_PMETO_EVENT	= BIT(3),
+	AMD_PEER_D0_EVENT	= BIT(4),
+	AMD_EVENT_INTMASK	= (AMD_PEER_FLUSH_EVENT |
+				AMD_PEER_RESET_EVENT | AMD_PEER_D3_EVENT |
+				AMD_PEER_PMETO_EVENT | AMD_PEER_D0_EVENT),
+
+	AMD_PMESTAT_OFFSET	= 0x480,
+	AMD_PMSGTRIG_OFFSET	= 0x490,
+	AMD_LTRLATENCY_OFFSET	= 0x494,
+	AMD_FLUSHTRIG_OFFSET	= 0x498,
+
+	/* SMU register*/
+	AMD_SMUACK_OFFSET	= 0x4A0,
+	AMD_SINRST_OFFSET	= 0x4A4,
+	AMD_RSPNUM_OFFSET	= 0x4A8,
+	AMD_SMU_SPADMUTEX	= 0x4B0,
+	AMD_SMU_SPADOFFSET	= 0x4B4,
+
+	AMD_PEER_OFFSET		= 0x400,
+};
+
+struct amd_ntb_dev;
+
+struct amd_ntb_vec {
+	struct amd_ntb_dev	*ndev;
+	int			num;
+};
+
+struct amd_ntb_dev {
+	struct ntb_dev ntb;
+
+	u32 ntb_side;
+	u32 lnk_sta;
+	u32 cntl_sta;
+	u32 peer_sta;
+
+	unsigned char mw_count;
+	unsigned char spad_count;
+	unsigned char db_count;
+	unsigned char msix_vec_count;
+
+	u64 db_valid_mask;
+	u64 db_mask;
+	u32 int_mask;
+
+	struct msix_entry *msix;
+	struct amd_ntb_vec *vec;
+
+	/* synchronize rmw access of db_mask and hw reg */
+	spinlock_t db_mask_lock;
+
+	void __iomem *self_mmio;
+	void __iomem *peer_mmio;
+	unsigned int self_spad;
+	unsigned int peer_spad;
+
+	struct delayed_work hb_timer;
+
+	struct dentry *debugfs_dir;
+	struct dentry *debugfs_info;
+};
+
+#define ndev_pdev(ndev) ((ndev)->ntb.pdev)
+#define ndev_name(ndev) pci_name(ndev_pdev(ndev))
+#define ndev_dev(ndev) (&ndev_pdev(ndev)->dev)
+#define ntb_ndev(__ntb) container_of(__ntb, struct amd_ntb_dev, ntb)
+#define hb_ndev(__work) container_of(__work, struct amd_ntb_dev, hb_timer.work)
+
+#endif
diff --git a/drivers/ntb/hw/intel/ntb_hw_intel.c b/drivers/ntb/hw/intel/ntb_hw_intel.c
index a198f8298258..40d04ef5da9e 100644
--- a/drivers/ntb/hw/intel/ntb_hw_intel.c
+++ b/drivers/ntb/hw/intel/ntb_hw_intel.c
@@ -875,7 +875,7 @@ static int intel_ntb_mw_set_trans(struct ntb_dev *ntb, int idx,
 	limit_reg = bar2_off(ndev->xlat_reg->bar2_limit, bar);
 
 	if (bar < 4 || !ndev->bar4_split) {
-		base = ioread64(mmio + base_reg);
+		base = ioread64(mmio + base_reg) & NTB_BAR_MASK_64;
 
 		/* Set the limit if supported, if size is not mw_size */
 		if (limit_reg && size != mw_size)
@@ -906,7 +906,7 @@ static int intel_ntb_mw_set_trans(struct ntb_dev *ntb, int idx,
 		if ((addr + size) & (~0ull << 32))
 			return -EINVAL;
 
-		base = ioread32(mmio + base_reg);
+		base = ioread32(mmio + base_reg) & NTB_BAR_MASK_32;
 
 		/* Set the limit if supported, if size is not mw_size */
 		if (limit_reg && size != mw_size)
diff --git a/drivers/ntb/hw/intel/ntb_hw_intel.h b/drivers/ntb/hw/intel/ntb_hw_intel.h
index 2eb4addd10d0..3ec149cf6562 100644
--- a/drivers/ntb/hw/intel/ntb_hw_intel.h
+++ b/drivers/ntb/hw/intel/ntb_hw_intel.h
@@ -245,6 +245,9 @@
 #define NTB_UNSAFE_DB			BIT_ULL(0)
 #define NTB_UNSAFE_SPAD			BIT_ULL(1)
 
+#define NTB_BAR_MASK_64			~(0xfull)
+#define NTB_BAR_MASK_32			~(0xfu)
+
 struct intel_ntb_dev;
 
 struct intel_ntb_reg {
@@ -334,7 +337,8 @@ struct intel_ntb_dev {
 #define ndev_pdev(ndev) ((ndev)->ntb.pdev)
 #define ndev_name(ndev) pci_name(ndev_pdev(ndev))
 #define ndev_dev(ndev) (&ndev_pdev(ndev)->dev)
-#define ntb_ndev(ntb) container_of(ntb, struct intel_ntb_dev, ntb)
-#define hb_ndev(work) container_of(work, struct intel_ntb_dev, hb_timer.work)
+#define ntb_ndev(__ntb) container_of(__ntb, struct intel_ntb_dev, ntb)
+#define hb_ndev(__work) container_of(__work, struct intel_ntb_dev, \
+				     hb_timer.work)
 
 #endif
diff --git a/drivers/ntb/ntb_transport.c b/drivers/ntb/ntb_transport.c
index 60654d524858..ec4775f0ec16 100644
--- a/drivers/ntb/ntb_transport.c
+++ b/drivers/ntb/ntb_transport.c
@@ -171,12 +171,14 @@ struct ntb_transport_qp {
 	u64 rx_err_ver;
 	u64 rx_memcpy;
 	u64 rx_async;
+	u64 dma_rx_prep_err;
 	u64 tx_bytes;
 	u64 tx_pkts;
 	u64 tx_ring_full;
 	u64 tx_err_no_buf;
 	u64 tx_memcpy;
 	u64 tx_async;
+	u64 dma_tx_prep_err;
 };
 
 struct ntb_transport_mw {
@@ -249,6 +251,8 @@ enum {
 #define QP_TO_MW(nt, qp)	((qp) % nt->mw_count)
 #define NTB_QP_DEF_NUM_ENTRIES	100
 #define NTB_LINK_DOWN_TIMEOUT	10
+#define DMA_RETRIES		20
+#define DMA_OUT_RESOURCE_TO	50
 
 static void ntb_transport_rxc_db(unsigned long data);
 static const struct ntb_ctx_ops ntb_transport_ops;
@@ -501,6 +505,12 @@ static ssize_t debugfs_read(struct file *filp, char __user *ubuf, size_t count,
 	out_offset += snprintf(buf + out_offset, out_count - out_offset,
 			       "free tx - \t%u\n",
 			       ntb_transport_tx_free_entry(qp));
+	out_offset += snprintf(buf + out_offset, out_count - out_offset,
+			       "DMA tx prep err - \t%llu\n",
+			       qp->dma_tx_prep_err);
+	out_offset += snprintf(buf + out_offset, out_count - out_offset,
+			       "DMA rx prep err - \t%llu\n",
+			       qp->dma_rx_prep_err);
 
 	out_offset += snprintf(buf + out_offset, out_count - out_offset,
 			       "\n");
@@ -726,6 +736,8 @@ static void ntb_qp_link_down_reset(struct ntb_transport_qp *qp)
 	qp->tx_err_no_buf = 0;
 	qp->tx_memcpy = 0;
 	qp->tx_async = 0;
+	qp->dma_tx_prep_err = 0;
+	qp->dma_rx_prep_err = 0;
 }
 
 static void ntb_qp_link_cleanup(struct ntb_transport_qp *qp)
@@ -1228,6 +1240,7 @@ static void ntb_async_rx(struct ntb_queue_entry *entry, void *offset)
 	struct dmaengine_unmap_data *unmap;
 	dma_cookie_t cookie;
 	void *buf = entry->buf;
+	int retries = 0;
 
 	len = entry->len;
 
@@ -1263,11 +1276,21 @@ static void ntb_async_rx(struct ntb_queue_entry *entry, void *offset)
 
 	unmap->from_cnt = 1;
 
-	txd = device->device_prep_dma_memcpy(chan, unmap->addr[1],
-					     unmap->addr[0], len,
-					     DMA_PREP_INTERRUPT);
-	if (!txd)
+	for (retries = 0; retries < DMA_RETRIES; retries++) {
+		txd = device->device_prep_dma_memcpy(chan, unmap->addr[1],
+						     unmap->addr[0], len,
+						     DMA_PREP_INTERRUPT);
+		if (txd)
+			break;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(DMA_OUT_RESOURCE_TO);
+	}
+
+	if (!txd) {
+		qp->dma_rx_prep_err++;
 		goto err_get_unmap;
+	}
 
 	txd->callback = ntb_rx_copy_callback;
 	txd->callback_param = entry;
@@ -1460,6 +1483,7 @@ static void ntb_async_tx(struct ntb_transport_qp *qp,
 	void __iomem *offset;
 	size_t len = entry->len;
 	void *buf = entry->buf;
+	int retries = 0;
 
 	offset = qp->tx_mw + qp->tx_max_frame * qp->tx_index;
 	hdr = offset + qp->tx_max_frame - sizeof(struct ntb_payload_header);
@@ -1494,10 +1518,20 @@ static void ntb_async_tx(struct ntb_transport_qp *qp,
 
 	unmap->to_cnt = 1;
 
-	txd = device->device_prep_dma_memcpy(chan, dest, unmap->addr[0], len,
-					     DMA_PREP_INTERRUPT);
-	if (!txd)
+	for (retries = 0; retries < DMA_RETRIES; retries++) {
+		txd = device->device_prep_dma_memcpy(chan, dest, unmap->addr[0],
+						     len, DMA_PREP_INTERRUPT);
+		if (txd)
+			break;
+
+		set_current_state(TASK_INTERRUPTIBLE);
+		schedule_timeout(DMA_OUT_RESOURCE_TO);
+	}
+
+	if (!txd) {
+		qp->dma_tx_prep_err++;
 		goto err_get_unmap;
+	}
 
 	txd->callback = ntb_tx_copy_callback;
 	txd->callback_param = entry;
@@ -1532,7 +1566,7 @@ static int ntb_process_tx(struct ntb_transport_qp *qp,
 
 	if (entry->len > qp->tx_max_frame - sizeof(struct ntb_payload_header)) {
 		if (qp->tx_handler)
-			qp->tx_handler(qp->cb_data, qp, NULL, -EIO);
+			qp->tx_handler(qp, qp->cb_data, NULL, -EIO);
 
 		ntb_list_add(&qp->ntb_tx_free_q_lock, &entry->entry,
 			     &qp->tx_free_q);
diff --git a/drivers/ntb/test/Kconfig b/drivers/ntb/test/Kconfig
index 01852f98a843..a5d0eda44438 100644
--- a/drivers/ntb/test/Kconfig
+++ b/drivers/ntb/test/Kconfig
@@ -17,3 +17,11 @@ config NTB_TOOL
 	 functioning at a basic level.
 
 	 If unsure, say N.
+
+config NTB_PERF
+	tristate "NTB RAW Perf Measuring Tool"
+	help
+	 This is a tool to measure raw NTB performance by transferring data
+	 to and from the window without additional software interaction.
+
+	 If unsure, say N.
diff --git a/drivers/ntb/test/Makefile b/drivers/ntb/test/Makefile
index 0ea32a324b6c..9e77e0b761c2 100644
--- a/drivers/ntb/test/Makefile
+++ b/drivers/ntb/test/Makefile
@@ -1,2 +1,3 @@
 obj-$(CONFIG_NTB_PINGPONG) += ntb_pingpong.o
 obj-$(CONFIG_NTB_TOOL) += ntb_tool.o
+obj-$(CONFIG_NTB_PERF) += ntb_perf.o
diff --git a/drivers/ntb/test/ntb_perf.c b/drivers/ntb/test/ntb_perf.c
new file mode 100644
index 000000000000..c8a37ba4b4f9
--- /dev/null
+++ b/drivers/ntb/test/ntb_perf.c
@@ -0,0 +1,748 @@
+/*
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ *   redistributing this file, you may do so under either license.
+ *
+ *   GPL LICENSE SUMMARY
+ *
+ *   Copyright(c) 2015 Intel Corporation. All rights reserved.
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of version 2 of the GNU General Public License as
+ *   published by the Free Software Foundation.
+ *
+ *   BSD LICENSE
+ *
+ *   Copyright(c) 2015 Intel Corporation. All rights reserved.
+ *
+ *   Redistribution and use in source and binary forms, with or without
+ *   modification, are permitted provided that the following conditions
+ *   are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copy
+ *       notice, this list of conditions and the following disclaimer in
+ *       the documentation and/or other materials provided with the
+ *       distribution.
+ *     * Neither the name of Intel Corporation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ *   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ *   OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ *   DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ *   THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ *   (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *   OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *   PCIe NTB Perf Linux driver
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/kthread.h>
+#include <linux/time.h>
+#include <linux/timer.h>
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/debugfs.h>
+#include <linux/dmaengine.h>
+#include <linux/delay.h>
+#include <linux/sizes.h>
+#include <linux/ntb.h>
+
+#define DRIVER_NAME		"ntb_perf"
+#define DRIVER_DESCRIPTION	"PCIe NTB Performance Measurement Tool"
+
+#define DRIVER_LICENSE		"Dual BSD/GPL"
+#define DRIVER_VERSION		"1.0"
+#define DRIVER_AUTHOR		"Dave Jiang <dave.jiang@intel.com>"
+
+#define PERF_LINK_DOWN_TIMEOUT	10
+#define PERF_VERSION		0xffff0001
+#define MAX_THREADS		32
+#define MAX_TEST_SIZE		SZ_1M
+#define MAX_SRCS		32
+#define DMA_OUT_RESOURCE_TO	50
+#define DMA_RETRIES		20
+#define SZ_4G			(1ULL << 32)
+#define MAX_SEG_ORDER		20 /* no larger than 1M for kmalloc buffer */
+
+MODULE_LICENSE(DRIVER_LICENSE);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+
+static struct dentry *perf_debugfs_dir;
+
+static unsigned int seg_order = 19; /* 512K */
+module_param(seg_order, uint, 0644);
+MODULE_PARM_DESC(seg_order, "size order [n^2] of buffer segment for testing");
+
+static unsigned int run_order = 32; /* 4G */
+module_param(run_order, uint, 0644);
+MODULE_PARM_DESC(run_order, "size order [n^2] of total data to transfer");
+
+static bool use_dma; /* default to 0 */
+module_param(use_dma, bool, 0644);
+MODULE_PARM_DESC(use_dma, "Using DMA engine to measure performance");
+
+struct perf_mw {
+	phys_addr_t	phys_addr;
+	resource_size_t	phys_size;
+	resource_size_t	xlat_align;
+	resource_size_t	xlat_align_size;
+	void __iomem	*vbase;
+	size_t		xlat_size;
+	size_t		buf_size;
+	void		*virt_addr;
+	dma_addr_t	dma_addr;
+};
+
+struct perf_ctx;
+
+struct pthr_ctx {
+	struct task_struct	*thread;
+	struct perf_ctx		*perf;
+	atomic_t		dma_sync;
+	struct dma_chan		*dma_chan;
+	int			dma_prep_err;
+	int			src_idx;
+	void			*srcs[MAX_SRCS];
+};
+
+struct perf_ctx {
+	struct ntb_dev		*ntb;
+	spinlock_t		db_lock;
+	struct perf_mw		mw;
+	bool			link_is_up;
+	struct work_struct	link_cleanup;
+	struct delayed_work	link_work;
+	struct dentry		*debugfs_node_dir;
+	struct dentry		*debugfs_run;
+	struct dentry		*debugfs_threads;
+	u8			perf_threads;
+	bool			run;
+	struct pthr_ctx		pthr_ctx[MAX_THREADS];
+	atomic_t		tsync;
+};
+
+enum {
+	VERSION = 0,
+	MW_SZ_HIGH,
+	MW_SZ_LOW,
+	SPAD_MSG,
+	SPAD_ACK,
+	MAX_SPAD
+};
+
+static void perf_link_event(void *ctx)
+{
+	struct perf_ctx *perf = ctx;
+
+	if (ntb_link_is_up(perf->ntb, NULL, NULL) == 1)
+		schedule_delayed_work(&perf->link_work, 2*HZ);
+	else
+		schedule_work(&perf->link_cleanup);
+}
+
+static void perf_db_event(void *ctx, int vec)
+{
+	struct perf_ctx *perf = ctx;
+	u64 db_bits, db_mask;
+
+	db_mask = ntb_db_vector_mask(perf->ntb, vec);
+	db_bits = ntb_db_read(perf->ntb);
+
+	dev_dbg(&perf->ntb->dev, "doorbell vec %d mask %#llx bits %#llx\n",
+		vec, db_mask, db_bits);
+}
+
+static const struct ntb_ctx_ops perf_ops = {
+	.link_event = perf_link_event,
+	.db_event = perf_db_event,
+};
+
+static void perf_copy_callback(void *data)
+{
+	struct pthr_ctx *pctx = data;
+
+	atomic_dec(&pctx->dma_sync);
+}
+
+static ssize_t perf_copy(struct pthr_ctx *pctx, char *dst,
+			 char *src, size_t size)
+{
+	struct perf_ctx *perf = pctx->perf;
+	struct dma_async_tx_descriptor *txd;
+	struct dma_chan *chan = pctx->dma_chan;
+	struct dma_device *device;
+	struct dmaengine_unmap_data *unmap;
+	dma_cookie_t cookie;
+	size_t src_off, dst_off;
+	struct perf_mw *mw = &perf->mw;
+	u64 vbase, dst_vaddr;
+	dma_addr_t dst_phys;
+	int retries = 0;
+
+	if (!use_dma) {
+		memcpy_toio(dst, src, size);
+		return size;
+	}
+
+	if (!chan) {
+		dev_err(&perf->ntb->dev, "DMA engine does not exist\n");
+		return -EINVAL;
+	}
+
+	device = chan->device;
+	src_off = (size_t)src & ~PAGE_MASK;
+	dst_off = (size_t)dst & ~PAGE_MASK;
+
+	if (!is_dma_copy_aligned(device, src_off, dst_off, size))
+		return -ENODEV;
+
+	vbase = (u64)(u64 *)mw->vbase;
+	dst_vaddr = (u64)(u64 *)dst;
+	dst_phys = mw->phys_addr + (dst_vaddr - vbase);
+
+	unmap = dmaengine_get_unmap_data(device->dev, 1, GFP_NOWAIT);
+	if (!unmap)
+		return -ENOMEM;
+
+	unmap->len = size;
+	unmap->addr[0] = dma_map_page(device->dev, virt_to_page(src),
+				      src_off, size, DMA_TO_DEVICE);
+	if (dma_mapping_error(device->dev, unmap->addr[0]))
+		goto err_get_unmap;
+
+	unmap->to_cnt = 1;
+
+	do {
+		txd = device->device_prep_dma_memcpy(chan, dst_phys,
+						     unmap->addr[0],
+						     size, DMA_PREP_INTERRUPT);
+		if (!txd) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(DMA_OUT_RESOURCE_TO);
+		}
+	} while (!txd && (++retries < DMA_RETRIES));
+
+	if (!txd) {
+		pctx->dma_prep_err++;
+		goto err_get_unmap;
+	}
+
+	txd->callback = perf_copy_callback;
+	txd->callback_param = pctx;
+	dma_set_unmap(txd, unmap);
+
+	cookie = dmaengine_submit(txd);
+	if (dma_submit_error(cookie))
+		goto err_set_unmap;
+
+	atomic_inc(&pctx->dma_sync);
+	dma_async_issue_pending(chan);
+
+	return size;
+
+err_set_unmap:
+	dmaengine_unmap_put(unmap);
+err_get_unmap:
+	dmaengine_unmap_put(unmap);
+	return 0;
+}
+
+static int perf_move_data(struct pthr_ctx *pctx, char *dst, char *src,
+			  u64 buf_size, u64 win_size, u64 total)
+{
+	int chunks, total_chunks, i;
+	int copied_chunks = 0;
+	u64 copied = 0, result;
+	char *tmp = dst;
+	u64 perf, diff_us;
+	ktime_t kstart, kstop, kdiff;
+
+	chunks = div64_u64(win_size, buf_size);
+	total_chunks = div64_u64(total, buf_size);
+	kstart = ktime_get();
+
+	for (i = 0; i < total_chunks; i++) {
+		result = perf_copy(pctx, tmp, src, buf_size);
+		copied += result;
+		copied_chunks++;
+		if (copied_chunks == chunks) {
+			tmp = dst;
+			copied_chunks = 0;
+		} else
+			tmp += buf_size;
+
+		/* Probably should schedule every 4GB to prevent soft hang. */
+		if (((copied % SZ_4G) == 0) && !use_dma) {
+			set_current_state(TASK_INTERRUPTIBLE);
+			schedule_timeout(1);
+		}
+	}
+
+	if (use_dma) {
+		pr_info("%s: All DMA descriptors submitted\n", current->comm);
+		while (atomic_read(&pctx->dma_sync) != 0)
+			msleep(20);
+	}
+
+	kstop = ktime_get();
+	kdiff = ktime_sub(kstop, kstart);
+	diff_us = ktime_to_us(kdiff);
+
+	pr_info("%s: copied %llu bytes\n", current->comm, copied);
+
+	pr_info("%s: lasted %llu usecs\n", current->comm, diff_us);
+
+	perf = div64_u64(copied, diff_us);
+
+	pr_info("%s: MBytes/s: %llu\n", current->comm, perf);
+
+	return 0;
+}
+
+static bool perf_dma_filter_fn(struct dma_chan *chan, void *node)
+{
+	return dev_to_node(&chan->dev->device) == (int)(unsigned long)node;
+}
+
+static int ntb_perf_thread(void *data)
+{
+	struct pthr_ctx *pctx = data;
+	struct perf_ctx *perf = pctx->perf;
+	struct pci_dev *pdev = perf->ntb->pdev;
+	struct perf_mw *mw = &perf->mw;
+	char *dst;
+	u64 win_size, buf_size, total;
+	void *src;
+	int rc, node, i;
+	struct dma_chan *dma_chan = NULL;
+
+	pr_info("kthread %s starting...\n", current->comm);
+
+	node = dev_to_node(&pdev->dev);
+
+	if (use_dma && !pctx->dma_chan) {
+		dma_cap_mask_t dma_mask;
+
+		dma_cap_zero(dma_mask);
+		dma_cap_set(DMA_MEMCPY, dma_mask);
+		dma_chan = dma_request_channel(dma_mask, perf_dma_filter_fn,
+					       (void *)(unsigned long)node);
+		if (!dma_chan) {
+			pr_warn("%s: cannot acquire DMA channel, quitting\n",
+				current->comm);
+			return -ENODEV;
+		}
+		pctx->dma_chan = dma_chan;
+	}
+
+	for (i = 0; i < MAX_SRCS; i++) {
+		pctx->srcs[i] = kmalloc_node(MAX_TEST_SIZE, GFP_KERNEL, node);
+		if (!pctx->srcs[i]) {
+			rc = -ENOMEM;
+			goto err;
+		}
+	}
+
+	win_size = mw->phys_size;
+	buf_size = 1ULL << seg_order;
+	total = 1ULL << run_order;
+
+	if (buf_size > MAX_TEST_SIZE)
+		buf_size = MAX_TEST_SIZE;
+
+	dst = (char *)mw->vbase;
+
+	atomic_inc(&perf->tsync);
+	while (atomic_read(&perf->tsync) != perf->perf_threads)
+		schedule();
+
+	src = pctx->srcs[pctx->src_idx];
+	pctx->src_idx = (pctx->src_idx + 1) & (MAX_SRCS - 1);
+
+	rc = perf_move_data(pctx, dst, src, buf_size, win_size, total);
+
+	atomic_dec(&perf->tsync);
+
+	if (rc < 0) {
+		pr_err("%s: failed\n", current->comm);
+		rc = -ENXIO;
+		goto err;
+	}
+
+	for (i = 0; i < MAX_SRCS; i++) {
+		kfree(pctx->srcs[i]);
+		pctx->srcs[i] = NULL;
+	}
+
+	return 0;
+
+err:
+	for (i = 0; i < MAX_SRCS; i++) {
+		kfree(pctx->srcs[i]);
+		pctx->srcs[i] = NULL;
+	}
+
+	if (dma_chan) {
+		dma_release_channel(dma_chan);
+		pctx->dma_chan = NULL;
+	}
+
+	return rc;
+}
+
+static void perf_free_mw(struct perf_ctx *perf)
+{
+	struct perf_mw *mw = &perf->mw;
+	struct pci_dev *pdev = perf->ntb->pdev;
+
+	if (!mw->virt_addr)
+		return;
+
+	ntb_mw_clear_trans(perf->ntb, 0);
+	dma_free_coherent(&pdev->dev, mw->buf_size,
+			  mw->virt_addr, mw->dma_addr);
+	mw->xlat_size = 0;
+	mw->buf_size = 0;
+	mw->virt_addr = NULL;
+}
+
+static int perf_set_mw(struct perf_ctx *perf, resource_size_t size)
+{
+	struct perf_mw *mw = &perf->mw;
+	size_t xlat_size, buf_size;
+
+	if (!size)
+		return -EINVAL;
+
+	xlat_size = round_up(size, mw->xlat_align_size);
+	buf_size = round_up(size, mw->xlat_align);
+
+	if (mw->xlat_size == xlat_size)
+		return 0;
+
+	if (mw->buf_size)
+		perf_free_mw(perf);
+
+	mw->xlat_size = xlat_size;
+	mw->buf_size = buf_size;
+
+	mw->virt_addr = dma_alloc_coherent(&perf->ntb->pdev->dev, buf_size,
+					   &mw->dma_addr, GFP_KERNEL);
+	if (!mw->virt_addr) {
+		mw->xlat_size = 0;
+		mw->buf_size = 0;
+	}
+
+	return 0;
+}
+
+static void perf_link_work(struct work_struct *work)
+{
+	struct perf_ctx *perf =
+		container_of(work, struct perf_ctx, link_work.work);
+	struct ntb_dev *ndev = perf->ntb;
+	struct pci_dev *pdev = ndev->pdev;
+	u32 val;
+	u64 size;
+	int rc;
+
+	dev_dbg(&perf->ntb->pdev->dev, "%s called\n", __func__);
+
+	size = perf->mw.phys_size;
+	ntb_peer_spad_write(ndev, MW_SZ_HIGH, upper_32_bits(size));
+	ntb_peer_spad_write(ndev, MW_SZ_LOW, lower_32_bits(size));
+	ntb_peer_spad_write(ndev, VERSION, PERF_VERSION);
+
+	/* now read what peer wrote */
+	val = ntb_spad_read(ndev, VERSION);
+	if (val != PERF_VERSION) {
+		dev_dbg(&pdev->dev, "Remote version = %#x\n", val);
+		goto out;
+	}
+
+	val = ntb_spad_read(ndev, MW_SZ_HIGH);
+	size = (u64)val << 32;
+
+	val = ntb_spad_read(ndev, MW_SZ_LOW);
+	size |= val;
+
+	dev_dbg(&pdev->dev, "Remote MW size = %#llx\n", size);
+
+	rc = perf_set_mw(perf, size);
+	if (rc)
+		goto out1;
+
+	perf->link_is_up = true;
+
+	return;
+
+out1:
+	perf_free_mw(perf);
+
+out:
+	if (ntb_link_is_up(ndev, NULL, NULL) == 1)
+		schedule_delayed_work(&perf->link_work,
+				      msecs_to_jiffies(PERF_LINK_DOWN_TIMEOUT));
+}
+
+static void perf_link_cleanup(struct work_struct *work)
+{
+	struct perf_ctx *perf = container_of(work,
+					     struct perf_ctx,
+					     link_cleanup);
+
+	dev_dbg(&perf->ntb->pdev->dev, "%s called\n", __func__);
+
+	if (!perf->link_is_up)
+		cancel_delayed_work_sync(&perf->link_work);
+}
+
+static int perf_setup_mw(struct ntb_dev *ntb, struct perf_ctx *perf)
+{
+	struct perf_mw *mw;
+	int rc;
+
+	mw = &perf->mw;
+
+	rc = ntb_mw_get_range(ntb, 0, &mw->phys_addr, &mw->phys_size,
+			      &mw->xlat_align, &mw->xlat_align_size);
+	if (rc)
+		return rc;
+
+	perf->mw.vbase = ioremap_wc(mw->phys_addr, mw->phys_size);
+	if (!mw->vbase)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static ssize_t debugfs_run_read(struct file *filp, char __user *ubuf,
+				size_t count, loff_t *offp)
+{
+	struct perf_ctx *perf = filp->private_data;
+	char *buf;
+	ssize_t ret, out_offset;
+
+	if (!perf)
+		return 0;
+
+	buf = kmalloc(64, GFP_KERNEL);
+	out_offset = snprintf(buf, 64, "%d\n", perf->run);
+	ret = simple_read_from_buffer(ubuf, count, offp, buf, out_offset);
+	kfree(buf);
+
+	return ret;
+}
+
+static ssize_t debugfs_run_write(struct file *filp, const char __user *ubuf,
+				 size_t count, loff_t *offp)
+{
+	struct perf_ctx *perf = filp->private_data;
+	int node, i;
+
+	if (!perf->link_is_up)
+		return 0;
+
+	if (perf->perf_threads == 0)
+		return 0;
+
+	if (atomic_read(&perf->tsync) == 0)
+		perf->run = false;
+
+	if (perf->run) {
+		/* lets stop the threads */
+		perf->run = false;
+		for (i = 0; i < MAX_THREADS; i++) {
+			if (perf->pthr_ctx[i].thread) {
+				kthread_stop(perf->pthr_ctx[i].thread);
+				perf->pthr_ctx[i].thread = NULL;
+			} else
+				break;
+		}
+	} else {
+		perf->run = true;
+
+		if (perf->perf_threads > MAX_THREADS) {
+			perf->perf_threads = MAX_THREADS;
+			pr_info("Reset total threads to: %u\n", MAX_THREADS);
+		}
+
+		/* no greater than 1M */
+		if (seg_order > MAX_SEG_ORDER) {
+			seg_order = MAX_SEG_ORDER;
+			pr_info("Fix seg_order to %u\n", seg_order);
+		}
+
+		if (run_order < seg_order) {
+			run_order = seg_order;
+			pr_info("Fix run_order to %u\n", run_order);
+		}
+
+		node = dev_to_node(&perf->ntb->pdev->dev);
+		/* launch kernel thread */
+		for (i = 0; i < perf->perf_threads; i++) {
+			struct pthr_ctx *pctx;
+
+			pctx = &perf->pthr_ctx[i];
+			atomic_set(&pctx->dma_sync, 0);
+			pctx->perf = perf;
+			pctx->thread =
+				kthread_create_on_node(ntb_perf_thread,
+						       (void *)pctx,
+						       node, "ntb_perf %d", i);
+			if (pctx->thread)
+				wake_up_process(pctx->thread);
+			else {
+				perf->run = false;
+				for (i = 0; i < MAX_THREADS; i++) {
+					if (pctx->thread) {
+						kthread_stop(pctx->thread);
+						pctx->thread = NULL;
+					}
+				}
+			}
+
+			if (perf->run == false)
+				return -ENXIO;
+		}
+
+	}
+
+	return count;
+}
+
+static const struct file_operations ntb_perf_debugfs_run = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = debugfs_run_read,
+	.write = debugfs_run_write,
+};
+
+static int perf_debugfs_setup(struct perf_ctx *perf)
+{
+	struct pci_dev *pdev = perf->ntb->pdev;
+
+	if (!debugfs_initialized())
+		return -ENODEV;
+
+	if (!perf_debugfs_dir) {
+		perf_debugfs_dir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+		if (!perf_debugfs_dir)
+			return -ENODEV;
+	}
+
+	perf->debugfs_node_dir = debugfs_create_dir(pci_name(pdev),
+						    perf_debugfs_dir);
+	if (!perf->debugfs_node_dir)
+		return -ENODEV;
+
+	perf->debugfs_run = debugfs_create_file("run", S_IRUSR | S_IWUSR,
+						perf->debugfs_node_dir, perf,
+						&ntb_perf_debugfs_run);
+	if (!perf->debugfs_run)
+		return -ENODEV;
+
+	perf->debugfs_threads = debugfs_create_u8("threads", S_IRUSR | S_IWUSR,
+						  perf->debugfs_node_dir,
+						  &perf->perf_threads);
+	if (!perf->debugfs_threads)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int perf_probe(struct ntb_client *client, struct ntb_dev *ntb)
+{
+	struct pci_dev *pdev = ntb->pdev;
+	struct perf_ctx *perf;
+	int node;
+	int rc = 0;
+
+	node = dev_to_node(&pdev->dev);
+
+	perf = kzalloc_node(sizeof(*perf), GFP_KERNEL, node);
+	if (!perf) {
+		rc = -ENOMEM;
+		goto err_perf;
+	}
+
+	perf->ntb = ntb;
+	perf->perf_threads = 1;
+	atomic_set(&perf->tsync, 0);
+	perf->run = false;
+	spin_lock_init(&perf->db_lock);
+	perf_setup_mw(ntb, perf);
+	INIT_DELAYED_WORK(&perf->link_work, perf_link_work);
+	INIT_WORK(&perf->link_cleanup, perf_link_cleanup);
+
+	rc = ntb_set_ctx(ntb, perf, &perf_ops);
+	if (rc)
+		goto err_ctx;
+
+	perf->link_is_up = false;
+	ntb_link_enable(ntb, NTB_SPEED_AUTO, NTB_WIDTH_AUTO);
+	ntb_link_event(ntb);
+
+	rc = perf_debugfs_setup(perf);
+	if (rc)
+		goto err_ctx;
+
+	return 0;
+
+err_ctx:
+	cancel_delayed_work_sync(&perf->link_work);
+	cancel_work_sync(&perf->link_cleanup);
+	kfree(perf);
+err_perf:
+	return rc;
+}
+
+static void perf_remove(struct ntb_client *client, struct ntb_dev *ntb)
+{
+	struct perf_ctx *perf = ntb->ctx;
+	int i;
+
+	dev_dbg(&perf->ntb->dev, "%s called\n", __func__);
+
+	cancel_delayed_work_sync(&perf->link_work);
+	cancel_work_sync(&perf->link_cleanup);
+
+	ntb_clear_ctx(ntb);
+	ntb_link_disable(ntb);
+
+	debugfs_remove_recursive(perf_debugfs_dir);
+	perf_debugfs_dir = NULL;
+
+	if (use_dma) {
+		for (i = 0; i < MAX_THREADS; i++) {
+			struct pthr_ctx *pctx = &perf->pthr_ctx[i];
+
+			if (pctx->dma_chan)
+				dma_release_channel(pctx->dma_chan);
+		}
+	}
+
+	kfree(perf);
+}
+
+static struct ntb_client perf_client = {
+	.ops = {
+		.probe = perf_probe,
+		.remove = perf_remove,
+	},
+};
+module_ntb_client(perf_client);
diff --git a/drivers/nvdimm/bus.c b/drivers/nvdimm/bus.c
index 7e2c43f701bc..5d28e9405f32 100644
--- a/drivers/nvdimm/bus.c
+++ b/drivers/nvdimm/bus.c
@@ -382,18 +382,18 @@ static const struct nd_cmd_desc __nd_cmd_bus_descs[] = {
 	[ND_CMD_ARS_CAP] = {
 		.in_num = 2,
 		.in_sizes = { 8, 8, },
-		.out_num = 2,
-		.out_sizes = { 4, 4, },
+		.out_num = 4,
+		.out_sizes = { 4, 4, 4, 4, },
 	},
 	[ND_CMD_ARS_START] = {
-		.in_num = 4,
-		.in_sizes = { 8, 8, 2, 6, },
-		.out_num = 1,
-		.out_sizes = { 4, },
+		.in_num = 5,
+		.in_sizes = { 8, 8, 2, 1, 5, },
+		.out_num = 2,
+		.out_sizes = { 4, 4, },
 	},
 	[ND_CMD_ARS_STATUS] = {
-		.out_num = 2,
-		.out_sizes = { 4, UINT_MAX, },
+		.out_num = 3,
+		.out_sizes = { 4, 4, UINT_MAX, },
 	},
 };
 
@@ -442,8 +442,8 @@ u32 nd_cmd_out_size(struct nvdimm *nvdimm, int cmd,
 		return in_field[1];
 	else if (nvdimm && cmd == ND_CMD_VENDOR && idx == 2)
 		return out_field[1];
-	else if (!nvdimm && cmd == ND_CMD_ARS_STATUS && idx == 1)
-		return ND_CMD_ARS_STATUS_MAX;
+	else if (!nvdimm && cmd == ND_CMD_ARS_STATUS && idx == 2)
+		return out_field[1] - 8;
 
 	return UINT_MAX;
 }
diff --git a/drivers/nvdimm/core.c b/drivers/nvdimm/core.c
index 82c49bb87055..2e2832b83c93 100644
--- a/drivers/nvdimm/core.c
+++ b/drivers/nvdimm/core.c
@@ -11,6 +11,7 @@
  * General Public License for more details.
  */
 #include <linux/libnvdimm.h>
+#include <linux/badblocks.h>
 #include <linux/export.h>
 #include <linux/module.h>
 #include <linux/blkdev.h>
@@ -325,6 +326,7 @@ struct nvdimm_bus *__nvdimm_bus_register(struct device *parent,
 	if (!nvdimm_bus)
 		return NULL;
 	INIT_LIST_HEAD(&nvdimm_bus->list);
+	INIT_LIST_HEAD(&nvdimm_bus->poison_list);
 	init_waitqueue_head(&nvdimm_bus->probe_wait);
 	nvdimm_bus->id = ida_simple_get(&nd_ida, 0, 0, GFP_KERNEL);
 	mutex_init(&nvdimm_bus->reconfig_mutex);
@@ -359,6 +361,172 @@ struct nvdimm_bus *__nvdimm_bus_register(struct device *parent,
 }
 EXPORT_SYMBOL_GPL(__nvdimm_bus_register);
 
+static void set_badblock(struct badblocks *bb, sector_t s, int num)
+{
+	dev_dbg(bb->dev, "Found a poison range (0x%llx, 0x%llx)\n",
+			(u64) s * 512, (u64) num * 512);
+	/* this isn't an error as the hardware will still throw an exception */
+	if (badblocks_set(bb, s, num, 1))
+		dev_info_once(bb->dev, "%s: failed for sector %llx\n",
+				__func__, (u64) s);
+}
+
+/**
+ * __add_badblock_range() - Convert a physical address range to bad sectors
+ * @bb:		badblocks instance to populate
+ * @ns_offset:	namespace offset where the error range begins (in bytes)
+ * @len:	number of bytes of poison to be added
+ *
+ * This assumes that the range provided with (ns_offset, len) is within
+ * the bounds of physical addresses for this namespace, i.e. lies in the
+ * interval [ns_start, ns_start + ns_size)
+ */
+static void __add_badblock_range(struct badblocks *bb, u64 ns_offset, u64 len)
+{
+	const unsigned int sector_size = 512;
+	sector_t start_sector;
+	u64 num_sectors;
+	u32 rem;
+
+	start_sector = div_u64(ns_offset, sector_size);
+	num_sectors = div_u64_rem(len, sector_size, &rem);
+	if (rem)
+		num_sectors++;
+
+	if (unlikely(num_sectors > (u64)INT_MAX)) {
+		u64 remaining = num_sectors;
+		sector_t s = start_sector;
+
+		while (remaining) {
+			int done = min_t(u64, remaining, INT_MAX);
+
+			set_badblock(bb, s, done);
+			remaining -= done;
+			s += done;
+		}
+	} else
+		set_badblock(bb, start_sector, num_sectors);
+}
+
+/**
+ * nvdimm_namespace_add_poison() - Convert a list of poison ranges to badblocks
+ * @ndns:	the namespace containing poison ranges
+ * @bb:		badblocks instance to populate
+ * @offset:	offset at the start of the namespace before 'sector 0'
+ *
+ * The poison list generated during NFIT initialization may contain multiple,
+ * possibly overlapping ranges in the SPA (System Physical Address) space.
+ * Compare each of these ranges to the namespace currently being initialized,
+ * and add badblocks to the gendisk for all matching sub-ranges
+ */
+void nvdimm_namespace_add_poison(struct nd_namespace_common *ndns,
+		struct badblocks *bb, resource_size_t offset)
+{
+	struct nd_namespace_io *nsio = to_nd_namespace_io(&ndns->dev);
+	struct nd_region *nd_region = to_nd_region(ndns->dev.parent);
+	struct nvdimm_bus *nvdimm_bus;
+	struct list_head *poison_list;
+	u64 ns_start, ns_end, ns_size;
+	struct nd_poison *pl;
+
+	ns_size = nvdimm_namespace_capacity(ndns) - offset;
+	ns_start = nsio->res.start + offset;
+	ns_end = nsio->res.end;
+
+	nvdimm_bus = to_nvdimm_bus(nd_region->dev.parent);
+	poison_list = &nvdimm_bus->poison_list;
+	if (list_empty(poison_list))
+		return;
+
+	list_for_each_entry(pl, poison_list, list) {
+		u64 pl_end = pl->start + pl->length - 1;
+
+		/* Discard intervals with no intersection */
+		if (pl_end < ns_start)
+			continue;
+		if (pl->start > ns_end)
+			continue;
+		/* Deal with any overlap after start of the namespace */
+		if (pl->start >= ns_start) {
+			u64 start = pl->start;
+			u64 len;
+
+			if (pl_end <= ns_end)
+				len = pl->length;
+			else
+				len = ns_start + ns_size - pl->start;
+			__add_badblock_range(bb, start - ns_start, len);
+			continue;
+		}
+		/* Deal with overlap for poison starting before the namespace */
+		if (pl->start < ns_start) {
+			u64 len;
+
+			if (pl_end < ns_end)
+				len = pl->start + pl->length - ns_start;
+			else
+				len = ns_size;
+			__add_badblock_range(bb, 0, len);
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(nvdimm_namespace_add_poison);
+
+static int __add_poison(struct nvdimm_bus *nvdimm_bus, u64 addr, u64 length)
+{
+	struct nd_poison *pl;
+
+	pl = kzalloc(sizeof(*pl), GFP_KERNEL);
+	if (!pl)
+		return -ENOMEM;
+
+	pl->start = addr;
+	pl->length = length;
+	list_add_tail(&pl->list, &nvdimm_bus->poison_list);
+
+	return 0;
+}
+
+int nvdimm_bus_add_poison(struct nvdimm_bus *nvdimm_bus, u64 addr, u64 length)
+{
+	struct nd_poison *pl;
+
+	if (list_empty(&nvdimm_bus->poison_list))
+		return __add_poison(nvdimm_bus, addr, length);
+
+	/*
+	 * There is a chance this is a duplicate, check for those first.
+	 * This will be the common case as ARS_STATUS returns all known
+	 * errors in the SPA space, and we can't query it per region
+	 */
+	list_for_each_entry(pl, &nvdimm_bus->poison_list, list)
+		if (pl->start == addr) {
+			/* If length has changed, update this list entry */
+			if (pl->length != length)
+				pl->length = length;
+			return 0;
+		}
+
+	/*
+	 * If not a duplicate or a simple length update, add the entry as is,
+	 * as any overlapping ranges will get resolved when the list is consumed
+	 * and converted to badblocks
+	 */
+	return __add_poison(nvdimm_bus, addr, length);
+}
+EXPORT_SYMBOL_GPL(nvdimm_bus_add_poison);
+
+static void free_poison_list(struct list_head *poison_list)
+{
+	struct nd_poison *pl, *next;
+
+	list_for_each_entry_safe(pl, next, poison_list, list) {
+		list_del(&pl->list);
+		kfree(pl);
+	}
+	list_del_init(poison_list);
+}
+
 static int child_unregister(struct device *dev, void *data)
 {
 	/*
@@ -385,6 +553,7 @@ void nvdimm_bus_unregister(struct nvdimm_bus *nvdimm_bus)
 
 	nd_synchronize();
 	device_for_each_child(&nvdimm_bus->dev, NULL, child_unregister);
+	free_poison_list(&nvdimm_bus->poison_list);
 	nvdimm_bus_destroy_ndctl(nvdimm_bus);
 
 	device_unregister(&nvdimm_bus->dev);
diff --git a/drivers/nvdimm/namespace_devs.c b/drivers/nvdimm/namespace_devs.c
index 0955b2cb10fe..9edf7eb7d17c 100644
--- a/drivers/nvdimm/namespace_devs.c
+++ b/drivers/nvdimm/namespace_devs.c
@@ -77,6 +77,59 @@ static bool is_namespace_io(struct device *dev)
 	return dev ? dev->type == &namespace_io_device_type : false;
 }
 
+static int is_uuid_busy(struct device *dev, void *data)
+{
+	u8 *uuid1 = data, *uuid2 = NULL;
+
+	if (is_namespace_pmem(dev)) {
+		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
+
+		uuid2 = nspm->uuid;
+	} else if (is_namespace_blk(dev)) {
+		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
+
+		uuid2 = nsblk->uuid;
+	} else if (is_nd_btt(dev)) {
+		struct nd_btt *nd_btt = to_nd_btt(dev);
+
+		uuid2 = nd_btt->uuid;
+	} else if (is_nd_pfn(dev)) {
+		struct nd_pfn *nd_pfn = to_nd_pfn(dev);
+
+		uuid2 = nd_pfn->uuid;
+	}
+
+	if (uuid2 && memcmp(uuid1, uuid2, NSLABEL_UUID_LEN) == 0)
+		return -EBUSY;
+
+	return 0;
+}
+
+static int is_namespace_uuid_busy(struct device *dev, void *data)
+{
+	if (is_nd_pmem(dev) || is_nd_blk(dev))
+		return device_for_each_child(dev, data, is_uuid_busy);
+	return 0;
+}
+
+/**
+ * nd_is_uuid_unique - verify that no other namespace has @uuid
+ * @dev: any device on a nvdimm_bus
+ * @uuid: uuid to check
+ */
+bool nd_is_uuid_unique(struct device *dev, u8 *uuid)
+{
+	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
+
+	if (!nvdimm_bus)
+		return false;
+	WARN_ON_ONCE(!is_nvdimm_bus_locked(&nvdimm_bus->dev));
+	if (device_for_each_child(&nvdimm_bus->dev, uuid,
+				is_namespace_uuid_busy) != 0)
+		return false;
+	return true;
+}
+
 bool pmem_should_map_pages(struct device *dev)
 {
 	struct nd_region *nd_region = to_nd_region(dev->parent);
@@ -104,20 +157,10 @@ const char *nvdimm_namespace_disk_name(struct nd_namespace_common *ndns,
 	struct nd_region *nd_region = to_nd_region(ndns->dev.parent);
 	const char *suffix = NULL;
 
-	if (ndns->claim) {
-		if (is_nd_btt(ndns->claim))
-			suffix = "s";
-		else if (is_nd_pfn(ndns->claim))
-			suffix = "m";
-		else
-			dev_WARN_ONCE(&ndns->dev, 1,
-					"unknown claim type by %s\n",
-					dev_name(ndns->claim));
-	}
+	if (ndns->claim && is_nd_btt(ndns->claim))
+		suffix = "s";
 
 	if (is_namespace_pmem(&ndns->dev) || is_namespace_io(&ndns->dev)) {
-		if (!suffix && pmem_should_map_pages(&ndns->dev))
-			suffix = "m";
 		sprintf(name, "pmem%d%s", nd_region->id, suffix ? suffix : "");
 	} else if (is_namespace_blk(&ndns->dev)) {
 		struct nd_namespace_blk *nsblk;
@@ -791,6 +834,15 @@ static void nd_namespace_pmem_set_size(struct nd_region *nd_region,
 	res->end = nd_region->ndr_start + size - 1;
 }
 
+static bool uuid_not_set(const u8 *uuid, struct device *dev, const char *where)
+{
+	if (!uuid) {
+		dev_dbg(dev, "%s: uuid not set\n", where);
+		return true;
+	}
+	return false;
+}
+
 static ssize_t __size_store(struct device *dev, unsigned long long val)
 {
 	resource_size_t allocated = 0, available = 0;
@@ -820,8 +872,12 @@ static ssize_t __size_store(struct device *dev, unsigned long long val)
 	 * We need a uuid for the allocation-label and dimm(s) on which
 	 * to store the label.
 	 */
-	if (!uuid || nd_region->ndr_mappings == 0)
+	if (uuid_not_set(uuid, dev, __func__))
 		return -ENXIO;
+	if (nd_region->ndr_mappings == 0) {
+		dev_dbg(dev, "%s: not associated with dimm(s)\n", __func__);
+		return -ENXIO;
+	}
 
 	div_u64_rem(val, SZ_4K * nd_region->ndr_mappings, &remainder);
 	if (remainder) {
@@ -1211,6 +1267,31 @@ static ssize_t holder_show(struct device *dev,
 }
 static DEVICE_ATTR_RO(holder);
 
+static ssize_t mode_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_namespace_common *ndns = to_ndns(dev);
+	struct device *claim;
+	char *mode;
+	ssize_t rc;
+
+	device_lock(dev);
+	claim = ndns->claim;
+	if (claim && is_nd_btt(claim))
+		mode = "safe";
+	else if (claim && is_nd_pfn(claim))
+		mode = "memory";
+	else if (!claim && pmem_should_map_pages(dev))
+		mode = "memory";
+	else
+		mode = "raw";
+	rc = sprintf(buf, "%s\n", mode);
+	device_unlock(dev);
+
+	return rc;
+}
+static DEVICE_ATTR_RO(mode);
+
 static ssize_t force_raw_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
@@ -1234,6 +1315,7 @@ static DEVICE_ATTR_RW(force_raw);
 static struct attribute *nd_namespace_attributes[] = {
 	&dev_attr_nstype.attr,
 	&dev_attr_size.attr,
+	&dev_attr_mode.attr,
 	&dev_attr_uuid.attr,
 	&dev_attr_holder.attr,
 	&dev_attr_resource.attr,
@@ -1267,7 +1349,8 @@ static umode_t namespace_visible(struct kobject *kobj,
 
 	if (a == &dev_attr_nstype.attr || a == &dev_attr_size.attr
 			|| a == &dev_attr_holder.attr
-			|| a == &dev_attr_force_raw.attr)
+			|| a == &dev_attr_force_raw.attr
+			|| a == &dev_attr_mode.attr)
 		return a->mode;
 
 	return 0;
@@ -1343,14 +1426,19 @@ struct nd_namespace_common *nvdimm_namespace_common_probe(struct device *dev)
 		struct nd_namespace_pmem *nspm;
 
 		nspm = to_nd_namespace_pmem(&ndns->dev);
-		if (!nspm->uuid) {
-			dev_dbg(&ndns->dev, "%s: uuid not set\n", __func__);
+		if (uuid_not_set(nspm->uuid, &ndns->dev, __func__))
 			return ERR_PTR(-ENODEV);
-		}
 	} else if (is_namespace_blk(&ndns->dev)) {
 		struct nd_namespace_blk *nsblk;
 
 		nsblk = to_nd_namespace_blk(&ndns->dev);
+		if (uuid_not_set(nsblk->uuid, &ndns->dev, __func__))
+			return ERR_PTR(-ENODEV);
+		if (!nsblk->lbasize) {
+			dev_dbg(&ndns->dev, "%s: sector size not set\n",
+				__func__);
+			return ERR_PTR(-ENODEV);
+		}
 		if (!nd_namespace_blk_validate(nsblk))
 			return ERR_PTR(-ENODEV);
 	}
@@ -1689,6 +1777,18 @@ void nd_region_create_blk_seed(struct nd_region *nd_region)
 		nd_device_register(nd_region->ns_seed);
 }
 
+void nd_region_create_pfn_seed(struct nd_region *nd_region)
+{
+	WARN_ON(!is_nvdimm_bus_locked(&nd_region->dev));
+	nd_region->pfn_seed = nd_pfn_create(nd_region);
+	/*
+	 * Seed creation failures are not fatal, provisioning is simply
+	 * disabled until memory becomes available
+	 */
+	if (!nd_region->pfn_seed)
+		dev_err(&nd_region->dev, "failed to create pfn namespace\n");
+}
+
 void nd_region_create_btt_seed(struct nd_region *nd_region)
 {
 	WARN_ON(!is_nvdimm_bus_locked(&nd_region->dev));
diff --git a/drivers/nvdimm/nd-core.h b/drivers/nvdimm/nd-core.h
index 159aed532042..1d1500f3d8b5 100644
--- a/drivers/nvdimm/nd-core.h
+++ b/drivers/nvdimm/nd-core.h
@@ -30,6 +30,7 @@ struct nvdimm_bus {
 	struct list_head list;
 	struct device dev;
 	int id, probe_active;
+	struct list_head poison_list;
 	struct mutex reconfig_mutex;
 };
 
@@ -52,6 +53,7 @@ void nd_region_probe_success(struct nvdimm_bus *nvdimm_bus, struct device *dev);
 struct nd_region;
 void nd_region_create_blk_seed(struct nd_region *nd_region);
 void nd_region_create_btt_seed(struct nd_region *nd_region);
+void nd_region_create_pfn_seed(struct nd_region *nd_region);
 void nd_region_disable(struct nvdimm_bus *nvdimm_bus, struct device *dev);
 int nvdimm_bus_create_ndctl(struct nvdimm_bus *nvdimm_bus);
 void nvdimm_bus_destroy_ndctl(struct nvdimm_bus *nvdimm_bus);
diff --git a/drivers/nvdimm/nd.h b/drivers/nvdimm/nd.h
index 417e521d299c..ba1633b9da31 100644
--- a/drivers/nvdimm/nd.h
+++ b/drivers/nvdimm/nd.h
@@ -29,13 +29,12 @@ enum {
 	ND_MAX_LANES = 256,
 	SECTOR_SHIFT = 9,
 	INT_LBASIZE_ALIGNMENT = 64,
-#if IS_ENABLED(CONFIG_NVDIMM_PFN)
-	ND_PFN_ALIGN = PAGES_PER_SECTION * PAGE_SIZE,
-	ND_PFN_MASK = ND_PFN_ALIGN - 1,
-#else
-	ND_PFN_ALIGN = 0,
-	ND_PFN_MASK = 0,
-#endif
+};
+
+struct nd_poison {
+	u64 start;
+	u64 length;
+	struct list_head list;
 };
 
 struct nvdimm_drvdata {
@@ -153,6 +152,7 @@ struct nd_pfn {
 	int id;
 	u8 *uuid;
 	struct device dev;
+	unsigned long align;
 	unsigned long npfns;
 	enum nd_pfn_mode mode;
 	struct nd_pfn_sb *pfn_sb;
@@ -262,6 +262,8 @@ int nvdimm_namespace_attach_btt(struct nd_namespace_common *ndns);
 int nvdimm_namespace_detach_btt(struct nd_namespace_common *ndns);
 const char *nvdimm_namespace_disk_name(struct nd_namespace_common *ndns,
 		char *name);
+void nvdimm_namespace_add_poison(struct nd_namespace_common *ndns,
+		struct badblocks *bb, resource_size_t offset);
 int nd_blk_region_init(struct nd_region *nd_region);
 void __nd_iostat_start(struct bio *bio, unsigned long *start);
 static inline bool nd_iostat_start(struct bio *bio, unsigned long *start)
diff --git a/drivers/nvdimm/pfn_devs.c b/drivers/nvdimm/pfn_devs.c
index 71805a1aa0f3..ae81a2f1da50 100644
--- a/drivers/nvdimm/pfn_devs.c
+++ b/drivers/nvdimm/pfn_devs.c
@@ -83,8 +83,7 @@ static ssize_t mode_store(struct device *dev,
 
 		if (strncmp(buf, "pmem\n", n) == 0
 				|| strncmp(buf, "pmem", n) == 0) {
-			/* TODO: allocate from PMEM support */
-			rc = -ENOTTY;
+			nd_pfn->mode = PFN_MODE_PMEM;
 		} else if (strncmp(buf, "ram\n", n) == 0
 				|| strncmp(buf, "ram", n) == 0)
 			nd_pfn->mode = PFN_MODE_RAM;
@@ -103,6 +102,52 @@ static ssize_t mode_store(struct device *dev,
 }
 static DEVICE_ATTR_RW(mode);
 
+static ssize_t align_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct nd_pfn *nd_pfn = to_nd_pfn(dev);
+
+	return sprintf(buf, "%lx\n", nd_pfn->align);
+}
+
+static ssize_t __align_store(struct nd_pfn *nd_pfn, const char *buf)
+{
+	unsigned long val;
+	int rc;
+
+	rc = kstrtoul(buf, 0, &val);
+	if (rc)
+		return rc;
+
+	if (!is_power_of_2(val) || val < PAGE_SIZE || val > SZ_1G)
+		return -EINVAL;
+
+	if (nd_pfn->dev.driver)
+		return -EBUSY;
+	else
+		nd_pfn->align = val;
+
+	return 0;
+}
+
+static ssize_t align_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	struct nd_pfn *nd_pfn = to_nd_pfn(dev);
+	ssize_t rc;
+
+	device_lock(dev);
+	nvdimm_bus_lock(dev);
+	rc = __align_store(nd_pfn, buf);
+	dev_dbg(dev, "%s: result: %zd wrote: %s%s", __func__,
+			rc, buf, buf[len - 1] == '\n' ? "" : "\n");
+	nvdimm_bus_unlock(dev);
+	device_unlock(dev);
+
+	return rc ? rc : len;
+}
+static DEVICE_ATTR_RW(align);
+
 static ssize_t uuid_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -164,6 +209,7 @@ static struct attribute *nd_pfn_attributes[] = {
 	&dev_attr_mode.attr,
 	&dev_attr_namespace.attr,
 	&dev_attr_uuid.attr,
+	&dev_attr_align.attr,
 	NULL,
 };
 
@@ -179,7 +225,6 @@ static const struct attribute_group *nd_pfn_attribute_groups[] = {
 };
 
 static struct device *__nd_pfn_create(struct nd_region *nd_region,
-		u8 *uuid, enum nd_pfn_mode mode,
 		struct nd_namespace_common *ndns)
 {
 	struct nd_pfn *nd_pfn;
@@ -199,10 +244,8 @@ static struct device *__nd_pfn_create(struct nd_region *nd_region,
 		return NULL;
 	}
 
-	nd_pfn->mode = mode;
-	if (uuid)
-		uuid = kmemdup(uuid, 16, GFP_KERNEL);
-	nd_pfn->uuid = uuid;
+	nd_pfn->mode = PFN_MODE_NONE;
+	nd_pfn->align = HPAGE_SIZE;
 	dev = &nd_pfn->dev;
 	dev_set_name(dev, "pfn%d.%d", nd_region->id, nd_pfn->id);
 	dev->parent = &nd_region->dev;
@@ -220,8 +263,7 @@ static struct device *__nd_pfn_create(struct nd_region *nd_region,
 
 struct device *nd_pfn_create(struct nd_region *nd_region)
 {
-	struct device *dev = __nd_pfn_create(nd_region, NULL, PFN_MODE_NONE,
-			NULL);
+	struct device *dev = __nd_pfn_create(nd_region, NULL);
 
 	if (dev)
 		__nd_device_register(dev);
@@ -230,10 +272,11 @@ struct device *nd_pfn_create(struct nd_region *nd_region)
 
 int nd_pfn_validate(struct nd_pfn *nd_pfn)
 {
-	struct nd_namespace_common *ndns = nd_pfn->ndns;
-	struct nd_pfn_sb *pfn_sb = nd_pfn->pfn_sb;
-	struct nd_namespace_io *nsio;
 	u64 checksum, offset;
+	struct nd_namespace_io *nsio;
+	struct nd_pfn_sb *pfn_sb = nd_pfn->pfn_sb;
+	struct nd_namespace_common *ndns = nd_pfn->ndns;
+	const u8 *parent_uuid = nd_dev_to_uuid(&ndns->dev);
 
 	if (!pfn_sb || !ndns)
 		return -ENODEV;
@@ -241,10 +284,6 @@ int nd_pfn_validate(struct nd_pfn *nd_pfn)
 	if (!is_nd_pmem(nd_pfn->dev.parent))
 		return -ENODEV;
 
-	/* section alignment for simple hotplug */
-	if (nvdimm_namespace_capacity(ndns) < ND_PFN_ALIGN)
-		return -ENODEV;
-
 	if (nvdimm_read_bytes(ndns, SZ_4K, pfn_sb, sizeof(*pfn_sb)))
 		return -ENXIO;
 
@@ -257,12 +296,13 @@ int nd_pfn_validate(struct nd_pfn *nd_pfn)
 		return -ENODEV;
 	pfn_sb->checksum = cpu_to_le64(checksum);
 
+	if (memcmp(pfn_sb->parent_uuid, parent_uuid, 16) != 0)
+		return -ENODEV;
+
 	switch (le32_to_cpu(pfn_sb->mode)) {
 	case PFN_MODE_RAM:
-		break;
 	case PFN_MODE_PMEM:
-		/* TODO: allocate from PMEM support */
-		return -ENOTTY;
+		break;
 	default:
 		return -ENXIO;
 	}
@@ -278,6 +318,12 @@ int nd_pfn_validate(struct nd_pfn *nd_pfn)
 			return -EINVAL;
 	}
 
+	if (nd_pfn->align > nvdimm_namespace_capacity(ndns)) {
+		dev_err(&nd_pfn->dev, "alignment: %lx exceeds capacity %llx\n",
+				nd_pfn->align, nvdimm_namespace_capacity(ndns));
+		return -EINVAL;
+	}
+
 	/*
 	 * These warnings are verbose because they can only trigger in
 	 * the case where the physical address alignment of the
@@ -286,17 +332,19 @@ int nd_pfn_validate(struct nd_pfn *nd_pfn)
 	 */
 	offset = le64_to_cpu(pfn_sb->dataoff);
 	nsio = to_nd_namespace_io(&ndns->dev);
-	if (nsio->res.start & ND_PFN_MASK) {
-		dev_err(&nd_pfn->dev,
-				"init failed: %s not section aligned\n",
-				dev_name(&ndns->dev));
-		return -EBUSY;
-	} else if (offset >= resource_size(&nsio->res)) {
+	if (offset >= resource_size(&nsio->res)) {
 		dev_err(&nd_pfn->dev, "pfn array size exceeds capacity of %s\n",
 				dev_name(&ndns->dev));
 		return -EBUSY;
 	}
 
+	nd_pfn->align = 1UL << ilog2(offset);
+	if (!is_power_of_2(offset) || offset < PAGE_SIZE) {
+		dev_err(&nd_pfn->dev, "bad offset: %#llx dax disabled\n",
+				offset);
+		return -ENXIO;
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(nd_pfn_validate);
@@ -313,7 +361,7 @@ int nd_pfn_probe(struct nd_namespace_common *ndns, void *drvdata)
 		return -ENODEV;
 
 	nvdimm_bus_lock(&ndns->dev);
-	dev = __nd_pfn_create(nd_region, NULL, PFN_MODE_NONE, ndns);
+	dev = __nd_pfn_create(nd_region, ndns);
 	nvdimm_bus_unlock(&ndns->dev);
 	if (!dev)
 		return -ENOMEM;
diff --git a/drivers/nvdimm/pmem.c b/drivers/nvdimm/pmem.c
index 8ee79893d2f5..8d0b54670184 100644
--- a/drivers/nvdimm/pmem.c
+++ b/drivers/nvdimm/pmem.c
@@ -21,9 +21,11 @@
 #include <linux/init.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
-#include <linux/memory_hotplug.h>
 #include <linux/moduleparam.h>
+#include <linux/badblocks.h>
+#include <linux/memremap.h>
 #include <linux/vmalloc.h>
+#include <linux/pfn_t.h>
 #include <linux/slab.h>
 #include <linux/pmem.h>
 #include <linux/nd.h>
@@ -39,13 +41,28 @@ struct pmem_device {
 	phys_addr_t		phys_addr;
 	/* when non-zero this device is hosting a 'pfn' instance */
 	phys_addr_t		data_offset;
+	u64			pfn_flags;
 	void __pmem		*virt_addr;
 	size_t			size;
+	struct badblocks	bb;
 };
 
 static int pmem_major;
 
-static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
+static bool is_bad_pmem(struct badblocks *bb, sector_t sector, unsigned int len)
+{
+	if (bb->count) {
+		sector_t first_bad;
+		int num_bad;
+
+		return !!badblocks_check(bb, sector, len / 512, &first_bad,
+				&num_bad);
+	}
+
+	return false;
+}
+
+static int pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 			unsigned int len, unsigned int off, int rw,
 			sector_t sector)
 {
@@ -54,6 +71,8 @@ static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 	void __pmem *pmem_addr = pmem->virt_addr + pmem_off;
 
 	if (rw == READ) {
+		if (unlikely(is_bad_pmem(&pmem->bb, sector, len)))
+			return -EIO;
 		memcpy_from_pmem(mem + off, pmem_addr, len);
 		flush_dcache_page(page);
 	} else {
@@ -62,10 +81,12 @@ static void pmem_do_bvec(struct pmem_device *pmem, struct page *page,
 	}
 
 	kunmap_atomic(mem);
+	return 0;
 }
 
 static blk_qc_t pmem_make_request(struct request_queue *q, struct bio *bio)
 {
+	int rc = 0;
 	bool do_acct;
 	unsigned long start;
 	struct bio_vec bvec;
@@ -74,9 +95,15 @@ static blk_qc_t pmem_make_request(struct request_queue *q, struct bio *bio)
 	struct pmem_device *pmem = bdev->bd_disk->private_data;
 
 	do_acct = nd_iostat_start(bio, &start);
-	bio_for_each_segment(bvec, bio, iter)
-		pmem_do_bvec(pmem, bvec.bv_page, bvec.bv_len, bvec.bv_offset,
-				bio_data_dir(bio), iter.bi_sector);
+	bio_for_each_segment(bvec, bio, iter) {
+		rc = pmem_do_bvec(pmem, bvec.bv_page, bvec.bv_len,
+				bvec.bv_offset, bio_data_dir(bio),
+				iter.bi_sector);
+		if (rc) {
+			bio->bi_error = rc;
+			break;
+		}
+	}
 	if (do_acct)
 		nd_iostat_end(bio, start);
 
@@ -91,23 +118,32 @@ static int pmem_rw_page(struct block_device *bdev, sector_t sector,
 		       struct page *page, int rw)
 {
 	struct pmem_device *pmem = bdev->bd_disk->private_data;
+	int rc;
 
-	pmem_do_bvec(pmem, page, PAGE_CACHE_SIZE, 0, rw, sector);
+	rc = pmem_do_bvec(pmem, page, PAGE_CACHE_SIZE, 0, rw, sector);
 	if (rw & WRITE)
 		wmb_pmem();
-	page_endio(page, rw & WRITE, 0);
 
-	return 0;
+	/*
+	 * The ->rw_page interface is subtle and tricky.  The core
+	 * retries on any error, so we can only invoke page_endio() in
+	 * the successful completion case.  Otherwise, we'll see crashes
+	 * caused by double completion.
+	 */
+	if (rc == 0)
+		page_endio(page, rw & WRITE, 0);
+
+	return rc;
 }
 
 static long pmem_direct_access(struct block_device *bdev, sector_t sector,
-		      void __pmem **kaddr, unsigned long *pfn)
+		      void __pmem **kaddr, pfn_t *pfn)
 {
 	struct pmem_device *pmem = bdev->bd_disk->private_data;
 	resource_size_t offset = sector * 512 + pmem->data_offset;
 
 	*kaddr = pmem->virt_addr + offset;
-	*pfn = (pmem->phys_addr + offset) >> PAGE_SHIFT;
+	*pfn = phys_to_pfn_t(pmem->phys_addr + offset, pmem->pfn_flags);
 
 	return pmem->size - offset;
 }
@@ -123,6 +159,7 @@ static struct pmem_device *pmem_alloc(struct device *dev,
 		struct resource *res, int id)
 {
 	struct pmem_device *pmem;
+	struct request_queue *q;
 
 	pmem = devm_kzalloc(dev, sizeof(*pmem), GFP_KERNEL);
 	if (!pmem)
@@ -140,16 +177,26 @@ static struct pmem_device *pmem_alloc(struct device *dev,
 		return ERR_PTR(-EBUSY);
 	}
 
-	if (pmem_should_map_pages(dev))
-		pmem->virt_addr = (void __pmem *) devm_memremap_pages(dev, res);
-	else
+	q = blk_alloc_queue_node(GFP_KERNEL, dev_to_node(dev));
+	if (!q)
+		return ERR_PTR(-ENOMEM);
+
+	pmem->pfn_flags = PFN_DEV;
+	if (pmem_should_map_pages(dev)) {
+		pmem->virt_addr = (void __pmem *) devm_memremap_pages(dev, res,
+				&q->q_usage_counter, NULL);
+		pmem->pfn_flags |= PFN_MAP;
+	} else
 		pmem->virt_addr = (void __pmem *) devm_memremap(dev,
 				pmem->phys_addr, pmem->size,
 				ARCH_MEMREMAP_PMEM);
 
-	if (IS_ERR(pmem->virt_addr))
+	if (IS_ERR(pmem->virt_addr)) {
+		blk_cleanup_queue(q);
 		return (void __force *) pmem->virt_addr;
+	}
 
+	pmem->pmem_queue = q;
 	return pmem;
 }
 
@@ -169,10 +216,6 @@ static int pmem_attach_disk(struct device *dev,
 	int nid = dev_to_node(dev);
 	struct gendisk *disk;
 
-	pmem->pmem_queue = blk_alloc_queue_node(GFP_KERNEL, nid);
-	if (!pmem->pmem_queue)
-		return -ENOMEM;
-
 	blk_queue_make_request(pmem->pmem_queue, pmem_make_request);
 	blk_queue_physical_block_size(pmem->pmem_queue, PAGE_SIZE);
 	blk_queue_max_hw_sectors(pmem->pmem_queue, UINT_MAX);
@@ -195,7 +238,12 @@ static int pmem_attach_disk(struct device *dev,
 	disk->driverfs_dev = dev;
 	set_capacity(disk, (pmem->size - pmem->data_offset) / 512);
 	pmem->pmem_disk = disk;
+	devm_exit_badblocks(dev, &pmem->bb);
+	if (devm_init_badblocks(dev, &pmem->bb))
+		return -ENOMEM;
+	nvdimm_namespace_add_poison(ndns, &pmem->bb, pmem->data_offset);
 
+	disk->bb = &pmem->bb;
 	add_disk(disk);
 	revalidate_disk(disk);
 
@@ -212,9 +260,13 @@ static int pmem_rw_bytes(struct nd_namespace_common *ndns,
 		return -EFAULT;
 	}
 
-	if (rw == READ)
+	if (rw == READ) {
+		unsigned int sz_align = ALIGN(size + (offset & (512 - 1)), 512);
+
+		if (unlikely(is_bad_pmem(&pmem->bb, offset / 512, sz_align)))
+			return -EIO;
 		memcpy_from_pmem(buf, pmem->virt_addr + offset, size);
-	else {
+	} else {
 		memcpy_to_pmem(pmem->virt_addr + offset, buf, size);
 		wmb_pmem();
 	}
@@ -238,14 +290,11 @@ static int nd_pfn_init(struct nd_pfn *nd_pfn)
 
 	nd_pfn->pfn_sb = pfn_sb;
 	rc = nd_pfn_validate(nd_pfn);
-	if (rc == 0 || rc == -EBUSY)
+	if (rc == -ENODEV)
+		/* no info block, do init */;
+	else
 		return rc;
 
-	/* section alignment for simple hotplug */
-	if (nvdimm_namespace_capacity(ndns) < ND_PFN_ALIGN
-			|| pmem->phys_addr & ND_PFN_MASK)
-		return -ENODEV;
-
 	nd_region = to_nd_region(nd_pfn->dev.parent);
 	if (nd_region->ro) {
 		dev_info(&nd_pfn->dev,
@@ -263,9 +312,9 @@ static int nd_pfn_init(struct nd_pfn *nd_pfn)
 	 * ->direct_access() to those that are included in the memmap.
 	 */
 	if (nd_pfn->mode == PFN_MODE_PMEM)
-		offset = ALIGN(SZ_8K + 64 * npfns, PMD_SIZE);
+		offset = ALIGN(SZ_8K + 64 * npfns, nd_pfn->align);
 	else if (nd_pfn->mode == PFN_MODE_RAM)
-		offset = SZ_8K;
+		offset = ALIGN(SZ_8K, nd_pfn->align);
 	else
 		goto err;
 
@@ -275,6 +324,7 @@ static int nd_pfn_init(struct nd_pfn *nd_pfn)
 	pfn_sb->npfns = cpu_to_le64(npfns);
 	memcpy(pfn_sb->signature, PFN_SIG, PFN_SIG_LEN);
 	memcpy(pfn_sb->uuid, nd_pfn->uuid, 16);
+	memcpy(pfn_sb->parent_uuid, nd_dev_to_uuid(&ndns->dev), 16);
 	pfn_sb->version_major = cpu_to_le16(1);
 	checksum = nd_sb_checksum((struct nd_gen_sb *) pfn_sb);
 	pfn_sb->checksum = cpu_to_le64(checksum);
@@ -311,12 +361,17 @@ static int nvdimm_namespace_attach_pfn(struct nd_namespace_common *ndns)
 	struct nd_namespace_io *nsio = to_nd_namespace_io(&ndns->dev);
 	struct nd_pfn *nd_pfn = to_nd_pfn(ndns->claim);
 	struct device *dev = &nd_pfn->dev;
-	struct vmem_altmap *altmap;
 	struct nd_region *nd_region;
+	struct vmem_altmap *altmap;
 	struct nd_pfn_sb *pfn_sb;
 	struct pmem_device *pmem;
+	struct request_queue *q;
 	phys_addr_t offset;
 	int rc;
+	struct vmem_altmap __altmap = {
+		.base_pfn = __phys_to_pfn(nsio->res.start),
+		.reserve = __phys_to_pfn(SZ_8K),
+	};
 
 	if (!nd_pfn->uuid || !nd_pfn->ndns)
 		return -ENODEV;
@@ -326,24 +381,25 @@ static int nvdimm_namespace_attach_pfn(struct nd_namespace_common *ndns)
 	if (rc)
 		return rc;
 
-	if (PAGE_SIZE != SZ_4K) {
-		dev_err(dev, "only supported on systems with 4K PAGE_SIZE\n");
-		return -ENXIO;
-	}
-	if (nsio->res.start & ND_PFN_MASK) {
-		dev_err(dev, "%s not memory hotplug section aligned\n",
-				dev_name(&ndns->dev));
-		return -ENXIO;
-	}
-
 	pfn_sb = nd_pfn->pfn_sb;
 	offset = le64_to_cpu(pfn_sb->dataoff);
 	nd_pfn->mode = le32_to_cpu(nd_pfn->pfn_sb->mode);
 	if (nd_pfn->mode == PFN_MODE_RAM) {
-		if (offset != SZ_8K)
+		if (offset < SZ_8K)
 			return -EINVAL;
 		nd_pfn->npfns = le64_to_cpu(pfn_sb->npfns);
 		altmap = NULL;
+	} else if (nd_pfn->mode == PFN_MODE_PMEM) {
+		nd_pfn->npfns = (resource_size(&nsio->res) - offset)
+			/ PAGE_SIZE;
+		if (le64_to_cpu(nd_pfn->pfn_sb->npfns) > nd_pfn->npfns)
+			dev_info(&nd_pfn->dev,
+					"number of pfns truncated from %lld to %ld\n",
+					le64_to_cpu(nd_pfn->pfn_sb->npfns),
+					nd_pfn->npfns);
+		altmap = & __altmap;
+		altmap->free = __phys_to_pfn(offset - SZ_8K);
+		altmap->alloc = 0;
 	} else {
 		rc = -ENXIO;
 		goto err;
@@ -351,8 +407,11 @@ static int nvdimm_namespace_attach_pfn(struct nd_namespace_common *ndns)
 
 	/* establish pfn range for lookup, and switch to direct map */
 	pmem = dev_get_drvdata(dev);
+	q = pmem->pmem_queue;
 	devm_memunmap(dev, (void __force *) pmem->virt_addr);
-	pmem->virt_addr = (void __pmem *) devm_memremap_pages(dev, &nsio->res);
+	pmem->virt_addr = (void __pmem *) devm_memremap_pages(dev, &nsio->res,
+			&q->q_usage_counter, altmap);
+	pmem->pfn_flags |= PFN_MAP;
 	if (IS_ERR(pmem->virt_addr)) {
 		rc = PTR_ERR(pmem->virt_addr);
 		goto err;
@@ -389,20 +448,26 @@ static int nd_pmem_probe(struct device *dev)
 	pmem->ndns = ndns;
 	dev_set_drvdata(dev, pmem);
 	ndns->rw_bytes = pmem_rw_bytes;
+	if (devm_init_badblocks(dev, &pmem->bb))
+		return -ENOMEM;
+	nvdimm_namespace_add_poison(ndns, &pmem->bb, 0);
 
-	if (is_nd_btt(dev))
+	if (is_nd_btt(dev)) {
+		/* btt allocates its own request_queue */
+		blk_cleanup_queue(pmem->pmem_queue);
+		pmem->pmem_queue = NULL;
 		return nvdimm_namespace_attach_btt(ndns);
+	}
 
 	if (is_nd_pfn(dev))
 		return nvdimm_namespace_attach_pfn(ndns);
 
-	if (nd_btt_probe(ndns, pmem) == 0) {
-		/* we'll come back as btt-pmem */
-		return -ENXIO;
-	}
-
-	if (nd_pfn_probe(ndns, pmem) == 0) {
-		/* we'll come back as pfn-pmem */
+	if (nd_btt_probe(ndns, pmem) == 0 || nd_pfn_probe(ndns, pmem) == 0) {
+		/*
+		 * We'll come back as either btt-pmem, or pfn-pmem, so
+		 * drop the queue allocation for now.
+		 */
+		blk_cleanup_queue(pmem->pmem_queue);
 		return -ENXIO;
 	}
 
diff --git a/drivers/nvdimm/region_devs.c b/drivers/nvdimm/region_devs.c
index 529f3f02e7b2..139bf71ca549 100644
--- a/drivers/nvdimm/region_devs.c
+++ b/drivers/nvdimm/region_devs.c
@@ -134,62 +134,6 @@ int nd_region_to_nstype(struct nd_region *nd_region)
 }
 EXPORT_SYMBOL(nd_region_to_nstype);
 
-static int is_uuid_busy(struct device *dev, void *data)
-{
-	struct nd_region *nd_region = to_nd_region(dev->parent);
-	u8 *uuid = data;
-
-	switch (nd_region_to_nstype(nd_region)) {
-	case ND_DEVICE_NAMESPACE_PMEM: {
-		struct nd_namespace_pmem *nspm = to_nd_namespace_pmem(dev);
-
-		if (!nspm->uuid)
-			break;
-		if (memcmp(uuid, nspm->uuid, NSLABEL_UUID_LEN) == 0)
-			return -EBUSY;
-		break;
-	}
-	case ND_DEVICE_NAMESPACE_BLK: {
-		struct nd_namespace_blk *nsblk = to_nd_namespace_blk(dev);
-
-		if (!nsblk->uuid)
-			break;
-		if (memcmp(uuid, nsblk->uuid, NSLABEL_UUID_LEN) == 0)
-			return -EBUSY;
-		break;
-	}
-	default:
-		break;
-	}
-
-	return 0;
-}
-
-static int is_namespace_uuid_busy(struct device *dev, void *data)
-{
-	if (is_nd_pmem(dev) || is_nd_blk(dev))
-		return device_for_each_child(dev, data, is_uuid_busy);
-	return 0;
-}
-
-/**
- * nd_is_uuid_unique - verify that no other namespace has @uuid
- * @dev: any device on a nvdimm_bus
- * @uuid: uuid to check
- */
-bool nd_is_uuid_unique(struct device *dev, u8 *uuid)
-{
-	struct nvdimm_bus *nvdimm_bus = walk_to_nvdimm_bus(dev);
-
-	if (!nvdimm_bus)
-		return false;
-	WARN_ON_ONCE(!is_nvdimm_bus_locked(&nvdimm_bus->dev));
-	if (device_for_each_child(&nvdimm_bus->dev, uuid,
-				is_namespace_uuid_busy) != 0)
-		return false;
-	return true;
-}
-
 static ssize_t size_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -406,6 +350,9 @@ static umode_t region_visible(struct kobject *kobj, struct attribute *a, int n)
 	struct nd_interleave_set *nd_set = nd_region->nd_set;
 	int type = nd_region_to_nstype(nd_region);
 
+	if (!is_nd_pmem(dev) && a == &dev_attr_pfn_seed.attr)
+		return 0;
+
 	if (a != &dev_attr_set_cookie.attr
 			&& a != &dev_attr_available_size.attr)
 		return a->mode;
@@ -487,6 +434,13 @@ static void nd_region_notify_driver_action(struct nvdimm_bus *nvdimm_bus,
 			nd_region_create_blk_seed(nd_region);
 		nvdimm_bus_unlock(dev);
 	}
+	if (is_nd_pfn(dev) && probe) {
+		nd_region = to_nd_region(dev->parent);
+		nvdimm_bus_lock(dev);
+		if (nd_region->pfn_seed == dev)
+			nd_region_create_pfn_seed(nd_region);
+		nvdimm_bus_unlock(dev);
+	}
 }
 
 void nd_region_probe_success(struct nvdimm_bus *nvdimm_bus, struct device *dev)
diff --git a/drivers/nvme/host/Kconfig b/drivers/nvme/host/Kconfig
index 002a94abdbc4..b586d84f2518 100644
--- a/drivers/nvme/host/Kconfig
+++ b/drivers/nvme/host/Kconfig
@@ -8,3 +8,15 @@ config BLK_DEV_NVME
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called nvme.
+
+config BLK_DEV_NVME_SCSI
+	bool "SCSI emulation for NVMe device nodes"
+	depends on BLK_DEV_NVME
+	---help---
+	  This adds support for the SG_IO ioctl on the NVMe character
+	  and block devices nodes, as well a a translation for a small
+	  number of selected SCSI commands to NVMe commands to the NVMe
+	  driver.  If you don't know what this means you probably want
+	  to say N here, unless you run a distro that abuses the SCSI
+	  emulation to provide stable device names for mount by id, like
+	  some OpenSuSE and SLES versions.
diff --git a/drivers/nvme/host/Makefile b/drivers/nvme/host/Makefile
index a5fe23952586..51bf90871549 100644
--- a/drivers/nvme/host/Makefile
+++ b/drivers/nvme/host/Makefile
@@ -1,5 +1,6 @@
 
 obj-$(CONFIG_BLK_DEV_NVME)     += nvme.o
 
-lightnvm-$(CONFIG_NVM)	:= lightnvm.o
-nvme-y		+= pci.o scsi.o $(lightnvm-y)
+lightnvm-$(CONFIG_NVM)			:= lightnvm.o
+nvme-y					+= core.o pci.o $(lightnvm-y)
+nvme-$(CONFIG_BLK_DEV_NVME_SCSI)        += scsi.o
diff --git a/drivers/nvme/host/core.c b/drivers/nvme/host/core.c
new file mode 100644
index 000000000000..03c46412fff4
--- /dev/null
+++ b/drivers/nvme/host/core.c
@@ -0,0 +1,1516 @@
+/*
+ * NVM Express device driver
+ * Copyright (c) 2011-2014, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/blkdev.h>
+#include <linux/blk-mq.h>
+#include <linux/delay.h>
+#include <linux/errno.h>
+#include <linux/hdreg.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/list_sort.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/pr.h>
+#include <linux/ptrace.h>
+#include <linux/nvme_ioctl.h>
+#include <linux/t10-pi.h>
+#include <scsi/sg.h>
+#include <asm/unaligned.h>
+
+#include "nvme.h"
+
+#define NVME_MINORS		(1U << MINORBITS)
+
+static int nvme_major;
+module_param(nvme_major, int, 0);
+
+static int nvme_char_major;
+module_param(nvme_char_major, int, 0);
+
+static LIST_HEAD(nvme_ctrl_list);
+DEFINE_SPINLOCK(dev_list_lock);
+
+static struct class *nvme_class;
+
+static void nvme_free_ns(struct kref *kref)
+{
+	struct nvme_ns *ns = container_of(kref, struct nvme_ns, kref);
+
+	if (ns->type == NVME_NS_LIGHTNVM)
+		nvme_nvm_unregister(ns->queue, ns->disk->disk_name);
+
+	spin_lock(&dev_list_lock);
+	ns->disk->private_data = NULL;
+	spin_unlock(&dev_list_lock);
+
+	put_disk(ns->disk);
+	ida_simple_remove(&ns->ctrl->ns_ida, ns->instance);
+	nvme_put_ctrl(ns->ctrl);
+	kfree(ns);
+}
+
+static void nvme_put_ns(struct nvme_ns *ns)
+{
+	kref_put(&ns->kref, nvme_free_ns);
+}
+
+static struct nvme_ns *nvme_get_ns_from_disk(struct gendisk *disk)
+{
+	struct nvme_ns *ns;
+
+	spin_lock(&dev_list_lock);
+	ns = disk->private_data;
+	if (ns && !kref_get_unless_zero(&ns->kref))
+		ns = NULL;
+	spin_unlock(&dev_list_lock);
+
+	return ns;
+}
+
+void nvme_requeue_req(struct request *req)
+{
+	unsigned long flags;
+
+	blk_mq_requeue_request(req);
+	spin_lock_irqsave(req->q->queue_lock, flags);
+	if (!blk_queue_stopped(req->q))
+		blk_mq_kick_requeue_list(req->q);
+	spin_unlock_irqrestore(req->q->queue_lock, flags);
+}
+
+struct request *nvme_alloc_request(struct request_queue *q,
+		struct nvme_command *cmd, unsigned int flags)
+{
+	bool write = cmd->common.opcode & 1;
+	struct request *req;
+
+	req = blk_mq_alloc_request(q, write, flags);
+	if (IS_ERR(req))
+		return req;
+
+	req->cmd_type = REQ_TYPE_DRV_PRIV;
+	req->cmd_flags |= REQ_FAILFAST_DRIVER;
+	req->__data_len = 0;
+	req->__sector = (sector_t) -1;
+	req->bio = req->biotail = NULL;
+
+	req->cmd = (unsigned char *)cmd;
+	req->cmd_len = sizeof(struct nvme_command);
+	req->special = (void *)0;
+
+	return req;
+}
+
+/*
+ * Returns 0 on success.  If the result is negative, it's a Linux error code;
+ * if the result is positive, it's an NVM Express status code
+ */
+int __nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void *buffer, unsigned bufflen, u32 *result, unsigned timeout)
+{
+	struct request *req;
+	int ret;
+
+	req = nvme_alloc_request(q, cmd, 0);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+
+	req->timeout = timeout ? timeout : ADMIN_TIMEOUT;
+
+	if (buffer && bufflen) {
+		ret = blk_rq_map_kern(q, req, buffer, bufflen, GFP_KERNEL);
+		if (ret)
+			goto out;
+	}
+
+	blk_execute_rq(req->q, NULL, req, 0);
+	if (result)
+		*result = (u32)(uintptr_t)req->special;
+	ret = req->errors;
+ out:
+	blk_mq_free_request(req);
+	return ret;
+}
+
+int nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void *buffer, unsigned bufflen)
+{
+	return __nvme_submit_sync_cmd(q, cmd, buffer, bufflen, NULL, 0);
+}
+
+int __nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void __user *ubuffer, unsigned bufflen,
+		void __user *meta_buffer, unsigned meta_len, u32 meta_seed,
+		u32 *result, unsigned timeout)
+{
+	bool write = cmd->common.opcode & 1;
+	struct nvme_ns *ns = q->queuedata;
+	struct gendisk *disk = ns ? ns->disk : NULL;
+	struct request *req;
+	struct bio *bio = NULL;
+	void *meta = NULL;
+	int ret;
+
+	req = nvme_alloc_request(q, cmd, 0);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
+
+	req->timeout = timeout ? timeout : ADMIN_TIMEOUT;
+
+	if (ubuffer && bufflen) {
+		ret = blk_rq_map_user(q, req, NULL, ubuffer, bufflen,
+				GFP_KERNEL);
+		if (ret)
+			goto out;
+		bio = req->bio;
+
+		if (!disk)
+			goto submit;
+		bio->bi_bdev = bdget_disk(disk, 0);
+		if (!bio->bi_bdev) {
+			ret = -ENODEV;
+			goto out_unmap;
+		}
+
+		if (meta_buffer && meta_len) {
+			struct bio_integrity_payload *bip;
+
+			meta = kmalloc(meta_len, GFP_KERNEL);
+			if (!meta) {
+				ret = -ENOMEM;
+				goto out_unmap;
+			}
+
+			if (write) {
+				if (copy_from_user(meta, meta_buffer,
+						meta_len)) {
+					ret = -EFAULT;
+					goto out_free_meta;
+				}
+			}
+
+			bip = bio_integrity_alloc(bio, GFP_KERNEL, 1);
+			if (IS_ERR(bip)) {
+				ret = PTR_ERR(bip);
+				goto out_free_meta;
+			}
+
+			bip->bip_iter.bi_size = meta_len;
+			bip->bip_iter.bi_sector = meta_seed;
+
+			ret = bio_integrity_add_page(bio, virt_to_page(meta),
+					meta_len, offset_in_page(meta));
+			if (ret != meta_len) {
+				ret = -ENOMEM;
+				goto out_free_meta;
+			}
+		}
+	}
+ submit:
+	blk_execute_rq(req->q, disk, req, 0);
+	ret = req->errors;
+	if (result)
+		*result = (u32)(uintptr_t)req->special;
+	if (meta && !ret && !write) {
+		if (copy_to_user(meta_buffer, meta, meta_len))
+			ret = -EFAULT;
+	}
+ out_free_meta:
+	kfree(meta);
+ out_unmap:
+	if (bio) {
+		if (disk && bio->bi_bdev)
+			bdput(bio->bi_bdev);
+		blk_rq_unmap_user(bio);
+	}
+ out:
+	blk_mq_free_request(req);
+	return ret;
+}
+
+int nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void __user *ubuffer, unsigned bufflen, u32 *result,
+		unsigned timeout)
+{
+	return __nvme_submit_user_cmd(q, cmd, ubuffer, bufflen, NULL, 0, 0,
+			result, timeout);
+}
+
+int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id)
+{
+	struct nvme_command c = { };
+	int error;
+
+	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
+	c.identify.opcode = nvme_admin_identify;
+	c.identify.cns = cpu_to_le32(1);
+
+	*id = kmalloc(sizeof(struct nvme_id_ctrl), GFP_KERNEL);
+	if (!*id)
+		return -ENOMEM;
+
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
+			sizeof(struct nvme_id_ctrl));
+	if (error)
+		kfree(*id);
+	return error;
+}
+
+static int nvme_identify_ns_list(struct nvme_ctrl *dev, unsigned nsid, __le32 *ns_list)
+{
+	struct nvme_command c = { };
+
+	c.identify.opcode = nvme_admin_identify;
+	c.identify.cns = cpu_to_le32(2);
+	c.identify.nsid = cpu_to_le32(nsid);
+	return nvme_submit_sync_cmd(dev->admin_q, &c, ns_list, 0x1000);
+}
+
+int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
+		struct nvme_id_ns **id)
+{
+	struct nvme_command c = { };
+	int error;
+
+	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
+	c.identify.opcode = nvme_admin_identify,
+	c.identify.nsid = cpu_to_le32(nsid),
+
+	*id = kmalloc(sizeof(struct nvme_id_ns), GFP_KERNEL);
+	if (!*id)
+		return -ENOMEM;
+
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
+			sizeof(struct nvme_id_ns));
+	if (error)
+		kfree(*id);
+	return error;
+}
+
+int nvme_get_features(struct nvme_ctrl *dev, unsigned fid, unsigned nsid,
+					dma_addr_t dma_addr, u32 *result)
+{
+	struct nvme_command c;
+
+	memset(&c, 0, sizeof(c));
+	c.features.opcode = nvme_admin_get_features;
+	c.features.nsid = cpu_to_le32(nsid);
+	c.features.prp1 = cpu_to_le64(dma_addr);
+	c.features.fid = cpu_to_le32(fid);
+
+	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0, result, 0);
+}
+
+int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
+					dma_addr_t dma_addr, u32 *result)
+{
+	struct nvme_command c;
+
+	memset(&c, 0, sizeof(c));
+	c.features.opcode = nvme_admin_set_features;
+	c.features.prp1 = cpu_to_le64(dma_addr);
+	c.features.fid = cpu_to_le32(fid);
+	c.features.dword11 = cpu_to_le32(dword11);
+
+	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0, result, 0);
+}
+
+int nvme_get_log_page(struct nvme_ctrl *dev, struct nvme_smart_log **log)
+{
+	struct nvme_command c = { };
+	int error;
+
+	c.common.opcode = nvme_admin_get_log_page,
+	c.common.nsid = cpu_to_le32(0xFFFFFFFF),
+	c.common.cdw10[0] = cpu_to_le32(
+			(((sizeof(struct nvme_smart_log) / 4) - 1) << 16) |
+			 NVME_LOG_SMART),
+
+	*log = kmalloc(sizeof(struct nvme_smart_log), GFP_KERNEL);
+	if (!*log)
+		return -ENOMEM;
+
+	error = nvme_submit_sync_cmd(dev->admin_q, &c, *log,
+			sizeof(struct nvme_smart_log));
+	if (error)
+		kfree(*log);
+	return error;
+}
+
+int nvme_set_queue_count(struct nvme_ctrl *ctrl, int *count)
+{
+	u32 q_count = (*count - 1) | ((*count - 1) << 16);
+	u32 result;
+	int status, nr_io_queues;
+
+	status = nvme_set_features(ctrl, NVME_FEAT_NUM_QUEUES, q_count, 0,
+			&result);
+	if (status)
+		return status;
+
+	nr_io_queues = min(result & 0xffff, result >> 16) + 1;
+	*count = min(*count, nr_io_queues);
+	return 0;
+}
+
+static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
+{
+	struct nvme_user_io io;
+	struct nvme_command c;
+	unsigned length, meta_len;
+	void __user *metadata;
+
+	if (copy_from_user(&io, uio, sizeof(io)))
+		return -EFAULT;
+	if (io.flags)
+		return -EINVAL;
+
+	switch (io.opcode) {
+	case nvme_cmd_write:
+	case nvme_cmd_read:
+	case nvme_cmd_compare:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	length = (io.nblocks + 1) << ns->lba_shift;
+	meta_len = (io.nblocks + 1) * ns->ms;
+	metadata = (void __user *)(uintptr_t)io.metadata;
+
+	if (ns->ext) {
+		length += meta_len;
+		meta_len = 0;
+	} else if (meta_len) {
+		if ((io.metadata & 3) || !io.metadata)
+			return -EINVAL;
+	}
+
+	memset(&c, 0, sizeof(c));
+	c.rw.opcode = io.opcode;
+	c.rw.flags = io.flags;
+	c.rw.nsid = cpu_to_le32(ns->ns_id);
+	c.rw.slba = cpu_to_le64(io.slba);
+	c.rw.length = cpu_to_le16(io.nblocks);
+	c.rw.control = cpu_to_le16(io.control);
+	c.rw.dsmgmt = cpu_to_le32(io.dsmgmt);
+	c.rw.reftag = cpu_to_le32(io.reftag);
+	c.rw.apptag = cpu_to_le16(io.apptag);
+	c.rw.appmask = cpu_to_le16(io.appmask);
+
+	return __nvme_submit_user_cmd(ns->queue, &c,
+			(void __user *)(uintptr_t)io.addr, length,
+			metadata, meta_len, io.slba, NULL, 0);
+}
+
+static int nvme_user_cmd(struct nvme_ctrl *ctrl, struct nvme_ns *ns,
+			struct nvme_passthru_cmd __user *ucmd)
+{
+	struct nvme_passthru_cmd cmd;
+	struct nvme_command c;
+	unsigned timeout = 0;
+	int status;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EACCES;
+	if (copy_from_user(&cmd, ucmd, sizeof(cmd)))
+		return -EFAULT;
+	if (cmd.flags)
+		return -EINVAL;
+
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = cmd.opcode;
+	c.common.flags = cmd.flags;
+	c.common.nsid = cpu_to_le32(cmd.nsid);
+	c.common.cdw2[0] = cpu_to_le32(cmd.cdw2);
+	c.common.cdw2[1] = cpu_to_le32(cmd.cdw3);
+	c.common.cdw10[0] = cpu_to_le32(cmd.cdw10);
+	c.common.cdw10[1] = cpu_to_le32(cmd.cdw11);
+	c.common.cdw10[2] = cpu_to_le32(cmd.cdw12);
+	c.common.cdw10[3] = cpu_to_le32(cmd.cdw13);
+	c.common.cdw10[4] = cpu_to_le32(cmd.cdw14);
+	c.common.cdw10[5] = cpu_to_le32(cmd.cdw15);
+
+	if (cmd.timeout_ms)
+		timeout = msecs_to_jiffies(cmd.timeout_ms);
+
+	status = nvme_submit_user_cmd(ns ? ns->queue : ctrl->admin_q, &c,
+			(void __user *)(uintptr_t)cmd.addr, cmd.data_len,
+			&cmd.result, timeout);
+	if (status >= 0) {
+		if (put_user(cmd.result, &ucmd->result))
+			return -EFAULT;
+	}
+
+	return status;
+}
+
+static int nvme_ioctl(struct block_device *bdev, fmode_t mode,
+		unsigned int cmd, unsigned long arg)
+{
+	struct nvme_ns *ns = bdev->bd_disk->private_data;
+
+	switch (cmd) {
+	case NVME_IOCTL_ID:
+		force_successful_syscall_return();
+		return ns->ns_id;
+	case NVME_IOCTL_ADMIN_CMD:
+		return nvme_user_cmd(ns->ctrl, NULL, (void __user *)arg);
+	case NVME_IOCTL_IO_CMD:
+		return nvme_user_cmd(ns->ctrl, ns, (void __user *)arg);
+	case NVME_IOCTL_SUBMIT_IO:
+		return nvme_submit_io(ns, (void __user *)arg);
+#ifdef CONFIG_BLK_DEV_NVME_SCSI
+	case SG_GET_VERSION_NUM:
+		return nvme_sg_get_version_num((void __user *)arg);
+	case SG_IO:
+		return nvme_sg_io(ns, (void __user *)arg);
+#endif
+	default:
+		return -ENOTTY;
+	}
+}
+
+#ifdef CONFIG_COMPAT
+static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
+			unsigned int cmd, unsigned long arg)
+{
+	switch (cmd) {
+	case SG_IO:
+		return -ENOIOCTLCMD;
+	}
+	return nvme_ioctl(bdev, mode, cmd, arg);
+}
+#else
+#define nvme_compat_ioctl	NULL
+#endif
+
+static int nvme_open(struct block_device *bdev, fmode_t mode)
+{
+	return nvme_get_ns_from_disk(bdev->bd_disk) ? 0 : -ENXIO;
+}
+
+static void nvme_release(struct gendisk *disk, fmode_t mode)
+{
+	nvme_put_ns(disk->private_data);
+}
+
+static int nvme_getgeo(struct block_device *bdev, struct hd_geometry *geo)
+{
+	/* some standard values */
+	geo->heads = 1 << 6;
+	geo->sectors = 1 << 5;
+	geo->cylinders = get_capacity(bdev->bd_disk) >> 11;
+	return 0;
+}
+
+#ifdef CONFIG_BLK_DEV_INTEGRITY
+static void nvme_init_integrity(struct nvme_ns *ns)
+{
+	struct blk_integrity integrity;
+
+	switch (ns->pi_type) {
+	case NVME_NS_DPS_PI_TYPE3:
+		integrity.profile = &t10_pi_type3_crc;
+		break;
+	case NVME_NS_DPS_PI_TYPE1:
+	case NVME_NS_DPS_PI_TYPE2:
+		integrity.profile = &t10_pi_type1_crc;
+		break;
+	default:
+		integrity.profile = NULL;
+		break;
+	}
+	integrity.tuple_size = ns->ms;
+	blk_integrity_register(ns->disk, &integrity);
+	blk_queue_max_integrity_segments(ns->queue, 1);
+}
+#else
+static void nvme_init_integrity(struct nvme_ns *ns)
+{
+}
+#endif /* CONFIG_BLK_DEV_INTEGRITY */
+
+static void nvme_config_discard(struct nvme_ns *ns)
+{
+	u32 logical_block_size = queue_logical_block_size(ns->queue);
+	ns->queue->limits.discard_zeroes_data = 0;
+	ns->queue->limits.discard_alignment = logical_block_size;
+	ns->queue->limits.discard_granularity = logical_block_size;
+	blk_queue_max_discard_sectors(ns->queue, 0xffffffff);
+	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue);
+}
+
+static int nvme_revalidate_disk(struct gendisk *disk)
+{
+	struct nvme_ns *ns = disk->private_data;
+	struct nvme_id_ns *id;
+	u8 lbaf, pi_type;
+	u16 old_ms;
+	unsigned short bs;
+
+	if (test_bit(NVME_NS_DEAD, &ns->flags)) {
+		set_capacity(disk, 0);
+		return -ENODEV;
+	}
+	if (nvme_identify_ns(ns->ctrl, ns->ns_id, &id)) {
+		dev_warn(ns->ctrl->dev, "%s: Identify failure nvme%dn%d\n",
+				__func__, ns->ctrl->instance, ns->ns_id);
+		return -ENODEV;
+	}
+	if (id->ncap == 0) {
+		kfree(id);
+		return -ENODEV;
+	}
+
+	if (nvme_nvm_ns_supported(ns, id) && ns->type != NVME_NS_LIGHTNVM) {
+		if (nvme_nvm_register(ns->queue, disk->disk_name)) {
+			dev_warn(ns->ctrl->dev,
+				"%s: LightNVM init failure\n", __func__);
+			kfree(id);
+			return -ENODEV;
+		}
+		ns->type = NVME_NS_LIGHTNVM;
+	}
+
+	if (ns->ctrl->vs >= NVME_VS(1, 1))
+		memcpy(ns->eui, id->eui64, sizeof(ns->eui));
+	if (ns->ctrl->vs >= NVME_VS(1, 2))
+		memcpy(ns->uuid, id->nguid, sizeof(ns->uuid));
+
+	old_ms = ns->ms;
+	lbaf = id->flbas & NVME_NS_FLBAS_LBA_MASK;
+	ns->lba_shift = id->lbaf[lbaf].ds;
+	ns->ms = le16_to_cpu(id->lbaf[lbaf].ms);
+	ns->ext = ns->ms && (id->flbas & NVME_NS_FLBAS_META_EXT);
+
+	/*
+	 * If identify namespace failed, use default 512 byte block size so
+	 * block layer can use before failing read/write for 0 capacity.
+	 */
+	if (ns->lba_shift == 0)
+		ns->lba_shift = 9;
+	bs = 1 << ns->lba_shift;
+	/* XXX: PI implementation requires metadata equal t10 pi tuple size */
+	pi_type = ns->ms == sizeof(struct t10_pi_tuple) ?
+					id->dps & NVME_NS_DPS_PI_MASK : 0;
+
+	blk_mq_freeze_queue(disk->queue);
+	if (blk_get_integrity(disk) && (ns->pi_type != pi_type ||
+				ns->ms != old_ms ||
+				bs != queue_logical_block_size(disk->queue) ||
+				(ns->ms && ns->ext)))
+		blk_integrity_unregister(disk);
+
+	ns->pi_type = pi_type;
+	blk_queue_logical_block_size(ns->queue, bs);
+
+	if (ns->ms && !blk_get_integrity(disk) && !ns->ext)
+		nvme_init_integrity(ns);
+	if (ns->ms && !(ns->ms == 8 && ns->pi_type) && !blk_get_integrity(disk))
+		set_capacity(disk, 0);
+	else
+		set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
+
+	if (ns->ctrl->oncs & NVME_CTRL_ONCS_DSM)
+		nvme_config_discard(ns);
+	blk_mq_unfreeze_queue(disk->queue);
+
+	kfree(id);
+	return 0;
+}
+
+static char nvme_pr_type(enum pr_type type)
+{
+	switch (type) {
+	case PR_WRITE_EXCLUSIVE:
+		return 1;
+	case PR_EXCLUSIVE_ACCESS:
+		return 2;
+	case PR_WRITE_EXCLUSIVE_REG_ONLY:
+		return 3;
+	case PR_EXCLUSIVE_ACCESS_REG_ONLY:
+		return 4;
+	case PR_WRITE_EXCLUSIVE_ALL_REGS:
+		return 5;
+	case PR_EXCLUSIVE_ACCESS_ALL_REGS:
+		return 6;
+	default:
+		return 0;
+	}
+};
+
+static int nvme_pr_command(struct block_device *bdev, u32 cdw10,
+				u64 key, u64 sa_key, u8 op)
+{
+	struct nvme_ns *ns = bdev->bd_disk->private_data;
+	struct nvme_command c;
+	u8 data[16] = { 0, };
+
+	put_unaligned_le64(key, &data[0]);
+	put_unaligned_le64(sa_key, &data[8]);
+
+	memset(&c, 0, sizeof(c));
+	c.common.opcode = op;
+	c.common.nsid = cpu_to_le32(ns->ns_id);
+	c.common.cdw10[0] = cpu_to_le32(cdw10);
+
+	return nvme_submit_sync_cmd(ns->queue, &c, data, 16);
+}
+
+static int nvme_pr_register(struct block_device *bdev, u64 old,
+		u64 new, unsigned flags)
+{
+	u32 cdw10;
+
+	if (flags & ~PR_FL_IGNORE_KEY)
+		return -EOPNOTSUPP;
+
+	cdw10 = old ? 2 : 0;
+	cdw10 |= (flags & PR_FL_IGNORE_KEY) ? 1 << 3 : 0;
+	cdw10 |= (1 << 30) | (1 << 31); /* PTPL=1 */
+	return nvme_pr_command(bdev, cdw10, old, new, nvme_cmd_resv_register);
+}
+
+static int nvme_pr_reserve(struct block_device *bdev, u64 key,
+		enum pr_type type, unsigned flags)
+{
+	u32 cdw10;
+
+	if (flags & ~PR_FL_IGNORE_KEY)
+		return -EOPNOTSUPP;
+
+	cdw10 = nvme_pr_type(type) << 8;
+	cdw10 |= ((flags & PR_FL_IGNORE_KEY) ? 1 << 3 : 0);
+	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_acquire);
+}
+
+static int nvme_pr_preempt(struct block_device *bdev, u64 old, u64 new,
+		enum pr_type type, bool abort)
+{
+	u32 cdw10 = nvme_pr_type(type) << 8 | abort ? 2 : 1;
+	return nvme_pr_command(bdev, cdw10, old, new, nvme_cmd_resv_acquire);
+}
+
+static int nvme_pr_clear(struct block_device *bdev, u64 key)
+{
+	u32 cdw10 = 1 | (key ? 1 << 3 : 0);
+	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_register);
+}
+
+static int nvme_pr_release(struct block_device *bdev, u64 key, enum pr_type type)
+{
+	u32 cdw10 = nvme_pr_type(type) << 8 | key ? 1 << 3 : 0;
+	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_release);
+}
+
+static const struct pr_ops nvme_pr_ops = {
+	.pr_register	= nvme_pr_register,
+	.pr_reserve	= nvme_pr_reserve,
+	.pr_release	= nvme_pr_release,
+	.pr_preempt	= nvme_pr_preempt,
+	.pr_clear	= nvme_pr_clear,
+};
+
+static const struct block_device_operations nvme_fops = {
+	.owner		= THIS_MODULE,
+	.ioctl		= nvme_ioctl,
+	.compat_ioctl	= nvme_compat_ioctl,
+	.open		= nvme_open,
+	.release	= nvme_release,
+	.getgeo		= nvme_getgeo,
+	.revalidate_disk= nvme_revalidate_disk,
+	.pr_ops		= &nvme_pr_ops,
+};
+
+static int nvme_wait_ready(struct nvme_ctrl *ctrl, u64 cap, bool enabled)
+{
+	unsigned long timeout =
+		((NVME_CAP_TIMEOUT(cap) + 1) * HZ / 2) + jiffies;
+	u32 csts, bit = enabled ? NVME_CSTS_RDY : 0;
+	int ret;
+
+	while ((ret = ctrl->ops->reg_read32(ctrl, NVME_REG_CSTS, &csts)) == 0) {
+		if ((csts & NVME_CSTS_RDY) == bit)
+			break;
+
+		msleep(100);
+		if (fatal_signal_pending(current))
+			return -EINTR;
+		if (time_after(jiffies, timeout)) {
+			dev_err(ctrl->dev,
+				"Device not ready; aborting %s\n", enabled ?
+						"initialisation" : "reset");
+			return -ENODEV;
+		}
+	}
+
+	return ret;
+}
+
+/*
+ * If the device has been passed off to us in an enabled state, just clear
+ * the enabled bit.  The spec says we should set the 'shutdown notification
+ * bits', but doing so may cause the device to complete commands to the
+ * admin queue ... and we don't know what memory that might be pointing at!
+ */
+int nvme_disable_ctrl(struct nvme_ctrl *ctrl, u64 cap)
+{
+	int ret;
+
+	ctrl->ctrl_config &= ~NVME_CC_SHN_MASK;
+	ctrl->ctrl_config &= ~NVME_CC_ENABLE;
+
+	ret = ctrl->ops->reg_write32(ctrl, NVME_REG_CC, ctrl->ctrl_config);
+	if (ret)
+		return ret;
+	return nvme_wait_ready(ctrl, cap, false);
+}
+
+int nvme_enable_ctrl(struct nvme_ctrl *ctrl, u64 cap)
+{
+	/*
+	 * Default to a 4K page size, with the intention to update this
+	 * path in the future to accomodate architectures with differing
+	 * kernel and IO page sizes.
+	 */
+	unsigned dev_page_min = NVME_CAP_MPSMIN(cap) + 12, page_shift = 12;
+	int ret;
+
+	if (page_shift < dev_page_min) {
+		dev_err(ctrl->dev,
+			"Minimum device page size %u too large for host (%u)\n",
+			1 << dev_page_min, 1 << page_shift);
+		return -ENODEV;
+	}
+
+	ctrl->page_size = 1 << page_shift;
+
+	ctrl->ctrl_config = NVME_CC_CSS_NVM;
+	ctrl->ctrl_config |= (page_shift - 12) << NVME_CC_MPS_SHIFT;
+	ctrl->ctrl_config |= NVME_CC_ARB_RR | NVME_CC_SHN_NONE;
+	ctrl->ctrl_config |= NVME_CC_IOSQES | NVME_CC_IOCQES;
+	ctrl->ctrl_config |= NVME_CC_ENABLE;
+
+	ret = ctrl->ops->reg_write32(ctrl, NVME_REG_CC, ctrl->ctrl_config);
+	if (ret)
+		return ret;
+	return nvme_wait_ready(ctrl, cap, true);
+}
+
+int nvme_shutdown_ctrl(struct nvme_ctrl *ctrl)
+{
+	unsigned long timeout = SHUTDOWN_TIMEOUT + jiffies;
+	u32 csts;
+	int ret;
+
+	ctrl->ctrl_config &= ~NVME_CC_SHN_MASK;
+	ctrl->ctrl_config |= NVME_CC_SHN_NORMAL;
+
+	ret = ctrl->ops->reg_write32(ctrl, NVME_REG_CC, ctrl->ctrl_config);
+	if (ret)
+		return ret;
+
+	while ((ret = ctrl->ops->reg_read32(ctrl, NVME_REG_CSTS, &csts)) == 0) {
+		if ((csts & NVME_CSTS_SHST_MASK) == NVME_CSTS_SHST_CMPLT)
+			break;
+
+		msleep(100);
+		if (fatal_signal_pending(current))
+			return -EINTR;
+		if (time_after(jiffies, timeout)) {
+			dev_err(ctrl->dev,
+				"Device shutdown incomplete; abort shutdown\n");
+			return -ENODEV;
+		}
+	}
+
+	return ret;
+}
+
+static void nvme_set_queue_limits(struct nvme_ctrl *ctrl,
+		struct request_queue *q)
+{
+	if (ctrl->max_hw_sectors) {
+		u32 max_segments =
+			(ctrl->max_hw_sectors / (ctrl->page_size >> 9)) + 1;
+
+		blk_queue_max_hw_sectors(q, ctrl->max_hw_sectors);
+		blk_queue_max_segments(q, min_t(u32, max_segments, USHRT_MAX));
+	}
+	if (ctrl->stripe_size)
+		blk_queue_chunk_sectors(q, ctrl->stripe_size >> 9);
+	if (ctrl->vwc & NVME_CTRL_VWC_PRESENT)
+		blk_queue_flush(q, REQ_FLUSH | REQ_FUA);
+	blk_queue_virt_boundary(q, ctrl->page_size - 1);
+}
+
+/*
+ * Initialize the cached copies of the Identify data and various controller
+ * register in our nvme_ctrl structure.  This should be called as soon as
+ * the admin queue is fully up and running.
+ */
+int nvme_init_identify(struct nvme_ctrl *ctrl)
+{
+	struct nvme_id_ctrl *id;
+	u64 cap;
+	int ret, page_shift;
+
+	ret = ctrl->ops->reg_read32(ctrl, NVME_REG_VS, &ctrl->vs);
+	if (ret) {
+		dev_err(ctrl->dev, "Reading VS failed (%d)\n", ret);
+		return ret;
+	}
+
+	ret = ctrl->ops->reg_read64(ctrl, NVME_REG_CAP, &cap);
+	if (ret) {
+		dev_err(ctrl->dev, "Reading CAP failed (%d)\n", ret);
+		return ret;
+	}
+	page_shift = NVME_CAP_MPSMIN(cap) + 12;
+
+	if (ctrl->vs >= NVME_VS(1, 1))
+		ctrl->subsystem = NVME_CAP_NSSRC(cap);
+
+	ret = nvme_identify_ctrl(ctrl, &id);
+	if (ret) {
+		dev_err(ctrl->dev, "Identify Controller failed (%d)\n", ret);
+		return -EIO;
+	}
+
+	ctrl->oncs = le16_to_cpup(&id->oncs);
+	atomic_set(&ctrl->abort_limit, id->acl + 1);
+	ctrl->vwc = id->vwc;
+	memcpy(ctrl->serial, id->sn, sizeof(id->sn));
+	memcpy(ctrl->model, id->mn, sizeof(id->mn));
+	memcpy(ctrl->firmware_rev, id->fr, sizeof(id->fr));
+	if (id->mdts)
+		ctrl->max_hw_sectors = 1 << (id->mdts + page_shift - 9);
+	else
+		ctrl->max_hw_sectors = UINT_MAX;
+
+	if ((ctrl->quirks & NVME_QUIRK_STRIPE_SIZE) && id->vs[3]) {
+		unsigned int max_hw_sectors;
+
+		ctrl->stripe_size = 1 << (id->vs[3] + page_shift);
+		max_hw_sectors = ctrl->stripe_size >> (page_shift - 9);
+		if (ctrl->max_hw_sectors) {
+			ctrl->max_hw_sectors = min(max_hw_sectors,
+							ctrl->max_hw_sectors);
+		} else {
+			ctrl->max_hw_sectors = max_hw_sectors;
+		}
+	}
+
+	nvme_set_queue_limits(ctrl, ctrl->admin_q);
+
+	kfree(id);
+	return 0;
+}
+
+static int nvme_dev_open(struct inode *inode, struct file *file)
+{
+	struct nvme_ctrl *ctrl;
+	int instance = iminor(inode);
+	int ret = -ENODEV;
+
+	spin_lock(&dev_list_lock);
+	list_for_each_entry(ctrl, &nvme_ctrl_list, node) {
+		if (ctrl->instance != instance)
+			continue;
+
+		if (!ctrl->admin_q) {
+			ret = -EWOULDBLOCK;
+			break;
+		}
+		if (!kref_get_unless_zero(&ctrl->kref))
+			break;
+		file->private_data = ctrl;
+		ret = 0;
+		break;
+	}
+	spin_unlock(&dev_list_lock);
+
+	return ret;
+}
+
+static int nvme_dev_release(struct inode *inode, struct file *file)
+{
+	nvme_put_ctrl(file->private_data);
+	return 0;
+}
+
+static int nvme_dev_user_cmd(struct nvme_ctrl *ctrl, void __user *argp)
+{
+	struct nvme_ns *ns;
+	int ret;
+
+	mutex_lock(&ctrl->namespaces_mutex);
+	if (list_empty(&ctrl->namespaces)) {
+		ret = -ENOTTY;
+		goto out_unlock;
+	}
+
+	ns = list_first_entry(&ctrl->namespaces, struct nvme_ns, list);
+	if (ns != list_last_entry(&ctrl->namespaces, struct nvme_ns, list)) {
+		dev_warn(ctrl->dev,
+			"NVME_IOCTL_IO_CMD not supported when multiple namespaces present!\n");
+		ret = -EINVAL;
+		goto out_unlock;
+	}
+
+	dev_warn(ctrl->dev,
+		"using deprecated NVME_IOCTL_IO_CMD ioctl on the char device!\n");
+	kref_get(&ns->kref);
+	mutex_unlock(&ctrl->namespaces_mutex);
+
+	ret = nvme_user_cmd(ctrl, ns, argp);
+	nvme_put_ns(ns);
+	return ret;
+
+out_unlock:
+	mutex_unlock(&ctrl->namespaces_mutex);
+	return ret;
+}
+
+static long nvme_dev_ioctl(struct file *file, unsigned int cmd,
+		unsigned long arg)
+{
+	struct nvme_ctrl *ctrl = file->private_data;
+	void __user *argp = (void __user *)arg;
+
+	switch (cmd) {
+	case NVME_IOCTL_ADMIN_CMD:
+		return nvme_user_cmd(ctrl, NULL, argp);
+	case NVME_IOCTL_IO_CMD:
+		return nvme_dev_user_cmd(ctrl, argp);
+	case NVME_IOCTL_RESET:
+		dev_warn(ctrl->dev, "resetting controller\n");
+		return ctrl->ops->reset_ctrl(ctrl);
+	case NVME_IOCTL_SUBSYS_RESET:
+		return nvme_reset_subsystem(ctrl);
+	default:
+		return -ENOTTY;
+	}
+}
+
+static const struct file_operations nvme_dev_fops = {
+	.owner		= THIS_MODULE,
+	.open		= nvme_dev_open,
+	.release	= nvme_dev_release,
+	.unlocked_ioctl	= nvme_dev_ioctl,
+	.compat_ioctl	= nvme_dev_ioctl,
+};
+
+static ssize_t nvme_sysfs_reset(struct device *dev,
+				struct device_attribute *attr, const char *buf,
+				size_t count)
+{
+	struct nvme_ctrl *ctrl = dev_get_drvdata(dev);
+	int ret;
+
+	ret = ctrl->ops->reset_ctrl(ctrl);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+static DEVICE_ATTR(reset_controller, S_IWUSR, NULL, nvme_sysfs_reset);
+
+static ssize_t uuid_show(struct device *dev, struct device_attribute *attr,
+								char *buf)
+{
+	struct nvme_ns *ns = dev_to_disk(dev)->private_data;
+	return sprintf(buf, "%pU\n", ns->uuid);
+}
+static DEVICE_ATTR(uuid, S_IRUGO, uuid_show, NULL);
+
+static ssize_t eui_show(struct device *dev, struct device_attribute *attr,
+								char *buf)
+{
+	struct nvme_ns *ns = dev_to_disk(dev)->private_data;
+	return sprintf(buf, "%8phd\n", ns->eui);
+}
+static DEVICE_ATTR(eui, S_IRUGO, eui_show, NULL);
+
+static ssize_t nsid_show(struct device *dev, struct device_attribute *attr,
+								char *buf)
+{
+	struct nvme_ns *ns = dev_to_disk(dev)->private_data;
+	return sprintf(buf, "%d\n", ns->ns_id);
+}
+static DEVICE_ATTR(nsid, S_IRUGO, nsid_show, NULL);
+
+static struct attribute *nvme_ns_attrs[] = {
+	&dev_attr_uuid.attr,
+	&dev_attr_eui.attr,
+	&dev_attr_nsid.attr,
+	NULL,
+};
+
+static umode_t nvme_attrs_are_visible(struct kobject *kobj,
+		struct attribute *a, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct nvme_ns *ns = dev_to_disk(dev)->private_data;
+
+	if (a == &dev_attr_uuid.attr) {
+		if (!memchr_inv(ns->uuid, 0, sizeof(ns->uuid)))
+			return 0;
+	}
+	if (a == &dev_attr_eui.attr) {
+		if (!memchr_inv(ns->eui, 0, sizeof(ns->eui)))
+			return 0;
+	}
+	return a->mode;
+}
+
+static const struct attribute_group nvme_ns_attr_group = {
+	.attrs		= nvme_ns_attrs,
+	.is_visible	= nvme_attrs_are_visible,
+};
+
+#define nvme_show_function(field)						\
+static ssize_t  field##_show(struct device *dev,				\
+			    struct device_attribute *attr, char *buf)		\
+{										\
+        struct nvme_ctrl *ctrl = dev_get_drvdata(dev);				\
+        return sprintf(buf, "%.*s\n", (int)sizeof(ctrl->field), ctrl->field);	\
+}										\
+static DEVICE_ATTR(field, S_IRUGO, field##_show, NULL);
+
+nvme_show_function(model);
+nvme_show_function(serial);
+nvme_show_function(firmware_rev);
+
+static struct attribute *nvme_dev_attrs[] = {
+	&dev_attr_reset_controller.attr,
+	&dev_attr_model.attr,
+	&dev_attr_serial.attr,
+	&dev_attr_firmware_rev.attr,
+	NULL
+};
+
+static struct attribute_group nvme_dev_attrs_group = {
+	.attrs = nvme_dev_attrs,
+};
+
+static const struct attribute_group *nvme_dev_attr_groups[] = {
+	&nvme_dev_attrs_group,
+	NULL,
+};
+
+static int ns_cmp(void *priv, struct list_head *a, struct list_head *b)
+{
+	struct nvme_ns *nsa = container_of(a, struct nvme_ns, list);
+	struct nvme_ns *nsb = container_of(b, struct nvme_ns, list);
+
+	return nsa->ns_id - nsb->ns_id;
+}
+
+static struct nvme_ns *nvme_find_ns(struct nvme_ctrl *ctrl, unsigned nsid)
+{
+	struct nvme_ns *ns;
+
+	lockdep_assert_held(&ctrl->namespaces_mutex);
+
+	list_for_each_entry(ns, &ctrl->namespaces, list) {
+		if (ns->ns_id == nsid)
+			return ns;
+		if (ns->ns_id > nsid)
+			break;
+	}
+	return NULL;
+}
+
+static void nvme_alloc_ns(struct nvme_ctrl *ctrl, unsigned nsid)
+{
+	struct nvme_ns *ns;
+	struct gendisk *disk;
+	int node = dev_to_node(ctrl->dev);
+
+	lockdep_assert_held(&ctrl->namespaces_mutex);
+
+	ns = kzalloc_node(sizeof(*ns), GFP_KERNEL, node);
+	if (!ns)
+		return;
+
+	ns->instance = ida_simple_get(&ctrl->ns_ida, 1, 0, GFP_KERNEL);
+	if (ns->instance < 0)
+		goto out_free_ns;
+
+	ns->queue = blk_mq_init_queue(ctrl->tagset);
+	if (IS_ERR(ns->queue))
+		goto out_release_instance;
+	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, ns->queue);
+	ns->queue->queuedata = ns;
+	ns->ctrl = ctrl;
+
+	disk = alloc_disk_node(0, node);
+	if (!disk)
+		goto out_free_queue;
+
+	kref_init(&ns->kref);
+	ns->ns_id = nsid;
+	ns->disk = disk;
+	ns->lba_shift = 9; /* set to a default value for 512 until disk is validated */
+
+
+	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
+	nvme_set_queue_limits(ctrl, ns->queue);
+
+	disk->major = nvme_major;
+	disk->first_minor = 0;
+	disk->fops = &nvme_fops;
+	disk->private_data = ns;
+	disk->queue = ns->queue;
+	disk->driverfs_dev = ctrl->device;
+	disk->flags = GENHD_FL_EXT_DEVT;
+	sprintf(disk->disk_name, "nvme%dn%d", ctrl->instance, ns->instance);
+
+	if (nvme_revalidate_disk(ns->disk))
+		goto out_free_disk;
+
+	list_add_tail(&ns->list, &ctrl->namespaces);
+	kref_get(&ctrl->kref);
+	if (ns->type == NVME_NS_LIGHTNVM)
+		return;
+
+	add_disk(ns->disk);
+	if (sysfs_create_group(&disk_to_dev(ns->disk)->kobj,
+					&nvme_ns_attr_group))
+		pr_warn("%s: failed to create sysfs group for identification\n",
+			ns->disk->disk_name);
+	return;
+ out_free_disk:
+	kfree(disk);
+ out_free_queue:
+	blk_cleanup_queue(ns->queue);
+ out_release_instance:
+	ida_simple_remove(&ctrl->ns_ida, ns->instance);
+ out_free_ns:
+	kfree(ns);
+}
+
+static void nvme_ns_remove(struct nvme_ns *ns)
+{
+	if (test_and_set_bit(NVME_NS_REMOVING, &ns->flags))
+		return;
+
+	if (ns->disk->flags & GENHD_FL_UP) {
+		if (blk_get_integrity(ns->disk))
+			blk_integrity_unregister(ns->disk);
+		sysfs_remove_group(&disk_to_dev(ns->disk)->kobj,
+					&nvme_ns_attr_group);
+		del_gendisk(ns->disk);
+		blk_mq_abort_requeue_list(ns->queue);
+		blk_cleanup_queue(ns->queue);
+	}
+	mutex_lock(&ns->ctrl->namespaces_mutex);
+	list_del_init(&ns->list);
+	mutex_unlock(&ns->ctrl->namespaces_mutex);
+	nvme_put_ns(ns);
+}
+
+static void nvme_validate_ns(struct nvme_ctrl *ctrl, unsigned nsid)
+{
+	struct nvme_ns *ns;
+
+	ns = nvme_find_ns(ctrl, nsid);
+	if (ns) {
+		if (revalidate_disk(ns->disk))
+			nvme_ns_remove(ns);
+	} else
+		nvme_alloc_ns(ctrl, nsid);
+}
+
+static int nvme_scan_ns_list(struct nvme_ctrl *ctrl, unsigned nn)
+{
+	struct nvme_ns *ns;
+	__le32 *ns_list;
+	unsigned i, j, nsid, prev = 0, num_lists = DIV_ROUND_UP(nn, 1024);
+	int ret = 0;
+
+	ns_list = kzalloc(0x1000, GFP_KERNEL);
+	if (!ns_list)
+		return -ENOMEM;
+
+	for (i = 0; i < num_lists; i++) {
+		ret = nvme_identify_ns_list(ctrl, prev, ns_list);
+		if (ret)
+			goto out;
+
+		for (j = 0; j < min(nn, 1024U); j++) {
+			nsid = le32_to_cpu(ns_list[j]);
+			if (!nsid)
+				goto out;
+
+			nvme_validate_ns(ctrl, nsid);
+
+			while (++prev < nsid) {
+				ns = nvme_find_ns(ctrl, prev);
+				if (ns)
+					nvme_ns_remove(ns);
+			}
+		}
+		nn -= j;
+	}
+ out:
+	kfree(ns_list);
+	return ret;
+}
+
+static void __nvme_scan_namespaces(struct nvme_ctrl *ctrl, unsigned nn)
+{
+	struct nvme_ns *ns, *next;
+	unsigned i;
+
+	lockdep_assert_held(&ctrl->namespaces_mutex);
+
+	for (i = 1; i <= nn; i++)
+		nvme_validate_ns(ctrl, i);
+
+	list_for_each_entry_safe(ns, next, &ctrl->namespaces, list) {
+		if (ns->ns_id > nn)
+			nvme_ns_remove(ns);
+	}
+}
+
+void nvme_scan_namespaces(struct nvme_ctrl *ctrl)
+{
+	struct nvme_id_ctrl *id;
+	unsigned nn;
+
+	if (nvme_identify_ctrl(ctrl, &id))
+		return;
+
+	mutex_lock(&ctrl->namespaces_mutex);
+	nn = le32_to_cpu(id->nn);
+	if (ctrl->vs >= NVME_VS(1, 1) &&
+	    !(ctrl->quirks & NVME_QUIRK_IDENTIFY_CNS)) {
+		if (!nvme_scan_ns_list(ctrl, nn))
+			goto done;
+	}
+	__nvme_scan_namespaces(ctrl, le32_to_cpup(&id->nn));
+ done:
+	list_sort(NULL, &ctrl->namespaces, ns_cmp);
+	mutex_unlock(&ctrl->namespaces_mutex);
+	kfree(id);
+}
+
+void nvme_remove_namespaces(struct nvme_ctrl *ctrl)
+{
+	struct nvme_ns *ns, *next;
+
+	list_for_each_entry_safe(ns, next, &ctrl->namespaces, list)
+		nvme_ns_remove(ns);
+}
+
+static DEFINE_IDA(nvme_instance_ida);
+
+static int nvme_set_instance(struct nvme_ctrl *ctrl)
+{
+	int instance, error;
+
+	do {
+		if (!ida_pre_get(&nvme_instance_ida, GFP_KERNEL))
+			return -ENODEV;
+
+		spin_lock(&dev_list_lock);
+		error = ida_get_new(&nvme_instance_ida, &instance);
+		spin_unlock(&dev_list_lock);
+	} while (error == -EAGAIN);
+
+	if (error)
+		return -ENODEV;
+
+	ctrl->instance = instance;
+	return 0;
+}
+
+static void nvme_release_instance(struct nvme_ctrl *ctrl)
+{
+	spin_lock(&dev_list_lock);
+	ida_remove(&nvme_instance_ida, ctrl->instance);
+	spin_unlock(&dev_list_lock);
+}
+
+void nvme_uninit_ctrl(struct nvme_ctrl *ctrl)
+ {
+	device_destroy(nvme_class, MKDEV(nvme_char_major, ctrl->instance));
+
+	spin_lock(&dev_list_lock);
+	list_del(&ctrl->node);
+	spin_unlock(&dev_list_lock);
+}
+
+static void nvme_free_ctrl(struct kref *kref)
+{
+	struct nvme_ctrl *ctrl = container_of(kref, struct nvme_ctrl, kref);
+
+	put_device(ctrl->device);
+	nvme_release_instance(ctrl);
+	ida_destroy(&ctrl->ns_ida);
+
+	ctrl->ops->free_ctrl(ctrl);
+}
+
+void nvme_put_ctrl(struct nvme_ctrl *ctrl)
+{
+	kref_put(&ctrl->kref, nvme_free_ctrl);
+}
+
+/*
+ * Initialize a NVMe controller structures.  This needs to be called during
+ * earliest initialization so that we have the initialized structured around
+ * during probing.
+ */
+int nvme_init_ctrl(struct nvme_ctrl *ctrl, struct device *dev,
+		const struct nvme_ctrl_ops *ops, unsigned long quirks)
+{
+	int ret;
+
+	INIT_LIST_HEAD(&ctrl->namespaces);
+	mutex_init(&ctrl->namespaces_mutex);
+	kref_init(&ctrl->kref);
+	ctrl->dev = dev;
+	ctrl->ops = ops;
+	ctrl->quirks = quirks;
+
+	ret = nvme_set_instance(ctrl);
+	if (ret)
+		goto out;
+
+	ctrl->device = device_create_with_groups(nvme_class, ctrl->dev,
+				MKDEV(nvme_char_major, ctrl->instance),
+				dev, nvme_dev_attr_groups,
+				"nvme%d", ctrl->instance);
+	if (IS_ERR(ctrl->device)) {
+		ret = PTR_ERR(ctrl->device);
+		goto out_release_instance;
+	}
+	get_device(ctrl->device);
+	dev_set_drvdata(ctrl->device, ctrl);
+	ida_init(&ctrl->ns_ida);
+
+	spin_lock(&dev_list_lock);
+	list_add_tail(&ctrl->node, &nvme_ctrl_list);
+	spin_unlock(&dev_list_lock);
+
+	return 0;
+out_release_instance:
+	nvme_release_instance(ctrl);
+out:
+	return ret;
+}
+
+/**
+ * nvme_kill_queues(): Ends all namespace queues
+ * @ctrl: the dead controller that needs to end
+ *
+ * Call this function when the driver determines it is unable to get the
+ * controller in a state capable of servicing IO.
+ */
+void nvme_kill_queues(struct nvme_ctrl *ctrl)
+{
+	struct nvme_ns *ns;
+
+	mutex_lock(&ctrl->namespaces_mutex);
+	list_for_each_entry(ns, &ctrl->namespaces, list) {
+		if (!kref_get_unless_zero(&ns->kref))
+			continue;
+
+		/*
+		 * Revalidating a dead namespace sets capacity to 0. This will
+		 * end buffered writers dirtying pages that can't be synced.
+		 */
+		if (!test_and_set_bit(NVME_NS_DEAD, &ns->flags))
+			revalidate_disk(ns->disk);
+
+		blk_set_queue_dying(ns->queue);
+		blk_mq_abort_requeue_list(ns->queue);
+		blk_mq_start_stopped_hw_queues(ns->queue, true);
+
+		nvme_put_ns(ns);
+	}
+	mutex_unlock(&ctrl->namespaces_mutex);
+}
+
+void nvme_stop_queues(struct nvme_ctrl *ctrl)
+{
+	struct nvme_ns *ns;
+
+	mutex_lock(&ctrl->namespaces_mutex);
+	list_for_each_entry(ns, &ctrl->namespaces, list) {
+		spin_lock_irq(ns->queue->queue_lock);
+		queue_flag_set(QUEUE_FLAG_STOPPED, ns->queue);
+		spin_unlock_irq(ns->queue->queue_lock);
+
+		blk_mq_cancel_requeue_work(ns->queue);
+		blk_mq_stop_hw_queues(ns->queue);
+	}
+	mutex_unlock(&ctrl->namespaces_mutex);
+}
+
+void nvme_start_queues(struct nvme_ctrl *ctrl)
+{
+	struct nvme_ns *ns;
+
+	mutex_lock(&ctrl->namespaces_mutex);
+	list_for_each_entry(ns, &ctrl->namespaces, list) {
+		queue_flag_clear_unlocked(QUEUE_FLAG_STOPPED, ns->queue);
+		blk_mq_start_stopped_hw_queues(ns->queue, true);
+		blk_mq_kick_requeue_list(ns->queue);
+	}
+	mutex_unlock(&ctrl->namespaces_mutex);
+}
+
+int __init nvme_core_init(void)
+{
+	int result;
+
+	result = register_blkdev(nvme_major, "nvme");
+	if (result < 0)
+		return result;
+	else if (result > 0)
+		nvme_major = result;
+
+	result = __register_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme",
+							&nvme_dev_fops);
+	if (result < 0)
+		goto unregister_blkdev;
+	else if (result > 0)
+		nvme_char_major = result;
+
+	nvme_class = class_create(THIS_MODULE, "nvme");
+	if (IS_ERR(nvme_class)) {
+		result = PTR_ERR(nvme_class);
+		goto unregister_chrdev;
+	}
+
+	return 0;
+
+ unregister_chrdev:
+	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
+ unregister_blkdev:
+	unregister_blkdev(nvme_major, "nvme");
+	return result;
+}
+
+void nvme_core_exit(void)
+{
+	unregister_blkdev(nvme_major, "nvme");
+	class_destroy(nvme_class);
+	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
+}
diff --git a/drivers/nvme/host/lightnvm.c b/drivers/nvme/host/lightnvm.c
index 15f2acb4d5cd..6bb15e4926dc 100644
--- a/drivers/nvme/host/lightnvm.c
+++ b/drivers/nvme/host/lightnvm.c
@@ -146,6 +146,17 @@ struct nvme_nvm_command {
 	};
 };
 
+#define NVME_NVM_LP_MLC_PAIRS 886
+struct nvme_nvm_lp_mlc {
+	__u16			num_pairs;
+	__u8			pairs[NVME_NVM_LP_MLC_PAIRS];
+};
+
+struct nvme_nvm_lp_tbl {
+	__u8			id[8];
+	struct nvme_nvm_lp_mlc	mlc;
+};
+
 struct nvme_nvm_id_group {
 	__u8			mtype;
 	__u8			fmtype;
@@ -169,7 +180,8 @@ struct nvme_nvm_id_group {
 	__le32			mpos;
 	__le32			mccap;
 	__le16			cpar;
-	__u8			reserved[906];
+	__u8			reserved[10];
+	struct nvme_nvm_lp_tbl lptbl;
 } __packed;
 
 struct nvme_nvm_addr_format {
@@ -266,6 +278,20 @@ static int init_grps(struct nvm_id *nvm_id, struct nvme_nvm_id *nvme_nvm_id)
 		dst->mccap = le32_to_cpu(src->mccap);
 
 		dst->cpar = le16_to_cpu(src->cpar);
+
+		if (dst->fmtype == NVM_ID_FMTYPE_MLC) {
+			memcpy(dst->lptbl.id, src->lptbl.id, 8);
+			dst->lptbl.mlc.num_pairs =
+					le16_to_cpu(src->lptbl.mlc.num_pairs);
+
+			if (dst->lptbl.mlc.num_pairs > NVME_NVM_LP_MLC_PAIRS) {
+				pr_err("nvm: number of MLC pairs not supported\n");
+				return -EINVAL;
+			}
+
+			memcpy(dst->lptbl.mlc.pairs, src->lptbl.mlc.pairs,
+						dst->lptbl.mlc.num_pairs);
+		}
 	}
 
 	return 0;
@@ -274,7 +300,6 @@ static int init_grps(struct nvm_id *nvm_id, struct nvme_nvm_id *nvme_nvm_id)
 static int nvme_nvm_identity(struct nvm_dev *nvmdev, struct nvm_id *nvm_id)
 {
 	struct nvme_ns *ns = nvmdev->q->queuedata;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_nvm_id *nvme_nvm_id;
 	struct nvme_nvm_command c = {};
 	int ret;
@@ -287,7 +312,7 @@ static int nvme_nvm_identity(struct nvm_dev *nvmdev, struct nvm_id *nvm_id)
 	if (!nvme_nvm_id)
 		return -ENOMEM;
 
-	ret = nvme_submit_sync_cmd(dev->admin_q, (struct nvme_command *)&c,
+	ret = nvme_submit_sync_cmd(ns->ctrl->admin_q, (struct nvme_command *)&c,
 				nvme_nvm_id, sizeof(struct nvme_nvm_id));
 	if (ret) {
 		ret = -EIO;
@@ -312,9 +337,8 @@ static int nvme_nvm_get_l2p_tbl(struct nvm_dev *nvmdev, u64 slba, u32 nlb,
 				nvm_l2p_update_fn *update_l2p, void *priv)
 {
 	struct nvme_ns *ns = nvmdev->q->queuedata;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_nvm_command c = {};
-	u32 len = queue_max_hw_sectors(dev->admin_q) << 9;
+	u32 len = queue_max_hw_sectors(ns->ctrl->admin_q) << 9;
 	u32 nlb_pr_rq = len / sizeof(u64);
 	u64 cmd_slba = slba;
 	void *entries;
@@ -332,10 +356,10 @@ static int nvme_nvm_get_l2p_tbl(struct nvm_dev *nvmdev, u64 slba, u32 nlb,
 		c.l2p.slba = cpu_to_le64(cmd_slba);
 		c.l2p.nlb = cpu_to_le32(cmd_nlb);
 
-		ret = nvme_submit_sync_cmd(dev->admin_q,
+		ret = nvme_submit_sync_cmd(ns->ctrl->admin_q,
 				(struct nvme_command *)&c, entries, len);
 		if (ret) {
-			dev_err(dev->dev, "L2P table transfer failed (%d)\n",
+			dev_err(ns->ctrl->dev, "L2P table transfer failed (%d)\n",
 									ret);
 			ret = -EIO;
 			goto out;
@@ -361,7 +385,7 @@ static int nvme_nvm_get_bb_tbl(struct nvm_dev *nvmdev, struct ppa_addr ppa,
 {
 	struct request_queue *q = nvmdev->q;
 	struct nvme_ns *ns = q->queuedata;
-	struct nvme_dev *dev = ns->dev;
+	struct nvme_ctrl *ctrl = ns->ctrl;
 	struct nvme_nvm_command c = {};
 	struct nvme_nvm_bb_tbl *bb_tbl;
 	int tblsz = sizeof(struct nvme_nvm_bb_tbl) + nr_blocks;
@@ -375,41 +399,36 @@ static int nvme_nvm_get_bb_tbl(struct nvm_dev *nvmdev, struct ppa_addr ppa,
 	if (!bb_tbl)
 		return -ENOMEM;
 
-	ret = nvme_submit_sync_cmd(dev->admin_q, (struct nvme_command *)&c,
+	ret = nvme_submit_sync_cmd(ctrl->admin_q, (struct nvme_command *)&c,
 								bb_tbl, tblsz);
 	if (ret) {
-		dev_err(dev->dev, "get bad block table failed (%d)\n", ret);
+		dev_err(ctrl->dev, "get bad block table failed (%d)\n", ret);
 		ret = -EIO;
 		goto out;
 	}
 
 	if (bb_tbl->tblid[0] != 'B' || bb_tbl->tblid[1] != 'B' ||
 		bb_tbl->tblid[2] != 'L' || bb_tbl->tblid[3] != 'T') {
-		dev_err(dev->dev, "bbt format mismatch\n");
+		dev_err(ctrl->dev, "bbt format mismatch\n");
 		ret = -EINVAL;
 		goto out;
 	}
 
 	if (le16_to_cpu(bb_tbl->verid) != 1) {
 		ret = -EINVAL;
-		dev_err(dev->dev, "bbt version not supported\n");
+		dev_err(ctrl->dev, "bbt version not supported\n");
 		goto out;
 	}
 
 	if (le32_to_cpu(bb_tbl->tblks) != nr_blocks) {
 		ret = -EINVAL;
-		dev_err(dev->dev, "bbt unsuspected blocks returned (%u!=%u)",
+		dev_err(ctrl->dev, "bbt unsuspected blocks returned (%u!=%u)",
 					le32_to_cpu(bb_tbl->tblks), nr_blocks);
 		goto out;
 	}
 
 	ppa = dev_to_generic_addr(nvmdev, ppa);
 	ret = update_bbtbl(ppa, nr_blocks, bb_tbl->blk, priv);
-	if (ret) {
-		ret = -EINTR;
-		goto out;
-	}
-
 out:
 	kfree(bb_tbl);
 	return ret;
@@ -419,7 +438,6 @@ static int nvme_nvm_set_bb_tbl(struct nvm_dev *nvmdev, struct nvm_rq *rqd,
 								int type)
 {
 	struct nvme_ns *ns = nvmdev->q->queuedata;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_nvm_command c = {};
 	int ret = 0;
 
@@ -429,10 +447,10 @@ static int nvme_nvm_set_bb_tbl(struct nvm_dev *nvmdev, struct nvm_rq *rqd,
 	c.set_bb.nlb = cpu_to_le16(rqd->nr_pages - 1);
 	c.set_bb.value = type;
 
-	ret = nvme_submit_sync_cmd(dev->admin_q, (struct nvme_command *)&c,
+	ret = nvme_submit_sync_cmd(ns->ctrl->admin_q, (struct nvme_command *)&c,
 								NULL, 0);
 	if (ret)
-		dev_err(dev->dev, "set bad block table failed (%d)\n", ret);
+		dev_err(ns->ctrl->dev, "set bad block table failed (%d)\n", ret);
 	return ret;
 }
 
@@ -453,11 +471,8 @@ static inline void nvme_nvm_rqtocmd(struct request *rq, struct nvm_rq *rqd,
 static void nvme_nvm_end_io(struct request *rq, int error)
 {
 	struct nvm_rq *rqd = rq->end_io_data;
-	struct nvm_dev *dev = rqd->dev;
 
-	if (dev->mt && dev->mt->end_io(rqd, error))
-		pr_err("nvme: err status: %x result: %lx\n",
-				rq->errors, (unsigned long)rq->special);
+	nvm_end_io(rqd, error);
 
 	kfree(rq->cmd);
 	blk_mq_free_request(rq);
@@ -471,7 +486,7 @@ static int nvme_nvm_submit_io(struct nvm_dev *dev, struct nvm_rq *rqd)
 	struct bio *bio = rqd->bio;
 	struct nvme_nvm_command *cmd;
 
-	rq = blk_mq_alloc_request(q, bio_rw(bio), GFP_KERNEL, 0);
+	rq = blk_mq_alloc_request(q, bio_rw(bio), 0);
 	if (IS_ERR(rq))
 		return -ENOMEM;
 
@@ -520,9 +535,8 @@ static int nvme_nvm_erase_block(struct nvm_dev *dev, struct nvm_rq *rqd)
 static void *nvme_nvm_create_dma_pool(struct nvm_dev *nvmdev, char *name)
 {
 	struct nvme_ns *ns = nvmdev->q->queuedata;
-	struct nvme_dev *dev = ns->dev;
 
-	return dma_pool_create(name, dev->dev, PAGE_SIZE, PAGE_SIZE, 0);
+	return dma_pool_create(name, ns->ctrl->dev, PAGE_SIZE, PAGE_SIZE, 0);
 }
 
 static void nvme_nvm_destroy_dma_pool(void *pool)
@@ -580,8 +594,9 @@ void nvme_nvm_unregister(struct request_queue *q, char *disk_name)
 
 int nvme_nvm_ns_supported(struct nvme_ns *ns, struct nvme_id_ns *id)
 {
-	struct nvme_dev *dev = ns->dev;
-	struct pci_dev *pdev = to_pci_dev(dev->dev);
+	struct nvme_ctrl *ctrl = ns->ctrl;
+	/* XXX: this is poking into PCI structures from generic code! */
+	struct pci_dev *pdev = to_pci_dev(ctrl->dev);
 
 	/* QEMU NVMe simulator - PCI ID + Vendor specific bit */
 	if (pdev->vendor == PCI_VENDOR_ID_CNEX &&
diff --git a/drivers/nvme/host/nvme.h b/drivers/nvme/host/nvme.h
index 044253dca30a..fb15ba5f5d19 100644
--- a/drivers/nvme/host/nvme.h
+++ b/drivers/nvme/host/nvme.h
@@ -19,58 +19,78 @@
 #include <linux/kref.h>
 #include <linux/blk-mq.h>
 
+enum {
+	/*
+	 * Driver internal status code for commands that were cancelled due
+	 * to timeouts or controller shutdown.  The value is negative so
+	 * that it a) doesn't overlap with the unsigned hardware error codes,
+	 * and b) can easily be tested for.
+	 */
+	NVME_SC_CANCELLED		= -EINTR,
+};
+
 extern unsigned char nvme_io_timeout;
 #define NVME_IO_TIMEOUT	(nvme_io_timeout * HZ)
 
+extern unsigned char admin_timeout;
+#define ADMIN_TIMEOUT	(admin_timeout * HZ)
+
+extern unsigned char shutdown_timeout;
+#define SHUTDOWN_TIMEOUT	(shutdown_timeout * HZ)
+
 enum {
 	NVME_NS_LBA		= 0,
 	NVME_NS_LIGHTNVM	= 1,
 };
 
 /*
- * Represents an NVM Express device.  Each nvme_dev is a PCI function.
+ * List of workarounds for devices that required behavior not specified in
+ * the standard.
  */
-struct nvme_dev {
-	struct list_head node;
-	struct nvme_queue **queues;
+enum nvme_quirks {
+	/*
+	 * Prefers I/O aligned to a stripe size specified in a vendor
+	 * specific Identify field.
+	 */
+	NVME_QUIRK_STRIPE_SIZE			= (1 << 0),
+
+	/*
+	 * The controller doesn't handle Identify value others than 0 or 1
+	 * correctly.
+	 */
+	NVME_QUIRK_IDENTIFY_CNS			= (1 << 1),
+};
+
+struct nvme_ctrl {
+	const struct nvme_ctrl_ops *ops;
 	struct request_queue *admin_q;
-	struct blk_mq_tag_set tagset;
-	struct blk_mq_tag_set admin_tagset;
-	u32 __iomem *dbs;
 	struct device *dev;
-	struct dma_pool *prp_page_pool;
-	struct dma_pool *prp_small_pool;
+	struct kref kref;
 	int instance;
-	unsigned queue_count;
-	unsigned online_queues;
-	unsigned max_qid;
-	int q_depth;
-	u32 db_stride;
-	u32 ctrl_config;
-	struct msix_entry *entry;
-	struct nvme_bar __iomem *bar;
+	struct blk_mq_tag_set *tagset;
 	struct list_head namespaces;
-	struct kref kref;
-	struct device *device;
-	struct work_struct reset_work;
-	struct work_struct probe_work;
-	struct work_struct scan_work;
+	struct mutex namespaces_mutex;
+	struct device *device;	/* char device */
+	struct list_head node;
+	struct ida ns_ida;
+
 	char name[12];
 	char serial[20];
 	char model[40];
 	char firmware_rev[8];
-	bool subsystem;
+
+	u32 ctrl_config;
+
+	u32 page_size;
 	u32 max_hw_sectors;
 	u32 stripe_size;
-	u32 page_size;
-	void __iomem *cmb;
-	dma_addr_t cmb_dma_addr;
-	u64 cmb_size;
-	u32 cmbsz;
 	u16 oncs;
-	u16 abort_limit;
+	atomic_t abort_limit;
 	u8 event_limit;
 	u8 vwc;
+	u32 vs;
+	bool subsystem;
+	unsigned long quirks;
 };
 
 /*
@@ -79,10 +99,14 @@ struct nvme_dev {
 struct nvme_ns {
 	struct list_head list;
 
-	struct nvme_dev *dev;
+	struct nvme_ctrl *ctrl;
 	struct request_queue *queue;
 	struct gendisk *disk;
 	struct kref kref;
+	int instance;
+
+	u8 eui[8];
+	u8 uuid[16];
 
 	unsigned ns_id;
 	int lba_shift;
@@ -90,45 +114,166 @@ struct nvme_ns {
 	bool ext;
 	u8 pi_type;
 	int type;
+	unsigned long flags;
+
+#define NVME_NS_REMOVING 0
+#define NVME_NS_DEAD     1
+
 	u64 mode_select_num_blocks;
 	u32 mode_select_block_len;
 };
 
-/*
- * The nvme_iod describes the data in an I/O, including the list of PRP
- * entries.  You can't see it in this data structure because C doesn't let
- * me express that.  Use nvme_alloc_iod to ensure there's enough space
- * allocated to store the PRP list.
- */
-struct nvme_iod {
-	unsigned long private;	/* For the use of the submitter of the I/O */
-	int npages;		/* In the PRP list. 0 means small pool in use */
-	int offset;		/* Of PRP list */
-	int nents;		/* Used in scatterlist */
-	int length;		/* Of data, in bytes */
-	dma_addr_t first_dma;
-	struct scatterlist meta_sg[1]; /* metadata requires single contiguous buffer */
-	struct scatterlist sg[0];
+struct nvme_ctrl_ops {
+	int (*reg_read32)(struct nvme_ctrl *ctrl, u32 off, u32 *val);
+	int (*reg_write32)(struct nvme_ctrl *ctrl, u32 off, u32 val);
+	int (*reg_read64)(struct nvme_ctrl *ctrl, u32 off, u64 *val);
+	bool (*io_incapable)(struct nvme_ctrl *ctrl);
+	int (*reset_ctrl)(struct nvme_ctrl *ctrl);
+	void (*free_ctrl)(struct nvme_ctrl *ctrl);
 };
 
+static inline bool nvme_ctrl_ready(struct nvme_ctrl *ctrl)
+{
+	u32 val = 0;
+
+	if (ctrl->ops->reg_read32(ctrl, NVME_REG_CSTS, &val))
+		return false;
+	return val & NVME_CSTS_RDY;
+}
+
+static inline bool nvme_io_incapable(struct nvme_ctrl *ctrl)
+{
+	u32 val = 0;
+
+	if (ctrl->ops->io_incapable(ctrl))
+		return true;
+	if (ctrl->ops->reg_read32(ctrl, NVME_REG_CSTS, &val))
+		return true;
+	return val & NVME_CSTS_CFS;
+}
+
+static inline int nvme_reset_subsystem(struct nvme_ctrl *ctrl)
+{
+	if (!ctrl->subsystem)
+		return -ENOTTY;
+	return ctrl->ops->reg_write32(ctrl, NVME_REG_NSSR, 0x4E564D65);
+}
+
 static inline u64 nvme_block_nr(struct nvme_ns *ns, sector_t sector)
 {
 	return (sector >> (ns->lba_shift - 9));
 }
 
+static inline void nvme_setup_flush(struct nvme_ns *ns,
+		struct nvme_command *cmnd)
+{
+	memset(cmnd, 0, sizeof(*cmnd));
+	cmnd->common.opcode = nvme_cmd_flush;
+	cmnd->common.nsid = cpu_to_le32(ns->ns_id);
+}
+
+static inline void nvme_setup_rw(struct nvme_ns *ns, struct request *req,
+		struct nvme_command *cmnd)
+{
+	u16 control = 0;
+	u32 dsmgmt = 0;
+
+	if (req->cmd_flags & REQ_FUA)
+		control |= NVME_RW_FUA;
+	if (req->cmd_flags & (REQ_FAILFAST_DEV | REQ_RAHEAD))
+		control |= NVME_RW_LR;
+
+	if (req->cmd_flags & REQ_RAHEAD)
+		dsmgmt |= NVME_RW_DSM_FREQ_PREFETCH;
+
+	memset(cmnd, 0, sizeof(*cmnd));
+	cmnd->rw.opcode = (rq_data_dir(req) ? nvme_cmd_write : nvme_cmd_read);
+	cmnd->rw.command_id = req->tag;
+	cmnd->rw.nsid = cpu_to_le32(ns->ns_id);
+	cmnd->rw.slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
+	cmnd->rw.length = cpu_to_le16((blk_rq_bytes(req) >> ns->lba_shift) - 1);
+
+	if (ns->ms) {
+		switch (ns->pi_type) {
+		case NVME_NS_DPS_PI_TYPE3:
+			control |= NVME_RW_PRINFO_PRCHK_GUARD;
+			break;
+		case NVME_NS_DPS_PI_TYPE1:
+		case NVME_NS_DPS_PI_TYPE2:
+			control |= NVME_RW_PRINFO_PRCHK_GUARD |
+					NVME_RW_PRINFO_PRCHK_REF;
+			cmnd->rw.reftag = cpu_to_le32(
+					nvme_block_nr(ns, blk_rq_pos(req)));
+			break;
+		}
+		if (!blk_integrity_rq(req))
+			control |= NVME_RW_PRINFO_PRACT;
+	}
+
+	cmnd->rw.control = cpu_to_le16(control);
+	cmnd->rw.dsmgmt = cpu_to_le32(dsmgmt);
+}
+
+
+static inline int nvme_error_status(u16 status)
+{
+	switch (status & 0x7ff) {
+	case NVME_SC_SUCCESS:
+		return 0;
+	case NVME_SC_CAP_EXCEEDED:
+		return -ENOSPC;
+	default:
+		return -EIO;
+	}
+}
+
+static inline bool nvme_req_needs_retry(struct request *req, u16 status)
+{
+	return !(status & NVME_SC_DNR || blk_noretry_request(req)) &&
+		(jiffies - req->start_time) < req->timeout;
+}
+
+int nvme_disable_ctrl(struct nvme_ctrl *ctrl, u64 cap);
+int nvme_enable_ctrl(struct nvme_ctrl *ctrl, u64 cap);
+int nvme_shutdown_ctrl(struct nvme_ctrl *ctrl);
+int nvme_init_ctrl(struct nvme_ctrl *ctrl, struct device *dev,
+		const struct nvme_ctrl_ops *ops, unsigned long quirks);
+void nvme_uninit_ctrl(struct nvme_ctrl *ctrl);
+void nvme_put_ctrl(struct nvme_ctrl *ctrl);
+int nvme_init_identify(struct nvme_ctrl *ctrl);
+
+void nvme_scan_namespaces(struct nvme_ctrl *ctrl);
+void nvme_remove_namespaces(struct nvme_ctrl *ctrl);
+
+void nvme_stop_queues(struct nvme_ctrl *ctrl);
+void nvme_start_queues(struct nvme_ctrl *ctrl);
+void nvme_kill_queues(struct nvme_ctrl *ctrl);
+
+struct request *nvme_alloc_request(struct request_queue *q,
+		struct nvme_command *cmd, unsigned int flags);
+void nvme_requeue_req(struct request *req);
 int nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
 		void *buf, unsigned bufflen);
 int __nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
-		void *buffer, void __user *ubuffer, unsigned bufflen,
+		void *buffer, unsigned bufflen,  u32 *result, unsigned timeout);
+int nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void __user *ubuffer, unsigned bufflen, u32 *result,
+		unsigned timeout);
+int __nvme_submit_user_cmd(struct request_queue *q, struct nvme_command *cmd,
+		void __user *ubuffer, unsigned bufflen,
+		void __user *meta_buffer, unsigned meta_len, u32 meta_seed,
 		u32 *result, unsigned timeout);
-int nvme_identify_ctrl(struct nvme_dev *dev, struct nvme_id_ctrl **id);
-int nvme_identify_ns(struct nvme_dev *dev, unsigned nsid,
+int nvme_identify_ctrl(struct nvme_ctrl *dev, struct nvme_id_ctrl **id);
+int nvme_identify_ns(struct nvme_ctrl *dev, unsigned nsid,
 		struct nvme_id_ns **id);
-int nvme_get_log_page(struct nvme_dev *dev, struct nvme_smart_log **log);
-int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
+int nvme_get_log_page(struct nvme_ctrl *dev, struct nvme_smart_log **log);
+int nvme_get_features(struct nvme_ctrl *dev, unsigned fid, unsigned nsid,
 			dma_addr_t dma_addr, u32 *result);
-int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
+int nvme_set_features(struct nvme_ctrl *dev, unsigned fid, unsigned dword11,
 			dma_addr_t dma_addr, u32 *result);
+int nvme_set_queue_count(struct nvme_ctrl *ctrl, int *count);
+
+extern spinlock_t dev_list_lock;
 
 struct sg_io_hdr;
 
@@ -154,4 +299,7 @@ static inline int nvme_nvm_ns_supported(struct nvme_ns *ns, struct nvme_id_ns *i
 }
 #endif /* CONFIG_NVM */
 
+int __init nvme_core_init(void);
+void nvme_core_exit(void);
+
 #endif /* _NVME_H */
diff --git a/drivers/nvme/host/pci.c b/drivers/nvme/host/pci.c
index 0c67b57be83c..680f5780750c 100644
--- a/drivers/nvme/host/pci.c
+++ b/drivers/nvme/host/pci.c
@@ -12,6 +12,7 @@
  * more details.
  */
 
+#include <linux/aer.h>
 #include <linux/bitops.h>
 #include <linux/blkdev.h>
 #include <linux/blk-mq.h>
@@ -28,10 +29,10 @@
 #include <linux/kdev_t.h>
 #include <linux/kthread.h>
 #include <linux/kernel.h>
-#include <linux/list_sort.h>
 #include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/mutex.h>
 #include <linux/pci.h>
 #include <linux/poison.h>
 #include <linux/ptrace.h>
@@ -39,23 +40,24 @@
 #include <linux/slab.h>
 #include <linux/t10-pi.h>
 #include <linux/types.h>
-#include <linux/pr.h>
-#include <scsi/sg.h>
 #include <linux/io-64-nonatomic-lo-hi.h>
 #include <asm/unaligned.h>
 
-#include <uapi/linux/nvme_ioctl.h>
 #include "nvme.h"
 
-#define NVME_MINORS		(1U << MINORBITS)
 #define NVME_Q_DEPTH		1024
 #define NVME_AQ_DEPTH		256
 #define SQ_SIZE(depth)		(depth * sizeof(struct nvme_command))
 #define CQ_SIZE(depth)		(depth * sizeof(struct nvme_completion))
-#define ADMIN_TIMEOUT		(admin_timeout * HZ)
-#define SHUTDOWN_TIMEOUT	(shutdown_timeout * HZ)
+		
+/*
+ * We handle AEN commands ourselves and don't even let the
+ * block layer know about them.
+ */
+#define NVME_NR_AEN_COMMANDS	1
+#define NVME_AQ_BLKMQ_DEPTH	(NVME_AQ_DEPTH - NVME_NR_AEN_COMMANDS)
 
-static unsigned char admin_timeout = 60;
+unsigned char admin_timeout = 60;
 module_param(admin_timeout, byte, 0644);
 MODULE_PARM_DESC(admin_timeout, "timeout in seconds for admin commands");
 
@@ -63,16 +65,10 @@ unsigned char nvme_io_timeout = 30;
 module_param_named(io_timeout, nvme_io_timeout, byte, 0644);
 MODULE_PARM_DESC(io_timeout, "timeout in seconds for I/O");
 
-static unsigned char shutdown_timeout = 5;
+unsigned char shutdown_timeout = 5;
 module_param(shutdown_timeout, byte, 0644);
 MODULE_PARM_DESC(shutdown_timeout, "timeout in seconds for controller shutdown");
 
-static int nvme_major;
-module_param(nvme_major, int, 0);
-
-static int nvme_char_major;
-module_param(nvme_char_major, int, 0);
-
 static int use_threaded_interrupts;
 module_param(use_threaded_interrupts, int, 0);
 
@@ -80,28 +76,60 @@ static bool use_cmb_sqes = true;
 module_param(use_cmb_sqes, bool, 0644);
 MODULE_PARM_DESC(use_cmb_sqes, "use controller's memory buffer for I/O SQes");
 
-static DEFINE_SPINLOCK(dev_list_lock);
 static LIST_HEAD(dev_list);
 static struct task_struct *nvme_thread;
 static struct workqueue_struct *nvme_workq;
 static wait_queue_head_t nvme_kthread_wait;
 
-static struct class *nvme_class;
+struct nvme_dev;
+struct nvme_queue;
 
-static int __nvme_reset(struct nvme_dev *dev);
 static int nvme_reset(struct nvme_dev *dev);
 static void nvme_process_cq(struct nvme_queue *nvmeq);
-static void nvme_dead_ctrl(struct nvme_dev *dev);
+static void nvme_dev_disable(struct nvme_dev *dev, bool shutdown);
 
-struct async_cmd_info {
-	struct kthread_work work;
-	struct kthread_worker *worker;
-	struct request *req;
-	u32 result;
-	int status;
-	void *ctx;
+/*
+ * Represents an NVM Express device.  Each nvme_dev is a PCI function.
+ */
+struct nvme_dev {
+	struct list_head node;
+	struct nvme_queue **queues;
+	struct blk_mq_tag_set tagset;
+	struct blk_mq_tag_set admin_tagset;
+	u32 __iomem *dbs;
+	struct device *dev;
+	struct dma_pool *prp_page_pool;
+	struct dma_pool *prp_small_pool;
+	unsigned queue_count;
+	unsigned online_queues;
+	unsigned max_qid;
+	int q_depth;
+	u32 db_stride;
+	struct msix_entry *entry;
+	void __iomem *bar;
+	struct work_struct reset_work;
+	struct work_struct scan_work;
+	struct work_struct remove_work;
+	struct mutex shutdown_lock;
+	bool subsystem;
+	void __iomem *cmb;
+	dma_addr_t cmb_dma_addr;
+	u64 cmb_size;
+	u32 cmbsz;
+	unsigned long flags;
+
+#define NVME_CTRL_RESETTING    0
+#define NVME_CTRL_REMOVING     1
+
+	struct nvme_ctrl ctrl;
+	struct completion ioq_wait;
 };
 
+static inline struct nvme_dev *to_nvme_dev(struct nvme_ctrl *ctrl)
+{
+	return container_of(ctrl, struct nvme_dev, ctrl);
+}
+
 /*
  * An NVM Express queue.  Each device has at least two (one for admin
  * commands and one for I/O commands).
@@ -126,7 +154,24 @@ struct nvme_queue {
 	u16 qid;
 	u8 cq_phase;
 	u8 cqe_seen;
-	struct async_cmd_info cmdinfo;
+};
+
+/*
+ * The nvme_iod describes the data in an I/O, including the list of PRP
+ * entries.  You can't see it in this data structure because C doesn't let
+ * me express that.  Use nvme_init_iod to ensure there's enough space
+ * allocated to store the PRP list.
+ */
+struct nvme_iod {
+	struct nvme_queue *nvmeq;
+	int aborted;
+	int npages;		/* In the PRP list. 0 means small pool in use */
+	int nents;		/* Used in scatterlist */
+	int length;		/* Of data, in bytes */
+	dma_addr_t first_dma;
+	struct scatterlist meta_sg; /* metadata requires single contiguous buffer */
+	struct scatterlist *sg;
+	struct scatterlist inline_sg[0];
 };
 
 /*
@@ -148,23 +193,11 @@ static inline void _nvme_check_size(void)
 	BUILD_BUG_ON(sizeof(struct nvme_smart_log) != 512);
 }
 
-typedef void (*nvme_completion_fn)(struct nvme_queue *, void *,
-						struct nvme_completion *);
-
-struct nvme_cmd_info {
-	nvme_completion_fn fn;
-	void *ctx;
-	int aborted;
-	struct nvme_queue *nvmeq;
-	struct nvme_iod iod[0];
-};
-
 /*
  * Max size of iod being embedded in the request payload
  */
 #define NVME_INT_PAGES		2
-#define NVME_INT_BYTES(dev)	(NVME_INT_PAGES * (dev)->page_size)
-#define NVME_INT_MASK		0x01
+#define NVME_INT_BYTES(dev)	(NVME_INT_PAGES * (dev)->ctrl.page_size)
 
 /*
  * Will slightly overestimate the number of pages needed.  This is OK
@@ -173,19 +206,22 @@ struct nvme_cmd_info {
  */
 static int nvme_npages(unsigned size, struct nvme_dev *dev)
 {
-	unsigned nprps = DIV_ROUND_UP(size + dev->page_size, dev->page_size);
+	unsigned nprps = DIV_ROUND_UP(size + dev->ctrl.page_size,
+				      dev->ctrl.page_size);
 	return DIV_ROUND_UP(8 * nprps, PAGE_SIZE - 8);
 }
 
-static unsigned int nvme_cmd_size(struct nvme_dev *dev)
+static unsigned int nvme_iod_alloc_size(struct nvme_dev *dev,
+		unsigned int size, unsigned int nseg)
 {
-	unsigned int ret = sizeof(struct nvme_cmd_info);
-
-	ret += sizeof(struct nvme_iod);
-	ret += sizeof(__le64 *) * nvme_npages(NVME_INT_BYTES(dev), dev);
-	ret += sizeof(struct scatterlist) * NVME_INT_PAGES;
+	return sizeof(__le64 *) * nvme_npages(size, dev) +
+			sizeof(struct scatterlist) * nseg;
+}
 
-	return ret;
+static unsigned int nvme_cmd_size(struct nvme_dev *dev)
+{
+	return sizeof(struct nvme_iod) +
+		nvme_iod_alloc_size(dev, NVME_INT_BYTES(dev), NVME_INT_PAGES);
 }
 
 static int nvme_admin_init_hctx(struct blk_mq_hw_ctx *hctx, void *data,
@@ -215,11 +251,11 @@ static int nvme_admin_init_request(void *data, struct request *req,
 				unsigned int numa_node)
 {
 	struct nvme_dev *dev = data;
-	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
 	struct nvme_queue *nvmeq = dev->queues[0];
 
 	BUG_ON(!nvmeq);
-	cmd->nvmeq = nvmeq;
+	iod->nvmeq = nvmeq;
 	return 0;
 }
 
@@ -242,148 +278,47 @@ static int nvme_init_request(void *data, struct request *req,
 				unsigned int numa_node)
 {
 	struct nvme_dev *dev = data;
-	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
 	struct nvme_queue *nvmeq = dev->queues[hctx_idx + 1];
 
 	BUG_ON(!nvmeq);
-	cmd->nvmeq = nvmeq;
+	iod->nvmeq = nvmeq;
 	return 0;
 }
 
-static void nvme_set_info(struct nvme_cmd_info *cmd, void *ctx,
-				nvme_completion_fn handler)
-{
-	cmd->fn = handler;
-	cmd->ctx = ctx;
-	cmd->aborted = 0;
-	blk_mq_start_request(blk_mq_rq_from_pdu(cmd));
-}
-
-static void *iod_get_private(struct nvme_iod *iod)
-{
-	return (void *) (iod->private & ~0x1UL);
-}
-
-/*
- * If bit 0 is set, the iod is embedded in the request payload.
- */
-static bool iod_should_kfree(struct nvme_iod *iod)
-{
-	return (iod->private & NVME_INT_MASK) == 0;
-}
-
-/* Special values must be less than 0x1000 */
-#define CMD_CTX_BASE		((void *)POISON_POINTER_DELTA)
-#define CMD_CTX_CANCELLED	(0x30C + CMD_CTX_BASE)
-#define CMD_CTX_COMPLETED	(0x310 + CMD_CTX_BASE)
-#define CMD_CTX_INVALID		(0x314 + CMD_CTX_BASE)
-
-static void special_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
+static void nvme_queue_scan(struct nvme_dev *dev)
 {
-	if (ctx == CMD_CTX_CANCELLED)
-		return;
-	if (ctx == CMD_CTX_COMPLETED) {
-		dev_warn(nvmeq->q_dmadev,
-				"completed id %d twice on queue %d\n",
-				cqe->command_id, le16_to_cpup(&cqe->sq_id));
-		return;
-	}
-	if (ctx == CMD_CTX_INVALID) {
-		dev_warn(nvmeq->q_dmadev,
-				"invalid id %d completed on queue %d\n",
-				cqe->command_id, le16_to_cpup(&cqe->sq_id));
+	/*
+	 * Do not queue new scan work when a controller is reset during
+	 * removal.
+	 */
+	if (test_bit(NVME_CTRL_REMOVING, &dev->flags))
 		return;
-	}
-	dev_warn(nvmeq->q_dmadev, "Unknown special completion %p\n", ctx);
+	queue_work(nvme_workq, &dev->scan_work);
 }
 
-static void *cancel_cmd_info(struct nvme_cmd_info *cmd, nvme_completion_fn *fn)
+static void nvme_complete_async_event(struct nvme_dev *dev,
+		struct nvme_completion *cqe)
 {
-	void *ctx;
-
-	if (fn)
-		*fn = cmd->fn;
-	ctx = cmd->ctx;
-	cmd->fn = special_completion;
-	cmd->ctx = CMD_CTX_CANCELLED;
-	return ctx;
-}
-
-static void async_req_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	u32 result = le32_to_cpup(&cqe->result);
-	u16 status = le16_to_cpup(&cqe->status) >> 1;
+	u16 status = le16_to_cpu(cqe->status) >> 1;
+	u32 result = le32_to_cpu(cqe->result);
 
 	if (status == NVME_SC_SUCCESS || status == NVME_SC_ABORT_REQ)
-		++nvmeq->dev->event_limit;
+		++dev->ctrl.event_limit;
 	if (status != NVME_SC_SUCCESS)
 		return;
 
 	switch (result & 0xff07) {
 	case NVME_AER_NOTICE_NS_CHANGED:
-		dev_info(nvmeq->q_dmadev, "rescanning\n");
-		schedule_work(&nvmeq->dev->scan_work);
+		dev_info(dev->dev, "rescanning\n");
+		nvme_queue_scan(dev);
 	default:
-		dev_warn(nvmeq->q_dmadev, "async event result %08x\n", result);
-	}
-}
-
-static void abort_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	struct request *req = ctx;
-
-	u16 status = le16_to_cpup(&cqe->status) >> 1;
-	u32 result = le32_to_cpup(&cqe->result);
-
-	blk_mq_free_request(req);
-
-	dev_warn(nvmeq->q_dmadev, "Abort status:%x result:%x", status, result);
-	++nvmeq->dev->abort_limit;
-}
-
-static void async_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	struct async_cmd_info *cmdinfo = ctx;
-	cmdinfo->result = le32_to_cpup(&cqe->result);
-	cmdinfo->status = le16_to_cpup(&cqe->status) >> 1;
-	queue_kthread_work(cmdinfo->worker, &cmdinfo->work);
-	blk_mq_free_request(cmdinfo->req);
-}
-
-static inline struct nvme_cmd_info *get_cmd_from_tag(struct nvme_queue *nvmeq,
-				  unsigned int tag)
-{
-	struct request *req = blk_mq_tag_to_rq(*nvmeq->tags, tag);
-
-	return blk_mq_rq_to_pdu(req);
-}
-
-/*
- * Called with local interrupts disabled and the q_lock held.  May not sleep.
- */
-static void *nvme_finish_cmd(struct nvme_queue *nvmeq, int tag,
-						nvme_completion_fn *fn)
-{
-	struct nvme_cmd_info *cmd = get_cmd_from_tag(nvmeq, tag);
-	void *ctx;
-	if (tag >= nvmeq->q_depth) {
-		*fn = special_completion;
-		return CMD_CTX_INVALID;
+		dev_warn(dev->dev, "async event result %08x\n", result);
 	}
-	if (fn)
-		*fn = cmd->fn;
-	ctx = cmd->ctx;
-	cmd->fn = special_completion;
-	cmd->ctx = CMD_CTX_COMPLETED;
-	return ctx;
 }
 
 /**
- * nvme_submit_cmd() - Copy a command into a queue and ring the doorbell
+ * __nvme_submit_cmd() - Copy a command into a queue and ring the doorbell
  * @nvmeq: The queue to use
  * @cmd: The command to send
  *
@@ -405,69 +340,44 @@ static void __nvme_submit_cmd(struct nvme_queue *nvmeq,
 	nvmeq->sq_tail = tail;
 }
 
-static void nvme_submit_cmd(struct nvme_queue *nvmeq, struct nvme_command *cmd)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&nvmeq->q_lock, flags);
-	__nvme_submit_cmd(nvmeq, cmd);
-	spin_unlock_irqrestore(&nvmeq->q_lock, flags);
-}
-
-static __le64 **iod_list(struct nvme_iod *iod)
+static __le64 **iod_list(struct request *req)
 {
-	return ((void *)iod) + iod->offset;
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	return (__le64 **)(iod->sg + req->nr_phys_segments);
 }
 
-static inline void iod_init(struct nvme_iod *iod, unsigned nbytes,
-			    unsigned nseg, unsigned long private)
+static int nvme_init_iod(struct request *rq, struct nvme_dev *dev)
 {
-	iod->private = private;
-	iod->offset = offsetof(struct nvme_iod, sg[nseg]);
-	iod->npages = -1;
-	iod->length = nbytes;
-	iod->nents = 0;
-}
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(rq);
+	int nseg = rq->nr_phys_segments;
+	unsigned size;
 
-static struct nvme_iod *
-__nvme_alloc_iod(unsigned nseg, unsigned bytes, struct nvme_dev *dev,
-		 unsigned long priv, gfp_t gfp)
-{
-	struct nvme_iod *iod = kmalloc(sizeof(struct nvme_iod) +
-				sizeof(__le64 *) * nvme_npages(bytes, dev) +
-				sizeof(struct scatterlist) * nseg, gfp);
-
-	if (iod)
-		iod_init(iod, bytes, nseg, priv);
-
-	return iod;
-}
-
-static struct nvme_iod *nvme_alloc_iod(struct request *rq, struct nvme_dev *dev,
-			               gfp_t gfp)
-{
-	unsigned size = !(rq->cmd_flags & REQ_DISCARD) ? blk_rq_bytes(rq) :
-                                                sizeof(struct nvme_dsm_range);
-	struct nvme_iod *iod;
-
-	if (rq->nr_phys_segments <= NVME_INT_PAGES &&
-	    size <= NVME_INT_BYTES(dev)) {
-		struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(rq);
+	if (rq->cmd_flags & REQ_DISCARD)
+		size = sizeof(struct nvme_dsm_range);
+	else
+		size = blk_rq_bytes(rq);
 
-		iod = cmd->iod;
-		iod_init(iod, size, rq->nr_phys_segments,
-				(unsigned long) rq | NVME_INT_MASK);
-		return iod;
+	if (nseg > NVME_INT_PAGES || size > NVME_INT_BYTES(dev)) {
+		iod->sg = kmalloc(nvme_iod_alloc_size(dev, size, nseg), GFP_ATOMIC);
+		if (!iod->sg)
+			return BLK_MQ_RQ_QUEUE_BUSY;
+	} else {
+		iod->sg = iod->inline_sg;
 	}
 
-	return __nvme_alloc_iod(rq->nr_phys_segments, size, dev,
-				(unsigned long) rq, gfp);
+	iod->aborted = 0;
+	iod->npages = -1;
+	iod->nents = 0;
+	iod->length = size;
+	return 0;
 }
 
-static void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
+static void nvme_free_iod(struct nvme_dev *dev, struct request *req)
 {
-	const int last_prp = dev->page_size / 8 - 1;
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	const int last_prp = dev->ctrl.page_size / 8 - 1;
 	int i;
-	__le64 **list = iod_list(iod);
+	__le64 **list = iod_list(req);
 	dma_addr_t prp_dma = iod->first_dma;
 
 	if (iod->npages == 0)
@@ -479,20 +389,8 @@ static void nvme_free_iod(struct nvme_dev *dev, struct nvme_iod *iod)
 		prp_dma = next_prp_dma;
 	}
 
-	if (iod_should_kfree(iod))
-		kfree(iod);
-}
-
-static int nvme_error_status(u16 status)
-{
-	switch (status & 0x7ff) {
-	case NVME_SC_SUCCESS:
-		return 0;
-	case NVME_SC_CAP_EXCEEDED:
-		return -ENOSPC;
-	default:
-		return -EIO;
-	}
+	if (iod->sg != iod->inline_sg)
+		kfree(iod->sg);
 }
 
 #ifdef CONFIG_BLK_DEV_INTEGRITY
@@ -549,27 +447,6 @@ static void nvme_dif_remap(struct request *req,
 	}
 	kunmap_atomic(pmap);
 }
-
-static void nvme_init_integrity(struct nvme_ns *ns)
-{
-	struct blk_integrity integrity;
-
-	switch (ns->pi_type) {
-	case NVME_NS_DPS_PI_TYPE3:
-		integrity.profile = &t10_pi_type3_crc;
-		break;
-	case NVME_NS_DPS_PI_TYPE1:
-	case NVME_NS_DPS_PI_TYPE2:
-		integrity.profile = &t10_pi_type1_crc;
-		break;
-	default:
-		integrity.profile = NULL;
-		break;
-	}
-	integrity.tuple_size = ns->ms;
-	blk_integrity_register(ns->disk, &integrity);
-	blk_queue_max_integrity_segments(ns->queue, 1);
-}
 #else /* CONFIG_BLK_DEV_INTEGRITY */
 static void nvme_dif_remap(struct request *req,
 			void (*dif_swap)(u32 p, u32 v, struct t10_pi_tuple *pi))
@@ -581,91 +458,27 @@ static void nvme_dif_prep(u32 p, u32 v, struct t10_pi_tuple *pi)
 static void nvme_dif_complete(u32 p, u32 v, struct t10_pi_tuple *pi)
 {
 }
-static void nvme_init_integrity(struct nvme_ns *ns)
-{
-}
 #endif
 
-static void req_completion(struct nvme_queue *nvmeq, void *ctx,
-						struct nvme_completion *cqe)
-{
-	struct nvme_iod *iod = ctx;
-	struct request *req = iod_get_private(iod);
-	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
-	u16 status = le16_to_cpup(&cqe->status) >> 1;
-	bool requeue = false;
-	int error = 0;
-
-	if (unlikely(status)) {
-		if (!(status & NVME_SC_DNR || blk_noretry_request(req))
-		    && (jiffies - req->start_time) < req->timeout) {
-			unsigned long flags;
-
-			requeue = true;
-			blk_mq_requeue_request(req);
-			spin_lock_irqsave(req->q->queue_lock, flags);
-			if (!blk_queue_stopped(req->q))
-				blk_mq_kick_requeue_list(req->q);
-			spin_unlock_irqrestore(req->q->queue_lock, flags);
-			goto release_iod;
-		}
-
-		if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
-			if (cmd_rq->ctx == CMD_CTX_CANCELLED)
-				error = -EINTR;
-			else
-				error = status;
-		} else {
-			error = nvme_error_status(status);
-		}
-	}
-
-	if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
-		u32 result = le32_to_cpup(&cqe->result);
-		req->special = (void *)(uintptr_t)result;
-	}
-
-	if (cmd_rq->aborted)
-		dev_warn(nvmeq->dev->dev,
-			"completing aborted command with status:%04x\n",
-			error);
-
-release_iod:
-	if (iod->nents) {
-		dma_unmap_sg(nvmeq->dev->dev, iod->sg, iod->nents,
-			rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		if (blk_integrity_rq(req)) {
-			if (!rq_data_dir(req))
-				nvme_dif_remap(req, nvme_dif_complete);
-			dma_unmap_sg(nvmeq->dev->dev, iod->meta_sg, 1,
-				rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE);
-		}
-	}
-	nvme_free_iod(nvmeq->dev, iod);
-
-	if (likely(!requeue))
-		blk_mq_complete_request(req, error);
-}
-
-/* length is in bytes.  gfp flags indicates whether we may sleep. */
-static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
-		int total_len, gfp_t gfp)
+static bool nvme_setup_prps(struct nvme_dev *dev, struct request *req,
+		int total_len)
 {
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
 	struct dma_pool *pool;
 	int length = total_len;
 	struct scatterlist *sg = iod->sg;
 	int dma_len = sg_dma_len(sg);
 	u64 dma_addr = sg_dma_address(sg);
-	u32 page_size = dev->page_size;
+	u32 page_size = dev->ctrl.page_size;
 	int offset = dma_addr & (page_size - 1);
 	__le64 *prp_list;
-	__le64 **list = iod_list(iod);
+	__le64 **list = iod_list(req);
 	dma_addr_t prp_dma;
 	int nprps, i;
 
 	length -= (page_size - offset);
 	if (length <= 0)
-		return total_len;
+		return true;
 
 	dma_len -= (page_size - offset);
 	if (dma_len) {
@@ -678,7 +491,7 @@ static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
 
 	if (length <= page_size) {
 		iod->first_dma = dma_addr;
-		return total_len;
+		return true;
 	}
 
 	nprps = DIV_ROUND_UP(length, page_size);
@@ -690,11 +503,11 @@ static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
 		iod->npages = 1;
 	}
 
-	prp_list = dma_pool_alloc(pool, gfp, &prp_dma);
+	prp_list = dma_pool_alloc(pool, GFP_ATOMIC, &prp_dma);
 	if (!prp_list) {
 		iod->first_dma = dma_addr;
 		iod->npages = -1;
-		return (total_len - length) + page_size;
+		return false;
 	}
 	list[0] = prp_list;
 	iod->first_dma = prp_dma;
@@ -702,9 +515,9 @@ static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
 	for (;;) {
 		if (i == page_size >> 3) {
 			__le64 *old_prp_list = prp_list;
-			prp_list = dma_pool_alloc(pool, gfp, &prp_dma);
+			prp_list = dma_pool_alloc(pool, GFP_ATOMIC, &prp_dma);
 			if (!prp_list)
-				return total_len - length;
+				return false;
 			list[iod->npages++] = prp_list;
 			prp_list[0] = old_prp_list[i - 1];
 			old_prp_list[i - 1] = cpu_to_le64(prp_dma);
@@ -724,115 +537,105 @@ static int nvme_setup_prps(struct nvme_dev *dev, struct nvme_iod *iod,
 		dma_len = sg_dma_len(sg);
 	}
 
-	return total_len;
+	return true;
 }
 
-static void nvme_submit_priv(struct nvme_queue *nvmeq, struct request *req,
-		struct nvme_iod *iod)
+static int nvme_map_data(struct nvme_dev *dev, struct request *req,
+		struct nvme_command *cmnd)
 {
-	struct nvme_command cmnd;
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	struct request_queue *q = req->q;
+	enum dma_data_direction dma_dir = rq_data_dir(req) ?
+			DMA_TO_DEVICE : DMA_FROM_DEVICE;
+	int ret = BLK_MQ_RQ_QUEUE_ERROR;
 
-	memcpy(&cmnd, req->cmd, sizeof(cmnd));
-	cmnd.rw.command_id = req->tag;
-	if (req->nr_phys_segments) {
-		cmnd.rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-		cmnd.rw.prp2 = cpu_to_le64(iod->first_dma);
-	}
+	sg_init_table(iod->sg, req->nr_phys_segments);
+	iod->nents = blk_rq_map_sg(q, req, iod->sg);
+	if (!iod->nents)
+		goto out;
 
-	__nvme_submit_cmd(nvmeq, &cmnd);
-}
+	ret = BLK_MQ_RQ_QUEUE_BUSY;
+	if (!dma_map_sg(dev->dev, iod->sg, iod->nents, dma_dir))
+		goto out;
 
-/*
- * We reuse the small pool to allocate the 16-byte range here as it is not
- * worth having a special pool for these or additional cases to handle freeing
- * the iod.
- */
-static void nvme_submit_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
-		struct request *req, struct nvme_iod *iod)
-{
-	struct nvme_dsm_range *range =
-				(struct nvme_dsm_range *)iod_list(iod)[0];
-	struct nvme_command cmnd;
+	if (!nvme_setup_prps(dev, req, blk_rq_bytes(req)))
+		goto out_unmap;
 
-	range->cattr = cpu_to_le32(0);
-	range->nlb = cpu_to_le32(blk_rq_bytes(req) >> ns->lba_shift);
-	range->slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
+	ret = BLK_MQ_RQ_QUEUE_ERROR;
+	if (blk_integrity_rq(req)) {
+		if (blk_rq_count_integrity_sg(q, req->bio) != 1)
+			goto out_unmap;
 
-	memset(&cmnd, 0, sizeof(cmnd));
-	cmnd.dsm.opcode = nvme_cmd_dsm;
-	cmnd.dsm.command_id = req->tag;
-	cmnd.dsm.nsid = cpu_to_le32(ns->ns_id);
-	cmnd.dsm.prp1 = cpu_to_le64(iod->first_dma);
-	cmnd.dsm.nr = 0;
-	cmnd.dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
+		sg_init_table(&iod->meta_sg, 1);
+		if (blk_rq_map_integrity_sg(q, req->bio, &iod->meta_sg) != 1)
+			goto out_unmap;
 
-	__nvme_submit_cmd(nvmeq, &cmnd);
-}
+		if (rq_data_dir(req))
+			nvme_dif_remap(req, nvme_dif_prep);
 
-static void nvme_submit_flush(struct nvme_queue *nvmeq, struct nvme_ns *ns,
-								int cmdid)
-{
-	struct nvme_command cmnd;
+		if (!dma_map_sg(dev->dev, &iod->meta_sg, 1, dma_dir))
+			goto out_unmap;
+	}
 
-	memset(&cmnd, 0, sizeof(cmnd));
-	cmnd.common.opcode = nvme_cmd_flush;
-	cmnd.common.command_id = cmdid;
-	cmnd.common.nsid = cpu_to_le32(ns->ns_id);
+	cmnd->rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
+	cmnd->rw.prp2 = cpu_to_le64(iod->first_dma);
+	if (blk_integrity_rq(req))
+		cmnd->rw.metadata = cpu_to_le64(sg_dma_address(&iod->meta_sg));
+	return BLK_MQ_RQ_QUEUE_OK;
 
-	__nvme_submit_cmd(nvmeq, &cmnd);
+out_unmap:
+	dma_unmap_sg(dev->dev, iod->sg, iod->nents, dma_dir);
+out:
+	return ret;
 }
 
-static int nvme_submit_iod(struct nvme_queue *nvmeq, struct nvme_iod *iod,
-							struct nvme_ns *ns)
+static void nvme_unmap_data(struct nvme_dev *dev, struct request *req)
 {
-	struct request *req = iod_get_private(iod);
-	struct nvme_command cmnd;
-	u16 control = 0;
-	u32 dsmgmt = 0;
-
-	if (req->cmd_flags & REQ_FUA)
-		control |= NVME_RW_FUA;
-	if (req->cmd_flags & (REQ_FAILFAST_DEV | REQ_RAHEAD))
-		control |= NVME_RW_LR;
-
-	if (req->cmd_flags & REQ_RAHEAD)
-		dsmgmt |= NVME_RW_DSM_FREQ_PREFETCH;
-
-	memset(&cmnd, 0, sizeof(cmnd));
-	cmnd.rw.opcode = (rq_data_dir(req) ? nvme_cmd_write : nvme_cmd_read);
-	cmnd.rw.command_id = req->tag;
-	cmnd.rw.nsid = cpu_to_le32(ns->ns_id);
-	cmnd.rw.prp1 = cpu_to_le64(sg_dma_address(iod->sg));
-	cmnd.rw.prp2 = cpu_to_le64(iod->first_dma);
-	cmnd.rw.slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
-	cmnd.rw.length = cpu_to_le16((blk_rq_bytes(req) >> ns->lba_shift) - 1);
-
-	if (ns->ms) {
-		switch (ns->pi_type) {
-		case NVME_NS_DPS_PI_TYPE3:
-			control |= NVME_RW_PRINFO_PRCHK_GUARD;
-			break;
-		case NVME_NS_DPS_PI_TYPE1:
-		case NVME_NS_DPS_PI_TYPE2:
-			control |= NVME_RW_PRINFO_PRCHK_GUARD |
-					NVME_RW_PRINFO_PRCHK_REF;
-			cmnd.rw.reftag = cpu_to_le32(
-					nvme_block_nr(ns, blk_rq_pos(req)));
-			break;
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	enum dma_data_direction dma_dir = rq_data_dir(req) ?
+			DMA_TO_DEVICE : DMA_FROM_DEVICE;
+
+	if (iod->nents) {
+		dma_unmap_sg(dev->dev, iod->sg, iod->nents, dma_dir);
+		if (blk_integrity_rq(req)) {
+			if (!rq_data_dir(req))
+				nvme_dif_remap(req, nvme_dif_complete);
+			dma_unmap_sg(dev->dev, &iod->meta_sg, 1, dma_dir);
 		}
-		if (blk_integrity_rq(req))
-			cmnd.rw.metadata =
-				cpu_to_le64(sg_dma_address(iod->meta_sg));
-		else
-			control |= NVME_RW_PRINFO_PRACT;
 	}
 
-	cmnd.rw.control = cpu_to_le16(control);
-	cmnd.rw.dsmgmt = cpu_to_le32(dsmgmt);
+	nvme_free_iod(dev, req);
+}
 
-	__nvme_submit_cmd(nvmeq, &cmnd);
+/*
+ * We reuse the small pool to allocate the 16-byte range here as it is not
+ * worth having a special pool for these or additional cases to handle freeing
+ * the iod.
+ */
+static int nvme_setup_discard(struct nvme_queue *nvmeq, struct nvme_ns *ns,
+		struct request *req, struct nvme_command *cmnd)
+{
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	struct nvme_dsm_range *range;
 
-	return 0;
+	range = dma_pool_alloc(nvmeq->dev->prp_small_pool, GFP_ATOMIC,
+						&iod->first_dma);
+	if (!range)
+		return BLK_MQ_RQ_QUEUE_BUSY;
+	iod_list(req)[0] = (__le64 *)range;
+	iod->npages = 0;
+
+	range->cattr = cpu_to_le32(0);
+	range->nlb = cpu_to_le32(blk_rq_bytes(req) >> ns->lba_shift);
+	range->slba = cpu_to_le64(nvme_block_nr(ns, blk_rq_pos(req)));
+
+	memset(cmnd, 0, sizeof(*cmnd));
+	cmnd->dsm.opcode = nvme_cmd_dsm;
+	cmnd->dsm.nsid = cpu_to_le32(ns->ns_id);
+	cmnd->dsm.prp1 = cpu_to_le64(iod->first_dma);
+	cmnd->dsm.nr = 0;
+	cmnd->dsm.attributes = cpu_to_le32(NVME_DSMGMT_AD);
+	return BLK_MQ_RQ_QUEUE_OK;
 }
 
 /*
@@ -845,9 +648,8 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 	struct nvme_queue *nvmeq = hctx->driver_data;
 	struct nvme_dev *dev = nvmeq->dev;
 	struct request *req = bd->rq;
-	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
-	struct nvme_iod *iod;
-	enum dma_data_direction dma_dir;
+	struct nvme_command cmnd;
+	int ret = BLK_MQ_RQ_QUEUE_OK;
 
 	/*
 	 * If formated with metadata, require the block layer provide a buffer
@@ -857,91 +659,80 @@ static int nvme_queue_rq(struct blk_mq_hw_ctx *hctx,
 	if (ns && ns->ms && !blk_integrity_rq(req)) {
 		if (!(ns->pi_type && ns->ms == 8) &&
 					req->cmd_type != REQ_TYPE_DRV_PRIV) {
-			blk_mq_complete_request(req, -EFAULT);
+			blk_mq_end_request(req, -EFAULT);
 			return BLK_MQ_RQ_QUEUE_OK;
 		}
 	}
 
-	iod = nvme_alloc_iod(req, dev, GFP_ATOMIC);
-	if (!iod)
-		return BLK_MQ_RQ_QUEUE_BUSY;
+	ret = nvme_init_iod(req, dev);
+	if (ret)
+		return ret;
 
 	if (req->cmd_flags & REQ_DISCARD) {
-		void *range;
-		/*
-		 * We reuse the small pool to allocate the 16-byte range here
-		 * as it is not worth having a special pool for these or
-		 * additional cases to handle freeing the iod.
-		 */
-		range = dma_pool_alloc(dev->prp_small_pool, GFP_ATOMIC,
-						&iod->first_dma);
-		if (!range)
-			goto retry_cmd;
-		iod_list(iod)[0] = (__le64 *)range;
-		iod->npages = 0;
-	} else if (req->nr_phys_segments) {
-		dma_dir = rq_data_dir(req) ? DMA_TO_DEVICE : DMA_FROM_DEVICE;
+		ret = nvme_setup_discard(nvmeq, ns, req, &cmnd);
+	} else {
+		if (req->cmd_type == REQ_TYPE_DRV_PRIV)
+			memcpy(&cmnd, req->cmd, sizeof(cmnd));
+		else if (req->cmd_flags & REQ_FLUSH)
+			nvme_setup_flush(ns, &cmnd);
+		else
+			nvme_setup_rw(ns, req, &cmnd);
 
-		sg_init_table(iod->sg, req->nr_phys_segments);
-		iod->nents = blk_rq_map_sg(req->q, req, iod->sg);
-		if (!iod->nents)
-			goto error_cmd;
+		if (req->nr_phys_segments)
+			ret = nvme_map_data(dev, req, &cmnd);
+	}
 
-		if (!dma_map_sg(nvmeq->q_dmadev, iod->sg, iod->nents, dma_dir))
-			goto retry_cmd;
+	if (ret)
+		goto out;
 
-		if (blk_rq_bytes(req) !=
-                    nvme_setup_prps(dev, iod, blk_rq_bytes(req), GFP_ATOMIC)) {
-			dma_unmap_sg(dev->dev, iod->sg, iod->nents, dma_dir);
-			goto retry_cmd;
-		}
-		if (blk_integrity_rq(req)) {
-			if (blk_rq_count_integrity_sg(req->q, req->bio) != 1) {
-				dma_unmap_sg(dev->dev, iod->sg, iod->nents,
-						dma_dir);
-				goto error_cmd;
-			}
+	cmnd.common.command_id = req->tag;
+	blk_mq_start_request(req);
 
-			sg_init_table(iod->meta_sg, 1);
-			if (blk_rq_map_integrity_sg(
-					req->q, req->bio, iod->meta_sg) != 1) {
-				dma_unmap_sg(dev->dev, iod->sg, iod->nents,
-						dma_dir);
-				goto error_cmd;
-			}
+	spin_lock_irq(&nvmeq->q_lock);
+	if (unlikely(nvmeq->cq_vector < 0)) {
+		if (ns && !test_bit(NVME_NS_DEAD, &ns->flags))
+			ret = BLK_MQ_RQ_QUEUE_BUSY;
+		else
+			ret = BLK_MQ_RQ_QUEUE_ERROR;
+		spin_unlock_irq(&nvmeq->q_lock);
+		goto out;
+	}
+	__nvme_submit_cmd(nvmeq, &cmnd);
+	nvme_process_cq(nvmeq);
+	spin_unlock_irq(&nvmeq->q_lock);
+	return BLK_MQ_RQ_QUEUE_OK;
+out:
+	nvme_free_iod(dev, req);
+	return ret;
+}
+
+static void nvme_complete_rq(struct request *req)
+{
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	struct nvme_dev *dev = iod->nvmeq->dev;
+	int error = 0;
 
-			if (rq_data_dir(req))
-				nvme_dif_remap(req, nvme_dif_prep);
+	nvme_unmap_data(dev, req);
 
-			if (!dma_map_sg(nvmeq->q_dmadev, iod->meta_sg, 1, dma_dir)) {
-				dma_unmap_sg(dev->dev, iod->sg, iod->nents,
-						dma_dir);
-				goto error_cmd;
-			}
+	if (unlikely(req->errors)) {
+		if (nvme_req_needs_retry(req, req->errors)) {
+			nvme_requeue_req(req);
+			return;
 		}
-	}
 
-	nvme_set_info(cmd, iod, req_completion);
-	spin_lock_irq(&nvmeq->q_lock);
-	if (req->cmd_type == REQ_TYPE_DRV_PRIV)
-		nvme_submit_priv(nvmeq, req, iod);
-	else if (req->cmd_flags & REQ_DISCARD)
-		nvme_submit_discard(nvmeq, ns, req, iod);
-	else if (req->cmd_flags & REQ_FLUSH)
-		nvme_submit_flush(nvmeq, ns, req->tag);
-	else
-		nvme_submit_iod(nvmeq, iod, ns);
+		if (req->cmd_type == REQ_TYPE_DRV_PRIV)
+			error = req->errors;
+		else
+			error = nvme_error_status(req->errors);
+	}
 
-	nvme_process_cq(nvmeq);
-	spin_unlock_irq(&nvmeq->q_lock);
-	return BLK_MQ_RQ_QUEUE_OK;
+	if (unlikely(iod->aborted)) {
+		dev_warn(dev->dev,
+			"completing aborted command with status: %04x\n",
+			req->errors);
+	}
 
- error_cmd:
-	nvme_free_iod(dev, iod);
-	return BLK_MQ_RQ_QUEUE_ERROR;
- retry_cmd:
-	nvme_free_iod(dev, iod);
-	return BLK_MQ_RQ_QUEUE_BUSY;
+	blk_mq_end_request(req, error);
 }
 
 static void __nvme_process_cq(struct nvme_queue *nvmeq, unsigned int *tag)
@@ -952,20 +743,47 @@ static void __nvme_process_cq(struct nvme_queue *nvmeq, unsigned int *tag)
 	phase = nvmeq->cq_phase;
 
 	for (;;) {
-		void *ctx;
-		nvme_completion_fn fn;
 		struct nvme_completion cqe = nvmeq->cqes[head];
-		if ((le16_to_cpu(cqe.status) & 1) != phase)
+		u16 status = le16_to_cpu(cqe.status);
+		struct request *req;
+
+		if ((status & 1) != phase)
 			break;
 		nvmeq->sq_head = le16_to_cpu(cqe.sq_head);
 		if (++head == nvmeq->q_depth) {
 			head = 0;
 			phase = !phase;
 		}
+
 		if (tag && *tag == cqe.command_id)
 			*tag = -1;
-		ctx = nvme_finish_cmd(nvmeq, cqe.command_id, &fn);
-		fn(nvmeq, ctx, &cqe);
+
+		if (unlikely(cqe.command_id >= nvmeq->q_depth)) {
+			dev_warn(nvmeq->q_dmadev,
+				"invalid id %d completed on queue %d\n",
+				cqe.command_id, le16_to_cpu(cqe.sq_id));
+			continue;
+		}
+
+		/*
+		 * AEN requests are special as they don't time out and can
+		 * survive any kind of queue freeze and often don't respond to
+		 * aborts.  We don't even bother to allocate a struct request
+		 * for them but rather special case them here.
+		 */
+		if (unlikely(nvmeq->qid == 0 &&
+				cqe.command_id >= NVME_AQ_BLKMQ_DEPTH)) {
+			nvme_complete_async_event(nvmeq->dev, &cqe);
+			continue;
+		}
+
+		req = blk_mq_tag_to_rq(*nvmeq->tags, cqe.command_id);
+		if (req->cmd_type == REQ_TYPE_DRV_PRIV) {
+			u32 result = le32_to_cpu(cqe.result);
+			req->special = (void *)(uintptr_t)result;
+		}
+		blk_mq_complete_request(req, status >> 1);
+
 	}
 
 	/* If the controller ignores the cq head doorbell and continuously
@@ -1028,111 +846,15 @@ static int nvme_poll(struct blk_mq_hw_ctx *hctx, unsigned int tag)
 	return 0;
 }
 
-/*
- * Returns 0 on success.  If the result is negative, it's a Linux error code;
- * if the result is positive, it's an NVM Express status code
- */
-int __nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
-		void *buffer, void __user *ubuffer, unsigned bufflen,
-		u32 *result, unsigned timeout)
+static void nvme_submit_async_event(struct nvme_dev *dev)
 {
-	bool write = cmd->common.opcode & 1;
-	struct bio *bio = NULL;
-	struct request *req;
-	int ret;
-
-	req = blk_mq_alloc_request(q, write, GFP_KERNEL, false);
-	if (IS_ERR(req))
-		return PTR_ERR(req);
-
-	req->cmd_type = REQ_TYPE_DRV_PRIV;
-	req->cmd_flags |= REQ_FAILFAST_DRIVER;
-	req->__data_len = 0;
-	req->__sector = (sector_t) -1;
-	req->bio = req->biotail = NULL;
-
-	req->timeout = timeout ? timeout : ADMIN_TIMEOUT;
-
-	req->cmd = (unsigned char *)cmd;
-	req->cmd_len = sizeof(struct nvme_command);
-	req->special = (void *)0;
-
-	if (buffer && bufflen) {
-		ret = blk_rq_map_kern(q, req, buffer, bufflen,
-				      __GFP_DIRECT_RECLAIM);
-		if (ret)
-			goto out;
-	} else if (ubuffer && bufflen) {
-		ret = blk_rq_map_user(q, req, NULL, ubuffer, bufflen,
-				      __GFP_DIRECT_RECLAIM);
-		if (ret)
-			goto out;
-		bio = req->bio;
-	}
-
-	blk_execute_rq(req->q, NULL, req, 0);
-	if (bio)
-		blk_rq_unmap_user(bio);
-	if (result)
-		*result = (u32)(uintptr_t)req->special;
-	ret = req->errors;
- out:
-	blk_mq_free_request(req);
-	return ret;
-}
-
-int nvme_submit_sync_cmd(struct request_queue *q, struct nvme_command *cmd,
-		void *buffer, unsigned bufflen)
-{
-	return __nvme_submit_sync_cmd(q, cmd, buffer, NULL, bufflen, NULL, 0);
-}
-
-static int nvme_submit_async_admin_req(struct nvme_dev *dev)
-{
-	struct nvme_queue *nvmeq = dev->queues[0];
 	struct nvme_command c;
-	struct nvme_cmd_info *cmd_info;
-	struct request *req;
-
-	req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_ATOMIC, true);
-	if (IS_ERR(req))
-		return PTR_ERR(req);
-
-	req->cmd_flags |= REQ_NO_TIMEOUT;
-	cmd_info = blk_mq_rq_to_pdu(req);
-	nvme_set_info(cmd_info, NULL, async_req_completion);
 
 	memset(&c, 0, sizeof(c));
 	c.common.opcode = nvme_admin_async_event;
-	c.common.command_id = req->tag;
-
-	blk_mq_free_request(req);
-	__nvme_submit_cmd(nvmeq, &c);
-	return 0;
-}
-
-static int nvme_submit_admin_async_cmd(struct nvme_dev *dev,
-			struct nvme_command *cmd,
-			struct async_cmd_info *cmdinfo, unsigned timeout)
-{
-	struct nvme_queue *nvmeq = dev->queues[0];
-	struct request *req;
-	struct nvme_cmd_info *cmd_rq;
-
-	req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_KERNEL, false);
-	if (IS_ERR(req))
-		return PTR_ERR(req);
+	c.common.command_id = NVME_AQ_BLKMQ_DEPTH + --dev->ctrl.event_limit;
 
-	req->timeout = timeout;
-	cmd_rq = blk_mq_rq_to_pdu(req);
-	cmdinfo->req = req;
-	nvme_set_info(cmd_rq, cmdinfo, async_completion);
-	cmdinfo->status = -EINTR;
-
-	cmd->common.command_id = req->tag;
-
-	nvme_submit_cmd(nvmeq, cmd);
-	return 0;
+	__nvme_submit_cmd(dev->queues[0], &c);
 }
 
 static int adapter_delete_queue(struct nvme_dev *dev, u8 opcode, u16 id)
@@ -1143,7 +865,7 @@ static int adapter_delete_queue(struct nvme_dev *dev, u8 opcode, u16 id)
 	c.delete_queue.opcode = opcode;
 	c.delete_queue.qid = cpu_to_le16(id);
 
-	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
+	return nvme_submit_sync_cmd(dev->ctrl.admin_q, &c, NULL, 0);
 }
 
 static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
@@ -1164,7 +886,7 @@ static int adapter_alloc_cq(struct nvme_dev *dev, u16 qid,
 	c.create_cq.cq_flags = cpu_to_le16(flags);
 	c.create_cq.irq_vector = cpu_to_le16(nvmeq->cq_vector);
 
-	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
+	return nvme_submit_sync_cmd(dev->ctrl.admin_q, &c, NULL, 0);
 }
 
 static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
@@ -1185,7 +907,7 @@ static int adapter_alloc_sq(struct nvme_dev *dev, u16 qid,
 	c.create_sq.sq_flags = cpu_to_le16(flags);
 	c.create_sq.cqid = cpu_to_le16(qid);
 
-	return nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
+	return nvme_submit_sync_cmd(dev->ctrl.admin_q, &c, NULL, 0);
 }
 
 static int adapter_delete_cq(struct nvme_dev *dev, u16 cqid)
@@ -1198,195 +920,111 @@ static int adapter_delete_sq(struct nvme_dev *dev, u16 sqid)
 	return adapter_delete_queue(dev, nvme_admin_delete_sq, sqid);
 }
 
-int nvme_identify_ctrl(struct nvme_dev *dev, struct nvme_id_ctrl **id)
-{
-	struct nvme_command c = { };
-	int error;
-
-	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
-	c.identify.opcode = nvme_admin_identify;
-	c.identify.cns = cpu_to_le32(1);
-
-	*id = kmalloc(sizeof(struct nvme_id_ctrl), GFP_KERNEL);
-	if (!*id)
-		return -ENOMEM;
-
-	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
-			sizeof(struct nvme_id_ctrl));
-	if (error)
-		kfree(*id);
-	return error;
-}
-
-int nvme_identify_ns(struct nvme_dev *dev, unsigned nsid,
-		struct nvme_id_ns **id)
-{
-	struct nvme_command c = { };
-	int error;
-
-	/* gcc-4.4.4 (at least) has issues with initializers and anon unions */
-	c.identify.opcode = nvme_admin_identify,
-	c.identify.nsid = cpu_to_le32(nsid),
-
-	*id = kmalloc(sizeof(struct nvme_id_ns), GFP_KERNEL);
-	if (!*id)
-		return -ENOMEM;
-
-	error = nvme_submit_sync_cmd(dev->admin_q, &c, *id,
-			sizeof(struct nvme_id_ns));
-	if (error)
-		kfree(*id);
-	return error;
-}
-
-int nvme_get_features(struct nvme_dev *dev, unsigned fid, unsigned nsid,
-					dma_addr_t dma_addr, u32 *result)
+static void abort_endio(struct request *req, int error)
 {
-	struct nvme_command c;
-
-	memset(&c, 0, sizeof(c));
-	c.features.opcode = nvme_admin_get_features;
-	c.features.nsid = cpu_to_le32(nsid);
-	c.features.prp1 = cpu_to_le64(dma_addr);
-	c.features.fid = cpu_to_le32(fid);
-
-	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, NULL, 0,
-			result, 0);
-}
-
-int nvme_set_features(struct nvme_dev *dev, unsigned fid, unsigned dword11,
-					dma_addr_t dma_addr, u32 *result)
-{
-	struct nvme_command c;
-
-	memset(&c, 0, sizeof(c));
-	c.features.opcode = nvme_admin_set_features;
-	c.features.prp1 = cpu_to_le64(dma_addr);
-	c.features.fid = cpu_to_le32(fid);
-	c.features.dword11 = cpu_to_le32(dword11);
-
-	return __nvme_submit_sync_cmd(dev->admin_q, &c, NULL, NULL, 0,
-			result, 0);
-}
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = iod->nvmeq;
+	u32 result = (u32)(uintptr_t)req->special;
+	u16 status = req->errors;
 
-int nvme_get_log_page(struct nvme_dev *dev, struct nvme_smart_log **log)
-{
-	struct nvme_command c = { };
-	int error;
-
-	c.common.opcode = nvme_admin_get_log_page,
-	c.common.nsid = cpu_to_le32(0xFFFFFFFF),
-	c.common.cdw10[0] = cpu_to_le32(
-			(((sizeof(struct nvme_smart_log) / 4) - 1) << 16) |
-			 NVME_LOG_SMART),
-
-	*log = kmalloc(sizeof(struct nvme_smart_log), GFP_KERNEL);
-	if (!*log)
-		return -ENOMEM;
+	dev_warn(nvmeq->q_dmadev, "Abort status:%x result:%x", status, result);
+	atomic_inc(&nvmeq->dev->ctrl.abort_limit);
 
-	error = nvme_submit_sync_cmd(dev->admin_q, &c, *log,
-			sizeof(struct nvme_smart_log));
-	if (error)
-		kfree(*log);
-	return error;
+	blk_mq_free_request(req);
 }
 
-/**
- * nvme_abort_req - Attempt aborting a request
- *
- * Schedule controller reset if the command was already aborted once before and
- * still hasn't been returned to the driver, or if this is the admin queue.
- */
-static void nvme_abort_req(struct request *req)
+static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
 {
-	struct nvme_cmd_info *cmd_rq = blk_mq_rq_to_pdu(req);
-	struct nvme_queue *nvmeq = cmd_rq->nvmeq;
+	struct nvme_iod *iod = blk_mq_rq_to_pdu(req);
+	struct nvme_queue *nvmeq = iod->nvmeq;
 	struct nvme_dev *dev = nvmeq->dev;
 	struct request *abort_req;
-	struct nvme_cmd_info *abort_cmd;
 	struct nvme_command cmd;
 
-	if (!nvmeq->qid || cmd_rq->aborted) {
-		spin_lock(&dev_list_lock);
-		if (!__nvme_reset(dev)) {
-			dev_warn(dev->dev,
-				 "I/O %d QID %d timeout, reset controller\n",
-				 req->tag, nvmeq->qid);
-		}
-		spin_unlock(&dev_list_lock);
-		return;
+	/*
+	 * Shutdown immediately if controller times out while starting. The
+	 * reset work will see the pci device disabled when it gets the forced
+	 * cancellation error. All outstanding requests are completed on
+	 * shutdown, so we return BLK_EH_HANDLED.
+	 */
+	if (test_bit(NVME_CTRL_RESETTING, &dev->flags)) {
+		dev_warn(dev->dev,
+			 "I/O %d QID %d timeout, disable controller\n",
+			 req->tag, nvmeq->qid);
+		nvme_dev_disable(dev, false);
+		req->errors = NVME_SC_CANCELLED;
+		return BLK_EH_HANDLED;
 	}
 
-	if (!dev->abort_limit)
-		return;
+	/*
+ 	 * Shutdown the controller immediately and schedule a reset if the
+ 	 * command was already aborted once before and still hasn't been
+ 	 * returned to the driver, or if this is the admin queue.
+	 */
+	if (!nvmeq->qid || iod->aborted) {
+		dev_warn(dev->dev,
+			 "I/O %d QID %d timeout, reset controller\n",
+			 req->tag, nvmeq->qid);
+		nvme_dev_disable(dev, false);
+		queue_work(nvme_workq, &dev->reset_work);
 
-	abort_req = blk_mq_alloc_request(dev->admin_q, WRITE, GFP_ATOMIC,
-									false);
-	if (IS_ERR(abort_req))
-		return;
+		/*
+		 * Mark the request as handled, since the inline shutdown
+		 * forces all outstanding requests to complete.
+		 */
+		req->errors = NVME_SC_CANCELLED;
+		return BLK_EH_HANDLED;
+	}
 
-	abort_cmd = blk_mq_rq_to_pdu(abort_req);
-	nvme_set_info(abort_cmd, abort_req, abort_completion);
+	iod->aborted = 1;
+
+	if (atomic_dec_return(&dev->ctrl.abort_limit) < 0) {
+		atomic_inc(&dev->ctrl.abort_limit);
+		return BLK_EH_RESET_TIMER;
+	}
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.abort.opcode = nvme_admin_abort_cmd;
 	cmd.abort.cid = req->tag;
 	cmd.abort.sqid = cpu_to_le16(nvmeq->qid);
-	cmd.abort.command_id = abort_req->tag;
 
-	--dev->abort_limit;
-	cmd_rq->aborted = 1;
+	dev_warn(nvmeq->q_dmadev, "I/O %d QID %d timeout, aborting\n",
+				 req->tag, nvmeq->qid);
+
+	abort_req = nvme_alloc_request(dev->ctrl.admin_q, &cmd,
+			BLK_MQ_REQ_NOWAIT);
+	if (IS_ERR(abort_req)) {
+		atomic_inc(&dev->ctrl.abort_limit);
+		return BLK_EH_RESET_TIMER;
+	}
+
+	abort_req->timeout = ADMIN_TIMEOUT;
+	abort_req->end_io_data = NULL;
+	blk_execute_rq_nowait(abort_req->q, NULL, abort_req, 0, abort_endio);
 
-	dev_warn(nvmeq->q_dmadev, "Aborting I/O %d QID %d\n", req->tag,
-							nvmeq->qid);
-	nvme_submit_cmd(dev->queues[0], &cmd);
+	/*
+	 * The aborted req will be completed on receiving the abort req.
+	 * We enable the timer again. If hit twice, it'll cause a device reset,
+	 * as the device then is in a faulty state.
+	 */
+	return BLK_EH_RESET_TIMER;
 }
 
 static void nvme_cancel_queue_ios(struct request *req, void *data, bool reserved)
 {
 	struct nvme_queue *nvmeq = data;
-	void *ctx;
-	nvme_completion_fn fn;
-	struct nvme_cmd_info *cmd;
-	struct nvme_completion cqe;
+	int status;
 
 	if (!blk_mq_request_started(req))
 		return;
 
-	cmd = blk_mq_rq_to_pdu(req);
-
-	if (cmd->ctx == CMD_CTX_CANCELLED)
-		return;
+	dev_dbg_ratelimited(nvmeq->q_dmadev,
+		 "Cancelling I/O %d QID %d\n", req->tag, nvmeq->qid);
 
+	status = NVME_SC_ABORT_REQ;
 	if (blk_queue_dying(req->q))
-		cqe.status = cpu_to_le16((NVME_SC_ABORT_REQ | NVME_SC_DNR) << 1);
-	else
-		cqe.status = cpu_to_le16(NVME_SC_ABORT_REQ << 1);
-
-
-	dev_warn(nvmeq->q_dmadev, "Cancelling I/O %d QID %d\n",
-						req->tag, nvmeq->qid);
-	ctx = cancel_cmd_info(cmd, &fn);
-	fn(nvmeq, ctx, &cqe);
-}
-
-static enum blk_eh_timer_return nvme_timeout(struct request *req, bool reserved)
-{
-	struct nvme_cmd_info *cmd = blk_mq_rq_to_pdu(req);
-	struct nvme_queue *nvmeq = cmd->nvmeq;
-
-	dev_warn(nvmeq->q_dmadev, "Timeout I/O %d QID %d\n", req->tag,
-							nvmeq->qid);
-	spin_lock_irq(&nvmeq->q_lock);
-	nvme_abort_req(req);
-	spin_unlock_irq(&nvmeq->q_lock);
-
-	/*
-	 * The aborted req will be completed on receiving the abort req.
-	 * We enable the timer again. If hit twice, it'll cause a device reset,
-	 * as the device then is in a faulty state.
-	 */
-	return BLK_EH_RESET_TIMER;
+		status |= NVME_SC_DNR;
+	blk_mq_complete_request(req, status);
 }
 
 static void nvme_free_queue(struct nvme_queue *nvmeq)
@@ -1429,8 +1067,8 @@ static int nvme_suspend_queue(struct nvme_queue *nvmeq)
 	nvmeq->cq_vector = -1;
 	spin_unlock_irq(&nvmeq->q_lock);
 
-	if (!nvmeq->qid && nvmeq->dev->admin_q)
-		blk_mq_freeze_queue_start(nvmeq->dev->admin_q);
+	if (!nvmeq->qid && nvmeq->dev->ctrl.admin_q)
+		blk_mq_stop_hw_queues(nvmeq->dev->ctrl.admin_q);
 
 	irq_set_affinity_hint(vector, NULL);
 	free_irq(vector, nvmeq);
@@ -1446,21 +1084,20 @@ static void nvme_clear_queue(struct nvme_queue *nvmeq)
 	spin_unlock_irq(&nvmeq->q_lock);
 }
 
-static void nvme_disable_queue(struct nvme_dev *dev, int qid)
+static void nvme_disable_admin_queue(struct nvme_dev *dev, bool shutdown)
 {
-	struct nvme_queue *nvmeq = dev->queues[qid];
+	struct nvme_queue *nvmeq = dev->queues[0];
 
 	if (!nvmeq)
 		return;
 	if (nvme_suspend_queue(nvmeq))
 		return;
 
-	/* Don't tell the adapter to delete the admin queue.
-	 * Don't tell a removed adapter to delete IO queues. */
-	if (qid && readl(&dev->bar->csts) != -1) {
-		adapter_delete_sq(dev, qid);
-		adapter_delete_cq(dev, qid);
-	}
+	if (shutdown)
+		nvme_shutdown_ctrl(&dev->ctrl);
+	else
+		nvme_disable_ctrl(&dev->ctrl, lo_hi_readq(
+						dev->bar + NVME_REG_CAP));
 
 	spin_lock_irq(&nvmeq->q_lock);
 	nvme_process_cq(nvmeq);
@@ -1471,11 +1108,12 @@ static int nvme_cmb_qdepth(struct nvme_dev *dev, int nr_io_queues,
 				int entry_size)
 {
 	int q_depth = dev->q_depth;
-	unsigned q_size_aligned = roundup(q_depth * entry_size, dev->page_size);
+	unsigned q_size_aligned = roundup(q_depth * entry_size,
+					  dev->ctrl.page_size);
 
 	if (q_size_aligned * nr_io_queues > dev->cmb_size) {
 		u64 mem_per_q = div_u64(dev->cmb_size, nr_io_queues);
-		mem_per_q = round_down(mem_per_q, dev->page_size);
+		mem_per_q = round_down(mem_per_q, dev->ctrl.page_size);
 		q_depth = div_u64(mem_per_q, entry_size);
 
 		/*
@@ -1494,8 +1132,8 @@ static int nvme_alloc_sq_cmds(struct nvme_dev *dev, struct nvme_queue *nvmeq,
 				int qid, int depth)
 {
 	if (qid && dev->cmb && use_cmb_sqes && NVME_CMB_SQS(dev->cmbsz)) {
-		unsigned offset = (qid - 1) *
-					roundup(SQ_SIZE(depth), dev->page_size);
+		unsigned offset = (qid - 1) * roundup(SQ_SIZE(depth),
+						      dev->ctrl.page_size);
 		nvmeq->sq_dma_addr = dev->cmb_dma_addr + offset;
 		nvmeq->sq_cmds_io = dev->cmb + offset;
 	} else {
@@ -1526,7 +1164,7 @@ static struct nvme_queue *nvme_alloc_queue(struct nvme_dev *dev, int qid,
 	nvmeq->q_dmadev = dev->dev;
 	nvmeq->dev = dev;
 	snprintf(nvmeq->irqname, sizeof(nvmeq->irqname), "nvme%dq%d",
-			dev->instance, qid);
+			dev->ctrl.instance, qid);
 	spin_lock_init(&nvmeq->q_lock);
 	nvmeq->cq_head = 0;
 	nvmeq->cq_phase = 1;
@@ -1603,79 +1241,9 @@ static int nvme_create_queue(struct nvme_queue *nvmeq, int qid)
 	return result;
 }
 
-static int nvme_wait_ready(struct nvme_dev *dev, u64 cap, bool enabled)
-{
-	unsigned long timeout;
-	u32 bit = enabled ? NVME_CSTS_RDY : 0;
-
-	timeout = ((NVME_CAP_TIMEOUT(cap) + 1) * HZ / 2) + jiffies;
-
-	while ((readl(&dev->bar->csts) & NVME_CSTS_RDY) != bit) {
-		msleep(100);
-		if (fatal_signal_pending(current))
-			return -EINTR;
-		if (time_after(jiffies, timeout)) {
-			dev_err(dev->dev,
-				"Device not ready; aborting %s\n", enabled ?
-						"initialisation" : "reset");
-			return -ENODEV;
-		}
-	}
-
-	return 0;
-}
-
-/*
- * If the device has been passed off to us in an enabled state, just clear
- * the enabled bit.  The spec says we should set the 'shutdown notification
- * bits', but doing so may cause the device to complete commands to the
- * admin queue ... and we don't know what memory that might be pointing at!
- */
-static int nvme_disable_ctrl(struct nvme_dev *dev, u64 cap)
-{
-	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
-	dev->ctrl_config &= ~NVME_CC_ENABLE;
-	writel(dev->ctrl_config, &dev->bar->cc);
-
-	return nvme_wait_ready(dev, cap, false);
-}
-
-static int nvme_enable_ctrl(struct nvme_dev *dev, u64 cap)
-{
-	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
-	dev->ctrl_config |= NVME_CC_ENABLE;
-	writel(dev->ctrl_config, &dev->bar->cc);
-
-	return nvme_wait_ready(dev, cap, true);
-}
-
-static int nvme_shutdown_ctrl(struct nvme_dev *dev)
-{
-	unsigned long timeout;
-
-	dev->ctrl_config &= ~NVME_CC_SHN_MASK;
-	dev->ctrl_config |= NVME_CC_SHN_NORMAL;
-
-	writel(dev->ctrl_config, &dev->bar->cc);
-
-	timeout = SHUTDOWN_TIMEOUT + jiffies;
-	while ((readl(&dev->bar->csts) & NVME_CSTS_SHST_MASK) !=
-							NVME_CSTS_SHST_CMPLT) {
-		msleep(100);
-		if (fatal_signal_pending(current))
-			return -EINTR;
-		if (time_after(jiffies, timeout)) {
-			dev_err(dev->dev,
-				"Device shutdown incomplete; abort shutdown\n");
-			return -ENODEV;
-		}
-	}
-
-	return 0;
-}
-
 static struct blk_mq_ops nvme_mq_admin_ops = {
 	.queue_rq	= nvme_queue_rq,
+	.complete	= nvme_complete_rq,
 	.map_queue	= blk_mq_map_queue,
 	.init_hctx	= nvme_admin_init_hctx,
 	.exit_hctx      = nvme_admin_exit_hctx,
@@ -1685,6 +1253,7 @@ static struct blk_mq_ops nvme_mq_admin_ops = {
 
 static struct blk_mq_ops nvme_mq_ops = {
 	.queue_rq	= nvme_queue_rq,
+	.complete	= nvme_complete_rq,
 	.map_queue	= blk_mq_map_queue,
 	.init_hctx	= nvme_init_hctx,
 	.init_request	= nvme_init_request,
@@ -1694,19 +1263,29 @@ static struct blk_mq_ops nvme_mq_ops = {
 
 static void nvme_dev_remove_admin(struct nvme_dev *dev)
 {
-	if (dev->admin_q && !blk_queue_dying(dev->admin_q)) {
-		blk_cleanup_queue(dev->admin_q);
+	if (dev->ctrl.admin_q && !blk_queue_dying(dev->ctrl.admin_q)) {
+		/*
+		 * If the controller was reset during removal, it's possible
+		 * user requests may be waiting on a stopped queue. Start the
+		 * queue to flush these to completion.
+		 */
+		blk_mq_start_stopped_hw_queues(dev->ctrl.admin_q, true);
+		blk_cleanup_queue(dev->ctrl.admin_q);
 		blk_mq_free_tag_set(&dev->admin_tagset);
 	}
 }
 
 static int nvme_alloc_admin_tags(struct nvme_dev *dev)
 {
-	if (!dev->admin_q) {
+	if (!dev->ctrl.admin_q) {
 		dev->admin_tagset.ops = &nvme_mq_admin_ops;
 		dev->admin_tagset.nr_hw_queues = 1;
-		dev->admin_tagset.queue_depth = NVME_AQ_DEPTH - 1;
-		dev->admin_tagset.reserved_tags = 1;
+
+		/*
+		 * Subtract one to leave an empty queue entry for 'Full Queue'
+		 * condition. See NVM-Express 1.2 specification, section 4.1.2.
+		 */
+		dev->admin_tagset.queue_depth = NVME_AQ_BLKMQ_DEPTH - 1;
 		dev->admin_tagset.timeout = ADMIN_TIMEOUT;
 		dev->admin_tagset.numa_node = dev_to_node(dev->dev);
 		dev->admin_tagset.cmd_size = nvme_cmd_size(dev);
@@ -1715,18 +1294,18 @@ static int nvme_alloc_admin_tags(struct nvme_dev *dev)
 		if (blk_mq_alloc_tag_set(&dev->admin_tagset))
 			return -ENOMEM;
 
-		dev->admin_q = blk_mq_init_queue(&dev->admin_tagset);
-		if (IS_ERR(dev->admin_q)) {
+		dev->ctrl.admin_q = blk_mq_init_queue(&dev->admin_tagset);
+		if (IS_ERR(dev->ctrl.admin_q)) {
 			blk_mq_free_tag_set(&dev->admin_tagset);
 			return -ENOMEM;
 		}
-		if (!blk_get_queue(dev->admin_q)) {
+		if (!blk_get_queue(dev->ctrl.admin_q)) {
 			nvme_dev_remove_admin(dev);
-			dev->admin_q = NULL;
+			dev->ctrl.admin_q = NULL;
 			return -ENODEV;
 		}
 	} else
-		blk_mq_unfreeze_queue(dev->admin_q);
+		blk_mq_start_stopped_hw_queues(dev->ctrl.admin_q, true);
 
 	return 0;
 }
@@ -1735,31 +1314,17 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 {
 	int result;
 	u32 aqa;
-	u64 cap = lo_hi_readq(&dev->bar->cap);
+	u64 cap = lo_hi_readq(dev->bar + NVME_REG_CAP);
 	struct nvme_queue *nvmeq;
-	/*
-	 * default to a 4K page size, with the intention to update this
-	 * path in the future to accomodate architectures with differing
-	 * kernel and IO page sizes.
-	 */
-	unsigned page_shift = 12;
-	unsigned dev_page_min = NVME_CAP_MPSMIN(cap) + 12;
-
-	if (page_shift < dev_page_min) {
-		dev_err(dev->dev,
-				"Minimum device page size (%u) too large for "
-				"host (%u)\n", 1 << dev_page_min,
-				1 << page_shift);
-		return -ENODEV;
-	}
 
-	dev->subsystem = readl(&dev->bar->vs) >= NVME_VS(1, 1) ?
+	dev->subsystem = readl(dev->bar + NVME_REG_VS) >= NVME_VS(1, 1) ?
 						NVME_CAP_NSSRC(cap) : 0;
 
-	if (dev->subsystem && (readl(&dev->bar->csts) & NVME_CSTS_NSSRO))
-		writel(NVME_CSTS_NSSRO, &dev->bar->csts);
+	if (dev->subsystem &&
+	    (readl(dev->bar + NVME_REG_CSTS) & NVME_CSTS_NSSRO))
+		writel(NVME_CSTS_NSSRO, dev->bar + NVME_REG_CSTS);
 
-	result = nvme_disable_ctrl(dev, cap);
+	result = nvme_disable_ctrl(&dev->ctrl, cap);
 	if (result < 0)
 		return result;
 
@@ -1773,18 +1338,11 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	aqa = nvmeq->q_depth - 1;
 	aqa |= aqa << 16;
 
-	dev->page_size = 1 << page_shift;
+	writel(aqa, dev->bar + NVME_REG_AQA);
+	lo_hi_writeq(nvmeq->sq_dma_addr, dev->bar + NVME_REG_ASQ);
+	lo_hi_writeq(nvmeq->cq_dma_addr, dev->bar + NVME_REG_ACQ);
 
-	dev->ctrl_config = NVME_CC_CSS_NVM;
-	dev->ctrl_config |= (page_shift - 12) << NVME_CC_MPS_SHIFT;
-	dev->ctrl_config |= NVME_CC_ARB_RR | NVME_CC_SHN_NONE;
-	dev->ctrl_config |= NVME_CC_IOSQES | NVME_CC_IOCQES;
-
-	writel(aqa, &dev->bar->aqa);
-	lo_hi_writeq(nvmeq->sq_dma_addr, &dev->bar->asq);
-	lo_hi_writeq(nvmeq->cq_dma_addr, &dev->bar->acq);
-
-	result = nvme_enable_ctrl(dev, cap);
+	result = nvme_enable_ctrl(&dev->ctrl, cap);
 	if (result)
 		goto free_nvmeq;
 
@@ -1802,406 +1360,6 @@ static int nvme_configure_admin_queue(struct nvme_dev *dev)
 	return result;
 }
 
-static int nvme_submit_io(struct nvme_ns *ns, struct nvme_user_io __user *uio)
-{
-	struct nvme_dev *dev = ns->dev;
-	struct nvme_user_io io;
-	struct nvme_command c;
-	unsigned length, meta_len;
-	int status, write;
-	dma_addr_t meta_dma = 0;
-	void *meta = NULL;
-	void __user *metadata;
-
-	if (copy_from_user(&io, uio, sizeof(io)))
-		return -EFAULT;
-
-	switch (io.opcode) {
-	case nvme_cmd_write:
-	case nvme_cmd_read:
-	case nvme_cmd_compare:
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	length = (io.nblocks + 1) << ns->lba_shift;
-	meta_len = (io.nblocks + 1) * ns->ms;
-	metadata = (void __user *)(uintptr_t)io.metadata;
-	write = io.opcode & 1;
-
-	if (ns->ext) {
-		length += meta_len;
-		meta_len = 0;
-	}
-	if (meta_len) {
-		if (((io.metadata & 3) || !io.metadata) && !ns->ext)
-			return -EINVAL;
-
-		meta = dma_alloc_coherent(dev->dev, meta_len,
-						&meta_dma, GFP_KERNEL);
-
-		if (!meta) {
-			status = -ENOMEM;
-			goto unmap;
-		}
-		if (write) {
-			if (copy_from_user(meta, metadata, meta_len)) {
-				status = -EFAULT;
-				goto unmap;
-			}
-		}
-	}
-
-	memset(&c, 0, sizeof(c));
-	c.rw.opcode = io.opcode;
-	c.rw.flags = io.flags;
-	c.rw.nsid = cpu_to_le32(ns->ns_id);
-	c.rw.slba = cpu_to_le64(io.slba);
-	c.rw.length = cpu_to_le16(io.nblocks);
-	c.rw.control = cpu_to_le16(io.control);
-	c.rw.dsmgmt = cpu_to_le32(io.dsmgmt);
-	c.rw.reftag = cpu_to_le32(io.reftag);
-	c.rw.apptag = cpu_to_le16(io.apptag);
-	c.rw.appmask = cpu_to_le16(io.appmask);
-	c.rw.metadata = cpu_to_le64(meta_dma);
-
-	status = __nvme_submit_sync_cmd(ns->queue, &c, NULL,
-			(void __user *)(uintptr_t)io.addr, length, NULL, 0);
- unmap:
-	if (meta) {
-		if (status == NVME_SC_SUCCESS && !write) {
-			if (copy_to_user(metadata, meta, meta_len))
-				status = -EFAULT;
-		}
-		dma_free_coherent(dev->dev, meta_len, meta, meta_dma);
-	}
-	return status;
-}
-
-static int nvme_user_cmd(struct nvme_dev *dev, struct nvme_ns *ns,
-			struct nvme_passthru_cmd __user *ucmd)
-{
-	struct nvme_passthru_cmd cmd;
-	struct nvme_command c;
-	unsigned timeout = 0;
-	int status;
-
-	if (!capable(CAP_SYS_ADMIN))
-		return -EACCES;
-	if (copy_from_user(&cmd, ucmd, sizeof(cmd)))
-		return -EFAULT;
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = cmd.opcode;
-	c.common.flags = cmd.flags;
-	c.common.nsid = cpu_to_le32(cmd.nsid);
-	c.common.cdw2[0] = cpu_to_le32(cmd.cdw2);
-	c.common.cdw2[1] = cpu_to_le32(cmd.cdw3);
-	c.common.cdw10[0] = cpu_to_le32(cmd.cdw10);
-	c.common.cdw10[1] = cpu_to_le32(cmd.cdw11);
-	c.common.cdw10[2] = cpu_to_le32(cmd.cdw12);
-	c.common.cdw10[3] = cpu_to_le32(cmd.cdw13);
-	c.common.cdw10[4] = cpu_to_le32(cmd.cdw14);
-	c.common.cdw10[5] = cpu_to_le32(cmd.cdw15);
-
-	if (cmd.timeout_ms)
-		timeout = msecs_to_jiffies(cmd.timeout_ms);
-
-	status = __nvme_submit_sync_cmd(ns ? ns->queue : dev->admin_q, &c,
-			NULL, (void __user *)(uintptr_t)cmd.addr, cmd.data_len,
-			&cmd.result, timeout);
-	if (status >= 0) {
-		if (put_user(cmd.result, &ucmd->result))
-			return -EFAULT;
-	}
-
-	return status;
-}
-
-static int nvme_subsys_reset(struct nvme_dev *dev)
-{
-	if (!dev->subsystem)
-		return -ENOTTY;
-
-	writel(0x4E564D65, &dev->bar->nssr); /* "NVMe" */
-	return 0;
-}
-
-static int nvme_ioctl(struct block_device *bdev, fmode_t mode, unsigned int cmd,
-							unsigned long arg)
-{
-	struct nvme_ns *ns = bdev->bd_disk->private_data;
-
-	switch (cmd) {
-	case NVME_IOCTL_ID:
-		force_successful_syscall_return();
-		return ns->ns_id;
-	case NVME_IOCTL_ADMIN_CMD:
-		return nvme_user_cmd(ns->dev, NULL, (void __user *)arg);
-	case NVME_IOCTL_IO_CMD:
-		return nvme_user_cmd(ns->dev, ns, (void __user *)arg);
-	case NVME_IOCTL_SUBMIT_IO:
-		return nvme_submit_io(ns, (void __user *)arg);
-	case SG_GET_VERSION_NUM:
-		return nvme_sg_get_version_num((void __user *)arg);
-	case SG_IO:
-		return nvme_sg_io(ns, (void __user *)arg);
-	default:
-		return -ENOTTY;
-	}
-}
-
-#ifdef CONFIG_COMPAT
-static int nvme_compat_ioctl(struct block_device *bdev, fmode_t mode,
-					unsigned int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case SG_IO:
-		return -ENOIOCTLCMD;
-	}
-	return nvme_ioctl(bdev, mode, cmd, arg);
-}
-#else
-#define nvme_compat_ioctl	NULL
-#endif
-
-static void nvme_free_dev(struct kref *kref);
-static void nvme_free_ns(struct kref *kref)
-{
-	struct nvme_ns *ns = container_of(kref, struct nvme_ns, kref);
-
-	if (ns->type == NVME_NS_LIGHTNVM)
-		nvme_nvm_unregister(ns->queue, ns->disk->disk_name);
-
-	spin_lock(&dev_list_lock);
-	ns->disk->private_data = NULL;
-	spin_unlock(&dev_list_lock);
-
-	kref_put(&ns->dev->kref, nvme_free_dev);
-	put_disk(ns->disk);
-	kfree(ns);
-}
-
-static int nvme_open(struct block_device *bdev, fmode_t mode)
-{
-	int ret = 0;
-	struct nvme_ns *ns;
-
-	spin_lock(&dev_list_lock);
-	ns = bdev->bd_disk->private_data;
-	if (!ns)
-		ret = -ENXIO;
-	else if (!kref_get_unless_zero(&ns->kref))
-		ret = -ENXIO;
-	spin_unlock(&dev_list_lock);
-
-	return ret;
-}
-
-static void nvme_release(struct gendisk *disk, fmode_t mode)
-{
-	struct nvme_ns *ns = disk->private_data;
-	kref_put(&ns->kref, nvme_free_ns);
-}
-
-static int nvme_getgeo(struct block_device *bd, struct hd_geometry *geo)
-{
-	/* some standard values */
-	geo->heads = 1 << 6;
-	geo->sectors = 1 << 5;
-	geo->cylinders = get_capacity(bd->bd_disk) >> 11;
-	return 0;
-}
-
-static void nvme_config_discard(struct nvme_ns *ns)
-{
-	u32 logical_block_size = queue_logical_block_size(ns->queue);
-	ns->queue->limits.discard_zeroes_data = 0;
-	ns->queue->limits.discard_alignment = logical_block_size;
-	ns->queue->limits.discard_granularity = logical_block_size;
-	blk_queue_max_discard_sectors(ns->queue, 0xffffffff);
-	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, ns->queue);
-}
-
-static int nvme_revalidate_disk(struct gendisk *disk)
-{
-	struct nvme_ns *ns = disk->private_data;
-	struct nvme_dev *dev = ns->dev;
-	struct nvme_id_ns *id;
-	u8 lbaf, pi_type;
-	u16 old_ms;
-	unsigned short bs;
-
-	if (nvme_identify_ns(dev, ns->ns_id, &id)) {
-		dev_warn(dev->dev, "%s: Identify failure nvme%dn%d\n", __func__,
-						dev->instance, ns->ns_id);
-		return -ENODEV;
-	}
-	if (id->ncap == 0) {
-		kfree(id);
-		return -ENODEV;
-	}
-
-	if (nvme_nvm_ns_supported(ns, id) && ns->type != NVME_NS_LIGHTNVM) {
-		if (nvme_nvm_register(ns->queue, disk->disk_name)) {
-			dev_warn(dev->dev,
-				"%s: LightNVM init failure\n", __func__);
-			kfree(id);
-			return -ENODEV;
-		}
-		ns->type = NVME_NS_LIGHTNVM;
-	}
-
-	old_ms = ns->ms;
-	lbaf = id->flbas & NVME_NS_FLBAS_LBA_MASK;
-	ns->lba_shift = id->lbaf[lbaf].ds;
-	ns->ms = le16_to_cpu(id->lbaf[lbaf].ms);
-	ns->ext = ns->ms && (id->flbas & NVME_NS_FLBAS_META_EXT);
-
-	/*
-	 * If identify namespace failed, use default 512 byte block size so
-	 * block layer can use before failing read/write for 0 capacity.
-	 */
-	if (ns->lba_shift == 0)
-		ns->lba_shift = 9;
-	bs = 1 << ns->lba_shift;
-
-	/* XXX: PI implementation requires metadata equal t10 pi tuple size */
-	pi_type = ns->ms == sizeof(struct t10_pi_tuple) ?
-					id->dps & NVME_NS_DPS_PI_MASK : 0;
-
-	blk_mq_freeze_queue(disk->queue);
-	if (blk_get_integrity(disk) && (ns->pi_type != pi_type ||
-				ns->ms != old_ms ||
-				bs != queue_logical_block_size(disk->queue) ||
-				(ns->ms && ns->ext)))
-		blk_integrity_unregister(disk);
-
-	ns->pi_type = pi_type;
-	blk_queue_logical_block_size(ns->queue, bs);
-
-	if (ns->ms && !ns->ext)
-		nvme_init_integrity(ns);
-
-	if ((ns->ms && !(ns->ms == 8 && ns->pi_type) &&
-						!blk_get_integrity(disk)) ||
-						ns->type == NVME_NS_LIGHTNVM)
-		set_capacity(disk, 0);
-	else
-		set_capacity(disk, le64_to_cpup(&id->nsze) << (ns->lba_shift - 9));
-
-	if (dev->oncs & NVME_CTRL_ONCS_DSM)
-		nvme_config_discard(ns);
-	blk_mq_unfreeze_queue(disk->queue);
-
-	kfree(id);
-	return 0;
-}
-
-static char nvme_pr_type(enum pr_type type)
-{
-	switch (type) {
-	case PR_WRITE_EXCLUSIVE:
-		return 1;
-	case PR_EXCLUSIVE_ACCESS:
-		return 2;
-	case PR_WRITE_EXCLUSIVE_REG_ONLY:
-		return 3;
-	case PR_EXCLUSIVE_ACCESS_REG_ONLY:
-		return 4;
-	case PR_WRITE_EXCLUSIVE_ALL_REGS:
-		return 5;
-	case PR_EXCLUSIVE_ACCESS_ALL_REGS:
-		return 6;
-	default:
-		return 0;
-	}
-};
-
-static int nvme_pr_command(struct block_device *bdev, u32 cdw10,
-				u64 key, u64 sa_key, u8 op)
-{
-	struct nvme_ns *ns = bdev->bd_disk->private_data;
-	struct nvme_command c;
-	u8 data[16] = { 0, };
-
-	put_unaligned_le64(key, &data[0]);
-	put_unaligned_le64(sa_key, &data[8]);
-
-	memset(&c, 0, sizeof(c));
-	c.common.opcode = op;
-	c.common.nsid = cpu_to_le32(ns->ns_id);
-	c.common.cdw10[0] = cpu_to_le32(cdw10);
-
-	return nvme_submit_sync_cmd(ns->queue, &c, data, 16);
-}
-
-static int nvme_pr_register(struct block_device *bdev, u64 old,
-		u64 new, unsigned flags)
-{
-	u32 cdw10;
-
-	if (flags & ~PR_FL_IGNORE_KEY)
-		return -EOPNOTSUPP;
-
-	cdw10 = old ? 2 : 0;
-	cdw10 |= (flags & PR_FL_IGNORE_KEY) ? 1 << 3 : 0;
-	cdw10 |= (1 << 30) | (1 << 31); /* PTPL=1 */
-	return nvme_pr_command(bdev, cdw10, old, new, nvme_cmd_resv_register);
-}
-
-static int nvme_pr_reserve(struct block_device *bdev, u64 key,
-		enum pr_type type, unsigned flags)
-{
-	u32 cdw10;
-
-	if (flags & ~PR_FL_IGNORE_KEY)
-		return -EOPNOTSUPP;
-
-	cdw10 = nvme_pr_type(type) << 8;
-	cdw10 |= ((flags & PR_FL_IGNORE_KEY) ? 1 << 3 : 0);
-	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_acquire);
-}
-
-static int nvme_pr_preempt(struct block_device *bdev, u64 old, u64 new,
-		enum pr_type type, bool abort)
-{
-	u32 cdw10 = nvme_pr_type(type) << 8 | abort ? 2 : 1;
-	return nvme_pr_command(bdev, cdw10, old, new, nvme_cmd_resv_acquire);
-}
-
-static int nvme_pr_clear(struct block_device *bdev, u64 key)
-{
-	u32 cdw10 = 1 | (key ? 1 << 3 : 0);
-	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_register);
-}
-
-static int nvme_pr_release(struct block_device *bdev, u64 key, enum pr_type type)
-{
-	u32 cdw10 = nvme_pr_type(type) << 8 | key ? 1 << 3 : 0;
-	return nvme_pr_command(bdev, cdw10, key, 0, nvme_cmd_resv_release);
-}
-
-static const struct pr_ops nvme_pr_ops = {
-	.pr_register	= nvme_pr_register,
-	.pr_reserve	= nvme_pr_reserve,
-	.pr_release	= nvme_pr_release,
-	.pr_preempt	= nvme_pr_preempt,
-	.pr_clear	= nvme_pr_clear,
-};
-
-static const struct block_device_operations nvme_fops = {
-	.owner		= THIS_MODULE,
-	.ioctl		= nvme_ioctl,
-	.compat_ioctl	= nvme_compat_ioctl,
-	.open		= nvme_open,
-	.release	= nvme_release,
-	.getgeo		= nvme_getgeo,
-	.revalidate_disk= nvme_revalidate_disk,
-	.pr_ops		= &nvme_pr_ops,
-};
-
 static int nvme_kthread(void *data)
 {
 	struct nvme_dev *dev, *next;
@@ -2211,14 +1369,20 @@ static int nvme_kthread(void *data)
 		spin_lock(&dev_list_lock);
 		list_for_each_entry_safe(dev, next, &dev_list, node) {
 			int i;
-			u32 csts = readl(&dev->bar->csts);
+			u32 csts = readl(dev->bar + NVME_REG_CSTS);
+
+			/*
+			 * Skip controllers currently under reset.
+			 */
+			if (work_pending(&dev->reset_work) || work_busy(&dev->reset_work))
+				continue;
 
 			if ((dev->subsystem && (csts & NVME_CSTS_NSSRO)) ||
 							csts & NVME_CSTS_CFS) {
-				if (!__nvme_reset(dev)) {
+				if (queue_work(nvme_workq, &dev->reset_work)) {
 					dev_warn(dev->dev,
 						"Failed status: %x, reset controller\n",
-						readl(&dev->bar->csts));
+						readl(dev->bar + NVME_REG_CSTS));
 				}
 				continue;
 			}
@@ -2229,11 +1393,8 @@ static int nvme_kthread(void *data)
 				spin_lock_irq(&nvmeq->q_lock);
 				nvme_process_cq(nvmeq);
 
-				while ((i == 0) && (dev->event_limit > 0)) {
-					if (nvme_submit_async_admin_req(dev))
-						break;
-					dev->event_limit--;
-				}
+				while (i == 0 && dev->ctrl.event_limit > 0)
+					nvme_submit_async_event(dev);
 				spin_unlock_irq(&nvmeq->q_lock);
 			}
 		}
@@ -2243,127 +1404,33 @@ static int nvme_kthread(void *data)
 	return 0;
 }
 
-static void nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid)
-{
-	struct nvme_ns *ns;
-	struct gendisk *disk;
-	int node = dev_to_node(dev->dev);
-
-	ns = kzalloc_node(sizeof(*ns), GFP_KERNEL, node);
-	if (!ns)
-		return;
-
-	ns->queue = blk_mq_init_queue(&dev->tagset);
-	if (IS_ERR(ns->queue))
-		goto out_free_ns;
-	queue_flag_set_unlocked(QUEUE_FLAG_NOMERGES, ns->queue);
-	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, ns->queue);
-	ns->dev = dev;
-	ns->queue->queuedata = ns;
-
-	disk = alloc_disk_node(0, node);
-	if (!disk)
-		goto out_free_queue;
-
-	kref_init(&ns->kref);
-	ns->ns_id = nsid;
-	ns->disk = disk;
-	ns->lba_shift = 9; /* set to a default value for 512 until disk is validated */
-	list_add_tail(&ns->list, &dev->namespaces);
-
-	blk_queue_logical_block_size(ns->queue, 1 << ns->lba_shift);
-	if (dev->max_hw_sectors) {
-		blk_queue_max_hw_sectors(ns->queue, dev->max_hw_sectors);
-		blk_queue_max_segments(ns->queue,
-			(dev->max_hw_sectors / (dev->page_size >> 9)) + 1);
-	}
-	if (dev->stripe_size)
-		blk_queue_chunk_sectors(ns->queue, dev->stripe_size >> 9);
-	if (dev->vwc & NVME_CTRL_VWC_PRESENT)
-		blk_queue_flush(ns->queue, REQ_FLUSH | REQ_FUA);
-	blk_queue_virt_boundary(ns->queue, dev->page_size - 1);
-
-	disk->major = nvme_major;
-	disk->first_minor = 0;
-	disk->fops = &nvme_fops;
-	disk->private_data = ns;
-	disk->queue = ns->queue;
-	disk->driverfs_dev = dev->device;
-	disk->flags = GENHD_FL_EXT_DEVT;
-	sprintf(disk->disk_name, "nvme%dn%d", dev->instance, nsid);
-
-	/*
-	 * Initialize capacity to 0 until we establish the namespace format and
-	 * setup integrity extentions if necessary. The revalidate_disk after
-	 * add_disk allows the driver to register with integrity if the format
-	 * requires it.
-	 */
-	set_capacity(disk, 0);
-	if (nvme_revalidate_disk(ns->disk))
-		goto out_free_disk;
-
-	kref_get(&dev->kref);
-	if (ns->type != NVME_NS_LIGHTNVM) {
-		add_disk(ns->disk);
-		if (ns->ms) {
-			struct block_device *bd = bdget_disk(ns->disk, 0);
-			if (!bd)
-				return;
-			if (blkdev_get(bd, FMODE_READ, NULL)) {
-				bdput(bd);
-				return;
-			}
-			blkdev_reread_part(bd);
-			blkdev_put(bd, FMODE_READ);
-		}
-	}
-	return;
- out_free_disk:
-	kfree(disk);
-	list_del(&ns->list);
- out_free_queue:
-	blk_cleanup_queue(ns->queue);
- out_free_ns:
-	kfree(ns);
-}
-
-/*
- * Create I/O queues.  Failing to create an I/O queue is not an issue,
- * we can continue with less than the desired amount of queues, and
- * even a controller without I/O queues an still be used to issue
- * admin commands.  This might be useful to upgrade a buggy firmware
- * for example.
- */
-static void nvme_create_io_queues(struct nvme_dev *dev)
+static int nvme_create_io_queues(struct nvme_dev *dev)
 {
 	unsigned i;
+	int ret = 0;
 
-	for (i = dev->queue_count; i <= dev->max_qid; i++)
-		if (!nvme_alloc_queue(dev, i, dev->q_depth))
+	for (i = dev->queue_count; i <= dev->max_qid; i++) {
+		if (!nvme_alloc_queue(dev, i, dev->q_depth)) {
+			ret = -ENOMEM;
 			break;
+		}
+	}
 
-	for (i = dev->online_queues; i <= dev->queue_count - 1; i++)
-		if (nvme_create_queue(dev->queues[i], i)) {
+	for (i = dev->online_queues; i <= dev->queue_count - 1; i++) {
+		ret = nvme_create_queue(dev->queues[i], i);
+		if (ret) {
 			nvme_free_queues(dev, i);
 			break;
 		}
-}
-
-static int set_queue_count(struct nvme_dev *dev, int count)
-{
-	int status;
-	u32 result;
-	u32 q_count = (count - 1) | ((count - 1) << 16);
-
-	status = nvme_set_features(dev, NVME_FEAT_NUM_QUEUES, q_count, 0,
-								&result);
-	if (status < 0)
-		return status;
-	if (status > 0) {
-		dev_err(dev->dev, "Could not set queue count (%d)\n", status);
-		return 0;
 	}
-	return min(result & 0xffff, result >> 16) + 1;
+
+	/*
+	 * Ignore failing Create SQ/CQ commands, we can continue with less
+	 * than the desired aount of queues, and even a controller without
+	 * I/O queues an still be used to issue admin commands.  This might
+	 * be useful to upgrade a buggy firmware for example.
+	 */
+	return ret >= 0 ? 0 : ret;
 }
 
 static void __iomem *nvme_map_cmb(struct nvme_dev *dev)
@@ -2378,11 +1445,11 @@ static void __iomem *nvme_map_cmb(struct nvme_dev *dev)
 	if (!use_cmb_sqes)
 		return NULL;
 
-	dev->cmbsz = readl(&dev->bar->cmbsz);
+	dev->cmbsz = readl(dev->bar + NVME_REG_CMBSZ);
 	if (!(NVME_CMB_SZ(dev->cmbsz)))
 		return NULL;
 
-	cmbloc = readl(&dev->bar->cmbloc);
+	cmbloc = readl(dev->bar + NVME_REG_CMBLOC);
 
 	szu = (u64)1 << (12 + 4 * NVME_CMB_SZU(dev->cmbsz));
 	size = szu * NVME_CMB_SZ(dev->cmbsz);
@@ -2430,11 +1497,20 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 	int result, i, vecs, nr_io_queues, size;
 
 	nr_io_queues = num_possible_cpus();
-	result = set_queue_count(dev, nr_io_queues);
-	if (result <= 0)
+	result = nvme_set_queue_count(&dev->ctrl, &nr_io_queues);
+	if (result < 0)
 		return result;
-	if (result < nr_io_queues)
-		nr_io_queues = result;
+
+	/*
+	 * Degraded controllers might return an error when setting the queue
+	 * count.  We still want to be able to bring them online and offer
+	 * access to the admin queue, as that might be only way to fix them up.
+	 */
+	if (result > 0) {
+		dev_err(dev->dev, "Could not set queue count (%d)\n", result);
+		nr_io_queues = 0;
+		result = 0;
+	}
 
 	if (dev->cmb && NVME_CMB_SQS(dev->cmbsz)) {
 		result = nvme_cmb_qdepth(dev, nr_io_queues,
@@ -2456,7 +1532,7 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 				return -ENOMEM;
 			size = db_bar_size(dev, nr_io_queues);
 		} while (1);
-		dev->dbs = ((void __iomem *)dev->bar) + 4096;
+		dev->dbs = dev->bar + 4096;
 		adminq->q_db = dev->dbs;
 	}
 
@@ -2500,115 +1576,115 @@ static int nvme_setup_io_queues(struct nvme_dev *dev)
 
 	/* Free previously allocated queues that are no longer usable */
 	nvme_free_queues(dev, nr_io_queues + 1);
-	nvme_create_io_queues(dev);
-
-	return 0;
+	return nvme_create_io_queues(dev);
 
  free_queues:
 	nvme_free_queues(dev, 1);
 	return result;
 }
 
-static int ns_cmp(void *priv, struct list_head *a, struct list_head *b)
+static void nvme_set_irq_hints(struct nvme_dev *dev)
 {
-	struct nvme_ns *nsa = container_of(a, struct nvme_ns, list);
-	struct nvme_ns *nsb = container_of(b, struct nvme_ns, list);
+	struct nvme_queue *nvmeq;
+	int i;
 
-	return nsa->ns_id - nsb->ns_id;
-}
+	for (i = 0; i < dev->online_queues; i++) {
+		nvmeq = dev->queues[i];
 
-static struct nvme_ns *nvme_find_ns(struct nvme_dev *dev, unsigned nsid)
-{
-	struct nvme_ns *ns;
+		if (!nvmeq->tags || !(*nvmeq->tags))
+			continue;
 
-	list_for_each_entry(ns, &dev->namespaces, list) {
-		if (ns->ns_id == nsid)
-			return ns;
-		if (ns->ns_id > nsid)
-			break;
+		irq_set_affinity_hint(dev->entry[nvmeq->cq_vector].vector,
+					blk_mq_tags_cpumask(*nvmeq->tags));
 	}
-	return NULL;
 }
 
-static inline bool nvme_io_incapable(struct nvme_dev *dev)
+static void nvme_dev_scan(struct work_struct *work)
 {
-	return (!dev->bar || readl(&dev->bar->csts) & NVME_CSTS_CFS ||
-							dev->online_queues < 2);
+	struct nvme_dev *dev = container_of(work, struct nvme_dev, scan_work);
+
+	if (!dev->tagset.tags)
+		return;
+	nvme_scan_namespaces(&dev->ctrl);
+	nvme_set_irq_hints(dev);
 }
 
-static void nvme_ns_remove(struct nvme_ns *ns)
+static void nvme_del_queue_end(struct request *req, int error)
 {
-	bool kill = nvme_io_incapable(ns->dev) && !blk_queue_dying(ns->queue);
-
-	if (kill) {
-		blk_set_queue_dying(ns->queue);
+	struct nvme_queue *nvmeq = req->end_io_data;
 
-		/*
-		 * The controller was shutdown first if we got here through
-		 * device removal. The shutdown may requeue outstanding
-		 * requests. These need to be aborted immediately so
-		 * del_gendisk doesn't block indefinitely for their completion.
-		 */
-		blk_mq_abort_requeue_list(ns->queue);
-	}
-	if (ns->disk->flags & GENHD_FL_UP)
-		del_gendisk(ns->disk);
-	if (kill || !blk_queue_dying(ns->queue)) {
-		blk_mq_abort_requeue_list(ns->queue);
-		blk_cleanup_queue(ns->queue);
-	}
-	list_del_init(&ns->list);
-	kref_put(&ns->kref, nvme_free_ns);
+	blk_mq_free_request(req);
+	complete(&nvmeq->dev->ioq_wait);
 }
 
-static void nvme_scan_namespaces(struct nvme_dev *dev, unsigned nn)
+static void nvme_del_cq_end(struct request *req, int error)
 {
-	struct nvme_ns *ns, *next;
-	unsigned i;
+	struct nvme_queue *nvmeq = req->end_io_data;
 
-	for (i = 1; i <= nn; i++) {
-		ns = nvme_find_ns(dev, i);
-		if (ns) {
-			if (revalidate_disk(ns->disk))
-				nvme_ns_remove(ns);
-		} else
-			nvme_alloc_ns(dev, i);
-	}
-	list_for_each_entry_safe(ns, next, &dev->namespaces, list) {
-		if (ns->ns_id > nn)
-			nvme_ns_remove(ns);
+	if (!error) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&nvmeq->q_lock, flags);
+		nvme_process_cq(nvmeq);
+		spin_unlock_irqrestore(&nvmeq->q_lock, flags);
 	}
-	list_sort(NULL, &dev->namespaces, ns_cmp);
+
+	nvme_del_queue_end(req, error);
 }
 
-static void nvme_set_irq_hints(struct nvme_dev *dev)
+static int nvme_delete_queue(struct nvme_queue *nvmeq, u8 opcode)
 {
-	struct nvme_queue *nvmeq;
-	int i;
+	struct request_queue *q = nvmeq->dev->ctrl.admin_q;
+	struct request *req;
+	struct nvme_command cmd;
 
-	for (i = 0; i < dev->online_queues; i++) {
-		nvmeq = dev->queues[i];
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.delete_queue.opcode = opcode;
+	cmd.delete_queue.qid = cpu_to_le16(nvmeq->qid);
 
-		if (!nvmeq->tags || !(*nvmeq->tags))
-			continue;
+	req = nvme_alloc_request(q, &cmd, BLK_MQ_REQ_NOWAIT);
+	if (IS_ERR(req))
+		return PTR_ERR(req);
 
-		irq_set_affinity_hint(dev->entry[nvmeq->cq_vector].vector,
-					blk_mq_tags_cpumask(*nvmeq->tags));
-	}
+	req->timeout = ADMIN_TIMEOUT;
+	req->end_io_data = nvmeq;
+
+	blk_execute_rq_nowait(q, NULL, req, false,
+			opcode == nvme_admin_delete_cq ?
+				nvme_del_cq_end : nvme_del_queue_end);
+	return 0;
 }
 
-static void nvme_dev_scan(struct work_struct *work)
+static void nvme_disable_io_queues(struct nvme_dev *dev)
 {
-	struct nvme_dev *dev = container_of(work, struct nvme_dev, scan_work);
-	struct nvme_id_ctrl *ctrl;
+	int pass;
+	unsigned long timeout;
+	u8 opcode = nvme_admin_delete_sq;
 
-	if (!dev->tagset.tags)
-		return;
-	if (nvme_identify_ctrl(dev, &ctrl))
-		return;
-	nvme_scan_namespaces(dev, le32_to_cpup(&ctrl->nn));
-	kfree(ctrl);
-	nvme_set_irq_hints(dev);
+	for (pass = 0; pass < 2; pass++) {
+		int sent = 0, i = dev->queue_count - 1;
+
+		reinit_completion(&dev->ioq_wait);
+ retry:
+		timeout = ADMIN_TIMEOUT;
+		for (; i > 0; i--) {
+			struct nvme_queue *nvmeq = dev->queues[i];
+
+			if (!pass)
+				nvme_suspend_queue(nvmeq);
+			if (nvme_delete_queue(nvmeq, opcode))
+				break;
+			++sent;
+		}
+		while (sent--) {
+			timeout = wait_for_completion_io_timeout(&dev->ioq_wait, timeout);
+			if (timeout == 0)
+				return;
+			if (i)
+				goto retry;
+		}
+		opcode = nvme_admin_delete_cq;
+	}
 }
 
 /*
@@ -2619,42 +1695,7 @@ static void nvme_dev_scan(struct work_struct *work)
  */
 static int nvme_dev_add(struct nvme_dev *dev)
 {
-	struct pci_dev *pdev = to_pci_dev(dev->dev);
-	int res;
-	struct nvme_id_ctrl *ctrl;
-	int shift = NVME_CAP_MPSMIN(lo_hi_readq(&dev->bar->cap)) + 12;
-
-	res = nvme_identify_ctrl(dev, &ctrl);
-	if (res) {
-		dev_err(dev->dev, "Identify Controller failed (%d)\n", res);
-		return -EIO;
-	}
-
-	dev->oncs = le16_to_cpup(&ctrl->oncs);
-	dev->abort_limit = ctrl->acl + 1;
-	dev->vwc = ctrl->vwc;
-	memcpy(dev->serial, ctrl->sn, sizeof(ctrl->sn));
-	memcpy(dev->model, ctrl->mn, sizeof(ctrl->mn));
-	memcpy(dev->firmware_rev, ctrl->fr, sizeof(ctrl->fr));
-	if (ctrl->mdts)
-		dev->max_hw_sectors = 1 << (ctrl->mdts + shift - 9);
-	else
-		dev->max_hw_sectors = UINT_MAX;
-	if ((pdev->vendor == PCI_VENDOR_ID_INTEL) &&
-			(pdev->device == 0x0953) && ctrl->vs[3]) {
-		unsigned int max_hw_sectors;
-
-		dev->stripe_size = 1 << (ctrl->vs[3] + shift);
-		max_hw_sectors = dev->stripe_size >> (shift - 9);
-		if (dev->max_hw_sectors) {
-			dev->max_hw_sectors = min(max_hw_sectors,
-							dev->max_hw_sectors);
-		} else
-			dev->max_hw_sectors = max_hw_sectors;
-	}
-	kfree(ctrl);
-
-	if (!dev->tagset.tags) {
+	if (!dev->ctrl.tagset) {
 		dev->tagset.ops = &nvme_mq_ops;
 		dev->tagset.nr_hw_queues = dev->online_queues - 1;
 		dev->tagset.timeout = NVME_IO_TIMEOUT;
@@ -2667,15 +1708,16 @@ static int nvme_dev_add(struct nvme_dev *dev)
 
 		if (blk_mq_alloc_tag_set(&dev->tagset))
 			return 0;
+		dev->ctrl.tagset = &dev->tagset;
 	}
-	schedule_work(&dev->scan_work);
+	nvme_queue_scan(dev);
 	return 0;
 }
 
-static int nvme_dev_map(struct nvme_dev *dev)
+static int nvme_pci_enable(struct nvme_dev *dev)
 {
 	u64 cap;
-	int bars, result = -ENOMEM;
+	int result = -ENOMEM;
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
 	if (pci_enable_device_mem(pdev))
@@ -2683,24 +1725,14 @@ static int nvme_dev_map(struct nvme_dev *dev)
 
 	dev->entry[0].vector = pdev->irq;
 	pci_set_master(pdev);
-	bars = pci_select_bars(pdev, IORESOURCE_MEM);
-	if (!bars)
-		goto disable_pci;
-
-	if (pci_request_selected_regions(pdev, bars, "nvme"))
-		goto disable_pci;
 
 	if (dma_set_mask_and_coherent(dev->dev, DMA_BIT_MASK(64)) &&
 	    dma_set_mask_and_coherent(dev->dev, DMA_BIT_MASK(32)))
 		goto disable;
 
-	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
-	if (!dev->bar)
-		goto disable;
-
-	if (readl(&dev->bar->csts) == -1) {
+	if (readl(dev->bar + NVME_REG_CSTS) == -1) {
 		result = -ENODEV;
-		goto unmap;
+		goto disable;
 	}
 
 	/*
@@ -2710,13 +1742,14 @@ static int nvme_dev_map(struct nvme_dev *dev)
 	if (!pdev->irq) {
 		result = pci_enable_msix(pdev, dev->entry, 1);
 		if (result < 0)
-			goto unmap;
+			goto disable;
 	}
 
-	cap = lo_hi_readq(&dev->bar->cap);
+	cap = lo_hi_readq(dev->bar + NVME_REG_CAP);
+
 	dev->q_depth = min_t(int, NVME_CAP_MQES(cap) + 1, NVME_Q_DEPTH);
 	dev->db_stride = 1 << NVME_CAP_STRIDE(cap);
-	dev->dbs = ((void __iomem *)dev->bar) + 4096;
+	dev->dbs = dev->bar + 4096;
 
 	/*
 	 * Temporary fix for the Apple controller found in the MacBook8,1 and
@@ -2729,23 +1762,27 @@ static int nvme_dev_map(struct nvme_dev *dev)
 			dev->q_depth);
 	}
 
-	if (readl(&dev->bar->vs) >= NVME_VS(1, 2))
+	if (readl(dev->bar + NVME_REG_VS) >= NVME_VS(1, 2))
 		dev->cmb = nvme_map_cmb(dev);
 
+	pci_enable_pcie_error_reporting(pdev);
+	pci_save_state(pdev);
 	return 0;
 
- unmap:
-	iounmap(dev->bar);
-	dev->bar = NULL;
  disable:
-	pci_release_regions(pdev);
- disable_pci:
 	pci_disable_device(pdev);
 	return result;
 }
 
 static void nvme_dev_unmap(struct nvme_dev *dev)
 {
+	if (dev->bar)
+		iounmap(dev->bar);
+	pci_release_regions(to_pci_dev(dev->dev));
+}
+
+static void nvme_pci_disable(struct nvme_dev *dev)
+{
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
 	if (pdev->msi_enabled)
@@ -2753,158 +1790,34 @@ static void nvme_dev_unmap(struct nvme_dev *dev)
 	else if (pdev->msix_enabled)
 		pci_disable_msix(pdev);
 
-	if (dev->bar) {
-		iounmap(dev->bar);
-		dev->bar = NULL;
-		pci_release_regions(pdev);
-	}
-
-	if (pci_is_enabled(pdev))
+	if (pci_is_enabled(pdev)) {
+		pci_disable_pcie_error_reporting(pdev);
 		pci_disable_device(pdev);
-}
-
-struct nvme_delq_ctx {
-	struct task_struct *waiter;
-	struct kthread_worker *worker;
-	atomic_t refcount;
-};
-
-static void nvme_wait_dq(struct nvme_delq_ctx *dq, struct nvme_dev *dev)
-{
-	dq->waiter = current;
-	mb();
-
-	for (;;) {
-		set_current_state(TASK_KILLABLE);
-		if (!atomic_read(&dq->refcount))
-			break;
-		if (!schedule_timeout(ADMIN_TIMEOUT) ||
-					fatal_signal_pending(current)) {
-			/*
-			 * Disable the controller first since we can't trust it
-			 * at this point, but leave the admin queue enabled
-			 * until all queue deletion requests are flushed.
-			 * FIXME: This may take a while if there are more h/w
-			 * queues than admin tags.
-			 */
-			set_current_state(TASK_RUNNING);
-			nvme_disable_ctrl(dev, lo_hi_readq(&dev->bar->cap));
-			nvme_clear_queue(dev->queues[0]);
-			flush_kthread_worker(dq->worker);
-			nvme_disable_queue(dev, 0);
-			return;
-		}
 	}
-	set_current_state(TASK_RUNNING);
-}
-
-static void nvme_put_dq(struct nvme_delq_ctx *dq)
-{
-	atomic_dec(&dq->refcount);
-	if (dq->waiter)
-		wake_up_process(dq->waiter);
-}
-
-static struct nvme_delq_ctx *nvme_get_dq(struct nvme_delq_ctx *dq)
-{
-	atomic_inc(&dq->refcount);
-	return dq;
-}
-
-static void nvme_del_queue_end(struct nvme_queue *nvmeq)
-{
-	struct nvme_delq_ctx *dq = nvmeq->cmdinfo.ctx;
-	nvme_put_dq(dq);
-
-	spin_lock_irq(&nvmeq->q_lock);
-	nvme_process_cq(nvmeq);
-	spin_unlock_irq(&nvmeq->q_lock);
-}
-
-static int adapter_async_del_queue(struct nvme_queue *nvmeq, u8 opcode,
-						kthread_work_func_t fn)
-{
-	struct nvme_command c;
-
-	memset(&c, 0, sizeof(c));
-	c.delete_queue.opcode = opcode;
-	c.delete_queue.qid = cpu_to_le16(nvmeq->qid);
-
-	init_kthread_work(&nvmeq->cmdinfo.work, fn);
-	return nvme_submit_admin_async_cmd(nvmeq->dev, &c, &nvmeq->cmdinfo,
-								ADMIN_TIMEOUT);
 }
 
-static void nvme_del_cq_work_handler(struct kthread_work *work)
+static int nvme_dev_list_add(struct nvme_dev *dev)
 {
-	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
-							cmdinfo.work);
-	nvme_del_queue_end(nvmeq);
-}
-
-static int nvme_delete_cq(struct nvme_queue *nvmeq)
-{
-	return adapter_async_del_queue(nvmeq, nvme_admin_delete_cq,
-						nvme_del_cq_work_handler);
-}
-
-static void nvme_del_sq_work_handler(struct kthread_work *work)
-{
-	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
-							cmdinfo.work);
-	int status = nvmeq->cmdinfo.status;
-
-	if (!status)
-		status = nvme_delete_cq(nvmeq);
-	if (status)
-		nvme_del_queue_end(nvmeq);
-}
-
-static int nvme_delete_sq(struct nvme_queue *nvmeq)
-{
-	return adapter_async_del_queue(nvmeq, nvme_admin_delete_sq,
-						nvme_del_sq_work_handler);
-}
-
-static void nvme_del_queue_start(struct kthread_work *work)
-{
-	struct nvme_queue *nvmeq = container_of(work, struct nvme_queue,
-							cmdinfo.work);
-	if (nvme_delete_sq(nvmeq))
-		nvme_del_queue_end(nvmeq);
-}
+	bool start_thread = false;
 
-static void nvme_disable_io_queues(struct nvme_dev *dev)
-{
-	int i;
-	DEFINE_KTHREAD_WORKER_ONSTACK(worker);
-	struct nvme_delq_ctx dq;
-	struct task_struct *kworker_task = kthread_run(kthread_worker_fn,
-					&worker, "nvme%d", dev->instance);
-
-	if (IS_ERR(kworker_task)) {
-		dev_err(dev->dev,
-			"Failed to create queue del task\n");
-		for (i = dev->queue_count - 1; i > 0; i--)
-			nvme_disable_queue(dev, i);
-		return;
+	spin_lock(&dev_list_lock);
+	if (list_empty(&dev_list) && IS_ERR_OR_NULL(nvme_thread)) {
+		start_thread = true;
+		nvme_thread = NULL;
 	}
+	list_add(&dev->node, &dev_list);
+	spin_unlock(&dev_list_lock);
 
-	dq.waiter = NULL;
-	atomic_set(&dq.refcount, 0);
-	dq.worker = &worker;
-	for (i = dev->queue_count - 1; i > 0; i--) {
-		struct nvme_queue *nvmeq = dev->queues[i];
+	if (start_thread) {
+		nvme_thread = kthread_run(nvme_kthread, NULL, "nvme");
+		wake_up_all(&nvme_kthread_wait);
+	} else
+		wait_event_killable(nvme_kthread_wait, nvme_thread);
 
-		if (nvme_suspend_queue(nvmeq))
-			continue;
-		nvmeq->cmdinfo.ctx = nvme_get_dq(&dq);
-		nvmeq->cmdinfo.worker = dq.worker;
-		init_kthread_work(&nvmeq->cmdinfo.work, nvme_del_queue_start);
-		queue_kthread_work(dq.worker, &nvmeq->cmdinfo.work);
-	}
-	nvme_wait_dq(&dq, dev);
-	kthread_stop(kworker_task);
+	if (IS_ERR_OR_NULL(nvme_thread))
+		return nvme_thread ? PTR_ERR(nvme_thread) : -EINTR;
+
+	return 0;
 }
 
 /*
@@ -2927,44 +1840,17 @@ static void nvme_dev_list_remove(struct nvme_dev *dev)
 		kthread_stop(tmp);
 }
 
-static void nvme_freeze_queues(struct nvme_dev *dev)
-{
-	struct nvme_ns *ns;
-
-	list_for_each_entry(ns, &dev->namespaces, list) {
-		blk_mq_freeze_queue_start(ns->queue);
-
-		spin_lock_irq(ns->queue->queue_lock);
-		queue_flag_set(QUEUE_FLAG_STOPPED, ns->queue);
-		spin_unlock_irq(ns->queue->queue_lock);
-
-		blk_mq_cancel_requeue_work(ns->queue);
-		blk_mq_stop_hw_queues(ns->queue);
-	}
-}
-
-static void nvme_unfreeze_queues(struct nvme_dev *dev)
-{
-	struct nvme_ns *ns;
-
-	list_for_each_entry(ns, &dev->namespaces, list) {
-		queue_flag_clear_unlocked(QUEUE_FLAG_STOPPED, ns->queue);
-		blk_mq_unfreeze_queue(ns->queue);
-		blk_mq_start_stopped_hw_queues(ns->queue, true);
-		blk_mq_kick_requeue_list(ns->queue);
-	}
-}
-
-static void nvme_dev_shutdown(struct nvme_dev *dev)
+static void nvme_dev_disable(struct nvme_dev *dev, bool shutdown)
 {
 	int i;
 	u32 csts = -1;
 
 	nvme_dev_list_remove(dev);
 
-	if (dev->bar) {
-		nvme_freeze_queues(dev);
-		csts = readl(&dev->bar->csts);
+	mutex_lock(&dev->shutdown_lock);
+	if (pci_is_enabled(to_pci_dev(dev->dev))) {
+		nvme_stop_queues(&dev->ctrl);
+		csts = readl(dev->bar + NVME_REG_CSTS);
 	}
 	if (csts & NVME_CSTS_CFS || !(csts & NVME_CSTS_RDY)) {
 		for (i = dev->queue_count - 1; i >= 0; i--) {
@@ -2973,30 +1859,13 @@ static void nvme_dev_shutdown(struct nvme_dev *dev)
 		}
 	} else {
 		nvme_disable_io_queues(dev);
-		nvme_shutdown_ctrl(dev);
-		nvme_disable_queue(dev, 0);
+		nvme_disable_admin_queue(dev, shutdown);
 	}
-	nvme_dev_unmap(dev);
+	nvme_pci_disable(dev);
 
 	for (i = dev->queue_count - 1; i >= 0; i--)
 		nvme_clear_queue(dev->queues[i]);
-}
-
-static void nvme_dev_remove(struct nvme_dev *dev)
-{
-	struct nvme_ns *ns, *next;
-
-	if (nvme_io_incapable(dev)) {
-		/*
-		 * If the device is not capable of IO (surprise hot-removal,
-		 * for example), we need to quiesce prior to deleting the
-		 * namespaces. This will end outstanding requests and prevent
-		 * attempts to sync dirty data.
-		 */
-		nvme_dev_shutdown(dev);
-	}
-	list_for_each_entry_safe(ns, next, &dev->namespaces, list)
-		nvme_ns_remove(ns);
+	mutex_unlock(&dev->shutdown_lock);
 }
 
 static int nvme_setup_prp_pools(struct nvme_dev *dev)
@@ -3022,157 +1891,73 @@ static void nvme_release_prp_pools(struct nvme_dev *dev)
 	dma_pool_destroy(dev->prp_small_pool);
 }
 
-static DEFINE_IDA(nvme_instance_ida);
-
-static int nvme_set_instance(struct nvme_dev *dev)
+static void nvme_pci_free_ctrl(struct nvme_ctrl *ctrl)
 {
-	int instance, error;
-
-	do {
-		if (!ida_pre_get(&nvme_instance_ida, GFP_KERNEL))
-			return -ENODEV;
-
-		spin_lock(&dev_list_lock);
-		error = ida_get_new(&nvme_instance_ida, &instance);
-		spin_unlock(&dev_list_lock);
-	} while (error == -EAGAIN);
-
-	if (error)
-		return -ENODEV;
-
-	dev->instance = instance;
-	return 0;
-}
-
-static void nvme_release_instance(struct nvme_dev *dev)
-{
-	spin_lock(&dev_list_lock);
-	ida_remove(&nvme_instance_ida, dev->instance);
-	spin_unlock(&dev_list_lock);
-}
-
-static void nvme_free_dev(struct kref *kref)
-{
-	struct nvme_dev *dev = container_of(kref, struct nvme_dev, kref);
+	struct nvme_dev *dev = to_nvme_dev(ctrl);
 
 	put_device(dev->dev);
-	put_device(dev->device);
-	nvme_release_instance(dev);
 	if (dev->tagset.tags)
 		blk_mq_free_tag_set(&dev->tagset);
-	if (dev->admin_q)
-		blk_put_queue(dev->admin_q);
+	if (dev->ctrl.admin_q)
+		blk_put_queue(dev->ctrl.admin_q);
 	kfree(dev->queues);
 	kfree(dev->entry);
 	kfree(dev);
 }
 
-static int nvme_dev_open(struct inode *inode, struct file *f)
+static void nvme_remove_dead_ctrl(struct nvme_dev *dev, int status)
 {
-	struct nvme_dev *dev;
-	int instance = iminor(inode);
-	int ret = -ENODEV;
+	dev_warn(dev->dev, "Removing after probe failure status: %d\n", status);
 
-	spin_lock(&dev_list_lock);
-	list_for_each_entry(dev, &dev_list, node) {
-		if (dev->instance == instance) {
-			if (!dev->admin_q) {
-				ret = -EWOULDBLOCK;
-				break;
-			}
-			if (!kref_get_unless_zero(&dev->kref))
-				break;
-			f->private_data = dev;
-			ret = 0;
-			break;
-		}
-	}
-	spin_unlock(&dev_list_lock);
-
-	return ret;
+	kref_get(&dev->ctrl.kref);
+	nvme_dev_disable(dev, false);
+	if (!schedule_work(&dev->remove_work))
+		nvme_put_ctrl(&dev->ctrl);
 }
 
-static int nvme_dev_release(struct inode *inode, struct file *f)
+static void nvme_reset_work(struct work_struct *work)
 {
-	struct nvme_dev *dev = f->private_data;
-	kref_put(&dev->kref, nvme_free_dev);
-	return 0;
-}
+	struct nvme_dev *dev = container_of(work, struct nvme_dev, reset_work);
+	int result = -ENODEV;
 
-static long nvme_dev_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
-{
-	struct nvme_dev *dev = f->private_data;
-	struct nvme_ns *ns;
-
-	switch (cmd) {
-	case NVME_IOCTL_ADMIN_CMD:
-		return nvme_user_cmd(dev, NULL, (void __user *)arg);
-	case NVME_IOCTL_IO_CMD:
-		if (list_empty(&dev->namespaces))
-			return -ENOTTY;
-		ns = list_first_entry(&dev->namespaces, struct nvme_ns, list);
-		return nvme_user_cmd(dev, ns, (void __user *)arg);
-	case NVME_IOCTL_RESET:
-		dev_warn(dev->dev, "resetting controller\n");
-		return nvme_reset(dev);
-	case NVME_IOCTL_SUBSYS_RESET:
-		return nvme_subsys_reset(dev);
-	default:
-		return -ENOTTY;
-	}
-}
+	if (WARN_ON(test_bit(NVME_CTRL_RESETTING, &dev->flags)))
+		goto out;
 
-static const struct file_operations nvme_dev_fops = {
-	.owner		= THIS_MODULE,
-	.open		= nvme_dev_open,
-	.release	= nvme_dev_release,
-	.unlocked_ioctl	= nvme_dev_ioctl,
-	.compat_ioctl	= nvme_dev_ioctl,
-};
+	/*
+	 * If we're called to reset a live controller first shut it down before
+	 * moving on.
+	 */
+	if (dev->ctrl.ctrl_config & NVME_CC_ENABLE)
+		nvme_dev_disable(dev, false);
 
-static void nvme_probe_work(struct work_struct *work)
-{
-	struct nvme_dev *dev = container_of(work, struct nvme_dev, probe_work);
-	bool start_thread = false;
-	int result;
+	set_bit(NVME_CTRL_RESETTING, &dev->flags);
 
-	result = nvme_dev_map(dev);
+	result = nvme_pci_enable(dev);
 	if (result)
 		goto out;
 
 	result = nvme_configure_admin_queue(dev);
 	if (result)
-		goto unmap;
-
-	spin_lock(&dev_list_lock);
-	if (list_empty(&dev_list) && IS_ERR_OR_NULL(nvme_thread)) {
-		start_thread = true;
-		nvme_thread = NULL;
-	}
-	list_add(&dev->node, &dev_list);
-	spin_unlock(&dev_list_lock);
-
-	if (start_thread) {
-		nvme_thread = kthread_run(nvme_kthread, NULL, "nvme");
-		wake_up_all(&nvme_kthread_wait);
-	} else
-		wait_event_killable(nvme_kthread_wait, nvme_thread);
-
-	if (IS_ERR_OR_NULL(nvme_thread)) {
-		result = nvme_thread ? PTR_ERR(nvme_thread) : -EINTR;
-		goto disable;
-	}
+		goto out;
 
 	nvme_init_queue(dev->queues[0], 0);
 	result = nvme_alloc_admin_tags(dev);
 	if (result)
-		goto disable;
+		goto out;
+
+	result = nvme_init_identify(&dev->ctrl);
+	if (result)
+		goto out;
 
 	result = nvme_setup_io_queues(dev);
 	if (result)
-		goto free_tags;
+		goto out;
 
-	dev->event_limit = 1;
+	dev->ctrl.event_limit = NVME_NR_AEN_COMMANDS;
+
+	result = nvme_dev_list_add(dev);
+	if (result)
+		goto out;
 
 	/*
 	 * Keep the controller around but remove all namespaces if we don't have
@@ -3180,117 +1965,101 @@ static void nvme_probe_work(struct work_struct *work)
 	 */
 	if (dev->online_queues < 2) {
 		dev_warn(dev->dev, "IO queues not created\n");
-		nvme_dev_remove(dev);
+		nvme_remove_namespaces(&dev->ctrl);
 	} else {
-		nvme_unfreeze_queues(dev);
+		nvme_start_queues(&dev->ctrl);
 		nvme_dev_add(dev);
 	}
 
+	clear_bit(NVME_CTRL_RESETTING, &dev->flags);
 	return;
 
- free_tags:
-	nvme_dev_remove_admin(dev);
-	blk_put_queue(dev->admin_q);
-	dev->admin_q = NULL;
-	dev->queues[0]->tags = NULL;
- disable:
-	nvme_disable_queue(dev, 0);
-	nvme_dev_list_remove(dev);
- unmap:
-	nvme_dev_unmap(dev);
  out:
-	if (!work_busy(&dev->reset_work))
-		nvme_dead_ctrl(dev);
+	nvme_remove_dead_ctrl(dev, result);
 }
 
-static int nvme_remove_dead_ctrl(void *arg)
+static void nvme_remove_dead_ctrl_work(struct work_struct *work)
 {
-	struct nvme_dev *dev = (struct nvme_dev *)arg;
+	struct nvme_dev *dev = container_of(work, struct nvme_dev, remove_work);
 	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
+	nvme_kill_queues(&dev->ctrl);
 	if (pci_get_drvdata(pdev))
 		pci_stop_and_remove_bus_device_locked(pdev);
-	kref_put(&dev->kref, nvme_free_dev);
-	return 0;
-}
-
-static void nvme_dead_ctrl(struct nvme_dev *dev)
-{
-	dev_warn(dev->dev, "Device failed to resume\n");
-	kref_get(&dev->kref);
-	if (IS_ERR(kthread_run(nvme_remove_dead_ctrl, dev, "nvme%d",
-						dev->instance))) {
-		dev_err(dev->dev,
-			"Failed to start controller remove task\n");
-		kref_put(&dev->kref, nvme_free_dev);
-	}
+	nvme_put_ctrl(&dev->ctrl);
 }
 
-static void nvme_reset_work(struct work_struct *ws)
+static int nvme_reset(struct nvme_dev *dev)
 {
-	struct nvme_dev *dev = container_of(ws, struct nvme_dev, reset_work);
-	bool in_probe = work_busy(&dev->probe_work);
-
-	nvme_dev_shutdown(dev);
+	if (!dev->ctrl.admin_q || blk_queue_dying(dev->ctrl.admin_q))
+		return -ENODEV;
 
-	/* Synchronize with device probe so that work will see failure status
-	 * and exit gracefully without trying to schedule another reset */
-	flush_work(&dev->probe_work);
+	if (!queue_work(nvme_workq, &dev->reset_work))
+		return -EBUSY;
 
-	/* Fail this device if reset occured during probe to avoid
-	 * infinite initialization loops. */
-	if (in_probe) {
-		nvme_dead_ctrl(dev);
-		return;
-	}
-	/* Schedule device resume asynchronously so the reset work is available
-	 * to cleanup errors that may occur during reinitialization */
-	schedule_work(&dev->probe_work);
+	flush_work(&dev->reset_work);
+	return 0;
 }
 
-static int __nvme_reset(struct nvme_dev *dev)
+static int nvme_pci_reg_read32(struct nvme_ctrl *ctrl, u32 off, u32 *val)
 {
-	if (work_pending(&dev->reset_work))
-		return -EBUSY;
-	list_del_init(&dev->node);
-	queue_work(nvme_workq, &dev->reset_work);
+	*val = readl(to_nvme_dev(ctrl)->bar + off);
 	return 0;
 }
 
-static int nvme_reset(struct nvme_dev *dev)
+static int nvme_pci_reg_write32(struct nvme_ctrl *ctrl, u32 off, u32 val)
 {
-	int ret;
+	writel(val, to_nvme_dev(ctrl)->bar + off);
+	return 0;
+}
 
-	if (!dev->admin_q || blk_queue_dying(dev->admin_q))
-		return -ENODEV;
+static int nvme_pci_reg_read64(struct nvme_ctrl *ctrl, u32 off, u64 *val)
+{
+	*val = readq(to_nvme_dev(ctrl)->bar + off);
+	return 0;
+}
 
-	spin_lock(&dev_list_lock);
-	ret = __nvme_reset(dev);
-	spin_unlock(&dev_list_lock);
+static bool nvme_pci_io_incapable(struct nvme_ctrl *ctrl)
+{
+	struct nvme_dev *dev = to_nvme_dev(ctrl);
 
-	if (!ret) {
-		flush_work(&dev->reset_work);
-		flush_work(&dev->probe_work);
-		return 0;
-	}
+	return !dev->bar || dev->online_queues < 2;
+}
 
-	return ret;
+static int nvme_pci_reset_ctrl(struct nvme_ctrl *ctrl)
+{
+	return nvme_reset(to_nvme_dev(ctrl));
 }
 
-static ssize_t nvme_sysfs_reset(struct device *dev,
-				struct device_attribute *attr, const char *buf,
-				size_t count)
+static const struct nvme_ctrl_ops nvme_pci_ctrl_ops = {
+	.reg_read32		= nvme_pci_reg_read32,
+	.reg_write32		= nvme_pci_reg_write32,
+	.reg_read64		= nvme_pci_reg_read64,
+	.io_incapable		= nvme_pci_io_incapable,
+	.reset_ctrl		= nvme_pci_reset_ctrl,
+	.free_ctrl		= nvme_pci_free_ctrl,
+};
+
+static int nvme_dev_map(struct nvme_dev *dev)
 {
-	struct nvme_dev *ndev = dev_get_drvdata(dev);
-	int ret;
+	int bars;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
 
-	ret = nvme_reset(ndev);
-	if (ret < 0)
-		return ret;
+	bars = pci_select_bars(pdev, IORESOURCE_MEM);
+	if (!bars)
+		return -ENODEV;
+	if (pci_request_selected_regions(pdev, bars, "nvme"))
+		return -ENODEV;
+
+	dev->bar = ioremap(pci_resource_start(pdev, 0), 8192);
+	if (!dev->bar)
+		goto release;
 
-	return count;
+       return 0;
+  release:
+       pci_release_regions(pdev);
+       return -ENODEV;
 }
-static DEVICE_ATTR(reset_controller, S_IWUSR, NULL, nvme_sysfs_reset);
 
 static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
@@ -3313,48 +2082,37 @@ static int nvme_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!dev->queues)
 		goto free;
 
-	INIT_LIST_HEAD(&dev->namespaces);
-	INIT_WORK(&dev->reset_work, nvme_reset_work);
 	dev->dev = get_device(&pdev->dev);
 	pci_set_drvdata(pdev, dev);
-	result = nvme_set_instance(dev);
+
+	result = nvme_dev_map(dev);
 	if (result)
-		goto put_pci;
+		goto free;
+
+	INIT_LIST_HEAD(&dev->node);
+	INIT_WORK(&dev->scan_work, nvme_dev_scan);
+	INIT_WORK(&dev->reset_work, nvme_reset_work);
+	INIT_WORK(&dev->remove_work, nvme_remove_dead_ctrl_work);
+	mutex_init(&dev->shutdown_lock);
+	init_completion(&dev->ioq_wait);
 
 	result = nvme_setup_prp_pools(dev);
 	if (result)
-		goto release;
-
-	kref_init(&dev->kref);
-	dev->device = device_create(nvme_class, &pdev->dev,
-				MKDEV(nvme_char_major, dev->instance),
-				dev, "nvme%d", dev->instance);
-	if (IS_ERR(dev->device)) {
-		result = PTR_ERR(dev->device);
-		goto release_pools;
-	}
-	get_device(dev->device);
-	dev_set_drvdata(dev->device, dev);
+		goto put_pci;
 
-	result = device_create_file(dev->device, &dev_attr_reset_controller);
+	result = nvme_init_ctrl(&dev->ctrl, &pdev->dev, &nvme_pci_ctrl_ops,
+			id->driver_data);
 	if (result)
-		goto put_dev;
+		goto release_pools;
 
-	INIT_LIST_HEAD(&dev->node);
-	INIT_WORK(&dev->scan_work, nvme_dev_scan);
-	INIT_WORK(&dev->probe_work, nvme_probe_work);
-	schedule_work(&dev->probe_work);
+	queue_work(nvme_workq, &dev->reset_work);
 	return 0;
 
- put_dev:
-	device_destroy(nvme_class, MKDEV(nvme_char_major, dev->instance));
-	put_device(dev->device);
  release_pools:
 	nvme_release_prp_pools(dev);
- release:
-	nvme_release_instance(dev);
  put_pci:
 	put_device(dev->dev);
+	nvme_dev_unmap(dev);
  free:
 	kfree(dev->queues);
 	kfree(dev->entry);
@@ -3367,54 +2125,48 @@ static void nvme_reset_notify(struct pci_dev *pdev, bool prepare)
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
 
 	if (prepare)
-		nvme_dev_shutdown(dev);
+		nvme_dev_disable(dev, false);
 	else
-		schedule_work(&dev->probe_work);
+		queue_work(nvme_workq, &dev->reset_work);
 }
 
 static void nvme_shutdown(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
-	nvme_dev_shutdown(dev);
+	nvme_dev_disable(dev, true);
 }
 
+/*
+ * The driver's remove may be called on a device in a partially initialized
+ * state. This function must not have any dependencies on the device state in
+ * order to proceed.
+ */
 static void nvme_remove(struct pci_dev *pdev)
 {
 	struct nvme_dev *dev = pci_get_drvdata(pdev);
 
-	spin_lock(&dev_list_lock);
-	list_del_init(&dev->node);
-	spin_unlock(&dev_list_lock);
-
+	set_bit(NVME_CTRL_REMOVING, &dev->flags);
 	pci_set_drvdata(pdev, NULL);
-	flush_work(&dev->probe_work);
-	flush_work(&dev->reset_work);
 	flush_work(&dev->scan_work);
-	device_remove_file(dev->device, &dev_attr_reset_controller);
-	nvme_dev_remove(dev);
-	nvme_dev_shutdown(dev);
+	nvme_remove_namespaces(&dev->ctrl);
+	nvme_uninit_ctrl(&dev->ctrl);
+	nvme_dev_disable(dev, true);
+	flush_work(&dev->reset_work);
 	nvme_dev_remove_admin(dev);
-	device_destroy(nvme_class, MKDEV(nvme_char_major, dev->instance));
 	nvme_free_queues(dev, 0);
 	nvme_release_cmb(dev);
 	nvme_release_prp_pools(dev);
-	kref_put(&dev->kref, nvme_free_dev);
+	nvme_dev_unmap(dev);
+	nvme_put_ctrl(&dev->ctrl);
 }
 
-/* These functions are yet to be implemented */
-#define nvme_error_detected NULL
-#define nvme_dump_registers NULL
-#define nvme_link_reset NULL
-#define nvme_slot_reset NULL
-#define nvme_error_resume NULL
-
 #ifdef CONFIG_PM_SLEEP
 static int nvme_suspend(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct nvme_dev *ndev = pci_get_drvdata(pdev);
 
-	nvme_dev_shutdown(ndev);
+	nvme_dev_disable(ndev, true);
 	return 0;
 }
 
@@ -3423,17 +2175,53 @@ static int nvme_resume(struct device *dev)
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct nvme_dev *ndev = pci_get_drvdata(pdev);
 
-	schedule_work(&ndev->probe_work);
+	queue_work(nvme_workq, &ndev->reset_work);
 	return 0;
 }
 #endif
 
 static SIMPLE_DEV_PM_OPS(nvme_dev_pm_ops, nvme_suspend, nvme_resume);
 
+static pci_ers_result_t nvme_error_detected(struct pci_dev *pdev,
+						pci_channel_state_t state)
+{
+	struct nvme_dev *dev = pci_get_drvdata(pdev);
+
+	/*
+	 * A frozen channel requires a reset. When detected, this method will
+	 * shutdown the controller to quiesce. The controller will be restarted
+	 * after the slot reset through driver's slot_reset callback.
+	 */
+	dev_warn(&pdev->dev, "error detected: state:%d\n", state);
+	switch (state) {
+	case pci_channel_io_normal:
+		return PCI_ERS_RESULT_CAN_RECOVER;
+	case pci_channel_io_frozen:
+		nvme_dev_disable(dev, false);
+		return PCI_ERS_RESULT_NEED_RESET;
+	case pci_channel_io_perm_failure:
+		return PCI_ERS_RESULT_DISCONNECT;
+	}
+	return PCI_ERS_RESULT_NEED_RESET;
+}
+
+static pci_ers_result_t nvme_slot_reset(struct pci_dev *pdev)
+{
+	struct nvme_dev *dev = pci_get_drvdata(pdev);
+
+	dev_info(&pdev->dev, "restart after slot reset\n");
+	pci_restore_state(pdev);
+	queue_work(nvme_workq, &dev->reset_work);
+	return PCI_ERS_RESULT_RECOVERED;
+}
+
+static void nvme_error_resume(struct pci_dev *pdev)
+{
+	pci_cleanup_aer_uncorrect_error_status(pdev);
+}
+
 static const struct pci_error_handlers nvme_err_handler = {
 	.error_detected	= nvme_error_detected,
-	.mmio_enabled	= nvme_dump_registers,
-	.link_reset	= nvme_link_reset,
 	.slot_reset	= nvme_slot_reset,
 	.resume		= nvme_error_resume,
 	.reset_notify	= nvme_reset_notify,
@@ -3443,6 +2231,10 @@ static const struct pci_error_handlers nvme_err_handler = {
 #define PCI_CLASS_STORAGE_EXPRESS	0x010802
 
 static const struct pci_device_id nvme_id_table[] = {
+	{ PCI_VDEVICE(INTEL, 0x0953),
+		.driver_data = NVME_QUIRK_STRIPE_SIZE, },
+	{ PCI_VDEVICE(INTEL, 0x5845),	/* Qemu emulated controller */
+		.driver_data = NVME_QUIRK_IDENTIFY_CNS, },
 	{ PCI_DEVICE_CLASS(PCI_CLASS_STORAGE_EXPRESS, 0xffffff) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_APPLE, 0x2001) },
 	{ 0, }
@@ -3467,40 +2259,21 @@ static int __init nvme_init(void)
 
 	init_waitqueue_head(&nvme_kthread_wait);
 
-	nvme_workq = create_singlethread_workqueue("nvme");
+	nvme_workq = alloc_workqueue("nvme", WQ_UNBOUND | WQ_MEM_RECLAIM, 0);
 	if (!nvme_workq)
 		return -ENOMEM;
 
-	result = register_blkdev(nvme_major, "nvme");
+	result = nvme_core_init();
 	if (result < 0)
 		goto kill_workq;
-	else if (result > 0)
-		nvme_major = result;
-
-	result = __register_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme",
-							&nvme_dev_fops);
-	if (result < 0)
-		goto unregister_blkdev;
-	else if (result > 0)
-		nvme_char_major = result;
-
-	nvme_class = class_create(THIS_MODULE, "nvme");
-	if (IS_ERR(nvme_class)) {
-		result = PTR_ERR(nvme_class);
-		goto unregister_chrdev;
-	}
 
 	result = pci_register_driver(&nvme_driver);
 	if (result)
-		goto destroy_class;
+		goto core_exit;
 	return 0;
 
- destroy_class:
-	class_destroy(nvme_class);
- unregister_chrdev:
-	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
- unregister_blkdev:
-	unregister_blkdev(nvme_major, "nvme");
+ core_exit:
+	nvme_core_exit();
  kill_workq:
 	destroy_workqueue(nvme_workq);
 	return result;
@@ -3509,10 +2282,8 @@ static int __init nvme_init(void)
 static void __exit nvme_exit(void)
 {
 	pci_unregister_driver(&nvme_driver);
-	unregister_blkdev(nvme_major, "nvme");
+	nvme_core_exit();
 	destroy_workqueue(nvme_workq);
-	class_destroy(nvme_class);
-	__unregister_chrdev(nvme_char_major, 0, NVME_MINORS, "nvme");
 	BUG_ON(nvme_thread && !IS_ERR(nvme_thread));
 	_nvme_check_size();
 }
diff --git a/drivers/nvme/host/scsi.c b/drivers/nvme/host/scsi.c
index c3d8d3887a31..e947e298a737 100644
--- a/drivers/nvme/host/scsi.c
+++ b/drivers/nvme/host/scsi.c
@@ -524,7 +524,7 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *inq_response,
 					int alloc_len)
 {
-	struct nvme_dev *dev = ns->dev;
+	struct nvme_ctrl *ctrl = ns->ctrl;
 	struct nvme_id_ns *id_ns;
 	int res;
 	int nvme_sc;
@@ -532,10 +532,10 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 	u8 resp_data_format = 0x02;
 	u8 protect;
 	u8 cmdque = 0x01 << 1;
-	u8 fw_offset = sizeof(dev->firmware_rev);
+	u8 fw_offset = sizeof(ctrl->firmware_rev);
 
 	/* nvme ns identify - use DPS value for PROTECT field */
-	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+	nvme_sc = nvme_identify_ns(ctrl, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		return res;
@@ -553,12 +553,12 @@ static int nvme_trans_standard_inquiry_page(struct nvme_ns *ns,
 	inq_response[5] = protect;	/* sccs=0 | acc=0 | tpgs=0 | pc3=0 */
 	inq_response[7] = cmdque;	/* wbus16=0 | sync=0 | vs=0 */
 	strncpy(&inq_response[8], "NVMe    ", 8);
-	strncpy(&inq_response[16], dev->model, 16);
+	strncpy(&inq_response[16], ctrl->model, 16);
 
-	while (dev->firmware_rev[fw_offset - 1] == ' ' && fw_offset > 4)
+	while (ctrl->firmware_rev[fw_offset - 1] == ' ' && fw_offset > 4)
 		fw_offset--;
 	fw_offset -= 4;
-	strncpy(&inq_response[32], dev->firmware_rev + fw_offset, 4);
+	strncpy(&inq_response[32], ctrl->firmware_rev + fw_offset, 4);
 
 	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
 	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
@@ -588,82 +588,113 @@ static int nvme_trans_unit_serial_page(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr, u8 *inq_response,
 					int alloc_len)
 {
-	struct nvme_dev *dev = ns->dev;
 	int xfer_len;
 
 	memset(inq_response, 0, STANDARD_INQUIRY_LENGTH);
 	inq_response[1] = INQ_UNIT_SERIAL_NUMBER_PAGE; /* Page Code */
 	inq_response[3] = INQ_SERIAL_NUMBER_LENGTH;    /* Page Length */
-	strncpy(&inq_response[4], dev->serial, INQ_SERIAL_NUMBER_LENGTH);
+	strncpy(&inq_response[4], ns->ctrl->serial, INQ_SERIAL_NUMBER_LENGTH);
 
 	xfer_len = min(alloc_len, STANDARD_INQUIRY_LENGTH);
 	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
 }
 
-static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
-					u8 *inq_response, int alloc_len)
+static int nvme_fill_device_id_eui64(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+		u8 *inq_response, int alloc_len)
 {
-	struct nvme_dev *dev = ns->dev;
-	int res;
-	int nvme_sc;
-	int xfer_len;
-	__be32 tmp_id = cpu_to_be32(ns->ns_id);
+	struct nvme_id_ns *id_ns;
+	int nvme_sc, res;
+	size_t len;
+	void *eui;
 
-	memset(inq_response, 0, alloc_len);
-	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;    /* Page Code */
-	if (readl(&dev->bar->vs) >= NVME_VS(1, 1)) {
-		struct nvme_id_ns *id_ns;
-		void *eui;
-		int len;
+	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		return res;
 
-		nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
-		res = nvme_trans_status_code(hdr, nvme_sc);
-		if (res)
-			return res;
+	eui = id_ns->eui64;
+	len = sizeof(id_ns->eui64);
 
-		eui = id_ns->eui64;
-		len = sizeof(id_ns->eui64);
-		if (readl(&dev->bar->vs) >= NVME_VS(1, 2)) {
-			if (bitmap_empty(eui, len * 8)) {
-				eui = id_ns->nguid;
-				len = sizeof(id_ns->nguid);
-			}
-		}
+	if (ns->ctrl->vs >= NVME_VS(1, 2)) {
 		if (bitmap_empty(eui, len * 8)) {
-			kfree(id_ns);
-			goto scsi_string;
+			eui = id_ns->nguid;
+			len = sizeof(id_ns->nguid);
 		}
+	}
 
-		inq_response[3] = 4 + len; /* Page Length */
-		/* Designation Descriptor start */
-		inq_response[4] = 0x01;    /* Proto ID=0h | Code set=1h */
-		inq_response[5] = 0x02;    /* PIV=0b | Asso=00b | Designator Type=2h */
-		inq_response[6] = 0x00;    /* Rsvd */
-		inq_response[7] = len;     /* Designator Length */
-		memcpy(&inq_response[8], eui, len);
-		kfree(id_ns);
-	} else {
- scsi_string:
-		if (alloc_len < 72) {
-			return nvme_trans_completion(hdr,
-					SAM_STAT_CHECK_CONDITION,
-					ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
-					SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
-		}
-		inq_response[3] = 0x48;    /* Page Length */
-		/* Designation Descriptor start */
-		inq_response[4] = 0x03;    /* Proto ID=0h | Code set=3h */
-		inq_response[5] = 0x08;    /* PIV=0b | Asso=00b | Designator Type=8h */
-		inq_response[6] = 0x00;    /* Rsvd */
-		inq_response[7] = 0x44;    /* Designator Length */
-
-		sprintf(&inq_response[8], "%04x", to_pci_dev(dev->dev)->vendor);
-		memcpy(&inq_response[12], dev->model, sizeof(dev->model));
-		sprintf(&inq_response[52], "%04x", tmp_id);
-		memcpy(&inq_response[56], dev->serial, sizeof(dev->serial));
+	if (bitmap_empty(eui, len * 8)) {
+		res = -EOPNOTSUPP;
+		goto out_free_id;
 	}
-	xfer_len = alloc_len;
-	return nvme_trans_copy_to_user(hdr, inq_response, xfer_len);
+
+	memset(inq_response, 0, alloc_len);
+	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;
+	inq_response[3] = 4 + len; /* Page Length */
+
+	/* Designation Descriptor start */
+	inq_response[4] = 0x01;	/* Proto ID=0h | Code set=1h */
+	inq_response[5] = 0x02;	/* PIV=0b | Asso=00b | Designator Type=2h */
+	inq_response[6] = 0x00;	/* Rsvd */
+	inq_response[7] = len;	/* Designator Length */
+	memcpy(&inq_response[8], eui, len);
+
+	res = nvme_trans_copy_to_user(hdr, inq_response, alloc_len);
+out_free_id:
+	kfree(id_ns);
+	return res;
+}
+
+static int nvme_fill_device_id_scsi_string(struct nvme_ns *ns,
+		struct sg_io_hdr *hdr, u8 *inq_response, int alloc_len)
+{
+	struct nvme_ctrl *ctrl = ns->ctrl;
+	struct nvme_id_ctrl *id_ctrl;
+	int nvme_sc, res;
+
+	if (alloc_len < 72) {
+		return nvme_trans_completion(hdr,
+				SAM_STAT_CHECK_CONDITION,
+				ILLEGAL_REQUEST, SCSI_ASC_INVALID_CDB,
+				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
+	}
+
+	nvme_sc = nvme_identify_ctrl(ctrl, &id_ctrl);
+	res = nvme_trans_status_code(hdr, nvme_sc);
+	if (res)
+		return res;
+
+	memset(inq_response, 0, alloc_len);
+	inq_response[1] = INQ_DEVICE_IDENTIFICATION_PAGE;
+	inq_response[3] = 0x48;	/* Page Length */
+
+	/* Designation Descriptor start */
+	inq_response[4] = 0x03;	/* Proto ID=0h | Code set=3h */
+	inq_response[5] = 0x08;	/* PIV=0b | Asso=00b | Designator Type=8h */
+	inq_response[6] = 0x00;	/* Rsvd */
+	inq_response[7] = 0x44;	/* Designator Length */
+
+	sprintf(&inq_response[8], "%04x", le16_to_cpu(id_ctrl->vid));
+	memcpy(&inq_response[12], ctrl->model, sizeof(ctrl->model));
+	sprintf(&inq_response[52], "%04x", cpu_to_be32(ns->ns_id));
+	memcpy(&inq_response[56], ctrl->serial, sizeof(ctrl->serial));
+
+	res = nvme_trans_copy_to_user(hdr, inq_response, alloc_len);
+	kfree(id_ctrl);
+	return res;
+}
+
+static int nvme_trans_device_id_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
+					u8 *resp, int alloc_len)
+{
+	int res;
+
+	if (ns->ctrl->vs >= NVME_VS(1, 1)) {
+		res = nvme_fill_device_id_eui64(ns, hdr, resp, alloc_len);
+		if (res != -EOPNOTSUPP)
+			return res;
+	}
+
+	return nvme_fill_device_id_scsi_string(ns, hdr, resp, alloc_len);
 }
 
 static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
@@ -672,7 +703,7 @@ static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	u8 *inq_response;
 	int res;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
+	struct nvme_ctrl *ctrl = ns->ctrl;
 	struct nvme_id_ctrl *id_ctrl;
 	struct nvme_id_ns *id_ns;
 	int xfer_len;
@@ -688,7 +719,7 @@ static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	if (inq_response == NULL)
 		return -ENOMEM;
 
-	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+	nvme_sc = nvme_identify_ns(ctrl, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		goto out_free_inq;
@@ -704,7 +735,7 @@ static int nvme_trans_ext_inq_page(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	app_chk = protect << 1;
 	ref_chk = protect;
 
-	nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
+	nvme_sc = nvme_identify_ctrl(ctrl, &id_ctrl);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		goto out_free_inq;
@@ -815,7 +846,6 @@ static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
 	int res;
 	int xfer_len;
 	u8 *log_response;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_smart_log *smart_log;
 	u8 temp_c;
 	u16 temp_k;
@@ -824,7 +854,7 @@ static int nvme_trans_log_info_exceptions(struct nvme_ns *ns,
 	if (log_response == NULL)
 		return -ENOMEM;
 
-	res = nvme_get_log_page(dev, &smart_log);
+	res = nvme_get_log_page(ns->ctrl, &smart_log);
 	if (res < 0)
 		goto out_free_response;
 
@@ -862,7 +892,6 @@ static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	int res;
 	int xfer_len;
 	u8 *log_response;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_smart_log *smart_log;
 	u32 feature_resp;
 	u8 temp_c_cur, temp_c_thresh;
@@ -872,7 +901,7 @@ static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	if (log_response == NULL)
 		return -ENOMEM;
 
-	res = nvme_get_log_page(dev, &smart_log);
+	res = nvme_get_log_page(ns->ctrl, &smart_log);
 	if (res < 0)
 		goto out_free_response;
 
@@ -886,7 +915,7 @@ static int nvme_trans_log_temperature(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	kfree(smart_log);
 
 	/* Get Features for Temp Threshold */
-	res = nvme_get_features(dev, NVME_FEAT_TEMP_THRESH, 0, 0,
+	res = nvme_get_features(ns->ctrl, NVME_FEAT_TEMP_THRESH, 0, 0,
 								&feature_resp);
 	if (res != NVME_SC_SUCCESS)
 		temp_c_thresh = LOG_TEMP_UNKNOWN;
@@ -948,7 +977,6 @@ static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 {
 	int res;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_id_ns *id_ns;
 	u8 flbas;
 	u32 lba_length;
@@ -958,7 +986,7 @@ static int nvme_trans_fill_blk_desc(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	else if (llbaa > 0 && len < MODE_PAGE_LLBAA_BLK_DES_LEN)
 		return -EINVAL;
 
-	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		return res;
@@ -1014,14 +1042,13 @@ static int nvme_trans_fill_caching_page(struct nvme_ns *ns,
 {
 	int res = 0;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	u32 feature_resp;
 	u8 vwc;
 
 	if (len < MODE_PAGE_CACHING_LEN)
 		return -EINVAL;
 
-	nvme_sc = nvme_get_features(dev, NVME_FEAT_VOLATILE_WC, 0, 0,
+	nvme_sc = nvme_get_features(ns->ctrl, NVME_FEAT_VOLATILE_WC, 0, 0,
 								&feature_resp);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
@@ -1207,12 +1234,11 @@ static int nvme_trans_power_state(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 {
 	int res;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_id_ctrl *id_ctrl;
 	int lowest_pow_st;	/* max npss = lowest power consumption */
 	unsigned ps_desired = 0;
 
-	nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
+	nvme_sc = nvme_identify_ctrl(ns->ctrl, &id_ctrl);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		return res;
@@ -1256,7 +1282,7 @@ static int nvme_trans_power_state(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 				SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
 		break;
 	}
-	nvme_sc = nvme_set_features(dev, NVME_FEAT_POWER_MGMT, ps_desired, 0,
+	nvme_sc = nvme_set_features(ns->ctrl, NVME_FEAT_POWER_MGMT, ps_desired, 0,
 				    NULL);
 	return nvme_trans_status_code(hdr, nvme_sc);
 }
@@ -1280,7 +1306,6 @@ static int nvme_trans_send_download_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr
 					u8 buffer_id)
 {
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_command c;
 
 	if (hdr->iovec_count > 0) {
@@ -1297,7 +1322,7 @@ static int nvme_trans_send_download_fw_cmd(struct nvme_ns *ns, struct sg_io_hdr
 	c.dlfw.numd = cpu_to_le32((tot_len/BYTES_TO_DWORDS) - 1);
 	c.dlfw.offset = cpu_to_le32(offset/BYTES_TO_DWORDS);
 
-	nvme_sc = __nvme_submit_sync_cmd(dev->admin_q, &c, NULL,
+	nvme_sc = nvme_submit_user_cmd(ns->ctrl->admin_q, &c,
 			hdr->dxferp, tot_len, NULL, 0);
 	return nvme_trans_status_code(hdr, nvme_sc);
 }
@@ -1364,14 +1389,13 @@ static int nvme_trans_modesel_get_mp(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 {
 	int res = 0;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	unsigned dword11;
 
 	switch (page_code) {
 	case MODE_PAGE_CACHING:
 		dword11 = ((mode_page[2] & CACHING_MODE_PAGE_WCE_MASK) ? 1 : 0);
-		nvme_sc = nvme_set_features(dev, NVME_FEAT_VOLATILE_WC, dword11,
-					    0, NULL);
+		nvme_sc = nvme_set_features(ns->ctrl, NVME_FEAT_VOLATILE_WC,
+					    dword11, 0, NULL);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		break;
 	case MODE_PAGE_CONTROL:
@@ -1473,7 +1497,6 @@ static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
 {
 	int res = 0;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	u8 flbas;
 
 	/*
@@ -1486,7 +1509,7 @@ static int nvme_trans_fmt_set_blk_size_count(struct nvme_ns *ns,
 	if (ns->mode_select_num_blocks == 0 || ns->mode_select_block_len == 0) {
 		struct nvme_id_ns *id_ns;
 
-		nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+		nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		if (res)
 			return res;
@@ -1570,7 +1593,6 @@ static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 {
 	int res;
 	int nvme_sc;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_id_ns *id_ns;
 	u8 i;
 	u8 flbas, nlbaf;
@@ -1579,7 +1601,7 @@ static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	struct nvme_command c;
 
 	/* Loop thru LBAF's in id_ns to match reqd lbaf, put in cdw10 */
-	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		return res;
@@ -1611,7 +1633,7 @@ static int nvme_trans_fmt_send_cmd(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	c.format.nsid = cpu_to_le32(ns->ns_id);
 	c.format.cdw10 = cpu_to_le32(cdw10);
 
-	nvme_sc = nvme_submit_sync_cmd(dev->admin_q, &c, NULL, 0);
+	nvme_sc = nvme_submit_sync_cmd(ns->ctrl->admin_q, &c, NULL, 0);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 
 	kfree(id_ns);
@@ -1704,7 +1726,7 @@ static int nvme_trans_do_nvme_io(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 			nvme_sc = NVME_SC_LBA_RANGE;
 			break;
 		}
-		nvme_sc = __nvme_submit_sync_cmd(ns->queue, &c, NULL,
+		nvme_sc = nvme_submit_user_cmd(ns->queue, &c,
 				next_mapping_addr, unit_len, NULL, 0);
 		if (nvme_sc)
 			break;
@@ -2040,7 +2062,6 @@ static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	u32 alloc_len;
 	u32 resp_size;
 	u32 xfer_len;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_id_ns *id_ns;
 	u8 *response;
 
@@ -2052,7 +2073,7 @@ static int nvme_trans_read_capacity(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 		resp_size = READ_CAP_10_RESP_SIZE;
 	}
 
-	nvme_sc = nvme_identify_ns(dev, ns->ns_id, &id_ns);
+	nvme_sc = nvme_identify_ns(ns->ctrl, ns->ns_id, &id_ns);
 	res = nvme_trans_status_code(hdr, nvme_sc);
 	if (res)
 		return res;	
@@ -2080,7 +2101,6 @@ static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	int nvme_sc;
 	u32 alloc_len, xfer_len, resp_size;
 	u8 *response;
-	struct nvme_dev *dev = ns->dev;
 	struct nvme_id_ctrl *id_ctrl;
 	u32 ll_length, lun_id;
 	u8 lun_id_offset = REPORT_LUNS_FIRST_LUN_OFFSET;
@@ -2094,7 +2114,7 @@ static int nvme_trans_report_luns(struct nvme_ns *ns, struct sg_io_hdr *hdr,
 	case ALL_LUNS_RETURNED:
 	case ALL_WELL_KNOWN_LUNS_RETURNED:
 	case RESTRICTED_LUNS_RETURNED:
-		nvme_sc = nvme_identify_ctrl(dev, &id_ctrl);
+		nvme_sc = nvme_identify_ctrl(ns->ctrl, &id_ctrl);
 		res = nvme_trans_status_code(hdr, nvme_sc);
 		if (res)
 			return res;
@@ -2295,9 +2315,7 @@ static int nvme_trans_test_unit_ready(struct nvme_ns *ns,
 					struct sg_io_hdr *hdr,
 					u8 *cmd)
 {
-	struct nvme_dev *dev = ns->dev;
-
-	if (!(readl(&dev->bar->csts) & NVME_CSTS_RDY))
+	if (nvme_ctrl_ready(ns->ctrl))
 		return nvme_trans_completion(hdr, SAM_STAT_CHECK_CONDITION,
 					    NOT_READY, SCSI_ASC_LUN_NOT_READY,
 					    SCSI_ASCQ_CAUSE_NOT_REPORTABLE);
diff --git a/drivers/nvmem/core.c b/drivers/nvmem/core.c
index 6fd4e5a5ef4a..9d11d9837312 100644
--- a/drivers/nvmem/core.c
+++ b/drivers/nvmem/core.c
@@ -70,6 +70,9 @@ static ssize_t bin_attr_nvmem_read(struct file *filp, struct kobject *kobj,
 	if (pos >= nvmem->size)
 		return 0;
 
+	if (count < nvmem->word_size)
+		return -EINVAL;
+
 	if (pos + count > nvmem->size)
 		count = nvmem->size - pos;
 
@@ -95,6 +98,9 @@ static ssize_t bin_attr_nvmem_write(struct file *filp, struct kobject *kobj,
 	if (pos >= nvmem->size)
 		return 0;
 
+	if (count < nvmem->word_size)
+		return -EINVAL;
+
 	if (pos + count > nvmem->size)
 		count = nvmem->size - pos;
 
diff --git a/drivers/nvmem/qfprom.c b/drivers/nvmem/qfprom.c
index afb67e7eeee4..3829e5fbf8c3 100644
--- a/drivers/nvmem/qfprom.c
+++ b/drivers/nvmem/qfprom.c
@@ -21,6 +21,7 @@ static struct regmap_config qfprom_regmap_config = {
 	.reg_bits = 32,
 	.val_bits = 8,
 	.reg_stride = 1,
+	.val_format_endian = REGMAP_ENDIAN_LITTLE,
 };
 
 static struct nvmem_config econfig = {
diff --git a/drivers/of/address.c b/drivers/of/address.c
index 9582c5703b3c..91a469d55b8f 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -597,7 +597,7 @@ static u64 __of_translate_address(struct device_node *dev,
 		pbus = of_match_bus(parent);
 		pbus->count_cells(dev, &pna, &pns);
 		if (!OF_CHECK_COUNTS(pna, pns)) {
-			printk(KERN_ERR "prom_parse: Bad cell count for %s\n",
+			pr_err("prom_parse: Bad cell count for %s\n",
 			       of_node_full_name(dev));
 			break;
 		}
diff --git a/drivers/of/dynamic.c b/drivers/of/dynamic.c
index 53826b84e0ec..c647bd1b6903 100644
--- a/drivers/of/dynamic.c
+++ b/drivers/of/dynamic.c
@@ -646,6 +646,7 @@ void of_changeset_init(struct of_changeset *ocs)
 	memset(ocs, 0, sizeof(*ocs));
 	INIT_LIST_HEAD(&ocs->entries);
 }
+EXPORT_SYMBOL_GPL(of_changeset_init);
 
 /**
  * of_changeset_destroy - Destroy a changeset
@@ -662,20 +663,9 @@ void of_changeset_destroy(struct of_changeset *ocs)
 	list_for_each_entry_safe_reverse(ce, cen, &ocs->entries, node)
 		__of_changeset_entry_destroy(ce);
 }
+EXPORT_SYMBOL_GPL(of_changeset_destroy);
 
-/**
- * of_changeset_apply - Applies a changeset
- *
- * @ocs:	changeset pointer
- *
- * Applies a changeset to the live tree.
- * Any side-effects of live tree state changes are applied here on
- * sucess, like creation/destruction of devices and side-effects
- * like creation of sysfs properties and directories.
- * Returns 0 on success, a negative error value in case of an error.
- * On error the partially applied effects are reverted.
- */
-int of_changeset_apply(struct of_changeset *ocs)
+int __of_changeset_apply(struct of_changeset *ocs)
 {
 	struct of_changeset_entry *ce;
 	int ret;
@@ -704,17 +694,30 @@ int of_changeset_apply(struct of_changeset *ocs)
 }
 
 /**
- * of_changeset_revert - Reverts an applied changeset
+ * of_changeset_apply - Applies a changeset
  *
  * @ocs:	changeset pointer
  *
- * Reverts a changeset returning the state of the tree to what it
- * was before the application.
- * Any side-effects like creation/destruction of devices and
- * removal of sysfs properties and directories are applied.
+ * Applies a changeset to the live tree.
+ * Any side-effects of live tree state changes are applied here on
+ * success, like creation/destruction of devices and side-effects
+ * like creation of sysfs properties and directories.
  * Returns 0 on success, a negative error value in case of an error.
+ * On error the partially applied effects are reverted.
  */
-int of_changeset_revert(struct of_changeset *ocs)
+int of_changeset_apply(struct of_changeset *ocs)
+{
+	int ret;
+
+	mutex_lock(&of_mutex);
+	ret = __of_changeset_apply(ocs);
+	mutex_unlock(&of_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_changeset_apply);
+
+int __of_changeset_revert(struct of_changeset *ocs)
 {
 	struct of_changeset_entry *ce;
 	int ret;
@@ -742,6 +745,29 @@ int of_changeset_revert(struct of_changeset *ocs)
 }
 
 /**
+ * of_changeset_revert - Reverts an applied changeset
+ *
+ * @ocs:	changeset pointer
+ *
+ * Reverts a changeset returning the state of the tree to what it
+ * was before the application.
+ * Any side-effects like creation/destruction of devices and
+ * removal of sysfs properties and directories are applied.
+ * Returns 0 on success, a negative error value in case of an error.
+ */
+int of_changeset_revert(struct of_changeset *ocs)
+{
+	int ret;
+
+	mutex_lock(&of_mutex);
+	ret = __of_changeset_revert(ocs);
+	mutex_unlock(&of_mutex);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(of_changeset_revert);
+
+/**
  * of_changeset_action - Perform a changeset action
  *
  * @ocs:	changeset pointer
@@ -779,3 +805,4 @@ int of_changeset_action(struct of_changeset *ocs, unsigned long action,
 	list_add_tail(&ce->node, &ocs->entries);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(of_changeset_action);
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 4fa916dffc91..e7bfc175b8e1 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -473,6 +473,7 @@ EXPORT_SYMBOL_GPL(of_irq_to_resource_table);
 
 struct of_intc_desc {
 	struct list_head	list;
+	of_irq_init_cb_t	irq_init_cb;
 	struct device_node	*dev;
 	struct device_node	*interrupt_parent;
 };
@@ -486,6 +487,7 @@ struct of_intc_desc {
  */
 void __init of_irq_init(const struct of_device_id *matches)
 {
+	const struct of_device_id *match;
 	struct device_node *np, *parent = NULL;
 	struct of_intc_desc *desc, *temp_desc;
 	struct list_head intc_desc_list, intc_parent_list;
@@ -493,10 +495,15 @@ void __init of_irq_init(const struct of_device_id *matches)
 	INIT_LIST_HEAD(&intc_desc_list);
 	INIT_LIST_HEAD(&intc_parent_list);
 
-	for_each_matching_node(np, matches) {
+	for_each_matching_node_and_match(np, matches, &match) {
 		if (!of_find_property(np, "interrupt-controller", NULL) ||
 				!of_device_is_available(np))
 			continue;
+
+		if (WARN(!match->data, "of_irq_init: no init function for %s\n",
+			 match->compatible))
+			continue;
+
 		/*
 		 * Here, we allocate and populate an of_intc_desc with the node
 		 * pointer, interrupt-parent device_node etc.
@@ -507,6 +514,7 @@ void __init of_irq_init(const struct of_device_id *matches)
 			goto err;
 		}
 
+		desc->irq_init_cb = match->data;
 		desc->dev = of_node_get(np);
 		desc->interrupt_parent = of_irq_find_parent(np);
 		if (desc->interrupt_parent == np)
@@ -526,27 +534,18 @@ void __init of_irq_init(const struct of_device_id *matches)
 		 * The assumption is that NULL parent means a root controller.
 		 */
 		list_for_each_entry_safe(desc, temp_desc, &intc_desc_list, list) {
-			const struct of_device_id *match;
 			int ret;
-			of_irq_init_cb_t irq_init_cb;
 
 			if (desc->interrupt_parent != parent)
 				continue;
 
 			list_del(&desc->list);
-			match = of_match_node(matches, desc->dev);
-			if (WARN(!match->data,
-			    "of_irq_init: no init function for %s\n",
-			    match->compatible)) {
-				kfree(desc);
-				continue;
-			}
 
-			pr_debug("of_irq_init: init %s @ %p, parent %p\n",
-				 match->compatible,
+			pr_debug("of_irq_init: init %s (%p), parent %p\n",
+				 desc->dev->full_name,
 				 desc->dev, desc->interrupt_parent);
-			irq_init_cb = (of_irq_init_cb_t)match->data;
-			ret = irq_init_cb(desc->dev, desc->interrupt_parent);
+			ret = desc->irq_init_cb(desc->dev,
+						desc->interrupt_parent);
 			if (ret) {
 				kfree(desc);
 				continue;
@@ -636,6 +635,13 @@ static u32 __of_msi_map_rid(struct device *dev, struct device_node **np,
 		msi_base = be32_to_cpup(msi_map + 2);
 		rid_len = be32_to_cpup(msi_map + 3);
 
+		if (rid_base & ~map_mask) {
+			dev_err(parent_dev,
+				"Invalid msi-map translation - msi-map-mask (0x%x) ignores rid-base (0x%x)\n",
+				map_mask, rid_base);
+			return rid_out;
+		}
+
 		msi_controller_node = of_find_node_by_phandle(phandle);
 
 		matched = (masked_rid >= rid_base &&
@@ -655,7 +661,7 @@ static u32 __of_msi_map_rid(struct device *dev, struct device_node **np,
 	if (!matched)
 		return rid_out;
 
-	rid_out = masked_rid + msi_base;
+	rid_out = masked_rid - rid_base + msi_base;
 	dev_dbg(dev,
 		"msi-map at: %s, using mask %08x, rid-base: %08x, msi-base: %08x, length: %08x, rid: %08x -> %08x\n",
 		dev_name(parent_dev), map_mask, rid_base, msi_base,
@@ -680,18 +686,6 @@ u32 of_msi_map_rid(struct device *dev, struct device_node *msi_np, u32 rid_in)
 	return __of_msi_map_rid(dev, &msi_np, rid_in);
 }
 
-static struct irq_domain *__of_get_msi_domain(struct device_node *np,
-					      enum irq_domain_bus_token token)
-{
-	struct irq_domain *d;
-
-	d = irq_find_matching_host(np, token);
-	if (!d)
-		d = irq_find_host(np);
-
-	return d;
-}
-
 /**
  * of_msi_map_get_device_domain - Use msi-map to find the relevant MSI domain
  * @dev: device for which the mapping is to be done.
@@ -707,7 +701,7 @@ struct irq_domain *of_msi_map_get_device_domain(struct device *dev, u32 rid)
 	struct device_node *np = NULL;
 
 	__of_msi_map_rid(dev, &np, rid);
-	return __of_get_msi_domain(np, DOMAIN_BUS_PCI_MSI);
+	return irq_find_matching_host(np, DOMAIN_BUS_PCI_MSI);
 }
 
 /**
@@ -731,7 +725,7 @@ struct irq_domain *of_msi_get_domain(struct device *dev,
 	/* Check for a single msi-parent property */
 	msi_np = of_parse_phandle(np, "msi-parent", 0);
 	if (msi_np && !of_property_read_bool(msi_np, "#msi-cells")) {
-		d = __of_get_msi_domain(msi_np, token);
+		d = irq_find_matching_host(msi_np, token);
 		if (!d)
 			of_node_put(msi_np);
 		return d;
@@ -745,7 +739,7 @@ struct irq_domain *of_msi_get_domain(struct device *dev,
 		while (!of_parse_phandle_with_args(np, "msi-parent",
 						   "#msi-cells",
 						   index, &args)) {
-			d = __of_get_msi_domain(args.np, token);
+			d = irq_find_matching_host(args.np, token);
 			if (d)
 				return d;
 
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index a87a868fed64..365dc7e83ab4 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -62,11 +62,9 @@ static int of_mdiobus_register_phy(struct mii_bus *mdio, struct device_node *chi
 	rc = irq_of_parse_and_map(child, 0);
 	if (rc > 0) {
 		phy->irq = rc;
-		if (mdio->irq)
-			mdio->irq[addr] = rc;
+		mdio->irq[addr] = rc;
 	} else {
-		if (mdio->irq)
-			phy->irq = mdio->irq[addr];
+		phy->irq = mdio->irq[addr];
 	}
 
 	if (of_property_read_bool(child, "broken-turn-around"))
@@ -75,7 +73,7 @@ static int of_mdiobus_register_phy(struct mii_bus *mdio, struct device_node *chi
 	/* Associate the OF node with the device structure so it
 	 * can be looked up later */
 	of_node_get(child);
-	phy->dev.of_node = child;
+	phy->mdio.dev.of_node = child;
 
 	/* All data is now stored in the phy struct;
 	 * register it */
@@ -92,6 +90,37 @@ static int of_mdiobus_register_phy(struct mii_bus *mdio, struct device_node *chi
 	return 0;
 }
 
+static int of_mdiobus_register_device(struct mii_bus *mdio,
+				      struct device_node *child,
+				      u32 addr)
+{
+	struct mdio_device *mdiodev;
+	int rc;
+
+	mdiodev = mdio_device_create(mdio, addr);
+	if (!mdiodev || IS_ERR(mdiodev))
+		return 1;
+
+	/* Associate the OF node with the device structure so it
+	 * can be looked up later.
+	 */
+	of_node_get(child);
+	mdiodev->dev.of_node = child;
+
+	/* All data is now stored in the mdiodev struct; register it. */
+	rc = mdio_device_register(mdiodev);
+	if (rc) {
+		mdio_device_free(mdiodev);
+		of_node_put(child);
+		return 1;
+	}
+
+	dev_dbg(&mdio->dev, "registered mdio device %s at address %i\n",
+		child->name, addr);
+
+	return 0;
+}
+
 int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
 {
 	u32 addr;
@@ -114,6 +143,63 @@ int of_mdio_parse_addr(struct device *dev, const struct device_node *np)
 }
 EXPORT_SYMBOL(of_mdio_parse_addr);
 
+/* The following is a list of PHY compatible strings which appear in
+ * some DTBs. The compatible string is never matched against a PHY
+ * driver, so is pointless. We only expect devices which are not PHYs
+ * to have a compatible string, so they can be matched to an MDIO
+ * driver.  Encourage users to upgrade their DT blobs to remove these.
+ */
+static const struct of_device_id whitelist_phys[] = {
+	{ .compatible = "brcm,40nm-ephy" },
+	{ .compatible = "marvell,88E1111", },
+	{ .compatible = "marvell,88e1116", },
+	{ .compatible = "marvell,88e1118", },
+	{ .compatible = "marvell,88e1145", },
+	{ .compatible = "marvell,88e1149r", },
+	{ .compatible = "marvell,88e1310", },
+	{ .compatible = "marvell,88E1510", },
+	{ .compatible = "marvell,88E1514", },
+	{ .compatible = "moxa,moxart-rtl8201cp", },
+	{}
+};
+
+/*
+ * Return true if the child node is for a phy. It must either:
+ * o Compatible string of "ethernet-phy-idX.X"
+ * o Compatible string of "ethernet-phy-ieee802.3-c45"
+ * o Compatible string of "ethernet-phy-ieee802.3-c22"
+ * o In the white list above (and issue a warning)
+ * o No compatibility string
+ *
+ * A device which is not a phy is expected to have a compatible string
+ * indicating what sort of device it is.
+ */
+static bool of_mdiobus_child_is_phy(struct device_node *child)
+{
+	u32 phy_id;
+
+	if (of_get_phy_id(child, &phy_id) != -EINVAL)
+		return true;
+
+	if (of_device_is_compatible(child, "ethernet-phy-ieee802.3-c45"))
+		return true;
+
+	if (of_device_is_compatible(child, "ethernet-phy-ieee802.3-c22"))
+		return true;
+
+	if (of_match_node(whitelist_phys, child)) {
+		pr_warn(FW_WARN
+			"%s: Whitelisted compatible string. Please remove\n",
+			child->full_name);
+		return true;
+	}
+
+	if (!of_find_property(child, "compatible", NULL))
+		return true;
+
+	return false;
+}
+
 /**
  * of_mdiobus_register - Register mii_bus and create PHYs from the device tree
  * @mdio: pointer to mii_bus structure
@@ -127,17 +213,12 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 	struct device_node *child;
 	const __be32 *paddr;
 	bool scanphys = false;
-	int addr, rc, i;
+	int addr, rc;
 
 	/* Mask out all PHYs from auto probing.  Instead the PHYs listed in
 	 * the device tree are populated after the bus has been registered */
 	mdio->phy_mask = ~0;
 
-	/* Clear all the IRQ properties */
-	if (mdio->irq)
-		for (i=0; i<PHY_MAX_ADDR; i++)
-			mdio->irq[i] = PHY_POLL;
-
 	mdio->dev.of_node = np;
 
 	/* Register the MDIO bus */
@@ -145,7 +226,7 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 	if (rc)
 		return rc;
 
-	/* Loop over the child nodes and register a phy_device for each one */
+	/* Loop over the child nodes and register a phy_device for each phy */
 	for_each_available_child_of_node(np, child) {
 		addr = of_mdio_parse_addr(&mdio->dev, child);
 		if (addr < 0) {
@@ -153,9 +234,10 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 			continue;
 		}
 
-		rc = of_mdiobus_register_phy(mdio, child, addr);
-		if (rc)
-			continue;
+		if (of_mdiobus_child_is_phy(child))
+			of_mdiobus_register_phy(mdio, child, addr);
+		else
+			of_mdiobus_register_device(mdio, child, addr);
 	}
 
 	if (!scanphys)
@@ -170,16 +252,15 @@ int of_mdiobus_register(struct mii_bus *mdio, struct device_node *np)
 
 		for (addr = 0; addr < PHY_MAX_ADDR; addr++) {
 			/* skip already registered PHYs */
-			if (mdio->phy_map[addr])
+			if (mdiobus_is_registered_device(mdio, addr))
 				continue;
 
 			/* be noisy to encourage people to set reg property */
 			dev_info(&mdio->dev, "scan phy %s at address %i\n",
 				 child->name, addr);
 
-			rc = of_mdiobus_register_phy(mdio, child, addr);
-			if (rc)
-				continue;
+			if (of_mdiobus_child_is_phy(child))
+				of_mdiobus_register_phy(mdio, child, addr);
 		}
 	}
 
@@ -203,11 +284,19 @@ static int of_phy_match(struct device *dev, void *phy_np)
 struct phy_device *of_phy_find_device(struct device_node *phy_np)
 {
 	struct device *d;
+	struct mdio_device *mdiodev;
+
 	if (!phy_np)
 		return NULL;
 
 	d = bus_find_device(&mdio_bus_type, NULL, phy_np, of_phy_match);
-	return d ? to_phy_device(d) : NULL;
+	if (d) {
+		mdiodev = to_mdio_device(d);
+		if (mdiodev->flags & MDIO_DEVICE_FLAG_PHY)
+			return to_phy_device(d);
+	}
+
+	return NULL;
 }
 EXPORT_SYMBOL(of_phy_find_device);
 
@@ -216,6 +305,7 @@ EXPORT_SYMBOL(of_phy_find_device);
  * @dev: pointer to net_device claiming the phy
  * @phy_np: Pointer to device tree node for the PHY
  * @hndlr: Link state callback for the network device
+ * @flags: flags to pass to the PHY
  * @iface: PHY data interface type
  *
  * If successful, returns a pointer to the phy_device with the embedded
@@ -238,7 +328,7 @@ struct phy_device *of_phy_connect(struct net_device *dev,
 	ret = phy_connect_direct(dev, phy, hndlr, iface);
 
 	/* refcount is held by phy_connect_direct() on success */
-	put_device(&phy->dev);
+	put_device(&phy->mdio.dev);
 
 	return ret ? NULL : phy;
 }
@@ -268,7 +358,7 @@ struct phy_device *of_phy_attach(struct net_device *dev,
 	ret = phy_attach_direct(dev, phy, flags, iface);
 
 	/* refcount is held by phy_attach_direct() on success */
-	put_device(&phy->dev);
+	put_device(&phy->mdio.dev);
 
 	return ret ? NULL : phy;
 }
diff --git a/drivers/of/of_private.h b/drivers/of/of_private.h
index 8e882e706cd8..829469faeb23 100644
--- a/drivers/of/of_private.h
+++ b/drivers/of/of_private.h
@@ -45,6 +45,8 @@ static inline struct device_node *kobj_to_device_node(struct kobject *kobj)
 extern int of_property_notify(int action, struct device_node *np,
 			      struct property *prop, struct property *old_prop);
 extern void of_node_release(struct kobject *kobj);
+extern int __of_changeset_apply(struct of_changeset *ocs);
+extern int __of_changeset_revert(struct of_changeset *ocs);
 #else /* CONFIG_OF_DYNAMIC */
 static inline int of_property_notify(int action, struct device_node *np,
 				     struct property *prop, struct property *old_prop)
diff --git a/drivers/of/overlay.c b/drivers/of/overlay.c
index 54e5af9d7377..82250815e9a5 100644
--- a/drivers/of/overlay.c
+++ b/drivers/of/overlay.c
@@ -379,9 +379,9 @@ int of_overlay_create(struct device_node *tree)
 	}
 
 	/* apply the changeset */
-	err = of_changeset_apply(&ov->cset);
+	err = __of_changeset_apply(&ov->cset);
 	if (err) {
-		pr_err("%s: of_changeset_apply() failed for tree@%s\n",
+		pr_err("%s: __of_changeset_apply() failed for tree@%s\n",
 				__func__, tree->full_name);
 		goto err_revert_overlay;
 	}
@@ -511,7 +511,7 @@ int of_overlay_destroy(int id)
 
 
 	list_del(&ov->node);
-	of_changeset_revert(&ov->cset);
+	__of_changeset_revert(&ov->cset);
 	of_free_overlay_info(ov);
 	idr_remove(&ov_idr, id);
 	of_changeset_destroy(&ov->cset);
@@ -542,7 +542,7 @@ int of_overlay_destroy_all(void)
 	/* the tail of list is guaranteed to be safe to remove */
 	list_for_each_entry_safe_reverse(ov, ovn, &ov_list, node) {
 		list_del(&ov->node);
-		of_changeset_revert(&ov->cset);
+		__of_changeset_revert(&ov->cset);
 		of_free_overlay_info(ov);
 		idr_remove(&ov_idr, ov->id);
 		kfree(ov);
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index af98343614d8..8d103e4968be 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -31,6 +31,7 @@ const struct of_device_id of_default_bus_match_table[] = {
 #endif /* CONFIG_ARM_AMBA */
 	{} /* Empty terminated list */
 };
+EXPORT_SYMBOL(of_default_bus_match_table);
 
 static int of_dev_node_match(struct device *dev, void *data)
 {
diff --git a/drivers/of/unittest.c b/drivers/of/unittest.c
index e16ea5717b7f..979b6e415cea 100644
--- a/drivers/of/unittest.c
+++ b/drivers/of/unittest.c
@@ -530,18 +530,14 @@ static void __init of_unittest_changeset(void)
 	unittest(!of_changeset_add_property(&chgset, parent, ppadd), "fail add prop\n");
 	unittest(!of_changeset_update_property(&chgset, parent, ppupdate), "fail update prop\n");
 	unittest(!of_changeset_remove_property(&chgset, parent, ppremove), "fail remove prop\n");
-	mutex_lock(&of_mutex);
 	unittest(!of_changeset_apply(&chgset), "apply failed\n");
-	mutex_unlock(&of_mutex);
 
 	/* Make sure node names are constructed correctly */
 	unittest((np = of_find_node_by_path("/testcase-data/changeset/n2/n21")),
 		 "'%s' not added\n", n21->full_name);
 	of_node_put(np);
 
-	mutex_lock(&of_mutex);
 	unittest(!of_changeset_revert(&chgset), "revert failed\n");
-	mutex_unlock(&of_mutex);
 
 	of_changeset_destroy(&chgset);
 #endif
@@ -757,6 +753,11 @@ static void __init of_unittest_match_node(void)
 	}
 }
 
+static struct resource test_bus_res = {
+	.start = 0xfffffff8,
+	.end = 0xfffffff9,
+	.flags = IORESOURCE_MEM,
+};
 static const struct platform_device_info test_bus_info = {
 	.name = "unittest-bus",
 };
@@ -800,6 +801,15 @@ static void __init of_unittest_platform_populate(void)
 		return;
 	test_bus->dev.of_node = np;
 
+	/*
+	 * Add a dummy resource to the test bus node after it is
+	 * registered to catch problems with un-inserted resources. The
+	 * DT code doesn't insert the resources, and it has caused the
+	 * kernel to oops in the past. This makes sure the same bug
+	 * doesn't crop up again.
+	 */
+	platform_device_add_resources(test_bus, &test_bus_res, 1);
+
 	of_platform_populate(np, match, NULL, &test_bus->dev);
 	for_each_child_of_node(np, child) {
 		for_each_child_of_node(child, grandchild)
diff --git a/drivers/oprofile/oprofilefs.c b/drivers/oprofile/oprofilefs.c
index dd92c5edf219..b48ac6300c79 100644
--- a/drivers/oprofile/oprofilefs.c
+++ b/drivers/oprofile/oprofilefs.c
@@ -138,22 +138,22 @@ static int __oprofilefs_create_file(struct dentry *root, char const *name,
 	struct dentry *dentry;
 	struct inode *inode;
 
-	mutex_lock(&d_inode(root)->i_mutex);
+	inode_lock(d_inode(root));
 	dentry = d_alloc_name(root, name);
 	if (!dentry) {
-		mutex_unlock(&d_inode(root)->i_mutex);
+		inode_unlock(d_inode(root));
 		return -ENOMEM;
 	}
 	inode = oprofilefs_get_inode(root->d_sb, S_IFREG | perm);
 	if (!inode) {
 		dput(dentry);
-		mutex_unlock(&d_inode(root)->i_mutex);
+		inode_unlock(d_inode(root));
 		return -ENOMEM;
 	}
 	inode->i_fop = fops;
 	inode->i_private = priv;
 	d_add(dentry, inode);
-	mutex_unlock(&d_inode(root)->i_mutex);
+	inode_unlock(d_inode(root));
 	return 0;
 }
 
@@ -215,22 +215,22 @@ struct dentry *oprofilefs_mkdir(struct dentry *parent, char const *name)
 	struct dentry *dentry;
 	struct inode *inode;
 
-	mutex_lock(&d_inode(parent)->i_mutex);
+	inode_lock(d_inode(parent));
 	dentry = d_alloc_name(parent, name);
 	if (!dentry) {
-		mutex_unlock(&d_inode(parent)->i_mutex);
+		inode_unlock(d_inode(parent));
 		return NULL;
 	}
 	inode = oprofilefs_get_inode(parent->d_sb, S_IFDIR | 0755);
 	if (!inode) {
 		dput(dentry);
-		mutex_unlock(&d_inode(parent)->i_mutex);
+		inode_unlock(d_inode(parent));
 		return NULL;
 	}
 	inode->i_op = &simple_dir_inode_operations;
 	inode->i_fop = &simple_dir_operations;
 	d_add(dentry, inode);
-	mutex_unlock(&d_inode(parent)->i_mutex);
+	inode_unlock(d_inode(parent));
 	return dentry;
 }
 
diff --git a/drivers/parisc/ccio-dma.c b/drivers/parisc/ccio-dma.c
index 8e11fb2831cd..e24b05996a1b 100644
--- a/drivers/parisc/ccio-dma.c
+++ b/drivers/parisc/ccio-dma.c
@@ -786,18 +786,27 @@ ccio_map_single(struct device *dev, void *addr, size_t size,
 	return CCIO_IOVA(iovp, offset);
 }
 
+
+static dma_addr_t
+ccio_map_page(struct device *dev, struct page *page, unsigned long offset,
+		size_t size, enum dma_data_direction direction,
+		struct dma_attrs *attrs)
+{
+	return ccio_map_single(dev, page_address(page) + offset, size,
+			direction);
+}
+
+
 /**
- * ccio_unmap_single - Unmap an address range from the IOMMU.
+ * ccio_unmap_page - Unmap an address range from the IOMMU.
  * @dev: The PCI device.
  * @addr: The start address of the DMA region.
  * @size: The length of the DMA region.
  * @direction: The direction of the DMA transaction (to/from device).
- *
- * This function implements the pci_unmap_single function.
  */
 static void 
-ccio_unmap_single(struct device *dev, dma_addr_t iova, size_t size, 
-		  enum dma_data_direction direction)
+ccio_unmap_page(struct device *dev, dma_addr_t iova, size_t size,
+		enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 	unsigned long flags; 
@@ -826,7 +835,7 @@ ccio_unmap_single(struct device *dev, dma_addr_t iova, size_t size,
 }
 
 /**
- * ccio_alloc_consistent - Allocate a consistent DMA mapping.
+ * ccio_alloc - Allocate a consistent DMA mapping.
  * @dev: The PCI device.
  * @size: The length of the DMA region.
  * @dma_handle: The DMA address handed back to the device (not the cpu).
@@ -834,7 +843,8 @@ ccio_unmap_single(struct device *dev, dma_addr_t iova, size_t size,
  * This function implements the pci_alloc_consistent function.
  */
 static void * 
-ccio_alloc_consistent(struct device *dev, size_t size, dma_addr_t *dma_handle, gfp_t flag)
+ccio_alloc(struct device *dev, size_t size, dma_addr_t *dma_handle, gfp_t flag,
+		struct dma_attrs *attrs)
 {
       void *ret;
 #if 0
@@ -858,7 +868,7 @@ ccio_alloc_consistent(struct device *dev, size_t size, dma_addr_t *dma_handle, g
 }
 
 /**
- * ccio_free_consistent - Free a consistent DMA mapping.
+ * ccio_free - Free a consistent DMA mapping.
  * @dev: The PCI device.
  * @size: The length of the DMA region.
  * @cpu_addr: The cpu address returned from the ccio_alloc_consistent.
@@ -867,10 +877,10 @@ ccio_alloc_consistent(struct device *dev, size_t size, dma_addr_t *dma_handle, g
  * This function implements the pci_free_consistent function.
  */
 static void 
-ccio_free_consistent(struct device *dev, size_t size, void *cpu_addr, 
-		     dma_addr_t dma_handle)
+ccio_free(struct device *dev, size_t size, void *cpu_addr,
+		dma_addr_t dma_handle, struct dma_attrs *attrs)
 {
-	ccio_unmap_single(dev, dma_handle, size, 0);
+	ccio_unmap_page(dev, dma_handle, size, 0, NULL);
 	free_pages((unsigned long)cpu_addr, get_order(size));
 }
 
@@ -897,7 +907,7 @@ ccio_free_consistent(struct device *dev, size_t size, void *cpu_addr,
  */
 static int
 ccio_map_sg(struct device *dev, struct scatterlist *sglist, int nents, 
-	    enum dma_data_direction direction)
+	    enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 	int coalesced, filled = 0;
@@ -974,7 +984,7 @@ ccio_map_sg(struct device *dev, struct scatterlist *sglist, int nents,
  */
 static void 
 ccio_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents, 
-	      enum dma_data_direction direction)
+	      enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 
@@ -993,27 +1003,22 @@ ccio_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 #ifdef CCIO_COLLECT_STATS
 		ioc->usg_pages += sg_dma_len(sglist) >> PAGE_SHIFT;
 #endif
-		ccio_unmap_single(dev, sg_dma_address(sglist),
-				  sg_dma_len(sglist), direction);
+		ccio_unmap_page(dev, sg_dma_address(sglist),
+				  sg_dma_len(sglist), direction, NULL);
 		++sglist;
 	}
 
 	DBG_RUN_SG("%s() DONE (nents %d)\n", __func__, nents);
 }
 
-static struct hppa_dma_ops ccio_ops = {
+static struct dma_map_ops ccio_ops = {
 	.dma_supported =	ccio_dma_supported,
-	.alloc_consistent =	ccio_alloc_consistent,
-	.alloc_noncoherent =	ccio_alloc_consistent,
-	.free_consistent =	ccio_free_consistent,
-	.map_single =		ccio_map_single,
-	.unmap_single =		ccio_unmap_single,
+	.alloc =		ccio_alloc,
+	.free =			ccio_free,
+	.map_page =		ccio_map_page,
+	.unmap_page =		ccio_unmap_page,
 	.map_sg = 		ccio_map_sg,
 	.unmap_sg = 		ccio_unmap_sg,
-	.dma_sync_single_for_cpu =	NULL,	/* NOP for U2/Uturn */
-	.dma_sync_single_for_device =	NULL,	/* NOP for U2/Uturn */
-	.dma_sync_sg_for_cpu =		NULL,	/* ditto */
-	.dma_sync_sg_for_device =		NULL,	/* ditto */
 };
 
 #ifdef CONFIG_PROC_FS
@@ -1062,7 +1067,7 @@ static int ccio_proc_info(struct seq_file *m, void *p)
 			   ioc->msingle_calls, ioc->msingle_pages,
 			   (int)((ioc->msingle_pages * 1000)/ioc->msingle_calls));
 
-		/* KLUGE - unmap_sg calls unmap_single for each mapped page */
+		/* KLUGE - unmap_sg calls unmap_page for each mapped page */
 		min = ioc->usingle_calls - ioc->usg_calls;
 		max = ioc->usingle_pages - ioc->usg_pages;
 		seq_printf(m, "pci_unmap_single: %8ld calls  %8ld pages (avg %d/1000)\n",
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index a0580afe1713..1133b5cc88ca 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -599,8 +599,10 @@ dino_fixup_bus(struct pci_bus *bus)
 		** P2PB's only have 2 BARs, no IRQs.
 		** I'd like to just ignore them for now.
 		*/
-		if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)
+		if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)  {
+			pcibios_init_bridge(dev);
 			continue;
+		}
 
 		/* null out the ROM resource if there is one (we don't
 		 * care about an expansion rom on parisc, since it
diff --git a/drivers/parisc/lba_pci.c b/drivers/parisc/lba_pci.c
index 42844c2bc065..2ec2aef4d211 100644
--- a/drivers/parisc/lba_pci.c
+++ b/drivers/parisc/lba_pci.c
@@ -790,8 +790,10 @@ lba_fixup_bus(struct pci_bus *bus)
                 /*
 		** P2PB's have no IRQs. ignore them.
 		*/
-		if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)
+		if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI) {
+			pcibios_init_bridge(dev);
 			continue;
+		}
 
 		/* Adjust INTERRUPT_LINE for this dev */
 		iosapic_fixup_irq(ldev->iosapic_obj, dev);
diff --git a/drivers/parisc/sba_iommu.c b/drivers/parisc/sba_iommu.c
index 225049b492e5..42ec4600b7e4 100644
--- a/drivers/parisc/sba_iommu.c
+++ b/drivers/parisc/sba_iommu.c
@@ -780,8 +780,18 @@ sba_map_single(struct device *dev, void *addr, size_t size,
 }
 
 
+static dma_addr_t
+sba_map_page(struct device *dev, struct page *page, unsigned long offset,
+		size_t size, enum dma_data_direction direction,
+		struct dma_attrs *attrs)
+{
+	return sba_map_single(dev, page_address(page) + offset, size,
+			direction);
+}
+
+
 /**
- * sba_unmap_single - unmap one IOVA and free resources
+ * sba_unmap_page - unmap one IOVA and free resources
  * @dev: instance of PCI owned by the driver that's asking.
  * @iova:  IOVA of driver buffer previously mapped.
  * @size:  number of bytes mapped in driver buffer.
@@ -790,8 +800,8 @@ sba_map_single(struct device *dev, void *addr, size_t size,
  * See Documentation/DMA-API-HOWTO.txt
  */
 static void
-sba_unmap_single(struct device *dev, dma_addr_t iova, size_t size,
-		 enum dma_data_direction direction)
+sba_unmap_page(struct device *dev, dma_addr_t iova, size_t size,
+		enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 #if DELAYED_RESOURCE_CNT > 0
@@ -858,15 +868,15 @@ sba_unmap_single(struct device *dev, dma_addr_t iova, size_t size,
 
 
 /**
- * sba_alloc_consistent - allocate/map shared mem for DMA
+ * sba_alloc - allocate/map shared mem for DMA
  * @hwdev: instance of PCI owned by the driver that's asking.
  * @size:  number of bytes mapped in driver buffer.
  * @dma_handle:  IOVA of new buffer.
  *
  * See Documentation/DMA-API-HOWTO.txt
  */
-static void *sba_alloc_consistent(struct device *hwdev, size_t size,
-					dma_addr_t *dma_handle, gfp_t gfp)
+static void *sba_alloc(struct device *hwdev, size_t size, dma_addr_t *dma_handle,
+		gfp_t gfp, struct dma_attrs *attrs)
 {
 	void *ret;
 
@@ -888,7 +898,7 @@ static void *sba_alloc_consistent(struct device *hwdev, size_t size,
 
 
 /**
- * sba_free_consistent - free/unmap shared mem for DMA
+ * sba_free - free/unmap shared mem for DMA
  * @hwdev: instance of PCI owned by the driver that's asking.
  * @size:  number of bytes mapped in driver buffer.
  * @vaddr:  virtual address IOVA of "consistent" buffer.
@@ -897,10 +907,10 @@ static void *sba_alloc_consistent(struct device *hwdev, size_t size,
  * See Documentation/DMA-API-HOWTO.txt
  */
 static void
-sba_free_consistent(struct device *hwdev, size_t size, void *vaddr,
-		    dma_addr_t dma_handle)
+sba_free(struct device *hwdev, size_t size, void *vaddr,
+		    dma_addr_t dma_handle, struct dma_attrs *attrs)
 {
-	sba_unmap_single(hwdev, dma_handle, size, 0);
+	sba_unmap_page(hwdev, dma_handle, size, 0, NULL);
 	free_pages((unsigned long) vaddr, get_order(size));
 }
 
@@ -933,7 +943,7 @@ int dump_run_sg = 0;
  */
 static int
 sba_map_sg(struct device *dev, struct scatterlist *sglist, int nents,
-	   enum dma_data_direction direction)
+	   enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 	int coalesced, filled = 0;
@@ -1016,7 +1026,7 @@ sba_map_sg(struct device *dev, struct scatterlist *sglist, int nents,
  */
 static void 
 sba_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
-	     enum dma_data_direction direction)
+	     enum dma_data_direction direction, struct dma_attrs *attrs)
 {
 	struct ioc *ioc;
 #ifdef ASSERT_PDIR_SANITY
@@ -1040,7 +1050,8 @@ sba_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 
 	while (sg_dma_len(sglist) && nents--) {
 
-		sba_unmap_single(dev, sg_dma_address(sglist), sg_dma_len(sglist), direction);
+		sba_unmap_page(dev, sg_dma_address(sglist), sg_dma_len(sglist),
+				direction, NULL);
 #ifdef SBA_COLLECT_STATS
 		ioc->usg_pages += ((sg_dma_address(sglist) & ~IOVP_MASK) + sg_dma_len(sglist) + IOVP_SIZE - 1) >> PAGE_SHIFT;
 		ioc->usingle_calls--;	/* kluge since call is unmap_sg() */
@@ -1058,19 +1069,14 @@ sba_unmap_sg(struct device *dev, struct scatterlist *sglist, int nents,
 
 }
 
-static struct hppa_dma_ops sba_ops = {
+static struct dma_map_ops sba_ops = {
 	.dma_supported =	sba_dma_supported,
-	.alloc_consistent =	sba_alloc_consistent,
-	.alloc_noncoherent =	sba_alloc_consistent,
-	.free_consistent =	sba_free_consistent,
-	.map_single =		sba_map_single,
-	.unmap_single =		sba_unmap_single,
+	.alloc =		sba_alloc,
+	.free =			sba_free,
+	.map_page =		sba_map_page,
+	.unmap_page =		sba_unmap_page,
 	.map_sg =		sba_map_sg,
 	.unmap_sg =		sba_unmap_sg,
-	.dma_sync_single_for_cpu =	NULL,
-	.dma_sync_single_for_device =	NULL,
-	.dma_sync_sg_for_cpu =		NULL,
-	.dma_sync_sg_for_device =	NULL,
 };
 
 
diff --git a/drivers/parport/share.c b/drivers/parport/share.c
index 5ce5ef211bdb..3308427ed9f7 100644
--- a/drivers/parport/share.c
+++ b/drivers/parport/share.c
@@ -1,6 +1,6 @@
 /*
  * Parallel-port resource manager code.
- * 
+ *
  * Authors: David Campbell <campbell@tirian.che.curtin.edu.au>
  *          Tim Waugh <tim@cyberelk.demon.co.uk>
  *          Jose Renau <renau@acm.org>
@@ -54,16 +54,16 @@ static LIST_HEAD(drivers);
 static DEFINE_MUTEX(registration_lock);
 
 /* What you can do to a port that's gone away.. */
-static void dead_write_lines (struct parport *p, unsigned char b){}
-static unsigned char dead_read_lines (struct parport *p) { return 0; }
-static unsigned char dead_frob_lines (struct parport *p, unsigned char b,
+static void dead_write_lines(struct parport *p, unsigned char b){}
+static unsigned char dead_read_lines(struct parport *p) { return 0; }
+static unsigned char dead_frob_lines(struct parport *p, unsigned char b,
 			     unsigned char c) { return 0; }
-static void dead_onearg (struct parport *p){}
-static void dead_initstate (struct pardevice *d, struct parport_state *s) { }
-static void dead_state (struct parport *p, struct parport_state *s) { }
-static size_t dead_write (struct parport *p, const void *b, size_t l, int f)
+static void dead_onearg(struct parport *p){}
+static void dead_initstate(struct pardevice *d, struct parport_state *s) { }
+static void dead_state(struct parport *p, struct parport_state *s) { }
+static size_t dead_write(struct parport *p, const void *b, size_t l, int f)
 { return 0; }
-static size_t dead_read (struct parport *p, void *b, size_t l, int f)
+static size_t dead_read(struct parport *p, void *b, size_t l, int f)
 { return 0; }
 static struct parport_operations dead_ops = {
 	.write_data	= dead_write_lines,	/* data */
@@ -93,7 +93,7 @@ static struct parport_operations dead_ops = {
 	.ecp_write_data	= dead_write,		/* ecp */
 	.ecp_read_data	= dead_read,
 	.ecp_write_addr	= dead_write,
- 
+
 	.compat_write_data	= dead_write,	/* compat */
 	.nibble_read_data	= dead_read,	/* nibble */
 	.byte_read_data		= dead_read,	/* byte */
@@ -148,7 +148,7 @@ void parport_bus_exit(void)
 /*
  * iterates through all the drivers registered with the bus and sends the port
  * details to the match_port callback of the driver, so that the driver can
- * know about the new port that just regsitered with the bus and decide if it
+ * know about the new port that just registered with the bus and decide if it
  * wants to use this new port.
  */
 static int driver_check(struct device_driver *dev_drv, void *_port)
@@ -194,7 +194,7 @@ static void detach_driver_chain(struct parport *port)
 	struct parport_driver *drv;
 	/* caller has exclusive registration_lock */
 	list_for_each_entry(drv, &drivers, list)
-		drv->detach (port);
+		drv->detach(port);
 
 	/*
 	 * call the detach function of the drivers registered in
@@ -205,11 +205,13 @@ static void detach_driver_chain(struct parport *port)
 }
 
 /* Ask kmod for some lowlevel drivers. */
-static void get_lowlevel_driver (void)
+static void get_lowlevel_driver(void)
 {
-	/* There is no actual module called this: you should set
-	 * up an alias for modutils. */
-	request_module ("parport_lowlevel");
+	/*
+	 * There is no actual module called this: you should set
+	 * up an alias for modutils.
+	 */
+	request_module("parport_lowlevel");
 }
 
 /*
@@ -265,7 +267,7 @@ int __parport_register_driver(struct parport_driver *drv, struct module *owner,
 			      const char *mod_name)
 {
 	if (list_empty(&portlist))
-		get_lowlevel_driver ();
+		get_lowlevel_driver();
 
 	if (drv->devmodel) {
 		/* using device model */
@@ -328,7 +330,7 @@ static int port_detach(struct device *dev, void *_drv)
  *	finished by the time this function returns.
  **/
 
-void parport_unregister_driver (struct parport_driver *drv)
+void parport_unregister_driver(struct parport_driver *drv)
 {
 	struct parport *port;
 
@@ -343,6 +345,7 @@ void parport_unregister_driver (struct parport_driver *drv)
 	}
 	mutex_unlock(&registration_lock);
 }
+EXPORT_SYMBOL(parport_unregister_driver);
 
 static void free_port(struct device *dev)
 {
@@ -372,12 +375,13 @@ static void free_port(struct device *dev)
  *	until the matching parport_put_port() call.
  **/
 
-struct parport *parport_get_port (struct parport *port)
+struct parport *parport_get_port(struct parport *port)
 {
 	struct device *dev = get_device(&port->bus_dev);
 
 	return to_parport_dev(dev);
 }
+EXPORT_SYMBOL(parport_get_port);
 
 void parport_del_port(struct parport *port)
 {
@@ -394,10 +398,11 @@ EXPORT_SYMBOL(parport_del_port);
  *	zero (port is no longer used), free_port is called.
  **/
 
-void parport_put_port (struct parport *port)
+void parport_put_port(struct parport *port)
 {
 	put_device(&port->bus_dev);
 }
+EXPORT_SYMBOL(parport_put_port);
 
 /**
  *	parport_register_port - register a parallel port
@@ -439,10 +444,8 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 	int ret;
 
 	tmp = kzalloc(sizeof(struct parport), GFP_KERNEL);
-	if (!tmp) {
-		printk(KERN_WARNING "parport: memory squeeze\n");
+	if (!tmp)
 		return NULL;
-	}
 
 	/* Init our structure */
 	tmp->base = base;
@@ -450,12 +453,12 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 	tmp->dma = dma;
 	tmp->muxport = tmp->daisy = tmp->muxsel = -1;
 	tmp->modes = 0;
- 	INIT_LIST_HEAD(&tmp->list);
+	INIT_LIST_HEAD(&tmp->list);
 	tmp->devices = tmp->cad = NULL;
 	tmp->flags = 0;
 	tmp->ops = ops;
 	tmp->physport = tmp;
-	memset (tmp->probe_info, 0, 5 * sizeof (struct parport_device_info));
+	memset(tmp->probe_info, 0, 5 * sizeof(struct parport_device_info));
 	rwlock_init(&tmp->cad_lock);
 	spin_lock_init(&tmp->waitlist_lock);
 	spin_lock_init(&tmp->pardevice_lock);
@@ -463,12 +466,11 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 	tmp->ieee1284.phase = IEEE1284_PH_FWD_IDLE;
 	sema_init(&tmp->ieee1284.irq, 0);
 	tmp->spintime = parport_default_spintime;
-	atomic_set (&tmp->ref_count, 1);
+	atomic_set(&tmp->ref_count, 1);
 	INIT_LIST_HEAD(&tmp->full_list);
 
 	name = kmalloc(15, GFP_KERNEL);
 	if (!name) {
-		printk(KERN_ERR "parport: memory squeeze\n");
 		kfree(tmp);
 		return NULL;
 	}
@@ -508,6 +510,7 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
 
 	return tmp;
 }
+EXPORT_SYMBOL(parport_register_port);
 
 /**
  *	parport_announce_port - tell device drivers about a parallel port
@@ -521,7 +524,7 @@ struct parport *parport_register_port(unsigned long base, int irq, int dma,
  *	functions will be called, with @port as the parameter.
  **/
 
-void parport_announce_port (struct parport *port)
+void parport_announce_port(struct parport *port)
 {
 	int i;
 
@@ -531,9 +534,8 @@ void parport_announce_port (struct parport *port)
 #endif
 
 	if (!port->dev)
-		printk(KERN_WARNING "%s: fix this legacy "
-				"no-device port driver!\n",
-				port->name);
+		printk(KERN_WARNING "%s: fix this legacy no-device port driver!\n",
+		       port->name);
 
 	parport_proc_register(port);
 	mutex_lock(&registration_lock);
@@ -547,7 +549,7 @@ void parport_announce_port (struct parport *port)
 	spin_unlock_irq(&parportlist_lock);
 
 	/* Let drivers know that new port(s) has arrived. */
-	attach_driver_chain (port);
+	attach_driver_chain(port);
 	for (i = 1; i < 3; i++) {
 		struct parport *slave = port->slaves[i-1];
 		if (slave)
@@ -555,6 +557,7 @@ void parport_announce_port (struct parport *port)
 	}
 	mutex_unlock(&registration_lock);
 }
+EXPORT_SYMBOL(parport_announce_port);
 
 /**
  *	parport_remove_port - deregister a parallel port
@@ -582,7 +585,7 @@ void parport_remove_port(struct parport *port)
 	mutex_lock(&registration_lock);
 
 	/* Spread the word. */
-	detach_driver_chain (port);
+	detach_driver_chain(port);
 
 #ifdef CONFIG_PARPORT_1284
 	/* Forget the IEEE1284.3 topology of the port. */
@@ -616,6 +619,7 @@ void parport_remove_port(struct parport *port)
 			parport_put_port(slave);
 	}
 }
+EXPORT_SYMBOL(parport_remove_port);
 
 /**
  *	parport_register_device - register a device on a parallel port
@@ -689,14 +693,14 @@ void parport_remove_port(struct parport *port)
 struct pardevice *
 parport_register_device(struct parport *port, const char *name,
 			int (*pf)(void *), void (*kf)(void *),
-			void (*irq_func)(void *), 
+			void (*irq_func)(void *),
 			int flags, void *handle)
 {
 	struct pardevice *tmp;
 
 	if (port->physport->flags & PARPORT_FLAG_EXCL) {
 		/* An exclusive device is registered. */
-		printk (KERN_DEBUG "%s: no more devices allowed\n",
+		printk(KERN_DEBUG "%s: no more devices allowed\n",
 			port->name);
 		return NULL;
 	}
@@ -722,28 +726,24 @@ parport_register_device(struct parport *port, const char *name,
 		}
 	}
 
-	/* We up our own module reference count, and that of the port
-           on which a device is to be registered, to ensure that
-           neither of us gets unloaded while we sleep in (e.g.)
-           kmalloc.
-         */
-	if (!try_module_get(port->ops->owner)) {
+	/*
+	 * We up our own module reference count, and that of the port
+	 * on which a device is to be registered, to ensure that
+	 * neither of us gets unloaded while we sleep in (e.g.)
+	 * kmalloc.
+	 */
+	if (!try_module_get(port->ops->owner))
 		return NULL;
-	}
-		
-	parport_get_port (port);
+
+	parport_get_port(port);
 
 	tmp = kmalloc(sizeof(struct pardevice), GFP_KERNEL);
-	if (tmp == NULL) {
-		printk(KERN_WARNING "%s: memory squeeze, couldn't register %s.\n", port->name, name);
+	if (!tmp)
 		goto out;
-	}
 
 	tmp->state = kmalloc(sizeof(struct parport_state), GFP_KERNEL);
-	if (tmp->state == NULL) {
-		printk(KERN_WARNING "%s: memory squeeze, couldn't register %s.\n", port->name, name);
+	if (!tmp->state)
 		goto out_free_pardevice;
-	}
 
 	tmp->name = name;
 	tmp->port = port;
@@ -767,19 +767,21 @@ parport_register_device(struct parport *port, const char *name,
 
 	if (flags & PARPORT_DEV_EXCL) {
 		if (port->physport->devices) {
-			spin_unlock (&port->physport->pardevice_lock);
-			printk (KERN_DEBUG
-				"%s: cannot grant exclusive access for "
-				"device %s\n", port->name, name);
+			spin_unlock(&port->physport->pardevice_lock);
+			printk(KERN_DEBUG
+				"%s: cannot grant exclusive access for device %s\n",
+				port->name, name);
 			goto out_free_all;
 		}
 		port->flags |= PARPORT_FLAG_EXCL;
 	}
 
 	tmp->next = port->physport->devices;
-	wmb(); /* Make sure that tmp->next is written before it's
-                  added to the list; see comments marked 'no locking
-                  required' */
+	wmb(); /*
+		* Make sure that tmp->next is written before it's
+		* added to the list; see comments marked 'no locking
+		* required'
+		*/
 	if (port->physport->devices)
 		port->physport->devices->prev = tmp;
 	port->physport->devices = tmp;
@@ -805,11 +807,12 @@ parport_register_device(struct parport *port, const char *name,
  out_free_pardevice:
 	kfree(tmp);
  out:
-	parport_put_port (port);
+	parport_put_port(port);
 	module_put(port->ops->owner);
 
 	return NULL;
 }
+EXPORT_SYMBOL(parport_register_device);
 
 static void free_pardevice(struct device *dev)
 {
@@ -968,7 +971,7 @@ void parport_unregister_device(struct pardevice *dev)
 	struct parport *port;
 
 #ifdef PARPORT_PARANOID
-	if (dev == NULL) {
+	if (!dev) {
 		printk(KERN_ERR "parport_unregister_device: passed NULL\n");
 		return;
 	}
@@ -985,7 +988,7 @@ void parport_unregister_device(struct pardevice *dev)
 	if (port->cad == dev) {
 		printk(KERN_DEBUG "%s: %s forgot to release port\n",
 		       port->name, dev->name);
-		parport_release (dev);
+		parport_release(dev);
 	}
 
 	spin_lock(&port->pardevice_lock);
@@ -1001,8 +1004,10 @@ void parport_unregister_device(struct pardevice *dev)
 
 	spin_unlock(&port->pardevice_lock);
 
-	/* Make sure we haven't left any pointers around in the wait
-	 * list. */
+	/*
+	 * Make sure we haven't left any pointers around in the wait
+	 * list.
+	 */
 	spin_lock_irq(&port->waitlist_lock);
 	if (dev->waitprev || dev->waitnext || port->waithead == dev) {
 		if (dev->waitprev)
@@ -1023,8 +1028,9 @@ void parport_unregister_device(struct pardevice *dev)
 		kfree(dev);
 
 	module_put(port->ops->owner);
-	parport_put_port (port);
+	parport_put_port(port);
 }
+EXPORT_SYMBOL(parport_unregister_device);
 
 /**
  *	parport_find_number - find a parallel port by number
@@ -1038,23 +1044,24 @@ void parport_unregister_device(struct pardevice *dev)
  *	gives you, use parport_put_port().
  */
 
-struct parport *parport_find_number (int number)
+struct parport *parport_find_number(int number)
 {
 	struct parport *port, *result = NULL;
 
 	if (list_empty(&portlist))
-		get_lowlevel_driver ();
+		get_lowlevel_driver();
 
-	spin_lock (&parportlist_lock);
+	spin_lock(&parportlist_lock);
 	list_for_each_entry(port, &portlist, list) {
 		if (port->number == number) {
-			result = parport_get_port (port);
+			result = parport_get_port(port);
 			break;
 		}
 	}
-	spin_unlock (&parportlist_lock);
+	spin_unlock(&parportlist_lock);
 	return result;
 }
+EXPORT_SYMBOL(parport_find_number);
 
 /**
  *	parport_find_base - find a parallel port by base address
@@ -1068,23 +1075,24 @@ struct parport *parport_find_number (int number)
  *	gives you, use parport_put_port().
  */
 
-struct parport *parport_find_base (unsigned long base)
+struct parport *parport_find_base(unsigned long base)
 {
 	struct parport *port, *result = NULL;
 
 	if (list_empty(&portlist))
-		get_lowlevel_driver ();
+		get_lowlevel_driver();
 
-	spin_lock (&parportlist_lock);
+	spin_lock(&parportlist_lock);
 	list_for_each_entry(port, &portlist, list) {
 		if (port->base == base) {
-			result = parport_get_port (port);
+			result = parport_get_port(port);
 			break;
 		}
 	}
-	spin_unlock (&parportlist_lock);
+	spin_unlock(&parportlist_lock);
 	return result;
 }
+EXPORT_SYMBOL(parport_find_base);
 
 /**
  *	parport_claim - claim access to a parallel port device
@@ -1111,8 +1119,9 @@ int parport_claim(struct pardevice *dev)
 	}
 
 	/* Preempt any current device */
-	write_lock_irqsave (&port->cad_lock, flags);
-	if ((oldcad = port->cad) != NULL) {
+	write_lock_irqsave(&port->cad_lock, flags);
+	oldcad = port->cad;
+	if (oldcad) {
 		if (oldcad->preempt) {
 			if (oldcad->preempt(oldcad->private))
 				goto blocked;
@@ -1121,8 +1130,10 @@ int parport_claim(struct pardevice *dev)
 			goto blocked;
 
 		if (port->cad != oldcad) {
-			/* I think we'll actually deadlock rather than
-                           get here, but just in case.. */
+			/*
+			 * I think we'll actually deadlock rather than
+			 * get here, but just in case..
+			 */
 			printk(KERN_WARNING
 			       "%s: %s released port when preempted!\n",
 			       port->name, oldcad->name);
@@ -1136,7 +1147,7 @@ int parport_claim(struct pardevice *dev)
 		dev->waiting = 0;
 
 		/* Take ourselves out of the wait list again.  */
-		spin_lock_irq (&port->waitlist_lock);
+		spin_lock_irq(&port->waitlist_lock);
 		if (dev->waitprev)
 			dev->waitprev->waitnext = dev->waitnext;
 		else
@@ -1145,7 +1156,7 @@ int parport_claim(struct pardevice *dev)
 			dev->waitnext->waitprev = dev->waitprev;
 		else
 			port->waittail = dev->waitprev;
-		spin_unlock_irq (&port->waitlist_lock);
+		spin_unlock_irq(&port->waitlist_lock);
 		dev->waitprev = dev->waitnext = NULL;
 	}
 
@@ -1162,7 +1173,7 @@ int parport_claim(struct pardevice *dev)
 	/* If it's a daisy chain device, select it. */
 	if (dev->daisy >= 0) {
 		/* This could be lazier. */
-		if (!parport_daisy_select (port, dev->daisy,
+		if (!parport_daisy_select(port, dev->daisy,
 					   IEEE1284_MODE_COMPAT))
 			port->daisy = dev->daisy;
 	}
@@ -1175,13 +1186,15 @@ int parport_claim(struct pardevice *dev)
 	return 0;
 
 blocked:
-	/* If this is the first time we tried to claim the port, register an
-	   interest.  This is only allowed for devices sleeping in
-	   parport_claim_or_block(), or those with a wakeup function.  */
+	/*
+	 * If this is the first time we tried to claim the port, register an
+	 * interest.  This is only allowed for devices sleeping in
+	 * parport_claim_or_block(), or those with a wakeup function.
+	 */
 
 	/* The cad_lock is still held for writing here */
 	if (dev->waiting & 2 || dev->wakeup) {
-		spin_lock (&port->waitlist_lock);
+		spin_lock(&port->waitlist_lock);
 		if (test_and_set_bit(0, &dev->waiting) == 0) {
 			/* First add ourselves to the end of the wait list. */
 			dev->waitnext = NULL;
@@ -1192,11 +1205,12 @@ blocked:
 			} else
 				port->waithead = port->waittail = dev;
 		}
-		spin_unlock (&port->waitlist_lock);
+		spin_unlock(&port->waitlist_lock);
 	}
-	write_unlock_irqrestore (&port->cad_lock, flags);
+	write_unlock_irqrestore(&port->cad_lock, flags);
 	return -EAGAIN;
 }
+EXPORT_SYMBOL(parport_claim);
 
 /**
  *	parport_claim_or_block - claim access to a parallel port device
@@ -1212,8 +1226,10 @@ int parport_claim_or_block(struct pardevice *dev)
 {
 	int r;
 
-	/* Signal to parport_claim() that we can wait even without a
-	   wakeup function.  */
+	/*
+	 * Signal to parport_claim() that we can wait even without a
+	 * wakeup function.
+	 */
 	dev->waiting = 2;
 
 	/* Try to claim the port.  If this fails, we need to sleep.  */
@@ -1231,14 +1247,15 @@ int parport_claim_or_block(struct pardevice *dev)
 		 * See also parport_release()
 		 */
 
-		/* If dev->waiting is clear now, an interrupt
-		   gave us the port and we would deadlock if we slept.  */
+		/*
+		 * If dev->waiting is clear now, an interrupt
+		 * gave us the port and we would deadlock if we slept.
+		 */
 		if (dev->waiting) {
 			wait_event_interruptible(dev->wait_q,
 						 !dev->waiting);
-			if (signal_pending (current)) {
+			if (signal_pending(current))
 				return -EINTR;
-			}
 			r = 1;
 		} else {
 			r = 0;
@@ -1250,15 +1267,15 @@ int parport_claim_or_block(struct pardevice *dev)
 
 #ifdef PARPORT_DEBUG_SHARING
 		if (dev->port->physport->cad != dev)
-			printk(KERN_DEBUG "%s: exiting parport_claim_or_block "
-			       "but %s owns port!\n", dev->name,
-			       dev->port->physport->cad ?
+			printk(KERN_DEBUG "%s: exiting parport_claim_or_block but %s owns port!\n",
+			       dev->name, dev->port->physport->cad ?
 			       dev->port->physport->cad->name:"nobody");
 #endif
 	}
 	dev->waiting = 0;
 	return r;
 }
+EXPORT_SYMBOL(parport_claim_or_block);
 
 /**
  *	parport_release - give up access to a parallel port device
@@ -1278,9 +1295,9 @@ void parport_release(struct pardevice *dev)
 	/* Make sure that dev is the current device */
 	write_lock_irqsave(&port->cad_lock, flags);
 	if (port->cad != dev) {
-		write_unlock_irqrestore (&port->cad_lock, flags);
-		printk(KERN_WARNING "%s: %s tried to release parport "
-		       "when not owner\n", port->name, dev->name);
+		write_unlock_irqrestore(&port->cad_lock, flags);
+		printk(KERN_WARNING "%s: %s tried to release parport when not owner\n",
+		       port->name, dev->name);
 		return;
 	}
 
@@ -1293,7 +1310,7 @@ void parport_release(struct pardevice *dev)
 
 	/* If this is a daisy device, deselect it. */
 	if (dev->daisy >= 0) {
-		parport_daisy_deselect_all (port);
+		parport_daisy_deselect_all(port);
 		port->daisy = -1;
 	}
 #endif
@@ -1304,8 +1321,10 @@ void parport_release(struct pardevice *dev)
 	/* Save control registers */
 	port->ops->save_state(port, dev->state);
 
-	/* If anybody is waiting, find out who's been there longest and
-	   then wake them up. (Note: no locking required) */
+	/*
+	 * If anybody is waiting, find out who's been there longest and
+	 * then wake them up. (Note: no locking required)
+	 */
 	/* !!! LOCKING IS NEEDED HERE */
 	for (pd = port->waithead; pd; pd = pd->waitnext) {
 		if (pd->waiting & 2) { /* sleeping in claim_or_block */
@@ -1322,14 +1341,17 @@ void parport_release(struct pardevice *dev)
 		}
 	}
 
-	/* Nobody was waiting, so walk the list to see if anyone is
-	   interested in being woken up. (Note: no locking required) */
+	/*
+	 * Nobody was waiting, so walk the list to see if anyone is
+	 * interested in being woken up. (Note: no locking required)
+	 */
 	/* !!! LOCKING IS NEEDED HERE */
-	for (pd = port->devices; (port->cad == NULL) && pd; pd = pd->next) {
+	for (pd = port->devices; !port->cad && pd; pd = pd->next) {
 		if (pd->wakeup && pd != dev)
 			pd->wakeup(pd->private);
 	}
 }
+EXPORT_SYMBOL(parport_release);
 
 irqreturn_t parport_irq_handler(int irq, void *dev_id)
 {
@@ -1339,22 +1361,6 @@ irqreturn_t parport_irq_handler(int irq, void *dev_id)
 
 	return IRQ_HANDLED;
 }
-
-/* Exported symbols for modules. */
-
-EXPORT_SYMBOL(parport_claim);
-EXPORT_SYMBOL(parport_claim_or_block);
-EXPORT_SYMBOL(parport_release);
-EXPORT_SYMBOL(parport_register_port);
-EXPORT_SYMBOL(parport_announce_port);
-EXPORT_SYMBOL(parport_remove_port);
-EXPORT_SYMBOL(parport_unregister_driver);
-EXPORT_SYMBOL(parport_register_device);
-EXPORT_SYMBOL(parport_unregister_device);
-EXPORT_SYMBOL(parport_get_port);
-EXPORT_SYMBOL(parport_put_port);
-EXPORT_SYMBOL(parport_find_number);
-EXPORT_SYMBOL(parport_find_base);
 EXPORT_SYMBOL(parport_irq_handler);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/pci/access.c b/drivers/pci/access.c
index 59ac36fe7c42..8c05b5ceeaec 100644
--- a/drivers/pci/access.c
+++ b/drivers/pci/access.c
@@ -25,7 +25,7 @@ DEFINE_RAW_SPINLOCK(pci_lock);
 #define PCI_word_BAD (pos & 1)
 #define PCI_dword_BAD (pos & 3)
 
-#define PCI_OP_READ(size,type,len) \
+#define PCI_OP_READ(size, type, len) \
 int pci_bus_read_config_##size \
 	(struct pci_bus *bus, unsigned int devfn, int pos, type *value)	\
 {									\
@@ -40,7 +40,7 @@ int pci_bus_read_config_##size \
 	return res;							\
 }
 
-#define PCI_OP_WRITE(size,type,len) \
+#define PCI_OP_WRITE(size, type, len) \
 int pci_bus_write_config_##size \
 	(struct pci_bus *bus, unsigned int devfn, int pos, type value)	\
 {									\
@@ -231,7 +231,7 @@ static noinline void pci_wait_cfg(struct pci_dev *dev)
 }
 
 /* Returns 0 on success, negative values indicate error. */
-#define PCI_USER_READ_CONFIG(size,type)					\
+#define PCI_USER_READ_CONFIG(size, type)					\
 int pci_user_read_config_##size						\
 	(struct pci_dev *dev, int pos, type *val)			\
 {									\
@@ -251,7 +251,7 @@ int pci_user_read_config_##size						\
 EXPORT_SYMBOL_GPL(pci_user_read_config_##size);
 
 /* Returns 0 on success, negative values indicate error. */
-#define PCI_USER_WRITE_CONFIG(size,type)				\
+#define PCI_USER_WRITE_CONFIG(size, type)				\
 int pci_user_write_config_##size					\
 	(struct pci_dev *dev, int pos, type val)			\
 {									\
diff --git a/drivers/pci/bus.c b/drivers/pci/bus.c
index d3346d23963b..89b3befc7155 100644
--- a/drivers/pci/bus.c
+++ b/drivers/pci/bus.c
@@ -140,6 +140,8 @@ static int pci_bus_alloc_from_region(struct pci_bus *bus, struct resource *res,
 	type_mask |= IORESOURCE_TYPE_BITS;
 
 	pci_bus_for_each_resource(bus, r, i) {
+		resource_size_t min_used = min;
+
 		if (!r)
 			continue;
 
@@ -163,12 +165,12 @@ static int pci_bus_alloc_from_region(struct pci_bus *bus, struct resource *res,
 		 * overrides "min".
 		 */
 		if (avail.start)
-			min = avail.start;
+			min_used = avail.start;
 
 		max = avail.end;
 
 		/* Ok, try it out.. */
-		ret = allocate_resource(r, res, size, min, max,
+		ret = allocate_resource(r, res, size, min_used, max,
 					align, alignf, alignf_data);
 		if (ret == 0)
 			return 0;
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index c0ad9aaa16a7..d1cdd9c992ac 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -14,6 +14,7 @@ config PCI_DRA7XX
 config PCI_MVEBU
 	bool "Marvell EBU PCIe controller"
 	depends on ARCH_MVEBU || ARCH_DOVE
+	depends on ARM
 	depends on OF
 
 config PCIE_DW
@@ -49,8 +50,7 @@ config PCI_RCAR_GEN2
 
 config PCI_RCAR_GEN2_PCIE
 	bool "Renesas R-Car PCIe controller"
-	depends on ARM
-	depends on ARCH_SHMOBILE || COMPILE_TEST
+	depends on ARCH_SHMOBILE || (ARM && COMPILE_TEST)
 	help
 	  Say Y here if you want PCIe controller support on R-Car Gen2 SoCs.
 
@@ -119,13 +119,11 @@ config PCI_VERSATILE
 	depends on ARCH_VERSATILE
 
 config PCIE_IPROC
-	tristate "Broadcom iProc PCIe controller"
-	depends on OF && (ARM || ARM64)
-	default n
+	tristate
 	help
 	  This enables the iProc PCIe core controller support for Broadcom's
-	  iProc family of SoCs. An appropriate bus interface driver also needs
-	  to be enabled
+	  iProc family of SoCs. An appropriate bus interface driver needs
+	  to be enabled to select this.
 
 config PCIE_IPROC_PLATFORM
 	tristate "Broadcom iProc PCIe platform bus driver"
@@ -148,6 +146,16 @@ config PCIE_IPROC_BCMA
 	  Say Y here if you want to use the Broadcom iProc PCIe controller
 	  through the BCMA bus interface
 
+config PCIE_IPROC_MSI
+	bool "Broadcom iProc PCIe MSI support"
+	depends on PCIE_IPROC_PLATFORM || PCIE_IPROC_BCMA
+	depends on PCI_MSI
+	select PCI_MSI_IRQ_DOMAIN
+	default ARCH_BCM_IPROC
+	help
+	  Say Y here if you want to enable MSI support for Broadcom's iProc
+	  PCIe controller
+
 config PCIE_ALTERA
 	bool "Altera PCIe controller"
 	depends on ARM || NIOS2
@@ -167,10 +175,21 @@ config PCIE_ALTERA_MSI
 
 config PCI_HISI
 	depends on OF && ARM64
-	bool "HiSilicon SoC HIP05 PCIe controller"
+	bool "HiSilicon Hip05 and Hip06 SoCs PCIe controllers"
 	select PCIEPORTBUS
 	select PCIE_DW
 	help
-	  Say Y here if you want PCIe controller support on HiSilicon HIP05 SoC
+	  Say Y here if you want PCIe controller support on HiSilicon
+	  Hip05 and Hip06 SoCs
+
+config PCIE_QCOM
+	bool "Qualcomm PCIe controller"
+	depends on ARCH_QCOM && OF
+	select PCIE_DW
+	select PCIEPORTBUS
+	help
+	  Say Y here to enable PCIe controller support on Qualcomm SoCs. The
+	  PCIe controller uses the Designware core plus Qualcomm-specific
+	  hardware wrappers.
 
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index 9d4d3c6924a1..7b2f20c6ccc6 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -15,8 +15,10 @@ obj-$(CONFIG_PCI_XGENE_MSI) += pci-xgene-msi.o
 obj-$(CONFIG_PCI_LAYERSCAPE) += pci-layerscape.o
 obj-$(CONFIG_PCI_VERSATILE) += pci-versatile.o
 obj-$(CONFIG_PCIE_IPROC) += pcie-iproc.o
+obj-$(CONFIG_PCIE_IPROC_MSI) += pcie-iproc-msi.o
 obj-$(CONFIG_PCIE_IPROC_PLATFORM) += pcie-iproc-platform.o
 obj-$(CONFIG_PCIE_IPROC_BCMA) += pcie-iproc-bcma.o
 obj-$(CONFIG_PCIE_ALTERA) += pcie-altera.o
 obj-$(CONFIG_PCIE_ALTERA_MSI) += pcie-altera-msi.o
 obj-$(CONFIG_PCI_HISI) += pcie-hisi.o
+obj-$(CONFIG_PCIE_QCOM) += pcie-qcom.o
diff --git a/drivers/pci/host/pci-dra7xx.c b/drivers/pci/host/pci-dra7xx.c
index 8c3688046c02..923607bdabc5 100644
--- a/drivers/pci/host/pci-dra7xx.c
+++ b/drivers/pci/host/pci-dra7xx.c
@@ -302,7 +302,8 @@ static int __init dra7xx_add_pcie_port(struct dra7xx_pcie *dra7xx,
 	}
 
 	ret = devm_request_irq(&pdev->dev, pp->irq,
-			       dra7xx_pcie_msi_irq_handler, IRQF_SHARED,
+			       dra7xx_pcie_msi_irq_handler,
+			       IRQF_SHARED | IRQF_NO_THREAD,
 			       "dra7-pcie-msi",	pp);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to request irq\n");
diff --git a/drivers/pci/host/pci-exynos.c b/drivers/pci/host/pci-exynos.c
index 01095e1160a4..d997d22d4231 100644
--- a/drivers/pci/host/pci-exynos.c
+++ b/drivers/pci/host/pci-exynos.c
@@ -522,7 +522,8 @@ static int __init exynos_add_pcie_port(struct pcie_port *pp,
 
 		ret = devm_request_irq(&pdev->dev, pp->msi_irq,
 					exynos_pcie_msi_irq_handler,
-					IRQF_SHARED, "exynos-pcie", pp);
+					IRQF_SHARED | IRQF_NO_THREAD,
+					"exynos-pcie", pp);
 		if (ret) {
 			dev_err(&pdev->dev, "failed to request msi irq\n");
 			return ret;
diff --git a/drivers/pci/host/pci-host-generic.c b/drivers/pci/host/pci-host-generic.c
index 5434c90db243..1652bc70b145 100644
--- a/drivers/pci/host/pci-host-generic.c
+++ b/drivers/pci/host/pci-host-generic.c
@@ -38,16 +38,7 @@ struct gen_pci_cfg_windows {
 	struct gen_pci_cfg_bus_ops		*ops;
 };
 
-/*
- * ARM pcibios functions expect the ARM struct pci_sys_data as the PCI
- * sysdata.  Add pci_sys_data as the first element in struct gen_pci so
- * that when we use a gen_pci pointer as sysdata, it is also a pointer to
- * a struct pci_sys_data.
- */
 struct gen_pci {
-#ifdef CONFIG_ARM
-	struct pci_sys_data			sys;
-#endif
 	struct pci_host_bridge			host;
 	struct gen_pci_cfg_windows		cfg;
 	struct list_head			resources;
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index 22e8224126fd..fe600964fa50 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -32,7 +32,7 @@
 #define to_imx6_pcie(x)	container_of(x, struct imx6_pcie, pp)
 
 struct imx6_pcie {
-	int			reset_gpio;
+	struct gpio_desc	*reset_gpio;
 	struct clk		*pcie_bus;
 	struct clk		*pcie_phy;
 	struct clk		*pcie;
@@ -122,7 +122,7 @@ static int pcie_phy_wait_ack(void __iomem *dbi_base, int addr)
 }
 
 /* Read from the 16-bit PCIe PHY control registers (not memory-mapped) */
-static int pcie_phy_read(void __iomem *dbi_base, int addr , int *data)
+static int pcie_phy_read(void __iomem *dbi_base, int addr, int *data)
 {
 	u32 val, phy_ctl;
 	int ret;
@@ -287,10 +287,10 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	usleep_range(200, 500);
 
 	/* Some boards don't have PCIe reset GPIO. */
-	if (gpio_is_valid(imx6_pcie->reset_gpio)) {
-		gpio_set_value(imx6_pcie->reset_gpio, 0);
+	if (imx6_pcie->reset_gpio) {
+		gpiod_set_value_cansleep(imx6_pcie->reset_gpio, 0);
 		msleep(100);
-		gpio_set_value(imx6_pcie->reset_gpio, 1);
+		gpiod_set_value_cansleep(imx6_pcie->reset_gpio, 1);
 	}
 	return 0;
 
@@ -537,7 +537,8 @@ static int __init imx6_add_pcie_port(struct pcie_port *pp,
 
 		ret = devm_request_irq(&pdev->dev, pp->msi_irq,
 				       imx6_pcie_msi_handler,
-				       IRQF_SHARED, "mx6-pcie-msi", pp);
+				       IRQF_SHARED | IRQF_NO_THREAD,
+				       "mx6-pcie-msi", pp);
 		if (ret) {
 			dev_err(&pdev->dev, "failed to request MSI irq\n");
 			return ret;
@@ -560,7 +561,6 @@ static int __init imx6_pcie_probe(struct platform_device *pdev)
 {
 	struct imx6_pcie *imx6_pcie;
 	struct pcie_port *pp;
-	struct device_node *np = pdev->dev.of_node;
 	struct resource *dbi_base;
 	int ret;
 
@@ -581,15 +581,8 @@ static int __init imx6_pcie_probe(struct platform_device *pdev)
 		return PTR_ERR(pp->dbi_base);
 
 	/* Fetch GPIOs */
-	imx6_pcie->reset_gpio = of_get_named_gpio(np, "reset-gpio", 0);
-	if (gpio_is_valid(imx6_pcie->reset_gpio)) {
-		ret = devm_gpio_request_one(&pdev->dev, imx6_pcie->reset_gpio,
-					    GPIOF_OUT_INIT_LOW, "PCIe reset");
-		if (ret) {
-			dev_err(&pdev->dev, "unable to get reset gpio\n");
-			return ret;
-		}
-	}
+	imx6_pcie->reset_gpio = devm_gpiod_get_optional(&pdev->dev, "reset",
+							GPIOD_OUT_LOW);
 
 	/* Fetch clocks */
 	imx6_pcie->pcie_phy = devm_clk_get(&pdev->dev, "pcie_phy");
diff --git a/drivers/pci/host/pci-keystone-dw.c b/drivers/pci/host/pci-keystone-dw.c
index ed34c9520a02..6153853ca9c3 100644
--- a/drivers/pci/host/pci-keystone-dw.c
+++ b/drivers/pci/host/pci-keystone-dw.c
@@ -58,11 +58,6 @@
 
 #define to_keystone_pcie(x)	container_of(x, struct keystone_pcie, pp)
 
-static inline struct pcie_port *sys_to_pcie(struct pci_sys_data *sys)
-{
-	return sys->private_data;
-}
-
 static inline void update_reg_offset_bit_pos(u32 offset, u32 *reg_offset,
 					     u32 *bit_pos)
 {
@@ -108,7 +103,7 @@ static void ks_dw_pcie_msi_irq_ack(struct irq_data *d)
 	struct pcie_port *pp;
 
 	msi = irq_data_get_msi_desc(d);
-	pp = sys_to_pcie(msi_desc_to_pci_sysdata(msi));
+	pp = (struct pcie_port *) msi_desc_to_pci_sysdata(msi);
 	ks_pcie = to_keystone_pcie(pp);
 	offset = d->irq - irq_linear_revmap(pp->irq_domain, 0);
 	update_reg_offset_bit_pos(offset, &reg_offset, &bit_pos);
@@ -146,7 +141,7 @@ static void ks_dw_pcie_msi_irq_mask(struct irq_data *d)
 	u32 offset;
 
 	msi = irq_data_get_msi_desc(d);
-	pp = sys_to_pcie(msi_desc_to_pci_sysdata(msi));
+	pp = (struct pcie_port *) msi_desc_to_pci_sysdata(msi);
 	ks_pcie = to_keystone_pcie(pp);
 	offset = d->irq - irq_linear_revmap(pp->irq_domain, 0);
 
@@ -167,7 +162,7 @@ static void ks_dw_pcie_msi_irq_unmask(struct irq_data *d)
 	u32 offset;
 
 	msi = irq_data_get_msi_desc(d);
-	pp = sys_to_pcie(msi_desc_to_pci_sysdata(msi));
+	pp = (struct pcie_port *) msi_desc_to_pci_sysdata(msi);
 	ks_pcie = to_keystone_pcie(pp);
 	offset = d->irq - irq_linear_revmap(pp->irq_domain, 0);
 
diff --git a/drivers/pci/host/pci-layerscape.c b/drivers/pci/host/pci-layerscape.c
index 3923bed93c7e..f39961bcf7aa 100644
--- a/drivers/pci/host/pci-layerscape.c
+++ b/drivers/pci/host/pci-layerscape.c
@@ -77,6 +77,16 @@ static void ls_pcie_fix_class(struct ls_pcie *pcie)
 	iowrite16(PCI_CLASS_BRIDGE_PCI, pcie->dbi + PCI_CLASS_DEVICE);
 }
 
+/* Drop MSG TLP except for Vendor MSG */
+static void ls_pcie_drop_msg_tlp(struct ls_pcie *pcie)
+{
+	u32 val;
+
+	val = ioread32(pcie->dbi + PCIE_STRFMR1);
+	val &= 0xDFFFFFFF;
+	iowrite32(val, pcie->dbi + PCIE_STRFMR1);
+}
+
 static int ls1021_pcie_link_up(struct pcie_port *pp)
 {
 	u32 state;
@@ -97,7 +107,7 @@ static int ls1021_pcie_link_up(struct pcie_port *pp)
 static void ls1021_pcie_host_init(struct pcie_port *pp)
 {
 	struct ls_pcie *pcie = to_ls_pcie(pp);
-	u32 val, index[2];
+	u32 index[2];
 
 	pcie->scfg = syscon_regmap_lookup_by_phandle(pp->dev->of_node,
 						     "fsl,pcie-scfg");
@@ -116,13 +126,7 @@ static void ls1021_pcie_host_init(struct pcie_port *pp)
 
 	dw_pcie_setup_rc(pp);
 
-	/*
-	 * LS1021A Workaround for internal TKT228622
-	 * to fix the INTx hang issue
-	 */
-	val = ioread32(pcie->dbi + PCIE_STRFMR1);
-	val &= 0xffff;
-	iowrite32(val, pcie->dbi + PCIE_STRFMR1);
+	ls_pcie_drop_msg_tlp(pcie);
 }
 
 static int ls_pcie_link_up(struct pcie_port *pp)
@@ -147,6 +151,7 @@ static void ls_pcie_host_init(struct pcie_port *pp)
 	iowrite32(1, pcie->dbi + PCIE_DBI_RO_WR_EN);
 	ls_pcie_fix_class(pcie);
 	ls_pcie_clear_multifunction(pcie);
+	ls_pcie_drop_msg_tlp(pcie);
 	iowrite32(0, pcie->dbi + PCIE_DBI_RO_WR_EN);
 }
 
diff --git a/drivers/pci/host/pci-rcar-gen2.c b/drivers/pci/host/pci-rcar-gen2.c
index c4f64bfee551..9980a4bdae7e 100644
--- a/drivers/pci/host/pci-rcar-gen2.c
+++ b/drivers/pci/host/pci-rcar-gen2.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/of_address.h>
 #include <linux/of_pci.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
@@ -102,6 +103,8 @@ struct rcar_pci_priv {
 	unsigned busnr;
 	int irq;
 	unsigned long window_size;
+	unsigned long window_addr;
+	unsigned long window_pci;
 };
 
 /* PCI configuration space operations */
@@ -239,8 +242,8 @@ static int rcar_pci_setup(int nr, struct pci_sys_data *sys)
 	       RCAR_PCI_ARBITER_PCIBP_MODE;
 	iowrite32(val, reg + RCAR_PCI_ARBITER_CTR_REG);
 
-	/* PCI-AHB mapping: 0x40000000 base */
-	iowrite32(0x40000000 | RCAR_PCIAHB_PREFETCH16,
+	/* PCI-AHB mapping */
+	iowrite32(priv->window_addr | RCAR_PCIAHB_PREFETCH16,
 		  reg + RCAR_PCIAHB_WIN1_CTR_REG);
 
 	/* AHB-PCI mapping: OHCI/EHCI registers */
@@ -251,7 +254,7 @@ static int rcar_pci_setup(int nr, struct pci_sys_data *sys)
 	iowrite32(RCAR_AHBPCI_WIN1_HOST | RCAR_AHBPCI_WIN_CTR_CFG,
 		  reg + RCAR_AHBPCI_WIN1_CTR_REG);
 	/* Set PCI-AHB Window1 address */
-	iowrite32(0x40000000 | PCI_BASE_ADDRESS_MEM_PREFETCH,
+	iowrite32(priv->window_pci | PCI_BASE_ADDRESS_MEM_PREFETCH,
 		  reg + PCI_BASE_ADDRESS_1);
 	/* Set AHB-PCI bridge PCI communication area address */
 	val = priv->cfg_res->start + RCAR_AHBPCI_PCICOM_OFFSET;
@@ -284,6 +287,64 @@ static struct pci_ops rcar_pci_ops = {
 	.write	= pci_generic_config_write,
 };
 
+static int pci_dma_range_parser_init(struct of_pci_range_parser *parser,
+				     struct device_node *node)
+{
+	const int na = 3, ns = 2;
+	int rlen;
+
+	parser->node = node;
+	parser->pna = of_n_addr_cells(node);
+	parser->np = parser->pna + na + ns;
+
+	parser->range = of_get_property(node, "dma-ranges", &rlen);
+	if (!parser->range)
+		return -ENOENT;
+
+	parser->end = parser->range + rlen / sizeof(__be32);
+	return 0;
+}
+
+static int rcar_pci_parse_map_dma_ranges(struct rcar_pci_priv *pci,
+					 struct device_node *np)
+{
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	int index = 0;
+
+	/* Failure to parse is ok as we fall back to defaults */
+	if (pci_dma_range_parser_init(&parser, np))
+		return 0;
+
+	/* Get the dma-ranges from DT */
+	for_each_of_pci_range(&parser, &range) {
+		/* Hardware only allows one inbound 32-bit range */
+		if (index)
+			return -EINVAL;
+
+		pci->window_addr = (unsigned long)range.cpu_addr;
+		pci->window_pci = (unsigned long)range.pci_addr;
+		pci->window_size = (unsigned long)range.size;
+
+		/* Catch HW limitations */
+		if (!(range.flags & IORESOURCE_PREFETCH)) {
+			dev_err(pci->dev, "window must be prefetchable\n");
+			return -EINVAL;
+		}
+		if (pci->window_addr) {
+			u32 lowaddr = 1 << (ffs(pci->window_addr) - 1);
+
+			if (lowaddr < pci->window_size) {
+				dev_err(pci->dev, "invalid window size/addr\n");
+				return -EINVAL;
+			}
+		}
+		index++;
+	}
+
+	return 0;
+}
+
 static int rcar_pci_probe(struct platform_device *pdev)
 {
 	struct resource *cfg_res, *mem_res;
@@ -329,6 +390,9 @@ static int rcar_pci_probe(struct platform_device *pdev)
 		return priv->irq;
 	}
 
+	/* default window addr and size if not specified in DT */
+	priv->window_addr = 0x40000000;
+	priv->window_pci = 0x40000000;
 	priv->window_size = SZ_1G;
 
 	if (pdev->dev.of_node) {
@@ -344,6 +408,12 @@ static int rcar_pci_probe(struct platform_device *pdev)
 		priv->busnr = busnr.start;
 		if (busnr.end != busnr.start)
 			dev_warn(&pdev->dev, "only one bus number supported\n");
+
+		ret = rcar_pci_parse_map_dma_ranges(priv, pdev->dev.of_node);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to parse dma-range\n");
+			return ret;
+		}
 	} else {
 		priv->busnr = pdev->id;
 	}
@@ -360,6 +430,7 @@ static int rcar_pci_probe(struct platform_device *pdev)
 }
 
 static struct of_device_id rcar_pci_of_match[] = {
+	{ .compatible = "renesas,pci-rcar-gen2", },
 	{ .compatible = "renesas,pci-r8a7790", },
 	{ .compatible = "renesas,pci-r8a7791", },
 	{ .compatible = "renesas,pci-r8a7794", },
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index 3018ae52e092..30323114c53c 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -1288,7 +1288,7 @@ static int tegra_pcie_enable_msi(struct tegra_pcie *pcie)
 
 	msi->irq = err;
 
-	err = request_irq(msi->irq, tegra_pcie_msi_irq, 0,
+	err = request_irq(msi->irq, tegra_pcie_msi_irq, IRQF_NO_THREAD,
 			  tegra_msi_irq_chip.name, pcie);
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
diff --git a/drivers/pci/host/pci-versatile.c b/drivers/pci/host/pci-versatile.c
index 0863d9cc25f8..f843a72dc51c 100644
--- a/drivers/pci/host/pci-versatile.c
+++ b/drivers/pci/host/pci-versatile.c
@@ -125,9 +125,6 @@ out_release_res:
 	return err;
 }
 
-/* Unused, temporary to satisfy ARM arch code */
-struct pci_sys_data sys;
-
 static int versatile_pci_probe(struct platform_device *pdev)
 {
 	struct resource *res;
@@ -208,7 +205,7 @@ static int versatile_pci_probe(struct platform_device *pdev)
 	pci_add_flags(PCI_ENABLE_PROC_DOMAINS);
 	pci_add_flags(PCI_REASSIGN_ALL_BUS | PCI_REASSIGN_ALL_RSRC);
 
-	bus = pci_scan_root_bus(&pdev->dev, 0, &pci_versatile_ops, &sys, &pci_res);
+	bus = pci_scan_root_bus(&pdev->dev, 0, &pci_versatile_ops, NULL, &pci_res);
 	if (!bus)
 		return -ENOMEM;
 
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index 02a7452bdf23..21716827847a 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -128,32 +128,26 @@ static inline void dw_pcie_writel_rc(struct pcie_port *pp, u32 val, u32 reg)
 static int dw_pcie_rd_own_conf(struct pcie_port *pp, int where, int size,
 			       u32 *val)
 {
-	int ret;
-
 	if (pp->ops->rd_own_conf)
-		ret = pp->ops->rd_own_conf(pp, where, size, val);
-	else
-		ret = dw_pcie_cfg_read(pp->dbi_base + where, size, val);
+		return pp->ops->rd_own_conf(pp, where, size, val);
 
-	return ret;
+	return dw_pcie_cfg_read(pp->dbi_base + where, size, val);
 }
 
 static int dw_pcie_wr_own_conf(struct pcie_port *pp, int where, int size,
 			       u32 val)
 {
-	int ret;
-
 	if (pp->ops->wr_own_conf)
-		ret = pp->ops->wr_own_conf(pp, where, size, val);
-	else
-		ret = dw_pcie_cfg_write(pp->dbi_base + where, size, val);
+		return pp->ops->wr_own_conf(pp, where, size, val);
 
-	return ret;
+	return dw_pcie_cfg_write(pp->dbi_base + where, size, val);
 }
 
 static void dw_pcie_prog_outbound_atu(struct pcie_port *pp, int index,
 		int type, u64 cpu_addr, u64 pci_addr, u32 size)
 {
+	u32 val;
+
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | index,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, lower_32_bits(cpu_addr), PCIE_ATU_LOWER_BASE);
@@ -164,6 +158,12 @@ static void dw_pcie_prog_outbound_atu(struct pcie_port *pp, int index,
 	dw_pcie_writel_rc(pp, upper_32_bits(pci_addr), PCIE_ATU_UPPER_TARGET);
 	dw_pcie_writel_rc(pp, type, PCIE_ATU_CR1);
 	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
+
+	/*
+	 * Make sure ATU enable takes effect before any subsequent config
+	 * and I/O accesses.
+	 */
+	dw_pcie_readl_rc(pp, PCIE_ATU_CR2, &val);
 }
 
 static struct irq_chip dw_msi_irq_chip = {
@@ -384,8 +384,8 @@ int dw_pcie_link_up(struct pcie_port *pp)
 {
 	if (pp->ops->link_up)
 		return pp->ops->link_up(pp);
-	else
-		return 0;
+
+	return 0;
 }
 
 static int dw_pcie_msi_map(struct irq_domain *domain, unsigned int irq,
@@ -571,6 +571,9 @@ static int dw_pcie_rd_other_conf(struct pcie_port *pp, struct pci_bus *bus,
 	u64 cpu_addr;
 	void __iomem *va_cfg_base;
 
+	if (pp->ops->rd_other_conf)
+		return pp->ops->rd_other_conf(pp, bus, devfn, where, size, val);
+
 	busdev = PCIE_ATU_BUS(bus->number) | PCIE_ATU_DEV(PCI_SLOT(devfn)) |
 		 PCIE_ATU_FUNC(PCI_FUNC(devfn));
 
@@ -605,6 +608,9 @@ static int dw_pcie_wr_other_conf(struct pcie_port *pp, struct pci_bus *bus,
 	u64 cpu_addr;
 	void __iomem *va_cfg_base;
 
+	if (pp->ops->wr_other_conf)
+		return pp->ops->wr_other_conf(pp, bus, devfn, where, size, val);
+
 	busdev = PCIE_ATU_BUS(bus->number) | PCIE_ATU_DEV(PCI_SLOT(devfn)) |
 		 PCIE_ATU_FUNC(PCI_FUNC(devfn));
 
@@ -658,46 +664,30 @@ static int dw_pcie_rd_conf(struct pci_bus *bus, u32 devfn, int where,
 			int size, u32 *val)
 {
 	struct pcie_port *pp = bus->sysdata;
-	int ret;
 
 	if (dw_pcie_valid_config(pp, bus, PCI_SLOT(devfn)) == 0) {
 		*val = 0xffffffff;
 		return PCIBIOS_DEVICE_NOT_FOUND;
 	}
 
-	if (bus->number != pp->root_bus_nr)
-		if (pp->ops->rd_other_conf)
-			ret = pp->ops->rd_other_conf(pp, bus, devfn,
-						where, size, val);
-		else
-			ret = dw_pcie_rd_other_conf(pp, bus, devfn,
-						where, size, val);
-	else
-		ret = dw_pcie_rd_own_conf(pp, where, size, val);
+	if (bus->number == pp->root_bus_nr)
+		return dw_pcie_rd_own_conf(pp, where, size, val);
 
-	return ret;
+	return dw_pcie_rd_other_conf(pp, bus, devfn, where, size, val);
 }
 
 static int dw_pcie_wr_conf(struct pci_bus *bus, u32 devfn,
 			int where, int size, u32 val)
 {
 	struct pcie_port *pp = bus->sysdata;
-	int ret;
 
 	if (dw_pcie_valid_config(pp, bus, PCI_SLOT(devfn)) == 0)
 		return PCIBIOS_DEVICE_NOT_FOUND;
 
-	if (bus->number != pp->root_bus_nr)
-		if (pp->ops->wr_other_conf)
-			ret = pp->ops->wr_other_conf(pp, bus, devfn,
-						where, size, val);
-		else
-			ret = dw_pcie_wr_other_conf(pp, bus, devfn,
-						where, size, val);
-	else
-		ret = dw_pcie_wr_own_conf(pp, where, size, val);
+	if (bus->number == pp->root_bus_nr)
+		return dw_pcie_wr_own_conf(pp, where, size, val);
 
-	return ret;
+	return dw_pcie_wr_other_conf(pp, bus, devfn, where, size, val);
 }
 
 static struct pci_ops dw_pcie_ops = {
diff --git a/drivers/pci/host/pcie-hisi.c b/drivers/pci/host/pcie-hisi.c
index 77f7c669a1b9..3e98d4edae2d 100644
--- a/drivers/pci/host/pcie-hisi.c
+++ b/drivers/pci/host/pcie-hisi.c
@@ -1,10 +1,11 @@
 /*
- * PCIe host controller driver for HiSilicon Hip05 SoC
+ * PCIe host controller driver for HiSilicon SoCs
  *
  * Copyright (C) 2015 HiSilicon Co., Ltd. http://www.hisilicon.com
  *
- * Author: Zhou Wang <wangzhou1@hisilicon.com>
- *         Dacai Zhu <zhudacai@hisilicon.com>
+ * Authors: Zhou Wang <wangzhou1@hisilicon.com>
+ *          Dacai Zhu <zhudacai@hisilicon.com>
+ *          Gabriele Paoloni <gabriele.paoloni@huawei.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -16,21 +17,31 @@
 #include <linux/of_address.h>
 #include <linux/of_pci.h>
 #include <linux/platform_device.h>
+#include <linux/of_device.h>
 #include <linux/regmap.h>
 
 #include "pcie-designware.h"
 
-#define PCIE_SUBCTRL_SYS_STATE4_REG                     0x6818
-#define PCIE_LTSSM_LINKUP_STATE                         0x11
-#define PCIE_LTSSM_STATE_MASK                           0x3F
+#define PCIE_LTSSM_LINKUP_STATE				0x11
+#define PCIE_LTSSM_STATE_MASK				0x3F
+#define PCIE_SUBCTRL_SYS_STATE4_REG			0x6818
+#define PCIE_SYS_STATE4						0x31c
+#define PCIE_HIP06_CTRL_OFF					0x1000
 
 #define to_hisi_pcie(x)	container_of(x, struct hisi_pcie, pp)
 
+struct hisi_pcie;
+
+struct pcie_soc_ops {
+	int (*hisi_pcie_link_up)(struct hisi_pcie *pcie);
+};
+
 struct hisi_pcie {
 	struct regmap *subctrl;
 	void __iomem *reg_base;
 	u32 port_id;
 	struct pcie_port pp;
+	struct pcie_soc_ops *soc_ops;
 };
 
 static inline void hisi_pcie_apb_writel(struct hisi_pcie *pcie,
@@ -44,7 +55,7 @@ static inline u32 hisi_pcie_apb_readl(struct hisi_pcie *pcie, u32 reg)
 	return readl(pcie->reg_base + reg);
 }
 
-/* Hip05 PCIe host only supports 32-bit config access */
+/* HipXX PCIe host only supports 32-bit config access */
 static int hisi_pcie_cfg_read(struct pcie_port *pp, int where, int size,
 			      u32 *val)
 {
@@ -69,7 +80,7 @@ static int hisi_pcie_cfg_read(struct pcie_port *pp, int where, int size,
 	return PCIBIOS_SUCCESSFUL;
 }
 
-/* Hip05 PCIe host only supports 32-bit config access */
+/* HipXX PCIe host only supports 32-bit config access */
 static int hisi_pcie_cfg_write(struct pcie_port *pp, int where, int  size,
 				u32 val)
 {
@@ -96,10 +107,9 @@ static int hisi_pcie_cfg_write(struct pcie_port *pp, int where, int  size,
 	return PCIBIOS_SUCCESSFUL;
 }
 
-static int hisi_pcie_link_up(struct pcie_port *pp)
+static int hisi_pcie_link_up_hip05(struct hisi_pcie *hisi_pcie)
 {
 	u32 val;
-	struct hisi_pcie *hisi_pcie = to_hisi_pcie(pp);
 
 	regmap_read(hisi_pcie->subctrl, PCIE_SUBCTRL_SYS_STATE4_REG +
 		    0x100 * hisi_pcie->port_id, &val);
@@ -107,6 +117,23 @@ static int hisi_pcie_link_up(struct pcie_port *pp)
 	return ((val & PCIE_LTSSM_STATE_MASK) == PCIE_LTSSM_LINKUP_STATE);
 }
 
+static int hisi_pcie_link_up_hip06(struct hisi_pcie *hisi_pcie)
+{
+	u32 val;
+
+	val = hisi_pcie_apb_readl(hisi_pcie, PCIE_HIP06_CTRL_OFF +
+			PCIE_SYS_STATE4);
+
+	return ((val & PCIE_LTSSM_STATE_MASK) == PCIE_LTSSM_LINKUP_STATE);
+}
+
+static int hisi_pcie_link_up(struct pcie_port *pp)
+{
+	struct hisi_pcie *hisi_pcie = to_hisi_pcie(pp);
+
+	return hisi_pcie->soc_ops->hisi_pcie_link_up(hisi_pcie);
+}
+
 static struct pcie_host_ops hisi_pcie_host_ops = {
 	.rd_own_conf = hisi_pcie_cfg_read,
 	.wr_own_conf = hisi_pcie_cfg_write,
@@ -145,7 +172,9 @@ static int hisi_pcie_probe(struct platform_device *pdev)
 {
 	struct hisi_pcie *hisi_pcie;
 	struct pcie_port *pp;
+	const struct of_device_id *match;
 	struct resource *reg;
+	struct device_driver *driver;
 	int ret;
 
 	hisi_pcie = devm_kzalloc(&pdev->dev, sizeof(*hisi_pcie), GFP_KERNEL);
@@ -154,6 +183,10 @@ static int hisi_pcie_probe(struct platform_device *pdev)
 
 	pp = &hisi_pcie->pp;
 	pp->dev = &pdev->dev;
+	driver = (pdev->dev).driver;
+
+	match = of_match_device(driver->of_match_table, &pdev->dev);
+	hisi_pcie->soc_ops = (struct pcie_soc_ops *) match->data;
 
 	hisi_pcie->subctrl =
 	syscon_regmap_lookup_by_compatible("hisilicon,pcie-sas-subctrl");
@@ -182,11 +215,27 @@ static int hisi_pcie_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static struct pcie_soc_ops hip05_ops = {
+		&hisi_pcie_link_up_hip05
+};
+
+static struct pcie_soc_ops hip06_ops = {
+		&hisi_pcie_link_up_hip06
+};
+
 static const struct of_device_id hisi_pcie_of_match[] = {
-	{.compatible = "hisilicon,hip05-pcie",},
+	{
+			.compatible = "hisilicon,hip05-pcie",
+			.data	    = (void *) &hip05_ops,
+	},
+	{
+			.compatible = "hisilicon,hip06-pcie",
+			.data	    = (void *) &hip06_ops,
+	},
 	{},
 };
 
+
 MODULE_DEVICE_TABLE(of, hisi_pcie_of_match);
 
 static struct platform_driver hisi_pcie_driver = {
@@ -198,3 +247,8 @@ static struct platform_driver hisi_pcie_driver = {
 };
 
 module_platform_driver(hisi_pcie_driver);
+
+MODULE_AUTHOR("Zhou Wang <wangzhou1@hisilicon.com>");
+MODULE_AUTHOR("Dacai Zhu <zhudacai@hisilicon.com>");
+MODULE_AUTHOR("Gabriele Paoloni <gabriele.paoloni@huawei.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-iproc-bcma.c b/drivers/pci/host/pcie-iproc-bcma.c
index 96a7d999fd5e..0d7bee4a0d26 100644
--- a/drivers/pci/host/pcie-iproc-bcma.c
+++ b/drivers/pci/host/pcie-iproc-bcma.c
@@ -55,6 +55,7 @@ static int iproc_pcie_bcma_probe(struct bcma_device *bdev)
 	bcma_set_drvdata(bdev, pcie);
 
 	pcie->base = bdev->io_addr;
+	pcie->base_addr = bdev->addr;
 
 	res_mem.start = bdev->addr_s[0];
 	res_mem.end = bdev->addr_s[0] + SZ_128M - 1;
diff --git a/drivers/pci/host/pcie-iproc-msi.c b/drivers/pci/host/pcie-iproc-msi.c
new file mode 100644
index 000000000000..9a2973bdc78a
--- /dev/null
+++ b/drivers/pci/host/pcie-iproc-msi.c
@@ -0,0 +1,675 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/irqdomain.h>
+#include <linux/msi.h>
+#include <linux/of_irq.h>
+#include <linux/of_pci.h>
+#include <linux/pci.h>
+
+#include "pcie-iproc.h"
+
+#define IPROC_MSI_INTR_EN_SHIFT        11
+#define IPROC_MSI_INTR_EN              BIT(IPROC_MSI_INTR_EN_SHIFT)
+#define IPROC_MSI_INT_N_EVENT_SHIFT    1
+#define IPROC_MSI_INT_N_EVENT          BIT(IPROC_MSI_INT_N_EVENT_SHIFT)
+#define IPROC_MSI_EQ_EN_SHIFT          0
+#define IPROC_MSI_EQ_EN                BIT(IPROC_MSI_EQ_EN_SHIFT)
+
+#define IPROC_MSI_EQ_MASK              0x3f
+
+/* Max number of GIC interrupts */
+#define NR_HW_IRQS                     6
+
+/* Number of entries in each event queue */
+#define EQ_LEN                         64
+
+/* Size of each event queue memory region */
+#define EQ_MEM_REGION_SIZE             SZ_4K
+
+/* Size of each MSI address region */
+#define MSI_MEM_REGION_SIZE            SZ_4K
+
+enum iproc_msi_reg {
+	IPROC_MSI_EQ_PAGE = 0,
+	IPROC_MSI_EQ_PAGE_UPPER,
+	IPROC_MSI_PAGE,
+	IPROC_MSI_PAGE_UPPER,
+	IPROC_MSI_CTRL,
+	IPROC_MSI_EQ_HEAD,
+	IPROC_MSI_EQ_TAIL,
+	IPROC_MSI_INTS_EN,
+	IPROC_MSI_REG_SIZE,
+};
+
+struct iproc_msi;
+
+/**
+ * iProc MSI group
+ *
+ * One MSI group is allocated per GIC interrupt, serviced by one iProc MSI
+ * event queue.
+ *
+ * @msi: pointer to iProc MSI data
+ * @gic_irq: GIC interrupt
+ * @eq: Event queue number
+ */
+struct iproc_msi_grp {
+	struct iproc_msi *msi;
+	int gic_irq;
+	unsigned int eq;
+};
+
+/**
+ * iProc event queue based MSI
+ *
+ * Only meant to be used on platforms without MSI support integrated into the
+ * GIC.
+ *
+ * @pcie: pointer to iProc PCIe data
+ * @reg_offsets: MSI register offsets
+ * @grps: MSI groups
+ * @nr_irqs: number of total interrupts connected to GIC
+ * @nr_cpus: number of toal CPUs
+ * @has_inten_reg: indicates the MSI interrupt enable register needs to be
+ * set explicitly (required for some legacy platforms)
+ * @bitmap: MSI vector bitmap
+ * @bitmap_lock: lock to protect access to the MSI bitmap
+ * @nr_msi_vecs: total number of MSI vectors
+ * @inner_domain: inner IRQ domain
+ * @msi_domain: MSI IRQ domain
+ * @nr_eq_region: required number of 4K aligned memory region for MSI event
+ * queues
+ * @nr_msi_region: required number of 4K aligned address region for MSI posted
+ * writes
+ * @eq_cpu: pointer to allocated memory region for MSI event queues
+ * @eq_dma: DMA address of MSI event queues
+ * @msi_addr: MSI address
+ */
+struct iproc_msi {
+	struct iproc_pcie *pcie;
+	const u16 (*reg_offsets)[IPROC_MSI_REG_SIZE];
+	struct iproc_msi_grp *grps;
+	int nr_irqs;
+	int nr_cpus;
+	bool has_inten_reg;
+	unsigned long *bitmap;
+	struct mutex bitmap_lock;
+	unsigned int nr_msi_vecs;
+	struct irq_domain *inner_domain;
+	struct irq_domain *msi_domain;
+	unsigned int nr_eq_region;
+	unsigned int nr_msi_region;
+	void *eq_cpu;
+	dma_addr_t eq_dma;
+	phys_addr_t msi_addr;
+};
+
+static const u16 iproc_msi_reg_paxb[NR_HW_IRQS][IPROC_MSI_REG_SIZE] = {
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x210, 0x250, 0x254, 0x208 },
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x214, 0x258, 0x25c, 0x208 },
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x218, 0x260, 0x264, 0x208 },
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x21c, 0x268, 0x26c, 0x208 },
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x220, 0x270, 0x274, 0x208 },
+	{ 0x200, 0x2c0, 0x204, 0x2c4, 0x224, 0x278, 0x27c, 0x208 },
+};
+
+static const u16 iproc_msi_reg_paxc[NR_HW_IRQS][IPROC_MSI_REG_SIZE] = {
+	{ 0xc00, 0xc04, 0xc08, 0xc0c, 0xc40, 0xc50, 0xc60 },
+	{ 0xc10, 0xc14, 0xc18, 0xc1c, 0xc44, 0xc54, 0xc64 },
+	{ 0xc20, 0xc24, 0xc28, 0xc2c, 0xc48, 0xc58, 0xc68 },
+	{ 0xc30, 0xc34, 0xc38, 0xc3c, 0xc4c, 0xc5c, 0xc6c },
+};
+
+static inline u32 iproc_msi_read_reg(struct iproc_msi *msi,
+				     enum iproc_msi_reg reg,
+				     unsigned int eq)
+{
+	struct iproc_pcie *pcie = msi->pcie;
+
+	return readl_relaxed(pcie->base + msi->reg_offsets[eq][reg]);
+}
+
+static inline void iproc_msi_write_reg(struct iproc_msi *msi,
+				       enum iproc_msi_reg reg,
+				       int eq, u32 val)
+{
+	struct iproc_pcie *pcie = msi->pcie;
+
+	writel_relaxed(val, pcie->base + msi->reg_offsets[eq][reg]);
+}
+
+static inline u32 hwirq_to_group(struct iproc_msi *msi, unsigned long hwirq)
+{
+	return (hwirq % msi->nr_irqs);
+}
+
+static inline unsigned int iproc_msi_addr_offset(struct iproc_msi *msi,
+						 unsigned long hwirq)
+{
+	if (msi->nr_msi_region > 1)
+		return hwirq_to_group(msi, hwirq) * MSI_MEM_REGION_SIZE;
+	else
+		return hwirq_to_group(msi, hwirq) * sizeof(u32);
+}
+
+static inline unsigned int iproc_msi_eq_offset(struct iproc_msi *msi, u32 eq)
+{
+	if (msi->nr_eq_region > 1)
+		return eq * EQ_MEM_REGION_SIZE;
+	else
+		return eq * EQ_LEN * sizeof(u32);
+}
+
+static struct irq_chip iproc_msi_irq_chip = {
+	.name = "iProc-MSI",
+};
+
+static struct msi_domain_info iproc_msi_domain_info = {
+	.flags = MSI_FLAG_USE_DEF_DOM_OPS | MSI_FLAG_USE_DEF_CHIP_OPS |
+		MSI_FLAG_PCI_MSIX,
+	.chip = &iproc_msi_irq_chip,
+};
+
+/*
+ * In iProc PCIe core, each MSI group is serviced by a GIC interrupt and a
+ * dedicated event queue.  Each MSI group can support up to 64 MSI vectors.
+ *
+ * The number of MSI groups varies between different iProc SoCs.  The total
+ * number of CPU cores also varies.  To support MSI IRQ affinity, we
+ * distribute GIC interrupts across all available CPUs.  MSI vector is moved
+ * from one GIC interrupt to another to steer to the target CPU.
+ *
+ * Assuming:
+ * - the number of MSI groups is M
+ * - the number of CPU cores is N
+ * - M is always a multiple of N
+ *
+ * Total number of raw MSI vectors = M * 64
+ * Total number of supported MSI vectors = (M * 64) / N
+ */
+static inline int hwirq_to_cpu(struct iproc_msi *msi, unsigned long hwirq)
+{
+	return (hwirq % msi->nr_cpus);
+}
+
+static inline unsigned long hwirq_to_canonical_hwirq(struct iproc_msi *msi,
+						     unsigned long hwirq)
+{
+	return (hwirq - hwirq_to_cpu(msi, hwirq));
+}
+
+static int iproc_msi_irq_set_affinity(struct irq_data *data,
+				      const struct cpumask *mask, bool force)
+{
+	struct iproc_msi *msi = irq_data_get_irq_chip_data(data);
+	int target_cpu = cpumask_first(mask);
+	int curr_cpu;
+
+	curr_cpu = hwirq_to_cpu(msi, data->hwirq);
+	if (curr_cpu == target_cpu)
+		return IRQ_SET_MASK_OK_DONE;
+
+	/* steer MSI to the target CPU */
+	data->hwirq = hwirq_to_canonical_hwirq(msi, data->hwirq) + target_cpu;
+
+	return IRQ_SET_MASK_OK;
+}
+
+static void iproc_msi_irq_compose_msi_msg(struct irq_data *data,
+					  struct msi_msg *msg)
+{
+	struct iproc_msi *msi = irq_data_get_irq_chip_data(data);
+	dma_addr_t addr;
+
+	addr = msi->msi_addr + iproc_msi_addr_offset(msi, data->hwirq);
+	msg->address_lo = lower_32_bits(addr);
+	msg->address_hi = upper_32_bits(addr);
+	msg->data = data->hwirq;
+}
+
+static struct irq_chip iproc_msi_bottom_irq_chip = {
+	.name = "MSI",
+	.irq_set_affinity = iproc_msi_irq_set_affinity,
+	.irq_compose_msi_msg = iproc_msi_irq_compose_msi_msg,
+};
+
+static int iproc_msi_irq_domain_alloc(struct irq_domain *domain,
+				      unsigned int virq, unsigned int nr_irqs,
+				      void *args)
+{
+	struct iproc_msi *msi = domain->host_data;
+	int hwirq;
+
+	mutex_lock(&msi->bitmap_lock);
+
+	/* Allocate 'nr_cpus' number of MSI vectors each time */
+	hwirq = bitmap_find_next_zero_area(msi->bitmap, msi->nr_msi_vecs, 0,
+					   msi->nr_cpus, 0);
+	if (hwirq < msi->nr_msi_vecs) {
+		bitmap_set(msi->bitmap, hwirq, msi->nr_cpus);
+	} else {
+		mutex_unlock(&msi->bitmap_lock);
+		return -ENOSPC;
+	}
+
+	mutex_unlock(&msi->bitmap_lock);
+
+	irq_domain_set_info(domain, virq, hwirq, &iproc_msi_bottom_irq_chip,
+			    domain->host_data, handle_simple_irq, NULL, NULL);
+
+	return 0;
+}
+
+static void iproc_msi_irq_domain_free(struct irq_domain *domain,
+				      unsigned int virq, unsigned int nr_irqs)
+{
+	struct irq_data *data = irq_domain_get_irq_data(domain, virq);
+	struct iproc_msi *msi = irq_data_get_irq_chip_data(data);
+	unsigned int hwirq;
+
+	mutex_lock(&msi->bitmap_lock);
+
+	hwirq = hwirq_to_canonical_hwirq(msi, data->hwirq);
+	bitmap_clear(msi->bitmap, hwirq, msi->nr_cpus);
+
+	mutex_unlock(&msi->bitmap_lock);
+
+	irq_domain_free_irqs_parent(domain, virq, nr_irqs);
+}
+
+static const struct irq_domain_ops msi_domain_ops = {
+	.alloc = iproc_msi_irq_domain_alloc,
+	.free = iproc_msi_irq_domain_free,
+};
+
+static inline u32 decode_msi_hwirq(struct iproc_msi *msi, u32 eq, u32 head)
+{
+	u32 *msg, hwirq;
+	unsigned int offs;
+
+	offs = iproc_msi_eq_offset(msi, eq) + head * sizeof(u32);
+	msg = (u32 *)(msi->eq_cpu + offs);
+	hwirq = *msg & IPROC_MSI_EQ_MASK;
+
+	/*
+	 * Since we have multiple hwirq mapped to a single MSI vector,
+	 * now we need to derive the hwirq at CPU0.  It can then be used to
+	 * mapped back to virq.
+	 */
+	return hwirq_to_canonical_hwirq(msi, hwirq);
+}
+
+static void iproc_msi_handler(struct irq_desc *desc)
+{
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	struct iproc_msi_grp *grp;
+	struct iproc_msi *msi;
+	struct iproc_pcie *pcie;
+	u32 eq, head, tail, nr_events;
+	unsigned long hwirq;
+	int virq;
+
+	chained_irq_enter(chip, desc);
+
+	grp = irq_desc_get_handler_data(desc);
+	msi = grp->msi;
+	pcie = msi->pcie;
+	eq = grp->eq;
+
+	/*
+	 * iProc MSI event queue is tracked by head and tail pointers.  Head
+	 * pointer indicates the next entry (MSI data) to be consumed by SW in
+	 * the queue and needs to be updated by SW.  iProc MSI core uses the
+	 * tail pointer as the next data insertion point.
+	 *
+	 * Entries between head and tail pointers contain valid MSI data.  MSI
+	 * data is guaranteed to be in the event queue memory before the tail
+	 * pointer is updated by the iProc MSI core.
+	 */
+	head = iproc_msi_read_reg(msi, IPROC_MSI_EQ_HEAD,
+				  eq) & IPROC_MSI_EQ_MASK;
+	do {
+		tail = iproc_msi_read_reg(msi, IPROC_MSI_EQ_TAIL,
+					  eq) & IPROC_MSI_EQ_MASK;
+
+		/*
+		 * Figure out total number of events (MSI data) to be
+		 * processed.
+		 */
+		nr_events = (tail < head) ?
+			(EQ_LEN - (head - tail)) : (tail - head);
+		if (!nr_events)
+			break;
+
+		/* process all outstanding events */
+		while (nr_events--) {
+			hwirq = decode_msi_hwirq(msi, eq, head);
+			virq = irq_find_mapping(msi->inner_domain, hwirq);
+			generic_handle_irq(virq);
+
+			head++;
+			head %= EQ_LEN;
+		}
+
+		/*
+		 * Now all outstanding events have been processed.  Update the
+		 * head pointer.
+		 */
+		iproc_msi_write_reg(msi, IPROC_MSI_EQ_HEAD, eq, head);
+
+		/*
+		 * Now go read the tail pointer again to see if there are new
+		 * oustanding events that came in during the above window.
+		 */
+	} while (true);
+
+	chained_irq_exit(chip, desc);
+}
+
+static void iproc_msi_enable(struct iproc_msi *msi)
+{
+	int i, eq;
+	u32 val;
+
+	/* Program memory region for each event queue */
+	for (i = 0; i < msi->nr_eq_region; i++) {
+		dma_addr_t addr = msi->eq_dma + (i * EQ_MEM_REGION_SIZE);
+
+		iproc_msi_write_reg(msi, IPROC_MSI_EQ_PAGE, i,
+				    lower_32_bits(addr));
+		iproc_msi_write_reg(msi, IPROC_MSI_EQ_PAGE_UPPER, i,
+				    upper_32_bits(addr));
+	}
+
+	/* Program address region for MSI posted writes */
+	for (i = 0; i < msi->nr_msi_region; i++) {
+		phys_addr_t addr = msi->msi_addr + (i * MSI_MEM_REGION_SIZE);
+
+		iproc_msi_write_reg(msi, IPROC_MSI_PAGE, i,
+				    lower_32_bits(addr));
+		iproc_msi_write_reg(msi, IPROC_MSI_PAGE_UPPER, i,
+				    upper_32_bits(addr));
+	}
+
+	for (eq = 0; eq < msi->nr_irqs; eq++) {
+		/* Enable MSI event queue */
+		val = IPROC_MSI_INTR_EN | IPROC_MSI_INT_N_EVENT |
+			IPROC_MSI_EQ_EN;
+		iproc_msi_write_reg(msi, IPROC_MSI_CTRL, eq, val);
+
+		/*
+		 * Some legacy platforms require the MSI interrupt enable
+		 * register to be set explicitly.
+		 */
+		if (msi->has_inten_reg) {
+			val = iproc_msi_read_reg(msi, IPROC_MSI_INTS_EN, eq);
+			val |= BIT(eq);
+			iproc_msi_write_reg(msi, IPROC_MSI_INTS_EN, eq, val);
+		}
+	}
+}
+
+static void iproc_msi_disable(struct iproc_msi *msi)
+{
+	u32 eq, val;
+
+	for (eq = 0; eq < msi->nr_irqs; eq++) {
+		if (msi->has_inten_reg) {
+			val = iproc_msi_read_reg(msi, IPROC_MSI_INTS_EN, eq);
+			val &= ~BIT(eq);
+			iproc_msi_write_reg(msi, IPROC_MSI_INTS_EN, eq, val);
+		}
+
+		val = iproc_msi_read_reg(msi, IPROC_MSI_CTRL, eq);
+		val &= ~(IPROC_MSI_INTR_EN | IPROC_MSI_INT_N_EVENT |
+			 IPROC_MSI_EQ_EN);
+		iproc_msi_write_reg(msi, IPROC_MSI_CTRL, eq, val);
+	}
+}
+
+static int iproc_msi_alloc_domains(struct device_node *node,
+				   struct iproc_msi *msi)
+{
+	msi->inner_domain = irq_domain_add_linear(NULL, msi->nr_msi_vecs,
+						  &msi_domain_ops, msi);
+	if (!msi->inner_domain)
+		return -ENOMEM;
+
+	msi->msi_domain = pci_msi_create_irq_domain(of_node_to_fwnode(node),
+						    &iproc_msi_domain_info,
+						    msi->inner_domain);
+	if (!msi->msi_domain) {
+		irq_domain_remove(msi->inner_domain);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void iproc_msi_free_domains(struct iproc_msi *msi)
+{
+	if (msi->msi_domain)
+		irq_domain_remove(msi->msi_domain);
+
+	if (msi->inner_domain)
+		irq_domain_remove(msi->inner_domain);
+}
+
+static void iproc_msi_irq_free(struct iproc_msi *msi, unsigned int cpu)
+{
+	int i;
+
+	for (i = cpu; i < msi->nr_irqs; i += msi->nr_cpus) {
+		irq_set_chained_handler_and_data(msi->grps[i].gic_irq,
+						 NULL, NULL);
+	}
+}
+
+static int iproc_msi_irq_setup(struct iproc_msi *msi, unsigned int cpu)
+{
+	int i, ret;
+	cpumask_var_t mask;
+	struct iproc_pcie *pcie = msi->pcie;
+
+	for (i = cpu; i < msi->nr_irqs; i += msi->nr_cpus) {
+		irq_set_chained_handler_and_data(msi->grps[i].gic_irq,
+						 iproc_msi_handler,
+						 &msi->grps[i]);
+		/* Dedicate GIC interrupt to each CPU core */
+		if (alloc_cpumask_var(&mask, GFP_KERNEL)) {
+			cpumask_clear(mask);
+			cpumask_set_cpu(cpu, mask);
+			ret = irq_set_affinity(msi->grps[i].gic_irq, mask);
+			if (ret)
+				dev_err(pcie->dev,
+					"failed to set affinity for IRQ%d\n",
+					msi->grps[i].gic_irq);
+			free_cpumask_var(mask);
+		} else {
+			dev_err(pcie->dev, "failed to alloc CPU mask\n");
+			ret = -EINVAL;
+		}
+
+		if (ret) {
+			/* Free all configured/unconfigured IRQs */
+			iproc_msi_irq_free(msi, cpu);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+int iproc_msi_init(struct iproc_pcie *pcie, struct device_node *node)
+{
+	struct iproc_msi *msi;
+	int i, ret;
+	unsigned int cpu;
+
+	if (!of_device_is_compatible(node, "brcm,iproc-msi"))
+		return -ENODEV;
+
+	if (!of_find_property(node, "msi-controller", NULL))
+		return -ENODEV;
+
+	if (pcie->msi)
+		return -EBUSY;
+
+	msi = devm_kzalloc(pcie->dev, sizeof(*msi), GFP_KERNEL);
+	if (!msi)
+		return -ENOMEM;
+
+	msi->pcie = pcie;
+	pcie->msi = msi;
+	msi->msi_addr = pcie->base_addr;
+	mutex_init(&msi->bitmap_lock);
+	msi->nr_cpus = num_possible_cpus();
+
+	msi->nr_irqs = of_irq_count(node);
+	if (!msi->nr_irqs) {
+		dev_err(pcie->dev, "found no MSI GIC interrupt\n");
+		return -ENODEV;
+	}
+
+	if (msi->nr_irqs > NR_HW_IRQS) {
+		dev_warn(pcie->dev, "too many MSI GIC interrupts defined %d\n",
+			 msi->nr_irqs);
+		msi->nr_irqs = NR_HW_IRQS;
+	}
+
+	if (msi->nr_irqs < msi->nr_cpus) {
+		dev_err(pcie->dev,
+			"not enough GIC interrupts for MSI affinity\n");
+		return -EINVAL;
+	}
+
+	if (msi->nr_irqs % msi->nr_cpus != 0) {
+		msi->nr_irqs -= msi->nr_irqs % msi->nr_cpus;
+		dev_warn(pcie->dev, "Reducing number of interrupts to %d\n",
+			 msi->nr_irqs);
+	}
+
+	switch (pcie->type) {
+	case IPROC_PCIE_PAXB:
+		msi->reg_offsets = iproc_msi_reg_paxb;
+		msi->nr_eq_region = 1;
+		msi->nr_msi_region = 1;
+		break;
+	case IPROC_PCIE_PAXC:
+		msi->reg_offsets = iproc_msi_reg_paxc;
+		msi->nr_eq_region = msi->nr_irqs;
+		msi->nr_msi_region = msi->nr_irqs;
+		break;
+	default:
+		dev_err(pcie->dev, "incompatible iProc PCIe interface\n");
+		return -EINVAL;
+	}
+
+	if (of_find_property(node, "brcm,pcie-msi-inten", NULL))
+		msi->has_inten_reg = true;
+
+	msi->nr_msi_vecs = msi->nr_irqs * EQ_LEN;
+	msi->bitmap = devm_kcalloc(pcie->dev, BITS_TO_LONGS(msi->nr_msi_vecs),
+				   sizeof(*msi->bitmap), GFP_KERNEL);
+	if (!msi->bitmap)
+		return -ENOMEM;
+
+	msi->grps = devm_kcalloc(pcie->dev, msi->nr_irqs, sizeof(*msi->grps),
+				 GFP_KERNEL);
+	if (!msi->grps)
+		return -ENOMEM;
+
+	for (i = 0; i < msi->nr_irqs; i++) {
+		unsigned int irq = irq_of_parse_and_map(node, i);
+
+		if (!irq) {
+			dev_err(pcie->dev, "unable to parse/map interrupt\n");
+			ret = -ENODEV;
+			goto free_irqs;
+		}
+		msi->grps[i].gic_irq = irq;
+		msi->grps[i].msi = msi;
+		msi->grps[i].eq = i;
+	}
+
+	/* Reserve memory for event queue and make sure memories are zeroed */
+	msi->eq_cpu = dma_zalloc_coherent(pcie->dev,
+					  msi->nr_eq_region * EQ_MEM_REGION_SIZE,
+					  &msi->eq_dma, GFP_KERNEL);
+	if (!msi->eq_cpu) {
+		ret = -ENOMEM;
+		goto free_irqs;
+	}
+
+	ret = iproc_msi_alloc_domains(node, msi);
+	if (ret) {
+		dev_err(pcie->dev, "failed to create MSI domains\n");
+		goto free_eq_dma;
+	}
+
+	for_each_online_cpu(cpu) {
+		ret = iproc_msi_irq_setup(msi, cpu);
+		if (ret)
+			goto free_msi_irq;
+	}
+
+	iproc_msi_enable(msi);
+
+	return 0;
+
+free_msi_irq:
+	for_each_online_cpu(cpu)
+		iproc_msi_irq_free(msi, cpu);
+	iproc_msi_free_domains(msi);
+
+free_eq_dma:
+	dma_free_coherent(pcie->dev, msi->nr_eq_region * EQ_MEM_REGION_SIZE,
+			  msi->eq_cpu, msi->eq_dma);
+
+free_irqs:
+	for (i = 0; i < msi->nr_irqs; i++) {
+		if (msi->grps[i].gic_irq)
+			irq_dispose_mapping(msi->grps[i].gic_irq);
+	}
+	pcie->msi = NULL;
+	return ret;
+}
+EXPORT_SYMBOL(iproc_msi_init);
+
+void iproc_msi_exit(struct iproc_pcie *pcie)
+{
+	struct iproc_msi *msi = pcie->msi;
+	unsigned int i, cpu;
+
+	if (!msi)
+		return;
+
+	iproc_msi_disable(msi);
+
+	for_each_online_cpu(cpu)
+		iproc_msi_irq_free(msi, cpu);
+
+	iproc_msi_free_domains(msi);
+
+	dma_free_coherent(pcie->dev, msi->nr_eq_region * EQ_MEM_REGION_SIZE,
+			  msi->eq_cpu, msi->eq_dma);
+
+	for (i = 0; i < msi->nr_irqs; i++) {
+		if (msi->grps[i].gic_irq)
+			irq_dispose_mapping(msi->grps[i].gic_irq);
+	}
+}
+EXPORT_SYMBOL(iproc_msi_exit);
diff --git a/drivers/pci/host/pcie-iproc-platform.c b/drivers/pci/host/pcie-iproc-platform.c
index c9550dc8b8ed..1738c5288eb6 100644
--- a/drivers/pci/host/pcie-iproc-platform.c
+++ b/drivers/pci/host/pcie-iproc-platform.c
@@ -26,8 +26,21 @@
 
 #include "pcie-iproc.h"
 
+static const struct of_device_id iproc_pcie_of_match_table[] = {
+	{
+		.compatible = "brcm,iproc-pcie",
+		.data = (int *)IPROC_PCIE_PAXB,
+	}, {
+		.compatible = "brcm,iproc-pcie-paxc",
+		.data = (int *)IPROC_PCIE_PAXC,
+	},
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, iproc_pcie_of_match_table);
+
 static int iproc_pcie_pltfm_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id;
 	struct iproc_pcie *pcie;
 	struct device_node *np = pdev->dev.of_node;
 	struct resource reg;
@@ -35,11 +48,16 @@ static int iproc_pcie_pltfm_probe(struct platform_device *pdev)
 	LIST_HEAD(res);
 	int ret;
 
+	of_id = of_match_device(iproc_pcie_of_match_table, &pdev->dev);
+	if (!of_id)
+		return -EINVAL;
+
 	pcie = devm_kzalloc(&pdev->dev, sizeof(struct iproc_pcie), GFP_KERNEL);
 	if (!pcie)
 		return -ENOMEM;
 
 	pcie->dev = &pdev->dev;
+	pcie->type = (enum iproc_pcie_type)of_id->data;
 	platform_set_drvdata(pdev, pcie);
 
 	ret = of_address_to_resource(np, 0, &reg);
@@ -53,6 +71,7 @@ static int iproc_pcie_pltfm_probe(struct platform_device *pdev)
 		dev_err(pcie->dev, "unable to map controller registers\n");
 		return -ENOMEM;
 	}
+	pcie->base_addr = reg.start;
 
 	if (of_property_read_bool(np, "brcm,pcie-ob")) {
 		u32 val;
@@ -114,12 +133,6 @@ static int iproc_pcie_pltfm_remove(struct platform_device *pdev)
 	return iproc_pcie_remove(pcie);
 }
 
-static const struct of_device_id iproc_pcie_of_match_table[] = {
-	{ .compatible = "brcm,iproc-pcie", },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, iproc_pcie_of_match_table);
-
 static struct platform_driver iproc_pcie_pltfm_driver = {
 	.driver = {
 		.name = "iproc-pcie",
diff --git a/drivers/pci/host/pcie-iproc.c b/drivers/pci/host/pcie-iproc.c
index eac719af16aa..a576aeeb22da 100644
--- a/drivers/pci/host/pcie-iproc.c
+++ b/drivers/pci/host/pcie-iproc.c
@@ -30,20 +30,16 @@
 
 #include "pcie-iproc.h"
 
-#define CLK_CONTROL_OFFSET           0x000
 #define EP_PERST_SOURCE_SELECT_SHIFT 2
 #define EP_PERST_SOURCE_SELECT       BIT(EP_PERST_SOURCE_SELECT_SHIFT)
 #define EP_MODE_SURVIVE_PERST_SHIFT  1
 #define EP_MODE_SURVIVE_PERST        BIT(EP_MODE_SURVIVE_PERST_SHIFT)
 #define RC_PCIE_RST_OUTPUT_SHIFT     0
 #define RC_PCIE_RST_OUTPUT           BIT(RC_PCIE_RST_OUTPUT_SHIFT)
+#define PAXC_RESET_MASK              0x7f
 
-#define CFG_IND_ADDR_OFFSET          0x120
 #define CFG_IND_ADDR_MASK            0x00001ffc
 
-#define CFG_IND_DATA_OFFSET          0x124
-
-#define CFG_ADDR_OFFSET              0x1f8
 #define CFG_ADDR_BUS_NUM_SHIFT       20
 #define CFG_ADDR_BUS_NUM_MASK        0x0ff00000
 #define CFG_ADDR_DEV_NUM_SHIFT       15
@@ -55,12 +51,8 @@
 #define CFG_ADDR_CFG_TYPE_SHIFT      0
 #define CFG_ADDR_CFG_TYPE_MASK       0x00000003
 
-#define CFG_DATA_OFFSET              0x1fc
-
-#define SYS_RC_INTX_EN               0x330
 #define SYS_RC_INTX_MASK             0xf
 
-#define PCIE_LINK_STATUS_OFFSET      0xf0c
 #define PCIE_PHYLINKUP_SHIFT         3
 #define PCIE_PHYLINKUP               BIT(PCIE_PHYLINKUP_SHIFT)
 #define PCIE_DL_ACTIVE_SHIFT         2
@@ -71,13 +63,54 @@
 #define OARR_SIZE_CFG_SHIFT          1
 #define OARR_SIZE_CFG                BIT(OARR_SIZE_CFG_SHIFT)
 
-#define OARR_LO(window)              (0xd20 + (window) * 8)
-#define OARR_HI(window)              (0xd24 + (window) * 8)
-#define OMAP_LO(window)              (0xd40 + (window) * 8)
-#define OMAP_HI(window)              (0xd44 + (window) * 8)
-
 #define MAX_NUM_OB_WINDOWS           2
 
+#define IPROC_PCIE_REG_INVALID 0xffff
+
+enum iproc_pcie_reg {
+	IPROC_PCIE_CLK_CTRL = 0,
+	IPROC_PCIE_CFG_IND_ADDR,
+	IPROC_PCIE_CFG_IND_DATA,
+	IPROC_PCIE_CFG_ADDR,
+	IPROC_PCIE_CFG_DATA,
+	IPROC_PCIE_INTX_EN,
+	IPROC_PCIE_OARR_LO,
+	IPROC_PCIE_OARR_HI,
+	IPROC_PCIE_OMAP_LO,
+	IPROC_PCIE_OMAP_HI,
+	IPROC_PCIE_LINK_STATUS,
+};
+
+/* iProc PCIe PAXB registers */
+static const u16 iproc_pcie_reg_paxb[] = {
+	[IPROC_PCIE_CLK_CTRL]     = 0x000,
+	[IPROC_PCIE_CFG_IND_ADDR] = 0x120,
+	[IPROC_PCIE_CFG_IND_DATA] = 0x124,
+	[IPROC_PCIE_CFG_ADDR]     = 0x1f8,
+	[IPROC_PCIE_CFG_DATA]     = 0x1fc,
+	[IPROC_PCIE_INTX_EN]      = 0x330,
+	[IPROC_PCIE_OARR_LO]      = 0xd20,
+	[IPROC_PCIE_OARR_HI]      = 0xd24,
+	[IPROC_PCIE_OMAP_LO]      = 0xd40,
+	[IPROC_PCIE_OMAP_HI]      = 0xd44,
+	[IPROC_PCIE_LINK_STATUS]  = 0xf0c,
+};
+
+/* iProc PCIe PAXC v1 registers */
+static const u16 iproc_pcie_reg_paxc[] = {
+	[IPROC_PCIE_CLK_CTRL]     = 0x000,
+	[IPROC_PCIE_CFG_IND_ADDR] = 0x1f0,
+	[IPROC_PCIE_CFG_IND_DATA] = 0x1f4,
+	[IPROC_PCIE_CFG_ADDR]     = 0x1f8,
+	[IPROC_PCIE_CFG_DATA]     = 0x1fc,
+	[IPROC_PCIE_INTX_EN]      = IPROC_PCIE_REG_INVALID,
+	[IPROC_PCIE_OARR_LO]      = IPROC_PCIE_REG_INVALID,
+	[IPROC_PCIE_OARR_HI]      = IPROC_PCIE_REG_INVALID,
+	[IPROC_PCIE_OMAP_LO]      = IPROC_PCIE_REG_INVALID,
+	[IPROC_PCIE_OMAP_HI]      = IPROC_PCIE_REG_INVALID,
+	[IPROC_PCIE_LINK_STATUS]  = IPROC_PCIE_REG_INVALID,
+};
+
 static inline struct iproc_pcie *iproc_data(struct pci_bus *bus)
 {
 	struct iproc_pcie *pcie;
@@ -91,6 +124,51 @@ static inline struct iproc_pcie *iproc_data(struct pci_bus *bus)
 	return pcie;
 }
 
+static inline bool iproc_pcie_reg_is_invalid(u16 reg_offset)
+{
+	return !!(reg_offset == IPROC_PCIE_REG_INVALID);
+}
+
+static inline u16 iproc_pcie_reg_offset(struct iproc_pcie *pcie,
+					enum iproc_pcie_reg reg)
+{
+	return pcie->reg_offsets[reg];
+}
+
+static inline u32 iproc_pcie_read_reg(struct iproc_pcie *pcie,
+				      enum iproc_pcie_reg reg)
+{
+	u16 offset = iproc_pcie_reg_offset(pcie, reg);
+
+	if (iproc_pcie_reg_is_invalid(offset))
+		return 0;
+
+	return readl(pcie->base + offset);
+}
+
+static inline void iproc_pcie_write_reg(struct iproc_pcie *pcie,
+					enum iproc_pcie_reg reg, u32 val)
+{
+	u16 offset = iproc_pcie_reg_offset(pcie, reg);
+
+	if (iproc_pcie_reg_is_invalid(offset))
+		return;
+
+	writel(val, pcie->base + offset);
+}
+
+static inline void iproc_pcie_ob_write(struct iproc_pcie *pcie,
+				       enum iproc_pcie_reg reg,
+				       unsigned window, u32 val)
+{
+	u16 offset = iproc_pcie_reg_offset(pcie, reg);
+
+	if (iproc_pcie_reg_is_invalid(offset))
+		return;
+
+	writel(val, pcie->base + offset + (window * 8));
+}
+
 /**
  * Note access to the configuration registers are protected at the higher layer
  * by 'pci_lock' in drivers/pci/access.c
@@ -104,18 +182,29 @@ static void __iomem *iproc_pcie_map_cfg_bus(struct pci_bus *bus,
 	unsigned fn = PCI_FUNC(devfn);
 	unsigned busno = bus->number;
 	u32 val;
+	u16 offset;
 
 	/* root complex access */
 	if (busno == 0) {
-		if (slot >= 1)
+		if (slot > 0 || fn > 0)
 			return NULL;
-		writel(where & CFG_IND_ADDR_MASK,
-		       pcie->base + CFG_IND_ADDR_OFFSET);
-		return (pcie->base + CFG_IND_DATA_OFFSET);
+
+		iproc_pcie_write_reg(pcie, IPROC_PCIE_CFG_IND_ADDR,
+				     where & CFG_IND_ADDR_MASK);
+		offset = iproc_pcie_reg_offset(pcie, IPROC_PCIE_CFG_IND_DATA);
+		if (iproc_pcie_reg_is_invalid(offset))
+			return NULL;
+		else
+			return (pcie->base + offset);
 	}
 
-	if (fn > 1)
-		return NULL;
+	/*
+	 * PAXC is connected to an internally emulated EP within the SoC.  It
+	 * allows only one device.
+	 */
+	if (pcie->type == IPROC_PCIE_PAXC)
+		if (slot > 0)
+			return NULL;
 
 	/* EP device access */
 	val = (busno << CFG_ADDR_BUS_NUM_SHIFT) |
@@ -123,9 +212,12 @@ static void __iomem *iproc_pcie_map_cfg_bus(struct pci_bus *bus,
 		(fn << CFG_ADDR_FUNC_NUM_SHIFT) |
 		(where & CFG_ADDR_REG_NUM_MASK) |
 		(1 & CFG_ADDR_CFG_TYPE_MASK);
-	writel(val, pcie->base + CFG_ADDR_OFFSET);
-
-	return (pcie->base + CFG_DATA_OFFSET);
+	iproc_pcie_write_reg(pcie, IPROC_PCIE_CFG_ADDR, val);
+	offset = iproc_pcie_reg_offset(pcie, IPROC_PCIE_CFG_DATA);
+	if (iproc_pcie_reg_is_invalid(offset))
+		return NULL;
+	else
+		return (pcie->base + offset);
 }
 
 static struct pci_ops iproc_pcie_ops = {
@@ -138,18 +230,29 @@ static void iproc_pcie_reset(struct iproc_pcie *pcie)
 {
 	u32 val;
 
+	if (pcie->type == IPROC_PCIE_PAXC) {
+		val = iproc_pcie_read_reg(pcie, IPROC_PCIE_CLK_CTRL);
+		val &= ~PAXC_RESET_MASK;
+		iproc_pcie_write_reg(pcie, IPROC_PCIE_CLK_CTRL, val);
+		udelay(100);
+		val |= PAXC_RESET_MASK;
+		iproc_pcie_write_reg(pcie, IPROC_PCIE_CLK_CTRL, val);
+		udelay(100);
+		return;
+	}
+
 	/*
 	 * Select perst_b signal as reset source. Put the device into reset,
 	 * and then bring it out of reset
 	 */
-	val = readl(pcie->base + CLK_CONTROL_OFFSET);
+	val = iproc_pcie_read_reg(pcie, IPROC_PCIE_CLK_CTRL);
 	val &= ~EP_PERST_SOURCE_SELECT & ~EP_MODE_SURVIVE_PERST &
 		~RC_PCIE_RST_OUTPUT;
-	writel(val, pcie->base + CLK_CONTROL_OFFSET);
+	iproc_pcie_write_reg(pcie, IPROC_PCIE_CLK_CTRL, val);
 	udelay(250);
 
 	val |= RC_PCIE_RST_OUTPUT;
-	writel(val, pcie->base + CLK_CONTROL_OFFSET);
+	iproc_pcie_write_reg(pcie, IPROC_PCIE_CLK_CTRL, val);
 	msleep(100);
 }
 
@@ -160,7 +263,14 @@ static int iproc_pcie_check_link(struct iproc_pcie *pcie, struct pci_bus *bus)
 	u16 pos, link_status;
 	bool link_is_active = false;
 
-	val = readl(pcie->base + PCIE_LINK_STATUS_OFFSET);
+	/*
+	 * PAXC connects to emulated endpoint devices directly and does not
+	 * have a Serdes.  Therefore skip the link detection logic here.
+	 */
+	if (pcie->type == IPROC_PCIE_PAXC)
+		return 0;
+
+	val = iproc_pcie_read_reg(pcie, IPROC_PCIE_LINK_STATUS);
 	if (!(val & PCIE_PHYLINKUP) || !(val & PCIE_DL_ACTIVE)) {
 		dev_err(pcie->dev, "PHY or data link is INACTIVE!\n");
 		return -ENODEV;
@@ -221,7 +331,7 @@ static int iproc_pcie_check_link(struct iproc_pcie *pcie, struct pci_bus *bus)
 
 static void iproc_pcie_enable(struct iproc_pcie *pcie)
 {
-	writel(SYS_RC_INTX_MASK, pcie->base + SYS_RC_INTX_EN);
+	iproc_pcie_write_reg(pcie, IPROC_PCIE_INTX_EN, SYS_RC_INTX_MASK);
 }
 
 /**
@@ -245,7 +355,7 @@ static int iproc_pcie_setup_ob(struct iproc_pcie *pcie, u64 axi_addr,
 
 	if (size > max_size) {
 		dev_err(pcie->dev,
-			"res size 0x%pap exceeds max supported size 0x%llx\n",
+			"res size %pap exceeds max supported size 0x%llx\n",
 			&size, max_size);
 		return -EINVAL;
 	}
@@ -272,11 +382,15 @@ static int iproc_pcie_setup_ob(struct iproc_pcie *pcie, u64 axi_addr,
 	axi_addr -= ob->axi_offset;
 
 	for (i = 0; i < MAX_NUM_OB_WINDOWS; i++) {
-		writel(lower_32_bits(axi_addr) | OARR_VALID |
-		       (ob->set_oarr_size ? 1 : 0), pcie->base + OARR_LO(i));
-		writel(upper_32_bits(axi_addr), pcie->base + OARR_HI(i));
-		writel(lower_32_bits(pci_addr), pcie->base + OMAP_LO(i));
-		writel(upper_32_bits(pci_addr), pcie->base + OMAP_HI(i));
+		iproc_pcie_ob_write(pcie, IPROC_PCIE_OARR_LO, i,
+				    lower_32_bits(axi_addr) | OARR_VALID |
+				    (ob->set_oarr_size ? 1 : 0));
+		iproc_pcie_ob_write(pcie, IPROC_PCIE_OARR_HI, i,
+				    upper_32_bits(axi_addr));
+		iproc_pcie_ob_write(pcie, IPROC_PCIE_OMAP_LO, i,
+				    lower_32_bits(pci_addr));
+		iproc_pcie_ob_write(pcie, IPROC_PCIE_OMAP_HI, i,
+				    upper_32_bits(pci_addr));
 
 		size -= ob->window_size;
 		if (size == 0)
@@ -319,6 +433,26 @@ static int iproc_pcie_map_ranges(struct iproc_pcie *pcie,
 	return 0;
 }
 
+static int iproc_pcie_msi_enable(struct iproc_pcie *pcie)
+{
+	struct device_node *msi_node;
+
+	msi_node = of_parse_phandle(pcie->dev->of_node, "msi-parent", 0);
+	if (!msi_node)
+		return -ENODEV;
+
+	/*
+	 * If another MSI controller is being used, the call below should fail
+	 * but that is okay
+	 */
+	return iproc_msi_init(pcie, msi_node);
+}
+
+static void iproc_pcie_msi_disable(struct iproc_pcie *pcie)
+{
+	iproc_msi_exit(pcie);
+}
+
 int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res)
 {
 	int ret;
@@ -340,6 +474,19 @@ int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res)
 		goto err_exit_phy;
 	}
 
+	switch (pcie->type) {
+	case IPROC_PCIE_PAXB:
+		pcie->reg_offsets = iproc_pcie_reg_paxb;
+		break;
+	case IPROC_PCIE_PAXC:
+		pcie->reg_offsets = iproc_pcie_reg_paxc;
+		break;
+	default:
+		dev_err(pcie->dev, "incompatible iProc PCIe interface\n");
+		ret = -EINVAL;
+		goto err_power_off_phy;
+	}
+
 	iproc_pcie_reset(pcie);
 
 	if (pcie->need_ob_cfg) {
@@ -373,6 +520,10 @@ int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res)
 
 	iproc_pcie_enable(pcie);
 
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		if (iproc_pcie_msi_enable(pcie))
+			dev_info(pcie->dev, "not using iProc MSI\n");
+
 	pci_scan_child_bus(bus);
 	pci_assign_unassigned_bus_resources(bus);
 	pci_fixup_irqs(pci_common_swizzle, pcie->map_irq);
@@ -397,6 +548,8 @@ int iproc_pcie_remove(struct iproc_pcie *pcie)
 	pci_stop_root_bus(pcie->root_bus);
 	pci_remove_root_bus(pcie->root_bus);
 
+	iproc_pcie_msi_disable(pcie);
+
 	phy_power_off(pcie->phy);
 	phy_exit(pcie->phy);
 
diff --git a/drivers/pci/host/pcie-iproc.h b/drivers/pci/host/pcie-iproc.h
index d3dc940f773a..e84d93c53c7b 100644
--- a/drivers/pci/host/pcie-iproc.h
+++ b/drivers/pci/host/pcie-iproc.h
@@ -15,6 +15,20 @@
 #define _PCIE_IPROC_H
 
 /**
+ * iProc PCIe interface type
+ *
+ * PAXB is the wrapper used in root complex that can be connected to an
+ * external endpoint device.
+ *
+ * PAXC is the wrapper used in root complex dedicated for internal emulated
+ * endpoint devices.
+ */
+enum iproc_pcie_type {
+	IPROC_PCIE_PAXB = 0,
+	IPROC_PCIE_PAXC,
+};
+
+/**
  * iProc PCIe outbound mapping
  * @set_oarr_size: indicates the OARR size bit needs to be set
  * @axi_offset: offset from the AXI address to the internal address used by
@@ -27,21 +41,30 @@ struct iproc_pcie_ob {
 	resource_size_t window_size;
 };
 
+struct iproc_msi;
+
 /**
  * iProc PCIe device
+ *
  * @dev: pointer to device data structure
+ * @type: iProc PCIe interface type
+ * @reg_offsets: register offsets
  * @base: PCIe host controller I/O register base
+ * @base_addr: PCIe host controller register base physical address
  * @sysdata: Per PCI controller data (ARM-specific)
  * @root_bus: pointer to root bus
  * @phy: optional PHY device that controls the Serdes
- * @irqs: interrupt IDs
  * @map_irq: function callback to map interrupts
- * @need_ob_cfg: indidates SW needs to configure the outbound mapping window
+ * @need_ob_cfg: indicates SW needs to configure the outbound mapping window
  * @ob: outbound mapping parameters
+ * @msi: MSI data
  */
 struct iproc_pcie {
 	struct device *dev;
+	enum iproc_pcie_type type;
+	const u16 *reg_offsets;
 	void __iomem *base;
+	phys_addr_t base_addr;
 #ifdef CONFIG_ARM
 	struct pci_sys_data sysdata;
 #endif
@@ -50,9 +73,24 @@ struct iproc_pcie {
 	int (*map_irq)(const struct pci_dev *, u8, u8);
 	bool need_ob_cfg;
 	struct iproc_pcie_ob ob;
+	struct iproc_msi *msi;
 };
 
 int iproc_pcie_setup(struct iproc_pcie *pcie, struct list_head *res);
 int iproc_pcie_remove(struct iproc_pcie *pcie);
 
+#ifdef CONFIG_PCIE_IPROC_MSI
+int iproc_msi_init(struct iproc_pcie *pcie, struct device_node *node);
+void iproc_msi_exit(struct iproc_pcie *pcie);
+#else
+static inline int iproc_msi_init(struct iproc_pcie *pcie,
+				 struct device_node *node)
+{
+	return -ENODEV;
+}
+static inline void iproc_msi_exit(struct iproc_pcie *pcie)
+{
+}
+#endif
+
 #endif /* _PCIE_IPROC_H */
diff --git a/drivers/pci/host/pcie-qcom.c b/drivers/pci/host/pcie-qcom.c
new file mode 100644
index 000000000000..e845fba19632
--- /dev/null
+++ b/drivers/pci/host/pcie-qcom.c
@@ -0,0 +1,616 @@
+/*
+ * Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+ * Copyright 2015 Linaro Limited.
+ *
+ * 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/clk.h>
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "pcie-designware.h"
+
+#define PCIE20_PARF_PHY_CTRL			0x40
+#define PCIE20_PARF_PHY_REFCLK			0x4C
+#define PCIE20_PARF_DBI_BASE_ADDR		0x168
+#define PCIE20_PARF_SLV_ADDR_SPACE_SIZE		0x16c
+#define PCIE20_PARF_AXI_MSTR_WR_ADDR_HALT	0x178
+
+#define PCIE20_ELBI_SYS_CTRL			0x04
+#define PCIE20_ELBI_SYS_CTRL_LT_ENABLE		BIT(0)
+
+#define PCIE20_CAP				0x70
+
+#define PERST_DELAY_US				1000
+
+struct qcom_pcie_resources_v0 {
+	struct clk *iface_clk;
+	struct clk *core_clk;
+	struct clk *phy_clk;
+	struct reset_control *pci_reset;
+	struct reset_control *axi_reset;
+	struct reset_control *ahb_reset;
+	struct reset_control *por_reset;
+	struct reset_control *phy_reset;
+	struct regulator *vdda;
+	struct regulator *vdda_phy;
+	struct regulator *vdda_refclk;
+};
+
+struct qcom_pcie_resources_v1 {
+	struct clk *iface;
+	struct clk *aux;
+	struct clk *master_bus;
+	struct clk *slave_bus;
+	struct reset_control *core;
+	struct regulator *vdda;
+};
+
+union qcom_pcie_resources {
+	struct qcom_pcie_resources_v0 v0;
+	struct qcom_pcie_resources_v1 v1;
+};
+
+struct qcom_pcie;
+
+struct qcom_pcie_ops {
+	int (*get_resources)(struct qcom_pcie *pcie);
+	int (*init)(struct qcom_pcie *pcie);
+	void (*deinit)(struct qcom_pcie *pcie);
+};
+
+struct qcom_pcie {
+	struct pcie_port pp;
+	struct device *dev;
+	union qcom_pcie_resources res;
+	void __iomem *parf;
+	void __iomem *dbi;
+	void __iomem *elbi;
+	struct phy *phy;
+	struct gpio_desc *reset;
+	struct qcom_pcie_ops *ops;
+};
+
+#define to_qcom_pcie(x)		container_of(x, struct qcom_pcie, pp)
+
+static void qcom_ep_reset_assert(struct qcom_pcie *pcie)
+{
+	gpiod_set_value(pcie->reset, 1);
+	usleep_range(PERST_DELAY_US, PERST_DELAY_US + 500);
+}
+
+static void qcom_ep_reset_deassert(struct qcom_pcie *pcie)
+{
+	gpiod_set_value(pcie->reset, 0);
+	usleep_range(PERST_DELAY_US, PERST_DELAY_US + 500);
+}
+
+static irqreturn_t qcom_pcie_msi_irq_handler(int irq, void *arg)
+{
+	struct pcie_port *pp = arg;
+
+	return dw_handle_msi_irq(pp);
+}
+
+static int qcom_pcie_establish_link(struct qcom_pcie *pcie)
+{
+	struct device *dev = pcie->dev;
+	unsigned int retries = 0;
+	u32 val;
+
+	if (dw_pcie_link_up(&pcie->pp))
+		return 0;
+
+	/* enable link training */
+	val = readl(pcie->elbi + PCIE20_ELBI_SYS_CTRL);
+	val |= PCIE20_ELBI_SYS_CTRL_LT_ENABLE;
+	writel(val, pcie->elbi + PCIE20_ELBI_SYS_CTRL);
+
+	do {
+		if (dw_pcie_link_up(&pcie->pp))
+			return 0;
+		usleep_range(250, 1000);
+	} while (retries < 200);
+
+	dev_warn(dev, "phy link never came up\n");
+
+	return -ETIMEDOUT;
+}
+
+static int qcom_pcie_get_resources_v0(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v0 *res = &pcie->res.v0;
+	struct device *dev = pcie->dev;
+
+	res->vdda = devm_regulator_get(dev, "vdda");
+	if (IS_ERR(res->vdda))
+		return PTR_ERR(res->vdda);
+
+	res->vdda_phy = devm_regulator_get(dev, "vdda_phy");
+	if (IS_ERR(res->vdda_phy))
+		return PTR_ERR(res->vdda_phy);
+
+	res->vdda_refclk = devm_regulator_get(dev, "vdda_refclk");
+	if (IS_ERR(res->vdda_refclk))
+		return PTR_ERR(res->vdda_refclk);
+
+	res->iface_clk = devm_clk_get(dev, "iface");
+	if (IS_ERR(res->iface_clk))
+		return PTR_ERR(res->iface_clk);
+
+	res->core_clk = devm_clk_get(dev, "core");
+	if (IS_ERR(res->core_clk))
+		return PTR_ERR(res->core_clk);
+
+	res->phy_clk = devm_clk_get(dev, "phy");
+	if (IS_ERR(res->phy_clk))
+		return PTR_ERR(res->phy_clk);
+
+	res->pci_reset = devm_reset_control_get(dev, "pci");
+	if (IS_ERR(res->pci_reset))
+		return PTR_ERR(res->pci_reset);
+
+	res->axi_reset = devm_reset_control_get(dev, "axi");
+	if (IS_ERR(res->axi_reset))
+		return PTR_ERR(res->axi_reset);
+
+	res->ahb_reset = devm_reset_control_get(dev, "ahb");
+	if (IS_ERR(res->ahb_reset))
+		return PTR_ERR(res->ahb_reset);
+
+	res->por_reset = devm_reset_control_get(dev, "por");
+	if (IS_ERR(res->por_reset))
+		return PTR_ERR(res->por_reset);
+
+	res->phy_reset = devm_reset_control_get(dev, "phy");
+	if (IS_ERR(res->phy_reset))
+		return PTR_ERR(res->phy_reset);
+
+	return 0;
+}
+
+static int qcom_pcie_get_resources_v1(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
+	struct device *dev = pcie->dev;
+
+	res->vdda = devm_regulator_get(dev, "vdda");
+	if (IS_ERR(res->vdda))
+		return PTR_ERR(res->vdda);
+
+	res->iface = devm_clk_get(dev, "iface");
+	if (IS_ERR(res->iface))
+		return PTR_ERR(res->iface);
+
+	res->aux = devm_clk_get(dev, "aux");
+	if (IS_ERR(res->aux))
+		return PTR_ERR(res->aux);
+
+	res->master_bus = devm_clk_get(dev, "master_bus");
+	if (IS_ERR(res->master_bus))
+		return PTR_ERR(res->master_bus);
+
+	res->slave_bus = devm_clk_get(dev, "slave_bus");
+	if (IS_ERR(res->slave_bus))
+		return PTR_ERR(res->slave_bus);
+
+	res->core = devm_reset_control_get(dev, "core");
+	if (IS_ERR(res->core))
+		return PTR_ERR(res->core);
+
+	return 0;
+}
+
+static void qcom_pcie_deinit_v0(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v0 *res = &pcie->res.v0;
+
+	reset_control_assert(res->pci_reset);
+	reset_control_assert(res->axi_reset);
+	reset_control_assert(res->ahb_reset);
+	reset_control_assert(res->por_reset);
+	reset_control_assert(res->pci_reset);
+	clk_disable_unprepare(res->iface_clk);
+	clk_disable_unprepare(res->core_clk);
+	clk_disable_unprepare(res->phy_clk);
+	regulator_disable(res->vdda);
+	regulator_disable(res->vdda_phy);
+	regulator_disable(res->vdda_refclk);
+}
+
+static int qcom_pcie_init_v0(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v0 *res = &pcie->res.v0;
+	struct device *dev = pcie->dev;
+	u32 val;
+	int ret;
+
+	ret = regulator_enable(res->vdda);
+	if (ret) {
+		dev_err(dev, "cannot enable vdda regulator\n");
+		return ret;
+	}
+
+	ret = regulator_enable(res->vdda_refclk);
+	if (ret) {
+		dev_err(dev, "cannot enable vdda_refclk regulator\n");
+		goto err_refclk;
+	}
+
+	ret = regulator_enable(res->vdda_phy);
+	if (ret) {
+		dev_err(dev, "cannot enable vdda_phy regulator\n");
+		goto err_vdda_phy;
+	}
+
+	ret = reset_control_assert(res->ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot assert ahb reset\n");
+		goto err_assert_ahb;
+	}
+
+	ret = clk_prepare_enable(res->iface_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable iface clock\n");
+		goto err_assert_ahb;
+	}
+
+	ret = clk_prepare_enable(res->phy_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable phy clock\n");
+		goto err_clk_phy;
+	}
+
+	ret = clk_prepare_enable(res->core_clk);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable core clock\n");
+		goto err_clk_core;
+	}
+
+	ret = reset_control_deassert(res->ahb_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert ahb reset\n");
+		goto err_deassert_ahb;
+	}
+
+	/* enable PCIe clocks and resets */
+	val = readl(pcie->parf + PCIE20_PARF_PHY_CTRL);
+	val &= ~BIT(0);
+	writel(val, pcie->parf + PCIE20_PARF_PHY_CTRL);
+
+	/* enable external reference clock */
+	val = readl(pcie->parf + PCIE20_PARF_PHY_REFCLK);
+	val |= BIT(16);
+	writel(val, pcie->parf + PCIE20_PARF_PHY_REFCLK);
+
+	ret = reset_control_deassert(res->phy_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert phy reset\n");
+		return ret;
+	}
+
+	ret = reset_control_deassert(res->pci_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert pci reset\n");
+		return ret;
+	}
+
+	ret = reset_control_deassert(res->por_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert por reset\n");
+		return ret;
+	}
+
+	ret = reset_control_deassert(res->axi_reset);
+	if (ret) {
+		dev_err(dev, "cannot deassert axi reset\n");
+		return ret;
+	}
+
+	/* wait for clock acquisition */
+	usleep_range(1000, 1500);
+
+	return 0;
+
+err_deassert_ahb:
+	clk_disable_unprepare(res->core_clk);
+err_clk_core:
+	clk_disable_unprepare(res->phy_clk);
+err_clk_phy:
+	clk_disable_unprepare(res->iface_clk);
+err_assert_ahb:
+	regulator_disable(res->vdda_phy);
+err_vdda_phy:
+	regulator_disable(res->vdda_refclk);
+err_refclk:
+	regulator_disable(res->vdda);
+
+	return ret;
+}
+
+static void qcom_pcie_deinit_v1(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
+
+	reset_control_assert(res->core);
+	clk_disable_unprepare(res->slave_bus);
+	clk_disable_unprepare(res->master_bus);
+	clk_disable_unprepare(res->iface);
+	clk_disable_unprepare(res->aux);
+	regulator_disable(res->vdda);
+}
+
+static int qcom_pcie_init_v1(struct qcom_pcie *pcie)
+{
+	struct qcom_pcie_resources_v1 *res = &pcie->res.v1;
+	struct device *dev = pcie->dev;
+	int ret;
+
+	ret = reset_control_deassert(res->core);
+	if (ret) {
+		dev_err(dev, "cannot deassert core reset\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(res->aux);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable aux clock\n");
+		goto err_res;
+	}
+
+	ret = clk_prepare_enable(res->iface);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable iface clock\n");
+		goto err_aux;
+	}
+
+	ret = clk_prepare_enable(res->master_bus);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable master_bus clock\n");
+		goto err_iface;
+	}
+
+	ret = clk_prepare_enable(res->slave_bus);
+	if (ret) {
+		dev_err(dev, "cannot prepare/enable slave_bus clock\n");
+		goto err_master;
+	}
+
+	ret = regulator_enable(res->vdda);
+	if (ret) {
+		dev_err(dev, "cannot enable vdda regulator\n");
+		goto err_slave;
+	}
+
+	/* change DBI base address */
+	writel(0, pcie->parf + PCIE20_PARF_DBI_BASE_ADDR);
+
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		u32 val = readl(pcie->parf + PCIE20_PARF_AXI_MSTR_WR_ADDR_HALT);
+
+		val |= BIT(31);
+		writel(val, pcie->parf + PCIE20_PARF_AXI_MSTR_WR_ADDR_HALT);
+	}
+
+	return 0;
+err_slave:
+	clk_disable_unprepare(res->slave_bus);
+err_master:
+	clk_disable_unprepare(res->master_bus);
+err_iface:
+	clk_disable_unprepare(res->iface);
+err_aux:
+	clk_disable_unprepare(res->aux);
+err_res:
+	reset_control_assert(res->core);
+
+	return ret;
+}
+
+static int qcom_pcie_link_up(struct pcie_port *pp)
+{
+	struct qcom_pcie *pcie = to_qcom_pcie(pp);
+	u16 val = readw(pcie->dbi + PCIE20_CAP + PCI_EXP_LNKSTA);
+
+	return !!(val & PCI_EXP_LNKSTA_DLLLA);
+}
+
+static void qcom_pcie_host_init(struct pcie_port *pp)
+{
+	struct qcom_pcie *pcie = to_qcom_pcie(pp);
+	int ret;
+
+	qcom_ep_reset_assert(pcie);
+
+	ret = pcie->ops->init(pcie);
+	if (ret)
+		goto err_deinit;
+
+	ret = phy_power_on(pcie->phy);
+	if (ret)
+		goto err_deinit;
+
+	dw_pcie_setup_rc(pp);
+
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		dw_pcie_msi_init(pp);
+
+	qcom_ep_reset_deassert(pcie);
+
+	ret = qcom_pcie_establish_link(pcie);
+	if (ret)
+		goto err;
+
+	return;
+err:
+	qcom_ep_reset_assert(pcie);
+	phy_power_off(pcie->phy);
+err_deinit:
+	pcie->ops->deinit(pcie);
+}
+
+static int qcom_pcie_rd_own_conf(struct pcie_port *pp, int where, int size,
+				 u32 *val)
+{
+	/* the device class is not reported correctly from the register */
+	if (where == PCI_CLASS_REVISION && size == 4) {
+		*val = readl(pp->dbi_base + PCI_CLASS_REVISION);
+		*val &= 0xff;	/* keep revision id */
+		*val |= PCI_CLASS_BRIDGE_PCI << 16;
+		return PCIBIOS_SUCCESSFUL;
+	}
+
+	return dw_pcie_cfg_read(pp->dbi_base + where, size, val);
+}
+
+static struct pcie_host_ops qcom_pcie_dw_ops = {
+	.link_up = qcom_pcie_link_up,
+	.host_init = qcom_pcie_host_init,
+	.rd_own_conf = qcom_pcie_rd_own_conf,
+};
+
+static const struct qcom_pcie_ops ops_v0 = {
+	.get_resources = qcom_pcie_get_resources_v0,
+	.init = qcom_pcie_init_v0,
+	.deinit = qcom_pcie_deinit_v0,
+};
+
+static const struct qcom_pcie_ops ops_v1 = {
+	.get_resources = qcom_pcie_get_resources_v1,
+	.init = qcom_pcie_init_v1,
+	.deinit = qcom_pcie_deinit_v1,
+};
+
+static int qcom_pcie_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct qcom_pcie *pcie;
+	struct pcie_port *pp;
+	int ret;
+
+	pcie = devm_kzalloc(dev, sizeof(*pcie), GFP_KERNEL);
+	if (!pcie)
+		return -ENOMEM;
+
+	pcie->ops = (struct qcom_pcie_ops *)of_device_get_match_data(dev);
+	pcie->dev = dev;
+
+	pcie->reset = devm_gpiod_get_optional(dev, "perst", GPIOD_OUT_LOW);
+	if (IS_ERR(pcie->reset))
+		return PTR_ERR(pcie->reset);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "parf");
+	pcie->parf = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pcie->parf))
+		return PTR_ERR(pcie->parf);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dbi");
+	pcie->dbi = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pcie->dbi))
+		return PTR_ERR(pcie->dbi);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "elbi");
+	pcie->elbi = devm_ioremap_resource(dev, res);
+	if (IS_ERR(pcie->elbi))
+		return PTR_ERR(pcie->elbi);
+
+	pcie->phy = devm_phy_optional_get(dev, "pciephy");
+	if (IS_ERR(pcie->phy))
+		return PTR_ERR(pcie->phy);
+
+	ret = pcie->ops->get_resources(pcie);
+	if (ret)
+		return ret;
+
+	pp = &pcie->pp;
+	pp->dev = dev;
+	pp->dbi_base = pcie->dbi;
+	pp->root_bus_nr = -1;
+	pp->ops = &qcom_pcie_dw_ops;
+
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		pp->msi_irq = platform_get_irq_byname(pdev, "msi");
+		if (pp->msi_irq < 0)
+			return pp->msi_irq;
+
+		ret = devm_request_irq(dev, pp->msi_irq,
+				       qcom_pcie_msi_irq_handler,
+				       IRQF_SHARED, "qcom-pcie-msi", pp);
+		if (ret) {
+			dev_err(dev, "cannot request msi irq\n");
+			return ret;
+		}
+	}
+
+	ret = phy_init(pcie->phy);
+	if (ret)
+		return ret;
+
+	ret = dw_pcie_host_init(pp);
+	if (ret) {
+		dev_err(dev, "cannot initialize host\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, pcie);
+
+	return 0;
+}
+
+static int qcom_pcie_remove(struct platform_device *pdev)
+{
+	struct qcom_pcie *pcie = platform_get_drvdata(pdev);
+
+	qcom_ep_reset_assert(pcie);
+	phy_power_off(pcie->phy);
+	phy_exit(pcie->phy);
+	pcie->ops->deinit(pcie);
+
+	return 0;
+}
+
+static const struct of_device_id qcom_pcie_match[] = {
+	{ .compatible = "qcom,pcie-ipq8064", .data = &ops_v0 },
+	{ .compatible = "qcom,pcie-apq8064", .data = &ops_v0 },
+	{ .compatible = "qcom,pcie-apq8084", .data = &ops_v1 },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, qcom_pcie_match);
+
+static struct platform_driver qcom_pcie_driver = {
+	.probe = qcom_pcie_probe,
+	.remove = qcom_pcie_remove,
+	.driver = {
+		.name = "qcom-pcie",
+		.of_match_table = qcom_pcie_match,
+	},
+};
+
+module_platform_driver(qcom_pcie_driver);
+
+MODULE_AUTHOR("Stanimir Varbanov <svarbanov@mm-sol.com>");
+MODULE_DESCRIPTION("Qualcomm PCIe root complex driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
index f4fa6c537448..4edb5181f4e2 100644
--- a/drivers/pci/host/pcie-rcar.c
+++ b/drivers/pci/host/pcie-rcar.c
@@ -26,6 +26,7 @@
 #include <linux/of_platform.h>
 #include <linux/pci.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 
 #define DRV_NAME "rcar-pcie"
@@ -94,6 +95,11 @@
 #define H1_PCIEPHYDOUTR		0x040014
 #define H1_PCIEPHYSR		0x040018
 
+/* R-Car Gen2 PHY */
+#define GEN2_PCIEPHYADDR	0x780
+#define GEN2_PCIEPHYDATA	0x784
+#define GEN2_PCIEPHYCTRL	0x78c
+
 #define INT_PCI_MSI_NR	32
 
 #define RCONF(x)	(PCICONF(0)+(x))
@@ -108,8 +114,6 @@
 #define RCAR_PCI_MAX_RESOURCES 4
 #define MAX_NR_INBOUND_MAPS 6
 
-static unsigned long global_io_offset;
-
 struct rcar_msi {
 	DECLARE_BITMAP(used, INT_PCI_MSI_NR);
 	struct irq_domain *domain;
@@ -126,20 +130,10 @@ static inline struct rcar_msi *to_rcar_msi(struct msi_controller *chip)
 }
 
 /* Structure representing the PCIe interface */
-/*
- * ARM pcibios functions expect the ARM struct pci_sys_data as the PCI
- * sysdata.  Add pci_sys_data as the first element in struct gen_pci so
- * that when we use a gen_pci pointer as sysdata, it is also a pointer to
- * a struct pci_sys_data.
- */
 struct rcar_pcie {
-#ifdef CONFIG_ARM
-	struct pci_sys_data	sys;
-#endif
 	struct device		*dev;
 	void __iomem		*base;
-	struct resource		res[RCAR_PCI_MAX_RESOURCES];
-	struct resource		busn;
+	struct list_head	resources;
 	int			root_bus_nr;
 	struct clk		*clk;
 	struct clk		*bus_clk;
@@ -323,10 +317,9 @@ static struct pci_ops rcar_pcie_ops = {
 	.write	= rcar_pcie_write_conf,
 };
 
-static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie)
+static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie,
+				   struct resource *res)
 {
-	struct resource *res = &pcie->res[win];
-
 	/* Setup PCIe address space mappings for each resource */
 	resource_size_t size;
 	resource_size_t res_start;
@@ -359,31 +352,33 @@ static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie)
 	rcar_pci_write_reg(pcie, mask, PCIEPTCTLR(win));
 }
 
-static int rcar_pcie_setup(struct list_head *resource, struct rcar_pcie *pcie)
+static int rcar_pcie_setup(struct list_head *resource, struct rcar_pcie *pci)
 {
-	struct resource *res;
-	int i;
-
-	pcie->root_bus_nr = pcie->busn.start;
+	struct resource_entry *win;
+	int i = 0;
 
 	/* Setup PCI resources */
-	for (i = 0; i < RCAR_PCI_MAX_RESOURCES; i++) {
+	resource_list_for_each_entry(win, &pci->resources) {
+		struct resource *res = win->res;
 
-		res = &pcie->res[i];
 		if (!res->flags)
 			continue;
 
-		rcar_pcie_setup_window(i, pcie);
-
-		if (res->flags & IORESOURCE_IO) {
-			phys_addr_t io_start = pci_pio_to_address(res->start);
-			pci_ioremap_io(global_io_offset, io_start);
-			global_io_offset += SZ_64K;
+		switch (resource_type(res)) {
+		case IORESOURCE_IO:
+		case IORESOURCE_MEM:
+			rcar_pcie_setup_window(i, pci, res);
+			i++;
+			break;
+		case IORESOURCE_BUS:
+			pci->root_bus_nr = res->start;
+			break;
+		default:
+			continue;
 		}
 
 		pci_add_resource(resource, res);
 	}
-	pci_add_resource(resource, &pcie->busn);
 
 	return 1;
 }
@@ -578,6 +573,26 @@ static int rcar_pcie_hw_init_h1(struct rcar_pcie *pcie)
 	return -ETIMEDOUT;
 }
 
+static int rcar_pcie_hw_init_gen2(struct rcar_pcie *pcie)
+{
+	/*
+	 * These settings come from the R-Car Series, 2nd Generation User's
+	 * Manual, section 50.3.1 (2) Initialization of the physical layer.
+	 */
+	rcar_pci_write_reg(pcie, 0x000f0030, GEN2_PCIEPHYADDR);
+	rcar_pci_write_reg(pcie, 0x00381203, GEN2_PCIEPHYDATA);
+	rcar_pci_write_reg(pcie, 0x00000001, GEN2_PCIEPHYCTRL);
+	rcar_pci_write_reg(pcie, 0x00000006, GEN2_PCIEPHYCTRL);
+
+	rcar_pci_write_reg(pcie, 0x000f0054, GEN2_PCIEPHYADDR);
+	/* The following value is for DC connection, no termination resistor */
+	rcar_pci_write_reg(pcie, 0x13802007, GEN2_PCIEPHYDATA);
+	rcar_pci_write_reg(pcie, 0x00000001, GEN2_PCIEPHYCTRL);
+	rcar_pci_write_reg(pcie, 0x00000006, GEN2_PCIEPHYCTRL);
+
+	return rcar_pcie_hw_init(pcie);
+}
+
 static int rcar_msi_alloc(struct rcar_msi *chip)
 {
 	int msi;
@@ -720,14 +735,16 @@ static int rcar_pcie_enable_msi(struct rcar_pcie *pcie)
 
 	/* Two irqs are for MSI, but they are also used for non-MSI irqs */
 	err = devm_request_irq(&pdev->dev, msi->irq1, rcar_pcie_msi_irq,
-			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+			       IRQF_SHARED | IRQF_NO_THREAD,
+			       rcar_msi_irq_chip.name, pcie);
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
 		goto err;
 	}
 
 	err = devm_request_irq(&pdev->dev, msi->irq2, rcar_pcie_msi_irq,
-			       IRQF_SHARED, rcar_msi_irq_chip.name, pcie);
+			       IRQF_SHARED | IRQF_NO_THREAD,
+			       rcar_msi_irq_chip.name, pcie);
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to request IRQ: %d\n", err);
 		goto err;
@@ -917,20 +934,71 @@ static int rcar_pcie_parse_map_dma_ranges(struct rcar_pcie *pcie,
 
 static const struct of_device_id rcar_pcie_of_match[] = {
 	{ .compatible = "renesas,pcie-r8a7779", .data = rcar_pcie_hw_init_h1 },
-	{ .compatible = "renesas,pcie-r8a7790", .data = rcar_pcie_hw_init },
-	{ .compatible = "renesas,pcie-r8a7791", .data = rcar_pcie_hw_init },
+	{ .compatible = "renesas,pcie-rcar-gen2", .data = rcar_pcie_hw_init_gen2 },
+	{ .compatible = "renesas,pcie-r8a7790", .data = rcar_pcie_hw_init_gen2 },
+	{ .compatible = "renesas,pcie-r8a7791", .data = rcar_pcie_hw_init_gen2 },
+	{ .compatible = "renesas,pcie-r8a7795", .data = rcar_pcie_hw_init },
 	{},
 };
 MODULE_DEVICE_TABLE(of, rcar_pcie_of_match);
 
+static void rcar_pcie_release_of_pci_ranges(struct rcar_pcie *pci)
+{
+	pci_free_resource_list(&pci->resources);
+}
+
+static int rcar_pcie_parse_request_of_pci_ranges(struct rcar_pcie *pci)
+{
+	int err;
+	struct device *dev = pci->dev;
+	struct device_node *np = dev->of_node;
+	resource_size_t iobase;
+	struct resource_entry *win;
+
+	err = of_pci_get_host_bridge_resources(np, 0, 0xff, &pci->resources, &iobase);
+	if (err)
+		return err;
+
+	resource_list_for_each_entry(win, &pci->resources) {
+		struct resource *parent, *res = win->res;
+
+		switch (resource_type(res)) {
+		case IORESOURCE_IO:
+			parent = &ioport_resource;
+			err = pci_remap_iospace(res, iobase);
+			if (err) {
+				dev_warn(dev, "error %d: failed to map resource %pR\n",
+					 err, res);
+				continue;
+			}
+			break;
+		case IORESOURCE_MEM:
+			parent = &iomem_resource;
+			break;
+
+		case IORESOURCE_BUS:
+		default:
+			continue;
+		}
+
+		err = devm_request_resource(dev, parent, res);
+		if (err)
+			goto out_release_res;
+	}
+
+	return 0;
+
+out_release_res:
+	rcar_pcie_release_of_pci_ranges(pci);
+	return err;
+}
+
 static int rcar_pcie_probe(struct platform_device *pdev)
 {
 	struct rcar_pcie *pcie;
 	unsigned int data;
-	struct of_pci_range range;
-	struct of_pci_range_parser parser;
 	const struct of_device_id *of_id;
-	int err, win = 0;
+	int err;
 	int (*hw_init_fn)(struct rcar_pcie *);
 
 	pcie = devm_kzalloc(&pdev->dev, sizeof(*pcie), GFP_KERNEL);
@@ -940,16 +1008,9 @@ static int rcar_pcie_probe(struct platform_device *pdev)
 	pcie->dev = &pdev->dev;
 	platform_set_drvdata(pdev, pcie);
 
-	/* Get the bus range */
-	if (of_pci_parse_bus_range(pdev->dev.of_node, &pcie->busn)) {
-		dev_err(&pdev->dev, "failed to parse bus-range property\n");
-		return -EINVAL;
-	}
+	INIT_LIST_HEAD(&pcie->resources);
 
-	if (of_pci_range_parser_init(&parser, pdev->dev.of_node)) {
-		dev_err(&pdev->dev, "missing ranges property\n");
-		return -EINVAL;
-	}
+	rcar_pcie_parse_request_of_pci_ranges(pcie);
 
 	err = rcar_pcie_get_resources(pdev, pcie);
 	if (err < 0) {
@@ -957,46 +1018,55 @@ static int rcar_pcie_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	for_each_of_pci_range(&parser, &range) {
-		err = of_pci_range_to_resource(&range, pdev->dev.of_node,
-						&pcie->res[win++]);
-		if (err < 0)
-			return err;
-
-		if (win > RCAR_PCI_MAX_RESOURCES)
-			break;
-	}
-
 	 err = rcar_pcie_parse_map_dma_ranges(pcie, pdev->dev.of_node);
 	 if (err)
 		return err;
 
-	if (IS_ENABLED(CONFIG_PCI_MSI)) {
-		err = rcar_pcie_enable_msi(pcie);
-		if (err < 0) {
-			dev_err(&pdev->dev,
-				"failed to enable MSI support: %d\n",
-				err);
-			return err;
-		}
-	}
-
 	of_id = of_match_device(rcar_pcie_of_match, pcie->dev);
 	if (!of_id || !of_id->data)
 		return -EINVAL;
 	hw_init_fn = of_id->data;
 
+	pm_runtime_enable(pcie->dev);
+	err = pm_runtime_get_sync(pcie->dev);
+	if (err < 0) {
+		dev_err(pcie->dev, "pm_runtime_get_sync failed\n");
+		goto err_pm_disable;
+	}
+
 	/* Failure to get a link might just be that no cards are inserted */
 	err = hw_init_fn(pcie);
 	if (err) {
 		dev_info(&pdev->dev, "PCIe link down\n");
-		return 0;
+		err = 0;
+		goto err_pm_put;
 	}
 
 	data = rcar_pci_read_reg(pcie, MACSR);
 	dev_info(&pdev->dev, "PCIe x%d: link up\n", (data >> 20) & 0x3f);
 
-	return rcar_pcie_enable(pcie);
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		err = rcar_pcie_enable_msi(pcie);
+		if (err < 0) {
+			dev_err(&pdev->dev,
+				"failed to enable MSI support: %d\n",
+				err);
+			goto err_pm_put;
+		}
+	}
+
+	err = rcar_pcie_enable(pcie);
+	if (err)
+		goto err_pm_put;
+
+	return 0;
+
+err_pm_put:
+	pm_runtime_put(pcie->dev);
+
+err_pm_disable:
+	pm_runtime_disable(pcie->dev);
+	return err;
 }
 
 static struct platform_driver rcar_pcie_driver = {
diff --git a/drivers/pci/host/pcie-spear13xx.c b/drivers/pci/host/pcie-spear13xx.c
index b95b7563c052..a6cd8233e8c0 100644
--- a/drivers/pci/host/pcie-spear13xx.c
+++ b/drivers/pci/host/pcie-spear13xx.c
@@ -279,7 +279,8 @@ static int spear13xx_add_pcie_port(struct pcie_port *pp,
 		return -ENODEV;
 	}
 	ret = devm_request_irq(dev, pp->irq, spear13xx_pcie_irq_handler,
-			       IRQF_SHARED, "spear1340-pcie", pp);
+			       IRQF_SHARED | IRQF_NO_THREAD,
+			       "spear1340-pcie", pp);
 	if (ret) {
 		dev_err(dev, "failed to request irq %d\n", pp->irq);
 		return ret;
diff --git a/drivers/pci/host/pcie-xilinx.c b/drivers/pci/host/pcie-xilinx.c
index 3c7a0d580b1e..4cfa46360d12 100644
--- a/drivers/pci/host/pcie-xilinx.c
+++ b/drivers/pci/host/pcie-xilinx.c
@@ -781,7 +781,8 @@ static int xilinx_pcie_parse_dt(struct xilinx_pcie_port *port)
 
 	port->irq = irq_of_parse_and_map(node, 0);
 	err = devm_request_irq(dev, port->irq, xilinx_pcie_intr_handler,
-			       IRQF_SHARED, "xilinx-pcie", port);
+			       IRQF_SHARED | IRQF_NO_THREAD,
+			       "xilinx-pcie", port);
 	if (err) {
 		dev_err(dev, "unable to request irq %d\n", port->irq);
 		return err;
diff --git a/drivers/pci/hotplug/acpi_pcihp.c b/drivers/pci/hotplug/acpi_pcihp.c
index 876ccc620440..a5e66df4ad14 100644
--- a/drivers/pci/hotplug/acpi_pcihp.c
+++ b/drivers/pci/hotplug/acpi_pcihp.c
@@ -36,10 +36,10 @@
 
 #define MY_NAME	"acpi_pcihp"
 
-#define dbg(fmt, arg...) do { if (debug_acpi) printk(KERN_DEBUG "%s: %s: " fmt , MY_NAME , __func__ , ## arg); } while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format , MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format , MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format , MY_NAME , ## arg)
+#define dbg(fmt, arg...) do { if (debug_acpi) printk(KERN_DEBUG "%s: %s: " fmt, MY_NAME, __func__, ## arg); } while (0)
+#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 #define	METHOD_NAME__SUN	"_SUN"
 #define	METHOD_NAME_OSHP	"OSHP"
@@ -132,7 +132,7 @@ int acpi_get_hp_hw_control_from_firmware(struct pci_dev *pdev, u32 flags)
 
 	while (handle) {
 		acpi_get_name(handle, ACPI_FULL_PATHNAME, &string);
-		dbg("Trying to get hotplug control for %s \n",
+		dbg("Trying to get hotplug control for %s\n",
 		    (char *)string.pointer);
 		status = acpi_run_oshp(handle);
 		if (ACPI_SUCCESS(status))
diff --git a/drivers/pci/hotplug/acpiphp.h b/drivers/pci/hotplug/acpiphp.h
index b0e61bf261a7..f0ebc8b9a15a 100644
--- a/drivers/pci/hotplug/acpiphp.h
+++ b/drivers/pci/hotplug/acpiphp.h
@@ -181,7 +181,7 @@ struct acpiphp_attention_info
 /* function prototypes */
 
 /* acpiphp_core.c */
-int acpiphp_register_attention(struct acpiphp_attention_info*info);
+int acpiphp_register_attention(struct acpiphp_attention_info *info);
 int acpiphp_unregister_attention(struct acpiphp_attention_info *info);
 int acpiphp_register_hotplug_slot(struct acpiphp_slot *slot, unsigned int sun);
 void acpiphp_unregister_hotplug_slot(struct acpiphp_slot *slot);
diff --git a/drivers/pci/hotplug/acpiphp_core.c b/drivers/pci/hotplug/acpiphp_core.c
index e291efcd02a2..3c81fc8b0103 100644
--- a/drivers/pci/hotplug/acpiphp_core.c
+++ b/drivers/pci/hotplug/acpiphp_core.c
@@ -63,13 +63,13 @@ MODULE_LICENSE("GPL");
 MODULE_PARM_DESC(disable, "disable acpiphp driver");
 module_param_named(disable, acpiphp_disabled, bool, 0444);
 
-static int enable_slot		(struct hotplug_slot *slot);
-static int disable_slot		(struct hotplug_slot *slot);
-static int set_attention_status (struct hotplug_slot *slot, u8 value);
-static int get_power_status	(struct hotplug_slot *slot, u8 *value);
-static int get_attention_status (struct hotplug_slot *slot, u8 *value);
-static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
-static int get_adapter_status	(struct hotplug_slot *slot, u8 *value);
+static int enable_slot(struct hotplug_slot *slot);
+static int disable_slot(struct hotplug_slot *slot);
+static int set_attention_status(struct hotplug_slot *slot, u8 value);
+static int get_power_status(struct hotplug_slot *slot, u8 *value);
+static int get_attention_status(struct hotplug_slot *slot, u8 *value);
+static int get_latch_status(struct hotplug_slot *slot, u8 *value);
+static int get_adapter_status(struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops acpi_hotplug_slot_ops = {
 	.enable_slot		= enable_slot,
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index ff538568a617..fa49f9143b80 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -707,7 +707,7 @@ static void acpiphp_sanitize_bus(struct pci_bus *bus)
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM;
 
 	list_for_each_entry_safe_reverse(dev, tmp, &bus->devices, bus_list) {
-		for (i=0; i<PCI_BRIDGE_RESOURCES; i++) {
+		for (i = 0; i < PCI_BRIDGE_RESOURCES; i++) {
 			struct resource *res = &dev->resource[i];
 			if ((res->flags & type_mask) && !res->start &&
 					res->end) {
@@ -953,8 +953,10 @@ int acpiphp_enable_slot(struct acpiphp_slot *slot)
 {
 	pci_lock_rescan_remove();
 
-	if (slot->flags & SLOT_IS_GOING_AWAY)
+	if (slot->flags & SLOT_IS_GOING_AWAY) {
+		pci_unlock_rescan_remove();
 		return -ENODEV;
+	}
 
 	/* configure all functions */
 	if (!(slot->flags & SLOT_ENABLED))
diff --git a/drivers/pci/hotplug/acpiphp_ibm.c b/drivers/pci/hotplug/acpiphp_ibm.c
index 6ca23998ee8f..2f6d3a1c1726 100644
--- a/drivers/pci/hotplug/acpiphp_ibm.c
+++ b/drivers/pci/hotplug/acpiphp_ibm.c
@@ -154,7 +154,8 @@ static union apci_descriptor *ibm_slot_from_id(int id)
 ibm_slot_done:
 	if (ret) {
 		ret = kmalloc(sizeof(union apci_descriptor), GFP_KERNEL);
-		memcpy(ret, des, sizeof(union apci_descriptor));
+		if (ret)
+			memcpy(ret, des, sizeof(union apci_descriptor));
 	}
 	kfree(table);
 	return ret;
@@ -175,8 +176,13 @@ static int ibm_set_attention_status(struct hotplug_slot *slot, u8 status)
 	acpi_status stat;
 	unsigned long long rc;
 	union apci_descriptor *ibm_slot;
+	int id = hpslot_to_sun(slot);
 
-	ibm_slot = ibm_slot_from_id(hpslot_to_sun(slot));
+	ibm_slot = ibm_slot_from_id(id);
+	if (!ibm_slot) {
+		pr_err("APLS null ACPI descriptor for slot %d\n", id);
+		return -ENODEV;
+	}
 
 	pr_debug("%s: set slot %d (%d) attention status to %d\n", __func__,
 			ibm_slot->slot.slot_num, ibm_slot->slot.slot_id,
@@ -215,8 +221,13 @@ static int ibm_set_attention_status(struct hotplug_slot *slot, u8 status)
 static int ibm_get_attention_status(struct hotplug_slot *slot, u8 *status)
 {
 	union apci_descriptor *ibm_slot;
+	int id = hpslot_to_sun(slot);
 
-	ibm_slot = ibm_slot_from_id(hpslot_to_sun(slot));
+	ibm_slot = ibm_slot_from_id(id);
+	if (!ibm_slot) {
+		pr_err("APLS null ACPI descriptor for slot %d\n", id);
+		return -ENODEV;
+	}
 
 	if (ibm_slot->slot.attn & 0xa0 || ibm_slot->slot.status[1] & 0x08)
 		*status = 1;
@@ -325,7 +336,7 @@ static int ibm_get_table_from_acpi(char **bufp)
 	}
 
 	size = 0;
-	for (i=0; i<package->package.count; i++) {
+	for (i = 0; i < package->package.count; i++) {
 		memcpy(&lbuf[size],
 				package->package.elements[i].buffer.pointer,
 				package->package.elements[i].buffer.length);
diff --git a/drivers/pci/hotplug/cpci_hotplug.h b/drivers/pci/hotplug/cpci_hotplug.h
index 6a0ddf757349..555bcde3b196 100644
--- a/drivers/pci/hotplug/cpci_hotplug.h
+++ b/drivers/pci/hotplug/cpci_hotplug.h
@@ -52,13 +52,13 @@ struct slot {
 };
 
 struct cpci_hp_controller_ops {
-	int (*query_enum) (void);
-	int (*enable_irq) (void);
-	int (*disable_irq) (void);
-	int (*check_irq) (void *dev_id);
-	int (*hardware_test) (struct slot *slot, u32 value);
-	u8  (*get_power) (struct slot *slot);
-	int (*set_power) (struct slot *slot, int value);
+	int (*query_enum)(void);
+	int (*enable_irq)(void);
+	int (*disable_irq)(void);
+	int (*check_irq)(void *dev_id);
+	int (*hardware_test)(struct slot *slot, u32 value);
+	u8  (*get_power)(struct slot *slot);
+	int (*set_power)(struct slot *slot, int value);
 };
 
 struct cpci_hp_controller {
diff --git a/drivers/pci/hotplug/cpci_hotplug_core.c b/drivers/pci/hotplug/cpci_hotplug_core.c
index 46db29395a62..7d3866c47312 100644
--- a/drivers/pci/hotplug/cpci_hotplug_core.c
+++ b/drivers/pci/hotplug/cpci_hotplug_core.c
@@ -45,12 +45,12 @@
 #define dbg(format, arg...)					\
 	do {							\
 		if (cpci_debug)					\
-			printk (KERN_DEBUG "%s: " format "\n",	\
-				MY_NAME , ## arg);		\
+			printk(KERN_DEBUG "%s: " format "\n",	\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME, ## arg)
 
 /* local variables */
 static DECLARE_RWSEM(list_rwsem);
@@ -238,21 +238,21 @@ cpci_hp_register_bus(struct pci_bus *bus, u8 first, u8 last)
 	 * with the pci_hotplug subsystem.
 	 */
 	for (i = first; i <= last; ++i) {
-		slot = kzalloc(sizeof (struct slot), GFP_KERNEL);
+		slot = kzalloc(sizeof(struct slot), GFP_KERNEL);
 		if (!slot) {
 			status = -ENOMEM;
 			goto error;
 		}
 
 		hotplug_slot =
-			kzalloc(sizeof (struct hotplug_slot), GFP_KERNEL);
+			kzalloc(sizeof(struct hotplug_slot), GFP_KERNEL);
 		if (!hotplug_slot) {
 			status = -ENOMEM;
 			goto error_slot;
 		}
 		slot->hotplug_slot = hotplug_slot;
 
-		info = kzalloc(sizeof (struct hotplug_slot_info), GFP_KERNEL);
+		info = kzalloc(sizeof(struct hotplug_slot_info), GFP_KERNEL);
 		if (!info) {
 			status = -ENOMEM;
 			goto error_hpslot;
diff --git a/drivers/pci/hotplug/cpci_hotplug_pci.c b/drivers/pci/hotplug/cpci_hotplug_pci.c
index 788db48dbbad..80c80017197d 100644
--- a/drivers/pci/hotplug/cpci_hotplug_pci.c
+++ b/drivers/pci/hotplug/cpci_hotplug_pci.c
@@ -38,12 +38,12 @@ extern int cpci_debug;
 #define dbg(format, arg...)					\
 	do {							\
 		if (cpci_debug)					\
-			printk (KERN_DEBUG "%s: " format "\n",	\
-				MY_NAME , ## arg);		\
+			printk(KERN_DEBUG "%s: " format "\n",	\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME, ## arg)
 
 
 u8 cpci_get_attention_status(struct slot *slot)
diff --git a/drivers/pci/hotplug/cpcihp_generic.c b/drivers/pci/hotplug/cpcihp_generic.c
index 66b7bbebe493..88a44a707b96 100644
--- a/drivers/pci/hotplug/cpcihp_generic.c
+++ b/drivers/pci/hotplug/cpcihp_generic.c
@@ -54,12 +54,12 @@
 #define dbg(format, arg...)					\
 	do {							\
 		if (debug)					\
-			printk (KERN_DEBUG "%s: " format "\n",	\
-				MY_NAME , ## arg);		\
+			printk(KERN_DEBUG "%s: " format "\n",	\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME, ## arg)
 
 /* local variables */
 static bool debug;
@@ -164,7 +164,7 @@ static int __init cpcihp_generic_init(void)
 	bus = dev->subordinate;
 	pci_dev_put(dev);
 
-	memset(&generic_hpc, 0, sizeof (struct cpci_hp_controller));
+	memset(&generic_hpc, 0, sizeof(struct cpci_hp_controller));
 	generic_hpc_ops.query_enum = query_enum;
 	generic_hpc.ops = &generic_hpc_ops;
 
diff --git a/drivers/pci/hotplug/cpcihp_zt5550.c b/drivers/pci/hotplug/cpcihp_zt5550.c
index 7ecf34e76a61..5f49c3fd736a 100644
--- a/drivers/pci/hotplug/cpcihp_zt5550.c
+++ b/drivers/pci/hotplug/cpcihp_zt5550.c
@@ -49,12 +49,12 @@
 #define dbg(format, arg...)					\
 	do {							\
 		if (debug)					\
-			printk (KERN_DEBUG "%s: " format "\n",	\
-				MY_NAME , ## arg);		\
+			printk(KERN_DEBUG "%s: " format "\n",	\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME, ## arg)
 
 /* local variables */
 static bool debug;
@@ -204,7 +204,7 @@ static int zt5550_hc_disable_irq(void)
 	return 0;
 }
 
-static int zt5550_hc_init_one (struct pci_dev *pdev, const struct pci_device_id *ent)
+static int zt5550_hc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	int status;
 
@@ -214,7 +214,7 @@ static int zt5550_hc_init_one (struct pci_dev *pdev, const struct pci_device_id
 
 	dbg("returned from zt5550_hc_config");
 
-	memset(&zt5550_hpc, 0, sizeof (struct cpci_hp_controller));
+	memset(&zt5550_hpc, 0, sizeof(struct cpci_hp_controller));
 	zt5550_hpc_ops.query_enum = zt5550_hc_query_enum;
 	zt5550_hpc.ops = &zt5550_hpc_ops;
 	if (!poll) {
diff --git a/drivers/pci/hotplug/cpqphp.h b/drivers/pci/hotplug/cpqphp.h
index b28b2d2184cd..9103a7b9f3b9 100644
--- a/drivers/pci/hotplug/cpqphp.h
+++ b/drivers/pci/hotplug/cpqphp.h
@@ -36,10 +36,10 @@
 
 #define MY_NAME	"cpqphp"
 
-#define dbg(fmt, arg...) do { if (cpqhp_debug) printk(KERN_DEBUG "%s: " fmt , MY_NAME , ## arg); } while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format , MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format , MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format , MY_NAME , ## arg)
+#define dbg(fmt, arg...) do { if (cpqhp_debug) printk(KERN_DEBUG "%s: " fmt, MY_NAME, ## arg); } while (0)
+#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 
 
@@ -424,7 +424,7 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func);
 int cpqhp_hardware_test(struct controller *ctrl, int test_num);
 
 /* resource functions */
-int	cpqhp_resource_sort_and_combine	(struct pci_resource **head);
+int	cpqhp_resource_sort_and_combine(struct pci_resource **head);
 
 /* pci functions */
 int cpqhp_set_irq(u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num);
@@ -685,7 +685,7 @@ static inline int cpq_get_latch_status(struct controller *ctrl,
 	u8 hp_slot;
 
 	hp_slot = slot->device - ctrl->slot_device_offset;
-	dbg("%s: slot->device = %d, ctrl->slot_device_offset = %d \n",
+	dbg("%s: slot->device = %d, ctrl->slot_device_offset = %d\n",
 	    __func__, slot->device, ctrl->slot_device_offset);
 
 	status = (readl(ctrl->hpc_reg + INT_INPUT_CLEAR) & (0x01L << hp_slot));
@@ -712,7 +712,7 @@ static inline int get_presence_status(struct controller *ctrl,
 
 static inline int wait_for_ctrl_irq(struct controller *ctrl)
 {
-        DECLARE_WAITQUEUE(wait, current);
+	DECLARE_WAITQUEUE(wait, current);
 	int retval = 0;
 
 	dbg("%s - start\n", __func__);
diff --git a/drivers/pci/hotplug/cpqphp_core.c b/drivers/pci/hotplug/cpqphp_core.c
index a53084ddc118..74f3a0695b43 100644
--- a/drivers/pci/hotplug/cpqphp_core.c
+++ b/drivers/pci/hotplug/cpqphp_core.c
@@ -291,7 +291,7 @@ static void release_slot(struct hotplug_slot *hotplug_slot)
 	kfree(slot);
 }
 
-static int ctrl_slot_cleanup (struct controller *ctrl)
+static int ctrl_slot_cleanup(struct controller *ctrl)
 {
 	struct slot *old_slot, *next_slot;
 
@@ -301,7 +301,7 @@ static int ctrl_slot_cleanup (struct controller *ctrl)
 	while (old_slot) {
 		/* memory will be freed by the release_slot callback */
 		next_slot = old_slot->next;
-		pci_hp_deregister (old_slot->hotplug_slot);
+		pci_hp_deregister(old_slot->hotplug_slot);
 		old_slot = next_slot;
 	}
 
@@ -413,9 +413,9 @@ cpqhp_set_attention_status(struct controller *ctrl, struct pci_func *func,
 	mutex_lock(&ctrl->crit_sect);
 
 	if (status == 1)
-		amber_LED_on (ctrl, hp_slot);
+		amber_LED_on(ctrl, hp_slot);
 	else if (status == 0)
-		amber_LED_off (ctrl, hp_slot);
+		amber_LED_off(ctrl, hp_slot);
 	else {
 		/* Done with exclusive hardware access */
 		mutex_unlock(&ctrl->crit_sect);
@@ -425,7 +425,7 @@ cpqhp_set_attention_status(struct controller *ctrl, struct pci_func *func,
 	set_SOGO(ctrl);
 
 	/* Wait for SOBS to be unset */
-	wait_for_ctrl_irq (ctrl);
+	wait_for_ctrl_irq(ctrl);
 
 	/* Done with exclusive hardware access */
 	mutex_unlock(&ctrl->crit_sect);
@@ -439,7 +439,7 @@ cpqhp_set_attention_status(struct controller *ctrl, struct pci_func *func,
  * @hotplug_slot: slot to change LED on
  * @status: LED control flag
  */
-static int set_attention_status (struct hotplug_slot *hotplug_slot, u8 status)
+static int set_attention_status(struct hotplug_slot *hotplug_slot, u8 status)
 {
 	struct pci_func *slot_func;
 	struct slot *slot = hotplug_slot->private;
@@ -610,7 +610,7 @@ static int ctrl_slot_setup(struct controller *ctrl,
 	u8 ctrl_slot;
 	u32 tempdword;
 	char name[SLOT_NAME_SIZE];
-	void __iomem *slot_entry= NULL;
+	void __iomem *slot_entry = NULL;
 	int result;
 
 	dbg("%s\n", __func__);
@@ -755,7 +755,7 @@ static int one_time_init(void)
 	if (cpqhp_debug)
 		pci_print_IRQ_route();
 
-	dbg("Initialize + Start the notification mechanism \n");
+	dbg("Initialize + Start the notification mechanism\n");
 
 	retval = cpqhp_event_start_thread();
 	if (retval)
@@ -772,7 +772,7 @@ static int one_time_init(void)
 	/* Map rom address */
 	cpqhp_rom_start = ioremap(ROM_PHY_ADDR, ROM_PHY_LEN);
 	if (!cpqhp_rom_start) {
-		err ("Could not ioremap memory region for ROM\n");
+		err("Could not ioremap memory region for ROM\n");
 		retval = -EIO;
 		goto error;
 	}
@@ -786,7 +786,7 @@ static int one_time_init(void)
 	smbios_table = detect_SMBIOS_pointer(cpqhp_rom_start,
 					cpqhp_rom_start + ROM_PHY_LEN);
 	if (!smbios_table) {
-		err ("Could not find the SMBIOS pointer in memory\n");
+		err("Could not find the SMBIOS pointer in memory\n");
 		retval = -EIO;
 		goto error_rom_start;
 	}
@@ -794,7 +794,7 @@ static int one_time_init(void)
 	smbios_start = ioremap(readl(smbios_table + ST_ADDRESS),
 					readw(smbios_table + ST_LENGTH));
 	if (!smbios_start) {
-		err ("Could not ioremap memory region taken from SMBIOS values\n");
+		err("Could not ioremap memory region taken from SMBIOS values\n");
 		retval = -EIO;
 		goto error_smbios_start;
 	}
@@ -1181,7 +1181,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	 * Finish setting up the hot plug ctrl device
 	 */
 	ctrl->slot_device_offset = readb(ctrl->hpc_reg + SLOT_MASK) >> 4;
-	dbg("NumSlots %d \n", ctrl->slot_device_offset);
+	dbg("NumSlots %d\n", ctrl->slot_device_offset);
 
 	ctrl->next_event = 0;
 
@@ -1198,7 +1198,7 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	writel(0xFFFFFFFFL, ctrl->hpc_reg + INT_MASK);
 
 	/* set up the interrupt */
-	dbg("HPC interrupt = %d \n", ctrl->interrupt);
+	dbg("HPC interrupt = %d\n", ctrl->interrupt);
 	if (request_irq(ctrl->interrupt, cpqhp_ctrl_intr,
 			IRQF_SHARED, MY_NAME, ctrl)) {
 		err("Can't get irq %d for the hotplug pci controller\n",
@@ -1321,7 +1321,7 @@ static void __exit unload_cpqphpd(void)
 	while (ctrl) {
 		if (ctrl->hpc_reg) {
 			u16 misc;
-			rc = read_slot_enable (ctrl);
+			rc = read_slot_enable(ctrl);
 
 			writeb(0, ctrl->hpc_reg + SLOT_SERR);
 			writel(0xFFFFFFC0L | ~rc, ctrl->hpc_reg + INT_MASK);
@@ -1361,7 +1361,7 @@ static void __exit unload_cpqphpd(void)
 			kfree(tres);
 		}
 
-		kfree (ctrl->pci_bus);
+		kfree(ctrl->pci_bus);
 
 		tctrl = ctrl;
 		ctrl = ctrl->next;
@@ -1446,7 +1446,7 @@ static int __init cpqhpc_init(void)
 
 	cpqhp_debug = debug;
 
-	info (DRIVER_DESC " version: " DRIVER_VERSION "\n");
+	info(DRIVER_DESC " version: " DRIVER_VERSION "\n");
 	cpqhp_initialize_debugfs();
 	result = pci_register_driver(&cpqhpc_driver);
 	dbg("pci_register_driver = %d\n", result);
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index c5cbefee5236..a55653b54eed 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -155,7 +155,7 @@ static u8 handle_presence_change(u16 change, struct controller *ctrl)
 	 * Presence Change
 	 */
 	dbg("cpqsbd:  Presence/Notify input change.\n");
-	dbg("         Changed bits are 0x%4.4x\n", change );
+	dbg("         Changed bits are 0x%4.4x\n", change);
 
 	for (hp_slot = 0; hp_slot < 6; hp_slot++) {
 		if (change & (0x0101 << hp_slot)) {
@@ -276,9 +276,9 @@ static u8 handle_power_fault(u8 change, struct controller *ctrl)
 				taskInfo->event_type = INT_POWER_FAULT;
 
 				if (ctrl->rev < 4) {
-					amber_LED_on (ctrl, hp_slot);
-					green_LED_off (ctrl, hp_slot);
-					set_SOGO (ctrl);
+					amber_LED_on(ctrl, hp_slot);
+					green_LED_off(ctrl, hp_slot);
+					set_SOGO(ctrl);
 
 					/* this is a fatal condition, we want
 					 * to crash the machine to protect from
@@ -438,7 +438,7 @@ static struct pci_resource *do_pre_bridge_resource_split(struct pci_resource **h
 
 	node = *head;
 
-	if (node->length & (alignment -1)) {
+	if (node->length & (alignment - 1)) {
 		/* this one isn't an aligned length, so we'll make a new entry
 		 * and split it up.
 		 */
@@ -835,13 +835,13 @@ int cpqhp_resource_sort_and_combine(struct pci_resource **head)
 	if (!(*head))
 		return 1;
 
-	dbg("*head->next = %p\n",(*head)->next);
+	dbg("*head->next = %p\n", (*head)->next);
 
 	if (!(*head)->next)
 		return 0;	/* only one item on the list, already sorted! */
 
-	dbg("*head->base = 0x%x\n",(*head)->base);
-	dbg("*head->next->base = 0x%x\n",(*head)->next->base);
+	dbg("*head->base = 0x%x\n", (*head)->base);
+	dbg("*head->next->base = 0x%x\n", (*head)->next->base);
 	while (out_of_order) {
 		out_of_order = 0;
 
@@ -917,7 +917,7 @@ irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data)
 		/* Read to clear posted writes */
 		misc = readw(ctrl->hpc_reg + MISC);
 
-		dbg ("%s - waking up\n", __func__);
+		dbg("%s - waking up\n", __func__);
 		wake_up_interruptible(&ctrl->queue);
 	}
 
@@ -1285,18 +1285,18 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 	/*
 	 * The board is already on
 	 */
-	else if (is_slot_enabled (ctrl, hp_slot))
+	else if (is_slot_enabled(ctrl, hp_slot))
 		rc = CARD_FUNCTIONING;
 	else {
 		mutex_lock(&ctrl->crit_sect);
 
 		/* turn on board without attaching to the bus */
-		enable_slot_power (ctrl, hp_slot);
+		enable_slot_power(ctrl, hp_slot);
 
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		/* Change bits in slot power register to force another shift out
 		 * NOTE: this is to work around the timer bug */
@@ -1307,7 +1307,7 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		adapter_speed = get_adapter_speed(ctrl, hp_slot);
 		if (bus->cur_bus_speed != adapter_speed)
@@ -1315,12 +1315,12 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 				rc = WRONG_BUS_FREQUENCY;
 
 		/* turn off board without attaching to the bus */
-		disable_slot_power (ctrl, hp_slot);
+		disable_slot_power(ctrl, hp_slot);
 
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		mutex_unlock(&ctrl->crit_sect);
 
@@ -1329,15 +1329,15 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
 		mutex_lock(&ctrl->crit_sect);
 
-		slot_enable (ctrl, hp_slot);
-		green_LED_blink (ctrl, hp_slot);
+		slot_enable(ctrl, hp_slot);
+		green_LED_blink(ctrl, hp_slot);
 
-		amber_LED_off (ctrl, hp_slot);
+		amber_LED_off(ctrl, hp_slot);
 
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		mutex_unlock(&ctrl->crit_sect);
 
@@ -1366,14 +1366,14 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
 			mutex_lock(&ctrl->crit_sect);
 
-			amber_LED_on (ctrl, hp_slot);
-			green_LED_off (ctrl, hp_slot);
-			slot_disable (ctrl, hp_slot);
+			amber_LED_on(ctrl, hp_slot);
+			green_LED_off(ctrl, hp_slot);
+			slot_disable(ctrl, hp_slot);
 
 			set_SOGO(ctrl);
 
 			/* Wait for SOBS to be unset */
-			wait_for_ctrl_irq (ctrl);
+			wait_for_ctrl_irq(ctrl);
 
 			mutex_unlock(&ctrl->crit_sect);
 
@@ -1392,14 +1392,14 @@ static u32 board_replaced(struct pci_func *func, struct controller *ctrl)
 
 			mutex_lock(&ctrl->crit_sect);
 
-			amber_LED_on (ctrl, hp_slot);
-			green_LED_off (ctrl, hp_slot);
-			slot_disable (ctrl, hp_slot);
+			amber_LED_on(ctrl, hp_slot);
+			green_LED_off(ctrl, hp_slot);
+			slot_disable(ctrl, hp_slot);
 
 			set_SOGO(ctrl);
 
 			/* Wait for SOBS to be unset */
-			wait_for_ctrl_irq (ctrl);
+			wait_for_ctrl_irq(ctrl);
 
 			mutex_unlock(&ctrl->crit_sect);
 		}
@@ -1443,7 +1443,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 	set_SOGO(ctrl);
 
 	/* Wait for SOBS to be unset */
-	wait_for_ctrl_irq (ctrl);
+	wait_for_ctrl_irq(ctrl);
 
 	/* Change bits in slot power register to force another shift out
 	 * NOTE: this is to work around the timer bug
@@ -1455,7 +1455,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 	set_SOGO(ctrl);
 
 	/* Wait for SOBS to be unset */
-	wait_for_ctrl_irq (ctrl);
+	wait_for_ctrl_irq(ctrl);
 
 	adapter_speed = get_adapter_speed(ctrl, hp_slot);
 	if (bus->cur_bus_speed != adapter_speed)
@@ -1463,7 +1463,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 			rc = WRONG_BUS_FREQUENCY;
 
 	/* turn off board without attaching to the bus */
-	disable_slot_power (ctrl, hp_slot);
+	disable_slot_power(ctrl, hp_slot);
 
 	set_SOGO(ctrl);
 
@@ -1484,20 +1484,20 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 	dbg("%s: after down\n", __func__);
 
 	dbg("%s: before slot_enable\n", __func__);
-	slot_enable (ctrl, hp_slot);
+	slot_enable(ctrl, hp_slot);
 
 	dbg("%s: before green_LED_blink\n", __func__);
-	green_LED_blink (ctrl, hp_slot);
+	green_LED_blink(ctrl, hp_slot);
 
 	dbg("%s: before amber_LED_blink\n", __func__);
-	amber_LED_off (ctrl, hp_slot);
+	amber_LED_off(ctrl, hp_slot);
 
 	dbg("%s: before set_SOGO\n", __func__);
 	set_SOGO(ctrl);
 
 	/* Wait for SOBS to be unset */
 	dbg("%s: before wait_for_ctrl_irq\n", __func__);
-	wait_for_ctrl_irq (ctrl);
+	wait_for_ctrl_irq(ctrl);
 	dbg("%s: after wait_for_ctrl_irq\n", __func__);
 
 	dbg("%s: before up\n", __func__);
@@ -1520,7 +1520,7 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 	} else {
 		/* Get vendor/device ID u32 */
 		ctrl->pci_bus->number = func->bus;
-		rc = pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(func->device, func->function), PCI_VENDOR_ID, &temp_register);
+		rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(func->device, func->function), PCI_VENDOR_ID, &temp_register);
 		dbg("%s: pci_read_config_dword returns %d\n", __func__, rc);
 		dbg("%s: temp_register is %x\n", __func__, temp_register);
 
@@ -1557,14 +1557,14 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 		if (rc) {
 			mutex_lock(&ctrl->crit_sect);
 
-			amber_LED_on (ctrl, hp_slot);
-			green_LED_off (ctrl, hp_slot);
-			slot_disable (ctrl, hp_slot);
+			amber_LED_on(ctrl, hp_slot);
+			green_LED_off(ctrl, hp_slot);
+			slot_disable(ctrl, hp_slot);
 
 			set_SOGO(ctrl);
 
 			/* Wait for SOBS to be unset */
-			wait_for_ctrl_irq (ctrl);
+			wait_for_ctrl_irq(ctrl);
 
 			mutex_unlock(&ctrl->crit_sect);
 			return rc;
@@ -1589,25 +1589,25 @@ static u32 board_added(struct pci_func *func, struct controller *ctrl)
 
 		mutex_lock(&ctrl->crit_sect);
 
-		green_LED_on (ctrl, hp_slot);
+		green_LED_on(ctrl, hp_slot);
 
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		mutex_unlock(&ctrl->crit_sect);
 	} else {
 		mutex_lock(&ctrl->crit_sect);
 
-		amber_LED_on (ctrl, hp_slot);
-		green_LED_off (ctrl, hp_slot);
-		slot_disable (ctrl, hp_slot);
+		amber_LED_on(ctrl, hp_slot);
+		green_LED_off(ctrl, hp_slot);
+		slot_disable(ctrl, hp_slot);
 
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 
 		mutex_unlock(&ctrl->crit_sect);
 
@@ -1672,8 +1672,8 @@ static u32 remove_board(struct pci_func *func, u32 replace_flag, struct controll
 
 	mutex_lock(&ctrl->crit_sect);
 
-	green_LED_off (ctrl, hp_slot);
-	slot_disable (ctrl, hp_slot);
+	green_LED_off(ctrl, hp_slot);
+	slot_disable(ctrl, hp_slot);
 
 	set_SOGO(ctrl);
 
@@ -1683,7 +1683,7 @@ static u32 remove_board(struct pci_func *func, u32 replace_flag, struct controll
 	writeb(temp_byte, ctrl->hpc_reg + SLOT_SERR);
 
 	/* Wait for SOBS to be unset */
-	wait_for_ctrl_irq (ctrl);
+	wait_for_ctrl_irq(ctrl);
 
 	mutex_unlock(&ctrl->crit_sect);
 
@@ -1755,7 +1755,7 @@ static int event_thread(void *data)
 		if (pushbutton_pending)
 			cpqhp_pushbutton_thread(pushbutton_pending);
 		else
-			for (ctrl = cpqhp_ctrl_list; ctrl; ctrl=ctrl->next)
+			for (ctrl = cpqhp_ctrl_list; ctrl; ctrl = ctrl->next)
 				interrupt_event_handler(ctrl);
 	}
 	dbg("event_thread signals exit\n");
@@ -1766,7 +1766,7 @@ int cpqhp_event_start_thread(void)
 {
 	cpqhp_event_thread = kthread_run(event_thread, NULL, "phpd_event");
 	if (IS_ERR(cpqhp_event_thread)) {
-		err ("Can't start up our event thread\n");
+		err("Can't start up our event thread\n");
 		return PTR_ERR(cpqhp_event_thread);
 	}
 
@@ -1794,7 +1794,7 @@ static int update_slot_info(struct controller *ctrl, struct slot *slot)
 	info->latch_status = cpq_get_latch_status(ctrl, slot);
 	info->adapter_status = get_presence_status(ctrl, slot);
 	result = pci_hp_change_slot_info(slot->hotplug_slot, info);
-	kfree (info);
+	kfree(info);
 	return result;
 }
 
@@ -1837,23 +1837,23 @@ static void interrupt_event_handler(struct controller *ctrl)
 					if (p_slot->state == BLINKINGOFF_STATE) {
 						/* slot is on */
 						dbg("turn on green LED\n");
-						green_LED_on (ctrl, hp_slot);
+						green_LED_on(ctrl, hp_slot);
 					} else if (p_slot->state == BLINKINGON_STATE) {
 						/* slot is off */
 						dbg("turn off green LED\n");
-						green_LED_off (ctrl, hp_slot);
+						green_LED_off(ctrl, hp_slot);
 					}
 
 					info(msg_button_cancel, p_slot->number);
 
 					p_slot->state = STATIC_STATE;
 
-					amber_LED_off (ctrl, hp_slot);
+					amber_LED_off(ctrl, hp_slot);
 
 					set_SOGO(ctrl);
 
 					/* Wait for SOBS to be unset */
-					wait_for_ctrl_irq (ctrl);
+					wait_for_ctrl_irq(ctrl);
 
 					mutex_unlock(&ctrl->crit_sect);
 				}
@@ -1861,7 +1861,7 @@ static void interrupt_event_handler(struct controller *ctrl)
 				else if (ctrl->event_queue[loop].event_type == INT_BUTTON_RELEASE) {
 					dbg("button release\n");
 
-					if (is_slot_enabled (ctrl, hp_slot)) {
+					if (is_slot_enabled(ctrl, hp_slot)) {
 						dbg("slot is on\n");
 						p_slot->state = BLINKINGOFF_STATE;
 						info(msg_button_off, p_slot->number);
@@ -1874,13 +1874,13 @@ static void interrupt_event_handler(struct controller *ctrl)
 
 					dbg("blink green LED and turn off amber\n");
 
-					amber_LED_off (ctrl, hp_slot);
-					green_LED_blink (ctrl, hp_slot);
+					amber_LED_off(ctrl, hp_slot);
+					green_LED_blink(ctrl, hp_slot);
 
 					set_SOGO(ctrl);
 
 					/* Wait for SOBS to be unset */
-					wait_for_ctrl_irq (ctrl);
+					wait_for_ctrl_irq(ctrl);
 
 					mutex_unlock(&ctrl->crit_sect);
 					init_timer(&p_slot->task_event);
@@ -1940,7 +1940,7 @@ void cpqhp_pushbutton_thread(unsigned long slot)
 		dbg("In power_down_board, func = %p, ctrl = %p\n", func, ctrl);
 		if (!func) {
 			dbg("Error! func NULL in %s\n", __func__);
-			return ;
+			return;
 		}
 
 		if (cpqhp_process_SS(ctrl, func) != 0) {
@@ -1962,7 +1962,7 @@ void cpqhp_pushbutton_thread(unsigned long slot)
 		dbg("In add_board, func = %p, ctrl = %p\n", func, ctrl);
 		if (!func) {
 			dbg("Error! func NULL in %s\n", __func__);
-			return ;
+			return;
 		}
 
 		if (ctrl != NULL) {
@@ -1973,7 +1973,7 @@ void cpqhp_pushbutton_thread(unsigned long slot)
 				set_SOGO(ctrl);
 
 				/* Wait for SOBS to be unset */
-				wait_for_ctrl_irq (ctrl);
+				wait_for_ctrl_irq(ctrl);
 			}
 		}
 
@@ -2086,7 +2086,7 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
 	unsigned int devfn;
 	struct slot *p_slot;
 	struct pci_bus *pci_bus = ctrl->pci_bus;
-	int physical_slot=0;
+	int physical_slot = 0;
 
 	device = func->device;
 	func = cpqhp_slot_find(ctrl->bus, device, index++);
@@ -2100,7 +2100,7 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
 		devfn = PCI_DEVFN(func->device, func->function);
 
 		/* Check the Class Code */
-		rc = pci_bus_read_config_byte (pci_bus, devfn, 0x0B, &class_code);
+		rc = pci_bus_read_config_byte(pci_bus, devfn, 0x0B, &class_code);
 		if (rc)
 			return rc;
 
@@ -2109,13 +2109,13 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
 			rc = REMOVE_NOT_SUPPORTED;
 		} else {
 			/* See if it's a bridge */
-			rc = pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
+			rc = pci_bus_read_config_byte(pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 			if (rc)
 				return rc;
 
 			/* If it's a bridge, check the VGA Enable bit */
 			if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
-				rc = pci_bus_read_config_byte (pci_bus, devfn, PCI_BRIDGE_CONTROL, &BCR);
+				rc = pci_bus_read_config_byte(pci_bus, devfn, PCI_BRIDGE_CONTROL, &BCR);
 				if (rc)
 					return rc;
 
@@ -2217,7 +2217,7 @@ int cpqhp_hardware_test(struct controller *ctrl, int test_num)
 			set_SOGO(ctrl);
 
 			/* Wait for SOGO interrupt */
-			wait_for_ctrl_irq (ctrl);
+			wait_for_ctrl_irq(ctrl);
 
 			/* Get ready for next iteration */
 			long_delay((3*HZ)/10);
@@ -2227,7 +2227,7 @@ int cpqhp_hardware_test(struct controller *ctrl, int test_num)
 			set_SOGO(ctrl);
 
 			/* Wait for SOGO interrupt */
-			wait_for_ctrl_irq (ctrl);
+			wait_for_ctrl_irq(ctrl);
 
 			/* Get ready for next iteration */
 			long_delay((3*HZ)/10);
@@ -2243,7 +2243,7 @@ int cpqhp_hardware_test(struct controller *ctrl, int test_num)
 		set_SOGO(ctrl);
 
 		/* Wait for SOBS to be unset */
-		wait_for_ctrl_irq (ctrl);
+		wait_for_ctrl_irq(ctrl);
 		break;
 	case 2:
 		/* Do other stuff here! */
@@ -2279,7 +2279,7 @@ static u32 configure_new_device(struct controller  *ctrl, struct pci_func  *func
 	dbg("%s\n", __func__);
 	/* Check for Multi-function device */
 	ctrl->pci_bus->number = func->bus;
-	rc = pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(func->device, func->function), 0x0E, &temp_byte);
+	rc = pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(func->device, func->function), 0x0E, &temp_byte);
 	if (rc) {
 		dbg("%s: rc = %d\n", __func__, rc);
 		return rc;
@@ -2296,7 +2296,7 @@ static u32 configure_new_device(struct controller  *ctrl, struct pci_func  *func
 		rc = configure_new_function(ctrl, new_slot, behind_bridge, resources);
 
 		if (rc) {
-			dbg("configure_new_function failed %d\n",rc);
+			dbg("configure_new_function failed %d\n", rc);
 			index = 0;
 
 			while (new_slot) {
@@ -2317,7 +2317,7 @@ static u32 configure_new_device(struct controller  *ctrl, struct pci_func  *func
 		 * and creates a board structure */
 
 		while ((function < max_functions) && (!stop_it)) {
-			pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(func->device, function), 0x00, &ID);
+			pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(func->device, function), 0x00, &ID);
 
 			if (ID == 0xFFFFFFFF) {
 				function++;
@@ -2543,10 +2543,10 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 
 		/* set Pre Mem base and Limit registers */
 		temp_word = p_mem_node->base >> 16;
-		rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_BASE, temp_word);
+		rc = pci_bus_write_config_word(pci_bus, devfn, PCI_PREF_MEMORY_BASE, temp_word);
 
 		temp_word = (p_mem_node->base + p_mem_node->length - 1) >> 16;
-		rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
+		rc = pci_bus_write_config_word(pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
 
 		/* Adjust this to compensate for extra adjustment in first loop
 		 */
@@ -2560,7 +2560,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 
 			ID = 0xFFFFFFFF;
 			pci_bus->number = hold_bus_node->base;
-			pci_bus_read_config_dword (pci_bus, PCI_DEVFN(device, 0), 0x00, &ID);
+			pci_bus_read_config_dword(pci_bus, PCI_DEVFN(device, 0), 0x00, &ID);
 			pci_bus->number = func->bus;
 
 			if (ID != 0xFFFFFFFF) {	  /*  device present */
@@ -2579,7 +2579,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 				new_slot->status = 0;
 
 				rc = configure_new_device(ctrl, new_slot, 1, &temp_resources);
-				dbg("configure_new_device rc=0x%x\n",rc);
+				dbg("configure_new_device rc=0x%x\n", rc);
 			}	/* End of IF (device in slot?) */
 		}		/* End of FOR loop */
 
@@ -2615,7 +2615,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 			temp_byte = temp_resources.bus_head->base - 1;
 
 			/* set subordinate bus */
-			rc = pci_bus_write_config_byte (pci_bus, devfn, PCI_SUBORDINATE_BUS, temp_byte);
+			rc = pci_bus_write_config_byte(pci_bus, devfn, PCI_SUBORDINATE_BUS, temp_byte);
 
 			if (temp_resources.bus_head->length == 0) {
 				kfree(temp_resources.bus_head);
@@ -2636,7 +2636,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 				hold_IO_node->base = io_node->base + io_node->length;
 
 				temp_byte = (hold_IO_node->base) >> 8;
-				rc = pci_bus_write_config_word (pci_bus, devfn, PCI_IO_BASE, temp_byte);
+				rc = pci_bus_write_config_word(pci_bus, devfn, PCI_IO_BASE, temp_byte);
 
 				return_resource(&(resources->io_head), io_node);
 			}
@@ -2655,13 +2655,13 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 					func->io_head = hold_IO_node;
 
 					temp_byte = (io_node->base - 1) >> 8;
-					rc = pci_bus_write_config_byte (pci_bus, devfn, PCI_IO_LIMIT, temp_byte);
+					rc = pci_bus_write_config_byte(pci_bus, devfn, PCI_IO_LIMIT, temp_byte);
 
 					return_resource(&(resources->io_head), io_node);
 				} else {
 					/* it doesn't need any IO */
 					temp_word = 0x0000;
-					rc = pci_bus_write_config_word (pci_bus, devfn, PCI_IO_LIMIT, temp_word);
+					rc = pci_bus_write_config_word(pci_bus, devfn, PCI_IO_LIMIT, temp_word);
 
 					return_resource(&(resources->io_head), io_node);
 					kfree(hold_IO_node);
@@ -2687,7 +2687,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 				hold_mem_node->base = mem_node->base + mem_node->length;
 
 				temp_word = (hold_mem_node->base) >> 16;
-				rc = pci_bus_write_config_word (pci_bus, devfn, PCI_MEMORY_BASE, temp_word);
+				rc = pci_bus_write_config_word(pci_bus, devfn, PCI_MEMORY_BASE, temp_word);
 
 				return_resource(&(resources->mem_head), mem_node);
 			}
@@ -2706,14 +2706,14 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 
 					/* configure end address */
 					temp_word = (mem_node->base - 1) >> 16;
-					rc = pci_bus_write_config_word (pci_bus, devfn, PCI_MEMORY_LIMIT, temp_word);
+					rc = pci_bus_write_config_word(pci_bus, devfn, PCI_MEMORY_LIMIT, temp_word);
 
 					/* Return unused resources to the pool */
 					return_resource(&(resources->mem_head), mem_node);
 				} else {
 					/* it doesn't need any Mem */
 					temp_word = 0x0000;
-					rc = pci_bus_write_config_word (pci_bus, devfn, PCI_MEMORY_LIMIT, temp_word);
+					rc = pci_bus_write_config_word(pci_bus, devfn, PCI_MEMORY_LIMIT, temp_word);
 
 					return_resource(&(resources->mem_head), mem_node);
 					kfree(hold_mem_node);
@@ -2739,7 +2739,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 				hold_p_mem_node->base = p_mem_node->base + p_mem_node->length;
 
 				temp_word = (hold_p_mem_node->base) >> 16;
-				rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_BASE, temp_word);
+				rc = pci_bus_write_config_word(pci_bus, devfn, PCI_PREF_MEMORY_BASE, temp_word);
 
 				return_resource(&(resources->p_mem_head), p_mem_node);
 			}
@@ -2758,13 +2758,13 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 					func->p_mem_head = hold_p_mem_node;
 
 					temp_word = (p_mem_node->base - 1) >> 16;
-					rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
+					rc = pci_bus_write_config_word(pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
 
 					return_resource(&(resources->p_mem_head), p_mem_node);
 				} else {
 					/* it doesn't need any PMem */
 					temp_word = 0x0000;
-					rc = pci_bus_write_config_word (pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
+					rc = pci_bus_write_config_word(pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, temp_word);
 
 					return_resource(&(resources->p_mem_head), p_mem_node);
 					kfree(hold_p_mem_node);
@@ -2790,16 +2790,16 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 					 *   PCI_COMMAND_INVALIDATE |
 					 *   PCI_COMMAND_PARITY |
 					 *   PCI_COMMAND_SERR */
-		rc = pci_bus_write_config_word (pci_bus, devfn, PCI_COMMAND, command);
+		rc = pci_bus_write_config_word(pci_bus, devfn, PCI_COMMAND, command);
 
 		/* set Bridge Control Register */
 		command = 0x07;		/* = PCI_BRIDGE_CTL_PARITY |
 					 *   PCI_BRIDGE_CTL_SERR |
 					 *   PCI_BRIDGE_CTL_NO_ISA */
-		rc = pci_bus_write_config_word (pci_bus, devfn, PCI_BRIDGE_CONTROL, command);
+		rc = pci_bus_write_config_word(pci_bus, devfn, PCI_BRIDGE_CONTROL, command);
 	} else if ((temp_byte & 0x7F) == PCI_HEADER_TYPE_NORMAL) {
 		/* Standard device */
-		rc = pci_bus_read_config_byte (pci_bus, devfn, 0x0B, &class_code);
+		rc = pci_bus_read_config_byte(pci_bus, devfn, 0x0B, &class_code);
 
 		if (class_code == PCI_BASE_CLASS_DISPLAY) {
 			/* Display (video) adapter (not supported) */
@@ -2810,9 +2810,9 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 			temp_register = 0xFFFFFFFF;
 
 			dbg("CND: bus=%d, devfn=%d, offset=%d\n", pci_bus->number, devfn, cloop);
-			rc = pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
+			rc = pci_bus_write_config_dword(pci_bus, devfn, cloop, temp_register);
 
-			rc = pci_bus_read_config_dword (pci_bus, devfn, cloop, &temp_register);
+			rc = pci_bus_read_config_dword(pci_bus, devfn, cloop, &temp_register);
 			dbg("CND: base = 0x%x\n", temp_register);
 
 			if (temp_register) {	  /* If this register is implemented */
@@ -2891,7 +2891,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 		}		/* End of base register loop */
 		if (cpqhp_legacy_mode) {
 			/* Figure out which interrupt pin this function uses */
-			rc = pci_bus_read_config_byte (pci_bus, devfn,
+			rc = pci_bus_read_config_byte(pci_bus, devfn,
 				PCI_INTERRUPT_PIN, &temp_byte);
 
 			/* If this function needs an interrupt and we are behind
@@ -2905,7 +2905,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 					resources->irqs->barber_pole - 1) & 0x03];
 			} else {
 				/* Program IRQ based on card type */
-				rc = pci_bus_read_config_byte (pci_bus, devfn, 0x0B, &class_code);
+				rc = pci_bus_read_config_byte(pci_bus, devfn, 0x0B, &class_code);
 
 				if (class_code == PCI_BASE_CLASS_STORAGE)
 					IRQ = cpqhp_disk_irq;
@@ -2914,7 +2914,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 			}
 
 			/* IRQ Line */
-			rc = pci_bus_write_config_byte (pci_bus, devfn, PCI_INTERRUPT_LINE, IRQ);
+			rc = pci_bus_write_config_byte(pci_bus, devfn, PCI_INTERRUPT_LINE, IRQ);
 		}
 
 		if (!behind_bridge) {
@@ -2950,7 +2950,7 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 					 *   PCI_COMMAND_INVALIDATE |
 					 *   PCI_COMMAND_PARITY |
 					 *   PCI_COMMAND_SERR */
-		rc = pci_bus_write_config_word (pci_bus, devfn,
+		rc = pci_bus_write_config_word(pci_bus, devfn,
 					PCI_COMMAND, temp_word);
 	} else {		/* End of Not-A-Bridge else */
 		/* It's some strange type of PCI adapter (Cardbus?) */
@@ -2961,11 +2961,11 @@ static int configure_new_function(struct controller *ctrl, struct pci_func *func
 
 	return 0;
 free_and_out:
-	cpqhp_destroy_resource_list (&temp_resources);
+	cpqhp_destroy_resource_list(&temp_resources);
 
-	return_resource(&(resources-> bus_head), hold_bus_node);
-	return_resource(&(resources-> io_head), hold_IO_node);
-	return_resource(&(resources-> mem_head), hold_mem_node);
-	return_resource(&(resources-> p_mem_head), hold_p_mem_node);
+	return_resource(&(resources->bus_head), hold_bus_node);
+	return_resource(&(resources->io_head), hold_IO_node);
+	return_resource(&(resources->mem_head), hold_mem_node);
+	return_resource(&(resources->p_mem_head), hold_p_mem_node);
 	return rc;
 }
diff --git a/drivers/pci/hotplug/cpqphp_nvram.c b/drivers/pci/hotplug/cpqphp_nvram.c
index 1e08ff8c229c..c25fc9061059 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.c
+++ b/drivers/pci/hotplug/cpqphp_nvram.c
@@ -114,10 +114,10 @@ static u32 add_byte(u32 **p_buffer, u8 value, u32 *used, u32 *avail)
 	if ((*used + 1) > *avail)
 		return(1);
 
-	*((u8*)*p_buffer) = value;
-	tByte = (u8**)p_buffer;
+	*((u8 *)*p_buffer) = value;
+	tByte = (u8 **)p_buffer;
 	(*tByte)++;
-	*used+=1;
+	*used += 1;
 	return(0);
 }
 
@@ -129,7 +129,7 @@ static u32 add_dword(u32 **p_buffer, u32 value, u32 *used, u32 *avail)
 
 	**p_buffer = value;
 	(*p_buffer)++;
-	*used+=4;
+	*used += 4;
 	return(0);
 }
 
@@ -141,7 +141,7 @@ static u32 add_dword(u32 **p_buffer, u32 value, u32 *used, u32 *avail)
  *
  * returns 0 for non-Compaq ROM, 1 for Compaq ROM
  */
-static int check_for_compaq_ROM (void __iomem *rom_start)
+static int check_for_compaq_ROM(void __iomem *rom_start)
 {
 	u8 temp1, temp2, temp3, temp4, temp5, temp6;
 	int result = 0;
@@ -160,12 +160,12 @@ static int check_for_compaq_ROM (void __iomem *rom_start)
 	    (temp6 == 'Q')) {
 		result = 1;
 	}
-	dbg ("%s - returned %d\n", __func__, result);
+	dbg("%s - returned %d\n", __func__, result);
 	return result;
 }
 
 
-static u32 access_EV (u16 operation, u8 *ev_name, u8 *buffer, u32 *buf_size)
+static u32 access_EV(u16 operation, u8 *ev_name, u8 *buffer, u32 *buf_size)
 {
 	unsigned long flags;
 	int op = operation;
@@ -197,7 +197,7 @@ static u32 access_EV (u16 operation, u8 *ev_name, u8 *buffer, u32 *buf_size)
  *
  * Read the hot plug Resource Table from NVRAM
  */
-static int load_HRT (void __iomem *rom_start)
+static int load_HRT(void __iomem *rom_start)
 {
 	u32 available;
 	u32 temp_dword;
@@ -232,7 +232,7 @@ static int load_HRT (void __iomem *rom_start)
  *
  * Save the hot plug Resource Table in NVRAM
  */
-static u32 store_HRT (void __iomem *rom_start)
+static u32 store_HRT(void __iomem *rom_start)
 {
 	u32 *buffer;
 	u32 *pFill;
@@ -252,7 +252,7 @@ static u32 store_HRT (void __iomem *rom_start)
 	if (!check_for_compaq_ROM(rom_start))
 		return(1);
 
-	buffer = (u32*) evbuffer;
+	buffer = (u32 *) evbuffer;
 
 	if (!buffer)
 		return(1);
@@ -306,7 +306,7 @@ static u32 store_HRT (void __iomem *rom_start)
 		loop = 0;
 
 		while (resNode) {
-			loop ++;
+			loop++;
 
 			/* base */
 			rc = add_dword(&pFill, resNode->base, &usedbytes, &available);
@@ -331,7 +331,7 @@ static u32 store_HRT (void __iomem *rom_start)
 		loop = 0;
 
 		while (resNode) {
-			loop ++;
+			loop++;
 
 			/* base */
 			rc = add_dword(&pFill, resNode->base, &usedbytes, &available);
@@ -356,7 +356,7 @@ static u32 store_HRT (void __iomem *rom_start)
 		loop = 0;
 
 		while (resNode) {
-			loop ++;
+			loop++;
 
 			/* base */
 			rc = add_dword(&pFill, resNode->base, &usedbytes, &available);
@@ -381,7 +381,7 @@ static u32 store_HRT (void __iomem *rom_start)
 		loop = 0;
 
 		while (resNode) {
-			loop ++;
+			loop++;
 
 			/* base */
 			rc = add_dword(&pFill, resNode->base, &usedbytes, &available);
@@ -408,7 +408,7 @@ static u32 store_HRT (void __iomem *rom_start)
 
 	temp_dword = usedbytes;
 
-	rc = access_EV(WRITE_EV, "CQTHPS", (u8*) buffer, &temp_dword);
+	rc = access_EV(WRITE_EV, "CQTHPS", (u8 *) buffer, &temp_dword);
 
 	dbg("usedbytes = 0x%x, length = 0x%x\n", usedbytes, temp_dword);
 
@@ -423,7 +423,7 @@ static u32 store_HRT (void __iomem *rom_start)
 }
 
 
-void compaq_nvram_init (void __iomem *rom_start)
+void compaq_nvram_init(void __iomem *rom_start)
 {
 	if (rom_start)
 		compaq_int15_entry_point = (rom_start + ROM_INT15_PHY_ADDR - ROM_PHY_ADDR);
@@ -435,7 +435,7 @@ void compaq_nvram_init (void __iomem *rom_start)
 }
 
 
-int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
+int compaq_nvram_load(void __iomem *rom_start, struct controller *ctrl)
 {
 	u8 bus, device, function;
 	u8 nummem, numpmem, numio, numbus;
@@ -451,7 +451,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 	if (!evbuffer_init) {
 		/* Read the resource list information in from NVRAM */
 		if (load_HRT(rom_start))
-			memset (evbuffer, 0, 1024);
+			memset(evbuffer, 0, 1024);
 
 		evbuffer_init = 1;
 	}
@@ -472,7 +472,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 
 		p_byte += 3;
 
-		if (p_byte > ((u8*)p_EV_header + evbuffer_length))
+		if (p_byte > ((u8 *)p_EV_header + evbuffer_length))
 			return 2;
 
 		bus = p_ev_ctrl->bus;
@@ -489,20 +489,20 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length))
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length))
 				return 2;
 
 			/* Skip forward to the next entry */
 			p_byte += (nummem + numpmem + numio + numbus) * 8;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length))
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length))
 				return 2;
 
 			p_ev_ctrl = (struct ev_hrt_ctrl *) p_byte;
 
 			p_byte += 3;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length))
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length))
 				return 2;
 
 			bus = p_ev_ctrl->bus;
@@ -517,7 +517,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 
 		p_byte += 4;
 
-		if (p_byte > ((u8*)p_EV_header + evbuffer_length))
+		if (p_byte > ((u8 *)p_EV_header + evbuffer_length))
 			return 2;
 
 		while (nummem--) {
@@ -526,20 +526,20 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 			if (!mem_node)
 				break;
 
-			mem_node->base = *(u32*)p_byte;
-			dbg("mem base = %8.8x\n",mem_node->base);
+			mem_node->base = *(u32 *)p_byte;
+			dbg("mem base = %8.8x\n", mem_node->base);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(mem_node);
 				return 2;
 			}
 
-			mem_node->length = *(u32*)p_byte;
-			dbg("mem length = %8.8x\n",mem_node->length);
+			mem_node->length = *(u32 *)p_byte;
+			dbg("mem length = %8.8x\n", mem_node->length);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(mem_node);
 				return 2;
 			}
@@ -554,20 +554,20 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 			if (!p_mem_node)
 				break;
 
-			p_mem_node->base = *(u32*)p_byte;
-			dbg("pre-mem base = %8.8x\n",p_mem_node->base);
+			p_mem_node->base = *(u32 *)p_byte;
+			dbg("pre-mem base = %8.8x\n", p_mem_node->base);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(p_mem_node);
 				return 2;
 			}
 
-			p_mem_node->length = *(u32*)p_byte;
-			dbg("pre-mem length = %8.8x\n",p_mem_node->length);
+			p_mem_node->length = *(u32 *)p_byte;
+			dbg("pre-mem length = %8.8x\n", p_mem_node->length);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(p_mem_node);
 				return 2;
 			}
@@ -582,20 +582,20 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 			if (!io_node)
 				break;
 
-			io_node->base = *(u32*)p_byte;
-			dbg("io base = %8.8x\n",io_node->base);
+			io_node->base = *(u32 *)p_byte;
+			dbg("io base = %8.8x\n", io_node->base);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(io_node);
 				return 2;
 			}
 
-			io_node->length = *(u32*)p_byte;
-			dbg("io length = %8.8x\n",io_node->length);
+			io_node->length = *(u32 *)p_byte;
+			dbg("io length = %8.8x\n", io_node->length);
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(io_node);
 				return 2;
 			}
@@ -610,18 +610,18 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 			if (!bus_node)
 				break;
 
-			bus_node->base = *(u32*)p_byte;
+			bus_node->base = *(u32 *)p_byte;
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(bus_node);
 				return 2;
 			}
 
-			bus_node->length = *(u32*)p_byte;
+			bus_node->length = *(u32 *)p_byte;
 			p_byte += 4;
 
-			if (p_byte > ((u8*)p_EV_header + evbuffer_length)) {
+			if (p_byte > ((u8 *)p_EV_header + evbuffer_length)) {
 				kfree(bus_node);
 				return 2;
 			}
@@ -650,7 +650,7 @@ int compaq_nvram_load (void __iomem *rom_start, struct controller *ctrl)
 }
 
 
-int compaq_nvram_store (void __iomem *rom_start)
+int compaq_nvram_store(void __iomem *rom_start)
 {
 	int rc = 1;
 
diff --git a/drivers/pci/hotplug/cpqphp_pci.c b/drivers/pci/hotplug/cpqphp_pci.c
index 1c8c2f130d31..e220d49307bd 100644
--- a/drivers/pci/hotplug/cpqphp_pci.c
+++ b/drivers/pci/hotplug/cpqphp_pci.c
@@ -81,7 +81,7 @@ static void __iomem *detect_HRT_floating_pointer(void __iomem *begin, void __iom
 }
 
 
-int cpqhp_configure_device (struct controller *ctrl, struct pci_func *func)
+int cpqhp_configure_device(struct controller *ctrl, struct pci_func *func)
 {
 	struct pci_bus *child;
 	int num;
@@ -89,7 +89,7 @@ int cpqhp_configure_device (struct controller *ctrl, struct pci_func *func)
 	pci_lock_rescan_remove();
 
 	if (func->pci_dev == NULL)
-		func->pci_dev = pci_get_bus_and_slot(func->bus,PCI_DEVFN(func->device, func->function));
+		func->pci_dev = pci_get_bus_and_slot(func->bus, PCI_DEVFN(func->device, func->function));
 
 	/* No pci device, we need to create it then */
 	if (func->pci_dev == NULL) {
@@ -128,7 +128,7 @@ int cpqhp_unconfigure_device(struct pci_func *func)
 	dbg("%s: bus/dev/func = %x/%x/%x\n", __func__, func->bus, func->device, func->function);
 
 	pci_lock_rescan_remove();
-	for (j=0; j<8 ; j++) {
+	for (j = 0; j < 8 ; j++) {
 		struct pci_dev *temp = pci_get_bus_and_slot(func->bus, PCI_DEVFN(func->device, j));
 		if (temp) {
 			pci_dev_put(temp);
@@ -143,11 +143,11 @@ static int PCI_RefinedAccessConfig(struct pci_bus *bus, unsigned int devfn, u8 o
 {
 	u32 vendID = 0;
 
-	if (pci_bus_read_config_dword (bus, devfn, PCI_VENDOR_ID, &vendID) == -1)
+	if (pci_bus_read_config_dword(bus, devfn, PCI_VENDOR_ID, &vendID) == -1)
 		return -1;
 	if (vendID == 0xffffffff)
 		return -1;
-	return pci_bus_read_config_dword (bus, devfn, offset, value);
+	return pci_bus_read_config_dword(bus, devfn, offset, value);
 }
 
 
@@ -158,7 +158,7 @@ static int PCI_RefinedAccessConfig(struct pci_bus *bus, unsigned int devfn, u8 o
  * @dev_num: device number of PCI device
  * @slot: pointer to u8 where slot number will be returned
  */
-int cpqhp_set_irq (u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num)
+int cpqhp_set_irq(u8 bus_num, u8 dev_num, u8 int_pin, u8 irq_num)
 {
 	int rc = 0;
 
@@ -230,7 +230,7 @@ static int PCI_ScanBusForNonBridge(struct controller *ctrl, u8 bus_num, u8 *dev_
 		dbg("Looking for bridge bus_num %d dev_num %d\n", bus_num, tdevice);
 		/* Yep we got one. bridge ? */
 		if ((work >> 8) == PCI_TO_PCI_BRIDGE_CLASS) {
-			pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(tdevice, 0), PCI_SECONDARY_BUS, &tbus);
+			pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(tdevice, 0), PCI_SECONDARY_BUS, &tbus);
 			/* XXX: no recursion, wtf? */
 			dbg("Recurse on bus_num %d tdevice %d\n", tbus, tdevice);
 			return 0;
@@ -257,16 +257,16 @@ static int PCI_GetBusDevHelper(struct controller *ctrl, u8 *bus_num, u8 *dev_num
 			*bus_num = tbus;
 			*dev_num = tdevice;
 			ctrl->pci_bus->number = tbus;
-			pci_bus_read_config_dword (ctrl->pci_bus, *dev_num, PCI_VENDOR_ID, &work);
+			pci_bus_read_config_dword(ctrl->pci_bus, *dev_num, PCI_VENDOR_ID, &work);
 			if (!nobridge || (work == 0xffffffff))
 				return 0;
 
 			dbg("bus_num %d devfn %d\n", *bus_num, *dev_num);
-			pci_bus_read_config_dword (ctrl->pci_bus, *dev_num, PCI_CLASS_REVISION, &work);
+			pci_bus_read_config_dword(ctrl->pci_bus, *dev_num, PCI_CLASS_REVISION, &work);
 			dbg("work >> 8 (%x) = BRIDGE (%x)\n", work >> 8, PCI_TO_PCI_BRIDGE_CLASS);
 
 			if ((work >> 8) == PCI_TO_PCI_BRIDGE_CLASS) {
-				pci_bus_read_config_byte (ctrl->pci_bus, *dev_num, PCI_SECONDARY_BUS, &tbus);
+				pci_bus_read_config_byte(ctrl->pci_bus, *dev_num, PCI_SECONDARY_BUS, &tbus);
 				dbg("Scan bus for Non Bridge: bus %d\n", tbus);
 				if (PCI_ScanBusForNonBridge(ctrl, tbus, dev_num) == 0) {
 					*bus_num = tbus;
@@ -280,7 +280,7 @@ static int PCI_GetBusDevHelper(struct controller *ctrl, u8 *bus_num, u8 *dev_num
 }
 
 
-int cpqhp_get_bus_dev (struct controller *ctrl, u8 *bus_num, u8 *dev_num, u8 slot)
+int cpqhp_get_bus_dev(struct controller *ctrl, u8 *bus_num, u8 *dev_num, u8 slot)
 {
 	/* plain (bridges allowed) */
 	return PCI_GetBusDevHelper(ctrl, bus_num, dev_num, slot, 0);
@@ -419,7 +419,7 @@ int cpqhp_save_config(struct controller *ctrl, int busnumber, int is_hot_plug)
 			new_slot->pci_dev = pci_get_bus_and_slot(new_slot->bus, (new_slot->device << 3) | new_slot->function);
 
 			for (cloop = 0; cloop < 0x20; cloop++) {
-				rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
+				rc = pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(device, function), cloop << 2, (u32 *) &(new_slot->config_space[cloop]));
 				if (rc)
 					return rc;
 			}
@@ -465,7 +465,7 @@ int cpqhp_save_config(struct controller *ctrl, int busnumber, int is_hot_plug)
  *
  * returns 0 if success
  */
-int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func *new_slot)
+int cpqhp_save_slot_config(struct controller *ctrl, struct pci_func *new_slot)
 {
 	long rc;
 	u8 class_code;
@@ -481,7 +481,7 @@ int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func *new_slot)
 	ID = 0xFFFFFFFF;
 
 	ctrl->pci_bus->number = new_slot->bus;
-	pci_bus_read_config_dword (ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), PCI_VENDOR_ID, &ID);
+	pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(new_slot->device, 0), PCI_VENDOR_ID, &ID);
 
 	if (ID == 0xFFFFFFFF)
 		return 2;
@@ -497,7 +497,7 @@ int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func *new_slot)
 	while (function < max_functions) {
 		if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
 			/*  Recurse the subordinate bus */
-			pci_bus_read_config_byte (ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_SECONDARY_BUS, &secondary_bus);
+			pci_bus_read_config_byte(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), PCI_SECONDARY_BUS, &secondary_bus);
 
 			sub_bus = (int) secondary_bus;
 
@@ -514,7 +514,7 @@ int cpqhp_save_slot_config (struct controller *ctrl, struct pci_func *new_slot)
 		new_slot->status = 0;
 
 		for (cloop = 0; cloop < 0x20; cloop++)
-			pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), cloop << 2, (u32 *) & (new_slot-> config_space [cloop]));
+			pci_bus_read_config_dword(ctrl->pci_bus, PCI_DEVFN(new_slot->device, function), cloop << 2, (u32 *) &(new_slot->config_space[cloop]));
 
 		function++;
 
@@ -571,10 +571,10 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func *func)
 		devfn = PCI_DEVFN(func->device, func->function);
 
 		/* Check for Bridge */
-		pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
+		pci_bus_read_config_byte(pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
 		if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
-			pci_bus_read_config_byte (pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
+			pci_bus_read_config_byte(pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
 
 			sub_bus = (int) secondary_bus;
 
@@ -595,8 +595,8 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func *func)
 			 */
 			for (cloop = 0x10; cloop <= 0x14; cloop += 4) {
 				temp_register = 0xFFFFFFFF;
-				pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
-				pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
+				pci_bus_write_config_dword(pci_bus, devfn, cloop, temp_register);
+				pci_bus_read_config_dword(pci_bus, devfn, cloop, &base);
 				/* If this register is implemented */
 				if (base) {
 					if (base & 0x01L) {
@@ -631,8 +631,8 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func *func)
 			/* Figure out IO and memory base lengths */
 			for (cloop = 0x10; cloop <= 0x24; cloop += 4) {
 				temp_register = 0xFFFFFFFF;
-				pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
-				pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
+				pci_bus_write_config_dword(pci_bus, devfn, cloop, temp_register);
+				pci_bus_read_config_dword(pci_bus, devfn, cloop, &base);
 
 				/* If this register is implemented */
 				if (base) {
@@ -686,7 +686,7 @@ int cpqhp_save_base_addr_length(struct controller *ctrl, struct pci_func *func)
  *
  * returns 0 if success
  */
-int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func *func)
+int cpqhp_save_used_resources(struct controller *ctrl, struct pci_func *func)
 {
 	u8 cloop;
 	u8 header_type;
@@ -791,7 +791,7 @@ int cpqhp_save_used_resources (struct controller *ctrl, struct pci_func *func)
 			}
 			/* Figure out IO and memory base lengths */
 			for (cloop = 0x10; cloop <= 0x14; cloop += 4) {
-				pci_bus_read_config_dword (pci_bus, devfn, cloop, &save_base);
+				pci_bus_read_config_dword(pci_bus, devfn, cloop, &save_base);
 
 				temp_register = 0xFFFFFFFF;
 				pci_bus_write_config_dword(pci_bus, devfn, cloop, temp_register);
@@ -972,13 +972,13 @@ int cpqhp_configure_board(struct controller *ctrl, struct pci_func *func)
 		 * registers are programmed last
 		 */
 		for (cloop = 0x3C; cloop > 0; cloop -= 4)
-			pci_bus_write_config_dword (pci_bus, devfn, cloop, func->config_space[cloop >> 2]);
+			pci_bus_write_config_dword(pci_bus, devfn, cloop, func->config_space[cloop >> 2]);
 
-		pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
+		pci_bus_read_config_byte(pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
 		/* If this is a bridge device, restore subordinate devices */
 		if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
-			pci_bus_read_config_byte (pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
+			pci_bus_read_config_byte(pci_bus, devfn, PCI_SECONDARY_BUS, &secondary_bus);
 
 			sub_bus = (int) secondary_bus;
 
@@ -998,7 +998,7 @@ int cpqhp_configure_board(struct controller *ctrl, struct pci_func *func)
 			 */
 
 			for (cloop = 16; cloop < 40; cloop += 4) {
-				pci_bus_read_config_dword (pci_bus, devfn, cloop, &temp);
+				pci_bus_read_config_dword(pci_bus, devfn, cloop, &temp);
 
 				if (temp != func->config_space[cloop >> 2]) {
 					dbg("Config space compare failure!!! offset = %x\n", cloop);
@@ -1050,7 +1050,7 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func)
 		pci_bus->number = func->bus;
 		devfn = PCI_DEVFN(func->device, func->function);
 
-		pci_bus_read_config_dword (pci_bus, devfn, PCI_VENDOR_ID, &temp_register);
+		pci_bus_read_config_dword(pci_bus, devfn, PCI_VENDOR_ID, &temp_register);
 
 		/* No adapter present */
 		if (temp_register == 0xFFFFFFFF)
@@ -1060,14 +1060,14 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func)
 			return(ADAPTER_NOT_SAME);
 
 		/* Check for same revision number and class code */
-		pci_bus_read_config_dword (pci_bus, devfn, PCI_CLASS_REVISION, &temp_register);
+		pci_bus_read_config_dword(pci_bus, devfn, PCI_CLASS_REVISION, &temp_register);
 
 		/* Adapter not the same */
 		if (temp_register != func->config_space[0x08 >> 2])
 			return(ADAPTER_NOT_SAME);
 
 		/* Check for Bridge */
-		pci_bus_read_config_byte (pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
+		pci_bus_read_config_byte(pci_bus, devfn, PCI_HEADER_TYPE, &header_type);
 
 		if ((header_type & 0x7F) == PCI_HEADER_TYPE_BRIDGE) {
 			/* In order to continue checking, we must program the
@@ -1076,7 +1076,7 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func)
 			 */
 
 			temp_register = func->config_space[0x18 >> 2];
-			pci_bus_write_config_dword (pci_bus, devfn, PCI_PRIMARY_BUS, temp_register);
+			pci_bus_write_config_dword(pci_bus, devfn, PCI_PRIMARY_BUS, temp_register);
 
 			secondary_bus = (temp_register >> 8) & 0xFF;
 
@@ -1094,7 +1094,7 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func)
 		/* Check to see if it is a standard config header */
 		else if ((header_type & 0x7F) == PCI_HEADER_TYPE_NORMAL) {
 			/* Check subsystem vendor and ID */
-			pci_bus_read_config_dword (pci_bus, devfn, PCI_SUBSYSTEM_VENDOR_ID, &temp_register);
+			pci_bus_read_config_dword(pci_bus, devfn, PCI_SUBSYSTEM_VENDOR_ID, &temp_register);
 
 			if (temp_register != func->config_space[0x2C >> 2]) {
 				/* If it's a SMART-2 and the register isn't
@@ -1108,8 +1108,8 @@ int cpqhp_valid_replace(struct controller *ctrl, struct pci_func *func)
 			/* Figure out IO and memory base lengths */
 			for (cloop = 0x10; cloop <= 0x24; cloop += 4) {
 				temp_register = 0xFFFFFFFF;
-				pci_bus_write_config_dword (pci_bus, devfn, cloop, temp_register);
-				pci_bus_read_config_dword (pci_bus, devfn, cloop, &base);
+				pci_bus_write_config_dword(pci_bus, devfn, cloop, temp_register);
+				pci_bus_read_config_dword(pci_bus, devfn, cloop, &base);
 
 				/* If this register is implemented */
 				if (base) {
@@ -1234,7 +1234,7 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
 	if (rc)
 		return rc;
 
-	one_slot = rom_resource_table + sizeof (struct hrt);
+	one_slot = rom_resource_table + sizeof(struct hrt);
 
 	i = readb(rom_resource_table + NUMBER_OF_ENTRIES);
 	dbg("number_of_entries = %d\n", i);
@@ -1263,12 +1263,12 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
 		/* If this entry isn't for our controller's bus, ignore it */
 		if (primary_bus != ctrl->bus) {
 			i--;
-			one_slot += sizeof (struct slot_rt);
+			one_slot += sizeof(struct slot_rt);
 			continue;
 		}
 		/* find out if this entry is for an occupied slot */
 		ctrl->pci_bus->number = primary_bus;
-		pci_bus_read_config_dword (ctrl->pci_bus, dev_func, PCI_VENDOR_ID, &temp_dword);
+		pci_bus_read_config_dword(ctrl->pci_bus, dev_func, PCI_VENDOR_ID, &temp_dword);
 		dbg("temp_D_word = %x\n", temp_dword);
 
 		if (temp_dword != 0xFFFFFFFF) {
@@ -1283,7 +1283,7 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
 			/* If we can't find a match, skip this table entry */
 			if (!func) {
 				i--;
-				one_slot += sizeof (struct slot_rt);
+				one_slot += sizeof(struct slot_rt);
 				continue;
 			}
 			/* this may not work and shouldn't be used */
@@ -1395,7 +1395,7 @@ int cpqhp_find_available_resources(struct controller *ctrl, void __iomem *rom_st
 		}
 
 		i--;
-		one_slot += sizeof (struct slot_rt);
+		one_slot += sizeof(struct slot_rt);
 	}
 
 	/* If all of the following fail, we don't have any resources for
@@ -1475,7 +1475,7 @@ int cpqhp_return_board_resources(struct pci_func *func, struct resource_lists *r
  *
  * Puts node back in the resource list pointed to by head
  */
-void cpqhp_destroy_resource_list (struct resource_lists *resources)
+void cpqhp_destroy_resource_list(struct resource_lists *resources)
 {
 	struct pci_resource *res, *tres;
 
@@ -1522,7 +1522,7 @@ void cpqhp_destroy_resource_list (struct resource_lists *resources)
  *
  * Puts node back in the resource list pointed to by head
  */
-void cpqhp_destroy_board_resources (struct pci_func *func)
+void cpqhp_destroy_board_resources(struct pci_func *func)
 {
 	struct pci_resource *res, *tres;
 
diff --git a/drivers/pci/hotplug/cpqphp_sysfs.c b/drivers/pci/hotplug/cpqphp_sysfs.c
index d81648f71425..775974deda74 100644
--- a/drivers/pci/hotplug/cpqphp_sysfs.c
+++ b/drivers/pci/hotplug/cpqphp_sysfs.c
@@ -39,7 +39,7 @@
 #include "cpqphp.h"
 
 static DEFINE_MUTEX(cpqphp_mutex);
-static int show_ctrl (struct controller *ctrl, char *buf)
+static int show_ctrl(struct controller *ctrl, char *buf)
 {
 	char *out = buf;
 	int index;
@@ -77,7 +77,7 @@ static int show_ctrl (struct controller *ctrl, char *buf)
 	return out - buf;
 }
 
-static int show_dev (struct controller *ctrl, char *buf)
+static int show_dev(struct controller *ctrl, char *buf)
 {
 	char *out = buf;
 	int index;
@@ -119,7 +119,7 @@ static int show_dev (struct controller *ctrl, char *buf)
 			out += sprintf(out, "start = %8.8x, length = %8.8x\n", res->base, res->length);
 			res = res->next;
 		}
-		slot=slot->next;
+		slot = slot->next;
 	}
 
 	return out - buf;
diff --git a/drivers/pci/hotplug/ibmphp.h b/drivers/pci/hotplug/ibmphp.h
index e3e46a7b3ee7..d3256838cb05 100644
--- a/drivers/pci/hotplug/ibmphp.h
+++ b/drivers/pci/hotplug/ibmphp.h
@@ -39,11 +39,11 @@ extern int ibmphp_debug;
 #else
 	#define MY_NAME THIS_MODULE->name
 #endif
-#define debug(fmt, arg...) do { if (ibmphp_debug == 1) printk(KERN_DEBUG "%s: " fmt , MY_NAME , ## arg); } while (0)
-#define debug_pci(fmt, arg...) do { if (ibmphp_debug) printk(KERN_DEBUG "%s: " fmt , MY_NAME , ## arg); } while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format , MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format , MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format , MY_NAME , ## arg)
+#define debug(fmt, arg...) do { if (ibmphp_debug == 1) printk(KERN_DEBUG "%s: " fmt, MY_NAME, ## arg); } while (0)
+#define debug_pci(fmt, arg...) do { if (ibmphp_debug) printk(KERN_DEBUG "%s: " fmt, MY_NAME, ## arg); } while (0)
+#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 
 /* EBDA stuff */
@@ -603,7 +603,7 @@ void ibmphp_hpc_stop_poll_thread(void);
 #define SLOT_CONNECT(s)	((u8) ((s & HPC_SLOT_CONNECT) \
 	? HPC_SLOT_DISCONNECTED : HPC_SLOT_CONNECTED))
 
-#define SLOT_ATTN(s,es)	((u8) ((es & HPC_SLOT_BLINK_ATTN) \
+#define SLOT_ATTN(s, es)	((u8) ((es & HPC_SLOT_BLINK_ATTN) \
 	? HPC_SLOT_ATTN_BLINK \
 	: ((s & HPC_SLOT_ATTN) ? HPC_SLOT_ATTN_ON : HPC_SLOT_ATTN_OFF)))
 
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index 15302475f5b7..5efd01d84498 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -39,11 +39,11 @@
 #include <asm/io_apic.h>
 #include "ibmphp.h"
 
-#define attn_on(sl)  ibmphp_hpc_writeslot (sl, HPC_SLOT_ATTNON)
-#define attn_off(sl) ibmphp_hpc_writeslot (sl, HPC_SLOT_ATTNOFF)
-#define attn_LED_blink(sl) ibmphp_hpc_writeslot (sl, HPC_SLOT_BLINKLED)
-#define get_ctrl_revision(sl, rev) ibmphp_hpc_readslot (sl, READ_REVLEVEL, rev)
-#define get_hpc_options(sl, opt) ibmphp_hpc_readslot (sl, READ_HPCOPTIONS, opt)
+#define attn_on(sl)  ibmphp_hpc_writeslot(sl, HPC_SLOT_ATTNON)
+#define attn_off(sl) ibmphp_hpc_writeslot(sl, HPC_SLOT_ATTNOFF)
+#define attn_LED_blink(sl) ibmphp_hpc_writeslot(sl, HPC_SLOT_BLINKLED)
+#define get_ctrl_revision(sl, rev) ibmphp_hpc_readslot(sl, READ_REVLEVEL, rev)
+#define get_hpc_options(sl, opt) ibmphp_hpc_readslot(sl, READ_HPCOPTIONS, opt)
 
 #define DRIVER_VERSION	"0.6"
 #define DRIVER_DESC	"IBM Hot Plug PCI Controller Driver"
@@ -52,9 +52,9 @@ int ibmphp_debug;
 
 static bool debug;
 module_param(debug, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC (debug, "Debugging mode enabled or not");
-MODULE_LICENSE ("GPL");
-MODULE_DESCRIPTION (DRIVER_DESC);
+MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION(DRIVER_DESC);
 
 struct pci_bus *ibmphp_pci_bus;
 static int max_slots;
@@ -113,14 +113,12 @@ static inline int slot_update(struct slot **sl)
 	return rc;
 }
 
-static int __init get_max_slots (void)
+static int __init get_max_slots(void)
 {
 	struct slot *slot_cur;
-	struct list_head *tmp;
 	u8 slot_count = 0;
 
-	list_for_each(tmp, &ibmphp_slot_head) {
-		slot_cur = list_entry(tmp, struct slot, ibm_slot_list);
+	list_for_each_entry(slot_cur, &ibmphp_slot_head, ibm_slot_list) {
 		/* sometimes the hot-pluggable slots start with 4 (not always from 1) */
 		slot_count = max(slot_count, slot_cur->number);
 	}
@@ -459,7 +457,7 @@ static int get_max_adapter_speed_1(struct hotplug_slot *hotplug_slot, u8 *value,
 					*value = SLOT_SPEED(myslot.ext_status);
 			} else
 				*value = MAX_ADAPTER_NONE;
-                }
+		}
 	}
 
 	if (flag)
@@ -501,16 +499,10 @@ static int get_bus_name(struct hotplug_slot *hotplug_slot, char *value)
 static int __init init_ops(void)
 {
 	struct slot *slot_cur;
-	struct list_head *tmp;
 	int retval;
 	int rc;
 
-	list_for_each(tmp, &ibmphp_slot_head) {
-		slot_cur = list_entry(tmp, struct slot, ibm_slot_list);
-
-		if (!slot_cur)
-			return -ENODEV;
-
+	list_for_each_entry(slot_cur, &ibmphp_slot_head, ibm_slot_list) {
 		debug("BEFORE GETTING SLOT STATUS, slot # %x\n",
 							slot_cur->number);
 		if (slot_cur->ctrl->revision == 0xFF)
@@ -620,11 +612,11 @@ int ibmphp_update_slot_info(struct slot *slot_cur)
 	info->attention_status = SLOT_ATTN(slot_cur->status,
 						slot_cur->ext_status);
 	info->latch_status = SLOT_LATCH(slot_cur->status);
-        if (!SLOT_PRESENT(slot_cur->status)) {
-                info->adapter_status = 0;
+	if (!SLOT_PRESENT(slot_cur->status)) {
+		info->adapter_status = 0;
 /*		info->max_adapter_speed_status = MAX_ADAPTER_NONE; */
 	} else {
-                info->adapter_status = 1;
+		info->adapter_status = 1;
 /*		get_max_adapter_speed_1(slot_cur->hotplug_slot,
 					&info->max_adapter_speed_status, 0); */
 	}
@@ -669,9 +661,7 @@ static struct pci_func *ibm_slot_find(u8 busno, u8 device, u8 function)
 {
 	struct pci_func *func_cur;
 	struct slot *slot_cur;
-	struct list_head *tmp;
-	list_for_each(tmp, &ibmphp_slot_head) {
-		slot_cur = list_entry(tmp, struct slot, ibm_slot_list);
+	list_for_each_entry(slot_cur, &ibmphp_slot_head, ibm_slot_list) {
 		if (slot_cur->func) {
 			func_cur = slot_cur->func;
 			while (func_cur) {
@@ -693,14 +683,12 @@ static struct pci_func *ibm_slot_find(u8 busno, u8 device, u8 function)
  *************************************************************/
 static void free_slots(void)
 {
-	struct slot *slot_cur;
-	struct list_head *tmp;
-	struct list_head *next;
+	struct slot *slot_cur, *next;
 
 	debug("%s -- enter\n", __func__);
 
-	list_for_each_safe(tmp, next, &ibmphp_slot_head) {
-		slot_cur = list_entry(tmp, struct slot, ibm_slot_list);
+	list_for_each_entry_safe(slot_cur, next, &ibmphp_slot_head,
+				 ibm_slot_list) {
 		pci_hp_deregister(slot_cur->hotplug_slot);
 	}
 	debug("%s -- exit\n", __func__);
@@ -866,7 +854,7 @@ static int set_bus(struct slot *slot_cur)
 	int retval;
 	static struct pci_device_id ciobx[] = {
 		{ PCI_DEVICE(PCI_VENDOR_ID_SERVERWORKS, 0x0101) },
-	        { },
+		{ },
 	};
 
 	debug("%s - entry slot # %d\n", __func__, slot_cur->number);
@@ -1182,7 +1170,7 @@ error_power:
 * HOT REMOVING ADAPTER CARD                                   *
 * INPUT: POINTER TO THE HOTPLUG SLOT STRUCTURE                *
 * OUTPUT: SUCCESS 0 ; FAILURE: UNCONFIGURE , VALIDATE         *
-          DISABLE POWER ,                                    *
+*		DISABLE POWER ,                               *
 **************************************************************/
 static int ibmphp_disable_slot(struct hotplug_slot *hotplug_slot)
 {
diff --git a/drivers/pci/hotplug/ibmphp_ebda.c b/drivers/pci/hotplug/ibmphp_ebda.c
index d9b197d5c6b4..43e345ac296b 100644
--- a/drivers/pci/hotplug/ibmphp_ebda.c
+++ b/drivers/pci/hotplug/ibmphp_ebda.c
@@ -49,32 +49,32 @@
  */
 
 /* Global lists */
-LIST_HEAD (ibmphp_ebda_pci_rsrc_head);
-LIST_HEAD (ibmphp_slot_head);
+LIST_HEAD(ibmphp_ebda_pci_rsrc_head);
+LIST_HEAD(ibmphp_slot_head);
 
 /* Local variables */
 static struct ebda_hpc_list *hpc_list_ptr;
 static struct ebda_rsrc_list *rsrc_list_ptr;
 static struct rio_table_hdr *rio_table_ptr = NULL;
-static LIST_HEAD (ebda_hpc_head);
-static LIST_HEAD (bus_info_head);
-static LIST_HEAD (rio_vg_head);
-static LIST_HEAD (rio_lo_head);
-static LIST_HEAD (opt_vg_head);
-static LIST_HEAD (opt_lo_head);
+static LIST_HEAD(ebda_hpc_head);
+static LIST_HEAD(bus_info_head);
+static LIST_HEAD(rio_vg_head);
+static LIST_HEAD(rio_lo_head);
+static LIST_HEAD(opt_vg_head);
+static LIST_HEAD(opt_lo_head);
 static void __iomem *io_mem;
 
 /* Local functions */
-static int ebda_rsrc_controller (void);
-static int ebda_rsrc_rsrc (void);
-static int ebda_rio_table (void);
+static int ebda_rsrc_controller(void);
+static int ebda_rsrc_rsrc(void);
+static int ebda_rio_table(void);
 
-static struct ebda_hpc_list * __init alloc_ebda_hpc_list (void)
+static struct ebda_hpc_list * __init alloc_ebda_hpc_list(void)
 {
 	return kzalloc(sizeof(struct ebda_hpc_list), GFP_KERNEL);
 }
 
-static struct controller *alloc_ebda_hpc (u32 slot_count, u32 bus_count)
+static struct controller *alloc_ebda_hpc(u32 slot_count, u32 bus_count)
 {
 	struct controller *controller;
 	struct ebda_hpc_slot *slots;
@@ -103,146 +103,146 @@ error:
 	return NULL;
 }
 
-static void free_ebda_hpc (struct controller *controller)
+static void free_ebda_hpc(struct controller *controller)
 {
-	kfree (controller->slots);
-	kfree (controller->buses);
-	kfree (controller);
+	kfree(controller->slots);
+	kfree(controller->buses);
+	kfree(controller);
 }
 
-static struct ebda_rsrc_list * __init alloc_ebda_rsrc_list (void)
+static struct ebda_rsrc_list * __init alloc_ebda_rsrc_list(void)
 {
 	return kzalloc(sizeof(struct ebda_rsrc_list), GFP_KERNEL);
 }
 
-static struct ebda_pci_rsrc *alloc_ebda_pci_rsrc (void)
+static struct ebda_pci_rsrc *alloc_ebda_pci_rsrc(void)
 {
 	return kzalloc(sizeof(struct ebda_pci_rsrc), GFP_KERNEL);
 }
 
-static void __init print_bus_info (void)
+static void __init print_bus_info(void)
 {
 	struct bus_info *ptr;
 
 	list_for_each_entry(ptr, &bus_info_head, bus_info_list) {
-		debug ("%s - slot_min = %x\n", __func__, ptr->slot_min);
-		debug ("%s - slot_max = %x\n", __func__, ptr->slot_max);
-		debug ("%s - slot_count = %x\n", __func__, ptr->slot_count);
-		debug ("%s - bus# = %x\n", __func__, ptr->busno);
-		debug ("%s - current_speed = %x\n", __func__, ptr->current_speed);
-		debug ("%s - controller_id = %x\n", __func__, ptr->controller_id);
-
-		debug ("%s - slots_at_33_conv = %x\n", __func__, ptr->slots_at_33_conv);
-		debug ("%s - slots_at_66_conv = %x\n", __func__, ptr->slots_at_66_conv);
-		debug ("%s - slots_at_66_pcix = %x\n", __func__, ptr->slots_at_66_pcix);
-		debug ("%s - slots_at_100_pcix = %x\n", __func__, ptr->slots_at_100_pcix);
-		debug ("%s - slots_at_133_pcix = %x\n", __func__, ptr->slots_at_133_pcix);
+		debug("%s - slot_min = %x\n", __func__, ptr->slot_min);
+		debug("%s - slot_max = %x\n", __func__, ptr->slot_max);
+		debug("%s - slot_count = %x\n", __func__, ptr->slot_count);
+		debug("%s - bus# = %x\n", __func__, ptr->busno);
+		debug("%s - current_speed = %x\n", __func__, ptr->current_speed);
+		debug("%s - controller_id = %x\n", __func__, ptr->controller_id);
+
+		debug("%s - slots_at_33_conv = %x\n", __func__, ptr->slots_at_33_conv);
+		debug("%s - slots_at_66_conv = %x\n", __func__, ptr->slots_at_66_conv);
+		debug("%s - slots_at_66_pcix = %x\n", __func__, ptr->slots_at_66_pcix);
+		debug("%s - slots_at_100_pcix = %x\n", __func__, ptr->slots_at_100_pcix);
+		debug("%s - slots_at_133_pcix = %x\n", __func__, ptr->slots_at_133_pcix);
 
 	}
 }
 
-static void print_lo_info (void)
+static void print_lo_info(void)
 {
 	struct rio_detail *ptr;
-	debug ("print_lo_info ----\n");
+	debug("print_lo_info ----\n");
 	list_for_each_entry(ptr, &rio_lo_head, rio_detail_list) {
-		debug ("%s - rio_node_id = %x\n", __func__, ptr->rio_node_id);
-		debug ("%s - rio_type = %x\n", __func__, ptr->rio_type);
-		debug ("%s - owner_id = %x\n", __func__, ptr->owner_id);
-		debug ("%s - first_slot_num = %x\n", __func__, ptr->first_slot_num);
-		debug ("%s - wpindex = %x\n", __func__, ptr->wpindex);
-		debug ("%s - chassis_num = %x\n", __func__, ptr->chassis_num);
+		debug("%s - rio_node_id = %x\n", __func__, ptr->rio_node_id);
+		debug("%s - rio_type = %x\n", __func__, ptr->rio_type);
+		debug("%s - owner_id = %x\n", __func__, ptr->owner_id);
+		debug("%s - first_slot_num = %x\n", __func__, ptr->first_slot_num);
+		debug("%s - wpindex = %x\n", __func__, ptr->wpindex);
+		debug("%s - chassis_num = %x\n", __func__, ptr->chassis_num);
 
 	}
 }
 
-static void print_vg_info (void)
+static void print_vg_info(void)
 {
 	struct rio_detail *ptr;
-	debug ("%s ---\n", __func__);
+	debug("%s ---\n", __func__);
 	list_for_each_entry(ptr, &rio_vg_head, rio_detail_list) {
-		debug ("%s - rio_node_id = %x\n", __func__, ptr->rio_node_id);
-		debug ("%s - rio_type = %x\n", __func__, ptr->rio_type);
-		debug ("%s - owner_id = %x\n", __func__, ptr->owner_id);
-		debug ("%s - first_slot_num = %x\n", __func__, ptr->first_slot_num);
-		debug ("%s - wpindex = %x\n", __func__, ptr->wpindex);
-		debug ("%s - chassis_num = %x\n", __func__, ptr->chassis_num);
+		debug("%s - rio_node_id = %x\n", __func__, ptr->rio_node_id);
+		debug("%s - rio_type = %x\n", __func__, ptr->rio_type);
+		debug("%s - owner_id = %x\n", __func__, ptr->owner_id);
+		debug("%s - first_slot_num = %x\n", __func__, ptr->first_slot_num);
+		debug("%s - wpindex = %x\n", __func__, ptr->wpindex);
+		debug("%s - chassis_num = %x\n", __func__, ptr->chassis_num);
 
 	}
 }
 
-static void __init print_ebda_pci_rsrc (void)
+static void __init print_ebda_pci_rsrc(void)
 {
 	struct ebda_pci_rsrc *ptr;
 
 	list_for_each_entry(ptr, &ibmphp_ebda_pci_rsrc_head, ebda_pci_rsrc_list) {
-		debug ("%s - rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
-			__func__, ptr->rsrc_type ,ptr->bus_num, ptr->dev_fun,ptr->start_addr, ptr->end_addr);
+		debug("%s - rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
+			__func__, ptr->rsrc_type, ptr->bus_num, ptr->dev_fun, ptr->start_addr, ptr->end_addr);
 	}
 }
 
-static void __init print_ibm_slot (void)
+static void __init print_ibm_slot(void)
 {
 	struct slot *ptr;
 
 	list_for_each_entry(ptr, &ibmphp_slot_head, ibm_slot_list) {
-		debug ("%s - slot_number: %x\n", __func__, ptr->number);
+		debug("%s - slot_number: %x\n", __func__, ptr->number);
 	}
 }
 
-static void __init print_opt_vg (void)
+static void __init print_opt_vg(void)
 {
 	struct opt_rio *ptr;
-	debug ("%s ---\n", __func__);
+	debug("%s ---\n", __func__);
 	list_for_each_entry(ptr, &opt_vg_head, opt_rio_list) {
-		debug ("%s - rio_type %x\n", __func__, ptr->rio_type);
-		debug ("%s - chassis_num: %x\n", __func__, ptr->chassis_num);
-		debug ("%s - first_slot_num: %x\n", __func__, ptr->first_slot_num);
-		debug ("%s - middle_num: %x\n", __func__, ptr->middle_num);
+		debug("%s - rio_type %x\n", __func__, ptr->rio_type);
+		debug("%s - chassis_num: %x\n", __func__, ptr->chassis_num);
+		debug("%s - first_slot_num: %x\n", __func__, ptr->first_slot_num);
+		debug("%s - middle_num: %x\n", __func__, ptr->middle_num);
 	}
 }
 
-static void __init print_ebda_hpc (void)
+static void __init print_ebda_hpc(void)
 {
 	struct controller *hpc_ptr;
 	u16 index;
 
 	list_for_each_entry(hpc_ptr, &ebda_hpc_head, ebda_hpc_list) {
 		for (index = 0; index < hpc_ptr->slot_count; index++) {
-			debug ("%s - physical slot#: %x\n", __func__, hpc_ptr->slots[index].slot_num);
-			debug ("%s - pci bus# of the slot: %x\n", __func__, hpc_ptr->slots[index].slot_bus_num);
-			debug ("%s - index into ctlr addr: %x\n", __func__, hpc_ptr->slots[index].ctl_index);
-			debug ("%s - cap of the slot: %x\n", __func__, hpc_ptr->slots[index].slot_cap);
+			debug("%s - physical slot#: %x\n", __func__, hpc_ptr->slots[index].slot_num);
+			debug("%s - pci bus# of the slot: %x\n", __func__, hpc_ptr->slots[index].slot_bus_num);
+			debug("%s - index into ctlr addr: %x\n", __func__, hpc_ptr->slots[index].ctl_index);
+			debug("%s - cap of the slot: %x\n", __func__, hpc_ptr->slots[index].slot_cap);
 		}
 
 		for (index = 0; index < hpc_ptr->bus_count; index++)
-			debug ("%s - bus# of each bus controlled by this ctlr: %x\n", __func__, hpc_ptr->buses[index].bus_num);
+			debug("%s - bus# of each bus controlled by this ctlr: %x\n", __func__, hpc_ptr->buses[index].bus_num);
 
-		debug ("%s - type of hpc: %x\n", __func__, hpc_ptr->ctlr_type);
+		debug("%s - type of hpc: %x\n", __func__, hpc_ptr->ctlr_type);
 		switch (hpc_ptr->ctlr_type) {
 		case 1:
-			debug ("%s - bus: %x\n", __func__, hpc_ptr->u.pci_ctlr.bus);
-			debug ("%s - dev_fun: %x\n", __func__, hpc_ptr->u.pci_ctlr.dev_fun);
-			debug ("%s - irq: %x\n", __func__, hpc_ptr->irq);
+			debug("%s - bus: %x\n", __func__, hpc_ptr->u.pci_ctlr.bus);
+			debug("%s - dev_fun: %x\n", __func__, hpc_ptr->u.pci_ctlr.dev_fun);
+			debug("%s - irq: %x\n", __func__, hpc_ptr->irq);
 			break;
 
 		case 0:
-			debug ("%s - io_start: %x\n", __func__, hpc_ptr->u.isa_ctlr.io_start);
-			debug ("%s - io_end: %x\n", __func__, hpc_ptr->u.isa_ctlr.io_end);
-			debug ("%s - irq: %x\n", __func__, hpc_ptr->irq);
+			debug("%s - io_start: %x\n", __func__, hpc_ptr->u.isa_ctlr.io_start);
+			debug("%s - io_end: %x\n", __func__, hpc_ptr->u.isa_ctlr.io_end);
+			debug("%s - irq: %x\n", __func__, hpc_ptr->irq);
 			break;
 
 		case 2:
 		case 4:
-			debug ("%s - wpegbbar: %lx\n", __func__, hpc_ptr->u.wpeg_ctlr.wpegbbar);
-			debug ("%s - i2c_addr: %x\n", __func__, hpc_ptr->u.wpeg_ctlr.i2c_addr);
-			debug ("%s - irq: %x\n", __func__, hpc_ptr->irq);
+			debug("%s - wpegbbar: %lx\n", __func__, hpc_ptr->u.wpeg_ctlr.wpegbbar);
+			debug("%s - i2c_addr: %x\n", __func__, hpc_ptr->u.wpeg_ctlr.i2c_addr);
+			debug("%s - irq: %x\n", __func__, hpc_ptr->irq);
 			break;
 		}
 	}
 }
 
-int __init ibmphp_access_ebda (void)
+int __init ibmphp_access_ebda(void)
 {
 	u8 format, num_ctlrs, rio_complete, hs_complete, ebda_sz;
 	u16 ebda_seg, num_entries, next_offset, offset, blk_id, sub_addr, re, rc_id, re_id, base;
@@ -252,12 +252,12 @@ int __init ibmphp_access_ebda (void)
 	rio_complete = 0;
 	hs_complete = 0;
 
-	io_mem = ioremap ((0x40 << 4) + 0x0e, 2);
-	if (!io_mem )
+	io_mem = ioremap((0x40 << 4) + 0x0e, 2);
+	if (!io_mem)
 		return -ENOMEM;
-	ebda_seg = readw (io_mem);
-	iounmap (io_mem);
-	debug ("returned ebda segment: %x\n", ebda_seg);
+	ebda_seg = readw(io_mem);
+	iounmap(io_mem);
+	debug("returned ebda segment: %x\n", ebda_seg);
 
 	io_mem = ioremap(ebda_seg<<4, 1);
 	if (!io_mem)
@@ -269,7 +269,7 @@ int __init ibmphp_access_ebda (void)
 		return -ENOMEM;
 
 	io_mem = ioremap(ebda_seg<<4, (ebda_sz * 1024));
-	if (!io_mem )
+	if (!io_mem)
 		return -ENOMEM;
 	next_offset = 0x180;
 
@@ -281,12 +281,12 @@ int __init ibmphp_access_ebda (void)
 			 "ibmphp_ebda: next read is beyond ebda_sz\n"))
 			break;
 
-		next_offset = readw (io_mem + offset);	/* offset of next blk */
+		next_offset = readw(io_mem + offset);	/* offset of next blk */
 
 		offset += 2;
 		if (next_offset == 0)	/* 0 indicate it's last blk */
 			break;
-		blk_id = readw (io_mem + offset);	/* this blk id */
+		blk_id = readw(io_mem + offset);	/* this blk id */
 
 		offset += 2;
 		/* check if it is hot swap block or rio block */
@@ -294,31 +294,31 @@ int __init ibmphp_access_ebda (void)
 			continue;
 		/* found hs table */
 		if (blk_id == 0x4853) {
-			debug ("now enter hot swap block---\n");
-			debug ("hot blk id: %x\n", blk_id);
-			format = readb (io_mem + offset);
+			debug("now enter hot swap block---\n");
+			debug("hot blk id: %x\n", blk_id);
+			format = readb(io_mem + offset);
 
 			offset += 1;
 			if (format != 4)
 				goto error_nodev;
-			debug ("hot blk format: %x\n", format);
+			debug("hot blk format: %x\n", format);
 			/* hot swap sub blk */
 			base = offset;
 
 			sub_addr = base;
-			re = readw (io_mem + sub_addr);	/* next sub blk */
+			re = readw(io_mem + sub_addr);	/* next sub blk */
 
 			sub_addr += 2;
-			rc_id = readw (io_mem + sub_addr);	/* sub blk id */
+			rc_id = readw(io_mem + sub_addr);	/* sub blk id */
 
 			sub_addr += 2;
 			if (rc_id != 0x5243)
 				goto error_nodev;
 			/* rc sub blk signature  */
-			num_ctlrs = readb (io_mem + sub_addr);
+			num_ctlrs = readb(io_mem + sub_addr);
 
 			sub_addr += 1;
-			hpc_list_ptr = alloc_ebda_hpc_list ();
+			hpc_list_ptr = alloc_ebda_hpc_list();
 			if (!hpc_list_ptr) {
 				rc = -ENOMEM;
 				goto out;
@@ -326,28 +326,28 @@ int __init ibmphp_access_ebda (void)
 			hpc_list_ptr->format = format;
 			hpc_list_ptr->num_ctlrs = num_ctlrs;
 			hpc_list_ptr->phys_addr = sub_addr;	/*  offset of RSRC_CONTROLLER blk */
-			debug ("info about hpc descriptor---\n");
-			debug ("hot blk format: %x\n", format);
-			debug ("num of controller: %x\n", num_ctlrs);
-			debug ("offset of hpc data structure entries: %x\n ", sub_addr);
+			debug("info about hpc descriptor---\n");
+			debug("hot blk format: %x\n", format);
+			debug("num of controller: %x\n", num_ctlrs);
+			debug("offset of hpc data structure entries: %x\n ", sub_addr);
 
 			sub_addr = base + re;	/* re sub blk */
 			/* FIXME: rc is never used/checked */
-			rc = readw (io_mem + sub_addr);	/* next sub blk */
+			rc = readw(io_mem + sub_addr);	/* next sub blk */
 
 			sub_addr += 2;
-			re_id = readw (io_mem + sub_addr);	/* sub blk id */
+			re_id = readw(io_mem + sub_addr);	/* sub blk id */
 
 			sub_addr += 2;
 			if (re_id != 0x5245)
 				goto error_nodev;
 
 			/* signature of re */
-			num_entries = readw (io_mem + sub_addr);
+			num_entries = readw(io_mem + sub_addr);
 
 			sub_addr += 2;	/* offset of RSRC_ENTRIES blk */
-			rsrc_list_ptr = alloc_ebda_rsrc_list ();
-			if (!rsrc_list_ptr ) {
+			rsrc_list_ptr = alloc_ebda_rsrc_list();
+			if (!rsrc_list_ptr) {
 				rc = -ENOMEM;
 				goto out;
 			}
@@ -355,26 +355,26 @@ int __init ibmphp_access_ebda (void)
 			rsrc_list_ptr->num_entries = num_entries;
 			rsrc_list_ptr->phys_addr = sub_addr;
 
-			debug ("info about rsrc descriptor---\n");
-			debug ("format: %x\n", format);
-			debug ("num of rsrc: %x\n", num_entries);
-			debug ("offset of rsrc data structure entries: %x\n ", sub_addr);
+			debug("info about rsrc descriptor---\n");
+			debug("format: %x\n", format);
+			debug("num of rsrc: %x\n", num_entries);
+			debug("offset of rsrc data structure entries: %x\n ", sub_addr);
 
 			hs_complete = 1;
 		} else {
 		/* found rio table, blk_id == 0x4752 */
-			debug ("now enter io table ---\n");
-			debug ("rio blk id: %x\n", blk_id);
+			debug("now enter io table ---\n");
+			debug("rio blk id: %x\n", blk_id);
 
 			rio_table_ptr = kzalloc(sizeof(struct rio_table_hdr), GFP_KERNEL);
 			if (!rio_table_ptr) {
 				rc = -ENOMEM;
 				goto out;
 			}
-			rio_table_ptr->ver_num = readb (io_mem + offset);
-			rio_table_ptr->scal_count = readb (io_mem + offset + 1);
-			rio_table_ptr->riodev_count = readb (io_mem + offset + 2);
-			rio_table_ptr->offset = offset +3 ;
+			rio_table_ptr->ver_num = readb(io_mem + offset);
+			rio_table_ptr->scal_count = readb(io_mem + offset + 1);
+			rio_table_ptr->riodev_count = readb(io_mem + offset + 2);
+			rio_table_ptr->offset = offset + 3 ;
 
 			debug("info about rio table hdr ---\n");
 			debug("ver_num: %x\nscal_count: %x\nriodev_count: %x\noffset of rio table: %x\n ",
@@ -390,28 +390,28 @@ int __init ibmphp_access_ebda (void)
 
 	if (rio_table_ptr) {
 		if (rio_complete && rio_table_ptr->ver_num == 3) {
-			rc = ebda_rio_table ();
+			rc = ebda_rio_table();
 			if (rc)
 				goto out;
 		}
 	}
-	rc = ebda_rsrc_controller ();
+	rc = ebda_rsrc_controller();
 	if (rc)
 		goto out;
 
-	rc = ebda_rsrc_rsrc ();
+	rc = ebda_rsrc_rsrc();
 	goto out;
 error_nodev:
 	rc = -ENODEV;
 out:
-	iounmap (io_mem);
+	iounmap(io_mem);
 	return rc;
 }
 
 /*
  * map info of scalability details and rio details from physical address
  */
-static int __init ebda_rio_table (void)
+static int __init ebda_rio_table(void)
 {
 	u16 offset;
 	u8 i;
@@ -425,39 +425,39 @@ static int __init ebda_rio_table (void)
 		rio_detail_ptr = kzalloc(sizeof(struct rio_detail), GFP_KERNEL);
 		if (!rio_detail_ptr)
 			return -ENOMEM;
-		rio_detail_ptr->rio_node_id = readb (io_mem + offset);
-		rio_detail_ptr->bbar = readl (io_mem + offset + 1);
-		rio_detail_ptr->rio_type = readb (io_mem + offset + 5);
-		rio_detail_ptr->owner_id = readb (io_mem + offset + 6);
-		rio_detail_ptr->port0_node_connect = readb (io_mem + offset + 7);
-		rio_detail_ptr->port0_port_connect = readb (io_mem + offset + 8);
-		rio_detail_ptr->port1_node_connect = readb (io_mem + offset + 9);
-		rio_detail_ptr->port1_port_connect = readb (io_mem + offset + 10);
-		rio_detail_ptr->first_slot_num = readb (io_mem + offset + 11);
-		rio_detail_ptr->status = readb (io_mem + offset + 12);
-		rio_detail_ptr->wpindex = readb (io_mem + offset + 13);
-		rio_detail_ptr->chassis_num = readb (io_mem + offset + 14);
-//		debug ("rio_node_id: %x\nbbar: %x\nrio_type: %x\nowner_id: %x\nport0_node: %x\nport0_port: %x\nport1_node: %x\nport1_port: %x\nfirst_slot_num: %x\nstatus: %x\n", rio_detail_ptr->rio_node_id, rio_detail_ptr->bbar, rio_detail_ptr->rio_type, rio_detail_ptr->owner_id, rio_detail_ptr->port0_node_connect, rio_detail_ptr->port0_port_connect, rio_detail_ptr->port1_node_connect, rio_detail_ptr->port1_port_connect, rio_detail_ptr->first_slot_num, rio_detail_ptr->status);
+		rio_detail_ptr->rio_node_id = readb(io_mem + offset);
+		rio_detail_ptr->bbar = readl(io_mem + offset + 1);
+		rio_detail_ptr->rio_type = readb(io_mem + offset + 5);
+		rio_detail_ptr->owner_id = readb(io_mem + offset + 6);
+		rio_detail_ptr->port0_node_connect = readb(io_mem + offset + 7);
+		rio_detail_ptr->port0_port_connect = readb(io_mem + offset + 8);
+		rio_detail_ptr->port1_node_connect = readb(io_mem + offset + 9);
+		rio_detail_ptr->port1_port_connect = readb(io_mem + offset + 10);
+		rio_detail_ptr->first_slot_num = readb(io_mem + offset + 11);
+		rio_detail_ptr->status = readb(io_mem + offset + 12);
+		rio_detail_ptr->wpindex = readb(io_mem + offset + 13);
+		rio_detail_ptr->chassis_num = readb(io_mem + offset + 14);
+//		debug("rio_node_id: %x\nbbar: %x\nrio_type: %x\nowner_id: %x\nport0_node: %x\nport0_port: %x\nport1_node: %x\nport1_port: %x\nfirst_slot_num: %x\nstatus: %x\n", rio_detail_ptr->rio_node_id, rio_detail_ptr->bbar, rio_detail_ptr->rio_type, rio_detail_ptr->owner_id, rio_detail_ptr->port0_node_connect, rio_detail_ptr->port0_port_connect, rio_detail_ptr->port1_node_connect, rio_detail_ptr->port1_port_connect, rio_detail_ptr->first_slot_num, rio_detail_ptr->status);
 		//create linked list of chassis
 		if (rio_detail_ptr->rio_type == 4 || rio_detail_ptr->rio_type == 5)
-			list_add (&rio_detail_ptr->rio_detail_list, &rio_vg_head);
+			list_add(&rio_detail_ptr->rio_detail_list, &rio_vg_head);
 		//create linked list of expansion box
 		else if (rio_detail_ptr->rio_type == 6 || rio_detail_ptr->rio_type == 7)
-			list_add (&rio_detail_ptr->rio_detail_list, &rio_lo_head);
+			list_add(&rio_detail_ptr->rio_detail_list, &rio_lo_head);
 		else
 			// not in my concern
-			kfree (rio_detail_ptr);
+			kfree(rio_detail_ptr);
 		offset += 15;
 	}
-	print_lo_info ();
-	print_vg_info ();
+	print_lo_info();
+	print_vg_info();
 	return 0;
 }
 
 /*
  * reorganizing linked list of chassis
  */
-static struct opt_rio *search_opt_vg (u8 chassis_num)
+static struct opt_rio *search_opt_vg(u8 chassis_num)
 {
 	struct opt_rio *ptr;
 	list_for_each_entry(ptr, &opt_vg_head, opt_rio_list) {
@@ -467,13 +467,13 @@ static struct opt_rio *search_opt_vg (u8 chassis_num)
 	return NULL;
 }
 
-static int __init combine_wpg_for_chassis (void)
+static int __init combine_wpg_for_chassis(void)
 {
 	struct opt_rio *opt_rio_ptr = NULL;
 	struct rio_detail *rio_detail_ptr = NULL;
 
 	list_for_each_entry(rio_detail_ptr, &rio_vg_head, rio_detail_list) {
-		opt_rio_ptr = search_opt_vg (rio_detail_ptr->chassis_num);
+		opt_rio_ptr = search_opt_vg(rio_detail_ptr->chassis_num);
 		if (!opt_rio_ptr) {
 			opt_rio_ptr = kzalloc(sizeof(struct opt_rio), GFP_KERNEL);
 			if (!opt_rio_ptr)
@@ -482,20 +482,20 @@ static int __init combine_wpg_for_chassis (void)
 			opt_rio_ptr->chassis_num = rio_detail_ptr->chassis_num;
 			opt_rio_ptr->first_slot_num = rio_detail_ptr->first_slot_num;
 			opt_rio_ptr->middle_num = rio_detail_ptr->first_slot_num;
-			list_add (&opt_rio_ptr->opt_rio_list, &opt_vg_head);
+			list_add(&opt_rio_ptr->opt_rio_list, &opt_vg_head);
 		} else {
-			opt_rio_ptr->first_slot_num = min (opt_rio_ptr->first_slot_num, rio_detail_ptr->first_slot_num);
-			opt_rio_ptr->middle_num = max (opt_rio_ptr->middle_num, rio_detail_ptr->first_slot_num);
+			opt_rio_ptr->first_slot_num = min(opt_rio_ptr->first_slot_num, rio_detail_ptr->first_slot_num);
+			opt_rio_ptr->middle_num = max(opt_rio_ptr->middle_num, rio_detail_ptr->first_slot_num);
 		}
 	}
-	print_opt_vg ();
+	print_opt_vg();
 	return 0;
 }
 
 /*
  * reorganizing linked list of expansion box
  */
-static struct opt_rio_lo *search_opt_lo (u8 chassis_num)
+static struct opt_rio_lo *search_opt_lo(u8 chassis_num)
 {
 	struct opt_rio_lo *ptr;
 	list_for_each_entry(ptr, &opt_lo_head, opt_rio_lo_list) {
@@ -505,13 +505,13 @@ static struct opt_rio_lo *search_opt_lo (u8 chassis_num)
 	return NULL;
 }
 
-static int combine_wpg_for_expansion (void)
+static int combine_wpg_for_expansion(void)
 {
 	struct opt_rio_lo *opt_rio_lo_ptr = NULL;
 	struct rio_detail *rio_detail_ptr = NULL;
 
 	list_for_each_entry(rio_detail_ptr, &rio_lo_head, rio_detail_list) {
-		opt_rio_lo_ptr = search_opt_lo (rio_detail_ptr->chassis_num);
+		opt_rio_lo_ptr = search_opt_lo(rio_detail_ptr->chassis_num);
 		if (!opt_rio_lo_ptr) {
 			opt_rio_lo_ptr = kzalloc(sizeof(struct opt_rio_lo), GFP_KERNEL);
 			if (!opt_rio_lo_ptr)
@@ -522,10 +522,10 @@ static int combine_wpg_for_expansion (void)
 			opt_rio_lo_ptr->middle_num = rio_detail_ptr->first_slot_num;
 			opt_rio_lo_ptr->pack_count = 1;
 
-			list_add (&opt_rio_lo_ptr->opt_rio_lo_list, &opt_lo_head);
+			list_add(&opt_rio_lo_ptr->opt_rio_lo_list, &opt_lo_head);
 		} else {
-			opt_rio_lo_ptr->first_slot_num = min (opt_rio_lo_ptr->first_slot_num, rio_detail_ptr->first_slot_num);
-			opt_rio_lo_ptr->middle_num = max (opt_rio_lo_ptr->middle_num, rio_detail_ptr->first_slot_num);
+			opt_rio_lo_ptr->first_slot_num = min(opt_rio_lo_ptr->first_slot_num, rio_detail_ptr->first_slot_num);
+			opt_rio_lo_ptr->middle_num = max(opt_rio_lo_ptr->middle_num, rio_detail_ptr->first_slot_num);
 			opt_rio_lo_ptr->pack_count = 2;
 		}
 	}
@@ -538,7 +538,7 @@ static int combine_wpg_for_expansion (void)
  * Arguments: slot_num, 1st slot number of the chassis we think we are on,
  * var (0 = chassis, 1 = expansion box)
  */
-static int first_slot_num (u8 slot_num, u8 first_slot, u8 var)
+static int first_slot_num(u8 slot_num, u8 first_slot, u8 var)
 {
 	struct opt_rio *opt_vg_ptr = NULL;
 	struct opt_rio_lo *opt_lo_ptr = NULL;
@@ -562,25 +562,25 @@ static int first_slot_num (u8 slot_num, u8 first_slot, u8 var)
 	return rc;
 }
 
-static struct opt_rio_lo *find_rxe_num (u8 slot_num)
+static struct opt_rio_lo *find_rxe_num(u8 slot_num)
 {
 	struct opt_rio_lo *opt_lo_ptr;
 
 	list_for_each_entry(opt_lo_ptr, &opt_lo_head, opt_rio_lo_list) {
 		//check to see if this slot_num belongs to expansion box
-		if ((slot_num >= opt_lo_ptr->first_slot_num) && (!first_slot_num (slot_num, opt_lo_ptr->first_slot_num, 1)))
+		if ((slot_num >= opt_lo_ptr->first_slot_num) && (!first_slot_num(slot_num, opt_lo_ptr->first_slot_num, 1)))
 			return opt_lo_ptr;
 	}
 	return NULL;
 }
 
-static struct opt_rio *find_chassis_num (u8 slot_num)
+static struct opt_rio *find_chassis_num(u8 slot_num)
 {
 	struct opt_rio *opt_vg_ptr;
 
 	list_for_each_entry(opt_vg_ptr, &opt_vg_head, opt_rio_list) {
 		//check to see if this slot_num belongs to chassis
-		if ((slot_num >= opt_vg_ptr->first_slot_num) && (!first_slot_num (slot_num, opt_vg_ptr->first_slot_num, 0)))
+		if ((slot_num >= opt_vg_ptr->first_slot_num) && (!first_slot_num(slot_num, opt_vg_ptr->first_slot_num, 0)))
 			return opt_vg_ptr;
 	}
 	return NULL;
@@ -589,7 +589,7 @@ static struct opt_rio *find_chassis_num (u8 slot_num)
 /* This routine will find out how many slots are in the chassis, so that
  * the slot numbers for rxe100 would start from 1, and not from 7, or 6 etc
  */
-static u8 calculate_first_slot (u8 slot_num)
+static u8 calculate_first_slot(u8 slot_num)
 {
 	u8 first_slot = 1;
 	struct slot *slot_cur;
@@ -606,7 +606,7 @@ static u8 calculate_first_slot (u8 slot_num)
 
 #define SLOT_NAME_SIZE 30
 
-static char *create_file_name (struct slot *slot_cur)
+static char *create_file_name(struct slot *slot_cur)
 {
 	struct opt_rio *opt_vg_ptr = NULL;
 	struct opt_rio_lo *opt_lo_ptr = NULL;
@@ -618,18 +618,18 @@ static char *create_file_name (struct slot *slot_cur)
 	u8 flag = 0;
 
 	if (!slot_cur) {
-		err ("Structure passed is empty\n");
+		err("Structure passed is empty\n");
 		return NULL;
 	}
 
 	slot_num = slot_cur->number;
 
-	memset (str, 0, sizeof(str));
+	memset(str, 0, sizeof(str));
 
 	if (rio_table_ptr) {
 		if (rio_table_ptr->ver_num == 3) {
-			opt_vg_ptr = find_chassis_num (slot_num);
-			opt_lo_ptr = find_rxe_num (slot_num);
+			opt_vg_ptr = find_chassis_num(slot_num);
+			opt_lo_ptr = find_rxe_num(slot_num);
 		}
 	}
 	if (opt_vg_ptr) {
@@ -662,7 +662,7 @@ static char *create_file_name (struct slot *slot_cur)
 	}
 	if (!flag) {
 		if (slot_cur->ctrl->ctlr_type == 4) {
-			first_slot = calculate_first_slot (slot_num);
+			first_slot = calculate_first_slot(slot_num);
 			which = 1;
 		} else {
 			which = 0;
@@ -698,7 +698,7 @@ static int fillslotinfo(struct hotplug_slot *hotplug_slot)
 	hotplug_slot->info->latch_status = SLOT_LATCH(slot->status);
 
 	// pci board - present:1 not:0
-	if (SLOT_PRESENT (slot->status))
+	if (SLOT_PRESENT(slot->status))
 		hotplug_slot->info->adapter_status = 1;
 	else
 		hotplug_slot->info->adapter_status = 0;
@@ -729,7 +729,7 @@ static void release_slot(struct hotplug_slot *hotplug_slot)
 	/* we don't want to actually remove the resources, since free_resources will do just that */
 	ibmphp_unconfigure_card(&slot, -1);
 
-	kfree (slot);
+	kfree(slot);
 }
 
 static struct pci_driver ibmphp_driver;
@@ -739,7 +739,7 @@ static struct pci_driver ibmphp_driver;
  * each hpc from physical address to a list of hot plug controllers based on
  * hpc descriptors.
  */
-static int __init ebda_rsrc_controller (void)
+static int __init ebda_rsrc_controller(void)
 {
 	u16 addr, addr_slot, addr_bus;
 	u8 ctlr_id, temp, bus_index;
@@ -757,25 +757,25 @@ static int __init ebda_rsrc_controller (void)
 	addr = hpc_list_ptr->phys_addr;
 	for (ctlr = 0; ctlr < hpc_list_ptr->num_ctlrs; ctlr++) {
 		bus_index = 1;
-		ctlr_id = readb (io_mem + addr);
+		ctlr_id = readb(io_mem + addr);
 		addr += 1;
-		slot_num = readb (io_mem + addr);
+		slot_num = readb(io_mem + addr);
 
 		addr += 1;
 		addr_slot = addr;	/* offset of slot structure */
 		addr += (slot_num * 4);
 
-		bus_num = readb (io_mem + addr);
+		bus_num = readb(io_mem + addr);
 
 		addr += 1;
 		addr_bus = addr;	/* offset of bus */
 		addr += (bus_num * 9);	/* offset of ctlr_type */
-		temp = readb (io_mem + addr);
+		temp = readb(io_mem + addr);
 
 		addr += 1;
 		/* init hpc structure */
-		hpc_ptr = alloc_ebda_hpc (slot_num, bus_num);
-		if (!hpc_ptr ) {
+		hpc_ptr = alloc_ebda_hpc(slot_num, bus_num);
+		if (!hpc_ptr) {
 			rc = -ENOMEM;
 			goto error_no_hpc;
 		}
@@ -783,23 +783,23 @@ static int __init ebda_rsrc_controller (void)
 		hpc_ptr->ctlr_relative_id = ctlr;
 		hpc_ptr->slot_count = slot_num;
 		hpc_ptr->bus_count = bus_num;
-		debug ("now enter ctlr data structure ---\n");
-		debug ("ctlr id: %x\n", ctlr_id);
-		debug ("ctlr_relative_id: %x\n", hpc_ptr->ctlr_relative_id);
-		debug ("count of slots controlled by this ctlr: %x\n", slot_num);
-		debug ("count of buses controlled by this ctlr: %x\n", bus_num);
+		debug("now enter ctlr data structure ---\n");
+		debug("ctlr id: %x\n", ctlr_id);
+		debug("ctlr_relative_id: %x\n", hpc_ptr->ctlr_relative_id);
+		debug("count of slots controlled by this ctlr: %x\n", slot_num);
+		debug("count of buses controlled by this ctlr: %x\n", bus_num);
 
 		/* init slot structure, fetch slot, bus, cap... */
 		slot_ptr = hpc_ptr->slots;
 		for (slot = 0; slot < slot_num; slot++) {
-			slot_ptr->slot_num = readb (io_mem + addr_slot);
-			slot_ptr->slot_bus_num = readb (io_mem + addr_slot + slot_num);
-			slot_ptr->ctl_index = readb (io_mem + addr_slot + 2*slot_num);
-			slot_ptr->slot_cap = readb (io_mem + addr_slot + 3*slot_num);
+			slot_ptr->slot_num = readb(io_mem + addr_slot);
+			slot_ptr->slot_bus_num = readb(io_mem + addr_slot + slot_num);
+			slot_ptr->ctl_index = readb(io_mem + addr_slot + 2*slot_num);
+			slot_ptr->slot_cap = readb(io_mem + addr_slot + 3*slot_num);
 
 			// create bus_info lined list --- if only one slot per bus: slot_min = slot_max
 
-			bus_info_ptr2 = ibmphp_find_same_bus_num (slot_ptr->slot_bus_num);
+			bus_info_ptr2 = ibmphp_find_same_bus_num(slot_ptr->slot_bus_num);
 			if (!bus_info_ptr2) {
 				bus_info_ptr1 = kzalloc(sizeof(struct bus_info), GFP_KERNEL);
 				if (!bus_info_ptr1) {
@@ -816,11 +816,11 @@ static int __init ebda_rsrc_controller (void)
 
 				bus_info_ptr1->controller_id = hpc_ptr->ctlr_id;
 
-				list_add_tail (&bus_info_ptr1->bus_info_list, &bus_info_head);
+				list_add_tail(&bus_info_ptr1->bus_info_list, &bus_info_head);
 
 			} else {
-				bus_info_ptr2->slot_min = min (bus_info_ptr2->slot_min, slot_ptr->slot_num);
-				bus_info_ptr2->slot_max = max (bus_info_ptr2->slot_max, slot_ptr->slot_num);
+				bus_info_ptr2->slot_min = min(bus_info_ptr2->slot_min, slot_ptr->slot_num);
+				bus_info_ptr2->slot_max = max(bus_info_ptr2->slot_max, slot_ptr->slot_num);
 				bus_info_ptr2->slot_count += 1;
 
 			}
@@ -834,17 +834,17 @@ static int __init ebda_rsrc_controller (void)
 		/* init bus structure */
 		bus_ptr = hpc_ptr->buses;
 		for (bus = 0; bus < bus_num; bus++) {
-			bus_ptr->bus_num = readb (io_mem + addr_bus + bus);
-			bus_ptr->slots_at_33_conv = readb (io_mem + addr_bus + bus_num + 8 * bus);
-			bus_ptr->slots_at_66_conv = readb (io_mem + addr_bus + bus_num + 8 * bus + 1);
+			bus_ptr->bus_num = readb(io_mem + addr_bus + bus);
+			bus_ptr->slots_at_33_conv = readb(io_mem + addr_bus + bus_num + 8 * bus);
+			bus_ptr->slots_at_66_conv = readb(io_mem + addr_bus + bus_num + 8 * bus + 1);
 
-			bus_ptr->slots_at_66_pcix = readb (io_mem + addr_bus + bus_num + 8 * bus + 2);
+			bus_ptr->slots_at_66_pcix = readb(io_mem + addr_bus + bus_num + 8 * bus + 2);
 
-			bus_ptr->slots_at_100_pcix = readb (io_mem + addr_bus + bus_num + 8 * bus + 3);
+			bus_ptr->slots_at_100_pcix = readb(io_mem + addr_bus + bus_num + 8 * bus + 3);
 
-			bus_ptr->slots_at_133_pcix = readb (io_mem + addr_bus + bus_num + 8 * bus + 4);
+			bus_ptr->slots_at_133_pcix = readb(io_mem + addr_bus + bus_num + 8 * bus + 4);
 
-			bus_info_ptr2 = ibmphp_find_same_bus_num (bus_ptr->bus_num);
+			bus_info_ptr2 = ibmphp_find_same_bus_num(bus_ptr->bus_num);
 			if (bus_info_ptr2) {
 				bus_info_ptr2->slots_at_33_conv = bus_ptr->slots_at_33_conv;
 				bus_info_ptr2->slots_at_66_conv = bus_ptr->slots_at_66_conv;
@@ -859,33 +859,33 @@ static int __init ebda_rsrc_controller (void)
 
 		switch (hpc_ptr->ctlr_type) {
 			case 1:
-				hpc_ptr->u.pci_ctlr.bus = readb (io_mem + addr);
-				hpc_ptr->u.pci_ctlr.dev_fun = readb (io_mem + addr + 1);
-				hpc_ptr->irq = readb (io_mem + addr + 2);
+				hpc_ptr->u.pci_ctlr.bus = readb(io_mem + addr);
+				hpc_ptr->u.pci_ctlr.dev_fun = readb(io_mem + addr + 1);
+				hpc_ptr->irq = readb(io_mem + addr + 2);
 				addr += 3;
-				debug ("ctrl bus = %x, ctlr devfun = %x, irq = %x\n",
+				debug("ctrl bus = %x, ctlr devfun = %x, irq = %x\n",
 					hpc_ptr->u.pci_ctlr.bus,
 					hpc_ptr->u.pci_ctlr.dev_fun, hpc_ptr->irq);
 				break;
 
 			case 0:
-				hpc_ptr->u.isa_ctlr.io_start = readw (io_mem + addr);
-				hpc_ptr->u.isa_ctlr.io_end = readw (io_mem + addr + 2);
-				if (!request_region (hpc_ptr->u.isa_ctlr.io_start,
+				hpc_ptr->u.isa_ctlr.io_start = readw(io_mem + addr);
+				hpc_ptr->u.isa_ctlr.io_end = readw(io_mem + addr + 2);
+				if (!request_region(hpc_ptr->u.isa_ctlr.io_start,
 						     (hpc_ptr->u.isa_ctlr.io_end - hpc_ptr->u.isa_ctlr.io_start + 1),
 						     "ibmphp")) {
 					rc = -ENODEV;
 					goto error_no_hp_slot;
 				}
-				hpc_ptr->irq = readb (io_mem + addr + 4);
+				hpc_ptr->irq = readb(io_mem + addr + 4);
 				addr += 5;
 				break;
 
 			case 2:
 			case 4:
-				hpc_ptr->u.wpeg_ctlr.wpegbbar = readl (io_mem + addr);
-				hpc_ptr->u.wpeg_ctlr.i2c_addr = readb (io_mem + addr + 4);
-				hpc_ptr->irq = readb (io_mem + addr + 5);
+				hpc_ptr->u.wpeg_ctlr.wpegbbar = readl(io_mem + addr);
+				hpc_ptr->u.wpeg_ctlr.i2c_addr = readb(io_mem + addr + 4);
+				hpc_ptr->irq = readb(io_mem + addr + 5);
 				addr += 6;
 				break;
 			default:
@@ -894,8 +894,8 @@ static int __init ebda_rsrc_controller (void)
 		}
 
 		//reorganize chassis' linked list
-		combine_wpg_for_chassis ();
-		combine_wpg_for_expansion ();
+		combine_wpg_for_chassis();
+		combine_wpg_for_expansion();
 		hpc_ptr->revision = 0xff;
 		hpc_ptr->options = 0xff;
 		hpc_ptr->starting_slot_num = hpc_ptr->slots[0].slot_num;
@@ -940,7 +940,7 @@ static int __init ebda_rsrc_controller (void)
 
 			tmp_slot->bus = hpc_ptr->slots[index].slot_bus_num;
 
-			bus_info_ptr1 = ibmphp_find_same_bus_num (hpc_ptr->slots[index].slot_bus_num);
+			bus_info_ptr1 = ibmphp_find_same_bus_num(hpc_ptr->slots[index].slot_bus_num);
 			if (!bus_info_ptr1) {
 				kfree(tmp_slot);
 				rc = -ENODEV;
@@ -961,18 +961,18 @@ static int __init ebda_rsrc_controller (void)
 			if (rc)
 				goto error;
 
-			rc = ibmphp_init_devno ((struct slot **) &hp_slot_ptr->private);
+			rc = ibmphp_init_devno((struct slot **) &hp_slot_ptr->private);
 			if (rc)
 				goto error;
 			hp_slot_ptr->ops = &ibmphp_hotplug_slot_ops;
 
 			// end of registering ibm slot with hotplug core
 
-			list_add (& ((struct slot *)(hp_slot_ptr->private))->ibm_slot_list, &ibmphp_slot_head);
+			list_add(&((struct slot *)(hp_slot_ptr->private))->ibm_slot_list, &ibmphp_slot_head);
 		}
 
-		print_bus_info ();
-		list_add (&hpc_ptr->ebda_hpc_list, &ebda_hpc_head );
+		print_bus_info();
+		list_add(&hpc_ptr->ebda_hpc_list, &ebda_hpc_head);
 
 	}			/* each hpc  */
 
@@ -982,20 +982,20 @@ static int __init ebda_rsrc_controller (void)
 			pci_find_bus(0, tmp_slot->bus), tmp_slot->device, name);
 	}
 
-	print_ebda_hpc ();
-	print_ibm_slot ();
+	print_ebda_hpc();
+	print_ibm_slot();
 	return 0;
 
 error:
-	kfree (hp_slot_ptr->private);
+	kfree(hp_slot_ptr->private);
 error_no_slot:
-	kfree (hp_slot_ptr->info);
+	kfree(hp_slot_ptr->info);
 error_no_hp_info:
-	kfree (hp_slot_ptr);
+	kfree(hp_slot_ptr);
 error_no_hp_slot:
-	free_ebda_hpc (hpc_ptr);
+	free_ebda_hpc(hpc_ptr);
 error_no_hpc:
-	iounmap (io_mem);
+	iounmap(io_mem);
 	return rc;
 }
 
@@ -1003,7 +1003,7 @@ error_no_hpc:
  * map info (bus, devfun, start addr, end addr..) of i/o, memory,
  * pfm from the physical addr to a list of resource.
  */
-static int __init ebda_rsrc_rsrc (void)
+static int __init ebda_rsrc_rsrc(void)
 {
 	u16 addr;
 	short rsrc;
@@ -1011,69 +1011,69 @@ static int __init ebda_rsrc_rsrc (void)
 	struct ebda_pci_rsrc *rsrc_ptr;
 
 	addr = rsrc_list_ptr->phys_addr;
-	debug ("now entering rsrc land\n");
-	debug ("offset of rsrc: %x\n", rsrc_list_ptr->phys_addr);
+	debug("now entering rsrc land\n");
+	debug("offset of rsrc: %x\n", rsrc_list_ptr->phys_addr);
 
 	for (rsrc = 0; rsrc < rsrc_list_ptr->num_entries; rsrc++) {
-		type = readb (io_mem + addr);
+		type = readb(io_mem + addr);
 
 		addr += 1;
 		rsrc_type = type & EBDA_RSRC_TYPE_MASK;
 
 		if (rsrc_type == EBDA_IO_RSRC_TYPE) {
-			rsrc_ptr = alloc_ebda_pci_rsrc ();
+			rsrc_ptr = alloc_ebda_pci_rsrc();
 			if (!rsrc_ptr) {
-				iounmap (io_mem);
+				iounmap(io_mem);
 				return -ENOMEM;
 			}
 			rsrc_ptr->rsrc_type = type;
 
-			rsrc_ptr->bus_num = readb (io_mem + addr);
-			rsrc_ptr->dev_fun = readb (io_mem + addr + 1);
-			rsrc_ptr->start_addr = readw (io_mem + addr + 2);
-			rsrc_ptr->end_addr = readw (io_mem + addr + 4);
+			rsrc_ptr->bus_num = readb(io_mem + addr);
+			rsrc_ptr->dev_fun = readb(io_mem + addr + 1);
+			rsrc_ptr->start_addr = readw(io_mem + addr + 2);
+			rsrc_ptr->end_addr = readw(io_mem + addr + 4);
 			addr += 6;
 
-			debug ("rsrc from io type ----\n");
-			debug ("rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
+			debug("rsrc from io type ----\n");
+			debug("rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
 				rsrc_ptr->rsrc_type, rsrc_ptr->bus_num, rsrc_ptr->dev_fun, rsrc_ptr->start_addr, rsrc_ptr->end_addr);
 
-			list_add (&rsrc_ptr->ebda_pci_rsrc_list, &ibmphp_ebda_pci_rsrc_head);
+			list_add(&rsrc_ptr->ebda_pci_rsrc_list, &ibmphp_ebda_pci_rsrc_head);
 		}
 
 		if (rsrc_type == EBDA_MEM_RSRC_TYPE || rsrc_type == EBDA_PFM_RSRC_TYPE) {
-			rsrc_ptr = alloc_ebda_pci_rsrc ();
-			if (!rsrc_ptr ) {
-				iounmap (io_mem);
+			rsrc_ptr = alloc_ebda_pci_rsrc();
+			if (!rsrc_ptr) {
+				iounmap(io_mem);
 				return -ENOMEM;
 			}
 			rsrc_ptr->rsrc_type = type;
 
-			rsrc_ptr->bus_num = readb (io_mem + addr);
-			rsrc_ptr->dev_fun = readb (io_mem + addr + 1);
-			rsrc_ptr->start_addr = readl (io_mem + addr + 2);
-			rsrc_ptr->end_addr = readl (io_mem + addr + 6);
+			rsrc_ptr->bus_num = readb(io_mem + addr);
+			rsrc_ptr->dev_fun = readb(io_mem + addr + 1);
+			rsrc_ptr->start_addr = readl(io_mem + addr + 2);
+			rsrc_ptr->end_addr = readl(io_mem + addr + 6);
 			addr += 10;
 
-			debug ("rsrc from mem or pfm ---\n");
-			debug ("rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
+			debug("rsrc from mem or pfm ---\n");
+			debug("rsrc type: %x bus#: %x dev_func: %x start addr: %x end addr: %x\n",
 				rsrc_ptr->rsrc_type, rsrc_ptr->bus_num, rsrc_ptr->dev_fun, rsrc_ptr->start_addr, rsrc_ptr->end_addr);
 
-			list_add (&rsrc_ptr->ebda_pci_rsrc_list, &ibmphp_ebda_pci_rsrc_head);
+			list_add(&rsrc_ptr->ebda_pci_rsrc_list, &ibmphp_ebda_pci_rsrc_head);
 		}
 	}
-	kfree (rsrc_list_ptr);
+	kfree(rsrc_list_ptr);
 	rsrc_list_ptr = NULL;
-	print_ebda_pci_rsrc ();
+	print_ebda_pci_rsrc();
 	return 0;
 }
 
-u16 ibmphp_get_total_controllers (void)
+u16 ibmphp_get_total_controllers(void)
 {
 	return hpc_list_ptr->num_ctlrs;
 }
 
-struct slot *ibmphp_get_slot_from_physical_num (u8 physical_num)
+struct slot *ibmphp_get_slot_from_physical_num(u8 physical_num)
 {
 	struct slot *slot;
 
@@ -1090,7 +1090,7 @@ struct slot *ibmphp_get_slot_from_physical_num (u8 physical_num)
  *	- the total number of the slots based on each bus
  *	  (if only one slot per bus slot_min = slot_max )
  */
-struct bus_info *ibmphp_find_same_bus_num (u32 num)
+struct bus_info *ibmphp_find_same_bus_num(u32 num)
 {
 	struct bus_info *ptr;
 
@@ -1104,7 +1104,7 @@ struct bus_info *ibmphp_find_same_bus_num (u32 num)
 /*  Finding relative bus number, in order to map corresponding
  *  bus register
  */
-int ibmphp_get_bus_index (u8 num)
+int ibmphp_get_bus_index(u8 num)
 {
 	struct bus_info *ptr;
 
@@ -1115,45 +1115,39 @@ int ibmphp_get_bus_index (u8 num)
 	return -ENODEV;
 }
 
-void ibmphp_free_bus_info_queue (void)
+void ibmphp_free_bus_info_queue(void)
 {
-	struct bus_info *bus_info;
-	struct list_head *list;
-	struct list_head *next;
+	struct bus_info *bus_info, *next;
 
-	list_for_each_safe (list, next, &bus_info_head ) {
-		bus_info = list_entry (list, struct bus_info, bus_info_list);
+	list_for_each_entry_safe(bus_info, next, &bus_info_head,
+				 bus_info_list) {
 		kfree (bus_info);
 	}
 }
 
-void ibmphp_free_ebda_hpc_queue (void)
+void ibmphp_free_ebda_hpc_queue(void)
 {
-	struct controller *controller = NULL;
-	struct list_head *list;
-	struct list_head *next;
+	struct controller *controller = NULL, *next;
 	int pci_flag = 0;
 
-	list_for_each_safe (list, next, &ebda_hpc_head) {
-		controller = list_entry (list, struct controller, ebda_hpc_list);
+	list_for_each_entry_safe(controller, next, &ebda_hpc_head,
+				 ebda_hpc_list) {
 		if (controller->ctlr_type == 0)
-			release_region (controller->u.isa_ctlr.io_start, (controller->u.isa_ctlr.io_end - controller->u.isa_ctlr.io_start + 1));
+			release_region(controller->u.isa_ctlr.io_start, (controller->u.isa_ctlr.io_end - controller->u.isa_ctlr.io_start + 1));
 		else if ((controller->ctlr_type == 1) && (!pci_flag)) {
 			++pci_flag;
-			pci_unregister_driver (&ibmphp_driver);
+			pci_unregister_driver(&ibmphp_driver);
 		}
-		free_ebda_hpc (controller);
+		free_ebda_hpc(controller);
 	}
 }
 
-void ibmphp_free_ebda_pci_rsrc_queue (void)
+void ibmphp_free_ebda_pci_rsrc_queue(void)
 {
-	struct ebda_pci_rsrc *resource;
-	struct list_head *list;
-	struct list_head *next;
+	struct ebda_pci_rsrc *resource, *next;
 
-	list_for_each_safe (list, next, &ibmphp_ebda_pci_rsrc_head) {
-		resource = list_entry (list, struct ebda_pci_rsrc, ebda_pci_rsrc_list);
+	list_for_each_entry_safe(resource, next, &ibmphp_ebda_pci_rsrc_head,
+				 ebda_pci_rsrc_list) {
 		kfree (resource);
 		resource = NULL;
 	}
@@ -1171,14 +1165,14 @@ static struct pci_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(pci, id_table);
 
-static int ibmphp_probe (struct pci_dev *, const struct pci_device_id *);
+static int ibmphp_probe(struct pci_dev *, const struct pci_device_id *);
 static struct pci_driver ibmphp_driver = {
 	.name		= "ibmphp",
 	.id_table	= id_table,
 	.probe		= ibmphp_probe,
 };
 
-int ibmphp_register_pci (void)
+int ibmphp_register_pci(void)
 {
 	struct controller *ctrl;
 	int rc = 0;
@@ -1191,18 +1185,18 @@ int ibmphp_register_pci (void)
 	}
 	return rc;
 }
-static int ibmphp_probe (struct pci_dev *dev, const struct pci_device_id *ids)
+static int ibmphp_probe(struct pci_dev *dev, const struct pci_device_id *ids)
 {
 	struct controller *ctrl;
 
-	debug ("inside ibmphp_probe\n");
+	debug("inside ibmphp_probe\n");
 
 	list_for_each_entry(ctrl, &ebda_hpc_head, ebda_hpc_list) {
 		if (ctrl->ctlr_type == 1) {
 			if ((dev->devfn == ctrl->u.pci_ctlr.dev_fun) && (dev->bus->number == ctrl->u.pci_ctlr.bus)) {
 				ctrl->ctrl_dev = dev;
-				debug ("found device!!!\n");
-				debug ("dev->device = %x, dev->subsystem_device = %x\n", dev->device, dev->subsystem_device);
+				debug("found device!!!\n");
+				debug("dev->device = %x, dev->subsystem_device = %x\n", dev->device, dev->subsystem_device);
 				return 0;
 			}
 		}
diff --git a/drivers/pci/hotplug/ibmphp_hpc.c b/drivers/pci/hotplug/ibmphp_hpc.c
index 220876715a08..a6b458e4ab46 100644
--- a/drivers/pci/hotplug/ibmphp_hpc.c
+++ b/drivers/pci/hotplug/ibmphp_hpc.c
@@ -40,7 +40,7 @@
 #include "ibmphp.h"
 
 static int to_debug = 0;
-#define debug_polling(fmt, arg...)	do { if (to_debug) debug (fmt, arg); } while (0)
+#define debug_polling(fmt, arg...)	do { if (to_debug) debug(fmt, arg); } while (0)
 
 //----------------------------------------------------------------------------
 // timeout values
@@ -110,16 +110,16 @@ static struct task_struct *ibmphp_poll_thread;
 //----------------------------------------------------------------------------
 // local function prototypes
 //----------------------------------------------------------------------------
-static u8 i2c_ctrl_read (struct controller *, void __iomem *, u8);
-static u8 i2c_ctrl_write (struct controller *, void __iomem *, u8, u8);
-static u8 hpc_writecmdtoindex (u8, u8);
-static u8 hpc_readcmdtoindex (u8, u8);
-static void get_hpc_access (void);
-static void free_hpc_access (void);
+static u8 i2c_ctrl_read(struct controller *, void __iomem *, u8);
+static u8 i2c_ctrl_write(struct controller *, void __iomem *, u8, u8);
+static u8 hpc_writecmdtoindex(u8, u8);
+static u8 hpc_readcmdtoindex(u8, u8);
+static void get_hpc_access(void);
+static void free_hpc_access(void);
 static int poll_hpc(void *data);
-static int process_changeinstatus (struct slot *, struct slot *);
-static int process_changeinlatch (u8, u8, struct controller *);
-static int hpc_wait_ctlr_notworking (int, struct controller *, void __iomem *, u8 *);
+static int process_changeinstatus(struct slot *, struct slot *);
+static int process_changeinlatch(u8, u8, struct controller *);
+static int hpc_wait_ctlr_notworking(int, struct controller *, void __iomem *, u8 *);
 //----------------------------------------------------------------------------
 
 
@@ -128,16 +128,16 @@ static int hpc_wait_ctlr_notworking (int, struct controller *, void __iomem *, u
 *
 * Action:  initialize semaphores and variables
 *---------------------------------------------------------------------*/
-void __init ibmphp_hpc_initvars (void)
+void __init ibmphp_hpc_initvars(void)
 {
-	debug ("%s - Entry\n", __func__);
+	debug("%s - Entry\n", __func__);
 
 	mutex_init(&sem_hpcaccess);
 	sema_init(&semOperations, 1);
 	sema_init(&sem_exit, 0);
 	to_debug = 0;
 
-	debug ("%s - Exit\n", __func__);
+	debug("%s - Exit\n", __func__);
 }
 
 /*----------------------------------------------------------------------
@@ -146,7 +146,7 @@ void __init ibmphp_hpc_initvars (void)
 * Action:  read from HPC over I2C
 *
 *---------------------------------------------------------------------*/
-static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8 index)
+static u8 i2c_ctrl_read(struct controller *ctlr_ptr, void __iomem *WPGBbar, u8 index)
 {
 	u8 status;
 	int i;
@@ -155,7 +155,7 @@ static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	unsigned long ultemp;
 	unsigned long data;	// actual data HILO format
 
-	debug_polling ("%s - Entry WPGBbar[%p] index[%x] \n", __func__, WPGBbar, index);
+	debug_polling("%s - Entry WPGBbar[%p] index[%x] \n", __func__, WPGBbar, index);
 
 	//--------------------------------------------------------------------
 	// READ - step 1
@@ -178,28 +178,28 @@ static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 		ultemp = ultemp << 8;
 		data |= ultemp;
 	} else {
-		err ("this controller type is not supported \n");
+		err("this controller type is not supported \n");
 		return HPC_ERROR;
 	}
 
-	wpg_data = swab32 (data);	// swap data before writing
+	wpg_data = swab32(data);	// swap data before writing
 	wpg_addr = WPGBbar + WPG_I2CMOSUP_OFFSET;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// READ - step 2 : clear the message buffer
 	data = 0x00000000;
-	wpg_data = swab32 (data);
+	wpg_data = swab32(data);
 	wpg_addr = WPGBbar + WPG_I2CMBUFL_OFFSET;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// READ - step 3 : issue start operation, I2C master control bit 30:ON
 	//                 2020 : [20] OR operation at [20] offset 0x20
 	data = WPG_I2CMCNTL_STARTOP_MASK;
-	wpg_data = swab32 (data);
+	wpg_data = swab32(data);
 	wpg_addr = WPGBbar + WPG_I2CMCNTL_OFFSET + WPG_I2C_OR;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// READ - step 4 : wait until start operation bit clears
@@ -207,14 +207,14 @@ static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	while (i) {
 		msleep(10);
 		wpg_addr = WPGBbar + WPG_I2CMCNTL_OFFSET;
-		wpg_data = readl (wpg_addr);
-		data = swab32 (wpg_data);
+		wpg_data = readl(wpg_addr);
+		data = swab32(wpg_data);
 		if (!(data & WPG_I2CMCNTL_STARTOP_MASK))
 			break;
 		i--;
 	}
 	if (i == 0) {
-		debug ("%s - Error : WPG timeout\n", __func__);
+		debug("%s - Error : WPG timeout\n", __func__);
 		return HPC_ERROR;
 	}
 	//--------------------------------------------------------------------
@@ -223,26 +223,26 @@ static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	while (i) {
 		msleep(10);
 		wpg_addr = WPGBbar + WPG_I2CSTAT_OFFSET;
-		wpg_data = readl (wpg_addr);
-		data = swab32 (wpg_data);
-		if (HPC_I2CSTATUS_CHECK (data))
+		wpg_data = readl(wpg_addr);
+		data = swab32(wpg_data);
+		if (HPC_I2CSTATUS_CHECK(data))
 			break;
 		i--;
 	}
 	if (i == 0) {
-		debug ("ctrl_read - Exit Error:I2C timeout\n");
+		debug("ctrl_read - Exit Error:I2C timeout\n");
 		return HPC_ERROR;
 	}
 
 	//--------------------------------------------------------------------
 	// READ - step 6 : get DATA
 	wpg_addr = WPGBbar + WPG_I2CMBUFL_OFFSET;
-	wpg_data = readl (wpg_addr);
-	data = swab32 (wpg_data);
+	wpg_data = readl(wpg_addr);
+	data = swab32(wpg_data);
 
 	status = (u8) data;
 
-	debug_polling ("%s - Exit index[%x] status[%x]\n", __func__, index, status);
+	debug_polling("%s - Exit index[%x] status[%x]\n", __func__, index, status);
 
 	return (status);
 }
@@ -254,7 +254,7 @@ static u8 i2c_ctrl_read (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 *
 * Return   0 or error codes
 *---------------------------------------------------------------------*/
-static u8 i2c_ctrl_write (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8 index, u8 cmd)
+static u8 i2c_ctrl_write(struct controller *ctlr_ptr, void __iomem *WPGBbar, u8 index, u8 cmd)
 {
 	u8 rc;
 	void __iomem *wpg_addr;	// base addr + offset
@@ -263,7 +263,7 @@ static u8 i2c_ctrl_write (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	unsigned long data;	// actual data HILO format
 	int i;
 
-	debug_polling ("%s - Entry WPGBbar[%p] index[%x] cmd[%x]\n", __func__, WPGBbar, index, cmd);
+	debug_polling("%s - Entry WPGBbar[%p] index[%x] cmd[%x]\n", __func__, WPGBbar, index, cmd);
 
 	rc = 0;
 	//--------------------------------------------------------------------
@@ -289,28 +289,28 @@ static u8 i2c_ctrl_write (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 		ultemp = ultemp << 8;
 		data |= ultemp;
 	} else {
-		err ("this controller type is not supported \n");
+		err("this controller type is not supported \n");
 		return HPC_ERROR;
 	}
 
-	wpg_data = swab32 (data);	// swap data before writing
+	wpg_data = swab32(data);	// swap data before writing
 	wpg_addr = WPGBbar + WPG_I2CMOSUP_OFFSET;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// WRITE - step 2 : clear the message buffer
 	data = 0x00000000 | (unsigned long)cmd;
-	wpg_data = swab32 (data);
+	wpg_data = swab32(data);
 	wpg_addr = WPGBbar + WPG_I2CMBUFL_OFFSET;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// WRITE - step 3 : issue start operation,I2C master control bit 30:ON
 	//                 2020 : [20] OR operation at [20] offset 0x20
 	data = WPG_I2CMCNTL_STARTOP_MASK;
-	wpg_data = swab32 (data);
+	wpg_data = swab32(data);
 	wpg_addr = WPGBbar + WPG_I2CMCNTL_OFFSET + WPG_I2C_OR;
-	writel (wpg_data, wpg_addr);
+	writel(wpg_data, wpg_addr);
 
 	//--------------------------------------------------------------------
 	// WRITE - step 4 : wait until start operation bit clears
@@ -318,14 +318,14 @@ static u8 i2c_ctrl_write (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	while (i) {
 		msleep(10);
 		wpg_addr = WPGBbar + WPG_I2CMCNTL_OFFSET;
-		wpg_data = readl (wpg_addr);
-		data = swab32 (wpg_data);
+		wpg_data = readl(wpg_addr);
+		data = swab32(wpg_data);
 		if (!(data & WPG_I2CMCNTL_STARTOP_MASK))
 			break;
 		i--;
 	}
 	if (i == 0) {
-		debug ("%s - Exit Error:WPG timeout\n", __func__);
+		debug("%s - Exit Error:WPG timeout\n", __func__);
 		rc = HPC_ERROR;
 	}
 
@@ -335,25 +335,25 @@ static u8 i2c_ctrl_write (struct controller *ctlr_ptr, void __iomem *WPGBbar, u8
 	while (i) {
 		msleep(10);
 		wpg_addr = WPGBbar + WPG_I2CSTAT_OFFSET;
-		wpg_data = readl (wpg_addr);
-		data = swab32 (wpg_data);
-		if (HPC_I2CSTATUS_CHECK (data))
+		wpg_data = readl(wpg_addr);
+		data = swab32(wpg_data);
+		if (HPC_I2CSTATUS_CHECK(data))
 			break;
 		i--;
 	}
 	if (i == 0) {
-		debug ("ctrl_read - Error : I2C timeout\n");
+		debug("ctrl_read - Error : I2C timeout\n");
 		rc = HPC_ERROR;
 	}
 
-	debug_polling ("%s Exit rc[%x]\n", __func__, rc);
+	debug_polling("%s Exit rc[%x]\n", __func__, rc);
 	return (rc);
 }
 
 //------------------------------------------------------------
 //  Read from ISA type HPC
 //------------------------------------------------------------
-static u8 isa_ctrl_read (struct controller *ctlr_ptr, u8 offset)
+static u8 isa_ctrl_read(struct controller *ctlr_ptr, u8 offset)
 {
 	u16 start_address;
 	u16 end_address;
@@ -361,56 +361,56 @@ static u8 isa_ctrl_read (struct controller *ctlr_ptr, u8 offset)
 
 	start_address = ctlr_ptr->u.isa_ctlr.io_start;
 	end_address = ctlr_ptr->u.isa_ctlr.io_end;
-	data = inb (start_address + offset);
+	data = inb(start_address + offset);
 	return data;
 }
 
 //--------------------------------------------------------------
 // Write to ISA type HPC
 //--------------------------------------------------------------
-static void isa_ctrl_write (struct controller *ctlr_ptr, u8 offset, u8 data)
+static void isa_ctrl_write(struct controller *ctlr_ptr, u8 offset, u8 data)
 {
 	u16 start_address;
 	u16 port_address;
 
 	start_address = ctlr_ptr->u.isa_ctlr.io_start;
 	port_address = start_address + (u16) offset;
-	outb (data, port_address);
+	outb(data, port_address);
 }
 
-static u8 pci_ctrl_read (struct controller *ctrl, u8 offset)
+static u8 pci_ctrl_read(struct controller *ctrl, u8 offset)
 {
 	u8 data = 0x00;
-	debug ("inside pci_ctrl_read\n");
+	debug("inside pci_ctrl_read\n");
 	if (ctrl->ctrl_dev)
-		pci_read_config_byte (ctrl->ctrl_dev, HPC_PCI_OFFSET + offset, &data);
+		pci_read_config_byte(ctrl->ctrl_dev, HPC_PCI_OFFSET + offset, &data);
 	return data;
 }
 
-static u8 pci_ctrl_write (struct controller *ctrl, u8 offset, u8 data)
+static u8 pci_ctrl_write(struct controller *ctrl, u8 offset, u8 data)
 {
 	u8 rc = -ENODEV;
-	debug ("inside pci_ctrl_write\n");
+	debug("inside pci_ctrl_write\n");
 	if (ctrl->ctrl_dev) {
-		pci_write_config_byte (ctrl->ctrl_dev, HPC_PCI_OFFSET + offset, data);
+		pci_write_config_byte(ctrl->ctrl_dev, HPC_PCI_OFFSET + offset, data);
 		rc = 0;
 	}
 	return rc;
 }
 
-static u8 ctrl_read (struct controller *ctlr, void __iomem *base, u8 offset)
+static u8 ctrl_read(struct controller *ctlr, void __iomem *base, u8 offset)
 {
 	u8 rc;
 	switch (ctlr->ctlr_type) {
 	case 0:
-		rc = isa_ctrl_read (ctlr, offset);
+		rc = isa_ctrl_read(ctlr, offset);
 		break;
 	case 1:
-		rc = pci_ctrl_read (ctlr, offset);
+		rc = pci_ctrl_read(ctlr, offset);
 		break;
 	case 2:
 	case 4:
-		rc = i2c_ctrl_read (ctlr, base, offset);
+		rc = i2c_ctrl_read(ctlr, base, offset);
 		break;
 	default:
 		return -ENODEV;
@@ -418,7 +418,7 @@ static u8 ctrl_read (struct controller *ctlr, void __iomem *base, u8 offset)
 	return rc;
 }
 
-static u8 ctrl_write (struct controller *ctlr, void __iomem *base, u8 offset, u8 data)
+static u8 ctrl_write(struct controller *ctlr, void __iomem *base, u8 offset, u8 data)
 {
 	u8 rc = 0;
 	switch (ctlr->ctlr_type) {
@@ -426,7 +426,7 @@ static u8 ctrl_write (struct controller *ctlr, void __iomem *base, u8 offset, u8
 		isa_ctrl_write(ctlr, offset, data);
 		break;
 	case 1:
-		rc = pci_ctrl_write (ctlr, offset, data);
+		rc = pci_ctrl_write(ctlr, offset, data);
 		break;
 	case 2:
 	case 4:
@@ -444,7 +444,7 @@ static u8 ctrl_write (struct controller *ctlr, void __iomem *base, u8 offset, u8
 *
 * Return   index, HPC_ERROR
 *---------------------------------------------------------------------*/
-static u8 hpc_writecmdtoindex (u8 cmd, u8 index)
+static u8 hpc_writecmdtoindex(u8 cmd, u8 index)
 {
 	u8 rc;
 
@@ -476,7 +476,7 @@ static u8 hpc_writecmdtoindex (u8 cmd, u8 index)
 		break;
 
 	default:
-		err ("hpc_writecmdtoindex - Error invalid cmd[%x]\n", cmd);
+		err("hpc_writecmdtoindex - Error invalid cmd[%x]\n", cmd);
 		rc = HPC_ERROR;
 	}
 
@@ -490,7 +490,7 @@ static u8 hpc_writecmdtoindex (u8 cmd, u8 index)
 *
 * Return   index, HPC_ERROR
 *---------------------------------------------------------------------*/
-static u8 hpc_readcmdtoindex (u8 cmd, u8 index)
+static u8 hpc_readcmdtoindex(u8 cmd, u8 index)
 {
 	u8 rc;
 
@@ -533,78 +533,77 @@ static u8 hpc_readcmdtoindex (u8 cmd, u8 index)
 *
 * Return   0 or error codes
 *---------------------------------------------------------------------*/
-int ibmphp_hpc_readslot (struct slot *pslot, u8 cmd, u8 *pstatus)
+int ibmphp_hpc_readslot(struct slot *pslot, u8 cmd, u8 *pstatus)
 {
 	void __iomem *wpg_bbar = NULL;
 	struct controller *ctlr_ptr;
-	struct list_head *pslotlist;
 	u8 index, status;
 	int rc = 0;
 	int busindex;
 
-	debug_polling ("%s - Entry pslot[%p] cmd[%x] pstatus[%p]\n", __func__, pslot, cmd, pstatus);
+	debug_polling("%s - Entry pslot[%p] cmd[%x] pstatus[%p]\n", __func__, pslot, cmd, pstatus);
 
 	if ((pslot == NULL)
 	    || ((pstatus == NULL) && (cmd != READ_ALLSTAT) && (cmd != READ_BUSSTATUS))) {
 		rc = -EINVAL;
-		err ("%s - Error invalid pointer, rc[%d]\n", __func__, rc);
+		err("%s - Error invalid pointer, rc[%d]\n", __func__, rc);
 		return rc;
 	}
 
 	if (cmd == READ_BUSSTATUS) {
-		busindex = ibmphp_get_bus_index (pslot->bus);
+		busindex = ibmphp_get_bus_index(pslot->bus);
 		if (busindex < 0) {
 			rc = -EINVAL;
-			err ("%s - Exit Error:invalid bus, rc[%d]\n", __func__, rc);
+			err("%s - Exit Error:invalid bus, rc[%d]\n", __func__, rc);
 			return rc;
 		} else
 			index = (u8) busindex;
 	} else
 		index = pslot->ctlr_index;
 
-	index = hpc_readcmdtoindex (cmd, index);
+	index = hpc_readcmdtoindex(cmd, index);
 
 	if (index == HPC_ERROR) {
 		rc = -EINVAL;
-		err ("%s - Exit Error:invalid index, rc[%d]\n", __func__, rc);
+		err("%s - Exit Error:invalid index, rc[%d]\n", __func__, rc);
 		return rc;
 	}
 
 	ctlr_ptr = pslot->ctrl;
 
-	get_hpc_access ();
+	get_hpc_access();
 
 	//--------------------------------------------------------------------
 	// map physical address to logical address
 	//--------------------------------------------------------------------
 	if ((ctlr_ptr->ctlr_type == 2) || (ctlr_ptr->ctlr_type == 4))
-		wpg_bbar = ioremap (ctlr_ptr->u.wpeg_ctlr.wpegbbar, WPG_I2C_IOREMAP_SIZE);
+		wpg_bbar = ioremap(ctlr_ptr->u.wpeg_ctlr.wpegbbar, WPG_I2C_IOREMAP_SIZE);
 
 	//--------------------------------------------------------------------
 	// check controller status before reading
 	//--------------------------------------------------------------------
-	rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar, &status);
+	rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar, &status);
 	if (!rc) {
 		switch (cmd) {
 		case READ_ALLSTAT:
 			// update the slot structure
 			pslot->ctrl->status = status;
-			pslot->status = ctrl_read (ctlr_ptr, wpg_bbar, index);
-			rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar,
+			pslot->status = ctrl_read(ctlr_ptr, wpg_bbar, index);
+			rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar,
 						       &status);
 			if (!rc)
-				pslot->ext_status = ctrl_read (ctlr_ptr, wpg_bbar, index + WPG_1ST_EXTSLOT_INDEX);
+				pslot->ext_status = ctrl_read(ctlr_ptr, wpg_bbar, index + WPG_1ST_EXTSLOT_INDEX);
 
 			break;
 
 		case READ_SLOTSTATUS:
 			// DO NOT update the slot structure
-			*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 
 		case READ_EXTSLOTSTATUS:
 			// DO NOT update the slot structure
-			*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 
 		case READ_CTLRSTATUS:
@@ -613,36 +612,36 @@ int ibmphp_hpc_readslot (struct slot *pslot, u8 cmd, u8 *pstatus)
 			break;
 
 		case READ_BUSSTATUS:
-			pslot->busstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			pslot->busstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 		case READ_REVLEVEL:
-			*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 		case READ_HPCOPTIONS:
-			*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 		case READ_SLOTLATCHLOWREG:
 			// DO NOT update the slot structure
-			*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, index);
+			*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, index);
 			break;
 
 			// Not used
 		case READ_ALLSLOT:
-			list_for_each (pslotlist, &ibmphp_slot_head) {
-				pslot = list_entry (pslotlist, struct slot, ibm_slot_list);
+			list_for_each_entry(pslot, &ibmphp_slot_head,
+					    ibm_slot_list) {
 				index = pslot->ctlr_index;
-				rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT, ctlr_ptr,
+				rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT, ctlr_ptr,
 								wpg_bbar, &status);
 				if (!rc) {
-					pslot->status = ctrl_read (ctlr_ptr, wpg_bbar, index);
-					rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT,
+					pslot->status = ctrl_read(ctlr_ptr, wpg_bbar, index);
+					rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT,
 									ctlr_ptr, wpg_bbar, &status);
 					if (!rc)
 						pslot->ext_status =
-						    ctrl_read (ctlr_ptr, wpg_bbar,
+						    ctrl_read(ctlr_ptr, wpg_bbar,
 								index + WPG_1ST_EXTSLOT_INDEX);
 				} else {
-					err ("%s - Error ctrl_read failed\n", __func__);
+					err("%s - Error ctrl_read failed\n", __func__);
 					rc = -EINVAL;
 					break;
 				}
@@ -659,11 +658,11 @@ int ibmphp_hpc_readslot (struct slot *pslot, u8 cmd, u8 *pstatus)
 
 	// remove physical to logical address mapping
 	if ((ctlr_ptr->ctlr_type == 2) || (ctlr_ptr->ctlr_type == 4))
-		iounmap (wpg_bbar);
+		iounmap(wpg_bbar);
 
-	free_hpc_access ();
+	free_hpc_access();
 
-	debug_polling ("%s - Exit rc[%d]\n", __func__, rc);
+	debug_polling("%s - Exit rc[%d]\n", __func__, rc);
 	return rc;
 }
 
@@ -672,7 +671,7 @@ int ibmphp_hpc_readslot (struct slot *pslot, u8 cmd, u8 *pstatus)
 *
 * Action: issue a WRITE command to HPC
 *---------------------------------------------------------------------*/
-int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
+int ibmphp_hpc_writeslot(struct slot *pslot, u8 cmd)
 {
 	void __iomem *wpg_bbar = NULL;
 	struct controller *ctlr_ptr;
@@ -682,55 +681,55 @@ int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
 	int rc = 0;
 	int timeout;
 
-	debug_polling ("%s - Entry pslot[%p] cmd[%x]\n", __func__, pslot, cmd);
+	debug_polling("%s - Entry pslot[%p] cmd[%x]\n", __func__, pslot, cmd);
 	if (pslot == NULL) {
 		rc = -EINVAL;
-		err ("%s - Error Exit rc[%d]\n", __func__, rc);
+		err("%s - Error Exit rc[%d]\n", __func__, rc);
 		return rc;
 	}
 
 	if ((cmd == HPC_BUS_33CONVMODE) || (cmd == HPC_BUS_66CONVMODE) ||
 		(cmd == HPC_BUS_66PCIXMODE) || (cmd == HPC_BUS_100PCIXMODE) ||
 		(cmd == HPC_BUS_133PCIXMODE)) {
-		busindex = ibmphp_get_bus_index (pslot->bus);
+		busindex = ibmphp_get_bus_index(pslot->bus);
 		if (busindex < 0) {
 			rc = -EINVAL;
-			err ("%s - Exit Error:invalid bus, rc[%d]\n", __func__, rc);
+			err("%s - Exit Error:invalid bus, rc[%d]\n", __func__, rc);
 			return rc;
 		} else
 			index = (u8) busindex;
 	} else
 		index = pslot->ctlr_index;
 
-	index = hpc_writecmdtoindex (cmd, index);
+	index = hpc_writecmdtoindex(cmd, index);
 
 	if (index == HPC_ERROR) {
 		rc = -EINVAL;
-		err ("%s - Error Exit rc[%d]\n", __func__, rc);
+		err("%s - Error Exit rc[%d]\n", __func__, rc);
 		return rc;
 	}
 
 	ctlr_ptr = pslot->ctrl;
 
-	get_hpc_access ();
+	get_hpc_access();
 
 	//--------------------------------------------------------------------
 	// map physical address to logical address
 	//--------------------------------------------------------------------
 	if ((ctlr_ptr->ctlr_type == 2) || (ctlr_ptr->ctlr_type == 4)) {
-		wpg_bbar = ioremap (ctlr_ptr->u.wpeg_ctlr.wpegbbar, WPG_I2C_IOREMAP_SIZE);
+		wpg_bbar = ioremap(ctlr_ptr->u.wpeg_ctlr.wpegbbar, WPG_I2C_IOREMAP_SIZE);
 
-		debug ("%s - ctlr id[%x] physical[%lx] logical[%lx] i2c[%x]\n", __func__,
+		debug("%s - ctlr id[%x] physical[%lx] logical[%lx] i2c[%x]\n", __func__,
 		ctlr_ptr->ctlr_id, (ulong) (ctlr_ptr->u.wpeg_ctlr.wpegbbar), (ulong) wpg_bbar,
 		ctlr_ptr->u.wpeg_ctlr.i2c_addr);
 	}
 	//--------------------------------------------------------------------
 	// check controller status before writing
 	//--------------------------------------------------------------------
-	rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar, &status);
+	rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar, &status);
 	if (!rc) {
 
-		ctrl_write (ctlr_ptr, wpg_bbar, index, cmd);
+		ctrl_write(ctlr_ptr, wpg_bbar, index, cmd);
 
 		//--------------------------------------------------------------------
 		// check controller is still not working on the command
@@ -738,11 +737,11 @@ int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
 		timeout = CMD_COMPLETE_TOUT_SEC;
 		done = 0;
 		while (!done) {
-			rc = hpc_wait_ctlr_notworking (HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar,
+			rc = hpc_wait_ctlr_notworking(HPC_CTLR_WORKING_TOUT, ctlr_ptr, wpg_bbar,
 							&status);
 			if (!rc) {
-				if (NEEDTOCHECK_CMDSTATUS (cmd)) {
-					if (CTLR_FINISHED (status) == HPC_CTLR_FINISHED_YES)
+				if (NEEDTOCHECK_CMDSTATUS(cmd)) {
+					if (CTLR_FINISHED(status) == HPC_CTLR_FINISHED_YES)
 						done = 1;
 				} else
 					done = 1;
@@ -751,7 +750,7 @@ int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
 				msleep(1000);
 				if (timeout < 1) {
 					done = 1;
-					err ("%s - Error command complete timeout\n", __func__);
+					err("%s - Error command complete timeout\n", __func__);
 					rc = -EFAULT;
 				} else
 					timeout--;
@@ -763,10 +762,10 @@ int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
 
 	// remove physical to logical address mapping
 	if ((ctlr_ptr->ctlr_type == 2) || (ctlr_ptr->ctlr_type == 4))
-		iounmap (wpg_bbar);
-	free_hpc_access ();
+		iounmap(wpg_bbar);
+	free_hpc_access();
 
-	debug_polling ("%s - Exit rc[%d]\n", __func__, rc);
+	debug_polling("%s - Exit rc[%d]\n", __func__, rc);
 	return rc;
 }
 
@@ -775,7 +774,7 @@ int ibmphp_hpc_writeslot (struct slot *pslot, u8 cmd)
 *
 * Action: make sure only one process can access HPC at one time
 *---------------------------------------------------------------------*/
-static void get_hpc_access (void)
+static void get_hpc_access(void)
 {
 	mutex_lock(&sem_hpcaccess);
 }
@@ -783,7 +782,7 @@ static void get_hpc_access (void)
 /*----------------------------------------------------------------------
 * Name:    free_hpc_access()
 *---------------------------------------------------------------------*/
-void free_hpc_access (void)
+void free_hpc_access(void)
 {
 	mutex_unlock(&sem_hpcaccess);
 }
@@ -793,21 +792,21 @@ void free_hpc_access (void)
 *
 * Action: make sure only one process can change the data structure
 *---------------------------------------------------------------------*/
-void ibmphp_lock_operations (void)
+void ibmphp_lock_operations(void)
 {
-	down (&semOperations);
+	down(&semOperations);
 	to_debug = 1;
 }
 
 /*----------------------------------------------------------------------
 * Name:    ibmphp_unlock_operations()
 *---------------------------------------------------------------------*/
-void ibmphp_unlock_operations (void)
+void ibmphp_unlock_operations(void)
 {
-	debug ("%s - Entry\n", __func__);
-	up (&semOperations);
+	debug("%s - Entry\n", __func__);
+	up(&semOperations);
 	to_debug = 0;
-	debug ("%s - Exit\n", __func__);
+	debug("%s - Exit\n", __func__);
 }
 
 /*----------------------------------------------------------------------
@@ -820,7 +819,6 @@ static int poll_hpc(void *data)
 {
 	struct slot myslot;
 	struct slot *pslot = NULL;
-	struct list_head *pslotlist;
 	int rc;
 	int poll_state = POLL_LATCH_REGISTER;
 	u8 oldlatchlow = 0x00;
@@ -828,28 +826,28 @@ static int poll_hpc(void *data)
 	int poll_count = 0;
 	u8 ctrl_count = 0x00;
 
-	debug ("%s - Entry\n", __func__);
+	debug("%s - Entry\n", __func__);
 
 	while (!kthread_should_stop()) {
 		/* try to get the lock to do some kind of hardware access */
-		down (&semOperations);
+		down(&semOperations);
 
 		switch (poll_state) {
 		case POLL_LATCH_REGISTER:
 			oldlatchlow = curlatchlow;
 			ctrl_count = 0x00;
-			list_for_each (pslotlist, &ibmphp_slot_head) {
+			list_for_each_entry(pslot, &ibmphp_slot_head,
+					    ibm_slot_list) {
 				if (ctrl_count >= ibmphp_get_total_controllers())
 					break;
-				pslot = list_entry (pslotlist, struct slot, ibm_slot_list);
 				if (pslot->ctrl->ctlr_relative_id == ctrl_count) {
 					ctrl_count++;
-					if (READ_SLOT_LATCH (pslot->ctrl)) {
-						rc = ibmphp_hpc_readslot (pslot,
+					if (READ_SLOT_LATCH(pslot->ctrl)) {
+						rc = ibmphp_hpc_readslot(pslot,
 									  READ_SLOTLATCHLOWREG,
 									  &curlatchlow);
 						if (oldlatchlow != curlatchlow)
-							process_changeinlatch (oldlatchlow,
+							process_changeinlatch(oldlatchlow,
 									       curlatchlow,
 									       pslot->ctrl);
 					}
@@ -859,25 +857,25 @@ static int poll_hpc(void *data)
 			poll_state = POLL_SLEEP;
 			break;
 		case POLL_SLOTS:
-			list_for_each (pslotlist, &ibmphp_slot_head) {
-				pslot = list_entry (pslotlist, struct slot, ibm_slot_list);
+			list_for_each_entry(pslot, &ibmphp_slot_head,
+					    ibm_slot_list) {
 				// make a copy of the old status
-				memcpy ((void *) &myslot, (void *) pslot,
-					sizeof (struct slot));
-				rc = ibmphp_hpc_readslot (pslot, READ_ALLSTAT, NULL);
+				memcpy((void *) &myslot, (void *) pslot,
+					sizeof(struct slot));
+				rc = ibmphp_hpc_readslot(pslot, READ_ALLSTAT, NULL);
 				if ((myslot.status != pslot->status)
 				    || (myslot.ext_status != pslot->ext_status))
-					process_changeinstatus (pslot, &myslot);
+					process_changeinstatus(pslot, &myslot);
 			}
 			ctrl_count = 0x00;
-			list_for_each (pslotlist, &ibmphp_slot_head) {
+			list_for_each_entry(pslot, &ibmphp_slot_head,
+					    ibm_slot_list) {
 				if (ctrl_count >= ibmphp_get_total_controllers())
 					break;
-				pslot = list_entry (pslotlist, struct slot, ibm_slot_list);
 				if (pslot->ctrl->ctlr_relative_id == ctrl_count) {
 					ctrl_count++;
-					if (READ_SLOT_LATCH (pslot->ctrl))
-						rc = ibmphp_hpc_readslot (pslot,
+					if (READ_SLOT_LATCH(pslot->ctrl))
+						rc = ibmphp_hpc_readslot(pslot,
 									  READ_SLOTLATCHLOWREG,
 									  &curlatchlow);
 				}
@@ -887,13 +885,13 @@ static int poll_hpc(void *data)
 			break;
 		case POLL_SLEEP:
 			/* don't sleep with a lock on the hardware */
-			up (&semOperations);
+			up(&semOperations);
 			msleep(POLL_INTERVAL_SEC * 1000);
 
 			if (kthread_should_stop())
 				goto out_sleep;
 
-			down (&semOperations);
+			down(&semOperations);
 
 			if (poll_count >= POLL_LATCH_CNT) {
 				poll_count = 0;
@@ -903,13 +901,13 @@ static int poll_hpc(void *data)
 			break;
 		}
 		/* give up the hardware semaphore */
-		up (&semOperations);
+		up(&semOperations);
 		/* sleep for a short time just for good measure */
 out_sleep:
 		msleep(100);
 	}
-	up (&sem_exit);
-	debug ("%s - Exit\n", __func__);
+	up(&sem_exit);
+	debug("%s - Exit\n", __func__);
 	return 0;
 }
 
@@ -929,14 +927,14 @@ out_sleep:
 *
 * Notes:
 *---------------------------------------------------------------------*/
-static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
+static int process_changeinstatus(struct slot *pslot, struct slot *poldslot)
 {
 	u8 status;
 	int rc = 0;
 	u8 disable = 0;
 	u8 update = 0;
 
-	debug ("process_changeinstatus - Entry pslot[%p], poldslot[%p]\n", pslot, poldslot);
+	debug("process_changeinstatus - Entry pslot[%p], poldslot[%p]\n", pslot, poldslot);
 
 	// bit 0 - HPC_SLOT_POWER
 	if ((pslot->status & 0x01) != (poldslot->status & 0x01))
@@ -958,7 +956,7 @@ static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
 	// bit 5 - HPC_SLOT_PWRGD
 	if ((pslot->status & 0x20) != (poldslot->status & 0x20))
 		// OFF -> ON: ignore, ON -> OFF: disable slot
-		if ((poldslot->status & 0x20) && (SLOT_CONNECT (poldslot->status) == HPC_SLOT_CONNECTED) && (SLOT_PRESENT (poldslot->status)))
+		if ((poldslot->status & 0x20) && (SLOT_CONNECT(poldslot->status) == HPC_SLOT_CONNECTED) && (SLOT_PRESENT(poldslot->status)))
 			disable = 1;
 
 	// bit 6 - HPC_SLOT_BUS_SPEED
@@ -969,20 +967,20 @@ static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
 		update = 1;
 		// OPEN -> CLOSE
 		if (pslot->status & 0x80) {
-			if (SLOT_PWRGD (pslot->status)) {
+			if (SLOT_PWRGD(pslot->status)) {
 				// power goes on and off after closing latch
 				// check again to make sure power is still ON
 				msleep(1000);
-				rc = ibmphp_hpc_readslot (pslot, READ_SLOTSTATUS, &status);
-				if (SLOT_PWRGD (status))
+				rc = ibmphp_hpc_readslot(pslot, READ_SLOTSTATUS, &status);
+				if (SLOT_PWRGD(status))
 					update = 1;
 				else	// overwrite power in pslot to OFF
 					pslot->status &= ~HPC_SLOT_POWER;
 			}
 		}
 		// CLOSE -> OPEN
-		else if ((SLOT_PWRGD (poldslot->status) == HPC_SLOT_PWRGD_GOOD)
-			&& (SLOT_CONNECT (poldslot->status) == HPC_SLOT_CONNECTED) && (SLOT_PRESENT (poldslot->status))) {
+		else if ((SLOT_PWRGD(poldslot->status) == HPC_SLOT_PWRGD_GOOD)
+			&& (SLOT_CONNECT(poldslot->status) == HPC_SLOT_CONNECTED) && (SLOT_PRESENT(poldslot->status))) {
 			disable = 1;
 		}
 		// else - ignore
@@ -992,15 +990,15 @@ static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
 		update = 1;
 
 	if (disable) {
-		debug ("process_changeinstatus - disable slot\n");
+		debug("process_changeinstatus - disable slot\n");
 		pslot->flag = 0;
-		rc = ibmphp_do_disable_slot (pslot);
+		rc = ibmphp_do_disable_slot(pslot);
 	}
 
 	if (update || disable)
-		ibmphp_update_slot_info (pslot);
+		ibmphp_update_slot_info(pslot);
 
-	debug ("%s - Exit rc[%d] disable[%x] update[%x]\n", __func__, rc, disable, update);
+	debug("%s - Exit rc[%d] disable[%x] update[%x]\n", __func__, rc, disable, update);
 
 	return rc;
 }
@@ -1015,32 +1013,32 @@ static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
 * Return   0 or error codes
 * Value:
 *---------------------------------------------------------------------*/
-static int process_changeinlatch (u8 old, u8 new, struct controller *ctrl)
+static int process_changeinlatch(u8 old, u8 new, struct controller *ctrl)
 {
 	struct slot myslot, *pslot;
 	u8 i;
 	u8 mask;
 	int rc = 0;
 
-	debug ("%s - Entry old[%x], new[%x]\n", __func__, old, new);
+	debug("%s - Entry old[%x], new[%x]\n", __func__, old, new);
 	// bit 0 reserved, 0 is LSB, check bit 1-6 for 6 slots
 
 	for (i = ctrl->starting_slot_num; i <= ctrl->ending_slot_num; i++) {
 		mask = 0x01 << i;
 		if ((mask & old) != (mask & new)) {
-			pslot = ibmphp_get_slot_from_physical_num (i);
+			pslot = ibmphp_get_slot_from_physical_num(i);
 			if (pslot) {
-				memcpy ((void *) &myslot, (void *) pslot, sizeof (struct slot));
-				rc = ibmphp_hpc_readslot (pslot, READ_ALLSTAT, NULL);
-				debug ("%s - call process_changeinstatus for slot[%d]\n", __func__, i);
-				process_changeinstatus (pslot, &myslot);
+				memcpy((void *) &myslot, (void *) pslot, sizeof(struct slot));
+				rc = ibmphp_hpc_readslot(pslot, READ_ALLSTAT, NULL);
+				debug("%s - call process_changeinstatus for slot[%d]\n", __func__, i);
+				process_changeinstatus(pslot, &myslot);
 			} else {
 				rc = -EINVAL;
-				err ("%s - Error bad pointer for slot[%d]\n", __func__, i);
+				err("%s - Error bad pointer for slot[%d]\n", __func__, i);
 			}
 		}
 	}
-	debug ("%s - Exit rc[%d]\n", __func__, rc);
+	debug("%s - Exit rc[%d]\n", __func__, rc);
 	return rc;
 }
 
@@ -1049,13 +1047,13 @@ static int process_changeinlatch (u8 old, u8 new, struct controller *ctrl)
 *
 * Action:  start polling thread
 *---------------------------------------------------------------------*/
-int __init ibmphp_hpc_start_poll_thread (void)
+int __init ibmphp_hpc_start_poll_thread(void)
 {
-	debug ("%s - Entry\n", __func__);
+	debug("%s - Entry\n", __func__);
 
 	ibmphp_poll_thread = kthread_run(poll_hpc, NULL, "hpc_poll");
 	if (IS_ERR(ibmphp_poll_thread)) {
-		err ("%s - Error, thread not started\n", __func__);
+		err("%s - Error, thread not started\n", __func__);
 		return PTR_ERR(ibmphp_poll_thread);
 	}
 	return 0;
@@ -1066,30 +1064,30 @@ int __init ibmphp_hpc_start_poll_thread (void)
 *
 * Action:  stop polling thread and cleanup
 *---------------------------------------------------------------------*/
-void __exit ibmphp_hpc_stop_poll_thread (void)
+void __exit ibmphp_hpc_stop_poll_thread(void)
 {
-	debug ("%s - Entry\n", __func__);
+	debug("%s - Entry\n", __func__);
 
 	kthread_stop(ibmphp_poll_thread);
-	debug ("before locking operations \n");
-	ibmphp_lock_operations ();
-	debug ("after locking operations \n");
+	debug("before locking operations\n");
+	ibmphp_lock_operations();
+	debug("after locking operations\n");
 
 	// wait for poll thread to exit
-	debug ("before sem_exit down \n");
-	down (&sem_exit);
-	debug ("after sem_exit down \n");
+	debug("before sem_exit down\n");
+	down(&sem_exit);
+	debug("after sem_exit down\n");
 
 	// cleanup
-	debug ("before free_hpc_access \n");
-	free_hpc_access ();
-	debug ("after free_hpc_access \n");
-	ibmphp_unlock_operations ();
-	debug ("after unlock operations \n");
-	up (&sem_exit);
-	debug ("after sem exit up\n");
-
-	debug ("%s - Exit\n", __func__);
+	debug("before free_hpc_access\n");
+	free_hpc_access();
+	debug("after free_hpc_access\n");
+	ibmphp_unlock_operations();
+	debug("after unlock operations\n");
+	up(&sem_exit);
+	debug("after sem exit up\n");
+
+	debug("%s - Exit\n", __func__);
 }
 
 /*----------------------------------------------------------------------
@@ -1100,32 +1098,32 @@ void __exit ibmphp_hpc_stop_poll_thread (void)
 * Return   0, HPC_ERROR
 * Value:
 *---------------------------------------------------------------------*/
-static int hpc_wait_ctlr_notworking (int timeout, struct controller *ctlr_ptr, void __iomem *wpg_bbar,
+static int hpc_wait_ctlr_notworking(int timeout, struct controller *ctlr_ptr, void __iomem *wpg_bbar,
 				    u8 *pstatus)
 {
 	int rc = 0;
 	u8 done = 0;
 
-	debug_polling ("hpc_wait_ctlr_notworking - Entry timeout[%d]\n", timeout);
+	debug_polling("hpc_wait_ctlr_notworking - Entry timeout[%d]\n", timeout);
 
 	while (!done) {
-		*pstatus = ctrl_read (ctlr_ptr, wpg_bbar, WPG_CTLR_INDEX);
+		*pstatus = ctrl_read(ctlr_ptr, wpg_bbar, WPG_CTLR_INDEX);
 		if (*pstatus == HPC_ERROR) {
 			rc = HPC_ERROR;
 			done = 1;
 		}
-		if (CTLR_WORKING (*pstatus) == HPC_CTLR_WORKING_NO)
+		if (CTLR_WORKING(*pstatus) == HPC_CTLR_WORKING_NO)
 			done = 1;
 		if (!done) {
 			msleep(1000);
 			if (timeout < 1) {
 				done = 1;
-				err ("HPCreadslot - Error ctlr timeout\n");
+				err("HPCreadslot - Error ctlr timeout\n");
 				rc = HPC_ERROR;
 			} else
 				timeout--;
 		}
 	}
-	debug_polling ("hpc_wait_ctlr_notworking - Exit rc[%x] status[%x]\n", rc, *pstatus);
+	debug_polling("hpc_wait_ctlr_notworking - Exit rc[%x] status[%x]\n", rc, *pstatus);
 	return rc;
 }
diff --git a/drivers/pci/hotplug/ibmphp_pci.c b/drivers/pci/hotplug/ibmphp_pci.c
index 814cea22a9fa..dc1876feb06f 100644
--- a/drivers/pci/hotplug/ibmphp_pci.c
+++ b/drivers/pci/hotplug/ibmphp_pci.c
@@ -37,8 +37,8 @@
 static int configure_device(struct pci_func *);
 static int configure_bridge(struct pci_func **, u8);
 static struct res_needed *scan_behind_bridge(struct pci_func *, u8);
-static int add_new_bus (struct bus_node *, struct resource_node *, struct resource_node *, struct resource_node *, u8);
-static u8 find_sec_number (u8 primary_busno, u8 slotno);
+static int add_new_bus(struct bus_node *, struct resource_node *, struct resource_node *, struct resource_node *, u8);
+static u8 find_sec_number(u8 primary_busno, u8 slotno);
 
 /*
  * NOTE..... If BIOS doesn't provide default routing, we assign:
@@ -47,7 +47,7 @@ static u8 find_sec_number (u8 primary_busno, u8 slotno);
  * We also assign the same irq numbers for multi function devices.
  * These are PIC mode, so shouldn't matter n.e.ways (hopefully)
  */
-static void assign_alt_irq (struct pci_func *cur_func, u8 class_code)
+static void assign_alt_irq(struct pci_func *cur_func, u8 class_code)
 {
 	int j;
 	for (j = 0; j < 4; j++) {
@@ -78,7 +78,7 @@ static void assign_alt_irq (struct pci_func *cur_func, u8 class_code)
  * if there is an error, will need to go through all previous functions and
  * unconfigure....or can add some code into unconfigure_card....
  */
-int ibmphp_configure_card (struct pci_func *func, u8 slotno)
+int ibmphp_configure_card(struct pci_func *func, u8 slotno)
 {
 	u16 vendor_id;
 	u32 class;
@@ -92,7 +92,7 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 	u8 flag;
 	u8 valid_device = 0x00; /* to see if we are able to read from card any device info at all */
 
-	debug ("inside configure_card, func->busno = %x\n", func->busno);
+	debug("inside configure_card, func->busno = %x\n", func->busno);
 
 	device = func->device;
 	cur_func = func;
@@ -109,15 +109,15 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 
 		cur_func->function = function;
 
-		debug ("inside the loop, cur_func->busno = %x, cur_func->device = %x, cur_func->function = %x\n",
+		debug("inside the loop, cur_func->busno = %x, cur_func->device = %x, cur_func->function = %x\n",
 			cur_func->busno, cur_func->device, cur_func->function);
 
-		pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
+		pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
 
-		debug ("vendor_id is %x\n", vendor_id);
+		debug("vendor_id is %x\n", vendor_id);
 		if (vendor_id != PCI_VENDOR_ID_NOTVALID) {
 			/* found correct device!!! */
-			debug ("found valid device, vendor_id = %x\n", vendor_id);
+			debug("found valid device, vendor_id = %x\n", vendor_id);
 
 			++valid_device;
 
@@ -126,29 +126,29 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 			 *         |_=> 0 = single function device, 1 = multi-function device
 			 */
 
-			pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
-			pci_bus_read_config_dword (ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
+			pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
+			pci_bus_read_config_dword(ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
 
 			class_code = class >> 24;
-			debug ("hrd_type = %x, class = %x, class_code %x\n", hdr_type, class, class_code);
+			debug("hrd_type = %x, class = %x, class_code %x\n", hdr_type, class, class_code);
 			class >>= 8;	/* to take revision out, class = class.subclass.prog i/f */
 			if (class == PCI_CLASS_NOT_DEFINED_VGA) {
-				err ("The device %x is VGA compatible and as is not supported for hot plugging. "
+				err("The device %x is VGA compatible and as is not supported for hot plugging. "
 				     "Please choose another device.\n", cur_func->device);
 				return -ENODEV;
 			} else if (class == PCI_CLASS_DISPLAY_VGA) {
-				err ("The device %x is not supported for hot plugging. Please choose another device.\n",
+				err("The device %x is not supported for hot plugging. Please choose another device.\n",
 				     cur_func->device);
 				return -ENODEV;
 			}
 			switch (hdr_type) {
 				case PCI_HEADER_TYPE_NORMAL:
-					debug ("single device case.... vendor id = %x, hdr_type = %x, class = %x\n", vendor_id, hdr_type, class);
-					assign_alt_irq (cur_func, class_code);
+					debug("single device case.... vendor id = %x, hdr_type = %x, class = %x\n", vendor_id, hdr_type, class);
+					assign_alt_irq(cur_func, class_code);
 					rc = configure_device(cur_func);
 					if (rc < 0) {
 						/* We need to do this in case some other BARs were properly inserted */
-						err ("was not able to configure devfunc %x on bus %x.\n",
+						err("was not able to configure devfunc %x on bus %x.\n",
 						     cur_func->device, cur_func->busno);
 						cleanup_count = 6;
 						goto error;
@@ -157,18 +157,18 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 					function = 0x8;
 					break;
 				case PCI_HEADER_TYPE_MULTIDEVICE:
-					assign_alt_irq (cur_func, class_code);
+					assign_alt_irq(cur_func, class_code);
 					rc = configure_device(cur_func);
 					if (rc < 0) {
 						/* We need to do this in case some other BARs were properly inserted */
-						err ("was not able to configure devfunc %x on bus %x...bailing out\n",
+						err("was not able to configure devfunc %x on bus %x...bailing out\n",
 						     cur_func->device, cur_func->busno);
 						cleanup_count = 6;
 						goto error;
 					}
 					newfunc = kzalloc(sizeof(*newfunc), GFP_KERNEL);
 					if (!newfunc) {
-						err ("out of system memory\n");
+						err("out of system memory\n");
 						return -ENOMEM;
 					}
 					newfunc->busno = cur_func->busno;
@@ -181,32 +181,32 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 				case PCI_HEADER_TYPE_MULTIBRIDGE:
 					class >>= 8;
 					if (class != PCI_CLASS_BRIDGE_PCI) {
-						err ("This %x is not PCI-to-PCI bridge, and as is not supported for hot-plugging.  Please insert another card.\n",
+						err("This %x is not PCI-to-PCI bridge, and as is not supported for hot-plugging.  Please insert another card.\n",
 						     cur_func->device);
 						return -ENODEV;
 					}
-					assign_alt_irq (cur_func, class_code);
-					rc = configure_bridge (&cur_func, slotno);
+					assign_alt_irq(cur_func, class_code);
+					rc = configure_bridge(&cur_func, slotno);
 					if (rc == -ENODEV) {
-						err ("You chose to insert Single Bridge, or nested bridges, this is not supported...\n");
-						err ("Bus %x, devfunc %x\n", cur_func->busno, cur_func->device);
+						err("You chose to insert Single Bridge, or nested bridges, this is not supported...\n");
+						err("Bus %x, devfunc %x\n", cur_func->busno, cur_func->device);
 						return rc;
 					}
 					if (rc) {
 						/* We need to do this in case some other BARs were properly inserted */
-						err ("was not able to hot-add PPB properly.\n");
+						err("was not able to hot-add PPB properly.\n");
 						func->bus = 1; /* To indicate to the unconfigure function that this is a PPB */
 						cleanup_count = 2;
 						goto error;
 					}
 
-					pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
+					pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
 					flag = 0;
 					for (i = 0; i < 32; i++) {
 						if (func->devices[i]) {
 							newfunc = kzalloc(sizeof(*newfunc), GFP_KERNEL);
 							if (!newfunc) {
-								err ("out of system memory\n");
+								err("out of system memory\n");
 								return -ENOMEM;
 							}
 							newfunc->busno = sec_number;
@@ -220,7 +220,7 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 							} else
 								cur_func->next = newfunc;
 
-							rc = ibmphp_configure_card (newfunc, slotno);
+							rc = ibmphp_configure_card(newfunc, slotno);
 							/* This could only happen if kmalloc failed */
 							if (rc) {
 								/* We need to do this in case bridge itself got configured properly, but devices behind it failed */
@@ -234,53 +234,53 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 
 					newfunc = kzalloc(sizeof(*newfunc), GFP_KERNEL);
 					if (!newfunc) {
-						err ("out of system memory\n");
+						err("out of system memory\n");
 						return -ENOMEM;
 					}
 					newfunc->busno = cur_func->busno;
 					newfunc->device = device;
 					for (j = 0; j < 4; j++)
 						newfunc->irq[j] = cur_func->irq[j];
-					for (prev_func = cur_func; prev_func->next; prev_func = prev_func->next) ;
+					for (prev_func = cur_func; prev_func->next; prev_func = prev_func->next);
 					prev_func->next = newfunc;
 					cur_func = newfunc;
 					break;
 				case PCI_HEADER_TYPE_BRIDGE:
 					class >>= 8;
-					debug ("class now is %x\n", class);
+					debug("class now is %x\n", class);
 					if (class != PCI_CLASS_BRIDGE_PCI) {
-						err ("This %x is not PCI-to-PCI bridge, and as is not supported for hot-plugging.  Please insert another card.\n",
+						err("This %x is not PCI-to-PCI bridge, and as is not supported for hot-plugging.  Please insert another card.\n",
 						     cur_func->device);
 						return -ENODEV;
 					}
 
-					assign_alt_irq (cur_func, class_code);
+					assign_alt_irq(cur_func, class_code);
 
-					debug ("cur_func->busno b4 configure_bridge is %x\n", cur_func->busno);
-					rc = configure_bridge (&cur_func, slotno);
+					debug("cur_func->busno b4 configure_bridge is %x\n", cur_func->busno);
+					rc = configure_bridge(&cur_func, slotno);
 					if (rc == -ENODEV) {
-						err ("You chose to insert Single Bridge, or nested bridges, this is not supported...\n");
-						err ("Bus %x, devfunc %x\n", cur_func->busno, cur_func->device);
+						err("You chose to insert Single Bridge, or nested bridges, this is not supported...\n");
+						err("Bus %x, devfunc %x\n", cur_func->busno, cur_func->device);
 						return rc;
 					}
 					if (rc) {
 						/* We need to do this in case some other BARs were properly inserted */
 						func->bus = 1; /* To indicate to the unconfigure function that this is a PPB */
-						err ("was not able to hot-add PPB properly.\n");
+						err("was not able to hot-add PPB properly.\n");
 						cleanup_count = 2;
 						goto error;
 					}
-					debug ("cur_func->busno = %x, device = %x, function = %x\n",
+					debug("cur_func->busno = %x, device = %x, function = %x\n",
 						cur_func->busno, device, function);
-					pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
-					debug ("after configuring bridge..., sec_number = %x\n", sec_number);
+					pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
+					debug("after configuring bridge..., sec_number = %x\n", sec_number);
 					flag = 0;
 					for (i = 0; i < 32; i++) {
 						if (func->devices[i]) {
-							debug ("inside for loop, device is %x\n", i);
+							debug("inside for loop, device is %x\n", i);
 							newfunc = kzalloc(sizeof(*newfunc), GFP_KERNEL);
 							if (!newfunc) {
-								err (" out of system memory\n");
+								err(" out of system memory\n");
 								return -ENOMEM;
 							}
 							newfunc->busno = sec_number;
@@ -289,12 +289,12 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 								newfunc->irq[j] = cur_func->irq[j];
 
 							if (flag) {
-								for (prev_func = cur_func; prev_func->next; prev_func = prev_func->next) ;
+								for (prev_func = cur_func; prev_func->next; prev_func = prev_func->next);
 								prev_func->next = newfunc;
 							} else
 								cur_func->next = newfunc;
 
-							rc = ibmphp_configure_card (newfunc, slotno);
+							rc = ibmphp_configure_card(newfunc, slotno);
 
 							/* Again, this case should not happen... For complete paranoia, will need to call remove_bus */
 							if (rc) {
@@ -310,7 +310,7 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 					function = 0x8;
 					break;
 				default:
-					err ("MAJOR PROBLEM!!!!, header type not supported? %x\n", hdr_type);
+					err("MAJOR PROBLEM!!!!, header type not supported? %x\n", hdr_type);
 					return -ENXIO;
 					break;
 			}	/* end of switch */
@@ -318,7 +318,7 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 	}	/* end of for */
 
 	if (!valid_device) {
-		err ("Cannot find any valid devices on the card.  Or unable to read from card.\n");
+		err("Cannot find any valid devices on the card.  Or unable to read from card.\n");
 		return -ENODEV;
 	}
 
@@ -327,13 +327,13 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 error:
 	for (i = 0; i < cleanup_count; i++) {
 		if (cur_func->io[i]) {
-			ibmphp_remove_resource (cur_func->io[i]);
+			ibmphp_remove_resource(cur_func->io[i]);
 			cur_func->io[i] = NULL;
 		} else if (cur_func->pfmem[i]) {
-			ibmphp_remove_resource (cur_func->pfmem[i]);
+			ibmphp_remove_resource(cur_func->pfmem[i]);
 			cur_func->pfmem[i] = NULL;
 		} else if (cur_func->mem[i]) {
-			ibmphp_remove_resource (cur_func->mem[i]);
+			ibmphp_remove_resource(cur_func->mem[i]);
 			cur_func->mem[i] = NULL;
 		}
 	}
@@ -345,7 +345,7 @@ error:
  * Input: pointer to the pci_func
  * Output: configured PCI, 0, or error
  */
-static int configure_device (struct pci_func *func)
+static int configure_device(struct pci_func *func)
 {
 	u32 bar[6];
 	u32 address[] = {
@@ -366,7 +366,7 @@ static int configure_device (struct pci_func *func)
 	struct resource_node *pfmem[6];
 	unsigned int devfn;
 
-	debug ("%s - inside\n", __func__);
+	debug("%s - inside\n", __func__);
 
 	devfn = PCI_DEVFN(func->device, func->function);
 	ibmphp_pci_bus->number = func->busno;
@@ -386,27 +386,27 @@ static int configure_device (struct pci_func *func)
 			pcibios_write_config_dword(cur_func->busno, cur_func->device,
 			PCI_BASE_ADDRESS_0 + 4 * count, 0xFFFFFFFF);
 		 */
-		pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
-		pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
+		pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
+		pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
 
 		if (!bar[count])	/* This BAR is not implemented */
 			continue;
 
-		debug ("Device %x BAR %d wants %x\n", func->device, count, bar[count]);
+		debug("Device %x BAR %d wants %x\n", func->device, count, bar[count]);
 
 		if (bar[count] & PCI_BASE_ADDRESS_SPACE_IO) {
 			/* This is IO */
-			debug ("inside IO SPACE\n");
+			debug("inside IO SPACE\n");
 
 			len[count] = bar[count] & 0xFFFFFFFC;
 			len[count] = ~len[count] + 1;
 
-			debug ("len[count] in IO %x, count %d\n", len[count], count);
+			debug("len[count] in IO %x, count %d\n", len[count], count);
 
 			io[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 
 			if (!io[count]) {
-				err ("out of system memory\n");
+				err("out of system memory\n");
 				return -ENOMEM;
 			}
 			io[count]->type = IO;
@@ -414,36 +414,36 @@ static int configure_device (struct pci_func *func)
 			io[count]->devfunc = PCI_DEVFN(func->device, func->function);
 			io[count]->len = len[count];
 			if (ibmphp_check_resource(io[count], 0) == 0) {
-				ibmphp_add_resource (io[count]);
+				ibmphp_add_resource(io[count]);
 				func->io[count] = io[count];
 			} else {
-				err ("cannot allocate requested io for bus %x device %x function %x len %x\n",
+				err("cannot allocate requested io for bus %x device %x function %x len %x\n",
 				     func->busno, func->device, func->function, len[count]);
-				kfree (io[count]);
+				kfree(io[count]);
 				return -EIO;
 			}
-			pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->io[count]->start);
+			pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->io[count]->start);
 
 			/* _______________This is for debugging purposes only_____________________ */
-			debug ("b4 writing, the IO address is %x\n", func->io[count]->start);
-			pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
-			debug ("after writing.... the start address is %x\n", bar[count]);
+			debug("b4 writing, the IO address is %x\n", func->io[count]->start);
+			pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
+			debug("after writing.... the start address is %x\n", bar[count]);
 			/* _________________________________________________________________________*/
 
 		} else {
 			/* This is Memory */
 			if (bar[count] & PCI_BASE_ADDRESS_MEM_PREFETCH) {
 				/* pfmem */
-				debug ("PFMEM SPACE\n");
+				debug("PFMEM SPACE\n");
 
 				len[count] = bar[count] & 0xFFFFFFF0;
 				len[count] = ~len[count] + 1;
 
-				debug ("len[count] in PFMEM %x, count %d\n", len[count], count);
+				debug("len[count] in PFMEM %x, count %d\n", len[count], count);
 
 				pfmem[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 				if (!pfmem[count]) {
-					err ("out of system memory\n");
+					err("out of system memory\n");
 					return -ENOMEM;
 				}
 				pfmem[count]->type = PFMEM;
@@ -452,64 +452,64 @@ static int configure_device (struct pci_func *func)
 							func->function);
 				pfmem[count]->len = len[count];
 				pfmem[count]->fromMem = 0;
-				if (ibmphp_check_resource (pfmem[count], 0) == 0) {
-					ibmphp_add_resource (pfmem[count]);
+				if (ibmphp_check_resource(pfmem[count], 0) == 0) {
+					ibmphp_add_resource(pfmem[count]);
 					func->pfmem[count] = pfmem[count];
 				} else {
 					mem_tmp = kzalloc(sizeof(*mem_tmp), GFP_KERNEL);
 					if (!mem_tmp) {
-						err ("out of system memory\n");
-						kfree (pfmem[count]);
+						err("out of system memory\n");
+						kfree(pfmem[count]);
 						return -ENOMEM;
 					}
 					mem_tmp->type = MEM;
 					mem_tmp->busno = pfmem[count]->busno;
 					mem_tmp->devfunc = pfmem[count]->devfunc;
 					mem_tmp->len = pfmem[count]->len;
-					debug ("there's no pfmem... going into mem.\n");
-					if (ibmphp_check_resource (mem_tmp, 0) == 0) {
-						ibmphp_add_resource (mem_tmp);
+					debug("there's no pfmem... going into mem.\n");
+					if (ibmphp_check_resource(mem_tmp, 0) == 0) {
+						ibmphp_add_resource(mem_tmp);
 						pfmem[count]->fromMem = 1;
 						pfmem[count]->rangeno = mem_tmp->rangeno;
 						pfmem[count]->start = mem_tmp->start;
 						pfmem[count]->end = mem_tmp->end;
-						ibmphp_add_pfmem_from_mem (pfmem[count]);
+						ibmphp_add_pfmem_from_mem(pfmem[count]);
 						func->pfmem[count] = pfmem[count];
 					} else {
-						err ("cannot allocate requested pfmem for bus %x, device %x, len %x\n",
+						err("cannot allocate requested pfmem for bus %x, device %x, len %x\n",
 						     func->busno, func->device, len[count]);
-						kfree (mem_tmp);
-						kfree (pfmem[count]);
+						kfree(mem_tmp);
+						kfree(pfmem[count]);
 						return -EIO;
 					}
 				}
 
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->pfmem[count]->start);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->pfmem[count]->start);
 
 				/*_______________This is for debugging purposes only______________________________*/
-				debug ("b4 writing, start address is %x\n", func->pfmem[count]->start);
-				pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
-				debug ("after writing, start address is %x\n", bar[count]);
+				debug("b4 writing, start address is %x\n", func->pfmem[count]->start);
+				pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
+				debug("after writing, start address is %x\n", bar[count]);
 				/*_________________________________________________________________________________*/
 
 				if (bar[count] & PCI_BASE_ADDRESS_MEM_TYPE_64) {	/* takes up another dword */
-					debug ("inside the mem 64 case, count %d\n", count);
+					debug("inside the mem 64 case, count %d\n", count);
 					count += 1;
 					/* on the 2nd dword, write all 0s, since we can't handle them n.e.ways */
-					pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0x00000000);
+					pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0x00000000);
 				}
 			} else {
 				/* regular memory */
-				debug ("REGULAR MEM SPACE\n");
+				debug("REGULAR MEM SPACE\n");
 
 				len[count] = bar[count] & 0xFFFFFFF0;
 				len[count] = ~len[count] + 1;
 
-				debug ("len[count] in Mem %x, count %d\n", len[count], count);
+				debug("len[count] in Mem %x, count %d\n", len[count], count);
 
 				mem[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 				if (!mem[count]) {
-					err ("out of system memory\n");
+					err("out of system memory\n");
 					return -ENOMEM;
 				}
 				mem[count]->type = MEM;
@@ -517,43 +517,43 @@ static int configure_device (struct pci_func *func)
 				mem[count]->devfunc = PCI_DEVFN(func->device,
 							func->function);
 				mem[count]->len = len[count];
-				if (ibmphp_check_resource (mem[count], 0) == 0) {
-					ibmphp_add_resource (mem[count]);
+				if (ibmphp_check_resource(mem[count], 0) == 0) {
+					ibmphp_add_resource(mem[count]);
 					func->mem[count] = mem[count];
 				} else {
-					err ("cannot allocate requested mem for bus %x, device %x, len %x\n",
+					err("cannot allocate requested mem for bus %x, device %x, len %x\n",
 					     func->busno, func->device, len[count]);
-					kfree (mem[count]);
+					kfree(mem[count]);
 					return -EIO;
 				}
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->mem[count]->start);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->mem[count]->start);
 				/* _______________________This is for debugging purposes only _______________________*/
-				debug ("b4 writing, start address is %x\n", func->mem[count]->start);
-				pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
-				debug ("after writing, the address is %x\n", bar[count]);
+				debug("b4 writing, start address is %x\n", func->mem[count]->start);
+				pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
+				debug("after writing, the address is %x\n", bar[count]);
 				/* __________________________________________________________________________________*/
 
 				if (bar[count] & PCI_BASE_ADDRESS_MEM_TYPE_64) {
 					/* takes up another dword */
-					debug ("inside mem 64 case, reg. mem, count %d\n", count);
+					debug("inside mem 64 case, reg. mem, count %d\n", count);
 					count += 1;
 					/* on the 2nd dword, write all 0s, since we can't handle them n.e.ways */
-					pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0x00000000);
+					pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0x00000000);
 				}
 			}
 		}		/* end of mem */
 	}			/* end of for */
 
 	func->bus = 0;		/* To indicate that this is not a PPB */
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_INTERRUPT_PIN, &irq);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_INTERRUPT_PIN, &irq);
 	if ((irq > 0x00) && (irq < 0x05))
-		pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_INTERRUPT_LINE, func->irq[irq - 1]);
+		pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_INTERRUPT_LINE, func->irq[irq - 1]);
 
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_CACHE_LINE_SIZE, CACHE);
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_LATENCY_TIMER, LATENCY);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_CACHE_LINE_SIZE, CACHE);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_LATENCY_TIMER, LATENCY);
 
-	pci_bus_write_config_dword (ibmphp_pci_bus, devfn, PCI_ROM_ADDRESS, 0x00L);
-	pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_COMMAND, DEVICEENABLE);
+	pci_bus_write_config_dword(ibmphp_pci_bus, devfn, PCI_ROM_ADDRESS, 0x00L);
+	pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_COMMAND, DEVICEENABLE);
 
 	return 0;
 }
@@ -563,7 +563,7 @@ static int configure_device (struct pci_func *func)
  * Parameters: pci_func
  * Returns:
  ******************************************************************************/
-static int configure_bridge (struct pci_func **func_passed, u8 slotno)
+static int configure_bridge(struct pci_func **func_passed, u8 slotno)
 {
 	int count;
 	int i;
@@ -597,7 +597,7 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 	u8 irq;
 	int retval;
 
-	debug ("%s - enter\n", __func__);
+	debug("%s - enter\n", __func__);
 
 	devfn = PCI_DEVFN(func->function, func->device);
 	ibmphp_pci_bus->number = func->busno;
@@ -606,43 +606,43 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 	 * behind it
 	 */
 
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, func->busno);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, func->busno);
 
 	/* _____________________For debugging purposes only __________________________
-	pci_bus_config_byte (ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, &pri_number);
-	debug ("primary # written into the bridge is %x\n", pri_number);
+	pci_bus_config_byte(ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, &pri_number);
+	debug("primary # written into the bridge is %x\n", pri_number);
 	 ___________________________________________________________________________*/
 
 	/* in EBDA, only get allocated 1 additional bus # per slot */
-	sec_number = find_sec_number (func->busno, slotno);
+	sec_number = find_sec_number(func->busno, slotno);
 	if (sec_number == 0xff) {
-		err ("cannot allocate secondary bus number for the bridged device\n");
+		err("cannot allocate secondary bus number for the bridged device\n");
 		return -EINVAL;
 	}
 
-	debug ("after find_sec_number, the number we got is %x\n", sec_number);
-	debug ("AFTER FIND_SEC_NUMBER, func->busno IS %x\n", func->busno);
+	debug("after find_sec_number, the number we got is %x\n", sec_number);
+	debug("AFTER FIND_SEC_NUMBER, func->busno IS %x\n", func->busno);
 
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, sec_number);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, sec_number);
 
 	/* __________________For debugging purposes only __________________________________
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
-	debug ("sec_number after write/read is %x\n", sec_number);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
+	debug("sec_number after write/read is %x\n", sec_number);
 	 ________________________________________________________________________________*/
 
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, sec_number);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, sec_number);
 
 	/* __________________For debugging purposes only ____________________________________
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, &sec_number);
-	debug ("subordinate number after write/read is %x\n", sec_number);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, &sec_number);
+	debug("subordinate number after write/read is %x\n", sec_number);
 	 __________________________________________________________________________________*/
 
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_CACHE_LINE_SIZE, CACHE);
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_LATENCY_TIMER, LATENCY);
-	pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_SEC_LATENCY_TIMER, LATENCY);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_CACHE_LINE_SIZE, CACHE);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_LATENCY_TIMER, LATENCY);
+	pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_SEC_LATENCY_TIMER, LATENCY);
 
-	debug ("func->busno is %x\n", func->busno);
-	debug ("sec_number after writing is %x\n", sec_number);
+	debug("func->busno is %x\n", func->busno);
+	debug("sec_number after writing is %x\n", sec_number);
 
 
 	/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
@@ -652,29 +652,29 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 
 	/* First we need to allocate mem/io for the bridge itself in case it needs it */
 	for (count = 0; address[count]; count++) {	/* for 2 BARs */
-		pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
-		pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
+		pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
+		pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
 
 		if (!bar[count]) {
 			/* This BAR is not implemented */
-			debug ("so we come here then, eh?, count = %d\n", count);
+			debug("so we come here then, eh?, count = %d\n", count);
 			continue;
 		}
 		//  tmp_bar = bar[count];
 
-		debug ("Bar %d wants %x\n", count, bar[count]);
+		debug("Bar %d wants %x\n", count, bar[count]);
 
 		if (bar[count] & PCI_BASE_ADDRESS_SPACE_IO) {
 			/* This is IO */
 			len[count] = bar[count] & 0xFFFFFFFC;
 			len[count] = ~len[count] + 1;
 
-			debug ("len[count] in IO = %x\n", len[count]);
+			debug("len[count] in IO = %x\n", len[count]);
 
 			bus_io[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 
 			if (!bus_io[count]) {
-				err ("out of system memory\n");
+				err("out of system memory\n");
 				retval = -ENOMEM;
 				goto error;
 			}
@@ -683,17 +683,17 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 			bus_io[count]->devfunc = PCI_DEVFN(func->device,
 							func->function);
 			bus_io[count]->len = len[count];
-			if (ibmphp_check_resource (bus_io[count], 0) == 0) {
-				ibmphp_add_resource (bus_io[count]);
+			if (ibmphp_check_resource(bus_io[count], 0) == 0) {
+				ibmphp_add_resource(bus_io[count]);
 				func->io[count] = bus_io[count];
 			} else {
-				err ("cannot allocate requested io for bus %x, device %x, len %x\n",
+				err("cannot allocate requested io for bus %x, device %x, len %x\n",
 				     func->busno, func->device, len[count]);
-				kfree (bus_io[count]);
+				kfree(bus_io[count]);
 				return -EIO;
 			}
 
-			pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->io[count]->start);
+			pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->io[count]->start);
 
 		} else {
 			/* This is Memory */
@@ -702,11 +702,11 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 				len[count] = bar[count] & 0xFFFFFFF0;
 				len[count] = ~len[count] + 1;
 
-				debug ("len[count] in PFMEM = %x\n", len[count]);
+				debug("len[count] in PFMEM = %x\n", len[count]);
 
 				bus_pfmem[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 				if (!bus_pfmem[count]) {
-					err ("out of system memory\n");
+					err("out of system memory\n");
 					retval = -ENOMEM;
 					goto error;
 				}
@@ -716,13 +716,13 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 							func->function);
 				bus_pfmem[count]->len = len[count];
 				bus_pfmem[count]->fromMem = 0;
-				if (ibmphp_check_resource (bus_pfmem[count], 0) == 0) {
-					ibmphp_add_resource (bus_pfmem[count]);
+				if (ibmphp_check_resource(bus_pfmem[count], 0) == 0) {
+					ibmphp_add_resource(bus_pfmem[count]);
 					func->pfmem[count] = bus_pfmem[count];
 				} else {
 					mem_tmp = kzalloc(sizeof(*mem_tmp), GFP_KERNEL);
 					if (!mem_tmp) {
-						err ("out of system memory\n");
+						err("out of system memory\n");
 						retval = -ENOMEM;
 						goto error;
 					}
@@ -730,28 +730,28 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 					mem_tmp->busno = bus_pfmem[count]->busno;
 					mem_tmp->devfunc = bus_pfmem[count]->devfunc;
 					mem_tmp->len = bus_pfmem[count]->len;
-					if (ibmphp_check_resource (mem_tmp, 0) == 0) {
-						ibmphp_add_resource (mem_tmp);
+					if (ibmphp_check_resource(mem_tmp, 0) == 0) {
+						ibmphp_add_resource(mem_tmp);
 						bus_pfmem[count]->fromMem = 1;
 						bus_pfmem[count]->rangeno = mem_tmp->rangeno;
-						ibmphp_add_pfmem_from_mem (bus_pfmem[count]);
+						ibmphp_add_pfmem_from_mem(bus_pfmem[count]);
 						func->pfmem[count] = bus_pfmem[count];
 					} else {
-						err ("cannot allocate requested pfmem for bus %x, device %x, len %x\n",
+						err("cannot allocate requested pfmem for bus %x, device %x, len %x\n",
 						     func->busno, func->device, len[count]);
-						kfree (mem_tmp);
-						kfree (bus_pfmem[count]);
+						kfree(mem_tmp);
+						kfree(bus_pfmem[count]);
 						return -EIO;
 					}
 				}
 
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->pfmem[count]->start);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->pfmem[count]->start);
 
 				if (bar[count] & PCI_BASE_ADDRESS_MEM_TYPE_64) {
 					/* takes up another dword */
 					count += 1;
 					/* on the 2nd dword, write all 0s, since we can't handle them n.e.ways */
-					pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0x00000000);
+					pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0x00000000);
 
 				}
 			} else {
@@ -759,11 +759,11 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 				len[count] = bar[count] & 0xFFFFFFF0;
 				len[count] = ~len[count] + 1;
 
-				debug ("len[count] in Memory is %x\n", len[count]);
+				debug("len[count] in Memory is %x\n", len[count]);
 
 				bus_mem[count] = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 				if (!bus_mem[count]) {
-					err ("out of system memory\n");
+					err("out of system memory\n");
 					retval = -ENOMEM;
 					goto error;
 				}
@@ -772,23 +772,23 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 				bus_mem[count]->devfunc = PCI_DEVFN(func->device,
 							func->function);
 				bus_mem[count]->len = len[count];
-				if (ibmphp_check_resource (bus_mem[count], 0) == 0) {
-					ibmphp_add_resource (bus_mem[count]);
+				if (ibmphp_check_resource(bus_mem[count], 0) == 0) {
+					ibmphp_add_resource(bus_mem[count]);
 					func->mem[count] = bus_mem[count];
 				} else {
-					err ("cannot allocate requested mem for bus %x, device %x, len %x\n",
+					err("cannot allocate requested mem for bus %x, device %x, len %x\n",
 					     func->busno, func->device, len[count]);
-					kfree (bus_mem[count]);
+					kfree(bus_mem[count]);
 					return -EIO;
 				}
 
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], func->mem[count]->start);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], func->mem[count]->start);
 
 				if (bar[count] & PCI_BASE_ADDRESS_MEM_TYPE_64) {
 					/* takes up another dword */
 					count += 1;
 					/* on the 2nd dword, write all 0s, since we can't handle them n.e.ways */
-					pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0x00000000);
+					pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0x00000000);
 
 				}
 			}
@@ -796,45 +796,45 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 	}			/* end of for  */
 
 	/* Now need to see how much space the devices behind the bridge needed */
-	amount_needed = scan_behind_bridge (func, sec_number);
+	amount_needed = scan_behind_bridge(func, sec_number);
 	if (amount_needed == NULL)
 		return -ENOMEM;
 
 	ibmphp_pci_bus->number = func->busno;
-	debug ("after coming back from scan_behind_bridge\n");
-	debug ("amount_needed->not_correct = %x\n", amount_needed->not_correct);
-	debug ("amount_needed->io = %x\n", amount_needed->io);
-	debug ("amount_needed->mem = %x\n", amount_needed->mem);
-	debug ("amount_needed->pfmem =  %x\n", amount_needed->pfmem);
+	debug("after coming back from scan_behind_bridge\n");
+	debug("amount_needed->not_correct = %x\n", amount_needed->not_correct);
+	debug("amount_needed->io = %x\n", amount_needed->io);
+	debug("amount_needed->mem = %x\n", amount_needed->mem);
+	debug("amount_needed->pfmem =  %x\n", amount_needed->pfmem);
 
 	if (amount_needed->not_correct) {
-		debug ("amount_needed is not correct\n");
+		debug("amount_needed is not correct\n");
 		for (count = 0; address[count]; count++) {
 			/* for 2 BARs */
 			if (bus_io[count]) {
-				ibmphp_remove_resource (bus_io[count]);
+				ibmphp_remove_resource(bus_io[count]);
 				func->io[count] = NULL;
 			} else if (bus_pfmem[count]) {
-				ibmphp_remove_resource (bus_pfmem[count]);
+				ibmphp_remove_resource(bus_pfmem[count]);
 				func->pfmem[count] = NULL;
 			} else if (bus_mem[count]) {
-				ibmphp_remove_resource (bus_mem[count]);
+				ibmphp_remove_resource(bus_mem[count]);
 				func->mem[count] = NULL;
 			}
 		}
-		kfree (amount_needed);
+		kfree(amount_needed);
 		return -ENODEV;
 	}
 
 	if (!amount_needed->io) {
-		debug ("it doesn't want IO?\n");
+		debug("it doesn't want IO?\n");
 		flag_io = 1;
 	} else {
-		debug ("it wants %x IO behind the bridge\n", amount_needed->io);
+		debug("it wants %x IO behind the bridge\n", amount_needed->io);
 		io = kzalloc(sizeof(*io), GFP_KERNEL);
 
 		if (!io) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			retval = -ENOMEM;
 			goto error;
 		}
@@ -842,21 +842,21 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 		io->busno = func->busno;
 		io->devfunc = PCI_DEVFN(func->device, func->function);
 		io->len = amount_needed->io;
-		if (ibmphp_check_resource (io, 1) == 0) {
-			debug ("were we able to add io\n");
-			ibmphp_add_resource (io);
+		if (ibmphp_check_resource(io, 1) == 0) {
+			debug("were we able to add io\n");
+			ibmphp_add_resource(io);
 			flag_io = 1;
 		}
 	}
 
 	if (!amount_needed->mem) {
-		debug ("it doesn't want n.e.memory?\n");
+		debug("it doesn't want n.e.memory?\n");
 		flag_mem = 1;
 	} else {
-		debug ("it wants %x memory behind the bridge\n", amount_needed->mem);
+		debug("it wants %x memory behind the bridge\n", amount_needed->mem);
 		mem = kzalloc(sizeof(*mem), GFP_KERNEL);
 		if (!mem) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			retval = -ENOMEM;
 			goto error;
 		}
@@ -864,21 +864,21 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 		mem->busno = func->busno;
 		mem->devfunc = PCI_DEVFN(func->device, func->function);
 		mem->len = amount_needed->mem;
-		if (ibmphp_check_resource (mem, 1) == 0) {
-			ibmphp_add_resource (mem);
+		if (ibmphp_check_resource(mem, 1) == 0) {
+			ibmphp_add_resource(mem);
 			flag_mem = 1;
-			debug ("were we able to add mem\n");
+			debug("were we able to add mem\n");
 		}
 	}
 
 	if (!amount_needed->pfmem) {
-		debug ("it doesn't want n.e.pfmem mem?\n");
+		debug("it doesn't want n.e.pfmem mem?\n");
 		flag_pfmem = 1;
 	} else {
-		debug ("it wants %x pfmemory behind the bridge\n", amount_needed->pfmem);
+		debug("it wants %x pfmemory behind the bridge\n", amount_needed->pfmem);
 		pfmem = kzalloc(sizeof(*pfmem), GFP_KERNEL);
 		if (!pfmem) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			retval = -ENOMEM;
 			goto error;
 		}
@@ -887,13 +887,13 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 		pfmem->devfunc = PCI_DEVFN(func->device, func->function);
 		pfmem->len = amount_needed->pfmem;
 		pfmem->fromMem = 0;
-		if (ibmphp_check_resource (pfmem, 1) == 0) {
-			ibmphp_add_resource (pfmem);
+		if (ibmphp_check_resource(pfmem, 1) == 0) {
+			ibmphp_add_resource(pfmem);
 			flag_pfmem = 1;
 		} else {
 			mem_tmp = kzalloc(sizeof(*mem_tmp), GFP_KERNEL);
 			if (!mem_tmp) {
-				err ("out of system memory\n");
+				err("out of system memory\n");
 				retval = -ENOMEM;
 				goto error;
 			}
@@ -901,18 +901,18 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 			mem_tmp->busno = pfmem->busno;
 			mem_tmp->devfunc = pfmem->devfunc;
 			mem_tmp->len = pfmem->len;
-			if (ibmphp_check_resource (mem_tmp, 1) == 0) {
-				ibmphp_add_resource (mem_tmp);
+			if (ibmphp_check_resource(mem_tmp, 1) == 0) {
+				ibmphp_add_resource(mem_tmp);
 				pfmem->fromMem = 1;
 				pfmem->rangeno = mem_tmp->rangeno;
-				ibmphp_add_pfmem_from_mem (pfmem);
+				ibmphp_add_pfmem_from_mem(pfmem);
 				flag_pfmem = 1;
 			}
 		}
 	}
 
-	debug ("b4 if (flag_io && flag_mem && flag_pfmem)\n");
-	debug ("flag_io = %x, flag_mem = %x, flag_pfmem = %x\n", flag_io, flag_mem, flag_pfmem);
+	debug("b4 if (flag_io && flag_mem && flag_pfmem)\n");
+	debug("flag_io = %x, flag_mem = %x, flag_pfmem = %x\n", flag_io, flag_mem, flag_pfmem);
 
 	if (flag_io && flag_mem && flag_pfmem) {
 		/* If on bootup, there was a bridged card in this slot,
@@ -920,127 +920,127 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 		 * back again, there's no way for us to remove the bus
 		 * struct, so no need to kmalloc, can use existing node
 		 */
-		bus = ibmphp_find_res_bus (sec_number);
+		bus = ibmphp_find_res_bus(sec_number);
 		if (!bus) {
 			bus = kzalloc(sizeof(*bus), GFP_KERNEL);
 			if (!bus) {
-				err ("out of system memory\n");
+				err("out of system memory\n");
 				retval = -ENOMEM;
 				goto error;
 			}
 			bus->busno = sec_number;
-			debug ("b4 adding new bus\n");
-			rc = add_new_bus (bus, io, mem, pfmem, func->busno);
+			debug("b4 adding new bus\n");
+			rc = add_new_bus(bus, io, mem, pfmem, func->busno);
 		} else if (!(bus->rangeIO) && !(bus->rangeMem) && !(bus->rangePFMem))
-			rc = add_new_bus (bus, io, mem, pfmem, 0xFF);
+			rc = add_new_bus(bus, io, mem, pfmem, 0xFF);
 		else {
-			err ("expected bus structure not empty?\n");
+			err("expected bus structure not empty?\n");
 			retval = -EIO;
 			goto error;
 		}
 		if (rc) {
 			if (rc == -ENOMEM) {
-				ibmphp_remove_bus (bus, func->busno);
-				kfree (amount_needed);
+				ibmphp_remove_bus(bus, func->busno);
+				kfree(amount_needed);
 				return rc;
 			}
 			retval = rc;
 			goto error;
 		}
-		pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_IO_BASE, &io_base);
-		pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &pfmem_base);
+		pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_IO_BASE, &io_base);
+		pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &pfmem_base);
 
 		if ((io_base & PCI_IO_RANGE_TYPE_MASK) == PCI_IO_RANGE_TYPE_32) {
-			debug ("io 32\n");
+			debug("io 32\n");
 			need_io_upper = 1;
 		}
 		if ((pfmem_base & PCI_PREF_RANGE_TYPE_MASK) == PCI_PREF_RANGE_TYPE_64) {
-			debug ("pfmem 64\n");
+			debug("pfmem 64\n");
 			need_pfmem_upper = 1;
 		}
 
 		if (bus->noIORanges) {
-			pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_IO_BASE, 0x00 | bus->rangeIO->start >> 8);
-			pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_IO_LIMIT, 0x00 | bus->rangeIO->end >> 8);
+			pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_IO_BASE, 0x00 | bus->rangeIO->start >> 8);
+			pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_IO_LIMIT, 0x00 | bus->rangeIO->end >> 8);
 
 			/* _______________This is for debugging purposes only ____________________
-			pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_IO_BASE, &temp);
-			debug ("io_base = %x\n", (temp & PCI_IO_RANGE_TYPE_MASK) << 8);
-			pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_IO_LIMIT, &temp);
-			debug ("io_limit = %x\n", (temp & PCI_IO_RANGE_TYPE_MASK) << 8);
+			pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_IO_BASE, &temp);
+			debug("io_base = %x\n", (temp & PCI_IO_RANGE_TYPE_MASK) << 8);
+			pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_IO_LIMIT, &temp);
+			debug("io_limit = %x\n", (temp & PCI_IO_RANGE_TYPE_MASK) << 8);
 			 ________________________________________________________________________*/
 
 			if (need_io_upper) {	/* since can't support n.e.ways */
-				pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_IO_BASE_UPPER16, 0x0000);
-				pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_IO_LIMIT_UPPER16, 0x0000);
+				pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_IO_BASE_UPPER16, 0x0000);
+				pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_IO_LIMIT_UPPER16, 0x0000);
 			}
 		} else {
-			pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_IO_BASE, 0x00);
-			pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_IO_LIMIT, 0x00);
+			pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_IO_BASE, 0x00);
+			pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_IO_LIMIT, 0x00);
 		}
 
 		if (bus->noMemRanges) {
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, 0x0000 | bus->rangeMem->start >> 16);
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, 0x0000 | bus->rangeMem->end >> 16);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, 0x0000 | bus->rangeMem->start >> 16);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, 0x0000 | bus->rangeMem->end >> 16);
 
 			/* ____________________This is for debugging purposes only ________________________
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, &temp);
-			debug ("mem_base = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, &temp);
-			debug ("mem_limit = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, &temp);
+			debug("mem_base = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, &temp);
+			debug("mem_limit = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
 			 __________________________________________________________________________________*/
 
 		} else {
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, 0xffff);
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, 0x0000);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, 0xffff);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, 0x0000);
 		}
 		if (bus->noPFMemRanges) {
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, 0x0000 | bus->rangePFMem->start >> 16);
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, 0x0000 | bus->rangePFMem->end >> 16);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, 0x0000 | bus->rangePFMem->start >> 16);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, 0x0000 | bus->rangePFMem->end >> 16);
 
 			/* __________________________This is for debugging purposes only _______________________
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &temp);
-			debug ("pfmem_base = %x", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, &temp);
-			debug ("pfmem_limit = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &temp);
+			debug("pfmem_base = %x", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, &temp);
+			debug("pfmem_limit = %x\n", (temp & PCI_MEMORY_RANGE_TYPE_MASK) << 16);
 			 ______________________________________________________________________________________*/
 
 			if (need_pfmem_upper) {	/* since can't support n.e.ways */
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, PCI_PREF_BASE_UPPER32, 0x00000000);
-				pci_bus_write_config_dword (ibmphp_pci_bus, devfn, PCI_PREF_LIMIT_UPPER32, 0x00000000);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, PCI_PREF_BASE_UPPER32, 0x00000000);
+				pci_bus_write_config_dword(ibmphp_pci_bus, devfn, PCI_PREF_LIMIT_UPPER32, 0x00000000);
 			}
 		} else {
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, 0xffff);
-			pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, 0x0000);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, 0xffff);
+			pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, 0x0000);
 		}
 
-		debug ("b4 writing control information\n");
+		debug("b4 writing control information\n");
 
-		pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_INTERRUPT_PIN, &irq);
+		pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_INTERRUPT_PIN, &irq);
 		if ((irq > 0x00) && (irq < 0x05))
-			pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_INTERRUPT_LINE, func->irq[irq - 1]);
+			pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_INTERRUPT_LINE, func->irq[irq - 1]);
 		/*
-		pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, ctrl);
-		pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, PCI_BRIDGE_CTL_PARITY);
-		pci_bus_write_config_byte (ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, PCI_BRIDGE_CTL_SERR);
+		pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, ctrl);
+		pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, PCI_BRIDGE_CTL_PARITY);
+		pci_bus_write_config_byte(ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, PCI_BRIDGE_CTL_SERR);
 		 */
 
-		pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_COMMAND, DEVICEENABLE);
-		pci_bus_write_config_word (ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, 0x07);
+		pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_COMMAND, DEVICEENABLE);
+		pci_bus_write_config_word(ibmphp_pci_bus, devfn, PCI_BRIDGE_CONTROL, 0x07);
 		for (i = 0; i < 32; i++) {
 			if (amount_needed->devices[i]) {
-				debug ("device where devices[i] is 1 = %x\n", i);
+				debug("device where devices[i] is 1 = %x\n", i);
 				func->devices[i] = 1;
 			}
 		}
 		func->bus = 1;	/* For unconfiguring, to indicate it's PPB */
 		func_passed = &func;
-		debug ("func->busno b4 returning is %x\n", func->busno);
-		debug ("func->busno b4 returning in the other structure is %x\n", (*func_passed)->busno);
-		kfree (amount_needed);
+		debug("func->busno b4 returning is %x\n", func->busno);
+		debug("func->busno b4 returning in the other structure is %x\n", (*func_passed)->busno);
+		kfree(amount_needed);
 		return 0;
 	} else {
-		err ("Configuring bridge was unsuccessful...\n");
+		err("Configuring bridge was unsuccessful...\n");
 		mem_tmp = NULL;
 		retval = -EIO;
 		goto error;
@@ -1049,20 +1049,20 @@ static int configure_bridge (struct pci_func **func_passed, u8 slotno)
 error:
 	kfree(amount_needed);
 	if (pfmem)
-		ibmphp_remove_resource (pfmem);
+		ibmphp_remove_resource(pfmem);
 	if (io)
-		ibmphp_remove_resource (io);
+		ibmphp_remove_resource(io);
 	if (mem)
-		ibmphp_remove_resource (mem);
+		ibmphp_remove_resource(mem);
 	for (i = 0; i < 2; i++) {	/* for 2 BARs */
 		if (bus_io[i]) {
-			ibmphp_remove_resource (bus_io[i]);
+			ibmphp_remove_resource(bus_io[i]);
 			func->io[i] = NULL;
 		} else if (bus_pfmem[i]) {
-			ibmphp_remove_resource (bus_pfmem[i]);
+			ibmphp_remove_resource(bus_pfmem[i]);
 			func->pfmem[i] = NULL;
 		} else if (bus_mem[i]) {
-			ibmphp_remove_resource (bus_mem[i]);
+			ibmphp_remove_resource(bus_mem[i]);
 			func->mem[i] = NULL;
 		}
 	}
@@ -1075,7 +1075,7 @@ error:
  * Input: bridge function
  * Output: amount of resources needed
  *****************************************************************************/
-static struct res_needed *scan_behind_bridge (struct pci_func *func, u8 busno)
+static struct res_needed *scan_behind_bridge(struct pci_func *func, u8 busno)
 {
 	int count, len[6];
 	u16 vendor_id;
@@ -1102,36 +1102,36 @@ static struct res_needed *scan_behind_bridge (struct pci_func *func, u8 busno)
 
 	ibmphp_pci_bus->number = busno;
 
-	debug ("the bus_no behind the bridge is %x\n", busno);
-	debug ("scanning devices behind the bridge...\n");
+	debug("the bus_no behind the bridge is %x\n", busno);
+	debug("scanning devices behind the bridge...\n");
 	for (device = 0; device < 32; device++) {
 		amount->devices[device] = 0;
 		for (function = 0; function < 8; function++) {
 			devfn = PCI_DEVFN(device, function);
 
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
 
 			if (vendor_id != PCI_VENDOR_ID_NOTVALID) {
 				/* found correct device!!! */
 				howmany++;
 
-				pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
-				pci_bus_read_config_dword (ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
+				pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
+				pci_bus_read_config_dword(ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
 
-				debug ("hdr_type behind the bridge is %x\n", hdr_type);
-				if (hdr_type & PCI_HEADER_TYPE_BRIDGE) {
-					err ("embedded bridges not supported for hot-plugging.\n");
+				debug("hdr_type behind the bridge is %x\n", hdr_type);
+				if ((hdr_type & 0x7f) == PCI_HEADER_TYPE_BRIDGE) {
+					err("embedded bridges not supported for hot-plugging.\n");
 					amount->not_correct = 1;
 					return amount;
 				}
 
 				class >>= 8;	/* to take revision out, class = class.subclass.prog i/f */
 				if (class == PCI_CLASS_NOT_DEFINED_VGA) {
-					err ("The device %x is VGA compatible and as is not supported for hot plugging.  Please choose another device.\n", device);
+					err("The device %x is VGA compatible and as is not supported for hot plugging.  Please choose another device.\n", device);
 					amount->not_correct = 1;
 					return amount;
 				} else if (class == PCI_CLASS_DISPLAY_VGA) {
-					err ("The device %x is not supported for hot plugging.  Please choose another device.\n", device);
+					err("The device %x is not supported for hot plugging.  Please choose another device.\n", device);
 					amount->not_correct = 1;
 					return amount;
 				}
@@ -1141,23 +1141,23 @@ static struct res_needed *scan_behind_bridge (struct pci_func *func, u8 busno)
 				for (count = 0; address[count]; count++) {
 					/* for 6 BARs */
 					/*
-					pci_bus_read_config_byte (ibmphp_pci_bus, devfn, address[count], &tmp);
+					pci_bus_read_config_byte(ibmphp_pci_bus, devfn, address[count], &tmp);
 					if (tmp & 0x01) // IO
-						pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFD);
+						pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFD);
 					else // MEMORY
-						pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
+						pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
 					*/
-					pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
-					pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &bar[count]);
+					pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
+					pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &bar[count]);
 
-					debug ("what is bar[count]? %x, count = %d\n", bar[count], count);
+					debug("what is bar[count]? %x, count = %d\n", bar[count], count);
 
 					if (!bar[count])	/* This BAR is not implemented */
 						continue;
 
 					//tmp_bar = bar[count];
 
-					debug ("count %d device %x function %x wants %x resources\n", count, device, function, bar[count]);
+					debug("count %d device %x function %x wants %x resources\n", count, device, function, bar[count]);
 
 					if (bar[count] & PCI_BASE_ADDRESS_SPACE_IO) {
 						/* This is IO */
@@ -1211,7 +1211,7 @@ static struct res_needed *scan_behind_bridge (struct pci_func *func, u8 busno)
  * Change: we also call these functions even if we configured the card ourselves (i.e., not
  * the bootup case), since it should work same way
  */
-static int unconfigure_boot_device (u8 busno, u8 device, u8 function)
+static int unconfigure_boot_device(u8 busno, u8 device, u8 function)
 {
 	u32 start_address;
 	u32 address[] = {
@@ -1234,30 +1234,30 @@ static int unconfigure_boot_device (u8 busno, u8 device, u8 function)
 	u32 tmp_address;
 	unsigned int devfn;
 
-	debug ("%s - enter\n", __func__);
+	debug("%s - enter\n", __func__);
 
-	bus = ibmphp_find_res_bus (busno);
+	bus = ibmphp_find_res_bus(busno);
 	if (!bus) {
-		debug ("cannot find corresponding bus.\n");
+		debug("cannot find corresponding bus.\n");
 		return -EINVAL;
 	}
 
 	devfn = PCI_DEVFN(device, function);
 	ibmphp_pci_bus->number = busno;
 	for (count = 0; address[count]; count++) {	/* for 6 BARs */
-		pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &start_address);
+		pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &start_address);
 
 		/* We can do this here, b/c by that time the device driver of the card has been stopped */
 
-		pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
-		pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &size);
-		pci_bus_write_config_dword (ibmphp_pci_bus, devfn, address[count], start_address);
+		pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], 0xFFFFFFFF);
+		pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &size);
+		pci_bus_write_config_dword(ibmphp_pci_bus, devfn, address[count], start_address);
 
-		debug ("start_address is %x\n", start_address);
-		debug ("busno, device, function %x %x %x\n", busno, device, function);
+		debug("start_address is %x\n", start_address);
+		debug("busno, device, function %x %x %x\n", busno, device, function);
 		if (!size) {
 			/* This BAR is not implemented */
-			debug ("is this bar no implemented?, count = %d\n", count);
+			debug("is this bar no implemented?, count = %d\n", count);
 			continue;
 		}
 		tmp_address = start_address;
@@ -1267,24 +1267,24 @@ static int unconfigure_boot_device (u8 busno, u8 device, u8 function)
 			size = size & 0xFFFFFFFC;
 			size = ~size + 1;
 			end_address = start_address + size - 1;
-			if (ibmphp_find_resource (bus, start_address, &io, IO) < 0) {
-				err ("cannot find corresponding IO resource to remove\n");
+			if (ibmphp_find_resource(bus, start_address, &io, IO) < 0) {
+				err("cannot find corresponding IO resource to remove\n");
 				return -EIO;
 			}
-			debug ("io->start = %x\n", io->start);
+			debug("io->start = %x\n", io->start);
 			temp_end = io->end;
 			start_address = io->end + 1;
-			ibmphp_remove_resource (io);
+			ibmphp_remove_resource(io);
 			/* This is needed b/c of the old I/O restrictions in the BIOS */
 			while (temp_end < end_address) {
-				if (ibmphp_find_resource (bus, start_address, &io, IO) < 0) {
-					err ("cannot find corresponding IO resource to remove\n");
+				if (ibmphp_find_resource(bus, start_address, &io, IO) < 0) {
+					err("cannot find corresponding IO resource to remove\n");
 					return -EIO;
 				}
-				debug ("io->start = %x\n", io->start);
+				debug("io->start = %x\n", io->start);
 				temp_end = io->end;
 				start_address = io->end + 1;
-				ibmphp_remove_resource (io);
+				ibmphp_remove_resource(io);
 			}
 
 			/* ????????? DO WE NEED TO WRITE ANYTHING INTO THE PCI CONFIG SPACE BACK ?????????? */
@@ -1292,29 +1292,29 @@ static int unconfigure_boot_device (u8 busno, u8 device, u8 function)
 			/* This is Memory */
 			if (start_address & PCI_BASE_ADDRESS_MEM_PREFETCH) {
 				/* pfmem */
-				debug ("start address of pfmem is %x\n", start_address);
+				debug("start address of pfmem is %x\n", start_address);
 				start_address &= PCI_BASE_ADDRESS_MEM_MASK;
 
-				if (ibmphp_find_resource (bus, start_address, &pfmem, PFMEM) < 0) {
-					err ("cannot find corresponding PFMEM resource to remove\n");
+				if (ibmphp_find_resource(bus, start_address, &pfmem, PFMEM) < 0) {
+					err("cannot find corresponding PFMEM resource to remove\n");
 					return -EIO;
 				}
 				if (pfmem) {
-					debug ("pfmem->start = %x\n", pfmem->start);
+					debug("pfmem->start = %x\n", pfmem->start);
 
 					ibmphp_remove_resource(pfmem);
 				}
 			} else {
 				/* regular memory */
-				debug ("start address of mem is %x\n", start_address);
+				debug("start address of mem is %x\n", start_address);
 				start_address &= PCI_BASE_ADDRESS_MEM_MASK;
 
-				if (ibmphp_find_resource (bus, start_address, &mem, MEM) < 0) {
-					err ("cannot find corresponding MEM resource to remove\n");
+				if (ibmphp_find_resource(bus, start_address, &mem, MEM) < 0) {
+					err("cannot find corresponding MEM resource to remove\n");
 					return -EIO;
 				}
 				if (mem) {
-					debug ("mem->start = %x\n", mem->start);
+					debug("mem->start = %x\n", mem->start);
 
 					ibmphp_remove_resource(mem);
 				}
@@ -1329,7 +1329,7 @@ static int unconfigure_boot_device (u8 busno, u8 device, u8 function)
 	return 0;
 }
 
-static int unconfigure_boot_bridge (u8 busno, u8 device, u8 function)
+static int unconfigure_boot_bridge(u8 busno, u8 device, u8 function)
 {
 	int count;
 	int bus_no, pri_no, sub_no, sec_no = 0;
@@ -1349,40 +1349,40 @@ static int unconfigure_boot_bridge (u8 busno, u8 device, u8 function)
 	devfn = PCI_DEVFN(device, function);
 	ibmphp_pci_bus->number = busno;
 	bus_no = (int) busno;
-	debug ("busno is %x\n", busno);
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, &pri_number);
-	debug ("%s - busno = %x, primary_number = %x\n", __func__, busno, pri_number);
+	debug("busno is %x\n", busno);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_PRIMARY_BUS, &pri_number);
+	debug("%s - busno = %x, primary_number = %x\n", __func__, busno, pri_number);
 
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
-	debug ("sec_number is %x\n", sec_number);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_number);
+	debug("sec_number is %x\n", sec_number);
 	sec_no = (int) sec_number;
 	pri_no = (int) pri_number;
 	if (pri_no != bus_no) {
-		err ("primary numbers in our structures and pci config space don't match.\n");
+		err("primary numbers in our structures and pci config space don't match.\n");
 		return -EINVAL;
 	}
 
-	pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, &sub_number);
+	pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SUBORDINATE_BUS, &sub_number);
 	sub_no = (int) sub_number;
-	debug ("sub_no is %d, sec_no is %d\n", sub_no, sec_no);
+	debug("sub_no is %d, sec_no is %d\n", sub_no, sec_no);
 	if (sec_no != sub_number) {
-		err ("there're more buses behind this bridge.  Hot removal is not supported.  Please choose another card\n");
+		err("there're more buses behind this bridge.  Hot removal is not supported.  Please choose another card\n");
 		return -ENODEV;
 	}
 
-	bus = ibmphp_find_res_bus (sec_number);
+	bus = ibmphp_find_res_bus(sec_number);
 	if (!bus) {
-		err ("cannot find Bus structure for the bridged device\n");
+		err("cannot find Bus structure for the bridged device\n");
 		return -EINVAL;
 	}
 	debug("bus->busno is %x\n", bus->busno);
 	debug("sec_number is %x\n", sec_number);
 
-	ibmphp_remove_bus (bus, busno);
+	ibmphp_remove_bus(bus, busno);
 
 	for (count = 0; address[count]; count++) {
 		/* for 2 BARs */
-		pci_bus_read_config_dword (ibmphp_pci_bus, devfn, address[count], &start_address);
+		pci_bus_read_config_dword(ibmphp_pci_bus, devfn, address[count], &start_address);
 
 		if (!start_address) {
 			/* This BAR is not implemented */
@@ -1394,14 +1394,14 @@ static int unconfigure_boot_bridge (u8 busno, u8 device, u8 function)
 		if (start_address & PCI_BASE_ADDRESS_SPACE_IO) {
 			/* This is IO */
 			start_address &= PCI_BASE_ADDRESS_IO_MASK;
-			if (ibmphp_find_resource (bus, start_address, &io, IO) < 0) {
-				err ("cannot find corresponding IO resource to remove\n");
+			if (ibmphp_find_resource(bus, start_address, &io, IO) < 0) {
+				err("cannot find corresponding IO resource to remove\n");
 				return -EIO;
 			}
 			if (io)
-				debug ("io->start = %x\n", io->start);
+				debug("io->start = %x\n", io->start);
 
-			ibmphp_remove_resource (io);
+			ibmphp_remove_resource(io);
 
 			/* ????????? DO WE NEED TO WRITE ANYTHING INTO THE PCI CONFIG SPACE BACK ?????????? */
 		} else {
@@ -1409,24 +1409,24 @@ static int unconfigure_boot_bridge (u8 busno, u8 device, u8 function)
 			if (start_address & PCI_BASE_ADDRESS_MEM_PREFETCH) {
 				/* pfmem */
 				start_address &= PCI_BASE_ADDRESS_MEM_MASK;
-				if (ibmphp_find_resource (bus, start_address, &pfmem, PFMEM) < 0) {
-					err ("cannot find corresponding PFMEM resource to remove\n");
+				if (ibmphp_find_resource(bus, start_address, &pfmem, PFMEM) < 0) {
+					err("cannot find corresponding PFMEM resource to remove\n");
 					return -EINVAL;
 				}
 				if (pfmem) {
-					debug ("pfmem->start = %x\n", pfmem->start);
+					debug("pfmem->start = %x\n", pfmem->start);
 
 					ibmphp_remove_resource(pfmem);
 				}
 			} else {
 				/* regular memory */
 				start_address &= PCI_BASE_ADDRESS_MEM_MASK;
-				if (ibmphp_find_resource (bus, start_address, &mem, MEM) < 0) {
-					err ("cannot find corresponding MEM resource to remove\n");
+				if (ibmphp_find_resource(bus, start_address, &mem, MEM) < 0) {
+					err("cannot find corresponding MEM resource to remove\n");
 					return -EINVAL;
 				}
 				if (mem) {
-					debug ("mem->start = %x\n", mem->start);
+					debug("mem->start = %x\n", mem->start);
 
 					ibmphp_remove_resource(mem);
 				}
@@ -1437,11 +1437,11 @@ static int unconfigure_boot_bridge (u8 busno, u8 device, u8 function)
 			}
 		}	/* end of mem */
 	}	/* end of for */
-	debug ("%s - exiting, returning success\n", __func__);
+	debug("%s - exiting, returning success\n", __func__);
 	return 0;
 }
 
-static int unconfigure_boot_card (struct slot *slot_cur)
+static int unconfigure_boot_card(struct slot *slot_cur)
 {
 	u16 vendor_id;
 	u32 class;
@@ -1453,57 +1453,57 @@ static int unconfigure_boot_card (struct slot *slot_cur)
 	unsigned int devfn;
 	u8 valid_device = 0x00; /* To see if we are ever able to find valid device and read it */
 
-	debug ("%s - enter\n", __func__);
+	debug("%s - enter\n", __func__);
 
 	device = slot_cur->device;
 	busno = slot_cur->bus;
 
-	debug ("b4 for loop, device is %x\n", device);
+	debug("b4 for loop, device is %x\n", device);
 	/* For every function on the card */
 	for (function = 0x0; function < 0x08; function++) {
 		devfn = PCI_DEVFN(device, function);
 		ibmphp_pci_bus->number = busno;
 
-		pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
+		pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
 
 		if (vendor_id != PCI_VENDOR_ID_NOTVALID) {
 			/* found correct device!!! */
 			++valid_device;
 
-			debug ("%s - found correct device\n", __func__);
+			debug("%s - found correct device\n", __func__);
 
 			/* header: x x x x x x x x
 			 *         | |___________|=> 1=PPB bridge, 0=normal device, 2=CardBus Bridge
 			 *         |_=> 0 = single function device, 1 = multi-function device
 			 */
 
-			pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
-			pci_bus_read_config_dword (ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
+			pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
+			pci_bus_read_config_dword(ibmphp_pci_bus, devfn, PCI_CLASS_REVISION, &class);
 
-			debug ("hdr_type %x, class %x\n", hdr_type, class);
+			debug("hdr_type %x, class %x\n", hdr_type, class);
 			class >>= 8;	/* to take revision out, class = class.subclass.prog i/f */
 			if (class == PCI_CLASS_NOT_DEFINED_VGA) {
-				err ("The device %x function %x is VGA compatible and is not supported for hot removing.  Please choose another device.\n", device, function);
+				err("The device %x function %x is VGA compatible and is not supported for hot removing.  Please choose another device.\n", device, function);
 				return -ENODEV;
 			} else if (class == PCI_CLASS_DISPLAY_VGA) {
-				err ("The device %x function %x is not supported for hot removing.  Please choose another device.\n", device, function);
+				err("The device %x function %x is not supported for hot removing.  Please choose another device.\n", device, function);
 				return -ENODEV;
 			}
 
 			switch (hdr_type) {
 				case PCI_HEADER_TYPE_NORMAL:
-					rc = unconfigure_boot_device (busno, device, function);
+					rc = unconfigure_boot_device(busno, device, function);
 					if (rc) {
-						err ("was not able to unconfigure device %x func %x on bus %x. bailing out...\n",
+						err("was not able to unconfigure device %x func %x on bus %x. bailing out...\n",
 						     device, function, busno);
 						return rc;
 					}
 					function = 0x8;
 					break;
 				case PCI_HEADER_TYPE_MULTIDEVICE:
-					rc = unconfigure_boot_device (busno, device, function);
+					rc = unconfigure_boot_device(busno, device, function);
 					if (rc) {
-						err ("was not able to unconfigure device %x func %x on bus %x. bailing out...\n",
+						err("was not able to unconfigure device %x func %x on bus %x. bailing out...\n",
 						     device, function, busno);
 						return rc;
 					}
@@ -1511,12 +1511,12 @@ static int unconfigure_boot_card (struct slot *slot_cur)
 				case PCI_HEADER_TYPE_BRIDGE:
 					class >>= 8;
 					if (class != PCI_CLASS_BRIDGE_PCI) {
-						err ("This device %x function %x is not PCI-to-PCI bridge, and is not supported for hot-removing.  Please try another card.\n", device, function);
+						err("This device %x function %x is not PCI-to-PCI bridge, and is not supported for hot-removing.  Please try another card.\n", device, function);
 						return -ENODEV;
 					}
-					rc = unconfigure_boot_bridge (busno, device, function);
+					rc = unconfigure_boot_bridge(busno, device, function);
 					if (rc != 0) {
-						err ("was not able to hot-remove PPB properly.\n");
+						err("was not able to hot-remove PPB properly.\n");
 						return rc;
 					}
 
@@ -1525,17 +1525,17 @@ static int unconfigure_boot_card (struct slot *slot_cur)
 				case PCI_HEADER_TYPE_MULTIBRIDGE:
 					class >>= 8;
 					if (class != PCI_CLASS_BRIDGE_PCI) {
-						err ("This device %x function %x is not PCI-to-PCI bridge,  and is not supported for hot-removing.  Please try another card.\n", device, function);
+						err("This device %x function %x is not PCI-to-PCI bridge,  and is not supported for hot-removing.  Please try another card.\n", device, function);
 						return -ENODEV;
 					}
-					rc = unconfigure_boot_bridge (busno, device, function);
+					rc = unconfigure_boot_bridge(busno, device, function);
 					if (rc != 0) {
-						err ("was not able to hot-remove PPB properly.\n");
+						err("was not able to hot-remove PPB properly.\n");
 						return rc;
 					}
 					break;
 				default:
-					err ("MAJOR PROBLEM!!!! Cannot read device's header\n");
+					err("MAJOR PROBLEM!!!! Cannot read device's header\n");
 					return -1;
 					break;
 			}	/* end of switch */
@@ -1543,7 +1543,7 @@ static int unconfigure_boot_card (struct slot *slot_cur)
 	}	/* end of for */
 
 	if (!valid_device) {
-		err ("Could not find device to unconfigure.  Or could not read the card.\n");
+		err("Could not find device to unconfigure.  Or could not read the card.\n");
 		return -1;
 	}
 	return 0;
@@ -1558,7 +1558,7 @@ static int unconfigure_boot_card (struct slot *slot_cur)
  *			!!!!!!!!!!!!!!!!!!!!!!!!!FOR BUSES!!!!!!!!!!!!
  * Returns: 0, -1, -ENODEV
  */
-int ibmphp_unconfigure_card (struct slot **slot_cur, int the_end)
+int ibmphp_unconfigure_card(struct slot **slot_cur, int the_end)
 {
 	int i;
 	int count;
@@ -1567,11 +1567,11 @@ int ibmphp_unconfigure_card (struct slot **slot_cur, int the_end)
 	struct pci_func *cur_func = NULL;
 	struct pci_func *temp_func;
 
-	debug ("%s - enter\n", __func__);
+	debug("%s - enter\n", __func__);
 
 	if (!the_end) {
 		/* Need to unconfigure the card */
-		rc = unconfigure_boot_card (sl);
+		rc = unconfigure_boot_card(sl);
 		if ((rc == -ENODEV) || (rc == -EIO) || (rc == -EINVAL)) {
 			/* In all other cases, will still need to get rid of func structure if it exists */
 			return rc;
@@ -1591,34 +1591,34 @@ int ibmphp_unconfigure_card (struct slot **slot_cur, int the_end)
 
 			for (i = 0; i < count; i++) {
 				if (cur_func->io[i]) {
-					debug ("io[%d] exists\n", i);
+					debug("io[%d] exists\n", i);
 					if (the_end > 0)
-						ibmphp_remove_resource (cur_func->io[i]);
+						ibmphp_remove_resource(cur_func->io[i]);
 					cur_func->io[i] = NULL;
 				}
 				if (cur_func->mem[i]) {
-					debug ("mem[%d] exists\n", i);
+					debug("mem[%d] exists\n", i);
 					if (the_end > 0)
-						ibmphp_remove_resource (cur_func->mem[i]);
+						ibmphp_remove_resource(cur_func->mem[i]);
 					cur_func->mem[i] = NULL;
 				}
 				if (cur_func->pfmem[i]) {
-					debug ("pfmem[%d] exists\n", i);
+					debug("pfmem[%d] exists\n", i);
 					if (the_end > 0)
-						ibmphp_remove_resource (cur_func->pfmem[i]);
+						ibmphp_remove_resource(cur_func->pfmem[i]);
 					cur_func->pfmem[i] = NULL;
 				}
 			}
 
 			temp_func = cur_func->next;
-			kfree (cur_func);
+			kfree(cur_func);
 			cur_func = temp_func;
 		}
 	}
 
 	sl->func = NULL;
 	*slot_cur = sl;
-	debug ("%s - exit\n", __func__);
+	debug("%s - exit\n", __func__);
 	return 0;
 }
 
@@ -1630,7 +1630,7 @@ int ibmphp_unconfigure_card (struct slot **slot_cur, int the_end)
  * Output: bus added to the correct spot
  *         0, -1, error
  */
-static int add_new_bus (struct bus_node *bus, struct resource_node *io, struct resource_node *mem, struct resource_node *pfmem, u8 parent_busno)
+static int add_new_bus(struct bus_node *bus, struct resource_node *io, struct resource_node *mem, struct resource_node *pfmem, u8 parent_busno)
 {
 	struct range_node *io_range = NULL;
 	struct range_node *mem_range = NULL;
@@ -1639,18 +1639,18 @@ static int add_new_bus (struct bus_node *bus, struct resource_node *io, struct r
 
 	/* Trying to find the parent bus number */
 	if (parent_busno != 0xFF) {
-		cur_bus	= ibmphp_find_res_bus (parent_busno);
+		cur_bus	= ibmphp_find_res_bus(parent_busno);
 		if (!cur_bus) {
-			err ("strange, cannot find bus which is supposed to be at the system... something is terribly wrong...\n");
+			err("strange, cannot find bus which is supposed to be at the system... something is terribly wrong...\n");
 			return -ENODEV;
 		}
 
-		list_add (&bus->bus_list, &cur_bus->bus_list);
+		list_add(&bus->bus_list, &cur_bus->bus_list);
 	}
 	if (io) {
 		io_range = kzalloc(sizeof(*io_range), GFP_KERNEL);
 		if (!io_range) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			return -ENOMEM;
 		}
 		io_range->start = io->start;
@@ -1662,7 +1662,7 @@ static int add_new_bus (struct bus_node *bus, struct resource_node *io, struct r
 	if (mem) {
 		mem_range = kzalloc(sizeof(*mem_range), GFP_KERNEL);
 		if (!mem_range) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			return -ENOMEM;
 		}
 		mem_range->start = mem->start;
@@ -1674,7 +1674,7 @@ static int add_new_bus (struct bus_node *bus, struct resource_node *io, struct r
 	if (pfmem) {
 		pfmem_range = kzalloc(sizeof(*pfmem_range), GFP_KERNEL);
 		if (!pfmem_range) {
-			err ("out of system memory\n");
+			err("out of system memory\n");
 			return -ENOMEM;
 		}
 		pfmem_range->start = pfmem->start;
@@ -1691,27 +1691,27 @@ static int add_new_bus (struct bus_node *bus, struct resource_node *io, struct r
  * Parameters: bus_number of the primary bus
  * Returns: bus_number of the secondary bus or 0xff in case of failure
  */
-static u8 find_sec_number (u8 primary_busno, u8 slotno)
+static u8 find_sec_number(u8 primary_busno, u8 slotno)
 {
 	int min, max;
 	u8 busno;
 	struct bus_info *bus;
 	struct bus_node *bus_cur;
 
-	bus = ibmphp_find_same_bus_num (primary_busno);
+	bus = ibmphp_find_same_bus_num(primary_busno);
 	if (!bus) {
-		err ("cannot get slot range of the bus from the BIOS\n");
+		err("cannot get slot range of the bus from the BIOS\n");
 		return 0xff;
 	}
 	max = bus->slot_max;
 	min = bus->slot_min;
 	if ((slotno > max) || (slotno < min)) {
-		err ("got the wrong range\n");
+		err("got the wrong range\n");
 		return 0xff;
 	}
 	busno = (u8) (slotno - (u8) min);
 	busno += primary_busno + 0x01;
-	bus_cur = ibmphp_find_res_bus (busno);
+	bus_cur = ibmphp_find_res_bus(busno);
 	/* either there is no such bus number, or there are no ranges, which
 	 * can only happen if we removed the bridged device in previous load
 	 * of the driver, and now only have the skeleton bus struct
diff --git a/drivers/pci/hotplug/ibmphp_res.c b/drivers/pci/hotplug/ibmphp_res.c
index f279060cf6e2..aee6e41001e1 100644
--- a/drivers/pci/hotplug/ibmphp_res.c
+++ b/drivers/pci/hotplug/ibmphp_res.c
@@ -36,28 +36,28 @@
 
 static int flags = 0;		/* for testing */
 
-static void update_resources (struct bus_node *bus_cur, int type, int rangeno);
-static int once_over (void);
-static int remove_ranges (struct bus_node *, struct bus_node *);
-static int update_bridge_ranges (struct bus_node **);
-static int add_bus_range (int type, struct range_node *, struct bus_node *);
-static void fix_resources (struct bus_node *);
-static struct bus_node *find_bus_wprev (u8, struct bus_node **, u8);
+static void update_resources(struct bus_node *bus_cur, int type, int rangeno);
+static int once_over(void);
+static int remove_ranges(struct bus_node *, struct bus_node *);
+static int update_bridge_ranges(struct bus_node **);
+static int add_bus_range(int type, struct range_node *, struct bus_node *);
+static void fix_resources(struct bus_node *);
+static struct bus_node *find_bus_wprev(u8, struct bus_node **, u8);
 
 static LIST_HEAD(gbuses);
 
-static struct bus_node * __init alloc_error_bus (struct ebda_pci_rsrc *curr, u8 busno, int flag)
+static struct bus_node * __init alloc_error_bus(struct ebda_pci_rsrc *curr, u8 busno, int flag)
 {
 	struct bus_node *newbus;
 
 	if (!(curr) && !(flag)) {
-		err ("NULL pointer passed\n");
+		err("NULL pointer passed\n");
 		return NULL;
 	}
 
 	newbus = kzalloc(sizeof(struct bus_node), GFP_KERNEL);
 	if (!newbus) {
-		err ("out of system memory\n");
+		err("out of system memory\n");
 		return NULL;
 	}
 
@@ -65,22 +65,22 @@ static struct bus_node * __init alloc_error_bus (struct ebda_pci_rsrc *curr, u8
 		newbus->busno = busno;
 	else
 		newbus->busno = curr->bus_num;
-	list_add_tail (&newbus->bus_list, &gbuses);
+	list_add_tail(&newbus->bus_list, &gbuses);
 	return newbus;
 }
 
-static struct resource_node * __init alloc_resources (struct ebda_pci_rsrc *curr)
+static struct resource_node * __init alloc_resources(struct ebda_pci_rsrc *curr)
 {
 	struct resource_node *rs;
 
 	if (!curr) {
-		err ("NULL passed to allocate\n");
+		err("NULL passed to allocate\n");
 		return NULL;
 	}
 
 	rs = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 	if (!rs) {
-		err ("out of system memory\n");
+		err("out of system memory\n");
 		return NULL;
 	}
 	rs->busno = curr->bus_num;
@@ -91,7 +91,7 @@ static struct resource_node * __init alloc_resources (struct ebda_pci_rsrc *curr
 	return rs;
 }
 
-static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node **new_range, struct ebda_pci_rsrc *curr, int flag, u8 first_bus)
+static int __init alloc_bus_range(struct bus_node **new_bus, struct range_node **new_range, struct ebda_pci_rsrc *curr, int flag, u8 first_bus)
 {
 	struct bus_node *newbus;
 	struct range_node *newrange;
@@ -100,7 +100,7 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 	if (first_bus) {
 		newbus = kzalloc(sizeof(struct bus_node), GFP_KERNEL);
 		if (!newbus) {
-			err ("out of system memory.\n");
+			err("out of system memory.\n");
 			return -ENOMEM;
 		}
 		newbus->busno = curr->bus_num;
@@ -122,8 +122,8 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 	newrange = kzalloc(sizeof(struct range_node), GFP_KERNEL);
 	if (!newrange) {
 		if (first_bus)
-			kfree (newbus);
-		err ("out of system memory\n");
+			kfree(newbus);
+		err("out of system memory\n");
 		return -ENOMEM;
 	}
 	newrange->start = curr->start_addr;
@@ -133,8 +133,8 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 		newrange->rangeno = 1;
 	else {
 		/* need to insert our range */
-		add_bus_range (flag, newrange, newbus);
-		debug ("%d resource Primary Bus inserted on bus %x [%x - %x]\n", flag, newbus->busno, newrange->start, newrange->end);
+		add_bus_range(flag, newrange, newbus);
+		debug("%d resource Primary Bus inserted on bus %x [%x - %x]\n", flag, newbus->busno, newrange->start, newrange->end);
 	}
 
 	switch (flag) {
@@ -143,9 +143,9 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 			if (first_bus)
 				newbus->noMemRanges = 1;
 			else {
-				debug ("First Memory Primary on bus %x, [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+				debug("First Memory Primary on bus %x, [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				++newbus->noMemRanges;
-				fix_resources (newbus);
+				fix_resources(newbus);
 			}
 			break;
 		case IO:
@@ -153,9 +153,9 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 			if (first_bus)
 				newbus->noIORanges = 1;
 			else {
-				debug ("First IO Primary on bus %x, [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+				debug("First IO Primary on bus %x, [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				++newbus->noIORanges;
-				fix_resources (newbus);
+				fix_resources(newbus);
 			}
 			break;
 		case PFMEM:
@@ -163,9 +163,9 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
 			if (first_bus)
 				newbus->noPFMemRanges = 1;
 			else {
-				debug ("1st PFMemory Primary on Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+				debug("1st PFMemory Primary on Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				++newbus->noPFMemRanges;
-				fix_resources (newbus);
+				fix_resources(newbus);
 			}
 
 			break;
@@ -183,7 +183,7 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
  * 2. If cannot allocate out of PFMem range, allocate from Mem ranges.  PFmemFromMem
  * are not sorted. (no need since use mem node). To not change the entire code, we
  * also add mem node whenever this case happens so as not to change
- * ibmphp_check_mem_resource etc (and since it really is taking Mem resource)
+ * ibmphp_check_mem_resource etc(and since it really is taking Mem resource)
  */
 
 /*****************************************************************************
@@ -196,25 +196,23 @@ static int __init alloc_bus_range (struct bus_node **new_bus, struct range_node
  * Input: ptr to the head of the resource list from EBDA
  * Output: 0, -1 or error codes
  ***************************************************************************/
-int __init ibmphp_rsrc_init (void)
+int __init ibmphp_rsrc_init(void)
 {
 	struct ebda_pci_rsrc *curr;
 	struct range_node *newrange = NULL;
 	struct bus_node *newbus = NULL;
 	struct bus_node *bus_cur;
 	struct bus_node *bus_prev;
-	struct list_head *tmp;
 	struct resource_node *new_io = NULL;
 	struct resource_node *new_mem = NULL;
 	struct resource_node *new_pfmem = NULL;
 	int rc;
-	struct list_head *tmp_ebda;
 
-	list_for_each (tmp_ebda, &ibmphp_ebda_pci_rsrc_head) {
-		curr = list_entry (tmp_ebda, struct ebda_pci_rsrc, ebda_pci_rsrc_list);
+	list_for_each_entry(curr, &ibmphp_ebda_pci_rsrc_head,
+			    ebda_pci_rsrc_list) {
 		if (!(curr->rsrc_type & PCIDEVMASK)) {
 			/* EBDA still lists non PCI devices, so ignore... */
-			debug ("this is not a PCI DEVICE in rsrc_init, please take care\n");
+			debug("this is not a PCI DEVICE in rsrc_init, please take care\n");
 			// continue;
 		}
 
@@ -223,17 +221,17 @@ int __init ibmphp_rsrc_init (void)
 			/* memory */
 			if ((curr->rsrc_type & RESTYPE) == MMASK) {
 				/* no bus structure exists in place yet */
-				if (list_empty (&gbuses)) {
+				if (list_empty(&gbuses)) {
 					rc = alloc_bus_range(&newbus, &newrange, curr, MEM, 1);
 					if (rc)
 						return rc;
-					list_add_tail (&newbus->bus_list, &gbuses);
-					debug ("gbuses = NULL, Memory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+					list_add_tail(&newbus->bus_list, &gbuses);
+					debug("gbuses = NULL, Memory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				} else {
-					bus_cur = find_bus_wprev (curr->bus_num, &bus_prev, 1);
+					bus_cur = find_bus_wprev(curr->bus_num, &bus_prev, 1);
 					/* found our bus */
 					if (bus_cur) {
-						rc = alloc_bus_range (&bus_cur, &newrange, curr, MEM, 0);
+						rc = alloc_bus_range(&bus_cur, &newrange, curr, MEM, 0);
 						if (rc)
 							return rc;
 					} else {
@@ -242,24 +240,24 @@ int __init ibmphp_rsrc_init (void)
 						if (rc)
 							return rc;
 
-						list_add_tail (&newbus->bus_list, &gbuses);
-						debug ("New Bus, Memory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+						list_add_tail(&newbus->bus_list, &gbuses);
+						debug("New Bus, Memory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 					}
 				}
 			} else if ((curr->rsrc_type & RESTYPE) == PFMASK) {
 				/* prefetchable memory */
-				if (list_empty (&gbuses)) {
+				if (list_empty(&gbuses)) {
 					/* no bus structure exists in place yet */
 					rc = alloc_bus_range(&newbus, &newrange, curr, PFMEM, 1);
 					if (rc)
 						return rc;
-					list_add_tail (&newbus->bus_list, &gbuses);
-					debug ("gbuses = NULL, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+					list_add_tail(&newbus->bus_list, &gbuses);
+					debug("gbuses = NULL, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				} else {
-					bus_cur = find_bus_wprev (curr->bus_num, &bus_prev, 1);
+					bus_cur = find_bus_wprev(curr->bus_num, &bus_prev, 1);
 					if (bus_cur) {
 						/* found our bus */
-						rc = alloc_bus_range (&bus_cur, &newrange, curr, PFMEM, 0);
+						rc = alloc_bus_range(&bus_cur, &newrange, curr, PFMEM, 0);
 						if (rc)
 							return rc;
 					} else {
@@ -267,23 +265,23 @@ int __init ibmphp_rsrc_init (void)
 						rc = alloc_bus_range(&newbus, &newrange, curr, PFMEM, 1);
 						if (rc)
 							return rc;
-						list_add_tail (&newbus->bus_list, &gbuses);
-						debug ("1st Bus, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+						list_add_tail(&newbus->bus_list, &gbuses);
+						debug("1st Bus, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 					}
 				}
 			} else if ((curr->rsrc_type & RESTYPE) == IOMASK) {
 				/* IO */
-				if (list_empty (&gbuses)) {
+				if (list_empty(&gbuses)) {
 					/* no bus structure exists in place yet */
 					rc = alloc_bus_range(&newbus, &newrange, curr, IO, 1);
 					if (rc)
 						return rc;
-					list_add_tail (&newbus->bus_list, &gbuses);
-					debug ("gbuses = NULL, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+					list_add_tail(&newbus->bus_list, &gbuses);
+					debug("gbuses = NULL, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 				} else {
-					bus_cur = find_bus_wprev (curr->bus_num, &bus_prev, 1);
+					bus_cur = find_bus_wprev(curr->bus_num, &bus_prev, 1);
 					if (bus_cur) {
-						rc = alloc_bus_range (&bus_cur, &newrange, curr, IO, 0);
+						rc = alloc_bus_range(&bus_cur, &newrange, curr, IO, 0);
 						if (rc)
 							return rc;
 					} else {
@@ -291,8 +289,8 @@ int __init ibmphp_rsrc_init (void)
 						rc = alloc_bus_range(&newbus, &newrange, curr, IO, 1);
 						if (rc)
 							return rc;
-						list_add_tail (&newbus->bus_list, &gbuses);
-						debug ("1st Bus, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
+						list_add_tail(&newbus->bus_list, &gbuses);
+						debug("1st Bus, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
 					}
 				}
 
@@ -304,7 +302,7 @@ int __init ibmphp_rsrc_init (void)
 			/* regular pci device resource */
 			if ((curr->rsrc_type & RESTYPE) == MMASK) {
 				/* Memory resource */
-				new_mem = alloc_resources (curr);
+				new_mem = alloc_resources(curr);
 				if (!new_mem)
 					return -ENOMEM;
 				new_mem->type = MEM;
@@ -315,25 +313,25 @@ int __init ibmphp_rsrc_init (void)
 				 * assign a -1 and then update once the range
 				 * actually appears...
 				 */
-				if (ibmphp_add_resource (new_mem) < 0) {
-					newbus = alloc_error_bus (curr, 0, 0);
+				if (ibmphp_add_resource(new_mem) < 0) {
+					newbus = alloc_error_bus(curr, 0, 0);
 					if (!newbus)
 						return -ENOMEM;
 					newbus->firstMem = new_mem;
 					++newbus->needMemUpdate;
 					new_mem->rangeno = -1;
 				}
-				debug ("Memory resource for device %x, bus %x, [%x - %x]\n", new_mem->devfunc, new_mem->busno, new_mem->start, new_mem->end);
+				debug("Memory resource for device %x, bus %x, [%x - %x]\n", new_mem->devfunc, new_mem->busno, new_mem->start, new_mem->end);
 
 			} else if ((curr->rsrc_type & RESTYPE) == PFMASK) {
 				/* PFMemory resource */
-				new_pfmem = alloc_resources (curr);
+				new_pfmem = alloc_resources(curr);
 				if (!new_pfmem)
 					return -ENOMEM;
 				new_pfmem->type = PFMEM;
 				new_pfmem->fromMem = 0;
-				if (ibmphp_add_resource (new_pfmem) < 0) {
-					newbus = alloc_error_bus (curr, 0, 0);
+				if (ibmphp_add_resource(new_pfmem) < 0) {
+					newbus = alloc_error_bus(curr, 0, 0);
 					if (!newbus)
 						return -ENOMEM;
 					newbus->firstPFMem = new_pfmem;
@@ -341,10 +339,10 @@ int __init ibmphp_rsrc_init (void)
 					new_pfmem->rangeno = -1;
 				}
 
-				debug ("PFMemory resource for device %x, bus %x, [%x - %x]\n", new_pfmem->devfunc, new_pfmem->busno, new_pfmem->start, new_pfmem->end);
+				debug("PFMemory resource for device %x, bus %x, [%x - %x]\n", new_pfmem->devfunc, new_pfmem->busno, new_pfmem->start, new_pfmem->end);
 			} else if ((curr->rsrc_type & RESTYPE) == IOMASK) {
 				/* IO resource */
-				new_io = alloc_resources (curr);
+				new_io = alloc_resources(curr);
 				if (!new_io)
 					return -ENOMEM;
 				new_io->type = IO;
@@ -356,27 +354,26 @@ int __init ibmphp_rsrc_init (void)
 				 * Can assign a -1 and then update once the
 				 * range actually appears...
 				 */
-				if (ibmphp_add_resource (new_io) < 0) {
-					newbus = alloc_error_bus (curr, 0, 0);
+				if (ibmphp_add_resource(new_io) < 0) {
+					newbus = alloc_error_bus(curr, 0, 0);
 					if (!newbus)
 						return -ENOMEM;
 					newbus->firstIO = new_io;
 					++newbus->needIOUpdate;
 					new_io->rangeno = -1;
 				}
-				debug ("IO resource for device %x, bus %x, [%x - %x]\n", new_io->devfunc, new_io->busno, new_io->start, new_io->end);
+				debug("IO resource for device %x, bus %x, [%x - %x]\n", new_io->devfunc, new_io->busno, new_io->start, new_io->end);
 			}
 		}
 	}
 
-	list_for_each (tmp, &gbuses) {
-		bus_cur = list_entry (tmp, struct bus_node, bus_list);
+	list_for_each_entry(bus_cur, &gbuses, bus_list) {
 		/* This is to get info about PPB resources, since EBDA doesn't put this info into the primary bus info */
-		rc = update_bridge_ranges (&bus_cur);
+		rc = update_bridge_ranges(&bus_cur);
 		if (rc)
 			return rc;
 	}
-	return once_over ();	/* This is to align ranges (so no -1) */
+	return once_over();	/* This is to align ranges (so no -1) */
 }
 
 /********************************************************************************
@@ -387,7 +384,7 @@ int __init ibmphp_rsrc_init (void)
  * Input: type of the resource, range to add, current bus
  * Output: 0 or -1, bus and range ptrs
  ********************************************************************************/
-static int add_bus_range (int type, struct range_node *range, struct bus_node *bus_cur)
+static int add_bus_range(int type, struct range_node *range, struct bus_node *bus_cur)
 {
 	struct range_node *range_cur = NULL;
 	struct range_node *range_prev;
@@ -452,7 +449,7 @@ static int add_bus_range (int type, struct range_node *range, struct bus_node *b
 		range_cur = range_cur->next;
 	}
 
-	update_resources (bus_cur, type, i_init + 1);
+	update_resources(bus_cur, type, i_init + 1);
 	return 0;
 }
 
@@ -462,7 +459,7 @@ static int add_bus_range (int type, struct range_node *range, struct bus_node *b
  *
  * Input: bus, type of the resource, the rangeno starting from which to update
  ******************************************************************************/
-static void update_resources (struct bus_node *bus_cur, int type, int rangeno)
+static void update_resources(struct bus_node *bus_cur, int type, int rangeno)
 {
 	struct resource_node *res = NULL;
 	u8 eol = 0;	/* end of list indicator */
@@ -506,9 +503,9 @@ static void update_resources (struct bus_node *bus_cur, int type, int rangeno)
 	}
 }
 
-static void fix_me (struct resource_node *res, struct bus_node *bus_cur, struct range_node *range)
+static void fix_me(struct resource_node *res, struct bus_node *bus_cur, struct range_node *range)
 {
-	char * str = "";
+	char *str = "";
 	switch (res->type) {
 		case IO:
 			str = "io";
@@ -526,7 +523,7 @@ static void fix_me (struct resource_node *res, struct bus_node *bus_cur, struct
 			while (range) {
 				if ((res->start >= range->start) && (res->end <= range->end)) {
 					res->rangeno = range->rangeno;
-					debug ("%s->rangeno in fix_resources is %d\n", str, res->rangeno);
+					debug("%s->rangeno in fix_resources is %d\n", str, res->rangeno);
 					switch (res->type) {
 						case IO:
 							--bus_cur->needIOUpdate;
@@ -561,27 +558,27 @@ static void fix_me (struct resource_node *res, struct bus_node *bus_cur, struct
  * Input: current bus
  * Output: none, list of resources for that bus are fixed if can be
  *******************************************************************************/
-static void fix_resources (struct bus_node *bus_cur)
+static void fix_resources(struct bus_node *bus_cur)
 {
 	struct range_node *range;
 	struct resource_node *res;
 
-	debug ("%s - bus_cur->busno = %d\n", __func__, bus_cur->busno);
+	debug("%s - bus_cur->busno = %d\n", __func__, bus_cur->busno);
 
 	if (bus_cur->needIOUpdate) {
 		res = bus_cur->firstIO;
 		range = bus_cur->rangeIO;
-		fix_me (res, bus_cur, range);
+		fix_me(res, bus_cur, range);
 	}
 	if (bus_cur->needMemUpdate) {
 		res = bus_cur->firstMem;
 		range = bus_cur->rangeMem;
-		fix_me (res, bus_cur, range);
+		fix_me(res, bus_cur, range);
 	}
 	if (bus_cur->needPFMemUpdate) {
 		res = bus_cur->firstPFMem;
 		range = bus_cur->rangePFMem;
-		fix_me (res, bus_cur, range);
+		fix_me(res, bus_cur, range);
 	}
 }
 
@@ -594,7 +591,7 @@ static void fix_resources (struct bus_node *bus_cur)
  * Output: ptrs assigned (to the node)
  * 0 or -1
  *******************************************************************************/
-int ibmphp_add_resource (struct resource_node *res)
+int ibmphp_add_resource(struct resource_node *res)
 {
 	struct resource_node *res_cur;
 	struct resource_node *res_prev;
@@ -602,18 +599,18 @@ int ibmphp_add_resource (struct resource_node *res)
 	struct range_node *range_cur = NULL;
 	struct resource_node *res_start = NULL;
 
-	debug ("%s - enter\n", __func__);
+	debug("%s - enter\n", __func__);
 
 	if (!res) {
-		err ("NULL passed to add\n");
+		err("NULL passed to add\n");
 		return -ENODEV;
 	}
 
-	bus_cur = find_bus_wprev (res->busno, NULL, 0);
+	bus_cur = find_bus_wprev(res->busno, NULL, 0);
 
 	if (!bus_cur) {
 		/* didn't find a bus, something's wrong!!! */
-		debug ("no bus in the system, either pci_dev's wrong or allocation failed\n");
+		debug("no bus in the system, either pci_dev's wrong or allocation failed\n");
 		return -ENODEV;
 	}
 
@@ -632,7 +629,7 @@ int ibmphp_add_resource (struct resource_node *res)
 			res_start = bus_cur->firstPFMem;
 			break;
 		default:
-			err ("cannot read the type of the resource to add... problem\n");
+			err("cannot read the type of the resource to add... problem\n");
 			return -EINVAL;
 	}
 	while (range_cur) {
@@ -663,7 +660,7 @@ int ibmphp_add_resource (struct resource_node *res)
 		res->rangeno = -1;
 	}
 
-	debug ("The range is %d\n", res->rangeno);
+	debug("The range is %d\n", res->rangeno);
 	if (!res_start) {
 		/* no first{IO,Mem,Pfmem} on the bus, 1st IO/Mem/Pfmem resource ever */
 		switch (res->type) {
@@ -683,7 +680,7 @@ int ibmphp_add_resource (struct resource_node *res)
 		res_cur = res_start;
 		res_prev = NULL;
 
-		debug ("res_cur->rangeno is %d\n", res_cur->rangeno);
+		debug("res_cur->rangeno is %d\n", res_cur->rangeno);
 
 		while (res_cur) {
 			if (res_cur->rangeno >= res->rangeno)
@@ -697,7 +694,7 @@ int ibmphp_add_resource (struct resource_node *res)
 
 		if (!res_cur) {
 			/* at the end of the resource list */
-			debug ("i should be here, [%x - %x]\n", res->start, res->end);
+			debug("i should be here, [%x - %x]\n", res->start, res->end);
 			res_prev->nextRange = res;
 			res->next = NULL;
 			res->nextRange = NULL;
@@ -765,7 +762,7 @@ int ibmphp_add_resource (struct resource_node *res)
 		}
 	}
 
-	debug ("%s - exit\n", __func__);
+	debug("%s - exit\n", __func__);
 	return 0;
 }
 
@@ -776,23 +773,23 @@ int ibmphp_add_resource (struct resource_node *res)
  * Output: modified resource list
  *        0 or error code
  ****************************************************************************/
-int ibmphp_remove_resource (struct resource_node *res)
+int ibmphp_remove_resource(struct resource_node *res)
 {
 	struct bus_node *bus_cur;
 	struct resource_node *res_cur = NULL;
 	struct resource_node *res_prev;
 	struct resource_node *mem_cur;
-	char * type = "";
+	char *type = "";
 
 	if (!res)  {
-		err ("resource to remove is NULL\n");
+		err("resource to remove is NULL\n");
 		return -ENODEV;
 	}
 
-	bus_cur = find_bus_wprev (res->busno, NULL, 0);
+	bus_cur = find_bus_wprev(res->busno, NULL, 0);
 
 	if (!bus_cur) {
-		err ("cannot find corresponding bus of the io resource to remove  bailing out...\n");
+		err("cannot find corresponding bus of the io resource to remove  bailing out...\n");
 		return -ENODEV;
 	}
 
@@ -810,7 +807,7 @@ int ibmphp_remove_resource (struct resource_node *res)
 			type = "pfmem";
 			break;
 		default:
-			err ("unknown type for resource to remove\n");
+			err("unknown type for resource to remove\n");
 			return -EINVAL;
 	}
 	res_prev = NULL;
@@ -848,16 +845,16 @@ int ibmphp_remove_resource (struct resource_node *res)
 							mem_cur = mem_cur->nextRange;
 					}
 					if (!mem_cur) {
-						err ("cannot find corresponding mem node for pfmem...\n");
+						err("cannot find corresponding mem node for pfmem...\n");
 						return -EINVAL;
 					}
 
-					ibmphp_remove_resource (mem_cur);
+					ibmphp_remove_resource(mem_cur);
 					if (!res_prev)
 						bus_cur->firstPFMemFromMem = res_cur->next;
 					else
 						res_prev->next = res_cur->next;
-					kfree (res_cur);
+					kfree(res_cur);
 					return 0;
 				}
 				res_prev = res_cur;
@@ -867,11 +864,11 @@ int ibmphp_remove_resource (struct resource_node *res)
 					res_cur = res_cur->nextRange;
 			}
 			if (!res_cur) {
-				err ("cannot find pfmem to delete...\n");
+				err("cannot find pfmem to delete...\n");
 				return -EINVAL;
 			}
 		} else {
-			err ("the %s resource is not in the list to be deleted...\n", type);
+			err("the %s resource is not in the list to be deleted...\n", type);
 			return -EINVAL;
 		}
 	}
@@ -914,7 +911,7 @@ int ibmphp_remove_resource (struct resource_node *res)
 					break;
 			}
 		}
-		kfree (res_cur);
+		kfree(res_cur);
 		return 0;
 	} else {
 		if (res_cur->next) {
@@ -929,14 +926,14 @@ int ibmphp_remove_resource (struct resource_node *res)
 			res_prev->next = NULL;
 			res_prev->nextRange = NULL;
 		}
-		kfree (res_cur);
+		kfree(res_cur);
 		return 0;
 	}
 
 	return 0;
 }
 
-static struct range_node *find_range (struct bus_node *bus_cur, struct resource_node *res)
+static struct range_node *find_range(struct bus_node *bus_cur, struct resource_node *res)
 {
 	struct range_node *range = NULL;
 
@@ -951,7 +948,7 @@ static struct range_node *find_range (struct bus_node *bus_cur, struct resource_
 			range = bus_cur->rangePFMem;
 			break;
 		default:
-			err ("cannot read resource type in find_range\n");
+			err("cannot read resource type in find_range\n");
 	}
 
 	while (range) {
@@ -971,7 +968,7 @@ static struct range_node *find_range (struct bus_node *bus_cur, struct resource_
  * Output: the correct start and end address are inputted into the resource node,
  *        0 or -EINVAL
  *****************************************************************************/
-int ibmphp_check_resource (struct resource_node *res, u8 bridge)
+int ibmphp_check_resource(struct resource_node *res, u8 bridge)
 {
 	struct bus_node *bus_cur;
 	struct range_node *range = NULL;
@@ -995,16 +992,16 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 	} else
 		tmp_divide = res->len;
 
-	bus_cur = find_bus_wprev (res->busno, NULL, 0);
+	bus_cur = find_bus_wprev(res->busno, NULL, 0);
 
 	if (!bus_cur) {
 		/* didn't find a bus, something's wrong!!! */
-		debug ("no bus in the system, either pci_dev's wrong or allocation failed\n");
+		debug("no bus in the system, either pci_dev's wrong or allocation failed\n");
 		return -EINVAL;
 	}
 
-	debug ("%s - enter\n", __func__);
-	debug ("bus_cur->busno is %d\n", bus_cur->busno);
+	debug("%s - enter\n", __func__);
+	debug("bus_cur->busno is %d\n", bus_cur->busno);
 
 	/* This is a quick fix to not mess up with the code very much.  i.e.,
 	 * 2000-2fff, len = 1000, but when we compare, we need it to be fff */
@@ -1024,17 +1021,17 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 			noranges = bus_cur->noPFMemRanges;
 			break;
 		default:
-			err ("wrong type of resource to check\n");
+			err("wrong type of resource to check\n");
 			return -EINVAL;
 	}
 	res_prev = NULL;
 
 	while (res_cur) {
-		range = find_range (bus_cur, res_cur);
-		debug ("%s - rangeno = %d\n", __func__, res_cur->rangeno);
+		range = find_range(bus_cur, res_cur);
+		debug("%s - rangeno = %d\n", __func__, res_cur->rangeno);
 
 		if (!range) {
-			err ("no range for the device exists... bailing out...\n");
+			err("no range for the device exists... bailing out...\n");
 			return -EINVAL;
 		}
 
@@ -1044,7 +1041,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 			len_tmp = res_cur->start - 1 - range->start;
 
 			if ((res_cur->start != range->start) && (len_tmp >= res->len)) {
-				debug ("len_tmp = %x\n", len_tmp);
+				debug("len_tmp = %x\n", len_tmp);
 
 				if ((len_tmp < len_cur) || (len_cur == 0)) {
 
@@ -1072,7 +1069,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 					}
 
 					if (flag && len_cur == res->len) {
-						debug ("but we are not here, right?\n");
+						debug("but we are not here, right?\n");
 						res->start = start_cur;
 						res->len += 1; /* To restore the balance */
 						res->end = res->start + res->len - 1;
@@ -1086,7 +1083,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 			len_tmp = range->end - (res_cur->end + 1);
 
 			if ((range->end != res_cur->end) && (len_tmp >= res->len)) {
-				debug ("len_tmp = %x\n", len_tmp);
+				debug("len_tmp = %x\n", len_tmp);
 				if ((len_tmp < len_cur) || (len_cur == 0)) {
 
 					if (((res_cur->end + 1) % tmp_divide) == 0) {
@@ -1262,7 +1259,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 
 		if ((!range) && (len_cur == 0)) {
 			/* have gone through the list of devices and ranges and haven't found n.e.thing */
-			err ("no appropriate range.. bailing out...\n");
+			err("no appropriate range.. bailing out...\n");
 			return -EINVAL;
 		} else if (len_cur) {
 			res->start = start_cur;
@@ -1273,7 +1270,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 	}
 
 	if (!res_cur) {
-		debug ("prev->rangeno = %d, noranges = %d\n", res_prev->rangeno, noranges);
+		debug("prev->rangeno = %d, noranges = %d\n", res_prev->rangeno, noranges);
 		if (res_prev->rangeno < noranges) {
 			/* if there're more ranges out there to check */
 			switch (res->type) {
@@ -1328,7 +1325,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 
 			if ((!range) && (len_cur == 0)) {
 				/* have gone through the list of devices and ranges and haven't found n.e.thing */
-				err ("no appropriate range.. bailing out...\n");
+				err("no appropriate range.. bailing out...\n");
 				return -EINVAL;
 			} else if (len_cur) {
 				res->start = start_cur;
@@ -1345,7 +1342,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 				return 0;
 			} else {
 				/* have gone through the list of devices and haven't found n.e.thing */
-				err ("no appropriate range.. bailing out...\n");
+				err("no appropriate range.. bailing out...\n");
 				return -EINVAL;
 			}
 		}
@@ -1359,23 +1356,23 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
  * Input: Bus
  * Output: 0, -ENODEV
  ********************************************************************************/
-int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
+int ibmphp_remove_bus(struct bus_node *bus, u8 parent_busno)
 {
 	struct resource_node *res_cur;
 	struct resource_node *res_tmp;
 	struct bus_node *prev_bus;
 	int rc;
 
-	prev_bus = find_bus_wprev (parent_busno, NULL, 0);
+	prev_bus = find_bus_wprev(parent_busno, NULL, 0);
 
 	if (!prev_bus) {
-		debug ("something terribly wrong. Cannot find parent bus to the one to remove\n");
+		debug("something terribly wrong. Cannot find parent bus to the one to remove\n");
 		return -ENODEV;
 	}
 
-	debug ("In ibmphp_remove_bus... prev_bus->busno is %x\n", prev_bus->busno);
+	debug("In ibmphp_remove_bus... prev_bus->busno is %x\n", prev_bus->busno);
 
-	rc = remove_ranges (bus, prev_bus);
+	rc = remove_ranges(bus, prev_bus);
 	if (rc)
 		return rc;
 
@@ -1387,7 +1384,7 @@ int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
 				res_cur = res_cur->next;
 			else
 				res_cur = res_cur->nextRange;
-			kfree (res_tmp);
+			kfree(res_tmp);
 			res_tmp = NULL;
 		}
 		bus->firstIO = NULL;
@@ -1400,7 +1397,7 @@ int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
 				res_cur = res_cur->next;
 			else
 				res_cur = res_cur->nextRange;
-			kfree (res_tmp);
+			kfree(res_tmp);
 			res_tmp = NULL;
 		}
 		bus->firstMem = NULL;
@@ -1413,7 +1410,7 @@ int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
 				res_cur = res_cur->next;
 			else
 				res_cur = res_cur->nextRange;
-			kfree (res_tmp);
+			kfree(res_tmp);
 			res_tmp = NULL;
 		}
 		bus->firstPFMem = NULL;
@@ -1425,14 +1422,14 @@ int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
 			res_tmp = res_cur;
 			res_cur = res_cur->next;
 
-			kfree (res_tmp);
+			kfree(res_tmp);
 			res_tmp = NULL;
 		}
 		bus->firstPFMemFromMem = NULL;
 	}
 
-	list_del (&bus->bus_list);
-	kfree (bus);
+	list_del(&bus->bus_list);
+	kfree(bus);
 	return 0;
 }
 
@@ -1442,7 +1439,7 @@ int ibmphp_remove_bus (struct bus_node *bus, u8 parent_busno)
  * Input: current bus, previous bus
  * Output: 0, -EINVAL
  ******************************************************************************/
-static int remove_ranges (struct bus_node *bus_cur, struct bus_node *bus_prev)
+static int remove_ranges(struct bus_node *bus_cur, struct bus_node *bus_prev)
 {
 	struct range_node *range_cur;
 	struct range_node *range_tmp;
@@ -1452,13 +1449,13 @@ static int remove_ranges (struct bus_node *bus_cur, struct bus_node *bus_prev)
 	if (bus_cur->noIORanges) {
 		range_cur = bus_cur->rangeIO;
 		for (i = 0; i < bus_cur->noIORanges; i++) {
-			if (ibmphp_find_resource (bus_prev, range_cur->start, &res, IO) < 0)
+			if (ibmphp_find_resource(bus_prev, range_cur->start, &res, IO) < 0)
 				return -EINVAL;
-			ibmphp_remove_resource (res);
+			ibmphp_remove_resource(res);
 
 			range_tmp = range_cur;
 			range_cur = range_cur->next;
-			kfree (range_tmp);
+			kfree(range_tmp);
 			range_tmp = NULL;
 		}
 		bus_cur->rangeIO = NULL;
@@ -1466,13 +1463,13 @@ static int remove_ranges (struct bus_node *bus_cur, struct bus_node *bus_prev)
 	if (bus_cur->noMemRanges) {
 		range_cur = bus_cur->rangeMem;
 		for (i = 0; i < bus_cur->noMemRanges; i++) {
-			if (ibmphp_find_resource (bus_prev, range_cur->start, &res, MEM) < 0)
+			if (ibmphp_find_resource(bus_prev, range_cur->start, &res, MEM) < 0)
 				return -EINVAL;
 
-			ibmphp_remove_resource (res);
+			ibmphp_remove_resource(res);
 			range_tmp = range_cur;
 			range_cur = range_cur->next;
-			kfree (range_tmp);
+			kfree(range_tmp);
 			range_tmp = NULL;
 		}
 		bus_cur->rangeMem = NULL;
@@ -1480,13 +1477,13 @@ static int remove_ranges (struct bus_node *bus_cur, struct bus_node *bus_prev)
 	if (bus_cur->noPFMemRanges) {
 		range_cur = bus_cur->rangePFMem;
 		for (i = 0; i < bus_cur->noPFMemRanges; i++) {
-			if (ibmphp_find_resource (bus_prev, range_cur->start, &res, PFMEM) < 0)
+			if (ibmphp_find_resource(bus_prev, range_cur->start, &res, PFMEM) < 0)
 				return -EINVAL;
 
-			ibmphp_remove_resource (res);
+			ibmphp_remove_resource(res);
 			range_tmp = range_cur;
 			range_cur = range_cur->next;
-			kfree (range_tmp);
+			kfree(range_tmp);
 			range_tmp = NULL;
 		}
 		bus_cur->rangePFMem = NULL;
@@ -1498,13 +1495,13 @@ static int remove_ranges (struct bus_node *bus_cur, struct bus_node *bus_prev)
  * find the resource node in the bus
  * Input: Resource needed, start address of the resource, type of resource
  */
-int ibmphp_find_resource (struct bus_node *bus, u32 start_address, struct resource_node **res, int flag)
+int ibmphp_find_resource(struct bus_node *bus, u32 start_address, struct resource_node **res, int flag)
 {
 	struct resource_node *res_cur = NULL;
-	char * type = "";
+	char *type = "";
 
 	if (!bus) {
-		err ("The bus passed in NULL to find resource\n");
+		err("The bus passed in NULL to find resource\n");
 		return -ENODEV;
 	}
 
@@ -1522,7 +1519,7 @@ int ibmphp_find_resource (struct bus_node *bus, u32 start_address, struct resour
 			type = "pfmem";
 			break;
 		default:
-			err ("wrong type of flag\n");
+			err("wrong type of flag\n");
 			return -EINVAL;
 	}
 
@@ -1548,17 +1545,17 @@ int ibmphp_find_resource (struct bus_node *bus, u32 start_address, struct resour
 				res_cur = res_cur->next;
 			}
 			if (!res_cur) {
-				debug ("SOS...cannot find %s resource in the bus.\n", type);
+				debug("SOS...cannot find %s resource in the bus.\n", type);
 				return -EINVAL;
 			}
 		} else {
-			debug ("SOS... cannot find %s resource in the bus.\n", type);
+			debug("SOS... cannot find %s resource in the bus.\n", type);
 			return -EINVAL;
 		}
 	}
 
 	if (*res)
-		debug ("*res->start = %x\n", (*res)->start);
+		debug("*res->start = %x\n", (*res)->start);
 
 	return 0;
 }
@@ -1569,21 +1566,18 @@ int ibmphp_find_resource (struct bus_node *bus, u32 start_address, struct resour
  * Parameters: none
  * Returns: none
  ***********************************************************************/
-void ibmphp_free_resources (void)
+void ibmphp_free_resources(void)
 {
-	struct bus_node *bus_cur = NULL;
+	struct bus_node *bus_cur = NULL, *next;
 	struct bus_node *bus_tmp;
 	struct range_node *range_cur;
 	struct range_node *range_tmp;
 	struct resource_node *res_cur;
 	struct resource_node *res_tmp;
-	struct list_head *tmp;
-	struct list_head *next;
 	int i = 0;
 	flags = 1;
 
-	list_for_each_safe (tmp, next, &gbuses) {
-		bus_cur = list_entry (tmp, struct bus_node, bus_list);
+	list_for_each_entry_safe(bus_cur, next, &gbuses, bus_list) {
 		if (bus_cur->noIORanges) {
 			range_cur = bus_cur->rangeIO;
 			for (i = 0; i < bus_cur->noIORanges; i++) {
@@ -1591,7 +1585,7 @@ void ibmphp_free_resources (void)
 					break;
 				range_tmp = range_cur;
 				range_cur = range_cur->next;
-				kfree (range_tmp);
+				kfree(range_tmp);
 				range_tmp = NULL;
 			}
 		}
@@ -1602,7 +1596,7 @@ void ibmphp_free_resources (void)
 					break;
 				range_tmp = range_cur;
 				range_cur = range_cur->next;
-				kfree (range_tmp);
+				kfree(range_tmp);
 				range_tmp = NULL;
 			}
 		}
@@ -1613,7 +1607,7 @@ void ibmphp_free_resources (void)
 					break;
 				range_tmp = range_cur;
 				range_cur = range_cur->next;
-				kfree (range_tmp);
+				kfree(range_tmp);
 				range_tmp = NULL;
 			}
 		}
@@ -1626,7 +1620,7 @@ void ibmphp_free_resources (void)
 					res_cur = res_cur->next;
 				else
 					res_cur = res_cur->nextRange;
-				kfree (res_tmp);
+				kfree(res_tmp);
 				res_tmp = NULL;
 			}
 			bus_cur->firstIO = NULL;
@@ -1639,7 +1633,7 @@ void ibmphp_free_resources (void)
 					res_cur = res_cur->next;
 				else
 					res_cur = res_cur->nextRange;
-				kfree (res_tmp);
+				kfree(res_tmp);
 				res_tmp = NULL;
 			}
 			bus_cur->firstMem = NULL;
@@ -1652,7 +1646,7 @@ void ibmphp_free_resources (void)
 					res_cur = res_cur->next;
 				else
 					res_cur = res_cur->nextRange;
-				kfree (res_tmp);
+				kfree(res_tmp);
 				res_tmp = NULL;
 			}
 			bus_cur->firstPFMem = NULL;
@@ -1664,15 +1658,15 @@ void ibmphp_free_resources (void)
 				res_tmp = res_cur;
 				res_cur = res_cur->next;
 
-				kfree (res_tmp);
+				kfree(res_tmp);
 				res_tmp = NULL;
 			}
 			bus_cur->firstPFMemFromMem = NULL;
 		}
 
 		bus_tmp = bus_cur;
-		list_del (&bus_cur->bus_list);
-		kfree (bus_tmp);
+		list_del(&bus_cur->bus_list);
+		kfree(bus_tmp);
 		bus_tmp = NULL;
 	}
 }
@@ -1685,16 +1679,14 @@ void ibmphp_free_resources (void)
  * a new Mem node
  * This routine is called right after initialization
  *******************************************************************************/
-static int __init once_over (void)
+static int __init once_over(void)
 {
 	struct resource_node *pfmem_cur;
 	struct resource_node *pfmem_prev;
 	struct resource_node *mem;
 	struct bus_node *bus_cur;
-	struct list_head *tmp;
 
-	list_for_each (tmp, &gbuses) {
-		bus_cur = list_entry (tmp, struct bus_node, bus_list);
+	list_for_each_entry(bus_cur, &gbuses, bus_list) {
 		if ((!bus_cur->rangePFMem) && (bus_cur->firstPFMem)) {
 			for (pfmem_cur = bus_cur->firstPFMem, pfmem_prev = NULL; pfmem_cur; pfmem_prev = pfmem_cur, pfmem_cur = pfmem_cur->next) {
 				pfmem_cur->fromMem = 1;
@@ -1716,7 +1708,7 @@ static int __init once_over (void)
 
 				mem = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 				if (!mem) {
-					err ("out of system memory\n");
+					err("out of system memory\n");
 					return -ENOMEM;
 				}
 				mem->type = MEM;
@@ -1725,8 +1717,8 @@ static int __init once_over (void)
 				mem->start = pfmem_cur->start;
 				mem->end = pfmem_cur->end;
 				mem->len = pfmem_cur->len;
-				if (ibmphp_add_resource (mem) < 0)
-					err ("Trouble...trouble... EBDA allocated pfmem from mem, but system doesn't display it has this space... unless not PCI device...\n");
+				if (ibmphp_add_resource(mem) < 0)
+					err("Trouble...trouble... EBDA allocated pfmem from mem, but system doesn't display it has this space... unless not PCI device...\n");
 				pfmem_cur->rangeno = mem->rangeno;
 			}	/* end for pfmem */
 		}	/* end if */
@@ -1734,12 +1726,12 @@ static int __init once_over (void)
 	return 0;
 }
 
-int ibmphp_add_pfmem_from_mem (struct resource_node *pfmem)
+int ibmphp_add_pfmem_from_mem(struct resource_node *pfmem)
 {
-	struct bus_node *bus_cur = find_bus_wprev (pfmem->busno, NULL, 0);
+	struct bus_node *bus_cur = find_bus_wprev(pfmem->busno, NULL, 0);
 
 	if (!bus_cur) {
-		err ("cannot find bus of pfmem to add...\n");
+		err("cannot find bus of pfmem to add...\n");
 		return -ENODEV;
 	}
 
@@ -1759,22 +1751,18 @@ int ibmphp_add_pfmem_from_mem (struct resource_node *pfmem)
  * Parameters: bus_number
  * Returns: Bus pointer or NULL
  */
-struct bus_node *ibmphp_find_res_bus (u8 bus_number)
+struct bus_node *ibmphp_find_res_bus(u8 bus_number)
 {
-	return find_bus_wprev (bus_number, NULL, 0);
+	return find_bus_wprev(bus_number, NULL, 0);
 }
 
-static struct bus_node *find_bus_wprev (u8 bus_number, struct bus_node **prev, u8 flag)
+static struct bus_node *find_bus_wprev(u8 bus_number, struct bus_node **prev, u8 flag)
 {
 	struct bus_node *bus_cur;
-	struct list_head *tmp;
-	struct list_head *tmp_prev;
 
-	list_for_each (tmp, &gbuses) {
-		tmp_prev = tmp->prev;
-		bus_cur = list_entry (tmp, struct bus_node, bus_list);
+	list_for_each_entry(bus_cur, &gbuses, bus_list) {
 		if (flag)
-			*prev = list_entry (tmp_prev, struct bus_node, bus_list);
+			*prev = list_prev_entry(bus_cur, bus_list);
 		if (bus_cur->busno == bus_number)
 			return bus_cur;
 	}
@@ -1782,23 +1770,21 @@ static struct bus_node *find_bus_wprev (u8 bus_number, struct bus_node **prev, u
 	return NULL;
 }
 
-void ibmphp_print_test (void)
+void ibmphp_print_test(void)
 {
 	int i = 0;
 	struct bus_node *bus_cur = NULL;
 	struct range_node *range;
 	struct resource_node *res;
-	struct list_head *tmp;
 
-	debug_pci ("*****************START**********************\n");
+	debug_pci("*****************START**********************\n");
 
 	if ((!list_empty(&gbuses)) && flags) {
-		err ("The GBUSES is not NULL?!?!?!?!?\n");
+		err("The GBUSES is not NULL?!?!?!?!?\n");
 		return;
 	}
 
-	list_for_each (tmp, &gbuses) {
-		bus_cur = list_entry (tmp, struct bus_node, bus_list);
+	list_for_each_entry(bus_cur, &gbuses, bus_list) {
 		debug_pci ("This is bus # %d.  There are\n", bus_cur->busno);
 		debug_pci ("IORanges = %d\t", bus_cur->noIORanges);
 		debug_pci ("MemRanges = %d\t", bus_cur->noMemRanges);
@@ -1807,42 +1793,42 @@ void ibmphp_print_test (void)
 		if (bus_cur->rangeIO) {
 			range = bus_cur->rangeIO;
 			for (i = 0; i < bus_cur->noIORanges; i++) {
-				debug_pci ("rangeno is %d\n", range->rangeno);
-				debug_pci ("[%x - %x]\n", range->start, range->end);
+				debug_pci("rangeno is %d\n", range->rangeno);
+				debug_pci("[%x - %x]\n", range->start, range->end);
 				range = range->next;
 			}
 		}
 
-		debug_pci ("The Mem Ranges are as follows:\n");
+		debug_pci("The Mem Ranges are as follows:\n");
 		if (bus_cur->rangeMem) {
 			range = bus_cur->rangeMem;
 			for (i = 0; i < bus_cur->noMemRanges; i++) {
-				debug_pci ("rangeno is %d\n", range->rangeno);
-				debug_pci ("[%x - %x]\n", range->start, range->end);
+				debug_pci("rangeno is %d\n", range->rangeno);
+				debug_pci("[%x - %x]\n", range->start, range->end);
 				range = range->next;
 			}
 		}
 
-		debug_pci ("The PFMem Ranges are as follows:\n");
+		debug_pci("The PFMem Ranges are as follows:\n");
 
 		if (bus_cur->rangePFMem) {
 			range = bus_cur->rangePFMem;
 			for (i = 0; i < bus_cur->noPFMemRanges; i++) {
-				debug_pci ("rangeno is %d\n", range->rangeno);
-				debug_pci ("[%x - %x]\n", range->start, range->end);
+				debug_pci("rangeno is %d\n", range->rangeno);
+				debug_pci("[%x - %x]\n", range->start, range->end);
 				range = range->next;
 			}
 		}
 
-		debug_pci ("The resources on this bus are as follows\n");
+		debug_pci("The resources on this bus are as follows\n");
 
-		debug_pci ("IO...\n");
+		debug_pci("IO...\n");
 		if (bus_cur->firstIO) {
 			res = bus_cur->firstIO;
 			while (res) {
-				debug_pci ("The range # is %d\n", res->rangeno);
-				debug_pci ("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
-				debug_pci ("[%x - %x], len=%x\n", res->start, res->end, res->len);
+				debug_pci("The range # is %d\n", res->rangeno);
+				debug_pci("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
+				debug_pci("[%x - %x], len=%x\n", res->start, res->end, res->len);
 				if (res->next)
 					res = res->next;
 				else if (res->nextRange)
@@ -1851,13 +1837,13 @@ void ibmphp_print_test (void)
 					break;
 			}
 		}
-		debug_pci ("Mem...\n");
+		debug_pci("Mem...\n");
 		if (bus_cur->firstMem) {
 			res = bus_cur->firstMem;
 			while (res) {
-				debug_pci ("The range # is %d\n", res->rangeno);
-				debug_pci ("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
-				debug_pci ("[%x - %x], len=%x\n", res->start, res->end, res->len);
+				debug_pci("The range # is %d\n", res->rangeno);
+				debug_pci("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
+				debug_pci("[%x - %x], len=%x\n", res->start, res->end, res->len);
 				if (res->next)
 					res = res->next;
 				else if (res->nextRange)
@@ -1866,13 +1852,13 @@ void ibmphp_print_test (void)
 					break;
 			}
 		}
-		debug_pci ("PFMem...\n");
+		debug_pci("PFMem...\n");
 		if (bus_cur->firstPFMem) {
 			res = bus_cur->firstPFMem;
 			while (res) {
-				debug_pci ("The range # is %d\n", res->rangeno);
-				debug_pci ("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
-				debug_pci ("[%x - %x], len=%x\n", res->start, res->end, res->len);
+				debug_pci("The range # is %d\n", res->rangeno);
+				debug_pci("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
+				debug_pci("[%x - %x], len=%x\n", res->start, res->end, res->len);
 				if (res->next)
 					res = res->next;
 				else if (res->nextRange)
@@ -1882,23 +1868,23 @@ void ibmphp_print_test (void)
 			}
 		}
 
-		debug_pci ("PFMemFromMem...\n");
+		debug_pci("PFMemFromMem...\n");
 		if (bus_cur->firstPFMemFromMem) {
 			res = bus_cur->firstPFMemFromMem;
 			while (res) {
-				debug_pci ("The range # is %d\n", res->rangeno);
-				debug_pci ("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
-				debug_pci ("[%x - %x], len=%x\n", res->start, res->end, res->len);
+				debug_pci("The range # is %d\n", res->rangeno);
+				debug_pci("The bus, devfnc is %d, %x\n", res->busno, res->devfunc);
+				debug_pci("[%x - %x], len=%x\n", res->start, res->end, res->len);
 				res = res->next;
 			}
 		}
 	}
-	debug_pci ("***********************END***********************\n");
+	debug_pci("***********************END***********************\n");
 }
 
-static int range_exists_already (struct range_node * range, struct bus_node * bus_cur, u8 type)
+static int range_exists_already(struct range_node *range, struct bus_node *bus_cur, u8 type)
 {
-	struct range_node * range_cur = NULL;
+	struct range_node *range_cur = NULL;
 	switch (type) {
 		case IO:
 			range_cur = bus_cur->rangeIO;
@@ -1910,7 +1896,7 @@ static int range_exists_already (struct range_node * range, struct bus_node * bu
 			range_cur = bus_cur->rangePFMem;
 			break;
 		default:
-			err ("wrong type passed to find out if range already exists\n");
+			err("wrong type passed to find out if range already exists\n");
 			return -ENODEV;
 	}
 
@@ -1937,7 +1923,7 @@ static int range_exists_already (struct range_node * range, struct bus_node * bu
  *	 behind them All these are TO DO.
  *	 Also need to add more error checkings... (from fnc returns etc)
  */
-static int __init update_bridge_ranges (struct bus_node **bus)
+static int __init update_bridge_ranges(struct bus_node **bus)
 {
 	u8 sec_busno, device, function, hdr_type, start_io_address, end_io_address;
 	u16 vendor_id, upper_io_start, upper_io_end, start_mem_address, end_mem_address;
@@ -1955,17 +1941,17 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 		return -ENODEV;
 	ibmphp_pci_bus->number = bus_cur->busno;
 
-	debug ("inside %s\n", __func__);
-	debug ("bus_cur->busno = %x\n", bus_cur->busno);
+	debug("inside %s\n", __func__);
+	debug("bus_cur->busno = %x\n", bus_cur->busno);
 
 	for (device = 0; device < 32; device++) {
 		for (function = 0x00; function < 0x08; function++) {
 			devfn = PCI_DEVFN(device, function);
-			pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
+			pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_VENDOR_ID, &vendor_id);
 
 			if (vendor_id != PCI_VENDOR_ID_NOTVALID) {
 				/* found correct device!!! */
-				pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
+				pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_HEADER_TYPE, &hdr_type);
 
 				switch (hdr_type) {
 					case PCI_HEADER_TYPE_NORMAL:
@@ -1984,18 +1970,18 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 						   temp++;
 						   }
 						 */
-						pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_busno);
-						bus_sec = find_bus_wprev (sec_busno, NULL, 0);
+						pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_SECONDARY_BUS, &sec_busno);
+						bus_sec = find_bus_wprev(sec_busno, NULL, 0);
 						/* this bus structure doesn't exist yet, PPB was configured during previous loading of ibmphp */
 						if (!bus_sec) {
-							bus_sec = alloc_error_bus (NULL, sec_busno, 1);
+							bus_sec = alloc_error_bus(NULL, sec_busno, 1);
 							/* the rest will be populated during NVRAM call */
 							return 0;
 						}
-						pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_IO_BASE, &start_io_address);
-						pci_bus_read_config_byte (ibmphp_pci_bus, devfn, PCI_IO_LIMIT, &end_io_address);
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_IO_BASE_UPPER16, &upper_io_start);
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_IO_LIMIT_UPPER16, &upper_io_end);
+						pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_IO_BASE, &start_io_address);
+						pci_bus_read_config_byte(ibmphp_pci_bus, devfn, PCI_IO_LIMIT, &end_io_address);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_IO_BASE_UPPER16, &upper_io_start);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_IO_LIMIT_UPPER16, &upper_io_end);
 						start_address = (start_io_address & PCI_IO_RANGE_MASK) << 8;
 						start_address |= (upper_io_start << 16);
 						end_address = (end_io_address & PCI_IO_RANGE_MASK) << 8;
@@ -2004,18 +1990,18 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 						if ((start_address) && (start_address <= end_address)) {
 							range = kzalloc(sizeof(struct range_node), GFP_KERNEL);
 							if (!range) {
-								err ("out of system memory\n");
+								err("out of system memory\n");
 								return -ENOMEM;
 							}
 							range->start = start_address;
 							range->end = end_address + 0xfff;
 
 							if (bus_sec->noIORanges > 0) {
-								if (!range_exists_already (range, bus_sec, IO)) {
-									add_bus_range (IO, range, bus_sec);
+								if (!range_exists_already(range, bus_sec, IO)) {
+									add_bus_range(IO, range, bus_sec);
 									++bus_sec->noIORanges;
 								} else {
-									kfree (range);
+									kfree(range);
 									range = NULL;
 								}
 							} else {
@@ -2024,13 +2010,13 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								bus_sec->rangeIO = range;
 								++bus_sec->noIORanges;
 							}
-							fix_resources (bus_sec);
+							fix_resources(bus_sec);
 
-							if (ibmphp_find_resource (bus_cur, start_address, &io, IO)) {
+							if (ibmphp_find_resource(bus_cur, start_address, &io, IO)) {
 								io = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 								if (!io) {
-									kfree (range);
-									err ("out of system memory\n");
+									kfree(range);
+									err("out of system memory\n");
 									return -ENOMEM;
 								}
 								io->type = IO;
@@ -2039,12 +2025,12 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								io->start = start_address;
 								io->end = end_address + 0xfff;
 								io->len = io->end - io->start + 1;
-								ibmphp_add_resource (io);
+								ibmphp_add_resource(io);
 							}
 						}
 
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, &start_mem_address);
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, &end_mem_address);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_BASE, &start_mem_address);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_MEMORY_LIMIT, &end_mem_address);
 
 						start_address = 0x00000000 | (start_mem_address & PCI_MEMORY_RANGE_MASK) << 16;
 						end_address = 0x00000000 | (end_mem_address & PCI_MEMORY_RANGE_MASK) << 16;
@@ -2053,18 +2039,18 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 
 							range = kzalloc(sizeof(struct range_node), GFP_KERNEL);
 							if (!range) {
-								err ("out of system memory\n");
+								err("out of system memory\n");
 								return -ENOMEM;
 							}
 							range->start = start_address;
 							range->end = end_address + 0xfffff;
 
 							if (bus_sec->noMemRanges > 0) {
-								if (!range_exists_already (range, bus_sec, MEM)) {
-									add_bus_range (MEM, range, bus_sec);
+								if (!range_exists_already(range, bus_sec, MEM)) {
+									add_bus_range(MEM, range, bus_sec);
 									++bus_sec->noMemRanges;
 								} else {
-									kfree (range);
+									kfree(range);
 									range = NULL;
 								}
 							} else {
@@ -2074,13 +2060,13 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								++bus_sec->noMemRanges;
 							}
 
-							fix_resources (bus_sec);
+							fix_resources(bus_sec);
 
-							if (ibmphp_find_resource (bus_cur, start_address, &mem, MEM)) {
+							if (ibmphp_find_resource(bus_cur, start_address, &mem, MEM)) {
 								mem = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 								if (!mem) {
-									kfree (range);
-									err ("out of system memory\n");
+									kfree(range);
+									err("out of system memory\n");
 									return -ENOMEM;
 								}
 								mem->type = MEM;
@@ -2089,13 +2075,13 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								mem->start = start_address;
 								mem->end = end_address + 0xfffff;
 								mem->len = mem->end - mem->start + 1;
-								ibmphp_add_resource (mem);
+								ibmphp_add_resource(mem);
 							}
 						}
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &start_mem_address);
-						pci_bus_read_config_word (ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, &end_mem_address);
-						pci_bus_read_config_dword (ibmphp_pci_bus, devfn, PCI_PREF_BASE_UPPER32, &upper_start);
-						pci_bus_read_config_dword (ibmphp_pci_bus, devfn, PCI_PREF_LIMIT_UPPER32, &upper_end);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_BASE, &start_mem_address);
+						pci_bus_read_config_word(ibmphp_pci_bus, devfn, PCI_PREF_MEMORY_LIMIT, &end_mem_address);
+						pci_bus_read_config_dword(ibmphp_pci_bus, devfn, PCI_PREF_BASE_UPPER32, &upper_start);
+						pci_bus_read_config_dword(ibmphp_pci_bus, devfn, PCI_PREF_LIMIT_UPPER32, &upper_end);
 						start_address = 0x00000000 | (start_mem_address & PCI_MEMORY_RANGE_MASK) << 16;
 						end_address = 0x00000000 | (end_mem_address & PCI_MEMORY_RANGE_MASK) << 16;
 #if BITS_PER_LONG == 64
@@ -2107,18 +2093,18 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 
 							range = kzalloc(sizeof(struct range_node), GFP_KERNEL);
 							if (!range) {
-								err ("out of system memory\n");
+								err("out of system memory\n");
 								return -ENOMEM;
 							}
 							range->start = start_address;
 							range->end = end_address + 0xfffff;
 
 							if (bus_sec->noPFMemRanges > 0) {
-								if (!range_exists_already (range, bus_sec, PFMEM)) {
-									add_bus_range (PFMEM, range, bus_sec);
+								if (!range_exists_already(range, bus_sec, PFMEM)) {
+									add_bus_range(PFMEM, range, bus_sec);
 									++bus_sec->noPFMemRanges;
 								} else {
-									kfree (range);
+									kfree(range);
 									range = NULL;
 								}
 							} else {
@@ -2128,12 +2114,12 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								++bus_sec->noPFMemRanges;
 							}
 
-							fix_resources (bus_sec);
-							if (ibmphp_find_resource (bus_cur, start_address, &pfmem, PFMEM)) {
+							fix_resources(bus_sec);
+							if (ibmphp_find_resource(bus_cur, start_address, &pfmem, PFMEM)) {
 								pfmem = kzalloc(sizeof(struct resource_node), GFP_KERNEL);
 								if (!pfmem) {
-									kfree (range);
-									err ("out of system memory\n");
+									kfree(range);
+									err("out of system memory\n");
 									return -ENOMEM;
 								}
 								pfmem->type = PFMEM;
@@ -2144,7 +2130,7 @@ static int __init update_bridge_ranges (struct bus_node **bus)
 								pfmem->len = pfmem->end - pfmem->start + 1;
 								pfmem->fromMem = 0;
 
-								ibmphp_add_resource (pfmem);
+								ibmphp_add_resource(pfmem);
 							}
 						}
 						break;
diff --git a/drivers/pci/hotplug/pci_hotplug_core.c b/drivers/pci/hotplug/pci_hotplug_core.c
index d1fab97d6b01..9acd1997c6fe 100644
--- a/drivers/pci/hotplug/pci_hotplug_core.c
+++ b/drivers/pci/hotplug/pci_hotplug_core.c
@@ -45,10 +45,10 @@
 
 #define MY_NAME	"pci_hotplug"
 
-#define dbg(fmt, arg...) do { if (debug) printk(KERN_DEBUG "%s: %s: " fmt , MY_NAME , __func__ , ## arg); } while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format , MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format , MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format , MY_NAME , ## arg)
+#define dbg(fmt, arg...) do { if (debug) printk(KERN_DEBUG "%s: %s: " fmt, MY_NAME, __func__, ## arg); } while (0)
+#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 
 /* local variables */
@@ -226,7 +226,7 @@ static ssize_t test_write_file(struct pci_slot *pci_slot, const char *buf,
 	u32 test;
 	int retval = 0;
 
-	ltest = simple_strtoul (buf, NULL, 10);
+	ltest = simple_strtoul(buf, NULL, 10);
 	test = (u32)(ltest & 0xffffffff);
 	dbg("test = %d\n", test);
 
@@ -396,10 +396,8 @@ static void fs_remove_slot(struct pci_slot *pci_slot)
 static struct hotplug_slot *get_slot_from_name(const char *name)
 {
 	struct hotplug_slot *slot;
-	struct list_head *tmp;
 
-	list_for_each(tmp, &pci_hotplug_slot_list) {
-		slot = list_entry(tmp, struct hotplug_slot, slot_list);
+	list_for_each_entry(slot, &pci_hotplug_slot_list, slot_list) {
 		if (strcmp(hotplug_slot_name(slot), name) == 0)
 			return slot;
 	}
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 62d6fe6c3714..e764918641ae 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -47,14 +47,14 @@ extern bool pciehp_debug;
 #define dbg(format, arg...)						\
 do {									\
 	if (pciehp_debug)						\
-		printk(KERN_DEBUG "%s: " format, MY_NAME , ## arg);	\
+		printk(KERN_DEBUG "%s: " format, MY_NAME, ## arg);	\
 } while (0)
 #define err(format, arg...)						\
-	printk(KERN_ERR "%s: " format, MY_NAME , ## arg)
+	printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
 #define info(format, arg...)						\
-	printk(KERN_INFO "%s: " format, MY_NAME , ## arg)
+	printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
 #define warn(format, arg...)						\
-	printk(KERN_WARNING "%s: " format, MY_NAME , ## arg)
+	printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 #define ctrl_dbg(ctrl, format, arg...)					\
 	do {								\
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 612b21a14df5..ac531e674a05 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -62,14 +62,14 @@ MODULE_PARM_DESC(pciehp_force, "Force pciehp, even if OSHP is missing");
 
 #define PCIE_MODULE_NAME "pciehp"
 
-static int set_attention_status (struct hotplug_slot *slot, u8 value);
-static int enable_slot		(struct hotplug_slot *slot);
-static int disable_slot		(struct hotplug_slot *slot);
-static int get_power_status	(struct hotplug_slot *slot, u8 *value);
-static int get_attention_status	(struct hotplug_slot *slot, u8 *value);
-static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
-static int get_adapter_status	(struct hotplug_slot *slot, u8 *value);
-static int reset_slot		(struct hotplug_slot *slot, int probe);
+static int set_attention_status(struct hotplug_slot *slot, u8 value);
+static int enable_slot(struct hotplug_slot *slot);
+static int disable_slot(struct hotplug_slot *slot);
+static int get_power_status(struct hotplug_slot *slot, u8 *value);
+static int get_attention_status(struct hotplug_slot *slot, u8 *value);
+static int get_latch_status(struct hotplug_slot *slot, u8 *value);
+static int get_adapter_status(struct hotplug_slot *slot, u8 *value);
+static int reset_slot(struct hotplug_slot *slot, int probe);
 
 /**
  * release_slot - free up the memory used by a slot
diff --git a/drivers/pci/hotplug/pciehp_ctrl.c b/drivers/pci/hotplug/pciehp_ctrl.c
index 4c8f4cde6854..880978b6d534 100644
--- a/drivers/pci/hotplug/pciehp_ctrl.c
+++ b/drivers/pci/hotplug/pciehp_ctrl.c
@@ -511,7 +511,9 @@ int pciehp_sysfs_disable_slot(struct slot *p_slot)
 	case STATIC_STATE:
 		p_slot->state = POWEROFF_STATE;
 		mutex_unlock(&p_slot->lock);
+		mutex_lock(&p_slot->hotplug_lock);
 		retval = pciehp_disable_slot(p_slot);
+		mutex_unlock(&p_slot->hotplug_lock);
 		mutex_lock(&p_slot->lock);
 		p_slot->state = STATIC_STATE;
 		break;
diff --git a/drivers/pci/hotplug/pcihp_skeleton.c b/drivers/pci/hotplug/pcihp_skeleton.c
index d062c008fc95..172ed89200cd 100644
--- a/drivers/pci/hotplug/pcihp_skeleton.c
+++ b/drivers/pci/hotplug/pcihp_skeleton.c
@@ -52,11 +52,11 @@ static LIST_HEAD(slot_list);
 	do {							\
 		if (debug)					\
 			printk(KERN_DEBUG "%s: " format "\n",	\
-				MY_NAME , ## arg);		\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME, ## arg)
 
 /* local variables */
 static bool debug;
@@ -72,14 +72,14 @@ MODULE_LICENSE("GPL");
 module_param(debug, bool, 0644);
 MODULE_PARM_DESC(debug, "Debugging mode enabled or not");
 
-static int enable_slot		(struct hotplug_slot *slot);
-static int disable_slot		(struct hotplug_slot *slot);
-static int set_attention_status (struct hotplug_slot *slot, u8 value);
-static int hardware_test	(struct hotplug_slot *slot, u32 value);
-static int get_power_status	(struct hotplug_slot *slot, u8 *value);
-static int get_attention_status	(struct hotplug_slot *slot, u8 *value);
-static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
-static int get_adapter_status	(struct hotplug_slot *slot, u8 *value);
+static int enable_slot(struct hotplug_slot *slot);
+static int disable_slot(struct hotplug_slot *slot);
+static int set_attention_status(struct hotplug_slot *slot, u8 value);
+static int hardware_test(struct hotplug_slot *slot, u32 value);
+static int get_power_status(struct hotplug_slot *slot, u8 *value);
+static int get_attention_status(struct hotplug_slot *slot, u8 *value);
+static int get_latch_status(struct hotplug_slot *slot, u8 *value);
+static int get_adapter_status(struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops skel_hotplug_slot_ops = {
 	.enable_slot =		enable_slot,
@@ -321,17 +321,14 @@ error:
 
 static void __exit cleanup_slots(void)
 {
-	struct list_head *tmp;
-	struct list_head *next;
-	struct slot *slot;
+	struct slot *slot, *next;
 
 	/*
 	 * Unregister all of our slots with the pci_hotplug subsystem.
 	 * Memory will be freed in release_slot() callback after slot's
 	 * lifespan is finished.
 	 */
-	list_for_each_safe(tmp, next, &slot_list) {
-		slot = list_entry(tmp, struct slot, slot_list);
+	list_for_each_entry_safe(slot, next, &slot_list, slot_list) {
 		list_del(&slot->slot_list);
 		pci_hp_deregister(slot->hotplug_slot);
 	}
diff --git a/drivers/pci/hotplug/rpadlpar_core.c b/drivers/pci/hotplug/rpadlpar_core.c
index e12bafdc42e0..b46b57d870fc 100644
--- a/drivers/pci/hotplug/rpadlpar_core.c
+++ b/drivers/pci/hotplug/rpadlpar_core.c
@@ -114,11 +114,10 @@ static struct device_node *find_dlpar_node(char *drc_name, int *node_type)
  */
 static struct slot *find_php_slot(struct device_node *dn)
 {
-	struct list_head *tmp, *n;
-	struct slot *slot;
+	struct slot *slot, *next;
 
-	list_for_each_safe(tmp, n, &rpaphp_slot_head) {
-		slot = list_entry(tmp, struct slot, rpaphp_slot_list);
+	list_for_each_entry_safe(slot, next, &rpaphp_slot_head,
+				 rpaphp_slot_list) {
 		if (slot->dn == dn)
 			return slot;
 	}
diff --git a/drivers/pci/hotplug/rpaphp.h b/drivers/pci/hotplug/rpaphp.h
index b2593e876a09..7db024e68fe6 100644
--- a/drivers/pci/hotplug/rpaphp.h
+++ b/drivers/pci/hotplug/rpaphp.h
@@ -51,11 +51,11 @@ extern bool rpaphp_debug;
 	do {							\
 		if (rpaphp_debug)				\
 			printk(KERN_DEBUG "%s: " format,	\
-				MY_NAME , ## arg);		\
+				MY_NAME, ## arg);		\
 	} while (0)
-#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME , ## arg)
-#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME , ## arg)
-#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME , ## arg)
+#define err(format, arg...) printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
+#define info(format, arg...) printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
+#define warn(format, arg...) printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 /* slot states */
 
diff --git a/drivers/pci/hotplug/rpaphp_core.c b/drivers/pci/hotplug/rpaphp_core.c
index f2945fa73d4f..611f6056221a 100644
--- a/drivers/pci/hotplug/rpaphp_core.c
+++ b/drivers/pci/hotplug/rpaphp_core.c
@@ -94,7 +94,7 @@ static int get_power_status(struct hotplug_slot *hotplug_slot, u8 *value)
 	int retval, level;
 	struct slot *slot = (struct slot *)hotplug_slot->private;
 
-	retval = rtas_get_power_level (slot->power_domain, &level);
+	retval = rtas_get_power_level(slot->power_domain, &level);
 	if (!retval)
 		*value = level;
 	return retval;
@@ -356,8 +356,7 @@ EXPORT_SYMBOL_GPL(rpaphp_add_slot);
 
 static void __exit cleanup_slots(void)
 {
-	struct list_head *tmp, *n;
-	struct slot *slot;
+	struct slot *slot, *next;
 
 	/*
 	 * Unregister all of our slots with the pci_hotplug subsystem,
@@ -365,8 +364,8 @@ static void __exit cleanup_slots(void)
 	 * memory will be freed in release_slot callback.
 	 */
 
-	list_for_each_safe(tmp, n, &rpaphp_slot_head) {
-		slot = list_entry(tmp, struct slot, rpaphp_slot_list);
+	list_for_each_entry_safe(slot, next, &rpaphp_slot_head,
+				 rpaphp_slot_list) {
 		list_del(&slot->rpaphp_slot_list);
 		pci_hp_deregister(slot->hotplug_slot);
 	}
diff --git a/drivers/pci/hotplug/rpaphp_pci.c b/drivers/pci/hotplug/rpaphp_pci.c
index 9243f3e7a1c9..7836d6913e67 100644
--- a/drivers/pci/hotplug/rpaphp_pci.c
+++ b/drivers/pci/hotplug/rpaphp_pci.c
@@ -126,7 +126,7 @@ int rpaphp_enable_slot(struct slot *slot)
 		if (rpaphp_debug) {
 			struct pci_dev *dev;
 			dbg("%s: pci_devs of slot[%s]\n", __func__, slot->dn->full_name);
-			list_for_each_entry (dev, &bus->devices, bus_list)
+			list_for_each_entry(dev, &bus->devices, bus_list)
 				dbg("\t%s\n", pci_name(dev));
 		}
 	}
diff --git a/drivers/pci/hotplug/rpaphp_slot.c b/drivers/pci/hotplug/rpaphp_slot.c
index a6082cc263f7..6937c725b00b 100644
--- a/drivers/pci/hotplug/rpaphp_slot.c
+++ b/drivers/pci/hotplug/rpaphp_slot.c
@@ -48,7 +48,7 @@ void dealloc_slot_struct(struct slot *slot)
 }
 
 struct slot *alloc_slot_struct(struct device_node *dn,
-                       int drc_index, char *drc_name, int power_domain)
+		int drc_index, char *drc_name, int power_domain)
 {
 	struct slot *slot;
 
diff --git a/drivers/pci/hotplug/s390_pci_hpc.c b/drivers/pci/hotplug/s390_pci_hpc.c
index d77e46bca54c..eb5efaef06ea 100644
--- a/drivers/pci/hotplug/s390_pci_hpc.c
+++ b/drivers/pci/hotplug/s390_pci_hpc.c
@@ -201,11 +201,10 @@ error:
 
 void zpci_exit_slot(struct zpci_dev *zdev)
 {
-	struct list_head *tmp, *n;
-	struct slot *slot;
+	struct slot *slot, *next;
 
-	list_for_each_safe(tmp, n, &s390_hotplug_slot_list) {
-		slot = list_entry(tmp, struct slot, slot_list);
+	list_for_each_entry_safe(slot, next, &s390_hotplug_slot_list,
+				 slot_list) {
 		if (slot->zdev != zdev)
 			continue;
 		list_del(&slot->slot_list);
diff --git a/drivers/pci/hotplug/sgi_hotplug.c b/drivers/pci/hotplug/sgi_hotplug.c
index c32fb786d48e..339bce0403dd 100644
--- a/drivers/pci/hotplug/sgi_hotplug.c
+++ b/drivers/pci/hotplug/sgi_hotplug.c
@@ -99,7 +99,7 @@ static ssize_t path_show(struct pci_slot *pci_slot, char *buf)
 	if (!slot)
 		return retval;
 
-	retval = sprintf (buf, "%s\n", slot->physical_path);
+	retval = sprintf(buf, "%s\n", slot->physical_path);
 	return retval;
 }
 
@@ -313,7 +313,7 @@ static int sn_slot_disable(struct hotplug_slot *bss_hotplug_slot,
 	}
 
 	if ((action == PCI_REQ_SLOT_DISABLE) && rc) {
-		dev_dbg(&slot->pci_bus->self->dev,"remove failed rc = %d\n", rc);
+		dev_dbg(&slot->pci_bus->self->dev, "remove failed rc = %d\n", rc);
 	}
 
 	return rc;
@@ -488,7 +488,7 @@ static int disable_slot(struct hotplug_slot *bss_hotplug_slot)
 
 	/* free the ACPI resources for the slot */
 	if (SN_ACPI_BASE_SUPPORT() &&
-            PCI_CONTROLLER(slot->pci_bus)->companion) {
+		PCI_CONTROLLER(slot->pci_bus)->companion) {
 		unsigned long long adr;
 		struct acpi_device *device;
 		acpi_handle phandle;
diff --git a/drivers/pci/hotplug/shpchp.h b/drivers/pci/hotplug/shpchp.h
index 5897d516427b..4da8fc601467 100644
--- a/drivers/pci/hotplug/shpchp.h
+++ b/drivers/pci/hotplug/shpchp.h
@@ -50,14 +50,14 @@ extern bool shpchp_debug;
 #define dbg(format, arg...)						\
 do {									\
 	if (shpchp_debug)						\
-		printk(KERN_DEBUG "%s: " format, MY_NAME , ## arg);	\
+		printk(KERN_DEBUG "%s: " format, MY_NAME, ## arg);	\
 } while (0)
 #define err(format, arg...)						\
-	printk(KERN_ERR "%s: " format, MY_NAME , ## arg)
+	printk(KERN_ERR "%s: " format, MY_NAME, ## arg)
 #define info(format, arg...)						\
-	printk(KERN_INFO "%s: " format, MY_NAME , ## arg)
+	printk(KERN_INFO "%s: " format, MY_NAME, ## arg)
 #define warn(format, arg...)						\
-	printk(KERN_WARNING "%s: " format, MY_NAME , ## arg)
+	printk(KERN_WARNING "%s: " format, MY_NAME, ## arg)
 
 #define ctrl_dbg(ctrl, format, arg...)					\
 	do {								\
@@ -84,7 +84,7 @@ struct slot {
 	u8 presence_save;
 	u8 pwr_save;
 	struct controller *ctrl;
-	struct hpc_ops *hpc_ops;
+	const struct hpc_ops *hpc_ops;
 	struct hotplug_slot *hotplug_slot;
 	struct list_head	slot_list;
 	struct delayed_work work;	/* work for button event */
@@ -106,7 +106,7 @@ struct controller {
 	int slot_num_inc;		/* 1 or -1 */
 	struct pci_dev *pci_dev;
 	struct list_head slot_list;
-	struct hpc_ops *hpc_ops;
+	const struct hpc_ops *hpc_ops;
 	wait_queue_head_t queue;	/* sleep & wake process */
 	u8 slot_device_offset;
 	u32 pcix_misc2_reg;	/* for amd pogo errata */
@@ -295,7 +295,7 @@ static inline void amd_pogo_errata_restore_misc_reg(struct slot *p_slot)
 		pci_write_config_dword(p_slot->ctrl->pci_dev, PCIX_MEM_BASE_LIMIT_OFFSET, rse_set);
 	}
 	/* restore MiscII register */
-	pci_read_config_dword(p_slot->ctrl->pci_dev, PCIX_MISCII_OFFSET, &pcix_misc2_temp );
+	pci_read_config_dword(p_slot->ctrl->pci_dev, PCIX_MISCII_OFFSET, &pcix_misc2_temp);
 
 	if (p_slot->ctrl->pcix_misc2_reg & SERRFATALENABLE_MASK)
 		pcix_misc2_temp |= SERRFATALENABLE_MASK;
diff --git a/drivers/pci/hotplug/shpchp_core.c b/drivers/pci/hotplug/shpchp_core.c
index 294ef4b10cf1..3454dc7385f1 100644
--- a/drivers/pci/hotplug/shpchp_core.c
+++ b/drivers/pci/hotplug/shpchp_core.c
@@ -57,13 +57,13 @@ MODULE_PARM_DESC(shpchp_poll_time, "Polling mechanism frequency, in seconds");
 
 #define SHPC_MODULE_NAME "shpchp"
 
-static int set_attention_status (struct hotplug_slot *slot, u8 value);
-static int enable_slot		(struct hotplug_slot *slot);
-static int disable_slot		(struct hotplug_slot *slot);
-static int get_power_status	(struct hotplug_slot *slot, u8 *value);
-static int get_attention_status	(struct hotplug_slot *slot, u8 *value);
-static int get_latch_status	(struct hotplug_slot *slot, u8 *value);
-static int get_adapter_status	(struct hotplug_slot *slot, u8 *value);
+static int set_attention_status(struct hotplug_slot *slot, u8 value);
+static int enable_slot(struct hotplug_slot *slot);
+static int disable_slot(struct hotplug_slot *slot);
+static int get_power_status(struct hotplug_slot *slot, u8 *value);
+static int get_attention_status(struct hotplug_slot *slot, u8 *value);
+static int get_latch_status(struct hotplug_slot *slot, u8 *value);
+static int get_adapter_status(struct hotplug_slot *slot, u8 *value);
 
 static struct hotplug_slot_ops shpchp_hotplug_slot_ops = {
 	.set_attention_status =	set_attention_status,
@@ -178,12 +178,9 @@ error:
 
 void cleanup_slots(struct controller *ctrl)
 {
-	struct list_head *tmp;
-	struct list_head *next;
-	struct slot *slot;
+	struct slot *slot, *next;
 
-	list_for_each_safe(tmp, next, &ctrl->slot_list) {
-		slot = list_entry(tmp, struct slot, slot_list);
+	list_for_each_entry_safe(slot, next, &ctrl->slot_list, slot_list) {
 		list_del(&slot->slot_list);
 		cancel_delayed_work(&slot->work);
 		destroy_workqueue(slot->wq);
@@ -194,7 +191,7 @@ void cleanup_slots(struct controller *ctrl)
 /*
  * set_attention_status - Turns the Amber LED for a slot on, off or blink
  */
-static int set_attention_status (struct hotplug_slot *hotplug_slot, u8 status)
+static int set_attention_status(struct hotplug_slot *hotplug_slot, u8 status)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 
@@ -207,7 +204,7 @@ static int set_attention_status (struct hotplug_slot *hotplug_slot, u8 status)
 	return 0;
 }
 
-static int enable_slot (struct hotplug_slot *hotplug_slot)
+static int enable_slot(struct hotplug_slot *hotplug_slot)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 
@@ -217,7 +214,7 @@ static int enable_slot (struct hotplug_slot *hotplug_slot)
 	return shpchp_sysfs_enable_slot(slot);
 }
 
-static int disable_slot (struct hotplug_slot *hotplug_slot)
+static int disable_slot(struct hotplug_slot *hotplug_slot)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 
@@ -227,7 +224,7 @@ static int disable_slot (struct hotplug_slot *hotplug_slot)
 	return shpchp_sysfs_disable_slot(slot);
 }
 
-static int get_power_status (struct hotplug_slot *hotplug_slot, u8 *value)
+static int get_power_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 	int retval;
@@ -242,7 +239,7 @@ static int get_power_status (struct hotplug_slot *hotplug_slot, u8 *value)
 	return 0;
 }
 
-static int get_attention_status (struct hotplug_slot *hotplug_slot, u8 *value)
+static int get_attention_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 	int retval;
@@ -257,7 +254,7 @@ static int get_attention_status (struct hotplug_slot *hotplug_slot, u8 *value)
 	return 0;
 }
 
-static int get_latch_status (struct hotplug_slot *hotplug_slot, u8 *value)
+static int get_latch_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 	int retval;
@@ -272,7 +269,7 @@ static int get_latch_status (struct hotplug_slot *hotplug_slot, u8 *value)
 	return 0;
 }
 
-static int get_adapter_status (struct hotplug_slot *hotplug_slot, u8 *value)
+static int get_adapter_status(struct hotplug_slot *hotplug_slot, u8 *value)
 {
 	struct slot *slot = get_slot(hotplug_slot);
 	int retval;
diff --git a/drivers/pci/hotplug/shpchp_hpc.c b/drivers/pci/hotplug/shpchp_hpc.c
index 7d223e9080ef..de0ea474fb73 100644
--- a/drivers/pci/hotplug/shpchp_hpc.c
+++ b/drivers/pci/hotplug/shpchp_hpc.c
@@ -542,7 +542,7 @@ static int hpc_set_attention_status(struct slot *slot, u8 value)
 	u8 slot_cmd = 0;
 
 	switch (value) {
-		case 0 :
+		case 0:
 			slot_cmd = SET_ATTN_OFF;	/* OFF */
 			break;
 		case 1:
@@ -910,7 +910,7 @@ static int shpc_get_max_bus_speed(struct controller *ctrl)
 	return retval;
 }
 
-static struct hpc_ops shpchp_hpc_ops = {
+static const struct hpc_ops shpchp_hpc_ops = {
 	.power_on_slot			= hpc_power_on_slot,
 	.slot_enable			= hpc_slot_enable,
 	.slot_disable			= hpc_slot_disable,
diff --git a/drivers/pci/hotplug/shpchp_sysfs.c b/drivers/pci/hotplug/shpchp_sysfs.c
index 52875b360463..7efb56a28c9f 100644
--- a/drivers/pci/hotplug/shpchp_sysfs.c
+++ b/drivers/pci/hotplug/shpchp_sysfs.c
@@ -35,7 +35,7 @@
 
 /* A few routines that create sysfs entries for the hot plug controller */
 
-static ssize_t show_ctrl (struct device *dev, struct device_attribute *attr, char *buf)
+static ssize_t show_ctrl(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct pci_dev *pdev;
 	char *out = buf;
@@ -43,7 +43,7 @@ static ssize_t show_ctrl (struct device *dev, struct device_attribute *attr, cha
 	struct resource *res;
 	struct pci_bus *bus;
 
-	pdev = container_of (dev, struct pci_dev, dev);
+	pdev = to_pci_dev(dev);
 	bus = pdev->subordinate;
 
 	out += sprintf(buf, "Free resources: memory\n");
@@ -83,11 +83,11 @@ static ssize_t show_ctrl (struct device *dev, struct device_attribute *attr, cha
 
 	return out - buf;
 }
-static DEVICE_ATTR (ctrl, S_IRUGO, show_ctrl, NULL);
+static DEVICE_ATTR(ctrl, S_IRUGO, show_ctrl, NULL);
 
-int shpchp_create_ctrl_files (struct controller *ctrl)
+int shpchp_create_ctrl_files(struct controller *ctrl)
 {
-	return device_create_file (&ctrl->pci_dev->dev, &dev_attr_ctrl);
+	return device_create_file(&ctrl->pci_dev->dev, &dev_attr_ctrl);
 }
 
 void shpchp_remove_ctrl_files(struct controller *ctrl)
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 7eaa4c87fec7..a080f4496fe2 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -257,6 +257,7 @@ void pci_msi_mask_irq(struct irq_data *data)
 {
 	msi_set_mask_bit(data, 1);
 }
+EXPORT_SYMBOL_GPL(pci_msi_mask_irq);
 
 /**
  * pci_msi_unmask_irq - Generic irq chip callback to unmask PCI/MSI interrupts
@@ -266,6 +267,7 @@ void pci_msi_unmask_irq(struct irq_data *data)
 {
 	msi_set_mask_bit(data, 0);
 }
+EXPORT_SYMBOL_GPL(pci_msi_unmask_irq);
 
 void default_restore_msi_irqs(struct pci_dev *dev)
 {
@@ -1024,10 +1026,6 @@ int pci_msi_enabled(void)
 }
 EXPORT_SYMBOL(pci_msi_enabled);
 
-void pci_msi_init_pci_dev(struct pci_dev *dev)
-{
-}
-
 /**
  * pci_enable_msi_range - configure device's MSI capability structure
  * @dev: device to configure
@@ -1126,6 +1124,7 @@ struct pci_dev *msi_desc_to_pci_dev(struct msi_desc *desc)
 {
 	return to_pci_dev(desc->dev);
 }
+EXPORT_SYMBOL(msi_desc_to_pci_dev);
 
 void *msi_desc_to_pci_sysdata(struct msi_desc *desc)
 {
@@ -1285,6 +1284,7 @@ struct irq_domain *pci_msi_create_irq_domain(struct fwnode_handle *fwnode,
 	domain->bus_token = DOMAIN_BUS_PCI_MSI;
 	return domain;
 }
+EXPORT_SYMBOL_GPL(pci_msi_create_irq_domain);
 
 /**
  * pci_msi_domain_alloc_irqs - Allocate interrupts for @dev in @domain
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index a32ba753e413..9a033e8ee9a4 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -9,7 +9,9 @@
 
 #include <linux/delay.h>
 #include <linux/init.h>
+#include <linux/irqdomain.h>
 #include <linux/pci.h>
+#include <linux/msi.h>
 #include <linux/pci_hotplug.h>
 #include <linux/module.h>
 #include <linux/pci-aspm.h>
@@ -529,7 +531,7 @@ static bool acpi_pci_need_resume(struct pci_dev *dev)
 	return !!adev->power.flags.dsw_present;
 }
 
-static struct pci_platform_pm_ops acpi_pci_platform_pm = {
+static const struct pci_platform_pm_ops acpi_pci_platform_pm = {
 	.is_manageable = acpi_pci_power_manageable,
 	.set_state = acpi_pci_set_power_state,
 	.choose_state = acpi_pci_choose_state,
@@ -689,6 +691,46 @@ static struct acpi_bus_type acpi_pci_bus = {
 	.cleanup = pci_acpi_cleanup,
 };
 
+
+static struct fwnode_handle *(*pci_msi_get_fwnode_cb)(struct device *dev);
+
+/**
+ * pci_msi_register_fwnode_provider - Register callback to retrieve fwnode
+ * @fn:       Callback matching a device to a fwnode that identifies a PCI
+ *            MSI domain.
+ *
+ * This should be called by irqchip driver, which is the parent of
+ * the MSI domain to provide callback interface to query fwnode.
+ */
+void
+pci_msi_register_fwnode_provider(struct fwnode_handle *(*fn)(struct device *))
+{
+	pci_msi_get_fwnode_cb = fn;
+}
+
+/**
+ * pci_host_bridge_acpi_msi_domain - Retrieve MSI domain of a PCI host bridge
+ * @bus:      The PCI host bridge bus.
+ *
+ * This function uses the callback function registered by
+ * pci_msi_register_fwnode_provider() to retrieve the irq_domain with
+ * type DOMAIN_BUS_PCI_MSI of the specified host bridge bus.
+ * This returns NULL on error or when the domain is not found.
+ */
+struct irq_domain *pci_host_bridge_acpi_msi_domain(struct pci_bus *bus)
+{
+	struct fwnode_handle *fwnode;
+
+	if (!pci_msi_get_fwnode_cb)
+		return NULL;
+
+	fwnode = pci_msi_get_fwnode_cb(&bus->dev);
+	if (!fwnode)
+		return NULL;
+
+	return irq_find_matching_fwnode(fwnode, DOMAIN_BUS_PCI_MSI);
+}
+
 static int __init acpi_pci_init(void)
 {
 	int ret;
diff --git a/drivers/pci/pci-label.c b/drivers/pci/pci-label.c
index 024b5c179348..0ae74d96ed85 100644
--- a/drivers/pci/pci-label.c
+++ b/drivers/pci/pci-label.c
@@ -77,7 +77,7 @@ static umode_t smbios_instance_string_exist(struct kobject *kobj,
 	struct device *dev;
 	struct pci_dev *pdev;
 
-	dev = container_of(kobj, struct device, kobj);
+	dev = kobj_to_dev(kobj);
 	pdev = to_pci_dev(dev);
 
 	return find_smbios_instance_string(pdev, NULL, SMBIOS_ATTR_NONE) ?
@@ -221,7 +221,7 @@ static umode_t acpi_index_string_exist(struct kobject *kobj,
 {
 	struct device *dev;
 
-	dev = container_of(kobj, struct device, kobj);
+	dev = kobj_to_dev(kobj);
 
 	if (device_has_dsm(dev))
 		return S_IRUGO;
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index eead54cd01b2..95d9e7bd933b 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -630,8 +630,7 @@ static ssize_t pci_read_config(struct file *filp, struct kobject *kobj,
 			       struct bin_attribute *bin_attr, char *buf,
 			       loff_t off, size_t count)
 {
-	struct pci_dev *dev = to_pci_dev(container_of(kobj, struct device,
-						      kobj));
+	struct pci_dev *dev = to_pci_dev(kobj_to_dev(kobj));
 	unsigned int size = 64;
 	loff_t init_off = off;
 	u8 *data = (u8 *) buf;
@@ -707,8 +706,7 @@ static ssize_t pci_write_config(struct file *filp, struct kobject *kobj,
 				struct bin_attribute *bin_attr, char *buf,
 				loff_t off, size_t count)
 {
-	struct pci_dev *dev = to_pci_dev(container_of(kobj, struct device,
-						      kobj));
+	struct pci_dev *dev = to_pci_dev(kobj_to_dev(kobj));
 	unsigned int size = count;
 	loff_t init_off = off;
 	u8 *data = (u8 *) buf;
@@ -769,8 +767,7 @@ static ssize_t read_vpd_attr(struct file *filp, struct kobject *kobj,
 			     struct bin_attribute *bin_attr, char *buf,
 			     loff_t off, size_t count)
 {
-	struct pci_dev *dev =
-		to_pci_dev(container_of(kobj, struct device, kobj));
+	struct pci_dev *dev = to_pci_dev(kobj_to_dev(kobj));
 
 	if (off > bin_attr->size)
 		count = 0;
@@ -784,8 +781,7 @@ static ssize_t write_vpd_attr(struct file *filp, struct kobject *kobj,
 			      struct bin_attribute *bin_attr, char *buf,
 			      loff_t off, size_t count)
 {
-	struct pci_dev *dev =
-		to_pci_dev(container_of(kobj, struct device, kobj));
+	struct pci_dev *dev = to_pci_dev(kobj_to_dev(kobj));
 
 	if (off > bin_attr->size)
 		count = 0;
@@ -812,8 +808,7 @@ static ssize_t pci_read_legacy_io(struct file *filp, struct kobject *kobj,
 				  struct bin_attribute *bin_attr, char *buf,
 				  loff_t off, size_t count)
 {
-	struct pci_bus *bus = to_pci_bus(container_of(kobj, struct device,
-						      kobj));
+	struct pci_bus *bus = to_pci_bus(kobj_to_dev(kobj));
 
 	/* Only support 1, 2 or 4 byte accesses */
 	if (count != 1 && count != 2 && count != 4)
@@ -838,8 +833,7 @@ static ssize_t pci_write_legacy_io(struct file *filp, struct kobject *kobj,
 				   struct bin_attribute *bin_attr, char *buf,
 				   loff_t off, size_t count)
 {
-	struct pci_bus *bus = to_pci_bus(container_of(kobj, struct device,
-						      kobj));
+	struct pci_bus *bus = to_pci_bus(kobj_to_dev(kobj));
 
 	/* Only support 1, 2 or 4 byte accesses */
 	if (count != 1 && count != 2 && count != 4)
@@ -863,8 +857,7 @@ static int pci_mmap_legacy_mem(struct file *filp, struct kobject *kobj,
 			       struct bin_attribute *attr,
 			       struct vm_area_struct *vma)
 {
-	struct pci_bus *bus = to_pci_bus(container_of(kobj, struct device,
-						      kobj));
+	struct pci_bus *bus = to_pci_bus(kobj_to_dev(kobj));
 
 	return pci_mmap_legacy_page_range(bus, vma, pci_mmap_mem);
 }
@@ -884,8 +877,7 @@ static int pci_mmap_legacy_io(struct file *filp, struct kobject *kobj,
 			      struct bin_attribute *attr,
 			      struct vm_area_struct *vma)
 {
-	struct pci_bus *bus = to_pci_bus(container_of(kobj, struct device,
-						      kobj));
+	struct pci_bus *bus = to_pci_bus(kobj_to_dev(kobj));
 
 	return pci_mmap_legacy_page_range(bus, vma, pci_mmap_io);
 }
@@ -1000,8 +992,7 @@ int pci_mmap_fits(struct pci_dev *pdev, int resno, struct vm_area_struct *vma,
 static int pci_mmap_resource(struct kobject *kobj, struct bin_attribute *attr,
 			     struct vm_area_struct *vma, int write_combine)
 {
-	struct pci_dev *pdev = to_pci_dev(container_of(kobj,
-						       struct device, kobj));
+	struct pci_dev *pdev = to_pci_dev(kobj_to_dev(kobj));
 	struct resource *res = attr->private;
 	enum pci_mmap_state mmap_type;
 	resource_size_t start, end;
@@ -1054,8 +1045,7 @@ static ssize_t pci_resource_io(struct file *filp, struct kobject *kobj,
 			       struct bin_attribute *attr, char *buf,
 			       loff_t off, size_t count, bool write)
 {
-	struct pci_dev *pdev = to_pci_dev(container_of(kobj,
-						       struct device, kobj));
+	struct pci_dev *pdev = to_pci_dev(kobj_to_dev(kobj));
 	struct resource *res = attr->private;
 	unsigned long port = off;
 	int i;
@@ -1225,7 +1215,7 @@ static ssize_t pci_write_rom(struct file *filp, struct kobject *kobj,
 			     struct bin_attribute *bin_attr, char *buf,
 			     loff_t off, size_t count)
 {
-	struct pci_dev *pdev = to_pci_dev(container_of(kobj, struct device, kobj));
+	struct pci_dev *pdev = to_pci_dev(kobj_to_dev(kobj));
 
 	if ((off ==  0) && (*buf == '0') && (count == 2))
 		pdev->rom_attr_enabled = 0;
@@ -1251,7 +1241,7 @@ static ssize_t pci_read_rom(struct file *filp, struct kobject *kobj,
 			    struct bin_attribute *bin_attr, char *buf,
 			    loff_t off, size_t count)
 {
-	struct pci_dev *pdev = to_pci_dev(container_of(kobj, struct device, kobj));
+	struct pci_dev *pdev = to_pci_dev(kobj_to_dev(kobj));
 	void __iomem *rom;
 	size_t size;
 
@@ -1372,10 +1362,10 @@ int __must_check pci_create_sysfs_dev_files(struct pci_dev *pdev)
 	if (!sysfs_initialized)
 		return -EACCES;
 
-	if (pdev->cfg_size < PCI_CFG_SPACE_EXP_SIZE)
-		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pci_config_attr);
-	else
+	if (pdev->cfg_size > PCI_CFG_SPACE_SIZE)
 		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pcie_config_attr);
+	else
+		retval = sysfs_create_bin_file(&pdev->dev.kobj, &pci_config_attr);
 	if (retval)
 		goto err;
 
@@ -1427,10 +1417,10 @@ err_rom_file:
 err_resource_files:
 	pci_remove_resource_files(pdev);
 err_config_file:
-	if (pdev->cfg_size < PCI_CFG_SPACE_EXP_SIZE)
-		sysfs_remove_bin_file(&pdev->dev.kobj, &pci_config_attr);
-	else
+	if (pdev->cfg_size > PCI_CFG_SPACE_SIZE)
 		sysfs_remove_bin_file(&pdev->dev.kobj, &pcie_config_attr);
+	else
+		sysfs_remove_bin_file(&pdev->dev.kobj, &pci_config_attr);
 err:
 	return retval;
 }
@@ -1464,10 +1454,10 @@ void pci_remove_sysfs_dev_files(struct pci_dev *pdev)
 
 	pci_remove_capabilities_sysfs(pdev);
 
-	if (pdev->cfg_size < PCI_CFG_SPACE_EXP_SIZE)
-		sysfs_remove_bin_file(&pdev->dev.kobj, &pci_config_attr);
-	else
+	if (pdev->cfg_size > PCI_CFG_SPACE_SIZE)
 		sysfs_remove_bin_file(&pdev->dev.kobj, &pcie_config_attr);
+	else
+		sysfs_remove_bin_file(&pdev->dev.kobj, &pci_config_attr);
 
 	pci_remove_resource_files(pdev);
 
@@ -1511,7 +1501,7 @@ static struct attribute *pci_dev_dev_attrs[] = {
 static umode_t pci_dev_attrs_are_visible(struct kobject *kobj,
 					 struct attribute *a, int n)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct pci_dev *pdev = to_pci_dev(dev);
 
 	if (a == &vga_attr.attr)
@@ -1530,7 +1520,7 @@ static struct attribute *pci_dev_hp_attrs[] = {
 static umode_t pci_dev_hp_attrs_are_visible(struct kobject *kobj,
 					    struct attribute *a, int n)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 	struct pci_dev *pdev = to_pci_dev(dev);
 
 	if (pdev->is_virtfn)
@@ -1554,7 +1544,7 @@ static struct attribute *sriov_dev_attrs[] = {
 static umode_t sriov_attrs_are_visible(struct kobject *kobj,
 				       struct attribute *a, int n)
 {
-	struct device *dev = container_of(kobj, struct device, kobj);
+	struct device *dev = kobj_to_dev(kobj);
 
 	if (!dev_is_pf(dev))
 		return 0;
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 314db8c1047a..f89db3af0607 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -527,9 +527,9 @@ static void pci_restore_bars(struct pci_dev *dev)
 		pci_update_resource(dev, i);
 }
 
-static struct pci_platform_pm_ops *pci_platform_pm;
+static const struct pci_platform_pm_ops *pci_platform_pm;
 
-int pci_set_platform_pm(struct pci_platform_pm_ops *ops)
+int pci_set_platform_pm(const struct pci_platform_pm_ops *ops)
 {
 	if (!ops->is_manageable || !ops->set_state || !ops->choose_state
 	    || !ops->sleep_wake)
@@ -1417,7 +1417,7 @@ struct pci_devres {
 
 static void pcim_release(struct device *gendev, void *res)
 {
-	struct pci_dev *dev = container_of(gendev, struct pci_dev, dev);
+	struct pci_dev *dev = to_pci_dev(gendev);
 	struct pci_devres *this = res;
 	int i;
 
@@ -1534,7 +1534,7 @@ void __weak pcibios_release_device(struct pci_dev *dev) {}
  * is the default implementation. Architecture implementations can
  * override this.
  */
-void __weak pcibios_disable_device (struct pci_dev *dev) {}
+void __weak pcibios_disable_device(struct pci_dev *dev) {}
 
 /**
  * pcibios_penalize_isa_irq - penalize an ISA IRQ
@@ -4772,8 +4772,10 @@ int pci_get_new_domain_nr(void)
 void pci_bus_assign_domain_nr(struct pci_bus *bus, struct device *parent)
 {
 	static int use_dt_domains = -1;
-	int domain = of_get_pci_domain_nr(parent->of_node);
+	int domain = -1;
 
+	if (parent)
+		domain = of_get_pci_domain_nr(parent->of_node);
 	/*
 	 * Check DT domain and use_dt_domains values.
 	 *
diff --git a/drivers/pci/pci.h b/drivers/pci/pci.h
index d390fc1475ec..9a1660f592ef 100644
--- a/drivers/pci/pci.h
+++ b/drivers/pci/pci.h
@@ -68,7 +68,7 @@ struct pci_platform_pm_ops {
 	bool (*need_resume)(struct pci_dev *dev);
 };
 
-int pci_set_platform_pm(struct pci_platform_pm_ops *ops);
+int pci_set_platform_pm(const struct pci_platform_pm_ops *ops);
 void pci_update_current_state(struct pci_dev *dev, pci_power_t state);
 void pci_power_up(struct pci_dev *dev);
 void pci_disable_enabled_device(struct pci_dev *dev);
@@ -144,10 +144,8 @@ extern unsigned int pci_pm_d3_delay;
 
 #ifdef CONFIG_PCI_MSI
 void pci_no_msi(void);
-void pci_msi_init_pci_dev(struct pci_dev *dev);
 #else
 static inline void pci_no_msi(void) { }
-static inline void pci_msi_init_pci_dev(struct pci_dev *dev) { }
 #endif
 
 static inline void pci_msi_set_enable(struct pci_dev *dev, int enable)
diff --git a/drivers/pci/pcie/aer/aer_inject.c b/drivers/pci/pcie/aer/aer_inject.c
index 182224acedbe..20db790465dd 100644
--- a/drivers/pci/pcie/aer/aer_inject.c
+++ b/drivers/pci/pcie/aer/aer_inject.c
@@ -41,12 +41,12 @@ struct aer_error_inj {
 	u32 header_log1;
 	u32 header_log2;
 	u32 header_log3;
-	u16 domain;
+	u32 domain;
 };
 
 struct aer_error {
 	struct list_head list;
-	u16 domain;
+	u32 domain;
 	unsigned int bus;
 	unsigned int devfn;
 	int pos_cap_err;
@@ -74,7 +74,7 @@ static LIST_HEAD(pci_bus_ops_list);
 /* Protect einjected and pci_bus_ops_list */
 static DEFINE_SPINLOCK(inject_lock);
 
-static void aer_error_init(struct aer_error *err, u16 domain,
+static void aer_error_init(struct aer_error *err, u32 domain,
 			   unsigned int bus, unsigned int devfn,
 			   int pos_cap_err)
 {
@@ -86,7 +86,7 @@ static void aer_error_init(struct aer_error *err, u16 domain,
 }
 
 /* inject_lock must be held before calling */
-static struct aer_error *__find_aer_error(u16 domain, unsigned int bus,
+static struct aer_error *__find_aer_error(u32 domain, unsigned int bus,
 					  unsigned int devfn)
 {
 	struct aer_error *err;
@@ -106,7 +106,7 @@ static struct aer_error *__find_aer_error_by_dev(struct pci_dev *dev)
 	int domain = pci_domain_nr(dev->bus);
 	if (domain < 0)
 		return NULL;
-	return __find_aer_error((u16)domain, dev->bus->number, dev->devfn);
+	return __find_aer_error(domain, dev->bus->number, dev->devfn);
 }
 
 /* inject_lock must be held before calling */
@@ -196,7 +196,7 @@ static int pci_read_aer(struct pci_bus *bus, unsigned int devfn, int where,
 	domain = pci_domain_nr(bus);
 	if (domain < 0)
 		goto out;
-	err = __find_aer_error((u16)domain, bus->number, devfn);
+	err = __find_aer_error(domain, bus->number, devfn);
 	if (!err)
 		goto out;
 
@@ -228,7 +228,7 @@ static int pci_write_aer(struct pci_bus *bus, unsigned int devfn, int where,
 	domain = pci_domain_nr(bus);
 	if (domain < 0)
 		goto out;
-	err = __find_aer_error((u16)domain, bus->number, devfn);
+	err = __find_aer_error(domain, bus->number, devfn);
 	if (!err)
 		goto out;
 
@@ -329,7 +329,7 @@ static int aer_inject(struct aer_error_inj *einj)
 	u32 sever, cor_mask, uncor_mask, cor_mask_orig = 0, uncor_mask_orig = 0;
 	int ret = 0;
 
-	dev = pci_get_domain_bus_and_slot((int)einj->domain, einj->bus, devfn);
+	dev = pci_get_domain_bus_and_slot(einj->domain, einj->bus, devfn);
 	if (!dev)
 		return -ENODEV;
 	rpdev = pcie_find_root_port(dev);
diff --git a/drivers/pci/pcie/aer/aerdrv.c b/drivers/pci/pcie/aer/aerdrv.c
index 0bf82a20a0fb..48d21e0edd56 100644
--- a/drivers/pci/pcie/aer/aerdrv.c
+++ b/drivers/pci/pcie/aer/aerdrv.c
@@ -262,7 +262,6 @@ static struct aer_rpc *aer_alloc_rpc(struct pcie_device *dev)
 	rpc->rpd = dev;
 	INIT_WORK(&rpc->dpc_handler, aer_isr);
 	mutex_init(&rpc->rpc_mutex);
-	init_waitqueue_head(&rpc->wait_release);
 
 	/* Use PCIe bus function to store rpc into PCIe device */
 	set_service_data(dev, rpc);
@@ -285,8 +284,7 @@ static void aer_remove(struct pcie_device *dev)
 		if (rpc->isr)
 			free_irq(dev->irq, dev);
 
-		wait_event(rpc->wait_release, rpc->prod_idx == rpc->cons_idx);
-
+		flush_work(&rpc->dpc_handler);
 		aer_disable_rootport(rpc);
 		kfree(rpc);
 		set_service_data(dev, NULL);
diff --git a/drivers/pci/pcie/aer/aerdrv.h b/drivers/pci/pcie/aer/aerdrv.h
index 84420b7c9456..945c939a86c5 100644
--- a/drivers/pci/pcie/aer/aerdrv.h
+++ b/drivers/pci/pcie/aer/aerdrv.h
@@ -72,7 +72,6 @@ struct aer_rpc {
 					 * recovery on the same
 					 * root port hierarchy
 					 */
-	wait_queue_head_t wait_release;
 };
 
 struct aer_broadcast_data {
diff --git a/drivers/pci/pcie/aer/aerdrv_core.c b/drivers/pci/pcie/aer/aerdrv_core.c
index fba785e9df75..521e39c1b66d 100644
--- a/drivers/pci/pcie/aer/aerdrv_core.c
+++ b/drivers/pci/pcie/aer/aerdrv_core.c
@@ -246,7 +246,7 @@ static int report_error_detected(struct pci_dev *dev, void *data)
 		!dev->driver->err_handler ||
 		!dev->driver->err_handler->error_detected) {
 		if (result_data->state == pci_channel_io_frozen &&
-			!(dev->hdr_type & PCI_HEADER_TYPE_BRIDGE)) {
+			dev->hdr_type != PCI_HEADER_TYPE_BRIDGE) {
 			/*
 			 * In case of fatal recovery, if one of down-
 			 * stream device has no driver. We might be
@@ -269,7 +269,7 @@ static int report_error_detected(struct pci_dev *dev, void *data)
 		 * without recovery.
 		 */
 
-		if (!(dev->hdr_type & PCI_HEADER_TYPE_BRIDGE))
+		if (dev->hdr_type != PCI_HEADER_TYPE_BRIDGE)
 			vote = PCI_ERS_RESULT_NO_AER_DRIVER;
 		else
 			vote = PCI_ERS_RESULT_NONE;
@@ -369,7 +369,7 @@ static pci_ers_result_t broadcast_error_message(struct pci_dev *dev,
 	else
 		result_data.result = PCI_ERS_RESULT_RECOVERED;
 
-	if (dev->hdr_type & PCI_HEADER_TYPE_BRIDGE) {
+	if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) {
 		/*
 		 * If the error is reported by a bridge, we think this error
 		 * is related to the downstream link of the bridge, so we
@@ -440,7 +440,7 @@ static pci_ers_result_t reset_link(struct pci_dev *dev)
 	pci_ers_result_t status;
 	struct pcie_port_service_driver *driver;
 
-	if (dev->hdr_type & PCI_HEADER_TYPE_BRIDGE) {
+	if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE) {
 		/* Reset this port for all subordinates */
 		udev = dev;
 	} else {
@@ -660,7 +660,7 @@ static int get_device_error_info(struct pci_dev *dev, struct aer_err_info *info)
 			&info->mask);
 		if (!(info->status & ~info->mask))
 			return 0;
-	} else if (dev->hdr_type & PCI_HEADER_TYPE_BRIDGE ||
+	} else if (dev->hdr_type == PCI_HEADER_TYPE_BRIDGE ||
 		info->severity == AER_NONFATAL) {
 
 		/* Link is still healthy for IO reads */
@@ -811,8 +811,6 @@ void aer_isr(struct work_struct *work)
 	while (get_e_source(rpc, &e_src))
 		aer_isr_one_error(p_device, &e_src);
 	mutex_unlock(&rpc->rpc_mutex);
-
-	wake_up(&rpc->wait_release);
 }
 
 /**
diff --git a/drivers/pci/pcie/aspm.c b/drivers/pci/pcie/aspm.c
index 317e3558a35e..2dfe7fdb77e7 100644
--- a/drivers/pci/pcie/aspm.c
+++ b/drivers/pci/pcie/aspm.c
@@ -834,21 +834,15 @@ static ssize_t link_state_store(struct device *dev,
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct pcie_link_state *link, *root = pdev->link_state->root;
-	u32 val, state = 0;
-
-	if (kstrtouint(buf, 10, &val))
-		return -EINVAL;
+	u32 state;
 
 	if (aspm_disabled)
 		return -EPERM;
-	if (n < 1 || val > 3)
-		return -EINVAL;
 
-	/* Convert requested state to ASPM state */
-	if (val & PCIE_LINK_STATE_L0S)
-		state |= ASPM_STATE_L0S;
-	if (val & PCIE_LINK_STATE_L1)
-		state |= ASPM_STATE_L1;
+	if (kstrtouint(buf, 10, &state))
+		return -EINVAL;
+	if ((state & ~ASPM_STATE_ALL) != 0)
+		return -EINVAL;
 
 	down_read(&pci_bus_sem);
 	mutex_lock(&aspm_lock);
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index edb1984201e9..6d7ab9bb0d5a 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -672,6 +672,8 @@ static struct irq_domain *pci_host_bridge_msi_domain(struct pci_bus *bus)
 	 * should be called from here.
 	 */
 	d = pci_host_bridge_of_msi_domain(bus);
+	if (!d)
+		d = pci_host_bridge_acpi_msi_domain(bus);
 
 	return d;
 }
@@ -1107,14 +1109,11 @@ static int pci_cfg_space_size_ext(struct pci_dev *dev)
 	int pos = PCI_CFG_SPACE_SIZE;
 
 	if (pci_read_config_dword(dev, pos, &status) != PCIBIOS_SUCCESSFUL)
-		goto fail;
+		return PCI_CFG_SPACE_SIZE;
 	if (status == 0xffffffff || pci_ext_cfg_is_aliased(dev))
-		goto fail;
+		return PCI_CFG_SPACE_SIZE;
 
 	return PCI_CFG_SPACE_EXP_SIZE;
-
- fail:
-	return PCI_CFG_SPACE_SIZE;
 }
 
 int pci_cfg_space_size(struct pci_dev *dev)
@@ -1127,25 +1126,23 @@ int pci_cfg_space_size(struct pci_dev *dev)
 	if (class == PCI_CLASS_BRIDGE_HOST)
 		return pci_cfg_space_size_ext(dev);
 
-	if (!pci_is_pcie(dev)) {
-		pos = pci_find_capability(dev, PCI_CAP_ID_PCIX);
-		if (!pos)
-			goto fail;
+	if (pci_is_pcie(dev))
+		return pci_cfg_space_size_ext(dev);
 
-		pci_read_config_dword(dev, pos + PCI_X_STATUS, &status);
-		if (!(status & (PCI_X_STATUS_266MHZ | PCI_X_STATUS_533MHZ)))
-			goto fail;
-	}
+	pos = pci_find_capability(dev, PCI_CAP_ID_PCIX);
+	if (!pos)
+		return PCI_CFG_SPACE_SIZE;
 
-	return pci_cfg_space_size_ext(dev);
+	pci_read_config_dword(dev, pos + PCI_X_STATUS, &status);
+	if (status & (PCI_X_STATUS_266MHZ | PCI_X_STATUS_533MHZ))
+		return pci_cfg_space_size_ext(dev);
 
- fail:
 	return PCI_CFG_SPACE_SIZE;
 }
 
 #define LEGACY_IO_RESOURCE	(IORESOURCE_IO | IORESOURCE_PCI_FIXED)
 
-void pci_msi_setup_pci_dev(struct pci_dev *dev)
+static void pci_msi_setup_pci_dev(struct pci_dev *dev)
 {
 	/*
 	 * Disable the MSI hardware to avoid screaming interrupts
@@ -1212,8 +1209,6 @@ int pci_setup_device(struct pci_dev *dev)
 	/* "Unknown power state" */
 	dev->current_state = PCI_UNKNOWN;
 
-	pci_msi_setup_pci_dev(dev);
-
 	/* Early fixups, before probing the BARs */
 	pci_fixup_device(pci_fixup_early, dev);
 	/* device class may be changed after fixup */
@@ -1603,8 +1598,8 @@ static void pci_init_capabilities(struct pci_dev *dev)
 	/* Enhanced Allocation */
 	pci_ea_init(dev);
 
-	/* MSI/MSI-X list */
-	pci_msi_init_pci_dev(dev);
+	/* Setup MSI caps & disable MSI/MSI-X interrupts */
+	pci_msi_setup_pci_dev(dev);
 
 	/* Buffers for saving PCIe and PCI-X capabilities */
 	pci_allocate_cap_save_buffers(dev);
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 7e327309cf69..0575a1e026b4 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -287,6 +287,18 @@ static void quirk_citrine(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_IBM,	PCI_DEVICE_ID_IBM_CITRINE,	quirk_citrine);
 
+/*
+ * This chip can cause bus lockups if config addresses above 0x600
+ * are read or written.
+ */
+static void quirk_nfp6000(struct pci_dev *dev)
+{
+	dev->cfg_size = 0x600;
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_NETRONOME,	PCI_DEVICE_ID_NETRONOME_NFP4000,	quirk_nfp6000);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_NETRONOME,	PCI_DEVICE_ID_NETRONOME_NFP6000,	quirk_nfp6000);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_NETRONOME,	PCI_DEVICE_ID_NETRONOME_NFP6000_VF,	quirk_nfp6000);
+
 /*  On IBM Crocodile ipr SAS adapters, expand BAR to system page size */
 static void quirk_extend_bar_to_page(struct pci_dev *dev)
 {
@@ -3405,7 +3417,9 @@ static int reset_intel_82599_sfp_virtfn(struct pci_dev *dev, int probe)
 	return 0;
 }
 
-#include "../gpu/drm/i915/i915_reg.h"
+#define SOUTH_CHICKEN2		0xc2004
+#define PCH_PP_STATUS		0xc7200
+#define PCH_PP_CONTROL		0xc7204
 #define MSG_CTL			0x45010
 #define NSDE_PWR_STATE		0xd0100
 #define IGD_OPERATION_TIMEOUT	10000     /* set timeout 10 seconds */
@@ -3620,6 +3634,10 @@ DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_TTI, 0x0642,
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_JMICRON,
 			 PCI_DEVICE_ID_JMICRON_JMB388_ESD,
 			 quirk_dma_func1_alias);
+/* https://bugzilla.kernel.org/show_bug.cgi?id=42679#c117 */
+DECLARE_PCI_FIXUP_HEADER(0x1c28, /* Lite-On */
+			 0x0122, /* Plextor M6E (Marvell 88SS9183)*/
+			 quirk_dma_func1_alias);
 
 /*
  * Some devices DMA with the wrong devfn, not just the wrong function.
diff --git a/drivers/pci/rom.c b/drivers/pci/rom.c
index eb0ad530dc43..9eaca39ef38d 100644
--- a/drivers/pci/rom.c
+++ b/drivers/pci/rom.c
@@ -77,25 +77,24 @@ size_t pci_get_rom_size(struct pci_dev *pdev, void __iomem *rom, size_t size)
 	do {
 		void __iomem *pds;
 		/* Standard PCI ROMs start out with these bytes 55 AA */
-		if (readb(image) != 0x55) {
-			dev_err(&pdev->dev, "Invalid ROM contents\n");
+		if (readw(image) != 0xAA55) {
+			dev_err(&pdev->dev, "Invalid PCI ROM header signature: expecting 0xaa55, got %#06x\n",
+				readw(image));
 			break;
 		}
-		if (readb(image + 1) != 0xAA)
-			break;
-		/* get the PCI data structure and check its signature */
+		/* get the PCI data structure and check its "PCIR" signature */
 		pds = image + readw(image + 24);
-		if (readb(pds) != 'P')
-			break;
-		if (readb(pds + 1) != 'C')
-			break;
-		if (readb(pds + 2) != 'I')
-			break;
-		if (readb(pds + 3) != 'R')
+		if (readl(pds) != 0x52494350) {
+			dev_err(&pdev->dev, "Invalid PCI ROM data signature: expecting 0x52494350, got %#010x\n",
+				readl(pds));
 			break;
+		}
 		last_image = readb(pds + 21) & 0x80;
 		length = readw(pds + 16);
 		image += length * 512;
+		/* Avoid iterating through memory outside the resource window */
+		if (image > rom + size)
+			break;
 	} while (length && !last_image);
 
 	/* never return a size larger than the PCI resource window */
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 1723ac1b30e1..7796d0a5befa 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -442,7 +442,7 @@ static void __assign_resources_sorted(struct list_head *head,
 					break;
 				}
 			}
-               }
+		}
 
 	}
 
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index c777b97207d5..5f70fee59a94 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -53,7 +53,7 @@ struct pcifront_device {
 };
 
 struct pcifront_sd {
-	int domain;
+	struct pci_sysdata sd;
 	struct pcifront_device *pdev;
 };
 
@@ -67,7 +67,9 @@ static inline void pcifront_init_sd(struct pcifront_sd *sd,
 				    unsigned int domain, unsigned int bus,
 				    struct pcifront_device *pdev)
 {
-	sd->domain = domain;
+	/* Because we do not expose that information via XenBus. */
+	sd->sd.node = first_online_node;
+	sd->sd.domain = domain;
 	sd->pdev = pdev;
 }
 
@@ -468,8 +470,8 @@ static int pcifront_scan_root(struct pcifront_device *pdev,
 	dev_info(&pdev->xdev->dev, "Creating PCI Frontend Bus %04x:%02x\n",
 		 domain, bus);
 
-	bus_entry = kmalloc(sizeof(*bus_entry), GFP_KERNEL);
-	sd = kmalloc(sizeof(*sd), GFP_KERNEL);
+	bus_entry = kzalloc(sizeof(*bus_entry), GFP_KERNEL);
+	sd = kzalloc(sizeof(*sd), GFP_KERNEL);
 	if (!bus_entry || !sd) {
 		err = -ENOMEM;
 		goto err_out;
diff --git a/drivers/perf/arm_pmu.c b/drivers/perf/arm_pmu.c
index be3755c973e9..166637f2917c 100644
--- a/drivers/perf/arm_pmu.c
+++ b/drivers/perf/arm_pmu.c
@@ -551,14 +551,6 @@ static void armpmu_init(struct arm_pmu *armpmu)
 	};
 }
 
-int armpmu_register(struct arm_pmu *armpmu, int type)
-{
-	armpmu_init(armpmu);
-	pr_info("enabled with %s PMU driver, %d counters available\n",
-			armpmu->name, armpmu->num_events);
-	return perf_pmu_register(&armpmu->pmu, armpmu->name, type);
-}
-
 /* Set at runtime when we know what CPU type we are. */
 static struct arm_pmu *__oprofile_cpu_pmu;
 
@@ -887,6 +879,8 @@ int arm_pmu_device_probe(struct platform_device *pdev,
 		return -ENOMEM;
 	}
 
+	armpmu_init(pmu);
+
 	if (!__oprofile_cpu_pmu)
 		__oprofile_cpu_pmu = pmu;
 
@@ -912,10 +906,13 @@ int arm_pmu_device_probe(struct platform_device *pdev,
 	if (ret)
 		goto out_free;
 
-	ret = armpmu_register(pmu, -1);
+	ret = perf_pmu_register(&pmu->pmu, pmu->name, -1);
 	if (ret)
 		goto out_destroy;
 
+	pr_info("enabled with %s PMU driver, %d counters available\n",
+			pmu->name, pmu->num_events);
+
 	return 0;
 
 out_destroy:
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 03cb3ea2d2c0..0124d17bd9fe 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -118,6 +118,13 @@ config PHY_RCAR_GEN2
 	help
 	  Support for USB PHY found on Renesas R-Car generation 2 SoCs.
 
+config PHY_RCAR_GEN3_USB2
+	tristate "Renesas R-Car generation 3 USB 2.0 PHY driver"
+	depends on OF && ARCH_SHMOBILE
+	select GENERIC_PHY
+	help
+	  Support for USB 2.0 PHY found on Renesas R-Car generation 3 SoCs.
+
 config OMAP_CONTROL_PHY
 	tristate "OMAP CONTROL PHY Driver"
 	depends on ARCH_OMAP2PLUS || COMPILE_TEST
@@ -215,6 +222,16 @@ config PHY_MT65XX_USB3
 	  for mt65xx SoCs. it supports two usb2.0 ports and
 	  one usb3.0 port.
 
+config PHY_HI6220_USB
+	tristate "hi6220 USB PHY support"
+	depends on (ARCH_HISI && ARM64) || COMPILE_TEST
+	select GENERIC_PHY
+	select MFD_SYSCON
+	help
+	  Enable this to support the HISILICON HI6220 USB PHY.
+
+	  To compile this driver as a module, choose M here.
+
 config PHY_SUN4I_USB
 	tristate "Allwinner sunxi SoC USB PHY driver"
 	depends on ARCH_SUNXI && HAS_IOMEM && OF
@@ -374,11 +391,11 @@ config PHY_TUSB1210
 
 config PHY_BRCMSTB_SATA
 	tristate "Broadcom STB SATA PHY driver"
-	depends on ARCH_BRCMSTB
+	depends on ARCH_BRCMSTB || BMIPS_GENERIC
 	depends on OF
 	select GENERIC_PHY
 	help
-	  Enable this to support the SATA3 PHY on 28nm Broadcom STB SoCs.
+	  Enable this to support the SATA3 PHY on 28nm or 40nm Broadcom STB SoCs.
 	  Likely useful only with CONFIG_SATA_BRCMSTB enabled.
 
 config PHY_CYGNUS_PCIE
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 075db1a81aa5..c80f09df3bb8 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -17,12 +17,14 @@ obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
 obj-$(CONFIG_PHY_MIPHY28LP) 		+= phy-miphy28lp.o
 obj-$(CONFIG_PHY_MIPHY365X)		+= phy-miphy365x.o
 obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
+obj-$(CONFIG_PHY_RCAR_GEN3_USB2)	+= phy-rcar-gen3-usb2.o
 obj-$(CONFIG_OMAP_CONTROL_PHY)		+= phy-omap-control.o
 obj-$(CONFIG_OMAP_USB2)			+= phy-omap-usb2.o
 obj-$(CONFIG_TI_PIPE3)			+= phy-ti-pipe3.o
 obj-$(CONFIG_TWL4030_USB)		+= phy-twl4030-usb.o
 obj-$(CONFIG_PHY_EXYNOS5250_SATA)	+= phy-exynos5250-sata.o
 obj-$(CONFIG_PHY_HIX5HD2_SATA)		+= phy-hix5hd2-sata.o
+obj-$(CONFIG_PHY_HI6220_USB)		+= phy-hi6220-usb.o
 obj-$(CONFIG_PHY_MT65XX_USB3)		+= phy-mt65xx-usb3.o
 obj-$(CONFIG_PHY_SUN4I_USB)		+= phy-sun4i-usb.o
 obj-$(CONFIG_PHY_SUN9I_USB)		+= phy-sun9i-usb.o
diff --git a/drivers/phy/phy-berlin-usb.c b/drivers/phy/phy-berlin-usb.c
index 797ba17c404f..2017751ede26 100644
--- a/drivers/phy/phy-berlin-usb.c
+++ b/drivers/phy/phy-berlin-usb.c
@@ -9,11 +9,9 @@
  * warranty of any kind, whether express or implied.
  */
 
-#include <linux/gpio.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
-#include <linux/of_gpio.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
@@ -195,7 +193,6 @@ static int phy_berlin_usb_probe(struct platform_device *pdev)
 		return PTR_ERR(phy);
 	}
 
-	platform_set_drvdata(pdev, priv);
 	phy_set_drvdata(phy, priv);
 
 	phy_provider =
diff --git a/drivers/phy/phy-brcmstb-sata.c b/drivers/phy/phy-brcmstb-sata.c
index cd9dba820566..a23172ff40e3 100644
--- a/drivers/phy/phy-brcmstb-sata.c
+++ b/drivers/phy/phy-brcmstb-sata.c
@@ -26,13 +26,21 @@
 
 #define SATA_MDIO_BANK_OFFSET				0x23c
 #define SATA_MDIO_REG_OFFSET(ofs)			((ofs) * 4)
-#define SATA_MDIO_REG_SPACE_SIZE			0x1000
-#define SATA_MDIO_REG_LENGTH				0x1f00
 
 #define MAX_PORTS					2
 
 /* Register offset between PHYs in PCB space */
-#define SATA_MDIO_REG_SPACE_SIZE			0x1000
+#define SATA_MDIO_REG_28NM_SPACE_SIZE			0x1000
+
+/* The older SATA PHY registers duplicated per port registers within the map,
+ * rather than having a separate map per port.
+ */
+#define SATA_MDIO_REG_40NM_SPACE_SIZE			0x10
+
+enum brcm_sata_phy_version {
+	BRCM_SATA_PHY_28NM,
+	BRCM_SATA_PHY_40NM,
+};
 
 struct brcm_sata_port {
 	int portnum;
@@ -44,11 +52,12 @@ struct brcm_sata_port {
 struct brcm_sata_phy {
 	struct device *dev;
 	void __iomem *phy_base;
+	enum brcm_sata_phy_version version;
 
 	struct brcm_sata_port phys[MAX_PORTS];
 };
 
-enum sata_mdio_phy_regs_28nm {
+enum sata_mdio_phy_regs {
 	PLL_REG_BANK_0				= 0x50,
 	PLL_REG_BANK_0_PLLCONTROL_0		= 0x81,
 
@@ -66,8 +75,16 @@ enum sata_mdio_phy_regs_28nm {
 static inline void __iomem *brcm_sata_phy_base(struct brcm_sata_port *port)
 {
 	struct brcm_sata_phy *priv = port->phy_priv;
+	u32 offset = 0;
+
+	if (priv->version == BRCM_SATA_PHY_28NM)
+		offset = SATA_MDIO_REG_28NM_SPACE_SIZE;
+	else if (priv->version == BRCM_SATA_PHY_40NM)
+		offset = SATA_MDIO_REG_40NM_SPACE_SIZE;
+	else
+		dev_err(priv->dev, "invalid phy version\n");
 
-	return priv->phy_base + (port->portnum * SATA_MDIO_REG_SPACE_SIZE);
+	return priv->phy_base + (port->portnum * offset);
 }
 
 static void brcm_sata_mdio_wr(void __iomem *addr, u32 bank, u32 ofs,
@@ -86,7 +103,7 @@ static void brcm_sata_mdio_wr(void __iomem *addr, u32 bank, u32 ofs,
 #define FMAX_VAL_DEFAULT	0x3df
 #define FMAX_VAL_SSC		0x83
 
-static void brcm_sata_cfg_ssc_28nm(struct brcm_sata_port *port)
+static void brcm_sata_cfg_ssc(struct brcm_sata_port *port)
 {
 	void __iomem *base = brcm_sata_phy_base(port);
 	struct brcm_sata_phy *priv = port->phy_priv;
@@ -117,18 +134,21 @@ static int brcm_sata_phy_init(struct phy *phy)
 {
 	struct brcm_sata_port *port = phy_get_drvdata(phy);
 
-	brcm_sata_cfg_ssc_28nm(port);
+	brcm_sata_cfg_ssc(port);
 
 	return 0;
 }
 
-static const struct phy_ops phy_ops_28nm = {
+static const struct phy_ops phy_ops = {
 	.init		= brcm_sata_phy_init,
 	.owner		= THIS_MODULE,
 };
 
 static const struct of_device_id brcm_sata_phy_of_match[] = {
-	{ .compatible	= "brcm,bcm7445-sata-phy" },
+	{ .compatible	= "brcm,bcm7445-sata-phy",
+	  .data = (void *)BRCM_SATA_PHY_28NM },
+	{ .compatible	= "brcm,bcm7425-sata-phy",
+	  .data = (void *)BRCM_SATA_PHY_40NM },
 	{},
 };
 MODULE_DEVICE_TABLE(of, brcm_sata_phy_of_match);
@@ -137,6 +157,7 @@ static int brcm_sata_phy_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct device_node *dn = dev->of_node, *child;
+	const struct of_device_id *of_id;
 	struct brcm_sata_phy *priv;
 	struct resource *res;
 	struct phy_provider *provider;
@@ -156,6 +177,12 @@ static int brcm_sata_phy_probe(struct platform_device *pdev)
 	if (IS_ERR(priv->phy_base))
 		return PTR_ERR(priv->phy_base);
 
+	of_id = of_match_node(brcm_sata_phy_of_match, dn);
+	if (of_id)
+		priv->version = (enum brcm_sata_phy_version)of_id->data;
+	else
+		priv->version = BRCM_SATA_PHY_28NM;
+
 	for_each_available_child_of_node(dn, child) {
 		unsigned int id;
 		struct brcm_sata_port *port;
@@ -181,7 +208,7 @@ static int brcm_sata_phy_probe(struct platform_device *pdev)
 		port = &priv->phys[id];
 		port->portnum = id;
 		port->phy_priv = priv;
-		port->phy = devm_phy_create(dev, child, &phy_ops_28nm);
+		port->phy = devm_phy_create(dev, child, &phy_ops);
 		port->ssc_en = of_property_read_bool(child, "brcm,enable-ssc");
 		if (IS_ERR(port->phy)) {
 			dev_err(dev, "failed to create PHY\n");
diff --git a/drivers/phy/phy-core.c b/drivers/phy/phy-core.c
index 8c7f27db6ad3..e7e574dc667a 100644
--- a/drivers/phy/phy-core.c
+++ b/drivers/phy/phy-core.c
@@ -275,20 +275,21 @@ EXPORT_SYMBOL_GPL(phy_exit);
 
 int phy_power_on(struct phy *phy)
 {
-	int ret;
+	int ret = 0;
 
 	if (!phy)
-		return 0;
+		goto out;
 
 	if (phy->pwr) {
 		ret = regulator_enable(phy->pwr);
 		if (ret)
-			return ret;
+			goto out;
 	}
 
 	ret = phy_pm_runtime_get_sync(phy);
 	if (ret < 0 && ret != -ENOTSUPP)
-		return ret;
+		goto err_pm_sync;
+
 	ret = 0; /* Override possible ret == -ENOTSUPP */
 
 	mutex_lock(&phy->mutex);
@@ -296,19 +297,20 @@ int phy_power_on(struct phy *phy)
 		ret = phy->ops->power_on(phy);
 		if (ret < 0) {
 			dev_err(&phy->dev, "phy poweron failed --> %d\n", ret);
-			goto out;
+			goto err_pwr_on;
 		}
 	}
 	++phy->power_count;
 	mutex_unlock(&phy->mutex);
 	return 0;
 
-out:
+err_pwr_on:
 	mutex_unlock(&phy->mutex);
 	phy_pm_runtime_put_sync(phy);
+err_pm_sync:
 	if (phy->pwr)
 		regulator_disable(phy->pwr);
-
+out:
 	return ret;
 }
 EXPORT_SYMBOL_GPL(phy_power_on);
diff --git a/drivers/phy/phy-hi6220-usb.c b/drivers/phy/phy-hi6220-usb.c
new file mode 100644
index 000000000000..b2141cbd4cf6
--- /dev/null
+++ b/drivers/phy/phy-hi6220-usb.c
@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2015 Linaro Ltd.
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+#include <linux/regmap.h>
+
+#define SC_PERIPH_CTRL4			0x00c
+
+#define CTRL4_PICO_SIDDQ		BIT(6)
+#define CTRL4_PICO_OGDISABLE		BIT(8)
+#define CTRL4_PICO_VBUSVLDEXT		BIT(10)
+#define CTRL4_PICO_VBUSVLDEXTSEL	BIT(11)
+#define CTRL4_OTG_PHY_SEL		BIT(21)
+
+#define SC_PERIPH_CTRL5			0x010
+
+#define CTRL5_USBOTG_RES_SEL		BIT(3)
+#define CTRL5_PICOPHY_ACAENB		BIT(4)
+#define CTRL5_PICOPHY_BC_MODE		BIT(5)
+#define CTRL5_PICOPHY_CHRGSEL		BIT(6)
+#define CTRL5_PICOPHY_VDATSRCEND	BIT(7)
+#define CTRL5_PICOPHY_VDATDETENB	BIT(8)
+#define CTRL5_PICOPHY_DCDENB		BIT(9)
+#define CTRL5_PICOPHY_IDDIG		BIT(10)
+
+#define SC_PERIPH_CTRL8			0x018
+#define SC_PERIPH_RSTEN0		0x300
+#define SC_PERIPH_RSTDIS0		0x304
+
+#define RST0_USBOTG_BUS			BIT(4)
+#define RST0_POR_PICOPHY		BIT(5)
+#define RST0_USBOTG			BIT(6)
+#define RST0_USBOTG_32K			BIT(7)
+
+#define EYE_PATTERN_PARA		0x7053348c
+
+struct hi6220_priv {
+	struct regmap *reg;
+	struct device *dev;
+};
+
+static void hi6220_phy_init(struct hi6220_priv *priv)
+{
+	struct regmap *reg = priv->reg;
+	u32 val, mask;
+
+	val = RST0_USBOTG_BUS | RST0_POR_PICOPHY |
+	      RST0_USBOTG | RST0_USBOTG_32K;
+	mask = val;
+	regmap_update_bits(reg, SC_PERIPH_RSTEN0, mask, val);
+	regmap_update_bits(reg, SC_PERIPH_RSTDIS0, mask, val);
+}
+
+static int hi6220_phy_setup(struct hi6220_priv *priv, bool on)
+{
+	struct regmap *reg = priv->reg;
+	u32 val, mask;
+	int ret;
+
+	if (on) {
+		val = CTRL5_USBOTG_RES_SEL | CTRL5_PICOPHY_ACAENB;
+		mask = val | CTRL5_PICOPHY_BC_MODE;
+		ret = regmap_update_bits(reg, SC_PERIPH_CTRL5, mask, val);
+		if (ret)
+			goto out;
+
+		val =  CTRL4_PICO_VBUSVLDEXT | CTRL4_PICO_VBUSVLDEXTSEL |
+		       CTRL4_OTG_PHY_SEL;
+		mask = val | CTRL4_PICO_SIDDQ | CTRL4_PICO_OGDISABLE;
+		ret = regmap_update_bits(reg, SC_PERIPH_CTRL4, mask, val);
+		if (ret)
+			goto out;
+
+		ret = regmap_write(reg, SC_PERIPH_CTRL8, EYE_PATTERN_PARA);
+		if (ret)
+			goto out;
+	} else {
+		val = CTRL4_PICO_SIDDQ;
+		mask = val;
+		ret = regmap_update_bits(reg, SC_PERIPH_CTRL4, mask, val);
+		if (ret)
+			goto out;
+	}
+
+	return 0;
+out:
+	dev_err(priv->dev, "failed to setup phy ret: %d\n", ret);
+	return ret;
+}
+
+static int hi6220_phy_start(struct phy *phy)
+{
+	struct hi6220_priv *priv = phy_get_drvdata(phy);
+
+	return hi6220_phy_setup(priv, true);
+}
+
+static int hi6220_phy_exit(struct phy *phy)
+{
+	struct hi6220_priv *priv = phy_get_drvdata(phy);
+
+	return hi6220_phy_setup(priv, false);
+}
+
+static struct phy_ops hi6220_phy_ops = {
+	.init		= hi6220_phy_start,
+	.exit		= hi6220_phy_exit,
+	.owner		= THIS_MODULE,
+};
+
+static int hi6220_phy_probe(struct platform_device *pdev)
+{
+	struct phy_provider *phy_provider;
+	struct device *dev = &pdev->dev;
+	struct phy *phy;
+	struct hi6220_priv *priv;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = dev;
+	priv->reg = syscon_regmap_lookup_by_phandle(dev->of_node,
+					"hisilicon,peripheral-syscon");
+	if (IS_ERR(priv->reg)) {
+		dev_err(dev, "no hisilicon,peripheral-syscon\n");
+		return PTR_ERR(priv->reg);
+	}
+
+	hi6220_phy_init(priv);
+
+	phy = devm_phy_create(dev, NULL, &hi6220_phy_ops);
+	if (IS_ERR(phy))
+		return PTR_ERR(phy);
+
+	phy_set_drvdata(phy, priv);
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	return PTR_ERR_OR_ZERO(phy_provider);
+}
+
+static const struct of_device_id hi6220_phy_of_match[] = {
+	{.compatible = "hisilicon,hi6220-usb-phy",},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, hi6220_phy_of_match);
+
+static struct platform_driver hi6220_phy_driver = {
+	.probe	= hi6220_phy_probe,
+	.driver = {
+		.name	= "hi6220-usb-phy",
+		.of_match_table	= hi6220_phy_of_match,
+	}
+};
+module_platform_driver(hi6220_phy_driver);
+
+MODULE_DESCRIPTION("HISILICON HI6220 USB PHY driver");
+MODULE_ALIAS("platform:hi6220-usb-phy");
+MODULE_LICENSE("GPL");
diff --git a/drivers/phy/phy-mt65xx-usb3.c b/drivers/phy/phy-mt65xx-usb3.c
index e427c3b788ff..c0e7b4b0cf5c 100644
--- a/drivers/phy/phy-mt65xx-usb3.c
+++ b/drivers/phy/phy-mt65xx-usb3.c
@@ -17,6 +17,7 @@
 #include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/io.h>
+#include <linux/iopoll.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
 #include <linux/phy/phy.h>
@@ -27,6 +28,7 @@
  * relative to USB3_SIF2_BASE base address
  */
 #define SSUSB_SIFSLV_SPLLC		0x0000
+#define SSUSB_SIFSLV_U2FREQ		0x0100
 
 /* offsets of sub-segment in each port registers */
 #define SSUSB_SIFSLV_U2PHY_COM_BASE	0x0000
@@ -41,6 +43,7 @@
 #define PA2_RG_SIF_U2PLL_FORCE_EN	BIT(18)
 
 #define U3P_USBPHYACR5		(SSUSB_SIFSLV_U2PHY_COM_BASE + 0x0014)
+#define PA5_RG_U2_HSTX_SRCAL_EN	BIT(15)
 #define PA5_RG_U2_HSTX_SRCTRL		GENMASK(14, 12)
 #define PA5_RG_U2_HSTX_SRCTRL_VAL(x)	((0x7 & (x)) << 12)
 #define PA5_RG_U2_HS_100U_U3_EN	BIT(11)
@@ -49,6 +52,8 @@
 #define PA6_RG_U2_ISO_EN		BIT(31)
 #define PA6_RG_U2_BC11_SW_EN		BIT(23)
 #define PA6_RG_U2_OTG_VBUSCMP_EN	BIT(20)
+#define PA6_RG_U2_SQTH		GENMASK(3, 0)
+#define PA6_RG_U2_SQTH_VAL(x)	(0xf & (x))
 
 #define U3P_U2PHYACR4		(SSUSB_SIFSLV_U2PHY_COM_BASE + 0x0020)
 #define P2C_RG_USB20_GPIO_CTL		BIT(9)
@@ -111,6 +116,24 @@
 #define XC3_RG_U3_XTAL_RX_PWD		BIT(9)
 #define XC3_RG_U3_FRC_XTAL_RX_PWD	BIT(8)
 
+#define U3P_U2FREQ_FMCR0	(SSUSB_SIFSLV_U2FREQ + 0x00)
+#define P2F_RG_MONCLK_SEL	GENMASK(27, 26)
+#define P2F_RG_MONCLK_SEL_VAL(x)	((0x3 & (x)) << 26)
+#define P2F_RG_FREQDET_EN	BIT(24)
+#define P2F_RG_CYCLECNT		GENMASK(23, 0)
+#define P2F_RG_CYCLECNT_VAL(x)	((P2F_RG_CYCLECNT) & (x))
+
+#define U3P_U2FREQ_VALUE	(SSUSB_SIFSLV_U2FREQ + 0x0c)
+
+#define U3P_U2FREQ_FMMONR1	(SSUSB_SIFSLV_U2FREQ + 0x10)
+#define P2F_USB_FM_VALID	BIT(0)
+#define P2F_RG_FRCK_EN		BIT(8)
+
+#define U3P_REF_CLK		26	/* MHZ */
+#define U3P_SLEW_RATE_COEF	28
+#define U3P_SR_COEF_DIVISOR	1000
+#define U3P_FM_DET_CYCLE_CNT	1024
+
 struct mt65xx_phy_instance {
 	struct phy *phy;
 	void __iomem *port_base;
@@ -126,6 +149,77 @@ struct mt65xx_u3phy {
 	int nphys;
 };
 
+static void hs_slew_rate_calibrate(struct mt65xx_u3phy *u3phy,
+	struct mt65xx_phy_instance *instance)
+{
+	void __iomem *sif_base = u3phy->sif_base;
+	int calibration_val;
+	int fm_out;
+	u32 tmp;
+
+	/* enable USB ring oscillator */
+	tmp = readl(instance->port_base + U3P_USBPHYACR5);
+	tmp |= PA5_RG_U2_HSTX_SRCAL_EN;
+	writel(tmp, instance->port_base + U3P_USBPHYACR5);
+	udelay(1);
+
+	/*enable free run clock */
+	tmp = readl(sif_base + U3P_U2FREQ_FMMONR1);
+	tmp |= P2F_RG_FRCK_EN;
+	writel(tmp, sif_base + U3P_U2FREQ_FMMONR1);
+
+	/* set cycle count as 1024, and select u2 channel */
+	tmp = readl(sif_base + U3P_U2FREQ_FMCR0);
+	tmp &= ~(P2F_RG_CYCLECNT | P2F_RG_MONCLK_SEL);
+	tmp |= P2F_RG_CYCLECNT_VAL(U3P_FM_DET_CYCLE_CNT);
+	tmp |= P2F_RG_MONCLK_SEL_VAL(instance->index);
+	writel(tmp, sif_base + U3P_U2FREQ_FMCR0);
+
+	/* enable frequency meter */
+	tmp = readl(sif_base + U3P_U2FREQ_FMCR0);
+	tmp |= P2F_RG_FREQDET_EN;
+	writel(tmp, sif_base + U3P_U2FREQ_FMCR0);
+
+	/* ignore return value */
+	readl_poll_timeout(sif_base + U3P_U2FREQ_FMMONR1, tmp,
+		  (tmp & P2F_USB_FM_VALID), 10, 200);
+
+	fm_out = readl(sif_base + U3P_U2FREQ_VALUE);
+
+	/* disable frequency meter */
+	tmp = readl(sif_base + U3P_U2FREQ_FMCR0);
+	tmp &= ~P2F_RG_FREQDET_EN;
+	writel(tmp, sif_base + U3P_U2FREQ_FMCR0);
+
+	/*disable free run clock */
+	tmp = readl(sif_base + U3P_U2FREQ_FMMONR1);
+	tmp &= ~P2F_RG_FRCK_EN;
+	writel(tmp, sif_base + U3P_U2FREQ_FMMONR1);
+
+	if (fm_out) {
+		/* ( 1024 / FM_OUT ) x reference clock frequency x 0.028 */
+		tmp = U3P_FM_DET_CYCLE_CNT * U3P_REF_CLK * U3P_SLEW_RATE_COEF;
+		tmp /= fm_out;
+		calibration_val = DIV_ROUND_CLOSEST(tmp, U3P_SR_COEF_DIVISOR);
+	} else {
+		/* if FM detection fail, set default value */
+		calibration_val = 4;
+	}
+	dev_dbg(u3phy->dev, "phy:%d, fm_out:%d, calib:%d\n",
+		instance->index, fm_out, calibration_val);
+
+	/* set HS slew rate */
+	tmp = readl(instance->port_base + U3P_USBPHYACR5);
+	tmp &= ~PA5_RG_U2_HSTX_SRCTRL;
+	tmp |= PA5_RG_U2_HSTX_SRCTRL_VAL(calibration_val);
+	writel(tmp, instance->port_base + U3P_USBPHYACR5);
+
+	/* disable USB ring oscillator */
+	tmp = readl(instance->port_base + U3P_USBPHYACR5);
+	tmp &= ~PA5_RG_U2_HSTX_SRCAL_EN;
+	writel(tmp, instance->port_base + U3P_USBPHYACR5);
+}
+
 static void phy_instance_init(struct mt65xx_u3phy *u3phy,
 	struct mt65xx_phy_instance *instance)
 {
@@ -165,9 +259,10 @@ static void phy_instance_init(struct mt65xx_u3phy *u3phy,
 		writel(tmp, port_base + U3P_U2PHYDTM0);
 	}
 
-	/* DP/DM BC1.1 path Disable */
 	tmp = readl(port_base + U3P_USBPHYACR6);
-	tmp &= ~PA6_RG_U2_BC11_SW_EN;
+	tmp &= ~PA6_RG_U2_BC11_SW_EN;	/* DP/DM BC1.1 path Disable */
+	tmp &= ~PA6_RG_U2_SQTH;
+	tmp |= PA6_RG_U2_SQTH_VAL(2);
 	writel(tmp, port_base + U3P_USBPHYACR6);
 
 	tmp = readl(port_base + U3P_U3PHYA_DA_REG0);
@@ -223,9 +318,9 @@ static void phy_instance_power_on(struct mt65xx_u3phy *u3phy,
 		tmp |= XC3_RG_U3_XTAL_RX_PWD | XC3_RG_U3_FRC_XTAL_RX_PWD;
 		writel(tmp, u3phy->sif_base + U3P_XTALCTL3);
 
-		/* [mt8173]disable Change 100uA current from SSUSB */
+		/* [mt8173]switch 100uA current to SSUSB */
 		tmp = readl(port_base + U3P_USBPHYACR5);
-		tmp &= ~PA5_RG_U2_HS_100U_U3_EN;
+		tmp |= PA5_RG_U2_HS_100U_U3_EN;
 		writel(tmp, port_base + U3P_USBPHYACR5);
 	}
 
@@ -270,7 +365,7 @@ static void phy_instance_power_off(struct mt65xx_u3phy *u3phy,
 	writel(tmp, port_base + U3P_USBPHYACR6);
 
 	if (!index) {
-		/* (also disable)Change 100uA current switch to USB2.0 */
+		/* switch 100uA current back to USB2.0 */
 		tmp = readl(port_base + U3P_USBPHYACR5);
 		tmp &= ~PA5_RG_U2_HS_100U_U3_EN;
 		writel(tmp, port_base + U3P_USBPHYACR5);
@@ -340,6 +435,7 @@ static int mt65xx_phy_power_on(struct phy *phy)
 	struct mt65xx_u3phy *u3phy = dev_get_drvdata(phy->dev.parent);
 
 	phy_instance_power_on(u3phy, instance);
+	hs_slew_rate_calibrate(u3phy, instance);
 	return 0;
 }
 
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index 0fe80589ffbe..c134989052f5 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -29,6 +29,8 @@
 #include <linux/delay.h>
 #include <linux/phy/omap_control_phy.h>
 #include <linux/phy/phy.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 #include <linux/of_platform.h>
 
 #define USB2PHY_DISCON_BYP_LATCH (1 << 31)
@@ -97,22 +99,38 @@ static int omap_usb_set_peripheral(struct usb_otg *otg,
 	return 0;
 }
 
+static int omap_usb_phy_power(struct omap_usb *phy, int on)
+{
+	u32 val;
+	int ret;
+
+	if (!phy->syscon_phy_power) {
+		omap_control_phy_power(phy->control_dev, on);
+		return 0;
+	}
+
+	if (on)
+		val = phy->power_on;
+	else
+		val = phy->power_off;
+
+	ret = regmap_update_bits(phy->syscon_phy_power, phy->power_reg,
+				 phy->mask, val);
+	return ret;
+}
+
 static int omap_usb_power_off(struct phy *x)
 {
 	struct omap_usb *phy = phy_get_drvdata(x);
 
-	omap_control_phy_power(phy->control_dev, 0);
-
-	return 0;
+	return omap_usb_phy_power(phy, false);
 }
 
 static int omap_usb_power_on(struct phy *x)
 {
 	struct omap_usb *phy = phy_get_drvdata(x);
 
-	omap_control_phy_power(phy->control_dev, 1);
-
-	return 0;
+	return omap_usb_phy_power(phy, true);
 }
 
 static int omap_usb_init(struct phy *x)
@@ -147,21 +165,38 @@ static const struct phy_ops ops = {
 static const struct usb_phy_data omap_usb2_data = {
 	.label = "omap_usb2",
 	.flags = OMAP_USB2_HAS_START_SRP | OMAP_USB2_HAS_SET_VBUS,
+	.mask = OMAP_DEV_PHY_PD,
+	.power_off = OMAP_DEV_PHY_PD,
 };
 
 static const struct usb_phy_data omap5_usb2_data = {
 	.label = "omap5_usb2",
 	.flags = 0,
+	.mask = OMAP_DEV_PHY_PD,
+	.power_off = OMAP_DEV_PHY_PD,
 };
 
 static const struct usb_phy_data dra7x_usb2_data = {
 	.label = "dra7x_usb2",
 	.flags = OMAP_USB2_CALIBRATE_FALSE_DISCONNECT,
+	.mask = OMAP_DEV_PHY_PD,
+	.power_off = OMAP_DEV_PHY_PD,
+};
+
+static const struct usb_phy_data dra7x_usb2_phy2_data = {
+	.label = "dra7x_usb2_phy2",
+	.flags = OMAP_USB2_CALIBRATE_FALSE_DISCONNECT,
+	.mask = OMAP_USB2_PHY_PD,
+	.power_off = OMAP_USB2_PHY_PD,
 };
 
 static const struct usb_phy_data am437x_usb2_data = {
 	.label = "am437x_usb2",
 	.flags =  0,
+	.mask = AM437X_USB2_PHY_PD | AM437X_USB2_OTG_PD |
+		AM437X_USB2_OTGVDET_EN | AM437X_USB2_OTGSESSEND_EN,
+	.power_on = AM437X_USB2_OTGVDET_EN | AM437X_USB2_OTGSESSEND_EN,
+	.power_off = AM437X_USB2_PHY_PD | AM437X_USB2_OTG_PD,
 };
 
 static const struct of_device_id omap_usb2_id_table[] = {
@@ -178,6 +213,10 @@ static const struct of_device_id omap_usb2_id_table[] = {
 		.data = &dra7x_usb2_data,
 	},
 	{
+		.compatible = "ti,dra7x-usb2-phy2",
+		.data = &dra7x_usb2_phy2_data,
+	},
+	{
 		.compatible = "ti,am437x-usb2",
 		.data = &am437x_usb2_data,
 	},
@@ -219,6 +258,9 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	phy->phy.label		= phy_data->label;
 	phy->phy.otg		= otg;
 	phy->phy.type		= USB_PHY_TYPE_USB2;
+	phy->mask		= phy_data->mask;
+	phy->power_on		= phy_data->power_on;
+	phy->power_off		= phy_data->power_off;
 
 	if (phy_data->flags & OMAP_USB2_CALIBRATE_FALSE_DISCONNECT) {
 		res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -228,21 +270,36 @@ static int omap_usb2_probe(struct platform_device *pdev)
 		phy->flags |= OMAP_USB2_CALIBRATE_FALSE_DISCONNECT;
 	}
 
-	control_node = of_parse_phandle(node, "ctrl-module", 0);
-	if (!control_node) {
-		dev_err(&pdev->dev, "Failed to get control device phandle\n");
-		return -EINVAL;
-	}
+	phy->syscon_phy_power = syscon_regmap_lookup_by_phandle(node,
+							"syscon-phy-power");
+	if (IS_ERR(phy->syscon_phy_power)) {
+		dev_dbg(&pdev->dev,
+			"can't get syscon-phy-power, using control device\n");
+		phy->syscon_phy_power = NULL;
+
+		control_node = of_parse_phandle(node, "ctrl-module", 0);
+		if (!control_node) {
+			dev_err(&pdev->dev,
+				"Failed to get control device phandle\n");
+			return -EINVAL;
+		}
 
-	control_pdev = of_find_device_by_node(control_node);
-	if (!control_pdev) {
-		dev_err(&pdev->dev, "Failed to get control device\n");
-		return -EINVAL;
+		control_pdev = of_find_device_by_node(control_node);
+		if (!control_pdev) {
+			dev_err(&pdev->dev, "Failed to get control device\n");
+			return -EINVAL;
+		}
+		phy->control_dev = &control_pdev->dev;
+	} else {
+		if (of_property_read_u32_index(node,
+					       "syscon-phy-power", 1,
+					       &phy->power_reg)) {
+			dev_err(&pdev->dev,
+				"couldn't get power reg. offset\n");
+			return -EINVAL;
+		}
 	}
 
-	phy->control_dev = &control_pdev->dev;
-	omap_control_phy_power(phy->control_dev, 0);
-
 	otg->set_host		= omap_usb_set_host;
 	otg->set_peripheral	= omap_usb_set_peripheral;
 	if (phy_data->flags & OMAP_USB2_HAS_SET_VBUS)
@@ -261,6 +318,7 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	}
 
 	phy_set_drvdata(generic_phy, phy);
+	omap_usb_power_off(generic_phy);
 
 	phy_provider = devm_of_phy_provider_register(phy->dev,
 			of_phy_simple_xlate);
diff --git a/drivers/phy/phy-rcar-gen3-usb2.c b/drivers/phy/phy-rcar-gen3-usb2.c
new file mode 100644
index 000000000000..ef332ef4abc7
--- /dev/null
+++ b/drivers/phy/phy-rcar-gen3-usb2.c
@@ -0,0 +1,378 @@
+/*
+ * Renesas R-Car Gen3 for USB2.0 PHY driver
+ *
+ * Copyright (C) 2015 Renesas Electronics Corporation
+ *
+ * This is based on the phy-rcar-gen2 driver:
+ * Copyright (C) 2014 Renesas Solutions Corp.
+ * Copyright (C) 2014 Cogent Embedded, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+
+/******* USB2.0 Host registers (original offset is +0x200) *******/
+#define USB2_INT_ENABLE		0x000
+#define USB2_USBCTR		0x00c
+#define USB2_SPD_RSM_TIMSET	0x10c
+#define USB2_OC_TIMSET		0x110
+#define USB2_COMMCTRL		0x600
+#define USB2_OBINTSTA		0x604
+#define USB2_OBINTEN		0x608
+#define USB2_VBCTRL		0x60c
+#define USB2_LINECTRL1		0x610
+#define USB2_ADPCTRL		0x630
+
+/* INT_ENABLE */
+#define USB2_INT_ENABLE_UCOM_INTEN	BIT(3)
+#define USB2_INT_ENABLE_USBH_INTB_EN	BIT(2)
+#define USB2_INT_ENABLE_USBH_INTA_EN	BIT(1)
+#define USB2_INT_ENABLE_INIT		(USB2_INT_ENABLE_UCOM_INTEN | \
+					 USB2_INT_ENABLE_USBH_INTB_EN | \
+					 USB2_INT_ENABLE_USBH_INTA_EN)
+
+/* USBCTR */
+#define USB2_USBCTR_DIRPD	BIT(2)
+#define USB2_USBCTR_PLL_RST	BIT(1)
+
+/* SPD_RSM_TIMSET */
+#define USB2_SPD_RSM_TIMSET_INIT	0x014e029b
+
+/* OC_TIMSET */
+#define USB2_OC_TIMSET_INIT		0x000209ab
+
+/* COMMCTRL */
+#define USB2_COMMCTRL_OTG_PERI		BIT(31)	/* 1 = Peripheral mode */
+
+/* OBINTSTA and OBINTEN */
+#define USB2_OBINT_SESSVLDCHG		BIT(12)
+#define USB2_OBINT_IDDIGCHG		BIT(11)
+#define USB2_OBINT_BITS			(USB2_OBINT_SESSVLDCHG | \
+					 USB2_OBINT_IDDIGCHG)
+
+/* VBCTRL */
+#define USB2_VBCTRL_DRVVBUSSEL		BIT(8)
+
+/* LINECTRL1 */
+#define USB2_LINECTRL1_DPRPD_EN		BIT(19)
+#define USB2_LINECTRL1_DP_RPD		BIT(18)
+#define USB2_LINECTRL1_DMRPD_EN		BIT(17)
+#define USB2_LINECTRL1_DM_RPD		BIT(16)
+
+/* ADPCTRL */
+#define USB2_ADPCTRL_OTGSESSVLD		BIT(20)
+#define USB2_ADPCTRL_IDDIG		BIT(19)
+#define USB2_ADPCTRL_IDPULLUP		BIT(5)	/* 1 = ID sampling is enabled */
+#define USB2_ADPCTRL_DRVVBUS		BIT(4)
+
+/******* HSUSB registers (original offset is +0x100) *******/
+#define HSUSB_LPSTS			0x02
+#define HSUSB_UGCTRL2			0x84
+
+/* Low Power Status register (LPSTS) */
+#define HSUSB_LPSTS_SUSPM		0x4000
+
+/* USB General control register 2 (UGCTRL2) */
+#define HSUSB_UGCTRL2_MASK		0x00000031 /* bit[31:6] should be 0 */
+#define HSUSB_UGCTRL2_USB0SEL		0x00000030
+#define HSUSB_UGCTRL2_USB0SEL_HOST	0x00000010
+#define HSUSB_UGCTRL2_USB0SEL_HS_USB	0x00000020
+#define HSUSB_UGCTRL2_USB0SEL_OTG	0x00000030
+
+struct rcar_gen3_data {
+	void __iomem *base;
+	struct clk *clk;
+};
+
+struct rcar_gen3_chan {
+	struct rcar_gen3_data usb2;
+	struct rcar_gen3_data hsusb;
+	struct phy *phy;
+};
+
+static void rcar_gen3_set_host_mode(struct rcar_gen3_chan *ch, int host)
+{
+	void __iomem *usb2_base = ch->usb2.base;
+	u32 val = readl(usb2_base + USB2_COMMCTRL);
+
+	dev_vdbg(&ch->phy->dev, "%s: %08x, %d\n", __func__, val, host);
+	if (host)
+		val &= ~USB2_COMMCTRL_OTG_PERI;
+	else
+		val |= USB2_COMMCTRL_OTG_PERI;
+	writel(val, usb2_base + USB2_COMMCTRL);
+}
+
+static void rcar_gen3_set_linectrl(struct rcar_gen3_chan *ch, int dp, int dm)
+{
+	void __iomem *usb2_base = ch->usb2.base;
+	u32 val = readl(usb2_base + USB2_LINECTRL1);
+
+	dev_vdbg(&ch->phy->dev, "%s: %08x, %d, %d\n", __func__, val, dp, dm);
+	val &= ~(USB2_LINECTRL1_DP_RPD | USB2_LINECTRL1_DM_RPD);
+	if (dp)
+		val |= USB2_LINECTRL1_DP_RPD;
+	if (dm)
+		val |= USB2_LINECTRL1_DM_RPD;
+	writel(val, usb2_base + USB2_LINECTRL1);
+}
+
+static void rcar_gen3_enable_vbus_ctrl(struct rcar_gen3_chan *ch, int vbus)
+{
+	void __iomem *usb2_base = ch->usb2.base;
+	u32 val = readl(usb2_base + USB2_ADPCTRL);
+
+	dev_vdbg(&ch->phy->dev, "%s: %08x, %d\n", __func__, val, vbus);
+	if (vbus)
+		val |= USB2_ADPCTRL_DRVVBUS;
+	else
+		val &= ~USB2_ADPCTRL_DRVVBUS;
+	writel(val, usb2_base + USB2_ADPCTRL);
+}
+
+static void rcar_gen3_init_for_host(struct rcar_gen3_chan *ch)
+{
+	rcar_gen3_set_linectrl(ch, 1, 1);
+	rcar_gen3_set_host_mode(ch, 1);
+	rcar_gen3_enable_vbus_ctrl(ch, 1);
+}
+
+static void rcar_gen3_init_for_peri(struct rcar_gen3_chan *ch)
+{
+	rcar_gen3_set_linectrl(ch, 0, 1);
+	rcar_gen3_set_host_mode(ch, 0);
+	rcar_gen3_enable_vbus_ctrl(ch, 0);
+}
+
+static bool rcar_gen3_check_vbus(struct rcar_gen3_chan *ch)
+{
+	return !!(readl(ch->usb2.base + USB2_ADPCTRL) &
+		  USB2_ADPCTRL_OTGSESSVLD);
+}
+
+static bool rcar_gen3_check_id(struct rcar_gen3_chan *ch)
+{
+	return !!(readl(ch->usb2.base + USB2_ADPCTRL) & USB2_ADPCTRL_IDDIG);
+}
+
+static void rcar_gen3_device_recognition(struct rcar_gen3_chan *ch)
+{
+	bool is_host = true;
+
+	/* B-device? */
+	if (rcar_gen3_check_id(ch) && rcar_gen3_check_vbus(ch))
+		is_host = false;
+
+	if (is_host)
+		rcar_gen3_init_for_host(ch);
+	else
+		rcar_gen3_init_for_peri(ch);
+}
+
+static void rcar_gen3_init_otg(struct rcar_gen3_chan *ch)
+{
+	void __iomem *usb2_base = ch->usb2.base;
+	u32 val;
+
+	val = readl(usb2_base + USB2_VBCTRL);
+	writel(val | USB2_VBCTRL_DRVVBUSSEL, usb2_base + USB2_VBCTRL);
+	writel(USB2_OBINT_BITS, usb2_base + USB2_OBINTSTA);
+	val = readl(usb2_base + USB2_OBINTEN);
+	writel(val | USB2_OBINT_BITS, usb2_base + USB2_OBINTEN);
+	val = readl(usb2_base + USB2_ADPCTRL);
+	writel(val | USB2_ADPCTRL_IDPULLUP, usb2_base + USB2_ADPCTRL);
+	val = readl(usb2_base + USB2_LINECTRL1);
+	rcar_gen3_set_linectrl(ch, 0, 0);
+	writel(val | USB2_LINECTRL1_DPRPD_EN | USB2_LINECTRL1_DMRPD_EN,
+	       usb2_base + USB2_LINECTRL1);
+
+	rcar_gen3_device_recognition(ch);
+}
+
+static int rcar_gen3_phy_usb2_init(struct phy *p)
+{
+	struct rcar_gen3_chan *channel = phy_get_drvdata(p);
+	void __iomem *usb2_base = channel->usb2.base;
+	void __iomem *hsusb_base = channel->hsusb.base;
+	u32 val;
+
+	/* Initialize USB2 part */
+	writel(USB2_INT_ENABLE_INIT, usb2_base + USB2_INT_ENABLE);
+	writel(USB2_SPD_RSM_TIMSET_INIT, usb2_base + USB2_SPD_RSM_TIMSET);
+	writel(USB2_OC_TIMSET_INIT, usb2_base + USB2_OC_TIMSET);
+
+	/* Initialize HSUSB part */
+	if (hsusb_base) {
+		val = readl(hsusb_base + HSUSB_UGCTRL2);
+		val = (val & ~HSUSB_UGCTRL2_USB0SEL) |
+		      HSUSB_UGCTRL2_USB0SEL_OTG;
+		writel(val & HSUSB_UGCTRL2_MASK, hsusb_base + HSUSB_UGCTRL2);
+
+		/* Initialize otg part */
+		rcar_gen3_init_otg(channel);
+	}
+
+	return 0;
+}
+
+static int rcar_gen3_phy_usb2_exit(struct phy *p)
+{
+	struct rcar_gen3_chan *channel = phy_get_drvdata(p);
+
+	writel(0, channel->usb2.base + USB2_INT_ENABLE);
+
+	return 0;
+}
+
+static int rcar_gen3_phy_usb2_power_on(struct phy *p)
+{
+	struct rcar_gen3_chan *channel = phy_get_drvdata(p);
+	void __iomem *usb2_base = channel->usb2.base;
+	void __iomem *hsusb_base = channel->hsusb.base;
+	u32 val;
+
+	val = readl(usb2_base + USB2_USBCTR);
+	val |= USB2_USBCTR_PLL_RST;
+	writel(val, usb2_base + USB2_USBCTR);
+	val &= ~USB2_USBCTR_PLL_RST;
+	writel(val, usb2_base + USB2_USBCTR);
+
+	/*
+	 * TODO: To reduce power consuming, this driver should set the SUSPM
+	 *	after the PHY detects ID pin as peripheral.
+	 */
+	if (hsusb_base) {
+		/* Power on HSUSB PHY */
+		val = readw(hsusb_base + HSUSB_LPSTS);
+		val |= HSUSB_LPSTS_SUSPM;
+		writew(val, hsusb_base + HSUSB_LPSTS);
+	}
+
+	return 0;
+}
+
+static int rcar_gen3_phy_usb2_power_off(struct phy *p)
+{
+	struct rcar_gen3_chan *channel = phy_get_drvdata(p);
+	void __iomem *hsusb_base = channel->hsusb.base;
+	u32 val;
+
+	if (hsusb_base) {
+		/* Power off HSUSB PHY */
+		val = readw(hsusb_base + HSUSB_LPSTS);
+		val &= ~HSUSB_LPSTS_SUSPM;
+		writew(val, hsusb_base + HSUSB_LPSTS);
+	}
+
+	return 0;
+}
+
+static struct phy_ops rcar_gen3_phy_usb2_ops = {
+	.init		= rcar_gen3_phy_usb2_init,
+	.exit		= rcar_gen3_phy_usb2_exit,
+	.power_on	= rcar_gen3_phy_usb2_power_on,
+	.power_off	= rcar_gen3_phy_usb2_power_off,
+	.owner		= THIS_MODULE,
+};
+
+static irqreturn_t rcar_gen3_phy_usb2_irq(int irq, void *_ch)
+{
+	struct rcar_gen3_chan *ch = _ch;
+	void __iomem *usb2_base = ch->usb2.base;
+	u32 status = readl(usb2_base + USB2_OBINTSTA);
+	irqreturn_t ret = IRQ_NONE;
+
+	if (status & USB2_OBINT_BITS) {
+		dev_vdbg(&ch->phy->dev, "%s: %08x\n", __func__, status);
+		writel(USB2_OBINT_BITS, usb2_base + USB2_OBINTSTA);
+		rcar_gen3_device_recognition(ch);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static const struct of_device_id rcar_gen3_phy_usb2_match_table[] = {
+	{ .compatible = "renesas,usb2-phy-r8a7795" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rcar_gen3_phy_usb2_match_table);
+
+static int rcar_gen3_phy_usb2_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rcar_gen3_chan *channel;
+	struct phy_provider *provider;
+	struct resource *res;
+
+	if (!dev->of_node) {
+		dev_err(dev, "This driver needs device tree\n");
+		return -EINVAL;
+	}
+
+	channel = devm_kzalloc(dev, sizeof(*channel), GFP_KERNEL);
+	if (!channel)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "usb2_host");
+	channel->usb2.base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(channel->usb2.base))
+		return PTR_ERR(channel->usb2.base);
+
+	/* "hsusb" memory resource is optional */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "hsusb");
+
+	/* To avoid error message by devm_ioremap_resource() */
+	if (res) {
+		int irq;
+
+		channel->hsusb.base = devm_ioremap_resource(dev, res);
+		if (IS_ERR(channel->hsusb.base))
+			channel->hsusb.base = NULL;
+		/* call request_irq for OTG */
+		irq = platform_get_irq(pdev, 0);
+		if (irq >= 0)
+			irq = devm_request_irq(dev, irq, rcar_gen3_phy_usb2_irq,
+					       IRQF_SHARED, dev_name(dev),
+					       channel);
+		if (irq < 0)
+			dev_err(dev, "No irq handler (%d)\n", irq);
+	}
+
+	/* devm_phy_create() will call pm_runtime_enable(dev); */
+	channel->phy = devm_phy_create(dev, NULL, &rcar_gen3_phy_usb2_ops);
+	if (IS_ERR(channel->phy)) {
+		dev_err(dev, "Failed to create USB2 PHY\n");
+		return PTR_ERR(channel->phy);
+	}
+
+	phy_set_drvdata(channel->phy, channel);
+
+	provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(provider))
+		dev_err(dev, "Failed to register PHY provider\n");
+
+	return PTR_ERR_OR_ZERO(provider);
+}
+
+static struct platform_driver rcar_gen3_phy_usb2_driver = {
+	.driver = {
+		.name		= "phy_rcar_gen3_usb2",
+		.of_match_table	= rcar_gen3_phy_usb2_match_table,
+	},
+	.probe	= rcar_gen3_phy_usb2_probe,
+};
+module_platform_driver(rcar_gen3_phy_usb2_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Renesas R-Car Gen3 USB 2.0 PHY");
+MODULE_AUTHOR("Yoshihiro Shimoda <yoshihiro.shimoda.uh@renesas.com>");
diff --git a/drivers/phy/phy-rockchip-usb.c b/drivers/phy/phy-rockchip-usb.c
index 62c43c435194..33a80eba1cb4 100644
--- a/drivers/phy/phy-rockchip-usb.c
+++ b/drivers/phy/phy-rockchip-usb.c
@@ -15,12 +15,14 @@
  */
 
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_platform.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/regulator/consumer.h>
@@ -36,120 +38,281 @@
 #define SIDDQ_ON		BIT(13)
 #define SIDDQ_OFF		(0 << 13)
 
+struct rockchip_usb_phys {
+	int reg;
+	const char *pll_name;
+};
+
+struct rockchip_usb_phy_pdata {
+	struct rockchip_usb_phys *phys;
+};
+
+struct rockchip_usb_phy_base {
+	struct device *dev;
+	struct regmap *reg_base;
+	const struct rockchip_usb_phy_pdata *pdata;
+};
+
 struct rockchip_usb_phy {
+	struct rockchip_usb_phy_base *base;
+	struct device_node *np;
 	unsigned int	reg_offset;
-	struct regmap	*reg_base;
 	struct clk	*clk;
+	struct clk      *clk480m;
+	struct clk_hw	clk480m_hw;
 	struct phy	*phy;
 };
 
 static int rockchip_usb_phy_power(struct rockchip_usb_phy *phy,
 					   bool siddq)
 {
-	return regmap_write(phy->reg_base, phy->reg_offset,
+	return regmap_write(phy->base->reg_base, phy->reg_offset,
 			    SIDDQ_WRITE_ENA | (siddq ? SIDDQ_ON : SIDDQ_OFF));
 }
 
-static int rockchip_usb_phy_power_off(struct phy *_phy)
+static unsigned long rockchip_usb_phy480m_recalc_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
 {
-	struct rockchip_usb_phy *phy = phy_get_drvdata(_phy);
-	int ret = 0;
+	return 480000000;
+}
+
+static void rockchip_usb_phy480m_disable(struct clk_hw *hw)
+{
+	struct rockchip_usb_phy *phy = container_of(hw,
+						    struct rockchip_usb_phy,
+						    clk480m_hw);
 
 	/* Power down usb phy analog blocks by set siddq 1 */
-	ret = rockchip_usb_phy_power(phy, 1);
-	if (ret)
-		return ret;
+	rockchip_usb_phy_power(phy, 1);
+}
 
-	clk_disable_unprepare(phy->clk);
+static int rockchip_usb_phy480m_enable(struct clk_hw *hw)
+{
+	struct rockchip_usb_phy *phy = container_of(hw,
+						    struct rockchip_usb_phy,
+						    clk480m_hw);
 
-	return 0;
+	/* Power up usb phy analog blocks by set siddq 0 */
+	return rockchip_usb_phy_power(phy, 0);
 }
 
-static int rockchip_usb_phy_power_on(struct phy *_phy)
+static int rockchip_usb_phy480m_is_enabled(struct clk_hw *hw)
 {
-	struct rockchip_usb_phy *phy = phy_get_drvdata(_phy);
-	int ret = 0;
+	struct rockchip_usb_phy *phy = container_of(hw,
+						    struct rockchip_usb_phy,
+						    clk480m_hw);
+	int ret;
+	u32 val;
 
-	ret = clk_prepare_enable(phy->clk);
-	if (ret)
+	ret = regmap_read(phy->base->reg_base, phy->reg_offset, &val);
+	if (ret < 0)
 		return ret;
 
-	/* Power up usb phy analog blocks by set siddq 0 */
-	ret = rockchip_usb_phy_power(phy, 0);
-	if (ret) {
-		clk_disable_unprepare(phy->clk);
-		return ret;
-	}
+	return (val & SIDDQ_ON) ? 0 : 1;
+}
+
+static const struct clk_ops rockchip_usb_phy480m_ops = {
+	.enable = rockchip_usb_phy480m_enable,
+	.disable = rockchip_usb_phy480m_disable,
+	.is_enabled = rockchip_usb_phy480m_is_enabled,
+	.recalc_rate = rockchip_usb_phy480m_recalc_rate,
+};
+
+static int rockchip_usb_phy_power_off(struct phy *_phy)
+{
+	struct rockchip_usb_phy *phy = phy_get_drvdata(_phy);
+
+	clk_disable_unprepare(phy->clk480m);
 
 	return 0;
 }
 
+static int rockchip_usb_phy_power_on(struct phy *_phy)
+{
+	struct rockchip_usb_phy *phy = phy_get_drvdata(_phy);
+
+	return clk_prepare_enable(phy->clk480m);
+}
+
 static const struct phy_ops ops = {
 	.power_on	= rockchip_usb_phy_power_on,
 	.power_off	= rockchip_usb_phy_power_off,
 	.owner		= THIS_MODULE,
 };
 
-static int rockchip_usb_phy_probe(struct platform_device *pdev)
+static void rockchip_usb_phy_action(void *data)
+{
+	struct rockchip_usb_phy *rk_phy = data;
+
+	of_clk_del_provider(rk_phy->np);
+	clk_unregister(rk_phy->clk480m);
+
+	if (rk_phy->clk)
+		clk_put(rk_phy->clk);
+}
+
+static int rockchip_usb_phy_init(struct rockchip_usb_phy_base *base,
+				 struct device_node *child)
 {
-	struct device *dev = &pdev->dev;
 	struct rockchip_usb_phy *rk_phy;
-	struct phy_provider *phy_provider;
-	struct device_node *child;
-	struct regmap *grf;
 	unsigned int reg_offset;
-	int err;
+	const char *clk_name;
+	struct clk_init_data init;
+	int err, i;
 
-	grf = syscon_regmap_lookup_by_phandle(dev->of_node, "rockchip,grf");
-	if (IS_ERR(grf)) {
-		dev_err(&pdev->dev, "Missing rockchip,grf property\n");
-		return PTR_ERR(grf);
+	rk_phy = devm_kzalloc(base->dev, sizeof(*rk_phy), GFP_KERNEL);
+	if (!rk_phy)
+		return -ENOMEM;
+
+	rk_phy->base = base;
+	rk_phy->np = child;
+
+	if (of_property_read_u32(child, "reg", &reg_offset)) {
+		dev_err(base->dev, "missing reg property in node %s\n",
+			child->name);
+		return -EINVAL;
 	}
 
-	for_each_available_child_of_node(dev->of_node, child) {
-		rk_phy = devm_kzalloc(dev, sizeof(*rk_phy), GFP_KERNEL);
-		if (!rk_phy) {
-			err = -ENOMEM;
-			goto put_child;
-		}
+	rk_phy->reg_offset = reg_offset;
+
+	rk_phy->clk = of_clk_get_by_name(child, "phyclk");
+	if (IS_ERR(rk_phy->clk))
+		rk_phy->clk = NULL;
 
-		if (of_property_read_u32(child, "reg", &reg_offset)) {
-			dev_err(dev, "missing reg property in node %s\n",
-				child->name);
-			err = -EINVAL;
-			goto put_child;
+	i = 0;
+	init.name = NULL;
+	while (base->pdata->phys[i].reg) {
+		if (base->pdata->phys[i].reg == reg_offset) {
+			init.name = base->pdata->phys[i].pll_name;
+			break;
 		}
+		i++;
+	}
 
-		rk_phy->reg_offset = reg_offset;
-		rk_phy->reg_base = grf;
+	if (!init.name) {
+		dev_err(base->dev, "phy data not found\n");
+		return -EINVAL;
+	}
 
-		rk_phy->clk = of_clk_get_by_name(child, "phyclk");
-		if (IS_ERR(rk_phy->clk))
-			rk_phy->clk = NULL;
+	if (rk_phy->clk) {
+		clk_name = __clk_get_name(rk_phy->clk);
+		init.flags = 0;
+		init.parent_names = &clk_name;
+		init.num_parents = 1;
+	} else {
+		init.flags = CLK_IS_ROOT;
+		init.parent_names = NULL;
+		init.num_parents = 0;
+	}
 
-		rk_phy->phy = devm_phy_create(dev, child, &ops);
-		if (IS_ERR(rk_phy->phy)) {
-			dev_err(dev, "failed to create PHY\n");
-			err = PTR_ERR(rk_phy->phy);
-			goto put_child;
-		}
-		phy_set_drvdata(rk_phy->phy, rk_phy);
+	init.ops = &rockchip_usb_phy480m_ops;
+	rk_phy->clk480m_hw.init = &init;
 
-		/* only power up usb phy when it use, so disable it when init*/
-		err = rockchip_usb_phy_power(rk_phy, 1);
-		if (err)
-			goto put_child;
+	rk_phy->clk480m = clk_register(base->dev, &rk_phy->clk480m_hw);
+	if (IS_ERR(rk_phy->clk480m)) {
+		err = PTR_ERR(rk_phy->clk480m);
+		goto err_clk;
+	}
+
+	err = of_clk_add_provider(child, of_clk_src_simple_get,
+				  rk_phy->clk480m);
+	if (err < 0)
+		goto err_clk_prov;
+
+	err = devm_add_action(base->dev, rockchip_usb_phy_action, rk_phy);
+	if (err)
+		goto err_devm_action;
+
+	rk_phy->phy = devm_phy_create(base->dev, child, &ops);
+	if (IS_ERR(rk_phy->phy)) {
+		dev_err(base->dev, "failed to create PHY\n");
+		return PTR_ERR(rk_phy->phy);
+	}
+	phy_set_drvdata(rk_phy->phy, rk_phy);
+
+	/* only power up usb phy when it use, so disable it when init*/
+	return rockchip_usb_phy_power(rk_phy, 1);
+
+err_devm_action:
+	of_clk_del_provider(child);
+err_clk_prov:
+	clk_unregister(rk_phy->clk480m);
+err_clk:
+	if (rk_phy->clk)
+		clk_put(rk_phy->clk);
+	return err;
+}
+
+static const struct rockchip_usb_phy_pdata rk3066a_pdata = {
+	.phys = (struct rockchip_usb_phys[]){
+		{ .reg = 0x17c, .pll_name = "sclk_otgphy0_480m" },
+		{ .reg = 0x188, .pll_name = "sclk_otgphy1_480m" },
+		{ /* sentinel */ }
+	},
+};
+
+static const struct rockchip_usb_phy_pdata rk3188_pdata = {
+	.phys = (struct rockchip_usb_phys[]){
+		{ .reg = 0x10c, .pll_name = "sclk_otgphy0_480m" },
+		{ .reg = 0x11c, .pll_name = "sclk_otgphy1_480m" },
+		{ /* sentinel */ }
+	},
+};
+
+static const struct rockchip_usb_phy_pdata rk3288_pdata = {
+	.phys = (struct rockchip_usb_phys[]){
+		{ .reg = 0x320, .pll_name = "sclk_otgphy0_480m" },
+		{ .reg = 0x334, .pll_name = "sclk_otgphy1_480m" },
+		{ .reg = 0x348, .pll_name = "sclk_otgphy2_480m" },
+		{ /* sentinel */ }
+	},
+};
+
+static int rockchip_usb_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rockchip_usb_phy_base *phy_base;
+	struct phy_provider *phy_provider;
+	const struct of_device_id *match;
+	struct device_node *child;
+	int err;
+
+	phy_base = devm_kzalloc(dev, sizeof(*phy_base), GFP_KERNEL);
+	if (!phy_base)
+		return -ENOMEM;
+
+	match = of_match_device(dev->driver->of_match_table, dev);
+	if (!match || !match->data) {
+		dev_err(dev, "missing phy data\n");
+		return -EINVAL;
+	}
+
+	phy_base->pdata = match->data;
+
+	phy_base->dev = dev;
+	phy_base->reg_base = syscon_regmap_lookup_by_phandle(dev->of_node,
+							     "rockchip,grf");
+	if (IS_ERR(phy_base->reg_base)) {
+		dev_err(&pdev->dev, "Missing rockchip,grf property\n");
+		return PTR_ERR(phy_base->reg_base);
+	}
+
+	for_each_available_child_of_node(dev->of_node, child) {
+		err = rockchip_usb_phy_init(phy_base, child);
+		if (err) {
+			of_node_put(child);
+			return err;
+		}
 	}
 
 	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
 	return PTR_ERR_OR_ZERO(phy_provider);
-put_child:
-	of_node_put(child);
-	return err;
 }
 
 static const struct of_device_id rockchip_usb_phy_dt_ids[] = {
-	{ .compatible = "rockchip,rk3288-usb-phy" },
+	{ .compatible = "rockchip,rk3066a-usb-phy", .data = &rk3066a_pdata },
+	{ .compatible = "rockchip,rk3188-usb-phy", .data = &rk3188_pdata },
+	{ .compatible = "rockchip,rk3288-usb-phy", .data = &rk3288_pdata },
 	{}
 };
 
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index b12964b70625..bae54f7a1f48 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -32,6 +32,7 @@
 #include <linux/mutex.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/of_device.h>
 #include <linux/of_gpio.h>
 #include <linux/phy/phy.h>
 #include <linux/phy/phy-sun4i-usb.h>
@@ -46,6 +47,9 @@
 #define REG_PHYBIST			0x08
 #define REG_PHYTUNE			0x0c
 #define REG_PHYCTL_A33			0x10
+#define REG_PHY_UNK_H3			0x20
+
+#define REG_PMU_UNK_H3			0x10
 
 #define PHYCTL_DATA			BIT(7)
 
@@ -79,7 +83,7 @@
 #define PHY_DISCON_TH_SEL		0x2a
 #define PHY_SQUELCH_DETECT		0x3c
 
-#define MAX_PHYS			3
+#define MAX_PHYS			4
 
 /*
  * Note do not raise the debounce time, we must report Vusb high within 100ms
@@ -88,12 +92,24 @@
 #define DEBOUNCE_TIME			msecs_to_jiffies(50)
 #define POLL_TIME			msecs_to_jiffies(250)
 
+enum sun4i_usb_phy_type {
+	sun4i_a10_phy,
+	sun8i_a33_phy,
+	sun8i_h3_phy,
+};
+
+struct sun4i_usb_phy_cfg {
+	int num_phys;
+	enum sun4i_usb_phy_type type;
+	u32 disc_thresh;
+	u8 phyctl_offset;
+	bool dedicated_clocks;
+};
+
 struct sun4i_usb_phy_data {
 	void __iomem *base;
+	const struct sun4i_usb_phy_cfg *cfg;
 	struct mutex mutex;
-	int num_phys;
-	u32 disc_thresh;
-	bool has_a33_phyctl;
 	struct sun4i_usb_phy {
 		struct phy *phy;
 		void __iomem *pmu;
@@ -159,17 +175,14 @@ static void sun4i_usb_phy_write(struct sun4i_usb_phy *phy, u32 addr, u32 data,
 {
 	struct sun4i_usb_phy_data *phy_data = to_sun4i_usb_phy_data(phy);
 	u32 temp, usbc_bit = BIT(phy->index * 2);
-	void *phyctl;
+	void *phyctl = phy_data->base + phy_data->cfg->phyctl_offset;
 	int i;
 
 	mutex_lock(&phy_data->mutex);
 
-	if (phy_data->has_a33_phyctl) {
-		phyctl = phy_data->base + REG_PHYCTL_A33;
+	if (phy_data->cfg->type == sun8i_a33_phy) {
 		/* A33 needs us to set phyctl to 0 explicitly */
 		writel(0, phyctl);
-	} else {
-		phyctl = phy_data->base + REG_PHYCTL_A10;
 	}
 
 	for (i = 0; i < len; i++) {
@@ -230,6 +243,7 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 	struct sun4i_usb_phy *phy = phy_get_drvdata(_phy);
 	struct sun4i_usb_phy_data *data = to_sun4i_usb_phy_data(phy);
 	int ret;
+	u32 val;
 
 	ret = clk_prepare_enable(phy->clk);
 	if (ret)
@@ -241,15 +255,26 @@ static int sun4i_usb_phy_init(struct phy *_phy)
 		return ret;
 	}
 
-	/* Enable USB 45 Ohm resistor calibration */
-	if (phy->index == 0)
-		sun4i_usb_phy_write(phy, PHY_RES45_CAL_EN, 0x01, 1);
+	if (data->cfg->type == sun8i_h3_phy) {
+		if (phy->index == 0) {
+			val = readl(data->base + REG_PHY_UNK_H3);
+			writel(val & ~1, data->base + REG_PHY_UNK_H3);
+		}
 
-	/* Adjust PHY's magnitude and rate */
-	sun4i_usb_phy_write(phy, PHY_TX_AMPLITUDE_TUNE, 0x14, 5);
+		val = readl(phy->pmu + REG_PMU_UNK_H3);
+		writel(val & ~2, phy->pmu + REG_PMU_UNK_H3);
+	} else {
+		/* Enable USB 45 Ohm resistor calibration */
+		if (phy->index == 0)
+			sun4i_usb_phy_write(phy, PHY_RES45_CAL_EN, 0x01, 1);
 
-	/* Disconnect threshold adjustment */
-	sun4i_usb_phy_write(phy, PHY_DISCON_TH_SEL, data->disc_thresh, 2);
+		/* Adjust PHY's magnitude and rate */
+		sun4i_usb_phy_write(phy, PHY_TX_AMPLITUDE_TUNE, 0x14, 5);
+
+		/* Disconnect threshold adjustment */
+		sun4i_usb_phy_write(phy, PHY_DISCON_TH_SEL,
+				    data->cfg->disc_thresh, 2);
+	}
 
 	sun4i_usb_phy_passby(phy, 1);
 
@@ -476,7 +501,7 @@ static struct phy *sun4i_usb_phy_xlate(struct device *dev,
 {
 	struct sun4i_usb_phy_data *data = dev_get_drvdata(dev);
 
-	if (args->args[0] >= data->num_phys)
+	if (args->args[0] >= data->cfg->num_phys)
 		return ERR_PTR(-ENODEV);
 
 	return data->phys[args->args[0]].phy;
@@ -511,7 +536,6 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct device_node *np = dev->of_node;
 	struct phy_provider *phy_provider;
-	bool dedicated_clocks;
 	struct resource *res;
 	int i, ret;
 
@@ -522,29 +546,9 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	mutex_init(&data->mutex);
 	INIT_DELAYED_WORK(&data->detect, sun4i_usb_phy0_id_vbus_det_scan);
 	dev_set_drvdata(dev, data);
-
-	if (of_device_is_compatible(np, "allwinner,sun5i-a13-usb-phy") ||
-	    of_device_is_compatible(np, "allwinner,sun8i-a23-usb-phy") ||
-	    of_device_is_compatible(np, "allwinner,sun8i-a33-usb-phy"))
-		data->num_phys = 2;
-	else
-		data->num_phys = 3;
-
-	if (of_device_is_compatible(np, "allwinner,sun5i-a13-usb-phy") ||
-	    of_device_is_compatible(np, "allwinner,sun7i-a20-usb-phy"))
-		data->disc_thresh = 2;
-	else
-		data->disc_thresh = 3;
-
-	if (of_device_is_compatible(np, "allwinner,sun6i-a31-usb-phy") ||
-	    of_device_is_compatible(np, "allwinner,sun8i-a23-usb-phy") ||
-	    of_device_is_compatible(np, "allwinner,sun8i-a33-usb-phy"))
-		dedicated_clocks = true;
-	else
-		dedicated_clocks = false;
-
-	if (of_device_is_compatible(np, "allwinner,sun8i-a33-usb-phy"))
-		data->has_a33_phyctl = true;
+	data->cfg = of_device_get_match_data(dev);
+	if (!data->cfg)
+		return -EINVAL;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy_ctrl");
 	data->base = devm_ioremap_resource(dev, res);
@@ -590,7 +594,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 		}
 	}
 
-	for (i = 0; i < data->num_phys; i++) {
+	for (i = 0; i < data->cfg->num_phys; i++) {
 		struct sun4i_usb_phy *phy = data->phys + i;
 		char name[16];
 
@@ -602,7 +606,7 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 			phy->vbus = NULL;
 		}
 
-		if (dedicated_clocks)
+		if (data->cfg->dedicated_clocks)
 			snprintf(name, sizeof(name), "usb%d_phy", i);
 		else
 			strlcpy(name, "usb_phy", sizeof(name));
@@ -689,13 +693,69 @@ static int sun4i_usb_phy_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static const struct sun4i_usb_phy_cfg sun4i_a10_cfg = {
+	.num_phys = 3,
+	.type = sun4i_a10_phy,
+	.disc_thresh = 3,
+	.phyctl_offset = REG_PHYCTL_A10,
+	.dedicated_clocks = false,
+};
+
+static const struct sun4i_usb_phy_cfg sun5i_a13_cfg = {
+	.num_phys = 2,
+	.type = sun4i_a10_phy,
+	.disc_thresh = 2,
+	.phyctl_offset = REG_PHYCTL_A10,
+	.dedicated_clocks = false,
+};
+
+static const struct sun4i_usb_phy_cfg sun6i_a31_cfg = {
+	.num_phys = 3,
+	.type = sun4i_a10_phy,
+	.disc_thresh = 3,
+	.phyctl_offset = REG_PHYCTL_A10,
+	.dedicated_clocks = true,
+};
+
+static const struct sun4i_usb_phy_cfg sun7i_a20_cfg = {
+	.num_phys = 3,
+	.type = sun4i_a10_phy,
+	.disc_thresh = 2,
+	.phyctl_offset = REG_PHYCTL_A10,
+	.dedicated_clocks = false,
+};
+
+static const struct sun4i_usb_phy_cfg sun8i_a23_cfg = {
+	.num_phys = 2,
+	.type = sun4i_a10_phy,
+	.disc_thresh = 3,
+	.phyctl_offset = REG_PHYCTL_A10,
+	.dedicated_clocks = true,
+};
+
+static const struct sun4i_usb_phy_cfg sun8i_a33_cfg = {
+	.num_phys = 2,
+	.type = sun8i_a33_phy,
+	.disc_thresh = 3,
+	.phyctl_offset = REG_PHYCTL_A33,
+	.dedicated_clocks = true,
+};
+
+static const struct sun4i_usb_phy_cfg sun8i_h3_cfg = {
+	.num_phys = 4,
+	.type = sun8i_h3_phy,
+	.disc_thresh = 3,
+	.dedicated_clocks = true,
+};
+
 static const struct of_device_id sun4i_usb_phy_of_match[] = {
-	{ .compatible = "allwinner,sun4i-a10-usb-phy" },
-	{ .compatible = "allwinner,sun5i-a13-usb-phy" },
-	{ .compatible = "allwinner,sun6i-a31-usb-phy" },
-	{ .compatible = "allwinner,sun7i-a20-usb-phy" },
-	{ .compatible = "allwinner,sun8i-a23-usb-phy" },
-	{ .compatible = "allwinner,sun8i-a33-usb-phy" },
+	{ .compatible = "allwinner,sun4i-a10-usb-phy", .data = &sun4i_a10_cfg },
+	{ .compatible = "allwinner,sun5i-a13-usb-phy", .data = &sun5i_a13_cfg },
+	{ .compatible = "allwinner,sun6i-a31-usb-phy", .data = &sun6i_a31_cfg },
+	{ .compatible = "allwinner,sun7i-a20-usb-phy", .data = &sun7i_a20_cfg },
+	{ .compatible = "allwinner,sun8i-a23-usb-phy", .data = &sun8i_a23_cfg },
+	{ .compatible = "allwinner,sun8i-a33-usb-phy", .data = &sun8i_a33_cfg },
+	{ .compatible = "allwinner,sun8i-h3-usb-phy", .data = &sun8i_h3_cfg },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, sun4i_usb_phy_of_match);
diff --git a/drivers/phy/phy-ti-pipe3.c b/drivers/phy/phy-ti-pipe3.c
index 93bc1120af12..0a477d24cf76 100644
--- a/drivers/phy/phy-ti-pipe3.c
+++ b/drivers/phy/phy-ti-pipe3.c
@@ -56,6 +56,18 @@
 
 #define SATA_PLL_SOFT_RESET	BIT(18)
 
+#define PIPE3_PHY_PWRCTL_CLK_CMD_MASK	0x003FC000
+#define PIPE3_PHY_PWRCTL_CLK_CMD_SHIFT	14
+
+#define PIPE3_PHY_PWRCTL_CLK_FREQ_MASK	0xFFC00000
+#define PIPE3_PHY_PWRCTL_CLK_FREQ_SHIFT	22
+
+#define PIPE3_PHY_TX_RX_POWERON		0x3
+#define PIPE3_PHY_TX_RX_POWEROFF	0x0
+
+#define PCIE_PCS_MASK			0xFF0000
+#define PCIE_PCS_DELAY_COUNT_SHIFT	0x10
+
 /*
  * This is an Empirical value that works, need to confirm the actual
  * value required for the PIPE3PHY_PLL_CONFIGURATION2.PLL_IDLE status
@@ -86,8 +98,12 @@ struct ti_pipe3 {
 	struct clk		*refclk;
 	struct clk		*div_clk;
 	struct pipe3_dpll_map	*dpll_map;
+	struct regmap		*phy_power_syscon; /* ctrl. reg. acces */
+	struct regmap		*pcs_syscon; /* ctrl. reg. acces */
 	struct regmap		*dpll_reset_syscon; /* ctrl. reg. acces */
 	unsigned int		dpll_reset_reg; /* reg. index within syscon */
+	unsigned int		power_reg; /* power reg. index within syscon */
+	unsigned int		pcie_pcs_reg; /* pcs reg. index in syscon */
 	bool			sata_refclk_enabled;
 };
 
@@ -144,20 +160,49 @@ static void ti_pipe3_disable_clocks(struct ti_pipe3 *phy);
 
 static int ti_pipe3_power_off(struct phy *x)
 {
+	u32 val;
+	int ret;
 	struct ti_pipe3 *phy = phy_get_drvdata(x);
 
-	omap_control_phy_power(phy->control_dev, 0);
+	if (!phy->phy_power_syscon) {
+		omap_control_phy_power(phy->control_dev, 0);
+		return 0;
+	}
 
-	return 0;
+	val = PIPE3_PHY_TX_RX_POWEROFF << PIPE3_PHY_PWRCTL_CLK_CMD_SHIFT;
+
+	ret = regmap_update_bits(phy->phy_power_syscon, phy->power_reg,
+				 PIPE3_PHY_PWRCTL_CLK_CMD_MASK, val);
+	return ret;
 }
 
 static int ti_pipe3_power_on(struct phy *x)
 {
+	u32 val;
+	u32 mask;
+	int ret;
+	unsigned long rate;
 	struct ti_pipe3 *phy = phy_get_drvdata(x);
 
-	omap_control_phy_power(phy->control_dev, 1);
+	if (!phy->phy_power_syscon) {
+		omap_control_phy_power(phy->control_dev, 1);
+		return 0;
+	}
 
-	return 0;
+	rate = clk_get_rate(phy->sys_clk);
+	if (!rate) {
+		dev_err(phy->dev, "Invalid clock rate\n");
+		return -EINVAL;
+	}
+	rate = rate / 1000000;
+	mask = OMAP_CTRL_PIPE3_PHY_PWRCTL_CLK_CMD_MASK |
+		  OMAP_CTRL_PIPE3_PHY_PWRCTL_CLK_FREQ_MASK;
+	val = PIPE3_PHY_TX_RX_POWERON << PIPE3_PHY_PWRCTL_CLK_CMD_SHIFT;
+	val |= rate << OMAP_CTRL_PIPE3_PHY_PWRCTL_CLK_FREQ_SHIFT;
+
+	ret = regmap_update_bits(phy->phy_power_syscon, phy->power_reg,
+				 mask, val);
+	return ret;
 }
 
 static int ti_pipe3_dpll_wait_lock(struct ti_pipe3 *phy)
@@ -229,8 +274,15 @@ static int ti_pipe3_init(struct phy *x)
 	 * 18-1804.
 	 */
 	if (of_device_is_compatible(phy->dev->of_node, "ti,phy-pipe3-pcie")) {
-		omap_control_pcie_pcs(phy->control_dev, 0x96);
-		return 0;
+		if (!phy->pcs_syscon) {
+			omap_control_pcie_pcs(phy->control_dev, 0x96);
+			return 0;
+		}
+
+		val = 0x96 << OMAP_CTRL_PCIE_PCS_DELAY_COUNT_SHIFT;
+		ret = regmap_update_bits(phy->pcs_syscon, phy->pcie_pcs_reg,
+					 PCIE_PCS_MASK, val);
+		return ret;
 	}
 
 	/* Bring it out of IDLE if it is IDLE */
@@ -308,51 +360,15 @@ static const struct phy_ops ops = {
 
 static const struct of_device_id ti_pipe3_id_table[];
 
-static int ti_pipe3_probe(struct platform_device *pdev)
+static int ti_pipe3_get_clk(struct ti_pipe3 *phy)
 {
-	struct ti_pipe3 *phy;
-	struct phy *generic_phy;
-	struct phy_provider *phy_provider;
-	struct resource *res;
-	struct device_node *node = pdev->dev.of_node;
-	struct device_node *control_node;
-	struct platform_device *control_pdev;
-	const struct of_device_id *match;
 	struct clk *clk;
+	struct device *dev = phy->dev;
+	struct device_node *node = dev->of_node;
 
-	phy = devm_kzalloc(&pdev->dev, sizeof(*phy), GFP_KERNEL);
-	if (!phy)
-		return -ENOMEM;
-
-	phy->dev		= &pdev->dev;
-
-	if (!of_device_is_compatible(node, "ti,phy-pipe3-pcie")) {
-		match = of_match_device(ti_pipe3_id_table, &pdev->dev);
-		if (!match)
-			return -EINVAL;
-
-		phy->dpll_map = (struct pipe3_dpll_map *)match->data;
-		if (!phy->dpll_map) {
-			dev_err(&pdev->dev, "no DPLL data\n");
-			return -EINVAL;
-		}
-
-		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
-						   "pll_ctrl");
-		phy->pll_ctrl_base = devm_ioremap_resource(&pdev->dev, res);
-		if (IS_ERR(phy->pll_ctrl_base))
-			return PTR_ERR(phy->pll_ctrl_base);
-
-		phy->sys_clk = devm_clk_get(phy->dev, "sysclk");
-		if (IS_ERR(phy->sys_clk)) {
-			dev_err(&pdev->dev, "unable to get sysclk\n");
-			return -EINVAL;
-		}
-	}
-
-	phy->refclk = devm_clk_get(phy->dev, "refclk");
+	phy->refclk = devm_clk_get(dev, "refclk");
 	if (IS_ERR(phy->refclk)) {
-		dev_err(&pdev->dev, "unable to get refclk\n");
+		dev_err(dev, "unable to get refclk\n");
 		/* older DTBs have missing refclk in SATA PHY
 		 * so don't bail out in case of SATA PHY.
 		 */
@@ -361,80 +377,194 @@ static int ti_pipe3_probe(struct platform_device *pdev)
 	}
 
 	if (!of_device_is_compatible(node, "ti,phy-pipe3-sata")) {
-		phy->wkupclk = devm_clk_get(phy->dev, "wkupclk");
+		phy->wkupclk = devm_clk_get(dev, "wkupclk");
 		if (IS_ERR(phy->wkupclk)) {
-			dev_err(&pdev->dev, "unable to get wkupclk\n");
+			dev_err(dev, "unable to get wkupclk\n");
 			return PTR_ERR(phy->wkupclk);
 		}
 	} else {
 		phy->wkupclk = ERR_PTR(-ENODEV);
-		phy->dpll_reset_syscon = syscon_regmap_lookup_by_phandle(node,
-							"syscon-pllreset");
-		if (IS_ERR(phy->dpll_reset_syscon)) {
-			dev_info(&pdev->dev,
-				 "can't get syscon-pllreset, sata dpll won't idle\n");
-			phy->dpll_reset_syscon = NULL;
-		} else {
-			if (of_property_read_u32_index(node,
-						       "syscon-pllreset", 1,
-						       &phy->dpll_reset_reg)) {
-				dev_err(&pdev->dev,
-					"couldn't get pllreset reg. offset\n");
-				return -EINVAL;
-			}
+	}
+
+	if (!of_device_is_compatible(node, "ti,phy-pipe3-pcie") ||
+	    phy->phy_power_syscon) {
+		phy->sys_clk = devm_clk_get(dev, "sysclk");
+		if (IS_ERR(phy->sys_clk)) {
+			dev_err(dev, "unable to get sysclk\n");
+			return -EINVAL;
 		}
 	}
 
 	if (of_device_is_compatible(node, "ti,phy-pipe3-pcie")) {
-
-		clk = devm_clk_get(phy->dev, "dpll_ref");
+		clk = devm_clk_get(dev, "dpll_ref");
 		if (IS_ERR(clk)) {
-			dev_err(&pdev->dev, "unable to get dpll ref clk\n");
+			dev_err(dev, "unable to get dpll ref clk\n");
 			return PTR_ERR(clk);
 		}
 		clk_set_rate(clk, 1500000000);
 
-		clk = devm_clk_get(phy->dev, "dpll_ref_m2");
+		clk = devm_clk_get(dev, "dpll_ref_m2");
 		if (IS_ERR(clk)) {
-			dev_err(&pdev->dev, "unable to get dpll ref m2 clk\n");
+			dev_err(dev, "unable to get dpll ref m2 clk\n");
 			return PTR_ERR(clk);
 		}
 		clk_set_rate(clk, 100000000);
 
-		clk = devm_clk_get(phy->dev, "phy-div");
+		clk = devm_clk_get(dev, "phy-div");
 		if (IS_ERR(clk)) {
-			dev_err(&pdev->dev, "unable to get phy-div clk\n");
+			dev_err(dev, "unable to get phy-div clk\n");
 			return PTR_ERR(clk);
 		}
 		clk_set_rate(clk, 100000000);
 
-		phy->div_clk = devm_clk_get(phy->dev, "div-clk");
+		phy->div_clk = devm_clk_get(dev, "div-clk");
 		if (IS_ERR(phy->div_clk)) {
-			dev_err(&pdev->dev, "unable to get div-clk\n");
+			dev_err(dev, "unable to get div-clk\n");
 			return PTR_ERR(phy->div_clk);
 		}
 	} else {
 		phy->div_clk = ERR_PTR(-ENODEV);
 	}
 
-	control_node = of_parse_phandle(node, "ctrl-module", 0);
-	if (!control_node) {
-		dev_err(&pdev->dev, "Failed to get control device phandle\n");
-		return -EINVAL;
+	return 0;
+}
+
+static int ti_pipe3_get_sysctrl(struct ti_pipe3 *phy)
+{
+	struct device *dev = phy->dev;
+	struct device_node *node = dev->of_node;
+	struct device_node *control_node;
+	struct platform_device *control_pdev;
+
+	phy->phy_power_syscon = syscon_regmap_lookup_by_phandle(node,
+							"syscon-phy-power");
+	if (IS_ERR(phy->phy_power_syscon)) {
+		dev_dbg(dev,
+			"can't get syscon-phy-power, using control device\n");
+		phy->phy_power_syscon = NULL;
+	} else {
+		if (of_property_read_u32_index(node,
+					       "syscon-phy-power", 1,
+					       &phy->power_reg)) {
+			dev_err(dev, "couldn't get power reg. offset\n");
+			return -EINVAL;
+		}
+	}
+
+	if (!phy->phy_power_syscon) {
+		control_node = of_parse_phandle(node, "ctrl-module", 0);
+		if (!control_node) {
+			dev_err(dev, "Failed to get control device phandle\n");
+			return -EINVAL;
+		}
+
+		control_pdev = of_find_device_by_node(control_node);
+		if (!control_pdev) {
+			dev_err(dev, "Failed to get control device\n");
+			return -EINVAL;
+		}
+
+		phy->control_dev = &control_pdev->dev;
+	}
+
+	if (of_device_is_compatible(node, "ti,phy-pipe3-pcie")) {
+		phy->pcs_syscon = syscon_regmap_lookup_by_phandle(node,
+								  "syscon-pcs");
+		if (IS_ERR(phy->pcs_syscon)) {
+			dev_dbg(dev,
+				"can't get syscon-pcs, using omap control\n");
+			phy->pcs_syscon = NULL;
+		} else {
+			if (of_property_read_u32_index(node,
+						       "syscon-pcs", 1,
+						       &phy->pcie_pcs_reg)) {
+				dev_err(dev,
+					"couldn't get pcie pcs reg. offset\n");
+				return -EINVAL;
+			}
+		}
+	}
+
+	if (of_device_is_compatible(node, "ti,phy-pipe3-sata")) {
+		phy->dpll_reset_syscon = syscon_regmap_lookup_by_phandle(node,
+							"syscon-pllreset");
+		if (IS_ERR(phy->dpll_reset_syscon)) {
+			dev_info(dev,
+				 "can't get syscon-pllreset, sata dpll won't idle\n");
+			phy->dpll_reset_syscon = NULL;
+		} else {
+			if (of_property_read_u32_index(node,
+						       "syscon-pllreset", 1,
+						       &phy->dpll_reset_reg)) {
+				dev_err(dev,
+					"couldn't get pllreset reg. offset\n");
+				return -EINVAL;
+			}
+		}
 	}
 
-	control_pdev = of_find_device_by_node(control_node);
-	if (!control_pdev) {
-		dev_err(&pdev->dev, "Failed to get control device\n");
+	return 0;
+}
+
+static int ti_pipe3_get_pll_base(struct ti_pipe3 *phy)
+{
+	struct resource *res;
+	const struct of_device_id *match;
+	struct device *dev = phy->dev;
+	struct device_node *node = dev->of_node;
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if (of_device_is_compatible(node, "ti,phy-pipe3-pcie"))
+		return 0;
+
+	match = of_match_device(ti_pipe3_id_table, dev);
+	if (!match)
+		return -EINVAL;
+
+	phy->dpll_map = (struct pipe3_dpll_map *)match->data;
+	if (!phy->dpll_map) {
+		dev_err(dev, "no DPLL data\n");
 		return -EINVAL;
 	}
 
-	phy->control_dev = &control_pdev->dev;
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+					   "pll_ctrl");
+	phy->pll_ctrl_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(phy->pll_ctrl_base))
+		return PTR_ERR(phy->pll_ctrl_base);
+
+	return 0;
+}
 
-	omap_control_phy_power(phy->control_dev, 0);
+static int ti_pipe3_probe(struct platform_device *pdev)
+{
+	struct ti_pipe3 *phy;
+	struct phy *generic_phy;
+	struct phy_provider *phy_provider;
+	struct device_node *node = pdev->dev.of_node;
+	struct device *dev = &pdev->dev;
+	int ret;
+
+	phy = devm_kzalloc(dev, sizeof(*phy), GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	phy->dev		= dev;
+
+	ret = ti_pipe3_get_pll_base(phy);
+	if (ret)
+		return ret;
+
+	ret = ti_pipe3_get_sysctrl(phy);
+	if (ret)
+		return ret;
+
+	ret = ti_pipe3_get_clk(phy);
+	if (ret)
+		return ret;
 
 	platform_set_drvdata(pdev, phy);
-	pm_runtime_enable(phy->dev);
+	pm_runtime_enable(dev);
 
 	/*
 	 * Prevent auto-disable of refclk for SATA PHY due to Errata i783
@@ -446,13 +576,15 @@ static int ti_pipe3_probe(struct platform_device *pdev)
 		}
 	}
 
-	generic_phy = devm_phy_create(phy->dev, NULL, &ops);
+	generic_phy = devm_phy_create(dev, NULL, &ops);
 	if (IS_ERR(generic_phy))
 		return PTR_ERR(generic_phy);
 
 	phy_set_drvdata(generic_phy, phy);
-	phy_provider = devm_of_phy_provider_register(phy->dev,
-			of_phy_simple_xlate);
+
+	ti_pipe3_power_off(generic_phy);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
 	if (IS_ERR(phy_provider))
 		return PTR_ERR(phy_provider);
 
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/phy-twl4030-usb.c
index 3a707dd14238..840f3eae428b 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/phy-twl4030-usb.c
@@ -34,7 +34,7 @@
 #include <linux/usb/otg.h>
 #include <linux/phy/phy.h>
 #include <linux/pm_runtime.h>
-#include <linux/usb/musb-omap.h>
+#include <linux/usb/musb.h>
 #include <linux/usb/ulpi.h>
 #include <linux/i2c/twl.h>
 #include <linux/regulator/consumer.h>
@@ -148,10 +148,10 @@
  * If VBUS is valid or ID is ground, then we know a
  * cable is present and we need to be runtime-enabled
  */
-static inline bool cable_present(enum omap_musb_vbus_id_status stat)
+static inline bool cable_present(enum musb_vbus_id_status stat)
 {
-	return stat == OMAP_MUSB_VBUS_VALID ||
-		stat == OMAP_MUSB_ID_GROUND;
+	return stat == MUSB_VBUS_VALID ||
+		stat == MUSB_ID_GROUND;
 }
 
 struct twl4030_usb {
@@ -170,7 +170,7 @@ struct twl4030_usb {
 	enum twl4030_usb_mode	usb_mode;
 
 	int			irq;
-	enum omap_musb_vbus_id_status linkstat;
+	enum musb_vbus_id_status linkstat;
 	bool			vbus_supplied;
 
 	struct delayed_work	id_workaround_work;
@@ -276,11 +276,11 @@ static bool twl4030_is_driving_vbus(struct twl4030_usb *twl)
 	return (ret & (ULPI_OTG_DRVVBUS | ULPI_OTG_CHRGVBUS)) ? true : false;
 }
 
-static enum omap_musb_vbus_id_status
+static enum musb_vbus_id_status
 	twl4030_usb_linkstat(struct twl4030_usb *twl)
 {
 	int	status;
-	enum omap_musb_vbus_id_status linkstat = OMAP_MUSB_UNKNOWN;
+	enum musb_vbus_id_status linkstat = MUSB_UNKNOWN;
 
 	twl->vbus_supplied = false;
 
@@ -306,14 +306,14 @@ static enum omap_musb_vbus_id_status
 		}
 
 		if (status & BIT(2))
-			linkstat = OMAP_MUSB_ID_GROUND;
+			linkstat = MUSB_ID_GROUND;
 		else if (status & BIT(7))
-			linkstat = OMAP_MUSB_VBUS_VALID;
+			linkstat = MUSB_VBUS_VALID;
 		else
-			linkstat = OMAP_MUSB_VBUS_OFF;
+			linkstat = MUSB_VBUS_OFF;
 	} else {
-		if (twl->linkstat != OMAP_MUSB_UNKNOWN)
-			linkstat = OMAP_MUSB_VBUS_OFF;
+		if (twl->linkstat != MUSB_UNKNOWN)
+			linkstat = MUSB_VBUS_OFF;
 	}
 
 	dev_dbg(twl->dev, "HW_CONDITIONS 0x%02x/%d; link %d\n",
@@ -535,7 +535,7 @@ static DEVICE_ATTR(vbus, 0444, twl4030_usb_vbus_show, NULL);
 static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 {
 	struct twl4030_usb *twl = _twl;
-	enum omap_musb_vbus_id_status status;
+	enum musb_vbus_id_status status;
 	bool status_changed = false;
 
 	status = twl4030_usb_linkstat(twl);
@@ -567,11 +567,11 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 			pm_runtime_mark_last_busy(twl->dev);
 			pm_runtime_put_autosuspend(twl->dev);
 		}
-		omap_musb_mailbox(status);
+		musb_mailbox(status);
 	}
 
 	/* don't schedule during sleep - irq works right then */
-	if (status == OMAP_MUSB_ID_GROUND && pm_runtime_active(twl->dev)) {
+	if (status == MUSB_ID_GROUND && pm_runtime_active(twl->dev)) {
 		cancel_delayed_work(&twl->id_workaround_work);
 		schedule_delayed_work(&twl->id_workaround_work, HZ);
 	}
@@ -670,7 +670,7 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	twl->dev		= &pdev->dev;
 	twl->irq		= platform_get_irq(pdev, 0);
 	twl->vbus_supplied	= false;
-	twl->linkstat		= OMAP_MUSB_UNKNOWN;
+	twl->linkstat		= MUSB_UNKNOWN;
 
 	twl->phy.dev		= twl->dev;
 	twl->phy.label		= "twl4030";
@@ -715,6 +715,7 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	pm_runtime_use_autosuspend(&pdev->dev);
 	pm_runtime_set_autosuspend_delay(&pdev->dev, 2000);
 	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
 
 	/* Our job is to use irqs and status from the power module
 	 * to keep the transceiver disabled when nothing's connected.
@@ -750,6 +751,7 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 	struct twl4030_usb *twl = platform_get_drvdata(pdev);
 	int val;
 
+	usb_remove_phy(&twl->phy);
 	pm_runtime_get_sync(twl->dev);
 	cancel_delayed_work(&twl->id_workaround_work);
 	device_remove_file(twl->dev, &dev_attr_vbus);
@@ -757,6 +759,13 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 	/* set transceiver mode to power on defaults */
 	twl4030_usb_set_mode(twl, -1);
 
+	/* idle ulpi before powering off */
+	if (cable_present(twl->linkstat))
+		pm_runtime_put_noidle(twl->dev);
+	pm_runtime_mark_last_busy(twl->dev);
+	pm_runtime_put_sync_suspend(twl->dev);
+	pm_runtime_disable(twl->dev);
+
 	/* autogate 60MHz ULPI clock,
 	 * clear dpll clock request for i2c access,
 	 * disable 32KHz
@@ -771,11 +780,6 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 	/* disable complete OTG block */
 	twl4030_usb_clear_bits(twl, POWER_CTRL, POWER_CTRL_OTG_ENAB);
 
-	if (cable_present(twl->linkstat))
-		pm_runtime_put_noidle(twl->dev);
-	pm_runtime_mark_last_busy(twl->dev);
-	pm_runtime_put(twl->dev);
-
 	return 0;
 }
 
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index 312c78b27a32..99a4c10ed43f 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -244,7 +244,7 @@ config PINCTRL_ZYNQ
 	select PINMUX
 	select GENERIC_PINCONF
 	help
-	  This selectes the pinctrl driver for Xilinx Zynq.
+	  This selects the pinctrl driver for Xilinx Zynq.
 
 source "drivers/pinctrl/bcm/Kconfig"
 source "drivers/pinctrl/berlin/Kconfig"
@@ -252,6 +252,7 @@ source "drivers/pinctrl/freescale/Kconfig"
 source "drivers/pinctrl/intel/Kconfig"
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/nomadik/Kconfig"
+source "drivers/pinctrl/pxa/Kconfig"
 source "drivers/pinctrl/qcom/Kconfig"
 source "drivers/pinctrl/samsung/Kconfig"
 source "drivers/pinctrl/sh-pfc/Kconfig"
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 738cb4929a49..bf1b5ca5180b 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -41,15 +41,16 @@ obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
 obj-$(CONFIG_PINCTRL_ZYNQ)	+= pinctrl-zynq.o
 
 obj-$(CONFIG_ARCH_BCM)		+= bcm/
-obj-$(CONFIG_ARCH_BERLIN)	+= berlin/
+obj-$(CONFIG_PINCTRL_BERLIN)	+= berlin/
 obj-y				+= freescale/
 obj-$(CONFIG_X86)		+= intel/
-obj-$(CONFIG_PLAT_ORION)        += mvebu/
+obj-$(CONFIG_PINCTRL_MVEBU)	+= mvebu/
 obj-y				+= nomadik/
+obj-$(CONFIG_ARCH_PXA)		+= pxa/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
 obj-$(CONFIG_PINCTRL_SAMSUNG)	+= samsung/
 obj-$(CONFIG_PINCTRL_SH_PFC)	+= sh-pfc/
-obj-$(CONFIG_PLAT_SPEAR)	+= spear/
+obj-$(CONFIG_PINCTRL_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
 obj-$(CONFIG_PINCTRL_UNIPHIER)	+= uniphier/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
diff --git a/drivers/pinctrl/bcm/Kconfig b/drivers/pinctrl/bcm/Kconfig
index cd11d4d9ad58..2cc74384cafa 100644
--- a/drivers/pinctrl/bcm/Kconfig
+++ b/drivers/pinctrl/bcm/Kconfig
@@ -9,6 +9,7 @@ config PINCTRL_BCM281XX
 	select PINCONF
 	select GENERIC_PINCONF
 	select REGMAP_MMIO
+	default ARCH_BCM_MOBILE
 	help
 	  Say Y here to support Broadcom BCM281xx pinctrl driver, which is used
 	  for the BCM281xx SoC family, including BCM11130, BCM11140, BCM11351,
@@ -20,27 +21,41 @@ config PINCTRL_BCM2835
 	select PINMUX
 	select PINCONF
 
-config PINCTRL_CYGNUS_GPIO
-	bool "Broadcom Cygnus GPIO (with PINCONF) driver"
-	depends on OF_GPIO && ARCH_BCM_CYGNUS
+config PINCTRL_IPROC_GPIO
+	bool "Broadcom iProc GPIO (with PINCONF) driver"
+	depends on OF_GPIO && (ARCH_BCM_IPROC || COMPILE_TEST)
 	select GPIOLIB_IRQCHIP
 	select PINCONF
 	select GENERIC_PINCONF
-	default ARCH_BCM_CYGNUS
+	default ARCH_BCM_IPROC
 	help
-	  Say yes here to enable the Broadcom Cygnus GPIO driver.
+	  Say yes here to enable the Broadcom iProc GPIO driver.
+
+	  The Broadcom iProc based SoCs- Cygnus, NS2, NSP and Stingray, use
+	  same GPIO Controller IP hence this driver could be used for all.
 
 	  The Broadcom Cygnus SoC has 3 GPIO controllers including the ASIU
 	  GPIO controller (ASIU), the chipCommonG GPIO controller (CCM), and
 	  the always-ON GPIO controller (CRMU/AON). All 3 GPIO controllers are
 	  supported by this driver.
 
-	  All 3 Cygnus GPIO controllers support basic PINCONF functions such
+	  The Broadcom NSP has two GPIO controllers including the ChipcommonA
+	  GPIO, the ChipcommonB GPIO. Later controller is supported by this
+	  driver.
+
+	  The Broadcom NS2 has two GPIO controller including the CRMU GPIO,
+	  the ChipcommonG GPIO. Both controllers are supported by this driver.
+
+	  The Broadcom Stingray GPIO controllers are supported by this driver.
+
+	  All above SoCs GPIO controllers support basic PINCONF functions such
 	  as bias pull up, pull down, and drive strength configurations, when
 	  these pins are muxed to GPIO.
 
-	  Pins from the ASIU GPIO can be individually muxed to GPIO function,
-	  through interaction with the Cygnus IOMUX controller.
+	  It provides the framework where pins from the individual GPIO can be
+	  individually muxed to GPIO function, through interaction with the
+	  SoCs IOMUX controller. This features could be used only on SoCs which
+	  support individual pin muxing.
 
 config PINCTRL_CYGNUS_MUX
 	bool "Broadcom Cygnus IOMUX driver"
@@ -54,3 +69,20 @@ config PINCTRL_CYGNUS_MUX
 	  The Broadcom Cygnus IOMUX driver supports group based IOMUX
 	  configuration, with the exception that certain individual pins
 	  can be overrided to GPIO function
+
+config PINCTRL_NSP_GPIO
+	bool "Broadcom NSP GPIO (with PINCONF) driver"
+	depends on OF_GPIO && (ARCH_BCM_NSP || COMPILE_TEST)
+	select GPIOLIB_IRQCHIP
+	select PINCONF
+	select GENERIC_PINCONF
+	default ARCH_BCM_NSP
+	help
+	  Say yes here to enable the Broadcom NSP GPIO driver.
+
+	  The Broadcom Northstar Plus SoC ChipcommonA GPIO controller is
+	  supported by this driver.
+
+	  The ChipcommonA GPIO controller support basic PINCONF functions such
+	  as bias pull up, pull down, and drive strength configurations, when
+	  these pins are muxed to GPIO.
diff --git a/drivers/pinctrl/bcm/Makefile b/drivers/pinctrl/bcm/Makefile
index 2b2f70ee804c..6148367d5e8c 100644
--- a/drivers/pinctrl/bcm/Makefile
+++ b/drivers/pinctrl/bcm/Makefile
@@ -2,5 +2,6 @@
 
 obj-$(CONFIG_PINCTRL_BCM281XX)		+= pinctrl-bcm281xx.o
 obj-$(CONFIG_PINCTRL_BCM2835)		+= pinctrl-bcm2835.o
-obj-$(CONFIG_PINCTRL_CYGNUS_GPIO)	+= pinctrl-cygnus-gpio.o
+obj-$(CONFIG_PINCTRL_IPROC_GPIO)	+= pinctrl-iproc-gpio.o
 obj-$(CONFIG_PINCTRL_CYGNUS_MUX)	+= pinctrl-cygnus-mux.o
+obj-$(CONFIG_PINCTRL_NSP_GPIO)		+= pinctrl-nsp-gpio.o
diff --git a/drivers/pinctrl/bcm/pinctrl-bcm2835.c b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
index 2e6ca69635aa..0f5997ceb494 100644
--- a/drivers/pinctrl/bcm/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/bcm/pinctrl-bcm2835.c
@@ -23,7 +23,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/err.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -337,14 +337,14 @@ static int bcm2835_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int bcm2835_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct bcm2835_pinctrl *pc = dev_get_drvdata(chip->dev);
+	struct bcm2835_pinctrl *pc = gpiochip_get_data(chip);
 
 	return bcm2835_gpio_get_bit(pc, GPLEV0, offset);
 }
 
 static void bcm2835_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct bcm2835_pinctrl *pc = dev_get_drvdata(chip->dev);
+	struct bcm2835_pinctrl *pc = gpiochip_get_data(chip);
 
 	bcm2835_gpio_set_bit(pc, value ? GPSET0 : GPCLR0, offset);
 }
@@ -358,7 +358,7 @@ static int bcm2835_gpio_direction_output(struct gpio_chip *chip,
 
 static int bcm2835_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct bcm2835_pinctrl *pc = dev_get_drvdata(chip->dev);
+	struct bcm2835_pinctrl *pc = gpiochip_get_data(chip);
 
 	return irq_linear_revmap(pc->irq_domain, offset);
 }
@@ -795,7 +795,7 @@ static int bcm2835_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
 	return 0;
 
 out:
-	kfree(maps);
+	bcm2835_pctl_dt_free_map(pctldev, maps, num_pins * maps_per_pin);
 	return err;
 }
 
@@ -964,7 +964,7 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 		return PTR_ERR(pc->base);
 
 	pc->gpio_chip = bcm2835_gpio_chip;
-	pc->gpio_chip.dev = dev;
+	pc->gpio_chip.parent = dev;
 	pc->gpio_chip.of_node = np;
 
 	pc->irq_domain = irq_domain_add_linear(np, BCM2835_NUM_GPIOS,
@@ -1021,7 +1021,7 @@ static int bcm2835_pinctrl_probe(struct platform_device *pdev)
 		}
 	}
 
-	err = gpiochip_add(&pc->gpio_chip);
+	err = gpiochip_add_data(&pc->gpio_chip, pc);
 	if (err) {
 		dev_err(dev, "could not add GPIO chip\n");
 		return err;
diff --git a/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c b/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c
index 12a48f498b75..3b2ac8f771ed 100644
--- a/drivers/pinctrl/bcm/pinctrl-cygnus-gpio.c
+++ b/drivers/pinctrl/bcm/pinctrl-iproc-gpio.c
@@ -10,14 +10,16 @@
  * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * This file contains the Broadcom Cygnus GPIO driver that supports 3
- * GPIO controllers on Cygnus including the ASIU GPIO controller, the
+ * This file contains the Broadcom Iproc GPIO driver that supports 3
+ * GPIO controllers on Iproc including the ASIU GPIO controller, the
  * chipCommonG GPIO controller, and the always-on GPIO controller. Basic
  * PINCONF such as bias pull up/down, and drive strength are also supported
  * in this driver.
  *
- * Pins from the ASIU GPIO can be individually muxed to GPIO function,
- * through the interaction with the Cygnus IOMUX controller
+ * It provides the functionality where pins from the GPIO can be
+ * individually muxed to GPIO function, if individual pad
+ * configuration is supported, through the interaction with respective
+ * SoCs IOMUX controller.
  */
 
 #include <linux/kernel.h>
@@ -34,42 +36,42 @@
 
 #include "../pinctrl-utils.h"
 
-#define CYGNUS_GPIO_DATA_IN_OFFSET   0x00
-#define CYGNUS_GPIO_DATA_OUT_OFFSET  0x04
-#define CYGNUS_GPIO_OUT_EN_OFFSET    0x08
-#define CYGNUS_GPIO_INT_TYPE_OFFSET  0x0c
-#define CYGNUS_GPIO_INT_DE_OFFSET    0x10
-#define CYGNUS_GPIO_INT_EDGE_OFFSET  0x14
-#define CYGNUS_GPIO_INT_MSK_OFFSET   0x18
-#define CYGNUS_GPIO_INT_STAT_OFFSET  0x1c
-#define CYGNUS_GPIO_INT_MSTAT_OFFSET 0x20
-#define CYGNUS_GPIO_INT_CLR_OFFSET   0x24
-#define CYGNUS_GPIO_PAD_RES_OFFSET   0x34
-#define CYGNUS_GPIO_RES_EN_OFFSET    0x38
+#define IPROC_GPIO_DATA_IN_OFFSET   0x00
+#define IPROC_GPIO_DATA_OUT_OFFSET  0x04
+#define IPROC_GPIO_OUT_EN_OFFSET    0x08
+#define IPROC_GPIO_INT_TYPE_OFFSET  0x0c
+#define IPROC_GPIO_INT_DE_OFFSET    0x10
+#define IPROC_GPIO_INT_EDGE_OFFSET  0x14
+#define IPROC_GPIO_INT_MSK_OFFSET   0x18
+#define IPROC_GPIO_INT_STAT_OFFSET  0x1c
+#define IPROC_GPIO_INT_MSTAT_OFFSET 0x20
+#define IPROC_GPIO_INT_CLR_OFFSET   0x24
+#define IPROC_GPIO_PAD_RES_OFFSET   0x34
+#define IPROC_GPIO_RES_EN_OFFSET    0x38
 
 /* drive strength control for ASIU GPIO */
-#define CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET 0x58
+#define IPROC_GPIO_ASIU_DRV0_CTRL_OFFSET 0x58
 
 /* drive strength control for CCM/CRMU (AON) GPIO */
-#define CYGNUS_GPIO_DRV0_CTRL_OFFSET  0x00
+#define IPROC_GPIO_DRV0_CTRL_OFFSET  0x00
 
 #define GPIO_BANK_SIZE 0x200
 #define NGPIOS_PER_BANK 32
 #define GPIO_BANK(pin) ((pin) / NGPIOS_PER_BANK)
 
-#define CYGNUS_GPIO_REG(pin, reg) (GPIO_BANK(pin) * GPIO_BANK_SIZE + (reg))
-#define CYGNUS_GPIO_SHIFT(pin) ((pin) % NGPIOS_PER_BANK)
+#define IPROC_GPIO_REG(pin, reg) (GPIO_BANK(pin) * GPIO_BANK_SIZE + (reg))
+#define IPROC_GPIO_SHIFT(pin) ((pin) % NGPIOS_PER_BANK)
 
 #define GPIO_DRV_STRENGTH_BIT_SHIFT  20
 #define GPIO_DRV_STRENGTH_BITS       3
 #define GPIO_DRV_STRENGTH_BIT_MASK   ((1 << GPIO_DRV_STRENGTH_BITS) - 1)
 
 /*
- * Cygnus GPIO core
+ * Iproc GPIO core
  *
  * @dev: pointer to device
- * @base: I/O register base for Cygnus GPIO controller
- * @io_ctrl: I/O register base for certain type of Cygnus GPIO controller that
+ * @base: I/O register base for Iproc GPIO controller
+ * @io_ctrl: I/O register base for certain type of Iproc GPIO controller that
  * has the PINCONF support implemented outside of the GPIO block
  * @lock: lock to protect access to I/O registers
  * @gc: GPIO chip
@@ -79,7 +81,7 @@
  * @pctl: pointer to pinctrl_dev
  * @pctldesc: pinctrl descriptor
  */
-struct cygnus_gpio {
+struct iproc_gpio {
 	struct device *dev;
 
 	void __iomem *base;
@@ -96,33 +98,33 @@ struct cygnus_gpio {
 	struct pinctrl_desc pctldesc;
 };
 
-static inline struct cygnus_gpio *to_cygnus_gpio(struct gpio_chip *gc)
+static inline struct iproc_gpio *to_iproc_gpio(struct gpio_chip *gc)
 {
-	return container_of(gc, struct cygnus_gpio, gc);
+	return container_of(gc, struct iproc_gpio, gc);
 }
 
 /*
  * Mapping from PINCONF pins to GPIO pins is 1-to-1
  */
-static inline unsigned cygnus_pin_to_gpio(unsigned pin)
+static inline unsigned iproc_pin_to_gpio(unsigned pin)
 {
 	return pin;
 }
 
 /**
- *  cygnus_set_bit - set or clear one bit (corresponding to the GPIO pin) in a
- *  Cygnus GPIO register
+ *  iproc_set_bit - set or clear one bit (corresponding to the GPIO pin) in a
+ *  Iproc GPIO register
  *
- *  @cygnus_gpio: Cygnus GPIO device
+ *  @iproc_gpio: Iproc GPIO device
  *  @reg: register offset
  *  @gpio: GPIO pin
  *  @set: set or clear
  */
-static inline void cygnus_set_bit(struct cygnus_gpio *chip, unsigned int reg,
+static inline void iproc_set_bit(struct iproc_gpio *chip, unsigned int reg,
 				  unsigned gpio, bool set)
 {
-	unsigned int offset = CYGNUS_GPIO_REG(gpio, reg);
-	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	unsigned int offset = IPROC_GPIO_REG(gpio, reg);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
 	u32 val;
 
 	val = readl(chip->base + offset);
@@ -133,19 +135,19 @@ static inline void cygnus_set_bit(struct cygnus_gpio *chip, unsigned int reg,
 	writel(val, chip->base + offset);
 }
 
-static inline bool cygnus_get_bit(struct cygnus_gpio *chip, unsigned int reg,
+static inline bool iproc_get_bit(struct iproc_gpio *chip, unsigned int reg,
 				  unsigned gpio)
 {
-	unsigned int offset = CYGNUS_GPIO_REG(gpio, reg);
-	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	unsigned int offset = IPROC_GPIO_REG(gpio, reg);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
 
 	return !!(readl(chip->base + offset) & BIT(shift));
 }
 
-static void cygnus_gpio_irq_handler(struct irq_desc *desc)
+static void iproc_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	struct irq_chip *irq_chip = irq_desc_get_chip(desc);
 	int i, bit;
 
@@ -154,7 +156,7 @@ static void cygnus_gpio_irq_handler(struct irq_desc *desc)
 	/* go through the entire GPIO banks and handle all interrupts */
 	for (i = 0; i < chip->num_banks; i++) {
 		unsigned long val = readl(chip->base + (i * GPIO_BANK_SIZE) +
-					  CYGNUS_GPIO_INT_MSTAT_OFFSET);
+					  IPROC_GPIO_INT_MSTAT_OFFSET);
 
 		for_each_set_bit(bit, &val, NGPIOS_PER_BANK) {
 			unsigned pin = NGPIOS_PER_BANK * i + bit;
@@ -165,7 +167,7 @@ static void cygnus_gpio_irq_handler(struct irq_desc *desc)
 			 * handler, so we do not leave any window
 			 */
 			writel(BIT(bit), chip->base + (i * GPIO_BANK_SIZE) +
-			       CYGNUS_GPIO_INT_CLR_OFFSET);
+			       IPROC_GPIO_INT_CLR_OFFSET);
 
 			generic_handle_irq(child_irq);
 		}
@@ -175,60 +177,60 @@ static void cygnus_gpio_irq_handler(struct irq_desc *desc)
 }
 
 
-static void cygnus_gpio_irq_ack(struct irq_data *d)
+static void iproc_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned gpio = d->hwirq;
-	unsigned int offset = CYGNUS_GPIO_REG(gpio,
-			CYGNUS_GPIO_INT_CLR_OFFSET);
-	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	unsigned int offset = IPROC_GPIO_REG(gpio,
+			IPROC_GPIO_INT_CLR_OFFSET);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
 	u32 val = BIT(shift);
 
 	writel(val, chip->base + offset);
 }
 
 /**
- *  cygnus_gpio_irq_set_mask - mask/unmask a GPIO interrupt
+ *  iproc_gpio_irq_set_mask - mask/unmask a GPIO interrupt
  *
  *  @d: IRQ chip data
  *  @unmask: mask/unmask GPIO interrupt
  */
-static void cygnus_gpio_irq_set_mask(struct irq_data *d, bool unmask)
+static void iproc_gpio_irq_set_mask(struct irq_data *d, bool unmask)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned gpio = d->hwirq;
 
-	cygnus_set_bit(chip, CYGNUS_GPIO_INT_MSK_OFFSET, gpio, unmask);
+	iproc_set_bit(chip, IPROC_GPIO_INT_MSK_OFFSET, gpio, unmask);
 }
 
-static void cygnus_gpio_irq_mask(struct irq_data *d)
+static void iproc_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_gpio_irq_set_mask(d, false);
+	iproc_gpio_irq_set_mask(d, false);
 	spin_unlock_irqrestore(&chip->lock, flags);
 }
 
-static void cygnus_gpio_irq_unmask(struct irq_data *d)
+static void iproc_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_gpio_irq_set_mask(d, true);
+	iproc_gpio_irq_set_mask(d, true);
 	spin_unlock_irqrestore(&chip->lock, flags);
 }
 
-static int cygnus_gpio_irq_set_type(struct irq_data *d, unsigned int type)
+static int iproc_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned gpio = d->hwirq;
 	bool level_triggered = false;
 	bool dual_edge = false;
@@ -263,10 +265,10 @@ static int cygnus_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	}
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_set_bit(chip, CYGNUS_GPIO_INT_TYPE_OFFSET, gpio,
+	iproc_set_bit(chip, IPROC_GPIO_INT_TYPE_OFFSET, gpio,
 		       level_triggered);
-	cygnus_set_bit(chip, CYGNUS_GPIO_INT_DE_OFFSET, gpio, dual_edge);
-	cygnus_set_bit(chip, CYGNUS_GPIO_INT_EDGE_OFFSET, gpio,
+	iproc_set_bit(chip, IPROC_GPIO_INT_DE_OFFSET, gpio, dual_edge);
+	iproc_set_bit(chip, IPROC_GPIO_INT_EDGE_OFFSET, gpio,
 		       rising_or_high);
 	spin_unlock_irqrestore(&chip->lock, flags);
 
@@ -277,32 +279,32 @@ static int cygnus_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	return 0;
 }
 
-static struct irq_chip cygnus_gpio_irq_chip = {
-	.name = "bcm-cygnus-gpio",
-	.irq_ack = cygnus_gpio_irq_ack,
-	.irq_mask = cygnus_gpio_irq_mask,
-	.irq_unmask = cygnus_gpio_irq_unmask,
-	.irq_set_type = cygnus_gpio_irq_set_type,
+static struct irq_chip iproc_gpio_irq_chip = {
+	.name = "bcm-iproc-gpio",
+	.irq_ack = iproc_gpio_irq_ack,
+	.irq_mask = iproc_gpio_irq_mask,
+	.irq_unmask = iproc_gpio_irq_unmask,
+	.irq_set_type = iproc_gpio_irq_set_type,
 };
 
 /*
- * Request the Cygnus IOMUX pinmux controller to mux individual pins to GPIO
+ * Request the Iproc IOMUX pinmux controller to mux individual pins to GPIO
  */
-static int cygnus_gpio_request(struct gpio_chip *gc, unsigned offset)
+static int iproc_gpio_request(struct gpio_chip *gc, unsigned offset)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned gpio = gc->base + offset;
 
-	/* not all Cygnus GPIO pins can be muxed individually */
+	/* not all Iproc GPIO pins can be muxed individually */
 	if (!chip->pinmux_is_supported)
 		return 0;
 
 	return pinctrl_request_gpio(gpio);
 }
 
-static void cygnus_gpio_free(struct gpio_chip *gc, unsigned offset)
+static void iproc_gpio_free(struct gpio_chip *gc, unsigned offset)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned gpio = gc->base + offset;
 
 	if (!chip->pinmux_is_supported)
@@ -311,13 +313,13 @@ static void cygnus_gpio_free(struct gpio_chip *gc, unsigned offset)
 	pinctrl_free_gpio(gpio);
 }
 
-static int cygnus_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
+static int iproc_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_set_bit(chip, CYGNUS_GPIO_OUT_EN_OFFSET, gpio, false);
+	iproc_set_bit(chip, IPROC_GPIO_OUT_EN_OFFSET, gpio, false);
 	spin_unlock_irqrestore(&chip->lock, flags);
 
 	dev_dbg(chip->dev, "gpio:%u set input\n", gpio);
@@ -325,15 +327,15 @@ static int cygnus_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
 	return 0;
 }
 
-static int cygnus_gpio_direction_output(struct gpio_chip *gc, unsigned gpio,
+static int iproc_gpio_direction_output(struct gpio_chip *gc, unsigned gpio,
 					int val)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_set_bit(chip, CYGNUS_GPIO_OUT_EN_OFFSET, gpio, true);
-	cygnus_set_bit(chip, CYGNUS_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
+	iproc_set_bit(chip, IPROC_GPIO_OUT_EN_OFFSET, gpio, true);
+	iproc_set_bit(chip, IPROC_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
 	spin_unlock_irqrestore(&chip->lock, flags);
 
 	dev_dbg(chip->dev, "gpio:%u set output, value:%d\n", gpio, val);
@@ -341,29 +343,29 @@ static int cygnus_gpio_direction_output(struct gpio_chip *gc, unsigned gpio,
 	return 0;
 }
 
-static void cygnus_gpio_set(struct gpio_chip *gc, unsigned gpio, int val)
+static void iproc_gpio_set(struct gpio_chip *gc, unsigned gpio, int val)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	cygnus_set_bit(chip, CYGNUS_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
+	iproc_set_bit(chip, IPROC_GPIO_DATA_OUT_OFFSET, gpio, !!(val));
 	spin_unlock_irqrestore(&chip->lock, flags);
 
 	dev_dbg(chip->dev, "gpio:%u set, value:%d\n", gpio, val);
 }
 
-static int cygnus_gpio_get(struct gpio_chip *gc, unsigned gpio)
+static int iproc_gpio_get(struct gpio_chip *gc, unsigned gpio)
 {
-	struct cygnus_gpio *chip = to_cygnus_gpio(gc);
-	unsigned int offset = CYGNUS_GPIO_REG(gpio,
-					      CYGNUS_GPIO_DATA_IN_OFFSET);
-	unsigned int shift = CYGNUS_GPIO_SHIFT(gpio);
+	struct iproc_gpio *chip = to_iproc_gpio(gc);
+	unsigned int offset = IPROC_GPIO_REG(gpio,
+					      IPROC_GPIO_DATA_IN_OFFSET);
+	unsigned int shift = IPROC_GPIO_SHIFT(gpio);
 
 	return !!(readl(chip->base + offset) & BIT(shift));
 }
 
-static int cygnus_get_groups_count(struct pinctrl_dev *pctldev)
+static int iproc_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	return 1;
 }
@@ -372,20 +374,20 @@ static int cygnus_get_groups_count(struct pinctrl_dev *pctldev)
  * Only one group: "gpio_grp", since this local pinctrl device only performs
  * GPIO specific PINCONF configurations
  */
-static const char *cygnus_get_group_name(struct pinctrl_dev *pctldev,
+static const char *iproc_get_group_name(struct pinctrl_dev *pctldev,
 					 unsigned selector)
 {
 	return "gpio_grp";
 }
 
-static const struct pinctrl_ops cygnus_pctrl_ops = {
-	.get_groups_count = cygnus_get_groups_count,
-	.get_group_name = cygnus_get_group_name,
+static const struct pinctrl_ops iproc_pctrl_ops = {
+	.get_groups_count = iproc_get_groups_count,
+	.get_group_name = iproc_get_group_name,
 	.dt_node_to_map = pinconf_generic_dt_node_to_map_pin,
 	.dt_free_map = pinctrl_utils_dt_free_map,
 };
 
-static int cygnus_gpio_set_pull(struct cygnus_gpio *chip, unsigned gpio,
+static int iproc_gpio_set_pull(struct iproc_gpio *chip, unsigned gpio,
 				bool disable, bool pull_up)
 {
 	unsigned long flags;
@@ -393,11 +395,11 @@ static int cygnus_gpio_set_pull(struct cygnus_gpio *chip, unsigned gpio,
 	spin_lock_irqsave(&chip->lock, flags);
 
 	if (disable) {
-		cygnus_set_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio, false);
+		iproc_set_bit(chip, IPROC_GPIO_RES_EN_OFFSET, gpio, false);
 	} else {
-		cygnus_set_bit(chip, CYGNUS_GPIO_PAD_RES_OFFSET, gpio,
+		iproc_set_bit(chip, IPROC_GPIO_PAD_RES_OFFSET, gpio,
 			       pull_up);
-		cygnus_set_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio, true);
+		iproc_set_bit(chip, IPROC_GPIO_RES_EN_OFFSET, gpio, true);
 	}
 
 	spin_unlock_irqrestore(&chip->lock, flags);
@@ -407,18 +409,18 @@ static int cygnus_gpio_set_pull(struct cygnus_gpio *chip, unsigned gpio,
 	return 0;
 }
 
-static void cygnus_gpio_get_pull(struct cygnus_gpio *chip, unsigned gpio,
+static void iproc_gpio_get_pull(struct iproc_gpio *chip, unsigned gpio,
 				 bool *disable, bool *pull_up)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
-	*disable = !cygnus_get_bit(chip, CYGNUS_GPIO_RES_EN_OFFSET, gpio);
-	*pull_up = cygnus_get_bit(chip, CYGNUS_GPIO_PAD_RES_OFFSET, gpio);
+	*disable = !iproc_get_bit(chip, IPROC_GPIO_RES_EN_OFFSET, gpio);
+	*pull_up = iproc_get_bit(chip, IPROC_GPIO_PAD_RES_OFFSET, gpio);
 	spin_unlock_irqrestore(&chip->lock, flags);
 }
 
-static int cygnus_gpio_set_strength(struct cygnus_gpio *chip, unsigned gpio,
+static int iproc_gpio_set_strength(struct iproc_gpio *chip, unsigned gpio,
 				    unsigned strength)
 {
 	void __iomem *base;
@@ -432,14 +434,14 @@ static int cygnus_gpio_set_strength(struct cygnus_gpio *chip, unsigned gpio,
 
 	if (chip->io_ctrl) {
 		base = chip->io_ctrl;
-		offset = CYGNUS_GPIO_DRV0_CTRL_OFFSET;
+		offset = IPROC_GPIO_DRV0_CTRL_OFFSET;
 	} else {
 		base = chip->base;
-		offset = CYGNUS_GPIO_REG(gpio,
-					 CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET);
+		offset = IPROC_GPIO_REG(gpio,
+					 IPROC_GPIO_ASIU_DRV0_CTRL_OFFSET);
 	}
 
-	shift = CYGNUS_GPIO_SHIFT(gpio);
+	shift = IPROC_GPIO_SHIFT(gpio);
 
 	dev_dbg(chip->dev, "gpio:%u set drive strength:%d mA\n", gpio,
 		strength);
@@ -458,7 +460,7 @@ static int cygnus_gpio_set_strength(struct cygnus_gpio *chip, unsigned gpio,
 	return 0;
 }
 
-static int cygnus_gpio_get_strength(struct cygnus_gpio *chip, unsigned gpio,
+static int iproc_gpio_get_strength(struct iproc_gpio *chip, unsigned gpio,
 				    u16 *strength)
 {
 	void __iomem *base;
@@ -468,14 +470,14 @@ static int cygnus_gpio_get_strength(struct cygnus_gpio *chip, unsigned gpio,
 
 	if (chip->io_ctrl) {
 		base = chip->io_ctrl;
-		offset = CYGNUS_GPIO_DRV0_CTRL_OFFSET;
+		offset = IPROC_GPIO_DRV0_CTRL_OFFSET;
 	} else {
 		base = chip->base;
-		offset = CYGNUS_GPIO_REG(gpio,
-					 CYGNUS_GPIO_ASIU_DRV0_CTRL_OFFSET);
+		offset = IPROC_GPIO_REG(gpio,
+					 IPROC_GPIO_ASIU_DRV0_CTRL_OFFSET);
 	}
 
-	shift = CYGNUS_GPIO_SHIFT(gpio);
+	shift = IPROC_GPIO_SHIFT(gpio);
 
 	spin_lock_irqsave(&chip->lock, flags);
 	*strength = 0;
@@ -493,44 +495,43 @@ static int cygnus_gpio_get_strength(struct cygnus_gpio *chip, unsigned gpio,
 	return 0;
 }
 
-static int cygnus_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
+static int iproc_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
 				 unsigned long *config)
 {
-	struct cygnus_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	struct iproc_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
 	enum pin_config_param param = pinconf_to_config_param(*config);
-	unsigned gpio = cygnus_pin_to_gpio(pin);
+	unsigned gpio = iproc_pin_to_gpio(pin);
 	u16 arg;
 	bool disable, pull_up;
 	int ret;
 
 	switch (param) {
 	case PIN_CONFIG_BIAS_DISABLE:
-		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		iproc_gpio_get_pull(chip, gpio, &disable, &pull_up);
 		if (disable)
 			return 0;
 		else
 			return -EINVAL;
 
 	case PIN_CONFIG_BIAS_PULL_UP:
-		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		iproc_gpio_get_pull(chip, gpio, &disable, &pull_up);
 		if (!disable && pull_up)
 			return 0;
 		else
 			return -EINVAL;
 
 	case PIN_CONFIG_BIAS_PULL_DOWN:
-		cygnus_gpio_get_pull(chip, gpio, &disable, &pull_up);
+		iproc_gpio_get_pull(chip, gpio, &disable, &pull_up);
 		if (!disable && !pull_up)
 			return 0;
 		else
 			return -EINVAL;
 
 	case PIN_CONFIG_DRIVE_STRENGTH:
-		ret = cygnus_gpio_get_strength(chip, gpio, &arg);
+		ret = iproc_gpio_get_strength(chip, gpio, &arg);
 		if (ret)
 			return ret;
-		else
-			*config = pinconf_to_config_packed(param, arg);
+		*config = pinconf_to_config_packed(param, arg);
 
 		return 0;
 
@@ -541,13 +542,13 @@ static int cygnus_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
 	return -ENOTSUPP;
 }
 
-static int cygnus_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
+static int iproc_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 				 unsigned long *configs, unsigned num_configs)
 {
-	struct cygnus_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	struct iproc_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
 	enum pin_config_param param;
 	u16 arg;
-	unsigned i, gpio = cygnus_pin_to_gpio(pin);
+	unsigned i, gpio = iproc_pin_to_gpio(pin);
 	int ret = -ENOTSUPP;
 
 	for (i = 0; i < num_configs; i++) {
@@ -556,25 +557,25 @@ static int cygnus_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 
 		switch (param) {
 		case PIN_CONFIG_BIAS_DISABLE:
-			ret = cygnus_gpio_set_pull(chip, gpio, true, false);
+			ret = iproc_gpio_set_pull(chip, gpio, true, false);
 			if (ret < 0)
 				goto out;
 			break;
 
 		case PIN_CONFIG_BIAS_PULL_UP:
-			ret = cygnus_gpio_set_pull(chip, gpio, false, true);
+			ret = iproc_gpio_set_pull(chip, gpio, false, true);
 			if (ret < 0)
 				goto out;
 			break;
 
 		case PIN_CONFIG_BIAS_PULL_DOWN:
-			ret = cygnus_gpio_set_pull(chip, gpio, false, false);
+			ret = iproc_gpio_set_pull(chip, gpio, false, false);
 			if (ret < 0)
 				goto out;
 			break;
 
 		case PIN_CONFIG_DRIVE_STRENGTH:
-			ret = cygnus_gpio_set_strength(chip, gpio, arg);
+			ret = iproc_gpio_set_strength(chip, gpio, arg);
 			if (ret < 0)
 				goto out;
 			break;
@@ -589,20 +590,20 @@ out:
 	return ret;
 }
 
-static const struct pinconf_ops cygnus_pconf_ops = {
+static const struct pinconf_ops iproc_pconf_ops = {
 	.is_generic = true,
-	.pin_config_get = cygnus_pin_config_get,
-	.pin_config_set = cygnus_pin_config_set,
+	.pin_config_get = iproc_pin_config_get,
+	.pin_config_set = iproc_pin_config_set,
 };
 
 /*
- * Cygnus GPIO controller supports some PINCONF related configurations such as
+ * Iproc GPIO controller supports some PINCONF related configurations such as
  * pull up, pull down, and drive strength, when the pin is configured to GPIO
  *
  * Here a local pinctrl device is created with simple 1-to-1 pin mapping to the
  * local GPIO pins
  */
-static int cygnus_gpio_register_pinconf(struct cygnus_gpio *chip)
+static int iproc_gpio_register_pinconf(struct iproc_gpio *chip)
 {
 	struct pinctrl_desc *pctldesc = &chip->pctldesc;
 	struct pinctrl_pin_desc *pins;
@@ -622,10 +623,10 @@ static int cygnus_gpio_register_pinconf(struct cygnus_gpio *chip)
 	}
 
 	pctldesc->name = dev_name(chip->dev);
-	pctldesc->pctlops = &cygnus_pctrl_ops;
+	pctldesc->pctlops = &iproc_pctrl_ops;
 	pctldesc->pins = pins;
 	pctldesc->npins = gc->ngpio;
-	pctldesc->confops = &cygnus_pconf_ops;
+	pctldesc->confops = &iproc_pconf_ops;
 
 	chip->pctl = pinctrl_register(pctldesc, chip->dev, chip);
 	if (IS_ERR(chip->pctl)) {
@@ -636,59 +637,27 @@ static int cygnus_gpio_register_pinconf(struct cygnus_gpio *chip)
 	return 0;
 }
 
-static void cygnus_gpio_unregister_pinconf(struct cygnus_gpio *chip)
+static void iproc_gpio_unregister_pinconf(struct iproc_gpio *chip)
 {
-	if (chip->pctl)
-		pinctrl_unregister(chip->pctl);
+	pinctrl_unregister(chip->pctl);
 }
 
-struct cygnus_gpio_data {
-	unsigned num_gpios;
-};
-
-static const struct cygnus_gpio_data cygnus_cmm_gpio_data = {
-	.num_gpios = 24,
-};
-
-static const struct cygnus_gpio_data cygnus_asiu_gpio_data = {
-	.num_gpios = 146,
+static const struct of_device_id iproc_gpio_of_match[] = {
+	{ .compatible = "brcm,cygnus-ccm-gpio" },
+	{ .compatible = "brcm,cygnus-asiu-gpio" },
+	{ .compatible = "brcm,cygnus-crmu-gpio" },
+	{ .compatible = "brcm,iproc-gpio" },
+	{ }
 };
 
-static const struct cygnus_gpio_data cygnus_crmu_gpio_data = {
-	.num_gpios = 6,
-};
-
-static const struct of_device_id cygnus_gpio_of_match[] = {
-	{
-		.compatible = "brcm,cygnus-ccm-gpio",
-		.data = &cygnus_cmm_gpio_data,
-	},
-	{
-		.compatible = "brcm,cygnus-asiu-gpio",
-		.data = &cygnus_asiu_gpio_data,
-	},
-	{
-		.compatible = "brcm,cygnus-crmu-gpio",
-		.data = &cygnus_crmu_gpio_data,
-	}
-};
-
-static int cygnus_gpio_probe(struct platform_device *pdev)
+static int iproc_gpio_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct resource *res;
-	struct cygnus_gpio *chip;
+	struct iproc_gpio *chip;
 	struct gpio_chip *gc;
 	u32 ngpios;
 	int irq, ret;
-	const struct of_device_id *match;
-	const struct cygnus_gpio_data *gpio_data;
-
-	match = of_match_device(cygnus_gpio_of_match, dev);
-	if (!match)
-		return -ENODEV;
-	gpio_data = match->data;
-	ngpios = gpio_data->num_gpios;
 
 	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
 	if (!chip)
@@ -713,6 +682,11 @@ static int cygnus_gpio_probe(struct platform_device *pdev)
 		}
 	}
 
+	if (of_property_read_u32(dev->of_node, "ngpios", &ngpios)) {
+		dev_err(&pdev->dev, "missing ngpios DT property\n");
+		return -ENODEV;
+	}
+
 	spin_lock_init(&chip->lock);
 
 	gc = &chip->gc;
@@ -720,14 +694,14 @@ static int cygnus_gpio_probe(struct platform_device *pdev)
 	gc->ngpio = ngpios;
 	chip->num_banks = (ngpios + NGPIOS_PER_BANK - 1) / NGPIOS_PER_BANK;
 	gc->label = dev_name(dev);
-	gc->dev = dev;
+	gc->parent = dev;
 	gc->of_node = dev->of_node;
-	gc->request = cygnus_gpio_request;
-	gc->free = cygnus_gpio_free;
-	gc->direction_input = cygnus_gpio_direction_input;
-	gc->direction_output = cygnus_gpio_direction_output;
-	gc->set = cygnus_gpio_set;
-	gc->get = cygnus_gpio_get;
+	gc->request = iproc_gpio_request;
+	gc->free = iproc_gpio_free;
+	gc->direction_input = iproc_gpio_direction_input;
+	gc->direction_output = iproc_gpio_direction_output;
+	gc->set = iproc_gpio_set;
+	gc->get = iproc_gpio_get;
 
 	chip->pinmux_is_supported = of_property_read_bool(dev->of_node,
 							"gpio-ranges");
@@ -738,7 +712,7 @@ static int cygnus_gpio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	ret = cygnus_gpio_register_pinconf(chip);
+	ret = iproc_gpio_register_pinconf(chip);
 	if (ret) {
 		dev_err(dev, "unable to register pinconf\n");
 		goto err_rm_gpiochip;
@@ -747,21 +721,21 @@ static int cygnus_gpio_probe(struct platform_device *pdev)
 	/* optional GPIO interrupt support */
 	irq = platform_get_irq(pdev, 0);
 	if (irq) {
-		ret = gpiochip_irqchip_add(gc, &cygnus_gpio_irq_chip, 0,
+		ret = gpiochip_irqchip_add(gc, &iproc_gpio_irq_chip, 0,
 					   handle_simple_irq, IRQ_TYPE_NONE);
 		if (ret) {
 			dev_err(dev, "no GPIO irqchip\n");
 			goto err_unregister_pinconf;
 		}
 
-		gpiochip_set_chained_irqchip(gc, &cygnus_gpio_irq_chip, irq,
-					     cygnus_gpio_irq_handler);
+		gpiochip_set_chained_irqchip(gc, &iproc_gpio_irq_chip, irq,
+					     iproc_gpio_irq_handler);
 	}
 
 	return 0;
 
 err_unregister_pinconf:
-	cygnus_gpio_unregister_pinconf(chip);
+	iproc_gpio_unregister_pinconf(chip);
 
 err_rm_gpiochip:
 	gpiochip_remove(gc);
@@ -769,16 +743,16 @@ err_rm_gpiochip:
 	return ret;
 }
 
-static struct platform_driver cygnus_gpio_driver = {
+static struct platform_driver iproc_gpio_driver = {
 	.driver = {
-		.name = "cygnus-gpio",
-		.of_match_table = cygnus_gpio_of_match,
+		.name = "iproc-gpio",
+		.of_match_table = iproc_gpio_of_match,
 	},
-	.probe = cygnus_gpio_probe,
+	.probe = iproc_gpio_probe,
 };
 
-static int __init cygnus_gpio_init(void)
+static int __init iproc_gpio_init(void)
 {
-	return platform_driver_probe(&cygnus_gpio_driver, cygnus_gpio_probe);
+	return platform_driver_probe(&iproc_gpio_driver, iproc_gpio_probe);
 }
-arch_initcall_sync(cygnus_gpio_init);
+arch_initcall_sync(iproc_gpio_init);
diff --git a/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c b/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c
new file mode 100644
index 000000000000..ac900435dc39
--- /dev/null
+++ b/drivers/pinctrl/bcm/pinctrl-nsp-gpio.c
@@ -0,0 +1,744 @@
+/*
+ * Copyright (C) 2015 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * This file contains the Broadcom Northstar Plus (NSP) GPIO driver that
+ * supports the chipCommonA GPIO controller. Basic PINCONF such as bias,
+ * pull up/down, slew and drive strength are also supported in this driver.
+ *
+ * Pins from the chipCommonA  GPIO can be individually muxed to GPIO function,
+ * through the interaction with the NSP IOMUX controller.
+ */
+
+#include <linux/gpio/driver.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_irq.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/slab.h>
+
+#include "../pinctrl-utils.h"
+
+#define NSP_CHIP_A_INT_STATUS		0x00
+#define NSP_CHIP_A_INT_MASK		0x04
+#define NSP_GPIO_DATA_IN		0x40
+#define NSP_GPIO_DATA_OUT		0x44
+#define NSP_GPIO_OUT_EN			0x48
+#define NSP_GPIO_INT_POLARITY		0x50
+#define NSP_GPIO_INT_MASK		0x54
+#define NSP_GPIO_EVENT			0x58
+#define NSP_GPIO_EVENT_INT_MASK		0x5c
+#define NSP_GPIO_EVENT_INT_POLARITY	0x64
+#define NSP_CHIP_A_GPIO_INT_BIT		0x01
+
+/* I/O parameters offset for chipcommon A GPIO */
+#define NSP_GPIO_DRV_CTRL		0x00
+#define NSP_GPIO_HYSTERESIS_EN		0x10
+#define NSP_GPIO_SLEW_RATE_EN		0x14
+#define NSP_PULL_UP_EN			0x18
+#define NSP_PULL_DOWN_EN		0x1c
+#define GPIO_DRV_STRENGTH_BITS		0x03
+
+/*
+ * nsp GPIO core
+ *
+ * @dev: pointer to device
+ * @base: I/O register base for nsp GPIO controller
+ * @io_ctrl: I/O register base for PINCONF support outside the GPIO block
+ * @gc: GPIO chip
+ * @pctl: pointer to pinctrl_dev
+ * @pctldesc: pinctrl descriptor
+ * @irq_domain: pointer to irq domain
+ * @lock: lock to protect access to I/O registers
+ */
+struct nsp_gpio {
+	struct device *dev;
+	void __iomem *base;
+	void __iomem *io_ctrl;
+	struct gpio_chip gc;
+	struct pinctrl_dev *pctl;
+	struct pinctrl_desc pctldesc;
+	struct irq_domain *irq_domain;
+	spinlock_t lock;
+};
+
+enum base_type {
+	REG,
+	IO_CTRL
+};
+
+/*
+ * Mapping from PINCONF pins to GPIO pins is 1-to-1
+ */
+static inline unsigned nsp_pin_to_gpio(unsigned pin)
+{
+	return pin;
+}
+
+/*
+ *  nsp_set_bit - set or clear one bit (corresponding to the GPIO pin) in a
+ *  nsp GPIO register
+ *
+ *  @nsp_gpio: nsp GPIO device
+ *  @base_type: reg base to modify
+ *  @reg: register offset
+ *  @gpio: GPIO pin
+ *  @set: set or clear
+ */
+static inline void nsp_set_bit(struct nsp_gpio *chip, enum base_type address,
+			       unsigned int reg, unsigned gpio, bool set)
+{
+	u32 val;
+	void __iomem *base_address;
+
+	if (address == IO_CTRL)
+		base_address = chip->io_ctrl;
+	else
+		base_address = chip->base;
+
+	val = readl(base_address + reg);
+	if (set)
+		val |= BIT(gpio);
+	else
+		val &= ~BIT(gpio);
+
+	writel(val, base_address + reg);
+}
+
+/*
+ *  nsp_get_bit - get one bit (corresponding to the GPIO pin) in a
+ *  nsp GPIO register
+ */
+static inline bool nsp_get_bit(struct nsp_gpio *chip, enum base_type address,
+			       unsigned int reg, unsigned gpio)
+{
+	if (address == IO_CTRL)
+		return !!(readl(chip->io_ctrl + reg) & BIT(gpio));
+	else
+		return !!(readl(chip->base + reg) & BIT(gpio));
+}
+
+static irqreturn_t nsp_gpio_irq_handler(int irq, void *data)
+{
+	struct nsp_gpio *chip = (struct nsp_gpio *)data;
+	struct gpio_chip gc = chip->gc;
+	int bit;
+	unsigned long int_bits = 0;
+	u32 int_status;
+
+	/* go through the entire GPIOs and handle all interrupts */
+	int_status = readl(chip->base + NSP_CHIP_A_INT_STATUS);
+	if (int_status & NSP_CHIP_A_GPIO_INT_BIT) {
+		unsigned int event, level;
+
+		/* Get level and edge interrupts */
+		event = readl(chip->base + NSP_GPIO_EVENT_INT_MASK) &
+			      readl(chip->base + NSP_GPIO_EVENT);
+		level = readl(chip->base + NSP_GPIO_DATA_IN) ^
+			      readl(chip->base + NSP_GPIO_INT_POLARITY);
+		level &= readl(chip->base + NSP_GPIO_INT_MASK);
+		int_bits = level | event;
+
+		for_each_set_bit(bit, &int_bits, gc.ngpio) {
+			/*
+			 * Clear the interrupt before invoking the
+			 * handler, so we do not leave any window
+			 */
+			writel(BIT(bit), chip->base + NSP_GPIO_EVENT);
+			generic_handle_irq(
+				irq_linear_revmap(chip->irq_domain, bit));
+		}
+	}
+
+	return  int_bits ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static void nsp_gpio_irq_ack(struct irq_data *d)
+{
+	struct nsp_gpio *chip = irq_data_get_irq_chip_data(d);
+	unsigned gpio = d->hwirq;
+	u32 val = BIT(gpio);
+	u32 trigger_type;
+
+	trigger_type = irq_get_trigger_type(d->irq);
+	if (trigger_type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
+		nsp_set_bit(chip, REG, NSP_GPIO_EVENT, gpio, val);
+}
+
+/*
+ *  nsp_gpio_irq_set_mask - mask/unmask a GPIO interrupt
+ *
+ *  @d: IRQ chip data
+ *  @unmask: mask/unmask GPIO interrupt
+ */
+static void nsp_gpio_irq_set_mask(struct irq_data *d, bool unmask)
+{
+	struct nsp_gpio *chip = irq_data_get_irq_chip_data(d);
+	unsigned gpio = d->hwirq;
+	u32 trigger_type;
+
+	trigger_type = irq_get_trigger_type(d->irq);
+	if (trigger_type & (IRQ_TYPE_EDGE_FALLING | IRQ_TYPE_EDGE_RISING))
+		nsp_set_bit(chip, REG, NSP_GPIO_EVENT_INT_MASK, gpio, unmask);
+	else
+		nsp_set_bit(chip, REG, NSP_GPIO_INT_MASK, gpio, unmask);
+}
+
+static void nsp_gpio_irq_mask(struct irq_data *d)
+{
+	struct nsp_gpio *chip = irq_data_get_irq_chip_data(d);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_gpio_irq_set_mask(d, false);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static void nsp_gpio_irq_unmask(struct irq_data *d)
+{
+	struct nsp_gpio *chip = irq_data_get_irq_chip_data(d);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_gpio_irq_set_mask(d, true);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static int nsp_gpio_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	struct nsp_gpio *chip = irq_data_get_irq_chip_data(d);
+	unsigned gpio = d->hwirq;
+	bool level_low;
+	bool falling;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	falling = nsp_get_bit(chip, REG, NSP_GPIO_EVENT_INT_POLARITY, gpio);
+	level_low = nsp_get_bit(chip, REG, NSP_GPIO_INT_POLARITY, gpio);
+
+	switch (type & IRQ_TYPE_SENSE_MASK) {
+	case IRQ_TYPE_EDGE_RISING:
+		falling = false;
+		break;
+
+	case IRQ_TYPE_EDGE_FALLING:
+		falling = true;
+		break;
+
+	case IRQ_TYPE_LEVEL_HIGH:
+		level_low = false;
+		break;
+
+	case IRQ_TYPE_LEVEL_LOW:
+		level_low = true;
+		break;
+
+	default:
+		dev_err(chip->dev, "invalid GPIO IRQ type 0x%x\n",
+			type);
+		spin_unlock_irqrestore(&chip->lock, flags);
+		return -EINVAL;
+	}
+
+	nsp_set_bit(chip, REG, NSP_GPIO_EVENT_INT_POLARITY, gpio, falling);
+	nsp_set_bit(chip, REG, NSP_GPIO_INT_POLARITY, gpio, level_low);
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u level_low:%s falling:%s\n", gpio,
+		level_low ? "true" : "false", falling ? "true" : "false");
+	return 0;
+}
+
+static struct irq_chip nsp_gpio_irq_chip = {
+	.name = "gpio-a",
+	.irq_enable = nsp_gpio_irq_unmask,
+	.irq_disable = nsp_gpio_irq_mask,
+	.irq_ack = nsp_gpio_irq_ack,
+	.irq_mask = nsp_gpio_irq_mask,
+	.irq_unmask = nsp_gpio_irq_unmask,
+	.irq_set_type = nsp_gpio_irq_set_type,
+};
+
+/*
+ * Request the nsp IOMUX pinmux controller to mux individual pins to GPIO
+ */
+static int nsp_gpio_request(struct gpio_chip *gc, unsigned offset)
+{
+	unsigned gpio = gc->base + offset;
+
+	return pinctrl_request_gpio(gpio);
+}
+
+static void nsp_gpio_free(struct gpio_chip *gc, unsigned offset)
+{
+	unsigned gpio = gc->base + offset;
+
+	pinctrl_free_gpio(gpio);
+}
+
+static int nsp_gpio_direction_input(struct gpio_chip *gc, unsigned gpio)
+{
+	struct nsp_gpio *chip = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_set_bit(chip, REG, NSP_GPIO_OUT_EN, gpio, false);
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set input\n", gpio);
+	return 0;
+}
+
+static int nsp_gpio_direction_output(struct gpio_chip *gc, unsigned gpio,
+				     int val)
+{
+	struct nsp_gpio *chip = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_set_bit(chip, REG, NSP_GPIO_OUT_EN, gpio, true);
+	nsp_set_bit(chip, REG, NSP_GPIO_DATA_OUT, gpio, !!(val));
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set output, value:%d\n", gpio, val);
+	return 0;
+}
+
+static void nsp_gpio_set(struct gpio_chip *gc, unsigned gpio, int val)
+{
+	struct nsp_gpio *chip = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_set_bit(chip, REG, NSP_GPIO_DATA_OUT, gpio, !!(val));
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set, value:%d\n", gpio, val);
+}
+
+static int nsp_gpio_get(struct gpio_chip *gc, unsigned gpio)
+{
+	struct nsp_gpio *chip = gpiochip_get_data(gc);
+
+	return !!(readl(chip->base + NSP_GPIO_DATA_IN) & BIT(gpio));
+}
+
+static int nsp_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
+{
+	struct nsp_gpio *chip = gpiochip_get_data(gc);
+
+	return irq_linear_revmap(chip->irq_domain, offset);
+}
+
+static int nsp_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	return 1;
+}
+
+/*
+ * Only one group: "gpio_grp", since this local pinctrl device only performs
+ * GPIO specific PINCONF configurations
+ */
+static const char *nsp_get_group_name(struct pinctrl_dev *pctldev,
+				      unsigned selector)
+{
+	return "gpio_grp";
+}
+
+static const struct pinctrl_ops nsp_pctrl_ops = {
+	.get_groups_count = nsp_get_groups_count,
+	.get_group_name = nsp_get_group_name,
+	.dt_node_to_map = pinconf_generic_dt_node_to_map_pin,
+	.dt_free_map = pinctrl_utils_dt_free_map,
+};
+
+static int nsp_gpio_set_slew(struct nsp_gpio *chip, unsigned gpio, u16 slew)
+{
+	if (slew)
+		nsp_set_bit(chip, IO_CTRL, NSP_GPIO_SLEW_RATE_EN, gpio, true);
+	else
+		nsp_set_bit(chip, IO_CTRL, NSP_GPIO_SLEW_RATE_EN, gpio, false);
+
+	return 0;
+}
+
+static int nsp_gpio_set_pull(struct nsp_gpio *chip, unsigned gpio,
+			     bool pull_up, bool pull_down)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	nsp_set_bit(chip, IO_CTRL, NSP_PULL_DOWN_EN, gpio, pull_down);
+	nsp_set_bit(chip, IO_CTRL, NSP_PULL_UP_EN, gpio, pull_up);
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	dev_dbg(chip->dev, "gpio:%u set pullup:%d pulldown: %d\n",
+		gpio, pull_up, pull_down);
+	return 0;
+}
+
+static void nsp_gpio_get_pull(struct nsp_gpio *chip, unsigned gpio,
+			      bool *pull_up, bool *pull_down)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	*pull_up = nsp_get_bit(chip, IO_CTRL, NSP_PULL_UP_EN, gpio);
+	*pull_down = nsp_get_bit(chip, IO_CTRL, NSP_PULL_DOWN_EN, gpio);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static int nsp_gpio_set_strength(struct nsp_gpio *chip, unsigned gpio,
+				 u16 strength)
+{
+	u32 offset, shift, i;
+	u32 val;
+	unsigned long flags;
+
+	/* make sure drive strength is supported */
+	if (strength < 2 || strength > 16 || (strength % 2))
+		return -ENOTSUPP;
+
+	shift = gpio;
+	offset = NSP_GPIO_DRV_CTRL;
+	dev_dbg(chip->dev, "gpio:%u set drive strength:%d mA\n", gpio,
+		strength);
+	spin_lock_irqsave(&chip->lock, flags);
+	strength = (strength / 2) - 1;
+	for (i = GPIO_DRV_STRENGTH_BITS; i > 0; i--) {
+		val = readl(chip->io_ctrl + offset);
+		val &= ~BIT(shift);
+		val |= ((strength >> (i-1)) & 0x1) << shift;
+		writel(val, chip->io_ctrl + offset);
+		offset += 4;
+	}
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+static int nsp_gpio_get_strength(struct nsp_gpio *chip, unsigned gpio,
+				 u16 *strength)
+{
+	unsigned int offset, shift;
+	u32 val;
+	unsigned long flags;
+	int i;
+
+	offset = NSP_GPIO_DRV_CTRL;
+	shift = gpio;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	*strength = 0;
+	for (i = (GPIO_DRV_STRENGTH_BITS - 1); i >= 0; i--) {
+		val = readl(chip->io_ctrl + offset) & BIT(shift);
+		val >>= shift;
+		*strength += (val << i);
+		offset += 4;
+	}
+
+	/* convert to mA */
+	*strength = (*strength + 1) * 2;
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+int nsp_pin_config_group_get(struct pinctrl_dev *pctldev, unsigned selector,
+			     unsigned long *config)
+{
+	return 0;
+}
+
+int nsp_pin_config_group_set(struct pinctrl_dev *pctldev, unsigned selector,
+			     unsigned long *configs, unsigned num_configs)
+{
+	return 0;
+}
+
+static int nsp_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
+			      unsigned long *config)
+{
+	struct nsp_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param = pinconf_to_config_param(*config);
+	unsigned int gpio;
+	u16 arg = 0;
+	bool pull_up, pull_down;
+	int ret;
+
+	gpio = nsp_pin_to_gpio(pin);
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		nsp_gpio_get_pull(chip, gpio, &pull_up, &pull_down);
+		if ((pull_up == false) && (pull_down == false))
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_BIAS_PULL_UP:
+		nsp_gpio_get_pull(chip, gpio, &pull_up, &pull_down);
+		if (pull_up)
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		nsp_gpio_get_pull(chip, gpio, &pull_up, &pull_down);
+		if (pull_down)
+			return 0;
+		else
+			return -EINVAL;
+
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		ret = nsp_gpio_get_strength(chip, gpio, &arg);
+		if (ret)
+			return ret;
+		*config = pinconf_to_config_packed(param, arg);
+		return 0;
+
+	default:
+		return -ENOTSUPP;
+	}
+}
+
+static int nsp_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
+			      unsigned long *configs, unsigned num_configs)
+{
+	struct nsp_gpio *chip = pinctrl_dev_get_drvdata(pctldev);
+	enum pin_config_param param;
+	u16 arg;
+	unsigned int i, gpio;
+	int ret = -ENOTSUPP;
+
+	gpio = nsp_pin_to_gpio(pin);
+	for (i = 0; i < num_configs; i++) {
+		param = pinconf_to_config_param(configs[i]);
+		arg = pinconf_to_config_argument(configs[i]);
+
+		switch (param) {
+		case PIN_CONFIG_BIAS_DISABLE:
+			ret = nsp_gpio_set_pull(chip, gpio, false, false);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_UP:
+			ret = nsp_gpio_set_pull(chip, gpio, true, false);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_BIAS_PULL_DOWN:
+			ret = nsp_gpio_set_pull(chip, gpio, false, true);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_DRIVE_STRENGTH:
+			ret = nsp_gpio_set_strength(chip, gpio, arg);
+			if (ret < 0)
+				goto out;
+			break;
+
+		case PIN_CONFIG_SLEW_RATE:
+			ret = nsp_gpio_set_slew(chip, gpio, arg);
+			if (ret < 0)
+				goto out;
+			break;
+
+		default:
+			dev_err(chip->dev, "invalid configuration\n");
+			return -ENOTSUPP;
+		}
+	}
+
+out:
+	return ret;
+}
+
+static const struct pinconf_ops nsp_pconf_ops = {
+	.is_generic = true,
+	.pin_config_get = nsp_pin_config_get,
+	.pin_config_set = nsp_pin_config_set,
+	.pin_config_group_get = nsp_pin_config_group_get,
+	.pin_config_group_set = nsp_pin_config_group_set,
+};
+
+/*
+ * NSP GPIO controller supports some PINCONF related configurations such as
+ * pull up, pull down, slew and drive strength, when the pin is configured
+ * to GPIO.
+ *
+ * Here a local pinctrl device is created with simple 1-to-1 pin mapping to the
+ * local GPIO pins
+ */
+static int nsp_gpio_register_pinconf(struct nsp_gpio *chip)
+{
+	struct pinctrl_desc *pctldesc = &chip->pctldesc;
+	struct pinctrl_pin_desc *pins;
+	struct gpio_chip *gc = &chip->gc;
+	int i;
+
+	pins = devm_kcalloc(chip->dev, gc->ngpio, sizeof(*pins), GFP_KERNEL);
+	if (!pins)
+		return -ENOMEM;
+	for (i = 0; i < gc->ngpio; i++) {
+		pins[i].number = i;
+		pins[i].name = devm_kasprintf(chip->dev, GFP_KERNEL,
+					      "gpio-%d", i);
+		if (!pins[i].name)
+			return -ENOMEM;
+	}
+	pctldesc->name = dev_name(chip->dev);
+	pctldesc->pctlops = &nsp_pctrl_ops;
+	pctldesc->pins = pins;
+	pctldesc->npins = gc->ngpio;
+	pctldesc->confops = &nsp_pconf_ops;
+
+	chip->pctl = pinctrl_register(pctldesc, chip->dev, chip);
+	if (IS_ERR(chip->pctl)) {
+		dev_err(chip->dev, "unable to register pinctrl device\n");
+		return PTR_ERR(chip->pctl);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id nsp_gpio_of_match[] = {
+	{.compatible = "brcm,nsp-gpio-a",},
+	{}
+};
+
+static int nsp_gpio_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct nsp_gpio *chip;
+	struct gpio_chip *gc;
+	u32 val, count;
+	int irq, ret;
+
+	if (of_property_read_u32(pdev->dev.of_node, "ngpios", &val)) {
+		dev_err(&pdev->dev, "Missing ngpios OF property\n");
+		return -ENODEV;
+	}
+
+	chip = devm_kzalloc(dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = dev;
+	platform_set_drvdata(pdev, chip);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	chip->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(chip->base)) {
+		dev_err(dev, "unable to map I/O memory\n");
+		return PTR_ERR(chip->base);
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	chip->io_ctrl = devm_ioremap_resource(dev, res);
+	if (IS_ERR(chip->io_ctrl)) {
+		dev_err(dev, "unable to map I/O memory\n");
+		return PTR_ERR(chip->io_ctrl);
+	}
+
+	spin_lock_init(&chip->lock);
+	gc = &chip->gc;
+	gc->base = -1;
+	gc->can_sleep = false;
+	gc->ngpio = val;
+	gc->label = dev_name(dev);
+	gc->parent = dev;
+	gc->of_node = dev->of_node;
+	gc->request = nsp_gpio_request;
+	gc->free = nsp_gpio_free;
+	gc->direction_input = nsp_gpio_direction_input;
+	gc->direction_output = nsp_gpio_direction_output;
+	gc->set = nsp_gpio_set;
+	gc->get = nsp_gpio_get;
+	gc->to_irq = nsp_gpio_to_irq;
+
+	/* optional GPIO interrupt support */
+	irq = platform_get_irq(pdev, 0);
+	if (irq > 0) {
+		/* Create irq domain so that each pin can be assigned an IRQ.*/
+		chip->irq_domain = irq_domain_add_linear(gc->of_node, gc->ngpio,
+							 &irq_domain_simple_ops,
+							 chip);
+		if (!chip->irq_domain) {
+			dev_err(&pdev->dev, "Couldn't allocate IRQ domain\n");
+			return -ENXIO;
+		}
+
+		/* Map each gpio to an IRQ and set the handler for gpiolib. */
+		for (count = 0; count < gc->ngpio; count++) {
+			int irq = irq_create_mapping(chip->irq_domain, count);
+
+			irq_set_chip_and_handler(irq, &nsp_gpio_irq_chip,
+						 handle_simple_irq);
+			irq_set_chip_data(irq, chip);
+		}
+
+		/* Install ISR for this GPIO controller. */
+		ret = devm_request_irq(&pdev->dev, irq, nsp_gpio_irq_handler,
+				       IRQF_SHARED, "gpio-a", chip);
+		if (ret) {
+			dev_err(&pdev->dev, "Unable to request IRQ%d: %d\n",
+				irq, ret);
+			goto err_rm_gpiochip;
+		}
+
+		val = readl(chip->base + NSP_CHIP_A_INT_MASK);
+		val = val | NSP_CHIP_A_GPIO_INT_BIT;
+		writel(val, (chip->base + NSP_CHIP_A_INT_MASK));
+	}
+
+	ret = gpiochip_add_data(gc, chip);
+	if (ret < 0) {
+		dev_err(dev, "unable to add GPIO chip\n");
+		return ret;
+	}
+
+	ret = nsp_gpio_register_pinconf(chip);
+	if (ret) {
+		dev_err(dev, "unable to register pinconf\n");
+		goto err_rm_gpiochip;
+	}
+
+	return 0;
+
+err_rm_gpiochip:
+	gpiochip_remove(gc);
+
+	return ret;
+}
+
+static struct platform_driver nsp_gpio_driver = {
+	.driver = {
+		.name = "nsp-gpio-a",
+		.of_match_table = nsp_gpio_of_match,
+	},
+	.probe = nsp_gpio_probe,
+};
+
+static int __init nsp_gpio_init(void)
+{
+	return platform_driver_probe(&nsp_gpio_driver, nsp_gpio_probe);
+}
+arch_initcall_sync(nsp_gpio_init);
diff --git a/drivers/pinctrl/berlin/Makefile b/drivers/pinctrl/berlin/Makefile
index 06f94029ad66..6f641ce2c830 100644
--- a/drivers/pinctrl/berlin/Makefile
+++ b/drivers/pinctrl/berlin/Makefile
@@ -1,4 +1,4 @@
-obj-$(CONFIG_PINCTRL_BERLIN)		+= berlin.o
+obj-y					+= berlin.o
 obj-$(CONFIG_PINCTRL_BERLIN_BG2)	+= berlin-bg2.o
 obj-$(CONFIG_PINCTRL_BERLIN_BG2CD)	+= berlin-bg2cd.o
 obj-$(CONFIG_PINCTRL_BERLIN_BG2Q)	+= berlin-bg2q.o
diff --git a/drivers/pinctrl/intel/pinctrl-baytrail.c b/drivers/pinctrl/intel/pinctrl-baytrail.c
index b59ce75b1947..21b79a446d5a 100644
--- a/drivers/pinctrl/intel/pinctrl-baytrail.c
+++ b/drivers/pinctrl/intel/pinctrl-baytrail.c
@@ -20,7 +20,7 @@
 #include <linux/types.h>
 #include <linux/bitops.h>
 #include <linux/interrupt.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/acpi.h>
 #include <linux/platform_device.h>
 #include <linux/seq_file.h>
@@ -147,12 +147,10 @@ struct byt_gpio {
 	struct byt_gpio_pin_context	*saved_context;
 };
 
-#define to_byt_gpio(c)	container_of(c, struct byt_gpio, chip)
-
 static void __iomem *byt_gpio_reg(struct gpio_chip *chip, unsigned offset,
 				 int reg)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	u32 reg_offset;
 
 	if (reg == BYT_INT_STAT_REG)
@@ -193,7 +191,7 @@ static u32 byt_get_gpio_mux(struct byt_gpio *vg, unsigned offset)
 
 static int byt_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	void __iomem *reg = byt_gpio_reg(chip, offset, BYT_CONF0_REG);
 	u32 value, gpio_mux;
 	unsigned long flags;
@@ -229,7 +227,7 @@ static int byt_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void byt_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 
 	byt_gpio_clear_triggering(vg, offset);
 	pm_runtime_put(&vg->pdev->dev);
@@ -237,7 +235,7 @@ static void byt_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static int byt_irq_type(struct irq_data *d, unsigned type)
 {
-	struct byt_gpio *vg = to_byt_gpio(irq_data_get_irq_chip_data(d));
+	struct byt_gpio *vg = gpiochip_get_data(irq_data_get_irq_chip_data(d));
 	u32 offset = irqd_to_hwirq(d);
 	u32 value;
 	unsigned long flags;
@@ -273,7 +271,7 @@ static int byt_irq_type(struct irq_data *d, unsigned type)
 static int byt_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	void __iomem *reg = byt_gpio_reg(chip, offset, BYT_VAL_REG);
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -281,12 +279,12 @@ static int byt_gpio_get(struct gpio_chip *chip, unsigned offset)
 	val = readl(reg);
 	raw_spin_unlock_irqrestore(&vg->lock, flags);
 
-	return val & BYT_LEVEL;
+	return !!(val & BYT_LEVEL);
 }
 
 static void byt_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	void __iomem *reg = byt_gpio_reg(chip, offset, BYT_VAL_REG);
 	unsigned long flags;
 	u32 old_val;
@@ -305,7 +303,7 @@ static void byt_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int byt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	void __iomem *reg = byt_gpio_reg(chip, offset, BYT_VAL_REG);
 	unsigned long flags;
 	u32 value;
@@ -324,7 +322,7 @@ static int byt_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int byt_gpio_direction_output(struct gpio_chip *chip,
 				     unsigned gpio, int value)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	void __iomem *conf_reg = byt_gpio_reg(chip, gpio, BYT_CONF0_REG);
 	void __iomem *reg = byt_gpio_reg(chip, gpio, BYT_VAL_REG);
 	unsigned long flags;
@@ -356,7 +354,7 @@ static int byt_gpio_direction_output(struct gpio_chip *chip,
 
 static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct byt_gpio *vg = to_byt_gpio(chip);
+	struct byt_gpio *vg = gpiochip_get_data(chip);
 	int i;
 	u32 conf0, val, offs;
 
@@ -428,7 +426,7 @@ static void byt_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 static void byt_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct irq_data *data = irq_desc_get_irq_data(desc);
-	struct byt_gpio *vg = to_byt_gpio(irq_desc_get_handler_data(desc));
+	struct byt_gpio *vg = gpiochip_get_data(irq_desc_get_handler_data(desc));
 	struct irq_chip *chip = irq_data_get_irq_chip(data);
 	u32 base, pin;
 	void __iomem *reg;
@@ -450,7 +448,7 @@ static void byt_gpio_irq_handler(struct irq_desc *desc)
 static void byt_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct byt_gpio *vg = to_byt_gpio(gc);
+	struct byt_gpio *vg = gpiochip_get_data(gc);
 	unsigned offset = irqd_to_hwirq(d);
 	void __iomem *reg;
 
@@ -463,7 +461,7 @@ static void byt_irq_ack(struct irq_data *d)
 static void byt_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct byt_gpio *vg = to_byt_gpio(gc);
+	struct byt_gpio *vg = gpiochip_get_data(gc);
 	unsigned offset = irqd_to_hwirq(d);
 	unsigned long flags;
 	void __iomem *reg;
@@ -498,7 +496,7 @@ static void byt_irq_unmask(struct irq_data *d)
 static void byt_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct byt_gpio *vg = to_byt_gpio(gc);
+	struct byt_gpio *vg = gpiochip_get_data(gc);
 
 	byt_gpio_clear_triggering(vg, irqd_to_hwirq(d));
 }
@@ -598,14 +596,14 @@ static int byt_gpio_probe(struct platform_device *pdev)
 	gc->dbg_show = byt_gpio_dbg_show;
 	gc->base = -1;
 	gc->can_sleep = false;
-	gc->dev = dev;
+	gc->parent = dev;
 
 #ifdef CONFIG_PM_SLEEP
 	vg->saved_context = devm_kcalloc(&pdev->dev, gc->ngpio,
 				       sizeof(*vg->saved_context), GFP_KERNEL);
 #endif
 
-	ret = gpiochip_add(gc);
+	ret = gpiochip_add_data(gc, vg);
 	if (ret) {
 		dev_err(&pdev->dev, "failed adding byt-gpio chip\n");
 		return ret;
diff --git a/drivers/pinctrl/intel/pinctrl-cherryview.c b/drivers/pinctrl/intel/pinctrl-cherryview.c
index 84936bae6e5e..4251e0747a3a 100644
--- a/drivers/pinctrl/intel/pinctrl-cherryview.c
+++ b/drivers/pinctrl/intel/pinctrl-cherryview.c
@@ -181,8 +181,6 @@ struct chv_pinctrl {
 	struct chv_pin_context *saved_pin_context;
 };
 
-#define gpiochip_to_pinctrl(c) container_of(c, struct chv_pinctrl, chip)
-
 #define ALTERNATE_FUNCTION(p, m, i)		\
 	{					\
 		.pin = (p),			\
@@ -1157,7 +1155,7 @@ static unsigned chv_gpio_offset_to_pin(struct chv_pinctrl *pctrl,
 
 static int chv_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(chip);
 	int pin = chv_gpio_offset_to_pin(pctrl, offset);
 	unsigned long flags;
 	u32 ctrl0, cfg;
@@ -1176,7 +1174,7 @@ static int chv_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void chv_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned pin = chv_gpio_offset_to_pin(pctrl, offset);
 	unsigned long flags;
 	void __iomem *reg;
@@ -1199,7 +1197,7 @@ static void chv_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int chv_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned pin = chv_gpio_offset_to_pin(pctrl, offset);
 	u32 ctrl0, direction;
 	unsigned long flags;
@@ -1240,7 +1238,7 @@ static const struct gpio_chip chv_gpio_chip = {
 static void chv_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(gc);
 	int pin = chv_gpio_offset_to_pin(pctrl, irqd_to_hwirq(d));
 	u32 intr_line;
 
@@ -1257,7 +1255,7 @@ static void chv_gpio_irq_ack(struct irq_data *d)
 static void chv_gpio_irq_mask_unmask(struct irq_data *d, bool mask)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(gc);
 	int pin = chv_gpio_offset_to_pin(pctrl, irqd_to_hwirq(d));
 	u32 value, intr_line;
 	unsigned long flags;
@@ -1302,7 +1300,7 @@ static unsigned chv_gpio_irq_startup(struct irq_data *d)
 	 */
 	if (irqd_get_trigger_type(d) == IRQ_TYPE_NONE) {
 		struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-		struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+		struct chv_pinctrl *pctrl = gpiochip_get_data(gc);
 		unsigned offset = irqd_to_hwirq(d);
 		int pin = chv_gpio_offset_to_pin(pctrl, offset);
 		irq_flow_handler_t handler;
@@ -1334,7 +1332,7 @@ static unsigned chv_gpio_irq_startup(struct irq_data *d)
 static int chv_gpio_irq_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(gc);
 	unsigned offset = irqd_to_hwirq(d);
 	int pin = chv_gpio_offset_to_pin(pctrl, offset);
 	unsigned long flags;
@@ -1407,7 +1405,7 @@ static struct irq_chip chv_gpio_irqchip = {
 static void chv_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct chv_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct chv_pinctrl *pctrl = gpiochip_get_data(gc);
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	unsigned long pending;
 	u32 intr_line;
@@ -1436,10 +1434,10 @@ static int chv_gpio_probe(struct chv_pinctrl *pctrl, int irq)
 
 	chip->ngpio = pctrl->community->ngpios;
 	chip->label = dev_name(pctrl->dev);
-	chip->dev = pctrl->dev;
+	chip->parent = pctrl->dev;
 	chip->base = -1;
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, pctrl);
 	if (ret) {
 		dev_err(pctrl->dev, "Failed to register gpiochip\n");
 		return ret;
diff --git a/drivers/pinctrl/intel/pinctrl-intel.c b/drivers/pinctrl/intel/pinctrl-intel.c
index 26f6b6ffea5b..c0f5586218c4 100644
--- a/drivers/pinctrl/intel/pinctrl-intel.c
+++ b/drivers/pinctrl/intel/pinctrl-intel.c
@@ -103,7 +103,6 @@ struct intel_pinctrl {
 	struct intel_pinctrl_context context;
 };
 
-#define gpiochip_to_pinctrl(c)	container_of(c, struct intel_pinctrl, chip)
 #define pin_to_padno(c, p)	((p) - (c)->pin_base)
 
 static struct intel_community *intel_get_community(struct intel_pinctrl *pctrl,
@@ -596,7 +595,7 @@ static const struct pinctrl_desc intel_pinctrl_desc = {
 
 static int intel_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(chip);
 	void __iomem *reg;
 
 	reg = intel_get_padcfg(pctrl, offset, PADCFG0);
@@ -608,7 +607,7 @@ static int intel_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void intel_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(chip);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(chip);
 	void __iomem *reg;
 
 	reg = intel_get_padcfg(pctrl, offset, PADCFG0);
@@ -652,7 +651,7 @@ static const struct gpio_chip intel_gpio_chip = {
 static void intel_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
 
@@ -673,7 +672,7 @@ static void intel_gpio_irq_ack(struct irq_data *d)
 static void intel_gpio_irq_mask_unmask(struct irq_data *d, bool mask)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
 	unsigned long flags;
@@ -713,7 +712,7 @@ static void intel_gpio_irq_unmask(struct irq_data *d)
 static int intel_gpio_irq_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	unsigned pin = irqd_to_hwirq(d);
 	unsigned long flags;
 	void __iomem *reg;
@@ -767,7 +766,7 @@ static int intel_gpio_irq_type(struct irq_data *d, unsigned type)
 static int intel_gpio_irq_wake(struct irq_data *d, unsigned int on)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct intel_pinctrl *pctrl = gpiochip_to_pinctrl(gc);
+	struct intel_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct intel_community *community;
 	unsigned pin = irqd_to_hwirq(d);
 	unsigned padno, gpp, gpp_offset;
@@ -872,10 +871,10 @@ static int intel_gpio_probe(struct intel_pinctrl *pctrl, int irq)
 
 	pctrl->chip.ngpio = pctrl->soc->npins;
 	pctrl->chip.label = dev_name(pctrl->dev);
-	pctrl->chip.dev = pctrl->dev;
+	pctrl->chip.parent = pctrl->dev;
 	pctrl->chip.base = -1;
 
-	ret = gpiochip_add(&pctrl->chip);
+	ret = gpiochip_add_data(&pctrl->chip, pctrl);
 	if (ret) {
 		dev_err(pctrl->dev, "failed to register gpiochip\n");
 		return ret;
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8127.c b/drivers/pinctrl/mediatek/pinctrl-mt8127.c
index b317b0b664ea..98e0bebfdf92 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt8127.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8127.c
@@ -351,7 +351,7 @@ static int __init mtk_pinctrl_init(void)
 	return platform_driver_register(&mtk_pinctrl_driver);
 }
 
-module_init(mtk_pinctrl_init);
+arch_initcall(mtk_pinctrl_init);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("MediaTek MT8127 Pinctrl Driver");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8135.c b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
index 404f1178511d..1c153b860f36 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt8135.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8135.c
@@ -366,7 +366,7 @@ static int __init mtk_pinctrl_init(void)
 	return platform_driver_register(&mtk_pinctrl_driver);
 }
 
-module_init(mtk_pinctrl_init);
+arch_initcall(mtk_pinctrl_init);
 
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MediaTek Pinctrl Driver");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mt8173.c b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
index ad271840d865..a62514eb2129 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mt8173.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mt8173.c
@@ -394,7 +394,7 @@ static int __init mtk_pinctrl_init(void)
 	return platform_driver_register(&mtk_pinctrl_driver);
 }
 
-module_init(mtk_pinctrl_init);
+arch_initcall(mtk_pinctrl_init);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("MediaTek Pinctrl Driver");
diff --git a/drivers/pinctrl/mediatek/pinctrl-mtk-common.c b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
index 5c717275a7fa..e96e86d2e745 100644
--- a/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
+++ b/drivers/pinctrl/mediatek/pinctrl-mtk-common.c
@@ -14,7 +14,7 @@
  */
 
 #include <linux/io.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
@@ -95,7 +95,7 @@ static void mtk_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
 	unsigned int reg_addr;
 	unsigned int bit;
-	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct mtk_pinctrl *pctl = gpiochip_get_data(chip);
 
 	reg_addr = mtk_get_port(pctl, offset) + pctl->devdata->dout_offset;
 	bit = BIT(offset & 0xf);
@@ -347,6 +347,7 @@ static int mtk_pconf_parse_conf(struct pinctrl_dev *pctldev,
 		ret = mtk_pconf_set_pull_select(pctl, pin, true, false, arg);
 		break;
 	case PIN_CONFIG_INPUT_ENABLE:
+		mtk_pmx_gpio_set_direction(pctldev, NULL, pin, true);
 		ret = mtk_pconf_set_ies_smt(pctl, pin, arg, param);
 		break;
 	case PIN_CONFIG_OUTPUT:
@@ -354,6 +355,7 @@ static int mtk_pconf_parse_conf(struct pinctrl_dev *pctldev,
 		ret = mtk_pmx_gpio_set_direction(pctldev, NULL, pin, false);
 		break;
 	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		mtk_pmx_gpio_set_direction(pctldev, NULL, pin, true);
 		ret = mtk_pconf_set_ies_smt(pctl, pin, arg, param);
 		break;
 	case PIN_CONFIG_DRIVE_STRENGTH:
@@ -509,6 +511,9 @@ static int mtk_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 
 	err = pinconf_generic_parse_dt_config(node, pctldev, &configs,
 		&num_configs);
+	if (err)
+		return err;
+
 	if (num_configs)
 		has_config = 1;
 
@@ -520,21 +525,23 @@ static int mtk_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	if (has_config && num_pins >= 1)
 		maps_per_pin++;
 
-	if (!num_pins || !maps_per_pin)
-		return -EINVAL;
+	if (!num_pins || !maps_per_pin) {
+		err = -EINVAL;
+		goto exit;
+	}
 
 	reserve = num_pins * maps_per_pin;
 
 	err = pinctrl_utils_reserve_map(pctldev, map,
 			reserved_maps, num_maps, reserve);
 	if (err < 0)
-		goto fail;
+		goto exit;
 
 	for (i = 0; i < num_pins; i++) {
 		err = of_property_read_u32_index(node, "pinmux",
 				i, &pinfunc);
 		if (err)
-			goto fail;
+			goto exit;
 
 		pin = MTK_GET_PIN_NO(pinfunc);
 		func = MTK_GET_PIN_FUNC(pinfunc);
@@ -543,20 +550,21 @@ static int mtk_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 				func >= ARRAY_SIZE(mtk_gpio_functions)) {
 			dev_err(pctl->dev, "invalid pins value.\n");
 			err = -EINVAL;
-			goto fail;
+			goto exit;
 		}
 
 		grp = mtk_pctrl_find_group_by_pin(pctl, pin);
 		if (!grp) {
 			dev_err(pctl->dev, "unable to match pin %d to group\n",
 					pin);
-			return -EINVAL;
+			err = -EINVAL;
+			goto exit;
 		}
 
 		err = mtk_pctrl_dt_node_to_map_func(pctl, pin, func, grp, map,
 				reserved_maps, num_maps);
 		if (err < 0)
-			goto fail;
+			goto exit;
 
 		if (has_config) {
 			err = pinctrl_utils_add_map_configs(pctldev, map,
@@ -564,13 +572,14 @@ static int mtk_pctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 					configs, num_configs,
 					PIN_MAP_TYPE_CONFIGS_GROUP);
 			if (err < 0)
-				goto fail;
+				goto exit;
 		}
 	}
 
-	return 0;
+	err = 0;
 
-fail:
+exit:
+	kfree(configs);
 	return err;
 }
 
@@ -591,6 +600,7 @@ static int mtk_pctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 				&reserved_maps, num_maps);
 		if (ret < 0) {
 			pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
+			of_node_put(np);
 			return ret;
 		}
 	}
@@ -742,7 +752,7 @@ static int mtk_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 	unsigned int bit;
 	unsigned int read_val = 0;
 
-	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct mtk_pinctrl *pctl = gpiochip_get_data(chip);
 
 	reg_addr =  mtk_get_port(pctl, offset) + pctl->devdata->dir_offset;
 	bit = BIT(offset & 0xf);
@@ -755,7 +765,7 @@ static int mtk_gpio_get(struct gpio_chip *chip, unsigned offset)
 	unsigned int reg_addr;
 	unsigned int bit;
 	unsigned int read_val = 0;
-	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct mtk_pinctrl *pctl = gpiochip_get_data(chip);
 
 	reg_addr = mtk_get_port(pctl, offset) +
 		pctl->devdata->din_offset;
@@ -768,7 +778,7 @@ static int mtk_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int mtk_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
 	const struct mtk_desc_pin *pin;
-	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct mtk_pinctrl *pctl = gpiochip_get_data(chip);
 	int irq;
 
 	pin = pctl->devdata->pins + offset;
@@ -936,7 +946,7 @@ static void mtk_eint_unmask(struct irq_data *d)
 static int mtk_gpio_set_debounce(struct gpio_chip *chip, unsigned offset,
 	unsigned debounce)
 {
-	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct mtk_pinctrl *pctl = dev_get_drvdata(chip->parent);
 	int eint_num, virq, eint_offset;
 	unsigned int set_offset, bit, clr_bit, clr_offset, rst, i, unmask, dbnc;
 	static const unsigned int dbnc_arr[] = {0 , 1, 16, 32, 64, 128, 256};
@@ -1345,10 +1355,10 @@ int mtk_pctrl_init(struct platform_device *pdev,
 	*pctl->chip = mtk_gpio_chip;
 	pctl->chip->ngpio = pctl->devdata->npins;
 	pctl->chip->label = dev_name(&pdev->dev);
-	pctl->chip->dev = &pdev->dev;
+	pctl->chip->parent = &pdev->dev;
 	pctl->chip->base = -1;
 
-	ret = gpiochip_add(pctl->chip);
+	ret = gpiochip_add_data(pctl->chip, pctl);
 	if (ret) {
 		ret = -EINVAL;
 		goto pctrl_error;
diff --git a/drivers/pinctrl/meson/pinctrl-meson.c b/drivers/pinctrl/meson/pinctrl-meson.c
index 84943e4cff09..50cab27c64d4 100644
--- a/drivers/pinctrl/meson/pinctrl-meson.c
+++ b/drivers/pinctrl/meson/pinctrl-meson.c
@@ -448,11 +448,6 @@ static const struct pinconf_ops meson_pinconf_ops = {
 	.is_generic		= true,
 };
 
-static inline struct meson_domain *to_meson_domain(struct gpio_chip *chip)
-{
-	return container_of(chip, struct meson_domain, chip);
-}
-
 static int meson_gpio_request(struct gpio_chip *chip, unsigned gpio)
 {
 	return pinctrl_request_gpio(chip->base + gpio);
@@ -460,14 +455,14 @@ static int meson_gpio_request(struct gpio_chip *chip, unsigned gpio)
 
 static void meson_gpio_free(struct gpio_chip *chip, unsigned gpio)
 {
-	struct meson_domain *domain = to_meson_domain(chip);
+	struct meson_domain *domain = gpiochip_get_data(chip);
 
 	pinctrl_free_gpio(domain->data->pin_base + gpio);
 }
 
 static int meson_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct meson_domain *domain = to_meson_domain(chip);
+	struct meson_domain *domain = gpiochip_get_data(chip);
 	unsigned int reg, bit, pin;
 	struct meson_bank *bank;
 	int ret;
@@ -485,7 +480,7 @@ static int meson_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 static int meson_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 				       int value)
 {
-	struct meson_domain *domain = to_meson_domain(chip);
+	struct meson_domain *domain = gpiochip_get_data(chip);
 	unsigned int reg, bit, pin;
 	struct meson_bank *bank;
 	int ret;
@@ -507,7 +502,7 @@ static int meson_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 static void meson_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 {
-	struct meson_domain *domain = to_meson_domain(chip);
+	struct meson_domain *domain = gpiochip_get_data(chip);
 	unsigned int reg, bit, pin;
 	struct meson_bank *bank;
 	int ret;
@@ -524,7 +519,7 @@ static void meson_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 
 static int meson_gpio_get(struct gpio_chip *chip, unsigned gpio)
 {
-	struct meson_domain *domain = to_meson_domain(chip);
+	struct meson_domain *domain = gpiochip_get_data(chip);
 	unsigned int reg, bit, val, pin;
 	struct meson_bank *bank;
 	int ret;
@@ -562,7 +557,7 @@ static int meson_gpiolib_register(struct meson_pinctrl *pc)
 		domain = &pc->domains[i];
 
 		domain->chip.label = domain->data->name;
-		domain->chip.dev = pc->dev;
+		domain->chip.parent = pc->dev;
 		domain->chip.request = meson_gpio_request;
 		domain->chip.free = meson_gpio_free;
 		domain->chip.direction_input = meson_gpio_direction_input;
@@ -575,7 +570,7 @@ static int meson_gpiolib_register(struct meson_pinctrl *pc)
 		domain->chip.of_node = domain->of_node;
 		domain->chip.of_gpio_n_cells = 2;
 
-		ret = gpiochip_add(&domain->chip);
+		ret = gpiochip_add_data(&domain->chip, domain);
 		if (ret) {
 			dev_err(pc->dev, "can't add gpio chip %s\n",
 				domain->data->name);
diff --git a/drivers/pinctrl/mvebu/Makefile b/drivers/pinctrl/mvebu/Makefile
index 554d8af14eeb..18270cd5ea43 100644
--- a/drivers/pinctrl/mvebu/Makefile
+++ b/drivers/pinctrl/mvebu/Makefile
@@ -1,4 +1,4 @@
-obj-$(CONFIG_PINCTRL_MVEBU)	+= pinctrl-mvebu.o
+obj-y				+= pinctrl-mvebu.o
 obj-$(CONFIG_PINCTRL_DOVE)	+= pinctrl-dove.o
 obj-$(CONFIG_PINCTRL_KIRKWOOD)	+= pinctrl-kirkwood.o
 obj-$(CONFIG_PINCTRL_ARMADA_370) += pinctrl-armada-370.o
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.c b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
index 77d2221d379d..3ef798fac81b 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.c
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
@@ -663,28 +663,23 @@ int mvebu_pinctrl_probe(struct platform_device *pdev)
 	/* assign mpp modes to groups */
 	for (n = 0; n < soc->nmodes; n++) {
 		struct mvebu_mpp_mode *mode = &soc->modes[n];
-		struct mvebu_pinctrl_group *grp =
-			mvebu_pinctrl_find_group_by_pid(pctl, mode->pid);
+		struct mvebu_mpp_ctrl_setting *set = &mode->settings[0];
+		struct mvebu_pinctrl_group *grp;
 		unsigned num_settings;
+		unsigned supp_settings;
 
-		if (!grp) {
-			dev_warn(&pdev->dev, "unknown pinctrl group %d\n",
-				mode->pid);
-			continue;
-		}
-
-		for (num_settings = 0; ;) {
-			struct mvebu_mpp_ctrl_setting *set =
-				&mode->settings[num_settings];
-
+		for (num_settings = 0, supp_settings = 0; ; set++) {
 			if (!set->name)
 				break;
+
 			num_settings++;
 
 			/* skip unsupported settings for this variant */
 			if (pctl->variant && !(pctl->variant & set->variant))
 				continue;
 
+			supp_settings++;
+
 			/* find gpio/gpo/gpi settings */
 			if (strcmp(set->name, "gpio") == 0)
 				set->flags = MVEBU_SETTING_GPI |
@@ -695,6 +690,17 @@ int mvebu_pinctrl_probe(struct platform_device *pdev)
 				set->flags = MVEBU_SETTING_GPI;
 		}
 
+		/* skip modes with no settings for this variant */
+		if (!supp_settings)
+			continue;
+
+		grp = mvebu_pinctrl_find_group_by_pid(pctl, mode->pid);
+		if (!grp) {
+			dev_warn(&pdev->dev, "unknown pinctrl group %d\n",
+				mode->pid);
+			continue;
+		}
+
 		grp->settings = mode->settings;
 		grp->num_settings = num_settings;
 	}
diff --git a/drivers/pinctrl/nomadik/pinctrl-abx500.c b/drivers/pinctrl/nomadik/pinctrl-abx500.c
index b59fbb4b1fb1..1f7469c9857d 100644
--- a/drivers/pinctrl/nomadik/pinctrl-abx500.c
+++ b/drivers/pinctrl/nomadik/pinctrl-abx500.c
@@ -109,19 +109,10 @@ struct abx500_pinctrl {
 	int irq_cluster_size;
 };
 
-/**
- * to_abx500_pinctrl() - get the pointer to abx500_pinctrl
- * @chip:	Member of the structure abx500_pinctrl
- */
-static inline struct abx500_pinctrl *to_abx500_pinctrl(struct gpio_chip *chip)
-{
-	return container_of(chip, struct abx500_pinctrl, chip);
-}
-
 static int abx500_gpio_get_bit(struct gpio_chip *chip, u8 reg,
 			       unsigned offset, bool *bit)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	u8 pos = offset % 8;
 	u8 val;
 	int ret;
@@ -143,7 +134,7 @@ static int abx500_gpio_get_bit(struct gpio_chip *chip, u8 reg,
 static int abx500_gpio_set_bits(struct gpio_chip *chip, u8 reg,
 				unsigned offset, int val)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	u8 pos = offset % 8;
 	int ret;
 
@@ -164,7 +155,7 @@ static int abx500_gpio_set_bits(struct gpio_chip *chip, u8 reg,
  */
 static int abx500_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	bool bit;
 	bool is_out;
 	u8 gpio_offset = offset - 1;
@@ -192,7 +183,7 @@ out:
 
 static void abx500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	int ret;
 
 	ret = abx500_gpio_set_bits(chip, AB8500_GPIO_OUT1_REG, offset, val);
@@ -200,6 +191,7 @@ static void abx500_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 		dev_err(pct->dev, "%s write failed (%d)\n", __func__, ret);
 }
 
+#ifdef CONFIG_DEBUG_FS
 static int abx500_get_pull_updown(struct abx500_pinctrl *pct, int offset,
 				  enum abx500_gpio_pull_updown *pull_updown)
 {
@@ -235,6 +227,7 @@ out:
 
 	return ret;
 }
+#endif
 
 static int abx500_set_pull_updown(struct abx500_pinctrl *pct,
 				  int offset, enum abx500_gpio_pull_updown val)
@@ -272,7 +265,7 @@ out:
 
 static bool abx500_pullud_supported(struct gpio_chip *chip, unsigned gpio)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	struct pullud *pullud = pct->soc->pullud;
 
 	return (pullud &&
@@ -284,7 +277,7 @@ static int abx500_gpio_direction_output(struct gpio_chip *chip,
 					unsigned offset,
 					int val)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	unsigned gpio;
 	int ret;
 
@@ -332,7 +325,7 @@ static int abx500_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int abx500_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	/* The AB8500 GPIO numbers are off by one */
 	int gpio = offset + 1;
 	int hwirq;
@@ -477,6 +470,7 @@ out:
 	return ret;
 }
 
+#ifdef CONFIG_DEBUG_FS
 static int abx500_get_mode(struct pinctrl_dev *pctldev, struct gpio_chip *chip,
 			  unsigned gpio)
 {
@@ -562,8 +556,6 @@ out:
 	return ret;
 }
 
-#ifdef CONFIG_DEBUG_FS
-
 #include <linux/seq_file.h>
 
 static void abx500_gpio_dbg_show_one(struct seq_file *s,
@@ -634,7 +626,7 @@ static void abx500_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
 	unsigned i;
 	unsigned gpio = chip->base;
-	struct abx500_pinctrl *pct = to_abx500_pinctrl(chip);
+	struct abx500_pinctrl *pct = gpiochip_get_data(chip);
 	struct pinctrl_dev *pctldev = pct->pctldev;
 
 	for (i = 0; i < chip->ngpio; i++, gpio++) {
@@ -986,7 +978,7 @@ static int abx500_pin_config_set(struct pinctrl_dev *pctldev,
 		param = pinconf_to_config_param(configs[i]);
 		argument = pinconf_to_config_argument(configs[i]);
 
-		dev_dbg(chip->dev, "pin %d [%#lx]: %s %s\n",
+		dev_dbg(chip->parent, "pin %d [%#lx]: %s %s\n",
 			pin, configs[i],
 			(param == PIN_CONFIG_OUTPUT) ? "output " : "input",
 			(param == PIN_CONFIG_OUTPUT) ?
@@ -1077,7 +1069,8 @@ static int abx500_pin_config_set(struct pinctrl_dev *pctldev,
 			break;
 
 		default:
-			dev_err(chip->dev, "illegal configuration requested\n");
+			dev_err(chip->parent,
+				"illegal configuration requested\n");
 		}
 	} /* for each config */
 out:
@@ -1172,7 +1165,7 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	pct->dev = &pdev->dev;
 	pct->parent = dev_get_drvdata(pdev->dev.parent);
 	pct->chip = abx500gpio_chip;
-	pct->chip.dev = &pdev->dev;
+	pct->chip.parent = &pdev->dev;
 	pct->chip.base = -1; /* Dynamic allocation */
 
 	match = of_match_device(abx500_gpio_match, &pdev->dev);
@@ -1210,7 +1203,7 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	pct->irq_cluster = pct->soc->gpio_irq_cluster;
 	pct->irq_cluster_size = pct->soc->ngpio_irq_cluster;
 
-	ret = gpiochip_add(&pct->chip);
+	ret = gpiochip_add_data(&pct->chip, pct);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
 		return ret;
diff --git a/drivers/pinctrl/nomadik/pinctrl-nomadik.c b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
index eebfae0c9b7c..352406108fa0 100644
--- a/drivers/pinctrl/nomadik/pinctrl-nomadik.c
+++ b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
@@ -438,7 +438,7 @@ nmk_gpio_disable_lazy_irq(struct nmk_gpio_chip *nmk_chip, unsigned offset)
 			       nmk_chip->addr + NMK_GPIO_FIMSC);
 	}
 
-	dev_dbg(nmk_chip->chip.dev, "%d: clearing interrupt mask\n", gpio);
+	dev_dbg(nmk_chip->chip.parent, "%d: clearing interrupt mask\n", gpio);
 }
 
 static void nmk_write_masked(void __iomem *reg, u32 mask, u32 value)
@@ -646,7 +646,7 @@ static inline int nmk_gpio_get_bitmask(int gpio)
 static void nmk_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
-	struct nmk_gpio_chip *nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 
 	clk_enable(nmk_chip->clk);
 	writel(nmk_gpio_get_bitmask(d->hwirq), nmk_chip->addr + NMK_GPIO_IC);
@@ -863,7 +863,7 @@ static void __nmk_gpio_irq_handler(struct irq_desc *desc, u32 status)
 static void nmk_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *chip = irq_desc_get_handler_data(desc);
-	struct nmk_gpio_chip *nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 	u32 status;
 
 	clk_enable(nmk_chip->clk);
@@ -876,7 +876,7 @@ static void nmk_gpio_irq_handler(struct irq_desc *desc)
 static void nmk_gpio_latent_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *chip = irq_desc_get_handler_data(desc);
-	struct nmk_gpio_chip *nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 	u32 status = nmk_chip->get_latent_status(nmk_chip->bank);
 
 	__nmk_gpio_irq_handler(desc, status);
@@ -886,8 +886,7 @@ static void nmk_gpio_latent_irq_handler(struct irq_desc *desc)
 
 static int nmk_gpio_make_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct nmk_gpio_chip *nmk_chip =
-		container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 
 	clk_enable(nmk_chip->clk);
 
@@ -900,8 +899,7 @@ static int nmk_gpio_make_input(struct gpio_chip *chip, unsigned offset)
 
 static int nmk_gpio_get_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct nmk_gpio_chip *nmk_chip =
-		container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 	u32 bit = 1 << offset;
 	int value;
 
@@ -917,8 +915,7 @@ static int nmk_gpio_get_input(struct gpio_chip *chip, unsigned offset)
 static void nmk_gpio_set_output(struct gpio_chip *chip, unsigned offset,
 				int val)
 {
-	struct nmk_gpio_chip *nmk_chip =
-		container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 
 	clk_enable(nmk_chip->clk);
 
@@ -930,8 +927,7 @@ static void nmk_gpio_set_output(struct gpio_chip *chip, unsigned offset,
 static int nmk_gpio_make_output(struct gpio_chip *chip, unsigned offset,
 				int val)
 {
-	struct nmk_gpio_chip *nmk_chip =
-		container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 
 	clk_enable(nmk_chip->clk);
 
@@ -951,8 +947,7 @@ static void nmk_gpio_dbg_show_one(struct seq_file *s,
 	unsigned offset, unsigned gpio)
 {
 	const char *label = gpiochip_is_requested(chip, offset);
-	struct nmk_gpio_chip *nmk_chip =
-		container_of(chip, struct nmk_gpio_chip, chip);
+	struct nmk_gpio_chip *nmk_chip = gpiochip_get_data(chip);
 	int mode;
 	bool is_out;
 	bool data_out;
@@ -1188,7 +1183,7 @@ static struct nmk_gpio_chip *nmk_gpio_populate_chip(struct device_node *np,
 	chip->base = id * NMK_GPIO_PER_CHIP;
 	chip->ngpio = NMK_GPIO_PER_CHIP;
 	chip->label = dev_name(&gpio_pdev->dev);
-	chip->dev = &gpio_pdev->dev;
+	chip->parent = &gpio_pdev->dev;
 
 	res = platform_get_resource(gpio_pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(&pdev->dev, res);
@@ -1278,7 +1273,7 @@ static int nmk_gpio_probe(struct platform_device *dev)
 	clk_disable(nmk_chip->clk);
 	chip->of_node = np;
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, nmk_chip);
 	if (ret)
 		return ret;
 
@@ -1789,7 +1784,7 @@ static int nmk_gpio_request_enable(struct pinctrl_dev *pctldev,
 		return -EINVAL;
 	}
 	chip = range->gc;
-	nmk_chip = container_of(chip, struct nmk_gpio_chip, chip);
+	nmk_chip = gpiochip_get_data(chip);
 
 	dev_dbg(npct->dev, "enable pin %u as GPIO\n", offset);
 
@@ -1890,7 +1885,7 @@ static int nmk_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 			if (slpm_val)
 				val = slpm_val - 1;
 
-			dev_dbg(nmk_chip->chip.dev,
+			dev_dbg(nmk_chip->chip.parent,
 				"pin %d: sleep pull %s, dir %s, val %s\n",
 				pin,
 				slpm_pull ? pullnames[pull] : "same",
@@ -1899,7 +1894,7 @@ static int nmk_pin_config_set(struct pinctrl_dev *pctldev, unsigned pin,
 				slpm_val ? (val ? "high" : "low") : "same");
 		}
 
-		dev_dbg(nmk_chip->chip.dev,
+		dev_dbg(nmk_chip->chip.parent,
 			"pin %d [%#lx]: pull %s, slpm %s (%s%s), lowemi %s\n",
 			pin, cfg, pullnames[pull], slpmnames[slpm],
 			output ? "output " : "input",
diff --git a/drivers/pinctrl/pinconf-generic.c b/drivers/pinctrl/pinconf-generic.c
index 099a3442ff42..79e6159712c2 100644
--- a/drivers/pinctrl/pinconf-generic.c
+++ b/drivers/pinctrl/pinconf-generic.c
@@ -220,6 +220,7 @@ static void parse_dt_cfg(struct device_node *np,
  * parse the config properties into generic pinconfig values.
  * @np: node containing the pinconfig properties
  * @configs: array with nconfigs entries containing the generic pinconf values
+ *           must be freed when no longer necessary.
  * @nconfigs: umber of configurations
  */
 int pinconf_generic_parse_dt_config(struct device_node *np,
diff --git a/drivers/pinctrl/pinctrl-adi2.c b/drivers/pinctrl/pinctrl-adi2.c
index fd342dffe4dc..ecb57635a37e 100644
--- a/drivers/pinctrl/pinctrl-adi2.c
+++ b/drivers/pinctrl/pinctrl-adi2.c
@@ -636,7 +636,7 @@ static int adi_pinmux_set(struct pinctrl_dev *pctldev, unsigned func_id,
 		if (range == NULL) /* should not happen */
 			return -ENODEV;
 
-		port = container_of(range->gc, struct gpio_port, chip);
+		port = gpiochip_get_data(range->gc);
 
 		spin_lock_irqsave(&port->lock, flags);
 
@@ -684,7 +684,7 @@ static int adi_pinmux_request_gpio(struct pinctrl_dev *pctldev,
 	unsigned long flags;
 	u8 offset;
 
-	port = container_of(range->gc, struct gpio_port, chip);
+	port = gpiochip_get_data(range->gc);
 	offset = pin_to_offset(range, pin);
 
 	spin_lock_irqsave(&port->lock, flags);
@@ -718,7 +718,7 @@ static int adi_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 	struct gpio_port *port;
 	unsigned long flags;
 
-	port = container_of(chip, struct gpio_port, chip);
+	port = gpiochip_get_data(chip);
 
 	spin_lock_irqsave(&port->lock, flags);
 
@@ -733,7 +733,7 @@ static int adi_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static void adi_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 	int value)
 {
-	struct gpio_port *port = container_of(chip, struct gpio_port, chip);
+	struct gpio_port *port = gpiochip_get_data(chip);
 	struct gpio_port_t *regs = port->regs;
 	unsigned long flags;
 
@@ -750,7 +750,7 @@ static void adi_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 static int adi_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 	int value)
 {
-	struct gpio_port *port = container_of(chip, struct gpio_port, chip);
+	struct gpio_port *port = gpiochip_get_data(chip);
 	struct gpio_port_t *regs = port->regs;
 	unsigned long flags;
 
@@ -770,7 +770,7 @@ static int adi_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static int adi_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_port *port = container_of(chip, struct gpio_port, chip);
+	struct gpio_port *port = gpiochip_get_data(chip);
 	struct gpio_port_t *regs = port->regs;
 	unsigned long flags;
 	int ret;
@@ -786,7 +786,7 @@ static int adi_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 
 static int adi_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct gpio_port *port = container_of(chip, struct gpio_port, chip);
+	struct gpio_port *port = gpiochip_get_data(chip);
 
 	if (port->irq_base >= 0)
 		return irq_find_mapping(port->domain, offset);
@@ -994,7 +994,7 @@ static int adi_gpio_probe(struct platform_device *pdev)
 	port->chip.ngpio		= port->width;
 	gpio = port->chip.base + port->width;
 
-	ret = gpiochip_add(&port->chip);
+	ret = gpiochip_add_data(&port->chip, port);
 	if (ret) {
 		dev_err(&pdev->dev, "Fail to add GPIO chip.\n");
 		goto out_remove_domain;
@@ -1102,32 +1102,24 @@ static struct platform_driver adi_gpio_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&adi_pinctrl_driver,
+	&adi_gpio_pint_driver,
+	&adi_gpio_driver,
+};
+
 static int __init adi_pinctrl_setup(void)
 {
 	int ret;
 
-	ret = platform_driver_register(&adi_pinctrl_driver);
+	ret = platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 	if (ret)
 		return ret;
 
-	ret = platform_driver_register(&adi_gpio_pint_driver);
-	if (ret)
-		goto pint_error;
-
-	ret = platform_driver_register(&adi_gpio_driver);
-	if (ret)
-		goto gpio_error;
-
 #ifdef CONFIG_PM
 	register_syscore_ops(&gpio_pm_syscore_ops);
 #endif
-	return ret;
-gpio_error:
-	platform_driver_unregister(&adi_gpio_pint_driver);
-pint_error:
-	platform_driver_unregister(&adi_pinctrl_driver);
-
-	return ret;
+	return 0;
 }
 arch_initcall(adi_pinctrl_setup);
 
diff --git a/drivers/pinctrl/pinctrl-amd.c b/drivers/pinctrl/pinctrl-amd.c
index 3318f1d6193c..657449431301 100644
--- a/drivers/pinctrl/pinctrl-amd.c
+++ b/drivers/pinctrl/pinctrl-amd.c
@@ -35,16 +35,11 @@
 #include "pinctrl-utils.h"
 #include "pinctrl-amd.h"
 
-static inline struct amd_gpio *to_amd_gpio(struct gpio_chip *gc)
-{
-	return container_of(gc, struct amd_gpio, gc);
-}
-
 static int amd_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
 	unsigned long flags;
 	u32 pin_reg;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + offset * 4);
@@ -71,7 +66,7 @@ static int amd_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
 {
 	u32 pin_reg;
 	unsigned long flags;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + offset * 4);
@@ -90,7 +85,7 @@ static int amd_gpio_get_value(struct gpio_chip *gc, unsigned offset)
 {
 	u32 pin_reg;
 	unsigned long flags;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + offset * 4);
@@ -103,7 +98,7 @@ static void amd_gpio_set_value(struct gpio_chip *gc, unsigned offset, int value)
 {
 	u32 pin_reg;
 	unsigned long flags;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + offset * 4);
@@ -122,7 +117,7 @@ static int amd_gpio_set_debounce(struct gpio_chip *gc, unsigned offset,
 	u32 pin_reg;
 	int ret = 0;
 	unsigned long flags;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + offset * 4);
@@ -186,7 +181,7 @@ static void amd_gpio_dbg_show(struct seq_file *s, struct gpio_chip *gc)
 	u32 pin_reg;
 	unsigned long flags;
 	unsigned int bank, i, pin_num;
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	char *level_trig;
 	char *active_level;
@@ -327,7 +322,7 @@ static void amd_gpio_irq_enable(struct irq_data *d)
 	u32 pin_reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
@@ -351,7 +346,7 @@ static void amd_gpio_irq_disable(struct irq_data *d)
 	u32 pin_reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
@@ -366,7 +361,7 @@ static void amd_gpio_irq_mask(struct irq_data *d)
 	u32 pin_reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
@@ -380,7 +375,7 @@ static void amd_gpio_irq_unmask(struct irq_data *d)
 	u32 pin_reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
@@ -394,7 +389,7 @@ static void amd_gpio_irq_eoi(struct irq_data *d)
 	u32 reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	reg = readl(gpio_dev->base + WAKE_INT_MASTER_REG);
@@ -409,7 +404,7 @@ static int amd_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	u32 pin_reg;
 	unsigned long flags;
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	spin_lock_irqsave(&gpio_dev->lock, flags);
 	pin_reg = readl(gpio_dev->base + (d->hwirq)*4);
@@ -504,7 +499,7 @@ static void amd_gpio_irq_handler(struct irq_desc *desc)
 	unsigned long flags;
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct amd_gpio *gpio_dev = to_amd_gpio(gc);
+	struct amd_gpio *gpio_dev = gpiochip_get_data(gc);
 
 	chained_irq_enter(chip, desc);
 	/*enable GPIO interrupt again*/
@@ -778,7 +773,7 @@ static int amd_gpio_probe(struct platform_device *pdev)
 	gpio_dev->gc.base			= 0;
 	gpio_dev->gc.label			= pdev->name;
 	gpio_dev->gc.owner			= THIS_MODULE;
-	gpio_dev->gc.dev			= &pdev->dev;
+	gpio_dev->gc.parent			= &pdev->dev;
 	gpio_dev->gc.ngpio			= TOTAL_NUMBER_OF_PINS;
 #if defined(CONFIG_OF_GPIO)
 	gpio_dev->gc.of_node			= pdev->dev.of_node;
@@ -795,7 +790,7 @@ static int amd_gpio_probe(struct platform_device *pdev)
 		return PTR_ERR(gpio_dev->pctrl);
 	}
 
-	ret = gpiochip_add(&gpio_dev->gc);
+	ret = gpiochip_add_data(&gpio_dev->gc, gpio_dev);
 	if (ret)
 		goto out1;
 
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index 56af28b95a44..e844fdc6d3a8 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -436,14 +436,9 @@ static struct pinctrl_desc as3722_pinctrl_desc = {
 	.owner = THIS_MODULE,
 };
 
-static inline struct as3722_pctrl_info *to_as_pci(struct gpio_chip *chip)
-{
-	return container_of(chip, struct as3722_pctrl_info, gpio_chip);
-}
-
 static int as3722_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct as3722_pctrl_info *as_pci = to_as_pci(chip);
+	struct as3722_pctrl_info *as_pci = gpiochip_get_data(chip);
 	struct as3722 *as3722 = as_pci->as3722;
 	int ret;
 	u32 reg;
@@ -491,7 +486,7 @@ static int as3722_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void as3722_gpio_set(struct gpio_chip *chip, unsigned offset,
 		int value)
 {
-	struct as3722_pctrl_info *as_pci = to_as_pci(chip);
+	struct as3722_pctrl_info *as_pci = gpiochip_get_data(chip);
 	struct as3722 *as3722 = as_pci->as3722;
 	int en_invert;
 	u32 val;
@@ -531,7 +526,7 @@ static int as3722_gpio_direction_output(struct gpio_chip *chip,
 
 static int as3722_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct as3722_pctrl_info *as_pci = to_as_pci(chip);
+	struct as3722_pctrl_info *as_pci = gpiochip_get_data(chip);
 
 	return as3722_irq_get_virq(as_pci->as3722, offset);
 }
@@ -582,9 +577,9 @@ static int as3722_pinctrl_probe(struct platform_device *pdev)
 	}
 
 	as_pci->gpio_chip = as3722_gpio_chip;
-	as_pci->gpio_chip.dev = &pdev->dev;
+	as_pci->gpio_chip.parent = &pdev->dev;
 	as_pci->gpio_chip.of_node = pdev->dev.parent->of_node;
-	ret = gpiochip_add(&as_pci->gpio_chip);
+	ret = gpiochip_add_data(&as_pci->gpio_chip, as_pci);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Couldn't register gpiochip, %d\n", ret);
 		goto fail_chip_add;
diff --git a/drivers/pinctrl/pinctrl-at91-pio4.c b/drivers/pinctrl/pinctrl-at91-pio4.c
index 33edd07d9149..ee69db6ae1c7 100644
--- a/drivers/pinctrl/pinctrl-at91-pio4.c
+++ b/drivers/pinctrl/pinctrl-at91-pio4.c
@@ -15,6 +15,8 @@
  */
 
 #include <linux/clk.h>
+#include <linux/gpio/driver.h>
+/* FIXME: needed for gpio_to_irq(), get rid of this */
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
@@ -290,7 +292,7 @@ static void atmel_gpio_irq_handler(struct irq_desc *desc)
 
 static int atmel_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct atmel_pioctrl *atmel_pioctrl = dev_get_drvdata(chip->dev);
+	struct atmel_pioctrl *atmel_pioctrl = gpiochip_get_data(chip);
 	struct atmel_pin *pin = atmel_pioctrl->pins[offset];
 	unsigned reg;
 
@@ -305,7 +307,7 @@ static int atmel_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int atmel_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct atmel_pioctrl *atmel_pioctrl = dev_get_drvdata(chip->dev);
+	struct atmel_pioctrl *atmel_pioctrl = gpiochip_get_data(chip);
 	struct atmel_pin *pin = atmel_pioctrl->pins[offset];
 	unsigned reg;
 
@@ -317,7 +319,7 @@ static int atmel_gpio_get(struct gpio_chip *chip, unsigned offset)
 static int atmel_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				       int value)
 {
-	struct atmel_pioctrl *atmel_pioctrl = dev_get_drvdata(chip->dev);
+	struct atmel_pioctrl *atmel_pioctrl = gpiochip_get_data(chip);
 	struct atmel_pin *pin = atmel_pioctrl->pins[offset];
 	unsigned reg;
 
@@ -336,7 +338,7 @@ static int atmel_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static void atmel_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
-	struct atmel_pioctrl *atmel_pioctrl = dev_get_drvdata(chip->dev);
+	struct atmel_pioctrl *atmel_pioctrl = gpiochip_get_data(chip);
 	struct atmel_pin *pin = atmel_pioctrl->pins[offset];
 
 	atmel_gpio_write(atmel_pioctrl, pin->bank,
@@ -346,7 +348,7 @@ static void atmel_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 
 static int atmel_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct atmel_pioctrl *atmel_pioctrl = dev_get_drvdata(chip->dev);
+	struct atmel_pioctrl *atmel_pioctrl = gpiochip_get_data(chip);
 
 	return irq_find_mapping(atmel_pioctrl->irq_domain, offset);
 }
@@ -500,7 +502,8 @@ static int atmel_pctl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	if (!num_pins) {
 		dev_err(pctldev->dev, "no pins found in node %s\n",
 			of_node_full_name(np));
-		return -EINVAL;
+		ret = -EINVAL;
+		goto exit;
 	}
 
 	/*
@@ -514,19 +517,19 @@ static int atmel_pctl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	ret = pinctrl_utils_reserve_map(pctldev, map, reserved_maps, num_maps,
 					reserve);
 	if (ret < 0)
-		return ret;
+		goto exit;
 
 	for (i = 0; i < num_pins; i++) {
 		const char *group, *func;
 
 		ret = of_property_read_u32_index(np, "pinmux", i, &pinfunc);
 		if (ret)
-			return ret;
+			goto exit;
 
 		ret = atmel_pctl_xlate_pinfunc(pctldev, np, pinfunc, &group,
 					       &func);
 		if (ret)
-			return ret;
+			goto exit;
 
 		pinctrl_utils_add_map_mux(pctldev, map, reserved_maps, num_maps,
 					  group, func);
@@ -537,11 +540,13 @@ static int atmel_pctl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 					configs, num_configs,
 					PIN_MAP_TYPE_CONFIGS_GROUP);
 			if (ret < 0)
-				return ret;
+				goto exit;
 		}
 	}
 
-	return 0;
+exit:
+	kfree(configs);
+	return ret;
 }
 
 static int atmel_pctl_dt_node_to_map(struct pinctrl_dev *pctldev,
@@ -969,7 +974,7 @@ static int atmel_pinctrl_probe(struct platform_device *pdev)
 	atmel_pioctrl->gpio_chip->of_node = dev->of_node;
 	atmel_pioctrl->gpio_chip->ngpio = atmel_pioctrl->npins;
 	atmel_pioctrl->gpio_chip->label = dev_name(dev);
-	atmel_pioctrl->gpio_chip->dev = dev;
+	atmel_pioctrl->gpio_chip->parent = dev;
 	atmel_pioctrl->gpio_chip->names = atmel_pioctrl->group_names;
 
 	atmel_pioctrl->pm_wakeup_sources = devm_kzalloc(dev,
@@ -1000,7 +1005,7 @@ static int atmel_pinctrl_probe(struct platform_device *pdev)
 		atmel_pioctrl->irqs[i] = res->start;
 		irq_set_chained_handler(res->start, atmel_gpio_irq_handler);
 		irq_set_handler_data(res->start, atmel_pioctrl);
-		dev_dbg(dev, "bank %i: hwirq=%u\n", i, res->start);
+		dev_dbg(dev, "bank %i: irq=%pr\n", i, res);
 	}
 
 	atmel_pioctrl->irq_domain = irq_domain_add_linear(dev->of_node,
@@ -1037,7 +1042,7 @@ static int atmel_pinctrl_probe(struct platform_device *pdev)
 		goto pinctrl_register_error;
 	}
 
-	ret = gpiochip_add(atmel_pioctrl->gpio_chip);
+	ret = gpiochip_add_data(atmel_pioctrl->gpio_chip, atmel_pioctrl);
 	if (ret) {
 		dev_err(dev, "failed to add gpiochip\n");
 		goto gpiochip_add_error;
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index 0d2fc0cff35e..523b6b794d1f 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -45,8 +45,6 @@ struct at91_gpio_chip {
 	struct at91_pinctrl_mux_ops *ops;	/* ops */
 };
 
-#define to_at91_gpio_chip(c) container_of(c, struct at91_gpio_chip, chip)
-
 static struct at91_gpio_chip *gpio_chips[MAX_GPIO_BANKS];
 
 static int gpio_banks;
@@ -811,7 +809,7 @@ static int at91_gpio_request_enable(struct pinctrl_dev *pctldev,
 		return -EINVAL;
 	}
 	chip = range->gc;
-	at91_chip = container_of(chip, struct at91_gpio_chip, chip);
+	at91_chip = gpiochip_get_data(chip);
 
 	dev_dbg(npct->dev, "enable pin %u as GPIO\n", offset);
 
@@ -1282,7 +1280,7 @@ static int at91_pinctrl_remove(struct platform_device *pdev)
 
 static int at91_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 	unsigned mask = 1 << offset;
 	u32 osr;
@@ -1293,7 +1291,7 @@ static int at91_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 
 static int at91_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 	unsigned mask = 1 << offset;
 
@@ -1303,7 +1301,7 @@ static int at91_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 
 static int at91_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 	unsigned mask = 1 << offset;
 	u32 pdsr;
@@ -1315,7 +1313,7 @@ static int at91_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void at91_gpio_set(struct gpio_chip *chip, unsigned offset,
 				int val)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 	unsigned mask = 1 << offset;
 
@@ -1325,7 +1323,7 @@ static void at91_gpio_set(struct gpio_chip *chip, unsigned offset,
 static void at91_gpio_set_multiple(struct gpio_chip *chip,
 				      unsigned long *mask, unsigned long *bits)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 
 #define BITS_MASK(bits) (((bits) == 32) ? ~0U : (BIT(bits) - 1))
@@ -1340,7 +1338,7 @@ static void at91_gpio_set_multiple(struct gpio_chip *chip,
 static int at91_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				int val)
 {
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 	unsigned mask = 1 << offset;
 
@@ -1355,7 +1353,7 @@ static void at91_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
 	enum at91_mux mode;
 	int i;
-	struct at91_gpio_chip *at91_gpio = to_at91_gpio_chip(chip);
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(chip);
 	void __iomem *pio = at91_gpio->regbase;
 
 	for (i = 0; i < chip->ngpio; i++) {
@@ -1570,9 +1568,7 @@ static void gpio_irq_handler(struct irq_desc *desc)
 {
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	struct gpio_chip *gpio_chip = irq_desc_get_handler_data(desc);
-	struct at91_gpio_chip *at91_gpio = container_of(gpio_chip,
-					   struct at91_gpio_chip, chip);
-
+	struct at91_gpio_chip *at91_gpio = gpiochip_get_data(gpio_chip);
 	void __iomem	*pio = at91_gpio->regbase;
 	unsigned long	isr;
 	int		n;
@@ -1648,7 +1644,7 @@ static int at91_gpio_of_irq_setup(struct platform_device *pdev,
 		return 0;
 	}
 
-	prev = container_of(gpiochip_prev, struct at91_gpio_chip, chip);
+	prev = gpiochip_get_data(gpiochip_prev);
 
 	/* we can only have 2 banks before */
 	for (i = 0; i < 2; i++) {
@@ -1750,7 +1746,7 @@ static int at91_gpio_probe(struct platform_device *pdev)
 	chip = &at91_chip->chip;
 	chip->of_node = np;
 	chip->label = dev_name(&pdev->dev);
-	chip->dev = &pdev->dev;
+	chip->parent = &pdev->dev;
 	chip->owner = THIS_MODULE;
 	chip->base = alias_idx * MAX_NB_GPIO_PER_BANK;
 
@@ -1783,7 +1779,7 @@ static int at91_gpio_probe(struct platform_device *pdev)
 	range->npins = chip->ngpio;
 	range->gc = chip;
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, at91_chip);
 	if (ret)
 		goto gpiochip_add_err;
 
@@ -1828,20 +1824,20 @@ static struct platform_driver at91_pinctrl_driver = {
 	.remove = at91_pinctrl_remove,
 };
 
+static struct platform_driver * const drivers[] = {
+	&at91_gpio_driver,
+	&at91_pinctrl_driver,
+};
+
 static int __init at91_pinctrl_init(void)
 {
-	int ret;
-
-	ret = platform_driver_register(&at91_gpio_driver);
-	if (ret)
-		return ret;
-	return platform_driver_register(&at91_pinctrl_driver);
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 arch_initcall(at91_pinctrl_init);
 
 static void __exit at91_pinctrl_exit(void)
 {
-	platform_driver_unregister(&at91_pinctrl_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 
 module_exit(at91_pinctrl_exit);
diff --git a/drivers/pinctrl/pinctrl-coh901.c b/drivers/pinctrl/pinctrl-coh901.c
index 813eb7c771ec..cf7788df0f95 100644
--- a/drivers/pinctrl/pinctrl-coh901.c
+++ b/drivers/pinctrl/pinctrl-coh901.c
@@ -208,25 +208,16 @@ bs335_gpio_config[U300_GPIO_NUM_PORTS][U300_GPIO_PINS_PER_PORT] = {
 	}
 };
 
-/**
- * to_u300_gpio() - get the pointer to u300_gpio
- * @chip: the gpio chip member of the structure u300_gpio
- */
-static inline struct u300_gpio *to_u300_gpio(struct gpio_chip *chip)
-{
-	return container_of(chip, struct u300_gpio, chip);
-}
-
 static int u300_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 
-	return readl(U300_PIN_REG(offset, dir)) & U300_PIN_BIT(offset);
+	return !!(readl(U300_PIN_REG(offset, dir)) & U300_PIN_BIT(offset));
 }
 
 static void u300_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -243,7 +234,7 @@ static void u300_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
 static int u300_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -259,7 +250,7 @@ static int u300_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int u300_gpio_direction_output(struct gpio_chip *chip, unsigned offset,
 				      int value)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 oldmode;
 	u32 val;
@@ -290,7 +281,7 @@ int u300_gpio_config_get(struct gpio_chip *chip,
 			 unsigned offset,
 			 unsigned long *config)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	enum pin_config_param param = (enum pin_config_param) *config;
 	bool biasmode;
 	u32 drmode;
@@ -348,7 +339,7 @@ int u300_gpio_config_get(struct gpio_chip *chip,
 int u300_gpio_config_set(struct gpio_chip *chip, unsigned offset,
 			 enum pin_config_param param)
 {
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -429,7 +420,7 @@ static void u300_toggle_trigger(struct u300_gpio *gpio, unsigned offset)
 static int u300_gpio_irq_type(struct irq_data *d, unsigned trigger)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	struct u300_gpio_port *port = &gpio->ports[d->hwirq >> 3];
 	int offset = d->hwirq;
 	u32 val;
@@ -466,7 +457,7 @@ static int u300_gpio_irq_type(struct irq_data *d, unsigned trigger)
 static void u300_gpio_irq_enable(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	struct u300_gpio_port *port = &gpio->ports[d->hwirq >> 3];
 	int offset = d->hwirq;
 	u32 val;
@@ -483,7 +474,7 @@ static void u300_gpio_irq_enable(struct irq_data *d)
 static void u300_gpio_irq_disable(struct irq_data *d)
 {
 	struct gpio_chip *chip = irq_data_get_irq_chip_data(d);
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	int offset = d->hwirq;
 	u32 val;
 	unsigned long flags;
@@ -506,7 +497,7 @@ static void u300_gpio_irq_handler(struct irq_desc *desc)
 	unsigned int irq = irq_desc_get_irq(desc);
 	struct irq_chip *parent_chip = irq_desc_get_chip(desc);
 	struct gpio_chip *chip = irq_desc_get_handler_data(desc);
-	struct u300_gpio *gpio = to_u300_gpio(chip);
+	struct u300_gpio *gpio = gpiochip_get_data(chip);
 	struct u300_gpio_port *port = &gpio->ports[irq - chip->base];
 	int pinoffset = port->number << 3; /* get the right stride */
 	unsigned long val;
@@ -637,7 +628,7 @@ static int __init u300_gpio_probe(struct platform_device *pdev)
 
 	gpio->chip = u300_gpio_chip;
 	gpio->chip.ngpio = U300_GPIO_NUM_PORTS * U300_GPIO_PINS_PER_PORT;
-	gpio->chip.dev = &pdev->dev;
+	gpio->chip.parent = &pdev->dev;
 	gpio->chip.base = 0;
 	gpio->dev = &pdev->dev;
 
@@ -684,7 +675,7 @@ static int __init u300_gpio_probe(struct platform_device *pdev)
 #ifdef CONFIG_OF_GPIO
 	gpio->chip.of_node = pdev->dev.of_node;
 #endif
-	err = gpiochip_add(&gpio->chip);
+	err = gpiochip_add_data(&gpio->chip, gpio);
 	if (err) {
 		dev_err(gpio->dev, "unable to add gpiochip: %d\n", err);
 		goto err_no_chip;
diff --git a/drivers/pinctrl/pinctrl-digicolor.c b/drivers/pinctrl/pinctrl-digicolor.c
index 38a7799f8257..f1343d6ca823 100644
--- a/drivers/pinctrl/pinctrl-digicolor.c
+++ b/drivers/pinctrl/pinctrl-digicolor.c
@@ -171,7 +171,7 @@ static struct pinmux_ops dc_pmxops = {
 
 static int dc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct dc_pinmap *pmap = container_of(chip, struct dc_pinmap, chip);
+	struct dc_pinmap *pmap = gpiochip_get_data(chip);
 	int reg_off = GP_DRIVE0(gpio/PINS_PER_COLLECTION);
 	int bit_off = gpio % PINS_PER_COLLECTION;
 	u8 drive;
@@ -191,7 +191,7 @@ static void dc_gpio_set(struct gpio_chip *chip, unsigned gpio, int value);
 static int dc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 				    int value)
 {
-	struct dc_pinmap *pmap = container_of(chip, struct dc_pinmap, chip);
+	struct dc_pinmap *pmap = gpiochip_get_data(chip);
 	int reg_off = GP_DRIVE0(gpio/PINS_PER_COLLECTION);
 	int bit_off = gpio % PINS_PER_COLLECTION;
 	u8 drive;
@@ -210,7 +210,7 @@ static int dc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio,
 
 static int dc_gpio_get(struct gpio_chip *chip, unsigned gpio)
 {
-	struct dc_pinmap *pmap = container_of(chip, struct dc_pinmap, chip);
+	struct dc_pinmap *pmap = gpiochip_get_data(chip);
 	int reg_off = GP_INPUT(gpio/PINS_PER_COLLECTION);
 	int bit_off = gpio % PINS_PER_COLLECTION;
 	u8 input;
@@ -222,7 +222,7 @@ static int dc_gpio_get(struct gpio_chip *chip, unsigned gpio)
 
 static void dc_gpio_set(struct gpio_chip *chip, unsigned gpio, int value)
 {
-	struct dc_pinmap *pmap = container_of(chip, struct dc_pinmap, chip);
+	struct dc_pinmap *pmap = gpiochip_get_data(chip);
 	int reg_off = GP_OUTPUT0(gpio/PINS_PER_COLLECTION);
 	int bit_off = gpio % PINS_PER_COLLECTION;
 	u8 output;
@@ -244,7 +244,7 @@ static int dc_gpiochip_add(struct dc_pinmap *pmap, struct device_node *np)
 	int ret;
 
 	chip->label		= DRIVER_NAME;
-	chip->dev		= pmap->dev;
+	chip->parent		= pmap->dev;
 	chip->request		= gpiochip_generic_request;
 	chip->free		= gpiochip_generic_free;
 	chip->direction_input	= dc_gpio_direction_input;
@@ -258,7 +258,7 @@ static int dc_gpiochip_add(struct dc_pinmap *pmap, struct device_node *np)
 
 	spin_lock_init(&pmap->lock);
 
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, pmap);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/pinctrl/pinctrl-lantiq.h b/drivers/pinctrl/pinctrl-lantiq.h
index eb89ba045228..e137d139e494 100644
--- a/drivers/pinctrl/pinctrl-lantiq.h
+++ b/drivers/pinctrl/pinctrl-lantiq.h
@@ -162,6 +162,14 @@ enum ltq_pin {
 	GPIO53,
 	GPIO54,
 	GPIO55,
+	GPIO56,
+	GPIO57,
+	GPIO58,
+	GPIO59,
+	GPIO60, /* 60 */
+	GPIO61,
+	GPIO62,
+	GPIO63,
 
 	GPIO64,
 	GPIO65,
diff --git a/drivers/pinctrl/pinctrl-pistachio.c b/drivers/pinctrl/pinctrl-pistachio.c
index 85c9046c690e..856f736cb1a6 100644
--- a/drivers/pinctrl/pinctrl-pistachio.c
+++ b/drivers/pinctrl/pinctrl-pistachio.c
@@ -842,14 +842,9 @@ static inline void pctl_writel(struct pistachio_pinctrl *pctl, u32 val, u32 reg)
 	writel(val, pctl->base + reg);
 }
 
-static inline struct pistachio_gpio_bank *gc_to_bank(struct gpio_chip *gc)
-{
-	return container_of(gc, struct pistachio_gpio_bank, gpio_chip);
-}
-
 static inline struct pistachio_gpio_bank *irqd_to_bank(struct irq_data *d)
 {
-	return gc_to_bank(irq_data_get_irq_chip_data(d));
+	return gpiochip_get_data(irq_data_get_irq_chip_data(d));
 }
 
 static inline u32 gpio_readl(struct pistachio_gpio_bank *bank, u32 reg)
@@ -992,7 +987,7 @@ static int pistachio_pinmux_enable(struct pinctrl_dev *pctldev,
 
 	range = pinctrl_find_gpio_range_from_pin(pctl->pctldev, pg->pin);
 	if (range)
-		gpio_disable(gc_to_bank(range->gc), pg->pin - range->pin_base);
+		gpio_disable(gpiochip_get_data(range->gc), pg->pin - range->pin_base);
 
 	return 0;
 }
@@ -1173,14 +1168,14 @@ static struct pinctrl_desc pistachio_pinctrl_desc = {
 
 static int pistachio_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(chip);
 
 	return !(gpio_readl(bank, GPIO_OUTPUT_EN) & BIT(offset));
 }
 
 static int pistachio_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(chip);
 	u32 reg;
 
 	if (gpio_readl(bank, GPIO_OUTPUT_EN) & BIT(offset))
@@ -1194,7 +1189,7 @@ static int pistachio_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void pistachio_gpio_set(struct gpio_chip *chip, unsigned offset,
 			       int value)
 {
-	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(chip);
 
 	gpio_mask_writel(bank, GPIO_OUTPUT, offset, !!value);
 }
@@ -1202,7 +1197,7 @@ static void pistachio_gpio_set(struct gpio_chip *chip, unsigned offset,
 static int pistachio_gpio_direction_input(struct gpio_chip *chip,
 					  unsigned offset)
 {
-	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(chip);
 
 	gpio_mask_writel(bank, GPIO_OUTPUT_EN, offset, 0);
 	gpio_enable(bank, offset);
@@ -1213,7 +1208,7 @@ static int pistachio_gpio_direction_input(struct gpio_chip *chip,
 static int pistachio_gpio_direction_output(struct gpio_chip *chip,
 					   unsigned offset, int value)
 {
-	struct pistachio_gpio_bank *bank = gc_to_bank(chip);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(chip);
 
 	pistachio_gpio_set(chip, offset, value);
 	gpio_mask_writel(bank, GPIO_OUTPUT_EN, offset, 1);
@@ -1303,7 +1298,7 @@ static int pistachio_gpio_irq_set_type(struct irq_data *data, unsigned int type)
 static void pistachio_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct pistachio_gpio_bank *bank = gc_to_bank(gc);
+	struct pistachio_gpio_bank *bank = gpiochip_get_data(gc);
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	unsigned long pending;
 	unsigned int pin;
@@ -1388,9 +1383,9 @@ static int pistachio_gpio_register(struct pistachio_pinctrl *pctl)
 		bank->pctl = pctl;
 		bank->base = pctl->base + GPIO_BANK_BASE(i);
 
-		bank->gpio_chip.dev = pctl->dev;
+		bank->gpio_chip.parent = pctl->dev;
 		bank->gpio_chip.of_node = child;
-		ret = gpiochip_add(&bank->gpio_chip);
+		ret = gpiochip_add_data(&bank->gpio_chip, bank);
 		if (ret < 0) {
 			dev_err(pctl->dev, "Failed to add GPIO chip %u: %d\n",
 				i, ret);
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index a0651128e23a..183545a068ad 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -224,11 +224,6 @@ static struct regmap_config rockchip_regmap_config = {
 	.reg_stride = 4,
 };
 
-static inline struct rockchip_pin_bank *gc_to_pin_bank(struct gpio_chip *gc)
-{
-	return container_of(gc, struct rockchip_pin_bank, gpio_chip);
-}
-
 static const inline struct rockchip_pin_group *pinctrl_name_to_group(
 					const struct rockchip_pinctrl *info,
 					const char *name)
@@ -614,6 +609,40 @@ static void rk3288_calc_drv_reg_and_bit(struct rockchip_pin_bank *bank,
 	}
 }
 
+#define RK3228_PULL_OFFSET		0x100
+
+static void rk3228_calc_pull_reg_and_bit(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+
+	*regmap = info->regmap_base;
+	*reg = RK3228_PULL_OFFSET;
+	*reg += bank->bank_num * RK3188_PULL_BANK_STRIDE;
+	*reg += ((pin_num / RK3188_PULL_PINS_PER_REG) * 4);
+
+	*bit = (pin_num % RK3188_PULL_PINS_PER_REG);
+	*bit *= RK3188_PULL_BITS_PER_PIN;
+}
+
+#define RK3228_DRV_GRF_OFFSET		0x200
+
+static void rk3228_calc_drv_reg_and_bit(struct rockchip_pin_bank *bank,
+				    int pin_num, struct regmap **regmap,
+				    int *reg, u8 *bit)
+{
+	struct rockchip_pinctrl *info = bank->drvdata;
+
+	*regmap = info->regmap_base;
+	*reg = RK3228_DRV_GRF_OFFSET;
+	*reg += bank->bank_num * RK3288_DRV_BANK_STRIDE;
+	*reg += ((pin_num / RK3288_DRV_PINS_PER_REG) * 4);
+
+	*bit = (pin_num % RK3288_DRV_PINS_PER_REG);
+	*bit *= RK3288_DRV_BITS_PER_PIN;
+}
+
 #define RK3368_PULL_GRF_OFFSET		0x100
 #define RK3368_PULL_PMU_OFFSET		0x10
 
@@ -939,7 +968,7 @@ static int _rockchip_pmx_gpio_set_direction(struct gpio_chip *chip,
 	unsigned long flags;
 	u32 data;
 
-	bank = gc_to_pin_bank(chip);
+	bank = gpiochip_get_data(chip);
 
 	ret = rockchip_set_mux(bank, pin, RK_FUNC_GPIO);
 	if (ret < 0)
@@ -1258,8 +1287,10 @@ static int rockchip_pinctrl_parse_functions(struct device_node *np,
 		func->groups[i] = child->name;
 		grp = &info->groups[grp_index++];
 		ret = rockchip_pinctrl_parse_groups(child, grp, info, i++);
-		if (ret)
+		if (ret) {
+			of_node_put(child);
 			return ret;
+		}
 	}
 
 	return 0;
@@ -1304,6 +1335,7 @@ static int rockchip_pinctrl_parse_dt(struct platform_device *pdev,
 		ret = rockchip_pinctrl_parse_functions(child, info, i++);
 		if (ret) {
 			dev_err(&pdev->dev, "failed to parse function\n");
+			of_node_put(child);
 			return ret;
 		}
 	}
@@ -1376,7 +1408,7 @@ static int rockchip_pinctrl_register(struct platform_device *pdev,
 
 static void rockchip_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct rockchip_pin_bank *bank = gc_to_pin_bank(gc);
+	struct rockchip_pin_bank *bank = gpiochip_get_data(gc);
 	void __iomem *reg = bank->reg_base + GPIO_SWPORT_DR;
 	unsigned long flags;
 	u32 data;
@@ -1400,7 +1432,7 @@ static void rockchip_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
  */
 static int rockchip_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct rockchip_pin_bank *bank = gc_to_pin_bank(gc);
+	struct rockchip_pin_bank *bank = gpiochip_get_data(gc);
 	u32 data;
 
 	clk_enable(bank->clk);
@@ -1439,7 +1471,7 @@ static int rockchip_gpio_direction_output(struct gpio_chip *gc,
  */
 static int rockchip_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct rockchip_pin_bank *bank = gc_to_pin_bank(gc);
+	struct rockchip_pin_bank *bank = gpiochip_get_data(gc);
 	unsigned int virq;
 
 	if (!bank->domain)
@@ -1754,11 +1786,11 @@ static int rockchip_gpiolib_register(struct platform_device *pdev,
 		gc = &bank->gpio_chip;
 		gc->base = bank->pin_base;
 		gc->ngpio = bank->nr_pins;
-		gc->dev = &pdev->dev;
+		gc->parent = &pdev->dev;
 		gc->of_node = bank->of_node;
 		gc->label = bank->name;
 
-		ret = gpiochip_add(gc);
+		ret = gpiochip_add_data(gc, bank);
 		if (ret) {
 			dev_err(&pdev->dev, "failed to register gpio_chip %s, error code: %d\n",
 							gc->label, ret);
@@ -2143,6 +2175,23 @@ static struct rockchip_pin_ctrl rk3188_pin_ctrl = {
 		.pull_calc_reg		= rk3188_calc_pull_reg_and_bit,
 };
 
+static struct rockchip_pin_bank rk3228_pin_banks[] = {
+	PIN_BANK(0, 32, "gpio0"),
+	PIN_BANK(1, 32, "gpio1"),
+	PIN_BANK(2, 32, "gpio2"),
+	PIN_BANK(3, 32, "gpio3"),
+};
+
+static struct rockchip_pin_ctrl rk3228_pin_ctrl = {
+		.pin_banks		= rk3228_pin_banks,
+		.nr_banks		= ARRAY_SIZE(rk3228_pin_banks),
+		.label			= "RK3228-GPIO",
+		.type			= RK3288,
+		.grf_mux_offset		= 0x0,
+		.pull_calc_reg		= rk3228_calc_pull_reg_and_bit,
+		.drv_calc_reg		= rk3228_calc_drv_reg_and_bit,
+};
+
 static struct rockchip_pin_bank rk3288_pin_banks[] = {
 	PIN_BANK_IOMUX_FLAGS(0, 24, "gpio0", IOMUX_SOURCE_PMU,
 					     IOMUX_SOURCE_PMU,
@@ -2220,6 +2269,8 @@ static const struct of_device_id rockchip_pinctrl_dt_match[] = {
 		.data = (void *)&rk3066b_pin_ctrl },
 	{ .compatible = "rockchip,rk3188-pinctrl",
 		.data = (void *)&rk3188_pin_ctrl },
+	{ .compatible = "rockchip,rk3228-pinctrl",
+		.data = (void *)&rk3228_pin_ctrl },
 	{ .compatible = "rockchip,rk3288-pinctrl",
 		.data = (void *)&rk3288_pin_ctrl },
 	{ .compatible = "rockchip,rk3368-pinctrl",
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index ef04b962c3d5..d24e5f1d1525 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -1484,10 +1484,7 @@ static void pcs_irq_free(struct pcs_device *pcs)
 static void pcs_free_resources(struct pcs_device *pcs)
 {
 	pcs_irq_free(pcs);
-
-	if (pcs->pctl)
-		pinctrl_unregister(pcs->pctl);
-
+	pinctrl_unregister(pcs->pctl);
 	pcs_free_funcs(pcs);
 	pcs_free_pingroups(pcs);
 }
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index b58d3f29148a..fac844a85cb4 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -203,9 +203,6 @@
 #define gpio_range_to_bank(chip) \
 		container_of(chip, struct st_gpio_bank, range)
 
-#define gpio_chip_to_bank(chip) \
-		container_of(chip, struct st_gpio_bank, gpio_chip)
-
 #define pc_to_bank(pc) \
 		container_of(pc, struct st_gpio_bank, pc)
 
@@ -744,14 +741,14 @@ static void st_gpio_direction(struct st_gpio_bank *bank,
 
 static int st_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
+	struct st_gpio_bank *bank = gpiochip_get_data(chip);
 
 	return !!(readl(bank->base + REG_PIO_PIN) & BIT(offset));
 }
 
 static void st_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
+	struct st_gpio_bank *bank = gpiochip_get_data(chip);
 	__st_gpio_set(bank, offset, value);
 }
 
@@ -765,7 +762,7 @@ static int st_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int st_gpio_direction_output(struct gpio_chip *chip,
 	unsigned offset, int value)
 {
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
+	struct st_gpio_bank *bank = gpiochip_get_data(chip);
 
 	__st_gpio_set(bank, offset, value);
 	pinctrl_gpio_direction_output(chip->base + offset);
@@ -775,7 +772,7 @@ static int st_gpio_direction_output(struct gpio_chip *chip,
 
 static int st_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct st_gpio_bank *bank = gpio_chip_to_bank(chip);
+	struct st_gpio_bank *bank = gpiochip_get_data(chip);
 	struct st_pio_control pc = bank->pc;
 	unsigned long config;
 	unsigned int direction = 0;
@@ -1325,7 +1322,7 @@ static int st_pctl_parse_functions(struct device_node *np,
 static void st_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
+	struct st_gpio_bank *bank = gpiochip_get_data(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_CLR_PMASK);
 }
@@ -1333,7 +1330,7 @@ static void st_gpio_irq_mask(struct irq_data *d)
 static void st_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
+	struct st_gpio_bank *bank = gpiochip_get_data(gc);
 
 	writel(BIT(d->hwirq), bank->base + REG_PIO_SET_PMASK);
 }
@@ -1341,7 +1338,7 @@ static void st_gpio_irq_unmask(struct irq_data *d)
 static int st_gpio_irq_set_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
+	struct st_gpio_bank *bank = gpiochip_get_data(gc);
 	unsigned long flags;
 	int comp, pin = d->hwirq;
 	u32 val;
@@ -1455,7 +1452,7 @@ static void st_gpio_irq_handler(struct irq_desc *desc)
 	/* interrupt dedicated per bank */
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct st_gpio_bank *bank = gpio_chip_to_bank(gc);
+	struct st_gpio_bank *bank = gpiochip_get_data(gc);
 
 	chained_irq_enter(chip, desc);
 	__gpio_irq_handler(bank);
@@ -1522,7 +1519,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	bank->gpio_chip.base = bank_num * ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.ngpio = ST_GPIO_PINS_PER_BANK;
 	bank->gpio_chip.of_node = np;
-	bank->gpio_chip.dev = dev;
+	bank->gpio_chip.parent = dev;
 	spin_lock_init(&bank->lock);
 
 	of_property_read_string(np, "st,bank-name", &range->name);
@@ -1532,7 +1529,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 	range->pin_base = range->base = range->id * ST_GPIO_PINS_PER_BANK;
 	range->npins = bank->gpio_chip.ngpio;
 	range->gc = &bank->gpio_chip;
-	err  = gpiochip_add(&bank->gpio_chip);
+	err  = gpiochip_add_data(&bank->gpio_chip, bank);
 	if (err) {
 		dev_err(dev, "Failed to add gpiochip(%d)!\n", bank_num);
 		return err;
diff --git a/drivers/pinctrl/pinctrl-tegra-xusb.c b/drivers/pinctrl/pinctrl-tegra-xusb.c
index 84a43e612952..bd3aa5a4fd6d 100644
--- a/drivers/pinctrl/pinctrl-tegra-xusb.c
+++ b/drivers/pinctrl/pinctrl-tegra-xusb.c
@@ -253,8 +253,10 @@ static int tegra_xusb_padctl_dt_node_to_map(struct pinctrl_dev *pinctrl,
 		err = tegra_xusb_padctl_parse_subnode(padctl, np, maps,
 						      &reserved_maps,
 						      num_maps);
-		if (err < 0)
+		if (err < 0) {
+			of_node_put(np);
 			return err;
+		}
 	}
 
 	return 0;
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 0fd7fd2b0f72..9da4da219a07 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -217,6 +217,7 @@ static int tegra_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 		if (ret < 0) {
 			pinctrl_utils_dt_free_map(pctldev, *map,
 				*num_maps);
+			of_node_put(np);
 			return ret;
 		}
 	}
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index ae724bdab3d3..412c6b78140a 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -7,6 +7,7 @@
  *  publishhed by the Free Software Foundation.
  *
  *  Copyright (C) 2012 John Crispin <blogic@openwrt.org>
+ *  Copyright (C) 2015 Martin Schiller <mschiller@tdt.de>
  */
 
 #include <linux/err.h>
@@ -24,7 +25,7 @@
 
 #include <lantiq_soc.h>
 
-/* we have 3 1/2 banks of 16 bit each */
+/* we have up to 4 banks of 16 bit each */
 #define PINS			16
 #define PORT3			3
 #define PORT(x)			(x / PINS)
@@ -35,7 +36,7 @@
 #define MUX_ALT1	0x2
 
 /*
- * each bank has this offset apart from the 1/2 bank that is mixed into the
+ * each bank has this offset apart from the 4th bank that is mixed into the
  * other 3 ranges
  */
 #define REG_OFF			0x30
@@ -51,7 +52,7 @@
 #define GPIO_PUDSEL(p)		(GPIO_BASE(p) + 0x1c)
 #define GPIO_PUDEN(p)		(GPIO_BASE(p) + 0x20)
 
-/* the 1/2 port needs special offsets for some registers */
+/* the 4th port needs special offsets for some registers */
 #define GPIO3_OD		(GPIO_BASE(0) + 0x24)
 #define GPIO3_PUDSEL		(GPIO_BASE(0) + 0x28)
 #define GPIO3_PUDEN		(GPIO_BASE(0) + 0x2C)
@@ -80,17 +81,18 @@
 #define FUNC_MUX(f, m)		\
 	{ .func = f, .mux = XWAY_MUX_##m, }
 
-#define XWAY_MAX_PIN		32
-#define XR9_MAX_PIN		56
-
 enum xway_mux {
 	XWAY_MUX_GPIO = 0,
 	XWAY_MUX_SPI,
 	XWAY_MUX_ASC,
+	XWAY_MUX_USIF,
 	XWAY_MUX_PCI,
+	XWAY_MUX_CBUS,
 	XWAY_MUX_CGU,
 	XWAY_MUX_EBU,
+	XWAY_MUX_EBU2,
 	XWAY_MUX_JTAG,
+	XWAY_MUX_MCD,
 	XWAY_MUX_EXIN,
 	XWAY_MUX_TDM,
 	XWAY_MUX_STP,
@@ -103,9 +105,15 @@ enum xway_mux {
 	XWAY_MUX_DFE,
 	XWAY_MUX_SDIO,
 	XWAY_MUX_GPHY,
+	XWAY_MUX_SSI,
+	XWAY_MUX_WIFI,
 	XWAY_MUX_NONE = 0xffff,
 };
 
+/* ---------  DEPRECATED: xr9 related code --------- */
+/* ----------  use xrx100/xrx200 instead  ---------- */
+#define XR9_MAX_PIN		56
+
 static const struct ltq_mfp_pin xway_mfp[] = {
 	/*       pin    f0	f1	f2	f3   */
 	MFP_XWAY(GPIO0, GPIO,	EXIN,	NONE,	TDM),
@@ -113,7 +121,7 @@ static const struct ltq_mfp_pin xway_mfp[] = {
 	MFP_XWAY(GPIO2, GPIO,	CGU,	EXIN,	GPHY),
 	MFP_XWAY(GPIO3, GPIO,	CGU,	NONE,	PCI),
 	MFP_XWAY(GPIO4, GPIO,	STP,	NONE,	ASC),
-	MFP_XWAY(GPIO5, GPIO,	STP,	NONE,	GPHY),
+	MFP_XWAY(GPIO5, GPIO,	STP,	GPHY,	NONE),
 	MFP_XWAY(GPIO6, GPIO,	STP,	GPT,	ASC),
 	MFP_XWAY(GPIO7, GPIO,	CGU,	PCI,	GPHY),
 	MFP_XWAY(GPIO8, GPIO,	CGU,	NMI,	NONE),
@@ -152,10 +160,10 @@ static const struct ltq_mfp_pin xway_mfp[] = {
 	MFP_XWAY(GPIO41, GPIO,	NONE,	NONE,	NONE),
 	MFP_XWAY(GPIO42, GPIO,	MDIO,	NONE,	NONE),
 	MFP_XWAY(GPIO43, GPIO,	MDIO,	NONE,	NONE),
-	MFP_XWAY(GPIO44, GPIO,	NONE,	GPHY,	SIN),
+	MFP_XWAY(GPIO44, GPIO,	MII,	SIN,	GPHY),
 	MFP_XWAY(GPIO45, GPIO,	NONE,	GPHY,	SIN),
 	MFP_XWAY(GPIO46, GPIO,	NONE,	NONE,	EXIN),
-	MFP_XWAY(GPIO47, GPIO,	NONE,	GPHY,	SIN),
+	MFP_XWAY(GPIO47, GPIO,	MII,	GPHY,	SIN),
 	MFP_XWAY(GPIO48, GPIO,	EBU,	NONE,	NONE),
 	MFP_XWAY(GPIO49, GPIO,	EBU,	NONE,	NONE),
 	MFP_XWAY(GPIO50, GPIO,	NONE,	NONE,	NONE),
@@ -166,42 +174,6 @@ static const struct ltq_mfp_pin xway_mfp[] = {
 	MFP_XWAY(GPIO55, GPIO,	NONE,	NONE,	NONE),
 };
 
-static const struct ltq_mfp_pin ase_mfp[] = {
-	/*       pin    f0	f1	f2	f3   */
-	MFP_XWAY(GPIO0, GPIO,	EXIN,	MII,	TDM),
-	MFP_XWAY(GPIO1, GPIO,	STP,	DFE,	EBU),
-	MFP_XWAY(GPIO2, GPIO,	STP,	DFE,	EPHY),
-	MFP_XWAY(GPIO3, GPIO,	STP,	EPHY,	EBU),
-	MFP_XWAY(GPIO4, GPIO,	GPT,	EPHY,	MII),
-	MFP_XWAY(GPIO5, GPIO,	MII,	ASC,	GPT),
-	MFP_XWAY(GPIO6, GPIO,	MII,	ASC,	EXIN),
-	MFP_XWAY(GPIO7, GPIO,	SPI,	MII,	JTAG),
-	MFP_XWAY(GPIO8, GPIO,	SPI,	MII,	JTAG),
-	MFP_XWAY(GPIO9, GPIO,	SPI,	MII,	JTAG),
-	MFP_XWAY(GPIO10, GPIO,	SPI,	MII,	JTAG),
-	MFP_XWAY(GPIO11, GPIO,	EBU,	CGU,	JTAG),
-	MFP_XWAY(GPIO12, GPIO,	EBU,	MII,	SDIO),
-	MFP_XWAY(GPIO13, GPIO,	EBU,	MII,	CGU),
-	MFP_XWAY(GPIO14, GPIO,	EBU,	SPI,	CGU),
-	MFP_XWAY(GPIO15, GPIO,	EBU,	SPI,	SDIO),
-	MFP_XWAY(GPIO16, GPIO,	NONE,	NONE,	NONE),
-	MFP_XWAY(GPIO17, GPIO,	NONE,	NONE,	NONE),
-	MFP_XWAY(GPIO18, GPIO,	NONE,	NONE,	NONE),
-	MFP_XWAY(GPIO19, GPIO,	EBU,	MII,	SDIO),
-	MFP_XWAY(GPIO20, GPIO,	EBU,	MII,	SDIO),
-	MFP_XWAY(GPIO21, GPIO,	EBU,	MII,	SDIO),
-	MFP_XWAY(GPIO22, GPIO,	EBU,	MII,	CGU),
-	MFP_XWAY(GPIO23, GPIO,	EBU,	MII,	CGU),
-	MFP_XWAY(GPIO24, GPIO,	EBU,	NONE,	MII),
-	MFP_XWAY(GPIO25, GPIO,	EBU,	MII,	GPT),
-	MFP_XWAY(GPIO26, GPIO,	EBU,	MII,	SDIO),
-	MFP_XWAY(GPIO27, GPIO,	EBU,	NONE,	MII),
-	MFP_XWAY(GPIO28, GPIO,	MII,	EBU,	SDIO),
-	MFP_XWAY(GPIO29, GPIO,	EBU,	MII,	EXIN),
-	MFP_XWAY(GPIO30, GPIO,	NONE,	NONE,	NONE),
-	MFP_XWAY(GPIO31, GPIO,	NONE,	NONE,	NONE),
-};
-
 static const unsigned pins_jtag[] = {GPIO15, GPIO16, GPIO17, GPIO19, GPIO35};
 static const unsigned pins_asc0[] = {GPIO11, GPIO12};
 static const unsigned pins_asc0_cts_rts[] = {GPIO9, GPIO10};
@@ -231,6 +203,8 @@ static const unsigned pins_nand_cle[] = {GPIO24};
 static const unsigned pins_nand_rdy[] = {GPIO48};
 static const unsigned pins_nand_rd[] = {GPIO49};
 
+static const unsigned xway_exin_pin_map[] = {GPIO0, GPIO1, GPIO2, GPIO39, GPIO46, GPIO9};
+
 static const unsigned pins_exin0[] = {GPIO0};
 static const unsigned pins_exin1[] = {GPIO1};
 static const unsigned pins_exin2[] = {GPIO2};
@@ -240,7 +214,7 @@ static const unsigned pins_exin5[] = {GPIO9};
 
 static const unsigned pins_spi[] = {GPIO16, GPIO17, GPIO18};
 static const unsigned pins_spi_cs1[] = {GPIO15};
-static const unsigned pins_spi_cs2[] = {GPIO21};
+static const unsigned pins_spi_cs2[] = {GPIO22};
 static const unsigned pins_spi_cs3[] = {GPIO13};
 static const unsigned pins_spi_cs4[] = {GPIO10};
 static const unsigned pins_spi_cs5[] = {GPIO9};
@@ -264,25 +238,6 @@ static const unsigned pins_pci_req2[] = {GPIO31};
 static const unsigned pins_pci_req3[] = {GPIO3};
 static const unsigned pins_pci_req4[] = {GPIO37};
 
-static const unsigned ase_pins_jtag[] = {GPIO7, GPIO8, GPIO9, GPIO10, GPIO11};
-static const unsigned ase_pins_asc[] = {GPIO5, GPIO6};
-static const unsigned ase_pins_stp[] = {GPIO1, GPIO2, GPIO3};
-static const unsigned ase_pins_ephy[] = {GPIO2, GPIO3, GPIO4};
-static const unsigned ase_pins_dfe[] = {GPIO1, GPIO2};
-
-static const unsigned ase_pins_spi[] = {GPIO8, GPIO9, GPIO10};
-static const unsigned ase_pins_spi_cs1[] = {GPIO7};
-static const unsigned ase_pins_spi_cs2[] = {GPIO15};
-static const unsigned ase_pins_spi_cs3[] = {GPIO14};
-
-static const unsigned ase_pins_exin0[] = {GPIO6};
-static const unsigned ase_pins_exin1[] = {GPIO29};
-static const unsigned ase_pins_exin2[] = {GPIO0};
-
-static const unsigned ase_pins_gpt1[] = {GPIO5};
-static const unsigned ase_pins_gpt2[] = {GPIO4};
-static const unsigned ase_pins_gpt3[] = {GPIO25};
-
 static const struct ltq_pin_group xway_grps[] = {
 	GRP_MUX("exin0", EXIN, pins_exin0),
 	GRP_MUX("exin1", EXIN, pins_exin1),
@@ -338,24 +293,6 @@ static const struct ltq_pin_group xway_grps[] = {
 	GRP_MUX("gphy1 led2", GPHY, pins_gphy1_led2),
 };
 
-static const struct ltq_pin_group ase_grps[] = {
-	GRP_MUX("exin0", EXIN, ase_pins_exin0),
-	GRP_MUX("exin1", EXIN, ase_pins_exin1),
-	GRP_MUX("exin2", EXIN, ase_pins_exin2),
-	GRP_MUX("jtag", JTAG, ase_pins_jtag),
-	GRP_MUX("stp", STP, ase_pins_stp),
-	GRP_MUX("asc", ASC, ase_pins_asc),
-	GRP_MUX("gpt1", GPT, ase_pins_gpt1),
-	GRP_MUX("gpt2", GPT, ase_pins_gpt2),
-	GRP_MUX("gpt3", GPT, ase_pins_gpt3),
-	GRP_MUX("ephy", EPHY, ase_pins_ephy),
-	GRP_MUX("dfe", DFE, ase_pins_dfe),
-	GRP_MUX("spi", SPI, ase_pins_spi),
-	GRP_MUX("spi_cs1", SPI, ase_pins_spi_cs1),
-	GRP_MUX("spi_cs2", SPI, ase_pins_spi_cs2),
-	GRP_MUX("spi_cs3", SPI, ase_pins_spi_cs3),
-};
-
 static const char * const xway_pci_grps[] = {"gnt1", "gnt2",
 						"gnt3", "req1",
 						"req2", "req3"};
@@ -395,30 +332,6 @@ static const char * const xrx_pci_grps[] = {"gnt1", "gnt2",
 						"req1", "req2",
 						"req3", "req4"};
 
-/* ase */
-static const char * const ase_exin_grps[] = {"exin0", "exin1", "exin2"};
-static const char * const ase_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
-static const char * const ase_dfe_grps[] = {"dfe"};
-static const char * const ase_ephy_grps[] = {"ephy"};
-static const char * const ase_asc_grps[] = {"asc"};
-static const char * const ase_jtag_grps[] = {"jtag"};
-static const char * const ase_stp_grps[] = {"stp"};
-static const char * const ase_spi_grps[] = {"spi", "spi_cs1",
-						"spi_cs2", "spi_cs3"};
-
-static const struct ltq_pmx_func danube_funcs[] = {
-	{"spi",		ARRAY_AND_SIZE(xway_spi_grps)},
-	{"asc",		ARRAY_AND_SIZE(xway_asc_grps)},
-	{"cgu",		ARRAY_AND_SIZE(xway_cgu_grps)},
-	{"jtag",	ARRAY_AND_SIZE(xway_jtag_grps)},
-	{"exin",	ARRAY_AND_SIZE(xway_exin_grps)},
-	{"stp",		ARRAY_AND_SIZE(xway_stp_grps)},
-	{"gpt",		ARRAY_AND_SIZE(xway_gpt_grps)},
-	{"nmi",		ARRAY_AND_SIZE(xway_nmi_grps)},
-	{"pci",		ARRAY_AND_SIZE(xway_pci_grps)},
-	{"ebu",		ARRAY_AND_SIZE(xway_ebu_grps)},
-};
-
 static const struct ltq_pmx_func xrx_funcs[] = {
 	{"spi",		ARRAY_AND_SIZE(xway_spi_grps)},
 	{"asc",		ARRAY_AND_SIZE(xway_asc_grps)},
@@ -434,17 +347,991 @@ static const struct ltq_pmx_func xrx_funcs[] = {
 	{"gphy",	ARRAY_AND_SIZE(xrx_gphy_grps)},
 };
 
+/* ---------  ase related code --------- */
+#define ASE_MAX_PIN		32
+
+static const struct ltq_mfp_pin ase_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	MII,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	STP,	DFE,	EBU),
+	MFP_XWAY(GPIO2, GPIO,	STP,	DFE,	EPHY),
+	MFP_XWAY(GPIO3, GPIO,	STP,	EPHY,	EBU),
+	MFP_XWAY(GPIO4, GPIO,	GPT,	EPHY,	MII),
+	MFP_XWAY(GPIO5, GPIO,	MII,	ASC,	GPT),
+	MFP_XWAY(GPIO6, GPIO,	MII,	ASC,	EXIN),
+	MFP_XWAY(GPIO7, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO8, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO9, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO10, GPIO,	SPI,	MII,	JTAG),
+	MFP_XWAY(GPIO11, GPIO,	EBU,	CGU,	JTAG),
+	MFP_XWAY(GPIO12, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO14, GPIO,	EBU,	SPI,	CGU),
+	MFP_XWAY(GPIO15, GPIO,	EBU,	SPI,	SDIO),
+	MFP_XWAY(GPIO16, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO17, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO18, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO19, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO20, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO21, GPIO,	EBU,	MII,	EBU2),
+	MFP_XWAY(GPIO22, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	MII,	CGU),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	EBU2,	MDIO),
+	MFP_XWAY(GPIO25, GPIO,	EBU,	MII,	GPT),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	MII,	SDIO),
+	MFP_XWAY(GPIO27, GPIO,	EBU,	NONE,	MDIO),
+	MFP_XWAY(GPIO28, GPIO,	MII,	EBU,	SDIO),
+	MFP_XWAY(GPIO29, GPIO,	EBU,	MII,	EXIN),
+	MFP_XWAY(GPIO30, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO31, GPIO,	NONE,	NONE,	NONE),
+};
+
+static const unsigned ase_exin_pin_map[] = {GPIO6, GPIO29, GPIO0};
+
+static const unsigned ase_pins_exin0[] = {GPIO6};
+static const unsigned ase_pins_exin1[] = {GPIO29};
+static const unsigned ase_pins_exin2[] = {GPIO0};
+
+static const unsigned ase_pins_jtag[] = {GPIO7, GPIO8, GPIO9, GPIO10, GPIO11};
+static const unsigned ase_pins_asc[] = {GPIO5, GPIO6};
+static const unsigned ase_pins_stp[] = {GPIO1, GPIO2, GPIO3};
+static const unsigned ase_pins_mdio[] = {GPIO24, GPIO27};
+static const unsigned ase_pins_ephy_led0[] = {GPIO2};
+static const unsigned ase_pins_ephy_led1[] = {GPIO3};
+static const unsigned ase_pins_ephy_led2[] = {GPIO4};
+static const unsigned ase_pins_dfe_led0[] = {GPIO1};
+static const unsigned ase_pins_dfe_led1[] = {GPIO2};
+
+static const unsigned ase_pins_spi[] = {GPIO8, GPIO9, GPIO10}; /* DEPRECATED */
+static const unsigned ase_pins_spi_di[] = {GPIO8};
+static const unsigned ase_pins_spi_do[] = {GPIO9};
+static const unsigned ase_pins_spi_clk[] = {GPIO10};
+static const unsigned ase_pins_spi_cs1[] = {GPIO7};
+static const unsigned ase_pins_spi_cs2[] = {GPIO15};
+static const unsigned ase_pins_spi_cs3[] = {GPIO14};
+
+static const unsigned ase_pins_gpt1[] = {GPIO5};
+static const unsigned ase_pins_gpt2[] = {GPIO4};
+static const unsigned ase_pins_gpt3[] = {GPIO25};
+
+static const unsigned ase_pins_clkout0[] = {GPIO23};
+static const unsigned ase_pins_clkout1[] = {GPIO22};
+static const unsigned ase_pins_clkout2[] = {GPIO14};
+
+static const struct ltq_pin_group ase_grps[] = {
+	GRP_MUX("exin0", EXIN, ase_pins_exin0),
+	GRP_MUX("exin1", EXIN, ase_pins_exin1),
+	GRP_MUX("exin2", EXIN, ase_pins_exin2),
+	GRP_MUX("jtag", JTAG, ase_pins_jtag),
+	GRP_MUX("spi", SPI, ase_pins_spi), /* DEPRECATED */
+	GRP_MUX("spi_di", SPI, ase_pins_spi_di),
+	GRP_MUX("spi_do", SPI, ase_pins_spi_do),
+	GRP_MUX("spi_clk", SPI, ase_pins_spi_clk),
+	GRP_MUX("spi_cs1", SPI, ase_pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, ase_pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, ase_pins_spi_cs3),
+	GRP_MUX("asc", ASC, ase_pins_asc),
+	GRP_MUX("stp", STP, ase_pins_stp),
+	GRP_MUX("gpt1", GPT, ase_pins_gpt1),
+	GRP_MUX("gpt2", GPT, ase_pins_gpt2),
+	GRP_MUX("gpt3", GPT, ase_pins_gpt3),
+	GRP_MUX("clkout0", CGU, ase_pins_clkout0),
+	GRP_MUX("clkout1", CGU, ase_pins_clkout1),
+	GRP_MUX("clkout2", CGU, ase_pins_clkout2),
+	GRP_MUX("mdio", MDIO, ase_pins_mdio),
+	GRP_MUX("dfe led0", DFE, ase_pins_dfe_led0),
+	GRP_MUX("dfe led1", DFE, ase_pins_dfe_led1),
+	GRP_MUX("ephy led0", EPHY, ase_pins_ephy_led0),
+	GRP_MUX("ephy led1", EPHY, ase_pins_ephy_led1),
+	GRP_MUX("ephy led2", EPHY, ase_pins_ephy_led2),
+};
+
+static const char * const ase_exin_grps[] = {"exin0", "exin1", "exin2"};
+static const char * const ase_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const ase_cgu_grps[] = {"clkout0", "clkout1",
+						"clkout2"};
+static const char * const ase_mdio_grps[] = {"mdio"};
+static const char * const ase_dfe_grps[] = {"dfe led0", "dfe led1"};
+static const char * const ase_ephy_grps[] = {"ephy led0", "ephy led1",
+						"ephy led2"};
+static const char * const ase_asc_grps[] = {"asc"};
+static const char * const ase_jtag_grps[] = {"jtag"};
+static const char * const ase_stp_grps[] = {"stp"};
+static const char * const ase_spi_grps[] = {"spi",  /* DEPRECATED */
+						"spi_di", "spi_do",
+						"spi_clk", "spi_cs1",
+						"spi_cs2", "spi_cs3"};
+
 static const struct ltq_pmx_func ase_funcs[] = {
 	{"spi",		ARRAY_AND_SIZE(ase_spi_grps)},
 	{"asc",		ARRAY_AND_SIZE(ase_asc_grps)},
+	{"cgu",		ARRAY_AND_SIZE(ase_cgu_grps)},
 	{"jtag",	ARRAY_AND_SIZE(ase_jtag_grps)},
 	{"exin",	ARRAY_AND_SIZE(ase_exin_grps)},
 	{"stp",		ARRAY_AND_SIZE(ase_stp_grps)},
 	{"gpt",		ARRAY_AND_SIZE(ase_gpt_grps)},
+	{"mdio",	ARRAY_AND_SIZE(ase_mdio_grps)},
 	{"ephy",	ARRAY_AND_SIZE(ase_ephy_grps)},
 	{"dfe",		ARRAY_AND_SIZE(ase_dfe_grps)},
 };
 
+/* ---------  danube related code --------- */
+#define DANUBE_MAX_PIN		32
+
+static const struct ltq_mfp_pin danube_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	SDIO,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	EXIN,	CBUS,	MII),
+	MFP_XWAY(GPIO2, GPIO,	CGU,	EXIN,	MII),
+	MFP_XWAY(GPIO3, GPIO,	CGU,	SDIO,	PCI),
+	MFP_XWAY(GPIO4, GPIO,	STP,	DFE,	ASC),
+	MFP_XWAY(GPIO5, GPIO,	STP,	MII,	DFE),
+	MFP_XWAY(GPIO6, GPIO,	STP,	GPT,	ASC),
+	MFP_XWAY(GPIO7, GPIO,	CGU,	CBUS,	MII),
+	MFP_XWAY(GPIO8, GPIO,	CGU,	NMI,	MII),
+	MFP_XWAY(GPIO9, GPIO,	ASC,	SPI,	MII),
+	MFP_XWAY(GPIO10, GPIO,	ASC,	SPI,	MII),
+	MFP_XWAY(GPIO11, GPIO,	ASC,	CBUS,	SPI),
+	MFP_XWAY(GPIO12, GPIO,	ASC,	CBUS,	MCD),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	SPI,	MII),
+	MFP_XWAY(GPIO14, GPIO,	CGU,	CBUS,	MII),
+	MFP_XWAY(GPIO15, GPIO,	SPI,	SDIO,	JTAG),
+	MFP_XWAY(GPIO16, GPIO,	SPI,	SDIO,	JTAG),
+	MFP_XWAY(GPIO17, GPIO,	SPI,	SDIO,	JTAG),
+	MFP_XWAY(GPIO18, GPIO,	SPI,	SDIO,	JTAG),
+	MFP_XWAY(GPIO19, GPIO,	PCI,	SDIO,	MII),
+	MFP_XWAY(GPIO20, GPIO,	JTAG,	SDIO,	MII),
+	MFP_XWAY(GPIO21, GPIO,	PCI,	EBU,	GPT),
+	MFP_XWAY(GPIO22, GPIO,	SPI,	MCD,	MII),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	PCI,	STP),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	TDM,	PCI),
+	MFP_XWAY(GPIO25, GPIO,	TDM,	SDIO,	ASC),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	TDM,	SDIO),
+	MFP_XWAY(GPIO27, GPIO,	TDM,	SDIO,	ASC),
+	MFP_XWAY(GPIO28, GPIO,	GPT,	MII,	SDIO),
+	MFP_XWAY(GPIO29, GPIO,	PCI,	CBUS,	MII),
+	MFP_XWAY(GPIO30, GPIO,	PCI,	CBUS,	MII),
+	MFP_XWAY(GPIO31, GPIO,	EBU,	PCI,	MII),
+};
+
+static const unsigned danube_exin_pin_map[] = {GPIO0, GPIO1, GPIO2};
+
+static const unsigned danube_pins_exin0[] = {GPIO0};
+static const unsigned danube_pins_exin1[] = {GPIO1};
+static const unsigned danube_pins_exin2[] = {GPIO2};
+
+static const unsigned danube_pins_jtag[] = {GPIO15, GPIO16, GPIO17, GPIO18, GPIO20};
+static const unsigned danube_pins_asc0[] = {GPIO11, GPIO12};
+static const unsigned danube_pins_asc0_cts_rts[] = {GPIO9, GPIO10};
+static const unsigned danube_pins_stp[] = {GPIO4, GPIO5, GPIO6};
+static const unsigned danube_pins_nmi[] = {GPIO8};
+
+static const unsigned danube_pins_dfe_led0[] = {GPIO4};
+static const unsigned danube_pins_dfe_led1[] = {GPIO5};
+
+static const unsigned danube_pins_ebu_a24[] = {GPIO13};
+static const unsigned danube_pins_ebu_clk[] = {GPIO21};
+static const unsigned danube_pins_ebu_cs1[] = {GPIO23};
+static const unsigned danube_pins_ebu_a23[] = {GPIO24};
+static const unsigned danube_pins_ebu_wait[] = {GPIO26};
+static const unsigned danube_pins_ebu_a25[] = {GPIO31};
+
+static const unsigned danube_pins_nand_ale[] = {GPIO13};
+static const unsigned danube_pins_nand_cs1[] = {GPIO23};
+static const unsigned danube_pins_nand_cle[] = {GPIO24};
+
+static const unsigned danube_pins_spi[] = {GPIO16, GPIO17, GPIO18}; /* DEPRECATED */
+static const unsigned danube_pins_spi_di[] = {GPIO16};
+static const unsigned danube_pins_spi_do[] = {GPIO17};
+static const unsigned danube_pins_spi_clk[] = {GPIO18};
+static const unsigned danube_pins_spi_cs1[] = {GPIO15};
+static const unsigned danube_pins_spi_cs2[] = {GPIO21};
+static const unsigned danube_pins_spi_cs3[] = {GPIO13};
+static const unsigned danube_pins_spi_cs4[] = {GPIO10};
+static const unsigned danube_pins_spi_cs5[] = {GPIO9};
+static const unsigned danube_pins_spi_cs6[] = {GPIO11};
+
+static const unsigned danube_pins_gpt1[] = {GPIO28};
+static const unsigned danube_pins_gpt2[] = {GPIO21};
+static const unsigned danube_pins_gpt3[] = {GPIO6};
+
+static const unsigned danube_pins_clkout0[] = {GPIO8};
+static const unsigned danube_pins_clkout1[] = {GPIO7};
+static const unsigned danube_pins_clkout2[] = {GPIO3};
+static const unsigned danube_pins_clkout3[] = {GPIO2};
+
+static const unsigned danube_pins_pci_gnt1[] = {GPIO30};
+static const unsigned danube_pins_pci_gnt2[] = {GPIO23};
+static const unsigned danube_pins_pci_gnt3[] = {GPIO19};
+static const unsigned danube_pins_pci_req1[] = {GPIO29};
+static const unsigned danube_pins_pci_req2[] = {GPIO31};
+static const unsigned danube_pins_pci_req3[] = {GPIO3};
+
+static const struct ltq_pin_group danube_grps[] = {
+	GRP_MUX("exin0", EXIN, danube_pins_exin0),
+	GRP_MUX("exin1", EXIN, danube_pins_exin1),
+	GRP_MUX("exin2", EXIN, danube_pins_exin2),
+	GRP_MUX("jtag", JTAG, danube_pins_jtag),
+	GRP_MUX("ebu a23", EBU, danube_pins_ebu_a23),
+	GRP_MUX("ebu a24", EBU, danube_pins_ebu_a24),
+	GRP_MUX("ebu a25", EBU, danube_pins_ebu_a25),
+	GRP_MUX("ebu clk", EBU, danube_pins_ebu_clk),
+	GRP_MUX("ebu cs1", EBU, danube_pins_ebu_cs1),
+	GRP_MUX("ebu wait", EBU, danube_pins_ebu_wait),
+	GRP_MUX("nand ale", EBU, danube_pins_nand_ale),
+	GRP_MUX("nand cs1", EBU, danube_pins_nand_cs1),
+	GRP_MUX("nand cle", EBU, danube_pins_nand_cle),
+	GRP_MUX("spi", SPI, danube_pins_spi), /* DEPRECATED */
+	GRP_MUX("spi_di", SPI, danube_pins_spi_di),
+	GRP_MUX("spi_do", SPI, danube_pins_spi_do),
+	GRP_MUX("spi_clk", SPI, danube_pins_spi_clk),
+	GRP_MUX("spi_cs1", SPI, danube_pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, danube_pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, danube_pins_spi_cs3),
+	GRP_MUX("spi_cs4", SPI, danube_pins_spi_cs4),
+	GRP_MUX("spi_cs5", SPI, danube_pins_spi_cs5),
+	GRP_MUX("spi_cs6", SPI, danube_pins_spi_cs6),
+	GRP_MUX("asc0", ASC, danube_pins_asc0),
+	GRP_MUX("asc0 cts rts", ASC, danube_pins_asc0_cts_rts),
+	GRP_MUX("stp", STP, danube_pins_stp),
+	GRP_MUX("nmi", NMI, danube_pins_nmi),
+	GRP_MUX("gpt1", GPT, danube_pins_gpt1),
+	GRP_MUX("gpt2", GPT, danube_pins_gpt2),
+	GRP_MUX("gpt3", GPT, danube_pins_gpt3),
+	GRP_MUX("clkout0", CGU, danube_pins_clkout0),
+	GRP_MUX("clkout1", CGU, danube_pins_clkout1),
+	GRP_MUX("clkout2", CGU, danube_pins_clkout2),
+	GRP_MUX("clkout3", CGU, danube_pins_clkout3),
+	GRP_MUX("gnt1", PCI, danube_pins_pci_gnt1),
+	GRP_MUX("gnt2", PCI, danube_pins_pci_gnt2),
+	GRP_MUX("gnt3", PCI, danube_pins_pci_gnt3),
+	GRP_MUX("req1", PCI, danube_pins_pci_req1),
+	GRP_MUX("req2", PCI, danube_pins_pci_req2),
+	GRP_MUX("req3", PCI, danube_pins_pci_req3),
+	GRP_MUX("dfe led0", DFE, danube_pins_dfe_led0),
+	GRP_MUX("dfe led1", DFE, danube_pins_dfe_led1),
+};
+
+static const char * const danube_pci_grps[] = {"gnt1", "gnt2",
+						"gnt3", "req1",
+						"req2", "req3"};
+static const char * const danube_spi_grps[] = {"spi", /* DEPRECATED */
+						"spi_di", "spi_do",
+						"spi_clk", "spi_cs1",
+						"spi_cs2", "spi_cs3",
+						"spi_cs4", "spi_cs5",
+						"spi_cs6"};
+static const char * const danube_cgu_grps[] = {"clkout0", "clkout1",
+						"clkout2", "clkout3"};
+static const char * const danube_ebu_grps[] = {"ebu a23", "ebu a24",
+						"ebu a25", "ebu cs1",
+						"ebu wait", "ebu clk",
+						"nand ale", "nand cs1",
+						"nand cle"};
+static const char * const danube_dfe_grps[] = {"dfe led0", "dfe led1"};
+static const char * const danube_exin_grps[] = {"exin0", "exin1", "exin2"};
+static const char * const danube_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const danube_asc_grps[] = {"asc0", "asc0 cts rts"};
+static const char * const danube_jtag_grps[] = {"jtag"};
+static const char * const danube_stp_grps[] = {"stp"};
+static const char * const danube_nmi_grps[] = {"nmi"};
+
+static const struct ltq_pmx_func danube_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(danube_spi_grps)},
+	{"asc",		ARRAY_AND_SIZE(danube_asc_grps)},
+	{"cgu",		ARRAY_AND_SIZE(danube_cgu_grps)},
+	{"jtag",	ARRAY_AND_SIZE(danube_jtag_grps)},
+	{"exin",	ARRAY_AND_SIZE(danube_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(danube_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(danube_gpt_grps)},
+	{"nmi",		ARRAY_AND_SIZE(danube_nmi_grps)},
+	{"pci",		ARRAY_AND_SIZE(danube_pci_grps)},
+	{"ebu",		ARRAY_AND_SIZE(danube_ebu_grps)},
+	{"dfe",		ARRAY_AND_SIZE(danube_dfe_grps)},
+};
+
+/* ---------  xrx100 related code --------- */
+#define XRX100_MAX_PIN		56
+
+static const struct ltq_mfp_pin xrx100_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	SDIO,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	EXIN,	CBUS,	SIN),
+	MFP_XWAY(GPIO2, GPIO,	CGU,	EXIN,	NONE),
+	MFP_XWAY(GPIO3, GPIO,	CGU,	SDIO,	PCI),
+	MFP_XWAY(GPIO4, GPIO,	STP,	DFE,	ASC),
+	MFP_XWAY(GPIO5, GPIO,	STP,	NONE,	DFE),
+	MFP_XWAY(GPIO6, GPIO,	STP,	GPT,	ASC),
+	MFP_XWAY(GPIO7, GPIO,	CGU,	CBUS,	NONE),
+	MFP_XWAY(GPIO8, GPIO,	CGU,	NMI,	NONE),
+	MFP_XWAY(GPIO9, GPIO,	ASC,	SPI,	EXIN),
+	MFP_XWAY(GPIO10, GPIO,	ASC,	SPI,	EXIN),
+	MFP_XWAY(GPIO11, GPIO,	ASC,	CBUS,	SPI),
+	MFP_XWAY(GPIO12, GPIO,	ASC,	CBUS,	MCD),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	SPI,	NONE),
+	MFP_XWAY(GPIO14, GPIO,	CGU,	NONE,	NONE),
+	MFP_XWAY(GPIO15, GPIO,	SPI,	SDIO,	MCD),
+	MFP_XWAY(GPIO16, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO17, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO18, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO19, GPIO,	PCI,	SDIO,	CGU),
+	MFP_XWAY(GPIO20, GPIO,	NONE,	SDIO,	EBU),
+	MFP_XWAY(GPIO21, GPIO,	PCI,	EBU,	GPT),
+	MFP_XWAY(GPIO22, GPIO,	SPI,	NONE,	EBU),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	PCI,	STP),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	TDM,	PCI),
+	MFP_XWAY(GPIO25, GPIO,	TDM,	SDIO,	ASC),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	TDM,	SDIO),
+	MFP_XWAY(GPIO27, GPIO,	TDM,	SDIO,	ASC),
+	MFP_XWAY(GPIO28, GPIO,	GPT,	NONE,	SDIO),
+	MFP_XWAY(GPIO29, GPIO,	PCI,	CBUS,	NONE),
+	MFP_XWAY(GPIO30, GPIO,	PCI,	CBUS,	NONE),
+	MFP_XWAY(GPIO31, GPIO,	EBU,	PCI,	NONE),
+	MFP_XWAY(GPIO32, GPIO,	MII,	NONE,	EBU),
+	MFP_XWAY(GPIO33, GPIO,	MII,	NONE,	EBU),
+	MFP_XWAY(GPIO34, GPIO,	SIN,	SSI,	NONE),
+	MFP_XWAY(GPIO35, GPIO,	SIN,	SSI,	NONE),
+	MFP_XWAY(GPIO36, GPIO,	SIN,	SSI,	NONE),
+	MFP_XWAY(GPIO37, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO38, GPIO,	PCI,	NONE,	NONE),
+	MFP_XWAY(GPIO39, GPIO,	NONE,	EXIN,	NONE),
+	MFP_XWAY(GPIO40, GPIO,	MII,	TDM,	NONE),
+	MFP_XWAY(GPIO41, GPIO,	MII,	TDM,	NONE),
+	MFP_XWAY(GPIO42, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO43, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO44, GPIO,	MII,	SIN,	NONE),
+	MFP_XWAY(GPIO45, GPIO,	MII,	NONE,	SIN),
+	MFP_XWAY(GPIO46, GPIO,	MII,	NONE,	EXIN),
+	MFP_XWAY(GPIO47, GPIO,	MII,	NONE,	SIN),
+	MFP_XWAY(GPIO48, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO49, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO50, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO51, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO52, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO53, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO54, GPIO,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO55, GPIO,	NONE,	NONE,	NONE),
+};
+
+static const unsigned xrx100_exin_pin_map[] = {GPIO0, GPIO1, GPIO2, GPIO39, GPIO10, GPIO9};
+
+static const unsigned xrx100_pins_exin0[] = {GPIO0};
+static const unsigned xrx100_pins_exin1[] = {GPIO1};
+static const unsigned xrx100_pins_exin2[] = {GPIO2};
+static const unsigned xrx100_pins_exin3[] = {GPIO39};
+static const unsigned xrx100_pins_exin4[] = {GPIO10};
+static const unsigned xrx100_pins_exin5[] = {GPIO9};
+
+static const unsigned xrx100_pins_asc0[] = {GPIO11, GPIO12};
+static const unsigned xrx100_pins_asc0_cts_rts[] = {GPIO9, GPIO10};
+static const unsigned xrx100_pins_stp[] = {GPIO4, GPIO5, GPIO6};
+static const unsigned xrx100_pins_nmi[] = {GPIO8};
+static const unsigned xrx100_pins_mdio[] = {GPIO42, GPIO43};
+
+static const unsigned xrx100_pins_dfe_led0[] = {GPIO4};
+static const unsigned xrx100_pins_dfe_led1[] = {GPIO5};
+
+static const unsigned xrx100_pins_ebu_a24[] = {GPIO13};
+static const unsigned xrx100_pins_ebu_clk[] = {GPIO21};
+static const unsigned xrx100_pins_ebu_cs1[] = {GPIO23};
+static const unsigned xrx100_pins_ebu_a23[] = {GPIO24};
+static const unsigned xrx100_pins_ebu_wait[] = {GPIO26};
+static const unsigned xrx100_pins_ebu_a25[] = {GPIO31};
+
+static const unsigned xrx100_pins_nand_ale[] = {GPIO13};
+static const unsigned xrx100_pins_nand_cs1[] = {GPIO23};
+static const unsigned xrx100_pins_nand_cle[] = {GPIO24};
+static const unsigned xrx100_pins_nand_rdy[] = {GPIO48};
+static const unsigned xrx100_pins_nand_rd[] = {GPIO49};
+
+static const unsigned xrx100_pins_spi_di[] = {GPIO16};
+static const unsigned xrx100_pins_spi_do[] = {GPIO17};
+static const unsigned xrx100_pins_spi_clk[] = {GPIO18};
+static const unsigned xrx100_pins_spi_cs1[] = {GPIO15};
+static const unsigned xrx100_pins_spi_cs2[] = {GPIO22};
+static const unsigned xrx100_pins_spi_cs3[] = {GPIO13};
+static const unsigned xrx100_pins_spi_cs4[] = {GPIO10};
+static const unsigned xrx100_pins_spi_cs5[] = {GPIO9};
+static const unsigned xrx100_pins_spi_cs6[] = {GPIO11};
+
+static const unsigned xrx100_pins_gpt1[] = {GPIO28};
+static const unsigned xrx100_pins_gpt2[] = {GPIO21};
+static const unsigned xrx100_pins_gpt3[] = {GPIO6};
+
+static const unsigned xrx100_pins_clkout0[] = {GPIO8};
+static const unsigned xrx100_pins_clkout1[] = {GPIO7};
+static const unsigned xrx100_pins_clkout2[] = {GPIO3};
+static const unsigned xrx100_pins_clkout3[] = {GPIO2};
+
+static const unsigned xrx100_pins_pci_gnt1[] = {GPIO30};
+static const unsigned xrx100_pins_pci_gnt2[] = {GPIO23};
+static const unsigned xrx100_pins_pci_gnt3[] = {GPIO19};
+static const unsigned xrx100_pins_pci_gnt4[] = {GPIO38};
+static const unsigned xrx100_pins_pci_req1[] = {GPIO29};
+static const unsigned xrx100_pins_pci_req2[] = {GPIO31};
+static const unsigned xrx100_pins_pci_req3[] = {GPIO3};
+static const unsigned xrx100_pins_pci_req4[] = {GPIO37};
+
+static const struct ltq_pin_group xrx100_grps[] = {
+	GRP_MUX("exin0", EXIN, xrx100_pins_exin0),
+	GRP_MUX("exin1", EXIN, xrx100_pins_exin1),
+	GRP_MUX("exin2", EXIN, xrx100_pins_exin2),
+	GRP_MUX("exin3", EXIN, xrx100_pins_exin3),
+	GRP_MUX("exin4", EXIN, xrx100_pins_exin4),
+	GRP_MUX("exin5", EXIN, xrx100_pins_exin5),
+	GRP_MUX("ebu a23", EBU, xrx100_pins_ebu_a23),
+	GRP_MUX("ebu a24", EBU, xrx100_pins_ebu_a24),
+	GRP_MUX("ebu a25", EBU, xrx100_pins_ebu_a25),
+	GRP_MUX("ebu clk", EBU, xrx100_pins_ebu_clk),
+	GRP_MUX("ebu cs1", EBU, xrx100_pins_ebu_cs1),
+	GRP_MUX("ebu wait", EBU, xrx100_pins_ebu_wait),
+	GRP_MUX("nand ale", EBU, xrx100_pins_nand_ale),
+	GRP_MUX("nand cs1", EBU, xrx100_pins_nand_cs1),
+	GRP_MUX("nand cle", EBU, xrx100_pins_nand_cle),
+	GRP_MUX("nand rdy", EBU, xrx100_pins_nand_rdy),
+	GRP_MUX("nand rd", EBU, xrx100_pins_nand_rd),
+	GRP_MUX("spi_di", SPI, xrx100_pins_spi_di),
+	GRP_MUX("spi_do", SPI, xrx100_pins_spi_do),
+	GRP_MUX("spi_clk", SPI, xrx100_pins_spi_clk),
+	GRP_MUX("spi_cs1", SPI, xrx100_pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, xrx100_pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, xrx100_pins_spi_cs3),
+	GRP_MUX("spi_cs4", SPI, xrx100_pins_spi_cs4),
+	GRP_MUX("spi_cs5", SPI, xrx100_pins_spi_cs5),
+	GRP_MUX("spi_cs6", SPI, xrx100_pins_spi_cs6),
+	GRP_MUX("asc0", ASC, xrx100_pins_asc0),
+	GRP_MUX("asc0 cts rts", ASC, xrx100_pins_asc0_cts_rts),
+	GRP_MUX("stp", STP, xrx100_pins_stp),
+	GRP_MUX("nmi", NMI, xrx100_pins_nmi),
+	GRP_MUX("gpt1", GPT, xrx100_pins_gpt1),
+	GRP_MUX("gpt2", GPT, xrx100_pins_gpt2),
+	GRP_MUX("gpt3", GPT, xrx100_pins_gpt3),
+	GRP_MUX("clkout0", CGU, xrx100_pins_clkout0),
+	GRP_MUX("clkout1", CGU, xrx100_pins_clkout1),
+	GRP_MUX("clkout2", CGU, xrx100_pins_clkout2),
+	GRP_MUX("clkout3", CGU, xrx100_pins_clkout3),
+	GRP_MUX("gnt1", PCI, xrx100_pins_pci_gnt1),
+	GRP_MUX("gnt2", PCI, xrx100_pins_pci_gnt2),
+	GRP_MUX("gnt3", PCI, xrx100_pins_pci_gnt3),
+	GRP_MUX("gnt4", PCI, xrx100_pins_pci_gnt4),
+	GRP_MUX("req1", PCI, xrx100_pins_pci_req1),
+	GRP_MUX("req2", PCI, xrx100_pins_pci_req2),
+	GRP_MUX("req3", PCI, xrx100_pins_pci_req3),
+	GRP_MUX("req4", PCI, xrx100_pins_pci_req4),
+	GRP_MUX("mdio", MDIO, xrx100_pins_mdio),
+	GRP_MUX("dfe led0", DFE, xrx100_pins_dfe_led0),
+	GRP_MUX("dfe led1", DFE, xrx100_pins_dfe_led1),
+};
+
+static const char * const xrx100_pci_grps[] = {"gnt1", "gnt2",
+						"gnt3", "gnt4",
+						"req1", "req2",
+						"req3", "req4"};
+static const char * const xrx100_spi_grps[] = {"spi_di", "spi_do",
+						"spi_clk", "spi_cs1",
+						"spi_cs2", "spi_cs3",
+						"spi_cs4", "spi_cs5",
+						"spi_cs6"};
+static const char * const xrx100_cgu_grps[] = {"clkout0", "clkout1",
+						"clkout2", "clkout3"};
+static const char * const xrx100_ebu_grps[] = {"ebu a23", "ebu a24",
+						"ebu a25", "ebu cs1",
+						"ebu wait", "ebu clk",
+						"nand ale", "nand cs1",
+						"nand cle", "nand rdy",
+						"nand rd"};
+static const char * const xrx100_exin_grps[] = {"exin0", "exin1", "exin2",
+						"exin3", "exin4", "exin5"};
+static const char * const xrx100_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const xrx100_asc_grps[] = {"asc0", "asc0 cts rts"};
+static const char * const xrx100_stp_grps[] = {"stp"};
+static const char * const xrx100_nmi_grps[] = {"nmi"};
+static const char * const xrx100_mdio_grps[] = {"mdio"};
+static const char * const xrx100_dfe_grps[] = {"dfe led0", "dfe led1"};
+
+static const struct ltq_pmx_func xrx100_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(xrx100_spi_grps)},
+	{"asc",		ARRAY_AND_SIZE(xrx100_asc_grps)},
+	{"cgu",		ARRAY_AND_SIZE(xrx100_cgu_grps)},
+	{"exin",	ARRAY_AND_SIZE(xrx100_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(xrx100_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(xrx100_gpt_grps)},
+	{"nmi",		ARRAY_AND_SIZE(xrx100_nmi_grps)},
+	{"pci",		ARRAY_AND_SIZE(xrx100_pci_grps)},
+	{"ebu",		ARRAY_AND_SIZE(xrx100_ebu_grps)},
+	{"mdio",	ARRAY_AND_SIZE(xrx100_mdio_grps)},
+	{"dfe",		ARRAY_AND_SIZE(xrx100_dfe_grps)},
+};
+
+/* ---------  xrx200 related code --------- */
+#define XRX200_MAX_PIN		50
+
+static const struct ltq_mfp_pin xrx200_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	SDIO,	TDM),
+	MFP_XWAY(GPIO1, GPIO,	EXIN,	CBUS,	SIN),
+	MFP_XWAY(GPIO2, GPIO,	CGU,	EXIN,	GPHY),
+	MFP_XWAY(GPIO3, GPIO,	CGU,	SDIO,	PCI),
+	MFP_XWAY(GPIO4, GPIO,	STP,	DFE,	USIF),
+	MFP_XWAY(GPIO5, GPIO,	STP,	GPHY,	DFE),
+	MFP_XWAY(GPIO6, GPIO,	STP,	GPT,	USIF),
+	MFP_XWAY(GPIO7, GPIO,	CGU,	CBUS,	GPHY),
+	MFP_XWAY(GPIO8, GPIO,	CGU,	NMI,	NONE),
+	MFP_XWAY(GPIO9, GPIO,	USIF,	SPI,	EXIN),
+	MFP_XWAY(GPIO10, GPIO,	USIF,	SPI,	EXIN),
+	MFP_XWAY(GPIO11, GPIO,	USIF,	CBUS,	SPI),
+	MFP_XWAY(GPIO12, GPIO,	USIF,	CBUS,	MCD),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	SPI,	NONE),
+	MFP_XWAY(GPIO14, GPIO,	CGU,	CBUS,	USIF),
+	MFP_XWAY(GPIO15, GPIO,	SPI,	SDIO,	MCD),
+	MFP_XWAY(GPIO16, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO17, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO18, GPIO,	SPI,	SDIO,	NONE),
+	MFP_XWAY(GPIO19, GPIO,	PCI,	SDIO,	CGU),
+	MFP_XWAY(GPIO20, GPIO,	NONE,	SDIO,	EBU),
+	MFP_XWAY(GPIO21, GPIO,	PCI,	EBU,	GPT),
+	MFP_XWAY(GPIO22, GPIO,	SPI,	CGU,	EBU),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	PCI,	STP),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	TDM,	PCI),
+	MFP_XWAY(GPIO25, GPIO,	TDM,	SDIO,	USIF),
+	MFP_XWAY(GPIO26, GPIO,	EBU,	TDM,	SDIO),
+	MFP_XWAY(GPIO27, GPIO,	TDM,	SDIO,	USIF),
+	MFP_XWAY(GPIO28, GPIO,	GPT,	PCI,	SDIO),
+	MFP_XWAY(GPIO29, GPIO,	PCI,	CBUS,	EXIN),
+	MFP_XWAY(GPIO30, GPIO,	PCI,	CBUS,	NONE),
+	MFP_XWAY(GPIO31, GPIO,	EBU,	PCI,	NONE),
+	MFP_XWAY(GPIO32, GPIO,	MII,	NONE,	EBU),
+	MFP_XWAY(GPIO33, GPIO,	MII,	NONE,	EBU),
+	MFP_XWAY(GPIO34, GPIO,	SIN,	SSI,	NONE),
+	MFP_XWAY(GPIO35, GPIO,	SIN,	SSI,	NONE),
+	MFP_XWAY(GPIO36, GPIO,	SIN,	SSI,	EXIN),
+	MFP_XWAY(GPIO37, GPIO,	USIF,	NONE,	PCI),
+	MFP_XWAY(GPIO38, GPIO,	PCI,	USIF,	NONE),
+	MFP_XWAY(GPIO39, GPIO,	USIF,	EXIN,	NONE),
+	MFP_XWAY(GPIO40, GPIO,	MII,	TDM,	NONE),
+	MFP_XWAY(GPIO41, GPIO,	MII,	TDM,	NONE),
+	MFP_XWAY(GPIO42, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO43, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO44, GPIO,	MII,	SIN,	GPHY),
+	MFP_XWAY(GPIO45, GPIO,	MII,	GPHY,	SIN),
+	MFP_XWAY(GPIO46, GPIO,	MII,	NONE,	EXIN),
+	MFP_XWAY(GPIO47, GPIO,	MII,	GPHY,	SIN),
+	MFP_XWAY(GPIO48, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO49, GPIO,	EBU,	NONE,	NONE),
+};
+
+static const unsigned xrx200_exin_pin_map[] = {GPIO0, GPIO1, GPIO2, GPIO39, GPIO10, GPIO9};
+
+static const unsigned xrx200_pins_exin0[] = {GPIO0};
+static const unsigned xrx200_pins_exin1[] = {GPIO1};
+static const unsigned xrx200_pins_exin2[] = {GPIO2};
+static const unsigned xrx200_pins_exin3[] = {GPIO39};
+static const unsigned xrx200_pins_exin4[] = {GPIO10};
+static const unsigned xrx200_pins_exin5[] = {GPIO9};
+
+static const unsigned xrx200_pins_usif_uart_rx[] = {GPIO11};
+static const unsigned xrx200_pins_usif_uart_tx[] = {GPIO12};
+static const unsigned xrx200_pins_usif_uart_rts[] = {GPIO9};
+static const unsigned xrx200_pins_usif_uart_cts[] = {GPIO10};
+static const unsigned xrx200_pins_usif_uart_dtr[] = {GPIO4};
+static const unsigned xrx200_pins_usif_uart_dsr[] = {GPIO6};
+static const unsigned xrx200_pins_usif_uart_dcd[] = {GPIO25};
+static const unsigned xrx200_pins_usif_uart_ri[] = {GPIO27};
+
+static const unsigned xrx200_pins_usif_spi_di[] = {GPIO11};
+static const unsigned xrx200_pins_usif_spi_do[] = {GPIO12};
+static const unsigned xrx200_pins_usif_spi_clk[] = {GPIO38};
+static const unsigned xrx200_pins_usif_spi_cs0[] = {GPIO37};
+static const unsigned xrx200_pins_usif_spi_cs1[] = {GPIO39};
+static const unsigned xrx200_pins_usif_spi_cs2[] = {GPIO14};
+
+static const unsigned xrx200_pins_stp[] = {GPIO4, GPIO5, GPIO6};
+static const unsigned xrx200_pins_nmi[] = {GPIO8};
+static const unsigned xrx200_pins_mdio[] = {GPIO42, GPIO43};
+
+static const unsigned xrx200_pins_dfe_led0[] = {GPIO4};
+static const unsigned xrx200_pins_dfe_led1[] = {GPIO5};
+
+static const unsigned xrx200_pins_gphy0_led0[] = {GPIO5};
+static const unsigned xrx200_pins_gphy0_led1[] = {GPIO7};
+static const unsigned xrx200_pins_gphy0_led2[] = {GPIO2};
+static const unsigned xrx200_pins_gphy1_led0[] = {GPIO44};
+static const unsigned xrx200_pins_gphy1_led1[] = {GPIO45};
+static const unsigned xrx200_pins_gphy1_led2[] = {GPIO47};
+
+static const unsigned xrx200_pins_ebu_a24[] = {GPIO13};
+static const unsigned xrx200_pins_ebu_clk[] = {GPIO21};
+static const unsigned xrx200_pins_ebu_cs1[] = {GPIO23};
+static const unsigned xrx200_pins_ebu_a23[] = {GPIO24};
+static const unsigned xrx200_pins_ebu_wait[] = {GPIO26};
+static const unsigned xrx200_pins_ebu_a25[] = {GPIO31};
+
+static const unsigned xrx200_pins_nand_ale[] = {GPIO13};
+static const unsigned xrx200_pins_nand_cs1[] = {GPIO23};
+static const unsigned xrx200_pins_nand_cle[] = {GPIO24};
+static const unsigned xrx200_pins_nand_rdy[] = {GPIO48};
+static const unsigned xrx200_pins_nand_rd[] = {GPIO49};
+
+static const unsigned xrx200_pins_spi_di[] = {GPIO16};
+static const unsigned xrx200_pins_spi_do[] = {GPIO17};
+static const unsigned xrx200_pins_spi_clk[] = {GPIO18};
+static const unsigned xrx200_pins_spi_cs1[] = {GPIO15};
+static const unsigned xrx200_pins_spi_cs2[] = {GPIO22};
+static const unsigned xrx200_pins_spi_cs3[] = {GPIO13};
+static const unsigned xrx200_pins_spi_cs4[] = {GPIO10};
+static const unsigned xrx200_pins_spi_cs5[] = {GPIO9};
+static const unsigned xrx200_pins_spi_cs6[] = {GPIO11};
+
+static const unsigned xrx200_pins_gpt1[] = {GPIO28};
+static const unsigned xrx200_pins_gpt2[] = {GPIO21};
+static const unsigned xrx200_pins_gpt3[] = {GPIO6};
+
+static const unsigned xrx200_pins_clkout0[] = {GPIO8};
+static const unsigned xrx200_pins_clkout1[] = {GPIO7};
+static const unsigned xrx200_pins_clkout2[] = {GPIO3};
+static const unsigned xrx200_pins_clkout3[] = {GPIO2};
+
+static const unsigned xrx200_pins_pci_gnt1[] = {GPIO28};
+static const unsigned xrx200_pins_pci_gnt2[] = {GPIO23};
+static const unsigned xrx200_pins_pci_gnt3[] = {GPIO19};
+static const unsigned xrx200_pins_pci_gnt4[] = {GPIO38};
+static const unsigned xrx200_pins_pci_req1[] = {GPIO29};
+static const unsigned xrx200_pins_pci_req2[] = {GPIO31};
+static const unsigned xrx200_pins_pci_req3[] = {GPIO3};
+static const unsigned xrx200_pins_pci_req4[] = {GPIO37};
+
+static const struct ltq_pin_group xrx200_grps[] = {
+	GRP_MUX("exin0", EXIN, xrx200_pins_exin0),
+	GRP_MUX("exin1", EXIN, xrx200_pins_exin1),
+	GRP_MUX("exin2", EXIN, xrx200_pins_exin2),
+	GRP_MUX("exin3", EXIN, xrx200_pins_exin3),
+	GRP_MUX("exin4", EXIN, xrx200_pins_exin4),
+	GRP_MUX("exin5", EXIN, xrx200_pins_exin5),
+	GRP_MUX("ebu a23", EBU, xrx200_pins_ebu_a23),
+	GRP_MUX("ebu a24", EBU, xrx200_pins_ebu_a24),
+	GRP_MUX("ebu a25", EBU, xrx200_pins_ebu_a25),
+	GRP_MUX("ebu clk", EBU, xrx200_pins_ebu_clk),
+	GRP_MUX("ebu cs1", EBU, xrx200_pins_ebu_cs1),
+	GRP_MUX("ebu wait", EBU, xrx200_pins_ebu_wait),
+	GRP_MUX("nand ale", EBU, xrx200_pins_nand_ale),
+	GRP_MUX("nand cs1", EBU, xrx200_pins_nand_cs1),
+	GRP_MUX("nand cle", EBU, xrx200_pins_nand_cle),
+	GRP_MUX("nand rdy", EBU, xrx200_pins_nand_rdy),
+	GRP_MUX("nand rd", EBU, xrx200_pins_nand_rd),
+	GRP_MUX("spi_di", SPI, xrx200_pins_spi_di),
+	GRP_MUX("spi_do", SPI, xrx200_pins_spi_do),
+	GRP_MUX("spi_clk", SPI, xrx200_pins_spi_clk),
+	GRP_MUX("spi_cs1", SPI, xrx200_pins_spi_cs1),
+	GRP_MUX("spi_cs2", SPI, xrx200_pins_spi_cs2),
+	GRP_MUX("spi_cs3", SPI, xrx200_pins_spi_cs3),
+	GRP_MUX("spi_cs4", SPI, xrx200_pins_spi_cs4),
+	GRP_MUX("spi_cs5", SPI, xrx200_pins_spi_cs5),
+	GRP_MUX("spi_cs6", SPI, xrx200_pins_spi_cs6),
+	GRP_MUX("usif uart_rx", USIF, xrx200_pins_usif_uart_rx),
+	GRP_MUX("usif uart_rx", USIF, xrx200_pins_usif_uart_tx),
+	GRP_MUX("usif uart_rts", USIF, xrx200_pins_usif_uart_rts),
+	GRP_MUX("usif uart_cts", USIF, xrx200_pins_usif_uart_cts),
+	GRP_MUX("usif uart_dtr", USIF, xrx200_pins_usif_uart_dtr),
+	GRP_MUX("usif uart_dsr", USIF, xrx200_pins_usif_uart_dsr),
+	GRP_MUX("usif uart_dcd", USIF, xrx200_pins_usif_uart_dcd),
+	GRP_MUX("usif uart_ri", USIF, xrx200_pins_usif_uart_ri),
+	GRP_MUX("usif spi_di", USIF, xrx200_pins_usif_spi_di),
+	GRP_MUX("usif spi_do", USIF, xrx200_pins_usif_spi_do),
+	GRP_MUX("usif spi_clk", USIF, xrx200_pins_usif_spi_clk),
+	GRP_MUX("usif spi_cs0", USIF, xrx200_pins_usif_spi_cs0),
+	GRP_MUX("usif spi_cs1", USIF, xrx200_pins_usif_spi_cs1),
+	GRP_MUX("usif spi_cs2", USIF, xrx200_pins_usif_spi_cs2),
+	GRP_MUX("stp", STP, xrx200_pins_stp),
+	GRP_MUX("nmi", NMI, xrx200_pins_nmi),
+	GRP_MUX("gpt1", GPT, xrx200_pins_gpt1),
+	GRP_MUX("gpt2", GPT, xrx200_pins_gpt2),
+	GRP_MUX("gpt3", GPT, xrx200_pins_gpt3),
+	GRP_MUX("clkout0", CGU, xrx200_pins_clkout0),
+	GRP_MUX("clkout1", CGU, xrx200_pins_clkout1),
+	GRP_MUX("clkout2", CGU, xrx200_pins_clkout2),
+	GRP_MUX("clkout3", CGU, xrx200_pins_clkout3),
+	GRP_MUX("gnt1", PCI, xrx200_pins_pci_gnt1),
+	GRP_MUX("gnt2", PCI, xrx200_pins_pci_gnt2),
+	GRP_MUX("gnt3", PCI, xrx200_pins_pci_gnt3),
+	GRP_MUX("gnt4", PCI, xrx200_pins_pci_gnt4),
+	GRP_MUX("req1", PCI, xrx200_pins_pci_req1),
+	GRP_MUX("req2", PCI, xrx200_pins_pci_req2),
+	GRP_MUX("req3", PCI, xrx200_pins_pci_req3),
+	GRP_MUX("req4", PCI, xrx200_pins_pci_req4),
+	GRP_MUX("mdio", MDIO, xrx200_pins_mdio),
+	GRP_MUX("dfe led0", DFE, xrx200_pins_dfe_led0),
+	GRP_MUX("dfe led1", DFE, xrx200_pins_dfe_led1),
+	GRP_MUX("gphy0 led0", GPHY, xrx200_pins_gphy0_led0),
+	GRP_MUX("gphy0 led1", GPHY, xrx200_pins_gphy0_led1),
+	GRP_MUX("gphy0 led2", GPHY, xrx200_pins_gphy0_led2),
+	GRP_MUX("gphy1 led0", GPHY, xrx200_pins_gphy1_led0),
+	GRP_MUX("gphy1 led1", GPHY, xrx200_pins_gphy1_led1),
+	GRP_MUX("gphy1 led2", GPHY, xrx200_pins_gphy1_led2),
+};
+
+static const char * const xrx200_pci_grps[] = {"gnt1", "gnt2",
+						"gnt3", "gnt4",
+						"req1", "req2",
+						"req3", "req4"};
+static const char * const xrx200_spi_grps[] = {"spi_di", "spi_do",
+						"spi_clk", "spi_cs1",
+						"spi_cs2", "spi_cs3",
+						"spi_cs4", "spi_cs5",
+						"spi_cs6"};
+static const char * const xrx200_cgu_grps[] = {"clkout0", "clkout1",
+						"clkout2", "clkout3"};
+static const char * const xrx200_ebu_grps[] = {"ebu a23", "ebu a24",
+						"ebu a25", "ebu cs1",
+						"ebu wait", "ebu clk",
+						"nand ale", "nand cs1",
+						"nand cle", "nand rdy",
+						"nand rd"};
+static const char * const xrx200_exin_grps[] = {"exin0", "exin1", "exin2",
+						"exin3", "exin4", "exin5"};
+static const char * const xrx200_gpt_grps[] = {"gpt1", "gpt2", "gpt3"};
+static const char * const xrx200_usif_grps[] = {"usif uart_rx", "usif uart_tx",
+						"usif uart_rts", "usif uart_cts",
+						"usif uart_dtr", "usif uart_dsr",
+						"usif uart_dcd", "usif uart_ri",
+						"usif spi_di", "usif spi_do",
+						"usif spi_clk", "usif spi_cs0",
+						"usif spi_cs1", "usif spi_cs2"};
+static const char * const xrx200_stp_grps[] = {"stp"};
+static const char * const xrx200_nmi_grps[] = {"nmi"};
+static const char * const xrx200_mdio_grps[] = {"mdio"};
+static const char * const xrx200_dfe_grps[] = {"dfe led0", "dfe led1"};
+static const char * const xrx200_gphy_grps[] = {"gphy0 led0", "gphy0 led1",
+						"gphy0 led2", "gphy1 led0",
+						"gphy1 led1", "gphy1 led2"};
+
+static const struct ltq_pmx_func xrx200_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(xrx200_spi_grps)},
+	{"usif",	ARRAY_AND_SIZE(xrx200_usif_grps)},
+	{"cgu",		ARRAY_AND_SIZE(xrx200_cgu_grps)},
+	{"exin",	ARRAY_AND_SIZE(xrx200_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(xrx200_stp_grps)},
+	{"gpt",		ARRAY_AND_SIZE(xrx200_gpt_grps)},
+	{"nmi",		ARRAY_AND_SIZE(xrx200_nmi_grps)},
+	{"pci",		ARRAY_AND_SIZE(xrx200_pci_grps)},
+	{"ebu",		ARRAY_AND_SIZE(xrx200_ebu_grps)},
+	{"mdio",	ARRAY_AND_SIZE(xrx200_mdio_grps)},
+	{"dfe",		ARRAY_AND_SIZE(xrx200_dfe_grps)},
+	{"gphy",	ARRAY_AND_SIZE(xrx200_gphy_grps)},
+};
+
+/* ---------  xrx300 related code --------- */
+#define XRX300_MAX_PIN		64
+
+static const struct ltq_mfp_pin xrx300_mfp[] = {
+	/*       pin    f0	f1	f2	f3   */
+	MFP_XWAY(GPIO0, GPIO,	EXIN,	EPHY,	NONE),
+	MFP_XWAY(GPIO1, GPIO,	NONE,	EXIN,	NONE),
+	MFP_XWAY(GPIO2, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO3, GPIO,	CGU,	NONE,	NONE),
+	MFP_XWAY(GPIO4, GPIO,	STP,	DFE,	NONE),
+	MFP_XWAY(GPIO5, GPIO,	STP,	EPHY,	DFE),
+	MFP_XWAY(GPIO6, GPIO,	STP,	NONE,	NONE),
+	MFP_XWAY(GPIO7, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO8, GPIO,	CGU,	GPHY,	EPHY),
+	MFP_XWAY(GPIO9, GPIO,	WIFI,	NONE,	EXIN),
+	MFP_XWAY(GPIO10, GPIO,	USIF,	SPI,	EXIN),
+	MFP_XWAY(GPIO11, GPIO,	USIF,	WIFI,	SPI),
+	MFP_XWAY(GPIO12, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO13, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO14, GPIO,	CGU,	USIF,	EPHY),
+	MFP_XWAY(GPIO15, GPIO,	SPI,	NONE,	MCD),
+	MFP_XWAY(GPIO16, GPIO,	SPI,	EXIN,	NONE),
+	MFP_XWAY(GPIO17, GPIO,	SPI,	NONE,	NONE),
+	MFP_XWAY(GPIO18, GPIO,	SPI,	NONE,	NONE),
+	MFP_XWAY(GPIO19, GPIO,	USIF,	NONE,	EPHY),
+	MFP_XWAY(GPIO20, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO21, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO22, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO23, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO24, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO25, GPIO,	TDM,	NONE,	NONE),
+	MFP_XWAY(GPIO26, GPIO,	TDM,	NONE,	NONE),
+	MFP_XWAY(GPIO27, GPIO,	TDM,	NONE,	NONE),
+	MFP_XWAY(GPIO28, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO29, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO30, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO31, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO32, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO33, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO34, GPIO,	NONE,	SSI,	NONE),
+	MFP_XWAY(GPIO35, GPIO,	NONE,	SSI,	NONE),
+	MFP_XWAY(GPIO36, GPIO,	NONE,	SSI,	NONE),
+	MFP_XWAY(GPIO37, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO38, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO39, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO40, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO41, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO42, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO43, GPIO,	MDIO,	NONE,	NONE),
+	MFP_XWAY(GPIO44, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO45, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO46, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO47, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO48, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO49, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO50, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO51, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO52, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO53, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO54, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO55, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO56, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO57, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO58, GPIO,	EBU,	TDM,	NONE),
+	MFP_XWAY(GPIO59, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO60, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO61, GPIO,	EBU,	NONE,	NONE),
+	MFP_XWAY(GPIO62, NONE,	NONE,	NONE,	NONE),
+	MFP_XWAY(GPIO63, NONE,	NONE,	NONE,	NONE),
+};
+
+static const unsigned xrx300_exin_pin_map[] = {GPIO0, GPIO1, GPIO16, GPIO10, GPIO9};
+
+static const unsigned xrx300_pins_exin0[] = {GPIO0};
+static const unsigned xrx300_pins_exin1[] = {GPIO1};
+static const unsigned xrx300_pins_exin2[] = {GPIO16};
+/* EXIN3 is not available on xrX300 */
+static const unsigned xrx300_pins_exin4[] = {GPIO10};
+static const unsigned xrx300_pins_exin5[] = {GPIO9};
+
+static const unsigned xrx300_pins_usif_uart_rx[] = {GPIO11};
+static const unsigned xrx300_pins_usif_uart_tx[] = {GPIO10};
+
+static const unsigned xrx300_pins_usif_spi_di[] = {GPIO11};
+static const unsigned xrx300_pins_usif_spi_do[] = {GPIO10};
+static const unsigned xrx300_pins_usif_spi_clk[] = {GPIO19};
+static const unsigned xrx300_pins_usif_spi_cs0[] = {GPIO14};
+
+static const unsigned xrx300_pins_stp[] = {GPIO4, GPIO5, GPIO6};
+static const unsigned xrx300_pins_mdio[] = {GPIO42, GPIO43};
+
+static const unsigned xrx300_pins_dfe_led0[] = {GPIO4};
+static const unsigned xrx300_pins_dfe_led1[] = {GPIO5};
+
+static const unsigned xrx300_pins_ephy0_led0[] = {GPIO5};
+static const unsigned xrx300_pins_ephy0_led1[] = {GPIO8};
+static const unsigned xrx300_pins_ephy1_led0[] = {GPIO14};
+static const unsigned xrx300_pins_ephy1_led1[] = {GPIO19};
+
+static const unsigned xrx300_pins_nand_ale[] = {GPIO13};
+static const unsigned xrx300_pins_nand_cs1[] = {GPIO23};
+static const unsigned xrx300_pins_nand_cle[] = {GPIO24};
+static const unsigned xrx300_pins_nand_rdy[] = {GPIO48};
+static const unsigned xrx300_pins_nand_rd[] = {GPIO49};
+static const unsigned xrx300_pins_nand_d1[] = {GPIO50};
+static const unsigned xrx300_pins_nand_d0[] = {GPIO51};
+static const unsigned xrx300_pins_nand_d2[] = {GPIO52};
+static const unsigned xrx300_pins_nand_d7[] = {GPIO53};
+static const unsigned xrx300_pins_nand_d6[] = {GPIO54};
+static const unsigned xrx300_pins_nand_d5[] = {GPIO55};
+static const unsigned xrx300_pins_nand_d4[] = {GPIO56};
+static const unsigned xrx300_pins_nand_d3[] = {GPIO57};
+static const unsigned xrx300_pins_nand_cs0[] = {GPIO58};
+static const unsigned xrx300_pins_nand_wr[] = {GPIO59};
+static const unsigned xrx300_pins_nand_wp[] = {GPIO60};
+static const unsigned xrx300_pins_nand_se[] = {GPIO61};
+
+static const unsigned xrx300_pins_spi_di[] = {GPIO16};
+static const unsigned xrx300_pins_spi_do[] = {GPIO17};
+static const unsigned xrx300_pins_spi_clk[] = {GPIO18};
+static const unsigned xrx300_pins_spi_cs1[] = {GPIO15};
+/* SPI_CS2 is not available on xrX300 */
+/* SPI_CS3 is not available on xrX300 */
+static const unsigned xrx300_pins_spi_cs4[] = {GPIO10};
+/* SPI_CS5 is not available on xrX300 */
+static const unsigned xrx300_pins_spi_cs6[] = {GPIO11};
+
+/* CLKOUT0 is not available on xrX300 */
+/* CLKOUT1 is not available on xrX300 */
+static const unsigned xrx300_pins_clkout2[] = {GPIO3};
+
+static const struct ltq_pin_group xrx300_grps[] = {
+	GRP_MUX("exin0", EXIN, xrx300_pins_exin0),
+	GRP_MUX("exin1", EXIN, xrx300_pins_exin1),
+	GRP_MUX("exin2", EXIN, xrx300_pins_exin2),
+	GRP_MUX("exin4", EXIN, xrx300_pins_exin4),
+	GRP_MUX("exin5", EXIN, xrx300_pins_exin5),
+	GRP_MUX("nand ale", EBU, xrx300_pins_nand_ale),
+	GRP_MUX("nand cs1", EBU, xrx300_pins_nand_cs1),
+	GRP_MUX("nand cle", EBU, xrx300_pins_nand_cle),
+	GRP_MUX("nand rdy", EBU, xrx300_pins_nand_rdy),
+	GRP_MUX("nand rd", EBU, xrx300_pins_nand_rd),
+	GRP_MUX("nand d1", EBU, xrx300_pins_nand_d1),
+	GRP_MUX("nand d0", EBU, xrx300_pins_nand_d0),
+	GRP_MUX("nand d2", EBU, xrx300_pins_nand_d2),
+	GRP_MUX("nand d7", EBU, xrx300_pins_nand_d7),
+	GRP_MUX("nand d6", EBU, xrx300_pins_nand_d6),
+	GRP_MUX("nand d5", EBU, xrx300_pins_nand_d5),
+	GRP_MUX("nand d4", EBU, xrx300_pins_nand_d4),
+	GRP_MUX("nand d3", EBU, xrx300_pins_nand_d3),
+	GRP_MUX("nand cs0", EBU, xrx300_pins_nand_cs0),
+	GRP_MUX("nand wr", EBU, xrx300_pins_nand_wr),
+	GRP_MUX("nand wp", EBU, xrx300_pins_nand_wp),
+	GRP_MUX("nand se", EBU, xrx300_pins_nand_se),
+	GRP_MUX("spi_di", SPI, xrx300_pins_spi_di),
+	GRP_MUX("spi_do", SPI, xrx300_pins_spi_do),
+	GRP_MUX("spi_clk", SPI, xrx300_pins_spi_clk),
+	GRP_MUX("spi_cs1", SPI, xrx300_pins_spi_cs1),
+	GRP_MUX("spi_cs4", SPI, xrx300_pins_spi_cs4),
+	GRP_MUX("spi_cs6", SPI, xrx300_pins_spi_cs6),
+	GRP_MUX("usif uart_rx", USIF, xrx300_pins_usif_uart_rx),
+	GRP_MUX("usif uart_tx", USIF, xrx300_pins_usif_uart_tx),
+	GRP_MUX("usif spi_di", USIF, xrx300_pins_usif_spi_di),
+	GRP_MUX("usif spi_do", USIF, xrx300_pins_usif_spi_do),
+	GRP_MUX("usif spi_clk", USIF, xrx300_pins_usif_spi_clk),
+	GRP_MUX("usif spi_cs0", USIF, xrx300_pins_usif_spi_cs0),
+	GRP_MUX("stp", STP, xrx300_pins_stp),
+	GRP_MUX("clkout2", CGU, xrx300_pins_clkout2),
+	GRP_MUX("mdio", MDIO, xrx300_pins_mdio),
+	GRP_MUX("dfe led0", DFE, xrx300_pins_dfe_led0),
+	GRP_MUX("dfe led1", DFE, xrx300_pins_dfe_led1),
+	GRP_MUX("ephy0 led0", GPHY, xrx300_pins_ephy0_led0),
+	GRP_MUX("ephy0 led1", GPHY, xrx300_pins_ephy0_led1),
+	GRP_MUX("ephy1 led0", GPHY, xrx300_pins_ephy1_led0),
+	GRP_MUX("ephy1 led1", GPHY, xrx300_pins_ephy1_led1),
+};
+
+static const char * const xrx300_spi_grps[] = {"spi_di", "spi_do",
+						"spi_clk", "spi_cs1",
+						"spi_cs4", "spi_cs6"};
+static const char * const xrx300_cgu_grps[] = {"clkout2"};
+static const char * const xrx300_ebu_grps[] = {"nand ale", "nand cs1",
+						"nand cle", "nand rdy",
+						"nand rd", "nand d1",
+						"nand d0", "nand d2",
+						"nand d7", "nand d6",
+						"nand d5", "nand d4",
+						"nand d3", "nand cs0",
+						"nand wr", "nand wp",
+						"nand se"};
+static const char * const xrx300_exin_grps[] = {"exin0", "exin1", "exin2",
+						"exin4", "exin5"};
+static const char * const xrx300_usif_grps[] = {"usif uart_rx", "usif uart_tx",
+						"usif spi_di", "usif spi_do",
+						"usif spi_clk", "usif spi_cs0"};
+static const char * const xrx300_stp_grps[] = {"stp"};
+static const char * const xrx300_mdio_grps[] = {"mdio"};
+static const char * const xrx300_dfe_grps[] = {"dfe led0", "dfe led1"};
+static const char * const xrx300_gphy_grps[] = {"ephy0 led0", "ephy0 led1",
+						"ephy1 led0", "ephy1 led1"};
+
+static const struct ltq_pmx_func xrx300_funcs[] = {
+	{"spi",		ARRAY_AND_SIZE(xrx300_spi_grps)},
+	{"usif",	ARRAY_AND_SIZE(xrx300_usif_grps)},
+	{"cgu",		ARRAY_AND_SIZE(xrx300_cgu_grps)},
+	{"exin",	ARRAY_AND_SIZE(xrx300_exin_grps)},
+	{"stp",		ARRAY_AND_SIZE(xrx300_stp_grps)},
+	{"ebu",		ARRAY_AND_SIZE(xrx300_ebu_grps)},
+	{"mdio",	ARRAY_AND_SIZE(xrx300_mdio_grps)},
+	{"dfe",		ARRAY_AND_SIZE(xrx300_dfe_grps)},
+	{"ephy",	ARRAY_AND_SIZE(xrx300_gphy_grps)},
+};
+
 /* ---------  pinconf related code --------- */
 static int xway_pinconf_get(struct pinctrl_dev *pctldev,
 				unsigned pin,
@@ -648,7 +1535,7 @@ static struct ltq_pinmux_info xway_info = {
 /* ---------  gpio_chip related code --------- */
 static void xway_gpio_set(struct gpio_chip *chip, unsigned int pin, int val)
 {
-	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->parent);
 
 	if (val)
 		gpio_setbit(info->membase[0], GPIO_OUT(pin), PORT_PIN(pin));
@@ -658,14 +1545,14 @@ static void xway_gpio_set(struct gpio_chip *chip, unsigned int pin, int val)
 
 static int xway_gpio_get(struct gpio_chip *chip, unsigned int pin)
 {
-	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->parent);
 
-	return gpio_getbit(info->membase[0], GPIO_IN(pin), PORT_PIN(pin));
+	return !!gpio_getbit(info->membase[0], GPIO_IN(pin), PORT_PIN(pin));
 }
 
 static int xway_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
 {
-	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->parent);
 
 	gpio_clearbit(info->membase[0], GPIO_DIR(pin), PORT_PIN(pin));
 
@@ -674,8 +1561,12 @@ static int xway_gpio_dir_in(struct gpio_chip *chip, unsigned int pin)
 
 static int xway_gpio_dir_out(struct gpio_chip *chip, unsigned int pin, int val)
 {
-	struct ltq_pinmux_info *info = dev_get_drvdata(chip->dev);
+	struct ltq_pinmux_info *info = dev_get_drvdata(chip->parent);
 
+	if (PORT(pin) == PORT3)
+		gpio_setbit(info->membase[0], GPIO3_OD, PORT_PIN(pin));
+	else
+		gpio_setbit(info->membase[0], GPIO_OD(pin), PORT_PIN(pin));
 	gpio_setbit(info->membase[0], GPIO_DIR(pin), PORT_PIN(pin));
 	xway_gpio_set(chip, pin, val);
 
@@ -695,10 +1586,7 @@ static struct gpio_chip xway_chip = {
 
 
 /* --------- register the pinctrl layer --------- */
-static const unsigned xway_exin_pin_map[] = {GPIO0, GPIO1, GPIO2, GPIO39, GPIO46, GPIO9};
-static const unsigned ase_exin_pins_map[] = {GPIO6, GPIO29, GPIO0};
-
-static struct pinctrl_xway_soc {
+struct pinctrl_xway_soc {
 	int pin_count;
 	const struct ltq_mfp_pin *mfp;
 	const struct ltq_pin_group *grps;
@@ -707,22 +1595,54 @@ static struct pinctrl_xway_soc {
 	unsigned int num_funcs;
 	const unsigned *exin;
 	unsigned int num_exin;
-} soc_cfg[] = {
-	/* legacy xway */
-	{XWAY_MAX_PIN, xway_mfp,
-		xway_grps, ARRAY_SIZE(xway_grps),
-		danube_funcs, ARRAY_SIZE(danube_funcs),
-		xway_exin_pin_map, 3},
-	/* xway xr9 series */
-	{XR9_MAX_PIN, xway_mfp,
-		xway_grps, ARRAY_SIZE(xway_grps),
-		xrx_funcs, ARRAY_SIZE(xrx_funcs),
-		xway_exin_pin_map, 6},
-	/* xway ase series */
-	{XWAY_MAX_PIN, ase_mfp,
-		ase_grps, ARRAY_SIZE(ase_grps),
-		ase_funcs, ARRAY_SIZE(ase_funcs),
-		ase_exin_pins_map, 3},
+};
+
+/* xway xr9 series (DEPRECATED: Use XWAY xRX100/xRX200 Family) */
+static struct pinctrl_xway_soc xr9_pinctrl = {
+	XR9_MAX_PIN, xway_mfp,
+	xway_grps, ARRAY_SIZE(xway_grps),
+	xrx_funcs, ARRAY_SIZE(xrx_funcs),
+	xway_exin_pin_map, 6
+};
+
+/* XWAY AMAZON Family */
+static struct pinctrl_xway_soc ase_pinctrl = {
+	ASE_MAX_PIN, ase_mfp,
+	ase_grps, ARRAY_SIZE(ase_grps),
+	ase_funcs, ARRAY_SIZE(ase_funcs),
+	ase_exin_pin_map, 3
+};
+
+/* XWAY DANUBE Family */
+static struct pinctrl_xway_soc danube_pinctrl = {
+	DANUBE_MAX_PIN, danube_mfp,
+	danube_grps, ARRAY_SIZE(danube_grps),
+	danube_funcs, ARRAY_SIZE(danube_funcs),
+	danube_exin_pin_map, 3
+};
+
+/* XWAY xRX100 Family */
+static struct pinctrl_xway_soc xrx100_pinctrl = {
+	XRX100_MAX_PIN, xrx100_mfp,
+	xrx100_grps, ARRAY_SIZE(xrx100_grps),
+	xrx100_funcs, ARRAY_SIZE(xrx100_funcs),
+	xrx100_exin_pin_map, 6
+};
+
+/* XWAY xRX200 Family */
+static struct pinctrl_xway_soc xrx200_pinctrl = {
+	XRX200_MAX_PIN, xrx200_mfp,
+	xrx200_grps, ARRAY_SIZE(xrx200_grps),
+	xrx200_funcs, ARRAY_SIZE(xrx200_funcs),
+	xrx200_exin_pin_map, 6
+};
+
+/* XWAY xRX300 Family */
+static struct pinctrl_xway_soc xrx300_pinctrl = {
+	XRX300_MAX_PIN, xrx300_mfp,
+	xrx300_grps, ARRAY_SIZE(xrx300_grps),
+	xrx300_funcs, ARRAY_SIZE(xrx300_funcs),
+	xrx300_exin_pin_map, 5
 };
 
 static struct pinctrl_gpio_range xway_gpio_range = {
@@ -731,9 +1651,14 @@ static struct pinctrl_gpio_range xway_gpio_range = {
 };
 
 static const struct of_device_id xway_match[] = {
-	{ .compatible = "lantiq,pinctrl-xway", .data = &soc_cfg[0]},
-	{ .compatible = "lantiq,pinctrl-xr9", .data = &soc_cfg[1]},
-	{ .compatible = "lantiq,pinctrl-ase", .data = &soc_cfg[2]},
+	{ .compatible = "lantiq,pinctrl-xway", .data = &danube_pinctrl}, /*DEPRECATED*/
+	{ .compatible = "lantiq,pinctrl-xr9", .data = &xr9_pinctrl}, /*DEPRECATED*/
+	{ .compatible = "lantiq,pinctrl-ase", .data = &ase_pinctrl}, /*DEPRECATED*/
+	{ .compatible = "lantiq,ase-pinctrl", .data = &ase_pinctrl},
+	{ .compatible = "lantiq,danube-pinctrl", .data = &danube_pinctrl},
+	{ .compatible = "lantiq,xrx100-pinctrl", .data = &xrx100_pinctrl},
+	{ .compatible = "lantiq,xrx200-pinctrl", .data = &xrx200_pinctrl},
+	{ .compatible = "lantiq,xrx300-pinctrl", .data = &xrx300_pinctrl},
 	{},
 };
 MODULE_DEVICE_TABLE(of, xway_match);
@@ -755,7 +1680,7 @@ static int pinmux_xway_probe(struct platform_device *pdev)
 	if (match)
 		xway_soc = (const struct pinctrl_xway_soc *) match->data;
 	else
-		xway_soc = &soc_cfg[0];
+		xway_soc = &danube_pinctrl;
 
 	/* find out how many pads we have */
 	xway_chip.ngpio = xway_soc->pin_count;
@@ -783,7 +1708,7 @@ static int pinmux_xway_probe(struct platform_device *pdev)
 	xway_pctrl_desc.pins = xway_info.pads;
 
 	/* load the gpio chip */
-	xway_chip.dev = &pdev->dev;
+	xway_chip.parent = &pdev->dev;
 	ret = gpiochip_add(&xway_chip);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register gpio chip\n");
diff --git a/drivers/pinctrl/pxa/Kconfig b/drivers/pinctrl/pxa/Kconfig
new file mode 100644
index 000000000000..990667ff772c
--- /dev/null
+++ b/drivers/pinctrl/pxa/Kconfig
@@ -0,0 +1,17 @@
+if (ARCH_PXA || COMPILE_TEST)
+
+config PINCTRL_PXA
+	bool
+	select PINMUX
+	select PINCONF
+	select GENERIC_PINCONF
+
+config PINCTRL_PXA27X
+	tristate "Marvell PXA27x pin controller driver"
+	select PINCTRL_PXA
+	default y if PXA27x
+	help
+	  This is the pinctrl, pinmux, pinconf driver for the Marvell
+	  PXA2xx block found in the pxa25x and pxa27x platforms.
+
+endif
diff --git a/drivers/pinctrl/pxa/Makefile b/drivers/pinctrl/pxa/Makefile
new file mode 100644
index 000000000000..f1d56af2bfc0
--- /dev/null
+++ b/drivers/pinctrl/pxa/Makefile
@@ -0,0 +1,2 @@
+# Marvell PXA pin control drivers
+obj-$(CONFIG_PINCTRL_PXA27X)	+= pinctrl-pxa2xx.o pinctrl-pxa27x.o
diff --git a/drivers/pinctrl/pxa/pinctrl-pxa27x.c b/drivers/pinctrl/pxa/pinctrl-pxa27x.c
new file mode 100644
index 000000000000..2e2c3709ef05
--- /dev/null
+++ b/drivers/pinctrl/pxa/pinctrl-pxa27x.c
@@ -0,0 +1,566 @@
+/*
+ * Marvell PXA27x family pin control
+ *
+ * Copyright (C) 2015 Robert Jarzmik
+ *
+ * This program is free software; 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/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-pxa2xx.h"
+
+static const struct pxa_desc_pin pxa27x_pins[] = {
+	PXA_GPIO_ONLY_PIN(PXA_PINCTRL_PIN(0)),
+	PXA_GPIO_ONLY_PIN(PXA_PINCTRL_PIN(1)),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(9),
+		     PXA_FUNCTION(0, 3, "FFCTS"),
+		     PXA_FUNCTION(1, 1, "HZ_CLK"),
+		     PXA_FUNCTION(1, 3, "CHOUT<0>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(10),
+		     PXA_FUNCTION(0, 1, "FFDCD"),
+		     PXA_FUNCTION(0, 3, "USB_P3_5"),
+		     PXA_FUNCTION(1, 1, "HZ_CLK"),
+		     PXA_FUNCTION(1, 3, "CHOUT<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(11),
+		     PXA_FUNCTION(0, 1, "EXT_SYNC<0>"),
+		     PXA_FUNCTION(0, 2, "SSPRXD2"),
+		     PXA_FUNCTION(0, 3, "USB_P3_1"),
+		     PXA_FUNCTION(1, 1, "CHOUT<0>"),
+		     PXA_FUNCTION(1, 1, "PWM_OUT<2>"),
+		     PXA_FUNCTION(1, 3, "48_MHz")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(12),
+		     PXA_FUNCTION(0, 1, "EXT_SYNC<1>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<7>"),
+		     PXA_FUNCTION(1, 1, "CHOUT<1>"),
+		     PXA_FUNCTION(1, 1, "PWM_OUT<3>"),
+		     PXA_FUNCTION(1, 3, "48_MHz")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(13),
+		     PXA_FUNCTION(0, 1, "CLK_EXT"),
+		     PXA_FUNCTION(0, 2, "KP_DKIN<7>"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<7>"),
+		     PXA_FUNCTION(1, 1, "SSPTXD2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(14),
+		     PXA_FUNCTION(0, 1, "L_VSYNC"),
+		     PXA_FUNCTION(0, 2, "SSPSFRM2"),
+		     PXA_FUNCTION(1, 1, "SSPSFRM2"),
+		     PXA_FUNCTION(1, 3, "UCLK")),
+	PXA_GPIO_ONLY_PIN(PXA_PINCTRL_PIN(15)),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(16),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<5>"),
+		     PXA_FUNCTION(1, 2, "PWM_OUT<0>"),
+		     PXA_FUNCTION(1, 3, "FFTXD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(17),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<6>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<6>"),
+		     PXA_FUNCTION(1, 2, "PWM_OUT<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(18),
+		     PXA_FUNCTION(0, 1, "RDY")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(19),
+		     PXA_FUNCTION(0, 1, "SSPSCLK2"),
+		     PXA_FUNCTION(0, 3, "FFRXD"),
+		     PXA_FUNCTION(1, 1, "SSPSCLK2"),
+		     PXA_FUNCTION(1, 2, "L_CS"),
+		     PXA_FUNCTION(1, 3, "nURST")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(20),
+		     PXA_FUNCTION(0, 1, "DREQ<0>"),
+		     PXA_FUNCTION(0, 2, "MBREQ"),
+		     PXA_FUNCTION(1, 1, "nSDCS<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(21),
+		     PXA_FUNCTION(1, 1, "nSDCS<3>"),
+		     PXA_FUNCTION(1, 2, "DVAL<0>"),
+		     PXA_FUNCTION(1, 3, "MBGNT")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(22),
+		     PXA_FUNCTION(0, 1, "SSPEXTCLK2"),
+		     PXA_FUNCTION(0, 2, "SSPSCLKEN2"),
+		     PXA_FUNCTION(0, 3, "SSPSCLK2"),
+		     PXA_FUNCTION(1, 1, "KP_MKOUT<7>"),
+		     PXA_FUNCTION(1, 2, "SSPSYSCLK2"),
+		     PXA_FUNCTION(1, 3, "SSPSCLK2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(23),
+		     PXA_FUNCTION(0, 2, "SSPSCLK"),
+		     PXA_FUNCTION(1, 1, "CIF_MCLK"),
+		     PXA_FUNCTION(1, 1, "SSPSCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(24),
+		     PXA_FUNCTION(0, 1, "CIF_FV"),
+		     PXA_FUNCTION(0, 2, "SSPSFRM"),
+		     PXA_FUNCTION(1, 1, "CIF_FV"),
+		     PXA_FUNCTION(1, 2, "SSPSFRM")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(25),
+		     PXA_FUNCTION(0, 1, "CIF_LV"),
+		     PXA_FUNCTION(1, 1, "CIF_LV"),
+		     PXA_FUNCTION(1, 2, "SSPTXD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(26),
+		     PXA_FUNCTION(0, 1, "SSPRXD"),
+		     PXA_FUNCTION(0, 2, "CIF_PCLK"),
+		     PXA_FUNCTION(0, 3, "FFCTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(27),
+		     PXA_FUNCTION(0, 1, "SSPEXTCLK"),
+		     PXA_FUNCTION(0, 2, "SSPSCLKEN"),
+		     PXA_FUNCTION(0, 3, "CIF_DD<0>"),
+		     PXA_FUNCTION(1, 1, "SSPSYSCLK"),
+		     PXA_FUNCTION(1, 3, "FFRTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(28),
+		     PXA_FUNCTION(0, 1, "AC97_BITCLK"),
+		     PXA_FUNCTION(0, 2, "I2S_BITCLK"),
+		     PXA_FUNCTION(0, 3, "SSPSFRM"),
+		     PXA_FUNCTION(1, 1, "I2S_BITCLK"),
+		     PXA_FUNCTION(1, 3, "SSPSFRM")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(29),
+		     PXA_FUNCTION(0, 1, "AC97_SDATA_IN_0"),
+		     PXA_FUNCTION(0, 2, "I2S_SDATA_IN"),
+		     PXA_FUNCTION(0, 3, "SSPSCLK"),
+		     PXA_FUNCTION(1, 1, "SSPRXD2"),
+		     PXA_FUNCTION(1, 3, "SSPSCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(30),
+		     PXA_FUNCTION(1, 1, "I2S_SDATA_OUT"),
+		     PXA_FUNCTION(1, 2, "AC97_SDATA_OUT"),
+		     PXA_FUNCTION(1, 3, "USB_P3_2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(31),
+		     PXA_FUNCTION(1, 1, "I2S_SYNC"),
+		     PXA_FUNCTION(1, 2, "AC97_SYNC"),
+		     PXA_FUNCTION(1, 3, "USB_P3_6")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(32),
+		     PXA_FUNCTION(1, 1, "MSSCLK"),
+		     PXA_FUNCTION(1, 2, "MMCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(33),
+		     PXA_FUNCTION(0, 1, "FFRXD"),
+		     PXA_FUNCTION(0, 2, "FFDSR"),
+		     PXA_FUNCTION(1, 1, "DVAL<1>"),
+		     PXA_FUNCTION(1, 2, "nCS<5>"),
+		     PXA_FUNCTION(1, 3, "MBGNT")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(34),
+		     PXA_FUNCTION(0, 1, "FFRXD"),
+		     PXA_FUNCTION(0, 2, "KP_MKIN<3>"),
+		     PXA_FUNCTION(0, 3, "SSPSCLK3"),
+		     PXA_FUNCTION(1, 1, "USB_P2_2"),
+		     PXA_FUNCTION(1, 3, "SSPSCLK3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(35),
+		     PXA_FUNCTION(0, 1, "FFCTS"),
+		     PXA_FUNCTION(0, 2, "USB_P2_1"),
+		     PXA_FUNCTION(0, 3, "SSPSFRM3"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<6>"),
+		     PXA_FUNCTION(1, 3, "SSPTXD3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(36),
+		     PXA_FUNCTION(0, 1, "FFDCD"),
+		     PXA_FUNCTION(0, 2, "SSPSCLK2"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<7>"),
+		     PXA_FUNCTION(1, 1, "USB_P2_4"),
+		     PXA_FUNCTION(1, 2, "SSPSCLK2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(37),
+		     PXA_FUNCTION(0, 1, "FFDSR"),
+		     PXA_FUNCTION(0, 2, "SSPSFRM2"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<3>"),
+		     PXA_FUNCTION(1, 1, "USB_P2_8"),
+		     PXA_FUNCTION(1, 2, "SSPSFRM2"),
+		     PXA_FUNCTION(1, 3, "FFTXD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(38),
+		     PXA_FUNCTION(0, 1, "FFRI"),
+		     PXA_FUNCTION(0, 2, "KP_MKIN<4>"),
+		     PXA_FUNCTION(0, 3, "USB_P2_3"),
+		     PXA_FUNCTION(1, 1, "SSPTXD3"),
+		     PXA_FUNCTION(1, 2, "SSPTXD2"),
+		     PXA_FUNCTION(1, 3, "PWM_OUT<0>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(39),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<4>"),
+		     PXA_FUNCTION(0, 3, "SSPSFRM3"),
+		     PXA_FUNCTION(1, 1, "USB_P2_6"),
+		     PXA_FUNCTION(1, 2, "FFTXD"),
+		     PXA_FUNCTION(1, 3, "SSPSFRM3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(40),
+		     PXA_FUNCTION(0, 1, "SSPRXD2"),
+		     PXA_FUNCTION(0, 3, "USB_P2_5"),
+		     PXA_FUNCTION(1, 1, "KP_MKOUT<6>"),
+		     PXA_FUNCTION(1, 2, "FFDTR"),
+		     PXA_FUNCTION(1, 3, "SSPSCLK3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(41),
+		     PXA_FUNCTION(0, 1, "FFRXD"),
+		     PXA_FUNCTION(0, 2, "USB_P2_7"),
+		     PXA_FUNCTION(0, 3, "SSPRXD3"),
+		     PXA_FUNCTION(1, 1, "KP_MKOUT<7>"),
+		     PXA_FUNCTION(1, 2, "FFRTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(42),
+		     PXA_FUNCTION(0, 1, "BTRXD"),
+		     PXA_FUNCTION(0, 2, "ICP_RXD"),
+		     PXA_FUNCTION(1, 3, "CIF_MCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(43),
+		     PXA_FUNCTION(0, 3, "CIF_FV"),
+		     PXA_FUNCTION(1, 1, "ICP_TXD"),
+		     PXA_FUNCTION(1, 2, "BTTXD"),
+		     PXA_FUNCTION(1, 3, "CIF_FV")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(44),
+		     PXA_FUNCTION(0, 1, "BTCTS"),
+		     PXA_FUNCTION(0, 3, "CIF_LV"),
+		     PXA_FUNCTION(1, 3, "CIF_LV")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(45),
+		     PXA_FUNCTION(0, 3, "CIF_PCLK"),
+		     PXA_FUNCTION(1, 1, "AC97_SYSCLK"),
+		     PXA_FUNCTION(1, 2, "BTRTS"),
+		     PXA_FUNCTION(1, 3, "SSPSYSCLK3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(46),
+		     PXA_FUNCTION(0, 1, "ICP_RXD"),
+		     PXA_FUNCTION(0, 2, "STD_RXD"),
+		     PXA_FUNCTION(1, 2, "PWM_OUT<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(47),
+		     PXA_FUNCTION(0, 1, "CIF_DD<0>"),
+		     PXA_FUNCTION(1, 1, "STD_TXD"),
+		     PXA_FUNCTION(1, 2, "ICP_TXD"),
+		     PXA_FUNCTION(1, 3, "PWM_OUT<3>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(48),
+		     PXA_FUNCTION(0, 1, "CIF_DD<5>"),
+		     PXA_FUNCTION(1, 1, "BB_OB_DAT<1>"),
+		     PXA_FUNCTION(1, 2, "nPOE")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(49),
+		     PXA_FUNCTION(1, 2, "nPWE")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(50),
+		     PXA_FUNCTION(0, 1, "CIF_DD<3>"),
+		     PXA_FUNCTION(0, 3, "SSPSCLK2"),
+		     PXA_FUNCTION(1, 1, "BB_OB_DAT<2>"),
+		     PXA_FUNCTION(1, 2, "nPIOR"),
+		     PXA_FUNCTION(1, 3, "SSPSCLK2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(51),
+		     PXA_FUNCTION(0, 1, "CIF_DD<2>"),
+		     PXA_FUNCTION(1, 1, "BB_OB_DAT<3>"),
+		     PXA_FUNCTION(1, 2, "nPIOW")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(52),
+		     PXA_FUNCTION(0, 1, "CIF_DD<4>"),
+		     PXA_FUNCTION(0, 2, "SSPSCLK3"),
+		     PXA_FUNCTION(1, 1, "BB_OB_CLK"),
+		     PXA_FUNCTION(1, 2, "SSPSCLK3")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(53),
+		     PXA_FUNCTION(0, 1, "FFRXD"),
+		     PXA_FUNCTION(0, 2, "USB_P2_3"),
+		     PXA_FUNCTION(1, 1, "BB_OB_STB"),
+		     PXA_FUNCTION(1, 2, "CIF_MCLK"),
+		     PXA_FUNCTION(1, 3, "SSPSYSCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(54),
+		     PXA_FUNCTION(0, 2, "BB_OB_WAIT"),
+		     PXA_FUNCTION(0, 3, "CIF_PCLK"),
+		     PXA_FUNCTION(1, 2, "nPCE<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(55),
+		     PXA_FUNCTION(0, 1, "CIF_DD<1>"),
+		     PXA_FUNCTION(0, 2, "BB_IB_DAT<1>"),
+		     PXA_FUNCTION(1, 2, "nPREG")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(56),
+		     PXA_FUNCTION(0, 1, "nPWAIT"),
+		     PXA_FUNCTION(0, 2, "BB_IB_DAT<2>"),
+		     PXA_FUNCTION(1, 1, "USB_P3_4")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(57),
+		     PXA_FUNCTION(0, 1, "nIOS16"),
+		     PXA_FUNCTION(0, 2, "BB_IB_DAT<3>"),
+		     PXA_FUNCTION(1, 3, "SSPTXD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(58),
+		     PXA_FUNCTION(0, 2, "LDD<0>"),
+		     PXA_FUNCTION(1, 2, "LDD<0>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(59),
+		     PXA_FUNCTION(0, 2, "LDD<1>"),
+		     PXA_FUNCTION(1, 2, "LDD<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(60),
+		     PXA_FUNCTION(0, 2, "LDD<2>"),
+		     PXA_FUNCTION(1, 2, "LDD<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(61),
+		     PXA_FUNCTION(0, 2, "LDD<3>"),
+		     PXA_FUNCTION(1, 2, "LDD<3>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(62),
+		     PXA_FUNCTION(0, 2, "LDD<4>"),
+		     PXA_FUNCTION(1, 2, "LDD<4>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(63),
+		     PXA_FUNCTION(0, 2, "LDD<5>"),
+		     PXA_FUNCTION(1, 2, "LDD<5>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(64),
+		     PXA_FUNCTION(0, 2, "LDD<6>"),
+		     PXA_FUNCTION(1, 2, "LDD<6>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(65),
+		     PXA_FUNCTION(0, 2, "LDD<7>"),
+		     PXA_FUNCTION(1, 2, "LDD<7>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(66),
+		     PXA_FUNCTION(0, 2, "LDD<8>"),
+		     PXA_FUNCTION(1, 2, "LDD<8>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(67),
+		     PXA_FUNCTION(0, 2, "LDD<9>"),
+		     PXA_FUNCTION(1, 2, "LDD<9>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(68),
+		     PXA_FUNCTION(0, 2, "LDD<10>"),
+		     PXA_FUNCTION(1, 2, "LDD<10>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(69),
+		     PXA_FUNCTION(0, 2, "LDD<11>"),
+		     PXA_FUNCTION(1, 2, "LDD<11>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(70),
+		     PXA_FUNCTION(0, 2, "LDD<12>"),
+		     PXA_FUNCTION(1, 2, "LDD<12>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(71),
+		     PXA_FUNCTION(0, 2, "LDD<13>"),
+		     PXA_FUNCTION(1, 2, "LDD<13>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(72),
+		     PXA_FUNCTION(0, 2, "LDD<14>"),
+		     PXA_FUNCTION(1, 2, "LDD<14>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(73),
+		     PXA_FUNCTION(0, 2, "LDD<15>"),
+		     PXA_FUNCTION(1, 2, "LDD<15>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(74),
+		     PXA_FUNCTION(1, 2, "L_FCLK_RD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(75),
+		     PXA_FUNCTION(1, 2, "L_LCLK_A0")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(76),
+		     PXA_FUNCTION(1, 2, "L_PCLK_WR")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(77),
+		     PXA_FUNCTION(1, 2, "L_BIAS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(78),
+		     PXA_FUNCTION(1, 1, "nPCE<2>"),
+		     PXA_FUNCTION(1, 2, "nCS<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(79),
+		     PXA_FUNCTION(1, 1, "PSKTSEL"),
+		     PXA_FUNCTION(1, 2, "nCS<3>"),
+		     PXA_FUNCTION(1, 3, "PWM_OUT<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(80),
+		     PXA_FUNCTION(0, 1, "DREQ<1>"),
+		     PXA_FUNCTION(0, 2, "MBREQ"),
+		     PXA_FUNCTION(1, 2, "nCS<4>"),
+		     PXA_FUNCTION(1, 3, "PWM_OUT<3>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(81),
+		     PXA_FUNCTION(0, 2, "CIF_DD<0>"),
+		     PXA_FUNCTION(1, 1, "SSPTXD3"),
+		     PXA_FUNCTION(1, 2, "BB_OB_DAT<0>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(82),
+		     PXA_FUNCTION(0, 1, "SSPRXD3"),
+		     PXA_FUNCTION(0, 2, "BB_IB_DAT<0>"),
+		     PXA_FUNCTION(0, 3, "CIF_DD<5>"),
+		     PXA_FUNCTION(1, 3, "FFDTR")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(83),
+		     PXA_FUNCTION(0, 1, "SSPSFRM3"),
+		     PXA_FUNCTION(0, 2, "BB_IB_CLK"),
+		     PXA_FUNCTION(0, 3, "CIF_DD<5>"),
+		     PXA_FUNCTION(1, 1, "SSPSFRM3"),
+		     PXA_FUNCTION(1, 2, "FFTXD"),
+		     PXA_FUNCTION(1, 3, "FFRTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(84),
+		     PXA_FUNCTION(0, 1, "SSPCLK3"),
+		     PXA_FUNCTION(0, 2, "BB_IB_STB"),
+		     PXA_FUNCTION(0, 3, "CIF_FV"),
+		     PXA_FUNCTION(1, 1, "SSPCLK3"),
+		     PXA_FUNCTION(1, 3, "CIF_FV")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(85),
+		     PXA_FUNCTION(0, 1, "FFRXD"),
+		     PXA_FUNCTION(0, 2, "DREQ<2>"),
+		     PXA_FUNCTION(0, 3, "CIF_LV"),
+		     PXA_FUNCTION(1, 1, "nPCE<1>"),
+		     PXA_FUNCTION(1, 2, "BB_IB_WAIT"),
+		     PXA_FUNCTION(1, 3, "CIF_LV")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(86),
+		     PXA_FUNCTION(0, 1, "SSPRXD2"),
+		     PXA_FUNCTION(0, 2, "LDD<16>"),
+		     PXA_FUNCTION(0, 3, "USB_P3_5"),
+		     PXA_FUNCTION(1, 1, "nPCE<1>"),
+		     PXA_FUNCTION(1, 2, "LDD<16>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(87),
+		     PXA_FUNCTION(0, 1, "nPCE<2>"),
+		     PXA_FUNCTION(0, 2, "LDD<17>"),
+		     PXA_FUNCTION(0, 3, "USB_P3_1"),
+		     PXA_FUNCTION(1, 1, "SSPTXD2"),
+		     PXA_FUNCTION(1, 2, "LDD<17>"),
+		     PXA_FUNCTION(1, 3, "SSPSFRM2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(88),
+		     PXA_FUNCTION(0, 1, "USBHPWR<1>"),
+		     PXA_FUNCTION(0, 2, "SSPRXD2"),
+		     PXA_FUNCTION(0, 3, "SSPSFRM2"),
+		     PXA_FUNCTION(1, 2, "SSPTXD2"),
+		     PXA_FUNCTION(1, 3, "SSPSFRM2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(89),
+		     PXA_FUNCTION(0, 1, "SSPRXD3"),
+		     PXA_FUNCTION(0, 3, "FFRI"),
+		     PXA_FUNCTION(1, 1, "AC97_SYSCLK"),
+		     PXA_FUNCTION(1, 2, "USBHPEN<1>"),
+		     PXA_FUNCTION(1, 3, "SSPTXD2")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(90),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<5>"),
+		     PXA_FUNCTION(0, 3, "USB_P3_5"),
+		     PXA_FUNCTION(1, 1, "CIF_DD<4>"),
+		     PXA_FUNCTION(1, 2, "nURST")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(91),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<6>"),
+		     PXA_FUNCTION(0, 3, "USB_P3_1"),
+		     PXA_FUNCTION(1, 1, "CIF_DD<5>"),
+		     PXA_FUNCTION(1, 2, "UCLK")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(92),
+		     PXA_FUNCTION(0, 1, "MMDAT<0>"),
+		     PXA_FUNCTION(1, 1, "MMDAT<0>"),
+		     PXA_FUNCTION(1, 2, "MSBS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(93),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<0>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<6>"),
+		     PXA_FUNCTION(1, 1, "AC97_SDATA_OUT")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(94),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<1>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<5>"),
+		     PXA_FUNCTION(1, 1, "AC97_SYNC")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(95),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<2>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<4>"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<6>"),
+		     PXA_FUNCTION(1, 1, "AC97_RESET_n")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(96),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<3>"),
+		     PXA_FUNCTION(0, 2, "MBREQ"),
+		     PXA_FUNCTION(0, 3, "FFRXD"),
+		     PXA_FUNCTION(1, 2, "DVAL<1>"),
+		     PXA_FUNCTION(1, 3, "KP_MKOUT<6>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(97),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<4>"),
+		     PXA_FUNCTION(0, 2, "DREQ<1>"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<3>"),
+		     PXA_FUNCTION(1, 2, "MBGNT")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(98),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<5>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<0>"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<4>"),
+		     PXA_FUNCTION(1, 1, "AC97_SYSCLK"),
+		     PXA_FUNCTION(1, 3, "FFRTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(99),
+		     PXA_FUNCTION(0, 1, "KP_DKIN<6>"),
+		     PXA_FUNCTION(0, 2, "AC97_SDATA_IN_1"),
+		     PXA_FUNCTION(0, 3, "KP_MKIN<5>"),
+		     PXA_FUNCTION(1, 3, "FFTXD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(100),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<0>"),
+		     PXA_FUNCTION(0, 2, "DREQ<2>"),
+		     PXA_FUNCTION(0, 3, "FFCTS")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(101),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(102),
+		     PXA_FUNCTION(0, 1, "KP_MKIN<2>"),
+		     PXA_FUNCTION(0, 3, "FFRXD"),
+		     PXA_FUNCTION(1, 1, "nPCE<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(103),
+		     PXA_FUNCTION(0, 1, "CIF_DD<3>"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<0>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(104),
+		     PXA_FUNCTION(0, 1, "CIF_DD<2>"),
+		     PXA_FUNCTION(1, 1, "PSKTSEL"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(105),
+		     PXA_FUNCTION(0, 1, "CIF_DD<1>"),
+		     PXA_FUNCTION(1, 1, "nPCE<2>"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(106),
+		     PXA_FUNCTION(0, 1, "CIF_DD<9>"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<3>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(107),
+		     PXA_FUNCTION(0, 1, "CIF_DD<8>"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<4>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(108),
+		     PXA_FUNCTION(0, 1, "CIF_DD<7>"),
+		     PXA_FUNCTION(1, 1, "CHOUT<0>"),
+		     PXA_FUNCTION(1, 2, "KP_MKOUT<5>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(109),
+		     PXA_FUNCTION(0, 1, "MMDAT<1>"),
+		     PXA_FUNCTION(0, 2, "MSSDIO"),
+		     PXA_FUNCTION(1, 1, "MMDAT<1>"),
+		     PXA_FUNCTION(1, 2, "MSSDIO")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(110),
+		     PXA_FUNCTION(0, 1, "MMDAT<2>"),
+		     PXA_FUNCTION(1, 1, "MMDAT<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(111),
+		     PXA_FUNCTION(0, 1, "MMDAT<3>"),
+		     PXA_FUNCTION(1, 1, "MMDAT<3>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(112),
+		     PXA_FUNCTION(0, 1, "MMCMD"),
+		     PXA_FUNCTION(0, 2, "nMSINS"),
+		     PXA_FUNCTION(1, 1, "MMCMD")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(113),
+		     PXA_FUNCTION(0, 3, "USB_P3_3"),
+		     PXA_FUNCTION(1, 1, "I2S_SYSCLK"),
+		     PXA_FUNCTION(1, 2, "AC97_RESET_n")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(114),
+		     PXA_FUNCTION(0, 1, "CIF_DD<1>"),
+		     PXA_FUNCTION(1, 1, "UEN"),
+		     PXA_FUNCTION(1, 2, "UVS0")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(115),
+		     PXA_FUNCTION(0, 1, "DREQ<0>"),
+		     PXA_FUNCTION(0, 2, "CIF_DD<3>"),
+		     PXA_FUNCTION(0, 3, "MBREQ"),
+		     PXA_FUNCTION(1, 1, "UEN"),
+		     PXA_FUNCTION(1, 2, "nUVS1"),
+		     PXA_FUNCTION(1, 3, "PWM_OUT<1>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(116),
+		     PXA_FUNCTION(0, 1, "CIF_DD<2>"),
+		     PXA_FUNCTION(0, 2, "AC97_SDATA_IN_0"),
+		     PXA_FUNCTION(0, 3, "UDET"),
+		     PXA_FUNCTION(1, 1, "DVAL<0>"),
+		     PXA_FUNCTION(1, 2, "nUVS2"),
+		     PXA_FUNCTION(1, 3, "MBGNT")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(117),
+		     PXA_FUNCTION(0, 1, "SCL"),
+		     PXA_FUNCTION(1, 1, "SCL")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(118),
+		     PXA_FUNCTION(0, 1, "SDA"),
+		     PXA_FUNCTION(1, 1, "SDA")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(119),
+		     PXA_FUNCTION(0, 1, "USBHPWR<2>")),
+	PXA_GPIO_PIN(PXA_PINCTRL_PIN(120),
+		     PXA_FUNCTION(1, 2, "USBHPEN<2>")),
+};
+
+static int pxa27x_pinctrl_probe(struct platform_device *pdev)
+{
+	int ret, i;
+	void __iomem *base_af[8];
+	void __iomem *base_dir[4];
+	void __iomem *base_sleep[4];
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base_af[0] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base_af[0]))
+		return PTR_ERR(base_af[0]);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	base_dir[0] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base_dir[0]))
+		return PTR_ERR(base_dir[0]);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	base_dir[3] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base_dir[3]))
+		return PTR_ERR(base_dir[3]);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 3);
+	base_sleep[0] = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base_sleep[0]))
+		return PTR_ERR(base_sleep[0]);
+
+	for (i = 0; i < ARRAY_SIZE(base_af); i++)
+		base_af[i] = base_af[0] + sizeof(base_af[0]) * i;
+	for (i = 0; i < 3; i++)
+		base_dir[i] = base_dir[0] + sizeof(base_dir[0]) * i;
+	for (i = 0; i < ARRAY_SIZE(base_sleep); i++)
+		base_sleep[i] = base_sleep[0] + sizeof(base_af[0]) * i;
+
+	ret = pxa2xx_pinctrl_init(pdev, pxa27x_pins, ARRAY_SIZE(pxa27x_pins),
+				  base_af, base_dir, base_sleep);
+	return ret;
+}
+
+static const struct of_device_id pxa27x_pinctrl_match[] = {
+	{ .compatible = "marvell,pxa27x-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, pxa27x_pinctrl_match);
+
+static struct platform_driver pxa27x_pinctrl_driver = {
+	.probe	= pxa27x_pinctrl_probe,
+	.driver	= {
+		.name		= "pxa27x-pinctrl",
+		.of_match_table	= pxa27x_pinctrl_match,
+	},
+};
+module_platform_driver(pxa27x_pinctrl_driver);
+
+MODULE_AUTHOR("Robert Jarzmik <robert.jarzmik@free.fr>");
+MODULE_DESCRIPTION("Marvell PXA27x pinctrl driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pinctrl/pxa/pinctrl-pxa2xx.c b/drivers/pinctrl/pxa/pinctrl-pxa2xx.c
new file mode 100644
index 000000000000..216f227c6009
--- /dev/null
+++ b/drivers/pinctrl/pxa/pinctrl-pxa2xx.c
@@ -0,0 +1,437 @@
+/*
+ * Marvell PXA2xx family pin control
+ *
+ * Copyright (C) 2015 Robert Jarzmik
+ *
+ * This program is free software; 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/bitops.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/module.h>
+#include <linux/pinctrl/pinconf.h>
+#include <linux/pinctrl/pinconf-generic.h>
+#include <linux/pinctrl/pinmux.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include "../pinctrl-utils.h"
+#include "pinctrl-pxa2xx.h"
+
+static int pxa2xx_pctrl_get_groups_count(struct pinctrl_dev *pctldev)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->ngroups;
+}
+
+static const char *pxa2xx_pctrl_get_group_name(struct pinctrl_dev *pctldev,
+					       unsigned tgroup)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_group *group = pctl->groups + tgroup;
+
+	return group->name;
+}
+
+static int pxa2xx_pctrl_get_group_pins(struct pinctrl_dev *pctldev,
+				       unsigned tgroup,
+				       const unsigned **pins,
+				       unsigned *num_pins)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_group *group = pctl->groups + tgroup;
+
+	*pins = (unsigned *)&group->pin;
+	*num_pins = 1;
+
+	return 0;
+}
+
+static const struct pinctrl_ops pxa2xx_pctl_ops = {
+#ifdef CONFIG_OF
+	.dt_node_to_map		= pinconf_generic_dt_node_to_map_all,
+	.dt_free_map		= pinctrl_utils_dt_free_map,
+#endif
+	.get_groups_count	= pxa2xx_pctrl_get_groups_count,
+	.get_group_name		= pxa2xx_pctrl_get_group_name,
+	.get_group_pins		= pxa2xx_pctrl_get_group_pins,
+};
+
+static struct pxa_desc_function *
+pxa_desc_by_func_group(struct pxa_pinctrl *pctl, const char *pin_name,
+		       const char *func_name)
+{
+	int i;
+	struct pxa_desc_function *df;
+
+	for (i = 0; i < pctl->npins; i++) {
+		const struct pxa_desc_pin *pin = pctl->ppins + i;
+
+		if (!strcmp(pin->pin.name, pin_name))
+			for (df = pin->functions; df->name; df++)
+				if (!strcmp(df->name, func_name))
+					return df;
+	}
+
+	return NULL;
+}
+
+static int pxa2xx_pmx_gpio_set_direction(struct pinctrl_dev *pctldev,
+					 struct pinctrl_gpio_range *range,
+					 unsigned pin,
+					 bool input)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	unsigned long flags;
+	uint32_t val;
+	void __iomem *gpdr;
+
+	gpdr = pctl->base_gpdr[pin / 32];
+	dev_dbg(pctl->dev, "set_direction(pin=%d): dir=%d\n",
+		pin, !input);
+
+	spin_lock_irqsave(&pctl->lock, flags);
+
+	val = readl_relaxed(gpdr);
+	val = (val & ~BIT(pin % 32)) | (input ? 0 : BIT(pin % 32));
+	writel_relaxed(val, gpdr);
+
+	spin_unlock_irqrestore(&pctl->lock, flags);
+
+	return 0;
+}
+
+static const char *pxa2xx_pmx_get_func_name(struct pinctrl_dev *pctldev,
+					    unsigned function)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_function *pf = pctl->functions + function;
+
+	return pf->name;
+}
+
+static int pxa2xx_get_functions_count(struct pinctrl_dev *pctldev)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+
+	return pctl->nfuncs;
+}
+
+static int pxa2xx_pmx_get_func_groups(struct pinctrl_dev *pctldev,
+				      unsigned function,
+				      const char * const **groups,
+				      unsigned * const num_groups)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_function *pf = pctl->functions + function;
+
+	*groups = pf->groups;
+	*num_groups = pf->ngroups;
+
+	return 0;
+}
+
+static int pxa2xx_pmx_set_mux(struct pinctrl_dev *pctldev, unsigned function,
+			      unsigned tgroup)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_group *group = pctl->groups + tgroup;
+	struct pxa_desc_function *df;
+	int pin, shift;
+	unsigned long flags;
+	void __iomem *gafr, *gpdr;
+	u32 val;
+
+
+	df = pxa_desc_by_func_group(pctl, group->name,
+				    (pctl->functions + function)->name);
+	if (!df)
+		return -EINVAL;
+
+	pin = group->pin;
+	gafr = pctl->base_gafr[pin / 16];
+	gpdr = pctl->base_gpdr[pin / 32];
+	shift = (pin % 16) << 1;
+	dev_dbg(pctl->dev, "set_mux(pin=%d): af=%d dir=%d\n",
+		pin, df->muxval >> 1, df->muxval & 0x1);
+
+	spin_lock_irqsave(&pctl->lock, flags);
+
+	val = readl_relaxed(gafr);
+	val = (val & ~(0x3 << shift)) | ((df->muxval >> 1) << shift);
+	writel_relaxed(val, gafr);
+
+	val = readl_relaxed(gpdr);
+	val = (val & ~BIT(pin % 32)) | ((df->muxval & 1) ? BIT(pin % 32) : 0);
+	writel_relaxed(val, gpdr);
+
+	spin_unlock_irqrestore(&pctl->lock, flags);
+
+	return 0;
+}
+static const struct pinmux_ops pxa2xx_pinmux_ops = {
+	.get_functions_count = pxa2xx_get_functions_count,
+	.get_function_name = pxa2xx_pmx_get_func_name,
+	.get_function_groups = pxa2xx_pmx_get_func_groups,
+	.set_mux = pxa2xx_pmx_set_mux,
+	.gpio_set_direction = pxa2xx_pmx_gpio_set_direction,
+};
+
+static int pxa2xx_pconf_group_get(struct pinctrl_dev *pctldev,
+				  unsigned group,
+				  unsigned long *config)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_group *g = pctl->groups + group;
+	unsigned long flags;
+	unsigned pin = g->pin;
+	void __iomem *pgsr = pctl->base_pgsr[pin / 32];
+	u32 val;
+
+	spin_lock_irqsave(&pctl->lock, flags);
+	val = readl_relaxed(pgsr) & BIT(pin % 32);
+	*config = val ? PIN_CONFIG_LOW_POWER_MODE : 0;
+	spin_unlock_irqrestore(&pctl->lock, flags);
+
+	dev_dbg(pctl->dev, "get sleep gpio state(pin=%d) %d\n",
+		pin, !!val);
+	return 0;
+}
+
+static int pxa2xx_pconf_group_set(struct pinctrl_dev *pctldev,
+				  unsigned group,
+				  unsigned long *configs,
+				  unsigned num_configs)
+{
+	struct pxa_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
+	struct pxa_pinctrl_group *g = pctl->groups + group;
+	unsigned long flags;
+	unsigned pin = g->pin;
+	void __iomem *pgsr = pctl->base_pgsr[pin / 32];
+	int i, is_set = 0;
+	u32 val;
+
+	for (i = 0; i < num_configs; i++) {
+		switch (pinconf_to_config_param(configs[i])) {
+		case PIN_CONFIG_LOW_POWER_MODE:
+			is_set = pinconf_to_config_argument(configs[i]);
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	dev_dbg(pctl->dev, "set sleep gpio state(pin=%d) %d\n",
+		pin, is_set);
+
+	spin_lock_irqsave(&pctl->lock, flags);
+	val = readl_relaxed(pgsr);
+	val = (val & ~BIT(pin % 32)) | (is_set ? BIT(pin % 32) : 0);
+	writel_relaxed(val, pgsr);
+	spin_unlock_irqrestore(&pctl->lock, flags);
+
+	return 0;
+}
+
+static const struct pinconf_ops pxa2xx_pconf_ops = {
+	.pin_config_group_get	= pxa2xx_pconf_group_get,
+	.pin_config_group_set	= pxa2xx_pconf_group_set,
+	.is_generic		= true,
+};
+
+static struct pinctrl_desc pxa2xx_pinctrl_desc = {
+	.confops	= &pxa2xx_pconf_ops,
+	.pctlops	= &pxa2xx_pctl_ops,
+	.pmxops		= &pxa2xx_pinmux_ops,
+};
+
+static const struct pxa_pinctrl_function *
+pxa2xx_find_function(struct pxa_pinctrl *pctl, const char *fname,
+		     const struct pxa_pinctrl_function *functions)
+{
+	const struct pxa_pinctrl_function *func;
+
+	for (func = functions; func->name; func++)
+		if (!strcmp(fname, func->name))
+			return func;
+
+	return NULL;
+}
+
+static int pxa2xx_build_functions(struct pxa_pinctrl *pctl)
+{
+	int i;
+	struct pxa_pinctrl_function *functions;
+	struct pxa_desc_function *df;
+
+	/*
+	 * Each pin can have at most 6 alternate functions, and 2 gpio functions
+	 * which are common to each pin. As there are more than 2 pins without
+	 * alternate function, 6 * npins is an absolute high limit of the number
+	 * of functions.
+	 */
+	functions = devm_kcalloc(pctl->dev, pctl->npins * 6,
+				 sizeof(*functions), GFP_KERNEL);
+	if (!functions)
+		return -ENOMEM;
+
+	for (i = 0; i < pctl->npins; i++)
+		for (df = pctl->ppins[i].functions; df->name; df++)
+			if (!pxa2xx_find_function(pctl, df->name, functions))
+				(functions + pctl->nfuncs++)->name = df->name;
+	pctl->functions = devm_kmemdup(pctl->dev, functions,
+				       pctl->nfuncs * sizeof(*functions),
+				       GFP_KERNEL);
+	if (!pctl->functions)
+		return -ENOMEM;
+
+	devm_kfree(pctl->dev, functions);
+	return 0;
+}
+
+static int pxa2xx_build_groups(struct pxa_pinctrl *pctl)
+{
+	int i, j, ngroups;
+	struct pxa_pinctrl_function *func;
+	struct pxa_desc_function *df;
+	char **gtmp;
+
+	gtmp = devm_kmalloc_array(pctl->dev, pctl->npins, sizeof(*gtmp),
+				  GFP_KERNEL);
+	if (!gtmp)
+		return -ENOMEM;
+
+	for (i = 0; i < pctl->nfuncs; i++) {
+		ngroups = 0;
+		for (j = 0; j < pctl->npins; j++)
+			for (df = pctl->ppins[j].functions; df->name;
+			     df++)
+				if (!strcmp(pctl->functions[i].name,
+					    df->name))
+					gtmp[ngroups++] = (char *)
+						pctl->ppins[j].pin.name;
+		func = pctl->functions + i;
+		func->ngroups = ngroups;
+		func->groups =
+			devm_kmalloc_array(pctl->dev, ngroups,
+					   sizeof(char *), GFP_KERNEL);
+		if (!func->groups)
+			return -ENOMEM;
+
+		memcpy(func->groups, gtmp, ngroups * sizeof(*gtmp));
+	}
+
+	devm_kfree(pctl->dev, gtmp);
+	return 0;
+}
+
+static int pxa2xx_build_state(struct pxa_pinctrl *pctl,
+			      const struct pxa_desc_pin *ppins, int npins)
+{
+	struct pxa_pinctrl_group *group;
+	struct pinctrl_pin_desc *pins;
+	int ret, i;
+
+	pctl->npins = npins;
+	pctl->ppins = ppins;
+	pctl->ngroups = npins;
+
+	pctl->desc.npins = npins;
+	pins = devm_kcalloc(pctl->dev, npins, sizeof(*pins), GFP_KERNEL);
+	if (!pins)
+		return -ENOMEM;
+
+	pctl->desc.pins = pins;
+	for (i = 0; i < npins; i++)
+		pins[i] = ppins[i].pin;
+
+	pctl->groups = devm_kmalloc_array(pctl->dev, pctl->ngroups,
+					  sizeof(*pctl->groups), GFP_KERNEL);
+	if (!pctl->groups)
+		return -ENOMEM;
+
+	for (i = 0; i < npins; i++) {
+		group = pctl->groups + i;
+		group->name = ppins[i].pin.name;
+		group->pin = ppins[i].pin.number;
+	}
+
+	ret = pxa2xx_build_functions(pctl);
+	if (ret)
+		return ret;
+
+	ret = pxa2xx_build_groups(pctl);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+int pxa2xx_pinctrl_init(struct platform_device *pdev,
+			const struct pxa_desc_pin *ppins, int npins,
+			void __iomem *base_gafr[], void __iomem *base_gpdr[],
+			void __iomem *base_pgsr[])
+{
+	struct pxa_pinctrl *pctl;
+	int ret, i, maxpin = 0;
+
+	for (i = 0; i < npins; i++)
+		maxpin = max_t(int, ppins[i].pin.number, maxpin);
+
+	pctl = devm_kzalloc(&pdev->dev, sizeof(*pctl), GFP_KERNEL);
+	if (!pctl)
+		return -ENOMEM;
+	pctl->base_gafr = devm_kcalloc(&pdev->dev, roundup(maxpin, 16),
+				       sizeof(*pctl->base_gafr), GFP_KERNEL);
+	pctl->base_gpdr = devm_kcalloc(&pdev->dev, roundup(maxpin, 32),
+				       sizeof(*pctl->base_gpdr), GFP_KERNEL);
+	pctl->base_pgsr = devm_kcalloc(&pdev->dev, roundup(maxpin, 32),
+				       sizeof(*pctl->base_pgsr), GFP_KERNEL);
+	if (!pctl->base_gafr || !pctl->base_gpdr || !pctl->base_pgsr)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, pctl);
+	spin_lock_init(&pctl->lock);
+
+	pctl->dev = &pdev->dev;
+	pctl->desc = pxa2xx_pinctrl_desc;
+	pctl->desc.name = dev_name(&pdev->dev);
+	pctl->desc.owner = THIS_MODULE;
+
+	for (i = 0; i < roundup(maxpin, 16); i += 16)
+		pctl->base_gafr[i / 16] = base_gafr[i / 16];
+	for (i = 0; i < roundup(maxpin, 32); i += 32) {
+		pctl->base_gpdr[i / 32] = base_gpdr[i / 32];
+		pctl->base_pgsr[i / 32] = base_pgsr[i / 32];
+	}
+
+	ret = pxa2xx_build_state(pctl, ppins, npins);
+	if (ret)
+		return ret;
+
+	pctl->pctl_dev = pinctrl_register(&pctl->desc, &pdev->dev, pctl);
+	if (IS_ERR(pctl->pctl_dev)) {
+		dev_err(&pdev->dev, "couldn't register pinctrl driver\n");
+		return PTR_ERR(pctl->pctl_dev);
+	}
+
+	dev_info(&pdev->dev, "initialized pxa2xx pinctrl driver\n");
+
+	return 0;
+}
+EXPORT_SYMBOL(pxa2xx_pinctrl_init);
+
+int pxa2xx_pinctrl_exit(struct platform_device *pdev)
+{
+	struct pxa_pinctrl *pctl = platform_get_drvdata(pdev);
+
+	pinctrl_unregister(pctl->pctl_dev);
+	return 0;
+}
diff --git a/drivers/pinctrl/pxa/pinctrl-pxa2xx.h b/drivers/pinctrl/pxa/pinctrl-pxa2xx.h
new file mode 100644
index 000000000000..8be1e0b79751
--- /dev/null
+++ b/drivers/pinctrl/pxa/pinctrl-pxa2xx.h
@@ -0,0 +1,92 @@
+/*
+ * Marvell PXA2xx family pin control
+ *
+ * Copyright (C) 2015 Robert Jarzmik
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ */
+
+#ifndef __PINCTRL_PXA_H
+#define __PINCTRL_PXA_H
+
+#define PXA_FUNCTION(_dir, _af, _name)				\
+	{							\
+		.name = _name,					\
+		.muxval = (_dir | (_af << 1)),			\
+	}
+
+#define PXA_PIN(_pin, funcs...)					\
+	{							\
+		.pin = _pin,					\
+		.functions = (struct pxa_desc_function[]){	\
+			funcs, { } },				\
+	}
+
+#define PXA_GPIO_PIN(_pin, funcs...)				\
+	{							\
+		.pin = _pin,					\
+		.functions = (struct pxa_desc_function[]){	\
+			PXA_FUNCTION(0, 0, "gpio_in"),		\
+			PXA_FUNCTION(1, 0, "gpio_out"),		\
+			funcs, { } },				\
+	}
+
+#define PXA_GPIO_ONLY_PIN(_pin)					\
+	{							\
+		.pin = _pin,					\
+		.functions = (struct pxa_desc_function[]){	\
+			PXA_FUNCTION(0, 0, "gpio_in"),		\
+			PXA_FUNCTION(1, 0, "gpio_out"),		\
+			{ } },					\
+	}
+
+#define PXA_PINCTRL_PIN(pin)		\
+	PINCTRL_PIN(pin, "P" #pin)
+
+struct pxa_desc_function {
+	const char	*name;
+	u8		muxval;
+};
+
+struct pxa_desc_pin {
+	struct pinctrl_pin_desc		pin;
+	struct pxa_desc_function	*functions;
+};
+
+struct pxa_pinctrl_group {
+	const char	*name;
+	unsigned	pin;
+};
+
+struct pxa_pinctrl_function {
+	const char	*name;
+	const char	**groups;
+	unsigned	ngroups;
+};
+
+struct pxa_pinctrl {
+	spinlock_t			lock;
+	void __iomem			**base_gafr;
+	void __iomem			**base_gpdr;
+	void __iomem			**base_pgsr;
+	struct device			*dev;
+	struct pinctrl_desc		desc;
+	struct pinctrl_dev		*pctl_dev;
+	unsigned			npins;
+	const struct pxa_desc_pin	*ppins;
+	unsigned			ngroups;
+	struct pxa_pinctrl_group	*groups;
+	unsigned			nfuncs;
+	struct pxa_pinctrl_function	*functions;
+	char				*name;
+};
+
+int pxa2xx_pinctrl_init(struct platform_device *pdev,
+			const struct pxa_desc_pin *ppins, int npins,
+			void __iomem *base_gafr[], void __iomem *base_gpdr[],
+			void __iomem *base_gpsr[]);
+
+#endif /* __PINCTRL_PXA_H */
diff --git a/drivers/pinctrl/qcom/Kconfig b/drivers/pinctrl/qcom/Kconfig
index 383263a92e59..eeac8cba8a21 100644
--- a/drivers/pinctrl/qcom/Kconfig
+++ b/drivers/pinctrl/qcom/Kconfig
@@ -63,6 +63,14 @@ config PINCTRL_MSM8916
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
 	  Qualcomm TLMM block found on the Qualcomm 8916 platform.
 
+config PINCTRL_MSM8996
+	tristate "Qualcomm MSM8996 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
+	  Qualcomm TLMM block found in the Qualcomm MSM8996 platform.
+
 config PINCTRL_QDF2XXX
 	tristate "Qualcomm Technologies QDF2xxx pin controller driver"
 	depends on GPIOLIB && ACPI
diff --git a/drivers/pinctrl/qcom/Makefile b/drivers/pinctrl/qcom/Makefile
index 13b190e72c21..dfb50a9fe04a 100644
--- a/drivers/pinctrl/qcom/Makefile
+++ b/drivers/pinctrl/qcom/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_PINCTRL_MSM8660)	+= pinctrl-msm8660.o
 obj-$(CONFIG_PINCTRL_MSM8960)	+= pinctrl-msm8960.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
 obj-$(CONFIG_PINCTRL_MSM8916)	+= pinctrl-msm8916.o
+obj-$(CONFIG_PINCTRL_MSM8996)   += pinctrl-msm8996.o
 obj-$(CONFIG_PINCTRL_QDF2XXX)	+= pinctrl-qdf2xxx.o
 obj-$(CONFIG_PINCTRL_QCOM_SPMI_PMIC) += pinctrl-spmi-gpio.o
 obj-$(CONFIG_PINCTRL_QCOM_SPMI_PMIC) += pinctrl-spmi-mpp.o
diff --git a/drivers/pinctrl/qcom/pinctrl-msm.c b/drivers/pinctrl/qcom/pinctrl-msm.c
index 146264a41ec8..8777cf083eef 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm.c
@@ -69,11 +69,6 @@ struct msm_pinctrl {
 	void __iomem *regs;
 };
 
-static inline struct msm_pinctrl *to_msm_pinctrl(struct gpio_chip *gc)
-{
-	return container_of(gc, struct msm_pinctrl, chip);
-}
-
 static int msm_get_groups_count(struct pinctrl_dev *pctldev)
 {
 	struct msm_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
@@ -381,7 +376,7 @@ static struct pinctrl_desc msm_pinctrl_desc = {
 static int msm_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -401,7 +396,7 @@ static int msm_gpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int msm_gpio_direction_output(struct gpio_chip *chip, unsigned offset, int value)
 {
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -428,7 +423,7 @@ static int msm_gpio_direction_output(struct gpio_chip *chip, unsigned offset, in
 static int msm_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(chip);
 	u32 val;
 
 	g = &pctrl->soc->groups[offset];
@@ -440,7 +435,7 @@ static int msm_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void msm_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned long flags;
 	u32 val;
 
@@ -468,7 +463,7 @@ static void msm_gpio_dbg_show_one(struct seq_file *s,
 				  unsigned gpio)
 {
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = container_of(chip, struct msm_pinctrl, chip);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(chip);
 	unsigned func;
 	int is_out;
 	int drive;
@@ -567,7 +562,7 @@ static void msm_gpio_update_dual_edge_pos(struct msm_pinctrl *pctrl,
 static void msm_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct msm_pingroup *g;
 	unsigned long flags;
 	u32 val;
@@ -588,7 +583,7 @@ static void msm_gpio_irq_mask(struct irq_data *d)
 static void msm_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct msm_pingroup *g;
 	unsigned long flags;
 	u32 val;
@@ -613,7 +608,7 @@ static void msm_gpio_irq_unmask(struct irq_data *d)
 static void msm_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct msm_pingroup *g;
 	unsigned long flags;
 	u32 val;
@@ -638,7 +633,7 @@ static void msm_gpio_irq_ack(struct irq_data *d)
 static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	const struct msm_pingroup *g;
 	unsigned long flags;
 	u32 val;
@@ -732,7 +727,7 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 static int msm_gpio_irq_set_wake(struct irq_data *d, unsigned int on)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&pctrl->lock, flags);
@@ -757,7 +752,7 @@ static void msm_gpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
 	const struct msm_pingroup *g;
-	struct msm_pinctrl *pctrl = to_msm_pinctrl(gc);
+	struct msm_pinctrl *pctrl = gpiochip_get_data(gc);
 	struct irq_chip *chip = irq_desc_get_chip(desc);
 	int irq_pin;
 	int handled = 0;
@@ -800,11 +795,11 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 	chip->base = 0;
 	chip->ngpio = ngpio;
 	chip->label = dev_name(pctrl->dev);
-	chip->dev = pctrl->dev;
+	chip->parent = pctrl->dev;
 	chip->owner = THIS_MODULE;
 	chip->of_node = pctrl->dev->of_node;
 
-	ret = gpiochip_add(&pctrl->chip);
+	ret = gpiochip_add_data(&pctrl->chip, pctrl);
 	if (ret) {
 		dev_err(pctrl->dev, "Failed register gpiochip\n");
 		return ret;
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8996.c b/drivers/pinctrl/qcom/pinctrl-msm8996.c
new file mode 100644
index 000000000000..c257927bea05
--- /dev/null
+++ b/drivers/pinctrl/qcom/pinctrl-msm8996.c
@@ -0,0 +1,1942 @@
+/*
+ * Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+#define FUNCTION(fname)			                \
+	[msm_mux_##fname] = {		                \
+		.name = #fname,				\
+		.groups = fname##_groups,               \
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define REG_BASE 0x0
+#define REG_SIZE 0x1000
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7, f8, f9)	\
+	{					        \
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = (unsigned)ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			msm_mux_gpio, /* gpio mode */	\
+			msm_mux_##f1,			\
+			msm_mux_##f2,			\
+			msm_mux_##f3,			\
+			msm_mux_##f4,			\
+			msm_mux_##f5,			\
+			msm_mux_##f6,			\
+			msm_mux_##f7,			\
+			msm_mux_##f8,			\
+			msm_mux_##f9			\
+		},				        \
+		.nfuncs = 10,				\
+		.ctl_reg = REG_BASE + REG_SIZE * id,	\
+		.io_reg = REG_BASE + 0x4 + REG_SIZE * id,		\
+		.intr_cfg_reg = REG_BASE + 0x8 + REG_SIZE * id,		\
+		.intr_status_reg = REG_BASE + 0xc + REG_SIZE * id,	\
+		.intr_target_reg = REG_BASE + 0x8 + REG_SIZE * id,	\
+		.mux_bit = 2,			\
+		.pull_bit = 0,			\
+		.drv_bit = 6,			\
+		.oe_bit = 9,			\
+		.in_bit = 0,			\
+		.out_bit = 1,			\
+		.intr_enable_bit = 0,		\
+		.intr_status_bit = 0,		\
+		.intr_target_bit = 5,		\
+		.intr_target_kpss_val = 3,	\
+		.intr_raw_status_bit = 4,	\
+		.intr_polarity_bit = 1,		\
+		.intr_detection_bit = 2,	\
+		.intr_detection_width = 2,	\
+	}
+
+#define SDC_QDSD_PINGROUP(pg_name, ctl, pull, drv)	\
+	{					        \
+		.name = #pg_name,			\
+		.pins = pg_name##_pins,			\
+		.npins = (unsigned)ARRAY_SIZE(pg_name##_pins),	\
+		.ctl_reg = ctl,				\
+		.io_reg = 0,				\
+		.intr_cfg_reg = 0,			\
+		.intr_status_reg = 0,			\
+		.intr_target_reg = 0,			\
+		.mux_bit = -1,				\
+		.pull_bit = pull,			\
+		.drv_bit = drv,				\
+		.oe_bit = -1,				\
+		.in_bit = -1,				\
+		.out_bit = -1,				\
+		.intr_enable_bit = -1,			\
+		.intr_status_bit = -1,			\
+		.intr_target_bit = -1,			\
+		.intr_raw_status_bit = -1,		\
+		.intr_polarity_bit = -1,		\
+		.intr_detection_bit = -1,		\
+		.intr_detection_width = -1,		\
+	}
+static const struct pinctrl_pin_desc msm8996_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+	PINCTRL_PIN(70, "GPIO_70"),
+	PINCTRL_PIN(71, "GPIO_71"),
+	PINCTRL_PIN(72, "GPIO_72"),
+	PINCTRL_PIN(73, "GPIO_73"),
+	PINCTRL_PIN(74, "GPIO_74"),
+	PINCTRL_PIN(75, "GPIO_75"),
+	PINCTRL_PIN(76, "GPIO_76"),
+	PINCTRL_PIN(77, "GPIO_77"),
+	PINCTRL_PIN(78, "GPIO_78"),
+	PINCTRL_PIN(79, "GPIO_79"),
+	PINCTRL_PIN(80, "GPIO_80"),
+	PINCTRL_PIN(81, "GPIO_81"),
+	PINCTRL_PIN(82, "GPIO_82"),
+	PINCTRL_PIN(83, "GPIO_83"),
+	PINCTRL_PIN(84, "GPIO_84"),
+	PINCTRL_PIN(85, "GPIO_85"),
+	PINCTRL_PIN(86, "GPIO_86"),
+	PINCTRL_PIN(87, "GPIO_87"),
+	PINCTRL_PIN(88, "GPIO_88"),
+	PINCTRL_PIN(89, "GPIO_89"),
+	PINCTRL_PIN(90, "GPIO_90"),
+	PINCTRL_PIN(91, "GPIO_91"),
+	PINCTRL_PIN(92, "GPIO_92"),
+	PINCTRL_PIN(93, "GPIO_93"),
+	PINCTRL_PIN(94, "GPIO_94"),
+	PINCTRL_PIN(95, "GPIO_95"),
+	PINCTRL_PIN(96, "GPIO_96"),
+	PINCTRL_PIN(97, "GPIO_97"),
+	PINCTRL_PIN(98, "GPIO_98"),
+	PINCTRL_PIN(99, "GPIO_99"),
+	PINCTRL_PIN(100, "GPIO_100"),
+	PINCTRL_PIN(101, "GPIO_101"),
+	PINCTRL_PIN(102, "GPIO_102"),
+	PINCTRL_PIN(103, "GPIO_103"),
+	PINCTRL_PIN(104, "GPIO_104"),
+	PINCTRL_PIN(105, "GPIO_105"),
+	PINCTRL_PIN(106, "GPIO_106"),
+	PINCTRL_PIN(107, "GPIO_107"),
+	PINCTRL_PIN(108, "GPIO_108"),
+	PINCTRL_PIN(109, "GPIO_109"),
+	PINCTRL_PIN(110, "GPIO_110"),
+	PINCTRL_PIN(111, "GPIO_111"),
+	PINCTRL_PIN(112, "GPIO_112"),
+	PINCTRL_PIN(113, "GPIO_113"),
+	PINCTRL_PIN(114, "GPIO_114"),
+	PINCTRL_PIN(115, "GPIO_115"),
+	PINCTRL_PIN(116, "GPIO_116"),
+	PINCTRL_PIN(117, "GPIO_117"),
+	PINCTRL_PIN(118, "GPIO_118"),
+	PINCTRL_PIN(119, "GPIO_119"),
+	PINCTRL_PIN(120, "GPIO_120"),
+	PINCTRL_PIN(121, "GPIO_121"),
+	PINCTRL_PIN(122, "GPIO_122"),
+	PINCTRL_PIN(123, "GPIO_123"),
+	PINCTRL_PIN(124, "GPIO_124"),
+	PINCTRL_PIN(125, "GPIO_125"),
+	PINCTRL_PIN(126, "GPIO_126"),
+	PINCTRL_PIN(127, "GPIO_127"),
+	PINCTRL_PIN(128, "GPIO_128"),
+	PINCTRL_PIN(129, "GPIO_129"),
+	PINCTRL_PIN(130, "GPIO_130"),
+	PINCTRL_PIN(131, "GPIO_131"),
+	PINCTRL_PIN(132, "GPIO_132"),
+	PINCTRL_PIN(133, "GPIO_133"),
+	PINCTRL_PIN(134, "GPIO_134"),
+	PINCTRL_PIN(135, "GPIO_135"),
+	PINCTRL_PIN(136, "GPIO_136"),
+	PINCTRL_PIN(137, "GPIO_137"),
+	PINCTRL_PIN(138, "GPIO_138"),
+	PINCTRL_PIN(139, "GPIO_139"),
+	PINCTRL_PIN(140, "GPIO_140"),
+	PINCTRL_PIN(141, "GPIO_141"),
+	PINCTRL_PIN(142, "GPIO_142"),
+	PINCTRL_PIN(143, "GPIO_143"),
+	PINCTRL_PIN(144, "GPIO_144"),
+	PINCTRL_PIN(145, "GPIO_145"),
+	PINCTRL_PIN(146, "GPIO_146"),
+	PINCTRL_PIN(147, "GPIO_147"),
+	PINCTRL_PIN(148, "GPIO_148"),
+	PINCTRL_PIN(149, "GPIO_149"),
+	PINCTRL_PIN(150, "SDC1_CLK"),
+	PINCTRL_PIN(151, "SDC1_CMD"),
+	PINCTRL_PIN(152, "SDC1_DATA"),
+	PINCTRL_PIN(153, "SDC2_CLK"),
+	PINCTRL_PIN(154, "SDC2_CMD"),
+	PINCTRL_PIN(155, "SDC2_DATA"),
+	PINCTRL_PIN(156, "SDC1_RCLK"),
+};
+
+#define DECLARE_MSM_GPIO_PINS(pin) \
+	static const unsigned int gpio##pin##_pins[] = { pin }
+DECLARE_MSM_GPIO_PINS(0);
+DECLARE_MSM_GPIO_PINS(1);
+DECLARE_MSM_GPIO_PINS(2);
+DECLARE_MSM_GPIO_PINS(3);
+DECLARE_MSM_GPIO_PINS(4);
+DECLARE_MSM_GPIO_PINS(5);
+DECLARE_MSM_GPIO_PINS(6);
+DECLARE_MSM_GPIO_PINS(7);
+DECLARE_MSM_GPIO_PINS(8);
+DECLARE_MSM_GPIO_PINS(9);
+DECLARE_MSM_GPIO_PINS(10);
+DECLARE_MSM_GPIO_PINS(11);
+DECLARE_MSM_GPIO_PINS(12);
+DECLARE_MSM_GPIO_PINS(13);
+DECLARE_MSM_GPIO_PINS(14);
+DECLARE_MSM_GPIO_PINS(15);
+DECLARE_MSM_GPIO_PINS(16);
+DECLARE_MSM_GPIO_PINS(17);
+DECLARE_MSM_GPIO_PINS(18);
+DECLARE_MSM_GPIO_PINS(19);
+DECLARE_MSM_GPIO_PINS(20);
+DECLARE_MSM_GPIO_PINS(21);
+DECLARE_MSM_GPIO_PINS(22);
+DECLARE_MSM_GPIO_PINS(23);
+DECLARE_MSM_GPIO_PINS(24);
+DECLARE_MSM_GPIO_PINS(25);
+DECLARE_MSM_GPIO_PINS(26);
+DECLARE_MSM_GPIO_PINS(27);
+DECLARE_MSM_GPIO_PINS(28);
+DECLARE_MSM_GPIO_PINS(29);
+DECLARE_MSM_GPIO_PINS(30);
+DECLARE_MSM_GPIO_PINS(31);
+DECLARE_MSM_GPIO_PINS(32);
+DECLARE_MSM_GPIO_PINS(33);
+DECLARE_MSM_GPIO_PINS(34);
+DECLARE_MSM_GPIO_PINS(35);
+DECLARE_MSM_GPIO_PINS(36);
+DECLARE_MSM_GPIO_PINS(37);
+DECLARE_MSM_GPIO_PINS(38);
+DECLARE_MSM_GPIO_PINS(39);
+DECLARE_MSM_GPIO_PINS(40);
+DECLARE_MSM_GPIO_PINS(41);
+DECLARE_MSM_GPIO_PINS(42);
+DECLARE_MSM_GPIO_PINS(43);
+DECLARE_MSM_GPIO_PINS(44);
+DECLARE_MSM_GPIO_PINS(45);
+DECLARE_MSM_GPIO_PINS(46);
+DECLARE_MSM_GPIO_PINS(47);
+DECLARE_MSM_GPIO_PINS(48);
+DECLARE_MSM_GPIO_PINS(49);
+DECLARE_MSM_GPIO_PINS(50);
+DECLARE_MSM_GPIO_PINS(51);
+DECLARE_MSM_GPIO_PINS(52);
+DECLARE_MSM_GPIO_PINS(53);
+DECLARE_MSM_GPIO_PINS(54);
+DECLARE_MSM_GPIO_PINS(55);
+DECLARE_MSM_GPIO_PINS(56);
+DECLARE_MSM_GPIO_PINS(57);
+DECLARE_MSM_GPIO_PINS(58);
+DECLARE_MSM_GPIO_PINS(59);
+DECLARE_MSM_GPIO_PINS(60);
+DECLARE_MSM_GPIO_PINS(61);
+DECLARE_MSM_GPIO_PINS(62);
+DECLARE_MSM_GPIO_PINS(63);
+DECLARE_MSM_GPIO_PINS(64);
+DECLARE_MSM_GPIO_PINS(65);
+DECLARE_MSM_GPIO_PINS(66);
+DECLARE_MSM_GPIO_PINS(67);
+DECLARE_MSM_GPIO_PINS(68);
+DECLARE_MSM_GPIO_PINS(69);
+DECLARE_MSM_GPIO_PINS(70);
+DECLARE_MSM_GPIO_PINS(71);
+DECLARE_MSM_GPIO_PINS(72);
+DECLARE_MSM_GPIO_PINS(73);
+DECLARE_MSM_GPIO_PINS(74);
+DECLARE_MSM_GPIO_PINS(75);
+DECLARE_MSM_GPIO_PINS(76);
+DECLARE_MSM_GPIO_PINS(77);
+DECLARE_MSM_GPIO_PINS(78);
+DECLARE_MSM_GPIO_PINS(79);
+DECLARE_MSM_GPIO_PINS(80);
+DECLARE_MSM_GPIO_PINS(81);
+DECLARE_MSM_GPIO_PINS(82);
+DECLARE_MSM_GPIO_PINS(83);
+DECLARE_MSM_GPIO_PINS(84);
+DECLARE_MSM_GPIO_PINS(85);
+DECLARE_MSM_GPIO_PINS(86);
+DECLARE_MSM_GPIO_PINS(87);
+DECLARE_MSM_GPIO_PINS(88);
+DECLARE_MSM_GPIO_PINS(89);
+DECLARE_MSM_GPIO_PINS(90);
+DECLARE_MSM_GPIO_PINS(91);
+DECLARE_MSM_GPIO_PINS(92);
+DECLARE_MSM_GPIO_PINS(93);
+DECLARE_MSM_GPIO_PINS(94);
+DECLARE_MSM_GPIO_PINS(95);
+DECLARE_MSM_GPIO_PINS(96);
+DECLARE_MSM_GPIO_PINS(97);
+DECLARE_MSM_GPIO_PINS(98);
+DECLARE_MSM_GPIO_PINS(99);
+DECLARE_MSM_GPIO_PINS(100);
+DECLARE_MSM_GPIO_PINS(101);
+DECLARE_MSM_GPIO_PINS(102);
+DECLARE_MSM_GPIO_PINS(103);
+DECLARE_MSM_GPIO_PINS(104);
+DECLARE_MSM_GPIO_PINS(105);
+DECLARE_MSM_GPIO_PINS(106);
+DECLARE_MSM_GPIO_PINS(107);
+DECLARE_MSM_GPIO_PINS(108);
+DECLARE_MSM_GPIO_PINS(109);
+DECLARE_MSM_GPIO_PINS(110);
+DECLARE_MSM_GPIO_PINS(111);
+DECLARE_MSM_GPIO_PINS(112);
+DECLARE_MSM_GPIO_PINS(113);
+DECLARE_MSM_GPIO_PINS(114);
+DECLARE_MSM_GPIO_PINS(115);
+DECLARE_MSM_GPIO_PINS(116);
+DECLARE_MSM_GPIO_PINS(117);
+DECLARE_MSM_GPIO_PINS(118);
+DECLARE_MSM_GPIO_PINS(119);
+DECLARE_MSM_GPIO_PINS(120);
+DECLARE_MSM_GPIO_PINS(121);
+DECLARE_MSM_GPIO_PINS(122);
+DECLARE_MSM_GPIO_PINS(123);
+DECLARE_MSM_GPIO_PINS(124);
+DECLARE_MSM_GPIO_PINS(125);
+DECLARE_MSM_GPIO_PINS(126);
+DECLARE_MSM_GPIO_PINS(127);
+DECLARE_MSM_GPIO_PINS(128);
+DECLARE_MSM_GPIO_PINS(129);
+DECLARE_MSM_GPIO_PINS(130);
+DECLARE_MSM_GPIO_PINS(131);
+DECLARE_MSM_GPIO_PINS(132);
+DECLARE_MSM_GPIO_PINS(133);
+DECLARE_MSM_GPIO_PINS(134);
+DECLARE_MSM_GPIO_PINS(135);
+DECLARE_MSM_GPIO_PINS(136);
+DECLARE_MSM_GPIO_PINS(137);
+DECLARE_MSM_GPIO_PINS(138);
+DECLARE_MSM_GPIO_PINS(139);
+DECLARE_MSM_GPIO_PINS(140);
+DECLARE_MSM_GPIO_PINS(141);
+DECLARE_MSM_GPIO_PINS(142);
+DECLARE_MSM_GPIO_PINS(143);
+DECLARE_MSM_GPIO_PINS(144);
+DECLARE_MSM_GPIO_PINS(145);
+DECLARE_MSM_GPIO_PINS(146);
+DECLARE_MSM_GPIO_PINS(147);
+DECLARE_MSM_GPIO_PINS(148);
+DECLARE_MSM_GPIO_PINS(149);
+
+static const unsigned int sdc1_clk_pins[] = { 150 };
+static const unsigned int sdc1_cmd_pins[] = { 151 };
+static const unsigned int sdc1_data_pins[] = { 152 };
+static const unsigned int sdc2_clk_pins[] = { 153 };
+static const unsigned int sdc2_cmd_pins[] = { 154 };
+static const unsigned int sdc2_data_pins[] = { 155 };
+static const unsigned int sdc1_rclk_pins[] = { 156 };
+
+enum msm8996_functions {
+	msm_mux_adsp_ext,
+	msm_mux_atest_bbrx0,
+	msm_mux_atest_bbrx1,
+	msm_mux_atest_char,
+	msm_mux_atest_char0,
+	msm_mux_atest_char1,
+	msm_mux_atest_char2,
+	msm_mux_atest_char3,
+	msm_mux_atest_gpsadc0,
+	msm_mux_atest_gpsadc1,
+	msm_mux_atest_tsens,
+	msm_mux_atest_tsens2,
+	msm_mux_atest_usb1,
+	msm_mux_atest_usb10,
+	msm_mux_atest_usb11,
+	msm_mux_atest_usb12,
+	msm_mux_atest_usb13,
+	msm_mux_atest_usb2,
+	msm_mux_atest_usb20,
+	msm_mux_atest_usb21,
+	msm_mux_atest_usb22,
+	msm_mux_atest_usb23,
+	msm_mux_audio_ref,
+	msm_mux_bimc_dte0,
+	msm_mux_bimc_dte1,
+	msm_mux_blsp10_spi,
+	msm_mux_blsp11_i2c_scl_b,
+	msm_mux_blsp11_i2c_sda_b,
+	msm_mux_blsp11_uart_rx_b,
+	msm_mux_blsp11_uart_tx_b,
+	msm_mux_blsp1_spi,
+	msm_mux_blsp2_spi,
+	msm_mux_blsp_i2c1,
+	msm_mux_blsp_i2c10,
+	msm_mux_blsp_i2c11,
+	msm_mux_blsp_i2c12,
+	msm_mux_blsp_i2c2,
+	msm_mux_blsp_i2c3,
+	msm_mux_blsp_i2c4,
+	msm_mux_blsp_i2c5,
+	msm_mux_blsp_i2c6,
+	msm_mux_blsp_i2c7,
+	msm_mux_blsp_i2c8,
+	msm_mux_blsp_i2c9,
+	msm_mux_blsp_spi1,
+	msm_mux_blsp_spi10,
+	msm_mux_blsp_spi11,
+	msm_mux_blsp_spi12,
+	msm_mux_blsp_spi2,
+	msm_mux_blsp_spi3,
+	msm_mux_blsp_spi4,
+	msm_mux_blsp_spi5,
+	msm_mux_blsp_spi6,
+	msm_mux_blsp_spi7,
+	msm_mux_blsp_spi8,
+	msm_mux_blsp_spi9,
+	msm_mux_blsp_uart1,
+	msm_mux_blsp_uart10,
+	msm_mux_blsp_uart11,
+	msm_mux_blsp_uart12,
+	msm_mux_blsp_uart2,
+	msm_mux_blsp_uart3,
+	msm_mux_blsp_uart4,
+	msm_mux_blsp_uart5,
+	msm_mux_blsp_uart6,
+	msm_mux_blsp_uart7,
+	msm_mux_blsp_uart8,
+	msm_mux_blsp_uart9,
+	msm_mux_blsp_uim1,
+	msm_mux_blsp_uim10,
+	msm_mux_blsp_uim11,
+	msm_mux_blsp_uim12,
+	msm_mux_blsp_uim2,
+	msm_mux_blsp_uim3,
+	msm_mux_blsp_uim4,
+	msm_mux_blsp_uim5,
+	msm_mux_blsp_uim6,
+	msm_mux_blsp_uim7,
+	msm_mux_blsp_uim8,
+	msm_mux_blsp_uim9,
+	msm_mux_btfm_slimbus,
+	msm_mux_cam_mclk,
+	msm_mux_cci_async,
+	msm_mux_cci_i2c,
+	msm_mux_cci_timer0,
+	msm_mux_cci_timer1,
+	msm_mux_cci_timer2,
+	msm_mux_cci_timer3,
+	msm_mux_cci_timer4,
+	msm_mux_cri_trng,
+	msm_mux_cri_trng0,
+	msm_mux_cri_trng1,
+	msm_mux_dac_calib0,
+	msm_mux_dac_calib1,
+	msm_mux_dac_calib10,
+	msm_mux_dac_calib11,
+	msm_mux_dac_calib12,
+	msm_mux_dac_calib13,
+	msm_mux_dac_calib14,
+	msm_mux_dac_calib15,
+	msm_mux_dac_calib16,
+	msm_mux_dac_calib17,
+	msm_mux_dac_calib18,
+	msm_mux_dac_calib19,
+	msm_mux_dac_calib2,
+	msm_mux_dac_calib20,
+	msm_mux_dac_calib21,
+	msm_mux_dac_calib22,
+	msm_mux_dac_calib23,
+	msm_mux_dac_calib24,
+	msm_mux_dac_calib25,
+	msm_mux_dac_calib26,
+	msm_mux_dac_calib3,
+	msm_mux_dac_calib4,
+	msm_mux_dac_calib5,
+	msm_mux_dac_calib6,
+	msm_mux_dac_calib7,
+	msm_mux_dac_calib8,
+	msm_mux_dac_calib9,
+	msm_mux_dac_gpio,
+	msm_mux_dbg_out,
+	msm_mux_ddr_bist,
+	msm_mux_edp_hot,
+	msm_mux_edp_lcd,
+	msm_mux_gcc_gp1_clk_a,
+	msm_mux_gcc_gp1_clk_b,
+	msm_mux_gcc_gp2_clk_a,
+	msm_mux_gcc_gp2_clk_b,
+	msm_mux_gcc_gp3_clk_a,
+	msm_mux_gcc_gp3_clk_b,
+	msm_mux_gsm_tx,
+	msm_mux_hdmi_cec,
+	msm_mux_hdmi_ddc,
+	msm_mux_hdmi_hot,
+	msm_mux_hdmi_rcv,
+	msm_mux_isense_dbg,
+	msm_mux_ldo_en,
+	msm_mux_ldo_update,
+	msm_mux_lpass_slimbus,
+	msm_mux_m_voc,
+	msm_mux_mdp_vsync,
+	msm_mux_mdp_vsync_p_b,
+	msm_mux_mdp_vsync_s_b,
+	msm_mux_modem_tsync,
+	msm_mux_mss_lte,
+	msm_mux_nav_dr,
+	msm_mux_nav_pps,
+	msm_mux_pa_indicator,
+	msm_mux_pci_e0,
+	msm_mux_pci_e1,
+	msm_mux_pci_e2,
+	msm_mux_pll_bypassnl,
+	msm_mux_pll_reset,
+	msm_mux_pri_mi2s,
+	msm_mux_prng_rosc,
+	msm_mux_pwr_crypto,
+	msm_mux_pwr_modem,
+	msm_mux_pwr_nav,
+	msm_mux_qdss_cti,
+	msm_mux_qdss_cti_trig_in_a,
+	msm_mux_qdss_cti_trig_in_b,
+	msm_mux_qdss_cti_trig_out_a,
+	msm_mux_qdss_cti_trig_out_b,
+	msm_mux_qdss_stm0,
+	msm_mux_qdss_stm1,
+	msm_mux_qdss_stm10,
+	msm_mux_qdss_stm11,
+	msm_mux_qdss_stm12,
+	msm_mux_qdss_stm13,
+	msm_mux_qdss_stm14,
+	msm_mux_qdss_stm15,
+	msm_mux_qdss_stm16,
+	msm_mux_qdss_stm17,
+	msm_mux_qdss_stm18,
+	msm_mux_qdss_stm19,
+	msm_mux_qdss_stm2,
+	msm_mux_qdss_stm20,
+	msm_mux_qdss_stm21,
+	msm_mux_qdss_stm22,
+	msm_mux_qdss_stm23,
+	msm_mux_qdss_stm24,
+	msm_mux_qdss_stm25,
+	msm_mux_qdss_stm26,
+	msm_mux_qdss_stm27,
+	msm_mux_qdss_stm28,
+	msm_mux_qdss_stm29,
+	msm_mux_qdss_stm3,
+	msm_mux_qdss_stm30,
+	msm_mux_qdss_stm31,
+	msm_mux_qdss_stm4,
+	msm_mux_qdss_stm5,
+	msm_mux_qdss_stm6,
+	msm_mux_qdss_stm7,
+	msm_mux_qdss_stm8,
+	msm_mux_qdss_stm9,
+	msm_mux_qdss_traceclk_a,
+	msm_mux_qdss_traceclk_b,
+	msm_mux_qdss_tracectl_a,
+	msm_mux_qdss_tracectl_b,
+	msm_mux_qdss_tracedata_11,
+	msm_mux_qdss_tracedata_12,
+	msm_mux_qdss_tracedata_a,
+	msm_mux_qdss_tracedata_b,
+	msm_mux_qspi0,
+	msm_mux_qspi1,
+	msm_mux_qspi2,
+	msm_mux_qspi3,
+	msm_mux_qspi_clk,
+	msm_mux_qspi_cs,
+	msm_mux_qua_mi2s,
+	msm_mux_sd_card,
+	msm_mux_sd_write,
+	msm_mux_sdc40,
+	msm_mux_sdc41,
+	msm_mux_sdc42,
+	msm_mux_sdc43,
+	msm_mux_sdc4_clk,
+	msm_mux_sdc4_cmd,
+	msm_mux_sec_mi2s,
+	msm_mux_spkr_i2s,
+	msm_mux_ssbi1,
+	msm_mux_ssbi2,
+	msm_mux_ssc_irq,
+	msm_mux_ter_mi2s,
+	msm_mux_tsense_pwm1,
+	msm_mux_tsense_pwm2,
+	msm_mux_tsif1_clk,
+	msm_mux_tsif1_data,
+	msm_mux_tsif1_en,
+	msm_mux_tsif1_error,
+	msm_mux_tsif1_sync,
+	msm_mux_tsif2_clk,
+	msm_mux_tsif2_data,
+	msm_mux_tsif2_en,
+	msm_mux_tsif2_error,
+	msm_mux_tsif2_sync,
+	msm_mux_uim1,
+	msm_mux_uim2,
+	msm_mux_uim3,
+	msm_mux_uim4,
+	msm_mux_uim_batt,
+	msm_mux_vfr_1,
+	msm_mux_gpio,
+	msm_mux_NA,
+};
+
+static const char * const gpio_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27", "gpio28",
+	"gpio29", "gpio30", "gpio31", "gpio32", "gpio33", "gpio34", "gpio35",
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40", "gpio41", "gpio42",
+	"gpio43", "gpio44", "gpio45", "gpio46", "gpio47", "gpio48", "gpio49",
+	"gpio50", "gpio51", "gpio52", "gpio53", "gpio54", "gpio55", "gpio56",
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68", "gpio69", "gpio70",
+	"gpio71", "gpio72", "gpio73", "gpio74", "gpio75", "gpio76", "gpio77",
+	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82", "gpio83", "gpio84",
+	"gpio85", "gpio86", "gpio87", "gpio88", "gpio89", "gpio90", "gpio91",
+	"gpio92", "gpio93", "gpio94", "gpio95", "gpio96", "gpio97", "gpio98",
+	"gpio99", "gpio100", "gpio101", "gpio102", "gpio103", "gpio104",
+	"gpio105", "gpio106", "gpio107", "gpio108", "gpio109", "gpio110",
+	"gpio111", "gpio112", "gpio113", "gpio114", "gpio115", "gpio116",
+	"gpio117", "gpio118", "gpio119", "gpio120", "gpio121", "gpio122",
+	"gpio123", "gpio124", "gpio125", "gpio126", "gpio127", "gpio128",
+	"gpio129", "gpio130", "gpio131", "gpio132", "gpio133", "gpio134",
+	"gpio135", "gpio136", "gpio137", "gpio138", "gpio139", "gpio140",
+	"gpio141", "gpio142", "gpio143", "gpio144", "gpio145", "gpio146",
+	"gpio147", "gpio148", "gpio149"
+};
+
+
+static const char * const blsp_uart1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3",
+};
+static const char * const blsp_spi1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3",
+};
+static const char * const blsp_i2c1_groups[] = {
+	"gpio2", "gpio3",
+};
+static const char * const blsp_uim1_groups[] = {
+	"gpio0", "gpio1",
+};
+static const char * const atest_tsens_groups[] = {
+	"gpio3",
+};
+static const char * const bimc_dte1_groups[] = {
+	"gpio3", "gpio5",
+};
+static const char * const blsp_spi8_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7",
+};
+static const char * const blsp_uart8_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7",
+};
+static const char * const blsp_uim8_groups[] = {
+	"gpio4", "gpio5",
+};
+static const char * const qdss_cti_trig_out_b_groups[] = {
+	"gpio4",
+};
+static const char * const dac_calib0_groups[] = {
+	"gpio4", "gpio41",
+};
+static const char * const bimc_dte0_groups[] = {
+	"gpio4", "gpio6",
+};
+static const char * const qdss_cti_trig_in_b_groups[] = {
+	"gpio5",
+};
+static const char * const dac_calib1_groups[] = {
+	"gpio5", "gpio42",
+};
+static const char * const dac_calib2_groups[] = {
+	"gpio6", "gpio43",
+};
+static const char * const atest_tsens2_groups[] = {
+	"gpio7",
+};
+static const char * const blsp_spi10_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11",
+};
+static const char * const blsp_uart10_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11",
+};
+static const char * const blsp_uim10_groups[] = {
+	"gpio8", "gpio9",
+};
+static const char * const atest_bbrx1_groups[] = {
+	"gpio8",
+};
+static const char * const atest_usb12_groups[] = {
+	"gpio9",
+};
+static const char * const mdp_vsync_groups[] = {
+	"gpio10", "gpio11", "gpio12",
+};
+static const char * const edp_lcd_groups[] = {
+	"gpio10",
+};
+static const char * const blsp_i2c10_groups[] = {
+	"gpio10", "gpio11",
+};
+static const char * const atest_usb11_groups[] = {
+	"gpio10",
+};
+static const char * const atest_gpsadc0_groups[] = {
+	"gpio11",
+};
+static const char * const edp_hot_groups[] = {
+	"gpio11",
+};
+static const char * const atest_usb10_groups[] = {
+	"gpio11",
+};
+static const char * const m_voc_groups[] = {
+	"gpio12",
+};
+static const char * const dac_gpio_groups[] = {
+	"gpio12",
+};
+static const char * const atest_char_groups[] = {
+	"gpio12",
+};
+static const char * const cam_mclk_groups[] = {
+	"gpio13", "gpio14", "gpio15", "gpio16",
+};
+static const char * const pll_bypassnl_groups[] = {
+	"gpio13",
+};
+static const char * const qdss_stm7_groups[] = {
+	"gpio13",
+};
+static const char * const blsp_i2c8_groups[] = {
+	"gpio6", "gpio7",
+};
+static const char * const atest_usb1_groups[] = {
+	"gpio7",
+};
+static const char * const atest_usb13_groups[] = {
+	"gpio8",
+};
+static const char * const atest_bbrx0_groups[] = {
+	"gpio9",
+};
+static const char * const atest_gpsadc1_groups[] = {
+	"gpio10",
+};
+static const char * const qdss_tracedata_b_groups[] = {
+	"gpio13", "gpio14", "gpio15", "gpio16", "gpio17", "gpio18", "gpio19",
+	"gpio21", "gpio22", "gpio23", "gpio26", "gpio29", "gpio57", "gpio58",
+	"gpio92", "gpio93",
+};
+static const char * const pll_reset_groups[] = {
+	"gpio14",
+};
+static const char * const qdss_stm6_groups[] = {
+	"gpio14",
+};
+static const char * const qdss_stm5_groups[] = {
+	"gpio15",
+};
+static const char * const qdss_stm4_groups[] = {
+	"gpio16",
+};
+static const char * const atest_usb2_groups[] = {
+	"gpio16",
+};
+static const char * const dac_calib3_groups[] = {
+	"gpio17", "gpio44",
+};
+static const char * const cci_i2c_groups[] = {
+	"gpio17", "gpio18", "gpio19", "gpio20",
+};
+static const char * const qdss_stm3_groups[] = {
+	"gpio17",
+};
+static const char * const atest_usb23_groups[] = {
+	"gpio17",
+};
+static const char * const atest_char3_groups[] = {
+	"gpio17",
+};
+static const char * const dac_calib4_groups[] = {
+	"gpio18", "gpio45",
+};
+static const char * const qdss_stm2_groups[] = {
+	"gpio18",
+};
+static const char * const atest_usb22_groups[] = {
+	"gpio18",
+};
+static const char * const atest_char2_groups[] = {
+	"gpio18",
+};
+static const char * const dac_calib5_groups[] = {
+	"gpio19", "gpio46",
+};
+static const char * const qdss_stm1_groups[] = {
+	"gpio19",
+};
+static const char * const atest_usb21_groups[] = {
+	"gpio19",
+};
+static const char * const atest_char1_groups[] = {
+	"gpio19",
+};
+static const char * const dac_calib6_groups[] = {
+	"gpio20", "gpio47",
+};
+static const char * const dbg_out_groups[] = {
+	"gpio20",
+};
+static const char * const qdss_stm0_groups[] = {
+	"gpio20",
+};
+static const char * const atest_usb20_groups[] = {
+	"gpio20",
+};
+static const char * const atest_char0_groups[] = {
+	"gpio20",
+};
+static const char * const dac_calib7_groups[] = {
+	"gpio21", "gpio48",
+};
+static const char * const cci_timer0_groups[] = {
+	"gpio21",
+};
+static const char * const qdss_stm13_groups[] = {
+	"gpio21",
+};
+static const char * const dac_calib8_groups[] = {
+	"gpio22", "gpio49",
+};
+static const char * const cci_timer1_groups[] = {
+	"gpio22",
+};
+static const char * const qdss_stm12_groups[] = {
+	"gpio22",
+};
+static const char * const dac_calib9_groups[] = {
+	"gpio23", "gpio50",
+};
+static const char * const cci_timer2_groups[] = {
+	"gpio23",
+};
+static const char * const qdss_stm11_groups[] = {
+	"gpio23",
+};
+static const char * const dac_calib10_groups[] = {
+	"gpio24", "gpio51",
+};
+static const char * const cci_timer3_groups[] = {
+	"gpio24",
+};
+static const char * const cci_async_groups[] = {
+	"gpio24", "gpio25", "gpio26",
+};
+static const char * const blsp1_spi_groups[] = {
+	"gpio24", "gpio27", "gpio28", "gpio90",
+};
+static const char * const qdss_stm10_groups[] = {
+	"gpio24",
+};
+static const char * const qdss_cti_trig_in_a_groups[] = {
+	"gpio24",
+};
+static const char * const dac_calib11_groups[] = {
+	"gpio25", "gpio52",
+};
+static const char * const cci_timer4_groups[] = {
+	"gpio25",
+};
+static const char * const blsp_spi6_groups[] = {
+	"gpio25", "gpio26", "gpio27", "gpio28",
+};
+static const char * const blsp_uart6_groups[] = {
+	"gpio25", "gpio26", "gpio27", "gpio28",
+};
+static const char * const blsp_uim6_groups[] = {
+	"gpio25", "gpio26",
+};
+static const char * const blsp2_spi_groups[] = {
+	"gpio25", "gpio29", "gpio30",
+};
+static const char * const qdss_stm9_groups[] = {
+	"gpio25",
+};
+static const char * const qdss_cti_trig_out_a_groups[] = {
+	"gpio25",
+};
+static const char * const dac_calib12_groups[] = {
+	"gpio26", "gpio53",
+};
+static const char * const qdss_stm8_groups[] = {
+	"gpio26",
+};
+static const char * const dac_calib13_groups[] = {
+	"gpio27", "gpio54",
+};
+static const char * const blsp_i2c6_groups[] = {
+	"gpio27", "gpio28",
+};
+static const char * const qdss_tracectl_a_groups[] = {
+	"gpio27",
+};
+static const char * const dac_calib14_groups[] = {
+	"gpio28", "gpio55",
+};
+static const char * const qdss_traceclk_a_groups[] = {
+	"gpio28",
+};
+static const char * const dac_calib15_groups[] = {
+	"gpio29", "gpio56",
+};
+static const char * const dac_calib16_groups[] = {
+	"gpio30", "gpio57",
+};
+static const char * const hdmi_rcv_groups[] = {
+	"gpio30",
+};
+static const char * const dac_calib17_groups[] = {
+	"gpio31", "gpio58",
+};
+static const char * const pwr_modem_groups[] = {
+	"gpio31",
+};
+static const char * const hdmi_cec_groups[] = {
+	"gpio31",
+};
+static const char * const pwr_nav_groups[] = {
+	"gpio32",
+};
+static const char * const dac_calib18_groups[] = {
+	"gpio32", "gpio59",
+};
+static const char * const hdmi_ddc_groups[] = {
+	"gpio32", "gpio33",
+};
+static const char * const pwr_crypto_groups[] = {
+	"gpio33",
+};
+static const char * const dac_calib19_groups[] = {
+	"gpio33", "gpio60",
+};
+static const char * const dac_calib20_groups[] = {
+	"gpio34", "gpio61",
+};
+static const char * const hdmi_hot_groups[] = {
+	"gpio34",
+};
+static const char * const dac_calib21_groups[] = {
+	"gpio35", "gpio62",
+};
+static const char * const pci_e0_groups[] = {
+	"gpio35", "gpio36",
+};
+static const char * const dac_calib22_groups[] = {
+	"gpio36", "gpio63",
+};
+static const char * const dac_calib23_groups[] = {
+	"gpio37", "gpio64",
+};
+static const char * const blsp_i2c2_groups[] = {
+	"gpio43", "gpio44",
+};
+static const char * const blsp_spi3_groups[] = {
+	"gpio45", "gpio46", "gpio47", "gpio48",
+};
+static const char * const blsp_uart3_groups[] = {
+	"gpio45", "gpio46", "gpio47", "gpio48",
+};
+static const char * const blsp_uim3_groups[] = {
+	"gpio45", "gpio46",
+};
+static const char * const blsp_i2c3_groups[] = {
+	"gpio47", "gpio48",
+};
+static const char * const dac_calib24_groups[] = {
+	"gpio38", "gpio65",
+};
+static const char * const dac_calib25_groups[] = {
+	"gpio39", "gpio66",
+};
+static const char * const tsif1_sync_groups[] = {
+	"gpio39",
+};
+static const char * const sd_write_groups[] = {
+	"gpio40",
+};
+static const char * const tsif1_error_groups[] = {
+	"gpio40",
+};
+static const char * const blsp_spi2_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44",
+};
+static const char * const blsp_uart2_groups[] = {
+	"gpio41", "gpio42", "gpio43", "gpio44",
+};
+static const char * const blsp_uim2_groups[] = {
+	"gpio41", "gpio42",
+};
+static const char * const qdss_cti_groups[] = {
+	"gpio41", "gpio42", "gpio100", "gpio101",
+};
+static const char * const uim3_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52",
+};
+static const char * const blsp_spi9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52",
+};
+static const char * const blsp_uart9_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52",
+};
+static const char * const blsp_uim9_groups[] = {
+	"gpio49", "gpio50",
+};
+static const char * const blsp10_spi_groups[] = {
+	"gpio49", "gpio50", "gpio51", "gpio52", "gpio88",
+};
+static const char * const blsp_i2c9_groups[] = {
+	"gpio51", "gpio52",
+};
+static const char * const blsp_spi7_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56",
+};
+static const char * const blsp_uart7_groups[] = {
+	"gpio53", "gpio54", "gpio55", "gpio56",
+};
+static const char * const blsp_uim7_groups[] = {
+	"gpio53", "gpio54",
+};
+static const char * const qdss_tracedata_a_groups[] = {
+	"gpio53", "gpio54", "gpio63", "gpio64", "gpio65", "gpio66", "gpio67",
+	"gpio74", "gpio75", "gpio76", "gpio77", "gpio85", "gpio86", "gpio87",
+	"gpio89", "gpio90",
+};
+static const char * const blsp_i2c7_groups[] = {
+	"gpio55", "gpio56",
+};
+static const char * const qua_mi2s_groups[] = {
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+};
+static const char * const gcc_gp1_clk_a_groups[] = {
+	"gpio57",
+};
+static const char * const uim4_groups[] = {
+	"gpio58", "gpio59", "gpio60", "gpio61",
+};
+static const char * const blsp_spi11_groups[] = {
+	"gpio58", "gpio59", "gpio60", "gpio61",
+};
+static const char * const blsp_uart11_groups[] = {
+	"gpio58", "gpio59", "gpio60", "gpio61",
+};
+static const char * const blsp_uim11_groups[] = {
+	"gpio58", "gpio59",
+};
+static const char * const gcc_gp2_clk_a_groups[] = {
+	"gpio58",
+};
+static const char * const gcc_gp3_clk_a_groups[] = {
+	"gpio59",
+};
+static const char * const blsp_i2c11_groups[] = {
+	"gpio60", "gpio61",
+};
+static const char * const cri_trng0_groups[] = {
+	"gpio60",
+};
+static const char * const cri_trng1_groups[] = {
+	"gpio61",
+};
+static const char * const cri_trng_groups[] = {
+	"gpio62",
+};
+static const char * const qdss_stm18_groups[] = {
+	"gpio63",
+};
+static const char * const pri_mi2s_groups[] = {
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68",
+};
+static const char * const qdss_stm17_groups[] = {
+	"gpio64",
+};
+static const char * const blsp_spi4_groups[] = {
+	"gpio65", "gpio66", "gpio67", "gpio68",
+};
+static const char * const blsp_uart4_groups[] = {
+	"gpio65", "gpio66", "gpio67", "gpio68",
+};
+static const char * const blsp_uim4_groups[] = {
+	"gpio65", "gpio66",
+};
+static const char * const qdss_stm16_groups[] = {
+	"gpio65",
+};
+static const char * const qdss_stm15_groups[] = {
+	"gpio66",
+};
+static const char * const dac_calib26_groups[] = {
+	"gpio67",
+};
+static const char * const blsp_i2c4_groups[] = {
+	"gpio67", "gpio68",
+};
+static const char * const qdss_stm14_groups[] = {
+	"gpio67",
+};
+static const char * const spkr_i2s_groups[] = {
+	"gpio69", "gpio70", "gpio71", "gpio72",
+};
+static const char * const audio_ref_groups[] = {
+	"gpio69",
+};
+static const char * const lpass_slimbus_groups[] = {
+	"gpio70", "gpio71", "gpio72",
+};
+static const char * const isense_dbg_groups[] = {
+	"gpio70",
+};
+static const char * const tsense_pwm1_groups[] = {
+	"gpio71",
+};
+static const char * const tsense_pwm2_groups[] = {
+	"gpio71",
+};
+static const char * const btfm_slimbus_groups[] = {
+	"gpio73", "gpio74",
+};
+static const char * const ter_mi2s_groups[] = {
+	"gpio74", "gpio75", "gpio76", "gpio77", "gpio78",
+};
+static const char * const qdss_stm22_groups[] = {
+	"gpio74",
+};
+static const char * const qdss_stm21_groups[] = {
+	"gpio75",
+};
+static const char * const qdss_stm20_groups[] = {
+	"gpio76",
+};
+static const char * const qdss_stm19_groups[] = {
+	"gpio77",
+};
+static const char * const ssc_irq_groups[] = {
+	"gpio78", "gpio79", "gpio80", "gpio117", "gpio118", "gpio119",
+	"gpio120", "gpio121", "gpio122", "gpio123", "gpio124", "gpio125",
+};
+static const char * const gcc_gp1_clk_b_groups[] = {
+	"gpio78",
+};
+static const char * const sec_mi2s_groups[] = {
+	"gpio79", "gpio80", "gpio81", "gpio82", "gpio83",
+};
+static const char * const blsp_spi5_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84",
+};
+static const char * const blsp_uart5_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84",
+};
+static const char * const blsp_uim5_groups[] = {
+	"gpio81", "gpio82",
+};
+static const char * const gcc_gp2_clk_b_groups[] = {
+	"gpio81",
+};
+static const char * const gcc_gp3_clk_b_groups[] = {
+	"gpio82",
+};
+static const char * const blsp_i2c5_groups[] = {
+	"gpio83", "gpio84",
+};
+static const char * const blsp_spi12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88",
+};
+static const char * const blsp_uart12_groups[] = {
+	"gpio85", "gpio86", "gpio87", "gpio88",
+};
+static const char * const blsp_uim12_groups[] = {
+	"gpio85", "gpio86",
+};
+static const char * const qdss_stm25_groups[] = {
+	"gpio85",
+};
+static const char * const qdss_stm31_groups[] = {
+	"gpio86",
+};
+static const char * const blsp_i2c12_groups[] = {
+	"gpio87", "gpio88",
+};
+static const char * const qdss_stm30_groups[] = {
+	"gpio87",
+};
+static const char * const qdss_stm29_groups[] = {
+	"gpio88",
+};
+static const char * const tsif1_clk_groups[] = {
+	"gpio89",
+};
+static const char * const qdss_stm28_groups[] = {
+	"gpio89",
+};
+static const char * const tsif1_en_groups[] = {
+	"gpio90",
+};
+static const char * const tsif1_data_groups[] = {
+	"gpio91",
+};
+static const char * const sdc4_cmd_groups[] = {
+	"gpio91",
+};
+static const char * const qdss_stm27_groups[] = {
+	"gpio91",
+};
+static const char * const qdss_traceclk_b_groups[] = {
+	"gpio91",
+};
+static const char * const tsif2_error_groups[] = {
+	"gpio92",
+};
+static const char * const sdc43_groups[] = {
+	"gpio92",
+};
+static const char * const vfr_1_groups[] = {
+	"gpio92",
+};
+static const char * const qdss_stm26_groups[] = {
+	"gpio92",
+};
+static const char * const tsif2_clk_groups[] = {
+	"gpio93",
+};
+static const char * const sdc4_clk_groups[] = {
+	"gpio93",
+};
+static const char * const qdss_stm24_groups[] = {
+	"gpio93",
+};
+static const char * const tsif2_en_groups[] = {
+	"gpio94",
+};
+static const char * const sdc42_groups[] = {
+	"gpio94",
+};
+static const char * const qdss_stm23_groups[] = {
+	"gpio94",
+};
+static const char * const qdss_tracectl_b_groups[] = {
+	"gpio94",
+};
+static const char * const sd_card_groups[] = {
+	"gpio95",
+};
+static const char * const tsif2_data_groups[] = {
+	"gpio95",
+};
+static const char * const sdc41_groups[] = {
+	"gpio95",
+};
+static const char * const tsif2_sync_groups[] = {
+	"gpio96",
+};
+static const char * const sdc40_groups[] = {
+	"gpio96",
+};
+static const char * const mdp_vsync_p_b_groups[] = {
+	"gpio97",
+};
+static const char * const ldo_en_groups[] = {
+	"gpio97",
+};
+static const char * const mdp_vsync_s_b_groups[] = {
+	"gpio98",
+};
+static const char * const ldo_update_groups[] = {
+	"gpio98",
+};
+static const char * const blsp11_uart_tx_b_groups[] = {
+	"gpio100",
+};
+static const char * const blsp11_uart_rx_b_groups[] = {
+	"gpio101",
+};
+static const char * const blsp11_i2c_sda_b_groups[] = {
+	"gpio102",
+};
+static const char * const prng_rosc_groups[] = {
+	"gpio102",
+};
+static const char * const blsp11_i2c_scl_b_groups[] = {
+	"gpio103",
+};
+static const char * const uim2_groups[] = {
+	"gpio105", "gpio106", "gpio107", "gpio108",
+};
+static const char * const uim1_groups[] = {
+	"gpio109", "gpio110", "gpio111", "gpio112",
+};
+static const char * const uim_batt_groups[] = {
+	"gpio113",
+};
+static const char * const pci_e2_groups[] = {
+	"gpio114", "gpio115", "gpio116",
+};
+static const char * const pa_indicator_groups[] = {
+	"gpio116",
+};
+static const char * const adsp_ext_groups[] = {
+	"gpio118",
+};
+static const char * const ddr_bist_groups[] = {
+	"gpio121", "gpio122", "gpio123", "gpio124",
+};
+static const char * const qdss_tracedata_11_groups[] = {
+	"gpio123",
+};
+static const char * const qdss_tracedata_12_groups[] = {
+	"gpio124",
+};
+static const char * const modem_tsync_groups[] = {
+	"gpio128",
+};
+static const char * const nav_dr_groups[] = {
+	"gpio128",
+};
+static const char * const nav_pps_groups[] = {
+	"gpio128",
+};
+static const char * const pci_e1_groups[] = {
+	"gpio130", "gpio131", "gpio132",
+};
+static const char * const gsm_tx_groups[] = {
+	"gpio134", "gpio135",
+};
+static const char * const qspi_cs_groups[] = {
+	"gpio138", "gpio141",
+};
+static const char * const ssbi2_groups[] = {
+	"gpio139",
+};
+static const char * const ssbi1_groups[] = {
+	"gpio140",
+};
+static const char * const mss_lte_groups[] = {
+	"gpio144", "gpio145",
+};
+static const char * const qspi_clk_groups[] = {
+	"gpio145",
+};
+static const char * const qspi0_groups[] = {
+	"gpio146",
+};
+static const char * const qspi1_groups[] = {
+	"gpio147",
+};
+static const char * const qspi2_groups[] = {
+	"gpio148",
+};
+static const char * const qspi3_groups[] = {
+	"gpio149",
+};
+
+static const struct msm_function msm8996_functions[] = {
+	FUNCTION(adsp_ext),
+	FUNCTION(atest_bbrx0),
+	FUNCTION(atest_bbrx1),
+	FUNCTION(atest_char),
+	FUNCTION(atest_char0),
+	FUNCTION(atest_char1),
+	FUNCTION(atest_char2),
+	FUNCTION(atest_char3),
+	FUNCTION(atest_gpsadc0),
+	FUNCTION(atest_gpsadc1),
+	FUNCTION(atest_tsens),
+	FUNCTION(atest_tsens2),
+	FUNCTION(atest_usb1),
+	FUNCTION(atest_usb10),
+	FUNCTION(atest_usb11),
+	FUNCTION(atest_usb12),
+	FUNCTION(atest_usb13),
+	FUNCTION(atest_usb2),
+	FUNCTION(atest_usb20),
+	FUNCTION(atest_usb21),
+	FUNCTION(atest_usb22),
+	FUNCTION(atest_usb23),
+	FUNCTION(audio_ref),
+	FUNCTION(bimc_dte0),
+	FUNCTION(bimc_dte1),
+	FUNCTION(blsp10_spi),
+	FUNCTION(blsp11_i2c_scl_b),
+	FUNCTION(blsp11_i2c_sda_b),
+	FUNCTION(blsp11_uart_rx_b),
+	FUNCTION(blsp11_uart_tx_b),
+	FUNCTION(blsp1_spi),
+	FUNCTION(blsp2_spi),
+	FUNCTION(blsp_i2c1),
+	FUNCTION(blsp_i2c10),
+	FUNCTION(blsp_i2c11),
+	FUNCTION(blsp_i2c12),
+	FUNCTION(blsp_i2c2),
+	FUNCTION(blsp_i2c3),
+	FUNCTION(blsp_i2c4),
+	FUNCTION(blsp_i2c5),
+	FUNCTION(blsp_i2c6),
+	FUNCTION(blsp_i2c7),
+	FUNCTION(blsp_i2c8),
+	FUNCTION(blsp_i2c9),
+	FUNCTION(blsp_spi1),
+	FUNCTION(blsp_spi10),
+	FUNCTION(blsp_spi11),
+	FUNCTION(blsp_spi12),
+	FUNCTION(blsp_spi2),
+	FUNCTION(blsp_spi3),
+	FUNCTION(blsp_spi4),
+	FUNCTION(blsp_spi5),
+	FUNCTION(blsp_spi6),
+	FUNCTION(blsp_spi7),
+	FUNCTION(blsp_spi8),
+	FUNCTION(blsp_spi9),
+	FUNCTION(blsp_uart1),
+	FUNCTION(blsp_uart10),
+	FUNCTION(blsp_uart11),
+	FUNCTION(blsp_uart12),
+	FUNCTION(blsp_uart2),
+	FUNCTION(blsp_uart3),
+	FUNCTION(blsp_uart4),
+	FUNCTION(blsp_uart5),
+	FUNCTION(blsp_uart6),
+	FUNCTION(blsp_uart7),
+	FUNCTION(blsp_uart8),
+	FUNCTION(blsp_uart9),
+	FUNCTION(blsp_uim1),
+	FUNCTION(blsp_uim10),
+	FUNCTION(blsp_uim11),
+	FUNCTION(blsp_uim12),
+	FUNCTION(blsp_uim2),
+	FUNCTION(blsp_uim3),
+	FUNCTION(blsp_uim4),
+	FUNCTION(blsp_uim5),
+	FUNCTION(blsp_uim6),
+	FUNCTION(blsp_uim7),
+	FUNCTION(blsp_uim8),
+	FUNCTION(blsp_uim9),
+	FUNCTION(btfm_slimbus),
+	FUNCTION(cam_mclk),
+	FUNCTION(cci_async),
+	FUNCTION(cci_i2c),
+	FUNCTION(cci_timer0),
+	FUNCTION(cci_timer1),
+	FUNCTION(cci_timer2),
+	FUNCTION(cci_timer3),
+	FUNCTION(cci_timer4),
+	FUNCTION(cri_trng),
+	FUNCTION(cri_trng0),
+	FUNCTION(cri_trng1),
+	FUNCTION(dac_calib0),
+	FUNCTION(dac_calib1),
+	FUNCTION(dac_calib10),
+	FUNCTION(dac_calib11),
+	FUNCTION(dac_calib12),
+	FUNCTION(dac_calib13),
+	FUNCTION(dac_calib14),
+	FUNCTION(dac_calib15),
+	FUNCTION(dac_calib16),
+	FUNCTION(dac_calib17),
+	FUNCTION(dac_calib18),
+	FUNCTION(dac_calib19),
+	FUNCTION(dac_calib2),
+	FUNCTION(dac_calib20),
+	FUNCTION(dac_calib21),
+	FUNCTION(dac_calib22),
+	FUNCTION(dac_calib23),
+	FUNCTION(dac_calib24),
+	FUNCTION(dac_calib25),
+	FUNCTION(dac_calib26),
+	FUNCTION(dac_calib3),
+	FUNCTION(dac_calib4),
+	FUNCTION(dac_calib5),
+	FUNCTION(dac_calib6),
+	FUNCTION(dac_calib7),
+	FUNCTION(dac_calib8),
+	FUNCTION(dac_calib9),
+	FUNCTION(dac_gpio),
+	FUNCTION(dbg_out),
+	FUNCTION(ddr_bist),
+	FUNCTION(edp_hot),
+	FUNCTION(edp_lcd),
+	FUNCTION(gcc_gp1_clk_a),
+	FUNCTION(gcc_gp1_clk_b),
+	FUNCTION(gcc_gp2_clk_a),
+	FUNCTION(gcc_gp2_clk_b),
+	FUNCTION(gcc_gp3_clk_a),
+	FUNCTION(gcc_gp3_clk_b),
+	FUNCTION(gpio),
+	FUNCTION(gsm_tx),
+	FUNCTION(hdmi_cec),
+	FUNCTION(hdmi_ddc),
+	FUNCTION(hdmi_hot),
+	FUNCTION(hdmi_rcv),
+	FUNCTION(isense_dbg),
+	FUNCTION(ldo_en),
+	FUNCTION(ldo_update),
+	FUNCTION(lpass_slimbus),
+	FUNCTION(m_voc),
+	FUNCTION(mdp_vsync),
+	FUNCTION(mdp_vsync_p_b),
+	FUNCTION(mdp_vsync_s_b),
+	FUNCTION(modem_tsync),
+	FUNCTION(mss_lte),
+	FUNCTION(nav_dr),
+	FUNCTION(nav_pps),
+	FUNCTION(pa_indicator),
+	FUNCTION(pci_e0),
+	FUNCTION(pci_e1),
+	FUNCTION(pci_e2),
+	FUNCTION(pll_bypassnl),
+	FUNCTION(pll_reset),
+	FUNCTION(pri_mi2s),
+	FUNCTION(prng_rosc),
+	FUNCTION(pwr_crypto),
+	FUNCTION(pwr_modem),
+	FUNCTION(pwr_nav),
+	FUNCTION(qdss_cti),
+	FUNCTION(qdss_cti_trig_in_a),
+	FUNCTION(qdss_cti_trig_in_b),
+	FUNCTION(qdss_cti_trig_out_a),
+	FUNCTION(qdss_cti_trig_out_b),
+	FUNCTION(qdss_stm0),
+	FUNCTION(qdss_stm1),
+	FUNCTION(qdss_stm10),
+	FUNCTION(qdss_stm11),
+	FUNCTION(qdss_stm12),
+	FUNCTION(qdss_stm13),
+	FUNCTION(qdss_stm14),
+	FUNCTION(qdss_stm15),
+	FUNCTION(qdss_stm16),
+	FUNCTION(qdss_stm17),
+	FUNCTION(qdss_stm18),
+	FUNCTION(qdss_stm19),
+	FUNCTION(qdss_stm2),
+	FUNCTION(qdss_stm20),
+	FUNCTION(qdss_stm21),
+	FUNCTION(qdss_stm22),
+	FUNCTION(qdss_stm23),
+	FUNCTION(qdss_stm24),
+	FUNCTION(qdss_stm25),
+	FUNCTION(qdss_stm26),
+	FUNCTION(qdss_stm27),
+	FUNCTION(qdss_stm28),
+	FUNCTION(qdss_stm29),
+	FUNCTION(qdss_stm3),
+	FUNCTION(qdss_stm30),
+	FUNCTION(qdss_stm31),
+	FUNCTION(qdss_stm4),
+	FUNCTION(qdss_stm5),
+	FUNCTION(qdss_stm6),
+	FUNCTION(qdss_stm7),
+	FUNCTION(qdss_stm8),
+	FUNCTION(qdss_stm9),
+	FUNCTION(qdss_traceclk_a),
+	FUNCTION(qdss_traceclk_b),
+	FUNCTION(qdss_tracectl_a),
+	FUNCTION(qdss_tracectl_b),
+	FUNCTION(qdss_tracedata_11),
+	FUNCTION(qdss_tracedata_12),
+	FUNCTION(qdss_tracedata_a),
+	FUNCTION(qdss_tracedata_b),
+	FUNCTION(qspi0),
+	FUNCTION(qspi1),
+	FUNCTION(qspi2),
+	FUNCTION(qspi3),
+	FUNCTION(qspi_clk),
+	FUNCTION(qspi_cs),
+	FUNCTION(qua_mi2s),
+	FUNCTION(sd_card),
+	FUNCTION(sd_write),
+	FUNCTION(sdc40),
+	FUNCTION(sdc41),
+	FUNCTION(sdc42),
+	FUNCTION(sdc43),
+	FUNCTION(sdc4_clk),
+	FUNCTION(sdc4_cmd),
+	FUNCTION(sec_mi2s),
+	FUNCTION(spkr_i2s),
+	FUNCTION(ssbi1),
+	FUNCTION(ssbi2),
+	FUNCTION(ssc_irq),
+	FUNCTION(ter_mi2s),
+	FUNCTION(tsense_pwm1),
+	FUNCTION(tsense_pwm2),
+	FUNCTION(tsif1_clk),
+	FUNCTION(tsif1_data),
+	FUNCTION(tsif1_en),
+	FUNCTION(tsif1_error),
+	FUNCTION(tsif1_sync),
+	FUNCTION(tsif2_clk),
+	FUNCTION(tsif2_data),
+	FUNCTION(tsif2_en),
+	FUNCTION(tsif2_error),
+	FUNCTION(tsif2_sync),
+	FUNCTION(uim1),
+	FUNCTION(uim2),
+	FUNCTION(uim3),
+	FUNCTION(uim4),
+	FUNCTION(uim_batt),
+	FUNCTION(vfr_1),
+};
+
+static const struct msm_pingroup msm8996_groups[] = {
+	PINGROUP(0, blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(1, blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(2, blsp_spi1, blsp_uart1, blsp_i2c1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(3, blsp_spi1, blsp_uart1, blsp_i2c1, NA, atest_tsens,
+		 bimc_dte1, NA, NA, NA),
+	PINGROUP(4, blsp_spi8, blsp_uart8, blsp_uim8, NA, qdss_cti_trig_out_b,
+		 dac_calib0, bimc_dte0, NA, NA),
+	PINGROUP(5, blsp_spi8, blsp_uart8, blsp_uim8, NA, qdss_cti_trig_in_b,
+		 dac_calib1, bimc_dte1, NA, NA),
+	PINGROUP(6, blsp_spi8, blsp_uart8, blsp_i2c8, NA, dac_calib2,
+		 bimc_dte0, NA, NA, NA),
+	PINGROUP(7, blsp_spi8, blsp_uart8, blsp_i2c8, NA, atest_tsens2,
+		 atest_usb1, NA, NA, NA),
+	PINGROUP(8, blsp_spi10, blsp_uart10, blsp_uim10, NA, atest_bbrx1,
+		 atest_usb13, NA, NA, NA),
+	PINGROUP(9, blsp_spi10, blsp_uart10, blsp_uim10, atest_bbrx0,
+		 atest_usb12, NA, NA, NA, NA),
+	PINGROUP(10, mdp_vsync, blsp_spi10, blsp_uart10, blsp_i2c10,
+		 atest_gpsadc1, atest_usb11, NA, NA, NA),
+	PINGROUP(11, mdp_vsync, blsp_spi10, blsp_uart10, blsp_i2c10,
+		 atest_gpsadc0, atest_usb10, NA, NA, NA),
+	PINGROUP(12, mdp_vsync, m_voc, dac_gpio, atest_char, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(13, cam_mclk, pll_bypassnl, qdss_stm7, qdss_tracedata_b, NA,
+		 NA, NA, NA, NA),
+	PINGROUP(14, cam_mclk, pll_reset, qdss_stm6, qdss_tracedata_b, NA, NA,
+		 NA, NA, NA),
+	PINGROUP(15, cam_mclk, qdss_stm5, qdss_tracedata_b, NA, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(16, cam_mclk, qdss_stm4, qdss_tracedata_b, NA, atest_usb2, NA,
+		 NA, NA, NA),
+	PINGROUP(17, cci_i2c, qdss_stm3, qdss_tracedata_b, dac_calib3,
+		 atest_usb23, atest_char3, NA, NA, NA),
+	PINGROUP(18, cci_i2c, qdss_stm2, qdss_tracedata_b, dac_calib4,
+		 atest_usb22, atest_char2, NA, NA, NA),
+	PINGROUP(19, cci_i2c, qdss_stm1, qdss_tracedata_b, dac_calib5,
+		 atest_usb21, atest_char1, NA, NA, NA),
+	PINGROUP(20, cci_i2c, dbg_out, qdss_stm0, dac_calib6, atest_usb20,
+		 atest_char0, NA, NA, NA),
+	PINGROUP(21, cci_timer0, qdss_stm13, qdss_tracedata_b, dac_calib7, NA,
+		 NA, NA, NA, NA),
+	PINGROUP(22, cci_timer1, qdss_stm12, qdss_tracedata_b, dac_calib8, NA,
+		 NA, NA, NA, NA),
+	PINGROUP(23, cci_timer2, blsp1_spi, qdss_stm11, qdss_tracedata_b,
+		 dac_calib9, NA, NA, NA, NA),
+	PINGROUP(24, cci_timer3, cci_async, blsp1_spi, qdss_stm10,
+		 qdss_cti_trig_in_a, dac_calib10, NA, NA, NA),
+	PINGROUP(25, cci_timer4, cci_async, blsp_spi6, blsp_uart6, blsp_uim6,
+		 blsp2_spi, qdss_stm9, qdss_cti_trig_out_a, dac_calib11),
+	PINGROUP(26, cci_async, blsp_spi6, blsp_uart6, blsp_uim6, qdss_stm8,
+		 qdss_tracedata_b, dac_calib12, NA, NA),
+	PINGROUP(27, blsp_spi6, blsp_uart6, blsp_i2c6, blsp1_spi,
+		 qdss_tracectl_a, dac_calib13, NA, NA, NA),
+	PINGROUP(28, blsp_spi6, blsp_uart6, blsp_i2c6, blsp1_spi,
+		 qdss_traceclk_a, dac_calib14, NA, NA, NA),
+	PINGROUP(29, blsp2_spi, NA, qdss_tracedata_b, dac_calib15, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(30, hdmi_rcv, blsp2_spi, dac_calib16, NA, NA, NA, NA, NA, NA),
+	PINGROUP(31, hdmi_cec, pwr_modem, dac_calib17, NA, NA, NA, NA, NA, NA),
+	PINGROUP(32, hdmi_ddc, pwr_nav, NA, dac_calib18, NA, NA, NA, NA, NA),
+	PINGROUP(33, hdmi_ddc, pwr_crypto, NA, dac_calib19, NA, NA, NA, NA, NA),
+	PINGROUP(34, hdmi_hot, NA, dac_calib20, NA, NA, NA, NA, NA, NA),
+	PINGROUP(35, pci_e0, NA, dac_calib21, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36, pci_e0, NA, dac_calib22, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37, NA, dac_calib23, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38, NA, dac_calib24, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39, tsif1_sync, NA, dac_calib25, NA, NA, NA, NA, NA, NA),
+	PINGROUP(40, sd_write, tsif1_error, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(41, blsp_spi2, blsp_uart2, blsp_uim2, NA, qdss_cti,
+		 dac_calib0, NA, NA, NA),
+	PINGROUP(42, blsp_spi2, blsp_uart2, blsp_uim2, NA, qdss_cti,
+		 dac_calib1, NA, NA, NA),
+	PINGROUP(43, blsp_spi2, blsp_uart2, blsp_i2c2, NA, dac_calib2, NA, NA,
+		 NA, NA),
+	PINGROUP(44, blsp_spi2, blsp_uart2, blsp_i2c2, NA, dac_calib3, NA, NA,
+		 NA, NA),
+	PINGROUP(45, blsp_spi3, blsp_uart3, blsp_uim3, NA, dac_calib4, NA, NA,
+		 NA, NA),
+	PINGROUP(46, blsp_spi3, blsp_uart3, blsp_uim3, NA, dac_calib5, NA, NA,
+		 NA, NA),
+	PINGROUP(47, blsp_spi3, blsp_uart3, blsp_i2c3, dac_calib6, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(48, blsp_spi3, blsp_uart3, blsp_i2c3, dac_calib7, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(49, uim3, blsp_spi9, blsp_uart9, blsp_uim9, blsp10_spi,
+		 dac_calib8, NA, NA, NA),
+	PINGROUP(50, uim3, blsp_spi9, blsp_uart9, blsp_uim9, blsp10_spi,
+		 dac_calib9, NA, NA, NA),
+	PINGROUP(51, uim3, blsp_spi9, blsp_uart9, blsp_i2c9, blsp10_spi,
+		 dac_calib10, NA, NA, NA),
+	PINGROUP(52, uim3, blsp_spi9, blsp_uart9, blsp_i2c9,
+		 blsp10_spi, dac_calib11, NA, NA, NA),
+	PINGROUP(53, blsp_spi7, blsp_uart7, blsp_uim7, NA, qdss_tracedata_a,
+		 dac_calib12, NA, NA, NA),
+	PINGROUP(54, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA,
+		 qdss_tracedata_a, dac_calib13, NA, NA),
+	PINGROUP(55, blsp_spi7, blsp_uart7, blsp_i2c7, NA, dac_calib14, NA, NA,
+		 NA, NA),
+	PINGROUP(56, blsp_spi7, blsp_uart7, blsp_i2c7, NA, dac_calib15, NA, NA,
+		 NA, NA),
+	PINGROUP(57, qua_mi2s, gcc_gp1_clk_a, NA, qdss_tracedata_b,
+		 dac_calib16, NA, NA, NA, NA),
+	PINGROUP(58, qua_mi2s, uim4, blsp_spi11, blsp_uart11, blsp_uim11,
+		 gcc_gp2_clk_a, NA, qdss_tracedata_b, dac_calib17),
+	PINGROUP(59, qua_mi2s, uim4, blsp_spi11, blsp_uart11, blsp_uim11,
+		 gcc_gp3_clk_a, NA, dac_calib18, NA),
+	PINGROUP(60, qua_mi2s, uim4, blsp_spi11, blsp_uart11, blsp_i2c11,
+		 cri_trng0, NA, dac_calib19, NA),
+	PINGROUP(61, qua_mi2s, uim4, blsp_spi11, blsp_uart11,
+		 blsp_i2c11, cri_trng1, NA, dac_calib20, NA),
+	PINGROUP(62, qua_mi2s, cri_trng, NA, dac_calib21, NA, NA, NA, NA, NA),
+	PINGROUP(63, qua_mi2s, NA, NA, qdss_stm18, qdss_tracedata_a,
+		 dac_calib22, NA, NA, NA),
+	PINGROUP(64, pri_mi2s, NA, qdss_stm17, qdss_tracedata_a, dac_calib23,
+		 NA, NA, NA, NA),
+	PINGROUP(65, pri_mi2s, blsp_spi4, blsp_uart4, blsp_uim4, NA,
+		 qdss_stm16, qdss_tracedata_a, dac_calib24, NA),
+	PINGROUP(66, pri_mi2s, blsp_spi4, blsp_uart4, blsp_uim4, NA,
+		 qdss_stm15, qdss_tracedata_a, dac_calib25, NA),
+	PINGROUP(67, pri_mi2s, blsp_spi4, blsp_uart4, blsp_i2c4, qdss_stm14,
+		 qdss_tracedata_a, dac_calib26, NA, NA),
+	PINGROUP(68, pri_mi2s, blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(69, spkr_i2s, audio_ref, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(70, lpass_slimbus, spkr_i2s, isense_dbg, NA, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(71, lpass_slimbus, spkr_i2s, tsense_pwm1, tsense_pwm2, NA, NA,
+		 NA, NA, NA),
+	PINGROUP(72, lpass_slimbus, spkr_i2s, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(73, btfm_slimbus, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74, btfm_slimbus, ter_mi2s, qdss_stm22, qdss_tracedata_a, NA,
+		 NA, NA, NA, NA),
+	PINGROUP(75, ter_mi2s, qdss_stm21, qdss_tracedata_a, NA, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(76, ter_mi2s, qdss_stm20, qdss_tracedata_a, NA, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(77, ter_mi2s, qdss_stm19, qdss_tracedata_a, NA, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(78, ter_mi2s, gcc_gp1_clk_b, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(79, sec_mi2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(80, sec_mi2s, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(81, sec_mi2s, blsp_spi5, blsp_uart5, blsp_uim5, gcc_gp2_clk_b,
+		 NA, NA, NA, NA),
+	PINGROUP(82, sec_mi2s, blsp_spi5, blsp_uart5, blsp_uim5, gcc_gp3_clk_b,
+		 NA, NA, NA, NA),
+	PINGROUP(83, sec_mi2s, blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(84, blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA, NA, NA, NA),
+	PINGROUP(85, blsp_spi12, blsp_uart12, blsp_uim12, NA, qdss_stm25,
+		 qdss_tracedata_a, NA, NA, NA),
+	PINGROUP(86, blsp_spi12, blsp_uart12, blsp_uim12, NA, NA, qdss_stm31,
+		 qdss_tracedata_a, NA, NA),
+	PINGROUP(87, blsp_spi12, blsp_uart12, blsp_i2c12, NA, qdss_stm30,
+		 qdss_tracedata_a, NA, NA, NA),
+	PINGROUP(88, blsp_spi12, blsp_uart12, blsp_i2c12, blsp10_spi, NA,
+		 qdss_stm29, NA, NA, NA),
+	PINGROUP(89, tsif1_clk, qdss_stm28, qdss_tracedata_a, NA, NA, NA, NA,
+		 NA, NA),
+	PINGROUP(90, tsif1_en, blsp1_spi, qdss_tracedata_a, NA, NA, NA, NA, NA,
+		 NA),
+	PINGROUP(91, tsif1_data, sdc4_cmd, qdss_stm27, qdss_traceclk_b, NA, NA,
+		 NA, NA, NA),
+	PINGROUP(92, tsif2_error, sdc43, vfr_1, qdss_stm26, qdss_tracedata_b,
+		 NA, NA, NA, NA),
+	PINGROUP(93, tsif2_clk, sdc4_clk, NA, qdss_stm24, qdss_tracedata_b, NA,
+		 NA, NA, NA),
+	PINGROUP(94, tsif2_en, sdc42, NA, qdss_stm23, qdss_tracectl_b, NA, NA,
+		 NA, NA),
+	PINGROUP(95, tsif2_data, sdc41, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(96, tsif2_sync, sdc40, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(97, NA, NA, mdp_vsync_p_b, ldo_en, NA, NA, NA, NA, NA),
+	PINGROUP(98, NA, NA, mdp_vsync_s_b, ldo_update, NA, NA, NA, NA, NA),
+	PINGROUP(99, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(100, NA, NA, blsp11_uart_tx_b, qdss_cti, NA, NA, NA, NA, NA),
+	PINGROUP(101, NA, blsp11_uart_rx_b, qdss_cti, NA, NA, NA, NA, NA, NA),
+	PINGROUP(102, NA, blsp11_i2c_sda_b, prng_rosc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(103, NA, blsp11_i2c_scl_b, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(104, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(105, uim2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(106, uim2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(107, uim2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(108, uim2, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(109, uim1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(110, uim1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(111, uim1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(112, uim1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(113, uim_batt, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(114, NA, pci_e2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(115, NA, pci_e2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(116, NA, pa_indicator, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(117, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(118, adsp_ext, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(119, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(120, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(121, ddr_bist, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(122, ddr_bist, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(123, ddr_bist, qdss_tracedata_11, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(124, ddr_bist, qdss_tracedata_12, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(125, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(126, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(127, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(128, NA, modem_tsync, nav_dr, nav_pps, NA, NA, NA, NA, NA),
+	PINGROUP(129, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(130, pci_e1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(131, pci_e1, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(132, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(133, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(134, gsm_tx, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(135, gsm_tx, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(136, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(137, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(138, NA, qspi_cs, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(139, NA, ssbi2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(140, NA, ssbi1, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(141, NA, qspi_cs, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(142, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(143, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(144, mss_lte, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(145, mss_lte, qspi_clk, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(146, NA, qspi0, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(147, NA, qspi1, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(148, NA, qspi2, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(149, NA, qspi3, NA, NA, NA, NA, NA, NA, NA),
+	SDC_QDSD_PINGROUP(sdc1_clk, 0x12c000, 13, 6),
+	SDC_QDSD_PINGROUP(sdc1_cmd, 0x12c000, 11, 3),
+	SDC_QDSD_PINGROUP(sdc1_data, 0x12c000, 9, 0),
+	SDC_QDSD_PINGROUP(sdc2_clk, 0x12d000, 14, 6),
+	SDC_QDSD_PINGROUP(sdc2_cmd, 0x12d000, 11, 3),
+	SDC_QDSD_PINGROUP(sdc2_data, 0x12d000, 9, 0),
+	SDC_QDSD_PINGROUP(sdc1_rclk, 0x12c000, 15, 0),
+};
+
+static const struct msm_pinctrl_soc_data msm8996_pinctrl = {
+	.pins = msm8996_pins,
+	.npins = ARRAY_SIZE(msm8996_pins),
+	.functions = msm8996_functions,
+	.nfunctions = ARRAY_SIZE(msm8996_functions),
+	.groups = msm8996_groups,
+	.ngroups = ARRAY_SIZE(msm8996_groups),
+	.ngpios = 150,
+};
+
+static int msm8996_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &msm8996_pinctrl);
+}
+
+static const struct of_device_id msm8996_pinctrl_of_match[] = {
+	{ .compatible = "qcom,msm8996-pinctrl", },
+	{ }
+};
+
+static struct platform_driver msm8996_pinctrl_driver = {
+	.driver = {
+		.name = "msm8996-pinctrl",
+		.of_match_table = msm8996_pinctrl_of_match,
+	},
+	.probe = msm8996_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init msm8996_pinctrl_init(void)
+{
+	return platform_driver_register(&msm8996_pinctrl_driver);
+}
+arch_initcall(msm8996_pinctrl_init);
+
+static void __exit msm8996_pinctrl_exit(void)
+{
+	platform_driver_unregister(&msm8996_pinctrl_driver);
+}
+module_exit(msm8996_pinctrl_exit);
+
+MODULE_DESCRIPTION("Qualcomm msm8996 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, msm8996_pinctrl_of_match);
diff --git a/drivers/pinctrl/qcom/pinctrl-qdf2xxx.c b/drivers/pinctrl/qcom/pinctrl-qdf2xxx.c
index e9ff3bc150bb..f448534edf46 100644
--- a/drivers/pinctrl/qcom/pinctrl-qdf2xxx.c
+++ b/drivers/pinctrl/qcom/pinctrl-qdf2xxx.c
@@ -32,6 +32,9 @@
 
 static struct msm_pinctrl_soc_data qdf2xxx_pinctrl;
 
+/* A reasonable limit to the number of GPIOS */
+#define MAX_GPIOS	256
+
 static int qdf2xxx_pinctrl_probe(struct platform_device *pdev)
 {
 	struct pinctrl_pin_desc *pins;
@@ -42,11 +45,13 @@ static int qdf2xxx_pinctrl_probe(struct platform_device *pdev)
 
 	/* Query the number of GPIOs from ACPI */
 	ret = device_property_read_u32(&pdev->dev, "num-gpios", &num_gpios);
-	if (ret < 0)
+	if (ret < 0) {
+		dev_warn(&pdev->dev, "missing num-gpios property\n");
 		return ret;
+	}
 
-	if (!num_gpios) {
-		dev_warn(&pdev->dev, "missing num-gpios property\n");
+	if (!num_gpios || num_gpios > MAX_GPIOS) {
+		dev_warn(&pdev->dev, "invalid num-gpios property\n");
 		return -ENODEV;
 	}
 
@@ -55,6 +60,9 @@ static int qdf2xxx_pinctrl_probe(struct platform_device *pdev)
 	groups = devm_kcalloc(&pdev->dev, num_gpios,
 		sizeof(struct msm_pingroup), GFP_KERNEL);
 
+	if (!pins || !groups)
+		return -ENOMEM;
+
 	for (i = 0; i < num_gpios; i++) {
 		pins[i].number = i;
 
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
index 6c42ca14d2fd..4e12ded3c773 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-gpio.c
@@ -14,6 +14,7 @@
 #include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_irq.h>
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/pinctrl/pinconf.h>
 #include <linux/pinctrl/pinmux.h>
@@ -158,11 +159,6 @@ static const char *const pmic_gpio_functions[] = {
 	PMIC_GPIO_FUNC_DTEST3, PMIC_GPIO_FUNC_DTEST4,
 };
 
-static inline struct pmic_gpio_state *to_gpio_state(struct gpio_chip *chip)
-{
-	return container_of(chip, struct pmic_gpio_state, chip);
-};
-
 static int pmic_gpio_read(struct pmic_gpio_state *state,
 			  struct pmic_gpio_pad *pad, unsigned int addr)
 {
@@ -495,7 +491,7 @@ static const struct pinconf_ops pmic_gpio_pinconf_ops = {
 
 static int pmic_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_INPUT_ENABLE, 1);
@@ -506,7 +502,7 @@ static int pmic_gpio_direction_input(struct gpio_chip *chip, unsigned pin)
 static int pmic_gpio_direction_output(struct gpio_chip *chip,
 				      unsigned pin, int val)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_OUTPUT, val);
@@ -516,7 +512,7 @@ static int pmic_gpio_direction_output(struct gpio_chip *chip,
 
 static int pmic_gpio_get(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	struct pmic_gpio_pad *pad;
 	int ret;
 
@@ -533,12 +529,12 @@ static int pmic_gpio_get(struct gpio_chip *chip, unsigned pin)
 		pad->out_value = ret & PMIC_MPP_REG_RT_STS_VAL_MASK;
 	}
 
-	return pad->out_value;
+	return !!pad->out_value;
 }
 
 static void pmic_gpio_set(struct gpio_chip *chip, unsigned pin, int value)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_OUTPUT, value);
@@ -561,7 +557,7 @@ static int pmic_gpio_of_xlate(struct gpio_chip *chip,
 
 static int pmic_gpio_to_irq(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	struct pmic_gpio_pad *pad;
 
 	pad = state->ctrl->desc->pins[pin].drv_data;
@@ -571,7 +567,7 @@ static int pmic_gpio_to_irq(struct gpio_chip *chip, unsigned pin)
 
 static void pmic_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct pmic_gpio_state *state = to_gpio_state(chip);
+	struct pmic_gpio_state *state = gpiochip_get_data(chip);
 	unsigned i;
 
 	for (i = 0; i < chip->ngpio; i++) {
@@ -693,18 +689,19 @@ static int pmic_gpio_probe(struct platform_device *pdev)
 	struct pmic_gpio_pad *pad, *pads;
 	struct pmic_gpio_state *state;
 	int ret, npins, i;
-	u32 res[2];
+	u32 reg;
 
-	ret = of_property_read_u32_array(dev->of_node, "reg", res, 2);
+	ret = of_property_read_u32(dev->of_node, "reg", &reg);
 	if (ret < 0) {
-		dev_err(dev, "missing base address and/or range");
+		dev_err(dev, "missing base address");
 		return ret;
 	}
 
-	npins = res[1] / PMIC_GPIO_ADDRESS_RANGE;
-
+	npins = platform_irq_count(pdev);
 	if (!npins)
 		return -EINVAL;
+	if (npins < 0)
+		return npins;
 
 	BUG_ON(npins > ARRAY_SIZE(pmic_gpio_groups));
 
@@ -752,7 +749,7 @@ static int pmic_gpio_probe(struct platform_device *pdev)
 		if (pad->irq < 0)
 			return pad->irq;
 
-		pad->base = res[0] + i * PMIC_GPIO_ADDRESS_RANGE;
+		pad->base = reg + i * PMIC_GPIO_ADDRESS_RANGE;
 
 		ret = pmic_gpio_populate(state, pad);
 		if (ret < 0)
@@ -760,7 +757,7 @@ static int pmic_gpio_probe(struct platform_device *pdev)
 	}
 
 	state->chip = pmic_gpio_gpio_template;
-	state->chip.dev = dev;
+	state->chip.parent = dev;
 	state->chip.base = -1;
 	state->chip.ngpio = npins;
 	state->chip.label = dev_name(dev);
@@ -771,7 +768,7 @@ static int pmic_gpio_probe(struct platform_device *pdev)
 	if (IS_ERR(state->ctrl))
 		return PTR_ERR(state->ctrl);
 
-	ret = gpiochip_add(&state->chip);
+	ret = gpiochip_add_data(&state->chip, state);
 	if (ret) {
 		dev_err(state->dev, "can't add gpio chip\n");
 		goto err_chip;
@@ -804,6 +801,7 @@ static int pmic_gpio_remove(struct platform_device *pdev)
 static const struct of_device_id pmic_gpio_of_match[] = {
 	{ .compatible = "qcom,pm8916-gpio" },	/* 4 GPIO's */
 	{ .compatible = "qcom,pm8941-gpio" },	/* 36 GPIO's */
+	{ .compatible = "qcom,pm8994-gpio" },	/* 22 GPIO's */
 	{ .compatible = "qcom,pma8084-gpio" },	/* 22 GPIO's */
 	{ },
 };
diff --git a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
index 9ce0e30e33e8..2f18323571a6 100644
--- a/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
+++ b/drivers/pinctrl/qcom/pinctrl-spmi-mpp.c
@@ -14,6 +14,7 @@
 #include <linux/gpio.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_irq.h>
 #include <linux/pinctrl/pinconf-generic.h>
 #include <linux/pinctrl/pinconf.h>
 #include <linux/pinctrl/pinmux.h>
@@ -178,11 +179,6 @@ static const char *const pmic_mpp_functions[] = {
 	"digital", "analog", "sink"
 };
 
-static inline struct pmic_mpp_state *to_mpp_state(struct gpio_chip *chip)
-{
-	return container_of(chip, struct pmic_mpp_state, chip);
-};
-
 static int pmic_mpp_read(struct pmic_mpp_state *state,
 			 struct pmic_mpp_pad *pad, unsigned int addr)
 {
@@ -556,7 +552,7 @@ static const struct pinconf_ops pmic_mpp_pinconf_ops = {
 
 static int pmic_mpp_direction_input(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_INPUT_ENABLE, 1);
@@ -567,7 +563,7 @@ static int pmic_mpp_direction_input(struct gpio_chip *chip, unsigned pin)
 static int pmic_mpp_direction_output(struct gpio_chip *chip,
 				     unsigned pin, int val)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_OUTPUT, val);
@@ -577,7 +573,7 @@ static int pmic_mpp_direction_output(struct gpio_chip *chip,
 
 static int pmic_mpp_get(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	struct pmic_mpp_pad *pad;
 	int ret;
 
@@ -591,12 +587,12 @@ static int pmic_mpp_get(struct gpio_chip *chip, unsigned pin)
 		pad->out_value = ret & PMIC_MPP_REG_RT_STS_VAL_MASK;
 	}
 
-	return pad->out_value;
+	return !!pad->out_value;
 }
 
 static void pmic_mpp_set(struct gpio_chip *chip, unsigned pin, int value)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	unsigned long config;
 
 	config = pinconf_to_config_packed(PIN_CONFIG_OUTPUT, value);
@@ -619,7 +615,7 @@ static int pmic_mpp_of_xlate(struct gpio_chip *chip,
 
 static int pmic_mpp_to_irq(struct gpio_chip *chip, unsigned pin)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	struct pmic_mpp_pad *pad;
 
 	pad = state->ctrl->desc->pins[pin].drv_data;
@@ -629,7 +625,7 @@ static int pmic_mpp_to_irq(struct gpio_chip *chip, unsigned pin)
 
 static void pmic_mpp_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 {
-	struct pmic_mpp_state *state = to_mpp_state(chip);
+	struct pmic_mpp_state *state = gpiochip_get_data(chip);
 	unsigned i;
 
 	for (i = 0; i < chip->ngpio; i++) {
@@ -795,17 +791,19 @@ static int pmic_mpp_probe(struct platform_device *pdev)
 	struct pmic_mpp_pad *pad, *pads;
 	struct pmic_mpp_state *state;
 	int ret, npins, i;
-	u32 res[2];
+	u32 reg;
 
-	ret = of_property_read_u32_array(dev->of_node, "reg", res, 2);
+	ret = of_property_read_u32(dev->of_node, "reg", &reg);
 	if (ret < 0) {
-		dev_err(dev, "missing base address and/or range");
+		dev_err(dev, "missing base address");
 		return ret;
 	}
 
-	npins = res[1] / PMIC_MPP_ADDRESS_RANGE;
+	npins = platform_irq_count(pdev);
 	if (!npins)
 		return -EINVAL;
+	if (npins < 0)
+		return npins;
 
 	BUG_ON(npins > ARRAY_SIZE(pmic_mpp_groups));
 
@@ -854,7 +852,7 @@ static int pmic_mpp_probe(struct platform_device *pdev)
 		if (pad->irq < 0)
 			return pad->irq;
 
-		pad->base = res[0] + i * PMIC_MPP_ADDRESS_RANGE;
+		pad->base = reg + i * PMIC_MPP_ADDRESS_RANGE;
 
 		ret = pmic_mpp_populate(state, pad);
 		if (ret < 0)
@@ -862,7 +860,7 @@ static int pmic_mpp_probe(struct platform_device *pdev)
 	}
 
 	state->chip = pmic_mpp_gpio_template;
-	state->chip.dev = dev;
+	state->chip.parent = dev;
 	state->chip.base = -1;
 	state->chip.ngpio = npins;
 	state->chip.label = dev_name(dev);
@@ -873,7 +871,7 @@ static int pmic_mpp_probe(struct platform_device *pdev)
 	if (IS_ERR(state->ctrl))
 		return PTR_ERR(state->ctrl);
 
-	ret = gpiochip_add(&state->chip);
+	ret = gpiochip_add_data(&state->chip, state);
 	if (ret) {
 		dev_err(state->dev, "can't add gpio chip\n");
 		goto err_chip;
@@ -907,6 +905,7 @@ static const struct of_device_id pmic_mpp_of_match[] = {
 	{ .compatible = "qcom,pm8841-mpp" },	/* 4 MPP's */
 	{ .compatible = "qcom,pm8916-mpp" },	/* 4 MPP's */
 	{ .compatible = "qcom,pm8941-mpp" },	/* 8 MPP's */
+	{ .compatible = "qcom,pm8994-mpp" },	/* 8 MPP's */
 	{ .compatible = "qcom,pma8084-mpp" },	/* 8 MPP's */
 	{ },
 };
diff --git a/drivers/pinctrl/qcom/pinctrl-ssbi-gpio.c b/drivers/pinctrl/qcom/pinctrl-ssbi-gpio.c
index 19a3c3bc2f1f..cd8580d9741d 100644
--- a/drivers/pinctrl/qcom/pinctrl-ssbi-gpio.c
+++ b/drivers/pinctrl/qcom/pinctrl-ssbi-gpio.c
@@ -23,6 +23,7 @@
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 
 #include <dt-bindings/pinctrl/qcom,pmic-gpio.h>
 
@@ -443,7 +444,7 @@ static struct pinctrl_desc pm8xxx_pinctrl_desc = {
 static int pm8xxx_gpio_direction_input(struct gpio_chip *chip,
 				       unsigned offset)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 	u8 val;
 
@@ -459,7 +460,7 @@ static int pm8xxx_gpio_direction_output(struct gpio_chip *chip,
 					unsigned offset,
 					int value)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 	u8 val;
 
@@ -477,7 +478,7 @@ static int pm8xxx_gpio_direction_output(struct gpio_chip *chip,
 
 static int pm8xxx_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 	bool state;
 	int ret;
@@ -495,7 +496,7 @@ static int pm8xxx_gpio_get(struct gpio_chip *chip, unsigned offset)
 
 static void pm8xxx_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 	u8 val;
 
@@ -524,7 +525,7 @@ static int pm8xxx_gpio_of_xlate(struct gpio_chip *chip,
 
 static int pm8xxx_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	return pin->irq;
@@ -539,7 +540,7 @@ static void pm8xxx_gpio_dbg_show_one(struct seq_file *s,
 				  unsigned offset,
 				  unsigned gpio)
 {
-	struct pm8xxx_gpio *pctrl = container_of(chip, struct pm8xxx_gpio, chip);
+	struct pm8xxx_gpio *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	static const char * const modes[] = {
@@ -650,11 +651,12 @@ static int pm8xxx_pin_populate(struct pm8xxx_gpio *pctrl,
 }
 
 static const struct of_device_id pm8xxx_gpio_of_match[] = {
-	{ .compatible = "qcom,pm8018-gpio", .data = (void *)6 },
-	{ .compatible = "qcom,pm8038-gpio", .data = (void *)12 },
-	{ .compatible = "qcom,pm8058-gpio", .data = (void *)40 },
-	{ .compatible = "qcom,pm8917-gpio", .data = (void *)38 },
-	{ .compatible = "qcom,pm8921-gpio", .data = (void *)44 },
+	{ .compatible = "qcom,pm8018-gpio" },
+	{ .compatible = "qcom,pm8038-gpio" },
+	{ .compatible = "qcom,pm8058-gpio" },
+	{ .compatible = "qcom,pm8917-gpio" },
+	{ .compatible = "qcom,pm8921-gpio" },
+	{ .compatible = "qcom,ssbi-gpio" },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, pm8xxx_gpio_of_match);
@@ -665,14 +667,19 @@ static int pm8xxx_gpio_probe(struct platform_device *pdev)
 	struct pinctrl_pin_desc *pins;
 	struct pm8xxx_gpio *pctrl;
 	int ret;
-	int i;
+	int i, npins;
 
 	pctrl = devm_kzalloc(&pdev->dev, sizeof(*pctrl), GFP_KERNEL);
 	if (!pctrl)
 		return -ENOMEM;
 
 	pctrl->dev = &pdev->dev;
-	pctrl->npins = (unsigned long)of_device_get_match_data(&pdev->dev);
+	npins = platform_irq_count(pdev);
+	if (!npins)
+		return -EINVAL;
+	if (npins < 0)
+		return npins;
+	pctrl->npins = npins;
 
 	pctrl->regmap = dev_get_regmap(pdev->dev.parent, NULL);
 	if (!pctrl->regmap) {
@@ -730,12 +737,12 @@ static int pm8xxx_gpio_probe(struct platform_device *pdev)
 
 	pctrl->chip = pm8xxx_gpio_template;
 	pctrl->chip.base = -1;
-	pctrl->chip.dev = &pdev->dev;
+	pctrl->chip.parent = &pdev->dev;
 	pctrl->chip.of_node = pdev->dev.of_node;
 	pctrl->chip.of_gpio_n_cells = 2;
 	pctrl->chip.label = dev_name(pctrl->dev);
 	pctrl->chip.ngpio = pctrl->npins;
-	ret = gpiochip_add(&pctrl->chip);
+	ret = gpiochip_add_data(&pctrl->chip, pctrl);
 	if (ret) {
 		dev_err(&pdev->dev, "failed register gpiochip\n");
 		goto unregister_pinctrl;
diff --git a/drivers/pinctrl/qcom/pinctrl-ssbi-mpp.c b/drivers/pinctrl/qcom/pinctrl-ssbi-mpp.c
index b868ef1766a0..54a5402a9079 100644
--- a/drivers/pinctrl/qcom/pinctrl-ssbi-mpp.c
+++ b/drivers/pinctrl/qcom/pinctrl-ssbi-mpp.c
@@ -23,6 +23,7 @@
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/of_device.h>
+#include <linux/of_irq.h>
 
 #include <dt-bindings/pinctrl/qcom,pmic-mpp.h>
 
@@ -448,7 +449,7 @@ static struct pinctrl_desc pm8xxx_pinctrl_desc = {
 static int pm8xxx_mpp_direction_input(struct gpio_chip *chip,
 				       unsigned offset)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	switch (pin->mode) {
@@ -472,7 +473,7 @@ static int pm8xxx_mpp_direction_output(struct gpio_chip *chip,
 					unsigned offset,
 					int value)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	switch (pin->mode) {
@@ -496,13 +497,13 @@ static int pm8xxx_mpp_direction_output(struct gpio_chip *chip,
 
 static int pm8xxx_mpp_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 	bool state;
 	int ret;
 
 	if (!pin->input)
-		return pin->output_value;
+		return !!pin->output_value;
 
 	ret = irq_get_irqchip_state(pin->irq, IRQCHIP_STATE_LINE_LEVEL, &state);
 	if (!ret)
@@ -513,7 +514,7 @@ static int pm8xxx_mpp_get(struct gpio_chip *chip, unsigned offset)
 
 static void pm8xxx_mpp_set(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	pin->output_value = !!value;
@@ -537,7 +538,7 @@ static int pm8xxx_mpp_of_xlate(struct gpio_chip *chip,
 
 static int pm8xxx_mpp_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	return pin->irq;
@@ -552,7 +553,7 @@ static void pm8xxx_mpp_dbg_show_one(struct seq_file *s,
 				  unsigned offset,
 				  unsigned gpio)
 {
-	struct pm8xxx_mpp *pctrl = container_of(chip, struct pm8xxx_mpp, chip);
+	struct pm8xxx_mpp *pctrl = gpiochip_get_data(chip);
 	struct pm8xxx_pin_data *pin = pctrl->desc.pins[offset].drv_data;
 
 	static const char * const aout_lvls[] = {
@@ -741,11 +742,12 @@ static int pm8xxx_pin_populate(struct pm8xxx_mpp *pctrl,
 }
 
 static const struct of_device_id pm8xxx_mpp_of_match[] = {
-	{ .compatible = "qcom,pm8018-mpp", .data = (void *)6 },
-	{ .compatible = "qcom,pm8038-mpp", .data = (void *)6 },
-	{ .compatible = "qcom,pm8917-mpp", .data = (void *)10 },
-	{ .compatible = "qcom,pm8821-mpp", .data = (void *)4 },
-	{ .compatible = "qcom,pm8921-mpp", .data = (void *)12 },
+	{ .compatible = "qcom,pm8018-mpp" },
+	{ .compatible = "qcom,pm8038-mpp" },
+	{ .compatible = "qcom,pm8917-mpp" },
+	{ .compatible = "qcom,pm8821-mpp" },
+	{ .compatible = "qcom,pm8921-mpp" },
+	{ .compatible = "qcom,ssbi-mpp" },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, pm8xxx_mpp_of_match);
@@ -756,14 +758,19 @@ static int pm8xxx_mpp_probe(struct platform_device *pdev)
 	struct pinctrl_pin_desc *pins;
 	struct pm8xxx_mpp *pctrl;
 	int ret;
-	int i;
+	int i, npins;
 
 	pctrl = devm_kzalloc(&pdev->dev, sizeof(*pctrl), GFP_KERNEL);
 	if (!pctrl)
 		return -ENOMEM;
 
 	pctrl->dev = &pdev->dev;
-	pctrl->npins = (unsigned long)of_device_get_match_data(&pdev->dev);
+	npins = platform_irq_count(pdev);
+	if (!npins)
+		return -EINVAL;
+	if (npins < 0)
+		return npins;
+	pctrl->npins = npins;
 
 	pctrl->regmap = dev_get_regmap(pdev->dev.parent, NULL);
 	if (!pctrl->regmap) {
@@ -821,12 +828,12 @@ static int pm8xxx_mpp_probe(struct platform_device *pdev)
 
 	pctrl->chip = pm8xxx_mpp_template;
 	pctrl->chip.base = -1;
-	pctrl->chip.dev = &pdev->dev;
+	pctrl->chip.parent = &pdev->dev;
 	pctrl->chip.of_node = pdev->dev.of_node;
 	pctrl->chip.of_gpio_n_cells = 2;
 	pctrl->chip.label = dev_name(pctrl->dev);
 	pctrl->chip.ngpio = pctrl->npins;
-	ret = gpiochip_add(&pctrl->chip);
+	ret = gpiochip_add_data(&pctrl->chip, pctrl);
 	if (ret) {
 		dev_err(&pdev->dev, "failed register gpiochip\n");
 		goto unregister_pinctrl;
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index 71ccf6a90b22..051b5bf701a8 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -176,7 +176,8 @@ static int exynos_irq_request_resources(struct irq_data *irqd)
 
 	ret = gpiochip_lock_as_irq(&bank->gpio_chip, irqd->hwirq);
 	if (ret) {
-		dev_err(bank->gpio_chip.dev, "unable to lock pin %s-%lu IRQ\n",
+		dev_err(bank->gpio_chip.parent,
+			"unable to lock pin %s-%lu IRQ\n",
 			bank->name, irqd->hwirq);
 		return ret;
 	}
@@ -1150,6 +1151,109 @@ const struct samsung_pin_ctrl exynos5260_pin_ctrl[] __initconst = {
 	},
 };
 
+/* pin banks of exynos5410 pin-controller 0 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks0[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpa0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(6, 0x020, "gpa1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpa2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(5, 0x060, "gpb0", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(5, 0x080, "gpb1", 0x10),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0A0, "gpb2", 0x14),
+	EXYNOS_PIN_BANK_EINTG(4, 0x0C0, "gpb3", 0x18),
+	EXYNOS_PIN_BANK_EINTG(7, 0x0E0, "gpc0", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(4, 0x100, "gpc3", 0x20),
+	EXYNOS_PIN_BANK_EINTG(7, 0x120, "gpc1", 0x24),
+	EXYNOS_PIN_BANK_EINTG(7, 0x140, "gpc2", 0x28),
+	EXYNOS_PIN_BANK_EINTN(2, 0x160, "gpm5"),
+	EXYNOS_PIN_BANK_EINTG(8, 0x180, "gpd1", 0x2c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x1A0, "gpe0", 0x30),
+	EXYNOS_PIN_BANK_EINTG(2, 0x1C0, "gpe1", 0x34),
+	EXYNOS_PIN_BANK_EINTG(6, 0x1E0, "gpf0", 0x38),
+	EXYNOS_PIN_BANK_EINTG(8, 0x200, "gpf1", 0x3c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x220, "gpg0", 0x40),
+	EXYNOS_PIN_BANK_EINTG(8, 0x240, "gpg1", 0x44),
+	EXYNOS_PIN_BANK_EINTG(2, 0x260, "gpg2", 0x48),
+	EXYNOS_PIN_BANK_EINTG(4, 0x280, "gph0", 0x4c),
+	EXYNOS_PIN_BANK_EINTG(8, 0x2A0, "gph1", 0x50),
+	EXYNOS_PIN_BANK_EINTN(8, 0x2C0, "gpm7"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x2E0, "gpy0"),
+	EXYNOS_PIN_BANK_EINTN(4, 0x300, "gpy1"),
+	EXYNOS_PIN_BANK_EINTN(6, 0x320, "gpy2"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x340, "gpy3"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x360, "gpy4"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x380, "gpy5"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x3A0, "gpy6"),
+	EXYNOS_PIN_BANK_EINTN(8, 0x3C0, "gpy7"),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC00, "gpx0", 0x00),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC20, "gpx1", 0x04),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC40, "gpx2", 0x08),
+	EXYNOS_PIN_BANK_EINTW(8, 0xC60, "gpx3", 0x0c),
+};
+
+/* pin banks of exynos5410 pin-controller 1 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks1[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(5, 0x000, "gpj0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpj1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x040, "gpj2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpj3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x080, "gpj4", 0x10),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0A0, "gpk0", 0x14),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0C0, "gpk1", 0x18),
+	EXYNOS_PIN_BANK_EINTG(8, 0x0E0, "gpk2", 0x1c),
+	EXYNOS_PIN_BANK_EINTG(7, 0x100, "gpk3", 0x20),
+};
+
+/* pin banks of exynos5410 pin-controller 2 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks2[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpv0", 0x00),
+	EXYNOS_PIN_BANK_EINTG(8, 0x020, "gpv1", 0x04),
+	EXYNOS_PIN_BANK_EINTG(8, 0x060, "gpv2", 0x08),
+	EXYNOS_PIN_BANK_EINTG(8, 0x080, "gpv3", 0x0c),
+	EXYNOS_PIN_BANK_EINTG(2, 0x0C0, "gpv4", 0x10),
+};
+
+/* pin banks of exynos5410 pin-controller 3 */
+static const struct samsung_pin_bank_data exynos5410_pin_banks3[] __initconst = {
+	EXYNOS_PIN_BANK_EINTG(7, 0x000, "gpz", 0x00),
+};
+
+/*
+ * Samsung pinctrl driver data for Exynos5410 SoC. Exynos5410 SoC includes
+ * four gpio/pin-mux/pinconfig controllers.
+ */
+const struct samsung_pin_ctrl exynos5410_pin_ctrl[] __initconst = {
+	{
+		/* pin-controller instance 0 data */
+		.pin_banks	= exynos5410_pin_banks0,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks0),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.eint_wkup_init = exynos_eint_wkup_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 1 data */
+		.pin_banks	= exynos5410_pin_banks1,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks1),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 2 data */
+		.pin_banks	= exynos5410_pin_banks2,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks2),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	}, {
+		/* pin-controller instance 3 data */
+		.pin_banks	= exynos5410_pin_banks3,
+		.nr_banks	= ARRAY_SIZE(exynos5410_pin_banks3),
+		.eint_gpio_init = exynos_eint_gpio_init,
+		.suspend	= exynos_pinctrl_suspend,
+		.resume		= exynos_pinctrl_resume,
+	},
+};
+
 /* pin banks of exynos5420 pin-controller 0 */
 static const struct samsung_pin_bank_data exynos5420_pin_banks0[] __initconst = {
 	EXYNOS_PIN_BANK_EINTG(8, 0x000, "gpy7", 0x00),
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos5440.c b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
index 82dc109f7ed4..00ab63abf1d9 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
@@ -15,7 +15,7 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/err.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/device.h>
 #include <linux/pinctrl/pinctrl.h>
 #include <linux/pinctrl/pinmux.h>
@@ -539,7 +539,7 @@ static const struct pinconf_ops exynos5440_pinconf_ops = {
 /* gpiolib gpio_set callback function */
 static void exynos5440_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	struct exynos5440_pinctrl_priv_data *priv = gpiochip_get_data(gc);
 	void __iomem *base = priv->reg_base;
 	u32 data;
 
@@ -553,7 +553,7 @@ static void exynos5440_gpio_set(struct gpio_chip *gc, unsigned offset, int value
 /* gpiolib gpio_get callback function */
 static int exynos5440_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	struct exynos5440_pinctrl_priv_data *priv = gpiochip_get_data(gc);
 	void __iomem *base = priv->reg_base;
 	u32 data;
 
@@ -566,7 +566,7 @@ static int exynos5440_gpio_get(struct gpio_chip *gc, unsigned offset)
 /* gpiolib gpio_direction_input callback function */
 static int exynos5440_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	struct exynos5440_pinctrl_priv_data *priv = gpiochip_get_data(gc);
 	void __iomem *base = priv->reg_base;
 	u32 data;
 
@@ -586,7 +586,7 @@ static int exynos5440_gpio_direction_input(struct gpio_chip *gc, unsigned offset
 static int exynos5440_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
 							int value)
 {
-	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	struct exynos5440_pinctrl_priv_data *priv = gpiochip_get_data(gc);
 	void __iomem *base = priv->reg_base;
 	u32 data;
 
@@ -607,7 +607,7 @@ static int exynos5440_gpio_direction_output(struct gpio_chip *gc, unsigned offse
 /* gpiolib gpio_to_irq callback function */
 static int exynos5440_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct exynos5440_pinctrl_priv_data *priv = dev_get_drvdata(gc->dev);
+	struct exynos5440_pinctrl_priv_data *priv = gpiochip_get_data(gc);
 	unsigned int virq;
 
 	if (offset < 16 || offset > 23)
@@ -817,7 +817,7 @@ static int exynos5440_gpiolib_register(struct platform_device *pdev,
 	priv->gc = gc;
 	gc->base = 0;
 	gc->ngpio = EXYNOS5440_MAX_PINS;
-	gc->dev = &pdev->dev;
+	gc->parent = &pdev->dev;
 	gc->set = exynos5440_gpio_set;
 	gc->get = exynos5440_gpio_get;
 	gc->direction_input = exynos5440_gpio_direction_input;
@@ -825,7 +825,7 @@ static int exynos5440_gpiolib_register(struct platform_device *pdev,
 	gc->to_irq = exynos5440_gpio_to_irq;
 	gc->label = "gpiolib-exynos5440";
 	gc->owner = THIS_MODULE;
-	ret = gpiochip_add(gc);
+	ret = gpiochip_add_data(gc, priv);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register gpio_chip %s, error "
 					"code: %d\n", gc->label, ret);
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index 3f622ccd8eab..5cc97f85db02 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -50,11 +50,6 @@ static LIST_HEAD(drvdata_list);
 
 static unsigned int pin_base;
 
-static inline struct samsung_pin_bank *gc_to_pin_bank(struct gpio_chip *gc)
-{
-	return container_of(gc, struct samsung_pin_bank, gpio_chip);
-}
-
 static int samsung_get_group_count(struct pinctrl_dev *pctldev)
 {
 	struct samsung_pinctrl_drv_data *pmx = pinctrl_dev_get_drvdata(pctldev);
@@ -519,25 +514,35 @@ static const struct pinconf_ops samsung_pinconf_ops = {
 	.pin_config_group_set	= samsung_pinconf_group_set,
 };
 
-/* gpiolib gpio_set callback function */
-static void samsung_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
+/*
+ * The samsung_gpio_set_vlaue() should be called with "bank->slock" held
+ * to avoid race condition.
+ */
+static void samsung_gpio_set_value(struct gpio_chip *gc,
+					  unsigned offset, int value)
 {
-	struct samsung_pin_bank *bank = gc_to_pin_bank(gc);
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
 	const struct samsung_pin_bank_type *type = bank->type;
-	unsigned long flags;
 	void __iomem *reg;
 	u32 data;
 
 	reg = bank->drvdata->virt_base + bank->pctl_offset;
 
-	spin_lock_irqsave(&bank->slock, flags);
-
 	data = readl(reg + type->reg_offset[PINCFG_TYPE_DAT]);
 	data &= ~(1 << offset);
 	if (value)
 		data |= 1 << offset;
 	writel(data, reg + type->reg_offset[PINCFG_TYPE_DAT]);
+}
 
+/* gpiolib gpio_set callback function */
+static void samsung_gpio_set(struct gpio_chip *gc, unsigned offset, int value)
+{
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&bank->slock, flags);
+	samsung_gpio_set_value(gc, offset, value);
 	spin_unlock_irqrestore(&bank->slock, flags);
 }
 
@@ -546,7 +551,7 @@ static int samsung_gpio_get(struct gpio_chip *gc, unsigned offset)
 {
 	void __iomem *reg;
 	u32 data;
-	struct samsung_pin_bank *bank = gc_to_pin_bank(gc);
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
 	const struct samsung_pin_bank_type *type = bank->type;
 
 	reg = bank->drvdata->virt_base + bank->pctl_offset;
@@ -558,6 +563,8 @@ static int samsung_gpio_get(struct gpio_chip *gc, unsigned offset)
 }
 
 /*
+ * The samsung_gpio_set_direction() should be called with "bank->slock" held
+ * to avoid race condition.
  * The calls to gpio_direction_output() and gpio_direction_input()
  * leads to this function call.
  */
@@ -569,9 +576,8 @@ static int samsung_gpio_set_direction(struct gpio_chip *gc,
 	struct samsung_pinctrl_drv_data *drvdata;
 	void __iomem *reg;
 	u32 data, mask, shift;
-	unsigned long flags;
 
-	bank = gc_to_pin_bank(gc);
+	bank = gpiochip_get_data(gc);
 	type = bank->type;
 	drvdata = bank->drvdata;
 
@@ -586,31 +592,42 @@ static int samsung_gpio_set_direction(struct gpio_chip *gc,
 		reg += 4;
 	}
 
-	spin_lock_irqsave(&bank->slock, flags);
-
 	data = readl(reg);
 	data &= ~(mask << shift);
 	if (!input)
 		data |= FUNC_OUTPUT << shift;
 	writel(data, reg);
 
-	spin_unlock_irqrestore(&bank->slock, flags);
-
 	return 0;
 }
 
 /* gpiolib gpio_direction_input callback function. */
 static int samsung_gpio_direction_input(struct gpio_chip *gc, unsigned offset)
 {
-	return samsung_gpio_set_direction(gc, offset, true);
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&bank->slock, flags);
+	ret = samsung_gpio_set_direction(gc, offset, true);
+	spin_unlock_irqrestore(&bank->slock, flags);
+	return ret;
 }
 
 /* gpiolib gpio_direction_output callback function. */
 static int samsung_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
 							int value)
 {
-	samsung_gpio_set(gc, offset, value);
-	return samsung_gpio_set_direction(gc, offset, false);
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&bank->slock, flags);
+	samsung_gpio_set_value(gc, offset, value);
+	ret = samsung_gpio_set_direction(gc, offset, false);
+	spin_unlock_irqrestore(&bank->slock, flags);
+
+	return ret;
 }
 
 /*
@@ -619,7 +636,7 @@ static int samsung_gpio_direction_output(struct gpio_chip *gc, unsigned offset,
  */
 static int samsung_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
-	struct samsung_pin_bank *bank = gc_to_pin_bank(gc);
+	struct samsung_pin_bank *bank = gpiochip_get_data(gc);
 	unsigned int virq;
 
 	if (!bank->irq_domain)
@@ -914,11 +931,11 @@ static int samsung_gpiolib_register(struct platform_device *pdev,
 		gc = &bank->gpio_chip;
 		gc->base = drvdata->pin_base + bank->pin_base;
 		gc->ngpio = bank->nr_pins;
-		gc->dev = &pdev->dev;
+		gc->parent = &pdev->dev;
 		gc->of_node = bank->of_node;
 		gc->label = bank->name;
 
-		ret = gpiochip_add(gc);
+		ret = gpiochip_add_data(gc, bank);
 		if (ret) {
 			dev_err(&pdev->dev, "failed to register gpio_chip %s, error code: %d\n",
 							gc->label, ret);
@@ -1222,6 +1239,8 @@ static const struct of_device_id samsung_pinctrl_dt_match[] = {
 		.data = (void *)exynos5250_pin_ctrl },
 	{ .compatible = "samsung,exynos5260-pinctrl",
 		.data = (void *)exynos5260_pin_ctrl },
+	{ .compatible = "samsung,exynos5410-pinctrl",
+		.data = (void *)exynos5410_pin_ctrl },
 	{ .compatible = "samsung,exynos5420-pinctrl",
 		.data = (void *)exynos5420_pin_ctrl },
 	{ .compatible = "samsung,exynos5433-pinctrl",
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.h b/drivers/pinctrl/samsung/pinctrl-samsung.h
index c1239ff6157d..cd31bfaf62cb 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.h
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.h
@@ -270,6 +270,7 @@ extern const struct samsung_pin_ctrl exynos4x12_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos4415_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5250_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5260_pin_ctrl[];
+extern const struct samsung_pin_ctrl exynos5410_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5420_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos5433_pin_ctrl[];
 extern const struct samsung_pin_ctrl exynos7_pin_ctrl[];
diff --git a/drivers/pinctrl/sh-pfc/gpio.c b/drivers/pinctrl/sh-pfc/gpio.c
index db3f09aa8993..a6681b8b17c3 100644
--- a/drivers/pinctrl/sh-pfc/gpio.c
+++ b/drivers/pinctrl/sh-pfc/gpio.c
@@ -38,14 +38,10 @@ struct sh_pfc_chip {
 	struct sh_pfc_gpio_pin		*pins;
 };
 
-static struct sh_pfc_chip *gpio_to_pfc_chip(struct gpio_chip *gc)
-{
-	return container_of(gc, struct sh_pfc_chip, gpio_chip);
-}
-
 static struct sh_pfc *gpio_to_pfc(struct gpio_chip *gc)
 {
-	return gpio_to_pfc_chip(gc)->pfc;
+	struct sh_pfc_chip *chip = gpiochip_get_data(gc);
+	return chip->pfc;
 }
 
 static void gpio_get_data_reg(struct sh_pfc_chip *chip, unsigned int offset,
@@ -178,14 +174,14 @@ static int gpio_pin_direction_input(struct gpio_chip *gc, unsigned offset)
 static int gpio_pin_direction_output(struct gpio_chip *gc, unsigned offset,
 				    int value)
 {
-	gpio_pin_set_value(gpio_to_pfc_chip(gc), offset, value);
+	gpio_pin_set_value(gpiochip_get_data(gc), offset, value);
 
 	return pinctrl_gpio_direction_output(offset);
 }
 
 static int gpio_pin_get(struct gpio_chip *gc, unsigned offset)
 {
-	struct sh_pfc_chip *chip = gpio_to_pfc_chip(gc);
+	struct sh_pfc_chip *chip = gpiochip_get_data(gc);
 	struct sh_pfc_gpio_data_reg *reg;
 	unsigned int bit;
 	unsigned int pos;
@@ -199,7 +195,7 @@ static int gpio_pin_get(struct gpio_chip *gc, unsigned offset)
 
 static void gpio_pin_set(struct gpio_chip *gc, unsigned offset, int value)
 {
-	gpio_pin_set_value(gpio_to_pfc_chip(gc), offset, value);
+	gpio_pin_set_value(gpiochip_get_data(gc), offset, value);
 }
 
 static int gpio_pin_to_irq(struct gpio_chip *gc, unsigned offset)
@@ -246,7 +242,7 @@ static int gpio_pin_setup(struct sh_pfc_chip *chip)
 	gc->to_irq = gpio_pin_to_irq;
 
 	gc->label = pfc->info->name;
-	gc->dev = pfc->dev;
+	gc->parent = pfc->dev;
 	gc->owner = THIS_MODULE;
 	gc->base = 0;
 	gc->ngpio = pfc->nr_gpio_pins;
@@ -322,7 +318,7 @@ sh_pfc_add_gpiochip(struct sh_pfc *pfc, int(*setup)(struct sh_pfc_chip *),
 	if (ret < 0)
 		return ERR_PTR(ret);
 
-	ret = gpiochip_add(&chip->gpio_chip);
+	ret = gpiochip_add_data(&chip->gpio_chip, chip);
 	if (unlikely(ret < 0))
 		return ERR_PTR(ret);
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-emev2.c b/drivers/pinctrl/sh-pfc/pfc-emev2.c
index 02118ab336fc..1cbbe04d7df6 100644
--- a/drivers/pinctrl/sh-pfc/pfc-emev2.c
+++ b/drivers/pinctrl/sh-pfc/pfc-emev2.c
@@ -258,18 +258,18 @@ static const u16 pinmux_data[] = {
 
 	/* GPSR0 */
 	/* V9 */
-	PINMUX_DATA(JT_SEL_MARK, FN_JT_SEL),
+	PINMUX_SINGLE(JT_SEL),
 	/* U9 */
-	PINMUX_DATA(ERR_RST_REQB_MARK, FN_ERR_RST_REQB),
+	PINMUX_SINGLE(ERR_RST_REQB),
 	/* V8 */
-	PINMUX_DATA(REF_CLKO_MARK, FN_REF_CLKO),
+	PINMUX_SINGLE(REF_CLKO),
 	/* U8 */
-	PINMUX_DATA(EXT_CLKI_MARK, FN_EXT_CLKI),
+	PINMUX_SINGLE(EXT_CLKI),
 	/* B22*/
 	PINMUX_IPSR_NOFN(LCD3_1_0_PORT18, LCD3_PXCLK, SEL_LCD3_1_0_00),
 	PINMUX_IPSR_NOFN(LCD3_1_0_PORT18, YUV3_CLK_O, SEL_LCD3_1_0_01),
 	/* C21 */
-	PINMUX_DATA(LCD3_PXCLKB_MARK, FN_LCD3_PXCLKB),
+	PINMUX_SINGLE(LCD3_PXCLKB),
 	/* A21 */
 	PINMUX_IPSR_NOFN(LCD3_1_0_PORT20, LCD3_CLK_I, SEL_LCD3_1_0_00),
 	PINMUX_IPSR_NOFN(LCD3_1_0_PORT20, YUV3_CLK_I, SEL_LCD3_1_0_01),
@@ -285,17 +285,17 @@ static const u16 pinmux_data[] = {
 
 	/* GPSR1 */
 	/* A20 */
-	PINMUX_DATA(LCD3_R0_MARK, FN_LCD3_R0),
+	PINMUX_SINGLE(LCD3_R0),
 	/* B20 */
-	PINMUX_DATA(LCD3_R1_MARK, FN_LCD3_R1),
+	PINMUX_SINGLE(LCD3_R1),
 	/* A19 */
-	PINMUX_DATA(LCD3_R2_MARK, FN_LCD3_R2),
+	PINMUX_SINGLE(LCD3_R2),
 	/* B19 */
-	PINMUX_DATA(LCD3_R3_MARK, FN_LCD3_R3),
+	PINMUX_SINGLE(LCD3_R3),
 	/* C19 */
-	PINMUX_DATA(LCD3_R4_MARK, FN_LCD3_R4),
+	PINMUX_SINGLE(LCD3_R4),
 	/* B18 */
-	PINMUX_DATA(LCD3_R5_MARK, FN_LCD3_R5),
+	PINMUX_SINGLE(LCD3_R5),
 	/* C18 */
 	PINMUX_IPSR_NOFN(LCD3_9_8_PORT38, LCD3_R6, SEL_LCD3_9_8_00),
 	PINMUX_IPSR_NOFN(LCD3_9_8_PORT38, TP33_CLK, SEL_LCD3_9_8_10),
@@ -367,9 +367,9 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_NOFN(LCD3_11_10_PORT43, YUV3_D15, SEL_LCD3_11_10_01),
 	PINMUX_IPSR_NOFN(LCD3_11_10_PORT43, TP33_DATA15, SEL_LCD3_11_10_10),
 	/* AA9 */
-	PINMUX_DATA(IIC0_SCL_MARK, FN_IIC0_SCL),
+	PINMUX_SINGLE(IIC0_SCL),
 	/* AA8 */
-	PINMUX_DATA(IIC0_SDA_MARK, FN_IIC0_SDA),
+	PINMUX_SINGLE(IIC0_SDA),
 	/* Y9 */
 	PINMUX_IPSR_NOFN(IIC_1_0_PORT46, IIC1_SCL, SEL_IIC_1_0_00),
 	PINMUX_IPSR_NOFN(IIC_1_0_PORT46, UART3_RX, SEL_IIC_1_0_01),
@@ -377,51 +377,51 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_NOFN(IIC_1_0_PORT47, IIC1_SDA, SEL_IIC_1_0_00),
 	PINMUX_IPSR_NOFN(IIC_1_0_PORT47, UART3_TX, SEL_IIC_1_0_01),
 	/* AC19 */
-	PINMUX_DATA(SD_CKI_MARK, FN_SD_CKI),
+	PINMUX_SINGLE(SD_CKI),
 	/* AB18 */
-	PINMUX_DATA(SDI0_CKO_MARK, FN_SDI0_CKO),
+	PINMUX_SINGLE(SDI0_CKO),
 	/* AC18 */
-	PINMUX_DATA(SDI0_CKI_MARK, FN_SDI0_CKI),
+	PINMUX_SINGLE(SDI0_CKI),
 	/* Y12 */
-	PINMUX_DATA(SDI0_CMD_MARK, FN_SDI0_CMD),
+	PINMUX_SINGLE(SDI0_CMD),
 	/* AA13 */
-	PINMUX_DATA(SDI0_DATA0_MARK, FN_SDI0_DATA0),
+	PINMUX_SINGLE(SDI0_DATA0),
 	/* Y13 */
-	PINMUX_DATA(SDI0_DATA1_MARK, FN_SDI0_DATA1),
+	PINMUX_SINGLE(SDI0_DATA1),
 	/* AA14 */
-	PINMUX_DATA(SDI0_DATA2_MARK, FN_SDI0_DATA2),
+	PINMUX_SINGLE(SDI0_DATA2),
 	/* Y14 */
-	PINMUX_DATA(SDI0_DATA3_MARK, FN_SDI0_DATA3),
+	PINMUX_SINGLE(SDI0_DATA3),
 	/* AA15 */
-	PINMUX_DATA(SDI0_DATA4_MARK, FN_SDI0_DATA4),
+	PINMUX_SINGLE(SDI0_DATA4),
 	/* Y15 */
-	PINMUX_DATA(SDI0_DATA5_MARK, FN_SDI0_DATA5),
+	PINMUX_SINGLE(SDI0_DATA5),
 	/* AA16 */
-	PINMUX_DATA(SDI0_DATA6_MARK, FN_SDI0_DATA6),
+	PINMUX_SINGLE(SDI0_DATA6),
 	/* Y16 */
-	PINMUX_DATA(SDI0_DATA7_MARK, FN_SDI0_DATA7),
+	PINMUX_SINGLE(SDI0_DATA7),
 	/* AB22 */
-	PINMUX_DATA(SDI1_CKO_MARK, FN_SDI1_CKO),
+	PINMUX_SINGLE(SDI1_CKO),
 	/* AA23 */
-	PINMUX_DATA(SDI1_CKI_MARK, FN_SDI1_CKI),
+	PINMUX_SINGLE(SDI1_CKI),
 	/* AC21 */
-	PINMUX_DATA(SDI1_CMD_MARK, FN_SDI1_CMD),
+	PINMUX_SINGLE(SDI1_CMD),
 
 	/* GPSR2 */
 	/* AB21 */
-	PINMUX_DATA(SDI1_DATA0_MARK, FN_SDI1_DATA0),
+	PINMUX_SINGLE(SDI1_DATA0),
 	/* AB20 */
-	PINMUX_DATA(SDI1_DATA1_MARK, FN_SDI1_DATA1),
+	PINMUX_SINGLE(SDI1_DATA1),
 	/* AB19 */
-	PINMUX_DATA(SDI1_DATA2_MARK, FN_SDI1_DATA2),
+	PINMUX_SINGLE(SDI1_DATA2),
 	/* AA19 */
-	PINMUX_DATA(SDI1_DATA3_MARK, FN_SDI1_DATA3),
+	PINMUX_SINGLE(SDI1_DATA3),
 	/* J23 */
-	PINMUX_DATA(AB_CLK_MARK, FN_AB_CLK),
+	PINMUX_SINGLE(AB_CLK),
 	/* D21 */
-	PINMUX_DATA(AB_CSB0_MARK, FN_AB_CSB0),
+	PINMUX_SINGLE(AB_CSB0),
 	/* E21 */
-	PINMUX_DATA(AB_CSB1_MARK, FN_AB_CSB1),
+	PINMUX_SINGLE(AB_CSB1),
 	/* F20 */
 	PINMUX_IPSR_NOFN(AB_1_0_PORT71, AB_CSB2, SEL_AB_1_0_00),
 	PINMUX_IPSR_NOFN(AB_1_0_PORT71, CF_CSB0, SEL_AB_1_0_10),
@@ -514,7 +514,7 @@ static const u16 pinmux_data[] = {
 
 	/* GPSR3 */
 	/* M21 */
-	PINMUX_DATA(AB_A20_MARK, FN_AB_A20),
+	PINMUX_SINGLE(AB_A20),
 	/* N21 */
 	PINMUX_IPSR_NOFN(AB_9_8_PORT97, AB_A21, SEL_AB_9_8_00),
 	PINMUX_IPSR_NOFN(AB_9_8_PORT97, SDI2_CKO, SEL_AB_9_8_01),
@@ -541,13 +541,13 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_NOFN(AB_13_12_PORT104, AB_A28, SEL_AB_13_12_00),
 	PINMUX_IPSR_NOFN(AB_13_12_PORT104, AB_BEN1, SEL_AB_13_12_10),
 	/* B8 */
-	PINMUX_DATA(USI0_CS1_MARK, FN_USI0_CS1),
+	PINMUX_SINGLE(USI0_CS1),
 	/* B9 */
-	PINMUX_DATA(USI0_CS2_MARK, FN_USI0_CS2),
+	PINMUX_SINGLE(USI0_CS2),
 	/* C10 */
-	PINMUX_DATA(USI1_DI_MARK, FN_USI1_DI),
+	PINMUX_SINGLE(USI1_DI),
 	/* D10 */
-	PINMUX_DATA(USI1_DO_MARK, FN_USI1_DO),
+	PINMUX_SINGLE(USI1_DO),
 	/* AB5 */
 	PINMUX_IPSR_NOFN(USI_1_0_PORT109, USI2_CLK, SEL_USI_1_0_00),
 	PINMUX_IPSR_NOFN(USI_1_0_PORT109, DTV_BCLK_B, SEL_USI_1_0_01),
@@ -587,49 +587,49 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_NOFN(USI_9_8_PORT121, PWM1, SEL_USI_9_8_00),
 	PINMUX_IPSR_NOFN(USI_9_8_PORT121, USI4_DO, SEL_USI_9_8_01),
 	/* V20 */
-	PINMUX_DATA(NTSC_CLK_MARK, FN_NTSC_CLK),
+	PINMUX_SINGLE(NTSC_CLK),
 	/* P20 */
-	PINMUX_DATA(NTSC_DATA0_MARK, FN_NTSC_DATA0),
+	PINMUX_SINGLE(NTSC_DATA0),
 	/* P18 */
-	PINMUX_DATA(NTSC_DATA1_MARK, FN_NTSC_DATA1),
+	PINMUX_SINGLE(NTSC_DATA1),
 	/* R20 */
-	PINMUX_DATA(NTSC_DATA2_MARK, FN_NTSC_DATA2),
+	PINMUX_SINGLE(NTSC_DATA2),
 	/* R18 */
-	PINMUX_DATA(NTSC_DATA3_MARK, FN_NTSC_DATA3),
+	PINMUX_SINGLE(NTSC_DATA3),
 	/* T20 */
-	PINMUX_DATA(NTSC_DATA4_MARK, FN_NTSC_DATA4),
+	PINMUX_SINGLE(NTSC_DATA4),
 
 	/* GPRS3 */
 	/* T18 */
-	PINMUX_DATA(NTSC_DATA5_MARK, FN_NTSC_DATA5),
+	PINMUX_SINGLE(NTSC_DATA5),
 	/* U20 */
-	PINMUX_DATA(NTSC_DATA6_MARK, FN_NTSC_DATA6),
+	PINMUX_SINGLE(NTSC_DATA6),
 	/* U18 */
-	PINMUX_DATA(NTSC_DATA7_MARK, FN_NTSC_DATA7),
+	PINMUX_SINGLE(NTSC_DATA7),
 	/* W23 */
-	PINMUX_DATA(CAM_CLKO_MARK, FN_CAM_CLKO),
+	PINMUX_SINGLE(CAM_CLKO),
 	/* Y23 */
-	PINMUX_DATA(CAM_CLKI_MARK, FN_CAM_CLKI),
+	PINMUX_SINGLE(CAM_CLKI),
 	/* W22 */
-	PINMUX_DATA(CAM_VS_MARK, FN_CAM_VS),
+	PINMUX_SINGLE(CAM_VS),
 	/* V21 */
-	PINMUX_DATA(CAM_HS_MARK, FN_CAM_HS),
+	PINMUX_SINGLE(CAM_HS),
 	/* T21 */
-	PINMUX_DATA(CAM_YUV0_MARK, FN_CAM_YUV0),
+	PINMUX_SINGLE(CAM_YUV0),
 	/* T22 */
-	PINMUX_DATA(CAM_YUV1_MARK, FN_CAM_YUV1),
+	PINMUX_SINGLE(CAM_YUV1),
 	/* T23 */
-	PINMUX_DATA(CAM_YUV2_MARK, FN_CAM_YUV2),
+	PINMUX_SINGLE(CAM_YUV2),
 	/* U21 */
-	PINMUX_DATA(CAM_YUV3_MARK, FN_CAM_YUV3),
+	PINMUX_SINGLE(CAM_YUV3),
 	/* U22 */
-	PINMUX_DATA(CAM_YUV4_MARK, FN_CAM_YUV4),
+	PINMUX_SINGLE(CAM_YUV4),
 	/* U23 */
-	PINMUX_DATA(CAM_YUV5_MARK, FN_CAM_YUV5),
+	PINMUX_SINGLE(CAM_YUV5),
 	/* V22 */
-	PINMUX_DATA(CAM_YUV6_MARK, FN_CAM_YUV6),
+	PINMUX_SINGLE(CAM_YUV6),
 	/* V23 */
-	PINMUX_DATA(CAM_YUV7_MARK, FN_CAM_YUV7),
+	PINMUX_SINGLE(CAM_YUV7),
 	/* K22 */
 	PINMUX_IPSR_NOFN(HSI_1_0_PORT143, USI5_CLK_B, SEL_HSI_1_0_01),
 	/* K23 */
@@ -647,17 +647,17 @@ static const u16 pinmux_data[] = {
 	/* M22 */
 	PINMUX_IPSR_NOFN(HSI_1_0_PORT150, USI5_DI_B, SEL_HSI_1_0_01),
 	/* D13 */
-	PINMUX_DATA(JT_TDO_MARK, FN_JT_TDO),
+	PINMUX_SINGLE(JT_TDO),
 	/* F13 */
-	PINMUX_DATA(JT_TDOEN_MARK, FN_JT_TDOEN),
+	PINMUX_SINGLE(JT_TDOEN),
 	/* AA12 */
-	PINMUX_DATA(USB_VBUS_MARK, FN_USB_VBUS),
+	PINMUX_SINGLE(USB_VBUS),
 	/* A12 */
-	PINMUX_DATA(LOWPWR_MARK, FN_LOWPWR),
+	PINMUX_SINGLE(LOWPWR),
 	/* Y11 */
-	PINMUX_DATA(UART1_RX_MARK, FN_UART1_RX),
+	PINMUX_SINGLE(UART1_RX),
 	/* Y10 */
-	PINMUX_DATA(UART1_TX_MARK, FN_UART1_TX),
+	PINMUX_SINGLE(UART1_TX),
 	/* AA10 */
 	PINMUX_IPSR_NOFN(UART_1_0_PORT157, UART1_CTSB, SEL_UART_1_0_00),
 	PINMUX_IPSR_NOFN(UART_1_0_PORT157, UART2_RX, SEL_UART_1_0_01),
@@ -749,7 +749,7 @@ static const unsigned int cf_ctrl_mux[] = {
 };
 
 static const unsigned int cf_data8_pins[] = {
-	/* CF_D[0:8] */
+	/* CF_D[0:7] */
 	77, 78, 79, 80,
 	81, 82, 83, 84,
 };
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index 279e9dd442e4..7f7c8a6e76e8 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -2214,7 +2214,7 @@ static const unsigned int lcd1_data9_mux[] = {
 	LCD1_D8_MARK,
 };
 static const unsigned int lcd1_data12_pins[] = {
-	/* D[0:12] */
+	/* D[0:11] */
 	4, 3, 2, 1, 0, 91, 92, 23,
 	93, 94, 21, 201,
 };
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7778.c b/drivers/pinctrl/sh-pfc/pfc-r8a7778.c
index bbd35dc1a0c4..ad09a670c2ff 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7778.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7778.c
@@ -548,17 +548,17 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(PENC0_MARK,		FN_PENC0),
-	PINMUX_DATA(PENC1_MARK,		FN_PENC1),
-	PINMUX_DATA(A1_MARK,		FN_A1),
-	PINMUX_DATA(A2_MARK,		FN_A2),
-	PINMUX_DATA(A3_MARK,		FN_A3),
-	PINMUX_DATA(WE0_MARK,		FN_WE0),
-	PINMUX_DATA(AUDIO_CLKA_MARK,	FN_AUDIO_CLKA),
-	PINMUX_DATA(AUDIO_CLKB_MARK,	FN_AUDIO_CLKB),
-	PINMUX_DATA(SSI_SCK34_MARK,	FN_SSI_SCK34),
-	PINMUX_DATA(AVS1_MARK,		FN_AVS1),
-	PINMUX_DATA(AVS2_MARK,		FN_AVS2),
+	PINMUX_SINGLE(PENC0),
+	PINMUX_SINGLE(PENC1),
+	PINMUX_SINGLE(A1),
+	PINMUX_SINGLE(A2),
+	PINMUX_SINGLE(A3),
+	PINMUX_SINGLE(WE0),
+	PINMUX_SINGLE(AUDIO_CLKA),
+	PINMUX_SINGLE(AUDIO_CLKB),
+	PINMUX_SINGLE(SSI_SCK34),
+	PINMUX_SINGLE(AVS1),
+	PINMUX_SINGLE(AVS2),
 
 	/* IPSR0 */
 	PINMUX_IPSR_DATA(IP0_1_0,	PRESETOUT),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7779.c b/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
index ed4e0788035c..bd17eccb6a89 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7779.c
@@ -23,13 +23,6 @@
 
 #include "sh_pfc.h"
 
-#define PORT_GP_9(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0, fn, sfx), PORT_GP_1(bank, 1, fn, sfx),	\
-	PORT_GP_1(bank, 2, fn, sfx), PORT_GP_1(bank, 3, fn, sfx),	\
-	PORT_GP_1(bank, 4, fn, sfx), PORT_GP_1(bank, 5, fn, sfx),	\
-	PORT_GP_1(bank, 6, fn, sfx), PORT_GP_1(bank, 7, fn, sfx),	\
-	PORT_GP_1(bank, 8, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
 	PORT_GP_32(0, fn, sfx),						\
 	PORT_GP_32(1, fn, sfx),						\
@@ -609,14 +602,14 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(AVS1_MARK, FN_AVS1),
-	PINMUX_DATA(AVS1_MARK, FN_AVS1),
-	PINMUX_DATA(A17_MARK, FN_A17),
-	PINMUX_DATA(A18_MARK, FN_A18),
-	PINMUX_DATA(A19_MARK, FN_A19),
+	PINMUX_SINGLE(AVS1),
+	PINMUX_SINGLE(AVS1),
+	PINMUX_SINGLE(A17),
+	PINMUX_SINGLE(A18),
+	PINMUX_SINGLE(A19),
 
-	PINMUX_DATA(USB_PENC0_MARK, FN_USB_PENC0),
-	PINMUX_DATA(USB_PENC1_MARK, FN_USB_PENC1),
+	PINMUX_SINGLE(USB_PENC0),
+	PINMUX_SINGLE(USB_PENC1),
 
 	PINMUX_IPSR_DATA(IP0_2_0, USB_PENC2),
 	PINMUX_IPSR_MSEL(IP0_2_0, SCK0, SEL_SCIF0_0),
@@ -2289,6 +2282,35 @@ static const unsigned int scif5_clk_d_pins[] = {
 static const unsigned int scif5_clk_d_mux[] = {
 	SCK5_D_MARK,
 };
+/* - SCIF Clock ------------------------------------------------------------- */
+static const unsigned int scif_clk_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(4, 28),
+};
+static const unsigned int scif_clk_mux[] = {
+	SCIF_CLK_MARK,
+};
+static const unsigned int scif_clk_b_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(4, 5),
+};
+static const unsigned int scif_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
+static const unsigned int scif_clk_c_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(4, 18),
+};
+static const unsigned int scif_clk_c_mux[] = {
+	SCIF_CLK_C_MARK,
+};
+static const unsigned int scif_clk_d_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(2, 29),
+};
+static const unsigned int scif_clk_d_mux[] = {
+	SCIF_CLK_D_MARK,
+};
 /* - SDHI0 ------------------------------------------------------------------ */
 static const unsigned int sdhi0_data1_pins[] = {
 	/* D0 */
@@ -2700,6 +2722,10 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scif5_clk_c),
 	SH_PFC_PIN_GROUP(scif5_data_d),
 	SH_PFC_PIN_GROUP(scif5_clk_d),
+	SH_PFC_PIN_GROUP(scif_clk),
+	SH_PFC_PIN_GROUP(scif_clk_b),
+	SH_PFC_PIN_GROUP(scif_clk_c),
+	SH_PFC_PIN_GROUP(scif_clk_d),
 	SH_PFC_PIN_GROUP(sdhi0_data1),
 	SH_PFC_PIN_GROUP(sdhi0_data4),
 	SH_PFC_PIN_GROUP(sdhi0_ctrl),
@@ -2909,6 +2935,13 @@ static const char * const scif5_groups[] = {
 	"scif5_clk_d",
 };
 
+static const char * const scif_clk_groups[] = {
+	"scif_clk",
+	"scif_clk_b",
+	"scif_clk_c",
+	"scif_clk_d",
+};
+
 static const char * const sdhi0_groups[] = {
 	"sdhi0_data1",
 	"sdhi0_data4",
@@ -3004,6 +3037,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(scif3),
 	SH_PFC_FUNCTION(scif4),
 	SH_PFC_FUNCTION(scif5),
+	SH_PFC_FUNCTION(scif_clk),
 	SH_PFC_FUNCTION(usb0),
 	SH_PFC_FUNCTION(usb1),
 	SH_PFC_FUNCTION(usb2),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
index d9924b0d53b7..a8b629bc7a55 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7790.c
@@ -26,23 +26,6 @@
 #include "core.h"
 #include "sh_pfc.h"
 
-#define PORT_GP_30(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
-	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx),	\
-	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
-	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
-	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
-	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
-	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
-	PORT_GP_1(bank, 14, fn, sfx), PORT_GP_1(bank, 15, fn, sfx),	\
-	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx),	\
-	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
-	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
-	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
-	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx),	\
-	PORT_GP_1(bank, 26, fn, sfx), PORT_GP_1(bank, 27, fn, sfx),	\
-	PORT_GP_1(bank, 28, fn, sfx), PORT_GP_1(bank, 29, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
 	PORT_GP_32(0, fn, sfx),						\
 	PORT_GP_30(1, fn, sfx),						\
@@ -806,15 +789,15 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(VI1_DATA7_VI1_B7_MARK, FN_VI1_DATA7_VI1_B7),
-	PINMUX_DATA(USB0_PWEN_MARK, FN_USB0_PWEN),
-	PINMUX_DATA(USB0_OVC_VBUS_MARK, FN_USB0_OVC_VBUS),
-	PINMUX_DATA(USB2_PWEN_MARK, FN_USB2_PWEN),
-	PINMUX_DATA(USB2_OVC_MARK, FN_USB2_OVC),
-	PINMUX_DATA(AVS1_MARK, FN_AVS1),
-	PINMUX_DATA(AVS2_MARK, FN_AVS2),
-	PINMUX_DATA(DU_DOTCLKIN0_MARK, FN_DU_DOTCLKIN0),
-	PINMUX_DATA(DU_DOTCLKIN2_MARK, FN_DU_DOTCLKIN2),
+	PINMUX_SINGLE(VI1_DATA7_VI1_B7),
+	PINMUX_SINGLE(USB0_PWEN),
+	PINMUX_SINGLE(USB0_OVC_VBUS),
+	PINMUX_SINGLE(USB2_PWEN),
+	PINMUX_SINGLE(USB2_OVC),
+	PINMUX_SINGLE(AVS1),
+	PINMUX_SINGLE(AVS2),
+	PINMUX_SINGLE(DU_DOTCLKIN0),
+	PINMUX_SINGLE(DU_DOTCLKIN2),
 
 	PINMUX_IPSR_DATA(IP0_2_0, D0),
 	PINMUX_IPSR_MSEL(IP0_2_0, MSIOF3_SCK_B, SEL_SOF3_1),
@@ -3236,6 +3219,21 @@ static const unsigned int scifb2_data_c_pins[] = {
 static const unsigned int scifb2_data_c_mux[] = {
 	SCIFB2_RXD_C_MARK, SCIFB2_TXD_C_MARK,
 };
+/* - SCIF Clock ------------------------------------------------------------- */
+static const unsigned int scif_clk_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(4, 26),
+};
+static const unsigned int scif_clk_mux[] = {
+	SCIF_CLK_MARK,
+};
+static const unsigned int scif_clk_b_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(5, 4),
+};
+static const unsigned int scif_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
 /* - SDHI0 ------------------------------------------------------------------ */
 static const unsigned int sdhi0_data1_pins[] = {
 	/* D0 */
@@ -4139,6 +4137,8 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scifb2_clk_b),
 	SH_PFC_PIN_GROUP(scifb2_ctrl_b),
 	SH_PFC_PIN_GROUP(scifb2_data_c),
+	SH_PFC_PIN_GROUP(scif_clk),
+	SH_PFC_PIN_GROUP(scif_clk_b),
 	SH_PFC_PIN_GROUP(sdhi0_data1),
 	SH_PFC_PIN_GROUP(sdhi0_data4),
 	SH_PFC_PIN_GROUP(sdhi0_ctrl),
@@ -4555,6 +4555,11 @@ static const char * const scifb2_groups[] = {
 	"scifb2_data_c",
 };
 
+static const char * const scif_clk_groups[] = {
+	"scif_clk",
+	"scif_clk_b",
+};
+
 static const char * const sdhi0_groups[] = {
 	"sdhi0_data1",
 	"sdhi0_data4",
@@ -4729,6 +4734,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(scifb0),
 	SH_PFC_FUNCTION(scifb1),
 	SH_PFC_FUNCTION(scifb2),
+	SH_PFC_FUNCTION(scif_clk),
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index 87a4f44147c1..4cfbb94ad5d0 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -2,6 +2,7 @@
  * r8a7791 processor support - PFC hardware block.
  *
  * Copyright (C) 2013 Renesas Electronics Corporation
+ * Copyright (C) 2014-2015 Cogent Embedded, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2
@@ -13,21 +14,6 @@
 #include "core.h"
 #include "sh_pfc.h"
 
-#define PORT_GP_26(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
-	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx),	\
-	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
-	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
-	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
-	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
-	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
-	PORT_GP_1(bank, 14, fn, sfx), PORT_GP_1(bank, 15, fn, sfx),	\
-	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx),	\
-	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
-	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
-	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
-	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
 	PORT_GP_32(0, fn, sfx),						\
 	PORT_GP_26(1, fn, sfx),						\
@@ -787,23 +773,23 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(EX_CS0_N_MARK, FN_EX_CS0_N),
-	PINMUX_DATA(RD_N_MARK, FN_RD_N),
-	PINMUX_DATA(AUDIO_CLKA_MARK, FN_AUDIO_CLKA),
-	PINMUX_DATA(VI0_CLK_MARK, FN_VI0_CLK),
-	PINMUX_DATA(VI0_DATA0_VI0_B0_MARK, FN_VI0_DATA0_VI0_B0),
-	PINMUX_DATA(VI0_DATA1_VI0_B1_MARK, FN_VI0_DATA1_VI0_B1),
-	PINMUX_DATA(VI0_DATA2_VI0_B2_MARK, FN_VI0_DATA2_VI0_B2),
-	PINMUX_DATA(VI0_DATA4_VI0_B4_MARK, FN_VI0_DATA4_VI0_B4),
-	PINMUX_DATA(VI0_DATA5_VI0_B5_MARK, FN_VI0_DATA5_VI0_B5),
-	PINMUX_DATA(VI0_DATA6_VI0_B6_MARK, FN_VI0_DATA6_VI0_B6),
-	PINMUX_DATA(VI0_DATA7_VI0_B7_MARK, FN_VI0_DATA7_VI0_B7),
-	PINMUX_DATA(USB0_PWEN_MARK, FN_USB0_PWEN),
-	PINMUX_DATA(USB0_OVC_MARK, FN_USB0_OVC),
-	PINMUX_DATA(USB1_PWEN_MARK, FN_USB1_PWEN),
-	PINMUX_DATA(USB1_OVC_MARK, FN_USB1_OVC),
-	PINMUX_DATA(DU0_DOTCLKIN_MARK, FN_DU0_DOTCLKIN),
-	PINMUX_DATA(SD1_CLK_MARK, FN_SD1_CLK),
+	PINMUX_SINGLE(EX_CS0_N),
+	PINMUX_SINGLE(RD_N),
+	PINMUX_SINGLE(AUDIO_CLKA),
+	PINMUX_SINGLE(VI0_CLK),
+	PINMUX_SINGLE(VI0_DATA0_VI0_B0),
+	PINMUX_SINGLE(VI0_DATA1_VI0_B1),
+	PINMUX_SINGLE(VI0_DATA2_VI0_B2),
+	PINMUX_SINGLE(VI0_DATA4_VI0_B4),
+	PINMUX_SINGLE(VI0_DATA5_VI0_B5),
+	PINMUX_SINGLE(VI0_DATA6_VI0_B6),
+	PINMUX_SINGLE(VI0_DATA7_VI0_B7),
+	PINMUX_SINGLE(USB0_PWEN),
+	PINMUX_SINGLE(USB0_OVC),
+	PINMUX_SINGLE(USB1_PWEN),
+	PINMUX_SINGLE(USB1_OVC),
+	PINMUX_SINGLE(DU0_DOTCLKIN),
+	PINMUX_SINGLE(SD1_CLK),
 
 	/* IPSR0 */
 	PINMUX_IPSR_DATA(IP0_0, D0),
@@ -1740,6 +1726,82 @@ static const unsigned int audio_clkout_mux[] = {
 	AUDIO_CLKOUT_MARK,
 };
 
+/* - AVB -------------------------------------------------------------------- */
+static const unsigned int avb_link_pins[] = {
+	RCAR_GP_PIN(5, 14),
+};
+static const unsigned int avb_link_mux[] = {
+	AVB_LINK_MARK,
+};
+static const unsigned int avb_magic_pins[] = {
+	RCAR_GP_PIN(5, 11),
+};
+static const unsigned int avb_magic_mux[] = {
+	AVB_MAGIC_MARK,
+};
+static const unsigned int avb_phy_int_pins[] = {
+	RCAR_GP_PIN(5, 16),
+};
+static const unsigned int avb_phy_int_mux[] = {
+	AVB_PHY_INT_MARK,
+};
+static const unsigned int avb_mdio_pins[] = {
+	RCAR_GP_PIN(5, 12), RCAR_GP_PIN(5, 9),
+};
+static const unsigned int avb_mdio_mux[] = {
+	AVB_MDC_MARK, AVB_MDIO_MARK,
+};
+static const unsigned int avb_mii_pins[] = {
+	RCAR_GP_PIN(5, 18), RCAR_GP_PIN(5, 19), RCAR_GP_PIN(5, 20),
+	RCAR_GP_PIN(5, 21),
+
+	RCAR_GP_PIN(5, 0), RCAR_GP_PIN(5, 1), RCAR_GP_PIN(5, 2),
+	RCAR_GP_PIN(5, 3),
+
+	RCAR_GP_PIN(5, 8), RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 10),
+	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 26), RCAR_GP_PIN(5, 27),
+	RCAR_GP_PIN(5, 28), RCAR_GP_PIN(5, 29),
+};
+static const unsigned int avb_mii_mux[] = {
+	AVB_TXD0_MARK, AVB_TXD1_MARK, AVB_TXD2_MARK,
+	AVB_TXD3_MARK,
+
+	AVB_RXD0_MARK, AVB_RXD1_MARK, AVB_RXD2_MARK,
+	AVB_RXD3_MARK,
+
+	AVB_RX_ER_MARK, AVB_RX_CLK_MARK, AVB_RX_DV_MARK,
+	AVB_CRS_MARK, AVB_TX_EN_MARK, AVB_TX_ER_MARK,
+	AVB_TX_CLK_MARK, AVB_COL_MARK,
+};
+static const unsigned int avb_gmii_pins[] = {
+	RCAR_GP_PIN(5, 18), RCAR_GP_PIN(5, 19), RCAR_GP_PIN(5, 20),
+	RCAR_GP_PIN(5, 21), RCAR_GP_PIN(5, 22), RCAR_GP_PIN(5, 23),
+	RCAR_GP_PIN(5, 24), RCAR_GP_PIN(5, 25),
+
+	RCAR_GP_PIN(5, 0), RCAR_GP_PIN(5, 1), RCAR_GP_PIN(5, 2),
+	RCAR_GP_PIN(5, 3), RCAR_GP_PIN(5, 4), RCAR_GP_PIN(5, 5),
+	RCAR_GP_PIN(5, 6), RCAR_GP_PIN(5, 7),
+
+	RCAR_GP_PIN(5, 8), RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 10),
+	RCAR_GP_PIN(5, 15), RCAR_GP_PIN(5, 30), RCAR_GP_PIN(5, 17),
+	RCAR_GP_PIN(5, 26), RCAR_GP_PIN(5, 27), RCAR_GP_PIN(5, 28),
+	RCAR_GP_PIN(5, 29),
+};
+static const unsigned int avb_gmii_mux[] = {
+	AVB_TXD0_MARK, AVB_TXD1_MARK, AVB_TXD2_MARK,
+	AVB_TXD3_MARK, AVB_TXD4_MARK, AVB_TXD5_MARK,
+	AVB_TXD6_MARK, AVB_TXD7_MARK,
+
+	AVB_RXD0_MARK, AVB_RXD1_MARK, AVB_RXD2_MARK,
+	AVB_RXD3_MARK, AVB_RXD4_MARK, AVB_RXD5_MARK,
+	AVB_RXD6_MARK, AVB_RXD7_MARK,
+
+	AVB_RX_ER_MARK, AVB_RX_CLK_MARK, AVB_RX_DV_MARK,
+	AVB_CRS_MARK, AVB_GTX_CLK_MARK, AVB_GTXREFCLK_MARK,
+	AVB_TX_EN_MARK, AVB_TX_ER_MARK, AVB_TX_CLK_MARK,
+	AVB_COL_MARK,
+};
+
 /* - CAN -------------------------------------------------------------------- */
 
 static const unsigned int can0_data_pins[] = {
@@ -3602,6 +3664,23 @@ static const unsigned int scifb2_data_d_pins[] = {
 static const unsigned int scifb2_data_d_mux[] = {
 	SCIFB2_RXD_D_MARK, SCIFB2_TXD_D_MARK,
 };
+
+/* - SCIF Clock ------------------------------------------------------------- */
+static const unsigned int scif_clk_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(2, 29),
+};
+static const unsigned int scif_clk_mux[] = {
+	SCIF_CLK_MARK,
+};
+static const unsigned int scif_clk_b_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(7, 19),
+};
+static const unsigned int scif_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
+
 /* - SDHI0 ------------------------------------------------------------------ */
 static const unsigned int sdhi0_data1_pins[] = {
 	/* D0 */
@@ -4258,6 +4337,12 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(audio_clk_b_b),
 	SH_PFC_PIN_GROUP(audio_clk_c),
 	SH_PFC_PIN_GROUP(audio_clkout),
+	SH_PFC_PIN_GROUP(avb_link),
+	SH_PFC_PIN_GROUP(avb_magic),
+	SH_PFC_PIN_GROUP(avb_phy_int),
+	SH_PFC_PIN_GROUP(avb_mdio),
+	SH_PFC_PIN_GROUP(avb_mii),
+	SH_PFC_PIN_GROUP(avb_gmii),
 	SH_PFC_PIN_GROUP(can0_data),
 	SH_PFC_PIN_GROUP(can0_data_b),
 	SH_PFC_PIN_GROUP(can0_data_c),
@@ -4510,6 +4595,8 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scifb2_data_c),
 	SH_PFC_PIN_GROUP(scifb2_clk_c),
 	SH_PFC_PIN_GROUP(scifb2_data_d),
+	SH_PFC_PIN_GROUP(scif_clk),
+	SH_PFC_PIN_GROUP(scif_clk_b),
 	SH_PFC_PIN_GROUP(sdhi0_data1),
 	SH_PFC_PIN_GROUP(sdhi0_data4),
 	SH_PFC_PIN_GROUP(sdhi0_ctrl),
@@ -4597,6 +4684,15 @@ static const char * const audio_clk_groups[] = {
 	"audio_clkout",
 };
 
+static const char * const avb_groups[] = {
+	"avb_link",
+	"avb_magic",
+	"avb_phy_int",
+	"avb_mdio",
+	"avb_mii",
+	"avb_gmii",
+};
+
 static const char * const can0_groups[] = {
 	"can0_data",
 	"can0_data_b",
@@ -4976,6 +5072,11 @@ static const char * const scifb2_groups[] = {
 	"scifb2_data_d",
 };
 
+static const char * const scif_clk_groups[] = {
+	"scif_clk",
+	"scif_clk_b",
+};
+
 static const char * const sdhi0_groups[] = {
 	"sdhi0_data1",
 	"sdhi0_data4",
@@ -5081,6 +5182,7 @@ static const char * const vin2_groups[] = {
 
 static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(audio_clk),
+	SH_PFC_FUNCTION(avb),
 	SH_PFC_FUNCTION(can0),
 	SH_PFC_FUNCTION(can1),
 	SH_PFC_FUNCTION(du),
@@ -5126,6 +5228,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(scifb0),
 	SH_PFC_FUNCTION(scifb1),
 	SH_PFC_FUNCTION(scifb2),
+	SH_PFC_FUNCTION(scif_clk),
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7794.c b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
index 086f6798b129..3718c7846bfd 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7794.c
@@ -15,25 +15,6 @@
 #include "core.h"
 #include "sh_pfc.h"
 
-#define PORT_GP_26(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
-	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx),	\
-	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
-	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
-	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
-	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
-	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
-	PORT_GP_1(bank, 14, fn, sfx), PORT_GP_1(bank, 15, fn, sfx),	\
-	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx),	\
-	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
-	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
-	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
-	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx)
-
-#define PORT_GP_28(bank, fn, sfx)					\
-	PORT_GP_26(bank, fn, sfx),					\
-	PORT_GP_1(bank, 26, fn, sfx), PORT_GP_1(bank, 27, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
 	PORT_GP_32(0, fn, sfx),						\
 	PORT_GP_26(1, fn, sfx),						\
@@ -618,28 +599,28 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(A2_MARK, FN_A2),
-	PINMUX_DATA(WE0_N_MARK, FN_WE0_N),
-	PINMUX_DATA(WE1_N_MARK, FN_WE1_N),
-	PINMUX_DATA(DACK0_MARK, FN_DACK0),
-	PINMUX_DATA(USB0_PWEN_MARK, FN_USB0_PWEN),
-	PINMUX_DATA(USB0_OVC_MARK, FN_USB0_OVC),
-	PINMUX_DATA(USB1_PWEN_MARK, FN_USB1_PWEN),
-	PINMUX_DATA(USB1_OVC_MARK, FN_USB1_OVC),
-	PINMUX_DATA(SD0_CLK_MARK, FN_SD0_CLK),
-	PINMUX_DATA(SD0_CMD_MARK, FN_SD0_CMD),
-	PINMUX_DATA(SD0_DATA0_MARK, FN_SD0_DATA0),
-	PINMUX_DATA(SD0_DATA1_MARK, FN_SD0_DATA1),
-	PINMUX_DATA(SD0_DATA2_MARK, FN_SD0_DATA2),
-	PINMUX_DATA(SD0_DATA3_MARK, FN_SD0_DATA3),
-	PINMUX_DATA(SD0_CD_MARK, FN_SD0_CD),
-	PINMUX_DATA(SD0_WP_MARK, FN_SD0_WP),
-	PINMUX_DATA(SD1_CLK_MARK, FN_SD1_CLK),
-	PINMUX_DATA(SD1_CMD_MARK, FN_SD1_CMD),
-	PINMUX_DATA(SD1_DATA0_MARK, FN_SD1_DATA0),
-	PINMUX_DATA(SD1_DATA1_MARK, FN_SD1_DATA1),
-	PINMUX_DATA(SD1_DATA2_MARK, FN_SD1_DATA2),
-	PINMUX_DATA(SD1_DATA3_MARK, FN_SD1_DATA3),
+	PINMUX_SINGLE(A2),
+	PINMUX_SINGLE(WE0_N),
+	PINMUX_SINGLE(WE1_N),
+	PINMUX_SINGLE(DACK0),
+	PINMUX_SINGLE(USB0_PWEN),
+	PINMUX_SINGLE(USB0_OVC),
+	PINMUX_SINGLE(USB1_PWEN),
+	PINMUX_SINGLE(USB1_OVC),
+	PINMUX_SINGLE(SD0_CLK),
+	PINMUX_SINGLE(SD0_CMD),
+	PINMUX_SINGLE(SD0_DATA0),
+	PINMUX_SINGLE(SD0_DATA1),
+	PINMUX_SINGLE(SD0_DATA2),
+	PINMUX_SINGLE(SD0_DATA3),
+	PINMUX_SINGLE(SD0_CD),
+	PINMUX_SINGLE(SD0_WP),
+	PINMUX_SINGLE(SD1_CLK),
+	PINMUX_SINGLE(SD1_CMD),
+	PINMUX_SINGLE(SD1_DATA0),
+	PINMUX_SINGLE(SD1_DATA1),
+	PINMUX_SINGLE(SD1_DATA2),
+	PINMUX_SINGLE(SD1_DATA3),
 
 	/* IPSR0 */
 	PINMUX_IPSR_DATA(IP0_0, SD1_CD),
@@ -2644,6 +2625,21 @@ static const unsigned int scifb2_ctrl_pins[] = {
 static const unsigned int scifb2_ctrl_mux[] = {
 	SCIFB2_RTS_N_MARK, SCIFB2_CTS_N_MARK,
 };
+/* - SCIF Clock ------------------------------------------------------------- */
+static const unsigned int scif_clk_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(1, 23),
+};
+static const unsigned int scif_clk_mux[] = {
+	SCIF_CLK_MARK,
+};
+static const unsigned int scif_clk_b_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(3, 29),
+};
+static const unsigned int scif_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
 /* - SDHI0 ------------------------------------------------------------------ */
 static const unsigned int sdhi0_data1_pins[] = {
 	/* D0 */
@@ -3071,6 +3067,8 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scifb2_data),
 	SH_PFC_PIN_GROUP(scifb2_clk),
 	SH_PFC_PIN_GROUP(scifb2_ctrl),
+	SH_PFC_PIN_GROUP(scif_clk),
+	SH_PFC_PIN_GROUP(scif_clk_b),
 	SH_PFC_PIN_GROUP(sdhi0_data1),
 	SH_PFC_PIN_GROUP(sdhi0_data4),
 	SH_PFC_PIN_GROUP(sdhi0_ctrl),
@@ -3354,6 +3352,11 @@ static const char * const scifb2_groups[] = {
 	"scifb2_ctrl",
 };
 
+static const char * const scif_clk_groups[] = {
+	"scif_clk",
+	"scif_clk_b",
+};
+
 static const char * const sdhi0_groups[] = {
 	"sdhi0_data1",
 	"sdhi0_data4",
@@ -3441,6 +3444,7 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(scifb0),
 	SH_PFC_FUNCTION(scifb1),
 	SH_PFC_FUNCTION(scifb2),
+	SH_PFC_FUNCTION(scif_clk),
 	SH_PFC_FUNCTION(sdhi0),
 	SH_PFC_FUNCTION(sdhi1),
 	SH_PFC_FUNCTION(sdhi2),
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7795.c b/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
index 7ddb2adfc5a5..ce4f5cdb0579 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7795.c
@@ -13,46 +13,15 @@
 #include "core.h"
 #include "sh_pfc.h"
 
-#define PORT_GP_3(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0,  fn, sfx), PORT_GP_1(bank, 1,  fn, sfx),	\
-	PORT_GP_1(bank, 2,  fn, sfx), PORT_GP_1(bank, 3,  fn, sfx)
-
-#define PORT_GP_14(bank, fn, sfx)					\
-	PORT_GP_3(bank, fn, sfx),					\
-	PORT_GP_1(bank, 4,  fn, sfx), PORT_GP_1(bank, 5,  fn, sfx),	\
-	PORT_GP_1(bank, 6,  fn, sfx), PORT_GP_1(bank, 7,  fn, sfx),	\
-	PORT_GP_1(bank, 8,  fn, sfx), PORT_GP_1(bank, 9,  fn, sfx),	\
-	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx),	\
-	PORT_GP_1(bank, 12, fn, sfx), PORT_GP_1(bank, 13, fn, sfx),	\
-	PORT_GP_1(bank, 14, fn, sfx)
-
-#define PORT_GP_15(bank, fn, sfx)					\
-	PORT_GP_14(bank, fn, sfx),   PORT_GP_1(bank, 15, fn, sfx)
-
-#define PORT_GP_17(bank, fn, sfx)					\
-	PORT_GP_15(bank, fn, sfx),					\
-	PORT_GP_1(bank, 16, fn, sfx), PORT_GP_1(bank, 17, fn, sfx)
-
-#define PORT_GP_25(bank, fn, sfx)					\
-	PORT_GP_17(bank, fn, sfx),					\
-	PORT_GP_1(bank, 18, fn, sfx), PORT_GP_1(bank, 19, fn, sfx),	\
-	PORT_GP_1(bank, 20, fn, sfx), PORT_GP_1(bank, 21, fn, sfx),	\
-	PORT_GP_1(bank, 22, fn, sfx), PORT_GP_1(bank, 23, fn, sfx),	\
-	PORT_GP_1(bank, 24, fn, sfx), PORT_GP_1(bank, 25, fn, sfx)
-
-#define PORT_GP_27(bank, fn, sfx)					\
-	PORT_GP_25(bank, fn, sfx),					\
-	PORT_GP_1(bank, 26, fn, sfx), PORT_GP_1(bank, 27, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
-	PORT_GP_15(0, fn, sfx),						\
-	PORT_GP_27(1, fn, sfx),						\
-	PORT_GP_14(2, fn, sfx),						\
-	PORT_GP_15(3, fn, sfx),						\
-	PORT_GP_17(4, fn, sfx),						\
-	PORT_GP_25(5, fn, sfx),						\
+	PORT_GP_16(0, fn, sfx),						\
+	PORT_GP_28(1, fn, sfx),						\
+	PORT_GP_15(2, fn, sfx),						\
+	PORT_GP_16(3, fn, sfx),						\
+	PORT_GP_18(4, fn, sfx),						\
+	PORT_GP_26(5, fn, sfx),						\
 	PORT_GP_32(6, fn, sfx),						\
-	PORT_GP_3(7, fn, sfx)
+	PORT_GP_4(7, fn, sfx)
 /*
  * F_() : just information
  * FM() : macro for FN_xxx / xxx_MARK
@@ -495,7 +464,7 @@ FM(IP16_31_28)	IP16_31_28
 #define MOD_SEL1_13		FM(SEL_SCIF3_0)		FM(SEL_SCIF3_1)
 #define MOD_SEL1_12		FM(SEL_SCIF2_0)		FM(SEL_SCIF2_1)
 #define MOD_SEL1_11		FM(SEL_SCIF1_0)		FM(SEL_SCIF1_1)
-#define MOD_SEL1_10		FM(SEL_SCIF_0)		FM(SEL_SCIF_1)
+#define MOD_SEL1_10		FM(SEL_SATA_0)		FM(SEL_SATA_1)
 #define MOD_SEL1_9		FM(SEL_REMOCON_0)	FM(SEL_REMOCON_1)
 #define MOD_SEL1_6		FM(SEL_RCAN0_0)		FM(SEL_RCAN0_1)
 #define MOD_SEL1_5		FM(SEL_PWM6_0)		FM(SEL_PWM6_1)
@@ -580,6 +549,25 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(),
 
+	PINMUX_SINGLE(AVS1),
+	PINMUX_SINGLE(AVS2),
+	PINMUX_SINGLE(HDMI0_CEC),
+	PINMUX_SINGLE(HDMI1_CEC),
+	PINMUX_SINGLE(MSIOF0_RXD),
+	PINMUX_SINGLE(MSIOF0_SCK),
+	PINMUX_SINGLE(MSIOF0_TXD),
+	PINMUX_SINGLE(SD2_CMD),
+	PINMUX_SINGLE(SD3_CLK),
+	PINMUX_SINGLE(SD3_CMD),
+	PINMUX_SINGLE(SD3_DAT0),
+	PINMUX_SINGLE(SD3_DAT1),
+	PINMUX_SINGLE(SD3_DAT2),
+	PINMUX_SINGLE(SD3_DAT3),
+	PINMUX_SINGLE(SD3_DS),
+	PINMUX_SINGLE(SSI_SCK5),
+	PINMUX_SINGLE(SSI_SDATA5),
+	PINMUX_SINGLE(SSI_WS5),
+
 	/* IPSR0 */
 	PINMUX_IPSR_DATA(IP0_3_0,	AVB_MDC),
 	PINMUX_IPSR_MSEL(IP0_3_0,	MSIOF2_SS2_C,		SEL_MSIOF2_2),
@@ -1033,7 +1021,7 @@ static const u16 pinmux_data[] = {
 	PINMUX_IPSR_DATA(IP9_19_16,	SD2_DAT3),
 
 	PINMUX_IPSR_DATA(IP9_23_20,	SD2_DS),
-	PINMUX_IPSR_MSEL(IP9_23_20,	SATA_DEVSLP_B,		SEL_SCIF_1),
+	PINMUX_IPSR_MSEL(IP9_23_20,	SATA_DEVSLP_B,		SEL_SATA_1),
 
 	PINMUX_IPSR_DATA(IP9_27_24,	SD3_DAT4),
 	PINMUX_IPSR_MSEL(IP9_27_24,	SD2_CD_A,		SEL_SDHI2_0),
@@ -1293,7 +1281,7 @@ static const u16 pinmux_data[] = {
 
 	PINMUX_IPSR_DATA(IP15_11_8,	SSI_SDATA6),
 	PINMUX_IPSR_MSEL(IP15_11_8,	SIM0_CLK_D,		SEL_SIMCARD_3),
-	PINMUX_IPSR_MSEL(IP15_11_8,	SATA_DEVSLP_A,		SEL_SCIF_0),
+	PINMUX_IPSR_MSEL(IP15_11_8,	SATA_DEVSLP_A,		SEL_SATA_0),
 
 	PINMUX_IPSR_DATA(IP15_15_12,	SSI_SCK78),
 	PINMUX_IPSR_MSEL(IP15_15_12,	HRX2_B,			SEL_HSCIF2_1),
@@ -1612,6 +1600,191 @@ static const unsigned int avb_avtp_capture_b_mux[] = {
 	AVB_AVTP_CAPTURE_B_MARK,
 };
 
+/* - HSCIF0 ----------------------------------------------------------------- */
+static const unsigned int hscif0_data_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 13), RCAR_GP_PIN(5, 14),
+};
+static const unsigned int hscif0_data_mux[] = {
+	HRX0_MARK, HTX0_MARK,
+};
+static const unsigned int hscif0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 12),
+};
+static const unsigned int hscif0_clk_mux[] = {
+	HSCK0_MARK,
+};
+static const unsigned int hscif0_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(5, 16), RCAR_GP_PIN(5, 15),
+};
+static const unsigned int hscif0_ctrl_mux[] = {
+	HRTS0_N_MARK, HCTS0_N_MARK,
+};
+/* - HSCIF1 ----------------------------------------------------------------- */
+static const unsigned int hscif1_data_a_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 5), RCAR_GP_PIN(5, 6),
+};
+static const unsigned int hscif1_data_a_mux[] = {
+	HRX1_A_MARK, HTX1_A_MARK,
+};
+static const unsigned int hscif1_clk_a_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(6, 21),
+};
+static const unsigned int hscif1_clk_a_mux[] = {
+	HSCK1_A_MARK,
+};
+static const unsigned int hscif1_ctrl_a_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(5, 8), RCAR_GP_PIN(5, 7),
+};
+static const unsigned int hscif1_ctrl_a_mux[] = {
+	HRTS1_N_A_MARK, HCTS1_N_A_MARK,
+};
+
+static const unsigned int hscif1_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(5, 1), RCAR_GP_PIN(5, 2),
+};
+static const unsigned int hscif1_data_b_mux[] = {
+	HRX1_B_MARK, HTX1_B_MARK,
+};
+static const unsigned int hscif1_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 0),
+};
+static const unsigned int hscif1_clk_b_mux[] = {
+	HSCK1_B_MARK,
+};
+static const unsigned int hscif1_ctrl_b_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(5, 4), RCAR_GP_PIN(5, 3),
+};
+static const unsigned int hscif1_ctrl_b_mux[] = {
+	HRTS1_N_B_MARK, HCTS1_N_B_MARK,
+};
+/* - HSCIF2 ----------------------------------------------------------------- */
+static const unsigned int hscif2_data_a_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(6, 8), RCAR_GP_PIN(6, 9),
+};
+static const unsigned int hscif2_data_a_mux[] = {
+	HRX2_A_MARK, HTX2_A_MARK,
+};
+static const unsigned int hscif2_clk_a_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(6, 10),
+};
+static const unsigned int hscif2_clk_a_mux[] = {
+	HSCK2_A_MARK,
+};
+static const unsigned int hscif2_ctrl_a_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(6, 7), RCAR_GP_PIN(6, 6),
+};
+static const unsigned int hscif2_ctrl_a_mux[] = {
+	HRTS2_N_A_MARK, HCTS2_N_A_MARK,
+};
+
+static const unsigned int hscif2_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(6, 17), RCAR_GP_PIN(6, 18),
+};
+static const unsigned int hscif2_data_b_mux[] = {
+	HRX2_B_MARK, HTX2_B_MARK,
+};
+static const unsigned int hscif2_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(6, 21),
+};
+static const unsigned int hscif2_clk_b_mux[] = {
+	HSCK1_B_MARK,
+};
+static const unsigned int hscif2_ctrl_b_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(6, 20), RCAR_GP_PIN(6, 19),
+};
+static const unsigned int hscif2_ctrl_b_mux[] = {
+	HRTS2_N_B_MARK, HCTS2_N_B_MARK,
+};
+/* - HSCIF3 ----------------------------------------------------------------- */
+static const unsigned int hscif3_data_a_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(1, 23), RCAR_GP_PIN(1, 24),
+};
+static const unsigned int hscif3_data_a_mux[] = {
+	HRX3_A_MARK, HTX3_A_MARK,
+};
+static const unsigned int hscif3_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 22),
+};
+static const unsigned int hscif3_clk_mux[] = {
+	HSCK3_MARK,
+};
+static const unsigned int hscif3_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(1, 26), RCAR_GP_PIN(1, 25),
+};
+static const unsigned int hscif3_ctrl_mux[] = {
+	HRTS3_N_MARK, HCTS3_N_MARK,
+};
+
+static const unsigned int hscif3_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 10), RCAR_GP_PIN(0, 11),
+};
+static const unsigned int hscif3_data_b_mux[] = {
+	HRX3_B_MARK, HTX3_B_MARK,
+};
+static const unsigned int hscif3_data_c_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(0, 14), RCAR_GP_PIN(0, 15),
+};
+static const unsigned int hscif3_data_c_mux[] = {
+	HRX3_C_MARK, HTX3_C_MARK,
+};
+static const unsigned int hscif3_data_d_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(2, 7), RCAR_GP_PIN(2, 8),
+};
+static const unsigned int hscif3_data_d_mux[] = {
+	HRX3_D_MARK, HTX3_D_MARK,
+};
+/* - HSCIF4 ----------------------------------------------------------------- */
+static const unsigned int hscif4_data_a_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(1, 12), RCAR_GP_PIN(1, 13),
+};
+static const unsigned int hscif4_data_a_mux[] = {
+	HRX4_A_MARK, HTX4_A_MARK,
+};
+static const unsigned int hscif4_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 11),
+};
+static const unsigned int hscif4_clk_mux[] = {
+	HSCK4_MARK,
+};
+static const unsigned int hscif4_ctrl_pins[] = {
+	/* RTS, CTS */
+	RCAR_GP_PIN(1, 15), RCAR_GP_PIN(1, 14),
+};
+static const unsigned int hscif4_ctrl_mux[] = {
+	HRTS4_N_MARK, HCTS3_N_MARK,
+};
+
+static const unsigned int hscif4_data_b_pins[] = {
+	/* RX, TX */
+	RCAR_GP_PIN(1, 8), RCAR_GP_PIN(1, 11),
+};
+static const unsigned int hscif4_data_b_mux[] = {
+	HRX4_B_MARK, HTX4_B_MARK,
+};
+
 /* - I2C -------------------------------------------------------------------- */
 static const unsigned int i2c1_a_pins[] = {
 	/* SDA, SCL */
@@ -1663,6 +1836,678 @@ static const unsigned int i2c6_c_mux[] = {
 	SDA6_C_MARK, SCL6_C_MARK,
 };
 
+/* - MSIOF0 ----------------------------------------------------------------- */
+static const unsigned int msiof0_clk_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 17),
+};
+static const unsigned int msiof0_clk_mux[] = {
+	MSIOF0_SCK_MARK,
+};
+static const unsigned int msiof0_sync_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(5, 18),
+};
+static const unsigned int msiof0_sync_mux[] = {
+	MSIOF0_SYNC_MARK,
+};
+static const unsigned int msiof0_ss1_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(5, 19),
+};
+static const unsigned int msiof0_ss1_mux[] = {
+	MSIOF0_SS1_MARK,
+};
+static const unsigned int msiof0_ss2_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(5, 21),
+};
+static const unsigned int msiof0_ss2_mux[] = {
+	MSIOF0_SS2_MARK,
+};
+static const unsigned int msiof0_txd_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(5, 20),
+};
+static const unsigned int msiof0_txd_mux[] = {
+	MSIOF0_TXD_MARK,
+};
+static const unsigned int msiof0_rxd_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(5, 22),
+};
+static const unsigned int msiof0_rxd_mux[] = {
+	MSIOF0_RXD_MARK,
+};
+/* - MSIOF1 ----------------------------------------------------------------- */
+static const unsigned int msiof1_clk_a_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(6, 8),
+};
+static const unsigned int msiof1_clk_a_mux[] = {
+	MSIOF1_SCK_A_MARK,
+};
+static const unsigned int msiof1_sync_a_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(6, 9),
+};
+static const unsigned int msiof1_sync_a_mux[] = {
+	MSIOF1_SYNC_A_MARK,
+};
+static const unsigned int msiof1_ss1_a_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(6, 5),
+};
+static const unsigned int msiof1_ss1_a_mux[] = {
+	MSIOF1_SS1_A_MARK,
+};
+static const unsigned int msiof1_ss2_a_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(6, 6),
+};
+static const unsigned int msiof1_ss2_a_mux[] = {
+	MSIOF1_SS2_A_MARK,
+};
+static const unsigned int msiof1_txd_a_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(6, 7),
+};
+static const unsigned int msiof1_txd_a_mux[] = {
+	MSIOF1_TXD_A_MARK,
+};
+static const unsigned int msiof1_rxd_a_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(6, 10),
+};
+static const unsigned int msiof1_rxd_a_mux[] = {
+	MSIOF1_RXD_A_MARK,
+};
+static const unsigned int msiof1_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 9),
+};
+static const unsigned int msiof1_clk_b_mux[] = {
+	MSIOF1_SCK_B_MARK,
+};
+static const unsigned int msiof1_sync_b_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(5, 3),
+};
+static const unsigned int msiof1_sync_b_mux[] = {
+	MSIOF1_SYNC_B_MARK,
+};
+static const unsigned int msiof1_ss1_b_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(5, 4),
+};
+static const unsigned int msiof1_ss1_b_mux[] = {
+	MSIOF1_SS1_B_MARK,
+};
+static const unsigned int msiof1_ss2_b_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(5, 0),
+};
+static const unsigned int msiof1_ss2_b_mux[] = {
+	MSIOF1_SS2_B_MARK,
+};
+static const unsigned int msiof1_txd_b_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(5, 8),
+};
+static const unsigned int msiof1_txd_b_mux[] = {
+	MSIOF1_TXD_B_MARK,
+};
+static const unsigned int msiof1_rxd_b_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(5, 7),
+};
+static const unsigned int msiof1_rxd_b_mux[] = {
+	MSIOF1_RXD_B_MARK,
+};
+static const unsigned int msiof1_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(6, 17),
+};
+static const unsigned int msiof1_clk_c_mux[] = {
+	MSIOF1_SCK_C_MARK,
+};
+static const unsigned int msiof1_sync_c_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(6, 18),
+};
+static const unsigned int msiof1_sync_c_mux[] = {
+	MSIOF1_SYNC_C_MARK,
+};
+static const unsigned int msiof1_ss1_c_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(6, 21),
+};
+static const unsigned int msiof1_ss1_c_mux[] = {
+	MSIOF1_SS1_C_MARK,
+};
+static const unsigned int msiof1_ss2_c_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(6, 27),
+};
+static const unsigned int msiof1_ss2_c_mux[] = {
+	MSIOF1_SS2_C_MARK,
+};
+static const unsigned int msiof1_txd_c_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(6, 20),
+};
+static const unsigned int msiof1_txd_c_mux[] = {
+	MSIOF1_TXD_C_MARK,
+};
+static const unsigned int msiof1_rxd_c_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(6, 19),
+};
+static const unsigned int msiof1_rxd_c_mux[] = {
+	MSIOF1_RXD_C_MARK,
+};
+static const unsigned int msiof1_clk_d_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 12),
+};
+static const unsigned int msiof1_clk_d_mux[] = {
+	MSIOF1_SCK_D_MARK,
+};
+static const unsigned int msiof1_sync_d_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(5, 15),
+};
+static const unsigned int msiof1_sync_d_mux[] = {
+	MSIOF1_SYNC_D_MARK,
+};
+static const unsigned int msiof1_ss1_d_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(5, 16),
+};
+static const unsigned int msiof1_ss1_d_mux[] = {
+	MSIOF1_SS1_D_MARK,
+};
+static const unsigned int msiof1_ss2_d_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(5, 21),
+};
+static const unsigned int msiof1_ss2_d_mux[] = {
+	MSIOF1_SS2_D_MARK,
+};
+static const unsigned int msiof1_txd_d_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(5, 14),
+};
+static const unsigned int msiof1_txd_d_mux[] = {
+	MSIOF1_TXD_D_MARK,
+};
+static const unsigned int msiof1_rxd_d_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(5, 13),
+};
+static const unsigned int msiof1_rxd_d_mux[] = {
+	MSIOF1_RXD_D_MARK,
+};
+static const unsigned int msiof1_clk_e_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 0),
+};
+static const unsigned int msiof1_clk_e_mux[] = {
+	MSIOF1_SCK_E_MARK,
+};
+static const unsigned int msiof1_sync_e_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(3, 1),
+};
+static const unsigned int msiof1_sync_e_mux[] = {
+	MSIOF1_SYNC_E_MARK,
+};
+static const unsigned int msiof1_ss1_e_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(3, 4),
+};
+static const unsigned int msiof1_ss1_e_mux[] = {
+	MSIOF1_SS1_E_MARK,
+};
+static const unsigned int msiof1_ss2_e_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(3, 5),
+};
+static const unsigned int msiof1_ss2_e_mux[] = {
+	MSIOF1_SS2_E_MARK,
+};
+static const unsigned int msiof1_txd_e_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(3, 3),
+};
+static const unsigned int msiof1_txd_e_mux[] = {
+	MSIOF1_TXD_E_MARK,
+};
+static const unsigned int msiof1_rxd_e_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(3, 2),
+};
+static const unsigned int msiof1_rxd_e_mux[] = {
+	MSIOF1_RXD_E_MARK,
+};
+static const unsigned int msiof1_clk_f_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(5, 23),
+};
+static const unsigned int msiof1_clk_f_mux[] = {
+	MSIOF1_SCK_F_MARK,
+};
+static const unsigned int msiof1_sync_f_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(5, 24),
+};
+static const unsigned int msiof1_sync_f_mux[] = {
+	MSIOF1_SYNC_F_MARK,
+};
+static const unsigned int msiof1_ss1_f_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(6, 1),
+};
+static const unsigned int msiof1_ss1_f_mux[] = {
+	MSIOF1_SS1_F_MARK,
+};
+static const unsigned int msiof1_ss2_f_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(6, 2),
+};
+static const unsigned int msiof1_ss2_f_mux[] = {
+	MSIOF1_SS2_F_MARK,
+};
+static const unsigned int msiof1_txd_f_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(6, 0),
+};
+static const unsigned int msiof1_txd_f_mux[] = {
+	MSIOF1_TXD_F_MARK,
+};
+static const unsigned int msiof1_rxd_f_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(5, 25),
+};
+static const unsigned int msiof1_rxd_f_mux[] = {
+	MSIOF1_RXD_F_MARK,
+};
+static const unsigned int msiof1_clk_g_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(3, 6),
+};
+static const unsigned int msiof1_clk_g_mux[] = {
+	MSIOF1_SCK_G_MARK,
+};
+static const unsigned int msiof1_sync_g_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(3, 7),
+};
+static const unsigned int msiof1_sync_g_mux[] = {
+	MSIOF1_SYNC_G_MARK,
+};
+static const unsigned int msiof1_ss1_g_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(3, 10),
+};
+static const unsigned int msiof1_ss1_g_mux[] = {
+	MSIOF1_SS1_G_MARK,
+};
+static const unsigned int msiof1_ss2_g_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(3, 11),
+};
+static const unsigned int msiof1_ss2_g_mux[] = {
+	MSIOF1_SS2_G_MARK,
+};
+static const unsigned int msiof1_txd_g_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(3, 9),
+};
+static const unsigned int msiof1_txd_g_mux[] = {
+	MSIOF1_TXD_G_MARK,
+};
+static const unsigned int msiof1_rxd_g_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(3, 8),
+};
+static const unsigned int msiof1_rxd_g_mux[] = {
+	MSIOF1_RXD_G_MARK,
+};
+/* - MSIOF2 ----------------------------------------------------------------- */
+static const unsigned int msiof2_clk_a_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 9),
+};
+static const unsigned int msiof2_clk_a_mux[] = {
+	MSIOF2_SCK_A_MARK,
+};
+static const unsigned int msiof2_sync_a_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(1, 8),
+};
+static const unsigned int msiof2_sync_a_mux[] = {
+	MSIOF2_SYNC_A_MARK,
+};
+static const unsigned int msiof2_ss1_a_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(1, 6),
+};
+static const unsigned int msiof2_ss1_a_mux[] = {
+	MSIOF2_SS1_A_MARK,
+};
+static const unsigned int msiof2_ss2_a_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(1, 7),
+};
+static const unsigned int msiof2_ss2_a_mux[] = {
+	MSIOF2_SS2_A_MARK,
+};
+static const unsigned int msiof2_txd_a_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(1, 11),
+};
+static const unsigned int msiof2_txd_a_mux[] = {
+	MSIOF2_TXD_A_MARK,
+};
+static const unsigned int msiof2_rxd_a_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(1, 10),
+};
+static const unsigned int msiof2_rxd_a_mux[] = {
+	MSIOF2_RXD_A_MARK,
+};
+static const unsigned int msiof2_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 4),
+};
+static const unsigned int msiof2_clk_b_mux[] = {
+	MSIOF2_SCK_B_MARK,
+};
+static const unsigned int msiof2_sync_b_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(0, 5),
+};
+static const unsigned int msiof2_sync_b_mux[] = {
+	MSIOF2_SYNC_B_MARK,
+};
+static const unsigned int msiof2_ss1_b_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(0, 0),
+};
+static const unsigned int msiof2_ss1_b_mux[] = {
+	MSIOF2_SS1_B_MARK,
+};
+static const unsigned int msiof2_ss2_b_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(0, 1),
+};
+static const unsigned int msiof2_ss2_b_mux[] = {
+	MSIOF2_SS2_B_MARK,
+};
+static const unsigned int msiof2_txd_b_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(0, 7),
+};
+static const unsigned int msiof2_txd_b_mux[] = {
+	MSIOF2_TXD_B_MARK,
+};
+static const unsigned int msiof2_rxd_b_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(0, 6),
+};
+static const unsigned int msiof2_rxd_b_mux[] = {
+	MSIOF2_RXD_B_MARK,
+};
+static const unsigned int msiof2_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(2, 12),
+};
+static const unsigned int msiof2_clk_c_mux[] = {
+	MSIOF2_SCK_C_MARK,
+};
+static const unsigned int msiof2_sync_c_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(2, 11),
+};
+static const unsigned int msiof2_sync_c_mux[] = {
+	MSIOF2_SYNC_C_MARK,
+};
+static const unsigned int msiof2_ss1_c_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(2, 10),
+};
+static const unsigned int msiof2_ss1_c_mux[] = {
+	MSIOF2_SS1_C_MARK,
+};
+static const unsigned int msiof2_ss2_c_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(2, 9),
+};
+static const unsigned int msiof2_ss2_c_mux[] = {
+	MSIOF2_SS2_C_MARK,
+};
+static const unsigned int msiof2_txd_c_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(2, 14),
+};
+static const unsigned int msiof2_txd_c_mux[] = {
+	MSIOF2_TXD_C_MARK,
+};
+static const unsigned int msiof2_rxd_c_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(2, 13),
+};
+static const unsigned int msiof2_rxd_c_mux[] = {
+	MSIOF2_RXD_C_MARK,
+};
+static const unsigned int msiof2_clk_d_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 8),
+};
+static const unsigned int msiof2_clk_d_mux[] = {
+	MSIOF2_SCK_D_MARK,
+};
+static const unsigned int msiof2_sync_d_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(0, 9),
+};
+static const unsigned int msiof2_sync_d_mux[] = {
+	MSIOF2_SYNC_D_MARK,
+};
+static const unsigned int msiof2_ss1_d_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(0, 12),
+};
+static const unsigned int msiof2_ss1_d_mux[] = {
+	MSIOF2_SS1_D_MARK,
+};
+static const unsigned int msiof2_ss2_d_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(0, 13),
+};
+static const unsigned int msiof2_ss2_d_mux[] = {
+	MSIOF2_SS2_D_MARK,
+};
+static const unsigned int msiof2_txd_d_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(0, 11),
+};
+static const unsigned int msiof2_txd_d_mux[] = {
+	MSIOF2_TXD_D_MARK,
+};
+static const unsigned int msiof2_rxd_d_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(0, 10),
+};
+static const unsigned int msiof2_rxd_d_mux[] = {
+	MSIOF2_RXD_D_MARK,
+};
+/* - MSIOF3 ----------------------------------------------------------------- */
+static const unsigned int msiof3_clk_a_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(0, 0),
+};
+static const unsigned int msiof3_clk_a_mux[] = {
+	MSIOF3_SCK_A_MARK,
+};
+static const unsigned int msiof3_sync_a_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(0, 1),
+};
+static const unsigned int msiof3_sync_a_mux[] = {
+	MSIOF3_SYNC_A_MARK,
+};
+static const unsigned int msiof3_ss1_a_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(0, 14),
+};
+static const unsigned int msiof3_ss1_a_mux[] = {
+	MSIOF3_SS1_A_MARK,
+};
+static const unsigned int msiof3_ss2_a_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(0, 15),
+};
+static const unsigned int msiof3_ss2_a_mux[] = {
+	MSIOF3_SS2_A_MARK,
+};
+static const unsigned int msiof3_txd_a_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(0, 3),
+};
+static const unsigned int msiof3_txd_a_mux[] = {
+	MSIOF3_TXD_A_MARK,
+};
+static const unsigned int msiof3_rxd_a_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(0, 2),
+};
+static const unsigned int msiof3_rxd_a_mux[] = {
+	MSIOF3_RXD_A_MARK,
+};
+static const unsigned int msiof3_clk_b_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 2),
+};
+static const unsigned int msiof3_clk_b_mux[] = {
+	MSIOF3_SCK_B_MARK,
+};
+static const unsigned int msiof3_sync_b_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(1, 0),
+};
+static const unsigned int msiof3_sync_b_mux[] = {
+	MSIOF3_SYNC_B_MARK,
+};
+static const unsigned int msiof3_ss1_b_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(1, 4),
+};
+static const unsigned int msiof3_ss1_b_mux[] = {
+	MSIOF3_SS1_B_MARK,
+};
+static const unsigned int msiof3_ss2_b_pins[] = {
+	/* SS2 */
+	RCAR_GP_PIN(1, 5),
+};
+static const unsigned int msiof3_ss2_b_mux[] = {
+	MSIOF3_SS2_B_MARK,
+};
+static const unsigned int msiof3_txd_b_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(1, 1),
+};
+static const unsigned int msiof3_txd_b_mux[] = {
+	MSIOF3_TXD_B_MARK,
+};
+static const unsigned int msiof3_rxd_b_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(1, 3),
+};
+static const unsigned int msiof3_rxd_b_mux[] = {
+	MSIOF3_RXD_B_MARK,
+};
+static const unsigned int msiof3_clk_c_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 12),
+};
+static const unsigned int msiof3_clk_c_mux[] = {
+	MSIOF3_SCK_C_MARK,
+};
+static const unsigned int msiof3_sync_c_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(1, 13),
+};
+static const unsigned int msiof3_sync_c_mux[] = {
+	MSIOF3_SYNC_C_MARK,
+};
+static const unsigned int msiof3_txd_c_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(1, 15),
+};
+static const unsigned int msiof3_txd_c_mux[] = {
+	MSIOF3_TXD_C_MARK,
+};
+static const unsigned int msiof3_rxd_c_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(1, 14),
+};
+static const unsigned int msiof3_rxd_c_mux[] = {
+	MSIOF3_RXD_C_MARK,
+};
+static const unsigned int msiof3_clk_d_pins[] = {
+	/* SCK */
+	RCAR_GP_PIN(1, 22),
+};
+static const unsigned int msiof3_clk_d_mux[] = {
+	MSIOF3_SCK_D_MARK,
+};
+static const unsigned int msiof3_sync_d_pins[] = {
+	/* SYNC */
+	RCAR_GP_PIN(1, 23),
+};
+static const unsigned int msiof3_sync_d_mux[] = {
+	MSIOF3_SYNC_D_MARK,
+};
+static const unsigned int msiof3_ss1_d_pins[] = {
+	/* SS1 */
+	RCAR_GP_PIN(1, 26),
+};
+static const unsigned int msiof3_ss1_d_mux[] = {
+	MSIOF3_SS1_D_MARK,
+};
+static const unsigned int msiof3_txd_d_pins[] = {
+	/* TXD */
+	RCAR_GP_PIN(1, 25),
+};
+static const unsigned int msiof3_txd_d_mux[] = {
+	MSIOF3_TXD_D_MARK,
+};
+static const unsigned int msiof3_rxd_d_pins[] = {
+	/* RXD */
+	RCAR_GP_PIN(1, 24),
+};
+static const unsigned int msiof3_rxd_d_mux[] = {
+	MSIOF3_RXD_D_MARK,
+};
+
+/* - SATA --------------------------------------------------------------------*/
+static const unsigned int sata0_devslp_a_pins[] = {
+	/* DEVSLP */
+	RCAR_GP_PIN(6, 16),
+};
+static const unsigned int sata0_devslp_a_mux[] = {
+	SATA_DEVSLP_A_MARK,
+};
+static const unsigned int sata0_devslp_b_pins[] = {
+	/* DEVSLP */
+	RCAR_GP_PIN(4, 6),
+};
+static const unsigned int sata0_devslp_b_mux[] = {
+	SATA_DEVSLP_B_MARK,
+};
+
 /* - SCIF0 ------------------------------------------------------------------ */
 static const unsigned int scif0_data_pins[] = {
 	/* RX, TX */
@@ -1845,6 +2690,228 @@ static const unsigned int scif5_clk_pins[] = {
 static const unsigned int scif5_clk_mux[] = {
 	SCK5_MARK,
 };
+/* - SDHI0 ------------------------------------------------------------------ */
+static const unsigned int sdhi0_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(3, 2),
+};
+static const unsigned int sdhi0_data1_mux[] = {
+	SD0_DAT0_MARK,
+};
+static const unsigned int sdhi0_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(3, 2), RCAR_GP_PIN(3, 3),
+	RCAR_GP_PIN(3, 4), RCAR_GP_PIN(3, 5),
+};
+static const unsigned int sdhi0_data4_mux[] = {
+	SD0_DAT0_MARK, SD0_DAT1_MARK,
+	SD0_DAT2_MARK, SD0_DAT3_MARK,
+};
+static const unsigned int sdhi0_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(3, 0), RCAR_GP_PIN(3, 1),
+};
+static const unsigned int sdhi0_ctrl_mux[] = {
+	SD0_CLK_MARK, SD0_CMD_MARK,
+};
+static const unsigned int sdhi0_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(3, 12),
+};
+static const unsigned int sdhi0_cd_mux[] = {
+	SD0_CD_MARK,
+};
+static const unsigned int sdhi0_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(3, 13),
+};
+static const unsigned int sdhi0_wp_mux[] = {
+	SD0_WP_MARK,
+};
+/* - SDHI1 ------------------------------------------------------------------ */
+static const unsigned int sdhi1_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(3, 8),
+};
+static const unsigned int sdhi1_data1_mux[] = {
+	SD1_DAT0_MARK,
+};
+static const unsigned int sdhi1_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(3, 8),  RCAR_GP_PIN(3, 9),
+	RCAR_GP_PIN(3, 10), RCAR_GP_PIN(3, 11),
+};
+static const unsigned int sdhi1_data4_mux[] = {
+	SD1_DAT0_MARK, SD1_DAT1_MARK,
+	SD1_DAT2_MARK, SD1_DAT3_MARK,
+};
+static const unsigned int sdhi1_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(3, 6), RCAR_GP_PIN(3, 7),
+};
+static const unsigned int sdhi1_ctrl_mux[] = {
+	SD1_CLK_MARK, SD1_CMD_MARK,
+};
+static const unsigned int sdhi1_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(3, 14),
+};
+static const unsigned int sdhi1_cd_mux[] = {
+	SD1_CD_MARK,
+};
+static const unsigned int sdhi1_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(3, 15),
+};
+static const unsigned int sdhi1_wp_mux[] = {
+	SD1_WP_MARK,
+};
+/* - SDHI2 ------------------------------------------------------------------ */
+static const unsigned int sdhi2_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(4, 2),
+};
+static const unsigned int sdhi2_data1_mux[] = {
+	SD2_DAT0_MARK,
+};
+static const unsigned int sdhi2_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(4, 2), RCAR_GP_PIN(4, 3),
+	RCAR_GP_PIN(4, 4), RCAR_GP_PIN(4, 5),
+};
+static const unsigned int sdhi2_data4_mux[] = {
+	SD2_DAT0_MARK, SD2_DAT1_MARK,
+	SD2_DAT2_MARK, SD2_DAT3_MARK,
+};
+static const unsigned int sdhi2_data8_pins[] = {
+	/* D[0:7] */
+	RCAR_GP_PIN(4, 2),  RCAR_GP_PIN(4, 3),
+	RCAR_GP_PIN(4, 4),  RCAR_GP_PIN(4, 5),
+	RCAR_GP_PIN(3, 8),  RCAR_GP_PIN(3, 9),
+	RCAR_GP_PIN(3, 10), RCAR_GP_PIN(3, 11),
+};
+static const unsigned int sdhi2_data8_mux[] = {
+	SD2_DAT0_MARK, SD2_DAT1_MARK,
+	SD2_DAT2_MARK, SD2_DAT3_MARK,
+	SD2_DAT4_MARK, SD2_DAT5_MARK,
+	SD2_DAT6_MARK, SD2_DAT7_MARK,
+};
+static const unsigned int sdhi2_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(4, 0), RCAR_GP_PIN(4, 1),
+};
+static const unsigned int sdhi2_ctrl_mux[] = {
+	SD2_CLK_MARK, SD2_CMD_MARK,
+};
+static const unsigned int sdhi2_cd_a_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(4, 13),
+};
+static const unsigned int sdhi2_cd_a_mux[] = {
+	SD2_CD_A_MARK,
+};
+static const unsigned int sdhi2_cd_b_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(5, 10),
+};
+static const unsigned int sdhi2_cd_b_mux[] = {
+	SD2_CD_B_MARK,
+};
+static const unsigned int sdhi2_wp_a_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(4, 14),
+};
+static const unsigned int sdhi2_wp_a_mux[] = {
+	SD2_WP_A_MARK,
+};
+static const unsigned int sdhi2_wp_b_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(5, 11),
+};
+static const unsigned int sdhi2_wp_b_mux[] = {
+	SD2_WP_B_MARK,
+};
+static const unsigned int sdhi2_ds_pins[] = {
+	/* DS */
+	RCAR_GP_PIN(4, 6),
+};
+static const unsigned int sdhi2_ds_mux[] = {
+	SD2_DS_MARK,
+};
+/* - SDHI3 ------------------------------------------------------------------ */
+static const unsigned int sdhi3_data1_pins[] = {
+	/* D0 */
+	RCAR_GP_PIN(4, 9),
+};
+static const unsigned int sdhi3_data1_mux[] = {
+	SD3_DAT0_MARK,
+};
+static const unsigned int sdhi3_data4_pins[] = {
+	/* D[0:3] */
+	RCAR_GP_PIN(4, 9),  RCAR_GP_PIN(4, 10),
+	RCAR_GP_PIN(4, 11), RCAR_GP_PIN(4, 12),
+};
+static const unsigned int sdhi3_data4_mux[] = {
+	SD3_DAT0_MARK, SD3_DAT1_MARK,
+	SD3_DAT2_MARK, SD3_DAT3_MARK,
+};
+static const unsigned int sdhi3_data8_pins[] = {
+	/* D[0:7] */
+	RCAR_GP_PIN(4, 9),  RCAR_GP_PIN(4, 10),
+	RCAR_GP_PIN(4, 11), RCAR_GP_PIN(4, 12),
+	RCAR_GP_PIN(4, 13), RCAR_GP_PIN(4, 14),
+	RCAR_GP_PIN(4, 15), RCAR_GP_PIN(4, 16),
+};
+static const unsigned int sdhi3_data8_mux[] = {
+	SD3_DAT0_MARK, SD3_DAT1_MARK,
+	SD3_DAT2_MARK, SD3_DAT3_MARK,
+	SD3_DAT4_MARK, SD3_DAT5_MARK,
+	SD3_DAT6_MARK, SD3_DAT7_MARK,
+};
+static const unsigned int sdhi3_ctrl_pins[] = {
+	/* CLK, CMD */
+	RCAR_GP_PIN(4, 7), RCAR_GP_PIN(4, 8),
+};
+static const unsigned int sdhi3_ctrl_mux[] = {
+	SD3_CLK_MARK, SD3_CMD_MARK,
+};
+static const unsigned int sdhi3_cd_pins[] = {
+	/* CD */
+	RCAR_GP_PIN(4, 15),
+};
+static const unsigned int sdhi3_cd_mux[] = {
+	SD3_CD_MARK,
+};
+static const unsigned int sdhi3_wp_pins[] = {
+	/* WP */
+	RCAR_GP_PIN(4, 16),
+};
+static const unsigned int sdhi3_wp_mux[] = {
+	SD3_WP_MARK,
+};
+static const unsigned int sdhi3_ds_pins[] = {
+	/* DS */
+	RCAR_GP_PIN(4, 17),
+};
+static const unsigned int sdhi3_ds_mux[] = {
+	SD3_DS_MARK,
+};
+
+/* - SCIF Clock ------------------------------------------------------------- */
+static const unsigned int scif_clk_a_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(6, 23),
+};
+static const unsigned int scif_clk_a_mux[] = {
+	SCIF_CLK_A_MARK,
+};
+static const unsigned int scif_clk_b_pins[] = {
+	/* SCIF_CLK */
+	RCAR_GP_PIN(5, 9),
+};
+static const unsigned int scif_clk_b_mux[] = {
+	SCIF_CLK_B_MARK,
+};
 
 /* - SSI -------------------------------------------------------------------- */
 static const unsigned int ssi0_data_pins[] = {
@@ -2050,6 +3117,31 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(avb_avtp_capture_a),
 	SH_PFC_PIN_GROUP(avb_avtp_match_b),
 	SH_PFC_PIN_GROUP(avb_avtp_capture_b),
+	SH_PFC_PIN_GROUP(hscif0_data),
+	SH_PFC_PIN_GROUP(hscif0_clk),
+	SH_PFC_PIN_GROUP(hscif0_ctrl),
+	SH_PFC_PIN_GROUP(hscif1_data_a),
+	SH_PFC_PIN_GROUP(hscif1_clk_a),
+	SH_PFC_PIN_GROUP(hscif1_ctrl_a),
+	SH_PFC_PIN_GROUP(hscif1_data_b),
+	SH_PFC_PIN_GROUP(hscif1_clk_b),
+	SH_PFC_PIN_GROUP(hscif1_ctrl_b),
+	SH_PFC_PIN_GROUP(hscif2_data_a),
+	SH_PFC_PIN_GROUP(hscif2_clk_a),
+	SH_PFC_PIN_GROUP(hscif2_ctrl_a),
+	SH_PFC_PIN_GROUP(hscif2_data_b),
+	SH_PFC_PIN_GROUP(hscif2_clk_b),
+	SH_PFC_PIN_GROUP(hscif2_ctrl_b),
+	SH_PFC_PIN_GROUP(hscif3_data_a),
+	SH_PFC_PIN_GROUP(hscif3_clk),
+	SH_PFC_PIN_GROUP(hscif3_ctrl),
+	SH_PFC_PIN_GROUP(hscif3_data_b),
+	SH_PFC_PIN_GROUP(hscif3_data_c),
+	SH_PFC_PIN_GROUP(hscif3_data_d),
+	SH_PFC_PIN_GROUP(hscif4_data_a),
+	SH_PFC_PIN_GROUP(hscif4_clk),
+	SH_PFC_PIN_GROUP(hscif4_ctrl),
+	SH_PFC_PIN_GROUP(hscif4_data_b),
 	SH_PFC_PIN_GROUP(i2c1_a),
 	SH_PFC_PIN_GROUP(i2c1_b),
 	SH_PFC_PIN_GROUP(i2c2_a),
@@ -2057,6 +3149,101 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(i2c6_a),
 	SH_PFC_PIN_GROUP(i2c6_b),
 	SH_PFC_PIN_GROUP(i2c6_c),
+	SH_PFC_PIN_GROUP(msiof0_clk),
+	SH_PFC_PIN_GROUP(msiof0_sync),
+	SH_PFC_PIN_GROUP(msiof0_ss1),
+	SH_PFC_PIN_GROUP(msiof0_ss2),
+	SH_PFC_PIN_GROUP(msiof0_txd),
+	SH_PFC_PIN_GROUP(msiof0_rxd),
+	SH_PFC_PIN_GROUP(msiof1_clk_a),
+	SH_PFC_PIN_GROUP(msiof1_sync_a),
+	SH_PFC_PIN_GROUP(msiof1_ss1_a),
+	SH_PFC_PIN_GROUP(msiof1_ss2_a),
+	SH_PFC_PIN_GROUP(msiof1_txd_a),
+	SH_PFC_PIN_GROUP(msiof1_rxd_a),
+	SH_PFC_PIN_GROUP(msiof1_clk_b),
+	SH_PFC_PIN_GROUP(msiof1_sync_b),
+	SH_PFC_PIN_GROUP(msiof1_ss1_b),
+	SH_PFC_PIN_GROUP(msiof1_ss2_b),
+	SH_PFC_PIN_GROUP(msiof1_txd_b),
+	SH_PFC_PIN_GROUP(msiof1_rxd_b),
+	SH_PFC_PIN_GROUP(msiof1_clk_c),
+	SH_PFC_PIN_GROUP(msiof1_sync_c),
+	SH_PFC_PIN_GROUP(msiof1_ss1_c),
+	SH_PFC_PIN_GROUP(msiof1_ss2_c),
+	SH_PFC_PIN_GROUP(msiof1_txd_c),
+	SH_PFC_PIN_GROUP(msiof1_rxd_c),
+	SH_PFC_PIN_GROUP(msiof1_clk_d),
+	SH_PFC_PIN_GROUP(msiof1_sync_d),
+	SH_PFC_PIN_GROUP(msiof1_ss1_d),
+	SH_PFC_PIN_GROUP(msiof1_ss2_d),
+	SH_PFC_PIN_GROUP(msiof1_txd_d),
+	SH_PFC_PIN_GROUP(msiof1_rxd_d),
+	SH_PFC_PIN_GROUP(msiof1_clk_e),
+	SH_PFC_PIN_GROUP(msiof1_sync_e),
+	SH_PFC_PIN_GROUP(msiof1_ss1_e),
+	SH_PFC_PIN_GROUP(msiof1_ss2_e),
+	SH_PFC_PIN_GROUP(msiof1_txd_e),
+	SH_PFC_PIN_GROUP(msiof1_rxd_e),
+	SH_PFC_PIN_GROUP(msiof1_clk_f),
+	SH_PFC_PIN_GROUP(msiof1_sync_f),
+	SH_PFC_PIN_GROUP(msiof1_ss1_f),
+	SH_PFC_PIN_GROUP(msiof1_ss2_f),
+	SH_PFC_PIN_GROUP(msiof1_txd_f),
+	SH_PFC_PIN_GROUP(msiof1_rxd_f),
+	SH_PFC_PIN_GROUP(msiof1_clk_g),
+	SH_PFC_PIN_GROUP(msiof1_sync_g),
+	SH_PFC_PIN_GROUP(msiof1_ss1_g),
+	SH_PFC_PIN_GROUP(msiof1_ss2_g),
+	SH_PFC_PIN_GROUP(msiof1_txd_g),
+	SH_PFC_PIN_GROUP(msiof1_rxd_g),
+	SH_PFC_PIN_GROUP(msiof2_clk_a),
+	SH_PFC_PIN_GROUP(msiof2_sync_a),
+	SH_PFC_PIN_GROUP(msiof2_ss1_a),
+	SH_PFC_PIN_GROUP(msiof2_ss2_a),
+	SH_PFC_PIN_GROUP(msiof2_txd_a),
+	SH_PFC_PIN_GROUP(msiof2_rxd_a),
+	SH_PFC_PIN_GROUP(msiof2_clk_b),
+	SH_PFC_PIN_GROUP(msiof2_sync_b),
+	SH_PFC_PIN_GROUP(msiof2_ss1_b),
+	SH_PFC_PIN_GROUP(msiof2_ss2_b),
+	SH_PFC_PIN_GROUP(msiof2_txd_b),
+	SH_PFC_PIN_GROUP(msiof2_rxd_b),
+	SH_PFC_PIN_GROUP(msiof2_clk_c),
+	SH_PFC_PIN_GROUP(msiof2_sync_c),
+	SH_PFC_PIN_GROUP(msiof2_ss1_c),
+	SH_PFC_PIN_GROUP(msiof2_ss2_c),
+	SH_PFC_PIN_GROUP(msiof2_txd_c),
+	SH_PFC_PIN_GROUP(msiof2_rxd_c),
+	SH_PFC_PIN_GROUP(msiof2_clk_d),
+	SH_PFC_PIN_GROUP(msiof2_sync_d),
+	SH_PFC_PIN_GROUP(msiof2_ss1_d),
+	SH_PFC_PIN_GROUP(msiof2_ss2_d),
+	SH_PFC_PIN_GROUP(msiof2_txd_d),
+	SH_PFC_PIN_GROUP(msiof2_rxd_d),
+	SH_PFC_PIN_GROUP(msiof3_clk_a),
+	SH_PFC_PIN_GROUP(msiof3_sync_a),
+	SH_PFC_PIN_GROUP(msiof3_ss1_a),
+	SH_PFC_PIN_GROUP(msiof3_ss2_a),
+	SH_PFC_PIN_GROUP(msiof3_txd_a),
+	SH_PFC_PIN_GROUP(msiof3_rxd_a),
+	SH_PFC_PIN_GROUP(msiof3_clk_b),
+	SH_PFC_PIN_GROUP(msiof3_sync_b),
+	SH_PFC_PIN_GROUP(msiof3_ss1_b),
+	SH_PFC_PIN_GROUP(msiof3_ss2_b),
+	SH_PFC_PIN_GROUP(msiof3_txd_b),
+	SH_PFC_PIN_GROUP(msiof3_rxd_b),
+	SH_PFC_PIN_GROUP(msiof3_clk_c),
+	SH_PFC_PIN_GROUP(msiof3_sync_c),
+	SH_PFC_PIN_GROUP(msiof3_txd_c),
+	SH_PFC_PIN_GROUP(msiof3_rxd_c),
+	SH_PFC_PIN_GROUP(msiof3_clk_d),
+	SH_PFC_PIN_GROUP(msiof3_sync_d),
+	SH_PFC_PIN_GROUP(msiof3_ss1_d),
+	SH_PFC_PIN_GROUP(msiof3_txd_d),
+	SH_PFC_PIN_GROUP(msiof3_rxd_d),
+	SH_PFC_PIN_GROUP(sata0_devslp_a),
+	SH_PFC_PIN_GROUP(sata0_devslp_b),
 	SH_PFC_PIN_GROUP(scif0_data),
 	SH_PFC_PIN_GROUP(scif0_clk),
 	SH_PFC_PIN_GROUP(scif0_ctrl),
@@ -2082,6 +3269,34 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(scif4_ctrl_c),
 	SH_PFC_PIN_GROUP(scif5_data),
 	SH_PFC_PIN_GROUP(scif5_clk),
+	SH_PFC_PIN_GROUP(scif_clk_a),
+	SH_PFC_PIN_GROUP(scif_clk_b),
+	SH_PFC_PIN_GROUP(sdhi0_data1),
+	SH_PFC_PIN_GROUP(sdhi0_data4),
+	SH_PFC_PIN_GROUP(sdhi0_ctrl),
+	SH_PFC_PIN_GROUP(sdhi0_cd),
+	SH_PFC_PIN_GROUP(sdhi0_wp),
+	SH_PFC_PIN_GROUP(sdhi1_data1),
+	SH_PFC_PIN_GROUP(sdhi1_data4),
+	SH_PFC_PIN_GROUP(sdhi1_ctrl),
+	SH_PFC_PIN_GROUP(sdhi1_cd),
+	SH_PFC_PIN_GROUP(sdhi1_wp),
+	SH_PFC_PIN_GROUP(sdhi2_data1),
+	SH_PFC_PIN_GROUP(sdhi2_data4),
+	SH_PFC_PIN_GROUP(sdhi2_data8),
+	SH_PFC_PIN_GROUP(sdhi2_ctrl),
+	SH_PFC_PIN_GROUP(sdhi2_cd_a),
+	SH_PFC_PIN_GROUP(sdhi2_wp_a),
+	SH_PFC_PIN_GROUP(sdhi2_cd_b),
+	SH_PFC_PIN_GROUP(sdhi2_wp_b),
+	SH_PFC_PIN_GROUP(sdhi2_ds),
+	SH_PFC_PIN_GROUP(sdhi3_data1),
+	SH_PFC_PIN_GROUP(sdhi3_data4),
+	SH_PFC_PIN_GROUP(sdhi3_data8),
+	SH_PFC_PIN_GROUP(sdhi3_ctrl),
+	SH_PFC_PIN_GROUP(sdhi3_cd),
+	SH_PFC_PIN_GROUP(sdhi3_wp),
+	SH_PFC_PIN_GROUP(sdhi3_ds),
 	SH_PFC_PIN_GROUP(ssi0_data),
 	SH_PFC_PIN_GROUP(ssi01239_ctrl),
 	SH_PFC_PIN_GROUP(ssi1_data_a),
@@ -2141,6 +3356,46 @@ static const char * const avb_groups[] = {
 	"avb_avtp_capture_b",
 };
 
+static const char * const hscif0_groups[] = {
+	"hscif0_data",
+	"hscif0_clk",
+	"hscif0_ctrl",
+};
+
+static const char * const hscif1_groups[] = {
+	"hscif1_data_a",
+	"hscif1_clk_a",
+	"hscif1_ctrl_a",
+	"hscif1_data_b",
+	"hscif1_clk_b",
+	"hscif1_ctrl_b",
+};
+
+static const char * const hscif2_groups[] = {
+	"hscif2_data_a",
+	"hscif2_clk_a",
+	"hscif2_ctrl_a",
+	"hscif2_data_b",
+	"hscif2_clk_b",
+	"hscif2_ctrl_b",
+};
+
+static const char * const hscif3_groups[] = {
+	"hscif3_data_a",
+	"hscif3_clk",
+	"hscif3_ctrl",
+	"hscif3_data_b",
+	"hscif3_data_c",
+	"hscif3_data_d",
+};
+
+static const char * const hscif4_groups[] = {
+	"hscif4_data_a",
+	"hscif4_clk",
+	"hscif4_ctrl",
+	"hscif4_data_b",
+};
+
 static const char * const i2c1_groups[] = {
 	"i2c1_a",
 	"i2c1_b",
@@ -2157,6 +3412,116 @@ static const char * const i2c6_groups[] = {
 	"i2c6_c",
 };
 
+static const char * const msiof0_groups[] = {
+	"msiof0_clk",
+	"msiof0_sync",
+	"msiof0_ss1",
+	"msiof0_ss2",
+	"msiof0_txd",
+	"msiof0_rxd",
+};
+
+static const char * const msiof1_groups[] = {
+	"msiof1_clk_a",
+	"msiof1_sync_a",
+	"msiof1_ss1_a",
+	"msiof1_ss2_a",
+	"msiof1_txd_a",
+	"msiof1_rxd_a",
+	"msiof1_clk_b",
+	"msiof1_sync_b",
+	"msiof1_ss1_b",
+	"msiof1_ss2_b",
+	"msiof1_txd_b",
+	"msiof1_rxd_b",
+	"msiof1_clk_c",
+	"msiof1_sync_c",
+	"msiof1_ss1_c",
+	"msiof1_ss2_c",
+	"msiof1_txd_c",
+	"msiof1_rxd_c",
+	"msiof1_clk_d",
+	"msiof1_sync_d",
+	"msiof1_ss1_d",
+	"msiof1_ss2_d",
+	"msiof1_txd_d",
+	"msiof1_rxd_d",
+	"msiof1_clk_e",
+	"msiof1_sync_e",
+	"msiof1_ss1_e",
+	"msiof1_ss2_e",
+	"msiof1_txd_e",
+	"msiof1_rxd_e",
+	"msiof1_clk_f",
+	"msiof1_sync_f",
+	"msiof1_ss1_f",
+	"msiof1_ss2_f",
+	"msiof1_txd_f",
+	"msiof1_rxd_f",
+	"msiof1_clk_g",
+	"msiof1_sync_g",
+	"msiof1_ss1_g",
+	"msiof1_ss2_g",
+	"msiof1_txd_g",
+	"msiof1_rxd_g",
+};
+
+static const char * const msiof2_groups[] = {
+	"msiof2_clk_a",
+	"msiof2_sync_a",
+	"msiof2_ss1_a",
+	"msiof2_ss2_a",
+	"msiof2_txd_a",
+	"msiof2_rxd_a",
+	"msiof2_clk_b",
+	"msiof2_sync_b",
+	"msiof2_ss1_b",
+	"msiof2_ss2_b",
+	"msiof2_txd_b",
+	"msiof2_rxd_b",
+	"msiof2_clk_c",
+	"msiof2_sync_c",
+	"msiof2_ss1_c",
+	"msiof2_ss2_c",
+	"msiof2_txd_c",
+	"msiof2_rxd_c",
+	"msiof2_clk_d",
+	"msiof2_sync_d",
+	"msiof2_ss1_d",
+	"msiof2_ss2_d",
+	"msiof2_txd_d",
+	"msiof2_rxd_d",
+};
+
+static const char * const msiof3_groups[] = {
+	"msiof3_clk_a",
+	"msiof3_sync_a",
+	"msiof3_ss1_a",
+	"msiof3_ss2_a",
+	"msiof3_txd_a",
+	"msiof3_rxd_a",
+	"msiof3_clk_b",
+	"msiof3_sync_b",
+	"msiof3_ss1_b",
+	"msiof3_ss2_b",
+	"msiof3_txd_b",
+	"msiof3_rxd_b",
+	"msiof3_clk_c",
+	"msiof3_sync_c",
+	"msiof3_txd_c",
+	"msiof3_rxd_c",
+	"msiof3_clk_d",
+	"msiof3_sync_d",
+	"msiof3_ss1_d",
+	"msiof3_txd_d",
+	"msiof3_rxd_d",
+};
+
+static const char * const sata0_groups[] = {
+	"sata0_devslp_a",
+	"sata0_devslp_b",
+};
+
 static const char * const scif0_groups[] = {
 	"scif0_data",
 	"scif0_clk",
@@ -2200,6 +3565,49 @@ static const char * const scif5_groups[] = {
 	"scif5_clk",
 };
 
+static const char * const scif_clk_groups[] = {
+	"scif_clk_a",
+	"scif_clk_b",
+};
+
+static const char * const sdhi0_groups[] = {
+	"sdhi0_data1",
+	"sdhi0_data4",
+	"sdhi0_ctrl",
+	"sdhi0_cd",
+	"sdhi0_wp",
+};
+
+static const char * const sdhi1_groups[] = {
+	"sdhi1_data1",
+	"sdhi1_data4",
+	"sdhi1_ctrl",
+	"sdhi1_cd",
+	"sdhi1_wp",
+};
+
+static const char * const sdhi2_groups[] = {
+	"sdhi2_data1",
+	"sdhi2_data4",
+	"sdhi2_data8",
+	"sdhi2_ctrl",
+	"sdhi2_cd_a",
+	"sdhi2_wp_a",
+	"sdhi2_cd_b",
+	"sdhi2_wp_b",
+	"sdhi2_ds",
+};
+
+static const char * const sdhi3_groups[] = {
+	"sdhi3_data1",
+	"sdhi3_data4",
+	"sdhi3_data8",
+	"sdhi3_ctrl",
+	"sdhi3_cd",
+	"sdhi3_wp",
+	"sdhi3_ds",
+};
+
 static const char * const ssi_groups[] = {
 	"ssi0_data",
 	"ssi01239_ctrl",
@@ -2231,15 +3639,30 @@ static const char * const ssi_groups[] = {
 static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(audio_clk),
 	SH_PFC_FUNCTION(avb),
+	SH_PFC_FUNCTION(hscif0),
+	SH_PFC_FUNCTION(hscif1),
+	SH_PFC_FUNCTION(hscif2),
+	SH_PFC_FUNCTION(hscif3),
+	SH_PFC_FUNCTION(hscif4),
 	SH_PFC_FUNCTION(i2c1),
 	SH_PFC_FUNCTION(i2c2),
 	SH_PFC_FUNCTION(i2c6),
+	SH_PFC_FUNCTION(msiof0),
+	SH_PFC_FUNCTION(msiof1),
+	SH_PFC_FUNCTION(msiof2),
+	SH_PFC_FUNCTION(msiof3),
+	SH_PFC_FUNCTION(sata0),
 	SH_PFC_FUNCTION(scif0),
 	SH_PFC_FUNCTION(scif1),
 	SH_PFC_FUNCTION(scif2),
 	SH_PFC_FUNCTION(scif3),
 	SH_PFC_FUNCTION(scif4),
 	SH_PFC_FUNCTION(scif5),
+	SH_PFC_FUNCTION(scif_clk),
+	SH_PFC_FUNCTION(sdhi0),
+	SH_PFC_FUNCTION(sdhi1),
+	SH_PFC_FUNCTION(sdhi2),
+	SH_PFC_FUNCTION(sdhi3),
 	SH_PFC_FUNCTION(ssi),
 };
 
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
index 6a69c8c5d943..d25e6f674d0a 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
@@ -2059,7 +2059,7 @@ static const unsigned int lcd2_data9_mux[] = {
 	LCD2D8_MARK,
 };
 static const unsigned int lcd2_data12_pins[] = {
-	/* D[0:12] */
+	/* D[0:11] */
 	128, 129, 142, 143, 144, 145, 138, 139,
 	140, 141, 130, 131,
 };
@@ -2198,6 +2198,420 @@ static const unsigned int mmc0_ctrl_1_pins[] = {
 static const unsigned int mmc0_ctrl_1_mux[] = {
 	MMCCMD1_MARK, MMCCLK1_MARK,
 };
+/* - MSIOF0 ----------------------------------------------------------------- */
+static const unsigned int msiof0_rsck_pins[] = {
+	/* RSCK */
+	66,
+};
+static const unsigned int msiof0_rsck_mux[] = {
+	MSIOF0_RSCK_MARK,
+};
+static const unsigned int msiof0_tsck_pins[] = {
+	/* TSCK */
+	64,
+};
+static const unsigned int msiof0_tsck_mux[] = {
+	MSIOF0_TSCK_MARK,
+};
+static const unsigned int msiof0_rsync_pins[] = {
+	/* RSYNC */
+	67,
+};
+static const unsigned int msiof0_rsync_mux[] = {
+	MSIOF0_RSYNC_MARK,
+};
+static const unsigned int msiof0_tsync_pins[] = {
+	/* TSYNC */
+	63,
+};
+static const unsigned int msiof0_tsync_mux[] = {
+	MSIOF0_TSYNC_MARK,
+};
+static const unsigned int msiof0_ss1_pins[] = {
+	/* SS1 */
+	62,
+};
+static const unsigned int msiof0_ss1_mux[] = {
+	MSIOF0_SS1_MARK,
+};
+static const unsigned int msiof0_ss2_pins[] = {
+	/* SS2 */
+	71,
+};
+static const unsigned int msiof0_ss2_mux[] = {
+	MSIOF0_SS2_MARK,
+};
+static const unsigned int msiof0_rxd_pins[] = {
+	/* RXD */
+	70,
+};
+static const unsigned int msiof0_rxd_mux[] = {
+	MSIOF0_RXD_MARK,
+};
+static const unsigned int msiof0_txd_pins[] = {
+	/* TXD */
+	65,
+};
+static const unsigned int msiof0_txd_mux[] = {
+	MSIOF0_TXD_MARK,
+};
+static const unsigned int msiof0_mck0_pins[] = {
+	/* MSCK0 */
+	68,
+};
+static const unsigned int msiof0_mck0_mux[] = {
+	MSIOF0_MCK0_MARK,
+};
+
+static const unsigned int msiof0_mck1_pins[] = {
+	/* MSCK1 */
+	69,
+};
+static const unsigned int msiof0_mck1_mux[] = {
+	MSIOF0_MCK1_MARK,
+};
+
+static const unsigned int msiof0l_rsck_pins[] = {
+	/* RSCK */
+	214,
+};
+static const unsigned int msiof0l_rsck_mux[] = {
+	MSIOF0L_RSCK_MARK,
+};
+static const unsigned int msiof0l_tsck_pins[] = {
+	/* TSCK */
+	219,
+};
+static const unsigned int msiof0l_tsck_mux[] = {
+	MSIOF0L_TSCK_MARK,
+};
+static const unsigned int msiof0l_rsync_pins[] = {
+	/* RSYNC */
+	215,
+};
+static const unsigned int msiof0l_rsync_mux[] = {
+	MSIOF0L_RSYNC_MARK,
+};
+static const unsigned int msiof0l_tsync_pins[] = {
+	/* TSYNC */
+	217,
+};
+static const unsigned int msiof0l_tsync_mux[] = {
+	MSIOF0L_TSYNC_MARK,
+};
+static const unsigned int msiof0l_ss1_a_pins[] = {
+	/* SS1 */
+	207,
+};
+static const unsigned int msiof0l_ss1_a_mux[] = {
+	PORT207_MSIOF0L_SS1_MARK,
+};
+static const unsigned int msiof0l_ss1_b_pins[] = {
+	/* SS1 */
+	210,
+};
+static const unsigned int msiof0l_ss1_b_mux[] = {
+	PORT210_MSIOF0L_SS1_MARK,
+};
+static const unsigned int msiof0l_ss2_a_pins[] = {
+	/* SS2 */
+	208,
+};
+static const unsigned int msiof0l_ss2_a_mux[] = {
+	PORT208_MSIOF0L_SS2_MARK,
+};
+static const unsigned int msiof0l_ss2_b_pins[] = {
+	/* SS2 */
+	211,
+};
+static const unsigned int msiof0l_ss2_b_mux[] = {
+	PORT211_MSIOF0L_SS2_MARK,
+};
+static const unsigned int msiof0l_rxd_pins[] = {
+	/* RXD */
+	221,
+};
+static const unsigned int msiof0l_rxd_mux[] = {
+	MSIOF0L_RXD_MARK,
+};
+static const unsigned int msiof0l_txd_pins[] = {
+	/* TXD */
+	222,
+};
+static const unsigned int msiof0l_txd_mux[] = {
+	MSIOF0L_TXD_MARK,
+};
+static const unsigned int msiof0l_mck0_pins[] = {
+	/* MSCK0 */
+	212,
+};
+static const unsigned int msiof0l_mck0_mux[] = {
+	MSIOF0L_MCK0_MARK,
+};
+static const unsigned int msiof0l_mck1_pins[] = {
+	/* MSCK1 */
+	213,
+};
+static const unsigned int msiof0l_mck1_mux[] = {
+	MSIOF0L_MCK1_MARK,
+};
+/* - MSIOF1 ----------------------------------------------------------------- */
+static const unsigned int msiof1_rsck_pins[] = {
+	/* RSCK */
+	234,
+};
+static const unsigned int msiof1_rsck_mux[] = {
+	MSIOF1_RSCK_MARK,
+};
+static const unsigned int msiof1_tsck_pins[] = {
+	/* TSCK */
+	232,
+};
+static const unsigned int msiof1_tsck_mux[] = {
+	MSIOF1_TSCK_MARK,
+};
+static const unsigned int msiof1_rsync_pins[] = {
+	/* RSYNC */
+	235,
+};
+static const unsigned int msiof1_rsync_mux[] = {
+	MSIOF1_RSYNC_MARK,
+};
+static const unsigned int msiof1_tsync_pins[] = {
+	/* TSYNC */
+	231,
+};
+static const unsigned int msiof1_tsync_mux[] = {
+	MSIOF1_TSYNC_MARK,
+};
+static const unsigned int msiof1_ss1_pins[] = {
+	/* SS1 */
+	238,
+};
+static const unsigned int msiof1_ss1_mux[] = {
+	MSIOF1_SS1_MARK,
+};
+static const unsigned int msiof1_ss2_pins[] = {
+	/* SS2 */
+	239,
+};
+static const unsigned int msiof1_ss2_mux[] = {
+	MSIOF1_SS2_MARK,
+};
+static const unsigned int msiof1_rxd_pins[] = {
+	/* RXD */
+	233,
+};
+static const unsigned int msiof1_rxd_mux[] = {
+	MSIOF1_RXD_MARK,
+};
+static const unsigned int msiof1_txd_pins[] = {
+	/* TXD */
+	230,
+};
+static const unsigned int msiof1_txd_mux[] = {
+	MSIOF1_TXD_MARK,
+};
+static const unsigned int msiof1_mck0_pins[] = {
+	/* MSCK0 */
+	236,
+};
+static const unsigned int msiof1_mck0_mux[] = {
+	MSIOF1_MCK0_MARK,
+};
+static const unsigned int msiof1_mck1_pins[] = {
+	/* MSCK1 */
+	237,
+};
+static const unsigned int msiof1_mck1_mux[] = {
+	MSIOF1_MCK1_MARK,
+};
+/* - MSIOF2 ----------------------------------------------------------------- */
+static const unsigned int msiof2_rsck_pins[] = {
+	/* RSCK */
+	151,
+};
+static const unsigned int msiof2_rsck_mux[] = {
+	MSIOF2_RSCK_MARK,
+};
+static const unsigned int msiof2_tsck_pins[] = {
+	/* TSCK */
+	135,
+};
+static const unsigned int msiof2_tsck_mux[] = {
+	MSIOF2_TSCK_MARK,
+};
+static const unsigned int msiof2_rsync_pins[] = {
+	/* RSYNC */
+	152,
+};
+static const unsigned int msiof2_rsync_mux[] = {
+	MSIOF2_RSYNC_MARK,
+};
+static const unsigned int msiof2_tsync_pins[] = {
+	/* TSYNC */
+	133,
+};
+static const unsigned int msiof2_tsync_mux[] = {
+	MSIOF2_TSYNC_MARK,
+};
+static const unsigned int msiof2_ss1_a_pins[] = {
+	/* SS1 */
+	131,
+};
+static const unsigned int msiof2_ss1_a_mux[] = {
+	PORT131_MSIOF2_SS1_MARK,
+};
+static const unsigned int msiof2_ss1_b_pins[] = {
+	/* SS1 */
+	153,
+};
+static const unsigned int msiof2_ss1_b_mux[] = {
+	PORT153_MSIOF2_SS1_MARK,
+};
+static const unsigned int msiof2_ss2_a_pins[] = {
+	/* SS2 */
+	132,
+};
+static const unsigned int msiof2_ss2_a_mux[] = {
+	PORT132_MSIOF2_SS2_MARK,
+};
+static const unsigned int msiof2_ss2_b_pins[] = {
+	/* SS2 */
+	156,
+};
+static const unsigned int msiof2_ss2_b_mux[] = {
+	PORT156_MSIOF2_SS2_MARK,
+};
+static const unsigned int msiof2_rxd_a_pins[] = {
+	/* RXD */
+	130,
+};
+static const unsigned int msiof2_rxd_a_mux[] = {
+	PORT130_MSIOF2_RXD_MARK,
+};
+static const unsigned int msiof2_rxd_b_pins[] = {
+	/* RXD */
+	157,
+};
+static const unsigned int msiof2_rxd_b_mux[] = {
+	PORT157_MSIOF2_RXD_MARK,
+};
+static const unsigned int msiof2_txd_pins[] = {
+	/* TXD */
+	134,
+};
+static const unsigned int msiof2_txd_mux[] = {
+	MSIOF2_TXD_MARK,
+};
+static const unsigned int msiof2_mck0_pins[] = {
+	/* MSCK0 */
+	154,
+};
+static const unsigned int msiof2_mck0_mux[] = {
+	MSIOF2_MCK0_MARK,
+};
+static const unsigned int msiof2_mck1_pins[] = {
+	/* MSCK1 */
+	155,
+};
+static const unsigned int msiof2_mck1_mux[] = {
+	MSIOF2_MCK1_MARK,
+};
+
+static const unsigned int msiof2r_tsck_pins[] = {
+	/* TSCK */
+	248,
+};
+static const unsigned int msiof2r_tsck_mux[] = {
+	MSIOF2R_TSCK_MARK,
+};
+static const unsigned int msiof2r_tsync_pins[] = {
+	/* TSYNC */
+	249,
+};
+static const unsigned int msiof2r_tsync_mux[] = {
+	MSIOF2R_TSYNC_MARK,
+};
+static const unsigned int msiof2r_rxd_pins[] = {
+	/* RXD */
+	244,
+};
+static const unsigned int msiof2r_rxd_mux[] = {
+	MSIOF2R_RXD_MARK,
+};
+static const unsigned int msiof2r_txd_pins[] = {
+	/* TXD */
+	245,
+};
+static const unsigned int msiof2r_txd_mux[] = {
+	MSIOF2R_TXD_MARK,
+};
+/* - MSIOF3 (Pin function name of MSIOF3 is named BBIF1) -------------------- */
+static const unsigned int msiof3_rsck_pins[] = {
+	/* RSCK */
+	115,
+};
+static const unsigned int msiof3_rsck_mux[] = {
+	BBIF1_RSCK_MARK,
+};
+static const unsigned int msiof3_tsck_pins[] = {
+	/* TSCK */
+	112,
+};
+static const unsigned int msiof3_tsck_mux[] = {
+	BBIF1_TSCK_MARK,
+};
+static const unsigned int msiof3_rsync_pins[] = {
+	/* RSYNC */
+	116,
+};
+static const unsigned int msiof3_rsync_mux[] = {
+	BBIF1_RSYNC_MARK,
+};
+static const unsigned int msiof3_tsync_pins[] = {
+	/* TSYNC */
+	113,
+};
+static const unsigned int msiof3_tsync_mux[] = {
+	BBIF1_TSYNC_MARK,
+};
+static const unsigned int msiof3_ss1_pins[] = {
+	/* SS1 */
+	117,
+};
+static const unsigned int msiof3_ss1_mux[] = {
+	BBIF1_SS1_MARK,
+};
+static const unsigned int msiof3_ss2_pins[] = {
+	/* SS2 */
+	109,
+};
+static const unsigned int msiof3_ss2_mux[] = {
+	BBIF1_SS2_MARK,
+};
+static const unsigned int msiof3_rxd_pins[] = {
+	/* RXD */
+	111,
+};
+static const unsigned int msiof3_rxd_mux[] = {
+	BBIF1_RXD_MARK,
+};
+static const unsigned int msiof3_txd_pins[] = {
+	/* TXD */
+	114,
+};
+static const unsigned int msiof3_txd_mux[] = {
+	BBIF1_TXD_MARK,
+};
+static const unsigned int msiof3_flow_pins[] = {
+	/* FLOW */
+	117,
+};
+static const unsigned int msiof3_flow_mux[] = {
+	BBIF1_FLOW_MARK,
+};
+
 /* - SCIFA0 ----------------------------------------------------------------- */
 static const unsigned int scifa0_data_pins[] = {
 	/* RXD, TXD */
@@ -2782,6 +3196,64 @@ static const struct sh_pfc_pin_group pinmux_groups[] = {
 	SH_PFC_PIN_GROUP(mmc0_data4_1),
 	SH_PFC_PIN_GROUP(mmc0_data8_1),
 	SH_PFC_PIN_GROUP(mmc0_ctrl_1),
+	SH_PFC_PIN_GROUP(msiof0_rsck),
+	SH_PFC_PIN_GROUP(msiof0_tsck),
+	SH_PFC_PIN_GROUP(msiof0_rsync),
+	SH_PFC_PIN_GROUP(msiof0_tsync),
+	SH_PFC_PIN_GROUP(msiof0_ss1),
+	SH_PFC_PIN_GROUP(msiof0_ss2),
+	SH_PFC_PIN_GROUP(msiof0_rxd),
+	SH_PFC_PIN_GROUP(msiof0_txd),
+	SH_PFC_PIN_GROUP(msiof0_mck0),
+	SH_PFC_PIN_GROUP(msiof0_mck1),
+	SH_PFC_PIN_GROUP(msiof0l_rsck),
+	SH_PFC_PIN_GROUP(msiof0l_tsck),
+	SH_PFC_PIN_GROUP(msiof0l_rsync),
+	SH_PFC_PIN_GROUP(msiof0l_tsync),
+	SH_PFC_PIN_GROUP(msiof0l_ss1_a),
+	SH_PFC_PIN_GROUP(msiof0l_ss1_b),
+	SH_PFC_PIN_GROUP(msiof0l_ss2_a),
+	SH_PFC_PIN_GROUP(msiof0l_ss2_b),
+	SH_PFC_PIN_GROUP(msiof0l_rxd),
+	SH_PFC_PIN_GROUP(msiof0l_txd),
+	SH_PFC_PIN_GROUP(msiof0l_mck0),
+	SH_PFC_PIN_GROUP(msiof0l_mck1),
+	SH_PFC_PIN_GROUP(msiof1_rsck),
+	SH_PFC_PIN_GROUP(msiof1_tsck),
+	SH_PFC_PIN_GROUP(msiof1_rsync),
+	SH_PFC_PIN_GROUP(msiof1_tsync),
+	SH_PFC_PIN_GROUP(msiof1_ss1),
+	SH_PFC_PIN_GROUP(msiof1_ss2),
+	SH_PFC_PIN_GROUP(msiof1_rxd),
+	SH_PFC_PIN_GROUP(msiof1_txd),
+	SH_PFC_PIN_GROUP(msiof1_mck0),
+	SH_PFC_PIN_GROUP(msiof1_mck1),
+	SH_PFC_PIN_GROUP(msiof2_rsck),
+	SH_PFC_PIN_GROUP(msiof2_tsck),
+	SH_PFC_PIN_GROUP(msiof2_rsync),
+	SH_PFC_PIN_GROUP(msiof2_tsync),
+	SH_PFC_PIN_GROUP(msiof2_ss1_a),
+	SH_PFC_PIN_GROUP(msiof2_ss1_b),
+	SH_PFC_PIN_GROUP(msiof2_ss2_a),
+	SH_PFC_PIN_GROUP(msiof2_ss2_b),
+	SH_PFC_PIN_GROUP(msiof2_rxd_a),
+	SH_PFC_PIN_GROUP(msiof2_rxd_b),
+	SH_PFC_PIN_GROUP(msiof2_txd),
+	SH_PFC_PIN_GROUP(msiof2_mck0),
+	SH_PFC_PIN_GROUP(msiof2_mck1),
+	SH_PFC_PIN_GROUP(msiof2r_tsck),
+	SH_PFC_PIN_GROUP(msiof2r_tsync),
+	SH_PFC_PIN_GROUP(msiof2r_rxd),
+	SH_PFC_PIN_GROUP(msiof2r_txd),
+	SH_PFC_PIN_GROUP(msiof3_rsck),
+	SH_PFC_PIN_GROUP(msiof3_tsck),
+	SH_PFC_PIN_GROUP(msiof3_rsync),
+	SH_PFC_PIN_GROUP(msiof3_tsync),
+	SH_PFC_PIN_GROUP(msiof3_ss1),
+	SH_PFC_PIN_GROUP(msiof3_ss2),
+	SH_PFC_PIN_GROUP(msiof3_rxd),
+	SH_PFC_PIN_GROUP(msiof3_txd),
+	SH_PFC_PIN_GROUP(msiof3_flow),
 	SH_PFC_PIN_GROUP(scifa0_data),
 	SH_PFC_PIN_GROUP(scifa0_clk),
 	SH_PFC_PIN_GROUP(scifa0_ctrl),
@@ -2982,6 +3454,76 @@ static const char * const mmc0_groups[] = {
 	"mmc0_ctrl_1",
 };
 
+static const char * const msiof0_groups[] = {
+	"msiof0_rsck",
+	"msiof0_tsck",
+	"msiof0_rsync",
+	"msiof0_tsync",
+	"msiof0_ss1",
+	"msiof0_ss2",
+	"msiof0_rxd",
+	"msiof0_txd",
+	"msiof0_mck0",
+	"msiof0_mck1",
+	"msiof0l_rsck",
+	"msiof0l_tsck",
+	"msiof0l_rsync",
+	"msiof0l_tsync",
+	"msiof0l_ss1_a",
+	"msiof0l_ss1_b",
+	"msiof0l_ss2_a",
+	"msiof0l_ss2_b",
+	"msiof0l_rxd",
+	"msiof0l_txd",
+	"msiof0l_mck0",
+	"msiof0l_mck1",
+};
+
+static const char * const msiof1_groups[] = {
+	"msiof1_rsck",
+	"msiof1_tsck",
+	"msiof1_rsync",
+	"msiof1_tsync",
+	"msiof1_ss1",
+	"msiof1_ss2",
+	"msiof1_rxd",
+	"msiof1_txd",
+	"msiof1_mck0",
+	"msiof1_mck1",
+};
+
+static const char * const msiof2_groups[] = {
+	"msiof2_rsck",
+	"msiof2_tsck",
+	"msiof2_rsync",
+	"msiof2_tsync",
+	"msiof2_ss1_a",
+	"msiof2_ss1_b",
+	"msiof2_ss2_a",
+	"msiof2_ss2_b",
+	"msiof2_rxd_a",
+	"msiof2_rxd_b",
+	"msiof2_txd",
+	"msiof2_mck0",
+	"msiof2_mck1",
+	"msiof2r_tsck",
+	"msiof2r_tsync",
+	"msiof2r_rxd",
+	"msiof2r_txd",
+};
+
+static const char * const msiof3_groups[] = {
+	"msiof3_rsck",
+	"msiof3_tsck",
+	"msiof3_rsync",
+	"msiof3_tsync",
+	"msiof3_ss1",
+	"msiof3_ss2",
+	"msiof3_rxd",
+	"msiof3_txd",
+	"msiof3_flow",
+};
+
 static const char * const scifa0_groups[] = {
 	"scifa0_data",
 	"scifa0_clk",
@@ -3116,6 +3658,10 @@ static const struct sh_pfc_function pinmux_functions[] = {
 	SH_PFC_FUNCTION(lcd),
 	SH_PFC_FUNCTION(lcd2),
 	SH_PFC_FUNCTION(mmc0),
+	SH_PFC_FUNCTION(msiof0),
+	SH_PFC_FUNCTION(msiof1),
+	SH_PFC_FUNCTION(msiof2),
+	SH_PFC_FUNCTION(msiof3),
 	SH_PFC_FUNCTION(scifa0),
 	SH_PFC_FUNCTION(scifa1),
 	SH_PFC_FUNCTION(scifa2),
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7734.c b/drivers/pinctrl/sh-pfc/pfc-sh7734.c
index 9842bb106796..b0b328b3130b 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7734.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7734.c
@@ -14,14 +14,6 @@
 
 #include "sh_pfc.h"
 
-#define PORT_GP_12(bank, fn, sfx)					\
-	PORT_GP_1(bank, 0, fn, sfx),  PORT_GP_1(bank, 1, fn, sfx),	\
-	PORT_GP_1(bank, 2, fn, sfx),  PORT_GP_1(bank, 3, fn, sfx),	\
-	PORT_GP_1(bank, 4, fn, sfx),  PORT_GP_1(bank, 5, fn, sfx),	\
-	PORT_GP_1(bank, 6, fn, sfx),  PORT_GP_1(bank, 7, fn, sfx),	\
-	PORT_GP_1(bank, 8, fn, sfx),  PORT_GP_1(bank, 9, fn, sfx),	\
-	PORT_GP_1(bank, 10, fn, sfx), PORT_GP_1(bank, 11, fn, sfx)
-
 #define CPU_ALL_PORT(fn, sfx)						\
 	PORT_GP_32(0, fn, sfx),						\
 	PORT_GP_32(1, fn, sfx),						\
@@ -585,15 +577,18 @@ enum {
 static const u16 pinmux_data[] = {
 	PINMUX_DATA_GP_ALL(), /* PINMUX_DATA(GP_M_N_DATA, GP_M_N_FN...), */
 
-	PINMUX_DATA(CLKOUT_MARK, FN_CLKOUT),
-	PINMUX_DATA(BS_MARK, FN_BS), PINMUX_DATA(CS0_MARK, FN_CS0),
-	PINMUX_DATA(EX_CS0_MARK, FN_EX_CS0),
-	PINMUX_DATA(RD_MARK, FN_RD), PINMUX_DATA(WE0_MARK, FN_WE0),
-	PINMUX_DATA(WE1_MARK, FN_WE1),
-	PINMUX_DATA(SCL0_MARK, FN_SCL0), PINMUX_DATA(PENC0_MARK, FN_PENC0),
-	PINMUX_DATA(USB_OVC0_MARK, FN_USB_OVC0),
-	PINMUX_DATA(IRQ2_B_MARK, FN_IRQ2_B),
-		PINMUX_DATA(IRQ3_B_MARK, FN_IRQ3_B),
+	PINMUX_SINGLE(CLKOUT),
+	PINMUX_SINGLE(BS),
+	PINMUX_SINGLE(CS0),
+	PINMUX_SINGLE(EX_CS0),
+	PINMUX_SINGLE(RD),
+	PINMUX_SINGLE(WE0),
+	PINMUX_SINGLE(WE1),
+	PINMUX_SINGLE(SCL0),
+	PINMUX_SINGLE(PENC0),
+	PINMUX_SINGLE(USB_OVC0),
+	PINMUX_SINGLE(IRQ2_B),
+	PINMUX_SINGLE(IRQ3_B),
 
 	/* IPSR0 */
 	PINMUX_IPSR_DATA(IP0_1_0, A0),
diff --git a/drivers/pinctrl/sh-pfc/pinctrl.c b/drivers/pinctrl/sh-pfc/pinctrl.c
index 863c3e30ce05..87b0a599afaf 100644
--- a/drivers/pinctrl/sh-pfc/pinctrl.c
+++ b/drivers/pinctrl/sh-pfc/pinctrl.c
@@ -273,8 +273,10 @@ static int sh_pfc_dt_node_to_map(struct pinctrl_dev *pctldev,
 	for_each_child_of_node(np, child) {
 		ret = sh_pfc_dt_subnode_to_map(pctldev, child, map, num_maps,
 					       &index);
-		if (ret < 0)
+		if (ret < 0) {
+			of_node_put(child);
 			goto done;
+		}
 	}
 
 	/* If no mapping has been found in child nodes try the config node. */
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index 7b373d43d981..2123ab49d6a5 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -199,28 +199,82 @@ struct sh_pfc_soc_info {
 	PINMUX_DATA(fn##_MARK, FN_##ms, FN_##ipsr, FN_##fn)
 
 /*
+ * Describe a pinmux configuration for a single-function pin with GPIO
+ * capability.
+ *   - fn: Function name
+ */
+#define PINMUX_SINGLE(fn)						\
+	PINMUX_DATA(fn##_MARK, FN_##fn)
+
+/*
  * GP port style (32 ports banks)
  */
 
 #define PORT_GP_CFG_1(bank, pin, fn, sfx, cfg) fn(bank, pin, GP_##bank##_##pin, sfx, cfg)
 #define PORT_GP_1(bank, pin, fn, sfx)	PORT_GP_CFG_1(bank, pin, fn, sfx, 0)
 
-#define PORT_GP_CFG_32(bank, fn, sfx, cfg)				\
+#define PORT_GP_CFG_4(bank, fn, sfx, cfg)						\
 	PORT_GP_CFG_1(bank, 0,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 1,  fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 2,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 3,  fn, sfx, cfg),	\
+	PORT_GP_CFG_1(bank, 2,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 3,  fn, sfx, cfg)
+#define PORT_GP_4(bank, fn, sfx)	PORT_GP_CFG_4(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_8(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_4(bank, fn, sfx, cfg),						\
 	PORT_GP_CFG_1(bank, 4,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 5,  fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 6,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 7,  fn, sfx, cfg),	\
+	PORT_GP_CFG_1(bank, 6,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 7,  fn, sfx, cfg)
+#define PORT_GP_8(bank, fn, sfx)	PORT_GP_CFG_8(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_9(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_8(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 8,  fn, sfx, cfg)
+#define PORT_GP_9(bank, fn, sfx)	PORT_GP_CFG_9(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_12(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_8(bank, fn, sfx, cfg),						\
 	PORT_GP_CFG_1(bank, 8,  fn, sfx, cfg), PORT_GP_CFG_1(bank, 9,  fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 10, fn, sfx, cfg), PORT_GP_CFG_1(bank, 11, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 12, fn, sfx, cfg), PORT_GP_CFG_1(bank, 13, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 14, fn, sfx, cfg), PORT_GP_CFG_1(bank, 15, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 16, fn, sfx, cfg), PORT_GP_CFG_1(bank, 17, fn, sfx, cfg),	\
+	PORT_GP_CFG_1(bank, 10, fn, sfx, cfg), PORT_GP_CFG_1(bank, 11, fn, sfx, cfg)
+#define PORT_GP_12(bank, fn, sfx)	PORT_GP_CFG_12(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_14(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_12(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 12, fn, sfx, cfg), PORT_GP_CFG_1(bank, 13, fn, sfx, cfg)
+#define PORT_GP_14(bank, fn, sfx)	PORT_GP_CFG_14(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_15(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_14(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 14, fn, sfx, cfg)
+#define PORT_GP_15(bank, fn, sfx)	PORT_GP_CFG_15(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_16(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_14(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 14, fn, sfx, cfg), PORT_GP_CFG_1(bank, 15, fn, sfx, cfg)
+#define PORT_GP_16(bank, fn, sfx)	PORT_GP_CFG_16(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_18(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_16(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 16, fn, sfx, cfg), PORT_GP_CFG_1(bank, 17, fn, sfx, cfg)
+#define PORT_GP_18(bank, fn, sfx)	PORT_GP_CFG_18(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_26(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_18(bank, fn, sfx, cfg),						\
 	PORT_GP_CFG_1(bank, 18, fn, sfx, cfg), PORT_GP_CFG_1(bank, 19, fn, sfx, cfg),	\
 	PORT_GP_CFG_1(bank, 20, fn, sfx, cfg), PORT_GP_CFG_1(bank, 21, fn, sfx, cfg),	\
 	PORT_GP_CFG_1(bank, 22, fn, sfx, cfg), PORT_GP_CFG_1(bank, 23, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 24, fn, sfx, cfg), PORT_GP_CFG_1(bank, 25, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 26, fn, sfx, cfg), PORT_GP_CFG_1(bank, 27, fn, sfx, cfg),	\
-	PORT_GP_CFG_1(bank, 28, fn, sfx, cfg), PORT_GP_CFG_1(bank, 29, fn, sfx, cfg),	\
+	PORT_GP_CFG_1(bank, 24, fn, sfx, cfg), PORT_GP_CFG_1(bank, 25, fn, sfx, cfg)
+#define PORT_GP_26(bank, fn, sfx)	PORT_GP_CFG_26(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_28(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_26(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 26, fn, sfx, cfg), PORT_GP_CFG_1(bank, 27, fn, sfx, cfg)
+#define PORT_GP_28(bank, fn, sfx)	PORT_GP_CFG_28(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_30(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_28(bank, fn, sfx, cfg),						\
+	PORT_GP_CFG_1(bank, 28, fn, sfx, cfg), PORT_GP_CFG_1(bank, 29, fn, sfx, cfg)
+#define PORT_GP_30(bank, fn, sfx)	PORT_GP_CFG_30(bank, fn, sfx, 0)
+
+#define PORT_GP_CFG_32(bank, fn, sfx, cfg)						\
+	PORT_GP_CFG_30(bank, fn, sfx, cfg),						\
 	PORT_GP_CFG_1(bank, 30, fn, sfx, cfg), PORT_GP_CFG_1(bank, 31, fn, sfx, cfg)
 #define PORT_GP_32(bank, fn, sfx)	PORT_GP_CFG_32(bank, fn, sfx, 0)
 
diff --git a/drivers/pinctrl/sirf/pinctrl-atlas7.c b/drivers/pinctrl/sirf/pinctrl-atlas7.c
index 829018c812bd..beb024c31a5d 100644
--- a/drivers/pinctrl/sirf/pinctrl-atlas7.c
+++ b/drivers/pinctrl/sirf/pinctrl-atlas7.c
@@ -161,6 +161,9 @@ enum altas7_pad_type {
 #define IN_DISABLE_VAL_1_REG_SET	0x0A88
 #define IN_DISABLE_VAL_1_REG_CLR	0x0A8C
 
+/* Offset of the SDIO9SEL*/
+#define SYS2PCI_SDIO9SEL 0x14
+
 struct dt_params {
 	const char *property;
 	int value;
@@ -355,11 +358,6 @@ struct atlas7_gpio_chip {
 	struct atlas7_gpio_bank banks[0];
 };
 
-static inline struct atlas7_gpio_chip *to_atlas7_gpio(struct gpio_chip *gc)
-{
-	return container_of(gc, struct atlas7_gpio_chip, chip);
-}
-
 /**
  * @dev: a pointer back to containing device
  * @virtbase: the offset to the controller in virtual memory
@@ -370,6 +368,7 @@ struct atlas7_pmx {
 	struct pinctrl_desc pctl_desc;
 	struct atlas7_pinctrl_data *pctl_data;
 	void __iomem *regs[ATLAS7_PINCTRL_REG_BANKS];
+	void __iomem *sys2pci_base;
 	u32 status_ds[NUM_OF_IN_DISABLE_REG];
 	u32 status_dsv[NUM_OF_IN_DISABLE_REG];
 	struct atlas7_pad_status sleep_data[ATLAS7_PINCTRL_TOTAL_PINS];
@@ -885,11 +884,12 @@ static const unsigned int lr_lcdrom_pins[] = { 73, 54, 57, 58, 59, 60, 61,
 		62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 56, 53, 55, };
 static const unsigned int lvds_analog_pins[] = { 149, 150, 151, 152, 153, 154,
 		155, 156, 157, 158, };
-static const unsigned int nd_df_pins[] = { 44, 43, 42, 41, 40, 39, 38, 37,
-		47, 46, 52, 51, 45, 49, 50, 48, 124, };
-static const unsigned int nd_df_nowp_pins[] = { 44, 43, 42, 41, 40, 39, 38,
-		37, 47, 46, 52, 51, 45, 49, 50, 48, };
+static const unsigned int nd_df_basic_pins[] = { 44, 43, 42, 41, 40, 39, 38,
+		37, 47, 46, 52, 45, 49, 50, 48, };
+static const unsigned int nd_df_wp_pins[] = { 124, };
+static const unsigned int nd_df_cs_pins[] = { 51, };
 static const unsigned int ps_pins[] = { 120, 119, 121, };
+static const unsigned int ps_no_dir_pins[] = { 119, };
 static const unsigned int pwc_core_on_pins[] = { 8, };
 static const unsigned int pwc_ext_on_pins[] = { 6, };
 static const unsigned int pwc_gpio3_clk_pins[] = { 3, };
@@ -944,7 +944,7 @@ static const unsigned int sd2_cdb_pins0[] = { 124, };
 static const unsigned int sd2_cdb_pins1[] = { 161, };
 static const unsigned int sd2_wpb_pins0[] = { 123, };
 static const unsigned int sd2_wpb_pins1[] = { 163, };
-static const unsigned int sd3_pins[] = { 85, 86, 87, 88, 89, 90, };
+static const unsigned int sd3_9_pins[] = { 85, 86, 87, 88, 89, 90, };
 static const unsigned int sd5_pins[] = { 91, 92, 93, 94, 95, 96, };
 static const unsigned int sd6_pins0[] = { 79, 78, 74, 75, 76, 77, };
 static const unsigned int sd6_pins1[] = { 101, 99, 100, 110, 109, 111, };
@@ -998,9 +998,9 @@ static const unsigned int vi_vip1_ext_pins[] = { 74, 75, 76, 77, 78, 79, 80,
 		81, 82, 83, 84, 108, 103, 104, 105, 106, 107, 102, 97, 98,
 		99, 100, };
 static const unsigned int vi_vip1_low8bit_pins[] = { 74, 75, 76, 77, 78, 79,
-		80, 81, };
-static const unsigned int vi_vip1_high8bit_pins[] = { 82, 83, 84, 108, 103,
-		104, 105, 106, };
+		80, 81, 82, 83, 84, };
+static const unsigned int vi_vip1_high8bit_pins[] = { 82, 83, 84, 103, 104,
+		105, 106, 107, 102, 97, 98, };
 
 /* definition of pin group table */
 struct atlas7_pin_group altas7_pin_groups[] = {
@@ -1142,9 +1142,11 @@ struct atlas7_pin_group altas7_pin_groups[] = {
 	GROUP("ld_ldd_lck_grp", ld_ldd_lck_pins),
 	GROUP("lr_lcdrom_grp", lr_lcdrom_pins),
 	GROUP("lvds_analog_grp", lvds_analog_pins),
-	GROUP("nd_df_grp", nd_df_pins),
-	GROUP("nd_df_nowp_grp", nd_df_nowp_pins),
+	GROUP("nd_df_basic_grp", nd_df_basic_pins),
+	GROUP("nd_df_wp_grp", nd_df_wp_pins),
+	GROUP("nd_df_cs_grp", nd_df_cs_pins),
 	GROUP("ps_grp", ps_pins),
+	GROUP("ps_no_dir_grp", ps_no_dir_pins),
 	GROUP("pwc_core_on_grp", pwc_core_on_pins),
 	GROUP("pwc_ext_on_grp", pwc_ext_on_pins),
 	GROUP("pwc_gpio3_clk_grp", pwc_gpio3_clk_pins),
@@ -1196,7 +1198,7 @@ struct atlas7_pin_group altas7_pin_groups[] = {
 	GROUP("sd2_cdb_grp1", sd2_cdb_pins1),
 	GROUP("sd2_wpb_grp0", sd2_wpb_pins0),
 	GROUP("sd2_wpb_grp1", sd2_wpb_pins1),
-	GROUP("sd3_grp", sd3_pins),
+	GROUP("sd3_9_grp", sd3_9_pins),
 	GROUP("sd5_grp", sd5_pins),
 	GROUP("sd6_grp0", sd6_pins0),
 	GROUP("sd6_grp1", sd6_pins1),
@@ -1421,9 +1423,11 @@ static const char * const ld_ldd_fck_grp[] = { "ld_ldd_fck_grp", };
 static const char * const ld_ldd_lck_grp[] = { "ld_ldd_lck_grp", };
 static const char * const lr_lcdrom_grp[] = { "lr_lcdrom_grp", };
 static const char * const lvds_analog_grp[] = { "lvds_analog_grp", };
-static const char * const nd_df_grp[] = { "nd_df_grp", };
-static const char * const nd_df_nowp_grp[] = { "nd_df_nowp_grp", };
+static const char * const nd_df_basic_grp[] = { "nd_df_basic_grp", };
+static const char * const nd_df_wp_grp[] = { "nd_df_wp_grp", };
+static const char * const nd_df_cs_grp[] = { "nd_df_cs_grp", };
 static const char * const ps_grp[] = { "ps_grp", };
+static const char * const ps_no_dir_grp[] = { "ps_no_dir_grp", };
 static const char * const pwc_core_on_grp[] = { "pwc_core_on_grp", };
 static const char * const pwc_ext_on_grp[] = { "pwc_ext_on_grp", };
 static const char * const pwc_gpio3_clk_grp[] = { "pwc_gpio3_clk_grp", };
@@ -1478,7 +1482,7 @@ static const char * const sd2_cdb_grp0[] = { "sd2_cdb_grp0", };
 static const char * const sd2_cdb_grp1[] = { "sd2_cdb_grp1", };
 static const char * const sd2_wpb_grp0[] = { "sd2_wpb_grp0", };
 static const char * const sd2_wpb_grp1[] = { "sd2_wpb_grp1", };
-static const char * const sd3_grp[] = { "sd3_grp", };
+static const char * const sd3_9_grp[] = { "sd3_9_grp", };
 static const char * const sd5_grp[] = { "sd5_grp", };
 static const char * const sd6_grp0[] = { "sd6_grp0", };
 static const char * const sd6_grp1[] = { "sd6_grp1", };
@@ -3174,7 +3178,7 @@ static struct atlas7_grp_mux lvds_analog_grp_mux = {
 	.pad_mux_list = lvds_analog_grp_pad_mux,
 };
 
-static struct atlas7_pad_mux nd_df_grp_pad_mux[] = {
+static struct atlas7_pad_mux nd_df_basic_grp_pad_mux[] = {
 	MUX(1, 44, 1, N, N, N, N),
 	MUX(1, 43, 1, N, N, N, N),
 	MUX(1, 42, 1, N, N, N, N),
@@ -3186,41 +3190,33 @@ static struct atlas7_pad_mux nd_df_grp_pad_mux[] = {
 	MUX(1, 47, 1, N, N, N, N),
 	MUX(1, 46, 1, N, N, N, N),
 	MUX(1, 52, 1, N, N, N, N),
-	MUX(1, 51, 1, N, N, N, N),
 	MUX(1, 45, 1, N, N, N, N),
 	MUX(1, 49, 1, N, N, N, N),
 	MUX(1, 50, 1, N, N, N, N),
 	MUX(1, 48, 1, N, N, N, N),
+};
+
+static struct atlas7_grp_mux nd_df_basic_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(nd_df_basic_grp_pad_mux),
+	.pad_mux_list = nd_df_basic_grp_pad_mux,
+};
+
+static struct atlas7_pad_mux nd_df_wp_grp_pad_mux[] = {
 	MUX(1, 124, 4, N, N, N, N),
 };
 
-static struct atlas7_grp_mux nd_df_grp_mux = {
-	.pad_mux_count = ARRAY_SIZE(nd_df_grp_pad_mux),
-	.pad_mux_list = nd_df_grp_pad_mux,
+static struct atlas7_grp_mux nd_df_wp_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(nd_df_wp_grp_pad_mux),
+	.pad_mux_list = nd_df_wp_grp_pad_mux,
 };
 
-static struct atlas7_pad_mux nd_df_nowp_grp_pad_mux[] = {
-	MUX(1, 44, 1, N, N, N, N),
-	MUX(1, 43, 1, N, N, N, N),
-	MUX(1, 42, 1, N, N, N, N),
-	MUX(1, 41, 1, N, N, N, N),
-	MUX(1, 40, 1, N, N, N, N),
-	MUX(1, 39, 1, N, N, N, N),
-	MUX(1, 38, 1, N, N, N, N),
-	MUX(1, 37, 1, N, N, N, N),
-	MUX(1, 47, 1, N, N, N, N),
-	MUX(1, 46, 1, N, N, N, N),
-	MUX(1, 52, 1, N, N, N, N),
+static struct atlas7_pad_mux nd_df_cs_grp_pad_mux[] = {
 	MUX(1, 51, 1, N, N, N, N),
-	MUX(1, 45, 1, N, N, N, N),
-	MUX(1, 49, 1, N, N, N, N),
-	MUX(1, 50, 1, N, N, N, N),
-	MUX(1, 48, 1, N, N, N, N),
 };
 
-static struct atlas7_grp_mux nd_df_nowp_grp_mux = {
-	.pad_mux_count = ARRAY_SIZE(nd_df_nowp_grp_pad_mux),
-	.pad_mux_list = nd_df_nowp_grp_pad_mux,
+static struct atlas7_grp_mux nd_df_cs_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(nd_df_cs_grp_pad_mux),
+	.pad_mux_list = nd_df_cs_grp_pad_mux,
 };
 
 static struct atlas7_pad_mux ps_grp_pad_mux[] = {
@@ -3234,6 +3230,15 @@ static struct atlas7_grp_mux ps_grp_mux = {
 	.pad_mux_list = ps_grp_pad_mux,
 };
 
+static struct atlas7_pad_mux ps_no_dir_grp_pad_mux[] = {
+	MUX(1, 119, 2, N, N, N, N),
+};
+
+static struct atlas7_grp_mux ps_no_dir_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(ps_no_dir_grp_pad_mux),
+	.pad_mux_list = ps_no_dir_grp_pad_mux,
+};
+
 static struct atlas7_pad_mux pwc_core_on_grp_pad_mux[] = {
 	MUX(0, 8, 1, N, N, N, N),
 };
@@ -3743,7 +3748,7 @@ static struct atlas7_grp_mux sd2_wpb_grp1_mux = {
 	.pad_mux_list = sd2_wpb_grp1_pad_mux,
 };
 
-static struct atlas7_pad_mux sd3_grp_pad_mux[] = {
+static struct atlas7_pad_mux sd3_9_grp_pad_mux[] = {
 	MUX(1, 85, 1, N, N, N, N),
 	MUX(1, 86, 1, N, N, N, N),
 	MUX(1, 87, 1, N, N, N, N),
@@ -3752,9 +3757,9 @@ static struct atlas7_pad_mux sd3_grp_pad_mux[] = {
 	MUX(1, 90, 1, N, N, N, N),
 };
 
-static struct atlas7_grp_mux sd3_grp_mux = {
-	.pad_mux_count = ARRAY_SIZE(sd3_grp_pad_mux),
-	.pad_mux_list = sd3_grp_pad_mux,
+static struct atlas7_grp_mux sd3_9_grp_mux = {
+	.pad_mux_count = ARRAY_SIZE(sd3_9_grp_pad_mux),
+	.pad_mux_list = sd3_9_grp_pad_mux,
 };
 
 static struct atlas7_pad_mux sd5_grp_pad_mux[] = {
@@ -4296,6 +4301,9 @@ static struct atlas7_pad_mux vi_vip1_low8bit_grp_pad_mux[] = {
 	MUX(1, 79, 1, N, N, N, N),
 	MUX(1, 80, 1, N, N, N, N),
 	MUX(1, 81, 1, N, N, N, N),
+	MUX(1, 82, 1, N, N, N, N),
+	MUX(1, 83, 1, N, N, N, N),
+	MUX(1, 84, 1, N, N, N, N),
 };
 
 static struct atlas7_grp_mux vi_vip1_low8bit_grp_mux = {
@@ -4307,11 +4315,14 @@ static struct atlas7_pad_mux vi_vip1_high8bit_grp_pad_mux[] = {
 	MUX(1, 82, 1, N, N, N, N),
 	MUX(1, 83, 1, N, N, N, N),
 	MUX(1, 84, 1, N, N, N, N),
-	MUX(1, 108, 2, N, N, N, N),
 	MUX(1, 103, 2, N, N, N, N),
 	MUX(1, 104, 2, N, N, N, N),
 	MUX(1, 105, 2, N, N, N, N),
 	MUX(1, 106, 2, N, N, N, N),
+	MUX(1, 107, 2, N, N, N, N),
+	MUX(1, 102, 2, N, N, N, N),
+	MUX(1, 97, 2, N, N, N, N),
+	MUX(1, 98, 2, N, N, N, N),
 };
 
 static struct atlas7_grp_mux vi_vip1_high8bit_grp_mux = {
@@ -4598,9 +4609,11 @@ static struct atlas7_pmx_func atlas7_pmx_functions[] = {
 	FUNCTION("ld_ldd_lck", ld_ldd_lck_grp, &ld_ldd_lck_grp_mux),
 	FUNCTION("lr_lcdrom", lr_lcdrom_grp, &lr_lcdrom_grp_mux),
 	FUNCTION("lvds_analog", lvds_analog_grp, &lvds_analog_grp_mux),
-	FUNCTION("nd_df", nd_df_grp, &nd_df_grp_mux),
-	FUNCTION("nd_df_nowp", nd_df_nowp_grp, &nd_df_nowp_grp_mux),
+	FUNCTION("nd_df_basic", nd_df_basic_grp, &nd_df_basic_grp_mux),
+	FUNCTION("nd_df_wp", nd_df_wp_grp, &nd_df_wp_grp_mux),
+	FUNCTION("nd_df_cs", nd_df_cs_grp, &nd_df_cs_grp_mux),
 	FUNCTION("ps", ps_grp, &ps_grp_mux),
+	FUNCTION("ps_no_dir", ps_no_dir_grp, &ps_no_dir_grp_mux),
 	FUNCTION("pwc_core_on", pwc_core_on_grp, &pwc_core_on_grp_mux),
 	FUNCTION("pwc_ext_on", pwc_ext_on_grp, &pwc_ext_on_grp_mux),
 	FUNCTION("pwc_gpio3_clk", pwc_gpio3_clk_grp, &pwc_gpio3_clk_grp_mux),
@@ -4686,10 +4699,11 @@ static struct atlas7_pmx_func atlas7_pmx_functions[] = {
 	FUNCTION("sd2_cdb_m1", sd2_cdb_grp1, &sd2_cdb_grp1_mux),
 	FUNCTION("sd2_wpb_m0", sd2_wpb_grp0, &sd2_wpb_grp0_mux),
 	FUNCTION("sd2_wpb_m1", sd2_wpb_grp1, &sd2_wpb_grp1_mux),
-	FUNCTION("sd3", sd3_grp, &sd3_grp_mux),
+	FUNCTION("sd3", sd3_9_grp, &sd3_9_grp_mux),
 	FUNCTION("sd5", sd5_grp, &sd5_grp_mux),
 	FUNCTION("sd6_m0", sd6_grp0, &sd6_grp0_mux),
 	FUNCTION("sd6_m1", sd6_grp1, &sd6_grp1_mux),
+	FUNCTION("sd9", sd3_9_grp, &sd3_9_grp_mux),
 	FUNCTION("sp0_ext_ldo_on",
 			sp0_ext_ldo_on_grp,
 			&sp0_ext_ldo_on_grp_mux),
@@ -5097,6 +5111,14 @@ static int atlas7_pmx_set_mux(struct pinctrl_dev *pctldev,
 	pr_debug("PMX DUMP ### Function:[%s] Group:[%s] #### START >>>\n",
 			pmx_func->name, pin_grp->name);
 
+	/* the sd3 and sd9 pin select by SYS2PCI_SDIO9SEL register */
+	if (pin_grp->pins == (unsigned int *)&sd3_9_pins) {
+		if (!strcmp(pmx_func->name, "sd9"))
+			writel(1, pmx->sys2pci_base + SYS2PCI_SDIO9SEL);
+		else
+			writel(0, pmx->sys2pci_base + SYS2PCI_SDIO9SEL);
+	}
+
 	grp_mux = pmx_func->grpmux;
 
 	for (idx = 0; idx < grp_mux->pad_mux_count; idx++) {
@@ -5385,12 +5407,27 @@ static int atlas7_pinmux_probe(struct platform_device *pdev)
 	struct atlas7_pmx *pmx;
 	struct device_node *np = pdev->dev.of_node;
 	u32 banks = ATLAS7_PINCTRL_REG_BANKS;
+	struct device_node *sys2pci_np;
+	struct resource res;
 
 	/* Create state holders etc for this driver */
 	pmx = devm_kzalloc(&pdev->dev, sizeof(*pmx), GFP_KERNEL);
 	if (!pmx)
 		return -ENOMEM;
 
+	/* The sd3 and sd9 shared all pins, and the function select by
+	 * SYS2PCI_SDIO9SEL register
+	 */
+	sys2pci_np = of_find_node_by_name(NULL, "sys2pci");
+	if (!sys2pci_np)
+		return -EINVAL;
+	ret = of_address_to_resource(sys2pci_np, 0, &res);
+	if (ret)
+		return ret;
+	pmx->sys2pci_base = devm_ioremap_resource(&pdev->dev, &res);
+	if (IS_ERR(pmx->sys2pci_base))
+		return -ENOMEM;
+
 	pmx->dev = &pdev->dev;
 
 	pmx->pctl_data = &atlas7_ioc_data;
@@ -5600,7 +5637,7 @@ static int __atlas7_gpio_to_pin(struct atlas7_gpio_chip *a7gc, u32 gpio)
 static void atlas7_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(gc);
 	struct atlas7_gpio_bank *bank;
 	void __iomem *ctrl_reg;
 	u32 val, pin_in_bank;
@@ -5638,7 +5675,7 @@ static void __atlas7_gpio_irq_mask(struct atlas7_gpio_chip *a7gc, int idx)
 static void atlas7_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(gc);
 	unsigned long flags;
 
 	spin_lock_irqsave(&a7gc->lock, flags);
@@ -5651,7 +5688,7 @@ static void atlas7_gpio_irq_mask(struct irq_data *d)
 static void atlas7_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(gc);
 	struct atlas7_gpio_bank *bank;
 	void __iomem *ctrl_reg;
 	u32 val, pin_in_bank;
@@ -5675,7 +5712,7 @@ static int atlas7_gpio_irq_type(struct irq_data *d,
 				unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(gc);
 	struct atlas7_gpio_bank *bank;
 	void __iomem *ctrl_reg;
 	u32 val, pin_in_bank;
@@ -5744,7 +5781,7 @@ static struct irq_chip atlas7_gpio_irq_chip = {
 static void atlas7_gpio_handle_irq(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(gc);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(gc);
 	struct atlas7_gpio_bank *bank = NULL;
 	u32 status, ctrl;
 	int pin_in_bank = 0, idx;
@@ -5812,7 +5849,7 @@ static void __atlas7_gpio_set_input(struct atlas7_gpio_chip *a7gc,
 static int atlas7_gpio_request(struct gpio_chip *chip,
 				unsigned int gpio)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	int ret;
 	unsigned long flags;
 
@@ -5840,7 +5877,7 @@ static int atlas7_gpio_request(struct gpio_chip *chip,
 static void atlas7_gpio_free(struct gpio_chip *chip,
 				unsigned int gpio)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&a7gc->lock, flags);
@@ -5856,7 +5893,7 @@ static void atlas7_gpio_free(struct gpio_chip *chip,
 static int atlas7_gpio_direction_input(struct gpio_chip *chip,
 					unsigned int gpio)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&a7gc->lock, flags);
@@ -5893,7 +5930,7 @@ static void __atlas7_gpio_set_output(struct atlas7_gpio_chip *a7gc,
 static int atlas7_gpio_direction_output(struct gpio_chip *chip,
 				unsigned int gpio, int value)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&a7gc->lock, flags);
@@ -5908,7 +5945,7 @@ static int atlas7_gpio_direction_output(struct gpio_chip *chip,
 static int atlas7_gpio_get_value(struct gpio_chip *chip,
 					unsigned int gpio)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	struct atlas7_gpio_bank *bank;
 	u32 val, pin_in_bank;
 	unsigned long flags;
@@ -5928,7 +5965,7 @@ static int atlas7_gpio_get_value(struct gpio_chip *chip,
 static void atlas7_gpio_set_value(struct gpio_chip *chip,
 				unsigned int gpio, int value)
 {
-	struct atlas7_gpio_chip *a7gc = to_atlas7_gpio(chip);
+	struct atlas7_gpio_chip *a7gc = gpiochip_get_data(chip);
 	struct atlas7_gpio_bank *bank;
 	void __iomem *ctrl_reg;
 	u32 ctrl, pin_in_bank;
@@ -6012,10 +6049,10 @@ static int atlas7_gpio_probe(struct platform_device *pdev)
 	chip->label = kstrdup(np->name, GFP_KERNEL);
 	chip->of_node = np;
 	chip->of_gpio_n_cells = 2;
-	chip->dev = &pdev->dev;
+	chip->parent = &pdev->dev;
 
 	/* Add gpio chip to system */
-	ret = gpiochip_add(chip);
+	ret = gpiochip_add_data(chip, a7gc);
 	if (ret) {
 		dev_err(&pdev->dev,
 		"%s: error in probe function with status %d\n",
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 2a8d69725de8..762c0c9c1278 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -85,12 +85,16 @@ static int sirfsoc_dt_node_to_map(struct pinctrl_dev *pctldev,
 	/* calculate number of maps required */
 	for_each_child_of_node(np_config, np) {
 		ret = of_property_read_string(np, "sirf,function", &function);
-		if (ret < 0)
+		if (ret < 0) {
+			of_node_put(np);
 			return ret;
+		}
 
 		ret = of_property_count_strings(np, "sirf,pins");
-		if (ret < 0)
+		if (ret < 0) {
+			of_node_put(np);
 			return ret;
+		}
 
 		count += ret;
 	}
@@ -399,11 +403,6 @@ static int __init sirfsoc_pinmux_init(void)
 }
 arch_initcall(sirfsoc_pinmux_init);
 
-static inline struct sirfsoc_gpio_chip *to_sirfsoc_gpio(struct gpio_chip *gc)
-{
-	return container_of(gc, struct sirfsoc_gpio_chip, chip.gc);
-}
-
 static inline struct sirfsoc_gpio_bank *
 sirfsoc_gpio_to_bank(struct sirfsoc_gpio_chip *sgpio, unsigned int offset)
 {
@@ -418,7 +417,7 @@ static inline int sirfsoc_gpio_to_bankoff(unsigned int offset)
 static void sirfsoc_gpio_irq_ack(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(gc);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
@@ -457,7 +456,7 @@ static void __sirfsoc_gpio_irq_mask(struct sirfsoc_gpio_chip *sgpio,
 static void sirfsoc_gpio_irq_mask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(gc);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 
 	__sirfsoc_gpio_irq_mask(sgpio, bank, d->hwirq % SIRFSOC_GPIO_BANK_SIZE);
@@ -466,7 +465,7 @@ static void sirfsoc_gpio_irq_mask(struct irq_data *d)
 static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(gc);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
@@ -487,7 +486,7 @@ static void sirfsoc_gpio_irq_unmask(struct irq_data *d)
 static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(gc);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, d->hwirq);
 	int idx = sirfsoc_gpio_to_bankoff(d->hwirq);
 	u32 val, offset;
@@ -549,7 +548,7 @@ static void sirfsoc_gpio_handle_irq(struct irq_desc *desc)
 {
 	unsigned int irq = irq_desc_get_irq(desc);
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(gc);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(gc);
 	struct sirfsoc_gpio_bank *bank;
 	u32 status, ctrl;
 	int idx = 0;
@@ -607,7 +606,7 @@ static inline void sirfsoc_gpio_set_input(struct sirfsoc_gpio_chip *sgpio,
 
 static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
@@ -630,7 +629,7 @@ static int sirfsoc_gpio_request(struct gpio_chip *chip, unsigned offset)
 
 static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	unsigned long flags;
 
@@ -646,7 +645,7 @@ static void sirfsoc_gpio_free(struct gpio_chip *chip, unsigned offset)
 
 static int sirfsoc_gpio_direction_input(struct gpio_chip *chip, unsigned gpio)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
 	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	unsigned long flags;
@@ -689,7 +688,7 @@ static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_chip *sgpio,
 static int sirfsoc_gpio_direction_output(struct gpio_chip *chip,
 	unsigned gpio, int value)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
 	int idx = sirfsoc_gpio_to_bankoff(gpio);
 	u32 offset;
@@ -708,7 +707,7 @@ static int sirfsoc_gpio_direction_output(struct gpio_chip *chip,
 
 static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 val;
 	unsigned long flags;
@@ -725,7 +724,7 @@ static int sirfsoc_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 static void sirfsoc_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 	int value)
 {
-	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
+	struct sirfsoc_gpio_chip *sgpio = gpiochip_get_data(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, offset);
 	u32 ctrl;
 	unsigned long flags;
@@ -811,10 +810,10 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	sgpio->chip.gc.of_node = np;
 	sgpio->chip.gc.of_xlate = sirfsoc_gpio_of_xlate;
 	sgpio->chip.gc.of_gpio_n_cells = 2;
-	sgpio->chip.gc.dev = &pdev->dev;
+	sgpio->chip.gc.parent = &pdev->dev;
 	sgpio->chip.regs = regs;
 
-	err = gpiochip_add(&sgpio->chip.gc);
+	err = gpiochip_add_data(&sgpio->chip.gc, sgpio);
 	if (err) {
 		dev_err(&pdev->dev, "%s: error in probe function with status %d\n",
 			np->full_name, err);
diff --git a/drivers/pinctrl/spear/Makefile b/drivers/pinctrl/spear/Makefile
index 0e400ebeb8ff..37b8412ac8a3 100644
--- a/drivers/pinctrl/spear/Makefile
+++ b/drivers/pinctrl/spear/Makefile
@@ -1,7 +1,7 @@
 # SPEAr pinmux support
 
 obj-$(CONFIG_PINCTRL_SPEAR_PLGPIO)	+= pinctrl-plgpio.o
-obj-$(CONFIG_PINCTRL_SPEAR)	+= pinctrl-spear.o
+obj-y				+= pinctrl-spear.o
 obj-$(CONFIG_PINCTRL_SPEAR3XX)	+= pinctrl-spear3xx.o
 obj-$(CONFIG_PINCTRL_SPEAR300)	+= pinctrl-spear300.o
 obj-$(CONFIG_PINCTRL_SPEAR310)	+= pinctrl-spear310.o
diff --git a/drivers/pinctrl/spear/pinctrl-plgpio.c b/drivers/pinctrl/spear/pinctrl-plgpio.c
index 1f0af250dbb5..4c9b863f8267 100644
--- a/drivers/pinctrl/spear/pinctrl-plgpio.c
+++ b/drivers/pinctrl/spear/pinctrl-plgpio.c
@@ -107,7 +107,7 @@ static inline void plgpio_reg_reset(void __iomem *base, u32 pin, u32 reg)
 /* gpio framework specific routines */
 static int plgpio_direction_input(struct gpio_chip *chip, unsigned offset)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 	unsigned long flags;
 
 	/* get correct offset for "offset" pin */
@@ -127,7 +127,7 @@ static int plgpio_direction_input(struct gpio_chip *chip, unsigned offset)
 static int plgpio_direction_output(struct gpio_chip *chip, unsigned offset,
 		int value)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 	unsigned long flags;
 	unsigned dir_offset = offset, wdata_offset = offset, tmp;
 
@@ -159,7 +159,7 @@ static int plgpio_direction_output(struct gpio_chip *chip, unsigned offset,
 
 static int plgpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 
 	if (offset >= chip->ngpio)
 		return -EINVAL;
@@ -176,7 +176,7 @@ static int plgpio_get_value(struct gpio_chip *chip, unsigned offset)
 
 static void plgpio_set_value(struct gpio_chip *chip, unsigned offset, int value)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 
 	if (offset >= chip->ngpio)
 		return;
@@ -196,7 +196,7 @@ static void plgpio_set_value(struct gpio_chip *chip, unsigned offset, int value)
 
 static int plgpio_request(struct gpio_chip *chip, unsigned offset)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 	int gpio = chip->base + offset;
 	unsigned long flags;
 	int ret = 0;
@@ -248,7 +248,7 @@ err0:
 
 static void plgpio_free(struct gpio_chip *chip, unsigned offset)
 {
-	struct plgpio *plgpio = container_of(chip, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(chip);
 	int gpio = chip->base + offset;
 	unsigned long flags;
 
@@ -280,7 +280,7 @@ disable_clk:
 static void plgpio_irq_disable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct plgpio *plgpio = container_of(gc, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	unsigned long flags;
 
@@ -299,7 +299,7 @@ static void plgpio_irq_disable(struct irq_data *d)
 static void plgpio_irq_enable(struct irq_data *d)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct plgpio *plgpio = container_of(gc, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	unsigned long flags;
 
@@ -318,7 +318,7 @@ static void plgpio_irq_enable(struct irq_data *d)
 static int plgpio_irq_set_type(struct irq_data *d, unsigned trigger)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
-	struct plgpio *plgpio = container_of(gc, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(gc);
 	int offset = d->hwirq;
 	void __iomem *reg_off;
 	unsigned int supported_type = 0, val;
@@ -359,7 +359,7 @@ static struct irq_chip plgpio_irqchip = {
 static void plgpio_irq_handler(struct irq_desc *desc)
 {
 	struct gpio_chip *gc = irq_desc_get_handler_data(desc);
-	struct plgpio *plgpio = container_of(gc, struct plgpio, chip);
+	struct plgpio *plgpio = gpiochip_get_data(gc);
 	struct irq_chip *irqchip = irq_desc_get_chip(desc);
 	int regs_count, count, pin, offset, i = 0;
 	unsigned long pending;
@@ -561,7 +561,7 @@ static int plgpio_probe(struct platform_device *pdev)
 	plgpio->chip.get = plgpio_get_value;
 	plgpio->chip.set = plgpio_set_value;
 	plgpio->chip.label = dev_name(&pdev->dev);
-	plgpio->chip.dev = &pdev->dev;
+	plgpio->chip.parent = &pdev->dev;
 	plgpio->chip.owner = THIS_MODULE;
 	plgpio->chip.of_node = pdev->dev.of_node;
 
@@ -573,7 +573,7 @@ static int plgpio_probe(struct platform_device *pdev)
 		}
 	}
 
-	ret = gpiochip_add(&plgpio->chip);
+	ret = gpiochip_add_data(&plgpio->chip, plgpio);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to add gpio chip\n");
 		goto unprepare_clk;
diff --git a/drivers/pinctrl/sunxi/Kconfig b/drivers/pinctrl/sunxi/Kconfig
index e68fd951129a..f8dbc8bec0e1 100644
--- a/drivers/pinctrl/sunxi/Kconfig
+++ b/drivers/pinctrl/sunxi/Kconfig
@@ -51,8 +51,17 @@ config PINCTRL_SUN8I_A23_R
 	depends on RESET_CONTROLLER
 	select PINCTRL_SUNXI_COMMON
 
+config PINCTRL_SUN8I_H3
+	def_bool MACH_SUN8I
+	select PINCTRL_SUNXI_COMMON
+
 config PINCTRL_SUN9I_A80
 	def_bool MACH_SUN9I
 	select PINCTRL_SUNXI_COMMON
 
+config PINCTRL_SUN9I_A80_R
+	def_bool MACH_SUN9I
+	depends on RESET_CONTROLLER
+	select PINCTRL_SUNXI_COMMON
+
 endif
diff --git a/drivers/pinctrl/sunxi/Makefile b/drivers/pinctrl/sunxi/Makefile
index e08029034510..ef82f22bb9ef 100644
--- a/drivers/pinctrl/sunxi/Makefile
+++ b/drivers/pinctrl/sunxi/Makefile
@@ -13,4 +13,6 @@ obj-$(CONFIG_PINCTRL_SUN8I_A23)		+= pinctrl-sun8i-a23.o
 obj-$(CONFIG_PINCTRL_SUN8I_A23_R)	+= pinctrl-sun8i-a23-r.o
 obj-$(CONFIG_PINCTRL_SUN8I_A33)		+= pinctrl-sun8i-a33.o
 obj-$(CONFIG_PINCTRL_SUN8I_A83T)	+= pinctrl-sun8i-a83t.o
+obj-$(CONFIG_PINCTRL_SUN8I_H3)		+= pinctrl-sun8i-h3.o
 obj-$(CONFIG_PINCTRL_SUN9I_A80)		+= pinctrl-sun9i-a80.o
+obj-$(CONFIG_PINCTRL_SUN9I_A80_R)	+= pinctrl-sun9i-a80-r.o
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun8i-h3.c b/drivers/pinctrl/sunxi/pinctrl-sun8i-h3.c
new file mode 100644
index 000000000000..11760bbe9d51
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun8i-h3.c
@@ -0,0 +1,516 @@
+/*
+ * Allwinner H3 SoCs pinctrl driver.
+ *
+ * Copyright (C) 2015 Jens Kuske <jenskuske@gmail.com>
+ *
+ * Based on pinctrl-sun8i-a23.c, which is:
+ * Copyright (C) 2014 Chen-Yu Tsai <wens@csie.org>
+ * Copyright (C) 2014 Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun8i_h3_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* TX */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* MS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 0)),	/* PA_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* CK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 1)),	/* PA_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* RTS */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DO */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 2)),	/* PA_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart2"),		/* CTS */
+		  SUNXI_FUNCTION(0x3, "jtag"),		/* DI */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 3)),	/* PA_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 4)),	/* PA_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart0"),		/* RX */
+		  SUNXI_FUNCTION(0x3, "pwm0"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 5)),	/* PA_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "sim"),		/* PWREN */
+		  SUNXI_FUNCTION(0x3, "pwm1"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 6)),	/* PA_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "sim"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 7)),	/* PA_EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "sim"),		/* DATA */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 8)),	/* PA_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "sim"),		/* RST */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 9)),	/* PA_EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "sim"),		/* DET */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 10)),	/* PA_EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "di"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 11)),	/* PA_EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2c0"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "di"),		/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 12)),	/* PA_EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CS */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 13)),	/* PA_EINT13 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 14)),	/* PA_EINT14 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MOSI */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 15)),	/* PA_EINT15 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spi1"),		/* MISO */
+		  SUNXI_FUNCTION(0x3, "uart3"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 16)),	/* PA_EINT16 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "spdif"),		/* OUT */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 17)),	/* PA_EINT17 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 18),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* SYNC */
+		  SUNXI_FUNCTION(0x3, "i2c1"),		/* SCK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 18)),	/* PA_EINT18 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 19),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "i2c1"),		/* SDA */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 19)),	/* PA_EINT19 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 20),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DOUT */
+		  SUNXI_FUNCTION(0x3, "sim"),		/* VPPEN */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 20)),	/* PA_EINT20 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(A, 21),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s0"),		/* DIN */
+		  SUNXI_FUNCTION(0x3, "sim"),		/* VPPPP */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 21)),	/* PA_EINT21 */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* WE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MOSI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* ALE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* MISO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CLE */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* CE1 */
+		  SUNXI_FUNCTION(0x3, "spi0")),		/* CS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* CE0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RE */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* RB0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* CMD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0")),	/* RB1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ0 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ1 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ2 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ3 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ4 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand0"),		/* DQ5 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQ6 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQ7 */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(C, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "nand"),		/* DQS */
+		  SUNXI_FUNCTION(0x3, "mmc2")),		/* RST */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXCTL/RXDV */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* RXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXD3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXD2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXD1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXD0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* CRS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXCTL/TXEN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* TXERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* CLKIN/COL */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 16),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* MDC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(D, 17),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "emac")),		/* MDIO */
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* PCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* CLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* MCLK */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* ERR */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* HSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* SYNC */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* VSYNC */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* DVLD */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D4 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D5 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D6 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* D7 */
+		  SUNXI_FUNCTION(0x3, "ts")),		/* D7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* SCK */
+		  SUNXI_FUNCTION(0x3, "i2c2")),		/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "csi"),		/* SDA */
+		  SUNXI_FUNCTION(0x3, "i2c2")),		/* SDA */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(E, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D1 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* MS */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D0 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DI */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CLK */
+		  SUNXI_FUNCTION(0x3, "uart0")),	/* TX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* CMD */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* DO */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D3 */
+		  SUNXI_FUNCTION(0x3, "uart0")),	/* RX */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc0"),		/* D2 */
+		  SUNXI_FUNCTION(0x3, "jtag")),		/* CK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(F, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out")),
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 0)),	/* PG_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* CMD */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 1)),	/* PG_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D0 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 2)),	/* PG_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D1 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 3)),	/* PG_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D2 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 4)),	/* PG_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "mmc1"),		/* D3 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 5)),	/* PG_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 6)),	/* PG_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 7)),	/* PG_EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* RTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 8)),	/* PG_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "uart1"),		/* CTS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 9)),	/* PG_EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* SYNC */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 10)),	/* PG_EINT10 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* CLK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 11)),	/* PG_EINT11 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* DOUT */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 12)),	/* PG_EINT12 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(G, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "i2s1"),		/* DIN */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 13)),	/* PG_EINT13 */
+};
+
+static const struct sunxi_pinctrl_desc sun8i_h3_pinctrl_data = {
+	.pins = sun8i_h3_pins,
+	.npins = ARRAY_SIZE(sun8i_h3_pins),
+	.irq_banks = 2,
+	.irq_read_needs_mux = true
+};
+
+static int sun8i_h3_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun8i_h3_pinctrl_data);
+}
+
+static const struct of_device_id sun8i_h3_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun8i-h3-pinctrl", },
+	{}
+};
+
+static struct platform_driver sun8i_h3_pinctrl_driver = {
+	.probe	= sun8i_h3_pinctrl_probe,
+	.driver	= {
+		.name		= "sun8i-h3-pinctrl",
+		.of_match_table	= sun8i_h3_pinctrl_match,
+	},
+};
+builtin_platform_driver(sun8i_h3_pinctrl_driver);
diff --git a/drivers/pinctrl/sunxi/pinctrl-sun9i-a80-r.c b/drivers/pinctrl/sunxi/pinctrl-sun9i-a80-r.c
new file mode 100644
index 000000000000..42547ffa20a8
--- /dev/null
+++ b/drivers/pinctrl/sunxi/pinctrl-sun9i-a80-r.c
@@ -0,0 +1,181 @@
+/*
+ * Allwinner A80 SoCs special pins pinctrl driver.
+ *
+ * Copyright (C) 2014 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pinctrl/pinctrl.h>
+#include <linux/reset.h>
+
+#include "pinctrl-sunxi.h"
+
+static const struct sunxi_desc_pin sun9i_a80_r_pins[] = {
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_uart"),	/* TX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 0)),	/* PL_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_uart"),	/* RX */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 1)),	/* PL_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag"),	/* TMS */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 2)),	/* PL_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag"),	/* TCK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 3)),	/* PL_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag"),	/* TDO */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 4)),	/* PL_EINT4 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 5),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_jtag"),	/* TDI */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 5)),	/* PL_EINT5 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 6),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_cir_rx"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 6)),	/* PL_EINT6 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 7),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "1wire"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 7)),	/* PL_EINT7 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_ps2"),		/* SCK1 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 8)),	/* PL_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(L, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_ps2"),		/* SDA1 */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 0, 9)),	/* PL_EINT9 */
+
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 0)),	/* PM_EINT0 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 1)),	/* PM_EINT1 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 2),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 2)),	/* PM_EINT2 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 3),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 3)),	/* PM_EINT3 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 4),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_i2s1"),	/* LRCKR */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 4)),	/* PM_EINT4 */
+
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 8),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_i2c1"),	/* SCK */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 8)),	/* PM_EINT8 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 9),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x3, "s_i2c1"),	/* SDA */
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 9)),	/* PM_EINT9 */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 10),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2s0"),	/* MCLK */
+		  SUNXI_FUNCTION(0x3, "s_i2s1")),	/* MCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 11),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2s0"),	/* BCLK */
+		  SUNXI_FUNCTION(0x3, "s_i2s1")),	/* BCLK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 12),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2s0"),	/* LRCK */
+		  SUNXI_FUNCTION(0x3, "s_i2s1")),	/* LRCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 13),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2s0"),	/* DIN */
+		  SUNXI_FUNCTION(0x3, "s_i2s1")),	/* DIN */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 14),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2s0"),	/* DOUT */
+		  SUNXI_FUNCTION(0x3, "s_i2s1")),	/* DOUT */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(M, 15),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION_IRQ_BANK(0x6, 1, 15)),	/* PM_EINT15 */
+
+	/* Hole */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(N, 0),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2c0"),	/* SCK */
+		  SUNXI_FUNCTION(0x3, "s_rsb")),	/* SCK */
+	SUNXI_PIN(SUNXI_PINCTRL_PIN(N, 1),
+		  SUNXI_FUNCTION(0x0, "gpio_in"),
+		  SUNXI_FUNCTION(0x1, "gpio_out"),
+		  SUNXI_FUNCTION(0x2, "s_i2c0"),	/* SDA */
+		  SUNXI_FUNCTION(0x3, "s_rsb")),	/* SDA */
+};
+
+static const struct sunxi_pinctrl_desc sun9i_a80_r_pinctrl_data = {
+	.pins = sun9i_a80_r_pins,
+	.npins = ARRAY_SIZE(sun9i_a80_r_pins),
+	.pin_base = PL_BASE,
+	.irq_banks = 2,
+};
+
+static int sun9i_a80_r_pinctrl_probe(struct platform_device *pdev)
+{
+	return sunxi_pinctrl_init(pdev,
+				  &sun9i_a80_r_pinctrl_data);
+}
+
+static const struct of_device_id sun9i_a80_r_pinctrl_match[] = {
+	{ .compatible = "allwinner,sun9i-a80-r-pinctrl", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, sun9i_a80_r_pinctrl_match);
+
+static struct platform_driver sun9i_a80_r_pinctrl_driver = {
+	.probe	= sun9i_a80_r_pinctrl_probe,
+	.driver	= {
+		.name		= "sun9i-a80-r-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= sun9i_a80_r_pinctrl_match,
+	},
+};
+module_platform_driver(sun9i_a80_r_pinctrl_driver);
+
+MODULE_AUTHOR("Maxime Ripard <maxime.ripard@free-electrons.com");
+MODULE_DESCRIPTION("Allwinner A80 R_PIO pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index dead97daca35..7a2465f5e71e 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -12,7 +12,7 @@
 
 #include <linux/io.h>
 #include <linux/clk.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
 #include <linux/module.h>
@@ -454,7 +454,7 @@ static int sunxi_pinctrl_gpio_direction_input(struct gpio_chip *chip,
 
 static int sunxi_pinctrl_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
-	struct sunxi_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct sunxi_pinctrl *pctl = gpiochip_get_data(chip);
 	u32 reg = sunxi_data_reg(offset);
 	u8 index = sunxi_data_offset(offset);
 	u32 set_mux = pctl->desc->irq_read_needs_mux &&
@@ -469,13 +469,13 @@ static int sunxi_pinctrl_gpio_get(struct gpio_chip *chip, unsigned offset)
 	if (set_mux)
 		sunxi_pmx_set(pctl->pctl_dev, offset, SUN4I_FUNC_IRQ);
 
-	return val;
+	return !!val;
 }
 
 static void sunxi_pinctrl_gpio_set(struct gpio_chip *chip,
 				unsigned offset, int value)
 {
-	struct sunxi_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct sunxi_pinctrl *pctl = gpiochip_get_data(chip);
 	u32 reg = sunxi_data_reg(offset);
 	u8 index = sunxi_data_offset(offset);
 	unsigned long flags;
@@ -522,7 +522,7 @@ static int sunxi_pinctrl_gpio_of_xlate(struct gpio_chip *gc,
 
 static int sunxi_pinctrl_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 {
-	struct sunxi_pinctrl *pctl = dev_get_drvdata(chip->dev);
+	struct sunxi_pinctrl *pctl = gpiochip_get_data(chip);
 	struct sunxi_desc_function *desc;
 	unsigned pinnum = pctl->desc->pin_base + offset;
 	unsigned irqnum;
@@ -536,7 +536,7 @@ static int sunxi_pinctrl_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
 
 	irqnum = desc->irqbank * IRQ_PER_BANK + desc->irqnum;
 
-	dev_dbg(chip->dev, "%s: request IRQ for GPIO %d, return %d\n",
+	dev_dbg(chip->parent, "%s: request IRQ for GPIO %d, return %d\n",
 		chip->label, offset + chip->base, irqnum);
 
 	return irq_find_mapping(pctl->domain, irqnum);
@@ -959,10 +959,10 @@ int sunxi_pinctrl_init(struct platform_device *pdev,
 	pctl->chip->ngpio = round_up(last_pin, PINS_PER_BANK) -
 			    pctl->desc->pin_base;
 	pctl->chip->label = dev_name(&pdev->dev);
-	pctl->chip->dev = &pdev->dev;
+	pctl->chip->parent = &pdev->dev;
 	pctl->chip->base = pctl->desc->pin_base;
 
-	ret = gpiochip_add(pctl->chip);
+	ret = gpiochip_add_data(pctl->chip, pctl);
 	if (ret)
 		goto pinctrl_error;
 
diff --git a/drivers/pinctrl/uniphier/Kconfig b/drivers/pinctrl/uniphier/Kconfig
index ad907072e09f..7abd614dc383 100644
--- a/drivers/pinctrl/uniphier/Kconfig
+++ b/drivers/pinctrl/uniphier/Kconfig
@@ -1,32 +1,35 @@
-if ARCH_UNIPHIER
-
-config PINCTRL_UNIPHIER
-	bool
+menuconfig PINCTRL_UNIPHIER
+	bool "UniPhier SoC pinctrl drivers"
+	depends on ARCH_UNIPHIER
+	depends on OF && MFD_SYSCON
+	default y
 	select PINMUX
 	select GENERIC_PINCONF
 
+if PINCTRL_UNIPHIER
+
 config PINCTRL_UNIPHIER_PH1_LD4
 	tristate "UniPhier PH1-LD4 SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 config PINCTRL_UNIPHIER_PH1_PRO4
 	tristate "UniPhier PH1-Pro4 SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 config PINCTRL_UNIPHIER_PH1_SLD8
 	tristate "UniPhier PH1-sLD8 SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 config PINCTRL_UNIPHIER_PH1_PRO5
 	tristate "UniPhier PH1-Pro5 SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 config PINCTRL_UNIPHIER_PROXSTREAM2
 	tristate "UniPhier ProXstream2 SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 config PINCTRL_UNIPHIER_PH1_LD6B
 	tristate "UniPhier PH1-LD6b SoC pinctrl driver"
-	select PINCTRL_UNIPHIER
+	default y
 
 endif
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index fb22d3f62480..5c261bf5542f 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -14,7 +14,7 @@
  */
 
 #include <linux/err.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
@@ -488,7 +488,7 @@ static struct pinctrl_desc wmt_desc = {
 
 static int wmt_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 {
-	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	struct wmt_pinctrl_data *data = gpiochip_get_data(chip);
 	u32 bank = WMT_BANK_FROM_PIN(offset);
 	u32 bit = WMT_BIT_FROM_PIN(offset);
 	u32 reg_dir = data->banks[bank].reg_dir;
@@ -503,7 +503,7 @@ static int wmt_gpio_get_direction(struct gpio_chip *chip, unsigned offset)
 
 static int wmt_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 {
-	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	struct wmt_pinctrl_data *data = gpiochip_get_data(chip);
 	u32 bank = WMT_BANK_FROM_PIN(offset);
 	u32 bit = WMT_BIT_FROM_PIN(offset);
 	u32 reg_data_in = data->banks[bank].reg_data_in;
@@ -519,7 +519,7 @@ static int wmt_gpio_get_value(struct gpio_chip *chip, unsigned offset)
 static void wmt_gpio_set_value(struct gpio_chip *chip, unsigned offset,
 			       int val)
 {
-	struct wmt_pinctrl_data *data = dev_get_drvdata(chip->dev);
+	struct wmt_pinctrl_data *data = gpiochip_get_data(chip);
 	u32 bank = WMT_BANK_FROM_PIN(offset);
 	u32 bit = WMT_BIT_FROM_PIN(offset);
 	u32 reg_data_out = data->banks[bank].reg_data_out;
@@ -575,7 +575,7 @@ int wmt_pinctrl_probe(struct platform_device *pdev,
 	wmt_desc.npins = data->npins;
 
 	data->gpio_chip = wmt_gpio_chip;
-	data->gpio_chip.dev = &pdev->dev;
+	data->gpio_chip.parent = &pdev->dev;
 	data->gpio_chip.of_node = pdev->dev.of_node;
 	data->gpio_chip.ngpio = data->nbanks * 32;
 
@@ -589,7 +589,7 @@ int wmt_pinctrl_probe(struct platform_device *pdev,
 		return PTR_ERR(data->pctl_dev);
 	}
 
-	err = gpiochip_add(&data->gpio_chip);
+	err = gpiochip_add_data(&data->gpio_chip, data);
 	if (err) {
 		dev_err(&pdev->dev, "could not add GPIO chip\n");
 		goto fail_gpio;
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index 1089eaa02b00..69f93a576e45 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -587,6 +587,20 @@ config EEEPC_WMI
 	  If you have an ACPI-WMI compatible Eee PC laptop (>= 1000), say Y or M
 	  here.
 
+config ASUS_WIRELESS
+	tristate "Asus Wireless Radio Control Driver"
+	depends on ACPI
+	depends on INPUT
+	---help---
+	  The Asus Wireless Radio Control handles the airplane mode hotkey
+	  present on some Asus laptops.
+
+	  Say Y or M here if you have an ASUS notebook with an airplane mode
+	  hotkey.
+
+	  If you choose to compile this driver as a module the module will be
+	  called asus-wireless.
+
 config ACPI_WMI
 	tristate "WMI"
 	depends on ACPI
@@ -641,6 +655,7 @@ config ACPI_TOSHIBA
 	depends on INPUT
 	depends on SERIO_I8042 || SERIO_I8042 = n
 	depends on ACPI_VIDEO || ACPI_VIDEO = n
+	depends on RFKILL || RFKILL = n
 	select INPUT_POLLDEV
 	select INPUT_SPARSEKMAP
 	---help---
@@ -731,6 +746,18 @@ config ACPI_CMPC
 	  keys as input device, backlight device, tablet and accelerometer
 	  devices.
 
+config INTEL_HID_EVENT
+	tristate "INTEL HID Event"
+	depends on ACPI
+	depends on INPUT
+	select INPUT_SPARSEKMAP
+	help
+	  This driver provides support for the Intel HID Event hotkey interface.
+	  Some laptops require this driver for hotkey support.
+
+	  To compile this driver as a module, choose M here: the module will
+	  be called intel_hid.
+
 config INTEL_SCU_IPC
 	bool "Intel SCU IPC Support"
 	depends on X86_INTEL_MID
@@ -940,8 +967,25 @@ config INTEL_PMC_IPC
 	with other entities in the CPU.
 
 config SURFACE_PRO3_BUTTON
-	tristate "Power/home/volume buttons driver for Microsoft Surface Pro 3 tablet"
+	tristate "Power/home/volume buttons driver for Microsoft Surface Pro 3/4 tablet"
 	depends on ACPI && INPUT
 	---help---
-	  This driver handles the power/home/volume buttons on the Microsoft Surface Pro 3 tablet.
+	  This driver handles the power/home/volume buttons on the Microsoft Surface Pro 3/4 tablet.
+
+config INTEL_PUNIT_IPC
+	tristate "Intel P-Unit IPC Driver"
+	---help---
+	  This driver provides support for Intel P-Unit Mailbox IPC mechanism,
+	  which is used to bridge the communications between kernel and P-Unit.
+
+config INTEL_TELEMETRY
+	tristate "Intel SoC Telemetry Driver"
+	default n
+	depends on INTEL_PMC_IPC && INTEL_PUNIT_IPC && X86_64
+	---help---
+	  This driver provides interfaces to configure and use
+	  telemetry for INTEL SoC from APL onwards. It is also
+	  used to get various SoC events and parameters
+	  directly via debugfs files. Various tools may use
+	  this interface for SoC state monitoring.
 endif # X86_PLATFORM_DEVICES
diff --git a/drivers/platform/x86/Makefile b/drivers/platform/x86/Makefile
index 3ca78a3eb6f8..40574e7390f3 100644
--- a/drivers/platform/x86/Makefile
+++ b/drivers/platform/x86/Makefile
@@ -5,6 +5,7 @@
 obj-$(CONFIG_ASUS_LAPTOP)	+= asus-laptop.o
 obj-$(CONFIG_ASUS_WMI)		+= asus-wmi.o
 obj-$(CONFIG_ASUS_NB_WMI)	+= asus-nb-wmi.o
+obj-$(CONFIG_ASUS_WIRELESS)	+= asus-wireless.o
 obj-$(CONFIG_EEEPC_LAPTOP)	+= eeepc-laptop.o
 obj-$(CONFIG_EEEPC_WMI)		+= eeepc-wmi.o
 obj-$(CONFIG_MSI_LAPTOP)	+= msi-laptop.o
@@ -41,6 +42,7 @@ obj-$(CONFIG_ACPI_TOSHIBA)	+= toshiba_acpi.o
 obj-$(CONFIG_TOSHIBA_BT_RFKILL)	+= toshiba_bluetooth.o
 obj-$(CONFIG_TOSHIBA_HAPS)	+= toshiba_haps.o
 obj-$(CONFIG_TOSHIBA_WMI)	+= toshiba-wmi.o
+obj-$(CONFIG_INTEL_HID_EVENT)	+= intel-hid.o
 obj-$(CONFIG_INTEL_SCU_IPC)	+= intel_scu_ipc.o
 obj-$(CONFIG_INTEL_SCU_IPC_UTIL) += intel_scu_ipcutil.o
 obj-$(CONFIG_INTEL_MFLD_THERMAL) += intel_mid_thermal.o
@@ -62,3 +64,7 @@ obj-$(CONFIG_PVPANIC)           += pvpanic.o
 obj-$(CONFIG_ALIENWARE_WMI)	+= alienware-wmi.o
 obj-$(CONFIG_INTEL_PMC_IPC)	+= intel_pmc_ipc.o
 obj-$(CONFIG_SURFACE_PRO3_BUTTON)	+= surfacepro3_button.o
+obj-$(CONFIG_INTEL_PUNIT_IPC)  += intel_punit_ipc.o
+obj-$(CONFIG_INTEL_TELEMETRY)	+= intel_telemetry_core.o \
+				   intel_telemetry_pltdrv.o \
+				   intel_telemetry_debugfs.o
diff --git a/drivers/platform/x86/apple-gmux.c b/drivers/platform/x86/apple-gmux.c
index 976efeb3f2ba..f236250ac106 100644
--- a/drivers/platform/x86/apple-gmux.c
+++ b/drivers/platform/x86/apple-gmux.c
@@ -3,6 +3,7 @@
  *
  *  Copyright (C) Canonical Ltd. <seth.forshee@canonical.com>
  *  Copyright (C) 2010-2012 Andreas Heider <andreas@meetr.de>
+ *  Copyright (C) 2015 Lukas Wunner <lukas@wunner.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
@@ -26,6 +27,24 @@
 #include <acpi/video.h>
 #include <asm/io.h>
 
+/**
+ * DOC: Overview
+ *
+ * :1:  http://www.latticesemi.com/en/Products/FPGAandCPLD/LatticeXP2.aspx
+ * :2:  http://www.renesas.com/products/mpumcu/h8s/h8s2100/h8s2113/index.jsp
+ *
+ * gmux is a microcontroller built into the MacBook Pro to support dual GPUs:
+ * A {1}[Lattice XP2] on pre-retinas, a {2}[Renesas R4F2113] on retinas.
+ *
+ * (The MacPro6,1 2013 also has a gmux, however it is unclear why since it has
+ * dual GPUs but no built-in display.)
+ *
+ * gmux is connected to the LPC bus of the southbridge. Its I/O ports are
+ * accessed differently depending on the microcontroller: Driver functions
+ * to access a pre-retina gmux are infixed `_pio_`, those for a retina gmux
+ * are infixed `_index_`.
+ */
+
 struct apple_gmux_data {
 	unsigned long iostart;
 	unsigned long iolen;
@@ -247,6 +266,20 @@ static bool gmux_is_indexed(struct apple_gmux_data *gmux_data)
 	return false;
 }
 
+/**
+ * DOC: Backlight control
+ *
+ * :3:  http://www.ti.com/lit/ds/symlink/lp8543.pdf
+ * :4:  http://www.ti.com/lit/ds/symlink/lp8545.pdf
+ *
+ * On single GPU MacBooks, the PWM signal for the backlight is generated by
+ * the GPU. On dual GPU MacBook Pros by contrast, either GPU may be suspended
+ * to conserve energy. Hence the PWM signal needs to be generated by a separate
+ * backlight driver which is controlled by gmux. The earliest generation
+ * MBP5 2008/09 uses a {3}[TI LP8543] backlight driver. All newer models
+ * use a {4}[TI LP8545].
+ */
+
 static int gmux_get_brightness(struct backlight_device *bd)
 {
 	struct apple_gmux_data *gmux_data = bl_get_data(bd);
@@ -273,6 +306,68 @@ static const struct backlight_ops gmux_bl_ops = {
 	.update_status = gmux_update_status,
 };
 
+/**
+ * DOC: Graphics mux
+ *
+ * :5:  http://pimg-fpiw.uspto.gov/fdd/07/870/086/0.pdf
+ * :6:  http://www.nxp.com/documents/data_sheet/CBTL06141.pdf
+ * :7:  http://www.ti.com/lit/ds/symlink/hd3ss212.pdf
+ * :8:  https://www.pericom.com/assets/Datasheets/PI3VDP12412.pdf
+ * :9:  http://www.ti.com/lit/ds/symlink/sn74lv4066a.pdf
+ * :10: http://pdf.datasheetarchive.com/indexerfiles/Datasheets-SW16/DSASW00308511.pdf
+ * :11: http://www.ti.com/lit/ds/symlink/ts3ds10224.pdf
+ *
+ * On pre-retinas, the LVDS outputs of both GPUs feed into gmux which muxes
+ * either of them to the panel. One of the tricks gmux has up its sleeve is
+ * to lengthen the blanking interval of its output during a switch to
+ * synchronize it with the GPU switched to. This allows for a flicker-free
+ * switch that is imperceptible by the user ({5}[US 8,687,007 B2]).
+ *
+ * On retinas, muxing is no longer done by gmux itself, but by a separate
+ * chip which is controlled by gmux. The chip is triple sourced, it is
+ * either an {6}[NXP CBTL06142], {7}[TI HD3SS212] or {8}[Pericom PI3VDP12412].
+ * The panel is driven with eDP instead of LVDS since the pixel clock
+ * required for retina resolution exceeds LVDS' limits.
+ *
+ * Pre-retinas are able to switch the panel's DDC pins separately.
+ * This is handled by a {9}[TI SN74LV4066A] which is controlled by gmux.
+ * The inactive GPU can thus probe the panel's EDID without switching over
+ * the entire panel. Retinas lack this functionality as the chips used for
+ * eDP muxing are incapable of switching the AUX channel separately (see
+ * the linked data sheets, Pericom would be capable but this is unused).
+ * However the retina panel has the NO_AUX_HANDSHAKE_LINK_TRAINING bit set
+ * in its DPCD, allowing the inactive GPU to skip the AUX handshake and
+ * set up the output with link parameters pre-calibrated by the active GPU.
+ *
+ * The external DP port is only fully switchable on the first two unibody
+ * MacBook Pro generations, MBP5 2008/09 and MBP6 2010. This is done by an
+ * {6}[NXP CBTL06141] which is controlled by gmux. It's the predecessor of the
+ * eDP mux on retinas, the difference being support for 2.7 versus 5.4 Gbit/s.
+ *
+ * The following MacBook Pro generations replaced the external DP port with a
+ * combined DP/Thunderbolt port and lost the ability to switch it between GPUs,
+ * connecting it either to the discrete GPU or the Thunderbolt controller.
+ * Oddly enough, while the full port is no longer switchable, AUX and HPD
+ * are still switchable by way of an {10}[NXP CBTL03062] (on pre-retinas
+ * MBP8 2011 and MBP9 2012) or two {11}[TI TS3DS10224] (on retinas) under the
+ * control of gmux. Since the integrated GPU is missing the main link,
+ * external displays appear to it as phantoms which fail to link-train.
+ *
+ * gmux receives the HPD signal of all display connectors and sends an
+ * interrupt on hotplug. On generations which cannot switch external ports,
+ * the discrete GPU can then be woken to drive the newly connected display.
+ * The ability to switch AUX on these generations could be used to improve
+ * reliability of hotplug detection by having the integrated GPU poll the
+ * ports while the discrete GPU is asleep, but currently we do not make use
+ * of this feature.
+ *
+ * gmux' initial switch state on bootup is user configurable via the EFI
+ * variable `gpu-power-prefs-fa4ce28d-b62f-4c99-9cc3-6815686e30f9` (5th byte,
+ * 1 = IGD, 0 = DIS). Based on this setting, the EFI firmware tells gmux to
+ * switch the panel and the external DP connector and allocates a framebuffer
+ * for the selected GPU.
+ */
+
 static int gmux_switchto(enum vga_switcheroo_client_id id)
 {
 	if (id == VGA_SWITCHEROO_IGD) {
@@ -288,6 +383,14 @@ static int gmux_switchto(enum vga_switcheroo_client_id id)
 	return 0;
 }
 
+/**
+ * DOC: Power control
+ *
+ * gmux is able to cut power to the discrete GPU. It automatically takes care
+ * of the correct sequence to tear down and bring up the power rails for
+ * core voltage, VRAM and PCIe.
+ */
+
 static int gmux_set_discrete_state(struct apple_gmux_data *gmux_data,
 				   enum vga_switcheroo_state state)
 {
@@ -352,6 +455,16 @@ static const struct vga_switcheroo_handler gmux_handler = {
 	.get_client_id = gmux_get_client_id,
 };
 
+/**
+ * DOC: Interrupt
+ *
+ * gmux is also connected to a GPIO pin of the southbridge and thereby is able
+ * to trigger an ACPI GPE. On the MBP5 2008/09 it's GPIO pin 22 of the Nvidia
+ * MCP79, on all following generations it's GPIO pin 6 of the Intel PCH.
+ * The GPE merely signals that an interrupt occurred, the actual type of event
+ * is identified by reading a gmux register.
+ */
+
 static inline void gmux_disable_interrupts(struct apple_gmux_data *gmux_data)
 {
 	gmux_write8(gmux_data, GMUX_PORT_INTERRUPT_ENABLE,
@@ -588,18 +701,20 @@ static int gmux_probe(struct pnp_dev *pnp, const struct pnp_device_id *id)
 		gmux_data->gpe = -1;
 	}
 
+	apple_gmux_data = gmux_data;
+	init_completion(&gmux_data->powerchange_done);
+	gmux_enable_interrupts(gmux_data);
+
 	if (vga_switcheroo_register_handler(&gmux_handler)) {
 		ret = -ENODEV;
 		goto err_register_handler;
 	}
 
-	init_completion(&gmux_data->powerchange_done);
-	apple_gmux_data = gmux_data;
-	gmux_enable_interrupts(gmux_data);
-
 	return 0;
 
 err_register_handler:
+	gmux_disable_interrupts(gmux_data);
+	apple_gmux_data = NULL;
 	if (gmux_data->gpe >= 0)
 		acpi_disable_gpe(NULL, gmux_data->gpe);
 err_enable_gpe:
diff --git a/drivers/platform/x86/asus-wireless.c b/drivers/platform/x86/asus-wireless.c
new file mode 100644
index 000000000000..9ec721e26532
--- /dev/null
+++ b/drivers/platform/x86/asus-wireless.c
@@ -0,0 +1,84 @@
+/*
+ * Asus Wireless Radio Control Driver
+ *
+ * Copyright (C) 2015-2016 Endless Mobile, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/acpi.h>
+#include <linux/input.h>
+#include <linux/pci_ids.h>
+
+struct asus_wireless_data {
+	struct input_dev *idev;
+};
+
+static void asus_wireless_notify(struct acpi_device *adev, u32 event)
+{
+	struct asus_wireless_data *data = acpi_driver_data(adev);
+
+	dev_dbg(&adev->dev, "event=%#x\n", event);
+	if (event != 0x88) {
+		dev_notice(&adev->dev, "Unknown ASHS event: %#x\n", event);
+		return;
+	}
+	input_report_key(data->idev, KEY_RFKILL, 1);
+	input_report_key(data->idev, KEY_RFKILL, 0);
+	input_sync(data->idev);
+}
+
+static int asus_wireless_add(struct acpi_device *adev)
+{
+	struct asus_wireless_data *data;
+
+	data = devm_kzalloc(&adev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+	adev->driver_data = data;
+
+	data->idev = devm_input_allocate_device(&adev->dev);
+	if (!data->idev)
+		return -ENOMEM;
+	data->idev->name = "Asus Wireless Radio Control";
+	data->idev->phys = "asus-wireless/input0";
+	data->idev->id.bustype = BUS_HOST;
+	data->idev->id.vendor = PCI_VENDOR_ID_ASUSTEK;
+	set_bit(EV_KEY, data->idev->evbit);
+	set_bit(KEY_RFKILL, data->idev->keybit);
+	return input_register_device(data->idev);
+}
+
+static int asus_wireless_remove(struct acpi_device *adev)
+{
+	return 0;
+}
+
+static const struct acpi_device_id device_ids[] = {
+	{"ATK4001", 0},
+	{"ATK4002", 0},
+	{"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, device_ids);
+
+static struct acpi_driver asus_wireless_driver = {
+	.name = "Asus Wireless Radio Control Driver",
+	.class = "hotkey",
+	.ids = device_ids,
+	.ops = {
+		.add = asus_wireless_add,
+		.remove = asus_wireless_remove,
+		.notify = asus_wireless_notify,
+	},
+};
+module_acpi_driver(asus_wireless_driver);
+
+MODULE_DESCRIPTION("Asus Wireless Radio Control Driver");
+MODULE_AUTHOR("João Paulo Rechi Vita <jprvita@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/asus-wmi.c b/drivers/platform/x86/asus-wmi.c
index f96f7b865267..a96630d52346 100644
--- a/drivers/platform/x86/asus-wmi.c
+++ b/drivers/platform/x86/asus-wmi.c
@@ -56,9 +56,6 @@ MODULE_AUTHOR("Corentin Chary <corentin.chary@gmail.com>, "
 MODULE_DESCRIPTION("Asus Generic WMI Driver");
 MODULE_LICENSE("GPL");
 
-#define to_platform_driver(drv)					\
-	(container_of((drv), struct platform_driver, driver))
-
 #define to_asus_wmi_driver(pdrv)					\
 	(container_of((pdrv), struct asus_wmi_driver, platform_driver))
 
diff --git a/drivers/platform/x86/dell-wmi.c b/drivers/platform/x86/dell-wmi.c
index f2d77fe696ac..368e193c2741 100644
--- a/drivers/platform/x86/dell-wmi.c
+++ b/drivers/platform/x86/dell-wmi.c
@@ -2,6 +2,7 @@
  * Dell WMI hotkeys
  *
  * Copyright (C) 2008 Red Hat <mjg@redhat.com>
+ * Copyright (C) 2014-2015 Pali Rohár <pali.rohar@gmail.com>
  *
  * Portions based on wistron_btns.c:
  * Copyright (C) 2005 Miloslav Trmac <mitr@volny.cz>
@@ -38,14 +39,17 @@
 #include <acpi/video.h>
 
 MODULE_AUTHOR("Matthew Garrett <mjg@redhat.com>");
+MODULE_AUTHOR("Pali Rohár <pali.rohar@gmail.com>");
 MODULE_DESCRIPTION("Dell laptop WMI hotkeys driver");
 MODULE_LICENSE("GPL");
 
 #define DELL_EVENT_GUID "9DBB5994-A997-11DA-B012-B622A1EF5492"
+#define DELL_DESCRIPTOR_GUID "8D9DDCBC-A997-11DA-B012-B622A1EF5492"
 
-static int acpi_video;
+static u32 dell_wmi_interface_version;
 
 MODULE_ALIAS("wmi:"DELL_EVENT_GUID);
+MODULE_ALIAS("wmi:"DELL_DESCRIPTOR_GUID);
 
 /*
  * Certain keys are flagged as KE_IGNORE. All of these are either
@@ -118,28 +122,48 @@ struct dell_bios_hotkey_table {
 
 static const struct dell_bios_hotkey_table *dell_bios_hotkey_table;
 
+/* Uninitialized entries here are KEY_RESERVED == 0. */
 static const u16 bios_to_linux_keycode[256] __initconst = {
-
-	KEY_MEDIA,	KEY_NEXTSONG,	KEY_PLAYPAUSE, KEY_PREVIOUSSONG,
-	KEY_STOPCD,	KEY_UNKNOWN,	KEY_UNKNOWN,	KEY_UNKNOWN,
-	KEY_WWW,	KEY_UNKNOWN,	KEY_VOLUMEDOWN, KEY_MUTE,
-	KEY_VOLUMEUP,	KEY_UNKNOWN,	KEY_BATTERY,	KEY_EJECTCD,
-	KEY_UNKNOWN,	KEY_SLEEP,	KEY_PROG1, KEY_BRIGHTNESSDOWN,
-	KEY_BRIGHTNESSUP,	KEY_UNKNOWN,	KEY_KBDILLUMTOGGLE,
-	KEY_UNKNOWN,	KEY_SWITCHVIDEOMODE,	KEY_UNKNOWN, KEY_UNKNOWN,
-	KEY_SWITCHVIDEOMODE,	KEY_UNKNOWN,	KEY_UNKNOWN, KEY_PROG2,
-	KEY_UNKNOWN,	KEY_UNKNOWN,	KEY_UNKNOWN,	KEY_UNKNOWN,
-	KEY_UNKNOWN,	KEY_UNKNOWN,	KEY_UNKNOWN,	KEY_MICMUTE,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-	0, 0, 0, 0, 0, 0, 0, 0, 0, KEY_PROG3
+	[0]	= KEY_MEDIA,
+	[1]	= KEY_NEXTSONG,
+	[2]	= KEY_PLAYPAUSE,
+	[3]	= KEY_PREVIOUSSONG,
+	[4]	= KEY_STOPCD,
+	[5]	= KEY_UNKNOWN,
+	[6]	= KEY_UNKNOWN,
+	[7]	= KEY_UNKNOWN,
+	[8]	= KEY_WWW,
+	[9]	= KEY_UNKNOWN,
+	[10]	= KEY_VOLUMEDOWN,
+	[11]	= KEY_MUTE,
+	[12]	= KEY_VOLUMEUP,
+	[13]	= KEY_UNKNOWN,
+	[14]	= KEY_BATTERY,
+	[15]	= KEY_EJECTCD,
+	[16]	= KEY_UNKNOWN,
+	[17]	= KEY_SLEEP,
+	[18]	= KEY_PROG1,
+	[19]	= KEY_BRIGHTNESSDOWN,
+	[20]	= KEY_BRIGHTNESSUP,
+	[21]	= KEY_UNKNOWN,
+	[22]	= KEY_KBDILLUMTOGGLE,
+	[23]	= KEY_UNKNOWN,
+	[24]	= KEY_SWITCHVIDEOMODE,
+	[25]	= KEY_UNKNOWN,
+	[26]	= KEY_UNKNOWN,
+	[27]	= KEY_SWITCHVIDEOMODE,
+	[28]	= KEY_UNKNOWN,
+	[29]	= KEY_UNKNOWN,
+	[30]	= KEY_PROG2,
+	[31]	= KEY_UNKNOWN,
+	[32]	= KEY_UNKNOWN,
+	[33]	= KEY_UNKNOWN,
+	[34]	= KEY_UNKNOWN,
+	[35]	= KEY_UNKNOWN,
+	[36]	= KEY_UNKNOWN,
+	[37]	= KEY_UNKNOWN,
+	[38]	= KEY_MICMUTE,
+	[255]	= KEY_PROG3,
 };
 
 static struct input_dev *dell_wmi_input_dev;
@@ -151,7 +175,8 @@ static void dell_wmi_process_key(int reported_key)
 	key = sparse_keymap_entry_from_scancode(dell_wmi_input_dev,
 						reported_key);
 	if (!key) {
-		pr_info("Unknown key %x pressed\n", reported_key);
+		pr_info("Unknown key with scancode 0x%x pressed\n",
+			reported_key);
 		return;
 	}
 
@@ -159,7 +184,8 @@ static void dell_wmi_process_key(int reported_key)
 
 	/* Don't report brightness notifications that will also come via ACPI */
 	if ((key->keycode == KEY_BRIGHTNESSUP ||
-	     key->keycode == KEY_BRIGHTNESSDOWN) && acpi_video)
+	     key->keycode == KEY_BRIGHTNESSDOWN) &&
+	    acpi_video_handles_brightness_key_presses())
 		return;
 
 	sparse_keymap_report_entry(dell_wmi_input_dev, key, 1, true);
@@ -211,6 +237,22 @@ static void dell_wmi_notify(u32 value, void *context)
 
 	buffer_end = buffer_entry + buffer_size;
 
+	/*
+	 * BIOS/ACPI on devices with WMI interface version 0 does not clear
+	 * buffer before filling it. So next time when BIOS/ACPI send WMI event
+	 * which is smaller as previous then it contains garbage in buffer from
+	 * previous event.
+	 *
+	 * BIOS/ACPI on devices with WMI interface version 1 clears buffer and
+	 * sometimes send more events in buffer at one call.
+	 *
+	 * So to prevent reading garbage from buffer we will process only first
+	 * one event on devices with WMI interface version 0.
+	 */
+	if (dell_wmi_interface_version == 0 && buffer_entry < buffer_end)
+		if (buffer_end > buffer_entry + buffer_entry[0] + 1)
+			buffer_end = buffer_entry + buffer_entry[0] + 1;
+
 	while (buffer_entry < buffer_end) {
 
 		len = buffer_entry[0];
@@ -309,9 +351,23 @@ static const struct key_entry * __init dell_wmi_prepare_new_keymap(void)
 	for (i = 0; i < hotkey_num; i++) {
 		const struct dell_bios_keymap_entry *bios_entry =
 					&dell_bios_hotkey_table->keymap[i];
-		u16 keycode = bios_entry->keycode < 256 ?
-				    bios_to_linux_keycode[bios_entry->keycode] :
-				    KEY_RESERVED;
+
+		/* Uninitialized entries are 0 aka KEY_RESERVED. */
+		u16 keycode = (bios_entry->keycode <
+			       ARRAY_SIZE(bios_to_linux_keycode)) ?
+			bios_to_linux_keycode[bios_entry->keycode] :
+			KEY_RESERVED;
+
+		/*
+		 * Log if we find an entry in the DMI table that we don't
+		 * understand.  If this happens, we should figure out what
+		 * the entry means and add it to bios_to_linux_keycode.
+		 */
+		if (keycode == KEY_RESERVED) {
+			pr_info("firmware scancode 0x%x maps to unrecognized keycode 0x%x\n",
+				bios_entry->scancode, bios_entry->keycode);
+			continue;
+		}
 
 		if (keycode == KEY_KBDILLUMTOGGLE)
 			keymap[i].type = KE_IGNORE;
@@ -387,18 +443,88 @@ static void __init find_hk_type(const struct dmi_header *dm, void *dummy)
 	}
 }
 
+/*
+ * Descriptor buffer is 128 byte long and contains:
+ *
+ *       Name             Offset  Length  Value
+ * Vendor Signature          0       4    "DELL"
+ * Object Signature          4       4    " WMI"
+ * WMI Interface Version     8       4    <version>
+ * WMI buffer length        12       4    4096
+ */
+static int __init dell_wmi_check_descriptor_buffer(void)
+{
+	struct acpi_buffer out = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *obj;
+	acpi_status status;
+	u32 *buffer;
+
+	status = wmi_query_block(DELL_DESCRIPTOR_GUID, 0, &out);
+	if (ACPI_FAILURE(status)) {
+		pr_err("Cannot read Dell descriptor buffer - %d\n", status);
+		return status;
+	}
+
+	obj = (union acpi_object *)out.pointer;
+	if (!obj) {
+		pr_err("Dell descriptor buffer is empty\n");
+		return -EINVAL;
+	}
+
+	if (obj->type != ACPI_TYPE_BUFFER) {
+		pr_err("Cannot read Dell descriptor buffer\n");
+		kfree(obj);
+		return -EINVAL;
+	}
+
+	if (obj->buffer.length != 128) {
+		pr_err("Dell descriptor buffer has invalid length (%d)\n",
+			obj->buffer.length);
+		if (obj->buffer.length < 16) {
+			kfree(obj);
+			return -EINVAL;
+		}
+	}
+
+	buffer = (u32 *)obj->buffer.pointer;
+
+	if (buffer[0] != 0x4C4C4544 && buffer[1] != 0x494D5720)
+		pr_warn("Dell descriptor buffer has invalid signature (%*ph)\n",
+			8, buffer);
+
+	if (buffer[2] != 0 && buffer[2] != 1)
+		pr_warn("Dell descriptor buffer has unknown version (%d)\n",
+			buffer[2]);
+
+	if (buffer[3] != 4096)
+		pr_warn("Dell descriptor buffer has invalid buffer length (%d)\n",
+			buffer[3]);
+
+	dell_wmi_interface_version = buffer[2];
+
+	pr_info("Detected Dell WMI interface version %u\n",
+		dell_wmi_interface_version);
+
+	kfree(obj);
+	return 0;
+}
+
 static int __init dell_wmi_init(void)
 {
 	int err;
 	acpi_status status;
 
-	if (!wmi_has_guid(DELL_EVENT_GUID)) {
-		pr_warn("No known WMI GUID found\n");
+	if (!wmi_has_guid(DELL_EVENT_GUID) ||
+	    !wmi_has_guid(DELL_DESCRIPTOR_GUID)) {
+		pr_warn("Dell WMI GUID were not found\n");
 		return -ENODEV;
 	}
 
+	err = dell_wmi_check_descriptor_buffer();
+	if (err)
+		return err;
+
 	dmi_walk(find_hk_type, NULL);
-	acpi_video = acpi_video_get_backlight_type() != acpi_backlight_vendor;
 
 	err = dell_wmi_input_setup();
 	if (err)
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index a313dfc0245f..d78ee151c9e4 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -865,6 +865,13 @@ static const struct dmi_system_id no_hw_rfkill_list[] = {
 		},
 	},
 	{
+		.ident = "Lenovo ideapad Y700-17ISK",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo ideapad Y700-17ISK"),
+		},
+	},
+	{
 		.ident = "Lenovo Yoga 2 11 / 13 / Pro",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
@@ -893,6 +900,13 @@ static const struct dmi_system_id no_hw_rfkill_list[] = {
 		},
 	},
 	{
+		.ident = "Lenovo Yoga 700",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo YOGA 700"),
+		},
+	},
+	{
 		.ident = "Lenovo Yoga 900",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
diff --git a/drivers/platform/x86/intel-hid.c b/drivers/platform/x86/intel-hid.c
new file mode 100644
index 000000000000..e20f23e04c24
--- /dev/null
+++ b/drivers/platform/x86/intel-hid.c
@@ -0,0 +1,288 @@
+/*
+ *  Intel HID event driver for Windows 8
+ *
+ *  Copyright (C) 2015 Alex Hung <alex.hung@canonical.com>
+ *  Copyright (C) 2015 Andrew Lutomirski <luto@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
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/platform_device.h>
+#include <linux/input/sparse-keymap.h>
+#include <linux/acpi.h>
+#include <acpi/acpi_bus.h>
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Alex Hung");
+
+static const struct acpi_device_id intel_hid_ids[] = {
+	{"INT33D5", 0},
+	{"", 0},
+};
+
+/* In theory, these are HID usages. */
+static const struct key_entry intel_hid_keymap[] = {
+	/* 1: LSuper (Page 0x07, usage 0xE3) -- unclear what to do */
+	/* 2: Toggle SW_ROTATE_LOCK -- easy to implement if seen in wild */
+	{ KE_KEY, 3, { KEY_NUMLOCK } },
+	{ KE_KEY, 4, { KEY_HOME } },
+	{ KE_KEY, 5, { KEY_END } },
+	{ KE_KEY, 6, { KEY_PAGEUP } },
+	{ KE_KEY, 7, { KEY_PAGEDOWN } },
+	{ KE_KEY, 8, { KEY_RFKILL } },
+	{ KE_KEY, 9, { KEY_POWER } },
+	{ KE_KEY, 11, { KEY_SLEEP } },
+	/* 13 has two different meanings in the spec -- ignore it. */
+	{ KE_KEY, 14, { KEY_STOPCD } },
+	{ KE_KEY, 15, { KEY_PLAYPAUSE } },
+	{ KE_KEY, 16, { KEY_MUTE } },
+	{ KE_KEY, 17, { KEY_VOLUMEUP } },
+	{ KE_KEY, 18, { KEY_VOLUMEDOWN } },
+	{ KE_KEY, 19, { KEY_BRIGHTNESSUP } },
+	{ KE_KEY, 20, { KEY_BRIGHTNESSDOWN } },
+	/* 27: wake -- needs special handling */
+	{ KE_END },
+};
+
+struct intel_hid_priv {
+	struct input_dev *input_dev;
+};
+
+static int intel_hid_set_enable(struct device *device, int enable)
+{
+	union acpi_object arg0 = { ACPI_TYPE_INTEGER };
+	struct acpi_object_list args = { 1, &arg0 };
+	acpi_status status;
+
+	arg0.integer.value = enable;
+	status = acpi_evaluate_object(ACPI_HANDLE(device), "HDSM", &args, NULL);
+	if (!ACPI_SUCCESS(status)) {
+		dev_warn(device, "failed to %sable hotkeys\n",
+			 enable ? "en" : "dis");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int intel_hid_pl_suspend_handler(struct device *device)
+{
+	intel_hid_set_enable(device, 0);
+	return 0;
+}
+
+static int intel_hid_pl_resume_handler(struct device *device)
+{
+	intel_hid_set_enable(device, 1);
+	return 0;
+}
+
+static const struct dev_pm_ops intel_hid_pl_pm_ops = {
+	.suspend  = intel_hid_pl_suspend_handler,
+	.resume  = intel_hid_pl_resume_handler,
+};
+
+static int intel_hid_input_setup(struct platform_device *device)
+{
+	struct intel_hid_priv *priv = dev_get_drvdata(&device->dev);
+	int ret;
+
+	priv->input_dev = input_allocate_device();
+	if (!priv->input_dev)
+		return -ENOMEM;
+
+	ret = sparse_keymap_setup(priv->input_dev, intel_hid_keymap, NULL);
+	if (ret)
+		goto err_free_device;
+
+	priv->input_dev->dev.parent = &device->dev;
+	priv->input_dev->name = "Intel HID events";
+	priv->input_dev->id.bustype = BUS_HOST;
+	set_bit(KEY_RFKILL, priv->input_dev->keybit);
+
+	ret = input_register_device(priv->input_dev);
+	if (ret)
+		goto err_free_device;
+
+	return 0;
+
+err_free_device:
+		input_free_device(priv->input_dev);
+		return ret;
+}
+
+static void intel_hid_input_destroy(struct platform_device *device)
+{
+	struct intel_hid_priv *priv = dev_get_drvdata(&device->dev);
+
+	input_unregister_device(priv->input_dev);
+}
+
+static void notify_handler(acpi_handle handle, u32 event, void *context)
+{
+	struct platform_device *device = context;
+	struct intel_hid_priv *priv = dev_get_drvdata(&device->dev);
+	unsigned long long ev_index;
+	acpi_status status;
+
+	/* The platform spec only defines one event code: 0xC0. */
+	if (event != 0xc0) {
+		dev_warn(&device->dev, "received unknown event (0x%x)\n",
+			 event);
+		return;
+	}
+
+	status = acpi_evaluate_integer(handle, "HDEM", NULL, &ev_index);
+	if (!ACPI_SUCCESS(status)) {
+		dev_warn(&device->dev, "failed to get event index\n");
+		return;
+	}
+
+	if (!sparse_keymap_report_event(priv->input_dev, ev_index, 1, true))
+		dev_info(&device->dev, "unknown event index 0x%llx\n",
+			 ev_index);
+}
+
+static int intel_hid_probe(struct platform_device *device)
+{
+	acpi_handle handle = ACPI_HANDLE(&device->dev);
+	struct intel_hid_priv *priv;
+	unsigned long long mode;
+	acpi_status status;
+	int err;
+
+	status = acpi_evaluate_integer(handle, "HDMM", NULL, &mode);
+	if (!ACPI_SUCCESS(status)) {
+		dev_warn(&device->dev, "failed to read mode\n");
+		return -ENODEV;
+	}
+
+	if (mode != 0) {
+		/*
+		 * This driver only implements "simple" mode.  There appear
+		 * to be no other modes, but we should be paranoid and check
+		 * for compatibility.
+		 */
+		dev_info(&device->dev, "platform is not in simple mode\n");
+		return -ENODEV;
+	}
+
+	priv = devm_kzalloc(&device->dev,
+			    sizeof(struct intel_hid_priv *), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+	dev_set_drvdata(&device->dev, priv);
+
+	err = intel_hid_input_setup(device);
+	if (err) {
+		pr_err("Failed to setup Intel HID hotkeys\n");
+		return err;
+	}
+
+	status = acpi_install_notify_handler(handle,
+					     ACPI_DEVICE_NOTIFY,
+					     notify_handler,
+					     device);
+	if (ACPI_FAILURE(status)) {
+		err = -EBUSY;
+		goto err_remove_input;
+	}
+
+	err = intel_hid_set_enable(&device->dev, 1);
+	if (err)
+		goto err_remove_notify;
+
+	return 0;
+
+err_remove_notify:
+	acpi_remove_notify_handler(handle, ACPI_DEVICE_NOTIFY, notify_handler);
+
+err_remove_input:
+	intel_hid_input_destroy(device);
+
+	return err;
+}
+
+static int intel_hid_remove(struct platform_device *device)
+{
+	acpi_handle handle = ACPI_HANDLE(&device->dev);
+
+	acpi_remove_notify_handler(handle, ACPI_DEVICE_NOTIFY, notify_handler);
+	intel_hid_input_destroy(device);
+	intel_hid_set_enable(&device->dev, 0);
+	acpi_remove_notify_handler(handle, ACPI_DEVICE_NOTIFY, notify_handler);
+
+	/*
+	 * Even if we failed to shut off the event stream, we can still
+	 * safely detach from the device.
+	 */
+	return 0;
+}
+
+static struct platform_driver intel_hid_pl_driver = {
+	.driver = {
+		.name = "intel-hid",
+		.acpi_match_table = intel_hid_ids,
+		.pm = &intel_hid_pl_pm_ops,
+	},
+	.probe = intel_hid_probe,
+	.remove = intel_hid_remove,
+};
+MODULE_DEVICE_TABLE(acpi, intel_hid_ids);
+
+/*
+ * Unfortunately, some laptops provide a _HID="INT33D5" device with
+ * _CID="PNP0C02".  This causes the pnpacpi scan driver to claim the
+ * ACPI node, so no platform device will be created.  The pnpacpi
+ * driver rejects this device in subsequent processing, so no physical
+ * node is created at all.
+ *
+ * As a workaround until the ACPI core figures out how to handle
+ * this corner case, manually ask the ACPI platform device code to
+ * claim the ACPI node.
+ */
+static acpi_status __init
+check_acpi_dev(acpi_handle handle, u32 lvl, void *context, void **rv)
+{
+	const struct acpi_device_id *ids = context;
+	struct acpi_device *dev;
+
+	if (acpi_bus_get_device(handle, &dev) != 0)
+		return AE_OK;
+
+	if (acpi_match_device_ids(dev, ids) == 0)
+		if (acpi_create_platform_device(dev))
+			dev_info(&dev->dev,
+				 "intel-hid: created platform device\n");
+
+	return AE_OK;
+}
+
+static int __init intel_hid_init(void)
+{
+	acpi_walk_namespace(ACPI_TYPE_DEVICE, ACPI_ROOT_OBJECT,
+			    ACPI_UINT32_MAX, check_acpi_dev, NULL,
+			    (void *)intel_hid_ids, NULL);
+
+	return platform_driver_register(&intel_hid_pl_driver);
+}
+module_init(intel_hid_init);
+
+static void __exit intel_hid_exit(void)
+{
+	platform_driver_unregister(&intel_hid_pl_driver);
+}
+module_exit(intel_hid_exit);
diff --git a/drivers/platform/x86/intel_pmc_ipc.c b/drivers/platform/x86/intel_pmc_ipc.c
index 28b2a12bb26d..092519e37de6 100644
--- a/drivers/platform/x86/intel_pmc_ipc.c
+++ b/drivers/platform/x86/intel_pmc_ipc.c
@@ -68,8 +68,13 @@
 #define PLAT_RESOURCE_IPC_INDEX		0
 #define PLAT_RESOURCE_IPC_SIZE		0x1000
 #define PLAT_RESOURCE_GCR_SIZE		0x1000
-#define PLAT_RESOURCE_PUNIT_DATA_INDEX	1
-#define PLAT_RESOURCE_PUNIT_INTER_INDEX	2
+#define PLAT_RESOURCE_BIOS_DATA_INDEX	1
+#define PLAT_RESOURCE_BIOS_IFACE_INDEX	2
+#define PLAT_RESOURCE_TELEM_SSRAM_INDEX	3
+#define PLAT_RESOURCE_ISP_DATA_INDEX	4
+#define PLAT_RESOURCE_ISP_IFACE_INDEX	5
+#define PLAT_RESOURCE_GTD_DATA_INDEX	6
+#define PLAT_RESOURCE_GTD_IFACE_INDEX	7
 #define PLAT_RESOURCE_ACPI_IO_INDEX	0
 
 /*
@@ -84,6 +89,10 @@
 #define TCO_BASE_OFFSET			0x60
 #define TCO_REGS_SIZE			16
 #define PUNIT_DEVICE_NAME		"intel_punit_ipc"
+#define TELEMETRY_DEVICE_NAME		"intel_telemetry"
+#define TELEM_SSRAM_SIZE		240
+#define TELEM_PMC_SSRAM_OFFSET		0x1B00
+#define TELEM_PUNIT_SSRAM_OFFSET	0x1A00
 
 static const int iTCO_version = 3;
 
@@ -105,11 +114,15 @@ static struct intel_pmc_ipc_dev {
 	int gcr_size;
 
 	/* punit */
-	resource_size_t punit_base;
-	int punit_size;
-	resource_size_t punit_base2;
-	int punit_size2;
 	struct platform_device *punit_dev;
+
+	/* Telemetry */
+	resource_size_t telem_pmc_ssram_base;
+	resource_size_t telem_punit_ssram_base;
+	int telem_pmc_ssram_size;
+	int telem_punit_ssram_size;
+	u8 telem_res_inval;
+	struct platform_device *telemetry_dev;
 } ipcdev;
 
 static char *ipc_err_sources[] = {
@@ -444,9 +457,22 @@ static const struct attribute_group intel_ipc_group = {
 	.attrs = intel_ipc_attrs,
 };
 
-#define PUNIT_RESOURCE_INTER		1
-static struct resource punit_res[] = {
-	/* Punit */
+static struct resource punit_res_array[] = {
+	/* Punit BIOS */
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	/* Punit ISP */
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	/* Punit GTD */
 	{
 		.flags = IORESOURCE_MEM,
 	},
@@ -478,10 +504,21 @@ static struct itco_wdt_platform_data tco_info = {
 	.version = 3,
 };
 
+#define TELEMETRY_RESOURCE_PUNIT_SSRAM	0
+#define TELEMETRY_RESOURCE_PMC_SSRAM	1
+static struct resource telemetry_res[] = {
+	/*Telemetry*/
+	{
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.flags = IORESOURCE_MEM,
+	},
+};
+
 static int ipc_create_punit_device(void)
 {
 	struct platform_device *pdev;
-	struct resource *res;
 	int ret;
 
 	pdev = platform_device_alloc(PUNIT_DEVICE_NAME, -1);
@@ -491,17 +528,8 @@ static int ipc_create_punit_device(void)
 	}
 
 	pdev->dev.parent = ipcdev.dev;
-
-	res = punit_res;
-	res->start = ipcdev.punit_base;
-	res->end = res->start + ipcdev.punit_size - 1;
-
-	res = punit_res + PUNIT_RESOURCE_INTER;
-	res->start = ipcdev.punit_base2;
-	res->end = res->start + ipcdev.punit_size2 - 1;
-
-	ret = platform_device_add_resources(pdev, punit_res,
-					    ARRAY_SIZE(punit_res));
+	ret = platform_device_add_resources(pdev, punit_res_array,
+					    ARRAY_SIZE(punit_res_array));
 	if (ret) {
 		dev_err(ipcdev.dev, "Failed to add platform punit resources\n");
 		goto err;
@@ -571,6 +599,51 @@ err:
 	return ret;
 }
 
+static int ipc_create_telemetry_device(void)
+{
+	struct platform_device *pdev;
+	struct resource *res;
+	int ret;
+
+	pdev = platform_device_alloc(TELEMETRY_DEVICE_NAME, -1);
+	if (!pdev) {
+		dev_err(ipcdev.dev,
+			"Failed to allocate telemetry platform device\n");
+		return -ENOMEM;
+	}
+
+	pdev->dev.parent = ipcdev.dev;
+
+	res = telemetry_res + TELEMETRY_RESOURCE_PUNIT_SSRAM;
+	res->start = ipcdev.telem_punit_ssram_base;
+	res->end = res->start + ipcdev.telem_punit_ssram_size - 1;
+
+	res = telemetry_res + TELEMETRY_RESOURCE_PMC_SSRAM;
+	res->start = ipcdev.telem_pmc_ssram_base;
+	res->end = res->start + ipcdev.telem_pmc_ssram_size - 1;
+
+	ret = platform_device_add_resources(pdev, telemetry_res,
+					    ARRAY_SIZE(telemetry_res));
+	if (ret) {
+		dev_err(ipcdev.dev,
+			"Failed to add telemetry platform resources\n");
+		goto err;
+	}
+
+	ret = platform_device_add(pdev);
+	if (ret) {
+		dev_err(ipcdev.dev,
+			"Failed to add telemetry platform device\n");
+		goto err;
+	}
+	ipcdev.telemetry_dev = pdev;
+
+	return 0;
+err:
+	platform_device_put(pdev);
+	return ret;
+}
+
 static int ipc_create_pmc_devices(void)
 {
 	int ret;
@@ -585,12 +658,20 @@ static int ipc_create_pmc_devices(void)
 		dev_err(ipcdev.dev, "Failed to add punit platform device\n");
 		platform_device_unregister(ipcdev.tco_dev);
 	}
+
+	if (!ipcdev.telem_res_inval) {
+		ret = ipc_create_telemetry_device();
+		if (ret)
+			dev_warn(ipcdev.dev,
+				"Failed to add telemetry platform device\n");
+	}
+
 	return ret;
 }
 
 static int ipc_plat_get_res(struct platform_device *pdev)
 {
-	struct resource *res;
+	struct resource *res, *punit_res;
 	void __iomem *addr;
 	int size;
 
@@ -603,32 +684,68 @@ static int ipc_plat_get_res(struct platform_device *pdev)
 	size = resource_size(res);
 	ipcdev.acpi_io_base = res->start;
 	ipcdev.acpi_io_size = size;
-	dev_info(&pdev->dev, "io res: %llx %x\n",
-		 (long long)res->start, (int)resource_size(res));
+	dev_info(&pdev->dev, "io res: %pR\n", res);
 
+	/* This is index 0 to cover BIOS data register */
+	punit_res = punit_res_array;
 	res = platform_get_resource(pdev, IORESOURCE_MEM,
-				    PLAT_RESOURCE_PUNIT_DATA_INDEX);
+				    PLAT_RESOURCE_BIOS_DATA_INDEX);
 	if (!res) {
-		dev_err(&pdev->dev, "Failed to get punit resource\n");
+		dev_err(&pdev->dev, "Failed to get res of punit BIOS data\n");
 		return -ENXIO;
 	}
-	size = resource_size(res);
-	ipcdev.punit_base = res->start;
-	ipcdev.punit_size = size;
-	dev_info(&pdev->dev, "punit data res: %llx %x\n",
-		 (long long)res->start, (int)resource_size(res));
+	*punit_res = *res;
+	dev_info(&pdev->dev, "punit BIOS data res: %pR\n", res);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM,
-				    PLAT_RESOURCE_PUNIT_INTER_INDEX);
+				    PLAT_RESOURCE_BIOS_IFACE_INDEX);
 	if (!res) {
-		dev_err(&pdev->dev, "Failed to get punit inter resource\n");
+		dev_err(&pdev->dev, "Failed to get res of punit BIOS iface\n");
 		return -ENXIO;
 	}
-	size = resource_size(res);
-	ipcdev.punit_base2 = res->start;
-	ipcdev.punit_size2 = size;
-	dev_info(&pdev->dev, "punit interface res: %llx %x\n",
-		 (long long)res->start, (int)resource_size(res));
+	/* This is index 1 to cover BIOS interface register */
+	*++punit_res = *res;
+	dev_info(&pdev->dev, "punit BIOS interface res: %pR\n", res);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_ISP_DATA_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get res of punit ISP data\n");
+		return -ENXIO;
+	}
+	/* This is index 2 to cover ISP data register */
+	*++punit_res = *res;
+	dev_info(&pdev->dev, "punit ISP data res: %pR\n", res);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_ISP_IFACE_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get res of punit ISP iface\n");
+		return -ENXIO;
+	}
+	/* This is index 3 to cover ISP interface register */
+	*++punit_res = *res;
+	dev_info(&pdev->dev, "punit ISP interface res: %pR\n", res);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_GTD_DATA_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get res of punit GTD data\n");
+		return -ENXIO;
+	}
+	/* This is index 4 to cover GTD data register */
+	*++punit_res = *res;
+	dev_info(&pdev->dev, "punit GTD data res: %pR\n", res);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_GTD_IFACE_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get res of punit GTD iface\n");
+		return -ENXIO;
+	}
+	/* This is index 5 to cover GTD interface register */
+	*++punit_res = *res;
+	dev_info(&pdev->dev, "punit GTD interface res: %pR\n", res);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM,
 				    PLAT_RESOURCE_IPC_INDEX);
@@ -651,8 +768,23 @@ static int ipc_plat_get_res(struct platform_device *pdev)
 
 	ipcdev.gcr_base = res->start + size;
 	ipcdev.gcr_size = PLAT_RESOURCE_GCR_SIZE;
-	dev_info(&pdev->dev, "ipc res: %llx %x\n",
-		 (long long)res->start, (int)resource_size(res));
+	dev_info(&pdev->dev, "ipc res: %pR\n", res);
+
+	ipcdev.telem_res_inval = 0;
+	res = platform_get_resource(pdev, IORESOURCE_MEM,
+				    PLAT_RESOURCE_TELEM_SSRAM_INDEX);
+	if (!res) {
+		dev_err(&pdev->dev, "Failed to get telemetry ssram resource\n");
+		ipcdev.telem_res_inval = 1;
+	} else {
+		ipcdev.telem_punit_ssram_base = res->start +
+						TELEM_PUNIT_SSRAM_OFFSET;
+		ipcdev.telem_punit_ssram_size = TELEM_SSRAM_SIZE;
+		ipcdev.telem_pmc_ssram_base = res->start +
+						TELEM_PMC_SSRAM_OFFSET;
+		ipcdev.telem_pmc_ssram_size = TELEM_SSRAM_SIZE;
+		dev_info(&pdev->dev, "telemetry ssram res: %pR\n", res);
+	}
 
 	return 0;
 }
@@ -711,6 +843,7 @@ err_sys:
 err_irq:
 	platform_device_unregister(ipcdev.tco_dev);
 	platform_device_unregister(ipcdev.punit_dev);
+	platform_device_unregister(ipcdev.telemetry_dev);
 err_device:
 	iounmap(ipcdev.ipc_base);
 	res = platform_get_resource(pdev, IORESOURCE_MEM,
@@ -728,6 +861,7 @@ static int ipc_plat_remove(struct platform_device *pdev)
 	free_irq(ipcdev.irq, &ipcdev);
 	platform_device_unregister(ipcdev.tco_dev);
 	platform_device_unregister(ipcdev.punit_dev);
+	platform_device_unregister(ipcdev.telemetry_dev);
 	iounmap(ipcdev.ipc_base);
 	res = platform_get_resource(pdev, IORESOURCE_MEM,
 				    PLAT_RESOURCE_IPC_INDEX);
diff --git a/drivers/platform/x86/intel_pmic_gpio.c b/drivers/platform/x86/intel_pmic_gpio.c
index 709f0afdafa8..0e73fd10ba72 100644
--- a/drivers/platform/x86/intel_pmic_gpio.c
+++ b/drivers/platform/x86/intel_pmic_gpio.c
@@ -274,11 +274,11 @@ static int platform_pmic_gpio_probe(struct platform_device *pdev)
 	pg->chip.base = pdata->gpio_base;
 	pg->chip.ngpio = NUM_GPIO;
 	pg->chip.can_sleep = 1;
-	pg->chip.dev = dev;
+	pg->chip.parent = dev;
 
 	mutex_init(&pg->buslock);
 
-	pg->chip.dev = dev;
+	pg->chip.parent = dev;
 	retval = gpiochip_add(&pg->chip);
 	if (retval) {
 		pr_err("Can not add pmic gpio chip\n");
diff --git a/drivers/platform/x86/intel_punit_ipc.c b/drivers/platform/x86/intel_punit_ipc.c
new file mode 100644
index 000000000000..bd875409a02d
--- /dev/null
+++ b/drivers/platform/x86/intel_punit_ipc.c
@@ -0,0 +1,342 @@
+/*
+ * Driver for the Intel P-Unit Mailbox IPC mechanism
+ *
+ * (C) Copyright 2015 Intel Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * The heart of the P-Unit is the Foxton microcontroller and its firmware,
+ * which provide mailbox interface for power management usage.
+ */
+
+#include <linux/module.h>
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/bitops.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <asm/intel_punit_ipc.h>
+
+/* IPC Mailbox registers */
+#define OFFSET_DATA_LOW		0x0
+#define OFFSET_DATA_HIGH	0x4
+/* bit field of interface register */
+#define	CMD_RUN			BIT(31)
+#define	CMD_ERRCODE_MASK	GENMASK(7, 0)
+#define	CMD_PARA1_SHIFT		8
+#define	CMD_PARA2_SHIFT		16
+
+#define CMD_TIMEOUT_SECONDS	1
+
+enum {
+	BASE_DATA = 0,
+	BASE_IFACE,
+	BASE_MAX,
+};
+
+typedef struct {
+	struct device *dev;
+	struct mutex lock;
+	int irq;
+	struct completion cmd_complete;
+	/* base of interface and data registers */
+	void __iomem *base[RESERVED_IPC][BASE_MAX];
+	IPC_TYPE type;
+} IPC_DEV;
+
+static IPC_DEV *punit_ipcdev;
+
+static inline u32 ipc_read_status(IPC_DEV *ipcdev, IPC_TYPE type)
+{
+	return readl(ipcdev->base[type][BASE_IFACE]);
+}
+
+static inline void ipc_write_cmd(IPC_DEV *ipcdev, IPC_TYPE type, u32 cmd)
+{
+	writel(cmd, ipcdev->base[type][BASE_IFACE]);
+}
+
+static inline u32 ipc_read_data_low(IPC_DEV *ipcdev, IPC_TYPE type)
+{
+	return readl(ipcdev->base[type][BASE_DATA] + OFFSET_DATA_LOW);
+}
+
+static inline u32 ipc_read_data_high(IPC_DEV *ipcdev, IPC_TYPE type)
+{
+	return readl(ipcdev->base[type][BASE_DATA] + OFFSET_DATA_HIGH);
+}
+
+static inline void ipc_write_data_low(IPC_DEV *ipcdev, IPC_TYPE type, u32 data)
+{
+	writel(data, ipcdev->base[type][BASE_DATA] + OFFSET_DATA_LOW);
+}
+
+static inline void ipc_write_data_high(IPC_DEV *ipcdev, IPC_TYPE type, u32 data)
+{
+	writel(data, ipcdev->base[type][BASE_DATA] + OFFSET_DATA_HIGH);
+}
+
+static const char *ipc_err_string(int error)
+{
+	if (error == IPC_PUNIT_ERR_SUCCESS)
+		return "no error";
+	else if (error == IPC_PUNIT_ERR_INVALID_CMD)
+		return "invalid command";
+	else if (error == IPC_PUNIT_ERR_INVALID_PARAMETER)
+		return "invalid parameter";
+	else if (error == IPC_PUNIT_ERR_CMD_TIMEOUT)
+		return "command timeout";
+	else if (error == IPC_PUNIT_ERR_CMD_LOCKED)
+		return "command locked";
+	else if (error == IPC_PUNIT_ERR_INVALID_VR_ID)
+		return "invalid vr id";
+	else if (error == IPC_PUNIT_ERR_VR_ERR)
+		return "vr error";
+	else
+		return "unknown error";
+}
+
+static int intel_punit_ipc_check_status(IPC_DEV *ipcdev, IPC_TYPE type)
+{
+	int loops = CMD_TIMEOUT_SECONDS * USEC_PER_SEC;
+	int errcode;
+	int status;
+
+	if (ipcdev->irq) {
+		if (!wait_for_completion_timeout(&ipcdev->cmd_complete,
+						 CMD_TIMEOUT_SECONDS * HZ)) {
+			dev_err(ipcdev->dev, "IPC timed out\n");
+			return -ETIMEDOUT;
+		}
+	} else {
+		while ((ipc_read_status(ipcdev, type) & CMD_RUN) && --loops)
+			udelay(1);
+		if (!loops) {
+			dev_err(ipcdev->dev, "IPC timed out\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	status = ipc_read_status(ipcdev, type);
+	errcode = status & CMD_ERRCODE_MASK;
+	if (errcode) {
+		dev_err(ipcdev->dev, "IPC failed: %s, IPC_STS=0x%x\n",
+			ipc_err_string(errcode), status);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+/**
+ * intel_punit_ipc_simple_command() - Simple IPC command
+ * @cmd:	IPC command code.
+ * @para1:	First 8bit parameter, set 0 if not used.
+ * @para2:	Second 8bit parameter, set 0 if not used.
+ *
+ * Send a IPC command to P-Unit when there is no data transaction
+ *
+ * Return:	IPC error code or 0 on success.
+ */
+int intel_punit_ipc_simple_command(int cmd, int para1, int para2)
+{
+	IPC_DEV *ipcdev = punit_ipcdev;
+	IPC_TYPE type;
+	u32 val;
+	int ret;
+
+	mutex_lock(&ipcdev->lock);
+
+	reinit_completion(&ipcdev->cmd_complete);
+	type = (cmd & IPC_PUNIT_CMD_TYPE_MASK) >> IPC_TYPE_OFFSET;
+
+	val = cmd & ~IPC_PUNIT_CMD_TYPE_MASK;
+	val |= CMD_RUN | para2 << CMD_PARA2_SHIFT | para1 << CMD_PARA1_SHIFT;
+	ipc_write_cmd(ipcdev, type, val);
+	ret = intel_punit_ipc_check_status(ipcdev, type);
+
+	mutex_unlock(&ipcdev->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(intel_punit_ipc_simple_command);
+
+/**
+ * intel_punit_ipc_command() - IPC command with data and pointers
+ * @cmd:	IPC command code.
+ * @para1:	First 8bit parameter, set 0 if not used.
+ * @para2:	Second 8bit parameter, set 0 if not used.
+ * @in:		Input data, 32bit for BIOS cmd, two 32bit for GTD and ISPD.
+ * @out:	Output data.
+ *
+ * Send a IPC command to P-Unit with data transaction
+ *
+ * Return:	IPC error code or 0 on success.
+ */
+int intel_punit_ipc_command(u32 cmd, u32 para1, u32 para2, u32 *in, u32 *out)
+{
+	IPC_DEV *ipcdev = punit_ipcdev;
+	IPC_TYPE type;
+	u32 val;
+	int ret;
+
+	mutex_lock(&ipcdev->lock);
+
+	reinit_completion(&ipcdev->cmd_complete);
+	type = (cmd & IPC_PUNIT_CMD_TYPE_MASK) >> IPC_TYPE_OFFSET;
+
+	if (in) {
+		ipc_write_data_low(ipcdev, type, *in);
+		if (type == GTDRIVER_IPC || type == ISPDRIVER_IPC)
+			ipc_write_data_high(ipcdev, type, *++in);
+	}
+
+	val = cmd & ~IPC_PUNIT_CMD_TYPE_MASK;
+	val |= CMD_RUN | para2 << CMD_PARA2_SHIFT | para1 << CMD_PARA1_SHIFT;
+	ipc_write_cmd(ipcdev, type, val);
+
+	ret = intel_punit_ipc_check_status(ipcdev, type);
+	if (ret)
+		goto out;
+
+	if (out) {
+		*out = ipc_read_data_low(ipcdev, type);
+		if (type == GTDRIVER_IPC || type == ISPDRIVER_IPC)
+			*++out = ipc_read_data_high(ipcdev, type);
+	}
+
+out:
+	mutex_unlock(&ipcdev->lock);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(intel_punit_ipc_command);
+
+static irqreturn_t intel_punit_ioc(int irq, void *dev_id)
+{
+	IPC_DEV *ipcdev = dev_id;
+
+	complete(&ipcdev->cmd_complete);
+	return IRQ_HANDLED;
+}
+
+static int intel_punit_get_bars(struct platform_device *pdev)
+{
+	struct resource *res;
+	void __iomem *addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[BIOS_IPC][BASE_DATA] = addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[BIOS_IPC][BASE_IFACE] = addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[ISPDRIVER_IPC][BASE_DATA] = addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 3);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[ISPDRIVER_IPC][BASE_IFACE] = addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 4);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[GTDRIVER_IPC][BASE_DATA] = addr;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 5);
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(addr))
+		return PTR_ERR(addr);
+	punit_ipcdev->base[GTDRIVER_IPC][BASE_IFACE] = addr;
+
+	return 0;
+}
+
+static int intel_punit_ipc_probe(struct platform_device *pdev)
+{
+	int irq, ret;
+
+	punit_ipcdev = devm_kzalloc(&pdev->dev,
+				    sizeof(*punit_ipcdev), GFP_KERNEL);
+	if (!punit_ipcdev)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, punit_ipcdev);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		punit_ipcdev->irq = 0;
+		dev_warn(&pdev->dev, "Invalid IRQ, using polling mode\n");
+	} else {
+		ret = devm_request_irq(&pdev->dev, irq, intel_punit_ioc,
+				       IRQF_NO_SUSPEND, "intel_punit_ipc",
+				       &punit_ipcdev);
+		if (ret) {
+			dev_err(&pdev->dev, "Failed to request irq: %d\n", irq);
+			return ret;
+		}
+		punit_ipcdev->irq = irq;
+	}
+
+	ret = intel_punit_get_bars(pdev);
+	if (ret)
+		goto out;
+
+	punit_ipcdev->dev = &pdev->dev;
+	mutex_init(&punit_ipcdev->lock);
+	init_completion(&punit_ipcdev->cmd_complete);
+
+out:
+	return ret;
+}
+
+static int intel_punit_ipc_remove(struct platform_device *pdev)
+{
+	return 0;
+}
+
+static const struct acpi_device_id punit_ipc_acpi_ids[] = {
+	{ "INT34D4", 0 },
+	{ }
+};
+
+static struct platform_driver intel_punit_ipc_driver = {
+	.probe = intel_punit_ipc_probe,
+	.remove = intel_punit_ipc_remove,
+	.driver = {
+		.name = "intel_punit_ipc",
+		.acpi_match_table = ACPI_PTR(punit_ipc_acpi_ids),
+	},
+};
+
+static int __init intel_punit_ipc_init(void)
+{
+	return platform_driver_register(&intel_punit_ipc_driver);
+}
+
+static void __exit intel_punit_ipc_exit(void)
+{
+	platform_driver_unregister(&intel_punit_ipc_driver);
+}
+
+MODULE_AUTHOR("Zha Qipeng <qipeng.zha@intel.com>");
+MODULE_DESCRIPTION("Intel P-Unit IPC driver");
+MODULE_LICENSE("GPL v2");
+
+/* Some modules are dependent on this, so init earlier */
+fs_initcall(intel_punit_ipc_init);
+module_exit(intel_punit_ipc_exit);
diff --git a/drivers/platform/x86/intel_scu_ipcutil.c b/drivers/platform/x86/intel_scu_ipcutil.c
index 02bc5a6343c3..aa454241489c 100644
--- a/drivers/platform/x86/intel_scu_ipcutil.c
+++ b/drivers/platform/x86/intel_scu_ipcutil.c
@@ -49,7 +49,7 @@ struct scu_ipc_data {
 
 static int scu_reg_access(u32 cmd, struct scu_ipc_data  *data)
 {
-	int count = data->count;
+	unsigned int count = data->count;
 
 	if (count == 0 || count == 3 || count > 4)
 		return -EINVAL;
diff --git a/drivers/platform/x86/intel_telemetry_core.c b/drivers/platform/x86/intel_telemetry_core.c
new file mode 100644
index 000000000000..a695a436a1c3
--- /dev/null
+++ b/drivers/platform/x86/intel_telemetry_core.c
@@ -0,0 +1,464 @@
+/*
+ * Intel SoC Core Telemetry Driver
+ * Copyright (C) 2015, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * Telemetry Framework provides platform related PM and performance statistics.
+ * This file provides the core telemetry API implementation.
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+
+#include <asm/intel_telemetry.h>
+
+#define DRIVER_NAME "intel_telemetry_core"
+
+struct telemetry_core_config {
+	struct telemetry_plt_config *plt_config;
+	struct telemetry_core_ops *telem_ops;
+};
+
+static struct telemetry_core_config telm_core_conf;
+
+static int telemetry_def_update_events(struct telemetry_evtconfig pss_evtconfig,
+				      struct telemetry_evtconfig ioss_evtconfig)
+{
+	return 0;
+}
+
+static int telemetry_def_set_sampling_period(u8 pss_period, u8 ioss_period)
+{
+	return 0;
+}
+
+static int telemetry_def_get_sampling_period(u8 *pss_min_period,
+					     u8 *pss_max_period,
+					     u8 *ioss_min_period,
+					     u8 *ioss_max_period)
+{
+	return 0;
+}
+
+static int telemetry_def_get_eventconfig(
+			struct telemetry_evtconfig *pss_evtconfig,
+			struct telemetry_evtconfig *ioss_evtconfig,
+			int pss_len, int ioss_len)
+{
+	return 0;
+}
+
+static int telemetry_def_get_trace_verbosity(enum telemetry_unit telem_unit,
+					     u32 *verbosity)
+{
+	return 0;
+}
+
+
+static int telemetry_def_set_trace_verbosity(enum telemetry_unit telem_unit,
+					     u32 verbosity)
+{
+	return 0;
+}
+
+static int telemetry_def_raw_read_eventlog(enum telemetry_unit telem_unit,
+					   struct telemetry_evtlog *evtlog,
+					   int len, int log_all_evts)
+{
+	return 0;
+}
+
+static int telemetry_def_read_eventlog(enum telemetry_unit telem_unit,
+				       struct telemetry_evtlog *evtlog,
+				       int len, int log_all_evts)
+{
+	return 0;
+}
+
+static int telemetry_def_add_events(u8 num_pss_evts, u8 num_ioss_evts,
+				    u32 *pss_evtmap, u32 *ioss_evtmap)
+{
+	return 0;
+}
+
+static int telemetry_def_reset_events(void)
+{
+	return 0;
+}
+
+static struct telemetry_core_ops telm_defpltops = {
+	.set_sampling_period = telemetry_def_set_sampling_period,
+	.get_sampling_period = telemetry_def_get_sampling_period,
+	.get_trace_verbosity = telemetry_def_get_trace_verbosity,
+	.set_trace_verbosity = telemetry_def_set_trace_verbosity,
+	.raw_read_eventlog = telemetry_def_raw_read_eventlog,
+	.get_eventconfig = telemetry_def_get_eventconfig,
+	.read_eventlog = telemetry_def_read_eventlog,
+	.update_events = telemetry_def_update_events,
+	.reset_events = telemetry_def_reset_events,
+	.add_events = telemetry_def_add_events,
+};
+
+/**
+ * telemetry_update_events() - Update telemetry Configuration
+ * @pss_evtconfig: PSS related config. No change if num_evts = 0.
+ * @pss_evtconfig: IOSS related config. No change if num_evts = 0.
+ *
+ * This API updates the IOSS & PSS Telemetry configuration. Old config
+ * is overwritten. Call telemetry_reset_events when logging is over
+ * All sample period values should be in the form of:
+ * bits[6:3] -> value; bits [0:2]-> Exponent; Period = (Value *16^Exponent)
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_update_events(struct telemetry_evtconfig pss_evtconfig,
+			    struct telemetry_evtconfig ioss_evtconfig)
+{
+	return telm_core_conf.telem_ops->update_events(pss_evtconfig,
+						       ioss_evtconfig);
+}
+EXPORT_SYMBOL_GPL(telemetry_update_events);
+
+
+/**
+ * telemetry_set_sampling_period() - Sets the IOSS & PSS sampling period
+ * @pss_period:  placeholder for PSS Period to be set.
+ *		 Set to 0 if not required to be updated
+ * @ioss_period: placeholder for IOSS Period to be set
+ *		 Set to 0 if not required to be updated
+ *
+ * All values should be in the form of:
+ * bits[6:3] -> value; bits [0:2]-> Exponent; Period = (Value *16^Exponent)
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_set_sampling_period(u8 pss_period, u8 ioss_period)
+{
+	return telm_core_conf.telem_ops->set_sampling_period(pss_period,
+							     ioss_period);
+}
+EXPORT_SYMBOL_GPL(telemetry_set_sampling_period);
+
+/**
+ * telemetry_get_sampling_period() - Get IOSS & PSS min & max sampling period
+ * @pss_min_period:  placeholder for PSS Min Period supported
+ * @pss_max_period:  placeholder for PSS Max Period supported
+ * @ioss_min_period: placeholder for IOSS Min Period supported
+ * @ioss_max_period: placeholder for IOSS Max Period supported
+ *
+ * All values should be in the form of:
+ * bits[6:3] -> value; bits [0:2]-> Exponent; Period = (Value *16^Exponent)
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_get_sampling_period(u8 *pss_min_period, u8 *pss_max_period,
+				  u8 *ioss_min_period, u8 *ioss_max_period)
+{
+	return telm_core_conf.telem_ops->get_sampling_period(pss_min_period,
+							     pss_max_period,
+							     ioss_min_period,
+							     ioss_max_period);
+}
+EXPORT_SYMBOL_GPL(telemetry_get_sampling_period);
+
+
+/**
+ * telemetry_reset_events() - Restore the IOSS & PSS configuration to default
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_reset_events(void)
+{
+	return telm_core_conf.telem_ops->reset_events();
+}
+EXPORT_SYMBOL_GPL(telemetry_reset_events);
+
+/**
+ * telemetry_get_eventconfig() - Returns the pss and ioss events enabled
+ * @pss_evtconfig: Pointer to PSS related configuration.
+ * @pss_evtconfig: Pointer to IOSS related configuration.
+ * @pss_len:	   Number of u32 elements allocated for pss_evtconfig array
+ * @ioss_len:	   Number of u32 elements allocated for ioss_evtconfig array
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_get_eventconfig(struct telemetry_evtconfig *pss_evtconfig,
+			      struct telemetry_evtconfig *ioss_evtconfig,
+			      int pss_len, int ioss_len)
+{
+	return telm_core_conf.telem_ops->get_eventconfig(pss_evtconfig,
+							 ioss_evtconfig,
+							 pss_len, ioss_len);
+}
+EXPORT_SYMBOL_GPL(telemetry_get_eventconfig);
+
+/**
+ * telemetry_add_events() - Add IOSS & PSS configuration to existing settings.
+ * @num_pss_evts:  Number of PSS Events (<29) in pss_evtmap. Can be 0.
+ * @num_ioss_evts: Number of IOSS Events (<29) in ioss_evtmap. Can be 0.
+ * @pss_evtmap:    Array of PSS Event-IDs to Enable
+ * @ioss_evtmap:   Array of PSS Event-IDs to Enable
+ *
+ * Events are appended to Old Configuration. In case of total events > 28, it
+ * returns error. Call telemetry_reset_events to reset after eventlog done
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_add_events(u8 num_pss_evts, u8 num_ioss_evts,
+			 u32 *pss_evtmap, u32 *ioss_evtmap)
+{
+	return telm_core_conf.telem_ops->add_events(num_pss_evts,
+						    num_ioss_evts, pss_evtmap,
+						    ioss_evtmap);
+}
+EXPORT_SYMBOL_GPL(telemetry_add_events);
+
+/**
+ * telemetry_read_events() - Fetches samples as specified by evtlog.telem_evt_id
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @evtlog:     Array of telemetry_evtlog structs to fill data
+ *		evtlog.telem_evt_id specifies the ids to read
+ * @len:	Length of array of evtlog
+ *
+ * Return: number of eventlogs read for success, < 0 for failure
+ */
+int telemetry_read_events(enum telemetry_unit telem_unit,
+			  struct telemetry_evtlog *evtlog, int len)
+{
+	return telm_core_conf.telem_ops->read_eventlog(telem_unit, evtlog,
+						       len, 0);
+}
+EXPORT_SYMBOL_GPL(telemetry_read_events);
+
+/**
+ * telemetry_raw_read_events() - Fetch samples specified by evtlog.telem_evt_id
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @evtlog:	Array of telemetry_evtlog structs to fill data
+ *		evtlog.telem_evt_id specifies the ids to read
+ * @len:	Length of array of evtlog
+ *
+ * The caller must take care of locking in this case.
+ *
+ * Return: number of eventlogs read for success, < 0 for failure
+ */
+int telemetry_raw_read_events(enum telemetry_unit telem_unit,
+			      struct telemetry_evtlog *evtlog, int len)
+{
+	return telm_core_conf.telem_ops->raw_read_eventlog(telem_unit, evtlog,
+							   len, 0);
+}
+EXPORT_SYMBOL_GPL(telemetry_raw_read_events);
+
+/**
+ * telemetry_read_eventlog() - Fetch the Telemetry log from PSS or IOSS
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @evtlog:	Array of telemetry_evtlog structs to fill data
+ * @len:	Length of array of evtlog
+ *
+ * Return: number of eventlogs read for success, < 0 for failure
+ */
+int telemetry_read_eventlog(enum telemetry_unit telem_unit,
+			    struct telemetry_evtlog *evtlog, int len)
+{
+	return telm_core_conf.telem_ops->read_eventlog(telem_unit, evtlog,
+						       len, 1);
+}
+EXPORT_SYMBOL_GPL(telemetry_read_eventlog);
+
+/**
+ * telemetry_raw_read_eventlog() - Fetch the Telemetry log from PSS or IOSS
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @evtlog:	Array of telemetry_evtlog structs to fill data
+ * @len:	Length of array of evtlog
+ *
+ * The caller must take care of locking in this case.
+ *
+ * Return: number of eventlogs read for success, < 0 for failure
+ */
+int telemetry_raw_read_eventlog(enum telemetry_unit telem_unit,
+				struct telemetry_evtlog *evtlog, int len)
+{
+	return telm_core_conf.telem_ops->raw_read_eventlog(telem_unit, evtlog,
+							   len, 1);
+}
+EXPORT_SYMBOL_GPL(telemetry_raw_read_eventlog);
+
+
+/**
+ * telemetry_get_trace_verbosity() - Get the IOSS & PSS Trace verbosity
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @verbosity:	Pointer to return Verbosity
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_get_trace_verbosity(enum telemetry_unit telem_unit,
+				  u32 *verbosity)
+{
+	return telm_core_conf.telem_ops->get_trace_verbosity(telem_unit,
+							     verbosity);
+}
+EXPORT_SYMBOL_GPL(telemetry_get_trace_verbosity);
+
+
+/**
+ * telemetry_set_trace_verbosity() - Update the IOSS & PSS Trace verbosity
+ * @telem_unit: Specify whether IOSS or PSS Read
+ * @verbosity:	Verbosity to set
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_set_trace_verbosity(enum telemetry_unit telem_unit, u32 verbosity)
+{
+	return telm_core_conf.telem_ops->set_trace_verbosity(telem_unit,
+							     verbosity);
+}
+EXPORT_SYMBOL_GPL(telemetry_set_trace_verbosity);
+
+/**
+ * telemetry_set_pltdata() - Set the platform specific Data
+ * @ops:	Pointer to ops structure
+ * @pltconfig:	Platform config data
+ *
+ * Usage by other than telemetry pltdrv module is invalid
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_set_pltdata(struct telemetry_core_ops *ops,
+			  struct telemetry_plt_config *pltconfig)
+{
+	if (ops)
+		telm_core_conf.telem_ops = ops;
+
+	if (pltconfig)
+		telm_core_conf.plt_config = pltconfig;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(telemetry_set_pltdata);
+
+/**
+ * telemetry_clear_pltdata() - Clear the platform specific Data
+ *
+ * Usage by other than telemetry pltdrv module is invalid
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_clear_pltdata(void)
+{
+	telm_core_conf.telem_ops = &telm_defpltops;
+	telm_core_conf.plt_config = NULL;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(telemetry_clear_pltdata);
+
+/**
+ * telemetry_pltconfig_valid() - Checkif platform config is valid
+ *
+ * Usage by other than telemetry module is invalid
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_pltconfig_valid(void)
+{
+	if (telm_core_conf.plt_config)
+		return 0;
+
+	else
+		return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(telemetry_pltconfig_valid);
+
+static inline int telemetry_get_pssevtname(enum telemetry_unit telem_unit,
+					   const char **name, int len)
+{
+	struct telemetry_unit_config psscfg;
+	int i;
+
+	if (!telm_core_conf.plt_config)
+		return -EINVAL;
+
+	psscfg = telm_core_conf.plt_config->pss_config;
+
+	if (len > psscfg.ssram_evts_used)
+		len = psscfg.ssram_evts_used;
+
+	for (i = 0; i < len; i++)
+		name[i] = psscfg.telem_evts[i].name;
+
+	return 0;
+}
+
+static inline int telemetry_get_iossevtname(enum telemetry_unit telem_unit,
+					    const char **name, int len)
+{
+	struct telemetry_unit_config iosscfg;
+	int i;
+
+	if (!(telm_core_conf.plt_config))
+		return -EINVAL;
+
+	iosscfg = telm_core_conf.plt_config->ioss_config;
+
+	if (len > iosscfg.ssram_evts_used)
+		len = iosscfg.ssram_evts_used;
+
+	for (i = 0; i < len; i++)
+		name[i] = iosscfg.telem_evts[i].name;
+
+	return 0;
+
+}
+
+/**
+ * telemetry_get_evtname() - Checkif platform config is valid
+ * @telem_unit:	Telemetry Unit to check
+ * @name:	Array of character pointers to contain name
+ * @len:	length of array name provided by user
+ *
+ * Usage by other than telemetry debugfs module is invalid
+ *
+ * Return: 0 success, < 0 for failure
+ */
+int telemetry_get_evtname(enum telemetry_unit telem_unit,
+			  const char **name, int len)
+{
+	int ret = -EINVAL;
+
+	if (telem_unit == TELEM_PSS)
+		ret = telemetry_get_pssevtname(telem_unit, name, len);
+
+	else if (telem_unit == TELEM_IOSS)
+		ret = telemetry_get_iossevtname(telem_unit, name, len);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(telemetry_get_evtname);
+
+static int __init telemetry_module_init(void)
+{
+	pr_info(pr_fmt(DRIVER_NAME) " Init\n");
+
+	telm_core_conf.telem_ops = &telm_defpltops;
+	return 0;
+}
+
+static void __exit telemetry_module_exit(void)
+{
+}
+
+module_init(telemetry_module_init);
+module_exit(telemetry_module_exit);
+
+MODULE_AUTHOR("Souvik Kumar Chakravarty <souvik.k.chakravarty@intel.com>");
+MODULE_DESCRIPTION("Intel SoC Telemetry Interface");
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/intel_telemetry_debugfs.c b/drivers/platform/x86/intel_telemetry_debugfs.c
new file mode 100644
index 000000000000..f5134acd6ff0
--- /dev/null
+++ b/drivers/platform/x86/intel_telemetry_debugfs.c
@@ -0,0 +1,1032 @@
+/*
+ * Intel SOC Telemetry debugfs Driver: Currently supports APL
+ * Copyright (c) 2015, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 provides the debugfs interfaces for telemetry.
+ * /sys/kernel/debug/telemetry/pss_info: Shows Primary Control Sub-Sys Counters
+ * /sys/kernel/debug/telemetry/ioss_info: Shows IO Sub-System Counters
+ * /sys/kernel/debug/telemetry/soc_states: Shows SoC State
+ * /sys/kernel/debug/telemetry/pss_trace_verbosity: Read and Change Tracing
+ *				Verbosity via firmware
+ * /sys/kernel/debug/telemetry/ioss_race_verbosity: Write and Change Tracing
+ *				Verbosity via firmware
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/pci.h>
+#include <linux/suspend.h>
+
+#include <asm/cpu_device_id.h>
+#include <asm/intel_pmc_ipc.h>
+#include <asm/intel_punit_ipc.h>
+#include <asm/intel_telemetry.h>
+
+#define DRIVER_NAME	"telemetry_soc_debugfs"
+#define DRIVER_VERSION	"1.0.0"
+
+/* ApolloLake SoC Event-IDs */
+#define TELEM_APL_PSS_PSTATES_ID	0x2802
+#define TELEM_APL_PSS_IDLE_ID		0x2806
+#define TELEM_APL_PCS_IDLE_BLOCKED_ID	0x2C00
+#define TELEM_APL_PCS_S0IX_BLOCKED_ID	0x2C01
+#define TELEM_APL_PSS_WAKEUP_ID		0x2C02
+#define TELEM_APL_PSS_LTR_BLOCKING_ID	0x2C03
+
+#define TELEM_APL_S0IX_TOTAL_OCC_ID	0x4000
+#define TELEM_APL_S0IX_SHLW_OCC_ID	0x4001
+#define TELEM_APL_S0IX_DEEP_OCC_ID	0x4002
+#define TELEM_APL_S0IX_TOTAL_RES_ID	0x4800
+#define TELEM_APL_S0IX_SHLW_RES_ID	0x4801
+#define TELEM_APL_S0IX_DEEP_RES_ID	0x4802
+#define TELEM_APL_D0IX_ID		0x581A
+#define TELEM_APL_D3_ID			0x5819
+#define TELEM_APL_PG_ID			0x5818
+
+#define TELEM_INFO_SRAMEVTS_MASK	0xFF00
+#define TELEM_INFO_SRAMEVTS_SHIFT	0x8
+#define TELEM_SSRAM_READ_TIMEOUT	10
+
+#define TELEM_MASK_BIT			1
+#define TELEM_MASK_BYTE			0xFF
+#define BYTES_PER_LONG			8
+#define TELEM_APL_MASK_PCS_STATE	0xF
+
+/* Max events in bitmap to check for */
+#define TELEM_PSS_IDLE_EVTS		25
+#define TELEM_PSS_IDLE_BLOCKED_EVTS	20
+#define TELEM_PSS_S0IX_BLOCKED_EVTS	20
+#define TELEM_PSS_S0IX_WAKEUP_EVTS	20
+#define TELEM_PSS_LTR_BLOCKING_EVTS	20
+#define TELEM_IOSS_DX_D0IX_EVTS		25
+#define TELEM_IOSS_PG_EVTS		30
+
+#define TELEM_EVT_LEN(x) (sizeof(x)/sizeof((x)[0]))
+
+#define TELEM_DEBUGFS_CPU(model, data) \
+	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_MWAIT, (unsigned long)&data}
+
+#define TELEM_CHECK_AND_PARSE_EVTS(EVTID, EVTNUM, BUF, EVTLOG, EVTDAT, MASK) { \
+	if (evtlog[index].telem_evtid == (EVTID)) { \
+		for (idx = 0; idx < (EVTNUM); idx++) \
+			(BUF)[idx] = ((EVTLOG) >> (EVTDAT)[idx].bit_pos) & \
+				     (MASK); \
+	continue; \
+	} \
+}
+
+#define TELEM_CHECK_AND_PARSE_CTRS(EVTID, CTR) { \
+	if (evtlog[index].telem_evtid == (EVTID)) { \
+		(CTR) = evtlog[index].telem_evtlog; \
+		continue; \
+	} \
+}
+
+#ifdef CONFIG_PM_SLEEP
+static u8 suspend_prep_ok;
+static u32 suspend_shlw_ctr_temp, suspend_deep_ctr_temp;
+static u64 suspend_shlw_res_temp, suspend_deep_res_temp;
+#endif
+
+struct telemetry_susp_stats {
+	u32 shlw_swake_ctr;
+	u32 deep_swake_ctr;
+	u64 shlw_swake_res;
+	u64 deep_swake_res;
+	u32 shlw_ctr;
+	u32 deep_ctr;
+	u64 shlw_res;
+	u64 deep_res;
+};
+
+/* Bitmap definitions for default counters in APL */
+struct telem_pss_idle_stateinfo {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_pss_idle_stateinfo telem_apl_pss_idle_data[] = {
+	{"IA_CORE0_C1E",		0},
+	{"IA_CORE1_C1E",		1},
+	{"IA_CORE2_C1E",		2},
+	{"IA_CORE3_C1E",		3},
+	{"IA_CORE0_C6",			16},
+	{"IA_CORE1_C6",			17},
+	{"IA_CORE2_C6",			18},
+	{"IA_CORE3_C6",			19},
+	{"IA_MODULE0_C7",		32},
+	{"IA_MODULE1_C7",		33},
+	{"GT_RC6",			40},
+	{"IUNIT_PROCESSING_IDLE",	41},
+	{"FAR_MEM_IDLE",		43},
+	{"DISPLAY_IDLE",		44},
+	{"IUNIT_INPUT_SYSTEM_IDLE",	45},
+	{"PCS_STATUS",			60},
+};
+
+struct telem_pcs_blkd_info {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_pcs_blkd_info telem_apl_pcs_idle_blkd_data[] = {
+	{"COMPUTE",			0},
+	{"MISC",			8},
+	{"MODULE_ACTIONS_PENDING",	16},
+	{"LTR",				24},
+	{"DISPLAY_WAKE",		32},
+	{"ISP_WAKE",			40},
+	{"PSF0_ACTIVE",			48},
+};
+
+static struct telem_pcs_blkd_info telem_apl_pcs_s0ix_blkd_data[] = {
+	{"LTR",				0},
+	{"IRTL",			8},
+	{"WAKE_DEADLINE_PENDING",	16},
+	{"DISPLAY",			24},
+	{"ISP",				32},
+	{"CORE",			40},
+	{"PMC",				48},
+	{"MISC",			56},
+};
+
+struct telem_pss_ltr_info {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_pss_ltr_info telem_apl_pss_ltr_data[] = {
+	{"CORE_ACTIVE",		0},
+	{"MEM_UP",		8},
+	{"DFX",			16},
+	{"DFX_FORCE_LTR",	24},
+	{"DISPLAY",		32},
+	{"ISP",			40},
+	{"SOUTH",		48},
+};
+
+struct telem_pss_wakeup_info {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_pss_wakeup_info telem_apl_pss_wakeup[] = {
+	{"IP_IDLE",			0},
+	{"DISPLAY_WAKE",		8},
+	{"VOLTAGE_REG_INT",		16},
+	{"DROWSY_TIMER (HOTPLUG)",	24},
+	{"CORE_WAKE",			32},
+	{"MISC_S0IX",			40},
+	{"MISC_ABORT",			56},
+};
+
+struct telem_ioss_d0ix_stateinfo {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_ioss_d0ix_stateinfo telem_apl_ioss_d0ix_data[] = {
+	{"CSE",		0},
+	{"SCC2",	1},
+	{"GMM",		2},
+	{"XDCI",	3},
+	{"XHCI",	4},
+	{"ISH",		5},
+	{"AVS",		6},
+	{"PCIE0P1",	7},
+	{"PECI0P0",	8},
+	{"LPSS",	9},
+	{"SCC",		10},
+	{"PWM",		11},
+	{"PCIE1_P3",    12},
+	{"PCIE1_P2",    13},
+	{"PCIE1_P1",    14},
+	{"PCIE1_P0",    15},
+	{"CNV",		16},
+	{"SATA",	17},
+	{"PRTC",	18},
+};
+
+struct telem_ioss_pg_info {
+	const char *name;
+	u32 bit_pos;
+};
+
+static struct telem_ioss_pg_info telem_apl_ioss_pg_data[] = {
+	{"LPSS",	0},
+	{"SCC",		1},
+	{"P2SB",	2},
+	{"SCC2",	3},
+	{"GMM",		4},
+	{"PCIE0",	5},
+	{"XDCI",	6},
+	{"xHCI",	7},
+	{"CSE",		8},
+	{"SPI",		9},
+	{"AVSPGD4",	10},
+	{"AVSPGD3",	11},
+	{"AVSPGD2",	12},
+	{"AVSPGD1",	13},
+	{"ISH",		14},
+	{"EXI",		15},
+	{"NPKVRC",	16},
+	{"NPKVNN",	17},
+	{"CUNIT",	18},
+	{"FUSE_CTRL",	19},
+	{"PCIE1",	20},
+	{"CNV",		21},
+	{"LPC",		22},
+	{"SATA",	23},
+	{"SMB",		24},
+	{"PRTC",	25},
+};
+
+
+struct telemetry_debugfs_conf {
+	struct telemetry_susp_stats suspend_stats;
+	struct dentry *telemetry_dbg_dir;
+
+	/* Bitmap Data */
+	struct telem_ioss_d0ix_stateinfo *ioss_d0ix_data;
+	struct telem_pss_idle_stateinfo *pss_idle_data;
+	struct telem_pcs_blkd_info *pcs_idle_blkd_data;
+	struct telem_pcs_blkd_info *pcs_s0ix_blkd_data;
+	struct telem_pss_wakeup_info *pss_wakeup;
+	struct telem_pss_ltr_info *pss_ltr_data;
+	struct telem_ioss_pg_info *ioss_pg_data;
+	u8 pcs_idle_blkd_evts;
+	u8 pcs_s0ix_blkd_evts;
+	u8 pss_wakeup_evts;
+	u8 pss_idle_evts;
+	u8 pss_ltr_evts;
+	u8 ioss_d0ix_evts;
+	u8 ioss_pg_evts;
+
+	/* IDs */
+	u16  pss_ltr_blocking_id;
+	u16  pcs_idle_blkd_id;
+	u16  pcs_s0ix_blkd_id;
+	u16  s0ix_total_occ_id;
+	u16  s0ix_shlw_occ_id;
+	u16  s0ix_deep_occ_id;
+	u16  s0ix_total_res_id;
+	u16  s0ix_shlw_res_id;
+	u16  s0ix_deep_res_id;
+	u16  pss_wakeup_id;
+	u16  ioss_d0ix_id;
+	u16  pstates_id;
+	u16  pss_idle_id;
+	u16  ioss_d3_id;
+	u16  ioss_pg_id;
+};
+
+static struct telemetry_debugfs_conf *debugfs_conf;
+
+static struct telemetry_debugfs_conf telem_apl_debugfs_conf = {
+	.pss_idle_data = telem_apl_pss_idle_data,
+	.pcs_idle_blkd_data = telem_apl_pcs_idle_blkd_data,
+	.pcs_s0ix_blkd_data = telem_apl_pcs_s0ix_blkd_data,
+	.pss_ltr_data = telem_apl_pss_ltr_data,
+	.pss_wakeup = telem_apl_pss_wakeup,
+	.ioss_d0ix_data = telem_apl_ioss_d0ix_data,
+	.ioss_pg_data = telem_apl_ioss_pg_data,
+
+	.pss_idle_evts = TELEM_EVT_LEN(telem_apl_pss_idle_data),
+	.pcs_idle_blkd_evts = TELEM_EVT_LEN(telem_apl_pcs_idle_blkd_data),
+	.pcs_s0ix_blkd_evts = TELEM_EVT_LEN(telem_apl_pcs_s0ix_blkd_data),
+	.pss_ltr_evts = TELEM_EVT_LEN(telem_apl_pss_ltr_data),
+	.pss_wakeup_evts = TELEM_EVT_LEN(telem_apl_pss_wakeup),
+	.ioss_d0ix_evts = TELEM_EVT_LEN(telem_apl_ioss_d0ix_data),
+	.ioss_pg_evts = TELEM_EVT_LEN(telem_apl_ioss_pg_data),
+
+	.pstates_id = TELEM_APL_PSS_PSTATES_ID,
+	.pss_idle_id = TELEM_APL_PSS_IDLE_ID,
+	.pcs_idle_blkd_id = TELEM_APL_PCS_IDLE_BLOCKED_ID,
+	.pcs_s0ix_blkd_id = TELEM_APL_PCS_S0IX_BLOCKED_ID,
+	.pss_wakeup_id = TELEM_APL_PSS_WAKEUP_ID,
+	.pss_ltr_blocking_id = TELEM_APL_PSS_LTR_BLOCKING_ID,
+	.s0ix_total_occ_id = TELEM_APL_S0IX_TOTAL_OCC_ID,
+	.s0ix_shlw_occ_id = TELEM_APL_S0IX_SHLW_OCC_ID,
+	.s0ix_deep_occ_id = TELEM_APL_S0IX_DEEP_OCC_ID,
+	.s0ix_total_res_id = TELEM_APL_S0IX_TOTAL_RES_ID,
+	.s0ix_shlw_res_id = TELEM_APL_S0IX_SHLW_RES_ID,
+	.s0ix_deep_res_id = TELEM_APL_S0IX_DEEP_RES_ID,
+	.ioss_d0ix_id = TELEM_APL_D0IX_ID,
+	.ioss_d3_id = TELEM_APL_D3_ID,
+	.ioss_pg_id = TELEM_APL_PG_ID,
+};
+
+static const struct x86_cpu_id telemetry_debugfs_cpu_ids[] = {
+	TELEM_DEBUGFS_CPU(0x5c, telem_apl_debugfs_conf),
+	{}
+};
+
+MODULE_DEVICE_TABLE(x86cpu, telemetry_debugfs_cpu_ids);
+
+static int telemetry_debugfs_check_evts(void)
+{
+	if ((debugfs_conf->pss_idle_evts > TELEM_PSS_IDLE_EVTS) ||
+	    (debugfs_conf->pcs_idle_blkd_evts > TELEM_PSS_IDLE_BLOCKED_EVTS) ||
+	    (debugfs_conf->pcs_s0ix_blkd_evts > TELEM_PSS_S0IX_BLOCKED_EVTS) ||
+	    (debugfs_conf->pss_ltr_evts > TELEM_PSS_LTR_BLOCKING_EVTS) ||
+	    (debugfs_conf->pss_wakeup_evts > TELEM_PSS_S0IX_WAKEUP_EVTS) ||
+	    (debugfs_conf->ioss_d0ix_evts > TELEM_IOSS_DX_D0IX_EVTS) ||
+	    (debugfs_conf->ioss_pg_evts > TELEM_IOSS_PG_EVTS))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int telem_pss_states_show(struct seq_file *s, void *unused)
+{
+	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	struct telemetry_debugfs_conf *conf = debugfs_conf;
+	const char *name[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	u32 pcs_idle_blkd[TELEM_PSS_IDLE_BLOCKED_EVTS],
+	    pcs_s0ix_blkd[TELEM_PSS_S0IX_BLOCKED_EVTS],
+	    pss_s0ix_wakeup[TELEM_PSS_S0IX_WAKEUP_EVTS],
+	    pss_ltr_blkd[TELEM_PSS_LTR_BLOCKING_EVTS],
+	    pss_idle[TELEM_PSS_IDLE_EVTS];
+	int index, idx, ret, err = 0;
+	u64 pstates = 0;
+
+	ret = telemetry_read_eventlog(TELEM_PSS, evtlog,
+				      TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (ret < 0)
+		return ret;
+
+	err = telemetry_get_evtname(TELEM_PSS, name,
+				    TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (err < 0)
+		return err;
+
+	seq_puts(s, "\n----------------------------------------------------\n");
+	seq_puts(s, "\tPSS TELEM EVENTLOG (Residency = field/19.2 us\n");
+	seq_puts(s, "----------------------------------------------------\n");
+	for (index = 0; index < ret; index++) {
+		seq_printf(s, "%-32s %llu\n",
+			   name[index], evtlog[index].telem_evtlog);
+
+		/* Fetch PSS IDLE State */
+		if (evtlog[index].telem_evtid == conf->pss_idle_id) {
+			pss_idle[conf->pss_idle_evts - 1] =
+			(evtlog[index].telem_evtlog >>
+			conf->pss_idle_data[conf->pss_idle_evts - 1].bit_pos) &
+			TELEM_APL_MASK_PCS_STATE;
+		}
+
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->pss_idle_id,
+					   conf->pss_idle_evts - 1,
+					   pss_idle, evtlog[index].telem_evtlog,
+					   conf->pss_idle_data, TELEM_MASK_BIT);
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->pcs_idle_blkd_id,
+					   conf->pcs_idle_blkd_evts,
+					   pcs_idle_blkd,
+					   evtlog[index].telem_evtlog,
+					   conf->pcs_idle_blkd_data,
+					   TELEM_MASK_BYTE);
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->pcs_s0ix_blkd_id,
+					   conf->pcs_s0ix_blkd_evts,
+					   pcs_s0ix_blkd,
+					   evtlog[index].telem_evtlog,
+					   conf->pcs_s0ix_blkd_data,
+					   TELEM_MASK_BYTE);
+
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->pss_wakeup_id,
+					   conf->pss_wakeup_evts,
+					   pss_s0ix_wakeup,
+					   evtlog[index].telem_evtlog,
+					   conf->pss_wakeup, TELEM_MASK_BYTE);
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->pss_ltr_blocking_id,
+					   conf->pss_ltr_evts, pss_ltr_blkd,
+					   evtlog[index].telem_evtlog,
+					   conf->pss_ltr_data, TELEM_MASK_BYTE);
+
+		if (evtlog[index].telem_evtid == debugfs_conf->pstates_id)
+			pstates = evtlog[index].telem_evtlog;
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "PStates\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Domain\t\t\t\tFreq(Mhz)\n");
+	seq_printf(s, " IA\t\t\t\t %llu\n GT\t\t\t\t %llu\n",
+		   (pstates & TELEM_MASK_BYTE)*100,
+		   ((pstates >> 8) & TELEM_MASK_BYTE)*50/3);
+
+	seq_printf(s, " IUNIT\t\t\t\t %llu\n SA\t\t\t\t %llu\n",
+		   ((pstates >> 16) & TELEM_MASK_BYTE)*25,
+		   ((pstates >> 24) & TELEM_MASK_BYTE)*50/3);
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "PSS IDLE Status\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Device\t\t\t\t\tIDLE\n");
+	for (index = 0; index < debugfs_conf->pss_idle_evts; index++) {
+		seq_printf(s, "%-32s\t%u\n",
+			   debugfs_conf->pss_idle_data[index].name,
+			   pss_idle[index]);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "PSS Idle blkd Status (~1ms saturating bucket)\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Blocker\t\t\t\t\tCount\n");
+	for (index = 0; index < debugfs_conf->pcs_idle_blkd_evts; index++) {
+		seq_printf(s, "%-32s\t%u\n",
+			   debugfs_conf->pcs_idle_blkd_data[index].name,
+			   pcs_idle_blkd[index]);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "PSS S0ix blkd Status (~1ms saturating bucket)\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Blocker\t\t\t\t\tCount\n");
+	for (index = 0; index < debugfs_conf->pcs_s0ix_blkd_evts; index++) {
+		seq_printf(s, "%-32s\t%u\n",
+			   debugfs_conf->pcs_s0ix_blkd_data[index].name,
+			   pcs_s0ix_blkd[index]);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "LTR Blocking Status (~1ms saturating bucket)\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Blocker\t\t\t\t\tCount\n");
+	for (index = 0; index < debugfs_conf->pss_ltr_evts; index++) {
+		seq_printf(s, "%-32s\t%u\n",
+			   debugfs_conf->pss_ltr_data[index].name,
+			   pss_s0ix_wakeup[index]);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "Wakes Status (~1ms saturating bucket)\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Wakes\t\t\t\t\tCount\n");
+	for (index = 0; index < debugfs_conf->pss_wakeup_evts; index++) {
+		seq_printf(s, "%-32s\t%u\n",
+			   debugfs_conf->pss_wakeup[index].name,
+			   pss_ltr_blkd[index]);
+	}
+
+	return 0;
+}
+
+static int telem_pss_state_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, telem_pss_states_show, inode->i_private);
+}
+
+static const struct file_operations telem_pss_ops = {
+	.open		= telem_pss_state_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+
+static int telem_ioss_states_show(struct seq_file *s, void *unused)
+{
+	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	const char *name[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	int index, ret, err;
+
+	ret = telemetry_read_eventlog(TELEM_IOSS, evtlog,
+				      TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (ret < 0)
+		return ret;
+
+	err = telemetry_get_evtname(TELEM_IOSS, name,
+				    TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (err < 0)
+		return err;
+
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "\tI0SS TELEMETRY EVENTLOG\n");
+	seq_puts(s, "--------------------------------------\n");
+	for (index = 0; index < ret; index++) {
+		seq_printf(s, "%-32s 0x%llx\n",
+			   name[index], evtlog[index].telem_evtlog);
+	}
+
+	return 0;
+}
+
+static int telem_ioss_state_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, telem_ioss_states_show, inode->i_private);
+}
+
+static const struct file_operations telem_ioss_ops = {
+	.open		= telem_ioss_state_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int telem_soc_states_show(struct seq_file *s, void *unused)
+{
+	u32 d3_sts[TELEM_IOSS_DX_D0IX_EVTS], d0ix_sts[TELEM_IOSS_DX_D0IX_EVTS];
+	u32 pg_sts[TELEM_IOSS_PG_EVTS], pss_idle[TELEM_PSS_IDLE_EVTS];
+	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	u32 s0ix_total_ctr = 0, s0ix_shlw_ctr = 0, s0ix_deep_ctr = 0;
+	u64 s0ix_total_res = 0, s0ix_shlw_res = 0, s0ix_deep_res = 0;
+	struct telemetry_debugfs_conf *conf = debugfs_conf;
+	struct pci_dev *dev = NULL;
+	int index, idx, ret;
+	u32 d3_state;
+	u16 pmcsr;
+
+	ret = telemetry_read_eventlog(TELEM_IOSS, evtlog,
+				      TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (ret < 0)
+		return ret;
+
+	for (index = 0; index < ret; index++) {
+		TELEM_CHECK_AND_PARSE_EVTS(conf->ioss_d3_id,
+					   conf->ioss_d0ix_evts,
+					   d3_sts, evtlog[index].telem_evtlog,
+					   conf->ioss_d0ix_data,
+					   TELEM_MASK_BIT);
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->ioss_pg_id, conf->ioss_pg_evts,
+					   pg_sts, evtlog[index].telem_evtlog,
+					   conf->ioss_pg_data, TELEM_MASK_BIT);
+
+		TELEM_CHECK_AND_PARSE_EVTS(conf->ioss_d0ix_id,
+					   conf->ioss_d0ix_evts,
+					   d0ix_sts, evtlog[index].telem_evtlog,
+					   conf->ioss_d0ix_data,
+					   TELEM_MASK_BIT);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_total_occ_id,
+					   s0ix_total_ctr);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_occ_id,
+					   s0ix_shlw_ctr);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_occ_id,
+					   s0ix_deep_ctr);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_total_res_id,
+					   s0ix_total_res);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_res_id,
+					   s0ix_shlw_res);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_res_id,
+					   s0ix_deep_res);
+	}
+
+	seq_puts(s, "\n---------------------------------------------------\n");
+	seq_puts(s, "S0IX Type\t\t\t Occurrence\t\t Residency(us)\n");
+	seq_puts(s, "---------------------------------------------------\n");
+
+	seq_printf(s, "S0IX Shallow\t\t\t %10u\t %10llu\n",
+		   s0ix_shlw_ctr -
+		   conf->suspend_stats.shlw_ctr -
+		   conf->suspend_stats.shlw_swake_ctr,
+		   (u64)((s0ix_shlw_res -
+		   conf->suspend_stats.shlw_res -
+		   conf->suspend_stats.shlw_swake_res)*10/192));
+
+	seq_printf(s, "S0IX Deep\t\t\t %10u\t %10llu\n",
+		   s0ix_deep_ctr -
+		   conf->suspend_stats.deep_ctr -
+		   conf->suspend_stats.deep_swake_ctr,
+		   (u64)((s0ix_deep_res -
+		   conf->suspend_stats.deep_res -
+		   conf->suspend_stats.deep_swake_res)*10/192));
+
+	seq_printf(s, "Suspend(With S0ixShallow)\t %10u\t %10llu\n",
+		   conf->suspend_stats.shlw_ctr,
+		   (u64)(conf->suspend_stats.shlw_res*10)/192);
+
+	seq_printf(s, "Suspend(With S0ixDeep)\t\t %10u\t %10llu\n",
+		   conf->suspend_stats.deep_ctr,
+		   (u64)(conf->suspend_stats.deep_res*10)/192);
+
+	seq_printf(s, "Suspend(With Shallow-Wakes)\t %10u\t %10llu\n",
+		   conf->suspend_stats.shlw_swake_ctr +
+		   conf->suspend_stats.deep_swake_ctr,
+		   (u64)((conf->suspend_stats.shlw_swake_res +
+		   conf->suspend_stats.deep_swake_res)*10/192));
+
+	seq_printf(s, "S0IX+Suspend Total\t\t %10u\t %10llu\n", s0ix_total_ctr,
+				(u64)(s0ix_total_res*10/192));
+	seq_puts(s, "\n-------------------------------------------------\n");
+	seq_puts(s, "\t\tDEVICE STATES\n");
+	seq_puts(s, "-------------------------------------------------\n");
+
+	for_each_pci_dev(dev) {
+		pci_read_config_word(dev, dev->pm_cap + PCI_PM_CTRL, &pmcsr);
+		d3_state = ((pmcsr & PCI_PM_CTRL_STATE_MASK) ==
+			    (__force int)PCI_D3hot) ? 1 : 0;
+
+		seq_printf(s, "pci %04x %04X %s %20.20s: ",
+			   dev->vendor, dev->device, dev_name(&dev->dev),
+			   dev_driver_string(&dev->dev));
+		seq_printf(s, " d3:%x\n", d3_state);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "D3/D0i3 Status\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Block\t\t D3\t D0i3\n");
+	for (index = 0; index < conf->ioss_d0ix_evts; index++) {
+		seq_printf(s, "%-10s\t %u\t %u\n",
+			   conf->ioss_d0ix_data[index].name,
+			   d3_sts[index], d0ix_sts[index]);
+	}
+
+	seq_puts(s, "\n--------------------------------------\n");
+	seq_puts(s, "South Complex PowerGate Status\n");
+	seq_puts(s, "--------------------------------------\n");
+	seq_puts(s, "Device\t\t PG\n");
+	for (index = 0; index < conf->ioss_pg_evts; index++) {
+		seq_printf(s, "%-10s\t %u\n",
+			   conf->ioss_pg_data[index].name,
+			   pg_sts[index]);
+	}
+
+	evtlog->telem_evtid = conf->pss_idle_id;
+	ret = telemetry_read_events(TELEM_PSS, evtlog, 1);
+	if (ret < 0)
+		return ret;
+
+	seq_puts(s, "\n-----------------------------------------\n");
+	seq_puts(s, "North Idle Status\n");
+	seq_puts(s, "-----------------------------------------\n");
+	for (idx = 0; idx < conf->pss_idle_evts - 1; idx++) {
+		pss_idle[idx] =	(evtlog->telem_evtlog >>
+				conf->pss_idle_data[idx].bit_pos) &
+				TELEM_MASK_BIT;
+	}
+
+	pss_idle[idx] = (evtlog->telem_evtlog >>
+			conf->pss_idle_data[idx].bit_pos) &
+			TELEM_APL_MASK_PCS_STATE;
+
+	for (index = 0; index < conf->pss_idle_evts; index++) {
+		seq_printf(s, "%-30s %u\n",
+			   conf->pss_idle_data[index].name,
+			   pss_idle[index]);
+	}
+
+	seq_puts(s, "\nPCS_STATUS Code\n");
+	seq_puts(s, "0:C0 1:C1 2:C1_DN_WT_DEV 3:C2 4:C2_WT_DE_MEM_UP\n");
+	seq_puts(s, "5:C2_WT_DE_MEM_DOWN 6:C2_UP_WT_DEV 7:C2_DN 8:C2_VOA\n");
+	seq_puts(s, "9:C2_VOA_UP 10:S0IX_PRE 11:S0IX\n");
+
+	return 0;
+}
+
+static int telem_soc_state_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, telem_soc_states_show, inode->i_private);
+}
+
+static const struct file_operations telem_socstate_ops = {
+	.open		= telem_soc_state_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int telem_pss_trc_verb_show(struct seq_file *s, void *unused)
+{
+	u32 verbosity;
+	int err;
+
+	err = telemetry_get_trace_verbosity(TELEM_PSS, &verbosity);
+	if (err) {
+		pr_err("Get PSS Trace Verbosity Failed with Error %d\n", err);
+		return -EFAULT;
+	}
+
+	seq_printf(s, "PSS Trace Verbosity %u\n", verbosity);
+	return 0;
+}
+
+static ssize_t telem_pss_trc_verb_write(struct file *file,
+					const char __user *userbuf,
+					size_t count, loff_t *ppos)
+{
+	u32 verbosity;
+	int err;
+
+	if (kstrtou32_from_user(userbuf, count, 0, &verbosity))
+		return -EFAULT;
+
+	err = telemetry_set_trace_verbosity(TELEM_PSS, verbosity);
+	if (err) {
+		pr_err("Changing PSS Trace Verbosity Failed. Error %d\n", err);
+		count = err;
+	}
+
+	return count;
+}
+
+static int telem_pss_trc_verb_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, telem_pss_trc_verb_show, inode->i_private);
+}
+
+static const struct file_operations telem_pss_trc_verb_ops = {
+	.open		= telem_pss_trc_verb_open,
+	.read		= seq_read,
+	.write		= telem_pss_trc_verb_write,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+
+static int telem_ioss_trc_verb_show(struct seq_file *s, void *unused)
+{
+	u32 verbosity;
+	int err;
+
+	err = telemetry_get_trace_verbosity(TELEM_IOSS, &verbosity);
+	if (err) {
+		pr_err("Get IOSS Trace Verbosity Failed with Error %d\n", err);
+		return -EFAULT;
+	}
+
+	seq_printf(s, "IOSS Trace Verbosity %u\n", verbosity);
+	return 0;
+}
+
+static ssize_t telem_ioss_trc_verb_write(struct file *file,
+					 const char __user *userbuf,
+					 size_t count, loff_t *ppos)
+{
+	u32 verbosity;
+	int err;
+
+	if (kstrtou32_from_user(userbuf, count, 0, &verbosity))
+		return -EFAULT;
+
+	err = telemetry_set_trace_verbosity(TELEM_IOSS, verbosity);
+	if (err) {
+		pr_err("Changing IOSS Trace Verbosity Failed. Error %d\n", err);
+		count = err;
+	}
+
+	return count;
+}
+
+static int telem_ioss_trc_verb_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, telem_ioss_trc_verb_show, inode->i_private);
+}
+
+static const struct file_operations telem_ioss_trc_verb_ops = {
+	.open		= telem_ioss_trc_verb_open,
+	.read		= seq_read,
+	.write		= telem_ioss_trc_verb_write,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+#ifdef CONFIG_PM_SLEEP
+static int pm_suspend_prep_cb(void)
+{
+	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	struct telemetry_debugfs_conf *conf = debugfs_conf;
+	int ret, index;
+
+	ret = telemetry_raw_read_eventlog(TELEM_IOSS, evtlog,
+			TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (ret < 0) {
+		suspend_prep_ok = 0;
+		goto out;
+	}
+
+	for (index = 0; index < ret; index++) {
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_occ_id,
+					   suspend_shlw_ctr_temp);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_occ_id,
+					   suspend_deep_ctr_temp);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_res_id,
+					   suspend_shlw_res_temp);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_res_id,
+					   suspend_deep_res_temp);
+	}
+	suspend_prep_ok = 1;
+out:
+	return NOTIFY_OK;
+}
+
+static int pm_suspend_exit_cb(void)
+{
+	struct telemetry_evtlog evtlog[TELEM_MAX_OS_ALLOCATED_EVENTS];
+	static u32 suspend_shlw_ctr_exit, suspend_deep_ctr_exit;
+	static u64 suspend_shlw_res_exit, suspend_deep_res_exit;
+	struct telemetry_debugfs_conf *conf = debugfs_conf;
+	int ret, index;
+
+	if (!suspend_prep_ok)
+		goto out;
+
+	ret = telemetry_raw_read_eventlog(TELEM_IOSS, evtlog,
+					  TELEM_MAX_OS_ALLOCATED_EVENTS);
+	if (ret < 0)
+		goto out;
+
+	for (index = 0; index < ret; index++) {
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_occ_id,
+					   suspend_shlw_ctr_exit);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_occ_id,
+					   suspend_deep_ctr_exit);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_shlw_res_id,
+					   suspend_shlw_res_exit);
+
+		TELEM_CHECK_AND_PARSE_CTRS(conf->s0ix_deep_res_id,
+					   suspend_deep_res_exit);
+	}
+
+	if ((suspend_shlw_ctr_exit < suspend_shlw_ctr_temp) ||
+	    (suspend_deep_ctr_exit < suspend_deep_ctr_temp) ||
+	    (suspend_shlw_res_exit < suspend_shlw_res_temp) ||
+	    (suspend_deep_res_exit < suspend_deep_res_temp)) {
+		pr_err("Wrong s0ix counters detected\n");
+		goto out;
+	}
+
+	suspend_shlw_ctr_exit -= suspend_shlw_ctr_temp;
+	suspend_deep_ctr_exit -= suspend_deep_ctr_temp;
+	suspend_shlw_res_exit -= suspend_shlw_res_temp;
+	suspend_deep_res_exit -= suspend_deep_res_temp;
+
+	if (suspend_shlw_ctr_exit == 1) {
+		conf->suspend_stats.shlw_ctr +=
+		suspend_shlw_ctr_exit;
+
+		conf->suspend_stats.shlw_res +=
+		suspend_shlw_res_exit;
+	}
+	/* Shallow Wakes Case */
+	else if (suspend_shlw_ctr_exit > 1) {
+		conf->suspend_stats.shlw_swake_ctr +=
+		suspend_shlw_ctr_exit;
+
+		conf->suspend_stats.shlw_swake_res +=
+		suspend_shlw_res_exit;
+	}
+
+	if (suspend_deep_ctr_exit == 1) {
+		conf->suspend_stats.deep_ctr +=
+		suspend_deep_ctr_exit;
+
+		conf->suspend_stats.deep_res +=
+		suspend_deep_res_exit;
+	}
+
+	/* Shallow Wakes Case */
+	else if (suspend_deep_ctr_exit > 1) {
+		conf->suspend_stats.deep_swake_ctr +=
+		suspend_deep_ctr_exit;
+
+		conf->suspend_stats.deep_swake_res +=
+		suspend_deep_res_exit;
+	}
+
+out:
+	suspend_prep_ok = 0;
+	return NOTIFY_OK;
+}
+
+static int pm_notification(struct notifier_block *this,
+			   unsigned long event, void *ptr)
+{
+	switch (event) {
+	case PM_SUSPEND_PREPARE:
+		return pm_suspend_prep_cb();
+	case PM_POST_SUSPEND:
+		return pm_suspend_exit_cb();
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block pm_notifier = {
+	.notifier_call = pm_notification,
+};
+#endif /* CONFIG_PM_SLEEP */
+
+static int __init telemetry_debugfs_init(void)
+{
+	const struct x86_cpu_id *id;
+	int err = -ENOMEM;
+	struct dentry *f;
+
+	/* Only APL supported for now */
+	id = x86_match_cpu(telemetry_debugfs_cpu_ids);
+	if (!id)
+		return -ENODEV;
+
+	debugfs_conf = (struct telemetry_debugfs_conf *)id->driver_data;
+
+	err = telemetry_pltconfig_valid();
+	if (err < 0)
+		return -ENODEV;
+
+	err = telemetry_debugfs_check_evts();
+	if (err < 0)
+		return -EINVAL;
+
+
+#ifdef CONFIG_PM_SLEEP
+	register_pm_notifier(&pm_notifier);
+#endif /* CONFIG_PM_SLEEP */
+
+	debugfs_conf->telemetry_dbg_dir = debugfs_create_dir("telemetry", NULL);
+	if (!debugfs_conf->telemetry_dbg_dir)
+		return -ENOMEM;
+
+	f = debugfs_create_file("pss_info", S_IFREG | S_IRUGO,
+				debugfs_conf->telemetry_dbg_dir, NULL,
+				&telem_pss_ops);
+	if (!f) {
+		pr_err("pss_sample_info debugfs register failed\n");
+		goto out;
+	}
+
+	f = debugfs_create_file("ioss_info", S_IFREG | S_IRUGO,
+				debugfs_conf->telemetry_dbg_dir, NULL,
+				&telem_ioss_ops);
+	if (!f) {
+		pr_err("ioss_sample_info debugfs register failed\n");
+		goto out;
+	}
+
+	f = debugfs_create_file("soc_states", S_IFREG | S_IRUGO,
+				debugfs_conf->telemetry_dbg_dir,
+				NULL, &telem_socstate_ops);
+	if (!f) {
+		pr_err("ioss_sample_info debugfs register failed\n");
+		goto out;
+	}
+
+	f = debugfs_create_file("pss_trace_verbosity", S_IFREG | S_IRUGO,
+				debugfs_conf->telemetry_dbg_dir, NULL,
+				&telem_pss_trc_verb_ops);
+	if (!f) {
+		pr_err("pss_trace_verbosity debugfs register failed\n");
+		goto out;
+	}
+
+	f = debugfs_create_file("ioss_trace_verbosity", S_IFREG | S_IRUGO,
+				debugfs_conf->telemetry_dbg_dir, NULL,
+				&telem_ioss_trc_verb_ops);
+	if (!f) {
+		pr_err("ioss_trace_verbosity debugfs register failed\n");
+		goto out;
+	}
+
+	return 0;
+
+out:
+	debugfs_remove_recursive(debugfs_conf->telemetry_dbg_dir);
+	debugfs_conf->telemetry_dbg_dir = NULL;
+
+	return err;
+}
+
+static void __exit telemetry_debugfs_exit(void)
+{
+	debugfs_remove_recursive(debugfs_conf->telemetry_dbg_dir);
+	debugfs_conf->telemetry_dbg_dir = NULL;
+}
+
+late_initcall(telemetry_debugfs_init);
+module_exit(telemetry_debugfs_exit);
+
+MODULE_AUTHOR("Souvik Kumar Chakravarty <souvik.k.chakravarty@intel.com>");
+MODULE_DESCRIPTION("Intel SoC Telemetry debugfs Interface");
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/intel_telemetry_pltdrv.c b/drivers/platform/x86/intel_telemetry_pltdrv.c
new file mode 100644
index 000000000000..f97019b0106f
--- /dev/null
+++ b/drivers/platform/x86/intel_telemetry_pltdrv.c
@@ -0,0 +1,1206 @@
+/*
+ * Intel SOC Telemetry Platform Driver: Currently supports APL
+ * Copyright (c) 2015, Intel Corporation.
+ * All Rights Reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * 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 provides the platform specific telemetry implementation for APL.
+ * It used the PUNIT and PMC IPC interfaces for configuring the counters.
+ * The accumulated results are fetched from SRAM.
+ */
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
+#include <linux/pci.h>
+#include <linux/suspend.h>
+#include <linux/platform_device.h>
+
+#include <asm/cpu_device_id.h>
+#include <asm/intel_pmc_ipc.h>
+#include <asm/intel_punit_ipc.h>
+#include <asm/intel_telemetry.h>
+
+#define DRIVER_NAME	"intel_telemetry"
+#define DRIVER_VERSION	"1.0.0"
+
+#define TELEM_TRC_VERBOSITY_MASK	0x3
+
+#define TELEM_MIN_PERIOD(x)		((x) & 0x7F0000)
+#define TELEM_MAX_PERIOD(x)		((x) & 0x7F000000)
+#define TELEM_SAMPLE_PERIOD_INVALID(x)	((x) & (BIT(7)))
+#define TELEM_CLEAR_SAMPLE_PERIOD(x)	((x) &= ~0x7F)
+
+#define TELEM_SAMPLING_DEFAULT_PERIOD	0xD
+
+#define TELEM_MAX_EVENTS_SRAM		28
+#define TELEM_MAX_OS_ALLOCATED_EVENTS	20
+#define TELEM_SSRAM_STARTTIME_OFFSET	8
+#define TELEM_SSRAM_EVTLOG_OFFSET	16
+
+#define IOSS_TELEM_EVENT_READ		0x0
+#define IOSS_TELEM_EVENT_WRITE		0x1
+#define IOSS_TELEM_INFO_READ		0x2
+#define IOSS_TELEM_TRACE_CTL_READ	0x5
+#define IOSS_TELEM_TRACE_CTL_WRITE	0x6
+#define IOSS_TELEM_EVENT_CTL_READ	0x7
+#define IOSS_TELEM_EVENT_CTL_WRITE	0x8
+#define IOSS_TELEM_EVT_CTRL_WRITE_SIZE	0x4
+#define IOSS_TELEM_READ_WORD		0x1
+#define IOSS_TELEM_WRITE_FOURBYTES	0x4
+#define IOSS_TELEM_EVT_WRITE_SIZE	0x3
+
+#define TELEM_INFO_SRAMEVTS_MASK	0xFF00
+#define TELEM_INFO_SRAMEVTS_SHIFT	0x8
+#define TELEM_SSRAM_READ_TIMEOUT	10
+
+#define TELEM_INFO_NENABLES_MASK	0xFF
+#define TELEM_EVENT_ENABLE		0x8000
+
+#define TELEM_MASK_BIT			1
+#define TELEM_MASK_BYTE			0xFF
+#define BYTES_PER_LONG			8
+#define TELEM_MASK_PCS_STATE		0xF
+
+#define TELEM_DISABLE(x)		((x) &= ~(BIT(31)))
+#define TELEM_CLEAR_EVENTS(x)		((x) |= (BIT(30)))
+#define TELEM_ENABLE_SRAM_EVT_TRACE(x)	((x) &= ~(BIT(30) | BIT(24)))
+#define TELEM_ENABLE_PERIODIC(x)	((x) |= (BIT(23) | BIT(31) | BIT(7)))
+#define TELEM_EXTRACT_VERBOSITY(x, y)	((y) = (((x) >> 27) & 0x3))
+#define TELEM_CLEAR_VERBOSITY_BITS(x)	((x) &= ~(BIT(27) | BIT(28)))
+#define TELEM_SET_VERBOSITY_BITS(x, y)	((x) |= ((y) << 27))
+
+#define TELEM_CPU(model, data) \
+	{ X86_VENDOR_INTEL, 6, model, X86_FEATURE_MWAIT, (unsigned long)&data }
+
+enum telemetry_action {
+	TELEM_UPDATE = 0,
+	TELEM_ADD,
+	TELEM_RESET,
+	TELEM_ACTION_NONE
+};
+
+struct telem_ssram_region {
+	u64 timestamp;
+	u64 start_time;
+	u64 events[TELEM_MAX_EVENTS_SRAM];
+};
+
+static struct telemetry_plt_config *telm_conf;
+
+/*
+ * The following counters are programmed by default during setup.
+ * Only 20 allocated to kernel driver
+ */
+static struct telemetry_evtmap
+	telemetry_apl_ioss_default_events[TELEM_MAX_OS_ALLOCATED_EVENTS] = {
+	{"SOC_S0IX_TOTAL_RES",			0x4800},
+	{"SOC_S0IX_TOTAL_OCC",			0x4000},
+	{"SOC_S0IX_SHALLOW_RES",		0x4801},
+	{"SOC_S0IX_SHALLOW_OCC",		0x4001},
+	{"SOC_S0IX_DEEP_RES",			0x4802},
+	{"SOC_S0IX_DEEP_OCC",			0x4002},
+	{"PMC_POWER_GATE",			0x5818},
+	{"PMC_D3_STATES",			0x5819},
+	{"PMC_D0I3_STATES",			0x581A},
+	{"PMC_S0IX_WAKE_REASON_GPIO",		0x6000},
+	{"PMC_S0IX_WAKE_REASON_TIMER",		0x6001},
+	{"PMC_S0IX_WAKE_REASON_VNNREQ",         0x6002},
+	{"PMC_S0IX_WAKE_REASON_LOWPOWER",       0x6003},
+	{"PMC_S0IX_WAKE_REASON_EXTERNAL",       0x6004},
+	{"PMC_S0IX_WAKE_REASON_MISC",           0x6005},
+	{"PMC_S0IX_BLOCKING_IPS_D3_D0I3",       0x6006},
+	{"PMC_S0IX_BLOCKING_IPS_PG",            0x6007},
+	{"PMC_S0IX_BLOCKING_MISC_IPS_PG",       0x6008},
+	{"PMC_S0IX_BLOCK_IPS_VNN_REQ",          0x6009},
+	{"PMC_S0IX_BLOCK_IPS_CLOCKS",           0x600B},
+};
+
+
+static struct telemetry_evtmap
+	telemetry_apl_pss_default_events[TELEM_MAX_OS_ALLOCATED_EVENTS] = {
+	{"IA_CORE0_C6_RES",			0x0400},
+	{"IA_CORE0_C6_CTR",			0x0000},
+	{"IA_MODULE0_C7_RES",			0x0410},
+	{"IA_MODULE0_C7_CTR",			0x000E},
+	{"IA_C0_RES",				0x0805},
+	{"PCS_LTR",				0x2801},
+	{"PSTATES",				0x2802},
+	{"SOC_S0I3_RES",			0x0409},
+	{"SOC_S0I3_CTR",			0x000A},
+	{"PCS_S0I3_CTR",			0x0009},
+	{"PCS_C1E_RES",				0x041A},
+	{"PCS_IDLE_STATUS",			0x2806},
+	{"IA_PERF_LIMITS",			0x280B},
+	{"GT_PERF_LIMITS",			0x280C},
+	{"PCS_WAKEUP_S0IX_CTR",			0x0030},
+	{"PCS_IDLE_BLOCKED",			0x2C00},
+	{"PCS_S0IX_BLOCKED",			0x2C01},
+	{"PCS_S0IX_WAKE_REASONS",		0x2C02},
+	{"PCS_LTR_BLOCKING",			0x2C03},
+	{"PC2_AND_MEM_SHALLOW_IDLE_RES",	0x1D40},
+};
+
+/* APL specific Data */
+static struct telemetry_plt_config telem_apl_config = {
+	.pss_config = {
+		.telem_evts = telemetry_apl_pss_default_events,
+	},
+	.ioss_config = {
+		.telem_evts = telemetry_apl_ioss_default_events,
+	},
+};
+
+static const struct x86_cpu_id telemetry_cpu_ids[] = {
+	TELEM_CPU(0x5c, telem_apl_config),
+	{}
+};
+
+MODULE_DEVICE_TABLE(x86cpu, telemetry_cpu_ids);
+
+static inline int telem_get_unitconfig(enum telemetry_unit telem_unit,
+				     struct telemetry_unit_config **unit_config)
+{
+	if (telem_unit == TELEM_PSS)
+		*unit_config = &(telm_conf->pss_config);
+	else if (telem_unit == TELEM_IOSS)
+		*unit_config = &(telm_conf->ioss_config);
+	else
+		return -EINVAL;
+
+	return 0;
+
+}
+
+static int telemetry_check_evtid(enum telemetry_unit telem_unit,
+				 u32 *evtmap, u8 len,
+				 enum telemetry_action action)
+{
+	struct telemetry_unit_config *unit_config;
+	int ret;
+
+	ret = telem_get_unitconfig(telem_unit, &unit_config);
+	if (ret < 0)
+		return ret;
+
+	switch (action) {
+	case TELEM_RESET:
+		if (len > TELEM_MAX_EVENTS_SRAM)
+			return -EINVAL;
+
+		break;
+
+	case TELEM_UPDATE:
+		if (len > TELEM_MAX_EVENTS_SRAM)
+			return -EINVAL;
+
+		if ((len > 0) && (evtmap == NULL))
+			return -EINVAL;
+
+		break;
+
+	case TELEM_ADD:
+		if ((len + unit_config->ssram_evts_used) >
+		    TELEM_MAX_EVENTS_SRAM)
+			return -EINVAL;
+
+		if ((len > 0) && (evtmap == NULL))
+			return -EINVAL;
+
+		break;
+
+	default:
+		pr_err("Unknown Telemetry action Specified %d\n", action);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+
+static inline int telemetry_plt_config_ioss_event(u32 evt_id, int index)
+{
+	u32 write_buf;
+	int ret;
+
+	write_buf = evt_id | TELEM_EVENT_ENABLE;
+	write_buf <<= BITS_PER_BYTE;
+	write_buf |= index;
+
+	ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				    IOSS_TELEM_EVENT_WRITE, (u8 *)&write_buf,
+				    IOSS_TELEM_EVT_WRITE_SIZE, NULL, 0);
+
+	return ret;
+}
+
+static inline int telemetry_plt_config_pss_event(u32 evt_id, int index)
+{
+	u32 write_buf;
+	int ret;
+
+	write_buf = evt_id | TELEM_EVENT_ENABLE;
+	ret = intel_punit_ipc_command(IPC_PUNIT_BIOS_WRITE_TELE_EVENT,
+				      index, 0, &write_buf, NULL);
+
+	return ret;
+}
+
+static int telemetry_setup_iossevtconfig(struct telemetry_evtconfig evtconfig,
+					 enum telemetry_action action)
+{
+	u8 num_ioss_evts, ioss_period;
+	int ret, index, idx;
+	u32 *ioss_evtmap;
+	u32 telem_ctrl;
+
+	num_ioss_evts = evtconfig.num_evts;
+	ioss_period = evtconfig.period;
+	ioss_evtmap = evtconfig.evtmap;
+
+	/* Get telemetry EVENT CTL */
+	ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				    IOSS_TELEM_EVENT_CTL_READ, NULL, 0,
+				    &telem_ctrl, IOSS_TELEM_READ_WORD);
+	if (ret) {
+		pr_err("IOSS TELEM_CTRL Read Failed\n");
+		return ret;
+	}
+
+	/* Disable Telemetry */
+	TELEM_DISABLE(telem_ctrl);
+
+	ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				    IOSS_TELEM_EVENT_CTL_WRITE,
+				    (u8 *)&telem_ctrl,
+				    IOSS_TELEM_EVT_CTRL_WRITE_SIZE,
+				    NULL, 0);
+	if (ret) {
+		pr_err("IOSS TELEM_CTRL Event Disable Write Failed\n");
+		return ret;
+	}
+
+
+	/* Reset Everything */
+	if (action == TELEM_RESET) {
+		/* Clear All Events */
+		TELEM_CLEAR_EVENTS(telem_ctrl);
+
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+					    IOSS_TELEM_EVENT_CTL_WRITE,
+					    (u8 *)&telem_ctrl,
+					    IOSS_TELEM_EVT_CTRL_WRITE_SIZE,
+					    NULL, 0);
+		if (ret) {
+			pr_err("IOSS TELEM_CTRL Event Disable Write Failed\n");
+			return ret;
+		}
+		telm_conf->ioss_config.ssram_evts_used = 0;
+
+		/* Configure Events */
+		for (idx = 0; idx < num_ioss_evts; idx++) {
+			if (telemetry_plt_config_ioss_event(
+			    telm_conf->ioss_config.telem_evts[idx].evt_id,
+			    idx)) {
+				pr_err("IOSS TELEM_RESET Fail for data: %x\n",
+				telm_conf->ioss_config.telem_evts[idx].evt_id);
+				continue;
+			}
+			telm_conf->ioss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Re-Configure Everything */
+	if (action == TELEM_UPDATE) {
+		/* Clear All Events */
+		TELEM_CLEAR_EVENTS(telem_ctrl);
+
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+					    IOSS_TELEM_EVENT_CTL_WRITE,
+					    (u8 *)&telem_ctrl,
+					    IOSS_TELEM_EVT_CTRL_WRITE_SIZE,
+					    NULL, 0);
+		if (ret) {
+			pr_err("IOSS TELEM_CTRL Event Disable Write Failed\n");
+			return ret;
+		}
+		telm_conf->ioss_config.ssram_evts_used = 0;
+
+		/* Configure Events */
+		for (index = 0; index < num_ioss_evts; index++) {
+			telm_conf->ioss_config.telem_evts[index].evt_id =
+			ioss_evtmap[index];
+
+			if (telemetry_plt_config_ioss_event(
+			    telm_conf->ioss_config.telem_evts[index].evt_id,
+			    index)) {
+				pr_err("IOSS TELEM_UPDATE Fail for Evt%x\n",
+					ioss_evtmap[index]);
+				continue;
+			}
+			telm_conf->ioss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Add some Events */
+	if (action == TELEM_ADD) {
+		/* Configure Events */
+		for (index = telm_conf->ioss_config.ssram_evts_used, idx = 0;
+		     idx < num_ioss_evts; index++, idx++) {
+			telm_conf->ioss_config.telem_evts[index].evt_id =
+			ioss_evtmap[idx];
+
+			if (telemetry_plt_config_ioss_event(
+			    telm_conf->ioss_config.telem_evts[index].evt_id,
+			    index)) {
+				pr_err("IOSS TELEM_ADD Fail for Event %x\n",
+					ioss_evtmap[idx]);
+				continue;
+			}
+			telm_conf->ioss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Enable Periodic Telemetry Events and enable SRAM trace */
+	TELEM_CLEAR_SAMPLE_PERIOD(telem_ctrl);
+	TELEM_ENABLE_SRAM_EVT_TRACE(telem_ctrl);
+	TELEM_ENABLE_PERIODIC(telem_ctrl);
+	telem_ctrl |= ioss_period;
+
+	ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				    IOSS_TELEM_EVENT_CTL_WRITE,
+				    (u8 *)&telem_ctrl,
+				    IOSS_TELEM_EVT_CTRL_WRITE_SIZE, NULL, 0);
+	if (ret) {
+		pr_err("IOSS TELEM_CTRL Event Enable Write Failed\n");
+		return ret;
+	}
+
+	telm_conf->ioss_config.curr_period = ioss_period;
+
+	return 0;
+}
+
+
+static int telemetry_setup_pssevtconfig(struct telemetry_evtconfig evtconfig,
+					enum telemetry_action action)
+{
+	u8 num_pss_evts, pss_period;
+	int ret, index, idx;
+	u32 *pss_evtmap;
+	u32 telem_ctrl;
+
+	num_pss_evts = evtconfig.num_evts;
+	pss_period = evtconfig.period;
+	pss_evtmap = evtconfig.evtmap;
+
+	/* PSS Config */
+	/* Get telemetry EVENT CTL */
+	ret = intel_punit_ipc_command(IPC_PUNIT_BIOS_READ_TELE_EVENT_CTRL,
+				      0, 0, NULL, &telem_ctrl);
+	if (ret) {
+		pr_err("PSS TELEM_CTRL Read Failed\n");
+		return ret;
+	}
+
+	/* Disable Telemetry */
+	TELEM_DISABLE(telem_ctrl);
+	ret = intel_punit_ipc_command(IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				      0, 0, &telem_ctrl, NULL);
+	if (ret) {
+		pr_err("PSS TELEM_CTRL Event Disable Write Failed\n");
+		return ret;
+	}
+
+	/* Reset Everything */
+	if (action == TELEM_RESET) {
+		/* Clear All Events */
+		TELEM_CLEAR_EVENTS(telem_ctrl);
+
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				0, 0, &telem_ctrl, NULL);
+		if (ret) {
+			pr_err("PSS TELEM_CTRL Event Disable Write Failed\n");
+			return ret;
+		}
+		telm_conf->pss_config.ssram_evts_used = 0;
+		/* Configure Events */
+		for (idx = 0; idx < num_pss_evts; idx++) {
+			if (telemetry_plt_config_pss_event(
+			    telm_conf->pss_config.telem_evts[idx].evt_id,
+			    idx)) {
+				pr_err("PSS TELEM_RESET Fail for Event %x\n",
+				telm_conf->pss_config.telem_evts[idx].evt_id);
+				continue;
+			}
+			telm_conf->pss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Re-Configure Everything */
+	if (action == TELEM_UPDATE) {
+		/* Clear All Events */
+		TELEM_CLEAR_EVENTS(telem_ctrl);
+
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				0, 0, &telem_ctrl, NULL);
+		if (ret) {
+			pr_err("PSS TELEM_CTRL Event Disable Write Failed\n");
+			return ret;
+		}
+		telm_conf->pss_config.ssram_evts_used = 0;
+
+		/* Configure Events */
+		for (index = 0; index < num_pss_evts; index++) {
+			telm_conf->pss_config.telem_evts[index].evt_id =
+			pss_evtmap[index];
+
+			if (telemetry_plt_config_pss_event(
+			    telm_conf->pss_config.telem_evts[index].evt_id,
+			    index)) {
+				pr_err("PSS TELEM_UPDATE Fail for Event %x\n",
+					pss_evtmap[index]);
+				continue;
+			}
+			telm_conf->pss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Add some Events */
+	if (action == TELEM_ADD) {
+		/* Configure Events */
+		for (index = telm_conf->pss_config.ssram_evts_used, idx = 0;
+		     idx < num_pss_evts; index++, idx++) {
+
+			telm_conf->pss_config.telem_evts[index].evt_id =
+			pss_evtmap[idx];
+
+			if (telemetry_plt_config_pss_event(
+			    telm_conf->pss_config.telem_evts[index].evt_id,
+			    index)) {
+				pr_err("PSS TELEM_ADD Fail for Event %x\n",
+					pss_evtmap[idx]);
+				continue;
+			}
+			telm_conf->pss_config.ssram_evts_used++;
+		}
+	}
+
+	/* Enable Periodic Telemetry Events and enable SRAM trace */
+	TELEM_CLEAR_SAMPLE_PERIOD(telem_ctrl);
+	TELEM_ENABLE_SRAM_EVT_TRACE(telem_ctrl);
+	TELEM_ENABLE_PERIODIC(telem_ctrl);
+	telem_ctrl |= pss_period;
+
+	ret = intel_punit_ipc_command(IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				      0, 0, &telem_ctrl, NULL);
+	if (ret) {
+		pr_err("PSS TELEM_CTRL Event Enable Write Failed\n");
+		return ret;
+	}
+
+	telm_conf->pss_config.curr_period = pss_period;
+
+	return 0;
+}
+
+static int telemetry_setup_evtconfig(struct telemetry_evtconfig pss_evtconfig,
+				     struct telemetry_evtconfig ioss_evtconfig,
+				     enum telemetry_action action)
+{
+	int ret;
+
+	mutex_lock(&(telm_conf->telem_lock));
+
+	if ((action == TELEM_UPDATE) && (telm_conf->telem_in_use)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	ret = telemetry_check_evtid(TELEM_PSS, pss_evtconfig.evtmap,
+				    pss_evtconfig.num_evts, action);
+	if (ret)
+		goto out;
+
+	ret = telemetry_check_evtid(TELEM_IOSS, ioss_evtconfig.evtmap,
+				    ioss_evtconfig.num_evts, action);
+	if (ret)
+		goto out;
+
+	if (ioss_evtconfig.num_evts) {
+		ret = telemetry_setup_iossevtconfig(ioss_evtconfig, action);
+		if (ret)
+			goto out;
+	}
+
+	if (pss_evtconfig.num_evts) {
+		ret = telemetry_setup_pssevtconfig(pss_evtconfig, action);
+		if (ret)
+			goto out;
+	}
+
+	if ((action == TELEM_UPDATE) || (action == TELEM_ADD))
+		telm_conf->telem_in_use = true;
+	else
+		telm_conf->telem_in_use = false;
+
+out:
+	mutex_unlock(&(telm_conf->telem_lock));
+	return ret;
+}
+
+static int telemetry_setup(struct platform_device *pdev)
+{
+	struct telemetry_evtconfig pss_evtconfig, ioss_evtconfig;
+	u32 read_buf, events, event_regs;
+	int ret;
+
+	ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY, IOSS_TELEM_INFO_READ,
+				    NULL, 0, &read_buf, IOSS_TELEM_READ_WORD);
+	if (ret) {
+		dev_err(&pdev->dev, "IOSS TELEM_INFO Read Failed\n");
+		return ret;
+	}
+
+	/* Get telemetry Info */
+	events = (read_buf & TELEM_INFO_SRAMEVTS_MASK) >>
+		  TELEM_INFO_SRAMEVTS_SHIFT;
+	event_regs = read_buf & TELEM_INFO_NENABLES_MASK;
+	if ((events < TELEM_MAX_EVENTS_SRAM) ||
+	    (event_regs < TELEM_MAX_EVENTS_SRAM)) {
+		dev_err(&pdev->dev, "IOSS:Insufficient Space for SRAM Trace\n");
+		dev_err(&pdev->dev, "SRAM Events %d; Event Regs %d\n",
+			events, event_regs);
+		return -ENOMEM;
+	}
+
+	telm_conf->ioss_config.min_period = TELEM_MIN_PERIOD(read_buf);
+	telm_conf->ioss_config.max_period = TELEM_MAX_PERIOD(read_buf);
+
+	/* PUNIT Mailbox Setup */
+	ret = intel_punit_ipc_command(IPC_PUNIT_BIOS_READ_TELE_INFO, 0, 0,
+				      NULL, &read_buf);
+	if (ret) {
+		dev_err(&pdev->dev, "PSS TELEM_INFO Read Failed\n");
+		return ret;
+	}
+
+	/* Get telemetry Info */
+	events = (read_buf & TELEM_INFO_SRAMEVTS_MASK) >>
+		  TELEM_INFO_SRAMEVTS_SHIFT;
+	event_regs = read_buf & TELEM_INFO_SRAMEVTS_MASK;
+	if ((events < TELEM_MAX_EVENTS_SRAM) ||
+	    (event_regs < TELEM_MAX_EVENTS_SRAM)) {
+		dev_err(&pdev->dev, "PSS:Insufficient Space for SRAM Trace\n");
+		dev_err(&pdev->dev, "SRAM Events %d; Event Regs %d\n",
+			events, event_regs);
+		return -ENOMEM;
+	}
+
+	telm_conf->pss_config.min_period = TELEM_MIN_PERIOD(read_buf);
+	telm_conf->pss_config.max_period = TELEM_MAX_PERIOD(read_buf);
+
+	pss_evtconfig.evtmap = NULL;
+	pss_evtconfig.num_evts = TELEM_MAX_OS_ALLOCATED_EVENTS;
+	pss_evtconfig.period = TELEM_SAMPLING_DEFAULT_PERIOD;
+
+	ioss_evtconfig.evtmap = NULL;
+	ioss_evtconfig.num_evts = TELEM_MAX_OS_ALLOCATED_EVENTS;
+	ioss_evtconfig.period = TELEM_SAMPLING_DEFAULT_PERIOD;
+
+	ret = telemetry_setup_evtconfig(pss_evtconfig, ioss_evtconfig,
+					TELEM_RESET);
+	if (ret) {
+		dev_err(&pdev->dev, "TELEMTRY Setup Failed\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int telemetry_plt_update_events(struct telemetry_evtconfig pss_evtconfig,
+				struct telemetry_evtconfig ioss_evtconfig)
+{
+	int ret;
+
+	if ((pss_evtconfig.num_evts > 0) &&
+	    (TELEM_SAMPLE_PERIOD_INVALID(pss_evtconfig.period))) {
+		pr_err("PSS Sampling Period Out of Range\n");
+		return -EINVAL;
+	}
+
+	if ((ioss_evtconfig.num_evts > 0) &&
+	    (TELEM_SAMPLE_PERIOD_INVALID(ioss_evtconfig.period))) {
+		pr_err("IOSS Sampling Period Out of Range\n");
+		return -EINVAL;
+	}
+
+	ret = telemetry_setup_evtconfig(pss_evtconfig, ioss_evtconfig,
+					TELEM_UPDATE);
+	if (ret)
+		pr_err("TELEMTRY Config Failed\n");
+
+	return ret;
+}
+
+
+static int telemetry_plt_set_sampling_period(u8 pss_period, u8 ioss_period)
+{
+	u32 telem_ctrl = 0;
+	int ret;
+
+	mutex_lock(&(telm_conf->telem_lock));
+	if (ioss_period) {
+		if (TELEM_SAMPLE_PERIOD_INVALID(ioss_period)) {
+			pr_err("IOSS Sampling Period Out of Range\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		/* Get telemetry EVENT CTL */
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+					    IOSS_TELEM_EVENT_CTL_READ, NULL, 0,
+					    &telem_ctrl, IOSS_TELEM_READ_WORD);
+		if (ret) {
+			pr_err("IOSS TELEM_CTRL Read Failed\n");
+			goto out;
+		}
+
+		/* Disable Telemetry */
+		TELEM_DISABLE(telem_ctrl);
+
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+					    IOSS_TELEM_EVENT_CTL_WRITE,
+					    (u8 *)&telem_ctrl,
+					    IOSS_TELEM_EVT_CTRL_WRITE_SIZE,
+					    NULL, 0);
+		if (ret) {
+			pr_err("IOSS TELEM_CTRL Event Disable Write Failed\n");
+			goto out;
+		}
+
+		/* Enable Periodic Telemetry Events and enable SRAM trace */
+		TELEM_CLEAR_SAMPLE_PERIOD(telem_ctrl);
+		TELEM_ENABLE_SRAM_EVT_TRACE(telem_ctrl);
+		TELEM_ENABLE_PERIODIC(telem_ctrl);
+		telem_ctrl |= ioss_period;
+
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+					    IOSS_TELEM_EVENT_CTL_WRITE,
+					    (u8 *)&telem_ctrl,
+					    IOSS_TELEM_EVT_CTRL_WRITE_SIZE,
+					    NULL, 0);
+		if (ret) {
+			pr_err("IOSS TELEM_CTRL Event Enable Write Failed\n");
+			goto out;
+		}
+		telm_conf->ioss_config.curr_period = ioss_period;
+	}
+
+	if (pss_period) {
+		if (TELEM_SAMPLE_PERIOD_INVALID(pss_period)) {
+			pr_err("PSS Sampling Period Out of Range\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		/* Get telemetry EVENT CTL */
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_READ_TELE_EVENT_CTRL,
+				0, 0, NULL, &telem_ctrl);
+		if (ret) {
+			pr_err("PSS TELEM_CTRL Read Failed\n");
+			goto out;
+		}
+
+		/* Disable Telemetry */
+		TELEM_DISABLE(telem_ctrl);
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				0, 0, &telem_ctrl, NULL);
+		if (ret) {
+			pr_err("PSS TELEM_CTRL Event Disable Write Failed\n");
+			goto out;
+		}
+
+		/* Enable Periodic Telemetry Events and enable SRAM trace */
+		TELEM_CLEAR_SAMPLE_PERIOD(telem_ctrl);
+		TELEM_ENABLE_SRAM_EVT_TRACE(telem_ctrl);
+		TELEM_ENABLE_PERIODIC(telem_ctrl);
+		telem_ctrl |= pss_period;
+
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_WRITE_TELE_EVENT_CTRL,
+				0, 0, &telem_ctrl, NULL);
+		if (ret) {
+			pr_err("PSS TELEM_CTRL Event Enable Write Failed\n");
+			goto out;
+		}
+		telm_conf->pss_config.curr_period = pss_period;
+	}
+
+out:
+	mutex_unlock(&(telm_conf->telem_lock));
+	return ret;
+}
+
+
+static int telemetry_plt_get_sampling_period(u8 *pss_min_period,
+					     u8 *pss_max_period,
+					     u8 *ioss_min_period,
+					     u8 *ioss_max_period)
+{
+	*pss_min_period = telm_conf->pss_config.min_period;
+	*pss_max_period = telm_conf->pss_config.max_period;
+	*ioss_min_period = telm_conf->ioss_config.min_period;
+	*ioss_max_period = telm_conf->ioss_config.max_period;
+
+	return 0;
+}
+
+
+static int telemetry_plt_reset_events(void)
+{
+	struct telemetry_evtconfig pss_evtconfig, ioss_evtconfig;
+	int ret;
+
+	pss_evtconfig.evtmap = NULL;
+	pss_evtconfig.num_evts = TELEM_MAX_OS_ALLOCATED_EVENTS;
+	pss_evtconfig.period = TELEM_SAMPLING_DEFAULT_PERIOD;
+
+	ioss_evtconfig.evtmap = NULL;
+	ioss_evtconfig.num_evts = TELEM_MAX_OS_ALLOCATED_EVENTS;
+	ioss_evtconfig.period = TELEM_SAMPLING_DEFAULT_PERIOD;
+
+	ret = telemetry_setup_evtconfig(pss_evtconfig, ioss_evtconfig,
+					TELEM_RESET);
+	if (ret)
+		pr_err("TELEMTRY Reset Failed\n");
+
+	return ret;
+}
+
+
+static int telemetry_plt_get_eventconfig(struct telemetry_evtconfig *pss_config,
+					struct telemetry_evtconfig *ioss_config,
+					int pss_len, int ioss_len)
+{
+	u32 *pss_evtmap, *ioss_evtmap;
+	u32 index;
+
+	pss_evtmap = pss_config->evtmap;
+	ioss_evtmap = ioss_config->evtmap;
+
+	mutex_lock(&(telm_conf->telem_lock));
+	pss_config->num_evts = telm_conf->pss_config.ssram_evts_used;
+	ioss_config->num_evts = telm_conf->ioss_config.ssram_evts_used;
+
+	pss_config->period = telm_conf->pss_config.curr_period;
+	ioss_config->period = telm_conf->ioss_config.curr_period;
+
+	if ((pss_len < telm_conf->pss_config.ssram_evts_used) ||
+	    (ioss_len < telm_conf->ioss_config.ssram_evts_used)) {
+		mutex_unlock(&(telm_conf->telem_lock));
+		return -EINVAL;
+	}
+
+	for (index = 0; index < telm_conf->pss_config.ssram_evts_used;
+	     index++) {
+		pss_evtmap[index] =
+		telm_conf->pss_config.telem_evts[index].evt_id;
+	}
+
+	for (index = 0; index < telm_conf->ioss_config.ssram_evts_used;
+	     index++) {
+		ioss_evtmap[index] =
+		telm_conf->ioss_config.telem_evts[index].evt_id;
+	}
+
+	mutex_unlock(&(telm_conf->telem_lock));
+	return 0;
+}
+
+
+static int telemetry_plt_add_events(u8 num_pss_evts, u8 num_ioss_evts,
+				    u32 *pss_evtmap, u32 *ioss_evtmap)
+{
+	struct telemetry_evtconfig pss_evtconfig, ioss_evtconfig;
+	int ret;
+
+	pss_evtconfig.evtmap = pss_evtmap;
+	pss_evtconfig.num_evts = num_pss_evts;
+	pss_evtconfig.period = telm_conf->pss_config.curr_period;
+
+	ioss_evtconfig.evtmap = ioss_evtmap;
+	ioss_evtconfig.num_evts = num_ioss_evts;
+	ioss_evtconfig.period = telm_conf->ioss_config.curr_period;
+
+	ret = telemetry_setup_evtconfig(pss_evtconfig, ioss_evtconfig,
+					TELEM_ADD);
+	if (ret)
+		pr_err("TELEMTRY ADD Failed\n");
+
+	return ret;
+}
+
+static int telem_evtlog_read(enum telemetry_unit telem_unit,
+			     struct telem_ssram_region *ssram_region, u8 len)
+{
+	struct telemetry_unit_config *unit_config;
+	u64 timestamp_prev, timestamp_next;
+	int ret, index, timeout = 0;
+
+	ret = telem_get_unitconfig(telem_unit, &unit_config);
+	if (ret < 0)
+		return ret;
+
+	if (len > unit_config->ssram_evts_used)
+		len = unit_config->ssram_evts_used;
+
+	do {
+		timestamp_prev = readq(unit_config->regmap);
+		if (!timestamp_prev) {
+			pr_err("Ssram under update. Please Try Later\n");
+			return -EBUSY;
+		}
+
+		ssram_region->start_time = readq(unit_config->regmap +
+						 TELEM_SSRAM_STARTTIME_OFFSET);
+
+		for (index = 0; index < len; index++) {
+			ssram_region->events[index] =
+			readq(unit_config->regmap + TELEM_SSRAM_EVTLOG_OFFSET +
+			      BYTES_PER_LONG*index);
+		}
+
+		timestamp_next = readq(unit_config->regmap);
+		if (!timestamp_next) {
+			pr_err("Ssram under update. Please Try Later\n");
+			return -EBUSY;
+		}
+
+		if (timeout++ > TELEM_SSRAM_READ_TIMEOUT) {
+			pr_err("Timeout while reading Events\n");
+			return -EBUSY;
+		}
+
+	} while (timestamp_prev != timestamp_next);
+
+	ssram_region->timestamp = timestamp_next;
+
+	return len;
+}
+
+static int telemetry_plt_raw_read_eventlog(enum telemetry_unit telem_unit,
+					   struct telemetry_evtlog *evtlog,
+					   int len, int log_all_evts)
+{
+	int index, idx1, ret, readlen = len;
+	struct telem_ssram_region ssram_region;
+	struct telemetry_evtmap *evtmap;
+
+	switch (telem_unit)	{
+	case TELEM_PSS:
+		evtmap = telm_conf->pss_config.telem_evts;
+		break;
+
+	case TELEM_IOSS:
+		evtmap = telm_conf->ioss_config.telem_evts;
+		break;
+
+	default:
+		pr_err("Unknown Telemetry Unit Specified %d\n", telem_unit);
+		return -EINVAL;
+	}
+
+	if (!log_all_evts)
+		readlen = TELEM_MAX_EVENTS_SRAM;
+
+	ret = telem_evtlog_read(telem_unit, &ssram_region, readlen);
+	if (ret < 0)
+		return ret;
+
+	/* Invalid evt-id array specified via length mismatch */
+	if ((!log_all_evts) && (len > ret))
+		return -EINVAL;
+
+	if (log_all_evts)
+		for (index = 0; index < ret; index++) {
+			evtlog[index].telem_evtlog = ssram_region.events[index];
+			evtlog[index].telem_evtid = evtmap[index].evt_id;
+		}
+	else
+		for (index = 0, readlen = 0; (index < ret) && (readlen < len);
+		     index++) {
+			for (idx1 = 0; idx1 < len; idx1++) {
+				/* Elements matched */
+				if (evtmap[index].evt_id ==
+				    evtlog[idx1].telem_evtid) {
+					evtlog[idx1].telem_evtlog =
+					ssram_region.events[index];
+					readlen++;
+
+					break;
+				}
+			}
+		}
+
+	return readlen;
+}
+
+static int telemetry_plt_read_eventlog(enum telemetry_unit telem_unit,
+		struct telemetry_evtlog *evtlog, int len, int log_all_evts)
+{
+	int ret;
+
+	mutex_lock(&(telm_conf->telem_lock));
+	ret = telemetry_plt_raw_read_eventlog(telem_unit, evtlog,
+					      len, log_all_evts);
+	mutex_unlock(&(telm_conf->telem_lock));
+
+	return ret;
+}
+
+static int telemetry_plt_get_trace_verbosity(enum telemetry_unit telem_unit,
+					     u32 *verbosity)
+{
+	u32 temp = 0;
+	int ret;
+
+	if (verbosity == NULL)
+		return -EINVAL;
+
+	mutex_lock(&(telm_conf->telem_trace_lock));
+	switch (telem_unit) {
+	case TELEM_PSS:
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_READ_TELE_TRACE_CTRL,
+				0, 0, NULL, &temp);
+		if (ret) {
+			pr_err("PSS TRACE_CTRL Read Failed\n");
+			goto out;
+		}
+
+		break;
+
+	case TELEM_IOSS:
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				IOSS_TELEM_TRACE_CTL_READ, NULL, 0, &temp,
+				IOSS_TELEM_READ_WORD);
+		if (ret) {
+			pr_err("IOSS TRACE_CTL Read Failed\n");
+			goto out;
+		}
+
+		break;
+
+	default:
+		pr_err("Unknown Telemetry Unit Specified %d\n", telem_unit);
+		ret = -EINVAL;
+		break;
+	}
+	TELEM_EXTRACT_VERBOSITY(temp, *verbosity);
+
+out:
+	mutex_unlock(&(telm_conf->telem_trace_lock));
+	return ret;
+}
+
+static int telemetry_plt_set_trace_verbosity(enum telemetry_unit telem_unit,
+					     u32 verbosity)
+{
+	u32 temp = 0;
+	int ret;
+
+	verbosity &= TELEM_TRC_VERBOSITY_MASK;
+
+	mutex_lock(&(telm_conf->telem_trace_lock));
+	switch (telem_unit) {
+	case TELEM_PSS:
+		ret = intel_punit_ipc_command(
+				IPC_PUNIT_BIOS_WRITE_TELE_TRACE_CTRL,
+				0, 0, &verbosity, NULL);
+		if (ret) {
+			pr_err("PSS TRACE_CTRL Verbosity Set Failed\n");
+			goto out;
+		}
+		break;
+
+	case TELEM_IOSS:
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				IOSS_TELEM_TRACE_CTL_READ, NULL, 0, &temp,
+				IOSS_TELEM_READ_WORD);
+		if (ret) {
+			pr_err("IOSS TRACE_CTL Read Failed\n");
+			goto out;
+		}
+
+		TELEM_CLEAR_VERBOSITY_BITS(temp);
+		TELEM_SET_VERBOSITY_BITS(temp, verbosity);
+
+		ret = intel_pmc_ipc_command(PMC_IPC_PMC_TELEMTRY,
+				IOSS_TELEM_TRACE_CTL_WRITE, (u8 *)&temp,
+				IOSS_TELEM_WRITE_FOURBYTES, NULL, 0);
+		if (ret) {
+			pr_err("IOSS TRACE_CTL Verbosity Set Failed\n");
+			goto out;
+		}
+		break;
+
+	default:
+		pr_err("Unknown Telemetry Unit Specified %d\n", telem_unit);
+		ret = -EINVAL;
+		break;
+	}
+
+out:
+	mutex_unlock(&(telm_conf->telem_trace_lock));
+	return ret;
+}
+
+static struct telemetry_core_ops telm_pltops = {
+	.get_trace_verbosity = telemetry_plt_get_trace_verbosity,
+	.set_trace_verbosity = telemetry_plt_set_trace_verbosity,
+	.set_sampling_period = telemetry_plt_set_sampling_period,
+	.get_sampling_period = telemetry_plt_get_sampling_period,
+	.raw_read_eventlog = telemetry_plt_raw_read_eventlog,
+	.get_eventconfig = telemetry_plt_get_eventconfig,
+	.update_events = telemetry_plt_update_events,
+	.read_eventlog = telemetry_plt_read_eventlog,
+	.reset_events = telemetry_plt_reset_events,
+	.add_events = telemetry_plt_add_events,
+};
+
+static int telemetry_pltdrv_probe(struct platform_device *pdev)
+{
+	struct resource *res0 = NULL, *res1 = NULL;
+	const struct x86_cpu_id *id;
+	int size, ret = -ENOMEM;
+
+	id = x86_match_cpu(telemetry_cpu_ids);
+	if (!id)
+		return -ENODEV;
+
+	telm_conf = (struct telemetry_plt_config *)id->driver_data;
+
+	res0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res0) {
+		ret = -EINVAL;
+		goto out;
+	}
+	size = resource_size(res0);
+	if (!devm_request_mem_region(&pdev->dev, res0->start, size,
+				     pdev->name)) {
+		ret = -EBUSY;
+		goto out;
+	}
+	telm_conf->pss_config.ssram_base_addr = res0->start;
+	telm_conf->pss_config.ssram_size = size;
+
+	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	if (!res1) {
+		ret = -EINVAL;
+		goto out;
+	}
+	size = resource_size(res1);
+	if (!devm_request_mem_region(&pdev->dev, res1->start, size,
+				     pdev->name)) {
+		ret = -EBUSY;
+		goto out;
+	}
+
+	telm_conf->ioss_config.ssram_base_addr = res1->start;
+	telm_conf->ioss_config.ssram_size = size;
+
+	telm_conf->pss_config.regmap = ioremap_nocache(
+					telm_conf->pss_config.ssram_base_addr,
+					telm_conf->pss_config.ssram_size);
+	if (!telm_conf->pss_config.regmap) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	telm_conf->ioss_config.regmap = ioremap_nocache(
+				telm_conf->ioss_config.ssram_base_addr,
+				telm_conf->ioss_config.ssram_size);
+	if (!telm_conf->ioss_config.regmap) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	mutex_init(&telm_conf->telem_lock);
+	mutex_init(&telm_conf->telem_trace_lock);
+
+	ret = telemetry_setup(pdev);
+	if (ret)
+		goto out;
+
+	ret = telemetry_set_pltdata(&telm_pltops, telm_conf);
+	if (ret) {
+		dev_err(&pdev->dev, "TELEMTRY Set Pltops Failed.\n");
+		goto out;
+	}
+
+	return 0;
+
+out:
+	if (res0)
+		release_mem_region(res0->start, resource_size(res0));
+	if (res1)
+		release_mem_region(res1->start, resource_size(res1));
+	if (telm_conf->pss_config.regmap)
+		iounmap(telm_conf->pss_config.regmap);
+	if (telm_conf->ioss_config.regmap)
+		iounmap(telm_conf->ioss_config.regmap);
+	dev_err(&pdev->dev, "TELEMTRY Setup Failed.\n");
+
+	return ret;
+}
+
+static int telemetry_pltdrv_remove(struct platform_device *pdev)
+{
+	telemetry_clear_pltdata();
+	iounmap(telm_conf->pss_config.regmap);
+	iounmap(telm_conf->ioss_config.regmap);
+
+	return 0;
+}
+
+static struct platform_driver telemetry_soc_driver = {
+	.probe		= telemetry_pltdrv_probe,
+	.remove		= telemetry_pltdrv_remove,
+	.driver		= {
+		.name	= DRIVER_NAME,
+	},
+};
+
+static int __init telemetry_module_init(void)
+{
+	pr_info(DRIVER_NAME ": version %s loaded\n", DRIVER_VERSION);
+	return platform_driver_register(&telemetry_soc_driver);
+}
+
+static void __exit telemetry_module_exit(void)
+{
+	platform_driver_unregister(&telemetry_soc_driver);
+}
+
+device_initcall(telemetry_module_init);
+module_exit(telemetry_module_exit);
+
+MODULE_AUTHOR("Souvik Kumar Chakravarty <souvik.k.chakravarty@intel.com>");
+MODULE_DESCRIPTION("Intel SoC Telemetry Platform Driver");
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL");
diff --git a/drivers/platform/x86/sony-laptop.c b/drivers/platform/x86/sony-laptop.c
index f73c29558cd3..e9caa347a9bf 100644
--- a/drivers/platform/x86/sony-laptop.c
+++ b/drivers/platform/x86/sony-laptop.c
@@ -1393,6 +1393,7 @@ static void sony_nc_function_setup(struct acpi_device *device,
 		case 0x0143:
 		case 0x014b:
 		case 0x014c:
+		case 0x0153:
 		case 0x0163:
 			result = sony_nc_kbd_backlight_setup(pf_device, handle);
 			if (result)
@@ -1490,6 +1491,7 @@ static void sony_nc_function_cleanup(struct platform_device *pd)
 		case 0x0143:
 		case 0x014b:
 		case 0x014c:
+		case 0x0153:
 		case 0x0163:
 			sony_nc_kbd_backlight_cleanup(pd, handle);
 			break;
@@ -1773,6 +1775,7 @@ struct kbd_backlight {
 	unsigned int base;
 	unsigned int mode;
 	unsigned int timeout;
+	unsigned int has_timeout;
 	struct device_attribute mode_attr;
 	struct device_attribute timeout_attr;
 };
@@ -1877,6 +1880,8 @@ static int sony_nc_kbd_backlight_setup(struct platform_device *pd,
 		unsigned int handle)
 {
 	int result;
+	int probe_base = 0;
+	int ctl_base = 0;
 	int ret = 0;
 
 	if (kbdbl_ctl) {
@@ -1885,11 +1890,25 @@ static int sony_nc_kbd_backlight_setup(struct platform_device *pd,
 		return -EBUSY;
 	}
 
-	/* verify the kbd backlight presence, these handles are not used for
-	 * keyboard backlight only
+	/* verify the kbd backlight presence, some of these handles are not used
+	 * for keyboard backlight only
 	 */
-	ret = sony_call_snc_handle(handle, handle == 0x0137 ? 0x0B00 : 0x0100,
-			&result);
+	switch (handle) {
+	case 0x0153:
+		probe_base = 0x0;
+		ctl_base = 0x0;
+		break;
+	case 0x0137:
+		probe_base = 0x0B00;
+		ctl_base = 0x0C00;
+		break;
+	default:
+		probe_base = 0x0100;
+		ctl_base = 0x4000;
+		break;
+	}
+
+	ret = sony_call_snc_handle(handle, probe_base, &result);
 	if (ret)
 		return ret;
 
@@ -1906,10 +1925,9 @@ static int sony_nc_kbd_backlight_setup(struct platform_device *pd,
 	kbdbl_ctl->mode = kbd_backlight;
 	kbdbl_ctl->timeout = kbd_backlight_timeout;
 	kbdbl_ctl->handle = handle;
-	if (handle == 0x0137)
-		kbdbl_ctl->base = 0x0C00;
-	else
-		kbdbl_ctl->base = 0x4000;
+	kbdbl_ctl->base = ctl_base;
+	/* Some models do not allow timeout control */
+	kbdbl_ctl->has_timeout = handle != 0x0153;
 
 	sysfs_attr_init(&kbdbl_ctl->mode_attr.attr);
 	kbdbl_ctl->mode_attr.attr.name = "kbd_backlight";
@@ -1917,22 +1935,28 @@ static int sony_nc_kbd_backlight_setup(struct platform_device *pd,
 	kbdbl_ctl->mode_attr.show = sony_nc_kbd_backlight_mode_show;
 	kbdbl_ctl->mode_attr.store = sony_nc_kbd_backlight_mode_store;
 
-	sysfs_attr_init(&kbdbl_ctl->timeout_attr.attr);
-	kbdbl_ctl->timeout_attr.attr.name = "kbd_backlight_timeout";
-	kbdbl_ctl->timeout_attr.attr.mode = S_IRUGO | S_IWUSR;
-	kbdbl_ctl->timeout_attr.show = sony_nc_kbd_backlight_timeout_show;
-	kbdbl_ctl->timeout_attr.store = sony_nc_kbd_backlight_timeout_store;
-
 	ret = device_create_file(&pd->dev, &kbdbl_ctl->mode_attr);
 	if (ret)
 		goto outkzalloc;
 
-	ret = device_create_file(&pd->dev, &kbdbl_ctl->timeout_attr);
-	if (ret)
-		goto outmode;
-
 	__sony_nc_kbd_backlight_mode_set(kbdbl_ctl->mode);
-	__sony_nc_kbd_backlight_timeout_set(kbdbl_ctl->timeout);
+
+	if (kbdbl_ctl->has_timeout) {
+		sysfs_attr_init(&kbdbl_ctl->timeout_attr.attr);
+		kbdbl_ctl->timeout_attr.attr.name = "kbd_backlight_timeout";
+		kbdbl_ctl->timeout_attr.attr.mode = S_IRUGO | S_IWUSR;
+		kbdbl_ctl->timeout_attr.show =
+			sony_nc_kbd_backlight_timeout_show;
+		kbdbl_ctl->timeout_attr.store =
+			sony_nc_kbd_backlight_timeout_store;
+
+		ret = device_create_file(&pd->dev, &kbdbl_ctl->timeout_attr);
+		if (ret)
+			goto outmode;
+
+		__sony_nc_kbd_backlight_timeout_set(kbdbl_ctl->timeout);
+	}
+
 
 	return 0;
 
@@ -1949,7 +1973,8 @@ static void sony_nc_kbd_backlight_cleanup(struct platform_device *pd,
 {
 	if (kbdbl_ctl && handle == kbdbl_ctl->handle) {
 		device_remove_file(&pd->dev, &kbdbl_ctl->mode_attr);
-		device_remove_file(&pd->dev, &kbdbl_ctl->timeout_attr);
+		if (kbdbl_ctl->has_timeout)
+			device_remove_file(&pd->dev, &kbdbl_ctl->timeout_attr);
 		kfree(kbdbl_ctl);
 		kbdbl_ctl = NULL;
 	}
diff --git a/drivers/platform/x86/surfacepro3_button.c b/drivers/platform/x86/surfacepro3_button.c
index f7dade3fd2ab..700e0fa0eec2 100644
--- a/drivers/platform/x86/surfacepro3_button.c
+++ b/drivers/platform/x86/surfacepro3_button.c
@@ -1,6 +1,6 @@
 /*
  * power/home/volume button support for
- * Microsoft Surface Pro 3 tablet.
+ * Microsoft Surface Pro 3/4 tablet.
  *
  * Copyright (c) 2015 Intel Corporation.
  * All rights reserved.
@@ -19,9 +19,10 @@
 #include <linux/acpi.h>
 #include <acpi/button.h>
 
-#define SURFACE_BUTTON_HID		"MSHW0028"
+#define SURFACE_PRO3_BUTTON_HID		"MSHW0028"
+#define SURFACE_PRO4_BUTTON_HID		"MSHW0040"
 #define SURFACE_BUTTON_OBJ_NAME		"VGBI"
-#define SURFACE_BUTTON_DEVICE_NAME	"Surface Pro 3 Buttons"
+#define SURFACE_BUTTON_DEVICE_NAME	"Surface Pro 3/4 Buttons"
 
 #define SURFACE_BUTTON_NOTIFY_PRESS_POWER	0xc6
 #define SURFACE_BUTTON_NOTIFY_RELEASE_POWER	0xc7
@@ -54,7 +55,8 @@ MODULE_LICENSE("GPL v2");
  * acpi_driver.
  */
 static const struct acpi_device_id surface_button_device_ids[] = {
-	{SURFACE_BUTTON_HID,    0},
+	{SURFACE_PRO3_BUTTON_HID,    0},
+	{SURFACE_PRO4_BUTTON_HID,    0},
 	{"", 0},
 };
 MODULE_DEVICE_TABLE(acpi, surface_button_device_ids);
@@ -109,7 +111,7 @@ static void surface_button_notify(struct acpi_device *device, u32 event)
 		break;
 	}
 	input = button->input;
-	if (KEY_RESERVED == key_code)
+	if (key_code == KEY_RESERVED)
 		return;
 	if (pressed)
 		pm_wakeup_event(&device->dev, 0);
diff --git a/drivers/platform/x86/tc1100-wmi.c b/drivers/platform/x86/tc1100-wmi.c
index 89aa976f0ab2..65b0a4845ddd 100644
--- a/drivers/platform/x86/tc1100-wmi.c
+++ b/drivers/platform/x86/tc1100-wmi.c
@@ -52,7 +52,9 @@ struct tc1100_data {
 	u32 jogdial;
 };
 
+#ifdef CONFIG_PM
 static struct tc1100_data suspend_data;
+#endif
 
 /* --------------------------------------------------------------------------
 				Device Management
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 0bed4733c4f0..a268a7abf8ab 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -303,6 +303,7 @@ static struct {
 	u32 hotkey_mask:1;
 	u32 hotkey_wlsw:1;
 	u32 hotkey_tablet:1;
+	u32 kbdlight:1;
 	u32 light:1;
 	u32 light_status:1;
 	u32 bright_acpimode:1;
@@ -4986,6 +4987,207 @@ static struct ibm_struct video_driver_data = {
 #endif /* CONFIG_THINKPAD_ACPI_VIDEO */
 
 /*************************************************************************
+ * Keyboard backlight subdriver
+ */
+
+static int kbdlight_set_level(int level)
+{
+	if (!hkey_handle)
+		return -ENXIO;
+
+	if (!acpi_evalf(hkey_handle, NULL, "MLCS", "dd", level))
+		return -EIO;
+
+	return 0;
+}
+
+static int kbdlight_get_level(void)
+{
+	int status = 0;
+
+	if (!hkey_handle)
+		return -ENXIO;
+
+	if (!acpi_evalf(hkey_handle, &status, "MLCG", "dd", 0))
+		return -EIO;
+
+	if (status < 0)
+		return status;
+
+	return status & 0x3;
+}
+
+static bool kbdlight_is_supported(void)
+{
+	int status = 0;
+
+	if (!hkey_handle)
+		return false;
+
+	if (!acpi_has_method(hkey_handle, "MLCG")) {
+		vdbg_printk(TPACPI_DBG_INIT, "kbdlight MLCG is unavailable\n");
+		return false;
+	}
+
+	if (!acpi_evalf(hkey_handle, &status, "MLCG", "qdd", 0)) {
+		vdbg_printk(TPACPI_DBG_INIT, "kbdlight MLCG failed\n");
+		return false;
+	}
+
+	if (status < 0) {
+		vdbg_printk(TPACPI_DBG_INIT, "kbdlight MLCG err: %d\n", status);
+		return false;
+	}
+
+	vdbg_printk(TPACPI_DBG_INIT, "kbdlight MLCG returned 0x%x\n", status);
+	/*
+	 * Guessed test for keyboard backlight:
+	 *
+	 * Machines with backlight keyboard return:
+	 *   b010100000010000000XX - ThinkPad X1 Carbon 3rd
+	 *   b110100010010000000XX - ThinkPad x230
+	 *   b010100000010000000XX - ThinkPad x240
+	 *   b010100000010000000XX - ThinkPad W541
+	 * (XX is current backlight level)
+	 *
+	 * Machines without backlight keyboard return:
+	 *   b10100001000000000000 - ThinkPad x230
+	 *   b10110001000000000000 - ThinkPad E430
+	 *   b00000000000000000000 - ThinkPad E450
+	 *
+	 * Candidate BITs for detection test (XOR):
+	 *   b01000000001000000000
+	 *              ^
+	 */
+	return status & BIT(9);
+}
+
+static void kbdlight_set_worker(struct work_struct *work)
+{
+	struct tpacpi_led_classdev *data =
+			container_of(work, struct tpacpi_led_classdev, work);
+
+	if (likely(tpacpi_lifecycle == TPACPI_LIFE_RUNNING))
+		kbdlight_set_level(data->new_state);
+}
+
+static void kbdlight_sysfs_set(struct led_classdev *led_cdev,
+			enum led_brightness brightness)
+{
+	struct tpacpi_led_classdev *data =
+			container_of(led_cdev,
+				     struct tpacpi_led_classdev,
+				     led_classdev);
+	data->new_state = brightness;
+	queue_work(tpacpi_wq, &data->work);
+}
+
+static enum led_brightness kbdlight_sysfs_get(struct led_classdev *led_cdev)
+{
+	int level;
+
+	level = kbdlight_get_level();
+	if (level < 0)
+		return 0;
+
+	return level;
+}
+
+static struct tpacpi_led_classdev tpacpi_led_kbdlight = {
+	.led_classdev = {
+		.name		= "tpacpi::kbd_backlight",
+		.max_brightness	= 2,
+		.brightness_set	= &kbdlight_sysfs_set,
+		.brightness_get	= &kbdlight_sysfs_get,
+		.flags		= LED_CORE_SUSPENDRESUME,
+	}
+};
+
+static int __init kbdlight_init(struct ibm_init_struct *iibm)
+{
+	int rc;
+
+	vdbg_printk(TPACPI_DBG_INIT, "initializing kbdlight subdriver\n");
+
+	TPACPI_ACPIHANDLE_INIT(hkey);
+	INIT_WORK(&tpacpi_led_kbdlight.work, kbdlight_set_worker);
+
+	if (!kbdlight_is_supported()) {
+		tp_features.kbdlight = 0;
+		vdbg_printk(TPACPI_DBG_INIT, "kbdlight is unsupported\n");
+		return 1;
+	}
+
+	tp_features.kbdlight = 1;
+
+	rc = led_classdev_register(&tpacpi_pdev->dev,
+				   &tpacpi_led_kbdlight.led_classdev);
+	if (rc < 0) {
+		tp_features.kbdlight = 0;
+		return rc;
+	}
+
+	return 0;
+}
+
+static void kbdlight_exit(void)
+{
+	if (tp_features.kbdlight)
+		led_classdev_unregister(&tpacpi_led_kbdlight.led_classdev);
+	flush_workqueue(tpacpi_wq);
+}
+
+static int kbdlight_read(struct seq_file *m)
+{
+	int level;
+
+	if (!tp_features.kbdlight) {
+		seq_printf(m, "status:\t\tnot supported\n");
+	} else {
+		level = kbdlight_get_level();
+		if (level < 0)
+			seq_printf(m, "status:\t\terror %d\n", level);
+		else
+			seq_printf(m, "status:\t\t%d\n", level);
+		seq_printf(m, "commands:\t0, 1, 2\n");
+	}
+
+	return 0;
+}
+
+static int kbdlight_write(char *buf)
+{
+	char *cmd;
+	int level = -1;
+
+	if (!tp_features.kbdlight)
+		return -ENODEV;
+
+	while ((cmd = next_cmd(&buf))) {
+		if (strlencmp(cmd, "0") == 0)
+			level = 0;
+		else if (strlencmp(cmd, "1") == 0)
+			level = 1;
+		else if (strlencmp(cmd, "2") == 0)
+			level = 2;
+		else
+			return -EINVAL;
+	}
+
+	if (level == -1)
+		return -EINVAL;
+
+	return kbdlight_set_level(level);
+}
+
+static struct ibm_struct kbdlight_driver_data = {
+	.name = "kbdlight",
+	.read = kbdlight_read,
+	.write = kbdlight_write,
+	.exit = kbdlight_exit,
+};
+
+/*************************************************************************
  * Light (thinklight) subdriver
  */
 
@@ -9207,6 +9409,10 @@ static struct ibm_init_struct ibms_init[] __initdata = {
 	},
 #endif
 	{
+		.init = kbdlight_init,
+		.data = &kbdlight_driver_data,
+	},
+	{
 		.init = light_init,
 		.data = &light_driver_data,
 	},
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index c01302989ee4..73833079bac8 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -51,6 +51,7 @@
 #include <linux/dmi.h>
 #include <linux/uaccess.h>
 #include <linux/miscdevice.h>
+#include <linux/rfkill.h>
 #include <linux/toshiba.h>
 #include <acpi/video.h>
 
@@ -114,6 +115,7 @@ MODULE_LICENSE("GPL");
 #define HCI_VIDEO_OUT			0x001c
 #define HCI_HOTKEY_EVENT		0x001e
 #define HCI_LCD_BRIGHTNESS		0x002a
+#define HCI_WIRELESS			0x0056
 #define HCI_ACCELEROMETER		0x006d
 #define HCI_KBD_ILLUMINATION		0x0095
 #define HCI_ECO_MODE			0x0097
@@ -148,6 +150,10 @@ MODULE_LICENSE("GPL");
 #define SCI_KBD_MODE_ON			0x8
 #define SCI_KBD_MODE_OFF		0x10
 #define SCI_KBD_TIME_MAX		0x3c001a
+#define HCI_WIRELESS_STATUS		0x1
+#define HCI_WIRELESS_WWAN		0x3
+#define HCI_WIRELESS_WWAN_STATUS	0x2000
+#define HCI_WIRELESS_WWAN_POWER		0x4000
 #define SCI_USB_CHARGE_MODE_MASK	0xff
 #define SCI_USB_CHARGE_DISABLED		0x00
 #define SCI_USB_CHARGE_ALTERNATE	0x09
@@ -169,6 +175,7 @@ struct toshiba_acpi_dev {
 	struct led_classdev kbd_led;
 	struct led_classdev eco_led;
 	struct miscdevice miscdev;
+	struct rfkill *wwan_rfk;
 
 	int force_fan;
 	int last_key_event;
@@ -197,12 +204,15 @@ struct toshiba_acpi_dev {
 	unsigned int kbd_function_keys_supported:1;
 	unsigned int panel_power_on_supported:1;
 	unsigned int usb_three_supported:1;
+	unsigned int wwan_supported:1;
 	unsigned int sysfs_created:1;
 	unsigned int special_functions;
 
+	bool kbd_event_generated;
 	bool kbd_led_registered;
 	bool illumination_led_registered;
 	bool eco_led_registered;
+	bool killswitch;
 };
 
 static struct toshiba_acpi_dev *toshiba_acpi;
@@ -516,6 +526,7 @@ static void toshiba_kbd_illum_available(struct toshiba_acpi_dev *dev)
 
 	dev->kbd_illum_supported = 0;
 	dev->kbd_led_registered = false;
+	dev->kbd_event_generated = false;
 
 	if (!sci_open(dev))
 		return;
@@ -1085,6 +1096,104 @@ static int toshiba_hotkey_event_type_get(struct toshiba_acpi_dev *dev,
 	return -EIO;
 }
 
+/* Wireless status (RFKill, WLAN, BT, WWAN) */
+static int toshiba_wireless_status(struct toshiba_acpi_dev *dev)
+{
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_WIRELESS, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status;
+
+	in[3] = HCI_WIRELESS_STATUS;
+	status = tci_raw(dev, in, out);
+
+	if (ACPI_FAILURE(status)) {
+		pr_err("ACPI call to get Wireless status failed\n");
+		return -EIO;
+	}
+
+	if (out[0] == TOS_NOT_SUPPORTED)
+		return -ENODEV;
+
+	if (out[0] != TOS_SUCCESS)
+		return -EIO;
+
+	dev->killswitch = !!(out[2] & HCI_WIRELESS_STATUS);
+
+	return 0;
+}
+
+/* WWAN */
+static void toshiba_wwan_available(struct toshiba_acpi_dev *dev)
+{
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_WIRELESS, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status;
+
+	dev->wwan_supported = 0;
+
+	/*
+	 * WWAN support can be queried by setting the in[3] value to
+	 * HCI_WIRELESS_WWAN (0x03).
+	 *
+	 * If supported, out[0] contains TOS_SUCCESS and out[2] contains
+	 * HCI_WIRELESS_WWAN_STATUS (0x2000).
+	 *
+	 * If not supported, out[0] contains TOS_INPUT_DATA_ERROR (0x8300)
+	 * or TOS_NOT_SUPPORTED (0x8000).
+	 */
+	in[3] = HCI_WIRELESS_WWAN;
+	status = tci_raw(dev, in, out);
+
+	if (ACPI_FAILURE(status)) {
+		pr_err("ACPI call to get WWAN status failed\n");
+		return;
+	}
+
+	if (out[0] != TOS_SUCCESS)
+		return;
+
+	dev->wwan_supported = (out[2] == HCI_WIRELESS_WWAN_STATUS);
+}
+
+static int toshiba_wwan_set(struct toshiba_acpi_dev *dev, u32 state)
+{
+	u32 in[TCI_WORDS] = { HCI_SET, HCI_WIRELESS, state, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status;
+
+	in[3] = HCI_WIRELESS_WWAN_STATUS;
+	status = tci_raw(dev, in, out);
+
+	if (ACPI_FAILURE(status)) {
+		pr_err("ACPI call to set WWAN status failed\n");
+		return -EIO;
+	}
+
+	if (out[0] == TOS_NOT_SUPPORTED)
+		return -ENODEV;
+
+	if (out[0] != TOS_SUCCESS)
+		return -EIO;
+
+	/*
+	 * Some devices only need to call HCI_WIRELESS_WWAN_STATUS to
+	 * (de)activate the device, but some others need the
+	 * HCI_WIRELESS_WWAN_POWER call as well.
+	 */
+	in[3] = HCI_WIRELESS_WWAN_POWER;
+	status = tci_raw(dev, in, out);
+
+	if (ACPI_FAILURE(status)) {
+		pr_err("ACPI call to set WWAN power failed\n");
+		return -EIO;
+	}
+
+	if (out[0] == TOS_NOT_SUPPORTED)
+		return -ENODEV;
+
+	return out[0] == TOS_SUCCESS ? 0 : -EIO;
+}
+
 /* Transflective Backlight */
 static int get_tr_backlight_status(struct toshiba_acpi_dev *dev, u32 *status)
 {
@@ -1535,6 +1644,11 @@ static const struct backlight_ops toshiba_backlight_data = {
 	.update_status  = set_lcd_status,
 };
 
+/* Keyboard backlight work */
+static void toshiba_acpi_kbd_bl_work(struct work_struct *work);
+
+static DECLARE_WORK(kbd_bl_work, toshiba_acpi_kbd_bl_work);
+
 /*
  * Sysfs files
  */
@@ -1634,6 +1748,24 @@ static ssize_t kbd_backlight_mode_store(struct device *dev,
 			return ret;
 
 		toshiba->kbd_mode = mode;
+
+		/*
+		 * Some laptop models with the second generation backlit
+		 * keyboard (type 2) do not generate the keyboard backlight
+		 * changed event (0x92), and thus, the driver will never update
+		 * the sysfs entries.
+		 *
+		 * The event is generated right when changing the keyboard
+		 * backlight mode and the *notify function will set the
+		 * kbd_event_generated to true.
+		 *
+		 * In case the event is not generated, schedule the keyboard
+		 * backlight work to update the sysfs entries and emulate the
+		 * event via genetlink.
+		 */
+		if (toshiba->kbd_type == 2 &&
+		    !toshiba_acpi->kbd_event_generated)
+			schedule_work(&kbd_bl_work);
 	}
 
 	return count;
@@ -2166,6 +2298,21 @@ static struct attribute_group toshiba_attr_group = {
 	.attrs = toshiba_attributes,
 };
 
+static void toshiba_acpi_kbd_bl_work(struct work_struct *work)
+{
+	struct acpi_device *acpi_dev = toshiba_acpi->acpi_dev;
+
+	/* Update the sysfs entries */
+	if (sysfs_update_group(&acpi_dev->dev.kobj,
+			       &toshiba_attr_group))
+		pr_err("Unable to update sysfs entries\n");
+
+	/* Emulate the keyboard backlight event */
+	acpi_bus_generate_netlink_event(acpi_dev->pnp.device_class,
+					dev_name(&acpi_dev->dev),
+					0x92, 0);
+}
+
 /*
  * Misc device
  */
@@ -2242,6 +2389,67 @@ static const struct file_operations toshiba_acpi_fops = {
 };
 
 /*
+ * WWAN RFKill handlers
+ */
+static int toshiba_acpi_wwan_set_block(void *data, bool blocked)
+{
+	struct toshiba_acpi_dev *dev = data;
+	int ret;
+
+	ret = toshiba_wireless_status(dev);
+	if (ret)
+		return ret;
+
+	if (!dev->killswitch)
+		return 0;
+
+	return toshiba_wwan_set(dev, !blocked);
+}
+
+static void toshiba_acpi_wwan_poll(struct rfkill *rfkill, void *data)
+{
+	struct toshiba_acpi_dev *dev = data;
+
+	if (toshiba_wireless_status(dev))
+		return;
+
+	rfkill_set_hw_state(dev->wwan_rfk, !dev->killswitch);
+}
+
+static const struct rfkill_ops wwan_rfk_ops = {
+	.set_block = toshiba_acpi_wwan_set_block,
+	.poll = toshiba_acpi_wwan_poll,
+};
+
+static int toshiba_acpi_setup_wwan_rfkill(struct toshiba_acpi_dev *dev)
+{
+	int ret = toshiba_wireless_status(dev);
+
+	if (ret)
+		return ret;
+
+	dev->wwan_rfk = rfkill_alloc("Toshiba WWAN",
+				     &dev->acpi_dev->dev,
+				     RFKILL_TYPE_WWAN,
+				     &wwan_rfk_ops,
+				     dev);
+	if (!dev->wwan_rfk) {
+		pr_err("Unable to allocate WWAN rfkill device\n");
+		return -ENOMEM;
+	}
+
+	rfkill_set_hw_state(dev->wwan_rfk, !dev->killswitch);
+
+	ret = rfkill_register(dev->wwan_rfk);
+	if (ret) {
+		pr_err("Unable to register WWAN rfkill device\n");
+		rfkill_destroy(dev->wwan_rfk);
+	}
+
+	return ret;
+}
+
+/*
  * Hotkeys
  */
 static int toshiba_acpi_enable_hotkeys(struct toshiba_acpi_dev *dev)
@@ -2484,6 +2692,14 @@ static int toshiba_acpi_setup_backlight(struct toshiba_acpi_dev *dev)
 	brightness = __get_lcd_brightness(dev);
 	if (brightness < 0)
 		return 0;
+	/*
+	 * If transflective backlight is supported and the brightness is zero
+	 * (lowest brightness level), the set_lcd_brightness function will
+	 * activate the transflective backlight, making the LCD appear to be
+	 * turned off, simply increment the brightness level to avoid that.
+	 */
+	if (dev->tr_backlight_supported && brightness == 0)
+		brightness++;
 	ret = set_lcd_brightness(dev, brightness);
 	if (ret) {
 		pr_debug("Backlight method is read-only, disabling backlight support\n");
@@ -2561,6 +2777,8 @@ static void print_supported_features(struct toshiba_acpi_dev *dev)
 		pr_cont(" panel-power-on");
 	if (dev->usb_three_supported)
 		pr_cont(" usb3");
+	if (dev->wwan_supported)
+		pr_cont(" wwan");
 
 	pr_cont("\n");
 }
@@ -2598,6 +2816,11 @@ static int toshiba_acpi_remove(struct acpi_device *acpi_dev)
 	if (dev->eco_led_registered)
 		led_classdev_unregister(&dev->eco_led);
 
+	if (dev->wwan_rfk) {
+		rfkill_unregister(dev->wwan_rfk);
+		rfkill_destroy(dev->wwan_rfk);
+	}
+
 	if (toshiba_acpi)
 		toshiba_acpi = NULL;
 
@@ -2736,6 +2959,10 @@ static int toshiba_acpi_add(struct acpi_device *acpi_dev)
 	ret = get_fan_status(dev, &dummy);
 	dev->fan_supported = !ret;
 
+	toshiba_wwan_available(dev);
+	if (dev->wwan_supported)
+		toshiba_acpi_setup_wwan_rfkill(dev);
+
 	print_supported_features(dev);
 
 	ret = sysfs_create_group(&dev->acpi_dev->dev.kobj,
@@ -2760,7 +2987,6 @@ error:
 static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 {
 	struct toshiba_acpi_dev *dev = acpi_driver_data(acpi_dev);
-	int ret;
 
 	switch (event) {
 	case 0x80: /* Hotkeys and some system events */
@@ -2790,10 +3016,10 @@ static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 		pr_info("SATA power event received %x\n", event);
 		break;
 	case 0x92: /* Keyboard backlight mode changed */
+		toshiba_acpi->kbd_event_generated = true;
 		/* Update sysfs entries */
-		ret = sysfs_update_group(&acpi_dev->dev.kobj,
-					 &toshiba_attr_group);
-		if (ret)
+		if (sysfs_update_group(&acpi_dev->dev.kobj,
+				       &toshiba_attr_group))
 			pr_err("Unable to update sysfs entries\n");
 		break;
 	case 0x85: /* Unknown */
@@ -2808,7 +3034,8 @@ static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 
 	acpi_bus_generate_netlink_event(acpi_dev->pnp.device_class,
 					dev_name(&acpi_dev->dev),
-					event, 0);
+					event, (event == 0x80) ?
+					dev->last_key_event : 0);
 }
 
 #ifdef CONFIG_PM_SLEEP
@@ -2832,12 +3059,15 @@ static int toshiba_acpi_resume(struct device *device)
 	struct toshiba_acpi_dev *dev = acpi_driver_data(to_acpi_device(device));
 
 	if (dev->hotkey_dev) {
-		int error = toshiba_acpi_enable_hotkeys(dev);
-
-		if (error)
+		if (toshiba_acpi_enable_hotkeys(dev))
 			pr_info("Unable to re-enable hotkeys\n");
 	}
 
+	if (dev->wwan_rfk) {
+		if (!toshiba_wireless_status(dev))
+			rfkill_set_hw_state(dev->wwan_rfk, !dev->killswitch);
+	}
+
 	return 0;
 }
 #endif
diff --git a/drivers/platform/x86/toshiba_bluetooth.c b/drivers/platform/x86/toshiba_bluetooth.c
index c5e45089ac51..5db495dd018e 100644
--- a/drivers/platform/x86/toshiba_bluetooth.c
+++ b/drivers/platform/x86/toshiba_bluetooth.c
@@ -78,7 +78,7 @@ static int toshiba_bluetooth_present(acpi_handle handle)
 	 */
 	result = acpi_evaluate_integer(handle, "_STA", NULL, &bt_present);
 	if (ACPI_FAILURE(result)) {
-		pr_err("ACPI call to query Bluetooth presence failed");
+		pr_err("ACPI call to query Bluetooth presence failed\n");
 		return -ENXIO;
 	} else if (!bt_present) {
 		pr_info("Bluetooth device not present\n");
diff --git a/drivers/pnp/driver.c b/drivers/pnp/driver.c
index 153a493b5413..63452f20e3e9 100644
--- a/drivers/pnp/driver.c
+++ b/drivers/pnp/driver.c
@@ -74,7 +74,6 @@ void pnp_device_detach(struct pnp_dev *pnp_dev)
 	if (pnp_dev->status == PNP_ATTACHED)
 		pnp_dev->status = PNP_READY;
 	mutex_unlock(&pnp_lock);
-	pnp_disable_dev(pnp_dev);
 }
 
 static int pnp_device_probe(struct device *dev)
@@ -131,6 +130,11 @@ static int pnp_device_remove(struct device *dev)
 			drv->remove(pnp_dev);
 		pnp_dev->driver = NULL;
 	}
+
+	if (pnp_dev->active &&
+	    (!drv || !(drv->flags & PNP_DRIVER_RES_DO_NOT_CHANGE)))
+		pnp_disable_dev(pnp_dev);
+
 	pnp_device_detach(pnp_dev);
 	return 0;
 }
diff --git a/drivers/pnp/quirks.c b/drivers/pnp/quirks.c
index 943c1cb9566c..d28e3ab9479c 100644
--- a/drivers/pnp/quirks.c
+++ b/drivers/pnp/quirks.c
@@ -342,7 +342,9 @@ static void quirk_amd_mmconfig_area(struct pnp_dev *dev)
 /* Device IDs of parts that have 32KB MCH space */
 static const unsigned int mch_quirk_devices[] = {
 	0x0154,	/* Ivy Bridge */
+	0x0a04, /* Haswell-ULT */
 	0x0c00,	/* Haswell */
+	0x1604, /* Broadwell */
 };
 
 static struct pci_dev *get_intel_host(void)
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 237d7aa73e8c..1ddd13cc0c07 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -160,22 +160,16 @@ config BATTERY_SBS
 config BATTERY_BQ27XXX
 	tristate "BQ27xxx battery driver"
 	help
-	  Say Y here to enable support for batteries with BQ27xxx (I2C/HDQ) chips.
+	  Say Y here to enable support for batteries with BQ27xxx chips.
 
 config BATTERY_BQ27XXX_I2C
-	bool "BQ27xxx I2C support"
+	tristate "BQ27xxx I2C support"
 	depends on BATTERY_BQ27XXX
 	depends on I2C
 	default y
 	help
-	  Say Y here to enable support for batteries with BQ27xxx (I2C) chips.
-
-config BATTERY_BQ27XXX_PLATFORM
-	bool "BQ27xxx HDQ support"
-	depends on BATTERY_BQ27XXX
-	default y
-	help
-	  Say Y here to enable support for batteries with BQ27xxx (HDQ) chips.
+	  Say Y here to enable support for batteries with BQ27xxx chips
+	  connected over an I2C bus.
 
 config BATTERY_DA9030
 	tristate "DA9030 battery driver"
@@ -508,8 +502,7 @@ config AXP20X_POWER
 	  This driver provides support for the power supply features of
 	  AXP20x PMIC.
 
-source "drivers/power/reset/Kconfig"
-
 endif # POWER_SUPPLY
 
+source "drivers/power/reset/Kconfig"
 source "drivers/power/avs/Kconfig"
diff --git a/drivers/power/Makefile b/drivers/power/Makefile
index b656638f8b39..0e4eab55f8d7 100644
--- a/drivers/power/Makefile
+++ b/drivers/power/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_BATTERY_IPAQ_MICRO) += ipaq_micro_battery.o
 obj-$(CONFIG_BATTERY_WM97XX)	+= wm97xx_battery.o
 obj-$(CONFIG_BATTERY_SBS)	+= sbs-battery.o
 obj-$(CONFIG_BATTERY_BQ27XXX)	+= bq27xxx_battery.o
+obj-$(CONFIG_BATTERY_BQ27XXX_I2C) += bq27xxx_battery_i2c.o
 obj-$(CONFIG_BATTERY_DA9030)	+= da9030_battery.o
 obj-$(CONFIG_BATTERY_DA9052)	+= da9052-battery.o
 obj-$(CONFIG_CHARGER_DA9150)	+= da9150-charger.o
diff --git a/drivers/power/bq2415x_charger.c b/drivers/power/bq2415x_charger.c
index 4afd76848bce..27e89536689a 100644
--- a/drivers/power/bq2415x_charger.c
+++ b/drivers/power/bq2415x_charger.c
@@ -1704,7 +1704,7 @@ error_4:
 error_3:
 	bq2415x_power_supply_exit(bq);
 error_2:
-	if (bq && bq->notify_node)
+	if (bq)
 		of_node_put(bq->notify_node);
 	kfree(name);
 error_1:
@@ -1724,9 +1724,7 @@ static int bq2415x_remove(struct i2c_client *client)
 	if (bq->nb.notifier_call)
 		power_supply_unreg_notifier(&bq->nb);
 
-	if (bq->notify_node)
-		of_node_put(bq->notify_node);
-
+	of_node_put(bq->notify_node);
 	bq2415x_sysfs_exit(bq);
 	bq2415x_power_supply_exit(bq);
 
diff --git a/drivers/power/bq27xxx_battery.c b/drivers/power/bq27xxx_battery.c
index 880233ce9343..6b027a418943 100644
--- a/drivers/power/bq27xxx_battery.c
+++ b/drivers/power/bq27xxx_battery.c
@@ -45,11 +45,7 @@
 #include <linux/delay.h>
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
-#include <linux/idr.h>
-#include <linux/i2c.h>
 #include <linux/slab.h>
-#include <linux/interrupt.h>
-#include <asm/unaligned.h>
 
 #include <linux/power/bq27xxx_battery.h>
 
@@ -78,11 +74,6 @@
 #define BQ27XXX_POWER_CONSTANT		(29200) /* 29.2 µV^2 * 1000 */
 #define BQ27XXX_CURRENT_CONSTANT	(3570) /* 3.57 µV * 1000 */
 
-struct bq27xxx_device_info;
-struct bq27xxx_access_methods {
-	int (*read)(struct bq27xxx_device_info *di, u8 reg, bool single);
-};
-
 #define INVALID_REG_ADDR	0xff
 
 /*
@@ -110,40 +101,6 @@ enum bq27xxx_reg_index {
 	BQ27XXX_REG_AP,		/* Average Power */
 };
 
-struct bq27xxx_reg_cache {
-	int temperature;
-	int time_to_empty;
-	int time_to_empty_avg;
-	int time_to_full;
-	int charge_full;
-	int cycle_count;
-	int capacity;
-	int energy;
-	int flags;
-	int power_avg;
-	int health;
-};
-
-struct bq27xxx_device_info {
-	struct device		*dev;
-	int			id;
-	enum bq27xxx_chip	chip;
-
-	struct bq27xxx_reg_cache cache;
-	int charge_design_full;
-
-	unsigned long last_update;
-	struct delayed_work work;
-
-	struct power_supply	*bat;
-
-	struct bq27xxx_access_methods bus;
-
-	struct mutex lock;
-
-	u8 *regs;
-};
-
 /* Register mappings */
 static u8 bq27000_regs[] = {
 	0x00,	/* CONTROL	*/
@@ -198,10 +155,10 @@ static u8 bq27500_regs[] = {
 	INVALID_REG_ADDR,	/* TTECP - NA	*/
 	0x0c,	/* NAC		*/
 	0x12,	/* LMD(FCC)	*/
-	0x1e,	/* CYCT		*/
+	0x2a,	/* CYCT		*/
 	INVALID_REG_ADDR,	/* AE - NA	*/
-	0x20,	/* SOC(RSOC)	*/
-	0x2e,	/* DCAP(ILMD)	*/
+	0x2c,	/* SOC(RSOC)	*/
+	0x3c,	/* DCAP(ILMD)	*/
 	INVALID_REG_ADDR,	/* AP - NA	*/
 };
 
@@ -242,7 +199,7 @@ static u8 bq27541_regs[] = {
 	INVALID_REG_ADDR,	/* AE - NA	*/
 	0x2c,	/* SOC(RSOC)	*/
 	0x3c,	/* DCAP		*/
-	0x76,	/* AP		*/
+	0x24,	/* AP		*/
 };
 
 static u8 bq27545_regs[] = {
@@ -471,7 +428,10 @@ static int bq27xxx_battery_read_soc(struct bq27xxx_device_info *di)
 {
 	int soc;
 
-	soc = bq27xxx_read(di, BQ27XXX_REG_SOC, false);
+	if (di->chip == BQ27000 || di->chip == BQ27010)
+		soc = bq27xxx_read(di, BQ27XXX_REG_SOC, true);
+	else
+		soc = bq27xxx_read(di, BQ27XXX_REG_SOC, false);
 
 	if (soc < 0)
 		dev_dbg(di->dev, "error reading State-of-Charge\n");
@@ -536,7 +496,10 @@ static int bq27xxx_battery_read_dcap(struct bq27xxx_device_info *di)
 {
 	int dcap;
 
-	dcap = bq27xxx_read(di, BQ27XXX_REG_DCAP, false);
+	if (di->chip == BQ27000 || di->chip == BQ27010)
+		dcap = bq27xxx_read(di, BQ27XXX_REG_DCAP, true);
+	else
+		dcap = bq27xxx_read(di, BQ27XXX_REG_DCAP, false);
 
 	if (dcap < 0) {
 		dev_dbg(di->dev, "error reading initial last measured discharge\n");
@@ -544,7 +507,7 @@ static int bq27xxx_battery_read_dcap(struct bq27xxx_device_info *di)
 	}
 
 	if (di->chip == BQ27000 || di->chip == BQ27010)
-		dcap *= BQ27XXX_CURRENT_CONSTANT / BQ27XXX_RS;
+		dcap = (dcap << 8) * BQ27XXX_CURRENT_CONSTANT / BQ27XXX_RS;
 	else
 		dcap *= 1000;
 
@@ -710,7 +673,7 @@ static int bq27xxx_battery_read_health(struct bq27xxx_device_info *di)
 	return POWER_SUPPLY_HEALTH_GOOD;
 }
 
-static void bq27xxx_battery_update(struct bq27xxx_device_info *di)
+void bq27xxx_battery_update(struct bq27xxx_device_info *di)
 {
 	struct bq27xxx_reg_cache cache = {0, };
 	bool has_ci_flag = di->chip == BQ27000 || di->chip == BQ27010;
@@ -722,7 +685,7 @@ static void bq27xxx_battery_update(struct bq27xxx_device_info *di)
 	if (cache.flags >= 0) {
 		cache.temperature = bq27xxx_battery_read_temperature(di);
 		if (has_ci_flag && (cache.flags & BQ27000_FLAG_CI)) {
-			dev_info(di->dev, "battery is not calibrated! ignoring capacity values\n");
+			dev_info_once(di->dev, "battery is not calibrated! ignoring capacity values\n");
 			cache.capacity = -ENODATA;
 			cache.energy = -ENODATA;
 			cache.time_to_empty = -ENODATA;
@@ -761,6 +724,7 @@ static void bq27xxx_battery_update(struct bq27xxx_device_info *di)
 
 	di->last_update = jiffies;
 }
+EXPORT_SYMBOL_GPL(bq27xxx_battery_update);
 
 static void bq27xxx_battery_poll(struct work_struct *work)
 {
@@ -991,32 +955,30 @@ static void bq27xxx_external_power_changed(struct power_supply *psy)
 	schedule_delayed_work(&di->work, 0);
 }
 
-static int bq27xxx_powersupply_init(struct bq27xxx_device_info *di,
-				    const char *name)
+int bq27xxx_battery_setup(struct bq27xxx_device_info *di)
 {
-	int ret;
 	struct power_supply_desc *psy_desc;
 	struct power_supply_config psy_cfg = { .drv_data = di, };
 
+	INIT_DELAYED_WORK(&di->work, bq27xxx_battery_poll);
+	mutex_init(&di->lock);
+	di->regs = bq27xxx_regs[di->chip];
+
 	psy_desc = devm_kzalloc(di->dev, sizeof(*psy_desc), GFP_KERNEL);
 	if (!psy_desc)
 		return -ENOMEM;
 
-	psy_desc->name = name;
+	psy_desc->name = di->name;
 	psy_desc->type = POWER_SUPPLY_TYPE_BATTERY;
 	psy_desc->properties = bq27xxx_battery_props[di->chip].props;
 	psy_desc->num_properties = bq27xxx_battery_props[di->chip].size;
 	psy_desc->get_property = bq27xxx_battery_get_property;
 	psy_desc->external_power_changed = bq27xxx_external_power_changed;
 
-	INIT_DELAYED_WORK(&di->work, bq27xxx_battery_poll);
-	mutex_init(&di->lock);
-
 	di->bat = power_supply_register_no_ws(di->dev, psy_desc, &psy_cfg);
 	if (IS_ERR(di->bat)) {
-		ret = PTR_ERR(di->bat);
-		dev_err(di->dev, "failed to register battery: %d\n", ret);
-		return ret;
+		dev_err(di->dev, "failed to register battery\n");
+		return PTR_ERR(di->bat);
 	}
 
 	dev_info(di->dev, "support ver. %s enabled\n", DRIVER_VERSION);
@@ -1025,8 +987,9 @@ static int bq27xxx_powersupply_init(struct bq27xxx_device_info *di,
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(bq27xxx_battery_setup);
 
-static void bq27xxx_powersupply_unregister(struct bq27xxx_device_info *di)
+void bq27xxx_battery_teardown(struct bq27xxx_device_info *di)
 {
 	/*
 	 * power_supply_unregister call bq27xxx_battery_get_property which
@@ -1042,192 +1005,7 @@ static void bq27xxx_powersupply_unregister(struct bq27xxx_device_info *di)
 
 	mutex_destroy(&di->lock);
 }
-
-/* i2c specific code */
-#ifdef CONFIG_BATTERY_BQ27XXX_I2C
-
-/* If the system has several batteries we need a different name for each
- * of them...
- */
-static DEFINE_IDR(battery_id);
-static DEFINE_MUTEX(battery_mutex);
-
-static irqreturn_t bq27xxx_battery_irq_handler_thread(int irq, void *data)
-{
-	struct bq27xxx_device_info *di = data;
-
-	bq27xxx_battery_update(di);
-
-	return IRQ_HANDLED;
-}
-
-static int bq27xxx_battery_i2c_read(struct bq27xxx_device_info *di, u8 reg,
-				    bool single)
-{
-	struct i2c_client *client = to_i2c_client(di->dev);
-	struct i2c_msg msg[2];
-	unsigned char data[2];
-	int ret;
-
-	if (!client->adapter)
-		return -ENODEV;
-
-	msg[0].addr = client->addr;
-	msg[0].flags = 0;
-	msg[0].buf = &reg;
-	msg[0].len = sizeof(reg);
-	msg[1].addr = client->addr;
-	msg[1].flags = I2C_M_RD;
-	msg[1].buf = data;
-	if (single)
-		msg[1].len = 1;
-	else
-		msg[1].len = 2;
-
-	ret = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
-	if (ret < 0)
-		return ret;
-
-	if (!single)
-		ret = get_unaligned_le16(data);
-	else
-		ret = data[0];
-
-	return ret;
-}
-
-static int bq27xxx_battery_i2c_probe(struct i2c_client *client,
-				     const struct i2c_device_id *id)
-{
-	char *name;
-	struct bq27xxx_device_info *di;
-	int num;
-	int retval = 0;
-
-	/* Get new ID for the new battery device */
-	mutex_lock(&battery_mutex);
-	num = idr_alloc(&battery_id, client, 0, 0, GFP_KERNEL);
-	mutex_unlock(&battery_mutex);
-	if (num < 0)
-		return num;
-
-	name = devm_kasprintf(&client->dev, GFP_KERNEL, "%s-%d", id->name, num);
-	if (!name) {
-		retval = -ENOMEM;
-		goto batt_failed;
-	}
-
-	di = devm_kzalloc(&client->dev, sizeof(*di), GFP_KERNEL);
-	if (!di) {
-		retval = -ENOMEM;
-		goto batt_failed;
-	}
-
-	di->id = num;
-	di->dev = &client->dev;
-	di->chip = id->driver_data;
-	di->bus.read = &bq27xxx_battery_i2c_read;
-	di->regs = bq27xxx_regs[di->chip];
-
-	retval = bq27xxx_powersupply_init(di, name);
-	if (retval)
-		goto batt_failed;
-
-	/* Schedule a polling after about 1 min */
-	schedule_delayed_work(&di->work, 60 * HZ);
-
-	i2c_set_clientdata(client, di);
-
-	if (client->irq) {
-		retval = devm_request_threaded_irq(&client->dev, client->irq,
-				NULL, bq27xxx_battery_irq_handler_thread,
-				IRQF_ONESHOT,
-				name, di);
-		if (retval) {
-			dev_err(&client->dev,
-				"Unable to register IRQ %d error %d\n",
-				client->irq, retval);
-			return retval;
-		}
-	}
-
-	return 0;
-
-batt_failed:
-	mutex_lock(&battery_mutex);
-	idr_remove(&battery_id, num);
-	mutex_unlock(&battery_mutex);
-
-	return retval;
-}
-
-static int bq27xxx_battery_i2c_remove(struct i2c_client *client)
-{
-	struct bq27xxx_device_info *di = i2c_get_clientdata(client);
-
-	bq27xxx_powersupply_unregister(di);
-
-	mutex_lock(&battery_mutex);
-	idr_remove(&battery_id, di->id);
-	mutex_unlock(&battery_mutex);
-
-	return 0;
-}
-
-static const struct i2c_device_id bq27xxx_id[] = {
-	{ "bq27200", BQ27000 },
-	{ "bq27210", BQ27010 },
-	{ "bq27500", BQ27500 },
-	{ "bq27510", BQ27500 },
-	{ "bq27520", BQ27500 },
-	{ "bq27530", BQ27530 },
-	{ "bq27531", BQ27530 },
-	{ "bq27541", BQ27541 },
-	{ "bq27542", BQ27541 },
-	{ "bq27546", BQ27541 },
-	{ "bq27742", BQ27541 },
-	{ "bq27545", BQ27545 },
-	{ "bq27421", BQ27421 },
-	{ "bq27425", BQ27421 },
-	{ "bq27441", BQ27421 },
-	{ "bq27621", BQ27421 },
-	{},
-};
-MODULE_DEVICE_TABLE(i2c, bq27xxx_id);
-
-static struct i2c_driver bq27xxx_battery_i2c_driver = {
-	.driver = {
-		.name = "bq27xxx-battery",
-	},
-	.probe = bq27xxx_battery_i2c_probe,
-	.remove = bq27xxx_battery_i2c_remove,
-	.id_table = bq27xxx_id,
-};
-
-static inline int bq27xxx_battery_i2c_init(void)
-{
-	int ret = i2c_add_driver(&bq27xxx_battery_i2c_driver);
-
-	if (ret)
-		pr_err("Unable to register BQ27xxx i2c driver\n");
-
-	return ret;
-}
-
-static inline void bq27xxx_battery_i2c_exit(void)
-{
-	i2c_del_driver(&bq27xxx_battery_i2c_driver);
-}
-
-#else
-
-static inline int bq27xxx_battery_i2c_init(void) { return 0; }
-static inline void bq27xxx_battery_i2c_exit(void) {};
-
-#endif
-
-/* platform specific code */
-#ifdef CONFIG_BATTERY_BQ27XXX_PLATFORM
+EXPORT_SYMBOL_GPL(bq27xxx_battery_teardown);
 
 static int bq27xxx_battery_platform_read(struct bq27xxx_device_info *di, u8 reg,
 					 bool single)
@@ -1267,7 +1045,6 @@ static int bq27xxx_battery_platform_probe(struct platform_device *pdev)
 {
 	struct bq27xxx_device_info *di;
 	struct bq27xxx_platform_data *pdata = pdev->dev.platform_data;
-	const char *name;
 
 	if (!pdata) {
 		dev_err(&pdev->dev, "no platform_data supplied\n");
@@ -1292,83 +1069,36 @@ static int bq27xxx_battery_platform_probe(struct platform_device *pdev)
 
 	di->dev = &pdev->dev;
 	di->chip = pdata->chip;
-	di->regs = bq27xxx_regs[di->chip];
-
-	name = pdata->name ?: dev_name(&pdev->dev);
-	di->bus.read = &bq27xxx_battery_platform_read;
+	di->name = pdata->name ?: dev_name(&pdev->dev);
+	di->bus.read = bq27xxx_battery_platform_read;
 
-	return bq27xxx_powersupply_init(di, name);
+	return bq27xxx_battery_setup(di);
 }
 
 static int bq27xxx_battery_platform_remove(struct platform_device *pdev)
 {
 	struct bq27xxx_device_info *di = platform_get_drvdata(pdev);
 
-	bq27xxx_powersupply_unregister(di);
+	bq27xxx_battery_teardown(di);
 
 	return 0;
 }
 
+static const struct platform_device_id bq27xxx_battery_platform_id_table[] = {
+	{ "bq27000-battery", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, bq27xxx_battery_platform_id_table);
+
 static struct platform_driver bq27xxx_battery_platform_driver = {
 	.probe	= bq27xxx_battery_platform_probe,
 	.remove = bq27xxx_battery_platform_remove,
 	.driver = {
 		.name = "bq27000-battery",
 	},
+	.id_table = bq27xxx_battery_platform_id_table,
 };
-
-static inline int bq27xxx_battery_platform_init(void)
-{
-	int ret = platform_driver_register(&bq27xxx_battery_platform_driver);
-
-	if (ret)
-		pr_err("Unable to register BQ27xxx platform driver\n");
-
-	return ret;
-}
-
-static inline void bq27xxx_battery_platform_exit(void)
-{
-	platform_driver_unregister(&bq27xxx_battery_platform_driver);
-}
-
-#else
-
-static inline int bq27xxx_battery_platform_init(void) { return 0; }
-static inline void bq27xxx_battery_platform_exit(void) {};
-
-#endif
-
-/*
- * Module stuff
- */
-
-static int __init bq27xxx_battery_init(void)
-{
-	int ret;
-
-	ret = bq27xxx_battery_i2c_init();
-	if (ret)
-		return ret;
-
-	ret = bq27xxx_battery_platform_init();
-	if (ret)
-		bq27xxx_battery_i2c_exit();
-
-	return ret;
-}
-module_init(bq27xxx_battery_init);
-
-static void __exit bq27xxx_battery_exit(void)
-{
-	bq27xxx_battery_platform_exit();
-	bq27xxx_battery_i2c_exit();
-}
-module_exit(bq27xxx_battery_exit);
-
-#ifdef CONFIG_BATTERY_BQ27XXX_PLATFORM
-MODULE_ALIAS("platform:bq27000-battery");
-#endif
+module_platform_driver(bq27xxx_battery_platform_driver);
 
 MODULE_AUTHOR("Rodolfo Giometti <giometti@linux.it>");
 MODULE_DESCRIPTION("BQ27xxx battery monitor driver");
diff --git a/drivers/power/bq27xxx_battery_i2c.c b/drivers/power/bq27xxx_battery_i2c.c
new file mode 100644
index 000000000000..8eafc6f0df88
--- /dev/null
+++ b/drivers/power/bq27xxx_battery_i2c.c
@@ -0,0 +1,181 @@
+/*
+ * SCI Reset driver for Keystone based devices
+ *
+ * Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/
+ *	Andrew F. Davis <afd@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 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/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <asm/unaligned.h>
+
+#include <linux/power/bq27xxx_battery.h>
+
+static DEFINE_IDR(battery_id);
+static DEFINE_MUTEX(battery_mutex);
+
+static irqreturn_t bq27xxx_battery_irq_handler_thread(int irq, void *data)
+{
+	struct bq27xxx_device_info *di = data;
+
+	bq27xxx_battery_update(di);
+
+	return IRQ_HANDLED;
+}
+
+static int bq27xxx_battery_i2c_read(struct bq27xxx_device_info *di, u8 reg,
+				    bool single)
+{
+	struct i2c_client *client = to_i2c_client(di->dev);
+	struct i2c_msg msg[2];
+	unsigned char data[2];
+	int ret;
+
+	if (!client->adapter)
+		return -ENODEV;
+
+	msg[0].addr = client->addr;
+	msg[0].flags = 0;
+	msg[0].buf = &reg;
+	msg[0].len = sizeof(reg);
+	msg[1].addr = client->addr;
+	msg[1].flags = I2C_M_RD;
+	msg[1].buf = data;
+	if (single)
+		msg[1].len = 1;
+	else
+		msg[1].len = 2;
+
+	ret = i2c_transfer(client->adapter, msg, ARRAY_SIZE(msg));
+	if (ret < 0)
+		return ret;
+
+	if (!single)
+		ret = get_unaligned_le16(data);
+	else
+		ret = data[0];
+
+	return ret;
+}
+
+static int bq27xxx_battery_i2c_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct bq27xxx_device_info *di;
+	int ret;
+	char *name;
+	int num;
+
+	/* Get new ID for the new battery device */
+	mutex_lock(&battery_mutex);
+	num = idr_alloc(&battery_id, client, 0, 0, GFP_KERNEL);
+	mutex_unlock(&battery_mutex);
+	if (num < 0)
+		return num;
+
+	name = devm_kasprintf(&client->dev, GFP_KERNEL, "%s-%d", id->name, num);
+	if (!name)
+		goto err_mem;
+
+	di = devm_kzalloc(&client->dev, sizeof(*di), GFP_KERNEL);
+	if (!di)
+		goto err_mem;
+
+	di->id = num;
+	di->dev = &client->dev;
+	di->chip = id->driver_data;
+	di->name = name;
+	di->bus.read = bq27xxx_battery_i2c_read;
+
+	ret = bq27xxx_battery_setup(di);
+	if (ret)
+		goto err_failed;
+
+	/* Schedule a polling after about 1 min */
+	schedule_delayed_work(&di->work, 60 * HZ);
+
+	i2c_set_clientdata(client, di);
+
+	if (client->irq) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+				NULL, bq27xxx_battery_irq_handler_thread,
+				IRQF_ONESHOT,
+				di->name, di);
+		if (ret) {
+			dev_err(&client->dev,
+				"Unable to register IRQ %d error %d\n",
+				client->irq, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+
+err_mem:
+	ret = -ENOMEM;
+
+err_failed:
+	mutex_lock(&battery_mutex);
+	idr_remove(&battery_id, num);
+	mutex_unlock(&battery_mutex);
+
+	return ret;
+}
+
+static int bq27xxx_battery_i2c_remove(struct i2c_client *client)
+{
+	struct bq27xxx_device_info *di = i2c_get_clientdata(client);
+
+	bq27xxx_battery_teardown(di);
+
+	mutex_lock(&battery_mutex);
+	idr_remove(&battery_id, di->id);
+	mutex_unlock(&battery_mutex);
+
+	return 0;
+}
+
+static const struct i2c_device_id bq27xxx_i2c_id_table[] = {
+	{ "bq27200", BQ27000 },
+	{ "bq27210", BQ27010 },
+	{ "bq27500", BQ27500 },
+	{ "bq27510", BQ27500 },
+	{ "bq27520", BQ27500 },
+	{ "bq27530", BQ27530 },
+	{ "bq27531", BQ27530 },
+	{ "bq27541", BQ27541 },
+	{ "bq27542", BQ27541 },
+	{ "bq27546", BQ27541 },
+	{ "bq27742", BQ27541 },
+	{ "bq27545", BQ27545 },
+	{ "bq27421", BQ27421 },
+	{ "bq27425", BQ27421 },
+	{ "bq27441", BQ27421 },
+	{ "bq27621", BQ27421 },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, bq27xxx_i2c_id_table);
+
+static struct i2c_driver bq27xxx_battery_i2c_driver = {
+	.driver = {
+		.name = "bq27xxx-battery",
+	},
+	.probe = bq27xxx_battery_i2c_probe,
+	.remove = bq27xxx_battery_i2c_remove,
+	.id_table = bq27xxx_i2c_id_table,
+};
+module_i2c_driver(bq27xxx_battery_i2c_driver);
+
+MODULE_AUTHOR("Andrew F. Davis <afd@ti.com>");
+MODULE_DESCRIPTION("BQ27xxx battery monitor i2c driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/ds2782_battery.c b/drivers/power/ds2782_battery.c
index ed4d756d21e4..a1b7e0592245 100644
--- a/drivers/power/ds2782_battery.c
+++ b/drivers/power/ds2782_battery.c
@@ -59,7 +59,7 @@ struct ds278x_info {
 	struct i2c_client	*client;
 	struct power_supply	*battery;
 	struct power_supply_desc	battery_desc;
-	struct ds278x_battery_ops  *ops;
+	const struct ds278x_battery_ops *ops;
 	struct delayed_work	bat_work;
 	int			id;
 	int                     rsns;
@@ -361,7 +361,7 @@ enum ds278x_num_id {
 	DS2786,
 };
 
-static struct ds278x_battery_ops ds278x_ops[] = {
+static const struct ds278x_battery_ops ds278x_ops[] = {
 	[DS2782] = {
 		.get_battery_current  = ds2782_get_current,
 		.get_battery_voltage  = ds2782_get_voltage,
diff --git a/drivers/power/generic-adc-battery.c b/drivers/power/generic-adc-battery.c
index fedc5818fab7..edb36bf781b0 100644
--- a/drivers/power/generic-adc-battery.c
+++ b/drivers/power/generic-adc-battery.c
@@ -206,7 +206,7 @@ static void gab_work(struct work_struct *work)
 	bool is_plugged;
 	int status;
 
-	delayed_work = container_of(work, struct delayed_work, work);
+	delayed_work = to_delayed_work(work);
 	adc_bat = container_of(delayed_work, struct gab, bat_work);
 	pdata = adc_bat->pdata;
 	status = adc_bat->status;
diff --git a/drivers/power/isp1704_charger.c b/drivers/power/isp1704_charger.c
index f2a7d970388f..46a292aa182d 100644
--- a/drivers/power/isp1704_charger.c
+++ b/drivers/power/isp1704_charger.c
@@ -76,7 +76,7 @@ static inline int isp1704_read(struct isp1704_charger *isp, u32 reg)
 	return usb_phy_io_read(isp->phy, reg);
 }
 
-static inline int isp1704_write(struct isp1704_charger *isp, u32 val, u32 reg)
+static inline int isp1704_write(struct isp1704_charger *isp, u32 reg, u32 val)
 {
 	return usb_phy_io_write(isp->phy, val, reg);
 }
diff --git a/drivers/power/max8903_charger.c b/drivers/power/max8903_charger.c
index 6d39d52040d4..17876caf31e5 100644
--- a/drivers/power/max8903_charger.c
+++ b/drivers/power/max8903_charger.c
@@ -291,10 +291,10 @@ static int max8903_probe(struct platform_device *pdev)
 
 	if (pdata->dc_valid) {
 		ret = devm_request_threaded_irq(dev, gpio_to_irq(pdata->dok),
-						NULL, max8903_dcin,
-						IRQF_TRIGGER_FALLING |
-						IRQF_TRIGGER_RISING,
-						"MAX8903 DC IN", data);
+					NULL, max8903_dcin,
+					IRQF_TRIGGER_FALLING |
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					"MAX8903 DC IN", data);
 		if (ret) {
 			dev_err(dev, "Cannot request irq %d for DC (%d)\n",
 					gpio_to_irq(pdata->dok), ret);
@@ -304,10 +304,10 @@ static int max8903_probe(struct platform_device *pdev)
 
 	if (pdata->usb_valid) {
 		ret = devm_request_threaded_irq(dev, gpio_to_irq(pdata->uok),
-						NULL, max8903_usbin,
-						IRQF_TRIGGER_FALLING |
-						IRQF_TRIGGER_RISING,
-						"MAX8903 USB IN", data);
+					NULL, max8903_usbin,
+					IRQF_TRIGGER_FALLING |
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					"MAX8903 USB IN", data);
 		if (ret) {
 			dev_err(dev, "Cannot request irq %d for USB (%d)\n",
 					gpio_to_irq(pdata->uok), ret);
@@ -317,10 +317,10 @@ static int max8903_probe(struct platform_device *pdev)
 
 	if (pdata->flt) {
 		ret = devm_request_threaded_irq(dev, gpio_to_irq(pdata->flt),
-						NULL, max8903_fault,
-						IRQF_TRIGGER_FALLING |
-						IRQF_TRIGGER_RISING,
-						"MAX8903 Fault", data);
+					NULL, max8903_fault,
+					IRQF_TRIGGER_FALLING |
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					"MAX8903 Fault", data);
 		if (ret) {
 			dev_err(dev, "Cannot request irq %d for Fault (%d)\n",
 					gpio_to_irq(pdata->flt), ret);
diff --git a/drivers/power/reset/at91-reset.c b/drivers/power/reset/at91-reset.c
index 3f6b5dd7c3d4..1b5d450586d1 100644
--- a/drivers/power/reset/at91-reset.c
+++ b/drivers/power/reset/at91-reset.c
@@ -198,6 +198,7 @@ static int __init at91_reset_probe(struct platform_device *pdev)
 			at91_ramc_base[idx] = of_iomap(np, 0);
 			if (!at91_ramc_base[idx]) {
 				dev_err(&pdev->dev, "Could not map ram controller address\n");
+				of_node_put(np);
 				return -ENODEV;
 			}
 			idx++;
diff --git a/drivers/power/test_power.c b/drivers/power/test_power.c
index 83c42ea88f2b..57246cdbd042 100644
--- a/drivers/power/test_power.c
+++ b/drivers/power/test_power.c
@@ -301,6 +301,8 @@ static int map_get_value(struct battery_property_map *map, const char *key,
 	buf[MAX_KEYLENGTH-1] = '\0';
 
 	cr = strnlen(buf, MAX_KEYLENGTH) - 1;
+	if (cr < 0)
+		return def_val;
 	if (buf[cr] == '\n')
 		buf[cr] = '\0';
 
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index 48747c28a43d..6c592dc71aee 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -388,7 +388,7 @@ static int get_domain_enable(struct powercap_zone *power_zone, bool *mode)
 }
 
 /* per RAPL domain ops, in the order of rapl_domain_type */
-static struct powercap_zone_ops zone_ops[] = {
+static const struct powercap_zone_ops zone_ops[] = {
 	/* RAPL_DOMAIN_PACKAGE */
 	{
 		.get_energy_uj = get_energy_counter,
@@ -584,7 +584,7 @@ static int get_max_power(struct powercap_zone *power_zone, int id,
 	return ret;
 }
 
-static struct powercap_zone_constraint_ops constraint_ops = {
+static const struct powercap_zone_constraint_ops constraint_ops = {
 	.set_power_limit_uw = set_power_limit,
 	.get_power_limit_uw = get_current_power_limit,
 	.set_time_window_us = set_time_window,
@@ -988,16 +988,16 @@ static void set_floor_freq_atom(struct rapl_domain *rd, bool enable)
 	}
 
 	if (!power_ctrl_orig_val)
-		iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_PMC_READ,
-			rapl_defaults->floor_freq_reg_addr,
-				&power_ctrl_orig_val);
+		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_CR_READ,
+			      rapl_defaults->floor_freq_reg_addr,
+			      &power_ctrl_orig_val);
 	mdata = power_ctrl_orig_val;
 	if (enable) {
 		mdata &= ~(0x7f << 8);
 		mdata |= 1 << 8;
 	}
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_PMC_WRITE,
-		rapl_defaults->floor_freq_reg_addr, mdata);
+	iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_CR_WRITE,
+		       rapl_defaults->floor_freq_reg_addr, mdata);
 }
 
 static u64 rapl_compute_time_window_core(struct rapl_package *rp, u64 value,
diff --git a/drivers/powercap/powercap_sys.c b/drivers/powercap/powercap_sys.c
index 84419af16f77..14bde0db8c24 100644
--- a/drivers/powercap/powercap_sys.c
+++ b/drivers/powercap/powercap_sys.c
@@ -293,8 +293,8 @@ err_alloc:
 }
 
 static int create_constraints(struct powercap_zone *power_zone,
-				int nr_constraints,
-				struct powercap_zone_constraint_ops *const_ops)
+			int nr_constraints,
+			const struct powercap_zone_constraint_ops *const_ops)
 {
 	int i;
 	int ret = 0;
@@ -492,13 +492,13 @@ static struct class powercap_class = {
 };
 
 struct powercap_zone *powercap_register_zone(
-				struct powercap_zone *power_zone,
-				struct powercap_control_type *control_type,
-				const char *name,
-				struct powercap_zone *parent,
-				const struct powercap_zone_ops *ops,
-				int nr_constraints,
-				struct powercap_zone_constraint_ops *const_ops)
+			struct powercap_zone *power_zone,
+			struct powercap_control_type *control_type,
+			const char *name,
+			struct powercap_zone *parent,
+			const struct powercap_zone_ops *ops,
+			int nr_constraints,
+			const struct powercap_zone_constraint_ops *const_ops)
 {
 	int result;
 	int nr_attrs;
diff --git a/drivers/ptp/ptp_ixp46x.c b/drivers/ptp/ptp_ixp46x.c
index 934c139916c6..ee4f183ef9ee 100644
--- a/drivers/ptp/ptp_ixp46x.c
+++ b/drivers/ptp/ptp_ixp46x.c
@@ -178,7 +178,6 @@ static int ptp_ixp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
 	u64 ns;
-	u32 remainder;
 	unsigned long flags;
 	struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
 	struct ixp46x_ts_regs *regs = ixp_clock->regs;
@@ -189,8 +188,7 @@ static int ptp_ixp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 
 	spin_unlock_irqrestore(&register_lock, flags);
 
-	ts->tv_sec = div_u64_rem(ns, 1000000000, &remainder);
-	ts->tv_nsec = remainder;
+	*ts = ns_to_timespec64(ns);
 	return 0;
 }
 
@@ -202,8 +200,7 @@ static int ptp_ixp_settime(struct ptp_clock_info *ptp,
 	struct ixp_clock *ixp_clock = container_of(ptp, struct ixp_clock, caps);
 	struct ixp46x_ts_regs *regs = ixp_clock->regs;
 
-	ns = ts->tv_sec * 1000000000ULL;
-	ns += ts->tv_nsec;
+	ns = timespec64_to_ns(ts);
 
 	spin_lock_irqsave(&register_lock, flags);
 
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index 2f4641a0e88b..8cf0dae78555 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -148,6 +148,7 @@ config PWM_EP93XX
 
 config PWM_FSL_FTM
 	tristate "Freescale FlexTimer Module (FTM) PWM support"
+	depends on HAS_IOMEM
 	depends on OF
 	select REGMAP_MMIO
 	help
@@ -222,18 +223,12 @@ config PWM_LPC32XX
 	  will be called pwm-lpc32xx.
 
 config PWM_LPSS
-	tristate "Intel LPSS PWM support"
-	depends on X86
-	help
-	  Generic PWM framework driver for Intel Low Power Subsystem PWM
-	  controller.
-
-	  To compile this driver as a module, choose M here: the module
-	  will be called pwm-lpss.
+	tristate
 
 config PWM_LPSS_PCI
 	tristate "Intel LPSS PWM PCI driver"
-	depends on PWM_LPSS && PCI
+	depends on X86 && PCI
+	select PWM_LPSS
 	help
 	  The PCI driver for Intel Low Power Subsystem PWM controller.
 
@@ -242,7 +237,8 @@ config PWM_LPSS_PCI
 
 config PWM_LPSS_PLATFORM
 	tristate "Intel LPSS PWM platform driver"
-	depends on PWM_LPSS && ACPI
+	depends on X86 && ACPI
+	select PWM_LPSS
 	help
 	  The platform driver for Intel Low Power Subsystem PWM controller.
 
@@ -270,6 +266,15 @@ config PWM_MXS
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-mxs.
 
+config PWM_OMAP_DMTIMER
+	tristate "OMAP Dual-Mode Timer PWM support"
+	depends on OF && ARCH_OMAP && OMAP_DM_TIMER
+	help
+	  Generic PWM framework driver for OMAP Dual-Mode Timer PWM output
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-omap-dmtimer
+
 config PWM_PCA9685
 	tristate "NXP PCA9685 PWM driver"
 	depends on I2C
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index 69b8275f3c08..dd35bc121a18 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -24,6 +24,7 @@ obj-$(CONFIG_PWM_LPSS_PCI)	+= pwm-lpss-pci.o
 obj-$(CONFIG_PWM_LPSS_PLATFORM)	+= pwm-lpss-platform.o
 obj-$(CONFIG_PWM_MTK_DISP)	+= pwm-mtk-disp.o
 obj-$(CONFIG_PWM_MXS)		+= pwm-mxs.o
+obj-$(CONFIG_PWM_OMAP_DMTIMER)	+= pwm-omap-dmtimer.o
 obj-$(CONFIG_PWM_PCA9685)	+= pwm-pca9685.o
 obj-$(CONFIG_PWM_PUV3)		+= pwm-puv3.o
 obj-$(CONFIG_PWM_PXA)		+= pwm-pxa.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index d24ca5f281b4..7831bc6b51dd 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -889,7 +889,7 @@ EXPORT_SYMBOL_GPL(devm_pwm_put);
   */
 bool pwm_can_sleep(struct pwm_device *pwm)
 {
-	return pwm->chip->can_sleep;
+	return true;
 }
 EXPORT_SYMBOL_GPL(pwm_can_sleep);
 
diff --git a/drivers/pwm/pwm-bcm2835.c b/drivers/pwm/pwm-bcm2835.c
index b4c7f956b6fa..c5dbf16d810b 100644
--- a/drivers/pwm/pwm-bcm2835.c
+++ b/drivers/pwm/pwm-bcm2835.c
@@ -29,7 +29,6 @@
 struct bcm2835_pwm {
 	struct pwm_chip chip;
 	struct device *dev;
-	unsigned long scaler;
 	void __iomem *base;
 	struct clk *clk;
 };
@@ -66,6 +65,15 @@ static int bcm2835_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 			      int duty_ns, int period_ns)
 {
 	struct bcm2835_pwm *pc = to_bcm2835_pwm(chip);
+	unsigned long rate = clk_get_rate(pc->clk);
+	unsigned long scaler;
+
+	if (!rate) {
+		dev_err(pc->dev, "failed to get clock rate\n");
+		return -EINVAL;
+	}
+
+	scaler = NSEC_PER_SEC / rate;
 
 	if (period_ns <= MIN_PERIOD) {
 		dev_err(pc->dev, "period %d not supported, minimum %d\n",
@@ -73,8 +81,8 @@ static int bcm2835_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 		return -EINVAL;
 	}
 
-	writel(duty_ns / pc->scaler, pc->base + DUTY(pwm->hwpwm));
-	writel(period_ns / pc->scaler, pc->base + PERIOD(pwm->hwpwm));
+	writel(duty_ns / scaler, pc->base + DUTY(pwm->hwpwm));
+	writel(period_ns / scaler, pc->base + PERIOD(pwm->hwpwm));
 
 	return 0;
 }
@@ -156,8 +164,6 @@ static int bcm2835_pwm_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	pc->scaler = NSEC_PER_SEC / clk_get_rate(pc->clk);
-
 	pc->chip.dev = &pdev->dev;
 	pc->chip.ops = &bcm2835_pwm_ops;
 	pc->chip.npwm = 2;
@@ -200,6 +206,6 @@ static struct platform_driver bcm2835_pwm_driver = {
 };
 module_platform_driver(bcm2835_pwm_driver);
 
-MODULE_AUTHOR("Bart Tanghe <bart.tanghe@thomasmore.be");
+MODULE_AUTHOR("Bart Tanghe <bart.tanghe@thomasmore.be>");
 MODULE_DESCRIPTION("Broadcom BCM2835 PWM driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-fsl-ftm.c b/drivers/pwm/pwm-fsl-ftm.c
index f9dfc8b6407a..7225ac6b3df5 100644
--- a/drivers/pwm/pwm-fsl-ftm.c
+++ b/drivers/pwm/pwm-fsl-ftm.c
@@ -80,7 +80,6 @@ struct fsl_pwm_chip {
 
 	struct mutex lock;
 
-	unsigned int use_count;
 	unsigned int cnt_select;
 	unsigned int clk_ps;
 
@@ -300,9 +299,6 @@ static int fsl_counter_clock_enable(struct fsl_pwm_chip *fpc)
 {
 	int ret;
 
-	if (fpc->use_count++ != 0)
-		return 0;
-
 	/* select counter clock source */
 	regmap_update_bits(fpc->regmap, FTM_SC, FTM_SC_CLK_MASK,
 			   FTM_SC_CLK(fpc->cnt_select));
@@ -334,25 +330,6 @@ static int fsl_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	return ret;
 }
 
-static void fsl_counter_clock_disable(struct fsl_pwm_chip *fpc)
-{
-	/*
-	 * already disabled, do nothing
-	 */
-	if (fpc->use_count == 0)
-		return;
-
-	/* there are still users, so can't disable yet */
-	if (--fpc->use_count > 0)
-		return;
-
-	/* no users left, disable PWM counter clock */
-	regmap_update_bits(fpc->regmap, FTM_SC, FTM_SC_CLK_MASK, 0);
-
-	clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_CNTEN]);
-	clk_disable_unprepare(fpc->clk[fpc->cnt_select]);
-}
-
 static void fsl_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct fsl_pwm_chip *fpc = to_fsl_chip(chip);
@@ -362,7 +339,8 @@ static void fsl_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 	regmap_update_bits(fpc->regmap, FTM_OUTMASK, BIT(pwm->hwpwm),
 			   BIT(pwm->hwpwm));
 
-	fsl_counter_clock_disable(fpc);
+	clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_CNTEN]);
+	clk_disable_unprepare(fpc->clk[fpc->cnt_select]);
 
 	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
 	if ((val & 0xFF) == 0xFF)
@@ -492,17 +470,24 @@ static int fsl_pwm_remove(struct platform_device *pdev)
 static int fsl_pwm_suspend(struct device *dev)
 {
 	struct fsl_pwm_chip *fpc = dev_get_drvdata(dev);
-	u32 val;
+	int i;
 
 	regcache_cache_only(fpc->regmap, true);
 	regcache_mark_dirty(fpc->regmap);
 
-	/* read from cache */
-	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
-	if ((val & 0xFF) != 0xFF) {
+	for (i = 0; i < fpc->chip.npwm; i++) {
+		struct pwm_device *pwm = &fpc->chip.pwms[i];
+
+		if (!test_bit(PWMF_REQUESTED, &pwm->flags))
+			continue;
+
+		clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_SYS]);
+
+		if (!pwm_is_enabled(pwm))
+			continue;
+
 		clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_CNTEN]);
 		clk_disable_unprepare(fpc->clk[fpc->cnt_select]);
-		clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_SYS]);
 	}
 
 	return 0;
@@ -511,12 +496,19 @@ static int fsl_pwm_suspend(struct device *dev)
 static int fsl_pwm_resume(struct device *dev)
 {
 	struct fsl_pwm_chip *fpc = dev_get_drvdata(dev);
-	u32 val;
+	int i;
+
+	for (i = 0; i < fpc->chip.npwm; i++) {
+		struct pwm_device *pwm = &fpc->chip.pwms[i];
+
+		if (!test_bit(PWMF_REQUESTED, &pwm->flags))
+			continue;
 
-	/* read from cache */
-	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
-	if ((val & 0xFF) != 0xFF) {
 		clk_prepare_enable(fpc->clk[FSL_PWM_CLK_SYS]);
+
+		if (!pwm_is_enabled(pwm))
+			continue;
+
 		clk_prepare_enable(fpc->clk[fpc->cnt_select]);
 		clk_prepare_enable(fpc->clk[FSL_PWM_CLK_CNTEN]);
 	}
diff --git a/drivers/pwm/pwm-lpc32xx.c b/drivers/pwm/pwm-lpc32xx.c
index 9fde60ce8e7b..4d470c1a406a 100644
--- a/drivers/pwm/pwm-lpc32xx.c
+++ b/drivers/pwm/pwm-lpc32xx.c
@@ -24,9 +24,7 @@ struct lpc32xx_pwm_chip {
 	void __iomem *base;
 };
 
-#define PWM_ENABLE	(1 << 31)
-#define PWM_RELOADV(x)	(((x) & 0xFF) << 8)
-#define PWM_DUTY(x)	((x) & 0xFF)
+#define PWM_ENABLE	BIT(31)
 
 #define to_lpc32xx_pwm_chip(_chip) \
 	container_of(_chip, struct lpc32xx_pwm_chip, chip)
@@ -38,40 +36,27 @@ static int lpc32xx_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	unsigned long long c;
 	int period_cycles, duty_cycles;
 	u32 val;
-
-	c = clk_get_rate(lpc32xx->clk) / 256;
-	c = c * period_ns;
-	do_div(c, NSEC_PER_SEC);
-
-	/* Handle high and low extremes */
-	if (c == 0)
-		c = 1;
-	if (c > 255)
-		c = 0; /* 0 set division by 256 */
-	period_cycles = c;
-
-	/* The duty-cycle value is as follows:
-	 *
-	 *  DUTY-CYCLE     HIGH LEVEL
-	 *      1            99.9%
-	 *      25           90.0%
-	 *      128          50.0%
-	 *      220          10.0%
-	 *      255           0.1%
-	 *      0             0.0%
-	 *
-	 * In other words, the register value is duty-cycle % 256 with
-	 * duty-cycle in the range 1-256.
-	 */
-	c = 256 * duty_ns;
-	do_div(c, period_ns);
-	if (c > 255)
-		c = 255;
-	duty_cycles = 256 - c;
+	c = clk_get_rate(lpc32xx->clk);
+
+	/* The highest acceptable divisor is 256, which is represented by 0 */
+	period_cycles = div64_u64(c * period_ns,
+			       (unsigned long long)NSEC_PER_SEC * 256);
+	if (!period_cycles || period_cycles > 256)
+		return -ERANGE;
+	if (period_cycles == 256)
+		period_cycles = 0;
+
+	/* Compute 256 x #duty/period value and care for corner cases */
+	duty_cycles = div64_u64((unsigned long long)(period_ns - duty_ns) * 256,
+				period_ns);
+	if (!duty_cycles)
+		duty_cycles = 1;
+	if (duty_cycles > 255)
+		duty_cycles = 255;
 
 	val = readl(lpc32xx->base + (pwm->hwpwm << 2));
 	val &= ~0xFFFF;
-	val |= PWM_RELOADV(period_cycles) | PWM_DUTY(duty_cycles);
+	val |= (period_cycles << 8) | duty_cycles;
 	writel(val, lpc32xx->base + (pwm->hwpwm << 2));
 
 	return 0;
@@ -83,7 +68,7 @@ static int lpc32xx_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	u32 val;
 	int ret;
 
-	ret = clk_enable(lpc32xx->clk);
+	ret = clk_prepare_enable(lpc32xx->clk);
 	if (ret)
 		return ret;
 
@@ -103,7 +88,7 @@ static void lpc32xx_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 	val &= ~PWM_ENABLE;
 	writel(val, lpc32xx->base + (pwm->hwpwm << 2));
 
-	clk_disable(lpc32xx->clk);
+	clk_disable_unprepare(lpc32xx->clk);
 }
 
 static const struct pwm_ops lpc32xx_pwm_ops = {
@@ -134,7 +119,7 @@ static int lpc32xx_pwm_probe(struct platform_device *pdev)
 
 	lpc32xx->chip.dev = &pdev->dev;
 	lpc32xx->chip.ops = &lpc32xx_pwm_ops;
-	lpc32xx->chip.npwm = 2;
+	lpc32xx->chip.npwm = 1;
 	lpc32xx->chip.base = -1;
 
 	ret = pwmchip_add(&lpc32xx->chip);
diff --git a/drivers/pwm/pwm-lpss.c b/drivers/pwm/pwm-lpss.c
index 25044104003b..295b963dbddb 100644
--- a/drivers/pwm/pwm-lpss.c
+++ b/drivers/pwm/pwm-lpss.c
@@ -13,10 +13,12 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
+#include <linux/time.h>
 
 #include "pwm-lpss.h"
 
@@ -24,11 +26,8 @@
 #define PWM_ENABLE			BIT(31)
 #define PWM_SW_UPDATE			BIT(30)
 #define PWM_BASE_UNIT_SHIFT		8
-#define PWM_BASE_UNIT_MASK		0x00ffff00
 #define PWM_ON_TIME_DIV_MASK		0x000000ff
 #define PWM_DIVISION_CORRECTION		0x2
-#define PWM_LIMIT			(0x8000 + PWM_DIVISION_CORRECTION)
-#define NSECS_PER_SEC			1000000000UL
 
 /* Size of each PWM register space if multiple */
 #define PWM_SIZE			0x400
@@ -36,13 +35,14 @@
 struct pwm_lpss_chip {
 	struct pwm_chip chip;
 	void __iomem *regs;
-	unsigned long clk_rate;
+	const struct pwm_lpss_boardinfo *info;
 };
 
 /* BayTrail */
 const struct pwm_lpss_boardinfo pwm_lpss_byt_info = {
 	.clk_rate = 25000000,
 	.npwm = 1,
+	.base_unit_bits = 16,
 };
 EXPORT_SYMBOL_GPL(pwm_lpss_byt_info);
 
@@ -50,6 +50,7 @@ EXPORT_SYMBOL_GPL(pwm_lpss_byt_info);
 const struct pwm_lpss_boardinfo pwm_lpss_bsw_info = {
 	.clk_rate = 19200000,
 	.npwm = 1,
+	.base_unit_bits = 16,
 };
 EXPORT_SYMBOL_GPL(pwm_lpss_bsw_info);
 
@@ -57,6 +58,7 @@ EXPORT_SYMBOL_GPL(pwm_lpss_bsw_info);
 const struct pwm_lpss_boardinfo pwm_lpss_bxt_info = {
 	.clk_rate = 19200000,
 	.npwm = 4,
+	.base_unit_bits = 22,
 };
 EXPORT_SYMBOL_GPL(pwm_lpss_bxt_info);
 
@@ -79,28 +81,37 @@ static inline void pwm_lpss_write(const struct pwm_device *pwm, u32 value)
 	writel(value, lpwm->regs + pwm->hwpwm * PWM_SIZE + PWM);
 }
 
+static void pwm_lpss_update(struct pwm_device *pwm)
+{
+	pwm_lpss_write(pwm, pwm_lpss_read(pwm) | PWM_SW_UPDATE);
+	/* Give it some time to propagate */
+	usleep_range(10, 50);
+}
+
 static int pwm_lpss_config(struct pwm_chip *chip, struct pwm_device *pwm,
 			   int duty_ns, int period_ns)
 {
 	struct pwm_lpss_chip *lpwm = to_lpwm(chip);
 	u8 on_time_div;
-	unsigned long c;
-	unsigned long long base_unit, freq = NSECS_PER_SEC;
+	unsigned long c, base_unit_range;
+	unsigned long long base_unit, freq = NSEC_PER_SEC;
 	u32 ctrl;
 
 	do_div(freq, period_ns);
 
-	/* The equation is: base_unit = ((freq / c) * 65536) + correction */
-	base_unit = freq * 65536;
+	/*
+	 * The equation is:
+	 * base_unit = ((freq / c) * base_unit_range) + correction
+	 */
+	base_unit_range = BIT(lpwm->info->base_unit_bits);
+	base_unit = freq * base_unit_range;
 
-	c = lpwm->clk_rate;
+	c = lpwm->info->clk_rate;
 	if (!c)
 		return -EINVAL;
 
 	do_div(base_unit, c);
 	base_unit += PWM_DIVISION_CORRECTION;
-	if (base_unit > PWM_LIMIT)
-		return -EINVAL;
 
 	if (duty_ns <= 0)
 		duty_ns = 1;
@@ -109,13 +120,20 @@ static int pwm_lpss_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	pm_runtime_get_sync(chip->dev);
 
 	ctrl = pwm_lpss_read(pwm);
-	ctrl &= ~(PWM_BASE_UNIT_MASK | PWM_ON_TIME_DIV_MASK);
-	ctrl |= (u16) base_unit << PWM_BASE_UNIT_SHIFT;
+	ctrl &= ~PWM_ON_TIME_DIV_MASK;
+	ctrl &= ~((base_unit_range - 1) << PWM_BASE_UNIT_SHIFT);
+	base_unit &= (base_unit_range - 1);
+	ctrl |= (u32) base_unit << PWM_BASE_UNIT_SHIFT;
 	ctrl |= on_time_div;
-	/* request PWM to update on next cycle */
-	ctrl |= PWM_SW_UPDATE;
 	pwm_lpss_write(pwm, ctrl);
 
+	/*
+	 * If the PWM is already enabled we need to notify the hardware
+	 * about the change by setting PWM_SW_UPDATE.
+	 */
+	if (pwm_is_enabled(pwm))
+		pwm_lpss_update(pwm);
+
 	pm_runtime_put(chip->dev);
 
 	return 0;
@@ -124,6 +142,12 @@ static int pwm_lpss_config(struct pwm_chip *chip, struct pwm_device *pwm,
 static int pwm_lpss_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	pm_runtime_get_sync(chip->dev);
+
+	/*
+	 * Hardware must first see PWM_SW_UPDATE before the PWM can be
+	 * enabled.
+	 */
+	pwm_lpss_update(pwm);
 	pwm_lpss_write(pwm, pwm_lpss_read(pwm) | PWM_ENABLE);
 	return 0;
 }
@@ -135,7 +159,6 @@ static void pwm_lpss_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 }
 
 static const struct pwm_ops pwm_lpss_ops = {
-	.free = pwm_lpss_disable,
 	.config = pwm_lpss_config,
 	.enable = pwm_lpss_enable,
 	.disable = pwm_lpss_disable,
@@ -156,7 +179,7 @@ struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev, struct resource *r,
 	if (IS_ERR(lpwm->regs))
 		return ERR_CAST(lpwm->regs);
 
-	lpwm->clk_rate = info->clk_rate;
+	lpwm->info = info;
 	lpwm->chip.dev = dev;
 	lpwm->chip.ops = &pwm_lpss_ops;
 	lpwm->chip.base = -1;
diff --git a/drivers/pwm/pwm-lpss.h b/drivers/pwm/pwm-lpss.h
index e8cf337ae1d1..04766e0d41aa 100644
--- a/drivers/pwm/pwm-lpss.h
+++ b/drivers/pwm/pwm-lpss.h
@@ -21,6 +21,7 @@ struct pwm_lpss_chip;
 struct pwm_lpss_boardinfo {
 	unsigned long clk_rate;
 	unsigned int npwm;
+	unsigned long base_unit_bits;
 };
 
 extern const struct pwm_lpss_boardinfo pwm_lpss_byt_info;
diff --git a/drivers/pwm/pwm-omap-dmtimer.c b/drivers/pwm/pwm-omap-dmtimer.c
new file mode 100644
index 000000000000..826634ec0d5c
--- /dev/null
+++ b/drivers/pwm/pwm-omap-dmtimer.c
@@ -0,0 +1,327 @@
+/*
+ * Copyright (c) 2015 Neil Armstrong <narmstrong@baylibre.com>
+ * Copyright (c) 2014 Joachim Eastwood <manabian@gmail.com>
+ * Copyright (c) 2012 NeilBrown <neilb@suse.de>
+ * Heavily based on earlier code which is:
+ * Copyright (c) 2010 Grant Erickson <marathon96@gmail.com>
+ *
+ * Also based on pwm-samsung.c
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * Description:
+ *   This file is the core OMAP support for the generic, Linux
+ *   PWM driver / controller, using the OMAP's dual-mode timers.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_data/pwm_omap_dmtimer.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+
+#define DM_TIMER_LOAD_MIN 0xfffffffe
+
+struct pwm_omap_dmtimer_chip {
+	struct pwm_chip chip;
+	struct mutex mutex;
+	pwm_omap_dmtimer *dm_timer;
+	struct pwm_omap_dmtimer_pdata *pdata;
+	struct platform_device *dm_timer_pdev;
+};
+
+static inline struct pwm_omap_dmtimer_chip *
+to_pwm_omap_dmtimer_chip(struct pwm_chip *chip)
+{
+	return container_of(chip, struct pwm_omap_dmtimer_chip, chip);
+}
+
+static int pwm_omap_dmtimer_calc_value(unsigned long clk_rate, int ns)
+{
+	u64 c = (u64)clk_rate * ns;
+
+	do_div(c, NSEC_PER_SEC);
+
+	return DM_TIMER_LOAD_MIN - c;
+}
+
+static void pwm_omap_dmtimer_start(struct pwm_omap_dmtimer_chip *omap)
+{
+	/*
+	 * According to OMAP 4 TRM section 22.2.4.10 the counter should be
+	 * started at 0xFFFFFFFE when overflow and match is used to ensure
+	 * that the PWM line is toggled on the first event.
+	 *
+	 * Note that omap_dm_timer_enable/disable is for register access and
+	 * not the timer counter itself.
+	 */
+	omap->pdata->enable(omap->dm_timer);
+	omap->pdata->write_counter(omap->dm_timer, DM_TIMER_LOAD_MIN);
+	omap->pdata->disable(omap->dm_timer);
+
+	omap->pdata->start(omap->dm_timer);
+}
+
+static int pwm_omap_dmtimer_enable(struct pwm_chip *chip,
+				   struct pwm_device *pwm)
+{
+	struct pwm_omap_dmtimer_chip *omap = to_pwm_omap_dmtimer_chip(chip);
+
+	mutex_lock(&omap->mutex);
+	pwm_omap_dmtimer_start(omap);
+	mutex_unlock(&omap->mutex);
+
+	return 0;
+}
+
+static void pwm_omap_dmtimer_disable(struct pwm_chip *chip,
+				     struct pwm_device *pwm)
+{
+	struct pwm_omap_dmtimer_chip *omap = to_pwm_omap_dmtimer_chip(chip);
+
+	mutex_lock(&omap->mutex);
+	omap->pdata->stop(omap->dm_timer);
+	mutex_unlock(&omap->mutex);
+}
+
+static int pwm_omap_dmtimer_config(struct pwm_chip *chip,
+				   struct pwm_device *pwm,
+				   int duty_ns, int period_ns)
+{
+	struct pwm_omap_dmtimer_chip *omap = to_pwm_omap_dmtimer_chip(chip);
+	int load_value, match_value;
+	struct clk *fclk;
+	unsigned long clk_rate;
+	bool timer_active;
+
+	dev_dbg(chip->dev, "duty cycle: %d, period %d\n", duty_ns, period_ns);
+
+	mutex_lock(&omap->mutex);
+	if (duty_ns == pwm_get_duty_cycle(pwm) &&
+	    period_ns == pwm_get_period(pwm)) {
+		/* No change - don't cause any transients. */
+		mutex_unlock(&omap->mutex);
+		return 0;
+	}
+
+	fclk = omap->pdata->get_fclk(omap->dm_timer);
+	if (!fclk) {
+		dev_err(chip->dev, "invalid pmtimer fclk\n");
+		mutex_unlock(&omap->mutex);
+		return -EINVAL;
+	}
+
+	clk_rate = clk_get_rate(fclk);
+	if (!clk_rate) {
+		dev_err(chip->dev, "invalid pmtimer fclk rate\n");
+		mutex_unlock(&omap->mutex);
+		return -EINVAL;
+	}
+
+	dev_dbg(chip->dev, "clk rate: %luHz\n", clk_rate);
+
+	/*
+	 * Calculate the appropriate load and match values based on the
+	 * specified period and duty cycle. The load value determines the
+	 * cycle time and the match value determines the duty cycle.
+	 */
+	load_value = pwm_omap_dmtimer_calc_value(clk_rate, period_ns);
+	match_value = pwm_omap_dmtimer_calc_value(clk_rate,
+						  period_ns - duty_ns);
+
+	/*
+	 * We MUST stop the associated dual-mode timer before attempting to
+	 * write its registers, but calls to omap_dm_timer_start/stop must
+	 * be balanced so check if timer is active before calling timer_stop.
+	 */
+	timer_active = pm_runtime_active(&omap->dm_timer_pdev->dev);
+	if (timer_active)
+		omap->pdata->stop(omap->dm_timer);
+
+	omap->pdata->set_load(omap->dm_timer, true, load_value);
+	omap->pdata->set_match(omap->dm_timer, true, match_value);
+
+	dev_dbg(chip->dev, "load value: %#08x (%d), match value: %#08x (%d)\n",
+		load_value, load_value,	match_value, match_value);
+
+	omap->pdata->set_pwm(omap->dm_timer,
+			      pwm->polarity == PWM_POLARITY_INVERSED,
+			      true,
+			      PWM_OMAP_DMTIMER_TRIGGER_OVERFLOW_AND_COMPARE);
+
+	/* If config was called while timer was running it must be reenabled. */
+	if (timer_active)
+		pwm_omap_dmtimer_start(omap);
+
+	mutex_unlock(&omap->mutex);
+
+	return 0;
+}
+
+static int pwm_omap_dmtimer_set_polarity(struct pwm_chip *chip,
+					 struct pwm_device *pwm,
+					 enum pwm_polarity polarity)
+{
+	struct pwm_omap_dmtimer_chip *omap = to_pwm_omap_dmtimer_chip(chip);
+
+	/*
+	 * PWM core will not call set_polarity while PWM is enabled so it's
+	 * safe to reconfigure the timer here without stopping it first.
+	 */
+	mutex_lock(&omap->mutex);
+	omap->pdata->set_pwm(omap->dm_timer,
+			      polarity == PWM_POLARITY_INVERSED,
+			      true,
+			      PWM_OMAP_DMTIMER_TRIGGER_OVERFLOW_AND_COMPARE);
+	mutex_unlock(&omap->mutex);
+
+	return 0;
+}
+
+static const struct pwm_ops pwm_omap_dmtimer_ops = {
+	.enable	= pwm_omap_dmtimer_enable,
+	.disable = pwm_omap_dmtimer_disable,
+	.config	= pwm_omap_dmtimer_config,
+	.set_polarity = pwm_omap_dmtimer_set_polarity,
+	.owner = THIS_MODULE,
+};
+
+static int pwm_omap_dmtimer_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *timer;
+	struct pwm_omap_dmtimer_chip *omap;
+	struct pwm_omap_dmtimer_pdata *pdata;
+	pwm_omap_dmtimer *dm_timer;
+	u32 prescaler;
+	int status;
+
+	pdata = dev_get_platdata(&pdev->dev);
+	if (!pdata) {
+		dev_err(&pdev->dev, "Missing dmtimer platform data\n");
+		return -EINVAL;
+	}
+
+	if (!pdata->request_by_node ||
+	    !pdata->free ||
+	    !pdata->enable ||
+	    !pdata->disable ||
+	    !pdata->get_fclk ||
+	    !pdata->start ||
+	    !pdata->stop ||
+	    !pdata->set_load ||
+	    !pdata->set_match ||
+	    !pdata->set_pwm ||
+	    !pdata->set_prescaler ||
+	    !pdata->write_counter) {
+		dev_err(&pdev->dev, "Incomplete dmtimer pdata structure\n");
+		return -EINVAL;
+	}
+
+	timer = of_parse_phandle(np, "ti,timers", 0);
+	if (!timer)
+		return -ENODEV;
+
+	if (!of_get_property(timer, "ti,timer-pwm", NULL)) {
+		dev_err(&pdev->dev, "Missing ti,timer-pwm capability\n");
+		return -ENODEV;
+	}
+
+	dm_timer = pdata->request_by_node(timer);
+	if (!dm_timer)
+		return -EPROBE_DEFER;
+
+	omap = devm_kzalloc(&pdev->dev, sizeof(*omap), GFP_KERNEL);
+	if (!omap) {
+		pdata->free(dm_timer);
+		return -ENOMEM;
+	}
+
+	omap->pdata = pdata;
+	omap->dm_timer = dm_timer;
+
+	omap->dm_timer_pdev = of_find_device_by_node(timer);
+	if (!omap->dm_timer_pdev) {
+		dev_err(&pdev->dev, "Unable to find timer pdev\n");
+		omap->pdata->free(dm_timer);
+		return -EINVAL;
+	}
+
+	/*
+	 * Ensure that the timer is stopped before we allow PWM core to call
+	 * pwm_enable.
+	 */
+	if (pm_runtime_active(&omap->dm_timer_pdev->dev))
+		omap->pdata->stop(omap->dm_timer);
+
+	/* setup dmtimer prescaler */
+	if (!of_property_read_u32(pdev->dev.of_node, "ti,prescaler",
+				&prescaler))
+		omap->pdata->set_prescaler(omap->dm_timer, prescaler);
+
+	omap->chip.dev = &pdev->dev;
+	omap->chip.ops = &pwm_omap_dmtimer_ops;
+	omap->chip.base = -1;
+	omap->chip.npwm = 1;
+	omap->chip.of_xlate = of_pwm_xlate_with_flags;
+	omap->chip.of_pwm_n_cells = 3;
+
+	mutex_init(&omap->mutex);
+
+	status = pwmchip_add(&omap->chip);
+	if (status < 0) {
+		dev_err(&pdev->dev, "failed to register PWM\n");
+		omap->pdata->free(omap->dm_timer);
+		return status;
+	}
+
+	platform_set_drvdata(pdev, omap);
+
+	return 0;
+}
+
+static int pwm_omap_dmtimer_remove(struct platform_device *pdev)
+{
+	struct pwm_omap_dmtimer_chip *omap = platform_get_drvdata(pdev);
+
+	if (pm_runtime_active(&omap->dm_timer_pdev->dev))
+		omap->pdata->stop(omap->dm_timer);
+
+	omap->pdata->free(omap->dm_timer);
+
+	mutex_destroy(&omap->mutex);
+
+	return pwmchip_remove(&omap->chip);
+}
+
+static const struct of_device_id pwm_omap_dmtimer_of_match[] = {
+	{.compatible = "ti,omap-dmtimer-pwm"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, pwm_omap_dmtimer_of_match);
+
+static struct platform_driver pwm_omap_dmtimer_driver = {
+	.driver = {
+		.name = "omap-dmtimer-pwm",
+		.of_match_table = of_match_ptr(pwm_omap_dmtimer_of_match),
+	},
+	.probe = pwm_omap_dmtimer_probe,
+	.remove	= pwm_omap_dmtimer_remove,
+};
+module_platform_driver(pwm_omap_dmtimer_driver);
+
+MODULE_AUTHOR("Grant Erickson <marathon96@gmail.com>");
+MODULE_AUTHOR("NeilBrown <neilb@suse.de>");
+MODULE_AUTHOR("Neil Armstrong <narmstrong@baylibre.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("OMAP PWM Driver using Dual-mode Timers");
diff --git a/drivers/pwm/pwm-rcar.c b/drivers/pwm/pwm-rcar.c
index 6e99a63ffa29..7b8ac0678137 100644
--- a/drivers/pwm/pwm-rcar.c
+++ b/drivers/pwm/pwm-rcar.c
@@ -81,7 +81,7 @@ static int rcar_pwm_get_clock_division(struct rcar_pwm_chip *rp, int period_ns)
 		max = (unsigned long long)NSEC_PER_SEC * RCAR_PWM_MAX_CYCLE *
 			(1 << div);
 		do_div(max, clk_rate);
-		if (period_ns < max)
+		if (period_ns <= max)
 			break;
 	}
 
diff --git a/drivers/rapidio/rio-sysfs.c b/drivers/rapidio/rio-sysfs.c
index cdb005c0094d..eda41563d06d 100644
--- a/drivers/rapidio/rio-sysfs.c
+++ b/drivers/rapidio/rio-sysfs.c
@@ -125,8 +125,7 @@ rio_read_config(struct file *filp, struct kobject *kobj,
 		struct bin_attribute *bin_attr,
 		char *buf, loff_t off, size_t count)
 {
-	struct rio_dev *dev =
-	    to_rio_dev(container_of(kobj, struct device, kobj));
+	struct rio_dev *dev = to_rio_dev(kobj_to_dev(kobj));
 	unsigned int size = 0x100;
 	loff_t init_off = off;
 	u8 *data = (u8 *) buf;
@@ -197,8 +196,7 @@ rio_write_config(struct file *filp, struct kobject *kobj,
 		 struct bin_attribute *bin_attr,
 		 char *buf, loff_t off, size_t count)
 {
-	struct rio_dev *dev =
-	    to_rio_dev(container_of(kobj, struct device, kobj));
+	struct rio_dev *dev = to_rio_dev(kobj_to_dev(kobj));
 	unsigned int size = count;
 	loff_t init_off = off;
 	u8 *data = (u8 *) buf;
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 8df0b0e62976..8155e80dd3f8 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -274,6 +274,15 @@ config REGULATOR_ISL6271A
 	help
 	  This driver supports ISL6271A voltage regulator chip.
 
+config REGULATOR_LM363X
+	tristate "TI LM363X voltage regulators"
+	depends on MFD_TI_LMU
+	help
+	  This driver supports LM3631 and LM3632 voltage regulators for
+	  the LCD bias.
+	  One boost output voltage is configurable and always on.
+	  Other LDOs are used for the display module.
+
 config REGULATOR_LP3971
 	tristate "National Semiconductors LP3971 PMIC regulator driver"
 	depends on I2C
@@ -446,6 +455,7 @@ config REGULATOR_MC13892
 config REGULATOR_MT6311
 	tristate "MediaTek MT6311 PMIC"
 	depends on I2C
+	select REGMAP_I2C
 	help
 	  Say y here to select this option to enable the power regulator of
 	  MediaTek MT6311 PMIC.
@@ -504,6 +514,22 @@ config REGULATOR_PFUZE100
 	  Say y here to support the regulators found on the Freescale
 	  PFUZE100/PFUZE200 PMIC.
 
+config REGULATOR_PV88060
+	tristate "Powerventure Semiconductor PV88060 regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  Say y here to support the voltage regulators and convertors
+	  PV88060
+
+config REGULATOR_PV88090
+	tristate "Powerventure Semiconductor PV88090 regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  Say y here to support the voltage regulators and convertors
+	  on PV88090
+
 config REGULATOR_PWM
 	tristate "PWM voltage regulator"
 	depends on PWM
@@ -588,10 +614,10 @@ config REGULATOR_S2MPA01
 	 via I2C bus. S2MPA01 has 10 Bucks and 26 LDO outputs.
 
 config REGULATOR_S2MPS11
-	tristate "Samsung S2MPS11/S2MPS13/S2MPS14/S2MPU02 voltage regulator"
+	tristate "Samsung S2MPS11/13/14/15/S2MPU02 voltage regulator"
 	depends on MFD_SEC_CORE
 	help
-	 This driver supports a Samsung S2MPS11/S2MPS13/S2MPS14/S2MPU02 voltage
+	 This driver supports a Samsung S2MPS11/13/14/15/S2MPU02 voltage
 	 output regulator via I2C bus. The chip is comprised of high efficient
 	 Buck converters including Dual-Phase Buck converter, Buck-Boost
 	 converter, various LDOs.
@@ -680,6 +706,13 @@ config REGULATOR_TPS6507X
 	  three step-down converters and two general-purpose LDO voltage regulators.
 	  It supports TI's software based Class-2 SmartReflex implementation.
 
+config REGULATOR_TPS65086
+	tristate "TI TPS65086 Power regulators"
+	depends on MFD_TPS65086
+	help
+	  This driver provides support for the voltage regulators on
+	  TI TPS65086 PMICs.
+
 config REGULATOR_TPS65090
 	tristate "TI TPS65090 Power regulator"
 	depends on MFD_TPS65090
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 0f8174913c17..980b1943fa81 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -36,6 +36,7 @@ obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
 obj-$(CONFIG_REGULATOR_HI6421) += hi6421-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
 obj-$(CONFIG_REGULATOR_ISL9305) += isl9305.o
+obj-$(CONFIG_REGULATOR_LM363X) += lm363x-regulator.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
 obj-$(CONFIG_REGULATOR_LP3972) += lp3972.o
 obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
@@ -66,6 +67,8 @@ obj-$(CONFIG_REGULATOR_QCOM_SMD_RPM) += qcom_smd-regulator.o
 obj-$(CONFIG_REGULATOR_QCOM_SPMI) += qcom_spmi-regulator.o
 obj-$(CONFIG_REGULATOR_PALMAS) += palmas-regulator.o
 obj-$(CONFIG_REGULATOR_PFUZE100) += pfuze100-regulator.o
+obj-$(CONFIG_REGULATOR_PV88060) += pv88060-regulator.o
+obj-$(CONFIG_REGULATOR_PV88090) += pv88090-regulator.o
 obj-$(CONFIG_REGULATOR_PWM) += pwm-regulator.o
 obj-$(CONFIG_REGULATOR_TPS51632) += tps51632-regulator.o
 obj-$(CONFIG_REGULATOR_PBIAS) += pbias-regulator.o
@@ -85,6 +88,7 @@ obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS62360) += tps62360-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65023) += tps65023-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6507X) += tps6507x-regulator.o
+obj-$(CONFIG_REGULATOR_TPS65086) += tps65086-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65090) += tps65090-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65217) += tps65217-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65218) += tps65218-regulator.o
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
index 35de22fdb7a0..f2e1a39ce0f3 100644
--- a/drivers/regulator/axp20x-regulator.c
+++ b/drivers/regulator/axp20x-regulator.c
@@ -27,8 +27,8 @@
 #define AXP20X_IO_ENABLED		0x03
 #define AXP20X_IO_DISABLED		0x07
 
-#define AXP22X_IO_ENABLED		0x04
-#define AXP22X_IO_DISABLED		0x03
+#define AXP22X_IO_ENABLED		0x03
+#define AXP22X_IO_DISABLED		0x04
 
 #define AXP20X_WORKMODE_DCDC2_MASK	BIT(2)
 #define AXP20X_WORKMODE_DCDC3_MASK	BIT(1)
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 73b7683355cd..744c9889f88d 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -132,24 +132,24 @@ static bool have_full_constraints(void)
 	return has_full_constraints || of_have_populated_dt();
 }
 
+static inline struct regulator_dev *rdev_get_supply(struct regulator_dev *rdev)
+{
+	if (rdev && rdev->supply)
+		return rdev->supply->rdev;
+
+	return NULL;
+}
+
 /**
  * regulator_lock_supply - lock a regulator and its supplies
  * @rdev:         regulator source
  */
 static void regulator_lock_supply(struct regulator_dev *rdev)
 {
-	struct regulator *supply;
-	int i = 0;
-
-	while (1) {
-		mutex_lock_nested(&rdev->mutex, i++);
-		supply = rdev->supply;
-
-		if (!rdev->supply)
-			return;
+	int i;
 
-		rdev = supply->rdev;
-	}
+	for (i = 0; rdev; rdev = rdev_get_supply(rdev), i++)
+		mutex_lock_nested(&rdev->mutex, i);
 }
 
 /**
@@ -2368,7 +2368,6 @@ static void regulator_disable_work(struct work_struct *work)
 int regulator_disable_deferred(struct regulator *regulator, int ms)
 {
 	struct regulator_dev *rdev = regulator->rdev;
-	int ret;
 
 	if (regulator->always_on)
 		return 0;
@@ -2380,13 +2379,9 @@ int regulator_disable_deferred(struct regulator *regulator, int ms)
 	rdev->deferred_disables++;
 	mutex_unlock(&rdev->mutex);
 
-	ret = queue_delayed_work(system_power_efficient_wq,
-				 &rdev->disable_work,
-				 msecs_to_jiffies(ms));
-	if (ret < 0)
-		return ret;
-	else
-		return 0;
+	queue_delayed_work(system_power_efficient_wq, &rdev->disable_work,
+			   msecs_to_jiffies(ms));
+	return 0;
 }
 EXPORT_SYMBOL_GPL(regulator_disable_deferred);
 
@@ -3451,8 +3446,10 @@ int regulator_bulk_get(struct device *dev, int num_consumers,
 		consumers[i].consumer = NULL;
 
 	for (i = 0; i < num_consumers; i++) {
-		consumers[i].consumer = regulator_get(dev,
-						      consumers[i].supply);
+		consumers[i].consumer = _regulator_get(dev,
+						       consumers[i].supply,
+						       false,
+						       !consumers[i].optional);
 		if (IS_ERR(consumers[i].consumer)) {
 			ret = PTR_ERR(consumers[i].consumer);
 			dev_err(dev, "Failed to get supply '%s': %d\n",
@@ -3708,7 +3705,7 @@ static umode_t regulator_attr_is_visible(struct kobject *kobj,
 					 struct attribute *attr, int idx)
 {
 	struct device *dev = kobj_to_dev(kobj);
-	struct regulator_dev *rdev = container_of(dev, struct regulator_dev, dev);
+	struct regulator_dev *rdev = dev_to_rdev(dev);
 	const struct regulator_ops *ops = rdev->desc->ops;
 	umode_t mode = attr->mode;
 
diff --git a/drivers/regulator/da903x.c b/drivers/regulator/da903x.c
index affa1b191314..33e8f3b8d2bd 100644
--- a/drivers/regulator/da903x.c
+++ b/drivers/regulator/da903x.c
@@ -257,7 +257,7 @@ static const struct regulator_linear_range da9034_ldo12_ranges[] = {
 	REGULATOR_LINEAR_RANGE(2700000, 8, 15, 50000),
 };
 
-static struct regulator_ops da903x_regulator_ldo_ops = {
+static const struct regulator_ops da903x_regulator_ldo_ops = {
 	.set_voltage_sel = da903x_set_voltage_sel,
 	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= regulator_list_voltage_linear,
@@ -268,7 +268,7 @@ static struct regulator_ops da903x_regulator_ldo_ops = {
 };
 
 /* NOTE: this is dedicated for the insane DA9030 LDO14 */
-static struct regulator_ops da9030_regulator_ldo14_ops = {
+static const struct regulator_ops da9030_regulator_ldo14_ops = {
 	.set_voltage_sel = da903x_set_voltage_sel,
 	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= da9030_list_ldo14_voltage,
@@ -279,7 +279,7 @@ static struct regulator_ops da9030_regulator_ldo14_ops = {
 };
 
 /* NOTE: this is dedicated for the DA9030 LDO1 and LDO15 that have locks  */
-static struct regulator_ops da9030_regulator_ldo1_15_ops = {
+static const struct regulator_ops da9030_regulator_ldo1_15_ops = {
 	.set_voltage_sel = da9030_set_ldo1_15_voltage_sel,
 	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= regulator_list_voltage_linear,
@@ -289,7 +289,7 @@ static struct regulator_ops da9030_regulator_ldo1_15_ops = {
 	.is_enabled	= da903x_is_enabled,
 };
 
-static struct regulator_ops da9034_regulator_dvc_ops = {
+static const struct regulator_ops da9034_regulator_dvc_ops = {
 	.set_voltage_sel = da9034_set_dvc_voltage_sel,
 	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= regulator_list_voltage_linear,
@@ -300,7 +300,7 @@ static struct regulator_ops da9034_regulator_dvc_ops = {
 };
 
 /* NOTE: this is dedicated for the insane LDO12 */
-static struct regulator_ops da9034_regulator_ldo12_ops = {
+static const struct regulator_ops da9034_regulator_ldo12_ops = {
 	.set_voltage_sel = da903x_set_voltage_sel,
 	.get_voltage_sel = da903x_get_voltage_sel,
 	.list_voltage	= regulator_list_voltage_linear_range,
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index 12a25b40e473..1050cb77561a 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -265,7 +265,7 @@ static int da9052_regulator_set_voltage_time_sel(struct regulator_dev *rdev,
 	return ret;
 }
 
-static struct regulator_ops da9052_dcdc_ops = {
+static const struct regulator_ops da9052_dcdc_ops = {
 	.get_current_limit = da9052_dcdc_get_current_limit,
 	.set_current_limit = da9052_dcdc_set_current_limit,
 
@@ -279,7 +279,7 @@ static struct regulator_ops da9052_dcdc_ops = {
 	.disable = regulator_disable_regmap,
 };
 
-static struct regulator_ops da9052_ldo_ops = {
+static const struct regulator_ops da9052_ldo_ops = {
 	.list_voltage = da9052_list_voltage,
 	.map_voltage = da9052_map_voltage,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
diff --git a/drivers/regulator/da9055-regulator.c b/drivers/regulator/da9055-regulator.c
index cafdafbffcaf..d029c941a1e1 100644
--- a/drivers/regulator/da9055-regulator.c
+++ b/drivers/regulator/da9055-regulator.c
@@ -324,7 +324,7 @@ static int da9055_suspend_disable(struct regulator_dev *rdev)
 		return 0;
 }
 
-static struct regulator_ops da9055_buck_ops = {
+static const struct regulator_ops da9055_buck_ops = {
 	.get_mode = da9055_buck_get_mode,
 	.set_mode = da9055_buck_set_mode,
 
@@ -345,7 +345,7 @@ static struct regulator_ops da9055_buck_ops = {
 	.set_suspend_mode = da9055_buck_set_mode,
 };
 
-static struct regulator_ops da9055_ldo_ops = {
+static const struct regulator_ops da9055_ldo_ops = {
 	.get_mode = da9055_ldo_get_mode,
 	.set_mode = da9055_ldo_set_mode,
 
diff --git a/drivers/regulator/da9062-regulator.c b/drivers/regulator/da9062-regulator.c
index 5638fe8d759d..0638c8b40521 100644
--- a/drivers/regulator/da9062-regulator.c
+++ b/drivers/regulator/da9062-regulator.c
@@ -371,7 +371,7 @@ static int da9062_ldo_set_suspend_mode(struct regulator_dev *rdev,
 	return regmap_field_write(regl->suspend_sleep, val);
 }
 
-static struct regulator_ops da9062_buck_ops = {
+static const struct regulator_ops da9062_buck_ops = {
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.is_enabled		= regulator_is_enabled_regmap,
@@ -389,7 +389,7 @@ static struct regulator_ops da9062_buck_ops = {
 	.set_suspend_mode	= da9062_buck_set_suspend_mode,
 };
 
-static struct regulator_ops da9062_ldo_ops = {
+static const struct regulator_ops da9062_ldo_ops = {
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.is_enabled		= regulator_is_enabled_regmap,
diff --git a/drivers/regulator/da9063-regulator.c b/drivers/regulator/da9063-regulator.c
index 536e931eb921..ed9e7e96f877 100644
--- a/drivers/regulator/da9063-regulator.c
+++ b/drivers/regulator/da9063-regulator.c
@@ -427,7 +427,7 @@ static int da9063_ldo_set_suspend_mode(struct regulator_dev *rdev, unsigned mode
 	return regmap_field_write(regl->suspend_sleep, val);
 }
 
-static struct regulator_ops da9063_buck_ops = {
+static const struct regulator_ops da9063_buck_ops = {
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.is_enabled		= regulator_is_enabled_regmap,
@@ -445,7 +445,7 @@ static struct regulator_ops da9063_buck_ops = {
 	.set_suspend_mode	= da9063_buck_set_suspend_mode,
 };
 
-static struct regulator_ops da9063_ldo_ops = {
+static const struct regulator_ops da9063_ldo_ops = {
 	.enable			= regulator_enable_regmap,
 	.disable		= regulator_disable_regmap,
 	.is_enabled		= regulator_is_enabled_regmap,
diff --git a/drivers/regulator/da9210-regulator.c b/drivers/regulator/da9210-regulator.c
index b3517830edb6..8b3cc9f0cd64 100644
--- a/drivers/regulator/da9210-regulator.c
+++ b/drivers/regulator/da9210-regulator.c
@@ -46,7 +46,7 @@ static int da9210_set_current_limit(struct regulator_dev *rdev, int min_uA,
 				    int max_uA);
 static int da9210_get_current_limit(struct regulator_dev *rdev);
 
-static struct regulator_ops da9210_buck_ops = {
+static const struct regulator_ops da9210_buck_ops = {
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
 	.is_enabled = regulator_is_enabled_regmap,
diff --git a/drivers/regulator/da9211-regulator.c b/drivers/regulator/da9211-regulator.c
index 04ef65b7eb3d..236abf473db5 100644
--- a/drivers/regulator/da9211-regulator.c
+++ b/drivers/regulator/da9211-regulator.c
@@ -219,7 +219,7 @@ static int da9211_get_current_limit(struct regulator_dev *rdev)
 	return current_limits[data];
 }
 
-static struct regulator_ops da9211_buck_ops = {
+static const struct regulator_ops da9211_buck_ops = {
 	.get_mode = da9211_buck_get_mode,
 	.set_mode = da9211_buck_set_mode,
 	.enable = regulator_enable_regmap,
diff --git a/drivers/regulator/devres.c b/drivers/regulator/devres.c
index 6ec1d400adae..6ad8ab4c578d 100644
--- a/drivers/regulator/devres.c
+++ b/drivers/regulator/devres.c
@@ -164,8 +164,11 @@ int devm_regulator_bulk_get(struct device *dev, int num_consumers,
 		consumers[i].consumer = NULL;
 
 	for (i = 0; i < num_consumers; i++) {
-		consumers[i].consumer = devm_regulator_get(dev,
-							   consumers[i].supply);
+		consumers[i].consumer = _devm_regulator_get(dev,
+							    consumers[i].supply,
+							    consumers[i].optional ?
+								OPTIONAL_GET :
+								NORMAL_GET);
 		if (IS_ERR(consumers[i].consumer)) {
 			ret = PTR_ERR(consumers[i].consumer);
 			dev_err(dev, "Failed to get supply '%s': %d\n",
diff --git a/drivers/regulator/lm363x-regulator.c b/drivers/regulator/lm363x-regulator.c
new file mode 100644
index 000000000000..f53e63301a20
--- /dev/null
+++ b/drivers/regulator/lm363x-regulator.c
@@ -0,0 +1,291 @@
+/*
+ * TI LM363X Regulator Driver
+ *
+ * Copyright 2015 Texas Instruments
+ *
+ * Author: Milo Kim <milo.kim@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/mfd/ti-lmu.h>
+#include <linux/mfd/ti-lmu-register.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/slab.h>
+
+/* LM3631 */
+#define LM3631_BOOST_VSEL_MAX		0x25
+#define LM3631_LDO_VSEL_MAX		0x28
+#define LM3631_CONT_VSEL_MAX		0x03
+#define LM3631_VBOOST_MIN		4500000
+#define LM3631_VCONT_MIN		1800000
+#define LM3631_VLDO_MIN			4000000
+#define ENABLE_TIME_USEC		1000
+
+/* LM3632 */
+#define LM3632_BOOST_VSEL_MAX		0x26
+#define LM3632_LDO_VSEL_MAX		0x29
+#define LM3632_VBOOST_MIN		4500000
+#define LM3632_VLDO_MIN			4000000
+
+/* Common */
+#define LM363X_STEP_50mV		50000
+#define LM363X_STEP_500mV		500000
+
+static const int ldo_cont_enable_time[] = {
+	0, 2000, 5000, 10000, 20000, 50000, 100000, 200000,
+};
+
+static int lm363x_regulator_enable_time(struct regulator_dev *rdev)
+{
+	enum lm363x_regulator_id id = rdev_get_id(rdev);
+	u8 val, addr, mask;
+
+	switch (id) {
+	case LM3631_LDO_CONT:
+		addr = LM3631_REG_ENTIME_VCONT;
+		mask = LM3631_ENTIME_CONT_MASK;
+		break;
+	case LM3631_LDO_OREF:
+		addr = LM3631_REG_ENTIME_VOREF;
+		mask = LM3631_ENTIME_MASK;
+		break;
+	case LM3631_LDO_POS:
+		addr = LM3631_REG_ENTIME_VPOS;
+		mask = LM3631_ENTIME_MASK;
+		break;
+	case LM3631_LDO_NEG:
+		addr = LM3631_REG_ENTIME_VNEG;
+		mask = LM3631_ENTIME_MASK;
+		break;
+	default:
+		return 0;
+	}
+
+	if (regmap_read(rdev->regmap, addr, (unsigned int *)&val))
+		return -EINVAL;
+
+	val = (val & mask) >> LM3631_ENTIME_SHIFT;
+
+	if (id == LM3631_LDO_CONT)
+		return ldo_cont_enable_time[val];
+	else
+		return ENABLE_TIME_USEC * val;
+}
+
+static struct regulator_ops lm363x_boost_voltage_table_ops = {
+	.list_voltage     = regulator_list_voltage_linear,
+	.set_voltage_sel  = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel  = regulator_get_voltage_sel_regmap,
+};
+
+static struct regulator_ops lm363x_regulator_voltage_table_ops = {
+	.list_voltage     = regulator_list_voltage_linear,
+	.set_voltage_sel  = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel  = regulator_get_voltage_sel_regmap,
+	.enable           = regulator_enable_regmap,
+	.disable          = regulator_disable_regmap,
+	.is_enabled       = regulator_is_enabled_regmap,
+	.enable_time      = lm363x_regulator_enable_time,
+};
+
+static const struct regulator_desc lm363x_regulator_desc[] = {
+	/* LM3631 */
+	{
+		.name           = "vboost",
+		.of_match	= "vboost",
+		.id             = LM3631_BOOST,
+		.ops            = &lm363x_boost_voltage_table_ops,
+		.n_voltages     = LM3631_BOOST_VSEL_MAX + 1,
+		.min_uV         = LM3631_VBOOST_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3631_REG_VOUT_BOOST,
+		.vsel_mask      = LM3631_VOUT_MASK,
+	},
+	{
+		.name           = "ldo_cont",
+		.of_match	= "vcont",
+		.id             = LM3631_LDO_CONT,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3631_CONT_VSEL_MAX + 1,
+		.min_uV         = LM3631_VCONT_MIN,
+		.uV_step        = LM363X_STEP_500mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3631_REG_VOUT_CONT,
+		.vsel_mask      = LM3631_VOUT_CONT_MASK,
+		.enable_reg     = LM3631_REG_LDO_CTRL2,
+		.enable_mask    = LM3631_EN_CONT_MASK,
+	},
+	{
+		.name           = "ldo_oref",
+		.of_match	= "voref",
+		.id             = LM3631_LDO_OREF,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3631_LDO_VSEL_MAX + 1,
+		.min_uV         = LM3631_VLDO_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3631_REG_VOUT_OREF,
+		.vsel_mask      = LM3631_VOUT_MASK,
+		.enable_reg     = LM3631_REG_LDO_CTRL1,
+		.enable_mask    = LM3631_EN_OREF_MASK,
+	},
+	{
+		.name           = "ldo_vpos",
+		.of_match	= "vpos",
+		.id             = LM3631_LDO_POS,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3631_LDO_VSEL_MAX + 1,
+		.min_uV         = LM3631_VLDO_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3631_REG_VOUT_POS,
+		.vsel_mask      = LM3631_VOUT_MASK,
+		.enable_reg     = LM3631_REG_LDO_CTRL1,
+		.enable_mask    = LM3631_EN_VPOS_MASK,
+	},
+	{
+		.name           = "ldo_vneg",
+		.of_match	= "vneg",
+		.id             = LM3631_LDO_NEG,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3631_LDO_VSEL_MAX + 1,
+		.min_uV         = LM3631_VLDO_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3631_REG_VOUT_NEG,
+		.vsel_mask      = LM3631_VOUT_MASK,
+		.enable_reg     = LM3631_REG_LDO_CTRL1,
+		.enable_mask    = LM3631_EN_VNEG_MASK,
+	},
+	/* LM3632 */
+	{
+		.name           = "vboost",
+		.of_match	= "vboost",
+		.id             = LM3632_BOOST,
+		.ops            = &lm363x_boost_voltage_table_ops,
+		.n_voltages     = LM3632_BOOST_VSEL_MAX + 1,
+		.min_uV         = LM3632_VBOOST_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3632_REG_VOUT_BOOST,
+		.vsel_mask      = LM3632_VOUT_MASK,
+	},
+	{
+		.name           = "ldo_vpos",
+		.of_match	= "vpos",
+		.id             = LM3632_LDO_POS,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3632_LDO_VSEL_MAX + 1,
+		.min_uV         = LM3632_VLDO_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3632_REG_VOUT_POS,
+		.vsel_mask      = LM3632_VOUT_MASK,
+		.enable_reg     = LM3632_REG_BIAS_CONFIG,
+		.enable_mask    = LM3632_EN_VPOS_MASK,
+	},
+	{
+		.name           = "ldo_vneg",
+		.of_match	= "vneg",
+		.id             = LM3632_LDO_NEG,
+		.ops            = &lm363x_regulator_voltage_table_ops,
+		.n_voltages     = LM3632_LDO_VSEL_MAX + 1,
+		.min_uV         = LM3632_VLDO_MIN,
+		.uV_step        = LM363X_STEP_50mV,
+		.type           = REGULATOR_VOLTAGE,
+		.owner          = THIS_MODULE,
+		.vsel_reg       = LM3632_REG_VOUT_NEG,
+		.vsel_mask      = LM3632_VOUT_MASK,
+		.enable_reg     = LM3632_REG_BIAS_CONFIG,
+		.enable_mask    = LM3632_EN_VNEG_MASK,
+	},
+};
+
+static int lm363x_regulator_of_get_enable_gpio(struct device_node *np, int id)
+{
+	/*
+	 * Check LCM_EN1/2_GPIO is configured.
+	 * Those pins are used for enabling VPOS/VNEG LDOs.
+	 */
+	switch (id) {
+	case LM3632_LDO_POS:
+		return of_get_named_gpio(np, "ti,lcm-en1-gpio", 0);
+	case LM3632_LDO_NEG:
+		return of_get_named_gpio(np, "ti,lcm-en2-gpio", 0);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int lm363x_regulator_probe(struct platform_device *pdev)
+{
+	struct ti_lmu *lmu = dev_get_drvdata(pdev->dev.parent);
+	struct regmap *regmap = lmu->regmap;
+	struct regulator_config cfg = { };
+	struct regulator_dev *rdev;
+	struct device *dev = &pdev->dev;
+	int id = pdev->id;
+	int ret, ena_gpio;
+
+	cfg.dev = dev;
+	cfg.regmap = regmap;
+
+	/*
+	 * LM3632 LDOs can be controlled by external pin.
+	 * Register update is required if the pin is used.
+	 */
+	ena_gpio = lm363x_regulator_of_get_enable_gpio(dev->of_node, id);
+	if (gpio_is_valid(ena_gpio)) {
+		cfg.ena_gpio = ena_gpio;
+		cfg.ena_gpio_flags = GPIOF_OUT_INIT_LOW;
+
+		ret = regmap_update_bits(regmap, LM3632_REG_BIAS_CONFIG,
+					 LM3632_EXT_EN_MASK,
+					 LM3632_EXT_EN_MASK);
+		if (ret) {
+			dev_err(dev, "External pin err: %d\n", ret);
+			return ret;
+		}
+	}
+
+	rdev = devm_regulator_register(dev, &lm363x_regulator_desc[id], &cfg);
+	if (IS_ERR(rdev)) {
+		ret = PTR_ERR(rdev);
+		dev_err(dev, "[%d] regulator register err: %d\n", id, ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver lm363x_regulator_driver = {
+	.probe = lm363x_regulator_probe,
+	.driver = {
+		.name = "lm363x-regulator",
+	},
+};
+
+module_platform_driver(lm363x_regulator_driver);
+
+MODULE_DESCRIPTION("TI LM363X Regulator Driver");
+MODULE_AUTHOR("Milo Kim");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:lm363x-regulator");
diff --git a/drivers/regulator/lp872x.c b/drivers/regulator/lp872x.c
index e5af07208f9d..19d758486553 100644
--- a/drivers/regulator/lp872x.c
+++ b/drivers/regulator/lp872x.c
@@ -108,7 +108,6 @@ struct lp872x {
 	struct lp872x_platform_data *pdata;
 	int num_regulators;
 	enum lp872x_dvs_state dvs_pin;
-	int dvs_gpio;
 };
 
 /* LP8720/LP8725 shared voltage table for LDOs */
@@ -520,6 +519,7 @@ static struct regulator_ops lp8725_buck_ops = {
 static struct regulator_desc lp8720_regulator_desc[] = {
 	{
 		.name = "ldo1",
+		.of_match = of_match_ptr("ldo1"),
 		.id = LP8720_ID_LDO1,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -533,6 +533,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 	},
 	{
 		.name = "ldo2",
+		.of_match = of_match_ptr("ldo2"),
 		.id = LP8720_ID_LDO2,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -546,6 +547,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 	},
 	{
 		.name = "ldo3",
+		.of_match = of_match_ptr("ldo3"),
 		.id = LP8720_ID_LDO3,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -559,6 +561,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 	},
 	{
 		.name = "ldo4",
+		.of_match = of_match_ptr("ldo4"),
 		.id = LP8720_ID_LDO4,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp8720_ldo4_vtbl),
@@ -572,6 +575,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 	},
 	{
 		.name = "ldo5",
+		.of_match = of_match_ptr("ldo5"),
 		.id = LP8720_ID_LDO5,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -585,6 +589,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 	},
 	{
 		.name = "buck",
+		.of_match = of_match_ptr("buck"),
 		.id = LP8720_ID_BUCK,
 		.ops = &lp8720_buck_ops,
 		.n_voltages = ARRAY_SIZE(lp8720_buck_vtbl),
@@ -599,6 +604,7 @@ static struct regulator_desc lp8720_regulator_desc[] = {
 static struct regulator_desc lp8725_regulator_desc[] = {
 	{
 		.name = "ldo1",
+		.of_match = of_match_ptr("ldo1"),
 		.id = LP8725_ID_LDO1,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -612,6 +618,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "ldo2",
+		.of_match = of_match_ptr("ldo2"),
 		.id = LP8725_ID_LDO2,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -625,6 +632,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "ldo3",
+		.of_match = of_match_ptr("ldo3"),
 		.id = LP8725_ID_LDO3,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -638,6 +646,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "ldo4",
+		.of_match = of_match_ptr("ldo4"),
 		.id = LP8725_ID_LDO4,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -651,6 +660,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "ldo5",
+		.of_match = of_match_ptr("ldo5"),
 		.id = LP8725_ID_LDO5,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp872x_ldo_vtbl),
@@ -664,6 +674,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "lilo1",
+		.of_match = of_match_ptr("lilo1"),
 		.id = LP8725_ID_LILO1,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp8725_lilo_vtbl),
@@ -677,6 +688,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "lilo2",
+		.of_match = of_match_ptr("lilo2"),
 		.id = LP8725_ID_LILO2,
 		.ops = &lp872x_ldo_ops,
 		.n_voltages = ARRAY_SIZE(lp8725_lilo_vtbl),
@@ -690,6 +702,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "buck1",
+		.of_match = of_match_ptr("buck1"),
 		.id = LP8725_ID_BUCK1,
 		.ops = &lp8725_buck_ops,
 		.n_voltages = ARRAY_SIZE(lp8725_buck_vtbl),
@@ -701,6 +714,7 @@ static struct regulator_desc lp8725_regulator_desc[] = {
 	},
 	{
 		.name = "buck2",
+		.of_match = of_match_ptr("buck2"),
 		.id = LP8725_ID_BUCK2,
 		.ops = &lp8725_buck_ops,
 		.n_voltages = ARRAY_SIZE(lp8725_buck_vtbl),
@@ -737,7 +751,6 @@ static int lp872x_init_dvs(struct lp872x *lp)
 	}
 
 	lp->dvs_pin = pinstate;
-	lp->dvs_gpio = gpio;
 
 	return 0;
 
diff --git a/drivers/regulator/lp8788-buck.c b/drivers/regulator/lp8788-buck.c
index a97bed90d39b..ec46290b647e 100644
--- a/drivers/regulator/lp8788-buck.c
+++ b/drivers/regulator/lp8788-buck.c
@@ -344,7 +344,7 @@ static unsigned int lp8788_buck_get_mode(struct regulator_dev *rdev)
 				REGULATOR_MODE_FAST : REGULATOR_MODE_NORMAL;
 }
 
-static struct regulator_ops lp8788_buck12_ops = {
+static const struct regulator_ops lp8788_buck12_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = lp8788_buck12_set_voltage_sel,
@@ -357,7 +357,7 @@ static struct regulator_ops lp8788_buck12_ops = {
 	.get_mode = lp8788_buck_get_mode,
 };
 
-static struct regulator_ops lp8788_buck34_ops = {
+static const struct regulator_ops lp8788_buck34_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.map_voltage = regulator_map_voltage_ascend,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
diff --git a/drivers/regulator/lp8788-ldo.c b/drivers/regulator/lp8788-ldo.c
index 9f22d079c8cc..cbfd35873575 100644
--- a/drivers/regulator/lp8788-ldo.c
+++ b/drivers/regulator/lp8788-ldo.c
@@ -170,7 +170,7 @@ static int lp8788_ldo_enable_time(struct regulator_dev *rdev)
 	return ENABLE_TIME_USEC * val;
 }
 
-static struct regulator_ops lp8788_ldo_voltage_table_ops = {
+static const struct regulator_ops lp8788_ldo_voltage_table_ops = {
 	.list_voltage = regulator_list_voltage_table,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -180,7 +180,7 @@ static struct regulator_ops lp8788_ldo_voltage_table_ops = {
 	.enable_time = lp8788_ldo_enable_time,
 };
 
-static struct regulator_ops lp8788_ldo_voltage_fixed_ops = {
+static const struct regulator_ops lp8788_ldo_voltage_fixed_ops = {
 	.list_voltage = regulator_list_voltage_linear,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
@@ -613,22 +613,20 @@ static struct platform_driver lp8788_aldo_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&lp8788_dldo_driver,
+	&lp8788_aldo_driver,
+};
+
 static int __init lp8788_ldo_init(void)
 {
-	int ret;
-
-	ret = platform_driver_register(&lp8788_dldo_driver);
-	if (ret)
-		return ret;
-
-	return platform_driver_register(&lp8788_aldo_driver);
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 subsys_initcall(lp8788_ldo_init);
 
 static void __exit lp8788_ldo_exit(void)
 {
-	platform_driver_unregister(&lp8788_aldo_driver);
-	platform_driver_unregister(&lp8788_dldo_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(lp8788_ldo_exit);
 
diff --git a/drivers/regulator/mt6311-regulator.c b/drivers/regulator/mt6311-regulator.c
index 02c4e5feca8e..0495716fd35f 100644
--- a/drivers/regulator/mt6311-regulator.c
+++ b/drivers/regulator/mt6311-regulator.c
@@ -30,6 +30,7 @@ static const struct regmap_config mt6311_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 	.max_register = MT6311_FQMTR_CON4,
+	.cache_type = REGCACHE_RBTREE,
 };
 
 /* Default limits measured in millivolts and milliamps */
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index 8217613807d3..6efc7ee8aea3 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -612,6 +612,18 @@ static struct regulator_ops palmas_ops_ldo = {
 	.map_voltage		= regulator_map_voltage_linear,
 };
 
+static struct regulator_ops palmas_ops_ldo9 = {
+	.is_enabled		= palmas_is_enabled_ldo,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.set_bypass		= regulator_set_bypass_regmap,
+	.get_bypass		= regulator_get_bypass_regmap,
+};
+
 static struct regulator_ops palmas_ops_ext_control_ldo = {
 	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
 	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
@@ -639,6 +651,19 @@ static struct regulator_ops tps65917_ops_ldo = {
 	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
 };
 
+static struct regulator_ops tps65917_ops_ldo_1_2 = {
+	.is_enabled		= palmas_is_enabled_ldo,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_bypass		= regulator_set_bypass_regmap,
+	.get_bypass		= regulator_get_bypass_regmap,
+};
+
 static int palmas_regulator_config_external(struct palmas *palmas, int id,
 		struct palmas_reg_init *reg_init)
 {
@@ -915,6 +940,13 @@ static int palmas_ldo_registration(struct palmas_pmic *pmic,
 			if (pdata && pdata->ldo6_vibrator &&
 			    (id == PALMAS_REG_LDO6))
 				desc->enable_time = 2000;
+
+			if (id == PALMAS_REG_LDO9) {
+				desc->ops = &palmas_ops_ldo9;
+				desc->bypass_reg = desc->enable_reg;
+				desc->bypass_mask =
+						PALMAS_LDO9_CTRL_LDO_BYPASS_EN;
+			}
 		} else {
 			if (!ddata->has_regen3 && id == PALMAS_REG_REGEN3)
 				continue;
@@ -1019,6 +1051,13 @@ static int tps65917_ldo_registration(struct palmas_pmic *pmic,
 			 * It is of the order of ~60mV/uS.
 			 */
 			desc->ramp_delay = 2500;
+			if (id == TPS65917_REG_LDO1 ||
+			    id == TPS65917_REG_LDO2) {
+				desc->ops = &tps65917_ops_ldo_1_2;
+				desc->bypass_reg = desc->enable_reg;
+				desc->bypass_mask =
+						TPS65917_LDO1_CTRL_BYPASS_EN;
+			}
 		} else {
 			desc->n_voltages = 1;
 			if (reg_init && reg_init->roof_floor)
diff --git a/drivers/regulator/pv88060-regulator.c b/drivers/regulator/pv88060-regulator.c
new file mode 100644
index 000000000000..094376c8de4b
--- /dev/null
+++ b/drivers/regulator/pv88060-regulator.c
@@ -0,0 +1,437 @@
+/*
+ * pv88060-regulator.c - Regulator device driver for PV88060
+ * Copyright (C) 2015  Powerventure Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regmap.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/proc_fs.h>
+#include <linux/uaccess.h>
+#include "pv88060-regulator.h"
+
+#define PV88060_MAX_REGULATORS	14
+
+/* PV88060 REGULATOR IDs */
+enum {
+	/* BUCKs */
+	PV88060_ID_BUCK1,
+
+	/* LDOs */
+	PV88060_ID_LDO1,
+	PV88060_ID_LDO2,
+	PV88060_ID_LDO3,
+	PV88060_ID_LDO4,
+	PV88060_ID_LDO5,
+	PV88060_ID_LDO6,
+	PV88060_ID_LDO7,
+
+	/* SWTs */
+	PV88060_ID_SW1,
+	PV88060_ID_SW2,
+	PV88060_ID_SW3,
+	PV88060_ID_SW4,
+	PV88060_ID_SW5,
+	PV88060_ID_SW6,
+};
+
+struct pv88060_regulator {
+	struct regulator_desc desc;
+	/* Current limiting */
+	unsigned	n_current_limits;
+	const int	*current_limits;
+	unsigned int limit_mask;
+	unsigned int conf;		/* buck configuration register */
+};
+
+struct pv88060 {
+	struct device *dev;
+	struct regmap *regmap;
+	struct regulator_dev *rdev[PV88060_MAX_REGULATORS];
+};
+
+static const struct regmap_config pv88060_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+/* Current limits array (in uA) for BUCK1
+ * Entry indexes corresponds to register values.
+ */
+
+static const int pv88060_buck1_limits[] = {
+	1496000, 2393000, 3291000, 4189000
+};
+
+static unsigned int pv88060_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct pv88060_regulator *info = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret, mode = 0;
+
+	ret = regmap_read(rdev->regmap, info->conf, &data);
+	if (ret < 0)
+		return ret;
+
+	switch (data & PV88060_BUCK_MODE_MASK) {
+	case PV88060_BUCK_MODE_SYNC:
+		mode = REGULATOR_MODE_FAST;
+		break;
+	case PV88060_BUCK_MODE_AUTO:
+		mode = REGULATOR_MODE_NORMAL;
+		break;
+	case PV88060_BUCK_MODE_SLEEP:
+		mode = REGULATOR_MODE_STANDBY;
+		break;
+	}
+
+	return mode;
+}
+
+static int pv88060_buck_set_mode(struct regulator_dev *rdev,
+					unsigned int mode)
+{
+	struct pv88060_regulator *info = rdev_get_drvdata(rdev);
+	int val = 0;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = PV88060_BUCK_MODE_SYNC;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		val = PV88060_BUCK_MODE_AUTO;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = PV88060_BUCK_MODE_SLEEP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_update_bits(rdev->regmap, info->conf,
+					PV88060_BUCK_MODE_MASK, val);
+}
+
+static int pv88060_set_current_limit(struct regulator_dev *rdev, int min,
+				    int max)
+{
+	struct pv88060_regulator *info = rdev_get_drvdata(rdev);
+	int i;
+
+	/* search for closest to maximum */
+	for (i = info->n_current_limits; i >= 0; i--) {
+		if (min <= info->current_limits[i]
+			&& max >= info->current_limits[i]) {
+			return regmap_update_bits(rdev->regmap,
+				info->conf,
+				info->limit_mask,
+				i << PV88060_BUCK_ILIM_SHIFT);
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int pv88060_get_current_limit(struct regulator_dev *rdev)
+{
+	struct pv88060_regulator *info = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(rdev->regmap, info->conf, &data);
+	if (ret < 0)
+		return ret;
+
+	data = (data & info->limit_mask) >> PV88060_BUCK_ILIM_SHIFT;
+	return info->current_limits[data];
+}
+
+static struct regulator_ops pv88060_buck_ops = {
+	.get_mode = pv88060_buck_get_mode,
+	.set_mode = pv88060_buck_set_mode,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.set_current_limit = pv88060_set_current_limit,
+	.get_current_limit = pv88060_get_current_limit,
+};
+
+static struct regulator_ops pv88060_ldo_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+};
+
+#define PV88060_BUCK(chip, regl_name, min, step, max, limits_array) \
+{\
+	.desc	=	{\
+		.id = chip##_ID_##regl_name,\
+		.name = __stringify(chip##_##regl_name),\
+		.of_match = of_match_ptr(#regl_name),\
+		.regulators_node = of_match_ptr("regulators"),\
+		.type = REGULATOR_VOLTAGE,\
+		.owner = THIS_MODULE,\
+		.ops = &pv88060_buck_ops,\
+		.min_uV = min,\
+		.uV_step = step,\
+		.n_voltages = ((max) - (min))/(step) + 1,\
+		.enable_reg = PV88060_REG_##regl_name##_CONF0,\
+		.enable_mask = PV88060_BUCK_EN, \
+		.vsel_reg = PV88060_REG_##regl_name##_CONF0,\
+		.vsel_mask = PV88060_VBUCK_MASK,\
+	},\
+	.current_limits = limits_array,\
+	.n_current_limits = ARRAY_SIZE(limits_array),\
+	.limit_mask = PV88060_BUCK_ILIM_MASK, \
+	.conf = PV88060_REG_##regl_name##_CONF1,\
+}
+
+#define PV88060_LDO(chip, regl_name, min, step, max) \
+{\
+	.desc	=	{\
+		.id = chip##_ID_##regl_name,\
+		.name = __stringify(chip##_##regl_name),\
+		.of_match = of_match_ptr(#regl_name),\
+		.regulators_node = of_match_ptr("regulators"),\
+		.type = REGULATOR_VOLTAGE,\
+		.owner = THIS_MODULE,\
+		.ops = &pv88060_ldo_ops,\
+		.min_uV = min, \
+		.uV_step = step, \
+		.n_voltages = (step) ? ((max - min) / step + 1) : 1, \
+		.enable_reg = PV88060_REG_##regl_name##_CONF, \
+		.enable_mask = PV88060_LDO_EN, \
+		.vsel_reg = PV88060_REG_##regl_name##_CONF, \
+		.vsel_mask = PV88060_VLDO_MASK, \
+	},\
+}
+
+#define PV88060_SW(chip, regl_name, max) \
+{\
+	.desc	=	{\
+		.id = chip##_ID_##regl_name,\
+		.name = __stringify(chip##_##regl_name),\
+		.of_match = of_match_ptr(#regl_name),\
+		.regulators_node = of_match_ptr("regulators"),\
+		.type = REGULATOR_VOLTAGE,\
+		.owner = THIS_MODULE,\
+		.ops = &pv88060_ldo_ops,\
+		.min_uV = max,\
+		.uV_step = 0,\
+		.n_voltages = 1,\
+		.enable_reg = PV88060_REG_##regl_name##_CONF,\
+		.enable_mask = PV88060_SW_EN,\
+	},\
+}
+
+static const struct pv88060_regulator pv88060_regulator_info[] = {
+	PV88060_BUCK(PV88060, BUCK1, 2800000, 12500, 4387500,
+		pv88060_buck1_limits),
+	PV88060_LDO(PV88060, LDO1, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO2, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO3, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO4, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO5, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO6, 1200000, 50000, 3350000),
+	PV88060_LDO(PV88060, LDO7, 1200000, 50000, 3350000),
+	PV88060_SW(PV88060, SW1, 5000000),
+	PV88060_SW(PV88060, SW2, 5000000),
+	PV88060_SW(PV88060, SW3, 5000000),
+	PV88060_SW(PV88060, SW4, 5000000),
+	PV88060_SW(PV88060, SW5, 5000000),
+	PV88060_SW(PV88060, SW6, 5000000),
+};
+
+static irqreturn_t pv88060_irq_handler(int irq, void *data)
+{
+	struct pv88060 *chip = data;
+	int i, reg_val, err, ret = IRQ_NONE;
+
+	err = regmap_read(chip->regmap, PV88060_REG_EVENT_A, &reg_val);
+	if (err < 0)
+		goto error_i2c;
+
+	if (reg_val & PV88060_E_VDD_FLT) {
+		for (i = 0; i < PV88060_MAX_REGULATORS; i++) {
+			if (chip->rdev[i] != NULL) {
+				regulator_notifier_call_chain(chip->rdev[i],
+					REGULATOR_EVENT_UNDER_VOLTAGE,
+					NULL);
+			}
+		}
+
+		err = regmap_update_bits(chip->regmap, PV88060_REG_EVENT_A,
+			PV88060_E_VDD_FLT, PV88060_E_VDD_FLT);
+		if (err < 0)
+			goto error_i2c;
+
+		ret = IRQ_HANDLED;
+	}
+
+	if (reg_val & PV88060_E_OVER_TEMP) {
+		for (i = 0; i < PV88060_MAX_REGULATORS; i++) {
+			if (chip->rdev[i] != NULL) {
+				regulator_notifier_call_chain(chip->rdev[i],
+					REGULATOR_EVENT_OVER_TEMP,
+					NULL);
+			}
+		}
+
+		err = regmap_update_bits(chip->regmap, PV88060_REG_EVENT_A,
+			PV88060_E_OVER_TEMP, PV88060_E_OVER_TEMP);
+		if (err < 0)
+			goto error_i2c;
+
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+
+error_i2c:
+	dev_err(chip->dev, "I2C error : %d\n", err);
+	return IRQ_NONE;
+}
+
+/*
+ * I2C driver interface functions
+ */
+static int pv88060_i2c_probe(struct i2c_client *i2c,
+		const struct i2c_device_id *id)
+{
+	struct regulator_init_data *init_data = dev_get_platdata(&i2c->dev);
+	struct pv88060 *chip;
+	struct regulator_config config = { };
+	int error, i, ret = 0;
+
+	chip = devm_kzalloc(&i2c->dev, sizeof(struct pv88060), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = &i2c->dev;
+	chip->regmap = devm_regmap_init_i2c(i2c, &pv88060_regmap_config);
+	if (IS_ERR(chip->regmap)) {
+		error = PTR_ERR(chip->regmap);
+		dev_err(chip->dev, "Failed to allocate register map: %d\n",
+			error);
+		return error;
+	}
+
+	i2c_set_clientdata(i2c, chip);
+
+	if (i2c->irq != 0) {
+		ret = regmap_write(chip->regmap, PV88060_REG_MASK_A, 0xFF);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to mask A reg: %d\n", ret);
+			return ret;
+		}
+
+		ret = regmap_write(chip->regmap, PV88060_REG_MASK_B, 0xFF);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to mask B reg: %d\n", ret);
+			return ret;
+		}
+
+		ret = regmap_write(chip->regmap, PV88060_REG_MASK_C, 0xFF);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to mask C reg: %d\n", ret);
+			return ret;
+		}
+
+		ret = devm_request_threaded_irq(&i2c->dev, i2c->irq, NULL,
+					pv88060_irq_handler,
+					IRQF_TRIGGER_LOW|IRQF_ONESHOT,
+					"pv88060", chip);
+		if (ret != 0) {
+			dev_err(chip->dev, "Failed to request IRQ: %d\n",
+				i2c->irq);
+			return ret;
+		}
+
+		ret = regmap_update_bits(chip->regmap, PV88060_REG_MASK_A,
+			PV88060_M_VDD_FLT | PV88060_M_OVER_TEMP, 0);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to update mask reg: %d\n", ret);
+			return ret;
+		}
+
+	} else {
+		dev_warn(chip->dev, "No IRQ configured\n");
+	}
+
+	config.dev = chip->dev;
+	config.regmap = chip->regmap;
+
+	for (i = 0; i < PV88060_MAX_REGULATORS; i++) {
+		if (init_data)
+			config.init_data = &init_data[i];
+
+		config.driver_data = (void *)&pv88060_regulator_info[i];
+		chip->rdev[i] = devm_regulator_register(chip->dev,
+			&pv88060_regulator_info[i].desc, &config);
+		if (IS_ERR(chip->rdev[i])) {
+			dev_err(chip->dev,
+				"Failed to register PV88060 regulator\n");
+			return PTR_ERR(chip->rdev[i]);
+		}
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id pv88060_i2c_id[] = {
+	{"pv88060", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, pv88060_i2c_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id pv88060_dt_ids[] = {
+	{ .compatible = "pvs,pv88060", .data = &pv88060_i2c_id[0] },
+	{},
+};
+MODULE_DEVICE_TABLE(of, pv88060_dt_ids);
+#endif
+
+static struct i2c_driver pv88060_regulator_driver = {
+	.driver = {
+		.name = "pv88060",
+		.of_match_table = of_match_ptr(pv88060_dt_ids),
+	},
+	.probe = pv88060_i2c_probe,
+	.id_table = pv88060_i2c_id,
+};
+
+module_i2c_driver(pv88060_regulator_driver);
+
+MODULE_AUTHOR("James Ban <James.Ban.opensource@diasemi.com>");
+MODULE_DESCRIPTION("Regulator device driver for Powerventure PV88060");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/pv88060-regulator.h b/drivers/regulator/pv88060-regulator.h
new file mode 100644
index 000000000000..02ca9203a172
--- /dev/null
+++ b/drivers/regulator/pv88060-regulator.h
@@ -0,0 +1,69 @@
+/*
+ * pv88060-regulator.h - Regulator definitions for PV88060
+ * Copyright (C) 2015 Powerventure Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __PV88060_REGISTERS_H__
+#define __PV88060_REGISTERS_H__
+
+/* System Control and Event Registers */
+#define	PV88060_REG_EVENT_A			0x04
+#define	PV88060_REG_MASK_A			0x08
+#define	PV88060_REG_MASK_B			0x09
+#define	PV88060_REG_MASK_C			0x0A
+
+/* Regulator Registers */
+#define	PV88060_REG_BUCK1_CONF0			0x1B
+#define	PV88060_REG_BUCK1_CONF1			0x1C
+#define	PV88060_REG_LDO1_CONF			0x1D
+#define	PV88060_REG_LDO2_CONF			0x1E
+#define	PV88060_REG_LDO3_CONF			0x1F
+#define	PV88060_REG_LDO4_CONF			0x20
+#define	PV88060_REG_LDO5_CONF			0x21
+#define	PV88060_REG_LDO6_CONF			0x22
+#define	PV88060_REG_LDO7_CONF			0x23
+
+#define	PV88060_REG_SW1_CONF			0x3B
+#define	PV88060_REG_SW2_CONF			0x3C
+#define	PV88060_REG_SW3_CONF			0x3D
+#define	PV88060_REG_SW4_CONF			0x3E
+#define	PV88060_REG_SW5_CONF			0x3F
+#define	PV88060_REG_SW6_CONF			0x40
+
+/* PV88060_REG_EVENT_A (addr=0x04) */
+#define	PV88060_E_VDD_FLT			0x01
+#define	PV88060_E_OVER_TEMP			0x02
+
+/* PV88060_REG_MASK_A (addr=0x08) */
+#define	PV88060_M_VDD_FLT			0x01
+#define	PV88060_M_OVER_TEMP			0x02
+
+/* PV88060_REG_BUCK1_CONF0 (addr=0x1B) */
+#define	PV88060_BUCK_EN			0x80
+#define PV88060_VBUCK_MASK			0x7F
+/* PV88060_REG_LDO1/2/3/4/5/6/7_CONT */
+#define	PV88060_LDO_EN			0x40
+#define PV88060_VLDO_MASK			0x3F
+/* PV88060_REG_SW1/2/3/4/5_CONF */
+#define	PV88060_SW_EN			0x80
+
+/* PV88060_REG_BUCK1_CONF1 (addr=0x1C) */
+#define	PV88060_BUCK_ILIM_SHIFT			2
+#define	PV88060_BUCK_ILIM_MASK			0x0C
+#define	PV88060_BUCK_MODE_SHIFT			0
+#define	PV88060_BUCK_MODE_MASK			0x03
+#define	PV88060_BUCK_MODE_SLEEP			0x00
+#define	PV88060_BUCK_MODE_AUTO			0x01
+#define	PV88060_BUCK_MODE_SYNC			0x02
+
+#endif	/* __PV88060_REGISTERS_H__ */
diff --git a/drivers/regulator/pv88090-regulator.c b/drivers/regulator/pv88090-regulator.c
new file mode 100644
index 000000000000..ac15f31b5fe0
--- /dev/null
+++ b/drivers/regulator/pv88090-regulator.c
@@ -0,0 +1,458 @@
+/*
+ * pv88090-regulator.c - Regulator device driver for PV88090
+ * Copyright (C) 2015  Powerventure Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regmap.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/proc_fs.h>
+#include <linux/uaccess.h>
+#include "pv88090-regulator.h"
+
+#define PV88090_MAX_REGULATORS	5
+
+/* PV88090 REGULATOR IDs */
+enum {
+	/* BUCKs */
+	PV88090_ID_BUCK1,
+	PV88090_ID_BUCK2,
+	PV88090_ID_BUCK3,
+
+	/* LDOs */
+	PV88090_ID_LDO1,
+	PV88090_ID_LDO2,
+};
+
+struct pv88090_regulator {
+	struct regulator_desc desc;
+	/* Current limiting */
+	unsigned	n_current_limits;
+	const int	*current_limits;
+	unsigned int limit_mask;
+	unsigned int conf;
+	unsigned int conf2;
+};
+
+struct pv88090 {
+	struct device *dev;
+	struct regmap *regmap;
+	struct regulator_dev *rdev[PV88090_MAX_REGULATORS];
+};
+
+struct pv88090_buck_voltage {
+	int min_uV;
+	int max_uV;
+	int uV_step;
+};
+
+static const struct regmap_config pv88090_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+};
+
+/* Current limits array (in uA) for BUCK1, BUCK2, BUCK3.
+ *  Entry indexes corresponds to register values.
+ */
+
+static const int pv88090_buck1_limits[] = {
+	 220000,  440000,  660000,  880000, 1100000, 1320000, 1540000, 1760000,
+	1980000, 2200000, 2420000, 2640000, 2860000, 3080000, 3300000, 3520000,
+	3740000, 3960000, 4180000, 4400000, 4620000, 4840000, 5060000, 5280000,
+	5500000, 5720000, 5940000, 6160000, 6380000, 6600000, 6820000, 7040000
+};
+
+static const int pv88090_buck23_limits[] = {
+	1496000, 2393000, 3291000, 4189000
+};
+
+static const struct pv88090_buck_voltage pv88090_buck_vol[3] = {
+	{
+		.min_uV = 600000,
+		.max_uV = 1393750,
+		.uV_step = 6250,
+	},
+
+	{
+		.min_uV = 1400000,
+		.max_uV = 2193750,
+		.uV_step = 6250,
+	},
+	{
+		.min_uV = 1250000,
+		.max_uV = 2837500,
+		.uV_step = 12500,
+	},
+};
+
+static unsigned int pv88090_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct pv88090_regulator *info = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret, mode = 0;
+
+	ret = regmap_read(rdev->regmap, info->conf, &data);
+	if (ret < 0)
+		return ret;
+
+	switch (data & PV88090_BUCK1_MODE_MASK) {
+	case PV88090_BUCK_MODE_SYNC:
+		mode = REGULATOR_MODE_FAST;
+		break;
+	case PV88090_BUCK_MODE_AUTO:
+		mode = REGULATOR_MODE_NORMAL;
+		break;
+	case PV88090_BUCK_MODE_SLEEP:
+		mode = REGULATOR_MODE_STANDBY;
+		break;
+	}
+
+	return mode;
+}
+
+static int pv88090_buck_set_mode(struct regulator_dev *rdev,
+					unsigned int mode)
+{
+	struct pv88090_regulator *info = rdev_get_drvdata(rdev);
+	int val = 0;
+
+	switch (mode) {
+	case REGULATOR_MODE_FAST:
+		val = PV88090_BUCK_MODE_SYNC;
+		break;
+	case REGULATOR_MODE_NORMAL:
+		val = PV88090_BUCK_MODE_AUTO;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		val = PV88090_BUCK_MODE_SLEEP;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return regmap_update_bits(rdev->regmap, info->conf,
+					PV88090_BUCK1_MODE_MASK, val);
+}
+
+static int pv88090_set_current_limit(struct regulator_dev *rdev, int min,
+				    int max)
+{
+	struct pv88090_regulator *info = rdev_get_drvdata(rdev);
+	int i;
+
+	/* search for closest to maximum */
+	for (i = info->n_current_limits; i >= 0; i--) {
+		if (min <= info->current_limits[i]
+			&& max >= info->current_limits[i]) {
+			return regmap_update_bits(rdev->regmap,
+				info->conf,
+				info->limit_mask,
+				i << PV88090_BUCK1_ILIM_SHIFT);
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int pv88090_get_current_limit(struct regulator_dev *rdev)
+{
+	struct pv88090_regulator *info = rdev_get_drvdata(rdev);
+	unsigned int data;
+	int ret;
+
+	ret = regmap_read(rdev->regmap, info->conf, &data);
+	if (ret < 0)
+		return ret;
+
+	data = (data & info->limit_mask) >> PV88090_BUCK1_ILIM_SHIFT;
+	return info->current_limits[data];
+}
+
+static struct regulator_ops pv88090_buck_ops = {
+	.get_mode = pv88090_buck_get_mode,
+	.set_mode = pv88090_buck_set_mode,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.set_current_limit = pv88090_set_current_limit,
+	.get_current_limit = pv88090_get_current_limit,
+};
+
+static struct regulator_ops pv88090_ldo_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+};
+
+#define PV88090_BUCK(chip, regl_name, min, step, max, limits_array) \
+{\
+	.desc	=	{\
+		.id = chip##_ID_##regl_name,\
+		.name = __stringify(chip##_##regl_name),\
+		.of_match = of_match_ptr(#regl_name),\
+		.regulators_node = of_match_ptr("regulators"),\
+		.type = REGULATOR_VOLTAGE,\
+		.owner = THIS_MODULE,\
+		.ops = &pv88090_buck_ops,\
+		.min_uV = min, \
+		.uV_step = step, \
+		.n_voltages = ((max) - (min))/(step) + 1, \
+		.enable_reg = PV88090_REG_##regl_name##_CONF0, \
+		.enable_mask = PV88090_##regl_name##_EN, \
+		.vsel_reg = PV88090_REG_##regl_name##_CONF0, \
+		.vsel_mask = PV88090_V##regl_name##_MASK, \
+	},\
+	.current_limits = limits_array, \
+	.n_current_limits = ARRAY_SIZE(limits_array), \
+	.limit_mask = PV88090_##regl_name##_ILIM_MASK, \
+	.conf = PV88090_REG_##regl_name##_CONF1, \
+	.conf2 = PV88090_REG_##regl_name##_CONF2, \
+}
+
+#define PV88090_LDO(chip, regl_name, min, step, max) \
+{\
+	.desc	=	{\
+		.id = chip##_ID_##regl_name,\
+		.name = __stringify(chip##_##regl_name),\
+		.of_match = of_match_ptr(#regl_name),\
+		.regulators_node = of_match_ptr("regulators"),\
+		.type = REGULATOR_VOLTAGE,\
+		.owner = THIS_MODULE,\
+		.ops = &pv88090_ldo_ops,\
+		.min_uV = min, \
+		.uV_step = step, \
+		.n_voltages = ((max) - (min))/(step) + 1, \
+		.enable_reg = PV88090_REG_##regl_name##_CONT, \
+		.enable_mask = PV88090_##regl_name##_EN, \
+		.vsel_reg = PV88090_REG_##regl_name##_CONT, \
+		.vsel_mask = PV88090_V##regl_name##_MASK, \
+	},\
+}
+
+static struct pv88090_regulator pv88090_regulator_info[] = {
+	PV88090_BUCK(PV88090, BUCK1, 600000, 6250, 1393750,
+		pv88090_buck1_limits),
+	PV88090_BUCK(PV88090, BUCK2, 600000, 6250, 1393750,
+		pv88090_buck23_limits),
+	PV88090_BUCK(PV88090, BUCK3, 600000, 6250, 1393750,
+		pv88090_buck23_limits),
+	PV88090_LDO(PV88090, LDO1, 1200000, 50000, 4350000),
+	PV88090_LDO(PV88090, LDO2,  650000, 25000, 2225000),
+};
+
+static irqreturn_t pv88090_irq_handler(int irq, void *data)
+{
+	struct pv88090 *chip = data;
+	int i, reg_val, err, ret = IRQ_NONE;
+
+	err = regmap_read(chip->regmap, PV88090_REG_EVENT_A, &reg_val);
+	if (err < 0)
+		goto error_i2c;
+
+	if (reg_val & PV88090_E_VDD_FLT) {
+		for (i = 0; i < PV88090_MAX_REGULATORS; i++) {
+			if (chip->rdev[i] != NULL) {
+				regulator_notifier_call_chain(chip->rdev[i],
+					REGULATOR_EVENT_UNDER_VOLTAGE,
+					NULL);
+			}
+		}
+
+		err = regmap_update_bits(chip->regmap, PV88090_REG_EVENT_A,
+			PV88090_E_VDD_FLT, PV88090_E_VDD_FLT);
+		if (err < 0)
+			goto error_i2c;
+
+		ret = IRQ_HANDLED;
+	}
+
+	if (reg_val & PV88090_E_OVER_TEMP) {
+		for (i = 0; i < PV88090_MAX_REGULATORS; i++) {
+			if (chip->rdev[i] != NULL) {
+				regulator_notifier_call_chain(chip->rdev[i],
+					REGULATOR_EVENT_OVER_TEMP,
+					NULL);
+			}
+		}
+
+		err = regmap_update_bits(chip->regmap, PV88090_REG_EVENT_A,
+			PV88090_E_OVER_TEMP, PV88090_E_OVER_TEMP);
+		if (err < 0)
+			goto error_i2c;
+
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+
+error_i2c:
+	dev_err(chip->dev, "I2C error : %d\n", err);
+	return IRQ_NONE;
+}
+
+/*
+ * I2C driver interface functions
+ */
+static int pv88090_i2c_probe(struct i2c_client *i2c,
+		const struct i2c_device_id *id)
+{
+	struct regulator_init_data *init_data = dev_get_platdata(&i2c->dev);
+	struct pv88090 *chip;
+	struct regulator_config config = { };
+	int error, i, ret = 0;
+	unsigned int conf2, range, index;
+
+	chip = devm_kzalloc(&i2c->dev, sizeof(struct pv88090), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	chip->dev = &i2c->dev;
+	chip->regmap = devm_regmap_init_i2c(i2c, &pv88090_regmap_config);
+	if (IS_ERR(chip->regmap)) {
+		error = PTR_ERR(chip->regmap);
+		dev_err(chip->dev, "Failed to allocate register map: %d\n",
+			error);
+		return error;
+	}
+
+	i2c_set_clientdata(i2c, chip);
+
+	if (i2c->irq != 0) {
+		ret = regmap_write(chip->regmap, PV88090_REG_MASK_A, 0xFF);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to mask A reg: %d\n", ret);
+			return ret;
+		}
+
+		ret = regmap_write(chip->regmap, PV88090_REG_MASK_B, 0xFF);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to mask B reg: %d\n", ret);
+			return ret;
+		}
+
+		ret = devm_request_threaded_irq(&i2c->dev, i2c->irq, NULL,
+					pv88090_irq_handler,
+					IRQF_TRIGGER_LOW|IRQF_ONESHOT,
+					"pv88090", chip);
+		if (ret != 0) {
+			dev_err(chip->dev, "Failed to request IRQ: %d\n",
+				i2c->irq);
+			return ret;
+		}
+
+		ret = regmap_update_bits(chip->regmap, PV88090_REG_MASK_A,
+			PV88090_M_VDD_FLT | PV88090_M_OVER_TEMP, 0);
+		if (ret < 0) {
+			dev_err(chip->dev,
+				"Failed to update mask reg: %d\n", ret);
+			return ret;
+		}
+
+	} else {
+		dev_warn(chip->dev, "No IRQ configured\n");
+	}
+
+	config.dev = chip->dev;
+	config.regmap = chip->regmap;
+
+	for (i = 0; i < PV88090_MAX_REGULATORS; i++) {
+		if (init_data)
+			config.init_data = &init_data[i];
+
+		if (i == PV88090_ID_BUCK2 || i == PV88090_ID_BUCK3) {
+			ret = regmap_read(chip->regmap,
+				pv88090_regulator_info[i].conf2, &conf2);
+			if (ret < 0)
+				return ret;
+
+			conf2 = (conf2 >> PV88090_BUCK_VDAC_RANGE_SHIFT) &
+				PV88090_BUCK_VDAC_RANGE_MASK;
+
+			ret = regmap_read(chip->regmap,
+				PV88090_REG_BUCK_FOLD_RANGE, &range);
+			if (ret < 0)
+				return ret;
+
+			range = (range >>
+				 (PV88080_BUCK_VRANGE_GAIN_SHIFT + i - 1)) &
+				PV88080_BUCK_VRANGE_GAIN_MASK;
+			index = ((range << 1) | conf2);
+
+			pv88090_regulator_info[i].desc.min_uV
+				= pv88090_buck_vol[index].min_uV;
+			pv88090_regulator_info[i].desc.uV_step
+				= pv88090_buck_vol[index].uV_step;
+			pv88090_regulator_info[i].desc.n_voltages
+				= ((pv88090_buck_vol[index].max_uV)
+				- (pv88090_buck_vol[index].min_uV))
+				/(pv88090_buck_vol[index].uV_step) + 1;
+		}
+
+		config.driver_data = (void *)&pv88090_regulator_info[i];
+		chip->rdev[i] = devm_regulator_register(chip->dev,
+			&pv88090_regulator_info[i].desc, &config);
+		if (IS_ERR(chip->rdev[i])) {
+			dev_err(chip->dev,
+				"Failed to register PV88090 regulator\n");
+			return PTR_ERR(chip->rdev[i]);
+		}
+	}
+
+	return 0;
+}
+
+static const struct i2c_device_id pv88090_i2c_id[] = {
+	{"pv88090", 0},
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, pv88090_i2c_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id pv88090_dt_ids[] = {
+	{ .compatible = "pvs,pv88090", .data = &pv88090_i2c_id[0] },
+	{},
+};
+MODULE_DEVICE_TABLE(of, pv88090_dt_ids);
+#endif
+
+static struct i2c_driver pv88090_regulator_driver = {
+	.driver = {
+		.name = "pv88090",
+		.of_match_table = of_match_ptr(pv88090_dt_ids),
+	},
+	.probe = pv88090_i2c_probe,
+	.id_table = pv88090_i2c_id,
+};
+
+module_i2c_driver(pv88090_regulator_driver);
+
+MODULE_AUTHOR("James Ban <James.Ban.opensource@diasemi.com>");
+MODULE_DESCRIPTION("Regulator device driver for Powerventure PV88090");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/pv88090-regulator.h b/drivers/regulator/pv88090-regulator.h
new file mode 100644
index 000000000000..d7aca8d8266d
--- /dev/null
+++ b/drivers/regulator/pv88090-regulator.h
@@ -0,0 +1,98 @@
+/*
+ * pv88090-regulator.h - Regulator definitions for PV88090
+ * Copyright (C) 2015 Powerventure Semiconductor Ltd.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __PV88090_REGISTERS_H__
+#define __PV88090_REGISTERS_H__
+
+/* System Control and Event Registers */
+#define	PV88090_REG_EVENT_A			0x03
+#define	PV88090_REG_MASK_A			0x06
+#define	PV88090_REG_MASK_B			0x07
+
+/* Regulator Registers */
+#define	PV88090_REG_BUCK1_CONF0			0x18
+#define	PV88090_REG_BUCK1_CONF1			0x19
+#define	PV88090_REG_BUCK1_CONF2			0x1a
+#define	PV88090_REG_BUCK2_CONF0			0x1b
+#define	PV88090_REG_BUCK2_CONF1			0x1c
+#define	PV88090_REG_BUCK2_CONF2			0x58
+#define	PV88090_REG_BUCK3_CONF0			0x1d
+#define	PV88090_REG_BUCK3_CONF1			0x1e
+#define	PV88090_REG_BUCK3_CONF2			0x5c
+
+#define	PV88090_REG_LDO1_CONT			0x1f
+#define	PV88090_REG_LDO2_CONT			0x20
+#define	PV88090_REG_LDO3_CONT			0x21
+#define	PV88090_REG_BUCK_FOLD_RANGE			0x61
+
+/* PV88090_REG_EVENT_A (addr=0x03) */
+#define	PV88090_E_VDD_FLT				0x01
+#define	PV88090_E_OVER_TEMP			0x02
+
+/* PV88090_REG_MASK_A (addr=0x06) */
+#define	PV88090_M_VDD_FLT				0x01
+#define	PV88090_M_OVER_TEMP			0x02
+
+/* PV88090_REG_BUCK1_CONF0 (addr=0x18) */
+#define	PV88090_BUCK1_EN				0x80
+#define PV88090_VBUCK1_MASK			0x7F
+/* PV88090_REG_BUCK2_CONF0 (addr=0x1b) */
+#define	PV88090_BUCK2_EN				0x80
+#define PV88090_VBUCK2_MASK			0x7F
+/* PV88090_REG_BUCK3_CONF0 (addr=0x1d) */
+#define	PV88090_BUCK3_EN				0x80
+#define PV88090_VBUCK3_MASK			0x7F
+/* PV88090_REG_LDO1_CONT (addr=0x1f) */
+#define	PV88090_LDO1_EN				0x40
+#define PV88090_VLDO1_MASK			0x3F
+/* PV88090_REG_LDO2_CONT (addr=0x20) */
+#define	PV88090_LDO2_EN				0x40
+#define PV88090_VLDO2_MASK			0x3F
+
+/* PV88090_REG_BUCK1_CONF1 (addr=0x19) */
+#define PV88090_BUCK1_ILIM_SHIFT			2
+#define PV88090_BUCK1_ILIM_MASK			0x7C
+#define PV88090_BUCK1_MODE_MASK			0x03
+
+/* PV88090_REG_BUCK2_CONF1 (addr=0x1c) */
+#define PV88090_BUCK2_ILIM_SHIFT			2
+#define PV88090_BUCK2_ILIM_MASK			0x0C
+#define PV88090_BUCK2_MODE_MASK			0x03
+
+/* PV88090_REG_BUCK3_CONF1 (addr=0x1e) */
+#define PV88090_BUCK3_ILIM_SHIFT			2
+#define PV88090_BUCK3_ILIM_MASK			0x0C
+#define PV88090_BUCK3_MODE_MASK			0x03
+
+#define	PV88090_BUCK_MODE_SLEEP			0x00
+#define	PV88090_BUCK_MODE_AUTO			0x01
+#define	PV88090_BUCK_MODE_SYNC			0x02
+
+/* PV88090_REG_BUCK2_CONF2 (addr=0x58) */
+/* PV88090_REG_BUCK3_CONF2 (addr=0x5c) */
+#define PV88090_BUCK_VDAC_RANGE_SHIFT			7
+#define PV88090_BUCK_VDAC_RANGE_MASK			0x01
+
+#define PV88090_BUCK_VDAC_RANGE_1			0x00
+#define PV88090_BUCK_VDAC_RANGE_2			0x01
+
+/* PV88090_REG_BUCK_FOLD_RANGE (addr=0x61) */
+#define PV88080_BUCK_VRANGE_GAIN_SHIFT			3
+#define PV88080_BUCK_VRANGE_GAIN_MASK			0x01
+
+#define PV88080_BUCK_VRANGE_GAIN_1			0x00
+#define PV88080_BUCK_VRANGE_GAIN_2			0x01
+
+#endif	/* __PV88090_REGISTERS_H__ */
diff --git a/drivers/regulator/qcom_smd-regulator.c b/drivers/regulator/qcom_smd-regulator.c
index 6fa0c7d13290..56a17ec5b5ef 100644
--- a/drivers/regulator/qcom_smd-regulator.c
+++ b/drivers/regulator/qcom_smd-regulator.c
@@ -153,6 +153,49 @@ static const struct regulator_ops rpm_switch_ops = {
 	.is_enabled = rpm_reg_is_enabled,
 };
 
+static const struct regulator_desc pma8084_hfsmps = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(375000,  0,  95, 12500),
+		REGULATOR_LINEAR_RANGE(1550000, 96, 158, 25000),
+	},
+	.n_linear_ranges = 2,
+	.n_voltages = 159,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pma8084_ftsmps = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(350000,  0, 184, 5000),
+		REGULATOR_LINEAR_RANGE(700000, 185, 339, 10000),
+	},
+	.n_linear_ranges = 2,
+	.n_voltages = 340,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pma8084_pldo = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(750000,  0,  30, 25000),
+		REGULATOR_LINEAR_RANGE(1500000, 31, 99, 50000),
+	},
+	.n_linear_ranges = 2,
+	.n_voltages = 100,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pma8084_nldo = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(750000, 0, 63, 12500),
+	},
+	.n_linear_ranges = 1,
+	.n_voltages = 64,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pma8084_switch = {
+	.ops = &rpm_switch_ops,
+};
+
 static const struct regulator_desc pm8x41_hfsmps = {
 	.linear_ranges = (struct regulator_linear_range[]) {
 		REGULATOR_LINEAR_RANGE( 375000,  0,  95, 12500),
@@ -211,6 +254,43 @@ static const struct regulator_desc pm8941_switch = {
 	.ops = &rpm_switch_ops,
 };
 
+static const struct regulator_desc pm8916_pldo = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(750000, 0, 208, 12500),
+	},
+	.n_linear_ranges = 1,
+	.n_voltages = 209,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pm8916_nldo = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(375000, 0, 93, 12500),
+	},
+	.n_linear_ranges = 1,
+	.n_voltages = 94,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pm8916_buck_lvo_smps = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(375000, 0, 95, 12500),
+		REGULATOR_LINEAR_RANGE(750000, 96, 127, 25000),
+	},
+	.n_linear_ranges = 2,
+	.n_voltages = 128,
+	.ops = &rpm_smps_ldo_ops,
+};
+
+static const struct regulator_desc pm8916_buck_hvo_smps = {
+	.linear_ranges = (struct regulator_linear_range[]) {
+		REGULATOR_LINEAR_RANGE(1550000, 0, 31, 25000),
+	},
+	.n_linear_ranges = 1,
+	.n_voltages = 32,
+	.ops = &rpm_smps_ldo_ops,
+};
+
 struct rpm_regulator_data {
 	const char *name;
 	u32 type;
@@ -231,6 +311,32 @@ static const struct rpm_regulator_data rpm_pm8841_regulators[] = {
 	{}
 };
 
+static const struct rpm_regulator_data rpm_pm8916_regulators[] = {
+	{ "s1", QCOM_SMD_RPM_SMPA, 1, &pm8916_buck_lvo_smps, "vdd_s1" },
+	{ "s2", QCOM_SMD_RPM_SMPA, 2, &pm8916_buck_lvo_smps, "vdd_s2" },
+	{ "s3", QCOM_SMD_RPM_SMPA, 3, &pm8916_buck_lvo_smps, "vdd_s3" },
+	{ "s4", QCOM_SMD_RPM_SMPA, 4, &pm8916_buck_hvo_smps, "vdd_s4" },
+	{ "l1", QCOM_SMD_RPM_LDOA, 1, &pm8916_nldo, "vdd_l1_l2_l3" },
+	{ "l2", QCOM_SMD_RPM_LDOA, 2, &pm8916_nldo, "vdd_l1_l2_l3" },
+	{ "l3", QCOM_SMD_RPM_LDOA, 3, &pm8916_nldo, "vdd_l1_l2_l3" },
+	{ "l4", QCOM_SMD_RPM_LDOA, 4, &pm8916_pldo, "vdd_l4_l5_l6" },
+	{ "l5", QCOM_SMD_RPM_LDOA, 5, &pm8916_pldo, "vdd_l4_l5_l6" },
+	{ "l6", QCOM_SMD_RPM_LDOA, 6, &pm8916_pldo, "vdd_l4_l5_l6" },
+	{ "l7", QCOM_SMD_RPM_LDOA, 7, &pm8916_pldo, "vdd_l7" },
+	{ "l8", QCOM_SMD_RPM_LDOA, 8, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18" },
+	{ "l9", QCOM_SMD_RPM_LDOA, 9, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18" },
+	{ "l10", QCOM_SMD_RPM_LDOA, 10, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l11", QCOM_SMD_RPM_LDOA, 11, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l12", QCOM_SMD_RPM_LDOA, 12, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l13", QCOM_SMD_RPM_LDOA, 13, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l14", QCOM_SMD_RPM_LDOA, 14, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l15", QCOM_SMD_RPM_LDOA, 15, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l16", QCOM_SMD_RPM_LDOA, 16, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l17", QCOM_SMD_RPM_LDOA, 17, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{ "l18", QCOM_SMD_RPM_LDOA, 18, &pm8916_pldo, "vdd_l8_l9_l10_l11_l12_l13_l14_l15_l16_l17_l18"},
+	{}
+};
+
 static const struct rpm_regulator_data rpm_pm8941_regulators[] = {
 	{ "s1", QCOM_SMD_RPM_SMPA, 1, &pm8x41_hfsmps, "vdd_s1" },
 	{ "s2", QCOM_SMD_RPM_SMPA, 2, &pm8x41_hfsmps, "vdd_s2" },
@@ -272,9 +378,62 @@ static const struct rpm_regulator_data rpm_pm8941_regulators[] = {
 	{}
 };
 
+static const struct rpm_regulator_data rpm_pma8084_regulators[] = {
+	{ "s1", QCOM_SMD_RPM_SMPA, 1, &pma8084_ftsmps, "vdd_s1" },
+	{ "s2", QCOM_SMD_RPM_SMPA, 2, &pma8084_ftsmps, "vdd_s2" },
+	{ "s3", QCOM_SMD_RPM_SMPA, 3, &pma8084_hfsmps, "vdd_s3" },
+	{ "s4", QCOM_SMD_RPM_SMPA, 4, &pma8084_hfsmps, "vdd_s4" },
+	{ "s5", QCOM_SMD_RPM_SMPA, 5, &pma8084_hfsmps, "vdd_s5" },
+	{ "s6", QCOM_SMD_RPM_SMPA, 6, &pma8084_ftsmps, "vdd_s6" },
+	{ "s7", QCOM_SMD_RPM_SMPA, 7, &pma8084_ftsmps, "vdd_s7" },
+	{ "s8", QCOM_SMD_RPM_SMPA, 8, &pma8084_ftsmps, "vdd_s8" },
+	{ "s9", QCOM_SMD_RPM_SMPA, 9, &pma8084_ftsmps, "vdd_s9" },
+	{ "s10", QCOM_SMD_RPM_SMPA, 10, &pma8084_ftsmps, "vdd_s10" },
+	{ "s11", QCOM_SMD_RPM_SMPA, 11, &pma8084_ftsmps, "vdd_s11" },
+	{ "s12", QCOM_SMD_RPM_SMPA, 12, &pma8084_ftsmps, "vdd_s12" },
+
+	{ "l1", QCOM_SMD_RPM_LDOA, 1, &pma8084_nldo, "vdd_l1_l11" },
+	{ "l2", QCOM_SMD_RPM_LDOA, 2, &pma8084_nldo, "vdd_l2_l3_l4_l27" },
+	{ "l3", QCOM_SMD_RPM_LDOA, 3, &pma8084_nldo, "vdd_l2_l3_l4_l27" },
+	{ "l4", QCOM_SMD_RPM_LDOA, 4, &pma8084_nldo, "vdd_l2_l3_l4_l27" },
+	{ "l5", QCOM_SMD_RPM_LDOA, 5, &pma8084_pldo, "vdd_l5_l7" },
+	{ "l6", QCOM_SMD_RPM_LDOA, 6, &pma8084_pldo, "vdd_l6_l12_l14_l15_l26" },
+	{ "l7", QCOM_SMD_RPM_LDOA, 7, &pma8084_pldo, "vdd_l5_l7" },
+	{ "l8", QCOM_SMD_RPM_LDOA, 8, &pma8084_pldo, "vdd_l8" },
+	{ "l9", QCOM_SMD_RPM_LDOA, 9, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l10", QCOM_SMD_RPM_LDOA, 10, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l11", QCOM_SMD_RPM_LDOA, 11, &pma8084_nldo, "vdd_l1_l11" },
+	{ "l12", QCOM_SMD_RPM_LDOA, 12, &pma8084_pldo, "vdd_l6_l12_l14_l15_l26" },
+	{ "l13", QCOM_SMD_RPM_LDOA, 13, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l14", QCOM_SMD_RPM_LDOA, 14, &pma8084_pldo, "vdd_l6_l12_l14_l15_l26" },
+	{ "l15", QCOM_SMD_RPM_LDOA, 15, &pma8084_pldo, "vdd_l6_l12_l14_l15_l26" },
+	{ "l16", QCOM_SMD_RPM_LDOA, 16, &pma8084_pldo, "vdd_l16_l25" },
+	{ "l17", QCOM_SMD_RPM_LDOA, 17, &pma8084_pldo, "vdd_l17" },
+	{ "l18", QCOM_SMD_RPM_LDOA, 18, &pma8084_pldo, "vdd_l18" },
+	{ "l19", QCOM_SMD_RPM_LDOA, 19, &pma8084_pldo, "vdd_l19" },
+	{ "l20", QCOM_SMD_RPM_LDOA, 20, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l21", QCOM_SMD_RPM_LDOA, 21, &pma8084_pldo, "vdd_l21" },
+	{ "l22", QCOM_SMD_RPM_LDOA, 22, &pma8084_pldo, "vdd_l22" },
+	{ "l23", QCOM_SMD_RPM_LDOA, 23, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l24", QCOM_SMD_RPM_LDOA, 24, &pma8084_pldo, "vdd_l9_l10_l13_l20_l23_l24" },
+	{ "l25", QCOM_SMD_RPM_LDOA, 25, &pma8084_pldo, "vdd_l16_l25" },
+	{ "l26", QCOM_SMD_RPM_LDOA, 26, &pma8084_pldo, "vdd_l6_l12_l14_l15_l26" },
+	{ "l27", QCOM_SMD_RPM_LDOA, 27, &pma8084_nldo, "vdd_l2_l3_l4_l27" },
+
+	{ "lvs1", QCOM_SMD_RPM_VSA, 1, &pma8084_switch },
+	{ "lvs2", QCOM_SMD_RPM_VSA, 2, &pma8084_switch },
+	{ "lvs3", QCOM_SMD_RPM_VSA, 3, &pma8084_switch },
+	{ "lvs4", QCOM_SMD_RPM_VSA, 4, &pma8084_switch },
+	{ "5vs1", QCOM_SMD_RPM_VSA, 5, &pma8084_switch },
+
+	{}
+};
+
 static const struct of_device_id rpm_of_match[] = {
 	{ .compatible = "qcom,rpm-pm8841-regulators", .data = &rpm_pm8841_regulators },
+	{ .compatible = "qcom,rpm-pm8916-regulators", .data = &rpm_pm8916_regulators },
 	{ .compatible = "qcom,rpm-pm8941-regulators", .data = &rpm_pm8941_regulators },
+	{ .compatible = "qcom,rpm-pma8084-regulators", .data = &rpm_pma8084_regulators },
 	{}
 };
 MODULE_DEVICE_TABLE(of, rpm_of_match);
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index 72fc3c32db49..3242ffc0cb25 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -32,6 +32,7 @@
 #include <linux/mfd/samsung/s2mps11.h>
 #include <linux/mfd/samsung/s2mps13.h>
 #include <linux/mfd/samsung/s2mps14.h>
+#include <linux/mfd/samsung/s2mps15.h>
 #include <linux/mfd/samsung/s2mpu02.h>
 
 /* The highest number of possible regulators for supported devices. */
@@ -661,6 +662,133 @@ static const struct regulator_desc s2mps14_regulators[] = {
 				    S2MPS14_BUCK1235_START_SEL),
 };
 
+static struct regulator_ops s2mps15_reg_ldo_ops = {
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+};
+
+static struct regulator_ops s2mps15_reg_buck_ops = {
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+};
+
+#define regulator_desc_s2mps15_ldo(num, range) {	\
+	.name		= "LDO"#num,			\
+	.id		= S2MPS15_LDO##num,		\
+	.ops		= &s2mps15_reg_ldo_ops,		\
+	.type		= REGULATOR_VOLTAGE,		\
+	.owner		= THIS_MODULE,			\
+	.linear_ranges	= range,			\
+	.n_linear_ranges = ARRAY_SIZE(range),		\
+	.n_voltages	= S2MPS15_LDO_N_VOLTAGES,	\
+	.vsel_reg	= S2MPS15_REG_L1CTRL + num - 1,	\
+	.vsel_mask	= S2MPS15_LDO_VSEL_MASK,	\
+	.enable_reg	= S2MPS15_REG_L1CTRL + num - 1,	\
+	.enable_mask	= S2MPS15_ENABLE_MASK		\
+}
+
+#define regulator_desc_s2mps15_buck(num, range) {			\
+	.name		= "BUCK"#num,					\
+	.id		= S2MPS15_BUCK##num,				\
+	.ops		= &s2mps15_reg_buck_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.linear_ranges	= range,					\
+	.n_linear_ranges = ARRAY_SIZE(range),				\
+	.ramp_delay	= 12500,					\
+	.n_voltages	= S2MPS15_BUCK_N_VOLTAGES,			\
+	.vsel_reg	= S2MPS15_REG_B1CTRL2 + ((num - 1) * 2),	\
+	.vsel_mask	= S2MPS15_BUCK_VSEL_MASK,			\
+	.enable_reg	= S2MPS15_REG_B1CTRL1 + ((num - 1) * 2),	\
+	.enable_mask	= S2MPS15_ENABLE_MASK				\
+}
+
+/* voltage range for s2mps15 LDO 3, 5, 15, 16, 18, 20, 23 and 27 */
+static const struct regulator_linear_range s2mps15_ldo_voltage_ranges1[] = {
+	REGULATOR_LINEAR_RANGE(1000000, 0xc, 0x38, 25000),
+};
+
+/* voltage range for s2mps15 LDO 2, 6, 14, 17, 19, 21, 24 and 25 */
+static const struct regulator_linear_range s2mps15_ldo_voltage_ranges2[] = {
+	REGULATOR_LINEAR_RANGE(1800000, 0x0, 0x3f, 25000),
+};
+
+/* voltage range for s2mps15 LDO 4, 11, 12, 13, 22 and 26 */
+static const struct regulator_linear_range s2mps15_ldo_voltage_ranges3[] = {
+	REGULATOR_LINEAR_RANGE(700000, 0x0, 0x34, 12500),
+};
+
+/* voltage range for s2mps15 LDO 7, 8, 9 and 10 */
+static const struct regulator_linear_range s2mps15_ldo_voltage_ranges4[] = {
+	REGULATOR_LINEAR_RANGE(700000, 0xc, 0x18, 25000),
+};
+
+/* voltage range for s2mps15 LDO 1 */
+static const struct regulator_linear_range s2mps15_ldo_voltage_ranges5[] = {
+	REGULATOR_LINEAR_RANGE(500000, 0x0, 0x20, 12500),
+};
+
+/* voltage range for s2mps15 BUCK 1, 2, 3, 4, 5, 6 and 7 */
+static const struct regulator_linear_range s2mps15_buck_voltage_ranges1[] = {
+	REGULATOR_LINEAR_RANGE(500000, 0x20, 0xb0, 6250),
+};
+
+/* voltage range for s2mps15 BUCK 8, 9 and 10 */
+static const struct regulator_linear_range s2mps15_buck_voltage_ranges2[] = {
+	REGULATOR_LINEAR_RANGE(1000000, 0x20, 0xc0, 12500),
+};
+
+static const struct regulator_desc s2mps15_regulators[] = {
+	regulator_desc_s2mps15_ldo(1, s2mps15_ldo_voltage_ranges5),
+	regulator_desc_s2mps15_ldo(2, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(3, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(4, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(5, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(6, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(7, s2mps15_ldo_voltage_ranges4),
+	regulator_desc_s2mps15_ldo(8, s2mps15_ldo_voltage_ranges4),
+	regulator_desc_s2mps15_ldo(9, s2mps15_ldo_voltage_ranges4),
+	regulator_desc_s2mps15_ldo(10, s2mps15_ldo_voltage_ranges4),
+	regulator_desc_s2mps15_ldo(11, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(12, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(13, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(14, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(15, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(16, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(17, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(18, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(19, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(20, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(21, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(22, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(23, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_ldo(24, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(25, s2mps15_ldo_voltage_ranges2),
+	regulator_desc_s2mps15_ldo(26, s2mps15_ldo_voltage_ranges3),
+	regulator_desc_s2mps15_ldo(27, s2mps15_ldo_voltage_ranges1),
+	regulator_desc_s2mps15_buck(1, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(2, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(3, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(4, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(5, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(6, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(7, s2mps15_buck_voltage_ranges1),
+	regulator_desc_s2mps15_buck(8, s2mps15_buck_voltage_ranges2),
+	regulator_desc_s2mps15_buck(9, s2mps15_buck_voltage_ranges2),
+	regulator_desc_s2mps15_buck(10, s2mps15_buck_voltage_ranges2),
+};
+
 static int s2mps14_pmic_enable_ext_control(struct s2mps11_info *s2mps11,
 		struct regulator_dev *rdev)
 {
@@ -974,6 +1102,10 @@ static int s2mps11_pmic_probe(struct platform_device *pdev)
 		regulators = s2mps14_regulators;
 		BUILD_BUG_ON(S2MPS_REGULATOR_MAX < s2mps11->rdev_num);
 		break;
+	case S2MPS15X:
+		s2mps11->rdev_num = ARRAY_SIZE(s2mps15_regulators);
+		regulators = s2mps15_regulators;
+		break;
 	case S2MPU02:
 		s2mps11->rdev_num = ARRAY_SIZE(s2mpu02_regulators);
 		regulators = s2mpu02_regulators;
@@ -1067,10 +1199,11 @@ out:
 }
 
 static const struct platform_device_id s2mps11_pmic_id[] = {
-	{ "s2mps11-pmic", S2MPS11X},
-	{ "s2mps13-pmic", S2MPS13X},
-	{ "s2mps14-pmic", S2MPS14X},
-	{ "s2mpu02-pmic", S2MPU02},
+	{ "s2mps11-regulator", S2MPS11X},
+	{ "s2mps13-regulator", S2MPS13X},
+	{ "s2mps14-regulator", S2MPS14X},
+	{ "s2mps15-regulator", S2MPS15X},
+	{ "s2mpu02-regulator", S2MPU02},
 	{ },
 };
 MODULE_DEVICE_TABLE(platform, s2mps11_pmic_id);
@@ -1097,5 +1230,5 @@ module_exit(s2mps11_pmic_exit);
 
 /* Module information */
 MODULE_AUTHOR("Sangbeom Kim <sbkim73@samsung.com>");
-MODULE_DESCRIPTION("SAMSUNG S2MPS11/S2MPS14/S2MPU02 Regulator Driver");
+MODULE_DESCRIPTION("SAMSUNG S2MPS11/S2MPS14/S2MPS15/S2MPU02 Regulator Driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
index ddc4f10e268a..584ef3dedca6 100644
--- a/drivers/regulator/tps6105x-regulator.c
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -27,90 +27,12 @@ static const unsigned int tps6105x_voltages[] = {
 	5000000, /* There is an additional 5V */
 };
 
-static int tps6105x_regulator_enable(struct regulator_dev *rdev)
-{
-	struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
-	int ret;
-
-	/* Activate voltage mode */
-	ret = regmap_update_bits(tps6105x->regmap, TPS6105X_REG_0,
-		TPS6105X_REG0_MODE_MASK,
-		TPS6105X_REG0_MODE_VOLTAGE << TPS6105X_REG0_MODE_SHIFT);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
-static int tps6105x_regulator_disable(struct regulator_dev *rdev)
-{
-	struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
-	int ret;
-
-	/* Set into shutdown mode */
-	ret = regmap_update_bits(tps6105x->regmap, TPS6105X_REG_0,
-		TPS6105X_REG0_MODE_MASK,
-		TPS6105X_REG0_MODE_SHUTDOWN << TPS6105X_REG0_MODE_SHIFT);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
-static int tps6105x_regulator_is_enabled(struct regulator_dev *rdev)
-{
-	struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
-	unsigned int regval;
-	int ret;
-
-	ret = regmap_read(tps6105x->regmap, TPS6105X_REG_0, &regval);
-	if (ret)
-		return ret;
-	regval &= TPS6105X_REG0_MODE_MASK;
-	regval >>= TPS6105X_REG0_MODE_SHIFT;
-
-	if (regval == TPS6105X_REG0_MODE_VOLTAGE)
-		return 1;
-
-	return 0;
-}
-
-static int tps6105x_regulator_get_voltage_sel(struct regulator_dev *rdev)
-{
-	struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
-	unsigned int regval;
-	int ret;
-
-	ret = regmap_read(tps6105x->regmap, TPS6105X_REG_0, &regval);
-	if (ret)
-		return ret;
-
-	regval &= TPS6105X_REG0_VOLTAGE_MASK;
-	regval >>= TPS6105X_REG0_VOLTAGE_SHIFT;
-	return (int) regval;
-}
-
-static int tps6105x_regulator_set_voltage_sel(struct regulator_dev *rdev,
-					      unsigned selector)
-{
-	struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
-	int ret;
-
-	ret = regmap_update_bits(tps6105x->regmap, TPS6105X_REG_0,
-				    TPS6105X_REG0_VOLTAGE_MASK,
-				    selector << TPS6105X_REG0_VOLTAGE_SHIFT);
-	if (ret)
-		return ret;
-
-	return 0;
-}
-
 static struct regulator_ops tps6105x_regulator_ops = {
-	.enable		= tps6105x_regulator_enable,
-	.disable	= tps6105x_regulator_disable,
-	.is_enabled	= tps6105x_regulator_is_enabled,
-	.get_voltage_sel = tps6105x_regulator_get_voltage_sel,
-	.set_voltage_sel = tps6105x_regulator_set_voltage_sel,
+	.enable		= regulator_enable_regmap,
+	.disable	= regulator_disable_regmap,
+	.is_enabled	= regulator_is_enabled_regmap,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.list_voltage	= regulator_list_voltage_table,
 };
 
@@ -122,6 +44,12 @@ static const struct regulator_desc tps6105x_regulator_desc = {
 	.owner		= THIS_MODULE,
 	.n_voltages	= ARRAY_SIZE(tps6105x_voltages),
 	.volt_table	= tps6105x_voltages,
+	.vsel_reg	= TPS6105X_REG_0,
+	.vsel_mask	= TPS6105X_REG0_VOLTAGE_MASK,
+	.enable_reg	= TPS6105X_REG_0,
+	.enable_mask	= TPS6105X_REG0_MODE_MASK,
+	.enable_val	= TPS6105X_REG0_MODE_VOLTAGE <<
+			  TPS6105X_REG0_MODE_SHIFT,
 };
 
 /*
@@ -144,6 +72,7 @@ static int tps6105x_regulator_probe(struct platform_device *pdev)
 	config.dev = &tps6105x->client->dev;
 	config.init_data = pdata->regulator_data;
 	config.driver_data = tps6105x;
+	config.regmap = tps6105x->regmap;
 
 	/* Register regulator with framework */
 	tps6105x->regulator = devm_regulator_register(&pdev->dev,
diff --git a/drivers/regulator/tps65086-regulator.c b/drivers/regulator/tps65086-regulator.c
new file mode 100644
index 000000000000..33f389d583ef
--- /dev/null
+++ b/drivers/regulator/tps65086-regulator.c
@@ -0,0 +1,251 @@
+/*
+ * Copyright (C) 2015 Texas Instruments Incorporated - http://www.ti.com/
+ *
+ * Author: Andrew F. Davis <afd@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 TPS65912 driver
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+
+#include <linux/mfd/tps65086.h>
+
+enum tps65086_regulators { BUCK1, BUCK2, BUCK3, BUCK4, BUCK5, BUCK6, LDOA1,
+	LDOA2, LDOA3, SWA1, SWB1, SWB2, VTT };
+
+#define TPS65086_REGULATOR(_name, _of, _id, _nv, _vr, _vm, _er, _em, _lr, _dr, _dm)	\
+	[_id] = {							\
+		.desc = {						\
+			.name			= _name,		\
+			.of_match		= of_match_ptr(_of),	\
+			.regulators_node	= "regulators",		\
+			.of_parse_cb		= tps65086_of_parse_cb,	\
+			.id			= _id,			\
+			.ops			= &reg_ops,		\
+			.n_voltages		= _nv,			\
+			.type			= REGULATOR_VOLTAGE,	\
+			.owner			= THIS_MODULE,		\
+			.vsel_reg		= _vr,			\
+			.vsel_mask		= _vm,			\
+			.enable_reg		= _er,			\
+			.enable_mask		= _em,			\
+			.volt_table		= NULL,			\
+			.linear_ranges		= _lr,			\
+			.n_linear_ranges	= ARRAY_SIZE(_lr),	\
+		},							\
+		.decay_reg = _dr,					\
+		.decay_mask = _dm,					\
+	}
+
+#define TPS65086_SWITCH(_name, _of, _id, _er, _em)			\
+	[_id] = {							\
+		.desc = {						\
+			.name			= _name,		\
+			.of_match		= of_match_ptr(_of),	\
+			.regulators_node	= "regulators",		\
+			.of_parse_cb		= tps65086_of_parse_cb,	\
+			.id			= _id,			\
+			.ops			= &switch_ops,		\
+			.type			= REGULATOR_VOLTAGE,	\
+			.owner			= THIS_MODULE,		\
+			.enable_reg		= _er,			\
+			.enable_mask		= _em,			\
+		},							\
+	}
+
+struct tps65086_regulator {
+	struct regulator_desc desc;
+	unsigned int decay_reg;
+	unsigned int decay_mask;
+};
+
+static const struct regulator_linear_range tps65086_buck126_10mv_ranges[] = {
+	REGULATOR_LINEAR_RANGE(0, 0x0, 0x0, 0),
+	REGULATOR_LINEAR_RANGE(410000, 0x1, 0x7F, 10000),
+};
+
+static const struct regulator_linear_range tps65086_buck126_25mv_ranges[] = {
+	REGULATOR_LINEAR_RANGE(0, 0x0, 0x0, 0),
+	REGULATOR_LINEAR_RANGE(1000000, 0x1, 0x18, 0),
+	REGULATOR_LINEAR_RANGE(1025000, 0x19, 0x7F, 25000),
+};
+
+static const struct regulator_linear_range tps65086_buck345_ranges[] = {
+	REGULATOR_LINEAR_RANGE(0, 0x0, 0x0, 0),
+	REGULATOR_LINEAR_RANGE(425000, 0x1, 0x7F, 25000),
+};
+
+static const struct regulator_linear_range tps65086_ldoa1_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1350000, 0x0, 0x0, 0),
+	REGULATOR_LINEAR_RANGE(1500000, 0x1, 0x7, 100000),
+	REGULATOR_LINEAR_RANGE(2300000, 0x8, 0xA, 100000),
+	REGULATOR_LINEAR_RANGE(2700000, 0xB, 0xD, 150000),
+	REGULATOR_LINEAR_RANGE(3300000, 0xE, 0xE, 0),
+};
+
+static const struct regulator_linear_range tps65086_ldoa23_ranges[] = {
+	REGULATOR_LINEAR_RANGE(700000, 0x0, 0xD, 50000),
+	REGULATOR_LINEAR_RANGE(1400000, 0xE, 0xF, 100000),
+};
+
+/* Operations permitted on regulators */
+static struct regulator_ops reg_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear_range,
+};
+
+/* Operations permitted on load switches */
+static struct regulator_ops switch_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static int tps65086_of_parse_cb(struct device_node *dev,
+				const struct regulator_desc *desc,
+				struct regulator_config *config);
+
+static struct tps65086_regulator regulators[] = {
+	TPS65086_REGULATOR("BUCK1", "buck1", BUCK1, 0x80, TPS65086_BUCK1CTRL,
+			   BUCK_VID_MASK, TPS65086_BUCK123CTRL, BIT(0),
+			   tps65086_buck126_10mv_ranges, TPS65086_BUCK1CTRL,
+			   BIT(0)),
+	TPS65086_REGULATOR("BUCK2", "buck2", BUCK2, 0x80, TPS65086_BUCK2CTRL,
+			   BUCK_VID_MASK, TPS65086_BUCK123CTRL, BIT(1),
+			   tps65086_buck126_10mv_ranges, TPS65086_BUCK2CTRL,
+			   BIT(0)),
+	TPS65086_REGULATOR("BUCK3", "buck3", BUCK3, 0x80, TPS65086_BUCK3VID,
+			   BUCK_VID_MASK, TPS65086_BUCK123CTRL, BIT(2),
+			   tps65086_buck345_ranges, TPS65086_BUCK3DECAY,
+			   BIT(0)),
+	TPS65086_REGULATOR("BUCK4", "buck4", BUCK4, 0x80, TPS65086_BUCK4VID,
+			   BUCK_VID_MASK, TPS65086_BUCK4CTRL, BIT(0),
+			   tps65086_buck345_ranges, TPS65086_BUCK4VID,
+			   BIT(0)),
+	TPS65086_REGULATOR("BUCK5", "buck5", BUCK5, 0x80, TPS65086_BUCK5VID,
+			   BUCK_VID_MASK, TPS65086_BUCK5CTRL, BIT(0),
+			   tps65086_buck345_ranges, TPS65086_BUCK5CTRL,
+			   BIT(0)),
+	TPS65086_REGULATOR("BUCK6", "buck6", BUCK6, 0x80, TPS65086_BUCK6VID,
+			   BUCK_VID_MASK, TPS65086_BUCK6CTRL, BIT(0),
+			   tps65086_buck126_10mv_ranges, TPS65086_BUCK6CTRL,
+			   BIT(0)),
+	TPS65086_REGULATOR("LDOA1", "ldoa1", LDOA1, 0xF, TPS65086_LDOA1CTRL,
+			   VDOA1_VID_MASK, TPS65086_LDOA1CTRL, BIT(0),
+			   tps65086_ldoa1_ranges, 0, 0),
+	TPS65086_REGULATOR("LDOA2", "ldoa2", LDOA2, 0x10, TPS65086_LDOA2VID,
+			   VDOA23_VID_MASK, TPS65086_LDOA2CTRL, BIT(0),
+			   tps65086_ldoa23_ranges, 0, 0),
+	TPS65086_REGULATOR("LDOA3", "ldoa3", LDOA3, 0x10, TPS65086_LDOA3VID,
+			   VDOA23_VID_MASK, TPS65086_LDOA3CTRL, BIT(0),
+			   tps65086_ldoa23_ranges, 0, 0),
+	TPS65086_SWITCH("SWA1", "swa1", SWA1, TPS65086_SWVTT_EN, BIT(5)),
+	TPS65086_SWITCH("SWB1", "swa2", SWB1, TPS65086_SWVTT_EN, BIT(6)),
+	TPS65086_SWITCH("SWB2", "swa3", SWB2, TPS65086_SWVTT_EN, BIT(7)),
+	TPS65086_SWITCH("VTT", "vtt", VTT, TPS65086_SWVTT_EN, BIT(4)),
+};
+
+static inline bool has_25mv_mode(int id)
+{
+	switch (id) {
+	case BUCK1:
+	case BUCK2:
+	case BUCK6:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static int tps65086_of_parse_cb(struct device_node *dev,
+				const struct regulator_desc *desc,
+				struct regulator_config *config)
+{
+	int ret;
+
+	/* Check for 25mV step mode */
+	if (has_25mv_mode(desc->id) &&
+			of_property_read_bool(config->of_node, "ti,regulator-step-size-25mv")) {
+		regulators[desc->id].desc.linear_ranges =
+				tps65086_buck126_25mv_ranges;
+		regulators[desc->id].desc.n_linear_ranges =
+				ARRAY_SIZE(tps65086_buck126_25mv_ranges);
+	}
+
+	/* Check for decay mode */
+	if (desc->id <= BUCK6 && of_property_read_bool(config->of_node, "ti,regulator-decay")) {
+		ret = regmap_write_bits(config->regmap,
+					regulators[desc->id].decay_reg,
+					regulators[desc->id].decay_mask,
+					regulators[desc->id].decay_mask);
+		if (ret) {
+			dev_err(config->dev, "Error setting decay\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int tps65086_regulator_probe(struct platform_device *pdev)
+{
+	struct tps65086 *tps = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	int i;
+
+	platform_set_drvdata(pdev, tps);
+
+	config.dev = &pdev->dev;
+	config.dev->of_node = tps->dev->of_node;
+	config.driver_data = tps;
+	config.regmap = tps->regmap;
+
+	for (i = 0; i < ARRAY_SIZE(regulators); i++) {
+		rdev = devm_regulator_register(&pdev->dev, &regulators[i].desc,
+					       &config);
+		if (IS_ERR(rdev)) {
+			dev_err(tps->dev, "failed to register %s regulator\n",
+				pdev->name);
+			return PTR_ERR(rdev);
+		}
+	}
+
+	return 0;
+}
+
+static const struct platform_device_id tps65086_regulator_id_table[] = {
+	{ "tps65086-regulator", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, tps65086_regulator_id_table);
+
+static struct platform_driver tps65086_regulator_driver = {
+	.driver = {
+		.name = "tps65086-regulator",
+	},
+	.probe = tps65086_regulator_probe,
+	.id_table = tps65086_regulator_id_table,
+};
+module_platform_driver(tps65086_regulator_driver);
+
+MODULE_AUTHOR("Andrew F. Davis <afd@ti.com>");
+MODULE_DESCRIPTION("TPS65086 Regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/tps65218-regulator.c b/drivers/regulator/tps65218-regulator.c
index a02c1b961039..a5e5634eeb9e 100644
--- a/drivers/regulator/tps65218-regulator.c
+++ b/drivers/regulator/tps65218-regulator.c
@@ -27,19 +27,22 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps65218.h>
 
-enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4, DCDC5, DCDC6, LDO1 };
+enum tps65218_regulators { DCDC1, DCDC2, DCDC3, DCDC4,
+			   DCDC5, DCDC6, LDO1, LS3 };
 
-#define TPS65218_REGULATOR(_name, _id, _ops, _n, _vr, _vm, _er, _em, \
-			    _lr, _nlr, _delay, _fuv)		\
+#define TPS65218_REGULATOR(_name, _id, _type, _ops, _n, _vr, _vm, _er, _em, \
+			    _cr, _cm, _lr, _nlr, _delay, _fuv)		\
 	{							\
 		.name			= _name,		\
 		.id			= _id,			\
 		.ops			= &_ops,		\
 		.n_voltages		= _n,			\
-		.type			= REGULATOR_VOLTAGE,	\
+		.type			= _type,	\
 		.owner			= THIS_MODULE,		\
 		.vsel_reg		= _vr,			\
 		.vsel_mask		= _vm,			\
+		.csel_reg		= _cr,			\
+		.csel_mask		= _cm,			\
 		.enable_reg		= _er,			\
 		.enable_mask		= _em,			\
 		.volt_table		= NULL,			\
@@ -80,6 +83,7 @@ static struct tps_info tps65218_pmic_regs[] = {
 	TPS65218_INFO(DCDC5, "DCDC5", 1000000, 1000000),
 	TPS65218_INFO(DCDC6, "DCDC6", 1800000, 1800000),
 	TPS65218_INFO(LDO1, "LDO1", 900000, 3400000),
+	TPS65218_INFO(LS3, "LS3", -1, -1),
 };
 
 #define TPS65218_OF_MATCH(comp, label) \
@@ -96,6 +100,7 @@ static const struct of_device_id tps65218_of_match[] = {
 	TPS65218_OF_MATCH("ti,tps65218-dcdc5", tps65218_pmic_regs[DCDC5]),
 	TPS65218_OF_MATCH("ti,tps65218-dcdc6", tps65218_pmic_regs[DCDC6]),
 	TPS65218_OF_MATCH("ti,tps65218-ldo1", tps65218_pmic_regs[LDO1]),
+	TPS65218_OF_MATCH("ti,tps65218-ls3", tps65218_pmic_regs[LS3]),
 	{ }
 };
 MODULE_DEVICE_TABLE(of, tps65218_of_match);
@@ -175,6 +180,68 @@ static struct regulator_ops tps65218_ldo1_dcdc34_ops = {
 	.map_voltage		= regulator_map_voltage_linear_range,
 };
 
+static const int ls3_currents[] = { 100, 200, 500, 1000 };
+
+static int tps65218_pmic_set_input_current_lim(struct regulator_dev *dev,
+					       int lim_uA)
+{
+	unsigned int index = 0;
+	unsigned int num_currents = ARRAY_SIZE(ls3_currents);
+	struct tps65218 *tps = rdev_get_drvdata(dev);
+
+	while (index < num_currents && ls3_currents[index] != lim_uA)
+		index++;
+
+	if (index == num_currents)
+		return -EINVAL;
+
+	return tps65218_set_bits(tps, dev->desc->csel_reg, dev->desc->csel_mask,
+				 index << 2, TPS65218_PROTECT_L1);
+}
+
+static int tps65218_pmic_set_current_limit(struct regulator_dev *dev,
+					   int min_uA, int max_uA)
+{
+	int index = 0;
+	unsigned int num_currents = ARRAY_SIZE(ls3_currents);
+	struct tps65218 *tps = rdev_get_drvdata(dev);
+
+	while (index < num_currents && ls3_currents[index] < max_uA)
+		index++;
+
+	index--;
+
+	if (index < 0 || ls3_currents[index] < min_uA)
+		return -EINVAL;
+
+	return tps65218_set_bits(tps, dev->desc->csel_reg, dev->desc->csel_mask,
+				 index << 2, TPS65218_PROTECT_L1);
+}
+
+static int tps65218_pmic_get_current_limit(struct regulator_dev *dev)
+{
+	int retval;
+	unsigned int index;
+	struct tps65218 *tps = rdev_get_drvdata(dev);
+
+	retval = tps65218_reg_read(tps, dev->desc->csel_reg, &index);
+	if (retval < 0)
+		return retval;
+
+	index = (index & dev->desc->csel_mask) >> 2;
+
+	return ls3_currents[index];
+}
+
+static struct regulator_ops tps65218_ls3_ops = {
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= tps65218_pmic_enable,
+	.disable		= tps65218_pmic_disable,
+	.set_input_current_limit = tps65218_pmic_set_input_current_lim,
+	.set_current_limit	= tps65218_pmic_set_current_limit,
+	.get_current_limit	= tps65218_pmic_get_current_limit,
+};
+
 /* Operations permitted on DCDC5, DCDC6 */
 static struct regulator_ops tps65218_dcdc56_pmic_ops = {
 	.is_enabled		= regulator_is_enabled_regmap,
@@ -183,36 +250,46 @@ static struct regulator_ops tps65218_dcdc56_pmic_ops = {
 };
 
 static const struct regulator_desc regulators[] = {
-	TPS65218_REGULATOR("DCDC1", TPS65218_DCDC_1, tps65218_dcdc12_ops, 64,
-			   TPS65218_REG_CONTROL_DCDC1,
-			   TPS65218_CONTROL_DCDC1_MASK,
-			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC1_EN,
-			   dcdc1_dcdc2_ranges, 2, 4000, 0),
-	TPS65218_REGULATOR("DCDC2", TPS65218_DCDC_2, tps65218_dcdc12_ops, 64,
-			   TPS65218_REG_CONTROL_DCDC2,
-			   TPS65218_CONTROL_DCDC2_MASK,
-			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC2_EN,
-			   dcdc1_dcdc2_ranges, 2, 4000, 0),
-	TPS65218_REGULATOR("DCDC3", TPS65218_DCDC_3, tps65218_ldo1_dcdc34_ops,
-			   64, TPS65218_REG_CONTROL_DCDC3,
+	TPS65218_REGULATOR("DCDC1", TPS65218_DCDC_1, REGULATOR_VOLTAGE,
+			   tps65218_dcdc12_ops, 64, TPS65218_REG_CONTROL_DCDC1,
+			   TPS65218_CONTROL_DCDC1_MASK, TPS65218_REG_ENABLE1,
+			   TPS65218_ENABLE1_DC1_EN, 0, 0, dcdc1_dcdc2_ranges,
+			   2, 4000, 0),
+	TPS65218_REGULATOR("DCDC2", TPS65218_DCDC_2, REGULATOR_VOLTAGE,
+			   tps65218_dcdc12_ops, 64, TPS65218_REG_CONTROL_DCDC2,
+			   TPS65218_CONTROL_DCDC2_MASK, TPS65218_REG_ENABLE1,
+			   TPS65218_ENABLE1_DC2_EN, 0, 0, dcdc1_dcdc2_ranges,
+			   2, 4000, 0),
+	TPS65218_REGULATOR("DCDC3", TPS65218_DCDC_3, REGULATOR_VOLTAGE,
+			   tps65218_ldo1_dcdc34_ops, 64,
+			   TPS65218_REG_CONTROL_DCDC3,
 			   TPS65218_CONTROL_DCDC3_MASK, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC3_EN, ldo1_dcdc3_ranges, 2, 0, 0),
-	TPS65218_REGULATOR("DCDC4", TPS65218_DCDC_4, tps65218_ldo1_dcdc34_ops,
-			   53, TPS65218_REG_CONTROL_DCDC4,
-			   TPS65218_CONTROL_DCDC4_MASK,
-			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC4_EN,
-			   dcdc4_ranges, 2, 0, 0),
-	TPS65218_REGULATOR("DCDC5", TPS65218_DCDC_5, tps65218_dcdc56_pmic_ops,
-			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC5_EN, NULL, 0, 0, 1000000),
-	TPS65218_REGULATOR("DCDC6", TPS65218_DCDC_6, tps65218_dcdc56_pmic_ops,
-			   1, -1, -1, TPS65218_REG_ENABLE1,
-			   TPS65218_ENABLE1_DC6_EN, NULL, 0, 0, 1800000),
-	TPS65218_REGULATOR("LDO1", TPS65218_LDO_1, tps65218_ldo1_dcdc34_ops, 64,
+			   TPS65218_ENABLE1_DC3_EN, 0, 0, ldo1_dcdc3_ranges, 2,
+			   0, 0),
+	TPS65218_REGULATOR("DCDC4", TPS65218_DCDC_4, REGULATOR_VOLTAGE,
+			   tps65218_ldo1_dcdc34_ops, 53,
+			   TPS65218_REG_CONTROL_DCDC4,
+			   TPS65218_CONTROL_DCDC4_MASK, TPS65218_REG_ENABLE1,
+			   TPS65218_ENABLE1_DC4_EN, 0, 0, dcdc4_ranges, 2,
+			   0, 0),
+	TPS65218_REGULATOR("DCDC5", TPS65218_DCDC_5, REGULATOR_VOLTAGE,
+			   tps65218_dcdc56_pmic_ops, 1, -1, -1,
+			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC5_EN, 0, 0,
+			   NULL, 0, 0, 1000000),
+	TPS65218_REGULATOR("DCDC6", TPS65218_DCDC_6, REGULATOR_VOLTAGE,
+			   tps65218_dcdc56_pmic_ops, 1, -1, -1,
+			   TPS65218_REG_ENABLE1, TPS65218_ENABLE1_DC6_EN, 0, 0,
+			   NULL, 0, 0, 1800000),
+	TPS65218_REGULATOR("LDO1", TPS65218_LDO_1, REGULATOR_VOLTAGE,
+			   tps65218_ldo1_dcdc34_ops, 64,
 			   TPS65218_REG_CONTROL_LDO1,
 			   TPS65218_CONTROL_LDO1_MASK, TPS65218_REG_ENABLE2,
-			   TPS65218_ENABLE2_LDO1_EN, ldo1_dcdc3_ranges,
+			   TPS65218_ENABLE2_LDO1_EN, 0, 0, ldo1_dcdc3_ranges,
 			   2, 0, 0),
+	TPS65218_REGULATOR("LS3", TPS65218_LS_3, REGULATOR_CURRENT,
+			   tps65218_ls3_ops, 0, 0, 0, TPS65218_REG_ENABLE2,
+			   TPS65218_ENABLE2_LS3_EN, TPS65218_REG_CONFIG2,
+			   TPS65218_CONFIG2_LS3ILIM_MASK, NULL, 0, 0, 0),
 };
 
 static int tps65218_regulator_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/wm831x-dcdc.c b/drivers/regulator/wm831x-dcdc.c
index 8cbb82ceec40..5a5bc4bb08d2 100644
--- a/drivers/regulator/wm831x-dcdc.c
+++ b/drivers/regulator/wm831x-dcdc.c
@@ -365,7 +365,7 @@ static int wm831x_buckv_get_current_limit(struct regulator_dev *rdev)
 	return wm831x_dcdc_ilim[val];
 }
 
-static struct regulator_ops wm831x_buckv_ops = {
+static const struct regulator_ops wm831x_buckv_ops = {
 	.set_voltage_sel = wm831x_buckv_set_voltage_sel,
 	.get_voltage_sel = wm831x_buckv_get_voltage_sel,
 	.list_voltage = wm831x_buckv_list_voltage,
@@ -585,7 +585,7 @@ static int wm831x_buckp_set_suspend_voltage(struct regulator_dev *rdev, int uV)
 	return wm831x_set_bits(wm831x, reg, WM831X_DC3_ON_VSEL_MASK, sel);
 }
 
-static struct regulator_ops wm831x_buckp_ops = {
+static const struct regulator_ops wm831x_buckp_ops = {
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = regulator_list_voltage_linear,
@@ -725,7 +725,7 @@ static int wm831x_boostp_get_status(struct regulator_dev *rdev)
 		return REGULATOR_STATUS_OFF;
 }
 
-static struct regulator_ops wm831x_boostp_ops = {
+static const struct regulator_ops wm831x_boostp_ops = {
 	.get_status = wm831x_boostp_get_status,
 
 	.is_enabled = regulator_is_enabled_regmap,
@@ -818,7 +818,7 @@ static struct platform_driver wm831x_boostp_driver = {
 
 #define WM831X_EPE_BASE 6
 
-static struct regulator_ops wm831x_epe_ops = {
+static const struct regulator_ops wm831x_epe_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
@@ -884,35 +884,22 @@ static struct platform_driver wm831x_epe_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&wm831x_buckv_driver,
+	&wm831x_buckp_driver,
+	&wm831x_boostp_driver,
+	&wm831x_epe_driver,
+};
+
 static int __init wm831x_dcdc_init(void)
 {
-	int ret;
-	ret = platform_driver_register(&wm831x_buckv_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x BUCKV driver: %d\n", ret);
-
-	ret = platform_driver_register(&wm831x_buckp_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x BUCKP driver: %d\n", ret);
-
-	ret = platform_driver_register(&wm831x_boostp_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x BOOST driver: %d\n", ret);
-
-	ret = platform_driver_register(&wm831x_epe_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x EPE driver: %d\n", ret);
-
-	return 0;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 subsys_initcall(wm831x_dcdc_init);
 
 static void __exit wm831x_dcdc_exit(void)
 {
-	platform_driver_unregister(&wm831x_epe_driver);
-	platform_driver_unregister(&wm831x_boostp_driver);
-	platform_driver_unregister(&wm831x_buckp_driver);
-	platform_driver_unregister(&wm831x_buckv_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(wm831x_dcdc_exit);
 
diff --git a/drivers/regulator/wm831x-isink.c b/drivers/regulator/wm831x-isink.c
index 1442828fcd9a..6dd891d7eee3 100644
--- a/drivers/regulator/wm831x-isink.c
+++ b/drivers/regulator/wm831x-isink.c
@@ -128,7 +128,7 @@ static int wm831x_isink_get_current(struct regulator_dev *rdev)
 	return wm831x_isinkv_values[ret];
 }
 
-static struct regulator_ops wm831x_isink_ops = {
+static const struct regulator_ops wm831x_isink_ops = {
 	.is_enabled = wm831x_isink_is_enabled,
 	.enable = wm831x_isink_enable,
 	.disable = wm831x_isink_disable,
diff --git a/drivers/regulator/wm831x-ldo.c b/drivers/regulator/wm831x-ldo.c
index 5a7b65e8a529..e4a6f888484e 100644
--- a/drivers/regulator/wm831x-ldo.c
+++ b/drivers/regulator/wm831x-ldo.c
@@ -198,7 +198,7 @@ static unsigned int wm831x_gp_ldo_get_optimum_mode(struct regulator_dev *rdev,
 }
 
 
-static struct regulator_ops wm831x_gp_ldo_ops = {
+static const struct regulator_ops wm831x_gp_ldo_ops = {
 	.list_voltage = regulator_list_voltage_linear_range,
 	.map_voltage = regulator_map_voltage_linear_range,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -409,7 +409,7 @@ static int wm831x_aldo_get_status(struct regulator_dev *rdev)
 		return regulator_mode_to_status(ret);
 }
 
-static struct regulator_ops wm831x_aldo_ops = {
+static const struct regulator_ops wm831x_aldo_ops = {
 	.list_voltage = regulator_list_voltage_linear_range,
 	.map_voltage = regulator_map_voltage_linear_range,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -557,7 +557,7 @@ static int wm831x_alive_ldo_get_status(struct regulator_dev *rdev)
 		return REGULATOR_STATUS_OFF;
 }
 
-static struct regulator_ops wm831x_alive_ldo_ops = {
+static const struct regulator_ops wm831x_alive_ldo_ops = {
 	.list_voltage = regulator_list_voltage_linear,
 	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -653,32 +653,21 @@ static struct platform_driver wm831x_alive_ldo_driver = {
 	},
 };
 
+static struct platform_driver * const drivers[] = {
+	&wm831x_gp_ldo_driver,
+	&wm831x_aldo_driver,
+	&wm831x_alive_ldo_driver,
+};
+
 static int __init wm831x_ldo_init(void)
 {
-	int ret;
-
-	ret = platform_driver_register(&wm831x_gp_ldo_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x GP LDO driver: %d\n", ret);
-
-	ret = platform_driver_register(&wm831x_aldo_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x ALDO driver: %d\n", ret);
-
-	ret = platform_driver_register(&wm831x_alive_ldo_driver);
-	if (ret != 0)
-		pr_err("Failed to register WM831x alive LDO driver: %d\n",
-		       ret);
-
-	return 0;
+	return platform_register_drivers(drivers, ARRAY_SIZE(drivers));
 }
 subsys_initcall(wm831x_ldo_init);
 
 static void __exit wm831x_ldo_exit(void)
 {
-	platform_driver_unregister(&wm831x_alive_ldo_driver);
-	platform_driver_unregister(&wm831x_aldo_driver);
-	platform_driver_unregister(&wm831x_gp_ldo_driver);
+	platform_unregister_drivers(drivers, ARRAY_SIZE(drivers));
 }
 module_exit(wm831x_ldo_exit);
 
diff --git a/drivers/regulator/wm8350-regulator.c b/drivers/regulator/wm8350-regulator.c
index 95f6b040186e..da9106bd2109 100644
--- a/drivers/regulator/wm8350-regulator.c
+++ b/drivers/regulator/wm8350-regulator.c
@@ -941,7 +941,7 @@ static unsigned int wm8350_dcdc_get_optimum_mode(struct regulator_dev *rdev,
 	return mode;
 }
 
-static struct regulator_ops wm8350_dcdc_ops = {
+static const struct regulator_ops wm8350_dcdc_ops = {
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.list_voltage = regulator_list_voltage_linear,
@@ -958,7 +958,7 @@ static struct regulator_ops wm8350_dcdc_ops = {
 	.set_suspend_mode = wm8350_dcdc_set_suspend_mode,
 };
 
-static struct regulator_ops wm8350_dcdc2_5_ops = {
+static const struct regulator_ops wm8350_dcdc2_5_ops = {
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
 	.is_enabled = regulator_is_enabled_regmap,
@@ -966,7 +966,7 @@ static struct regulator_ops wm8350_dcdc2_5_ops = {
 	.set_suspend_disable = wm8350_dcdc25_set_suspend_disable,
 };
 
-static struct regulator_ops wm8350_ldo_ops = {
+static const struct regulator_ops wm8350_ldo_ops = {
 	.map_voltage = regulator_map_voltage_linear_range,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -980,7 +980,7 @@ static struct regulator_ops wm8350_ldo_ops = {
 	.set_suspend_disable = wm8350_ldo_set_suspend_disable,
 };
 
-static struct regulator_ops wm8350_isink_ops = {
+static const struct regulator_ops wm8350_isink_ops = {
 	.set_current_limit = wm8350_isink_set_current,
 	.get_current_limit = wm8350_isink_get_current,
 	.enable = wm8350_isink_enable,
diff --git a/drivers/regulator/wm8400-regulator.c b/drivers/regulator/wm8400-regulator.c
index 82d829000851..fb1837657b64 100644
--- a/drivers/regulator/wm8400-regulator.c
+++ b/drivers/regulator/wm8400-regulator.c
@@ -24,7 +24,7 @@ static const struct regulator_linear_range wm8400_ldo_ranges[] = {
 	REGULATOR_LINEAR_RANGE(1700000, 15, 31, 100000),
 };
 
-static struct regulator_ops wm8400_ldo_ops = {
+static const struct regulator_ops wm8400_ldo_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
@@ -106,7 +106,7 @@ static unsigned int wm8400_dcdc_get_optimum_mode(struct regulator_dev *dev,
 	return REGULATOR_MODE_NORMAL;
 }
 
-static struct regulator_ops wm8400_dcdc_ops = {
+static const struct regulator_ops wm8400_dcdc_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.enable = regulator_enable_regmap,
 	.disable = regulator_disable_regmap,
diff --git a/drivers/regulator/wm8994-regulator.c b/drivers/regulator/wm8994-regulator.c
index 750e0bd61f9f..7a4ce6df4f22 100644
--- a/drivers/regulator/wm8994-regulator.c
+++ b/drivers/regulator/wm8994-regulator.c
@@ -36,7 +36,7 @@ struct wm8994_ldo {
 #define WM8994_LDO1_MAX_SELECTOR 0x7
 #define WM8994_LDO2_MAX_SELECTOR 0x3
 
-static struct regulator_ops wm8994_ldo1_ops = {
+static const struct regulator_ops wm8994_ldo1_ops = {
 	.list_voltage = regulator_list_voltage_linear,
 	.map_voltage = regulator_map_voltage_linear,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -69,7 +69,7 @@ static int wm8994_ldo2_list_voltage(struct regulator_dev *rdev,
 	}
 }
 
-static struct regulator_ops wm8994_ldo2_ops = {
+static const struct regulator_ops wm8994_ldo2_ops = {
 	.list_voltage = wm8994_ldo2_list_voltage,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
diff --git a/drivers/remoteproc/remoteproc_virtio.c b/drivers/remoteproc/remoteproc_virtio.c
index e1a10232a943..e44872fb9e5e 100644
--- a/drivers/remoteproc/remoteproc_virtio.c
+++ b/drivers/remoteproc/remoteproc_virtio.c
@@ -147,7 +147,7 @@ static void rproc_virtio_del_vqs(struct virtio_device *vdev)
 static int rproc_virtio_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 		       struct virtqueue *vqs[],
 		       vq_callback_t *callbacks[],
-		       const char *names[])
+		       const char * const names[])
 {
 	struct rproc *rproc = vdev_to_rproc(vdev);
 	int i, ret;
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index 0615f50a14cd..df37212a5cbd 100644
--- a/drivers/reset/Kconfig
+++ b/drivers/reset/Kconfig
@@ -13,3 +13,4 @@ menuconfig RESET_CONTROLLER
 	  If unsure, say no.
 
 source "drivers/reset/sti/Kconfig"
+source "drivers/reset/hisilicon/Kconfig"
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index 85d5904e5480..4d7178e46afa 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -1,8 +1,9 @@
-obj-$(CONFIG_RESET_CONTROLLER) += core.o
+obj-y += core.o
 obj-$(CONFIG_ARCH_LPC18XX) += reset-lpc18xx.o
 obj-$(CONFIG_ARCH_SOCFPGA) += reset-socfpga.o
 obj-$(CONFIG_ARCH_BERLIN) += reset-berlin.o
 obj-$(CONFIG_ARCH_SUNXI) += reset-sunxi.o
 obj-$(CONFIG_ARCH_STI) += sti/
+obj-$(CONFIG_ARCH_HISI) += hisilicon/
 obj-$(CONFIG_ARCH_ZYNQ) += reset-zynq.o
 obj-$(CONFIG_ATH79) += reset-ath79.o
diff --git a/drivers/reset/core.c b/drivers/reset/core.c
index 7955e00d04d4..87376638948d 100644
--- a/drivers/reset/core.c
+++ b/drivers/reset/core.c
@@ -30,7 +30,6 @@ static LIST_HEAD(reset_controller_list);
  */
 struct reset_control {
 	struct reset_controller_dev *rcdev;
-	struct device *dev;
 	unsigned int id;
 };
 
@@ -95,7 +94,7 @@ int reset_control_reset(struct reset_control *rstc)
 	if (rstc->rcdev->ops->reset)
 		return rstc->rcdev->ops->reset(rstc->rcdev, rstc->id);
 
-	return -ENOSYS;
+	return -ENOTSUPP;
 }
 EXPORT_SYMBOL_GPL(reset_control_reset);
 
@@ -108,7 +107,7 @@ int reset_control_assert(struct reset_control *rstc)
 	if (rstc->rcdev->ops->assert)
 		return rstc->rcdev->ops->assert(rstc->rcdev, rstc->id);
 
-	return -ENOSYS;
+	return -ENOTSUPP;
 }
 EXPORT_SYMBOL_GPL(reset_control_assert);
 
@@ -121,7 +120,7 @@ int reset_control_deassert(struct reset_control *rstc)
 	if (rstc->rcdev->ops->deassert)
 		return rstc->rcdev->ops->deassert(rstc->rcdev, rstc->id);
 
-	return -ENOSYS;
+	return -ENOTSUPP;
 }
 EXPORT_SYMBOL_GPL(reset_control_deassert);
 
@@ -136,32 +135,29 @@ int reset_control_status(struct reset_control *rstc)
 	if (rstc->rcdev->ops->status)
 		return rstc->rcdev->ops->status(rstc->rcdev, rstc->id);
 
-	return -ENOSYS;
+	return -ENOTSUPP;
 }
 EXPORT_SYMBOL_GPL(reset_control_status);
 
 /**
- * of_reset_control_get - Lookup and obtain a reference to a reset controller.
+ * of_reset_control_get_by_index - Lookup and obtain a reference to a reset
+ * controller by index.
  * @node: device to be reset by the controller
- * @id: reset line name
+ * @index: index of the reset controller
  *
- * Returns a struct reset_control or IS_ERR() condition containing errno.
- *
- * Use of id names is optional.
+ * This is to be used to perform a list of resets for a device or power domain
+ * in whatever order. Returns a struct reset_control or IS_ERR() condition
+ * containing errno.
  */
-struct reset_control *of_reset_control_get(struct device_node *node,
-					   const char *id)
+struct reset_control *of_reset_control_get_by_index(struct device_node *node,
+					   int index)
 {
 	struct reset_control *rstc = ERR_PTR(-EPROBE_DEFER);
 	struct reset_controller_dev *r, *rcdev;
 	struct of_phandle_args args;
-	int index = 0;
 	int rstc_id;
 	int ret;
 
-	if (id)
-		index = of_property_match_string(node,
-						 "reset-names", id);
 	ret = of_parse_phandle_with_args(node, "resets", "#reset-cells",
 					 index, &args);
 	if (ret)
@@ -202,6 +198,30 @@ struct reset_control *of_reset_control_get(struct device_node *node,
 
 	return rstc;
 }
+EXPORT_SYMBOL_GPL(of_reset_control_get_by_index);
+
+/**
+ * of_reset_control_get - Lookup and obtain a reference to a reset controller.
+ * @node: device to be reset by the controller
+ * @id: reset line name
+ *
+ * Returns a struct reset_control or IS_ERR() condition containing errno.
+ *
+ * Use of id names is optional.
+ */
+struct reset_control *of_reset_control_get(struct device_node *node,
+					   const char *id)
+{
+	int index = 0;
+
+	if (id) {
+		index = of_property_match_string(node,
+						 "reset-names", id);
+		if (index < 0)
+			return ERR_PTR(-ENOENT);
+	}
+	return of_reset_control_get_by_index(node, index);
+}
 EXPORT_SYMBOL_GPL(of_reset_control_get);
 
 /**
@@ -215,16 +235,10 @@ EXPORT_SYMBOL_GPL(of_reset_control_get);
  */
 struct reset_control *reset_control_get(struct device *dev, const char *id)
 {
-	struct reset_control *rstc;
-
 	if (!dev)
 		return ERR_PTR(-EINVAL);
 
-	rstc = of_reset_control_get(dev->of_node, id);
-	if (!IS_ERR(rstc))
-		rstc->dev = dev;
-
-	return rstc;
+	return of_reset_control_get(dev->of_node, id);
 }
 EXPORT_SYMBOL_GPL(reset_control_get);
 
diff --git a/drivers/reset/hisilicon/Kconfig b/drivers/reset/hisilicon/Kconfig
new file mode 100644
index 000000000000..26bf95a83a8e
--- /dev/null
+++ b/drivers/reset/hisilicon/Kconfig
@@ -0,0 +1,5 @@
+config COMMON_RESET_HI6220
+	tristate "Hi6220 Reset Driver"
+	depends on (ARCH_HISI && RESET_CONTROLLER)
+	help
+	  Build the Hisilicon Hi6220 reset driver.
diff --git a/drivers/reset/hisilicon/Makefile b/drivers/reset/hisilicon/Makefile
new file mode 100644
index 000000000000..c932f86e2f10
--- /dev/null
+++ b/drivers/reset/hisilicon/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_COMMON_RESET_HI6220) += hi6220_reset.o
diff --git a/drivers/reset/hisilicon/hi6220_reset.c b/drivers/reset/hisilicon/hi6220_reset.c
new file mode 100644
index 000000000000..7787a9b1cc67
--- /dev/null
+++ b/drivers/reset/hisilicon/hi6220_reset.c
@@ -0,0 +1,109 @@
+/*
+ * Hisilicon Hi6220 reset controller driver
+ *
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * Author: Feng Chen <puck.chen@hisilicon.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/io.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/bitops.h>
+#include <linux/of.h>
+#include <linux/reset-controller.h>
+#include <linux/reset.h>
+#include <linux/platform_device.h>
+
+#define ASSERT_OFFSET            0x300
+#define DEASSERT_OFFSET          0x304
+#define MAX_INDEX                0x509
+
+#define to_reset_data(x) container_of(x, struct hi6220_reset_data, rc_dev)
+
+struct hi6220_reset_data {
+	void __iomem			*assert_base;
+	void __iomem			*deassert_base;
+	struct reset_controller_dev	rc_dev;
+};
+
+static int hi6220_reset_assert(struct reset_controller_dev *rc_dev,
+			       unsigned long idx)
+{
+	struct hi6220_reset_data *data = to_reset_data(rc_dev);
+
+	int bank = idx >> 8;
+	int offset = idx & 0xff;
+
+	writel(BIT(offset), data->assert_base + (bank * 0x10));
+
+	return 0;
+}
+
+static int hi6220_reset_deassert(struct reset_controller_dev *rc_dev,
+				 unsigned long idx)
+{
+	struct hi6220_reset_data *data = to_reset_data(rc_dev);
+
+	int bank = idx >> 8;
+	int offset = idx & 0xff;
+
+	writel(BIT(offset), data->deassert_base + (bank * 0x10));
+
+	return 0;
+}
+
+static struct reset_control_ops hi6220_reset_ops = {
+	.assert = hi6220_reset_assert,
+	.deassert = hi6220_reset_deassert,
+};
+
+static int hi6220_reset_probe(struct platform_device *pdev)
+{
+	struct hi6220_reset_data *data;
+	struct resource *res;
+	void __iomem *src_base;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	src_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(src_base))
+		return PTR_ERR(src_base);
+
+	data->assert_base = src_base + ASSERT_OFFSET;
+	data->deassert_base = src_base + DEASSERT_OFFSET;
+	data->rc_dev.nr_resets = MAX_INDEX;
+	data->rc_dev.ops = &hi6220_reset_ops;
+	data->rc_dev.of_node = pdev->dev.of_node;
+
+	reset_controller_register(&data->rc_dev);
+
+	return 0;
+}
+
+static const struct of_device_id hi6220_reset_match[] = {
+	{ .compatible = "hisilicon,hi6220-sysctrl" },
+	{ },
+};
+
+static struct platform_driver hi6220_reset_driver = {
+	.probe = hi6220_reset_probe,
+	.driver = {
+		.name = "reset-hi6220",
+		.of_match_table = hi6220_reset_match,
+	},
+};
+
+static int __init hi6220_reset_init(void)
+{
+	return platform_driver_register(&hi6220_reset_driver);
+}
+
+postcore_initcall(hi6220_reset_init);
diff --git a/drivers/reset/reset-ath79.c b/drivers/reset/reset-ath79.c
index 9aaf646ece55..692fc890e94b 100644
--- a/drivers/reset/reset-ath79.c
+++ b/drivers/reset/reset-ath79.c
@@ -15,13 +15,17 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/reset-controller.h>
+#include <linux/reboot.h>
 
 struct ath79_reset {
 	struct reset_controller_dev rcdev;
+	struct notifier_block restart_nb;
 	void __iomem *base;
 	spinlock_t lock;
 };
 
+#define FULL_CHIP_RESET 24
+
 static int ath79_reset_update(struct reset_controller_dev *rcdev,
 			unsigned long id, bool assert)
 {
@@ -72,10 +76,22 @@ static struct reset_control_ops ath79_reset_ops = {
 	.status = ath79_reset_status,
 };
 
+static int ath79_reset_restart_handler(struct notifier_block *nb,
+				unsigned long action, void *data)
+{
+	struct ath79_reset *ath79_reset =
+		container_of(nb, struct ath79_reset, restart_nb);
+
+	ath79_reset_assert(&ath79_reset->rcdev, FULL_CHIP_RESET);
+
+	return NOTIFY_DONE;
+}
+
 static int ath79_reset_probe(struct platform_device *pdev)
 {
 	struct ath79_reset *ath79_reset;
 	struct resource *res;
+	int err;
 
 	ath79_reset = devm_kzalloc(&pdev->dev,
 				sizeof(*ath79_reset), GFP_KERNEL);
@@ -96,13 +112,25 @@ static int ath79_reset_probe(struct platform_device *pdev)
 	ath79_reset->rcdev.of_reset_n_cells = 1;
 	ath79_reset->rcdev.nr_resets = 32;
 
-	return reset_controller_register(&ath79_reset->rcdev);
+	err = reset_controller_register(&ath79_reset->rcdev);
+	if (err)
+		return err;
+
+	ath79_reset->restart_nb.notifier_call = ath79_reset_restart_handler;
+	ath79_reset->restart_nb.priority = 128;
+
+	err = register_restart_handler(&ath79_reset->restart_nb);
+	if (err)
+		dev_warn(&pdev->dev, "Failed to register restart handler\n");
+
+	return 0;
 }
 
 static int ath79_reset_remove(struct platform_device *pdev)
 {
 	struct ath79_reset *ath79_reset = platform_get_drvdata(pdev);
 
+	unregister_restart_handler(&ath79_reset->restart_nb);
 	reset_controller_unregister(&ath79_reset->rcdev);
 
 	return 0;
diff --git a/drivers/reset/reset-berlin.c b/drivers/reset/reset-berlin.c
index 3c922d37255c..970b1ad60293 100644
--- a/drivers/reset/reset-berlin.c
+++ b/drivers/reset/reset-berlin.c
@@ -87,9 +87,7 @@ static int berlin2_reset_probe(struct platform_device *pdev)
 	priv->rcdev.of_reset_n_cells = 2;
 	priv->rcdev.of_xlate = berlin_reset_xlate;
 
-	reset_controller_register(&priv->rcdev);
-
-	return 0;
+	return reset_controller_register(&priv->rcdev);
 }
 
 static const struct of_device_id berlin_reset_dt_match[] = {
diff --git a/drivers/reset/reset-socfpga.c b/drivers/reset/reset-socfpga.c
index 1a6c5d66c83b..b7d773d9248c 100644
--- a/drivers/reset/reset-socfpga.c
+++ b/drivers/reset/reset-socfpga.c
@@ -133,9 +133,8 @@ static int socfpga_reset_probe(struct platform_device *pdev)
 	data->rcdev.nr_resets = NR_BANKS * BITS_PER_LONG;
 	data->rcdev.ops = &socfpga_reset_ops;
 	data->rcdev.of_node = pdev->dev.of_node;
-	reset_controller_register(&data->rcdev);
 
-	return 0;
+	return reset_controller_register(&data->rcdev);
 }
 
 static int socfpga_reset_remove(struct platform_device *pdev)
diff --git a/drivers/reset/reset-sunxi.c b/drivers/reset/reset-sunxi.c
index 3d95c87160b3..8d41a18da17f 100644
--- a/drivers/reset/reset-sunxi.c
+++ b/drivers/reset/reset-sunxi.c
@@ -108,9 +108,8 @@ static int sunxi_reset_init(struct device_node *np)
 	data->rcdev.nr_resets = size * 32;
 	data->rcdev.ops = &sunxi_reset_ops;
 	data->rcdev.of_node = np;
-	reset_controller_register(&data->rcdev);
 
-	return 0;
+	return reset_controller_register(&data->rcdev);
 
 err_alloc:
 	kfree(data);
@@ -122,7 +121,7 @@ err_alloc:
  * our system, before we can even think of using a regular device
  * driver for it.
  */
-static const struct of_device_id sunxi_early_reset_dt_ids[] __initdata = {
+static const struct of_device_id sunxi_early_reset_dt_ids[] __initconst = {
 	{ .compatible = "allwinner,sun6i-a31-ahb1-reset", },
 	{ /* sentinel */ },
 };
diff --git a/drivers/reset/reset-zynq.c b/drivers/reset/reset-zynq.c
index 89318a5d5bd7..c6b3cd8b40ad 100644
--- a/drivers/reset/reset-zynq.c
+++ b/drivers/reset/reset-zynq.c
@@ -121,9 +121,8 @@ static int zynq_reset_probe(struct platform_device *pdev)
 	priv->rcdev.nr_resets = resource_size(res) / 4 * BITS_PER_LONG;
 	priv->rcdev.ops = &zynq_reset_ops;
 	priv->rcdev.of_node = pdev->dev.of_node;
-	reset_controller_register(&priv->rcdev);
 
-	return 0;
+	return reset_controller_register(&priv->rcdev);
 }
 
 static int zynq_reset_remove(struct platform_device *pdev)
diff --git a/drivers/reset/sti/reset-stih407.c b/drivers/reset/sti/reset-stih407.c
index 827eb3dae47d..6fb22af990c0 100644
--- a/drivers/reset/sti/reset-stih407.c
+++ b/drivers/reset/sti/reset-stih407.c
@@ -52,6 +52,7 @@ static const struct syscfg_reset_channel_data stih407_powerdowns[] = {
 };
 
 /* Reset Generator control 0/1 */
+#define SYSCFG_5128	0x200
 #define SYSCFG_5131	0x20c
 #define SYSCFG_5132	0x210
 
@@ -96,6 +97,10 @@ static const struct syscfg_reset_channel_data stih407_softresets[] = {
 	[STIH407_ERAM_HVA_SOFTRESET] = STIH407_SRST_CORE(SYSCFG_5132, 1),
 	[STIH407_LPM_SOFTRESET] = STIH407_SRST_SBC(SYSCFG_4002, 2),
 	[STIH407_KEYSCAN_SOFTRESET] = STIH407_SRST_LPM(LPM_SYSCFG_1, 8),
+	[STIH407_ST231_AUD_SOFTRESET] = STIH407_SRST_CORE(SYSCFG_5131, 26),
+	[STIH407_ST231_DMU_SOFTRESET] = STIH407_SRST_CORE(SYSCFG_5131, 27),
+	[STIH407_ST231_GP0_SOFTRESET] = STIH407_SRST_CORE(SYSCFG_5131, 28),
+	[STIH407_ST231_GP1_SOFTRESET] = STIH407_SRST_CORE(SYSCFG_5128, 2),
 };
 
 /* PicoPHY reset/control */
diff --git a/drivers/reset/sti/reset-syscfg.c b/drivers/reset/sti/reset-syscfg.c
index a145cc066d4a..1600cc7557f5 100644
--- a/drivers/reset/sti/reset-syscfg.c
+++ b/drivers/reset/sti/reset-syscfg.c
@@ -103,17 +103,42 @@ static int syscfg_reset_deassert(struct reset_controller_dev *rcdev,
 static int syscfg_reset_dev(struct reset_controller_dev *rcdev,
 			    unsigned long idx)
 {
-	int err = syscfg_reset_assert(rcdev, idx);
+	int err;
+
+	err = syscfg_reset_assert(rcdev, idx);
 	if (err)
 		return err;
 
 	return syscfg_reset_deassert(rcdev, idx);
 }
 
+static int syscfg_reset_status(struct reset_controller_dev *rcdev,
+			       unsigned long idx)
+{
+	struct syscfg_reset_controller *rst = to_syscfg_reset_controller(rcdev);
+	const struct syscfg_reset_channel *ch;
+	u32 ret_val = 0;
+	int err;
+
+	if (idx >= rcdev->nr_resets)
+		return -EINVAL;
+
+	ch = &rst->channels[idx];
+	if (ch->ack)
+		err = regmap_field_read(ch->ack, &ret_val);
+	else
+		err = regmap_field_read(ch->reset, &ret_val);
+	if (err)
+		return err;
+
+	return rst->active_low ? !ret_val : !!ret_val;
+}
+
 static struct reset_control_ops syscfg_reset_ops = {
 	.reset    = syscfg_reset_dev,
 	.assert   = syscfg_reset_assert,
 	.deassert = syscfg_reset_deassert,
+	.status   = syscfg_reset_status,
 };
 
 static int syscfg_reset_controller_register(struct device *dev,
diff --git a/drivers/rpmsg/virtio_rpmsg_bus.c b/drivers/rpmsg/virtio_rpmsg_bus.c
index 73354ee27877..1fcd27c1f183 100644
--- a/drivers/rpmsg/virtio_rpmsg_bus.c
+++ b/drivers/rpmsg/virtio_rpmsg_bus.c
@@ -945,7 +945,7 @@ static void rpmsg_ns_cb(struct rpmsg_channel *rpdev, void *data, int len,
 static int rpmsg_probe(struct virtio_device *vdev)
 {
 	vq_callback_t *vq_cbs[] = { rpmsg_recv_done, rpmsg_xmit_done };
-	const char *names[] = { "input", "output" };
+	static const char * const names[] = { "input", "output" };
 	struct virtqueue *vqs[2];
 	struct virtproc_info *vrp;
 	void *bufs_va;
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index 2a524244afec..376322f71fd5 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -558,6 +558,16 @@ config RTC_DRV_FM3130
 	  This driver can also be built as a module. If so the module
 	  will be called rtc-fm3130.
 
+config RTC_DRV_RX8010
+	tristate "Epson RX8010SJ"
+	depends on I2C
+	help
+	  If you say yes here you get support for the Epson RX8010SJ RTC
+	  chip.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-rx8010.
+
 config RTC_DRV_RX8581
 	tristate "Epson RX-8581"
 	help
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 231f76451615..62d61b26ca7e 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -128,6 +128,7 @@ obj-$(CONFIG_RTC_DRV_RS5C372)	+= rtc-rs5c372.o
 obj-$(CONFIG_RTC_DRV_RV3029C2)	+= rtc-rv3029c2.o
 obj-$(CONFIG_RTC_DRV_RV8803)	+= rtc-rv8803.o
 obj-$(CONFIG_RTC_DRV_RX4581)	+= rtc-rx4581.o
+obj-$(CONFIG_RTC_DRV_RX8010)	+= rtc-rx8010.o
 obj-$(CONFIG_RTC_DRV_RX8025)	+= rtc-rx8025.o
 obj-$(CONFIG_RTC_DRV_RX8581)	+= rtc-rx8581.o
 obj-$(CONFIG_RTC_DRV_S35390A)	+= rtc-s35390a.o
diff --git a/drivers/rtc/rtc-abx80x.c b/drivers/rtc/rtc-abx80x.c
index afea84c7a155..d41bbcd653f6 100644
--- a/drivers/rtc/rtc-abx80x.c
+++ b/drivers/rtc/rtc-abx80x.c
@@ -27,10 +27,28 @@
 #define ABX8XX_REG_YR		0x06
 #define ABX8XX_REG_WD		0x07
 
+#define ABX8XX_REG_AHTH		0x08
+#define ABX8XX_REG_ASC		0x09
+#define ABX8XX_REG_AMN		0x0a
+#define ABX8XX_REG_AHR		0x0b
+#define ABX8XX_REG_ADA		0x0c
+#define ABX8XX_REG_AMO		0x0d
+#define ABX8XX_REG_AWD		0x0e
+
+#define ABX8XX_REG_STATUS	0x0f
+#define ABX8XX_STATUS_AF	BIT(2)
+
 #define ABX8XX_REG_CTRL1	0x10
 #define ABX8XX_CTRL_WRITE	BIT(0)
+#define ABX8XX_CTRL_ARST	BIT(2)
 #define ABX8XX_CTRL_12_24	BIT(6)
 
+#define ABX8XX_REG_IRQ		0x12
+#define ABX8XX_IRQ_AIE		BIT(2)
+#define ABX8XX_IRQ_IM_1_4	(0x3 << 5)
+
+#define ABX8XX_REG_CD_TIMER_CTL	0x18
+
 #define ABX8XX_REG_CFG_KEY	0x1f
 #define ABX8XX_CFG_KEY_MISC	0x9d
 
@@ -63,8 +81,6 @@ static struct abx80x_cap abx80x_caps[] = {
 	[ABX80X] = {.pn = 0}
 };
 
-static struct i2c_driver abx80x_driver;
-
 static int abx80x_enable_trickle_charger(struct i2c_client *client,
 					 u8 trickle_cfg)
 {
@@ -148,9 +164,111 @@ static int abx80x_rtc_set_time(struct device *dev, struct rtc_time *tm)
 	return 0;
 }
 
+static irqreturn_t abx80x_handle_irq(int irq, void *dev_id)
+{
+	struct i2c_client *client = dev_id;
+	struct rtc_device *rtc = i2c_get_clientdata(client);
+	int status;
+
+	status = i2c_smbus_read_byte_data(client, ABX8XX_REG_STATUS);
+	if (status < 0)
+		return IRQ_NONE;
+
+	if (status & ABX8XX_STATUS_AF)
+		rtc_update_irq(rtc, 1, RTC_AF | RTC_IRQF);
+
+	i2c_smbus_write_byte_data(client, ABX8XX_REG_STATUS, 0);
+
+	return IRQ_HANDLED;
+}
+
+static int abx80x_read_alarm(struct device *dev, struct rtc_wkalrm *t)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	unsigned char buf[7];
+
+	int irq_mask, err;
+
+	if (client->irq <= 0)
+		return -EINVAL;
+
+	err = i2c_smbus_read_i2c_block_data(client, ABX8XX_REG_ASC,
+					    sizeof(buf), buf);
+	if (err)
+		return err;
+
+	irq_mask = i2c_smbus_read_byte_data(client, ABX8XX_REG_IRQ);
+	if (irq_mask < 0)
+		return irq_mask;
+
+	t->time.tm_sec = bcd2bin(buf[0] & 0x7F);
+	t->time.tm_min = bcd2bin(buf[1] & 0x7F);
+	t->time.tm_hour = bcd2bin(buf[2] & 0x3F);
+	t->time.tm_mday = bcd2bin(buf[3] & 0x3F);
+	t->time.tm_mon = bcd2bin(buf[4] & 0x1F) - 1;
+	t->time.tm_wday = buf[5] & 0x7;
+
+	t->enabled = !!(irq_mask & ABX8XX_IRQ_AIE);
+	t->pending = (buf[6] & ABX8XX_STATUS_AF) && t->enabled;
+
+	return err;
+}
+
+static int abx80x_set_alarm(struct device *dev, struct rtc_wkalrm *t)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	u8 alarm[6];
+	int err;
+
+	if (client->irq <= 0)
+		return -EINVAL;
+
+	alarm[0] = 0x0;
+	alarm[1] = bin2bcd(t->time.tm_sec);
+	alarm[2] = bin2bcd(t->time.tm_min);
+	alarm[3] = bin2bcd(t->time.tm_hour);
+	alarm[4] = bin2bcd(t->time.tm_mday);
+	alarm[5] = bin2bcd(t->time.tm_mon + 1);
+
+	err = i2c_smbus_write_i2c_block_data(client, ABX8XX_REG_AHTH,
+					     sizeof(alarm), alarm);
+	if (err < 0) {
+		dev_err(&client->dev, "Unable to write alarm registers\n");
+		return -EIO;
+	}
+
+	if (t->enabled) {
+		err = i2c_smbus_write_byte_data(client, ABX8XX_REG_IRQ,
+						(ABX8XX_IRQ_IM_1_4 |
+						 ABX8XX_IRQ_AIE));
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int abx80x_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	int err;
+
+	if (enabled)
+		err = i2c_smbus_write_byte_data(client, ABX8XX_REG_IRQ,
+						(ABX8XX_IRQ_IM_1_4 |
+						 ABX8XX_IRQ_AIE));
+	else
+		err = i2c_smbus_write_byte_data(client, ABX8XX_REG_IRQ,
+						ABX8XX_IRQ_IM_1_4);
+	return err;
+}
+
 static const struct rtc_class_ops abx80x_rtc_ops = {
 	.read_time	= abx80x_rtc_read_time,
 	.set_time	= abx80x_rtc_set_time,
+	.read_alarm	= abx80x_read_alarm,
+	.set_alarm	= abx80x_set_alarm,
+	.alarm_irq_enable = abx80x_alarm_irq_enable,
 };
 
 static int abx80x_dt_trickle_cfg(struct device_node *np)
@@ -225,7 +343,8 @@ static int abx80x_probe(struct i2c_client *client,
 	}
 
 	err = i2c_smbus_write_byte_data(client, ABX8XX_REG_CTRL1,
-					((data & ~ABX8XX_CTRL_12_24) |
+					((data & ~(ABX8XX_CTRL_12_24 |
+						   ABX8XX_CTRL_ARST)) |
 					 ABX8XX_CTRL_WRITE));
 	if (err < 0) {
 		dev_err(&client->dev, "Unable to write control register\n");
@@ -260,7 +379,12 @@ static int abx80x_probe(struct i2c_client *client,
 		abx80x_enable_trickle_charger(client, trickle_cfg);
 	}
 
-	rtc = devm_rtc_device_register(&client->dev, abx80x_driver.driver.name,
+	err = i2c_smbus_write_byte_data(client, ABX8XX_REG_CD_TIMER_CTL,
+					BIT(2));
+	if (err)
+		return err;
+
+	rtc = devm_rtc_device_register(&client->dev, "abx8xx",
 				       &abx80x_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(rtc))
@@ -268,6 +392,19 @@ static int abx80x_probe(struct i2c_client *client,
 
 	i2c_set_clientdata(client, rtc);
 
+	if (client->irq > 0) {
+		dev_info(&client->dev, "IRQ %d supplied\n", client->irq);
+		err = devm_request_threaded_irq(&client->dev, client->irq, NULL,
+						abx80x_handle_irq,
+						IRQF_SHARED | IRQF_ONESHOT,
+						"abx8xx",
+						client);
+		if (err) {
+			dev_err(&client->dev, "unable to request IRQ, alarms disabled\n");
+			client->irq = 0;
+		}
+	}
+
 	return 0;
 }
 
@@ -286,6 +423,7 @@ static const struct i2c_device_id abx80x_id[] = {
 	{ "ab1803", AB1803 },
 	{ "ab1804", AB1804 },
 	{ "ab1805", AB1805 },
+	{ "rv1805", AB1805 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, abx80x_id);
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index 8f7034ba7d9e..84fb541038be 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -725,7 +725,7 @@ cmos_do_probe(struct device *dev, struct resource *ports, int rtc_irq)
 			rtc_cmos_int_handler = cmos_interrupt;
 
 		retval = request_irq(rtc_irq, rtc_cmos_int_handler,
-				0, dev_name(&cmos_rtc.rtc->dev),
+				IRQF_SHARED, dev_name(&cmos_rtc.rtc->dev),
 				cmos_rtc.rtc);
 		if (retval < 0) {
 			dev_dbg(dev, "IRQ %d is already in use\n", rtc_irq);
diff --git a/drivers/rtc/rtc-da9063.c b/drivers/rtc/rtc-da9063.c
index d6c853bbfa9f..f85cae240f12 100644
--- a/drivers/rtc/rtc-da9063.c
+++ b/drivers/rtc/rtc-da9063.c
@@ -191,24 +191,13 @@ static void da9063_tm_to_data(struct rtc_time *tm, u8 *data,
 {
 	const struct da9063_compatible_rtc_regmap *config = rtc->config;
 
-	data[RTC_SEC] &= ~config->rtc_count_sec_mask;
-	data[RTC_SEC] |= tm->tm_sec & config->rtc_count_sec_mask;
-
-	data[RTC_MIN] &= ~config->rtc_count_min_mask;
-	data[RTC_MIN] |= tm->tm_min & config->rtc_count_min_mask;
-
-	data[RTC_HOUR] &= ~config->rtc_count_hour_mask;
-	data[RTC_HOUR] |= tm->tm_hour & config->rtc_count_hour_mask;
-
-	data[RTC_DAY] &= ~config->rtc_count_day_mask;
-	data[RTC_DAY] |= tm->tm_mday & config->rtc_count_day_mask;
-
-	data[RTC_MONTH] &= ~config->rtc_count_month_mask;
-	data[RTC_MONTH] |= MONTHS_TO_DA9063(tm->tm_mon) &
+	data[RTC_SEC]   = tm->tm_sec & config->rtc_count_sec_mask;
+	data[RTC_MIN]   = tm->tm_min & config->rtc_count_min_mask;
+	data[RTC_HOUR]  = tm->tm_hour & config->rtc_count_hour_mask;
+	data[RTC_DAY]   = tm->tm_mday & config->rtc_count_day_mask;
+	data[RTC_MONTH] = MONTHS_TO_DA9063(tm->tm_mon) &
 				config->rtc_count_month_mask;
-
-	data[RTC_YEAR] &= ~config->rtc_count_year_mask;
-	data[RTC_YEAR] |= YEARS_TO_DA9063(tm->tm_year) &
+	data[RTC_YEAR]  = YEARS_TO_DA9063(tm->tm_year) &
 				config->rtc_count_year_mask;
 }
 
diff --git a/drivers/rtc/rtc-ds1305.c b/drivers/rtc/rtc-ds1305.c
index 85706a9f82c9..f39691eea736 100644
--- a/drivers/rtc/rtc-ds1305.c
+++ b/drivers/rtc/rtc-ds1305.c
@@ -186,9 +186,7 @@ static int ds1305_get_time(struct device *dev, struct rtc_time *time)
 	if (status < 0)
 		return status;
 
-	dev_vdbg(dev, "%s: %02x %02x %02x, %02x %02x %02x %02x\n",
-		"read", buf[0], buf[1], buf[2], buf[3],
-		buf[4], buf[5], buf[6]);
+	dev_vdbg(dev, "%s: %3ph, %4ph\n", "read", &buf[0], &buf[3]);
 
 	/* Decode the registers */
 	time->tm_sec = bcd2bin(buf[DS1305_SEC]);
@@ -232,9 +230,7 @@ static int ds1305_set_time(struct device *dev, struct rtc_time *time)
 	*bp++ = bin2bcd(time->tm_mon + 1);
 	*bp++ = bin2bcd(time->tm_year - 100);
 
-	dev_dbg(dev, "%s: %02x %02x %02x, %02x %02x %02x %02x\n",
-		"write", buf[1], buf[2], buf[3],
-		buf[4], buf[5], buf[6], buf[7]);
+	dev_dbg(dev, "%s: %3ph, %4ph\n", "write", &buf[1], &buf[4]);
 
 	/* use write-then-read since dma from stack is nonportable */
 	return spi_write_then_read(ds1305->spi, buf, sizeof(buf),
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index aa705bb4748c..cf685f67b391 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -460,13 +460,8 @@ static int ds1337_read_alarm(struct device *dev, struct rtc_wkalrm *t)
 		return -EIO;
 	}
 
-	dev_dbg(dev, "%s: %02x %02x %02x %02x, %02x %02x %02x, %02x %02x\n",
-			"alarm read",
-			ds1307->regs[0], ds1307->regs[1],
-			ds1307->regs[2], ds1307->regs[3],
-			ds1307->regs[4], ds1307->regs[5],
-			ds1307->regs[6], ds1307->regs[7],
-			ds1307->regs[8]);
+	dev_dbg(dev, "%s: %4ph, %3ph, %2ph\n", "alarm read",
+		&ds1307->regs[0], &ds1307->regs[4], &ds1307->regs[7]);
 
 	/*
 	 * report alarm time (ALARM1); assume 24 hour and day-of-month modes,
@@ -522,12 +517,8 @@ static int ds1337_set_alarm(struct device *dev, struct rtc_wkalrm *t)
 	control = ds1307->regs[7];
 	status = ds1307->regs[8];
 
-	dev_dbg(dev, "%s: %02x %02x %02x %02x, %02x %02x %02x, %02x %02x\n",
-			"alarm set (old status)",
-			ds1307->regs[0], ds1307->regs[1],
-			ds1307->regs[2], ds1307->regs[3],
-			ds1307->regs[4], ds1307->regs[5],
-			ds1307->regs[6], control, status);
+	dev_dbg(dev, "%s: %4ph, %3ph, %02x %02x\n", "alarm set (old status)",
+		&ds1307->regs[0], &ds1307->regs[4], control, status);
 
 	/* set ALARM1, using 24 hour and day-of-month modes */
 	buf[0] = bin2bcd(t->time.tm_sec);
diff --git a/drivers/rtc/rtc-ds1685.c b/drivers/rtc/rtc-ds1685.c
index 05a51ef52703..535050fc5e9f 100644
--- a/drivers/rtc/rtc-ds1685.c
+++ b/drivers/rtc/rtc-ds1685.c
@@ -853,7 +853,7 @@ ds1685_rtc_proc(struct device *dev, struct seq_file *seq)
 	   "Periodic Rate\t: %s\n"
 	   "SQW Freq\t: %s\n"
 #ifdef CONFIG_RTC_DS1685_PROC_REGS
-	   "Serial #\t: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n"
+	   "Serial #\t: %8phC\n"
 	   "Register Status\t:\n"
 	   "   Ctrl A\t: UIP  DV2  DV1  DV0  RS3  RS2  RS1  RS0\n"
 	   "\t\t:  %s\n"
@@ -872,7 +872,7 @@ ds1685_rtc_proc(struct device *dev, struct seq_file *seq)
 	   "   Ctrl 4B\t: ABE  E32k  CS  RCE  PRS  RIE  WIE  KSE\n"
 	   "\t\t:  %s\n",
 #else
-	   "Serial #\t: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
+	   "Serial #\t: %8phC\n",
 #endif
 	   model,
 	   ((ctrla & RTC_CTRL_A_DV1) ? "enabled" : "disabled"),
@@ -888,7 +888,7 @@ ds1685_rtc_proc(struct device *dev, struct seq_file *seq)
 	   (!((ctrl4b & RTC_CTRL_4B_E32K)) ?
 	    ds1685_rtc_sqw_freq[(ctrla & RTC_CTRL_A_RS_MASK)] : "32768Hz"),
 #ifdef CONFIG_RTC_DS1685_PROC_REGS
-	   ssn[0], ssn[1], ssn[2], ssn[3], ssn[4], ssn[5], ssn[6], ssn[7],
+	   ssn,
 	   ds1685_rtc_print_regs(ctrla, bits[0]),
 	   ds1685_rtc_print_regs(ctrlb, bits[1]),
 	   ds1685_rtc_print_regs(ctrlc, bits[2]),
@@ -896,7 +896,7 @@ ds1685_rtc_proc(struct device *dev, struct seq_file *seq)
 	   ds1685_rtc_print_regs(ctrl4a, bits[4]),
 	   ds1685_rtc_print_regs(ctrl4b, bits[5]));
 #else
-	   ssn[0], ssn[1], ssn[2], ssn[3], ssn[4], ssn[5], ssn[6], ssn[7]);
+	   ssn);
 #endif
 	return 0;
 }
@@ -1114,7 +1114,7 @@ ds1685_rtc_sysfs_battery_show(struct device *dev,
 
 	ctrld = rtc->read(rtc, RTC_CTRL_D);
 
-	return snprintf(buf, 13, "%s\n",
+	return sprintf(buf, "%s\n",
 			(ctrld & RTC_CTRL_D_VRT) ? "ok" : "not ok or N/A");
 }
 static DEVICE_ATTR(battery, S_IRUGO, ds1685_rtc_sysfs_battery_show, NULL);
@@ -1137,7 +1137,7 @@ ds1685_rtc_sysfs_auxbatt_show(struct device *dev,
 	ctrl4a = rtc->read(rtc, RTC_EXT_CTRL_4A);
 	ds1685_rtc_switch_to_bank0(rtc);
 
-	return snprintf(buf, 13, "%s\n",
+	return sprintf(buf, "%s\n",
 			(ctrl4a & RTC_CTRL_4A_VRT2) ? "ok" : "not ok or N/A");
 }
 static DEVICE_ATTR(auxbatt, S_IRUGO, ds1685_rtc_sysfs_auxbatt_show, NULL);
@@ -1160,11 +1160,7 @@ ds1685_rtc_sysfs_serial_show(struct device *dev,
 	ds1685_rtc_get_ssn(rtc, ssn);
 	ds1685_rtc_switch_to_bank0(rtc);
 
-	return snprintf(buf, 24, "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			ssn[0], ssn[1], ssn[2], ssn[3], ssn[4], ssn[5],
-			ssn[6], ssn[7]);
-
-	return 0;
+	return sprintf(buf, "%8phC\n", ssn);
 }
 static DEVICE_ATTR(serial, S_IRUGO, ds1685_rtc_sysfs_serial_show, NULL);
 
@@ -1287,7 +1283,7 @@ ds1685_rtc_sysfs_ctrl_regs_show(struct device *dev,
 	tmp = rtc->read(rtc, reg_info->reg) & reg_info->bit;
 	ds1685_rtc_switch_to_bank0(rtc);
 
-	return snprintf(buf, 2, "%d\n", (tmp ? 1 : 0));
+	return sprintf(buf, "%d\n", (tmp ? 1 : 0));
 }
 
 /**
@@ -1623,7 +1619,7 @@ ds1685_rtc_sysfs_time_regs_show(struct device *dev,
 	tmp = ds1685_rtc_bcd2bin(rtc, tmp, bcd_reg_info->mask,
 				 bin_reg_info->mask);
 
-	return snprintf(buf, 4, "%d\n", tmp);
+	return sprintf(buf, "%d\n", tmp);
 }
 
 /**
diff --git a/drivers/rtc/rtc-ds2404.c b/drivers/rtc/rtc-ds2404.c
index 7885edd3d507..16310fe79d76 100644
--- a/drivers/rtc/rtc-ds2404.c
+++ b/drivers/rtc/rtc-ds2404.c
@@ -48,7 +48,7 @@ struct ds2404_gpio {
 
 struct ds2404 {
 	struct ds2404_gpio *gpio;
-	struct ds2404_chip_ops *ops;
+	const struct ds2404_chip_ops *ops;
 	struct rtc_device *rtc;
 };
 
@@ -95,7 +95,7 @@ static void ds2404_gpio_unmap(struct ds2404 *chip)
 		gpio_free(ds2404_gpio[i].gpio);
 }
 
-static struct ds2404_chip_ops ds2404_gpio_ops = {
+static const struct ds2404_chip_ops ds2404_gpio_ops = {
 	.map_io		= ds2404_gpio_map,
 	.unmap_io	= ds2404_gpio_unmap,
 };
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index 3806961b4348..96d38609d803 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -191,11 +191,69 @@ static int efi_set_time(struct device *dev, struct rtc_time *tm)
 	return status == EFI_SUCCESS ? 0 : -EINVAL;
 }
 
+static int efi_procfs(struct device *dev, struct seq_file *seq)
+{
+	efi_time_t      eft, alm;
+	efi_time_cap_t  cap;
+	efi_bool_t      enabled, pending;
+
+	memset(&eft, 0, sizeof(eft));
+	memset(&alm, 0, sizeof(alm));
+	memset(&cap, 0, sizeof(cap));
+
+	efi.get_time(&eft, &cap);
+	efi.get_wakeup_time(&enabled, &pending, &alm);
+
+	seq_printf(seq,
+		   "Time\t\t: %u:%u:%u.%09u\n"
+		   "Date\t\t: %u-%u-%u\n"
+		   "Daylight\t: %u\n",
+		   eft.hour, eft.minute, eft.second, eft.nanosecond,
+		   eft.year, eft.month, eft.day,
+		   eft.daylight);
+
+	if (eft.timezone == EFI_UNSPECIFIED_TIMEZONE)
+		seq_puts(seq, "Timezone\t: unspecified\n");
+	else
+		/* XXX fixme: convert to string? */
+		seq_printf(seq, "Timezone\t: %u\n", eft.timezone);
+
+	seq_printf(seq,
+		   "Alarm Time\t: %u:%u:%u.%09u\n"
+		   "Alarm Date\t: %u-%u-%u\n"
+		   "Alarm Daylight\t: %u\n"
+		   "Enabled\t\t: %s\n"
+		   "Pending\t\t: %s\n",
+		   alm.hour, alm.minute, alm.second, alm.nanosecond,
+		   alm.year, alm.month, alm.day,
+		   alm.daylight,
+		   enabled == 1 ? "yes" : "no",
+		   pending == 1 ? "yes" : "no");
+
+	if (eft.timezone == EFI_UNSPECIFIED_TIMEZONE)
+		seq_puts(seq, "Timezone\t: unspecified\n");
+	else
+		/* XXX fixme: convert to string? */
+		seq_printf(seq, "Timezone\t: %u\n", alm.timezone);
+
+	/*
+	 * now prints the capabilities
+	 */
+	seq_printf(seq,
+		   "Resolution\t: %u\n"
+		   "Accuracy\t: %u\n"
+		   "SetstoZero\t: %u\n",
+		   cap.resolution, cap.accuracy, cap.sets_to_zero);
+
+	return 0;
+}
+
 static const struct rtc_class_ops efi_rtc_ops = {
-	.read_time = efi_read_time,
-	.set_time = efi_set_time,
-	.read_alarm = efi_read_alarm,
-	.set_alarm = efi_set_alarm,
+	.read_time	= efi_read_time,
+	.set_time	= efi_set_time,
+	.read_alarm	= efi_read_alarm,
+	.set_alarm	= efi_set_alarm,
+	.proc		= efi_procfs,
 };
 
 static int __init efi_rtc_probe(struct platform_device *dev)
diff --git a/drivers/rtc/rtc-gemini.c b/drivers/rtc/rtc-gemini.c
index e84184647d15..f46b6d46a51b 100644
--- a/drivers/rtc/rtc-gemini.c
+++ b/drivers/rtc/rtc-gemini.c
@@ -156,7 +156,6 @@ static int gemini_rtc_remove(struct platform_device *pdev)
 	struct gemini_rtc *rtc = platform_get_drvdata(pdev);
 
 	rtc_device_unregister(rtc->rtc_dev);
-	platform_set_drvdata(pdev, NULL);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-imxdi.c b/drivers/rtc/rtc-imxdi.c
index 7bffd7f0e306..8d8049bdfaf6 100644
--- a/drivers/rtc/rtc-imxdi.c
+++ b/drivers/rtc/rtc-imxdi.c
@@ -303,7 +303,7 @@ static int di_handle_invalid_state(struct imxdi_dev *imxdi, u32 dsr)
 	sec = readl(imxdi->ioaddr + DTCMR);
 	if (sec != 0)
 		dev_warn(&imxdi->pdev->dev,
-			 "The security violation has happend at %u seconds\n",
+			 "The security violation has happened at %u seconds\n",
 			 sec);
 	/*
 	 * the timer cannot be set/modified if
diff --git a/drivers/rtc/rtc-lpc32xx.c b/drivers/rtc/rtc-lpc32xx.c
index f923f7324788..887871c3d526 100644
--- a/drivers/rtc/rtc-lpc32xx.c
+++ b/drivers/rtc/rtc-lpc32xx.c
@@ -205,7 +205,7 @@ static int lpc32xx_rtc_probe(struct platform_device *pdev)
 	u32 tmp;
 
 	rtcirq = platform_get_irq(pdev, 0);
-	if (rtcirq < 0 || rtcirq >= NR_IRQS) {
+	if (rtcirq < 0) {
 		dev_warn(&pdev->dev, "Can't get interrupt resource\n");
 		rtcirq = -1;
 	}
diff --git a/drivers/rtc/rtc-opal.c b/drivers/rtc/rtc-opal.c
index df39ce02a99d..9c18d6fd8107 100644
--- a/drivers/rtc/rtc-opal.c
+++ b/drivers/rtc/rtc-opal.c
@@ -40,7 +40,7 @@ static void opal_to_tm(u32 y_m_d, u64 h_m_s_ms, struct rtc_time *tm)
 	tm->tm_min  = bcd2bin((h_m_s_ms >> 48) & 0xff);
 	tm->tm_sec  = bcd2bin((h_m_s_ms >> 40) & 0xff);
 
-	GregorianDay(tm);
+	tm->tm_wday = -1;
 }
 
 static void tm_to_opal(struct rtc_time *tm, u32 *y_m_d, u64 *h_m_s_ms)
diff --git a/drivers/rtc/rtc-pcf8523.c b/drivers/rtc/rtc-pcf8523.c
index e7ebcc0b7e59..988566caaaa6 100644
--- a/drivers/rtc/rtc-pcf8523.c
+++ b/drivers/rtc/rtc-pcf8523.c
@@ -219,6 +219,17 @@ static int pcf8523_rtc_set_time(struct device *dev, struct rtc_time *tm)
 	u8 regs[8];
 	int err;
 
+	/*
+	 * The hardware can only store values between 0 and 99 in it's YEAR
+	 * register (with 99 overflowing to 0 on increment).
+	 * After 2100-02-28 we could start interpreting the year to be in the
+	 * interval [2100, 2199], but there is no path to switch in a smooth way
+	 * because the chip handles YEAR=0x00 (and the out-of-spec
+	 * YEAR=0xa0) as a leap year, but 2100 isn't.
+	 */
+	if (tm->tm_year < 100 || tm->tm_year >= 200)
+		return -EINVAL;
+
 	err = pcf8523_stop_rtc(client);
 	if (err < 0)
 		return err;
diff --git a/drivers/rtc/rtc-proc.c b/drivers/rtc/rtc-proc.c
index ffa69e1c9245..31e7e23cc5be 100644
--- a/drivers/rtc/rtc-proc.c
+++ b/drivers/rtc/rtc-proc.c
@@ -112,19 +112,21 @@ static int rtc_proc_open(struct inode *inode, struct file *file)
 	int ret;
 	struct rtc_device *rtc = PDE_DATA(inode);
 
-	if (!try_module_get(THIS_MODULE))
+	if (!try_module_get(rtc->owner))
 		return -ENODEV;
 
 	ret = single_open(file, rtc_proc_show, rtc);
 	if (ret)
-		module_put(THIS_MODULE);
+		module_put(rtc->owner);
 	return ret;
 }
 
 static int rtc_proc_release(struct inode *inode, struct file *file)
 {
 	int res = single_release(inode, file);
-	module_put(THIS_MODULE);
+	struct rtc_device *rtc = PDE_DATA(inode);
+
+	module_put(rtc->owner);
 	return res;
 }
 
diff --git a/drivers/rtc/rtc-rv8803.c b/drivers/rtc/rtc-rv8803.c
index e7329e21bfe3..7155c0816aa6 100644
--- a/drivers/rtc/rtc-rv8803.c
+++ b/drivers/rtc/rtc-rv8803.c
@@ -61,7 +61,7 @@ static irqreturn_t rv8803_handle_irq(int irq, void *dev_id)
 	struct i2c_client *client = dev_id;
 	struct rv8803_data *rv8803 = i2c_get_clientdata(client);
 	unsigned long events = 0;
-	u8 flags;
+	int flags;
 
 	spin_lock(&rv8803->flags_lock);
 
@@ -502,6 +502,7 @@ static int rv8803_remove(struct i2c_client *client)
 
 static const struct i2c_device_id rv8803_id[] = {
 	{ "rv8803", 0 },
+	{ "rx8900", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, rv8803_id);
diff --git a/drivers/rtc/rtc-rx8010.c b/drivers/rtc/rtc-rx8010.c
new file mode 100644
index 000000000000..772d221ec2d9
--- /dev/null
+++ b/drivers/rtc/rtc-rx8010.c
@@ -0,0 +1,523 @@
+/*
+ * Driver for the Epson RTC module RX-8010 SJ
+ *
+ * Copyright(C) Timesys Corporation 2015
+ * Copyright(C) General Electric Company 2015
+ *
+ * This program is free software; you can 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/bcd.h>
+#include <linux/bitops.h>
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/rtc.h>
+
+#define RX8010_SEC     0x10
+#define RX8010_MIN     0x11
+#define RX8010_HOUR    0x12
+#define RX8010_WDAY    0x13
+#define RX8010_MDAY    0x14
+#define RX8010_MONTH   0x15
+#define RX8010_YEAR    0x16
+#define RX8010_YEAR    0x16
+#define RX8010_RESV17  0x17
+#define RX8010_ALMIN   0x18
+#define RX8010_ALHOUR  0x19
+#define RX8010_ALWDAY  0x1A
+#define RX8010_TCOUNT0 0x1B
+#define RX8010_TCOUNT1 0x1C
+#define RX8010_EXT     0x1D
+#define RX8010_FLAG    0x1E
+#define RX8010_CTRL    0x1F
+/* 0x20 to 0x2F are user registers */
+#define RX8010_RESV30  0x30
+#define RX8010_RESV31  0x32
+#define RX8010_IRQ     0x32
+
+#define RX8010_EXT_WADA  BIT(3)
+
+#define RX8010_FLAG_VLF  BIT(1)
+#define RX8010_FLAG_AF   BIT(3)
+#define RX8010_FLAG_TF   BIT(4)
+#define RX8010_FLAG_UF   BIT(5)
+
+#define RX8010_CTRL_AIE  BIT(3)
+#define RX8010_CTRL_UIE  BIT(5)
+#define RX8010_CTRL_STOP BIT(6)
+#define RX8010_CTRL_TEST BIT(7)
+
+#define RX8010_ALARM_AE  BIT(7)
+
+static const struct i2c_device_id rx8010_id[] = {
+	{ "rx8010", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, rx8010_id);
+
+struct rx8010_data {
+	struct i2c_client *client;
+	struct rtc_device *rtc;
+	u8 ctrlreg;
+	spinlock_t flags_lock;
+};
+
+static irqreturn_t rx8010_irq_1_handler(int irq, void *dev_id)
+{
+	struct i2c_client *client = dev_id;
+	struct rx8010_data *rx8010 = i2c_get_clientdata(client);
+	int flagreg;
+
+	spin_lock(&rx8010->flags_lock);
+
+	flagreg = i2c_smbus_read_byte_data(client, RX8010_FLAG);
+
+	if (flagreg <= 0) {
+		spin_unlock(&rx8010->flags_lock);
+		return IRQ_NONE;
+	}
+
+	if (flagreg & RX8010_FLAG_VLF)
+		dev_warn(&client->dev, "Frequency stop detected\n");
+
+	if (flagreg & RX8010_FLAG_TF) {
+		flagreg &= ~RX8010_FLAG_TF;
+		rtc_update_irq(rx8010->rtc, 1, RTC_PF | RTC_IRQF);
+	}
+
+	if (flagreg & RX8010_FLAG_AF) {
+		flagreg &= ~RX8010_FLAG_AF;
+		rtc_update_irq(rx8010->rtc, 1, RTC_AF | RTC_IRQF);
+	}
+
+	if (flagreg & RX8010_FLAG_UF) {
+		flagreg &= ~RX8010_FLAG_UF;
+		rtc_update_irq(rx8010->rtc, 1, RTC_UF | RTC_IRQF);
+	}
+
+	i2c_smbus_write_byte_data(client, RX8010_FLAG, flagreg);
+
+	spin_unlock(&rx8010->flags_lock);
+	return IRQ_HANDLED;
+}
+
+static int rx8010_get_time(struct device *dev, struct rtc_time *dt)
+{
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	u8 date[7];
+	int flagreg;
+	int err;
+
+	flagreg = i2c_smbus_read_byte_data(rx8010->client, RX8010_FLAG);
+	if (flagreg < 0)
+		return flagreg;
+
+	if (flagreg & RX8010_FLAG_VLF) {
+		dev_warn(dev, "Frequency stop detected\n");
+		return -EINVAL;
+	}
+
+	err = i2c_smbus_read_i2c_block_data(rx8010->client, RX8010_SEC,
+					    7, date);
+	if (err != 7)
+		return err < 0 ? err : -EIO;
+
+	dt->tm_sec = bcd2bin(date[RX8010_SEC - RX8010_SEC] & 0x7f);
+	dt->tm_min = bcd2bin(date[RX8010_MIN - RX8010_SEC] & 0x7f);
+	dt->tm_hour = bcd2bin(date[RX8010_HOUR - RX8010_SEC] & 0x3f);
+	dt->tm_mday = bcd2bin(date[RX8010_MDAY - RX8010_SEC] & 0x3f);
+	dt->tm_mon = bcd2bin(date[RX8010_MONTH - RX8010_SEC] & 0x1f) - 1;
+	dt->tm_year = bcd2bin(date[RX8010_YEAR - RX8010_SEC]) + 100;
+	dt->tm_wday = ffs(date[RX8010_WDAY - RX8010_SEC] & 0x7f);
+
+	return rtc_valid_tm(dt);
+}
+
+static int rx8010_set_time(struct device *dev, struct rtc_time *dt)
+{
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	u8 date[7];
+	int ctrl, flagreg;
+	int ret;
+	unsigned long irqflags;
+
+	if ((dt->tm_year < 100) || (dt->tm_year > 199))
+		return -EINVAL;
+
+	/* set STOP bit before changing clock/calendar */
+	ctrl = i2c_smbus_read_byte_data(rx8010->client, RX8010_CTRL);
+	if (ctrl < 0)
+		return ctrl;
+	rx8010->ctrlreg = ctrl | RX8010_CTRL_STOP;
+	ret = i2c_smbus_write_byte_data(rx8010->client, RX8010_CTRL,
+					rx8010->ctrlreg);
+	if (ret < 0)
+		return ret;
+
+	date[RX8010_SEC - RX8010_SEC] = bin2bcd(dt->tm_sec);
+	date[RX8010_MIN - RX8010_SEC] = bin2bcd(dt->tm_min);
+	date[RX8010_HOUR - RX8010_SEC] = bin2bcd(dt->tm_hour);
+	date[RX8010_MDAY - RX8010_SEC] = bin2bcd(dt->tm_mday);
+	date[RX8010_MONTH - RX8010_SEC] = bin2bcd(dt->tm_mon + 1);
+	date[RX8010_YEAR - RX8010_SEC] = bin2bcd(dt->tm_year - 100);
+	date[RX8010_WDAY - RX8010_SEC] = bin2bcd(1 << dt->tm_wday);
+
+	ret = i2c_smbus_write_i2c_block_data(rx8010->client,
+					     RX8010_SEC, 7, date);
+	if (ret < 0)
+		return ret;
+
+	/* clear STOP bit after changing clock/calendar */
+	ctrl = i2c_smbus_read_byte_data(rx8010->client, RX8010_CTRL);
+	if (ctrl < 0)
+		return ctrl;
+	rx8010->ctrlreg = ctrl & ~RX8010_CTRL_STOP;
+	ret = i2c_smbus_write_byte_data(rx8010->client, RX8010_CTRL,
+					rx8010->ctrlreg);
+	if (ret < 0)
+		return ret;
+
+	spin_lock_irqsave(&rx8010->flags_lock, irqflags);
+
+	flagreg = i2c_smbus_read_byte_data(rx8010->client, RX8010_FLAG);
+	if (flagreg < 0) {
+		spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+		return flagreg;
+	}
+
+	if (flagreg & RX8010_FLAG_VLF)
+		ret = i2c_smbus_write_byte_data(rx8010->client, RX8010_FLAG,
+						flagreg & ~RX8010_FLAG_VLF);
+
+	spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+
+	return 0;
+}
+
+static int rx8010_init_client(struct i2c_client *client)
+{
+	struct rx8010_data *rx8010 = i2c_get_clientdata(client);
+	u8 ctrl[2];
+	int need_clear = 0, err = 0;
+
+	/* Initialize reserved registers as specified in datasheet */
+	err = i2c_smbus_write_byte_data(client, RX8010_RESV17, 0xD8);
+	if (err < 0)
+		return err;
+
+	err = i2c_smbus_write_byte_data(client, RX8010_RESV30, 0x00);
+	if (err < 0)
+		return err;
+
+	err = i2c_smbus_write_byte_data(client, RX8010_RESV31, 0x08);
+	if (err < 0)
+		return err;
+
+	err = i2c_smbus_write_byte_data(client, RX8010_IRQ, 0x00);
+	if (err < 0)
+		return err;
+
+	err = i2c_smbus_read_i2c_block_data(rx8010->client, RX8010_FLAG,
+					    2, ctrl);
+	if (err != 2)
+		return err < 0 ? err : -EIO;
+
+	if (ctrl[0] & RX8010_FLAG_VLF)
+		dev_warn(&client->dev, "Frequency stop was detected\n");
+
+	if (ctrl[0] & RX8010_FLAG_AF) {
+		dev_warn(&client->dev, "Alarm was detected\n");
+		need_clear = 1;
+	}
+
+	if (ctrl[0] & RX8010_FLAG_TF)
+		need_clear = 1;
+
+	if (ctrl[0] & RX8010_FLAG_UF)
+		need_clear = 1;
+
+	if (need_clear) {
+		ctrl[0] &= ~(RX8010_FLAG_AF | RX8010_FLAG_TF | RX8010_FLAG_UF);
+		err = i2c_smbus_write_byte_data(client, RX8010_FLAG, ctrl[0]);
+		if (err < 0)
+			return err;
+	}
+
+	rx8010->ctrlreg = (ctrl[1] & ~RX8010_CTRL_TEST);
+
+	return err;
+}
+
+static int rx8010_read_alarm(struct device *dev, struct rtc_wkalrm *t)
+{
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	struct i2c_client *client = rx8010->client;
+	u8 alarmvals[3];
+	int flagreg;
+	int err;
+
+	err = i2c_smbus_read_i2c_block_data(client, RX8010_ALMIN, 3, alarmvals);
+	if (err != 3)
+		return err < 0 ? err : -EIO;
+
+	flagreg = i2c_smbus_read_byte_data(client, RX8010_FLAG);
+	if (flagreg < 0)
+		return flagreg;
+
+	t->time.tm_sec = 0;
+	t->time.tm_min = bcd2bin(alarmvals[0] & 0x7f);
+	t->time.tm_hour = bcd2bin(alarmvals[1] & 0x3f);
+
+	if (alarmvals[2] & RX8010_ALARM_AE)
+		t->time.tm_mday = -1;
+	else
+		t->time.tm_mday = bcd2bin(alarmvals[2] & 0x7f);
+
+	t->time.tm_wday = -1;
+	t->time.tm_mon = -1;
+	t->time.tm_year = -1;
+
+	t->enabled = !!(rx8010->ctrlreg & RX8010_CTRL_AIE);
+	t->pending = (flagreg & RX8010_FLAG_AF) && t->enabled;
+
+	return err;
+}
+
+static int rx8010_set_alarm(struct device *dev, struct rtc_wkalrm *t)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	u8 alarmvals[3];
+	int extreg, flagreg;
+	int err;
+	unsigned long irqflags;
+
+	spin_lock_irqsave(&rx8010->flags_lock, irqflags);
+	flagreg = i2c_smbus_read_byte_data(client, RX8010_FLAG);
+	if (flagreg < 0) {
+		spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+		return flagreg;
+	}
+
+	if (rx8010->ctrlreg & (RX8010_CTRL_AIE | RX8010_CTRL_UIE)) {
+		rx8010->ctrlreg &= ~(RX8010_CTRL_AIE | RX8010_CTRL_UIE);
+		err = i2c_smbus_write_byte_data(rx8010->client, RX8010_CTRL,
+						rx8010->ctrlreg);
+		if (err < 0) {
+			spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+			return err;
+		}
+	}
+
+	flagreg &= ~RX8010_FLAG_AF;
+	err = i2c_smbus_write_byte_data(rx8010->client, RX8010_FLAG, flagreg);
+	spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+	if (err < 0)
+		return err;
+
+	alarmvals[0] = bin2bcd(t->time.tm_min);
+	alarmvals[1] = bin2bcd(t->time.tm_hour);
+	alarmvals[2] = bin2bcd(t->time.tm_mday);
+
+	err = i2c_smbus_write_i2c_block_data(rx8010->client, RX8010_ALMIN,
+					     2, alarmvals);
+	if (err < 0)
+		return err;
+
+	extreg = i2c_smbus_read_byte_data(client, RX8010_EXT);
+	if (extreg < 0)
+		return extreg;
+
+	extreg |= RX8010_EXT_WADA;
+	err = i2c_smbus_write_byte_data(rx8010->client, RX8010_EXT, extreg);
+	if (err < 0)
+		return err;
+
+	if (alarmvals[2] == 0)
+		alarmvals[2] |= RX8010_ALARM_AE;
+
+	err = i2c_smbus_write_byte_data(rx8010->client, RX8010_ALWDAY,
+					alarmvals[2]);
+	if (err < 0)
+		return err;
+
+	if (t->enabled) {
+		if (rx8010->rtc->uie_rtctimer.enabled)
+			rx8010->ctrlreg |= RX8010_CTRL_UIE;
+		if (rx8010->rtc->aie_timer.enabled)
+			rx8010->ctrlreg |=
+				(RX8010_CTRL_AIE | RX8010_CTRL_UIE);
+
+		err = i2c_smbus_write_byte_data(rx8010->client, RX8010_CTRL,
+						rx8010->ctrlreg);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+static int rx8010_alarm_irq_enable(struct device *dev,
+				   unsigned int enabled)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	int flagreg;
+	u8 ctrl;
+	int err;
+
+	ctrl = rx8010->ctrlreg;
+
+	if (enabled) {
+		if (rx8010->rtc->uie_rtctimer.enabled)
+			ctrl |= RX8010_CTRL_UIE;
+		if (rx8010->rtc->aie_timer.enabled)
+			ctrl |= (RX8010_CTRL_AIE | RX8010_CTRL_UIE);
+	} else {
+		if (!rx8010->rtc->uie_rtctimer.enabled)
+			ctrl &= ~RX8010_CTRL_UIE;
+		if (!rx8010->rtc->aie_timer.enabled)
+			ctrl &= ~RX8010_CTRL_AIE;
+	}
+
+	flagreg = i2c_smbus_read_byte_data(client, RX8010_FLAG);
+	if (flagreg < 0)
+		return flagreg;
+
+	flagreg &= ~RX8010_FLAG_AF;
+	err = i2c_smbus_write_byte_data(rx8010->client, RX8010_FLAG, flagreg);
+	if (err < 0)
+		return err;
+
+	if (ctrl != rx8010->ctrlreg) {
+		rx8010->ctrlreg = ctrl;
+		err = i2c_smbus_write_byte_data(rx8010->client, RX8010_CTRL,
+						rx8010->ctrlreg);
+		if (err < 0)
+			return err;
+	}
+
+	return 0;
+}
+
+static int rx8010_ioctl(struct device *dev, unsigned int cmd, unsigned long arg)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+	struct rx8010_data *rx8010 = dev_get_drvdata(dev);
+	int ret, tmp;
+	int flagreg;
+	unsigned long irqflags;
+
+	switch (cmd) {
+	case RTC_VL_READ:
+		flagreg = i2c_smbus_read_byte_data(rx8010->client, RX8010_FLAG);
+		if (flagreg < 0)
+			return flagreg;
+
+		tmp = !!(flagreg & RX8010_FLAG_VLF);
+		if (copy_to_user((void __user *)arg, &tmp, sizeof(int)))
+			return -EFAULT;
+
+		return 0;
+
+	case RTC_VL_CLR:
+		spin_lock_irqsave(&rx8010->flags_lock, irqflags);
+		flagreg = i2c_smbus_read_byte_data(rx8010->client, RX8010_FLAG);
+		if (flagreg < 0) {
+			spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+			return flagreg;
+		}
+
+		flagreg &= ~RX8010_FLAG_VLF;
+		ret = i2c_smbus_write_byte_data(client, RX8010_FLAG, flagreg);
+		spin_unlock_irqrestore(&rx8010->flags_lock, irqflags);
+		if (ret < 0)
+			return ret;
+
+		return 0;
+
+	default:
+		return -ENOIOCTLCMD;
+	}
+}
+
+static struct rtc_class_ops rx8010_rtc_ops = {
+	.read_time = rx8010_get_time,
+	.set_time = rx8010_set_time,
+	.ioctl = rx8010_ioctl,
+};
+
+static int rx8010_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
+	struct rx8010_data *rx8010;
+	int err = 0;
+
+	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_BYTE_DATA
+		| I2C_FUNC_SMBUS_I2C_BLOCK)) {
+		dev_err(&adapter->dev, "doesn't support required functionality\n");
+		return -EIO;
+	}
+
+	rx8010 = devm_kzalloc(&client->dev, sizeof(struct rx8010_data),
+			      GFP_KERNEL);
+	if (!rx8010)
+		return -ENOMEM;
+
+	rx8010->client = client;
+	i2c_set_clientdata(client, rx8010);
+
+	spin_lock_init(&rx8010->flags_lock);
+
+	err = rx8010_init_client(client);
+	if (err)
+		return err;
+
+	if (client->irq > 0) {
+		dev_info(&client->dev, "IRQ %d supplied\n", client->irq);
+		err = devm_request_threaded_irq(&client->dev, client->irq, NULL,
+						rx8010_irq_1_handler,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						"rx8010", client);
+
+		if (err) {
+			dev_err(&client->dev, "unable to request IRQ\n");
+			client->irq = 0;
+		} else {
+			rx8010_rtc_ops.read_alarm = rx8010_read_alarm;
+			rx8010_rtc_ops.set_alarm = rx8010_set_alarm;
+			rx8010_rtc_ops.alarm_irq_enable = rx8010_alarm_irq_enable;
+		}
+	}
+
+	rx8010->rtc = devm_rtc_device_register(&client->dev, client->name,
+		&rx8010_rtc_ops, THIS_MODULE);
+
+	if (IS_ERR(rx8010->rtc)) {
+		dev_err(&client->dev, "unable to register the class device\n");
+		return PTR_ERR(rx8010->rtc);
+	}
+
+	rx8010->rtc->max_user_freq = 1;
+
+	return err;
+}
+
+static struct i2c_driver rx8010_driver = {
+	.driver = {
+		.name = "rtc-rx8010",
+	},
+	.probe		= rx8010_probe,
+	.id_table	= rx8010_id,
+};
+
+module_i2c_driver(rx8010_driver);
+
+MODULE_AUTHOR("Akshay Bhat <akshay.bhat@timesys.com>");
+MODULE_DESCRIPTION("Epson RX8010SJ RTC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/rtc/rtc-s5m.c b/drivers/rtc/rtc-s5m.c
index f2504b4eef34..7407d7394bb4 100644
--- a/drivers/rtc/rtc-s5m.c
+++ b/drivers/rtc/rtc-s5m.c
@@ -38,7 +38,22 @@
  */
 #define UDR_READ_RETRY_CNT	5
 
-/* Registers used by the driver which are different between chipsets. */
+/*
+ * Registers used by the driver which are different between chipsets.
+ *
+ * Operations like read time and write alarm/time require updating
+ * specific fields in UDR register. These fields usually are auto-cleared
+ * (with some exceptions).
+ *
+ * Table of operations per device:
+ *
+ * Device     | Write time | Read time | Write alarm
+ * =================================================
+ * S5M8767    | UDR + TIME |           | UDR
+ * S2MPS11/14 | WUDR       | RUDR      | WUDR + RUDR
+ * S2MPS13    | WUDR       | RUDR      | WUDR + AUDR
+ * S2MPS15    | WUDR       | RUDR      | AUDR
+ */
 struct s5m_rtc_reg_config {
 	/* Number of registers used for setting time/alarm0/alarm1 */
 	unsigned int regs_count;
@@ -55,9 +70,16 @@ struct s5m_rtc_reg_config {
 	 * will enable update of time or alarm register. Then it will be
 	 * auto-cleared after successful update.
 	 */
-	unsigned int rtc_udr_update;
-	/* Mask for UDR field in 'rtc_udr_update' register */
-	unsigned int rtc_udr_mask;
+	unsigned int udr_update;
+	/* Auto-cleared mask in UDR field for writing time and alarm */
+	unsigned int autoclear_udr_mask;
+	/*
+	 * Masks in UDR field for time and alarm operations.
+	 * The read time mask can be 0. Rest should not.
+	 */
+	unsigned int read_time_udr_mask;
+	unsigned int write_time_udr_mask;
+	unsigned int write_alarm_udr_mask;
 };
 
 /* Register map for S5M8763 and S5M8767 */
@@ -67,22 +89,56 @@ static const struct s5m_rtc_reg_config s5m_rtc_regs = {
 	.ctrl			= S5M_ALARM1_CONF,
 	.alarm0			= S5M_ALARM0_SEC,
 	.alarm1			= S5M_ALARM1_SEC,
-	.rtc_udr_update		= S5M_RTC_UDR_CON,
-	.rtc_udr_mask		= S5M_RTC_UDR_MASK,
+	.udr_update		= S5M_RTC_UDR_CON,
+	.autoclear_udr_mask	= S5M_RTC_UDR_MASK,
+	.read_time_udr_mask	= 0, /* Not needed */
+	.write_time_udr_mask	= S5M_RTC_UDR_MASK | S5M_RTC_TIME_EN_MASK,
+	.write_alarm_udr_mask	= S5M_RTC_UDR_MASK,
+};
+
+/* Register map for S2MPS13 */
+static const struct s5m_rtc_reg_config s2mps13_rtc_regs = {
+	.regs_count		= 7,
+	.time			= S2MPS_RTC_SEC,
+	.ctrl			= S2MPS_RTC_CTRL,
+	.alarm0			= S2MPS_ALARM0_SEC,
+	.alarm1			= S2MPS_ALARM1_SEC,
+	.udr_update		= S2MPS_RTC_UDR_CON,
+	.autoclear_udr_mask	= S2MPS_RTC_WUDR_MASK,
+	.read_time_udr_mask	= S2MPS_RTC_RUDR_MASK,
+	.write_time_udr_mask	= S2MPS_RTC_WUDR_MASK,
+	.write_alarm_udr_mask	= S2MPS_RTC_WUDR_MASK | S2MPS13_RTC_AUDR_MASK,
+};
+
+/* Register map for S2MPS11/14 */
+static const struct s5m_rtc_reg_config s2mps14_rtc_regs = {
+	.regs_count		= 7,
+	.time			= S2MPS_RTC_SEC,
+	.ctrl			= S2MPS_RTC_CTRL,
+	.alarm0			= S2MPS_ALARM0_SEC,
+	.alarm1			= S2MPS_ALARM1_SEC,
+	.udr_update		= S2MPS_RTC_UDR_CON,
+	.autoclear_udr_mask	= S2MPS_RTC_WUDR_MASK,
+	.read_time_udr_mask	= S2MPS_RTC_RUDR_MASK,
+	.write_time_udr_mask	= S2MPS_RTC_WUDR_MASK,
+	.write_alarm_udr_mask	= S2MPS_RTC_WUDR_MASK | S2MPS_RTC_RUDR_MASK,
 };
 
 /*
- * Register map for S2MPS14.
- * It may be also suitable for S2MPS11 but this was not tested.
+ * Register map for S2MPS15 - in comparison to S2MPS14 the WUDR and AUDR bits
+ * are swapped.
  */
-static const struct s5m_rtc_reg_config s2mps_rtc_regs = {
+static const struct s5m_rtc_reg_config s2mps15_rtc_regs = {
 	.regs_count		= 7,
 	.time			= S2MPS_RTC_SEC,
 	.ctrl			= S2MPS_RTC_CTRL,
 	.alarm0			= S2MPS_ALARM0_SEC,
 	.alarm1			= S2MPS_ALARM1_SEC,
-	.rtc_udr_update		= S2MPS_RTC_UDR_CON,
-	.rtc_udr_mask		= S2MPS_RTC_WUDR_MASK,
+	.udr_update		= S2MPS_RTC_UDR_CON,
+	.autoclear_udr_mask	= S2MPS_RTC_WUDR_MASK,
+	.read_time_udr_mask	= S2MPS_RTC_RUDR_MASK,
+	.write_time_udr_mask	= S2MPS15_RTC_WUDR_MASK,
+	.write_alarm_udr_mask	= S2MPS15_RTC_AUDR_MASK,
 };
 
 struct s5m_rtc_info {
@@ -166,9 +222,8 @@ static inline int s5m8767_wait_for_udr_update(struct s5m_rtc_info *info)
 	unsigned int data;
 
 	do {
-		ret = regmap_read(info->regmap, info->regs->rtc_udr_update,
-				&data);
-	} while (--retry && (data & info->regs->rtc_udr_mask) && !ret);
+		ret = regmap_read(info->regmap, info->regs->udr_update, &data);
+	} while (--retry && (data & info->regs->autoclear_udr_mask) && !ret);
 
 	if (!retry)
 		dev_err(info->dev, "waiting for UDR update, reached max number of retries\n");
@@ -188,6 +243,7 @@ static inline int s5m_check_peding_alarm_interrupt(struct s5m_rtc_info *info,
 		ret = regmap_read(info->regmap, S5M_RTC_STATUS, &val);
 		val &= S5M_ALARM0_STATUS;
 		break;
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		ret = regmap_read(info->s5m87xx->regmap_pmic, S2MPS14_REG_ST2,
@@ -213,17 +269,15 @@ static inline int s5m8767_rtc_set_time_reg(struct s5m_rtc_info *info)
 	int ret;
 	unsigned int data;
 
-	ret = regmap_read(info->regmap, info->regs->rtc_udr_update, &data);
+	ret = regmap_read(info->regmap, info->regs->udr_update, &data);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to read update reg(%d)\n", ret);
 		return ret;
 	}
 
-	data |= info->regs->rtc_udr_mask;
-	if (info->device_type == S5M8763X || info->device_type == S5M8767X)
-		data |= S5M_RTC_TIME_EN_MASK;
+	data |= info->regs->write_time_udr_mask;
 
-	ret = regmap_write(info->regmap, info->regs->rtc_udr_update, data);
+	ret = regmap_write(info->regmap, info->regs->udr_update, data);
 	if (ret < 0) {
 		dev_err(info->dev, "failed to write update reg(%d)\n", ret);
 		return ret;
@@ -239,30 +293,29 @@ static inline int s5m8767_rtc_set_alarm_reg(struct s5m_rtc_info *info)
 	int ret;
 	unsigned int data;
 
-	ret = regmap_read(info->regmap, info->regs->rtc_udr_update, &data);
+	ret = regmap_read(info->regmap, info->regs->udr_update, &data);
 	if (ret < 0) {
 		dev_err(info->dev, "%s: fail to read update reg(%d)\n",
 			__func__, ret);
 		return ret;
 	}
 
-	data |= info->regs->rtc_udr_mask;
+	data |= info->regs->write_alarm_udr_mask;
 	switch (info->device_type) {
 	case S5M8763X:
 	case S5M8767X:
 		data &= ~S5M_RTC_TIME_EN_MASK;
 		break;
+	case S2MPS15X:
 	case S2MPS14X:
-		data |= S2MPS_RTC_RUDR_MASK;
-		break;
 	case S2MPS13X:
-		data |= S2MPS13_RTC_AUDR_MASK;
+		/* No exceptions needed */
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	ret = regmap_write(info->regmap, info->regs->rtc_udr_update, data);
+	ret = regmap_write(info->regmap, info->regs->udr_update, data);
 	if (ret < 0) {
 		dev_err(info->dev, "%s: fail to write update reg(%d)\n",
 			__func__, ret);
@@ -273,7 +326,7 @@ static inline int s5m8767_rtc_set_alarm_reg(struct s5m_rtc_info *info)
 
 	/* On S2MPS13 the AUDR is not auto-cleared */
 	if (info->device_type == S2MPS13X)
-		regmap_update_bits(info->regmap, info->regs->rtc_udr_update,
+		regmap_update_bits(info->regmap, info->regs->udr_update,
 				   S2MPS13_RTC_AUDR_MASK, 0);
 
 	return ret;
@@ -317,10 +370,11 @@ static int s5m_rtc_read_time(struct device *dev, struct rtc_time *tm)
 	u8 data[info->regs->regs_count];
 	int ret;
 
-	if (info->device_type == S2MPS14X || info->device_type == S2MPS13X) {
+	if (info->regs->read_time_udr_mask) {
 		ret = regmap_update_bits(info->regmap,
-				info->regs->rtc_udr_update,
-				S2MPS_RTC_RUDR_MASK, S2MPS_RTC_RUDR_MASK);
+				info->regs->udr_update,
+				info->regs->read_time_udr_mask,
+				info->regs->read_time_udr_mask);
 		if (ret) {
 			dev_err(dev,
 				"Failed to prepare registers for time reading: %d\n",
@@ -339,6 +393,7 @@ static int s5m_rtc_read_time(struct device *dev, struct rtc_time *tm)
 		break;
 
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		s5m8767_data_to_tm(data, tm, info->rtc_24hr_mode);
@@ -366,6 +421,7 @@ static int s5m_rtc_set_time(struct device *dev, struct rtc_time *tm)
 		s5m8763_tm_to_data(tm, data);
 		break;
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		ret = s5m8767_tm_to_data(tm, data);
@@ -414,6 +470,7 @@ static int s5m_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 		break;
 
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		s5m8767_data_to_tm(data, &alrm->time, info->rtc_24hr_mode);
@@ -463,6 +520,7 @@ static int s5m_rtc_stop_alarm(struct s5m_rtc_info *info)
 		break;
 
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		for (i = 0; i < info->regs->regs_count; i++)
@@ -508,6 +566,7 @@ static int s5m_rtc_start_alarm(struct s5m_rtc_info *info)
 		break;
 
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		data[RTC_SEC] |= ALARM_ENABLE_MASK;
@@ -548,6 +607,7 @@ static int s5m_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
 		break;
 
 	case S5M8767X:
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		s5m8767_tm_to_data(&alrm->time, data);
@@ -631,6 +691,7 @@ static int s5m8767_rtc_init_reg(struct s5m_rtc_info *info)
 		ret = regmap_raw_write(info->regmap, S5M_ALARM0_CONF, data, 2);
 		break;
 
+	case S2MPS15X:
 	case S2MPS14X:
 	case S2MPS13X:
 		data[0] = (0 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
@@ -679,10 +740,19 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	switch (platform_get_device_id(pdev)->driver_data) {
+	case S2MPS15X:
+		regmap_cfg = &s2mps14_rtc_regmap_config;
+		info->regs = &s2mps15_rtc_regs;
+		alarm_irq = S2MPS14_IRQ_RTCA0;
+		break;
 	case S2MPS14X:
+		regmap_cfg = &s2mps14_rtc_regmap_config;
+		info->regs = &s2mps14_rtc_regs;
+		alarm_irq = S2MPS14_IRQ_RTCA0;
+		break;
 	case S2MPS13X:
 		regmap_cfg = &s2mps14_rtc_regmap_config;
-		info->regs = &s2mps_rtc_regs;
+		info->regs = &s2mps13_rtc_regs;
 		alarm_irq = S2MPS14_IRQ_RTCA0;
 		break;
 	case S5M8763X:
@@ -805,6 +875,7 @@ static const struct platform_device_id s5m_rtc_id[] = {
 	{ "s5m-rtc",		S5M8767X },
 	{ "s2mps13-rtc",	S2MPS13X },
 	{ "s2mps14-rtc",	S2MPS14X },
+	{ "s2mps15-rtc",	S2MPS15X },
 	{ },
 };
 MODULE_DEVICE_TABLE(platform, s5m_rtc_id);
diff --git a/drivers/rtc/rtc-sunxi.c b/drivers/rtc/rtc-sunxi.c
index 52543ae37c98..abada609ddc7 100644
--- a/drivers/rtc/rtc-sunxi.c
+++ b/drivers/rtc/rtc-sunxi.c
@@ -133,7 +133,7 @@ struct sunxi_rtc_data_year {
 	unsigned char leap_shift;	/* bit shift to get the leap year */
 };
 
-static struct sunxi_rtc_data_year data_year_param[] = {
+static const struct sunxi_rtc_data_year data_year_param[] = {
 	[0] = {
 		.min		= 2010,
 		.max		= 2073,
@@ -151,7 +151,7 @@ static struct sunxi_rtc_data_year data_year_param[] = {
 struct sunxi_rtc_dev {
 	struct rtc_device *rtc;
 	struct device *dev;
-	struct sunxi_rtc_data_year *data_year;
+	const struct sunxi_rtc_data_year *data_year;
 	void __iomem *base;
 	int irq;
 };
@@ -175,7 +175,7 @@ static irqreturn_t sunxi_rtc_alarmirq(int irq, void *id)
 	return IRQ_NONE;
 }
 
-static void sunxi_rtc_setaie(int to, struct sunxi_rtc_dev *chip)
+static void sunxi_rtc_setaie(unsigned int to, struct sunxi_rtc_dev *chip)
 {
 	u32 alrm_val = 0;
 	u32 alrm_irq_val = 0;
@@ -343,7 +343,7 @@ static int sunxi_rtc_settime(struct device *dev, struct rtc_time *rtc_tm)
 	struct sunxi_rtc_dev *chip = dev_get_drvdata(dev);
 	u32 date = 0;
 	u32 time = 0;
-	int year;
+	unsigned int year;
 
 	/*
 	 * the input rtc_tm->tm_year is the offset relative to 1900. We use
@@ -353,8 +353,8 @@ static int sunxi_rtc_settime(struct device *dev, struct rtc_time *rtc_tm)
 
 	year = rtc_tm->tm_year + 1900;
 	if (year < chip->data_year->min || year > chip->data_year->max) {
-		dev_err(dev, "rtc only supports year in range %d - %d\n",
-				chip->data_year->min, chip->data_year->max);
+		dev_err(dev, "rtc only supports year in range %u - %u\n",
+			chip->data_year->min, chip->data_year->max);
 		return -EINVAL;
 	}
 
@@ -436,7 +436,6 @@ static int sunxi_rtc_probe(struct platform_device *pdev)
 {
 	struct sunxi_rtc_dev *chip;
 	struct resource *res;
-	const struct of_device_id *of_id;
 	int ret;
 
 	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
@@ -463,12 +462,11 @@ static int sunxi_rtc_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	of_id = of_match_device(sunxi_rtc_dt_ids, &pdev->dev);
-	if (!of_id) {
+	chip->data_year = of_device_get_match_data(&pdev->dev);
+	if (!chip->data_year) {
 		dev_err(&pdev->dev, "Unable to setup RTC data\n");
 		return -ENODEV;
 	}
-	chip->data_year = (struct sunxi_rtc_data_year *) of_id->data;
 
 	/* clear the alarm count value */
 	writel(0, chip->base + SUNXI_ALRM_DHMS);
diff --git a/drivers/rtc/rtc-sysfs.c b/drivers/rtc/rtc-sysfs.c
index 7273855ed02e..463e286064ab 100644
--- a/drivers/rtc/rtc-sysfs.c
+++ b/drivers/rtc/rtc-sysfs.c
@@ -91,7 +91,12 @@ max_user_freq_store(struct device *dev, struct device_attribute *attr,
 		const char *buf, size_t n)
 {
 	struct rtc_device *rtc = to_rtc_device(dev);
-	unsigned long val = simple_strtoul(buf, NULL, 0);
+	unsigned long val;
+	int err;
+
+	err = kstrtoul(buf, 0, &val);
+	if (err)
+		return err;
 
 	if (val >= 4096 || val == 0)
 		return -EINVAL;
@@ -175,7 +180,9 @@ wakealarm_store(struct device *dev, struct device_attribute *attr,
 		} else
 			adjust = 1;
 	}
-	alarm = simple_strtoul(buf_ptr, NULL, 0);
+	retval = kstrtoul(buf_ptr, 0, &alarm);
+	if (retval)
+		return retval;
 	if (adjust) {
 		alarm += now;
 	}
diff --git a/drivers/rtc/rtc-v3020.c b/drivers/rtc/rtc-v3020.c
index f9f97098c254..7a0436329d6c 100644
--- a/drivers/rtc/rtc-v3020.c
+++ b/drivers/rtc/rtc-v3020.c
@@ -57,7 +57,7 @@ struct v3020 {
 	/* GPIO access */
 	struct gpio *gpio;
 
-	struct v3020_chip_ops *ops;
+	const struct v3020_chip_ops *ops;
 
 	struct rtc_device *rtc;
 };
@@ -95,7 +95,7 @@ static unsigned char v3020_mmio_read_bit(struct v3020 *chip)
 	return !!(readl(chip->ioaddress) & (1 << chip->leftshift));
 }
 
-static struct v3020_chip_ops v3020_mmio_ops = {
+static const struct v3020_chip_ops v3020_mmio_ops = {
 	.map_io		= v3020_mmio_map,
 	.unmap_io	= v3020_mmio_unmap,
 	.read_bit	= v3020_mmio_read_bit,
@@ -158,7 +158,7 @@ static unsigned char v3020_gpio_read_bit(struct v3020 *chip)
 	return bit;
 }
 
-static struct v3020_chip_ops v3020_gpio_ops = {
+static const struct v3020_chip_ops v3020_gpio_ops = {
 	.map_io		= v3020_gpio_map,
 	.unmap_io	= v3020_gpio_unmap,
 	.read_bit	= v3020_gpio_read_bit,
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index a263c10359e1..c78db05e75b1 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -2556,8 +2556,12 @@ static void __dasd_process_request_queue(struct dasd_block *block)
 		return;
 	}
 
-	/* if device ist stopped do not fetch new requests */
-	if (basedev->stopped)
+	/*
+	 * if device is stopped do not fetch new requests
+	 * except failfast is active which will let requests fail
+	 * immediately in __dasd_block_start_head()
+	 */
+	if (basedev->stopped && !(basedev->features & DASD_FEATURE_FAILFAST))
 		return;
 
 	/* Now we try to fetch requests from the request queue */
@@ -3031,6 +3035,7 @@ static void dasd_setup_queue(struct dasd_block *block)
 		max = block->base->discipline->max_blocks << block->s2b_shift;
 	}
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, block->request_queue);
+	block->request_queue->limits.max_dev_sectors = max;
 	blk_queue_logical_block_size(block->request_queue,
 				     block->bp_block);
 	blk_queue_max_hw_sectors(block->request_queue, max);
diff --git a/drivers/s390/block/dasd_alias.c b/drivers/s390/block/dasd_alias.c
index 184b1dbeb554..286782c60da4 100644
--- a/drivers/s390/block/dasd_alias.c
+++ b/drivers/s390/block/dasd_alias.c
@@ -264,8 +264,10 @@ void dasd_alias_disconnect_device_from_lcu(struct dasd_device *device)
 		spin_unlock_irqrestore(&lcu->lock, flags);
 		cancel_work_sync(&lcu->suc_data.worker);
 		spin_lock_irqsave(&lcu->lock, flags);
-		if (device == lcu->suc_data.device)
+		if (device == lcu->suc_data.device) {
+			dasd_put_device(device);
 			lcu->suc_data.device = NULL;
+		}
 	}
 	was_pending = 0;
 	if (device == lcu->ruac_data.device) {
@@ -273,8 +275,10 @@ void dasd_alias_disconnect_device_from_lcu(struct dasd_device *device)
 		was_pending = 1;
 		cancel_delayed_work_sync(&lcu->ruac_data.dwork);
 		spin_lock_irqsave(&lcu->lock, flags);
-		if (device == lcu->ruac_data.device)
+		if (device == lcu->ruac_data.device) {
+			dasd_put_device(device);
 			lcu->ruac_data.device = NULL;
+		}
 	}
 	private->lcu = NULL;
 	spin_unlock_irqrestore(&lcu->lock, flags);
@@ -549,8 +553,10 @@ static void lcu_update_work(struct work_struct *work)
 	if ((rc && (rc != -EOPNOTSUPP)) || (lcu->flags & NEED_UAC_UPDATE)) {
 		DBF_DEV_EVENT(DBF_WARNING, device, "could not update"
 			    " alias data in lcu (rc = %d), retry later", rc);
-		schedule_delayed_work(&lcu->ruac_data.dwork, 30*HZ);
+		if (!schedule_delayed_work(&lcu->ruac_data.dwork, 30*HZ))
+			dasd_put_device(device);
 	} else {
+		dasd_put_device(device);
 		lcu->ruac_data.device = NULL;
 		lcu->flags &= ~UPDATE_PENDING;
 	}
@@ -593,8 +599,10 @@ static int _schedule_lcu_update(struct alias_lcu *lcu,
 	 */
 	if (!usedev)
 		return -EINVAL;
+	dasd_get_device(usedev);
 	lcu->ruac_data.device = usedev;
-	schedule_delayed_work(&lcu->ruac_data.dwork, 0);
+	if (!schedule_delayed_work(&lcu->ruac_data.dwork, 0))
+		dasd_put_device(usedev);
 	return 0;
 }
 
@@ -723,7 +731,7 @@ static int reset_summary_unit_check(struct alias_lcu *lcu,
 	ASCEBC((char *) &cqr->magic, 4);
 	ccw = cqr->cpaddr;
 	ccw->cmd_code = DASD_ECKD_CCW_RSCK;
-	ccw->flags = 0 ;
+	ccw->flags = CCW_FLAG_SLI;
 	ccw->count = 16;
 	ccw->cda = (__u32)(addr_t) cqr->data;
 	((char *)cqr->data)[0] = reason;
@@ -930,6 +938,7 @@ static void summary_unit_check_handling_work(struct work_struct *work)
 	/* 3. read new alias configuration */
 	_schedule_lcu_update(lcu, device);
 	lcu->suc_data.device = NULL;
+	dasd_put_device(device);
 	spin_unlock_irqrestore(&lcu->lock, flags);
 }
 
@@ -989,6 +998,8 @@ void dasd_alias_handle_summary_unit_check(struct dasd_device *device,
 	}
 	lcu->suc_data.reason = reason;
 	lcu->suc_data.device = device;
+	dasd_get_device(device);
 	spin_unlock(&lcu->lock);
-	schedule_work(&lcu->suc_data.worker);
+	if (!schedule_work(&lcu->suc_data.worker))
+		dasd_put_device(device);
 };
diff --git a/drivers/s390/block/dasd_diag.c b/drivers/s390/block/dasd_diag.c
index cb61f300f8b5..277b5c8c825c 100644
--- a/drivers/s390/block/dasd_diag.c
+++ b/drivers/s390/block/dasd_diag.c
@@ -67,7 +67,7 @@ static const u8 DASD_DIAG_CMS1[] = { 0xc3, 0xd4, 0xe2, 0xf1 };/* EBCDIC CMS1 */
  * and function code cmd.
  * In case of an exception return 3. Otherwise return result of bitwise OR of
  * resulting condition code and DIAG return code. */
-static inline int dia250(void *iob, int cmd)
+static inline int __dia250(void *iob, int cmd)
 {
 	register unsigned long reg2 asm ("2") = (unsigned long) iob;
 	typedef union {
@@ -77,7 +77,6 @@ static inline int dia250(void *iob, int cmd)
 	int rc;
 
 	rc = 3;
-	diag_stat_inc(DIAG_STAT_X250);
 	asm volatile(
 		"	diag	2,%2,0x250\n"
 		"0:	ipm	%0\n"
@@ -91,6 +90,12 @@ static inline int dia250(void *iob, int cmd)
 	return rc;
 }
 
+static inline int dia250(void *iob, int cmd)
+{
+	diag_stat_inc(DIAG_STAT_X250);
+	return __dia250(iob, cmd);
+}
+
 /* Initialize block I/O to DIAG device using the specified blocksize and
  * block offset. On success, return zero and set end_block to contain the
  * number of blocks on the device minus the specified offset. Return non-zero
diff --git a/drivers/s390/block/dcssblk.c b/drivers/s390/block/dcssblk.c
index 94a8f4ab57bc..ce7b70181740 100644
--- a/drivers/s390/block/dcssblk.c
+++ b/drivers/s390/block/dcssblk.c
@@ -17,6 +17,7 @@
 #include <linux/completion.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
+#include <linux/pfn_t.h>
 #include <asm/extmem.h>
 #include <asm/io.h>
 
@@ -30,7 +31,7 @@ static void dcssblk_release(struct gendisk *disk, fmode_t mode);
 static blk_qc_t dcssblk_make_request(struct request_queue *q,
 						struct bio *bio);
 static long dcssblk_direct_access(struct block_device *bdev, sector_t secnum,
-			 void __pmem **kaddr, unsigned long *pfn);
+			 void __pmem **kaddr, pfn_t *pfn);
 
 static char dcssblk_segments[DCSSBLK_PARM_LEN] = "\0";
 
@@ -883,20 +884,18 @@ fail:
 
 static long
 dcssblk_direct_access (struct block_device *bdev, sector_t secnum,
-			void __pmem **kaddr, unsigned long *pfn)
+			void __pmem **kaddr, pfn_t *pfn)
 {
 	struct dcssblk_dev_info *dev_info;
 	unsigned long offset, dev_sz;
-	void *addr;
 
 	dev_info = bdev->bd_disk->private_data;
 	if (!dev_info)
 		return -ENODEV;
 	dev_sz = dev_info->end - dev_info->start;
 	offset = secnum * 512;
-	addr = (void *) (dev_info->start + offset);
-	*pfn = virt_to_phys(addr) >> PAGE_SHIFT;
-	*kaddr = (void __pmem *) addr;
+	*kaddr = (void __pmem *) (dev_info->start + offset);
+	*pfn = __pfn_to_pfn_t(PFN_DOWN(dev_info->start + offset), PFN_DEV);
 
 	return dev_sz - offset;
 }
diff --git a/drivers/s390/char/Kconfig b/drivers/s390/char/Kconfig
index eaca3e006301..b3f1c458905f 100644
--- a/drivers/s390/char/Kconfig
+++ b/drivers/s390/char/Kconfig
@@ -78,19 +78,6 @@ config SCLP_VT220_CONSOLE
 	  Include support for using an IBM SCLP VT220-compatible terminal as a
 	  Linux system console.
 
-config SCLP_CPI
-	def_tristate m
-	prompt "Control-Program Identification"
-	depends on S390
-	help
-	  This option enables the hardware console interface for system
-	  identification. This is commonly used for workload management and
-	  gives you a nice name for the system on the service element.
-	  Please select this option as a module since built-in operation is
-	  completely untested.
-	  You should only select this option if you know what you are doing,
-	  need this feature and intend to run your kernel in LPAR.
-
 config SCLP_ASYNC
 	def_tristate m
 	prompt "Support for Call Home via Asynchronous SCLP Records"
@@ -125,6 +112,14 @@ config HMC_DRV
 	  transfer cache size from it's default value 0.5MB to N bytes. If N
 	  is zero, then no caching is performed.
 
+config SCLP_OFB
+	def_bool n
+	prompt "Support for Open-for-Business SCLP Event"
+	depends on S390
+	help
+	  This option enables the Open-for-Business interface to the s390
+	  Service Element.
+
 config S390_TAPE
 	def_tristate m
 	prompt "S/390 tape device support"
diff --git a/drivers/s390/char/Makefile b/drivers/s390/char/Makefile
index 6fa9364d1c07..dd2f7c832e5e 100644
--- a/drivers/s390/char/Makefile
+++ b/drivers/s390/char/Makefile
@@ -16,7 +16,6 @@ obj-$(CONFIG_TN3215) += con3215.o
 obj-$(CONFIG_SCLP_TTY) += sclp_tty.o
 obj-$(CONFIG_SCLP_CONSOLE) += sclp_con.o
 obj-$(CONFIG_SCLP_VT220_TTY) += sclp_vt220.o
-obj-$(CONFIG_SCLP_CPI) += sclp_cpi.o
 obj-$(CONFIG_SCLP_ASYNC) += sclp_async.o
 
 obj-$(CONFIG_VMLOGRDR) += vmlogrdr.o
@@ -30,9 +29,7 @@ obj-$(CONFIG_S390_TAPE_3590) += tape_3590.o
 obj-$(CONFIG_MONREADER) += monreader.o
 obj-$(CONFIG_MONWRITER) += monwriter.o
 obj-$(CONFIG_S390_VMUR) += vmur.o
-
-zcore_mod-objs := sclp_sdias.o zcore.o
-obj-$(CONFIG_CRASH_DUMP) += zcore_mod.o
+obj-$(CONFIG_CRASH_DUMP) += sclp_sdias.o zcore.o
 
 hmcdrv-objs := hmcdrv_mod.o hmcdrv_dev.o hmcdrv_ftp.o hmcdrv_cache.o diag_ftp.o sclp_ftp.o
 obj-$(CONFIG_HMC_DRV) += hmcdrv.o
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index 0fc3fe5fd5b8..7d82bbcb12df 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -922,6 +922,8 @@ static int __init con3215_init(void)
 	spin_lock_init(&raw3215_freelist_lock);
 	for (i = 0; i < NR_3215_REQ; i++) {
 		req = kzalloc(sizeof(struct raw3215_req), GFP_KERNEL | GFP_DMA);
+		if (!req)
+			return -ENOMEM;
 		req->next = raw3215_freelist;
 		raw3215_freelist = req;
 	}
diff --git a/drivers/s390/char/con3270.c b/drivers/s390/char/con3270.c
index 7c511add5aa7..4d7a9badfede 100644
--- a/drivers/s390/char/con3270.c
+++ b/drivers/s390/char/con3270.c
@@ -606,6 +606,8 @@ con3270_init(void)
 		return PTR_ERR(rp);
 
 	condev = kzalloc(sizeof(struct con3270), GFP_KERNEL | GFP_DMA);
+	if (!condev)
+		return -ENOMEM;
 	condev->view.dev = rp;
 
 	condev->read = raw3270_request_alloc(0);
diff --git a/drivers/s390/char/hmcdrv_ftp.c b/drivers/s390/char/hmcdrv_ftp.c
index d4b61d9088fb..8cb7d8fbadd6 100644
--- a/drivers/s390/char/hmcdrv_ftp.c
+++ b/drivers/s390/char/hmcdrv_ftp.c
@@ -37,7 +37,7 @@ struct hmcdrv_ftp_ops {
 static enum hmcdrv_ftp_cmdid hmcdrv_ftp_cmd_getid(const char *cmd, int len);
 static int hmcdrv_ftp_parse(char *cmd, struct hmcdrv_ftp_cmdspec *ftp);
 
-static struct hmcdrv_ftp_ops *hmcdrv_ftp_funcs; /* current operations */
+static const struct hmcdrv_ftp_ops *hmcdrv_ftp_funcs; /* current operations */
 static DEFINE_MUTEX(hmcdrv_ftp_mutex); /* mutex for hmcdrv_ftp_funcs */
 static unsigned hmcdrv_ftp_refcnt; /* start/shutdown reference counter */
 
@@ -290,13 +290,13 @@ ssize_t hmcdrv_ftp_cmd(char __kernel *cmd, loff_t offset,
  */
 int hmcdrv_ftp_startup(void)
 {
-	static struct hmcdrv_ftp_ops hmcdrv_ftp_zvm = {
+	static const struct hmcdrv_ftp_ops hmcdrv_ftp_zvm = {
 		.startup = diag_ftp_startup,
 		.shutdown = diag_ftp_shutdown,
 		.transfer = diag_ftp_cmd
 	};
 
-	static struct hmcdrv_ftp_ops hmcdrv_ftp_lpar = {
+	static const struct hmcdrv_ftp_ops hmcdrv_ftp_lpar = {
 		.startup = sclp_ftp_startup,
 		.shutdown = sclp_ftp_shutdown,
 		.transfer = sclp_ftp_cmd
diff --git a/drivers/s390/char/sclp.c b/drivers/s390/char/sclp.c
index f58bf4c6c3ee..272898225dbb 100644
--- a/drivers/s390/char/sclp.c
+++ b/drivers/s390/char/sclp.c
@@ -579,9 +579,8 @@ sclp_sync_wait(void)
 	old_tick = local_tick_disable();
 	trace_hardirqs_on();
 	__ctl_store(cr0, 0, 0);
-	cr0_sync = cr0;
-	cr0_sync &= 0xffff00a0;
-	cr0_sync |= 0x00000200;
+	cr0_sync = cr0 & ~CR0_IRQ_SUBCLASS_MASK;
+	cr0_sync |= 1UL << (63 - 54);
 	__ctl_load(cr0_sync, 0, 0);
 	__arch_local_irq_stosm(0x01);
 	/* Loop until driver state indicates finished request */
diff --git a/drivers/s390/char/sclp_config.c b/drivers/s390/char/sclp_config.c
index 944156207477..2ced50ccca63 100644
--- a/drivers/s390/char/sclp_config.c
+++ b/drivers/s390/char/sclp_config.c
@@ -11,6 +11,8 @@
 #include <linux/cpu.h>
 #include <linux/device.h>
 #include <linux/workqueue.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
 #include <asm/smp.h>
 
 #include "sclp.h"
@@ -20,8 +22,22 @@ struct conf_mgm_data {
 	u8 ev_qualifier;
 } __attribute__((packed));
 
+#define OFB_DATA_MAX 64
+
+struct sclp_ofb_evbuf {
+	struct evbuf_header header;
+	struct conf_mgm_data cm_data;
+	char ev_data[OFB_DATA_MAX];
+} __packed;
+
+struct sclp_ofb_sccb {
+	struct sccb_header header;
+	struct sclp_ofb_evbuf ofb_evbuf;
+} __packed;
+
 #define EV_QUAL_CPU_CHANGE	1
 #define EV_QUAL_CAP_CHANGE	3
+#define EV_QUAL_OPEN4BUSINESS	5
 
 static struct work_struct sclp_cpu_capability_work;
 static struct work_struct sclp_cpu_change_work;
@@ -63,15 +79,99 @@ static void sclp_conf_receiver_fn(struct evbuf_header *evbuf)
 
 static struct sclp_register sclp_conf_register =
 {
+#ifdef CONFIG_SCLP_OFB
+	.send_mask    = EVTYP_CONFMGMDATA_MASK,
+#endif
 	.receive_mask = EVTYP_CONFMGMDATA_MASK,
 	.receiver_fn  = sclp_conf_receiver_fn,
 };
 
+#ifdef CONFIG_SCLP_OFB
+static int sclp_ofb_send_req(char *ev_data, size_t len)
+{
+	static DEFINE_MUTEX(send_mutex);
+	struct sclp_ofb_sccb *sccb;
+	int rc, response;
+
+	if (len > OFB_DATA_MAX)
+		return -EINVAL;
+	sccb = (struct sclp_ofb_sccb *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+	if (!sccb)
+		return -ENOMEM;
+	/* Setup SCCB for Control-Program Identification */
+	sccb->header.length = sizeof(struct sclp_ofb_sccb);
+	sccb->ofb_evbuf.header.length = sizeof(struct sclp_ofb_evbuf);
+	sccb->ofb_evbuf.header.type = EVTYP_CONFMGMDATA;
+	sccb->ofb_evbuf.cm_data.ev_qualifier = EV_QUAL_OPEN4BUSINESS;
+	memcpy(sccb->ofb_evbuf.ev_data, ev_data, len);
+
+	if (!(sclp_conf_register.sclp_receive_mask & EVTYP_CONFMGMDATA_MASK))
+		pr_warn("SCLP receiver did not register to receive "
+			"Configuration Management Data Events.\n");
+
+	mutex_lock(&send_mutex);
+	rc = sclp_sync_request(SCLP_CMDW_WRITE_EVENT_DATA, sccb);
+	mutex_unlock(&send_mutex);
+	if (rc)
+		goto out;
+	response = sccb->header.response_code;
+	if (response != 0x0020) {
+		pr_err("Open for Business request failed with response code "
+		       "0x%04x\n", response);
+		rc = -EIO;
+	}
+out:
+	free_page((unsigned long)sccb);
+	return rc;
+}
+
+static ssize_t sysfs_ofb_data_write(struct file *filp, struct kobject *kobj,
+				    struct bin_attribute *bin_attr,
+				    char *buf, loff_t off, size_t count)
+{
+	int rc;
+
+	rc = sclp_ofb_send_req(buf, count);
+	return rc ?: count;
+}
+
+static struct bin_attribute ofb_bin_attr = {
+	.attr = {
+		.name = "event_data",
+		.mode = S_IWUSR,
+	},
+	.write = sysfs_ofb_data_write,
+};
+#endif
+
+static int __init sclp_ofb_setup(void)
+{
+#ifdef CONFIG_SCLP_OFB
+	struct kset *ofb_kset;
+	int rc;
+
+	ofb_kset = kset_create_and_add("ofb", NULL, firmware_kobj);
+	if (!ofb_kset)
+		return -ENOMEM;
+	rc = sysfs_create_bin_file(&ofb_kset->kobj, &ofb_bin_attr);
+	if (rc) {
+		kset_unregister(ofb_kset);
+		return rc;
+	}
+#endif
+	return 0;
+}
+
 static int __init sclp_conf_init(void)
 {
+	int rc;
+
 	INIT_WORK(&sclp_cpu_capability_work, sclp_cpu_capability_notify);
 	INIT_WORK(&sclp_cpu_change_work, sclp_cpu_change_notify);
-	return sclp_register(&sclp_conf_register);
+	rc = sclp_register(&sclp_conf_register);
+	if (rc)
+		return rc;
+	return sclp_ofb_setup();
 }
 
 __initcall(sclp_conf_init);
diff --git a/drivers/s390/char/sclp_cpi.c b/drivers/s390/char/sclp_cpi.c
deleted file mode 100644
index d70d8c20229c..000000000000
--- a/drivers/s390/char/sclp_cpi.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- *    SCLP control programm identification
- *
- *    Copyright IBM Corp. 2001, 2007
- *    Author(s): Martin Peschke <mpeschke@de.ibm.com>
- *		 Michael Ernst <mernst@de.ibm.com>
- */
-
-#include <linux/kmod.h>
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/version.h>
-#include "sclp_cpi_sys.h"
-
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Identify this operating system instance "
-		   "to the System z hardware");
-MODULE_AUTHOR("Martin Peschke <mpeschke@de.ibm.com>, "
-	      "Michael Ernst <mernst@de.ibm.com>");
-
-static char *system_name = "";
-static char *sysplex_name = "";
-
-module_param(system_name, charp, 0);
-MODULE_PARM_DESC(system_name, "e.g. hostname - max. 8 characters");
-module_param(sysplex_name, charp, 0);
-MODULE_PARM_DESC(sysplex_name, "if applicable - max. 8 characters");
-
-static int __init cpi_module_init(void)
-{
-	return sclp_cpi_set_data(system_name, sysplex_name, "LINUX",
-				 LINUX_VERSION_CODE);
-}
-
-static void __exit cpi_module_exit(void)
-{
-}
-
-module_init(cpi_module_init);
-module_exit(cpi_module_exit);
diff --git a/drivers/s390/char/sclp_early.c b/drivers/s390/char/sclp_early.c
index 7bc6df3100ef..6804354c42bd 100644
--- a/drivers/s390/char/sclp_early.c
+++ b/drivers/s390/char/sclp_early.c
@@ -40,10 +40,14 @@ struct read_info_sccb {
 	u8	fac85;			/* 85 */
 	u8	_pad_86[91 - 86];	/* 86-90 */
 	u8	flags;			/* 91 */
-	u8	_pad_92[100 - 92];	/* 92-99 */
+	u8	_pad_92[99 - 92];	/* 92-98 */
+	u8	hamaxpow;		/* 99 */
 	u32	rnsize2;		/* 100-103 */
 	u64	rnmax2;			/* 104-111 */
-	u8	_pad_112[120 - 112];	/* 112-119 */
+	u8	_pad_112[116 - 112];	/* 112-115 */
+	u8	fac116;			/* 116 */
+	u8	_pad_117[119 - 117];	/* 117-118 */
+	u8	fac119;			/* 119 */
 	u16	hcpua;			/* 120-121 */
 	u8	_pad_122[4096 - 122];	/* 122-4095 */
 } __packed __aligned(PAGE_SIZE);
@@ -108,6 +112,8 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 	sclp.facilities = sccb->facilities;
 	sclp.has_sprp = !!(sccb->fac84 & 0x02);
 	sclp.has_core_type = !!(sccb->fac84 & 0x01);
+	sclp.has_esca = !!(sccb->fac116 & 0x08);
+	sclp.has_hvs = !!(sccb->fac119 & 0x80);
 	if (sccb->fac85 & 0x02)
 		S390_lowcore.machine_flags |= MACHINE_FLAG_ESOP;
 	sclp.rnmax = sccb->rnmax ? sccb->rnmax : sccb->rnmax2;
@@ -115,6 +121,11 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 	sclp.rzm <<= 20;
 	sclp.ibc = sccb->ibc;
 
+	if (sccb->hamaxpow && sccb->hamaxpow < 64)
+		sclp.hamax = (1UL << sccb->hamaxpow) - 1;
+	else
+		sclp.hamax = U64_MAX;
+
 	if (!sccb->hcpua) {
 		if (MACHINE_IS_VM)
 			sclp.max_cores = 64;
@@ -131,6 +142,7 @@ static void __init sclp_facilities_detect(struct read_info_sccb *sccb)
 			continue;
 		sclp.has_siif = cpue->siif;
 		sclp.has_sigpif = cpue->sigpif;
+		sclp.has_sief2 = cpue->sief2;
 		break;
 	}
 
diff --git a/drivers/s390/char/vmcp.c b/drivers/s390/char/vmcp.c
index 0fdedadff7bc..2a67b496a9e2 100644
--- a/drivers/s390/char/vmcp.c
+++ b/drivers/s390/char/vmcp.c
@@ -88,14 +88,9 @@ vmcp_write(struct file *file, const char __user *buff, size_t count,
 
 	if (count > 240)
 		return -EINVAL;
-	cmd = kmalloc(count + 1, GFP_KERNEL);
-	if (!cmd)
-		return -ENOMEM;
-	if (copy_from_user(cmd, buff, count)) {
-		kfree(cmd);
-		return -EFAULT;
-	}
-	cmd[count] = '\0';
+	cmd = memdup_user_nul(buff, count);
+	if (IS_ERR(cmd))
+		return PTR_ERR(cmd);
 	session = file->private_data;
 	if (mutex_lock_interruptible(&session->mutex)) {
 		kfree(cmd);
diff --git a/drivers/s390/char/vmur.c b/drivers/s390/char/vmur.c
index 0efb27f6f199..6c30e93ab8fa 100644
--- a/drivers/s390/char/vmur.c
+++ b/drivers/s390/char/vmur.c
@@ -782,24 +782,11 @@ static int ur_release(struct inode *inode, struct file *file)
 
 static loff_t ur_llseek(struct file *file, loff_t offset, int whence)
 {
-	loff_t newpos;
-
 	if ((file->f_flags & O_ACCMODE) != O_RDONLY)
 		return -ESPIPE; /* seek allowed only for reader */
 	if (offset % PAGE_SIZE)
 		return -ESPIPE; /* only multiples of 4K allowed */
-	switch (whence) {
-	case 0: /* SEEK_SET */
-		newpos = offset;
-		break;
-	case 1: /* SEEK_CUR */
-		newpos = file->f_pos + offset;
-		break;
-	default:
-		return -EINVAL;
-	}
-	file->f_pos = newpos;
-	return newpos;
+	return no_seek_end_llseek(file, offset, whence);
 }
 
 static const struct file_operations ur_fops = {
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 823f41fc4bbd..5043ecfa1fbc 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -28,13 +28,12 @@
 #include <asm/processor.h>
 #include <asm/irqflags.h>
 #include <asm/checksum.h>
+#include <asm/os_info.h>
 #include <asm/switch_to.h>
 #include "sclp.h"
 
 #define TRACE(x...) debug_sprintf_event(zcore_dbf, 1, x)
 
-#define TO_USER		1
-#define TO_KERNEL	0
 #define CHUNK_INFO_SIZE	34 /* 2 16-byte char, each followed by blank */
 
 enum arch_id {
@@ -42,241 +41,93 @@ enum arch_id {
 	ARCH_S390X	= 1,
 };
 
-/* dump system info */
-
-struct sys_info {
-	enum arch_id	 arch;
-	unsigned long	 sa_base;
-	u32		 sa_size;
-	int		 cpu_map[NR_CPUS];
-	unsigned long	 mem_size;
-	struct save_area lc_mask;
-};
-
 struct ipib_info {
 	unsigned long	ipib;
 	u32		checksum;
 }  __attribute__((packed));
 
-static struct sys_info sys_info;
 static struct debug_info *zcore_dbf;
 static int hsa_available;
 static struct dentry *zcore_dir;
-static struct dentry *zcore_file;
 static struct dentry *zcore_memmap_file;
 static struct dentry *zcore_reipl_file;
 static struct dentry *zcore_hsa_file;
 static struct ipl_parameter_block *ipl_block;
 
+static char hsa_buf[PAGE_SIZE] __aligned(PAGE_SIZE);
+
 /*
- * Copy memory from HSA to kernel or user memory (not reentrant):
+ * Copy memory from HSA to user memory (not reentrant):
  *
- * @dest:  Kernel or user buffer where memory should be copied to
+ * @dest:  User buffer where memory should be copied to
  * @src:   Start address within HSA where data should be copied
  * @count: Size of buffer, which should be copied
- * @mode:  Either TO_KERNEL or TO_USER
  */
-int memcpy_hsa(void *dest, unsigned long src, size_t count, int mode)
+int memcpy_hsa_user(void __user *dest, unsigned long src, size_t count)
 {
-	int offs, blk_num;
-	static char buf[PAGE_SIZE] __attribute__((__aligned__(PAGE_SIZE)));
+	unsigned long offset, bytes;
 
 	if (!hsa_available)
 		return -ENODATA;
-	if (count == 0)
-		return 0;
 
-	/* copy first block */
-	offs = 0;
-	if ((src % PAGE_SIZE) != 0) {
-		blk_num = src / PAGE_SIZE + 2;
-		if (sclp_sdias_copy(buf, blk_num, 1)) {
+	while (count) {
+		if (sclp_sdias_copy(hsa_buf, src / PAGE_SIZE + 2, 1)) {
 			TRACE("sclp_sdias_copy() failed\n");
 			return -EIO;
 		}
-		offs = min((PAGE_SIZE - (src % PAGE_SIZE)), count);
-		if (mode == TO_USER) {
-			if (copy_to_user((__force __user void*) dest,
-					 buf + (src % PAGE_SIZE), offs))
-				return -EFAULT;
-		} else
-			memcpy(dest, buf + (src % PAGE_SIZE), offs);
-	}
-	if (offs == count)
-		goto out;
-
-	/* copy middle */
-	for (; (offs + PAGE_SIZE) <= count; offs += PAGE_SIZE) {
-		blk_num = (src + offs) / PAGE_SIZE + 2;
-		if (sclp_sdias_copy(buf, blk_num, 1)) {
-			TRACE("sclp_sdias_copy() failed\n");
-			return -EIO;
-		}
-		if (mode == TO_USER) {
-			if (copy_to_user((__force __user void*) dest + offs,
-					 buf, PAGE_SIZE))
-				return -EFAULT;
-		} else
-			memcpy(dest + offs, buf, PAGE_SIZE);
-	}
-	if (offs == count)
-		goto out;
-
-	/* copy last block */
-	blk_num = (src + offs) / PAGE_SIZE + 2;
-	if (sclp_sdias_copy(buf, blk_num, 1)) {
-		TRACE("sclp_sdias_copy() failed\n");
-		return -EIO;
-	}
-	if (mode == TO_USER) {
-		if (copy_to_user((__force __user void*) dest + offs, buf,
-				 count - offs))
+		offset = src % PAGE_SIZE;
+		bytes = min(PAGE_SIZE - offset, count);
+		if (copy_to_user(dest, hsa_buf + offset, bytes))
 			return -EFAULT;
-	} else
-		memcpy(dest + offs, buf, count - offs);
-out:
-	return 0;
-}
-
-static int memcpy_hsa_user(void __user *dest, unsigned long src, size_t count)
-{
-	return memcpy_hsa((void __force *) dest, src, count, TO_USER);
-}
-
-static int memcpy_hsa_kernel(void *dest, unsigned long src, size_t count)
-{
-	return memcpy_hsa(dest, src, count, TO_KERNEL);
-}
-
-static int __init init_cpu_info(enum arch_id arch)
-{
-	struct save_area_ext *sa_ext;
-
-	/* get info for boot cpu from lowcore, stored in the HSA */
-
-	sa_ext = dump_save_areas.areas[0];
-	if (!sa_ext)
-		return -ENOMEM;
-	if (memcpy_hsa_kernel(&sa_ext->sa, sys_info.sa_base,
-			      sys_info.sa_size) < 0) {
-		TRACE("could not copy from HSA\n");
-		kfree(sa_ext);
-		return -EIO;
+		src += bytes;
+		dest += bytes;
+		count -= bytes;
 	}
-	if (MACHINE_HAS_VX)
-		save_vx_regs_safe(sa_ext->vx_regs);
 	return 0;
 }
 
-static DEFINE_MUTEX(zcore_mutex);
-
-#define DUMP_VERSION	0x5
-#define DUMP_MAGIC	0xa8190173618f23fdULL
-#define DUMP_ARCH_S390X	2
-#define DUMP_ARCH_S390	1
-#define HEADER_SIZE	4096
-
-/* dump header dumped according to s390 crash dump format */
-
-struct zcore_header {
-	u64 magic;
-	u32 version;
-	u32 header_size;
-	u32 dump_level;
-	u32 page_size;
-	u64 mem_size;
-	u64 mem_start;
-	u64 mem_end;
-	u32 num_pages;
-	u32 pad1;
-	u64 tod;
-	struct cpuid cpu_id;
-	u32 arch_id;
-	u32 volnr;
-	u32 build_arch;
-	u64 rmem_size;
-	u8 mvdump;
-	u16 cpu_cnt;
-	u16 real_cpu_cnt;
-	u8 end_pad1[0x200-0x061];
-	u64 mvdump_sign;
-	u64 mvdump_zipl_time;
-	u8 end_pad2[0x800-0x210];
-	u32 lc_vec[512];
-} __attribute__((packed,__aligned__(16)));
-
-static struct zcore_header zcore_header = {
-	.magic		= DUMP_MAGIC,
-	.version	= DUMP_VERSION,
-	.header_size	= 4096,
-	.dump_level	= 0,
-	.page_size	= PAGE_SIZE,
-	.mem_start	= 0,
-	.build_arch	= DUMP_ARCH_S390X,
-};
-
 /*
- * Copy lowcore info to buffer. Use map in order to copy only register parts.
+ * Copy memory from HSA to kernel memory (not reentrant):
  *
- * @buf:    User buffer
- * @sa:     Pointer to save area
- * @sa_off: Offset in save area to copy
- * @len:    Number of bytes to copy
+ * @dest:  Kernel or user buffer where memory should be copied to
+ * @src:   Start address within HSA where data should be copied
+ * @count: Size of buffer, which should be copied
  */
-static int copy_lc(void __user *buf, void *sa, int sa_off, int len)
+int memcpy_hsa_kernel(void *dest, unsigned long src, size_t count)
 {
-	int i;
-	char *lc_mask = (char*)&sys_info.lc_mask;
+	unsigned long offset, bytes;
 
-	for (i = 0; i < len; i++) {
-		if (!lc_mask[i + sa_off])
-			continue;
-		if (copy_to_user(buf + i, sa + sa_off + i, 1))
-			return -EFAULT;
+	if (!hsa_available)
+		return -ENODATA;
+
+	while (count) {
+		if (sclp_sdias_copy(hsa_buf, src / PAGE_SIZE + 2, 1)) {
+			TRACE("sclp_sdias_copy() failed\n");
+			return -EIO;
+		}
+		offset = src % PAGE_SIZE;
+		bytes = min(PAGE_SIZE - offset, count);
+		memcpy(dest, hsa_buf + offset, bytes);
+		src += bytes;
+		dest += bytes;
+		count -= bytes;
 	}
 	return 0;
 }
 
-/*
- * Copy lowcores info to memory, if necessary
- *
- * @buf:   User buffer
- * @addr:  Start address of buffer in dump memory
- * @count: Size of buffer
- */
-static int zcore_add_lc(char __user *buf, unsigned long start, size_t count)
+static int __init init_cpu_info(void)
 {
-	unsigned long end;
-	int i;
-
-	if (count == 0)
-		return 0;
+	struct save_area *sa;
 
-	end = start + count;
-	for (i = 0; i < dump_save_areas.count; i++) {
-		unsigned long cp_start, cp_end; /* copy range */
-		unsigned long sa_start, sa_end; /* save area range */
-		unsigned long prefix;
-		unsigned long sa_off, len, buf_off;
-		struct save_area *save_area = &dump_save_areas.areas[i]->sa;
-
-		prefix = save_area->pref_reg;
-		sa_start = prefix + sys_info.sa_base;
-		sa_end = prefix + sys_info.sa_base + sys_info.sa_size;
-
-		if ((end < sa_start) || (start > sa_end))
-			continue;
-		cp_start = max(start, sa_start);
-		cp_end = min(end, sa_end);
-
-		buf_off = cp_start - start;
-		sa_off = cp_start - sa_start;
-		len = cp_end - cp_start;
-
-		TRACE("copy_lc for: %lx\n", start);
-		if (copy_lc(buf + buf_off, save_area, sa_off, len))
-			return -EFAULT;
+	/* get info for boot cpu from lowcore, stored in the HSA */
+	sa = save_area_boot_cpu();
+	if (!sa)
+		return -ENOMEM;
+	if (memcpy_hsa_kernel(hsa_buf, __LC_FPREGS_SAVE_AREA, 512) < 0) {
+		TRACE("could not copy from HSA\n");
+		return -EIO;
 	}
+	save_area_add_regs(sa, hsa_buf); /* vx registers are saved in smp.c */
 	return 0;
 }
 
@@ -289,126 +140,6 @@ static void release_hsa(void)
 	hsa_available = 0;
 }
 
-/*
- * Read routine for zcore character device
- * First 4K are dump header
- * Next 32MB are HSA Memory
- * Rest is read from absolute Memory
- */
-static ssize_t zcore_read(struct file *file, char __user *buf, size_t count,
-			  loff_t *ppos)
-{
-	unsigned long mem_start; /* Start address in memory */
-	size_t mem_offs;	 /* Offset in dump memory */
-	size_t hdr_count;	 /* Size of header part of output buffer */
-	size_t size;
-	int rc;
-
-	mutex_lock(&zcore_mutex);
-
-	if (*ppos > (sys_info.mem_size + HEADER_SIZE)) {
-		rc = -EINVAL;
-		goto fail;
-	}
-
-	count = min(count, (size_t) (sys_info.mem_size + HEADER_SIZE - *ppos));
-
-	/* Copy dump header */
-	if (*ppos < HEADER_SIZE) {
-		size = min(count, (size_t) (HEADER_SIZE - *ppos));
-		if (copy_to_user(buf, &zcore_header + *ppos, size)) {
-			rc = -EFAULT;
-			goto fail;
-		}
-		hdr_count = size;
-		mem_start = 0;
-	} else {
-		hdr_count = 0;
-		mem_start = *ppos - HEADER_SIZE;
-	}
-
-	mem_offs = 0;
-
-	/* Copy from HSA data */
-	if (*ppos < sclp.hsa_size + HEADER_SIZE) {
-		size = min((count - hdr_count),
-			   (size_t) (sclp.hsa_size - mem_start));
-		rc = memcpy_hsa_user(buf + hdr_count, mem_start, size);
-		if (rc)
-			goto fail;
-
-		mem_offs += size;
-	}
-
-	/* Copy from real mem */
-	size = count - mem_offs - hdr_count;
-	rc = copy_to_user_real(buf + hdr_count + mem_offs,
-			       (void *) mem_start + mem_offs, size);
-	if (rc)
-		goto fail;
-
-	/*
-	 * Since s390 dump analysis tools like lcrash or crash
-	 * expect register sets in the prefix pages of the cpus,
-	 * we copy them into the read buffer, if necessary.
-	 * buf + hdr_count: Start of memory part of output buffer
-	 * mem_start: Start memory address to copy from
-	 * count - hdr_count: Size of memory area to copy
-	 */
-	if (zcore_add_lc(buf + hdr_count, mem_start, count - hdr_count)) {
-		rc = -EFAULT;
-		goto fail;
-	}
-	*ppos += count;
-fail:
-	mutex_unlock(&zcore_mutex);
-	return (rc < 0) ? rc : count;
-}
-
-static int zcore_open(struct inode *inode, struct file *filp)
-{
-	if (!hsa_available)
-		return -ENODATA;
-	else
-		return capable(CAP_SYS_RAWIO) ? 0 : -EPERM;
-}
-
-static int zcore_release(struct inode *inode, struct file *filep)
-{
-	if (hsa_available)
-		release_hsa();
-	return 0;
-}
-
-static loff_t zcore_lseek(struct file *file, loff_t offset, int orig)
-{
-	loff_t rc;
-
-	mutex_lock(&zcore_mutex);
-	switch (orig) {
-	case 0:
-		file->f_pos = offset;
-		rc = file->f_pos;
-		break;
-	case 1:
-		file->f_pos += offset;
-		rc = file->f_pos;
-		break;
-	default:
-		rc = -EINVAL;
-	}
-	mutex_unlock(&zcore_mutex);
-	return rc;
-}
-
-static const struct file_operations zcore_fops = {
-	.owner		= THIS_MODULE,
-	.llseek		= zcore_lseek,
-	.read		= zcore_read,
-	.open		= zcore_open,
-	.release	= zcore_release,
-};
-
 static ssize_t zcore_memmap_read(struct file *filp, char __user *buf,
 				 size_t count, loff_t *ppos)
 {
@@ -512,50 +243,6 @@ static const struct file_operations zcore_hsa_fops = {
 	.llseek		= no_llseek,
 };
 
-static void __init set_lc_mask(struct save_area *map)
-{
-	memset(&map->fp_regs, 0xff, sizeof(map->fp_regs));
-	memset(&map->gp_regs, 0xff, sizeof(map->gp_regs));
-	memset(&map->psw, 0xff, sizeof(map->psw));
-	memset(&map->pref_reg, 0xff, sizeof(map->pref_reg));
-	memset(&map->fp_ctrl_reg, 0xff, sizeof(map->fp_ctrl_reg));
-	memset(&map->tod_reg, 0xff, sizeof(map->tod_reg));
-	memset(&map->timer, 0xff, sizeof(map->timer));
-	memset(&map->clk_cmp, 0xff, sizeof(map->clk_cmp));
-	memset(&map->acc_regs, 0xff, sizeof(map->acc_regs));
-	memset(&map->ctrl_regs, 0xff, sizeof(map->ctrl_regs));
-}
-
-/*
- * Initialize dump globals for a given architecture
- */
-static int __init sys_info_init(enum arch_id arch, unsigned long mem_end)
-{
-	int rc;
-
-	switch (arch) {
-	case ARCH_S390X:
-		pr_alert("DETECTED 'S390X (64 bit) OS'\n");
-		break;
-	case ARCH_S390:
-		pr_alert("DETECTED 'S390 (32 bit) OS'\n");
-		break;
-	default:
-		pr_alert("0x%x is an unknown architecture.\n",arch);
-		return -EINVAL;
-	}
-	sys_info.sa_base = SAVE_AREA_BASE;
-	sys_info.sa_size = sizeof(struct save_area);
-	sys_info.arch = arch;
-	set_lc_mask(&sys_info.lc_mask);
-	rc = init_cpu_info(arch);
-	if (rc)
-		return rc;
-	sys_info.mem_size = mem_end;
-
-	return 0;
-}
-
 static int __init check_sdias(void)
 {
 	if (!sclp.hsa_size) {
@@ -565,43 +252,6 @@ static int __init check_sdias(void)
 	return 0;
 }
 
-static int __init get_mem_info(unsigned long *mem, unsigned long *end)
-{
-	struct memblock_region *reg;
-
-	for_each_memblock(memory, reg) {
-		*mem += reg->size;
-		*end = max_t(unsigned long, *end, reg->base + reg->size);
-	}
-	return 0;
-}
-
-static void __init zcore_header_init(int arch, struct zcore_header *hdr,
-				     unsigned long mem_size)
-{
-	u32 prefix;
-	int i;
-
-	if (arch == ARCH_S390X)
-		hdr->arch_id = DUMP_ARCH_S390X;
-	else
-		hdr->arch_id = DUMP_ARCH_S390;
-	hdr->mem_size = mem_size;
-	hdr->rmem_size = mem_size;
-	hdr->mem_end = sys_info.mem_size;
-	hdr->num_pages = mem_size / PAGE_SIZE;
-	hdr->tod = get_tod_clock();
-	get_cpu_id(&hdr->cpu_id);
-	for (i = 0; i < dump_save_areas.count; i++) {
-		prefix = dump_save_areas.areas[i]->sa.pref_reg;
-		hdr->real_cpu_cnt++;
-		if (!prefix)
-			continue;
-		hdr->lc_vec[hdr->cpu_cnt] = prefix;
-		hdr->cpu_cnt++;
-	}
-}
-
 /*
  * Provide IPL parameter information block from either HSA or memory
  * for future reipl
@@ -634,11 +284,9 @@ static int __init zcore_reipl_init(void)
 
 static int __init zcore_init(void)
 {
-	unsigned long mem_size, mem_end;
 	unsigned char arch;
 	int rc;
 
-	mem_size = mem_end = 0;
 	if (ipl_info.type != IPL_TYPE_FCP_DUMP)
 		return -ENODATA;
 	if (OLDMEM_BASE)
@@ -672,14 +320,10 @@ static int __init zcore_init(void)
 		goto fail;
 	}
 
-	rc = get_mem_info(&mem_size, &mem_end);
-	if (rc)
-		goto fail;
-
-	rc = sys_info_init(arch, mem_end);
+	pr_alert("DETECTED 'S390X (64 bit) OS'\n");
+	rc = init_cpu_info();
 	if (rc)
 		goto fail;
-	zcore_header_init(arch, &zcore_header, mem_size);
 
 	rc = zcore_reipl_init();
 	if (rc)
@@ -690,17 +334,11 @@ static int __init zcore_init(void)
 		rc = -ENOMEM;
 		goto fail;
 	}
-	zcore_file = debugfs_create_file("mem", S_IRUSR, zcore_dir, NULL,
-					 &zcore_fops);
-	if (!zcore_file) {
-		rc = -ENOMEM;
-		goto fail_dir;
-	}
 	zcore_memmap_file = debugfs_create_file("memmap", S_IRUSR, zcore_dir,
 						NULL, &zcore_memmap_fops);
 	if (!zcore_memmap_file) {
 		rc = -ENOMEM;
-		goto fail_file;
+		goto fail_dir;
 	}
 	zcore_reipl_file = debugfs_create_file("reipl", S_IRUSR, zcore_dir,
 						NULL, &zcore_reipl_fops);
@@ -720,8 +358,6 @@ fail_reipl_file:
 	debugfs_remove(zcore_reipl_file);
 fail_memmap_file:
 	debugfs_remove(zcore_memmap_file);
-fail_file:
-	debugfs_remove(zcore_file);
 fail_dir:
 	debugfs_remove(zcore_dir);
 fail:
@@ -737,7 +373,6 @@ static void __exit zcore_exit(void)
 	debugfs_remove(zcore_hsa_file);
 	debugfs_remove(zcore_reipl_file);
 	debugfs_remove(zcore_memmap_file);
-	debugfs_remove(zcore_file);
 	debugfs_remove(zcore_dir);
 	diag308(DIAG308_REL_HSA, NULL);
 }
diff --git a/drivers/s390/cio/Makefile b/drivers/s390/cio/Makefile
index 8c4a386e97f6..3ab9aedeb84a 100644
--- a/drivers/s390/cio/Makefile
+++ b/drivers/s390/cio/Makefile
@@ -2,8 +2,11 @@
 # Makefile for the S/390 common i/o drivers
 #
 
+# The following is required for define_trace.h to find ./trace.h
+CFLAGS_trace.o := -I$(src)
+
 obj-y += airq.o blacklist.o chsc.o cio.o css.o chp.o idset.o isc.o \
-	fcx.o itcw.o crw.o ccwreq.o
+	fcx.o itcw.o crw.o ccwreq.o trace.o ioasm.o
 ccw_device-objs += device.o device_fsm.o device_ops.o
 ccw_device-objs += device_id.o device_pgid.o device_status.o
 obj-y += ccw_device.o cmf.o
diff --git a/drivers/s390/cio/airq.c b/drivers/s390/cio/airq.c
index 56eb4ee4deba..99b5db469097 100644
--- a/drivers/s390/cio/airq.c
+++ b/drivers/s390/cio/airq.c
@@ -89,6 +89,7 @@ static irqreturn_t do_airq_interrupt(int irq, void *dummy)
 
 	set_cpu_flag(CIF_NOHZ_DELAY);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
+	trace_s390_cio_adapter_int(tpi_info);
 	head = &airq_lists[tpi_info->isc];
 	rcu_read_lock();
 	hlist_for_each_entry_rcu(airq, head, list)
diff --git a/drivers/s390/cio/chp.c b/drivers/s390/cio/chp.c
index c692dfebd0ba..50597f9522fe 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -139,11 +139,11 @@ static ssize_t chp_measurement_chars_read(struct file *filp,
 
 	device = container_of(kobj, struct device, kobj);
 	chp = to_channelpath(device);
-	if (!chp->cmg_chars)
+	if (chp->cmg == -1)
 		return 0;
 
-	return memory_read_from_buffer(buf, count, &off,
-				chp->cmg_chars, sizeof(struct cmg_chars));
+	return memory_read_from_buffer(buf, count, &off, &chp->cmg_chars,
+				       sizeof(chp->cmg_chars));
 }
 
 static struct bin_attribute chp_measurement_chars_attr = {
@@ -416,7 +416,8 @@ static void chp_release(struct device *dev)
  * chp_update_desc - update channel-path description
  * @chp - channel-path
  *
- * Update the channel-path description of the specified channel-path.
+ * Update the channel-path description of the specified channel-path
+ * including channel measurement related information.
  * Return zero on success, non-zero otherwise.
  */
 int chp_update_desc(struct channel_path *chp)
@@ -428,8 +429,10 @@ int chp_update_desc(struct channel_path *chp)
 		return rc;
 
 	rc = chsc_determine_fmt1_channel_path_desc(chp->chpid, &chp->desc_fmt1);
+	if (rc)
+		return rc;
 
-	return rc;
+	return chsc_get_channel_measurement_chars(chp);
 }
 
 /**
@@ -466,14 +469,6 @@ int chp_new(struct chp_id chpid)
 		ret = -ENODEV;
 		goto out_free;
 	}
-	/* Get channel-measurement characteristics. */
-	if (css_chsc_characteristics.scmc && css_chsc_characteristics.secm) {
-		ret = chsc_get_channel_measurement_chars(chp);
-		if (ret)
-			goto out_free;
-	} else {
-		chp->cmg = -1;
-	}
 	dev_set_name(&chp->dev, "chp%x.%02x", chpid.cssid, chpid.id);
 
 	/* make it known to the system */
diff --git a/drivers/s390/cio/chp.h b/drivers/s390/cio/chp.h
index 4efd5b867cc3..af0232290dc4 100644
--- a/drivers/s390/cio/chp.h
+++ b/drivers/s390/cio/chp.h
@@ -48,7 +48,7 @@ struct channel_path {
 	/* Channel-measurement related stuff: */
 	int cmg;
 	int shared;
-	void *cmg_chars;
+	struct cmg_chars cmg_chars;
 };
 
 /* Return channel_path struct for given chpid. */
diff --git a/drivers/s390/cio/chsc.c b/drivers/s390/cio/chsc.c
index a831d18596a5..c424c0c7367e 100644
--- a/drivers/s390/cio/chsc.c
+++ b/drivers/s390/cio/chsc.c
@@ -14,6 +14,7 @@
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/device.h>
+#include <linux/mutex.h>
 #include <linux/pci.h>
 
 #include <asm/cio.h>
@@ -224,8 +225,9 @@ out_unreg:
 
 void chsc_chp_offline(struct chp_id chpid)
 {
-	char dbf_txt[15];
+	struct channel_path *chp = chpid_to_chp(chpid);
 	struct chp_link link;
+	char dbf_txt[15];
 
 	sprintf(dbf_txt, "chpr%x.%02x", chpid.cssid, chpid.id);
 	CIO_TRACE_EVENT(2, dbf_txt);
@@ -236,6 +238,11 @@ void chsc_chp_offline(struct chp_id chpid)
 	link.chpid = chpid;
 	/* Wait until previous actions have settled. */
 	css_wait_for_slow_path();
+
+	mutex_lock(&chp->lock);
+	chp_update_desc(chp);
+	mutex_unlock(&chp->lock);
+
 	for_each_subchannel_staged(s390_subchannel_remove_chpid, NULL, &link);
 }
 
@@ -690,8 +697,9 @@ static void chsc_process_crw(struct crw *crw0, struct crw *crw1, int overflow)
 
 void chsc_chp_online(struct chp_id chpid)
 {
-	char dbf_txt[15];
+	struct channel_path *chp = chpid_to_chp(chpid);
 	struct chp_link link;
+	char dbf_txt[15];
 
 	sprintf(dbf_txt, "cadd%x.%02x", chpid.cssid, chpid.id);
 	CIO_TRACE_EVENT(2, dbf_txt);
@@ -701,6 +709,11 @@ void chsc_chp_online(struct chp_id chpid)
 		link.chpid = chpid;
 		/* Wait until previous actions have settled. */
 		css_wait_for_slow_path();
+
+		mutex_lock(&chp->lock);
+		chp_update_desc(chp);
+		mutex_unlock(&chp->lock);
+
 		for_each_subchannel_staged(__s390_process_res_acc, NULL,
 					   &link);
 		css_schedule_reprobe();
@@ -967,22 +980,19 @@ static void
 chsc_initialize_cmg_chars(struct channel_path *chp, u8 cmcv,
 			  struct cmg_chars *chars)
 {
-	struct cmg_chars *cmg_chars;
 	int i, mask;
 
-	cmg_chars = chp->cmg_chars;
 	for (i = 0; i < NR_MEASUREMENT_CHARS; i++) {
 		mask = 0x80 >> (i + 3);
 		if (cmcv & mask)
-			cmg_chars->values[i] = chars->values[i];
+			chp->cmg_chars.values[i] = chars->values[i];
 		else
-			cmg_chars->values[i] = 0;
+			chp->cmg_chars.values[i] = 0;
 	}
 }
 
 int chsc_get_channel_measurement_chars(struct channel_path *chp)
 {
-	struct cmg_chars *cmg_chars;
 	int ccode, ret;
 
 	struct {
@@ -1006,10 +1016,11 @@ int chsc_get_channel_measurement_chars(struct channel_path *chp)
 		u32 data[NR_MEASUREMENT_CHARS];
 	} __attribute__ ((packed)) *scmc_area;
 
-	chp->cmg_chars = NULL;
-	cmg_chars = kmalloc(sizeof(*cmg_chars), GFP_KERNEL);
-	if (!cmg_chars)
-		return -ENOMEM;
+	chp->shared = -1;
+	chp->cmg = -1;
+
+	if (!css_chsc_characteristics.scmc || !css_chsc_characteristics.secm)
+		return 0;
 
 	spin_lock_irq(&chsc_page_lock);
 	memset(chsc_page, 0, PAGE_SIZE);
@@ -1031,25 +1042,19 @@ int chsc_get_channel_measurement_chars(struct channel_path *chp)
 			      scmc_area->response.code);
 		goto out;
 	}
-	if (scmc_area->not_valid) {
-		chp->cmg = -1;
-		chp->shared = -1;
+	if (scmc_area->not_valid)
 		goto out;
-	}
+
 	chp->cmg = scmc_area->cmg;
 	chp->shared = scmc_area->shared;
 	if (chp->cmg != 2 && chp->cmg != 3) {
 		/* No cmg-dependent data. */
 		goto out;
 	}
-	chp->cmg_chars = cmg_chars;
 	chsc_initialize_cmg_chars(chp, scmc_area->cmcv,
 				  (struct cmg_chars *) &scmc_area->data);
 out:
 	spin_unlock_irq(&chsc_page_lock);
-	if (!chp->cmg_chars)
-		kfree(cmg_chars);
-
 	return ret;
 }
 
diff --git a/drivers/s390/cio/chsc_sch.c b/drivers/s390/cio/chsc_sch.c
index 213159dec89e..b6f12c2bb114 100644
--- a/drivers/s390/cio/chsc_sch.c
+++ b/drivers/s390/cio/chsc_sch.c
@@ -133,7 +133,7 @@ static int chsc_subchannel_prepare(struct subchannel *sch)
 	 * since we don't have a way to clear the subchannel and
 	 * cannot disable it with a request running.
 	 */
-	cc = stsch_err(sch->schid, &schib);
+	cc = stsch(sch->schid, &schib);
 	if (!cc && scsw_stctl(&schib.scsw))
 		return -EAGAIN;
 	return 0;
@@ -185,8 +185,7 @@ static int __init chsc_init_dbfs(void)
 	debug_set_level(chsc_debug_log_id, 2);
 	return 0;
 out:
-	if (chsc_debug_msg_id)
-		debug_unregister(chsc_debug_msg_id);
+	debug_unregister(chsc_debug_msg_id);
 	return -ENOMEM;
 }
 
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 690b8547e828..39a8ae54e9c1 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -41,6 +41,7 @@
 #include "blacklist.h"
 #include "cio_debug.h"
 #include "chp.h"
+#include "trace.h"
 
 debug_info_t *cio_debug_msg_id;
 debug_info_t *cio_debug_trace_id;
@@ -76,12 +77,9 @@ static int __init cio_debug_init(void)
 	return 0;
 
 out_unregister:
-	if (cio_debug_msg_id)
-		debug_unregister(cio_debug_msg_id);
-	if (cio_debug_trace_id)
-		debug_unregister(cio_debug_trace_id);
-	if (cio_debug_crw_id)
-		debug_unregister(cio_debug_crw_id);
+	debug_unregister(cio_debug_msg_id);
+	debug_unregister(cio_debug_trace_id);
+	debug_unregister(cio_debug_crw_id);
 	return -1;
 }
 
@@ -348,18 +346,18 @@ int cio_commit_config(struct subchannel *sch)
 	struct schib schib;
 	struct irb irb;
 
-	if (stsch_err(sch->schid, &schib) || !css_sch_is_valid(&schib))
+	if (stsch(sch->schid, &schib) || !css_sch_is_valid(&schib))
 		return -ENODEV;
 
 	for (retry = 0; retry < 5; retry++) {
 		/* copy desired changes to local schib */
 		cio_apply_config(sch, &schib);
-		ccode = msch_err(sch->schid, &schib);
+		ccode = msch(sch->schid, &schib);
 		if (ccode < 0) /* -EIO if msch gets a program check. */
 			return ccode;
 		switch (ccode) {
 		case 0: /* successful */
-			if (stsch_err(sch->schid, &schib) ||
+			if (stsch(sch->schid, &schib) ||
 			    !css_sch_is_valid(&schib))
 				return -ENODEV;
 			if (cio_check_config(sch, &schib)) {
@@ -394,7 +392,7 @@ int cio_update_schib(struct subchannel *sch)
 {
 	struct schib schib;
 
-	if (stsch_err(sch->schid, &schib) || !css_sch_is_valid(&schib))
+	if (stsch(sch->schid, &schib) || !css_sch_is_valid(&schib))
 		return -ENODEV;
 
 	memcpy(&sch->schib, &schib, sizeof(schib));
@@ -503,7 +501,7 @@ int cio_validate_subchannel(struct subchannel *sch, struct subchannel_id schid)
 	 * If stsch gets an exception, it means the current subchannel set
 	 * is not valid.
 	 */
-	ccode = stsch_err(schid, &sch->schib);
+	ccode = stsch(schid, &sch->schib);
 	if (ccode) {
 		err = (ccode == 3) ? -ENXIO : ccode;
 		goto out;
@@ -542,6 +540,7 @@ static irqreturn_t do_cio_interrupt(int irq, void *dummy)
 
 	set_cpu_flag(CIF_NOHZ_DELAY);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
+	trace_s390_cio_interrupt(tpi_info);
 	irb = this_cpu_ptr(&cio_irb);
 	sch = (struct subchannel *)(unsigned long) tpi_info->intparm;
 	if (!sch) {
@@ -619,7 +618,7 @@ static int cio_test_for_console(struct subchannel_id schid, void *data)
 {
 	struct schib schib;
 
-	if (stsch_err(schid, &schib) != 0)
+	if (stsch(schid, &schib) != 0)
 		return -ENXIO;
 	if ((schib.pmcw.st == SUBCHANNEL_TYPE_IO) && schib.pmcw.dnv &&
 	    (schib.pmcw.dev == console_devno)) {
@@ -638,7 +637,7 @@ static int cio_get_console_sch_no(void)
 	if (console_irq != -1) {
 		/* VM provided us with the irq number of the console. */
 		schid.sch_no = console_irq;
-		if (stsch_err(schid, &schib) != 0 ||
+		if (stsch(schid, &schib) != 0 ||
 		    (schib.pmcw.st != SUBCHANNEL_TYPE_IO) || !schib.pmcw.dnv)
 			return -1;
 		console_devno = schib.pmcw.dev;
@@ -708,10 +707,10 @@ __disable_subchannel_easy(struct subchannel_id schid, struct schib *schib)
 	cc = 0;
 	for (retry=0;retry<3;retry++) {
 		schib->pmcw.ena = 0;
-		cc = msch_err(schid, schib);
+		cc = msch(schid, schib);
 		if (cc)
 			return (cc==3?-ENODEV:-EBUSY);
-		if (stsch_err(schid, schib) || !css_sch_is_valid(schib))
+		if (stsch(schid, schib) || !css_sch_is_valid(schib))
 			return -ENODEV;
 		if (!schib->pmcw.ena)
 			return 0;
@@ -758,7 +757,7 @@ static int stsch_reset(struct subchannel_id schid, struct schib *addr)
 
 	pgm_check_occured = 0;
 	s390_base_pgm_handler_fn = cio_reset_pgm_check_handler;
-	rc = stsch_err(schid, addr);
+	rc = stsch(schid, addr);
 	s390_base_pgm_handler_fn = NULL;
 
 	/* The program check handler could have changed pgm_check_occured. */
@@ -795,7 +794,7 @@ static int __shutdown_subchannel_easy(struct subchannel_id schid, void *data)
 			/* No default clear strategy */
 			break;
 		}
-		stsch_err(schid, &schib);
+		stsch(schid, &schib);
 		__disable_subchannel_easy(schid, &schib);
 	}
 out:
@@ -917,7 +916,7 @@ void reipl_ccw_dev(struct ccw_dev_id *devid)
 {
 	struct subchannel_id uninitialized_var(schid);
 
-	s390_reset_system(NULL, NULL, NULL);
+	s390_reset_system();
 	if (reipl_find_schid(devid, &schid) != 0)
 		panic("IPL Device not found\n");
 	do_reipl_asm(*((__u32*)&schid));
@@ -943,7 +942,7 @@ int __init cio_get_iplinfo(struct cio_iplinfo *iplinfo)
 		if (__chsc_enable_facility(&sda_area, CHSC_SDA_OC_MSS))
 			return -ENODEV;
 	}
-	if (stsch_err(schid, &schib))
+	if (stsch(schid, &schib))
 		return -ENODEV;
 	if (schib.pmcw.st != SUBCHANNEL_TYPE_IO)
 		return -ENODEV;
diff --git a/drivers/s390/cio/cio.h b/drivers/s390/cio/cio.h
index a01376ae1749..93de0b46b489 100644
--- a/drivers/s390/cio/cio.h
+++ b/drivers/s390/cio/cio.h
@@ -45,6 +45,18 @@ struct pmcw {
 				/*  ... in an operand exception.       */
 } __attribute__ ((packed));
 
+/* I/O-Interruption Code as stored by TEST PENDING INTERRUPTION (TPI). */
+struct tpi_info {
+	struct subchannel_id schid;
+	u32 intparm;
+	u32 adapter_IO:1;
+	u32 :1;
+	u32 isc:3;
+	u32 :27;
+	u32 type:3;
+	u32 :12;
+} __packed __aligned(4);
+
 /* Target SCHIB configuration. */
 struct schib_config {
 	u64 mba;
diff --git a/drivers/s390/cio/crw.c b/drivers/s390/cio/crw.c
index 0f8a25f98b10..3d3cd402b376 100644
--- a/drivers/s390/cio/crw.c
+++ b/drivers/s390/cio/crw.c
@@ -14,6 +14,7 @@
 #include <linux/wait.h>
 #include <asm/crw.h>
 #include <asm/ctl_reg.h>
+#include "ioasm.h"
 
 static DEFINE_MUTEX(crw_handler_mutex);
 static crw_handler_t crw_handlers[NR_RSCS];
diff --git a/drivers/s390/cio/css.c b/drivers/s390/cio/css.c
index 489e703dc82d..3d2b20ee613f 100644
--- a/drivers/s390/cio/css.c
+++ b/drivers/s390/cio/css.c
@@ -390,7 +390,7 @@ static int css_evaluate_new_subchannel(struct subchannel_id schid, int slow)
 		/* Will be done on the slow path. */
 		return -EAGAIN;
 	}
-	if (stsch_err(schid, &schib)) {
+	if (stsch(schid, &schib)) {
 		/* Subchannel is not provided. */
 		return -ENXIO;
 	}
diff --git a/drivers/s390/cio/device_fsm.c b/drivers/s390/cio/device_fsm.c
index 92e03b42e661..8327d47e08b6 100644
--- a/drivers/s390/cio/device_fsm.c
+++ b/drivers/s390/cio/device_fsm.c
@@ -44,7 +44,7 @@ static void ccw_timeout_log(struct ccw_device *cdev)
 	sch = to_subchannel(cdev->dev.parent);
 	private = to_io_private(sch);
 	orb = &private->orb;
-	cc = stsch_err(sch->schid, &schib);
+	cc = stsch(sch->schid, &schib);
 
 	printk(KERN_WARNING "cio: ccw device timeout occurred at %llx, "
 	       "device information:\n", get_tod_clock());
diff --git a/drivers/s390/cio/io_sch.h b/drivers/s390/cio/io_sch.h
index b108f4a5c7dd..8975060af96c 100644
--- a/drivers/s390/cio/io_sch.h
+++ b/drivers/s390/cio/io_sch.h
@@ -169,49 +169,4 @@ struct ccw_device_private {
 	enum interruption_class int_class;
 };
 
-static inline int rsch(struct subchannel_id schid)
-{
-	register struct subchannel_id reg1 asm("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	rsch\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode)
-		: "d" (reg1)
-		: "cc", "memory");
-	return ccode;
-}
-
-static inline int hsch(struct subchannel_id schid)
-{
-	register struct subchannel_id reg1 asm("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	hsch\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode)
-		: "d" (reg1)
-		: "cc");
-	return ccode;
-}
-
-static inline int xsch(struct subchannel_id schid)
-{
-	register struct subchannel_id reg1 asm("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	.insn	rre,0xb2760000,%1,0\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode)
-		: "d" (reg1)
-		: "cc");
-	return ccode;
-}
-
 #endif
diff --git a/drivers/s390/cio/ioasm.c b/drivers/s390/cio/ioasm.c
new file mode 100644
index 000000000000..98984818618f
--- /dev/null
+++ b/drivers/s390/cio/ioasm.c
@@ -0,0 +1,224 @@
+/*
+ * Channel subsystem I/O instructions.
+ */
+
+#include <linux/export.h>
+
+#include <asm/chpid.h>
+#include <asm/schid.h>
+#include <asm/crw.h>
+
+#include "ioasm.h"
+#include "orb.h"
+#include "cio.h"
+
+int stsch(struct subchannel_id schid, struct schib *addr)
+{
+	register struct subchannel_id reg1 asm ("1") = schid;
+	int ccode = -EIO;
+
+	asm volatile(
+		"	stsch	0(%3)\n"
+		"0:	ipm	%0\n"
+		"	srl	%0,28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: "+d" (ccode), "=m" (*addr)
+		: "d" (reg1), "a" (addr)
+		: "cc");
+	trace_s390_cio_stsch(schid, addr, ccode);
+
+	return ccode;
+}
+EXPORT_SYMBOL(stsch);
+
+int msch(struct subchannel_id schid, struct schib *addr)
+{
+	register struct subchannel_id reg1 asm ("1") = schid;
+	int ccode = -EIO;
+
+	asm volatile(
+		"	msch	0(%2)\n"
+		"0:	ipm	%0\n"
+		"	srl	%0,28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: "+d" (ccode)
+		: "d" (reg1), "a" (addr), "m" (*addr)
+		: "cc");
+	trace_s390_cio_msch(schid, addr, ccode);
+
+	return ccode;
+}
+
+int tsch(struct subchannel_id schid, struct irb *addr)
+{
+	register struct subchannel_id reg1 asm ("1") = schid;
+	int ccode;
+
+	asm volatile(
+		"	tsch	0(%3)\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode), "=m" (*addr)
+		: "d" (reg1), "a" (addr)
+		: "cc");
+	trace_s390_cio_tsch(schid, addr, ccode);
+
+	return ccode;
+}
+
+int ssch(struct subchannel_id schid, union orb *addr)
+{
+	register struct subchannel_id reg1 asm("1") = schid;
+	int ccode = -EIO;
+
+	asm volatile(
+		"	ssch	0(%2)\n"
+		"0:	ipm	%0\n"
+		"	srl	%0,28\n"
+		"1:\n"
+		EX_TABLE(0b, 1b)
+		: "+d" (ccode)
+		: "d" (reg1), "a" (addr), "m" (*addr)
+		: "cc", "memory");
+	trace_s390_cio_ssch(schid, addr, ccode);
+
+	return ccode;
+}
+EXPORT_SYMBOL(ssch);
+
+int csch(struct subchannel_id schid)
+{
+	register struct subchannel_id reg1 asm("1") = schid;
+	int ccode;
+
+	asm volatile(
+		"	csch\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode)
+		: "d" (reg1)
+		: "cc");
+	trace_s390_cio_csch(schid, ccode);
+
+	return ccode;
+}
+EXPORT_SYMBOL(csch);
+
+int tpi(struct tpi_info *addr)
+{
+	int ccode;
+
+	asm volatile(
+		"	tpi	0(%2)\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode), "=m" (*addr)
+		: "a" (addr)
+		: "cc");
+	trace_s390_cio_tpi(addr, ccode);
+
+	return ccode;
+}
+
+int chsc(void *chsc_area)
+{
+	typedef struct { char _[4096]; } addr_type;
+	int cc;
+
+	asm volatile(
+		"	.insn	rre,0xb25f0000,%2,0\n"
+		"	ipm	%0\n"
+		"	srl	%0,28\n"
+		: "=d" (cc), "=m" (*(addr_type *) chsc_area)
+		: "d" (chsc_area), "m" (*(addr_type *) chsc_area)
+		: "cc");
+	trace_s390_cio_chsc(chsc_area, cc);
+
+	return cc;
+}
+EXPORT_SYMBOL(chsc);
+
+int rchp(struct chp_id chpid)
+{
+	register struct chp_id reg1 asm ("1") = chpid;
+	int ccode;
+
+	asm volatile(
+		"	lr	1,%1\n"
+		"	rchp\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode) : "d" (reg1) : "cc");
+	trace_s390_cio_rchp(chpid, ccode);
+
+	return ccode;
+}
+
+int rsch(struct subchannel_id schid)
+{
+	register struct subchannel_id reg1 asm("1") = schid;
+	int ccode;
+
+	asm volatile(
+		"	rsch\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode)
+		: "d" (reg1)
+		: "cc", "memory");
+	trace_s390_cio_rsch(schid, ccode);
+
+	return ccode;
+}
+
+int hsch(struct subchannel_id schid)
+{
+	register struct subchannel_id reg1 asm("1") = schid;
+	int ccode;
+
+	asm volatile(
+		"	hsch\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode)
+		: "d" (reg1)
+		: "cc");
+	trace_s390_cio_hsch(schid, ccode);
+
+	return ccode;
+}
+
+int xsch(struct subchannel_id schid)
+{
+	register struct subchannel_id reg1 asm("1") = schid;
+	int ccode;
+
+	asm volatile(
+		"	xsch\n"
+		"	ipm	%0\n"
+		"	srl	%0,28"
+		: "=d" (ccode)
+		: "d" (reg1)
+		: "cc");
+	trace_s390_cio_xsch(schid, ccode);
+
+	return ccode;
+}
+
+int stcrw(struct crw *crw)
+{
+	int ccode;
+
+	asm volatile(
+		"	stcrw	0(%2)\n"
+		"	ipm	%0\n"
+		"	srl	%0,28\n"
+		: "=d" (ccode), "=m" (*crw)
+		: "a" (crw)
+		: "cc");
+	trace_s390_cio_stcrw(crw, ccode);
+
+	return ccode;
+}
diff --git a/drivers/s390/cio/ioasm.h b/drivers/s390/cio/ioasm.h
index 4d80fc67a06b..b31ee6bff1e4 100644
--- a/drivers/s390/cio/ioasm.h
+++ b/drivers/s390/cio/ioasm.h
@@ -3,165 +3,26 @@
 
 #include <asm/chpid.h>
 #include <asm/schid.h>
+#include <asm/crw.h>
 #include "orb.h"
 #include "cio.h"
+#include "trace.h"
 
 /*
- * TPI info structure
+ * Some S390 specific IO instructions
  */
-struct tpi_info {
-	struct subchannel_id schid;
-	__u32 intparm;		 /* interruption parameter */
-	__u32 adapter_IO : 1;
-	__u32 reserved2	 : 1;
-	__u32 isc	 : 3;
-	__u32 reserved3	 : 12;
-	__u32 int_type	 : 3;
-	__u32 reserved4	 : 12;
-} __attribute__ ((packed));
 
-
-/*
- * Some S390 specific IO instructions as inline
- */
-
-static inline int stsch_err(struct subchannel_id schid, struct schib *addr)
-{
-	register struct subchannel_id reg1 asm ("1") = schid;
-	int ccode = -EIO;
-
-	asm volatile(
-		"	stsch	0(%3)\n"
-		"0:	ipm	%0\n"
-		"	srl	%0,28\n"
-		"1:\n"
-		EX_TABLE(0b,1b)
-		: "+d" (ccode), "=m" (*addr)
-		: "d" (reg1), "a" (addr)
-		: "cc");
-	return ccode;
-}
-
-static inline int msch(struct subchannel_id schid, struct schib *addr)
-{
-	register struct subchannel_id reg1 asm ("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	msch	0(%2)\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode)
-		: "d" (reg1), "a" (addr), "m" (*addr)
-		: "cc");
-	return ccode;
-}
-
-static inline int msch_err(struct subchannel_id schid, struct schib *addr)
-{
-	register struct subchannel_id reg1 asm ("1") = schid;
-	int ccode = -EIO;
-
-	asm volatile(
-		"	msch	0(%2)\n"
-		"0:	ipm	%0\n"
-		"	srl	%0,28\n"
-		"1:\n"
-		EX_TABLE(0b,1b)
-		: "+d" (ccode)
-		: "d" (reg1), "a" (addr), "m" (*addr)
-		: "cc");
-	return ccode;
-}
-
-static inline int tsch(struct subchannel_id schid, struct irb *addr)
-{
-	register struct subchannel_id reg1 asm ("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	tsch	0(%3)\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode), "=m" (*addr)
-		: "d" (reg1), "a" (addr)
-		: "cc");
-	return ccode;
-}
-
-static inline int ssch(struct subchannel_id schid, union orb *addr)
-{
-	register struct subchannel_id reg1 asm("1") = schid;
-	int ccode = -EIO;
-
-	asm volatile(
-		"	ssch	0(%2)\n"
-		"0:	ipm	%0\n"
-		"	srl	%0,28\n"
-		"1:\n"
-		EX_TABLE(0b, 1b)
-		: "+d" (ccode)
-		: "d" (reg1), "a" (addr), "m" (*addr)
-		: "cc", "memory");
-	return ccode;
-}
-
-static inline int csch(struct subchannel_id schid)
-{
-	register struct subchannel_id reg1 asm("1") = schid;
-	int ccode;
-
-	asm volatile(
-		"	csch\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode)
-		: "d" (reg1)
-		: "cc");
-	return ccode;
-}
-
-static inline int tpi(struct tpi_info *addr)
-{
-	int ccode;
-
-	asm volatile(
-		"	tpi	0(%2)\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode), "=m" (*addr)
-		: "a" (addr)
-		: "cc");
-	return ccode;
-}
-
-static inline int chsc(void *chsc_area)
-{
-	typedef struct { char _[4096]; } addr_type;
-	int cc;
-
-	asm volatile(
-		"	.insn	rre,0xb25f0000,%2,0\n"
-		"	ipm	%0\n"
-		"	srl	%0,28\n"
-		: "=d" (cc), "=m" (*(addr_type *) chsc_area)
-		: "d" (chsc_area), "m" (*(addr_type *) chsc_area)
-		: "cc");
-	return cc;
-}
-
-static inline int rchp(struct chp_id chpid)
-{
-	register struct chp_id reg1 asm ("1") = chpid;
-	int ccode;
-
-	asm volatile(
-		"	lr	1,%1\n"
-		"	rchp\n"
-		"	ipm	%0\n"
-		"	srl	%0,28"
-		: "=d" (ccode) : "d" (reg1) : "cc");
-	return ccode;
-}
+int stsch(struct subchannel_id schid, struct schib *addr);
+int msch(struct subchannel_id schid, struct schib *addr);
+int tsch(struct subchannel_id schid, struct irb *addr);
+int ssch(struct subchannel_id schid, union orb *addr);
+int csch(struct subchannel_id schid);
+int tpi(struct tpi_info *addr);
+int chsc(void *chsc_area);
+int rchp(struct chp_id chpid);
+int rsch(struct subchannel_id schid);
+int hsch(struct subchannel_id schid);
+int xsch(struct subchannel_id schid);
+int stcrw(struct crw *crw);
 
 #endif
diff --git a/drivers/s390/cio/qdio_debug.c b/drivers/s390/cio/qdio_debug.c
index f1f3baa8e6e4..b6fc147f83d8 100644
--- a/drivers/s390/cio/qdio_debug.c
+++ b/drivers/s390/cio/qdio_debug.c
@@ -366,8 +366,6 @@ void qdio_debug_exit(void)
 {
 	qdio_clear_dbf_list();
 	debugfs_remove(debugfs_root);
-	if (qdio_dbf_setup)
-		debug_unregister(qdio_dbf_setup);
-	if (qdio_dbf_error)
-		debug_unregister(qdio_dbf_error);
+	debug_unregister(qdio_dbf_setup);
+	debug_unregister(qdio_dbf_error);
 }
diff --git a/drivers/s390/cio/trace.c b/drivers/s390/cio/trace.c
new file mode 100644
index 000000000000..8e706669ac8b
--- /dev/null
+++ b/drivers/s390/cio/trace.c
@@ -0,0 +1,24 @@
+/*
+ * Tracepoint definitions for s390_cio
+ *
+ * Copyright IBM Corp. 2015
+ * Author(s): Peter Oberparleiter <oberpar@linux.vnet.ibm.com>
+ */
+
+#include <asm/crw.h>
+#include "cio.h"
+
+#define CREATE_TRACE_POINTS
+#include "trace.h"
+
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_stsch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_msch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_tsch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_tpi);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_ssch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_csch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_hsch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_xsch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_rsch);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_rchp);
+EXPORT_TRACEPOINT_SYMBOL(s390_cio_chsc);
diff --git a/drivers/s390/cio/trace.h b/drivers/s390/cio/trace.h
new file mode 100644
index 000000000000..5b807a09f21b
--- /dev/null
+++ b/drivers/s390/cio/trace.h
@@ -0,0 +1,363 @@
+/*
+ * Tracepoint header for the s390 Common I/O layer (CIO)
+ *
+ * Copyright IBM Corp. 2015
+ * Author(s): Peter Oberparleiter <oberpar@linux.vnet.ibm.com>
+ */
+
+#include <linux/kernel.h>
+#include <asm/crw.h>
+#include <uapi/asm/chpid.h>
+#include <uapi/asm/schid.h>
+#include "cio.h"
+#include "orb.h"
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM s390
+
+#if !defined(_TRACE_S390_CIO_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _TRACE_S390_CIO_H
+
+#include <linux/tracepoint.h>
+
+DECLARE_EVENT_CLASS(s390_class_schib,
+	TP_PROTO(struct subchannel_id schid, struct schib *schib, int cc),
+	TP_ARGS(schid, schib, cc),
+	TP_STRUCT__entry(
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+		__field(u16, devno)
+		__field_struct(struct schib, schib)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->cssid = schid.cssid;
+		__entry->ssid = schid.ssid;
+		__entry->schno = schid.sch_no;
+		__entry->devno = schib->pmcw.dev;
+		__entry->schib = *schib;
+		__entry->cc = cc;
+	),
+	TP_printk("schid=%x.%x.%04x cc=%d ena=%d st=%d dnv=%d dev=%04x "
+		  "lpm=0x%02x pnom=0x%02x lpum=0x%02x pim=0x%02x pam=0x%02x "
+		  "pom=0x%02x chpids=%016llx",
+		  __entry->cssid, __entry->ssid, __entry->schno, __entry->cc,
+		  __entry->schib.pmcw.ena, __entry->schib.pmcw.st,
+		  __entry->schib.pmcw.dnv, __entry->schib.pmcw.dev,
+		  __entry->schib.pmcw.lpm, __entry->schib.pmcw.pnom,
+		  __entry->schib.pmcw.lpum, __entry->schib.pmcw.pim,
+		  __entry->schib.pmcw.pam, __entry->schib.pmcw.pom,
+		  *((u64 *) __entry->schib.pmcw.chpid)
+	)
+);
+
+/**
+ * s390_cio_stsch -  Store Subchannel instruction (STSCH) was performed
+ * @schid: Subchannel ID
+ * @schib: Subchannel-Information block
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schib, s390_cio_stsch,
+	TP_PROTO(struct subchannel_id schid, struct schib *schib, int cc),
+	TP_ARGS(schid, schib, cc)
+);
+
+/**
+ * s390_cio_msch -  Modify Subchannel instruction (MSCH) was performed
+ * @schid: Subchannel ID
+ * @schib: Subchannel-Information block
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schib, s390_cio_msch,
+	TP_PROTO(struct subchannel_id schid, struct schib *schib, int cc),
+	TP_ARGS(schid, schib, cc)
+);
+
+/**
+ * s390_cio_tsch - Test Subchannel instruction (TSCH) was performed
+ * @schid: Subchannel ID
+ * @irb: Interruption-Response Block
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_tsch,
+	TP_PROTO(struct subchannel_id schid, struct irb *irb, int cc),
+	TP_ARGS(schid, irb, cc),
+	TP_STRUCT__entry(
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+		__field_struct(struct irb, irb)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->cssid = schid.cssid;
+		__entry->ssid = schid.ssid;
+		__entry->schno = schid.sch_no;
+		__entry->irb = *irb;
+		__entry->cc = cc;
+	),
+	TP_printk("schid=%x.%x.%04x cc=%d dcc=%d pno=%d fctl=0x%x actl=0x%x "
+		  "stctl=0x%x dstat=0x%x cstat=0x%x",
+		  __entry->cssid, __entry->ssid, __entry->schno, __entry->cc,
+		  scsw_cc(&__entry->irb.scsw), scsw_pno(&__entry->irb.scsw),
+		  scsw_fctl(&__entry->irb.scsw), scsw_actl(&__entry->irb.scsw),
+		  scsw_stctl(&__entry->irb.scsw),
+		  scsw_dstat(&__entry->irb.scsw), scsw_cstat(&__entry->irb.scsw)
+	)
+);
+
+/**
+ * s390_cio_tpi - Test Pending Interruption instruction (TPI) was performed
+ * @addr: Address of the I/O interruption code or %NULL
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_tpi,
+	TP_PROTO(struct tpi_info *addr, int cc),
+	TP_ARGS(addr, cc),
+	TP_STRUCT__entry(
+		__field(int, cc)
+		__field_struct(struct tpi_info, tpi_info)
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+	),
+	TP_fast_assign(
+		__entry->cc = cc;
+		if (cc != 0)
+			memset(&__entry->tpi_info, 0, sizeof(struct tpi_info));
+		else if (addr)
+			__entry->tpi_info = *addr;
+		else {
+			memcpy(&__entry->tpi_info, &S390_lowcore.subchannel_id,
+			       sizeof(struct tpi_info));
+		}
+		__entry->cssid = __entry->tpi_info.schid.cssid;
+		__entry->ssid = __entry->tpi_info.schid.ssid;
+		__entry->schno = __entry->tpi_info.schid.sch_no;
+	),
+	TP_printk("schid=%x.%x.%04x cc=%d a=%d isc=%d type=%d",
+		  __entry->cssid, __entry->ssid, __entry->schno, __entry->cc,
+		  __entry->tpi_info.adapter_IO, __entry->tpi_info.isc,
+		  __entry->tpi_info.type
+	)
+);
+
+/**
+ * s390_cio_ssch - Start Subchannel instruction (SSCH) was performed
+ * @schid: Subchannel ID
+ * @orb: Operation-Request Block
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_ssch,
+	TP_PROTO(struct subchannel_id schid, union orb *orb, int cc),
+	TP_ARGS(schid, orb, cc),
+	TP_STRUCT__entry(
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+		__field_struct(union orb, orb)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->cssid = schid.cssid;
+		__entry->ssid = schid.ssid;
+		__entry->schno = schid.sch_no;
+		__entry->orb = *orb;
+		__entry->cc = cc;
+	),
+	TP_printk("schid=%x.%x.%04x cc=%d", __entry->cssid, __entry->ssid,
+		  __entry->schno, __entry->cc
+	)
+);
+
+DECLARE_EVENT_CLASS(s390_class_schid,
+	TP_PROTO(struct subchannel_id schid, int cc),
+	TP_ARGS(schid, cc),
+	TP_STRUCT__entry(
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->cssid = schid.cssid;
+		__entry->ssid = schid.ssid;
+		__entry->schno = schid.sch_no;
+		__entry->cc = cc;
+	),
+	TP_printk("schid=%x.%x.%04x cc=%d", __entry->cssid, __entry->ssid,
+		  __entry->schno, __entry->cc
+	)
+);
+
+/**
+ * s390_cio_csch - Clear Subchannel instruction (CSCH) was performed
+ * @schid: Subchannel ID
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schid, s390_cio_csch,
+	TP_PROTO(struct subchannel_id schid, int cc),
+	TP_ARGS(schid, cc)
+);
+
+/**
+ * s390_cio_hsch - Halt Subchannel instruction (HSCH) was performed
+ * @schid: Subchannel ID
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schid, s390_cio_hsch,
+	TP_PROTO(struct subchannel_id schid, int cc),
+	TP_ARGS(schid, cc)
+);
+
+/**
+ * s390_cio_xsch - Cancel Subchannel instruction (XSCH) was performed
+ * @schid: Subchannel ID
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schid, s390_cio_xsch,
+	TP_PROTO(struct subchannel_id schid, int cc),
+	TP_ARGS(schid, cc)
+);
+
+/**
+ * s390_cio_rsch - Resume Subchannel instruction (RSCH) was performed
+ * @schid: Subchannel ID
+ * @cc: Condition code
+ */
+DEFINE_EVENT(s390_class_schid, s390_cio_rsch,
+	TP_PROTO(struct subchannel_id schid, int cc),
+	TP_ARGS(schid, cc)
+);
+
+/**
+ * s390_cio_rchp - Reset Channel Path (RCHP) instruction was performed
+ * @chpid: Channel-Path Identifier
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_rchp,
+	TP_PROTO(struct chp_id chpid, int cc),
+	TP_ARGS(chpid, cc),
+	TP_STRUCT__entry(
+		__field(u8, cssid)
+		__field(u8, id)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->cssid = chpid.cssid;
+		__entry->id = chpid.id;
+		__entry->cc = cc;
+	),
+	TP_printk("chpid=%x.%02x cc=%d", __entry->cssid, __entry->id,
+		  __entry->cc
+	)
+);
+
+#define CHSC_MAX_REQUEST_LEN		64
+#define CHSC_MAX_RESPONSE_LEN		64
+
+/**
+ * s390_cio_chsc - Channel Subsystem Call (CHSC) instruction was performed
+ * @chsc: CHSC block
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_chsc,
+	TP_PROTO(struct chsc_header *chsc, int cc),
+	TP_ARGS(chsc, cc),
+	TP_STRUCT__entry(
+		__field(int, cc)
+		__field(u16, code)
+		__field(u16, rcode)
+		__array(u8, request, CHSC_MAX_REQUEST_LEN)
+		__array(u8, response, CHSC_MAX_RESPONSE_LEN)
+	),
+	TP_fast_assign(
+		__entry->cc = cc;
+		__entry->code = chsc->code;
+		memcpy(&entry->request, chsc,
+		       min_t(u16, chsc->length, CHSC_MAX_REQUEST_LEN));
+		chsc = (struct chsc_header *) ((char *) chsc + chsc->length);
+		__entry->rcode = chsc->code;
+		memcpy(&entry->response, chsc,
+		       min_t(u16, chsc->length, CHSC_MAX_RESPONSE_LEN));
+	),
+	TP_printk("code=0x%04x cc=%d rcode=0x%04x", __entry->code,
+		  __entry->cc, __entry->rcode)
+);
+
+/**
+ * s390_cio_interrupt - An I/O interrupt occurred
+ * @tpi_info: Address of the I/O interruption code
+ */
+TRACE_EVENT(s390_cio_interrupt,
+	TP_PROTO(struct tpi_info *tpi_info),
+	TP_ARGS(tpi_info),
+	TP_STRUCT__entry(
+		__field_struct(struct tpi_info, tpi_info)
+		__field(u8, cssid)
+		__field(u8, ssid)
+		__field(u16, schno)
+	),
+	TP_fast_assign(
+		__entry->tpi_info = *tpi_info;
+		__entry->cssid = __entry->tpi_info.schid.cssid;
+		__entry->ssid = __entry->tpi_info.schid.ssid;
+		__entry->schno = __entry->tpi_info.schid.sch_no;
+	),
+	TP_printk("schid=%x.%x.%04x isc=%d type=%d",
+		  __entry->cssid, __entry->ssid, __entry->schno,
+		  __entry->tpi_info.isc, __entry->tpi_info.type
+	)
+);
+
+/**
+ * s390_cio_adapter_int - An adapter interrupt occurred
+ * @tpi_info: Address of the I/O interruption code
+ */
+TRACE_EVENT(s390_cio_adapter_int,
+	TP_PROTO(struct tpi_info *tpi_info),
+	TP_ARGS(tpi_info),
+	TP_STRUCT__entry(
+		__field_struct(struct tpi_info, tpi_info)
+	),
+	TP_fast_assign(
+		__entry->tpi_info = *tpi_info;
+	),
+	TP_printk("isc=%d", __entry->tpi_info.isc)
+);
+
+/**
+ * s390_cio_stcrw - Store Channel Report Word (STCRW) was performed
+ * @crw: Channel Report Word
+ * @cc: Condition code
+ */
+TRACE_EVENT(s390_cio_stcrw,
+	TP_PROTO(struct crw *crw, int cc),
+	TP_ARGS(crw, cc),
+	TP_STRUCT__entry(
+		__field_struct(struct crw, crw)
+		__field(int, cc)
+	),
+	TP_fast_assign(
+		__entry->crw = *crw;
+		__entry->cc = cc;
+	),
+	TP_printk("cc=%d slct=%d oflw=%d chn=%d rsc=%d anc=%d erc=0x%x "
+		  "rsid=0x%x",
+		  __entry->cc, __entry->crw.slct, __entry->crw.oflw,
+		  __entry->crw.chn, __entry->crw.rsc,  __entry->crw.anc,
+		  __entry->crw.erc, __entry->crw.rsid
+	)
+);
+
+#endif /* _TRACE_S390_CIO_H */
+
+/* This part must be outside 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/s390/crypto/zcrypt_api.c b/drivers/s390/crypto/zcrypt_api.c
index 9f8fa42c062c..5d3d04c040c2 100644
--- a/drivers/s390/crypto/zcrypt_api.c
+++ b/drivers/s390/crypto/zcrypt_api.c
@@ -1428,10 +1428,8 @@ int __init zcrypt_debug_init(void)
 void zcrypt_debug_exit(void)
 {
 	debugfs_remove(debugfs_root);
-	if (zcrypt_dbf_common)
-		debug_unregister(zcrypt_dbf_common);
-	if (zcrypt_dbf_devices)
-		debug_unregister(zcrypt_dbf_devices);
+	debug_unregister(zcrypt_dbf_common);
+	debug_unregister(zcrypt_dbf_devices);
 }
 
 /**
diff --git a/drivers/s390/crypto/zcrypt_error.h b/drivers/s390/crypto/zcrypt_error.h
index 7b23f43c7b08..de1b6c1d172c 100644
--- a/drivers/s390/crypto/zcrypt_error.h
+++ b/drivers/s390/crypto/zcrypt_error.h
@@ -112,9 +112,10 @@ static inline int convert_error(struct zcrypt_device *zdev,
 		atomic_set(&zcrypt_rescan_req, 1);
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
-			zdev->ap_dev->qid, zdev->online, ehdr->reply_code);
+			AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online,
+			ehdr->reply_code);
 		return -EAGAIN;
 	case REP82_ERROR_TRANSPORT_FAIL:
 	case REP82_ERROR_MACHINE_FAILURE:
@@ -123,16 +124,18 @@ static inline int convert_error(struct zcrypt_device *zdev,
 		atomic_set(&zcrypt_rescan_req, 1);
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
-			zdev->ap_dev->qid, zdev->online, ehdr->reply_code);
+			AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online,
+			ehdr->reply_code);
 		return -EAGAIN;
 	default:
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
-			zdev->ap_dev->qid, zdev->online, ehdr->reply_code);
+			AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online,
+			ehdr->reply_code);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
diff --git a/drivers/s390/crypto/zcrypt_msgtype50.c b/drivers/s390/crypto/zcrypt_msgtype50.c
index 74edf2934e7c..eedfaa2cf715 100644
--- a/drivers/s390/crypto/zcrypt_msgtype50.c
+++ b/drivers/s390/crypto/zcrypt_msgtype50.c
@@ -336,9 +336,10 @@ static int convert_type80(struct zcrypt_device *zdev,
 		/* The result is too short, the CEX2A card may not do that.. */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
-			       zdev->ap_dev->qid, zdev->online, t80h->code);
+			       AP_QID_DEVICE(zdev->ap_dev->qid),
+			       zdev->online, t80h->code);
 
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
@@ -368,9 +369,9 @@ static int convert_response(struct zcrypt_device *zdev,
 	default: /* Unknown response type, this should NEVER EVER happen */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%dfail",
-			       zdev->ap_dev->qid, zdev->online);
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
diff --git a/drivers/s390/crypto/zcrypt_msgtype6.c b/drivers/s390/crypto/zcrypt_msgtype6.c
index 9a2dd472c1cc..21959719daef 100644
--- a/drivers/s390/crypto/zcrypt_msgtype6.c
+++ b/drivers/s390/crypto/zcrypt_msgtype6.c
@@ -572,9 +572,9 @@ static int convert_type86_ica(struct zcrypt_device *zdev,
 			return -EINVAL;
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%drc%d",
-			       zdev->ap_dev->qid, zdev->online,
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online,
 			       msg->hdr.reply_code);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
@@ -715,9 +715,9 @@ static int convert_response_ica(struct zcrypt_device *zdev,
 	default: /* Unknown response type, this should NEVER EVER happen */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%dfail",
-			       zdev->ap_dev->qid, zdev->online);
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
@@ -747,9 +747,9 @@ static int convert_response_xcrb(struct zcrypt_device *zdev,
 		xcRB->status = 0x0008044DL; /* HDD_InvalidParm */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%dfail",
-			       zdev->ap_dev->qid, zdev->online);
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
@@ -773,9 +773,9 @@ static int convert_response_ep11_xcrb(struct zcrypt_device *zdev,
 	default: /* Unknown response type, this should NEVER EVER happen */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%dfail",
-			       zdev->ap_dev->qid, zdev->online);
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online);
 		return -EAGAIN; /* repeat the request on a different device. */
 	}
 }
@@ -800,9 +800,9 @@ static int convert_response_rng(struct zcrypt_device *zdev,
 	default: /* Unknown response type, this should NEVER EVER happen */
 		zdev->online = 0;
 		pr_err("Cryptographic device %x failed and was set offline\n",
-		       zdev->ap_dev->qid);
+		       AP_QID_DEVICE(zdev->ap_dev->qid));
 		ZCRYPT_DBF_DEV(DBF_ERR, zdev, "dev%04xo%dfail",
-			       zdev->ap_dev->qid, zdev->online);
+			       AP_QID_DEVICE(zdev->ap_dev->qid), zdev->online);
 		return -EAGAIN;	/* repeat the request on a different device. */
 	}
 }
diff --git a/drivers/s390/net/ctcm_main.c b/drivers/s390/net/ctcm_main.c
index 05c37d6d4afe..c3e22523faf3 100644
--- a/drivers/s390/net/ctcm_main.c
+++ b/drivers/s390/net/ctcm_main.c
@@ -1677,11 +1677,8 @@ static int ctcm_shutdown_device(struct ccwgroup_device *cgdev)
 
 	ccw_device_set_offline(cgdev->cdev[1]);
 	ccw_device_set_offline(cgdev->cdev[0]);
-
-	if (priv->channel[CTCM_READ])
-		channel_remove(priv->channel[CTCM_READ]);
-	if (priv->channel[CTCM_WRITE])
-		channel_remove(priv->channel[CTCM_WRITE]);
+	channel_remove(priv->channel[CTCM_READ]);
+	channel_remove(priv->channel[CTCM_WRITE]);
 	priv->channel[CTCM_READ] = priv->channel[CTCM_WRITE] = NULL;
 
 	return 0;
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 1766a20ebcb1..ec2e014e885c 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -981,6 +981,10 @@ int qeth_send_setassparms(struct qeth_card *, struct qeth_cmd_buffer *, __u16,
 			  int (*reply_cb)(struct qeth_card *,
 					  struct qeth_reply *, unsigned long),
 			  void *);
+struct qeth_cmd_buffer *qeth_get_setassparms_cmd(struct qeth_card *,
+						 enum qeth_ipa_funcs,
+						 __u16, __u16,
+						 enum qeth_prot_versions);
 int qeth_start_ipa_tx_checksum(struct qeth_card *);
 int qeth_set_rx_csum(struct qeth_card *, int);
 
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index 31ac53fa5cee..787153764120 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -2684,8 +2684,6 @@ void qeth_print_status_message(struct qeth_card *card)
 			sprintf(card->info.mcl_level, "%02x%02x",
 				card->info.mcl_level[2],
 				card->info.mcl_level[3]);
-
-			card->info.mcl_level[QETH_MCL_LENGTH] = 0;
 			break;
 		}
 		/* fallthrough */
@@ -5297,10 +5295,10 @@ static int qeth_setassparms_cb(struct qeth_card *card,
 	return 0;
 }
 
-static struct qeth_cmd_buffer *qeth_get_setassparms_cmd(struct qeth_card *card,
-						  enum qeth_ipa_funcs ipa_func,
-						  __u16 cmd_code, __u16 len,
-						  enum qeth_prot_versions prot)
+struct qeth_cmd_buffer *qeth_get_setassparms_cmd(struct qeth_card *card,
+						 enum qeth_ipa_funcs ipa_func,
+						 __u16 cmd_code, __u16 len,
+						 enum qeth_prot_versions prot)
 {
 	struct qeth_cmd_buffer *iob;
 	struct qeth_ipa_cmd *cmd;
@@ -5319,6 +5317,7 @@ static struct qeth_cmd_buffer *qeth_get_setassparms_cmd(struct qeth_card *card,
 
 	return iob;
 }
+EXPORT_SYMBOL_GPL(qeth_get_setassparms_cmd);
 
 int qeth_send_setassparms(struct qeth_card *card,
 			  struct qeth_cmd_buffer *iob, __u16 len, long data,
diff --git a/drivers/s390/net/qeth_l2_main.c b/drivers/s390/net/qeth_l2_main.c
index 8f1b091e1732..80b1979e8d95 100644
--- a/drivers/s390/net/qeth_l2_main.c
+++ b/drivers/s390/net/qeth_l2_main.c
@@ -1126,6 +1126,7 @@ static int qeth_l2_setup_netdev(struct qeth_card *card)
 	qeth_l2_request_initial_mac(card);
 	SET_NETDEV_DEV(card->dev, &card->gdev->dev);
 	netif_napi_add(card->dev, &card->napi, qeth_l2_poll, QETH_NAPI_WEIGHT);
+	netif_carrier_off(card->dev);
 	return register_netdev(card->dev);
 }
 
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index 543960e96b42..7c8c68c26540 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -1043,28 +1043,6 @@ static int qeth_l3_default_setassparms_cb(struct qeth_card *card,
 	return 0;
 }
 
-static struct qeth_cmd_buffer *qeth_l3_get_setassparms_cmd(
-	struct qeth_card *card, enum qeth_ipa_funcs ipa_func, __u16 cmd_code,
-	__u16 len, enum qeth_prot_versions prot)
-{
-	struct qeth_cmd_buffer *iob;
-	struct qeth_ipa_cmd *cmd;
-
-	QETH_CARD_TEXT(card, 4, "getasscm");
-	iob = qeth_get_ipacmd_buffer(card, IPA_CMD_SETASSPARMS, prot);
-
-	if (iob) {
-		cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
-		cmd->data.setassparms.hdr.assist_no = ipa_func;
-		cmd->data.setassparms.hdr.length = 8 + len;
-		cmd->data.setassparms.hdr.command_code = cmd_code;
-		cmd->data.setassparms.hdr.return_code = 0;
-		cmd->data.setassparms.hdr.seq_no = 0;
-	}
-
-	return iob;
-}
-
 #ifdef CONFIG_QETH_IPV6
 static int qeth_l3_send_simple_setassparms_ipv6(struct qeth_card *card,
 		enum qeth_ipa_funcs ipa_func, __u16 cmd_code)
@@ -1073,7 +1051,7 @@ static int qeth_l3_send_simple_setassparms_ipv6(struct qeth_card *card,
 	struct qeth_cmd_buffer *iob;
 
 	QETH_CARD_TEXT(card, 4, "simassp6");
-	iob = qeth_l3_get_setassparms_cmd(card, ipa_func, cmd_code,
+	iob = qeth_get_setassparms_cmd(card, ipa_func, cmd_code,
 				       0, QETH_PROT_IPV6);
 	if (!iob)
 		return -ENOMEM;
@@ -2344,10 +2322,11 @@ static int qeth_l3_query_arp_cache_info(struct qeth_card *card,
 
 	QETH_CARD_TEXT_(card, 3, "qarpipv%i", prot);
 
-	iob = qeth_l3_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
-			IPA_CMD_ASS_ARP_QUERY_INFO,
-			sizeof(struct qeth_arp_query_data) - sizeof(char),
-			prot);
+	iob = qeth_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
+				       IPA_CMD_ASS_ARP_QUERY_INFO,
+				       sizeof(struct qeth_arp_query_data)
+						- sizeof(char),
+				       prot);
 	if (!iob)
 		return -ENOMEM;
 	cmd = (struct qeth_ipa_cmd *)(iob->data+IPA_PDU_HEADER_SIZE);
@@ -2439,7 +2418,7 @@ static int qeth_l3_arp_add_entry(struct qeth_card *card,
 		return -EOPNOTSUPP;
 	}
 
-	iob = qeth_l3_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
+	iob = qeth_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
 				       IPA_CMD_ASS_ARP_ADD_ENTRY,
 				       sizeof(struct qeth_arp_cache_entry),
 				       QETH_PROT_IPV4);
@@ -2480,7 +2459,7 @@ static int qeth_l3_arp_remove_entry(struct qeth_card *card,
 		return -EOPNOTSUPP;
 	}
 	memcpy(buf, entry, 12);
-	iob = qeth_l3_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
+	iob = qeth_get_setassparms_cmd(card, IPA_ARP_PROCESSING,
 				       IPA_CMD_ASS_ARP_REMOVE_ENTRY,
 				       12,
 				       QETH_PROT_IPV4);
@@ -2818,7 +2797,7 @@ static inline int qeth_l3_tso_elements(struct sk_buff *skb)
 {
 	unsigned long tcpd = (unsigned long)tcp_hdr(skb) +
 		tcp_hdr(skb)->doff * 4;
-	int tcpd_len = skb->len - (tcpd - (unsigned long)skb->data);
+	int tcpd_len = skb_headlen(skb) - (tcpd - (unsigned long)skb->data);
 	int elements = PFN_UP(tcpd + tcpd_len - 1) - PFN_DOWN(tcpd);
 
 	elements += qeth_get_elements_for_frags(skb);
@@ -3220,6 +3199,7 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 
 	SET_NETDEV_DEV(card->dev, &card->gdev->dev);
 	netif_napi_add(card->dev, &card->napi, qeth_l3_poll, QETH_NAPI_WEIGHT);
+	netif_carrier_off(card->dev);
 	return register_netdev(card->dev);
 }
 
diff --git a/drivers/s390/virtio/kvm_virtio.c b/drivers/s390/virtio/kvm_virtio.c
index 53fb975c404b..1d060fd293a3 100644
--- a/drivers/s390/virtio/kvm_virtio.c
+++ b/drivers/s390/virtio/kvm_virtio.c
@@ -255,7 +255,7 @@ static void kvm_del_vqs(struct virtio_device *vdev)
 static int kvm_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 			struct virtqueue *vqs[],
 			vq_callback_t *callbacks[],
-			const char *names[])
+			const char * const names[])
 {
 	struct kvm_device *kdev = to_kvmdev(vdev);
 	int i;
diff --git a/drivers/s390/virtio/virtio_ccw.c b/drivers/s390/virtio/virtio_ccw.c
index 1b831598df7c..bf2d1300a957 100644
--- a/drivers/s390/virtio/virtio_ccw.c
+++ b/drivers/s390/virtio/virtio_ccw.c
@@ -635,7 +635,7 @@ out:
 static int virtio_ccw_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 			       struct virtqueue *vqs[],
 			       vq_callback_t *callbacks[],
-			       const char *names[])
+			       const char * const names[])
 {
 	struct virtio_ccw_device *vcdev = to_vc_device(vdev);
 	unsigned long *indicatorp = NULL;
diff --git a/drivers/sbus/char/openprom.c b/drivers/sbus/char/openprom.c
index 5843288f64bc..e077ebd89319 100644
--- a/drivers/sbus/char/openprom.c
+++ b/drivers/sbus/char/openprom.c
@@ -390,16 +390,9 @@ static int copyin_string(char __user *user, size_t len, char **ptr)
 	if ((ssize_t)len < 0 || (ssize_t)(len + 1) < 0)
 		return -EINVAL;
 
-	tmp = kmalloc(len + 1, GFP_KERNEL);
-	if (!tmp)
-		return -ENOMEM;
-
-	if (copy_from_user(tmp, user, len)) {
-		kfree(tmp);
-		return -EFAULT;
-	}
-
-	tmp[len] = '\0';
+	tmp = memdup_user_nul(user, len);
+	if (IS_ERR(tmp))
+		return PTR_ERR(tmp);
 
 	*ptr = tmp;
 
diff --git a/drivers/scsi/3w-xxxx.c b/drivers/scsi/3w-xxxx.c
index 2940bd769936..25aba1613e21 100644
--- a/drivers/scsi/3w-xxxx.c
+++ b/drivers/scsi/3w-xxxx.c
@@ -1045,6 +1045,9 @@ static int tw_chrdev_open(struct inode *inode, struct file *file)
 static const struct file_operations tw_fops = {
 	.owner		= THIS_MODULE,
 	.unlocked_ioctl	= tw_chrdev_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl   = tw_chrdev_ioctl,
+#endif
 	.open		= tw_chrdev_open,
 	.release	= NULL,
 	.llseek		= noop_llseek,
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 64eed87d34a8..e2f31c93717d 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -194,6 +194,7 @@ config CHR_DEV_SCH
 config SCSI_ENCLOSURE
 	tristate "SCSI Enclosure Support"
 	depends on SCSI && ENCLOSURE_SERVICES
+	depends on m || SCSI_SAS_ATTRS != m
 	help
 	  Enclosures are devices sitting on or in SCSI backplanes that
 	  manage devices.  If you have a disk cage, the chances are that
@@ -474,6 +475,7 @@ config SCSI_AACRAID
 source "drivers/scsi/aic7xxx/Kconfig.aic7xxx"
 source "drivers/scsi/aic7xxx/Kconfig.aic79xx"
 source "drivers/scsi/aic94xx/Kconfig"
+source "drivers/scsi/hisi_sas/Kconfig"
 source "drivers/scsi/mvsas/Kconfig"
 
 config SCSI_MVUMI
@@ -1104,6 +1106,7 @@ config SCSI_IPR
 	tristate "IBM Power Linux RAID adapter support"
 	depends on PCI && SCSI && ATA
 	select FW_LOADER
+	select IRQ_POLL
 	---help---
 	  This driver supports the IBM Power Linux family RAID adapters.
 	  This includes IBM pSeries 5712, 5703, 5709, and 570A, as well
@@ -1618,23 +1621,6 @@ config ATARI_SCSI
 	  ST-DMA, replacing ACSI).  It does NOT support other schemes, like
 	  in the Hades (without DMA).
 
-config ATARI_SCSI_TOSHIBA_DELAY
-	bool "Long delays for Toshiba CD-ROMs"
-	depends on ATARI_SCSI
-	help
-	  This option increases the delay after a SCSI arbitration to
-	  accommodate some flaky Toshiba CD-ROM drives. Say Y if you intend to
-	  use a Toshiba CD-ROM drive; otherwise, the option is not needed and
-	  would impact performance a bit, so say N.
-
-config ATARI_SCSI_RESET_BOOT
-	bool "Reset SCSI-devices at boottime"
-	depends on ATARI_SCSI
-	help
-	  Reset the devices on your Atari whenever it boots.  This makes the
-	  boot process fractionally longer but may assist recovery from errors
-	  that leave the devices with SCSI operations partway completed.
-
 config MAC_SCSI
 	tristate "Macintosh NCR5380 SCSI"
 	depends on MAC && SCSI=y
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index c14bca4a9675..862ab4efad61 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -157,6 +157,7 @@ obj-$(CONFIG_CHR_DEV_SCH)	+= ch.o
 obj-$(CONFIG_SCSI_ENCLOSURE)	+= ses.o
 
 obj-$(CONFIG_SCSI_OSD_INITIATOR) += osd/
+obj-$(CONFIG_SCSI_HISI_SAS) += hisi_sas/
 
 # This goes last, so that "real" scsi devices probe earlier
 obj-$(CONFIG_SCSI_DEBUG)	+= scsi_debug.o
diff --git a/drivers/scsi/NCR5380.c b/drivers/scsi/NCR5380.c
index a777e5c412df..d72867257346 100644
--- a/drivers/scsi/NCR5380.c
+++ b/drivers/scsi/NCR5380.c
@@ -1,17 +1,17 @@
-/* 
+/*
  * NCR 5380 generic driver routines.  These should make it *trivial*
- *      to implement 5380 SCSI drivers under Linux with a non-trantor
- *      architecture.
+ * to implement 5380 SCSI drivers under Linux with a non-trantor
+ * architecture.
  *
- *      Note that these routines also work with NR53c400 family chips.
+ * Note that these routines also work with NR53c400 family chips.
  *
  * Copyright 1993, Drew Eckhardt
- *      Visionary Computing 
- *      (Unix and Linux consulting and custom programming)
- *      drew@colorado.edu
- *      +1 (303) 666-5836
+ * Visionary Computing
+ * (Unix and Linux consulting and custom programming)
+ * drew@colorado.edu
+ * +1 (303) 666-5836
  *
- * For more information, please consult 
+ * For more information, please consult
  *
  * NCR 5380 Family
  * SCSI Protocol Controller
@@ -25,84 +25,28 @@
  */
 
 /*
- * Revision 1.10 1998/9/2	Alan Cox
- *				(alan@lxorguk.ukuu.org.uk)
- * Fixed up the timer lockups reported so far. Things still suck. Looking 
- * forward to 2.3 and per device request queues. Then it'll be possible to
- * SMP thread this beast and improve life no end.
- 
- * Revision 1.9  1997/7/27	Ronald van Cuijlenborg
- *				(ronald.van.cuijlenborg@tip.nl or nutty@dds.nl)
- * (hopefully) fixed and enhanced USLEEP
- * added support for DTC3181E card (for Mustek scanner)
- *
-
- * Revision 1.8			Ingmar Baumgart
- *				(ingmar@gonzo.schwaben.de)
- * added support for NCR53C400a card
- *
-
- * Revision 1.7  1996/3/2       Ray Van Tassle (rayvt@comm.mot.com)
- * added proc_info
- * added support needed for DTC 3180/3280
- * fixed a couple of bugs
- *
-
- * Revision 1.5  1994/01/19  09:14:57  drew
- * Fixed udelay() hack that was being used on DATAOUT phases
- * instead of a proper wait for the final handshake.
- *
- * Revision 1.4  1994/01/19  06:44:25  drew
- * *** empty log message ***
- *
- * Revision 1.3  1994/01/19  05:24:40  drew
- * Added support for TCR LAST_BYTE_SENT bit.
- *
- * Revision 1.2  1994/01/15  06:14:11  drew
- * REAL DMA support, bug fixes.
- *
- * Revision 1.1  1994/01/15  06:00:54  drew
- * Initial revision
- *
+ * With contributions from Ray Van Tassle, Ingmar Baumgart,
+ * Ronald van Cuijlenborg, Alan Cox and others.
  */
 
 /*
- * Further development / testing that should be done : 
+ * Further development / testing that should be done :
  * 1.  Cleanup the NCR5380_transfer_dma function and DMA operation complete
- *     code so that everything does the same thing that's done at the 
- *     end of a pseudo-DMA read operation.
+ * code so that everything does the same thing that's done at the
+ * end of a pseudo-DMA read operation.
  *
  * 2.  Fix REAL_DMA (interrupt driven, polled works fine) -
- *     basically, transfer size needs to be reduced by one 
- *     and the last byte read as is done with PSEUDO_DMA.
- * 
- * 4.  Test SCSI-II tagged queueing (I have no devices which support 
- *      tagged queueing)
- *
- * 5.  Test linked command handling code after Eric is ready with 
- *      the high level code.
+ * basically, transfer size needs to be reduced by one
+ * and the last byte read as is done with PSEUDO_DMA.
+ *
+ * 4.  Test SCSI-II tagged queueing (I have no devices which support
+ * tagged queueing)
  */
-#include <scsi/scsi_dbg.h>
-#include <scsi/scsi_transport_spi.h>
-
-#if (NDEBUG & NDEBUG_LISTS)
-#define LIST(x,y) {printk("LINE:%d   Adding %p to %p\n", __LINE__, (void*)(x), (void*)(y)); if ((x)==(y)) udelay(5); }
-#define REMOVE(w,x,y,z) {printk("LINE:%d   Removing: %p->%p  %p->%p \n", __LINE__, (void*)(w), (void*)(x), (void*)(y), (void*)(z)); if ((x)==(y)) udelay(5); }
-#else
-#define LIST(x,y)
-#define REMOVE(w,x,y,z)
-#endif
 
 #ifndef notyet
-#undef LINKED
 #undef REAL_DMA
 #endif
 
-#ifdef REAL_DMA_POLL
-#undef READ_OVERRUNS
-#define READ_OVERRUNS
-#endif
-
 #ifdef BOARD_REQUIRES_NO_DELAY
 #define io_recovery_delay(x)
 #else
@@ -112,44 +56,28 @@
 /*
  * Design
  *
- * This is a generic 5380 driver.  To use it on a different platform, 
+ * This is a generic 5380 driver.  To use it on a different platform,
  * one simply writes appropriate system specific macros (ie, data
- * transfer - some PC's will use the I/O bus, 68K's must use 
+ * transfer - some PC's will use the I/O bus, 68K's must use
  * memory mapped) and drops this file in their 'C' wrapper.
  *
- * (Note from hch:  unfortunately it was not enough for the different
- * m68k folks and instead of improving this driver they copied it
- * and hacked it up for their needs.  As a consequence they lost
- * most updates to this driver.  Maybe someone will fix all these
- * drivers to use a common core one day..)
- *
- * As far as command queueing, two queues are maintained for 
+ * As far as command queueing, two queues are maintained for
  * each 5380 in the system - commands that haven't been issued yet,
- * and commands that are currently executing.  This means that an 
- * unlimited number of commands may be queued, letting 
- * more commands propagate from the higher driver levels giving higher 
- * throughput.  Note that both I_T_L and I_T_L_Q nexuses are supported, 
- * allowing multiple commands to propagate all the way to a SCSI-II device 
+ * and commands that are currently executing.  This means that an
+ * unlimited number of commands may be queued, letting
+ * more commands propagate from the higher driver levels giving higher
+ * throughput.  Note that both I_T_L and I_T_L_Q nexuses are supported,
+ * allowing multiple commands to propagate all the way to a SCSI-II device
  * while a command is already executing.
  *
  *
- * Issues specific to the NCR5380 : 
+ * Issues specific to the NCR5380 :
  *
- * When used in a PIO or pseudo-dma mode, the NCR5380 is a braindead 
- * piece of hardware that requires you to sit in a loop polling for 
- * the REQ signal as long as you are connected.  Some devices are 
- * brain dead (ie, many TEXEL CD ROM drives) and won't disconnect 
- * while doing long seek operations.
- * 
- * The workaround for this is to keep track of devices that have
- * disconnected.  If the device hasn't disconnected, for commands that
- * should disconnect, we do something like 
- *
- * while (!REQ is asserted) { sleep for N usecs; poll for M usecs }
- * 
- * Some tweaking of N and M needs to be done.  An algorithm based 
- * on "time to data" would give the best results as long as short time
- * to datas (ie, on the same track) were considered, however these 
+ * When used in a PIO or pseudo-dma mode, the NCR5380 is a braindead
+ * piece of hardware that requires you to sit in a loop polling for
+ * the REQ signal as long as you are connected.  Some devices are
+ * brain dead (ie, many TEXEL CD ROM drives) and won't disconnect
+ * while doing long seek operations. [...] These
  * broken devices are the exception rather than the rule and I'd rather
  * spend my time optimizing for the normal case.
  *
@@ -159,23 +87,23 @@
  * which is started from a workqueue for each NCR5380 host in the
  * system.  It attempts to establish I_T_L or I_T_L_Q nexuses by
  * removing the commands from the issue queue and calling
- * NCR5380_select() if a nexus is not established. 
+ * NCR5380_select() if a nexus is not established.
  *
  * Once a nexus is established, the NCR5380_information_transfer()
  * phase goes through the various phases as instructed by the target.
  * if the target goes into MSG IN and sends a DISCONNECT message,
  * the command structure is placed into the per instance disconnected
- * queue, and NCR5380_main tries to find more work.  If the target is 
+ * queue, and NCR5380_main tries to find more work.  If the target is
  * idle for too long, the system will try to sleep.
  *
  * If a command has disconnected, eventually an interrupt will trigger,
  * calling NCR5380_intr()  which will in turn call NCR5380_reselect
  * to reestablish a nexus.  This will run main if necessary.
  *
- * On command termination, the done function will be called as 
+ * On command termination, the done function will be called as
  * appropriate.
  *
- * SCSI pointers are maintained in the SCp field of SCSI command 
+ * SCSI pointers are maintained in the SCp field of SCSI command
  * structures, being initialized after the command is connected
  * in NCR5380_select, and set as appropriate in NCR5380_information_transfer.
  * Note that in violation of the standard, an implicit SAVE POINTERS operation
@@ -185,73 +113,48 @@
 /*
  * Using this file :
  * This file a skeleton Linux SCSI driver for the NCR 5380 series
- * of chips.  To use it, you write an architecture specific functions 
+ * of chips.  To use it, you write an architecture specific functions
  * and macros and include this file in your driver.
  *
- * These macros control options : 
- * AUTOPROBE_IRQ - if defined, the NCR5380_probe_irq() function will be 
- *      defined.
- * 
+ * These macros control options :
+ * AUTOPROBE_IRQ - if defined, the NCR5380_probe_irq() function will be
+ * defined.
+ *
  * AUTOSENSE - if defined, REQUEST SENSE will be performed automatically
- *      for commands that return with a CHECK CONDITION status. 
+ * for commands that return with a CHECK CONDITION status.
  *
  * DIFFERENTIAL - if defined, NCR53c81 chips will use external differential
- *      transceivers. 
+ * transceivers.
  *
  * DONT_USE_INTR - if defined, never use interrupts, even if we probe or
- *      override-configure an IRQ.
- *
- * LIMIT_TRANSFERSIZE - if defined, limit the pseudo-dma transfers to 512
- *      bytes at a time.  Since interrupts are disabled by default during
- *      these transfers, we might need this to give reasonable interrupt
- *      service time if the transfer size gets too large.
- *
- * LINKED - if defined, linked commands are supported.
+ * override-configure an IRQ.
  *
  * PSEUDO_DMA - if defined, PSEUDO DMA is used during the data transfer phases.
  *
  * REAL_DMA - if defined, REAL DMA is used during the data transfer phases.
  *
  * REAL_DMA_POLL - if defined, REAL DMA is used but the driver doesn't
- *      rely on phase mismatch and EOP interrupts to determine end 
- *      of phase.
- *
- * UNSAFE - leave interrupts enabled during pseudo-DMA transfers.  You
- *          only really want to use this if you're having a problem with
- *          dropped characters during high speed communications, and even
- *          then, you're going to be better off twiddling with transfersize
- *          in the high level code.
- *
- * Defaults for these will be provided although the user may want to adjust 
- * these to allocate CPU resources to the SCSI driver or "real" code.
- * 
- * USLEEP_SLEEP - amount of time, in jiffies, to sleep
- *
- * USLEEP_POLL - amount of time, in jiffies, to poll
+ * rely on phase mismatch and EOP interrupts to determine end
+ * of phase.
  *
  * These macros MUST be defined :
- * NCR5380_local_declare() - declare any local variables needed for your
- *      transfer routines.
  *
- * NCR5380_setup(instance) - initialize any local variables needed from a given
- *      instance of the host adapter for NCR5380_{read,write,pread,pwrite}
- * 
  * NCR5380_read(register)  - read from the specified register
  *
- * NCR5380_write(register, value) - write to the specific register 
+ * NCR5380_write(register, value) - write to the specific register
  *
- * NCR5380_implementation_fields  - additional fields needed for this 
- *      specific implementation of the NCR5380
+ * NCR5380_implementation_fields  - additional fields needed for this
+ * specific implementation of the NCR5380
  *
  * Either real DMA *or* pseudo DMA may be implemented
- * REAL functions : 
+ * REAL functions :
  * NCR5380_REAL_DMA should be defined if real DMA is to be used.
- * Note that the DMA setup functions should return the number of bytes 
- *      that they were able to program the controller for.
+ * Note that the DMA setup functions should return the number of bytes
+ * that they were able to program the controller for.
  *
- * Also note that generic i386/PC versions of these macros are 
- *      available as NCR5380_i386_dma_write_setup,
- *      NCR5380_i386_dma_read_setup, and NCR5380_i386_dma_residual.
+ * Also note that generic i386/PC versions of these macros are
+ * available as NCR5380_i386_dma_write_setup,
+ * NCR5380_i386_dma_read_setup, and NCR5380_i386_dma_residual.
  *
  * NCR5380_dma_write_setup(instance, src, count) - initialize
  * NCR5380_dma_read_setup(instance, dst, count) - initialize
@@ -262,25 +165,25 @@
  * NCR5380_pread(instance, dst, count);
  *
  * The generic driver is initialized by calling NCR5380_init(instance),
- * after setting the appropriate host specific fields and ID.  If the 
+ * after setting the appropriate host specific fields and ID.  If the
  * driver wishes to autoprobe for an IRQ line, the NCR5380_probe_irq(instance,
  * possible) function may be used.
  */
 
-static int do_abort(struct Scsi_Host *host);
-static void do_reset(struct Scsi_Host *host);
+static int do_abort(struct Scsi_Host *);
+static void do_reset(struct Scsi_Host *);
 
-/*
- *	initialize_SCp		-	init the scsi pointer field
- *	@cmd: command block to set up
+/**
+ * initialize_SCp - init the scsi pointer field
+ * @cmd: command block to set up
  *
- *	Set up the internal fields in the SCSI command.
+ * Set up the internal fields in the SCSI command.
  */
 
 static inline void initialize_SCp(struct scsi_cmnd *cmd)
 {
-	/* 
-	 * Initialize the Scsi Pointer field so that all of the commands in the 
+	/*
+	 * Initialize the Scsi Pointer field so that all of the commands in the
 	 * various queues are valid.
 	 */
 
@@ -295,120 +198,123 @@ static inline void initialize_SCp(struct scsi_cmnd *cmd)
 		cmd->SCp.ptr = NULL;
 		cmd->SCp.this_residual = 0;
 	}
+
+	cmd->SCp.Status = 0;
+	cmd->SCp.Message = 0;
 }
 
 /**
- *	NCR5380_poll_politely	-	wait for NCR5380 status bits
- *	@instance: controller to poll
- *	@reg: 5380 register to poll
- *	@bit: Bitmask to check
- *	@val: Value required to exit
- *
- *	Polls the NCR5380 in a reasonably efficient manner waiting for
- *	an event to occur, after a short quick poll we begin giving the
- *	CPU back in non IRQ contexts
- *
- *	Returns the value of the register or a negative error code.
+ * NCR5380_poll_politely2 - wait for two chip register values
+ * @instance: controller to poll
+ * @reg1: 5380 register to poll
+ * @bit1: Bitmask to check
+ * @val1: Expected value
+ * @reg2: Second 5380 register to poll
+ * @bit2: Second bitmask to check
+ * @val2: Second expected value
+ * @wait: Time-out in jiffies
+ *
+ * Polls the chip in a reasonably efficient manner waiting for an
+ * event to occur. After a short quick poll we begin to yield the CPU
+ * (if possible). In irq contexts the time-out is arbitrarily limited.
+ * Callers may hold locks as long as they are held in irq mode.
+ *
+ * Returns 0 if either or both event(s) occurred otherwise -ETIMEDOUT.
  */
- 
-static int NCR5380_poll_politely(struct Scsi_Host *instance, int reg, int bit, int val, int t)
+
+static int NCR5380_poll_politely2(struct Scsi_Host *instance,
+                                  int reg1, int bit1, int val1,
+                                  int reg2, int bit2, int val2, int wait)
 {
-	NCR5380_local_declare();
-	int n = 500;		/* At about 8uS a cycle for the cpu access */
-	unsigned long end = jiffies + t;
-	int r;
-	
-	NCR5380_setup(instance);
-
-	while( n-- > 0)
-	{
-		r = NCR5380_read(reg);
-		if((r & bit) == val)
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	unsigned long deadline = jiffies + wait;
+	unsigned long n;
+
+	/* Busy-wait for up to 10 ms */
+	n = min(10000U, jiffies_to_usecs(wait));
+	n *= hostdata->accesses_per_ms;
+	n /= 2000;
+	do {
+		if ((NCR5380_read(reg1) & bit1) == val1)
+			return 0;
+		if ((NCR5380_read(reg2) & bit2) == val2)
 			return 0;
 		cpu_relax();
-	}
-	
-	/* t time yet ? */
-	while(time_before(jiffies, end))
-	{
-		r = NCR5380_read(reg);
-		if((r & bit) == val)
+	} while (n--);
+
+	if (irqs_disabled() || in_interrupt())
+		return -ETIMEDOUT;
+
+	/* Repeatedly sleep for 1 ms until deadline */
+	while (time_is_after_jiffies(deadline)) {
+		schedule_timeout_uninterruptible(1);
+		if ((NCR5380_read(reg1) & bit1) == val1)
+			return 0;
+		if ((NCR5380_read(reg2) & bit2) == val2)
 			return 0;
-		if(!in_interrupt())
-			cond_resched();
-		else
-			cpu_relax();
 	}
+
 	return -ETIMEDOUT;
 }
 
-static struct {
-	unsigned char value;
-	const char *name;
-} phases[] __maybe_unused = {
-	{PHASE_DATAOUT, "DATAOUT"}, 
-	{PHASE_DATAIN, "DATAIN"}, 
-	{PHASE_CMDOUT, "CMDOUT"}, 
-	{PHASE_STATIN, "STATIN"}, 
-	{PHASE_MSGOUT, "MSGOUT"}, 
-	{PHASE_MSGIN, "MSGIN"}, 
-	{PHASE_UNKNOWN, "UNKNOWN"}
-};
+static inline int NCR5380_poll_politely(struct Scsi_Host *instance,
+                                        int reg, int bit, int val, int wait)
+{
+	return NCR5380_poll_politely2(instance, reg, bit, val,
+	                                        reg, bit, val, wait);
+}
 
 #if NDEBUG
 static struct {
 	unsigned char mask;
 	const char *name;
-} signals[] = { 
-	{SR_DBP, "PARITY"}, 
-	{SR_RST, "RST"}, 
-	{SR_BSY, "BSY"}, 
-	{SR_REQ, "REQ"}, 
-	{SR_MSG, "MSG"}, 
-	{SR_CD, "CD"}, 
-	{SR_IO, "IO"}, 
-	{SR_SEL, "SEL"}, 
+} signals[] = {
+	{SR_DBP, "PARITY"},
+	{SR_RST, "RST"},
+	{SR_BSY, "BSY"},
+	{SR_REQ, "REQ"},
+	{SR_MSG, "MSG"},
+	{SR_CD, "CD"},
+	{SR_IO, "IO"},
+	{SR_SEL, "SEL"},
 	{0, NULL}
-}, 
+},
 basrs[] = {
-	{BASR_ATN, "ATN"}, 
-	{BASR_ACK, "ACK"}, 
+	{BASR_ATN, "ATN"},
+	{BASR_ACK, "ACK"},
 	{0, NULL}
-}, 
-icrs[] = { 
-	{ICR_ASSERT_RST, "ASSERT RST"}, 
-	{ICR_ASSERT_ACK, "ASSERT ACK"}, 
-	{ICR_ASSERT_BSY, "ASSERT BSY"}, 
-	{ICR_ASSERT_SEL, "ASSERT SEL"}, 
-	{ICR_ASSERT_ATN, "ASSERT ATN"}, 
-	{ICR_ASSERT_DATA, "ASSERT DATA"}, 
+},
+icrs[] = {
+	{ICR_ASSERT_RST, "ASSERT RST"},
+	{ICR_ASSERT_ACK, "ASSERT ACK"},
+	{ICR_ASSERT_BSY, "ASSERT BSY"},
+	{ICR_ASSERT_SEL, "ASSERT SEL"},
+	{ICR_ASSERT_ATN, "ASSERT ATN"},
+	{ICR_ASSERT_DATA, "ASSERT DATA"},
 	{0, NULL}
-}, 
-mrs[] = { 
-	{MR_BLOCK_DMA_MODE, "MODE BLOCK DMA"}, 
-	{MR_TARGET, "MODE TARGET"}, 
-	{MR_ENABLE_PAR_CHECK, "MODE PARITY CHECK"}, 
-	{MR_ENABLE_PAR_INTR, "MODE PARITY INTR"}, 
-	{MR_MONITOR_BSY, "MODE MONITOR BSY"}, 
-	{MR_DMA_MODE, "MODE DMA"}, 
-	{MR_ARBITRATE, "MODE ARBITRATION"}, 
+},
+mrs[] = {
+	{MR_BLOCK_DMA_MODE, "MODE BLOCK DMA"},
+	{MR_TARGET, "MODE TARGET"},
+	{MR_ENABLE_PAR_CHECK, "MODE PARITY CHECK"},
+	{MR_ENABLE_PAR_INTR, "MODE PARITY INTR"},
+	{MR_ENABLE_EOP_INTR, "MODE EOP INTR"},
+	{MR_MONITOR_BSY, "MODE MONITOR BSY"},
+	{MR_DMA_MODE, "MODE DMA"},
+	{MR_ARBITRATE, "MODE ARBITRATION"},
 	{0, NULL}
 };
 
 /**
- *	NCR5380_print	-	print scsi bus signals
- *	@instance:	adapter state to dump
- *
- *	Print the SCSI bus signals for debugging purposes
+ * NCR5380_print - print scsi bus signals
+ * @instance: adapter state to dump
  *
- *	Locks: caller holds hostdata lock (not essential)
+ * Print the SCSI bus signals for debugging purposes
  */
 
 static void NCR5380_print(struct Scsi_Host *instance)
 {
-	NCR5380_local_declare();
 	unsigned char status, data, basr, mr, icr, i;
-	NCR5380_setup(instance);
 
 	data = NCR5380_read(CURRENT_SCSI_DATA_REG);
 	status = NCR5380_read(STATUS_REG);
@@ -435,117 +341,56 @@ static void NCR5380_print(struct Scsi_Host *instance)
 	printk("\n");
 }
 
+static struct {
+	unsigned char value;
+	const char *name;
+} phases[] = {
+	{PHASE_DATAOUT, "DATAOUT"},
+	{PHASE_DATAIN, "DATAIN"},
+	{PHASE_CMDOUT, "CMDOUT"},
+	{PHASE_STATIN, "STATIN"},
+	{PHASE_MSGOUT, "MSGOUT"},
+	{PHASE_MSGIN, "MSGIN"},
+	{PHASE_UNKNOWN, "UNKNOWN"}
+};
 
-/* 
- *	NCR5380_print_phase	-	show SCSI phase
- *	@instance: adapter to dump
- *
- * 	Print the current SCSI phase for debugging purposes
+/**
+ * NCR5380_print_phase - show SCSI phase
+ * @instance: adapter to dump
  *
- *	Locks: none
+ * Print the current SCSI phase for debugging purposes
  */
 
 static void NCR5380_print_phase(struct Scsi_Host *instance)
 {
-	NCR5380_local_declare();
 	unsigned char status;
 	int i;
-	NCR5380_setup(instance);
 
 	status = NCR5380_read(STATUS_REG);
 	if (!(status & SR_REQ))
-		printk("scsi%d : REQ not asserted, phase unknown.\n", instance->host_no);
+		shost_printk(KERN_DEBUG, instance, "REQ not asserted, phase unknown.\n");
 	else {
-		for (i = 0; (phases[i].value != PHASE_UNKNOWN) && (phases[i].value != (status & PHASE_MASK)); ++i);
-		printk("scsi%d : phase %s\n", instance->host_no, phases[i].name);
+		for (i = 0; (phases[i].value != PHASE_UNKNOWN) &&
+		     (phases[i].value != (status & PHASE_MASK)); ++i)
+			;
+		shost_printk(KERN_DEBUG, instance, "phase %s\n", phases[i].name);
 	}
 }
 #endif
 
-/*
- * These need tweaking, and would probably work best as per-device 
- * flags initialized differently for disk, tape, cd, etc devices.
- * People with broken devices are free to experiment as to what gives
- * the best results for them.
- *
- * USLEEP_SLEEP should be a minimum seek time.
- *
- * USLEEP_POLL should be a maximum rotational latency.
- */
-#ifndef USLEEP_SLEEP
-/* 20 ms (reasonable hard disk speed) */
-#define USLEEP_SLEEP msecs_to_jiffies(20)
-#endif
-/* 300 RPM (floppy speed) */
-#ifndef USLEEP_POLL
-#define USLEEP_POLL msecs_to_jiffies(200)
-#endif
-#ifndef USLEEP_WAITLONG
-/* RvC: (reasonable time to wait on select error) */
-#define USLEEP_WAITLONG USLEEP_SLEEP
-#endif
 
-/* 
- * Function : int should_disconnect (unsigned char cmd)
- *
- * Purpose : decide whether a command would normally disconnect or 
- *      not, since if it won't disconnect we should go to sleep.
- *
- * Input : cmd - opcode of SCSI command
- *
- * Returns : DISCONNECT_LONG if we should disconnect for a really long 
- *      time (ie always, sleep, look for REQ active, sleep), 
- *      DISCONNECT_TIME_TO_DATA if we would only disconnect for a normal
- *      time-to-data delay, DISCONNECT_NONE if this command would return
- *      immediately.
- *
- *      Future sleep algorithms based on time to data can exploit 
- *      something like this so they can differentiate between "normal" 
- *      (ie, read, write, seek) and unusual commands (ie, * format).
- *
- * Note : We don't deal with commands that handle an immediate disconnect,
- *        
- */
-
-static int should_disconnect(unsigned char cmd)
-{
-	switch (cmd) {
-	case READ_6:
-	case WRITE_6:
-	case SEEK_6:
-	case READ_10:
-	case WRITE_10:
-	case SEEK_10:
-		return DISCONNECT_TIME_TO_DATA;
-	case FORMAT_UNIT:
-	case SEARCH_HIGH:
-	case SEARCH_LOW:
-	case SEARCH_EQUAL:
-		return DISCONNECT_LONG;
-	default:
-		return DISCONNECT_NONE;
-	}
-}
-
-static void NCR5380_set_timer(struct NCR5380_hostdata *hostdata, unsigned long timeout)
-{
-	hostdata->time_expires = jiffies + timeout;
-	schedule_delayed_work(&hostdata->coroutine, timeout);
-}
-
-
-static int probe_irq __initdata = 0;
+static int probe_irq __initdata;
 
 /**
- *	probe_intr	-	helper for IRQ autoprobe
- *	@irq: interrupt number
- *	@dev_id: unused
- *	@regs: unused
+ * probe_intr	-	helper for IRQ autoprobe
+ * @irq: interrupt number
+ * @dev_id: unused
+ * @regs: unused
  *
- *	Set a flag to indicate the IRQ in question was received. This is
- *	used by the IRQ probe code.
+ * Set a flag to indicate the IRQ in question was received. This is
+ * used by the IRQ probe code.
  */
- 
+
 static irqreturn_t __init probe_intr(int irq, void *dev_id)
 {
 	probe_irq = irq;
@@ -553,24 +398,20 @@ static irqreturn_t __init probe_intr(int irq, void *dev_id)
 }
 
 /**
- *	NCR5380_probe_irq	-	find the IRQ of an NCR5380
- *	@instance: NCR5380 controller
- *	@possible: bitmask of ISA IRQ lines
+ * NCR5380_probe_irq	-	find the IRQ of an NCR5380
+ * @instance: NCR5380 controller
+ * @possible: bitmask of ISA IRQ lines
  *
- *	Autoprobe for the IRQ line used by the NCR5380 by triggering an IRQ
- *	and then looking to see what interrupt actually turned up.
- *
- *	Locks: none, irqs must be enabled on entry
+ * Autoprobe for the IRQ line used by the NCR5380 by triggering an IRQ
+ * and then looking to see what interrupt actually turned up.
  */
 
 static int __init __maybe_unused NCR5380_probe_irq(struct Scsi_Host *instance,
 						int possible)
 {
-	NCR5380_local_declare();
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned long timeout;
 	int trying_irqs, i, mask;
-	NCR5380_setup(instance);
 
 	for (trying_irqs = 0, i = 1, mask = 2; i < 16; ++i, mask <<= 1)
 		if ((mask & possible) && (request_irq(i, &probe_intr, 0, "NCR-probe", NULL) == 0))
@@ -581,7 +422,7 @@ static int __init __maybe_unused NCR5380_probe_irq(struct Scsi_Host *instance,
 
 	/*
 	 * A interrupt is triggered whenever BSY = false, SEL = true
-	 * and a bit set in the SELECT_ENABLE_REG is asserted on the 
+	 * and a bit set in the SELECT_ENABLE_REG is asserted on the
 	 * SCSI bus.
 	 *
 	 * Note that the bus is only driven when the phase control signals
@@ -596,7 +437,7 @@ static int __init __maybe_unused NCR5380_probe_irq(struct Scsi_Host *instance,
 
 	while (probe_irq == NO_IRQ && time_before(jiffies, timeout))
 		schedule_timeout_uninterruptible(1);
-	
+
 	NCR5380_write(SELECT_ENABLE_REG, 0);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
@@ -608,12 +449,10 @@ static int __init __maybe_unused NCR5380_probe_irq(struct Scsi_Host *instance,
 }
 
 /**
- *	NCR58380_info - report driver and host information
- *	@instance: relevant scsi host instance
- *
- *	For use as the host template info() handler.
+ * NCR58380_info - report driver and host information
+ * @instance: relevant scsi host instance
  *
- *	Locks: none
+ * For use as the host template info() handler.
  */
 
 static const char *NCR5380_info(struct Scsi_Host *instance)
@@ -633,20 +472,14 @@ static void prepare_info(struct Scsi_Host *instance)
 	         "can_queue %d, cmd_per_lun %d, "
 	         "sg_tablesize %d, this_id %d, "
 	         "flags { %s%s%s}, "
-#if defined(USLEEP_POLL) && defined(USLEEP_WAITLONG)
-		 "USLEEP_POLL %lu, USLEEP_WAITLONG %lu, "
-#endif
 	         "options { %s} ",
 	         instance->hostt->name, instance->io_port, instance->n_io_port,
 	         instance->base, instance->irq,
 	         instance->can_queue, instance->cmd_per_lun,
 	         instance->sg_tablesize, instance->this_id,
-	         hostdata->flags & FLAG_NCR53C400     ? "NCR53C400 "     : "",
-	         hostdata->flags & FLAG_DTC3181E      ? "DTC3181E "      : "",
+	         hostdata->flags & FLAG_NO_DMA_FIXUP  ? "NO_DMA_FIXUP "  : "",
 	         hostdata->flags & FLAG_NO_PSEUDO_DMA ? "NO_PSEUDO_DMA " : "",
-#if defined(USLEEP_POLL) && defined(USLEEP_WAITLONG)
-	         USLEEP_POLL, USLEEP_WAITLONG,
-#endif
+	         hostdata->flags & FLAG_TOSHIBA_DELAY ? "TOSHIBA_DELAY "  : "",
 #ifdef AUTOPROBE_IRQ
 	         "AUTOPROBE_IRQ "
 #endif
@@ -665,46 +498,10 @@ static void prepare_info(struct Scsi_Host *instance)
 #ifdef PSEUDO_DMA
 	         "PSEUDO_DMA "
 #endif
-#ifdef UNSAFE
-	         "UNSAFE "
-#endif
-#ifdef NCR53C400
-	         "NCR53C400 "
-#endif
 	         "");
 }
 
-/**
- *	NCR5380_print_status 	-	dump controller info
- *	@instance: controller to dump
- *
- *	Print commands in the various queues, called from NCR5380_abort 
- *	and NCR5380_debug to aid debugging.
- *
- *	Locks: called functions disable irqs
- */
-
-static void NCR5380_print_status(struct Scsi_Host *instance)
-{
-	NCR5380_dprint(NDEBUG_ANY, instance);
-	NCR5380_dprint_phase(NDEBUG_ANY, instance);
-}
-
 #ifdef PSEUDO_DMA
-/******************************************/
-/*
- * /proc/scsi/[dtc pas16 t128 generic]/[0-ASC_NUM_BOARD_SUPPORTED]
- *
- * *buffer: I/O buffer
- * **start: if inout == FALSE pointer into buffer where user read should start
- * offset: current offset
- * length: length of buffer
- * hostno: Scsi_Host host_no
- * inout: TRUE - user is writing; FALSE - user is reading
- *
- * Return the number of bytes read from or written
- */
-
 static int __maybe_unused NCR5380_write_info(struct Scsi_Host *instance,
 	char *buffer, int length)
 {
@@ -714,104 +511,41 @@ static int __maybe_unused NCR5380_write_info(struct Scsi_Host *instance,
 	hostdata->spin_max_w = 0;
 	return 0;
 }
-#endif
-
-static
-void lprint_Scsi_Cmnd(struct scsi_cmnd *cmd, struct seq_file *m);
-static
-void lprint_command(unsigned char *cmd, struct seq_file *m);
-static
-void lprint_opcode(int opcode, struct seq_file *m);
 
 static int __maybe_unused NCR5380_show_info(struct seq_file *m,
-	struct Scsi_Host *instance)
+                                            struct Scsi_Host *instance)
 {
-	struct NCR5380_hostdata *hostdata;
-	struct scsi_cmnd *ptr;
-
-	hostdata = (struct NCR5380_hostdata *) instance->hostdata;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
-#ifdef PSEUDO_DMA
 	seq_printf(m, "Highwater I/O busy spin counts: write %d, read %d\n",
 	        hostdata->spin_max_w, hostdata->spin_max_r);
-#endif
-	spin_lock_irq(instance->host_lock);
-	if (!hostdata->connected)
-		seq_printf(m, "scsi%d: no currently connected command\n", instance->host_no);
-	else
-		lprint_Scsi_Cmnd((struct scsi_cmnd *) hostdata->connected, m);
-	seq_printf(m, "scsi%d: issue_queue\n", instance->host_no);
-	for (ptr = (struct scsi_cmnd *) hostdata->issue_queue; ptr; ptr = (struct scsi_cmnd *) ptr->host_scribble)
-		lprint_Scsi_Cmnd(ptr, m);
-
-	seq_printf(m, "scsi%d: disconnected_queue\n", instance->host_no);
-	for (ptr = (struct scsi_cmnd *) hostdata->disconnected_queue; ptr; ptr = (struct scsi_cmnd *) ptr->host_scribble)
-		lprint_Scsi_Cmnd(ptr, m);
-	spin_unlock_irq(instance->host_lock);
 	return 0;
 }
-
-static void lprint_Scsi_Cmnd(struct scsi_cmnd *cmd, struct seq_file *m)
-{
-	seq_printf(m, "scsi%d : destination target %d, lun %llu\n", cmd->device->host->host_no, cmd->device->id, cmd->device->lun);
-	seq_puts(m, "        command = ");
-	lprint_command(cmd->cmnd, m);
-}
-
-static void lprint_command(unsigned char *command, struct seq_file *m)
-{
-	int i, s;
-	lprint_opcode(command[0], m);
-	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
-		seq_printf(m, "%02x ", command[i]);
-	seq_putc(m, '\n');
-}
-
-static void lprint_opcode(int opcode, struct seq_file *m)
-{
-	seq_printf(m, "%2d (0x%02x)", opcode, opcode);
-}
-
+#endif
 
 /**
- *	NCR5380_init	-	initialise an NCR5380
- *	@instance: adapter to configure
- *	@flags: control flags
+ * NCR5380_init - initialise an NCR5380
+ * @instance: adapter to configure
+ * @flags: control flags
  *
- *	Initializes *instance and corresponding 5380 chip,
- *      with flags OR'd into the initial flags value.
+ * Initializes *instance and corresponding 5380 chip,
+ * with flags OR'd into the initial flags value.
  *
- *	Notes : I assume that the host, hostno, and id bits have been
- *      set correctly.  I don't care about the irq and other fields. 
+ * Notes : I assume that the host, hostno, and id bits have been
+ * set correctly. I don't care about the irq and other fields.
  *
- *	Returns 0 for success
- *
- *	Locks: interrupts must be enabled when we are called 
+ * Returns 0 for success
  */
 
 static int NCR5380_init(struct Scsi_Host *instance, int flags)
 {
-	NCR5380_local_declare();
-	int i, pass;
-	unsigned long timeout;
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-
-	if(in_interrupt())
-		printk(KERN_ERR "NCR5380_init called with interrupts off!\n");
-	/* 
-	 * On NCR53C400 boards, NCR5380 registers are mapped 8 past 
-	 * the base address.
-	 */
-
-#ifdef NCR53C400
-	if (flags & FLAG_NCR53C400)
-		instance->NCR5380_instance_name += NCR53C400_address_adjust;
-#endif
-
-	NCR5380_setup(instance);
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int i;
+	unsigned long deadline;
 
-	hostdata->aborted = 0;
+	hostdata->host = instance;
 	hostdata->id_mask = 1 << instance->this_id;
+	hostdata->id_higher_mask = 0;
 	for (i = hostdata->id_mask; i <= 0x80; i <<= 1)
 		if (i > hostdata->id_mask)
 			hostdata->id_higher_mask |= i;
@@ -820,21 +554,21 @@ static int NCR5380_init(struct Scsi_Host *instance, int flags)
 #ifdef REAL_DMA
 	hostdata->dmalen = 0;
 #endif
-	hostdata->targets_present = 0;
+	spin_lock_init(&hostdata->lock);
 	hostdata->connected = NULL;
-	hostdata->issue_queue = NULL;
-	hostdata->disconnected_queue = NULL;
-	
-	INIT_DELAYED_WORK(&hostdata->coroutine, NCR5380_main);
-	
-	/* The CHECK code seems to break the 53C400. Will check it later maybe */
-	if (flags & FLAG_NCR53C400)
-		hostdata->flags = FLAG_HAS_LAST_BYTE_SENT | flags;
-	else
-		hostdata->flags = FLAG_CHECK_LAST_BYTE_SENT | flags;
+	hostdata->sensing = NULL;
+	INIT_LIST_HEAD(&hostdata->autosense);
+	INIT_LIST_HEAD(&hostdata->unissued);
+	INIT_LIST_HEAD(&hostdata->disconnected);
 
-	hostdata->host = instance;
-	hostdata->time_expires = 0;
+	hostdata->flags = flags;
+
+	INIT_WORK(&hostdata->main_task, NCR5380_main);
+	hostdata->work_q = alloc_workqueue("ncr5380_%d",
+	                        WQ_UNBOUND | WQ_MEM_RECLAIM,
+	                        1, instance->host_no);
+	if (!hostdata->work_q)
+		return -ENOMEM;
 
 	prepare_info(instance);
 
@@ -843,43 +577,69 @@ static int NCR5380_init(struct Scsi_Host *instance, int flags)
 	NCR5380_write(TARGET_COMMAND_REG, 0);
 	NCR5380_write(SELECT_ENABLE_REG, 0);
 
-#ifdef NCR53C400
-	if (hostdata->flags & FLAG_NCR53C400) {
-		NCR5380_write(C400_CONTROL_STATUS_REG, CSR_BASE);
-	}
-#endif
+	/* Calibrate register polling loop */
+	i = 0;
+	deadline = jiffies + 1;
+	do {
+		cpu_relax();
+	} while (time_is_after_jiffies(deadline));
+	deadline += msecs_to_jiffies(256);
+	do {
+		NCR5380_read(STATUS_REG);
+		++i;
+		cpu_relax();
+	} while (time_is_after_jiffies(deadline));
+	hostdata->accesses_per_ms = i / 256;
 
-	/* 
-	 * Detect and correct bus wedge problems.
-	 *
-	 * If the system crashed, it may have crashed in a state 
-	 * where a SCSI command was still executing, and the 
-	 * SCSI bus is not in a BUS FREE STATE.
-	 *
-	 * If this is the case, we'll try to abort the currently
-	 * established nexus which we know nothing about, and that
-	 * failing, do a hard reset of the SCSI bus 
-	 */
+	return 0;
+}
+
+/**
+ * NCR5380_maybe_reset_bus - Detect and correct bus wedge problems.
+ * @instance: adapter to check
+ *
+ * If the system crashed, it may have crashed with a connected target and
+ * the SCSI bus busy. Check for BUS FREE phase. If not, try to abort the
+ * currently established nexus, which we know nothing about. Failing that
+ * do a bus reset.
+ *
+ * Note that a bus reset will cause the chip to assert IRQ.
+ *
+ * Returns 0 if successful, otherwise -ENXIO.
+ */
+
+static int NCR5380_maybe_reset_bus(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int pass;
 
 	for (pass = 1; (NCR5380_read(STATUS_REG) & SR_BSY) && pass <= 6; ++pass) {
 		switch (pass) {
 		case 1:
 		case 3:
 		case 5:
-			printk(KERN_INFO "scsi%d: SCSI bus busy, waiting up to five seconds\n", instance->host_no);
-			timeout = jiffies + 5 * HZ;
-			NCR5380_poll_politely(instance, STATUS_REG, SR_BSY, 0, 5*HZ);
+			shost_printk(KERN_ERR, instance, "SCSI bus busy, waiting up to five seconds\n");
+			NCR5380_poll_politely(instance,
+			                      STATUS_REG, SR_BSY, 0, 5 * HZ);
 			break;
 		case 2:
-			printk(KERN_WARNING "scsi%d: bus busy, attempting abort\n", instance->host_no);
+			shost_printk(KERN_ERR, instance, "bus busy, attempting abort\n");
 			do_abort(instance);
 			break;
 		case 4:
-			printk(KERN_WARNING "scsi%d: bus busy, attempting reset\n", instance->host_no);
+			shost_printk(KERN_ERR, instance, "bus busy, attempting reset\n");
 			do_reset(instance);
+			/* Wait after a reset; the SCSI standard calls for
+			 * 250ms, we wait 500ms to be on the safe side.
+			 * But some Toshiba CD-ROMs need ten times that.
+			 */
+			if (hostdata->flags & FLAG_TOSHIBA_DELAY)
+				msleep(2500);
+			else
+				msleep(500);
 			break;
 		case 6:
-			printk(KERN_ERR "scsi%d: bus locked solid or invalid override\n", instance->host_no);
+			shost_printk(KERN_ERR, instance, "bus locked solid\n");
 			return -ENXIO;
 		}
 	}
@@ -887,450 +647,513 @@ static int NCR5380_init(struct Scsi_Host *instance, int flags)
 }
 
 /**
- *	NCR5380_exit	-	remove an NCR5380
- *	@instance: adapter to remove
+ * NCR5380_exit - remove an NCR5380
+ * @instance: adapter to remove
+ *
+ * Assumes that no more work can be queued (e.g. by NCR5380_intr).
  */
 
 static void NCR5380_exit(struct Scsi_Host *instance)
 {
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
-	cancel_delayed_work_sync(&hostdata->coroutine);
+	cancel_work_sync(&hostdata->main_task);
+	destroy_workqueue(hostdata->work_q);
 }
 
 /**
- *	NCR5380_queue_command 		-	queue a command
- *	@cmd: SCSI command
- *	@done: completion handler
- *
- *      cmd is added to the per instance issue_queue, with minor 
- *      twiddling done to the host specific fields of cmd.  If the 
- *      main coroutine is not running, it is restarted.
+ * complete_cmd - finish processing a command and return it to the SCSI ML
+ * @instance: the host instance
+ * @cmd: command to complete
+ */
+
+static void complete_cmd(struct Scsi_Host *instance,
+                         struct scsi_cmnd *cmd)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+
+	dsprintk(NDEBUG_QUEUES, instance, "complete_cmd: cmd %p\n", cmd);
+
+	if (hostdata->sensing == cmd) {
+		/* Autosense processing ends here */
+		if ((cmd->result & 0xff) != SAM_STAT_GOOD) {
+			scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+			set_host_byte(cmd, DID_ERROR);
+		} else
+			scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+		hostdata->sensing = NULL;
+	}
+
+	hostdata->busy[scmd_id(cmd)] &= ~(1 << cmd->device->lun);
+
+	cmd->scsi_done(cmd);
+}
+
+/**
+ * NCR5380_queue_command - queue a command
+ * @instance: the relevant SCSI adapter
+ * @cmd: SCSI command
  *
- *	Locks: host lock taken by caller
+ * cmd is added to the per-instance issue queue, with minor
+ * twiddling done to the host specific fields of cmd.  If the
+ * main coroutine is not running, it is restarted.
  */
 
-static int NCR5380_queue_command_lck(struct scsi_cmnd *cmd, void (*done) (struct scsi_cmnd *))
+static int NCR5380_queue_command(struct Scsi_Host *instance,
+                                 struct scsi_cmnd *cmd)
 {
-	struct Scsi_Host *instance = cmd->device->host;
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-	struct scsi_cmnd *tmp;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
+	unsigned long flags;
 
 #if (NDEBUG & NDEBUG_NO_WRITE)
 	switch (cmd->cmnd[0]) {
 	case WRITE_6:
 	case WRITE_10:
-		printk("scsi%d : WRITE attempted with NO_WRITE debugging flag set\n", instance->host_no);
+		shost_printk(KERN_DEBUG, instance, "WRITE attempted with NDEBUG_NO_WRITE set\n");
 		cmd->result = (DID_ERROR << 16);
-		done(cmd);
+		cmd->scsi_done(cmd);
 		return 0;
 	}
-#endif				/* (NDEBUG & NDEBUG_NO_WRITE) */
-
-	/* 
-	 * We use the host_scribble field as a pointer to the next command  
-	 * in a queue 
-	 */
+#endif /* (NDEBUG & NDEBUG_NO_WRITE) */
 
-	cmd->host_scribble = NULL;
-	cmd->scsi_done = done;
 	cmd->result = 0;
 
-	/* 
-	 * Insert the cmd into the issue queue. Note that REQUEST SENSE 
+	spin_lock_irqsave(&hostdata->lock, flags);
+
+	/*
+	 * Insert the cmd into the issue queue. Note that REQUEST SENSE
 	 * commands are added to the head of the queue since any command will
-	 * clear the contingent allegiance condition that exists and the 
+	 * clear the contingent allegiance condition that exists and the
 	 * sense data is only guaranteed to be valid while the condition exists.
 	 */
 
-	if (!(hostdata->issue_queue) || (cmd->cmnd[0] == REQUEST_SENSE)) {
-		LIST(cmd, hostdata->issue_queue);
-		cmd->host_scribble = (unsigned char *) hostdata->issue_queue;
-		hostdata->issue_queue = cmd;
-	} else {
-		for (tmp = (struct scsi_cmnd *) hostdata->issue_queue; tmp->host_scribble; tmp = (struct scsi_cmnd *) tmp->host_scribble);
-		LIST(cmd, tmp);
-		tmp->host_scribble = (unsigned char *) cmd;
-	}
-	dprintk(NDEBUG_QUEUES, "scsi%d : command added to %s of queue\n", instance->host_no, (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
+	if (cmd->cmnd[0] == REQUEST_SENSE)
+		list_add(&ncmd->list, &hostdata->unissued);
+	else
+		list_add_tail(&ncmd->list, &hostdata->unissued);
+
+	spin_unlock_irqrestore(&hostdata->lock, flags);
+
+	dsprintk(NDEBUG_QUEUES, instance, "command %p added to %s of queue\n",
+	         cmd, (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
 
-	/* Run the coroutine if it isn't already running. */
 	/* Kick off command processing */
-	schedule_delayed_work(&hostdata->coroutine, 0);
+	queue_work(hostdata->work_q, &hostdata->main_task);
 	return 0;
 }
 
-static DEF_SCSI_QCMD(NCR5380_queue_command)
+/**
+ * dequeue_next_cmd - dequeue a command for processing
+ * @instance: the scsi host instance
+ *
+ * Priority is given to commands on the autosense queue. These commands
+ * need autosense because of a CHECK CONDITION result.
+ *
+ * Returns a command pointer if a command is found for a target that is
+ * not already busy. Otherwise returns NULL.
+ */
+
+static struct scsi_cmnd *dequeue_next_cmd(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	struct NCR5380_cmd *ncmd;
+	struct scsi_cmnd *cmd;
+
+	if (list_empty(&hostdata->autosense)) {
+		list_for_each_entry(ncmd, &hostdata->unissued, list) {
+			cmd = NCR5380_to_scmd(ncmd);
+			dsprintk(NDEBUG_QUEUES, instance, "dequeue: cmd=%p target=%d busy=0x%02x lun=%llu\n",
+			         cmd, scmd_id(cmd), hostdata->busy[scmd_id(cmd)], cmd->device->lun);
+
+			if (!(hostdata->busy[scmd_id(cmd)] & (1 << cmd->device->lun))) {
+				list_del(&ncmd->list);
+				dsprintk(NDEBUG_QUEUES, instance,
+				         "dequeue: removed %p from issue queue\n", cmd);
+				return cmd;
+			}
+		}
+	} else {
+		/* Autosense processing begins here */
+		ncmd = list_first_entry(&hostdata->autosense,
+		                        struct NCR5380_cmd, list);
+		list_del(&ncmd->list);
+		cmd = NCR5380_to_scmd(ncmd);
+		dsprintk(NDEBUG_QUEUES, instance,
+		         "dequeue: removed %p from autosense queue\n", cmd);
+		scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
+		hostdata->sensing = cmd;
+		return cmd;
+	}
+	return NULL;
+}
+
+static void requeue_cmd(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
+
+	if (hostdata->sensing) {
+		scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+		list_add(&ncmd->list, &hostdata->autosense);
+		hostdata->sensing = NULL;
+	} else
+		list_add(&ncmd->list, &hostdata->unissued);
+}
 
 /**
- *	NCR5380_main	-	NCR state machines
- *
- *	NCR5380_main is a coroutine that runs as long as more work can 
- *      be done on the NCR5380 host adapters in a system.  Both 
- *      NCR5380_queue_command() and NCR5380_intr() will try to start it 
- *      in case it is not running.
- * 
- *	Locks: called as its own thread with no locks held. Takes the
- *	host lock and called routines may take the isa dma lock.
+ * NCR5380_main - NCR state machines
+ *
+ * NCR5380_main is a coroutine that runs as long as more work can
+ * be done on the NCR5380 host adapters in a system.  Both
+ * NCR5380_queue_command() and NCR5380_intr() will try to start it
+ * in case it is not running.
  */
 
 static void NCR5380_main(struct work_struct *work)
 {
 	struct NCR5380_hostdata *hostdata =
-		container_of(work, struct NCR5380_hostdata, coroutine.work);
+		container_of(work, struct NCR5380_hostdata, main_task);
 	struct Scsi_Host *instance = hostdata->host;
-	struct scsi_cmnd *tmp, *prev;
+	struct scsi_cmnd *cmd;
 	int done;
-	
-	spin_lock_irq(instance->host_lock);
+
 	do {
-		/* Lock held here */
 		done = 1;
-		if (!hostdata->connected && !hostdata->selecting) {
-			dprintk(NDEBUG_MAIN, "scsi%d : not connected\n", instance->host_no);
-			/*
-			 * Search through the issue_queue for a command destined
-			 * for a target that's not busy.
-			 */
-			for (tmp = (struct scsi_cmnd *) hostdata->issue_queue, prev = NULL; tmp; prev = tmp, tmp = (struct scsi_cmnd *) tmp->host_scribble)
-			{
-				if (prev != tmp)
-				    dprintk(NDEBUG_LISTS, "MAIN tmp=%p   target=%d   busy=%d lun=%llu\n", tmp, tmp->device->id, hostdata->busy[tmp->device->id], tmp->device->lun);
-				/*  When we find one, remove it from the issue queue. */
-				if (!(hostdata->busy[tmp->device->id] &
-				      (1 << (u8)(tmp->device->lun & 0xff)))) {
-					if (prev) {
-						REMOVE(prev, prev->host_scribble, tmp, tmp->host_scribble);
-						prev->host_scribble = tmp->host_scribble;
-					} else {
-						REMOVE(-1, hostdata->issue_queue, tmp, tmp->host_scribble);
-						hostdata->issue_queue = (struct scsi_cmnd *) tmp->host_scribble;
-					}
-					tmp->host_scribble = NULL;
 
-					/* 
-					 * Attempt to establish an I_T_L nexus here. 
-					 * On success, instance->hostdata->connected is set.
-					 * On failure, we must add the command back to the
-					 *   issue queue so we can keep trying. 
-					 */
-					dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, "scsi%d : main() : command for target %d lun %llu removed from issue_queue\n", instance->host_no, tmp->device->id, tmp->device->lun);
-	
-					/*
-					 * A successful selection is defined as one that 
-					 * leaves us with the command connected and 
-					 * in hostdata->connected, OR has terminated the
-					 * command.
-					 *
-					 * With successful commands, we fall through
-					 * and see if we can do an information transfer,
-					 * with failures we will restart.
-					 */
-					hostdata->selecting = NULL;
-					/* RvC: have to preset this to indicate a new command is being performed */
+		spin_lock_irq(&hostdata->lock);
+		while (!hostdata->connected &&
+		       (cmd = dequeue_next_cmd(instance))) {
 
-					/*
-					 * REQUEST SENSE commands are issued without tagged
-					 * queueing, even on SCSI-II devices because the
-					 * contingent allegiance condition exists for the
-					 * entire unit.
-					 */
+			dsprintk(NDEBUG_MAIN, instance, "main: dequeued %p\n", cmd);
 
-					if (!NCR5380_select(instance, tmp)) {
-						break;
-					} else {
-						LIST(tmp, hostdata->issue_queue);
-						tmp->host_scribble = (unsigned char *) hostdata->issue_queue;
-						hostdata->issue_queue = tmp;
-						done = 0;
-						dprintk(NDEBUG_MAIN|NDEBUG_QUEUES, "scsi%d : main(): select() failed, returned to issue_queue\n", instance->host_no);
-					}
-					/* lock held here still */
-				}	/* if target/lun is not busy */
-			}	/* for */
-			/* exited locked */
-		}	/* if (!hostdata->connected) */
-		if (hostdata->selecting) {
-			tmp = (struct scsi_cmnd *) hostdata->selecting;
-			/* Selection will drop and retake the lock */
-			if (!NCR5380_select(instance, tmp)) {
-				/* Ok ?? */
+			/*
+			 * Attempt to establish an I_T_L nexus here.
+			 * On success, instance->hostdata->connected is set.
+			 * On failure, we must add the command back to the
+			 * issue queue so we can keep trying.
+			 */
+			/*
+			 * REQUEST SENSE commands are issued without tagged
+			 * queueing, even on SCSI-II devices because the
+			 * contingent allegiance condition exists for the
+			 * entire unit.
+			 */
+
+			cmd = NCR5380_select(instance, cmd);
+			if (!cmd) {
+				dsprintk(NDEBUG_MAIN, instance, "main: select complete\n");
 			} else {
-				/* RvC: device failed, so we wait a long time
-				   this is needed for Mustek scanners, that
-				   do not respond to commands immediately
-				   after a scan */
-				printk(KERN_DEBUG "scsi%d: device %d did not respond in time\n", instance->host_no, tmp->device->id);
-				LIST(tmp, hostdata->issue_queue);
-				tmp->host_scribble = (unsigned char *) hostdata->issue_queue;
-				hostdata->issue_queue = tmp;
-				NCR5380_set_timer(hostdata, USLEEP_WAITLONG);
+				dsprintk(NDEBUG_MAIN | NDEBUG_QUEUES, instance,
+				         "main: select failed, returning %p to queue\n", cmd);
+				requeue_cmd(instance, cmd);
 			}
-		}	/* if hostdata->selecting */
+		}
 		if (hostdata->connected
 #ifdef REAL_DMA
 		    && !hostdata->dmalen
 #endif
-		    && (!hostdata->time_expires || time_before_eq(hostdata->time_expires, jiffies))
 		    ) {
-			dprintk(NDEBUG_MAIN, "scsi%d : main() : performing information transfer\n", instance->host_no);
+			dsprintk(NDEBUG_MAIN, instance, "main: performing information transfer\n");
 			NCR5380_information_transfer(instance);
-			dprintk(NDEBUG_MAIN, "scsi%d : main() : done set false\n", instance->host_no);
 			done = 0;
-		} else
-			break;
+		}
+		spin_unlock_irq(&hostdata->lock);
+		if (!done)
+			cond_resched();
 	} while (!done);
-	
-	spin_unlock_irq(instance->host_lock);
 }
 
 #ifndef DONT_USE_INTR
 
 /**
- * 	NCR5380_intr	-	generic NCR5380 irq handler
- *	@irq: interrupt number
- *	@dev_id: device info
- *
- *	Handle interrupts, reestablishing I_T_L or I_T_L_Q nexuses
- *      from the disconnected queue, and restarting NCR5380_main() 
- *      as required.
- *
- *	Locks: takes the needed instance locks
+ * NCR5380_intr - generic NCR5380 irq handler
+ * @irq: interrupt number
+ * @dev_id: device info
+ *
+ * Handle interrupts, reestablishing I_T_L or I_T_L_Q nexuses
+ * from the disconnected queue, and restarting NCR5380_main()
+ * as required.
+ *
+ * The chip can assert IRQ in any of six different conditions. The IRQ flag
+ * is then cleared by reading the Reset Parity/Interrupt Register (RPIR).
+ * Three of these six conditions are latched in the Bus and Status Register:
+ * - End of DMA (cleared by ending DMA Mode)
+ * - Parity error (cleared by reading RPIR)
+ * - Loss of BSY (cleared by reading RPIR)
+ * Two conditions have flag bits that are not latched:
+ * - Bus phase mismatch (non-maskable in DMA Mode, cleared by ending DMA Mode)
+ * - Bus reset (non-maskable)
+ * The remaining condition has no flag bit at all:
+ * - Selection/reselection
+ *
+ * Hence, establishing the cause(s) of any interrupt is partly guesswork.
+ * In "The DP8490 and DP5380 Comparison Guide", National Semiconductor
+ * claimed that "the design of the [DP8490] interrupt logic ensures
+ * interrupts will not be lost (they can be on the DP5380)."
+ * The L5380/53C80 datasheet from LOGIC Devices has more details.
+ *
+ * Checking for bus reset by reading RST is futile because of interrupt
+ * latency, but a bus reset will reset chip logic. Checking for parity error
+ * is unnecessary because that interrupt is never enabled. A Loss of BSY
+ * condition will clear DMA Mode. We can tell when this occurs because the
+ * the Busy Monitor interrupt is enabled together with DMA Mode.
  */
 
-static irqreturn_t NCR5380_intr(int dummy, void *dev_id)
+static irqreturn_t NCR5380_intr(int irq, void *dev_id)
 {
-	NCR5380_local_declare();
 	struct Scsi_Host *instance = dev_id;
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-	int done;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int handled = 0;
 	unsigned char basr;
 	unsigned long flags;
 
-	dprintk(NDEBUG_INTR, "scsi : NCR5380 irq %d triggered\n",
-		instance->irq);
+	spin_lock_irqsave(&hostdata->lock, flags);
+
+	basr = NCR5380_read(BUS_AND_STATUS_REG);
+	if (basr & BASR_IRQ) {
+		unsigned char mr = NCR5380_read(MODE_REG);
+		unsigned char sr = NCR5380_read(STATUS_REG);
+
+		dsprintk(NDEBUG_INTR, instance, "IRQ %d, BASR 0x%02x, SR 0x%02x, MR 0x%02x\n",
+		         irq, basr, sr, mr);
 
-	do {
-		done = 1;
-		spin_lock_irqsave(instance->host_lock, flags);
-		/* Look for pending interrupts */
-		NCR5380_setup(instance);
-		basr = NCR5380_read(BUS_AND_STATUS_REG);
-		/* XXX dispatch to appropriate routine if found and done=0 */
-		if (basr & BASR_IRQ) {
-			NCR5380_dprint(NDEBUG_INTR, instance);
-			if ((NCR5380_read(STATUS_REG) & (SR_SEL | SR_IO)) == (SR_SEL | SR_IO)) {
-				done = 0;
-				dprintk(NDEBUG_INTR, "scsi%d : SEL interrupt\n", instance->host_no);
-				NCR5380_reselect(instance);
-				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-			} else if (basr & BASR_PARITY_ERROR) {
-				dprintk(NDEBUG_INTR, "scsi%d : PARITY interrupt\n", instance->host_no);
-				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-			} else if ((NCR5380_read(STATUS_REG) & SR_RST) == SR_RST) {
-				dprintk(NDEBUG_INTR, "scsi%d : RESET interrupt\n", instance->host_no);
-				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-			} else {
 #if defined(REAL_DMA)
-				/*
-				 * We should only get PHASE MISMATCH and EOP interrupts
-				 * if we have DMA enabled, so do a sanity check based on
-				 * the current setting of the MODE register.
-				 */
+		if ((mr & MR_DMA_MODE) || (mr & MR_MONITOR_BSY)) {
+			/* Probably End of DMA, Phase Mismatch or Loss of BSY.
+			 * We ack IRQ after clearing Mode Register. Workarounds
+			 * for End of DMA errata need to happen in DMA Mode.
+			 */
 
-				if ((NCR5380_read(MODE_REG) & MR_DMA) && ((basr & BASR_END_DMA_TRANSFER) || !(basr & BASR_PHASE_MATCH))) {
-					int transferred;
+			dsprintk(NDEBUG_INTR, instance, "interrupt in DMA mode\n");
 
-					if (!hostdata->connected)
-						panic("scsi%d : received end of DMA interrupt with no connected cmd\n", instance->hostno);
+			int transferred;
 
-					transferred = (hostdata->dmalen - NCR5380_dma_residual(instance));
-					hostdata->connected->SCp.this_residual -= transferred;
-					hostdata->connected->SCp.ptr += transferred;
-					hostdata->dmalen = 0;
+			if (!hostdata->connected)
+				panic("scsi%d : DMA interrupt with no connected cmd\n",
+				      instance->hostno);
 
-					(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-							
-					/* FIXME: we need to poll briefly then defer a workqueue task ! */
-					NCR5380_poll_politely(hostdata, BUS_AND_STATUS_REG, BASR_ACK, 0, 2*HZ);
+			transferred = hostdata->dmalen - NCR5380_dma_residual(instance);
+			hostdata->connected->SCp.this_residual -= transferred;
+			hostdata->connected->SCp.ptr += transferred;
+			hostdata->dmalen = 0;
 
-					NCR5380_write(MODE_REG, MR_BASE);
-					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-				}
-#else
-				dprintk(NDEBUG_INTR, "scsi : unknown interrupt, BASR 0x%X, MR 0x%X, SR 0x%x\n", basr, NCR5380_read(MODE_REG), NCR5380_read(STATUS_REG));
-				(void) NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-#endif
+			/* FIXME: we need to poll briefly then defer a workqueue task ! */
+			NCR5380_poll_politely(hostdata, BUS_AND_STATUS_REG, BASR_ACK, 0, 2 * HZ);
+
+			NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+			NCR5380_write(MODE_REG, MR_BASE);
+			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+		} else
+#endif /* REAL_DMA */
+		if ((NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_mask) &&
+		    (sr & (SR_SEL | SR_IO | SR_BSY | SR_RST)) == (SR_SEL | SR_IO)) {
+			/* Probably reselected */
+			NCR5380_write(SELECT_ENABLE_REG, 0);
+			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+
+			dsprintk(NDEBUG_INTR, instance, "interrupt with SEL and IO\n");
+
+			if (!hostdata->connected) {
+				NCR5380_reselect(instance);
+				queue_work(hostdata->work_q, &hostdata->main_task);
 			}
-		}	/* if BASR_IRQ */
-		spin_unlock_irqrestore(instance->host_lock, flags);
-		if(!done)
-			schedule_delayed_work(&hostdata->coroutine, 0);
-	} while (!done);
-	return IRQ_HANDLED;
+			if (!hostdata->connected)
+				NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+		} else {
+			/* Probably Bus Reset */
+			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+
+			dsprintk(NDEBUG_INTR, instance, "unknown interrupt\n");
+		}
+		handled = 1;
+	} else {
+		shost_printk(KERN_NOTICE, instance, "interrupt without IRQ bit\n");
+	}
+
+	spin_unlock_irqrestore(&hostdata->lock, flags);
+
+	return IRQ_RETVAL(handled);
 }
 
-#endif 
+#endif
 
-/* 
+/*
  * Function : int NCR5380_select(struct Scsi_Host *instance,
- *                               struct scsi_cmnd *cmd)
+ * struct scsi_cmnd *cmd)
  *
  * Purpose : establishes I_T_L or I_T_L_Q nexus for new or existing command,
- *      including ARBITRATION, SELECTION, and initial message out for 
- *      IDENTIFY and queue messages. 
- *
- * Inputs : instance - instantiation of the 5380 driver on which this 
- *      target lives, cmd - SCSI command to execute.
- * 
- * Returns : -1 if selection could not execute for some reason,
- *      0 if selection succeeded or failed because the target 
- *      did not respond.
- *
- * Side effects : 
- *      If bus busy, arbitration failed, etc, NCR5380_select() will exit 
- *              with registers as they should have been on entry - ie
- *              SELECT_ENABLE will be set appropriately, the NCR5380
- *              will cease to drive any SCSI bus signals.
- *
- *      If successful : I_T_L or I_T_L_Q nexus will be established, 
- *              instance->connected will be set to cmd.  
- *              SELECT interrupt will be disabled.
- *
- *      If failed (no target) : cmd->scsi_done() will be called, and the 
- *              cmd->result host byte set to DID_BAD_TARGET.
- *
- *	Locks: caller holds hostdata lock in IRQ mode
+ * including ARBITRATION, SELECTION, and initial message out for
+ * IDENTIFY and queue messages.
+ *
+ * Inputs : instance - instantiation of the 5380 driver on which this
+ * target lives, cmd - SCSI command to execute.
+ *
+ * Returns cmd if selection failed but should be retried,
+ * NULL if selection failed and should not be retried, or
+ * NULL if selection succeeded (hostdata->connected == cmd).
+ *
+ * Side effects :
+ * If bus busy, arbitration failed, etc, NCR5380_select() will exit
+ * with registers as they should have been on entry - ie
+ * SELECT_ENABLE will be set appropriately, the NCR5380
+ * will cease to drive any SCSI bus signals.
+ *
+ * If successful : I_T_L or I_T_L_Q nexus will be established,
+ * instance->connected will be set to cmd.
+ * SELECT interrupt will be disabled.
+ *
+ * If failed (no target) : cmd->scsi_done() will be called, and the
+ * cmd->result host byte set to DID_BAD_TARGET.
  */
- 
-static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
+
+static struct scsi_cmnd *NCR5380_select(struct Scsi_Host *instance,
+                                        struct scsi_cmnd *cmd)
 {
-	NCR5380_local_declare();
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char tmp[3], phase;
 	unsigned char *data;
 	int len;
-	unsigned long timeout;
-	unsigned char value;
 	int err;
-	NCR5380_setup(instance);
-
-	if (hostdata->selecting)
-		goto part2;
-
-	hostdata->restart_select = 0;
 
 	NCR5380_dprint(NDEBUG_ARBITRATION, instance);
-	dprintk(NDEBUG_ARBITRATION, "scsi%d : starting arbitration, id = %d\n", instance->host_no, instance->this_id);
+	dsprintk(NDEBUG_ARBITRATION, instance, "starting arbitration, id = %d\n",
+	         instance->this_id);
+
+	/*
+	 * Arbitration and selection phases are slow and involve dropping the
+	 * lock, so we have to watch out for EH. An exception handler may
+	 * change 'selecting' to NULL. This function will then return NULL
+	 * so that the caller will forget about 'cmd'. (During information
+	 * transfer phases, EH may change 'connected' to NULL.)
+	 */
+	hostdata->selecting = cmd;
 
-	/* 
-	 * Set the phase bits to 0, otherwise the NCR5380 won't drive the 
+	/*
+	 * Set the phase bits to 0, otherwise the NCR5380 won't drive the
 	 * data bus during SELECTION.
 	 */
 
 	NCR5380_write(TARGET_COMMAND_REG, 0);
 
-	/* 
+	/*
 	 * Start arbitration.
 	 */
 
 	NCR5380_write(OUTPUT_DATA_REG, hostdata->id_mask);
 	NCR5380_write(MODE_REG, MR_ARBITRATE);
 
+	/* The chip now waits for BUS FREE phase. Then after the 800 ns
+	 * Bus Free Delay, arbitration will begin.
+	 */
 
-	/* We can be relaxed here, interrupts are on, we are
-	   in workqueue context, the birds are singing in the trees */
-	spin_unlock_irq(instance->host_lock);
-	err = NCR5380_poll_politely(instance, INITIATOR_COMMAND_REG, ICR_ARBITRATION_PROGRESS, ICR_ARBITRATION_PROGRESS, 5*HZ);
-	spin_lock_irq(instance->host_lock);
+	spin_unlock_irq(&hostdata->lock);
+	err = NCR5380_poll_politely2(instance, MODE_REG, MR_ARBITRATE, 0,
+	                INITIATOR_COMMAND_REG, ICR_ARBITRATION_PROGRESS,
+	                                       ICR_ARBITRATION_PROGRESS, HZ);
+	spin_lock_irq(&hostdata->lock);
+	if (!(NCR5380_read(MODE_REG) & MR_ARBITRATE)) {
+		/* Reselection interrupt */
+		goto out;
+	}
 	if (err < 0) {
-		printk(KERN_DEBUG "scsi: arbitration timeout at %d\n", __LINE__);
 		NCR5380_write(MODE_REG, MR_BASE);
-		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		goto failed;
+		shost_printk(KERN_ERR, instance,
+		             "select: arbitration timeout\n");
+		goto out;
 	}
+	spin_unlock_irq(&hostdata->lock);
 
-	dprintk(NDEBUG_ARBITRATION, "scsi%d : arbitration complete\n", instance->host_no);
-
-	/* 
-	 * The arbitration delay is 2.2us, but this is a minimum and there is 
-	 * no maximum so we can safely sleep for ceil(2.2) usecs to accommodate
-	 * the integral nature of udelay().
-	 *
-	 */
-
+	/* The SCSI-2 arbitration delay is 2.4 us */
 	udelay(3);
 
 	/* Check for lost arbitration */
-	if ((NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) || (NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_higher_mask) || (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST)) {
-		NCR5380_write(MODE_REG, MR_BASE);
-		dprintk(NDEBUG_ARBITRATION, "scsi%d : lost arbitration, deasserting MR_ARBITRATE\n", instance->host_no);
-		goto failed;
-	}
-	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_SEL);
-
-	if (!(hostdata->flags & FLAG_DTC3181E) &&
-	    /* RvC: DTC3181E has some trouble with this
-	     *      so we simply removed it. Seems to work with
-	     *      only Mustek scanner attached
-	     */
+	if ((NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
+	    (NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_higher_mask) ||
 	    (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST)) {
 		NCR5380_write(MODE_REG, MR_BASE);
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		dprintk(NDEBUG_ARBITRATION, "scsi%d : lost arbitration, deasserting ICR_ASSERT_SEL\n", instance->host_no);
-		goto failed;
+		dsprintk(NDEBUG_ARBITRATION, instance, "lost arbitration, deasserting MR_ARBITRATE\n");
+		spin_lock_irq(&hostdata->lock);
+		goto out;
 	}
-	/* 
-	 * Again, bus clear + bus settle time is 1.2us, however, this is 
+
+	/* After/during arbitration, BSY should be asserted.
+	 * IBM DPES-31080 Version S31Q works now
+	 * Tnx to Thomas_Roesch@m2.maus.de for finding this! (Roman)
+	 */
+	NCR5380_write(INITIATOR_COMMAND_REG,
+		      ICR_BASE | ICR_ASSERT_SEL | ICR_ASSERT_BSY);
+
+	/*
+	 * Again, bus clear + bus settle time is 1.2us, however, this is
 	 * a minimum so we'll udelay ceil(1.2)
 	 */
 
-	udelay(2);
+	if (hostdata->flags & FLAG_TOSHIBA_DELAY)
+		udelay(15);
+	else
+		udelay(2);
+
+	spin_lock_irq(&hostdata->lock);
+
+	/* NCR5380_reselect() clears MODE_REG after a reselection interrupt */
+	if (!(NCR5380_read(MODE_REG) & MR_ARBITRATE))
+		goto out;
+
+	if (!hostdata->selecting) {
+		NCR5380_write(MODE_REG, MR_BASE);
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+		goto out;
+	}
 
-	dprintk(NDEBUG_ARBITRATION, "scsi%d : won arbitration\n", instance->host_no);
+	dsprintk(NDEBUG_ARBITRATION, instance, "won arbitration\n");
 
-	/* 
-	 * Now that we have won arbitration, start Selection process, asserting 
+	/*
+	 * Now that we have won arbitration, start Selection process, asserting
 	 * the host and target ID's on the SCSI bus.
 	 */
 
-	NCR5380_write(OUTPUT_DATA_REG, (hostdata->id_mask | (1 << scmd_id(cmd))));
+	NCR5380_write(OUTPUT_DATA_REG, hostdata->id_mask | (1 << scmd_id(cmd)));
 
-	/* 
+	/*
 	 * Raise ATN while SEL is true before BSY goes false from arbitration,
 	 * since this is the only way to guarantee that we'll get a MESSAGE OUT
 	 * phase immediately after selection.
 	 */
 
-	NCR5380_write(INITIATOR_COMMAND_REG, (ICR_BASE | ICR_ASSERT_BSY | ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_SEL));
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_BSY |
+	              ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_SEL);
 	NCR5380_write(MODE_REG, MR_BASE);
 
-	/* 
+	/*
 	 * Reselect interrupts must be turned off prior to the dropping of BSY,
 	 * otherwise we will trigger an interrupt.
 	 */
 	NCR5380_write(SELECT_ENABLE_REG, 0);
 
+	spin_unlock_irq(&hostdata->lock);
+
 	/*
-	 * The initiator shall then wait at least two deskew delays and release 
+	 * The initiator shall then wait at least two deskew delays and release
 	 * the BSY signal.
 	 */
-	udelay(1);		/* wingel -- wait two bus deskew delay >2*45ns */
+	udelay(1);        /* wingel -- wait two bus deskew delay >2*45ns */
 
 	/* Reset BSY */
-	NCR5380_write(INITIATOR_COMMAND_REG, (ICR_BASE | ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_SEL));
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA |
+	              ICR_ASSERT_ATN | ICR_ASSERT_SEL);
 
-	/* 
+	/*
 	 * Something weird happens when we cease to drive BSY - looks
-	 * like the board/chip is letting us do another read before the 
+	 * like the board/chip is letting us do another read before the
 	 * appropriate propagation delay has expired, and we're confusing
 	 * a BSY signal from ourselves as the target's response to SELECTION.
 	 *
 	 * A small delay (the 'C++' frontend breaks the pipeline with an
 	 * unnecessary jump, making it work on my 386-33/Trantor T128, the
-	 * tighter 'C' code breaks and requires this) solves the problem - 
-	 * the 1 us delay is arbitrary, and only used because this delay will 
-	 * be the same on other platforms and since it works here, it should 
+	 * tighter 'C' code breaks and requires this) solves the problem -
+	 * the 1 us delay is arbitrary, and only used because this delay will
+	 * be the same on other platforms and since it works here, it should
 	 * work there.
 	 *
 	 * wingel suggests that this could be due to failing to wait
@@ -1339,50 +1162,43 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 
 	udelay(1);
 
-	dprintk(NDEBUG_SELECTION, "scsi%d : selecting target %d\n", instance->host_no, scmd_id(cmd));
+	dsprintk(NDEBUG_SELECTION, instance, "selecting target %d\n", scmd_id(cmd));
 
-	/* 
-	 * The SCSI specification calls for a 250 ms timeout for the actual 
+	/*
+	 * The SCSI specification calls for a 250 ms timeout for the actual
 	 * selection.
 	 */
 
-	timeout = jiffies + msecs_to_jiffies(250);
-
-	/* 
-	 * XXX very interesting - we're seeing a bounce where the BSY we 
-	 * asserted is being reflected / still asserted (propagation delay?)
-	 * and it's detecting as true.  Sigh.
-	 */
-
-	hostdata->select_time = 0;	/* we count the clock ticks at which we polled */
-	hostdata->selecting = cmd;
+	err = NCR5380_poll_politely(instance, STATUS_REG, SR_BSY, SR_BSY,
+	                            msecs_to_jiffies(250));
 
-part2:
-	/* RvC: here we enter after a sleeping period, or immediately after
-	   execution of part 1
-	   we poll only once ech clock tick */
-	value = NCR5380_read(STATUS_REG) & (SR_BSY | SR_IO);
-
-	if (!value && (hostdata->select_time < HZ/4)) {
-		/* RvC: we still must wait for a device response */
-		hostdata->select_time++;	/* after 25 ticks the device has failed */
-		NCR5380_set_timer(hostdata, 1);
-		return 0;	/* RvC: we return here with hostdata->selecting set,
-				   to go to sleep */
-	}
-
-	hostdata->selecting = NULL;/* clear this pointer, because we passed the
-					   waiting period */
 	if ((NCR5380_read(STATUS_REG) & (SR_SEL | SR_IO)) == (SR_SEL | SR_IO)) {
+		spin_lock_irq(&hostdata->lock);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		NCR5380_reselect(instance);
-		printk("scsi%d : reselection after won arbitration?\n", instance->host_no);
+		if (!hostdata->connected)
+			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+		shost_printk(KERN_ERR, instance, "reselection after won arbitration?\n");
+		goto out;
+	}
+
+	if (err < 0) {
+		spin_lock_irq(&hostdata->lock);
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		return -1;
+		/* Can't touch cmd if it has been reclaimed by the scsi ML */
+		if (hostdata->selecting) {
+			cmd->result = DID_BAD_TARGET << 16;
+			complete_cmd(instance, cmd);
+			dsprintk(NDEBUG_SELECTION, instance, "target did not respond within 250ms\n");
+			cmd = NULL;
+		}
+		goto out;
 	}
-	/* 
-	 * No less than two deskew delays after the initiator detects the 
-	 * BSY signal is true, it shall release the SEL signal and may 
+
+	/*
+	 * No less than two deskew delays after the initiator detects the
+	 * BSY signal is true, it shall release the SEL signal and may
 	 * change the DATA BUS.                                     -wingel
 	 */
 
@@ -1390,53 +1206,38 @@ part2:
 
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 
-	if (!(NCR5380_read(STATUS_REG) & SR_BSY)) {
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		if (hostdata->targets_present & (1 << scmd_id(cmd))) {
-			printk(KERN_DEBUG "scsi%d : weirdness\n", instance->host_no);
-			if (hostdata->restart_select)
-				printk(KERN_DEBUG "\trestart select\n");
-			NCR5380_dprint(NDEBUG_SELECTION, instance);
-			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-			return -1;
-		}
-		cmd->result = DID_BAD_TARGET << 16;
-		cmd->scsi_done(cmd);
-		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		dprintk(NDEBUG_SELECTION, "scsi%d : target did not respond within 250ms\n", instance->host_no);
-		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		return 0;
-	}
-	hostdata->targets_present |= (1 << scmd_id(cmd));
-
 	/*
-	 * Since we followed the SCSI spec, and raised ATN while SEL 
+	 * Since we followed the SCSI spec, and raised ATN while SEL
 	 * was true but before BSY was false during selection, the information
 	 * transfer phase should be a MESSAGE OUT phase so that we can send the
 	 * IDENTIFY message.
-	 * 
+	 *
 	 * If SCSI-II tagged queuing is enabled, we also send a SIMPLE_QUEUE_TAG
 	 * message (2 bytes) with a tag ID that we increment with every command
 	 * until it wraps back to 0.
 	 *
 	 * XXX - it turns out that there are some broken SCSI-II devices,
-	 *       which claim to support tagged queuing but fail when more than
-	 *       some number of commands are issued at once.
+	 * which claim to support tagged queuing but fail when more than
+	 * some number of commands are issued at once.
 	 */
 
 	/* Wait for start of REQ/ACK handshake */
 
-	spin_unlock_irq(instance->host_lock);
 	err = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ);
-	spin_lock_irq(instance->host_lock);
-	
-	if(err) {
-		printk(KERN_ERR "scsi%d: timeout at NCR5380.c:%d\n", instance->host_no, __LINE__);
+	spin_lock_irq(&hostdata->lock);
+	if (err < 0) {
+		shost_printk(KERN_ERR, instance, "select: REQ timeout\n");
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		goto failed;
+		goto out;
+	}
+	if (!hostdata->selecting) {
+		do_abort(instance);
+		goto out;
 	}
 
-	dprintk(NDEBUG_SELECTION, "scsi%d : target %d selected, going into MESSAGE OUT phase.\n", instance->host_no, cmd->device->id);
+	dsprintk(NDEBUG_SELECTION, instance, "target %d selected, going into MESSAGE OUT phase.\n",
+	         scmd_id(cmd));
 	tmp[0] = IDENTIFY(((instance->irq == NO_IRQ) ? 0 : 1), cmd->device->lun);
 
 	len = 1;
@@ -1446,104 +1247,82 @@ part2:
 	data = tmp;
 	phase = PHASE_MSGOUT;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
-	dprintk(NDEBUG_SELECTION, "scsi%d : nexus established.\n", instance->host_no);
+	dsprintk(NDEBUG_SELECTION, instance, "nexus established.\n");
 	/* XXX need to handle errors here */
+
 	hostdata->connected = cmd;
-	hostdata->busy[cmd->device->id] |= (1 << (cmd->device->lun & 0xFF));
+	hostdata->busy[cmd->device->id] |= 1 << cmd->device->lun;
 
 	initialize_SCp(cmd);
 
-	return 0;
-
-	/* Selection failed */
-failed:
-	return -1;
+	cmd = NULL;
 
+out:
+	if (!hostdata->selecting)
+		return NULL;
+	hostdata->selecting = NULL;
+	return cmd;
 }
 
-/* 
- * Function : int NCR5380_transfer_pio (struct Scsi_Host *instance, 
- *      unsigned char *phase, int *count, unsigned char **data)
+/*
+ * Function : int NCR5380_transfer_pio (struct Scsi_Host *instance,
+ * unsigned char *phase, int *count, unsigned char **data)
  *
  * Purpose : transfers data in given phase using polled I/O
  *
- * Inputs : instance - instance of driver, *phase - pointer to 
- *      what phase is expected, *count - pointer to number of 
- *      bytes to transfer, **data - pointer to data pointer.
- * 
+ * Inputs : instance - instance of driver, *phase - pointer to
+ * what phase is expected, *count - pointer to number of
+ * bytes to transfer, **data - pointer to data pointer.
+ *
  * Returns : -1 when different phase is entered without transferring
- *      maximum number of bytes, 0 if all bytes or transferred or exit
- *      is in same phase.
+ * maximum number of bytes, 0 if all bytes are transferred or exit
+ * is in same phase.
  *
- *      Also, *phase, *count, *data are modified in place.
+ * Also, *phase, *count, *data are modified in place.
  *
  * XXX Note : handling for bus free may be useful.
  */
 
 /*
- * Note : this code is not as quick as it could be, however it 
+ * Note : this code is not as quick as it could be, however it
  * IS 100% reliable, and for the actual data transfer where speed
  * counts, we will always do a pseudo DMA or DMA transfer.
  */
 
-static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase, int *count, unsigned char **data) {
-	NCR5380_local_declare();
+static int NCR5380_transfer_pio(struct Scsi_Host *instance,
+				unsigned char *phase, int *count,
+				unsigned char **data)
+{
 	unsigned char p = *phase, tmp;
 	int c = *count;
 	unsigned char *d = *data;
-	/*
-	 *      RvC: some administrative data to process polling time
-	 */
-	int break_allowed = 0;
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-	NCR5380_setup(instance);
-
-	if (!(p & SR_IO))
-		dprintk(NDEBUG_PIO, "scsi%d : pio write %d bytes\n", instance->host_no, c);
-	else
-		dprintk(NDEBUG_PIO, "scsi%d : pio read %d bytes\n", instance->host_no, c);
 
-	/* 
-	 * The NCR5380 chip will only drive the SCSI bus when the 
+	/*
+	 * The NCR5380 chip will only drive the SCSI bus when the
 	 * phase specified in the appropriate bits of the TARGET COMMAND
 	 * REGISTER match the STATUS REGISTER
 	 */
 
-	 NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(p));
+	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(p));
 
-	/* RvC: don't know if this is necessary, but other SCSI I/O is short
-	 *      so breaks are not necessary there
-	 */
-	if ((p == PHASE_DATAIN) || (p == PHASE_DATAOUT)) {
-		break_allowed = 1;
-	}
 	do {
-		/* 
-		 * Wait for assertion of REQ, after which the phase bits will be 
-		 * valid 
-		 */
-
-		/* RvC: we simply poll once, after that we stop temporarily
-		 *      and let the device buffer fill up
-		 *      if breaking is not allowed, we keep polling as long as needed
+		/*
+		 * Wait for assertion of REQ, after which the phase bits will be
+		 * valid
 		 */
 
-		/* FIXME */
-		while (!((tmp = NCR5380_read(STATUS_REG)) & SR_REQ) && !break_allowed);
-		if (!(tmp & SR_REQ)) {
-			/* timeout condition */
-			NCR5380_set_timer(hostdata, USLEEP_SLEEP);
+		if (NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ) < 0)
 			break;
-		}
 
-		dprintk(NDEBUG_HANDSHAKE, "scsi%d : REQ detected\n", instance->host_no);
+		dsprintk(NDEBUG_HANDSHAKE, instance, "REQ asserted\n");
 
 		/* Check for phase mismatch */
-		if ((tmp & PHASE_MASK) != p) {
-			dprintk(NDEBUG_HANDSHAKE, "scsi%d : phase mismatch\n", instance->host_no);
-			NCR5380_dprint_phase(NDEBUG_HANDSHAKE, instance);
+		if ((NCR5380_read(STATUS_REG) & PHASE_MASK) != p) {
+			dsprintk(NDEBUG_PIO, instance, "phase mismatch\n");
+			NCR5380_dprint_phase(NDEBUG_PIO, instance);
 			break;
 		}
+
 		/* Do actual transfer from SCSI bus to / from memory */
 		if (!(p & SR_IO))
 			NCR5380_write(OUTPUT_DATA_REG, *d);
@@ -1552,7 +1331,7 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 
 		++d;
 
-		/* 
+		/*
 		 * The SCSI standard suggests that in MSGOUT phase, the initiator
 		 * should drop ATN on the last byte of the message phase
 		 * after REQ has been asserted for the handshake but before
@@ -1563,29 +1342,34 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 			if (!((p & SR_MSG) && c > 1)) {
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA);
 				NCR5380_dprint(NDEBUG_PIO, instance);
-				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA | ICR_ASSERT_ACK);
+				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
+				              ICR_ASSERT_DATA | ICR_ASSERT_ACK);
 			} else {
-				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA | ICR_ASSERT_ATN);
+				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
+				              ICR_ASSERT_DATA | ICR_ASSERT_ATN);
 				NCR5380_dprint(NDEBUG_PIO, instance);
-				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
+				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
+				              ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
 			}
 		} else {
 			NCR5380_dprint(NDEBUG_PIO, instance);
 			NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ACK);
 		}
 
-		/* FIXME - if this fails bus reset ?? */
-		NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, 0, 5*HZ);
-		dprintk(NDEBUG_HANDSHAKE, "scsi%d : req false, handshake complete\n", instance->host_no);
+		if (NCR5380_poll_politely(instance,
+		                          STATUS_REG, SR_REQ, 0, 5 * HZ) < 0)
+			break;
+
+		dsprintk(NDEBUG_HANDSHAKE, instance, "REQ negated, handshake complete\n");
 
 /*
- * We have several special cases to consider during REQ/ACK handshaking : 
- * 1.  We were in MSGOUT phase, and we are on the last byte of the 
- *      message.  ATN must be dropped as ACK is dropped.
+ * We have several special cases to consider during REQ/ACK handshaking :
+ * 1.  We were in MSGOUT phase, and we are on the last byte of the
+ * message.  ATN must be dropped as ACK is dropped.
  *
- * 2.  We are in a MSGIN phase, and we are on the last byte of the  
- *      message.  We must exit with ACK asserted, so that the calling
- *      code may raise ATN before dropping ACK to reject the message.
+ * 2.  We are in a MSGIN phase, and we are on the last byte of the
+ * message.  We must exit with ACK asserted, so that the calling
+ * code may raise ATN before dropping ACK to reject the message.
  *
  * 3.  ACK and ATN are clear and the target may proceed as normal.
  */
@@ -1597,12 +1381,16 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 		}
 	} while (--c);
 
-	dprintk(NDEBUG_PIO, "scsi%d : residual %d\n", instance->host_no, c);
+	dsprintk(NDEBUG_PIO, instance, "residual %d\n", c);
 
 	*count = c;
 	*data = d;
 	tmp = NCR5380_read(STATUS_REG);
-	if (tmp & SR_REQ)
+	/* The phase read from the bus is valid if either REQ is (already)
+	 * asserted or if ACK hasn't been released yet. The latter applies if
+	 * we're in MSG IN, DATA IN or STATUS and all bytes have been received.
+	 */
+	if ((tmp & SR_REQ) || ((tmp & SR_IO) && c == 0))
 		*phase = tmp & PHASE_MASK;
 	else
 		*phase = PHASE_UNKNOWN;
@@ -1614,79 +1402,80 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance, unsigned char *phase
 }
 
 /**
- *	do_reset	-	issue a reset command
- *	@host: adapter to reset
+ * do_reset - issue a reset command
+ * @instance: adapter to reset
  *
- *	Issue a reset sequence to the NCR5380 and try and get the bus
- *	back into sane shape.
+ * Issue a reset sequence to the NCR5380 and try and get the bus
+ * back into sane shape.
  *
- *	Locks: caller holds queue lock
+ * This clears the reset interrupt flag because there may be no handler for
+ * it. When the driver is initialized, the NCR5380_intr() handler has not yet
+ * been installed. And when in EH we may have released the ST DMA interrupt.
  */
- 
-static void do_reset(struct Scsi_Host *host) {
-	NCR5380_local_declare();
-	NCR5380_setup(host);
 
-	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(NCR5380_read(STATUS_REG) & PHASE_MASK));
+static void do_reset(struct Scsi_Host *instance)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	NCR5380_write(TARGET_COMMAND_REG,
+	              PHASE_SR_TO_TCR(NCR5380_read(STATUS_REG) & PHASE_MASK));
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST);
-	udelay(25);
+	udelay(50);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+	(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+	local_irq_restore(flags);
 }
 
-/*
- * Function : do_abort (Scsi_Host *host)
- * 
- * Purpose : abort the currently established nexus.  Should only be 
- *      called from a routine which can drop into a 
- * 
- * Returns : 0 on success, -1 on failure.
- *
- * Locks: queue lock held by caller
- *	FIXME: sort this out and get new_eh running
+/**
+ * do_abort - abort the currently established nexus by going to
+ * MESSAGE OUT phase and sending an ABORT message.
+ * @instance: relevant scsi host instance
+ *
+ * Returns 0 on success, -1 on failure.
  */
 
-static int do_abort(struct Scsi_Host *host) {
-	NCR5380_local_declare();
+static int do_abort(struct Scsi_Host *instance)
+{
 	unsigned char *msgptr, phase, tmp;
 	int len;
 	int rc;
-	NCR5380_setup(host);
-
 
 	/* Request message out phase */
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 
-	/* 
-	 * Wait for the target to indicate a valid phase by asserting 
-	 * REQ.  Once this happens, we'll have either a MSGOUT phase 
-	 * and can immediately send the ABORT message, or we'll have some 
+	/*
+	 * Wait for the target to indicate a valid phase by asserting
+	 * REQ.  Once this happens, we'll have either a MSGOUT phase
+	 * and can immediately send the ABORT message, or we'll have some
 	 * other phase and will have to source/sink data.
-	 * 
+	 *
 	 * We really don't care what value was on the bus or what value
 	 * the target sees, so we just handshake.
 	 */
 
-	rc = NCR5380_poll_politely(host, STATUS_REG, SR_REQ, SR_REQ, 60 * HZ);
-	
-	if(rc < 0)
-		return -1;
+	rc = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, 10 * HZ);
+	if (rc < 0)
+		goto timeout;
+
+	tmp = NCR5380_read(STATUS_REG) & PHASE_MASK;
 
-	tmp = (unsigned char)rc;
-	
 	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(tmp));
 
-	if ((tmp & PHASE_MASK) != PHASE_MSGOUT) {
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
-		rc = NCR5380_poll_politely(host, STATUS_REG, SR_REQ, 0, 3*HZ);
+	if (tmp != PHASE_MSGOUT) {
+		NCR5380_write(INITIATOR_COMMAND_REG,
+		              ICR_BASE | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
+		rc = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, 0, 3 * HZ);
+		if (rc < 0)
+			goto timeout;
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
-		if(rc == -1)
-			return -1;
 	}
+
 	tmp = ABORT;
 	msgptr = &tmp;
 	len = 1;
 	phase = PHASE_MSGOUT;
-	NCR5380_transfer_pio(host, &phase, &len, &msgptr);
+	NCR5380_transfer_pio(instance, &phase, &len, &msgptr);
 
 	/*
 	 * If we got here, and the command completed successfully,
@@ -1694,32 +1483,37 @@ static int do_abort(struct Scsi_Host *host) {
 	 */
 
 	return len ? -1 : 0;
+
+timeout:
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+	return -1;
 }
 
 #if defined(REAL_DMA) || defined(PSEUDO_DMA) || defined (REAL_DMA_POLL)
-/* 
- * Function : int NCR5380_transfer_dma (struct Scsi_Host *instance, 
- *      unsigned char *phase, int *count, unsigned char **data)
+/*
+ * Function : int NCR5380_transfer_dma (struct Scsi_Host *instance,
+ * unsigned char *phase, int *count, unsigned char **data)
  *
  * Purpose : transfers data in given phase using either real
- *      or pseudo DMA.
+ * or pseudo DMA.
  *
- * Inputs : instance - instance of driver, *phase - pointer to 
- *      what phase is expected, *count - pointer to number of 
- *      bytes to transfer, **data - pointer to data pointer.
- * 
- * Returns : -1 when different phase is entered without transferring
- *      maximum number of bytes, 0 if all bytes or transferred or exit
- *      is in same phase.
+ * Inputs : instance - instance of driver, *phase - pointer to
+ * what phase is expected, *count - pointer to number of
+ * bytes to transfer, **data - pointer to data pointer.
  *
- *      Also, *phase, *count, *data are modified in place.
+ * Returns : -1 when different phase is entered without transferring
+ * maximum number of bytes, 0 if all bytes or transferred or exit
+ * is in same phase.
  *
- *	Locks: io_request lock held by caller
+ * Also, *phase, *count, *data are modified in place.
  */
 
 
-static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase, int *count, unsigned char **data) {
-	NCR5380_local_declare();
+static int NCR5380_transfer_dma(struct Scsi_Host *instance,
+				unsigned char *phase, int *count,
+				unsigned char **data)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	register int c = *count;
 	register unsigned char p = *phase;
 	register unsigned char *d = *data;
@@ -1730,54 +1524,47 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 	unsigned char saved_data = 0, overrun = 0, residue;
 #endif
 
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-
-	NCR5380_setup(instance);
-
 	if ((tmp = (NCR5380_read(STATUS_REG) & PHASE_MASK)) != p) {
 		*phase = tmp;
 		return -1;
 	}
 #if defined(REAL_DMA) || defined(REAL_DMA_POLL)
-#ifdef READ_OVERRUNS
 	if (p & SR_IO) {
-		c -= 2;
+		if (!(hostdata->flags & FLAG_NO_DMA_FIXUPS))
+			c -= 2;
 	}
-#endif
-	dprintk(NDEBUG_DMA, "scsi%d : initializing DMA channel %d for %s, %d bytes %s %0x\n", instance->host_no, instance->dma_channel, (p & SR_IO) ? "reading" : "writing", c, (p & SR_IO) ? "to" : "from", (unsigned) d);
 	hostdata->dma_len = (p & SR_IO) ? NCR5380_dma_read_setup(instance, d, c) : NCR5380_dma_write_setup(instance, d, c);
+
+	dsprintk(NDEBUG_DMA, instance, "initializing DMA %s: length %d, address %p\n",
+	         (p & SR_IO) ? "receive" : "send", c, *data);
 #endif
 
 	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(p));
 
 #ifdef REAL_DMA
-	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_ENABLE_EOP_INTR | MR_MONITOR_BSY);
+	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY |
+	                        MR_ENABLE_EOP_INTR);
 #elif defined(REAL_DMA_POLL)
-	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE);
+	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY);
 #else
 	/*
 	 * Note : on my sample board, watch-dog timeouts occurred when interrupts
-	 * were not disabled for the duration of a single DMA transfer, from 
+	 * were not disabled for the duration of a single DMA transfer, from
 	 * before the setting of DMA mode to after transfer of the last byte.
 	 */
 
-#if defined(PSEUDO_DMA) && defined(UNSAFE)
-	spin_unlock_irq(instance->host_lock);
-#endif
-	/* KLL May need eop and parity in 53c400 */
-	if (hostdata->flags & FLAG_NCR53C400)
-		NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE |
-				MR_ENABLE_PAR_CHECK | MR_ENABLE_PAR_INTR |
-				MR_ENABLE_EOP_INTR | MR_MONITOR_BSY);
+	if (hostdata->flags & FLAG_NO_DMA_FIXUP)
+		NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY |
+		                        MR_ENABLE_EOP_INTR);
 	else
-		NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE);
+		NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY);
 #endif				/* def REAL_DMA */
 
 	dprintk(NDEBUG_DMA, "scsi%d : mode reg = 0x%X\n", instance->host_no, NCR5380_read(MODE_REG));
 
-	/* 
-	 *	On the PAS16 at least I/O recovery delays are not needed here.
-	 *	Everyone else seems to want them.
+	/*
+	 * On the PAS16 at least I/O recovery delays are not needed here.
+	 * Everyone else seems to want them.
 	 */
 
 	if (p & SR_IO) {
@@ -1797,49 +1584,49 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 	} while ((tmp & BASR_PHASE_MATCH) && !(tmp & (BASR_BUSY_ERROR | BASR_END_DMA_TRANSFER)));
 
 /*
-   At this point, either we've completed DMA, or we have a phase mismatch,
-   or we've unexpectedly lost BUSY (which is a real error).
-
-   For write DMAs, we want to wait until the last byte has been
-   transferred out over the bus before we turn off DMA mode.  Alas, there
-   seems to be no terribly good way of doing this on a 5380 under all
-   conditions.  For non-scatter-gather operations, we can wait until REQ
-   and ACK both go false, or until a phase mismatch occurs.  Gather-writes
-   are nastier, since the device will be expecting more data than we
-   are prepared to send it, and REQ will remain asserted.  On a 53C8[01] we
-   could test LAST BIT SENT to assure transfer (I imagine this is precisely
-   why this signal was added to the newer chips) but on the older 538[01]
-   this signal does not exist.  The workaround for this lack is a watchdog;
-   we bail out of the wait-loop after a modest amount of wait-time if
-   the usual exit conditions are not met.  Not a terribly clean or
-   correct solution :-%
-
-   Reads are equally tricky due to a nasty characteristic of the NCR5380.
-   If the chip is in DMA mode for an READ, it will respond to a target's
-   REQ by latching the SCSI data into the INPUT DATA register and asserting
-   ACK, even if it has _already_ been notified by the DMA controller that
-   the current DMA transfer has completed!  If the NCR5380 is then taken
-   out of DMA mode, this already-acknowledged byte is lost.
-
-   This is not a problem for "one DMA transfer per command" reads, because
-   the situation will never arise... either all of the data is DMA'ed
-   properly, or the target switches to MESSAGE IN phase to signal a
-   disconnection (either operation bringing the DMA to a clean halt).
-   However, in order to handle scatter-reads, we must work around the
-   problem.  The chosen fix is to DMA N-2 bytes, then check for the
-   condition before taking the NCR5380 out of DMA mode.  One or two extra
-   bytes are transferred via PIO as necessary to fill out the original
-   request.
+ * At this point, either we've completed DMA, or we have a phase mismatch,
+ * or we've unexpectedly lost BUSY (which is a real error).
+ *
+ * For DMA sends, we want to wait until the last byte has been
+ * transferred out over the bus before we turn off DMA mode.  Alas, there
+ * seems to be no terribly good way of doing this on a 5380 under all
+ * conditions.  For non-scatter-gather operations, we can wait until REQ
+ * and ACK both go false, or until a phase mismatch occurs.  Gather-sends
+ * are nastier, since the device will be expecting more data than we
+ * are prepared to send it, and REQ will remain asserted.  On a 53C8[01] we
+ * could test Last Byte Sent to assure transfer (I imagine this is precisely
+ * why this signal was added to the newer chips) but on the older 538[01]
+ * this signal does not exist.  The workaround for this lack is a watchdog;
+ * we bail out of the wait-loop after a modest amount of wait-time if
+ * the usual exit conditions are not met.  Not a terribly clean or
+ * correct solution :-%
+ *
+ * DMA receive is equally tricky due to a nasty characteristic of the NCR5380.
+ * If the chip is in DMA receive mode, it will respond to a target's
+ * REQ by latching the SCSI data into the INPUT DATA register and asserting
+ * ACK, even if it has _already_ been notified by the DMA controller that
+ * the current DMA transfer has completed!  If the NCR5380 is then taken
+ * out of DMA mode, this already-acknowledged byte is lost. This is
+ * not a problem for "one DMA transfer per READ command", because
+ * the situation will never arise... either all of the data is DMA'ed
+ * properly, or the target switches to MESSAGE IN phase to signal a
+ * disconnection (either operation bringing the DMA to a clean halt).
+ * However, in order to handle scatter-receive, we must work around the
+ * problem.  The chosen fix is to DMA N-2 bytes, then check for the
+ * condition before taking the NCR5380 out of DMA mode.  One or two extra
+ * bytes are transferred via PIO as necessary to fill out the original
+ * request.
  */
 
 	if (p & SR_IO) {
-#ifdef READ_OVERRUNS
-		udelay(10);
-		if (((NCR5380_read(BUS_AND_STATUS_REG) & (BASR_PHASE_MATCH | BASR_ACK)) == (BASR_PHASE_MATCH | BASR_ACK))) {
-			saved_data = NCR5380_read(INPUT_DATA_REGISTER);
-			overrun = 1;
+		if (!(hostdata->flags & FLAG_NO_DMA_FIXUPS)) {
+			udelay(10);
+			if ((NCR5380_read(BUS_AND_STATUS_REG) & (BASR_PHASE_MATCH | BASR_ACK)) ==
+			    (BASR_PHASE_MATCH | BASR_ACK)) {
+				saved_data = NCR5380_read(INPUT_DATA_REGISTER);
+				overrun = 1;
+			}
 		}
-#endif
 	} else {
 		int limit = 100;
 		while (((tmp = NCR5380_read(BUS_AND_STATUS_REG)) & BASR_ACK) || (NCR5380_read(STATUS_REG) & SR_REQ)) {
@@ -1850,7 +1637,8 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		}
 	}
 
-	dprintk(NDEBUG_DMA, "scsi%d : polled DMA transfer complete, basr 0x%X, sr 0x%X\n", instance->host_no, tmp, NCR5380_read(STATUS_REG));
+	dsprintk(NDEBUG_DMA, "polled DMA transfer complete, basr 0x%02x, sr 0x%02x\n",
+	         tmp, NCR5380_read(STATUS_REG));
 
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
@@ -1861,8 +1649,8 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 	*data += c;
 	*phase = NCR5380_read(STATUS_REG) & PHASE_MASK;
 
-#ifdef READ_OVERRUNS
-	if (*phase == p && (p & SR_IO) && residue == 0) {
+	if (!(hostdata->flags & FLAG_NO_DMA_FIXUPS) &&
+	    *phase == p && (p & SR_IO) && residue == 0) {
 		if (overrun) {
 			dprintk(NDEBUG_DMA, "Got an input overrun, using saved byte\n");
 			**data = saved_data;
@@ -1877,7 +1665,6 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 		NCR5380_transfer_pio(instance, phase, &cnt, data);
 		*count -= toPIO - cnt;
 	}
-#endif
 
 	dprintk(NDEBUG_DMA, "Return with data ptr = 0x%X, count %d, last 0x%X, next 0x%X\n", *data, *count, *(*data + *count - 1), *(*data + *count));
 	return 0;
@@ -1886,95 +1673,64 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 	return 0;
 #else				/* defined(REAL_DMA_POLL) */
 	if (p & SR_IO) {
-#ifdef DMA_WORKS_RIGHT
-		foo = NCR5380_pread(instance, d, c);
-#else
-		int diff = 1;
-		if (hostdata->flags & FLAG_NCR53C400) {
-			diff = 0;
-		}
-		if (!(foo = NCR5380_pread(instance, d, c - diff))) {
+		foo = NCR5380_pread(instance, d,
+			hostdata->flags & FLAG_NO_DMA_FIXUP ? c : c - 1);
+		if (!foo && !(hostdata->flags & FLAG_NO_DMA_FIXUP)) {
 			/*
-			 * We can't disable DMA mode after successfully transferring 
+			 * We can't disable DMA mode after successfully transferring
 			 * what we plan to be the last byte, since that would open up
-			 * a race condition where if the target asserted REQ before 
+			 * a race condition where if the target asserted REQ before
 			 * we got the DMA mode reset, the NCR5380 would have latched
 			 * an additional byte into the INPUT DATA register and we'd
 			 * have dropped it.
-			 * 
-			 * The workaround was to transfer one fewer bytes than we 
-			 * intended to with the pseudo-DMA read function, wait for 
+			 *
+			 * The workaround was to transfer one fewer bytes than we
+			 * intended to with the pseudo-DMA read function, wait for
 			 * the chip to latch the last byte, read it, and then disable
 			 * pseudo-DMA mode.
-			 * 
+			 *
 			 * After REQ is asserted, the NCR5380 asserts DRQ and ACK.
 			 * REQ is deasserted when ACK is asserted, and not reasserted
 			 * until ACK goes false.  Since the NCR5380 won't lower ACK
 			 * until DACK is asserted, which won't happen unless we twiddle
-			 * the DMA port or we take the NCR5380 out of DMA mode, we 
-			 * can guarantee that we won't handshake another extra 
+			 * the DMA port or we take the NCR5380 out of DMA mode, we
+			 * can guarantee that we won't handshake another extra
 			 * byte.
 			 */
 
-			if (!(hostdata->flags & FLAG_NCR53C400)) {
-				while (!(NCR5380_read(BUS_AND_STATUS_REG) & BASR_DRQ));
-				/* Wait for clean handshake */
-				while (NCR5380_read(STATUS_REG) & SR_REQ);
-				d[c - 1] = NCR5380_read(INPUT_DATA_REG);
+			if (NCR5380_poll_politely(instance, BUS_AND_STATUS_REG,
+			                          BASR_DRQ, BASR_DRQ, HZ) < 0) {
+				foo = -1;
+				shost_printk(KERN_ERR, instance, "PDMA read: DRQ timeout\n");
+			}
+			if (NCR5380_poll_politely(instance, STATUS_REG,
+			                          SR_REQ, 0, HZ) < 0) {
+				foo = -1;
+				shost_printk(KERN_ERR, instance, "PDMA read: !REQ timeout\n");
 			}
+			d[c - 1] = NCR5380_read(INPUT_DATA_REG);
 		}
-#endif
 	} else {
-#ifdef DMA_WORKS_RIGHT
 		foo = NCR5380_pwrite(instance, d, c);
-#else
-		int timeout;
-		dprintk(NDEBUG_C400_PWRITE, "About to pwrite %d bytes\n", c);
-		if (!(foo = NCR5380_pwrite(instance, d, c))) {
+		if (!foo && !(hostdata->flags & FLAG_NO_DMA_FIXUP)) {
 			/*
-			 * Wait for the last byte to be sent.  If REQ is being asserted for 
-			 * the byte we're interested, we'll ACK it and it will go false.  
+			 * Wait for the last byte to be sent.  If REQ is being asserted for
+			 * the byte we're interested, we'll ACK it and it will go false.
 			 */
-			if (!(hostdata->flags & FLAG_HAS_LAST_BYTE_SENT)) {
-				timeout = 20000;
-				while (!(NCR5380_read(BUS_AND_STATUS_REG) & BASR_DRQ) && (NCR5380_read(BUS_AND_STATUS_REG) & BASR_PHASE_MATCH));
-
-				if (!timeout)
-					dprintk(NDEBUG_LAST_BYTE_SENT, "scsi%d : timed out on last byte\n", instance->host_no);
-
-				if (hostdata->flags & FLAG_CHECK_LAST_BYTE_SENT) {
-					hostdata->flags &= ~FLAG_CHECK_LAST_BYTE_SENT;
-					if (NCR5380_read(TARGET_COMMAND_REG) & TCR_LAST_BYTE_SENT) {
-						hostdata->flags |= FLAG_HAS_LAST_BYTE_SENT;
-						dprintk(NDEBUG_LAST_BYTE_SENT, "scsi%d : last byte sent works\n", instance->host_no);
-					}
-				}
-			} else {
-				dprintk(NDEBUG_C400_PWRITE, "Waiting for LASTBYTE\n");
-				while (!(NCR5380_read(TARGET_COMMAND_REG) & TCR_LAST_BYTE_SENT));
-				dprintk(NDEBUG_C400_PWRITE, "Got LASTBYTE\n");
+			if (NCR5380_poll_politely2(instance,
+			     BUS_AND_STATUS_REG, BASR_DRQ, BASR_DRQ,
+			     BUS_AND_STATUS_REG, BASR_PHASE_MATCH, 0, HZ) < 0) {
+				foo = -1;
+				shost_printk(KERN_ERR, instance, "PDMA write: DRQ and phase timeout\n");
 			}
 		}
-#endif
 	}
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-
-	if ((!(p & SR_IO)) && (hostdata->flags & FLAG_NCR53C400)) {
-		dprintk(NDEBUG_C400_PWRITE, "53C400w: Checking for IRQ\n");
-		if (NCR5380_read(BUS_AND_STATUS_REG) & BASR_IRQ) {
-			dprintk(NDEBUG_C400_PWRITE, "53C400w:    got it, reading reset interrupt reg\n");
-			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-		} else {
-			printk("53C400w:    IRQ NOT THERE!\n");
-		}
-	}
+	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	*data = d + c;
 	*count = 0;
 	*phase = NCR5380_read(STATUS_REG) & PHASE_MASK;
-#if defined(PSEUDO_DMA) && defined(UNSAFE)
-	spin_lock_irq(instance->host_lock);
-#endif				/* defined(REAL_DMA_POLL) */
 	return foo;
 #endif				/* def REAL_DMA */
 }
@@ -1983,25 +1739,23 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase
 /*
  * Function : NCR5380_information_transfer (struct Scsi_Host *instance)
  *
- * Purpose : run through the various SCSI phases and do as the target 
- *      directs us to.  Operates on the currently connected command, 
- *      instance->connected.
+ * Purpose : run through the various SCSI phases and do as the target
+ * directs us to.  Operates on the currently connected command,
+ * instance->connected.
  *
  * Inputs : instance, instance for which we are doing commands
  *
- * Side effects : SCSI things happen, the disconnected queue will be 
- *      modified if a command disconnects, *instance->connected will
- *      change.
- *
- * XXX Note : we need to watch for bus free or a reset condition here 
- *      to recover from an unexpected bus free condition.
+ * Side effects : SCSI things happen, the disconnected queue will be
+ * modified if a command disconnects, *instance->connected will
+ * change.
  *
- * Locks: io_request_lock held by caller in IRQ mode
+ * XXX Note : we need to watch for bus free or a reset condition here
+ * to recover from an unexpected bus free condition.
  */
 
-static void NCR5380_information_transfer(struct Scsi_Host *instance) {
-	NCR5380_local_declare();
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *)instance->hostdata;
+static void NCR5380_information_transfer(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char msgout = NOP;
 	int sink = 0;
 	int len;
@@ -2010,13 +1764,11 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 #endif
 	unsigned char *data;
 	unsigned char phase, tmp, extended_msg[10], old_phase = 0xff;
-	struct scsi_cmnd *cmd = (struct scsi_cmnd *) hostdata->connected;
-	/* RvC: we need to set the end of the polling time */
-	unsigned long poll_time = jiffies + USLEEP_POLL;
+	struct scsi_cmnd *cmd;
 
-	NCR5380_setup(instance);
+	while ((cmd = hostdata->connected)) {
+		struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
 
-	while (1) {
 		tmp = NCR5380_read(STATUS_REG);
 		/* We only have a valid SCSI phase when REQ is asserted */
 		if (tmp & SR_REQ) {
@@ -2028,24 +1780,28 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 			if (sink && (phase != PHASE_MSGOUT)) {
 				NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(tmp));
 
-				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
-				while (NCR5380_read(STATUS_REG) & SR_REQ);
-				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
+				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN |
+				              ICR_ASSERT_ACK);
+				while (NCR5380_read(STATUS_REG) & SR_REQ)
+					;
+				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
+				              ICR_ASSERT_ATN);
 				sink = 0;
 				continue;
 			}
+
 			switch (phase) {
-			case PHASE_DATAIN:
 			case PHASE_DATAOUT:
 #if (NDEBUG & NDEBUG_NO_DATAOUT)
-				printk("scsi%d : NDEBUG_NO_DATAOUT set, attempted DATAOUT aborted\n", instance->host_no);
+				shost_printk(KERN_DEBUG, instance, "NDEBUG_NO_DATAOUT set, attempted DATAOUT aborted\n");
 				sink = 1;
 				do_abort(instance);
 				cmd->result = DID_ERROR << 16;
-				cmd->scsi_done(cmd);
+				complete_cmd(instance, cmd);
 				return;
 #endif
-				/* 
+			case PHASE_DATAIN:
+				/*
 				 * If there is no room left in the current buffer in the
 				 * scatter-gather list, move onto the next one.
 				 */
@@ -2055,10 +1811,13 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					--cmd->SCp.buffers_residual;
 					cmd->SCp.this_residual = cmd->SCp.buffer->length;
 					cmd->SCp.ptr = sg_virt(cmd->SCp.buffer);
-					dprintk(NDEBUG_INFORMATION, "scsi%d : %d bytes and %d buffers left\n", instance->host_no, cmd->SCp.this_residual, cmd->SCp.buffers_residual);
+					dsprintk(NDEBUG_INFORMATION, instance, "%d bytes and %d buffers left\n",
+					         cmd->SCp.this_residual,
+					         cmd->SCp.buffers_residual);
 				}
+
 				/*
-				 * The preferred transfer method is going to be 
+				 * The preferred transfer method is going to be
 				 * PSEUDO-DMA for systems that are strictly PIO,
 				 * since we can let the hardware do the handshaking.
 				 *
@@ -2068,50 +1827,39 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				 */
 
 #if defined(PSEUDO_DMA) || defined(REAL_DMA_POLL)
-				/* KLL
-				 * PSEUDO_DMA is defined here. If this is the g_NCR5380
-				 * driver then it will always be defined, so the
-				 * FLAG_NO_PSEUDO_DMA is used to inhibit PDMA in the base
-				 * NCR5380 case.  I think this is a fairly clean solution.
-				 * We supplement these 2 if's with the flag.
-				 */
-#ifdef NCR5380_dma_xfer_len
-				if (!cmd->device->borken && !(hostdata->flags & FLAG_NO_PSEUDO_DMA) && (transfersize = NCR5380_dma_xfer_len(instance, cmd)) != 0) {
-#else
-				transfersize = cmd->transfersize;
-
-#ifdef LIMIT_TRANSFERSIZE	/* If we have problems with interrupt service */
-				if (transfersize > 512)
-					transfersize = 512;
-#endif				/* LIMIT_TRANSFERSIZE */
-
-				if (!cmd->device->borken && transfersize && !(hostdata->flags & FLAG_NO_PSEUDO_DMA) && cmd->SCp.this_residual && !(cmd->SCp.this_residual % transfersize)) {
-					/* Limit transfers to 32K, for xx400 & xx406
-					 * pseudoDMA that transfers in 128 bytes blocks. */
-					if (transfersize > 32 * 1024)
-						transfersize = 32 * 1024;
-#endif
+				transfersize = 0;
+				if (!cmd->device->borken &&
+				    !(hostdata->flags & FLAG_NO_PSEUDO_DMA))
+					transfersize = NCR5380_dma_xfer_len(instance, cmd, phase);
+
+				if (transfersize) {
 					len = transfersize;
-					if (NCR5380_transfer_dma(instance, &phase, &len, (unsigned char **) &cmd->SCp.ptr)) {
+					if (NCR5380_transfer_dma(instance, &phase,
+					    &len, (unsigned char **)&cmd->SCp.ptr)) {
 						/*
-						 * If the watchdog timer fires, all future accesses to this
-						 * device will use the polled-IO.
+						 * If the watchdog timer fires, all future
+						 * accesses to this device will use the
+						 * polled-IO.
 						 */
 						scmd_printk(KERN_INFO, cmd,
-							    "switching to slow handshake\n");
+							"switching to slow handshake\n");
 						cmd->device->borken = 1;
-						NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 						sink = 1;
 						do_abort(instance);
 						cmd->result = DID_ERROR << 16;
-						cmd->scsi_done(cmd);
+						complete_cmd(instance, cmd);
 						/* XXX - need to source or sink data here, as appropriate */
 					} else
 						cmd->SCp.this_residual -= transfersize - len;
 				} else
 #endif				/* defined(PSEUDO_DMA) || defined(REAL_DMA_POLL) */
-					NCR5380_transfer_pio(instance, &phase, (int *) &cmd->SCp.this_residual, (unsigned char **)
-							     &cmd->SCp.ptr);
+				{
+					spin_unlock_irq(&hostdata->lock);
+					NCR5380_transfer_pio(instance, &phase,
+					                     (int *)&cmd->SCp.this_residual,
+					                     (unsigned char **)&cmd->SCp.ptr);
+					spin_lock_irq(&hostdata->lock);
+				}
 				break;
 			case PHASE_MSGIN:
 				len = 1;
@@ -2120,101 +1868,42 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				cmd->SCp.Message = tmp;
 
 				switch (tmp) {
-					/*
-					 * Linking lets us reduce the time required to get the 
-					 * next command out to the device, hopefully this will
-					 * mean we don't waste another revolution due to the delays
-					 * required by ARBITRATION and another SELECTION.
-					 *
-					 * In the current implementation proposal, low level drivers
-					 * merely have to start the next command, pointed to by 
-					 * next_link, done() is called as with unlinked commands.
-					 */
-#ifdef LINKED
-				case LINKED_CMD_COMPLETE:
-				case LINKED_FLG_CMD_COMPLETE:
-					/* Accept message by clearing ACK */
-					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					dprintk(NDEBUG_LINKED, "scsi%d : target %d lun %llu linked command complete.\n", instance->host_no, cmd->device->id, cmd->device->lun);
-					/* 
-					 * Sanity check : A linked command should only terminate with
-					 * one of these messages if there are more linked commands
-					 * available.
-					 */
-					if (!cmd->next_link) {
-					    printk("scsi%d : target %d lun %llu linked command complete, no next_link\n" instance->host_no, cmd->device->id, cmd->device->lun);
-						sink = 1;
-						do_abort(instance);
-						return;
-					}
-					initialize_SCp(cmd->next_link);
-					/* The next command is still part of this process */
-					cmd->next_link->tag = cmd->tag;
-					cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					dprintk(NDEBUG_LINKED, "scsi%d : target %d lun %llu linked request done, calling scsi_done().\n", instance->host_no, cmd->device->id, cmd->device->lun);
-					cmd->scsi_done(cmd);
-					cmd = hostdata->connected;
-					break;
-#endif				/* def LINKED */
 				case ABORT:
 				case COMMAND_COMPLETE:
 					/* Accept message by clearing ACK */
 					sink = 1;
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					hostdata->connected = NULL;
-					dprintk(NDEBUG_QUEUES, "scsi%d : command for target %d, lun %llu completed\n", instance->host_no, cmd->device->id, cmd->device->lun);
-					hostdata->busy[cmd->device->id] &= ~(1 << (cmd->device->lun & 0xFF));
-
-					/* 
-					 * I'm not sure what the correct thing to do here is : 
-					 * 
-					 * If the command that just executed is NOT a request 
-					 * sense, the obvious thing to do is to set the result
-					 * code to the values of the stored parameters.
-					 * 
-					 * If it was a REQUEST SENSE command, we need some way 
-					 * to differentiate between the failure code of the original
-					 * and the failure code of the REQUEST sense - the obvious
-					 * case is success, where we fall through and leave the result
-					 * code unchanged.
-					 * 
-					 * The non-obvious place is where the REQUEST SENSE failed 
-					 */
-
-					if (cmd->cmnd[0] != REQUEST_SENSE)
-						cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					else if (status_byte(cmd->SCp.Status) != GOOD)
-						cmd->result = (cmd->result & 0x00ffff) | (DID_ERROR << 16);
+					dsprintk(NDEBUG_QUEUES, instance,
+					         "COMMAND COMPLETE %p target %d lun %llu\n",
+					         cmd, scmd_id(cmd), cmd->device->lun);
 
-					if ((cmd->cmnd[0] == REQUEST_SENSE) &&
-						hostdata->ses.cmd_len) {
-						scsi_eh_restore_cmnd(cmd, &hostdata->ses);
-						hostdata->ses.cmd_len = 0 ;
-					}
-
-					if ((cmd->cmnd[0] != REQUEST_SENSE) && (status_byte(cmd->SCp.Status) == CHECK_CONDITION)) {
-						scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
-
-						dprintk(NDEBUG_AUTOSENSE, "scsi%d : performing request sense\n", instance->host_no);
+					hostdata->connected = NULL;
 
-						LIST(cmd, hostdata->issue_queue);
-						cmd->host_scribble = (unsigned char *)
-						    hostdata->issue_queue;
-						hostdata->issue_queue = (struct scsi_cmnd *) cmd;
-						dprintk(NDEBUG_QUEUES, "scsi%d : REQUEST SENSE added to head of issue queue\n", instance->host_no);
-					} else {
-						cmd->scsi_done(cmd);
+					cmd->result &= ~0xffff;
+					cmd->result |= cmd->SCp.Status;
+					cmd->result |= cmd->SCp.Message << 8;
+
+					if (cmd->cmnd[0] == REQUEST_SENSE)
+						complete_cmd(instance, cmd);
+					else {
+						if (cmd->SCp.Status == SAM_STAT_CHECK_CONDITION ||
+						    cmd->SCp.Status == SAM_STAT_COMMAND_TERMINATED) {
+							dsprintk(NDEBUG_QUEUES, instance, "autosense: adding cmd %p to tail of autosense queue\n",
+							         cmd);
+							list_add_tail(&ncmd->list,
+							              &hostdata->autosense);
+						} else
+							complete_cmd(instance, cmd);
 					}
 
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-					/* 
-					 * Restore phase bits to 0 so an interrupted selection, 
+					/*
+					 * Restore phase bits to 0 so an interrupted selection,
 					 * arbitration can resume.
 					 */
 					NCR5380_write(TARGET_COMMAND_REG, 0);
 
-					while ((NCR5380_read(STATUS_REG) & SR_BSY) && !hostdata->connected)
-						barrier();
+					/* Enable reselect interrupts */
+					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					return;
 				case MESSAGE_REJECT:
 					/* Accept message by clearing ACK */
@@ -2229,38 +1918,33 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					default:
 						break;
 					}
-				case DISCONNECT:{
-						/* Accept message by clearing ACK */
-						NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-						cmd->device->disconnect = 1;
-						LIST(cmd, hostdata->disconnected_queue);
-						cmd->host_scribble = (unsigned char *)
-						    hostdata->disconnected_queue;
-						hostdata->connected = NULL;
-						hostdata->disconnected_queue = cmd;
-						dprintk(NDEBUG_QUEUES, "scsi%d : command for target %d lun %llu was moved from connected to" "  the disconnected_queue\n", instance->host_no, cmd->device->id, cmd->device->lun);
-						/* 
-						 * Restore phase bits to 0 so an interrupted selection, 
-						 * arbitration can resume.
-						 */
-						NCR5380_write(TARGET_COMMAND_REG, 0);
-
-						/* Enable reselect interrupts */
-						NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-						/* Wait for bus free to avoid nasty timeouts - FIXME timeout !*/
-						/* NCR538_poll_politely(instance, STATUS_REG, SR_BSY, 0, 30 * HZ); */
-						while ((NCR5380_read(STATUS_REG) & SR_BSY) && !hostdata->connected)
-							barrier();
-						return;
-					}
-					/* 
+					break;
+				case DISCONNECT:
+					/* Accept message by clearing ACK */
+					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+					hostdata->connected = NULL;
+					list_add(&ncmd->list, &hostdata->disconnected);
+					dsprintk(NDEBUG_INFORMATION | NDEBUG_QUEUES,
+					         instance, "connected command %p for target %d lun %llu moved to disconnected queue\n",
+					         cmd, scmd_id(cmd), cmd->device->lun);
+
+					/*
+					 * Restore phase bits to 0 so an interrupted selection,
+					 * arbitration can resume.
+					 */
+					NCR5380_write(TARGET_COMMAND_REG, 0);
+
+					/* Enable reselect interrupts */
+					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+					return;
+					/*
 					 * The SCSI data pointer is *IMPLICITLY* saved on a disconnect
-					 * operation, in violation of the SCSI spec so we can safely 
+					 * operation, in violation of the SCSI spec so we can safely
 					 * ignore SAVE/RESTORE pointers calls.
 					 *
-					 * Unfortunately, some disks violate the SCSI spec and 
+					 * Unfortunately, some disks violate the SCSI spec and
 					 * don't issue the required SAVE_POINTERS message before
-					 * disconnecting, and we have to break spec to remain 
+					 * disconnecting, and we have to break spec to remain
 					 * compatible.
 					 */
 				case SAVE_POINTERS:
@@ -2269,31 +1953,28 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 					break;
 				case EXTENDED_MESSAGE:
-/* 
- * Extended messages are sent in the following format :
- * Byte         
- * 0            EXTENDED_MESSAGE == 1
- * 1            length (includes one byte for code, doesn't 
- *              include first two bytes)
- * 2            code
- * 3..length+1  arguments
- *
- * Start the extended message buffer with the EXTENDED_MESSAGE
- * byte, since spi_print_msg() wants the whole thing.  
- */
+					/*
+					 * Start the message buffer with the EXTENDED_MESSAGE
+					 * byte, since spi_print_msg() wants the whole thing.
+					 */
 					extended_msg[0] = EXTENDED_MESSAGE;
 					/* Accept first byte by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					dprintk(NDEBUG_EXTENDED, "scsi%d : receiving extended message\n", instance->host_no);
+
+					spin_unlock_irq(&hostdata->lock);
+
+					dsprintk(NDEBUG_EXTENDED, instance, "receiving extended message\n");
 
 					len = 2;
 					data = extended_msg + 1;
 					phase = PHASE_MSGIN;
 					NCR5380_transfer_pio(instance, &phase, &len, &data);
+					dsprintk(NDEBUG_EXTENDED, instance, "length %d, code 0x%02x\n",
+					         (int)extended_msg[1],
+					         (int)extended_msg[2]);
 
-					dprintk(NDEBUG_EXTENDED, "scsi%d : length=%d, code=0x%02x\n", instance->host_no, (int) extended_msg[1], (int) extended_msg[2]);
-
-					if (!len && extended_msg[1] <= (sizeof(extended_msg) - 1)) {
+					if (!len && extended_msg[1] > 0 &&
+					    extended_msg[1] <= sizeof(extended_msg) - 2) {
 						/* Accept third byte by clearing ACK */
 						NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 						len = extended_msg[1] - 1;
@@ -2301,7 +1982,8 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 						phase = PHASE_MSGIN;
 
 						NCR5380_transfer_pio(instance, &phase, &len, &data);
-						dprintk(NDEBUG_EXTENDED, "scsi%d : message received, residual %d\n", instance->host_no, len);
+						dsprintk(NDEBUG_EXTENDED, instance, "message received, residual %d\n",
+						         len);
 
 						switch (extended_msg[2]) {
 						case EXTENDED_SDTR:
@@ -2311,34 +1993,42 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 							tmp = 0;
 						}
 					} else if (len) {
-						printk("scsi%d: error receiving extended message\n", instance->host_no);
+						shost_printk(KERN_ERR, instance, "error receiving extended message\n");
 						tmp = 0;
 					} else {
-						printk("scsi%d: extended message code %02x length %d is too long\n", instance->host_no, extended_msg[2], extended_msg[1]);
+						shost_printk(KERN_NOTICE, instance, "extended message code %02x length %d is too long\n",
+						             extended_msg[2], extended_msg[1]);
 						tmp = 0;
 					}
+
+					spin_lock_irq(&hostdata->lock);
+					if (!hostdata->connected)
+						return;
+
 					/* Fall through to reject message */
 
-					/* 
-					 * If we get something weird that we aren't expecting, 
+					/*
+					 * If we get something weird that we aren't expecting,
 					 * reject it.
 					 */
 				default:
 					if (!tmp) {
-						printk("scsi%d: rejecting message ", instance->host_no);
+						shost_printk(KERN_ERR, instance, "rejecting message ");
 						spi_print_msg(extended_msg);
 						printk("\n");
 					} else if (tmp != EXTENDED_MESSAGE)
 						scmd_printk(KERN_INFO, cmd,
-							"rejecting unknown message %02x\n",tmp);
+						            "rejecting unknown message %02x\n",
+						            tmp);
 					else
 						scmd_printk(KERN_INFO, cmd,
-							"rejecting unknown extended message code %02x, length %d\n", extended_msg[1], extended_msg[0]);
+						            "rejecting unknown extended message code %02x, length %d\n",
+						            extended_msg[1], extended_msg[0]);
 
 					msgout = MESSAGE_REJECT;
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 					break;
-				}	/* switch (tmp) */
+				} /* switch (tmp) */
 				break;
 			case PHASE_MSGOUT:
 				len = 1;
@@ -2346,10 +2036,9 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				hostdata->last_message = msgout;
 				NCR5380_transfer_pio(instance, &phase, &len, &data);
 				if (msgout == ABORT) {
-					hostdata->busy[cmd->device->id] &= ~(1 << (cmd->device->lun & 0xFF));
 					hostdata->connected = NULL;
 					cmd->result = DID_ERROR << 16;
-					cmd->scsi_done(cmd);
+					complete_cmd(instance, cmd);
 					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					return;
 				}
@@ -2358,17 +2047,12 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 			case PHASE_CMDOUT:
 				len = cmd->cmd_len;
 				data = cmd->cmnd;
-				/* 
-				 * XXX for performance reasons, on machines with a 
-				 * PSEUDO-DMA architecture we should probably 
-				 * use the dma transfer function.  
+				/*
+				 * XXX for performance reasons, on machines with a
+				 * PSEUDO-DMA architecture we should probably
+				 * use the dma transfer function.
 				 */
 				NCR5380_transfer_pio(instance, &phase, &len, &data);
-				if (!cmd->device->disconnect && should_disconnect(cmd->cmnd[0])) {
-					NCR5380_set_timer(hostdata, USLEEP_SLEEP);
-					dprintk(NDEBUG_USLEEP, "scsi%d : issued command, sleeping until %lu\n", instance->host_no, hostdata->time_expires);
-					return;
-				}
 				break;
 			case PHASE_STATIN:
 				len = 1;
@@ -2377,46 +2061,37 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance) {
 				cmd->SCp.Status = tmp;
 				break;
 			default:
-				printk("scsi%d : unknown phase\n", instance->host_no);
+				shost_printk(KERN_ERR, instance, "unknown phase\n");
 				NCR5380_dprint(NDEBUG_ANY, instance);
-			}	/* switch(phase) */
-		}		/* if (tmp * SR_REQ) */
-		else {
-			/* RvC: go to sleep if polling time expired
-			 */
-			if (!cmd->device->disconnect && time_after_eq(jiffies, poll_time)) {
-				NCR5380_set_timer(hostdata, USLEEP_SLEEP);
-				dprintk(NDEBUG_USLEEP, "scsi%d : poll timed out, sleeping until %lu\n", instance->host_no, hostdata->time_expires);
-				return;
-			}
+			} /* switch(phase) */
+		} else {
+			spin_unlock_irq(&hostdata->lock);
+			NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ);
+			spin_lock_irq(&hostdata->lock);
 		}
-	}			/* while (1) */
+	}
 }
 
 /*
  * Function : void NCR5380_reselect (struct Scsi_Host *instance)
  *
- * Purpose : does reselection, initializing the instance->connected 
- *      field to point to the scsi_cmnd for which the I_T_L or I_T_L_Q
- *      nexus has been reestablished,
- *      
- * Inputs : instance - this instance of the NCR5380.
+ * Purpose : does reselection, initializing the instance->connected
+ * field to point to the scsi_cmnd for which the I_T_L or I_T_L_Q
+ * nexus has been reestablished,
  *
- * Locks: io_request_lock held by caller if IRQ driven
+ * Inputs : instance - this instance of the NCR5380.
  */
 
-static void NCR5380_reselect(struct Scsi_Host *instance) {
-	NCR5380_local_declare();
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *)
-	 instance->hostdata;
+static void NCR5380_reselect(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char target_mask;
 	unsigned char lun, phase;
 	int len;
 	unsigned char msg[3];
 	unsigned char *data;
-	struct scsi_cmnd *tmp = NULL, *prev;
-	int abort = 0;
-	NCR5380_setup(instance);
+	struct NCR5380_cmd *ncmd;
+	struct scsi_cmnd *tmp;
 
 	/*
 	 * Disable arbitration, etc. since the host adapter obviously
@@ -2424,12 +2099,12 @@ static void NCR5380_reselect(struct Scsi_Host *instance) {
 	 */
 
 	NCR5380_write(MODE_REG, MR_BASE);
-	hostdata->restart_select = 1;
 
 	target_mask = NCR5380_read(CURRENT_SCSI_DATA_REG) & ~(hostdata->id_mask);
-	dprintk(NDEBUG_SELECTION, "scsi%d : reselect\n", instance->host_no);
 
-	/* 
+	dsprintk(NDEBUG_RESELECTION, instance, "reselect\n");
+
+	/*
 	 * At this point, we have detected that our SCSI ID is on the bus,
 	 * SEL is true and BSY was false for at least one bus settle delay
 	 * (400 ns).
@@ -2439,103 +2114,110 @@ static void NCR5380_reselect(struct Scsi_Host *instance) {
 	 */
 
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_BSY);
-
-	/* FIXME: timeout too long, must fail to workqueue */	
-	if(NCR5380_poll_politely(instance, STATUS_REG, SR_SEL, 0, 2*HZ)<0)
-		abort = 1;
-		
+	if (NCR5380_poll_politely(instance,
+	                          STATUS_REG, SR_SEL, 0, 2 * HZ) < 0) {
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+		return;
+	}
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
 	/*
 	 * Wait for target to go into MSGIN.
-	 * FIXME: timeout needed and fail to work queeu
 	 */
 
-	if(NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, 2*HZ))
-		abort = 1;
+	if (NCR5380_poll_politely(instance,
+	                          STATUS_REG, SR_REQ, SR_REQ, 2 * HZ) < 0) {
+		do_abort(instance);
+		return;
+	}
 
 	len = 1;
 	data = msg;
 	phase = PHASE_MSGIN;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
 
+	if (len) {
+		do_abort(instance);
+		return;
+	}
+
 	if (!(msg[0] & 0x80)) {
-		printk(KERN_ERR "scsi%d : expecting IDENTIFY message, got ", instance->host_no);
+		shost_printk(KERN_ERR, instance, "expecting IDENTIFY message, got ");
 		spi_print_msg(msg);
-		abort = 1;
-	} else {
-		/* Accept message by clearing ACK */
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		lun = (msg[0] & 0x07);
+		printk("\n");
+		do_abort(instance);
+		return;
+	}
+	lun = msg[0] & 0x07;
 
-		/* 
-		 * We need to add code for SCSI-II to track which devices have
-		 * I_T_L_Q nexuses established, and which have simple I_T_L
-		 * nexuses so we can chose to do additional data transfer.
-		 */
+	/*
+	 * We need to add code for SCSI-II to track which devices have
+	 * I_T_L_Q nexuses established, and which have simple I_T_L
+	 * nexuses so we can chose to do additional data transfer.
+	 */
 
-		/* 
-		 * Find the command corresponding to the I_T_L or I_T_L_Q  nexus we 
-		 * just reestablished, and remove it from the disconnected queue.
-		 */
+	/*
+	 * Find the command corresponding to the I_T_L or I_T_L_Q  nexus we
+	 * just reestablished, and remove it from the disconnected queue.
+	 */
 
+	tmp = NULL;
+	list_for_each_entry(ncmd, &hostdata->disconnected, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
 
-		for (tmp = (struct scsi_cmnd *) hostdata->disconnected_queue, prev = NULL; tmp; prev = tmp, tmp = (struct scsi_cmnd *) tmp->host_scribble)
-			if ((target_mask == (1 << tmp->device->id)) && (lun == (u8)tmp->device->lun)
-			    ) {
-				if (prev) {
-					REMOVE(prev, prev->host_scribble, tmp, tmp->host_scribble);
-					prev->host_scribble = tmp->host_scribble;
-				} else {
-					REMOVE(-1, hostdata->disconnected_queue, tmp, tmp->host_scribble);
-					hostdata->disconnected_queue = (struct scsi_cmnd *) tmp->host_scribble;
-				}
-				tmp->host_scribble = NULL;
-				break;
-			}
-		if (!tmp) {
-			printk(KERN_ERR "scsi%d : warning : target bitmask %02x lun %d not in disconnect_queue.\n", instance->host_no, target_mask, lun);
-			/* 
-			 * Since we have an established nexus that we can't do anything with,
-			 * we must abort it.  
-			 */
-			abort = 1;
+		if (target_mask == (1 << scmd_id(cmd)) &&
+		    lun == (u8)cmd->device->lun) {
+			list_del(&ncmd->list);
+			tmp = cmd;
+			break;
 		}
 	}
 
-	if (abort) {
-		do_abort(instance);
+	if (tmp) {
+		dsprintk(NDEBUG_RESELECTION | NDEBUG_QUEUES, instance,
+		         "reselect: removed %p from disconnected queue\n", tmp);
 	} else {
-		hostdata->connected = tmp;
-		dprintk(NDEBUG_RESELECTION, "scsi%d : nexus established, target = %d, lun = %llu, tag = %d\n", instance->host_no, tmp->device->id, tmp->device->lun, tmp->tag);
+		shost_printk(KERN_ERR, instance, "target bitmask 0x%02x lun %d not in disconnected queue.\n",
+		             target_mask, lun);
+		/*
+		 * Since we have an established nexus that we can't do anything
+		 * with, we must abort it.
+		 */
+		do_abort(instance);
+		return;
 	}
+
+	/* Accept message by clearing ACK */
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+
+	hostdata->connected = tmp;
+	dsprintk(NDEBUG_RESELECTION, instance, "nexus established, target %d, lun %llu, tag %d\n",
+	         scmd_id(tmp), tmp->device->lun, tmp->tag);
 }
 
 /*
  * Function : void NCR5380_dma_complete (struct Scsi_Host *instance)
  *
  * Purpose : called by interrupt handler when DMA finishes or a phase
- *      mismatch occurs (which would finish the DMA transfer).  
+ * mismatch occurs (which would finish the DMA transfer).
  *
  * Inputs : instance - this instance of the NCR5380.
  *
  * Returns : pointer to the scsi_cmnd structure for which the I_T_L
- *      nexus has been reestablished, on failure NULL is returned.
+ * nexus has been reestablished, on failure NULL is returned.
  */
 
 #ifdef REAL_DMA
 static void NCR5380_dma_complete(NCR5380_instance * instance) {
-	NCR5380_local_declare();
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int transferred;
-	NCR5380_setup(instance);
 
 	/*
 	 * XXX this might not be right.
 	 *
 	 * Wait for final byte to transfer, ie wait for ACK to go false.
 	 *
-	 * We should use the Last Byte Sent bit, unfortunately this is 
+	 * We should use the Last Byte Sent bit, unfortunately this is
 	 * not available on the 5380/5381 (only the various CMOS chips)
 	 *
 	 * FIXME: timeout, and need to handle long timeout/irq case
@@ -2543,7 +2225,6 @@ static void NCR5380_dma_complete(NCR5380_instance * instance) {
 
 	NCR5380_poll_politely(instance, BUS_AND_STATUS_REG, BASR_ACK, 0, 5*HZ);
 
-	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
 	/*
@@ -2560,190 +2241,251 @@ static void NCR5380_dma_complete(NCR5380_instance * instance) {
 }
 #endif				/* def REAL_DMA */
 
-/*
- * Function : int NCR5380_abort (struct scsi_cmnd *cmd)
- *
- * Purpose : abort a command
- *
- * Inputs : cmd - the scsi_cmnd to abort, code - code to set the
- *      host byte of the result field to, if zero DID_ABORTED is
- *      used.
- *
- * Returns : SUCCESS - success, FAILED on failure.
- *
- *	XXX - there is no way to abort the command that is currently
- *	connected, you have to wait for it to complete.  If this is
- *	a problem, we could implement longjmp() / setjmp(), setjmp()
- *	called where the loop started in NCR5380_main().
- *
- * Locks: host lock taken by caller
+/**
+ * list_find_cmd - test for presence of a command in a linked list
+ * @haystack: list of commands
+ * @needle: command to search for
  */
 
-static int NCR5380_abort(struct scsi_cmnd *cmd)
+static bool list_find_cmd(struct list_head *haystack,
+                          struct scsi_cmnd *needle)
 {
-	NCR5380_local_declare();
-	struct Scsi_Host *instance = cmd->device->host;
-	struct NCR5380_hostdata *hostdata = (struct NCR5380_hostdata *) instance->hostdata;
-	struct scsi_cmnd *tmp, **prev;
+	struct NCR5380_cmd *ncmd;
 
-	scmd_printk(KERN_WARNING, cmd, "aborting command\n");
+	list_for_each_entry(ncmd, haystack, list)
+		if (NCR5380_to_scmd(ncmd) == needle)
+			return true;
+	return false;
+}
 
-	NCR5380_print_status(instance);
+/**
+ * list_remove_cmd - remove a command from linked list
+ * @haystack: list of commands
+ * @needle: command to remove
+ */
 
-	NCR5380_setup(instance);
+static bool list_del_cmd(struct list_head *haystack,
+                         struct scsi_cmnd *needle)
+{
+	if (list_find_cmd(haystack, needle)) {
+		struct NCR5380_cmd *ncmd = scsi_cmd_priv(needle);
 
-	dprintk(NDEBUG_ABORT, "scsi%d : abort called\n", instance->host_no);
-	dprintk(NDEBUG_ABORT, "        basr 0x%X, sr 0x%X\n", NCR5380_read(BUS_AND_STATUS_REG), NCR5380_read(STATUS_REG));
+		list_del(&ncmd->list);
+		return true;
+	}
+	return false;
+}
 
-#if 0
-/*
- * Case 1 : If the command is the currently executing command, 
- * we'll set the aborted flag and return control so that 
- * information transfer routine can exit cleanly.
+/**
+ * NCR5380_abort - scsi host eh_abort_handler() method
+ * @cmd: the command to be aborted
+ *
+ * Try to abort a given command by removing it from queues and/or sending
+ * the target an abort message. This may not succeed in causing a target
+ * to abort the command. Nonetheless, the low-level driver must forget about
+ * the command because the mid-layer reclaims it and it may be re-issued.
+ *
+ * The normal path taken by a command is as follows. For EH we trace this
+ * same path to locate and abort the command.
+ *
+ * unissued -> selecting -> [unissued -> selecting ->]... connected ->
+ * [disconnected -> connected ->]...
+ * [autosense -> connected ->] done
+ *
+ * If cmd is unissued then just remove it.
+ * If cmd is disconnected, try to select the target.
+ * If cmd is connected, try to send an abort message.
+ * If cmd is waiting for autosense, give it a chance to complete but check
+ * that it isn't left connected.
+ * If cmd was not found at all then presumably it has already been completed,
+ * in which case return SUCCESS to try to avoid further EH measures.
+ * If the command has not completed yet, we must not fail to find it.
  */
 
-	if (hostdata->connected == cmd) {
-		dprintk(NDEBUG_ABORT, "scsi%d : aborting connected command\n", instance->host_no);
-		hostdata->aborted = 1;
-/*
- * We should perform BSY checking, and make sure we haven't slipped
- * into BUS FREE.
- */
+static int NCR5380_abort(struct scsi_cmnd *cmd)
+{
+	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	unsigned long flags;
+	int result = SUCCESS;
 
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_ASSERT_ATN);
-/* 
- * Since we can't change phases until we've completed the current 
- * handshake, we have to source or sink a byte of data if the current
- * phase is not MSGOUT.
- */
+	spin_lock_irqsave(&hostdata->lock, flags);
 
-/* 
- * Return control to the executing NCR drive so we can clear the
- * aborted flag and get back into our main loop.
- */
+#if (NDEBUG & NDEBUG_ANY)
+	scmd_printk(KERN_INFO, cmd, __func__);
+#endif
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
 
-		return SUCCESS;
+	if (list_del_cmd(&hostdata->unissued, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: removed %p from issue queue\n", cmd);
+		cmd->result = DID_ABORT << 16;
+		cmd->scsi_done(cmd); /* No tag or busy flag to worry about */
 	}
-#endif
 
-/* 
- * Case 2 : If the command hasn't been issued yet, we simply remove it 
- *          from the issue queue.
- */
- 
-	dprintk(NDEBUG_ABORT, "scsi%d : abort going into loop.\n", instance->host_no);
-	for (prev = (struct scsi_cmnd **) &(hostdata->issue_queue), tmp = (struct scsi_cmnd *) hostdata->issue_queue; tmp; prev = (struct scsi_cmnd **) &(tmp->host_scribble), tmp = (struct scsi_cmnd *) tmp->host_scribble)
-		if (cmd == tmp) {
-			REMOVE(5, *prev, tmp, tmp->host_scribble);
-			(*prev) = (struct scsi_cmnd *) tmp->host_scribble;
-			tmp->host_scribble = NULL;
-			tmp->result = DID_ABORT << 16;
-			dprintk(NDEBUG_ABORT, "scsi%d : abort removed command from issue queue.\n", instance->host_no);
-			tmp->scsi_done(tmp);
-			return SUCCESS;
+	if (hostdata->selecting == cmd) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: cmd %p == selecting\n", cmd);
+		hostdata->selecting = NULL;
+		cmd->result = DID_ABORT << 16;
+		complete_cmd(instance, cmd);
+		goto out;
+	}
+
+	if (list_del_cmd(&hostdata->disconnected, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: removed %p from disconnected list\n", cmd);
+		cmd->result = DID_ERROR << 16;
+		if (!hostdata->connected)
+			NCR5380_select(instance, cmd);
+		if (hostdata->connected != cmd) {
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
+		}
+	}
+
+	if (hostdata->connected == cmd) {
+		dsprintk(NDEBUG_ABORT, instance, "abort: cmd %p is connected\n", cmd);
+		hostdata->connected = NULL;
+		if (do_abort(instance)) {
+			set_host_byte(cmd, DID_ERROR);
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
 		}
-#if (NDEBUG  & NDEBUG_ABORT)
-	/* KLL */
-		else if (prev == tmp)
-			printk(KERN_ERR "scsi%d : LOOP\n", instance->host_no);
+		set_host_byte(cmd, DID_ABORT);
+#ifdef REAL_DMA
+		hostdata->dma_len = 0;
 #endif
+		if (cmd->cmnd[0] == REQUEST_SENSE)
+			complete_cmd(instance, cmd);
+		else {
+			struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
 
-/* 
- * Case 3 : If any commands are connected, we're going to fail the abort
- *          and let the high level SCSI driver retry at a later time or 
- *          issue a reset.
- *
- *          Timeouts, and therefore aborted commands, will be highly unlikely
- *          and handling them cleanly in this situation would make the common
- *          case of noresets less efficient, and would pollute our code.  So,
- *          we fail.
- */
+			/* Perform autosense for this command */
+			list_add(&ncmd->list, &hostdata->autosense);
+		}
+	}
 
-	if (hostdata->connected) {
-		dprintk(NDEBUG_ABORT, "scsi%d : abort failed, command connected.\n", instance->host_no);
-		return FAILED;
+	if (list_find_cmd(&hostdata->autosense, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: found %p on sense queue\n", cmd);
+		spin_unlock_irqrestore(&hostdata->lock, flags);
+		queue_work(hostdata->work_q, &hostdata->main_task);
+		msleep(1000);
+		spin_lock_irqsave(&hostdata->lock, flags);
+		if (list_del_cmd(&hostdata->autosense, cmd)) {
+			dsprintk(NDEBUG_ABORT, instance,
+			         "abort: removed %p from sense queue\n", cmd);
+			set_host_byte(cmd, DID_ABORT);
+			complete_cmd(instance, cmd);
+			goto out;
+		}
 	}
-/*
- * Case 4: If the command is currently disconnected from the bus, and 
- *      there are no connected commands, we reconnect the I_T_L or 
- *      I_T_L_Q nexus associated with it, go into message out, and send 
- *      an abort message.
- *
- * This case is especially ugly. In order to reestablish the nexus, we
- * need to call NCR5380_select().  The easiest way to implement this 
- * function was to abort if the bus was busy, and let the interrupt
- * handler triggered on the SEL for reselect take care of lost arbitrations
- * where necessary, meaning interrupts need to be enabled.
- *
- * When interrupts are enabled, the queues may change - so we 
- * can't remove it from the disconnected queue before selecting it
- * because that could cause a failure in hashing the nexus if that 
- * device reselected.
- * 
- * Since the queues may change, we can't use the pointers from when we
- * first locate it.
- *
- * So, we must first locate the command, and if NCR5380_select()
- * succeeds, then issue the abort, relocate the command and remove
- * it from the disconnected queue.
- */
 
-	for (tmp = (struct scsi_cmnd *) hostdata->disconnected_queue; tmp; tmp = (struct scsi_cmnd *) tmp->host_scribble)
-		if (cmd == tmp) {
-			dprintk(NDEBUG_ABORT, "scsi%d : aborting disconnected command.\n", instance->host_no);
+	if (hostdata->connected == cmd) {
+		dsprintk(NDEBUG_ABORT, instance, "abort: cmd %p is connected\n", cmd);
+		hostdata->connected = NULL;
+		if (do_abort(instance)) {
+			set_host_byte(cmd, DID_ERROR);
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
+		}
+		set_host_byte(cmd, DID_ABORT);
+#ifdef REAL_DMA
+		hostdata->dma_len = 0;
+#endif
+		complete_cmd(instance, cmd);
+	}
 
-			if (NCR5380_select(instance, cmd))
-				return FAILED;
-			dprintk(NDEBUG_ABORT, "scsi%d : nexus reestablished.\n", instance->host_no);
+out:
+	if (result == FAILED)
+		dsprintk(NDEBUG_ABORT, instance, "abort: failed to abort %p\n", cmd);
+	else
+		dsprintk(NDEBUG_ABORT, instance, "abort: successfully aborted %p\n", cmd);
 
-			do_abort(instance);
+	queue_work(hostdata->work_q, &hostdata->main_task);
+	spin_unlock_irqrestore(&hostdata->lock, flags);
 
-			for (prev = (struct scsi_cmnd **) &(hostdata->disconnected_queue), tmp = (struct scsi_cmnd *) hostdata->disconnected_queue; tmp; prev = (struct scsi_cmnd **) &(tmp->host_scribble), tmp = (struct scsi_cmnd *) tmp->host_scribble)
-				if (cmd == tmp) {
-					REMOVE(5, *prev, tmp, tmp->host_scribble);
-					*prev = (struct scsi_cmnd *) tmp->host_scribble;
-					tmp->host_scribble = NULL;
-					tmp->result = DID_ABORT << 16;
-					tmp->scsi_done(tmp);
-					return SUCCESS;
-				}
-		}
-/*
- * Case 5 : If we reached this point, the command was not found in any of 
- *          the queues.
- *
- * We probably reached this point because of an unlikely race condition
- * between the command completing successfully and the abortion code,
- * so we won't panic, but we will notify the user in case something really
- * broke.
- */
-	printk(KERN_WARNING "scsi%d : warning : SCSI command probably completed successfully\n"
-			"         before abortion\n", instance->host_no);
-	return FAILED;
+	return result;
 }
 
 
-/* 
- * Function : int NCR5380_bus_reset (struct scsi_cmnd *cmd)
- * 
- * Purpose : reset the SCSI bus.
- *
- * Returns : SUCCESS
+/**
+ * NCR5380_bus_reset - reset the SCSI bus
+ * @cmd: SCSI command undergoing EH
  *
- * Locks: host lock taken by caller
+ * Returns SUCCESS
  */
 
 static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
 {
 	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int i;
+	unsigned long flags;
+	struct NCR5380_cmd *ncmd;
 
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-	NCR5380_print_status(instance);
+	spin_lock_irqsave(&hostdata->lock, flags);
+
+#if (NDEBUG & NDEBUG_ANY)
+	scmd_printk(KERN_INFO, cmd, __func__);
+#endif
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
 
-	spin_lock_irq(instance->host_lock);
 	do_reset(instance);
-	spin_unlock_irq(instance->host_lock);
+
+	/* reset NCR registers */
+	NCR5380_write(MODE_REG, MR_BASE);
+	NCR5380_write(TARGET_COMMAND_REG, 0);
+	NCR5380_write(SELECT_ENABLE_REG, 0);
+
+	/* After the reset, there are no more connected or disconnected commands
+	 * and no busy units; so clear the low-level status here to avoid
+	 * conflicts when the mid-level code tries to wake up the affected
+	 * commands!
+	 */
+
+	hostdata->selecting = NULL;
+
+	list_for_each_entry(ncmd, &hostdata->disconnected, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
+
+		set_host_byte(cmd, DID_RESET);
+		cmd->scsi_done(cmd);
+	}
+
+	list_for_each_entry(ncmd, &hostdata->autosense, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
+
+		set_host_byte(cmd, DID_RESET);
+		cmd->scsi_done(cmd);
+	}
+
+	if (hostdata->connected) {
+		set_host_byte(hostdata->connected, DID_RESET);
+		complete_cmd(instance, hostdata->connected);
+		hostdata->connected = NULL;
+	}
+
+	if (hostdata->sensing) {
+		set_host_byte(hostdata->connected, DID_RESET);
+		complete_cmd(instance, hostdata->sensing);
+		hostdata->sensing = NULL;
+	}
+
+	for (i = 0; i < 8; ++i)
+		hostdata->busy[i] = 0;
+#ifdef REAL_DMA
+	hostdata->dma_len = 0;
+#endif
+
+	queue_work(hostdata->work_q, &hostdata->main_task);
+	spin_unlock_irqrestore(&hostdata->lock, flags);
 
 	return SUCCESS;
 }
diff --git a/drivers/scsi/NCR5380.h b/drivers/scsi/NCR5380.h
index 162112dd1bf8..a79288682a74 100644
--- a/drivers/scsi/NCR5380.h
+++ b/drivers/scsi/NCR5380.h
@@ -22,8 +22,13 @@
 #ifndef NCR5380_H
 #define NCR5380_H
 
+#include <linux/delay.h>
 #include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/workqueue.h>
+#include <scsi/scsi_dbg.h>
 #include <scsi/scsi_eh.h>
+#include <scsi/scsi_transport_spi.h>
 
 #define NDEBUG_ARBITRATION	0x1
 #define NDEBUG_AUTOSENSE	0x2
@@ -158,8 +163,7 @@
 /* Write any value to this register to start an ini mode DMA receive */
 #define START_DMA_INITIATOR_RECEIVE_REG 7	/* wo */
 
-#define C400_CONTROL_STATUS_REG NCR53C400_register_offset-8	/* rw */
-
+/* NCR 53C400(A) Control Status Register bits: */
 #define CSR_RESET              0x80	/* wo  Resets 53c400 */
 #define CSR_53C80_REG          0x80	/* ro  5380 registers busy */
 #define CSR_TRANS_DIR          0x40	/* rw  Data transfer direction */
@@ -176,16 +180,6 @@
 #define CSR_BASE CSR_53C80_INTR
 #endif
 
-/* Number of 128-byte blocks to be transferred */
-#define C400_BLOCK_COUNTER_REG   NCR53C400_register_offset-7	/* rw */
-
-/* Resume transfer after disconnect */
-#define C400_RESUME_TRANSFER_REG NCR53C400_register_offset-6	/* wo */
-
-/* Access to host buffer stack */
-#define C400_HOST_BUFFER         NCR53C400_register_offset-4	/* rw */
-
-
 /* Note : PHASE_* macros are based on the values of the STATUS register */
 #define PHASE_MASK 	(SR_MSG | SR_CD | SR_IO)
 
@@ -205,16 +199,6 @@
 
 #define PHASE_SR_TO_TCR(phase) ((phase) >> 2)
 
-/*
- * The internal should_disconnect() function returns these based on the 
- * expected length of a disconnect if a device supports disconnect/
- * reconnect.
- */
-
-#define DISCONNECT_NONE		0
-#define DISCONNECT_TIME_TO_DATA	1
-#define DISCONNECT_LONG		2
-
 /* 
  * "Special" value for the (unsigned char) command tag, to indicate
  * I_T_L nexus instead of I_T_L_Q.
@@ -236,15 +220,11 @@
 #define NO_IRQ		0
 #endif
 
-#define FLAG_HAS_LAST_BYTE_SENT		1	/* NCR53c81 or better */
-#define FLAG_CHECK_LAST_BYTE_SENT	2	/* Only test once */
-#define FLAG_NCR53C400			4	/* NCR53c400 */
+#define FLAG_NO_DMA_FIXUP		1	/* No DMA errata workarounds */
 #define FLAG_NO_PSEUDO_DMA		8	/* Inhibit DMA */
-#define FLAG_DTC3181E			16	/* DTC3181E */
 #define FLAG_LATE_DMA_SETUP		32	/* Setup NCR before DMA H/W */
 #define FLAG_TAGGED_QUEUING		64	/* as X3T9.2 spelled it */
-
-#ifndef ASM
+#define FLAG_TOSHIBA_DELAY		128	/* Allow for borken CD-ROMs */
 
 #ifdef SUPPORT_TAGS
 struct tag_alloc {
@@ -258,33 +238,24 @@ struct NCR5380_hostdata {
 	NCR5380_implementation_fields;		/* implementation specific */
 	struct Scsi_Host *host;			/* Host backpointer */
 	unsigned char id_mask, id_higher_mask;	/* 1 << id, all bits greater */
-	unsigned char targets_present;		/* targets we have connected
-						   to, so we can call a select
-						   failure a retryable condition */
-	volatile unsigned char busy[8];		/* index = target, bit = lun */
+	unsigned char busy[8];			/* index = target, bit = lun */
 #if defined(REAL_DMA) || defined(REAL_DMA_POLL)
-	volatile int dma_len;			/* requested length of DMA */
+	int dma_len;				/* requested length of DMA */
 #endif
-	volatile unsigned char last_message;	/* last message OUT */
-	volatile struct scsi_cmnd *connected;	/* currently connected command */
-	volatile struct scsi_cmnd *issue_queue;	/* waiting to be issued */
-	volatile struct scsi_cmnd *disconnected_queue;	/* waiting for reconnect */
-	volatile int restart_select;		/* we have disconnected,
-						   used to restart 
-						   NCR5380_select() */
-	volatile unsigned aborted:1;		/* flag, says aborted */
+	unsigned char last_message;		/* last message OUT */
+	struct scsi_cmnd *connected;		/* currently connected cmnd */
+	struct scsi_cmnd *selecting;		/* cmnd to be connected */
+	struct list_head unissued;		/* waiting to be issued */
+	struct list_head autosense;		/* priority issue queue */
+	struct list_head disconnected;		/* waiting for reconnect */
+	spinlock_t lock;			/* protects this struct */
 	int flags;
-	unsigned long time_expires;		/* in jiffies, set prior to sleeping */
-	int select_time;			/* timer in select for target response */
-	volatile struct scsi_cmnd *selecting;
-	struct delayed_work coroutine;		/* our co-routine */
 	struct scsi_eh_save ses;
+	struct scsi_cmnd *sensing;
 	char info[256];
 	int read_overruns;                /* number of bytes to cut from a
 	                                   * transfer to handle chip overruns */
-	int retain_dma_intr;
 	struct work_struct main_task;
-	volatile int main_running;
 #ifdef SUPPORT_TAGS
 	struct tag_alloc TagAlloc[8][8];	/* 8 targets and 8 LUNs */
 #endif
@@ -292,10 +263,23 @@ struct NCR5380_hostdata {
 	unsigned spin_max_r;
 	unsigned spin_max_w;
 #endif
+	struct workqueue_struct *work_q;
+	unsigned long accesses_per_ms;	/* chip register accesses per ms */
 };
 
 #ifdef __KERNEL__
 
+struct NCR5380_cmd {
+	struct list_head list;
+};
+
+#define NCR5380_CMD_SIZE		(sizeof(struct NCR5380_cmd))
+
+static inline struct scsi_cmnd *NCR5380_to_scmd(struct NCR5380_cmd *ncmd_ptr)
+{
+	return ((struct scsi_cmnd *)ncmd_ptr) - 1;
+}
+
 #ifndef NDEBUG
 #define NDEBUG (0)
 #endif
@@ -304,6 +288,11 @@ struct NCR5380_hostdata {
 	do { if ((NDEBUG) & (flg)) \
 		printk(KERN_DEBUG fmt, ## __VA_ARGS__); } while (0)
 
+#define dsprintk(flg, host, fmt, ...) \
+	do { if ((NDEBUG) & (flg)) \
+		shost_printk(KERN_DEBUG, host, fmt, ## __VA_ARGS__); \
+	} while (0)
+
 #if NDEBUG
 #define NCR5380_dprint(flg, arg) \
 	do { if ((NDEBUG) & (flg)) NCR5380_print(arg); } while (0)
@@ -320,6 +309,7 @@ static void NCR5380_print(struct Scsi_Host *instance);
 static int NCR5380_probe_irq(struct Scsi_Host *instance, int possible);
 #endif
 static int NCR5380_init(struct Scsi_Host *instance, int flags);
+static int NCR5380_maybe_reset_bus(struct Scsi_Host *);
 static void NCR5380_exit(struct Scsi_Host *instance);
 static void NCR5380_information_transfer(struct Scsi_Host *instance);
 #ifndef DONT_USE_INTR
@@ -328,7 +318,7 @@ static irqreturn_t NCR5380_intr(int irq, void *dev_id);
 static void NCR5380_main(struct work_struct *work);
 static const char *NCR5380_info(struct Scsi_Host *instance);
 static void NCR5380_reselect(struct Scsi_Host *instance);
-static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd);
+static struct scsi_cmnd *NCR5380_select(struct Scsi_Host *, struct scsi_cmnd *);
 #if defined(PSEUDO_DMA) || defined(REAL_DMA) || defined(REAL_DMA_POLL)
 static int NCR5380_transfer_dma(struct Scsi_Host *instance, unsigned char *phase, int *count, unsigned char **data);
 #endif
@@ -443,5 +433,4 @@ static __inline__ int NCR5380_pc_dma_residual(struct Scsi_Host *instance)
 #endif				/* defined(i386) || defined(__alpha__) */
 #endif				/* defined(REAL_DMA)  */
 #endif				/* __KERNEL__ */
-#endif				/* ndef ASM */
 #endif				/* NCR5380_H */
diff --git a/drivers/scsi/aacraid/linit.c b/drivers/scsi/aacraid/linit.c
index 3b6e5c67e853..76eaa38ffd6e 100644
--- a/drivers/scsi/aacraid/linit.c
+++ b/drivers/scsi/aacraid/linit.c
@@ -1318,7 +1318,7 @@ static int aac_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 }
 
 #if (defined(CONFIG_PM))
-void aac_release_resources(struct aac_dev *aac)
+static void aac_release_resources(struct aac_dev *aac)
 {
 	int i;
 
diff --git a/drivers/scsi/aic7xxx/aic79xx.h b/drivers/scsi/aic7xxx/aic79xx.h
index df2e0e5367d2..d47b527b25dd 100644
--- a/drivers/scsi/aic7xxx/aic79xx.h
+++ b/drivers/scsi/aic7xxx/aic79xx.h
@@ -624,7 +624,7 @@ struct scb {
 };
 
 TAILQ_HEAD(scb_tailq, scb);
-LIST_HEAD(scb_list, scb);
+BSD_LIST_HEAD(scb_list, scb);
 
 struct scb_data {
 	/*
@@ -1069,7 +1069,7 @@ struct ahd_softc {
 	/*
 	 * SCBs that have been sent to the controller
 	 */
-	LIST_HEAD(, scb)	  pending_scbs;
+	BSD_LIST_HEAD(, scb)	  pending_scbs;
 
 	/*
 	 * Current register window mode information.
diff --git a/drivers/scsi/aic7xxx/aic79xx_osm.h b/drivers/scsi/aic7xxx/aic79xx_osm.h
index c58fa33c6592..728193a42e6e 100644
--- a/drivers/scsi/aic7xxx/aic79xx_osm.h
+++ b/drivers/scsi/aic7xxx/aic79xx_osm.h
@@ -65,11 +65,6 @@
 /* Core SCSI definitions */
 #define AIC_LIB_PREFIX ahd
 
-/* Name space conflict with BSD queue macros */
-#ifdef LIST_HEAD
-#undef LIST_HEAD
-#endif
-
 #include "cam.h"
 #include "queue.h"
 #include "scsi_message.h"
diff --git a/drivers/scsi/aic7xxx/aic7xxx.h b/drivers/scsi/aic7xxx/aic7xxx.h
index f695774645c1..4ce4e903a759 100644
--- a/drivers/scsi/aic7xxx/aic7xxx.h
+++ b/drivers/scsi/aic7xxx/aic7xxx.h
@@ -916,7 +916,7 @@ struct ahc_softc {
 	/*
 	 * SCBs that have been sent to the controller
 	 */
-	LIST_HEAD(, scb)	  pending_scbs;
+	BSD_LIST_HEAD(, scb)	  pending_scbs;
 
 	/*
 	 * Counting lock for deferring the release of additional
diff --git a/drivers/scsi/aic7xxx/aic7xxx_osm.h b/drivers/scsi/aic7xxx/aic7xxx_osm.h
index bc4cca92ff04..54c702864103 100644
--- a/drivers/scsi/aic7xxx/aic7xxx_osm.h
+++ b/drivers/scsi/aic7xxx/aic7xxx_osm.h
@@ -82,11 +82,6 @@
 /* Core SCSI definitions */
 #define AIC_LIB_PREFIX ahc
 
-/* Name space conflict with BSD queue macros */
-#ifdef LIST_HEAD
-#undef LIST_HEAD
-#endif
-
 #include "cam.h"
 #include "queue.h"
 #include "scsi_message.h"
diff --git a/drivers/scsi/aic7xxx/queue.h b/drivers/scsi/aic7xxx/queue.h
index 8adf8003a164..ba602981f193 100644
--- a/drivers/scsi/aic7xxx/queue.h
+++ b/drivers/scsi/aic7xxx/queue.h
@@ -246,7 +246,7 @@ struct {								\
 /*
  * List declarations.
  */
-#define	LIST_HEAD(name, type)						\
+#define	BSD_LIST_HEAD(name, type)					\
 struct name {								\
 	struct type *lh_first;	/* first element */			\
 }
diff --git a/drivers/scsi/aic94xx/aic94xx_sas.h b/drivers/scsi/aic94xx/aic94xx_sas.h
index 912e6b755f74..101072cab70f 100644
--- a/drivers/scsi/aic94xx/aic94xx_sas.h
+++ b/drivers/scsi/aic94xx/aic94xx_sas.h
@@ -327,46 +327,9 @@ struct scb_header {
 
 #define LUN_SIZE                8
 
-/* See SAS spec, task IU
- */
-struct ssp_task_iu {
-	u8     lun[LUN_SIZE];	  /* BE */
-	u16    _r_a;
-	u8     tmf;
-	u8     _r_b;
-	__be16 tag;		  /* BE */
-	u8     _r_c[14];
-} __attribute__ ((packed));
-
-/* See SAS spec, command IU
- */
-struct ssp_command_iu {
-	u8     lun[LUN_SIZE];
-	u8     _r_a;
-	u8     efb_prio_attr;	  /* enable first burst, task prio & attr */
-#define EFB_MASK        0x80
-#define TASK_PRIO_MASK	0x78
-#define TASK_ATTR_MASK  0x07
-
-	u8    _r_b;
-	u8     add_cdb_len;	  /* in dwords, since bit 0,1 are reserved */
-	union {
-		u8     cdb[16];
-		struct {
-			__le64 long_cdb_addr;	  /* bus address, LE */
-			__le32 long_cdb_size;	  /* LE */
-			u8     _r_c[3];
-			u8     eol_ds;		  /* eol:6,6, ds:5,4 */
-		} long_cdb;	  /* sequencer extension */
-	};
-} __attribute__ ((packed));
-
-struct xfer_rdy_iu {
-	__be32 requested_offset;  /* BE */
-	__be32 write_data_len;	  /* BE */
-	__be32 _r_a;
-} __attribute__ ((packed));
-
+#define EFB_MASK                0x80
+#define TASK_PRIO_MASK          0x78
+#define TASK_ATTR_MASK          0x07
 /* ---------- SCB tasks ---------- */
 
 /* This is both ssp_task and long_ssp_task
@@ -511,7 +474,7 @@ struct abort_task {
 	u8     proto_conn_rate;
 	__le32 _r_a;
 	struct ssp_frame_hdr ssp_frame;
-	struct ssp_task_iu ssp_task;
+	struct ssp_tmf_iu ssp_task;
 	__le16 sister_scb;
 	__le16 conn_handle;
 	u8     flags;	  /* ovrd_itnl_timer:3,3, suspend_data_trans:2,2 */
@@ -549,7 +512,7 @@ struct clear_nexus {
 	u8     _r_b[3];
 	u8     conn_mask;
 	u8     _r_c[19];
-	struct ssp_task_iu ssp_task; /* LUN and TAG */
+	struct ssp_tmf_iu ssp_task; /* LUN and TAG */
 	__le16 _r_d;
 	__le16 conn_handle;
 	__le64 _r_e;
@@ -562,7 +525,7 @@ struct initiate_ssp_tmf {
 	u8     proto_conn_rate;
 	__le32 _r_a;
 	struct ssp_frame_hdr ssp_frame;
-	struct ssp_task_iu ssp_task;
+	struct ssp_tmf_iu ssp_task;
 	__le16 sister_scb;
 	__le16 conn_handle;
 	u8     flags;	  /* itnl override and suspend data tx */
diff --git a/drivers/scsi/arcmsr/arcmsr.h b/drivers/scsi/arcmsr/arcmsr.h
index 3bcaaac0ae4b..cf99f8cf4cdd 100644
--- a/drivers/scsi/arcmsr/arcmsr.h
+++ b/drivers/scsi/arcmsr/arcmsr.h
@@ -52,7 +52,7 @@ struct device_attribute;
 	#define ARCMSR_MAX_FREECCB_NUM	320
 #define ARCMSR_MAX_OUTSTANDING_CMD	255
 #endif
-#define ARCMSR_DRIVER_VERSION		"v1.30.00.04-20140919"
+#define ARCMSR_DRIVER_VERSION		"v1.30.00.22-20151126"
 #define ARCMSR_SCSI_INITIATOR_ID						255
 #define ARCMSR_MAX_XFER_SECTORS							512
 #define ARCMSR_MAX_XFER_SECTORS_B						4096
@@ -74,6 +74,9 @@ struct device_attribute;
 #ifndef PCI_DEVICE_ID_ARECA_1214
 	#define PCI_DEVICE_ID_ARECA_1214	0x1214
 #endif
+#ifndef PCI_DEVICE_ID_ARECA_1203
+	#define PCI_DEVICE_ID_ARECA_1203	0x1203
+#endif
 /*
 **********************************************************************************
 **
@@ -245,6 +248,12 @@ struct FIRMWARE_INFO
 /* window of "instruction flags" from iop to driver */
 #define ARCMSR_IOP2DRV_DOORBELL                       0x00020408
 #define ARCMSR_IOP2DRV_DOORBELL_MASK                  0x0002040C
+/* window of "instruction flags" from iop to driver */
+#define ARCMSR_IOP2DRV_DOORBELL_1203                  0x00021870
+#define ARCMSR_IOP2DRV_DOORBELL_MASK_1203             0x00021874
+/* window of "instruction flags" from driver to iop */
+#define ARCMSR_DRV2IOP_DOORBELL_1203                  0x00021878
+#define ARCMSR_DRV2IOP_DOORBELL_MASK_1203             0x0002187C
 /* ARECA FLAG LANGUAGE */
 /* ioctl transfer */
 #define ARCMSR_IOP2DRV_DATA_WRITE_OK                  0x00000001
@@ -288,6 +297,9 @@ struct FIRMWARE_INFO
 #define ARCMSR_MESSAGE_RBUFFER			      0x0000ff00
 /* iop message_rwbuffer for message command */
 #define ARCMSR_MESSAGE_RWBUFFER			      0x0000fa00
+
+#define MEM_BASE0(x)	(u32 __iomem *)((unsigned long)acb->mem_base0 + x)
+#define MEM_BASE1(x)	(u32 __iomem *)((unsigned long)acb->mem_base1 + x)
 /* 
 ************************************************************************
 **                SPEC. for Areca HBC adapter
diff --git a/drivers/scsi/arcmsr/arcmsr_hba.c b/drivers/scsi/arcmsr/arcmsr_hba.c
index 333db5953607..7640498964a5 100644
--- a/drivers/scsi/arcmsr/arcmsr_hba.c
+++ b/drivers/scsi/arcmsr/arcmsr_hba.c
@@ -114,6 +114,7 @@ static void arcmsr_hardware_reset(struct AdapterControlBlock *acb);
 static const char *arcmsr_info(struct Scsi_Host *);
 static irqreturn_t arcmsr_interrupt(struct AdapterControlBlock *acb);
 static void arcmsr_free_irq(struct pci_dev *, struct AdapterControlBlock *);
+static void arcmsr_wait_firmware_ready(struct AdapterControlBlock *acb);
 static int arcmsr_adjust_disk_queue_depth(struct scsi_device *sdev, int queue_depth)
 {
 	if (queue_depth > ARCMSR_MAX_CMD_PERLUN)
@@ -157,6 +158,8 @@ static struct pci_device_id arcmsr_device_id_table[] = {
 		.driver_data = ACB_ADAPTER_TYPE_B},
 	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1202),
 		.driver_data = ACB_ADAPTER_TYPE_B},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1203),
+		.driver_data = ACB_ADAPTER_TYPE_B},
 	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1210),
 		.driver_data = ACB_ADAPTER_TYPE_A},
 	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1214),
@@ -495,6 +498,91 @@ static void arcmsr_flush_adapter_cache(struct AdapterControlBlock *acb)
 	}
 }
 
+static bool arcmsr_alloc_io_queue(struct AdapterControlBlock *acb)
+{
+	bool rtn = true;
+	void *dma_coherent;
+	dma_addr_t dma_coherent_handle;
+	struct pci_dev *pdev = acb->pdev;
+
+	switch (acb->adapter_type) {
+	case ACB_ADAPTER_TYPE_B: {
+		struct MessageUnit_B *reg;
+		acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_B), 32);
+		dma_coherent = dma_zalloc_coherent(&pdev->dev, acb->roundup_ccbsize,
+			&dma_coherent_handle, GFP_KERNEL);
+		if (!dma_coherent) {
+			pr_notice("arcmsr%d: DMA allocation failed\n", acb->host->host_no);
+			return false;
+		}
+		acb->dma_coherent_handle2 = dma_coherent_handle;
+		acb->dma_coherent2 = dma_coherent;
+		reg = (struct MessageUnit_B *)dma_coherent;
+		acb->pmuB = reg;
+		if (acb->pdev->device == PCI_DEVICE_ID_ARECA_1203) {
+			reg->drv2iop_doorbell = MEM_BASE0(ARCMSR_DRV2IOP_DOORBELL_1203);
+			reg->drv2iop_doorbell_mask = MEM_BASE0(ARCMSR_DRV2IOP_DOORBELL_MASK_1203);
+			reg->iop2drv_doorbell = MEM_BASE0(ARCMSR_IOP2DRV_DOORBELL_1203);
+			reg->iop2drv_doorbell_mask = MEM_BASE0(ARCMSR_IOP2DRV_DOORBELL_MASK_1203);
+		} else {
+			reg->drv2iop_doorbell = MEM_BASE0(ARCMSR_DRV2IOP_DOORBELL);
+			reg->drv2iop_doorbell_mask = MEM_BASE0(ARCMSR_DRV2IOP_DOORBELL_MASK);
+			reg->iop2drv_doorbell = MEM_BASE0(ARCMSR_IOP2DRV_DOORBELL);
+			reg->iop2drv_doorbell_mask = MEM_BASE0(ARCMSR_IOP2DRV_DOORBELL_MASK);
+		}
+		reg->message_wbuffer = MEM_BASE1(ARCMSR_MESSAGE_WBUFFER);
+		reg->message_rbuffer = MEM_BASE1(ARCMSR_MESSAGE_RBUFFER);
+		reg->message_rwbuffer = MEM_BASE1(ARCMSR_MESSAGE_RWBUFFER);
+		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg;
+
+		acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_D), 32);
+		dma_coherent = dma_zalloc_coherent(&pdev->dev, acb->roundup_ccbsize,
+			&dma_coherent_handle, GFP_KERNEL);
+		if (!dma_coherent) {
+			pr_notice("arcmsr%d: DMA allocation failed\n", acb->host->host_no);
+			return false;
+		}
+		acb->dma_coherent_handle2 = dma_coherent_handle;
+		acb->dma_coherent2 = dma_coherent;
+		reg = (struct MessageUnit_D *)dma_coherent;
+		acb->pmuD = reg;
+		reg->chip_id = MEM_BASE0(ARCMSR_ARC1214_CHIP_ID);
+		reg->cpu_mem_config = MEM_BASE0(ARCMSR_ARC1214_CPU_MEMORY_CONFIGURATION);
+		reg->i2o_host_interrupt_mask = MEM_BASE0(ARCMSR_ARC1214_I2_HOST_INTERRUPT_MASK);
+		reg->sample_at_reset = MEM_BASE0(ARCMSR_ARC1214_SAMPLE_RESET);
+		reg->reset_request = MEM_BASE0(ARCMSR_ARC1214_RESET_REQUEST);
+		reg->host_int_status = MEM_BASE0(ARCMSR_ARC1214_MAIN_INTERRUPT_STATUS);
+		reg->pcief0_int_enable = MEM_BASE0(ARCMSR_ARC1214_PCIE_F0_INTERRUPT_ENABLE);
+		reg->inbound_msgaddr0 = MEM_BASE0(ARCMSR_ARC1214_INBOUND_MESSAGE0);
+		reg->inbound_msgaddr1 = MEM_BASE0(ARCMSR_ARC1214_INBOUND_MESSAGE1);
+		reg->outbound_msgaddr0 = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_MESSAGE0);
+		reg->outbound_msgaddr1 = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_MESSAGE1);
+		reg->inbound_doorbell = MEM_BASE0(ARCMSR_ARC1214_INBOUND_DOORBELL);
+		reg->outbound_doorbell = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_DOORBELL);
+		reg->outbound_doorbell_enable = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_DOORBELL_ENABLE);
+		reg->inboundlist_base_low = MEM_BASE0(ARCMSR_ARC1214_INBOUND_LIST_BASE_LOW);
+		reg->inboundlist_base_high = MEM_BASE0(ARCMSR_ARC1214_INBOUND_LIST_BASE_HIGH);
+		reg->inboundlist_write_pointer = MEM_BASE0(ARCMSR_ARC1214_INBOUND_LIST_WRITE_POINTER);
+		reg->outboundlist_base_low = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_LIST_BASE_LOW);
+		reg->outboundlist_base_high = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_LIST_BASE_HIGH);
+		reg->outboundlist_copy_pointer = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_LIST_COPY_POINTER);
+		reg->outboundlist_read_pointer = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_LIST_READ_POINTER);
+		reg->outboundlist_interrupt_cause = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_INTERRUPT_CAUSE);
+		reg->outboundlist_interrupt_enable = MEM_BASE0(ARCMSR_ARC1214_OUTBOUND_INTERRUPT_ENABLE);
+		reg->message_wbuffer = MEM_BASE0(ARCMSR_ARC1214_MESSAGE_WBUFFER);
+		reg->message_rbuffer = MEM_BASE0(ARCMSR_ARC1214_MESSAGE_RBUFFER);
+		reg->msgcode_rwbuffer = MEM_BASE0(ARCMSR_ARC1214_MESSAGE_RWBUFFER);
+		}
+		break;
+	default:
+		break;
+	}
+	return rtn;
+}
+
 static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
 {
 	struct pci_dev *pdev = acb->pdev;
@@ -739,9 +827,12 @@ static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if(!error){
 		goto pci_release_regs;
 	}
+	error = arcmsr_alloc_io_queue(acb);
+	if (!error)
+		goto unmap_pci_region;
 	error = arcmsr_get_firmware_spec(acb);
 	if(!error){
-		goto unmap_pci_region;
+		goto free_hbb_mu;
 	}
 	error = arcmsr_alloc_ccb_pool(acb);
 	if(error){
@@ -2622,9 +2713,6 @@ static bool arcmsr_hbaA_get_config(struct AdapterControlBlock *acb)
 static bool arcmsr_hbaB_get_config(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
-	struct pci_dev *pdev = acb->pdev;
-	void *dma_coherent;
-	dma_addr_t dma_coherent_handle;
 	char *acb_firm_model = acb->firm_model;
 	char *acb_firm_version = acb->firm_version;
 	char *acb_device_map = acb->device_map;
@@ -2636,30 +2724,16 @@ static bool arcmsr_hbaB_get_config(struct AdapterControlBlock *acb)
 	/*firm_version,21,84-99*/
 	int count;
 
-	acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_B), 32);
-	dma_coherent = dma_alloc_coherent(&pdev->dev, acb->roundup_ccbsize,
-			&dma_coherent_handle, GFP_KERNEL);
-	if (!dma_coherent){
-		printk(KERN_NOTICE
-			"arcmsr%d: dma_alloc_coherent got error for hbb mu\n",
-			acb->host->host_no);
-		return false;
-	}
-	acb->dma_coherent_handle2 = dma_coherent_handle;
-	acb->dma_coherent2 = dma_coherent;
-	reg = (struct MessageUnit_B *)dma_coherent;
-	acb->pmuB = reg;
-	reg->drv2iop_doorbell= (uint32_t __iomem *)((unsigned long)acb->mem_base0 + ARCMSR_DRV2IOP_DOORBELL);
-	reg->drv2iop_doorbell_mask = (uint32_t __iomem *)((unsigned long)acb->mem_base0 + ARCMSR_DRV2IOP_DOORBELL_MASK);
-	reg->iop2drv_doorbell = (uint32_t __iomem *)((unsigned long)acb->mem_base0 + ARCMSR_IOP2DRV_DOORBELL);
-	reg->iop2drv_doorbell_mask = (uint32_t __iomem *)((unsigned long)acb->mem_base0 + ARCMSR_IOP2DRV_DOORBELL_MASK);
-	reg->message_wbuffer = (uint32_t __iomem *)((unsigned long)acb->mem_base1 + ARCMSR_MESSAGE_WBUFFER);
-	reg->message_rbuffer =  (uint32_t __iomem *)((unsigned long)acb->mem_base1 + ARCMSR_MESSAGE_RBUFFER);
-	reg->message_rwbuffer = (uint32_t __iomem *)((unsigned long)acb->mem_base1 + ARCMSR_MESSAGE_RWBUFFER);
 	iop_firm_model = (char __iomem *)(&reg->message_rwbuffer[15]);	/*firm_model,15,60-67*/
 	iop_firm_version = (char __iomem *)(&reg->message_rwbuffer[17]);	/*firm_version,17,68-83*/
 	iop_device_map = (char __iomem *)(&reg->message_rwbuffer[21]);	/*firm_version,21,84-99*/
 
+	arcmsr_wait_firmware_ready(acb);
+	writel(ARCMSR_MESSAGE_START_DRIVER_MODE, reg->drv2iop_doorbell);
+	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
+		printk(KERN_ERR "arcmsr%d: can't set driver mode.\n", acb->host->host_no);
+		return false;
+	}
 	writel(ARCMSR_MESSAGE_GET_CONFIG, reg->drv2iop_doorbell);
 	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'get adapter firmware \
@@ -2694,15 +2768,15 @@ static bool arcmsr_hbaB_get_config(struct AdapterControlBlock *acb)
 		acb->firm_model,
 		acb->firm_version);
 
-	acb->signature = readl(&reg->message_rwbuffer[1]);
+	acb->signature = readl(&reg->message_rwbuffer[0]);
 	/*firm_signature,1,00-03*/
-	acb->firm_request_len = readl(&reg->message_rwbuffer[2]);
+	acb->firm_request_len = readl(&reg->message_rwbuffer[1]);
 	/*firm_request_len,1,04-07*/
-	acb->firm_numbers_queue = readl(&reg->message_rwbuffer[3]);
+	acb->firm_numbers_queue = readl(&reg->message_rwbuffer[2]);
 	/*firm_numbers_queue,2,08-11*/
-	acb->firm_sdram_size = readl(&reg->message_rwbuffer[4]);
+	acb->firm_sdram_size = readl(&reg->message_rwbuffer[3]);
 	/*firm_sdram_size,3,12-15*/
-	acb->firm_hd_channels = readl(&reg->message_rwbuffer[5]);
+	acb->firm_hd_channels = readl(&reg->message_rwbuffer[4]);
 	/*firm_ide_channels,4,16-19*/
 	acb->firm_cfg_version = readl(&reg->message_rwbuffer[25]);  /*firm_cfg_version,25,100-103*/
 	/*firm_ide_channels,4,16-19*/
@@ -2777,70 +2851,8 @@ static bool arcmsr_hbaD_get_config(struct AdapterControlBlock *acb)
 	char __iomem *iop_firm_version;
 	char __iomem *iop_device_map;
 	u32 count;
-	struct MessageUnit_D *reg;
-	void *dma_coherent2;
-	dma_addr_t dma_coherent_handle2;
-	struct pci_dev *pdev = acb->pdev;
+	struct MessageUnit_D *reg = acb->pmuD;
 
-	acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_D), 32);
-	dma_coherent2 = dma_alloc_coherent(&pdev->dev, acb->roundup_ccbsize,
-		&dma_coherent_handle2, GFP_KERNEL);
-	if (!dma_coherent2) {
-		pr_notice("DMA allocation failed...\n");
-		return false;
-	}
-	memset(dma_coherent2, 0, acb->roundup_ccbsize);
-	acb->dma_coherent_handle2 = dma_coherent_handle2;
-	acb->dma_coherent2 = dma_coherent2;
-	reg = (struct MessageUnit_D *)dma_coherent2;
-	acb->pmuD = reg;
-	reg->chip_id = acb->mem_base0 + ARCMSR_ARC1214_CHIP_ID;
-	reg->cpu_mem_config = acb->mem_base0 +
-		ARCMSR_ARC1214_CPU_MEMORY_CONFIGURATION;
-	reg->i2o_host_interrupt_mask = acb->mem_base0 +
-		ARCMSR_ARC1214_I2_HOST_INTERRUPT_MASK;
-	reg->sample_at_reset = acb->mem_base0 + ARCMSR_ARC1214_SAMPLE_RESET;
-	reg->reset_request = acb->mem_base0 + ARCMSR_ARC1214_RESET_REQUEST;
-	reg->host_int_status = acb->mem_base0 +
-		ARCMSR_ARC1214_MAIN_INTERRUPT_STATUS;
-	reg->pcief0_int_enable = acb->mem_base0 +
-		ARCMSR_ARC1214_PCIE_F0_INTERRUPT_ENABLE;
-	reg->inbound_msgaddr0 = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_MESSAGE0;
-	reg->inbound_msgaddr1 = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_MESSAGE1;
-	reg->outbound_msgaddr0 = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_MESSAGE0;
-	reg->outbound_msgaddr1 = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_MESSAGE1;
-	reg->inbound_doorbell = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_DOORBELL;
-	reg->outbound_doorbell = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_DOORBELL;
-	reg->outbound_doorbell_enable = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_DOORBELL_ENABLE;
-	reg->inboundlist_base_low = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_LIST_BASE_LOW;
-	reg->inboundlist_base_high = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_LIST_BASE_HIGH;
-	reg->inboundlist_write_pointer = acb->mem_base0 +
-		ARCMSR_ARC1214_INBOUND_LIST_WRITE_POINTER;
-	reg->outboundlist_base_low = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_LIST_BASE_LOW;
-	reg->outboundlist_base_high = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_LIST_BASE_HIGH;
-	reg->outboundlist_copy_pointer = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_LIST_COPY_POINTER;
-	reg->outboundlist_read_pointer = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_LIST_READ_POINTER;
-	reg->outboundlist_interrupt_cause = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_INTERRUPT_CAUSE;
-	reg->outboundlist_interrupt_enable = acb->mem_base0 +
-		ARCMSR_ARC1214_OUTBOUND_INTERRUPT_ENABLE;
-	reg->message_wbuffer = acb->mem_base0 + ARCMSR_ARC1214_MESSAGE_WBUFFER;
-	reg->message_rbuffer = acb->mem_base0 + ARCMSR_ARC1214_MESSAGE_RBUFFER;
-	reg->msgcode_rwbuffer = acb->mem_base0 +
-		ARCMSR_ARC1214_MESSAGE_RWBUFFER;
 	iop_firm_model = (char __iomem *)(&reg->msgcode_rwbuffer[15]);
 	iop_firm_version = (char __iomem *)(&reg->msgcode_rwbuffer[17]);
 	iop_device_map = (char __iomem *)(&reg->msgcode_rwbuffer[21]);
@@ -2855,8 +2867,6 @@ static bool arcmsr_hbaD_get_config(struct AdapterControlBlock *acb)
 	if (!arcmsr_hbaD_wait_msgint_ready(acb)) {
 		pr_notice("arcmsr%d: wait get adapter firmware "
 			"miscellaneous data timeout\n", acb->host->host_no);
-		dma_free_coherent(&acb->pdev->dev, acb->roundup_ccbsize,
-			acb->dma_coherent2, acb->dma_coherent_handle2);
 		return false;
 	}
 	count = 8;
@@ -2880,15 +2890,15 @@ static bool arcmsr_hbaD_get_config(struct AdapterControlBlock *acb)
 		iop_device_map++;
 		count--;
 	}
-	acb->signature = readl(&reg->msgcode_rwbuffer[1]);
+	acb->signature = readl(&reg->msgcode_rwbuffer[0]);
 	/*firm_signature,1,00-03*/
-	acb->firm_request_len = readl(&reg->msgcode_rwbuffer[2]);
+	acb->firm_request_len = readl(&reg->msgcode_rwbuffer[1]);
 	/*firm_request_len,1,04-07*/
-	acb->firm_numbers_queue = readl(&reg->msgcode_rwbuffer[3]);
+	acb->firm_numbers_queue = readl(&reg->msgcode_rwbuffer[2]);
 	/*firm_numbers_queue,2,08-11*/
-	acb->firm_sdram_size = readl(&reg->msgcode_rwbuffer[4]);
+	acb->firm_sdram_size = readl(&reg->msgcode_rwbuffer[3]);
 	/*firm_sdram_size,3,12-15*/
-	acb->firm_hd_channels = readl(&reg->msgcode_rwbuffer[5]);
+	acb->firm_hd_channels = readl(&reg->msgcode_rwbuffer[4]);
 	/*firm_hd_channels,4,16-19*/
 	acb->firm_cfg_version = readl(&reg->msgcode_rwbuffer[25]);
 	pr_notice("Areca RAID Controller%d: Model %s, F/W %s\n",
@@ -3998,6 +4008,7 @@ static const char *arcmsr_info(struct Scsi_Host *host)
 	case PCI_DEVICE_ID_ARECA_1160:
 	case PCI_DEVICE_ID_ARECA_1170:
 	case PCI_DEVICE_ID_ARECA_1201:
+	case PCI_DEVICE_ID_ARECA_1203:
 	case PCI_DEVICE_ID_ARECA_1220:
 	case PCI_DEVICE_ID_ARECA_1230:
 	case PCI_DEVICE_ID_ARECA_1260:
diff --git a/drivers/scsi/arm/cumana_1.c b/drivers/scsi/arm/cumana_1.c
index d28d6c0f18c0..221f18c5df93 100644
--- a/drivers/scsi/arm/cumana_1.c
+++ b/drivers/scsi/arm/cumana_1.c
@@ -4,9 +4,7 @@
  * Copyright 1995-2002, Russell King
  */
 #include <linux/module.h>
-#include <linux/signal.h>
 #include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/blkdev.h>
 #include <linux/init.h>
 
@@ -15,15 +13,14 @@
 
 #include <scsi/scsi_host.h>
 
-#include <scsi/scsicam.h>
-
 #define PSEUDO_DMA
 
 #define priv(host)			((struct NCR5380_hostdata *)(host)->hostdata)
-#define NCR5380_local_declare()		struct Scsi_Host *_instance
-#define NCR5380_setup(instance)		_instance = instance
-#define NCR5380_read(reg)		cumanascsi_read(_instance, reg)
-#define NCR5380_write(reg, value)	cumanascsi_write(_instance, reg, value)
+#define NCR5380_read(reg)		cumanascsi_read(instance, reg)
+#define NCR5380_write(reg, value)	cumanascsi_write(instance, reg, value)
+
+#define NCR5380_dma_xfer_len(instance, cmd, phase)	(cmd->transfersize)
+
 #define NCR5380_intr			cumanascsi_intr
 #define NCR5380_queue_command		cumanascsi_queue_command
 #define NCR5380_info			cumanascsi_info
@@ -211,6 +208,8 @@ static struct scsi_host_template cumanascsi_template = {
 	.cmd_per_lun		= 2,
 	.use_clustering		= DISABLE_CLUSTERING,
 	.proc_name		= "CumanaSCSI-1",
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 
 static int cumanascsi1_probe(struct expansion_card *ec,
@@ -240,23 +239,21 @@ static int cumanascsi1_probe(struct expansion_card *ec,
 
 	host->irq = ec->irq;
 
-	NCR5380_init(host, 0);
+	ret = NCR5380_init(host, 0);
+	if (ret)
+		goto out_unmap;
+
+	NCR5380_maybe_reset_bus(host);
 
         priv(host)->ctrl = 0;
         writeb(0, priv(host)->base + CTRL);
 
-	host->n_io_port = 255;
-	if (!(request_region(host->io_port, host->n_io_port, "CumanaSCSI-1"))) {
-		ret = -EBUSY;
-		goto out_unmap;
-	}
-
 	ret = request_irq(host->irq, cumanascsi_intr, 0,
 			  "CumanaSCSI-1", host);
 	if (ret) {
 		printk("scsi%d: IRQ%d not free: %d\n",
 		    host->host_no, host->irq, ret);
-		goto out_unmap;
+		goto out_exit;
 	}
 
 	ret = scsi_add_host(host, &ec->dev);
@@ -268,6 +265,8 @@ static int cumanascsi1_probe(struct expansion_card *ec,
 
  out_free_irq:
 	free_irq(host->irq, host);
+ out_exit:
+	NCR5380_exit(host);
  out_unmap:
 	iounmap(priv(host)->base);
 	iounmap(priv(host)->dma);
diff --git a/drivers/scsi/arm/oak.c b/drivers/scsi/arm/oak.c
index 7c6fa1479c9c..1fab1d1896b1 100644
--- a/drivers/scsi/arm/oak.c
+++ b/drivers/scsi/arm/oak.c
@@ -5,9 +5,7 @@
  */
 
 #include <linux/module.h>
-#include <linux/signal.h>
 #include <linux/ioport.h>
-#include <linux/delay.h>
 #include <linux/blkdev.h>
 #include <linux/init.h>
 
@@ -20,14 +18,16 @@
 #define DONT_USE_INTR
 
 #define priv(host)			((struct NCR5380_hostdata *)(host)->hostdata)
-#define NCR5380_local_declare()		void __iomem *_base
-#define NCR5380_setup(host)		_base = priv(host)->base
 
-#define NCR5380_read(reg)		readb(_base + ((reg) << 2))
-#define NCR5380_write(reg, value)	writeb(value, _base + ((reg) << 2))
+#define NCR5380_read(reg) \
+	readb(priv(instance)->base + ((reg) << 2))
+#define NCR5380_write(reg, value) \
+	writeb(value, priv(instance)->base + ((reg) << 2))
+
+#define NCR5380_dma_xfer_len(instance, cmd, phase)	(cmd->transfersize)
+
 #define NCR5380_queue_command		oakscsi_queue_command
 #define NCR5380_info			oakscsi_info
-#define NCR5380_show_info		oakscsi_show_info
 
 #define NCR5380_implementation_fields	\
 	void __iomem *base
@@ -103,7 +103,6 @@ printk("reading %p len %d\n", addr, len);
 
 static struct scsi_host_template oakscsi_template = {
 	.module			= THIS_MODULE,
-	.show_info		= oakscsi_show_info,
 	.name			= "Oak 16-bit SCSI",
 	.info			= oakscsi_info,
 	.queuecommand		= oakscsi_queue_command,
@@ -115,6 +114,8 @@ static struct scsi_host_template oakscsi_template = {
 	.cmd_per_lun		= 2,
 	.use_clustering		= DISABLE_CLUSTERING,
 	.proc_name		= "oakscsi",
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 
 static int oakscsi_probe(struct expansion_card *ec, const struct ecard_id *id)
@@ -142,15 +143,21 @@ static int oakscsi_probe(struct expansion_card *ec, const struct ecard_id *id)
 	host->irq = NO_IRQ;
 	host->n_io_port = 255;
 
-	NCR5380_init(host, 0);
+	ret = NCR5380_init(host, 0);
+	if (ret)
+		goto out_unmap;
+
+	NCR5380_maybe_reset_bus(host);
 
 	ret = scsi_add_host(host, &ec->dev);
 	if (ret)
-		goto out_unmap;
+		goto out_exit;
 
 	scsi_scan_host(host);
 	goto out;
 
+ out_exit:
+	NCR5380_exit(host);
  out_unmap:
 	iounmap(priv(host)->base);
  unreg:
diff --git a/drivers/scsi/atari_NCR5380.c b/drivers/scsi/atari_NCR5380.c
index db87ece6edb2..e65478651ca9 100644
--- a/drivers/scsi/atari_NCR5380.c
+++ b/drivers/scsi/atari_NCR5380.c
@@ -1,15 +1,15 @@
 /*
  * NCR 5380 generic driver routines.  These should make it *trivial*
- *	to implement 5380 SCSI drivers under Linux with a non-trantor
- *	architecture.
+ * to implement 5380 SCSI drivers under Linux with a non-trantor
+ * architecture.
  *
- *	Note that these routines also work with NR53c400 family chips.
+ * Note that these routines also work with NR53c400 family chips.
  *
  * Copyright 1993, Drew Eckhardt
- *	Visionary Computing
- *	(Unix and Linux consulting and custom programming)
- *	drew@colorado.edu
- *	+1 (303) 666-5836
+ * Visionary Computing
+ * (Unix and Linux consulting and custom programming)
+ * drew@colorado.edu
+ * +1 (303) 666-5836
  *
  * For more information, please consult
  *
@@ -24,84 +24,10 @@
  * 1+ (800) 334-5454
  */
 
-/*
- * ++roman: To port the 5380 driver to the Atari, I had to do some changes in
- * this file, too:
- *
- *  - Some of the debug statements were incorrect (undefined variables and the
- *    like). I fixed that.
- *
- *  - In information_transfer(), I think a #ifdef was wrong. Looking at the
- *    possible DMA transfer size should also happen for REAL_DMA. I added this
- *    in the #if statement.
- *
- *  - When using real DMA, information_transfer() should return in a DATAOUT
- *    phase after starting the DMA. It has nothing more to do.
- *
- *  - The interrupt service routine should run main after end of DMA, too (not
- *    only after RESELECTION interrupts). Additionally, it should _not_ test
- *    for more interrupts after running main, since a DMA process may have
- *    been started and interrupts are turned on now. The new int could happen
- *    inside the execution of NCR5380_intr(), leading to recursive
- *    calls.
- *
- *  - I've added a function merge_contiguous_buffers() that tries to
- *    merge scatter-gather buffers that are located at contiguous
- *    physical addresses and can be processed with the same DMA setup.
- *    Since most scatter-gather operations work on a page (4K) of
- *    4 buffers (1K), in more than 90% of all cases three interrupts and
- *    DMA setup actions are saved.
- *
- * - I've deleted all the stuff for AUTOPROBE_IRQ, REAL_DMA_POLL, PSEUDO_DMA
- *    and USLEEP, because these were messing up readability and will never be
- *    needed for Atari SCSI.
- *
- * - I've revised the NCR5380_main() calling scheme (relax the 'main_running'
- *   stuff), and 'main' is executed in a bottom half if awoken by an
- *   interrupt.
- *
- * - The code was quite cluttered up by "#if (NDEBUG & NDEBUG_*) printk..."
- *   constructs. In my eyes, this made the source rather unreadable, so I
- *   finally replaced that by the *_PRINTK() macros.
- *
- */
-
-/*
- * Further development / testing that should be done :
- * 1.  Test linked command handling code after Eric is ready with
- *     the high level code.
- */
+/* Ported to Atari by Roman Hodek and others. */
 
 /* Adapted for the sun3 by Sam Creasey. */
 
-#include <scsi/scsi_dbg.h>
-#include <scsi/scsi_transport_spi.h>
-
-#if (NDEBUG & NDEBUG_LISTS)
-#define LIST(x, y)						\
-	do {							\
-		printk("LINE:%d   Adding %p to %p\n",		\
-		       __LINE__, (void*)(x), (void*)(y));	\
-		if ((x) == (y))					\
-			udelay(5);				\
-	} while (0)
-#define REMOVE(w, x, y, z)					\
-	do {							\
-		printk("LINE:%d   Removing: %p->%p  %p->%p \n",	\
-		       __LINE__, (void*)(w), (void*)(x),	\
-		       (void*)(y), (void*)(z));			\
-		if ((x) == (y))					\
-			udelay(5);				\
-	} while (0)
-#else
-#define LIST(x,y)
-#define REMOVE(w,x,y,z)
-#endif
-
-#ifndef notyet
-#undef LINKED
-#endif
-
 /*
  * Design
  *
@@ -126,17 +52,7 @@
  * piece of hardware that requires you to sit in a loop polling for
  * the REQ signal as long as you are connected.  Some devices are
  * brain dead (ie, many TEXEL CD ROM drives) and won't disconnect
- * while doing long seek operations.
- *
- * The workaround for this is to keep track of devices that have
- * disconnected.  If the device hasn't disconnected, for commands that
- * should disconnect, we do something like
- *
- * while (!REQ is asserted) { sleep for N usecs; poll for M usecs }
- *
- * Some tweaking of N and M needs to be done.  An algorithm based
- * on "time to data" would give the best results as long as short time
- * to datas (ie, on the same track) were considered, however these
+ * while doing long seek operations. [...] These
  * broken devices are the exception rather than the rule and I'd rather
  * spend my time optimizing for the normal case.
  *
@@ -177,12 +93,10 @@
  *
  * These macros control options :
  * AUTOSENSE - if defined, REQUEST SENSE will be performed automatically
- *	for commands that return with a CHECK CONDITION status.
+ * for commands that return with a CHECK CONDITION status.
  *
  * DIFFERENTIAL - if defined, NCR53c81 chips will use external differential
- *	transceivers.
- *
- * LINKED - if defined, linked commands are supported.
+ * transceivers.
  *
  * REAL_DMA - if defined, REAL DMA is used during the data transfer phases.
  *
@@ -195,17 +109,17 @@
  * NCR5380_write(register, value) - write to the specific register
  *
  * NCR5380_implementation_fields  - additional fields needed for this
- *      specific implementation of the NCR5380
+ * specific implementation of the NCR5380
  *
  * Either real DMA *or* pseudo DMA may be implemented
  * REAL functions :
  * NCR5380_REAL_DMA should be defined if real DMA is to be used.
  * Note that the DMA setup functions should return the number of bytes
- *	that they were able to program the controller for.
+ * that they were able to program the controller for.
  *
  * Also note that generic i386/PC versions of these macros are
- *	available as NCR5380_i386_dma_write_setup,
- *	NCR5380_i386_dma_read_setup, and NCR5380_i386_dma_residual.
+ * available as NCR5380_i386_dma_write_setup,
+ * NCR5380_i386_dma_read_setup, and NCR5380_i386_dma_residual.
  *
  * NCR5380_dma_write_setup(instance, src, count) - initialize
  * NCR5380_dma_read_setup(instance, dst, count) - initialize
@@ -221,18 +135,8 @@
  * possible) function may be used.
  */
 
-/* Macros ease life... :-) */
-#define	SETUP_HOSTDATA(in)				\
-    struct NCR5380_hostdata *hostdata =			\
-	(struct NCR5380_hostdata *)(in)->hostdata
-#define	HOSTDATA(in) ((struct NCR5380_hostdata *)(in)->hostdata)
-
-#define	NEXT(cmd)		((struct scsi_cmnd *)(cmd)->host_scribble)
-#define	SET_NEXT(cmd,next)	((cmd)->host_scribble = (void *)(next))
-#define	NEXTADDR(cmd)		((struct scsi_cmnd **)&(cmd)->host_scribble)
-
-#define	HOSTNO		instance->host_no
-#define	H_NO(cmd)	(cmd)->device->host->host_no
+static int do_abort(struct Scsi_Host *);
+static void do_reset(struct Scsi_Host *);
 
 #ifdef SUPPORT_TAGS
 
@@ -251,9 +155,7 @@
  * cannot know it in advance :-( We just see a QUEUE_FULL status being
  * returned. So, in this case, the driver internal queue size assumption is
  * reduced to the number of active tags if QUEUE_FULL is returned by the
- * target. The command is returned to the mid-level, but with status changed
- * to BUSY, since --as I've seen-- the mid-level can't handle QUEUE_FULL
- * correctly.
+ * target.
  *
  * We're also not allowed running tagged commands as long as an untagged
  * command is active. And REQUEST SENSE commands after a contingent allegiance
@@ -304,7 +206,8 @@ static void __init init_tags(struct NCR5380_hostdata *hostdata)
 static int is_lun_busy(struct scsi_cmnd *cmd, int should_be_tagged)
 {
 	u8 lun = cmd->device->lun;
-	SETUP_HOSTDATA(cmd->device->host);
+	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
 	if (hostdata->busy[cmd->device->id] & (1 << lun))
 		return 1;
@@ -314,8 +217,8 @@ static int is_lun_busy(struct scsi_cmnd *cmd, int should_be_tagged)
 		return 0;
 	if (hostdata->TagAlloc[scmd_id(cmd)][lun].nr_allocated >=
 	    hostdata->TagAlloc[scmd_id(cmd)][lun].queue_size) {
-		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d: no free tags\n",
-			   H_NO(cmd), cmd->device->id, lun);
+		dsprintk(NDEBUG_TAGS, instance, "target %d lun %d: no free tags\n",
+		         scmd_id(cmd), lun);
 		return 1;
 	}
 	return 0;
@@ -330,7 +233,8 @@ static int is_lun_busy(struct scsi_cmnd *cmd, int should_be_tagged)
 static void cmd_get_tag(struct scsi_cmnd *cmd, int should_be_tagged)
 {
 	u8 lun = cmd->device->lun;
-	SETUP_HOSTDATA(cmd->device->host);
+	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
 	/* If we or the target don't support tagged queuing, allocate the LUN for
 	 * an untagged command.
@@ -340,18 +244,16 @@ static void cmd_get_tag(struct scsi_cmnd *cmd, int should_be_tagged)
 	    !cmd->device->tagged_supported) {
 		cmd->tag = TAG_NONE;
 		hostdata->busy[cmd->device->id] |= (1 << lun);
-		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d now allocated by untagged "
-			   "command\n", H_NO(cmd), cmd->device->id, lun);
+		dsprintk(NDEBUG_TAGS, instance, "target %d lun %d now allocated by untagged command\n",
+		         scmd_id(cmd), lun);
 	} else {
 		struct tag_alloc *ta = &hostdata->TagAlloc[scmd_id(cmd)][lun];
 
 		cmd->tag = find_first_zero_bit(ta->allocated, MAX_TAGS);
 		set_bit(cmd->tag, ta->allocated);
 		ta->nr_allocated++;
-		dprintk(NDEBUG_TAGS, "scsi%d: using tag %d for target %d lun %d "
-			   "(now %d tags in use)\n",
-			   H_NO(cmd), cmd->tag, cmd->device->id,
-			   lun, ta->nr_allocated);
+		dsprintk(NDEBUG_TAGS, instance, "using tag %d for target %d lun %d (%d tags allocated)\n",
+		         cmd->tag, scmd_id(cmd), lun, ta->nr_allocated);
 	}
 }
 
@@ -363,21 +265,22 @@ static void cmd_get_tag(struct scsi_cmnd *cmd, int should_be_tagged)
 static void cmd_free_tag(struct scsi_cmnd *cmd)
 {
 	u8 lun = cmd->device->lun;
-	SETUP_HOSTDATA(cmd->device->host);
+	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
 	if (cmd->tag == TAG_NONE) {
 		hostdata->busy[cmd->device->id] &= ~(1 << lun);
-		dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %d untagged cmd finished\n",
-			   H_NO(cmd), cmd->device->id, lun);
+		dsprintk(NDEBUG_TAGS, instance, "target %d lun %d untagged cmd freed\n",
+		         scmd_id(cmd), lun);
 	} else if (cmd->tag >= MAX_TAGS) {
-		printk(KERN_NOTICE "scsi%d: trying to free bad tag %d!\n",
-		       H_NO(cmd), cmd->tag);
+		shost_printk(KERN_NOTICE, instance,
+		             "trying to free bad tag %d!\n", cmd->tag);
 	} else {
 		struct tag_alloc *ta = &hostdata->TagAlloc[scmd_id(cmd)][lun];
 		clear_bit(cmd->tag, ta->allocated);
 		ta->nr_allocated--;
-		dprintk(NDEBUG_TAGS, "scsi%d: freed tag %d for target %d lun %d\n",
-			   H_NO(cmd), cmd->tag, cmd->device->id, lun);
+		dsprintk(NDEBUG_TAGS, instance, "freed tag %d for target %d lun %d\n",
+		         cmd->tag, scmd_id(cmd), lun);
 	}
 }
 
@@ -401,17 +304,15 @@ static void free_all_tags(struct NCR5380_hostdata *hostdata)
 
 #endif /* SUPPORT_TAGS */
 
-
-/*
- * Function: void merge_contiguous_buffers( struct scsi_cmnd *cmd )
- *
- * Purpose: Try to merge several scatter-gather requests into one DMA
- *    transfer. This is possible if the scatter buffers lie on
- *    physical contiguous addresses.
- *
- * Parameters: struct scsi_cmnd *cmd
- *    The command to work on. The first scatter buffer's data are
- *    assumed to be already transferred into ptr/this_residual.
+/**
+ * merge_contiguous_buffers - coalesce scatter-gather list entries
+ * @cmd: command requesting IO
+ *
+ * Try to merge several scatter-gather buffers into one DMA transfer.
+ * This is possible if the scatter buffers lie on physically
+ * contiguous addresses. The first scatter-gather buffer's data are
+ * assumed to be already transferred into cmd->SCp.this_residual.
+ * Every buffer merged avoids an interrupt and a DMA setup operation.
  */
 
 static void merge_contiguous_buffers(struct scsi_cmnd *cmd)
@@ -463,9 +364,7 @@ static inline void initialize_SCp(struct scsi_cmnd *cmd)
 		cmd->SCp.buffers_residual = scsi_sg_count(cmd) - 1;
 		cmd->SCp.ptr = sg_virt(cmd->SCp.buffer);
 		cmd->SCp.this_residual = cmd->SCp.buffer->length;
-		/* ++roman: Try to merge some scatter-buffers if they are at
-		 * contiguous physical addresses.
-		 */
+
 		merge_contiguous_buffers(cmd);
 	} else {
 		cmd->SCp.buffer = NULL;
@@ -473,31 +372,110 @@ static inline void initialize_SCp(struct scsi_cmnd *cmd)
 		cmd->SCp.ptr = NULL;
 		cmd->SCp.this_residual = 0;
 	}
+
+	cmd->SCp.Status = 0;
+	cmd->SCp.Message = 0;
+}
+
+/**
+ * NCR5380_poll_politely2 - wait for two chip register values
+ * @instance: controller to poll
+ * @reg1: 5380 register to poll
+ * @bit1: Bitmask to check
+ * @val1: Expected value
+ * @reg2: Second 5380 register to poll
+ * @bit2: Second bitmask to check
+ * @val2: Second expected value
+ * @wait: Time-out in jiffies
+ *
+ * Polls the chip in a reasonably efficient manner waiting for an
+ * event to occur. After a short quick poll we begin to yield the CPU
+ * (if possible). In irq contexts the time-out is arbitrarily limited.
+ * Callers may hold locks as long as they are held in irq mode.
+ *
+ * Returns 0 if either or both event(s) occurred otherwise -ETIMEDOUT.
+ */
+
+static int NCR5380_poll_politely2(struct Scsi_Host *instance,
+                                  int reg1, int bit1, int val1,
+                                  int reg2, int bit2, int val2, int wait)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	unsigned long deadline = jiffies + wait;
+	unsigned long n;
+
+	/* Busy-wait for up to 10 ms */
+	n = min(10000U, jiffies_to_usecs(wait));
+	n *= hostdata->accesses_per_ms;
+	n /= 2000;
+	do {
+		if ((NCR5380_read(reg1) & bit1) == val1)
+			return 0;
+		if ((NCR5380_read(reg2) & bit2) == val2)
+			return 0;
+		cpu_relax();
+	} while (n--);
+
+	if (irqs_disabled() || in_interrupt())
+		return -ETIMEDOUT;
+
+	/* Repeatedly sleep for 1 ms until deadline */
+	while (time_is_after_jiffies(deadline)) {
+		schedule_timeout_uninterruptible(1);
+		if ((NCR5380_read(reg1) & bit1) == val1)
+			return 0;
+		if ((NCR5380_read(reg2) & bit2) == val2)
+			return 0;
+	}
+
+	return -ETIMEDOUT;
 }
 
-#include <linux/delay.h>
+static inline int NCR5380_poll_politely(struct Scsi_Host *instance,
+                                        int reg, int bit, int val, int wait)
+{
+	return NCR5380_poll_politely2(instance, reg, bit, val,
+	                                        reg, bit, val, wait);
+}
 
 #if NDEBUG
 static struct {
 	unsigned char mask;
 	const char *name;
 } signals[] = {
-	{ SR_DBP, "PARITY"}, { SR_RST, "RST" }, { SR_BSY, "BSY" },
-	{ SR_REQ, "REQ" }, { SR_MSG, "MSG" }, { SR_CD,  "CD" }, { SR_IO, "IO" },
-	{ SR_SEL, "SEL" }, {0, NULL}
-}, basrs[] = {
-	{BASR_ATN, "ATN"}, {BASR_ACK, "ACK"}, {0, NULL}
-}, icrs[] = {
-	{ICR_ASSERT_RST, "ASSERT RST"},{ICR_ASSERT_ACK, "ASSERT ACK"},
-	{ICR_ASSERT_BSY, "ASSERT BSY"}, {ICR_ASSERT_SEL, "ASSERT SEL"},
-	{ICR_ASSERT_ATN, "ASSERT ATN"}, {ICR_ASSERT_DATA, "ASSERT DATA"},
+	{SR_DBP, "PARITY"},
+	{SR_RST, "RST"},
+	{SR_BSY, "BSY"},
+	{SR_REQ, "REQ"},
+	{SR_MSG, "MSG"},
+	{SR_CD, "CD"},
+	{SR_IO, "IO"},
+	{SR_SEL, "SEL"},
+	{0, NULL}
+},
+basrs[] = {
+	{BASR_ATN, "ATN"},
+	{BASR_ACK, "ACK"},
 	{0, NULL}
-}, mrs[] = {
-	{MR_BLOCK_DMA_MODE, "MODE BLOCK DMA"}, {MR_TARGET, "MODE TARGET"},
-	{MR_ENABLE_PAR_CHECK, "MODE PARITY CHECK"}, {MR_ENABLE_PAR_INTR,
-	"MODE PARITY INTR"}, {MR_ENABLE_EOP_INTR,"MODE EOP INTR"},
+},
+icrs[] = {
+	{ICR_ASSERT_RST, "ASSERT RST"},
+	{ICR_ASSERT_ACK, "ASSERT ACK"},
+	{ICR_ASSERT_BSY, "ASSERT BSY"},
+	{ICR_ASSERT_SEL, "ASSERT SEL"},
+	{ICR_ASSERT_ATN, "ASSERT ATN"},
+	{ICR_ASSERT_DATA, "ASSERT DATA"},
+	{0, NULL}
+},
+mrs[] = {
+	{MR_BLOCK_DMA_MODE, "MODE BLOCK DMA"},
+	{MR_TARGET, "MODE TARGET"},
+	{MR_ENABLE_PAR_CHECK, "MODE PARITY CHECK"},
+	{MR_ENABLE_PAR_INTR, "MODE PARITY INTR"},
+	{MR_ENABLE_EOP_INTR, "MODE EOP INTR"},
 	{MR_MONITOR_BSY, "MODE MONITOR BSY"},
-	{MR_DMA_MODE, "MODE DMA"}, {MR_ARBITRATE, "MODE ARBITRATION"},
+	{MR_DMA_MODE, "MODE DMA"},
+	{MR_ARBITRATE, "MODE ARBITRATION"},
 	{0, NULL}
 };
 
@@ -511,15 +489,13 @@ static struct {
 static void NCR5380_print(struct Scsi_Host *instance)
 {
 	unsigned char status, data, basr, mr, icr, i;
-	unsigned long flags;
 
-	local_irq_save(flags);
 	data = NCR5380_read(CURRENT_SCSI_DATA_REG);
 	status = NCR5380_read(STATUS_REG);
 	mr = NCR5380_read(MODE_REG);
 	icr = NCR5380_read(INITIATOR_COMMAND_REG);
 	basr = NCR5380_read(BUS_AND_STATUS_REG);
-	local_irq_restore(flags);
+
 	printk("STATUS_REG: %02x ", status);
 	for (i = 0; signals[i].mask; ++i)
 		if (status & signals[i].mask)
@@ -543,8 +519,12 @@ static struct {
 	unsigned char value;
 	const char *name;
 } phases[] = {
-	{PHASE_DATAOUT, "DATAOUT"}, {PHASE_DATAIN, "DATAIN"}, {PHASE_CMDOUT, "CMDOUT"},
-	{PHASE_STATIN, "STATIN"}, {PHASE_MSGOUT, "MSGOUT"}, {PHASE_MSGIN, "MSGIN"},
+	{PHASE_DATAOUT, "DATAOUT"},
+	{PHASE_DATAIN, "DATAIN"},
+	{PHASE_CMDOUT, "CMDOUT"},
+	{PHASE_STATIN, "STATIN"},
+	{PHASE_MSGOUT, "MSGOUT"},
+	{PHASE_MSGIN, "MSGIN"},
 	{PHASE_UNKNOWN, "UNKNOWN"}
 };
 
@@ -553,8 +533,6 @@ static struct {
  * @instance: adapter to dump
  *
  * Print the current SCSI phase for debugging purposes
- *
- * Locks: none
  */
 
 static void NCR5380_print_phase(struct Scsi_Host *instance)
@@ -564,54 +542,21 @@ static void NCR5380_print_phase(struct Scsi_Host *instance)
 
 	status = NCR5380_read(STATUS_REG);
 	if (!(status & SR_REQ))
-		printk(KERN_DEBUG "scsi%d: REQ not asserted, phase unknown.\n", HOSTNO);
+		shost_printk(KERN_DEBUG, instance, "REQ not asserted, phase unknown.\n");
 	else {
 		for (i = 0; (phases[i].value != PHASE_UNKNOWN) &&
 		     (phases[i].value != (status & PHASE_MASK)); ++i)
 			;
-		printk(KERN_DEBUG "scsi%d: phase %s\n", HOSTNO, phases[i].name);
+		shost_printk(KERN_DEBUG, instance, "phase %s\n", phases[i].name);
 	}
 }
-
 #endif
 
-/*
- * ++roman: New scheme of calling NCR5380_main()
- *
- * If we're not in an interrupt, we can call our main directly, it cannot be
- * already running. Else, we queue it on a task queue, if not 'main_running'
- * tells us that a lower level is already executing it. This way,
- * 'main_running' needs not be protected in a special way.
- *
- * queue_main() is a utility function for putting our main onto the task
- * queue, if main_running is false. It should be called only from a
- * interrupt or bottom half.
- */
-
-#include <linux/gfp.h>
-#include <linux/workqueue.h>
-#include <linux/interrupt.h>
-
-static inline void queue_main(struct NCR5380_hostdata *hostdata)
-{
-	if (!hostdata->main_running) {
-		/* If in interrupt and NCR5380_main() not already running,
-		   queue it on the 'immediate' task queue, to be processed
-		   immediately after the current interrupt processing has
-		   finished. */
-		schedule_work(&hostdata->main_task);
-	}
-	/* else: nothing to do: the running NCR5380_main() will pick up
-	   any newly queued command. */
-}
-
 /**
  * NCR58380_info - report driver and host information
  * @instance: relevant scsi host instance
  *
  * For use as the host template info() handler.
- *
- * Locks: none
  */
 
 static const char *NCR5380_info(struct Scsi_Host *instance)
@@ -630,13 +575,14 @@ static void prepare_info(struct Scsi_Host *instance)
 	         "base 0x%lx, irq %d, "
 	         "can_queue %d, cmd_per_lun %d, "
 	         "sg_tablesize %d, this_id %d, "
-	         "flags { %s}, "
+	         "flags { %s%s}, "
 	         "options { %s} ",
 	         instance->hostt->name, instance->io_port, instance->n_io_port,
 	         instance->base, instance->irq,
 	         instance->can_queue, instance->cmd_per_lun,
 	         instance->sg_tablesize, instance->this_id,
 	         hostdata->flags & FLAG_TAGGED_QUEUING ? "TAGGED_QUEUING " : "",
+	         hostdata->flags & FLAG_TOSHIBA_DELAY  ? "TOSHIBA_DELAY "  : "",
 #ifdef DIFFERENTIAL
 	         "DIFFERENTIAL "
 #endif
@@ -653,102 +599,6 @@ static void prepare_info(struct Scsi_Host *instance)
 }
 
 /**
- * NCR5380_print_status - dump controller info
- * @instance: controller to dump
- *
- * Print commands in the various queues, called from NCR5380_abort
- * to aid debugging.
- */
-
-static void lprint_Scsi_Cmnd(struct scsi_cmnd *cmd)
-{
-	int i, s;
-	unsigned char *command;
-	printk("scsi%d: destination target %d, lun %llu\n",
-		H_NO(cmd), cmd->device->id, cmd->device->lun);
-	printk(KERN_CONT "        command = ");
-	command = cmd->cmnd;
-	printk(KERN_CONT "%2d (0x%02x)", command[0], command[0]);
-	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
-		printk(KERN_CONT " %02x", command[i]);
-	printk("\n");
-}
-
-static void NCR5380_print_status(struct Scsi_Host *instance)
-{
-	struct NCR5380_hostdata *hostdata;
-	struct scsi_cmnd *ptr;
-	unsigned long flags;
-
-	NCR5380_dprint(NDEBUG_ANY, instance);
-	NCR5380_dprint_phase(NDEBUG_ANY, instance);
-
-	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
-
-	local_irq_save(flags);
-	printk("NCR5380: coroutine is%s running.\n",
-		hostdata->main_running ? "" : "n't");
-	if (!hostdata->connected)
-		printk("scsi%d: no currently connected command\n", HOSTNO);
-	else
-		lprint_Scsi_Cmnd((struct scsi_cmnd *) hostdata->connected);
-	printk("scsi%d: issue_queue\n", HOSTNO);
-	for (ptr = (struct scsi_cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
-		lprint_Scsi_Cmnd(ptr);
-
-	printk("scsi%d: disconnected_queue\n", HOSTNO);
-	for (ptr = (struct scsi_cmnd *) hostdata->disconnected_queue; ptr;
-	     ptr = NEXT(ptr))
-		lprint_Scsi_Cmnd(ptr);
-
-	local_irq_restore(flags);
-	printk("\n");
-}
-
-static void show_Scsi_Cmnd(struct scsi_cmnd *cmd, struct seq_file *m)
-{
-	int i, s;
-	unsigned char *command;
-	seq_printf(m, "scsi%d: destination target %d, lun %llu\n",
-		H_NO(cmd), cmd->device->id, cmd->device->lun);
-	seq_puts(m, "        command = ");
-	command = cmd->cmnd;
-	seq_printf(m, "%2d (0x%02x)", command[0], command[0]);
-	for (i = 1, s = COMMAND_SIZE(command[0]); i < s; ++i)
-		seq_printf(m, " %02x", command[i]);
-	seq_putc(m, '\n');
-}
-
-static int __maybe_unused NCR5380_show_info(struct seq_file *m,
-                                            struct Scsi_Host *instance)
-{
-	struct NCR5380_hostdata *hostdata;
-	struct scsi_cmnd *ptr;
-	unsigned long flags;
-
-	hostdata = (struct NCR5380_hostdata *)instance->hostdata;
-
-	local_irq_save(flags);
-	seq_printf(m, "NCR5380: coroutine is%s running.\n",
-		hostdata->main_running ? "" : "n't");
-	if (!hostdata->connected)
-		seq_printf(m, "scsi%d: no currently connected command\n", HOSTNO);
-	else
-		show_Scsi_Cmnd((struct scsi_cmnd *) hostdata->connected, m);
-	seq_printf(m, "scsi%d: issue_queue\n", HOSTNO);
-	for (ptr = (struct scsi_cmnd *)hostdata->issue_queue; ptr; ptr = NEXT(ptr))
-		show_Scsi_Cmnd(ptr, m);
-
-	seq_printf(m, "scsi%d: disconnected_queue\n", HOSTNO);
-	for (ptr = (struct scsi_cmnd *) hostdata->disconnected_queue; ptr;
-	     ptr = NEXT(ptr))
-		show_Scsi_Cmnd(ptr, m);
-
-	local_irq_restore(flags);
-	return 0;
-}
-
-/**
  * NCR5380_init - initialise an NCR5380
  * @instance: adapter to configure
  * @flags: control flags
@@ -764,11 +614,11 @@ static int __maybe_unused NCR5380_show_info(struct seq_file *m,
 
 static int __init NCR5380_init(struct Scsi_Host *instance, int flags)
 {
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int i;
-	SETUP_HOSTDATA(instance);
+	unsigned long deadline;
 
 	hostdata->host = instance;
-	hostdata->aborted = 0;
 	hostdata->id_mask = 1 << instance->this_id;
 	hostdata->id_higher_mask = 0;
 	for (i = hostdata->id_mask; i <= 0x80; i <<= 1)
@@ -782,13 +632,21 @@ static int __init NCR5380_init(struct Scsi_Host *instance, int flags)
 #if defined (REAL_DMA)
 	hostdata->dma_len = 0;
 #endif
-	hostdata->targets_present = 0;
+	spin_lock_init(&hostdata->lock);
 	hostdata->connected = NULL;
-	hostdata->issue_queue = NULL;
-	hostdata->disconnected_queue = NULL;
+	hostdata->sensing = NULL;
+	INIT_LIST_HEAD(&hostdata->autosense);
+	INIT_LIST_HEAD(&hostdata->unissued);
+	INIT_LIST_HEAD(&hostdata->disconnected);
+
 	hostdata->flags = flags;
 
 	INIT_WORK(&hostdata->main_task, NCR5380_main);
+	hostdata->work_q = alloc_workqueue("ncr5380_%d",
+	                        WQ_UNBOUND | WQ_MEM_RECLAIM,
+	                        1, instance->host_no);
+	if (!hostdata->work_q)
+		return -ENOMEM;
 
 	prepare_info(instance);
 
@@ -797,6 +655,72 @@ static int __init NCR5380_init(struct Scsi_Host *instance, int flags)
 	NCR5380_write(TARGET_COMMAND_REG, 0);
 	NCR5380_write(SELECT_ENABLE_REG, 0);
 
+	/* Calibrate register polling loop */
+	i = 0;
+	deadline = jiffies + 1;
+	do {
+		cpu_relax();
+	} while (time_is_after_jiffies(deadline));
+	deadline += msecs_to_jiffies(256);
+	do {
+		NCR5380_read(STATUS_REG);
+		++i;
+		cpu_relax();
+	} while (time_is_after_jiffies(deadline));
+	hostdata->accesses_per_ms = i / 256;
+
+	return 0;
+}
+
+/**
+ * NCR5380_maybe_reset_bus - Detect and correct bus wedge problems.
+ * @instance: adapter to check
+ *
+ * If the system crashed, it may have crashed with a connected target and
+ * the SCSI bus busy. Check for BUS FREE phase. If not, try to abort the
+ * currently established nexus, which we know nothing about. Failing that
+ * do a bus reset.
+ *
+ * Note that a bus reset will cause the chip to assert IRQ.
+ *
+ * Returns 0 if successful, otherwise -ENXIO.
+ */
+
+static int NCR5380_maybe_reset_bus(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int pass;
+
+	for (pass = 1; (NCR5380_read(STATUS_REG) & SR_BSY) && pass <= 6; ++pass) {
+		switch (pass) {
+		case 1:
+		case 3:
+		case 5:
+			shost_printk(KERN_ERR, instance, "SCSI bus busy, waiting up to five seconds\n");
+			NCR5380_poll_politely(instance,
+			                      STATUS_REG, SR_BSY, 0, 5 * HZ);
+			break;
+		case 2:
+			shost_printk(KERN_ERR, instance, "bus busy, attempting abort\n");
+			do_abort(instance);
+			break;
+		case 4:
+			shost_printk(KERN_ERR, instance, "bus busy, attempting reset\n");
+			do_reset(instance);
+			/* Wait after a reset; the SCSI standard calls for
+			 * 250ms, we wait 500ms to be on the safe side.
+			 * But some Toshiba CD-ROMs need ten times that.
+			 */
+			if (hostdata->flags & FLAG_TOSHIBA_DELAY)
+				msleep(2500);
+			else
+				msleep(500);
+			break;
+		case 6:
+			shost_printk(KERN_ERR, instance, "bus locked solid\n");
+			return -ENXIO;
+		}
+	}
 	return 0;
 }
 
@@ -812,6 +736,38 @@ static void NCR5380_exit(struct Scsi_Host *instance)
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
 	cancel_work_sync(&hostdata->main_task);
+	destroy_workqueue(hostdata->work_q);
+}
+
+/**
+ * complete_cmd - finish processing a command and return it to the SCSI ML
+ * @instance: the host instance
+ * @cmd: command to complete
+ */
+
+static void complete_cmd(struct Scsi_Host *instance,
+                         struct scsi_cmnd *cmd)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+
+	dsprintk(NDEBUG_QUEUES, instance, "complete_cmd: cmd %p\n", cmd);
+
+	if (hostdata->sensing == cmd) {
+		/* Autosense processing ends here */
+		if ((cmd->result & 0xff) != SAM_STAT_GOOD) {
+			scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+			set_host_byte(cmd, DID_ERROR);
+		} else
+			scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+		hostdata->sensing = NULL;
+	}
+
+#ifdef SUPPORT_TAGS
+	cmd_free_tag(cmd);
+#else
+	hostdata->busy[scmd_id(cmd)] &= ~(1 << cmd->device->lun);
+#endif
+	cmd->scsi_done(cmd);
 }
 
 /**
@@ -819,7 +775,7 @@ static void NCR5380_exit(struct Scsi_Host *instance)
  * @instance: the relevant SCSI adapter
  * @cmd: SCSI command
  *
- * cmd is added to the per instance issue_queue, with minor
+ * cmd is added to the per-instance issue queue, with minor
  * twiddling done to the host specific fields of cmd.  If the
  * main coroutine is not running, it is restarted.
  */
@@ -828,44 +784,23 @@ static int NCR5380_queue_command(struct Scsi_Host *instance,
                                  struct scsi_cmnd *cmd)
 {
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
-	struct scsi_cmnd *tmp;
+	struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
 	unsigned long flags;
 
 #if (NDEBUG & NDEBUG_NO_WRITE)
 	switch (cmd->cmnd[0]) {
 	case WRITE_6:
 	case WRITE_10:
-		printk(KERN_NOTICE "scsi%d: WRITE attempted with NO_WRITE debugging flag set\n",
-		       H_NO(cmd));
+		shost_printk(KERN_DEBUG, instance, "WRITE attempted with NDEBUG_NO_WRITE set\n");
 		cmd->result = (DID_ERROR << 16);
 		cmd->scsi_done(cmd);
 		return 0;
 	}
 #endif /* (NDEBUG & NDEBUG_NO_WRITE) */
 
-	/*
-	 * We use the host_scribble field as a pointer to the next command
-	 * in a queue
-	 */
-
-	SET_NEXT(cmd, NULL);
 	cmd->result = 0;
 
 	/*
-	 * Insert the cmd into the issue queue. Note that REQUEST SENSE
-	 * commands are added to the head of the queue since any command will
-	 * clear the contingent allegiance condition that exists and the
-	 * sense data is only guaranteed to be valid while the condition exists.
-	 */
-
-	/* ++guenther: now that the issue queue is being set up, we can lock ST-DMA.
-	 * Otherwise a running NCR5380_main may steal the lock.
-	 * Lock before actually inserting due to fairness reasons explained in
-	 * atari_scsi.c. If we insert first, then it's impossible for this driver
-	 * to release the lock.
-	 * Stop timer for this command while waiting for the lock, or timeouts
-	 * may happen (and they really do), and it's no good if the command doesn't
-	 * appear in any of the queues.
 	 * ++roman: Just disabling the NCR interrupt isn't sufficient here,
 	 * because also a timer int can trigger an abort or reset, which would
 	 * alter queues and touch the lock.
@@ -873,7 +808,7 @@ static int NCR5380_queue_command(struct Scsi_Host *instance,
 	if (!NCR5380_acquire_dma_irq(instance))
 		return SCSI_MLQUEUE_HOST_BUSY;
 
-	local_irq_save(flags);
+	spin_lock_irqsave(&hostdata->lock, flags);
 
 	/*
 	 * Insert the cmd into the issue queue. Note that REQUEST SENSE
@@ -882,33 +817,18 @@ static int NCR5380_queue_command(struct Scsi_Host *instance,
 	 * sense data is only guaranteed to be valid while the condition exists.
 	 */
 
-	if (!(hostdata->issue_queue) || (cmd->cmnd[0] == REQUEST_SENSE)) {
-		LIST(cmd, hostdata->issue_queue);
-		SET_NEXT(cmd, hostdata->issue_queue);
-		hostdata->issue_queue = cmd;
-	} else {
-		for (tmp = (struct scsi_cmnd *)hostdata->issue_queue;
-		     NEXT(tmp); tmp = NEXT(tmp))
-			;
-		LIST(cmd, tmp);
-		SET_NEXT(tmp, cmd);
-	}
-	local_irq_restore(flags);
+	if (cmd->cmnd[0] == REQUEST_SENSE)
+		list_add(&ncmd->list, &hostdata->unissued);
+	else
+		list_add_tail(&ncmd->list, &hostdata->unissued);
 
-	dprintk(NDEBUG_QUEUES, "scsi%d: command added to %s of queue\n", H_NO(cmd),
-		  (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
+	spin_unlock_irqrestore(&hostdata->lock, flags);
 
-	/* If queue_command() is called from an interrupt (real one or bottom
-	 * half), we let queue_main() do the job of taking care about main. If it
-	 * is already running, this is a no-op, else main will be queued.
-	 *
-	 * If we're not in an interrupt, we can call NCR5380_main()
-	 * unconditionally, because it cannot be already running.
-	 */
-	if (in_interrupt() || irqs_disabled())
-		queue_main(hostdata);
-	else
-		NCR5380_main(&hostdata->main_task);
+	dsprintk(NDEBUG_QUEUES, instance, "command %p added to %s of queue\n",
+	         cmd, (cmd->cmnd[0] == REQUEST_SENSE) ? "head" : "tail");
+
+	/* Kick off command processing */
+	queue_work(hostdata->work_q, &hostdata->main_task);
 	return 0;
 }
 
@@ -917,22 +837,85 @@ static inline void maybe_release_dma_irq(struct Scsi_Host *instance)
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 
 	/* Caller does the locking needed to set & test these data atomically */
-	if (!hostdata->disconnected_queue &&
-	    !hostdata->issue_queue &&
+	if (list_empty(&hostdata->disconnected) &&
+	    list_empty(&hostdata->unissued) &&
+	    list_empty(&hostdata->autosense) &&
 	    !hostdata->connected &&
-	    !hostdata->retain_dma_intr)
+	    !hostdata->selecting)
 		NCR5380_release_dma_irq(instance);
 }
 
 /**
+ * dequeue_next_cmd - dequeue a command for processing
+ * @instance: the scsi host instance
+ *
+ * Priority is given to commands on the autosense queue. These commands
+ * need autosense because of a CHECK CONDITION result.
+ *
+ * Returns a command pointer if a command is found for a target that is
+ * not already busy. Otherwise returns NULL.
+ */
+
+static struct scsi_cmnd *dequeue_next_cmd(struct Scsi_Host *instance)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	struct NCR5380_cmd *ncmd;
+	struct scsi_cmnd *cmd;
+
+	if (list_empty(&hostdata->autosense)) {
+		list_for_each_entry(ncmd, &hostdata->unissued, list) {
+			cmd = NCR5380_to_scmd(ncmd);
+			dsprintk(NDEBUG_QUEUES, instance, "dequeue: cmd=%p target=%d busy=0x%02x lun=%llu\n",
+			         cmd, scmd_id(cmd), hostdata->busy[scmd_id(cmd)], cmd->device->lun);
+
+			if (
+#ifdef SUPPORT_TAGS
+			    !is_lun_busy(cmd, 1)
+#else
+			    !(hostdata->busy[scmd_id(cmd)] & (1 << cmd->device->lun))
+#endif
+			) {
+				list_del(&ncmd->list);
+				dsprintk(NDEBUG_QUEUES, instance,
+				         "dequeue: removed %p from issue queue\n", cmd);
+				return cmd;
+			}
+		}
+	} else {
+		/* Autosense processing begins here */
+		ncmd = list_first_entry(&hostdata->autosense,
+		                        struct NCR5380_cmd, list);
+		list_del(&ncmd->list);
+		cmd = NCR5380_to_scmd(ncmd);
+		dsprintk(NDEBUG_QUEUES, instance,
+		         "dequeue: removed %p from autosense queue\n", cmd);
+		scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
+		hostdata->sensing = cmd;
+		return cmd;
+	}
+	return NULL;
+}
+
+static void requeue_cmd(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
+
+	if (hostdata->sensing) {
+		scsi_eh_restore_cmnd(cmd, &hostdata->ses);
+		list_add(&ncmd->list, &hostdata->autosense);
+		hostdata->sensing = NULL;
+	} else
+		list_add(&ncmd->list, &hostdata->unissued);
+}
+
+/**
  * NCR5380_main - NCR state machines
  *
  * NCR5380_main is a coroutine that runs as long as more work can
  * be done on the NCR5380 host adapters in a system.  Both
  * NCR5380_queue_command() and NCR5380_intr() will try to start it
  * in case it is not running.
- *
- * Locks: called as its own thread with no locks held.
  */
 
 static void NCR5380_main(struct work_struct *work)
@@ -940,154 +923,69 @@ static void NCR5380_main(struct work_struct *work)
 	struct NCR5380_hostdata *hostdata =
 		container_of(work, struct NCR5380_hostdata, main_task);
 	struct Scsi_Host *instance = hostdata->host;
-	struct scsi_cmnd *tmp, *prev;
+	struct scsi_cmnd *cmd;
 	int done;
-	unsigned long flags;
 
 	/*
-	 * We run (with interrupts disabled) until we're sure that none of
-	 * the host adapters have anything that can be done, at which point
-	 * we set main_running to 0 and exit.
-	 *
-	 * Interrupts are enabled before doing various other internal
-	 * instructions, after we've decided that we need to run through
-	 * the loop again.
-	 *
-	 * this should prevent any race conditions.
-	 *
 	 * ++roman: Just disabling the NCR interrupt isn't sufficient here,
 	 * because also a timer int can trigger an abort or reset, which can
 	 * alter queues and touch the Falcon lock.
 	 */
 
-	/* Tell int handlers main() is now already executing.  Note that
-	   no races are possible here. If an int comes in before
-	   'main_running' is set here, and queues/executes main via the
-	   task queue, it doesn't do any harm, just this instance of main
-	   won't find any work left to do. */
-	if (hostdata->main_running)
-		return;
-	hostdata->main_running = 1;
-
-	local_save_flags(flags);
 	do {
-		local_irq_disable();	/* Freeze request queues */
 		done = 1;
 
-		if (!hostdata->connected) {
-			dprintk(NDEBUG_MAIN, "scsi%d: not connected\n", HOSTNO);
-			/*
-			 * Search through the issue_queue for a command destined
-			 * for a target that's not busy.
-			 */
-#if (NDEBUG & NDEBUG_LISTS)
-			for (tmp = (struct scsi_cmnd *) hostdata->issue_queue, prev = NULL;
-			     tmp && (tmp != prev); prev = tmp, tmp = NEXT(tmp))
-				;
-			/*printk("%p  ", tmp);*/
-			if ((tmp == prev) && tmp)
-				printk(" LOOP\n");
-			/* else printk("\n"); */
-#endif
-			for (tmp = (struct scsi_cmnd *) hostdata->issue_queue,
-			     prev = NULL; tmp; prev = tmp, tmp = NEXT(tmp)) {
-				u8 lun = tmp->device->lun;
-
-				dprintk(NDEBUG_LISTS,
-				        "MAIN tmp=%p target=%d busy=%d lun=%d\n",
-				        tmp, scmd_id(tmp), hostdata->busy[scmd_id(tmp)],
-				        lun);
-				/*  When we find one, remove it from the issue queue. */
-				/* ++guenther: possible race with Falcon locking */
-				if (
-#ifdef SUPPORT_TAGS
-				    !is_lun_busy( tmp, tmp->cmnd[0] != REQUEST_SENSE)
-#else
-				    !(hostdata->busy[tmp->device->id] & (1 << lun))
-#endif
-				    ) {
-					/* ++guenther: just to be sure, this must be atomic */
-					local_irq_disable();
-					if (prev) {
-						REMOVE(prev, NEXT(prev), tmp, NEXT(tmp));
-						SET_NEXT(prev, NEXT(tmp));
-					} else {
-						REMOVE(-1, hostdata->issue_queue, tmp, NEXT(tmp));
-						hostdata->issue_queue = NEXT(tmp);
-					}
-					SET_NEXT(tmp, NULL);
-					hostdata->retain_dma_intr++;
+		spin_lock_irq(&hostdata->lock);
+		while (!hostdata->connected &&
+		       (cmd = dequeue_next_cmd(instance))) {
 
-					/* reenable interrupts after finding one */
-					local_irq_restore(flags);
+			dsprintk(NDEBUG_MAIN, instance, "main: dequeued %p\n", cmd);
 
-					/*
-					 * Attempt to establish an I_T_L nexus here.
-					 * On success, instance->hostdata->connected is set.
-					 * On failure, we must add the command back to the
-					 *   issue queue so we can keep trying.
-					 */
-					dprintk(NDEBUG_MAIN, "scsi%d: main(): command for target %d "
-						    "lun %d removed from issue_queue\n",
-						    HOSTNO, tmp->device->id, lun);
-					/*
-					 * REQUEST SENSE commands are issued without tagged
-					 * queueing, even on SCSI-II devices because the
-					 * contingent allegiance condition exists for the
-					 * entire unit.
-					 */
-					/* ++roman: ...and the standard also requires that
-					 * REQUEST SENSE command are untagged.
-					 */
+			/*
+			 * Attempt to establish an I_T_L nexus here.
+			 * On success, instance->hostdata->connected is set.
+			 * On failure, we must add the command back to the
+			 * issue queue so we can keep trying.
+			 */
+			/*
+			 * REQUEST SENSE commands are issued without tagged
+			 * queueing, even on SCSI-II devices because the
+			 * contingent allegiance condition exists for the
+			 * entire unit.
+			 */
+			/* ++roman: ...and the standard also requires that
+			 * REQUEST SENSE command are untagged.
+			 */
 
 #ifdef SUPPORT_TAGS
-					cmd_get_tag(tmp, tmp->cmnd[0] != REQUEST_SENSE);
+			cmd_get_tag(cmd, cmd->cmnd[0] != REQUEST_SENSE);
 #endif
-					if (!NCR5380_select(instance, tmp)) {
-						local_irq_disable();
-						hostdata->retain_dma_intr--;
-						/* release if target did not response! */
-						maybe_release_dma_irq(instance);
-						local_irq_restore(flags);
-						break;
-					} else {
-						local_irq_disable();
-						LIST(tmp, hostdata->issue_queue);
-						SET_NEXT(tmp, hostdata->issue_queue);
-						hostdata->issue_queue = tmp;
+			cmd = NCR5380_select(instance, cmd);
+			if (!cmd) {
+				dsprintk(NDEBUG_MAIN, instance, "main: select complete\n");
+				maybe_release_dma_irq(instance);
+			} else {
+				dsprintk(NDEBUG_MAIN | NDEBUG_QUEUES, instance,
+				         "main: select failed, returning %p to queue\n", cmd);
+				requeue_cmd(instance, cmd);
 #ifdef SUPPORT_TAGS
-						cmd_free_tag(tmp);
+				cmd_free_tag(cmd);
 #endif
-						hostdata->retain_dma_intr--;
-						local_irq_restore(flags);
-						dprintk(NDEBUG_MAIN, "scsi%d: main(): select() failed, "
-							    "returned to issue_queue\n", HOSTNO);
-						if (hostdata->connected)
-							break;
-					}
-				} /* if target/lun/target queue is not busy */
-			} /* for issue_queue */
-		} /* if (!hostdata->connected) */
-
+			}
+		}
 		if (hostdata->connected
 #ifdef REAL_DMA
 		    && !hostdata->dma_len
 #endif
 		    ) {
-			local_irq_restore(flags);
-			dprintk(NDEBUG_MAIN, "scsi%d: main: performing information transfer\n",
-				    HOSTNO);
+			dsprintk(NDEBUG_MAIN, instance, "main: performing information transfer\n");
 			NCR5380_information_transfer(instance);
-			dprintk(NDEBUG_MAIN, "scsi%d: main: done set false\n", HOSTNO);
 			done = 0;
 		}
+		spin_unlock_irq(&hostdata->lock);
+		if (!done)
+			cond_resched();
 	} while (!done);
-
-	/* Better allow ints _after_ 'main_running' has been cleared, else
-	   an interrupt could believe we'll pick up the work it left for
-	   us, but we won't see it anymore here... */
-	hostdata->main_running = 0;
-	local_irq_restore(flags);
 }
 
 
@@ -1096,27 +994,20 @@ static void NCR5380_main(struct work_struct *work)
  * Function : void NCR5380_dma_complete (struct Scsi_Host *instance)
  *
  * Purpose : Called by interrupt handler when DMA finishes or a phase
- *	mismatch occurs (which would finish the DMA transfer).
+ * mismatch occurs (which would finish the DMA transfer).
  *
  * Inputs : instance - this instance of the NCR5380.
- *
  */
 
 static void NCR5380_dma_complete(struct Scsi_Host *instance)
 {
-	SETUP_HOSTDATA(instance);
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int transferred;
 	unsigned char **data;
-	volatile int *count;
+	int *count;
 	int saved_data = 0, overrun = 0;
 	unsigned char p;
 
-	if (!hostdata->connected) {
-		printk(KERN_WARNING "scsi%d: received end of DMA interrupt with "
-		       "no connected cmd\n", HOSTNO);
-		return;
-	}
-
 	if (hostdata->read_overruns) {
 		p = hostdata->connected->SCp.phase;
 		if (p & SR_IO) {
@@ -1126,15 +1017,11 @@ static void NCR5380_dma_complete(struct Scsi_Host *instance)
 			    (BASR_PHASE_MATCH|BASR_ACK)) {
 				saved_data = NCR5380_read(INPUT_DATA_REG);
 				overrun = 1;
-				dprintk(NDEBUG_DMA, "scsi%d: read overrun handled\n", HOSTNO);
+				dsprintk(NDEBUG_DMA, instance, "read overrun handled\n");
 			}
 		}
 	}
 
-	dprintk(NDEBUG_DMA, "scsi%d: real DMA transfer complete, basr 0x%X, sr 0x%X\n",
-		   HOSTNO, NCR5380_read(BUS_AND_STATUS_REG),
-		   NCR5380_read(STATUS_REG));
-
 #if defined(CONFIG_SUN3)
 	if ((sun3scsi_dma_finish(rq_data_dir(hostdata->connected->request)))) {
 		pr_err("scsi%d: overrun in UDC counter -- not prepared to deal with this!\n",
@@ -1153,9 +1040,9 @@ static void NCR5380_dma_complete(struct Scsi_Host *instance)
 	}
 #endif
 
-	(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 
 	transferred = hostdata->dma_len - NCR5380_dma_residual(instance);
 	hostdata->dma_len = 0;
@@ -1194,140 +1081,160 @@ static void NCR5380_dma_complete(struct Scsi_Host *instance)
  * Handle interrupts, reestablishing I_T_L or I_T_L_Q nexuses
  * from the disconnected queue, and restarting NCR5380_main()
  * as required.
+ *
+ * The chip can assert IRQ in any of six different conditions. The IRQ flag
+ * is then cleared by reading the Reset Parity/Interrupt Register (RPIR).
+ * Three of these six conditions are latched in the Bus and Status Register:
+ * - End of DMA (cleared by ending DMA Mode)
+ * - Parity error (cleared by reading RPIR)
+ * - Loss of BSY (cleared by reading RPIR)
+ * Two conditions have flag bits that are not latched:
+ * - Bus phase mismatch (non-maskable in DMA Mode, cleared by ending DMA Mode)
+ * - Bus reset (non-maskable)
+ * The remaining condition has no flag bit at all:
+ * - Selection/reselection
+ *
+ * Hence, establishing the cause(s) of any interrupt is partly guesswork.
+ * In "The DP8490 and DP5380 Comparison Guide", National Semiconductor
+ * claimed that "the design of the [DP8490] interrupt logic ensures
+ * interrupts will not be lost (they can be on the DP5380)."
+ * The L5380/53C80 datasheet from LOGIC Devices has more details.
+ *
+ * Checking for bus reset by reading RST is futile because of interrupt
+ * latency, but a bus reset will reset chip logic. Checking for parity error
+ * is unnecessary because that interrupt is never enabled. A Loss of BSY
+ * condition will clear DMA Mode. We can tell when this occurs because the
+ * the Busy Monitor interrupt is enabled together with DMA Mode.
  */
 
 static irqreturn_t NCR5380_intr(int irq, void *dev_id)
 {
 	struct Scsi_Host *instance = dev_id;
-	int done = 1, handled = 0;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	int handled = 0;
 	unsigned char basr;
+	unsigned long flags;
 
-	dprintk(NDEBUG_INTR, "scsi%d: NCR5380 irq triggered\n", HOSTNO);
+	spin_lock_irqsave(&hostdata->lock, flags);
 
-	/* Look for pending interrupts */
 	basr = NCR5380_read(BUS_AND_STATUS_REG);
-	dprintk(NDEBUG_INTR, "scsi%d: BASR=%02x\n", HOSTNO, basr);
-	/* dispatch to appropriate routine if found and done=0 */
 	if (basr & BASR_IRQ) {
-		NCR5380_dprint(NDEBUG_INTR, instance);
-		if ((NCR5380_read(STATUS_REG) & (SR_SEL|SR_IO)) == (SR_SEL|SR_IO)) {
-			done = 0;
-			dprintk(NDEBUG_INTR, "scsi%d: SEL interrupt\n", HOSTNO);
-			NCR5380_reselect(instance);
-			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-		} else if (basr & BASR_PARITY_ERROR) {
-			dprintk(NDEBUG_INTR, "scsi%d: PARITY interrupt\n", HOSTNO);
-			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-		} else if ((NCR5380_read(STATUS_REG) & SR_RST) == SR_RST) {
-			dprintk(NDEBUG_INTR, "scsi%d: RESET interrupt\n", HOSTNO);
-			(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-		} else {
-			/*
-			 * The rest of the interrupt conditions can occur only during a
-			 * DMA transfer
-			 */
+		unsigned char mr = NCR5380_read(MODE_REG);
+		unsigned char sr = NCR5380_read(STATUS_REG);
+
+		dsprintk(NDEBUG_INTR, instance, "IRQ %d, BASR 0x%02x, SR 0x%02x, MR 0x%02x\n",
+		         irq, basr, sr, mr);
 
 #if defined(REAL_DMA)
-			/*
-			 * We should only get PHASE MISMATCH and EOP interrupts if we have
-			 * DMA enabled, so do a sanity check based on the current setting
-			 * of the MODE register.
+		if ((mr & MR_DMA_MODE) || (mr & MR_MONITOR_BSY)) {
+			/* Probably End of DMA, Phase Mismatch or Loss of BSY.
+			 * We ack IRQ after clearing Mode Register. Workarounds
+			 * for End of DMA errata need to happen in DMA Mode.
 			 */
 
-			if ((NCR5380_read(MODE_REG) & MR_DMA_MODE) &&
-			    ((basr & BASR_END_DMA_TRANSFER) ||
-			     !(basr & BASR_PHASE_MATCH))) {
+			dsprintk(NDEBUG_INTR, instance, "interrupt in DMA mode\n");
 
-				dprintk(NDEBUG_INTR, "scsi%d: PHASE MISM or EOP interrupt\n", HOSTNO);
-				NCR5380_dma_complete( instance );
-				done = 0;
-			} else
+			if (hostdata->connected) {
+				NCR5380_dma_complete(instance);
+				queue_work(hostdata->work_q, &hostdata->main_task);
+			} else {
+				NCR5380_write(MODE_REG, MR_BASE);
+				NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+			}
+		} else
 #endif /* REAL_DMA */
-			{
-/* MS: Ignore unknown phase mismatch interrupts (caused by EOP interrupt) */
-				if (basr & BASR_PHASE_MATCH)
-					dprintk(NDEBUG_INTR, "scsi%d: unknown interrupt, "
-					       "BASR 0x%x, MR 0x%x, SR 0x%x\n",
-					       HOSTNO, basr, NCR5380_read(MODE_REG),
-					       NCR5380_read(STATUS_REG));
-				(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+		if ((NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_mask) &&
+		    (sr & (SR_SEL | SR_IO | SR_BSY | SR_RST)) == (SR_SEL | SR_IO)) {
+			/* Probably reselected */
+			NCR5380_write(SELECT_ENABLE_REG, 0);
+			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+
+			dsprintk(NDEBUG_INTR, instance, "interrupt with SEL and IO\n");
+
+			if (!hostdata->connected) {
+				NCR5380_reselect(instance);
+				queue_work(hostdata->work_q, &hostdata->main_task);
+			}
+			if (!hostdata->connected)
+				NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+		} else {
+			/* Probably Bus Reset */
+			NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+
+			dsprintk(NDEBUG_INTR, instance, "unknown interrupt\n");
 #ifdef SUN3_SCSI_VME
-				dregs->csr |= CSR_DMA_ENABLE;
+			dregs->csr |= CSR_DMA_ENABLE;
 #endif
-			}
-		} /* if !(SELECTION || PARITY) */
+		}
 		handled = 1;
-	} /* BASR & IRQ */ else {
-		printk(KERN_NOTICE "scsi%d: interrupt without IRQ bit set in BASR, "
-		       "BASR 0x%X, MR 0x%X, SR 0x%x\n", HOSTNO, basr,
-		       NCR5380_read(MODE_REG), NCR5380_read(STATUS_REG));
-		(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+	} else {
+		shost_printk(KERN_NOTICE, instance, "interrupt without IRQ bit\n");
 #ifdef SUN3_SCSI_VME
 		dregs->csr |= CSR_DMA_ENABLE;
 #endif
 	}
 
-	if (!done) {
-		dprintk(NDEBUG_INTR, "scsi%d: in int routine, calling main\n", HOSTNO);
-		/* Put a call to NCR5380_main() on the queue... */
-		queue_main(shost_priv(instance));
-	}
+	spin_unlock_irqrestore(&hostdata->lock, flags);
+
 	return IRQ_RETVAL(handled);
 }
 
 /*
  * Function : int NCR5380_select(struct Scsi_Host *instance,
- *                               struct scsi_cmnd *cmd)
+ * struct scsi_cmnd *cmd)
  *
  * Purpose : establishes I_T_L or I_T_L_Q nexus for new or existing command,
- *	including ARBITRATION, SELECTION, and initial message out for
- *	IDENTIFY and queue messages.
+ * including ARBITRATION, SELECTION, and initial message out for
+ * IDENTIFY and queue messages.
  *
  * Inputs : instance - instantiation of the 5380 driver on which this
- *	target lives, cmd - SCSI command to execute.
+ * target lives, cmd - SCSI command to execute.
  *
- * Returns : -1 if selection could not execute for some reason,
- *	0 if selection succeeded or failed because the target
- *	did not respond.
+ * Returns cmd if selection failed but should be retried,
+ * NULL if selection failed and should not be retried, or
+ * NULL if selection succeeded (hostdata->connected == cmd).
  *
  * Side effects :
- *	If bus busy, arbitration failed, etc, NCR5380_select() will exit
- *		with registers as they should have been on entry - ie
- *		SELECT_ENABLE will be set appropriately, the NCR5380
- *		will cease to drive any SCSI bus signals.
+ * If bus busy, arbitration failed, etc, NCR5380_select() will exit
+ * with registers as they should have been on entry - ie
+ * SELECT_ENABLE will be set appropriately, the NCR5380
+ * will cease to drive any SCSI bus signals.
  *
- *	If successful : I_T_L or I_T_L_Q nexus will be established,
- *		instance->connected will be set to cmd.
- *		SELECT interrupt will be disabled.
+ * If successful : I_T_L or I_T_L_Q nexus will be established,
+ * instance->connected will be set to cmd.
+ * SELECT interrupt will be disabled.
  *
- *	If failed (no target) : cmd->scsi_done() will be called, and the
- *		cmd->result host byte set to DID_BAD_TARGET.
+ * If failed (no target) : cmd->scsi_done() will be called, and the
+ * cmd->result host byte set to DID_BAD_TARGET.
  */
 
-static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
+static struct scsi_cmnd *NCR5380_select(struct Scsi_Host *instance,
+                                        struct scsi_cmnd *cmd)
 {
-	SETUP_HOSTDATA(instance);
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char tmp[3], phase;
 	unsigned char *data;
 	int len;
-	unsigned long timeout;
-	unsigned long flags;
+	int err;
 
-	hostdata->restart_select = 0;
 	NCR5380_dprint(NDEBUG_ARBITRATION, instance);
-	dprintk(NDEBUG_ARBITRATION, "scsi%d: starting arbitration, id = %d\n", HOSTNO,
-		   instance->this_id);
+	dsprintk(NDEBUG_ARBITRATION, instance, "starting arbitration, id = %d\n",
+	         instance->this_id);
+
+	/*
+	 * Arbitration and selection phases are slow and involve dropping the
+	 * lock, so we have to watch out for EH. An exception handler may
+	 * change 'selecting' to NULL. This function will then return NULL
+	 * so that the caller will forget about 'cmd'. (During information
+	 * transfer phases, EH may change 'connected' to NULL.)
+	 */
+	hostdata->selecting = cmd;
 
 	/*
 	 * Set the phase bits to 0, otherwise the NCR5380 won't drive the
 	 * data bus during SELECTION.
 	 */
 
-	local_irq_save(flags);
-	if (hostdata->connected) {
-		local_irq_restore(flags);
-		return -1;
-	}
 	NCR5380_write(TARGET_COMMAND_REG, 0);
 
 	/*
@@ -1337,96 +1244,77 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 	NCR5380_write(OUTPUT_DATA_REG, hostdata->id_mask);
 	NCR5380_write(MODE_REG, MR_ARBITRATE);
 
-	local_irq_restore(flags);
-
-	/* Wait for arbitration logic to complete */
-#if defined(NCR_TIMEOUT)
-	{
-		unsigned long timeout = jiffies + 2*NCR_TIMEOUT;
+	/* The chip now waits for BUS FREE phase. Then after the 800 ns
+	 * Bus Free Delay, arbitration will begin.
+	 */
 
-		while (!(NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_PROGRESS) &&
-		       time_before(jiffies, timeout) && !hostdata->connected)
-			;
-		if (time_after_eq(jiffies, timeout)) {
-			printk("scsi : arbitration timeout at %d\n", __LINE__);
-			NCR5380_write(MODE_REG, MR_BASE);
-			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-			return -1;
-		}
+	spin_unlock_irq(&hostdata->lock);
+	err = NCR5380_poll_politely2(instance, MODE_REG, MR_ARBITRATE, 0,
+	                INITIATOR_COMMAND_REG, ICR_ARBITRATION_PROGRESS,
+	                                       ICR_ARBITRATION_PROGRESS, HZ);
+	spin_lock_irq(&hostdata->lock);
+	if (!(NCR5380_read(MODE_REG) & MR_ARBITRATE)) {
+		/* Reselection interrupt */
+		goto out;
 	}
-#else /* NCR_TIMEOUT */
-	while (!(NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_PROGRESS) &&
-	       !hostdata->connected)
-		;
-#endif
-
-	dprintk(NDEBUG_ARBITRATION, "scsi%d: arbitration complete\n", HOSTNO);
-
-	if (hostdata->connected) {
+	if (err < 0) {
 		NCR5380_write(MODE_REG, MR_BASE);
-		return -1;
+		shost_printk(KERN_ERR, instance,
+		             "select: arbitration timeout\n");
+		goto out;
 	}
-	/*
-	 * The arbitration delay is 2.2us, but this is a minimum and there is
-	 * no maximum so we can safely sleep for ceil(2.2) usecs to accommodate
-	 * the integral nature of udelay().
-	 *
-	 */
+	spin_unlock_irq(&hostdata->lock);
 
+	/* The SCSI-2 arbitration delay is 2.4 us */
 	udelay(3);
 
 	/* Check for lost arbitration */
 	if ((NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
 	    (NCR5380_read(CURRENT_SCSI_DATA_REG) & hostdata->id_higher_mask) ||
-	    (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
-	    hostdata->connected) {
+	    (NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST)) {
 		NCR5380_write(MODE_REG, MR_BASE);
-		dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting MR_ARBITRATE\n",
-			   HOSTNO);
-		return -1;
+		dsprintk(NDEBUG_ARBITRATION, instance, "lost arbitration, deasserting MR_ARBITRATE\n");
+		spin_lock_irq(&hostdata->lock);
+		goto out;
 	}
 
-	/* after/during arbitration, BSY should be asserted.
-	   IBM DPES-31080 Version S31Q works now */
-	/* Tnx to Thomas_Roesch@m2.maus.de for finding this! (Roman) */
+	/* After/during arbitration, BSY should be asserted.
+	 * IBM DPES-31080 Version S31Q works now
+	 * Tnx to Thomas_Roesch@m2.maus.de for finding this! (Roman)
+	 */
 	NCR5380_write(INITIATOR_COMMAND_REG,
 		      ICR_BASE | ICR_ASSERT_SEL | ICR_ASSERT_BSY);
 
-	if ((NCR5380_read(INITIATOR_COMMAND_REG) & ICR_ARBITRATION_LOST) ||
-	    hostdata->connected) {
-		NCR5380_write(MODE_REG, MR_BASE);
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		dprintk(NDEBUG_ARBITRATION, "scsi%d: lost arbitration, deasserting ICR_ASSERT_SEL\n",
-			   HOSTNO);
-		return -1;
-	}
-
 	/*
 	 * Again, bus clear + bus settle time is 1.2us, however, this is
 	 * a minimum so we'll udelay ceil(1.2)
 	 */
 
-#ifdef CONFIG_ATARI_SCSI_TOSHIBA_DELAY
-	/* ++roman: But some targets (see above :-) seem to need a bit more... */
-	udelay(15);
-#else
-	udelay(2);
-#endif
+	if (hostdata->flags & FLAG_TOSHIBA_DELAY)
+		udelay(15);
+	else
+		udelay(2);
 
-	if (hostdata->connected) {
+	spin_lock_irq(&hostdata->lock);
+
+	/* NCR5380_reselect() clears MODE_REG after a reselection interrupt */
+	if (!(NCR5380_read(MODE_REG) & MR_ARBITRATE))
+		goto out;
+
+	if (!hostdata->selecting) {
 		NCR5380_write(MODE_REG, MR_BASE);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		return -1;
+		goto out;
 	}
 
-	dprintk(NDEBUG_ARBITRATION, "scsi%d: won arbitration\n", HOSTNO);
+	dsprintk(NDEBUG_ARBITRATION, instance, "won arbitration\n");
 
 	/*
 	 * Now that we have won arbitration, start Selection process, asserting
 	 * the host and target ID's on the SCSI bus.
 	 */
 
-	NCR5380_write(OUTPUT_DATA_REG, (hostdata->id_mask | (1 << cmd->device->id)));
+	NCR5380_write(OUTPUT_DATA_REG, hostdata->id_mask | (1 << scmd_id(cmd)));
 
 	/*
 	 * Raise ATN while SEL is true before BSY goes false from arbitration,
@@ -1434,22 +1322,18 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 	 * phase immediately after selection.
 	 */
 
-	NCR5380_write(INITIATOR_COMMAND_REG, (ICR_BASE | ICR_ASSERT_BSY |
-		      ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_SEL ));
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_BSY |
+	              ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_SEL);
 	NCR5380_write(MODE_REG, MR_BASE);
 
 	/*
 	 * Reselect interrupts must be turned off prior to the dropping of BSY,
 	 * otherwise we will trigger an interrupt.
 	 */
-
-	if (hostdata->connected) {
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		return -1;
-	}
-
 	NCR5380_write(SELECT_ENABLE_REG, 0);
 
+	spin_unlock_irq(&hostdata->lock);
+
 	/*
 	 * The initiator shall then wait at least two deskew delays and release
 	 * the BSY signal.
@@ -1457,8 +1341,8 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 	udelay(1);        /* wingel -- wait two bus deskew delay >2*45ns */
 
 	/* Reset BSY */
-	NCR5380_write(INITIATOR_COMMAND_REG, (ICR_BASE | ICR_ASSERT_DATA |
-		      ICR_ASSERT_ATN | ICR_ASSERT_SEL));
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA |
+	              ICR_ASSERT_ATN | ICR_ASSERT_SEL);
 
 	/*
 	 * Something weird happens when we cease to drive BSY - looks
@@ -1479,45 +1363,39 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 
 	udelay(1);
 
-	dprintk(NDEBUG_SELECTION, "scsi%d: selecting target %d\n", HOSTNO, cmd->device->id);
+	dsprintk(NDEBUG_SELECTION, instance, "selecting target %d\n", scmd_id(cmd));
 
 	/*
 	 * The SCSI specification calls for a 250 ms timeout for the actual
 	 * selection.
 	 */
 
-	timeout = jiffies + msecs_to_jiffies(250);
-
-	/*
-	 * XXX very interesting - we're seeing a bounce where the BSY we
-	 * asserted is being reflected / still asserted (propagation delay?)
-	 * and it's detecting as true.  Sigh.
-	 */
-
-#if 0
-	/* ++roman: If a target conformed to the SCSI standard, it wouldn't assert
-	 * IO while SEL is true. But again, there are some disks out the in the
-	 * world that do that nevertheless. (Somebody claimed that this announces
-	 * reselection capability of the target.) So we better skip that test and
-	 * only wait for BSY... (Famous german words: Der Klügere gibt nach :-)
-	 */
-
-	while (time_before(jiffies, timeout) &&
-	       !(NCR5380_read(STATUS_REG) & (SR_BSY | SR_IO)))
-		;
+	err = NCR5380_poll_politely(instance, STATUS_REG, SR_BSY, SR_BSY,
+	                            msecs_to_jiffies(250));
 
 	if ((NCR5380_read(STATUS_REG) & (SR_SEL | SR_IO)) == (SR_SEL | SR_IO)) {
+		spin_lock_irq(&hostdata->lock);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		NCR5380_reselect(instance);
-		printk(KERN_ERR "scsi%d: reselection after won arbitration?\n",
-		       HOSTNO);
+		if (!hostdata->connected)
+			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+		shost_printk(KERN_ERR, instance, "reselection after won arbitration?\n");
+		goto out;
+	}
+
+	if (err < 0) {
+		spin_lock_irq(&hostdata->lock);
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		return -1;
+		/* Can't touch cmd if it has been reclaimed by the scsi ML */
+		if (hostdata->selecting) {
+			cmd->result = DID_BAD_TARGET << 16;
+			complete_cmd(instance, cmd);
+			dsprintk(NDEBUG_SELECTION, instance, "target did not respond within 250ms\n");
+			cmd = NULL;
+		}
+		goto out;
 	}
-#else
-	while (time_before(jiffies, timeout) && !(NCR5380_read(STATUS_REG) & SR_BSY))
-		;
-#endif
 
 	/*
 	 * No less than two deskew delays after the initiator detects the
@@ -1529,29 +1407,6 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 
-	if (!(NCR5380_read(STATUS_REG) & SR_BSY)) {
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-		if (hostdata->targets_present & (1 << cmd->device->id)) {
-			printk(KERN_ERR "scsi%d: weirdness\n", HOSTNO);
-			if (hostdata->restart_select)
-				printk(KERN_NOTICE "\trestart select\n");
-			NCR5380_dprint(NDEBUG_ANY, instance);
-			NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-			return -1;
-		}
-		cmd->result = DID_BAD_TARGET << 16;
-#ifdef SUPPORT_TAGS
-		cmd_free_tag(cmd);
-#endif
-		cmd->scsi_done(cmd);
-		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		dprintk(NDEBUG_SELECTION, "scsi%d: target did not respond within 250ms\n", HOSTNO);
-		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-		return 0;
-	}
-
-	hostdata->targets_present |= (1 << cmd->device->id);
-
 	/*
 	 * Since we followed the SCSI spec, and raised ATN while SEL
 	 * was true but before BSY was false during selection, the information
@@ -1563,16 +1418,27 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 	 * until it wraps back to 0.
 	 *
 	 * XXX - it turns out that there are some broken SCSI-II devices,
-	 *	     which claim to support tagged queuing but fail when more than
-	 *	     some number of commands are issued at once.
+	 * which claim to support tagged queuing but fail when more than
+	 * some number of commands are issued at once.
 	 */
 
 	/* Wait for start of REQ/ACK handshake */
-	while (!(NCR5380_read(STATUS_REG) & SR_REQ))
-		;
 
-	dprintk(NDEBUG_SELECTION, "scsi%d: target %d selected, going into MESSAGE OUT phase.\n",
-		   HOSTNO, cmd->device->id);
+	err = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ);
+	spin_lock_irq(&hostdata->lock);
+	if (err < 0) {
+		shost_printk(KERN_ERR, instance, "select: REQ timeout\n");
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+		NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+		goto out;
+	}
+	if (!hostdata->selecting) {
+		do_abort(instance);
+		goto out;
+	}
+
+	dsprintk(NDEBUG_SELECTION, instance, "target %d selected, going into MESSAGE OUT phase.\n",
+	         scmd_id(cmd));
 	tmp[0] = IDENTIFY(1, cmd->device->lun);
 
 #ifdef SUPPORT_TAGS
@@ -1591,11 +1457,12 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 	data = tmp;
 	phase = PHASE_MSGOUT;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
-	dprintk(NDEBUG_SELECTION, "scsi%d: nexus established.\n", HOSTNO);
+	dsprintk(NDEBUG_SELECTION, instance, "nexus established.\n");
 	/* XXX need to handle errors here */
+
 	hostdata->connected = cmd;
 #ifndef SUPPORT_TAGS
-	hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
+	hostdata->busy[cmd->device->id] |= 1 << cmd->device->lun;
 #endif
 #ifdef SUN3_SCSI_VME
 	dregs->csr |= CSR_INTR;
@@ -1603,24 +1470,30 @@ static int NCR5380_select(struct Scsi_Host *instance, struct scsi_cmnd *cmd)
 
 	initialize_SCp(cmd);
 
-	return 0;
+	cmd = NULL;
+
+out:
+	if (!hostdata->selecting)
+		return NULL;
+	hostdata->selecting = NULL;
+	return cmd;
 }
 
 /*
  * Function : int NCR5380_transfer_pio (struct Scsi_Host *instance,
- *      unsigned char *phase, int *count, unsigned char **data)
+ * unsigned char *phase, int *count, unsigned char **data)
  *
  * Purpose : transfers data in given phase using polled I/O
  *
  * Inputs : instance - instance of driver, *phase - pointer to
- *	what phase is expected, *count - pointer to number of
- *	bytes to transfer, **data - pointer to data pointer.
+ * what phase is expected, *count - pointer to number of
+ * bytes to transfer, **data - pointer to data pointer.
  *
  * Returns : -1 when different phase is entered without transferring
- *	maximum number of bytes, 0 if all bytes are transferred or exit
- *	is in same phase.
+ * maximum number of bytes, 0 if all bytes are transferred or exit
+ * is in same phase.
  *
- *	Also, *phase, *count, *data are modified in place.
+ * Also, *phase, *count, *data are modified in place.
  *
  * XXX Note : handling for bus free may be useful.
  */
@@ -1635,9 +1508,9 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 				unsigned char *phase, int *count,
 				unsigned char **data)
 {
-	register unsigned char p = *phase, tmp;
-	register int c = *count;
-	register unsigned char *d = *data;
+	unsigned char p = *phase, tmp;
+	int c = *count;
+	unsigned char *d = *data;
 
 	/*
 	 * The NCR5380 chip will only drive the SCSI bus when the
@@ -1652,14 +1525,15 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		 * Wait for assertion of REQ, after which the phase bits will be
 		 * valid
 		 */
-		while (!((tmp = NCR5380_read(STATUS_REG)) & SR_REQ))
-			;
 
-		dprintk(NDEBUG_HANDSHAKE, "scsi%d: REQ detected\n", HOSTNO);
+		if (NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ) < 0)
+			break;
+
+		dsprintk(NDEBUG_HANDSHAKE, instance, "REQ asserted\n");
 
 		/* Check for phase mismatch */
-		if ((tmp & PHASE_MASK) != p) {
-			dprintk(NDEBUG_PIO, "scsi%d: phase mismatch\n", HOSTNO);
+		if ((NCR5380_read(STATUS_REG) & PHASE_MASK) != p) {
+			dsprintk(NDEBUG_PIO, instance, "phase mismatch\n");
 			NCR5380_dprint_phase(NDEBUG_PIO, instance);
 			break;
 		}
@@ -1684,35 +1558,36 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_DATA);
 				NCR5380_dprint(NDEBUG_PIO, instance);
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
-					      ICR_ASSERT_DATA | ICR_ASSERT_ACK);
+				              ICR_ASSERT_DATA | ICR_ASSERT_ACK);
 			} else {
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
-					      ICR_ASSERT_DATA | ICR_ASSERT_ATN);
+				              ICR_ASSERT_DATA | ICR_ASSERT_ATN);
 				NCR5380_dprint(NDEBUG_PIO, instance);
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
-					      ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
+				              ICR_ASSERT_DATA | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
 			}
 		} else {
 			NCR5380_dprint(NDEBUG_PIO, instance);
 			NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ACK);
 		}
 
-		while (NCR5380_read(STATUS_REG) & SR_REQ)
-			;
+		if (NCR5380_poll_politely(instance,
+		                          STATUS_REG, SR_REQ, 0, 5 * HZ) < 0)
+			break;
 
-		dprintk(NDEBUG_HANDSHAKE, "scsi%d: req false, handshake complete\n", HOSTNO);
+		dsprintk(NDEBUG_HANDSHAKE, instance, "REQ negated, handshake complete\n");
 
-		/*
-		 * We have several special cases to consider during REQ/ACK handshaking :
-		 * 1.  We were in MSGOUT phase, and we are on the last byte of the
-		 *	message.  ATN must be dropped as ACK is dropped.
-		 *
-		 * 2.  We are in a MSGIN phase, and we are on the last byte of the
-		 *	message.  We must exit with ACK asserted, so that the calling
-		 *	code may raise ATN before dropping ACK to reject the message.
-		 *
-		 * 3.  ACK and ATN are clear and the target may proceed as normal.
-		 */
+/*
+ * We have several special cases to consider during REQ/ACK handshaking :
+ * 1.  We were in MSGOUT phase, and we are on the last byte of the
+ * message.  ATN must be dropped as ACK is dropped.
+ *
+ * 2.  We are in a MSGIN phase, and we are on the last byte of the
+ * message.  We must exit with ACK asserted, so that the calling
+ * code may raise ATN before dropping ACK to reject the message.
+ *
+ * 3.  ACK and ATN are clear and the target may proceed as normal.
+ */
 		if (!(p == PHASE_MSGIN && c == 1)) {
 			if (p == PHASE_MSGOUT && c > 1)
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
@@ -1721,16 +1596,16 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		}
 	} while (--c);
 
-	dprintk(NDEBUG_PIO, "scsi%d: residual %d\n", HOSTNO, c);
+	dsprintk(NDEBUG_PIO, instance, "residual %d\n", c);
 
 	*count = c;
 	*data = d;
 	tmp = NCR5380_read(STATUS_REG);
 	/* The phase read from the bus is valid if either REQ is (already)
-	 * asserted or if ACK hasn't been released yet. The latter is the case if
-	 * we're in MSGIN and all wanted bytes have been received.
+	 * asserted or if ACK hasn't been released yet. The latter applies if
+	 * we're in MSG IN, DATA IN or STATUS and all bytes have been received.
 	 */
-	if ((tmp & SR_REQ) || (p == PHASE_MSGIN && c == 0))
+	if ((tmp & SR_REQ) || ((tmp & SR_IO) && c == 0))
 		*phase = tmp & PHASE_MASK;
 	else
 		*phase = PHASE_UNKNOWN;
@@ -1741,19 +1616,45 @@ static int NCR5380_transfer_pio(struct Scsi_Host *instance,
 		return -1;
 }
 
-/*
- * Function : do_abort (Scsi_Host *host)
+/**
+ * do_reset - issue a reset command
+ * @instance: adapter to reset
  *
- * Purpose : abort the currently established nexus.  Should only be
- *	called from a routine which can drop into a
+ * Issue a reset sequence to the NCR5380 and try and get the bus
+ * back into sane shape.
  *
- * Returns : 0 on success, -1 on failure.
+ * This clears the reset interrupt flag because there may be no handler for
+ * it. When the driver is initialized, the NCR5380_intr() handler has not yet
+ * been installed. And when in EH we may have released the ST DMA interrupt.
+ */
+
+static void do_reset(struct Scsi_Host *instance)
+{
+	unsigned long flags;
+
+	local_irq_save(flags);
+	NCR5380_write(TARGET_COMMAND_REG,
+	              PHASE_SR_TO_TCR(NCR5380_read(STATUS_REG) & PHASE_MASK));
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST);
+	udelay(50);
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+	(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
+	local_irq_restore(flags);
+}
+
+/**
+ * do_abort - abort the currently established nexus by going to
+ * MESSAGE OUT phase and sending an ABORT message.
+ * @instance: relevant scsi host instance
+ *
+ * Returns 0 on success, -1 on failure.
  */
 
 static int do_abort(struct Scsi_Host *instance)
 {
-	unsigned char tmp, *msgptr, phase;
+	unsigned char *msgptr, phase, tmp;
 	int len;
+	int rc;
 
 	/* Request message out phase */
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
@@ -1768,16 +1669,20 @@ static int do_abort(struct Scsi_Host *instance)
 	 * the target sees, so we just handshake.
 	 */
 
-	while (!((tmp = NCR5380_read(STATUS_REG)) & SR_REQ))
-		;
+	rc = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, 10 * HZ);
+	if (rc < 0)
+		goto timeout;
+
+	tmp = NCR5380_read(STATUS_REG) & PHASE_MASK;
 
 	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(tmp));
 
-	if ((tmp & PHASE_MASK) != PHASE_MSGOUT) {
-		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN |
-			      ICR_ASSERT_ACK);
-		while (NCR5380_read(STATUS_REG) & SR_REQ)
-			;
+	if (tmp != PHASE_MSGOUT) {
+		NCR5380_write(INITIATOR_COMMAND_REG,
+		              ICR_BASE | ICR_ASSERT_ATN | ICR_ASSERT_ACK);
+		rc = NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, 0, 3 * HZ);
+		if (rc < 0)
+			goto timeout;
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN);
 	}
 
@@ -1793,26 +1698,29 @@ static int do_abort(struct Scsi_Host *instance)
 	 */
 
 	return len ? -1 : 0;
+
+timeout:
+	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+	return -1;
 }
 
 #if defined(REAL_DMA)
 /*
  * Function : int NCR5380_transfer_dma (struct Scsi_Host *instance,
- *      unsigned char *phase, int *count, unsigned char **data)
+ * unsigned char *phase, int *count, unsigned char **data)
  *
  * Purpose : transfers data in given phase using either real
- *	or pseudo DMA.
+ * or pseudo DMA.
  *
  * Inputs : instance - instance of driver, *phase - pointer to
- *	what phase is expected, *count - pointer to number of
- *	bytes to transfer, **data - pointer to data pointer.
+ * what phase is expected, *count - pointer to number of
+ * bytes to transfer, **data - pointer to data pointer.
  *
  * Returns : -1 when different phase is entered without transferring
- *	maximum number of bytes, 0 if all bytes or transferred or exit
- *	is in same phase.
- *
- *	Also, *phase, *count, *data are modified in place.
+ * maximum number of bytes, 0 if all bytes or transferred or exit
+ * is in same phase.
  *
+ * Also, *phase, *count, *data are modified in place.
  */
 
 
@@ -1820,10 +1728,9 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 				unsigned char *phase, int *count,
 				unsigned char **data)
 {
-	SETUP_HOSTDATA(instance);
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	register int c = *count;
 	register unsigned char p = *phase;
-	unsigned long flags;
 
 #if defined(CONFIG_SUN3)
 	/* sanity check */
@@ -1834,29 +1741,22 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 	}
 	hostdata->dma_len = c;
 
-	dprintk(NDEBUG_DMA, "scsi%d: initializing DMA for %s, %d bytes %s %p\n",
-		instance->host_no, (p & SR_IO) ? "reading" : "writing",
-		c, (p & SR_IO) ? "to" : "from", *data);
+	dsprintk(NDEBUG_DMA, instance, "initializing DMA %s: length %d, address %p\n",
+	         (p & SR_IO) ? "receive" : "send", c, *data);
 
 	/* netbsd turns off ints here, why not be safe and do it too */
-	local_irq_save(flags);
 
 	/* send start chain */
 	sun3scsi_dma_start(c, *data);
 
+	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(p));
+	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY |
+	                        MR_ENABLE_EOP_INTR);
 	if (p & SR_IO) {
-		NCR5380_write(TARGET_COMMAND_REG, 1);
-		NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		NCR5380_write(INITIATOR_COMMAND_REG, 0);
-		NCR5380_write(MODE_REG,
-			      (NCR5380_read(MODE_REG) | MR_DMA_MODE | MR_ENABLE_EOP_INTR));
 		NCR5380_write(START_DMA_INITIATOR_RECEIVE_REG, 0);
 	} else {
-		NCR5380_write(TARGET_COMMAND_REG, 0);
-		NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 		NCR5380_write(INITIATOR_COMMAND_REG, ICR_ASSERT_DATA);
-		NCR5380_write(MODE_REG,
-			      (NCR5380_read(MODE_REG) | MR_DMA_MODE | MR_ENABLE_EOP_INTR));
 		NCR5380_write(START_DMA_SEND_REG, 0);
 	}
 
@@ -1864,8 +1764,6 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 	dregs->csr |= CSR_DMA_ENABLE;
 #endif
 
-	local_irq_restore(flags);
-
 	sun3_dma_active = 1;
 
 #else /* !defined(CONFIG_SUN3) */
@@ -1880,25 +1778,20 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 	if (hostdata->read_overruns && (p & SR_IO))
 		c -= hostdata->read_overruns;
 
-	dprintk(NDEBUG_DMA, "scsi%d: initializing DMA for %s, %d bytes %s %p\n",
-		   HOSTNO, (p & SR_IO) ? "reading" : "writing",
-		   c, (p & SR_IO) ? "to" : "from", d);
+	dsprintk(NDEBUG_DMA, instance, "initializing DMA %s: length %d, address %p\n",
+	         (p & SR_IO) ? "receive" : "send", c, d);
 
 	NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(p));
-
-#ifdef REAL_DMA
-	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_ENABLE_EOP_INTR | MR_MONITOR_BSY);
-#endif /* def REAL_DMA  */
+	NCR5380_write(MODE_REG, MR_BASE | MR_DMA_MODE | MR_MONITOR_BSY |
+	                        MR_ENABLE_EOP_INTR);
 
 	if (!(hostdata->flags & FLAG_LATE_DMA_SETUP)) {
 		/* On the Medusa, it is a must to initialize the DMA before
 		 * starting the NCR. This is also the cleaner way for the TT.
 		 */
-		local_irq_save(flags);
 		hostdata->dma_len = (p & SR_IO) ?
 			NCR5380_dma_read_setup(instance, d, c) :
 			NCR5380_dma_write_setup(instance, d, c);
-		local_irq_restore(flags);
 	}
 
 	if (p & SR_IO)
@@ -1912,11 +1805,9 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
 		/* On the Falcon, the DMA setup must be done after the last */
 		/* NCR access, else the DMA setup gets trashed!
 		 */
-		local_irq_save(flags);
 		hostdata->dma_len = (p & SR_IO) ?
 			NCR5380_dma_read_setup(instance, d, c) :
 			NCR5380_dma_write_setup(instance, d, c);
-		local_irq_restore(flags);
 	}
 #endif /* !defined(CONFIG_SUN3) */
 
@@ -1928,23 +1819,22 @@ static int NCR5380_transfer_dma(struct Scsi_Host *instance,
  * Function : NCR5380_information_transfer (struct Scsi_Host *instance)
  *
  * Purpose : run through the various SCSI phases and do as the target
- *	directs us to.  Operates on the currently connected command,
- *	instance->connected.
+ * directs us to.  Operates on the currently connected command,
+ * instance->connected.
  *
  * Inputs : instance, instance for which we are doing commands
  *
  * Side effects : SCSI things happen, the disconnected queue will be
- *	modified if a command disconnects, *instance->connected will
- *	change.
+ * modified if a command disconnects, *instance->connected will
+ * change.
  *
  * XXX Note : we need to watch for bus free or a reset condition here
- *	to recover from an unexpected bus free condition.
+ * to recover from an unexpected bus free condition.
  */
 
 static void NCR5380_information_transfer(struct Scsi_Host *instance)
 {
-	SETUP_HOSTDATA(instance);
-	unsigned long flags;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char msgout = NOP;
 	int sink = 0;
 	int len;
@@ -1953,13 +1843,15 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 #endif
 	unsigned char *data;
 	unsigned char phase, tmp, extended_msg[10], old_phase = 0xff;
-	struct scsi_cmnd *cmd = (struct scsi_cmnd *) hostdata->connected;
+	struct scsi_cmnd *cmd;
 
 #ifdef SUN3_SCSI_VME
 	dregs->csr |= CSR_INTR;
 #endif
 
-	while (1) {
+	while ((cmd = hostdata->connected)) {
+		struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
+
 		tmp = NCR5380_read(STATUS_REG);
 		/* We only have a valid SCSI phase when REQ is asserted */
 		if (tmp & SR_REQ) {
@@ -1984,7 +1876,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				/* this command setup for dma yet? */
 				if ((count >= DMA_MIN_SIZE) && (sun3_dma_setup_done != cmd)) {
 					if (cmd->request->cmd_type == REQ_TYPE_FS) {
-						sun3scsi_dma_setup(d, count,
+						sun3scsi_dma_setup(instance, d, count,
 						                   rq_data_dir(cmd->request));
 						sun3_dma_setup_done = cmd;
 					}
@@ -2000,11 +1892,11 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				NCR5380_write(TARGET_COMMAND_REG, PHASE_SR_TO_TCR(tmp));
 
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_ATN |
-					      ICR_ASSERT_ACK);
+				              ICR_ASSERT_ACK);
 				while (NCR5380_read(STATUS_REG) & SR_REQ)
 					;
 				NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
-					      ICR_ASSERT_ATN);
+				              ICR_ASSERT_ATN);
 				sink = 0;
 				continue;
 			}
@@ -2012,12 +1904,11 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 			switch (phase) {
 			case PHASE_DATAOUT:
 #if (NDEBUG & NDEBUG_NO_DATAOUT)
-				printk("scsi%d: NDEBUG_NO_DATAOUT set, attempted DATAOUT "
-				       "aborted\n", HOSTNO);
+				shost_printk(KERN_DEBUG, instance, "NDEBUG_NO_DATAOUT set, attempted DATAOUT aborted\n");
 				sink = 1;
 				do_abort(instance);
 				cmd->result = DID_ERROR << 16;
-				cmd->scsi_done(cmd);
+				complete_cmd(instance, cmd);
 				return;
 #endif
 			case PHASE_DATAIN:
@@ -2031,13 +1922,10 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					--cmd->SCp.buffers_residual;
 					cmd->SCp.this_residual = cmd->SCp.buffer->length;
 					cmd->SCp.ptr = sg_virt(cmd->SCp.buffer);
-					/* ++roman: Try to merge some scatter-buffers if
-					 * they are at contiguous physical addresses.
-					 */
 					merge_contiguous_buffers(cmd);
-					dprintk(NDEBUG_INFORMATION, "scsi%d: %d bytes and %d buffers left\n",
-						   HOSTNO, cmd->SCp.this_residual,
-						   cmd->SCp.buffers_residual);
+					dsprintk(NDEBUG_INFORMATION, instance, "%d bytes and %d buffers left\n",
+					         cmd->SCp.this_residual,
+					         cmd->SCp.buffers_residual);
 				}
 
 				/*
@@ -2051,16 +1939,18 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				 */
 
 				/* ++roman: I suggest, this should be
-				 *   #if def(REAL_DMA)
+				 * #if def(REAL_DMA)
 				 * instead of leaving REAL_DMA out.
 				 */
 
 #if defined(REAL_DMA)
-				if (
 #if !defined(CONFIG_SUN3)
-				    !cmd->device->borken &&
+				transfersize = 0;
+				if (!cmd->device->borken)
 #endif
-				    (transfersize = NCR5380_dma_xfer_len(instance, cmd, phase)) >= DMA_MIN_SIZE) {
+					transfersize = NCR5380_dma_xfer_len(instance, cmd, phase);
+
+				if (transfersize >= DMA_MIN_SIZE) {
 					len = transfersize;
 					cmd->SCp.phase = phase;
 					if (NCR5380_transfer_dma(instance, &phase,
@@ -2068,16 +1958,15 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						/*
 						 * If the watchdog timer fires, all future
 						 * accesses to this device will use the
-						 * polled-IO. */
+						 * polled-IO.
+						 */
 						scmd_printk(KERN_INFO, cmd,
 							"switching to slow handshake\n");
 						cmd->device->borken = 1;
-						NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE |
-							ICR_ASSERT_ATN);
 						sink = 1;
 						do_abort(instance);
 						cmd->result = DID_ERROR << 16;
-						cmd->scsi_done(cmd);
+						complete_cmd(instance, cmd);
 						/* XXX - need to source or sink data here, as appropriate */
 					} else {
 #ifdef REAL_DMA
@@ -2093,9 +1982,13 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					}
 				} else
 #endif /* defined(REAL_DMA) */
+				{
+					spin_unlock_irq(&hostdata->lock);
 					NCR5380_transfer_pio(instance, &phase,
-							     (int *)&cmd->SCp.this_residual,
-							     (unsigned char **)&cmd->SCp.ptr);
+					                     (int *)&cmd->SCp.this_residual,
+					                     (unsigned char **)&cmd->SCp.ptr);
+					spin_lock_irq(&hostdata->lock);
+				}
 #if defined(CONFIG_SUN3) && defined(REAL_DMA)
 				/* if we had intended to dma that command clear it */
 				if (sun3_dma_setup_done == cmd)
@@ -2105,162 +1998,64 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 			case PHASE_MSGIN:
 				len = 1;
 				data = &tmp;
-				NCR5380_write(SELECT_ENABLE_REG, 0);	/* disable reselects */
 				NCR5380_transfer_pio(instance, &phase, &len, &data);
 				cmd->SCp.Message = tmp;
 
 				switch (tmp) {
-				/*
-				 * Linking lets us reduce the time required to get the
-				 * next command out to the device, hopefully this will
-				 * mean we don't waste another revolution due to the delays
-				 * required by ARBITRATION and another SELECTION.
-				 *
-				 * In the current implementation proposal, low level drivers
-				 * merely have to start the next command, pointed to by
-				 * next_link, done() is called as with unlinked commands.
-				 */
-#ifdef LINKED
-				case LINKED_CMD_COMPLETE:
-				case LINKED_FLG_CMD_COMPLETE:
-					/* Accept message by clearing ACK */
-					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-
-					dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %llu linked command "
-						   "complete.\n", HOSTNO, cmd->device->id, cmd->device->lun);
-
-					/* Enable reselect interrupts */
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-					/*
-					 * Sanity check : A linked command should only terminate
-					 * with one of these messages if there are more linked
-					 * commands available.
-					 */
-
-					if (!cmd->next_link) {
-						 printk(KERN_NOTICE "scsi%d: target %d lun %llu "
-							"linked command complete, no next_link\n",
-							HOSTNO, cmd->device->id, cmd->device->lun);
-						sink = 1;
-						do_abort(instance);
-						return;
-					}
-
-					initialize_SCp(cmd->next_link);
-					/* The next command is still part of this process; copy it
-					 * and don't free it! */
-					cmd->next_link->tag = cmd->tag;
-					cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					dprintk(NDEBUG_LINKED, "scsi%d: target %d lun %llu linked request "
-						   "done, calling scsi_done().\n",
-						   HOSTNO, cmd->device->id, cmd->device->lun);
-					cmd->scsi_done(cmd);
-					cmd = hostdata->connected;
-					break;
-#endif /* def LINKED */
 				case ABORT:
 				case COMMAND_COMPLETE:
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d, lun %llu "
-						  "completed\n", HOSTNO, cmd->device->id, cmd->device->lun);
+					dsprintk(NDEBUG_QUEUES, instance,
+					         "COMMAND COMPLETE %p target %d lun %llu\n",
+					         cmd, scmd_id(cmd), cmd->device->lun);
 
-					local_irq_save(flags);
-					hostdata->retain_dma_intr++;
 					hostdata->connected = NULL;
 #ifdef SUPPORT_TAGS
 					cmd_free_tag(cmd);
 					if (status_byte(cmd->SCp.Status) == QUEUE_FULL) {
-						/* Turn a QUEUE FULL status into BUSY, I think the
-						 * mid level cannot handle QUEUE FULL :-( (The
-						 * command is retried after BUSY). Also update our
-						 * queue size to the number of currently issued
-						 * commands now.
-						 */
-						/* ++Andreas: the mid level code knows about
-						   QUEUE_FULL now. */
-						struct tag_alloc *ta = &hostdata->TagAlloc[scmd_id(cmd)][cmd->device->lun];
-						dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %llu returned "
-							   "QUEUE_FULL after %d commands\n",
-							   HOSTNO, cmd->device->id, cmd->device->lun,
-							   ta->nr_allocated);
+						u8 lun = cmd->device->lun;
+						struct tag_alloc *ta = &hostdata->TagAlloc[scmd_id(cmd)][lun];
+
+						dsprintk(NDEBUG_TAGS, instance,
+						         "QUEUE_FULL %p target %d lun %d nr_allocated %d\n",
+						         cmd, scmd_id(cmd), lun, ta->nr_allocated);
 						if (ta->queue_size > ta->nr_allocated)
-							ta->nr_allocated = ta->queue_size;
+							ta->queue_size = ta->nr_allocated;
 					}
-#else
-					hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
 #endif
-					/* Enable reselect interrupts */
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-
-					/*
-					 * I'm not sure what the correct thing to do here is :
-					 *
-					 * If the command that just executed is NOT a request
-					 * sense, the obvious thing to do is to set the result
-					 * code to the values of the stored parameters.
-					 *
-					 * If it was a REQUEST SENSE command, we need some way to
-					 * differentiate between the failure code of the original
-					 * and the failure code of the REQUEST sense - the obvious
-					 * case is success, where we fall through and leave the
-					 * result code unchanged.
-					 *
-					 * The non-obvious place is where the REQUEST SENSE failed
-					 */
-
-					if (cmd->cmnd[0] != REQUEST_SENSE)
-						cmd->result = cmd->SCp.Status | (cmd->SCp.Message << 8);
-					else if (status_byte(cmd->SCp.Status) != GOOD)
-						cmd->result = (cmd->result & 0x00ffff) | (DID_ERROR << 16);
-
-					if ((cmd->cmnd[0] == REQUEST_SENSE) &&
-						hostdata->ses.cmd_len) {
-						scsi_eh_restore_cmnd(cmd, &hostdata->ses);
-						hostdata->ses.cmd_len = 0 ;
-					}
-
-					if ((cmd->cmnd[0] != REQUEST_SENSE) &&
-					    (status_byte(cmd->SCp.Status) == CHECK_CONDITION)) {
-						scsi_eh_prep_cmnd(cmd, &hostdata->ses, NULL, 0, ~0);
-
-						dprintk(NDEBUG_AUTOSENSE, "scsi%d: performing request sense\n", HOSTNO);
 
-						LIST(cmd,hostdata->issue_queue);
-						SET_NEXT(cmd, hostdata->issue_queue);
-						hostdata->issue_queue = (struct scsi_cmnd *) cmd;
-						dprintk(NDEBUG_QUEUES, "scsi%d: REQUEST SENSE added to head of "
-							  "issue queue\n", H_NO(cmd));
-					} else {
-						cmd->scsi_done(cmd);
+					cmd->result &= ~0xffff;
+					cmd->result |= cmd->SCp.Status;
+					cmd->result |= cmd->SCp.Message << 8;
+
+					if (cmd->cmnd[0] == REQUEST_SENSE)
+						complete_cmd(instance, cmd);
+					else {
+						if (cmd->SCp.Status == SAM_STAT_CHECK_CONDITION ||
+						    cmd->SCp.Status == SAM_STAT_COMMAND_TERMINATED) {
+							dsprintk(NDEBUG_QUEUES, instance, "autosense: adding cmd %p to tail of autosense queue\n",
+							         cmd);
+							list_add_tail(&ncmd->list,
+							              &hostdata->autosense);
+						} else
+							complete_cmd(instance, cmd);
 					}
 
-					local_irq_restore(flags);
-
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					/*
 					 * Restore phase bits to 0 so an interrupted selection,
 					 * arbitration can resume.
 					 */
 					NCR5380_write(TARGET_COMMAND_REG, 0);
 
-					while ((NCR5380_read(STATUS_REG) & SR_BSY) && !hostdata->connected)
-						barrier();
+					/* Enable reselect interrupts */
+					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 
-					local_irq_save(flags);
-					hostdata->retain_dma_intr--;
-					/* ++roman: For Falcon SCSI, release the lock on the
-					 * ST-DMA here if no other commands are waiting on the
-					 * disconnected queue.
-					 */
 					maybe_release_dma_irq(instance);
-					local_irq_restore(flags);
 					return;
 				case MESSAGE_REJECT:
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					/* Enable reselect interrupts */
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					switch (hostdata->last_message) {
 					case HEAD_OF_QUEUE_TAG:
 					case ORDERED_QUEUE_TAG:
@@ -2274,27 +2069,20 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						cmd->device->tagged_supported = 0;
 						hostdata->busy[cmd->device->id] |= (1 << cmd->device->lun);
 						cmd->tag = TAG_NONE;
-						dprintk(NDEBUG_TAGS, "scsi%d: target %d lun %llu rejected "
-							   "QUEUE_TAG message; tagged queuing "
-							   "disabled\n",
-							   HOSTNO, cmd->device->id, cmd->device->lun);
+						dsprintk(NDEBUG_TAGS, instance, "target %d lun %llu rejected QUEUE_TAG message; tagged queuing disabled\n",
+						         scmd_id(cmd), cmd->device->lun);
 						break;
 					}
 					break;
 				case DISCONNECT:
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					local_irq_save(flags);
-					cmd->device->disconnect = 1;
-					LIST(cmd,hostdata->disconnected_queue);
-					SET_NEXT(cmd, hostdata->disconnected_queue);
 					hostdata->connected = NULL;
-					hostdata->disconnected_queue = cmd;
-					local_irq_restore(flags);
-					dprintk(NDEBUG_QUEUES, "scsi%d: command for target %d lun %llu was "
-						  "moved from connected to the "
-						  "disconnected_queue\n", HOSTNO,
-						  cmd->device->id, cmd->device->lun);
+					list_add(&ncmd->list, &hostdata->disconnected);
+					dsprintk(NDEBUG_INFORMATION | NDEBUG_QUEUES,
+					         instance, "connected command %p for target %d lun %llu moved to disconnected queue\n",
+					         cmd, scmd_id(cmd), cmd->device->lun);
+
 					/*
 					 * Restore phase bits to 0 so an interrupted selection,
 					 * arbitration can resume.
@@ -2303,9 +2091,6 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 
 					/* Enable reselect interrupts */
 					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
-					/* Wait for bus free to avoid nasty timeouts */
-					while ((NCR5380_read(STATUS_REG) & SR_BSY) && !hostdata->connected)
-						barrier();
 #ifdef SUN3_SCSI_VME
 					dregs->csr |= CSR_DMA_ENABLE;
 #endif
@@ -2324,37 +2109,30 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				case RESTORE_POINTERS:
 					/* Accept message by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-					/* Enable reselect interrupts */
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					break;
 				case EXTENDED_MESSAGE:
 					/*
-					 * Extended messages are sent in the following format :
-					 * Byte
-					 * 0		EXTENDED_MESSAGE == 1
-					 * 1		length (includes one byte for code, doesn't
-					 *		include first two bytes)
-					 * 2		code
-					 * 3..length+1	arguments
-					 *
-					 * Start the extended message buffer with the EXTENDED_MESSAGE
+					 * Start the message buffer with the EXTENDED_MESSAGE
 					 * byte, since spi_print_msg() wants the whole thing.
 					 */
 					extended_msg[0] = EXTENDED_MESSAGE;
 					/* Accept first byte by clearing ACK */
 					NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
-					dprintk(NDEBUG_EXTENDED, "scsi%d: receiving extended message\n", HOSTNO);
+					spin_unlock_irq(&hostdata->lock);
+
+					dsprintk(NDEBUG_EXTENDED, instance, "receiving extended message\n");
 
 					len = 2;
 					data = extended_msg + 1;
 					phase = PHASE_MSGIN;
 					NCR5380_transfer_pio(instance, &phase, &len, &data);
-					dprintk(NDEBUG_EXTENDED, "scsi%d: length=%d, code=0x%02x\n", HOSTNO,
-						   (int)extended_msg[1], (int)extended_msg[2]);
+					dsprintk(NDEBUG_EXTENDED, instance, "length %d, code 0x%02x\n",
+					         (int)extended_msg[1],
+					         (int)extended_msg[2]);
 
-					if (!len && extended_msg[1] <=
-					    (sizeof(extended_msg) - 1)) {
+					if (!len && extended_msg[1] > 0 &&
+					    extended_msg[1] <= sizeof(extended_msg) - 2) {
 						/* Accept third byte by clearing ACK */
 						NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 						len = extended_msg[1] - 1;
@@ -2362,8 +2140,8 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 						phase = PHASE_MSGIN;
 
 						NCR5380_transfer_pio(instance, &phase, &len, &data);
-						dprintk(NDEBUG_EXTENDED, "scsi%d: message received, residual %d\n",
-							   HOSTNO, len);
+						dsprintk(NDEBUG_EXTENDED, instance, "message received, residual %d\n",
+						         len);
 
 						switch (extended_msg[2]) {
 						case EXTENDED_SDTR:
@@ -2373,15 +2151,18 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 							tmp = 0;
 						}
 					} else if (len) {
-						printk(KERN_NOTICE "scsi%d: error receiving "
-						       "extended message\n", HOSTNO);
+						shost_printk(KERN_ERR, instance, "error receiving extended message\n");
 						tmp = 0;
 					} else {
-						printk(KERN_NOTICE "scsi%d: extended message "
-							   "code %02x length %d is too long\n",
-							   HOSTNO, extended_msg[2], extended_msg[1]);
+						shost_printk(KERN_NOTICE, instance, "extended message code %02x length %d is too long\n",
+						             extended_msg[2], extended_msg[1]);
 						tmp = 0;
 					}
+
+					spin_lock_irq(&hostdata->lock);
+					if (!hostdata->connected)
+						return;
+
 					/* Fall through to reject message */
 
 					/*
@@ -2390,8 +2171,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 					 */
 				default:
 					if (!tmp) {
-						printk(KERN_INFO "scsi%d: rejecting message ",
-						       instance->host_no);
+						shost_printk(KERN_ERR, instance, "rejecting message ");
 						spi_print_msg(extended_msg);
 						printk("\n");
 					} else if (tmp != EXTENDED_MESSAGE)
@@ -2414,18 +2194,11 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				hostdata->last_message = msgout;
 				NCR5380_transfer_pio(instance, &phase, &len, &data);
 				if (msgout == ABORT) {
-					local_irq_save(flags);
-#ifdef SUPPORT_TAGS
-					cmd_free_tag(cmd);
-#else
-					hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
-#endif
 					hostdata->connected = NULL;
 					cmd->result = DID_ERROR << 16;
-					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
+					complete_cmd(instance, cmd);
 					maybe_release_dma_irq(instance);
-					local_irq_restore(flags);
-					cmd->scsi_done(cmd);
+					NCR5380_write(SELECT_ENABLE_REG, hostdata->id_mask);
 					return;
 				}
 				msgout = NOP;
@@ -2447,22 +2220,25 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 				cmd->SCp.Status = tmp;
 				break;
 			default:
-				printk("scsi%d: unknown phase\n", HOSTNO);
+				shost_printk(KERN_ERR, instance, "unknown phase\n");
 				NCR5380_dprint(NDEBUG_ANY, instance);
 			} /* switch(phase) */
-		} /* if (tmp * SR_REQ) */
-	} /* while (1) */
+		} else {
+			spin_unlock_irq(&hostdata->lock);
+			NCR5380_poll_politely(instance, STATUS_REG, SR_REQ, SR_REQ, HZ);
+			spin_lock_irq(&hostdata->lock);
+		}
+	}
 }
 
 /*
  * Function : void NCR5380_reselect (struct Scsi_Host *instance)
  *
  * Purpose : does reselection, initializing the instance->connected
- *	field to point to the scsi_cmnd for which the I_T_L or I_T_L_Q
- *	nexus has been reestablished,
+ * field to point to the scsi_cmnd for which the I_T_L or I_T_L_Q
+ * nexus has been reestablished,
  *
  * Inputs : instance - this instance of the NCR5380.
- *
  */
 
 
@@ -2471,7 +2247,7 @@ static void NCR5380_information_transfer(struct Scsi_Host *instance)
 
 static void NCR5380_reselect(struct Scsi_Host *instance)
 {
-	SETUP_HOSTDATA(instance);
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	unsigned char target_mask;
 	unsigned char lun;
 #ifdef SUPPORT_TAGS
@@ -2480,7 +2256,8 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	unsigned char msg[3];
 	int __maybe_unused len;
 	unsigned char __maybe_unused *data, __maybe_unused phase;
-	struct scsi_cmnd *tmp = NULL, *prev;
+	struct NCR5380_cmd *ncmd;
+	struct scsi_cmnd *tmp;
 
 	/*
 	 * Disable arbitration, etc. since the host adapter obviously
@@ -2488,11 +2265,10 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	 */
 
 	NCR5380_write(MODE_REG, MR_BASE);
-	hostdata->restart_select = 1;
 
 	target_mask = NCR5380_read(CURRENT_SCSI_DATA_REG) & ~(hostdata->id_mask);
 
-	dprintk(NDEBUG_RESELECTION, "scsi%d: reselect\n", HOSTNO);
+	dsprintk(NDEBUG_RESELECTION, instance, "reselect\n");
 
 	/*
 	 * At this point, we have detected that our SCSI ID is on the bus,
@@ -2504,17 +2280,22 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	 */
 
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_BSY);
-
-	while (NCR5380_read(STATUS_REG) & SR_SEL)
-		;
+	if (NCR5380_poll_politely(instance,
+	                          STATUS_REG, SR_SEL, 0, 2 * HZ) < 0) {
+		NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
+		return;
+	}
 	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 
 	/*
 	 * Wait for target to go into MSGIN.
 	 */
 
-	while (!(NCR5380_read(STATUS_REG) & SR_REQ))
-		;
+	if (NCR5380_poll_politely(instance,
+	                          STATUS_REG, SR_REQ, SR_REQ, 2 * HZ) < 0) {
+		do_abort(instance);
+		return;
+	}
 
 #if defined(CONFIG_SUN3) && defined(REAL_DMA)
 	/* acknowledge toggle to MSGIN */
@@ -2527,15 +2308,21 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	data = msg;
 	phase = PHASE_MSGIN;
 	NCR5380_transfer_pio(instance, &phase, &len, &data);
+
+	if (len) {
+		do_abort(instance);
+		return;
+	}
 #endif
 
 	if (!(msg[0] & 0x80)) {
-		printk(KERN_DEBUG "scsi%d: expecting IDENTIFY message, got ", HOSTNO);
+		shost_printk(KERN_ERR, instance, "expecting IDENTIFY message, got ");
 		spi_print_msg(msg);
+		printk("\n");
 		do_abort(instance);
 		return;
 	}
-	lun = (msg[0] & 0x07);
+	lun = msg[0] & 0x07;
 
 #if defined(SUPPORT_TAGS) && !defined(CONFIG_SUN3)
 	/* If the phase is still MSGIN, the target wants to send some more
@@ -2551,8 +2338,8 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 		if (!NCR5380_transfer_pio(instance, &phase, &len, &data) &&
 		    msg[1] == SIMPLE_QUEUE_TAG)
 			tag = msg[2];
-		dprintk(NDEBUG_TAGS, "scsi%d: target mask %02x, lun %d sent tag %d at "
-			   "reselection\n", HOSTNO, target_mask, lun, tag);
+		dsprintk(NDEBUG_TAGS, instance, "reselect: target mask %02x, lun %d sent tag %d\n",
+		         target_mask, lun, tag);
 	}
 #endif
 
@@ -2561,36 +2348,34 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 	 * just reestablished, and remove it from the disconnected queue.
 	 */
 
-	for (tmp = (struct scsi_cmnd *) hostdata->disconnected_queue, prev = NULL;
-	     tmp; prev = tmp, tmp = NEXT(tmp)) {
-		if ((target_mask == (1 << tmp->device->id)) && (lun == tmp->device->lun)
+	tmp = NULL;
+	list_for_each_entry(ncmd, &hostdata->disconnected, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
+
+		if (target_mask == (1 << scmd_id(cmd)) &&
+		    lun == (u8)cmd->device->lun
 #ifdef SUPPORT_TAGS
-		    && (tag == tmp->tag)
+		    && (tag == cmd->tag)
 #endif
 		    ) {
-			if (prev) {
-				REMOVE(prev, NEXT(prev), tmp, NEXT(tmp));
-				SET_NEXT(prev, NEXT(tmp));
-			} else {
-				REMOVE(-1, hostdata->disconnected_queue, tmp, NEXT(tmp));
-				hostdata->disconnected_queue = NEXT(tmp);
-			}
-			SET_NEXT(tmp, NULL);
+			list_del(&ncmd->list);
+			tmp = cmd;
 			break;
 		}
 	}
 
-	if (!tmp) {
-		printk(KERN_WARNING "scsi%d: warning: target bitmask %02x lun %d "
-#ifdef SUPPORT_TAGS
-		       "tag %d "
-#endif
-		       "not in disconnected_queue.\n",
-		       HOSTNO, target_mask, lun
+	if (tmp) {
+		dsprintk(NDEBUG_RESELECTION | NDEBUG_QUEUES, instance,
+		         "reselect: removed %p from disconnected queue\n", tmp);
+	} else {
+
 #ifdef SUPPORT_TAGS
-		       , tag
+		shost_printk(KERN_ERR, instance, "target bitmask 0x%02x lun %d tag %d not in disconnected queue.\n",
+		             target_mask, lun, tag);
+#else
+		shost_printk(KERN_ERR, instance, "target bitmask 0x%02x lun %d not in disconnected queue.\n",
+		             target_mask, lun);
 #endif
-			);
 		/*
 		 * Since we have an established nexus that we can't do anything
 		 * with, we must abort it.
@@ -2614,7 +2399,8 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 		}
 		/* setup this command for dma if not already */
 		if ((count >= DMA_MIN_SIZE) && (sun3_dma_setup_done != tmp)) {
-			sun3scsi_dma_setup(d, count, rq_data_dir(tmp->request));
+			sun3scsi_dma_setup(instance, d, count,
+			                   rq_data_dir(tmp->request));
 			sun3_dma_setup_done = tmp;
 		}
 	}
@@ -2639,235 +2425,196 @@ static void NCR5380_reselect(struct Scsi_Host *instance)
 		if (!NCR5380_transfer_pio(instance, &phase, &len, &data) &&
 		    msg[1] == SIMPLE_QUEUE_TAG)
 			tag = msg[2];
-		dprintk(NDEBUG_TAGS, "scsi%d: target mask %02x, lun %d sent tag %d at reselection\n"
-			HOSTNO, target_mask, lun, tag);
+		dsprintk(NDEBUG_TAGS, instance, "reselect: target mask %02x, lun %d sent tag %d\n"
+		         target_mask, lun, tag);
 	}
 #endif
 
 	hostdata->connected = tmp;
-	dprintk(NDEBUG_RESELECTION, "scsi%d: nexus established, target = %d, lun = %llu, tag = %d\n",
-		   HOSTNO, tmp->device->id, tmp->device->lun, tmp->tag);
+	dsprintk(NDEBUG_RESELECTION, instance, "nexus established, target %d, lun %llu, tag %d\n",
+	         scmd_id(tmp), tmp->device->lun, tmp->tag);
 }
 
 
-/*
- * Function : int NCR5380_abort (struct scsi_cmnd *cmd)
- *
- * Purpose : abort a command
- *
- * Inputs : cmd - the scsi_cmnd to abort, code - code to set the
- *	host byte of the result field to, if zero DID_ABORTED is
- *	used.
- *
- * Returns : SUCCESS - success, FAILED on failure.
- *
- * XXX - there is no way to abort the command that is currently
- *	 connected, you have to wait for it to complete.  If this is
- *	 a problem, we could implement longjmp() / setjmp(), setjmp()
- *	 called where the loop started in NCR5380_main().
+/**
+ * list_find_cmd - test for presence of a command in a linked list
+ * @haystack: list of commands
+ * @needle: command to search for
  */
 
-static
-int NCR5380_abort(struct scsi_cmnd *cmd)
+static bool list_find_cmd(struct list_head *haystack,
+                          struct scsi_cmnd *needle)
 {
-	struct Scsi_Host *instance = cmd->device->host;
-	SETUP_HOSTDATA(instance);
-	struct scsi_cmnd *tmp, **prev;
-	unsigned long flags;
+	struct NCR5380_cmd *ncmd;
 
-	scmd_printk(KERN_NOTICE, cmd, "aborting command\n");
+	list_for_each_entry(ncmd, haystack, list)
+		if (NCR5380_to_scmd(ncmd) == needle)
+			return true;
+	return false;
+}
 
-	NCR5380_print_status(instance);
+/**
+ * list_remove_cmd - remove a command from linked list
+ * @haystack: list of commands
+ * @needle: command to remove
+ */
 
-	local_irq_save(flags);
+static bool list_del_cmd(struct list_head *haystack,
+                         struct scsi_cmnd *needle)
+{
+	if (list_find_cmd(haystack, needle)) {
+		struct NCR5380_cmd *ncmd = scsi_cmd_priv(needle);
 
-	dprintk(NDEBUG_ABORT, "scsi%d: abort called basr 0x%02x, sr 0x%02x\n", HOSTNO,
-		    NCR5380_read(BUS_AND_STATUS_REG),
-		    NCR5380_read(STATUS_REG));
+		list_del(&ncmd->list);
+		return true;
+	}
+	return false;
+}
 
-#if 1
-	/*
-	 * Case 1 : If the command is the currently executing command,
-	 * we'll set the aborted flag and return control so that
-	 * information transfer routine can exit cleanly.
-	 */
+/**
+ * NCR5380_abort - scsi host eh_abort_handler() method
+ * @cmd: the command to be aborted
+ *
+ * Try to abort a given command by removing it from queues and/or sending
+ * the target an abort message. This may not succeed in causing a target
+ * to abort the command. Nonetheless, the low-level driver must forget about
+ * the command because the mid-layer reclaims it and it may be re-issued.
+ *
+ * The normal path taken by a command is as follows. For EH we trace this
+ * same path to locate and abort the command.
+ *
+ * unissued -> selecting -> [unissued -> selecting ->]... connected ->
+ * [disconnected -> connected ->]...
+ * [autosense -> connected ->] done
+ *
+ * If cmd is unissued then just remove it.
+ * If cmd is disconnected, try to select the target.
+ * If cmd is connected, try to send an abort message.
+ * If cmd is waiting for autosense, give it a chance to complete but check
+ * that it isn't left connected.
+ * If cmd was not found at all then presumably it has already been completed,
+ * in which case return SUCCESS to try to avoid further EH measures.
+ * If the command has not completed yet, we must not fail to find it.
+ */
 
-	if (hostdata->connected == cmd) {
+static int NCR5380_abort(struct scsi_cmnd *cmd)
+{
+	struct Scsi_Host *instance = cmd->device->host;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	unsigned long flags;
+	int result = SUCCESS;
 
-		dprintk(NDEBUG_ABORT, "scsi%d: aborting connected command\n", HOSTNO);
-		/*
-		 * We should perform BSY checking, and make sure we haven't slipped
-		 * into BUS FREE.
-		 */
+	spin_lock_irqsave(&hostdata->lock, flags);
 
-		/*	NCR5380_write(INITIATOR_COMMAND_REG, ICR_ASSERT_ATN); */
-		/*
-		 * Since we can't change phases until we've completed the current
-		 * handshake, we have to source or sink a byte of data if the current
-		 * phase is not MSGOUT.
-		 */
+#if (NDEBUG & NDEBUG_ANY)
+	scmd_printk(KERN_INFO, cmd, __func__);
+#endif
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
 
-		/*
-		 * Return control to the executing NCR drive so we can clear the
-		 * aborted flag and get back into our main loop.
-		 */
+	if (list_del_cmd(&hostdata->unissued, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: removed %p from issue queue\n", cmd);
+		cmd->result = DID_ABORT << 16;
+		cmd->scsi_done(cmd); /* No tag or busy flag to worry about */
+	}
 
-		if (do_abort(instance) == 0) {
-			hostdata->aborted = 1;
-			hostdata->connected = NULL;
-			cmd->result = DID_ABORT << 16;
-#ifdef SUPPORT_TAGS
-			cmd_free_tag(cmd);
-#else
-			hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
-#endif
-			maybe_release_dma_irq(instance);
-			local_irq_restore(flags);
-			cmd->scsi_done(cmd);
-			return SUCCESS;
-		} else {
-			local_irq_restore(flags);
-			printk("scsi%d: abort of connected command failed!\n", HOSTNO);
-			return FAILED;
-		}
+	if (hostdata->selecting == cmd) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: cmd %p == selecting\n", cmd);
+		hostdata->selecting = NULL;
+		cmd->result = DID_ABORT << 16;
+		complete_cmd(instance, cmd);
+		goto out;
 	}
-#endif
 
-	/*
-	 * Case 2 : If the command hasn't been issued yet, we simply remove it
-	 *	    from the issue queue.
-	 */
-	for (prev = (struct scsi_cmnd **)&(hostdata->issue_queue),
-	     tmp = (struct scsi_cmnd *)hostdata->issue_queue;
-	     tmp; prev = NEXTADDR(tmp), tmp = NEXT(tmp)) {
-		if (cmd == tmp) {
-			REMOVE(5, *prev, tmp, NEXT(tmp));
-			(*prev) = NEXT(tmp);
-			SET_NEXT(tmp, NULL);
-			tmp->result = DID_ABORT << 16;
-			maybe_release_dma_irq(instance);
-			local_irq_restore(flags);
-			dprintk(NDEBUG_ABORT, "scsi%d: abort removed command from issue queue.\n",
-				    HOSTNO);
-			/* Tagged queuing note: no tag to free here, hasn't been assigned
-			 * yet... */
-			tmp->scsi_done(tmp);
-			return SUCCESS;
+	if (list_del_cmd(&hostdata->disconnected, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: removed %p from disconnected list\n", cmd);
+		cmd->result = DID_ERROR << 16;
+		if (!hostdata->connected)
+			NCR5380_select(instance, cmd);
+		if (hostdata->connected != cmd) {
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
 		}
 	}
 
-	/*
-	 * Case 3 : If any commands are connected, we're going to fail the abort
-	 *	    and let the high level SCSI driver retry at a later time or
-	 *	    issue a reset.
-	 *
-	 *	    Timeouts, and therefore aborted commands, will be highly unlikely
-	 *          and handling them cleanly in this situation would make the common
-	 *	    case of noresets less efficient, and would pollute our code.  So,
-	 *	    we fail.
-	 */
+	if (hostdata->connected == cmd) {
+		dsprintk(NDEBUG_ABORT, instance, "abort: cmd %p is connected\n", cmd);
+		hostdata->connected = NULL;
+		if (do_abort(instance)) {
+			set_host_byte(cmd, DID_ERROR);
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
+		}
+		set_host_byte(cmd, DID_ABORT);
+#ifdef REAL_DMA
+		hostdata->dma_len = 0;
+#endif
+		if (cmd->cmnd[0] == REQUEST_SENSE)
+			complete_cmd(instance, cmd);
+		else {
+			struct NCR5380_cmd *ncmd = scsi_cmd_priv(cmd);
 
-	if (hostdata->connected) {
-		local_irq_restore(flags);
-		dprintk(NDEBUG_ABORT, "scsi%d: abort failed, command connected.\n", HOSTNO);
-		return FAILED;
+			/* Perform autosense for this command */
+			list_add(&ncmd->list, &hostdata->autosense);
+		}
 	}
 
-	/*
-	 * Case 4: If the command is currently disconnected from the bus, and
-	 *	there are no connected commands, we reconnect the I_T_L or
-	 *	I_T_L_Q nexus associated with it, go into message out, and send
-	 *      an abort message.
-	 *
-	 * This case is especially ugly. In order to reestablish the nexus, we
-	 * need to call NCR5380_select().  The easiest way to implement this
-	 * function was to abort if the bus was busy, and let the interrupt
-	 * handler triggered on the SEL for reselect take care of lost arbitrations
-	 * where necessary, meaning interrupts need to be enabled.
-	 *
-	 * When interrupts are enabled, the queues may change - so we
-	 * can't remove it from the disconnected queue before selecting it
-	 * because that could cause a failure in hashing the nexus if that
-	 * device reselected.
-	 *
-	 * Since the queues may change, we can't use the pointers from when we
-	 * first locate it.
-	 *
-	 * So, we must first locate the command, and if NCR5380_select()
-	 * succeeds, then issue the abort, relocate the command and remove
-	 * it from the disconnected queue.
-	 */
-
-	for (tmp = (struct scsi_cmnd *) hostdata->disconnected_queue; tmp;
-	     tmp = NEXT(tmp)) {
-		if (cmd == tmp) {
-			local_irq_restore(flags);
-			dprintk(NDEBUG_ABORT, "scsi%d: aborting disconnected command.\n", HOSTNO);
-
-			if (NCR5380_select(instance, cmd))
-				return FAILED;
-
-			dprintk(NDEBUG_ABORT, "scsi%d: nexus reestablished.\n", HOSTNO);
-
-			do_abort(instance);
-
-			local_irq_save(flags);
-			for (prev = (struct scsi_cmnd **)&(hostdata->disconnected_queue),
-			     tmp = (struct scsi_cmnd *)hostdata->disconnected_queue;
-			     tmp; prev = NEXTADDR(tmp), tmp = NEXT(tmp)) {
-				if (cmd == tmp) {
-					REMOVE(5, *prev, tmp, NEXT(tmp));
-					*prev = NEXT(tmp);
-					SET_NEXT(tmp, NULL);
-					tmp->result = DID_ABORT << 16;
-					/* We must unlock the tag/LUN immediately here, since the
-					 * target goes to BUS FREE and doesn't send us another
-					 * message (COMMAND_COMPLETE or the like)
-					 */
-#ifdef SUPPORT_TAGS
-					cmd_free_tag(tmp);
-#else
-					hostdata->busy[cmd->device->id] &= ~(1 << cmd->device->lun);
-#endif
-					maybe_release_dma_irq(instance);
-					local_irq_restore(flags);
-					tmp->scsi_done(tmp);
-					return SUCCESS;
-				}
-			}
+	if (list_find_cmd(&hostdata->autosense, cmd)) {
+		dsprintk(NDEBUG_ABORT, instance,
+		         "abort: found %p on sense queue\n", cmd);
+		spin_unlock_irqrestore(&hostdata->lock, flags);
+		queue_work(hostdata->work_q, &hostdata->main_task);
+		msleep(1000);
+		spin_lock_irqsave(&hostdata->lock, flags);
+		if (list_del_cmd(&hostdata->autosense, cmd)) {
+			dsprintk(NDEBUG_ABORT, instance,
+			         "abort: removed %p from sense queue\n", cmd);
+			set_host_byte(cmd, DID_ABORT);
+			complete_cmd(instance, cmd);
+			goto out;
 		}
 	}
 
-	/* Maybe it is sufficient just to release the ST-DMA lock... (if
-	 * possible at all) At least, we should check if the lock could be
-	 * released after the abort, in case it is kept due to some bug.
-	 */
-	maybe_release_dma_irq(instance);
-	local_irq_restore(flags);
+	if (hostdata->connected == cmd) {
+		dsprintk(NDEBUG_ABORT, instance, "abort: cmd %p is connected\n", cmd);
+		hostdata->connected = NULL;
+		if (do_abort(instance)) {
+			set_host_byte(cmd, DID_ERROR);
+			complete_cmd(instance, cmd);
+			result = FAILED;
+			goto out;
+		}
+		set_host_byte(cmd, DID_ABORT);
+#ifdef REAL_DMA
+		hostdata->dma_len = 0;
+#endif
+		complete_cmd(instance, cmd);
+	}
 
-	/*
-	 * Case 5 : If we reached this point, the command was not found in any of
-	 *	    the queues.
-	 *
-	 * We probably reached this point because of an unlikely race condition
-	 * between the command completing successfully and the abortion code,
-	 * so we won't panic, but we will notify the user in case something really
-	 * broke.
-	 */
+out:
+	if (result == FAILED)
+		dsprintk(NDEBUG_ABORT, instance, "abort: failed to abort %p\n", cmd);
+	else
+		dsprintk(NDEBUG_ABORT, instance, "abort: successfully aborted %p\n", cmd);
 
-	printk(KERN_INFO "scsi%d: warning : SCSI command probably completed successfully before abortion\n", HOSTNO);
+	queue_work(hostdata->work_q, &hostdata->main_task);
+	maybe_release_dma_irq(instance);
+	spin_unlock_irqrestore(&hostdata->lock, flags);
 
-	return FAILED;
+	return result;
 }
 
 
-/*
- * Function : int NCR5380_reset (struct scsi_cmnd *cmd)
- *
- * Purpose : reset the SCSI bus.
- *
- * Returns : SUCCESS or FAILURE
+/**
+ * NCR5380_bus_reset - reset the SCSI bus
+ * @cmd: SCSI command undergoing EH
  *
+ * Returns SUCCESS
  */
 
 static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
@@ -2876,23 +2623,22 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int i;
 	unsigned long flags;
+	struct NCR5380_cmd *ncmd;
 
-	NCR5380_print_status(instance);
+	spin_lock_irqsave(&hostdata->lock, flags);
+
+#if (NDEBUG & NDEBUG_ANY)
+	scmd_printk(KERN_INFO, cmd, __func__);
+#endif
+	NCR5380_dprint(NDEBUG_ANY, instance);
+	NCR5380_dprint_phase(NDEBUG_ANY, instance);
+
+	do_reset(instance);
 
-	/* get in phase */
-	NCR5380_write(TARGET_COMMAND_REG,
-		      PHASE_SR_TO_TCR(NCR5380_read(STATUS_REG)));
-	/* assert RST */
-	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST);
-	udelay(40);
 	/* reset NCR registers */
-	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
 	NCR5380_write(MODE_REG, MR_BASE);
 	NCR5380_write(TARGET_COMMAND_REG, 0);
 	NCR5380_write(SELECT_ENABLE_REG, 0);
-	/* ++roman: reset interrupt condition! otherwise no interrupts don't get
-	 * through anymore ... */
-	(void)NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 
 	/* After the reset, there are no more connected or disconnected commands
 	 * and no busy units; so clear the low-level status here to avoid
@@ -2900,17 +2646,34 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
 	 * commands!
 	 */
 
-	if (hostdata->issue_queue)
-		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted issued command(s)\n", H_NO(cmd));
-	if (hostdata->connected)
-		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted a connected command\n", H_NO(cmd));
-	if (hostdata->disconnected_queue)
-		dprintk(NDEBUG_ABORT, "scsi%d: reset aborted disconnected command(s)\n", H_NO(cmd));
+	hostdata->selecting = NULL;
+
+	list_for_each_entry(ncmd, &hostdata->disconnected, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
+
+		set_host_byte(cmd, DID_RESET);
+		cmd->scsi_done(cmd);
+	}
+
+	list_for_each_entry(ncmd, &hostdata->autosense, list) {
+		struct scsi_cmnd *cmd = NCR5380_to_scmd(ncmd);
+
+		set_host_byte(cmd, DID_RESET);
+		cmd->scsi_done(cmd);
+	}
+
+	if (hostdata->connected) {
+		set_host_byte(hostdata->connected, DID_RESET);
+		complete_cmd(instance, hostdata->connected);
+		hostdata->connected = NULL;
+	}
+
+	if (hostdata->sensing) {
+		set_host_byte(hostdata->connected, DID_RESET);
+		complete_cmd(instance, hostdata->sensing);
+		hostdata->sensing = NULL;
+	}
 
-	local_irq_save(flags);
-	hostdata->issue_queue = NULL;
-	hostdata->connected = NULL;
-	hostdata->disconnected_queue = NULL;
 #ifdef SUPPORT_TAGS
 	free_all_tags(hostdata);
 #endif
@@ -2920,8 +2683,9 @@ static int NCR5380_bus_reset(struct scsi_cmnd *cmd)
 	hostdata->dma_len = 0;
 #endif
 
+	queue_work(hostdata->work_q, &hostdata->main_task);
 	maybe_release_dma_irq(instance);
-	local_irq_restore(flags);
+	spin_unlock_irqrestore(&hostdata->lock, flags);
 
 	return SUCCESS;
 }
diff --git a/drivers/scsi/atari_scsi.c b/drivers/scsi/atari_scsi.c
index 5ede3daa93dc..78d1b2963f2c 100644
--- a/drivers/scsi/atari_scsi.c
+++ b/drivers/scsi/atari_scsi.c
@@ -66,7 +66,6 @@
 
 #include <linux/module.h>
 #include <linux/types.h>
-#include <linux/delay.h>
 #include <linux/blkdev.h>
 #include <linux/interrupt.h>
 #include <linux/init.h>
@@ -98,7 +97,6 @@
 
 #define NCR5380_queue_command           atari_scsi_queue_command
 #define NCR5380_abort                   atari_scsi_abort
-#define NCR5380_show_info               atari_scsi_show_info
 #define NCR5380_info                    atari_scsi_info
 
 #define NCR5380_dma_read_setup(instance, data, count) \
@@ -161,23 +159,10 @@ static inline unsigned long SCSI_DMA_GETADR(void)
 	return adr;
 }
 
-#define HOSTDATA_DMALEN		(((struct NCR5380_hostdata *) \
-				(atari_scsi_host->hostdata))->dma_len)
-
-/* Time (in jiffies) to wait after a reset; the SCSI standard calls for 250ms,
- * we usually do 0.5s to be on the safe side. But Toshiba CD-ROMs once more
- * need ten times the standard value... */
-#ifndef CONFIG_ATARI_SCSI_TOSHIBA_DELAY
-#define	AFTER_RESET_DELAY	(HZ/2)
-#else
-#define	AFTER_RESET_DELAY	(5*HZ/2)
-#endif
-
 #ifdef REAL_DMA
 static void atari_scsi_fetch_restbytes(void);
 #endif
 
-static struct Scsi_Host *atari_scsi_host;
 static unsigned char (*atari_scsi_reg_read)(unsigned char reg);
 static void (*atari_scsi_reg_write)(unsigned char reg, unsigned char value);
 
@@ -208,12 +193,12 @@ static int setup_cmd_per_lun = -1;
 module_param(setup_cmd_per_lun, int, 0);
 static int setup_sg_tablesize = -1;
 module_param(setup_sg_tablesize, int, 0);
-#ifdef SUPPORT_TAGS
 static int setup_use_tagged_queuing = -1;
 module_param(setup_use_tagged_queuing, int, 0);
-#endif
 static int setup_hostid = -1;
 module_param(setup_hostid, int, 0);
+static int setup_toshiba_delay = -1;
+module_param(setup_toshiba_delay, int, 0);
 
 
 #if defined(REAL_DMA)
@@ -273,15 +258,17 @@ static void scsi_dma_buserr(int irq, void *dummy)
 #endif
 
 
-static irqreturn_t scsi_tt_intr(int irq, void *dummy)
+static irqreturn_t scsi_tt_intr(int irq, void *dev)
 {
 #ifdef REAL_DMA
+	struct Scsi_Host *instance = dev;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int dma_stat;
 
 	dma_stat = tt_scsi_dma.dma_ctrl;
 
-	dprintk(NDEBUG_INTR, "scsi%d: NCR5380 interrupt, DMA status = %02x\n",
-		   atari_scsi_host->host_no, dma_stat & 0xff);
+	dsprintk(NDEBUG_INTR, instance, "NCR5380 interrupt, DMA status = %02x\n",
+	         dma_stat & 0xff);
 
 	/* Look if it was the DMA that has interrupted: First possibility
 	 * is that a bus error occurred...
@@ -304,7 +291,8 @@ static irqreturn_t scsi_tt_intr(int irq, void *dummy)
 	 * data reg!
 	 */
 	if ((dma_stat & 0x02) && !(dma_stat & 0x40)) {
-		atari_dma_residual = HOSTDATA_DMALEN - (SCSI_DMA_READ_P(dma_addr) - atari_dma_startaddr);
+		atari_dma_residual = hostdata->dma_len -
+			(SCSI_DMA_READ_P(dma_addr) - atari_dma_startaddr);
 
 		dprintk(NDEBUG_DMA, "SCSI DMA: There are %ld residual bytes.\n",
 			   atari_dma_residual);
@@ -356,15 +344,17 @@ static irqreturn_t scsi_tt_intr(int irq, void *dummy)
 
 #endif /* REAL_DMA */
 
-	NCR5380_intr(irq, dummy);
+	NCR5380_intr(irq, dev);
 
 	return IRQ_HANDLED;
 }
 
 
-static irqreturn_t scsi_falcon_intr(int irq, void *dummy)
+static irqreturn_t scsi_falcon_intr(int irq, void *dev)
 {
 #ifdef REAL_DMA
+	struct Scsi_Host *instance = dev;
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int dma_stat;
 
 	/* Turn off DMA and select sector counter register before
@@ -399,7 +389,7 @@ static irqreturn_t scsi_falcon_intr(int irq, void *dummy)
 			printk(KERN_ERR "SCSI DMA error: %ld bytes lost in "
 			       "ST-DMA fifo\n", transferred & 15);
 
-		atari_dma_residual = HOSTDATA_DMALEN - transferred;
+		atari_dma_residual = hostdata->dma_len - transferred;
 		dprintk(NDEBUG_DMA, "SCSI DMA: There are %ld residual bytes.\n",
 			   atari_dma_residual);
 	} else
@@ -411,13 +401,14 @@ static irqreturn_t scsi_falcon_intr(int irq, void *dummy)
 		 * data to the original destination address.
 		 */
 		memcpy(atari_dma_orig_addr, phys_to_virt(atari_dma_startaddr),
-		       HOSTDATA_DMALEN - atari_dma_residual);
+		       hostdata->dma_len - atari_dma_residual);
 		atari_dma_orig_addr = NULL;
 	}
 
 #endif /* REAL_DMA */
 
-	NCR5380_intr(irq, dummy);
+	NCR5380_intr(irq, dev);
+
 	return IRQ_HANDLED;
 }
 
@@ -488,7 +479,7 @@ static int __init atari_scsi_setup(char *str)
 	 * Defaults depend on TT or Falcon, determined at run time.
 	 * Negative values mean don't change.
 	 */
-	int ints[6];
+	int ints[8];
 
 	get_options(str, ARRAY_SIZE(ints), ints);
 
@@ -504,10 +495,11 @@ static int __init atari_scsi_setup(char *str)
 		setup_sg_tablesize = ints[3];
 	if (ints[0] >= 4)
 		setup_hostid = ints[4];
-#ifdef SUPPORT_TAGS
 	if (ints[0] >= 5)
 		setup_use_tagged_queuing = ints[5];
-#endif
+	/* ints[6] (use_pdma) is ignored */
+	if (ints[0] >= 7)
+		setup_toshiba_delay = ints[7];
 
 	return 1;
 }
@@ -516,38 +508,6 @@ __setup("atascsi=", atari_scsi_setup);
 #endif /* !MODULE */
 
 
-#ifdef CONFIG_ATARI_SCSI_RESET_BOOT
-static void __init atari_scsi_reset_boot(void)
-{
-	unsigned long end;
-
-	/*
-	 * Do a SCSI reset to clean up the bus during initialization. No messing
-	 * with the queues, interrupts, or locks necessary here.
-	 */
-
-	printk("Atari SCSI: resetting the SCSI bus...");
-
-	/* get in phase */
-	NCR5380_write(TARGET_COMMAND_REG,
-		      PHASE_SR_TO_TCR(NCR5380_read(STATUS_REG)));
-
-	/* assert RST */
-	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST);
-	/* The min. reset hold time is 25us, so 40us should be enough */
-	udelay(50);
-	/* reset RST and interrupt */
-	NCR5380_write(INITIATOR_COMMAND_REG, ICR_BASE);
-	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-
-	end = jiffies + AFTER_RESET_DELAY;
-	while (time_before(jiffies, end))
-		barrier();
-
-	printk(" done\n");
-}
-#endif
-
 #if defined(REAL_DMA)
 
 static unsigned long atari_scsi_dma_setup(struct Scsi_Host *instance,
@@ -815,14 +775,14 @@ static int atari_scsi_bus_reset(struct scsi_cmnd *cmd)
 static struct scsi_host_template atari_scsi_template = {
 	.module			= THIS_MODULE,
 	.proc_name		= DRV_MODULE_NAME,
-	.show_info		= atari_scsi_show_info,
 	.name			= "Atari native SCSI",
 	.info			= atari_scsi_info,
 	.queuecommand		= atari_scsi_queue_command,
 	.eh_abort_handler	= atari_scsi_abort,
 	.eh_bus_reset_handler	= atari_scsi_bus_reset,
 	.this_id		= 7,
-	.use_clustering		= DISABLE_CLUSTERING
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
 };
 
 static int __init atari_scsi_probe(struct platform_device *pdev)
@@ -880,7 +840,7 @@ static int __init atari_scsi_probe(struct platform_device *pdev)
 	} else {
 		/* Test if a host id is set in the NVRam */
 		if (ATARIHW_PRESENT(TT_CLK) && nvram_check_checksum()) {
-			unsigned char b = nvram_read_byte(14);
+			unsigned char b = nvram_read_byte(16);
 
 			/* Arbitration enabled? (for TOS)
 			 * If yes, use configured host ID
@@ -915,21 +875,18 @@ static int __init atari_scsi_probe(struct platform_device *pdev)
 		error = -ENOMEM;
 		goto fail_alloc;
 	}
-	atari_scsi_host = instance;
-
-#ifdef CONFIG_ATARI_SCSI_RESET_BOOT
-	atari_scsi_reset_boot();
-#endif
 
 	instance->irq = irq->start;
 
 	host_flags |= IS_A_TT() ? 0 : FLAG_LATE_DMA_SETUP;
-
 #ifdef SUPPORT_TAGS
 	host_flags |= setup_use_tagged_queuing > 0 ? FLAG_TAGGED_QUEUING : 0;
 #endif
+	host_flags |= setup_toshiba_delay > 0 ? FLAG_TOSHIBA_DELAY : 0;
 
-	NCR5380_init(instance, host_flags);
+	error = NCR5380_init(instance, host_flags);
+	if (error)
+		goto fail_init;
 
 	if (IS_A_TT()) {
 		error = request_irq(instance->irq, scsi_tt_intr, 0,
@@ -975,6 +932,8 @@ static int __init atari_scsi_probe(struct platform_device *pdev)
 #endif
 	}
 
+	NCR5380_maybe_reset_bus(instance);
+
 	error = scsi_add_host(instance, NULL);
 	if (error)
 		goto fail_host;
@@ -989,6 +948,7 @@ fail_host:
 		free_irq(instance->irq, instance);
 fail_irq:
 	NCR5380_exit(instance);
+fail_init:
 	scsi_host_put(instance);
 fail_alloc:
 	if (atari_dma_buffer)
diff --git a/drivers/scsi/atp870u.c b/drivers/scsi/atp870u.c
index 05301bc752ee..8b52a9dbb9cf 100644
--- a/drivers/scsi/atp870u.c
+++ b/drivers/scsi/atp870u.c
@@ -41,77 +41,128 @@
 
 static struct scsi_host_template atp870u_template;
 static void send_s870(struct atp_unit *dev,unsigned char c);
-static void is885(struct atp_unit *dev, unsigned int wkport,unsigned char c);
-static void tscam_885(void);
+static void atp_is(struct atp_unit *dev, unsigned char c, bool wide_chip, unsigned char lvdmode);
+
+static inline void atp_writeb_base(struct atp_unit *atp, u8 reg, u8 val)
+{
+	outb(val, atp->baseport + reg);
+}
+
+static inline void atp_writew_base(struct atp_unit *atp, u8 reg, u16 val)
+{
+	outw(val, atp->baseport + reg);
+}
+
+static inline void atp_writeb_io(struct atp_unit *atp, u8 channel, u8 reg, u8 val)
+{
+	outb(val, atp->ioport[channel] + reg);
+}
+
+static inline void atp_writew_io(struct atp_unit *atp, u8 channel, u8 reg, u16 val)
+{
+	outw(val, atp->ioport[channel] + reg);
+}
+
+static inline void atp_writeb_pci(struct atp_unit *atp, u8 channel, u8 reg, u8 val)
+{
+	outb(val, atp->pciport[channel] + reg);
+}
+
+static inline void atp_writel_pci(struct atp_unit *atp, u8 channel, u8 reg, u32 val)
+{
+	outl(val, atp->pciport[channel] + reg);
+}
+
+static inline u8 atp_readb_base(struct atp_unit *atp, u8 reg)
+{
+	return inb(atp->baseport + reg);
+}
+
+static inline u16 atp_readw_base(struct atp_unit *atp, u8 reg)
+{
+	return inw(atp->baseport + reg);
+}
+
+static inline u32 atp_readl_base(struct atp_unit *atp, u8 reg)
+{
+	return inl(atp->baseport + reg);
+}
+
+static inline u8 atp_readb_io(struct atp_unit *atp, u8 channel, u8 reg)
+{
+	return inb(atp->ioport[channel] + reg);
+}
+
+static inline u16 atp_readw_io(struct atp_unit *atp, u8 channel, u8 reg)
+{
+	return inw(atp->ioport[channel] + reg);
+}
+
+static inline u8 atp_readb_pci(struct atp_unit *atp, u8 channel, u8 reg)
+{
+	return inb(atp->pciport[channel] + reg);
+}
+
+static inline bool is880(struct atp_unit *atp)
+{
+	return atp->pdev->device == ATP880_DEVID1 ||
+	       atp->pdev->device == ATP880_DEVID2;
+}
+
+static inline bool is885(struct atp_unit *atp)
+{
+	return atp->pdev->device == ATP885_DEVID;
+}
 
 static irqreturn_t atp870u_intr_handle(int irq, void *dev_id)
 {
 	unsigned long flags;
-	unsigned short int tmpcip, id;
+	unsigned short int id;
 	unsigned char i, j, c, target_id, lun,cmdp;
 	unsigned char *prd;
 	struct scsi_cmnd *workreq;
-	unsigned int workport, tmport, tmport1;
 	unsigned long adrcnt, k;
 #ifdef ED_DBGP
 	unsigned long l;
 #endif
-	int errstus;
 	struct Scsi_Host *host = dev_id;
 	struct atp_unit *dev = (struct atp_unit *)&host->hostdata;
 
 	for (c = 0; c < 2; c++) {
-		tmport = dev->ioport[c] + 0x1f;
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x1f);
 		if ((j & 0x80) != 0)
-		{			
-	   		goto ch_sel;
-		}
+			break;
 		dev->in_int[c] = 0;
 	}
-	return IRQ_NONE;
-ch_sel:
+	if ((j & 0x80) == 0)
+		return IRQ_NONE;
 #ifdef ED_DBGP	
 	printk("atp870u_intr_handle enter\n");
 #endif	
 	dev->in_int[c] = 1;
-	cmdp = inb(dev->ioport[c] + 0x10);
-	workport = dev->ioport[c];
+	cmdp = atp_readb_io(dev, c, 0x10);
 	if (dev->working[c] != 0) {
-		if (dev->dev_id == ATP885_DEVID) {
-			tmport1 = workport + 0x16;
-			if ((inb(tmport1) & 0x80) == 0)
-				outb((inb(tmport1) | 0x80), tmport1);
+		if (is885(dev)) {
+			if ((atp_readb_io(dev, c, 0x16) & 0x80) == 0)
+				atp_writeb_io(dev, c, 0x16, (atp_readb_io(dev, c, 0x16) | 0x80));
 		}		
-		tmpcip = dev->pciport[c];
-		if ((inb(tmpcip) & 0x08) != 0)
+		if ((atp_readb_pci(dev, c, 0x00) & 0x08) != 0)
 		{
-			tmpcip += 0x2;
 			for (k=0; k < 1000; k++) {
-				if ((inb(tmpcip) & 0x08) == 0) {
-					goto stop_dma;
-				}
-				if ((inb(tmpcip) & 0x01) == 0) {
-					goto stop_dma;
-				}
+				if ((atp_readb_pci(dev, c, 2) & 0x08) == 0)
+					break;
+				if ((atp_readb_pci(dev, c, 2) & 0x01) == 0)
+					break;
 			}
 		}
-stop_dma:
-		tmpcip = dev->pciport[c];
-		outb(0x00, tmpcip);
-		tmport -= 0x08;
+		atp_writeb_pci(dev, c, 0, 0x00);
 		
-		i = inb(tmport);
+		i = atp_readb_io(dev, c, 0x17);
 		
-		if (dev->dev_id == ATP885_DEVID) {
-			tmpcip += 2;
-			outb(0x06, tmpcip);
-			tmpcip -= 2;
-		}
+		if (is885(dev))
+			atp_writeb_pci(dev, c, 2, 0x06);
 
-		tmport -= 0x02;
-		target_id = inb(tmport);
-		tmport += 0x02;
+		target_id = atp_readb_io(dev, c, 0x15);
 
 		/*
 		 *	Remap wide devices onto id numbers
@@ -129,7 +180,7 @@ stop_dma:
 		     }
 		     dev->last_cmd[c] |= 0x40;
 		}
-		if (dev->dev_id == ATP885_DEVID) 
+		if (is885(dev))
 			dev->r1f[c][target_id] |= j;
 #ifdef ED_DBGP
 		printk("atp870u_intr_handle status = %x\n",i);
@@ -138,12 +189,11 @@ stop_dma:
 			if ((dev->last_cmd[c] & 0xf0) != 0x40) {
 			   dev->last_cmd[c] = 0xff;
 			}
-			if (dev->dev_id == ATP885_DEVID) {
-				tmport -= 0x05;
+			if (is885(dev)) {
 				adrcnt = 0;
-				((unsigned char *) &adrcnt)[2] = inb(tmport++);
-				((unsigned char *) &adrcnt)[1] = inb(tmport++);
-				((unsigned char *) &adrcnt)[0] = inb(tmport);
+				((unsigned char *) &adrcnt)[2] = atp_readb_io(dev, c, 0x12);
+				((unsigned char *) &adrcnt)[1] = atp_readb_io(dev, c, 0x13);
+				((unsigned char *) &adrcnt)[0] = atp_readb_io(dev, c, 0x14);
 				if (dev->id[c][target_id].last_len != adrcnt)
 				{
 			   		k = dev->id[c][target_id].last_len;
@@ -152,7 +202,7 @@ stop_dma:
 			   	dev->id[c][target_id].last_len = adrcnt;			   
 				}
 #ifdef ED_DBGP
-				printk("tmport = %x dev->id[c][target_id].last_len = %d dev->id[c][target_id].tran_len = %d\n",tmport,dev->id[c][target_id].last_len,dev->id[c][target_id].tran_len);
+				printk("dev->id[c][target_id].last_len = %d dev->id[c][target_id].tran_len = %d\n",dev->id[c][target_id].last_len,dev->id[c][target_id].tran_len);
 #endif		
 			}
 
@@ -160,11 +210,9 @@ stop_dma:
 			 *      Flip wide
 			 */			
 			if (dev->wide_id[c] != 0) {
-				tmport = workport + 0x1b;
-				outb(0x01, tmport);
-				while ((inb(tmport) & 0x01) != 0x01) {
-					outb(0x01, tmport);
-				}
+				atp_writeb_io(dev, c, 0x1b, 0x01);
+				while ((atp_readb_io(dev, c, 0x1b) & 0x01) != 0x01)
+					atp_writeb_io(dev, c, 0x1b, 0x01);
 			}		
 			/*
 			 *	Issue more commands
@@ -185,37 +233,34 @@ stop_dma:
 #ifdef ED_DBGP
 				printk("Status 0x85 return\n");
 #endif				
-			goto handled;
+			return IRQ_HANDLED;
 		}
 
 		if (i == 0x40) {
 		     dev->last_cmd[c] |= 0x40;
 		     dev->in_int[c] = 0;
-		     goto handled;
+		     return IRQ_HANDLED;
 		}
 
 		if (i == 0x21) {
 			if ((dev->last_cmd[c] & 0xf0) != 0x40) {
 			   dev->last_cmd[c] = 0xff;
 			}
-			tmport -= 0x05;
 			adrcnt = 0;
-			((unsigned char *) &adrcnt)[2] = inb(tmport++);
-			((unsigned char *) &adrcnt)[1] = inb(tmport++);
-			((unsigned char *) &adrcnt)[0] = inb(tmport);
+			((unsigned char *) &adrcnt)[2] = atp_readb_io(dev, c, 0x12);
+			((unsigned char *) &adrcnt)[1] = atp_readb_io(dev, c, 0x13);
+			((unsigned char *) &adrcnt)[0] = atp_readb_io(dev, c, 0x14);
 			k = dev->id[c][target_id].last_len;
 			k -= adrcnt;
 			dev->id[c][target_id].tran_len = k;
 			dev->id[c][target_id].last_len = adrcnt;
-			tmport -= 0x04;
-			outb(0x41, tmport);
-			tmport += 0x08;
-			outb(0x08, tmport);
+			atp_writeb_io(dev, c, 0x10, 0x41);
+			atp_writeb_io(dev, c, 0x18, 0x08);
 			dev->in_int[c] = 0;
-			goto handled;
+			return IRQ_HANDLED;
 		}
 
-		if (dev->dev_id == ATP885_DEVID) {
+		if (is885(dev)) {
 			if ((i == 0x4c) || (i == 0x4d) || (i == 0x8c) || (i == 0x8d)) {
 		   		if ((i == 0x4c) || (i == 0x8c)) 
 		      			i=0x48;
@@ -229,11 +274,9 @@ stop_dma:
 			printk(KERN_DEBUG "Device reselect\n");
 #endif			
 			lun = 0;
-			tmport -= 0x07;
-			if (cmdp == 0x44 || i==0x80) {
-				tmport += 0x0d;
-				lun = inb(tmport) & 0x07;
-			} else {
+			if (cmdp == 0x44 || i == 0x80)
+				lun = atp_readb_io(dev, c, 0x1d) & 0x07;
+			else {
 				if ((dev->last_cmd[c] & 0xf0) != 0x40) {
 				   dev->last_cmd[c] = 0xff;
 				}
@@ -241,49 +284,41 @@ stop_dma:
 #ifdef ED_DBGP
 					printk("cmdp = 0x41\n");
 #endif						
-					tmport += 0x02;
 					adrcnt = 0;
-					((unsigned char *) &adrcnt)[2] = inb(tmport++);
-					((unsigned char *) &adrcnt)[1] = inb(tmport++);
-					((unsigned char *) &adrcnt)[0] = inb(tmport);
+					((unsigned char *) &adrcnt)[2] = atp_readb_io(dev, c, 0x12);
+					((unsigned char *) &adrcnt)[1] = atp_readb_io(dev, c, 0x13);
+					((unsigned char *) &adrcnt)[0] = atp_readb_io(dev, c, 0x14);
 					k = dev->id[c][target_id].last_len;
 					k -= adrcnt;
 					dev->id[c][target_id].tran_len = k;
 					dev->id[c][target_id].last_len = adrcnt;
-					tmport += 0x04;
-					outb(0x08, tmport);
+					atp_writeb_io(dev, c, 0x18, 0x08);
 					dev->in_int[c] = 0;
-					goto handled;
+					return IRQ_HANDLED;
 				} else {
 #ifdef ED_DBGP
 					printk("cmdp != 0x41\n");
 #endif						
-					outb(0x46, tmport);
+					atp_writeb_io(dev, c, 0x10, 0x46);
 					dev->id[c][target_id].dirct = 0x00;
-					tmport += 0x02;
-					outb(0x00, tmport++);
-					outb(0x00, tmport++);
-					outb(0x00, tmport++);
-					tmport += 0x03;
-					outb(0x08, tmport);
+					atp_writeb_io(dev, c, 0x12, 0x00);
+					atp_writeb_io(dev, c, 0x13, 0x00);
+					atp_writeb_io(dev, c, 0x14, 0x00);
+					atp_writeb_io(dev, c, 0x18, 0x08);
 					dev->in_int[c] = 0;
-					goto handled;
+					return IRQ_HANDLED;
 				}
 			}
 			if (dev->last_cmd[c] != 0xff) {
 			   dev->last_cmd[c] |= 0x40;
 			}
-			if (dev->dev_id == ATP885_DEVID) {
-				j = inb(dev->baseport + 0x29) & 0xfe;
-				outb(j, dev->baseport + 0x29);
-				tmport = workport + 0x16;
-			} else {
-				tmport = workport + 0x10;
-				outb(0x45, tmport);
-				tmport += 0x06;				
-			}
-			
-			target_id = inb(tmport);
+			if (is885(dev)) {
+				j = atp_readb_base(dev, 0x29) & 0xfe;
+				atp_writeb_base(dev, 0x29, j);
+			} else
+				atp_writeb_io(dev, c, 0x10, 0x45);
+
+			target_id = atp_readb_io(dev, c, 0x16);
 			/*
 			 *	Remap wide identifiers
 			 */
@@ -292,10 +327,8 @@ stop_dma:
 			} else {
 				target_id &= 0x07;
 			}
-			if (dev->dev_id == ATP885_DEVID) {
-				tmport = workport + 0x10;
-				outb(0x45, tmport);
-			}
+			if (is885(dev))
+				atp_writeb_io(dev, c, 0x10, 0x45);
 			workreq = dev->id[c][target_id].curr_req;
 #ifdef ED_DBGP			
 			scmd_printk(KERN_DEBUG, workreq, "CDB");
@@ -304,18 +337,16 @@ stop_dma:
 			printk("\n");
 #endif	
 			
-			tmport = workport + 0x0f;
-			outb(lun, tmport);
-			tmport += 0x02;
-			outb(dev->id[c][target_id].devsp, tmport++);
+			atp_writeb_io(dev, c, 0x0f, lun);
+			atp_writeb_io(dev, c, 0x11, dev->id[c][target_id].devsp);
 			adrcnt = dev->id[c][target_id].tran_len;
 			k = dev->id[c][target_id].last_len;
 
-			outb(((unsigned char *) &k)[2], tmport++);
-			outb(((unsigned char *) &k)[1], tmport++);
-			outb(((unsigned char *) &k)[0], tmport++);
+			atp_writeb_io(dev, c, 0x12, ((unsigned char *) &k)[2]);
+			atp_writeb_io(dev, c, 0x13, ((unsigned char *) &k)[1]);
+			atp_writeb_io(dev, c, 0x14, ((unsigned char *) &k)[0]);
 #ifdef ED_DBGP			
-			printk("k %x, k[0] 0x%x k[1] 0x%x k[2] 0x%x\n", k, inb(tmport-1), inb(tmport-2), inb(tmport-3));
+			printk("k %x, k[0] 0x%x k[1] 0x%x k[2] 0x%x\n", k, atp_readb_io(dev, c, 0x14), atp_readb_io(dev, c, 0x13), atp_readb_io(dev, c, 0x12));
 #endif			
 			/* Remap wide */
 			j = target_id;
@@ -324,35 +355,28 @@ stop_dma:
 			}
 			/* Add direction */
 			j |= dev->id[c][target_id].dirct;
-			outb(j, tmport++);
-			outb(0x80,tmport);
+			atp_writeb_io(dev, c, 0x15, j);
+			atp_writeb_io(dev, c, 0x16, 0x80);
 			
 			/* enable 32 bit fifo transfer */	
-			if (dev->dev_id == ATP885_DEVID) {
-				tmpcip = dev->pciport[c] + 1;
-				i=inb(tmpcip) & 0xf3;
+			if (is885(dev)) {
+				i = atp_readb_pci(dev, c, 1) & 0xf3;
 				//j=workreq->cmnd[0];	    		    	
 				if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
 				   i |= 0x0c;
 				}
-				outb(i,tmpcip);		    		    		
-			} else if ((dev->dev_id == ATP880_DEVID1) ||
-	    		    	   (dev->dev_id == ATP880_DEVID2) ) {
-				tmport = workport - 0x05;
-				if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
-					outb((unsigned char) ((inb(tmport) & 0x3f) | 0xc0), tmport);
-				} else {
-					outb((unsigned char) (inb(tmport) & 0x3f), tmport);
-				}
+				atp_writeb_pci(dev, c, 1, i);
+			} else if (is880(dev)) {
+				if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a))
+					atp_writeb_base(dev, 0x3b, (atp_readb_base(dev, 0x3b) & 0x3f) | 0xc0);
+				else
+					atp_writeb_base(dev, 0x3b, atp_readb_base(dev, 0x3b) & 0x3f);
 			} else {				
-				tmport = workport + 0x3a;
-				if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
-					outb((unsigned char) ((inb(tmport) & 0xf3) | 0x08), tmport);
-				} else {
-					outb((unsigned char) (inb(tmport) & 0xf3), tmport);
-				}														
+				if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a))
+					atp_writeb_base(dev, 0x3a, (atp_readb_base(dev, 0x3a) & 0xf3) | 0x08);
+				else
+					atp_writeb_base(dev, 0x3a, atp_readb_base(dev, 0x3a) & 0xf3);
 			}	
-			tmport = workport + 0x1b;
 			j = 0;
 			id = 1;
 			id = id << target_id;
@@ -362,18 +386,16 @@ stop_dma:
 			if ((id & dev->wide_id[c]) != 0) {
 				j |= 0x01;
 			}
-			outb(j, tmport);
-			while ((inb(tmport) & 0x01) != j) {
-				outb(j,tmport);
-			}
+			atp_writeb_io(dev, c, 0x1b, j);
+			while ((atp_readb_io(dev, c, 0x1b) & 0x01) != j)
+				atp_writeb_io(dev, c, 0x1b, j);
 			if (dev->id[c][target_id].last_len == 0) {
-				tmport = workport + 0x18;
-				outb(0x08, tmport);
+				atp_writeb_io(dev, c, 0x18, 0x08);
 				dev->in_int[c] = 0;
 #ifdef ED_DBGP
 				printk("dev->id[c][target_id].last_len = 0\n");
 #endif					
-				goto handled;
+				return IRQ_HANDLED;
 			}
 #ifdef ED_DBGP
 			printk("target_id = %d adrcnt = %d\n",target_id,adrcnt);
@@ -401,39 +423,33 @@ stop_dma:
 					}
 				}				
 			}
-			tmpcip = dev->pciport[c] + 0x04;
-			outl(dev->id[c][target_id].prdaddr, tmpcip);
+			atp_writel_pci(dev, c, 0x04, dev->id[c][target_id].prdaddr);
 #ifdef ED_DBGP
 			printk("dev->id[%d][%d].prdaddr 0x%8x\n", c, target_id, dev->id[c][target_id].prdaddr);
 #endif
-			if (dev->dev_id == ATP885_DEVID) {
-				tmpcip -= 0x04;
-			} else {
-				tmpcip -= 0x02;
-				outb(0x06, tmpcip);
-				outb(0x00, tmpcip);
-				tmpcip -= 0x02;
+			if (!is885(dev)) {
+				atp_writeb_pci(dev, c, 2, 0x06);
+				atp_writeb_pci(dev, c, 2, 0x00);
 			}
-			tmport = workport + 0x18;
 			/*
 			 *	Check transfer direction
 			 */
 			if (dev->id[c][target_id].dirct != 0) {
-				outb(0x08, tmport);
-				outb(0x01, tmpcip);
+				atp_writeb_io(dev, c, 0x18, 0x08);
+				atp_writeb_pci(dev, c, 0, 0x01);
 				dev->in_int[c] = 0;
 #ifdef ED_DBGP
 				printk("status 0x80 return dirct != 0\n");
 #endif				
-				goto handled;
+				return IRQ_HANDLED;
 			}
-			outb(0x08, tmport);
-			outb(0x09, tmpcip);
+			atp_writeb_io(dev, c, 0x18, 0x08);
+			atp_writeb_pci(dev, c, 0, 0x09);
 			dev->in_int[c] = 0;
 #ifdef ED_DBGP
 			printk("status 0x80 return dirct = 0\n");
 #endif			
-			goto handled;
+			return IRQ_HANDLED;
 		}
 
 		/*
@@ -442,31 +458,22 @@ stop_dma:
 
 		workreq = dev->id[c][target_id].curr_req;
 
-		if (i == 0x42) {
-			if ((dev->last_cmd[c] & 0xf0) != 0x40)
-			{
-			   dev->last_cmd[c] = 0xff;
-			}
-			errstus = 0x02;
-			workreq->result = errstus;
-			goto go_42;
-		}
-		if (i == 0x16) {
+		if (i == 0x42 || i == 0x16) {
 			if ((dev->last_cmd[c] & 0xf0) != 0x40) {
 			   dev->last_cmd[c] = 0xff;
 			}
-			errstus = 0;
-			tmport -= 0x08;
-			errstus = inb(tmport);
-			if (((dev->r1f[c][target_id] & 0x10) != 0)&&(dev->dev_id==ATP885_DEVID)) {
-			   printk(KERN_WARNING "AEC67162 CRC ERROR !\n");
-			   errstus = 0x02;
-			}
-			workreq->result = errstus;
-go_42:
-			if (dev->dev_id == ATP885_DEVID) {		
-				j = inb(dev->baseport + 0x29) | 0x01;
-				outb(j, dev->baseport + 0x29);
+			if (i == 0x16) {
+				workreq->result = atp_readb_io(dev, c, 0x0f);
+				if (((dev->r1f[c][target_id] & 0x10) != 0) && is885(dev)) {
+					printk(KERN_WARNING "AEC67162 CRC ERROR !\n");
+					workreq->result = 0x02;
+				}
+			} else
+				workreq->result = 0x02;
+
+			if (is885(dev)) {
+				j = atp_readb_base(dev, 0x29) | 0x01;
+				atp_writeb_base(dev, 0x29, j);
 			}
 			/*
 			 *	Complete the command
@@ -488,11 +495,9 @@ go_42:
 			 *      Take it back wide
 			 */
 			if (dev->wide_id[c] != 0) {
-				tmport = workport + 0x1b;
-				outb(0x01, tmport);
-				while ((inb(tmport) & 0x01) != 0x01) {
-					outb(0x01, tmport);
-				}       
+				atp_writeb_io(dev, c, 0x1b, 0x01);
+				while ((atp_readb_io(dev, c, 0x1b) & 0x01) != 0x01)
+					atp_writeb_io(dev, c, 0x1b, 0x01);
 			} 
 			/*
 			 *	If there is stuff to send and nothing going then send it
@@ -507,7 +512,7 @@ go_42:
 			}
 			spin_unlock_irqrestore(dev->host->host_lock, flags);
 			dev->in_int[c] = 0;
-			goto handled;
+			return IRQ_HANDLED;
 		}
 		if ((dev->last_cmd[c] & 0xf0) != 0x40) {
 		   dev->last_cmd[c] = 0xff;
@@ -517,84 +522,54 @@ go_42:
 		}
 		i &= 0x0f;
 		if (i == 0x09) {
-			tmpcip += 4;
-			outl(dev->id[c][target_id].prdaddr, tmpcip);
-			tmpcip = tmpcip - 2;
-			outb(0x06, tmpcip);
-			outb(0x00, tmpcip);
-			tmpcip = tmpcip - 2;
-			tmport = workport + 0x10;
-			outb(0x41, tmport);
-			if (dev->dev_id == ATP885_DEVID) {
-				tmport += 2;
+			atp_writel_pci(dev, c, 4, dev->id[c][target_id].prdaddr);
+			atp_writeb_pci(dev, c, 2, 0x06);
+			atp_writeb_pci(dev, c, 2, 0x00);
+			atp_writeb_io(dev, c, 0x10, 0x41);
+			if (is885(dev)) {
 				k = dev->id[c][target_id].last_len;
-				outb((unsigned char) (((unsigned char *) (&k))[2]), tmport++);
-				outb((unsigned char) (((unsigned char *) (&k))[1]), tmport++);
-				outb((unsigned char) (((unsigned char *) (&k))[0]), tmport);
+				atp_writeb_io(dev, c, 0x12, ((unsigned char *) (&k))[2]);
+				atp_writeb_io(dev, c, 0x13, ((unsigned char *) (&k))[1]);
+				atp_writeb_io(dev, c, 0x14, ((unsigned char *) (&k))[0]);
 				dev->id[c][target_id].dirct = 0x00;
-				tmport += 0x04;
 			} else {
 				dev->id[c][target_id].dirct = 0x00;
-				tmport += 0x08;				
 			}
-			outb(0x08, tmport);
-			outb(0x09, tmpcip);
+			atp_writeb_io(dev, c, 0x18, 0x08);
+			atp_writeb_pci(dev, c, 0, 0x09);
 			dev->in_int[c] = 0;
-			goto handled;
+			return IRQ_HANDLED;
 		}
 		if (i == 0x08) {
-			tmpcip += 4;
-			outl(dev->id[c][target_id].prdaddr, tmpcip);
-			tmpcip = tmpcip - 2;
-			outb(0x06, tmpcip);
-			outb(0x00, tmpcip);
-			tmpcip = tmpcip - 2;
-			tmport = workport + 0x10;
-			outb(0x41, tmport);
-			if (dev->dev_id == ATP885_DEVID) {		
-				tmport += 2;
+			atp_writel_pci(dev, c, 4, dev->id[c][target_id].prdaddr);
+			atp_writeb_pci(dev, c, 2, 0x06);
+			atp_writeb_pci(dev, c, 2, 0x00);
+			atp_writeb_io(dev, c, 0x10, 0x41);
+			if (is885(dev)) {
 				k = dev->id[c][target_id].last_len;
-				outb((unsigned char) (((unsigned char *) (&k))[2]), tmport++);
-				outb((unsigned char) (((unsigned char *) (&k))[1]), tmport++);
-				outb((unsigned char) (((unsigned char *) (&k))[0]), tmport++);
-			} else {
-				tmport += 5;
+				atp_writeb_io(dev, c, 0x12, ((unsigned char *) (&k))[2]);
+				atp_writeb_io(dev, c, 0x13, ((unsigned char *) (&k))[1]);
+				atp_writeb_io(dev, c, 0x14, ((unsigned char *) (&k))[0]);
 			}
-			outb((unsigned char) (inb(tmport) | 0x20), tmport);
+			atp_writeb_io(dev, c, 0x15, atp_readb_io(dev, c, 0x15) | 0x20);
 			dev->id[c][target_id].dirct = 0x20;
-			tmport += 0x03;
-			outb(0x08, tmport);
-			outb(0x01, tmpcip);
+			atp_writeb_io(dev, c, 0x18, 0x08);
+			atp_writeb_pci(dev, c, 0, 0x01);
 			dev->in_int[c] = 0;
-			goto handled;
-		}
-		tmport -= 0x07;
-		if (i == 0x0a) {
-			outb(0x30, tmport);
-		} else {
-			outb(0x46, tmport);
+			return IRQ_HANDLED;
 		}
+		if (i == 0x0a)
+			atp_writeb_io(dev, c, 0x10, 0x30);
+		else
+			atp_writeb_io(dev, c, 0x10, 0x46);
 		dev->id[c][target_id].dirct = 0x00;
-		tmport += 0x02;
-		outb(0x00, tmport++);
-		outb(0x00, tmport++);
-		outb(0x00, tmport++);
-		tmport += 0x03;
-		outb(0x08, tmport);
-		dev->in_int[c] = 0;
-		goto handled;
-	} else {
-//		tmport = workport + 0x17;
-//		inb(tmport);
-//		dev->working[c] = 0;
-		dev->in_int[c] = 0;
-		goto handled;
+		atp_writeb_io(dev, c, 0x12, 0x00);
+		atp_writeb_io(dev, c, 0x13, 0x00);
+		atp_writeb_io(dev, c, 0x14, 0x00);
+		atp_writeb_io(dev, c, 0x18, 0x08);
 	}
-	
-handled:
-#ifdef ED_DBGP
-	printk("atp870u_intr_handle exit\n");
-#endif			
+	dev->in_int[c] = 0;
+
 	return IRQ_HANDLED;
 }
 /**
@@ -608,7 +583,7 @@ static int atp870u_queuecommand_lck(struct scsi_cmnd *req_p,
 			 void (*done) (struct scsi_cmnd *))
 {
 	unsigned char c;
-	unsigned int tmport,m;	
+	unsigned int m;
 	struct atp_unit *dev;
 	struct Scsi_Host *host;
 
@@ -677,11 +652,10 @@ static int atp870u_queuecommand_lck(struct scsi_cmnd *req_p,
 		return 0;
 	}
 	dev->quereq[c][dev->quend[c]] = req_p;
-	tmport = dev->ioport[c] + 0x1c;
 #ifdef ED_DBGP	
-	printk("dev->ioport[c] = %x inb(tmport) = %x dev->in_int[%d] = %d dev->in_snd[%d] = %d\n",dev->ioport[c],inb(tmport),c,dev->in_int[c],c,dev->in_snd[c]);
+	printk("dev->ioport[c] = %x atp_readb_io(dev, c, 0x1c) = %x dev->in_int[%d] = %d dev->in_snd[%d] = %d\n",dev->ioport[c],atp_readb_io(dev, c, 0x1c),c,dev->in_int[c],c,dev->in_snd[c]);
 #endif
-	if ((inb(tmport) == 0) && (dev->in_int[c] == 0) && (dev->in_snd[c] == 0)) {
+	if ((atp_readb_io(dev, c, 0x1c) == 0) && (dev->in_int[c] == 0) && (dev->in_snd[c] == 0)) {
 #ifdef ED_DBGP
 		printk("Call sent_s870(atp870u_queuecommand)\n");
 #endif		
@@ -706,14 +680,12 @@ static DEF_SCSI_QCMD(atp870u_queuecommand)
  */
 static void send_s870(struct atp_unit *dev,unsigned char c)
 {
-	unsigned int tmport;
-	struct scsi_cmnd *workreq;
+	struct scsi_cmnd *workreq = NULL;
 	unsigned int i;//,k;
 	unsigned char  j, target_id;
 	unsigned char *prd;
-	unsigned short int tmpcip, w;
+	unsigned short int w;
 	unsigned long l, bttl = 0;
-	unsigned int workport;
 	unsigned long  sg_count;
 
 	if (dev->in_snd[c] != 0) {
@@ -729,53 +701,42 @@ static void send_s870(struct atp_unit *dev,unsigned char c)
 	if ((dev->last_cmd[c] != 0xff) && ((dev->last_cmd[c] & 0x40) != 0)) {
 		dev->last_cmd[c] &= 0x0f;
 		workreq = dev->id[c][dev->last_cmd[c]].curr_req;
-		if (workreq != NULL) {	/* check NULL pointer */
-		   goto cmd_subp;
-		}
-		dev->last_cmd[c] = 0xff;	
-		if (dev->quhd[c] == dev->quend[c]) {
-		   	dev->in_snd[c] = 0;
-		   	return ;
+		if (!workreq) {
+			dev->last_cmd[c] = 0xff;
+			if (dev->quhd[c] == dev->quend[c]) {
+				dev->in_snd[c] = 0;
+				return;
+			}
 		}
 	}
-	if ((dev->last_cmd[c] != 0xff) && (dev->working[c] != 0)) {
-	     	dev->in_snd[c] = 0;
-	     	return ;
-	}
-	dev->working[c]++;
-	j = dev->quhd[c];
-	dev->quhd[c]++;
-	if (dev->quhd[c] >= qcnt) {
-		dev->quhd[c] = 0;
-	}
-	workreq = dev->quereq[c][dev->quhd[c]];
-	if (dev->id[c][scmd_id(workreq)].curr_req == NULL) {
+	if (!workreq) {
+		if ((dev->last_cmd[c] != 0xff) && (dev->working[c] != 0)) {
+			dev->in_snd[c] = 0;
+			return;
+		}
+		dev->working[c]++;
+		j = dev->quhd[c];
+		dev->quhd[c]++;
+		if (dev->quhd[c] >= qcnt)
+			dev->quhd[c] = 0;
+		workreq = dev->quereq[c][dev->quhd[c]];
+		if (dev->id[c][scmd_id(workreq)].curr_req != NULL) {
+			dev->quhd[c] = j;
+			dev->working[c]--;
+			dev->in_snd[c] = 0;
+			return;
+		}
 		dev->id[c][scmd_id(workreq)].curr_req = workreq;
 		dev->last_cmd[c] = scmd_id(workreq);
-		goto cmd_subp;
-	}	
-	dev->quhd[c] = j;
-	dev->working[c]--;
-	dev->in_snd[c] = 0;
-	return;
-cmd_subp:
-	workport = dev->ioport[c];
-	tmport = workport + 0x1f;
-	if ((inb(tmport) & 0xb0) != 0) {
-		goto abortsnd;
-	}
-	tmport = workport + 0x1c;
-	if (inb(tmport) == 0) {
-		goto oktosend;
 	}
-abortsnd:
+	if ((atp_readb_io(dev, c, 0x1f) & 0xb0) != 0 || atp_readb_io(dev, c, 0x1c) != 0) {
 #ifdef ED_DBGP
-	printk("Abort to Send\n");
+		printk("Abort to Send\n");
 #endif
-	dev->last_cmd[c] |= 0x40;
-	dev->in_snd[c] = 0;
-	return;
-oktosend:
+		dev->last_cmd[c] |= 0x40;
+		dev->in_snd[c] = 0;
+		return;
+	}
 #ifdef ED_DBGP
 	printk("OK to Send\n");
 	scmd_printk(KERN_DEBUG, workreq, "CDB");
@@ -786,9 +747,9 @@ oktosend:
 #endif	
 	l = scsi_bufflen(workreq);
 
-	if (dev->dev_id == ATP885_DEVID) {
-		j = inb(dev->baseport + 0x29) & 0xfe;
-		outb(j, dev->baseport + 0x29);
+	if (is885(dev)) {
+		j = atp_readb_base(dev, 0x29) & 0xfe;
+		atp_writeb_base(dev, 0x29, j);
 		dev->r1f[c][scmd_id(workreq)] = 0;
 	}
 	
@@ -800,7 +761,6 @@ oktosend:
 		l = 0;
 	}
 
-	tmport = workport + 0x1b;
 	j = 0;
 	target_id = scmd_id(workreq);
 
@@ -812,9 +772,9 @@ oktosend:
 	if ((w & dev->wide_id[c]) != 0) {
 		j |= 0x01;
 	}
-	outb(j, tmport);
-	while ((inb(tmport) & 0x01) != j) {
-		outb(j,tmport);
+	atp_writeb_io(dev, c, 0x1b, j);
+	while ((atp_readb_io(dev, c, 0x1b) & 0x01) != j) {
+		atp_writeb_pci(dev, c, 0x1b, j);
 #ifdef ED_DBGP
 		printk("send_s870 while loop 1\n");
 #endif
@@ -823,24 +783,19 @@ oktosend:
 	 *	Write the command
 	 */
 
-	tmport = workport;
-	outb(workreq->cmd_len, tmport++);
-	outb(0x2c, tmport++);
-	if (dev->dev_id == ATP885_DEVID) {
-		outb(0x7f, tmport++);
-	} else {
-		outb(0xcf, tmport++); 	
-	}	
-	for (i = 0; i < workreq->cmd_len; i++) {
-		outb(workreq->cmnd[i], tmport++);
-	}
-	tmport = workport + 0x0f;
-	outb(workreq->device->lun, tmport);
-	tmport += 0x02;
+	atp_writeb_io(dev, c, 0x00, workreq->cmd_len);
+	atp_writeb_io(dev, c, 0x01, 0x2c);
+	if (is885(dev))
+		atp_writeb_io(dev, c, 0x02, 0x7f);
+	else
+		atp_writeb_io(dev, c, 0x02, 0xcf);
+	for (i = 0; i < workreq->cmd_len; i++)
+		atp_writeb_io(dev, c, 0x03 + i, workreq->cmnd[i]);
+	atp_writeb_io(dev, c, 0x0f, workreq->device->lun);
 	/*
 	 *	Write the target
 	 */
-	outb(dev->id[c][target_id].devsp, tmport++);	 
+	atp_writeb_io(dev, c, 0x11, dev->id[c][target_id].devsp);
 #ifdef ED_DBGP	
 	printk("dev->id[%d][%d].devsp = %2x\n",c,target_id,dev->id[c][target_id].devsp);
 #endif
@@ -849,9 +804,9 @@ oktosend:
 	/*
 	 *	Write transfer size
 	 */
-	outb((unsigned char) (((unsigned char *) (&l))[2]), tmport++);
-	outb((unsigned char) (((unsigned char *) (&l))[1]), tmport++);
-	outb((unsigned char) (((unsigned char *) (&l))[0]), tmport++);
+	atp_writeb_io(dev, c, 0x12, ((unsigned char *) (&l))[2]);
+	atp_writeb_io(dev, c, 0x13, ((unsigned char *) (&l))[1]);
+	atp_writeb_io(dev, c, 0x14, ((unsigned char *) (&l))[0]);
 	j = target_id;	
 	dev->id[c][j].last_len = l;
 	dev->id[c][j].tran_len = 0;
@@ -867,29 +822,24 @@ oktosend:
 	/*
 	 *	Check transfer direction
 	 */
-	if (workreq->sc_data_direction == DMA_TO_DEVICE) {
-		outb((unsigned char) (j | 0x20), tmport++);
-	} else {
-		outb(j, tmport++);
-	}
-	outb((unsigned char) (inb(tmport) | 0x80), tmport);
-	outb(0x80, tmport);
-	tmport = workport + 0x1c;
+	if (workreq->sc_data_direction == DMA_TO_DEVICE)
+		atp_writeb_io(dev, c, 0x15, j | 0x20);
+	else
+		atp_writeb_io(dev, c, 0x15, j);
+	atp_writeb_io(dev, c, 0x16, atp_readb_io(dev, c, 0x16) | 0x80);
+	atp_writeb_io(dev, c, 0x16, 0x80);
 	dev->id[c][target_id].dirct = 0;
 	if (l == 0) {
-		if (inb(tmport) == 0) {
-			tmport = workport + 0x18;
+		if (atp_readb_io(dev, c, 0x1c) == 0) {
 #ifdef ED_DBGP
 			printk("change SCSI_CMD_REG 0x08\n");	
 #endif				
-			outb(0x08, tmport);
-		} else {
+			atp_writeb_io(dev, c, 0x18, 0x08);
+		} else
 			dev->last_cmd[c] |= 0x40;
-		}
 		dev->in_snd[c] = 0;
 		return;
 	}
-	tmpcip = dev->pciport[c];
 	prd = dev->id[c][target_id].prd_table;
 	dev->id[c][target_id].prd_pos = prd;
 
@@ -926,50 +876,37 @@ oktosend:
 		printk("2. bttl %x, l %x\n",bttl, l);
 #endif			
 	}
-	tmpcip += 4;
 #ifdef ED_DBGP		
-	printk("send_s870: prdaddr_2 0x%8x tmpcip %x target_id %d\n", dev->id[c][target_id].prdaddr,tmpcip,target_id);
+	printk("send_s870: prdaddr_2 0x%8x target_id %d\n", dev->id[c][target_id].prdaddr,target_id);
 #endif	
 	dev->id[c][target_id].prdaddr = dev->id[c][target_id].prd_bus;
-	outl(dev->id[c][target_id].prdaddr, tmpcip);
-	tmpcip = tmpcip - 2;
-	outb(0x06, tmpcip);
-	outb(0x00, tmpcip);
-	if (dev->dev_id == ATP885_DEVID) {
-		tmpcip--;
-		j=inb(tmpcip) & 0xf3;
+	atp_writel_pci(dev, c, 4, dev->id[c][target_id].prdaddr);
+	atp_writeb_pci(dev, c, 2, 0x06);
+	atp_writeb_pci(dev, c, 2, 0x00);
+	if (is885(dev)) {
+		j = atp_readb_pci(dev, c, 1) & 0xf3;
 		if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) ||
 	    	(workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
 	   		j |= 0x0c;
 		}
-		outb(j,tmpcip);
-		tmpcip--;	    	
-	} else if ((dev->dev_id == ATP880_DEVID1) ||
-	    	   (dev->dev_id == ATP880_DEVID2)) {
-		tmpcip =tmpcip -2;	
-		tmport = workport - 0x05;
-		if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
-			outb((unsigned char) ((inb(tmport) & 0x3f) | 0xc0), tmport);
-		} else {
-			outb((unsigned char) (inb(tmport) & 0x3f), tmport);
-		}		
+		atp_writeb_pci(dev, c, 1, j);
+	} else if (is880(dev)) {
+		if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a))
+			atp_writeb_base(dev, 0x3b, (atp_readb_base(dev, 0x3b) & 0x3f) | 0xc0);
+		else
+			atp_writeb_base(dev, 0x3b, atp_readb_base(dev, 0x3b) & 0x3f);
 	} else {		
-		tmpcip =tmpcip -2;
-		tmport = workport + 0x3a;
-		if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a)) {
-			outb((inb(tmport) & 0xf3) | 0x08, tmport);
-		} else {
-			outb(inb(tmport) & 0xf3, tmport);
-		}		
+		if ((workreq->cmnd[0] == 0x08) || (workreq->cmnd[0] == 0x28) || (workreq->cmnd[0] == 0x0a) || (workreq->cmnd[0] == 0x2a))
+			atp_writeb_base(dev, 0x3a, (atp_readb_base(dev, 0x3a) & 0xf3) | 0x08);
+		else
+			atp_writeb_base(dev, 0x3a, atp_readb_base(dev, 0x3a) & 0xf3);
 	}	
-	tmport = workport + 0x1c;
 
 	if(workreq->sc_data_direction == DMA_TO_DEVICE) {
 		dev->id[c][target_id].dirct = 0x20;
-		if (inb(tmport) == 0) {
-			tmport = workport + 0x18;
-			outb(0x08, tmport);
-			outb(0x01, tmpcip);
+		if (atp_readb_io(dev, c, 0x1c) == 0) {
+			atp_writeb_io(dev, c, 0x18, 0x08);
+			atp_writeb_pci(dev, c, 0, 0x01);
 #ifdef ED_DBGP		
 		printk( "start DMA(to target)\n");
 #endif				
@@ -979,10 +916,9 @@ oktosend:
 		dev->in_snd[c] = 0;
 		return;
 	}
-	if (inb(tmport) == 0) {		
-		tmport = workport + 0x18;
-		outb(0x08, tmport);
-		outb(0x09, tmpcip);
+	if (atp_readb_io(dev, c, 0x1c) == 0) {
+		atp_writeb_io(dev, c, 0x18, 0x08);
+		atp_writeb_pci(dev, c, 0, 0x09);
 #ifdef ED_DBGP		
 		printk( "start DMA(to host)\n");
 #endif			
@@ -996,49 +932,40 @@ oktosend:
 
 static unsigned char fun_scam(struct atp_unit *dev, unsigned short int *val)
 {
-	unsigned int tmport;
 	unsigned short int i, k;
 	unsigned char j;
 
-	tmport = dev->ioport[0] + 0x1c;
-	outw(*val, tmport);
-FUN_D7:
+	atp_writew_io(dev, 0, 0x1c, *val);
 	for (i = 0; i < 10; i++) {	/* stable >= bus settle delay(400 ns)  */
-		k = inw(tmport);
+		k = atp_readw_io(dev, 0, 0x1c);
 		j = (unsigned char) (k >> 8);
-		if ((k & 0x8000) != 0) {	/* DB7 all release?    */
-			goto FUN_D7;
-		}
+		if ((k & 0x8000) != 0)	/* DB7 all release?    */
+			i = 0;
 	}
 	*val |= 0x4000;		/* assert DB6           */
-	outw(*val, tmport);
+	atp_writew_io(dev, 0, 0x1c, *val);
 	*val &= 0xdfff;		/* assert DB5           */
-	outw(*val, tmport);
-FUN_D5:
+	atp_writew_io(dev, 0, 0x1c, *val);
 	for (i = 0; i < 10; i++) {	/* stable >= bus settle delay(400 ns) */
-		if ((inw(tmport) & 0x2000) != 0) {	/* DB5 all release?       */
-			goto FUN_D5;
-		}
+		if ((atp_readw_io(dev, 0, 0x1c) & 0x2000) != 0)	/* DB5 all release?       */
+			i = 0;
 	}
 	*val |= 0x8000;		/* no DB4-0, assert DB7    */
 	*val &= 0xe0ff;
-	outw(*val, tmport);
+	atp_writew_io(dev, 0, 0x1c, *val);
 	*val &= 0xbfff;		/* release DB6             */
-	outw(*val, tmport);
-FUN_D6:
+	atp_writew_io(dev, 0, 0x1c, *val);
 	for (i = 0; i < 10; i++) {	/* stable >= bus settle delay(400 ns)  */
-		if ((inw(tmport) & 0x4000) != 0) {	/* DB6 all release?  */
-			goto FUN_D6;
-		}
+		if ((atp_readw_io(dev, 0, 0x1c) & 0x4000) != 0)	/* DB6 all release?  */
+			i = 0;
 	}
 
 	return j;
 }
 
-static void tscam(struct Scsi_Host *host)
+static void tscam(struct Scsi_Host *host, bool wide_chip, u8 scam_on)
 {
 
-	unsigned int tmport;
 	unsigned char i, j, k;
 	unsigned long n;
 	unsigned short int m, assignid_map, val;
@@ -1055,31 +982,28 @@ static void tscam(struct Scsi_Host *host)
 	}
  */
 
-	tmport = dev->ioport[0] + 1;
-	outb(0x08, tmport++);
-	outb(0x7f, tmport);
-	tmport = dev->ioport[0] + 0x11;
-	outb(0x20, tmport);
+	atp_writeb_io(dev, 0, 1, 0x08);
+	atp_writeb_io(dev, 0, 2, 0x7f);
+	atp_writeb_io(dev, 0, 0x11, 0x20);
 
-	if ((dev->scam_on & 0x40) == 0) {
+	if ((scam_on & 0x40) == 0) {
 		return;
 	}
 	m = 1;
 	m <<= dev->host_id[0];
 	j = 16;
-	if (dev->chip_ver < 4) {
+	if (!wide_chip) {
 		m |= 0xff00;
 		j = 8;
 	}
 	assignid_map = m;
-	tmport = dev->ioport[0] + 0x02;
-	outb(0x02, tmport++);	/* 2*2=4ms,3EH 2/32*3E=3.9ms */
-	outb(0, tmport++);
-	outb(0, tmport++);
-	outb(0, tmport++);
-	outb(0, tmport++);
-	outb(0, tmport++);
-	outb(0, tmport++);
+	atp_writeb_io(dev, 0, 0x02, 0x02);	/* 2*2=4ms,3EH 2/32*3E=3.9ms */
+	atp_writeb_io(dev, 0, 0x03, 0);
+	atp_writeb_io(dev, 0, 0x04, 0);
+	atp_writeb_io(dev, 0, 0x05, 0);
+	atp_writeb_io(dev, 0, 0x06, 0);
+	atp_writeb_io(dev, 0, 0x07, 0);
+	atp_writeb_io(dev, 0, 0x08, 0);
 
 	for (i = 0; i < j; i++) {
 		m = 1;
@@ -1087,92 +1011,73 @@ static void tscam(struct Scsi_Host *host)
 		if ((m & assignid_map) != 0) {
 			continue;
 		}
-		tmport = dev->ioport[0] + 0x0f;
-		outb(0, tmport++);
-		tmport += 0x02;
-		outb(0, tmport++);
-		outb(0, tmport++);
-		outb(0, tmport++);
+		atp_writeb_io(dev, 0, 0x0f, 0);
+		atp_writeb_io(dev, 0, 0x12, 0);
+		atp_writeb_io(dev, 0, 0x13, 0);
+		atp_writeb_io(dev, 0, 0x14, 0);
 		if (i > 7) {
 			k = (i & 0x07) | 0x40;
 		} else {
 			k = i;
 		}
-		outb(k, tmport++);
-		tmport = dev->ioport[0] + 0x1b;
-		if (dev->chip_ver == 4) {
-			outb(0x01, tmport);
-		} else {
-			outb(0x00, tmport);
-		}
-wait_rdyok:
-		tmport = dev->ioport[0] + 0x18;
-		outb(0x09, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-		tmport -= 0x08;
-		k = inb(tmport);
-		if (k != 0x16) {
-			if ((k == 0x85) || (k == 0x42)) {
-				continue;
-			}
-			tmport = dev->ioport[0] + 0x10;
-			outb(0x41, tmport);
-			goto wait_rdyok;
-		}
+		atp_writeb_io(dev, 0, 0x15, k);
+		if (wide_chip)
+			atp_writeb_io(dev, 0, 0x1b, 0x01);
+		else
+			atp_writeb_io(dev, 0, 0x1b, 0x00);
+		do {
+			atp_writeb_io(dev, 0, 0x18, 0x09);
+
+			while ((atp_readb_io(dev, 0, 0x1f) & 0x80) == 0x00)
+				cpu_relax();
+			k = atp_readb_io(dev, 0, 0x17);
+			if ((k == 0x85) || (k == 0x42))
+				break;
+			if (k != 0x16)
+				atp_writeb_io(dev, 0, 0x10, 0x41);
+		} while (k != 0x16);
+		if ((k == 0x85) || (k == 0x42))
+			continue;
 		assignid_map |= m;
 
 	}
-	tmport = dev->ioport[0] + 0x02;
-	outb(0x7f, tmport);
-	tmport = dev->ioport[0] + 0x1b;
-	outb(0x02, tmport);
+	atp_writeb_io(dev, 0, 0x02, 0x7f);
+	atp_writeb_io(dev, 0, 0x1b, 0x02);
 
-	outb(0, 0x80);
+	udelay(2);
 
 	val = 0x0080;		/* bsy  */
-	tmport = dev->ioport[0] + 0x1c;
-	outw(val, tmport);
+	atp_writew_io(dev, 0, 0x1c, val);
 	val |= 0x0040;		/* sel  */
-	outw(val, tmport);
+	atp_writew_io(dev, 0, 0x1c, val);
 	val |= 0x0004;		/* msg  */
-	outw(val, tmport);
-	inb(0x80);		/* 2 deskew delay(45ns*2=90ns) */
+	atp_writew_io(dev, 0, 0x1c, val);
+	udelay(2);		/* 2 deskew delay(45ns*2=90ns) */
 	val &= 0x007f;		/* no bsy  */
-	outw(val, tmport);
+	atp_writew_io(dev, 0, 0x1c, val);
 	mdelay(128);
 	val &= 0x00fb;		/* after 1ms no msg */
-	outw(val, tmport);
-wait_nomsg:
-	if ((inb(tmport) & 0x04) != 0) {
-		goto wait_nomsg;
-	}
-	outb(1, 0x80);
+	atp_writew_io(dev, 0, 0x1c, val);
+	while ((atp_readb_io(dev, 0, 0x1c) & 0x04) != 0)
+		;
+	udelay(2);
 	udelay(100);
-	for (n = 0; n < 0x30000; n++) {
-		if ((inb(tmport) & 0x80) != 0) {	/* bsy ? */
-			goto wait_io;
-		}
-	}
-	goto TCM_SYNC;
-wait_io:
-	for (n = 0; n < 0x30000; n++) {
-		if ((inb(tmport) & 0x81) == 0x0081) {
-			goto wait_io1;
-		}
-	}
-	goto TCM_SYNC;
-wait_io1:
-	inb(0x80);
-	val |= 0x8003;		/* io,cd,db7  */
-	outw(val, tmport);
-	inb(0x80);
-	val &= 0x00bf;		/* no sel     */
-	outw(val, tmport);
-	outb(2, 0x80);
-TCM_SYNC:
+	for (n = 0; n < 0x30000; n++)
+		if ((atp_readb_io(dev, 0, 0x1c) & 0x80) != 0)	/* bsy ? */
+			break;
+	if (n < 0x30000)
+		for (n = 0; n < 0x30000; n++)
+			if ((atp_readb_io(dev, 0, 0x1c) & 0x81) == 0x0081) {
+				udelay(2);
+				val |= 0x8003;		/* io,cd,db7  */
+				atp_writew_io(dev, 0, 0x1c, val);
+				udelay(2);
+				val &= 0x00bf;		/* no sel     */
+				atp_writew_io(dev, 0, 0x1c, val);
+				udelay(2);
+				break;
+			}
+	while (1) {
 	/*
 	 * The funny division into multiple delays is to accomodate
 	 * arches like ARM where udelay() multiplies its argument by
@@ -1183,55 +1088,48 @@ TCM_SYNC:
 	 */
 	mdelay(2);
 	udelay(48);
-	if ((inb(tmport) & 0x80) == 0x00) {	/* bsy ? */
-		outw(0, tmport--);
-		outb(0, tmport);
-		tmport = dev->ioport[0] + 0x15;
-		outb(0, tmport);
-		tmport += 0x03;
-		outb(0x09, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0)
+	if ((atp_readb_io(dev, 0, 0x1c) & 0x80) == 0x00) {	/* bsy ? */
+		atp_writew_io(dev, 0, 0x1c, 0);
+		atp_writeb_io(dev, 0, 0x1b, 0);
+		atp_writeb_io(dev, 0, 0x15, 0);
+		atp_writeb_io(dev, 0, 0x18, 0x09);
+		while ((atp_readb_io(dev, 0, 0x1f) & 0x80) == 0)
 			cpu_relax();
-		tmport -= 0x08;
-		inb(tmport);
+		atp_readb_io(dev, 0, 0x17);
 		return;
 	}
 	val &= 0x00ff;		/* synchronization  */
 	val |= 0x3f00;
 	fun_scam(dev, &val);
-	outb(3, 0x80);
+	udelay(2);
 	val &= 0x00ff;		/* isolation        */
 	val |= 0x2000;
 	fun_scam(dev, &val);
-	outb(4, 0x80);
+	udelay(2);
 	i = 8;
 	j = 0;
-TCM_ID:
-	if ((inw(tmport) & 0x2000) == 0) {
-		goto TCM_ID;
-	}
-	outb(5, 0x80);
-	val &= 0x00ff;		/* get ID_STRING */
-	val |= 0x2000;
-	k = fun_scam(dev, &val);
-	if ((k & 0x03) == 0) {
-		goto TCM_5;
-	}
-	mbuf[j] <<= 0x01;
-	mbuf[j] &= 0xfe;
-	if ((k & 0x02) != 0) {
-		mbuf[j] |= 0x01;
-	}
-	i--;
-	if (i > 0) {
-		goto TCM_ID;
+
+	while (1) {
+		if ((atp_readw_io(dev, 0, 0x1c) & 0x2000) == 0)
+			continue;
+		udelay(2);
+		val &= 0x00ff;		/* get ID_STRING */
+		val |= 0x2000;
+		k = fun_scam(dev, &val);
+		if ((k & 0x03) == 0)
+			break;
+		mbuf[j] <<= 0x01;
+		mbuf[j] &= 0xfe;
+		if ((k & 0x02) != 0)
+			mbuf[j] |= 0x01;
+		i--;
+		if (i > 0)
+			continue;
+		j++;
+		i = 8;
 	}
-	j++;
-	i = 8;
-	goto TCM_ID;
 
-TCM_5:			/* isolation complete..  */
+	/* isolation complete..  */
 /*    mbuf[32]=0;
 	printk(" \n%x %x %x %s\n ",assignid_map,mbuf[0],mbuf[1],&mbuf[2]); */
 	i = 15;
@@ -1239,33 +1137,33 @@ TCM_5:			/* isolation complete..  */
 	if ((j & 0x20) != 0) {	/* bit5=1:ID up to 7      */
 		i = 7;
 	}
-	if ((j & 0x06) == 0) {	/* IDvalid?             */
-		goto G2Q5;
-	}
-	k = mbuf[1];
-small_id:
-	m = 1;
-	m <<= k;
-	if ((m & assignid_map) == 0) {
-		goto G2Q_QUIN;
-	}
-	if (k > 0) {
-		k--;
-		goto small_id;
-	}
-G2Q5:			/* srch from max acceptable ID#  */
-	k = i;			/* max acceptable ID#            */
-G2Q_LP:
-	m = 1;
-	m <<= k;
-	if ((m & assignid_map) == 0) {
-		goto G2Q_QUIN;
+	if ((j & 0x06) != 0) {	/* IDvalid?             */
+		k = mbuf[1];
+		while (1) {
+			m = 1;
+			m <<= k;
+			if ((m & assignid_map) == 0)
+				break;
+			if (k > 0)
+				k--;
+			else
+				break;
+		}
 	}
-	if (k > 0) {
-		k--;
-		goto G2Q_LP;
+	if ((m & assignid_map) != 0) {	/* srch from max acceptable ID#  */
+		k = i;			/* max acceptable ID#            */
+		while (1) {
+			m = 1;
+			m <<= k;
+			if ((m & assignid_map) == 0)
+				break;
+			if (k > 0)
+				k--;
+			else
+				break;
+		}
 	}
-G2Q_QUIN:		/* k=binID#,       */
+	/* k=binID#,       */
 	assignid_map |= m;
 	if (k < 8) {
 		quintet[0] = 0x38;	/* 1st dft ID<8    */
@@ -1284,1227 +1182,6 @@ G2Q_QUIN:		/* k=binID#,       */
 	val |= m;
 	fun_scam(dev, &val);
 
-	goto TCM_SYNC;
-
-}
-
-static void is870(struct atp_unit *dev, unsigned int wkport)
-{
-	unsigned int tmport;
-	unsigned char i, j, k, rmb, n;
-	unsigned short int m;
-	static unsigned char mbuf[512];
-	static unsigned char satn[9] = { 0, 0, 0, 0, 0, 0, 0, 6, 6 };
-	static unsigned char inqd[9] = { 0x12, 0, 0, 0, 0x24, 0, 0, 0x24, 6 };
-	static unsigned char synn[6] = { 0x80, 1, 3, 1, 0x19, 0x0e };
-	static unsigned char synu[6] = { 0x80, 1, 3, 1, 0x0c, 0x0e };
-	static unsigned char synw[6] = { 0x80, 1, 3, 1, 0x0c, 0x07 };
-	static unsigned char wide[6] = { 0x80, 1, 2, 3, 1, 0 };
-	
-	tmport = wkport + 0x3a;
-	outb((unsigned char) (inb(tmport) | 0x10), tmport);
-
-	for (i = 0; i < 16; i++) {
-		if ((dev->chip_ver != 4) && (i > 7)) {
-			break;
-		}
-		m = 1;
-		m = m << i;
-		if ((m & dev->active_id[0]) != 0) {
-			continue;
-		}
-		if (i == dev->host_id[0]) {
-			printk(KERN_INFO "         ID: %2d  Host Adapter\n", dev->host_id[0]);
-			continue;
-		}
-		tmport = wkport + 0x1b;
-		if (dev->chip_ver == 4) {
-			outb(0x01, tmport);
-		} else {
-			outb(0x00, tmport);
-		}
-		tmport = wkport + 1;
-		outb(0x08, tmport++);
-		outb(0x7f, tmport++);
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		j = i;
-		if ((j & 0x08) != 0) {
-			j = (j & 0x07) | 0x40;
-		}
-		outb(j, tmport);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-
-		dev->active_id[0] |= m;
-
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-		tmport = wkport + 0x04;
-		outb(0x00, tmport);
-
-phase_cmd:
-		tmport = wkport + 0x18;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			tmport = wkport + 0x10;
-			outb(0x41, tmport);
-			goto phase_cmd;
-		}
-sel_ok:
-		tmport = wkport + 3;
-		outb(inqd[0], tmport++);
-		outb(inqd[1], tmport++);
-		outb(inqd[2], tmport++);
-		outb(inqd[3], tmport++);
-		outb(inqd[4], tmport++);
-		outb(inqd[5], tmport);
-		tmport += 0x07;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(inqd[6], tmport++);
-		outb(inqd[7], tmport++);
-		tmport += 0x03;
-		outb(inqd[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-		tmport = wkport + 0x1b;
-		if (dev->chip_ver == 4)
-			outb(0x00, tmport);
-
-		tmport = wkport + 0x18;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		j = 0;
-rd_inq_data:
-		k = inb(tmport);
-		if ((k & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[j++] = inb(tmport);
-			tmport += 0x06;
-			goto rd_inq_data;
-		}
-		if ((k & 0x80) == 0) {
-			goto rd_inq_data;
-		}
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j == 0x16) {
-			goto inq_ok;
-		}
-		tmport = wkport + 0x10;
-		outb(0x46, tmport);
-		tmport += 0x02;
-		outb(0, tmport++);
-		outb(0, tmport++);
-		outb(0, tmport++);
-		tmport += 0x03;
-		outb(0x08, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x16) {
-			goto sel_ok;
-		}
-inq_ok:
-		mbuf[36] = 0;
-		printk(KERN_INFO "         ID: %2d  %s\n", i, &mbuf[8]);
-		dev->id[0][i].devtype = mbuf[0];
-		rmb = mbuf[1];
-		n = mbuf[7];
-		if (dev->chip_ver != 4) {
-			goto not_wide;
-		}
-		if ((mbuf[7] & 0x60) == 0) {
-			goto not_wide;
-		}
-		if ((dev->global_map[0] & 0x20) == 0) {
-			goto not_wide;
-		}
-		tmport = wkport + 0x1b;
-		outb(0x01, tmport);
-		tmport = wkport + 3;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-try_wide:
-		j = 0;
-		tmport = wkport + 0x14;
-		outb(0x05, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(wide[j++], tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto try_wide;
-		}
-		continue;
-widep_out:
-		tmport = wkport + 0x18;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(0, tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto widep_out;
-		}
-		continue;
-widep_in:
-		tmport = wkport + 0x14;
-		outb(0xff, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		k = 0;
-widep_in1:
-		j = inb(tmport);
-		if ((j & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
-			goto widep_in1;
-		}
-		if ((j & 0x80) == 0x00) {
-			goto widep_in1;
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto widep_out;
-		}
-		continue;
-widep_cmd:
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			if (j == 0x4e) {
-				goto widep_out;
-			}
-			continue;
-		}
-		if (mbuf[0] != 0x01) {
-			goto not_wide;
-		}
-		if (mbuf[1] != 0x02) {
-			goto not_wide;
-		}
-		if (mbuf[2] != 0x03) {
-			goto not_wide;
-		}
-		if (mbuf[3] != 0x01) {
-			goto not_wide;
-		}
-		m = 1;
-		m = m << i;
-		dev->wide_id[0] |= m;
-not_wide:
-		if ((dev->id[0][i].devtype == 0x00) || (dev->id[0][i].devtype == 0x07) || ((dev->id[0][i].devtype == 0x05) && ((n & 0x10) != 0))) {
-			goto set_sync;
-		}
-		continue;
-set_sync:
-		tmport = wkport + 0x1b;
-		j = 0;
-		if ((m & dev->wide_id[0]) != 0) {
-			j |= 0x01;
-		}
-		outb(j, tmport);
-		tmport = wkport + 3;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-try_sync:
-		j = 0;
-		tmport = wkport + 0x14;
-		outb(0x06, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				if ((m & dev->wide_id[0]) != 0) {
-					outb(synw[j++], tmport);
-				} else {
-					if ((m & dev->ultra_map[0]) != 0) {
-						outb(synu[j++], tmport);
-					} else {
-						outb(synn[j++], tmport);
-					}
-				}
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto try_sync;
-		}
-		continue;
-phase_outs:
-		tmport = wkport + 0x18;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00) {
-			if ((inb(tmport) & 0x01) != 0x00) {
-				tmport -= 0x06;
-				outb(0x00, tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j == 0x85) {
-			goto tar_dcons;
-		}
-		j &= 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto phase_outs;
-		}
-		continue;
-phase_ins:
-		tmport = wkport + 0x14;
-		outb(0xff, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		k = 0;
-phase_ins1:
-		j = inb(tmport);
-		if ((j & 0x01) != 0x00) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
-			goto phase_ins1;
-		}
-		if ((j & 0x80) == 0x00) {
-			goto phase_ins1;
-		}
-		tmport -= 0x08;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		j = inb(tmport);
-		if (j == 0x85) {
-			goto tar_dcons;
-		}
-		j &= 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto phase_outs;
-		}
-		continue;
-phase_cmds:
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-tar_dcons:
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			continue;
-		}
-		if (mbuf[0] != 0x01) {
-			continue;
-		}
-		if (mbuf[1] != 0x03) {
-			continue;
-		}
-		if (mbuf[4] == 0x00) {
-			continue;
-		}
-		if (mbuf[3] > 0x64) {
-			continue;
-		}
-		if (mbuf[4] > 0x0c) {
-			mbuf[4] = 0x0c;
-		}
-		dev->id[0][i].devsp = mbuf[4];
-		if ((mbuf[3] < 0x0d) && (rmb == 0)) {
-			j = 0xa0;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x1a) {
-			j = 0x20;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x33) {
-			j = 0x40;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x4c) {
-			j = 0x50;
-			goto set_syn_ok;
-		}
-		j = 0x60;
-set_syn_ok:
-		dev->id[0][i].devsp = (dev->id[0][i].devsp & 0x0f) | j;
-	}
-	tmport = wkport + 0x3a;
-	outb((unsigned char) (inb(tmport) & 0xef), tmport);
-}
-
-static void is880(struct atp_unit *dev, unsigned int wkport)
-{
-	unsigned int tmport;
-	unsigned char i, j, k, rmb, n, lvdmode;
-	unsigned short int m;
-	static unsigned char mbuf[512];
-	static unsigned char satn[9] = { 0, 0, 0, 0, 0, 0, 0, 6, 6 };
-	static unsigned char inqd[9] = { 0x12, 0, 0, 0, 0x24, 0, 0, 0x24, 6 };
-	static unsigned char synn[6] = { 0x80, 1, 3, 1, 0x19, 0x0e };
-	unsigned char synu[6] = { 0x80, 1, 3, 1, 0x0a, 0x0e };
-	static unsigned char synw[6] = { 0x80, 1, 3, 1, 0x19, 0x0e };
-	unsigned char synuw[6] = { 0x80, 1, 3, 1, 0x0a, 0x0e };
-	static unsigned char wide[6] = { 0x80, 1, 2, 3, 1, 0 };
-	static unsigned char u3[9] = { 0x80, 1, 6, 4, 0x09, 00, 0x0e, 0x01, 0x02 };
-
-	lvdmode = inb(wkport + 0x3f) & 0x40;
-
-	for (i = 0; i < 16; i++) {
-		m = 1;
-		m = m << i;
-		if ((m & dev->active_id[0]) != 0) {
-			continue;
-		}
-		if (i == dev->host_id[0]) {
-			printk(KERN_INFO "         ID: %2d  Host Adapter\n", dev->host_id[0]);
-			continue;
-		}
-		tmport = wkport + 0x5b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x41;
-		outb(0x08, tmport++);
-		outb(0x7f, tmport++);
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		j = i;
-		if ((j & 0x08) != 0) {
-			j = (j & 0x07) | 0x40;
-		}
-		outb(j, tmport);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-		dev->active_id[0] |= m;
-
-		tmport = wkport + 0x50;
-		outb(0x30, tmport);
-		tmport = wkport + 0x54;
-		outb(0x00, tmport);
-
-phase_cmd:
-		tmport = wkport + 0x58;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			tmport = wkport + 0x50;
-			outb(0x41, tmport);
-			goto phase_cmd;
-		}
-sel_ok:
-		tmport = wkport + 0x43;
-		outb(inqd[0], tmport++);
-		outb(inqd[1], tmport++);
-		outb(inqd[2], tmport++);
-		outb(inqd[3], tmport++);
-		outb(inqd[4], tmport++);
-		outb(inqd[5], tmport);
-		tmport += 0x07;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(inqd[6], tmport++);
-		outb(inqd[7], tmport++);
-		tmport += 0x03;
-		outb(inqd[8], tmport);
-		tmport += 0x07;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-		tmport = wkport + 0x5b;
-		outb(0x00, tmport);
-		tmport = wkport + 0x58;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		j = 0;
-rd_inq_data:
-		k = inb(tmport);
-		if ((k & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[j++] = inb(tmport);
-			tmport += 0x06;
-			goto rd_inq_data;
-		}
-		if ((k & 0x80) == 0) {
-			goto rd_inq_data;
-		}
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j == 0x16) {
-			goto inq_ok;
-		}
-		tmport = wkport + 0x50;
-		outb(0x46, tmport);
-		tmport += 0x02;
-		outb(0, tmport++);
-		outb(0, tmport++);
-		outb(0, tmport++);
-		tmport += 0x03;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x16)
-			goto sel_ok;
-
-inq_ok:
-		mbuf[36] = 0;
-		printk(KERN_INFO "         ID: %2d  %s\n", i, &mbuf[8]);
-		dev->id[0][i].devtype = mbuf[0];
-		rmb = mbuf[1];
-		n = mbuf[7];
-		if ((mbuf[7] & 0x60) == 0) {
-			goto not_wide;
-		}
-		if ((i < 8) && ((dev->global_map[0] & 0x20) == 0)) {
-			goto not_wide;
-		}
-		if (lvdmode == 0) {
-			goto chg_wide;
-		}
-		if (dev->sp[0][i] != 0x04)	// force u2
-		{
-			goto chg_wide;
-		}
-
-		tmport = wkport + 0x5b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x43;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-
-try_u3:
-		j = 0;
-		tmport = wkport + 0x54;
-		outb(0x09, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(u3[j++], tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto u3p_in;
-		}
-		if (j == 0x0a) {
-			goto u3p_cmd;
-		}
-		if (j == 0x0e) {
-			goto try_u3;
-		}
-		continue;
-u3p_out:
-		tmport = wkport + 0x58;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(0, tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto u3p_in;
-		}
-		if (j == 0x0a) {
-			goto u3p_cmd;
-		}
-		if (j == 0x0e) {
-			goto u3p_out;
-		}
-		continue;
-u3p_in:
-		tmport = wkport + 0x54;
-		outb(0x09, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		k = 0;
-u3p_in1:
-		j = inb(tmport);
-		if ((j & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
-			goto u3p_in1;
-		}
-		if ((j & 0x80) == 0x00) {
-			goto u3p_in1;
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto u3p_in;
-		}
-		if (j == 0x0a) {
-			goto u3p_cmd;
-		}
-		if (j == 0x0e) {
-			goto u3p_out;
-		}
-		continue;
-u3p_cmd:
-		tmport = wkport + 0x50;
-		outb(0x30, tmport);
-		tmport = wkport + 0x54;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			if (j == 0x4e) {
-				goto u3p_out;
-			}
-			continue;
-		}
-		if (mbuf[0] != 0x01) {
-			goto chg_wide;
-		}
-		if (mbuf[1] != 0x06) {
-			goto chg_wide;
-		}
-		if (mbuf[2] != 0x04) {
-			goto chg_wide;
-		}
-		if (mbuf[3] == 0x09) {
-			m = 1;
-			m = m << i;
-			dev->wide_id[0] |= m;
-			dev->id[0][i].devsp = 0xce;
-			continue;
-		}
-chg_wide:
-		tmport = wkport + 0x5b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x43;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		tmport -= 0x08;
-		if (inb(tmport) != 0x11 && inb(tmport) != 0x8e)
-			continue;
-
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-			
-try_wide:
-		j = 0;
-		tmport = wkport + 0x54;
-		outb(0x05, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(wide[j++], tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-			
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto try_wide;
-		}
-		continue;
-widep_out:
-		tmport = wkport + 0x58;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(0, tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto widep_out;
-		}
-		continue;
-widep_in:
-		tmport = wkport + 0x54;
-		outb(0xff, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		k = 0;
-widep_in1:
-		j = inb(tmport);
-		if ((j & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
-			goto widep_in1;
-		}
-		if ((j & 0x80) == 0x00) {
-			goto widep_in1;
-		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto widep_in;
-		}
-		if (j == 0x0a) {
-			goto widep_cmd;
-		}
-		if (j == 0x0e) {
-			goto widep_out;
-		}
-		continue;
-widep_cmd:
-		tmport = wkport + 0x50;
-		outb(0x30, tmport);
-		tmport = wkport + 0x54;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			if (j == 0x4e) {
-				goto widep_out;
-			}
-			continue;
-		}
-		if (mbuf[0] != 0x01) {
-			goto not_wide;
-		}
-		if (mbuf[1] != 0x02) {
-			goto not_wide;
-		}
-		if (mbuf[2] != 0x03) {
-			goto not_wide;
-		}
-		if (mbuf[3] != 0x01) {
-			goto not_wide;
-		}
-		m = 1;
-		m = m << i;
-		dev->wide_id[0] |= m;
-not_wide:
-		if ((dev->id[0][i].devtype == 0x00) || (dev->id[0][i].devtype == 0x07) || ((dev->id[0][i].devtype == 0x05) && ((n & 0x10) != 0))) {
-			m = 1;
-			m = m << i;
-			if ((dev->async[0] & m) != 0) {
-				goto set_sync;
-			}
-		}
-		continue;
-set_sync:
-		if (dev->sp[0][i] == 0x02) {
-			synu[4] = 0x0c;
-			synuw[4] = 0x0c;
-		} else {
-			if (dev->sp[0][i] >= 0x03) {
-				synu[4] = 0x0a;
-				synuw[4] = 0x0a;
-			}
-		}
-		tmport = wkport + 0x5b;
-		j = 0;
-		if ((m & dev->wide_id[0]) != 0) {
-			j |= 0x01;
-		}
-		outb(j, tmport);
-		tmport = wkport + 0x43;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[0][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
-			continue;
-		}
-		while (inb(tmport) != 0x8e)
-			cpu_relax();
-
-try_sync:
-		j = 0;
-		tmport = wkport + 0x54;
-		outb(0x06, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				if ((m & dev->wide_id[0]) != 0) {
-					if ((m & dev->ultra_map[0]) != 0) {
-						outb(synuw[j++], tmport);
-					} else {
-						outb(synw[j++], tmport);
-					}
-				} else {
-					if ((m & dev->ultra_map[0]) != 0) {
-						outb(synu[j++], tmport);
-					} else {
-						outb(synn[j++], tmport);
-					}
-				}
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		j = inb(tmport) & 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto try_sync;
-		}
-		continue;
-phase_outs:
-		tmport = wkport + 0x58;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00) {
-			if ((inb(tmport) & 0x01) != 0x00) {
-				tmport -= 0x06;
-				outb(0x00, tmport);
-				tmport += 0x06;
-			}
-		}
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j == 0x85) {
-			goto tar_dcons;
-		}
-		j &= 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto phase_outs;
-		}
-		continue;
-phase_ins:
-		tmport = wkport + 0x54;
-		outb(0x06, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		k = 0;
-phase_ins1:
-		j = inb(tmport);
-		if ((j & 0x01) != 0x00) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
-			goto phase_ins1;
-		}
-		if ((j & 0x80) == 0x00) {
-			goto phase_ins1;
-		}
-		tmport -= 0x08;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		j = inb(tmport);
-		if (j == 0x85) {
-			goto tar_dcons;
-		}
-		j &= 0x0f;
-		if (j == 0x0f) {
-			goto phase_ins;
-		}
-		if (j == 0x0a) {
-			goto phase_cmds;
-		}
-		if (j == 0x0e) {
-			goto phase_outs;
-		}
-		continue;
-phase_cmds:
-		tmport = wkport + 0x50;
-		outb(0x30, tmport);
-tar_dcons:
-		tmport = wkport + 0x54;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
-			cpu_relax();
-
-		tmport -= 0x08;
-		j = inb(tmport);
-		if (j != 0x16) {
-			continue;
-		}
-		if (mbuf[0] != 0x01) {
-			continue;
-		}
-		if (mbuf[1] != 0x03) {
-			continue;
-		}
-		if (mbuf[4] == 0x00) {
-			continue;
-		}
-		if (mbuf[3] > 0x64) {
-			continue;
-		}
-		if (mbuf[4] > 0x0e) {
-			mbuf[4] = 0x0e;
-		}
-		dev->id[0][i].devsp = mbuf[4];
-		if (mbuf[3] < 0x0c) {
-			j = 0xb0;
-			goto set_syn_ok;
-		}
-		if ((mbuf[3] < 0x0d) && (rmb == 0)) {
-			j = 0xa0;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x1a) {
-			j = 0x20;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x33) {
-			j = 0x40;
-			goto set_syn_ok;
-		}
-		if (mbuf[3] < 0x4c) {
-			j = 0x50;
-			goto set_syn_ok;
-		}
-		j = 0x60;
-set_syn_ok:
-		dev->id[0][i].devsp = (dev->id[0][i].devsp & 0x0f) | j;
 	}
 }
 
@@ -2560,491 +1237,345 @@ static int atp870u_init_tables(struct Scsi_Host *host)
 	return 0;
 }
 
-/* return non-zero on detection */
-static int atp870u_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+static void atp_set_host_id(struct atp_unit *atp, u8 c, u8 host_id)
 {
-	unsigned char k, m, c;
-	unsigned long flags;
-	unsigned int base_io, tmport, error,n;
-	unsigned char host_id;
-	struct Scsi_Host *shpnt = NULL;
-	struct atp_unit *atpdev, *p;
-	unsigned char setupdata[2][16];
-	int count = 0;
+	atp_writeb_io(atp, c, 0, host_id | 0x08);
+	atp_writeb_io(atp, c, 0x18, 0);
+	while ((atp_readb_io(atp, c, 0x1f) & 0x80) == 0)
+		mdelay(1);
+	atp_readb_io(atp, c, 0x17);
+	atp_writeb_io(atp, c, 1, 8);
+	atp_writeb_io(atp, c, 2, 0x7f);
+	atp_writeb_io(atp, c, 0x11, 0x20);
+}
 
-	atpdev = kzalloc(sizeof(*atpdev), GFP_KERNEL);
-	if (!atpdev)
-		return -ENOMEM;
+static void atp870_init(struct Scsi_Host *shpnt)
+{
+	struct atp_unit *atpdev = shost_priv(shpnt);
+	struct pci_dev *pdev = atpdev->pdev;
+	unsigned char k, host_id;
+	u8 scam_on;
+	bool wide_chip =
+		(pdev->device == PCI_DEVICE_ID_ARTOP_AEC7610 &&
+		 pdev->revision == 4) ||
+		(pdev->device == PCI_DEVICE_ID_ARTOP_AEC7612UW) ||
+		(pdev->device == PCI_DEVICE_ID_ARTOP_AEC7612SUW);
+
+	pci_read_config_byte(pdev, 0x49, &host_id);
+
+	dev_info(&pdev->dev, "ACARD AEC-671X PCI Ultra/W SCSI-2/3 Host Adapter: IO:%lx, IRQ:%d.\n",
+		 shpnt->io_port, shpnt->irq);
+
+	atpdev->ioport[0] = shpnt->io_port;
+	atpdev->pciport[0] = shpnt->io_port + 0x20;
+	host_id &= 0x07;
+	atpdev->host_id[0] = host_id;
+	scam_on = atp_readb_pci(atpdev, 0, 2);
+	atpdev->global_map[0] = atp_readb_base(atpdev, 0x2d);
+	atpdev->ultra_map[0] = atp_readw_base(atpdev, 0x2e);
+
+	if (atpdev->ultra_map[0] == 0) {
+		scam_on = 0x00;
+		atpdev->global_map[0] = 0x20;
+		atpdev->ultra_map[0] = 0xffff;
+	}
 
-	if (pci_enable_device(pdev))
-		goto err_eio;
+	if (pdev->revision > 0x07)	/* check if atp876 chip */
+		atp_writeb_base(atpdev, 0x3e, 0x00); /* enable terminator */
+
+	k = (atp_readb_base(atpdev, 0x3a) & 0xf3) | 0x10;
+	atp_writeb_base(atpdev, 0x3a, k);
+	atp_writeb_base(atpdev, 0x3a, k & 0xdf);
+	mdelay(32);
+	atp_writeb_base(atpdev, 0x3a, k);
+	mdelay(32);
+	atp_set_host_id(atpdev, 0, host_id);
+
+	tscam(shpnt, wide_chip, scam_on);
+	atp_writeb_base(atpdev, 0x3a, atp_readb_base(atpdev, 0x3a) | 0x10);
+	atp_is(atpdev, 0, wide_chip, 0);
+	atp_writeb_base(atpdev, 0x3a, atp_readb_base(atpdev, 0x3a) & 0xef);
+	atp_writeb_base(atpdev, 0x3b, atp_readb_base(atpdev, 0x3b) | 0x20);
+	shpnt->max_id = wide_chip ? 16 : 8;
+	shpnt->this_id = host_id;
+}
 
-        if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
-                printk(KERN_INFO "atp870u: use 32bit DMA mask.\n");
-        } else {
-                printk(KERN_ERR "atp870u: DMA mask required but not available.\n");
-		goto err_eio;
-        }
+static void atp880_init(struct Scsi_Host *shpnt)
+{
+	struct atp_unit *atpdev = shost_priv(shpnt);
+	struct pci_dev *pdev = atpdev->pdev;
+	unsigned char k, m, host_id;
+	unsigned int n;
 
-	/*
-	 * It's probably easier to weed out some revisions like
-	 * this than via the PCI device table
-	 */
-	if (ent->device == PCI_DEVICE_ID_ARTOP_AEC7610) {
-		atpdev->chip_ver = pdev->revision;
-		if (atpdev->chip_ver < 2)
-			goto err_eio;
-	}
+	pci_write_config_byte(pdev, PCI_LATENCY_TIMER, 0x80);
 
-	switch (ent->device) {
-	case PCI_DEVICE_ID_ARTOP_AEC7612UW:
-	case PCI_DEVICE_ID_ARTOP_AEC7612SUW:
-	case ATP880_DEVID1:	
-	case ATP880_DEVID2:	
-	case ATP885_DEVID:	
-		atpdev->chip_ver = 0x04;
-	default:
-		break;
-	}
-	base_io = pci_resource_start(pdev, 0);
-	base_io &= 0xfffffff8;
-
-	if ((ent->device == ATP880_DEVID1)||(ent->device == ATP880_DEVID2)) {
-		atpdev->chip_ver = pdev->revision;
-		pci_write_config_byte(pdev, PCI_LATENCY_TIMER, 0x80);//JCC082803
-
-		host_id = inb(base_io + 0x39);
-		host_id >>= 0x04;
-
-		printk(KERN_INFO "   ACARD AEC-67160 PCI Ultra3 LVD Host Adapter: %d"
-			"    IO:%x, IRQ:%d.\n", count, base_io, pdev->irq);
-		atpdev->ioport[0] = base_io + 0x40;
-		atpdev->pciport[0] = base_io + 0x28;
-		atpdev->dev_id = ent->device;
-		atpdev->host_id[0] = host_id;
-
-		tmport = base_io + 0x22;
-		atpdev->scam_on = inb(tmport);
-		tmport += 0x13;
-		atpdev->global_map[0] = inb(tmport);
-		tmport += 0x07;
-		atpdev->ultra_map[0] = inw(tmport);
-
-		n = 0x3f09;
-next_fblk_880:
-		if (n >= 0x4000)
-			goto flash_ok_880;
+	atpdev->ioport[0] = shpnt->io_port + 0x40;
+	atpdev->pciport[0] = shpnt->io_port + 0x28;
+
+	host_id = atp_readb_base(atpdev, 0x39) >> 4;
 
+	dev_info(&pdev->dev, "ACARD AEC-67160 PCI Ultra3 LVD Host Adapter: IO:%lx, IRQ:%d.\n",
+		 shpnt->io_port, shpnt->irq);
+	atpdev->host_id[0] = host_id;
+
+	atpdev->global_map[0] = atp_readb_base(atpdev, 0x35);
+	atpdev->ultra_map[0] = atp_readw_base(atpdev, 0x3c);
+
+	n = 0x3f09;
+	while (n < 0x4000) {
 		m = 0;
-		outw(n, base_io + 0x34);
+		atp_writew_base(atpdev, 0x34, n);
 		n += 0x0002;
-		if (inb(base_io + 0x30) == 0xff)
-			goto flash_ok_880;
-
-		atpdev->sp[0][m++] = inb(base_io + 0x30);
-		atpdev->sp[0][m++] = inb(base_io + 0x31);
-		atpdev->sp[0][m++] = inb(base_io + 0x32);
-		atpdev->sp[0][m++] = inb(base_io + 0x33);
-		outw(n, base_io + 0x34);
+		if (atp_readb_base(atpdev, 0x30) == 0xff)
+			break;
+
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x30);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x31);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x32);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x33);
+		atp_writew_base(atpdev, 0x34, n);
 		n += 0x0002;
-		atpdev->sp[0][m++] = inb(base_io + 0x30);
-		atpdev->sp[0][m++] = inb(base_io + 0x31);
-		atpdev->sp[0][m++] = inb(base_io + 0x32);
-		atpdev->sp[0][m++] = inb(base_io + 0x33);
-		outw(n, base_io + 0x34);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x30);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x31);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x32);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x33);
+		atp_writew_base(atpdev, 0x34, n);
 		n += 0x0002;
-		atpdev->sp[0][m++] = inb(base_io + 0x30);
-		atpdev->sp[0][m++] = inb(base_io + 0x31);
-		atpdev->sp[0][m++] = inb(base_io + 0x32);
-		atpdev->sp[0][m++] = inb(base_io + 0x33);
-		outw(n, base_io + 0x34);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x30);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x31);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x32);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x33);
+		atp_writew_base(atpdev, 0x34, n);
 		n += 0x0002;
-		atpdev->sp[0][m++] = inb(base_io + 0x30);
-		atpdev->sp[0][m++] = inb(base_io + 0x31);
-		atpdev->sp[0][m++] = inb(base_io + 0x32);
-		atpdev->sp[0][m++] = inb(base_io + 0x33);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x30);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x31);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x32);
+		atpdev->sp[0][m++] = atp_readb_base(atpdev, 0x33);
 		n += 0x0018;
-		goto next_fblk_880;
-flash_ok_880:
-		outw(0, base_io + 0x34);
-		atpdev->ultra_map[0] = 0;
-		atpdev->async[0] = 0;
+	}
+	atp_writew_base(atpdev, 0x34, 0);
+	atpdev->ultra_map[0] = 0;
+	atpdev->async[0] = 0;
+	for (k = 0; k < 16; k++) {
+		n = 1 << k;
+		if (atpdev->sp[0][k] > 1)
+			atpdev->ultra_map[0] |= n;
+		else
+			if (atpdev->sp[0][k] == 0)
+				atpdev->async[0] |= n;
+	}
+	atpdev->async[0] = ~(atpdev->async[0]);
+	atp_writeb_base(atpdev, 0x35, atpdev->global_map[0]);
+
+	k = atp_readb_base(atpdev, 0x38) & 0x80;
+	atp_writeb_base(atpdev, 0x38, k);
+	atp_writeb_base(atpdev, 0x3b, 0x20);
+	mdelay(32);
+	atp_writeb_base(atpdev, 0x3b, 0);
+	mdelay(32);
+	atp_readb_io(atpdev, 0, 0x1b);
+	atp_readb_io(atpdev, 0, 0x17);
+
+	atp_set_host_id(atpdev, 0, host_id);
+
+	tscam(shpnt, true, atp_readb_base(atpdev, 0x22));
+	atp_is(atpdev, 0, true, atp_readb_base(atpdev, 0x3f) & 0x40);
+	atp_writeb_base(atpdev, 0x38, 0xb0);
+	shpnt->max_id = 16;
+	shpnt->this_id = host_id;
+}
+
+static void atp885_init(struct Scsi_Host *shpnt)
+{
+	struct atp_unit *atpdev = shost_priv(shpnt);
+	struct pci_dev *pdev = atpdev->pdev;
+	unsigned char k, m, c;
+	unsigned int n;
+	unsigned char setupdata[2][16];
+
+	dev_info(&pdev->dev, "ACARD AEC-67162 PCI Ultra3 LVD Host Adapter: IO:%lx, IRQ:%d.\n",
+		 shpnt->io_port, shpnt->irq);
+
+	atpdev->ioport[0] = shpnt->io_port + 0x80;
+	atpdev->ioport[1] = shpnt->io_port + 0xc0;
+	atpdev->pciport[0] = shpnt->io_port + 0x40;
+	atpdev->pciport[1] = shpnt->io_port + 0x50;
+
+	c = atp_readb_base(atpdev, 0x29);
+	atp_writeb_base(atpdev, 0x29, c | 0x04);
+
+	n = 0x1f80;
+	while (n < 0x2000) {
+		atp_writew_base(atpdev, 0x3c, n);
+		if (atp_readl_base(atpdev, 0x38) == 0xffffffff)
+			break;
+		for (m = 0; m < 2; m++) {
+			atpdev->global_map[m] = 0;
+			for (k = 0; k < 4; k++) {
+				atp_writew_base(atpdev, 0x3c, n++);
+				((unsigned long *)&setupdata[m][0])[k] = atp_readl_base(atpdev, 0x38);
+			}
+			for (k = 0; k < 4; k++) {
+				atp_writew_base(atpdev, 0x3c, n++);
+				((unsigned long *)&atpdev->sp[m][0])[k] = atp_readl_base(atpdev, 0x38);
+			}
+			n += 8;
+		}
+	}
+	c = atp_readb_base(atpdev, 0x29);
+	atp_writeb_base(atpdev, 0x29, c & 0xfb);
+	for (c = 0; c < 2; c++) {
+		atpdev->ultra_map[c] = 0;
+		atpdev->async[c] = 0;
 		for (k = 0; k < 16; k++) {
-			n = 1;
-			n = n << k;
-			if (atpdev->sp[0][k] > 1) {
-				atpdev->ultra_map[0] |= n;
-			} else {
-				if (atpdev->sp[0][k] == 0)
-					atpdev->async[0] |= n;
- 			}
-	 	}
-		atpdev->async[0] = ~(atpdev->async[0]);
-		outb(atpdev->global_map[0], base_io + 0x35);
- 
-		shpnt = scsi_host_alloc(&atp870u_template, sizeof(struct atp_unit));
-		if (!shpnt)
-			goto err_nomem;
-
-		p = (struct atp_unit *)&shpnt->hostdata;
-
-		atpdev->host = shpnt;
-		atpdev->pdev = pdev;
-		pci_set_drvdata(pdev, p);
-		memcpy(p, atpdev, sizeof(*atpdev));
-		if (atp870u_init_tables(shpnt) < 0) {
-			printk(KERN_ERR "Unable to allocate tables for Acard controller\n");
-			goto unregister;
-		}
-
-		if (request_irq(pdev->irq, atp870u_intr_handle, IRQF_SHARED, "atp880i", shpnt)) {
- 			printk(KERN_ERR "Unable to allocate IRQ%d for Acard controller.\n", pdev->irq);
-			goto free_tables;
-		}
-
-		spin_lock_irqsave(shpnt->host_lock, flags);
-		tmport = base_io + 0x38;
-		k = inb(tmport) & 0x80;
-		outb(k, tmport);
-		tmport += 0x03;
-		outb(0x20, tmport);
-		mdelay(32);
-		outb(0, tmport);
-		mdelay(32);
-		tmport = base_io + 0x5b;
-		inb(tmport);
-		tmport -= 0x04;
-		inb(tmport);
-		tmport = base_io + 0x40;
-		outb((host_id | 0x08), tmport);
-		tmport += 0x18;
-		outb(0, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0)
-			mdelay(1);
-		tmport -= 0x08;
-		inb(tmport);
-		tmport = base_io + 0x41;
-		outb(8, tmport++);
-		outb(0x7f, tmport);
-		tmport = base_io + 0x51;
-		outb(0x20, tmport);
-
-		tscam(shpnt);
-		is880(p, base_io);
-		tmport = base_io + 0x38;
-		outb(0xb0, tmport);
-		shpnt->max_id = 16;
-		shpnt->this_id = host_id;
-		shpnt->unique_id = base_io;
-		shpnt->io_port = base_io;
-		shpnt->n_io_port = 0x60;	/* Number of bytes of I/O space used */
-		shpnt->irq = pdev->irq;			
-	} else if (ent->device == ATP885_DEVID) {	
-			printk(KERN_INFO "   ACARD AEC-67162 PCI Ultra3 LVD Host Adapter:  IO:%x, IRQ:%d.\n"
-			       , base_io, pdev->irq);
-        	
-		atpdev->pdev = pdev;
-		atpdev->dev_id  = ent->device;
-		atpdev->baseport = base_io;
-		atpdev->ioport[0] = base_io + 0x80;
-		atpdev->ioport[1] = base_io + 0xc0;
-		atpdev->pciport[0] = base_io + 0x40;
-		atpdev->pciport[1] = base_io + 0x50;
-				
-		shpnt = scsi_host_alloc(&atp870u_template, sizeof(struct atp_unit));
-		if (!shpnt)
-			goto err_nomem;
-        	
-		p = (struct atp_unit *)&shpnt->hostdata;
-        	
-		atpdev->host = shpnt;
-		atpdev->pdev = pdev;
-		pci_set_drvdata(pdev, p);
-		memcpy(p, atpdev, sizeof(struct atp_unit));
-		if (atp870u_init_tables(shpnt) < 0)
-			goto unregister;
-			
-#ifdef ED_DBGP		
-	printk("request_irq() shpnt %p hostdata %p\n", shpnt, p);
-#endif	        
-		if (request_irq(pdev->irq, atp870u_intr_handle, IRQF_SHARED, "atp870u", shpnt)) {
-				printk(KERN_ERR "Unable to allocate IRQ for Acard controller.\n");
-			goto free_tables;
+			n = 1 << k;
+			if (atpdev->sp[c][k] > 1)
+				atpdev->ultra_map[c] |= n;
+			else
+				if (atpdev->sp[c][k] == 0)
+					atpdev->async[c] |= n;
+		}
+		atpdev->async[c] = ~(atpdev->async[c]);
+
+		if (atpdev->global_map[c] == 0) {
+			k = setupdata[c][1];
+			if ((k & 0x40) != 0)
+				atpdev->global_map[c] |= 0x20;
+			k &= 0x07;
+			atpdev->global_map[c] |= k;
+			if ((setupdata[c][2] & 0x04) != 0)
+				atpdev->global_map[c] |= 0x08;
+			atpdev->host_id[c] = setupdata[c][0] & 0x07;
 		}
-		
-		spin_lock_irqsave(shpnt->host_lock, flags);        					
-        			
-		c=inb(base_io + 0x29);
-		outb((c | 0x04),base_io + 0x29);
-        	
-		n=0x1f80;
-next_fblk_885:
-		if (n >= 0x2000) {
-		   goto flash_ok_885;
-		}
-		outw(n,base_io + 0x3c);
-		if (inl(base_io + 0x38) == 0xffffffff) {
-		   goto flash_ok_885;
-		}
-		for (m=0; m < 2; m++) {
-		    p->global_map[m]= 0;
-		    for (k=0; k < 4; k++) {
-			outw(n++,base_io + 0x3c);
-			((unsigned long *)&setupdata[m][0])[k]=inl(base_io + 0x38);
-		    }
-		    for (k=0; k < 4; k++) {
-			outw(n++,base_io + 0x3c);
-			((unsigned long *)&p->sp[m][0])[k]=inl(base_io + 0x38);
-		    }
-		    n += 8;
-		}
-		goto next_fblk_885;
-flash_ok_885:
-#ifdef ED_DBGP
-		printk( "Flash Read OK\n");
-#endif	
-		c=inb(base_io + 0x29);
-		outb((c & 0xfb),base_io + 0x29);
-		for (c=0;c < 2;c++) {
-		    p->ultra_map[c]=0;
-		    p->async[c] = 0;
-		    for (k=0; k < 16; k++) {
-			n=1;
-			n = n << k;
-			if (p->sp[c][k] > 1) {
-			   p->ultra_map[c] |= n;
-			} else {
-			   if (p->sp[c][k] == 0) {
-			      p->async[c] |= n;
-			   }
-			}
-		    }
-		    p->async[c] = ~(p->async[c]);
-
-		    if (p->global_map[c] == 0) {
-		       k=setupdata[c][1];
-		       if ((k & 0x40) != 0)
-			  p->global_map[c] |= 0x20;
-		       k &= 0x07;
-		       p->global_map[c] |= k;
-		       if ((setupdata[c][2] & 0x04) != 0)
-			  p->global_map[c] |= 0x08;
-		       p->host_id[c] = setupdata[c][0] & 0x07;
-		    }
-		}
-
-		k = inb(base_io + 0x28) & 0x8f;
-		k |= 0x10;
-		outb(k, base_io + 0x28);
-		outb(0x80, base_io + 0x41);
-		outb(0x80, base_io + 0x51);
-		mdelay(100);
-		outb(0, base_io + 0x41);
-		outb(0, base_io + 0x51);
-		mdelay(1000);
-		inb(base_io + 0x9b);
-		inb(base_io + 0x97);
-		inb(base_io + 0xdb);
-		inb(base_io + 0xd7);
-		tmport = base_io + 0x80;
-		k=p->host_id[0];
-		if (k > 7)
-		   k = (k & 0x07) | 0x40;
-		k |= 0x08;
-		outb(k, tmport);
-		tmport += 0x18;
-		outb(0, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0)
-			cpu_relax();
-	
-		tmport -= 0x08;
-		inb(tmport);
-		tmport = base_io + 0x81;
-		outb(8, tmport++);
-		outb(0x7f, tmport);
-		tmport = base_io + 0x91;
-		outb(0x20, tmport);
-
-		tmport = base_io + 0xc0;
-		k=p->host_id[1];
-		if (k > 7)
-		   k = (k & 0x07) | 0x40;
-		k |= 0x08;
-		outb(k, tmport);
-		tmport += 0x18;
-		outb(0, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0)
-			cpu_relax();
+	}
 
-		tmport -= 0x08;
-		inb(tmport);
-		tmport = base_io + 0xc1;
-		outb(8, tmport++);
-		outb(0x7f, tmport);
-		tmport = base_io + 0xd1;
-		outb(0x20, tmport);
-
-		tscam_885();
-		printk(KERN_INFO "   Scanning Channel A SCSI Device ...\n");
-		is885(p, base_io + 0x80, 0);
-		printk(KERN_INFO "   Scanning Channel B SCSI Device ...\n");
-		is885(p, base_io + 0xc0, 1);
-
-		k = inb(base_io + 0x28) & 0xcf;
-		k |= 0xc0;
-		outb(k, base_io + 0x28);
-		k = inb(base_io + 0x1f) | 0x80;
-		outb(k, base_io + 0x1f);
-		k = inb(base_io + 0x29) | 0x01;
-		outb(k, base_io + 0x29);
-#ifdef ED_DBGP
-		//printk("atp885: atp_host[0] 0x%p\n", atp_host[0]);
-#endif		
-		shpnt->max_id = 16;
-		shpnt->max_lun = (p->global_map[0] & 0x07) + 1;
-		shpnt->max_channel = 1;
-		shpnt->this_id = p->host_id[0];
-		shpnt->unique_id = base_io;
-		shpnt->io_port = base_io;
-		shpnt->n_io_port = 0xff;	/* Number of bytes of I/O space used */
-		shpnt->irq = pdev->irq;
-				
-	} else {
-		error = pci_read_config_byte(pdev, 0x49, &host_id);
+	k = atp_readb_base(atpdev, 0x28) & 0x8f;
+	k |= 0x10;
+	atp_writeb_base(atpdev, 0x28, k);
+	atp_writeb_pci(atpdev, 0, 1, 0x80);
+	atp_writeb_pci(atpdev, 1, 1, 0x80);
+	mdelay(100);
+	atp_writeb_pci(atpdev, 0, 1, 0);
+	atp_writeb_pci(atpdev, 1, 1, 0);
+	mdelay(1000);
+	atp_readb_io(atpdev, 0, 0x1b);
+	atp_readb_io(atpdev, 0, 0x17);
+	atp_readb_io(atpdev, 1, 0x1b);
+	atp_readb_io(atpdev, 1, 0x17);
+
+	k = atpdev->host_id[0];
+	if (k > 7)
+		k = (k & 0x07) | 0x40;
+	atp_set_host_id(atpdev, 0, k);
+
+	k = atpdev->host_id[1];
+	if (k > 7)
+		k = (k & 0x07) | 0x40;
+	atp_set_host_id(atpdev, 1, k);
+
+	mdelay(600); /* this delay used to be called tscam_885() */
+	dev_info(&pdev->dev, "Scanning Channel A SCSI Device ...\n");
+	atp_is(atpdev, 0, true, atp_readb_io(atpdev, 0, 0x1b) >> 7);
+	atp_writeb_io(atpdev, 0, 0x16, 0x80);
+	dev_info(&pdev->dev, "Scanning Channel B SCSI Device ...\n");
+	atp_is(atpdev, 1, true, atp_readb_io(atpdev, 1, 0x1b) >> 7);
+	atp_writeb_io(atpdev, 1, 0x16, 0x80);
+	k = atp_readb_base(atpdev, 0x28) & 0xcf;
+	k |= 0xc0;
+	atp_writeb_base(atpdev, 0x28, k);
+	k = atp_readb_base(atpdev, 0x1f) | 0x80;
+	atp_writeb_base(atpdev, 0x1f, k);
+	k = atp_readb_base(atpdev, 0x29) | 0x01;
+	atp_writeb_base(atpdev, 0x29, k);
+	shpnt->max_id = 16;
+	shpnt->max_lun = (atpdev->global_map[0] & 0x07) + 1;
+	shpnt->max_channel = 1;
+	shpnt->this_id = atpdev->host_id[0];
+}
 
-		printk(KERN_INFO "   ACARD AEC-671X PCI Ultra/W SCSI-2/3 Host Adapter: %d "
-			"IO:%x, IRQ:%d.\n", count, base_io, pdev->irq);
+/* return non-zero on detection */
+static int atp870u_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct Scsi_Host *shpnt = NULL;
+	struct atp_unit *atpdev;
+	int err;
 
-		atpdev->ioport[0] = base_io;
-		atpdev->pciport[0] = base_io + 0x20;
-		atpdev->dev_id = ent->device;
-		host_id &= 0x07;
-		atpdev->host_id[0] = host_id;
-		tmport = base_io + 0x22;
-		atpdev->scam_on = inb(tmport);
-		tmport += 0x0b;
-		atpdev->global_map[0] = inb(tmport++);
-		atpdev->ultra_map[0] = inw(tmport);
+	if (ent->device == PCI_DEVICE_ID_ARTOP_AEC7610 && pdev->revision < 2) {
+		dev_err(&pdev->dev, "ATP850S chips (AEC6710L/F cards) are not supported.\n");
+		return -ENODEV;
+	}
 
-		if (atpdev->ultra_map[0] == 0) {
-			atpdev->scam_on = 0x00;
-			atpdev->global_map[0] = 0x20;
-			atpdev->ultra_map[0] = 0xffff;
-		}
+	err = pci_enable_device(pdev);
+	if (err)
+		goto fail;
 
-		shpnt = scsi_host_alloc(&atp870u_template, sizeof(struct atp_unit));
-		if (!shpnt)
-			goto err_nomem;
+	if (pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
+                printk(KERN_ERR "atp870u: DMA mask required but not available.\n");
+                err = -EIO;
+                goto disable_device;
+        }
 
-		p = (struct atp_unit *)&shpnt->hostdata;
-		
-		atpdev->host = shpnt;
-		atpdev->pdev = pdev;
-		pci_set_drvdata(pdev, p);
-		memcpy(p, atpdev, sizeof(*atpdev));
-		if (atp870u_init_tables(shpnt) < 0)
-			goto unregister;
-
-		if (request_irq(pdev->irq, atp870u_intr_handle, IRQF_SHARED, "atp870i", shpnt)) {
-			printk(KERN_ERR "Unable to allocate IRQ%d for Acard controller.\n", pdev->irq);
-			goto free_tables;
-		}
-
-		spin_lock_irqsave(shpnt->host_lock, flags);
-		if (atpdev->chip_ver > 0x07) {	/* check if atp876 chip then enable terminator */
-			tmport = base_io + 0x3e;
-			outb(0x00, tmport);
-		}
- 
-		tmport = base_io + 0x3a;
-		k = (inb(tmport) & 0xf3) | 0x10;
-		outb(k, tmport);
-		outb((k & 0xdf), tmport);
-		mdelay(32);
-		outb(k, tmport);
-		mdelay(32);
-		tmport = base_io;
-		outb((host_id | 0x08), tmport);
-		tmport += 0x18;
-		outb(0, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0)
-			mdelay(1);
-
-		tmport -= 0x08;
-		inb(tmport);
-		tmport = base_io + 1;
-		outb(8, tmport++);
-		outb(0x7f, tmport);
-		tmport = base_io + 0x11;
-		outb(0x20, tmport);
-
-		tscam(shpnt);
-		is870(p, base_io);
-		tmport = base_io + 0x3a;
-		outb((inb(tmport) & 0xef), tmport);
-		tmport++;
-		outb((inb(tmport) | 0x20), tmport);
-		if (atpdev->chip_ver == 4)
-			shpnt->max_id = 16;
-		else		
-			shpnt->max_id = 8;
-		shpnt->this_id = host_id;
-		shpnt->unique_id = base_io;
-		shpnt->io_port = base_io;
-		shpnt->n_io_port = 0x40;	/* Number of bytes of I/O space used */
-		shpnt->irq = pdev->irq;		
-	} 
-		spin_unlock_irqrestore(shpnt->host_lock, flags);
-		if(ent->device==ATP885_DEVID) {
-			if(!request_region(base_io, 0xff, "atp870u")) /* Register the IO ports that we use */
-				goto request_io_fail;
-		} else if((ent->device==ATP880_DEVID1)||(ent->device==ATP880_DEVID2)) {
-			if(!request_region(base_io, 0x60, "atp870u")) /* Register the IO ports that we use */
-				goto request_io_fail;
-		} else {
-			if(!request_region(base_io, 0x40, "atp870u")) /* Register the IO ports that we use */
-				goto request_io_fail;
-		}				
-		count++;
-		if (scsi_add_host(shpnt, &pdev->dev))
-			goto scsi_add_fail;
-		scsi_scan_host(shpnt);
-#ifdef ED_DBGP			
-		printk("atp870u_prob : exit\n");
-#endif		
-		return 0;
+	err = pci_request_regions(pdev, "atp870u");
+	if (err)
+		goto disable_device;
+	pci_set_master(pdev);
+
+        err = -ENOMEM;
+	shpnt = scsi_host_alloc(&atp870u_template, sizeof(struct atp_unit));
+	if (!shpnt)
+		goto release_region;
+
+	atpdev = shost_priv(shpnt);
+
+	atpdev->host = shpnt;
+	atpdev->pdev = pdev;
+	pci_set_drvdata(pdev, atpdev);
+
+	shpnt->io_port = pci_resource_start(pdev, 0);
+	shpnt->io_port &= 0xfffffff8;
+	shpnt->n_io_port = pci_resource_len(pdev, 0);
+	atpdev->baseport = shpnt->io_port;
+	shpnt->unique_id = shpnt->io_port;
+	shpnt->irq = pdev->irq;
+
+	err = atp870u_init_tables(shpnt);
+	if (err) {
+		dev_err(&pdev->dev, "Unable to allocate tables for Acard controller\n");
+		goto unregister;
+	}
 
-scsi_add_fail:
-	printk("atp870u_prob:scsi_add_fail\n");
-	if(ent->device==ATP885_DEVID) {
-		release_region(base_io, 0xff);
-	} else if((ent->device==ATP880_DEVID1)||(ent->device==ATP880_DEVID2)) {
-		release_region(base_io, 0x60);
-	} else {
-		release_region(base_io, 0x40);
+	if (is880(atpdev))
+		atp880_init(shpnt);
+	else if (is885(atpdev))
+		atp885_init(shpnt);
+	else
+		atp870_init(shpnt);
+
+	err = request_irq(shpnt->irq, atp870u_intr_handle, IRQF_SHARED, "atp870u", shpnt);
+	if (err) {
+		dev_err(&pdev->dev, "Unable to allocate IRQ %d.\n", shpnt->irq);
+		goto free_tables;
 	}
-request_io_fail:
-	printk("atp870u_prob:request_io_fail\n");
-	free_irq(pdev->irq, shpnt);
+
+	err = scsi_add_host(shpnt, &pdev->dev);
+	if (err)
+		goto scsi_add_fail;
+	scsi_scan_host(shpnt);
+
+	return 0;
+
+scsi_add_fail:
+	free_irq(shpnt->irq, shpnt);
 free_tables:
-	printk("atp870u_prob:free_table\n");
 	atp870u_free_tables(shpnt);
 unregister:
-	printk("atp870u_prob:unregister\n");
 	scsi_host_put(shpnt);
-	return -1;		
-err_eio:
-	kfree(atpdev);
-	return -EIO;
-err_nomem:
-	kfree(atpdev);
-	return -ENOMEM;
+release_region:
+	pci_release_regions(pdev);
+disable_device:
+	pci_disable_device(pdev);
+fail:
+	return err;
 }
 
 /* The abort command does not leave the device in a clean state where
@@ -3055,7 +1586,6 @@ static int atp870u_abort(struct scsi_cmnd * SCpnt)
 {
 	unsigned char  j, k, c;
 	struct scsi_cmnd *workrequ;
-	unsigned int tmport;
 	struct atp_unit *dev;	
 	struct Scsi_Host *host;
 	host = SCpnt->device->host;
@@ -3065,18 +1595,13 @@ static int atp870u_abort(struct scsi_cmnd * SCpnt)
 	printk(" atp870u: abort Channel = %x \n", c);
 	printk("working=%x last_cmd=%x ", dev->working[c], dev->last_cmd[c]);
 	printk(" quhdu=%x quendu=%x ", dev->quhd[c], dev->quend[c]);
-	tmport = dev->ioport[c];
 	for (j = 0; j < 0x18; j++) {
-		printk(" r%2x=%2x", j, inb(tmport++));
+		printk(" r%2x=%2x", j, atp_readb_io(dev, c, j));
 	}
-	tmport += 0x04;
-	printk(" r1c=%2x", inb(tmport));
-	tmport += 0x03;
-	printk(" r1f=%2x in_snd=%2x ", inb(tmport), dev->in_snd[c]);
-	tmport= dev->pciport[c];
-	printk(" d00=%2x", inb(tmport));
-	tmport += 0x02;
-	printk(" d02=%2x", inb(tmport));
+	printk(" r1c=%2x", atp_readb_io(dev, c, 0x1c));
+	printk(" r1f=%2x in_snd=%2x ", atp_readb_io(dev, c, 0x1f), dev->in_snd[c]);
+	printk(" d00=%2x", atp_readb_pci(dev, c, 0x00));
+	printk(" d02=%2x", atp_readb_pci(dev, c, 0x02));
 	for(j=0;j<16;j++) {
 	   if (dev->id[c][j].curr_req != NULL) {
 		workrequ = dev->id[c][j].curr_req;
@@ -3136,12 +1661,10 @@ static void atp870u_remove (struct pci_dev *pdev)
 	
 	
 	scsi_remove_host(pshost);
-	printk(KERN_INFO "free_irq : %d\n",pshost->irq);
 	free_irq(pshost->irq, pshost);
-	release_region(pshost->io_port, pshost->n_io_port);
-	printk(KERN_INFO "atp870u_free_tables : %p\n",pshost);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
 	atp870u_free_tables(pshost);
-	printk(KERN_INFO "scsi_host_put : %p\n",pshost);
 	scsi_host_put(pshost);
 }
 MODULE_LICENSE("GPL");
@@ -3185,52 +1708,26 @@ static struct pci_driver atp870u_driver = {
 	.remove		= atp870u_remove,
 };
 
-static int __init atp870u_init(void)
-{
-#ifdef ED_DBGP	
-	printk("atp870u_init: Entry\n");
-#endif	
-	return pci_register_driver(&atp870u_driver);
-}
-
-static void __exit atp870u_exit(void)
-{
-#ifdef ED_DBGP	
-	printk("atp870u_exit: Entry\n");
-#endif
-	pci_unregister_driver(&atp870u_driver);
-}
-
-static void tscam_885(void)
-{
-	unsigned char i;
-
-	for (i = 0; i < 0x2; i++) {
-		mdelay(300);
-	}
-	return;
-}
-
-
+module_pci_driver(atp870u_driver);
 
-static void is885(struct atp_unit *dev, unsigned int wkport,unsigned char c)
+static void atp_is(struct atp_unit *dev, unsigned char c, bool wide_chip, unsigned char lvdmode)
 {
-	unsigned int tmport;
-	unsigned char i, j, k, rmb, n, lvdmode;
+	unsigned char i, j, k, rmb, n;
 	unsigned short int m;
 	static unsigned char mbuf[512];
-	static unsigned char satn[9] =	{0, 0, 0, 0, 0, 0, 0, 6, 6};
-	static unsigned char inqd[9] =	{0x12, 0, 0, 0, 0x24, 0, 0, 0x24, 6};
-	static unsigned char synn[6] =	{0x80, 1, 3, 1, 0x19, 0x0e};
-	unsigned char synu[6] =  {0x80, 1, 3, 1, 0x0a, 0x0e};
-	static unsigned char synw[6] =	{0x80, 1, 3, 1, 0x19, 0x0e};
-	unsigned char synuw[6] =  {0x80, 1, 3, 1, 0x0a, 0x0e};
-	static unsigned char wide[6] =	{0x80, 1, 2, 3, 1, 0};
-	static unsigned char u3[9] = { 0x80,1,6,4,0x09,00,0x0e,0x01,0x02 };
-
-	lvdmode=inb(wkport + 0x1b) >> 7;
+	static unsigned char satn[9] = { 0, 0, 0, 0, 0, 0, 0, 6, 6 };
+	static unsigned char inqd[9] = { 0x12, 0, 0, 0, 0x24, 0, 0, 0x24, 6 };
+	static unsigned char synn[6] = { 0x80, 1, 3, 1, 0x19, 0x0e };
+	unsigned char synu[6] = { 0x80, 1, 3, 1, 0x0a, 0x0e };
+	static unsigned char synw[6] = { 0x80, 1, 3, 1, 0x19, 0x0e };
+	static unsigned char synw_870[6] = { 0x80, 1, 3, 1, 0x0c, 0x07 };
+	unsigned char synuw[6] = { 0x80, 1, 3, 1, 0x0a, 0x0e };
+	static unsigned char wide[6] = { 0x80, 1, 2, 3, 1, 0 };
+	static unsigned char u3[9] = { 0x80, 1, 6, 4, 0x09, 00, 0x0e, 0x01, 0x02 };
 
 	for (i = 0; i < 16; i++) {
+		if (!wide_chip && (i > 7))
+			break;
 		m = 1;
 		m = m << i;
 		if ((m & dev->active_id[c]) != 0) {
@@ -3240,192 +1737,172 @@ static void is885(struct atp_unit *dev, unsigned int wkport,unsigned char c)
 			printk(KERN_INFO "         ID: %2d  Host Adapter\n", dev->host_id[c]);
 			continue;
 		}
-		tmport = wkport + 0x1b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x01;
-		outb(0x08, tmport++);
-		outb(0x7f, tmport++);
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[c][i].devsp, tmport++);
-		
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
+		atp_writeb_io(dev, c, 0x1b, wide_chip ? 0x01 : 0x00);
+		atp_writeb_io(dev, c, 1, 0x08);
+		atp_writeb_io(dev, c, 2, 0x7f);
+		atp_writeb_io(dev, c, 3, satn[0]);
+		atp_writeb_io(dev, c, 4, satn[1]);
+		atp_writeb_io(dev, c, 5, satn[2]);
+		atp_writeb_io(dev, c, 6, satn[3]);
+		atp_writeb_io(dev, c, 7, satn[4]);
+		atp_writeb_io(dev, c, 8, satn[5]);
+		atp_writeb_io(dev, c, 0x0f, 0);
+		atp_writeb_io(dev, c, 0x11, dev->id[c][i].devsp);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, satn[6]);
+		atp_writeb_io(dev, c, 0x14, satn[7]);
 		j = i;
 		if ((j & 0x08) != 0) {
 			j = (j & 0x07) | 0x40;
 		}
-		outb(j, tmport);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
+		atp_writeb_io(dev, c, 0x15, j);
+		atp_writeb_io(dev, c, 0x18, satn[8]);
 
-		while ((inb(tmport) & 0x80) == 0x00)
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x11 && atp_readb_io(dev, c, 0x17) != 0x8e)
 			continue;
-		}
-		while (inb(tmport) != 0x8e)
+
+		while (atp_readb_io(dev, c, 0x17) != 0x8e)
 			cpu_relax();
+
 		dev->active_id[c] |= m;
 
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
+		atp_writeb_io(dev, c, 0x10, 0x30);
+		if (is885(dev) || is880(dev))
+			atp_writeb_io(dev, c, 0x14, 0x00);
+		else /* result of is870() merge - is this a bug? */
+			atp_writeb_io(dev, c, 0x04, 0x00);
 
 phase_cmd:
-		tmport = wkport + 0x18;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x18, 0x08);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		j = inb(tmport);
+
+		j = atp_readb_io(dev, c, 0x17);
 		if (j != 0x16) {
-			tmport = wkport + 0x10;
-			outb(0x41, tmport);
+			atp_writeb_io(dev, c, 0x10, 0x41);
 			goto phase_cmd;
 		}
 sel_ok:
-		tmport = wkport + 0x03;
-		outb(inqd[0], tmport++);
-		outb(inqd[1], tmport++);
-		outb(inqd[2], tmport++);
-		outb(inqd[3], tmport++);
-		outb(inqd[4], tmport++);
-		outb(inqd[5], tmport);
-		tmport += 0x07;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[c][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(inqd[6], tmport++);
-		outb(inqd[7], tmport++);
-		tmport += 0x03;
-		outb(inqd[8], tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 3, inqd[0]);
+		atp_writeb_io(dev, c, 4, inqd[1]);
+		atp_writeb_io(dev, c, 5, inqd[2]);
+		atp_writeb_io(dev, c, 6, inqd[3]);
+		atp_writeb_io(dev, c, 7, inqd[4]);
+		atp_writeb_io(dev, c, 8, inqd[5]);
+		atp_writeb_io(dev, c, 0x0f, 0);
+		atp_writeb_io(dev, c, 0x11, dev->id[c][i].devsp);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, inqd[6]);
+		atp_writeb_io(dev, c, 0x14, inqd[7]);
+		atp_writeb_io(dev, c, 0x18, inqd[8]);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x11 && atp_readb_io(dev, c, 0x17) != 0x8e)
 			continue;
-		}
-		while (inb(tmport) != 0x8e)
+
+		while (atp_readb_io(dev, c, 0x17) != 0x8e)
 			cpu_relax();
-		tmport = wkport + 0x1b;
-		outb(0x00, tmport);
-		tmport = wkport + 0x18;
-		outb(0x08, tmport);
-		tmport += 0x07;
+
+		if (wide_chip)
+			atp_writeb_io(dev, c, 0x1b, 0x00);
+
+		atp_writeb_io(dev, c, 0x18, 0x08);
 		j = 0;
 rd_inq_data:
-		k = inb(tmport);
+		k = atp_readb_io(dev, c, 0x1f);
 		if ((k & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[j++] = inb(tmport);
-			tmport += 0x06;
+			mbuf[j++] = atp_readb_io(dev, c, 0x19);
 			goto rd_inq_data;
 		}
 		if ((k & 0x80) == 0) {
 			goto rd_inq_data;
 		}
-		tmport -= 0x08;
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x17);
 		if (j == 0x16) {
 			goto inq_ok;
 		}
-		tmport = wkport + 0x10;
-		outb(0x46, tmport);
-		tmport += 0x02;
-		outb(0, tmport++);
-		outb(0, tmport++);
-		outb(0, tmport++);
-		tmport += 0x03;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x10, 0x46);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, 0);
+		atp_writeb_io(dev, c, 0x14, 0);
+		atp_writeb_io(dev, c, 0x18, 0x08);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if (inb(tmport) != 0x16) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x16)
 			goto sel_ok;
-		}
+
 inq_ok:
 		mbuf[36] = 0;
-		printk( KERN_INFO"         ID: %2d  %s\n", i, &mbuf[8]);
+		printk(KERN_INFO "         ID: %2d  %s\n", i, &mbuf[8]);
 		dev->id[c][i].devtype = mbuf[0];
 		rmb = mbuf[1];
 		n = mbuf[7];
+		if (!wide_chip)
+			goto not_wide;
 		if ((mbuf[7] & 0x60) == 0) {
 			goto not_wide;
 		}
-		if ((i < 8) && ((dev->global_map[c] & 0x20) == 0)) {
-			goto not_wide;
+		if (is885(dev) || is880(dev)) {
+			if ((i < 8) && ((dev->global_map[c] & 0x20) == 0))
+				goto not_wide;
+		} else { /* result of is870() merge - is this a bug? */
+			if ((dev->global_map[c] & 0x20) == 0)
+				goto not_wide;
 		}
 		if (lvdmode == 0) {
-		   goto chg_wide;
-		}
-		if (dev->sp[c][i] != 0x04) {	// force u2
-		   goto chg_wide;
-		}
-
-		tmport = wkport + 0x1b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x03;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[c][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
+			goto chg_wide;
+		}
+		if (dev->sp[c][i] != 0x04)	// force u2
+		{
+			goto chg_wide;
+		}
+
+		atp_writeb_io(dev, c, 0x1b, 0x01);
+		atp_writeb_io(dev, c, 3, satn[0]);
+		atp_writeb_io(dev, c, 4, satn[1]);
+		atp_writeb_io(dev, c, 5, satn[2]);
+		atp_writeb_io(dev, c, 6, satn[3]);
+		atp_writeb_io(dev, c, 7, satn[4]);
+		atp_writeb_io(dev, c, 8, satn[5]);
+		atp_writeb_io(dev, c, 0x0f, 0);
+		atp_writeb_io(dev, c, 0x11, dev->id[c][i].devsp);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, satn[6]);
+		atp_writeb_io(dev, c, 0x14, satn[7]);
+		atp_writeb_io(dev, c, 0x18, satn[8]);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x11 && atp_readb_io(dev, c, 0x17) != 0x8e)
 			continue;
-		}
-		while (inb(tmport) != 0x8e)
+
+		while (atp_readb_io(dev, c, 0x17) != 0x8e)
 			cpu_relax();
+
 try_u3:
 		j = 0;
-		tmport = wkport + 0x14;
-		outb(0x09, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(u3[j++], tmport);
-				tmport += 0x06;
-			}
+		atp_writeb_io(dev, c, 0x14, 0x09);
+		atp_writeb_io(dev, c, 0x18, 0x20);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0)
+				atp_writeb_io(dev, c, 0x19, u3[j++]);
 			cpu_relax();
 		}
-		tmport -= 0x08;
-		while ((inb(tmport) & 0x80) == 0x00)
+
+		while ((atp_readb_io(dev, c, 0x17) & 0x80) == 0x00)
 			cpu_relax();
-		j = inb(tmport) & 0x0f;
+
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto u3p_in;
 		}
@@ -3437,19 +1914,13 @@ try_u3:
 		}
 		continue;
 u3p_out:
-		tmport = wkport + 0x18;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(0, tmport);
-				tmport += 0x06;
-			}
+		atp_writeb_io(dev, c, 0x18, 0x20);
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0)
+				atp_writeb_io(dev, c, 0x19, 0);
 			cpu_relax();
 		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto u3p_in;
 		}
@@ -3461,25 +1932,19 @@ u3p_out:
 		}
 		continue;
 u3p_in:
-		tmport = wkport + 0x14;
-		outb(0x09, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
+		atp_writeb_io(dev, c, 0x14, 0x09);
+		atp_writeb_io(dev, c, 0x18, 0x20);
 		k = 0;
 u3p_in1:
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x1f);
 		if ((j & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
+			mbuf[k++] = atp_readb_io(dev, c, 0x19);
 			goto u3p_in1;
 		}
 		if ((j & 0x80) == 0x00) {
 			goto u3p_in1;
 		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto u3p_in;
 		}
@@ -3491,16 +1956,13 @@ u3p_in1:
 		}
 		continue;
 u3p_cmd:
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00);
-		tmport -= 0x08;
-		j = inb(tmport);
+		atp_writeb_io(dev, c, 0x10, 0x30);
+		atp_writeb_io(dev, c, 0x14, 0x00);
+		atp_writeb_io(dev, c, 0x18, 0x08);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00);
+
+		j = atp_readb_io(dev, c, 0x17);
 		if (j != 0x16) {
 			if (j == 0x4e) {
 				goto u3p_out;
@@ -3527,54 +1989,44 @@ u3p_cmd:
 			continue;
 		}
 chg_wide:
-		tmport = wkport + 0x1b;
-		outb(0x01, tmport);
-		tmport = wkport + 0x03;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[c][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x1b, 0x01);
+		atp_writeb_io(dev, c, 3, satn[0]);
+		atp_writeb_io(dev, c, 4, satn[1]);
+		atp_writeb_io(dev, c, 5, satn[2]);
+		atp_writeb_io(dev, c, 6, satn[3]);
+		atp_writeb_io(dev, c, 7, satn[4]);
+		atp_writeb_io(dev, c, 8, satn[5]);
+		atp_writeb_io(dev, c, 0x0f, 0);
+		atp_writeb_io(dev, c, 0x11, dev->id[c][i].devsp);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, satn[6]);
+		atp_writeb_io(dev, c, 0x14, satn[7]);
+		atp_writeb_io(dev, c, 0x18, satn[8]);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x11 && atp_readb_io(dev, c, 0x17) != 0x8e)
 			continue;
-		}
-		while (inb(tmport) != 0x8e)
+
+		while (atp_readb_io(dev, c, 0x17) != 0x8e)
 			cpu_relax();
+
 try_wide:
 		j = 0;
-		tmport = wkport + 0x14;
-		outb(0x05, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(wide[j++], tmport);
-				tmport += 0x06;
-			}
+		atp_writeb_io(dev, c, 0x14, 0x05);
+		atp_writeb_io(dev, c, 0x18, 0x20);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0)
+				atp_writeb_io(dev, c, 0x19, wide[j++]);
 			cpu_relax();
 		}
-		tmport -= 0x08;
-		while ((inb(tmport) & 0x80) == 0x00)
+
+		while ((atp_readb_io(dev, c, 0x17) & 0x80) == 0x00)
 			cpu_relax();
-		j = inb(tmport) & 0x0f;
+
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto widep_in;
 		}
@@ -3586,19 +2038,13 @@ try_wide:
 		}
 		continue;
 widep_out:
-		tmport = wkport + 0x18;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
-				outb(0, tmport);
-				tmport += 0x06;
-			}
+		atp_writeb_io(dev, c, 0x18, 0x20);
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0)
+				atp_writeb_io(dev, c, 0x19, 0);
 			cpu_relax();
 		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto widep_in;
 		}
@@ -3610,25 +2056,19 @@ widep_out:
 		}
 		continue;
 widep_in:
-		tmport = wkport + 0x14;
-		outb(0xff, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
+		atp_writeb_io(dev, c, 0x14, 0xff);
+		atp_writeb_io(dev, c, 0x18, 0x20);
 		k = 0;
 widep_in1:
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x1f);
 		if ((j & 0x01) != 0) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
+			mbuf[k++] = atp_readb_io(dev, c, 0x19);
 			goto widep_in1;
 		}
 		if ((j & 0x80) == 0x00) {
 			goto widep_in1;
 		}
-		tmport -= 0x08;
-		j = inb(tmport) & 0x0f;
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto widep_in;
 		}
@@ -3640,17 +2080,14 @@ widep_in1:
 		}
 		continue;
 widep_cmd:
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x10, 0x30);
+		atp_writeb_io(dev, c, 0x14, 0x00);
+		atp_writeb_io(dev, c, 0x18, 0x08);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		j = inb(tmport);
+
+		j = atp_readb_io(dev, c, 0x17);
 		if (j != 0x16) {
 			if (j == 0x4e) {
 				goto widep_out;
@@ -3673,88 +2110,81 @@ widep_cmd:
 		m = m << i;
 		dev->wide_id[c] |= m;
 not_wide:
-		if ((dev->id[c][i].devtype == 0x00) || (dev->id[c][i].devtype == 0x07) ||
-		    ((dev->id[c][i].devtype == 0x05) && ((n & 0x10) != 0))) {
+		if ((dev->id[c][i].devtype == 0x00) || (dev->id[c][i].devtype == 0x07) || ((dev->id[c][i].devtype == 0x05) && ((n & 0x10) != 0))) {
 			m = 1;
 			m = m << i;
 			if ((dev->async[c] & m) != 0) {
-			   goto set_sync;
+				goto set_sync;
 			}
 		}
 		continue;
 set_sync:
-		if (dev->sp[c][i] == 0x02) {
-		   synu[4]=0x0c;
-		   synuw[4]=0x0c;
+		if ((!is885(dev) && !is880(dev)) || (dev->sp[c][i] == 0x02)) {
+			synu[4] = 0x0c;
+			synuw[4] = 0x0c;
 		} else {
-		   if (dev->sp[c][i] >= 0x03) {
-		      synu[4]=0x0a;
-		      synuw[4]=0x0a;
-		   }
+			if (dev->sp[c][i] >= 0x03) {
+				synu[4] = 0x0a;
+				synuw[4] = 0x0a;
+			}
 		}
-		tmport = wkport + 0x1b;
 		j = 0;
 		if ((m & dev->wide_id[c]) != 0) {
 			j |= 0x01;
 		}
-		outb(j, tmport);
-		tmport = wkport + 0x03;
-		outb(satn[0], tmport++);
-		outb(satn[1], tmport++);
-		outb(satn[2], tmport++);
-		outb(satn[3], tmport++);
-		outb(satn[4], tmport++);
-		outb(satn[5], tmport++);
-		tmport += 0x06;
-		outb(0, tmport);
-		tmport += 0x02;
-		outb(dev->id[c][i].devsp, tmport++);
-		outb(0, tmport++);
-		outb(satn[6], tmport++);
-		outb(satn[7], tmport++);
-		tmport += 0x03;
-		outb(satn[8], tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x1b, j);
+		atp_writeb_io(dev, c, 3, satn[0]);
+		atp_writeb_io(dev, c, 4, satn[1]);
+		atp_writeb_io(dev, c, 5, satn[2]);
+		atp_writeb_io(dev, c, 6, satn[3]);
+		atp_writeb_io(dev, c, 7, satn[4]);
+		atp_writeb_io(dev, c, 8, satn[5]);
+		atp_writeb_io(dev, c, 0x0f, 0);
+		atp_writeb_io(dev, c, 0x11, dev->id[c][i].devsp);
+		atp_writeb_io(dev, c, 0x12, 0);
+		atp_writeb_io(dev, c, 0x13, satn[6]);
+		atp_writeb_io(dev, c, 0x14, satn[7]);
+		atp_writeb_io(dev, c, 0x18, satn[8]);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		if ((inb(tmport) != 0x11) && (inb(tmport) != 0x8e)) {
+
+		if (atp_readb_io(dev, c, 0x17) != 0x11 && atp_readb_io(dev, c, 0x17) != 0x8e)
 			continue;
-		}
-		while (inb(tmport) != 0x8e)
+
+		while (atp_readb_io(dev, c, 0x17) != 0x8e)
 			cpu_relax();
+
 try_sync:
 		j = 0;
-		tmport = wkport + 0x14;
-		outb(0x06, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
-
-		while ((inb(tmport) & 0x80) == 0) {
-			if ((inb(tmport) & 0x01) != 0) {
-				tmport -= 0x06;
+		atp_writeb_io(dev, c, 0x14, 0x06);
+		atp_writeb_io(dev, c, 0x18, 0x20);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0) {
 				if ((m & dev->wide_id[c]) != 0) {
-					if ((m & dev->ultra_map[c]) != 0) {
-						outb(synuw[j++], tmport);
-					} else {
-						outb(synw[j++], tmport);
-					}
+					if (is885(dev) || is880(dev)) {
+						if ((m & dev->ultra_map[c]) != 0) {
+							atp_writeb_io(dev, c, 0x19, synuw[j++]);
+						} else {
+							atp_writeb_io(dev, c, 0x19, synw[j++]);
+						}
+					} else
+						atp_writeb_io(dev, c, 0x19, synw_870[j++]);
 				} else {
 					if ((m & dev->ultra_map[c]) != 0) {
-						outb(synu[j++], tmport);
+						atp_writeb_io(dev, c, 0x19, synu[j++]);
 					} else {
-						outb(synn[j++], tmport);
+						atp_writeb_io(dev, c, 0x19, synn[j++]);
 					}
 				}
-				tmport += 0x06;
 			}
 		}
-		tmport -= 0x08;
-		while ((inb(tmport) & 0x80) == 0x00)
+
+		while ((atp_readb_io(dev, c, 0x17) & 0x80) == 0x00)
 			cpu_relax();
-		j = inb(tmport) & 0x0f;
+
+		j = atp_readb_io(dev, c, 0x17) & 0x0f;
 		if (j == 0x0f) {
 			goto phase_ins;
 		}
@@ -3766,19 +2196,13 @@ try_sync:
 		}
 		continue;
 phase_outs:
-		tmport = wkport + 0x18;
-		outb(0x20, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00) {
-			if ((inb(tmport) & 0x01) != 0x00) {
-				tmport -= 0x06;
-				outb(0x00, tmport);
-				tmport += 0x06;
-			}
+		atp_writeb_io(dev, c, 0x18, 0x20);
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00) {
+			if ((atp_readb_io(dev, c, 0x1f) & 0x01) != 0x00)
+				atp_writeb_io(dev, c, 0x19, 0x00);
 			cpu_relax();
 		}
-		tmport -= 0x08;
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x17);
 		if (j == 0x85) {
 			goto tar_dcons;
 		}
@@ -3794,26 +2218,25 @@ phase_outs:
 		}
 		continue;
 phase_ins:
-		tmport = wkport + 0x14;
-		outb(0x06, tmport);
-		tmport += 0x04;
-		outb(0x20, tmport);
-		tmport += 0x07;
+		if (is885(dev) || is880(dev))
+			atp_writeb_io(dev, c, 0x14, 0x06);
+		else
+			atp_writeb_io(dev, c, 0x14, 0xff);
+		atp_writeb_io(dev, c, 0x18, 0x20);
 		k = 0;
 phase_ins1:
-		j = inb(tmport);
+		j = atp_readb_io(dev, c, 0x1f);
 		if ((j & 0x01) != 0x00) {
-			tmport -= 0x06;
-			mbuf[k++] = inb(tmport);
-			tmport += 0x06;
+			mbuf[k++] = atp_readb_io(dev, c, 0x19);
 			goto phase_ins1;
 		}
 		if ((j & 0x80) == 0x00) {
 			goto phase_ins1;
 		}
-		tmport -= 0x08;
-		while ((inb(tmport) & 0x80) == 0x00);
-		j = inb(tmport);
+
+		while ((atp_readb_io(dev, c, 0x17) & 0x80) == 0x00);
+
+		j = atp_readb_io(dev, c, 0x17);
 		if (j == 0x85) {
 			goto tar_dcons;
 		}
@@ -3829,18 +2252,15 @@ phase_ins1:
 		}
 		continue;
 phase_cmds:
-		tmport = wkport + 0x10;
-		outb(0x30, tmport);
+		atp_writeb_io(dev, c, 0x10, 0x30);
 tar_dcons:
-		tmport = wkport + 0x14;
-		outb(0x00, tmport);
-		tmport += 0x04;
-		outb(0x08, tmport);
-		tmport += 0x07;
-		while ((inb(tmport) & 0x80) == 0x00)
+		atp_writeb_io(dev, c, 0x14, 0x00);
+		atp_writeb_io(dev, c, 0x18, 0x08);
+
+		while ((atp_readb_io(dev, c, 0x1f) & 0x80) == 0x00)
 			cpu_relax();
-		tmport -= 0x08;
-		j = inb(tmport);
+
+		j = atp_readb_io(dev, c, 0x17);
 		if (j != 0x16) {
 			continue;
 		}
@@ -3856,14 +2276,21 @@ tar_dcons:
 		if (mbuf[3] > 0x64) {
 			continue;
 		}
-		if (mbuf[4] > 0x0e) {
-			mbuf[4] = 0x0e;
+		if (is885(dev) || is880(dev)) {
+			if (mbuf[4] > 0x0e) {
+				mbuf[4] = 0x0e;
+			}
+		} else {
+			if (mbuf[4] > 0x0c) {
+				mbuf[4] = 0x0c;
+			}
 		}
 		dev->id[c][i].devsp = mbuf[4];
-		if (mbuf[3] < 0x0c){
-			j = 0xb0;
-			goto set_syn_ok;
-		}
+		if (is885(dev) || is880(dev))
+			if (mbuf[3] < 0x0c) {
+				j = 0xb0;
+				goto set_syn_ok;
+			}
 		if ((mbuf[3] < 0x0d) && (rmb == 0)) {
 			j = 0xa0;
 			goto set_syn_ok;
@@ -3881,16 +2308,10 @@ tar_dcons:
 			goto set_syn_ok;
 		}
 		j = 0x60;
-	      set_syn_ok:
+set_syn_ok:
 		dev->id[c][i].devsp = (dev->id[c][i].devsp & 0x0f) | j;
-#ifdef ED_DBGP		
+#ifdef ED_DBGP
 		printk("dev->id[%2d][%2d].devsp = %2x\n",c,i,dev->id[c][i].devsp);
 #endif
 	}
-	tmport = wkport + 0x16;
-	outb(0x80, tmport);
 }
-
-module_init(atp870u_init);
-module_exit(atp870u_exit);
-
diff --git a/drivers/scsi/atp870u.h b/drivers/scsi/atp870u.h
index 5cf62566ad42..9b839b1e895a 100644
--- a/drivers/scsi/atp870u.h
+++ b/drivers/scsi/atp870u.h
@@ -26,22 +26,18 @@ struct atp_unit
 	unsigned long baseport;
 	unsigned long ioport[2];
 	unsigned long pciport[2];
-	unsigned long irq;
 	unsigned char last_cmd[2];
 	unsigned char in_snd[2];
 	unsigned char in_int[2];
 	unsigned char quhd[2];
 	unsigned char quend[2];
 	unsigned char global_map[2];
-	unsigned char chip_ver;
-	unsigned char scam_on;
 	unsigned char host_id[2];
 	unsigned int working[2];
 	unsigned short wide_id[2];
 	unsigned short active_id[2];
 	unsigned short ultra_map[2];
 	unsigned short async[2];
-	unsigned short dev_id;
 	unsigned char sp[2][16];
 	unsigned char r1f[2][16];		
 	struct scsi_cmnd *quereq[2][qcnt];
diff --git a/drivers/scsi/be2iscsi/Kconfig b/drivers/scsi/be2iscsi/Kconfig
index 4e7cad272469..bad5f32e1f67 100644
--- a/drivers/scsi/be2iscsi/Kconfig
+++ b/drivers/scsi/be2iscsi/Kconfig
@@ -3,6 +3,7 @@ config BE2ISCSI
 	depends on PCI && SCSI && NET
 	select SCSI_ISCSI_ATTRS
 	select ISCSI_BOOT_SYSFS
+	select IRQ_POLL
 
 	help
 	This driver implements the iSCSI functionality for Emulex
diff --git a/drivers/scsi/be2iscsi/be.h b/drivers/scsi/be2iscsi/be.h
index 77f992e74726..a41c6432f444 100644
--- a/drivers/scsi/be2iscsi/be.h
+++ b/drivers/scsi/be2iscsi/be.h
@@ -20,7 +20,7 @@
 
 #include <linux/pci.h>
 #include <linux/if_vlan.h>
-#include <linux/blk-iopoll.h>
+#include <linux/irq_poll.h>
 #define FW_VER_LEN	32
 #define MCC_Q_LEN	128
 #define MCC_CQ_LEN	256
@@ -101,7 +101,7 @@ struct be_eq_obj {
 	struct beiscsi_hba *phba;
 	struct be_queue_info *cq;
 	struct work_struct work_cqs; /* Work Item */
-	struct blk_iopoll	iopoll;
+	struct irq_poll	iopoll;
 };
 
 struct be_mcc_obj {
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index b7087ba69d8d..022e87b62e40 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -1292,9 +1292,9 @@ static void beiscsi_flush_cq(struct beiscsi_hba *phba)
 
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
-		blk_iopoll_disable(&pbe_eq->iopoll);
+		irq_poll_disable(&pbe_eq->iopoll);
 		beiscsi_process_cq(pbe_eq);
-		blk_iopoll_enable(&pbe_eq->iopoll);
+		irq_poll_enable(&pbe_eq->iopoll);
 	}
 }
 
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index fe0c5143f8e6..cb9072a841be 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -910,8 +910,7 @@ static irqreturn_t be_isr_msix(int irq, void *dev_id)
 	num_eq_processed = 0;
 	while (eqe->dw[offsetof(struct amap_eq_entry, valid) / 32]
 				& EQE_VALID_MASK) {
-		if (!blk_iopoll_sched_prep(&pbe_eq->iopoll))
-			blk_iopoll_sched(&pbe_eq->iopoll);
+		irq_poll_sched(&pbe_eq->iopoll);
 
 		AMAP_SET_BITS(struct amap_eq_entry, valid, eqe, 0);
 		queue_tail_inc(eq);
@@ -972,8 +971,7 @@ static irqreturn_t be_isr(int irq, void *dev_id)
 			spin_unlock_irqrestore(&phba->isr_lock, flags);
 			num_mcceq_processed++;
 		} else {
-			if (!blk_iopoll_sched_prep(&pbe_eq->iopoll))
-				blk_iopoll_sched(&pbe_eq->iopoll);
+			irq_poll_sched(&pbe_eq->iopoll);
 			num_ioeq_processed++;
 		}
 		AMAP_SET_BITS(struct amap_eq_entry, valid, eqe, 0);
@@ -2295,7 +2293,7 @@ void beiscsi_process_all_cqs(struct work_struct *work)
 	hwi_ring_eq_db(phba, pbe_eq->q.id, 0, 0, 1, 1);
 }
 
-static int be_iopoll(struct blk_iopoll *iop, int budget)
+static int be_iopoll(struct irq_poll *iop, int budget)
 {
 	unsigned int ret;
 	struct beiscsi_hba *phba;
@@ -2306,7 +2304,7 @@ static int be_iopoll(struct blk_iopoll *iop, int budget)
 	pbe_eq->cq_count += ret;
 	if (ret < budget) {
 		phba = pbe_eq->phba;
-		blk_iopoll_complete(iop);
+		irq_poll_complete(iop);
 		beiscsi_log(phba, KERN_INFO,
 			    BEISCSI_LOG_CONFIG | BEISCSI_LOG_IO,
 			    "BM_%d : rearm pbe_eq->q.id =%d\n",
@@ -5293,7 +5291,7 @@ static void beiscsi_quiesce(struct beiscsi_hba *phba,
 
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
-		blk_iopoll_disable(&pbe_eq->iopoll);
+		irq_poll_disable(&pbe_eq->iopoll);
 	}
 
 	if (unload_state == BEISCSI_CLEAN_UNLOAD) {
@@ -5579,9 +5577,8 @@ static void beiscsi_eeh_resume(struct pci_dev *pdev)
 
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
-		blk_iopoll_init(&pbe_eq->iopoll, be_iopoll_budget,
+		irq_poll_init(&pbe_eq->iopoll, be_iopoll_budget,
 				be_iopoll);
-		blk_iopoll_enable(&pbe_eq->iopoll);
 	}
 
 	i = (phba->msix_enabled) ? i : 0;
@@ -5752,9 +5749,8 @@ static int beiscsi_dev_probe(struct pci_dev *pcidev,
 
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
-		blk_iopoll_init(&pbe_eq->iopoll, be_iopoll_budget,
+		irq_poll_init(&pbe_eq->iopoll, be_iopoll_budget,
 				be_iopoll);
-		blk_iopoll_enable(&pbe_eq->iopoll);
 	}
 
 	i = (phba->msix_enabled) ? i : 0;
@@ -5795,7 +5791,7 @@ free_blkenbld:
 	destroy_workqueue(phba->wq);
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
-		blk_iopoll_disable(&pbe_eq->iopoll);
+		irq_poll_disable(&pbe_eq->iopoll);
 	}
 free_twq:
 	beiscsi_clean_port(phba);
diff --git a/drivers/scsi/bfa/bfa.h b/drivers/scsi/bfa/bfa.h
index 4ad7e368bbc2..0e119d838e1b 100644
--- a/drivers/scsi/bfa/bfa.h
+++ b/drivers/scsi/bfa/bfa.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_core.c b/drivers/scsi/bfa/bfa_core.c
index e3f67b097a5c..2ea0db4b62a7 100644
--- a/drivers/scsi/bfa/bfa_core.c
+++ b/drivers/scsi/bfa/bfa_core.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_cs.h b/drivers/scsi/bfa/bfa_cs.h
index 91a8aa394db5..da9cf655be26 100644
--- a/drivers/scsi/bfa/bfa_cs.h
+++ b/drivers/scsi/bfa/bfa_cs.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_defs.h b/drivers/scsi/bfa/bfa_defs.h
index 877b86dd2837..5dc3782d615b 100644
--- a/drivers/scsi/bfa/bfa_defs.h
+++ b/drivers/scsi/bfa/bfa_defs.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_defs_fcs.h b/drivers/scsi/bfa/bfa_defs_fcs.h
index 06f0a163ca35..5815a904574d 100644
--- a/drivers/scsi/bfa/bfa_defs_fcs.h
+++ b/drivers/scsi/bfa/bfa_defs_fcs.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_defs_svc.h b/drivers/scsi/bfa/bfa_defs_svc.h
index 638f441ffc38..e81707f938cb 100644
--- a/drivers/scsi/bfa/bfa_defs_svc.h
+++ b/drivers/scsi/bfa/bfa_defs_svc.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fc.h b/drivers/scsi/bfa/bfa_fc.h
index 64069a0a3d0d..18b7304d6b0b 100644
--- a/drivers/scsi/bfa/bfa_fc.h
+++ b/drivers/scsi/bfa/bfa_fc.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcbuild.c b/drivers/scsi/bfa/bfa_fcbuild.c
index dce787f6cca2..b8dadc9cc993 100644
--- a/drivers/scsi/bfa/bfa_fcbuild.c
+++ b/drivers/scsi/bfa/bfa_fcbuild.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcbuild.h b/drivers/scsi/bfa/bfa_fcbuild.h
index 03c753d1e548..b109a8813401 100644
--- a/drivers/scsi/bfa/bfa_fcbuild.h
+++ b/drivers/scsi/bfa/bfa_fcbuild.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcpim.c b/drivers/scsi/bfa/bfa_fcpim.c
index d7385d1d9c5a..20982e7cdd81 100644
--- a/drivers/scsi/bfa/bfa_fcpim.c
+++ b/drivers/scsi/bfa/bfa_fcpim.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcpim.h b/drivers/scsi/bfa/bfa_fcpim.h
index e693af6e5930..e93921dec347 100644
--- a/drivers/scsi/bfa/bfa_fcpim.h
+++ b/drivers/scsi/bfa/bfa_fcpim.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcs.c b/drivers/scsi/bfa/bfa_fcs.c
index 0f19455951ec..1e7e139d71ea 100644
--- a/drivers/scsi/bfa/bfa_fcs.c
+++ b/drivers/scsi/bfa/bfa_fcs.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcs.h b/drivers/scsi/bfa/bfa_fcs.h
index 42bcb970445a..06dc215ea050 100644
--- a/drivers/scsi/bfa/bfa_fcs.h
+++ b/drivers/scsi/bfa/bfa_fcs.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -633,7 +634,7 @@ void bfa_fcs_fcpim_uf_recv(struct bfa_fcs_itnim_s *itnim,
 
 /*
  * HBA Attribute Block : BFA internal representation. Note : Some variable
- * sizes have been trimmed to suit BFA For Ex : Model will be "Brocade". Based
+ * sizes have been trimmed to suit BFA For Ex : Model will be "QLogic ". Based
  * on this the size has been reduced to 16 bytes from the standard's 64 bytes.
  */
 struct bfa_fcs_fdmi_hba_attr_s {
diff --git a/drivers/scsi/bfa/bfa_fcs_fcpim.c b/drivers/scsi/bfa/bfa_fcs_fcpim.c
index 6dc7926a3edd..4f089d76afb1 100644
--- a/drivers/scsi/bfa/bfa_fcs_fcpim.c
+++ b/drivers/scsi/bfa/bfa_fcs_fcpim.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_fcs_lport.c b/drivers/scsi/bfa/bfa_fcs_lport.c
index ff75ef891755..7733ad5305d4 100644
--- a/drivers/scsi/bfa/bfa_fcs_lport.c
+++ b/drivers/scsi/bfa/bfa_fcs_lport.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -2653,7 +2654,7 @@ bfa_fcs_fdmi_get_hbaattr(struct bfa_fcs_lport_fdmi_s *fdmi,
 
 	strncpy(hba_attr->node_sym_name.symname,
 		port->port_cfg.node_sym_name.symname, BFA_SYMNAME_MAXLEN);
-	strcpy(hba_attr->vendor_info, "BROCADE");
+	strcpy(hba_attr->vendor_info, "QLogic");
 	hba_attr->num_ports =
 		cpu_to_be32(bfa_ioc_get_nports(&port->fcs->bfa->ioc));
 	hba_attr->fabric_name = port->fabric->lps->pr_nwwn;
diff --git a/drivers/scsi/bfa/bfa_fcs_rport.c b/drivers/scsi/bfa/bfa_fcs_rport.c
index 2035b0d64351..de50349a39ce 100644
--- a/drivers/scsi/bfa/bfa_fcs_rport.c
+++ b/drivers/scsi/bfa/bfa_fcs_rport.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_hw_cb.c b/drivers/scsi/bfa/bfa_hw_cb.c
index ea24d4c6e67a..c4a0c0eb88a5 100644
--- a/drivers/scsi/bfa/bfa_hw_cb.c
+++ b/drivers/scsi/bfa/bfa_hw_cb.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_hw_ct.c b/drivers/scsi/bfa/bfa_hw_ct.c
index 637527f48b40..b0ff378dece2 100644
--- a/drivers/scsi/bfa/bfa_hw_ct.c
+++ b/drivers/scsi/bfa/bfa_hw_ct.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_ioc.c b/drivers/scsi/bfa/bfa_ioc.c
index 98f7e8cca52d..251e2ff8ff5f 100644
--- a/drivers/scsi/bfa/bfa_ioc.c
+++ b/drivers/scsi/bfa/bfa_ioc.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -2697,7 +2698,7 @@ bfa_ioc_reset_fwstate(struct bfa_ioc_s *ioc)
 	bfa_ioc_set_alt_ioc_fwstate(ioc, BFI_IOC_UNINIT);
 }
 
-#define BFA_MFG_NAME "Brocade"
+#define BFA_MFG_NAME "QLogic"
 void
 bfa_ioc_get_adapter_attr(struct bfa_ioc_s *ioc,
 			 struct bfa_adapter_attr_s *ad_attr)
diff --git a/drivers/scsi/bfa/bfa_ioc.h b/drivers/scsi/bfa/bfa_ioc.h
index a38aafa030b3..713745da44c6 100644
--- a/drivers/scsi/bfa/bfa_ioc.h
+++ b/drivers/scsi/bfa/bfa_ioc.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_ioc_cb.c b/drivers/scsi/bfa/bfa_ioc_cb.c
index 453c2f5b5561..f1b80da298c8 100644
--- a/drivers/scsi/bfa/bfa_ioc_cb.c
+++ b/drivers/scsi/bfa/bfa_ioc_cb.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_ioc_ct.c b/drivers/scsi/bfa/bfa_ioc_ct.c
index bd53150e4ee0..651a8fb93037 100644
--- a/drivers/scsi/bfa/bfa_ioc_ct.c
+++ b/drivers/scsi/bfa/bfa_ioc_ct.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_modules.h b/drivers/scsi/bfa/bfa_modules.h
index a14c784ff3fc..53135f21fa0e 100644
--- a/drivers/scsi/bfa/bfa_modules.h
+++ b/drivers/scsi/bfa/bfa_modules.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_plog.h b/drivers/scsi/bfa/bfa_plog.h
index 1c9baa68339b..da570c0b8275 100644
--- a/drivers/scsi/bfa/bfa_plog.h
+++ b/drivers/scsi/bfa/bfa_plog.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_port.c b/drivers/scsi/bfa/bfa_port.c
index 8ea7697deb9b..da1721e0d167 100644
--- a/drivers/scsi/bfa/bfa_port.c
+++ b/drivers/scsi/bfa/bfa_port.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_port.h b/drivers/scsi/bfa/bfa_port.h
index 2fcab6bc6280..26dc1bf14c85 100644
--- a/drivers/scsi/bfa/bfa_port.h
+++ b/drivers/scsi/bfa/bfa_port.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_svc.c b/drivers/scsi/bfa/bfa_svc.c
index 625225f31081..12de292175ef 100644
--- a/drivers/scsi/bfa/bfa_svc.c
+++ b/drivers/scsi/bfa/bfa_svc.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfa_svc.h b/drivers/scsi/bfa/bfa_svc.h
index ef07365991e7..ea2278bc78a8 100644
--- a/drivers/scsi/bfa/bfa_svc.h
+++ b/drivers/scsi/bfa/bfa_svc.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfad.c b/drivers/scsi/bfa/bfad.c
index cc3b9d3d6d40..9d253cb83ee7 100644
--- a/drivers/scsi/bfa/bfad.c
+++ b/drivers/scsi/bfa/bfad.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -130,13 +131,9 @@ MODULE_PARM_DESC(bfa_linkup_delay, "Link up delay, default=30 secs for "
 			"boot port. Otherwise 10 secs in RHEL4 & 0 for "
 			"[RHEL5, SLES10, ESX40] Range[>0]");
 module_param(msix_disable_cb, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(msix_disable_cb, "Disable Message Signaled Interrupts "
-			"for Brocade-415/425/815/825 cards, default=0, "
-			" Range[false:0|true:1]");
+MODULE_PARM_DESC(msix_disable_cb, "Disable Message Signaled Interrupts for QLogic-415/425/815/825 cards, default=0 Range[false:0|true:1]");
 module_param(msix_disable_ct, int, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(msix_disable_ct, "Disable Message Signaled Interrupts "
-			"if possible for Brocade-1010/1020/804/1007/902/1741 "
-			"cards, default=0, Range[false:0|true:1]");
+MODULE_PARM_DESC(msix_disable_ct, "Disable Message Signaled Interrupts if possible for QLogic-1010/1020/804/1007/902/1741 cards, default=0, Range[false:0|true:1]");
 module_param(fdmi_enable, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(fdmi_enable, "Enables fdmi registration, default=1, "
 				"Range[false:0|true:1]");
@@ -838,8 +835,7 @@ bfad_drv_init(struct bfad_s *bfad)
 		printk(KERN_WARNING "bfad%d bfad_hal_mem_alloc failure\n",
 		       bfad->inst_no);
 		printk(KERN_WARNING
-			"Not enough memory to attach all Brocade HBA ports, %s",
-			"System may need more memory.\n");
+			"Not enough memory to attach all QLogic BR-series HBA ports. System may need more memory.\n");
 		return BFA_STATUS_FAILED;
 	}
 
@@ -1710,7 +1706,7 @@ bfad_init(void)
 {
 	int		error = 0;
 
-	printk(KERN_INFO "Brocade BFA FC/FCOE SCSI driver - version: %s\n",
+	pr_info("QLogic BR-series BFA FC/FCOE SCSI driver - version: %s\n",
 			BFAD_DRIVER_VERSION);
 
 	if (num_sgpgs > 0)
@@ -1817,6 +1813,6 @@ bfad_free_fwimg(void)
 module_init(bfad_init);
 module_exit(bfad_exit);
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Brocade Fibre Channel HBA Driver" BFAD_PROTO_NAME);
-MODULE_AUTHOR("Brocade Communications Systems, Inc.");
+MODULE_DESCRIPTION("QLogic BR-series Fibre Channel HBA Driver" BFAD_PROTO_NAME);
+MODULE_AUTHOR("QLogic Corporation");
 MODULE_VERSION(BFAD_DRIVER_VERSION);
diff --git a/drivers/scsi/bfa/bfad_attr.c b/drivers/scsi/bfa/bfad_attr.c
index 40be670a1cbc..13db3b7bc873 100644
--- a/drivers/scsi/bfa/bfad_attr.c
+++ b/drivers/scsi/bfa/bfad_attr.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -750,65 +751,65 @@ bfad_im_model_desc_show(struct device *dev, struct device_attribute *attr,
 
 	bfa_get_adapter_model(&bfad->bfa, model);
 	nports = bfa_get_nports(&bfad->bfa);
-	if (!strcmp(model, "Brocade-425"))
+	if (!strcmp(model, "QLogic-425"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 4Gbps PCIe dual port FC HBA");
-	else if (!strcmp(model, "Brocade-825"))
+			"QLogic BR-series 4Gbps PCIe dual port FC HBA");
+	else if (!strcmp(model, "QLogic-825"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 8Gbps PCIe dual port FC HBA");
-	else if (!strcmp(model, "Brocade-42B"))
+			"QLogic BR-series 8Gbps PCIe dual port FC HBA");
+	else if (!strcmp(model, "QLogic-42B"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 4Gbps PCIe dual port FC HBA for HP");
-	else if (!strcmp(model, "Brocade-82B"))
+			"QLogic BR-series 4Gbps PCIe dual port FC HBA for HP");
+	else if (!strcmp(model, "QLogic-82B"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 8Gbps PCIe dual port FC HBA for HP");
-	else if (!strcmp(model, "Brocade-1010"))
+			"QLogic BR-series 8Gbps PCIe dual port FC HBA for HP");
+	else if (!strcmp(model, "QLogic-1010"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 10Gbps single port CNA");
-	else if (!strcmp(model, "Brocade-1020"))
+			"QLogic BR-series 10Gbps single port CNA");
+	else if (!strcmp(model, "QLogic-1020"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 10Gbps dual port CNA");
-	else if (!strcmp(model, "Brocade-1007"))
+			"QLogic BR-series 10Gbps dual port CNA");
+	else if (!strcmp(model, "QLogic-1007"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 10Gbps CNA for IBM Blade Center");
-	else if (!strcmp(model, "Brocade-415"))
+			"QLogic BR-series 10Gbps CNA for IBM Blade Center");
+	else if (!strcmp(model, "QLogic-415"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 4Gbps PCIe single port FC HBA");
-	else if (!strcmp(model, "Brocade-815"))
+			"QLogic BR-series 4Gbps PCIe single port FC HBA");
+	else if (!strcmp(model, "QLogic-815"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 8Gbps PCIe single port FC HBA");
-	else if (!strcmp(model, "Brocade-41B"))
+			"QLogic BR-series 8Gbps PCIe single port FC HBA");
+	else if (!strcmp(model, "QLogic-41B"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 4Gbps PCIe single port FC HBA for HP");
-	else if (!strcmp(model, "Brocade-81B"))
+			"QLogic BR-series 4Gbps PCIe single port FC HBA for HP");
+	else if (!strcmp(model, "QLogic-81B"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 8Gbps PCIe single port FC HBA for HP");
-	else if (!strcmp(model, "Brocade-804"))
+			"QLogic BR-series 8Gbps PCIe single port FC HBA for HP");
+	else if (!strcmp(model, "QLogic-804"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 8Gbps FC HBA for HP Bladesystem C-class");
-	else if (!strcmp(model, "Brocade-1741"))
+			"QLogic BR-series 8Gbps FC HBA for HP Bladesystem C-class");
+	else if (!strcmp(model, "QLogic-1741"))
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-			"Brocade 10Gbps CNA for Dell M-Series Blade Servers");
-	else if (strstr(model, "Brocade-1860")) {
+			"QLogic BR-series 10Gbps CNA for Dell M-Series Blade Servers");
+	else if (strstr(model, "QLogic-1860")) {
 		if (nports == 1 && bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 10Gbps single port CNA");
+				"QLogic BR-series 10Gbps single port CNA");
 		else if (nports == 1 && !bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe single port FC HBA");
+				"QLogic BR-series 16Gbps PCIe single port FC HBA");
 		else if (nports == 2 && bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 10Gbps dual port CNA");
+				"QLogic BR-series 10Gbps dual port CNA");
 		else if (nports == 2 && !bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe dual port FC HBA");
-	} else if (!strcmp(model, "Brocade-1867")) {
+				"QLogic BR-series 16Gbps PCIe dual port FC HBA");
+	} else if (!strcmp(model, "QLogic-1867")) {
 		if (nports == 1 && !bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe single port FC HBA for IBM");
+				"QLogic BR-series 16Gbps PCIe single port FC HBA for IBM");
 		else if (nports == 2 && !bfa_ioc_is_cna(&bfad->bfa.ioc))
 			snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
-				"Brocade 16Gbps PCIe dual port FC HBA for IBM");
+				"QLogic BR-series 16Gbps PCIe dual port FC HBA for IBM");
 	} else
 		snprintf(model_descr, BFA_ADAPTER_MODEL_DESCR_LEN,
 			"Invalid Model");
diff --git a/drivers/scsi/bfa/bfad_bsg.c b/drivers/scsi/bfa/bfad_bsg.c
index 023b9d42ad9a..d1ad0208dfe7 100644
--- a/drivers/scsi/bfa/bfad_bsg.c
+++ b/drivers/scsi/bfa/bfad_bsg.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfad_bsg.h b/drivers/scsi/bfa/bfad_bsg.h
index 90abef691585..917e140dfbcc 100644
--- a/drivers/scsi/bfa/bfad_bsg.h
+++ b/drivers/scsi/bfa/bfad_bsg.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfad_debugfs.c b/drivers/scsi/bfa/bfad_debugfs.c
index 74a307c0a240..8dcd8c70c7ee 100644
--- a/drivers/scsi/bfa/bfad_debugfs.c
+++ b/drivers/scsi/bfa/bfad_debugfs.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfad_drv.h b/drivers/scsi/bfa/bfad_drv.h
index 8b97877d42cf..f9e862093a25 100644
--- a/drivers/scsi/bfa/bfad_drv.h
+++ b/drivers/scsi/bfa/bfad_drv.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -57,7 +58,7 @@
 #ifdef BFA_DRIVER_VERSION
 #define BFAD_DRIVER_VERSION    BFA_DRIVER_VERSION
 #else
-#define BFAD_DRIVER_VERSION    "3.2.23.0"
+#define BFAD_DRIVER_VERSION    "3.2.25.0"
 #endif
 
 #define BFAD_PROTO_NAME FCPI_NAME
diff --git a/drivers/scsi/bfa/bfad_im.c b/drivers/scsi/bfa/bfad_im.c
index 299c6f80d460..6c805e13f8dd 100644
--- a/drivers/scsi/bfa/bfad_im.c
+++ b/drivers/scsi/bfa/bfad_im.c
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -185,7 +186,7 @@ bfad_im_info(struct Scsi_Host *shost)
 
 	memset(bfa_buf, 0, sizeof(bfa_buf));
 	snprintf(bfa_buf, sizeof(bfa_buf),
-		"Brocade FC/FCOE Adapter, " "hwpath: %s driver: %s",
+		"QLogic BR-series FC/FCOE Adapter, hwpath: %s driver: %s",
 		bfad->pci_name, BFAD_DRIVER_VERSION);
 
 	return bfa_buf;
@@ -271,6 +272,19 @@ bfad_im_target_reset_send(struct bfad_s *bfad, struct scsi_cmnd *cmnd,
 	cmnd->host_scribble = NULL;
 	cmnd->SCp.Status = 0;
 	bfa_itnim = bfa_fcs_itnim_get_halitn(&itnim->fcs_itnim);
+	/*
+	 * bfa_itnim can be NULL if the port gets disconnected and the bfa
+	 * and fcs layers have cleaned up their nexus with the targets and
+	 * the same has not been cleaned up by the shim
+	 */
+	if (bfa_itnim == NULL) {
+		bfa_tskim_free(tskim);
+		BFA_LOG(KERN_ERR, bfad, bfa_log_level,
+			"target reset, bfa_itnim is NULL\n");
+		rc = BFA_STATUS_FAILED;
+		goto out;
+	}
+
 	memset(&scsilun, 0, sizeof(scsilun));
 	bfa_tskim_start(tskim, bfa_itnim, scsilun,
 			    FCP_TM_TARGET_RESET, BFAD_TARGET_RESET_TMO);
@@ -326,6 +340,19 @@ bfad_im_reset_lun_handler(struct scsi_cmnd *cmnd)
 	cmnd->SCp.ptr = (char *)&wq;
 	cmnd->SCp.Status = 0;
 	bfa_itnim = bfa_fcs_itnim_get_halitn(&itnim->fcs_itnim);
+	/*
+	 * bfa_itnim can be NULL if the port gets disconnected and the bfa
+	 * and fcs layers have cleaned up their nexus with the targets and
+	 * the same has not been cleaned up by the shim
+	 */
+	if (bfa_itnim == NULL) {
+		bfa_tskim_free(tskim);
+		BFA_LOG(KERN_ERR, bfad, bfa_log_level,
+			"lun reset, bfa_itnim is NULL\n");
+		spin_unlock_irqrestore(&bfad->bfad_lock, flags);
+		rc = FAILED;
+		goto out;
+	}
 	int_to_scsilun(cmnd->device->lun, &scsilun);
 	bfa_tskim_start(tskim, bfa_itnim, scsilun,
 			    FCP_TM_LUN_RESET, BFAD_LUN_RESET_TMO);
diff --git a/drivers/scsi/bfa/bfad_im.h b/drivers/scsi/bfa/bfad_im.h
index f6c1023e502a..836fdc221edd 100644
--- a/drivers/scsi/bfa/bfad_im.h
+++ b/drivers/scsi/bfa/bfad_im.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfi.h b/drivers/scsi/bfa/bfi.h
index 9ef91f907dec..97600dcec649 100644
--- a/drivers/scsi/bfa/bfi.h
+++ b/drivers/scsi/bfa/bfi.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfi_ms.h b/drivers/scsi/bfa/bfi_ms.h
index 1a3fe5ad58fa..ae5bfe039fcc 100644
--- a/drivers/scsi/bfa/bfi_ms.h
+++ b/drivers/scsi/bfa/bfi_ms.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
diff --git a/drivers/scsi/bfa/bfi_reg.h b/drivers/scsi/bfa/bfi_reg.h
index 99133bcf53f9..fd5b87616e8b 100644
--- a/drivers/scsi/bfa/bfi_reg.h
+++ b/drivers/scsi/bfa/bfi_reg.h
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2005-2010 Brocade Communications Systems, Inc.
+ * Copyright (c) 2005-2014 Brocade Communications Systems, Inc.
+ * Copyright (c) 2014- QLogic Corporation.
  * All rights reserved
- * www.brocade.com
+ * www.qlogic.com
  *
- * Linux driver for Brocade Fibre Channel Host Bus Adapter.
+ * Linux driver for QLogic BR-series Fibre Channel Host Bus Adapter.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License (GPL) Version 2 as
@@ -16,7 +17,7 @@
  */
 
 /*
- * bfi_reg.h ASIC register defines for all Brocade adapter ASICs
+ * bfi_reg.h ASIC register defines for all QLogic BR-series adapter ASICs
  */
 
 #ifndef __BFI_REG_H__
diff --git a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
index 0e2bee937fe8..e22a268fd311 100644
--- a/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
+++ b/drivers/scsi/cxgbi/cxgb3i/cxgb3i.c
@@ -57,7 +57,7 @@ MODULE_PARM_DESC(cxgb3i_snd_win, "TCP send window in bytes (default=128KB)");
 
 static int cxgb3i_rx_credit_thres = 10 * 1024;
 module_param(cxgb3i_rx_credit_thres, int, 0644);
-MODULE_PARM_DESC(rx_credit_thres,
+MODULE_PARM_DESC(cxgb3i_rx_credit_thres,
 		 "RX credits return threshold in bytes (default=10KB)");
 
 static unsigned int cxgb3i_max_connect = 8 * 1024;
diff --git a/drivers/scsi/cxlflash/common.h b/drivers/scsi/cxlflash/common.h
index c11cd193f896..5ada9268a450 100644
--- a/drivers/scsi/cxlflash/common.h
+++ b/drivers/scsi/cxlflash/common.h
@@ -165,6 +165,8 @@ struct afu {
 	struct sisl_host_map __iomem *host_map;		/* MC host map */
 	struct sisl_ctrl_map __iomem *ctrl_map;		/* MC control map */
 
+	struct kref mapcount;
+
 	ctx_hndl_t ctx_hndl;	/* master's context handle */
 	u64 *hrrq_start;
 	u64 *hrrq_end;
diff --git a/drivers/scsi/cxlflash/main.c b/drivers/scsi/cxlflash/main.c
index 1e5bf0ca81da..f6d90ce8f3b7 100644
--- a/drivers/scsi/cxlflash/main.c
+++ b/drivers/scsi/cxlflash/main.c
@@ -368,6 +368,7 @@ out:
 
 no_room:
 	afu->read_room = true;
+	kref_get(&cfg->afu->mapcount);
 	schedule_work(&cfg->work_q);
 	rc = SCSI_MLQUEUE_HOST_BUSY;
 	goto out;
@@ -473,6 +474,16 @@ out:
 	return rc;
 }
 
+static void afu_unmap(struct kref *ref)
+{
+	struct afu *afu = container_of(ref, struct afu, mapcount);
+
+	if (likely(afu->afu_map)) {
+		cxl_psa_unmap((void __iomem *)afu->afu_map);
+		afu->afu_map = NULL;
+	}
+}
+
 /**
  * cxlflash_driver_info() - information handler for this host driver
  * @host:	SCSI host associated with device.
@@ -503,6 +514,7 @@ static int cxlflash_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *scp)
 	ulong lock_flags;
 	short lflag = 0;
 	int rc = 0;
+	int kref_got = 0;
 
 	dev_dbg_ratelimited(dev, "%s: (scp=%p) %d/%d/%d/%llu "
 			    "cdb=(%08X-%08X-%08X-%08X)\n",
@@ -547,6 +559,9 @@ static int cxlflash_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *scp)
 		goto out;
 	}
 
+	kref_get(&cfg->afu->mapcount);
+	kref_got = 1;
+
 	cmd->rcb.ctx_id = afu->ctx_hndl;
 	cmd->rcb.port_sel = port_sel;
 	cmd->rcb.lun_id = lun_to_lunid(scp->device->lun);
@@ -587,6 +602,8 @@ static int cxlflash_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *scp)
 	}
 
 out:
+	if (kref_got)
+		kref_put(&afu->mapcount, afu_unmap);
 	pr_devel("%s: returning rc=%d\n", __func__, rc);
 	return rc;
 }
@@ -632,20 +649,36 @@ static void free_mem(struct cxlflash_cfg *cfg)
  * @cfg:	Internal structure associated with the host.
  *
  * Safe to call with AFU in a partially allocated/initialized state.
+ *
+ * Cleans up all state associated with the command queue, and unmaps
+ * the MMIO space.
+ *
+ *  - complete() will take care of commands we initiated (they'll be checked
+ *  in as part of the cleanup that occurs after the completion)
+ *
+ *  - cmd_checkin() will take care of entries that we did not initiate and that
+ *  have not (and will not) complete because they are sitting on a [now stale]
+ *  hardware queue
  */
 static void stop_afu(struct cxlflash_cfg *cfg)
 {
 	int i;
 	struct afu *afu = cfg->afu;
+	struct afu_cmd *cmd;
 
 	if (likely(afu)) {
-		for (i = 0; i < CXLFLASH_NUM_CMDS; i++)
-			complete(&afu->cmd[i].cevent);
+		for (i = 0; i < CXLFLASH_NUM_CMDS; i++) {
+			cmd = &afu->cmd[i];
+			complete(&cmd->cevent);
+			if (!atomic_read(&cmd->free))
+				cmd_checkin(cmd);
+		}
 
 		if (likely(afu->afu_map)) {
 			cxl_psa_unmap((void __iomem *)afu->afu_map);
 			afu->afu_map = NULL;
 		}
+		kref_put(&afu->mapcount, afu_unmap);
 	}
 }
 
@@ -731,8 +764,8 @@ static void cxlflash_remove(struct pci_dev *pdev)
 		scsi_remove_host(cfg->host);
 		/* fall through */
 	case INIT_STATE_AFU:
-		term_afu(cfg);
 		cancel_work_sync(&cfg->work_q);
+		term_afu(cfg);
 	case INIT_STATE_PCI:
 		pci_release_regions(cfg->dev);
 		pci_disable_device(pdev);
@@ -1108,7 +1141,7 @@ static const struct asyc_intr_info ainfo[] = {
 	{SISL_ASTATUS_FC1_OTHER, "other error", 1, CLR_FC_ERROR | LINK_RESET},
 	{SISL_ASTATUS_FC1_LOGO, "target initiated LOGO", 1, 0},
 	{SISL_ASTATUS_FC1_CRC_T, "CRC threshold exceeded", 1, LINK_RESET},
-	{SISL_ASTATUS_FC1_LOGI_R, "login timed out, retrying", 1, 0},
+	{SISL_ASTATUS_FC1_LOGI_R, "login timed out, retrying", 1, LINK_RESET},
 	{SISL_ASTATUS_FC1_LOGI_F, "login failed", 1, CLR_FC_ERROR},
 	{SISL_ASTATUS_FC1_LOGI_S, "login succeeded", 1, SCAN_HOST},
 	{SISL_ASTATUS_FC1_LINK_DN, "link down", 1, 0},
@@ -1316,6 +1349,7 @@ static irqreturn_t cxlflash_async_err_irq(int irq, void *data)
 				__func__, port);
 			cfg->lr_state = LINK_RESET_REQUIRED;
 			cfg->lr_port = port;
+			kref_get(&cfg->afu->mapcount);
 			schedule_work(&cfg->work_q);
 		}
 
@@ -1336,6 +1370,7 @@ static irqreturn_t cxlflash_async_err_irq(int irq, void *data)
 
 		if (info->action & SCAN_HOST) {
 			atomic_inc(&cfg->scan_host_needed);
+			kref_get(&cfg->afu->mapcount);
 			schedule_work(&cfg->work_q);
 		}
 	}
@@ -1731,6 +1766,7 @@ static int init_afu(struct cxlflash_cfg *cfg)
 		rc = -ENOMEM;
 		goto err1;
 	}
+	kref_init(&afu->mapcount);
 
 	/* No byte reverse on reading afu_version or string will be backwards */
 	reg = readq(&afu->afu_map->global.regs.afu_version);
@@ -1765,8 +1801,7 @@ out:
 	return rc;
 
 err2:
-	cxl_psa_unmap((void __iomem *)afu->afu_map);
-	afu->afu_map = NULL;
+	kref_put(&afu->mapcount, afu_unmap);
 err1:
 	term_mc(cfg, UNDO_START);
 	goto out;
@@ -2274,6 +2309,7 @@ static struct scsi_host_template driver_template = {
  * Device dependent values
  */
 static struct dev_dependent_vals dev_corsa_vals = { CXLFLASH_MAX_SECTORS };
+static struct dev_dependent_vals dev_flash_gt_vals = { CXLFLASH_MAX_SECTORS };
 
 /*
  * PCI device binding table
@@ -2281,6 +2317,8 @@ static struct dev_dependent_vals dev_corsa_vals = { CXLFLASH_MAX_SECTORS };
 static struct pci_device_id cxlflash_pci_table[] = {
 	{PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_IBM_CORSA,
 	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, (kernel_ulong_t)&dev_corsa_vals},
+	{PCI_VENDOR_ID_IBM, PCI_DEVICE_ID_IBM_FLASH_GT,
+	 PCI_ANY_ID, PCI_ANY_ID, 0, 0, (kernel_ulong_t)&dev_flash_gt_vals},
 	{}
 };
 
@@ -2339,6 +2377,7 @@ static void cxlflash_worker_thread(struct work_struct *work)
 
 	if (atomic_dec_if_positive(&cfg->scan_host_needed) >= 0)
 		scsi_scan_host(cfg->host);
+	kref_put(&afu->mapcount, afu_unmap);
 }
 
 /**
@@ -2585,8 +2624,7 @@ static struct pci_driver cxlflash_driver = {
  */
 static int __init init_cxlflash(void)
 {
-	pr_info("%s: IBM Power CXL Flash Adapter: %s\n",
-		__func__, CXLFLASH_DRIVER_DATE);
+	pr_info("%s: %s\n", __func__, CXLFLASH_ADAPTER_NAME);
 
 	cxlflash_list_init();
 
diff --git a/drivers/scsi/cxlflash/main.h b/drivers/scsi/cxlflash/main.h
index 60324566c14f..0faed422c7f4 100644
--- a/drivers/scsi/cxlflash/main.h
+++ b/drivers/scsi/cxlflash/main.h
@@ -22,10 +22,9 @@
 
 #define CXLFLASH_NAME		"cxlflash"
 #define CXLFLASH_ADAPTER_NAME	"IBM POWER CXL Flash Adapter"
-#define CXLFLASH_DRIVER_DATE	"(August 13, 2015)"
 
-#define PCI_DEVICE_ID_IBM_CORSA	0x04F0
-#define CXLFLASH_SUBS_DEV_ID	0x04F0
+#define PCI_DEVICE_ID_IBM_CORSA		0x04F0
+#define PCI_DEVICE_ID_IBM_FLASH_GT	0x0600
 
 /* Since there is only one target, make it 0 */
 #define CXLFLASH_TARGET		0
diff --git a/drivers/scsi/cxlflash/superpipe.c b/drivers/scsi/cxlflash/superpipe.c
index cac2e6a50efd..f4020dbb55c3 100644
--- a/drivers/scsi/cxlflash/superpipe.c
+++ b/drivers/scsi/cxlflash/superpipe.c
@@ -1372,7 +1372,7 @@ static int cxlflash_disk_attach(struct scsi_device *sdev,
 	}
 
 	ctx = cxl_dev_context_init(cfg->dev);
-	if (unlikely(IS_ERR_OR_NULL(ctx))) {
+	if (IS_ERR_OR_NULL(ctx)) {
 		dev_err(dev, "%s: Could not initialize context %p\n",
 			__func__, ctx);
 		rc = -ENODEV;
@@ -1380,7 +1380,7 @@ static int cxlflash_disk_attach(struct scsi_device *sdev,
 	}
 
 	ctxid = cxl_process_element(ctx);
-	if (unlikely((ctxid > MAX_CONTEXT) || (ctxid < 0))) {
+	if (unlikely((ctxid >= MAX_CONTEXT) || (ctxid < 0))) {
 		dev_err(dev, "%s: ctxid (%d) invalid!\n", __func__, ctxid);
 		rc = -EPERM;
 		goto err2;
@@ -1500,7 +1500,7 @@ static int recover_context(struct cxlflash_cfg *cfg, struct ctx_info *ctxi)
 	struct afu *afu = cfg->afu;
 
 	ctx = cxl_dev_context_init(cfg->dev);
-	if (unlikely(IS_ERR_OR_NULL(ctx))) {
+	if (IS_ERR_OR_NULL(ctx)) {
 		dev_err(dev, "%s: Could not initialize context %p\n",
 			__func__, ctx);
 		rc = -ENODEV;
@@ -1508,7 +1508,7 @@ static int recover_context(struct cxlflash_cfg *cfg, struct ctx_info *ctxi)
 	}
 
 	ctxid = cxl_process_element(ctx);
-	if (unlikely((ctxid > MAX_CONTEXT) || (ctxid < 0))) {
+	if (unlikely((ctxid >= MAX_CONTEXT) || (ctxid < 0))) {
 		dev_err(dev, "%s: ctxid (%d) invalid!\n", __func__, ctxid);
 		rc = -EPERM;
 		goto err1;
diff --git a/drivers/scsi/cxlflash/vlun.c b/drivers/scsi/cxlflash/vlun.c
index a53f583e2d7b..50f8e9300770 100644
--- a/drivers/scsi/cxlflash/vlun.c
+++ b/drivers/scsi/cxlflash/vlun.c
@@ -1008,6 +1008,8 @@ int cxlflash_disk_virtual_open(struct scsi_device *sdev, void *arg)
 	virt->last_lba = last_lba;
 	virt->rsrc_handle = rsrc_handle;
 
+	if (lli->port_sel == BOTH_PORTS)
+		virt->hdr.return_flags |= DK_CXLFLASH_ALL_PORTS_ACTIVE;
 out:
 	if (likely(ctxi))
 		put_context(ctxi);
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index cc2773b5de68..5a328bf81836 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -22,7 +22,9 @@
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/module.h>
+#include <asm/unaligned.h>
 #include <scsi/scsi.h>
+#include <scsi/scsi_dbg.h>
 #include <scsi/scsi_eh.h>
 #include <scsi/scsi_dh.h>
 
@@ -58,8 +60,9 @@
 #define ALUA_FAILOVER_TIMEOUT		60
 #define ALUA_FAILOVER_RETRIES		5
 
-/* flags passed from user level */
+/* device handler flags */
 #define ALUA_OPTIMIZE_STPG		1
+#define ALUA_RTPG_EXT_HDR_UNSUPP	2
 
 struct alua_dh_data {
 	int			group_id;
@@ -73,7 +76,6 @@ struct alua_dh_data {
 	int			bufflen;
 	unsigned char		transition_tmo;
 	unsigned char		sense[SCSI_SENSE_BUFFERSIZE];
-	int			senselen;
 	struct scsi_device	*sdev;
 	activate_complete	callback_fn;
 	void			*callback_data;
@@ -83,7 +85,6 @@ struct alua_dh_data {
 #define ALUA_POLICY_SWITCH_ALL		1
 
 static char print_alua_state(int);
-static int alua_check_sense(struct scsi_device *, struct scsi_sense_hdr *);
 
 static int realloc_buffer(struct alua_dh_data *h, unsigned len)
 {
@@ -131,93 +132,47 @@ static struct request *get_alua_req(struct scsi_device *sdev,
 }
 
 /*
- * submit_vpd_inquiry - Issue an INQUIRY VPD page 0x83 command
- * @sdev: sdev the command should be sent to
- */
-static int submit_vpd_inquiry(struct scsi_device *sdev, struct alua_dh_data *h)
-{
-	struct request *rq;
-	int err = SCSI_DH_RES_TEMP_UNAVAIL;
-
-	rq = get_alua_req(sdev, h->buff, h->bufflen, READ);
-	if (!rq)
-		goto done;
-
-	/* Prepare the command. */
-	rq->cmd[0] = INQUIRY;
-	rq->cmd[1] = 1;
-	rq->cmd[2] = 0x83;
-	rq->cmd[4] = h->bufflen;
-	rq->cmd_len = COMMAND_SIZE(INQUIRY);
-
-	rq->sense = h->sense;
-	memset(rq->sense, 0, SCSI_SENSE_BUFFERSIZE);
-	rq->sense_len = h->senselen = 0;
-
-	err = blk_execute_rq(rq->q, NULL, rq, 1);
-	if (err == -EIO) {
-		sdev_printk(KERN_INFO, sdev,
-			    "%s: evpd inquiry failed with %x\n",
-			    ALUA_DH_NAME, rq->errors);
-		h->senselen = rq->sense_len;
-		err = SCSI_DH_IO;
-	}
-	blk_put_request(rq);
-done:
-	return err;
-}
-
-/*
  * submit_rtpg - Issue a REPORT TARGET GROUP STATES command
  * @sdev: sdev the command should be sent to
  */
-static unsigned submit_rtpg(struct scsi_device *sdev, struct alua_dh_data *h,
-			    bool rtpg_ext_hdr_req)
+static unsigned submit_rtpg(struct scsi_device *sdev, struct alua_dh_data *h)
 {
 	struct request *rq;
-	int err = SCSI_DH_RES_TEMP_UNAVAIL;
+	int err = 0;
 
 	rq = get_alua_req(sdev, h->buff, h->bufflen, READ);
-	if (!rq)
+	if (!rq) {
+		err = DRIVER_BUSY << 24;
 		goto done;
+	}
 
 	/* Prepare the command. */
 	rq->cmd[0] = MAINTENANCE_IN;
-	if (rtpg_ext_hdr_req)
+	if (!(h->flags & ALUA_RTPG_EXT_HDR_UNSUPP))
 		rq->cmd[1] = MI_REPORT_TARGET_PGS | MI_EXT_HDR_PARAM_FMT;
 	else
 		rq->cmd[1] = MI_REPORT_TARGET_PGS;
-	rq->cmd[6] = (h->bufflen >> 24) & 0xff;
-	rq->cmd[7] = (h->bufflen >> 16) & 0xff;
-	rq->cmd[8] = (h->bufflen >>  8) & 0xff;
-	rq->cmd[9] = h->bufflen & 0xff;
+	put_unaligned_be32(h->bufflen, &rq->cmd[6]);
 	rq->cmd_len = COMMAND_SIZE(MAINTENANCE_IN);
 
 	rq->sense = h->sense;
 	memset(rq->sense, 0, SCSI_SENSE_BUFFERSIZE);
-	rq->sense_len = h->senselen = 0;
+	rq->sense_len = 0;
 
-	err = blk_execute_rq(rq->q, NULL, rq, 1);
-	if (err == -EIO) {
-		sdev_printk(KERN_INFO, sdev,
-			    "%s: rtpg failed with %x\n",
-			    ALUA_DH_NAME, rq->errors);
-		h->senselen = rq->sense_len;
-		err = SCSI_DH_IO;
-	}
+	blk_execute_rq(rq->q, NULL, rq, 1);
+	if (rq->errors)
+		err = rq->errors;
 	blk_put_request(rq);
 done:
 	return err;
 }
 
 /*
- * alua_stpg - Evaluate SET TARGET GROUP STATES
+ * stpg_endio - Evaluate SET TARGET GROUP STATES
  * @sdev: the device to be evaluated
  * @state: the new target group state
  *
- * Send a SET TARGET GROUP STATES command to the device.
- * We only have to test here if we should resubmit the command;
- * any other error is assumed as a failure.
+ * Evaluate a SET TARGET GROUP STATES command response.
  */
 static void stpg_endio(struct request *req, int error)
 {
@@ -231,22 +186,21 @@ static void stpg_endio(struct request *req, int error)
 		goto done;
 	}
 
-	if (req->sense_len > 0) {
-		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
-					   &sense_hdr);
-		if (!err) {
-			err = SCSI_DH_IO;
+	if (scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
+				 &sense_hdr)) {
+		if (sense_hdr.sense_key == NOT_READY &&
+		    sense_hdr.asc == 0x04 && sense_hdr.ascq == 0x0a) {
+			/* ALUA state transition already in progress */
+			err = SCSI_DH_OK;
 			goto done;
 		}
-		err = alua_check_sense(h->sdev, &sense_hdr);
-		if (err == ADD_TO_MLQUEUE) {
+		if (sense_hdr.sense_key == UNIT_ATTENTION) {
 			err = SCSI_DH_RETRY;
 			goto done;
 		}
-		sdev_printk(KERN_INFO, h->sdev,
-			    "%s: stpg sense code: %02x/%02x/%02x\n",
-			    ALUA_DH_NAME, sense_hdr.sense_key,
-			    sense_hdr.asc, sense_hdr.ascq);
+		sdev_printk(KERN_INFO, h->sdev, "%s: stpg failed\n",
+			    ALUA_DH_NAME);
+		scsi_print_sense_hdr(h->sdev, ALUA_DH_NAME, &sense_hdr);
 		err = SCSI_DH_IO;
 	} else if (error)
 		err = SCSI_DH_IO;
@@ -284,8 +238,7 @@ static unsigned submit_stpg(struct alua_dh_data *h)
 	/* Prepare the data buffer */
 	memset(h->buff, 0, stpg_len);
 	h->buff[4] = TPGS_STATE_OPTIMIZED & 0x0f;
-	h->buff[6] = (h->group_id >> 8) & 0xff;
-	h->buff[7] = h->group_id & 0xff;
+	put_unaligned_be16(h->group_id, &h->buff[6]);
 
 	rq = get_alua_req(sdev, h->buff, stpg_len, WRITE);
 	if (!rq)
@@ -294,15 +247,12 @@ static unsigned submit_stpg(struct alua_dh_data *h)
 	/* Prepare the command. */
 	rq->cmd[0] = MAINTENANCE_OUT;
 	rq->cmd[1] = MO_SET_TARGET_PGS;
-	rq->cmd[6] = (stpg_len >> 24) & 0xff;
-	rq->cmd[7] = (stpg_len >> 16) & 0xff;
-	rq->cmd[8] = (stpg_len >>  8) & 0xff;
-	rq->cmd[9] = stpg_len & 0xff;
+	put_unaligned_be32(stpg_len, &rq->cmd[6]);
 	rq->cmd_len = COMMAND_SIZE(MAINTENANCE_OUT);
 
 	rq->sense = h->sense;
 	memset(rq->sense, 0, SCSI_SENSE_BUFFERSIZE);
-	rq->sense_len = h->senselen = 0;
+	rq->sense_len = 0;
 	rq->end_io_data = h;
 
 	blk_execute_rq_nowait(rq->q, NULL, rq, 1, stpg_endio);
@@ -316,12 +266,23 @@ static unsigned submit_stpg(struct alua_dh_data *h)
  * Examine the TPGS setting of the sdev to find out if ALUA
  * is supported.
  */
-static int alua_check_tpgs(struct scsi_device *sdev, struct alua_dh_data *h)
+static int alua_check_tpgs(struct scsi_device *sdev)
 {
-	int err = SCSI_DH_OK;
+	int tpgs = TPGS_MODE_NONE;
+
+	/*
+	 * ALUA support for non-disk devices is fraught with
+	 * difficulties, so disable it for now.
+	 */
+	if (sdev->type != TYPE_DISK) {
+		sdev_printk(KERN_INFO, sdev,
+			    "%s: disable for non-disk devices\n",
+			    ALUA_DH_NAME);
+		return tpgs;
+	}
 
-	h->tpgs = scsi_device_tpgs(sdev);
-	switch (h->tpgs) {
+	tpgs = scsi_device_tpgs(sdev);
+	switch (tpgs) {
 	case TPGS_MODE_EXPLICIT|TPGS_MODE_IMPLICIT:
 		sdev_printk(KERN_INFO, sdev,
 			    "%s: supports implicit and explicit TPGS\n",
@@ -335,71 +296,34 @@ static int alua_check_tpgs(struct scsi_device *sdev, struct alua_dh_data *h)
 		sdev_printk(KERN_INFO, sdev, "%s: supports implicit TPGS\n",
 			    ALUA_DH_NAME);
 		break;
-	default:
-		h->tpgs = TPGS_MODE_NONE;
+	case TPGS_MODE_NONE:
 		sdev_printk(KERN_INFO, sdev, "%s: not supported\n",
 			    ALUA_DH_NAME);
-		err = SCSI_DH_DEV_UNSUPP;
+		break;
+	default:
+		sdev_printk(KERN_INFO, sdev,
+			    "%s: unsupported TPGS setting %d\n",
+			    ALUA_DH_NAME, tpgs);
+		tpgs = TPGS_MODE_NONE;
 		break;
 	}
 
-	return err;
+	return tpgs;
 }
 
 /*
- * alua_vpd_inquiry - Evaluate INQUIRY vpd page 0x83
+ * alua_check_vpd - Evaluate INQUIRY vpd page 0x83
  * @sdev: device to be checked
  *
  * Extract the relative target port and the target port group
  * descriptor from the list of identificators.
  */
-static int alua_vpd_inquiry(struct scsi_device *sdev, struct alua_dh_data *h)
+static int alua_check_vpd(struct scsi_device *sdev, struct alua_dh_data *h)
 {
-	int len;
-	unsigned err;
-	unsigned char *d;
-
- retry:
-	err = submit_vpd_inquiry(sdev, h);
-
-	if (err != SCSI_DH_OK)
-		return err;
-
-	/* Check if vpd page exceeds initial buffer */
-	len = (h->buff[2] << 8) + h->buff[3] + 4;
-	if (len > h->bufflen) {
-		/* Resubmit with the correct length */
-		if (realloc_buffer(h, len)) {
-			sdev_printk(KERN_WARNING, sdev,
-				    "%s: kmalloc buffer failed\n",
-				    ALUA_DH_NAME);
-			/* Temporary failure, bypass */
-			return SCSI_DH_DEV_TEMP_BUSY;
-		}
-		goto retry;
-	}
+	int rel_port = -1, group_id;
 
-	/*
-	 * Now look for the correct descriptor.
-	 */
-	d = h->buff + 4;
-	while (d < h->buff + len) {
-		switch (d[1] & 0xf) {
-		case 0x4:
-			/* Relative target port */
-			h->rel_port = (d[6] << 8) + d[7];
-			break;
-		case 0x5:
-			/* Target port group */
-			h->group_id = (d[6] << 8) + d[7];
-			break;
-		default:
-			break;
-		}
-		d += d[3] + 4;
-	}
-
-	if (h->group_id == -1) {
+	group_id = scsi_vpd_tpg_id(sdev, &rel_port);
+	if (group_id < 0) {
 		/*
 		 * Internal error; TPGS supported but required
 		 * VPD identification descriptors not present.
@@ -408,16 +332,16 @@ static int alua_vpd_inquiry(struct scsi_device *sdev, struct alua_dh_data *h)
 		sdev_printk(KERN_INFO, sdev,
 			    "%s: No target port descriptors found\n",
 			    ALUA_DH_NAME);
-		h->state = TPGS_STATE_OPTIMIZED;
-		h->tpgs = TPGS_MODE_NONE;
-		err = SCSI_DH_DEV_UNSUPP;
-	} else {
-		sdev_printk(KERN_INFO, sdev,
-			    "%s: port group %02x rel port %02x\n",
-			    ALUA_DH_NAME, h->group_id, h->rel_port);
+		return SCSI_DH_DEV_UNSUPP;
 	}
+	h->state = TPGS_STATE_OPTIMIZED;
+	h->group_id = group_id;
 
-	return err;
+	sdev_printk(KERN_INFO, sdev,
+		    "%s: port group %02x rel port %02x\n",
+		    ALUA_DH_NAME, h->group_id, h->rel_port);
+
+	return 0;
 }
 
 static char print_alua_state(int state)
@@ -452,28 +376,6 @@ static int alua_check_sense(struct scsi_device *sdev,
 			 * LUN Not Accessible - ALUA state transition
 			 */
 			return ADD_TO_MLQUEUE;
-		if (sense_hdr->asc == 0x04 && sense_hdr->ascq == 0x0b)
-			/*
-			 * LUN Not Accessible -- Target port in standby state
-			 */
-			return SUCCESS;
-		if (sense_hdr->asc == 0x04 && sense_hdr->ascq == 0x0c)
-			/*
-			 * LUN Not Accessible -- Target port in unavailable state
-			 */
-			return SUCCESS;
-		if (sense_hdr->asc == 0x04 && sense_hdr->ascq == 0x12)
-			/*
-			 * LUN Not Ready -- Offline
-			 */
-			return SUCCESS;
-		if (sdev->allow_restart &&
-		    sense_hdr->asc == 0x04 && sense_hdr->ascq == 0x02)
-			/*
-			 * if the device is not started, we need to wake
-			 * the error handler to start the motor
-			 */
-			return FAILED;
 		break;
 	case UNIT_ATTENTION:
 		if (sense_hdr->asc == 0x29 && sense_hdr->ascq == 0x00)
@@ -533,8 +435,7 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h, int wait_
 	struct scsi_sense_hdr sense_hdr;
 	int len, k, off, valid_states = 0;
 	unsigned char *ucp;
-	unsigned err;
-	bool rtpg_ext_hdr_req = 1;
+	unsigned err, retval;
 	unsigned long expiry, interval = 0;
 	unsigned int tpg_desc_tbl_off;
 	unsigned char orig_transition_tmo;
@@ -545,13 +446,17 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h, int wait_
 		expiry = round_jiffies_up(jiffies + h->transition_tmo * HZ);
 
  retry:
-	err = submit_rtpg(sdev, h, rtpg_ext_hdr_req);
-
-	if (err == SCSI_DH_IO && h->senselen > 0) {
-		err = scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
-					   &sense_hdr);
-		if (!err)
+	retval = submit_rtpg(sdev, h);
+	if (retval) {
+		if (!scsi_normalize_sense(h->sense, SCSI_SENSE_BUFFERSIZE,
+					  &sense_hdr)) {
+			sdev_printk(KERN_INFO, sdev,
+				    "%s: rtpg failed, result %d\n",
+				    ALUA_DH_NAME, retval);
+			if (driver_byte(retval) == DRIVER_BUSY)
+				return SCSI_DH_DEV_TEMP_BUSY;
 			return SCSI_DH_IO;
+		}
 
 		/*
 		 * submit_rtpg() has failed on existing arrays
@@ -561,27 +466,34 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h, int wait_
 		 * The retry without rtpg_ext_hdr_req set
 		 * handles this.
 		 */
-		if (rtpg_ext_hdr_req == 1 &&
+		if (!(h->flags & ALUA_RTPG_EXT_HDR_UNSUPP) &&
 		    sense_hdr.sense_key == ILLEGAL_REQUEST &&
 		    sense_hdr.asc == 0x24 && sense_hdr.ascq == 0) {
-			rtpg_ext_hdr_req = 0;
+			h->flags |= ALUA_RTPG_EXT_HDR_UNSUPP;
 			goto retry;
 		}
-
-		err = alua_check_sense(sdev, &sense_hdr);
-		if (err == ADD_TO_MLQUEUE && time_before(jiffies, expiry))
+		/*
+		 * Retry on ALUA state transition or if any
+		 * UNIT ATTENTION occurred.
+		 */
+		if (sense_hdr.sense_key == NOT_READY &&
+		    sense_hdr.asc == 0x04 && sense_hdr.ascq == 0x0a)
+			err = SCSI_DH_RETRY;
+		else if (sense_hdr.sense_key == UNIT_ATTENTION)
+			err = SCSI_DH_RETRY;
+		if (err == SCSI_DH_RETRY && time_before(jiffies, expiry)) {
+			sdev_printk(KERN_ERR, sdev, "%s: rtpg retry\n",
+				    ALUA_DH_NAME);
+			scsi_print_sense_hdr(sdev, ALUA_DH_NAME, &sense_hdr);
 			goto retry;
-		sdev_printk(KERN_INFO, sdev,
-			    "%s: rtpg sense code %02x/%02x/%02x\n",
-			    ALUA_DH_NAME, sense_hdr.sense_key,
-			    sense_hdr.asc, sense_hdr.ascq);
-		err = SCSI_DH_IO;
+		}
+		sdev_printk(KERN_ERR, sdev, "%s: rtpg failed\n",
+			    ALUA_DH_NAME);
+		scsi_print_sense_hdr(sdev, ALUA_DH_NAME, &sense_hdr);
+		return SCSI_DH_IO;
 	}
-	if (err != SCSI_DH_OK)
-		return err;
 
-	len = (h->buff[0] << 24) + (h->buff[1] << 16) +
-		(h->buff[2] << 8) + h->buff[3] + 4;
+	len = get_unaligned_be32(&h->buff[0]) + 4;
 
 	if (len > h->bufflen) {
 		/* Resubmit with the correct length */
@@ -616,7 +528,7 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h, int wait_
 	     k < len;
 	     k += off, ucp += off) {
 
-		if (h->group_id == (ucp[2] << 8) + ucp[3]) {
+		if (h->group_id == get_unaligned_be16(&ucp[2])) {
 			h->state = ucp[0] & 0x0f;
 			h->pref = ucp[0] >> 7;
 			valid_states = ucp[1];
@@ -674,13 +586,13 @@ static int alua_rtpg(struct scsi_device *sdev, struct alua_dh_data *h, int wait_
  */
 static int alua_initialize(struct scsi_device *sdev, struct alua_dh_data *h)
 {
-	int err;
+	int err = SCSI_DH_DEV_UNSUPP;
 
-	err = alua_check_tpgs(sdev, h);
-	if (err != SCSI_DH_OK)
+	h->tpgs = alua_check_tpgs(sdev);
+	if (h->tpgs == TPGS_MODE_NONE)
 		goto out;
 
-	err = alua_vpd_inquiry(sdev, h);
+	err = alua_check_vpd(sdev, h);
 	if (err != SCSI_DH_OK)
 		goto out;
 
diff --git a/drivers/scsi/device_handler/scsi_dh_rdac.c b/drivers/scsi/device_handler/scsi_dh_rdac.c
index 361358134315..93880ed6291c 100644
--- a/drivers/scsi/device_handler/scsi_dh_rdac.c
+++ b/drivers/scsi/device_handler/scsi_dh_rdac.c
@@ -562,7 +562,7 @@ static int mode_select_handle_sense(struct scsi_device *sdev,
 			/*
 			 * Command Lock contention
 			 */
-			err = SCSI_DH_RETRY;
+			err = SCSI_DH_IMM_RETRY;
 		break;
 	default:
 		break;
@@ -612,6 +612,8 @@ retry:
 		err = mode_select_handle_sense(sdev, h->sense);
 		if (err == SCSI_DH_RETRY && retry_cnt--)
 			goto retry;
+		if (err == SCSI_DH_IMM_RETRY)
+			goto retry;
 	}
 	if (err == SCSI_DH_OK) {
 		h->state = RDAC_STATE_ACTIVE;
diff --git a/drivers/scsi/dmx3191d.c b/drivers/scsi/dmx3191d.c
index 3e088125a8be..6c14e68b9e1a 100644
--- a/drivers/scsi/dmx3191d.c
+++ b/drivers/scsi/dmx3191d.c
@@ -36,17 +36,10 @@
 
 #define DONT_USE_INTR
 
-#define NCR5380_read(reg)		inb(port + reg)
-#define NCR5380_write(reg, value)	outb(value, port + reg)
+#define NCR5380_read(reg)		inb(instance->io_port + reg)
+#define NCR5380_write(reg, value)	outb(value, instance->io_port + reg)
 
 #define NCR5380_implementation_fields	/* none */
-#define NCR5380_local_declare()		unsigned int port
-#define NCR5380_setup(instance)		port = instance->io_port
-
-/*
- * Includes needed for NCR5380.[ch] (XXX: Move them to NCR5380.h)
- */
-#include <linux/delay.h>
 
 #include "NCR5380.h"
 #include "NCR5380.c"
@@ -56,6 +49,7 @@
 
 
 static struct scsi_host_template dmx3191d_driver_template = {
+	.module			= THIS_MODULE,
 	.proc_name		= DMX3191D_DRIVER_NAME,
 	.name			= "Domex DMX3191D",
 	.info			= NCR5380_info,
@@ -67,6 +61,8 @@ static struct scsi_host_template dmx3191d_driver_template = {
 	.sg_tablesize		= SG_ALL,
 	.cmd_per_lun		= 2,
 	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 
 static int dmx3191d_probe_one(struct pci_dev *pdev,
@@ -97,17 +93,25 @@ static int dmx3191d_probe_one(struct pci_dev *pdev,
 	 */
 	shost->irq = NO_IRQ;
 
-	NCR5380_init(shost, FLAG_NO_PSEUDO_DMA | FLAG_DTC3181E);
+	error = NCR5380_init(shost, FLAG_NO_PSEUDO_DMA);
+	if (error)
+		goto out_host_put;
+
+	NCR5380_maybe_reset_bus(shost);
 
 	pci_set_drvdata(pdev, shost);
 
 	error = scsi_add_host(shost, &pdev->dev);
 	if (error)
-		goto out_release_region;
+		goto out_exit;
 
 	scsi_scan_host(shost);
 	return 0;
 
+out_exit:
+	NCR5380_exit(shost);
+out_host_put:
+	scsi_host_put(shost);
  out_release_region:
 	release_region(io, DMX3191D_REGION_LEN);
  out_disable_device:
@@ -119,15 +123,14 @@ static int dmx3191d_probe_one(struct pci_dev *pdev,
 static void dmx3191d_remove_one(struct pci_dev *pdev)
 {
 	struct Scsi_Host *shost = pci_get_drvdata(pdev);
+	unsigned long io = shost->io_port;
 
 	scsi_remove_host(shost);
 
 	NCR5380_exit(shost);
-
-	release_region(shost->io_port, DMX3191D_REGION_LEN);
-	pci_disable_device(pdev);
-
 	scsi_host_put(shost);
+	release_region(io, DMX3191D_REGION_LEN);
+	pci_disable_device(pdev);
 }
 
 static struct pci_device_id dmx3191d_pci_tbl[] = {
diff --git a/drivers/scsi/dtc.c b/drivers/scsi/dtc.c
index 4c74c7ba2dff..6c736b071cf4 100644
--- a/drivers/scsi/dtc.c
+++ b/drivers/scsi/dtc.c
@@ -1,9 +1,5 @@
-
 #define PSEUDO_DMA
 #define DONT_USE_INTR
-#define UNSAFE			/* Leave interrupts enabled during pseudo-dma I/O */
-#define DMA_WORKS_RIGHT
-
 
 /*
  * DTC 3180/3280 driver, by
@@ -50,15 +46,13 @@
 
 
 #include <linux/module.h>
-#include <linux/signal.h>
 #include <linux/blkdev.h>
-#include <linux/delay.h>
-#include <linux/stat.h>
 #include <linux/string.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <scsi/scsi_host.h>
+
 #include "dtc.h"
 #define AUTOPROBE_IRQ
 #include "NCR5380.h"
@@ -150,7 +144,7 @@ static const struct signature {
 
 static int __init dtc_setup(char *str)
 {
-	static int commandline_current = 0;
+	static int commandline_current;
 	int i;
 	int ints[10];
 
@@ -188,7 +182,7 @@ __setup("dtc=", dtc_setup);
 
 static int __init dtc_detect(struct scsi_host_template * tpnt)
 {
-	static int current_override = 0, current_base = 0;
+	static int current_override, current_base;
 	struct Scsi_Host *instance;
 	unsigned int addr;
 	void __iomem *base;
@@ -205,9 +199,8 @@ static int __init dtc_detect(struct scsi_host_template * tpnt)
 				addr = 0;
 		} else
 			for (; !addr && (current_base < NO_BASES); ++current_base) {
-#if (DTCDEBUG & DTCDEBUG_INIT)
-				printk(KERN_DEBUG "scsi-dtc : probing address %08x\n", bases[current_base].address);
-#endif
+				dprintk(NDEBUG_INIT, "dtc: probing address 0x%08x\n",
+				        (unsigned int)bases[current_base].address);
 				if (bases[current_base].noauto)
 					continue;
 				base = ioremap(bases[current_base].address, 0x2000);
@@ -216,18 +209,14 @@ static int __init dtc_detect(struct scsi_host_template * tpnt)
 				for (sig = 0; sig < NO_SIGNATURES; ++sig) {
 					if (check_signature(base + signatures[sig].offset, signatures[sig].string, strlen(signatures[sig].string))) {
 						addr = bases[current_base].address;
-#if (DTCDEBUG & DTCDEBUG_INIT)
-						printk(KERN_DEBUG "scsi-dtc : detected board.\n");
-#endif
+						dprintk(NDEBUG_INIT, "dtc: detected board\n");
 						goto found;
 					}
 				}
 				iounmap(base);
 			}
 
-#if defined(DTCDEBUG) && (DTCDEBUG & DTCDEBUG_INIT)
-		printk(KERN_DEBUG "scsi-dtc : base = %08x\n", addr);
-#endif
+		dprintk(NDEBUG_INIT, "dtc: addr = 0x%08x\n", addr);
 
 		if (!addr)
 			break;
@@ -235,12 +224,15 @@ static int __init dtc_detect(struct scsi_host_template * tpnt)
 found:
 		instance = scsi_register(tpnt, sizeof(struct NCR5380_hostdata));
 		if (instance == NULL)
-			break;
+			goto out_unmap;
 
 		instance->base = addr;
 		((struct NCR5380_hostdata *)(instance)->hostdata)->base = base;
 
-		NCR5380_init(instance, 0);
+		if (NCR5380_init(instance, FLAG_NO_DMA_FIXUP))
+			goto out_unregister;
+
+		NCR5380_maybe_reset_bus(instance);
 
 		NCR5380_write(DTC_CONTROL_REG, CSR_5380_INTR);	/* Enable int's */
 		if (overrides[current_override].irq != IRQ_AUTO)
@@ -271,14 +263,19 @@ found:
 			printk(KERN_WARNING "scsi%d : interrupts not used. Might as well not jumper it.\n", instance->host_no);
 		instance->irq = NO_IRQ;
 #endif
-#if defined(DTCDEBUG) && (DTCDEBUG & DTCDEBUG_INIT)
-		printk("scsi%d : irq = %d\n", instance->host_no, instance->irq);
-#endif
+		dprintk(NDEBUG_INIT, "scsi%d : irq = %d\n",
+		        instance->host_no, instance->irq);
 
 		++current_override;
 		++count;
 	}
 	return count;
+
+out_unregister:
+	scsi_unregister(instance);
+out_unmap:
+	iounmap(base);
+	return count;
 }
 
 /*
@@ -331,12 +328,8 @@ static inline int NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst,
 	unsigned char *d = dst;
 	int i;			/* For counting time spent in the poll-loop */
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
 
 	i = 0;
-	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-	NCR5380_write(MODE_REG, MR_ENABLE_EOP_INTR | MR_DMA_MODE);
 	if (instance->irq == NO_IRQ)
 		NCR5380_write(DTC_CONTROL_REG, CSR_DIR_READ);
 	else
@@ -348,7 +341,7 @@ static inline int NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst,
 		while (NCR5380_read(DTC_CONTROL_REG) & CSR_HOST_BUF_NOT_RDY)
 			++i;
 		rtrc(3);
-		memcpy_fromio(d, base + DTC_DATA_BUF, 128);
+		memcpy_fromio(d, hostdata->base + DTC_DATA_BUF, 128);
 		d += 128;
 		len -= 128;
 		rtrc(7);
@@ -358,9 +351,7 @@ static inline int NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst,
 	rtrc(4);
 	while (!(NCR5380_read(DTC_CONTROL_REG) & D_CR_ACCESS))
 		++i;
-	NCR5380_write(MODE_REG, 0);	/* Clear the operating mode */
 	rtrc(0);
-	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	if (i > hostdata->spin_max_r)
 		hostdata->spin_max_r = i;
 	return (0);
@@ -383,12 +374,7 @@ static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src,
 {
 	int i;
 	struct NCR5380_hostdata *hostdata = shost_priv(instance);
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
 
-	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
-	NCR5380_write(MODE_REG, MR_ENABLE_EOP_INTR | MR_DMA_MODE);
-	/* set direction (write) */
 	if (instance->irq == NO_IRQ)
 		NCR5380_write(DTC_CONTROL_REG, 0);
 	else
@@ -400,7 +386,7 @@ static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src,
 		while (NCR5380_read(DTC_CONTROL_REG) & CSR_HOST_BUF_NOT_RDY)
 			++i;
 		rtrc(3);
-		memcpy_toio(base + DTC_DATA_BUF, src, 128);
+		memcpy_toio(hostdata->base + DTC_DATA_BUF, src, 128);
 		src += 128;
 		len -= 128;
 	}
@@ -413,47 +399,60 @@ static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src,
 		++i;
 	rtrc(7);
 	/* Check for parity error here. fixme. */
-	NCR5380_write(MODE_REG, 0);	/* Clear the operating mode */
 	rtrc(0);
 	if (i > hostdata->spin_max_w)
 		hostdata->spin_max_w = i;
 	return (0);
 }
 
+static int dtc_dma_xfer_len(struct scsi_cmnd *cmd)
+{
+	int transfersize = cmd->transfersize;
+
+	/* Limit transfers to 32K, for xx400 & xx406
+	 * pseudoDMA that transfers in 128 bytes blocks.
+	 */
+	if (transfersize > 32 * 1024 && cmd->SCp.this_residual &&
+	    !(cmd->SCp.this_residual % transfersize))
+		transfersize = 32 * 1024;
+
+	return transfersize;
+}
+
 MODULE_LICENSE("GPL");
 
 #include "NCR5380.c"
 
 static int dtc_release(struct Scsi_Host *shost)
 {
-	NCR5380_local_declare();
-	NCR5380_setup(shost);
+	struct NCR5380_hostdata *hostdata = shost_priv(shost);
+
 	if (shost->irq != NO_IRQ)
 		free_irq(shost->irq, shost);
 	NCR5380_exit(shost);
-	if (shost->io_port && shost->n_io_port)
-		release_region(shost->io_port, shost->n_io_port);
 	scsi_unregister(shost);
-	iounmap(base);
+	iounmap(hostdata->base);
 	return 0;
 }
 
 static struct scsi_host_template driver_template = {
-	.name				= "DTC 3180/3280 ",
-	.detect				= dtc_detect,
-	.release			= dtc_release,
-	.proc_name			= "dtc3x80",
-	.show_info			= dtc_show_info,
-	.write_info			= dtc_write_info,
-	.info				= dtc_info,
-	.queuecommand			= dtc_queue_command,
-	.eh_abort_handler		= dtc_abort,
-	.eh_bus_reset_handler		= dtc_bus_reset,
-	.bios_param     		= dtc_biosparam,
-	.can_queue      		= CAN_QUEUE,
-	.this_id        		= 7,
-	.sg_tablesize   		= SG_ALL,
-	.cmd_per_lun    		= CMD_PER_LUN,
-	.use_clustering 		= DISABLE_CLUSTERING,
+	.name			= "DTC 3180/3280",
+	.detect			= dtc_detect,
+	.release		= dtc_release,
+	.proc_name		= "dtc3x80",
+	.show_info		= dtc_show_info,
+	.write_info		= dtc_write_info,
+	.info			= dtc_info,
+	.queuecommand		= dtc_queue_command,
+	.eh_abort_handler	= dtc_abort,
+	.eh_bus_reset_handler	= dtc_bus_reset,
+	.bios_param		= dtc_biosparam,
+	.can_queue		= 32,
+	.this_id		= 7,
+	.sg_tablesize		= SG_ALL,
+	.cmd_per_lun		= 2,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 #include "scsi_module.c"
diff --git a/drivers/scsi/dtc.h b/drivers/scsi/dtc.h
index 78a2332e9064..56732cba8aba 100644
--- a/drivers/scsi/dtc.h
+++ b/drivers/scsi/dtc.h
@@ -10,54 +10,17 @@
 #ifndef DTC3280_H
 #define DTC3280_H
 
-#define DTCDEBUG 0
-#define DTCDEBUG_INIT	0x1
-#define DTCDEBUG_TRANSFER 0x2
-
-#ifndef CMD_PER_LUN
-#define CMD_PER_LUN 2
-#endif
-
-#ifndef CAN_QUEUE
-#define CAN_QUEUE 32 
-#endif
-
 #define NCR5380_implementation_fields \
     void __iomem *base
 
-#define NCR5380_local_declare() \
-    void __iomem *base
-
-#define NCR5380_setup(instance) \
-    base = ((struct NCR5380_hostdata *)(instance)->hostdata)->base
+#define DTC_address(reg) \
+	(((struct NCR5380_hostdata *)shost_priv(instance))->base + DTC_5380_OFFSET + reg)
 
-#define DTC_address(reg) (base + DTC_5380_OFFSET + reg)
-
-#define dbNCR5380_read(reg)                                              \
-    (rval=readb(DTC_address(reg)), \
-     (((unsigned char) printk("DTC : read register %d at addr %p is: %02x\n"\
-    , (reg), DTC_address(reg), rval)), rval ) )
-
-#define dbNCR5380_write(reg, value) do {                                  \
-    printk("DTC : write %02x to register %d at address %p\n",         \
-            (value), (reg), DTC_address(reg));     \
-    writeb(value, DTC_address(reg));} while(0)
-
-
-#if !(DTCDEBUG & DTCDEBUG_TRANSFER) 
 #define NCR5380_read(reg) (readb(DTC_address(reg)))
 #define NCR5380_write(reg, value) (writeb(value, DTC_address(reg)))
-#else
-#define NCR5380_read(reg) (readb(DTC_address(reg)))
-#define xNCR5380_read(reg)						\
-    (((unsigned char) printk("DTC : read register %d at address %p\n"\
-    , (reg), DTC_address(reg))), readb(DTC_address(reg)))
 
-#define NCR5380_write(reg, value) do {					\
-    printk("DTC : write %02x to register %d at address %p\n", 	\
-	    (value), (reg), DTC_address(reg));	\
-    writeb(value, DTC_address(reg));} while(0)
-#endif
+#define NCR5380_dma_xfer_len(instance, cmd, phase) \
+        dtc_dma_xfer_len(cmd)
 
 #define NCR5380_intr			dtc_intr
 #define NCR5380_queue_command		dtc_queue_command
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index f4424063b860..0efe7112fc1f 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1625,7 +1625,7 @@ static int fcoe_xmit(struct fc_lport *lport, struct fc_frame *fp)
 
 	/* crc offload */
 	if (likely(lport->crc_offload)) {
-		skb->ip_summed = CHECKSUM_UNNECESSARY;
+		skb->ip_summed = CHECKSUM_PARTIAL;
 		skb->csum_start = skb_headroom(skb);
 		skb->csum_offset = skb->len;
 		crc = 0;
diff --git a/drivers/scsi/g_NCR5380.c b/drivers/scsi/g_NCR5380.c
index f8d2478b11cc..90091e693020 100644
--- a/drivers/scsi/g_NCR5380.c
+++ b/drivers/scsi/g_NCR5380.c
@@ -56,40 +56,31 @@
  *     
  */
 
-/* settings for DTC3181E card with only Mustek scanner attached */
-#define USLEEP_POLL	msecs_to_jiffies(10)
-#define USLEEP_SLEEP	msecs_to_jiffies(200)
-#define USLEEP_WAITLONG	msecs_to_jiffies(5000)
-
 #define AUTOPROBE_IRQ
 
 #ifdef CONFIG_SCSI_GENERIC_NCR53C400
-#define NCR53C400_PSEUDO_DMA 1
 #define PSEUDO_DMA
-#define NCR53C400
 #endif
 
 #include <asm/io.h>
-#include <linux/signal.h>
 #include <linux/blkdev.h>
+#include <linux/module.h>
 #include <scsi/scsi_host.h>
 #include "g_NCR5380.h"
 #include "NCR5380.h"
-#include <linux/stat.h>
 #include <linux/init.h>
 #include <linux/ioport.h>
 #include <linux/isapnp.h>
-#include <linux/delay.h>
 #include <linux/interrupt.h>
 
-#define NCR_NOT_SET 0
-static int ncr_irq = NCR_NOT_SET;
-static int ncr_dma = NCR_NOT_SET;
-static int ncr_addr = NCR_NOT_SET;
-static int ncr_5380 = NCR_NOT_SET;
-static int ncr_53c400 = NCR_NOT_SET;
-static int ncr_53c400a = NCR_NOT_SET;
-static int dtc_3181e = NCR_NOT_SET;
+static int ncr_irq;
+static int ncr_dma;
+static int ncr_addr;
+static int ncr_5380;
+static int ncr_53c400;
+static int ncr_53c400a;
+static int dtc_3181e;
+static int hp_c2502;
 
 static struct override {
 	NCR5380_map_type NCR5380_map_name;
@@ -121,7 +112,7 @@ static struct override {
 
 static void __init internal_setup(int board, char *str, int *ints)
 {
-	static int commandline_current = 0;
+	static int commandline_current;
 	switch (board) {
 	case BOARD_NCR5380:
 		if (ints[0] != 2 && ints[0] != 3) {
@@ -235,6 +226,30 @@ static int __init do_DTC3181E_setup(char *str)
 
 #endif
 
+#ifndef SCSI_G_NCR5380_MEM
+/*
+ * Configure I/O address of 53C400A or DTC436 by writing magic numbers
+ * to ports 0x779 and 0x379.
+ */
+static void magic_configure(int idx, u8 irq, u8 magic[])
+{
+	u8 cfg = 0;
+
+	outb(magic[0], 0x779);
+	outb(magic[1], 0x379);
+	outb(magic[2], 0x379);
+	outb(magic[3], 0x379);
+	outb(magic[4], 0x379);
+
+	/* allowed IRQs for HP C2502 */
+	if (irq != 2 && irq != 3 && irq != 4 && irq != 5 && irq != 7)
+		irq = 0;
+	if (idx >= 0 && idx <= 7)
+		cfg = 0x80 | idx | (irq << 4);
+	outb(cfg, 0x379);
+}
+#endif
+
 /**
  * 	generic_NCR5380_detect	-	look for NCR5380 controllers
  *	@tpnt: the scsi template
@@ -243,19 +258,18 @@ static int __init do_DTC3181E_setup(char *str)
  *	and DTC436(ISAPnP) controllers. If overrides have been set we use
  *	them.
  *
- *	The caller supplied NCR5380_init function is invoked from here, before
- *	the interrupt line is taken.
- *
  *	Locks: none
  */
 
 static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 {
-	static int current_override = 0;
+	static int current_override;
 	int count;
 	unsigned int *ports;
+	u8 *magic = NULL;
 #ifndef SCSI_G_NCR5380_MEM
 	int i;
+	int port_idx = -1;
 	unsigned long region_size = 16;
 #endif
 	static unsigned int __initdata ncr_53c400a_ports[] = {
@@ -264,27 +278,36 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 	static unsigned int __initdata dtc_3181e_ports[] = {
 		0x220, 0x240, 0x280, 0x2a0, 0x2c0, 0x300, 0x320, 0x340, 0
 	};
-	int flags = 0;
+	static u8 ncr_53c400a_magic[] __initdata = {	/* 53C400A & DTC436 */
+		0x59, 0xb9, 0xc5, 0xae, 0xa6
+	};
+	static u8 hp_c2502_magic[] __initdata = {	/* HP C2502 */
+		0x0f, 0x22, 0xf0, 0x20, 0x80
+	};
+	int flags;
 	struct Scsi_Host *instance;
+	struct NCR5380_hostdata *hostdata;
 #ifdef SCSI_G_NCR5380_MEM
 	unsigned long base;
 	void __iomem *iomem;
 #endif
 
-	if (ncr_irq != NCR_NOT_SET)
+	if (ncr_irq)
 		overrides[0].irq = ncr_irq;
-	if (ncr_dma != NCR_NOT_SET)
+	if (ncr_dma)
 		overrides[0].dma = ncr_dma;
-	if (ncr_addr != NCR_NOT_SET)
+	if (ncr_addr)
 		overrides[0].NCR5380_map_name = (NCR5380_map_type) ncr_addr;
-	if (ncr_5380 != NCR_NOT_SET)
+	if (ncr_5380)
 		overrides[0].board = BOARD_NCR5380;
-	else if (ncr_53c400 != NCR_NOT_SET)
+	else if (ncr_53c400)
 		overrides[0].board = BOARD_NCR53C400;
-	else if (ncr_53c400a != NCR_NOT_SET)
+	else if (ncr_53c400a)
 		overrides[0].board = BOARD_NCR53C400A;
-	else if (dtc_3181e != NCR_NOT_SET)
+	else if (dtc_3181e)
 		overrides[0].board = BOARD_DTC3181E;
+	else if (hp_c2502)
+		overrides[0].board = BOARD_HP_C2502;
 #ifndef SCSI_G_NCR5380_MEM
 	if (!current_override && isapnp_present()) {
 		struct pnp_dev *dev = NULL;
@@ -318,41 +341,45 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 		}
 	}
 #endif
-	tpnt->proc_name = "g_NCR5380";
 
 	for (count = 0; current_override < NO_OVERRIDES; ++current_override) {
 		if (!(overrides[current_override].NCR5380_map_name))
 			continue;
 
 		ports = NULL;
+		flags = 0;
 		switch (overrides[current_override].board) {
 		case BOARD_NCR5380:
 			flags = FLAG_NO_PSEUDO_DMA;
 			break;
 		case BOARD_NCR53C400:
-			flags = FLAG_NCR53C400;
+#ifdef PSEUDO_DMA
+			flags = FLAG_NO_DMA_FIXUP;
+#endif
 			break;
 		case BOARD_NCR53C400A:
-			flags = FLAG_NO_PSEUDO_DMA;
+			flags = FLAG_NO_DMA_FIXUP;
+			ports = ncr_53c400a_ports;
+			magic = ncr_53c400a_magic;
+			break;
+		case BOARD_HP_C2502:
+			flags = FLAG_NO_DMA_FIXUP;
 			ports = ncr_53c400a_ports;
+			magic = hp_c2502_magic;
 			break;
 		case BOARD_DTC3181E:
-			flags = FLAG_NO_PSEUDO_DMA | FLAG_DTC3181E;
+			flags = FLAG_NO_DMA_FIXUP;
 			ports = dtc_3181e_ports;
+			magic = ncr_53c400a_magic;
 			break;
 		}
 
 #ifndef SCSI_G_NCR5380_MEM
-		if (ports) {
+		if (ports && magic) {
 			/* wakeup sequence for the NCR53C400A and DTC3181E */
 
 			/* Disable the adapter and look for a free io port */
-			outb(0x59, 0x779);
-			outb(0xb9, 0x379);
-			outb(0xc5, 0x379);
-			outb(0xae, 0x379);
-			outb(0xa6, 0x379);
-			outb(0x00, 0x379);
+			magic_configure(-1, 0, magic);
 
 			if (overrides[current_override].NCR5380_map_name != PORT_AUTO)
 				for (i = 0; ports[i]; i++) {
@@ -371,17 +398,12 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 				}
 			if (ports[i]) {
 				/* At this point we have our region reserved */
-				outb(0x59, 0x779);
-				outb(0xb9, 0x379);
-				outb(0xc5, 0x379);
-				outb(0xae, 0x379);
-				outb(0xa6, 0x379);
-				outb(0x80 | i, 0x379);	/* set io port to be used */
+				magic_configure(i, 0, magic); /* no IRQ yet */
 				outb(0xc0, ports[i] + 9);
 				if (inb(ports[i] + 9) != 0x80)
 					continue;
-				else
-					overrides[current_override].NCR5380_map_name = ports[i];
+				overrides[current_override].NCR5380_map_name = ports[i];
+				port_idx = i;
 			} else
 				continue;
 		}
@@ -403,24 +425,65 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 		}
 #endif
 		instance = scsi_register(tpnt, sizeof(struct NCR5380_hostdata));
-		if (instance == NULL) {
-#ifndef SCSI_G_NCR5380_MEM
-			release_region(overrides[current_override].NCR5380_map_name, region_size);
-#else
-			iounmap(iomem);
-			release_mem_region(base, NCR5380_region_size);
-#endif
-			continue;
-		}
+		if (instance == NULL)
+			goto out_release;
+		hostdata = shost_priv(instance);
 
-		instance->NCR5380_instance_name = overrides[current_override].NCR5380_map_name;
 #ifndef SCSI_G_NCR5380_MEM
+		instance->io_port = overrides[current_override].NCR5380_map_name;
 		instance->n_io_port = region_size;
+		hostdata->io_width = 1; /* 8-bit PDMA by default */
+
+		/*
+		 * On NCR53C400 boards, NCR5380 registers are mapped 8 past
+		 * the base address.
+		 */
+		switch (overrides[current_override].board) {
+		case BOARD_NCR53C400:
+			instance->io_port += 8;
+			hostdata->c400_ctl_status = 0;
+			hostdata->c400_blk_cnt = 1;
+			hostdata->c400_host_buf = 4;
+			break;
+		case BOARD_DTC3181E:
+			hostdata->io_width = 2;	/* 16-bit PDMA */
+			/* fall through */
+		case BOARD_NCR53C400A:
+		case BOARD_HP_C2502:
+			hostdata->c400_ctl_status = 9;
+			hostdata->c400_blk_cnt = 10;
+			hostdata->c400_host_buf = 8;
+			break;
+		}
 #else
-		((struct NCR5380_hostdata *)instance->hostdata)->iomem = iomem;
+		instance->base = overrides[current_override].NCR5380_map_name;
+		hostdata->iomem = iomem;
+		switch (overrides[current_override].board) {
+		case BOARD_NCR53C400:
+			hostdata->c400_ctl_status = 0x100;
+			hostdata->c400_blk_cnt = 0x101;
+			hostdata->c400_host_buf = 0x104;
+			break;
+		case BOARD_DTC3181E:
+		case BOARD_NCR53C400A:
+		case BOARD_HP_C2502:
+			pr_err(DRV_MODULE_NAME ": unknown register offsets\n");
+			goto out_unregister;
+		}
 #endif
 
-		NCR5380_init(instance, flags);
+		if (NCR5380_init(instance, flags))
+			goto out_unregister;
+
+		switch (overrides[current_override].board) {
+		case BOARD_NCR53C400:
+		case BOARD_DTC3181E:
+		case BOARD_NCR53C400A:
+		case BOARD_HP_C2502:
+			NCR5380_write(hostdata->c400_ctl_status, CSR_BASE);
+		}
+
+		NCR5380_maybe_reset_bus(instance);
 
 		if (overrides[current_override].irq != IRQ_AUTO)
 			instance->irq = overrides[current_override].irq;
@@ -431,12 +494,18 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 		if (instance->irq == 255)
 			instance->irq = NO_IRQ;
 
-		if (instance->irq != NO_IRQ)
+		if (instance->irq != NO_IRQ) {
+#ifndef SCSI_G_NCR5380_MEM
+			/* set IRQ for HP C2502 */
+			if (overrides[current_override].board == BOARD_HP_C2502)
+				magic_configure(port_idx, instance->irq, magic);
+#endif
 			if (request_irq(instance->irq, generic_NCR5380_intr,
 					0, "NCR5380", instance)) {
 				printk(KERN_WARNING "scsi%d : IRQ%d not free, interrupts disabled\n", instance->host_no, instance->irq);
 				instance->irq = NO_IRQ;
 			}
+		}
 
 		if (instance->irq == NO_IRQ) {
 			printk(KERN_INFO "scsi%d : interrupts not enabled. for better interactive performance,\n", instance->host_no);
@@ -447,6 +516,17 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
 		++count;
 	}
 	return count;
+
+out_unregister:
+	scsi_unregister(instance);
+out_release:
+#ifndef SCSI_G_NCR5380_MEM
+	release_region(overrides[current_override].NCR5380_map_name, region_size);
+#else
+	iounmap(iomem);
+	release_mem_region(base, NCR5380_region_size);
+#endif
+	return count;
 }
 
 /**
@@ -460,21 +540,15 @@ static int __init generic_NCR5380_detect(struct scsi_host_template *tpnt)
  
 static int generic_NCR5380_release_resources(struct Scsi_Host *instance)
 {
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-	
 	if (instance->irq != NO_IRQ)
 		free_irq(instance->irq, instance);
 	NCR5380_exit(instance);
-
 #ifndef SCSI_G_NCR5380_MEM
-	release_region(instance->NCR5380_instance_name, instance->n_io_port);
+	release_region(instance->io_port, instance->n_io_port);
 #else
 	iounmap(((struct NCR5380_hostdata *)instance->hostdata)->iomem);
-	release_mem_region(instance->NCR5380_instance_name, NCR5380_region_size);
+	release_mem_region(instance->base, NCR5380_region_size);
 #endif
-
-
 	return 0;
 }
 
@@ -507,7 +581,7 @@ generic_NCR5380_biosparam(struct scsi_device *sdev, struct block_device *bdev,
 }
 #endif
 
-#ifdef NCR53C400_PSEUDO_DMA
+#ifdef PSEUDO_DMA
 
 /**
  *	NCR5380_pread		-	pseudo DMA read
@@ -521,75 +595,68 @@ generic_NCR5380_biosparam(struct scsi_device *sdev, struct block_device *bdev,
  
 static inline int NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst, int len)
 {
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int blocks = len / 128;
 	int start = 0;
-	int bl;
-
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
 
-	NCR5380_write(C400_CONTROL_STATUS_REG, CSR_BASE | CSR_TRANS_DIR);
-	NCR5380_write(C400_BLOCK_COUNTER_REG, blocks);
+	NCR5380_write(hostdata->c400_ctl_status, CSR_BASE | CSR_TRANS_DIR);
+	NCR5380_write(hostdata->c400_blk_cnt, blocks);
 	while (1) {
-		if ((bl = NCR5380_read(C400_BLOCK_COUNTER_REG)) == 0) {
+		if (NCR5380_read(hostdata->c400_blk_cnt) == 0)
 			break;
-		}
-		if (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_GATED_53C80_IRQ) {
+		if (NCR5380_read(hostdata->c400_ctl_status) & CSR_GATED_53C80_IRQ) {
 			printk(KERN_ERR "53C400r: Got 53C80_IRQ start=%d, blocks=%d\n", start, blocks);
 			return -1;
 		}
-		while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_HOST_BUF_NOT_RDY);
+		while (NCR5380_read(hostdata->c400_ctl_status) & CSR_HOST_BUF_NOT_RDY)
+			; /* FIXME - no timeout */
 
 #ifndef SCSI_G_NCR5380_MEM
-		{
-			int i;
-			for (i = 0; i < 128; i++)
-				dst[start + i] = NCR5380_read(C400_HOST_BUFFER);
-		}
+		if (hostdata->io_width == 2)
+			insw(instance->io_port + hostdata->c400_host_buf,
+							dst + start, 64);
+		else
+			insb(instance->io_port + hostdata->c400_host_buf,
+							dst + start, 128);
 #else
 		/* implies SCSI_G_NCR5380_MEM */
-		memcpy_fromio(dst + start, iomem + NCR53C400_host_buffer, 128);
+		memcpy_fromio(dst + start,
+		              hostdata->iomem + NCR53C400_host_buffer, 128);
 #endif
 		start += 128;
 		blocks--;
 	}
 
 	if (blocks) {
-		while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_HOST_BUF_NOT_RDY)
-		{
-			// FIXME - no timeout
-		}
+		while (NCR5380_read(hostdata->c400_ctl_status) & CSR_HOST_BUF_NOT_RDY)
+			; /* FIXME - no timeout */
 
 #ifndef SCSI_G_NCR5380_MEM
-		{
-			int i;	
-			for (i = 0; i < 128; i++)
-				dst[start + i] = NCR5380_read(C400_HOST_BUFFER);
-		}
+		if (hostdata->io_width == 2)
+			insw(instance->io_port + hostdata->c400_host_buf,
+							dst + start, 64);
+		else
+			insb(instance->io_port + hostdata->c400_host_buf,
+							dst + start, 128);
 #else
 		/* implies SCSI_G_NCR5380_MEM */
-		memcpy_fromio(dst + start, iomem + NCR53C400_host_buffer, 128);
+		memcpy_fromio(dst + start,
+		              hostdata->iomem + NCR53C400_host_buffer, 128);
 #endif
 		start += 128;
 		blocks--;
 	}
 
-	if (!(NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_GATED_53C80_IRQ))
+	if (!(NCR5380_read(hostdata->c400_ctl_status) & CSR_GATED_53C80_IRQ))
 		printk("53C400r: no 53C80 gated irq after transfer");
 
-#if 0
-	/*
-	 *	DON'T DO THIS - THEY NEVER ARRIVE!
-	 */
-	printk("53C400r: Waiting for 53C80 registers\n");
-	while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_53C80_REG)
+	/* wait for 53C80 registers to be available */
+	while (!(NCR5380_read(hostdata->c400_ctl_status) & CSR_53C80_REG))
 		;
-#endif
+
 	if (!(NCR5380_read(BUS_AND_STATUS_REG) & BASR_END_DMA_TRANSFER))
 		printk(KERN_ERR "53C400r: no end dma signal\n");
 		
-	NCR5380_write(MODE_REG, MR_BASE);
-	NCR5380_read(RESET_PARITY_INTERRUPT_REG);
 	return 0;
 }
 
@@ -605,89 +672,91 @@ static inline int NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst,
 
 static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src, int len)
 {
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
 	int blocks = len / 128;
 	int start = 0;
-	int bl;
-	int i;
 
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-
-	NCR5380_write(C400_CONTROL_STATUS_REG, CSR_BASE);
-	NCR5380_write(C400_BLOCK_COUNTER_REG, blocks);
+	NCR5380_write(hostdata->c400_ctl_status, CSR_BASE);
+	NCR5380_write(hostdata->c400_blk_cnt, blocks);
 	while (1) {
-		if (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_GATED_53C80_IRQ) {
+		if (NCR5380_read(hostdata->c400_ctl_status) & CSR_GATED_53C80_IRQ) {
 			printk(KERN_ERR "53C400w: Got 53C80_IRQ start=%d, blocks=%d\n", start, blocks);
 			return -1;
 		}
 
-		if ((bl = NCR5380_read(C400_BLOCK_COUNTER_REG)) == 0) {
+		if (NCR5380_read(hostdata->c400_blk_cnt) == 0)
 			break;
-		}
-		while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_HOST_BUF_NOT_RDY)
+		while (NCR5380_read(hostdata->c400_ctl_status) & CSR_HOST_BUF_NOT_RDY)
 			; // FIXME - timeout
 #ifndef SCSI_G_NCR5380_MEM
-		{
-			for (i = 0; i < 128; i++)
-				NCR5380_write(C400_HOST_BUFFER, src[start + i]);
-		}
+		if (hostdata->io_width == 2)
+			outsw(instance->io_port + hostdata->c400_host_buf,
+							src + start, 64);
+		else
+			outsb(instance->io_port + hostdata->c400_host_buf,
+							src + start, 128);
 #else
 		/* implies SCSI_G_NCR5380_MEM */
-		memcpy_toio(iomem + NCR53C400_host_buffer, src + start, 128);
+		memcpy_toio(hostdata->iomem + NCR53C400_host_buffer,
+		            src + start, 128);
 #endif
 		start += 128;
 		blocks--;
 	}
 	if (blocks) {
-		while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_HOST_BUF_NOT_RDY)
+		while (NCR5380_read(hostdata->c400_ctl_status) & CSR_HOST_BUF_NOT_RDY)
 			; // FIXME - no timeout
 
 #ifndef SCSI_G_NCR5380_MEM
-		{
-			for (i = 0; i < 128; i++)
-				NCR5380_write(C400_HOST_BUFFER, src[start + i]);
-		}
+		if (hostdata->io_width == 2)
+			outsw(instance->io_port + hostdata->c400_host_buf,
+							src + start, 64);
+		else
+			outsb(instance->io_port + hostdata->c400_host_buf,
+							src + start, 128);
 #else
 		/* implies SCSI_G_NCR5380_MEM */
-		memcpy_toio(iomem + NCR53C400_host_buffer, src + start, 128);
+		memcpy_toio(hostdata->iomem + NCR53C400_host_buffer,
+		            src + start, 128);
 #endif
 		start += 128;
 		blocks--;
 	}
 
-#if 0
-	printk("53C400w: waiting for registers to be available\n");
-	THEY NEVER DO ! while (NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_53C80_REG);
-	printk("53C400w: Got em\n");
-#endif
-
-	/* Let's wait for this instead - could be ugly */
-	/* All documentation says to check for this. Maybe my hardware is too
-	 * fast. Waiting for it seems to work fine! KLL
-	 */
-	while (!(i = NCR5380_read(C400_CONTROL_STATUS_REG) & CSR_GATED_53C80_IRQ))
-		;	// FIXME - no timeout
-
-	/*
-	 * I know. i is certainly != 0 here but the loop is new. See previous
-	 * comment.
-	 */
-	if (i) {
-		if (!((i = NCR5380_read(BUS_AND_STATUS_REG)) & BASR_END_DMA_TRANSFER))
-			printk(KERN_ERR "53C400w: No END OF DMA bit - WHOOPS! BASR=%0x\n", i);
-	} else
-		printk(KERN_ERR "53C400w: no 53C80 gated irq after transfer (last block)\n");
+	/* wait for 53C80 registers to be available */
+	while (!(NCR5380_read(hostdata->c400_ctl_status) & CSR_53C80_REG)) {
+		udelay(4); /* DTC436 chip hangs without this */
+		/* FIXME - no timeout */
+	}
 
-#if 0
 	if (!(NCR5380_read(BUS_AND_STATUS_REG) & BASR_END_DMA_TRANSFER)) {
 		printk(KERN_ERR "53C400w: no end dma signal\n");
 	}
-#endif
+
 	while (!(NCR5380_read(TARGET_COMMAND_REG) & TCR_LAST_BYTE_SENT))
 		; 	// TIMEOUT
 	return 0;
 }
-#endif				/* PSEUDO_DMA */
+
+static int generic_NCR5380_dma_xfer_len(struct scsi_cmnd *cmd)
+{
+	int transfersize = cmd->transfersize;
+
+	/* Limit transfers to 32K, for xx400 & xx406
+	 * pseudoDMA that transfers in 128 bytes blocks.
+	 */
+	if (transfersize > 32 * 1024 && cmd->SCp.this_residual &&
+	    !(cmd->SCp.this_residual % transfersize))
+		transfersize = 32 * 1024;
+
+	/* 53C400 datasheet: non-modulo-128-byte transfers should use PIO */
+	if (transfersize % 128)
+		transfersize = 0;
+
+	return transfersize;
+}
+
+#endif /* PSEUDO_DMA */
 
 /*
  *	Include the NCR5380 core code that we build our driver around	
@@ -696,22 +765,24 @@ static inline int NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src,
 #include "NCR5380.c"
 
 static struct scsi_host_template driver_template = {
-	.show_info      	= generic_NCR5380_show_info,
-	.name           	= "Generic NCR5380/NCR53C400 SCSI",
-	.detect         	= generic_NCR5380_detect,
-	.release        	= generic_NCR5380_release_resources,
-	.info           	= generic_NCR5380_info,
-	.queuecommand   	= generic_NCR5380_queue_command,
+	.proc_name		= DRV_MODULE_NAME,
+	.name			= "Generic NCR5380/NCR53C400 SCSI",
+	.detect			= generic_NCR5380_detect,
+	.release		= generic_NCR5380_release_resources,
+	.info			= generic_NCR5380_info,
+	.queuecommand		= generic_NCR5380_queue_command,
 	.eh_abort_handler	= generic_NCR5380_abort,
 	.eh_bus_reset_handler	= generic_NCR5380_bus_reset,
-	.bios_param     	= NCR5380_BIOSPARAM,
-	.can_queue      	= CAN_QUEUE,
-        .this_id        	= 7,
-        .sg_tablesize   	= SG_ALL,
-	.cmd_per_lun    	= CMD_PER_LUN,
-        .use_clustering		= DISABLE_CLUSTERING,
+	.bios_param		= NCR5380_BIOSPARAM,
+	.can_queue		= 16,
+	.this_id		= 7,
+	.sg_tablesize		= SG_ALL,
+	.cmd_per_lun		= 2,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
-#include <linux/module.h>
+
 #include "scsi_module.c"
 
 module_param(ncr_irq, int, 0);
@@ -721,6 +792,7 @@ module_param(ncr_5380, int, 0);
 module_param(ncr_53c400, int, 0);
 module_param(ncr_53c400a, int, 0);
 module_param(dtc_3181e, int, 0);
+module_param(hp_c2502, int, 0);
 MODULE_LICENSE("GPL");
 
 #if !defined(SCSI_G_NCR5380_MEM) && defined(MODULE)
diff --git a/drivers/scsi/g_NCR5380.h b/drivers/scsi/g_NCR5380.h
index bea1a3b9b862..6f3d2ac4f185 100644
--- a/drivers/scsi/g_NCR5380.h
+++ b/drivers/scsi/g_NCR5380.h
@@ -14,81 +14,67 @@
 #ifndef GENERIC_NCR5380_H
 #define GENERIC_NCR5380_H
 
-#ifdef NCR53C400
+#ifdef CONFIG_SCSI_GENERIC_NCR53C400
 #define BIOSPARAM
 #define NCR5380_BIOSPARAM generic_NCR5380_biosparam
 #else
 #define NCR5380_BIOSPARAM NULL
 #endif
 
-#ifndef ASM
-
-#ifndef CMD_PER_LUN
-#define CMD_PER_LUN 2
-#endif
-
-#ifndef CAN_QUEUE
-#define CAN_QUEUE 16
-#endif
-
 #define __STRVAL(x) #x
 #define STRVAL(x) __STRVAL(x)
 
 #ifndef SCSI_G_NCR5380_MEM
+#define DRV_MODULE_NAME "g_NCR5380"
 
-#define NCR5380_map_config port
 #define NCR5380_map_type int
 #define NCR5380_map_name port
-#define NCR5380_instance_name io_port
-#define NCR53C400_register_offset 0
-#define NCR53C400_address_adjust 8
 
-#ifdef NCR53C400
+#ifdef CONFIG_SCSI_GENERIC_NCR53C400
 #define NCR5380_region_size 16
 #else
 #define NCR5380_region_size 8
 #endif
 
-#define NCR5380_read(reg) (inb(NCR5380_map_name + (reg)))
-#define NCR5380_write(reg, value) (outb((value), (NCR5380_map_name + (reg))))
+#define NCR5380_read(reg) \
+	inb(instance->io_port + (reg))
+#define NCR5380_write(reg, value) \
+	outb(value, instance->io_port + (reg))
 
 #define NCR5380_implementation_fields \
-    NCR5380_map_type NCR5380_map_name
-
-#define NCR5380_local_declare() \
-    register NCR5380_implementation_fields
-
-#define NCR5380_setup(instance) \
-    NCR5380_map_name = (NCR5380_map_type)((instance)->NCR5380_instance_name)
+	int c400_ctl_status; \
+	int c400_blk_cnt; \
+	int c400_host_buf; \
+	int io_width;
 
 #else 
 /* therefore SCSI_G_NCR5380_MEM */
+#define DRV_MODULE_NAME "g_NCR5380_mmio"
 
-#define NCR5380_map_config memory
 #define NCR5380_map_type unsigned long
 #define NCR5380_map_name base
-#define NCR5380_instance_name base
-#define NCR53C400_register_offset 0x108
-#define NCR53C400_address_adjust 0
 #define NCR53C400_mem_base 0x3880
 #define NCR53C400_host_buffer 0x3900
 #define NCR5380_region_size 0x3a00
 
-#define NCR5380_read(reg) readb(iomem + NCR53C400_mem_base + (reg))
-#define NCR5380_write(reg, value) writeb(value, iomem + NCR53C400_mem_base + (reg))
+#define NCR5380_read(reg) \
+	readb(((struct NCR5380_hostdata *)shost_priv(instance))->iomem + \
+	      NCR53C400_mem_base + (reg))
+#define NCR5380_write(reg, value) \
+	writeb(value, ((struct NCR5380_hostdata *)shost_priv(instance))->iomem + \
+	       NCR53C400_mem_base + (reg))
 
 #define NCR5380_implementation_fields \
-    NCR5380_map_type NCR5380_map_name; \
-    void __iomem *iomem;
-
-#define NCR5380_local_declare() \
-    register void __iomem *iomem
-
-#define NCR5380_setup(instance) \
-    iomem = (((struct NCR5380_hostdata *)(instance)->hostdata)->iomem)
+	void __iomem *iomem; \
+	int c400_ctl_status; \
+	int c400_blk_cnt; \
+	int c400_host_buf;
 
 #endif
 
+#define NCR5380_dma_xfer_len(instance, cmd, phase) \
+        generic_NCR5380_dma_xfer_len(cmd)
+
 #define NCR5380_intr generic_NCR5380_intr
 #define NCR5380_queue_command generic_NCR5380_queue_command
 #define NCR5380_abort generic_NCR5380_abort
@@ -102,7 +88,7 @@
 #define BOARD_NCR53C400	1
 #define BOARD_NCR53C400A 2
 #define BOARD_DTC3181E	3
+#define BOARD_HP_C2502	4
 
-#endif /* ndef ASM */
 #endif /* GENERIC_NCR5380_H */
 
diff --git a/drivers/scsi/hisi_sas/Kconfig b/drivers/scsi/hisi_sas/Kconfig
new file mode 100644
index 000000000000..d1dd1616f983
--- /dev/null
+++ b/drivers/scsi/hisi_sas/Kconfig
@@ -0,0 +1,8 @@
+config SCSI_HISI_SAS
+	tristate "HiSilicon SAS"
+	depends on HAS_DMA && HAS_IOMEM
+	depends on ARM64 || COMPILE_TEST
+	select SCSI_SAS_LIBSAS
+	select BLK_DEV_INTEGRITY
+	help
+		This driver supports HiSilicon's SAS HBA
diff --git a/drivers/scsi/hisi_sas/Makefile b/drivers/scsi/hisi_sas/Makefile
new file mode 100644
index 000000000000..3e70eae81343
--- /dev/null
+++ b/drivers/scsi/hisi_sas/Makefile
@@ -0,0 +1,2 @@
+obj-$(CONFIG_SCSI_HISI_SAS)		+= hisi_sas_main.o
+obj-$(CONFIG_SCSI_HISI_SAS)		+= hisi_sas_v1_hw.o
diff --git a/drivers/scsi/hisi_sas/hisi_sas.h b/drivers/scsi/hisi_sas/hisi_sas.h
new file mode 100644
index 000000000000..5af2e4187f01
--- /dev/null
+++ b/drivers/scsi/hisi_sas/hisi_sas.h
@@ -0,0 +1,341 @@
+/*
+ * Copyright (c) 2015 Linaro Ltd.
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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 _HISI_SAS_H_
+#define _HISI_SAS_H_
+
+#include <linux/dmapool.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <scsi/libsas.h>
+
+#define DRV_VERSION "v1.0"
+
+#define HISI_SAS_MAX_PHYS	9
+#define HISI_SAS_MAX_QUEUES	32
+#define HISI_SAS_QUEUE_SLOTS 512
+#define HISI_SAS_MAX_ITCT_ENTRIES 4096
+#define HISI_SAS_MAX_DEVICES HISI_SAS_MAX_ITCT_ENTRIES
+#define HISI_SAS_COMMAND_ENTRIES 8192
+
+#define HISI_SAS_STATUS_BUF_SZ \
+		(sizeof(struct hisi_sas_err_record) + 1024)
+#define HISI_SAS_COMMAND_TABLE_SZ \
+		(((sizeof(union hisi_sas_command_table)+3)/4)*4)
+
+#define HISI_SAS_MAX_SSP_RESP_SZ (sizeof(struct ssp_frame_hdr) + 1024)
+#define HISI_SAS_MAX_SMP_RESP_SZ 1028
+
+struct hisi_hba;
+
+enum {
+	PORT_TYPE_SAS = (1U << 1),
+	PORT_TYPE_SATA = (1U << 0),
+};
+
+enum dev_status {
+	HISI_SAS_DEV_NORMAL,
+	HISI_SAS_DEV_EH,
+};
+
+enum hisi_sas_dev_type {
+	HISI_SAS_DEV_TYPE_STP = 0,
+	HISI_SAS_DEV_TYPE_SSP,
+	HISI_SAS_DEV_TYPE_SATA,
+};
+
+struct hisi_sas_phy {
+	struct hisi_hba	*hisi_hba;
+	struct hisi_sas_port	*port;
+	struct asd_sas_phy	sas_phy;
+	struct sas_identify	identify;
+	struct timer_list	timer;
+	struct work_struct	phyup_ws;
+	u64		port_id; /* from hw */
+	u64		dev_sas_addr;
+	u64		phy_type;
+	u64		frame_rcvd_size;
+	u8		frame_rcvd[32];
+	u8		phy_attached;
+	u8		reserved[3];
+	enum sas_linkrate	minimum_linkrate;
+	enum sas_linkrate	maximum_linkrate;
+};
+
+struct hisi_sas_port {
+	struct asd_sas_port	sas_port;
+	u8	port_attached;
+	u8	id; /* from hw */
+	struct list_head	list;
+};
+
+struct hisi_sas_cq {
+	struct hisi_hba *hisi_hba;
+	int	id;
+};
+
+struct hisi_sas_device {
+	enum sas_device_type	dev_type;
+	struct hisi_hba		*hisi_hba;
+	struct domain_device	*sas_device;
+	u64 attached_phy;
+	u64 device_id;
+	u64 running_req;
+	u8 dev_status;
+};
+
+struct hisi_sas_slot {
+	struct list_head entry;
+	struct sas_task *task;
+	struct hisi_sas_port	*port;
+	u64	n_elem;
+	int	dlvry_queue;
+	int	dlvry_queue_slot;
+	int	cmplt_queue;
+	int	cmplt_queue_slot;
+	int	idx;
+	void	*cmd_hdr;
+	dma_addr_t cmd_hdr_dma;
+	void	*status_buffer;
+	dma_addr_t status_buffer_dma;
+	void *command_table;
+	dma_addr_t command_table_dma;
+	struct hisi_sas_sge_page *sge_page;
+	dma_addr_t sge_page_dma;
+};
+
+struct hisi_sas_tmf_task {
+	u8 tmf;
+	u16 tag_of_task_to_be_managed;
+};
+
+struct hisi_sas_hw {
+	int (*hw_init)(struct hisi_hba *hisi_hba);
+	void (*setup_itct)(struct hisi_hba *hisi_hba,
+			   struct hisi_sas_device *device);
+	void (*sl_notify)(struct hisi_hba *hisi_hba, int phy_no);
+	int (*get_free_slot)(struct hisi_hba *hisi_hba, int *q, int *s);
+	void (*start_delivery)(struct hisi_hba *hisi_hba);
+	int (*prep_ssp)(struct hisi_hba *hisi_hba,
+			struct hisi_sas_slot *slot, int is_tmf,
+			struct hisi_sas_tmf_task *tmf);
+	int (*prep_smp)(struct hisi_hba *hisi_hba,
+			struct hisi_sas_slot *slot);
+	int (*slot_complete)(struct hisi_hba *hisi_hba,
+			     struct hisi_sas_slot *slot, int abort);
+	void (*phy_enable)(struct hisi_hba *hisi_hba, int phy_no);
+	void (*phy_disable)(struct hisi_hba *hisi_hba, int phy_no);
+	void (*phy_hard_reset)(struct hisi_hba *hisi_hba, int phy_no);
+	void (*free_device)(struct hisi_hba *hisi_hba,
+			    struct hisi_sas_device *dev);
+	int (*get_wideport_bitmap)(struct hisi_hba *hisi_hba, int port_id);
+	int complete_hdr_size;
+};
+
+struct hisi_hba {
+	/* This must be the first element, used by SHOST_TO_SAS_HA */
+	struct sas_ha_struct *p;
+
+	struct platform_device *pdev;
+	void __iomem *regs;
+	struct regmap *ctrl;
+	u32 ctrl_reset_reg;
+	u32 ctrl_reset_sts_reg;
+	u32 ctrl_clock_ena_reg;
+	u8 sas_addr[SAS_ADDR_SIZE];
+
+	int n_phy;
+	int scan_finished;
+	spinlock_t lock;
+
+	struct timer_list timer;
+	struct workqueue_struct *wq;
+
+	int slot_index_count;
+	unsigned long *slot_index_tags;
+
+	/* SCSI/SAS glue */
+	struct sas_ha_struct sha;
+	struct Scsi_Host *shost;
+
+	struct hisi_sas_cq cq[HISI_SAS_MAX_QUEUES];
+	struct hisi_sas_phy phy[HISI_SAS_MAX_PHYS];
+	struct hisi_sas_port port[HISI_SAS_MAX_PHYS];
+
+	int	queue_count;
+	int	queue;
+	struct hisi_sas_slot	*slot_prep;
+
+	struct dma_pool *sge_page_pool;
+	struct hisi_sas_device	devices[HISI_SAS_MAX_DEVICES];
+	struct dma_pool *command_table_pool;
+	struct dma_pool *status_buffer_pool;
+	struct hisi_sas_cmd_hdr	*cmd_hdr[HISI_SAS_MAX_QUEUES];
+	dma_addr_t cmd_hdr_dma[HISI_SAS_MAX_QUEUES];
+	void *complete_hdr[HISI_SAS_MAX_QUEUES];
+	dma_addr_t complete_hdr_dma[HISI_SAS_MAX_QUEUES];
+	struct hisi_sas_initial_fis *initial_fis;
+	dma_addr_t initial_fis_dma;
+	struct hisi_sas_itct *itct;
+	dma_addr_t itct_dma;
+	struct hisi_sas_iost *iost;
+	dma_addr_t iost_dma;
+	struct hisi_sas_breakpoint *breakpoint;
+	dma_addr_t breakpoint_dma;
+	struct hisi_sas_breakpoint *sata_breakpoint;
+	dma_addr_t sata_breakpoint_dma;
+	struct hisi_sas_slot	*slot_info;
+	const struct hisi_sas_hw *hw;	/* Low level hw interface */
+};
+
+/* Generic HW DMA host memory structures */
+/* Delivery queue header */
+struct hisi_sas_cmd_hdr {
+	/* dw0 */
+	__le32 dw0;
+
+	/* dw1 */
+	__le32 dw1;
+
+	/* dw2 */
+	__le32 dw2;
+
+	/* dw3 */
+	__le32 transfer_tags;
+
+	/* dw4 */
+	__le32 data_transfer_len;
+
+	/* dw5 */
+	__le32 first_burst_num;
+
+	/* dw6 */
+	__le32 sg_len;
+
+	/* dw7 */
+	__le32 dw7;
+
+	/* dw8-9 */
+	__le64 cmd_table_addr;
+
+	/* dw10-11 */
+	__le64 sts_buffer_addr;
+
+	/* dw12-13 */
+	__le64 prd_table_addr;
+
+	/* dw14-15 */
+	__le64 dif_prd_table_addr;
+};
+
+struct hisi_sas_itct {
+	__le64 qw0;
+	__le64 sas_addr;
+	__le64 qw2;
+	__le64 qw3;
+	__le64 qw4;
+	__le64 qw_sata_ncq0_3;
+	__le64 qw_sata_ncq7_4;
+	__le64 qw_sata_ncq11_8;
+	__le64 qw_sata_ncq15_12;
+	__le64 qw_sata_ncq19_16;
+	__le64 qw_sata_ncq23_20;
+	__le64 qw_sata_ncq27_24;
+	__le64 qw_sata_ncq31_28;
+	__le64 qw_non_ncq_iptt;
+	__le64 qw_rsvd0;
+	__le64 qw_rsvd1;
+};
+
+struct hisi_sas_iost {
+	__le64 qw0;
+	__le64 qw1;
+	__le64 qw2;
+	__le64 qw3;
+};
+
+struct hisi_sas_err_record {
+	/* dw0 */
+	__le32 dma_err_type;
+
+	/* dw1 */
+	__le32 trans_tx_fail_type;
+
+	/* dw2 */
+	__le32 trans_rx_fail_type;
+
+	/* dw3 */
+	u32 rsvd;
+};
+
+struct hisi_sas_initial_fis {
+	struct hisi_sas_err_record err_record;
+	struct dev_to_host_fis fis;
+	u32 rsvd[3];
+};
+
+struct hisi_sas_breakpoint {
+	u8	data[128];	/*io128 byte*/
+};
+
+struct hisi_sas_sge {
+	__le64 addr;
+	__le32 page_ctrl_0;
+	__le32 page_ctrl_1;
+	__le32 data_len;
+	__le32 data_off;
+};
+
+struct hisi_sas_command_table_smp {
+	u8 bytes[44];
+};
+
+struct hisi_sas_command_table_stp {
+	struct	host_to_dev_fis command_fis;
+	u8	dummy[12];
+	u8	atapi_cdb[ATAPI_CDB_LEN];
+};
+
+#define HISI_SAS_SGE_PAGE_CNT SCSI_MAX_SG_SEGMENTS
+struct hisi_sas_sge_page {
+	struct hisi_sas_sge sge[HISI_SAS_SGE_PAGE_CNT];
+};
+
+struct hisi_sas_command_table_ssp {
+	struct ssp_frame_hdr hdr;
+	union {
+		struct {
+			struct ssp_command_iu task;
+			u32 prot[6];
+		};
+		struct ssp_tmf_iu ssp_task;
+		struct xfer_rdy_iu xfer_rdy;
+		struct ssp_response_iu ssp_res;
+	} u;
+};
+
+union hisi_sas_command_table {
+	struct hisi_sas_command_table_ssp ssp;
+	struct hisi_sas_command_table_smp smp;
+	struct hisi_sas_command_table_stp stp;
+};
+extern int hisi_sas_probe(struct platform_device *pdev,
+			  const struct hisi_sas_hw *ops);
+extern int hisi_sas_remove(struct platform_device *pdev);
+
+extern void hisi_sas_phy_down(struct hisi_hba *hisi_hba, int phy_no, int rdy);
+extern void hisi_sas_slot_task_free(struct hisi_hba *hisi_hba,
+				    struct sas_task *task,
+				    struct hisi_sas_slot *slot);
+#endif
diff --git a/drivers/scsi/hisi_sas/hisi_sas_main.c b/drivers/scsi/hisi_sas/hisi_sas_main.c
new file mode 100644
index 000000000000..99b1950d751c
--- /dev/null
+++ b/drivers/scsi/hisi_sas/hisi_sas_main.c
@@ -0,0 +1,1358 @@
+/*
+ * Copyright (c) 2015 Linaro Ltd.
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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 "hisi_sas.h"
+#define DRV_NAME "hisi_sas"
+
+#define DEV_IS_EXPANDER(type) \
+	((type == SAS_EDGE_EXPANDER_DEVICE) || \
+	(type == SAS_FANOUT_EXPANDER_DEVICE))
+
+#define DEV_IS_GONE(dev) \
+	((!dev) || (dev->dev_type == SAS_PHY_UNUSED))
+
+static struct hisi_hba *dev_to_hisi_hba(struct domain_device *device)
+{
+	return device->port->ha->lldd_ha;
+}
+
+static void hisi_sas_slot_index_clear(struct hisi_hba *hisi_hba, int slot_idx)
+{
+	void *bitmap = hisi_hba->slot_index_tags;
+
+	clear_bit(slot_idx, bitmap);
+}
+
+static void hisi_sas_slot_index_free(struct hisi_hba *hisi_hba, int slot_idx)
+{
+	hisi_sas_slot_index_clear(hisi_hba, slot_idx);
+}
+
+static void hisi_sas_slot_index_set(struct hisi_hba *hisi_hba, int slot_idx)
+{
+	void *bitmap = hisi_hba->slot_index_tags;
+
+	set_bit(slot_idx, bitmap);
+}
+
+static int hisi_sas_slot_index_alloc(struct hisi_hba *hisi_hba, int *slot_idx)
+{
+	unsigned int index;
+	void *bitmap = hisi_hba->slot_index_tags;
+
+	index = find_first_zero_bit(bitmap, hisi_hba->slot_index_count);
+	if (index >= hisi_hba->slot_index_count)
+		return -SAS_QUEUE_FULL;
+	hisi_sas_slot_index_set(hisi_hba, index);
+	*slot_idx = index;
+	return 0;
+}
+
+static void hisi_sas_slot_index_init(struct hisi_hba *hisi_hba)
+{
+	int i;
+
+	for (i = 0; i < hisi_hba->slot_index_count; ++i)
+		hisi_sas_slot_index_clear(hisi_hba, i);
+}
+
+void hisi_sas_slot_task_free(struct hisi_hba *hisi_hba, struct sas_task *task,
+			     struct hisi_sas_slot *slot)
+{
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	if (!slot->task)
+		return;
+
+	if (!sas_protocol_ata(task->task_proto))
+		if (slot->n_elem)
+			dma_unmap_sg(dev, task->scatter, slot->n_elem,
+				     task->data_dir);
+
+	if (slot->command_table)
+		dma_pool_free(hisi_hba->command_table_pool,
+			      slot->command_table, slot->command_table_dma);
+
+	if (slot->status_buffer)
+		dma_pool_free(hisi_hba->status_buffer_pool,
+			      slot->status_buffer, slot->status_buffer_dma);
+
+	if (slot->sge_page)
+		dma_pool_free(hisi_hba->sge_page_pool, slot->sge_page,
+			      slot->sge_page_dma);
+
+	list_del_init(&slot->entry);
+	task->lldd_task = NULL;
+	slot->task = NULL;
+	slot->port = NULL;
+	hisi_sas_slot_index_free(hisi_hba, slot->idx);
+	memset(slot, 0, sizeof(*slot));
+}
+EXPORT_SYMBOL_GPL(hisi_sas_slot_task_free);
+
+static int hisi_sas_task_prep_smp(struct hisi_hba *hisi_hba,
+				  struct hisi_sas_slot *slot)
+{
+	return hisi_hba->hw->prep_smp(hisi_hba, slot);
+}
+
+static int hisi_sas_task_prep_ssp(struct hisi_hba *hisi_hba,
+				  struct hisi_sas_slot *slot, int is_tmf,
+				  struct hisi_sas_tmf_task *tmf)
+{
+	return hisi_hba->hw->prep_ssp(hisi_hba, slot, is_tmf, tmf);
+}
+
+static int hisi_sas_task_prep(struct sas_task *task, struct hisi_hba *hisi_hba,
+			      int is_tmf, struct hisi_sas_tmf_task *tmf,
+			      int *pass)
+{
+	struct domain_device *device = task->dev;
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_sas_port *port;
+	struct hisi_sas_slot *slot;
+	struct hisi_sas_cmd_hdr	*cmd_hdr_base;
+	struct device *dev = &hisi_hba->pdev->dev;
+	int dlvry_queue_slot, dlvry_queue, n_elem = 0, rc, slot_idx;
+
+	if (!device->port) {
+		struct task_status_struct *ts = &task->task_status;
+
+		ts->resp = SAS_TASK_UNDELIVERED;
+		ts->stat = SAS_PHY_DOWN;
+		/*
+		 * libsas will use dev->port, should
+		 * not call task_done for sata
+		 */
+		if (device->dev_type != SAS_SATA_DEV)
+			task->task_done(task);
+		return 0;
+	}
+
+	if (DEV_IS_GONE(sas_dev)) {
+		if (sas_dev)
+			dev_info(dev, "task prep: device %llu not ready\n",
+				 sas_dev->device_id);
+		else
+			dev_info(dev, "task prep: device %016llx not ready\n",
+				 SAS_ADDR(device->sas_addr));
+
+		rc = SAS_PHY_DOWN;
+		return rc;
+	}
+	port = device->port->lldd_port;
+	if (port && !port->port_attached && !tmf) {
+		if (sas_protocol_ata(task->task_proto)) {
+			struct task_status_struct *ts = &task->task_status;
+
+			dev_info(dev,
+				 "task prep: SATA/STP port%d not attach device\n",
+				 device->port->id);
+			ts->resp = SAS_TASK_COMPLETE;
+			ts->stat = SAS_PHY_DOWN;
+			task->task_done(task);
+		} else {
+			struct task_status_struct *ts = &task->task_status;
+
+			dev_info(dev,
+				 "task prep: SAS port%d does not attach device\n",
+				 device->port->id);
+			ts->resp = SAS_TASK_UNDELIVERED;
+			ts->stat = SAS_PHY_DOWN;
+			task->task_done(task);
+		}
+		return 0;
+	}
+
+	if (!sas_protocol_ata(task->task_proto)) {
+		if (task->num_scatter) {
+			n_elem = dma_map_sg(dev, task->scatter,
+					    task->num_scatter, task->data_dir);
+			if (!n_elem) {
+				rc = -ENOMEM;
+				goto prep_out;
+			}
+		}
+	} else
+		n_elem = task->num_scatter;
+
+	rc = hisi_sas_slot_index_alloc(hisi_hba, &slot_idx);
+	if (rc)
+		goto err_out;
+	rc = hisi_hba->hw->get_free_slot(hisi_hba, &dlvry_queue,
+					 &dlvry_queue_slot);
+	if (rc)
+		goto err_out_tag;
+
+	slot = &hisi_hba->slot_info[slot_idx];
+	memset(slot, 0, sizeof(struct hisi_sas_slot));
+
+	slot->idx = slot_idx;
+	slot->n_elem = n_elem;
+	slot->dlvry_queue = dlvry_queue;
+	slot->dlvry_queue_slot = dlvry_queue_slot;
+	cmd_hdr_base = hisi_hba->cmd_hdr[dlvry_queue];
+	slot->cmd_hdr = &cmd_hdr_base[dlvry_queue_slot];
+	slot->task = task;
+	slot->port = port;
+	task->lldd_task = slot;
+
+	slot->status_buffer = dma_pool_alloc(hisi_hba->status_buffer_pool,
+					     GFP_ATOMIC,
+					     &slot->status_buffer_dma);
+	if (!slot->status_buffer) {
+		rc = -ENOMEM;
+		goto err_out_slot_buf;
+	}
+	memset(slot->status_buffer, 0, HISI_SAS_STATUS_BUF_SZ);
+
+	slot->command_table = dma_pool_alloc(hisi_hba->command_table_pool,
+					     GFP_ATOMIC,
+					     &slot->command_table_dma);
+	if (!slot->command_table) {
+		rc = -ENOMEM;
+		goto err_out_status_buf;
+	}
+	memset(slot->command_table, 0, HISI_SAS_COMMAND_TABLE_SZ);
+	memset(slot->cmd_hdr, 0, sizeof(struct hisi_sas_cmd_hdr));
+
+	switch (task->task_proto) {
+	case SAS_PROTOCOL_SMP:
+		rc = hisi_sas_task_prep_smp(hisi_hba, slot);
+		break;
+	case SAS_PROTOCOL_SSP:
+		rc = hisi_sas_task_prep_ssp(hisi_hba, slot, is_tmf, tmf);
+		break;
+	case SAS_PROTOCOL_SATA:
+	case SAS_PROTOCOL_STP:
+	case SAS_PROTOCOL_SATA | SAS_PROTOCOL_STP:
+	default:
+		dev_err(dev, "task prep: unknown/unsupported proto (0x%x)\n",
+			task->task_proto);
+		rc = -EINVAL;
+		break;
+	}
+
+	if (rc) {
+		dev_err(dev, "task prep: rc = 0x%x\n", rc);
+		if (slot->sge_page)
+			goto err_out_sge;
+		goto err_out_command_table;
+	}
+
+	list_add_tail(&slot->entry, &port->list);
+	spin_lock(&task->task_state_lock);
+	task->task_state_flags |= SAS_TASK_AT_INITIATOR;
+	spin_unlock(&task->task_state_lock);
+
+	hisi_hba->slot_prep = slot;
+
+	sas_dev->running_req++;
+	++(*pass);
+
+	return 0;
+
+err_out_sge:
+	dma_pool_free(hisi_hba->sge_page_pool, slot->sge_page,
+		slot->sge_page_dma);
+err_out_command_table:
+	dma_pool_free(hisi_hba->command_table_pool, slot->command_table,
+		slot->command_table_dma);
+err_out_status_buf:
+	dma_pool_free(hisi_hba->status_buffer_pool, slot->status_buffer,
+		slot->status_buffer_dma);
+err_out_slot_buf:
+	/* Nothing to be done */
+err_out_tag:
+	hisi_sas_slot_index_free(hisi_hba, slot_idx);
+err_out:
+	dev_err(dev, "task prep: failed[%d]!\n", rc);
+	if (!sas_protocol_ata(task->task_proto))
+		if (n_elem)
+			dma_unmap_sg(dev, task->scatter, n_elem,
+				     task->data_dir);
+prep_out:
+	return rc;
+}
+
+static int hisi_sas_task_exec(struct sas_task *task, gfp_t gfp_flags,
+			      int is_tmf, struct hisi_sas_tmf_task *tmf)
+{
+	u32 rc;
+	u32 pass = 0;
+	unsigned long flags;
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(task->dev);
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	/* protect task_prep and start_delivery sequence */
+	spin_lock_irqsave(&hisi_hba->lock, flags);
+	rc = hisi_sas_task_prep(task, hisi_hba, is_tmf, tmf, &pass);
+	if (rc)
+		dev_err(dev, "task exec: failed[%d]!\n", rc);
+
+	if (likely(pass))
+		hisi_hba->hw->start_delivery(hisi_hba);
+	spin_unlock_irqrestore(&hisi_hba->lock, flags);
+
+	return rc;
+}
+
+static void hisi_sas_bytes_dmaed(struct hisi_hba *hisi_hba, int phy_no)
+{
+	struct hisi_sas_phy *phy = &hisi_hba->phy[phy_no];
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	struct sas_ha_struct *sas_ha;
+
+	if (!phy->phy_attached)
+		return;
+
+	sas_ha = &hisi_hba->sha;
+	sas_ha->notify_phy_event(sas_phy, PHYE_OOB_DONE);
+
+	if (sas_phy->phy) {
+		struct sas_phy *sphy = sas_phy->phy;
+
+		sphy->negotiated_linkrate = sas_phy->linkrate;
+		sphy->minimum_linkrate = phy->minimum_linkrate;
+		sphy->minimum_linkrate_hw = SAS_LINK_RATE_1_5_GBPS;
+		sphy->maximum_linkrate = phy->maximum_linkrate;
+	}
+
+	if (phy->phy_type & PORT_TYPE_SAS) {
+		struct sas_identify_frame *id;
+
+		id = (struct sas_identify_frame *)phy->frame_rcvd;
+		id->dev_type = phy->identify.device_type;
+		id->initiator_bits = SAS_PROTOCOL_ALL;
+		id->target_bits = phy->identify.target_port_protocols;
+	} else if (phy->phy_type & PORT_TYPE_SATA) {
+		/*Nothing*/
+	}
+
+	sas_phy->frame_rcvd_size = phy->frame_rcvd_size;
+	sas_ha->notify_port_event(sas_phy, PORTE_BYTES_DMAED);
+}
+
+static struct hisi_sas_device *hisi_sas_alloc_dev(struct domain_device *device)
+{
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(device);
+	struct hisi_sas_device *sas_dev = NULL;
+	int i;
+
+	spin_lock(&hisi_hba->lock);
+	for (i = 0; i < HISI_SAS_MAX_DEVICES; i++) {
+		if (hisi_hba->devices[i].dev_type == SAS_PHY_UNUSED) {
+			hisi_hba->devices[i].device_id = i;
+			sas_dev = &hisi_hba->devices[i];
+			sas_dev->dev_status = HISI_SAS_DEV_NORMAL;
+			sas_dev->dev_type = device->dev_type;
+			sas_dev->hisi_hba = hisi_hba;
+			sas_dev->sas_device = device;
+			break;
+		}
+	}
+	spin_unlock(&hisi_hba->lock);
+
+	return sas_dev;
+}
+
+static int hisi_sas_dev_found(struct domain_device *device)
+{
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(device);
+	struct domain_device *parent_dev = device->parent;
+	struct hisi_sas_device *sas_dev;
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	sas_dev = hisi_sas_alloc_dev(device);
+	if (!sas_dev) {
+		dev_err(dev, "fail alloc dev: max support %d devices\n",
+			HISI_SAS_MAX_DEVICES);
+		return -EINVAL;
+	}
+
+	device->lldd_dev = sas_dev;
+	hisi_hba->hw->setup_itct(hisi_hba, sas_dev);
+
+	if (parent_dev && DEV_IS_EXPANDER(parent_dev->dev_type)) {
+		int phy_no;
+		u8 phy_num = parent_dev->ex_dev.num_phys;
+		struct ex_phy *phy;
+
+		for (phy_no = 0; phy_no < phy_num; phy_no++) {
+			phy = &parent_dev->ex_dev.ex_phy[phy_no];
+			if (SAS_ADDR(phy->attached_sas_addr) ==
+				SAS_ADDR(device->sas_addr)) {
+				sas_dev->attached_phy = phy_no;
+				break;
+			}
+		}
+
+		if (phy_no == phy_num) {
+			dev_info(dev, "dev found: no attached "
+				 "dev:%016llx at ex:%016llx\n",
+				 SAS_ADDR(device->sas_addr),
+				 SAS_ADDR(parent_dev->sas_addr));
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void hisi_sas_scan_start(struct Scsi_Host *shost)
+{
+	struct hisi_hba *hisi_hba = shost_priv(shost);
+	int i;
+
+	for (i = 0; i < hisi_hba->n_phy; ++i)
+		hisi_sas_bytes_dmaed(hisi_hba, i);
+
+	hisi_hba->scan_finished = 1;
+}
+
+static int hisi_sas_scan_finished(struct Scsi_Host *shost, unsigned long time)
+{
+	struct hisi_hba *hisi_hba = shost_priv(shost);
+	struct sas_ha_struct *sha = &hisi_hba->sha;
+
+	if (hisi_hba->scan_finished == 0)
+		return 0;
+
+	sas_drain_work(sha);
+	return 1;
+}
+
+static void hisi_sas_phyup_work(struct work_struct *work)
+{
+	struct hisi_sas_phy *phy =
+		container_of(work, struct hisi_sas_phy, phyup_ws);
+	struct hisi_hba *hisi_hba = phy->hisi_hba;
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	int phy_no = sas_phy->id;
+
+	hisi_hba->hw->sl_notify(hisi_hba, phy_no); /* This requires a sleep */
+	hisi_sas_bytes_dmaed(hisi_hba, phy_no);
+}
+
+static void hisi_sas_phy_init(struct hisi_hba *hisi_hba, int phy_no)
+{
+	struct hisi_sas_phy *phy = &hisi_hba->phy[phy_no];
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+
+	phy->hisi_hba = hisi_hba;
+	phy->port = NULL;
+	init_timer(&phy->timer);
+	sas_phy->enabled = (phy_no < hisi_hba->n_phy) ? 1 : 0;
+	sas_phy->class = SAS;
+	sas_phy->iproto = SAS_PROTOCOL_ALL;
+	sas_phy->tproto = 0;
+	sas_phy->type = PHY_TYPE_PHYSICAL;
+	sas_phy->role = PHY_ROLE_INITIATOR;
+	sas_phy->oob_mode = OOB_NOT_CONNECTED;
+	sas_phy->linkrate = SAS_LINK_RATE_UNKNOWN;
+	sas_phy->id = phy_no;
+	sas_phy->sas_addr = &hisi_hba->sas_addr[0];
+	sas_phy->frame_rcvd = &phy->frame_rcvd[0];
+	sas_phy->ha = (struct sas_ha_struct *)hisi_hba->shost->hostdata;
+	sas_phy->lldd_phy = phy;
+
+	INIT_WORK(&phy->phyup_ws, hisi_sas_phyup_work);
+}
+
+static void hisi_sas_port_notify_formed(struct asd_sas_phy *sas_phy)
+{
+	struct sas_ha_struct *sas_ha = sas_phy->ha;
+	struct hisi_hba *hisi_hba = sas_ha->lldd_ha;
+	struct hisi_sas_phy *phy = sas_phy->lldd_phy;
+	struct asd_sas_port *sas_port = sas_phy->port;
+	struct hisi_sas_port *port = &hisi_hba->port[sas_phy->id];
+	unsigned long flags;
+
+	if (!sas_port)
+		return;
+
+	spin_lock_irqsave(&hisi_hba->lock, flags);
+	port->port_attached = 1;
+	port->id = phy->port_id;
+	phy->port = port;
+	sas_port->lldd_port = port;
+	spin_unlock_irqrestore(&hisi_hba->lock, flags);
+}
+
+static void hisi_sas_do_release_task(struct hisi_hba *hisi_hba, int phy_no,
+				     struct domain_device *device)
+{
+	struct hisi_sas_phy *phy;
+	struct hisi_sas_port *port;
+	struct hisi_sas_slot *slot, *slot2;
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	phy = &hisi_hba->phy[phy_no];
+	port = phy->port;
+	if (!port)
+		return;
+
+	list_for_each_entry_safe(slot, slot2, &port->list, entry) {
+		struct sas_task *task;
+
+		task = slot->task;
+		if (device && task->dev != device)
+			continue;
+
+		dev_info(dev, "Release slot [%d:%d], task [%p]:\n",
+			 slot->dlvry_queue, slot->dlvry_queue_slot, task);
+		hisi_hba->hw->slot_complete(hisi_hba, slot, 1);
+	}
+}
+
+static void hisi_sas_port_notify_deformed(struct asd_sas_phy *sas_phy)
+{
+	struct domain_device *device;
+	struct hisi_sas_phy *phy = sas_phy->lldd_phy;
+	struct asd_sas_port *sas_port = sas_phy->port;
+
+	list_for_each_entry(device, &sas_port->dev_list, dev_list_node)
+		hisi_sas_do_release_task(phy->hisi_hba, sas_phy->id, device);
+}
+
+static void hisi_sas_release_task(struct hisi_hba *hisi_hba,
+			struct domain_device *device)
+{
+	struct asd_sas_port *port = device->port;
+	struct asd_sas_phy *sas_phy;
+
+	list_for_each_entry(sas_phy, &port->phy_list, port_phy_el)
+		hisi_sas_do_release_task(hisi_hba, sas_phy->id, device);
+}
+
+static void hisi_sas_dev_gone(struct domain_device *device)
+{
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(device);
+	struct device *dev = &hisi_hba->pdev->dev;
+	u64 dev_id = sas_dev->device_id;
+
+	dev_info(dev, "found dev[%lld:%x] is gone\n",
+		 sas_dev->device_id, sas_dev->dev_type);
+
+	hisi_hba->hw->free_device(hisi_hba, sas_dev);
+	device->lldd_dev = NULL;
+	memset(sas_dev, 0, sizeof(*sas_dev));
+	sas_dev->device_id = dev_id;
+	sas_dev->dev_type = SAS_PHY_UNUSED;
+	sas_dev->dev_status = HISI_SAS_DEV_NORMAL;
+}
+
+static int hisi_sas_queue_command(struct sas_task *task, gfp_t gfp_flags)
+{
+	return hisi_sas_task_exec(task, gfp_flags, 0, NULL);
+}
+
+static int hisi_sas_control_phy(struct asd_sas_phy *sas_phy, enum phy_func func,
+				void *funcdata)
+{
+	struct sas_ha_struct *sas_ha = sas_phy->ha;
+	struct hisi_hba *hisi_hba = sas_ha->lldd_ha;
+	int phy_no = sas_phy->id;
+
+	switch (func) {
+	case PHY_FUNC_HARD_RESET:
+		hisi_hba->hw->phy_hard_reset(hisi_hba, phy_no);
+		break;
+
+	case PHY_FUNC_LINK_RESET:
+		hisi_hba->hw->phy_enable(hisi_hba, phy_no);
+		hisi_hba->hw->phy_hard_reset(hisi_hba, phy_no);
+		break;
+
+	case PHY_FUNC_DISABLE:
+		hisi_hba->hw->phy_disable(hisi_hba, phy_no);
+		break;
+
+	case PHY_FUNC_SET_LINK_RATE:
+	case PHY_FUNC_RELEASE_SPINUP_HOLD:
+	default:
+		return -EOPNOTSUPP;
+	}
+	return 0;
+}
+
+static void hisi_sas_task_done(struct sas_task *task)
+{
+	if (!del_timer(&task->slow_task->timer))
+		return;
+	complete(&task->slow_task->completion);
+}
+
+static void hisi_sas_tmf_timedout(unsigned long data)
+{
+	struct sas_task *task = (struct sas_task *)data;
+
+	task->task_state_flags |= SAS_TASK_STATE_ABORTED;
+	complete(&task->slow_task->completion);
+}
+
+#define TASK_TIMEOUT 20
+#define TASK_RETRY 3
+static int hisi_sas_exec_internal_tmf_task(struct domain_device *device,
+					   void *parameter, u32 para_len,
+					   struct hisi_sas_tmf_task *tmf)
+{
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_hba *hisi_hba = sas_dev->hisi_hba;
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct sas_task *task;
+	int res, retry;
+
+	for (retry = 0; retry < TASK_RETRY; retry++) {
+		task = sas_alloc_slow_task(GFP_KERNEL);
+		if (!task)
+			return -ENOMEM;
+
+		task->dev = device;
+		task->task_proto = device->tproto;
+
+		memcpy(&task->ssp_task, parameter, para_len);
+		task->task_done = hisi_sas_task_done;
+
+		task->slow_task->timer.data = (unsigned long) task;
+		task->slow_task->timer.function = hisi_sas_tmf_timedout;
+		task->slow_task->timer.expires = jiffies + TASK_TIMEOUT*HZ;
+		add_timer(&task->slow_task->timer);
+
+		res = hisi_sas_task_exec(task, GFP_KERNEL, 1, tmf);
+
+		if (res) {
+			del_timer(&task->slow_task->timer);
+			dev_err(dev, "abort tmf: executing internal task failed: %d\n",
+				res);
+			goto ex_err;
+		}
+
+		wait_for_completion(&task->slow_task->completion);
+		res = TMF_RESP_FUNC_FAILED;
+		/* Even TMF timed out, return direct. */
+		if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
+			if (!(task->task_state_flags & SAS_TASK_STATE_DONE)) {
+				dev_err(dev, "abort tmf: TMF task[%d] timeout\n",
+					tmf->tag_of_task_to_be_managed);
+				if (task->lldd_task) {
+					struct hisi_sas_slot *slot =
+						task->lldd_task;
+
+					hisi_sas_slot_task_free(hisi_hba,
+								task, slot);
+				}
+
+				goto ex_err;
+			}
+		}
+
+		if (task->task_status.resp == SAS_TASK_COMPLETE &&
+		    task->task_status.stat == SAM_STAT_GOOD) {
+			res = TMF_RESP_FUNC_COMPLETE;
+			break;
+		}
+
+		if (task->task_status.resp == SAS_TASK_COMPLETE &&
+		      task->task_status.stat == SAS_DATA_UNDERRUN) {
+			/* no error, but return the number of bytes of
+			 * underrun
+			 */
+			dev_warn(dev, "abort tmf: task to dev %016llx "
+				 "resp: 0x%x sts 0x%x underrun\n",
+				 SAS_ADDR(device->sas_addr),
+				 task->task_status.resp,
+				 task->task_status.stat);
+			res = task->task_status.residual;
+			break;
+		}
+
+		if (task->task_status.resp == SAS_TASK_COMPLETE &&
+			task->task_status.stat == SAS_DATA_OVERRUN) {
+			dev_warn(dev, "abort tmf: blocked task error\n");
+			res = -EMSGSIZE;
+			break;
+		}
+
+		dev_warn(dev, "abort tmf: task to dev "
+			 "%016llx resp: 0x%x status 0x%x\n",
+			 SAS_ADDR(device->sas_addr), task->task_status.resp,
+			 task->task_status.stat);
+		sas_free_task(task);
+		task = NULL;
+	}
+ex_err:
+	WARN_ON(retry == TASK_RETRY);
+	sas_free_task(task);
+	return res;
+}
+
+static int hisi_sas_debug_issue_ssp_tmf(struct domain_device *device,
+				u8 *lun, struct hisi_sas_tmf_task *tmf)
+{
+	struct sas_ssp_task ssp_task;
+
+	if (!(device->tproto & SAS_PROTOCOL_SSP))
+		return TMF_RESP_FUNC_ESUPP;
+
+	memcpy(ssp_task.LUN, lun, 8);
+
+	return hisi_sas_exec_internal_tmf_task(device, &ssp_task,
+				sizeof(ssp_task), tmf);
+}
+
+static int hisi_sas_abort_task(struct sas_task *task)
+{
+	struct scsi_lun lun;
+	struct hisi_sas_tmf_task tmf_task;
+	struct domain_device *device = task->dev;
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(task->dev);
+	struct device *dev = &hisi_hba->pdev->dev;
+	int rc = TMF_RESP_FUNC_FAILED;
+	unsigned long flags;
+
+	if (!sas_dev) {
+		dev_warn(dev, "Device has been removed\n");
+		return TMF_RESP_FUNC_FAILED;
+	}
+
+	spin_lock_irqsave(&task->task_state_lock, flags);
+	if (task->task_state_flags & SAS_TASK_STATE_DONE) {
+		spin_unlock_irqrestore(&task->task_state_lock, flags);
+		rc = TMF_RESP_FUNC_COMPLETE;
+		goto out;
+	}
+
+	spin_unlock_irqrestore(&task->task_state_lock, flags);
+	sas_dev->dev_status = HISI_SAS_DEV_EH;
+	if (task->lldd_task && task->task_proto & SAS_PROTOCOL_SSP) {
+		struct scsi_cmnd *cmnd = task->uldd_task;
+		struct hisi_sas_slot *slot = task->lldd_task;
+		u32 tag = slot->idx;
+
+		int_to_scsilun(cmnd->device->lun, &lun);
+		tmf_task.tmf = TMF_ABORT_TASK;
+		tmf_task.tag_of_task_to_be_managed = cpu_to_le16(tag);
+
+		rc = hisi_sas_debug_issue_ssp_tmf(task->dev, lun.scsi_lun,
+						  &tmf_task);
+
+		/* if successful, clear the task and callback forwards.*/
+		if (rc == TMF_RESP_FUNC_COMPLETE) {
+			if (task->lldd_task) {
+				struct hisi_sas_slot *slot;
+
+				slot = &hisi_hba->slot_info
+					[tmf_task.tag_of_task_to_be_managed];
+				spin_lock_irqsave(&hisi_hba->lock, flags);
+				hisi_hba->hw->slot_complete(hisi_hba, slot, 1);
+				spin_unlock_irqrestore(&hisi_hba->lock, flags);
+			}
+		}
+
+	} else if (task->task_proto & SAS_PROTOCOL_SATA ||
+		task->task_proto & SAS_PROTOCOL_STP) {
+		if (task->dev->dev_type == SAS_SATA_DEV) {
+			struct hisi_slot_info *slot = task->lldd_task;
+
+			dev_notice(dev, "abort task: hba=%p task=%p slot=%p\n",
+				   hisi_hba, task, slot);
+			task->task_state_flags |= SAS_TASK_STATE_ABORTED;
+			rc = TMF_RESP_FUNC_COMPLETE;
+			goto out;
+		}
+
+	}
+
+out:
+	if (rc != TMF_RESP_FUNC_COMPLETE)
+		dev_notice(dev, "abort task: rc=%d\n", rc);
+	return rc;
+}
+
+static int hisi_sas_abort_task_set(struct domain_device *device, u8 *lun)
+{
+	struct hisi_sas_tmf_task tmf_task;
+	int rc = TMF_RESP_FUNC_FAILED;
+
+	tmf_task.tmf = TMF_ABORT_TASK_SET;
+	rc = hisi_sas_debug_issue_ssp_tmf(device, lun, &tmf_task);
+
+	return rc;
+}
+
+static int hisi_sas_clear_aca(struct domain_device *device, u8 *lun)
+{
+	int rc = TMF_RESP_FUNC_FAILED;
+	struct hisi_sas_tmf_task tmf_task;
+
+	tmf_task.tmf = TMF_CLEAR_ACA;
+	rc = hisi_sas_debug_issue_ssp_tmf(device, lun, &tmf_task);
+
+	return rc;
+}
+
+static int hisi_sas_debug_I_T_nexus_reset(struct domain_device *device)
+{
+	struct sas_phy *phy = sas_get_local_phy(device);
+	int rc, reset_type = (device->dev_type == SAS_SATA_DEV ||
+			(device->tproto & SAS_PROTOCOL_STP)) ? 0 : 1;
+	rc = sas_phy_reset(phy, reset_type);
+	sas_put_local_phy(phy);
+	msleep(2000);
+	return rc;
+}
+
+static int hisi_sas_I_T_nexus_reset(struct domain_device *device)
+{
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(device);
+	unsigned long flags;
+	int rc = TMF_RESP_FUNC_FAILED;
+
+	if (sas_dev->dev_status != HISI_SAS_DEV_EH)
+		return TMF_RESP_FUNC_FAILED;
+	sas_dev->dev_status = HISI_SAS_DEV_NORMAL;
+
+	rc = hisi_sas_debug_I_T_nexus_reset(device);
+
+	spin_lock_irqsave(&hisi_hba->lock, flags);
+	hisi_sas_release_task(hisi_hba, device);
+	spin_unlock_irqrestore(&hisi_hba->lock, flags);
+
+	return 0;
+}
+
+static int hisi_sas_lu_reset(struct domain_device *device, u8 *lun)
+{
+	struct hisi_sas_tmf_task tmf_task;
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_hba *hisi_hba = dev_to_hisi_hba(device);
+	struct device *dev = &hisi_hba->pdev->dev;
+	unsigned long flags;
+	int rc = TMF_RESP_FUNC_FAILED;
+
+	tmf_task.tmf = TMF_LU_RESET;
+	sas_dev->dev_status = HISI_SAS_DEV_EH;
+	rc = hisi_sas_debug_issue_ssp_tmf(device, lun, &tmf_task);
+	if (rc == TMF_RESP_FUNC_COMPLETE) {
+		spin_lock_irqsave(&hisi_hba->lock, flags);
+		hisi_sas_release_task(hisi_hba, device);
+		spin_unlock_irqrestore(&hisi_hba->lock, flags);
+	}
+
+	/* If failed, fall-through I_T_Nexus reset */
+	dev_err(dev, "lu_reset: for device[%llx]:rc= %d\n",
+		sas_dev->device_id, rc);
+	return rc;
+}
+
+static int hisi_sas_query_task(struct sas_task *task)
+{
+	struct scsi_lun lun;
+	struct hisi_sas_tmf_task tmf_task;
+	int rc = TMF_RESP_FUNC_FAILED;
+
+	if (task->lldd_task && task->task_proto & SAS_PROTOCOL_SSP) {
+		struct scsi_cmnd *cmnd = task->uldd_task;
+		struct domain_device *device = task->dev;
+		struct hisi_sas_slot *slot = task->lldd_task;
+		u32 tag = slot->idx;
+
+		int_to_scsilun(cmnd->device->lun, &lun);
+		tmf_task.tmf = TMF_QUERY_TASK;
+		tmf_task.tag_of_task_to_be_managed = cpu_to_le16(tag);
+
+		rc = hisi_sas_debug_issue_ssp_tmf(device,
+						  lun.scsi_lun,
+						  &tmf_task);
+		switch (rc) {
+		/* The task is still in Lun, release it then */
+		case TMF_RESP_FUNC_SUCC:
+		/* The task is not in Lun or failed, reset the phy */
+		case TMF_RESP_FUNC_FAILED:
+		case TMF_RESP_FUNC_COMPLETE:
+			break;
+		}
+	}
+	return rc;
+}
+
+static void hisi_sas_port_formed(struct asd_sas_phy *sas_phy)
+{
+	hisi_sas_port_notify_formed(sas_phy);
+}
+
+static void hisi_sas_port_deformed(struct asd_sas_phy *sas_phy)
+{
+	hisi_sas_port_notify_deformed(sas_phy);
+}
+
+static void hisi_sas_phy_disconnected(struct hisi_sas_phy *phy)
+{
+	phy->phy_attached = 0;
+	phy->phy_type = 0;
+	phy->port = NULL;
+}
+
+void hisi_sas_phy_down(struct hisi_hba *hisi_hba, int phy_no, int rdy)
+{
+	struct hisi_sas_phy *phy = &hisi_hba->phy[phy_no];
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	struct sas_ha_struct *sas_ha = &hisi_hba->sha;
+
+	if (rdy) {
+		/* Phy down but ready */
+		hisi_sas_bytes_dmaed(hisi_hba, phy_no);
+		hisi_sas_port_notify_formed(sas_phy);
+	} else {
+		struct hisi_sas_port *port  = phy->port;
+
+		/* Phy down and not ready */
+		sas_ha->notify_phy_event(sas_phy, PHYE_LOSS_OF_SIGNAL);
+		sas_phy_disconnected(sas_phy);
+
+		if (port) {
+			if (phy->phy_type & PORT_TYPE_SAS) {
+				int port_id = port->id;
+
+				if (!hisi_hba->hw->get_wideport_bitmap(hisi_hba,
+								       port_id))
+					port->port_attached = 0;
+			} else if (phy->phy_type & PORT_TYPE_SATA)
+				port->port_attached = 0;
+		}
+		hisi_sas_phy_disconnected(phy);
+	}
+}
+EXPORT_SYMBOL_GPL(hisi_sas_phy_down);
+
+static struct scsi_transport_template *hisi_sas_stt;
+
+static struct scsi_host_template hisi_sas_sht = {
+	.module			= THIS_MODULE,
+	.name			= DRV_NAME,
+	.queuecommand		= sas_queuecommand,
+	.target_alloc		= sas_target_alloc,
+	.slave_configure	= sas_slave_configure,
+	.scan_finished		= hisi_sas_scan_finished,
+	.scan_start		= hisi_sas_scan_start,
+	.change_queue_depth	= sas_change_queue_depth,
+	.bios_param		= sas_bios_param,
+	.can_queue		= 1,
+	.this_id		= -1,
+	.sg_tablesize		= SG_ALL,
+	.max_sectors		= SCSI_DEFAULT_MAX_SECTORS,
+	.use_clustering		= ENABLE_CLUSTERING,
+	.eh_device_reset_handler = sas_eh_device_reset_handler,
+	.eh_bus_reset_handler	= sas_eh_bus_reset_handler,
+	.target_destroy		= sas_target_destroy,
+	.ioctl			= sas_ioctl,
+};
+
+static struct sas_domain_function_template hisi_sas_transport_ops = {
+	.lldd_dev_found		= hisi_sas_dev_found,
+	.lldd_dev_gone		= hisi_sas_dev_gone,
+	.lldd_execute_task	= hisi_sas_queue_command,
+	.lldd_control_phy	= hisi_sas_control_phy,
+	.lldd_abort_task	= hisi_sas_abort_task,
+	.lldd_abort_task_set	= hisi_sas_abort_task_set,
+	.lldd_clear_aca		= hisi_sas_clear_aca,
+	.lldd_I_T_nexus_reset	= hisi_sas_I_T_nexus_reset,
+	.lldd_lu_reset		= hisi_sas_lu_reset,
+	.lldd_query_task	= hisi_sas_query_task,
+	.lldd_port_formed	= hisi_sas_port_formed,
+	.lldd_port_deformed	= hisi_sas_port_deformed,
+};
+
+static int hisi_sas_alloc(struct hisi_hba *hisi_hba, struct Scsi_Host *shost)
+{
+	int i, s;
+	struct platform_device *pdev = hisi_hba->pdev;
+	struct device *dev = &pdev->dev;
+
+	spin_lock_init(&hisi_hba->lock);
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		hisi_sas_phy_init(hisi_hba, i);
+		hisi_hba->port[i].port_attached = 0;
+		hisi_hba->port[i].id = -1;
+		INIT_LIST_HEAD(&hisi_hba->port[i].list);
+	}
+
+	for (i = 0; i < HISI_SAS_MAX_DEVICES; i++) {
+		hisi_hba->devices[i].dev_type = SAS_PHY_UNUSED;
+		hisi_hba->devices[i].device_id = i;
+		hisi_hba->devices[i].dev_status = HISI_SAS_DEV_NORMAL;
+	}
+
+	for (i = 0; i < hisi_hba->queue_count; i++) {
+		struct hisi_sas_cq *cq = &hisi_hba->cq[i];
+
+		/* Completion queue structure */
+		cq->id = i;
+		cq->hisi_hba = hisi_hba;
+
+		/* Delivery queue */
+		s = sizeof(struct hisi_sas_cmd_hdr) * HISI_SAS_QUEUE_SLOTS;
+		hisi_hba->cmd_hdr[i] = dma_alloc_coherent(dev, s,
+					&hisi_hba->cmd_hdr_dma[i], GFP_KERNEL);
+		if (!hisi_hba->cmd_hdr[i])
+			goto err_out;
+		memset(hisi_hba->cmd_hdr[i], 0, s);
+
+		/* Completion queue */
+		s = hisi_hba->hw->complete_hdr_size * HISI_SAS_QUEUE_SLOTS;
+		hisi_hba->complete_hdr[i] = dma_alloc_coherent(dev, s,
+				&hisi_hba->complete_hdr_dma[i], GFP_KERNEL);
+		if (!hisi_hba->complete_hdr[i])
+			goto err_out;
+		memset(hisi_hba->complete_hdr[i], 0, s);
+	}
+
+	s = HISI_SAS_STATUS_BUF_SZ;
+	hisi_hba->status_buffer_pool = dma_pool_create("status_buffer",
+						       dev, s, 16, 0);
+	if (!hisi_hba->status_buffer_pool)
+		goto err_out;
+
+	s = HISI_SAS_COMMAND_TABLE_SZ;
+	hisi_hba->command_table_pool = dma_pool_create("command_table",
+						       dev, s, 16, 0);
+	if (!hisi_hba->command_table_pool)
+		goto err_out;
+
+	s = HISI_SAS_MAX_ITCT_ENTRIES * sizeof(struct hisi_sas_itct);
+	hisi_hba->itct = dma_alloc_coherent(dev, s, &hisi_hba->itct_dma,
+					    GFP_KERNEL);
+	if (!hisi_hba->itct)
+		goto err_out;
+
+	memset(hisi_hba->itct, 0, s);
+
+	hisi_hba->slot_info = devm_kcalloc(dev, HISI_SAS_COMMAND_ENTRIES,
+					   sizeof(struct hisi_sas_slot),
+					   GFP_KERNEL);
+	if (!hisi_hba->slot_info)
+		goto err_out;
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_iost);
+	hisi_hba->iost = dma_alloc_coherent(dev, s, &hisi_hba->iost_dma,
+					    GFP_KERNEL);
+	if (!hisi_hba->iost)
+		goto err_out;
+
+	memset(hisi_hba->iost, 0, s);
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_breakpoint);
+	hisi_hba->breakpoint = dma_alloc_coherent(dev, s,
+				&hisi_hba->breakpoint_dma, GFP_KERNEL);
+	if (!hisi_hba->breakpoint)
+		goto err_out;
+
+	memset(hisi_hba->breakpoint, 0, s);
+
+	hisi_hba->slot_index_count = HISI_SAS_COMMAND_ENTRIES;
+	s = hisi_hba->slot_index_count / sizeof(unsigned long);
+	hisi_hba->slot_index_tags = devm_kzalloc(dev, s, GFP_KERNEL);
+	if (!hisi_hba->slot_index_tags)
+		goto err_out;
+
+	hisi_hba->sge_page_pool = dma_pool_create("status_sge", dev,
+				sizeof(struct hisi_sas_sge_page), 16, 0);
+	if (!hisi_hba->sge_page_pool)
+		goto err_out;
+
+	s = sizeof(struct hisi_sas_initial_fis) * HISI_SAS_MAX_PHYS;
+	hisi_hba->initial_fis = dma_alloc_coherent(dev, s,
+				&hisi_hba->initial_fis_dma, GFP_KERNEL);
+	if (!hisi_hba->initial_fis)
+		goto err_out;
+	memset(hisi_hba->initial_fis, 0, s);
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_breakpoint) * 2;
+	hisi_hba->sata_breakpoint = dma_alloc_coherent(dev, s,
+				&hisi_hba->sata_breakpoint_dma, GFP_KERNEL);
+	if (!hisi_hba->sata_breakpoint)
+		goto err_out;
+	memset(hisi_hba->sata_breakpoint, 0, s);
+
+	hisi_sas_slot_index_init(hisi_hba);
+
+	hisi_hba->wq = create_singlethread_workqueue(dev_name(dev));
+	if (!hisi_hba->wq) {
+		dev_err(dev, "sas_alloc: failed to create workqueue\n");
+		goto err_out;
+	}
+
+	return 0;
+err_out:
+	return -ENOMEM;
+}
+
+static void hisi_sas_free(struct hisi_hba *hisi_hba)
+{
+	struct device *dev = &hisi_hba->pdev->dev;
+	int i, s;
+
+	for (i = 0; i < hisi_hba->queue_count; i++) {
+		s = sizeof(struct hisi_sas_cmd_hdr) * HISI_SAS_QUEUE_SLOTS;
+		if (hisi_hba->cmd_hdr[i])
+			dma_free_coherent(dev, s,
+					  hisi_hba->cmd_hdr[i],
+					  hisi_hba->cmd_hdr_dma[i]);
+
+		s = hisi_hba->hw->complete_hdr_size * HISI_SAS_QUEUE_SLOTS;
+		if (hisi_hba->complete_hdr[i])
+			dma_free_coherent(dev, s,
+					  hisi_hba->complete_hdr[i],
+					  hisi_hba->complete_hdr_dma[i]);
+	}
+
+	dma_pool_destroy(hisi_hba->status_buffer_pool);
+	dma_pool_destroy(hisi_hba->command_table_pool);
+	dma_pool_destroy(hisi_hba->sge_page_pool);
+
+	s = HISI_SAS_MAX_ITCT_ENTRIES * sizeof(struct hisi_sas_itct);
+	if (hisi_hba->itct)
+		dma_free_coherent(dev, s,
+				  hisi_hba->itct, hisi_hba->itct_dma);
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_iost);
+	if (hisi_hba->iost)
+		dma_free_coherent(dev, s,
+				  hisi_hba->iost, hisi_hba->iost_dma);
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_breakpoint);
+	if (hisi_hba->breakpoint)
+		dma_free_coherent(dev, s,
+				  hisi_hba->breakpoint,
+				  hisi_hba->breakpoint_dma);
+
+
+	s = sizeof(struct hisi_sas_initial_fis) * HISI_SAS_MAX_PHYS;
+	if (hisi_hba->initial_fis)
+		dma_free_coherent(dev, s,
+				  hisi_hba->initial_fis,
+				  hisi_hba->initial_fis_dma);
+
+	s = HISI_SAS_COMMAND_ENTRIES * sizeof(struct hisi_sas_breakpoint) * 2;
+	if (hisi_hba->sata_breakpoint)
+		dma_free_coherent(dev, s,
+				  hisi_hba->sata_breakpoint,
+				  hisi_hba->sata_breakpoint_dma);
+
+	if (hisi_hba->wq)
+		destroy_workqueue(hisi_hba->wq);
+}
+
+static struct Scsi_Host *hisi_sas_shost_alloc(struct platform_device *pdev,
+					      const struct hisi_sas_hw *hw)
+{
+	struct resource *res;
+	struct Scsi_Host *shost;
+	struct hisi_hba *hisi_hba;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = pdev->dev.of_node;
+	struct property *sas_addr_prop;
+
+	shost = scsi_host_alloc(&hisi_sas_sht, sizeof(*hisi_hba));
+	if (!shost)
+		goto err_out;
+	hisi_hba = shost_priv(shost);
+
+	hisi_hba->hw = hw;
+	hisi_hba->pdev = pdev;
+	hisi_hba->shost = shost;
+	SHOST_TO_SAS_HA(shost) = &hisi_hba->sha;
+
+	init_timer(&hisi_hba->timer);
+
+	sas_addr_prop = of_find_property(np, "sas-addr", NULL);
+	if (!sas_addr_prop || (sas_addr_prop->length != SAS_ADDR_SIZE))
+		goto err_out;
+	memcpy(hisi_hba->sas_addr, sas_addr_prop->value, SAS_ADDR_SIZE);
+
+	if (of_property_read_u32(np, "ctrl-reset-reg",
+				 &hisi_hba->ctrl_reset_reg))
+		goto err_out;
+
+	if (of_property_read_u32(np, "ctrl-reset-sts-reg",
+				 &hisi_hba->ctrl_reset_sts_reg))
+		goto err_out;
+
+	if (of_property_read_u32(np, "ctrl-clock-ena-reg",
+				 &hisi_hba->ctrl_clock_ena_reg))
+		goto err_out;
+
+	if (of_property_read_u32(np, "phy-count", &hisi_hba->n_phy))
+		goto err_out;
+
+	if (of_property_read_u32(np, "queue-count", &hisi_hba->queue_count))
+		goto err_out;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	hisi_hba->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(hisi_hba->regs))
+		goto err_out;
+
+	hisi_hba->ctrl = syscon_regmap_lookup_by_phandle(
+				np, "hisilicon,sas-syscon");
+	if (IS_ERR(hisi_hba->ctrl))
+		goto err_out;
+
+	if (hisi_sas_alloc(hisi_hba, shost)) {
+		hisi_sas_free(hisi_hba);
+		goto err_out;
+	}
+
+	return shost;
+err_out:
+	dev_err(dev, "shost alloc failed\n");
+	return NULL;
+}
+
+static void hisi_sas_init_add(struct hisi_hba *hisi_hba)
+{
+	int i;
+
+	for (i = 0; i < hisi_hba->n_phy; i++)
+		memcpy(&hisi_hba->phy[i].dev_sas_addr,
+		       hisi_hba->sas_addr,
+		       SAS_ADDR_SIZE);
+}
+
+int hisi_sas_probe(struct platform_device *pdev,
+			 const struct hisi_sas_hw *hw)
+{
+	struct Scsi_Host *shost;
+	struct hisi_hba *hisi_hba;
+	struct device *dev = &pdev->dev;
+	struct asd_sas_phy **arr_phy;
+	struct asd_sas_port **arr_port;
+	struct sas_ha_struct *sha;
+	int rc, phy_nr, port_nr, i;
+
+	shost = hisi_sas_shost_alloc(pdev, hw);
+	if (!shost) {
+		rc = -ENOMEM;
+		goto err_out_ha;
+	}
+
+	sha = SHOST_TO_SAS_HA(shost);
+	hisi_hba = shost_priv(shost);
+	platform_set_drvdata(pdev, sha);
+
+	if (dma_set_mask_and_coherent(dev, DMA_BIT_MASK(64)) &&
+	    dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32))) {
+		dev_err(dev, "No usable DMA addressing method\n");
+		rc = -EIO;
+		goto err_out_ha;
+	}
+
+	phy_nr = port_nr = hisi_hba->n_phy;
+
+	arr_phy = devm_kcalloc(dev, phy_nr, sizeof(void *), GFP_KERNEL);
+	arr_port = devm_kcalloc(dev, port_nr, sizeof(void *), GFP_KERNEL);
+	if (!arr_phy || !arr_port)
+		return -ENOMEM;
+
+	sha->sas_phy = arr_phy;
+	sha->sas_port = arr_port;
+	sha->core.shost = shost;
+	sha->lldd_ha = hisi_hba;
+
+	shost->transportt = hisi_sas_stt;
+	shost->max_id = HISI_SAS_MAX_DEVICES;
+	shost->max_lun = ~0;
+	shost->max_channel = 1;
+	shost->max_cmd_len = 16;
+	shost->sg_tablesize = min_t(u16, SG_ALL, HISI_SAS_SGE_PAGE_CNT);
+	shost->can_queue = HISI_SAS_COMMAND_ENTRIES;
+	shost->cmd_per_lun = HISI_SAS_COMMAND_ENTRIES;
+
+	sha->sas_ha_name = DRV_NAME;
+	sha->dev = &hisi_hba->pdev->dev;
+	sha->lldd_module = THIS_MODULE;
+	sha->sas_addr = &hisi_hba->sas_addr[0];
+	sha->num_phys = hisi_hba->n_phy;
+	sha->core.shost = hisi_hba->shost;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		sha->sas_phy[i] = &hisi_hba->phy[i].sas_phy;
+		sha->sas_port[i] = &hisi_hba->port[i].sas_port;
+	}
+
+	hisi_sas_init_add(hisi_hba);
+
+	rc = hisi_hba->hw->hw_init(hisi_hba);
+	if (rc)
+		goto err_out_ha;
+
+	rc = scsi_add_host(shost, &pdev->dev);
+	if (rc)
+		goto err_out_ha;
+
+	rc = sas_register_ha(sha);
+	if (rc)
+		goto err_out_register_ha;
+
+	scsi_scan_host(shost);
+
+	return 0;
+
+err_out_register_ha:
+	scsi_remove_host(shost);
+err_out_ha:
+	kfree(shost);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(hisi_sas_probe);
+
+int hisi_sas_remove(struct platform_device *pdev)
+{
+	struct sas_ha_struct *sha = platform_get_drvdata(pdev);
+	struct hisi_hba *hisi_hba = sha->lldd_ha;
+
+	scsi_remove_host(sha->core.shost);
+	sas_unregister_ha(sha);
+	sas_remove_host(sha->core.shost);
+
+	hisi_sas_free(hisi_hba);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(hisi_sas_remove);
+
+static __init int hisi_sas_init(void)
+{
+	pr_info("hisi_sas: driver version %s\n", DRV_VERSION);
+
+	hisi_sas_stt = sas_domain_attach_transport(&hisi_sas_transport_ops);
+	if (!hisi_sas_stt)
+		return -ENOMEM;
+
+	return 0;
+}
+
+static __exit void hisi_sas_exit(void)
+{
+	sas_release_transport(hisi_sas_stt);
+}
+
+module_init(hisi_sas_init);
+module_exit(hisi_sas_exit);
+
+MODULE_VERSION(DRV_VERSION);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("John Garry <john.garry@huawei.com>");
+MODULE_DESCRIPTION("HISILICON SAS controller driver");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/scsi/hisi_sas/hisi_sas_v1_hw.c b/drivers/scsi/hisi_sas/hisi_sas_v1_hw.c
new file mode 100644
index 000000000000..eea24d7531cf
--- /dev/null
+++ b/drivers/scsi/hisi_sas/hisi_sas_v1_hw.c
@@ -0,0 +1,1832 @@
+/*
+ * Copyright (c) 2015 Linaro Ltd.
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the 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 "hisi_sas.h"
+#define DRV_NAME "hisi_sas_v1_hw"
+
+/* global registers need init*/
+#define DLVRY_QUEUE_ENABLE		0x0
+#define IOST_BASE_ADDR_LO		0x8
+#define IOST_BASE_ADDR_HI		0xc
+#define ITCT_BASE_ADDR_LO		0x10
+#define ITCT_BASE_ADDR_HI		0x14
+#define BROKEN_MSG_ADDR_LO		0x18
+#define BROKEN_MSG_ADDR_HI		0x1c
+#define PHY_CONTEXT			0x20
+#define PHY_STATE			0x24
+#define PHY_PORT_NUM_MA			0x28
+#define PORT_STATE			0x2c
+#define PHY_CONN_RATE			0x30
+#define HGC_TRANS_TASK_CNT_LIMIT	0x38
+#define AXI_AHB_CLK_CFG			0x3c
+#define HGC_SAS_TXFAIL_RETRY_CTRL	0x84
+#define HGC_GET_ITV_TIME		0x90
+#define DEVICE_MSG_WORK_MODE		0x94
+#define I_T_NEXUS_LOSS_TIME		0xa0
+#define BUS_INACTIVE_LIMIT_TIME		0xa8
+#define REJECT_TO_OPEN_LIMIT_TIME	0xac
+#define CFG_AGING_TIME			0xbc
+#define CFG_AGING_TIME_ITCT_REL_OFF	0
+#define CFG_AGING_TIME_ITCT_REL_MSK	(0x1 << CFG_AGING_TIME_ITCT_REL_OFF)
+#define HGC_DFX_CFG2			0xc0
+#define FIS_LIST_BADDR_L		0xc4
+#define CFG_1US_TIMER_TRSH		0xcc
+#define CFG_SAS_CONFIG			0xd4
+#define HGC_IOST_ECC_ADDR		0x140
+#define HGC_IOST_ECC_ADDR_BAD_OFF	16
+#define HGC_IOST_ECC_ADDR_BAD_MSK	(0x3ff << HGC_IOST_ECC_ADDR_BAD_OFF)
+#define HGC_DQ_ECC_ADDR			0x144
+#define HGC_DQ_ECC_ADDR_BAD_OFF		16
+#define HGC_DQ_ECC_ADDR_BAD_MSK		(0xfff << HGC_DQ_ECC_ADDR_BAD_OFF)
+#define HGC_INVLD_DQE_INFO		0x148
+#define HGC_INVLD_DQE_INFO_DQ_OFF	0
+#define HGC_INVLD_DQE_INFO_DQ_MSK	(0xffff << HGC_INVLD_DQE_INFO_DQ_OFF)
+#define HGC_INVLD_DQE_INFO_TYPE_OFF	16
+#define HGC_INVLD_DQE_INFO_TYPE_MSK	(0x1 << HGC_INVLD_DQE_INFO_TYPE_OFF)
+#define HGC_INVLD_DQE_INFO_FORCE_OFF	17
+#define HGC_INVLD_DQE_INFO_FORCE_MSK	(0x1 << HGC_INVLD_DQE_INFO_FORCE_OFF)
+#define HGC_INVLD_DQE_INFO_PHY_OFF	18
+#define HGC_INVLD_DQE_INFO_PHY_MSK	(0x1 << HGC_INVLD_DQE_INFO_PHY_OFF)
+#define HGC_INVLD_DQE_INFO_ABORT_OFF	19
+#define HGC_INVLD_DQE_INFO_ABORT_MSK	(0x1 << HGC_INVLD_DQE_INFO_ABORT_OFF)
+#define HGC_INVLD_DQE_INFO_IPTT_OF_OFF	20
+#define HGC_INVLD_DQE_INFO_IPTT_OF_MSK	(0x1 << HGC_INVLD_DQE_INFO_IPTT_OF_OFF)
+#define HGC_INVLD_DQE_INFO_SSP_ERR_OFF	21
+#define HGC_INVLD_DQE_INFO_SSP_ERR_MSK	(0x1 << HGC_INVLD_DQE_INFO_SSP_ERR_OFF)
+#define HGC_INVLD_DQE_INFO_OFL_OFF	22
+#define HGC_INVLD_DQE_INFO_OFL_MSK	(0x1 << HGC_INVLD_DQE_INFO_OFL_OFF)
+#define HGC_ITCT_ECC_ADDR		0x150
+#define HGC_ITCT_ECC_ADDR_BAD_OFF	16
+#define HGC_ITCT_ECC_ADDR_BAD_MSK	(0x3ff << HGC_ITCT_ECC_ADDR_BAD_OFF)
+#define HGC_AXI_FIFO_ERR_INFO		0x154
+#define INT_COAL_EN			0x1bc
+#define OQ_INT_COAL_TIME		0x1c0
+#define OQ_INT_COAL_CNT			0x1c4
+#define ENT_INT_COAL_TIME		0x1c8
+#define ENT_INT_COAL_CNT		0x1cc
+#define OQ_INT_SRC			0x1d0
+#define OQ_INT_SRC_MSK			0x1d4
+#define ENT_INT_SRC1			0x1d8
+#define ENT_INT_SRC2			0x1dc
+#define ENT_INT_SRC2_DQ_CFG_ERR_OFF	25
+#define ENT_INT_SRC2_DQ_CFG_ERR_MSK	(0x1 << ENT_INT_SRC2_DQ_CFG_ERR_OFF)
+#define ENT_INT_SRC2_CQ_CFG_ERR_OFF	27
+#define ENT_INT_SRC2_CQ_CFG_ERR_MSK	(0x1 << ENT_INT_SRC2_CQ_CFG_ERR_OFF)
+#define ENT_INT_SRC2_AXI_WRONG_INT_OFF	28
+#define ENT_INT_SRC2_AXI_WRONG_INT_MSK	(0x1 << ENT_INT_SRC2_AXI_WRONG_INT_OFF)
+#define ENT_INT_SRC2_AXI_OVERLF_INT_OFF	29
+#define ENT_INT_SRC2_AXI_OVERLF_INT_MSK	(0x1 << ENT_INT_SRC2_AXI_OVERLF_INT_OFF)
+#define ENT_INT_SRC_MSK1		0x1e0
+#define ENT_INT_SRC_MSK2		0x1e4
+#define SAS_ECC_INTR			0x1e8
+#define SAS_ECC_INTR_DQ_ECC1B_OFF	0
+#define SAS_ECC_INTR_DQ_ECC1B_MSK	(0x1 << SAS_ECC_INTR_DQ_ECC1B_OFF)
+#define SAS_ECC_INTR_DQ_ECCBAD_OFF	1
+#define SAS_ECC_INTR_DQ_ECCBAD_MSK	(0x1 << SAS_ECC_INTR_DQ_ECCBAD_OFF)
+#define SAS_ECC_INTR_IOST_ECC1B_OFF	2
+#define SAS_ECC_INTR_IOST_ECC1B_MSK	(0x1 << SAS_ECC_INTR_IOST_ECC1B_OFF)
+#define SAS_ECC_INTR_IOST_ECCBAD_OFF	3
+#define SAS_ECC_INTR_IOST_ECCBAD_MSK	(0x1 << SAS_ECC_INTR_IOST_ECCBAD_OFF)
+#define SAS_ECC_INTR_ITCT_ECC1B_OFF	4
+#define SAS_ECC_INTR_ITCT_ECC1B_MSK	(0x1 << SAS_ECC_INTR_ITCT_ECC1B_OFF)
+#define SAS_ECC_INTR_ITCT_ECCBAD_OFF	5
+#define SAS_ECC_INTR_ITCT_ECCBAD_MSK	(0x1 << SAS_ECC_INTR_ITCT_ECCBAD_OFF)
+#define SAS_ECC_INTR_MSK		0x1ec
+#define HGC_ERR_STAT_EN			0x238
+#define DLVRY_Q_0_BASE_ADDR_LO		0x260
+#define DLVRY_Q_0_BASE_ADDR_HI		0x264
+#define DLVRY_Q_0_DEPTH			0x268
+#define DLVRY_Q_0_WR_PTR		0x26c
+#define DLVRY_Q_0_RD_PTR		0x270
+#define COMPL_Q_0_BASE_ADDR_LO		0x4e0
+#define COMPL_Q_0_BASE_ADDR_HI		0x4e4
+#define COMPL_Q_0_DEPTH			0x4e8
+#define COMPL_Q_0_WR_PTR		0x4ec
+#define COMPL_Q_0_RD_PTR		0x4f0
+#define HGC_ECC_ERR			0x7d0
+
+/* phy registers need init */
+#define PORT_BASE			(0x800)
+
+#define PHY_CFG				(PORT_BASE + 0x0)
+#define PHY_CFG_ENA_OFF			0
+#define PHY_CFG_ENA_MSK			(0x1 << PHY_CFG_ENA_OFF)
+#define PHY_CFG_DC_OPT_OFF		2
+#define PHY_CFG_DC_OPT_MSK		(0x1 << PHY_CFG_DC_OPT_OFF)
+#define PROG_PHY_LINK_RATE		(PORT_BASE + 0xc)
+#define PROG_PHY_LINK_RATE_MAX_OFF	0
+#define PROG_PHY_LINK_RATE_MAX_MSK	(0xf << PROG_PHY_LINK_RATE_MAX_OFF)
+#define PROG_PHY_LINK_RATE_MIN_OFF	4
+#define PROG_PHY_LINK_RATE_MIN_MSK	(0xf << PROG_PHY_LINK_RATE_MIN_OFF)
+#define PROG_PHY_LINK_RATE_OOB_OFF	8
+#define PROG_PHY_LINK_RATE_OOB_MSK	(0xf << PROG_PHY_LINK_RATE_OOB_OFF)
+#define PHY_CTRL			(PORT_BASE + 0x14)
+#define PHY_CTRL_RESET_OFF		0
+#define PHY_CTRL_RESET_MSK		(0x1 << PHY_CTRL_RESET_OFF)
+#define PHY_RATE_NEGO			(PORT_BASE + 0x30)
+#define PHY_PCN				(PORT_BASE + 0x44)
+#define SL_TOUT_CFG			(PORT_BASE + 0x8c)
+#define SL_CONTROL			(PORT_BASE + 0x94)
+#define SL_CONTROL_NOTIFY_EN_OFF	0
+#define SL_CONTROL_NOTIFY_EN_MSK	(0x1 << SL_CONTROL_NOTIFY_EN_OFF)
+#define TX_ID_DWORD0			(PORT_BASE + 0x9c)
+#define TX_ID_DWORD1			(PORT_BASE + 0xa0)
+#define TX_ID_DWORD2			(PORT_BASE + 0xa4)
+#define TX_ID_DWORD3			(PORT_BASE + 0xa8)
+#define TX_ID_DWORD4			(PORT_BASE + 0xaC)
+#define TX_ID_DWORD5			(PORT_BASE + 0xb0)
+#define TX_ID_DWORD6			(PORT_BASE + 0xb4)
+#define RX_IDAF_DWORD0			(PORT_BASE + 0xc4)
+#define RX_IDAF_DWORD1			(PORT_BASE + 0xc8)
+#define RX_IDAF_DWORD2			(PORT_BASE + 0xcc)
+#define RX_IDAF_DWORD3			(PORT_BASE + 0xd0)
+#define RX_IDAF_DWORD4			(PORT_BASE + 0xd4)
+#define RX_IDAF_DWORD5			(PORT_BASE + 0xd8)
+#define RX_IDAF_DWORD6			(PORT_BASE + 0xdc)
+#define RXOP_CHECK_CFG_H		(PORT_BASE + 0xfc)
+#define DONE_RECEIVED_TIME		(PORT_BASE + 0x12c)
+#define CON_CFG_DRIVER			(PORT_BASE + 0x130)
+#define PHY_CONFIG2			(PORT_BASE + 0x1a8)
+#define PHY_CONFIG2_FORCE_TXDEEMPH_OFF	3
+#define PHY_CONFIG2_FORCE_TXDEEMPH_MSK	(0x1 << PHY_CONFIG2_FORCE_TXDEEMPH_OFF)
+#define PHY_CONFIG2_TX_TRAIN_COMP_OFF	24
+#define PHY_CONFIG2_TX_TRAIN_COMP_MSK	(0x1 << PHY_CONFIG2_TX_TRAIN_COMP_OFF)
+#define CHL_INT0			(PORT_BASE + 0x1b0)
+#define CHL_INT0_PHYCTRL_NOTRDY_OFF	0
+#define CHL_INT0_PHYCTRL_NOTRDY_MSK	(0x1 << CHL_INT0_PHYCTRL_NOTRDY_OFF)
+#define CHL_INT0_SN_FAIL_NGR_OFF	2
+#define CHL_INT0_SN_FAIL_NGR_MSK	(0x1 << CHL_INT0_SN_FAIL_NGR_OFF)
+#define CHL_INT0_DWS_LOST_OFF		4
+#define CHL_INT0_DWS_LOST_MSK		(0x1 << CHL_INT0_DWS_LOST_OFF)
+#define CHL_INT0_SL_IDAF_FAIL_OFF	10
+#define CHL_INT0_SL_IDAF_FAIL_MSK	(0x1 << CHL_INT0_SL_IDAF_FAIL_OFF)
+#define CHL_INT0_ID_TIMEOUT_OFF		11
+#define CHL_INT0_ID_TIMEOUT_MSK		(0x1 << CHL_INT0_ID_TIMEOUT_OFF)
+#define CHL_INT0_SL_OPAF_FAIL_OFF	12
+#define CHL_INT0_SL_OPAF_FAIL_MSK	(0x1 << CHL_INT0_SL_OPAF_FAIL_OFF)
+#define CHL_INT0_SL_PS_FAIL_OFF		21
+#define CHL_INT0_SL_PS_FAIL_MSK		(0x1 << CHL_INT0_SL_PS_FAIL_OFF)
+#define CHL_INT1			(PORT_BASE + 0x1b4)
+#define CHL_INT2			(PORT_BASE + 0x1b8)
+#define CHL_INT2_SL_RX_BC_ACK_OFF	2
+#define CHL_INT2_SL_RX_BC_ACK_MSK	(0x1 << CHL_INT2_SL_RX_BC_ACK_OFF)
+#define CHL_INT2_SL_PHY_ENA_OFF		6
+#define CHL_INT2_SL_PHY_ENA_MSK		(0x1 << CHL_INT2_SL_PHY_ENA_OFF)
+#define CHL_INT0_MSK			(PORT_BASE + 0x1bc)
+#define CHL_INT0_MSK_PHYCTRL_NOTRDY_OFF	0
+#define CHL_INT0_MSK_PHYCTRL_NOTRDY_MSK	(0x1 << CHL_INT0_MSK_PHYCTRL_NOTRDY_OFF)
+#define CHL_INT1_MSK			(PORT_BASE + 0x1c0)
+#define CHL_INT2_MSK			(PORT_BASE + 0x1c4)
+#define CHL_INT_COAL_EN			(PORT_BASE + 0x1d0)
+#define DMA_TX_STATUS			(PORT_BASE + 0x2d0)
+#define DMA_TX_STATUS_BUSY_OFF		0
+#define DMA_TX_STATUS_BUSY_MSK		(0x1 << DMA_TX_STATUS_BUSY_OFF)
+#define DMA_RX_STATUS			(PORT_BASE + 0x2e8)
+#define DMA_RX_STATUS_BUSY_OFF		0
+#define DMA_RX_STATUS_BUSY_MSK		(0x1 << DMA_RX_STATUS_BUSY_OFF)
+
+#define AXI_CFG				0x5100
+#define RESET_VALUE			0x7ffff
+
+/* HW dma structures */
+/* Delivery queue header */
+/* dw0 */
+#define CMD_HDR_RESP_REPORT_OFF		5
+#define CMD_HDR_RESP_REPORT_MSK		0x20
+#define CMD_HDR_TLR_CTRL_OFF		6
+#define CMD_HDR_TLR_CTRL_MSK		0xc0
+#define CMD_HDR_PORT_OFF		17
+#define CMD_HDR_PORT_MSK		0xe0000
+#define CMD_HDR_PRIORITY_OFF		27
+#define CMD_HDR_PRIORITY_MSK		0x8000000
+#define CMD_HDR_MODE_OFF		28
+#define CMD_HDR_MODE_MSK		0x10000000
+#define CMD_HDR_CMD_OFF			29
+#define CMD_HDR_CMD_MSK			0xe0000000
+/* dw1 */
+#define CMD_HDR_VERIFY_DTL_OFF		10
+#define CMD_HDR_VERIFY_DTL_MSK		0x400
+#define CMD_HDR_SSP_FRAME_TYPE_OFF	13
+#define CMD_HDR_SSP_FRAME_TYPE_MSK	0xe000
+#define CMD_HDR_DEVICE_ID_OFF		16
+#define CMD_HDR_DEVICE_ID_MSK		0xffff0000
+/* dw2 */
+#define CMD_HDR_CFL_OFF			0
+#define CMD_HDR_CFL_MSK			0x1ff
+#define CMD_HDR_MRFL_OFF		15
+#define CMD_HDR_MRFL_MSK		0xff8000
+#define CMD_HDR_FIRST_BURST_OFF		25
+#define CMD_HDR_FIRST_BURST_MSK		0x2000000
+/* dw3 */
+#define CMD_HDR_IPTT_OFF		0
+#define CMD_HDR_IPTT_MSK		0xffff
+/* dw6 */
+#define CMD_HDR_DATA_SGL_LEN_OFF	16
+#define CMD_HDR_DATA_SGL_LEN_MSK	0xffff0000
+
+/* Completion header */
+#define CMPLT_HDR_IPTT_OFF		0
+#define CMPLT_HDR_IPTT_MSK		(0xffff << CMPLT_HDR_IPTT_OFF)
+#define CMPLT_HDR_CMD_CMPLT_OFF		17
+#define CMPLT_HDR_CMD_CMPLT_MSK		(0x1 << CMPLT_HDR_CMD_CMPLT_OFF)
+#define CMPLT_HDR_ERR_RCRD_XFRD_OFF	18
+#define CMPLT_HDR_ERR_RCRD_XFRD_MSK	(0x1 << CMPLT_HDR_ERR_RCRD_XFRD_OFF)
+#define CMPLT_HDR_RSPNS_XFRD_OFF	19
+#define CMPLT_HDR_RSPNS_XFRD_MSK	(0x1 << CMPLT_HDR_RSPNS_XFRD_OFF)
+#define CMPLT_HDR_IO_CFG_ERR_OFF	27
+#define CMPLT_HDR_IO_CFG_ERR_MSK	(0x1 << CMPLT_HDR_IO_CFG_ERR_OFF)
+
+/* ITCT header */
+/* qw0 */
+#define ITCT_HDR_DEV_TYPE_OFF		0
+#define ITCT_HDR_DEV_TYPE_MSK		(0x3ULL << ITCT_HDR_DEV_TYPE_OFF)
+#define ITCT_HDR_VALID_OFF		2
+#define ITCT_HDR_VALID_MSK		(0x1ULL << ITCT_HDR_VALID_OFF)
+#define ITCT_HDR_AWT_CONTROL_OFF	4
+#define ITCT_HDR_AWT_CONTROL_MSK	(0x1ULL << ITCT_HDR_AWT_CONTROL_OFF)
+#define ITCT_HDR_MAX_CONN_RATE_OFF	5
+#define ITCT_HDR_MAX_CONN_RATE_MSK	(0xfULL << ITCT_HDR_MAX_CONN_RATE_OFF)
+#define ITCT_HDR_VALID_LINK_NUM_OFF	9
+#define ITCT_HDR_VALID_LINK_NUM_MSK	(0xfULL << ITCT_HDR_VALID_LINK_NUM_OFF)
+#define ITCT_HDR_PORT_ID_OFF		13
+#define ITCT_HDR_PORT_ID_MSK		(0x7ULL << ITCT_HDR_PORT_ID_OFF)
+#define ITCT_HDR_SMP_TIMEOUT_OFF	16
+#define ITCT_HDR_SMP_TIMEOUT_MSK	(0xffffULL << ITCT_HDR_SMP_TIMEOUT_OFF)
+/* qw1 */
+#define ITCT_HDR_MAX_SAS_ADDR_OFF	0
+#define ITCT_HDR_MAX_SAS_ADDR_MSK	(0xffffffffffffffff << \
+					ITCT_HDR_MAX_SAS_ADDR_OFF)
+/* qw2 */
+#define ITCT_HDR_IT_NEXUS_LOSS_TL_OFF	0
+#define ITCT_HDR_IT_NEXUS_LOSS_TL_MSK	(0xffffULL << \
+					ITCT_HDR_IT_NEXUS_LOSS_TL_OFF)
+#define ITCT_HDR_BUS_INACTIVE_TL_OFF	16
+#define ITCT_HDR_BUS_INACTIVE_TL_MSK	(0xffffULL << \
+					ITCT_HDR_BUS_INACTIVE_TL_OFF)
+#define ITCT_HDR_MAX_CONN_TL_OFF	32
+#define ITCT_HDR_MAX_CONN_TL_MSK	(0xffffULL << \
+					ITCT_HDR_MAX_CONN_TL_OFF)
+#define ITCT_HDR_REJ_OPEN_TL_OFF	48
+#define ITCT_HDR_REJ_OPEN_TL_MSK	(0xffffULL << \
+					ITCT_HDR_REJ_OPEN_TL_OFF)
+
+/* Err record header */
+#define ERR_HDR_DMA_TX_ERR_TYPE_OFF	0
+#define ERR_HDR_DMA_TX_ERR_TYPE_MSK	(0xffff << ERR_HDR_DMA_TX_ERR_TYPE_OFF)
+#define ERR_HDR_DMA_RX_ERR_TYPE_OFF	16
+#define ERR_HDR_DMA_RX_ERR_TYPE_MSK	(0xffff << ERR_HDR_DMA_RX_ERR_TYPE_OFF)
+
+struct hisi_sas_complete_v1_hdr {
+	__le32 data;
+};
+
+enum {
+	HISI_SAS_PHY_BCAST_ACK = 0,
+	HISI_SAS_PHY_SL_PHY_ENABLED,
+	HISI_SAS_PHY_INT_ABNORMAL,
+	HISI_SAS_PHY_INT_NR
+};
+
+enum {
+	DMA_TX_ERR_BASE = 0x0,
+	DMA_RX_ERR_BASE = 0x100,
+	TRANS_TX_FAIL_BASE = 0x200,
+	TRANS_RX_FAIL_BASE = 0x300,
+
+	/* dma tx */
+	DMA_TX_DIF_CRC_ERR = DMA_TX_ERR_BASE, /* 0x0 */
+	DMA_TX_DIF_APP_ERR, /* 0x1 */
+	DMA_TX_DIF_RPP_ERR, /* 0x2 */
+	DMA_TX_AXI_BUS_ERR, /* 0x3 */
+	DMA_TX_DATA_SGL_OVERFLOW_ERR, /* 0x4 */
+	DMA_TX_DIF_SGL_OVERFLOW_ERR, /* 0x5 */
+	DMA_TX_UNEXP_XFER_RDY_ERR, /* 0x6 */
+	DMA_TX_XFER_RDY_OFFSET_ERR, /* 0x7 */
+	DMA_TX_DATA_UNDERFLOW_ERR, /* 0x8 */
+	DMA_TX_XFER_RDY_LENGTH_OVERFLOW_ERR, /* 0x9 */
+
+	/* dma rx */
+	DMA_RX_BUFFER_ECC_ERR = DMA_RX_ERR_BASE, /* 0x100 */
+	DMA_RX_DIF_CRC_ERR, /* 0x101 */
+	DMA_RX_DIF_APP_ERR, /* 0x102 */
+	DMA_RX_DIF_RPP_ERR, /* 0x103 */
+	DMA_RX_RESP_BUFFER_OVERFLOW_ERR, /* 0x104 */
+	DMA_RX_AXI_BUS_ERR, /* 0x105 */
+	DMA_RX_DATA_SGL_OVERFLOW_ERR, /* 0x106 */
+	DMA_RX_DIF_SGL_OVERFLOW_ERR, /* 0x107 */
+	DMA_RX_DATA_OFFSET_ERR, /* 0x108 */
+	DMA_RX_UNEXP_RX_DATA_ERR, /* 0x109 */
+	DMA_RX_DATA_OVERFLOW_ERR, /* 0x10a */
+	DMA_RX_DATA_UNDERFLOW_ERR, /* 0x10b */
+	DMA_RX_UNEXP_RETRANS_RESP_ERR, /* 0x10c */
+
+	/* trans tx */
+	TRANS_TX_RSVD0_ERR = TRANS_TX_FAIL_BASE, /* 0x200 */
+	TRANS_TX_PHY_NOT_ENABLE_ERR, /* 0x201 */
+	TRANS_TX_OPEN_REJCT_WRONG_DEST_ERR, /* 0x202 */
+	TRANS_TX_OPEN_REJCT_ZONE_VIOLATION_ERR, /* 0x203 */
+	TRANS_TX_OPEN_REJCT_BY_OTHER_ERR, /* 0x204 */
+	TRANS_TX_RSVD1_ERR, /* 0x205 */
+	TRANS_TX_OPEN_REJCT_AIP_TIMEOUT_ERR, /* 0x206 */
+	TRANS_TX_OPEN_REJCT_STP_BUSY_ERR, /* 0x207 */
+	TRANS_TX_OPEN_REJCT_PROTOCOL_NOT_SUPPORT_ERR, /* 0x208 */
+	TRANS_TX_OPEN_REJCT_RATE_NOT_SUPPORT_ERR, /* 0x209 */
+	TRANS_TX_OPEN_REJCT_BAD_DEST_ERR, /* 0x20a */
+	TRANS_TX_OPEN_BREAK_RECEIVE_ERR, /* 0x20b */
+	TRANS_TX_LOW_PHY_POWER_ERR, /* 0x20c */
+	TRANS_TX_OPEN_REJCT_PATHWAY_BLOCKED_ERR, /* 0x20d */
+	TRANS_TX_OPEN_TIMEOUT_ERR, /* 0x20e */
+	TRANS_TX_OPEN_REJCT_NO_DEST_ERR, /* 0x20f */
+	TRANS_TX_OPEN_RETRY_ERR, /* 0x210 */
+	TRANS_TX_RSVD2_ERR, /* 0x211 */
+	TRANS_TX_BREAK_TIMEOUT_ERR, /* 0x212 */
+	TRANS_TX_BREAK_REQUEST_ERR, /* 0x213 */
+	TRANS_TX_BREAK_RECEIVE_ERR, /* 0x214 */
+	TRANS_TX_CLOSE_TIMEOUT_ERR, /* 0x215 */
+	TRANS_TX_CLOSE_NORMAL_ERR, /* 0x216 */
+	TRANS_TX_CLOSE_PHYRESET_ERR, /* 0x217 */
+	TRANS_TX_WITH_CLOSE_DWS_TIMEOUT_ERR, /* 0x218 */
+	TRANS_TX_WITH_CLOSE_COMINIT_ERR, /* 0x219 */
+	TRANS_TX_NAK_RECEIVE_ERR, /* 0x21a */
+	TRANS_TX_ACK_NAK_TIMEOUT_ERR, /* 0x21b */
+	TRANS_TX_CREDIT_TIMEOUT_ERR, /* 0x21c */
+	TRANS_TX_IPTT_CONFLICT_ERR, /* 0x21d */
+	TRANS_TX_TXFRM_TYPE_ERR, /* 0x21e */
+	TRANS_TX_TXSMP_LENGTH_ERR, /* 0x21f */
+
+	/* trans rx */
+	TRANS_RX_FRAME_CRC_ERR = TRANS_RX_FAIL_BASE, /* 0x300 */
+	TRANS_RX_FRAME_DONE_ERR, /* 0x301 */
+	TRANS_RX_FRAME_ERRPRM_ERR, /* 0x302 */
+	TRANS_RX_FRAME_NO_CREDIT_ERR, /* 0x303 */
+	TRANS_RX_RSVD0_ERR, /* 0x304 */
+	TRANS_RX_FRAME_OVERRUN_ERR, /* 0x305 */
+	TRANS_RX_FRAME_NO_EOF_ERR, /* 0x306 */
+	TRANS_RX_LINK_BUF_OVERRUN_ERR, /* 0x307 */
+	TRANS_RX_BREAK_TIMEOUT_ERR, /* 0x308 */
+	TRANS_RX_BREAK_REQUEST_ERR, /* 0x309 */
+	TRANS_RX_BREAK_RECEIVE_ERR, /* 0x30a */
+	TRANS_RX_CLOSE_TIMEOUT_ERR, /* 0x30b */
+	TRANS_RX_CLOSE_NORMAL_ERR, /* 0x30c */
+	TRANS_RX_CLOSE_PHYRESET_ERR, /* 0x30d */
+	TRANS_RX_WITH_CLOSE_DWS_TIMEOUT_ERR, /* 0x30e */
+	TRANS_RX_WITH_CLOSE_COMINIT_ERR, /* 0x30f */
+	TRANS_RX_DATA_LENGTH0_ERR, /* 0x310 */
+	TRANS_RX_BAD_HASH_ERR, /* 0x311 */
+	TRANS_RX_XRDY_ZERO_ERR, /* 0x312 */
+	TRANS_RX_SSP_FRAME_LEN_ERR, /* 0x313 */
+	TRANS_RX_TRANS_RX_RSVD1_ERR, /* 0x314 */
+	TRANS_RX_NO_BALANCE_ERR, /* 0x315 */
+	TRANS_RX_TRANS_RX_RSVD2_ERR, /* 0x316 */
+	TRANS_RX_TRANS_RX_RSVD3_ERR, /* 0x317 */
+	TRANS_RX_BAD_FRAME_TYPE_ERR, /* 0x318 */
+	TRANS_RX_SMP_FRAME_LEN_ERR, /* 0x319 */
+	TRANS_RX_SMP_RESP_TIMEOUT_ERR, /* 0x31a */
+};
+
+#define HISI_SAS_PHY_MAX_INT_NR (HISI_SAS_PHY_INT_NR * HISI_SAS_MAX_PHYS)
+#define HISI_SAS_CQ_MAX_INT_NR (HISI_SAS_MAX_QUEUES)
+#define HISI_SAS_FATAL_INT_NR (2)
+
+#define HISI_SAS_MAX_INT_NR \
+	(HISI_SAS_PHY_MAX_INT_NR + HISI_SAS_CQ_MAX_INT_NR +\
+	HISI_SAS_FATAL_INT_NR)
+
+static u32 hisi_sas_read32(struct hisi_hba *hisi_hba, u32 off)
+{
+	void __iomem *regs = hisi_hba->regs + off;
+
+	return readl(regs);
+}
+
+static u32 hisi_sas_read32_relaxed(struct hisi_hba *hisi_hba, u32 off)
+{
+	void __iomem *regs = hisi_hba->regs + off;
+
+	return readl_relaxed(regs);
+}
+
+static void hisi_sas_write32(struct hisi_hba *hisi_hba,
+				    u32 off, u32 val)
+{
+	void __iomem *regs = hisi_hba->regs + off;
+
+	writel(val, regs);
+}
+
+static void hisi_sas_phy_write32(struct hisi_hba *hisi_hba,
+					int phy_no, u32 off, u32 val)
+{
+	void __iomem *regs = hisi_hba->regs + (0x400 * phy_no) + off;
+
+	writel(val, regs);
+}
+
+static u32 hisi_sas_phy_read32(struct hisi_hba *hisi_hba,
+				      int phy_no, u32 off)
+{
+	void __iomem *regs = hisi_hba->regs + (0x400 * phy_no) + off;
+
+	return readl(regs);
+}
+
+static void config_phy_opt_mode_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	u32 cfg = hisi_sas_phy_read32(hisi_hba, phy_no, PHY_CFG);
+
+	cfg &= ~PHY_CFG_DC_OPT_MSK;
+	cfg |= 1 << PHY_CFG_DC_OPT_OFF;
+	hisi_sas_phy_write32(hisi_hba, phy_no, PHY_CFG, cfg);
+}
+
+static void config_tx_tfe_autoneg_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	u32 cfg = hisi_sas_phy_read32(hisi_hba, phy_no, PHY_CONFIG2);
+
+	cfg &= ~PHY_CONFIG2_FORCE_TXDEEMPH_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_no, PHY_CONFIG2, cfg);
+}
+
+static void config_id_frame_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	struct sas_identify_frame identify_frame;
+	u32 *identify_buffer;
+
+	memset(&identify_frame, 0, sizeof(identify_frame));
+	identify_frame.dev_type = SAS_END_DEVICE;
+	identify_frame.frame_type = 0;
+	identify_frame._un1 = 1;
+	identify_frame.initiator_bits = SAS_PROTOCOL_ALL;
+	identify_frame.target_bits = SAS_PROTOCOL_NONE;
+	memcpy(&identify_frame._un4_11[0], hisi_hba->sas_addr, SAS_ADDR_SIZE);
+	memcpy(&identify_frame.sas_addr[0], hisi_hba->sas_addr,	SAS_ADDR_SIZE);
+	identify_frame.phy_id = phy_no;
+	identify_buffer = (u32 *)(&identify_frame);
+
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD0,
+			__swab32(identify_buffer[0]));
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD1,
+			identify_buffer[2]);
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD2,
+			identify_buffer[1]);
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD3,
+			identify_buffer[4]);
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD4,
+			identify_buffer[3]);
+	hisi_sas_phy_write32(hisi_hba, phy_no, TX_ID_DWORD5,
+			__swab32(identify_buffer[5]));
+}
+
+static void init_id_frame_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int i;
+
+	for (i = 0; i < hisi_hba->n_phy; i++)
+		config_id_frame_v1_hw(hisi_hba, i);
+}
+
+static void setup_itct_v1_hw(struct hisi_hba *hisi_hba,
+			     struct hisi_sas_device *sas_dev)
+{
+	struct domain_device *device = sas_dev->sas_device;
+	struct device *dev = &hisi_hba->pdev->dev;
+	u64 qw0, device_id = sas_dev->device_id;
+	struct hisi_sas_itct *itct = &hisi_hba->itct[device_id];
+
+	memset(itct, 0, sizeof(*itct));
+
+	/* qw0 */
+	qw0 = 0;
+	switch (sas_dev->dev_type) {
+	case SAS_END_DEVICE:
+	case SAS_EDGE_EXPANDER_DEVICE:
+	case SAS_FANOUT_EXPANDER_DEVICE:
+		qw0 = HISI_SAS_DEV_TYPE_SSP << ITCT_HDR_DEV_TYPE_OFF;
+		break;
+	default:
+		dev_warn(dev, "setup itct: unsupported dev type (%d)\n",
+			 sas_dev->dev_type);
+	}
+
+	qw0 |= ((1 << ITCT_HDR_VALID_OFF) |
+		(1 << ITCT_HDR_AWT_CONTROL_OFF) |
+		(device->max_linkrate << ITCT_HDR_MAX_CONN_RATE_OFF) |
+		(1 << ITCT_HDR_VALID_LINK_NUM_OFF) |
+		(device->port->id << ITCT_HDR_PORT_ID_OFF));
+	itct->qw0 = cpu_to_le64(qw0);
+
+	/* qw1 */
+	memcpy(&itct->sas_addr, device->sas_addr, SAS_ADDR_SIZE);
+	itct->sas_addr = __swab64(itct->sas_addr);
+
+	/* qw2 */
+	itct->qw2 = cpu_to_le64((500ULL << ITCT_HDR_IT_NEXUS_LOSS_TL_OFF) |
+				(0xff00ULL << ITCT_HDR_BUS_INACTIVE_TL_OFF) |
+				(0xff00ULL << ITCT_HDR_MAX_CONN_TL_OFF) |
+				(0xff00ULL << ITCT_HDR_REJ_OPEN_TL_OFF));
+}
+
+static void free_device_v1_hw(struct hisi_hba *hisi_hba,
+			      struct hisi_sas_device *sas_dev)
+{
+	u64 dev_id = sas_dev->device_id;
+	struct hisi_sas_itct *itct = &hisi_hba->itct[dev_id];
+	u64 qw0;
+	u32 reg_val = hisi_sas_read32(hisi_hba, CFG_AGING_TIME);
+
+	reg_val |= CFG_AGING_TIME_ITCT_REL_MSK;
+	hisi_sas_write32(hisi_hba, CFG_AGING_TIME, reg_val);
+
+	/* free itct */
+	udelay(1);
+	reg_val = hisi_sas_read32(hisi_hba, CFG_AGING_TIME);
+	reg_val &= ~CFG_AGING_TIME_ITCT_REL_MSK;
+	hisi_sas_write32(hisi_hba, CFG_AGING_TIME, reg_val);
+
+	qw0 = cpu_to_le64(itct->qw0);
+	qw0 &= ~ITCT_HDR_VALID_MSK;
+	itct->qw0 = cpu_to_le64(qw0);
+}
+
+static int reset_hw_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int i;
+	unsigned long end_time;
+	u32 val;
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		u32 phy_ctrl = hisi_sas_phy_read32(hisi_hba, i, PHY_CTRL);
+
+		phy_ctrl |= PHY_CTRL_RESET_MSK;
+		hisi_sas_phy_write32(hisi_hba, i, PHY_CTRL, phy_ctrl);
+	}
+	msleep(1); /* It is safe to wait for 50us */
+
+	/* Ensure DMA tx & rx idle */
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		u32 dma_tx_status, dma_rx_status;
+
+		end_time = jiffies + msecs_to_jiffies(1000);
+
+		while (1) {
+			dma_tx_status = hisi_sas_phy_read32(hisi_hba, i,
+							    DMA_TX_STATUS);
+			dma_rx_status = hisi_sas_phy_read32(hisi_hba, i,
+							    DMA_RX_STATUS);
+
+			if (!(dma_tx_status & DMA_TX_STATUS_BUSY_MSK) &&
+				!(dma_rx_status & DMA_RX_STATUS_BUSY_MSK))
+				break;
+
+			msleep(20);
+			if (time_after(jiffies, end_time))
+				return -EIO;
+		}
+	}
+
+	/* Ensure axi bus idle */
+	end_time = jiffies + msecs_to_jiffies(1000);
+	while (1) {
+		u32 axi_status =
+			hisi_sas_read32(hisi_hba, AXI_CFG);
+
+		if (axi_status == 0)
+			break;
+
+		msleep(20);
+		if (time_after(jiffies, end_time))
+			return -EIO;
+	}
+
+	/* Apply reset and disable clock */
+	/* clk disable reg is offset by +4 bytes from clk enable reg */
+	regmap_write(hisi_hba->ctrl, hisi_hba->ctrl_reset_reg,
+		     RESET_VALUE);
+	regmap_write(hisi_hba->ctrl, hisi_hba->ctrl_clock_ena_reg + 4,
+		     RESET_VALUE);
+	msleep(1);
+	regmap_read(hisi_hba->ctrl, hisi_hba->ctrl_reset_sts_reg, &val);
+	if (RESET_VALUE != (val & RESET_VALUE)) {
+		dev_err(dev, "Reset failed\n");
+		return -EIO;
+	}
+
+	/* De-reset and enable clock */
+	/* deassert rst reg is offset by +4 bytes from assert reg */
+	regmap_write(hisi_hba->ctrl, hisi_hba->ctrl_reset_reg + 4,
+		     RESET_VALUE);
+	regmap_write(hisi_hba->ctrl, hisi_hba->ctrl_clock_ena_reg,
+		     RESET_VALUE);
+	msleep(1);
+	regmap_read(hisi_hba->ctrl, hisi_hba->ctrl_reset_sts_reg, &val);
+	if (val & RESET_VALUE) {
+		dev_err(dev, "De-reset failed\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static void init_reg_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int i;
+
+	/* Global registers init*/
+	hisi_sas_write32(hisi_hba, DLVRY_QUEUE_ENABLE,
+			 (u32)((1ULL << hisi_hba->queue_count) - 1));
+	hisi_sas_write32(hisi_hba, HGC_TRANS_TASK_CNT_LIMIT, 0x11);
+	hisi_sas_write32(hisi_hba, DEVICE_MSG_WORK_MODE, 0x1);
+	hisi_sas_write32(hisi_hba, HGC_SAS_TXFAIL_RETRY_CTRL, 0x1ff);
+	hisi_sas_write32(hisi_hba, HGC_ERR_STAT_EN, 0x401);
+	hisi_sas_write32(hisi_hba, CFG_1US_TIMER_TRSH, 0x64);
+	hisi_sas_write32(hisi_hba, HGC_GET_ITV_TIME, 0x1);
+	hisi_sas_write32(hisi_hba, I_T_NEXUS_LOSS_TIME, 0x64);
+	hisi_sas_write32(hisi_hba, BUS_INACTIVE_LIMIT_TIME, 0x2710);
+	hisi_sas_write32(hisi_hba, REJECT_TO_OPEN_LIMIT_TIME, 0x1);
+	hisi_sas_write32(hisi_hba, CFG_AGING_TIME, 0x7a12);
+	hisi_sas_write32(hisi_hba, HGC_DFX_CFG2, 0x9c40);
+	hisi_sas_write32(hisi_hba, FIS_LIST_BADDR_L, 0x2);
+	hisi_sas_write32(hisi_hba, INT_COAL_EN, 0xc);
+	hisi_sas_write32(hisi_hba, OQ_INT_COAL_TIME, 0x186a0);
+	hisi_sas_write32(hisi_hba, OQ_INT_COAL_CNT, 1);
+	hisi_sas_write32(hisi_hba, ENT_INT_COAL_TIME, 0x1);
+	hisi_sas_write32(hisi_hba, ENT_INT_COAL_CNT, 0x1);
+	hisi_sas_write32(hisi_hba, OQ_INT_SRC, 0xffffffff);
+	hisi_sas_write32(hisi_hba, OQ_INT_SRC_MSK, 0);
+	hisi_sas_write32(hisi_hba, ENT_INT_SRC1, 0xffffffff);
+	hisi_sas_write32(hisi_hba, ENT_INT_SRC_MSK1, 0);
+	hisi_sas_write32(hisi_hba, ENT_INT_SRC2, 0xffffffff);
+	hisi_sas_write32(hisi_hba, ENT_INT_SRC_MSK2, 0);
+	hisi_sas_write32(hisi_hba, SAS_ECC_INTR_MSK, 0);
+	hisi_sas_write32(hisi_hba, AXI_AHB_CLK_CFG, 0x2);
+	hisi_sas_write32(hisi_hba, CFG_SAS_CONFIG, 0x22000000);
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		hisi_sas_phy_write32(hisi_hba, i, PROG_PHY_LINK_RATE, 0x88a);
+		hisi_sas_phy_write32(hisi_hba, i, PHY_CONFIG2, 0x7c080);
+		hisi_sas_phy_write32(hisi_hba, i, PHY_RATE_NEGO, 0x415ee00);
+		hisi_sas_phy_write32(hisi_hba, i, PHY_PCN, 0x80a80000);
+		hisi_sas_phy_write32(hisi_hba, i, SL_TOUT_CFG, 0x7d7d7d7d);
+		hisi_sas_phy_write32(hisi_hba, i, DONE_RECEIVED_TIME, 0x0);
+		hisi_sas_phy_write32(hisi_hba, i, RXOP_CHECK_CFG_H, 0x1000);
+		hisi_sas_phy_write32(hisi_hba, i, DONE_RECEIVED_TIME, 0);
+		hisi_sas_phy_write32(hisi_hba, i, CON_CFG_DRIVER, 0x13f0a);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT_COAL_EN, 3);
+		hisi_sas_phy_write32(hisi_hba, i, DONE_RECEIVED_TIME, 8);
+	}
+
+	for (i = 0; i < hisi_hba->queue_count; i++) {
+		/* Delivery queue */
+		hisi_sas_write32(hisi_hba,
+				 DLVRY_Q_0_BASE_ADDR_HI + (i * 0x14),
+				 upper_32_bits(hisi_hba->cmd_hdr_dma[i]));
+
+		hisi_sas_write32(hisi_hba,
+				 DLVRY_Q_0_BASE_ADDR_LO + (i * 0x14),
+				 lower_32_bits(hisi_hba->cmd_hdr_dma[i]));
+
+		hisi_sas_write32(hisi_hba,
+				 DLVRY_Q_0_DEPTH + (i * 0x14),
+				 HISI_SAS_QUEUE_SLOTS);
+
+		/* Completion queue */
+		hisi_sas_write32(hisi_hba,
+				 COMPL_Q_0_BASE_ADDR_HI + (i * 0x14),
+				 upper_32_bits(hisi_hba->complete_hdr_dma[i]));
+
+		hisi_sas_write32(hisi_hba,
+				 COMPL_Q_0_BASE_ADDR_LO + (i * 0x14),
+				 lower_32_bits(hisi_hba->complete_hdr_dma[i]));
+
+		hisi_sas_write32(hisi_hba, COMPL_Q_0_DEPTH + (i * 0x14),
+				 HISI_SAS_QUEUE_SLOTS);
+	}
+
+	/* itct */
+	hisi_sas_write32(hisi_hba, ITCT_BASE_ADDR_LO,
+			 lower_32_bits(hisi_hba->itct_dma));
+
+	hisi_sas_write32(hisi_hba, ITCT_BASE_ADDR_HI,
+			 upper_32_bits(hisi_hba->itct_dma));
+
+	/* iost */
+	hisi_sas_write32(hisi_hba, IOST_BASE_ADDR_LO,
+			 lower_32_bits(hisi_hba->iost_dma));
+
+	hisi_sas_write32(hisi_hba, IOST_BASE_ADDR_HI,
+			 upper_32_bits(hisi_hba->iost_dma));
+
+	/* breakpoint */
+	hisi_sas_write32(hisi_hba, BROKEN_MSG_ADDR_LO,
+			 lower_32_bits(hisi_hba->breakpoint_dma));
+
+	hisi_sas_write32(hisi_hba, BROKEN_MSG_ADDR_HI,
+			 upper_32_bits(hisi_hba->breakpoint_dma));
+}
+
+static int hw_init_v1_hw(struct hisi_hba *hisi_hba)
+{
+	struct device *dev = &hisi_hba->pdev->dev;
+	int rc;
+
+	rc = reset_hw_v1_hw(hisi_hba);
+	if (rc) {
+		dev_err(dev, "hisi_sas_reset_hw failed, rc=%d", rc);
+		return rc;
+	}
+
+	msleep(100);
+	init_reg_v1_hw(hisi_hba);
+
+	init_id_frame_v1_hw(hisi_hba);
+
+	return 0;
+}
+
+static void enable_phy_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	u32 cfg = hisi_sas_phy_read32(hisi_hba, phy_no, PHY_CFG);
+
+	cfg |= PHY_CFG_ENA_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_no, PHY_CFG, cfg);
+}
+
+static void disable_phy_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	u32 cfg = hisi_sas_phy_read32(hisi_hba, phy_no, PHY_CFG);
+
+	cfg &= ~PHY_CFG_ENA_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_no, PHY_CFG, cfg);
+}
+
+static void start_phy_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	config_id_frame_v1_hw(hisi_hba, phy_no);
+	config_phy_opt_mode_v1_hw(hisi_hba, phy_no);
+	config_tx_tfe_autoneg_v1_hw(hisi_hba, phy_no);
+	enable_phy_v1_hw(hisi_hba, phy_no);
+}
+
+static void stop_phy_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	disable_phy_v1_hw(hisi_hba, phy_no);
+}
+
+static void phy_hard_reset_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	stop_phy_v1_hw(hisi_hba, phy_no);
+	msleep(100);
+	start_phy_v1_hw(hisi_hba, phy_no);
+}
+
+static void start_phys_v1_hw(unsigned long data)
+{
+	struct hisi_hba *hisi_hba = (struct hisi_hba *)data;
+	int i;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT2_MSK, 0x12a);
+		start_phy_v1_hw(hisi_hba, i);
+	}
+}
+
+static void phys_init_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int i;
+	struct timer_list *timer = &hisi_hba->timer;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT2_MSK, 0x6a);
+		hisi_sas_phy_read32(hisi_hba, i, CHL_INT2_MSK);
+	}
+
+	setup_timer(timer, start_phys_v1_hw, (unsigned long)hisi_hba);
+	mod_timer(timer, jiffies + HZ);
+}
+
+static void sl_notify_v1_hw(struct hisi_hba *hisi_hba, int phy_no)
+{
+	u32 sl_control;
+
+	sl_control = hisi_sas_phy_read32(hisi_hba, phy_no, SL_CONTROL);
+	sl_control |= SL_CONTROL_NOTIFY_EN_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_no, SL_CONTROL, sl_control);
+	msleep(1);
+	sl_control = hisi_sas_phy_read32(hisi_hba, phy_no, SL_CONTROL);
+	sl_control &= ~SL_CONTROL_NOTIFY_EN_MSK;
+	hisi_sas_phy_write32(hisi_hba, phy_no, SL_CONTROL, sl_control);
+}
+
+static int get_wideport_bitmap_v1_hw(struct hisi_hba *hisi_hba, int port_id)
+{
+	int i, bitmap = 0;
+	u32 phy_port_num_ma = hisi_sas_read32(hisi_hba, PHY_PORT_NUM_MA);
+
+	for (i = 0; i < hisi_hba->n_phy; i++)
+		if (((phy_port_num_ma >> (i * 4)) & 0xf) == port_id)
+			bitmap |= 1 << i;
+
+	return bitmap;
+}
+
+/**
+ * This function allocates across all queues to load balance.
+ * Slots are allocated from queues in a round-robin fashion.
+ *
+ * The callpath to this function and upto writing the write
+ * queue pointer should be safe from interruption.
+ */
+static int get_free_slot_v1_hw(struct hisi_hba *hisi_hba, int *q, int *s)
+{
+	struct device *dev = &hisi_hba->pdev->dev;
+	u32 r, w;
+	int queue = hisi_hba->queue;
+
+	while (1) {
+		w = hisi_sas_read32_relaxed(hisi_hba,
+				    DLVRY_Q_0_WR_PTR + (queue * 0x14));
+		r = hisi_sas_read32_relaxed(hisi_hba,
+				    DLVRY_Q_0_RD_PTR + (queue * 0x14));
+		if (r == (w+1) % HISI_SAS_QUEUE_SLOTS) {
+			queue = (queue + 1) % hisi_hba->queue_count;
+			if (queue == hisi_hba->queue) {
+				dev_warn(dev, "could not find free slot\n");
+				return -EAGAIN;
+			}
+			continue;
+		}
+		break;
+	}
+	hisi_hba->queue = (queue + 1) % hisi_hba->queue_count;
+	*q = queue;
+	*s = w;
+	return 0;
+}
+
+static void start_delivery_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int dlvry_queue = hisi_hba->slot_prep->dlvry_queue;
+	int dlvry_queue_slot = hisi_hba->slot_prep->dlvry_queue_slot;
+
+	hisi_sas_write32(hisi_hba,
+			 DLVRY_Q_0_WR_PTR + (dlvry_queue * 0x14),
+			 ++dlvry_queue_slot % HISI_SAS_QUEUE_SLOTS);
+}
+
+static int prep_prd_sge_v1_hw(struct hisi_hba *hisi_hba,
+			      struct hisi_sas_slot *slot,
+			      struct hisi_sas_cmd_hdr *hdr,
+			      struct scatterlist *scatter,
+			      int n_elem)
+{
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct scatterlist *sg;
+	int i;
+
+	if (n_elem > HISI_SAS_SGE_PAGE_CNT) {
+		dev_err(dev, "prd err: n_elem(%d) > HISI_SAS_SGE_PAGE_CNT",
+			n_elem);
+		return -EINVAL;
+	}
+
+	slot->sge_page = dma_pool_alloc(hisi_hba->sge_page_pool, GFP_ATOMIC,
+					&slot->sge_page_dma);
+	if (!slot->sge_page)
+		return -ENOMEM;
+
+	for_each_sg(scatter, sg, n_elem, i) {
+		struct hisi_sas_sge *entry = &slot->sge_page->sge[i];
+
+		entry->addr = cpu_to_le64(sg_dma_address(sg));
+		entry->page_ctrl_0 = entry->page_ctrl_1 = 0;
+		entry->data_len = cpu_to_le32(sg_dma_len(sg));
+		entry->data_off = 0;
+	}
+
+	hdr->prd_table_addr = cpu_to_le64(slot->sge_page_dma);
+
+	hdr->sg_len = cpu_to_le32(n_elem << CMD_HDR_DATA_SGL_LEN_OFF);
+
+	return 0;
+}
+
+static int prep_smp_v1_hw(struct hisi_hba *hisi_hba,
+			  struct hisi_sas_slot *slot)
+{
+	struct sas_task *task = slot->task;
+	struct hisi_sas_cmd_hdr *hdr = slot->cmd_hdr;
+	struct domain_device *device = task->dev;
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct hisi_sas_port *port = slot->port;
+	struct scatterlist *sg_req, *sg_resp;
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	dma_addr_t req_dma_addr;
+	unsigned int req_len, resp_len;
+	int elem, rc;
+
+	/*
+	* DMA-map SMP request, response buffers
+	*/
+	/* req */
+	sg_req = &task->smp_task.smp_req;
+	elem = dma_map_sg(dev, sg_req, 1, DMA_TO_DEVICE);
+	if (!elem)
+		return -ENOMEM;
+	req_len = sg_dma_len(sg_req);
+	req_dma_addr = sg_dma_address(sg_req);
+
+	/* resp */
+	sg_resp = &task->smp_task.smp_resp;
+	elem = dma_map_sg(dev, sg_resp, 1, DMA_FROM_DEVICE);
+	if (!elem) {
+		rc = -ENOMEM;
+		goto err_out_req;
+	}
+	resp_len = sg_dma_len(sg_resp);
+	if ((req_len & 0x3) || (resp_len & 0x3)) {
+		rc = -EINVAL;
+		goto err_out_resp;
+	}
+
+	/* create header */
+	/* dw0 */
+	hdr->dw0 = cpu_to_le32((port->id << CMD_HDR_PORT_OFF) |
+			       (1 << CMD_HDR_PRIORITY_OFF) | /* high pri */
+			       (1 << CMD_HDR_MODE_OFF) | /* ini mode */
+			       (2 << CMD_HDR_CMD_OFF)); /* smp */
+
+	/* map itct entry */
+	hdr->dw1 = cpu_to_le32(sas_dev->device_id << CMD_HDR_DEVICE_ID_OFF);
+
+	/* dw2 */
+	hdr->dw2 = cpu_to_le32((((req_len-4)/4) << CMD_HDR_CFL_OFF) |
+			       (HISI_SAS_MAX_SMP_RESP_SZ/4 <<
+			       CMD_HDR_MRFL_OFF));
+
+	hdr->transfer_tags = cpu_to_le32(slot->idx << CMD_HDR_IPTT_OFF);
+
+	hdr->cmd_table_addr = cpu_to_le64(req_dma_addr);
+	hdr->sts_buffer_addr = cpu_to_le64(slot->status_buffer_dma);
+
+	return 0;
+
+err_out_resp:
+	dma_unmap_sg(dev, &slot->task->smp_task.smp_resp, 1,
+		     DMA_FROM_DEVICE);
+err_out_req:
+	dma_unmap_sg(dev, &slot->task->smp_task.smp_req, 1,
+		     DMA_TO_DEVICE);
+	return rc;
+}
+
+static int prep_ssp_v1_hw(struct hisi_hba *hisi_hba,
+			  struct hisi_sas_slot *slot, int is_tmf,
+			  struct hisi_sas_tmf_task *tmf)
+{
+	struct sas_task *task = slot->task;
+	struct hisi_sas_cmd_hdr *hdr = slot->cmd_hdr;
+	struct domain_device *device = task->dev;
+	struct hisi_sas_device *sas_dev = device->lldd_dev;
+	struct hisi_sas_port *port = slot->port;
+	struct sas_ssp_task *ssp_task = &task->ssp_task;
+	struct scsi_cmnd *scsi_cmnd = ssp_task->cmd;
+	int has_data = 0, rc, priority = is_tmf;
+	u8 *buf_cmd, fburst = 0;
+	u32 dw1, dw2;
+
+	/* create header */
+	hdr->dw0 = cpu_to_le32((1 << CMD_HDR_RESP_REPORT_OFF) |
+			       (0x2 << CMD_HDR_TLR_CTRL_OFF) |
+			       (port->id << CMD_HDR_PORT_OFF) |
+			       (priority << CMD_HDR_PRIORITY_OFF) |
+			       (1 << CMD_HDR_MODE_OFF) | /* ini mode */
+			       (1 << CMD_HDR_CMD_OFF)); /* ssp */
+
+	dw1 = 1 << CMD_HDR_VERIFY_DTL_OFF;
+
+	if (is_tmf) {
+		dw1 |= 3 << CMD_HDR_SSP_FRAME_TYPE_OFF;
+	} else {
+		switch (scsi_cmnd->sc_data_direction) {
+		case DMA_TO_DEVICE:
+			dw1 |= 2 << CMD_HDR_SSP_FRAME_TYPE_OFF;
+			has_data = 1;
+			break;
+		case DMA_FROM_DEVICE:
+			dw1 |= 1 << CMD_HDR_SSP_FRAME_TYPE_OFF;
+			has_data = 1;
+			break;
+		default:
+			dw1 |= 0 << CMD_HDR_SSP_FRAME_TYPE_OFF;
+		}
+	}
+
+	/* map itct entry */
+	dw1 |= sas_dev->device_id << CMD_HDR_DEVICE_ID_OFF;
+	hdr->dw1 = cpu_to_le32(dw1);
+
+	if (is_tmf) {
+		dw2 = ((sizeof(struct ssp_tmf_iu) +
+			sizeof(struct ssp_frame_hdr)+3)/4) <<
+			CMD_HDR_CFL_OFF;
+	} else {
+		dw2 = ((sizeof(struct ssp_command_iu) +
+			sizeof(struct ssp_frame_hdr)+3)/4) <<
+			CMD_HDR_CFL_OFF;
+	}
+
+	dw2 |= (HISI_SAS_MAX_SSP_RESP_SZ/4) << CMD_HDR_MRFL_OFF;
+
+	hdr->transfer_tags = cpu_to_le32(slot->idx << CMD_HDR_IPTT_OFF);
+
+	if (has_data) {
+		rc = prep_prd_sge_v1_hw(hisi_hba, slot, hdr, task->scatter,
+					slot->n_elem);
+		if (rc)
+			return rc;
+	}
+
+	hdr->data_transfer_len = cpu_to_le32(task->total_xfer_len);
+	hdr->cmd_table_addr = cpu_to_le64(slot->command_table_dma);
+	hdr->sts_buffer_addr = cpu_to_le64(slot->status_buffer_dma);
+
+	buf_cmd = slot->command_table + sizeof(struct ssp_frame_hdr);
+	if (task->ssp_task.enable_first_burst) {
+		fburst = (1 << 7);
+		dw2 |= 1 << CMD_HDR_FIRST_BURST_OFF;
+	}
+	hdr->dw2 = cpu_to_le32(dw2);
+
+	memcpy(buf_cmd, &task->ssp_task.LUN, 8);
+	if (!is_tmf) {
+		buf_cmd[9] = fburst | task->ssp_task.task_attr |
+				(task->ssp_task.task_prio << 3);
+		memcpy(buf_cmd + 12, task->ssp_task.cmd->cmnd,
+				task->ssp_task.cmd->cmd_len);
+	} else {
+		buf_cmd[10] = tmf->tmf;
+		switch (tmf->tmf) {
+		case TMF_ABORT_TASK:
+		case TMF_QUERY_TASK:
+			buf_cmd[12] =
+				(tmf->tag_of_task_to_be_managed >> 8) & 0xff;
+			buf_cmd[13] =
+				tmf->tag_of_task_to_be_managed & 0xff;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+/* by default, task resp is complete */
+static void slot_err_v1_hw(struct hisi_hba *hisi_hba,
+			   struct sas_task *task,
+			   struct hisi_sas_slot *slot)
+{
+	struct task_status_struct *ts = &task->task_status;
+	struct hisi_sas_err_record *err_record = slot->status_buffer;
+	struct device *dev = &hisi_hba->pdev->dev;
+
+	switch (task->task_proto) {
+	case SAS_PROTOCOL_SSP:
+	{
+		int error = -1;
+		u32 dma_err_type = cpu_to_le32(err_record->dma_err_type);
+		u32 dma_tx_err_type = ((dma_err_type &
+					ERR_HDR_DMA_TX_ERR_TYPE_MSK)) >>
+					ERR_HDR_DMA_TX_ERR_TYPE_OFF;
+		u32 dma_rx_err_type = ((dma_err_type &
+					ERR_HDR_DMA_RX_ERR_TYPE_MSK)) >>
+					ERR_HDR_DMA_RX_ERR_TYPE_OFF;
+		u32 trans_tx_fail_type =
+				cpu_to_le32(err_record->trans_tx_fail_type);
+		u32 trans_rx_fail_type =
+				cpu_to_le32(err_record->trans_rx_fail_type);
+
+		if (dma_tx_err_type) {
+			/* dma tx err */
+			error = ffs(dma_tx_err_type)
+				- 1 + DMA_TX_ERR_BASE;
+		} else if (dma_rx_err_type) {
+			/* dma rx err */
+			error = ffs(dma_rx_err_type)
+				- 1 + DMA_RX_ERR_BASE;
+		} else if (trans_tx_fail_type) {
+			/* trans tx err */
+			error = ffs(trans_tx_fail_type)
+				- 1 + TRANS_TX_FAIL_BASE;
+		} else if (trans_rx_fail_type) {
+			/* trans rx err */
+			error = ffs(trans_rx_fail_type)
+				- 1 + TRANS_RX_FAIL_BASE;
+		}
+
+		switch (error) {
+		case DMA_TX_DATA_UNDERFLOW_ERR:
+		case DMA_RX_DATA_UNDERFLOW_ERR:
+		{
+			ts->residual = 0;
+			ts->stat = SAS_DATA_UNDERRUN;
+			break;
+		}
+		case DMA_TX_DATA_SGL_OVERFLOW_ERR:
+		case DMA_TX_DIF_SGL_OVERFLOW_ERR:
+		case DMA_TX_XFER_RDY_LENGTH_OVERFLOW_ERR:
+		case DMA_RX_DATA_OVERFLOW_ERR:
+		case TRANS_RX_FRAME_OVERRUN_ERR:
+		case TRANS_RX_LINK_BUF_OVERRUN_ERR:
+		{
+			ts->stat = SAS_DATA_OVERRUN;
+			ts->residual = 0;
+			break;
+		}
+		case TRANS_TX_PHY_NOT_ENABLE_ERR:
+		{
+			ts->stat = SAS_PHY_DOWN;
+			break;
+		}
+		case TRANS_TX_OPEN_REJCT_WRONG_DEST_ERR:
+		case TRANS_TX_OPEN_REJCT_ZONE_VIOLATION_ERR:
+		case TRANS_TX_OPEN_REJCT_BY_OTHER_ERR:
+		case TRANS_TX_OPEN_REJCT_AIP_TIMEOUT_ERR:
+		case TRANS_TX_OPEN_REJCT_STP_BUSY_ERR:
+		case TRANS_TX_OPEN_REJCT_PROTOCOL_NOT_SUPPORT_ERR:
+		case TRANS_TX_OPEN_REJCT_RATE_NOT_SUPPORT_ERR:
+		case TRANS_TX_OPEN_REJCT_BAD_DEST_ERR:
+		case TRANS_TX_OPEN_BREAK_RECEIVE_ERR:
+		case TRANS_TX_OPEN_REJCT_PATHWAY_BLOCKED_ERR:
+		case TRANS_TX_OPEN_REJCT_NO_DEST_ERR:
+		case TRANS_TX_OPEN_RETRY_ERR:
+		{
+			ts->stat = SAS_OPEN_REJECT;
+			ts->open_rej_reason = SAS_OREJ_UNKNOWN;
+			break;
+		}
+		case TRANS_TX_OPEN_TIMEOUT_ERR:
+		{
+			ts->stat = SAS_OPEN_TO;
+			break;
+		}
+		case TRANS_TX_NAK_RECEIVE_ERR:
+		case TRANS_TX_ACK_NAK_TIMEOUT_ERR:
+		{
+			ts->stat = SAS_NAK_R_ERR;
+			break;
+		}
+		default:
+		{
+			ts->stat = SAM_STAT_CHECK_CONDITION;
+			break;
+		}
+		}
+	}
+		break;
+	case SAS_PROTOCOL_SMP:
+		ts->stat = SAM_STAT_CHECK_CONDITION;
+		break;
+
+	case SAS_PROTOCOL_SATA:
+	case SAS_PROTOCOL_STP:
+	case SAS_PROTOCOL_SATA | SAS_PROTOCOL_STP:
+	{
+		dev_err(dev, "slot err: SATA/STP not supported");
+	}
+		break;
+	default:
+		break;
+	}
+
+}
+
+static int slot_complete_v1_hw(struct hisi_hba *hisi_hba,
+			       struct hisi_sas_slot *slot, int abort)
+{
+	struct sas_task *task = slot->task;
+	struct hisi_sas_device *sas_dev;
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct task_status_struct *ts;
+	struct domain_device *device;
+	enum exec_status sts;
+	struct hisi_sas_complete_v1_hdr *complete_queue =
+			(struct hisi_sas_complete_v1_hdr *)
+			hisi_hba->complete_hdr[slot->cmplt_queue];
+	struct hisi_sas_complete_v1_hdr *complete_hdr;
+	u32 cmplt_hdr_data;
+
+	complete_hdr = &complete_queue[slot->cmplt_queue_slot];
+	cmplt_hdr_data = le32_to_cpu(complete_hdr->data);
+
+	if (unlikely(!task || !task->lldd_task || !task->dev))
+		return -EINVAL;
+
+	ts = &task->task_status;
+	device = task->dev;
+	sas_dev = device->lldd_dev;
+
+	task->task_state_flags &=
+		~(SAS_TASK_STATE_PENDING | SAS_TASK_AT_INITIATOR);
+	task->task_state_flags |= SAS_TASK_STATE_DONE;
+
+	memset(ts, 0, sizeof(*ts));
+	ts->resp = SAS_TASK_COMPLETE;
+
+	if (unlikely(!sas_dev || abort)) {
+		if (!sas_dev)
+			dev_dbg(dev, "slot complete: port has not device\n");
+		ts->stat = SAS_PHY_DOWN;
+		goto out;
+	}
+
+	if (cmplt_hdr_data & CMPLT_HDR_IO_CFG_ERR_MSK) {
+		u32 info_reg = hisi_sas_read32(hisi_hba, HGC_INVLD_DQE_INFO);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_DQ_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq IPTT err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_TYPE_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq type err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_FORCE_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq force phy err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_PHY_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq phy id err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_ABORT_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq abort flag err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_IPTT_OF_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq IPTT or ICT err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_SSP_ERR_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq SSP frame type err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		if (info_reg & HGC_INVLD_DQE_INFO_OFL_MSK)
+			dev_err(dev, "slot complete: [%d:%d] has dq order frame len err",
+				slot->cmplt_queue, slot->cmplt_queue_slot);
+
+		ts->stat = SAS_OPEN_REJECT;
+		ts->open_rej_reason = SAS_OREJ_UNKNOWN;
+		goto out;
+	}
+
+	if (cmplt_hdr_data & CMPLT_HDR_ERR_RCRD_XFRD_MSK &&
+		!(cmplt_hdr_data & CMPLT_HDR_RSPNS_XFRD_MSK)) {
+
+		slot_err_v1_hw(hisi_hba, task, slot);
+		goto out;
+	}
+
+	switch (task->task_proto) {
+	case SAS_PROTOCOL_SSP:
+	{
+		struct ssp_response_iu *iu = slot->status_buffer +
+			sizeof(struct hisi_sas_err_record);
+		sas_ssp_task_response(dev, task, iu);
+		break;
+	}
+	case SAS_PROTOCOL_SMP:
+	{
+		void *to;
+		struct scatterlist *sg_resp = &task->smp_task.smp_resp;
+
+		ts->stat = SAM_STAT_GOOD;
+		to = kmap_atomic(sg_page(sg_resp));
+
+		dma_unmap_sg(dev, &task->smp_task.smp_resp, 1,
+			     DMA_FROM_DEVICE);
+		dma_unmap_sg(dev, &task->smp_task.smp_req, 1,
+			     DMA_TO_DEVICE);
+		memcpy(to + sg_resp->offset,
+		       slot->status_buffer +
+		       sizeof(struct hisi_sas_err_record),
+		       sg_dma_len(sg_resp));
+		kunmap_atomic(to);
+		break;
+	}
+	case SAS_PROTOCOL_SATA:
+	case SAS_PROTOCOL_STP:
+	case SAS_PROTOCOL_SATA | SAS_PROTOCOL_STP:
+		dev_err(dev, "slot complete: SATA/STP not supported");
+		break;
+
+	default:
+		ts->stat = SAM_STAT_CHECK_CONDITION;
+		break;
+	}
+
+	if (!slot->port->port_attached) {
+		dev_err(dev, "slot complete: port %d has removed\n",
+			slot->port->sas_port.id);
+		ts->stat = SAS_PHY_DOWN;
+	}
+
+out:
+	if (sas_dev && sas_dev->running_req)
+		sas_dev->running_req--;
+
+	hisi_sas_slot_task_free(hisi_hba, task, slot);
+	sts = ts->stat;
+
+	if (task->task_done)
+		task->task_done(task);
+
+	return sts;
+}
+
+/* Interrupts */
+static irqreturn_t int_phyup_v1_hw(int irq_no, void *p)
+{
+	struct hisi_sas_phy *phy = p;
+	struct hisi_hba *hisi_hba = phy->hisi_hba;
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	int i, phy_no = sas_phy->id;
+	u32 irq_value, context, port_id, link_rate;
+	u32 *frame_rcvd = (u32 *)sas_phy->frame_rcvd;
+	struct sas_identify_frame *id = (struct sas_identify_frame *)frame_rcvd;
+	irqreturn_t res = IRQ_HANDLED;
+
+	irq_value = hisi_sas_phy_read32(hisi_hba, phy_no, CHL_INT2);
+	if (!(irq_value & CHL_INT2_SL_PHY_ENA_MSK)) {
+		dev_dbg(dev, "phyup: irq_value = %x not set enable bit\n",
+			irq_value);
+		res = IRQ_NONE;
+		goto end;
+	}
+
+	context = hisi_sas_read32(hisi_hba, PHY_CONTEXT);
+	if (context & 1 << phy_no) {
+		dev_err(dev, "phyup: phy%d SATA attached equipment\n",
+			phy_no);
+		goto end;
+	}
+
+	port_id = (hisi_sas_read32(hisi_hba, PHY_PORT_NUM_MA) >> (4 * phy_no))
+		  & 0xf;
+	if (port_id == 0xf) {
+		dev_err(dev, "phyup: phy%d invalid portid\n", phy_no);
+		res = IRQ_NONE;
+		goto end;
+	}
+
+	for (i = 0; i < 6; i++) {
+		u32 idaf = hisi_sas_phy_read32(hisi_hba, phy_no,
+					RX_IDAF_DWORD0 + (i * 4));
+		frame_rcvd[i] = __swab32(idaf);
+	}
+
+	/* Get the linkrate */
+	link_rate = hisi_sas_read32(hisi_hba, PHY_CONN_RATE);
+	link_rate = (link_rate >> (phy_no * 4)) & 0xf;
+	sas_phy->linkrate = link_rate;
+	sas_phy->oob_mode = SAS_OOB_MODE;
+	memcpy(sas_phy->attached_sas_addr,
+		&id->sas_addr, SAS_ADDR_SIZE);
+	dev_info(dev, "phyup: phy%d link_rate=%d\n",
+		 phy_no, link_rate);
+	phy->port_id = port_id;
+	phy->phy_type &= ~(PORT_TYPE_SAS | PORT_TYPE_SATA);
+	phy->phy_type |= PORT_TYPE_SAS;
+	phy->phy_attached = 1;
+	phy->identify.device_type = id->dev_type;
+	phy->frame_rcvd_size =	sizeof(struct sas_identify_frame);
+	if (phy->identify.device_type == SAS_END_DEVICE)
+		phy->identify.target_port_protocols =
+			SAS_PROTOCOL_SSP;
+	else if (phy->identify.device_type != SAS_PHY_UNUSED)
+		phy->identify.target_port_protocols =
+			SAS_PROTOCOL_SMP;
+	queue_work(hisi_hba->wq, &phy->phyup_ws);
+
+end:
+	hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT2,
+			     CHL_INT2_SL_PHY_ENA_MSK);
+
+	if (irq_value & CHL_INT2_SL_PHY_ENA_MSK) {
+		u32 chl_int0 = hisi_sas_phy_read32(hisi_hba, phy_no, CHL_INT0);
+
+		chl_int0 &= ~CHL_INT0_PHYCTRL_NOTRDY_MSK;
+		hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0, chl_int0);
+		hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0_MSK, 0x3ce3ee);
+	}
+
+	return res;
+}
+
+static irqreturn_t int_bcast_v1_hw(int irq, void *p)
+{
+	struct hisi_sas_phy *phy = p;
+	struct hisi_hba *hisi_hba = phy->hisi_hba;
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	struct sas_ha_struct *sha = &hisi_hba->sha;
+	struct device *dev = &hisi_hba->pdev->dev;
+	int phy_no = sas_phy->id;
+	u32 irq_value;
+	irqreturn_t res = IRQ_HANDLED;
+
+	irq_value = hisi_sas_phy_read32(hisi_hba, phy_no, CHL_INT2);
+
+	if (!(irq_value & CHL_INT2_SL_RX_BC_ACK_MSK)) {
+		dev_err(dev, "bcast: irq_value = %x not set enable bit",
+			irq_value);
+		res = IRQ_NONE;
+		goto end;
+	}
+
+	sha->notify_port_event(sas_phy, PORTE_BROADCAST_RCVD);
+
+end:
+	hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT2,
+			     CHL_INT2_SL_RX_BC_ACK_MSK);
+
+	return res;
+}
+
+static irqreturn_t int_abnormal_v1_hw(int irq, void *p)
+{
+	struct hisi_sas_phy *phy = p;
+	struct hisi_hba *hisi_hba = phy->hisi_hba;
+	struct device *dev = &hisi_hba->pdev->dev;
+	struct asd_sas_phy *sas_phy = &phy->sas_phy;
+	u32 irq_value, irq_mask_old;
+	int phy_no = sas_phy->id;
+
+	/* mask_int0 */
+	irq_mask_old = hisi_sas_phy_read32(hisi_hba, phy_no, CHL_INT0_MSK);
+	hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0_MSK, 0x3fffff);
+
+	/* read int0 */
+	irq_value = hisi_sas_phy_read32(hisi_hba, phy_no, CHL_INT0);
+
+	if (irq_value & CHL_INT0_PHYCTRL_NOTRDY_MSK) {
+		u32 phy_state = hisi_sas_read32(hisi_hba, PHY_STATE);
+
+		hisi_sas_phy_down(hisi_hba, phy_no,
+				  (phy_state & 1 << phy_no) ? 1 : 0);
+	}
+
+	if (irq_value & CHL_INT0_ID_TIMEOUT_MSK)
+		dev_dbg(dev, "abnormal: ID_TIMEOUT phy%d identify timeout\n",
+			phy_no);
+
+	if (irq_value & CHL_INT0_DWS_LOST_MSK)
+		dev_dbg(dev, "abnormal: DWS_LOST phy%d dws lost\n", phy_no);
+
+	if (irq_value & CHL_INT0_SN_FAIL_NGR_MSK)
+		dev_dbg(dev, "abnormal: SN_FAIL_NGR phy%d sn fail ngr\n",
+			phy_no);
+
+	if (irq_value & CHL_INT0_SL_IDAF_FAIL_MSK ||
+		irq_value & CHL_INT0_SL_OPAF_FAIL_MSK)
+		dev_dbg(dev, "abnormal: SL_ID/OPAF_FAIL phy%d check adr frm err\n",
+			phy_no);
+
+	if (irq_value & CHL_INT0_SL_PS_FAIL_OFF)
+		dev_dbg(dev, "abnormal: SL_PS_FAIL phy%d fail\n", phy_no);
+
+	/* write to zero */
+	hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0, irq_value);
+
+	if (irq_value & CHL_INT0_PHYCTRL_NOTRDY_MSK)
+		hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0_MSK,
+				0x3fffff & ~CHL_INT0_MSK_PHYCTRL_NOTRDY_MSK);
+	else
+		hisi_sas_phy_write32(hisi_hba, phy_no, CHL_INT0_MSK,
+				irq_mask_old);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cq_interrupt_v1_hw(int irq, void *p)
+{
+	struct hisi_sas_cq *cq = p;
+	struct hisi_hba *hisi_hba = cq->hisi_hba;
+	struct hisi_sas_slot *slot;
+	int queue = cq->id;
+	struct hisi_sas_complete_v1_hdr *complete_queue =
+			(struct hisi_sas_complete_v1_hdr *)
+			hisi_hba->complete_hdr[queue];
+	u32 irq_value, rd_point, wr_point;
+
+	irq_value = hisi_sas_read32(hisi_hba, OQ_INT_SRC);
+
+	hisi_sas_write32(hisi_hba, OQ_INT_SRC, 1 << queue);
+
+	rd_point = hisi_sas_read32(hisi_hba,
+			COMPL_Q_0_RD_PTR + (0x14 * queue));
+	wr_point = hisi_sas_read32(hisi_hba,
+			COMPL_Q_0_WR_PTR + (0x14 * queue));
+
+	while (rd_point != wr_point) {
+		struct hisi_sas_complete_v1_hdr *complete_hdr;
+		int idx;
+		u32 cmplt_hdr_data;
+
+		complete_hdr = &complete_queue[rd_point];
+		cmplt_hdr_data = cpu_to_le32(complete_hdr->data);
+		idx = (cmplt_hdr_data & CMPLT_HDR_IPTT_MSK) >>
+		      CMPLT_HDR_IPTT_OFF;
+		slot = &hisi_hba->slot_info[idx];
+
+		/* The completion queue and queue slot index are not
+		 * necessarily the same as the delivery queue and
+		 * queue slot index.
+		 */
+		slot->cmplt_queue_slot = rd_point;
+		slot->cmplt_queue = queue;
+		slot_complete_v1_hw(hisi_hba, slot, 0);
+
+		if (++rd_point >= HISI_SAS_QUEUE_SLOTS)
+			rd_point = 0;
+	}
+
+	/* update rd_point */
+	hisi_sas_write32(hisi_hba, COMPL_Q_0_RD_PTR + (0x14 * queue), rd_point);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t fatal_ecc_int_v1_hw(int irq, void *p)
+{
+	struct hisi_hba *hisi_hba = p;
+	struct device *dev = &hisi_hba->pdev->dev;
+	u32 ecc_int = hisi_sas_read32(hisi_hba, SAS_ECC_INTR);
+
+	if (ecc_int & SAS_ECC_INTR_DQ_ECC1B_MSK) {
+		u32 ecc_err = hisi_sas_read32(hisi_hba, HGC_ECC_ERR);
+
+		panic("%s: Fatal DQ 1b ECC interrupt (0x%x)\n",
+		      dev_name(dev), ecc_err);
+	}
+
+	if (ecc_int & SAS_ECC_INTR_DQ_ECCBAD_MSK) {
+		u32 addr = (hisi_sas_read32(hisi_hba, HGC_DQ_ECC_ADDR) &
+				HGC_DQ_ECC_ADDR_BAD_MSK) >>
+				HGC_DQ_ECC_ADDR_BAD_OFF;
+
+		panic("%s: Fatal DQ RAM ECC interrupt @ 0x%08x\n",
+		      dev_name(dev), addr);
+	}
+
+	if (ecc_int & SAS_ECC_INTR_IOST_ECC1B_MSK) {
+		u32 ecc_err = hisi_sas_read32(hisi_hba, HGC_ECC_ERR);
+
+		panic("%s: Fatal IOST 1b ECC interrupt (0x%x)\n",
+		      dev_name(dev), ecc_err);
+	}
+
+	if (ecc_int & SAS_ECC_INTR_IOST_ECCBAD_MSK) {
+		u32 addr = (hisi_sas_read32(hisi_hba, HGC_IOST_ECC_ADDR) &
+				HGC_IOST_ECC_ADDR_BAD_MSK) >>
+				HGC_IOST_ECC_ADDR_BAD_OFF;
+
+		panic("%s: Fatal IOST RAM ECC interrupt @ 0x%08x\n",
+		      dev_name(dev), addr);
+	}
+
+	if (ecc_int & SAS_ECC_INTR_ITCT_ECCBAD_MSK) {
+		u32 addr = (hisi_sas_read32(hisi_hba, HGC_ITCT_ECC_ADDR) &
+				HGC_ITCT_ECC_ADDR_BAD_MSK) >>
+				HGC_ITCT_ECC_ADDR_BAD_OFF;
+
+		panic("%s: Fatal TCT RAM ECC interrupt @ 0x%08x\n",
+		      dev_name(dev), addr);
+	}
+
+	if (ecc_int & SAS_ECC_INTR_ITCT_ECC1B_MSK) {
+		u32 ecc_err = hisi_sas_read32(hisi_hba, HGC_ECC_ERR);
+
+		panic("%s: Fatal ITCT 1b ECC interrupt (0x%x)\n",
+		      dev_name(dev), ecc_err);
+	}
+
+	hisi_sas_write32(hisi_hba, SAS_ECC_INTR, ecc_int | 0x3f);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t fatal_axi_int_v1_hw(int irq, void *p)
+{
+	struct hisi_hba *hisi_hba = p;
+	struct device *dev = &hisi_hba->pdev->dev;
+	u32 axi_int = hisi_sas_read32(hisi_hba, ENT_INT_SRC2);
+	u32 axi_info = hisi_sas_read32(hisi_hba, HGC_AXI_FIFO_ERR_INFO);
+
+	if (axi_int & ENT_INT_SRC2_DQ_CFG_ERR_MSK)
+		panic("%s: Fatal DQ_CFG_ERR interrupt (0x%x)\n",
+		      dev_name(dev), axi_info);
+
+	if (axi_int & ENT_INT_SRC2_CQ_CFG_ERR_MSK)
+		panic("%s: Fatal CQ_CFG_ERR interrupt (0x%x)\n",
+		      dev_name(dev), axi_info);
+
+	if (axi_int & ENT_INT_SRC2_AXI_WRONG_INT_MSK)
+		panic("%s: Fatal AXI_WRONG_INT interrupt (0x%x)\n",
+		      dev_name(dev), axi_info);
+
+	if (axi_int & ENT_INT_SRC2_AXI_OVERLF_INT_MSK)
+		panic("%s: Fatal AXI_OVERLF_INT incorrect interrupt (0x%x)\n",
+		      dev_name(dev), axi_info);
+
+	hisi_sas_write32(hisi_hba, ENT_INT_SRC2, axi_int | 0x30000000);
+
+	return IRQ_HANDLED;
+}
+
+static irq_handler_t phy_interrupts[HISI_SAS_PHY_INT_NR] = {
+	int_bcast_v1_hw,
+	int_phyup_v1_hw,
+	int_abnormal_v1_hw
+};
+
+static irq_handler_t fatal_interrupts[HISI_SAS_MAX_QUEUES] = {
+	fatal_ecc_int_v1_hw,
+	fatal_axi_int_v1_hw
+};
+
+static int interrupt_init_v1_hw(struct hisi_hba *hisi_hba)
+{
+	struct platform_device *pdev = hisi_hba->pdev;
+	struct device *dev = &pdev->dev;
+	int i, j, irq, rc, idx;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		struct hisi_sas_phy *phy = &hisi_hba->phy[i];
+
+		idx = i * HISI_SAS_PHY_INT_NR;
+		for (j = 0; j < HISI_SAS_PHY_INT_NR; j++, idx++) {
+			irq = platform_get_irq(pdev, idx);
+			if (!irq) {
+				dev_err(dev,
+					"irq init: fail map phy interrupt %d\n",
+					idx);
+				return -ENOENT;
+			}
+
+			rc = devm_request_irq(dev, irq, phy_interrupts[j], 0,
+					      DRV_NAME " phy", phy);
+			if (rc) {
+				dev_err(dev, "irq init: could not request "
+					"phy interrupt %d, rc=%d\n",
+					irq, rc);
+				return -ENOENT;
+			}
+		}
+	}
+
+	idx = hisi_hba->n_phy * HISI_SAS_PHY_INT_NR;
+	for (i = 0; i < hisi_hba->queue_count; i++, idx++) {
+		irq = platform_get_irq(pdev, idx);
+		if (!irq) {
+			dev_err(dev, "irq init: could not map cq interrupt %d\n",
+				idx);
+			return -ENOENT;
+		}
+
+		rc = devm_request_irq(dev, irq, cq_interrupt_v1_hw, 0,
+				      DRV_NAME " cq", &hisi_hba->cq[i]);
+		if (rc) {
+			dev_err(dev, "irq init: could not request cq interrupt %d, rc=%d\n",
+				irq, rc);
+			return -ENOENT;
+		}
+	}
+
+	idx = (hisi_hba->n_phy * HISI_SAS_PHY_INT_NR) + hisi_hba->queue_count;
+	for (i = 0; i < HISI_SAS_FATAL_INT_NR; i++, idx++) {
+		irq = platform_get_irq(pdev, idx);
+		if (!irq) {
+			dev_err(dev, "irq init: could not map fatal interrupt %d\n",
+				idx);
+			return -ENOENT;
+		}
+
+		rc = devm_request_irq(dev, irq, fatal_interrupts[i], 0,
+				      DRV_NAME " fatal", hisi_hba);
+		if (rc) {
+			dev_err(dev,
+				"irq init: could not request fatal interrupt %d, rc=%d\n",
+				irq, rc);
+			return -ENOENT;
+		}
+	}
+
+	return 0;
+}
+
+static int interrupt_openall_v1_hw(struct hisi_hba *hisi_hba)
+{
+	int i;
+	u32 val;
+
+	for (i = 0; i < hisi_hba->n_phy; i++) {
+		/* Clear interrupt status */
+		val = hisi_sas_phy_read32(hisi_hba, i, CHL_INT0);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT0, val);
+		val = hisi_sas_phy_read32(hisi_hba, i, CHL_INT1);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT1, val);
+		val = hisi_sas_phy_read32(hisi_hba, i, CHL_INT2);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT2, val);
+
+		/* Unmask interrupt */
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT0_MSK, 0x3ce3ee);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT1_MSK, 0x17fff);
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT2_MSK, 0x8000012a);
+
+		/* bypass chip bug mask abnormal intr */
+		hisi_sas_phy_write32(hisi_hba, i, CHL_INT0_MSK,
+				0x3fffff & ~CHL_INT0_MSK_PHYCTRL_NOTRDY_MSK);
+	}
+
+	return 0;
+}
+
+static int hisi_sas_v1_init(struct hisi_hba *hisi_hba)
+{
+	int rc;
+
+	rc = hw_init_v1_hw(hisi_hba);
+	if (rc)
+		return rc;
+
+	rc = interrupt_init_v1_hw(hisi_hba);
+	if (rc)
+		return rc;
+
+	rc = interrupt_openall_v1_hw(hisi_hba);
+	if (rc)
+		return rc;
+
+	phys_init_v1_hw(hisi_hba);
+
+	return 0;
+}
+
+static const struct hisi_sas_hw hisi_sas_v1_hw = {
+	.hw_init = hisi_sas_v1_init,
+	.setup_itct = setup_itct_v1_hw,
+	.sl_notify = sl_notify_v1_hw,
+	.free_device = free_device_v1_hw,
+	.prep_smp = prep_smp_v1_hw,
+	.prep_ssp = prep_ssp_v1_hw,
+	.get_free_slot = get_free_slot_v1_hw,
+	.start_delivery = start_delivery_v1_hw,
+	.slot_complete = slot_complete_v1_hw,
+	.phy_enable = enable_phy_v1_hw,
+	.phy_disable = disable_phy_v1_hw,
+	.phy_hard_reset = phy_hard_reset_v1_hw,
+	.get_wideport_bitmap = get_wideport_bitmap_v1_hw,
+	.complete_hdr_size = sizeof(struct hisi_sas_complete_v1_hdr),
+};
+
+static int hisi_sas_v1_probe(struct platform_device *pdev)
+{
+	return hisi_sas_probe(pdev, &hisi_sas_v1_hw);
+}
+
+static int hisi_sas_v1_remove(struct platform_device *pdev)
+{
+	return hisi_sas_remove(pdev);
+}
+
+static const struct of_device_id sas_v1_of_match[] = {
+	{ .compatible = "hisilicon,hip05-sas-v1",},
+	{},
+};
+MODULE_DEVICE_TABLE(of, sas_v1_of_match);
+
+static struct platform_driver hisi_sas_v1_driver = {
+	.probe = hisi_sas_v1_probe,
+	.remove = hisi_sas_v1_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table = sas_v1_of_match,
+	},
+};
+
+module_platform_driver(hisi_sas_v1_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("John Garry <john.garry@huawei.com>");
+MODULE_DESCRIPTION("HISILICON SAS controller v1 hw driver");
+MODULE_ALIAS("platform:" DRV_NAME);
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index a3860367b568..38ce0e308fbe 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -750,7 +750,6 @@ static ssize_t host_show_hp_ssd_smart_path_enabled(struct device *dev,
 }
 
 #define MAX_PATHS 8
-
 static ssize_t path_info_show(struct device *dev,
 	     struct device_attribute *attr, char *buf)
 {
@@ -792,10 +791,8 @@ static ssize_t path_info_show(struct device *dev,
 				hdev->bus, hdev->target, hdev->lun,
 				scsi_device_type(hdev->devtype));
 
-		if (hdev->external ||
-			hdev->devtype == TYPE_RAID ||
-			is_logical_device(hdev)) {
-			output_len += snprintf(buf + output_len,
+		if (hdev->devtype == TYPE_RAID || is_logical_device(hdev)) {
+			output_len += scnprintf(buf + output_len,
 						PAGE_SIZE - output_len,
 						"%s\n", active);
 			continue;
@@ -808,29 +805,28 @@ static ssize_t path_info_show(struct device *dev,
 			phys_connector[0] = '0';
 		if (phys_connector[1] < '0')
 			phys_connector[1] = '0';
-		if (hdev->phys_connector[i] > 0)
-			output_len += snprintf(buf + output_len,
+		output_len += scnprintf(buf + output_len,
 				PAGE_SIZE - output_len,
 				"PORT: %.2s ",
 				phys_connector);
 		if (hdev->devtype == TYPE_DISK && hdev->expose_device) {
 			if (box == 0 || box == 0xFF) {
-				output_len += snprintf(buf + output_len,
+				output_len += scnprintf(buf + output_len,
 					PAGE_SIZE - output_len,
 					"BAY: %hhu %s\n",
 					bay, active);
 			} else {
-				output_len += snprintf(buf + output_len,
+				output_len += scnprintf(buf + output_len,
 					PAGE_SIZE - output_len,
 					"BOX: %hhu BAY: %hhu %s\n",
 					box, bay, active);
 			}
 		} else if (box != 0 && box != 0xFF) {
-			output_len += snprintf(buf + output_len,
+			output_len += scnprintf(buf + output_len,
 				PAGE_SIZE - output_len, "BOX: %hhu %s\n",
 				box, active);
 		} else
-			output_len += snprintf(buf + output_len,
+			output_len += scnprintf(buf + output_len,
 				PAGE_SIZE - output_len, "%s\n", active);
 	}
 
@@ -3191,6 +3187,87 @@ out:
 	return rc;
 }
 
+/*
+ * get enclosure information
+ * struct ReportExtendedLUNdata *rlep - Used for BMIC drive number
+ * struct hpsa_scsi_dev_t *encl_dev - device entry for enclosure
+ * Uses id_physical_device to determine the box_index.
+ */
+static void hpsa_get_enclosure_info(struct ctlr_info *h,
+			unsigned char *scsi3addr,
+			struct ReportExtendedLUNdata *rlep, int rle_index,
+			struct hpsa_scsi_dev_t *encl_dev)
+{
+	int rc = -1;
+	struct CommandList *c = NULL;
+	struct ErrorInfo *ei = NULL;
+	struct bmic_sense_storage_box_params *bssbp = NULL;
+	struct bmic_identify_physical_device *id_phys = NULL;
+	struct ext_report_lun_entry *rle = &rlep->LUN[rle_index];
+	u16 bmic_device_index = 0;
+
+	bmic_device_index = GET_BMIC_DRIVE_NUMBER(&rle->lunid[0]);
+
+	if (bmic_device_index == 0xFF00)
+		goto out;
+
+	bssbp = kzalloc(sizeof(*bssbp), GFP_KERNEL);
+	if (!bssbp)
+		goto out;
+
+	id_phys = kzalloc(sizeof(*id_phys), GFP_KERNEL);
+	if (!id_phys)
+		goto out;
+
+	rc = hpsa_bmic_id_physical_device(h, scsi3addr, bmic_device_index,
+						id_phys, sizeof(*id_phys));
+	if (rc) {
+		dev_warn(&h->pdev->dev, "%s: id_phys failed %d bdi[0x%x]\n",
+			__func__, encl_dev->external, bmic_device_index);
+		goto out;
+	}
+
+	c = cmd_alloc(h);
+
+	rc = fill_cmd(c, BMIC_SENSE_STORAGE_BOX_PARAMS, h, bssbp,
+			sizeof(*bssbp), 0, RAID_CTLR_LUNID, TYPE_CMD);
+
+	if (rc)
+		goto out;
+
+	if (id_phys->phys_connector[1] == 'E')
+		c->Request.CDB[5] = id_phys->box_index;
+	else
+		c->Request.CDB[5] = 0;
+
+	rc = hpsa_scsi_do_simple_cmd_with_retry(h, c, PCI_DMA_FROMDEVICE,
+						NO_TIMEOUT);
+	if (rc)
+		goto out;
+
+	ei = c->err_info;
+	if (ei->CommandStatus != 0 && ei->CommandStatus != CMD_DATA_UNDERRUN) {
+		rc = -1;
+		goto out;
+	}
+
+	encl_dev->box[id_phys->active_path_number] = bssbp->phys_box_on_port;
+	memcpy(&encl_dev->phys_connector[id_phys->active_path_number],
+		bssbp->phys_connector, sizeof(bssbp->phys_connector));
+
+	rc = IO_OK;
+out:
+	kfree(bssbp);
+	kfree(id_phys);
+
+	if (c)
+		cmd_free(h, c);
+
+	if (rc != IO_OK)
+		hpsa_show_dev_msg(KERN_INFO, h, encl_dev,
+			"Error, could not get enclosure information\n");
+}
+
 static u64 hpsa_get_sas_address_from_report_physical(struct ctlr_info *h,
 						unsigned char *scsi3addr)
 {
@@ -4032,7 +4109,8 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h)
 
 		/* skip masked non-disk devices */
 		if (MASKED_DEVICE(lunaddrbytes) && physical_device &&
-			(physdev_list->LUN[phys_dev_index].device_flags & 0x01))
+		   (physdev_list->LUN[phys_dev_index].device_type != 0x06) &&
+		   (physdev_list->LUN[phys_dev_index].device_flags & 0x01))
 			continue;
 
 		/* Get device type, vendor, model, device id */
@@ -4116,7 +4194,12 @@ static void hpsa_update_scsi_devices(struct ctlr_info *h)
 			break;
 		case TYPE_TAPE:
 		case TYPE_MEDIUM_CHANGER:
+			ncurrent++;
+			break;
 		case TYPE_ENCLOSURE:
+			hpsa_get_enclosure_info(h, lunaddrbytes,
+						physdev_list, phys_dev_index,
+						this_device);
 			ncurrent++;
 			break;
 		case TYPE_RAID:
@@ -6629,6 +6712,16 @@ static int fill_cmd(struct CommandList *c, u8 cmd, struct ctlr_info *h,
 			c->Request.CDB[7] = (size >> 16) & 0xFF;
 			c->Request.CDB[8] = (size >> 8) & 0XFF;
 			break;
+		case BMIC_SENSE_STORAGE_BOX_PARAMS:
+			c->Request.CDBLen = 10;
+			c->Request.type_attr_dir =
+				TYPE_ATTR_DIR(cmd_type, ATTR_SIMPLE, XFER_READ);
+			c->Request.Timeout = 0;
+			c->Request.CDB[0] = BMIC_READ;
+			c->Request.CDB[6] = BMIC_SENSE_STORAGE_BOX_PARAMS;
+			c->Request.CDB[7] = (size >> 16) & 0xFF;
+			c->Request.CDB[8] = (size >> 8) & 0XFF;
+			break;
 		case BMIC_IDENTIFY_CONTROLLER:
 			c->Request.CDBLen = 10;
 			c->Request.type_attr_dir =
diff --git a/drivers/scsi/hpsa.h b/drivers/scsi/hpsa.h
index ae5beda1bdb5..fdd39fc0b199 100644
--- a/drivers/scsi/hpsa.h
+++ b/drivers/scsi/hpsa.h
@@ -400,7 +400,7 @@ struct offline_device_entry {
 #define HPSA_PHYSICAL_DEVICE_BUS	0
 #define HPSA_RAID_VOLUME_BUS		1
 #define HPSA_EXTERNAL_RAID_VOLUME_BUS	2
-#define HPSA_HBA_BUS			3
+#define HPSA_HBA_BUS			0
 
 /*
 	Send the command to the hardware
diff --git a/drivers/scsi/hpsa_cmd.h b/drivers/scsi/hpsa_cmd.h
index d92ef0d352b5..6a919ada96b3 100644
--- a/drivers/scsi/hpsa_cmd.h
+++ b/drivers/scsi/hpsa_cmd.h
@@ -291,6 +291,7 @@ struct SenseSubsystem_info {
 #define BMIC_SENSE_DIAG_OPTIONS 0xF5
 #define HPSA_DIAG_OPTS_DISABLE_RLD_CACHING 0x40000000
 #define BMIC_SENSE_SUBSYSTEM_INFORMATION 0x66
+#define BMIC_SENSE_STORAGE_BOX_PARAMS 0x65
 
 /* Command List Structure */
 union SCSI3Addr {
@@ -842,5 +843,17 @@ struct bmic_sense_subsystem_info {
 	u8	pad[332];
 };
 
+struct bmic_sense_storage_box_params {
+	u8	reserved[36];
+	u8	inquiry_valid;
+	u8	reserved_1[68];
+	u8	phys_box_on_port;
+	u8	reserved_2[22];
+	u16	connection_info;
+	u8	reserver_3[84];
+	u8	phys_connector[2];
+	u8	reserved_4[296];
+};
+
 #pragma pack()
 #endif /* HPSA_CMD_H */
diff --git a/drivers/scsi/imm.c b/drivers/scsi/imm.c
index 4e1a632ccf16..f8b88fa78e62 100644
--- a/drivers/scsi/imm.c
+++ b/drivers/scsi/imm.c
@@ -43,6 +43,7 @@ typedef struct {
 	unsigned dp:1;		/* Data phase present           */
 	unsigned rd:1;		/* Read data in data phase      */
 	unsigned wanted:1;	/* Parport sharing busy flag    */
+	unsigned int dev_no;	/* Device number		*/
 	wait_queue_head_t *waiting;
 	struct Scsi_Host *host;
 	struct list_head list;
@@ -1120,15 +1121,40 @@ static struct scsi_host_template imm_template = {
 
 static LIST_HEAD(imm_hosts);
 
+/*
+ * Finds the first available device number that can be alloted to the
+ * new imm device and returns the address of the previous node so that
+ * we can add to the tail and have a list in the ascending order.
+ */
+
+static inline imm_struct *find_parent(void)
+{
+	imm_struct *dev, *par = NULL;
+	unsigned int cnt = 0;
+
+	if (list_empty(&imm_hosts))
+		return NULL;
+
+	list_for_each_entry(dev, &imm_hosts, list) {
+		if (dev->dev_no != cnt)
+			return par;
+		cnt++;
+		par = dev;
+	}
+
+	return par;
+}
+
 static int __imm_attach(struct parport *pb)
 {
 	struct Scsi_Host *host;
-	imm_struct *dev;
+	imm_struct *dev, *temp;
 	DECLARE_WAIT_QUEUE_HEAD_ONSTACK(waiting);
 	DEFINE_WAIT(wait);
 	int ports;
 	int modes, ppb;
 	int err = -ENOMEM;
+	struct pardev_cb imm_cb;
 
 	init_waitqueue_head(&waiting);
 
@@ -1141,9 +1167,15 @@ static int __imm_attach(struct parport *pb)
 	dev->mode = IMM_AUTODETECT;
 	INIT_LIST_HEAD(&dev->list);
 
-	dev->dev = parport_register_device(pb, "imm", NULL, imm_wakeup,
-						NULL, 0, dev);
+	temp = find_parent();
+	if (temp)
+		dev->dev_no = temp->dev_no + 1;
+
+	memset(&imm_cb, 0, sizeof(imm_cb));
+	imm_cb.private = dev;
+	imm_cb.wakeup = imm_wakeup;
 
+	dev->dev = parport_register_dev_model(pb, "imm", &imm_cb, dev->dev_no);
 	if (!dev->dev)
 		goto out;
 
@@ -1207,7 +1239,10 @@ static int __imm_attach(struct parport *pb)
 	host->unique_id = pb->number;
 	*(imm_struct **)&host->hostdata = dev;
 	dev->host = host;
-	list_add_tail(&dev->list, &imm_hosts);
+	if (!temp)
+		list_add_tail(&dev->list, &imm_hosts);
+	else
+		list_add_tail(&dev->list, &temp->list);
 	err = scsi_add_host(host, NULL);
 	if (err)
 		goto out2;
@@ -1245,9 +1280,10 @@ static void imm_detach(struct parport *pb)
 }
 
 static struct parport_driver imm_driver = {
-	.name	= "imm",
-	.attach	= imm_attach,
-	.detach	= imm_detach,
+	.name		= "imm",
+	.match_port	= imm_attach,
+	.detach		= imm_detach,
+	.devmodel	= true,
 };
 
 static int __init imm_driver_init(void)
diff --git a/drivers/scsi/initio.c b/drivers/scsi/initio.c
index 6a926bae76b2..7a91cf3ff173 100644
--- a/drivers/scsi/initio.c
+++ b/drivers/scsi/initio.c
@@ -110,11 +110,6 @@
 #define i91u_MAXQUEUE		2
 #define i91u_REVID "Initio INI-9X00U/UW SCSI device driver; Revision: 1.04a"
 
-#define I950_DEVICE_ID	0x9500	/* Initio's inic-950 product ID   */
-#define I940_DEVICE_ID	0x9400	/* Initio's inic-940 product ID   */
-#define I935_DEVICE_ID	0x9401	/* Initio's inic-935 product ID   */
-#define I920_DEVICE_ID	0x0002	/* Initio's other product ID      */
-
 #ifdef DEBUG_i91u
 static unsigned int i91u_debug = DEBUG_DEFAULT;
 #endif
@@ -127,17 +122,6 @@ static int setup_debug = 0;
 
 static void i91uSCBPost(u8 * pHcb, u8 * pScb);
 
-/* PCI Devices supported by this driver */
-static struct pci_device_id i91u_pci_devices[] = {
-	{ PCI_VENDOR_ID_INIT,  I950_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{ PCI_VENDOR_ID_INIT,  I940_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{ PCI_VENDOR_ID_INIT,  I935_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{ PCI_VENDOR_ID_INIT,  I920_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{ PCI_VENDOR_ID_DOMEX, I920_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
-	{ }
-};
-MODULE_DEVICE_TABLE(pci, i91u_pci_devices);
-
 #define DEBUG_INTERRUPT 0
 #define DEBUG_QUEUE     0
 #define DEBUG_STATE     0
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 536cd5a80422..d6a691e27d33 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -3638,7 +3638,7 @@ static struct device_attribute ipr_ioa_reset_attr = {
 	.store = ipr_store_reset_adapter
 };
 
-static int ipr_iopoll(struct blk_iopoll *iop, int budget);
+static int ipr_iopoll(struct irq_poll *iop, int budget);
  /**
  * ipr_show_iopoll_weight - Show ipr polling mode
  * @dev:	class device struct
@@ -3681,34 +3681,33 @@ static ssize_t ipr_store_iopoll_weight(struct device *dev,
 	int i;
 
 	if (!ioa_cfg->sis64) {
-		dev_info(&ioa_cfg->pdev->dev, "blk-iopoll not supported on this adapter\n");
+		dev_info(&ioa_cfg->pdev->dev, "irq_poll not supported on this adapter\n");
 		return -EINVAL;
 	}
 	if (kstrtoul(buf, 10, &user_iopoll_weight))
 		return -EINVAL;
 
 	if (user_iopoll_weight > 256) {
-		dev_info(&ioa_cfg->pdev->dev, "Invalid blk-iopoll weight. It must be less than 256\n");
+		dev_info(&ioa_cfg->pdev->dev, "Invalid irq_poll weight. It must be less than 256\n");
 		return -EINVAL;
 	}
 
 	if (user_iopoll_weight == ioa_cfg->iopoll_weight) {
-		dev_info(&ioa_cfg->pdev->dev, "Current blk-iopoll weight has the same weight\n");
+		dev_info(&ioa_cfg->pdev->dev, "Current irq_poll weight has the same weight\n");
 		return strlen(buf);
 	}
 
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
 		for (i = 1; i < ioa_cfg->hrrq_num; i++)
-			blk_iopoll_disable(&ioa_cfg->hrrq[i].iopoll);
+			irq_poll_disable(&ioa_cfg->hrrq[i].iopoll);
 	}
 
 	spin_lock_irqsave(shost->host_lock, lock_flags);
 	ioa_cfg->iopoll_weight = user_iopoll_weight;
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
 		for (i = 1; i < ioa_cfg->hrrq_num; i++) {
-			blk_iopoll_init(&ioa_cfg->hrrq[i].iopoll,
+			irq_poll_init(&ioa_cfg->hrrq[i].iopoll,
 					ioa_cfg->iopoll_weight, ipr_iopoll);
-			blk_iopoll_enable(&ioa_cfg->hrrq[i].iopoll);
 		}
 	}
 	spin_unlock_irqrestore(shost->host_lock, lock_flags);
@@ -4003,13 +4002,17 @@ static ssize_t ipr_store_update_fw(struct device *dev,
 	struct ipr_sglist *sglist;
 	char fname[100];
 	char *src;
-	int len, result, dnld_size;
+	char *endline;
+	int result, dnld_size;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
 
-	len = snprintf(fname, 99, "%s", buf);
-	fname[len-1] = '\0';
+	snprintf(fname, sizeof(fname), "%s", buf);
+
+	endline = strchr(fname, '\n');
+	if (endline)
+		*endline = '\0';
 
 	if (request_firmware(&fw_entry, fname, &ioa_cfg->pdev->dev)) {
 		dev_err(&ioa_cfg->pdev->dev, "Firmware file %s not found\n", fname);
@@ -5569,7 +5572,7 @@ static int ipr_process_hrrq(struct ipr_hrr_queue *hrr_queue, int budget,
 	return num_hrrq;
 }
 
-static int ipr_iopoll(struct blk_iopoll *iop, int budget)
+static int ipr_iopoll(struct irq_poll *iop, int budget)
 {
 	struct ipr_ioa_cfg *ioa_cfg;
 	struct ipr_hrr_queue *hrrq;
@@ -5585,7 +5588,7 @@ static int ipr_iopoll(struct blk_iopoll *iop, int budget)
 	completed_ops = ipr_process_hrrq(hrrq, budget, &doneq);
 
 	if (completed_ops < budget)
-		blk_iopoll_complete(iop);
+		irq_poll_complete(iop);
 	spin_unlock_irqrestore(hrrq->lock, hrrq_flags);
 
 	list_for_each_entry_safe(ipr_cmd, temp, &doneq, queue) {
@@ -5693,8 +5696,7 @@ static irqreturn_t ipr_isr_mhrrq(int irq, void *devp)
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
 		if ((be32_to_cpu(*hrrq->hrrq_curr) & IPR_HRRQ_TOGGLE_BIT) ==
 		       hrrq->toggle_bit) {
-			if (!blk_iopoll_sched_prep(&hrrq->iopoll))
-				blk_iopoll_sched(&hrrq->iopoll);
+			irq_poll_sched(&hrrq->iopoll);
 			spin_unlock_irqrestore(hrrq->lock, hrrq_flags);
 			return IRQ_HANDLED;
 		}
@@ -10405,9 +10407,8 @@ static int ipr_probe(struct pci_dev *pdev, const struct pci_device_id *dev_id)
 
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
 		for (i = 1; i < ioa_cfg->hrrq_num; i++) {
-			blk_iopoll_init(&ioa_cfg->hrrq[i].iopoll,
+			irq_poll_init(&ioa_cfg->hrrq[i].iopoll,
 					ioa_cfg->iopoll_weight, ipr_iopoll);
-			blk_iopoll_enable(&ioa_cfg->hrrq[i].iopoll);
 		}
 	}
 
@@ -10436,7 +10437,7 @@ static void ipr_shutdown(struct pci_dev *pdev)
 	if (ioa_cfg->iopoll_weight && ioa_cfg->sis64 && ioa_cfg->nvectors > 1) {
 		ioa_cfg->iopoll_weight = 0;
 		for (i = 1; i < ioa_cfg->hrrq_num; i++)
-			blk_iopoll_disable(&ioa_cfg->hrrq[i].iopoll);
+			irq_poll_disable(&ioa_cfg->hrrq[i].iopoll);
 	}
 
 	while (ioa_cfg->in_reset_reload) {
diff --git a/drivers/scsi/ipr.h b/drivers/scsi/ipr.h
index a34c7a5a995e..56c57068300a 100644
--- a/drivers/scsi/ipr.h
+++ b/drivers/scsi/ipr.h
@@ -32,7 +32,7 @@
 #include <linux/libata.h>
 #include <linux/list.h>
 #include <linux/kref.h>
-#include <linux/blk-iopoll.h>
+#include <linux/irq_poll.h>
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
 
@@ -517,7 +517,7 @@ struct ipr_hrr_queue {
 	u8 allow_cmds:1;
 	u8 removing_ioa:1;
 
-	struct blk_iopoll iopoll;
+	struct irq_poll iopoll;
 };
 
 /* Command packet structure */
diff --git a/drivers/scsi/lpfc/lpfc.h b/drivers/scsi/lpfc/lpfc.h
index ceee9a3fd9e5..90a3ca5a4dbd 100644
--- a/drivers/scsi/lpfc/lpfc.h
+++ b/drivers/scsi/lpfc/lpfc.h
@@ -386,7 +386,6 @@ struct lpfc_vport {
 	uint32_t work_port_events; /* Timeout to be handled  */
 #define WORKER_DISC_TMO                0x1	/* vport: Discovery timeout */
 #define WORKER_ELS_TMO                 0x2	/* vport: ELS timeout */
-#define WORKER_FDMI_TMO                0x4	/* vport: FDMI timeout */
 #define WORKER_DELAYED_DISC_TMO        0x8	/* vport: delayed discovery */
 
 #define WORKER_MBOX_TMO                0x100	/* hba: MBOX timeout */
@@ -396,7 +395,6 @@ struct lpfc_vport {
 #define WORKER_RAMP_UP_QUEUE           0x1000	/* hba: Increase Q depth */
 #define WORKER_SERVICE_TXQ             0x2000	/* hba: IOCBs on the txq */
 
-	struct timer_list fc_fdmitmo;
 	struct timer_list els_tmofunc;
 	struct timer_list delayed_disc_tmo;
 
@@ -405,6 +403,7 @@ struct lpfc_vport {
 	uint8_t load_flag;
 #define FC_LOADING		0x1	/* HBA in process of loading drvr */
 #define FC_UNLOADING		0x2	/* HBA in process of unloading drvr */
+#define FC_ALLOW_FDMI		0x4	/* port is ready for FDMI requests */
 	/* Vport Config Parameters */
 	uint32_t cfg_scan_down;
 	uint32_t cfg_lun_queue_depth;
@@ -414,10 +413,6 @@ struct lpfc_vport {
 	uint32_t cfg_peer_port_login;
 	uint32_t cfg_fcp_class;
 	uint32_t cfg_use_adisc;
-	uint32_t cfg_fdmi_on;
-#define LPFC_FDMI_SUPPORT	1	/* bit 0 - FDMI supported? */
-#define LPFC_FDMI_REG_DELAY	2	/* bit 1 - 60 sec registration delay */
-#define LPFC_FDMI_ALL_ATTRIB	4	/* bit 2 - register ALL attributes? */
 	uint32_t cfg_discovery_threads;
 	uint32_t cfg_log_verbose;
 	uint32_t cfg_max_luns;
@@ -443,6 +438,10 @@ struct lpfc_vport {
 	unsigned long rcv_buffer_time_stamp;
 	uint32_t vport_flag;
 #define STATIC_VPORT	1
+
+	uint16_t fdmi_num_disc;
+	uint32_t fdmi_hba_mask;
+	uint32_t fdmi_port_mask;
 };
 
 struct hbq_s {
@@ -755,6 +754,11 @@ struct lpfc_hba {
 #define LPFC_DELAY_INIT_LINK              1	/* layered driver hold off */
 #define LPFC_DELAY_INIT_LINK_INDEFINITELY 2	/* wait, manual intervention */
 	uint32_t cfg_enable_dss;
+	uint32_t cfg_fdmi_on;
+#define LPFC_FDMI_NO_SUPPORT	0	/* FDMI not supported */
+#define LPFC_FDMI_SUPPORT	1	/* FDMI supported? */
+#define LPFC_FDMI_SMART_SAN	2	/* SmartSAN supported */
+	uint32_t cfg_enable_SmartSAN;
 	lpfc_vpd_t vpd;		/* vital product data */
 
 	struct pci_dev *pcidev;
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index f6446d759d7f..343ae9482891 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -4572,19 +4572,27 @@ LPFC_ATTR_R(multi_ring_type, FC_TYPE_IP, 1,
 	     255, "Identifies TYPE for additional ring configuration");
 
 /*
-# lpfc_fdmi_on: controls FDMI support.
-#               Set                NOT Set
-#       bit 0 = FDMI support       no FDMI support
-#           LPFC_FDMI_SUPPORT just turns basic support on/off
-#       bit 1 = Register delay     no register delay  (60 seconds)
-#           LPFC_FDMI_REG_DELAY	60 sec registration delay after FDMI login
-#       bit 2 = All attributes     Use a attribute subset
-#           LPFC_FDMI_ALL_ATTRIB applies to both port and HBA attributes
-#           Port attrutes subset: 1 thru 6 OR all: 1 thru 0xd 0x101 0x102 0x103
-#           HBA attributes subset: 1 thru 0xb OR all: 1 thru 0xc
-# Value range [0,7]. Default value is 0.
+# lpfc_enable_SmartSAN: Sets up FDMI support for SmartSAN
+#       0  = SmartSAN functionality disabled (default)
+#       1  = SmartSAN functionality enabled
+# This parameter will override the value of lpfc_fdmi_on module parameter.
+# Value range is [0,1]. Default value is 0.
 */
-LPFC_VPORT_ATTR_RW(fdmi_on, 0, 0, 7, "Enable FDMI support");
+LPFC_ATTR_R(enable_SmartSAN, 0, 0, 1, "Enable SmartSAN functionality");
+
+/*
+# lpfc_fdmi_on: Controls FDMI support.
+#       0       No FDMI support (default)
+#       1       Traditional FDMI support
+#       2       Smart SAN support
+# If lpfc_enable_SmartSAN is set 1, the driver sets lpfc_fdmi_on to value 2
+# overwriting the current value.  If lpfc_enable_SmartSAN is set 0, the
+# driver uses the current value of lpfc_fdmi_on provided it has value 0 or 1.
+# A value of 2 with lpfc_enable_SmartSAN set to 0 causes the driver to
+# set lpfc_fdmi_on back to 1.
+# Value range [0,2]. Default value is 0.
+*/
+LPFC_ATTR_R(fdmi_on, 0, 0, 2, "Enable FDMI support");
 
 /*
 # Specifies the maximum number of ELS cmds we can have outstanding (for
@@ -4815,6 +4823,7 @@ struct device_attribute *lpfc_hba_attrs[] = {
 	&dev_attr_lpfc_multi_ring_rctl,
 	&dev_attr_lpfc_multi_ring_type,
 	&dev_attr_lpfc_fdmi_on,
+	&dev_attr_lpfc_enable_SmartSAN,
 	&dev_attr_lpfc_max_luns,
 	&dev_attr_lpfc_enable_npiv,
 	&dev_attr_lpfc_fcf_failover_policy,
@@ -4887,7 +4896,6 @@ struct device_attribute *lpfc_vport_attrs[] = {
 	&dev_attr_lpfc_fcp_class,
 	&dev_attr_lpfc_use_adisc,
 	&dev_attr_lpfc_first_burst_size,
-	&dev_attr_lpfc_fdmi_on,
 	&dev_attr_lpfc_max_luns,
 	&dev_attr_nport_evt_cnt,
 	&dev_attr_npiv_info,
@@ -5247,7 +5255,7 @@ lpfc_get_host_speed(struct Scsi_Host *shost)
 
 	spin_lock_irq(shost->host_lock);
 
-	if (lpfc_is_link_up(phba)) {
+	if ((lpfc_is_link_up(phba)) && (!(phba->hba_flag & HBA_FCOE_MODE))) {
 		switch(phba->fc_linkspeed) {
 		case LPFC_LINK_SPEED_1GHZ:
 			fc_host_speed(shost) = FC_PORTSPEED_1GBIT;
@@ -5826,6 +5834,8 @@ lpfc_get_cfgparam(struct lpfc_hba *phba)
 	lpfc_enable_npiv_init(phba, lpfc_enable_npiv);
 	lpfc_fcf_failover_policy_init(phba, lpfc_fcf_failover_policy);
 	lpfc_enable_rrq_init(phba, lpfc_enable_rrq);
+	lpfc_fdmi_on_init(phba, lpfc_fdmi_on);
+	lpfc_enable_SmartSAN_init(phba, lpfc_enable_SmartSAN);
 	lpfc_use_msi_init(phba, lpfc_use_msi);
 	lpfc_fcp_imax_init(phba, lpfc_fcp_imax);
 	lpfc_fcp_cpu_map_init(phba, lpfc_fcp_cpu_map);
@@ -5846,6 +5856,15 @@ lpfc_get_cfgparam(struct lpfc_hba *phba)
 		phba->cfg_poll = 0;
 	else
 		phba->cfg_poll = lpfc_poll;
+
+	/* Ensure fdmi_on and enable_SmartSAN don't conflict */
+	if (phba->cfg_enable_SmartSAN) {
+		phba->cfg_fdmi_on = LPFC_FDMI_SMART_SAN;
+	} else {
+		if (phba->cfg_fdmi_on == LPFC_FDMI_SMART_SAN)
+			phba->cfg_fdmi_on = LPFC_FDMI_SUPPORT;
+	}
+
 	phba->cfg_soft_wwnn = 0L;
 	phba->cfg_soft_wwpn = 0L;
 	lpfc_sg_seg_cnt_init(phba, lpfc_sg_seg_cnt);
@@ -5879,7 +5898,6 @@ lpfc_get_vport_cfgparam(struct lpfc_vport *vport)
 	lpfc_use_adisc_init(vport, lpfc_use_adisc);
 	lpfc_first_burst_size_init(vport, lpfc_first_burst_size);
 	lpfc_max_scsicmpl_time_init(vport, lpfc_max_scsicmpl_time);
-	lpfc_fdmi_on_init(vport, lpfc_fdmi_on);
 	lpfc_discovery_threads_init(vport, lpfc_discovery_threads);
 	lpfc_max_luns_init(vport, lpfc_max_luns);
 	lpfc_scan_down_init(vport, lpfc_scan_down);
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index b0e6fe46448d..4e55b35180a4 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -72,6 +72,7 @@ void lpfc_cancel_all_vport_retry_delay_timer(struct lpfc_hba *);
 void lpfc_retry_pport_discovery(struct lpfc_hba *);
 void lpfc_release_rpi(struct lpfc_hba *, struct lpfc_vport *, uint16_t);
 
+void lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_dflt_rpi(struct lpfc_hba *, LPFC_MBOXQ_t *);
 void lpfc_mbx_cmpl_fabric_reg_login(struct lpfc_hba *, LPFC_MBOXQ_t *);
@@ -167,9 +168,8 @@ void lpfc_ct_unsol_event(struct lpfc_hba *, struct lpfc_sli_ring *,
 			 struct lpfc_iocbq *);
 int lpfc_ct_handle_unsol_abort(struct lpfc_hba *, struct hbq_dmabuf *);
 int lpfc_ns_cmd(struct lpfc_vport *, int, uint8_t, uint32_t);
-int lpfc_fdmi_cmd(struct lpfc_vport *, struct lpfc_nodelist *, int);
-void lpfc_fdmi_tmo(unsigned long);
-void lpfc_fdmi_timeout_handler(struct lpfc_vport *);
+int lpfc_fdmi_cmd(struct lpfc_vport *, struct lpfc_nodelist *, int, uint32_t);
+void lpfc_fdmi_num_disc_check(struct lpfc_vport *);
 void lpfc_delayed_disc_tmo(unsigned long);
 void lpfc_delayed_disc_timeout_handler(struct lpfc_vport *);
 
diff --git a/drivers/scsi/lpfc/lpfc_ct.c b/drivers/scsi/lpfc/lpfc_ct.c
index 8fded1f7605f..79e261d2a0c8 100644
--- a/drivers/scsi/lpfc/lpfc_ct.c
+++ b/drivers/scsi/lpfc/lpfc_ct.c
@@ -48,15 +48,26 @@
 #include "lpfc_vport.h"
 #include "lpfc_debugfs.h"
 
-/* FDMI Port Speed definitions */
-#define HBA_PORTSPEED_1GBIT		0x0001	/* 1 GBit/sec */
-#define HBA_PORTSPEED_2GBIT		0x0002	/* 2 GBit/sec */
-#define HBA_PORTSPEED_4GBIT		0x0008	/* 4 GBit/sec */
-#define HBA_PORTSPEED_10GBIT		0x0004	/* 10 GBit/sec */
-#define HBA_PORTSPEED_8GBIT		0x0010	/* 8 GBit/sec */
-#define HBA_PORTSPEED_16GBIT		0x0020	/* 16 GBit/sec */
-#define HBA_PORTSPEED_32GBIT		0x0040  /* 32 GBit/sec */
-#define HBA_PORTSPEED_UNKNOWN		0x0800	/* Unknown */
+/* FDMI Port Speed definitions - FC-GS-7 */
+#define HBA_PORTSPEED_1GFC		0x00000001	/* 1G FC */
+#define HBA_PORTSPEED_2GFC		0x00000002	/* 2G FC */
+#define HBA_PORTSPEED_4GFC		0x00000008	/* 4G FC */
+#define HBA_PORTSPEED_10GFC		0x00000004	/* 10G FC */
+#define HBA_PORTSPEED_8GFC		0x00000010	/* 8G FC */
+#define HBA_PORTSPEED_16GFC		0x00000020	/* 16G FC */
+#define HBA_PORTSPEED_32GFC		0x00000040	/* 32G FC */
+#define HBA_PORTSPEED_20GFC		0x00000080	/* 20G FC */
+#define HBA_PORTSPEED_40GFC		0x00000100	/* 40G FC */
+#define HBA_PORTSPEED_128GFC		0x00000200	/* 128G FC */
+#define HBA_PORTSPEED_64GFC		0x00000400	/* 64G FC */
+#define HBA_PORTSPEED_256GFC		0x00000800	/* 256G FC */
+#define HBA_PORTSPEED_UNKNOWN		0x00008000	/* Unknown */
+#define HBA_PORTSPEED_10GE		0x00010000	/* 10G E */
+#define HBA_PORTSPEED_40GE		0x00020000	/* 40G E */
+#define HBA_PORTSPEED_100GE		0x00040000	/* 100G E */
+#define HBA_PORTSPEED_25GE		0x00080000	/* 25G E */
+#define HBA_PORTSPEED_50GE		0x00100000	/* 50G E */
+#define HBA_PORTSPEED_400GE		0x00200000	/* 400G E */
 
 #define FOURBYTES	4
 
@@ -287,6 +298,17 @@ lpfc_ct_free_iocb(struct lpfc_hba *phba, struct lpfc_iocbq *ctiocb)
 	return 0;
 }
 
+/**
+ * lpfc_gen_req - Build and issue a GEN_REQUEST command  to the SLI Layer
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @bmp: Pointer to BPL for SLI command
+ * @inp: Pointer to data buffer for response data.
+ * @outp: Pointer to data buffer that hold the CT command.
+ * @cmpl: completion routine to call when command completes
+ * @ndlp: Destination NPort nodelist entry
+ *
+ * This function as the final part for issuing a CT command.
+ */
 static int
 lpfc_gen_req(struct lpfc_vport *vport, struct lpfc_dmabuf *bmp,
 	     struct lpfc_dmabuf *inp, struct lpfc_dmabuf *outp,
@@ -311,7 +333,7 @@ lpfc_gen_req(struct lpfc_vport *vport, struct lpfc_dmabuf *bmp,
 	icmd->un.genreq64.bdl.addrHigh = putPaddrHigh(bmp->phys);
 	icmd->un.genreq64.bdl.addrLow = putPaddrLow(bmp->phys);
 	icmd->un.genreq64.bdl.bdeFlags = BUFF_TYPE_BLP_64;
-	icmd->un.genreq64.bdl.bdeSize = (num_entry * sizeof (struct ulp_bde64));
+	icmd->un.genreq64.bdl.bdeSize = (num_entry * sizeof(struct ulp_bde64));
 
 	if (usr_flg)
 		geniocb->context3 = NULL;
@@ -370,6 +392,16 @@ lpfc_gen_req(struct lpfc_vport *vport, struct lpfc_dmabuf *bmp,
 	return 0;
 }
 
+/**
+ * lpfc_ct_cmd - Build and issue a CT command
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @inmp: Pointer to data buffer for response data.
+ * @bmp: Pointer to BPL for SLI command
+ * @ndlp: Destination NPort nodelist entry
+ * @cmpl: completion routine to call when command completes
+ *
+ * This function is called for issuing a CT command.
+ */
 static int
 lpfc_ct_cmd(struct lpfc_vport *vport, struct lpfc_dmabuf *inmp,
 	    struct lpfc_dmabuf *bmp, struct lpfc_nodelist *ndlp,
@@ -453,7 +485,7 @@ lpfc_ns_rsp(struct lpfc_vport *vport, struct lpfc_dmabuf *mp, uint32_t Size)
 			Cnt -= 16;	/* subtract length of CT header */
 
 		/* Loop through entire NameServer list of DIDs */
-		while (Cnt >= sizeof (uint32_t)) {
+		while (Cnt >= sizeof(uint32_t)) {
 			/* Get next DID from NameServer List */
 			CTentry = *ctptr++;
 			Did = ((be32_to_cpu(CTentry)) & Mask_DID);
@@ -558,7 +590,7 @@ lpfc_ns_rsp(struct lpfc_vport *vport, struct lpfc_dmabuf *mp, uint32_t Size)
 			}
 			if (CTentry & (cpu_to_be32(SLI_CT_LAST_ENTRY)))
 				goto nsout1;
-			Cnt -= sizeof (uint32_t);
+			Cnt -= sizeof(uint32_t);
 		}
 		ctptr = NULL;
 
@@ -1146,7 +1178,7 @@ lpfc_ns_cmd(struct lpfc_vport *vport, int cmdcode,
 
 	/* fill in BDEs for command */
 	/* Allocate buffer for command payload */
-	mp = kmalloc(sizeof (struct lpfc_dmabuf), GFP_KERNEL);
+	mp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
 	if (!mp) {
 		rc=2;
 		goto ns_cmd_exit;
@@ -1160,7 +1192,7 @@ lpfc_ns_cmd(struct lpfc_vport *vport, int cmdcode,
 	}
 
 	/* Allocate buffer for Buffer ptr list */
-	bmp = kmalloc(sizeof (struct lpfc_dmabuf), GFP_KERNEL);
+	bmp = kmalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
 	if (!bmp) {
 		rc=4;
 		goto ns_cmd_free_mpvirt;
@@ -1204,7 +1236,7 @@ lpfc_ns_cmd(struct lpfc_vport *vport, int cmdcode,
 	bpl->tus.w = le32_to_cpu(bpl->tus.w);
 
 	CtReq = (struct lpfc_sli_ct_request *) mp->virt;
-	memset(CtReq, 0, sizeof (struct lpfc_sli_ct_request));
+	memset(CtReq, 0, sizeof(struct lpfc_sli_ct_request));
 	CtReq->RevisionId.bits.Revision = SLI_CT_REVISION;
 	CtReq->RevisionId.bits.InId = 0;
 	CtReq->FsType = SLI_CT_DIRECTORY_SERVICE;
@@ -1244,7 +1276,7 @@ lpfc_ns_cmd(struct lpfc_vport *vport, int cmdcode,
 		    cpu_to_be16(SLI_CTNS_RNN_ID);
 		CtReq->un.rnn.PortId = cpu_to_be32(vport->fc_myDID);
 		memcpy(CtReq->un.rnn.wwnn,  &vport->fc_nodename,
-		       sizeof (struct lpfc_name));
+		       sizeof(struct lpfc_name));
 		cmpl = lpfc_cmpl_ct_cmd_rnn_id;
 		break;
 
@@ -1264,7 +1296,7 @@ lpfc_ns_cmd(struct lpfc_vport *vport, int cmdcode,
 		CtReq->CommandResponse.bits.CmdRsp =
 		    cpu_to_be16(SLI_CTNS_RSNN_NN);
 		memcpy(CtReq->un.rsnn.wwnn, &vport->fc_nodename,
-		       sizeof (struct lpfc_name));
+		       sizeof(struct lpfc_name));
 		size = sizeof(CtReq->un.rsnn.symbname);
 		CtReq->un.rsnn.len =
 			lpfc_vport_symbolic_node_name(vport,
@@ -1319,20 +1351,29 @@ ns_cmd_exit:
 	return 1;
 }
 
+/**
+ * lpfc_cmpl_ct_disc_fdmi - Handle a discovery FDMI completion
+ * @phba: Pointer to HBA context object.
+ * @cmdiocb: Pointer to the command IOCBQ.
+ * @rspiocb: Pointer to the response IOCBQ.
+ *
+ * This function to handle the completion of a driver initiated FDMI
+ * CT command issued during discovery.
+ */
 static void
-lpfc_cmpl_ct_cmd_fdmi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
-		      struct lpfc_iocbq * rspiocb)
+lpfc_cmpl_ct_disc_fdmi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
+		       struct lpfc_iocbq *rspiocb)
 {
+	struct lpfc_vport *vport = cmdiocb->vport;
 	struct lpfc_dmabuf *inp = cmdiocb->context1;
 	struct lpfc_dmabuf *outp = cmdiocb->context2;
-	struct lpfc_sli_ct_request *CTrsp = outp->virt;
 	struct lpfc_sli_ct_request *CTcmd = inp->virt;
-	struct lpfc_nodelist *ndlp;
+	struct lpfc_sli_ct_request *CTrsp = outp->virt;
 	uint16_t fdmi_cmd = CTcmd->CommandResponse.bits.CmdRsp;
 	uint16_t fdmi_rsp = CTrsp->CommandResponse.bits.CmdRsp;
-	struct lpfc_vport *vport = cmdiocb->vport;
 	IOCB_t *irsp = &rspiocb->iocb;
-	uint32_t latt;
+	struct lpfc_nodelist *ndlp;
+	uint32_t latt, cmd, err;
 
 	latt = lpfc_els_chk_latt(vport);
 	lpfc_debugfs_disc_trc(vport, LPFC_DISC_TRC_CT,
@@ -1340,91 +1381,1115 @@ lpfc_cmpl_ct_cmd_fdmi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 		irsp->ulpStatus, irsp->un.ulpWord[4], latt);
 
 	if (latt || irsp->ulpStatus) {
+
+		/* Look for a retryable error */
+		if (irsp->ulpStatus == IOSTAT_LOCAL_REJECT) {
+			switch ((irsp->un.ulpWord[4] & IOERR_PARAM_MASK)) {
+			case IOERR_SLI_ABORTED:
+			case IOERR_ABORT_IN_PROGRESS:
+			case IOERR_SEQUENCE_TIMEOUT:
+			case IOERR_ILLEGAL_FRAME:
+			case IOERR_NO_RESOURCES:
+			case IOERR_ILLEGAL_COMMAND:
+				cmdiocb->retry++;
+				if (cmdiocb->retry >= LPFC_FDMI_MAX_RETRY)
+					break;
+
+				/* Retry the same FDMI command */
+				err = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING,
+							  cmdiocb, 0);
+				if (err == IOCB_ERROR)
+					break;
+				return;
+			default:
+				break;
+			}
+		}
+
 		lpfc_printf_vlog(vport, KERN_INFO, LOG_DISCOVERY,
 				 "0229 FDMI cmd %04x failed, latt = %d "
 				 "ulpStatus: x%x, rid x%x\n",
 				 be16_to_cpu(fdmi_cmd), latt, irsp->ulpStatus,
 				 irsp->un.ulpWord[4]);
-		goto fail_out;
 	}
+	lpfc_ct_free_iocb(phba, cmdiocb);
 
 	ndlp = lpfc_findnode_did(vport, FDMI_DID);
 	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
-		goto fail_out;
+		return;
 
+	/* Check for a CT LS_RJT response */
+	cmd =  be16_to_cpu(fdmi_cmd);
 	if (fdmi_rsp == cpu_to_be16(SLI_CT_RESPONSE_FS_RJT)) {
 		/* FDMI rsp failed */
 		lpfc_printf_vlog(vport, KERN_INFO, LOG_DISCOVERY,
-				 "0220 FDMI rsp failed Data: x%x\n",
-				 be16_to_cpu(fdmi_cmd));
+				 "0220 FDMI cmd failed FS_RJT Data: x%x", cmd);
+
+		/* Should we fallback to FDMI-2 / FDMI-1 ? */
+		switch (cmd) {
+		case SLI_MGMT_RHBA:
+			if (vport->fdmi_hba_mask == LPFC_FDMI2_HBA_ATTR) {
+				/* Fallback to FDMI-1 */
+				vport->fdmi_hba_mask = LPFC_FDMI1_HBA_ATTR;
+				vport->fdmi_port_mask = LPFC_FDMI1_PORT_ATTR;
+				/* Start over */
+				lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DHBA, 0);
+			}
+			return;
+
+		case SLI_MGMT_RPRT:
+			if (vport->fdmi_port_mask == LPFC_FDMI2_PORT_ATTR) {
+				/* Fallback to FDMI-1 */
+				vport->fdmi_port_mask = LPFC_FDMI1_PORT_ATTR;
+				/* Start over */
+				lpfc_fdmi_cmd(vport, ndlp, cmd, 0);
+			}
+			if (vport->fdmi_port_mask == LPFC_FDMI2_SMART_ATTR) {
+				vport->fdmi_port_mask = LPFC_FDMI2_PORT_ATTR;
+				/* Retry the same command */
+				lpfc_fdmi_cmd(vport, ndlp, cmd, 0);
+			}
+			return;
+
+		case SLI_MGMT_RPA:
+			if (vport->fdmi_port_mask == LPFC_FDMI2_PORT_ATTR) {
+				/* Fallback to FDMI-1 */
+				vport->fdmi_hba_mask = LPFC_FDMI1_HBA_ATTR;
+				vport->fdmi_port_mask = LPFC_FDMI1_PORT_ATTR;
+				/* Start over */
+				lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DHBA, 0);
+			}
+			if (vport->fdmi_port_mask == LPFC_FDMI2_SMART_ATTR) {
+				vport->fdmi_port_mask = LPFC_FDMI2_PORT_ATTR;
+				/* Retry the same command */
+				lpfc_fdmi_cmd(vport, ndlp, cmd, 0);
+			}
+			return;
+		}
 	}
 
-fail_out:
-	lpfc_ct_free_iocb(phba, cmdiocb);
+	/*
+	 * On success, need to cycle thru FDMI registration for discovery
+	 * DHBA -> DPRT -> RHBA -> RPA  (physical port)
+	 * DPRT -> RPRT (vports)
+	 */
+	switch (cmd) {
+	case SLI_MGMT_RHBA:
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RPA, 0);
+		break;
+
+	case SLI_MGMT_DHBA:
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DPRT, 0);
+		break;
+
+	case SLI_MGMT_DPRT:
+		if (vport->port_type == LPFC_PHYSICAL_PORT)
+			lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RHBA, 0);
+		else
+			lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RPRT, 0);
+		break;
+	}
+	return;
 }
 
-static void
-lpfc_cmpl_ct_disc_fdmi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
-		       struct lpfc_iocbq *rspiocb)
+
+/**
+ * lpfc_fdmi_num_disc_check - Check how many mapped NPorts we are connected to
+ * @vport: pointer to a host virtual N_Port data structure.
+ *
+ * Called from hbeat timeout routine to check if the number of discovered
+ * ports has changed. If so, re-register thar port Attribute.
+ */
+void
+lpfc_fdmi_num_disc_check(struct lpfc_vport *vport)
 {
-	struct lpfc_vport *vport = cmdiocb->vport;
-	struct lpfc_dmabuf *inp = cmdiocb->context1;
-	struct lpfc_sli_ct_request *CTcmd = inp->virt;
-	uint16_t fdmi_cmd = CTcmd->CommandResponse.bits.CmdRsp;
+	struct lpfc_hba *phba = vport->phba;
 	struct lpfc_nodelist *ndlp;
+	uint16_t cnt;
+
+	if (!lpfc_is_link_up(phba))
+		return;
+
+	if (!(vport->fdmi_port_mask & LPFC_FDMI_PORT_ATTR_num_disc))
+		return;
 
-	lpfc_cmpl_ct_cmd_fdmi(phba, cmdiocb, rspiocb);
+	cnt = lpfc_find_map_node(vport);
+	if (cnt == vport->fdmi_num_disc)
+		return;
 
 	ndlp = lpfc_findnode_did(vport, FDMI_DID);
 	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
 		return;
 
-	/*
-	 * Need to cycle thru FDMI registration for discovery
-	 * DHBA -> DPRT -> RHBA -> RPA
-	 */
-	switch (be16_to_cpu(fdmi_cmd)) {
-	case SLI_MGMT_RHBA:
-		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RPA);
-		break;
+	if (vport->port_type == LPFC_PHYSICAL_PORT) {
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RPA,
+			      LPFC_FDMI_PORT_ATTR_num_disc);
+	} else {
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RPRT,
+			      LPFC_FDMI_PORT_ATTR_num_disc);
+	}
+}
 
-	case SLI_MGMT_DHBA:
-		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DPRT);
-		break;
+/* Routines for all individual HBA attributes */
+int
+lpfc_fdmi_hba_attr_wwnn(struct lpfc_vport *vport, struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
 
-	case SLI_MGMT_DPRT:
-		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_RHBA);
-		break;
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, sizeof(struct lpfc_name));
+
+	memcpy(&ae->un.AttrWWN, &vport->fc_sparam.nodeName,
+	       sizeof(struct lpfc_name));
+	size = FOURBYTES + sizeof(struct lpfc_name);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_NODENAME);
+	return size;
+}
+int
+lpfc_fdmi_hba_attr_manufacturer(struct lpfc_vport *vport,
+				struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString,
+		"Emulex Corporation",
+		       sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_MANUFACTURER);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_sn(struct lpfc_vport *vport, struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, phba->SerialNumber,
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_SERIAL_NUMBER);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_model(struct lpfc_vport *vport,
+			 struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, phba->ModelName,
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString, sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_MODEL);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_description(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, phba->ModelDesc,
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+				  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_MODEL_DESCRIPTION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_hdw_ver(struct lpfc_vport *vport,
+			   struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	lpfc_vpd_t *vp = &phba->vpd;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t i, j, incr, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	/* Convert JEDEC ID to ascii for hardware version */
+	incr = vp->rev.biuRev;
+	for (i = 0; i < 8; i++) {
+		j = (incr & 0xf);
+		if (j <= 9)
+			ae->un.AttrString[7 - i] =
+			    (char)((uint8_t) 0x30 +
+				   (uint8_t) j);
+		else
+			ae->un.AttrString[7 - i] =
+			    (char)((uint8_t) 0x61 +
+				   (uint8_t) (j - 10));
+		incr = (incr >> 4);
 	}
+	size = FOURBYTES + 8;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_HARDWARE_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_drvr_ver(struct lpfc_vport *vport,
+			    struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, lpfc_release_version,
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_DRIVER_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_rom_ver(struct lpfc_vport *vport,
+			   struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	if (phba->sli_rev == LPFC_SLI_REV4)
+		lpfc_decode_firmware_rev(phba, ae->un.AttrString, 1);
+	else
+		strncpy(ae->un.AttrString, phba->OptionROMVersion,
+			sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_OPTION_ROM_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_fmw_ver(struct lpfc_vport *vport,
+			   struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	lpfc_decode_firmware_rev(phba, ae->un.AttrString, 1);
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_FIRMWARE_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_os_ver(struct lpfc_vport *vport,
+			  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	snprintf(ae->un.AttrString, sizeof(ae->un.AttrString), "%s %s %s",
+		 init_utsname()->sysname,
+		 init_utsname()->release,
+		 init_utsname()->version);
+
+	len = strnlen(ae->un.AttrString, sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_OS_NAME_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_ct_len(struct lpfc_vport *vport,
+			  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	ae->un.AttrInt =  cpu_to_be32(LPFC_MAX_CT_SIZE);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_MAX_CT_PAYLOAD_LEN);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_symbolic_name(struct lpfc_vport *vport,
+				 struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	len = lpfc_vport_symbolic_node_name(vport,
+				ae->un.AttrString, 256);
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_SYM_NODENAME);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_vendor_info(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	/* Nothing is defined for this currently */
+	ae->un.AttrInt =  cpu_to_be32(0);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_VENDOR_INFO);
+	return size;
 }
 
+int
+lpfc_fdmi_hba_attr_num_ports(struct lpfc_vport *vport,
+			     struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	/* Each driver instance corresponds to a single port */
+	ae->un.AttrInt =  cpu_to_be32(1);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_NUM_PORTS);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_fabric_wwnn(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, sizeof(struct lpfc_name));
+
+	memcpy(&ae->un.AttrWWN, &vport->fabric_nodename,
+	       sizeof(struct lpfc_name));
+	size = FOURBYTES + sizeof(struct lpfc_name);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_FABRIC_WWNN);
+	return size;
+}
 
 int
-lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
+lpfc_fdmi_hba_attr_bios_ver(struct lpfc_vport *vport,
+			    struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	lpfc_decode_firmware_rev(phba, ae->un.AttrString, 1);
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_BIOS_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_bios_state(struct lpfc_vport *vport,
+			      struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	/* Driver doesn't have access to this information */
+	ae->un.AttrInt =  cpu_to_be32(0);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_BIOS_STATE);
+	return size;
+}
+
+int
+lpfc_fdmi_hba_attr_vendor_id(struct lpfc_vport *vport,
+			     struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, "EMULEX",
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RHBA_VENDOR_ID);
+	return size;
+}
+
+/* Routines for all individual PORT attributes */
+int
+lpfc_fdmi_port_attr_fc4type(struct lpfc_vport *vport,
+			    struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 32);
+
+	ae->un.AttrTypes[3] = 0x02; /* Type 1 - ELS */
+	ae->un.AttrTypes[2] = 0x01; /* Type 8 - FCP */
+	ae->un.AttrTypes[7] = 0x01; /* Type 32 - CT */
+	size = FOURBYTES + 32;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SUPPORTED_FC4_TYPES);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_support_speed(struct lpfc_vport *vport,
+				  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba   *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	ae->un.AttrInt = 0;
+	if (!(phba->hba_flag & HBA_FCOE_MODE)) {
+		if (phba->lmt & LMT_32Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_32GFC;
+		if (phba->lmt & LMT_16Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_16GFC;
+		if (phba->lmt & LMT_10Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_10GFC;
+		if (phba->lmt & LMT_8Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_8GFC;
+		if (phba->lmt & LMT_4Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_4GFC;
+		if (phba->lmt & LMT_2Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_2GFC;
+		if (phba->lmt & LMT_1Gb)
+			ae->un.AttrInt |= HBA_PORTSPEED_1GFC;
+	} else {
+		/* FCoE links support only one speed */
+		switch (phba->fc_linkspeed) {
+		case LPFC_ASYNC_LINK_SPEED_10GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_10GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_25GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_25GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_40GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_40GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_100GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_100GE;
+			break;
+		}
+	}
+	ae->un.AttrInt = cpu_to_be32(ae->un.AttrInt);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SUPPORTED_SPEED);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_speed(struct lpfc_vport *vport,
+			  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba   *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	if (!(phba->hba_flag & HBA_FCOE_MODE)) {
+		switch (phba->fc_linkspeed) {
+		case LPFC_LINK_SPEED_1GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_1GFC;
+			break;
+		case LPFC_LINK_SPEED_2GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_2GFC;
+			break;
+		case LPFC_LINK_SPEED_4GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_4GFC;
+			break;
+		case LPFC_LINK_SPEED_8GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_8GFC;
+			break;
+		case LPFC_LINK_SPEED_10GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_10GFC;
+			break;
+		case LPFC_LINK_SPEED_16GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_16GFC;
+			break;
+		case LPFC_LINK_SPEED_32GHZ:
+			ae->un.AttrInt = HBA_PORTSPEED_32GFC;
+			break;
+		default:
+			ae->un.AttrInt = HBA_PORTSPEED_UNKNOWN;
+			break;
+		}
+	} else {
+		switch (phba->fc_linkspeed) {
+		case LPFC_ASYNC_LINK_SPEED_10GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_10GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_25GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_25GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_40GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_40GE;
+			break;
+		case LPFC_ASYNC_LINK_SPEED_100GBPS:
+			ae->un.AttrInt = HBA_PORTSPEED_100GE;
+			break;
+		default:
+			ae->un.AttrInt = HBA_PORTSPEED_UNKNOWN;
+			break;
+		}
+	}
+
+	ae->un.AttrInt = cpu_to_be32(ae->un.AttrInt);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_PORT_SPEED);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_max_frame(struct lpfc_vport *vport,
+			      struct lpfc_fdmi_attr_def *ad)
+{
+	struct serv_parm *hsp;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	hsp = (struct serv_parm *)&vport->fc_sparam;
+	ae->un.AttrInt = (((uint32_t) hsp->cmn.bbRcvSizeMsb) << 8) |
+			  (uint32_t) hsp->cmn.bbRcvSizeLsb;
+	ae->un.AttrInt = cpu_to_be32(ae->un.AttrInt);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_MAX_FRAME_SIZE);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_os_devname(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	snprintf(ae->un.AttrString, sizeof(ae->un.AttrString),
+		 "/sys/class/scsi_host/host%d", shost->host_no);
+	len = strnlen((char *)ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_OS_DEVICE_NAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_host_name(struct lpfc_vport *vport,
+			      struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	snprintf(ae->un.AttrString, sizeof(ae->un.AttrString), "%s",
+		 init_utsname()->nodename);
+
+	len = strnlen(ae->un.AttrString, sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_HOST_NAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_wwnn(struct lpfc_vport *vport,
+			 struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0,  sizeof(struct lpfc_name));
+
+	memcpy(&ae->un.AttrWWN, &vport->fc_sparam.nodeName,
+	       sizeof(struct lpfc_name));
+	size = FOURBYTES + sizeof(struct lpfc_name);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_NODENAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_wwpn(struct lpfc_vport *vport,
+			 struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0,  sizeof(struct lpfc_name));
+
+	memcpy(&ae->un.AttrWWN, &vport->fc_sparam.portName,
+	       sizeof(struct lpfc_name));
+	size = FOURBYTES + sizeof(struct lpfc_name);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_PORTNAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_symbolic_name(struct lpfc_vport *vport,
+				  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	len = lpfc_vport_symbolic_port_name(vport, ae->un.AttrString, 256);
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SYM_PORTNAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_port_type(struct lpfc_vport *vport,
+			      struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	if (phba->fc_topology == LPFC_TOPOLOGY_LOOP)
+		ae->un.AttrInt =  cpu_to_be32(LPFC_FDMI_PORTTYPE_NLPORT);
+	else
+		ae->un.AttrInt =  cpu_to_be32(LPFC_FDMI_PORTTYPE_NPORT);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_PORT_TYPE);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_class(struct lpfc_vport *vport,
+			  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	ae->un.AttrInt = cpu_to_be32(FC_COS_CLASS2 | FC_COS_CLASS3);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SUPPORTED_CLASS);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_fabric_wwpn(struct lpfc_vport *vport,
+				struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0,  sizeof(struct lpfc_name));
+
+	memcpy(&ae->un.AttrWWN, &vport->fabric_portname,
+	       sizeof(struct lpfc_name));
+	size = FOURBYTES + sizeof(struct lpfc_name);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_FABRICNAME);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_active_fc4type(struct lpfc_vport *vport,
+				   struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 32);
+
+	ae->un.AttrTypes[3] = 0x02; /* Type 1 - ELS */
+	ae->un.AttrTypes[2] = 0x01; /* Type 8 - FCP */
+	ae->un.AttrTypes[7] = 0x01; /* Type 32 - CT */
+	size = FOURBYTES + 32;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_ACTIVE_FC4_TYPES);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_port_state(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	/* Link Up - operational */
+	ae->un.AttrInt =  cpu_to_be32(LPFC_FDMI_PORTSTATE_ONLINE);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_PORT_STATE);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_num_disc(struct lpfc_vport *vport,
+			     struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	vport->fdmi_num_disc = lpfc_find_map_node(vport);
+	ae->un.AttrInt = cpu_to_be32(vport->fdmi_num_disc);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_DISC_PORT);
+	return size;
+}
+
+int
+lpfc_fdmi_port_attr_nportid(struct lpfc_vport *vport,
+			    struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	ae->un.AttrInt =  cpu_to_be32(vport->fc_myDID);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_PORT_ID);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_service(struct lpfc_vport *vport,
+			     struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, "Smart SAN Initiator",
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_SERVICE);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_guid(struct lpfc_vport *vport,
+			  struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	memcpy(&ae->un.AttrString, &vport->fc_sparam.nodeName,
+	       sizeof(struct lpfc_name));
+	memcpy((((uint8_t *)&ae->un.AttrString) +
+		sizeof(struct lpfc_name)),
+		&vport->fc_sparam.portName, sizeof(struct lpfc_name));
+	size = FOURBYTES + (2 * sizeof(struct lpfc_name));
+	ad->AttrLen =  cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_GUID);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_version(struct lpfc_vport *vport,
+			     struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, "Smart SAN Version 1.0",
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString,
+			  sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen =  cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_VERSION);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_model(struct lpfc_vport *vport,
+			   struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t len, size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	memset(ae, 0, 256);
+
+	strncpy(ae->un.AttrString, phba->ModelName,
+		sizeof(ae->un.AttrString));
+	len = strnlen(ae->un.AttrString, sizeof(ae->un.AttrString));
+	len += (len & 3) ? (4 - (len & 3)) : 4;
+	size = FOURBYTES + len;
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_MODEL);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_port_info(struct lpfc_vport *vport,
+			       struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+
+	/* SRIOV (type 3) is not supported */
+	if (vport->vpi)
+		ae->un.AttrInt =  cpu_to_be32(2);  /* NPIV */
+	else
+		ae->un.AttrInt =  cpu_to_be32(1);  /* Physical */
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_PORT_INFO);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_qos(struct lpfc_vport *vport,
+			 struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	ae->un.AttrInt =  cpu_to_be32(0);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_QOS);
+	return size;
+}
+
+int
+lpfc_fdmi_smart_attr_security(struct lpfc_vport *vport,
+			      struct lpfc_fdmi_attr_def *ad)
+{
+	struct lpfc_fdmi_attr_entry *ae;
+	uint32_t size;
+
+	ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
+	ae->un.AttrInt =  cpu_to_be32(0);
+	size = FOURBYTES + sizeof(uint32_t);
+	ad->AttrLen = cpu_to_be16(size);
+	ad->AttrType = cpu_to_be16(RPRT_SMART_SECURITY);
+	return size;
+}
+
+/* RHBA attribute jump table */
+int (*lpfc_fdmi_hba_action[])
+	(struct lpfc_vport *vport, struct lpfc_fdmi_attr_def *ad) = {
+	/* Action routine                 Mask bit     Attribute type */
+	lpfc_fdmi_hba_attr_wwnn,	  /* bit0     RHBA_NODENAME           */
+	lpfc_fdmi_hba_attr_manufacturer,  /* bit1     RHBA_MANUFACTURER       */
+	lpfc_fdmi_hba_attr_sn,		  /* bit2     RHBA_SERIAL_NUMBER      */
+	lpfc_fdmi_hba_attr_model,	  /* bit3     RHBA_MODEL              */
+	lpfc_fdmi_hba_attr_description,	  /* bit4     RHBA_MODEL_DESCRIPTION  */
+	lpfc_fdmi_hba_attr_hdw_ver,	  /* bit5     RHBA_HARDWARE_VERSION   */
+	lpfc_fdmi_hba_attr_drvr_ver,	  /* bit6     RHBA_DRIVER_VERSION     */
+	lpfc_fdmi_hba_attr_rom_ver,	  /* bit7     RHBA_OPTION_ROM_VERSION */
+	lpfc_fdmi_hba_attr_fmw_ver,	  /* bit8     RHBA_FIRMWARE_VERSION   */
+	lpfc_fdmi_hba_attr_os_ver,	  /* bit9     RHBA_OS_NAME_VERSION    */
+	lpfc_fdmi_hba_attr_ct_len,	  /* bit10    RHBA_MAX_CT_PAYLOAD_LEN */
+	lpfc_fdmi_hba_attr_symbolic_name, /* bit11    RHBA_SYM_NODENAME       */
+	lpfc_fdmi_hba_attr_vendor_info,	  /* bit12    RHBA_VENDOR_INFO        */
+	lpfc_fdmi_hba_attr_num_ports,	  /* bit13    RHBA_NUM_PORTS          */
+	lpfc_fdmi_hba_attr_fabric_wwnn,	  /* bit14    RHBA_FABRIC_WWNN        */
+	lpfc_fdmi_hba_attr_bios_ver,	  /* bit15    RHBA_BIOS_VERSION       */
+	lpfc_fdmi_hba_attr_bios_state,	  /* bit16    RHBA_BIOS_STATE         */
+	lpfc_fdmi_hba_attr_vendor_id,	  /* bit17    RHBA_VENDOR_ID          */
+};
+
+/* RPA / RPRT attribute jump table */
+int (*lpfc_fdmi_port_action[])
+	(struct lpfc_vport *vport, struct lpfc_fdmi_attr_def *ad) = {
+	/* Action routine                   Mask bit   Attribute type */
+	lpfc_fdmi_port_attr_fc4type,        /* bit0   RPRT_SUPPORT_FC4_TYPES  */
+	lpfc_fdmi_port_attr_support_speed,  /* bit1   RPRT_SUPPORTED_SPEED    */
+	lpfc_fdmi_port_attr_speed,          /* bit2   RPRT_PORT_SPEED         */
+	lpfc_fdmi_port_attr_max_frame,      /* bit3   RPRT_MAX_FRAME_SIZE     */
+	lpfc_fdmi_port_attr_os_devname,     /* bit4   RPRT_OS_DEVICE_NAME     */
+	lpfc_fdmi_port_attr_host_name,      /* bit5   RPRT_HOST_NAME          */
+	lpfc_fdmi_port_attr_wwnn,           /* bit6   RPRT_NODENAME           */
+	lpfc_fdmi_port_attr_wwpn,           /* bit7   RPRT_PORTNAME           */
+	lpfc_fdmi_port_attr_symbolic_name,  /* bit8   RPRT_SYM_PORTNAME       */
+	lpfc_fdmi_port_attr_port_type,      /* bit9   RPRT_PORT_TYPE          */
+	lpfc_fdmi_port_attr_class,          /* bit10  RPRT_SUPPORTED_CLASS    */
+	lpfc_fdmi_port_attr_fabric_wwpn,    /* bit11  RPRT_FABRICNAME         */
+	lpfc_fdmi_port_attr_active_fc4type, /* bit12  RPRT_ACTIVE_FC4_TYPES   */
+	lpfc_fdmi_port_attr_port_state,     /* bit13  RPRT_PORT_STATE         */
+	lpfc_fdmi_port_attr_num_disc,       /* bit14  RPRT_DISC_PORT          */
+	lpfc_fdmi_port_attr_nportid,        /* bit15  RPRT_PORT_ID            */
+	lpfc_fdmi_smart_attr_service,       /* bit16  RPRT_SMART_SERVICE      */
+	lpfc_fdmi_smart_attr_guid,          /* bit17  RPRT_SMART_GUID         */
+	lpfc_fdmi_smart_attr_version,       /* bit18  RPRT_SMART_VERSION      */
+	lpfc_fdmi_smart_attr_model,         /* bit19  RPRT_SMART_MODEL        */
+	lpfc_fdmi_smart_attr_port_info,     /* bit20  RPRT_SMART_PORT_INFO    */
+	lpfc_fdmi_smart_attr_qos,           /* bit21  RPRT_SMART_QOS          */
+	lpfc_fdmi_smart_attr_security,      /* bit22  RPRT_SMART_SECURITY     */
+};
+
+/**
+ * lpfc_fdmi_cmd - Build and send a FDMI cmd to the specified NPort
+ * @vport: pointer to a host virtual N_Port data structure.
+ * @ndlp: ndlp to send FDMI cmd to (if NULL use FDMI_DID)
+ * cmdcode: FDMI command to send
+ * mask: Mask of HBA or PORT Attributes to send
+ *
+ * Builds and sends a FDMI command using the CT subsystem.
+ */
+int
+lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
+	      int cmdcode, uint32_t new_mask)
 {
 	struct lpfc_hba *phba = vport->phba;
 	struct lpfc_dmabuf *mp, *bmp;
 	struct lpfc_sli_ct_request *CtReq;
 	struct ulp_bde64 *bpl;
+	uint32_t bit_pos;
 	uint32_t size;
 	uint32_t rsp_size;
+	uint32_t mask;
 	struct lpfc_fdmi_reg_hba *rh;
 	struct lpfc_fdmi_port_entry *pe;
 	struct lpfc_fdmi_reg_portattr *pab = NULL;
 	struct lpfc_fdmi_attr_block *ab = NULL;
-	struct lpfc_fdmi_attr_entry *ae;
-	struct lpfc_fdmi_attr_def *ad;
-	void (*cmpl) (struct lpfc_hba *, struct lpfc_iocbq *,
-		      struct lpfc_iocbq *);
+	int  (*func)(struct lpfc_vport *vport, struct lpfc_fdmi_attr_def *ad);
+	void (*cmpl)(struct lpfc_hba *, struct lpfc_iocbq *,
+		     struct lpfc_iocbq *);
 
-	if (ndlp == NULL) {
-		ndlp = lpfc_findnode_did(vport, FDMI_DID);
-		if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
-			return 0;
-		cmpl = lpfc_cmpl_ct_cmd_fdmi; /* cmd interface */
-	} else {
-		cmpl = lpfc_cmpl_ct_disc_fdmi; /* called from discovery */
-	}
+	if (!ndlp || !NLP_CHK_NODE_ACT(ndlp))
+		return 0;
+
+	cmpl = lpfc_cmpl_ct_disc_fdmi; /* called from discovery */
 
 	/* fill in BDEs for command */
 	/* Allocate buffer for command payload */
@@ -1470,573 +2535,99 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 	switch (cmdcode) {
 	case SLI_MGMT_RHAT:
 	case SLI_MGMT_RHBA:
-		{
-			lpfc_vpd_t *vp = &phba->vpd;
-			uint32_t i, j, incr;
-			int len = 0;
+		rh = (struct lpfc_fdmi_reg_hba *)&CtReq->un.PortID;
+		/* HBA Identifier */
+		memcpy(&rh->hi.PortName, &phba->pport->fc_sparam.portName,
+		       sizeof(struct lpfc_name));
 
-			rh = (struct lpfc_fdmi_reg_hba *)&CtReq->un.PortID;
-			/* HBA Identifier */
-			memcpy(&rh->hi.PortName, &vport->fc_sparam.portName,
+		if (cmdcode == SLI_MGMT_RHBA) {
+			/* Registered Port List */
+			/* One entry (port) per adapter */
+			rh->rpl.EntryCnt = cpu_to_be32(1);
+			memcpy(&rh->rpl.pe, &phba->pport->fc_sparam.portName,
 			       sizeof(struct lpfc_name));
 
-			if (cmdcode == SLI_MGMT_RHBA) {
-				/* Registered Port List */
-				/* One entry (port) per adapter */
-				rh->rpl.EntryCnt = cpu_to_be32(1);
-				memcpy(&rh->rpl.pe, &vport->fc_sparam.portName,
-				       sizeof(struct lpfc_name));
-
-				/* point to the HBA attribute block */
-				size = 2 * sizeof(struct lpfc_name) +
-					FOURBYTES;
-			} else {
-				size = sizeof(struct lpfc_name);
-			}
-			ab = (struct lpfc_fdmi_attr_block *)
-				((uint8_t *)rh + size);
-			ab->EntryCnt = 0;
-			size += FOURBYTES;
-
-			/*
-			 * Point to beginning of first HBA attribute entry
-			 */
-			/* #1 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(struct lpfc_name));
-			ad->AttrType = cpu_to_be16(RHBA_NODENAME);
-			ad->AttrLen =  cpu_to_be16(FOURBYTES
-						+ sizeof(struct lpfc_name));
-			memcpy(&ae->un.NodeName, &vport->fc_sparam.nodeName,
-			       sizeof(struct lpfc_name));
-			ab->EntryCnt++;
-			size += FOURBYTES + sizeof(struct lpfc_name);
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #2 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.Manufacturer));
-			ad->AttrType = cpu_to_be16(RHBA_MANUFACTURER);
-			strncpy(ae->un.Manufacturer, "Emulex Corporation",
-				sizeof(ae->un.Manufacturer));
-			len = strnlen(ae->un.Manufacturer,
-					  sizeof(ae->un.Manufacturer));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #3 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.SerialNumber));
-			ad->AttrType = cpu_to_be16(RHBA_SERIAL_NUMBER);
-			strncpy(ae->un.SerialNumber, phba->SerialNumber,
-				sizeof(ae->un.SerialNumber));
-			len = strnlen(ae->un.SerialNumber,
-					  sizeof(ae->un.SerialNumber));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #4 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.Model));
-			ad->AttrType = cpu_to_be16(RHBA_MODEL);
-			strncpy(ae->un.Model, phba->ModelName,
-				sizeof(ae->un.Model));
-			len = strnlen(ae->un.Model, sizeof(ae->un.Model));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #5 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.ModelDescription));
-			ad->AttrType = cpu_to_be16(RHBA_MODEL_DESCRIPTION);
-			strncpy(ae->un.ModelDescription, phba->ModelDesc,
-				sizeof(ae->un.ModelDescription));
-			len = strnlen(ae->un.ModelDescription,
-					  sizeof(ae->un.ModelDescription));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + 8) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #6 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, 8);
-			ad->AttrType = cpu_to_be16(RHBA_HARDWARE_VERSION);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 8);
-			/* Convert JEDEC ID to ascii for hardware version */
-			incr = vp->rev.biuRev;
-			for (i = 0; i < 8; i++) {
-				j = (incr & 0xf);
-				if (j <= 9)
-					ae->un.HardwareVersion[7 - i] =
-					    (char)((uint8_t)0x30 +
-						   (uint8_t)j);
-				else
-					ae->un.HardwareVersion[7 - i] =
-					    (char)((uint8_t)0x61 +
-						   (uint8_t)(j - 10));
-				incr = (incr >> 4);
+			/* point to the HBA attribute block */
+			size = 2 * sizeof(struct lpfc_name) +
+				FOURBYTES;
+		} else {
+			size = sizeof(struct lpfc_name);
+		}
+		ab = (struct lpfc_fdmi_attr_block *)((uint8_t *)rh + size);
+		ab->EntryCnt = 0;
+		size += FOURBYTES;
+		bit_pos = 0;
+		if (new_mask)
+			mask = new_mask;
+		else
+			mask = vport->fdmi_hba_mask;
+
+		/* Mask will dictate what attributes to build in the request */
+		while (mask) {
+			if (mask & 0x1) {
+				func = lpfc_fdmi_hba_action[bit_pos];
+				size += func(vport,
+					     (struct lpfc_fdmi_attr_def *)
+					     ((uint8_t *)rh + size));
+				ab->EntryCnt++;
+				if ((size + 256) >
+				    (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
+					goto hba_out;
 			}
-			ab->EntryCnt++;
-			size += FOURBYTES + 8;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #7 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.DriverVersion));
-			ad->AttrType = cpu_to_be16(RHBA_DRIVER_VERSION);
-			strncpy(ae->un.DriverVersion, lpfc_release_version,
-				sizeof(ae->un.DriverVersion));
-			len = strnlen(ae->un.DriverVersion,
-					sizeof(ae->un.DriverVersion));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #8 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.OptionROMVersion));
-			ad->AttrType = cpu_to_be16(RHBA_OPTION_ROM_VERSION);
-			strncpy(ae->un.OptionROMVersion, phba->OptionROMVersion,
-				sizeof(ae->un.OptionROMVersion));
-			len = strnlen(ae->un.OptionROMVersion,
-				      sizeof(ae->un.OptionROMVersion));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #9 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.FirmwareVersion));
-			ad->AttrType = cpu_to_be16(RHBA_FIRMWARE_VERSION);
-			lpfc_decode_firmware_rev(phba, ae->un.FirmwareVersion,
-				1);
-			len = strnlen(ae->un.FirmwareVersion,
-					sizeof(ae->un.FirmwareVersion));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #10 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.OsNameVersion));
-			ad->AttrType = cpu_to_be16(RHBA_OS_NAME_VERSION);
-			snprintf(ae->un.OsNameVersion,
-				 sizeof(ae->un.OsNameVersion),
-				 "%s %s %s",
-				 init_utsname()->sysname,
-				 init_utsname()->release,
-				 init_utsname()->version);
-			len = strnlen(ae->un.OsNameVersion,
-				      sizeof(ae->un.OsNameVersion));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/* #11 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType =
-				cpu_to_be16(RHBA_MAX_CT_PAYLOAD_LEN);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			ae->un.MaxCTPayloadLen = cpu_to_be32(LPFC_MAX_CT_SIZE);
-			ab->EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto hba_out;
-
-			/*
-			 * Currently switches don't seem to support the
-			 * following extended HBA attributes.
-			 */
-			if (!(vport->cfg_fdmi_on & LPFC_FDMI_ALL_ATTRIB))
-				goto hba_out;
-
-			/* #12 HBA attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)rh + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.NodeSymName));
-			ad->AttrType = cpu_to_be16(RHBA_SYM_NODENAME);
-			len = lpfc_vport_symbolic_node_name(vport,
-				ae->un.NodeSymName, sizeof(ae->un.NodeSymName));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			ab->EntryCnt++;
-			size += FOURBYTES + len;
-hba_out:
-			ab->EntryCnt = cpu_to_be32(ab->EntryCnt);
-			/* Total size */
-			size = GID_REQUEST_SZ - 4 + size;
+			mask = mask >> 1;
+			bit_pos++;
 		}
+hba_out:
+		ab->EntryCnt = cpu_to_be32(ab->EntryCnt);
+		/* Total size */
+		size = GID_REQUEST_SZ - 4 + size;
 		break;
 
 	case SLI_MGMT_RPRT:
 	case SLI_MGMT_RPA:
-		{
-			struct serv_parm *hsp;
-			int len = 0;
-
-			if (cmdcode == SLI_MGMT_RPRT) {
-				rh = (struct lpfc_fdmi_reg_hba *)
-					&CtReq->un.PortID;
-				/* HBA Identifier */
-				memcpy(&rh->hi.PortName,
-				       &vport->fc_sparam.portName,
-				       sizeof(struct lpfc_name));
-				pab = (struct lpfc_fdmi_reg_portattr *)
-					&rh->rpl.EntryCnt;
-			} else
-				pab = (struct lpfc_fdmi_reg_portattr *)
-					&CtReq->un.PortID;
-			size = sizeof(struct lpfc_name) + FOURBYTES;
-			memcpy((uint8_t *)&pab->PortName,
-			       (uint8_t *)&vport->fc_sparam.portName,
+		pab = (struct lpfc_fdmi_reg_portattr *)&CtReq->un.PortID;
+		if (cmdcode == SLI_MGMT_RPRT) {
+			rh = (struct lpfc_fdmi_reg_hba *)pab;
+			/* HBA Identifier */
+			memcpy(&rh->hi.PortName,
+			       &phba->pport->fc_sparam.portName,
 			       sizeof(struct lpfc_name));
-			pab->ab.EntryCnt = 0;
-
-			/* #1 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.FC4Types));
-			ad->AttrType =
-				cpu_to_be16(RPRT_SUPPORTED_FC4_TYPES);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 32);
-			ae->un.FC4Types[0] = 0x40; /* Type 1 - ELS */
-			ae->un.FC4Types[1] = 0x80; /* Type 8 - FCP */
-			ae->un.FC4Types[4] = 0x80; /* Type 32 - CT */
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 32;
-
-			/* #2 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_SUPPORTED_SPEED);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			ae->un.SupportSpeed = 0;
-			if (phba->lmt & LMT_32Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_32GBIT;
-			if (phba->lmt & LMT_16Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_16GBIT;
-			if (phba->lmt & LMT_10Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_10GBIT;
-			if (phba->lmt & LMT_8Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_8GBIT;
-			if (phba->lmt & LMT_4Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_4GBIT;
-			if (phba->lmt & LMT_2Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_2GBIT;
-			if (phba->lmt & LMT_1Gb)
-				ae->un.SupportSpeed |= HBA_PORTSPEED_1GBIT;
-			ae->un.SupportSpeed =
-				cpu_to_be32(ae->un.SupportSpeed);
-
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-
-			/* #3 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_PORT_SPEED);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			switch (phba->fc_linkspeed) {
-			case LPFC_LINK_SPEED_1GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_1GBIT;
-				break;
-			case LPFC_LINK_SPEED_2GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_2GBIT;
-				break;
-			case LPFC_LINK_SPEED_4GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_4GBIT;
-				break;
-			case LPFC_LINK_SPEED_8GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_8GBIT;
-				break;
-			case LPFC_LINK_SPEED_10GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_10GBIT;
-				break;
-			case LPFC_LINK_SPEED_16GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_16GBIT;
-				break;
-			case LPFC_LINK_SPEED_32GHZ:
-				ae->un.PortSpeed = HBA_PORTSPEED_32GBIT;
-				break;
-			default:
-				ae->un.PortSpeed = HBA_PORTSPEED_UNKNOWN;
-				break;
-			}
-			ae->un.PortSpeed = cpu_to_be32(ae->un.PortSpeed);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-
-			/* #4 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_MAX_FRAME_SIZE);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			hsp = (struct serv_parm *)&vport->fc_sparam;
-			ae->un.MaxFrameSize =
-			    (((uint32_t)hsp->cmn.
-			      bbRcvSizeMsb) << 8) | (uint32_t)hsp->cmn.
-			    bbRcvSizeLsb;
-			ae->un.MaxFrameSize =
-				cpu_to_be32(ae->un.MaxFrameSize);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #5 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.OsDeviceName));
-			ad->AttrType = cpu_to_be16(RPRT_OS_DEVICE_NAME);
-			strncpy((char *)ae->un.OsDeviceName, LPFC_DRIVER_NAME,
-				sizeof(ae->un.OsDeviceName));
-			len = strnlen((char *)ae->un.OsDeviceName,
-					  sizeof(ae->un.OsDeviceName));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #6 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.HostName));
-			snprintf(ae->un.HostName, sizeof(ae->un.HostName), "%s",
-				 init_utsname()->nodename);
-			ad->AttrType = cpu_to_be16(RPRT_HOST_NAME);
-			len = strnlen(ae->un.HostName,
-					sizeof(ae->un.HostName));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen =
-				cpu_to_be16(FOURBYTES + len);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + sizeof(struct lpfc_name)) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
+			pab = (struct lpfc_fdmi_reg_portattr *)
+				((uint8_t *)pab +  sizeof(struct lpfc_name));
+		}
 
-			/*
-			 * Currently switches don't seem to support the
-			 * following extended Port attributes.
-			 */
-			if (!(vport->cfg_fdmi_on & LPFC_FDMI_ALL_ATTRIB))
-				goto port_out;
-
-			/* #7 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0,  sizeof(struct lpfc_name));
-			ad->AttrType = cpu_to_be16(RPRT_NODENAME);
-			ad->AttrLen =  cpu_to_be16(FOURBYTES
-						+ sizeof(struct lpfc_name));
-			memcpy(&ae->un.NodeName, &vport->fc_sparam.nodeName,
-			       sizeof(struct lpfc_name));
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + sizeof(struct lpfc_name);
-			if ((size + sizeof(struct lpfc_name)) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #8 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0,  sizeof(struct lpfc_name));
-			ad->AttrType = cpu_to_be16(RPRT_PORTNAME);
-			ad->AttrLen =  cpu_to_be16(FOURBYTES
-						+ sizeof(struct lpfc_name));
-			memcpy(&ae->un.PortName, &vport->fc_sparam.portName,
-			       sizeof(struct lpfc_name));
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + sizeof(struct lpfc_name);
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #9 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.NodeSymName));
-			ad->AttrType = cpu_to_be16(RPRT_SYM_PORTNAME);
-			len = lpfc_vport_symbolic_port_name(vport,
-				ae->un.NodeSymName, sizeof(ae->un.NodeSymName));
-			len += (len & 3) ? (4 - (len & 3)) : 4;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + len);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + len;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #10 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_PORT_TYPE);
-			ae->un.PortState = 0;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #11 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_SUPPORTED_CLASS);
-			ae->un.SupportClass =
-				cpu_to_be32(FC_COS_CLASS2 | FC_COS_CLASS3);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + sizeof(struct lpfc_name)) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #12 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(struct lpfc_name));
-			ad->AttrType = cpu_to_be16(RPRT_FABRICNAME);
-			ad->AttrLen =  cpu_to_be16(FOURBYTES
-						+ sizeof(struct lpfc_name));
-			memcpy(&ae->un.FabricName, &vport->fabric_nodename,
-			       sizeof(struct lpfc_name));
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + sizeof(struct lpfc_name);
-			if ((size + LPFC_FDMI_MAX_AE_SIZE) >
-					(LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #13 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			memset(ae, 0, sizeof(ae->un.FC4Types));
-			ad->AttrType =
-				cpu_to_be16(RPRT_ACTIVE_FC4_TYPES);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 32);
-			ae->un.FC4Types[0] = 0x40; /* Type 1 - ELS */
-			ae->un.FC4Types[1] = 0x80; /* Type 8 - FCP */
-			ae->un.FC4Types[4] = 0x80; /* Type 32 - CT */
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 32;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #257 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_PORT_STATE);
-			ae->un.PortState = 0;
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #258 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_DISC_PORT);
-			ae->un.PortState = lpfc_find_map_node(vport);
-			ae->un.PortState = cpu_to_be32(ae->un.PortState);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-			if ((size + 4) > (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
-				goto port_out;
-
-			/* #259 Port attribute entry */
-			ad = (struct lpfc_fdmi_attr_def *)
-				((uint8_t *)pab + size);
-			ae = (struct lpfc_fdmi_attr_entry *)&ad->AttrValue;
-			ad->AttrType = cpu_to_be16(RPRT_PORT_ID);
-			ae->un.PortId =  cpu_to_be32(vport->fc_myDID);
-			ad->AttrLen = cpu_to_be16(FOURBYTES + 4);
-			pab->ab.EntryCnt++;
-			size += FOURBYTES + 4;
-port_out:
-			pab->ab.EntryCnt = cpu_to_be32(pab->ab.EntryCnt);
-			/* Total size */
-			size = GID_REQUEST_SZ - 4 + size;
+		memcpy((uint8_t *)&pab->PortName,
+		       (uint8_t *)&vport->fc_sparam.portName,
+		       sizeof(struct lpfc_name));
+		size += sizeof(struct lpfc_name) + FOURBYTES;
+		pab->ab.EntryCnt = 0;
+		bit_pos = 0;
+		if (new_mask)
+			mask = new_mask;
+		else
+			mask = vport->fdmi_port_mask;
+
+		/* Mask will dictate what attributes to build in the request */
+		while (mask) {
+			if (mask & 0x1) {
+				func = lpfc_fdmi_port_action[bit_pos];
+				size += func(vport,
+					     (struct lpfc_fdmi_attr_def *)
+					     ((uint8_t *)pab + size));
+				pab->ab.EntryCnt++;
+				if ((size + 256) >
+				    (LPFC_BPL_SIZE - LPFC_CT_PREAMBLE))
+					goto port_out;
+			}
+			mask = mask >> 1;
+			bit_pos++;
 		}
+port_out:
+		pab->ab.EntryCnt = cpu_to_be32(pab->ab.EntryCnt);
+		/* Total size */
+		if (cmdcode == SLI_MGMT_RPRT)
+			size += sizeof(struct lpfc_name);
+		size = GID_REQUEST_SZ - 4 + size;
 		break;
 
 	case SLI_MGMT_GHAT:
@@ -2158,41 +2749,6 @@ lpfc_delayed_disc_timeout_handler(struct lpfc_vport *vport)
 }
 
 void
-lpfc_fdmi_tmo(unsigned long ptr)
-{
-	struct lpfc_vport *vport = (struct lpfc_vport *)ptr;
-	struct lpfc_hba   *phba = vport->phba;
-	uint32_t tmo_posted;
-	unsigned long iflag;
-
-	spin_lock_irqsave(&vport->work_port_lock, iflag);
-	tmo_posted = vport->work_port_events & WORKER_FDMI_TMO;
-	if (!tmo_posted)
-		vport->work_port_events |= WORKER_FDMI_TMO;
-	spin_unlock_irqrestore(&vport->work_port_lock, iflag);
-
-	if (!tmo_posted)
-		lpfc_worker_wake_up(phba);
-	return;
-}
-
-void
-lpfc_fdmi_timeout_handler(struct lpfc_vport *vport)
-{
-	struct lpfc_nodelist *ndlp;
-
-	ndlp = lpfc_findnode_did(vport, FDMI_DID);
-	if (ndlp && NLP_CHK_NODE_ACT(ndlp)) {
-		if (init_utsname()->nodename[0] != '\0')
-			lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DHBA);
-		else
-			mod_timer(&vport->fc_fdmitmo, jiffies +
-				  msecs_to_jiffies(1000 * 60));
-	}
-	return;
-}
-
-void
 lpfc_decode_firmware_rev(struct lpfc_hba *phba, char *fwrevision, int flag)
 {
 	struct lpfc_sli *psli = &phba->sli;
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index b6fa257ea3e0..7f5abb8f52bc 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -455,9 +455,9 @@ int
 lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 {
 	struct lpfc_hba  *phba = vport->phba;
-	LPFC_MBOXQ_t *mboxq;
+	LPFC_MBOXQ_t *mboxq = NULL;
 	struct lpfc_nodelist *ndlp;
-	struct lpfc_dmabuf *dmabuf;
+	struct lpfc_dmabuf *dmabuf = NULL;
 	int rc = 0;
 
 	/* move forward in case of SLI4 FC port loopback test and pt2pt mode */
@@ -471,25 +471,33 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 		}
 	}
 
-	dmabuf = kzalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
-	if (!dmabuf) {
+	mboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mboxq) {
 		rc = -ENOMEM;
 		goto fail;
 	}
-	dmabuf->virt = lpfc_mbuf_alloc(phba, MEM_PRI, &dmabuf->phys);
-	if (!dmabuf->virt) {
-		rc = -ENOMEM;
-		goto fail_free_dmabuf;
-	}
 
-	mboxq = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-	if (!mboxq) {
-		rc = -ENOMEM;
-		goto fail_free_coherent;
+	/* Supply CSP's only if we are fabric connect or pt-to-pt connect */
+	if ((vport->fc_flag & FC_FABRIC) || (vport->fc_flag & FC_PT2PT)) {
+		dmabuf = kzalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
+		if (!dmabuf) {
+			rc = -ENOMEM;
+			goto fail;
+		}
+		dmabuf->virt = lpfc_mbuf_alloc(phba, MEM_PRI, &dmabuf->phys);
+		if (!dmabuf->virt) {
+			rc = -ENOMEM;
+			goto fail;
+		}
+		memcpy(dmabuf->virt, &phba->fc_fabparam,
+		       sizeof(struct serv_parm));
 	}
+
 	vport->port_state = LPFC_FABRIC_CFG_LINK;
-	memcpy(dmabuf->virt, &phba->fc_fabparam, sizeof(vport->fc_sparam));
-	lpfc_reg_vfi(mboxq, vport, dmabuf->phys);
+	if (dmabuf)
+		lpfc_reg_vfi(mboxq, vport, dmabuf->phys);
+	else
+		lpfc_reg_vfi(mboxq, vport, 0);
 
 	mboxq->mbox_cmpl = lpfc_mbx_cmpl_reg_vfi;
 	mboxq->vport = vport;
@@ -497,17 +505,19 @@ lpfc_issue_reg_vfi(struct lpfc_vport *vport)
 	rc = lpfc_sli_issue_mbox(phba, mboxq, MBX_NOWAIT);
 	if (rc == MBX_NOT_FINISHED) {
 		rc = -ENXIO;
-		goto fail_free_mbox;
+		goto fail;
 	}
 	return 0;
 
-fail_free_mbox:
-	mempool_free(mboxq, phba->mbox_mem_pool);
-fail_free_coherent:
-	lpfc_mbuf_free(phba, dmabuf->virt, dmabuf->phys);
-fail_free_dmabuf:
-	kfree(dmabuf);
 fail:
+	if (mboxq)
+		mempool_free(mboxq, phba->mbox_mem_pool);
+	if (dmabuf) {
+		if (dmabuf->virt)
+			lpfc_mbuf_free(phba, dmabuf->virt, dmabuf->phys);
+		kfree(dmabuf);
+	}
+
 	lpfc_vport_set_state(vport, FC_VPORT_FAILED);
 	lpfc_printf_vlog(vport, KERN_ERR, LOG_ELS,
 		"0289 Issue Register VFI failed: Err %d\n", rc);
@@ -678,6 +688,21 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 				sp->cmn.bbRcvSizeLsb;
 
 	fabric_param_changed = lpfc_check_clean_addr_bit(vport, sp);
+	if (fabric_param_changed) {
+		/* Reset FDMI attribute masks based on config parameter */
+		if (phba->cfg_fdmi_on == LPFC_FDMI_NO_SUPPORT) {
+			vport->fdmi_hba_mask = 0;
+			vport->fdmi_port_mask = 0;
+		} else {
+			/* Setup appropriate attribute masks */
+			vport->fdmi_hba_mask = LPFC_FDMI2_HBA_ATTR;
+			if (phba->cfg_fdmi_on == LPFC_FDMI_SMART_SAN)
+				vport->fdmi_port_mask = LPFC_FDMI2_SMART_ATTR;
+			else
+				vport->fdmi_port_mask = LPFC_FDMI2_PORT_ATTR;
+		}
+
+	}
 	memcpy(&vport->fabric_portname, &sp->portName,
 			sizeof(struct lpfc_name));
 	memcpy(&vport->fabric_nodename, &sp->nodeName,
@@ -711,9 +736,10 @@ lpfc_cmpl_els_flogi_fabric(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	 * For FC we need to do some special processing because of the SLI
 	 * Port's default settings of the Common Service Parameters.
 	 */
-	if (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC) {
+	if ((phba->sli_rev == LPFC_SLI_REV4) &&
+	    (phba->sli4_hba.lnk_info.lnk_tp == LPFC_LNK_TYPE_FC)) {
 		/* If physical FC port changed, unreg VFI and ALL VPIs / RPIs */
-		if ((phba->sli_rev == LPFC_SLI_REV4) && fabric_param_changed)
+		if (fabric_param_changed)
 			lpfc_unregister_fcf_prep(phba);
 
 		/* This should just update the VFI CSPs*/
@@ -824,13 +850,21 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 
 	spin_lock_irq(shost->host_lock);
 	vport->fc_flag &= ~(FC_FABRIC | FC_PUBLIC_LOOP);
+	vport->fc_flag |= FC_PT2PT;
 	spin_unlock_irq(shost->host_lock);
 
-	phba->fc_edtov = FF_DEF_EDTOV;
-	phba->fc_ratov = FF_DEF_RATOV;
+	/* If physical FC port changed, unreg VFI and ALL VPIs / RPIs */
+	if ((phba->sli_rev == LPFC_SLI_REV4) && phba->fc_topology_changed) {
+		lpfc_unregister_fcf_prep(phba);
+
+		spin_lock_irq(shost->host_lock);
+		vport->fc_flag &= ~FC_VFI_REGISTERED;
+		spin_unlock_irq(shost->host_lock);
+		phba->fc_topology_changed = 0;
+	}
+
 	rc = memcmp(&vport->fc_portname, &sp->portName,
 		    sizeof(vport->fc_portname));
-	memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
 
 	if (rc >= 0) {
 		/* This side will initiate the PLOGI */
@@ -839,38 +873,14 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 		spin_unlock_irq(shost->host_lock);
 
 		/*
-		 * N_Port ID cannot be 0, set our to LocalID the other
-		 * side will be RemoteID.
+		 * N_Port ID cannot be 0, set our Id to LocalID
+		 * the other side will be RemoteID.
 		 */
 
 		/* not equal */
 		if (rc)
 			vport->fc_myDID = PT2PT_LocalID;
 
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (!mbox)
-			goto fail;
-
-		lpfc_config_link(phba, mbox);
-
-		mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-		mbox->vport = vport;
-		rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-		if (rc == MBX_NOT_FINISHED) {
-			mempool_free(mbox, phba->mbox_mem_pool);
-			goto fail;
-		}
-
-		/*
-		 * For SLI4, the VFI/VPI are registered AFTER the
-		 * Nport with the higher WWPN sends the PLOGI with
-		 * an assigned NPortId.
-		 */
-
-		/* not equal */
-		if ((phba->sli_rev == LPFC_SLI_REV4) && rc)
-			lpfc_issue_reg_vfi(vport);
-
 		/* Decrement ndlp reference count indicating that ndlp can be
 		 * safely released when other references to it are done.
 		 */
@@ -912,29 +922,20 @@ lpfc_cmpl_els_flogi_nport(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	/* If we are pt2pt with another NPort, force NPIV off! */
 	phba->sli3_options &= ~LPFC_SLI3_NPIV_ENABLED;
 
-	spin_lock_irq(shost->host_lock);
-	vport->fc_flag |= FC_PT2PT;
-	spin_unlock_irq(shost->host_lock);
-	/* If physical FC port changed, unreg VFI and ALL VPIs / RPIs */
-	if ((phba->sli_rev == LPFC_SLI_REV4) && phba->fc_topology_changed) {
-		lpfc_unregister_fcf_prep(phba);
+	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+	if (!mbox)
+		goto fail;
 
-		/* The FC_VFI_REGISTERED flag will get clear in the cmpl
-		 * handler for unreg_vfi, but if we don't force the
-		 * FC_VFI_REGISTERED flag then the reg_vfi mailbox could be
-		 * built with the update bit set instead of just the vp bit to
-		 * change the Nport ID.  We need to have the vp set and the
-		 * Upd cleared on topology changes.
-		 */
-		spin_lock_irq(shost->host_lock);
-		vport->fc_flag &= ~FC_VFI_REGISTERED;
-		spin_unlock_irq(shost->host_lock);
-		phba->fc_topology_changed = 0;
-		lpfc_issue_reg_vfi(vport);
+	lpfc_config_link(phba, mbox);
+
+	mbox->mbox_cmpl = lpfc_mbx_cmpl_local_config_link;
+	mbox->vport = vport;
+	rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+	if (rc == MBX_NOT_FINISHED) {
+		mempool_free(mbox, phba->mbox_mem_pool);
+		goto fail;
 	}
 
-	/* Start discovery - this should just do CLEAR_LA */
-	lpfc_disc_start(vport);
 	return 0;
 fail:
 	return -ENXIO;
@@ -1157,6 +1158,7 @@ flogifail:
 	spin_lock_irq(&phba->hbalock);
 	phba->fcf.fcf_flag &= ~FCF_DISCOVERY;
 	spin_unlock_irq(&phba->hbalock);
+
 	lpfc_nlp_put(ndlp);
 
 	if (!lpfc_error_lost_link(irsp)) {
@@ -3792,14 +3794,17 @@ lpfc_cmpl_els_rsp(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 				lpfc_nlp_set_state(vport, ndlp,
 					   NLP_STE_REG_LOGIN_ISSUE);
 			}
+
+			ndlp->nlp_flag |= NLP_REG_LOGIN_SEND;
 			if (lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT)
 			    != MBX_NOT_FINISHED)
 				goto out;
-			else
-				/* Decrement the ndlp reference count we
-				 * set for this failed mailbox command.
-				 */
-				lpfc_nlp_put(ndlp);
+
+			/* Decrement the ndlp reference count we
+			 * set for this failed mailbox command.
+			 */
+			lpfc_nlp_put(ndlp);
+			ndlp->nlp_flag &= ~NLP_REG_LOGIN_SEND;
 
 			/* ELS rsp: Cannot issue reg_login for <NPortid> */
 			lpfc_printf_vlog(vport, KERN_ERR, LOG_ELS,
@@ -3856,6 +3861,7 @@ out:
 				 * the routine lpfc_els_free_iocb.
 				 */
 				cmdiocb->context1 = NULL;
+
 	}
 
 	lpfc_els_free_iocb(phba, cmdiocb);
@@ -3898,6 +3904,7 @@ lpfc_els_rsp_acc(struct lpfc_vport *vport, uint32_t flag,
 	IOCB_t *oldcmd;
 	struct lpfc_iocbq *elsiocb;
 	uint8_t *pcmd;
+	struct serv_parm *sp;
 	uint16_t cmdsize;
 	int rc;
 	ELS_PKT *els_pkt_ptr;
@@ -3927,6 +3934,7 @@ lpfc_els_rsp_acc(struct lpfc_vport *vport, uint32_t flag,
 			"Issue ACC:       did:x%x flg:x%x",
 			ndlp->nlp_DID, ndlp->nlp_flag, 0);
 		break;
+	case ELS_CMD_FLOGI:
 	case ELS_CMD_PLOGI:
 		cmdsize = (sizeof(struct serv_parm) + sizeof(uint32_t));
 		elsiocb = lpfc_prep_els_iocb(vport, 0, cmdsize, oldiocb->retry,
@@ -3944,10 +3952,34 @@ lpfc_els_rsp_acc(struct lpfc_vport *vport, uint32_t flag,
 
 		*((uint32_t *) (pcmd)) = ELS_CMD_ACC;
 		pcmd += sizeof(uint32_t);
-		memcpy(pcmd, &vport->fc_sparam, sizeof(struct serv_parm));
+		sp = (struct serv_parm *)pcmd;
+
+		if (flag == ELS_CMD_FLOGI) {
+			/* Copy the received service parameters back */
+			memcpy(sp, &phba->fc_fabparam,
+			       sizeof(struct serv_parm));
+
+			/* Clear the F_Port bit */
+			sp->cmn.fPort = 0;
+
+			/* Mark all class service parameters as invalid */
+			sp->cls1.classValid = 0;
+			sp->cls2.classValid = 0;
+			sp->cls3.classValid = 0;
+			sp->cls4.classValid = 0;
+
+			/* Copy our worldwide names */
+			memcpy(&sp->portName, &vport->fc_sparam.portName,
+			       sizeof(struct lpfc_name));
+			memcpy(&sp->nodeName, &vport->fc_sparam.nodeName,
+			       sizeof(struct lpfc_name));
+		} else {
+			memcpy(pcmd, &vport->fc_sparam,
+			       sizeof(struct serv_parm));
+		}
 
 		lpfc_debugfs_disc_trc(vport, LPFC_DISC_TRC_ELS_RSP,
-			"Issue ACC PLOGI: did:x%x flg:x%x",
+			"Issue ACC FLOGI/PLOGI: did:x%x flg:x%x",
 			ndlp->nlp_DID, ndlp->nlp_flag, 0);
 		break;
 	case ELS_CMD_PRLO:
@@ -4673,6 +4705,23 @@ lpfc_rdp_res_link_error(struct fc_rdp_link_error_status_desc *desc,
 	desc->length = cpu_to_be32(sizeof(desc->info));
 }
 
+int
+lpfc_rdp_res_fec_desc(struct fc_fec_rdp_desc *desc, READ_LNK_VAR *stat)
+{
+	if (bf_get(lpfc_read_link_stat_gec2, stat) == 0)
+		return 0;
+	desc->tag = cpu_to_be32(RDP_FEC_DESC_TAG);
+
+	desc->info.CorrectedBlocks =
+		cpu_to_be32(stat->fecCorrBlkCount);
+	desc->info.UncorrectableBlocks =
+		cpu_to_be32(stat->fecUncorrBlkCount);
+
+	desc->length = cpu_to_be32(sizeof(desc->info));
+
+	return sizeof(struct fc_fec_rdp_desc);
+}
+
 void
 lpfc_rdp_res_speed(struct fc_rdp_port_speed_desc *desc, struct lpfc_hba *phba)
 {
@@ -4681,26 +4730,26 @@ lpfc_rdp_res_speed(struct fc_rdp_port_speed_desc *desc, struct lpfc_hba *phba)
 
 	desc->tag = cpu_to_be32(RDP_PORT_SPEED_DESC_TAG);
 
-	switch (phba->sli4_hba.link_state.speed) {
-	case LPFC_FC_LA_SPEED_1G:
+	switch (phba->fc_linkspeed) {
+	case LPFC_LINK_SPEED_1GHZ:
 		rdp_speed = RDP_PS_1GB;
 		break;
-	case LPFC_FC_LA_SPEED_2G:
+	case LPFC_LINK_SPEED_2GHZ:
 		rdp_speed = RDP_PS_2GB;
 		break;
-	case LPFC_FC_LA_SPEED_4G:
+	case LPFC_LINK_SPEED_4GHZ:
 		rdp_speed = RDP_PS_4GB;
 		break;
-	case LPFC_FC_LA_SPEED_8G:
+	case LPFC_LINK_SPEED_8GHZ:
 		rdp_speed = RDP_PS_8GB;
 		break;
-	case LPFC_FC_LA_SPEED_10G:
+	case LPFC_LINK_SPEED_10GHZ:
 		rdp_speed = RDP_PS_10GB;
 		break;
-	case LPFC_FC_LA_SPEED_16G:
+	case LPFC_LINK_SPEED_16GHZ:
 		rdp_speed = RDP_PS_16GB;
 		break;
-	case LPFC_FC_LA_SPEED_32G:
+	case LPFC_LINK_SPEED_32GHZ:
 		rdp_speed = RDP_PS_32GB;
 		break;
 	default:
@@ -4778,15 +4827,18 @@ lpfc_els_rdp_cmpl(struct lpfc_hba *phba, struct lpfc_rdp_context *rdp_context,
 	struct lpfc_nodelist *ndlp = rdp_context->ndlp;
 	struct lpfc_vport *vport = ndlp->vport;
 	struct lpfc_iocbq *elsiocb;
+	struct ulp_bde64 *bpl;
 	IOCB_t *icmd;
 	uint8_t *pcmd;
 	struct ls_rjt *stat;
 	struct fc_rdp_res_frame *rdp_res;
 	uint32_t cmdsize;
-	int rc;
+	int rc, fec_size;
 
 	if (status != SUCCESS)
 		goto error;
+
+	/* This will change once we know the true size of the RDP payload */
 	cmdsize = sizeof(struct fc_rdp_res_frame);
 
 	elsiocb = lpfc_prep_els_iocb(vport, 0, cmdsize,
@@ -4823,10 +4875,18 @@ lpfc_els_rdp_cmpl(struct lpfc_hba *phba, struct lpfc_rdp_context *rdp_context,
 	lpfc_rdp_res_diag_port_names(&rdp_res->diag_port_names_desc, phba);
 	lpfc_rdp_res_attach_port_names(&rdp_res->attached_port_names_desc,
 			vport, ndlp);
-	rdp_res->length = cpu_to_be32(RDP_DESC_PAYLOAD_SIZE);
-
+	fec_size = lpfc_rdp_res_fec_desc(&rdp_res->fec_desc,
+			&rdp_context->link_stat);
+	rdp_res->length = cpu_to_be32(fec_size + RDP_DESC_PAYLOAD_SIZE);
 	elsiocb->iocb_cmpl = lpfc_cmpl_els_rsp;
 
+	/* Now that we know the true size of the payload, update the BPL */
+	bpl = (struct ulp_bde64 *)
+		(((struct lpfc_dmabuf *)(elsiocb->context3))->virt);
+	bpl->tus.f.bdeSize = (fec_size + RDP_DESC_PAYLOAD_SIZE + 8);
+	bpl->tus.f.bdeFlags = 0;
+	bpl->tus.w = le32_to_cpu(bpl->tus.w);
+
 	phba->fc_stat.elsXmitACC++;
 	rc = lpfc_sli_issue_iocb(phba, LPFC_ELS_RING, elsiocb, 0);
 	if (rc == IOCB_ERROR)
@@ -4956,13 +5016,12 @@ lpfc_els_rcv_rdp(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 	if (RDP_NPORT_ID_SIZE !=
 			be32_to_cpu(rdp_req->nport_id_desc.length))
 		goto rjt_logerr;
-	rdp_context = kmalloc(sizeof(struct lpfc_rdp_context), GFP_KERNEL);
+	rdp_context = kzalloc(sizeof(struct lpfc_rdp_context), GFP_KERNEL);
 	if (!rdp_context) {
 		rjt_err = LSRJT_UNABLE_TPC;
 		goto error;
 	}
 
-	memset(rdp_context, 0, sizeof(struct lpfc_rdp_context));
 	cmd = &cmdiocb->iocb;
 	rdp_context->ndlp = lpfc_nlp_get(ndlp);
 	rdp_context->ox_id = cmd->unsli3.rcvsli3.ox_id;
@@ -5739,7 +5798,6 @@ lpfc_els_rcv_flogi(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 	IOCB_t *icmd = &cmdiocb->iocb;
 	struct serv_parm *sp;
 	LPFC_MBOXQ_t *mbox;
-	struct ls_rjt stat;
 	uint32_t cmd, did;
 	int rc;
 	uint32_t fc_flag = 0;
@@ -5765,135 +5823,92 @@ lpfc_els_rcv_flogi(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 		return 1;
 	}
 
-	if ((lpfc_check_sparm(vport, ndlp, sp, CLASS3, 1))) {
-		/* For a FLOGI we accept, then if our portname is greater
-		 * then the remote portname we initiate Nport login.
-		 */
+	(void) lpfc_check_sparm(vport, ndlp, sp, CLASS3, 1);
 
-		rc = memcmp(&vport->fc_portname, &sp->portName,
-			    sizeof(struct lpfc_name));
 
-		if (!rc) {
-			if (phba->sli_rev < LPFC_SLI_REV4) {
-				mbox = mempool_alloc(phba->mbox_mem_pool,
-						     GFP_KERNEL);
-				if (!mbox)
-					return 1;
-				lpfc_linkdown(phba);
-				lpfc_init_link(phba, mbox,
-					       phba->cfg_topology,
-					       phba->cfg_link_speed);
-				mbox->u.mb.un.varInitLnk.lipsr_AL_PA = 0;
-				mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-				mbox->vport = vport;
-				rc = lpfc_sli_issue_mbox(phba, mbox,
-							 MBX_NOWAIT);
-				lpfc_set_loopback_flag(phba);
-				if (rc == MBX_NOT_FINISHED)
-					mempool_free(mbox, phba->mbox_mem_pool);
-				return 1;
-			} else {
-				/* abort the flogi coming back to ourselves
-				 * due to external loopback on the port.
-				 */
-				lpfc_els_abort_flogi(phba);
-				return 0;
-			}
-		} else if (rc > 0) {	/* greater than */
-			spin_lock_irq(shost->host_lock);
-			vport->fc_flag |= FC_PT2PT_PLOGI;
-			spin_unlock_irq(shost->host_lock);
+	/*
+	 * If our portname is greater than the remote portname,
+	 * then we initiate Nport login.
+	 */
 
-			/* If we have the high WWPN we can assign our own
-			 * myDID; otherwise, we have to WAIT for a PLOGI
-			 * from the remote NPort to find out what it
-			 * will be.
-			 */
-			vport->fc_myDID = PT2PT_LocalID;
-		} else
-			vport->fc_myDID = PT2PT_RemoteID;
+	rc = memcmp(&vport->fc_portname, &sp->portName,
+		    sizeof(struct lpfc_name));
 
-		/*
-		 * The vport state should go to LPFC_FLOGI only
-		 * AFTER we issue a FLOGI, not receive one.
+	if (!rc) {
+		if (phba->sli_rev < LPFC_SLI_REV4) {
+			mbox = mempool_alloc(phba->mbox_mem_pool,
+					     GFP_KERNEL);
+			if (!mbox)
+				return 1;
+			lpfc_linkdown(phba);
+			lpfc_init_link(phba, mbox,
+				       phba->cfg_topology,
+				       phba->cfg_link_speed);
+			mbox->u.mb.un.varInitLnk.lipsr_AL_PA = 0;
+			mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+			mbox->vport = vport;
+			rc = lpfc_sli_issue_mbox(phba, mbox,
+						 MBX_NOWAIT);
+			lpfc_set_loopback_flag(phba);
+			if (rc == MBX_NOT_FINISHED)
+				mempool_free(mbox, phba->mbox_mem_pool);
+			return 1;
+		}
+
+		/* abort the flogi coming back to ourselves
+		 * due to external loopback on the port.
 		 */
+		lpfc_els_abort_flogi(phba);
+		return 0;
+
+	} else if (rc > 0) {	/* greater than */
 		spin_lock_irq(shost->host_lock);
-		fc_flag = vport->fc_flag;
-		port_state = vport->port_state;
-		vport->fc_flag |= FC_PT2PT;
-		vport->fc_flag &= ~(FC_FABRIC | FC_PUBLIC_LOOP);
+		vport->fc_flag |= FC_PT2PT_PLOGI;
 		spin_unlock_irq(shost->host_lock);
-		lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
-				 "3311 Rcv Flogi PS x%x new PS x%x "
-				 "fc_flag x%x new fc_flag x%x\n",
-				 port_state, vport->port_state,
-				 fc_flag, vport->fc_flag);
 
-		/*
-		 * We temporarily set fc_myDID to make it look like we are
-		 * a Fabric. This is done just so we end up with the right
-		 * did / sid on the FLOGI ACC rsp.
+		/* If we have the high WWPN we can assign our own
+		 * myDID; otherwise, we have to WAIT for a PLOGI
+		 * from the remote NPort to find out what it
+		 * will be.
 		 */
-		did = vport->fc_myDID;
-		vport->fc_myDID = Fabric_DID;
-
+		vport->fc_myDID = PT2PT_LocalID;
 	} else {
-		/* Reject this request because invalid parameters */
-		stat.un.b.lsRjtRsvd0 = 0;
-		stat.un.b.lsRjtRsnCode = LSRJT_UNABLE_TPC;
-		stat.un.b.lsRjtRsnCodeExp = LSEXP_SPARM_OPTIONS;
-		stat.un.b.vendorUnique = 0;
-
-		/*
-		 * We temporarily set fc_myDID to make it look like we are
-		 * a Fabric. This is done just so we end up with the right
-		 * did / sid on the FLOGI LS_RJT rsp.
-		 */
-		did = vport->fc_myDID;
-		vport->fc_myDID = Fabric_DID;
-
-		lpfc_els_rsp_reject(vport, stat.un.lsRjtError, cmdiocb, ndlp,
-			NULL);
+		vport->fc_myDID = PT2PT_RemoteID;
+	}
 
-		/* Now lets put fc_myDID back to what its supposed to be */
-		vport->fc_myDID = did;
+	/*
+	 * The vport state should go to LPFC_FLOGI only
+	 * AFTER we issue a FLOGI, not receive one.
+	 */
+	spin_lock_irq(shost->host_lock);
+	fc_flag = vport->fc_flag;
+	port_state = vport->port_state;
+	vport->fc_flag |= FC_PT2PT;
+	vport->fc_flag &= ~(FC_FABRIC | FC_PUBLIC_LOOP);
+	spin_unlock_irq(shost->host_lock);
+	lpfc_printf_vlog(vport, KERN_INFO, LOG_ELS,
+			 "3311 Rcv Flogi PS x%x new PS x%x "
+			 "fc_flag x%x new fc_flag x%x\n",
+			 port_state, vport->port_state,
+			 fc_flag, vport->fc_flag);
 
-		return 1;
-	}
+	/*
+	 * We temporarily set fc_myDID to make it look like we are
+	 * a Fabric. This is done just so we end up with the right
+	 * did / sid on the FLOGI ACC rsp.
+	 */
+	did = vport->fc_myDID;
+	vport->fc_myDID = Fabric_DID;
 
-	/* send our FLOGI first */
-	if (vport->port_state < LPFC_FLOGI) {
-		vport->fc_myDID = 0;
-		lpfc_initial_flogi(vport);
-		vport->fc_myDID = Fabric_DID;
-	}
+	memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
 
 	/* Send back ACC */
-	lpfc_els_rsp_acc(vport, ELS_CMD_PLOGI, cmdiocb, ndlp, NULL);
+	lpfc_els_rsp_acc(vport, ELS_CMD_FLOGI, cmdiocb, ndlp, NULL);
 
 	/* Now lets put fc_myDID back to what its supposed to be */
 	vport->fc_myDID = did;
 
-	if (!(vport->fc_flag & FC_PT2PT_PLOGI)) {
-
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (!mbox)
-			goto fail;
-
-		lpfc_config_link(phba, mbox);
-
-		mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-		mbox->vport = vport;
-		rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-		if (rc == MBX_NOT_FINISHED) {
-			mempool_free(mbox, phba->mbox_mem_pool);
-			goto fail;
-		}
-	}
-
 	return 0;
-fail:
-	return 1;
 }
 
 /**
@@ -7345,7 +7360,7 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 
 	/* reject till our FLOGI completes */
 	if ((vport->port_state < LPFC_FABRIC_CFG_LINK) &&
-		(cmd != ELS_CMD_FLOGI)) {
+	    (cmd != ELS_CMD_FLOGI)) {
 		rjt_err = LSRJT_UNABLE_TPC;
 		rjt_exp = LSEXP_NOTHING_MORE;
 		goto lsrjt;
@@ -7381,6 +7396,7 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 			rjt_exp = LSEXP_NOTHING_MORE;
 			break;
 		}
+
 		if (vport->port_state < LPFC_DISC_AUTH) {
 			if (!(phba->pport->fc_flag & FC_PT2PT) ||
 				(phba->pport->fc_flag & FC_PT2PT_PLOGI)) {
@@ -7730,6 +7746,35 @@ lpfc_els_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	}
 }
 
+void
+lpfc_start_fdmi(struct lpfc_vport *vport)
+{
+	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_nodelist *ndlp;
+
+	/* If this is the first time, allocate an ndlp and initialize
+	 * it. Otherwise, make sure the node is enabled and then do the
+	 * login.
+	 */
+	ndlp = lpfc_findnode_did(vport, FDMI_DID);
+	if (!ndlp) {
+		ndlp = mempool_alloc(phba->nlp_mem_pool, GFP_KERNEL);
+		if (ndlp) {
+			lpfc_nlp_init(vport, ndlp, FDMI_DID);
+			ndlp->nlp_type |= NLP_FABRIC;
+		} else {
+			return;
+		}
+	}
+	if (!NLP_CHK_NODE_ACT(ndlp))
+		ndlp = lpfc_enable_node(vport, ndlp, NLP_STE_NPR_NODE);
+
+	if (ndlp) {
+		lpfc_nlp_set_state(vport, ndlp, NLP_STE_PLOGI_ISSUE);
+		lpfc_issue_els_plogi(vport, ndlp->nlp_DID, 0);
+	}
+}
+
 /**
  * lpfc_do_scr_ns_plogi - Issue a plogi to the name server for scr
  * @phba: pointer to lpfc hba data structure.
@@ -7746,7 +7791,7 @@ lpfc_els_unsol_event(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 void
 lpfc_do_scr_ns_plogi(struct lpfc_hba *phba, struct lpfc_vport *vport)
 {
-	struct lpfc_nodelist *ndlp, *ndlp_fdmi;
+	struct lpfc_nodelist *ndlp;
 	struct Scsi_Host *shost = lpfc_shost_from_vport(vport);
 
 	/*
@@ -7804,32 +7849,9 @@ lpfc_do_scr_ns_plogi(struct lpfc_hba *phba, struct lpfc_vport *vport)
 		return;
 	}
 
-	if (vport->cfg_fdmi_on & LPFC_FDMI_SUPPORT) {
-		/* If this is the first time, allocate an ndlp and initialize
-		 * it. Otherwise, make sure the node is enabled and then do the
-		 * login.
-		 */
-		ndlp_fdmi = lpfc_findnode_did(vport, FDMI_DID);
-		if (!ndlp_fdmi) {
-			ndlp_fdmi = mempool_alloc(phba->nlp_mem_pool,
-						  GFP_KERNEL);
-			if (ndlp_fdmi) {
-				lpfc_nlp_init(vport, ndlp_fdmi, FDMI_DID);
-				ndlp_fdmi->nlp_type |= NLP_FABRIC;
-			} else
-				return;
-		}
-		if (!NLP_CHK_NODE_ACT(ndlp_fdmi))
-			ndlp_fdmi = lpfc_enable_node(vport,
-						     ndlp_fdmi,
-						     NLP_STE_NPR_NODE);
-
-		if (ndlp_fdmi) {
-			lpfc_nlp_set_state(vport, ndlp_fdmi,
-					   NLP_STE_PLOGI_ISSUE);
-			lpfc_issue_els_plogi(vport, ndlp_fdmi->nlp_DID, 0);
-		}
-	}
+	if ((phba->cfg_fdmi_on > LPFC_FDMI_NO_SUPPORT) &&
+	    (vport->load_flag & FC_ALLOW_FDMI))
+		lpfc_start_fdmi(vport);
 }
 
 /**
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index bfc2442dd74a..c37d72effbff 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -674,8 +674,6 @@ lpfc_work_done(struct lpfc_hba *phba)
 				lpfc_mbox_timeout_handler(phba);
 			if (work_port_events & WORKER_FABRIC_BLOCK_TMO)
 				lpfc_unblock_fabric_iocbs(phba);
-			if (work_port_events & WORKER_FDMI_TMO)
-				lpfc_fdmi_timeout_handler(vport);
 			if (work_port_events & WORKER_RAMP_DOWN_QUEUE)
 				lpfc_ramp_down_queue_handler(phba);
 			if (work_port_events & WORKER_DELAYED_DISC_TMO)
@@ -1083,7 +1081,7 @@ out:
 }
 
 
-static void
+void
 lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 {
 	struct lpfc_vport *vport = pmb->vport;
@@ -1113,8 +1111,10 @@ lpfc_mbx_cmpl_local_config_link(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 	/* Start discovery by sending a FLOGI. port_state is identically
 	 * LPFC_FLOGI while waiting for FLOGI cmpl
 	 */
-	if (vport->port_state != LPFC_FLOGI || vport->fc_flag & FC_PT2PT_PLOGI)
+	if (vport->port_state != LPFC_FLOGI)
 		lpfc_initial_flogi(vport);
+	else if (vport->fc_flag & FC_PT2PT)
+		lpfc_disc_start(vport);
 	return;
 
 out:
@@ -2963,8 +2963,10 @@ lpfc_mbx_cmpl_reg_vfi(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 
 out_free_mem:
 	mempool_free(mboxq, phba->mbox_mem_pool);
-	lpfc_mbuf_free(phba, dmabuf->virt, dmabuf->phys);
-	kfree(dmabuf);
+	if (dmabuf) {
+		lpfc_mbuf_free(phba, dmabuf->virt, dmabuf->phys);
+		kfree(dmabuf);
+	}
 	return;
 }
 
@@ -3035,19 +3037,22 @@ lpfc_mbx_process_link_up(struct lpfc_hba *phba, struct lpfc_mbx_read_top *la)
 	uint32_t fc_flags = 0;
 
 	spin_lock_irq(&phba->hbalock);
-	switch (bf_get(lpfc_mbx_read_top_link_spd, la)) {
-	case LPFC_LINK_SPEED_1GHZ:
-	case LPFC_LINK_SPEED_2GHZ:
-	case LPFC_LINK_SPEED_4GHZ:
-	case LPFC_LINK_SPEED_8GHZ:
-	case LPFC_LINK_SPEED_10GHZ:
-	case LPFC_LINK_SPEED_16GHZ:
-	case LPFC_LINK_SPEED_32GHZ:
-		phba->fc_linkspeed = bf_get(lpfc_mbx_read_top_link_spd, la);
-		break;
-	default:
-		phba->fc_linkspeed = LPFC_LINK_SPEED_UNKNOWN;
-		break;
+	phba->fc_linkspeed = bf_get(lpfc_mbx_read_top_link_spd, la);
+
+	if (!(phba->hba_flag & HBA_FCOE_MODE)) {
+		switch (bf_get(lpfc_mbx_read_top_link_spd, la)) {
+		case LPFC_LINK_SPEED_1GHZ:
+		case LPFC_LINK_SPEED_2GHZ:
+		case LPFC_LINK_SPEED_4GHZ:
+		case LPFC_LINK_SPEED_8GHZ:
+		case LPFC_LINK_SPEED_10GHZ:
+		case LPFC_LINK_SPEED_16GHZ:
+		case LPFC_LINK_SPEED_32GHZ:
+			break;
+		default:
+			phba->fc_linkspeed = LPFC_LINK_SPEED_UNKNOWN;
+			break;
+		}
 	}
 
 	if (phba->fc_topology &&
@@ -3448,10 +3453,10 @@ lpfc_mbx_cmpl_reg_login(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 		spin_lock_irq(shost->host_lock);
 		ndlp->nlp_flag &= ~NLP_IGNR_REG_CMPL;
 		spin_unlock_irq(shost->host_lock);
-	} else
-		/* Good status, call state machine */
-		lpfc_disc_state_machine(vport, ndlp, pmb,
-				NLP_EVT_CMPL_REG_LOGIN);
+	}
+
+	/* Call state machine */
+	lpfc_disc_state_machine(vport, ndlp, pmb, NLP_EVT_CMPL_REG_LOGIN);
 
 	lpfc_mbuf_free(phba, mp->virt, mp->phys);
 	kfree(mp);
@@ -5550,15 +5555,15 @@ lpfc_mbx_cmpl_fdmi_reg_login(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 			 ndlp->nlp_usg_map, ndlp);
 	/*
 	 * Start issuing Fabric-Device Management Interface (FDMI) command to
-	 * 0xfffffa (FDMI well known port) or Delay issuing FDMI command if
-	 * fdmi-on=2 (supporting RPA/hostnmae)
+	 * 0xfffffa (FDMI well known port).
+	 * DHBA -> DPRT -> RHBA -> RPA  (physical port)
+	 * DPRT -> RPRT (vports)
 	 */
-
-	if (vport->cfg_fdmi_on & LPFC_FDMI_REG_DELAY)
-		mod_timer(&vport->fc_fdmitmo,
-			  jiffies + msecs_to_jiffies(1000 * 60));
+	if (vport->port_type == LPFC_PHYSICAL_PORT)
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DHBA, 0);
 	else
-		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DHBA);
+		lpfc_fdmi_cmd(vport, ndlp, SLI_MGMT_DPRT, 0);
+
 
 	/* decrement the node reference count held for this callback
 	 * function.
diff --git a/drivers/scsi/lpfc/lpfc_hw.h b/drivers/scsi/lpfc/lpfc_hw.h
index 2cce88e967ce..dd20412c7e4c 100644
--- a/drivers/scsi/lpfc/lpfc_hw.h
+++ b/drivers/scsi/lpfc/lpfc_hw.h
@@ -1097,6 +1097,18 @@ struct fc_rdp_port_name_desc {
 };
 
 
+struct fc_rdp_fec_info {
+	uint32_t CorrectedBlocks;
+	uint32_t UncorrectableBlocks;
+};
+
+#define RDP_FEC_DESC_TAG  0x00010005
+struct fc_fec_rdp_desc {
+	uint32_t tag;
+	uint32_t length;
+	struct fc_rdp_fec_info info;
+};
+
 struct fc_rdp_link_error_status_payload_info {
 	struct fc_link_status link_status; /* 24 bytes */
 	uint32_t  port_type;             /* bits 31-30 only */
@@ -1196,14 +1208,15 @@ struct fc_rdp_res_frame {
 	struct fc_rdp_link_error_status_desc link_error_desc; /* Word 13-21 */
 	struct fc_rdp_port_name_desc diag_port_names_desc;    /* Word 22-27 */
 	struct fc_rdp_port_name_desc attached_port_names_desc;/* Word 28-33 */
+	struct fc_fec_rdp_desc fec_desc;	      /* FC Word 34 - 37 */
 };
 
 
 #define RDP_DESC_PAYLOAD_SIZE (sizeof(struct fc_rdp_link_service_desc) \
-			+ sizeof(struct fc_rdp_sfp_desc) \
-			+ sizeof(struct fc_rdp_port_speed_desc) \
-			+ sizeof(struct fc_rdp_link_error_status_desc) \
-			+ (sizeof(struct fc_rdp_port_name_desc) * 2))
+				+ sizeof(struct fc_rdp_sfp_desc) \
+				+ sizeof(struct fc_rdp_port_speed_desc) \
+				+ sizeof(struct fc_rdp_link_error_status_desc) \
+				+ (sizeof(struct fc_rdp_port_name_desc) * 2))
 
 
 /******** FDMI ********/
@@ -1233,31 +1246,10 @@ struct lpfc_fdmi_attr_def { /* Defined in TLV format */
 /* Attribute Entry */
 struct lpfc_fdmi_attr_entry {
 	union {
-		uint32_t VendorSpecific;
-		uint32_t SupportClass;
-		uint32_t SupportSpeed;
-		uint32_t PortSpeed;
-		uint32_t MaxFrameSize;
-		uint32_t MaxCTPayloadLen;
-		uint32_t PortState;
-		uint32_t PortId;
-		struct lpfc_name NodeName;
-		struct lpfc_name PortName;
-		struct lpfc_name FabricName;
-		uint8_t FC4Types[32];
-		uint8_t Manufacturer[64];
-		uint8_t SerialNumber[64];
-		uint8_t Model[256];
-		uint8_t ModelDescription[256];
-		uint8_t HardwareVersion[256];
-		uint8_t DriverVersion[256];
-		uint8_t OptionROMVersion[256];
-		uint8_t FirmwareVersion[256];
-		uint8_t OsHostName[256];
-		uint8_t NodeSymName[256];
-		uint8_t OsDeviceName[256];
-		uint8_t OsNameVersion[256];
-		uint8_t HostName[256];
+		uint32_t AttrInt;
+		uint8_t  AttrTypes[32];
+		uint8_t  AttrString[256];
+		struct lpfc_name AttrWWN;
 	} un;
 };
 
@@ -1327,6 +1319,8 @@ struct lpfc_fdmi_reg_portattr {
 #define  SLI_MGMT_DPRT     0x310	/* De-register Port */
 #define  SLI_MGMT_DPA      0x311	/* De-register Port attributes */
 
+#define LPFC_FDMI_MAX_RETRY     3  /* Max retries for a FDMI command */
+
 /*
  * HBA Attribute Types
  */
@@ -1342,6 +1336,39 @@ struct lpfc_fdmi_reg_portattr {
 #define  RHBA_OS_NAME_VERSION	 0xa /* 4 to 256 byte ASCII string */
 #define  RHBA_MAX_CT_PAYLOAD_LEN 0xb /* 32-bit unsigned int */
 #define  RHBA_SYM_NODENAME       0xc /* 4 to 256 byte ASCII string */
+#define  RHBA_VENDOR_INFO        0xd  /* 32-bit unsigned int */
+#define  RHBA_NUM_PORTS          0xe  /* 32-bit unsigned int */
+#define  RHBA_FABRIC_WWNN        0xf  /* 8 byte WWNN */
+#define  RHBA_BIOS_VERSION       0x10 /* 4 to 256 byte ASCII string */
+#define  RHBA_BIOS_STATE         0x11 /* 32-bit unsigned int */
+#define  RHBA_VENDOR_ID          0xe0 /* 8 byte ASCII string */
+
+/* Bit mask for all individual HBA attributes */
+#define LPFC_FDMI_HBA_ATTR_wwnn			0x00000001
+#define LPFC_FDMI_HBA_ATTR_manufacturer		0x00000002
+#define LPFC_FDMI_HBA_ATTR_sn			0x00000004
+#define LPFC_FDMI_HBA_ATTR_model		0x00000008
+#define LPFC_FDMI_HBA_ATTR_description		0x00000010
+#define LPFC_FDMI_HBA_ATTR_hdw_ver		0x00000020
+#define LPFC_FDMI_HBA_ATTR_drvr_ver		0x00000040
+#define LPFC_FDMI_HBA_ATTR_rom_ver		0x00000080
+#define LPFC_FDMI_HBA_ATTR_fmw_ver		0x00000100
+#define LPFC_FDMI_HBA_ATTR_os_ver		0x00000200
+#define LPFC_FDMI_HBA_ATTR_ct_len		0x00000400
+#define LPFC_FDMI_HBA_ATTR_symbolic_name	0x00000800
+#define LPFC_FDMI_HBA_ATTR_vendor_info		0x00001000 /* Not used */
+#define LPFC_FDMI_HBA_ATTR_num_ports		0x00002000
+#define LPFC_FDMI_HBA_ATTR_fabric_wwnn		0x00004000
+#define LPFC_FDMI_HBA_ATTR_bios_ver		0x00008000
+#define LPFC_FDMI_HBA_ATTR_bios_state		0x00010000 /* Not used */
+#define LPFC_FDMI_HBA_ATTR_vendor_id		0x00020000
+
+/* Bit mask for FDMI-1 defined HBA attributes */
+#define LPFC_FDMI1_HBA_ATTR			0x000007ff
+
+/* Bit mask for FDMI-2 defined HBA attributes */
+/* Skip vendor_info and bios_state */
+#define LPFC_FDMI2_HBA_ATTR			0x0002efff
 
 /*
  * Port Attrubute Types
@@ -1353,15 +1380,65 @@ struct lpfc_fdmi_reg_portattr {
 #define  RPRT_OS_DEVICE_NAME          0x5 /* 4 to 256 byte ASCII string */
 #define  RPRT_HOST_NAME               0x6 /* 4 to 256 byte ASCII string */
 #define  RPRT_NODENAME                0x7 /* 8 byte WWNN */
-#define  RPRT_PORTNAME                0x8 /* 8 byte WWNN */
+#define  RPRT_PORTNAME                0x8 /* 8 byte WWPN */
 #define  RPRT_SYM_PORTNAME            0x9 /* 4 to 256 byte ASCII string */
 #define  RPRT_PORT_TYPE               0xa /* 32-bit unsigned int */
 #define  RPRT_SUPPORTED_CLASS         0xb /* 32-bit unsigned int */
-#define  RPRT_FABRICNAME              0xc /* 8 byte Fabric WWNN */
+#define  RPRT_FABRICNAME              0xc /* 8 byte Fabric WWPN */
 #define  RPRT_ACTIVE_FC4_TYPES        0xd /* 32 byte binary array */
 #define  RPRT_PORT_STATE              0x101 /* 32-bit unsigned int */
 #define  RPRT_DISC_PORT               0x102 /* 32-bit unsigned int */
 #define  RPRT_PORT_ID                 0x103 /* 32-bit unsigned int */
+#define  RPRT_SMART_SERVICE           0xf100 /* 4 to 256 byte ASCII string */
+#define  RPRT_SMART_GUID              0xf101 /* 8 byte WWNN + 8 byte WWPN */
+#define  RPRT_SMART_VERSION           0xf102 /* 4 to 256 byte ASCII string */
+#define  RPRT_SMART_MODEL             0xf103 /* 4 to 256 byte ASCII string */
+#define  RPRT_SMART_PORT_INFO         0xf104 /* 32-bit unsigned int */
+#define  RPRT_SMART_QOS               0xf105 /* 32-bit unsigned int */
+#define  RPRT_SMART_SECURITY          0xf106 /* 32-bit unsigned int */
+
+/* Bit mask for all individual PORT attributes */
+#define LPFC_FDMI_PORT_ATTR_fc4type		0x00000001
+#define LPFC_FDMI_PORT_ATTR_support_speed	0x00000002
+#define LPFC_FDMI_PORT_ATTR_speed		0x00000004
+#define LPFC_FDMI_PORT_ATTR_max_frame		0x00000008
+#define LPFC_FDMI_PORT_ATTR_os_devname		0x00000010
+#define LPFC_FDMI_PORT_ATTR_host_name		0x00000020
+#define LPFC_FDMI_PORT_ATTR_wwnn		0x00000040
+#define LPFC_FDMI_PORT_ATTR_wwpn		0x00000080
+#define LPFC_FDMI_PORT_ATTR_symbolic_name	0x00000100
+#define LPFC_FDMI_PORT_ATTR_port_type		0x00000200
+#define LPFC_FDMI_PORT_ATTR_class		0x00000400
+#define LPFC_FDMI_PORT_ATTR_fabric_wwpn		0x00000800
+#define LPFC_FDMI_PORT_ATTR_port_state		0x00001000
+#define LPFC_FDMI_PORT_ATTR_active_fc4type	0x00002000
+#define LPFC_FDMI_PORT_ATTR_num_disc		0x00004000
+#define LPFC_FDMI_PORT_ATTR_nportid		0x00008000
+#define LPFC_FDMI_SMART_ATTR_service		0x00010000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_guid		0x00020000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_version		0x00040000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_model		0x00080000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_port_info		0x00100000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_qos		0x00200000 /* Vendor specific */
+#define LPFC_FDMI_SMART_ATTR_security		0x00400000 /* Vendor specific */
+
+/* Bit mask for FDMI-1 defined PORT attributes */
+#define LPFC_FDMI1_PORT_ATTR			0x0000003f
+
+/* Bit mask for FDMI-2 defined PORT attributes */
+#define LPFC_FDMI2_PORT_ATTR			0x0000ffff
+
+/* Bit mask for Smart SAN defined PORT attributes */
+#define LPFC_FDMI2_SMART_ATTR			0x007fffff
+
+/* Defines for PORT port state attribute */
+#define LPFC_FDMI_PORTSTATE_UNKNOWN	1
+#define LPFC_FDMI_PORTSTATE_ONLINE	2
+
+/* Defines for PORT port type attribute */
+#define LPFC_FDMI_PORTTYPE_UNKNOWN	0
+#define LPFC_FDMI_PORTTYPE_NPORT	1
+#define LPFC_FDMI_PORTTYPE_NLPORT	2
 
 /*
  *  Begin HBA configuration parameters.
@@ -2498,10 +2575,38 @@ typedef struct {
 /* Structure for MB Command READ_LINK_STAT (18) */
 
 typedef struct {
-	uint32_t rsvd1;
+	uint32_t word0;
+
+#define lpfc_read_link_stat_rec_SHIFT   0
+#define lpfc_read_link_stat_rec_MASK   0x1
+#define lpfc_read_link_stat_rec_WORD   word0
+
+#define lpfc_read_link_stat_gec_SHIFT	1
+#define lpfc_read_link_stat_gec_MASK   0x1
+#define lpfc_read_link_stat_gec_WORD   word0
+
+#define lpfc_read_link_stat_w02oftow23of_SHIFT	2
+#define lpfc_read_link_stat_w02oftow23of_MASK   0x3FFFFF
+#define lpfc_read_link_stat_w02oftow23of_WORD   word0
+
+#define lpfc_read_link_stat_rsvd_SHIFT	24
+#define lpfc_read_link_stat_rsvd_MASK   0x1F
+#define lpfc_read_link_stat_rsvd_WORD   word0
+
+#define lpfc_read_link_stat_gec2_SHIFT  29
+#define lpfc_read_link_stat_gec2_MASK   0x1
+#define lpfc_read_link_stat_gec2_WORD   word0
+
+#define lpfc_read_link_stat_clrc_SHIFT  30
+#define lpfc_read_link_stat_clrc_MASK   0x1
+#define lpfc_read_link_stat_clrc_WORD   word0
+
+#define lpfc_read_link_stat_clof_SHIFT  31
+#define lpfc_read_link_stat_clof_MASK   0x1
+#define lpfc_read_link_stat_clof_WORD   word0
+
 	uint32_t linkFailureCnt;
 	uint32_t lossSyncCnt;
-
 	uint32_t lossSignalCnt;
 	uint32_t primSeqErrCnt;
 	uint32_t invalidXmitWord;
@@ -2509,6 +2614,19 @@ typedef struct {
 	uint32_t primSeqTimeout;
 	uint32_t elasticOverrun;
 	uint32_t arbTimeout;
+	uint32_t advRecBufCredit;
+	uint32_t curRecBufCredit;
+	uint32_t advTransBufCredit;
+	uint32_t curTransBufCredit;
+	uint32_t recEofCount;
+	uint32_t recEofdtiCount;
+	uint32_t recEofniCount;
+	uint32_t recSofcount;
+	uint32_t rsvd1;
+	uint32_t rsvd2;
+	uint32_t recDrpXriCount;
+	uint32_t fecCorrBlkCount;
+	uint32_t fecUncorrBlkCount;
 } READ_LNK_VAR;
 
 /* Structure for MB Command REG_LOGIN (19) */
diff --git a/drivers/scsi/lpfc/lpfc_hw4.h b/drivers/scsi/lpfc/lpfc_hw4.h
index 33ec4fa39ccb..608f9415fb08 100644
--- a/drivers/scsi/lpfc/lpfc_hw4.h
+++ b/drivers/scsi/lpfc/lpfc_hw4.h
@@ -3317,6 +3317,7 @@ struct lpfc_acqe_link {
 #define LPFC_ASYNC_LINK_SPEED_20GBPS		0x5
 #define LPFC_ASYNC_LINK_SPEED_25GBPS		0x6
 #define LPFC_ASYNC_LINK_SPEED_40GBPS		0x7
+#define LPFC_ASYNC_LINK_SPEED_100GBPS		0x8
 #define lpfc_acqe_link_duplex_SHIFT		16
 #define lpfc_acqe_link_duplex_MASK		0x000000FF
 #define lpfc_acqe_link_duplex_WORD		word0
@@ -3447,23 +3448,50 @@ struct lpfc_acqe_fc_la {
 struct lpfc_acqe_misconfigured_event {
 	struct {
 	uint32_t word0;
-#define lpfc_sli_misconfigured_port0_SHIFT	0
-#define lpfc_sli_misconfigured_port0_MASK	0x000000FF
-#define lpfc_sli_misconfigured_port0_WORD	word0
-#define lpfc_sli_misconfigured_port1_SHIFT	8
-#define lpfc_sli_misconfigured_port1_MASK	0x000000FF
-#define lpfc_sli_misconfigured_port1_WORD	word0
-#define lpfc_sli_misconfigured_port2_SHIFT	16
-#define lpfc_sli_misconfigured_port2_MASK	0x000000FF
-#define lpfc_sli_misconfigured_port2_WORD	word0
-#define lpfc_sli_misconfigured_port3_SHIFT	24
-#define lpfc_sli_misconfigured_port3_MASK	0x000000FF
-#define lpfc_sli_misconfigured_port3_WORD	word0
+#define lpfc_sli_misconfigured_port0_state_SHIFT	0
+#define lpfc_sli_misconfigured_port0_state_MASK		0x000000FF
+#define lpfc_sli_misconfigured_port0_state_WORD		word0
+#define lpfc_sli_misconfigured_port1_state_SHIFT	8
+#define lpfc_sli_misconfigured_port1_state_MASK		0x000000FF
+#define lpfc_sli_misconfigured_port1_state_WORD		word0
+#define lpfc_sli_misconfigured_port2_state_SHIFT	16
+#define lpfc_sli_misconfigured_port2_state_MASK		0x000000FF
+#define lpfc_sli_misconfigured_port2_state_WORD		word0
+#define lpfc_sli_misconfigured_port3_state_SHIFT	24
+#define lpfc_sli_misconfigured_port3_state_MASK		0x000000FF
+#define lpfc_sli_misconfigured_port3_state_WORD		word0
+	uint32_t word1;
+#define lpfc_sli_misconfigured_port0_op_SHIFT		0
+#define lpfc_sli_misconfigured_port0_op_MASK		0x00000001
+#define lpfc_sli_misconfigured_port0_op_WORD		word1
+#define lpfc_sli_misconfigured_port0_severity_SHIFT	1
+#define lpfc_sli_misconfigured_port0_severity_MASK	0x00000003
+#define lpfc_sli_misconfigured_port0_severity_WORD	word1
+#define lpfc_sli_misconfigured_port1_op_SHIFT		8
+#define lpfc_sli_misconfigured_port1_op_MASK		0x00000001
+#define lpfc_sli_misconfigured_port1_op_WORD		word1
+#define lpfc_sli_misconfigured_port1_severity_SHIFT	9
+#define lpfc_sli_misconfigured_port1_severity_MASK	0x00000003
+#define lpfc_sli_misconfigured_port1_severity_WORD	word1
+#define lpfc_sli_misconfigured_port2_op_SHIFT		16
+#define lpfc_sli_misconfigured_port2_op_MASK		0x00000001
+#define lpfc_sli_misconfigured_port2_op_WORD		word1
+#define lpfc_sli_misconfigured_port2_severity_SHIFT	17
+#define lpfc_sli_misconfigured_port2_severity_MASK	0x00000003
+#define lpfc_sli_misconfigured_port2_severity_WORD	word1
+#define lpfc_sli_misconfigured_port3_op_SHIFT		24
+#define lpfc_sli_misconfigured_port3_op_MASK		0x00000001
+#define lpfc_sli_misconfigured_port3_op_WORD		word1
+#define lpfc_sli_misconfigured_port3_severity_SHIFT	25
+#define lpfc_sli_misconfigured_port3_severity_MASK	0x00000003
+#define lpfc_sli_misconfigured_port3_severity_WORD	word1
 	} theEvent;
 #define LPFC_SLI_EVENT_STATUS_VALID			0x00
 #define LPFC_SLI_EVENT_STATUS_NOT_PRESENT	0x01
 #define LPFC_SLI_EVENT_STATUS_WRONG_TYPE	0x02
 #define LPFC_SLI_EVENT_STATUS_UNSUPPORTED	0x03
+#define LPFC_SLI_EVENT_STATUS_UNQUALIFIED	0x04
+#define LPFC_SLI_EVENT_STATUS_UNCERTIFIED	0x05
 };
 
 struct lpfc_acqe_sli {
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index db9446c612da..a544366a367e 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -1184,8 +1184,10 @@ lpfc_hb_timeout_handler(struct lpfc_hba *phba)
 
 	vports = lpfc_create_vport_work_array(phba);
 	if (vports != NULL)
-		for (i = 0; i <= phba->max_vports && vports[i] != NULL; i++)
+		for (i = 0; i <= phba->max_vports && vports[i] != NULL; i++) {
 			lpfc_rcv_seq_check_edtov(vports[i]);
+			lpfc_fdmi_num_disc_check(vports[i]);
+		}
 	lpfc_destroy_vport_work_array(phba, vports);
 
 	if ((phba->link_state == LPFC_HBA_ERROR) ||
@@ -1290,6 +1292,10 @@ lpfc_hb_timeout_handler(struct lpfc_hba *phba)
 				jiffies +
 				msecs_to_jiffies(1000 * LPFC_HB_MBOX_TIMEOUT));
 		}
+	} else {
+			mod_timer(&phba->hb_tmofunc,
+				jiffies +
+				msecs_to_jiffies(1000 * LPFC_HB_MBOX_INTERVAL));
 	}
 }
 
@@ -2621,7 +2627,6 @@ void
 lpfc_stop_vport_timers(struct lpfc_vport *vport)
 {
 	del_timer_sync(&vport->els_tmofunc);
-	del_timer_sync(&vport->fc_fdmitmo);
 	del_timer_sync(&vport->delayed_disc_tmo);
 	lpfc_can_disctmo(vport);
 	return;
@@ -3340,10 +3345,6 @@ lpfc_create_port(struct lpfc_hba *phba, int instance, struct device *dev)
 	vport->fc_disctmo.function = lpfc_disc_timeout;
 	vport->fc_disctmo.data = (unsigned long)vport;
 
-	init_timer(&vport->fc_fdmitmo);
-	vport->fc_fdmitmo.function = lpfc_fdmi_tmo;
-	vport->fc_fdmitmo.data = (unsigned long)vport;
-
 	init_timer(&vport->els_tmofunc);
 	vport->els_tmofunc.function = lpfc_els_timeout;
 	vport->els_tmofunc.data = (unsigned long)vport;
@@ -3709,49 +3710,6 @@ lpfc_sli4_parse_latt_type(struct lpfc_hba *phba,
 }
 
 /**
- * lpfc_sli4_parse_latt_link_speed - Parse sli4 link-attention link speed
- * @phba: pointer to lpfc hba data structure.
- * @acqe_link: pointer to the async link completion queue entry.
- *
- * This routine is to parse the SLI4 link-attention link speed and translate
- * it into the base driver's link-attention link speed coding.
- *
- * Return: Link-attention link speed in terms of base driver's coding.
- **/
-static uint8_t
-lpfc_sli4_parse_latt_link_speed(struct lpfc_hba *phba,
-				struct lpfc_acqe_link *acqe_link)
-{
-	uint8_t link_speed;
-
-	switch (bf_get(lpfc_acqe_link_speed, acqe_link)) {
-	case LPFC_ASYNC_LINK_SPEED_ZERO:
-	case LPFC_ASYNC_LINK_SPEED_10MBPS:
-	case LPFC_ASYNC_LINK_SPEED_100MBPS:
-		link_speed = LPFC_LINK_SPEED_UNKNOWN;
-		break;
-	case LPFC_ASYNC_LINK_SPEED_1GBPS:
-		link_speed = LPFC_LINK_SPEED_1GHZ;
-		break;
-	case LPFC_ASYNC_LINK_SPEED_10GBPS:
-		link_speed = LPFC_LINK_SPEED_10GHZ;
-		break;
-	case LPFC_ASYNC_LINK_SPEED_20GBPS:
-	case LPFC_ASYNC_LINK_SPEED_25GBPS:
-	case LPFC_ASYNC_LINK_SPEED_40GBPS:
-		link_speed = LPFC_LINK_SPEED_UNKNOWN;
-		break;
-	default:
-		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-				"0483 Invalid link-attention link speed: x%x\n",
-				bf_get(lpfc_acqe_link_speed, acqe_link));
-		link_speed = LPFC_LINK_SPEED_UNKNOWN;
-		break;
-	}
-	return link_speed;
-}
-
-/**
  * lpfc_sli_port_speed_get - Get sli3 link speed code to link speed
  * @phba: pointer to lpfc hba data structure.
  *
@@ -3767,27 +3725,35 @@ lpfc_sli_port_speed_get(struct lpfc_hba *phba)
 	if (!lpfc_is_link_up(phba))
 		return 0;
 
-	switch (phba->fc_linkspeed) {
-	case LPFC_LINK_SPEED_1GHZ:
-		link_speed = 1000;
-		break;
-	case LPFC_LINK_SPEED_2GHZ:
-		link_speed = 2000;
-		break;
-	case LPFC_LINK_SPEED_4GHZ:
-		link_speed = 4000;
-		break;
-	case LPFC_LINK_SPEED_8GHZ:
-		link_speed = 8000;
-		break;
-	case LPFC_LINK_SPEED_10GHZ:
-		link_speed = 10000;
-		break;
-	case LPFC_LINK_SPEED_16GHZ:
-		link_speed = 16000;
-		break;
-	default:
-		link_speed = 0;
+	if (phba->sli_rev <= LPFC_SLI_REV3) {
+		switch (phba->fc_linkspeed) {
+		case LPFC_LINK_SPEED_1GHZ:
+			link_speed = 1000;
+			break;
+		case LPFC_LINK_SPEED_2GHZ:
+			link_speed = 2000;
+			break;
+		case LPFC_LINK_SPEED_4GHZ:
+			link_speed = 4000;
+			break;
+		case LPFC_LINK_SPEED_8GHZ:
+			link_speed = 8000;
+			break;
+		case LPFC_LINK_SPEED_10GHZ:
+			link_speed = 10000;
+			break;
+		case LPFC_LINK_SPEED_16GHZ:
+			link_speed = 16000;
+			break;
+		default:
+			link_speed = 0;
+		}
+	} else {
+		if (phba->sli4_hba.link_state.logical_speed)
+			link_speed =
+			      phba->sli4_hba.link_state.logical_speed;
+		else
+			link_speed = phba->sli4_hba.link_state.speed;
 	}
 	return link_speed;
 }
@@ -3983,7 +3949,7 @@ lpfc_sli4_async_link_evt(struct lpfc_hba *phba,
 	la->eventTag = acqe_link->event_tag;
 	bf_set(lpfc_mbx_read_top_att_type, la, att_type);
 	bf_set(lpfc_mbx_read_top_link_spd, la,
-	       lpfc_sli4_parse_latt_link_speed(phba, acqe_link));
+	       (bf_get(lpfc_acqe_link_speed, acqe_link)));
 
 	/* Fake the the following irrelvant fields */
 	bf_set(lpfc_mbx_read_top_topology, la, LPFC_TOPOLOGY_PT_PT);
@@ -4113,22 +4079,18 @@ lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
 	char message[128];
 	uint8_t status;
 	uint8_t evt_type;
+	uint8_t operational = 0;
 	struct temp_event temp_event_data;
 	struct lpfc_acqe_misconfigured_event *misconfigured;
 	struct Scsi_Host  *shost;
 
 	evt_type = bf_get(lpfc_trailer_type, acqe_sli);
 
-	/* Special case Lancer */
-	if (bf_get(lpfc_sli_intf_if_type, &phba->sli4_hba.sli_intf) !=
-		 LPFC_SLI_INTF_IF_TYPE_2) {
-		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
-				"2901 Async SLI event - Event Data1:x%08x Event Data2:"
-				"x%08x SLI Event Type:%d\n",
-				acqe_sli->event_data1, acqe_sli->event_data2,
-				evt_type);
-		return;
-	}
+	lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
+			"2901 Async SLI event - Event Data1:x%08x Event Data2:"
+			"x%08x SLI Event Type:%d\n",
+			acqe_sli->event_data1, acqe_sli->event_data2,
+			evt_type);
 
 	port_name = phba->Port[0];
 	if (port_name == 0x00)
@@ -4174,29 +4136,46 @@ lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
 		/* fetch the status for this port */
 		switch (phba->sli4_hba.lnk_info.lnk_no) {
 		case LPFC_LINK_NUMBER_0:
-			status = bf_get(lpfc_sli_misconfigured_port0,
+			status = bf_get(lpfc_sli_misconfigured_port0_state,
+					&misconfigured->theEvent);
+			operational = bf_get(lpfc_sli_misconfigured_port0_op,
 					&misconfigured->theEvent);
 			break;
 		case LPFC_LINK_NUMBER_1:
-			status = bf_get(lpfc_sli_misconfigured_port1,
+			status = bf_get(lpfc_sli_misconfigured_port1_state,
+					&misconfigured->theEvent);
+			operational = bf_get(lpfc_sli_misconfigured_port1_op,
 					&misconfigured->theEvent);
 			break;
 		case LPFC_LINK_NUMBER_2:
-			status = bf_get(lpfc_sli_misconfigured_port2,
+			status = bf_get(lpfc_sli_misconfigured_port2_state,
+					&misconfigured->theEvent);
+			operational = bf_get(lpfc_sli_misconfigured_port2_op,
 					&misconfigured->theEvent);
 			break;
 		case LPFC_LINK_NUMBER_3:
-			status = bf_get(lpfc_sli_misconfigured_port3,
+			status = bf_get(lpfc_sli_misconfigured_port3_state,
+					&misconfigured->theEvent);
+			operational = bf_get(lpfc_sli_misconfigured_port3_op,
 					&misconfigured->theEvent);
 			break;
 		default:
-			status = ~LPFC_SLI_EVENT_STATUS_VALID;
-			break;
+			lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
+					"3296 "
+					"LPFC_SLI_EVENT_TYPE_MISCONFIGURED "
+					"event: Invalid link %d",
+					phba->sli4_hba.lnk_info.lnk_no);
+			return;
 		}
 
+		/* Skip if optic state unchanged */
+		if (phba->sli4_hba.lnk_info.optic_state == status)
+			return;
+
 		switch (status) {
 		case LPFC_SLI_EVENT_STATUS_VALID:
-			return; /* no message if the sfp is okay */
+			sprintf(message, "Physical Link is functional");
+			break;
 		case LPFC_SLI_EVENT_STATUS_NOT_PRESENT:
 			sprintf(message, "Optics faulted/incorrectly "
 				"installed/not installed - Reseat optics, "
@@ -4211,15 +4190,26 @@ lpfc_sli4_async_sli_evt(struct lpfc_hba *phba, struct lpfc_acqe_sli *acqe_sli)
 			sprintf(message, "Incompatible optics - Replace with "
 				"compatible optics for card to function.");
 			break;
+		case LPFC_SLI_EVENT_STATUS_UNQUALIFIED:
+			sprintf(message, "Unqualified optics - Replace with "
+				"Avago optics for Warranty and Technical "
+				"Support - Link is%s operational",
+				(operational) ? "" : " not");
+			break;
+		case LPFC_SLI_EVENT_STATUS_UNCERTIFIED:
+			sprintf(message, "Uncertified optics - Replace with "
+				"Avago-certified optics to enable link "
+				"operation - Link is%s operational",
+				(operational) ? "" : " not");
+			break;
 		default:
 			/* firmware is reporting a status we don't know about */
 			sprintf(message, "Unknown event status x%02x", status);
 			break;
 		}
-
+		phba->sli4_hba.lnk_info.optic_state = status;
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
-				"3176 Misconfigured Physical Port - "
-				"Port Name %c %s\n", port_name, message);
+				"3176 Port Name %c %s\n", port_name, message);
 		break;
 	case LPFC_SLI_EVENT_TYPE_REMOTE_DPORT:
 		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -5293,6 +5283,9 @@ lpfc_sli4_driver_resource_setup(struct lpfc_hba *phba)
 	INIT_LIST_HEAD(&phba->sli4_hba.lpfc_vfi_blk_list);
 	INIT_LIST_HEAD(&phba->lpfc_vpi_blk_list);
 
+	/* initialize optic_state to 0xFF */
+	phba->sli4_hba.lnk_info.optic_state = 0xff;
+
 	/* Initialize the driver internal SLI layer lists. */
 	lpfc_sli_setup(phba);
 	lpfc_sli_queue_setup(phba);
@@ -6159,6 +6152,20 @@ lpfc_create_shost(struct lpfc_hba *phba)
 	/* Put reference to SCSI host to driver's device private data */
 	pci_set_drvdata(phba->pcidev, shost);
 
+	/*
+	 * At this point we are fully registered with PSA. In addition,
+	 * any initial discovery should be completed.
+	 */
+	vport->load_flag |= FC_ALLOW_FDMI;
+	if (phba->cfg_fdmi_on > LPFC_FDMI_NO_SUPPORT) {
+
+		/* Setup appropriate attribute masks */
+		vport->fdmi_hba_mask = LPFC_FDMI2_HBA_ATTR;
+		if (phba->cfg_fdmi_on == LPFC_FDMI_SMART_SAN)
+			vport->fdmi_port_mask = LPFC_FDMI2_SMART_ATTR;
+		else
+			vport->fdmi_port_mask = LPFC_FDMI2_PORT_ATTR;
+	}
 	return 0;
 }
 
@@ -8833,9 +8840,12 @@ found:
 				 * already mapped to this phys_id.
 				 */
 				if (cpup->irq != LPFC_VECTOR_MAP_EMPTY) {
-					chann[saved_chann] =
-						cpup->channel_id;
-					saved_chann++;
+					if (saved_chann <=
+					    LPFC_FCP_IO_CHAN_MAX) {
+						chann[saved_chann] =
+							cpup->channel_id;
+						saved_chann++;
+					}
 					goto out;
 				}
 
diff --git a/drivers/scsi/lpfc/lpfc_mem.c b/drivers/scsi/lpfc/lpfc_mem.c
index 3fa65338d3f5..4fb3581d4614 100644
--- a/drivers/scsi/lpfc/lpfc_mem.c
+++ b/drivers/scsi/lpfc/lpfc_mem.c
@@ -231,15 +231,13 @@ lpfc_mem_free(struct lpfc_hba *phba)
 	if (phba->lpfc_hbq_pool)
 		pci_pool_destroy(phba->lpfc_hbq_pool);
 	phba->lpfc_hbq_pool = NULL;
-
-	if (phba->rrq_pool)
-		mempool_destroy(phba->rrq_pool);
+	mempool_destroy(phba->rrq_pool);
 	phba->rrq_pool = NULL;
 
 	/* Free NLP memory pool */
 	mempool_destroy(phba->nlp_mem_pool);
 	phba->nlp_mem_pool = NULL;
-	if (phba->sli_rev == LPFC_SLI_REV4 && phba->active_rrq_pool) {
+	if (phba->sli_rev == LPFC_SLI_REV4) {
 		mempool_destroy(phba->active_rrq_pool);
 		phba->active_rrq_pool = NULL;
 	}
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index ed9a2c80c4aa..193733e8c823 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -280,38 +280,12 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	uint32_t *lp;
 	IOCB_t *icmd;
 	struct serv_parm *sp;
+	uint32_t ed_tov;
 	LPFC_MBOXQ_t *mbox;
 	struct ls_rjt stat;
 	int rc;
 
 	memset(&stat, 0, sizeof (struct ls_rjt));
-	if (vport->port_state <= LPFC_FDISC) {
-		/* Before responding to PLOGI, check for pt2pt mode.
-		 * If we are pt2pt, with an outstanding FLOGI, abort
-		 * the FLOGI and resend it first.
-		 */
-		if (vport->fc_flag & FC_PT2PT) {
-			 lpfc_els_abort_flogi(phba);
-		        if (!(vport->fc_flag & FC_PT2PT_PLOGI)) {
-				/* If the other side is supposed to initiate
-				 * the PLOGI anyway, just ACC it now and
-				 * move on with discovery.
-				 */
-				phba->fc_edtov = FF_DEF_EDTOV;
-				phba->fc_ratov = FF_DEF_RATOV;
-				/* Start discovery - this should just do
-				   CLEAR_LA */
-				lpfc_disc_start(vport);
-			} else
-				lpfc_initial_flogi(vport);
-		} else {
-			stat.un.b.lsRjtRsnCode = LSRJT_LOGICAL_BSY;
-			stat.un.b.lsRjtRsnCodeExp = LSEXP_NOTHING_MORE;
-			lpfc_els_rsp_reject(vport, stat.un.lsRjtError, cmdiocb,
-					    ndlp, NULL);
-			return 0;
-		}
-	}
 	pcmd = (struct lpfc_dmabuf *) cmdiocb->context2;
 	lp = (uint32_t *) pcmd->virt;
 	sp = (struct serv_parm *) ((uint8_t *) lp + sizeof (uint32_t));
@@ -404,30 +378,46 @@ lpfc_rcv_plogi(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp,
 	/* Check for Nport to NPort pt2pt protocol */
 	if ((vport->fc_flag & FC_PT2PT) &&
 	    !(vport->fc_flag & FC_PT2PT_PLOGI)) {
-
 		/* rcv'ed PLOGI decides what our NPortId will be */
 		vport->fc_myDID = icmd->un.rcvels.parmRo;
-		mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
-		if (mbox == NULL)
-			goto out;
-		lpfc_config_link(phba, mbox);
-		mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
-		mbox->vport = vport;
-		rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
-		if (rc == MBX_NOT_FINISHED) {
-			mempool_free(mbox, phba->mbox_mem_pool);
-			goto out;
+
+		ed_tov = be32_to_cpu(sp->cmn.e_d_tov);
+		if (sp->cmn.edtovResolution) {
+			/* E_D_TOV ticks are in nanoseconds */
+			ed_tov = (phba->fc_edtov + 999999) / 1000000;
 		}
+
 		/*
-		 * For SLI4, the VFI/VPI are registered AFTER the
-		 * Nport with the higher WWPN sends us a PLOGI with
-		 * our assigned NPortId.
+		 * For pt-to-pt, use the larger EDTOV
+		 * RATOV = 2 * EDTOV
 		 */
+		if (ed_tov > phba->fc_edtov)
+			phba->fc_edtov = ed_tov;
+		phba->fc_ratov = (2 * phba->fc_edtov) / 1000;
+
+		memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
+
+		/* Issue config_link / reg_vfi to account for updated TOV's */
+
 		if (phba->sli_rev == LPFC_SLI_REV4)
 			lpfc_issue_reg_vfi(vport);
+		else {
+			mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+			if (mbox == NULL)
+				goto out;
+			lpfc_config_link(phba, mbox);
+			mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+			mbox->vport = vport;
+			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+			if (rc == MBX_NOT_FINISHED) {
+				mempool_free(mbox, phba->mbox_mem_pool);
+				goto out;
+			}
+		}
 
 		lpfc_can_disctmo(vport);
 	}
+
 	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!mbox)
 		goto out;
@@ -1038,7 +1028,9 @@ lpfc_cmpl_plogi_plogi_issue(struct lpfc_vport *vport,
 	uint32_t *lp;
 	IOCB_t *irsp;
 	struct serv_parm *sp;
+	uint32_t ed_tov;
 	LPFC_MBOXQ_t *mbox;
+	int rc;
 
 	cmdiocb = (struct lpfc_iocbq *) arg;
 	rspiocb = cmdiocb->context_un.rsp_iocb;
@@ -1094,18 +1086,63 @@ lpfc_cmpl_plogi_plogi_issue(struct lpfc_vport *vport,
 	ndlp->nlp_maxframe =
 		((sp->cmn.bbRcvSizeMsb & 0x0F) << 8) | sp->cmn.bbRcvSizeLsb;
 
+	if ((vport->fc_flag & FC_PT2PT) &&
+	    (vport->fc_flag & FC_PT2PT_PLOGI)) {
+		ed_tov = be32_to_cpu(sp->cmn.e_d_tov);
+		if (sp->cmn.edtovResolution) {
+			/* E_D_TOV ticks are in nanoseconds */
+			ed_tov = (phba->fc_edtov + 999999) / 1000000;
+		}
+
+		/*
+		 * Use the larger EDTOV
+		 * RATOV = 2 * EDTOV for pt-to-pt
+		 */
+		if (ed_tov > phba->fc_edtov)
+			phba->fc_edtov = ed_tov;
+		phba->fc_ratov = (2 * phba->fc_edtov) / 1000;
+
+		memcpy(&phba->fc_fabparam, sp, sizeof(struct serv_parm));
+
+		/* Issue config_link / reg_vfi to account for updated TOV's */
+		if (phba->sli_rev == LPFC_SLI_REV4) {
+			lpfc_issue_reg_vfi(vport);
+		} else {
+			mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
+			if (!mbox) {
+				lpfc_printf_vlog(vport, KERN_ERR, LOG_ELS,
+						 "0133 PLOGI: no memory "
+						 "for config_link "
+						 "Data: x%x x%x x%x x%x\n",
+						 ndlp->nlp_DID, ndlp->nlp_state,
+						 ndlp->nlp_flag, ndlp->nlp_rpi);
+				goto out;
+			}
+
+			lpfc_config_link(phba, mbox);
+
+			mbox->mbox_cmpl = lpfc_sli_def_mbox_cmpl;
+			mbox->vport = vport;
+			rc = lpfc_sli_issue_mbox(phba, mbox, MBX_NOWAIT);
+			if (rc == MBX_NOT_FINISHED) {
+				mempool_free(mbox, phba->mbox_mem_pool);
+				goto out;
+			}
+		}
+	}
+
+	lpfc_unreg_rpi(vport, ndlp);
+
 	mbox = mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
 	if (!mbox) {
 		lpfc_printf_vlog(vport, KERN_ERR, LOG_ELS,
-			"0133 PLOGI: no memory for reg_login "
-			"Data: x%x x%x x%x x%x\n",
-			ndlp->nlp_DID, ndlp->nlp_state,
-			ndlp->nlp_flag, ndlp->nlp_rpi);
+				 "0018 PLOGI: no memory for reg_login "
+				 "Data: x%x x%x x%x x%x\n",
+				 ndlp->nlp_DID, ndlp->nlp_state,
+				 ndlp->nlp_flag, ndlp->nlp_rpi);
 		goto out;
 	}
 
-	lpfc_unreg_rpi(vport, ndlp);
-
 	if (lpfc_reg_rpi(phba, vport->vpi, irsp->un.elsreq64.remoteID,
 			 (uint8_t *) sp, mbox, ndlp->nlp_rpi) == 0) {
 		switch (ndlp->nlp_DID) {
@@ -2299,6 +2336,9 @@ lpfc_cmpl_reglogin_npr_node(struct lpfc_vport *vport,
 		if (vport->phba->sli_rev < LPFC_SLI_REV4)
 			ndlp->nlp_rpi = mb->un.varWords[0];
 		ndlp->nlp_flag |= NLP_RPI_REGISTERED;
+		if (ndlp->nlp_flag & NLP_LOGO_ACC) {
+			lpfc_unreg_rpi(vport, ndlp);
+		}
 	} else {
 		if (ndlp->nlp_flag & NLP_NODEV_REMOVE) {
 			lpfc_drop_node(vport, ndlp);
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 4679ed4444a7..152b3c8a5428 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -3676,6 +3676,7 @@ static void
 lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 		    struct lpfc_iocbq *rsp_iocb)
 {
+	struct lpfc_hba *phba = vport->phba;
 	struct scsi_cmnd *cmnd = lpfc_cmd->pCmd;
 	struct fcp_cmnd *fcpcmd = lpfc_cmd->fcp_cmnd;
 	struct fcp_rsp *fcprsp = lpfc_cmd->fcp_rsp;
@@ -3685,6 +3686,7 @@ lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 	uint32_t *lp;
 	uint32_t host_status = DID_OK;
 	uint32_t rsplen = 0;
+	uint32_t fcpDl;
 	uint32_t logit = LOG_FCP | LOG_FCP_ERROR;
 
 
@@ -3755,13 +3757,14 @@ lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 			 fcprsp->rspInfo3);
 
 	scsi_set_resid(cmnd, 0);
+	fcpDl = be32_to_cpu(fcpcmd->fcpDl);
 	if (resp_info & RESID_UNDER) {
 		scsi_set_resid(cmnd, be32_to_cpu(fcprsp->rspResId));
 
 		lpfc_printf_vlog(vport, KERN_INFO, LOG_FCP_UNDER,
 				 "9025 FCP Read Underrun, expected %d, "
 				 "residual %d Data: x%x x%x x%x\n",
-				 be32_to_cpu(fcpcmd->fcpDl),
+				 fcpDl,
 				 scsi_get_resid(cmnd), fcpi_parm, cmnd->cmnd[0],
 				 cmnd->underflow);
 
@@ -3777,7 +3780,7 @@ lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 					 LOG_FCP | LOG_FCP_ERROR,
 					 "9026 FCP Read Check Error "
 					 "and Underrun Data: x%x x%x x%x x%x\n",
-					 be32_to_cpu(fcpcmd->fcpDl),
+					 fcpDl,
 					 scsi_get_resid(cmnd), fcpi_parm,
 					 cmnd->cmnd[0]);
 			scsi_set_resid(cmnd, scsi_bufflen(cmnd));
@@ -3812,13 +3815,25 @@ lpfc_handle_fcp_err(struct lpfc_vport *vport, struct lpfc_scsi_buf *lpfc_cmd,
 	 * Check SLI validation that all the transfer was actually done
 	 * (fcpi_parm should be zero). Apply check only to reads.
 	 */
-	} else if (fcpi_parm && (cmnd->sc_data_direction == DMA_FROM_DEVICE)) {
+	} else if (fcpi_parm) {
 		lpfc_printf_vlog(vport, KERN_WARNING, LOG_FCP | LOG_FCP_ERROR,
-				 "9029 FCP Read Check Error Data: "
+				 "9029 FCP %s Check Error xri x%x  Data: "
 				 "x%x x%x x%x x%x x%x\n",
-				 be32_to_cpu(fcpcmd->fcpDl),
-				 be32_to_cpu(fcprsp->rspResId),
+				 ((cmnd->sc_data_direction == DMA_FROM_DEVICE) ?
+				 "Read" : "Write"),
+				 ((phba->sli_rev == LPFC_SLI_REV4) ?
+				 lpfc_cmd->cur_iocbq.sli4_xritag :
+				 rsp_iocb->iocb.ulpContext),
+				 fcpDl, be32_to_cpu(fcprsp->rspResId),
 				 fcpi_parm, cmnd->cmnd[0], scsi_status);
+
+		/* There is some issue with the LPe12000 that causes it
+		 * to miscalculate the fcpi_parm and falsely trip this
+		 * recovery logic.  Detect this case and don't error when true.
+		 */
+		if (fcpi_parm > fcpDl)
+			goto out;
+
 		switch (scsi_status) {
 		case SAM_STAT_GOOD:
 		case SAM_STAT_CHECK_CONDITION:
@@ -3908,9 +3923,9 @@ lpfc_scsi_cmd_iocb_cmpl(struct lpfc_hba *phba, struct lpfc_iocbq *pIocbIn,
 	uint32_t logit = LOG_FCP;
 
 	/* Sanity check on return of outstanding command */
-	if (!(lpfc_cmd->pCmd))
-		return;
 	cmd = lpfc_cmd->pCmd;
+	if (!cmd)
+		return;
 	shost = cmd->device->host;
 
 	lpfc_cmd->result = (pIocbOut->iocb.un.ulpWord[4] & IOERR_PARAM_MASK);
@@ -4446,15 +4461,7 @@ lpfc_info(struct Scsi_Host *host)
 				 phba->Port);
 		}
 		len = strlen(lpfcinfobuf);
-		if (phba->sli_rev <= LPFC_SLI_REV3) {
-			link_speed = lpfc_sli_port_speed_get(phba);
-		} else {
-			if (phba->sli4_hba.link_state.logical_speed)
-				link_speed =
-				      phba->sli4_hba.link_state.logical_speed;
-			else
-				link_speed = phba->sli4_hba.link_state.speed;
-		}
+		link_speed = lpfc_sli_port_speed_get(phba);
 		if (link_speed != 0)
 			snprintf(lpfcinfobuf + len, 384-len,
 				 " Logical Link Speed: %d Mbps", link_speed);
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index f9585cdd8933..92dfd6a5178c 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -14842,10 +14842,12 @@ lpfc_fc_frame_add(struct lpfc_vport *vport, struct hbq_dmabuf *dmabuf)
 	struct lpfc_dmabuf *h_buf;
 	struct hbq_dmabuf *seq_dmabuf = NULL;
 	struct hbq_dmabuf *temp_dmabuf = NULL;
+	uint8_t	found = 0;
 
 	INIT_LIST_HEAD(&dmabuf->dbuf.list);
 	dmabuf->time_stamp = jiffies;
 	new_hdr = (struct fc_frame_header *)dmabuf->hbuf.virt;
+
 	/* Use the hdr_buf to find the sequence that this frame belongs to */
 	list_for_each_entry(h_buf, &vport->rcv_buffer_list, list) {
 		temp_hdr = (struct fc_frame_header *)h_buf->virt;
@@ -14885,7 +14887,8 @@ lpfc_fc_frame_add(struct lpfc_vport *vport, struct hbq_dmabuf *dmabuf)
 		return seq_dmabuf;
 	}
 	/* find the correct place in the sequence to insert this frame */
-	list_for_each_entry_reverse(d_buf, &seq_dmabuf->dbuf.list, list) {
+	d_buf = list_entry(seq_dmabuf->dbuf.list.prev, typeof(*d_buf), list);
+	while (!found) {
 		temp_dmabuf = container_of(d_buf, struct hbq_dmabuf, dbuf);
 		temp_hdr = (struct fc_frame_header *)temp_dmabuf->hbuf.virt;
 		/*
@@ -14895,9 +14898,17 @@ lpfc_fc_frame_add(struct lpfc_vport *vport, struct hbq_dmabuf *dmabuf)
 		if (be16_to_cpu(new_hdr->fh_seq_cnt) >
 			be16_to_cpu(temp_hdr->fh_seq_cnt)) {
 			list_add(&dmabuf->dbuf.list, &temp_dmabuf->dbuf.list);
-			return seq_dmabuf;
+			found = 1;
+			break;
 		}
+
+		if (&d_buf->list == &seq_dmabuf->dbuf.list)
+			break;
+		d_buf = list_entry(d_buf->list.prev, typeof(*d_buf), list);
 	}
+
+	if (found)
+		return seq_dmabuf;
 	return NULL;
 }
 
@@ -16173,7 +16184,7 @@ fail_fcf_read:
 }
 
 /**
- * lpfc_check_next_fcf_pri
+ * lpfc_check_next_fcf_pri_level
  * phba pointer to the lpfc_hba struct for this port.
  * This routine is called from the lpfc_sli4_fcf_rr_next_index_get
  * routine when the rr_bmask is empty. The FCF indecies are put into the
@@ -16329,8 +16340,12 @@ next_priority:
 
 	if (next_fcf_index < LPFC_SLI4_FCF_TBL_INDX_MAX &&
 		phba->fcf.fcf_pri[next_fcf_index].fcf_rec.flag &
-		LPFC_FCF_FLOGI_FAILED)
+		LPFC_FCF_FLOGI_FAILED) {
+		if (list_is_singular(&phba->fcf.fcf_pri_list))
+			return LPFC_FCOE_FCF_NEXT_NONE;
+
 		goto next_priority;
+	}
 
 	lpfc_printf_log(phba, KERN_INFO, LOG_FIP,
 			"2845 Get next roundrobin failover FCF (x%x)\n",
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index 1e916e16ce98..cd780c29495a 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -442,6 +442,7 @@ struct lpfc_sli4_lnk_info {
 #define LPFC_LNK_GE	0x0 /* FCoE */
 #define LPFC_LNK_FC	0x1 /* FC   */
 	uint8_t lnk_no;
+	uint8_t optic_state;
 };
 
 #define LPFC_SLI4_HANDLER_CNT		(LPFC_FCP_IO_CHAN_MAX+ \
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index ea53aa664759..4dc22562aaf1 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "11.0.0.0."
+#define LPFC_DRIVER_VERSION "11.0.0.10."
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/lpfc/lpfc_vport.c b/drivers/scsi/lpfc/lpfc_vport.c
index 769012663a8f..b3f85def18cc 100644
--- a/drivers/scsi/lpfc/lpfc_vport.c
+++ b/drivers/scsi/lpfc/lpfc_vport.c
@@ -393,6 +393,14 @@ lpfc_vport_create(struct fc_vport *fc_vport, bool disable)
 	*(struct lpfc_vport **)fc_vport->dd_data = vport;
 	vport->fc_vport = fc_vport;
 
+	/* At this point we are fully registered with SCSI Layer.  */
+	vport->load_flag |= FC_ALLOW_FDMI;
+	if (phba->cfg_fdmi_on > LPFC_FDMI_NO_SUPPORT) {
+		/* Setup appropriate attribute masks */
+		vport->fdmi_hba_mask = phba->pport->fdmi_hba_mask;
+		vport->fdmi_port_mask = phba->pport->fdmi_port_mask;
+	}
+
 	/*
 	 * In SLI4, the vpi must be activated before it can be used
 	 * by the port.
diff --git a/drivers/scsi/mac_scsi.c b/drivers/scsi/mac_scsi.c
index d64a769b8155..bb2381314a2b 100644
--- a/drivers/scsi/mac_scsi.c
+++ b/drivers/scsi/mac_scsi.c
@@ -12,7 +12,6 @@
  */
 
 #include <linux/types.h>
-#include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/ioport.h>
 #include <linux/init.h>
@@ -32,14 +31,13 @@
 #define PSEUDO_DMA
 
 #define NCR5380_implementation_fields   unsigned char *pdma_base
-#define NCR5380_local_declare()         struct Scsi_Host *_instance
-#define NCR5380_setup(instance)         _instance = instance
 
-#define NCR5380_read(reg)               macscsi_read(_instance, reg)
-#define NCR5380_write(reg, value)       macscsi_write(_instance, reg, value)
+#define NCR5380_read(reg)               macscsi_read(instance, reg)
+#define NCR5380_write(reg, value)       macscsi_write(instance, reg, value)
 
 #define NCR5380_pread                   macscsi_pread
 #define NCR5380_pwrite                  macscsi_pwrite
+#define NCR5380_dma_xfer_len(instance, cmd, phase)	(cmd->transfersize)
 
 #define NCR5380_intr                    macscsi_intr
 #define NCR5380_queue_command           macscsi_queue_command
@@ -51,8 +49,6 @@
 
 #include "NCR5380.h"
 
-#define RESET_BOOT
-
 static int setup_can_queue = -1;
 module_param(setup_can_queue, int, 0);
 static int setup_cmd_per_lun = -1;
@@ -65,17 +61,8 @@ static int setup_use_tagged_queuing = -1;
 module_param(setup_use_tagged_queuing, int, 0);
 static int setup_hostid = -1;
 module_param(setup_hostid, int, 0);
-
-/* Time (in jiffies) to wait after a reset; the SCSI standard calls for 250ms,
- * we usually do 0.5s to be on the safe side. But Toshiba CD-ROMs once more
- * need ten times the standard value... */
-#define TOSHIBA_DELAY
-
-#ifdef TOSHIBA_DELAY
-#define	AFTER_RESET_DELAY	(5*HZ/2)
-#else
-#define	AFTER_RESET_DELAY	(HZ/2)
-#endif
+static int setup_toshiba_delay = -1;
+module_param(setup_toshiba_delay, int, 0);
 
 /*
  * NCR 5380 register access functions
@@ -94,12 +81,12 @@ static inline void macscsi_write(struct Scsi_Host *instance, int reg, int value)
 #ifndef MODULE
 static int __init mac_scsi_setup(char *str)
 {
-	int ints[7];
+	int ints[8];
 
 	(void)get_options(str, ARRAY_SIZE(ints), ints);
 
-	if (ints[0] < 1 || ints[0] > 6) {
-		pr_err("Usage: mac5380=<can_queue>[,<cmd_per_lun>[,<sg_tablesize>[,<hostid>[,<use_tags>[,<use_pdma>]]]]]\n");
+	if (ints[0] < 1) {
+		pr_err("Usage: mac5380=<can_queue>[,<cmd_per_lun>[,<sg_tablesize>[,<hostid>[,<use_tags>[,<use_pdma>[,<toshiba_delay>]]]]]]\n");
 		return 0;
 	}
 	if (ints[0] >= 1)
@@ -114,50 +101,14 @@ static int __init mac_scsi_setup(char *str)
 		setup_use_tagged_queuing = ints[5];
 	if (ints[0] >= 6)
 		setup_use_pdma = ints[6];
+	if (ints[0] >= 7)
+		setup_toshiba_delay = ints[7];
 	return 1;
 }
 
 __setup("mac5380=", mac_scsi_setup);
 #endif /* !MODULE */
 
-#ifdef RESET_BOOT
-/*
- * Our 'bus reset on boot' function
- */
-
-static void mac_scsi_reset_boot(struct Scsi_Host *instance)
-{
-	unsigned long end;
-
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-	
-	/*
-	 * Do a SCSI reset to clean up the bus during initialization. No messing
-	 * with the queues, interrupts, or locks necessary here.
-	 */
-
-	printk(KERN_INFO "Macintosh SCSI: resetting the SCSI bus..." );
-
-	/* get in phase */
-	NCR5380_write( TARGET_COMMAND_REG,
-		      PHASE_SR_TO_TCR( NCR5380_read(STATUS_REG) ));
-
-	/* assert RST */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST );
-	/* The min. reset hold time is 25us, so 40us should be enough */
-	udelay( 50 );
-	/* reset RST and interrupt */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE );
-	NCR5380_read( RESET_PARITY_INTERRUPT_REG );
-
-	for( end = jiffies + AFTER_RESET_DELAY; time_before(jiffies, end); )
-		barrier();
-
-	printk(KERN_INFO " done\n" );
-}
-#endif
-
 #ifdef PSEUDO_DMA
 /* 
    Pseudo-DMA: (Ove Edlund)
@@ -235,9 +186,6 @@ static int macscsi_pread(struct Scsi_Host *instance,
 	unsigned char *d;
 	unsigned char *s;
 
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-
 	s = hostdata->pdma_base + (INPUT_DATA_REG << 4);
 	d = dst;
 
@@ -329,9 +277,6 @@ static int macscsi_pwrite(struct Scsi_Host *instance,
 	unsigned char *s;
 	unsigned char *d;
 
-	NCR5380_local_declare();
-	NCR5380_setup(instance);
-
 	s = src;
 	d = hostdata->pdma_base + (OUTPUT_DATA_REG << 4);
 
@@ -364,20 +309,22 @@ static int macscsi_pwrite(struct Scsi_Host *instance,
 #define PFX                     DRV_MODULE_NAME ": "
 
 static struct scsi_host_template mac_scsi_template = {
-	.module				= THIS_MODULE,
-	.proc_name			= DRV_MODULE_NAME,
-	.show_info			= macscsi_show_info,
-	.write_info			= macscsi_write_info,
-	.name				= "Macintosh NCR5380 SCSI",
-	.info				= macscsi_info,
-	.queuecommand			= macscsi_queue_command,
-	.eh_abort_handler		= macscsi_abort,
-	.eh_bus_reset_handler		= macscsi_bus_reset,
-	.can_queue			= 16,
-	.this_id			= 7,
-	.sg_tablesize			= SG_ALL,
-	.cmd_per_lun			= 2,
-	.use_clustering			= DISABLE_CLUSTERING
+	.module			= THIS_MODULE,
+	.proc_name		= DRV_MODULE_NAME,
+	.show_info		= macscsi_show_info,
+	.write_info		= macscsi_write_info,
+	.name			= "Macintosh NCR5380 SCSI",
+	.info			= macscsi_info,
+	.queuecommand		= macscsi_queue_command,
+	.eh_abort_handler	= macscsi_abort,
+	.eh_bus_reset_handler	= macscsi_bus_reset,
+	.can_queue		= 16,
+	.this_id		= 7,
+	.sg_tablesize		= SG_ALL,
+	.cmd_per_lun		= 2,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 
 static int __init mac_scsi_probe(struct platform_device *pdev)
@@ -432,15 +379,14 @@ static int __init mac_scsi_probe(struct platform_device *pdev)
 	} else
 		host_flags |= FLAG_NO_PSEUDO_DMA;
 
-#ifdef RESET_BOOT
-	mac_scsi_reset_boot(instance);
-#endif
-
 #ifdef SUPPORT_TAGS
 	host_flags |= setup_use_tagged_queuing > 0 ? FLAG_TAGGED_QUEUING : 0;
 #endif
+	host_flags |= setup_toshiba_delay > 0 ? FLAG_TOSHIBA_DELAY : 0;
 
-	NCR5380_init(instance, host_flags);
+	error = NCR5380_init(instance, host_flags);
+	if (error)
+		goto fail_init;
 
 	if (instance->irq != NO_IRQ) {
 		error = request_irq(instance->irq, macscsi_intr, IRQF_SHARED,
@@ -449,6 +395,8 @@ static int __init mac_scsi_probe(struct platform_device *pdev)
 			goto fail_irq;
 	}
 
+	NCR5380_maybe_reset_bus(instance);
+
 	error = scsi_add_host(instance, NULL);
 	if (error)
 		goto fail_host;
@@ -463,6 +411,7 @@ fail_host:
 		free_irq(instance->irq, instance);
 fail_irq:
 	NCR5380_exit(instance);
+fail_init:
 	scsi_host_put(instance);
 	return error;
 }
diff --git a/drivers/scsi/megaraid/megaraid_mm.c b/drivers/scsi/megaraid/megaraid_mm.c
index a70692779a16..4cf9ed96414f 100644
--- a/drivers/scsi/megaraid/megaraid_mm.c
+++ b/drivers/scsi/megaraid/megaraid_mm.c
@@ -179,8 +179,12 @@ mraid_mm_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 	/*
 	 * The following call will block till a kioc is available
+	 * or return NULL if the list head is empty for the pointer
+	 * of type mraid_mmapt passed to mraid_mm_alloc_kioc
 	 */
 	kioc = mraid_mm_alloc_kioc(adp);
+	if (!kioc)
+		return -ENXIO;
 
 	/*
 	 * User sent the old mimd_t ioctl packet. Convert it to uioc_t.
diff --git a/drivers/scsi/mpt3sas/mpt3sas_base.c b/drivers/scsi/mpt3sas/mpt3sas_base.c
index 11393ebf1a68..83658acddd58 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_base.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_base.c
@@ -2020,8 +2020,10 @@ mpt3sas_base_unmap_resources(struct MPT3SAS_ADAPTER *ioc)
 	_base_free_irq(ioc);
 	_base_disable_msix(ioc);
 
-	if (ioc->msix96_vector)
+	if (ioc->msix96_vector) {
 		kfree(ioc->replyPostRegisterIndex);
+		ioc->replyPostRegisterIndex = NULL;
+	}
 
 	if (ioc->chip_phys) {
 		iounmap(ioc->chip);
diff --git a/drivers/scsi/mvsas/mv_94xx.c b/drivers/scsi/mvsas/mv_94xx.c
index 9270d15ff1a4..f6fc4a705924 100644
--- a/drivers/scsi/mvsas/mv_94xx.c
+++ b/drivers/scsi/mvsas/mv_94xx.c
@@ -330,6 +330,51 @@ static void mvs_94xx_phy_enable(struct mvs_info *mvi, u32 phy_id)
 	mvs_write_port_vsr_data(mvi, phy_id, tmp & 0xfd7fffff);
 }
 
+static void mvs_94xx_sgpio_init(struct mvs_info *mvi)
+{
+	void __iomem *regs = mvi->regs_ex - 0x10200;
+	u32 tmp;
+
+	tmp = mr32(MVS_HST_CHIP_CONFIG);
+	tmp |= 0x100;
+	mw32(MVS_HST_CHIP_CONFIG, tmp);
+
+	mw32(MVS_SGPIO_CTRL + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		MVS_SGPIO_CTRL_SDOUT_AUTO << MVS_SGPIO_CTRL_SDOUT_SHIFT);
+
+	mw32(MVS_SGPIO_CFG1 + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		8 << MVS_SGPIO_CFG1_LOWA_SHIFT |
+		8 << MVS_SGPIO_CFG1_HIA_SHIFT |
+		4 << MVS_SGPIO_CFG1_LOWB_SHIFT |
+		4 << MVS_SGPIO_CFG1_HIB_SHIFT |
+		2 << MVS_SGPIO_CFG1_MAXACTON_SHIFT |
+		1 << MVS_SGPIO_CFG1_FORCEACTOFF_SHIFT
+	);
+
+	mw32(MVS_SGPIO_CFG2 + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		(300000 / 100) << MVS_SGPIO_CFG2_CLK_SHIFT | /* 100kHz clock */
+		66 << MVS_SGPIO_CFG2_BLINK_SHIFT /* (66 * 0,121 Hz?)*/
+	);
+
+	mw32(MVS_SGPIO_CFG0 + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		MVS_SGPIO_CFG0_ENABLE |
+		MVS_SGPIO_CFG0_BLINKA |
+		MVS_SGPIO_CFG0_BLINKB |
+		/* 3*4 data bits / PDU */
+		(12 - 1) << MVS_SGPIO_CFG0_AUT_BITLEN_SHIFT
+	);
+
+	mw32(MVS_SGPIO_DCTRL + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		DEFAULT_SGPIO_BITS);
+
+	mw32(MVS_SGPIO_DSRC + MVS_SGPIO_HOST_OFFSET * mvi->id,
+		((mvi->id * 4) + 3) << (8 * 3) |
+		((mvi->id * 4) + 2) << (8 * 2) |
+		((mvi->id * 4) + 1) << (8 * 1) |
+		((mvi->id * 4) + 0) << (8 * 0));
+
+}
+
 static int mvs_94xx_init(struct mvs_info *mvi)
 {
 	void __iomem *regs = mvi->regs;
@@ -533,6 +578,8 @@ static int mvs_94xx_init(struct mvs_info *mvi)
 	/* Enable SRS interrupt */
 	mw32(MVS_INT_MASK_SRS_0, 0xFFFF);
 
+	mvs_94xx_sgpio_init(mvi);
+
 	return 0;
 }
 
@@ -1005,6 +1052,92 @@ static void mvs_94xx_tune_interrupt(struct mvs_info *mvi, u32 time)
 
 }
 
+static int mvs_94xx_gpio_write(struct mvs_prv_info *mvs_prv,
+			u8 reg_type, u8 reg_index,
+			u8 reg_count, u8 *write_data)
+{
+	int i;
+
+	switch (reg_type) {
+
+	case SAS_GPIO_REG_TX_GP:
+		if (reg_index == 0)
+			return -EINVAL;
+
+		if (reg_count > 1)
+			return -EINVAL;
+
+		if (reg_count == 0)
+			return 0;
+
+		/* maximum supported bits = hosts * 4 drives * 3 bits */
+		for (i = 0; i < mvs_prv->n_host * 4 * 3; i++) {
+
+			/* select host */
+			struct mvs_info *mvi = mvs_prv->mvi[i/(4*3)];
+
+			void __iomem *regs = mvi->regs_ex - 0x10200;
+
+			int drive = (i/3) & (4-1); /* drive number on host */
+			u32 block = mr32(MVS_SGPIO_DCTRL +
+				MVS_SGPIO_HOST_OFFSET * mvi->id);
+
+
+			/*
+			* if bit is set then create a mask with the first
+			* bit of the drive set in the mask ...
+			*/
+			u32 bit = (write_data[i/8] & (1 << (i&(8-1)))) ?
+				1<<(24-drive*8) : 0;
+
+			/*
+			* ... and then shift it to the right position based
+			* on the led type (activity/id/fail)
+			*/
+			switch (i%3) {
+			case 0: /* activity */
+				block &= ~((0x7 << MVS_SGPIO_DCTRL_ACT_SHIFT)
+					<< (24-drive*8));
+					/* hardwire activity bit to SOF */
+				block |= LED_BLINKA_SOF << (
+					MVS_SGPIO_DCTRL_ACT_SHIFT +
+					(24-drive*8));
+				break;
+			case 1: /* id */
+				block &= ~((0x3 << MVS_SGPIO_DCTRL_LOC_SHIFT)
+					<< (24-drive*8));
+				block |= bit << MVS_SGPIO_DCTRL_LOC_SHIFT;
+				break;
+			case 2: /* fail */
+				block &= ~((0x7 << MVS_SGPIO_DCTRL_ERR_SHIFT)
+					<< (24-drive*8));
+				block |= bit << MVS_SGPIO_DCTRL_ERR_SHIFT;
+				break;
+			}
+
+			mw32(MVS_SGPIO_DCTRL + MVS_SGPIO_HOST_OFFSET * mvi->id,
+				block);
+
+		}
+
+		return reg_count;
+
+	case SAS_GPIO_REG_TX:
+		if (reg_index + reg_count > mvs_prv->n_host)
+			return -EINVAL;
+
+		for (i = 0; i < reg_count; i++) {
+			struct mvs_info *mvi = mvs_prv->mvi[i+reg_index];
+			void __iomem *regs = mvi->regs_ex - 0x10200;
+
+			mw32(MVS_SGPIO_DCTRL + MVS_SGPIO_HOST_OFFSET * mvi->id,
+				be32_to_cpu(((u32 *) write_data)[i]));
+		}
+		return reg_count;
+	}
+	return -ENOSYS;
+}
+
 const struct mvs_dispatch mvs_94xx_dispatch = {
 	"mv94xx",
 	mvs_94xx_init,
@@ -1057,5 +1190,6 @@ const struct mvs_dispatch mvs_94xx_dispatch = {
 	mvs_94xx_fix_dma,
 	mvs_94xx_tune_interrupt,
 	mvs_94xx_non_spec_ncq_error,
+	mvs_94xx_gpio_write,
 };
 
diff --git a/drivers/scsi/mvsas/mv_94xx.h b/drivers/scsi/mvsas/mv_94xx.h
index 14e197497b46..578960803a00 100644
--- a/drivers/scsi/mvsas/mv_94xx.h
+++ b/drivers/scsi/mvsas/mv_94xx.h
@@ -38,6 +38,10 @@ enum VANIR_REVISION_ID {
 	VANIR_C2_REV		= 0xC2,
 };
 
+enum host_registers {
+	MVS_HST_CHIP_CONFIG	= 0x10104,	/* chip configuration */
+};
+
 enum hw_registers {
 	MVS_GBL_CTL		= 0x04,  /* global control */
 	MVS_GBL_INT_STAT	= 0x00,  /* global irq status */
@@ -239,6 +243,73 @@ struct mvs_prd {
 	__le32			im_len;
 } __attribute__ ((packed));
 
+enum sgpio_registers {
+	MVS_SGPIO_HOST_OFFSET	= 0x100,	/* offset between hosts */
+
+	MVS_SGPIO_CFG0	= 0xc200,
+	MVS_SGPIO_CFG0_ENABLE	= (1 << 0),	/* enable pins */
+	MVS_SGPIO_CFG0_BLINKB	= (1 << 1),	/* blink generators */
+	MVS_SGPIO_CFG0_BLINKA	= (1 << 2),
+	MVS_SGPIO_CFG0_INVSCLK	= (1 << 3),	/* invert signal? */
+	MVS_SGPIO_CFG0_INVSLOAD	= (1 << 4),
+	MVS_SGPIO_CFG0_INVSDOUT	= (1 << 5),
+	MVS_SGPIO_CFG0_SLOAD_FALLEDGE = (1 << 6),	/* rise/fall edge? */
+	MVS_SGPIO_CFG0_SDOUT_FALLEDGE = (1 << 7),
+	MVS_SGPIO_CFG0_SDIN_RISEEDGE = (1 << 8),
+	MVS_SGPIO_CFG0_MAN_BITLEN_SHIFT = 18,	/* bits/frame manual mode */
+	MVS_SGPIO_CFG0_AUT_BITLEN_SHIFT = 24,	/* bits/frame auto mode */
+
+	MVS_SGPIO_CFG1	= 0xc204,	/* blink timing register */
+	MVS_SGPIO_CFG1_LOWA_SHIFT	= 0,	/* A off time */
+	MVS_SGPIO_CFG1_HIA_SHIFT	= 4,	/* A on time */
+	MVS_SGPIO_CFG1_LOWB_SHIFT	= 8,	/* B off time */
+	MVS_SGPIO_CFG1_HIB_SHIFT	= 12,	/* B on time */
+	MVS_SGPIO_CFG1_MAXACTON_SHIFT	= 16,	/* max activity on time */
+
+		/* force activity off time */
+	MVS_SGPIO_CFG1_FORCEACTOFF_SHIFT	= 20,
+		/* stretch activity on time */
+	MVS_SGPIO_CFG1_STRCHACTON_SHIFT	= 24,
+		/* stretch activiity off time */
+	MVS_SGPIO_CFG1_STRCHACTOFF_SHIFT	= 28,
+
+
+	MVS_SGPIO_CFG2	= 0xc208,	/* clock speed register */
+	MVS_SGPIO_CFG2_CLK_SHIFT	= 0,
+	MVS_SGPIO_CFG2_BLINK_SHIFT	= 20,
+
+	MVS_SGPIO_CTRL	= 0xc20c,	/* SDOUT/SDIN mode control */
+	MVS_SGPIO_CTRL_SDOUT_AUTO	= 2,
+	MVS_SGPIO_CTRL_SDOUT_SHIFT	= 2,
+
+	MVS_SGPIO_DSRC	= 0xc220,	/* map ODn bits to drives */
+
+	MVS_SGPIO_DCTRL	= 0xc238,
+	MVS_SGPIO_DCTRL_ERR_SHIFT	= 0,
+	MVS_SGPIO_DCTRL_LOC_SHIFT	= 3,
+	MVS_SGPIO_DCTRL_ACT_SHIFT	= 5,
+};
+
+enum sgpio_led_status {
+	LED_OFF	= 0,
+	LED_ON	= 1,
+	LED_BLINKA	= 2,
+	LED_BLINKA_INV	= 3,
+	LED_BLINKA_SOF	= 4,
+	LED_BLINKA_EOF	= 5,
+	LED_BLINKB	= 6,
+	LED_BLINKB_INV	= 7,
+};
+
+#define DEFAULT_SGPIO_BITS ((LED_BLINKA_SOF << \
+				MVS_SGPIO_DCTRL_ACT_SHIFT) << (8 * 3) | \
+			(LED_BLINKA_SOF << \
+				MVS_SGPIO_DCTRL_ACT_SHIFT) << (8 * 2) | \
+			(LED_BLINKA_SOF << \
+				MVS_SGPIO_DCTRL_ACT_SHIFT) << (8 * 1) | \
+			(LED_BLINKA_SOF << \
+				MVS_SGPIO_DCTRL_ACT_SHIFT) << (8 * 0))
+
 /*
  * these registers are accessed through port vendor
  * specific address/data registers
diff --git a/drivers/scsi/mvsas/mv_init.c b/drivers/scsi/mvsas/mv_init.c
index 675e7fab0796..c7c250519c4b 100644
--- a/drivers/scsi/mvsas/mv_init.c
+++ b/drivers/scsi/mvsas/mv_init.c
@@ -84,6 +84,8 @@ static struct sas_domain_function_template mvs_transport_ops = {
 	.lldd_port_formed	= mvs_port_formed,
 	.lldd_port_deformed     = mvs_port_deformed,
 
+	.lldd_write_gpio	= mvs_gpio_write,
+
 };
 
 static void mvs_phy_init(struct mvs_info *mvi, int phy_id)
diff --git a/drivers/scsi/mvsas/mv_sas.c b/drivers/scsi/mvsas/mv_sas.c
index 9c780740fb82..83cd3ea2df41 100644
--- a/drivers/scsi/mvsas/mv_sas.c
+++ b/drivers/scsi/mvsas/mv_sas.c
@@ -737,8 +737,8 @@ static int mvs_task_prep(struct sas_task *task, struct mvs_info *mvi, int is_tmf
 			mv_dprintk("device %016llx not ready.\n",
 				SAS_ADDR(dev->sas_addr));
 
-			rc = SAS_PHY_DOWN;
-			return rc;
+		rc = SAS_PHY_DOWN;
+		return rc;
 	}
 	tei.port = dev->port->lldd_port;
 	if (tei.port && !tei.port->port_attached && !tmf) {
@@ -2105,3 +2105,16 @@ int mvs_int_rx(struct mvs_info *mvi, bool self_clear)
 	return 0;
 }
 
+int mvs_gpio_write(struct sas_ha_struct *sha, u8 reg_type, u8 reg_index,
+			u8 reg_count, u8 *write_data)
+{
+	struct mvs_prv_info *mvs_prv = sha->lldd_ha;
+	struct mvs_info *mvi = mvs_prv->mvi[0];
+
+	if (MVS_CHIP_DISP->gpio_write) {
+		return MVS_CHIP_DISP->gpio_write(mvs_prv, reg_type,
+			reg_index, reg_count, write_data);
+	}
+
+	return -ENOSYS;
+}
diff --git a/drivers/scsi/mvsas/mv_sas.h b/drivers/scsi/mvsas/mv_sas.h
index dc409c04747a..f9afd4cdd4c4 100644
--- a/drivers/scsi/mvsas/mv_sas.h
+++ b/drivers/scsi/mvsas/mv_sas.h
@@ -103,6 +103,7 @@ enum dev_reset {
 };
 
 struct mvs_info;
+struct mvs_prv_info;
 
 struct mvs_dispatch {
 	char *name;
@@ -172,6 +173,8 @@ struct mvs_dispatch {
 				int buf_len, int from, void *prd);
 	void (*tune_interrupt)(struct mvs_info *mvi, u32 time);
 	void (*non_spec_ncq_error)(struct mvs_info *mvi);
+	int (*gpio_write)(struct mvs_prv_info *mvs_prv, u8 reg_type,
+			u8 reg_index, u8 reg_count, u8 *write_data);
 
 };
 
@@ -476,5 +479,7 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events);
 void mvs_update_phyinfo(struct mvs_info *mvi, int i, int get_st);
 int mvs_int_rx(struct mvs_info *mvi, bool self_clear);
 struct mvs_device *mvs_find_dev_by_reg_set(struct mvs_info *mvi, u8 reg_set);
+int mvs_gpio_write(struct sas_ha_struct *, u8 reg_type, u8 reg_index,
+			u8 reg_count, u8 *write_data);
 #endif
 
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index 0cccd6033feb..d8a2b5185f56 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -170,10 +170,7 @@ static int _osd_get_print_system_info(struct osd_dev *od,
 
 	/* FIXME: Where are the time utilities */
 	pFirst = get_attrs[a++].val_ptr;
-	OSD_INFO("CLOCK                  [0x%02x%02x%02x%02x%02x%02x]\n",
-		((char *)pFirst)[0], ((char *)pFirst)[1],
-		((char *)pFirst)[2], ((char *)pFirst)[3],
-		((char *)pFirst)[4], ((char *)pFirst)[5]);
+	OSD_INFO("CLOCK                  [0x%6phN]\n", pFirst);
 
 	if (a < nelem) { /* IBM-OSD-SIM bug, Might not have it */
 		unsigned len = get_attrs[a].len;
diff --git a/drivers/scsi/pas16.c b/drivers/scsi/pas16.c
index e81eadd08afc..512037e27783 100644
--- a/drivers/scsi/pas16.c
+++ b/drivers/scsi/pas16.c
@@ -1,6 +1,4 @@
 #define PSEUDO_DMA
-#define UNSAFE  /* Not unsafe for PAS16 -- use it */
-#define PDEBUG 0
 
 /*
  * This driver adapted from Drew Eckhardt's Trantor T128 driver
@@ -71,14 +69,10 @@
  
 #include <linux/module.h>
 
-#include <linux/signal.h>
-#include <linux/proc_fs.h>
 #include <asm/io.h>
 #include <asm/dma.h>
 #include <linux/blkdev.h>
-#include <linux/delay.h>
 #include <linux/interrupt.h>
-#include <linux/stat.h>
 #include <linux/init.h>
 
 #include <scsi/scsi_host.h>
@@ -87,8 +81,8 @@
 #include "NCR5380.h"
 
 
-static unsigned short pas16_addr = 0;
-static int pas16_irq = 0;
+static unsigned short pas16_addr;
+static int pas16_irq;
  
 
 static const int scsi_irq_translate[] =
@@ -146,22 +140,6 @@ static const unsigned short  pas16_offset[ 8 ] =
 		    * START_DMA_INITIATOR_RECEIVE_REG wo
 		    */
     };
-/*----------------------------------------------------------------*/
-/* the following will set the monitor border color (useful to find
- where something crashed or gets stuck at */
-/* 1 = blue
- 2 = green
- 3 = cyan
- 4 = red
- 5 = magenta
- 6 = yellow
- 7 = white
-*/
-#if 1
-#define rtrc(i) {inb(0x3da); outb(0x31, 0x3c0); outb((i), 0x3c0);}
-#else
-#define rtrc(i) {}
-#endif
 
 
 /*
@@ -205,7 +183,7 @@ static void __init
 	outb( 0x01, io_port + P_TIMEOUT_STATUS_REG_OFFSET );   /* Reset TC */
 	outb( 0x01, io_port + WAIT_STATE );   /* 1 Wait state */
 
-	NCR5380_read( RESET_PARITY_INTERRUPT_REG );
+	inb(io_port + pas16_offset[RESET_PARITY_INTERRUPT_REG]);
 
 	/* Set the SCSI interrupt pointer without mucking up the sound
 	 * interrupt pointer in the same byte.
@@ -280,13 +258,13 @@ static int __init
      * put in an additional test to try to weed them out.
      */
 
-    outb( 0x01, io_port + WAIT_STATE ); 	/* 1 Wait state */
-    NCR5380_write( MODE_REG, 0x20 );		/* Is it really SCSI? */
-    if( NCR5380_read( MODE_REG ) != 0x20 )	/* Write to a reg.    */
-	return 0;				/* and try to read    */
-    NCR5380_write( MODE_REG, 0x00 );		/* it back.	      */
-    if( NCR5380_read( MODE_REG ) != 0x00 )
-	return 0;
+	outb(0x01, io_port + WAIT_STATE);             /* 1 Wait state */
+	outb(0x20, io_port + pas16_offset[MODE_REG]); /* Is it really SCSI? */
+	if (inb(io_port + pas16_offset[MODE_REG]) != 0x20) /* Write to a reg. */
+		return 0;                                  /* and try to read */
+	outb(0x00, io_port + pas16_offset[MODE_REG]);      /* it back. */
+	if (inb(io_port + pas16_offset[MODE_REG]) != 0x00)
+		return 0;
 
     return 1;
 }
@@ -305,7 +283,7 @@ static int __init
 
 static int __init pas16_setup(char *str)
 {
-    static int commandline_current = 0;
+	static int commandline_current;
     int i;
     int ints[10];
 
@@ -344,8 +322,8 @@ __setup("pas16=", pas16_setup);
 
 static int __init pas16_detect(struct scsi_host_template *tpnt)
 {
-    static int current_override = 0;
-    static unsigned short current_base = 0;
+	static int current_override;
+	static unsigned short current_base;
     struct Scsi_Host *instance;
     unsigned short io_port;
     int  count;
@@ -377,34 +355,32 @@ static int __init pas16_detect(struct scsi_host_template *tpnt)
 	}
 	else
 	    for (; !io_port && (current_base < NO_BASES); ++current_base) {
-#if (PDEBUG & PDEBUG_INIT)
-    printk("scsi-pas16 : probing io_port %04x\n", (unsigned int) bases[current_base].io_port);
-#endif
+		dprintk(NDEBUG_INIT, "pas16: probing io_port 0x%04x\n",
+		        (unsigned int)bases[current_base].io_port);
 		if ( !bases[current_base].noauto &&
 		     pas16_hw_detect( current_base ) ){
 			io_port = bases[current_base].io_port;
 			init_board( io_port, default_irqs[ current_base ], 0 ); 
-#if (PDEBUG & PDEBUG_INIT)
-			printk("scsi-pas16 : detected board.\n");
-#endif
+			dprintk(NDEBUG_INIT, "pas16: detected board\n");
 		}
     }
 
-
-#if defined(PDEBUG) && (PDEBUG & PDEBUG_INIT)
-	printk("scsi-pas16 : io_port = %04x\n", (unsigned int) io_port);
-#endif
+	dprintk(NDEBUG_INIT, "pas16: io_port = 0x%04x\n",
+	        (unsigned int)io_port);
 
 	if (!io_port)
 	    break;
 
 	instance = scsi_register (tpnt, sizeof(struct NCR5380_hostdata));
 	if(instance == NULL)
-		break;
+		goto out;
 		
 	instance->io_port = io_port;
 
-	NCR5380_init(instance, 0);
+	if (NCR5380_init(instance, 0))
+		goto out_unregister;
+
+	NCR5380_maybe_reset_bus(instance);
 
 	if (overrides[current_override].irq != IRQ_AUTO)
 	    instance->irq = overrides[current_override].irq;
@@ -431,14 +407,18 @@ static int __init pas16_detect(struct scsi_host_template *tpnt)
 	    outb( (inb(io_port + IO_CONFIG_3) & 0x0f), io_port + IO_CONFIG_3 );
 	}
 
-#if defined(PDEBUG) && (PDEBUG & PDEBUG_INIT)
-	printk("scsi%d : irq = %d\n", instance->host_no, instance->irq);
-#endif
+	dprintk(NDEBUG_INIT, "scsi%d : irq = %d\n",
+	        instance->host_no, instance->irq);
 
 	++current_override;
 	++count;
     }
     return count;
+
+out_unregister:
+	scsi_unregister(instance);
+out:
+	return count;
 }
 
 /*
@@ -561,29 +541,29 @@ static int pas16_release(struct Scsi_Host *shost)
 	if (shost->irq != NO_IRQ)
 		free_irq(shost->irq, shost);
 	NCR5380_exit(shost);
-	if (shost->io_port && shost->n_io_port)
-		release_region(shost->io_port, shost->n_io_port);
 	scsi_unregister(shost);
 	return 0;
 }
 
 static struct scsi_host_template driver_template = {
-	.name           = "Pro Audio Spectrum-16 SCSI",
-	.detect         = pas16_detect,
-	.release        = pas16_release,
-	.proc_name      = "pas16",
-	.show_info      = pas16_show_info,
-	.write_info     = pas16_write_info,
-	.info           = pas16_info,
-	.queuecommand   = pas16_queue_command,
-	.eh_abort_handler = pas16_abort,
-	.eh_bus_reset_handler = pas16_bus_reset,
-	.bios_param     = pas16_biosparam, 
-	.can_queue      = CAN_QUEUE,
-	.this_id        = 7,
-	.sg_tablesize   = SG_ALL,
-	.cmd_per_lun    = CMD_PER_LUN,
-	.use_clustering = DISABLE_CLUSTERING,
+	.name			= "Pro Audio Spectrum-16 SCSI",
+	.detect			= pas16_detect,
+	.release		= pas16_release,
+	.proc_name		= "pas16",
+	.show_info		= pas16_show_info,
+	.write_info		= pas16_write_info,
+	.info			= pas16_info,
+	.queuecommand		= pas16_queue_command,
+	.eh_abort_handler	= pas16_abort,
+	.eh_bus_reset_handler	= pas16_bus_reset,
+	.bios_param		= pas16_biosparam,
+	.can_queue		= 32,
+	.this_id		= 7,
+	.sg_tablesize		= SG_ALL,
+	.cmd_per_lun		= 2,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 #include "scsi_module.c"
 
diff --git a/drivers/scsi/pas16.h b/drivers/scsi/pas16.h
index c6109c80050b..d37527717225 100644
--- a/drivers/scsi/pas16.h
+++ b/drivers/scsi/pas16.h
@@ -24,9 +24,6 @@
 #ifndef PAS16_H
 #define PAS16_H
 
-#define PDEBUG_INIT	0x1
-#define PDEBUG_TRANSFER 0x2
-
 #define PAS16_DEFAULT_BASE_1  0x388
 #define PAS16_DEFAULT_BASE_2  0x384
 #define PAS16_DEFAULT_BASE_3  0x38c
@@ -98,46 +95,16 @@
 #define OPERATION_MODE_1 0xec03
 #define IO_CONFIG_3 0xf002
 
+#define NCR5380_implementation_fields /* none */
 
-#ifndef ASM
-
-#ifndef CMD_PER_LUN
-#define CMD_PER_LUN 2
-#endif
-
-#ifndef CAN_QUEUE
-#define CAN_QUEUE 32 
-#endif
-
-#define NCR5380_implementation_fields \
-    volatile unsigned short io_port
-
-#define NCR5380_local_declare() \
-    volatile unsigned short io_port
+#define PAS16_io_port(reg) (instance->io_port + pas16_offset[(reg)])
 
-#define NCR5380_setup(instance) \
-    io_port = (instance)->io_port
-
-#define PAS16_io_port(reg) ( io_port + pas16_offset[(reg)] )
-
-#if !(PDEBUG & PDEBUG_TRANSFER) 
 #define NCR5380_read(reg) ( inb(PAS16_io_port(reg)) )
 #define NCR5380_write(reg, value) ( outb((value),PAS16_io_port(reg)) )
-#else
-#define NCR5380_read(reg)						\
-    (((unsigned char) printk("scsi%d : read register %d at io_port %04x\n"\
-    , instance->hostno, (reg), PAS16_io_port(reg))), inb( PAS16_io_port(reg)) )
-
-#define NCR5380_write(reg, value) 					\
-    (printk("scsi%d : write %02x to register %d at io_port %04x\n", 	\
-	    instance->hostno, (value), (reg), PAS16_io_port(reg)),	\
-    outb( (value),PAS16_io_port(reg) ) )
-
-#endif
 
+#define NCR5380_dma_xfer_len(instance, cmd, phase)	(cmd->transfersize)
 
 #define NCR5380_intr pas16_intr
-#define do_NCR5380_intr do_pas16_intr
 #define NCR5380_queue_command pas16_queue_command
 #define NCR5380_abort pas16_abort
 #define NCR5380_bus_reset pas16_bus_reset
@@ -150,5 +117,4 @@
    
 #define PAS16_IRQS 0xd4a8 
 
-#endif /* ndef ASM */
 #endif /* PAS16_H */
diff --git a/drivers/scsi/qla2xxx/Kconfig b/drivers/scsi/qla2xxx/Kconfig
index a0f732b138e4..10aa18ba05fd 100644
--- a/drivers/scsi/qla2xxx/Kconfig
+++ b/drivers/scsi/qla2xxx/Kconfig
@@ -18,9 +18,6 @@ config SCSI_QLA_FC
 	2322, 6322        ql2322_fw.bin
 	24xx, 54xx        ql2400_fw.bin
 	25xx              ql2500_fw.bin
-	2031              ql2600_fw.bin
-	8031              ql8300_fw.bin
-	27xx              ql2700_fw.bin
 
 	Upon request, the driver caches the firmware image until
 	the driver is unloaded.
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 6b942d9e5b74..6992ebc50c87 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -824,6 +824,41 @@ static struct bin_attribute sysfs_reset_attr = {
 };
 
 static ssize_t
+qla2x00_issue_logo(struct file *filp, struct kobject *kobj,
+			struct bin_attribute *bin_attr,
+			char *buf, loff_t off, size_t count)
+{
+	struct scsi_qla_host *vha = shost_priv(dev_to_shost(container_of(kobj,
+	    struct device, kobj)));
+	int type;
+	int rval = 0;
+	port_id_t did;
+
+	type = simple_strtol(buf, NULL, 10);
+
+	did.b.domain = (type & 0x00ff0000) >> 16;
+	did.b.area = (type & 0x0000ff00) >> 8;
+	did.b.al_pa = (type & 0x000000ff);
+
+	ql_log(ql_log_info, vha, 0x70e3, "portid=%02x%02x%02x done\n",
+	    did.b.domain, did.b.area, did.b.al_pa);
+
+	ql_log(ql_log_info, vha, 0x70e4, "%s: %d\n", __func__, type);
+
+	rval = qla24xx_els_dcmd_iocb(vha, ELS_DCMD_LOGO, did);
+	return count;
+}
+
+static struct bin_attribute sysfs_issue_logo_attr = {
+	.attr = {
+		.name = "issue_logo",
+		.mode = S_IWUSR,
+	},
+	.size = 0,
+	.write = qla2x00_issue_logo,
+};
+
+static ssize_t
 qla2x00_sysfs_read_xgmac_stats(struct file *filp, struct kobject *kobj,
 		       struct bin_attribute *bin_attr,
 		       char *buf, loff_t off, size_t count)
@@ -937,6 +972,7 @@ static struct sysfs_entry {
 	{ "vpd", &sysfs_vpd_attr, 1 },
 	{ "sfp", &sysfs_sfp_attr, 1 },
 	{ "reset", &sysfs_reset_attr, },
+	{ "issue_logo", &sysfs_issue_logo_attr, },
 	{ "xgmac_stats", &sysfs_xgmac_stats_attr, 3 },
 	{ "dcbx_tlv", &sysfs_dcbx_tlv_attr, 3 },
 	{ NULL },
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index 34dc9a35670b..cd0d94ea7f74 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -14,25 +14,24 @@
  * | Module Init and Probe        |       0x017f       | 0x0146         |
  * |                              |                    | 0x015b-0x0160	|
  * |                              |                    | 0x016e-0x0170  |
- * | Mailbox commands             |       0x118d       | 0x1115-0x1116	|
- * |                              |                    | 0x111a-0x111b  |
+ * | Mailbox commands             |       0x1192       |		|
+ * |                              |                    |		|
  * | Device Discovery             |       0x2016       | 0x2020-0x2022, |
  * |                              |                    | 0x2011-0x2012, |
  * |                              |                    | 0x2099-0x20a4  |
- * | Queue Command and IO tracing |       0x3075       | 0x300b         |
+ * | Queue Command and IO tracing |       0x3074       | 0x300b         |
  * |                              |                    | 0x3027-0x3028  |
  * |                              |                    | 0x303d-0x3041  |
  * |                              |                    | 0x302d,0x3033  |
  * |                              |                    | 0x3036,0x3038  |
  * |                              |                    | 0x303a		|
  * | DPC Thread                   |       0x4023       | 0x4002,0x4013  |
- * | Async Events                 |       0x508a       | 0x502b-0x502f  |
- * |                              |                    | 0x5047		|
+ * | Async Events                 |       0x5089       | 0x502b-0x502f  |
  * |                              |                    | 0x5084,0x5075	|
  * |                              |                    | 0x503d,0x5044  |
  * |                              |                    | 0x507b,0x505f	|
  * | Timer Routines               |       0x6012       |                |
- * | User Space Interactions      |       0x70e2       | 0x7018,0x702e  |
+ * | User Space Interactions      |       0x70e65      | 0x7018,0x702e  |
  * |				  |		       | 0x7020,0x7024  |
  * |                              |                    | 0x7039,0x7045  |
  * |                              |                    | 0x7073-0x7075  |
@@ -60,15 +59,11 @@
  * |                              |                    | 0xb13c-0xb140  |
  * |                              |                    | 0xb149		|
  * | MultiQ                       |       0xc00c       |		|
- * | Misc                         |       0xd300       | 0xd016-0xd017	|
- * |                              |                    | 0xd021,0xd024	|
- * |                              |                    | 0xd025,0xd029	|
- * |                              |                    | 0xd02a,0xd02e	|
- * |                              |                    | 0xd031-0xd0ff	|
+ * | Misc                         |       0xd301       | 0xd031-0xd0ff	|
  * |                              |                    | 0xd101-0xd1fe	|
  * |                              |                    | 0xd214-0xd2fe	|
  * | Target Mode		  |	  0xe080       |		|
- * | Target Mode Management	  |	  0xf096       | 0xf002		|
+ * | Target Mode Management	  |	  0xf09b       | 0xf002		|
  * |                              |                    | 0xf046-0xf049  |
  * | Target Mode Task Management  |	  0x1000d      |		|
  * ----------------------------------------------------------------------
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index 388d79088b59..9872f3429e53 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -259,7 +259,7 @@
 #define LOOP_DOWN_TIME			255	/* 240 */
 #define	LOOP_DOWN_RESET			(LOOP_DOWN_TIME - 30)
 
-#define DEFAULT_OUTSTANDING_COMMANDS	1024
+#define DEFAULT_OUTSTANDING_COMMANDS	4096
 #define MIN_OUTSTANDING_COMMANDS	128
 
 /* ISP request and response entry counts (37-65535) */
@@ -267,11 +267,13 @@
 #define REQUEST_ENTRY_CNT_2200		2048	/* Number of request entries. */
 #define REQUEST_ENTRY_CNT_24XX		2048	/* Number of request entries. */
 #define REQUEST_ENTRY_CNT_83XX		8192	/* Number of request entries. */
+#define RESPONSE_ENTRY_CNT_83XX		4096	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_2100		64	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_2300		512	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_MQ		128	/* Number of response entries.*/
 #define ATIO_ENTRY_CNT_24XX		4096	/* Number of ATIO entries. */
 #define RESPONSE_ENTRY_CNT_FX00		256     /* Number of response entries.*/
+#define EXTENDED_EXCH_ENTRY_CNT		32768   /* Entries for offload case */
 
 struct req_que;
 struct qla_tgt_sess;
@@ -309,6 +311,14 @@ struct srb_cmd {
 /* To identify if a srb is of T10-CRC type. @sp => srb_t pointer */
 #define IS_PROT_IO(sp)	(sp->flags & SRB_CRC_CTX_DSD_VALID)
 
+struct els_logo_payload {
+	uint8_t opcode;
+	uint8_t rsvd[3];
+	uint8_t s_id[3];
+	uint8_t rsvd1[1];
+	uint8_t wwpn[WWN_SIZE];
+};
+
 /*
  * SRB extensions.
  */
@@ -322,6 +332,15 @@ struct srb_iocb {
 			uint16_t data[2];
 		} logio;
 		struct {
+#define ELS_DCMD_TIMEOUT 20
+#define ELS_DCMD_LOGO 0x5
+			uint32_t flags;
+			uint32_t els_cmd;
+			struct completion comp;
+			struct els_logo_payload *els_logo_pyld;
+			dma_addr_t els_logo_pyld_dma;
+		} els_logo;
+		struct {
 			/*
 			 * Values for flags field below are as
 			 * defined in tsk_mgmt_entry struct
@@ -382,7 +401,7 @@ struct srb_iocb {
 #define SRB_FXIOCB_DCMD	10
 #define SRB_FXIOCB_BCMD	11
 #define SRB_ABT_CMD	12
-
+#define SRB_ELS_DCMD	13
 
 typedef struct srb {
 	atomic_t ref_count;
@@ -891,6 +910,7 @@ struct mbx_cmd_32 {
 #define MBC_DISABLE_VI			0x24	/* Disable VI operation. */
 #define MBC_ENABLE_VI			0x25	/* Enable VI operation. */
 #define MBC_GET_FIRMWARE_OPTION		0x28	/* Get Firmware Options. */
+#define MBC_GET_MEM_OFFLOAD_CNTRL_STAT	0x34	/* Memory Offload ctrl/Stat*/
 #define MBC_SET_FIRMWARE_OPTION		0x38	/* Set Firmware Options. */
 #define MBC_LOOP_PORT_BYPASS		0x40	/* Loop Port Bypass. */
 #define MBC_LOOP_PORT_ENABLE		0x41	/* Loop Port Enable. */
@@ -2695,11 +2715,16 @@ struct isp_operations {
 
 struct scsi_qla_host;
 
+
+#define QLA83XX_RSPQ_MSIX_ENTRY_NUMBER 1 /* refer to qla83xx_msix_entries */
+
 struct qla_msix_entry {
 	int have_irq;
 	uint32_t vector;
 	uint16_t entry;
 	struct rsp_que *rsp;
+	struct irq_affinity_notify irq_notify;
+	int cpuid;
 };
 
 #define	WATCH_INTERVAL		1       /* number of seconds */
@@ -2910,12 +2935,15 @@ struct qlt_hw_data {
 	uint32_t num_qfull_cmds_dropped;
 	spinlock_t q_full_lock;
 	uint32_t leak_exchg_thresh_hold;
+	spinlock_t sess_lock;
+	int rspq_vector_cpuid;
+	spinlock_t atio_lock ____cacheline_aligned;
 };
 
 #define MAX_QFULL_CMDS_ALLOC	8192
 #define Q_FULL_THRESH_HOLD_PERCENT 90
 #define Q_FULL_THRESH_HOLD(ha) \
-	((ha->fw_xcb_count/100) * Q_FULL_THRESH_HOLD_PERCENT)
+	((ha->cur_fw_xcb_count/100) * Q_FULL_THRESH_HOLD_PERCENT)
 
 #define LEAK_EXCHG_THRESH_HOLD_PERCENT 75	/* 75 percent */
 
@@ -2962,10 +2990,12 @@ struct qla_hw_data {
 		uint32_t	isp82xx_no_md_cap:1;
 		uint32_t	host_shutting_down:1;
 		uint32_t	idc_compl_status:1;
-
 		uint32_t        mr_reset_hdlr_active:1;
 		uint32_t        mr_intr_valid:1;
+
 		uint32_t	fawwpn_enabled:1;
+		uint32_t	exlogins_enabled:1;
+		uint32_t	exchoffld_enabled:1;
 		/* 35 bits */
 	} flags;
 
@@ -3237,6 +3267,21 @@ struct qla_hw_data {
 	void		*async_pd;
 	dma_addr_t	async_pd_dma;
 
+#define ENABLE_EXTENDED_LOGIN	BIT_7
+
+	/* Extended Logins  */
+	void		*exlogin_buf;
+	dma_addr_t	exlogin_buf_dma;
+	int		exlogin_size;
+
+#define ENABLE_EXCHANGE_OFFLD	BIT_2
+
+	/* Exchange Offload */
+	void		*exchoffld_buf;
+	dma_addr_t	exchoffld_buf_dma;
+	int		exchoffld_size;
+	int 		exchoffld_count;
+
 	void		*swl;
 
 	/* These are used by mailbox operations. */
@@ -3279,8 +3324,14 @@ struct qla_hw_data {
 #define RISC_START_ADDRESS_2100 0x1000
 #define RISC_START_ADDRESS_2300 0x800
 #define RISC_START_ADDRESS_2400 0x100000
-	uint16_t	fw_xcb_count;
-	uint16_t	fw_iocb_count;
+
+	uint16_t	orig_fw_tgt_xcb_count;
+	uint16_t	cur_fw_tgt_xcb_count;
+	uint16_t	orig_fw_xcb_count;
+	uint16_t	cur_fw_xcb_count;
+	uint16_t	orig_fw_iocb_count;
+	uint16_t	cur_fw_iocb_count;
+	uint16_t	fw_max_fcf_count;
 
 	uint32_t	fw_shared_ram_start;
 	uint32_t	fw_shared_ram_end;
@@ -3323,6 +3374,9 @@ struct qla_hw_data {
 	uint32_t	chain_offset;
 	struct dentry *dfs_dir;
 	struct dentry *dfs_fce;
+	struct dentry *dfs_tgt_counters;
+	struct dentry *dfs_fw_resource_cnt;
+
 	dma_addr_t	fce_dma;
 	void		*fce;
 	uint32_t	fce_bufs;
@@ -3480,6 +3534,18 @@ struct qla_hw_data {
 	int	allow_cna_fw_dump;
 };
 
+struct qla_tgt_counters {
+	uint64_t qla_core_sbt_cmd;
+	uint64_t core_qla_que_buf;
+	uint64_t qla_core_ret_ctio;
+	uint64_t core_qla_snd_status;
+	uint64_t qla_core_ret_sta_ctio;
+	uint64_t core_qla_free_cmd;
+	uint64_t num_q_full_sent;
+	uint64_t num_alloc_iocb_failed;
+	uint64_t num_term_xchg_sent;
+};
+
 /*
  * Qlogic scsi host structure
  */
@@ -3595,6 +3661,10 @@ typedef struct scsi_qla_host {
 	atomic_t		generation_tick;
 	/* Time when global fcport update has been scheduled */
 	int			total_fcport_update_gen;
+	/* List of pending LOGOs, protected by tgt_mutex */
+	struct list_head	logo_list;
+	/* List of pending PLOGI acks, protected by hw lock */
+	struct list_head	plogi_ack_list;
 
 	uint32_t	vp_abort_cnt;
 
@@ -3632,6 +3702,7 @@ typedef struct scsi_qla_host {
 
 	atomic_t	vref_count;
 	struct qla8044_reset_template reset_tmplt;
+	struct qla_tgt_counters tgt_counters;
 } scsi_qla_host_t;
 
 #define SET_VP_IDX	1
diff --git a/drivers/scsi/qla2xxx/qla_dfs.c b/drivers/scsi/qla2xxx/qla_dfs.c
index 15cf074ffa3c..cd8b96a4b0dd 100644
--- a/drivers/scsi/qla2xxx/qla_dfs.c
+++ b/drivers/scsi/qla2xxx/qla_dfs.c
@@ -13,6 +13,85 @@ static struct dentry *qla2x00_dfs_root;
 static atomic_t qla2x00_dfs_root_count;
 
 static int
+qla_dfs_fw_resource_cnt_show(struct seq_file *s, void *unused)
+{
+	struct scsi_qla_host *vha = s->private;
+	struct qla_hw_data *ha = vha->hw;
+
+	seq_puts(s, "FW Resource count\n\n");
+	seq_printf(s, "Original TGT exchg count[%d]\n",
+	    ha->orig_fw_tgt_xcb_count);
+	seq_printf(s, "current TGT exchg count[%d]\n",
+	    ha->cur_fw_tgt_xcb_count);
+	seq_printf(s, "original Initiator Exchange count[%d]\n",
+	    ha->orig_fw_xcb_count);
+	seq_printf(s, "Current Initiator Exchange count[%d]\n",
+	    ha->cur_fw_xcb_count);
+	seq_printf(s, "Original IOCB count[%d]\n", ha->orig_fw_iocb_count);
+	seq_printf(s, "Current IOCB count[%d]\n", ha->cur_fw_iocb_count);
+	seq_printf(s, "MAX VP count[%d]\n", ha->max_npiv_vports);
+	seq_printf(s, "MAX FCF count[%d]\n", ha->fw_max_fcf_count);
+
+	return 0;
+}
+
+static int
+qla_dfs_fw_resource_cnt_open(struct inode *inode, struct file *file)
+{
+	struct scsi_qla_host *vha = inode->i_private;
+	return single_open(file, qla_dfs_fw_resource_cnt_show, vha);
+}
+
+static const struct file_operations dfs_fw_resource_cnt_ops = {
+	.open           = qla_dfs_fw_resource_cnt_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static int
+qla_dfs_tgt_counters_show(struct seq_file *s, void *unused)
+{
+	struct scsi_qla_host *vha = s->private;
+
+	seq_puts(s, "Target Counters\n");
+	seq_printf(s, "qla_core_sbt_cmd = %lld\n",
+		vha->tgt_counters.qla_core_sbt_cmd);
+	seq_printf(s, "qla_core_ret_sta_ctio = %lld\n",
+		vha->tgt_counters.qla_core_ret_sta_ctio);
+	seq_printf(s, "qla_core_ret_ctio = %lld\n",
+		vha->tgt_counters.qla_core_ret_ctio);
+	seq_printf(s, "core_qla_que_buf = %lld\n",
+		vha->tgt_counters.core_qla_que_buf);
+	seq_printf(s, "core_qla_snd_status = %lld\n",
+		vha->tgt_counters.core_qla_snd_status);
+	seq_printf(s, "core_qla_free_cmd = %lld\n",
+		vha->tgt_counters.core_qla_free_cmd);
+	seq_printf(s, "num alloc iocb failed = %lld\n",
+		vha->tgt_counters.num_alloc_iocb_failed);
+	seq_printf(s, "num term exchange sent = %lld\n",
+		vha->tgt_counters.num_term_xchg_sent);
+	seq_printf(s, "num Q full sent = %lld\n",
+		vha->tgt_counters.num_q_full_sent);
+
+	return 0;
+}
+
+static int
+qla_dfs_tgt_counters_open(struct inode *inode, struct file *file)
+{
+	struct scsi_qla_host *vha = inode->i_private;
+	return single_open(file, qla_dfs_tgt_counters_show, vha);
+}
+
+static const struct file_operations dfs_tgt_counters_ops = {
+	.open           = qla_dfs_tgt_counters_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static int
 qla2x00_dfs_fce_show(struct seq_file *s, void *unused)
 {
 	scsi_qla_host_t *vha = s->private;
@@ -146,6 +225,22 @@ create_dir:
 	atomic_inc(&qla2x00_dfs_root_count);
 
 create_nodes:
+	ha->dfs_fw_resource_cnt = debugfs_create_file("fw_resource_count",
+	    S_IRUSR, ha->dfs_dir, vha, &dfs_fw_resource_cnt_ops);
+	if (!ha->dfs_fw_resource_cnt) {
+		ql_log(ql_log_warn, vha, 0x00fd,
+		    "Unable to create debugFS fw_resource_count node.\n");
+		goto out;
+	}
+
+	ha->dfs_tgt_counters = debugfs_create_file("tgt_counters", S_IRUSR,
+	    ha->dfs_dir, vha, &dfs_tgt_counters_ops);
+	if (!ha->dfs_tgt_counters) {
+		ql_log(ql_log_warn, vha, 0xd301,
+		    "Unable to create debugFS tgt_counters node.\n");
+		goto out;
+	}
+
 	ha->dfs_fce = debugfs_create_file("fce", S_IRUSR, ha->dfs_dir, vha,
 	    &dfs_fce_ops);
 	if (!ha->dfs_fce) {
@@ -161,6 +256,17 @@ int
 qla2x00_dfs_remove(scsi_qla_host_t *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
+
+	if (ha->dfs_fw_resource_cnt) {
+		debugfs_remove(ha->dfs_fw_resource_cnt);
+		ha->dfs_fw_resource_cnt = NULL;
+	}
+
+	if (ha->dfs_tgt_counters) {
+		debugfs_remove(ha->dfs_tgt_counters);
+		ha->dfs_tgt_counters = NULL;
+	}
+
 	if (ha->dfs_fce) {
 		debugfs_remove(ha->dfs_fce);
 		ha->dfs_fce = NULL;
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index 7686bfe9a4a9..0103e468e357 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -44,6 +44,8 @@ extern int qla2x00_find_new_loop_id(scsi_qla_host_t *, fc_port_t *);
 extern int qla2x00_fabric_login(scsi_qla_host_t *, fc_port_t *, uint16_t *);
 extern int qla2x00_local_device_login(scsi_qla_host_t *, fc_port_t *);
 
+extern int qla24xx_els_dcmd_iocb(scsi_qla_host_t *, int, port_id_t);
+
 extern void qla2x00_update_fcports(scsi_qla_host_t *);
 
 extern int qla2x00_abort_isp(scsi_qla_host_t *);
@@ -117,6 +119,8 @@ extern int ql2xdontresethba;
 extern uint64_t ql2xmaxlun;
 extern int ql2xmdcapmask;
 extern int ql2xmdenable;
+extern int ql2xexlogins;
+extern int ql2xexchoffld;
 
 extern int qla2x00_loop_reset(scsi_qla_host_t *);
 extern void qla2x00_abort_all_cmds(scsi_qla_host_t *, int);
@@ -135,6 +139,10 @@ extern int qla2x00_post_async_adisc_work(struct scsi_qla_host *, fc_port_t *,
     uint16_t *);
 extern int qla2x00_post_async_adisc_done_work(struct scsi_qla_host *,
     fc_port_t *, uint16_t *);
+extern int qla2x00_set_exlogins_buffer(struct scsi_qla_host *);
+extern void qla2x00_free_exlogin_buffer(struct qla_hw_data *);
+extern int qla2x00_set_exchoffld_buffer(struct scsi_qla_host *);
+extern void qla2x00_free_exchoffld_buffer(struct qla_hw_data *);
 
 extern int qla81xx_restart_mpi_firmware(scsi_qla_host_t *);
 
@@ -323,8 +331,7 @@ extern int
 qla2x00_get_id_list(scsi_qla_host_t *, void *, dma_addr_t, uint16_t *);
 
 extern int
-qla2x00_get_resource_cnts(scsi_qla_host_t *, uint16_t *, uint16_t *,
-    uint16_t *, uint16_t *, uint16_t *, uint16_t *);
+qla2x00_get_resource_cnts(scsi_qla_host_t *);
 
 extern int
 qla2x00_get_fcal_position_map(scsi_qla_host_t *ha, char *pos_map);
@@ -766,4 +773,11 @@ extern int qla8044_abort_isp(scsi_qla_host_t *);
 extern int qla8044_check_fw_alive(struct scsi_qla_host *);
 
 extern void qlt_host_reset_handler(struct qla_hw_data *ha);
+extern int qla_get_exlogin_status(scsi_qla_host_t *, uint16_t *,
+	uint16_t *);
+extern int qla_set_exlogin_mem_cfg(scsi_qla_host_t *vha, dma_addr_t phys_addr);
+extern int qla_get_exchoffld_status(scsi_qla_host_t *, uint16_t *, uint16_t *);
+extern int qla_set_exchoffld_mem_cfg(scsi_qla_host_t *, dma_addr_t);
+extern void qlt_handle_abts_recv(struct scsi_qla_host *, response_t *);
+
 #endif /* _QLA_GBL_H */
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 16a1935cc9c1..692a7570b5e1 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -1766,10 +1766,10 @@ qla2x00_alloc_outstanding_cmds(struct qla_hw_data *ha, struct req_que *req)
 	    (ql2xmultique_tag || ql2xmaxqueues > 1)))
 		req->num_outstanding_cmds = DEFAULT_OUTSTANDING_COMMANDS;
 	else {
-		if (ha->fw_xcb_count <= ha->fw_iocb_count)
-			req->num_outstanding_cmds = ha->fw_xcb_count;
+		if (ha->cur_fw_xcb_count <= ha->cur_fw_iocb_count)
+			req->num_outstanding_cmds = ha->cur_fw_xcb_count;
 		else
-			req->num_outstanding_cmds = ha->fw_iocb_count;
+			req->num_outstanding_cmds = ha->cur_fw_iocb_count;
 	}
 
 	req->outstanding_cmds = kzalloc(sizeof(srb_t *) *
@@ -1843,9 +1843,23 @@ qla2x00_setup_chip(scsi_qla_host_t *vha)
 			ql_dbg(ql_dbg_init, vha, 0x00ca,
 			    "Starting firmware.\n");
 
+			if (ql2xexlogins)
+				ha->flags.exlogins_enabled = 1;
+
+			if (ql2xexchoffld)
+				ha->flags.exchoffld_enabled = 1;
+
 			rval = qla2x00_execute_fw(vha, srisc_address);
 			/* Retrieve firmware information. */
 			if (rval == QLA_SUCCESS) {
+				rval = qla2x00_set_exlogins_buffer(vha);
+				if (rval != QLA_SUCCESS)
+					goto failed;
+
+				rval = qla2x00_set_exchoffld_buffer(vha);
+				if (rval != QLA_SUCCESS)
+					goto failed;
+
 enable_82xx_npiv:
 				fw_major_version = ha->fw_major_version;
 				if (IS_P3P_TYPE(ha))
@@ -1864,9 +1878,7 @@ enable_82xx_npiv:
 						ha->max_npiv_vports =
 						    MIN_MULTI_ID_FABRIC - 1;
 				}
-				qla2x00_get_resource_cnts(vha, NULL,
-				    &ha->fw_xcb_count, NULL, &ha->fw_iocb_count,
-				    &ha->max_npiv_vports, NULL);
+				qla2x00_get_resource_cnts(vha);
 
 				/*
 				 * Allocate the array of outstanding commands
@@ -2192,7 +2204,7 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 	/* Clear outstanding commands array. */
 	for (que = 0; que < ha->max_req_queues; que++) {
 		req = ha->req_q_map[que];
-		if (!req)
+		if (!req || !test_bit(que, ha->req_qid_map))
 			continue;
 		req->out_ptr = (void *)(req->ring + req->length);
 		*req->out_ptr = 0;
@@ -2209,7 +2221,7 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 
 	for (que = 0; que < ha->max_rsp_queues; que++) {
 		rsp = ha->rsp_q_map[que];
-		if (!rsp)
+		if (!rsp || !test_bit(que, ha->rsp_qid_map))
 			continue;
 		rsp->in_ptr = (void *)(rsp->ring + rsp->length);
 		*rsp->in_ptr = 0;
@@ -2248,7 +2260,7 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 	if (IS_FWI2_CAPABLE(ha)) {
 		mid_init_cb->options = cpu_to_le16(BIT_1);
 		mid_init_cb->init_cb.execution_throttle =
-		    cpu_to_le16(ha->fw_xcb_count);
+		    cpu_to_le16(ha->cur_fw_xcb_count);
 		/* D-Port Status */
 		if (IS_DPORT_CAPABLE(ha))
 			mid_init_cb->init_cb.firmware_options_1 |=
@@ -3053,6 +3065,26 @@ qla2x00_configure_loop(scsi_qla_host_t *vha)
 			atomic_set(&vha->loop_state, LOOP_READY);
 			ql_dbg(ql_dbg_disc, vha, 0x2069,
 			    "LOOP READY.\n");
+
+			/*
+			 * Process any ATIO queue entries that came in
+			 * while we weren't online.
+			 */
+			if (qla_tgt_mode_enabled(vha)) {
+				if (IS_QLA27XX(ha) || IS_QLA83XX(ha)) {
+					spin_lock_irqsave(&ha->tgt.atio_lock,
+					    flags);
+					qlt_24xx_process_atio_queue(vha, 0);
+					spin_unlock_irqrestore(
+					    &ha->tgt.atio_lock, flags);
+				} else {
+					spin_lock_irqsave(&ha->hardware_lock,
+					    flags);
+					qlt_24xx_process_atio_queue(vha, 1);
+					spin_unlock_irqrestore(
+					    &ha->hardware_lock, flags);
+				}
+			}
 		}
 	}
 
@@ -4907,7 +4939,6 @@ qla2x00_restart_isp(scsi_qla_host_t *vha)
 	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req = ha->req_q_map[0];
 	struct rsp_que *rsp = ha->rsp_q_map[0];
-	unsigned long flags;
 
 	/* If firmware needs to be loaded */
 	if (qla2x00_isp_firmware(vha)) {
@@ -4929,17 +4960,6 @@ qla2x00_restart_isp(scsi_qla_host_t *vha)
 			/* Issue a marker after FW becomes ready. */
 			qla2x00_marker(vha, req, rsp, 0, 0, MK_SYNC_ALL);
 
-			vha->flags.online = 1;
-
-			/*
-			 * Process any ATIO queue entries that came in
-			 * while we weren't online.
-			 */
-			spin_lock_irqsave(&ha->hardware_lock, flags);
-			if (qla_tgt_mode_enabled(vha))
-				qlt_24xx_process_atio_queue(vha);
-			spin_unlock_irqrestore(&ha->hardware_lock, flags);
-
 			set_bit(LOOP_RESYNC_NEEDED, &vha->dpc_flags);
 		}
 
@@ -4961,7 +4981,7 @@ qla25xx_init_queues(struct qla_hw_data *ha)
 
 	for (i = 1; i < ha->max_rsp_queues; i++) {
 		rsp = ha->rsp_q_map[i];
-		if (rsp) {
+		if (rsp && test_bit(i, ha->rsp_qid_map)) {
 			rsp->options &= ~BIT_0;
 			ret = qla25xx_init_rsp_que(base_vha, rsp);
 			if (ret != QLA_SUCCESS)
@@ -4976,8 +4996,8 @@ qla25xx_init_queues(struct qla_hw_data *ha)
 	}
 	for (i = 1; i < ha->max_req_queues; i++) {
 		req = ha->req_q_map[i];
-		if (req) {
-		/* Clear outstanding commands array. */
+		if (req && test_bit(i, ha->req_qid_map)) {
+			/* Clear outstanding commands array. */
 			req->options &= ~BIT_0;
 			ret = qla25xx_init_req_que(base_vha, req);
 			if (ret != QLA_SUCCESS)
diff --git a/drivers/scsi/qla2xxx/qla_inline.h b/drivers/scsi/qla2xxx/qla_inline.h
index fee9eb7c8a60..a6b7f1588aa4 100644
--- a/drivers/scsi/qla2xxx/qla_inline.h
+++ b/drivers/scsi/qla2xxx/qla_inline.h
@@ -258,6 +258,8 @@ qla2x00_init_timer(srb_t *sp, unsigned long tmo)
 	if ((IS_QLAFX00(sp->fcport->vha->hw)) &&
 	    (sp->type == SRB_FXIOCB_DCMD))
 		init_completion(&sp->u.iocb_cmd.u.fxiocb.fxiocb_comp);
+	if (sp->type == SRB_ELS_DCMD)
+		init_completion(&sp->u.iocb_cmd.u.els_logo.comp);
 }
 
 static inline int
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index c49df34e9b35..b41265a75ed5 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -1868,6 +1868,7 @@ skip_cmd_array:
 	}
 
 queuing_error:
+	vha->tgt_counters.num_alloc_iocb_failed++;
 	return pkt;
 }
 
@@ -2010,6 +2011,190 @@ qla24xx_tm_iocb(srb_t *sp, struct tsk_mgmt_entry *tsk)
 }
 
 static void
+qla2x00_els_dcmd_sp_free(void *ptr, void *data)
+{
+	struct scsi_qla_host *vha = (scsi_qla_host_t *)ptr;
+	struct qla_hw_data *ha = vha->hw;
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *elsio = &sp->u.iocb_cmd;
+
+	kfree(sp->fcport);
+
+	if (elsio->u.els_logo.els_logo_pyld)
+		dma_free_coherent(&ha->pdev->dev, DMA_POOL_SIZE,
+		    elsio->u.els_logo.els_logo_pyld,
+		    elsio->u.els_logo.els_logo_pyld_dma);
+
+	del_timer(&elsio->timer);
+	qla2x00_rel_sp(vha, sp);
+}
+
+static void
+qla2x00_els_dcmd_iocb_timeout(void *data)
+{
+	srb_t *sp = (srb_t *)data;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+	fc_port_t *fcport = sp->fcport;
+	struct scsi_qla_host *vha = fcport->vha;
+	struct qla_hw_data *ha = vha->hw;
+	unsigned long flags = 0;
+
+	ql_dbg(ql_dbg_io, vha, 0x3069,
+	    "%s Timeout, hdl=%x, portid=%02x%02x%02x\n",
+	    sp->name, sp->handle, fcport->d_id.b.domain, fcport->d_id.b.area,
+	    fcport->d_id.b.al_pa);
+
+	/* Abort the exchange */
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	if (ha->isp_ops->abort_command(sp)) {
+		ql_dbg(ql_dbg_io, vha, 0x3070,
+		    "mbx abort_command failed.\n");
+	} else {
+		ql_dbg(ql_dbg_io, vha, 0x3071,
+		    "mbx abort_command success.\n");
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	complete(&lio->u.els_logo.comp);
+}
+
+static void
+qla2x00_els_dcmd_sp_done(void *data, void *ptr, int res)
+{
+	srb_t *sp = (srb_t *)ptr;
+	fc_port_t *fcport = sp->fcport;
+	struct srb_iocb *lio = &sp->u.iocb_cmd;
+	struct scsi_qla_host *vha = fcport->vha;
+
+	ql_dbg(ql_dbg_io, vha, 0x3072,
+	    "%s hdl=%x, portid=%02x%02x%02x done\n",
+	    sp->name, sp->handle, fcport->d_id.b.domain,
+	    fcport->d_id.b.area, fcport->d_id.b.al_pa);
+
+	complete(&lio->u.els_logo.comp);
+}
+
+int
+qla24xx_els_dcmd_iocb(scsi_qla_host_t *vha, int els_opcode,
+    port_id_t remote_did)
+{
+	srb_t *sp;
+	fc_port_t *fcport = NULL;
+	struct srb_iocb *elsio = NULL;
+	struct qla_hw_data *ha = vha->hw;
+	struct els_logo_payload logo_pyld;
+	int rval = QLA_SUCCESS;
+
+	fcport = qla2x00_alloc_fcport(vha, GFP_KERNEL);
+	if (!fcport) {
+	       ql_log(ql_log_info, vha, 0x70e5, "fcport allocation failed\n");
+	       return -ENOMEM;
+	}
+
+	/* Alloc SRB structure */
+	sp = qla2x00_get_sp(vha, fcport, GFP_KERNEL);
+	if (!sp) {
+		kfree(fcport);
+		ql_log(ql_log_info, vha, 0x70e6,
+		 "SRB allocation failed\n");
+		return -ENOMEM;
+	}
+
+	elsio = &sp->u.iocb_cmd;
+	fcport->loop_id = 0xFFFF;
+	fcport->d_id.b.domain = remote_did.b.domain;
+	fcport->d_id.b.area = remote_did.b.area;
+	fcport->d_id.b.al_pa = remote_did.b.al_pa;
+
+	ql_dbg(ql_dbg_io, vha, 0x3073, "portid=%02x%02x%02x done\n",
+	    fcport->d_id.b.domain, fcport->d_id.b.area, fcport->d_id.b.al_pa);
+
+	sp->type = SRB_ELS_DCMD;
+	sp->name = "ELS_DCMD";
+	sp->fcport = fcport;
+	qla2x00_init_timer(sp, ELS_DCMD_TIMEOUT);
+	elsio->timeout = qla2x00_els_dcmd_iocb_timeout;
+	sp->done = qla2x00_els_dcmd_sp_done;
+	sp->free = qla2x00_els_dcmd_sp_free;
+
+	elsio->u.els_logo.els_logo_pyld = dma_alloc_coherent(&ha->pdev->dev,
+			    DMA_POOL_SIZE, &elsio->u.els_logo.els_logo_pyld_dma,
+			    GFP_KERNEL);
+
+	if (!elsio->u.els_logo.els_logo_pyld) {
+		sp->free(vha, sp);
+		return QLA_FUNCTION_FAILED;
+	}
+
+	memset(&logo_pyld, 0, sizeof(struct els_logo_payload));
+
+	elsio->u.els_logo.els_cmd = els_opcode;
+	logo_pyld.opcode = els_opcode;
+	logo_pyld.s_id[0] = vha->d_id.b.al_pa;
+	logo_pyld.s_id[1] = vha->d_id.b.area;
+	logo_pyld.s_id[2] = vha->d_id.b.domain;
+	host_to_fcp_swap(logo_pyld.s_id, sizeof(uint32_t));
+	memcpy(&logo_pyld.wwpn, vha->port_name, WWN_SIZE);
+
+	memcpy(elsio->u.els_logo.els_logo_pyld, &logo_pyld,
+	    sizeof(struct els_logo_payload));
+
+	rval = qla2x00_start_sp(sp);
+	if (rval != QLA_SUCCESS) {
+		sp->free(vha, sp);
+		return QLA_FUNCTION_FAILED;
+	}
+
+	ql_dbg(ql_dbg_io, vha, 0x3074,
+	    "%s LOGO sent, hdl=%x, loopid=%x, portid=%02x%02x%02x.\n",
+	    sp->name, sp->handle, fcport->loop_id, fcport->d_id.b.domain,
+	    fcport->d_id.b.area, fcport->d_id.b.al_pa);
+
+	wait_for_completion(&elsio->u.els_logo.comp);
+
+	sp->free(vha, sp);
+	return rval;
+}
+
+static void
+qla24xx_els_logo_iocb(srb_t *sp, struct els_entry_24xx *els_iocb)
+{
+	scsi_qla_host_t *vha = sp->fcport->vha;
+	struct srb_iocb *elsio = &sp->u.iocb_cmd;
+
+	els_iocb->entry_type = ELS_IOCB_TYPE;
+	els_iocb->entry_count = 1;
+	els_iocb->sys_define = 0;
+	els_iocb->entry_status = 0;
+	els_iocb->handle = sp->handle;
+	els_iocb->nport_handle = cpu_to_le16(sp->fcport->loop_id);
+	els_iocb->tx_dsd_count = 1;
+	els_iocb->vp_index = vha->vp_idx;
+	els_iocb->sof_type = EST_SOFI3;
+	els_iocb->rx_dsd_count = 0;
+	els_iocb->opcode = elsio->u.els_logo.els_cmd;
+
+	els_iocb->port_id[0] = sp->fcport->d_id.b.al_pa;
+	els_iocb->port_id[1] = sp->fcport->d_id.b.area;
+	els_iocb->port_id[2] = sp->fcport->d_id.b.domain;
+	els_iocb->control_flags = 0;
+
+	els_iocb->tx_byte_count = sizeof(struct els_logo_payload);
+	els_iocb->tx_address[0] =
+	    cpu_to_le32(LSD(elsio->u.els_logo.els_logo_pyld_dma));
+	els_iocb->tx_address[1] =
+	    cpu_to_le32(MSD(elsio->u.els_logo.els_logo_pyld_dma));
+	els_iocb->tx_len = cpu_to_le32(sizeof(struct els_logo_payload));
+
+	els_iocb->rx_byte_count = 0;
+	els_iocb->rx_address[0] = 0;
+	els_iocb->rx_address[1] = 0;
+	els_iocb->rx_len = 0;
+
+	sp->fcport->vha->qla_stats.control_requests++;
+}
+
+static void
 qla24xx_els_iocb(srb_t *sp, struct els_entry_24xx *els_iocb)
 {
 	struct fc_bsg_job *bsg_job = sp->u.bsg_job;
@@ -2623,6 +2808,9 @@ qla2x00_start_sp(srb_t *sp)
 			qlafx00_abort_iocb(sp, pkt) :
 			qla24xx_abort_iocb(sp, pkt);
 		break;
+	case SRB_ELS_DCMD:
+		qla24xx_els_logo_iocb(sp, pkt);
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index ccf6a7f99024..4af95479a9db 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -18,6 +18,10 @@ static void qla2x00_status_entry(scsi_qla_host_t *, struct rsp_que *, void *);
 static void qla2x00_status_cont_entry(struct rsp_que *, sts_cont_entry_t *);
 static void qla2x00_error_entry(scsi_qla_host_t *, struct rsp_que *,
 	sts_entry_t *);
+static void qla_irq_affinity_notify(struct irq_affinity_notify *,
+    const cpumask_t *);
+static void qla_irq_affinity_release(struct kref *);
+
 
 /**
  * qla2100_intr_handler() - Process interrupts for the ISP2100 and ISP2200.
@@ -1418,6 +1422,12 @@ qla24xx_els_ct_entry(scsi_qla_host_t *vha, struct req_que *req,
 	case SRB_CT_CMD:
 		type = "ct pass-through";
 		break;
+	case SRB_ELS_DCMD:
+		type = "Driver ELS logo";
+		ql_dbg(ql_dbg_user, vha, 0x5047,
+		    "Completing %s: (%p) type=%d.\n", type, sp, sp->type);
+		sp->done(vha, sp, 0);
+		return;
 	default:
 		ql_dbg(ql_dbg_user, vha, 0x503e,
 		    "Unrecognized SRB: (%p) type=%d.\n", sp, sp->type);
@@ -2542,6 +2552,14 @@ void qla24xx_process_response_queue(struct scsi_qla_host *vha,
 	if (!vha->flags.online)
 		return;
 
+	if (rsp->msix->cpuid != smp_processor_id()) {
+		/* if kernel does not notify qla of IRQ's CPU change,
+		 * then set it here.
+		 */
+		rsp->msix->cpuid = smp_processor_id();
+		ha->tgt.rspq_vector_cpuid = rsp->msix->cpuid;
+	}
+
 	while (rsp->ring_ptr->signature != RESPONSE_PROCESSED) {
 		pkt = (struct sts_entry_24xx *)rsp->ring_ptr;
 
@@ -2587,8 +2605,14 @@ process_err:
 			qla24xx_els_ct_entry(vha, rsp->req, pkt, ELS_IOCB_TYPE);
 			break;
 		case ABTS_RECV_24XX:
-			/* ensure that the ATIO queue is empty */
-			qlt_24xx_process_atio_queue(vha);
+			if (IS_QLA83XX(ha) || IS_QLA27XX(ha)) {
+				/* ensure that the ATIO queue is empty */
+				qlt_handle_abts_recv(vha, (response_t *)pkt);
+				break;
+			} else {
+				/* drop through */
+				qlt_24xx_process_atio_queue(vha, 1);
+			}
 		case ABTS_RESP_24XX:
 		case CTIO_TYPE7:
 		case NOTIFY_ACK_TYPE:
@@ -2755,13 +2779,22 @@ qla24xx_intr_handler(int irq, void *dev_id)
 		case INTR_RSP_QUE_UPDATE_83XX:
 			qla24xx_process_response_queue(vha, rsp);
 			break;
-		case INTR_ATIO_QUE_UPDATE:
-			qlt_24xx_process_atio_queue(vha);
+		case INTR_ATIO_QUE_UPDATE:{
+			unsigned long flags2;
+			spin_lock_irqsave(&ha->tgt.atio_lock, flags2);
+			qlt_24xx_process_atio_queue(vha, 1);
+			spin_unlock_irqrestore(&ha->tgt.atio_lock, flags2);
 			break;
-		case INTR_ATIO_RSP_QUE_UPDATE:
-			qlt_24xx_process_atio_queue(vha);
+		}
+		case INTR_ATIO_RSP_QUE_UPDATE: {
+			unsigned long flags2;
+			spin_lock_irqsave(&ha->tgt.atio_lock, flags2);
+			qlt_24xx_process_atio_queue(vha, 1);
+			spin_unlock_irqrestore(&ha->tgt.atio_lock, flags2);
+
 			qla24xx_process_response_queue(vha, rsp);
 			break;
+		}
 		default:
 			ql_dbg(ql_dbg_async, vha, 0x504f,
 			    "Unrecognized interrupt type (%d).\n", stat * 0xff);
@@ -2920,13 +2953,22 @@ qla24xx_msix_default(int irq, void *dev_id)
 		case INTR_RSP_QUE_UPDATE_83XX:
 			qla24xx_process_response_queue(vha, rsp);
 			break;
-		case INTR_ATIO_QUE_UPDATE:
-			qlt_24xx_process_atio_queue(vha);
+		case INTR_ATIO_QUE_UPDATE:{
+			unsigned long flags2;
+			spin_lock_irqsave(&ha->tgt.atio_lock, flags2);
+			qlt_24xx_process_atio_queue(vha, 1);
+			spin_unlock_irqrestore(&ha->tgt.atio_lock, flags2);
 			break;
-		case INTR_ATIO_RSP_QUE_UPDATE:
-			qlt_24xx_process_atio_queue(vha);
+		}
+		case INTR_ATIO_RSP_QUE_UPDATE: {
+			unsigned long flags2;
+			spin_lock_irqsave(&ha->tgt.atio_lock, flags2);
+			qlt_24xx_process_atio_queue(vha, 1);
+			spin_unlock_irqrestore(&ha->tgt.atio_lock, flags2);
+
 			qla24xx_process_response_queue(vha, rsp);
 			break;
+		}
 		default:
 			ql_dbg(ql_dbg_async, vha, 0x5051,
 			    "Unrecognized interrupt type (%d).\n", stat & 0xff);
@@ -2973,8 +3015,11 @@ qla24xx_disable_msix(struct qla_hw_data *ha)
 
 	for (i = 0; i < ha->msix_count; i++) {
 		qentry = &ha->msix_entries[i];
-		if (qentry->have_irq)
+		if (qentry->have_irq) {
+			/* un-register irq cpu affinity notification */
+			irq_set_affinity_notifier(qentry->vector, NULL);
 			free_irq(qentry->vector, qentry->rsp);
+		}
 	}
 	pci_disable_msix(ha->pdev);
 	kfree(ha->msix_entries);
@@ -3018,9 +3063,9 @@ qla24xx_enable_msix(struct qla_hw_data *ha, struct rsp_que *rsp)
 		    "MSI-X: Failed to enable support "
 		    "-- %d/%d\n Retry with %d vectors.\n",
 		    ha->msix_count, ret, ret);
+		ha->msix_count = ret;
+		ha->max_rsp_queues = ha->msix_count - 1;
 	}
-	ha->msix_count = ret;
-	ha->max_rsp_queues = ha->msix_count - 1;
 	ha->msix_entries = kzalloc(sizeof(struct qla_msix_entry) *
 				ha->msix_count, GFP_KERNEL);
 	if (!ha->msix_entries) {
@@ -3037,6 +3082,9 @@ qla24xx_enable_msix(struct qla_hw_data *ha, struct rsp_que *rsp)
 		qentry->entry = entries[i].entry;
 		qentry->have_irq = 0;
 		qentry->rsp = NULL;
+		qentry->irq_notify.notify  = qla_irq_affinity_notify;
+		qentry->irq_notify.release = qla_irq_affinity_release;
+		qentry->cpuid = -1;
 	}
 
 	/* Enable MSI-X vectors for the base queue */
@@ -3055,6 +3103,18 @@ qla24xx_enable_msix(struct qla_hw_data *ha, struct rsp_que *rsp)
 		qentry->have_irq = 1;
 		qentry->rsp = rsp;
 		rsp->msix = qentry;
+
+		/* Register for CPU affinity notification. */
+		irq_set_affinity_notifier(qentry->vector, &qentry->irq_notify);
+
+		/* Schedule work (ie. trigger a notification) to read cpu
+		 * mask for this specific irq.
+		 * kref_get is required because
+		* irq_affinity_notify() will do
+		* kref_put().
+		*/
+		kref_get(&qentry->irq_notify.kref);
+		schedule_work(&qentry->irq_notify.work);
 	}
 
 	/*
@@ -3234,3 +3294,47 @@ int qla25xx_request_irq(struct rsp_que *rsp)
 	msix->rsp = rsp;
 	return ret;
 }
+
+
+/* irq_set_affinity/irqbalance will trigger notification of cpu mask update */
+static void qla_irq_affinity_notify(struct irq_affinity_notify *notify,
+	const cpumask_t *mask)
+{
+	struct qla_msix_entry *e =
+		container_of(notify, struct qla_msix_entry, irq_notify);
+	struct qla_hw_data *ha;
+	struct scsi_qla_host *base_vha;
+
+	/* user is recommended to set mask to just 1 cpu */
+	e->cpuid = cpumask_first(mask);
+
+	ha = e->rsp->hw;
+	base_vha = pci_get_drvdata(ha->pdev);
+
+	ql_dbg(ql_dbg_init, base_vha, 0xffff,
+	    "%s: host %ld : vector %d cpu %d \n", __func__,
+	    base_vha->host_no, e->vector, e->cpuid);
+
+	if (e->have_irq) {
+		if ((IS_QLA83XX(ha) || IS_QLA27XX(ha)) &&
+		    (e->entry == QLA83XX_RSPQ_MSIX_ENTRY_NUMBER)) {
+			ha->tgt.rspq_vector_cpuid = e->cpuid;
+			ql_dbg(ql_dbg_init, base_vha, 0xffff,
+			    "%s: host%ld: rspq vector %d cpu %d  runtime change\n",
+			    __func__, base_vha->host_no, e->vector, e->cpuid);
+		}
+	}
+}
+
+static void qla_irq_affinity_release(struct kref *ref)
+{
+	struct irq_affinity_notify *notify =
+		container_of(ref, struct irq_affinity_notify, kref);
+	struct qla_msix_entry *e =
+		container_of(notify, struct qla_msix_entry, irq_notify);
+	struct scsi_qla_host *base_vha = pci_get_drvdata(e->rsp->hw->pdev);
+
+	ql_dbg(ql_dbg_init, base_vha, 0xffff,
+	    "%s: host%ld: vector %d cpu %d \n", __func__,
+	    base_vha->host_no, e->vector, e->cpuid);
+}
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index cb11e04be568..87e6758302f6 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -489,6 +489,13 @@ qla2x00_execute_fw(scsi_qla_host_t *vha, uint32_t risc_addr)
 			    EXTENDED_BB_CREDITS);
 		} else
 			mcp->mb[4] = 0;
+
+		if (ha->flags.exlogins_enabled)
+			mcp->mb[4] |= ENABLE_EXTENDED_LOGIN;
+
+		if (ha->flags.exchoffld_enabled)
+			mcp->mb[4] |= ENABLE_EXCHANGE_OFFLD;
+
 		mcp->out_mb |= MBX_4|MBX_3|MBX_2|MBX_1;
 		mcp->in_mb |= MBX_1;
 	} else {
@@ -521,6 +528,226 @@ qla2x00_execute_fw(scsi_qla_host_t *vha, uint32_t risc_addr)
 }
 
 /*
+ * qla_get_exlogin_status
+ *	Get extended login status
+ *	uses the memory offload control/status Mailbox
+ *
+ * Input:
+ *	ha:		adapter state pointer.
+ *	fwopt:		firmware options
+ *
+ * Returns:
+ *	qla2x00 local function status
+ *
+ * Context:
+ *	Kernel context.
+ */
+#define	FETCH_XLOGINS_STAT	0x8
+int
+qla_get_exlogin_status(scsi_qla_host_t *vha, uint16_t *buf_sz,
+	uint16_t *ex_logins_cnt)
+{
+	int rval;
+	mbx_cmd_t	mc;
+	mbx_cmd_t	*mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x118f,
+	    "Entered %s\n", __func__);
+
+	memset(mcp->mb, 0 , sizeof(mcp->mb));
+	mcp->mb[0] = MBC_GET_MEM_OFFLOAD_CNTRL_STAT;
+	mcp->mb[1] = FETCH_XLOGINS_STAT;
+	mcp->out_mb = MBX_1|MBX_0;
+	mcp->in_mb = MBX_10|MBX_4|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+
+	rval = qla2x00_mailbox_command(vha, mcp);
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1115, "Failed=%x.\n", rval);
+	} else {
+		*buf_sz = mcp->mb[4];
+		*ex_logins_cnt = mcp->mb[10];
+
+		ql_log(ql_log_info, vha, 0x1190,
+		    "buffer size 0x%x, exchange login count=%d\n",
+		    mcp->mb[4], mcp->mb[10]);
+
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1116,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qla_set_exlogin_mem_cfg
+ *	set extended login memory configuration
+ *	Mbx needs to be issues before init_cb is set
+ *
+ * Input:
+ *	ha:		adapter state pointer.
+ *	buffer:		buffer pointer
+ *	phys_addr:	physical address of buffer
+ *	size:		size of buffer
+ *	TARGET_QUEUE_LOCK must be released
+ *	ADAPTER_STATE_LOCK must be release
+ *
+ * Returns:
+ *	qla2x00 local funxtion status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+#define CONFIG_XLOGINS_MEM	0x3
+int
+qla_set_exlogin_mem_cfg(scsi_qla_host_t *vha, dma_addr_t phys_addr)
+{
+	int		rval;
+	mbx_cmd_t	mc;
+	mbx_cmd_t	*mcp = &mc;
+	struct qla_hw_data *ha = vha->hw;
+	int configured_count;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x111a,
+	    "Entered %s.\n", __func__);
+
+	memset(mcp->mb, 0 , sizeof(mcp->mb));
+	mcp->mb[0] = MBC_GET_MEM_OFFLOAD_CNTRL_STAT;
+	mcp->mb[1] = CONFIG_XLOGINS_MEM;
+	mcp->mb[2] = MSW(phys_addr);
+	mcp->mb[3] = LSW(phys_addr);
+	mcp->mb[6] = MSW(MSD(phys_addr));
+	mcp->mb[7] = LSW(MSD(phys_addr));
+	mcp->mb[8] = MSW(ha->exlogin_size);
+	mcp->mb[9] = LSW(ha->exlogin_size);
+	mcp->out_mb = MBX_9|MBX_8|MBX_7|MBX_6|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_11|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qla2x00_mailbox_command(vha, mcp);
+	if (rval != QLA_SUCCESS) {
+		/*EMPTY*/
+		ql_dbg(ql_dbg_mbx, vha, 0x111b, "Failed=%x.\n", rval);
+	} else {
+		configured_count = mcp->mb[11];
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x118c,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qla_get_exchoffld_status
+ *	Get exchange offload status
+ *	uses the memory offload control/status Mailbox
+ *
+ * Input:
+ *	ha:		adapter state pointer.
+ *	fwopt:		firmware options
+ *
+ * Returns:
+ *	qla2x00 local function status
+ *
+ * Context:
+ *	Kernel context.
+ */
+#define	FETCH_XCHOFFLD_STAT	0x2
+int
+qla_get_exchoffld_status(scsi_qla_host_t *vha, uint16_t *buf_sz,
+	uint16_t *ex_logins_cnt)
+{
+	int rval;
+	mbx_cmd_t	mc;
+	mbx_cmd_t	*mcp = &mc;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1019,
+	    "Entered %s\n", __func__);
+
+	memset(mcp->mb, 0 , sizeof(mcp->mb));
+	mcp->mb[0] = MBC_GET_MEM_OFFLOAD_CNTRL_STAT;
+	mcp->mb[1] = FETCH_XCHOFFLD_STAT;
+	mcp->out_mb = MBX_1|MBX_0;
+	mcp->in_mb = MBX_10|MBX_4|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+
+	rval = qla2x00_mailbox_command(vha, mcp);
+	if (rval != QLA_SUCCESS) {
+		ql_dbg(ql_dbg_mbx, vha, 0x1155, "Failed=%x.\n", rval);
+	} else {
+		*buf_sz = mcp->mb[4];
+		*ex_logins_cnt = mcp->mb[10];
+
+		ql_log(ql_log_info, vha, 0x118e,
+		    "buffer size 0x%x, exchange offload count=%d\n",
+		    mcp->mb[4], mcp->mb[10]);
+
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1156,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
+ * qla_set_exchoffld_mem_cfg
+ *	Set exchange offload memory configuration
+ *	Mbx needs to be issues before init_cb is set
+ *
+ * Input:
+ *	ha:		adapter state pointer.
+ *	buffer:		buffer pointer
+ *	phys_addr:	physical address of buffer
+ *	size:		size of buffer
+ *	TARGET_QUEUE_LOCK must be released
+ *	ADAPTER_STATE_LOCK must be release
+ *
+ * Returns:
+ *	qla2x00 local funxtion status code.
+ *
+ * Context:
+ *	Kernel context.
+ */
+#define CONFIG_XCHOFFLD_MEM	0x3
+int
+qla_set_exchoffld_mem_cfg(scsi_qla_host_t *vha, dma_addr_t phys_addr)
+{
+	int		rval;
+	mbx_cmd_t	mc;
+	mbx_cmd_t	*mcp = &mc;
+	struct qla_hw_data *ha = vha->hw;
+
+	ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1157,
+	    "Entered %s.\n", __func__);
+
+	memset(mcp->mb, 0 , sizeof(mcp->mb));
+	mcp->mb[0] = MBC_GET_MEM_OFFLOAD_CNTRL_STAT;
+	mcp->mb[1] = CONFIG_XCHOFFLD_MEM;
+	mcp->mb[2] = MSW(phys_addr);
+	mcp->mb[3] = LSW(phys_addr);
+	mcp->mb[6] = MSW(MSD(phys_addr));
+	mcp->mb[7] = LSW(MSD(phys_addr));
+	mcp->mb[8] = MSW(ha->exlogin_size);
+	mcp->mb[9] = LSW(ha->exlogin_size);
+	mcp->out_mb = MBX_9|MBX_8|MBX_7|MBX_6|MBX_3|MBX_2|MBX_1|MBX_0;
+	mcp->in_mb = MBX_11|MBX_0;
+	mcp->tov = MBX_TOV_SECONDS;
+	mcp->flags = 0;
+	rval = qla2x00_mailbox_command(vha, mcp);
+	if (rval != QLA_SUCCESS) {
+		/*EMPTY*/
+		ql_dbg(ql_dbg_mbx, vha, 0x1158, "Failed=%x.\n", rval);
+	} else {
+		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1192,
+		    "Done %s.\n", __func__);
+	}
+
+	return rval;
+}
+
+/*
  * qla2x00_get_fw_version
  *	Get firmware version.
  *
@@ -594,6 +821,16 @@ qla2x00_get_fw_version(scsi_qla_host_t *vha)
 		ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x112f,
 		    "%s: Ext_FwAttributes Upper: 0x%x, Lower: 0x%x.\n",
 		    __func__, mcp->mb[17], mcp->mb[16]);
+
+		if (ha->fw_attributes_h & 0x4)
+			ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x118d,
+			    "%s: Firmware supports Extended Login 0x%x\n",
+			    __func__, ha->fw_attributes_h);
+
+		if (ha->fw_attributes_h & 0x8)
+			ql_dbg(ql_dbg_mbx + ql_dbg_verbose, vha, 0x1191,
+			    "%s: Firmware supports Exchange Offload 0x%x\n",
+			    __func__, ha->fw_attributes_h);
 	}
 
 	if (IS_QLA27XX(ha)) {
@@ -2383,10 +2620,9 @@ qla2x00_get_id_list(scsi_qla_host_t *vha, void *id_list, dma_addr_t id_list_dma,
  *	Kernel context.
  */
 int
-qla2x00_get_resource_cnts(scsi_qla_host_t *vha, uint16_t *cur_xchg_cnt,
-    uint16_t *orig_xchg_cnt, uint16_t *cur_iocb_cnt,
-    uint16_t *orig_iocb_cnt, uint16_t *max_npiv_vports, uint16_t *max_fcfs)
+qla2x00_get_resource_cnts(scsi_qla_host_t *vha)
 {
+	struct qla_hw_data *ha = vha->hw;
 	int rval;
 	mbx_cmd_t mc;
 	mbx_cmd_t *mcp = &mc;
@@ -2414,19 +2650,16 @@ qla2x00_get_resource_cnts(scsi_qla_host_t *vha, uint16_t *cur_xchg_cnt,
 		    mcp->mb[3], mcp->mb[6], mcp->mb[7], mcp->mb[10],
 		    mcp->mb[11], mcp->mb[12]);
 
-		if (cur_xchg_cnt)
-			*cur_xchg_cnt = mcp->mb[3];
-		if (orig_xchg_cnt)
-			*orig_xchg_cnt = mcp->mb[6];
-		if (cur_iocb_cnt)
-			*cur_iocb_cnt = mcp->mb[7];
-		if (orig_iocb_cnt)
-			*orig_iocb_cnt = mcp->mb[10];
-		if (vha->hw->flags.npiv_supported && max_npiv_vports)
-			*max_npiv_vports = mcp->mb[11];
-		if ((IS_QLA81XX(vha->hw) || IS_QLA83XX(vha->hw) ||
-		    IS_QLA27XX(vha->hw)) && max_fcfs)
-			*max_fcfs = mcp->mb[12];
+		ha->orig_fw_tgt_xcb_count =  mcp->mb[1];
+		ha->cur_fw_tgt_xcb_count = mcp->mb[2];
+		ha->cur_fw_xcb_count = mcp->mb[3];
+		ha->orig_fw_xcb_count = mcp->mb[6];
+		ha->cur_fw_iocb_count = mcp->mb[7];
+		ha->orig_fw_iocb_count = mcp->mb[10];
+		if (ha->flags.npiv_supported)
+			ha->max_npiv_vports = mcp->mb[11];
+		if (IS_QLA81XX(ha) || IS_QLA83XX(ha) || IS_QLA27XX(ha))
+			ha->fw_max_fcf_count = mcp->mb[12];
 	}
 
 	return (rval);
diff --git a/drivers/scsi/qla2xxx/qla_mid.c b/drivers/scsi/qla2xxx/qla_mid.c
index c5dd594f6c31..cf7ba52bae66 100644
--- a/drivers/scsi/qla2xxx/qla_mid.c
+++ b/drivers/scsi/qla2xxx/qla_mid.c
@@ -600,7 +600,7 @@ qla25xx_delete_queues(struct scsi_qla_host *vha)
 	/* Delete request queues */
 	for (cnt = 1; cnt < ha->max_req_queues; cnt++) {
 		req = ha->req_q_map[cnt];
-		if (req) {
+		if (req && test_bit(cnt, ha->req_qid_map)) {
 			ret = qla25xx_delete_req_que(vha, req);
 			if (ret != QLA_SUCCESS) {
 				ql_log(ql_log_warn, vha, 0x00ea,
@@ -614,7 +614,7 @@ qla25xx_delete_queues(struct scsi_qla_host *vha)
 	/* Delete response queues */
 	for (cnt = 1; cnt < ha->max_rsp_queues; cnt++) {
 		rsp = ha->rsp_q_map[cnt];
-		if (rsp) {
+		if (rsp && test_bit(cnt, ha->rsp_qid_map)) {
 			ret = qla25xx_delete_rsp_que(vha, rsp);
 			if (ret != QLA_SUCCESS) {
 				ql_log(ql_log_warn, vha, 0x00eb,
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index bfa9a64c316b..f6c7ce35b542 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -221,6 +221,18 @@ MODULE_PARM_DESC(ql2xmdenable,
 		"0 - MiniDump disabled. "
 		"1 (Default) - MiniDump enabled.");
 
+int ql2xexlogins = 0;
+module_param(ql2xexlogins, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(ql2xexlogins,
+		 "Number of extended Logins. "
+		 "0 (Default)- Disabled.");
+
+int ql2xexchoffld = 0;
+module_param(ql2xexchoffld, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(ql2xexchoffld,
+		 "Number of exchanges to offload. "
+		 "0 (Default)- Disabled.");
+
 /*
  * SCSI host template entry points
  */
@@ -397,6 +409,9 @@ static void qla2x00_free_queues(struct qla_hw_data *ha)
 	int cnt;
 
 	for (cnt = 0; cnt < ha->max_req_queues; cnt++) {
+		if (!test_bit(cnt, ha->req_qid_map))
+			continue;
+
 		req = ha->req_q_map[cnt];
 		qla2x00_free_req_que(ha, req);
 	}
@@ -404,6 +419,9 @@ static void qla2x00_free_queues(struct qla_hw_data *ha)
 	ha->req_q_map = NULL;
 
 	for (cnt = 0; cnt < ha->max_rsp_queues; cnt++) {
+		if (!test_bit(cnt, ha->rsp_qid_map))
+			continue;
+
 		rsp = ha->rsp_q_map[cnt];
 		qla2x00_free_rsp_que(ha, rsp);
 	}
@@ -2324,6 +2342,9 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	ha->tgt.enable_class_2 = ql2xenableclass2;
 	INIT_LIST_HEAD(&ha->tgt.q_full_list);
 	spin_lock_init(&ha->tgt.q_full_lock);
+	spin_lock_init(&ha->tgt.sess_lock);
+	spin_lock_init(&ha->tgt.atio_lock);
+
 
 	/* Clear our data area */
 	ha->bars = bars;
@@ -2468,7 +2489,7 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		ha->max_fibre_devices = MAX_FIBRE_DEVICES_2400;
 		ha->mbx_count = MAILBOX_REGISTER_COUNT;
 		req_length = REQUEST_ENTRY_CNT_83XX;
-		rsp_length = RESPONSE_ENTRY_CNT_2300;
+		rsp_length = RESPONSE_ENTRY_CNT_83XX;
 		ha->tgt.atio_q_length = ATIO_ENTRY_CNT_24XX;
 		ha->max_loop_id = SNS_LAST_LOOP_ID_2300;
 		ha->init_cb_size = sizeof(struct mid_init_cb_81xx);
@@ -2498,8 +2519,8 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		ha->portnum = PCI_FUNC(ha->pdev->devfn);
 		ha->max_fibre_devices = MAX_FIBRE_DEVICES_2400;
 		ha->mbx_count = MAILBOX_REGISTER_COUNT;
-		req_length = REQUEST_ENTRY_CNT_24XX;
-		rsp_length = RESPONSE_ENTRY_CNT_2300;
+		req_length = REQUEST_ENTRY_CNT_83XX;
+		rsp_length = RESPONSE_ENTRY_CNT_83XX;
 		ha->tgt.atio_q_length = ATIO_ENTRY_CNT_24XX;
 		ha->max_loop_id = SNS_LAST_LOOP_ID_2300;
 		ha->init_cb_size = sizeof(struct mid_init_cb_81xx);
@@ -3128,6 +3149,14 @@ qla2x00_remove_one(struct pci_dev *pdev)
 
 	base_vha->flags.online = 0;
 
+	/* free DMA memory */
+	if (ha->exlogin_buf)
+		qla2x00_free_exlogin_buffer(ha);
+
+	/* free DMA memory */
+	if (ha->exchoffld_buf)
+		qla2x00_free_exchoffld_buffer(ha);
+
 	qla2x00_destroy_deferred_work(ha);
 
 	qlt_remove_target(ha, base_vha);
@@ -3587,6 +3616,140 @@ fail:
 	return -ENOMEM;
 }
 
+int
+qla2x00_set_exlogins_buffer(scsi_qla_host_t *vha)
+{
+	int rval;
+	uint16_t	size, max_cnt, temp;
+	struct qla_hw_data *ha = vha->hw;
+
+	/* Return if we don't need to alloacate any extended logins */
+	if (!ql2xexlogins)
+		return QLA_SUCCESS;
+
+	ql_log(ql_log_info, vha, 0xd021, "EXLOGIN count: %d.\n", ql2xexlogins);
+	max_cnt = 0;
+	rval = qla_get_exlogin_status(vha, &size, &max_cnt);
+	if (rval != QLA_SUCCESS) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0xd029,
+		    "Failed to get exlogin status.\n");
+		return rval;
+	}
+
+	temp = (ql2xexlogins > max_cnt) ? max_cnt : ql2xexlogins;
+	ha->exlogin_size = (size * temp);
+	ql_log(ql_log_info, vha, 0xd024,
+		"EXLOGIN: max_logins=%d, portdb=0x%x, total=%d.\n",
+		max_cnt, size, temp);
+
+	ql_log(ql_log_info, vha, 0xd025, "EXLOGIN: requested size=0x%x\n",
+		ha->exlogin_size);
+
+	/* Get consistent memory for extended logins */
+	ha->exlogin_buf = dma_alloc_coherent(&ha->pdev->dev,
+	    ha->exlogin_size, &ha->exlogin_buf_dma, GFP_KERNEL);
+	if (!ha->exlogin_buf) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0xd02a,
+		    "Failed to allocate memory for exlogin_buf_dma.\n");
+		return -ENOMEM;
+	}
+
+	/* Now configure the dma buffer */
+	rval = qla_set_exlogin_mem_cfg(vha, ha->exlogin_buf_dma);
+	if (rval) {
+		ql_log(ql_log_fatal, vha, 0x00cf,
+		    "Setup extended login buffer  ****FAILED****.\n");
+		qla2x00_free_exlogin_buffer(ha);
+	}
+
+	return rval;
+}
+
+/*
+* qla2x00_free_exlogin_buffer
+*
+* Input:
+*	ha = adapter block pointer
+*/
+void
+qla2x00_free_exlogin_buffer(struct qla_hw_data *ha)
+{
+	if (ha->exlogin_buf) {
+		dma_free_coherent(&ha->pdev->dev, ha->exlogin_size,
+		    ha->exlogin_buf, ha->exlogin_buf_dma);
+		ha->exlogin_buf = NULL;
+		ha->exlogin_size = 0;
+	}
+}
+
+int
+qla2x00_set_exchoffld_buffer(scsi_qla_host_t *vha)
+{
+	int rval;
+	uint16_t	size, max_cnt, temp;
+	struct qla_hw_data *ha = vha->hw;
+
+	/* Return if we don't need to alloacate any extended logins */
+	if (!ql2xexchoffld)
+		return QLA_SUCCESS;
+
+	ql_log(ql_log_info, vha, 0xd014,
+	    "Exchange offload count: %d.\n", ql2xexlogins);
+
+	max_cnt = 0;
+	rval = qla_get_exchoffld_status(vha, &size, &max_cnt);
+	if (rval != QLA_SUCCESS) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0xd012,
+		    "Failed to get exlogin status.\n");
+		return rval;
+	}
+
+	temp = (ql2xexchoffld > max_cnt) ? max_cnt : ql2xexchoffld;
+	ha->exchoffld_size = (size * temp);
+	ql_log(ql_log_info, vha, 0xd016,
+		"Exchange offload: max_count=%d, buffers=0x%x, total=%d.\n",
+		max_cnt, size, temp);
+
+	ql_log(ql_log_info, vha, 0xd017,
+	    "Exchange Buffers requested size = 0x%x\n", ha->exchoffld_size);
+
+	/* Get consistent memory for extended logins */
+	ha->exchoffld_buf = dma_alloc_coherent(&ha->pdev->dev,
+	    ha->exchoffld_size, &ha->exchoffld_buf_dma, GFP_KERNEL);
+	if (!ha->exchoffld_buf) {
+		ql_log_pci(ql_log_fatal, ha->pdev, 0xd013,
+		    "Failed to allocate memory for exchoffld_buf_dma.\n");
+		return -ENOMEM;
+	}
+
+	/* Now configure the dma buffer */
+	rval = qla_set_exchoffld_mem_cfg(vha, ha->exchoffld_buf_dma);
+	if (rval) {
+		ql_log(ql_log_fatal, vha, 0xd02e,
+		    "Setup exchange offload buffer ****FAILED****.\n");
+		qla2x00_free_exchoffld_buffer(ha);
+	}
+
+	return rval;
+}
+
+/*
+* qla2x00_free_exchoffld_buffer
+*
+* Input:
+*	ha = adapter block pointer
+*/
+void
+qla2x00_free_exchoffld_buffer(struct qla_hw_data *ha)
+{
+	if (ha->exchoffld_buf) {
+		dma_free_coherent(&ha->pdev->dev, ha->exchoffld_size,
+		    ha->exchoffld_buf, ha->exchoffld_buf_dma);
+		ha->exchoffld_buf = NULL;
+		ha->exchoffld_size = 0;
+	}
+}
+
 /*
 * qla2x00_free_fw_dump
 *	Frees fw dump stuff.
@@ -3766,6 +3929,8 @@ struct scsi_qla_host *qla2x00_create_host(struct scsi_host_template *sht,
 	INIT_LIST_HEAD(&vha->list);
 	INIT_LIST_HEAD(&vha->qla_cmd_list);
 	INIT_LIST_HEAD(&vha->qla_sess_op_cmd_list);
+	INIT_LIST_HEAD(&vha->logo_list);
+	INIT_LIST_HEAD(&vha->plogi_ack_list);
 
 	spin_lock_init(&vha->work_lock);
 	spin_lock_init(&vha->cmd_list_lock);
@@ -5843,6 +6008,3 @@ MODULE_FIRMWARE(FW_FILE_ISP2300);
 MODULE_FIRMWARE(FW_FILE_ISP2322);
 MODULE_FIRMWARE(FW_FILE_ISP24XX);
 MODULE_FIRMWARE(FW_FILE_ISP25XX);
-MODULE_FIRMWARE(FW_FILE_ISP2031);
-MODULE_FIRMWARE(FW_FILE_ISP8031);
-MODULE_FIRMWARE(FW_FILE_ISP27XX);
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index 75514a15bea0..ee967becd257 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -100,12 +100,12 @@ enum fcp_resp_rsp_codes {
  */
 /* Predefs for callbacks handed to qla2xxx LLD */
 static void qlt_24xx_atio_pkt(struct scsi_qla_host *ha,
-	struct atio_from_isp *pkt);
+	struct atio_from_isp *pkt, uint8_t);
 static void qlt_response_pkt(struct scsi_qla_host *ha, response_t *pkt);
 static int qlt_issue_task_mgmt(struct qla_tgt_sess *sess, uint32_t lun,
 	int fn, void *iocb, int flags);
 static void qlt_send_term_exchange(struct scsi_qla_host *ha, struct qla_tgt_cmd
-	*cmd, struct atio_from_isp *atio, int ha_locked);
+	*cmd, struct atio_from_isp *atio, int ha_locked, int ul_abort);
 static void qlt_reject_free_srr_imm(struct scsi_qla_host *ha,
 	struct qla_tgt_srr_imm *imm, int ha_lock);
 static void qlt_abort_cmd_on_host_reset(struct scsi_qla_host *vha,
@@ -118,10 +118,13 @@ static void qlt_send_notify_ack(struct scsi_qla_host *vha,
 	struct imm_ntfy_from_isp *ntfy,
 	uint32_t add_flags, uint16_t resp_code, int resp_code_valid,
 	uint16_t srr_flags, uint16_t srr_reject_code, uint8_t srr_explan);
+static void qlt_send_term_imm_notif(struct scsi_qla_host *vha,
+	struct imm_ntfy_from_isp *imm, int ha_locked);
 /*
  * Global Variables
  */
 static struct kmem_cache *qla_tgt_mgmt_cmd_cachep;
+static struct kmem_cache *qla_tgt_plogi_cachep;
 static mempool_t *qla_tgt_mgmt_cmd_mempool;
 static struct workqueue_struct *qla_tgt_wq;
 static DEFINE_MUTEX(qla_tgt_mutex);
@@ -226,8 +229,8 @@ static inline void qlt_decr_num_pend_cmds(struct scsi_qla_host *vha)
 	spin_unlock_irqrestore(&vha->hw->tgt.q_full_lock, flags);
 }
 
-static void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
-	struct atio_from_isp *atio)
+static bool qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
+	struct atio_from_isp *atio, uint8_t ha_locked)
 {
 	ql_dbg(ql_dbg_tgt, vha, 0xe072,
 		"%s: qla_target(%d): type %x ox_id %04x\n",
@@ -248,7 +251,7 @@ static void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 			    atio->u.isp24.fcp_hdr.d_id[2]);
 			break;
 		}
-		qlt_24xx_atio_pkt(host, atio);
+		qlt_24xx_atio_pkt(host, atio, ha_locked);
 		break;
 	}
 
@@ -271,7 +274,7 @@ static void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 				break;
 			}
 		}
-		qlt_24xx_atio_pkt(host, atio);
+		qlt_24xx_atio_pkt(host, atio, ha_locked);
 		break;
 	}
 
@@ -282,7 +285,7 @@ static void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 		break;
 	}
 
-	return;
+	return false;
 }
 
 void qlt_response_pkt_all_vps(struct scsi_qla_host *vha, response_t *pkt)
@@ -389,6 +392,131 @@ void qlt_response_pkt_all_vps(struct scsi_qla_host *vha, response_t *pkt)
 
 }
 
+/*
+ * All qlt_plogi_ack_t operations are protected by hardware_lock
+ */
+
+/*
+ * This is a zero-base ref-counting solution, since hardware_lock
+ * guarantees that ref_count is not modified concurrently.
+ * Upon successful return content of iocb is undefined
+ */
+static qlt_plogi_ack_t *
+qlt_plogi_ack_find_add(struct scsi_qla_host *vha, port_id_t *id,
+		       struct imm_ntfy_from_isp *iocb)
+{
+	qlt_plogi_ack_t *pla;
+
+	list_for_each_entry(pla, &vha->plogi_ack_list, list) {
+		if (pla->id.b24 == id->b24) {
+			qlt_send_term_imm_notif(vha, &pla->iocb, 1);
+			pla->iocb = *iocb;
+			return pla;
+		}
+	}
+
+	pla = kmem_cache_zalloc(qla_tgt_plogi_cachep, GFP_ATOMIC);
+	if (!pla) {
+		ql_dbg(ql_dbg_async, vha, 0x5088,
+		       "qla_target(%d): Allocation of plogi_ack failed\n",
+		       vha->vp_idx);
+		return NULL;
+	}
+
+	pla->iocb = *iocb;
+	pla->id = *id;
+	list_add_tail(&pla->list, &vha->plogi_ack_list);
+
+	return pla;
+}
+
+static void qlt_plogi_ack_unref(struct scsi_qla_host *vha, qlt_plogi_ack_t *pla)
+{
+	BUG_ON(!pla->ref_count);
+	pla->ref_count--;
+
+	if (pla->ref_count)
+		return;
+
+	ql_dbg(ql_dbg_async, vha, 0x5089,
+	    "Sending PLOGI ACK to wwn %8phC s_id %02x:%02x:%02x loop_id %#04x"
+	    " exch %#x ox_id %#x\n", pla->iocb.u.isp24.port_name,
+	    pla->iocb.u.isp24.port_id[2], pla->iocb.u.isp24.port_id[1],
+	    pla->iocb.u.isp24.port_id[0],
+	    le16_to_cpu(pla->iocb.u.isp24.nport_handle),
+	    pla->iocb.u.isp24.exchange_address, pla->iocb.ox_id);
+	qlt_send_notify_ack(vha, &pla->iocb, 0, 0, 0, 0, 0, 0);
+
+	list_del(&pla->list);
+	kmem_cache_free(qla_tgt_plogi_cachep, pla);
+}
+
+static void
+qlt_plogi_ack_link(struct scsi_qla_host *vha, qlt_plogi_ack_t *pla,
+    struct qla_tgt_sess *sess, qlt_plogi_link_t link)
+{
+	/* Inc ref_count first because link might already be pointing at pla */
+	pla->ref_count++;
+
+	if (sess->plogi_link[link])
+		qlt_plogi_ack_unref(vha, sess->plogi_link[link]);
+
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf097,
+	    "Linking sess %p [%d] wwn %8phC with PLOGI ACK to wwn %8phC"
+	    " s_id %02x:%02x:%02x, ref=%d\n", sess, link, sess->port_name,
+	    pla->iocb.u.isp24.port_name, pla->iocb.u.isp24.port_id[2],
+	    pla->iocb.u.isp24.port_id[1], pla->iocb.u.isp24.port_id[0],
+	    pla->ref_count);
+
+	sess->plogi_link[link] = pla;
+}
+
+typedef struct {
+	/* These fields must be initialized by the caller */
+	port_id_t id;
+	/*
+	 * number of cmds dropped while we were waiting for
+	 * initiator to ack LOGO initialize to 1 if LOGO is
+	 * triggered by a command, otherwise, to 0
+	 */
+	int cmd_count;
+
+	/* These fields are used by callee */
+	struct list_head list;
+} qlt_port_logo_t;
+
+static void
+qlt_send_first_logo(struct scsi_qla_host *vha, qlt_port_logo_t *logo)
+{
+	qlt_port_logo_t *tmp;
+	int res;
+
+	mutex_lock(&vha->vha_tgt.tgt_mutex);
+
+	list_for_each_entry(tmp, &vha->logo_list, list) {
+		if (tmp->id.b24 == logo->id.b24) {
+			tmp->cmd_count += logo->cmd_count;
+			mutex_unlock(&vha->vha_tgt.tgt_mutex);
+			return;
+		}
+	}
+
+	list_add_tail(&logo->list, &vha->logo_list);
+
+	mutex_unlock(&vha->vha_tgt.tgt_mutex);
+
+	res = qla24xx_els_dcmd_iocb(vha, ELS_DCMD_LOGO, logo->id);
+
+	mutex_lock(&vha->vha_tgt.tgt_mutex);
+	list_del(&logo->list);
+	mutex_unlock(&vha->vha_tgt.tgt_mutex);
+
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf098,
+	    "Finished LOGO to %02x:%02x:%02x, dropped %d cmds, res = %#x\n",
+	    logo->id.b.domain, logo->id.b.area, logo->id.b.al_pa,
+	    logo->cmd_count, res);
+}
+
 static void qlt_free_session_done(struct work_struct *work)
 {
 	struct qla_tgt_sess *sess = container_of(work, struct qla_tgt_sess,
@@ -402,14 +530,21 @@ static void qlt_free_session_done(struct work_struct *work)
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf084,
 		"%s: se_sess %p / sess %p from port %8phC loop_id %#04x"
-		" s_id %02x:%02x:%02x logout %d keep %d plogi %d\n",
+		" s_id %02x:%02x:%02x logout %d keep %d els_logo %d\n",
 		__func__, sess->se_sess, sess, sess->port_name, sess->loop_id,
 		sess->s_id.b.domain, sess->s_id.b.area, sess->s_id.b.al_pa,
 		sess->logout_on_delete, sess->keep_nport_handle,
-		sess->plogi_ack_needed);
+		sess->send_els_logo);
 
 	BUG_ON(!tgt);
 
+	if (sess->send_els_logo) {
+		qlt_port_logo_t logo;
+		logo.id = sess->s_id;
+		logo.cmd_count = 0;
+		qlt_send_first_logo(vha, &logo);
+	}
+
 	if (sess->logout_on_delete) {
 		int rc;
 
@@ -455,9 +590,34 @@ static void qlt_free_session_done(struct work_struct *work)
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
-	if (sess->plogi_ack_needed)
-		qlt_send_notify_ack(vha, &sess->tm_iocb,
-				    0, 0, 0, 0, 0, 0);
+	{
+		qlt_plogi_ack_t *own =
+		    sess->plogi_link[QLT_PLOGI_LINK_SAME_WWN];
+		qlt_plogi_ack_t *con =
+		    sess->plogi_link[QLT_PLOGI_LINK_CONFLICT];
+
+		if (con) {
+			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf099,
+			    "se_sess %p / sess %p port %8phC is gone,"
+			    " %s (ref=%d), releasing PLOGI for %8phC (ref=%d)\n",
+			    sess->se_sess, sess, sess->port_name,
+			    own ? "releasing own PLOGI" :
+			    "no own PLOGI pending",
+			    own ? own->ref_count : -1,
+			    con->iocb.u.isp24.port_name, con->ref_count);
+			qlt_plogi_ack_unref(vha, con);
+		} else {
+			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf09a,
+			    "se_sess %p / sess %p port %8phC is gone, %s (ref=%d)\n",
+			    sess->se_sess, sess, sess->port_name,
+			    own ? "releasing own PLOGI" :
+			    "no own PLOGI pending",
+			    own ? own->ref_count : -1);
+		}
+
+		if (own)
+			qlt_plogi_ack_unref(vha, own);
+	}
 
 	list_del(&sess->sess_list_entry);
 
@@ -476,7 +636,7 @@ static void qlt_free_session_done(struct work_struct *work)
 		wake_up_all(&tgt->waitQ);
 }
 
-/* ha->hardware_lock supposed to be held on entry */
+/* ha->tgt.sess_lock supposed to be held on entry */
 void qlt_unreg_sess(struct qla_tgt_sess *sess)
 {
 	struct scsi_qla_host *vha = sess->vha;
@@ -492,7 +652,7 @@ void qlt_unreg_sess(struct qla_tgt_sess *sess)
 }
 EXPORT_SYMBOL(qlt_unreg_sess);
 
-/* ha->hardware_lock supposed to be held on entry */
+
 static int qlt_reset(struct scsi_qla_host *vha, void *iocb, int mcmd)
 {
 	struct qla_hw_data *ha = vha->hw;
@@ -502,12 +662,15 @@ static int qlt_reset(struct scsi_qla_host *vha, void *iocb, int mcmd)
 	int res = 0;
 	struct imm_ntfy_from_isp *n = (struct imm_ntfy_from_isp *)iocb;
 	struct atio_from_isp *a = (struct atio_from_isp *)iocb;
+	unsigned long flags;
 
 	loop_id = le16_to_cpu(n->u.isp24.nport_handle);
 	if (loop_id == 0xFFFF) {
 		/* Global event */
 		atomic_inc(&vha->vha_tgt.qla_tgt->tgt_global_resets_count);
+		spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 		qlt_clear_tgt_db(vha->vha_tgt.qla_tgt);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 #if 0 /* FIXME: do we need to choose a session here? */
 		if (!list_empty(&ha->tgt.qla_tgt->sess_list)) {
 			sess = list_entry(ha->tgt.qla_tgt->sess_list.next,
@@ -534,7 +697,9 @@ static int qlt_reset(struct scsi_qla_host *vha, void *iocb, int mcmd)
 			sess = NULL;
 #endif
 	} else {
+		spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 		sess = ha->tgt.tgt_ops->find_sess_by_loop_id(vha, loop_id);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	}
 
 	ql_dbg(ql_dbg_tgt, vha, 0xe000,
@@ -556,7 +721,7 @@ static int qlt_reset(struct scsi_qla_host *vha, void *iocb, int mcmd)
 	    iocb, QLA24XX_MGMT_SEND_NACK);
 }
 
-/* ha->hardware_lock supposed to be held on entry */
+/* ha->tgt.sess_lock supposed to be held on entry */
 static void qlt_schedule_sess_for_deletion(struct qla_tgt_sess *sess,
 	bool immediate)
 {
@@ -600,7 +765,7 @@ static void qlt_schedule_sess_for_deletion(struct qla_tgt_sess *sess,
 		    sess->expires - jiffies);
 }
 
-/* ha->hardware_lock supposed to be held on entry */
+/* ha->tgt.sess_lock supposed to be held on entry */
 static void qlt_clear_tgt_db(struct qla_tgt *tgt)
 {
 	struct qla_tgt_sess *sess;
@@ -636,12 +801,12 @@ static int qla24xx_get_loop_id(struct scsi_qla_host *vha, const uint8_t *s_id,
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf045,
 		    "qla_target(%d): get_id_list() failed: %x\n",
 		    vha->vp_idx, rc);
-		res = -1;
+		res = -EBUSY;
 		goto out_free_id_list;
 	}
 
 	id_iter = (char *)gid_list;
-	res = -1;
+	res = -ENOENT;
 	for (i = 0; i < entries; i++) {
 		struct gid_list_info *gid = (struct gid_list_info *)id_iter;
 		if ((gid->al_pa == s_id[2]) &&
@@ -660,7 +825,7 @@ out_free_id_list:
 	return res;
 }
 
-/* ha->hardware_lock supposed to be held on entry */
+/* ha->tgt.sess_lock supposed to be held on entry */
 static void qlt_undelete_sess(struct qla_tgt_sess *sess)
 {
 	BUG_ON(sess->deleted != QLA_SESS_DELETION_PENDING);
@@ -678,7 +843,7 @@ static void qlt_del_sess_work_fn(struct delayed_work *work)
 	struct qla_tgt_sess *sess;
 	unsigned long flags, elapsed;
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	while (!list_empty(&tgt->del_sess_list)) {
 		sess = list_entry(tgt->del_sess_list.next, typeof(*sess),
 		    del_list_entry);
@@ -699,7 +864,7 @@ static void qlt_del_sess_work_fn(struct delayed_work *work)
 			break;
 		}
 	}
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 }
 
 /*
@@ -717,7 +882,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 	unsigned char be_sid[3];
 
 	/* Check to avoid double sessions */
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	list_for_each_entry(sess, &vha->vha_tgt.qla_tgt->sess_list,
 				sess_list_entry) {
 		if (!memcmp(sess->port_name, fcport->port_name, WWN_SIZE)) {
@@ -732,7 +897,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 
 			/* Cannot undelete at this point */
 			if (sess->deleted == QLA_SESS_DELETION_IN_PROGRESS) {
-				spin_unlock_irqrestore(&ha->hardware_lock,
+				spin_unlock_irqrestore(&ha->tgt.sess_lock,
 				    flags);
 				return NULL;
 			}
@@ -749,12 +914,12 @@ static struct qla_tgt_sess *qlt_create_sess(
 
 			qlt_do_generation_tick(vha, &sess->generation);
 
-			spin_unlock_irqrestore(&ha->hardware_lock, flags);
+			spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 
 			return sess;
 		}
 	}
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 
 	sess = kzalloc(sizeof(*sess), GFP_KERNEL);
 	if (!sess) {
@@ -799,7 +964,7 @@ static struct qla_tgt_sess *qlt_create_sess(
 	}
 	/*
 	 * Take an extra reference to ->sess_kref here to handle qla_tgt_sess
-	 * access across ->hardware_lock reaquire.
+	 * access across ->tgt.sess_lock reaquire.
 	 */
 	kref_get(&sess->se_sess->sess_kref);
 
@@ -807,11 +972,11 @@ static struct qla_tgt_sess *qlt_create_sess(
 	BUILD_BUG_ON(sizeof(sess->port_name) != sizeof(fcport->port_name));
 	memcpy(sess->port_name, fcport->port_name, sizeof(sess->port_name));
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	list_add_tail(&sess->sess_list_entry, &vha->vha_tgt.qla_tgt->sess_list);
 	vha->vha_tgt.qla_tgt->sess_count++;
 	qlt_do_generation_tick(vha, &sess->generation);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf04b,
 	    "qla_target(%d): %ssession for wwn %8phC (loop_id %d, "
@@ -842,23 +1007,23 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 	if (qla_ini_mode_enabled(vha))
 		return;
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	if (tgt->tgt_stop) {
-		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 		return;
 	}
 	sess = qlt_find_sess_by_port_name(tgt, fcport->port_name);
 	if (!sess) {
-		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 
 		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_create_sess(vha, fcport, false);
 		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
-		spin_lock_irqsave(&ha->hardware_lock, flags);
+		spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	} else if (sess->deleted == QLA_SESS_DELETION_IN_PROGRESS) {
 		/* Point of no return */
-		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 		return;
 	} else {
 		kref_get(&sess->se_sess->sess_kref);
@@ -887,7 +1052,7 @@ void qlt_fc_port_added(struct scsi_qla_host *vha, fc_port_t *fcport)
 		sess->local = 0;
 	}
 	ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 }
 
 /*
@@ -899,6 +1064,7 @@ qlt_fc_port_deleted(struct scsi_qla_host *vha, fc_port_t *fcport, int max_gen)
 {
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_tgt_sess *sess;
+	unsigned long flags;
 
 	if (!vha->hw->tgt.tgt_ops)
 		return;
@@ -906,15 +1072,19 @@ qlt_fc_port_deleted(struct scsi_qla_host *vha, fc_port_t *fcport, int max_gen)
 	if (!tgt)
 		return;
 
+	spin_lock_irqsave(&vha->hw->tgt.sess_lock, flags);
 	if (tgt->tgt_stop) {
+		spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 		return;
 	}
 	sess = qlt_find_sess_by_port_name(tgt, fcport->port_name);
 	if (!sess) {
+		spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 		return;
 	}
 
 	if (max_gen - sess->generation < 0) {
+		spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf092,
 		    "Ignoring stale deletion request for se_sess %p / sess %p"
 		    " for port %8phC, req_gen %d, sess_gen %d\n",
@@ -927,6 +1097,7 @@ qlt_fc_port_deleted(struct scsi_qla_host *vha, fc_port_t *fcport, int max_gen)
 
 	sess->local = 1;
 	qlt_schedule_sess_for_deletion(sess, false);
+	spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 }
 
 static inline int test_tgt_sess_count(struct qla_tgt *tgt)
@@ -984,10 +1155,10 @@ int qlt_stop_phase1(struct qla_tgt *tgt)
 	 * Lock is needed, because we still can get an incoming packet.
 	 */
 	mutex_lock(&vha->vha_tgt.tgt_mutex);
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	tgt->tgt_stop = 1;
 	qlt_clear_tgt_db(tgt);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	mutex_unlock(&vha->vha_tgt.tgt_mutex);
 	mutex_unlock(&qla_tgt_mutex);
 
@@ -1040,7 +1211,7 @@ void qlt_stop_phase2(struct qla_tgt *tgt)
 
 	mutex_lock(&vha->vha_tgt.tgt_mutex);
 	spin_lock_irqsave(&ha->hardware_lock, flags);
-	while (tgt->irq_cmd_count != 0) {
+	while ((tgt->irq_cmd_count != 0) || (tgt->atio_irq_cmd_count != 0)) {
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		udelay(2);
 		spin_lock_irqsave(&ha->hardware_lock, flags);
@@ -1309,7 +1480,7 @@ static int abort_cmd_for_tag(struct scsi_qla_host *vha, uint32_t tag)
 
 	list_for_each_entry(cmd, &vha->qla_cmd_list, cmd_list) {
 		if (tag == cmd->atio.u.isp24.exchange_addr) {
-			cmd->state = QLA_TGT_STATE_ABORTED;
+			cmd->aborted = 1;
 			spin_unlock(&vha->cmd_list_lock);
 			return 1;
 		}
@@ -1351,7 +1522,7 @@ static void abort_cmds_for_lun(struct scsi_qla_host *vha,
 		cmd_lun = scsilun_to_int(
 			(struct scsi_lun *)&cmd->atio.u.isp24.fcp_cmnd.lun);
 		if (cmd_key == key && cmd_lun == lun)
-			cmd->state = QLA_TGT_STATE_ABORTED;
+			cmd->aborted = 1;
 	}
 	spin_unlock(&vha->cmd_list_lock);
 }
@@ -1435,6 +1606,7 @@ static void qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 	uint32_t tag = abts->exchange_addr_to_abort;
 	uint8_t s_id[3];
 	int rc;
+	unsigned long flags;
 
 	if (le32_to_cpu(abts->fcp_hdr_le.parameter) & ABTS_PARAM_ABORT_SEQ) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf053,
@@ -1462,6 +1634,7 @@ static void qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 	s_id[1] = abts->fcp_hdr_le.s_id[1];
 	s_id[2] = abts->fcp_hdr_le.s_id[0];
 
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha, s_id);
 	if (!sess) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf012,
@@ -1469,12 +1642,17 @@ static void qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 		    vha->vp_idx);
 		rc = qlt_sched_sess_work(vha->vha_tgt.qla_tgt,
 		    QLA_TGT_SESS_WORK_ABORT, abts, sizeof(*abts));
+
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
+
 		if (rc != 0) {
 			qlt_24xx_send_abts_resp(vha, abts, FCP_TMF_REJECTED,
 			    false);
 		}
 		return;
 	}
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
+
 
 	if (sess->deleted == QLA_SESS_DELETION_IN_PROGRESS) {
 		qlt_24xx_send_abts_resp(vha, abts, FCP_TMF_REJECTED, false);
@@ -1560,15 +1738,15 @@ void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *mcmd)
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
-	if (qla2x00_reset_active(vha) || mcmd->reset_count != ha->chip_reset) {
+	if (!vha->flags.online || mcmd->reset_count != ha->chip_reset) {
 		/*
-		 * Either a chip reset is active or this request was from
+		 * Either the port is not online or this request was from
 		 * previous life, just abort the processing.
 		 */
 		ql_dbg(ql_dbg_async, vha, 0xe100,
-			"RESET-TMR active/old-count/new-count = %d/%d/%d.\n",
-			qla2x00_reset_active(vha), mcmd->reset_count,
-			ha->chip_reset);
+			"RESET-TMR online/active/old-count/new-count = %d/%d/%d/%d.\n",
+			vha->flags.online, qla2x00_reset_active(vha),
+			mcmd->reset_count, ha->chip_reset);
 		ha->tgt.tgt_ops->free_mcmd(mcmd);
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		return;
@@ -1578,7 +1756,7 @@ void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *mcmd)
 		qlt_send_notify_ack(vha, &mcmd->orig_iocb.imm_ntfy,
 		    0, 0, 0, 0, 0, 0);
 	else {
-		if (mcmd->se_cmd.se_tmr_req->function == TMR_ABORT_TASK)
+		if (mcmd->orig_iocb.atio.u.raw.entry_type == ABTS_RECV_24XX)
 			qlt_24xx_send_abts_resp(vha, &mcmd->orig_iocb.abts,
 			    mcmd->fc_tm_rsp, false);
 		else
@@ -2487,7 +2665,7 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 			/* no need to terminate. FW already freed exchange. */
 			qlt_abort_cmd_on_host_reset(cmd->vha, cmd);
 		else
-			qlt_send_term_exchange(vha, cmd, &cmd->atio, 1);
+			qlt_send_term_exchange(vha, cmd, &cmd->atio, 1, 0);
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		return 0;
 	}
@@ -2510,17 +2688,22 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
-	if (qla2x00_reset_active(vha) || cmd->reset_count != ha->chip_reset) {
+	if (xmit_type == QLA_TGT_XMIT_STATUS)
+		vha->tgt_counters.core_qla_snd_status++;
+	else
+		vha->tgt_counters.core_qla_que_buf++;
+
+	if (!vha->flags.online || cmd->reset_count != ha->chip_reset) {
 		/*
-		 * Either a chip reset is active or this request was from
+		 * Either the port is not online or this request was from
 		 * previous life, just abort the processing.
 		 */
 		cmd->state = QLA_TGT_STATE_PROCESSED;
 		qlt_abort_cmd_on_host_reset(cmd->vha, cmd);
 		ql_dbg(ql_dbg_async, vha, 0xe101,
-			"RESET-RSP active/old-count/new-count = %d/%d/%d.\n",
-			qla2x00_reset_active(vha), cmd->reset_count,
-			ha->chip_reset);
+			"RESET-RSP online/active/old-count/new-count = %d/%d/%d/%d.\n",
+			vha->flags.online, qla2x00_reset_active(vha),
+			cmd->reset_count, ha->chip_reset);
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		return 0;
 	}
@@ -2651,18 +2834,18 @@ int qlt_rdy_to_xfer(struct qla_tgt_cmd *cmd)
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
-	if (qla2x00_reset_active(vha) || (cmd->reset_count != ha->chip_reset) ||
+	if (!vha->flags.online || (cmd->reset_count != ha->chip_reset) ||
 	    (cmd->sess && cmd->sess->deleted == QLA_SESS_DELETION_IN_PROGRESS)) {
 		/*
-		 * Either a chip reset is active or this request was from
+		 * Either the port is not online or this request was from
 		 * previous life, just abort the processing.
 		 */
 		cmd->state = QLA_TGT_STATE_NEED_DATA;
 		qlt_abort_cmd_on_host_reset(cmd->vha, cmd);
 		ql_dbg(ql_dbg_async, vha, 0xe102,
-			"RESET-XFR active/old-count/new-count = %d/%d/%d.\n",
-			qla2x00_reset_active(vha), cmd->reset_count,
-			ha->chip_reset);
+			"RESET-XFR online/active/old-count/new-count = %d/%d/%d/%d.\n",
+			vha->flags.online, qla2x00_reset_active(vha),
+			cmd->reset_count, ha->chip_reset);
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		return 0;
 	}
@@ -2957,12 +3140,13 @@ static int __qlt_send_term_exchange(struct scsi_qla_host *vha,
 			ret = 1;
 	}
 
+	vha->tgt_counters.num_term_xchg_sent++;
 	pkt->entry_count = 1;
 	pkt->handle = QLA_TGT_SKIP_HANDLE | CTIO_COMPLETION_HANDLE_MARK;
 
 	ctio24 = (struct ctio7_to_24xx *)pkt;
 	ctio24->entry_type = CTIO_TYPE7;
-	ctio24->nport_handle = cmd ? cmd->loop_id : CTIO7_NHANDLE_UNRECOGNIZED;
+	ctio24->nport_handle = CTIO7_NHANDLE_UNRECOGNIZED;
 	ctio24->timeout = cpu_to_le16(QLA_TGT_TIMEOUT);
 	ctio24->vp_index = vha->vp_idx;
 	ctio24->initiator_id[0] = atio->u.isp24.fcp_hdr.s_id[2];
@@ -2989,7 +3173,8 @@ static int __qlt_send_term_exchange(struct scsi_qla_host *vha,
 }
 
 static void qlt_send_term_exchange(struct scsi_qla_host *vha,
-	struct qla_tgt_cmd *cmd, struct atio_from_isp *atio, int ha_locked)
+	struct qla_tgt_cmd *cmd, struct atio_from_isp *atio, int ha_locked,
+	int ul_abort)
 {
 	unsigned long flags = 0;
 	int rc;
@@ -3009,8 +3194,7 @@ static void qlt_send_term_exchange(struct scsi_qla_host *vha,
 		qlt_alloc_qfull_cmd(vha, atio, 0, 0);
 
 done:
-	if (cmd && ((cmd->state != QLA_TGT_STATE_ABORTED) ||
-	    !cmd->cmd_sent_to_fw)) {
+	if (cmd && !ul_abort && !cmd->aborted) {
 		if (cmd->sg_mapped)
 			qlt_unmap_sg(vha, cmd);
 		vha->hw->tgt.tgt_ops->free_cmd(cmd);
@@ -3028,7 +3212,7 @@ static void qlt_init_term_exchange(struct scsi_qla_host *vha)
 	struct qla_tgt_cmd *cmd, *tcmd;
 
 	vha->hw->tgt.leak_exchg_thresh_hold =
-	    (vha->hw->fw_xcb_count/100) * LEAK_EXCHG_THRESH_HOLD_PERCENT;
+	    (vha->hw->cur_fw_xcb_count/100) * LEAK_EXCHG_THRESH_HOLD_PERCENT;
 
 	cmd = tcmd = NULL;
 	if (!list_empty(&vha->hw->tgt.q_full_list)) {
@@ -3058,7 +3242,7 @@ static void qlt_chk_exch_leak_thresh_hold(struct scsi_qla_host *vha)
 
 		ql_dbg(ql_dbg_tgt, vha, 0xe079,
 		    "Chip reset due to exchange starvation: %d/%d.\n",
-		    total_leaked, vha->hw->fw_xcb_count);
+		    total_leaked, vha->hw->cur_fw_xcb_count);
 
 		if (IS_P3P_TYPE(vha->hw))
 			set_bit(FCOE_CTX_RESET_NEEDED, &vha->dpc_flags);
@@ -3069,21 +3253,38 @@ static void qlt_chk_exch_leak_thresh_hold(struct scsi_qla_host *vha)
 
 }
 
-void qlt_abort_cmd(struct qla_tgt_cmd *cmd)
+int qlt_abort_cmd(struct qla_tgt_cmd *cmd)
 {
 	struct qla_tgt *tgt = cmd->tgt;
 	struct scsi_qla_host *vha = tgt->vha;
 	struct se_cmd *se_cmd = &cmd->se_cmd;
+	unsigned long flags;
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf014,
 	    "qla_target(%d): terminating exchange for aborted cmd=%p "
 	    "(se_cmd=%p, tag=%llu)", vha->vp_idx, cmd, &cmd->se_cmd,
 	    se_cmd->tag);
 
-	cmd->state = QLA_TGT_STATE_ABORTED;
+	spin_lock_irqsave(&cmd->cmd_lock, flags);
+	if (cmd->aborted) {
+		spin_unlock_irqrestore(&cmd->cmd_lock, flags);
+		/*
+		 * It's normal to see 2 calls in this path:
+		 *  1) XFER Rdy completion + CMD_T_ABORT
+		 *  2) TCM TMR - drain_state_list
+		 */
+	        ql_dbg(ql_dbg_tgt_mgt, vha, 0xffff,
+			"multiple abort. %p transport_state %x, t_state %x,"
+			" se_cmd_flags %x \n", cmd, cmd->se_cmd.transport_state,
+			cmd->se_cmd.t_state,cmd->se_cmd.se_cmd_flags);
+		return EIO;
+	}
+	cmd->aborted = 1;
 	cmd->cmd_flags |= BIT_6;
+	spin_unlock_irqrestore(&cmd->cmd_lock, flags);
 
-	qlt_send_term_exchange(vha, cmd, &cmd->atio, 0);
+	qlt_send_term_exchange(vha, cmd, &cmd->atio, 0, 1);
+	return 0;
 }
 EXPORT_SYMBOL(qlt_abort_cmd);
 
@@ -3098,6 +3299,9 @@ void qlt_free_cmd(struct qla_tgt_cmd *cmd)
 
 	BUG_ON(cmd->cmd_in_wq);
 
+	if (cmd->sg_mapped)
+		qlt_unmap_sg(cmd->vha, cmd);
+
 	if (!cmd->q_full)
 		qlt_decr_num_pend_cmds(cmd->vha);
 
@@ -3215,7 +3419,7 @@ static int qlt_term_ctio_exchange(struct scsi_qla_host *vha, void *ctio,
 		term = 1;
 
 	if (term)
-		qlt_send_term_exchange(vha, cmd, &cmd->atio, 1);
+		qlt_send_term_exchange(vha, cmd, &cmd->atio, 1, 0);
 
 	return term;
 }
@@ -3300,9 +3504,6 @@ qlt_abort_cmd_on_host_reset(struct scsi_qla_host *vha, struct qla_tgt_cmd *cmd)
 
 		ha->tgt.tgt_ops->handle_data(cmd);
 		return;
-	} else if (cmd->state == QLA_TGT_STATE_ABORTED) {
-		ql_dbg(ql_dbg_io, vha, 0xff02,
-		    "HOST-ABORT: handle=%d, state=ABORTED.\n", handle);
 	} else {
 		ql_dbg(ql_dbg_io, vha, 0xff03,
 		    "HOST-ABORT: handle=%d, state=BAD(%d).\n", handle,
@@ -3398,13 +3599,27 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 
 		case CTIO_PORT_LOGGED_OUT:
 		case CTIO_PORT_UNAVAILABLE:
+		{
+			int logged_out =
+				(status & 0xFFFF) == CTIO_PORT_LOGGED_OUT;
+
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf059,
-			    "qla_target(%d): CTIO with PORT LOGGED "
-			    "OUT (29) or PORT UNAVAILABLE (28) status %x "
+			    "qla_target(%d): CTIO with %s status %x "
 			    "received (state %x, se_cmd %p)\n", vha->vp_idx,
+			    logged_out ? "PORT LOGGED OUT" : "PORT UNAVAILABLE",
 			    status, cmd->state, se_cmd);
-			break;
 
+			if (logged_out && cmd->sess) {
+				/*
+				 * Session is already logged out, but we need
+				 * to notify initiator, who's not aware of this
+				 */
+				cmd->sess->logout_on_delete = 0;
+				cmd->sess->send_els_logo = 1;
+				qlt_schedule_sess_for_deletion(cmd->sess, true);
+			}
+			break;
+		}
 		case CTIO_SRR_RECEIVED:
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf05a,
 			    "qla_target(%d): CTIO with SRR_RECEIVED"
@@ -3454,14 +3669,14 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 		}
 
 
-		/* "cmd->state == QLA_TGT_STATE_ABORTED" means
+		/* "cmd->aborted" means
 		 * cmd is already aborted/terminated, we don't
 		 * need to terminate again.  The exchange is already
 		 * cleaned up/freed at FW level.  Just cleanup at driver
 		 * level.
 		 */
 		if ((cmd->state != QLA_TGT_STATE_NEED_DATA) &&
-		    (cmd->state != QLA_TGT_STATE_ABORTED)) {
+		    (!cmd->aborted)) {
 			cmd->cmd_flags |= BIT_13;
 			if (qlt_term_ctio_exchange(vha, ctio, cmd, status))
 				return;
@@ -3479,7 +3694,7 @@ skip_term:
 
 		ha->tgt.tgt_ops->handle_data(cmd);
 		return;
-	} else if (cmd->state == QLA_TGT_STATE_ABORTED) {
+	} else if (cmd->aborted) {
 		cmd->cmd_flags |= BIT_18;
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf01e,
 		  "Aborted command %p (tag %lld) finished\n", cmd, se_cmd->tag);
@@ -3491,7 +3706,7 @@ skip_term:
 	}
 
 	if (unlikely(status != CTIO_SUCCESS) &&
-		(cmd->state != QLA_TGT_STATE_ABORTED)) {
+		!cmd->aborted) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf01f, "Finishing failed CTIO\n");
 		dump_stack();
 	}
@@ -3553,13 +3768,14 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 	if (tgt->tgt_stop)
 		goto out_term;
 
-	if (cmd->state == QLA_TGT_STATE_ABORTED) {
+	if (cmd->aborted) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf082,
 		    "cmd with tag %u is aborted\n",
 		    cmd->atio.u.isp24.exchange_addr);
 		goto out_term;
 	}
 
+	spin_lock_init(&cmd->cmd_lock);
 	cdb = &atio->u.isp24.fcp_cmnd.cdb[0];
 	cmd->se_cmd.tag = atio->u.isp24.exchange_addr;
 	cmd->unpacked_lun = scsilun_to_int(
@@ -3589,9 +3805,9 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 	/*
 	 * Drop extra session reference from qla_tgt_handle_cmd_for_atio*(
 	 */
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	return;
 
 out_term:
@@ -3602,12 +3818,15 @@ out_term:
 	 */
 	cmd->cmd_flags |= BIT_2;
 	spin_lock_irqsave(&ha->hardware_lock, flags);
-	qlt_send_term_exchange(vha, NULL, &cmd->atio, 1);
+	qlt_send_term_exchange(vha, NULL, &cmd->atio, 1, 0);
 
 	qlt_decr_num_pend_cmds(vha);
 	percpu_ida_free(&sess->se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
-	ha->tgt.tgt_ops->put_sess(sess);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
+	ha->tgt.tgt_ops->put_sess(sess);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 }
 
 static void qlt_do_work(struct work_struct *work)
@@ -3692,10 +3911,8 @@ static void qlt_create_sess_from_atio(struct work_struct *work)
 		goto out_term;
 	}
 
-	mutex_lock(&vha->vha_tgt.tgt_mutex);
 	sess = qlt_make_local_sess(vha, s_id);
 	/* sess has an extra creation ref. */
-	mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
 	if (!sess)
 		goto out_term;
@@ -3723,7 +3940,7 @@ static void qlt_create_sess_from_atio(struct work_struct *work)
 
 out_term:
 	spin_lock_irqsave(&ha->hardware_lock, flags);
-	qlt_send_term_exchange(vha, NULL, &op->atio, 1);
+	qlt_send_term_exchange(vha, NULL, &op->atio, 1, 0);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	kfree(op);
 
@@ -3787,13 +4004,24 @@ static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 
 	cmd->cmd_in_wq = 1;
 	cmd->cmd_flags |= BIT_0;
+	cmd->se_cmd.cpuid = ha->msix_count ?
+		ha->tgt.rspq_vector_cpuid : WORK_CPU_UNBOUND;
 
 	spin_lock(&vha->cmd_list_lock);
 	list_add_tail(&cmd->cmd_list, &vha->qla_cmd_list);
 	spin_unlock(&vha->cmd_list_lock);
 
 	INIT_WORK(&cmd->work, qlt_do_work);
-	queue_work(qla_tgt_wq, &cmd->work);
+	if (ha->msix_count) {
+		if (cmd->atio.u.isp24.fcp_cmnd.rddata)
+			queue_work_on(smp_processor_id(), qla_tgt_wq,
+			    &cmd->work);
+		else
+			queue_work_on(cmd->se_cmd.cpuid, qla_tgt_wq,
+			    &cmd->work);
+	} else {
+		queue_work(qla_tgt_wq, &cmd->work);
+	}
 	return 0;
 
 }
@@ -3917,13 +4145,18 @@ static int qlt_handle_task_mgmt(struct scsi_qla_host *vha, void *iocb)
 	struct qla_tgt_sess *sess;
 	uint32_t lun, unpacked_lun;
 	int fn;
+	unsigned long flags;
 
 	tgt = vha->vha_tgt.qla_tgt;
 
 	lun = a->u.isp24.fcp_cmnd.lun;
 	fn = a->u.isp24.fcp_cmnd.task_mgmt_flags;
+
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha,
 	    a->u.isp24.fcp_hdr.s_id);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
+
 	unpacked_lun = scsilun_to_int((struct scsi_lun *)&lun);
 
 	if (!sess) {
@@ -3987,10 +4220,14 @@ static int qlt_abort_task(struct scsi_qla_host *vha,
 	struct qla_hw_data *ha = vha->hw;
 	struct qla_tgt_sess *sess;
 	int loop_id;
+	unsigned long flags;
 
 	loop_id = GET_TARGET_ID(ha, (struct atio_from_isp *)iocb);
 
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	sess = ha->tgt.tgt_ops->find_sess_by_loop_id(vha, loop_id);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
+
 	if (sess == NULL) {
 		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf025,
 		    "qla_target(%d): task abort for unexisting "
@@ -4022,15 +4259,6 @@ void qlt_logo_completion_handler(fc_port_t *fcport, int rc)
 	}
 }
 
-static void qlt_swap_imm_ntfy_iocb(struct imm_ntfy_from_isp *a,
-    struct imm_ntfy_from_isp *b)
-{
-	struct imm_ntfy_from_isp tmp;
-	memcpy(&tmp, a, sizeof(struct imm_ntfy_from_isp));
-	memcpy(a, b, sizeof(struct imm_ntfy_from_isp));
-	memcpy(b, &tmp, sizeof(struct imm_ntfy_from_isp));
-}
-
 /*
 * ha->hardware_lock supposed to be held on entry (to protect tgt->sess_list)
 *
@@ -4040,11 +4268,13 @@ static void qlt_swap_imm_ntfy_iocb(struct imm_ntfy_from_isp *a,
 */
 static struct qla_tgt_sess *
 qlt_find_sess_invalidate_other(struct qla_tgt *tgt, uint64_t wwn,
-    port_id_t port_id, uint16_t loop_id)
+    port_id_t port_id, uint16_t loop_id, struct qla_tgt_sess **conflict_sess)
 {
 	struct qla_tgt_sess *sess = NULL, *other_sess;
 	uint64_t other_wwn;
 
+	*conflict_sess = NULL;
+
 	list_for_each_entry(other_sess, &tgt->sess_list, sess_list_entry) {
 
 		other_wwn = wwn_to_u64(other_sess->port_name);
@@ -4072,9 +4302,10 @@ qlt_find_sess_invalidate_other(struct qla_tgt *tgt, uint64_t wwn,
 			} else {
 				/*
 				 * Another wwn used to have our s_id/loop_id
-				 * combo - kill the session, but don't log out
+				 * kill the session, but don't free the loop_id
 				 */
-				sess->logout_on_delete = 0;
+				other_sess->keep_nport_handle = 1;
+				*conflict_sess = other_sess;
 				qlt_schedule_sess_for_deletion(other_sess,
 				    true);
 			}
@@ -4119,7 +4350,7 @@ static int abort_cmds_for_s_id(struct scsi_qla_host *vha, port_id_t *s_id)
 	list_for_each_entry(cmd, &vha->qla_cmd_list, cmd_list) {
 		uint32_t cmd_key = sid_to_key(cmd->atio.u.isp24.fcp_hdr.s_id);
 		if (cmd_key == key) {
-			cmd->state = QLA_TGT_STATE_ABORTED;
+			cmd->aborted = 1;
 			count++;
 		}
 	}
@@ -4136,12 +4367,14 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 {
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	struct qla_hw_data *ha = vha->hw;
-	struct qla_tgt_sess *sess = NULL;
+	struct qla_tgt_sess *sess = NULL, *conflict_sess = NULL;
 	uint64_t wwn;
 	port_id_t port_id;
 	uint16_t loop_id;
 	uint16_t wd3_lo;
 	int res = 0;
+	qlt_plogi_ack_t *pla;
+	unsigned long flags;
 
 	wwn = wwn_to_u64(iocb->u.isp24.port_name);
 
@@ -4165,27 +4398,20 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 		/* Mark all stale commands in qla_tgt_wq for deletion */
 		abort_cmds_for_s_id(vha, &port_id);
 
-		if (wwn)
+		if (wwn) {
+			spin_lock_irqsave(&tgt->ha->tgt.sess_lock, flags);
 			sess = qlt_find_sess_invalidate_other(tgt, wwn,
-			    port_id, loop_id);
+			    port_id, loop_id, &conflict_sess);
+			spin_unlock_irqrestore(&tgt->ha->tgt.sess_lock, flags);
+		}
 
-		if (!sess || IS_SW_RESV_ADDR(sess->s_id)) {
+		if (IS_SW_RESV_ADDR(port_id) || (!sess && !conflict_sess)) {
 			res = 1;
 			break;
 		}
 
-		if (sess->plogi_ack_needed) {
-			/*
-			 * Initiator sent another PLOGI before last PLOGI could
-			 * finish. Swap plogi iocbs and terminate old one
-			 * without acking, new one will get acked when session
-			 * deletion completes.
-			 */
-			ql_log(ql_log_warn, sess->vha, 0xf094,
-			    "sess %p received double plogi.\n", sess);
-
-			qlt_swap_imm_ntfy_iocb(iocb, &sess->tm_iocb);
-
+		pla = qlt_plogi_ack_find_add(vha, &port_id, iocb);
+		if (!pla) {
 			qlt_send_term_imm_notif(vha, iocb, 1);
 
 			res = 0;
@@ -4194,13 +4420,14 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 
 		res = 0;
 
-		/*
-		 * Save immediate Notif IOCB for Ack when sess is done
-		 * and being deleted.
-		 */
-		memcpy(&sess->tm_iocb, iocb, sizeof(sess->tm_iocb));
-		sess->plogi_ack_needed  = 1;
+		if (conflict_sess)
+			qlt_plogi_ack_link(vha, pla, conflict_sess,
+			    QLT_PLOGI_LINK_CONFLICT);
+
+		if (!sess)
+			break;
 
+		qlt_plogi_ack_link(vha, pla, sess, QLT_PLOGI_LINK_SAME_WWN);
 		 /*
 		  * Under normal circumstances we want to release nport handle
 		  * during LOGO process to avoid nport handle leaks inside FW.
@@ -4227,9 +4454,21 @@ static int qlt_24xx_handle_els(struct scsi_qla_host *vha,
 	case ELS_PRLI:
 		wd3_lo = le16_to_cpu(iocb->u.isp24.u.prli.wd3_lo);
 
-		if (wwn)
+		if (wwn) {
+			spin_lock_irqsave(&tgt->ha->tgt.sess_lock, flags);
 			sess = qlt_find_sess_invalidate_other(tgt, wwn, port_id,
-			    loop_id);
+			    loop_id, &conflict_sess);
+			spin_unlock_irqrestore(&tgt->ha->tgt.sess_lock, flags);
+		}
+
+		if (conflict_sess) {
+			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf09b,
+			    "PRLI with conflicting sess %p port %8phC\n",
+			    conflict_sess, conflict_sess->port_name);
+			qlt_send_term_imm_notif(vha, iocb, 1);
+			res = 0;
+			break;
+		}
 
 		if (sess != NULL) {
 			if (sess->deleted) {
@@ -4554,7 +4793,7 @@ out_reject:
 		dump_stack();
 	} else {
 		cmd->cmd_flags |= BIT_9;
-		qlt_send_term_exchange(vha, cmd, &cmd->atio, 1);
+		qlt_send_term_exchange(vha, cmd, &cmd->atio, 1, 0);
 	}
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 }
@@ -4733,7 +4972,7 @@ static void qlt_prepare_srr_imm(struct scsi_qla_host *vha,
 				    sctio, sctio->srr_id);
 				list_del(&sctio->srr_list_entry);
 				qlt_send_term_exchange(vha, sctio->cmd,
-				    &sctio->cmd->atio, 1);
+				    &sctio->cmd->atio, 1, 0);
 				kfree(sctio);
 			}
 		}
@@ -4899,11 +5138,14 @@ static int __qlt_send_busy(struct scsi_qla_host *vha,
 	struct qla_hw_data *ha = vha->hw;
 	request_t *pkt;
 	struct qla_tgt_sess *sess = NULL;
+	unsigned long flags;
 
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha,
 	    atio->u.isp24.fcp_hdr.s_id);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	if (!sess) {
-		qlt_send_term_exchange(vha, NULL, atio, 1);
+		qlt_send_term_exchange(vha, NULL, atio, 1, 0);
 		return 0;
 	}
 	/* Sending marker isn't necessary, since we called from ISR */
@@ -4916,6 +5158,7 @@ static int __qlt_send_busy(struct scsi_qla_host *vha,
 		return -ENOMEM;
 	}
 
+	vha->tgt_counters.num_q_full_sent++;
 	pkt->entry_count = 1;
 	pkt->handle = QLA_TGT_SKIP_HANDLE | CTIO_COMPLETION_HANDLE_MARK;
 
@@ -5129,11 +5372,12 @@ qlt_chk_qfull_thresh_hold(struct scsi_qla_host *vha,
 /* ha->hardware_lock supposed to be held on entry */
 /* called via callback from qla2xxx */
 static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
-	struct atio_from_isp *atio)
+	struct atio_from_isp *atio, uint8_t ha_locked)
 {
 	struct qla_hw_data *ha = vha->hw;
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	int rc;
+	unsigned long flags;
 
 	if (unlikely(tgt == NULL)) {
 		ql_dbg(ql_dbg_io, vha, 0x3064,
@@ -5145,7 +5389,7 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 	 * Otherwise, some commands can stuck.
 	 */
 
-	tgt->irq_cmd_count++;
+	tgt->atio_irq_cmd_count++;
 
 	switch (atio->u.raw.entry_type) {
 	case ATIO_TYPE7:
@@ -5155,7 +5399,11 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 			    "qla_target(%d): ATIO_TYPE7 "
 			    "received with UNKNOWN exchange address, "
 			    "sending QUEUE_FULL\n", vha->vp_idx);
+			if (!ha_locked)
+				spin_lock_irqsave(&ha->hardware_lock, flags);
 			qlt_send_busy(vha, atio, SAM_STAT_TASK_SET_FULL);
+			if (!ha_locked)
+				spin_unlock_irqrestore(&ha->hardware_lock, flags);
 			break;
 		}
 
@@ -5164,7 +5412,7 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 		if (likely(atio->u.isp24.fcp_cmnd.task_mgmt_flags == 0)) {
 			rc = qlt_chk_qfull_thresh_hold(vha, atio);
 			if (rc != 0) {
-				tgt->irq_cmd_count--;
+				tgt->atio_irq_cmd_count--;
 				return;
 			}
 			rc = qlt_handle_cmd_for_atio(vha, atio);
@@ -5173,11 +5421,20 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 		}
 		if (unlikely(rc != 0)) {
 			if (rc == -ESRCH) {
+				if (!ha_locked)
+					spin_lock_irqsave
+						(&ha->hardware_lock, flags);
+
 #if 1 /* With TERM EXCHANGE some FC cards refuse to boot */
 				qlt_send_busy(vha, atio, SAM_STAT_BUSY);
 #else
-				qlt_send_term_exchange(vha, NULL, atio, 1);
+				qlt_send_term_exchange(vha, NULL, atio, 1, 0);
 #endif
+
+				if (!ha_locked)
+					spin_unlock_irqrestore
+						(&ha->hardware_lock, flags);
+
 			} else {
 				if (tgt->tgt_stop) {
 					ql_dbg(ql_dbg_tgt, vha, 0xe059,
@@ -5189,7 +5446,13 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 					    "qla_target(%d): Unable to send "
 					    "command to target, sending BUSY "
 					    "status.\n", vha->vp_idx);
+					if (!ha_locked)
+						spin_lock_irqsave(
+						    &ha->hardware_lock, flags);
 					qlt_send_busy(vha, atio, SAM_STAT_BUSY);
+					if (!ha_locked)
+						spin_unlock_irqrestore(
+						    &ha->hardware_lock, flags);
 				}
 			}
 		}
@@ -5206,7 +5469,12 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 			break;
 		}
 		ql_dbg(ql_dbg_tgt, vha, 0xe02e, "%s", "IMMED_NOTIFY ATIO");
+
+		if (!ha_locked)
+			spin_lock_irqsave(&ha->hardware_lock, flags);
 		qlt_handle_imm_notify(vha, (struct imm_ntfy_from_isp *)atio);
+		if (!ha_locked)
+			spin_unlock_irqrestore(&ha->hardware_lock, flags);
 		break;
 	}
 
@@ -5217,7 +5485,7 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 		break;
 	}
 
-	tgt->irq_cmd_count--;
+	tgt->atio_irq_cmd_count--;
 }
 
 /* ha->hardware_lock supposed to be held on entry */
@@ -5277,7 +5545,7 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 #if 1 /* With TERM EXCHANGE some FC cards refuse to boot */
 				qlt_send_busy(vha, atio, 0);
 #else
-				qlt_send_term_exchange(vha, NULL, atio, 1);
+				qlt_send_term_exchange(vha, NULL, atio, 1, 0);
 #endif
 			} else {
 				if (tgt->tgt_stop) {
@@ -5286,7 +5554,7 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 					    "command to target, sending TERM "
 					    "EXCHANGE for rsp\n");
 					qlt_send_term_exchange(vha, NULL,
-					    atio, 1);
+					    atio, 1, 0);
 				} else {
 					ql_dbg(ql_dbg_tgt, vha, 0xe060,
 					    "qla_target(%d): Unable to send "
@@ -5534,12 +5802,16 @@ static struct qla_tgt_sess *qlt_make_local_sess(struct scsi_qla_host *vha,
 	int rc, global_resets;
 	uint16_t loop_id = 0;
 
+	mutex_lock(&vha->vha_tgt.tgt_mutex);
+
 retry:
 	global_resets =
 	    atomic_read(&vha->vha_tgt.qla_tgt->tgt_global_resets_count);
 
 	rc = qla24xx_get_loop_id(vha, s_id, &loop_id);
 	if (rc != 0) {
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
+
 		if ((s_id[0] == 0xFF) &&
 		    (s_id[1] == 0xFC)) {
 			/*
@@ -5550,17 +5822,27 @@ retry:
 			    "Unable to find initiator with S_ID %x:%x:%x",
 			    s_id[0], s_id[1], s_id[2]);
 		} else
-			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf071,
+			ql_log(ql_log_info, vha, 0xf071,
 			    "qla_target(%d): Unable to find "
 			    "initiator with S_ID %x:%x:%x",
 			    vha->vp_idx, s_id[0], s_id[1],
 			    s_id[2]);
+
+		if (rc == -ENOENT) {
+			qlt_port_logo_t logo;
+			sid_to_portid(s_id, &logo.id);
+			logo.cmd_count = 1;
+			qlt_send_first_logo(vha, &logo);
+		}
+
 		return NULL;
 	}
 
 	fcport = qlt_get_port_database(vha, loop_id);
-	if (!fcport)
+	if (!fcport) {
+		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 		return NULL;
+	}
 
 	if (global_resets !=
 	    atomic_read(&vha->vha_tgt.qla_tgt->tgt_global_resets_count)) {
@@ -5575,6 +5857,8 @@ retry:
 
 	sess = qlt_create_sess(vha, fcport, true);
 
+	mutex_unlock(&vha->vha_tgt.tgt_mutex);
+
 	kfree(fcport);
 	return sess;
 }
@@ -5585,15 +5869,15 @@ static void qlt_abort_work(struct qla_tgt *tgt,
 	struct scsi_qla_host *vha = tgt->vha;
 	struct qla_hw_data *ha = vha->hw;
 	struct qla_tgt_sess *sess = NULL;
-	unsigned long flags;
+	unsigned long flags = 0, flags2 = 0;
 	uint32_t be_s_id;
 	uint8_t s_id[3];
 	int rc;
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags2);
 
 	if (tgt->tgt_stop)
-		goto out_term;
+		goto out_term2;
 
 	s_id[0] = prm->abts.fcp_hdr_le.s_id[2];
 	s_id[1] = prm->abts.fcp_hdr_le.s_id[1];
@@ -5602,41 +5886,47 @@ static void qlt_abort_work(struct qla_tgt *tgt,
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha,
 	    (unsigned char *)&be_s_id);
 	if (!sess) {
-		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags2);
 
-		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_make_local_sess(vha, s_id);
 		/* sess has got an extra creation ref */
-		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
-		spin_lock_irqsave(&ha->hardware_lock, flags);
+		spin_lock_irqsave(&ha->tgt.sess_lock, flags2);
 		if (!sess)
-			goto out_term;
+			goto out_term2;
 	} else {
 		if (sess->deleted == QLA_SESS_DELETION_IN_PROGRESS) {
 			sess = NULL;
-			goto out_term;
+			goto out_term2;
 		}
 
 		kref_get(&sess->se_sess->sess_kref);
 	}
 
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
 	if (tgt->tgt_stop)
 		goto out_term;
 
 	rc = __qlt_24xx_handle_abts(vha, &prm->abts, sess);
 	if (rc != 0)
 		goto out_term;
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags2);
 	return;
 
+out_term2:
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+
 out_term:
 	qlt_24xx_send_abts_resp(vha, &prm->abts, FCP_TMF_REJECTED, false);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+
 	if (sess)
 		ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags2);
 }
 
 static void qlt_tmr_work(struct qla_tgt *tgt,
@@ -5653,7 +5943,7 @@ static void qlt_tmr_work(struct qla_tgt *tgt,
 	int fn;
 	void *iocb;
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 
 	if (tgt->tgt_stop)
 		goto out_term;
@@ -5661,14 +5951,12 @@ static void qlt_tmr_work(struct qla_tgt *tgt,
 	s_id = prm->tm_iocb2.u.isp24.fcp_hdr.s_id;
 	sess = ha->tgt.tgt_ops->find_sess_by_s_id(vha, s_id);
 	if (!sess) {
-		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 
-		mutex_lock(&vha->vha_tgt.tgt_mutex);
 		sess = qlt_make_local_sess(vha, s_id);
 		/* sess has got an extra creation ref */
-		mutex_unlock(&vha->vha_tgt.tgt_mutex);
 
-		spin_lock_irqsave(&ha->hardware_lock, flags);
+		spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 		if (!sess)
 			goto out_term;
 	} else {
@@ -5690,14 +5978,14 @@ static void qlt_tmr_work(struct qla_tgt *tgt,
 		goto out_term;
 
 	ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	return;
 
 out_term:
-	qlt_send_term_exchange(vha, NULL, &prm->tm_iocb2, 1);
+	qlt_send_term_exchange(vha, NULL, &prm->tm_iocb2, 1, 0);
 	if (sess)
 		ha->tgt.tgt_ops->put_sess(sess);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 }
 
 static void qlt_sess_work_fn(struct work_struct *work)
@@ -6002,6 +6290,7 @@ qlt_enable_vha(struct scsi_qla_host *vha)
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	unsigned long flags;
 	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
+	int rspq_ent = QLA83XX_RSPQ_MSIX_ENTRY_NUMBER;
 
 	if (!tgt) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe069,
@@ -6020,6 +6309,17 @@ qlt_enable_vha(struct scsi_qla_host *vha)
 		qla24xx_disable_vp(vha);
 		qla24xx_enable_vp(vha);
 	} else {
+		if (ha->msix_entries) {
+			ql_dbg(ql_dbg_tgt, vha, 0xffff,
+			    "%s: host%ld : vector %d cpu %d\n",
+			    __func__, vha->host_no,
+			    ha->msix_entries[rspq_ent].vector,
+			    ha->msix_entries[rspq_ent].cpuid);
+
+			ha->tgt.rspq_vector_cpuid =
+			    ha->msix_entries[rspq_ent].cpuid;
+		}
+
 		set_bit(ISP_ABORT_NEEDED, &base_vha->dpc_flags);
 		qla2xxx_wake_dpc(base_vha);
 		qla2x00_wait_for_hba_online(base_vha);
@@ -6131,7 +6431,7 @@ qlt_init_atio_q_entries(struct scsi_qla_host *vha)
  * @ha: SCSI driver HA context
  */
 void
-qlt_24xx_process_atio_queue(struct scsi_qla_host *vha)
+qlt_24xx_process_atio_queue(struct scsi_qla_host *vha, uint8_t ha_locked)
 {
 	struct qla_hw_data *ha = vha->hw;
 	struct atio_from_isp *pkt;
@@ -6144,7 +6444,8 @@ qlt_24xx_process_atio_queue(struct scsi_qla_host *vha)
 		pkt = (struct atio_from_isp *)ha->tgt.atio_ring_ptr;
 		cnt = pkt->u.raw.entry_count;
 
-		qlt_24xx_atio_pkt_all_vps(vha, (struct atio_from_isp *)pkt);
+		qlt_24xx_atio_pkt_all_vps(vha, (struct atio_from_isp *)pkt,
+		    ha_locked);
 
 		for (i = 0; i < cnt; i++) {
 			ha->tgt.atio_ring_index++;
@@ -6265,10 +6566,21 @@ qlt_24xx_config_nvram_stage2(struct scsi_qla_host *vha,
 {
 	struct qla_hw_data *ha = vha->hw;
 
+	if (!QLA_TGT_MODE_ENABLED())
+		return;
+
 	if (ha->tgt.node_name_set) {
 		memcpy(icb->node_name, ha->tgt.tgt_node_name, WWN_SIZE);
 		icb->firmware_options_1 |= cpu_to_le32(BIT_14);
 	}
+
+	/* disable ZIO at start time. */
+	if (!vha->flags.init_done) {
+		uint32_t tmp;
+		tmp = le32_to_cpu(icb->firmware_options_2);
+		tmp &= ~(BIT_3 | BIT_2 | BIT_1 | BIT_0);
+		icb->firmware_options_2 = cpu_to_le32(tmp);
+	}
 }
 
 void
@@ -6359,6 +6671,15 @@ qlt_81xx_config_nvram_stage2(struct scsi_qla_host *vha,
 		memcpy(icb->node_name, ha->tgt.tgt_node_name, WWN_SIZE);
 		icb->firmware_options_1 |= cpu_to_le32(BIT_14);
 	}
+
+	/* disable ZIO at start time. */
+	if (!vha->flags.init_done) {
+		uint32_t tmp;
+		tmp = le32_to_cpu(icb->firmware_options_2);
+		tmp &= ~(BIT_3 | BIT_2 | BIT_1 | BIT_0);
+		icb->firmware_options_2 = cpu_to_le32(tmp);
+	}
+
 }
 
 void
@@ -6428,16 +6749,59 @@ qla83xx_msix_atio_q(int irq, void *dev_id)
 	ha = rsp->hw;
 	vha = pci_get_drvdata(ha->pdev);
 
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.atio_lock, flags);
 
-	qlt_24xx_process_atio_queue(vha);
-	qla24xx_process_response_queue(vha, rsp);
+	qlt_24xx_process_atio_queue(vha, 0);
 
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.atio_lock, flags);
 
 	return IRQ_HANDLED;
 }
 
+static void
+qlt_handle_abts_recv_work(struct work_struct *work)
+{
+	struct qla_tgt_sess_op *op = container_of(work,
+		struct qla_tgt_sess_op, work);
+	scsi_qla_host_t *vha = op->vha;
+	struct qla_hw_data *ha = vha->hw;
+	unsigned long flags;
+
+	if (qla2x00_reset_active(vha) || (op->chip_reset != ha->chip_reset))
+		return;
+
+	spin_lock_irqsave(&ha->tgt.atio_lock, flags);
+	qlt_24xx_process_atio_queue(vha, 0);
+	spin_unlock_irqrestore(&ha->tgt.atio_lock, flags);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	qlt_response_pkt_all_vps(vha, (response_t *)&op->atio);
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+void
+qlt_handle_abts_recv(struct scsi_qla_host *vha, response_t *pkt)
+{
+	struct qla_tgt_sess_op *op;
+
+	op = kzalloc(sizeof(*op), GFP_ATOMIC);
+
+	if (!op) {
+		/* do not reach for ATIO queue here.  This is best effort err
+		 * recovery at this point.
+		 */
+		qlt_response_pkt_all_vps(vha, pkt);
+		return;
+	}
+
+	memcpy(&op->atio, pkt, sizeof(*pkt));
+	op->vha = vha;
+	op->chip_reset = vha->hw->chip_reset;
+	INIT_WORK(&op->work, qlt_handle_abts_recv_work);
+	queue_work(qla_tgt_wq, &op->work);
+	return;
+}
+
 int
 qlt_mem_alloc(struct qla_hw_data *ha)
 {
@@ -6532,13 +6896,25 @@ int __init qlt_init(void)
 		return -ENOMEM;
 	}
 
+	qla_tgt_plogi_cachep = kmem_cache_create("qla_tgt_plogi_cachep",
+						 sizeof(qlt_plogi_ack_t),
+						 __alignof__(qlt_plogi_ack_t),
+						 0, NULL);
+
+	if (!qla_tgt_plogi_cachep) {
+		ql_log(ql_log_fatal, NULL, 0xe06d,
+		    "kmem_cache_create for qla_tgt_plogi_cachep failed\n");
+		ret = -ENOMEM;
+		goto out_mgmt_cmd_cachep;
+	}
+
 	qla_tgt_mgmt_cmd_mempool = mempool_create(25, mempool_alloc_slab,
 	    mempool_free_slab, qla_tgt_mgmt_cmd_cachep);
 	if (!qla_tgt_mgmt_cmd_mempool) {
 		ql_log(ql_log_fatal, NULL, 0xe06e,
 		    "mempool_create for qla_tgt_mgmt_cmd_mempool failed\n");
 		ret = -ENOMEM;
-		goto out_mgmt_cmd_cachep;
+		goto out_plogi_cachep;
 	}
 
 	qla_tgt_wq = alloc_workqueue("qla_tgt_wq", 0, 0);
@@ -6555,6 +6931,8 @@ int __init qlt_init(void)
 
 out_cmd_mempool:
 	mempool_destroy(qla_tgt_mgmt_cmd_mempool);
+out_plogi_cachep:
+	kmem_cache_destroy(qla_tgt_plogi_cachep);
 out_mgmt_cmd_cachep:
 	kmem_cache_destroy(qla_tgt_mgmt_cmd_cachep);
 	return ret;
@@ -6567,5 +6945,6 @@ void qlt_exit(void)
 
 	destroy_workqueue(qla_tgt_wq);
 	mempool_destroy(qla_tgt_mgmt_cmd_mempool);
+	kmem_cache_destroy(qla_tgt_plogi_cachep);
 	kmem_cache_destroy(qla_tgt_mgmt_cmd_cachep);
 }
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index bca584ae45b7..22a6a767fe07 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -787,7 +787,7 @@ int qla2x00_wait_for_hba_online(struct scsi_qla_host *);
 #define QLA_TGT_STATE_NEED_DATA		1 /* target needs data to continue */
 #define QLA_TGT_STATE_DATA_IN		2 /* Data arrived + target processing */
 #define QLA_TGT_STATE_PROCESSED		3 /* target done processing */
-#define QLA_TGT_STATE_ABORTED		4 /* Command aborted */
+
 
 /* Special handles */
 #define QLA_TGT_NULL_HANDLE	0
@@ -835,6 +835,7 @@ struct qla_tgt {
 	 * HW lock.
 	 */
 	int irq_cmd_count;
+	int atio_irq_cmd_count;
 
 	int datasegs_per_cmd, datasegs_per_cont, sg_tablesize;
 
@@ -883,6 +884,7 @@ struct qla_tgt {
 
 struct qla_tgt_sess_op {
 	struct scsi_qla_host *vha;
+	uint32_t chip_reset;
 	struct atio_from_isp atio;
 	struct work_struct work;
 	struct list_head cmd_list;
@@ -896,6 +898,19 @@ enum qla_sess_deletion {
 	QLA_SESS_DELETION_IN_PROGRESS	= 2,
 };
 
+typedef enum {
+	QLT_PLOGI_LINK_SAME_WWN,
+	QLT_PLOGI_LINK_CONFLICT,
+	QLT_PLOGI_LINK_MAX
+} qlt_plogi_link_t;
+
+typedef struct {
+	struct list_head		list;
+	struct imm_ntfy_from_isp	iocb;
+	port_id_t			id;
+	int				ref_count;
+} qlt_plogi_ack_t;
+
 /*
  * Equivilant to IT Nexus (Initiator-Target)
  */
@@ -907,8 +922,8 @@ struct qla_tgt_sess {
 	unsigned int deleted:2;
 	unsigned int local:1;
 	unsigned int logout_on_delete:1;
-	unsigned int plogi_ack_needed:1;
 	unsigned int keep_nport_handle:1;
+	unsigned int send_els_logo:1;
 
 	unsigned char logout_completed;
 
@@ -925,11 +940,39 @@ struct qla_tgt_sess {
 	uint8_t port_name[WWN_SIZE];
 	struct work_struct free_work;
 
-	union {
-		struct imm_ntfy_from_isp tm_iocb;
-	};
+	qlt_plogi_ack_t *plogi_link[QLT_PLOGI_LINK_MAX];
 };
 
+typedef enum {
+	/*
+	 * BIT_0 - Atio Arrival / schedule to work
+	 * BIT_1 - qlt_do_work
+	 * BIT_2 - qlt_do work failed
+	 * BIT_3 - xfer rdy/tcm_qla2xxx_write_pending
+	 * BIT_4 - read respond/tcm_qla2xx_queue_data_in
+	 * BIT_5 - status respond / tcm_qla2xx_queue_status
+	 * BIT_6 - tcm request to abort/Term exchange.
+	 *	pre_xmit_response->qlt_send_term_exchange
+	 * BIT_7 - SRR received (qlt_handle_srr->qlt_xmit_response)
+	 * BIT_8 - SRR received (qlt_handle_srr->qlt_rdy_to_xfer)
+	 * BIT_9 - SRR received (qla_handle_srr->qlt_send_term_exchange)
+	 * BIT_10 - Data in - hanlde_data->tcm_qla2xxx_handle_data
+
+	 * BIT_12 - good completion - qlt_ctio_do_completion -->free_cmd
+	 * BIT_13 - Bad completion -
+	 *	qlt_ctio_do_completion --> qlt_term_ctio_exchange
+	 * BIT_14 - Back end data received/sent.
+	 * BIT_15 - SRR prepare ctio
+	 * BIT_16 - complete free
+	 * BIT_17 - flush - qlt_abort_cmd_on_host_reset
+	 * BIT_18 - completion w/abort status
+	 * BIT_19 - completion w/unknown status
+	 * BIT_20 - tcm_qla2xxx_free_cmd
+	 */
+	CMD_FLAG_DATA_WORK = BIT_11,
+	CMD_FLAG_DATA_WORK_FREE = BIT_21,
+} cmd_flags_t;
+
 struct qla_tgt_cmd {
 	struct se_cmd se_cmd;
 	struct qla_tgt_sess *sess;
@@ -939,6 +982,7 @@ struct qla_tgt_cmd {
 	/* Sense buffer that will be mapped into outgoing status */
 	unsigned char sense_buffer[TRANSPORT_SENSE_BUFFER];
 
+	spinlock_t cmd_lock;
 	/* to save extra sess dereferences */
 	unsigned int conf_compl_supported:1;
 	unsigned int sg_mapped:1;
@@ -949,6 +993,7 @@ struct qla_tgt_cmd {
 	unsigned int term_exchg:1;
 	unsigned int cmd_sent_to_fw:1;
 	unsigned int cmd_in_wq:1;
+	unsigned int aborted:1;
 
 	struct scatterlist *sg;	/* cmd data buffer SG vector */
 	int sg_cnt;		/* SG segments count */
@@ -972,30 +1017,8 @@ struct qla_tgt_cmd {
 
 	uint64_t jiffies_at_alloc;
 	uint64_t jiffies_at_free;
-	/* BIT_0 - Atio Arrival / schedule to work
-	 * BIT_1 - qlt_do_work
-	 * BIT_2 - qlt_do work failed
-	 * BIT_3 - xfer rdy/tcm_qla2xxx_write_pending
-	 * BIT_4 - read respond/tcm_qla2xx_queue_data_in
-	 * BIT_5 - status respond / tcm_qla2xx_queue_status
-	 * BIT_6 - tcm request to abort/Term exchange.
-	 *	pre_xmit_response->qlt_send_term_exchange
-	 * BIT_7 - SRR received (qlt_handle_srr->qlt_xmit_response)
-	 * BIT_8 - SRR received (qlt_handle_srr->qlt_rdy_to_xfer)
-	 * BIT_9 - SRR received (qla_handle_srr->qlt_send_term_exchange)
-	 * BIT_10 - Data in - hanlde_data->tcm_qla2xxx_handle_data
-	 * BIT_11 - Data actually going to TCM : tcm_qla2xx_handle_data_work
-	 * BIT_12 - good completion - qlt_ctio_do_completion -->free_cmd
-	 * BIT_13 - Bad completion -
-	 *	qlt_ctio_do_completion --> qlt_term_ctio_exchange
-	 * BIT_14 - Back end data received/sent.
-	 * BIT_15 - SRR prepare ctio
-	 * BIT_16 - complete free
-	 * BIT_17 - flush - qlt_abort_cmd_on_host_reset
-	 * BIT_18 - completion w/abort status
-	 * BIT_19 - completion w/unknown status
-	 */
-	uint32_t cmd_flags;
+
+	cmd_flags_t cmd_flags;
 };
 
 struct qla_tgt_sess_work_param {
@@ -1120,13 +1143,21 @@ static inline uint32_t sid_to_key(const uint8_t *s_id)
 	return key;
 }
 
+static inline void sid_to_portid(const uint8_t *s_id, port_id_t *p)
+{
+	memset(p, 0, sizeof(*p));
+	p->b.domain = s_id[0];
+	p->b.area = s_id[1];
+	p->b.al_pa = s_id[2];
+}
+
 /*
  * Exported symbols from qla_target.c LLD logic used by qla2xxx code..
  */
 extern void qlt_response_pkt_all_vps(struct scsi_qla_host *, response_t *);
 extern int qlt_rdy_to_xfer(struct qla_tgt_cmd *);
 extern int qlt_xmit_response(struct qla_tgt_cmd *, int, uint8_t);
-extern void qlt_abort_cmd(struct qla_tgt_cmd *);
+extern int qlt_abort_cmd(struct qla_tgt_cmd *);
 extern void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_mcmd(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_cmd(struct qla_tgt_cmd *cmd);
@@ -1135,7 +1166,7 @@ extern void qlt_enable_vha(struct scsi_qla_host *);
 extern void qlt_vport_create(struct scsi_qla_host *, struct qla_hw_data *);
 extern void qlt_rff_id(struct scsi_qla_host *, struct ct_sns_req *);
 extern void qlt_init_atio_q_entries(struct scsi_qla_host *);
-extern void qlt_24xx_process_atio_queue(struct scsi_qla_host *);
+extern void qlt_24xx_process_atio_queue(struct scsi_qla_host *, uint8_t);
 extern void qlt_24xx_config_rings(struct scsi_qla_host *);
 extern void qlt_24xx_config_nvram_stage1(struct scsi_qla_host *,
 	struct nvram_24xx *);
diff --git a/drivers/scsi/qla2xxx/qla_tmpl.c b/drivers/scsi/qla2xxx/qla_tmpl.c
index ddbe2e7ac14d..c3e622524604 100644
--- a/drivers/scsi/qla2xxx/qla_tmpl.c
+++ b/drivers/scsi/qla2xxx/qla_tmpl.c
@@ -395,6 +395,10 @@ qla27xx_fwdt_entry_t263(struct scsi_qla_host *vha,
 	if (ent->t263.queue_type == T263_QUEUE_TYPE_REQ) {
 		for (i = 0; i < vha->hw->max_req_queues; i++) {
 			struct req_que *req = vha->hw->req_q_map[i];
+
+			if (!test_bit(i, vha->hw->req_qid_map))
+				continue;
+
 			if (req || !buf) {
 				length = req ?
 				    req->length : REQUEST_ENTRY_CNT_24XX;
@@ -408,6 +412,10 @@ qla27xx_fwdt_entry_t263(struct scsi_qla_host *vha,
 	} else if (ent->t263.queue_type == T263_QUEUE_TYPE_RSP) {
 		for (i = 0; i < vha->hw->max_rsp_queues; i++) {
 			struct rsp_que *rsp = vha->hw->rsp_q_map[i];
+
+			if (!test_bit(i, vha->hw->rsp_qid_map))
+				continue;
+
 			if (rsp || !buf) {
 				length = rsp ?
 				    rsp->length : RESPONSE_ENTRY_CNT_MQ;
@@ -634,6 +642,10 @@ qla27xx_fwdt_entry_t274(struct scsi_qla_host *vha,
 	if (ent->t274.queue_type == T274_QUEUE_TYPE_REQ_SHAD) {
 		for (i = 0; i < vha->hw->max_req_queues; i++) {
 			struct req_que *req = vha->hw->req_q_map[i];
+
+			if (!test_bit(i, vha->hw->req_qid_map))
+				continue;
+
 			if (req || !buf) {
 				qla27xx_insert16(i, buf, len);
 				qla27xx_insert16(1, buf, len);
@@ -645,6 +657,10 @@ qla27xx_fwdt_entry_t274(struct scsi_qla_host *vha,
 	} else if (ent->t274.queue_type == T274_QUEUE_TYPE_RSP_SHAD) {
 		for (i = 0; i < vha->hw->max_rsp_queues; i++) {
 			struct rsp_que *rsp = vha->hw->rsp_q_map[i];
+
+			if (!test_bit(i, vha->hw->rsp_qid_map))
+				continue;
+
 			if (rsp || !buf) {
 				qla27xx_insert16(i, buf, len);
 				qla27xx_insert16(1, buf, len);
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index 81af294f15a7..1808a01cfb7e 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -284,6 +284,7 @@ static void tcm_qla2xxx_complete_free(struct work_struct *work)
 
 	WARN_ON(cmd->cmd_flags &  BIT_16);
 
+	cmd->vha->tgt_counters.qla_core_ret_sta_ctio++;
 	cmd->cmd_flags |= BIT_16;
 	transport_generic_free_cmd(&cmd->se_cmd, 0);
 }
@@ -295,9 +296,14 @@ static void tcm_qla2xxx_complete_free(struct work_struct *work)
  */
 static void tcm_qla2xxx_free_cmd(struct qla_tgt_cmd *cmd)
 {
+	cmd->vha->tgt_counters.core_qla_free_cmd++;
 	cmd->cmd_in_wq = 1;
+
+	BUG_ON(cmd->cmd_flags & BIT_20);
+	cmd->cmd_flags |= BIT_20;
+
 	INIT_WORK(&cmd->work, tcm_qla2xxx_complete_free);
-	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
+	queue_work_on(smp_processor_id(), tcm_qla2xxx_free_wq, &cmd->work);
 }
 
 /*
@@ -342,9 +348,9 @@ static int tcm_qla2xxx_shutdown_session(struct se_session *se_sess)
 	BUG_ON(!sess);
 	vha = sess->vha;
 
-	spin_lock_irqsave(&vha->hw->hardware_lock, flags);
+	spin_lock_irqsave(&vha->hw->tgt.sess_lock, flags);
 	target_sess_cmd_list_set_waiting(se_sess);
-	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
+	spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 
 	return 1;
 }
@@ -358,9 +364,9 @@ static void tcm_qla2xxx_close_session(struct se_session *se_sess)
 	BUG_ON(!sess);
 	vha = sess->vha;
 
-	spin_lock_irqsave(&vha->hw->hardware_lock, flags);
+	spin_lock_irqsave(&vha->hw->tgt.sess_lock, flags);
 	qlt_unreg_sess(sess);
-	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
+	spin_unlock_irqrestore(&vha->hw->tgt.sess_lock, flags);
 }
 
 static u32 tcm_qla2xxx_sess_get_index(struct se_session *se_sess)
@@ -372,6 +378,20 @@ static int tcm_qla2xxx_write_pending(struct se_cmd *se_cmd)
 {
 	struct qla_tgt_cmd *cmd = container_of(se_cmd,
 				struct qla_tgt_cmd, se_cmd);
+
+	if (cmd->aborted) {
+		/* Cmd can loop during Q-full.  tcm_qla2xxx_aborted_task
+		 * can get ahead of this cmd. tcm_qla2xxx_aborted_task
+		 * already kick start the free.
+		 */
+		pr_debug("write_pending aborted cmd[%p] refcount %d "
+			"transport_state %x, t_state %x, se_cmd_flags %x\n",
+			cmd,cmd->se_cmd.cmd_kref.refcount.counter,
+			cmd->se_cmd.transport_state,
+			cmd->se_cmd.t_state,
+			cmd->se_cmd.se_cmd_flags);
+		return 0;
+	}
 	cmd->cmd_flags |= BIT_3;
 	cmd->bufflen = se_cmd->data_length;
 	cmd->dma_data_direction = target_reverse_dma_direction(se_cmd);
@@ -403,7 +423,7 @@ static int tcm_qla2xxx_write_pending_status(struct se_cmd *se_cmd)
 	    se_cmd->t_state == TRANSPORT_COMPLETE_QF_WP) {
 		spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
 		wait_for_completion_timeout(&se_cmd->t_transport_stop_comp,
-					    3 * HZ);
+						50);
 		return 0;
 	}
 	spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
@@ -442,6 +462,9 @@ static int tcm_qla2xxx_handle_cmd(scsi_qla_host_t *vha, struct qla_tgt_cmd *cmd,
 	if (bidi)
 		flags |= TARGET_SCF_BIDI_OP;
 
+	if (se_cmd->cpuid != WORK_CPU_UNBOUND)
+		flags |= TARGET_SCF_USE_CPUID;
+
 	sess = cmd->sess;
 	if (!sess) {
 		pr_err("Unable to locate struct qla_tgt_sess from qla_tgt_cmd\n");
@@ -454,6 +477,7 @@ static int tcm_qla2xxx_handle_cmd(scsi_qla_host_t *vha, struct qla_tgt_cmd *cmd,
 		return -EINVAL;
 	}
 
+	cmd->vha->tgt_counters.qla_core_sbt_cmd++;
 	return target_submit_cmd(se_cmd, se_sess, cdb, &cmd->sense_buffer[0],
 				cmd->unpacked_lun, data_length, fcp_task_attr,
 				data_dir, flags);
@@ -462,13 +486,26 @@ static int tcm_qla2xxx_handle_cmd(scsi_qla_host_t *vha, struct qla_tgt_cmd *cmd,
 static void tcm_qla2xxx_handle_data_work(struct work_struct *work)
 {
 	struct qla_tgt_cmd *cmd = container_of(work, struct qla_tgt_cmd, work);
+	unsigned long flags;
 
 	/*
 	 * Ensure that the complete FCP WRITE payload has been received.
 	 * Otherwise return an exception via CHECK_CONDITION status.
 	 */
 	cmd->cmd_in_wq = 0;
-	cmd->cmd_flags |= BIT_11;
+
+	spin_lock_irqsave(&cmd->cmd_lock, flags);
+	cmd->cmd_flags |= CMD_FLAG_DATA_WORK;
+	if (cmd->aborted) {
+		cmd->cmd_flags |= CMD_FLAG_DATA_WORK_FREE;
+		spin_unlock_irqrestore(&cmd->cmd_lock, flags);
+
+		tcm_qla2xxx_free_cmd(cmd);
+		return;
+	}
+	spin_unlock_irqrestore(&cmd->cmd_lock, flags);
+
+	cmd->vha->tgt_counters.qla_core_ret_ctio++;
 	if (!cmd->write_data_transferred) {
 		/*
 		 * Check if se_cmd has already been aborted via LUN_RESET, and
@@ -500,7 +537,7 @@ static void tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
 	cmd->cmd_flags |= BIT_10;
 	cmd->cmd_in_wq = 1;
 	INIT_WORK(&cmd->work, tcm_qla2xxx_handle_data_work);
-	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
+	queue_work_on(smp_processor_id(), tcm_qla2xxx_free_wq, &cmd->work);
 }
 
 static void tcm_qla2xxx_handle_dif_work(struct work_struct *work)
@@ -542,6 +579,20 @@ static int tcm_qla2xxx_queue_data_in(struct se_cmd *se_cmd)
 	struct qla_tgt_cmd *cmd = container_of(se_cmd,
 				struct qla_tgt_cmd, se_cmd);
 
+	if (cmd->aborted) {
+		/* Cmd can loop during Q-full.  tcm_qla2xxx_aborted_task
+		 * can get ahead of this cmd. tcm_qla2xxx_aborted_task
+		 * already kick start the free.
+		 */
+		pr_debug("queue_data_in aborted cmd[%p] refcount %d "
+			"transport_state %x, t_state %x, se_cmd_flags %x\n",
+			cmd,cmd->se_cmd.cmd_kref.refcount.counter,
+			cmd->se_cmd.transport_state,
+			cmd->se_cmd.t_state,
+			cmd->se_cmd.se_cmd_flags);
+		return 0;
+	}
+
 	cmd->cmd_flags |= BIT_4;
 	cmd->bufflen = se_cmd->data_length;
 	cmd->dma_data_direction = target_reverse_dma_direction(se_cmd);
@@ -633,17 +684,40 @@ static void tcm_qla2xxx_queue_tm_rsp(struct se_cmd *se_cmd)
 	qlt_xmit_tm_rsp(mcmd);
 }
 
+
+#define DATA_WORK_NOT_FREE(_flags) \
+	(( _flags & (CMD_FLAG_DATA_WORK|CMD_FLAG_DATA_WORK_FREE)) == \
+	 CMD_FLAG_DATA_WORK)
 static void tcm_qla2xxx_aborted_task(struct se_cmd *se_cmd)
 {
 	struct qla_tgt_cmd *cmd = container_of(se_cmd,
 				struct qla_tgt_cmd, se_cmd);
-	qlt_abort_cmd(cmd);
+	unsigned long flags;
+
+	if (qlt_abort_cmd(cmd))
+		return;
+
+	spin_lock_irqsave(&cmd->cmd_lock, flags);
+	if ((cmd->state == QLA_TGT_STATE_NEW)||
+		((cmd->state == QLA_TGT_STATE_DATA_IN) &&
+		 DATA_WORK_NOT_FREE(cmd->cmd_flags)) ) {
+
+		cmd->cmd_flags |= CMD_FLAG_DATA_WORK_FREE;
+		spin_unlock_irqrestore(&cmd->cmd_lock, flags);
+		/* Cmd have not reached firmware.
+		 * Use this trigger to free it. */
+		tcm_qla2xxx_free_cmd(cmd);
+		return;
+	}
+	spin_unlock_irqrestore(&cmd->cmd_lock, flags);
+	return;
+
 }
 
 static void tcm_qla2xxx_clear_sess_lookup(struct tcm_qla2xxx_lport *,
 			struct tcm_qla2xxx_nacl *, struct qla_tgt_sess *);
 /*
- * Expected to be called with struct qla_hw_data->hardware_lock held
+ * Expected to be called with struct qla_hw_data->tgt.sess_lock held
  */
 static void tcm_qla2xxx_clear_nacl_from_fcport_map(struct qla_tgt_sess *sess)
 {
@@ -697,13 +771,13 @@ static void tcm_qla2xxx_put_sess(struct qla_tgt_sess *sess)
 	if (!sess)
 		return;
 
-	assert_spin_locked(&sess->vha->hw->hardware_lock);
+	assert_spin_locked(&sess->vha->hw->tgt.sess_lock);
 	kref_put(&sess->se_sess->sess_kref, tcm_qla2xxx_release_session);
 }
 
 static void tcm_qla2xxx_shutdown_sess(struct qla_tgt_sess *sess)
 {
-	assert_spin_locked(&sess->vha->hw->hardware_lock);
+	assert_spin_locked(&sess->vha->hw->tgt.sess_lock);
 	target_sess_cmd_list_set_waiting(sess->se_sess);
 }
 
@@ -1077,7 +1151,7 @@ static struct se_portal_group *tcm_qla2xxx_npiv_make_tpg(
 }
 
 /*
- * Expected to be called with struct qla_hw_data->hardware_lock held
+ * Expected to be called with struct qla_hw_data->tgt.sess_lock held
  */
 static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_s_id(
 	scsi_qla_host_t *vha,
@@ -1116,7 +1190,7 @@ static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_s_id(
 }
 
 /*
- * Expected to be called with struct qla_hw_data->hardware_lock held
+ * Expected to be called with struct qla_hw_data->tgt.sess_lock held
  */
 static void tcm_qla2xxx_set_sess_by_s_id(
 	struct tcm_qla2xxx_lport *lport,
@@ -1182,7 +1256,7 @@ static void tcm_qla2xxx_set_sess_by_s_id(
 }
 
 /*
- * Expected to be called with struct qla_hw_data->hardware_lock held
+ * Expected to be called with struct qla_hw_data->tgt.sess_lock held
  */
 static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_loop_id(
 	scsi_qla_host_t *vha,
@@ -1221,7 +1295,7 @@ static struct qla_tgt_sess *tcm_qla2xxx_find_sess_by_loop_id(
 }
 
 /*
- * Expected to be called with struct qla_hw_data->hardware_lock held
+ * Expected to be called with struct qla_hw_data->tgt.sess_lock held
  */
 static void tcm_qla2xxx_set_sess_by_loop_id(
 	struct tcm_qla2xxx_lport *lport,
@@ -1285,7 +1359,7 @@ static void tcm_qla2xxx_set_sess_by_loop_id(
 }
 
 /*
- * Should always be called with qla_hw_data->hardware_lock held.
+ * Should always be called with qla_hw_data->tgt.sess_lock held.
  */
 static void tcm_qla2xxx_clear_sess_lookup(struct tcm_qla2xxx_lport *lport,
 		struct tcm_qla2xxx_nacl *nacl, struct qla_tgt_sess *sess)
@@ -1353,7 +1427,7 @@ static int tcm_qla2xxx_check_initiator_node_acl(
 	struct qla_tgt_sess *sess = qla_tgt_sess;
 	unsigned char port_name[36];
 	unsigned long flags;
-	int num_tags = (ha->fw_xcb_count) ? ha->fw_xcb_count :
+	int num_tags = (ha->cur_fw_xcb_count) ? ha->cur_fw_xcb_count :
 		       TCM_QLA2XXX_DEFAULT_TAGS;
 
 	lport = vha->vha_tgt.target_lport_ptr;
@@ -1401,12 +1475,12 @@ static int tcm_qla2xxx_check_initiator_node_acl(
 	 * And now setup the new se_nacl and session pointers into our HW lport
 	 * mappings for fabric S_ID and LOOP_ID.
 	 */
-	spin_lock_irqsave(&ha->hardware_lock, flags);
+	spin_lock_irqsave(&ha->tgt.sess_lock, flags);
 	tcm_qla2xxx_set_sess_by_s_id(lport, se_nacl, nacl, se_sess,
 			qla_tgt_sess, s_id);
 	tcm_qla2xxx_set_sess_by_loop_id(lport, se_nacl, nacl, se_sess,
 			qla_tgt_sess, loop_id);
-	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+	spin_unlock_irqrestore(&ha->tgt.sess_lock, flags);
 	/*
 	 * Finally register the new FC Nexus with TCM
 	 */
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index d07fb653f5dc..b1bf42b93fcc 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -782,7 +782,7 @@ void scsi_attach_vpd(struct scsi_device *sdev)
 	int vpd_len = SCSI_VPD_PG_LEN;
 	int pg80_supported = 0;
 	int pg83_supported = 0;
-	unsigned char *vpd_buf;
+	unsigned char __rcu *vpd_buf, *orig_vpd_buf = NULL;
 
 	if (sdev->skip_vpd_pages)
 		return;
@@ -828,8 +828,16 @@ retry_pg80:
 			kfree(vpd_buf);
 			goto retry_pg80;
 		}
+		mutex_lock(&sdev->inquiry_mutex);
+		orig_vpd_buf = sdev->vpd_pg80;
 		sdev->vpd_pg80_len = result;
-		sdev->vpd_pg80 = vpd_buf;
+		rcu_assign_pointer(sdev->vpd_pg80, vpd_buf);
+		mutex_unlock(&sdev->inquiry_mutex);
+		synchronize_rcu();
+		if (orig_vpd_buf) {
+			kfree(orig_vpd_buf);
+			orig_vpd_buf = NULL;
+		}
 		vpd_len = SCSI_VPD_PG_LEN;
 	}
 
@@ -849,8 +857,14 @@ retry_pg83:
 			kfree(vpd_buf);
 			goto retry_pg83;
 		}
+		mutex_lock(&sdev->inquiry_mutex);
+		orig_vpd_buf = sdev->vpd_pg83;
 		sdev->vpd_pg83_len = result;
-		sdev->vpd_pg83 = vpd_buf;
+		rcu_assign_pointer(sdev->vpd_pg83, vpd_buf);
+		mutex_unlock(&sdev->inquiry_mutex);
+		synchronize_rcu();
+		if (orig_vpd_buf)
+			kfree(orig_vpd_buf);
 	}
 }
 
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index d09d60293c27..f3d69a98c725 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -129,7 +129,7 @@ static const char *scsi_debug_version_date = "20141022";
 #define DEF_NO_LUN_0   0
 #define DEF_NUM_PARTS   0
 #define DEF_OPTS   0
-#define DEF_OPT_BLKS 64
+#define DEF_OPT_BLKS 1024
 #define DEF_PHYSBLK_EXP 0
 #define DEF_PTYPE   0
 #define DEF_REMOVABLE false
@@ -679,7 +679,7 @@ static void *fake_store(unsigned long long lba)
 
 static struct sd_dif_tuple *dif_store(sector_t sector)
 {
-	sector = do_div(sector, sdebug_store_sectors);
+	sector = sector_div(sector, sdebug_store_sectors);
 
 	return dif_storep + sector;
 }
@@ -2781,7 +2781,7 @@ static unsigned long lba_to_map_index(sector_t lba)
 		lba += scsi_debug_unmap_granularity -
 			scsi_debug_unmap_alignment;
 	}
-	do_div(lba, scsi_debug_unmap_granularity);
+	sector_div(lba, scsi_debug_unmap_granularity);
 
 	return lba;
 }
@@ -4140,7 +4140,7 @@ MODULE_PARM_DESC(no_lun_0, "no LU number 0 (def=0 -> have lun 0)");
 MODULE_PARM_DESC(no_uld, "stop ULD (e.g. sd driver) attaching (def=0))");
 MODULE_PARM_DESC(num_parts, "number of partitions(def=0)");
 MODULE_PARM_DESC(num_tgts, "number of targets per host to simulate(def=1)");
-MODULE_PARM_DESC(opt_blks, "optimal transfer length in block (def=64)");
+MODULE_PARM_DESC(opt_blks, "optimal transfer length in blocks (def=1024)");
 MODULE_PARM_DESC(opts, "1->noise, 2->medium_err, 4->timeout, 8->recovered_err... (def=0)");
 MODULE_PARM_DESC(physblk_exp, "physical block exponent (def=0)");
 MODULE_PARM_DESC(ptype, "SCSI peripheral type(def=0[disk])");
@@ -4847,10 +4847,10 @@ static int __init scsi_debug_init(void)
 	/* play around with geometry, don't waste too much on track 0 */
 	sdebug_heads = 8;
 	sdebug_sectors_per = 32;
-	if (scsi_debug_dev_size_mb >= 16)
-		sdebug_heads = 32;
-	else if (scsi_debug_dev_size_mb >= 256)
+	if (scsi_debug_dev_size_mb >= 256)
 		sdebug_heads = 64;
+	else if (scsi_debug_dev_size_mb >= 16)
+		sdebug_heads = 32;
 	sdebug_cylinders_per = (unsigned long)sdebug_capacity /
 			       (sdebug_sectors_per * sdebug_heads);
 	if (sdebug_cylinders_per >= 1024) {
diff --git a/drivers/scsi/scsi_devinfo.c b/drivers/scsi/scsi_devinfo.c
index 2c1160c7ec92..bbfbfd9e5aa3 100644
--- a/drivers/scsi/scsi_devinfo.c
+++ b/drivers/scsi/scsi_devinfo.c
@@ -205,6 +205,8 @@ static struct {
 	{"Intel", "Multi-Flex", NULL, BLIST_NO_RSOC},
 	{"iRiver", "iFP Mass Driver", NULL, BLIST_NOT_LOCKABLE | BLIST_INQUIRY_36},
 	{"LASOUND", "CDX7405", "3.10", BLIST_MAX5LUN | BLIST_SINGLELUN},
+	{"Marvell", "Console", NULL, BLIST_SKIP_VPD_PAGES},
+	{"Marvell", "91xx Config", "1.01", BLIST_SKIP_VPD_PAGES},
 	{"MATSHITA", "PD-1", NULL, BLIST_FORCELUN | BLIST_SINGLELUN},
 	{"MATSHITA", "DMC-LC5", NULL, BLIST_NOT_LOCKABLE | BLIST_INQUIRY_36},
 	{"MATSHITA", "DMC-LC40", NULL, BLIST_NOT_LOCKABLE | BLIST_INQUIRY_36},
@@ -227,6 +229,7 @@ static struct {
 	{"Promise", "VTrak E610f", NULL, BLIST_SPARSELUN | BLIST_NO_RSOC},
 	{"Promise", "", NULL, BLIST_SPARSELUN},
 	{"QNAP", "iSCSI Storage", NULL, BLIST_MAX_1024},
+	{"SYNOLOGY", "iSCSI Storage", NULL, BLIST_MAX_1024},
 	{"QUANTUM", "XP34301", "1071", BLIST_NOTQ},
 	{"REGAL", "CDC-4X", NULL, BLIST_MAX5LUN | BLIST_SINGLELUN},
 	{"SanDisk", "ImageMate CF-SD1", NULL, BLIST_FORCELUN},
diff --git a/drivers/scsi/scsi_dh.c b/drivers/scsi/scsi_dh.c
index e7649ed3f667..54d446c9f56e 100644
--- a/drivers/scsi/scsi_dh.c
+++ b/drivers/scsi/scsi_dh.c
@@ -153,76 +153,11 @@ static void scsi_dh_handler_detach(struct scsi_device *sdev)
 	module_put(sdev->handler->module);
 }
 
-/*
- * Functions for sysfs attribute 'dh_state'
- */
-static ssize_t
-store_dh_state(struct device *dev, struct device_attribute *attr,
-	       const char *buf, size_t count)
-{
-	struct scsi_device *sdev = to_scsi_device(dev);
-	struct scsi_device_handler *scsi_dh;
-	int err = -EINVAL;
-
-	if (sdev->sdev_state == SDEV_CANCEL ||
-	    sdev->sdev_state == SDEV_DEL)
-		return -ENODEV;
-
-	if (!sdev->handler) {
-		/*
-		 * Attach to a device handler
-		 */
-		scsi_dh = scsi_dh_lookup(buf);
-		if (!scsi_dh)
-			return err;
-		err = scsi_dh_handler_attach(sdev, scsi_dh);
-	} else {
-		if (!strncmp(buf, "detach", 6)) {
-			/*
-			 * Detach from a device handler
-			 */
-			sdev_printk(KERN_WARNING, sdev,
-				    "can't detach handler %s.\n",
-				    sdev->handler->name);
-			err = -EINVAL;
-		} else if (!strncmp(buf, "activate", 8)) {
-			/*
-			 * Activate a device handler
-			 */
-			if (sdev->handler->activate)
-				err = sdev->handler->activate(sdev, NULL, NULL);
-			else
-				err = 0;
-		}
-	}
-
-	return err<0?err:count;
-}
-
-static ssize_t
-show_dh_state(struct device *dev, struct device_attribute *attr, char *buf)
-{
-	struct scsi_device *sdev = to_scsi_device(dev);
-
-	if (!sdev->handler)
-		return snprintf(buf, 20, "detached\n");
-
-	return snprintf(buf, 20, "%s\n", sdev->handler->name);
-}
-
-static struct device_attribute scsi_dh_state_attr =
-	__ATTR(dh_state, S_IRUGO | S_IWUSR, show_dh_state,
-	       store_dh_state);
-
 int scsi_dh_add_device(struct scsi_device *sdev)
 {
 	struct scsi_device_handler *devinfo = NULL;
 	const char *drv;
-	int err;
-
-	err = device_create_file(&sdev->sdev_gendev, &scsi_dh_state_attr);
-	if (err)
-		return err;
+	int err = 0;
 
 	drv = scsi_dh_find_driver(sdev);
 	if (drv)
@@ -238,11 +173,6 @@ void scsi_dh_release_device(struct scsi_device *sdev)
 		scsi_dh_handler_detach(sdev);
 }
 
-void scsi_dh_remove_device(struct scsi_device *sdev)
-{
-	device_remove_file(&sdev->sdev_gendev, &scsi_dh_state_attr);
-}
-
 /*
  * scsi_register_device_handler - register a device handler personality
  *      module.
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index dd8ad2a44510..8c6e31874171 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -23,6 +23,7 @@
 #include <linux/scatterlist.h>
 #include <linux/blk-mq.h>
 #include <linux/ratelimit.h>
+#include <asm/unaligned.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -1343,6 +1344,7 @@ scsi_prep_return(struct request_queue *q, struct request *req, int ret)
 
 	switch (ret) {
 	case BLKPREP_KILL:
+	case BLKPREP_INVALID:
 		req->errors = DID_NO_CONNECT << 16;
 		/* release the command and kill it */
 		if (req->special) {
@@ -3154,3 +3156,190 @@ void sdev_enable_disk_events(struct scsi_device *sdev)
 	atomic_dec(&sdev->disk_events_disable_depth);
 }
 EXPORT_SYMBOL(sdev_enable_disk_events);
+
+/**
+ * scsi_vpd_lun_id - return a unique device identification
+ * @sdev: SCSI device
+ * @id:   buffer for the identification
+ * @id_len:  length of the buffer
+ *
+ * Copies a unique device identification into @id based
+ * on the information in the VPD page 0x83 of the device.
+ * The string will be formatted as a SCSI name string.
+ *
+ * Returns the length of the identification or error on failure.
+ * If the identifier is longer than the supplied buffer the actual
+ * identifier length is returned and the buffer is not zero-padded.
+ */
+int scsi_vpd_lun_id(struct scsi_device *sdev, char *id, size_t id_len)
+{
+	u8 cur_id_type = 0xff;
+	u8 cur_id_size = 0;
+	unsigned char *d, *cur_id_str;
+	unsigned char __rcu *vpd_pg83;
+	int id_size = -EINVAL;
+
+	rcu_read_lock();
+	vpd_pg83 = rcu_dereference(sdev->vpd_pg83);
+	if (!vpd_pg83) {
+		rcu_read_unlock();
+		return -ENXIO;
+	}
+
+	/*
+	 * Look for the correct descriptor.
+	 * Order of preference for lun descriptor:
+	 * - SCSI name string
+	 * - NAA IEEE Registered Extended
+	 * - EUI-64 based 16-byte
+	 * - EUI-64 based 12-byte
+	 * - NAA IEEE Registered
+	 * - NAA IEEE Extended
+	 * as longer descriptors reduce the likelyhood
+	 * of identification clashes.
+	 */
+
+	/* The id string must be at least 20 bytes + terminating NULL byte */
+	if (id_len < 21) {
+		rcu_read_unlock();
+		return -EINVAL;
+	}
+
+	memset(id, 0, id_len);
+	d = vpd_pg83 + 4;
+	while (d < vpd_pg83 + sdev->vpd_pg83_len) {
+		/* Skip designators not referring to the LUN */
+		if ((d[1] & 0x30) != 0x00)
+			goto next_desig;
+
+		switch (d[1] & 0xf) {
+		case 0x2:
+			/* EUI-64 */
+			if (cur_id_size > d[3])
+				break;
+			/* Prefer NAA IEEE Registered Extended */
+			if (cur_id_type == 0x3 &&
+			    cur_id_size == d[3])
+				break;
+			cur_id_size = d[3];
+			cur_id_str = d + 4;
+			cur_id_type = d[1] & 0xf;
+			switch (cur_id_size) {
+			case 8:
+				id_size = snprintf(id, id_len,
+						   "eui.%8phN",
+						   cur_id_str);
+				break;
+			case 12:
+				id_size = snprintf(id, id_len,
+						   "eui.%12phN",
+						   cur_id_str);
+				break;
+			case 16:
+				id_size = snprintf(id, id_len,
+						   "eui.%16phN",
+						   cur_id_str);
+				break;
+			default:
+				cur_id_size = 0;
+				break;
+			}
+			break;
+		case 0x3:
+			/* NAA */
+			if (cur_id_size > d[3])
+				break;
+			cur_id_size = d[3];
+			cur_id_str = d + 4;
+			cur_id_type = d[1] & 0xf;
+			switch (cur_id_size) {
+			case 8:
+				id_size = snprintf(id, id_len,
+						   "naa.%8phN",
+						   cur_id_str);
+				break;
+			case 16:
+				id_size = snprintf(id, id_len,
+						   "naa.%16phN",
+						   cur_id_str);
+				break;
+			default:
+				cur_id_size = 0;
+				break;
+			}
+			break;
+		case 0x8:
+			/* SCSI name string */
+			if (cur_id_size + 4 > d[3])
+				break;
+			/* Prefer others for truncated descriptor */
+			if (cur_id_size && d[3] > id_len)
+				break;
+			cur_id_size = id_size = d[3];
+			cur_id_str = d + 4;
+			cur_id_type = d[1] & 0xf;
+			if (cur_id_size >= id_len)
+				cur_id_size = id_len - 1;
+			memcpy(id, cur_id_str, cur_id_size);
+			/* Decrease priority for truncated descriptor */
+			if (cur_id_size != id_size)
+				cur_id_size = 6;
+			break;
+		default:
+			break;
+		}
+next_desig:
+		d += d[3] + 4;
+	}
+	rcu_read_unlock();
+
+	return id_size;
+}
+EXPORT_SYMBOL(scsi_vpd_lun_id);
+
+/*
+ * scsi_vpd_tpg_id - return a target port group identifier
+ * @sdev: SCSI device
+ *
+ * Returns the Target Port Group identifier from the information
+ * froom VPD page 0x83 of the device.
+ *
+ * Returns the identifier or error on failure.
+ */
+int scsi_vpd_tpg_id(struct scsi_device *sdev, int *rel_id)
+{
+	unsigned char *d;
+	unsigned char __rcu *vpd_pg83;
+	int group_id = -EAGAIN, rel_port = -1;
+
+	rcu_read_lock();
+	vpd_pg83 = rcu_dereference(sdev->vpd_pg83);
+	if (!vpd_pg83) {
+		rcu_read_unlock();
+		return -ENXIO;
+	}
+
+	d = sdev->vpd_pg83 + 4;
+	while (d < sdev->vpd_pg83 + sdev->vpd_pg83_len) {
+		switch (d[1] & 0xf) {
+		case 0x4:
+			/* Relative target port */
+			rel_port = get_unaligned_be16(&d[6]);
+			break;
+		case 0x5:
+			/* Target port group */
+			group_id = get_unaligned_be16(&d[6]);
+			break;
+		default:
+			break;
+		}
+		d += d[3] + 4;
+	}
+	rcu_read_unlock();
+
+	if (group_id >= 0 && rel_id && rel_port != -1)
+		*rel_id = rel_port;
+
+	return group_id;
+}
+EXPORT_SYMBOL(scsi_vpd_tpg_id);
diff --git a/drivers/scsi/scsi_priv.h b/drivers/scsi/scsi_priv.h
index 4d01cdb1b348..27b4d0a6a01d 100644
--- a/drivers/scsi/scsi_priv.h
+++ b/drivers/scsi/scsi_priv.h
@@ -174,12 +174,11 @@ extern struct async_domain scsi_sd_probe_domain;
 #ifdef CONFIG_SCSI_DH
 int scsi_dh_add_device(struct scsi_device *sdev);
 void scsi_dh_release_device(struct scsi_device *sdev);
-void scsi_dh_remove_device(struct scsi_device *sdev);
 #else
 static inline int scsi_dh_add_device(struct scsi_device *sdev) { return 0; }
 static inline void scsi_dh_release_device(struct scsi_device *sdev) { }
-static inline void scsi_dh_remove_device(struct scsi_device *sdev) { }
 #endif
+static inline void scsi_dh_remove_device(struct scsi_device *sdev) { }
 
 /* 
  * internal scsi timeout functions: for use by mid-layer and transport
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 054923e3393c..6a820668d442 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -236,6 +236,7 @@ static struct scsi_device *scsi_alloc_sdev(struct scsi_target *starget,
 	INIT_LIST_HEAD(&sdev->starved_entry);
 	INIT_LIST_HEAD(&sdev->event_list);
 	spin_lock_init(&sdev->list_lock);
+	mutex_init(&sdev->inquiry_mutex);
 	INIT_WORK(&sdev->event_work, scsi_evt_thread);
 	INIT_WORK(&sdev->requeue_work, scsi_requeue_run_queue);
 
@@ -1519,6 +1520,9 @@ EXPORT_SYMBOL(scsi_add_device);
 void scsi_rescan_device(struct device *dev)
 {
 	device_lock(dev);
+
+	scsi_attach_vpd(to_scsi_device(dev));
+
 	if (dev->driver && try_module_get(dev->driver->owner)) {
 		struct scsi_driver *drv = to_scsi_driver(dev->driver);
 
diff --git a/drivers/scsi/scsi_sysfs.c b/drivers/scsi/scsi_sysfs.c
index 21930c9ac9cd..00bc7218a7f8 100644
--- a/drivers/scsi/scsi_sysfs.c
+++ b/drivers/scsi/scsi_sysfs.c
@@ -17,6 +17,7 @@
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
 #include <scsi/scsi_tcq.h>
+#include <scsi/scsi_dh.h>
 #include <scsi/scsi_transport.h>
 #include <scsi/scsi_driver.h>
 
@@ -760,11 +761,15 @@ show_vpd_##_page(struct file *filp, struct kobject *kobj,	\
 {									\
 	struct device *dev = container_of(kobj, struct device, kobj);	\
 	struct scsi_device *sdev = to_scsi_device(dev);			\
+	int ret;							\
 	if (!sdev->vpd_##_page)						\
 		return -EINVAL;						\
-	return memory_read_from_buffer(buf, count, &off,		\
-				       sdev->vpd_##_page,		\
+	rcu_read_lock();						\
+	ret = memory_read_from_buffer(buf, count, &off,			\
+				      rcu_dereference(sdev->vpd_##_page), \
 				       sdev->vpd_##_page##_len);	\
+	rcu_read_unlock();						\
+	return ret;						\
 }									\
 static struct bin_attribute dev_attr_vpd_##_page = {		\
 	.attr =	{.name = __stringify(vpd_##_page), .mode = S_IRUGO },	\
@@ -901,6 +906,76 @@ static DEVICE_ATTR(queue_depth, S_IRUGO | S_IWUSR, sdev_show_queue_depth,
 		   sdev_store_queue_depth);
 
 static ssize_t
+sdev_show_wwid(struct device *dev, struct device_attribute *attr,
+		    char *buf)
+{
+	struct scsi_device *sdev = to_scsi_device(dev);
+	ssize_t count;
+
+	count = scsi_vpd_lun_id(sdev, buf, PAGE_SIZE);
+	if (count > 0) {
+		buf[count] = '\n';
+		count++;
+	}
+	return count;
+}
+static DEVICE_ATTR(wwid, S_IRUGO, sdev_show_wwid, NULL);
+
+#ifdef CONFIG_SCSI_DH
+static ssize_t
+sdev_show_dh_state(struct device *dev, struct device_attribute *attr,
+		   char *buf)
+{
+	struct scsi_device *sdev = to_scsi_device(dev);
+
+	if (!sdev->handler)
+		return snprintf(buf, 20, "detached\n");
+
+	return snprintf(buf, 20, "%s\n", sdev->handler->name);
+}
+
+static ssize_t
+sdev_store_dh_state(struct device *dev, struct device_attribute *attr,
+		    const char *buf, size_t count)
+{
+	struct scsi_device *sdev = to_scsi_device(dev);
+	int err = -EINVAL;
+
+	if (sdev->sdev_state == SDEV_CANCEL ||
+	    sdev->sdev_state == SDEV_DEL)
+		return -ENODEV;
+
+	if (!sdev->handler) {
+		/*
+		 * Attach to a device handler
+		 */
+		err = scsi_dh_attach(sdev->request_queue, buf);
+	} else if (!strncmp(buf, "activate", 8)) {
+		/*
+		 * Activate a device handler
+		 */
+		if (sdev->handler->activate)
+			err = sdev->handler->activate(sdev, NULL, NULL);
+		else
+			err = 0;
+	} else if (!strncmp(buf, "detach", 6)) {
+		/*
+		 * Detach from a device handler
+		 */
+		sdev_printk(KERN_WARNING, sdev,
+			    "can't detach handler %s.\n",
+			    sdev->handler->name);
+		err = -EINVAL;
+	}
+
+	return err < 0 ? err : count;
+}
+
+static DEVICE_ATTR(dh_state, S_IRUGO | S_IWUSR, sdev_show_dh_state,
+		   sdev_store_dh_state);
+#endif
+
+static ssize_t
 sdev_show_queue_ramp_up_period(struct device *dev,
 			       struct device_attribute *attr,
 			       char *buf)
@@ -969,6 +1044,10 @@ static struct attribute *scsi_sdev_attrs[] = {
 	&dev_attr_modalias.attr,
 	&dev_attr_queue_depth.attr,
 	&dev_attr_queue_type.attr,
+	&dev_attr_wwid.attr,
+#ifdef CONFIG_SCSI_DH
+	&dev_attr_dh_state.attr,
+#endif
 	&dev_attr_queue_ramp_up_period.attr,
 	REF_EVT(media_change),
 	REF_EVT(inquiry_change_reported),
@@ -1058,11 +1137,12 @@ int scsi_sysfs_add_sdev(struct scsi_device *sdev)
 	}
 
 	error = scsi_dh_add_device(sdev);
-	if (error) {
+	if (error)
+		/*
+		 * device_handler is optional, so any error can be ignored
+		 */
 		sdev_printk(KERN_INFO, sdev,
 				"failed to add device handler: %d\n", error);
-		return error;
-	}
 
 	device_enable_async_suspend(&sdev->sdev_dev);
 	error = device_add(&sdev->sdev_dev);
@@ -1192,16 +1272,18 @@ static void __scsi_remove_target(struct scsi_target *starget)
 void scsi_remove_target(struct device *dev)
 {
 	struct Scsi_Host *shost = dev_to_shost(dev->parent);
-	struct scsi_target *starget;
+	struct scsi_target *starget, *last_target = NULL;
 	unsigned long flags;
 
 restart:
 	spin_lock_irqsave(shost->host_lock, flags);
 	list_for_each_entry(starget, &shost->__targets, siblings) {
-		if (starget->state == STARGET_DEL)
+		if (starget->state == STARGET_DEL ||
+		    starget == last_target)
 			continue;
 		if (starget->dev.parent == dev || &starget->dev == dev) {
 			kref_get(&starget->reap_ref);
+			last_target = starget;
 			spin_unlock_irqrestore(shost->host_lock, flags);
 			__scsi_remove_target(starget);
 			scsi_target_reap(starget);
diff --git a/drivers/scsi/scsi_transport_fc.c b/drivers/scsi/scsi_transport_fc.c
index 24eaaf66af71..8a8822641b26 100644
--- a/drivers/scsi/scsi_transport_fc.c
+++ b/drivers/scsi/scsi_transport_fc.c
@@ -2586,7 +2586,7 @@ fc_rport_final_delete(struct work_struct *work)
 	transport_remove_device(dev);
 	device_del(dev);
 	transport_destroy_device(dev);
-	put_device(&shost->shost_gendev);	/* for fc_host->rport list */
+	scsi_host_put(shost);			/* for fc_host->rport list */
 	put_device(dev);			/* for self-reference */
 }
 
@@ -2650,7 +2650,7 @@ fc_rport_create(struct Scsi_Host *shost, int channel,
 	else
 		rport->scsi_target_id = -1;
 	list_add_tail(&rport->peers, &fc_host->rports);
-	get_device(&shost->shost_gendev);	/* for fc_host->rport list */
+	scsi_host_get(shost);			/* for fc_host->rport list */
 
 	spin_unlock_irqrestore(shost->host_lock, flags);
 
@@ -2685,7 +2685,7 @@ delete_rport:
 	transport_destroy_device(dev);
 	spin_lock_irqsave(shost->host_lock, flags);
 	list_del(&rport->peers);
-	put_device(&shost->shost_gendev);	/* for fc_host->rport list */
+	scsi_host_put(shost);			/* for fc_host->rport list */
 	spin_unlock_irqrestore(shost->host_lock, flags);
 	put_device(dev->parent);
 	kfree(rport);
@@ -3383,7 +3383,7 @@ fc_vport_setup(struct Scsi_Host *shost, int channel, struct device *pdev,
 	fc_host->npiv_vports_inuse++;
 	vport->number = fc_host->next_vport_number++;
 	list_add_tail(&vport->peers, &fc_host->vports);
-	get_device(&shost->shost_gendev);	/* for fc_host->vport list */
+	scsi_host_get(shost);			/* for fc_host->vport list */
 
 	spin_unlock_irqrestore(shost->host_lock, flags);
 
@@ -3441,7 +3441,7 @@ delete_vport:
 	transport_destroy_device(dev);
 	spin_lock_irqsave(shost->host_lock, flags);
 	list_del(&vport->peers);
-	put_device(&shost->shost_gendev);	/* for fc_host->vport list */
+	scsi_host_put(shost);			/* for fc_host->vport list */
 	fc_host->npiv_vports_inuse--;
 	spin_unlock_irqrestore(shost->host_lock, flags);
 	put_device(dev->parent);
@@ -3504,7 +3504,7 @@ fc_vport_terminate(struct fc_vport *vport)
 		vport->flags |= FC_VPORT_DELETED;
 		list_del(&vport->peers);
 		fc_host->npiv_vports_inuse--;
-		put_device(&shost->shost_gendev);  /* for fc_host->vport list */
+		scsi_host_put(shost);		/* for fc_host->vport list */
 	}
 	spin_unlock_irqrestore(shost->host_lock, flags);
 
diff --git a/drivers/scsi/scsi_transport_sas.c b/drivers/scsi/scsi_transport_sas.c
index 30d26e345dcc..80520e2f0fa2 100644
--- a/drivers/scsi/scsi_transport_sas.c
+++ b/drivers/scsi/scsi_transport_sas.c
@@ -341,6 +341,22 @@ static int do_sas_phy_delete(struct device *dev, void *data)
 }
 
 /**
+ * is_sas_attached - check if device is SAS attached
+ * @sdev: scsi device to check
+ *
+ * returns true if the device is SAS attached
+ */
+int is_sas_attached(struct scsi_device *sdev)
+{
+	struct Scsi_Host *shost = sdev->host;
+
+	return shost->transportt->host_attrs.ac.class ==
+		&sas_host_class.class;
+}
+EXPORT_SYMBOL(is_sas_attached);
+
+
+/**
  * sas_remove_children  -  tear down a devices SAS data structures
  * @dev:	device belonging to the sas object
  *
@@ -367,6 +383,20 @@ void sas_remove_host(struct Scsi_Host *shost)
 EXPORT_SYMBOL(sas_remove_host);
 
 /**
+ * sas_get_address - return the SAS address of the device
+ * @sdev: scsi device
+ *
+ * Returns the SAS address of the scsi device
+ */
+u64 sas_get_address(struct scsi_device *sdev)
+{
+	struct sas_end_device *rdev = sas_sdev_to_rdev(sdev);
+
+	return rdev->rphy.identify.sas_address;
+}
+EXPORT_SYMBOL(sas_get_address);
+
+/**
  * sas_tlr_supported - checking TLR bit in vpd 0x90
  * @sdev: scsi device struct
  *
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 4e08d1cd704d..d749da765df1 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -761,7 +761,7 @@ static int sd_setup_discard_cmnd(struct scsi_cmnd *cmd)
 		break;
 
 	default:
-		ret = BLKPREP_KILL;
+		ret = BLKPREP_INVALID;
 		goto out;
 	}
 
@@ -839,7 +839,7 @@ static int sd_setup_write_same_cmnd(struct scsi_cmnd *cmd)
 	int ret;
 
 	if (sdkp->device->no_write_same)
-		return BLKPREP_KILL;
+		return BLKPREP_INVALID;
 
 	BUG_ON(bio_offset(bio) || bio_iovec(bio).bv_len != sdp->sector_size);
 
@@ -2893,7 +2893,7 @@ static int sd_revalidate_disk(struct gendisk *disk)
 	    sdkp->opt_xfer_blocks <= SD_DEF_XFER_BLOCKS &&
 	    sdkp->opt_xfer_blocks * sdp->sector_size >= PAGE_CACHE_SIZE)
 		rw_max = q->limits.io_opt =
-			logical_to_sectors(sdp, sdkp->opt_xfer_blocks);
+			sdkp->opt_xfer_blocks * sdp->sector_size;
 	else
 		rw_max = BLK_DEF_MAX_SECTORS;
 
@@ -3268,8 +3268,8 @@ static int sd_suspend_common(struct device *dev, bool ignore_stop_errors)
 	struct scsi_disk *sdkp = dev_get_drvdata(dev);
 	int ret = 0;
 
-	if (!sdkp)
-		return 0;	/* this can happen */
+	if (!sdkp)	/* E.g.: runtime suspend following sd_remove() */
+		return 0;
 
 	if (sdkp->WCE && sdkp->media_present) {
 		sd_printk(KERN_NOTICE, sdkp, "Synchronizing SCSI cache\n");
@@ -3308,6 +3308,9 @@ static int sd_resume(struct device *dev)
 {
 	struct scsi_disk *sdkp = dev_get_drvdata(dev);
 
+	if (!sdkp)	/* E.g.: runtime resume at the start of sd_probe() */
+		return 0;
+
 	if (!sdkp->device->manage_start_stop)
 		return 0;
 
diff --git a/drivers/scsi/ses.c b/drivers/scsi/ses.c
index 044d06410d4c..53ef1cb6418e 100644
--- a/drivers/scsi/ses.c
+++ b/drivers/scsi/ses.c
@@ -34,6 +34,8 @@
 #include <scsi/scsi_driver.h>
 #include <scsi/scsi_host.h>
 
+#include <scsi/scsi_transport_sas.h>
+
 struct ses_device {
 	unsigned char *page1;
 	unsigned char *page1_types;
@@ -579,31 +581,15 @@ static void ses_enclosure_data_process(struct enclosure_device *edev,
 static void ses_match_to_enclosure(struct enclosure_device *edev,
 				   struct scsi_device *sdev)
 {
-	unsigned char *desc;
 	struct efd efd = {
 		.addr = 0,
 	};
 
 	ses_enclosure_data_process(edev, to_scsi_device(edev->edev.parent), 0);
 
-	if (!sdev->vpd_pg83_len)
-		return;
-
-	desc = sdev->vpd_pg83 + 4;
-	while (desc < sdev->vpd_pg83 + sdev->vpd_pg83_len) {
-		enum scsi_protocol proto = desc[0] >> 4;
-		u8 code_set = desc[0] & 0x0f;
-		u8 piv = desc[1] & 0x80;
-		u8 assoc = (desc[1] & 0x30) >> 4;
-		u8 type = desc[1] & 0x0f;
-		u8 len = desc[3];
-
-		if (piv && code_set == 1 && assoc == 1
-		    && proto == SCSI_PROTOCOL_SAS && type == 3 && len == 8)
-			efd.addr = get_unaligned_be64(&desc[4]);
+	if (is_sas_attached(sdev))
+		efd.addr = sas_get_address(sdev);
 
-		desc += len + 4;
-	}
 	if (efd.addr) {
 		efd.dev = &sdev->sdev_gendev;
 
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 503ab8b46c0b..5e820674432c 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -1261,7 +1261,7 @@ sg_mmap(struct file *filp, struct vm_area_struct *vma)
 	}
 
 	sfp->mmap_called = 1;
-	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
 	vma->vm_private_data = sfp;
 	vma->vm_ops = &sg_mmap_vm_ops;
 	return 0;
diff --git a/drivers/scsi/sr.c b/drivers/scsi/sr.c
index 8bd54a64efd6..64c867405ad4 100644
--- a/drivers/scsi/sr.c
+++ b/drivers/scsi/sr.c
@@ -144,6 +144,9 @@ static int sr_runtime_suspend(struct device *dev)
 {
 	struct scsi_cd *cd = dev_get_drvdata(dev);
 
+	if (!cd)	/* E.g.: runtime suspend following sr_remove() */
+		return 0;
+
 	if (cd->media_present)
 		return -EBUSY;
 	else
@@ -985,6 +988,7 @@ static int sr_remove(struct device *dev)
 	scsi_autopm_get_device(cd->device);
 
 	del_gendisk(cd->disk);
+	dev_set_drvdata(dev, NULL);
 
 	mutex_lock(&sr_ref_mutex);
 	kref_put(&cd->kref, sr_kref_release);
diff --git a/drivers/scsi/st.h b/drivers/scsi/st.h
index b6486b5d8681..8c732c8de015 100644
--- a/drivers/scsi/st.h
+++ b/drivers/scsi/st.h
@@ -148,8 +148,6 @@ struct scsi_tape {
 	int tape_type;
 	int long_timeout;	/* timeout for commands known to take long time */
 
-	unsigned long max_pfn;	/* the maximum page number reachable by the HBA */
-
 	/* Mode characteristics */
 	struct st_modedef modes[ST_NBR_MODES];
 	int current_mode;
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index 3fba42ad9fb8..292c04eec9ad 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -41,6 +41,8 @@
 #include <scsi/scsi_eh.h>
 #include <scsi/scsi_devinfo.h>
 #include <scsi/scsi_dbg.h>
+#include <scsi/scsi_transport_fc.h>
+#include <scsi/scsi_transport.h>
 
 /*
  * All wire protocol details (storage protocol between the guest and the host)
@@ -92,9 +94,8 @@ enum vstor_packet_operation {
  */
 
 struct hv_fc_wwn_packet {
-	bool	primary_active;
-	u8	reserved1;
-	u8	reserved2;
+	u8	primary_active;
+	u8	reserved1[3];
 	u8	primary_port_wwn[8];
 	u8	primary_node_wwn[8];
 	u8	secondary_port_wwn[8];
@@ -164,6 +165,26 @@ static int sense_buffer_size = PRE_WIN8_STORVSC_SENSE_BUFFER_SIZE;
 */
 static int vmstor_proto_version;
 
+#define STORVSC_LOGGING_NONE	0
+#define STORVSC_LOGGING_ERROR	1
+#define STORVSC_LOGGING_WARN	2
+
+static int logging_level = STORVSC_LOGGING_ERROR;
+module_param(logging_level, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(logging_level,
+	"Logging level, 0 - None, 1 - Error (default), 2 - Warning.");
+
+static inline bool do_logging(int level)
+{
+	return logging_level >= level;
+}
+
+#define storvsc_log(dev, level, fmt, ...)			\
+do {								\
+	if (do_logging(level))					\
+		dev_warn(&(dev)->device, fmt, ##__VA_ARGS__);	\
+} while (0)
+
 struct vmscsi_win8_extension {
 	/*
 	 * The following were added in Windows 8
@@ -370,7 +391,7 @@ module_param(storvsc_ringbuffer_size, int, S_IRUGO);
 MODULE_PARM_DESC(storvsc_ringbuffer_size, "Ring buffer size (bytes)");
 
 module_param(storvsc_vcpus_per_sub_channel, int, S_IRUGO);
-MODULE_PARM_DESC(vcpus_per_sub_channel, "Ratio of VCPUs to subchannels");
+MODULE_PARM_DESC(storvsc_vcpus_per_sub_channel, "Ratio of VCPUs to subchannels");
 /*
  * Timeout in seconds for all devices managed by this driver.
  */
@@ -378,6 +399,9 @@ static int storvsc_timeout = 180;
 
 static int msft_blist_flags = BLIST_TRY_VPD_PAGES;
 
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+static struct scsi_transport_template *fc_transport_template;
+#endif
 
 static void storvsc_on_channel_callback(void *context);
 
@@ -437,6 +461,11 @@ struct storvsc_device {
 	/* Used for vsc/vsp channel reset process */
 	struct storvsc_cmd_request init_request;
 	struct storvsc_cmd_request reset_request;
+	/*
+	 * Currently active port and node names for FC devices.
+	 */
+	u64 node_name;
+	u64 port_name;
 };
 
 struct hv_host_device {
@@ -449,19 +478,18 @@ struct hv_host_device {
 struct storvsc_scan_work {
 	struct work_struct work;
 	struct Scsi_Host *host;
-	uint lun;
+	u8 lun;
+	u8 tgt_id;
 };
 
 static void storvsc_device_scan(struct work_struct *work)
 {
 	struct storvsc_scan_work *wrk;
-	uint lun;
 	struct scsi_device *sdev;
 
 	wrk = container_of(work, struct storvsc_scan_work, work);
-	lun = wrk->lun;
 
-	sdev = scsi_device_lookup(wrk->host, 0, 0, lun);
+	sdev = scsi_device_lookup(wrk->host, 0, wrk->tgt_id, wrk->lun);
 	if (!sdev)
 		goto done;
 	scsi_rescan_device(&sdev->sdev_gendev);
@@ -512,7 +540,7 @@ static void storvsc_remove_lun(struct work_struct *work)
 	if (!scsi_host_get(wrk->host))
 		goto done;
 
-	sdev = scsi_device_lookup(wrk->host, 0, 0, wrk->lun);
+	sdev = scsi_device_lookup(wrk->host, 0, wrk->tgt_id, wrk->lun);
 
 	if (sdev) {
 		scsi_remove_device(sdev);
@@ -676,29 +704,36 @@ static void  handle_multichannel_storage(struct hv_device *device, int max_chns)
 	vmbus_are_subchannels_present(device->channel);
 }
 
-static int storvsc_channel_init(struct hv_device *device)
+static void cache_wwn(struct storvsc_device *stor_device,
+		      struct vstor_packet *vstor_packet)
 {
-	struct storvsc_device *stor_device;
-	struct storvsc_cmd_request *request;
-	struct vstor_packet *vstor_packet;
-	int ret, t, i;
-	int max_chns;
-	bool process_sub_channels = false;
+	/*
+	 * Cache the currently active port and node ww names.
+	 */
+	if (vstor_packet->wwn_packet.primary_active) {
+		stor_device->node_name =
+			wwn_to_u64(vstor_packet->wwn_packet.primary_node_wwn);
+		stor_device->port_name =
+			wwn_to_u64(vstor_packet->wwn_packet.primary_port_wwn);
+	} else {
+		stor_device->node_name =
+			wwn_to_u64(vstor_packet->wwn_packet.secondary_node_wwn);
+		stor_device->port_name =
+			wwn_to_u64(vstor_packet->wwn_packet.secondary_port_wwn);
+	}
+}
 
-	stor_device = get_out_stor_device(device);
-	if (!stor_device)
-		return -ENODEV;
 
-	request = &stor_device->init_request;
+static int storvsc_execute_vstor_op(struct hv_device *device,
+				    struct storvsc_cmd_request *request,
+				    bool status_check)
+{
+	struct vstor_packet *vstor_packet;
+	int ret, t;
+
 	vstor_packet = &request->vstor_packet;
 
-	/*
-	 * Now, initiate the vsc/vsp initialization protocol on the open
-	 * channel
-	 */
-	memset(request, 0, sizeof(struct storvsc_cmd_request));
 	init_completion(&request->wait_event);
-	vstor_packet->operation = VSTOR_OPERATION_BEGIN_INITIALIZATION;
 	vstor_packet->flags = REQUEST_COMPLETION_FLAG;
 
 	ret = vmbus_sendpacket(device->channel, vstor_packet,
@@ -708,27 +743,56 @@ static int storvsc_channel_init(struct hv_device *device)
 			       VM_PKT_DATA_INBAND,
 			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
 	if (ret != 0)
-		goto cleanup;
+		return ret;
 
 	t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
-	if (t == 0) {
-		ret = -ETIMEDOUT;
-		goto cleanup;
-	}
+	if (t == 0)
+		return -ETIMEDOUT;
+
+	if (!status_check)
+		return ret;
 
 	if (vstor_packet->operation != VSTOR_OPERATION_COMPLETE_IO ||
-	    vstor_packet->status != 0) {
-		ret = -EINVAL;
-		goto cleanup;
-	}
+	    vstor_packet->status != 0)
+		return -EINVAL;
+
+	return ret;
+}
+
+static int storvsc_channel_init(struct hv_device *device, bool is_fc)
+{
+	struct storvsc_device *stor_device;
+	struct storvsc_cmd_request *request;
+	struct vstor_packet *vstor_packet;
+	int ret, i;
+	int max_chns;
+	bool process_sub_channels = false;
 
+	stor_device = get_out_stor_device(device);
+	if (!stor_device)
+		return -ENODEV;
+
+	request = &stor_device->init_request;
+	vstor_packet = &request->vstor_packet;
+
+	/*
+	 * Now, initiate the vsc/vsp initialization protocol on the open
+	 * channel
+	 */
+	memset(request, 0, sizeof(struct storvsc_cmd_request));
+	vstor_packet->operation = VSTOR_OPERATION_BEGIN_INITIALIZATION;
+	ret = storvsc_execute_vstor_op(device, request, true);
+	if (ret)
+		return ret;
+	/*
+	 * Query host supported protocol version.
+	 */
 
 	for (i = 0; i < ARRAY_SIZE(vmstor_protocols); i++) {
 		/* reuse the packet for version range supported */
 		memset(vstor_packet, 0, sizeof(struct vstor_packet));
 		vstor_packet->operation =
 			VSTOR_OPERATION_QUERY_PROTOCOL_VERSION;
-		vstor_packet->flags = REQUEST_COMPLETION_FLAG;
 
 		vstor_packet->version.major_minor =
 			vmstor_protocols[i].protocol_version;
@@ -737,26 +801,12 @@ static int storvsc_channel_init(struct hv_device *device)
 		 * The revision number is only used in Windows; set it to 0.
 		 */
 		vstor_packet->version.revision = 0;
-
-		ret = vmbus_sendpacket(device->channel, vstor_packet,
-			       (sizeof(struct vstor_packet) -
-				vmscsi_size_delta),
-			       (unsigned long)request,
-			       VM_PKT_DATA_INBAND,
-			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
+		ret = storvsc_execute_vstor_op(device, request, false);
 		if (ret != 0)
-			goto cleanup;
+			return ret;
 
-		t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
-		if (t == 0) {
-			ret = -ETIMEDOUT;
-			goto cleanup;
-		}
-
-		if (vstor_packet->operation != VSTOR_OPERATION_COMPLETE_IO) {
-			ret = -EINVAL;
-			goto cleanup;
-		}
+		if (vstor_packet->operation != VSTOR_OPERATION_COMPLETE_IO)
+			return -EINVAL;
 
 		if (vstor_packet->status == 0) {
 			vmstor_proto_version =
@@ -772,37 +822,15 @@ static int storvsc_channel_init(struct hv_device *device)
 		}
 	}
 
-	if (vstor_packet->status != 0) {
-		ret = -EINVAL;
-		goto cleanup;
-	}
+	if (vstor_packet->status != 0)
+		return -EINVAL;
 
 
 	memset(vstor_packet, 0, sizeof(struct vstor_packet));
 	vstor_packet->operation = VSTOR_OPERATION_QUERY_PROPERTIES;
-	vstor_packet->flags = REQUEST_COMPLETION_FLAG;
-
-	ret = vmbus_sendpacket(device->channel, vstor_packet,
-			       (sizeof(struct vstor_packet) -
-				vmscsi_size_delta),
-			       (unsigned long)request,
-			       VM_PKT_DATA_INBAND,
-			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
-
+	ret = storvsc_execute_vstor_op(device, request, true);
 	if (ret != 0)
-		goto cleanup;
-
-	t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
-	if (t == 0) {
-		ret = -ETIMEDOUT;
-		goto cleanup;
-	}
-
-	if (vstor_packet->operation != VSTOR_OPERATION_COMPLETE_IO ||
-	    vstor_packet->status != 0) {
-		ret = -EINVAL;
-		goto cleanup;
-	}
+		return ret;
 
 	/*
 	 * Check to see if multi-channel support is there.
@@ -818,37 +846,34 @@ static int storvsc_channel_init(struct hv_device *device)
 	stor_device->max_transfer_bytes =
 		vstor_packet->storage_channel_properties.max_transfer_bytes;
 
-	memset(vstor_packet, 0, sizeof(struct vstor_packet));
-	vstor_packet->operation = VSTOR_OPERATION_END_INITIALIZATION;
-	vstor_packet->flags = REQUEST_COMPLETION_FLAG;
-
-	ret = vmbus_sendpacket(device->channel, vstor_packet,
-			       (sizeof(struct vstor_packet) -
-				vmscsi_size_delta),
-			       (unsigned long)request,
-			       VM_PKT_DATA_INBAND,
-			       VMBUS_DATA_PACKET_FLAG_COMPLETION_REQUESTED);
+	if (!is_fc)
+		goto done;
 
+	/*
+	 * For FC devices retrieve FC HBA data.
+	 */
+	memset(vstor_packet, 0, sizeof(struct vstor_packet));
+	vstor_packet->operation = VSTOR_OPERATION_FCHBA_DATA;
+	ret = storvsc_execute_vstor_op(device, request, true);
 	if (ret != 0)
-		goto cleanup;
+		return ret;
 
-	t = wait_for_completion_timeout(&request->wait_event, 5*HZ);
-	if (t == 0) {
-		ret = -ETIMEDOUT;
-		goto cleanup;
-	}
+	/*
+	 * Cache the currently active port and node ww names.
+	 */
+	cache_wwn(stor_device, vstor_packet);
 
-	if (vstor_packet->operation != VSTOR_OPERATION_COMPLETE_IO ||
-	    vstor_packet->status != 0) {
-		ret = -EINVAL;
-		goto cleanup;
-	}
+done:
+
+	memset(vstor_packet, 0, sizeof(struct vstor_packet));
+	vstor_packet->operation = VSTOR_OPERATION_END_INITIALIZATION;
+	ret = storvsc_execute_vstor_op(device, request, true);
+	if (ret != 0)
+		return ret;
 
 	if (process_sub_channels)
 		handle_multichannel_storage(device, max_chns);
 
-
-cleanup:
 	return ret;
 }
 
@@ -915,24 +940,22 @@ static void storvsc_handle_error(struct vmscsi_request *vm_srb,
 
 	wrk->host = host;
 	wrk->lun = vm_srb->lun;
+	wrk->tgt_id = vm_srb->target_id;
 	INIT_WORK(&wrk->work, process_err_fn);
 	schedule_work(&wrk->work);
 }
 
 
-static void storvsc_command_completion(struct storvsc_cmd_request *cmd_request)
+static void storvsc_command_completion(struct storvsc_cmd_request *cmd_request,
+				       struct storvsc_device *stor_dev)
 {
 	struct scsi_cmnd *scmnd = cmd_request->cmd;
-	struct hv_host_device *host_dev = shost_priv(scmnd->device->host);
 	struct scsi_sense_hdr sense_hdr;
 	struct vmscsi_request *vm_srb;
 	struct Scsi_Host *host;
-	struct storvsc_device *stor_dev;
-	struct hv_device *dev = host_dev->dev;
 	u32 payload_sz = cmd_request->payload_sz;
 	void *payload = cmd_request->payload;
 
-	stor_dev = get_in_stor_device(dev);
 	host = stor_dev->host;
 
 	vm_srb = &cmd_request->vstor_packet.vm_srb;
@@ -941,7 +964,8 @@ static void storvsc_command_completion(struct storvsc_cmd_request *cmd_request)
 
 	if (scmnd->result) {
 		if (scsi_normalize_sense(scmnd->sense_buffer,
-				SCSI_SENSE_BUFFERSIZE, &sense_hdr))
+				SCSI_SENSE_BUFFERSIZE, &sense_hdr) &&
+		    do_logging(STORVSC_LOGGING_ERROR))
 			scsi_print_sense_hdr(scmnd->device, "storvsc",
 					     &sense_hdr);
 	}
@@ -961,14 +985,13 @@ static void storvsc_command_completion(struct storvsc_cmd_request *cmd_request)
 		kfree(payload);
 }
 
-static void storvsc_on_io_completion(struct hv_device *device,
+static void storvsc_on_io_completion(struct storvsc_device *stor_device,
 				  struct vstor_packet *vstor_packet,
 				  struct storvsc_cmd_request *request)
 {
-	struct storvsc_device *stor_device;
 	struct vstor_packet *stor_pkt;
+	struct hv_device *device = stor_device->device;
 
-	stor_device = hv_get_drvdata(device);
 	stor_pkt = &request->vstor_packet;
 
 	/*
@@ -995,6 +1018,13 @@ static void storvsc_on_io_completion(struct hv_device *device,
 	stor_pkt->vm_srb.sense_info_length =
 	vstor_packet->vm_srb.sense_info_length;
 
+	if (vstor_packet->vm_srb.scsi_status != 0 ||
+	    vstor_packet->vm_srb.srb_status != SRB_STATUS_SUCCESS)
+		storvsc_log(device, STORVSC_LOGGING_WARN,
+			"cmd 0x%x scsi status 0x%x srb status 0x%x\n",
+			stor_pkt->vm_srb.cdb[0],
+			vstor_packet->vm_srb.scsi_status,
+			vstor_packet->vm_srb.srb_status);
 
 	if ((vstor_packet->vm_srb.scsi_status & 0xFF) == 0x02) {
 		/* CHECK_CONDITION */
@@ -1002,6 +1032,10 @@ static void storvsc_on_io_completion(struct hv_device *device,
 			SRB_STATUS_AUTOSENSE_VALID) {
 			/* autosense data available */
 
+			storvsc_log(device, STORVSC_LOGGING_WARN,
+				"stor pkt %p autosense data valid - len %d\n",
+				request, vstor_packet->vm_srb.sense_info_length);
+
 			memcpy(request->cmd->sense_buffer,
 			       vstor_packet->vm_srb.sense_data,
 			       vstor_packet->vm_srb.sense_info_length);
@@ -1012,7 +1046,7 @@ static void storvsc_on_io_completion(struct hv_device *device,
 	stor_pkt->vm_srb.data_transfer_length =
 	vstor_packet->vm_srb.data_transfer_length;
 
-	storvsc_command_completion(request);
+	storvsc_command_completion(request, stor_device);
 
 	if (atomic_dec_and_test(&stor_device->num_outstanding_req) &&
 		stor_device->drain_notify)
@@ -1021,21 +1055,19 @@ static void storvsc_on_io_completion(struct hv_device *device,
 
 }
 
-static void storvsc_on_receive(struct hv_device *device,
+static void storvsc_on_receive(struct storvsc_device *stor_device,
 			     struct vstor_packet *vstor_packet,
 			     struct storvsc_cmd_request *request)
 {
 	struct storvsc_scan_work *work;
-	struct storvsc_device *stor_device;
 
 	switch (vstor_packet->operation) {
 	case VSTOR_OPERATION_COMPLETE_IO:
-		storvsc_on_io_completion(device, vstor_packet, request);
+		storvsc_on_io_completion(stor_device, vstor_packet, request);
 		break;
 
 	case VSTOR_OPERATION_REMOVE_DEVICE:
 	case VSTOR_OPERATION_ENUMERATE_BUS:
-		stor_device = get_in_stor_device(device);
 		work = kmalloc(sizeof(struct storvsc_scan_work), GFP_ATOMIC);
 		if (!work)
 			return;
@@ -1045,6 +1077,13 @@ static void storvsc_on_receive(struct hv_device *device,
 		schedule_work(&work->work);
 		break;
 
+	case VSTOR_OPERATION_FCHBA_DATA:
+		cache_wwn(stor_device, vstor_packet);
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+		fc_host_node_name(stor_device->host) = stor_device->node_name;
+		fc_host_port_name(stor_device->host) = stor_device->port_name;
+#endif
+		break;
 	default:
 		break;
 	}
@@ -1088,7 +1127,7 @@ static void storvsc_on_channel_callback(void *context)
 					vmscsi_size_delta));
 				complete(&request->wait_event);
 			} else {
-				storvsc_on_receive(device,
+				storvsc_on_receive(stor_device,
 						(struct vstor_packet *)packet,
 						request);
 			}
@@ -1100,7 +1139,8 @@ static void storvsc_on_channel_callback(void *context)
 	return;
 }
 
-static int storvsc_connect_to_vsp(struct hv_device *device, u32 ring_size)
+static int storvsc_connect_to_vsp(struct hv_device *device, u32 ring_size,
+				  bool is_fc)
 {
 	struct vmstorage_channel_properties props;
 	int ret;
@@ -1117,7 +1157,7 @@ static int storvsc_connect_to_vsp(struct hv_device *device, u32 ring_size)
 	if (ret != 0)
 		return ret;
 
-	ret = storvsc_channel_init(device);
+	ret = storvsc_channel_init(device, is_fc);
 
 	return ret;
 }
@@ -1542,6 +1582,7 @@ static int storvsc_probe(struct hv_device *device,
 	struct Scsi_Host *host;
 	struct hv_host_device *host_dev;
 	bool dev_is_ide = ((dev_id->driver_data == IDE_GUID) ? true : false);
+	bool is_fc = ((dev_id->driver_data == SFC_GUID) ? true : false);
 	int target = 0;
 	struct storvsc_device *stor_device;
 	int max_luns_per_target;
@@ -1599,7 +1640,7 @@ static int storvsc_probe(struct hv_device *device,
 	hv_set_drvdata(device, stor_device);
 
 	stor_device->port_number = host->host_no;
-	ret = storvsc_connect_to_vsp(device, storvsc_ringbuffer_size);
+	ret = storvsc_connect_to_vsp(device, storvsc_ringbuffer_size, is_fc);
 	if (ret)
 		goto err_out1;
 
@@ -1611,6 +1652,9 @@ static int storvsc_probe(struct hv_device *device,
 		host->max_lun = STORVSC_FC_MAX_LUNS_PER_TARGET;
 		host->max_id = STORVSC_FC_MAX_TARGETS;
 		host->max_channel = STORVSC_FC_MAX_CHANNELS - 1;
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+		host->transportt = fc_transport_template;
+#endif
 		break;
 
 	case SCSI_GUID:
@@ -1650,6 +1694,12 @@ static int storvsc_probe(struct hv_device *device,
 			goto err_out2;
 		}
 	}
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+	if (host->transportt == fc_transport_template) {
+		fc_host_node_name(host) = stor_device->node_name;
+		fc_host_port_name(host) = stor_device->port_name;
+	}
+#endif
 	return 0;
 
 err_out2:
@@ -1675,6 +1725,10 @@ static int storvsc_remove(struct hv_device *dev)
 	struct storvsc_device *stor_device = hv_get_drvdata(dev);
 	struct Scsi_Host *host = stor_device->host;
 
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+	if (host->transportt == fc_transport_template)
+		fc_remove_host(host);
+#endif
 	scsi_remove_host(host);
 	storvsc_dev_remove(dev);
 	scsi_host_put(host);
@@ -1689,8 +1743,16 @@ static struct hv_driver storvsc_drv = {
 	.remove = storvsc_remove,
 };
 
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+static struct fc_function_template fc_transport_functions = {
+	.show_host_node_name = 1,
+	.show_host_port_name = 1,
+};
+#endif
+
 static int __init storvsc_drv_init(void)
 {
+	int ret;
 
 	/*
 	 * Divide the ring buffer data size (which is 1 page less
@@ -1705,12 +1767,33 @@ static int __init storvsc_drv_init(void)
 		vmscsi_size_delta,
 		sizeof(u64)));
 
-	return vmbus_driver_register(&storvsc_drv);
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+	fc_transport_template = fc_attach_transport(&fc_transport_functions);
+	if (!fc_transport_template)
+		return -ENODEV;
+
+	/*
+	 * Install Hyper-V specific timeout handler.
+	 */
+	fc_transport_template->eh_timed_out = storvsc_eh_timed_out;
+#endif
+
+	ret = vmbus_driver_register(&storvsc_drv);
+
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+	if (ret)
+		fc_release_transport(fc_transport_template);
+#endif
+
+	return ret;
 }
 
 static void __exit storvsc_drv_exit(void)
 {
 	vmbus_driver_unregister(&storvsc_drv);
+#if IS_ENABLED(CONFIG_SCSI_FC_ATTRS)
+	fc_release_transport(fc_transport_template);
+#endif
 }
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/scsi/sun3_scsi.c b/drivers/scsi/sun3_scsi.c
index 22a42836d193..b9de487bbd31 100644
--- a/drivers/scsi/sun3_scsi.c
+++ b/drivers/scsi/sun3_scsi.c
@@ -53,13 +53,12 @@
 #define NCR5380_queue_command           sun3scsi_queue_command
 #define NCR5380_bus_reset               sun3scsi_bus_reset
 #define NCR5380_abort                   sun3scsi_abort
-#define NCR5380_show_info               sun3scsi_show_info
 #define NCR5380_info                    sun3scsi_info
 
 #define NCR5380_dma_read_setup(instance, data, count) \
-        sun3scsi_dma_setup(data, count, 0)
+        sun3scsi_dma_setup(instance, data, count, 0)
 #define NCR5380_dma_write_setup(instance, data, count) \
-        sun3scsi_dma_setup(data, count, 1)
+        sun3scsi_dma_setup(instance, data, count, 1)
 #define NCR5380_dma_residual(instance) \
         sun3scsi_dma_residual(instance)
 #define NCR5380_dma_xfer_len(instance, cmd, phase) \
@@ -86,10 +85,6 @@ module_param(setup_use_tagged_queuing, int, 0);
 static int setup_hostid = -1;
 module_param(setup_hostid, int, 0);
 
-/* #define RESET_BOOT */
-
-#define	AFTER_RESET_DELAY	(HZ/2)
-
 /* ms to wait after hitting dma regs */
 #define SUN3_DMA_DELAY 10
 
@@ -100,11 +95,10 @@ static struct scsi_cmnd *sun3_dma_setup_done;
 static unsigned char *sun3_scsi_regp;
 static volatile struct sun3_dma_regs *dregs;
 static struct sun3_udc_regs *udc_regs;
-static unsigned char *sun3_dma_orig_addr = NULL;
-static unsigned long sun3_dma_orig_count = 0;
-static int sun3_dma_active = 0;
-static unsigned long last_residual = 0;
-static struct Scsi_Host *default_instance;
+static unsigned char *sun3_dma_orig_addr;
+static unsigned long sun3_dma_orig_count;
+static int sun3_dma_active;
+static unsigned long last_residual;
 
 /*
  * NCR 5380 register access functions
@@ -144,50 +138,12 @@ static inline void sun3_udc_write(unsigned short val, unsigned char reg)
 }
 #endif
 
-#ifdef RESET_BOOT
-static void sun3_scsi_reset_boot(struct Scsi_Host *instance)
-{
-	unsigned long end;
-	
-	/*
-	 * Do a SCSI reset to clean up the bus during initialization. No
-	 * messing with the queues, interrupts, or locks necessary here.
-	 */
-
-	printk( "Sun3 SCSI: resetting the SCSI bus..." );
-
-	/* switch off SCSI IRQ - catch an interrupt without IRQ bit set else */
-//       	sun3_disable_irq( IRQ_SUN3_SCSI );
-
-	/* get in phase */
-	NCR5380_write( TARGET_COMMAND_REG,
-		      PHASE_SR_TO_TCR( NCR5380_read(STATUS_REG) ));
-
-	/* assert RST */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE | ICR_ASSERT_RST );
-
-	/* The min. reset hold time is 25us, so 40us should be enough */
-	udelay( 50 );
-
-	/* reset RST and interrupt */
-	NCR5380_write( INITIATOR_COMMAND_REG, ICR_BASE );
-	NCR5380_read( RESET_PARITY_INTERRUPT_REG );
-
-	for( end = jiffies + AFTER_RESET_DELAY; time_before(jiffies, end); )
-		barrier();
-
-	/* switch on SCSI IRQ again */
-//       	sun3_enable_irq( IRQ_SUN3_SCSI );
-
-	printk( " done\n" );
-}
-#endif
-
 // safe bits for the CSR
 #define CSR_GOOD 0x060f
 
-static irqreturn_t scsi_sun3_intr(int irq, void *dummy)
+static irqreturn_t scsi_sun3_intr(int irq, void *dev)
 {
+	struct Scsi_Host *instance = dev;
 	unsigned short csr = dregs->csr;
 	int handled = 0;
 
@@ -196,46 +152,24 @@ static irqreturn_t scsi_sun3_intr(int irq, void *dummy)
 #endif
 
 	if(csr & ~CSR_GOOD) {
-		if(csr & CSR_DMA_BUSERR) {
-			printk("scsi%d: bus error in dma\n", default_instance->host_no);
-		}
-
-		if(csr & CSR_DMA_CONFLICT) {
-			printk("scsi%d: dma conflict\n", default_instance->host_no);
-		}
+		if (csr & CSR_DMA_BUSERR)
+			shost_printk(KERN_ERR, instance, "bus error in DMA\n");
+		if (csr & CSR_DMA_CONFLICT)
+			shost_printk(KERN_ERR, instance, "DMA conflict\n");
 		handled = 1;
 	}
 
 	if(csr & (CSR_SDB_INT | CSR_DMA_INT)) {
-		NCR5380_intr(irq, dummy);
+		NCR5380_intr(irq, dev);
 		handled = 1;
 	}
 
 	return IRQ_RETVAL(handled);
 }
 
-/*
- * Debug stuff - to be called on NMI, or sysrq key. Use at your own risk; 
- * reentering NCR5380_print_status seems to have ugly side effects
- */
-
-/* this doesn't seem to get used at all -- sam */
-#if 0
-void sun3_sun3_debug (void)
-{
-	unsigned long flags;
-
-	if (default_instance) {
-			local_irq_save(flags);
-			NCR5380_print_status(default_instance);
-			local_irq_restore(flags);
-	}
-}
-#endif
-
-
 /* sun3scsi_dma_setup() -- initialize the dma controller for a read/write */
-static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int write_flag)
+static unsigned long sun3scsi_dma_setup(struct Scsi_Host *instance,
+                                void *data, unsigned long count, int write_flag)
 {
 	void *addr;
 
@@ -287,10 +221,9 @@ static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int wri
 	dregs->csr |= CSR_FIFO;
 	
 	if(dregs->fifo_count != count) { 
-		printk("scsi%d: fifo_mismatch %04x not %04x\n",
-		       default_instance->host_no, dregs->fifo_count,
-		       (unsigned int) count);
-		NCR5380_dprint(NDEBUG_DMA, default_instance);
+		shost_printk(KERN_ERR, instance, "FIFO mismatch %04x not %04x\n",
+		             dregs->fifo_count, (unsigned int) count);
+		NCR5380_dprint(NDEBUG_DMA, instance);
 	}
 
 	/* setup udc */
@@ -325,21 +258,6 @@ static unsigned long sun3scsi_dma_setup(void *data, unsigned long count, int wri
 
 }
 
-#ifndef SUN3_SCSI_VME
-static inline unsigned long sun3scsi_dma_count(struct Scsi_Host *instance)
-{
-	unsigned short resid;
-
-	dregs->udc_addr = 0x32; 
-	udelay(SUN3_DMA_DELAY);
-	resid = dregs->udc_data;
-	udelay(SUN3_DMA_DELAY);
-	resid *= 2;
-
-	return (unsigned long) resid;
-}
-#endif
-
 static inline unsigned long sun3scsi_dma_residual(struct Scsi_Host *instance)
 {
 	return last_residual;
@@ -437,7 +355,10 @@ static int sun3scsi_dma_finish(int write_flag)
 		}
 	}
 
-	count = sun3scsi_dma_count(default_instance);
+	dregs->udc_addr = 0x32;
+	udelay(SUN3_DMA_DELAY);
+	count = 2 * dregs->udc_data;
+	udelay(SUN3_DMA_DELAY);
 
 	fifo = dregs->fifo_count;
 	last_residual = fifo;
@@ -502,17 +423,17 @@ static int sun3scsi_dma_finish(int write_flag)
 static struct scsi_host_template sun3_scsi_template = {
 	.module			= THIS_MODULE,
 	.proc_name		= DRV_MODULE_NAME,
-	.show_info		= sun3scsi_show_info,
 	.name			= SUN3_SCSI_NAME,
 	.info			= sun3scsi_info,
 	.queuecommand		= sun3scsi_queue_command,
-	.eh_abort_handler      	= sun3scsi_abort,
-	.eh_bus_reset_handler  	= sun3scsi_bus_reset,
+	.eh_abort_handler	= sun3scsi_abort,
+	.eh_bus_reset_handler	= sun3scsi_bus_reset,
 	.can_queue		= 16,
 	.this_id		= 7,
 	.sg_tablesize		= SG_NONE,
 	.cmd_per_lun		= 2,
-	.use_clustering		= DISABLE_CLUSTERING
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
 };
 
 static int __init sun3_scsi_probe(struct platform_device *pdev)
@@ -591,7 +512,6 @@ static int __init sun3_scsi_probe(struct platform_device *pdev)
 		error = -ENOMEM;
 		goto fail_alloc;
 	}
-	default_instance = instance;
 
 	instance->io_port = (unsigned long)ioaddr;
 	instance->irq = irq->start;
@@ -600,7 +520,9 @@ static int __init sun3_scsi_probe(struct platform_device *pdev)
 	host_flags |= setup_use_tagged_queuing > 0 ? FLAG_TAGGED_QUEUING : 0;
 #endif
 
-	NCR5380_init(instance, host_flags);
+	error = NCR5380_init(instance, host_flags);
+	if (error)
+		goto fail_init;
 
 	error = request_irq(instance->irq, scsi_sun3_intr, 0,
 	                    "NCR5380", instance);
@@ -631,9 +553,7 @@ static int __init sun3_scsi_probe(struct platform_device *pdev)
 	dregs->ivect = VME_DATA24 | (instance->irq & 0xff);
 #endif
 
-#ifdef RESET_BOOT
-	sun3_scsi_reset_boot(instance);
-#endif
+	NCR5380_maybe_reset_bus(instance);
 
 	error = scsi_add_host(instance, NULL);
 	if (error)
@@ -649,6 +569,7 @@ fail_host:
 		free_irq(instance->irq, instance);
 fail_irq:
 	NCR5380_exit(instance);
+fail_init:
 	scsi_host_put(instance);
 fail_alloc:
 	if (udc_regs)
diff --git a/drivers/scsi/t128.c b/drivers/scsi/t128.c
index 87828acbf7c6..4615fda60dbd 100644
--- a/drivers/scsi/t128.c
+++ b/drivers/scsi/t128.c
@@ -68,14 +68,11 @@
  * 15 9-11
  */
  
-#include <linux/signal.h>
 #include <linux/io.h>
 #include <linux/blkdev.h>
 #include <linux/interrupt.h>
-#include <linux/stat.h>
 #include <linux/init.h>
 #include <linux/module.h>
-#include <linux/delay.h>
 
 #include <scsi/scsi_host.h>
 #include "t128.h"
@@ -126,7 +123,7 @@ static struct signature {
 
 static int __init t128_setup(char *str)
 {
-    static int commandline_current = 0;
+	static int commandline_current;
     int i;
     int ints[10];
 
@@ -165,7 +162,7 @@ __setup("t128=", t128_setup);
 
 static int __init t128_detect(struct scsi_host_template *tpnt)
 {
-    static int current_override = 0, current_base = 0;
+	static int current_override, current_base;
     struct Scsi_Host *instance;
     unsigned long base;
     void __iomem *p;
@@ -182,9 +179,8 @@ static int __init t128_detect(struct scsi_host_template *tpnt)
 		base = 0;
 	} else 
 	    for (; !base && (current_base < NO_BASES); ++current_base) {
-#if (TDEBUG & TDEBUG_INIT)
-    printk("scsi-t128 : probing address %08x\n", bases[current_base].address);
-#endif
+		dprintk(NDEBUG_INIT, "t128: probing address 0x%08x\n",
+		        bases[current_base].address);
 		if (bases[current_base].noauto)
 			continue;
 		p = ioremap(bases[current_base].address, 0x2000);
@@ -195,17 +191,13 @@ static int __init t128_detect(struct scsi_host_template *tpnt)
 					signatures[sig].string,
 					strlen(signatures[sig].string))) {
 			base = bases[current_base].address;
-#if (TDEBUG & TDEBUG_INIT)
-			printk("scsi-t128 : detected board.\n");
-#endif
+			dprintk(NDEBUG_INIT, "t128: detected board\n");
 			goto found;
 		    }
 		iounmap(p);
 	    }
 
-#if defined(TDEBUG) && (TDEBUG & TDEBUG_INIT)
-	printk("scsi-t128 : base = %08x\n", (unsigned int) base);
-#endif
+	dprintk(NDEBUG_INIT, "t128: base = 0x%08x\n", (unsigned int)base);
 
 	if (!base)
 	    break;
@@ -213,12 +205,15 @@ static int __init t128_detect(struct scsi_host_template *tpnt)
 found:
 	instance = scsi_register (tpnt, sizeof(struct NCR5380_hostdata));
 	if(instance == NULL)
-		break;
-		
+		goto out_unmap;
+
 	instance->base = base;
 	((struct NCR5380_hostdata *)instance->hostdata)->base = p;
 
-	NCR5380_init(instance, 0);
+	if (NCR5380_init(instance, 0))
+		goto out_unregister;
+
+	NCR5380_maybe_reset_bus(instance);
 
 	if (overrides[current_override].irq != IRQ_AUTO)
 	    instance->irq = overrides[current_override].irq;
@@ -242,27 +237,30 @@ found:
 	    printk("scsi%d : please jumper the board for a free IRQ.\n", instance->host_no);
 	}
 
-#if defined(TDEBUG) && (TDEBUG & TDEBUG_INIT)
-	printk("scsi%d : irq = %d\n", instance->host_no, instance->irq);
-#endif
+	dprintk(NDEBUG_INIT, "scsi%d: irq = %d\n",
+	        instance->host_no, instance->irq);
 
 	++current_override;
 	++count;
     }
     return count;
+
+out_unregister:
+	scsi_unregister(instance);
+out_unmap:
+	iounmap(p);
+	return count;
 }
 
 static int t128_release(struct Scsi_Host *shost)
 {
-	NCR5380_local_declare();
-	NCR5380_setup(shost);
+	struct NCR5380_hostdata *hostdata = shost_priv(shost);
+
 	if (shost->irq != NO_IRQ)
 		free_irq(shost->irq, shost);
 	NCR5380_exit(shost);
-	if (shost->io_port && shost->n_io_port)
-		release_region(shost->io_port, shost->n_io_port);
 	scsi_unregister(shost);
-	iounmap(base);
+	iounmap(hostdata->base);
 	return 0;
 }
 
@@ -308,14 +306,14 @@ static int t128_biosparam(struct scsi_device *sdev, struct block_device *bdev,
  * 	timeout.
  */
 
-static inline int NCR5380_pread (struct Scsi_Host *instance, unsigned char *dst,
-    int len) {
-    NCR5380_local_declare();
-    void __iomem *reg;
+static inline int
+NCR5380_pread(struct Scsi_Host *instance, unsigned char *dst, int len)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	void __iomem *reg, *base = hostdata->base;
     unsigned char *d = dst;
     register int i = len;
 
-    NCR5380_setup(instance);
     reg = base + T_DATA_REG_OFFSET;
 
 #if 0
@@ -354,14 +352,14 @@ static inline int NCR5380_pread (struct Scsi_Host *instance, unsigned char *dst,
  * 	timeout.
  */
 
-static inline int NCR5380_pwrite (struct Scsi_Host *instance, unsigned char *src,
-    int len) {
-    NCR5380_local_declare();
-    void __iomem *reg;
+static inline int
+NCR5380_pwrite(struct Scsi_Host *instance, unsigned char *src, int len)
+{
+	struct NCR5380_hostdata *hostdata = shost_priv(instance);
+	void __iomem *reg, *base = hostdata->base;
     unsigned char *s = src;
     register int i = len;
 
-    NCR5380_setup(instance);
     reg = base + T_DATA_REG_OFFSET;
 
 #if 0
@@ -392,21 +390,23 @@ MODULE_LICENSE("GPL");
 #include "NCR5380.c"
 
 static struct scsi_host_template driver_template = {
-	.name           = "Trantor T128/T128F/T228",
-	.detect         = t128_detect,
-	.release        = t128_release,
-	.proc_name      = "t128",
-	.show_info      = t128_show_info,
-	.write_info     = t128_write_info,
-	.info           = t128_info,
-	.queuecommand   = t128_queue_command,
-	.eh_abort_handler = t128_abort,
-	.eh_bus_reset_handler    = t128_bus_reset,
-	.bios_param     = t128_biosparam,
-	.can_queue      = CAN_QUEUE,
-        .this_id        = 7,
-	.sg_tablesize   = SG_ALL,
-	.cmd_per_lun    = CMD_PER_LUN,
-	.use_clustering = DISABLE_CLUSTERING,
+	.name			= "Trantor T128/T128F/T228",
+	.detect			= t128_detect,
+	.release		= t128_release,
+	.proc_name		= "t128",
+	.show_info		= t128_show_info,
+	.write_info		= t128_write_info,
+	.info			= t128_info,
+	.queuecommand		= t128_queue_command,
+	.eh_abort_handler	= t128_abort,
+	.eh_bus_reset_handler	= t128_bus_reset,
+	.bios_param		= t128_biosparam,
+	.can_queue		= 32,
+	.this_id		= 7,
+	.sg_tablesize		= SG_ALL,
+	.cmd_per_lun		= 2,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.cmd_size		= NCR5380_CMD_SIZE,
+	.max_sectors		= 128,
 };
 #include "scsi_module.c"
diff --git a/drivers/scsi/t128.h b/drivers/scsi/t128.h
index 2c7371454dfd..dd16d85497e1 100644
--- a/drivers/scsi/t128.h
+++ b/drivers/scsi/t128.h
@@ -23,10 +23,6 @@
 #ifndef T128_H
 #define T128_H
 
-#define TDEBUG		0
-#define TDEBUG_INIT	0x1
-#define TDEBUG_TRANSFER 0x2
-
 /*
  * The trantor boards are memory mapped. They use an NCR5380 or
  * equivalent (my sample board had part second sourced from ZILOG).
@@ -71,44 +67,18 @@
 
 #define T_DATA_REG_OFFSET	0x1e00	/* rw 512 bytes long */
 
-#ifndef ASM
-
-#ifndef CMD_PER_LUN
-#define CMD_PER_LUN 2
-#endif
-
-#ifndef CAN_QUEUE
-#define CAN_QUEUE 32
-#endif
-
 #define NCR5380_implementation_fields \
     void __iomem *base
 
-#define NCR5380_local_declare() \
-    void __iomem *base
-
-#define NCR5380_setup(instance) \
-    base = ((struct NCR5380_hostdata *)(instance->hostdata))->base
+#define T128_address(reg) \
+	(((struct NCR5380_hostdata *)shost_priv(instance))->base + T_5380_OFFSET + ((reg) * 0x20))
 
-#define T128_address(reg) (base + T_5380_OFFSET + ((reg) * 0x20))
-
-#if !(TDEBUG & TDEBUG_TRANSFER)
 #define NCR5380_read(reg) readb(T128_address(reg))
 #define NCR5380_write(reg, value) writeb((value),(T128_address(reg)))
-#else
-#define NCR5380_read(reg)						\
-    (((unsigned char) printk("scsi%d : read register %d at address %08x\n"\
-    , instance->hostno, (reg), T128_address(reg))), readb(T128_address(reg)))
-
-#define NCR5380_write(reg, value) {					\
-    printk("scsi%d : write %02x to register %d at address %08x\n",	\
-	    instance->hostno, (value), (reg), T128_address(reg));	\
-    writeb((value), (T128_address(reg)));				\
-}
-#endif
+
+#define NCR5380_dma_xfer_len(instance, cmd, phase)	(cmd->transfersize)
 
 #define NCR5380_intr t128_intr
-#define do_NCR5380_intr do_t128_intr
 #define NCR5380_queue_command t128_queue_command
 #define NCR5380_abort t128_abort
 #define NCR5380_bus_reset t128_bus_reset
@@ -121,5 +91,4 @@
 
 #define T128_IRQS 0xc4a8
 
-#endif /* ndef ASM */
 #endif /* T128_H */
diff --git a/drivers/scsi/ufs/ufshcd-pltfrm.c b/drivers/scsi/ufs/ufshcd-pltfrm.c
index 9714f2a8b329..d2a7b127b05c 100644
--- a/drivers/scsi/ufs/ufshcd-pltfrm.c
+++ b/drivers/scsi/ufs/ufshcd-pltfrm.c
@@ -333,7 +333,7 @@ int ufshcd_pltfrm_init(struct platform_device *pdev,
 
 	err = ufshcd_init(hba, mmio_base, irq);
 	if (err) {
-		dev_err(dev, "Intialization failed\n");
+		dev_err(dev, "Initialization failed\n");
 		goto out_disable_rpm;
 	}
 
diff --git a/drivers/scsi/vmw_pvscsi.c b/drivers/scsi/vmw_pvscsi.c
index 0f133c1817de..6164634aff18 100644
--- a/drivers/scsi/vmw_pvscsi.c
+++ b/drivers/scsi/vmw_pvscsi.c
@@ -349,9 +349,9 @@ static void pvscsi_create_sg(struct pvscsi_ctx *ctx,
  * Map all data buffers for a command into PCI space and
  * setup the scatter/gather list if needed.
  */
-static void pvscsi_map_buffers(struct pvscsi_adapter *adapter,
-			       struct pvscsi_ctx *ctx, struct scsi_cmnd *cmd,
-			       struct PVSCSIRingReqDesc *e)
+static int pvscsi_map_buffers(struct pvscsi_adapter *adapter,
+			      struct pvscsi_ctx *ctx, struct scsi_cmnd *cmd,
+			      struct PVSCSIRingReqDesc *e)
 {
 	unsigned count;
 	unsigned bufflen = scsi_bufflen(cmd);
@@ -360,18 +360,30 @@ static void pvscsi_map_buffers(struct pvscsi_adapter *adapter,
 	e->dataLen = bufflen;
 	e->dataAddr = 0;
 	if (bufflen == 0)
-		return;
+		return 0;
 
 	sg = scsi_sglist(cmd);
 	count = scsi_sg_count(cmd);
 	if (count != 0) {
 		int segs = scsi_dma_map(cmd);
-		if (segs > 1) {
+
+		if (segs == -ENOMEM) {
+			scmd_printk(KERN_ERR, cmd,
+				    "vmw_pvscsi: Failed to map cmd sglist for DMA.\n");
+			return -ENOMEM;
+		} else if (segs > 1) {
 			pvscsi_create_sg(ctx, sg, segs);
 
 			e->flags |= PVSCSI_FLAG_CMD_WITH_SG_LIST;
 			ctx->sglPA = pci_map_single(adapter->dev, ctx->sgl,
 						    SGL_SIZE, PCI_DMA_TODEVICE);
+			if (pci_dma_mapping_error(adapter->dev, ctx->sglPA)) {
+				scmd_printk(KERN_ERR, cmd,
+					    "vmw_pvscsi: Failed to map ctx sglist for DMA.\n");
+				scsi_dma_unmap(cmd);
+				ctx->sglPA = 0;
+				return -ENOMEM;
+			}
 			e->dataAddr = ctx->sglPA;
 		} else
 			e->dataAddr = sg_dma_address(sg);
@@ -382,8 +394,15 @@ static void pvscsi_map_buffers(struct pvscsi_adapter *adapter,
 		 */
 		ctx->dataPA = pci_map_single(adapter->dev, sg, bufflen,
 					     cmd->sc_data_direction);
+		if (pci_dma_mapping_error(adapter->dev, ctx->dataPA)) {
+			scmd_printk(KERN_ERR, cmd,
+				    "vmw_pvscsi: Failed to map direct data buffer for DMA.\n");
+			return -ENOMEM;
+		}
 		e->dataAddr = ctx->dataPA;
 	}
+
+	return 0;
 }
 
 static void pvscsi_unmap_buffers(const struct pvscsi_adapter *adapter,
@@ -690,6 +709,12 @@ static int pvscsi_queue_ring(struct pvscsi_adapter *adapter,
 		ctx->sensePA = pci_map_single(adapter->dev, cmd->sense_buffer,
 					      SCSI_SENSE_BUFFERSIZE,
 					      PCI_DMA_FROMDEVICE);
+		if (pci_dma_mapping_error(adapter->dev, ctx->sensePA)) {
+			scmd_printk(KERN_ERR, cmd,
+				    "vmw_pvscsi: Failed to map sense buffer for DMA.\n");
+			ctx->sensePA = 0;
+			return -ENOMEM;
+		}
 		e->senseAddr = ctx->sensePA;
 		e->senseLen = SCSI_SENSE_BUFFERSIZE;
 	} else {
@@ -711,7 +736,15 @@ static int pvscsi_queue_ring(struct pvscsi_adapter *adapter,
 	else
 		e->flags = 0;
 
-	pvscsi_map_buffers(adapter, ctx, cmd, e);
+	if (pvscsi_map_buffers(adapter, ctx, cmd, e) != 0) {
+		if (cmd->sense_buffer) {
+			pci_unmap_single(adapter->dev, ctx->sensePA,
+					 SCSI_SENSE_BUFFERSIZE,
+					 PCI_DMA_FROMDEVICE);
+			ctx->sensePA = 0;
+		}
+		return -ENOMEM;
+	}
 
 	e->context = pvscsi_map_context(adapter, ctx);
 
diff --git a/drivers/scsi/vmw_pvscsi.h b/drivers/scsi/vmw_pvscsi.h
index ee16f0c5c47d..12712c92f37a 100644
--- a/drivers/scsi/vmw_pvscsi.h
+++ b/drivers/scsi/vmw_pvscsi.h
@@ -26,7 +26,7 @@
 
 #include <linux/types.h>
 
-#define PVSCSI_DRIVER_VERSION_STRING   "1.0.5.0-k"
+#define PVSCSI_DRIVER_VERSION_STRING   "1.0.6.0-k"
 
 #define PVSCSI_MAX_NUM_SG_ENTRIES_PER_SEGMENT 128
 
diff --git a/drivers/sh/clk/core.c b/drivers/sh/clk/core.c
index be56b22ca941..92863e3818e5 100644
--- a/drivers/sh/clk/core.c
+++ b/drivers/sh/clk/core.c
@@ -469,6 +469,9 @@ void clk_enable_init_clocks(void)
 
 unsigned long clk_get_rate(struct clk *clk)
 {
+	if (!clk)
+		return 0;
+
 	return clk->rate;
 }
 EXPORT_SYMBOL_GPL(clk_get_rate);
@@ -478,6 +481,9 @@ int clk_set_rate(struct clk *clk, unsigned long rate)
 	int ret = -EOPNOTSUPP;
 	unsigned long flags;
 
+	if (!clk)
+		return 0;
+
 	spin_lock_irqsave(&clock_lock, flags);
 
 	if (likely(clk->ops && clk->ops->set_rate)) {
@@ -535,12 +541,18 @@ EXPORT_SYMBOL_GPL(clk_set_parent);
 
 struct clk *clk_get_parent(struct clk *clk)
 {
+	if (!clk)
+		return NULL;
+
 	return clk->parent;
 }
 EXPORT_SYMBOL_GPL(clk_get_parent);
 
 long clk_round_rate(struct clk *clk, unsigned long rate)
 {
+	if (!clk)
+		return 0;
+
 	if (likely(clk->ops && clk->ops->round_rate)) {
 		unsigned long flags, rounded;
 
@@ -555,94 +567,6 @@ long clk_round_rate(struct clk *clk, unsigned long rate)
 }
 EXPORT_SYMBOL_GPL(clk_round_rate);
 
-long clk_round_parent(struct clk *clk, unsigned long target,
-		      unsigned long *best_freq, unsigned long *parent_freq,
-		      unsigned int div_min, unsigned int div_max)
-{
-	struct cpufreq_frequency_table *freq, *best = NULL;
-	unsigned long error = ULONG_MAX, freq_high, freq_low, div;
-	struct clk *parent = clk_get_parent(clk);
-
-	if (!parent) {
-		*parent_freq = 0;
-		*best_freq = clk_round_rate(clk, target);
-		return abs(target - *best_freq);
-	}
-
-	cpufreq_for_each_valid_entry(freq, parent->freq_table) {
-		if (unlikely(freq->frequency / target <= div_min - 1)) {
-			unsigned long freq_max;
-
-			freq_max = (freq->frequency + div_min / 2) / div_min;
-			if (error > target - freq_max) {
-				error = target - freq_max;
-				best = freq;
-				if (best_freq)
-					*best_freq = freq_max;
-			}
-
-			pr_debug("too low freq %u, error %lu\n", freq->frequency,
-				 target - freq_max);
-
-			if (!error)
-				break;
-
-			continue;
-		}
-
-		if (unlikely(freq->frequency / target >= div_max)) {
-			unsigned long freq_min;
-
-			freq_min = (freq->frequency + div_max / 2) / div_max;
-			if (error > freq_min - target) {
-				error = freq_min - target;
-				best = freq;
-				if (best_freq)
-					*best_freq = freq_min;
-			}
-
-			pr_debug("too high freq %u, error %lu\n", freq->frequency,
-				 freq_min - target);
-
-			if (!error)
-				break;
-
-			continue;
-		}
-
-		div = freq->frequency / target;
-		freq_high = freq->frequency / div;
-		freq_low = freq->frequency / (div + 1);
-
-		if (freq_high - target < error) {
-			error = freq_high - target;
-			best = freq;
-			if (best_freq)
-				*best_freq = freq_high;
-		}
-
-		if (target - freq_low < error) {
-			error = target - freq_low;
-			best = freq;
-			if (best_freq)
-				*best_freq = freq_low;
-		}
-
-		pr_debug("%u / %lu = %lu, / %lu = %lu, best %lu, parent %u\n",
-			 freq->frequency, div, freq_high, div + 1, freq_low,
-			 *best_freq, best->frequency);
-
-		if (!error)
-			break;
-	}
-
-	if (parent_freq)
-		*parent_freq = best->frequency;
-
-	return error;
-}
-EXPORT_SYMBOL_GPL(clk_round_parent);
-
 #ifdef CONFIG_PM
 static void clks_core_resume(void)
 {
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index 91a003011acf..a9bac3bf20de 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -34,7 +34,7 @@ static struct pm_clk_notifier_block platform_bus_notifier = {
 
 static int __init sh_pm_runtime_init(void)
 {
-	if (IS_ENABLED(CONFIG_ARCH_SHMOBILE)) {
+	if (IS_ENABLED(CONFIG_OF) && IS_ENABLED(CONFIG_ARCH_SHMOBILE)) {
 		if (!of_find_compatible_node(NULL, NULL,
 					     "renesas,cpg-mstp-clocks"))
 			return 0;
diff --git a/drivers/soc/Kconfig b/drivers/soc/Kconfig
index 4e853ed2c82b..88260205a261 100644
--- a/drivers/soc/Kconfig
+++ b/drivers/soc/Kconfig
@@ -1,10 +1,13 @@
 menu "SOC (System On Chip) specific Drivers"
 
+source "drivers/soc/bcm/Kconfig"
 source "drivers/soc/brcmstb/Kconfig"
+source "drivers/soc/fsl/qe/Kconfig"
 source "drivers/soc/mediatek/Kconfig"
 source "drivers/soc/qcom/Kconfig"
 source "drivers/soc/rockchip/Kconfig"
 source "drivers/soc/sunxi/Kconfig"
+source "drivers/soc/tegra/Kconfig"
 source "drivers/soc/ti/Kconfig"
 source "drivers/soc/versatile/Kconfig"
 
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
index f2ba2e932ae1..2afdc74f7491 100644
--- a/drivers/soc/Makefile
+++ b/drivers/soc/Makefile
@@ -2,8 +2,11 @@
 # Makefile for the Linux Kernel SOC specific device drivers.
 #
 
+obj-y				+= bcm/
 obj-$(CONFIG_SOC_BRCMSTB)	+= brcmstb/
+obj-$(CONFIG_ARCH_DOVE)		+= dove/
 obj-$(CONFIG_MACH_DOVE)		+= dove/
+obj-y				+= fsl/
 obj-$(CONFIG_ARCH_MEDIATEK)	+= mediatek/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
diff --git a/drivers/soc/bcm/Kconfig b/drivers/soc/bcm/Kconfig
new file mode 100644
index 000000000000..3066edea184d
--- /dev/null
+++ b/drivers/soc/bcm/Kconfig
@@ -0,0 +1,9 @@
+config RASPBERRYPI_POWER
+	bool "Raspberry Pi power domain driver"
+	depends on ARCH_BCM2835 || COMPILE_TEST
+	depends on RASPBERRYPI_FIRMWARE=y
+	select PM_GENERIC_DOMAINS if PM
+	select PM_GENERIC_DOMAINS_OF if PM
+	help
+	  This enables support for the RPi power domains which can be enabled
+	  or disabled via the RPi firmware.
diff --git a/drivers/soc/bcm/Makefile b/drivers/soc/bcm/Makefile
new file mode 100644
index 000000000000..63aa3eb23087
--- /dev/null
+++ b/drivers/soc/bcm/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_RASPBERRYPI_POWER)	+= raspberrypi-power.o
diff --git a/drivers/soc/bcm/raspberrypi-power.c b/drivers/soc/bcm/raspberrypi-power.c
new file mode 100644
index 000000000000..fe96a8b956fb
--- /dev/null
+++ b/drivers/soc/bcm/raspberrypi-power.c
@@ -0,0 +1,247 @@
+/* (C) 2015 Pengutronix, Alexander Aring <aar@pengutronix.de>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Authors:
+ * Alexander Aring <aar@pengutronix.de>
+ * Eric Anholt <eric@anholt.net>
+ */
+
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_domain.h>
+#include <dt-bindings/power/raspberrypi-power.h>
+#include <soc/bcm2835/raspberrypi-firmware.h>
+
+/*
+ * Firmware indices for the old power domains interface.  Only a few
+ * of them were actually implemented.
+ */
+#define RPI_OLD_POWER_DOMAIN_USB		3
+#define RPI_OLD_POWER_DOMAIN_V3D		10
+
+struct rpi_power_domain {
+	u32 domain;
+	bool enabled;
+	bool old_interface;
+	struct generic_pm_domain base;
+	struct rpi_firmware *fw;
+};
+
+struct rpi_power_domains {
+	bool has_new_interface;
+	struct genpd_onecell_data xlate;
+	struct rpi_firmware *fw;
+	struct rpi_power_domain domains[RPI_POWER_DOMAIN_COUNT];
+};
+
+/*
+ * Packet definition used by RPI_FIRMWARE_SET_POWER_STATE and
+ * RPI_FIRMWARE_SET_DOMAIN_STATE
+ */
+struct rpi_power_domain_packet {
+	u32 domain;
+	u32 on;
+} __packet;
+
+/*
+ * Asks the firmware to enable or disable power on a specific power
+ * domain.
+ */
+static int rpi_firmware_set_power(struct rpi_power_domain *rpi_domain, bool on)
+{
+	struct rpi_power_domain_packet packet;
+
+	packet.domain = rpi_domain->domain;
+	packet.on = on;
+	return rpi_firmware_property(rpi_domain->fw,
+				     rpi_domain->old_interface ?
+				     RPI_FIRMWARE_SET_POWER_STATE :
+				     RPI_FIRMWARE_SET_DOMAIN_STATE,
+				     &packet, sizeof(packet));
+}
+
+static int rpi_domain_off(struct generic_pm_domain *domain)
+{
+	struct rpi_power_domain *rpi_domain =
+		container_of(domain, struct rpi_power_domain, base);
+
+	return rpi_firmware_set_power(rpi_domain, false);
+}
+
+static int rpi_domain_on(struct generic_pm_domain *domain)
+{
+	struct rpi_power_domain *rpi_domain =
+		container_of(domain, struct rpi_power_domain, base);
+
+	return rpi_firmware_set_power(rpi_domain, true);
+}
+
+static void rpi_common_init_power_domain(struct rpi_power_domains *rpi_domains,
+					 int xlate_index, const char *name)
+{
+	struct rpi_power_domain *dom = &rpi_domains->domains[xlate_index];
+
+	dom->fw = rpi_domains->fw;
+
+	dom->base.name = name;
+	dom->base.power_on = rpi_domain_on;
+	dom->base.power_off = rpi_domain_off;
+
+	/*
+	 * Treat all power domains as off at boot.
+	 *
+	 * The firmware itself may be keeping some domains on, but
+	 * from Linux's perspective all we control is the refcounts
+	 * that we give to the firmware, and we can't ask the firmware
+	 * to turn off something that we haven't ourselves turned on.
+	 */
+	pm_genpd_init(&dom->base, NULL, true);
+
+	rpi_domains->xlate.domains[xlate_index] = &dom->base;
+}
+
+static void rpi_init_power_domain(struct rpi_power_domains *rpi_domains,
+				  int xlate_index, const char *name)
+{
+	struct rpi_power_domain *dom = &rpi_domains->domains[xlate_index];
+
+	if (!rpi_domains->has_new_interface)
+		return;
+
+	/* The DT binding index is the firmware's domain index minus one. */
+	dom->domain = xlate_index + 1;
+
+	rpi_common_init_power_domain(rpi_domains, xlate_index, name);
+}
+
+static void rpi_init_old_power_domain(struct rpi_power_domains *rpi_domains,
+				      int xlate_index, int domain,
+				      const char *name)
+{
+	struct rpi_power_domain *dom = &rpi_domains->domains[xlate_index];
+
+	dom->old_interface = true;
+	dom->domain = domain;
+
+	rpi_common_init_power_domain(rpi_domains, xlate_index, name);
+}
+
+/*
+ * Detects whether the firmware supports the new power domains interface.
+ *
+ * The firmware doesn't actually return an error on an unknown tag,
+ * and just skips over it, so we do the detection by putting an
+ * unexpected value in the return field and checking if it was
+ * unchanged.
+ */
+static bool
+rpi_has_new_domain_support(struct rpi_power_domains *rpi_domains)
+{
+	struct rpi_power_domain_packet packet;
+	int ret;
+
+	packet.domain = RPI_POWER_DOMAIN_ARM;
+	packet.on = ~0;
+
+	ret = rpi_firmware_property(rpi_domains->fw,
+				    RPI_FIRMWARE_GET_DOMAIN_STATE,
+				    &packet, sizeof(packet));
+
+	return ret == 0 && packet.on != ~0;
+}
+
+static int rpi_power_probe(struct platform_device *pdev)
+{
+	struct device_node *fw_np;
+	struct device *dev = &pdev->dev;
+	struct rpi_power_domains *rpi_domains;
+
+	rpi_domains = devm_kzalloc(dev, sizeof(*rpi_domains), GFP_KERNEL);
+	if (!rpi_domains)
+		return -ENOMEM;
+
+	rpi_domains->xlate.domains =
+		devm_kzalloc(dev, sizeof(*rpi_domains->xlate.domains) *
+			     RPI_POWER_DOMAIN_COUNT, GFP_KERNEL);
+	if (!rpi_domains->xlate.domains)
+		return -ENOMEM;
+
+	rpi_domains->xlate.num_domains = RPI_POWER_DOMAIN_COUNT;
+
+	fw_np = of_parse_phandle(pdev->dev.of_node, "firmware", 0);
+	if (!fw_np) {
+		dev_err(&pdev->dev, "no firmware node\n");
+		return -ENODEV;
+	}
+
+	rpi_domains->fw = rpi_firmware_get(fw_np);
+	of_node_put(fw_np);
+	if (!rpi_domains->fw)
+		return -EPROBE_DEFER;
+
+	rpi_domains->has_new_interface =
+		rpi_has_new_domain_support(rpi_domains);
+
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_I2C0, "I2C0");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_I2C1, "I2C1");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_I2C2, "I2C2");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_VIDEO_SCALER,
+			      "VIDEO_SCALER");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_VPU1, "VPU1");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_HDMI, "HDMI");
+
+	/*
+	 * Use the old firmware interface for USB power, so that we
+	 * can turn it on even if the firmware hasn't been updated.
+	 */
+	rpi_init_old_power_domain(rpi_domains, RPI_POWER_DOMAIN_USB,
+				  RPI_OLD_POWER_DOMAIN_USB, "USB");
+
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_VEC, "VEC");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_JPEG, "JPEG");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_H264, "H264");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_V3D, "V3D");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_ISP, "ISP");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_UNICAM0, "UNICAM0");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_UNICAM1, "UNICAM1");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_CCP2RX, "CCP2RX");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_CSI2, "CSI2");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_CPI, "CPI");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_DSI0, "DSI0");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_DSI1, "DSI1");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_TRANSPOSER,
+			      "TRANSPOSER");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_CCP2TX, "CCP2TX");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_CDP, "CDP");
+	rpi_init_power_domain(rpi_domains, RPI_POWER_DOMAIN_ARM, "ARM");
+
+	of_genpd_add_provider_onecell(dev->of_node, &rpi_domains->xlate);
+
+	platform_set_drvdata(pdev, rpi_domains);
+
+	return 0;
+}
+
+static const struct of_device_id rpi_power_of_match[] = {
+	{ .compatible = "raspberrypi,bcm2835-power", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, rpi_power_of_match);
+
+static struct platform_driver rpi_power_driver = {
+	.driver = {
+		.name = "raspberrypi-power",
+		.of_match_table = rpi_power_of_match,
+	},
+	.probe		= rpi_power_probe,
+};
+builtin_platform_driver(rpi_power_driver);
+
+MODULE_AUTHOR("Alexander Aring <aar@pengutronix.de>");
+MODULE_AUTHOR("Eric Anholt <eric@anholt.net>");
+MODULE_DESCRIPTION("Raspberry Pi power domain driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/soc/dove/pmu.c b/drivers/soc/dove/pmu.c
index abd087917f80..039374e9fdc0 100644
--- a/drivers/soc/dove/pmu.c
+++ b/drivers/soc/dove/pmu.c
@@ -305,6 +305,49 @@ static int __init dove_init_pmu_irq(struct pmu_data *pmu, int irq)
 	return 0;
 }
 
+int __init dove_init_pmu_legacy(const struct dove_pmu_initdata *initdata)
+{
+	const struct dove_pmu_domain_initdata *domain_initdata;
+	struct pmu_data *pmu;
+	int ret;
+
+	pmu = kzalloc(sizeof(*pmu), GFP_KERNEL);
+	if (!pmu)
+		return -ENOMEM;
+
+	spin_lock_init(&pmu->lock);
+	pmu->pmc_base = initdata->pmc_base;
+	pmu->pmu_base = initdata->pmu_base;
+
+	pmu_reset_init(pmu);
+	for (domain_initdata = initdata->domains; domain_initdata->name;
+	     domain_initdata++) {
+		struct pmu_domain *domain;
+
+		domain = kzalloc(sizeof(*domain), GFP_KERNEL);
+		if (domain) {
+			domain->pmu = pmu;
+			domain->pwr_mask = domain_initdata->pwr_mask;
+			domain->rst_mask = domain_initdata->rst_mask;
+			domain->iso_mask = domain_initdata->iso_mask;
+			domain->base.name = domain_initdata->name;
+
+			__pmu_domain_register(domain, NULL);
+		}
+	}
+
+	ret = dove_init_pmu_irq(pmu, initdata->irq);
+	if (ret)
+		pr_err("dove_init_pmu_irq() failed: %d\n", ret);
+
+	if (pmu->irq_domain)
+		irq_domain_associate_many(pmu->irq_domain,
+					  initdata->irq_domain_start,
+					  0, NR_PMU_IRQS);
+
+	return 0;
+}
+
 /*
  * pmu: power-manager@d0000 {
  *	compatible = "marvell,dove-pmu";
diff --git a/drivers/soc/fsl/Makefile b/drivers/soc/fsl/Makefile
new file mode 100644
index 000000000000..203307fd92c1
--- /dev/null
+++ b/drivers/soc/fsl/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the Linux Kernel SOC fsl specific device drivers
+#
+
+obj-$(CONFIG_QUICC_ENGINE)		+= qe/
+obj-$(CONFIG_CPM)			+= qe/
diff --git a/drivers/soc/fsl/qe/Kconfig b/drivers/soc/fsl/qe/Kconfig
new file mode 100644
index 000000000000..20978f2058a6
--- /dev/null
+++ b/drivers/soc/fsl/qe/Kconfig
@@ -0,0 +1,38 @@
+#
+# QE Communication options
+#
+
+config QUICC_ENGINE
+	bool "Freescale QUICC Engine (QE) Support"
+	depends on FSL_SOC && PPC32
+	select GENERIC_ALLOCATOR
+	select CRC32
+	help
+	  The QUICC Engine (QE) is a new generation of communications
+	  coprocessors on Freescale embedded CPUs (akin to CPM in older chips).
+	  Selecting this option means that you wish to build a kernel
+	  for a machine with a QE coprocessor.
+
+config UCC_SLOW
+	bool
+	default y if SERIAL_QE
+	help
+	  This option provides qe_lib support to UCC slow
+	  protocols: UART, BISYNC, QMC
+
+config UCC_FAST
+	bool
+	default y if UCC_GETH
+	help
+	  This option provides qe_lib support to UCC fast
+	  protocols: HDLC, Ethernet, ATM, transparent
+
+config UCC
+	bool
+	default y if UCC_FAST || UCC_SLOW
+
+config QE_USB
+	bool
+	default y if USB_FSL_QE
+	help
+	  QE USB Controller support
diff --git a/drivers/soc/fsl/qe/Makefile b/drivers/soc/fsl/qe/Makefile
new file mode 100644
index 000000000000..ffac5410c5c7
--- /dev/null
+++ b/drivers/soc/fsl/qe/Makefile
@@ -0,0 +1,10 @@
+#
+# Makefile for the linux ppc-specific parts of QE
+#
+obj-$(CONFIG_QUICC_ENGINE)+= qe.o qe_common.o qe_ic.o qe_io.o
+obj-$(CONFIG_CPM)	+= qe_common.o
+obj-$(CONFIG_UCC)	+= ucc.o
+obj-$(CONFIG_UCC_SLOW)	+= ucc_slow.o
+obj-$(CONFIG_UCC_FAST)	+= ucc_fast.o
+obj-$(CONFIG_QE_USB)	+= usb.o
+obj-$(CONFIG_QE_GPIO)	+= gpio.o
diff --git a/drivers/soc/fsl/qe/gpio.c b/drivers/soc/fsl/qe/gpio.c
new file mode 100644
index 000000000000..aa5c11acf212
--- /dev/null
+++ b/drivers/soc/fsl/qe/gpio.c
@@ -0,0 +1,317 @@
+/*
+ * QUICC Engine GPIOs
+ *
+ * Copyright (c) MontaVista Software, Inc. 2008.
+ *
+ * Author: Anton Vorontsov <avorontsov@ru.mvista.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/export.h>
+#include <soc/fsl/qe/qe.h>
+
+struct qe_gpio_chip {
+	struct of_mm_gpio_chip mm_gc;
+	spinlock_t lock;
+
+	unsigned long pin_flags[QE_PIO_PINS];
+#define QE_PIN_REQUESTED 0
+
+	/* shadowed data register to clear/set bits safely */
+	u32 cpdata;
+
+	/* saved_regs used to restore dedicated functions */
+	struct qe_pio_regs saved_regs;
+};
+
+static inline struct qe_gpio_chip *
+to_qe_gpio_chip(struct of_mm_gpio_chip *mm_gc)
+{
+	return container_of(mm_gc, struct qe_gpio_chip, mm_gc);
+}
+
+static void qe_gpio_save_regs(struct of_mm_gpio_chip *mm_gc)
+{
+	struct qe_gpio_chip *qe_gc = to_qe_gpio_chip(mm_gc);
+	struct qe_pio_regs __iomem *regs = mm_gc->regs;
+
+	qe_gc->cpdata = in_be32(&regs->cpdata);
+	qe_gc->saved_regs.cpdata = qe_gc->cpdata;
+	qe_gc->saved_regs.cpdir1 = in_be32(&regs->cpdir1);
+	qe_gc->saved_regs.cpdir2 = in_be32(&regs->cpdir2);
+	qe_gc->saved_regs.cppar1 = in_be32(&regs->cppar1);
+	qe_gc->saved_regs.cppar2 = in_be32(&regs->cppar2);
+	qe_gc->saved_regs.cpodr = in_be32(&regs->cpodr);
+}
+
+static int qe_gpio_get(struct gpio_chip *gc, unsigned int gpio)
+{
+	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
+	struct qe_pio_regs __iomem *regs = mm_gc->regs;
+	u32 pin_mask = 1 << (QE_PIO_PINS - 1 - gpio);
+
+	return in_be32(&regs->cpdata) & pin_mask;
+}
+
+static void qe_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
+	struct qe_gpio_chip *qe_gc = to_qe_gpio_chip(mm_gc);
+	struct qe_pio_regs __iomem *regs = mm_gc->regs;
+	unsigned long flags;
+	u32 pin_mask = 1 << (QE_PIO_PINS - 1 - gpio);
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	if (val)
+		qe_gc->cpdata |= pin_mask;
+	else
+		qe_gc->cpdata &= ~pin_mask;
+
+	out_be32(&regs->cpdata, qe_gc->cpdata);
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+}
+
+static int qe_gpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
+{
+	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
+	struct qe_gpio_chip *qe_gc = to_qe_gpio_chip(mm_gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	__par_io_config_pin(mm_gc->regs, gpio, QE_PIO_DIR_IN, 0, 0, 0);
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+
+	return 0;
+}
+
+static int qe_gpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+	struct of_mm_gpio_chip *mm_gc = to_of_mm_gpio_chip(gc);
+	struct qe_gpio_chip *qe_gc = to_qe_gpio_chip(mm_gc);
+	unsigned long flags;
+
+	qe_gpio_set(gc, gpio, val);
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	__par_io_config_pin(mm_gc->regs, gpio, QE_PIO_DIR_OUT, 0, 0, 0);
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+
+	return 0;
+}
+
+struct qe_pin {
+	/*
+	 * The qe_gpio_chip name is unfortunate, we should change that to
+	 * something like qe_pio_controller. Someday.
+	 */
+	struct qe_gpio_chip *controller;
+	int num;
+};
+
+/**
+ * qe_pin_request - Request a QE pin
+ * @np:		device node to get a pin from
+ * @index:	index of a pin in the device tree
+ * Context:	non-atomic
+ *
+ * This function return qe_pin so that you could use it with the rest of
+ * the QE Pin Multiplexing API.
+ */
+struct qe_pin *qe_pin_request(struct device_node *np, int index)
+{
+	struct qe_pin *qe_pin;
+	struct gpio_chip *gc;
+	struct of_mm_gpio_chip *mm_gc;
+	struct qe_gpio_chip *qe_gc;
+	int err;
+	unsigned long flags;
+
+	qe_pin = kzalloc(sizeof(*qe_pin), GFP_KERNEL);
+	if (!qe_pin) {
+		pr_debug("%s: can't allocate memory\n", __func__);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	err = of_get_gpio(np, index);
+	if (err < 0)
+		goto err0;
+	gc = gpio_to_chip(err);
+	if (WARN_ON(!gc))
+		goto err0;
+
+	if (!of_device_is_compatible(gc->of_node, "fsl,mpc8323-qe-pario-bank")) {
+		pr_debug("%s: tried to get a non-qe pin\n", __func__);
+		err = -EINVAL;
+		goto err0;
+	}
+
+	mm_gc = to_of_mm_gpio_chip(gc);
+	qe_gc = to_qe_gpio_chip(mm_gc);
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	err -= gc->base;
+	if (test_and_set_bit(QE_PIN_REQUESTED, &qe_gc->pin_flags[err]) == 0) {
+		qe_pin->controller = qe_gc;
+		qe_pin->num = err;
+		err = 0;
+	} else {
+		err = -EBUSY;
+	}
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+
+	if (!err)
+		return qe_pin;
+err0:
+	kfree(qe_pin);
+	pr_debug("%s failed with status %d\n", __func__, err);
+	return ERR_PTR(err);
+}
+EXPORT_SYMBOL(qe_pin_request);
+
+/**
+ * qe_pin_free - Free a pin
+ * @qe_pin:	pointer to the qe_pin structure
+ * Context:	any
+ *
+ * This function frees the qe_pin structure and makes a pin available
+ * for further qe_pin_request() calls.
+ */
+void qe_pin_free(struct qe_pin *qe_pin)
+{
+	struct qe_gpio_chip *qe_gc = qe_pin->controller;
+	unsigned long flags;
+	const int pin = qe_pin->num;
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+	test_and_clear_bit(QE_PIN_REQUESTED, &qe_gc->pin_flags[pin]);
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+
+	kfree(qe_pin);
+}
+EXPORT_SYMBOL(qe_pin_free);
+
+/**
+ * qe_pin_set_dedicated - Revert a pin to a dedicated peripheral function mode
+ * @qe_pin:	pointer to the qe_pin structure
+ * Context:	any
+ *
+ * This function resets a pin to a dedicated peripheral function that
+ * has been set up by the firmware.
+ */
+void qe_pin_set_dedicated(struct qe_pin *qe_pin)
+{
+	struct qe_gpio_chip *qe_gc = qe_pin->controller;
+	struct qe_pio_regs __iomem *regs = qe_gc->mm_gc.regs;
+	struct qe_pio_regs *sregs = &qe_gc->saved_regs;
+	int pin = qe_pin->num;
+	u32 mask1 = 1 << (QE_PIO_PINS - (pin + 1));
+	u32 mask2 = 0x3 << (QE_PIO_PINS - (pin % (QE_PIO_PINS / 2) + 1) * 2);
+	bool second_reg = pin > (QE_PIO_PINS / 2) - 1;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	if (second_reg) {
+		clrsetbits_be32(&regs->cpdir2, mask2, sregs->cpdir2 & mask2);
+		clrsetbits_be32(&regs->cppar2, mask2, sregs->cppar2 & mask2);
+	} else {
+		clrsetbits_be32(&regs->cpdir1, mask2, sregs->cpdir1 & mask2);
+		clrsetbits_be32(&regs->cppar1, mask2, sregs->cppar1 & mask2);
+	}
+
+	if (sregs->cpdata & mask1)
+		qe_gc->cpdata |= mask1;
+	else
+		qe_gc->cpdata &= ~mask1;
+
+	out_be32(&regs->cpdata, qe_gc->cpdata);
+	clrsetbits_be32(&regs->cpodr, mask1, sregs->cpodr & mask1);
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+}
+EXPORT_SYMBOL(qe_pin_set_dedicated);
+
+/**
+ * qe_pin_set_gpio - Set a pin to the GPIO mode
+ * @qe_pin:	pointer to the qe_pin structure
+ * Context:	any
+ *
+ * This function sets a pin to the GPIO mode.
+ */
+void qe_pin_set_gpio(struct qe_pin *qe_pin)
+{
+	struct qe_gpio_chip *qe_gc = qe_pin->controller;
+	struct qe_pio_regs __iomem *regs = qe_gc->mm_gc.regs;
+	unsigned long flags;
+
+	spin_lock_irqsave(&qe_gc->lock, flags);
+
+	/* Let's make it input by default, GPIO API is able to change that. */
+	__par_io_config_pin(regs, qe_pin->num, QE_PIO_DIR_IN, 0, 0, 0);
+
+	spin_unlock_irqrestore(&qe_gc->lock, flags);
+}
+EXPORT_SYMBOL(qe_pin_set_gpio);
+
+static int __init qe_add_gpiochips(void)
+{
+	struct device_node *np;
+
+	for_each_compatible_node(np, NULL, "fsl,mpc8323-qe-pario-bank") {
+		int ret;
+		struct qe_gpio_chip *qe_gc;
+		struct of_mm_gpio_chip *mm_gc;
+		struct gpio_chip *gc;
+
+		qe_gc = kzalloc(sizeof(*qe_gc), GFP_KERNEL);
+		if (!qe_gc) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		spin_lock_init(&qe_gc->lock);
+
+		mm_gc = &qe_gc->mm_gc;
+		gc = &mm_gc->gc;
+
+		mm_gc->save_regs = qe_gpio_save_regs;
+		gc->ngpio = QE_PIO_PINS;
+		gc->direction_input = qe_gpio_dir_in;
+		gc->direction_output = qe_gpio_dir_out;
+		gc->get = qe_gpio_get;
+		gc->set = qe_gpio_set;
+
+		ret = of_mm_gpiochip_add(np, mm_gc);
+		if (ret)
+			goto err;
+		continue;
+err:
+		pr_err("%s: registration failed with status %d\n",
+		       np->full_name, ret);
+		kfree(qe_gc);
+		/* try others anyway */
+	}
+	return 0;
+}
+arch_initcall(qe_add_gpiochips);
diff --git a/drivers/soc/fsl/qe/qe.c b/drivers/soc/fsl/qe/qe.c
new file mode 100644
index 000000000000..709fc63809e5
--- /dev/null
+++ b/drivers/soc/fsl/qe/qe.c
@@ -0,0 +1,719 @@
+/*
+ * Copyright (C) 2006-2010 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Authors: 	Shlomi Gridish <gridish@freescale.com>
+ * 		Li Yang <leoli@freescale.com>
+ * Based on cpm2_common.c from Dan Malek (dmalek@jlc.net)
+ *
+ * Description:
+ * General Purpose functions for the global management of the
+ * QUICC Engine (QE).
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the 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/errno.h>
+#include <linux/sched.h>
+#include <linux/kernel.h>
+#include <linux/param.h>
+#include <linux/string.h>
+#include <linux/spinlock.h>
+#include <linux/mm.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/ioport.h>
+#include <linux/crc32.h>
+#include <linux/mod_devicetable.h>
+#include <linux/of_platform.h>
+#include <asm/irq.h>
+#include <asm/page.h>
+#include <asm/pgtable.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+#include <asm/prom.h>
+#include <asm/rheap.h>
+
+static void qe_snums_init(void);
+static int qe_sdma_init(void);
+
+static DEFINE_SPINLOCK(qe_lock);
+DEFINE_SPINLOCK(cmxgcr_lock);
+EXPORT_SYMBOL(cmxgcr_lock);
+
+/* QE snum state */
+enum qe_snum_state {
+	QE_SNUM_STATE_USED,
+	QE_SNUM_STATE_FREE
+};
+
+/* QE snum */
+struct qe_snum {
+	u8 num;
+	enum qe_snum_state state;
+};
+
+/* We allocate this here because it is used almost exclusively for
+ * the communication processor devices.
+ */
+struct qe_immap __iomem *qe_immr;
+EXPORT_SYMBOL(qe_immr);
+
+static struct qe_snum snums[QE_NUM_OF_SNUM];	/* Dynamically allocated SNUMs */
+static unsigned int qe_num_of_snum;
+
+static phys_addr_t qebase = -1;
+
+phys_addr_t get_qe_base(void)
+{
+	struct device_node *qe;
+	int size;
+	const u32 *prop;
+
+	if (qebase != -1)
+		return qebase;
+
+	qe = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!qe) {
+		qe = of_find_node_by_type(NULL, "qe");
+		if (!qe)
+			return qebase;
+	}
+
+	prop = of_get_property(qe, "reg", &size);
+	if (prop && size >= sizeof(*prop))
+		qebase = of_translate_address(qe, prop);
+	of_node_put(qe);
+
+	return qebase;
+}
+
+EXPORT_SYMBOL(get_qe_base);
+
+void qe_reset(void)
+{
+	if (qe_immr == NULL)
+		qe_immr = ioremap(get_qe_base(), QE_IMMAP_SIZE);
+
+	qe_snums_init();
+
+	qe_issue_cmd(QE_RESET, QE_CR_SUBBLOCK_INVALID,
+		     QE_CR_PROTOCOL_UNSPECIFIED, 0);
+
+	/* Reclaim the MURAM memory for our use. */
+	qe_muram_init();
+
+	if (qe_sdma_init())
+		panic("sdma init failed!");
+}
+
+int qe_issue_cmd(u32 cmd, u32 device, u8 mcn_protocol, u32 cmd_input)
+{
+	unsigned long flags;
+	u8 mcn_shift = 0, dev_shift = 0;
+	u32 ret;
+
+	spin_lock_irqsave(&qe_lock, flags);
+	if (cmd == QE_RESET) {
+		out_be32(&qe_immr->cp.cecr, (u32) (cmd | QE_CR_FLG));
+	} else {
+		if (cmd == QE_ASSIGN_PAGE) {
+			/* Here device is the SNUM, not sub-block */
+			dev_shift = QE_CR_SNUM_SHIFT;
+		} else if (cmd == QE_ASSIGN_RISC) {
+			/* Here device is the SNUM, and mcnProtocol is
+			 * e_QeCmdRiscAssignment value */
+			dev_shift = QE_CR_SNUM_SHIFT;
+			mcn_shift = QE_CR_MCN_RISC_ASSIGN_SHIFT;
+		} else {
+			if (device == QE_CR_SUBBLOCK_USB)
+				mcn_shift = QE_CR_MCN_USB_SHIFT;
+			else
+				mcn_shift = QE_CR_MCN_NORMAL_SHIFT;
+		}
+
+		out_be32(&qe_immr->cp.cecdr, cmd_input);
+		out_be32(&qe_immr->cp.cecr,
+			 (cmd | QE_CR_FLG | ((u32) device << dev_shift) | (u32)
+			  mcn_protocol << mcn_shift));
+	}
+
+	/* wait for the QE_CR_FLG to clear */
+	ret = spin_event_timeout((in_be32(&qe_immr->cp.cecr) & QE_CR_FLG) == 0,
+			   100, 0);
+	/* On timeout (e.g. failure), the expression will be false (ret == 0),
+	   otherwise it will be true (ret == 1). */
+	spin_unlock_irqrestore(&qe_lock, flags);
+
+	return ret == 1;
+}
+EXPORT_SYMBOL(qe_issue_cmd);
+
+/* Set a baud rate generator. This needs lots of work. There are
+ * 16 BRGs, which can be connected to the QE channels or output
+ * as clocks. The BRGs are in two different block of internal
+ * memory mapped space.
+ * The BRG clock is the QE clock divided by 2.
+ * It was set up long ago during the initial boot phase and is
+ * is given to us.
+ * Baud rate clocks are zero-based in the driver code (as that maps
+ * to port numbers). Documentation uses 1-based numbering.
+ */
+static unsigned int brg_clk = 0;
+
+unsigned int qe_get_brg_clk(void)
+{
+	struct device_node *qe;
+	int size;
+	const u32 *prop;
+
+	if (brg_clk)
+		return brg_clk;
+
+	qe = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!qe) {
+		qe = of_find_node_by_type(NULL, "qe");
+		if (!qe)
+			return brg_clk;
+	}
+
+	prop = of_get_property(qe, "brg-frequency", &size);
+	if (prop && size == sizeof(*prop))
+		brg_clk = *prop;
+
+	of_node_put(qe);
+
+	return brg_clk;
+}
+EXPORT_SYMBOL(qe_get_brg_clk);
+
+/* Program the BRG to the given sampling rate and multiplier
+ *
+ * @brg: the BRG, QE_BRG1 - QE_BRG16
+ * @rate: the desired sampling rate
+ * @multiplier: corresponds to the value programmed in GUMR_L[RDCR] or
+ * GUMR_L[TDCR].  E.g., if this BRG is the RX clock, and GUMR_L[RDCR]=01,
+ * then 'multiplier' should be 8.
+ */
+int qe_setbrg(enum qe_clock brg, unsigned int rate, unsigned int multiplier)
+{
+	u32 divisor, tempval;
+	u32 div16 = 0;
+
+	if ((brg < QE_BRG1) || (brg > QE_BRG16))
+		return -EINVAL;
+
+	divisor = qe_get_brg_clk() / (rate * multiplier);
+
+	if (divisor > QE_BRGC_DIVISOR_MAX + 1) {
+		div16 = QE_BRGC_DIV16;
+		divisor /= 16;
+	}
+
+	/* Errata QE_General4, which affects some MPC832x and MPC836x SOCs, says
+	   that the BRG divisor must be even if you're not using divide-by-16
+	   mode. */
+	if (!div16 && (divisor & 1) && (divisor > 3))
+		divisor++;
+
+	tempval = ((divisor - 1) << QE_BRGC_DIVISOR_SHIFT) |
+		QE_BRGC_ENABLE | div16;
+
+	out_be32(&qe_immr->brg.brgc[brg - QE_BRG1], tempval);
+
+	return 0;
+}
+EXPORT_SYMBOL(qe_setbrg);
+
+/* Convert a string to a QE clock source enum
+ *
+ * This function takes a string, typically from a property in the device
+ * tree, and returns the corresponding "enum qe_clock" value.
+*/
+enum qe_clock qe_clock_source(const char *source)
+{
+	unsigned int i;
+
+	if (strcasecmp(source, "none") == 0)
+		return QE_CLK_NONE;
+
+	if (strncasecmp(source, "brg", 3) == 0) {
+		i = simple_strtoul(source + 3, NULL, 10);
+		if ((i >= 1) && (i <= 16))
+			return (QE_BRG1 - 1) + i;
+		else
+			return QE_CLK_DUMMY;
+	}
+
+	if (strncasecmp(source, "clk", 3) == 0) {
+		i = simple_strtoul(source + 3, NULL, 10);
+		if ((i >= 1) && (i <= 24))
+			return (QE_CLK1 - 1) + i;
+		else
+			return QE_CLK_DUMMY;
+	}
+
+	return QE_CLK_DUMMY;
+}
+EXPORT_SYMBOL(qe_clock_source);
+
+/* Initialize SNUMs (thread serial numbers) according to
+ * QE Module Control chapter, SNUM table
+ */
+static void qe_snums_init(void)
+{
+	int i;
+	static const u8 snum_init_76[] = {
+		0x04, 0x05, 0x0C, 0x0D, 0x14, 0x15, 0x1C, 0x1D,
+		0x24, 0x25, 0x2C, 0x2D, 0x34, 0x35, 0x88, 0x89,
+		0x98, 0x99, 0xA8, 0xA9, 0xB8, 0xB9, 0xC8, 0xC9,
+		0xD8, 0xD9, 0xE8, 0xE9, 0x44, 0x45, 0x4C, 0x4D,
+		0x54, 0x55, 0x5C, 0x5D, 0x64, 0x65, 0x6C, 0x6D,
+		0x74, 0x75, 0x7C, 0x7D, 0x84, 0x85, 0x8C, 0x8D,
+		0x94, 0x95, 0x9C, 0x9D, 0xA4, 0xA5, 0xAC, 0xAD,
+		0xB4, 0xB5, 0xBC, 0xBD, 0xC4, 0xC5, 0xCC, 0xCD,
+		0xD4, 0xD5, 0xDC, 0xDD, 0xE4, 0xE5, 0xEC, 0xED,
+		0xF4, 0xF5, 0xFC, 0xFD,
+	};
+	static const u8 snum_init_46[] = {
+		0x04, 0x05, 0x0C, 0x0D, 0x14, 0x15, 0x1C, 0x1D,
+		0x24, 0x25, 0x2C, 0x2D, 0x34, 0x35, 0x88, 0x89,
+		0x98, 0x99, 0xA8, 0xA9, 0xB8, 0xB9, 0xC8, 0xC9,
+		0xD8, 0xD9, 0xE8, 0xE9, 0x08, 0x09, 0x18, 0x19,
+		0x28, 0x29, 0x38, 0x39, 0x48, 0x49, 0x58, 0x59,
+		0x68, 0x69, 0x78, 0x79, 0x80, 0x81,
+	};
+	static const u8 *snum_init;
+
+	qe_num_of_snum = qe_get_num_of_snums();
+
+	if (qe_num_of_snum == 76)
+		snum_init = snum_init_76;
+	else
+		snum_init = snum_init_46;
+
+	for (i = 0; i < qe_num_of_snum; i++) {
+		snums[i].num = snum_init[i];
+		snums[i].state = QE_SNUM_STATE_FREE;
+	}
+}
+
+int qe_get_snum(void)
+{
+	unsigned long flags;
+	int snum = -EBUSY;
+	int i;
+
+	spin_lock_irqsave(&qe_lock, flags);
+	for (i = 0; i < qe_num_of_snum; i++) {
+		if (snums[i].state == QE_SNUM_STATE_FREE) {
+			snums[i].state = QE_SNUM_STATE_USED;
+			snum = snums[i].num;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&qe_lock, flags);
+
+	return snum;
+}
+EXPORT_SYMBOL(qe_get_snum);
+
+void qe_put_snum(u8 snum)
+{
+	int i;
+
+	for (i = 0; i < qe_num_of_snum; i++) {
+		if (snums[i].num == snum) {
+			snums[i].state = QE_SNUM_STATE_FREE;
+			break;
+		}
+	}
+}
+EXPORT_SYMBOL(qe_put_snum);
+
+static int qe_sdma_init(void)
+{
+	struct sdma __iomem *sdma = &qe_immr->sdma;
+	static unsigned long sdma_buf_offset = (unsigned long)-ENOMEM;
+
+	if (!sdma)
+		return -ENODEV;
+
+	/* allocate 2 internal temporary buffers (512 bytes size each) for
+	 * the SDMA */
+	if (IS_ERR_VALUE(sdma_buf_offset)) {
+		sdma_buf_offset = qe_muram_alloc(512 * 2, 4096);
+		if (IS_ERR_VALUE(sdma_buf_offset))
+			return -ENOMEM;
+	}
+
+	out_be32(&sdma->sdebcr, (u32) sdma_buf_offset & QE_SDEBCR_BA_MASK);
+ 	out_be32(&sdma->sdmr, (QE_SDMR_GLB_1_MSK |
+ 					(0x1 << QE_SDMR_CEN_SHIFT)));
+
+	return 0;
+}
+
+/* The maximum number of RISCs we support */
+#define MAX_QE_RISC     4
+
+/* Firmware information stored here for qe_get_firmware_info() */
+static struct qe_firmware_info qe_firmware_info;
+
+/*
+ * Set to 1 if QE firmware has been uploaded, and therefore
+ * qe_firmware_info contains valid data.
+ */
+static int qe_firmware_uploaded;
+
+/*
+ * Upload a QE microcode
+ *
+ * This function is a worker function for qe_upload_firmware().  It does
+ * the actual uploading of the microcode.
+ */
+static void qe_upload_microcode(const void *base,
+	const struct qe_microcode *ucode)
+{
+	const __be32 *code = base + be32_to_cpu(ucode->code_offset);
+	unsigned int i;
+
+	if (ucode->major || ucode->minor || ucode->revision)
+		printk(KERN_INFO "qe-firmware: "
+			"uploading microcode '%s' version %u.%u.%u\n",
+			ucode->id, ucode->major, ucode->minor, ucode->revision);
+	else
+		printk(KERN_INFO "qe-firmware: "
+			"uploading microcode '%s'\n", ucode->id);
+
+	/* Use auto-increment */
+	out_be32(&qe_immr->iram.iadd, be32_to_cpu(ucode->iram_offset) |
+		QE_IRAM_IADD_AIE | QE_IRAM_IADD_BADDR);
+
+	for (i = 0; i < be32_to_cpu(ucode->count); i++)
+		out_be32(&qe_immr->iram.idata, be32_to_cpu(code[i]));
+	
+	/* Set I-RAM Ready Register */
+	out_be32(&qe_immr->iram.iready, be32_to_cpu(QE_IRAM_READY));
+}
+
+/*
+ * Upload a microcode to the I-RAM at a specific address.
+ *
+ * See Documentation/powerpc/qe_firmware.txt for information on QE microcode
+ * uploading.
+ *
+ * Currently, only version 1 is supported, so the 'version' field must be
+ * set to 1.
+ *
+ * The SOC model and revision are not validated, they are only displayed for
+ * informational purposes.
+ *
+ * 'calc_size' is the calculated size, in bytes, of the firmware structure and
+ * all of the microcode structures, minus the CRC.
+ *
+ * 'length' is the size that the structure says it is, including the CRC.
+ */
+int qe_upload_firmware(const struct qe_firmware *firmware)
+{
+	unsigned int i;
+	unsigned int j;
+	u32 crc;
+	size_t calc_size = sizeof(struct qe_firmware);
+	size_t length;
+	const struct qe_header *hdr;
+
+	if (!firmware) {
+		printk(KERN_ERR "qe-firmware: invalid pointer\n");
+		return -EINVAL;
+	}
+
+	hdr = &firmware->header;
+	length = be32_to_cpu(hdr->length);
+
+	/* Check the magic */
+	if ((hdr->magic[0] != 'Q') || (hdr->magic[1] != 'E') ||
+	    (hdr->magic[2] != 'F')) {
+		printk(KERN_ERR "qe-firmware: not a microcode\n");
+		return -EPERM;
+	}
+
+	/* Check the version */
+	if (hdr->version != 1) {
+		printk(KERN_ERR "qe-firmware: unsupported version\n");
+		return -EPERM;
+	}
+
+	/* Validate some of the fields */
+	if ((firmware->count < 1) || (firmware->count > MAX_QE_RISC)) {
+		printk(KERN_ERR "qe-firmware: invalid data\n");
+		return -EINVAL;
+	}
+
+	/* Validate the length and check if there's a CRC */
+	calc_size += (firmware->count - 1) * sizeof(struct qe_microcode);
+
+	for (i = 0; i < firmware->count; i++)
+		/*
+		 * For situations where the second RISC uses the same microcode
+		 * as the first, the 'code_offset' and 'count' fields will be
+		 * zero, so it's okay to add those.
+		 */
+		calc_size += sizeof(__be32) *
+			be32_to_cpu(firmware->microcode[i].count);
+
+	/* Validate the length */
+	if (length != calc_size + sizeof(__be32)) {
+		printk(KERN_ERR "qe-firmware: invalid length\n");
+		return -EPERM;
+	}
+
+	/* Validate the CRC */
+	crc = be32_to_cpu(*(__be32 *)((void *)firmware + calc_size));
+	if (crc != crc32(0, firmware, calc_size)) {
+		printk(KERN_ERR "qe-firmware: firmware CRC is invalid\n");
+		return -EIO;
+	}
+
+	/*
+	 * If the microcode calls for it, split the I-RAM.
+	 */
+	if (!firmware->split)
+		setbits16(&qe_immr->cp.cercr, QE_CP_CERCR_CIR);
+
+	if (firmware->soc.model)
+		printk(KERN_INFO
+			"qe-firmware: firmware '%s' for %u V%u.%u\n",
+			firmware->id, be16_to_cpu(firmware->soc.model),
+			firmware->soc.major, firmware->soc.minor);
+	else
+		printk(KERN_INFO "qe-firmware: firmware '%s'\n",
+			firmware->id);
+
+	/*
+	 * The QE only supports one microcode per RISC, so clear out all the
+	 * saved microcode information and put in the new.
+	 */
+	memset(&qe_firmware_info, 0, sizeof(qe_firmware_info));
+	strlcpy(qe_firmware_info.id, firmware->id, sizeof(qe_firmware_info.id));
+	qe_firmware_info.extended_modes = firmware->extended_modes;
+	memcpy(qe_firmware_info.vtraps, firmware->vtraps,
+		sizeof(firmware->vtraps));
+
+	/* Loop through each microcode. */
+	for (i = 0; i < firmware->count; i++) {
+		const struct qe_microcode *ucode = &firmware->microcode[i];
+
+		/* Upload a microcode if it's present */
+		if (ucode->code_offset)
+			qe_upload_microcode(firmware, ucode);
+
+		/* Program the traps for this processor */
+		for (j = 0; j < 16; j++) {
+			u32 trap = be32_to_cpu(ucode->traps[j]);
+
+			if (trap)
+				out_be32(&qe_immr->rsp[i].tibcr[j], trap);
+		}
+
+		/* Enable traps */
+		out_be32(&qe_immr->rsp[i].eccr, be32_to_cpu(ucode->eccr));
+	}
+
+	qe_firmware_uploaded = 1;
+
+	return 0;
+}
+EXPORT_SYMBOL(qe_upload_firmware);
+
+/*
+ * Get info on the currently-loaded firmware
+ *
+ * This function also checks the device tree to see if the boot loader has
+ * uploaded a firmware already.
+ */
+struct qe_firmware_info *qe_get_firmware_info(void)
+{
+	static int initialized;
+	struct property *prop;
+	struct device_node *qe;
+	struct device_node *fw = NULL;
+	const char *sprop;
+	unsigned int i;
+
+	/*
+	 * If we haven't checked yet, and a driver hasn't uploaded a firmware
+	 * yet, then check the device tree for information.
+	 */
+	if (qe_firmware_uploaded)
+		return &qe_firmware_info;
+
+	if (initialized)
+		return NULL;
+
+	initialized = 1;
+
+	/*
+	 * Newer device trees have an "fsl,qe" compatible property for the QE
+	 * node, but we still need to support older device trees.
+	*/
+	qe = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!qe) {
+		qe = of_find_node_by_type(NULL, "qe");
+		if (!qe)
+			return NULL;
+	}
+
+	/* Find the 'firmware' child node */
+	for_each_child_of_node(qe, fw) {
+		if (strcmp(fw->name, "firmware") == 0)
+			break;
+	}
+
+	of_node_put(qe);
+
+	/* Did we find the 'firmware' node? */
+	if (!fw)
+		return NULL;
+
+	qe_firmware_uploaded = 1;
+
+	/* Copy the data into qe_firmware_info*/
+	sprop = of_get_property(fw, "id", NULL);
+	if (sprop)
+		strlcpy(qe_firmware_info.id, sprop,
+			sizeof(qe_firmware_info.id));
+
+	prop = of_find_property(fw, "extended-modes", NULL);
+	if (prop && (prop->length == sizeof(u64))) {
+		const u64 *iprop = prop->value;
+
+		qe_firmware_info.extended_modes = *iprop;
+	}
+
+	prop = of_find_property(fw, "virtual-traps", NULL);
+	if (prop && (prop->length == 32)) {
+		const u32 *iprop = prop->value;
+
+		for (i = 0; i < ARRAY_SIZE(qe_firmware_info.vtraps); i++)
+			qe_firmware_info.vtraps[i] = iprop[i];
+	}
+
+	of_node_put(fw);
+
+	return &qe_firmware_info;
+}
+EXPORT_SYMBOL(qe_get_firmware_info);
+
+unsigned int qe_get_num_of_risc(void)
+{
+	struct device_node *qe;
+	int size;
+	unsigned int num_of_risc = 0;
+	const u32 *prop;
+
+	qe = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!qe) {
+		/* Older devices trees did not have an "fsl,qe"
+		 * compatible property, so we need to look for
+		 * the QE node by name.
+		 */
+		qe = of_find_node_by_type(NULL, "qe");
+		if (!qe)
+			return num_of_risc;
+	}
+
+	prop = of_get_property(qe, "fsl,qe-num-riscs", &size);
+	if (prop && size == sizeof(*prop))
+		num_of_risc = *prop;
+
+	of_node_put(qe);
+
+	return num_of_risc;
+}
+EXPORT_SYMBOL(qe_get_num_of_risc);
+
+unsigned int qe_get_num_of_snums(void)
+{
+	struct device_node *qe;
+	int size;
+	unsigned int num_of_snums;
+	const u32 *prop;
+
+	num_of_snums = 28; /* The default number of snum for threads is 28 */
+	qe = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!qe) {
+		/* Older devices trees did not have an "fsl,qe"
+		 * compatible property, so we need to look for
+		 * the QE node by name.
+		 */
+		qe = of_find_node_by_type(NULL, "qe");
+		if (!qe)
+			return num_of_snums;
+	}
+
+	prop = of_get_property(qe, "fsl,qe-num-snums", &size);
+	if (prop && size == sizeof(*prop)) {
+		num_of_snums = *prop;
+		if ((num_of_snums < 28) || (num_of_snums > QE_NUM_OF_SNUM)) {
+			/* No QE ever has fewer than 28 SNUMs */
+			pr_err("QE: number of snum is invalid\n");
+			of_node_put(qe);
+			return -EINVAL;
+		}
+	}
+
+	of_node_put(qe);
+
+	return num_of_snums;
+}
+EXPORT_SYMBOL(qe_get_num_of_snums);
+
+static int __init qe_init(void)
+{
+	struct device_node *np;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,qe");
+	if (!np)
+		return -ENODEV;
+	qe_reset();
+	of_node_put(np);
+	return 0;
+}
+subsys_initcall(qe_init);
+
+#if defined(CONFIG_SUSPEND) && defined(CONFIG_PPC_85xx)
+static int qe_resume(struct platform_device *ofdev)
+{
+	if (!qe_alive_during_sleep())
+		qe_reset();
+	return 0;
+}
+
+static int qe_probe(struct platform_device *ofdev)
+{
+	return 0;
+}
+
+static const struct of_device_id qe_ids[] = {
+	{ .compatible = "fsl,qe", },
+	{ },
+};
+
+static struct platform_driver qe_driver = {
+	.driver = {
+		.name = "fsl-qe",
+		.of_match_table = qe_ids,
+	},
+	.probe = qe_probe,
+	.resume = qe_resume,
+};
+
+static int __init qe_drv_init(void)
+{
+	return platform_driver_register(&qe_driver);
+}
+device_initcall(qe_drv_init);
+#endif /* defined(CONFIG_SUSPEND) && defined(CONFIG_PPC_85xx) */
diff --git a/drivers/soc/fsl/qe/qe_common.c b/drivers/soc/fsl/qe/qe_common.c
new file mode 100644
index 000000000000..419fa5b7be4d
--- /dev/null
+++ b/drivers/soc/fsl/qe/qe_common.c
@@ -0,0 +1,235 @@
+/*
+ * Common CPM code
+ *
+ * Author: Scott Wood <scottwood@freescale.com>
+ *
+ * Copyright 2007-2008,2010 Freescale Semiconductor, Inc.
+ *
+ * Some parts derived from commproc.c/cpm2_common.c, which is:
+ * Copyright (c) 1997 Dan error_act (dmalek@jlc.net)
+ * Copyright (c) 1999-2001 Dan Malek <dan@embeddedalley.com>
+ * Copyright (c) 2000 MontaVista Software, Inc (source@mvista.com)
+ * 2006 (c) MontaVista Software, Inc.
+ * Vitaly Bordug <vbordug@ru.mvista.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.
+ */
+#include <linux/genalloc.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/of_device.h>
+#include <linux/spinlock.h>
+#include <linux/export.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <soc/fsl/qe/qe.h>
+
+static struct gen_pool *muram_pool;
+static spinlock_t cpm_muram_lock;
+static u8 __iomem *muram_vbase;
+static phys_addr_t muram_pbase;
+
+struct muram_block {
+	struct list_head head;
+	unsigned long start;
+	int size;
+};
+
+static LIST_HEAD(muram_block_list);
+
+/* max address size we deal with */
+#define OF_MAX_ADDR_CELLS	4
+#define GENPOOL_OFFSET		(4096 * 8)
+
+int cpm_muram_init(void)
+{
+	struct device_node *np;
+	struct resource r;
+	u32 zero[OF_MAX_ADDR_CELLS] = {};
+	resource_size_t max = 0;
+	int i = 0;
+	int ret = 0;
+
+	if (muram_pbase)
+		return 0;
+
+	spin_lock_init(&cpm_muram_lock);
+	np = of_find_compatible_node(NULL, NULL, "fsl,cpm-muram-data");
+	if (!np) {
+		/* try legacy bindings */
+		np = of_find_node_by_name(NULL, "data-only");
+		if (!np) {
+			pr_err("Cannot find CPM muram data node");
+			ret = -ENODEV;
+			goto out_muram;
+		}
+	}
+
+	muram_pool = gen_pool_create(0, -1);
+	muram_pbase = of_translate_address(np, zero);
+	if (muram_pbase == (phys_addr_t)OF_BAD_ADDR) {
+		pr_err("Cannot translate zero through CPM muram node");
+		ret = -ENODEV;
+		goto out_pool;
+	}
+
+	while (of_address_to_resource(np, i++, &r) == 0) {
+		if (r.end > max)
+			max = r.end;
+		ret = gen_pool_add(muram_pool, r.start - muram_pbase +
+				   GENPOOL_OFFSET, resource_size(&r), -1);
+		if (ret) {
+			pr_err("QE: couldn't add muram to pool!\n");
+			goto out_pool;
+		}
+	}
+
+	muram_vbase = ioremap(muram_pbase, max - muram_pbase + 1);
+	if (!muram_vbase) {
+		pr_err("Cannot map QE muram");
+		ret = -ENOMEM;
+		goto out_pool;
+	}
+	goto out_muram;
+out_pool:
+	gen_pool_destroy(muram_pool);
+out_muram:
+	of_node_put(np);
+	return ret;
+}
+
+/*
+ * cpm_muram_alloc - allocate the requested size worth of multi-user ram
+ * @size: number of bytes to allocate
+ * @align: requested alignment, in bytes
+ *
+ * This function returns an offset into the muram area.
+ * Use cpm_dpram_addr() to get the virtual address of the area.
+ * Use cpm_muram_free() to free the allocation.
+ */
+unsigned long cpm_muram_alloc(unsigned long size, unsigned long align)
+{
+	unsigned long start;
+	unsigned long flags;
+	struct genpool_data_align muram_pool_data;
+
+	spin_lock_irqsave(&cpm_muram_lock, flags);
+	muram_pool_data.align = align;
+	start = cpm_muram_alloc_common(size, gen_pool_first_fit_align,
+				       &muram_pool_data);
+	spin_unlock_irqrestore(&cpm_muram_lock, flags);
+	return start;
+}
+EXPORT_SYMBOL(cpm_muram_alloc);
+
+/**
+ * cpm_muram_free - free a chunk of multi-user ram
+ * @offset: The beginning of the chunk as returned by cpm_muram_alloc().
+ */
+int cpm_muram_free(unsigned long offset)
+{
+	unsigned long flags;
+	int size;
+	struct muram_block *tmp;
+
+	size = 0;
+	spin_lock_irqsave(&cpm_muram_lock, flags);
+	list_for_each_entry(tmp, &muram_block_list, head) {
+		if (tmp->start == offset) {
+			size = tmp->size;
+			list_del(&tmp->head);
+			kfree(tmp);
+			break;
+		}
+	}
+	gen_pool_free(muram_pool, offset + GENPOOL_OFFSET, size);
+	spin_unlock_irqrestore(&cpm_muram_lock, flags);
+	return size;
+}
+EXPORT_SYMBOL(cpm_muram_free);
+
+/*
+ * cpm_muram_alloc_fixed - reserve a specific region of multi-user ram
+ * @offset: offset of allocation start address
+ * @size: number of bytes to allocate
+ * This function returns an offset into the muram area
+ * Use cpm_dpram_addr() to get the virtual address of the area.
+ * Use cpm_muram_free() to free the allocation.
+ */
+unsigned long cpm_muram_alloc_fixed(unsigned long offset, unsigned long size)
+{
+	unsigned long start;
+	unsigned long flags;
+	struct genpool_data_fixed muram_pool_data_fixed;
+
+	spin_lock_irqsave(&cpm_muram_lock, flags);
+	muram_pool_data_fixed.offset = offset + GENPOOL_OFFSET;
+	start = cpm_muram_alloc_common(size, gen_pool_fixed_alloc,
+				       &muram_pool_data_fixed);
+	spin_unlock_irqrestore(&cpm_muram_lock, flags);
+	return start;
+}
+EXPORT_SYMBOL(cpm_muram_alloc_fixed);
+
+/*
+ * cpm_muram_alloc_common - cpm_muram_alloc common code
+ * @size: number of bytes to allocate
+ * @algo: algorithm for alloc.
+ * @data: data for genalloc's algorithm.
+ *
+ * This function returns an offset into the muram area.
+ */
+unsigned long cpm_muram_alloc_common(unsigned long size, genpool_algo_t algo,
+				     void *data)
+{
+	struct muram_block *entry;
+	unsigned long start;
+
+	start = gen_pool_alloc_algo(muram_pool, size, algo, data);
+	if (!start)
+		goto out2;
+	start = start - GENPOOL_OFFSET;
+	memset_io(cpm_muram_addr(start), 0, size);
+	entry = kmalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		goto out1;
+	entry->start = start;
+	entry->size = size;
+	list_add(&entry->head, &muram_block_list);
+
+	return start;
+out1:
+	gen_pool_free(muram_pool, start, size);
+out2:
+	return (unsigned long)-ENOMEM;
+}
+
+/**
+ * cpm_muram_addr - turn a muram offset into a virtual address
+ * @offset: muram offset to convert
+ */
+void __iomem *cpm_muram_addr(unsigned long offset)
+{
+	return muram_vbase + offset;
+}
+EXPORT_SYMBOL(cpm_muram_addr);
+
+unsigned long cpm_muram_offset(void __iomem *addr)
+{
+	return addr - (void __iomem *)muram_vbase;
+}
+EXPORT_SYMBOL(cpm_muram_offset);
+
+/**
+ * cpm_muram_dma - turn a muram virtual address into a DMA address
+ * @offset: virtual address from cpm_muram_addr() to convert
+ */
+dma_addr_t cpm_muram_dma(void __iomem *addr)
+{
+	return muram_pbase + ((u8 __iomem *)addr - muram_vbase);
+}
+EXPORT_SYMBOL(cpm_muram_dma);
diff --git a/drivers/soc/fsl/qe/qe_ic.c b/drivers/soc/fsl/qe/qe_ic.c
new file mode 100644
index 000000000000..b77d01ff8330
--- /dev/null
+++ b/drivers/soc/fsl/qe/qe_ic.c
@@ -0,0 +1,503 @@
+/*
+ * arch/powerpc/sysdev/qe_lib/qe_ic.c
+ *
+ * Copyright (C) 2006 Freescale Semiconductor, Inc.  All rights reserved.
+ *
+ * Author: Li Yang <leoli@freescale.com>
+ * Based on code from Shlomi Gridish <gridish@freescale.com>
+ *
+ * QUICC ENGINE Interrupt Controller
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/reboot.h>
+#include <linux/slab.h>
+#include <linux/stddef.h>
+#include <linux/sched.h>
+#include <linux/signal.h>
+#include <linux/device.h>
+#include <linux/spinlock.h>
+#include <asm/irq.h>
+#include <asm/io.h>
+#include <soc/fsl/qe/qe_ic.h>
+
+#include "qe_ic.h"
+
+static DEFINE_RAW_SPINLOCK(qe_ic_lock);
+
+static struct qe_ic_info qe_ic_info[] = {
+	[1] = {
+	       .mask = 0x00008000,
+	       .mask_reg = QEIC_CIMR,
+	       .pri_code = 0,
+	       .pri_reg = QEIC_CIPWCC,
+	       },
+	[2] = {
+	       .mask = 0x00004000,
+	       .mask_reg = QEIC_CIMR,
+	       .pri_code = 1,
+	       .pri_reg = QEIC_CIPWCC,
+	       },
+	[3] = {
+	       .mask = 0x00002000,
+	       .mask_reg = QEIC_CIMR,
+	       .pri_code = 2,
+	       .pri_reg = QEIC_CIPWCC,
+	       },
+	[10] = {
+		.mask = 0x00000040,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 1,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[11] = {
+		.mask = 0x00000020,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 2,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[12] = {
+		.mask = 0x00000010,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 3,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[13] = {
+		.mask = 0x00000008,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 4,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[14] = {
+		.mask = 0x00000004,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 5,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[15] = {
+		.mask = 0x00000002,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 6,
+		.pri_reg = QEIC_CIPZCC,
+		},
+	[20] = {
+		.mask = 0x10000000,
+		.mask_reg = QEIC_CRIMR,
+		.pri_code = 3,
+		.pri_reg = QEIC_CIPRTA,
+		},
+	[25] = {
+		.mask = 0x00800000,
+		.mask_reg = QEIC_CRIMR,
+		.pri_code = 0,
+		.pri_reg = QEIC_CIPRTB,
+		},
+	[26] = {
+		.mask = 0x00400000,
+		.mask_reg = QEIC_CRIMR,
+		.pri_code = 1,
+		.pri_reg = QEIC_CIPRTB,
+		},
+	[27] = {
+		.mask = 0x00200000,
+		.mask_reg = QEIC_CRIMR,
+		.pri_code = 2,
+		.pri_reg = QEIC_CIPRTB,
+		},
+	[28] = {
+		.mask = 0x00100000,
+		.mask_reg = QEIC_CRIMR,
+		.pri_code = 3,
+		.pri_reg = QEIC_CIPRTB,
+		},
+	[32] = {
+		.mask = 0x80000000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 0,
+		.pri_reg = QEIC_CIPXCC,
+		},
+	[33] = {
+		.mask = 0x40000000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 1,
+		.pri_reg = QEIC_CIPXCC,
+		},
+	[34] = {
+		.mask = 0x20000000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 2,
+		.pri_reg = QEIC_CIPXCC,
+		},
+	[35] = {
+		.mask = 0x10000000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 3,
+		.pri_reg = QEIC_CIPXCC,
+		},
+	[36] = {
+		.mask = 0x08000000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 4,
+		.pri_reg = QEIC_CIPXCC,
+		},
+	[40] = {
+		.mask = 0x00800000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 0,
+		.pri_reg = QEIC_CIPYCC,
+		},
+	[41] = {
+		.mask = 0x00400000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 1,
+		.pri_reg = QEIC_CIPYCC,
+		},
+	[42] = {
+		.mask = 0x00200000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 2,
+		.pri_reg = QEIC_CIPYCC,
+		},
+	[43] = {
+		.mask = 0x00100000,
+		.mask_reg = QEIC_CIMR,
+		.pri_code = 3,
+		.pri_reg = QEIC_CIPYCC,
+		},
+};
+
+static inline u32 qe_ic_read(volatile __be32  __iomem * base, unsigned int reg)
+{
+	return in_be32(base + (reg >> 2));
+}
+
+static inline void qe_ic_write(volatile __be32  __iomem * base, unsigned int reg,
+			       u32 value)
+{
+	out_be32(base + (reg >> 2), value);
+}
+
+static inline struct qe_ic *qe_ic_from_irq(unsigned int virq)
+{
+	return irq_get_chip_data(virq);
+}
+
+static inline struct qe_ic *qe_ic_from_irq_data(struct irq_data *d)
+{
+	return irq_data_get_irq_chip_data(d);
+}
+
+static void qe_ic_unmask_irq(struct irq_data *d)
+{
+	struct qe_ic *qe_ic = qe_ic_from_irq_data(d);
+	unsigned int src = irqd_to_hwirq(d);
+	unsigned long flags;
+	u32 temp;
+
+	raw_spin_lock_irqsave(&qe_ic_lock, flags);
+
+	temp = qe_ic_read(qe_ic->regs, qe_ic_info[src].mask_reg);
+	qe_ic_write(qe_ic->regs, qe_ic_info[src].mask_reg,
+		    temp | qe_ic_info[src].mask);
+
+	raw_spin_unlock_irqrestore(&qe_ic_lock, flags);
+}
+
+static void qe_ic_mask_irq(struct irq_data *d)
+{
+	struct qe_ic *qe_ic = qe_ic_from_irq_data(d);
+	unsigned int src = irqd_to_hwirq(d);
+	unsigned long flags;
+	u32 temp;
+
+	raw_spin_lock_irqsave(&qe_ic_lock, flags);
+
+	temp = qe_ic_read(qe_ic->regs, qe_ic_info[src].mask_reg);
+	qe_ic_write(qe_ic->regs, qe_ic_info[src].mask_reg,
+		    temp & ~qe_ic_info[src].mask);
+
+	/* Flush the above write before enabling interrupts; otherwise,
+	 * spurious interrupts will sometimes happen.  To be 100% sure
+	 * that the write has reached the device before interrupts are
+	 * enabled, the mask register would have to be read back; however,
+	 * this is not required for correctness, only to avoid wasting
+	 * time on a large number of spurious interrupts.  In testing,
+	 * a sync reduced the observed spurious interrupts to zero.
+	 */
+	mb();
+
+	raw_spin_unlock_irqrestore(&qe_ic_lock, flags);
+}
+
+static struct irq_chip qe_ic_irq_chip = {
+	.name = "QEIC",
+	.irq_unmask = qe_ic_unmask_irq,
+	.irq_mask = qe_ic_mask_irq,
+	.irq_mask_ack = qe_ic_mask_irq,
+};
+
+static int qe_ic_host_match(struct irq_domain *h, struct device_node *node,
+			    enum irq_domain_bus_token bus_token)
+{
+	/* Exact match, unless qe_ic node is NULL */
+	struct device_node *of_node = irq_domain_get_of_node(h);
+	return of_node == NULL || of_node == node;
+}
+
+static int qe_ic_host_map(struct irq_domain *h, unsigned int virq,
+			  irq_hw_number_t hw)
+{
+	struct qe_ic *qe_ic = h->host_data;
+	struct irq_chip *chip;
+
+	if (qe_ic_info[hw].mask == 0) {
+		printk(KERN_ERR "Can't map reserved IRQ\n");
+		return -EINVAL;
+	}
+	/* Default chip */
+	chip = &qe_ic->hc_irq;
+
+	irq_set_chip_data(virq, qe_ic);
+	irq_set_status_flags(virq, IRQ_LEVEL);
+
+	irq_set_chip_and_handler(virq, chip, handle_level_irq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops qe_ic_host_ops = {
+	.match = qe_ic_host_match,
+	.map = qe_ic_host_map,
+	.xlate = irq_domain_xlate_onetwocell,
+};
+
+/* Return an interrupt vector or NO_IRQ if no interrupt is pending. */
+unsigned int qe_ic_get_low_irq(struct qe_ic *qe_ic)
+{
+	int irq;
+
+	BUG_ON(qe_ic == NULL);
+
+	/* get the interrupt source vector. */
+	irq = qe_ic_read(qe_ic->regs, QEIC_CIVEC) >> 26;
+
+	if (irq == 0)
+		return NO_IRQ;
+
+	return irq_linear_revmap(qe_ic->irqhost, irq);
+}
+
+/* Return an interrupt vector or NO_IRQ if no interrupt is pending. */
+unsigned int qe_ic_get_high_irq(struct qe_ic *qe_ic)
+{
+	int irq;
+
+	BUG_ON(qe_ic == NULL);
+
+	/* get the interrupt source vector. */
+	irq = qe_ic_read(qe_ic->regs, QEIC_CHIVEC) >> 26;
+
+	if (irq == 0)
+		return NO_IRQ;
+
+	return irq_linear_revmap(qe_ic->irqhost, irq);
+}
+
+void __init qe_ic_init(struct device_node *node, unsigned int flags,
+		       void (*low_handler)(struct irq_desc *desc),
+		       void (*high_handler)(struct irq_desc *desc))
+{
+	struct qe_ic *qe_ic;
+	struct resource res;
+	u32 temp = 0, ret, high_active = 0;
+
+	ret = of_address_to_resource(node, 0, &res);
+	if (ret)
+		return;
+
+	qe_ic = kzalloc(sizeof(*qe_ic), GFP_KERNEL);
+	if (qe_ic == NULL)
+		return;
+
+	qe_ic->irqhost = irq_domain_add_linear(node, NR_QE_IC_INTS,
+					       &qe_ic_host_ops, qe_ic);
+	if (qe_ic->irqhost == NULL) {
+		kfree(qe_ic);
+		return;
+	}
+
+	qe_ic->regs = ioremap(res.start, resource_size(&res));
+
+	qe_ic->hc_irq = qe_ic_irq_chip;
+
+	qe_ic->virq_high = irq_of_parse_and_map(node, 0);
+	qe_ic->virq_low = irq_of_parse_and_map(node, 1);
+
+	if (qe_ic->virq_low == NO_IRQ) {
+		printk(KERN_ERR "Failed to map QE_IC low IRQ\n");
+		kfree(qe_ic);
+		return;
+	}
+
+	/* default priority scheme is grouped. If spread mode is    */
+	/* required, configure cicr accordingly.                    */
+	if (flags & QE_IC_SPREADMODE_GRP_W)
+		temp |= CICR_GWCC;
+	if (flags & QE_IC_SPREADMODE_GRP_X)
+		temp |= CICR_GXCC;
+	if (flags & QE_IC_SPREADMODE_GRP_Y)
+		temp |= CICR_GYCC;
+	if (flags & QE_IC_SPREADMODE_GRP_Z)
+		temp |= CICR_GZCC;
+	if (flags & QE_IC_SPREADMODE_GRP_RISCA)
+		temp |= CICR_GRTA;
+	if (flags & QE_IC_SPREADMODE_GRP_RISCB)
+		temp |= CICR_GRTB;
+
+	/* choose destination signal for highest priority interrupt */
+	if (flags & QE_IC_HIGH_SIGNAL) {
+		temp |= (SIGNAL_HIGH << CICR_HPIT_SHIFT);
+		high_active = 1;
+	}
+
+	qe_ic_write(qe_ic->regs, QEIC_CICR, temp);
+
+	irq_set_handler_data(qe_ic->virq_low, qe_ic);
+	irq_set_chained_handler(qe_ic->virq_low, low_handler);
+
+	if (qe_ic->virq_high != NO_IRQ &&
+			qe_ic->virq_high != qe_ic->virq_low) {
+		irq_set_handler_data(qe_ic->virq_high, qe_ic);
+		irq_set_chained_handler(qe_ic->virq_high, high_handler);
+	}
+}
+
+void qe_ic_set_highest_priority(unsigned int virq, int high)
+{
+	struct qe_ic *qe_ic = qe_ic_from_irq(virq);
+	unsigned int src = virq_to_hw(virq);
+	u32 temp = 0;
+
+	temp = qe_ic_read(qe_ic->regs, QEIC_CICR);
+
+	temp &= ~CICR_HP_MASK;
+	temp |= src << CICR_HP_SHIFT;
+
+	temp &= ~CICR_HPIT_MASK;
+	temp |= (high ? SIGNAL_HIGH : SIGNAL_LOW) << CICR_HPIT_SHIFT;
+
+	qe_ic_write(qe_ic->regs, QEIC_CICR, temp);
+}
+
+/* Set Priority level within its group, from 1 to 8 */
+int qe_ic_set_priority(unsigned int virq, unsigned int priority)
+{
+	struct qe_ic *qe_ic = qe_ic_from_irq(virq);
+	unsigned int src = virq_to_hw(virq);
+	u32 temp;
+
+	if (priority > 8 || priority == 0)
+		return -EINVAL;
+	if (src > 127)
+		return -EINVAL;
+	if (qe_ic_info[src].pri_reg == 0)
+		return -EINVAL;
+
+	temp = qe_ic_read(qe_ic->regs, qe_ic_info[src].pri_reg);
+
+	if (priority < 4) {
+		temp &= ~(0x7 << (32 - priority * 3));
+		temp |= qe_ic_info[src].pri_code << (32 - priority * 3);
+	} else {
+		temp &= ~(0x7 << (24 - priority * 3));
+		temp |= qe_ic_info[src].pri_code << (24 - priority * 3);
+	}
+
+	qe_ic_write(qe_ic->regs, qe_ic_info[src].pri_reg, temp);
+
+	return 0;
+}
+
+/* Set a QE priority to use high irq, only priority 1~2 can use high irq */
+int qe_ic_set_high_priority(unsigned int virq, unsigned int priority, int high)
+{
+	struct qe_ic *qe_ic = qe_ic_from_irq(virq);
+	unsigned int src = virq_to_hw(virq);
+	u32 temp, control_reg = QEIC_CICNR, shift = 0;
+
+	if (priority > 2 || priority == 0)
+		return -EINVAL;
+
+	switch (qe_ic_info[src].pri_reg) {
+	case QEIC_CIPZCC:
+		shift = CICNR_ZCC1T_SHIFT;
+		break;
+	case QEIC_CIPWCC:
+		shift = CICNR_WCC1T_SHIFT;
+		break;
+	case QEIC_CIPYCC:
+		shift = CICNR_YCC1T_SHIFT;
+		break;
+	case QEIC_CIPXCC:
+		shift = CICNR_XCC1T_SHIFT;
+		break;
+	case QEIC_CIPRTA:
+		shift = CRICR_RTA1T_SHIFT;
+		control_reg = QEIC_CRICR;
+		break;
+	case QEIC_CIPRTB:
+		shift = CRICR_RTB1T_SHIFT;
+		control_reg = QEIC_CRICR;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	shift += (2 - priority) * 2;
+	temp = qe_ic_read(qe_ic->regs, control_reg);
+	temp &= ~(SIGNAL_MASK << shift);
+	temp |= (high ? SIGNAL_HIGH : SIGNAL_LOW) << shift;
+	qe_ic_write(qe_ic->regs, control_reg, temp);
+
+	return 0;
+}
+
+static struct bus_type qe_ic_subsys = {
+	.name = "qe_ic",
+	.dev_name = "qe_ic",
+};
+
+static struct device device_qe_ic = {
+	.id = 0,
+	.bus = &qe_ic_subsys,
+};
+
+static int __init init_qe_ic_sysfs(void)
+{
+	int rc;
+
+	printk(KERN_DEBUG "Registering qe_ic with sysfs...\n");
+
+	rc = subsys_system_register(&qe_ic_subsys, NULL);
+	if (rc) {
+		printk(KERN_ERR "Failed registering qe_ic sys class\n");
+		return -ENODEV;
+	}
+	rc = device_register(&device_qe_ic);
+	if (rc) {
+		printk(KERN_ERR "Failed registering qe_ic sys device\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+subsys_initcall(init_qe_ic_sysfs);
diff --git a/drivers/soc/fsl/qe/qe_ic.h b/drivers/soc/fsl/qe/qe_ic.h
new file mode 100644
index 000000000000..926a2ed42319
--- /dev/null
+++ b/drivers/soc/fsl/qe/qe_ic.h
@@ -0,0 +1,103 @@
+/*
+ * drivers/soc/fsl/qe/qe_ic.h
+ *
+ * QUICC ENGINE Interrupt Controller Header
+ *
+ * Copyright (C) 2006 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Author: Li Yang <leoli@freescale.com>
+ * Based on code from Shlomi Gridish <gridish@freescale.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 _POWERPC_SYSDEV_QE_IC_H
+#define _POWERPC_SYSDEV_QE_IC_H
+
+#include <soc/fsl/qe/qe_ic.h>
+
+#define NR_QE_IC_INTS		64
+
+/* QE IC registers offset */
+#define QEIC_CICR		0x00
+#define QEIC_CIVEC		0x04
+#define QEIC_CRIPNR		0x08
+#define QEIC_CIPNR		0x0c
+#define QEIC_CIPXCC		0x10
+#define QEIC_CIPYCC		0x14
+#define QEIC_CIPWCC		0x18
+#define QEIC_CIPZCC		0x1c
+#define QEIC_CIMR		0x20
+#define QEIC_CRIMR		0x24
+#define QEIC_CICNR		0x28
+#define QEIC_CIPRTA		0x30
+#define QEIC_CIPRTB		0x34
+#define QEIC_CRICR		0x3c
+#define QEIC_CHIVEC		0x60
+
+/* Interrupt priority registers */
+#define CIPCC_SHIFT_PRI0	29
+#define CIPCC_SHIFT_PRI1	26
+#define CIPCC_SHIFT_PRI2	23
+#define CIPCC_SHIFT_PRI3	20
+#define CIPCC_SHIFT_PRI4	13
+#define CIPCC_SHIFT_PRI5	10
+#define CIPCC_SHIFT_PRI6	7
+#define CIPCC_SHIFT_PRI7	4
+
+/* CICR priority modes */
+#define CICR_GWCC		0x00040000
+#define CICR_GXCC		0x00020000
+#define CICR_GYCC		0x00010000
+#define CICR_GZCC		0x00080000
+#define CICR_GRTA		0x00200000
+#define CICR_GRTB		0x00400000
+#define CICR_HPIT_SHIFT		8
+#define CICR_HPIT_MASK		0x00000300
+#define CICR_HP_SHIFT		24
+#define CICR_HP_MASK		0x3f000000
+
+/* CICNR */
+#define CICNR_WCC1T_SHIFT	20
+#define CICNR_ZCC1T_SHIFT	28
+#define CICNR_YCC1T_SHIFT	12
+#define CICNR_XCC1T_SHIFT	4
+
+/* CRICR */
+#define CRICR_RTA1T_SHIFT	20
+#define CRICR_RTB1T_SHIFT	28
+
+/* Signal indicator */
+#define SIGNAL_MASK		3
+#define SIGNAL_HIGH		2
+#define SIGNAL_LOW		0
+
+struct qe_ic {
+	/* Control registers offset */
+	volatile u32 __iomem *regs;
+
+	/* The remapper for this QEIC */
+	struct irq_domain *irqhost;
+
+	/* The "linux" controller struct */
+	struct irq_chip hc_irq;
+
+	/* VIRQ numbers of QE high/low irqs */
+	unsigned int virq_high;
+	unsigned int virq_low;
+};
+
+/*
+ * QE interrupt controller internal structure
+ */
+struct qe_ic_info {
+	u32	mask;	  /* location of this source at the QIMR register. */
+	u32	mask_reg; /* Mask register offset */
+	u8	pri_code; /* for grouped interrupts sources - the interrupt
+			     code as appears at the group priority register */
+	u32	pri_reg;  /* Group priority register offset */
+};
+
+#endif /* _POWERPC_SYSDEV_QE_IC_H */
diff --git a/drivers/soc/fsl/qe/qe_io.c b/drivers/soc/fsl/qe/qe_io.c
new file mode 100644
index 000000000000..7ae59abc7863
--- /dev/null
+++ b/drivers/soc/fsl/qe/qe_io.c
@@ -0,0 +1,192 @@
+/*
+ * arch/powerpc/sysdev/qe_lib/qe_io.c
+ *
+ * QE Parallel I/O ports configuration routines
+ *
+ * Copyright 2006 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Author: Li Yang <LeoLi@freescale.com>
+ * Based on code from Shlomi Gridish <gridish@freescale.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/stddef.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/module.h>
+#include <linux/ioport.h>
+
+#include <asm/io.h>
+#include <soc/fsl/qe/qe.h>
+#include <asm/prom.h>
+#include <sysdev/fsl_soc.h>
+
+#undef DEBUG
+
+static struct qe_pio_regs __iomem *par_io;
+static int num_par_io_ports = 0;
+
+int par_io_init(struct device_node *np)
+{
+	struct resource res;
+	int ret;
+	const u32 *num_ports;
+
+	/* Map Parallel I/O ports registers */
+	ret = of_address_to_resource(np, 0, &res);
+	if (ret)
+		return ret;
+	par_io = ioremap(res.start, resource_size(&res));
+
+	num_ports = of_get_property(np, "num-ports", NULL);
+	if (num_ports)
+		num_par_io_ports = *num_ports;
+
+	return 0;
+}
+
+void __par_io_config_pin(struct qe_pio_regs __iomem *par_io, u8 pin, int dir,
+			 int open_drain, int assignment, int has_irq)
+{
+	u32 pin_mask1bit;
+	u32 pin_mask2bits;
+	u32 new_mask2bits;
+	u32 tmp_val;
+
+	/* calculate pin location for single and 2 bits information */
+	pin_mask1bit = (u32) (1 << (QE_PIO_PINS - (pin + 1)));
+
+	/* Set open drain, if required */
+	tmp_val = in_be32(&par_io->cpodr);
+	if (open_drain)
+		out_be32(&par_io->cpodr, pin_mask1bit | tmp_val);
+	else
+		out_be32(&par_io->cpodr, ~pin_mask1bit & tmp_val);
+
+	/* define direction */
+	tmp_val = (pin > (QE_PIO_PINS / 2) - 1) ?
+		in_be32(&par_io->cpdir2) :
+		in_be32(&par_io->cpdir1);
+
+	/* get all bits mask for 2 bit per port */
+	pin_mask2bits = (u32) (0x3 << (QE_PIO_PINS -
+				(pin % (QE_PIO_PINS / 2) + 1) * 2));
+
+	/* Get the final mask we need for the right definition */
+	new_mask2bits = (u32) (dir << (QE_PIO_PINS -
+				(pin % (QE_PIO_PINS / 2) + 1) * 2));
+
+	/* clear and set 2 bits mask */
+	if (pin > (QE_PIO_PINS / 2) - 1) {
+		out_be32(&par_io->cpdir2,
+			 ~pin_mask2bits & tmp_val);
+		tmp_val &= ~pin_mask2bits;
+		out_be32(&par_io->cpdir2, new_mask2bits | tmp_val);
+	} else {
+		out_be32(&par_io->cpdir1,
+			 ~pin_mask2bits & tmp_val);
+		tmp_val &= ~pin_mask2bits;
+		out_be32(&par_io->cpdir1, new_mask2bits | tmp_val);
+	}
+	/* define pin assignment */
+	tmp_val = (pin > (QE_PIO_PINS / 2) - 1) ?
+		in_be32(&par_io->cppar2) :
+		in_be32(&par_io->cppar1);
+
+	new_mask2bits = (u32) (assignment << (QE_PIO_PINS -
+			(pin % (QE_PIO_PINS / 2) + 1) * 2));
+	/* clear and set 2 bits mask */
+	if (pin > (QE_PIO_PINS / 2) - 1) {
+		out_be32(&par_io->cppar2,
+			 ~pin_mask2bits & tmp_val);
+		tmp_val &= ~pin_mask2bits;
+		out_be32(&par_io->cppar2, new_mask2bits | tmp_val);
+	} else {
+		out_be32(&par_io->cppar1,
+			 ~pin_mask2bits & tmp_val);
+		tmp_val &= ~pin_mask2bits;
+		out_be32(&par_io->cppar1, new_mask2bits | tmp_val);
+	}
+}
+EXPORT_SYMBOL(__par_io_config_pin);
+
+int par_io_config_pin(u8 port, u8 pin, int dir, int open_drain,
+		      int assignment, int has_irq)
+{
+	if (!par_io || port >= num_par_io_ports)
+		return -EINVAL;
+
+	__par_io_config_pin(&par_io[port], pin, dir, open_drain, assignment,
+			    has_irq);
+	return 0;
+}
+EXPORT_SYMBOL(par_io_config_pin);
+
+int par_io_data_set(u8 port, u8 pin, u8 val)
+{
+	u32 pin_mask, tmp_val;
+
+	if (port >= num_par_io_ports)
+		return -EINVAL;
+	if (pin >= QE_PIO_PINS)
+		return -EINVAL;
+	/* calculate pin location */
+	pin_mask = (u32) (1 << (QE_PIO_PINS - 1 - pin));
+
+	tmp_val = in_be32(&par_io[port].cpdata);
+
+	if (val == 0)		/* clear */
+		out_be32(&par_io[port].cpdata, ~pin_mask & tmp_val);
+	else			/* set */
+		out_be32(&par_io[port].cpdata, pin_mask | tmp_val);
+
+	return 0;
+}
+EXPORT_SYMBOL(par_io_data_set);
+
+int par_io_of_config(struct device_node *np)
+{
+	struct device_node *pio;
+	const phandle *ph;
+	int pio_map_len;
+	const unsigned int *pio_map;
+
+	if (par_io == NULL) {
+		printk(KERN_ERR "par_io not initialized\n");
+		return -1;
+	}
+
+	ph = of_get_property(np, "pio-handle", NULL);
+	if (ph == NULL) {
+		printk(KERN_ERR "pio-handle not available\n");
+		return -1;
+	}
+
+	pio = of_find_node_by_phandle(*ph);
+
+	pio_map = of_get_property(pio, "pio-map", &pio_map_len);
+	if (pio_map == NULL) {
+		printk(KERN_ERR "pio-map is not set!\n");
+		return -1;
+	}
+	pio_map_len /= sizeof(unsigned int);
+	if ((pio_map_len % 6) != 0) {
+		printk(KERN_ERR "pio-map format wrong!\n");
+		return -1;
+	}
+
+	while (pio_map_len > 0) {
+		par_io_config_pin((u8) pio_map[0], (u8) pio_map[1],
+				(int) pio_map[2], (int) pio_map[3],
+				(int) pio_map[4], (int) pio_map[5]);
+		pio_map += 6;
+		pio_map_len -= 6;
+	}
+	of_node_put(pio);
+	return 0;
+}
+EXPORT_SYMBOL(par_io_of_config);
diff --git a/drivers/soc/fsl/qe/ucc.c b/drivers/soc/fsl/qe/ucc.c
new file mode 100644
index 000000000000..b59d3358f9bd
--- /dev/null
+++ b/drivers/soc/fsl/qe/ucc.c
@@ -0,0 +1,212 @@
+/*
+ * arch/powerpc/sysdev/qe_lib/ucc.c
+ *
+ * QE UCC API Set - UCC specific routines implementations.
+ *
+ * Copyright (C) 2006 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Authors: 	Shlomi Gridish <gridish@freescale.com>
+ * 		Li Yang <leoli@freescale.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/stddef.h>
+#include <linux/spinlock.h>
+#include <linux/export.h>
+
+#include <asm/irq.h>
+#include <asm/io.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+#include <soc/fsl/qe/ucc.h>
+
+int ucc_set_qe_mux_mii_mng(unsigned int ucc_num)
+{
+	unsigned long flags;
+
+	if (ucc_num > UCC_MAX_NUM - 1)
+		return -EINVAL;
+
+	spin_lock_irqsave(&cmxgcr_lock, flags);
+	clrsetbits_be32(&qe_immr->qmx.cmxgcr, QE_CMXGCR_MII_ENET_MNG,
+		ucc_num << QE_CMXGCR_MII_ENET_MNG_SHIFT);
+	spin_unlock_irqrestore(&cmxgcr_lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(ucc_set_qe_mux_mii_mng);
+
+/* Configure the UCC to either Slow or Fast.
+ *
+ * A given UCC can be figured to support either "slow" devices (e.g. UART)
+ * or "fast" devices (e.g. Ethernet).
+ *
+ * 'ucc_num' is the UCC number, from 0 - 7.
+ *
+ * This function also sets the UCC_GUEMR_SET_RESERVED3 bit because that bit
+ * must always be set to 1.
+ */
+int ucc_set_type(unsigned int ucc_num, enum ucc_speed_type speed)
+{
+	u8 __iomem *guemr;
+
+	/* The GUEMR register is at the same location for both slow and fast
+	   devices, so we just use uccX.slow.guemr. */
+	switch (ucc_num) {
+	case 0: guemr = &qe_immr->ucc1.slow.guemr;
+		break;
+	case 1: guemr = &qe_immr->ucc2.slow.guemr;
+		break;
+	case 2: guemr = &qe_immr->ucc3.slow.guemr;
+		break;
+	case 3: guemr = &qe_immr->ucc4.slow.guemr;
+		break;
+	case 4: guemr = &qe_immr->ucc5.slow.guemr;
+		break;
+	case 5: guemr = &qe_immr->ucc6.slow.guemr;
+		break;
+	case 6: guemr = &qe_immr->ucc7.slow.guemr;
+		break;
+	case 7: guemr = &qe_immr->ucc8.slow.guemr;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	clrsetbits_8(guemr, UCC_GUEMR_MODE_MASK,
+		UCC_GUEMR_SET_RESERVED3 | speed);
+
+	return 0;
+}
+
+static void get_cmxucr_reg(unsigned int ucc_num, __be32 __iomem **cmxucr,
+	unsigned int *reg_num, unsigned int *shift)
+{
+	unsigned int cmx = ((ucc_num & 1) << 1) + (ucc_num > 3);
+
+	*reg_num = cmx + 1;
+	*cmxucr = &qe_immr->qmx.cmxucr[cmx];
+	*shift = 16 - 8 * (ucc_num & 2);
+}
+
+int ucc_mux_set_grant_tsa_bkpt(unsigned int ucc_num, int set, u32 mask)
+{
+	__be32 __iomem *cmxucr;
+	unsigned int reg_num;
+	unsigned int shift;
+
+	/* check if the UCC number is in range. */
+	if (ucc_num > UCC_MAX_NUM - 1)
+		return -EINVAL;
+
+	get_cmxucr_reg(ucc_num, &cmxucr, &reg_num, &shift);
+
+	if (set)
+		setbits32(cmxucr, mask << shift);
+	else
+		clrbits32(cmxucr, mask << shift);
+
+	return 0;
+}
+
+int ucc_set_qe_mux_rxtx(unsigned int ucc_num, enum qe_clock clock,
+	enum comm_dir mode)
+{
+	__be32 __iomem *cmxucr;
+	unsigned int reg_num;
+	unsigned int shift;
+	u32 clock_bits = 0;
+
+	/* check if the UCC number is in range. */
+	if (ucc_num > UCC_MAX_NUM - 1)
+		return -EINVAL;
+
+	/* The communications direction must be RX or TX */
+	if (!((mode == COMM_DIR_RX) || (mode == COMM_DIR_TX)))
+		return -EINVAL;
+
+	get_cmxucr_reg(ucc_num, &cmxucr, &reg_num, &shift);
+
+	switch (reg_num) {
+	case 1:
+		switch (clock) {
+		case QE_BRG1:	clock_bits = 1; break;
+		case QE_BRG2:	clock_bits = 2; break;
+		case QE_BRG7:	clock_bits = 3; break;
+		case QE_BRG8:	clock_bits = 4; break;
+		case QE_CLK9:	clock_bits = 5; break;
+		case QE_CLK10:	clock_bits = 6; break;
+		case QE_CLK11:	clock_bits = 7; break;
+		case QE_CLK12:	clock_bits = 8; break;
+		case QE_CLK15:	clock_bits = 9; break;
+		case QE_CLK16:	clock_bits = 10; break;
+		default: break;
+		}
+		break;
+	case 2:
+		switch (clock) {
+		case QE_BRG5:	clock_bits = 1; break;
+		case QE_BRG6:	clock_bits = 2; break;
+		case QE_BRG7:	clock_bits = 3; break;
+		case QE_BRG8:	clock_bits = 4; break;
+		case QE_CLK13:	clock_bits = 5; break;
+		case QE_CLK14:	clock_bits = 6; break;
+		case QE_CLK19:	clock_bits = 7; break;
+		case QE_CLK20:	clock_bits = 8; break;
+		case QE_CLK15:	clock_bits = 9; break;
+		case QE_CLK16:	clock_bits = 10; break;
+		default: break;
+		}
+		break;
+	case 3:
+		switch (clock) {
+		case QE_BRG9:	clock_bits = 1; break;
+		case QE_BRG10:	clock_bits = 2; break;
+		case QE_BRG15:	clock_bits = 3; break;
+		case QE_BRG16:	clock_bits = 4; break;
+		case QE_CLK3:	clock_bits = 5; break;
+		case QE_CLK4:	clock_bits = 6; break;
+		case QE_CLK17:	clock_bits = 7; break;
+		case QE_CLK18:	clock_bits = 8; break;
+		case QE_CLK7:	clock_bits = 9; break;
+		case QE_CLK8:	clock_bits = 10; break;
+		case QE_CLK16:	clock_bits = 11; break;
+		default: break;
+		}
+		break;
+	case 4:
+		switch (clock) {
+		case QE_BRG13:	clock_bits = 1; break;
+		case QE_BRG14:	clock_bits = 2; break;
+		case QE_BRG15:	clock_bits = 3; break;
+		case QE_BRG16:	clock_bits = 4; break;
+		case QE_CLK5:	clock_bits = 5; break;
+		case QE_CLK6:	clock_bits = 6; break;
+		case QE_CLK21:	clock_bits = 7; break;
+		case QE_CLK22:	clock_bits = 8; break;
+		case QE_CLK7:	clock_bits = 9; break;
+		case QE_CLK8:	clock_bits = 10; break;
+		case QE_CLK16:	clock_bits = 11; break;
+		default: break;
+		}
+		break;
+	default: break;
+	}
+
+	/* Check for invalid combination of clock and UCC number */
+	if (!clock_bits)
+		return -ENOENT;
+
+	if (mode == COMM_DIR_RX)
+		shift += 4;
+
+	clrsetbits_be32(cmxucr, QE_CMXUCR_TX_CLK_SRC_MASK << shift,
+		clock_bits << shift);
+
+	return 0;
+}
diff --git a/drivers/soc/fsl/qe/ucc_fast.c b/drivers/soc/fsl/qe/ucc_fast.c
new file mode 100644
index 000000000000..a7689310fe40
--- /dev/null
+++ b/drivers/soc/fsl/qe/ucc_fast.c
@@ -0,0 +1,363 @@
+/*
+ * Copyright (C) 2006 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Authors: 	Shlomi Gridish <gridish@freescale.com>
+ * 		Li Yang <leoli@freescale.com>
+ *
+ * Description:
+ * QE UCC Fast API Set - UCC Fast specific routines implementations.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the 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/errno.h>
+#include <linux/slab.h>
+#include <linux/stddef.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/export.h>
+
+#include <asm/io.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+
+#include <soc/fsl/qe/ucc.h>
+#include <soc/fsl/qe/ucc_fast.h>
+
+void ucc_fast_dump_regs(struct ucc_fast_private * uccf)
+{
+	printk(KERN_INFO "UCC%u Fast registers:\n", uccf->uf_info->ucc_num);
+	printk(KERN_INFO "Base address: 0x%p\n", uccf->uf_regs);
+
+	printk(KERN_INFO "gumr  : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->gumr, in_be32(&uccf->uf_regs->gumr));
+	printk(KERN_INFO "upsmr : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->upsmr, in_be32(&uccf->uf_regs->upsmr));
+	printk(KERN_INFO "utodr : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->utodr, in_be16(&uccf->uf_regs->utodr));
+	printk(KERN_INFO "udsr  : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->udsr, in_be16(&uccf->uf_regs->udsr));
+	printk(KERN_INFO "ucce  : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->ucce, in_be32(&uccf->uf_regs->ucce));
+	printk(KERN_INFO "uccm  : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->uccm, in_be32(&uccf->uf_regs->uccm));
+	printk(KERN_INFO "uccs  : addr=0x%p, val=0x%02x\n",
+		  &uccf->uf_regs->uccs, in_8(&uccf->uf_regs->uccs));
+	printk(KERN_INFO "urfb  : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->urfb, in_be32(&uccf->uf_regs->urfb));
+	printk(KERN_INFO "urfs  : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->urfs, in_be16(&uccf->uf_regs->urfs));
+	printk(KERN_INFO "urfet : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->urfet, in_be16(&uccf->uf_regs->urfet));
+	printk(KERN_INFO "urfset: addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->urfset, in_be16(&uccf->uf_regs->urfset));
+	printk(KERN_INFO "utfb  : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->utfb, in_be32(&uccf->uf_regs->utfb));
+	printk(KERN_INFO "utfs  : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->utfs, in_be16(&uccf->uf_regs->utfs));
+	printk(KERN_INFO "utfet : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->utfet, in_be16(&uccf->uf_regs->utfet));
+	printk(KERN_INFO "utftt : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->utftt, in_be16(&uccf->uf_regs->utftt));
+	printk(KERN_INFO "utpt  : addr=0x%p, val=0x%04x\n",
+		  &uccf->uf_regs->utpt, in_be16(&uccf->uf_regs->utpt));
+	printk(KERN_INFO "urtry : addr=0x%p, val=0x%08x\n",
+		  &uccf->uf_regs->urtry, in_be32(&uccf->uf_regs->urtry));
+	printk(KERN_INFO "guemr : addr=0x%p, val=0x%02x\n",
+		  &uccf->uf_regs->guemr, in_8(&uccf->uf_regs->guemr));
+}
+EXPORT_SYMBOL(ucc_fast_dump_regs);
+
+u32 ucc_fast_get_qe_cr_subblock(int uccf_num)
+{
+	switch (uccf_num) {
+	case 0: return QE_CR_SUBBLOCK_UCCFAST1;
+	case 1: return QE_CR_SUBBLOCK_UCCFAST2;
+	case 2: return QE_CR_SUBBLOCK_UCCFAST3;
+	case 3: return QE_CR_SUBBLOCK_UCCFAST4;
+	case 4: return QE_CR_SUBBLOCK_UCCFAST5;
+	case 5: return QE_CR_SUBBLOCK_UCCFAST6;
+	case 6: return QE_CR_SUBBLOCK_UCCFAST7;
+	case 7: return QE_CR_SUBBLOCK_UCCFAST8;
+	default: return QE_CR_SUBBLOCK_INVALID;
+	}
+}
+EXPORT_SYMBOL(ucc_fast_get_qe_cr_subblock);
+
+void ucc_fast_transmit_on_demand(struct ucc_fast_private * uccf)
+{
+	out_be16(&uccf->uf_regs->utodr, UCC_FAST_TOD);
+}
+EXPORT_SYMBOL(ucc_fast_transmit_on_demand);
+
+void ucc_fast_enable(struct ucc_fast_private * uccf, enum comm_dir mode)
+{
+	struct ucc_fast __iomem *uf_regs;
+	u32 gumr;
+
+	uf_regs = uccf->uf_regs;
+
+	/* Enable reception and/or transmission on this UCC. */
+	gumr = in_be32(&uf_regs->gumr);
+	if (mode & COMM_DIR_TX) {
+		gumr |= UCC_FAST_GUMR_ENT;
+		uccf->enabled_tx = 1;
+	}
+	if (mode & COMM_DIR_RX) {
+		gumr |= UCC_FAST_GUMR_ENR;
+		uccf->enabled_rx = 1;
+	}
+	out_be32(&uf_regs->gumr, gumr);
+}
+EXPORT_SYMBOL(ucc_fast_enable);
+
+void ucc_fast_disable(struct ucc_fast_private * uccf, enum comm_dir mode)
+{
+	struct ucc_fast __iomem *uf_regs;
+	u32 gumr;
+
+	uf_regs = uccf->uf_regs;
+
+	/* Disable reception and/or transmission on this UCC. */
+	gumr = in_be32(&uf_regs->gumr);
+	if (mode & COMM_DIR_TX) {
+		gumr &= ~UCC_FAST_GUMR_ENT;
+		uccf->enabled_tx = 0;
+	}
+	if (mode & COMM_DIR_RX) {
+		gumr &= ~UCC_FAST_GUMR_ENR;
+		uccf->enabled_rx = 0;
+	}
+	out_be32(&uf_regs->gumr, gumr);
+}
+EXPORT_SYMBOL(ucc_fast_disable);
+
+int ucc_fast_init(struct ucc_fast_info * uf_info, struct ucc_fast_private ** uccf_ret)
+{
+	struct ucc_fast_private *uccf;
+	struct ucc_fast __iomem *uf_regs;
+	u32 gumr;
+	int ret;
+
+	if (!uf_info)
+		return -EINVAL;
+
+	/* check if the UCC port number is in range. */
+	if ((uf_info->ucc_num < 0) || (uf_info->ucc_num > UCC_MAX_NUM - 1)) {
+		printk(KERN_ERR "%s: illegal UCC number\n", __func__);
+		return -EINVAL;
+	}
+
+	/* Check that 'max_rx_buf_length' is properly aligned (4). */
+	if (uf_info->max_rx_buf_length & (UCC_FAST_MRBLR_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: max_rx_buf_length not aligned\n",
+			__func__);
+		return -EINVAL;
+	}
+
+	/* Validate Virtual Fifo register values */
+	if (uf_info->urfs < UCC_FAST_URFS_MIN_VAL) {
+		printk(KERN_ERR "%s: urfs is too small\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->urfs & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: urfs is not aligned\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->urfet & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: urfet is not aligned.\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->urfset & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: urfset is not aligned\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->utfs & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: utfs is not aligned\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->utfet & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: utfet is not aligned\n", __func__);
+		return -EINVAL;
+	}
+
+	if (uf_info->utftt & (UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT - 1)) {
+		printk(KERN_ERR "%s: utftt is not aligned\n", __func__);
+		return -EINVAL;
+	}
+
+	uccf = kzalloc(sizeof(struct ucc_fast_private), GFP_KERNEL);
+	if (!uccf) {
+		printk(KERN_ERR "%s: Cannot allocate private data\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Fill fast UCC structure */
+	uccf->uf_info = uf_info;
+	/* Set the PHY base address */
+	uccf->uf_regs = ioremap(uf_info->regs, sizeof(struct ucc_fast));
+	if (uccf->uf_regs == NULL) {
+		printk(KERN_ERR "%s: Cannot map UCC registers\n", __func__);
+		kfree(uccf);
+		return -ENOMEM;
+	}
+
+	uccf->enabled_tx = 0;
+	uccf->enabled_rx = 0;
+	uccf->stopped_tx = 0;
+	uccf->stopped_rx = 0;
+	uf_regs = uccf->uf_regs;
+	uccf->p_ucce = &uf_regs->ucce;
+	uccf->p_uccm = &uf_regs->uccm;
+#ifdef CONFIG_UGETH_TX_ON_DEMAND
+	uccf->p_utodr = &uf_regs->utodr;
+#endif
+#ifdef STATISTICS
+	uccf->tx_frames = 0;
+	uccf->rx_frames = 0;
+	uccf->rx_discarded = 0;
+#endif				/* STATISTICS */
+
+	/* Set UCC to fast type */
+	ret = ucc_set_type(uf_info->ucc_num, UCC_SPEED_TYPE_FAST);
+	if (ret) {
+		printk(KERN_ERR "%s: cannot set UCC type\n", __func__);
+		ucc_fast_free(uccf);
+		return ret;
+	}
+
+	uccf->mrblr = uf_info->max_rx_buf_length;
+
+	/* Set GUMR */
+	/* For more details see the hardware spec. */
+	gumr = uf_info->ttx_trx;
+	if (uf_info->tci)
+		gumr |= UCC_FAST_GUMR_TCI;
+	if (uf_info->cdp)
+		gumr |= UCC_FAST_GUMR_CDP;
+	if (uf_info->ctsp)
+		gumr |= UCC_FAST_GUMR_CTSP;
+	if (uf_info->cds)
+		gumr |= UCC_FAST_GUMR_CDS;
+	if (uf_info->ctss)
+		gumr |= UCC_FAST_GUMR_CTSS;
+	if (uf_info->txsy)
+		gumr |= UCC_FAST_GUMR_TXSY;
+	if (uf_info->rsyn)
+		gumr |= UCC_FAST_GUMR_RSYN;
+	gumr |= uf_info->synl;
+	if (uf_info->rtsm)
+		gumr |= UCC_FAST_GUMR_RTSM;
+	gumr |= uf_info->renc;
+	if (uf_info->revd)
+		gumr |= UCC_FAST_GUMR_REVD;
+	gumr |= uf_info->tenc;
+	gumr |= uf_info->tcrc;
+	gumr |= uf_info->mode;
+	out_be32(&uf_regs->gumr, gumr);
+
+	/* Allocate memory for Tx Virtual Fifo */
+	uccf->ucc_fast_tx_virtual_fifo_base_offset =
+	    qe_muram_alloc(uf_info->utfs, UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT);
+	if (IS_ERR_VALUE(uccf->ucc_fast_tx_virtual_fifo_base_offset)) {
+		printk(KERN_ERR "%s: cannot allocate MURAM for TX FIFO\n",
+			__func__);
+		uccf->ucc_fast_tx_virtual_fifo_base_offset = 0;
+		ucc_fast_free(uccf);
+		return -ENOMEM;
+	}
+
+	/* Allocate memory for Rx Virtual Fifo */
+	uccf->ucc_fast_rx_virtual_fifo_base_offset =
+		qe_muram_alloc(uf_info->urfs +
+			   UCC_FAST_RECEIVE_VIRTUAL_FIFO_SIZE_FUDGE_FACTOR,
+			   UCC_FAST_VIRT_FIFO_REGS_ALIGNMENT);
+	if (IS_ERR_VALUE(uccf->ucc_fast_rx_virtual_fifo_base_offset)) {
+		printk(KERN_ERR "%s: cannot allocate MURAM for RX FIFO\n",
+			__func__);
+		uccf->ucc_fast_rx_virtual_fifo_base_offset = 0;
+		ucc_fast_free(uccf);
+		return -ENOMEM;
+	}
+
+	/* Set Virtual Fifo registers */
+	out_be16(&uf_regs->urfs, uf_info->urfs);
+	out_be16(&uf_regs->urfet, uf_info->urfet);
+	out_be16(&uf_regs->urfset, uf_info->urfset);
+	out_be16(&uf_regs->utfs, uf_info->utfs);
+	out_be16(&uf_regs->utfet, uf_info->utfet);
+	out_be16(&uf_regs->utftt, uf_info->utftt);
+	/* utfb, urfb are offsets from MURAM base */
+	out_be32(&uf_regs->utfb, uccf->ucc_fast_tx_virtual_fifo_base_offset);
+	out_be32(&uf_regs->urfb, uccf->ucc_fast_rx_virtual_fifo_base_offset);
+
+	/* Mux clocking */
+	/* Grant Support */
+	ucc_set_qe_mux_grant(uf_info->ucc_num, uf_info->grant_support);
+	/* Breakpoint Support */
+	ucc_set_qe_mux_bkpt(uf_info->ucc_num, uf_info->brkpt_support);
+	/* Set Tsa or NMSI mode. */
+	ucc_set_qe_mux_tsa(uf_info->ucc_num, uf_info->tsa);
+	/* If NMSI (not Tsa), set Tx and Rx clock. */
+	if (!uf_info->tsa) {
+		/* Rx clock routing */
+		if ((uf_info->rx_clock != QE_CLK_NONE) &&
+		    ucc_set_qe_mux_rxtx(uf_info->ucc_num, uf_info->rx_clock,
+					COMM_DIR_RX)) {
+			printk(KERN_ERR "%s: illegal value for RX clock\n",
+			       __func__);
+			ucc_fast_free(uccf);
+			return -EINVAL;
+		}
+		/* Tx clock routing */
+		if ((uf_info->tx_clock != QE_CLK_NONE) &&
+		    ucc_set_qe_mux_rxtx(uf_info->ucc_num, uf_info->tx_clock,
+					COMM_DIR_TX)) {
+			printk(KERN_ERR "%s: illegal value for TX clock\n",
+			       __func__);
+			ucc_fast_free(uccf);
+			return -EINVAL;
+		}
+	}
+
+	/* Set interrupt mask register at UCC level. */
+	out_be32(&uf_regs->uccm, uf_info->uccm_mask);
+
+	/* First, clear anything pending at UCC level,
+	 * otherwise, old garbage may come through
+	 * as soon as the dam is opened. */
+
+	/* Writing '1' clears */
+	out_be32(&uf_regs->ucce, 0xffffffff);
+
+	*uccf_ret = uccf;
+	return 0;
+}
+EXPORT_SYMBOL(ucc_fast_init);
+
+void ucc_fast_free(struct ucc_fast_private * uccf)
+{
+	if (!uccf)
+		return;
+
+	if (uccf->ucc_fast_tx_virtual_fifo_base_offset)
+		qe_muram_free(uccf->ucc_fast_tx_virtual_fifo_base_offset);
+
+	if (uccf->ucc_fast_rx_virtual_fifo_base_offset)
+		qe_muram_free(uccf->ucc_fast_rx_virtual_fifo_base_offset);
+
+	if (uccf->uf_regs)
+		iounmap(uccf->uf_regs);
+
+	kfree(uccf);
+}
+EXPORT_SYMBOL(ucc_fast_free);
diff --git a/drivers/soc/fsl/qe/ucc_slow.c b/drivers/soc/fsl/qe/ucc_slow.c
new file mode 100644
index 000000000000..9334bdbd9b30
--- /dev/null
+++ b/drivers/soc/fsl/qe/ucc_slow.c
@@ -0,0 +1,374 @@
+/*
+ * Copyright (C) 2006 Freescale Semiconductor, Inc. All rights reserved.
+ *
+ * Authors: 	Shlomi Gridish <gridish@freescale.com>
+ * 		Li Yang <leoli@freescale.com>
+ *
+ * Description:
+ * QE UCC Slow API Set - UCC Slow specific routines implementations.
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the 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/errno.h>
+#include <linux/slab.h>
+#include <linux/stddef.h>
+#include <linux/interrupt.h>
+#include <linux/err.h>
+#include <linux/export.h>
+
+#include <asm/io.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+
+#include <soc/fsl/qe/ucc.h>
+#include <soc/fsl/qe/ucc_slow.h>
+
+u32 ucc_slow_get_qe_cr_subblock(int uccs_num)
+{
+	switch (uccs_num) {
+	case 0: return QE_CR_SUBBLOCK_UCCSLOW1;
+	case 1: return QE_CR_SUBBLOCK_UCCSLOW2;
+	case 2: return QE_CR_SUBBLOCK_UCCSLOW3;
+	case 3: return QE_CR_SUBBLOCK_UCCSLOW4;
+	case 4: return QE_CR_SUBBLOCK_UCCSLOW5;
+	case 5: return QE_CR_SUBBLOCK_UCCSLOW6;
+	case 6: return QE_CR_SUBBLOCK_UCCSLOW7;
+	case 7: return QE_CR_SUBBLOCK_UCCSLOW8;
+	default: return QE_CR_SUBBLOCK_INVALID;
+	}
+}
+EXPORT_SYMBOL(ucc_slow_get_qe_cr_subblock);
+
+void ucc_slow_graceful_stop_tx(struct ucc_slow_private * uccs)
+{
+	struct ucc_slow_info *us_info = uccs->us_info;
+	u32 id;
+
+	id = ucc_slow_get_qe_cr_subblock(us_info->ucc_num);
+	qe_issue_cmd(QE_GRACEFUL_STOP_TX, id,
+			 QE_CR_PROTOCOL_UNSPECIFIED, 0);
+}
+EXPORT_SYMBOL(ucc_slow_graceful_stop_tx);
+
+void ucc_slow_stop_tx(struct ucc_slow_private * uccs)
+{
+	struct ucc_slow_info *us_info = uccs->us_info;
+	u32 id;
+
+	id = ucc_slow_get_qe_cr_subblock(us_info->ucc_num);
+	qe_issue_cmd(QE_STOP_TX, id, QE_CR_PROTOCOL_UNSPECIFIED, 0);
+}
+EXPORT_SYMBOL(ucc_slow_stop_tx);
+
+void ucc_slow_restart_tx(struct ucc_slow_private * uccs)
+{
+	struct ucc_slow_info *us_info = uccs->us_info;
+	u32 id;
+
+	id = ucc_slow_get_qe_cr_subblock(us_info->ucc_num);
+	qe_issue_cmd(QE_RESTART_TX, id, QE_CR_PROTOCOL_UNSPECIFIED, 0);
+}
+EXPORT_SYMBOL(ucc_slow_restart_tx);
+
+void ucc_slow_enable(struct ucc_slow_private * uccs, enum comm_dir mode)
+{
+	struct ucc_slow *us_regs;
+	u32 gumr_l;
+
+	us_regs = uccs->us_regs;
+
+	/* Enable reception and/or transmission on this UCC. */
+	gumr_l = in_be32(&us_regs->gumr_l);
+	if (mode & COMM_DIR_TX) {
+		gumr_l |= UCC_SLOW_GUMR_L_ENT;
+		uccs->enabled_tx = 1;
+	}
+	if (mode & COMM_DIR_RX) {
+		gumr_l |= UCC_SLOW_GUMR_L_ENR;
+		uccs->enabled_rx = 1;
+	}
+	out_be32(&us_regs->gumr_l, gumr_l);
+}
+EXPORT_SYMBOL(ucc_slow_enable);
+
+void ucc_slow_disable(struct ucc_slow_private * uccs, enum comm_dir mode)
+{
+	struct ucc_slow *us_regs;
+	u32 gumr_l;
+
+	us_regs = uccs->us_regs;
+
+	/* Disable reception and/or transmission on this UCC. */
+	gumr_l = in_be32(&us_regs->gumr_l);
+	if (mode & COMM_DIR_TX) {
+		gumr_l &= ~UCC_SLOW_GUMR_L_ENT;
+		uccs->enabled_tx = 0;
+	}
+	if (mode & COMM_DIR_RX) {
+		gumr_l &= ~UCC_SLOW_GUMR_L_ENR;
+		uccs->enabled_rx = 0;
+	}
+	out_be32(&us_regs->gumr_l, gumr_l);
+}
+EXPORT_SYMBOL(ucc_slow_disable);
+
+/* Initialize the UCC for Slow operations
+ *
+ * The caller should initialize the following us_info
+ */
+int ucc_slow_init(struct ucc_slow_info * us_info, struct ucc_slow_private ** uccs_ret)
+{
+	struct ucc_slow_private *uccs;
+	u32 i;
+	struct ucc_slow __iomem *us_regs;
+	u32 gumr;
+	struct qe_bd *bd;
+	u32 id;
+	u32 command;
+	int ret = 0;
+
+	if (!us_info)
+		return -EINVAL;
+
+	/* check if the UCC port number is in range. */
+	if ((us_info->ucc_num < 0) || (us_info->ucc_num > UCC_MAX_NUM - 1)) {
+		printk(KERN_ERR "%s: illegal UCC number\n", __func__);
+		return -EINVAL;
+	}
+
+	/*
+	 * Set mrblr
+	 * Check that 'max_rx_buf_length' is properly aligned (4), unless
+	 * rfw is 1, meaning that QE accepts one byte at a time, unlike normal
+	 * case when QE accepts 32 bits at a time.
+	 */
+	if ((!us_info->rfw) &&
+		(us_info->max_rx_buf_length & (UCC_SLOW_MRBLR_ALIGNMENT - 1))) {
+		printk(KERN_ERR "max_rx_buf_length not aligned.\n");
+		return -EINVAL;
+	}
+
+	uccs = kzalloc(sizeof(struct ucc_slow_private), GFP_KERNEL);
+	if (!uccs) {
+		printk(KERN_ERR "%s: Cannot allocate private data\n",
+			__func__);
+		return -ENOMEM;
+	}
+
+	/* Fill slow UCC structure */
+	uccs->us_info = us_info;
+	/* Set the PHY base address */
+	uccs->us_regs = ioremap(us_info->regs, sizeof(struct ucc_slow));
+	if (uccs->us_regs == NULL) {
+		printk(KERN_ERR "%s: Cannot map UCC registers\n", __func__);
+		kfree(uccs);
+		return -ENOMEM;
+	}
+
+	uccs->saved_uccm = 0;
+	uccs->p_rx_frame = 0;
+	us_regs = uccs->us_regs;
+	uccs->p_ucce = (u16 *) & (us_regs->ucce);
+	uccs->p_uccm = (u16 *) & (us_regs->uccm);
+#ifdef STATISTICS
+	uccs->rx_frames = 0;
+	uccs->tx_frames = 0;
+	uccs->rx_discarded = 0;
+#endif				/* STATISTICS */
+
+	/* Get PRAM base */
+	uccs->us_pram_offset =
+		qe_muram_alloc(UCC_SLOW_PRAM_SIZE, ALIGNMENT_OF_UCC_SLOW_PRAM);
+	if (IS_ERR_VALUE(uccs->us_pram_offset)) {
+		printk(KERN_ERR "%s: cannot allocate MURAM for PRAM", __func__);
+		ucc_slow_free(uccs);
+		return -ENOMEM;
+	}
+	id = ucc_slow_get_qe_cr_subblock(us_info->ucc_num);
+	qe_issue_cmd(QE_ASSIGN_PAGE_TO_DEVICE, id, us_info->protocol,
+		     uccs->us_pram_offset);
+
+	uccs->us_pram = qe_muram_addr(uccs->us_pram_offset);
+
+	/* Set UCC to slow type */
+	ret = ucc_set_type(us_info->ucc_num, UCC_SPEED_TYPE_SLOW);
+	if (ret) {
+		printk(KERN_ERR "%s: cannot set UCC type", __func__);
+		ucc_slow_free(uccs);
+		return ret;
+	}
+
+	out_be16(&uccs->us_pram->mrblr, us_info->max_rx_buf_length);
+
+	INIT_LIST_HEAD(&uccs->confQ);
+
+	/* Allocate BDs. */
+	uccs->rx_base_offset =
+		qe_muram_alloc(us_info->rx_bd_ring_len * sizeof(struct qe_bd),
+				QE_ALIGNMENT_OF_BD);
+	if (IS_ERR_VALUE(uccs->rx_base_offset)) {
+		printk(KERN_ERR "%s: cannot allocate %u RX BDs\n", __func__,
+			us_info->rx_bd_ring_len);
+		uccs->rx_base_offset = 0;
+		ucc_slow_free(uccs);
+		return -ENOMEM;
+	}
+
+	uccs->tx_base_offset =
+		qe_muram_alloc(us_info->tx_bd_ring_len * sizeof(struct qe_bd),
+			QE_ALIGNMENT_OF_BD);
+	if (IS_ERR_VALUE(uccs->tx_base_offset)) {
+		printk(KERN_ERR "%s: cannot allocate TX BDs", __func__);
+		uccs->tx_base_offset = 0;
+		ucc_slow_free(uccs);
+		return -ENOMEM;
+	}
+
+	/* Init Tx bds */
+	bd = uccs->confBd = uccs->tx_bd = qe_muram_addr(uccs->tx_base_offset);
+	for (i = 0; i < us_info->tx_bd_ring_len - 1; i++) {
+		/* clear bd buffer */
+		out_be32(&bd->buf, 0);
+		/* set bd status and length */
+		out_be32((u32 *) bd, 0);
+		bd++;
+	}
+	/* for last BD set Wrap bit */
+	out_be32(&bd->buf, 0);
+	out_be32((u32 *) bd, cpu_to_be32(T_W));
+
+	/* Init Rx bds */
+	bd = uccs->rx_bd = qe_muram_addr(uccs->rx_base_offset);
+	for (i = 0; i < us_info->rx_bd_ring_len - 1; i++) {
+		/* set bd status and length */
+		out_be32((u32*)bd, 0);
+		/* clear bd buffer */
+		out_be32(&bd->buf, 0);
+		bd++;
+	}
+	/* for last BD set Wrap bit */
+	out_be32((u32*)bd, cpu_to_be32(R_W));
+	out_be32(&bd->buf, 0);
+
+	/* Set GUMR (For more details see the hardware spec.). */
+	/* gumr_h */
+	gumr = us_info->tcrc;
+	if (us_info->cdp)
+		gumr |= UCC_SLOW_GUMR_H_CDP;
+	if (us_info->ctsp)
+		gumr |= UCC_SLOW_GUMR_H_CTSP;
+	if (us_info->cds)
+		gumr |= UCC_SLOW_GUMR_H_CDS;
+	if (us_info->ctss)
+		gumr |= UCC_SLOW_GUMR_H_CTSS;
+	if (us_info->tfl)
+		gumr |= UCC_SLOW_GUMR_H_TFL;
+	if (us_info->rfw)
+		gumr |= UCC_SLOW_GUMR_H_RFW;
+	if (us_info->txsy)
+		gumr |= UCC_SLOW_GUMR_H_TXSY;
+	if (us_info->rtsm)
+		gumr |= UCC_SLOW_GUMR_H_RTSM;
+	out_be32(&us_regs->gumr_h, gumr);
+
+	/* gumr_l */
+	gumr = us_info->tdcr | us_info->rdcr | us_info->tenc | us_info->renc |
+		us_info->diag | us_info->mode;
+	if (us_info->tci)
+		gumr |= UCC_SLOW_GUMR_L_TCI;
+	if (us_info->rinv)
+		gumr |= UCC_SLOW_GUMR_L_RINV;
+	if (us_info->tinv)
+		gumr |= UCC_SLOW_GUMR_L_TINV;
+	if (us_info->tend)
+		gumr |= UCC_SLOW_GUMR_L_TEND;
+	out_be32(&us_regs->gumr_l, gumr);
+
+	/* Function code registers */
+
+	/* if the data is in cachable memory, the 'global' */
+	/* in the function code should be set. */
+	uccs->us_pram->tbmr = UCC_BMR_BO_BE;
+	uccs->us_pram->rbmr = UCC_BMR_BO_BE;
+
+	/* rbase, tbase are offsets from MURAM base */
+	out_be16(&uccs->us_pram->rbase, uccs->rx_base_offset);
+	out_be16(&uccs->us_pram->tbase, uccs->tx_base_offset);
+
+	/* Mux clocking */
+	/* Grant Support */
+	ucc_set_qe_mux_grant(us_info->ucc_num, us_info->grant_support);
+	/* Breakpoint Support */
+	ucc_set_qe_mux_bkpt(us_info->ucc_num, us_info->brkpt_support);
+	/* Set Tsa or NMSI mode. */
+	ucc_set_qe_mux_tsa(us_info->ucc_num, us_info->tsa);
+	/* If NMSI (not Tsa), set Tx and Rx clock. */
+	if (!us_info->tsa) {
+		/* Rx clock routing */
+		if (ucc_set_qe_mux_rxtx(us_info->ucc_num, us_info->rx_clock,
+					COMM_DIR_RX)) {
+			printk(KERN_ERR "%s: illegal value for RX clock\n",
+			       __func__);
+			ucc_slow_free(uccs);
+			return -EINVAL;
+		}
+		/* Tx clock routing */
+		if (ucc_set_qe_mux_rxtx(us_info->ucc_num, us_info->tx_clock,
+					COMM_DIR_TX)) {
+			printk(KERN_ERR "%s: illegal value for TX clock\n",
+			       __func__);
+			ucc_slow_free(uccs);
+			return -EINVAL;
+		}
+	}
+
+	/* Set interrupt mask register at UCC level. */
+	out_be16(&us_regs->uccm, us_info->uccm_mask);
+
+	/* First, clear anything pending at UCC level,
+	 * otherwise, old garbage may come through
+	 * as soon as the dam is opened. */
+
+	/* Writing '1' clears */
+	out_be16(&us_regs->ucce, 0xffff);
+
+	/* Issue QE Init command */
+	if (us_info->init_tx && us_info->init_rx)
+		command = QE_INIT_TX_RX;
+	else if (us_info->init_tx)
+		command = QE_INIT_TX;
+	else
+		command = QE_INIT_RX;	/* We know at least one is TRUE */
+
+	qe_issue_cmd(command, id, us_info->protocol, 0);
+
+	*uccs_ret = uccs;
+	return 0;
+}
+EXPORT_SYMBOL(ucc_slow_init);
+
+void ucc_slow_free(struct ucc_slow_private * uccs)
+{
+	if (!uccs)
+		return;
+
+	if (uccs->rx_base_offset)
+		qe_muram_free(uccs->rx_base_offset);
+
+	if (uccs->tx_base_offset)
+		qe_muram_free(uccs->tx_base_offset);
+
+	if (uccs->us_pram)
+		qe_muram_free(uccs->us_pram_offset);
+
+	if (uccs->us_regs)
+		iounmap(uccs->us_regs);
+
+	kfree(uccs);
+}
+EXPORT_SYMBOL(ucc_slow_free);
+
diff --git a/drivers/soc/fsl/qe/usb.c b/drivers/soc/fsl/qe/usb.c
new file mode 100644
index 000000000000..111f7ab80f04
--- /dev/null
+++ b/drivers/soc/fsl/qe/usb.c
@@ -0,0 +1,56 @@
+/*
+ * QE USB routines
+ *
+ * Copyright 2006 Freescale Semiconductor, Inc.
+ *               Shlomi Gridish <gridish@freescale.com>
+ *               Jerry Huang <Chang-Ming.Huang@freescale.com>
+ * Copyright (c) MontaVista Software, Inc. 2008.
+ *               Anton Vorontsov <avorontsov@ru.mvista.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/export.h>
+#include <linux/io.h>
+#include <soc/fsl/qe/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+
+int qe_usb_clock_set(enum qe_clock clk, int rate)
+{
+	struct qe_mux __iomem *mux = &qe_immr->qmx;
+	unsigned long flags;
+	u32 val;
+
+	switch (clk) {
+	case QE_CLK3:  val = QE_CMXGCR_USBCS_CLK3;  break;
+	case QE_CLK5:  val = QE_CMXGCR_USBCS_CLK5;  break;
+	case QE_CLK7:  val = QE_CMXGCR_USBCS_CLK7;  break;
+	case QE_CLK9:  val = QE_CMXGCR_USBCS_CLK9;  break;
+	case QE_CLK13: val = QE_CMXGCR_USBCS_CLK13; break;
+	case QE_CLK17: val = QE_CMXGCR_USBCS_CLK17; break;
+	case QE_CLK19: val = QE_CMXGCR_USBCS_CLK19; break;
+	case QE_CLK21: val = QE_CMXGCR_USBCS_CLK21; break;
+	case QE_BRG9:  val = QE_CMXGCR_USBCS_BRG9;  break;
+	case QE_BRG10: val = QE_CMXGCR_USBCS_BRG10; break;
+	default:
+		pr_err("%s: requested unknown clock %d\n", __func__, clk);
+		return -EINVAL;
+	}
+
+	if (qe_clock_is_brg(clk))
+		qe_setbrg(clk, rate, 1);
+
+	spin_lock_irqsave(&cmxgcr_lock, flags);
+
+	clrsetbits_be32(&mux->cmxgcr, QE_CMXGCR_USBCS, val);
+
+	spin_unlock_irqrestore(&cmxgcr_lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(qe_usb_clock_set);
diff --git a/drivers/soc/mediatek/mtk-scpsys.c b/drivers/soc/mediatek/mtk-scpsys.c
index 4d4203c896c4..0221387e5e27 100644
--- a/drivers/soc/mediatek/mtk-scpsys.c
+++ b/drivers/soc/mediatek/mtk-scpsys.c
@@ -15,12 +15,13 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/mfd/syscon.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pm_domain.h>
 #include <linux/regmap.h>
 #include <linux/soc/mediatek/infracfg.h>
+#include <linux/regulator/consumer.h>
 #include <dt-bindings/power/mt8173-power.h>
 
 #define SPM_VDE_PWR_CON			0x0210
@@ -179,6 +180,7 @@ struct scp_domain {
 	u32 sram_pdn_ack_bits;
 	u32 bus_prot_mask;
 	bool active_wakeup;
+	struct regulator *supply;
 };
 
 struct scp {
@@ -221,6 +223,12 @@ static int scpsys_power_on(struct generic_pm_domain *genpd)
 	int ret;
 	int i;
 
+	if (scpd->supply) {
+		ret = regulator_enable(scpd->supply);
+		if (ret)
+			return ret;
+	}
+
 	for (i = 0; i < MAX_CLKS && scpd->clk[i]; i++) {
 		ret = clk_prepare_enable(scpd->clk[i]);
 		if (ret) {
@@ -299,6 +307,9 @@ err_pwr_ack:
 			clk_disable_unprepare(scpd->clk[i]);
 	}
 err_clk:
+	if (scpd->supply)
+		regulator_disable(scpd->supply);
+
 	dev_err(scp->dev, "Failed to power on domain %s\n", genpd->name);
 
 	return ret;
@@ -379,6 +390,9 @@ static int scpsys_power_off(struct generic_pm_domain *genpd)
 	for (i = 0; i < MAX_CLKS && scpd->clk[i]; i++)
 		clk_disable_unprepare(scpd->clk[i]);
 
+	if (scpd->supply)
+		regulator_disable(scpd->supply);
+
 	return 0;
 
 out:
@@ -448,6 +462,19 @@ static int __init scpsys_probe(struct platform_device *pdev)
 		return PTR_ERR(scp->infracfg);
 	}
 
+	for (i = 0; i < NUM_DOMAINS; i++) {
+		struct scp_domain *scpd = &scp->domains[i];
+		const struct scp_domain_data *data = &scp_domain_data[i];
+
+		scpd->supply = devm_regulator_get_optional(&pdev->dev, data->name);
+		if (IS_ERR(scpd->supply)) {
+			if (PTR_ERR(scpd->supply) == -ENODEV)
+				scpd->supply = NULL;
+			else
+				return PTR_ERR(scpd->supply);
+		}
+	}
+
 	pd_data->num_domains = NUM_DOMAINS;
 
 	for (i = 0; i < NUM_DOMAINS; i++) {
@@ -521,5 +548,4 @@ static struct platform_driver scpsys_drv = {
 		.of_match_table = of_match_ptr(of_scpsys_match_tbl),
 	},
 };
-
-module_platform_driver_probe(scpsys_drv, scpsys_probe);
+builtin_platform_driver_probe(scpsys_drv, scpsys_probe);
diff --git a/drivers/soc/qcom/Kconfig b/drivers/soc/qcom/Kconfig
index eec76141d9b9..461b387d03cc 100644
--- a/drivers/soc/qcom/Kconfig
+++ b/drivers/soc/qcom/Kconfig
@@ -13,6 +13,7 @@ config QCOM_GSBI
 config QCOM_PM
 	bool "Qualcomm Power Management"
 	depends on ARCH_QCOM && !ARM64
+	select ARM_CPU_SUSPEND
 	select QCOM_SCM
 	help
 	  QCOM Platform specific power driver to manage cores and L2 low power
@@ -49,3 +50,29 @@ config QCOM_SMD_RPM
 
 	  Say M here if you want to include support for the Qualcomm RPM as a
 	  module. This will build a module called "qcom-smd-rpm".
+
+config QCOM_SMEM_STATE
+	bool
+
+config QCOM_SMP2P
+	tristate "Qualcomm Shared Memory Point to Point support"
+	depends on QCOM_SMEM
+	select QCOM_SMEM_STATE
+	help
+	  Say yes here to support the Qualcomm Shared Memory Point to Point
+	  protocol.
+
+config QCOM_SMSM
+	tristate "Qualcomm Shared Memory State Machine"
+	depends on QCOM_SMEM
+	select QCOM_SMEM_STATE
+	help
+	  Say yes here to support the Qualcomm Shared Memory State Machine.
+	  The state machine is represented by bits in shared memory.
+
+config QCOM_WCNSS_CTRL
+	tristate "Qualcomm WCNSS control driver"
+	depends on QCOM_SMD
+	help
+	  Client driver for the WCNSS_CTRL SMD channel, used to download nv
+	  firmware to a newly booted WCNSS chip.
diff --git a/drivers/soc/qcom/Makefile b/drivers/soc/qcom/Makefile
index 10a93d168e0e..fdd664edf0bd 100644
--- a/drivers/soc/qcom/Makefile
+++ b/drivers/soc/qcom/Makefile
@@ -3,3 +3,7 @@ obj-$(CONFIG_QCOM_PM)	+=	spm.o
 obj-$(CONFIG_QCOM_SMD) +=	smd.o
 obj-$(CONFIG_QCOM_SMD_RPM)	+= smd-rpm.o
 obj-$(CONFIG_QCOM_SMEM) +=	smem.o
+obj-$(CONFIG_QCOM_SMEM_STATE) += smem_state.o
+obj-$(CONFIG_QCOM_SMP2P)	+= smp2p.o
+obj-$(CONFIG_QCOM_SMSM)	+= smsm.o
+obj-$(CONFIG_QCOM_WCNSS_CTRL) += wcnss_ctrl.o
diff --git a/drivers/soc/qcom/smd-rpm.c b/drivers/soc/qcom/smd-rpm.c
index 2969321e1b09..731fa066f712 100644
--- a/drivers/soc/qcom/smd-rpm.c
+++ b/drivers/soc/qcom/smd-rpm.c
@@ -219,6 +219,8 @@ static void qcom_smd_rpm_remove(struct qcom_smd_device *sdev)
 }
 
 static const struct of_device_id qcom_smd_rpm_of_match[] = {
+	{ .compatible = "qcom,rpm-apq8084" },
+	{ .compatible = "qcom,rpm-msm8916" },
 	{ .compatible = "qcom,rpm-msm8974" },
 	{}
 };
diff --git a/drivers/soc/qcom/smd.c b/drivers/soc/qcom/smd.c
index 86b598cff91a..498fd0581a45 100644
--- a/drivers/soc/qcom/smd.c
+++ b/drivers/soc/qcom/smd.c
@@ -434,20 +434,15 @@ static void smd_copy_to_fifo(void __iomem *dst,
 /*
  * Copy count bytes of data using 32bit accesses, if that is required.
  */
-static void smd_copy_from_fifo(void *_dst,
-			       const void __iomem *_src,
+static void smd_copy_from_fifo(void *dst,
+			       const void __iomem *src,
 			       size_t count,
 			       bool word_aligned)
 {
-	u32 *dst = (u32 *)_dst;
-	u32 *src = (u32 *)_src;
-
 	if (word_aligned) {
-		count /= sizeof(u32);
-		while (count--)
-			*dst++ = __raw_readl(src++);
+		__ioread32_copy(dst, src, count / sizeof(u32));
 	} else {
-		memcpy_fromio(_dst, _src, count);
+		memcpy_fromio(dst, src, count);
 	}
 }
 
diff --git a/drivers/soc/qcom/smem_state.c b/drivers/soc/qcom/smem_state.c
new file mode 100644
index 000000000000..54261decb369
--- /dev/null
+++ b/drivers/soc/qcom/smem_state.c
@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2015, Sony Mobile Communications Inc.
+ * Copyright (c) 2012-2013, 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/device.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/soc/qcom/smem_state.h>
+
+static LIST_HEAD(smem_states);
+static DEFINE_MUTEX(list_lock);
+
+/**
+ * struct qcom_smem_state - state context
+ * @refcount:	refcount for the state
+ * @orphan:	boolean indicator that this state has been unregistered
+ * @list:	entry in smem_states list
+ * @of_node:	of_node to use for matching the state in DT
+ * @priv:	implementation private data
+ * @ops:	ops for the state
+ */
+struct qcom_smem_state {
+	struct kref refcount;
+	bool orphan;
+
+	struct list_head list;
+	struct device_node *of_node;
+
+	void *priv;
+
+	struct qcom_smem_state_ops ops;
+};
+
+/**
+ * qcom_smem_state_update_bits() - update the masked bits in state with value
+ * @state:	state handle acquired by calling qcom_smem_state_get()
+ * @mask:	bit mask for the change
+ * @value:	new value for the masked bits
+ *
+ * Returns 0 on success, otherwise negative errno.
+ */
+int qcom_smem_state_update_bits(struct qcom_smem_state *state,
+				u32 mask,
+				u32 value)
+{
+	if (state->orphan)
+		return -ENXIO;
+
+	if (!state->ops.update_bits)
+		return -ENOTSUPP;
+
+	return state->ops.update_bits(state->priv, mask, value);
+}
+EXPORT_SYMBOL_GPL(qcom_smem_state_update_bits);
+
+static struct qcom_smem_state *of_node_to_state(struct device_node *np)
+{
+	struct qcom_smem_state *state;
+
+	mutex_lock(&list_lock);
+
+	list_for_each_entry(state, &smem_states, list) {
+		if (state->of_node == np) {
+			kref_get(&state->refcount);
+			goto unlock;
+		}
+	}
+	state = ERR_PTR(-EPROBE_DEFER);
+
+unlock:
+	mutex_unlock(&list_lock);
+
+	return state;
+}
+
+/**
+ * qcom_smem_state_get() - acquire handle to a state
+ * @dev:	client device pointer
+ * @con_id:	name of the state to lookup
+ * @bit:	flags from the state reference, indicating which bit's affected
+ *
+ * Returns handle to the state, or ERR_PTR(). qcom_smem_state_put() must be
+ * called to release the returned state handle.
+ */
+struct qcom_smem_state *qcom_smem_state_get(struct device *dev,
+					    const char *con_id,
+					    unsigned *bit)
+{
+	struct qcom_smem_state *state;
+	struct of_phandle_args args;
+	int index = 0;
+	int ret;
+
+	if (con_id) {
+		index = of_property_match_string(dev->of_node,
+						 "qcom,state-names",
+						 con_id);
+		if (index < 0) {
+			dev_err(dev, "missing qcom,state-names\n");
+			return ERR_PTR(index);
+		}
+	}
+
+	ret = of_parse_phandle_with_args(dev->of_node,
+					 "qcom,state",
+					 "#qcom,state-cells",
+					 index,
+					 &args);
+	if (ret) {
+		dev_err(dev, "failed to parse qcom,state property\n");
+		return ERR_PTR(ret);
+	}
+
+	if (args.args_count != 1) {
+		dev_err(dev, "invalid #qcom,state-cells\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	state = of_node_to_state(args.np);
+	if (IS_ERR(state))
+		goto put;
+
+	*bit = args.args[0];
+
+put:
+	of_node_put(args.np);
+	return state;
+}
+EXPORT_SYMBOL_GPL(qcom_smem_state_get);
+
+static void qcom_smem_state_release(struct kref *ref)
+{
+	struct qcom_smem_state *state = container_of(ref, struct qcom_smem_state, refcount);
+
+	list_del(&state->list);
+	kfree(state);
+}
+
+/**
+ * qcom_smem_state_put() - release state handle
+ * @state:	state handle to be released
+ */
+void qcom_smem_state_put(struct qcom_smem_state *state)
+{
+	mutex_lock(&list_lock);
+	kref_put(&state->refcount, qcom_smem_state_release);
+	mutex_unlock(&list_lock);
+}
+EXPORT_SYMBOL_GPL(qcom_smem_state_put);
+
+/**
+ * qcom_smem_state_register() - register a new state
+ * @of_node:	of_node used for matching client lookups
+ * @ops:	implementation ops
+ * @priv:	implementation specific private data
+ */
+struct qcom_smem_state *qcom_smem_state_register(struct device_node *of_node,
+						 const struct qcom_smem_state_ops *ops,
+						 void *priv)
+{
+	struct qcom_smem_state *state;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return ERR_PTR(-ENOMEM);
+
+	kref_init(&state->refcount);
+
+	state->of_node = of_node;
+	state->ops = *ops;
+	state->priv = priv;
+
+	mutex_lock(&list_lock);
+	list_add(&state->list, &smem_states);
+	mutex_unlock(&list_lock);
+
+	return state;
+}
+EXPORT_SYMBOL_GPL(qcom_smem_state_register);
+
+/**
+ * qcom_smem_state_unregister() - unregister a registered state
+ * @state:	state handle to be unregistered
+ */
+void qcom_smem_state_unregister(struct qcom_smem_state *state)
+{
+	state->orphan = true;
+	qcom_smem_state_put(state);
+}
+EXPORT_SYMBOL_GPL(qcom_smem_state_unregister);
diff --git a/drivers/soc/qcom/smp2p.c b/drivers/soc/qcom/smp2p.c
new file mode 100644
index 000000000000..f1eed7f9dd67
--- /dev/null
+++ b/drivers/soc/qcom/smp2p.c
@@ -0,0 +1,578 @@
+/*
+ * Copyright (c) 2015, Sony Mobile Communications AB.
+ * Copyright (c) 2012-2013, 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/interrupt.h>
+#include <linux/list.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/soc/qcom/smem.h>
+#include <linux/soc/qcom/smem_state.h>
+#include <linux/spinlock.h>
+
+/*
+ * The Shared Memory Point to Point (SMP2P) protocol facilitates communication
+ * of a single 32-bit value between two processors.  Each value has a single
+ * writer (the local side) and a single reader (the remote side). Values are
+ * uniquely identified in the system by the directed edge (local processor ID
+ * to remote processor ID) and a string identifier.
+ *
+ * Each processor is responsible for creating the outgoing SMEM items and each
+ * item is writable by the local processor and readable by the remote
+ * processor.  By using two separate SMEM items that are single-reader and
+ * single-writer, SMP2P does not require any remote locking mechanisms.
+ *
+ * The driver uses the Linux GPIO and interrupt framework to expose a virtual
+ * GPIO for each outbound entry and a virtual interrupt controller for each
+ * inbound entry.
+ */
+
+#define SMP2P_MAX_ENTRY 16
+#define SMP2P_MAX_ENTRY_NAME 16
+
+#define SMP2P_FEATURE_SSR_ACK 0x1
+
+#define SMP2P_MAGIC 0x504d5324
+
+/**
+ * struct smp2p_smem_item - in memory communication structure
+ * @magic:		magic number
+ * @version:		version - must be 1
+ * @features:		features flag - currently unused
+ * @local_pid:		processor id of sending end
+ * @remote_pid:		processor id of receiving end
+ * @total_entries:	number of entries - always SMP2P_MAX_ENTRY
+ * @valid_entries:	number of allocated entries
+ * @flags:
+ * @entries:		individual communication entries
+ *     @name:		name of the entry
+ *     @value:		content of the entry
+ */
+struct smp2p_smem_item {
+	u32 magic;
+	u8 version;
+	unsigned features:24;
+	u16 local_pid;
+	u16 remote_pid;
+	u16 total_entries;
+	u16 valid_entries;
+	u32 flags;
+
+	struct {
+		u8 name[SMP2P_MAX_ENTRY_NAME];
+		u32 value;
+	} entries[SMP2P_MAX_ENTRY];
+} __packed;
+
+/**
+ * struct smp2p_entry - driver context matching one entry
+ * @node:	list entry to keep track of allocated entries
+ * @smp2p:	reference to the device driver context
+ * @name:	name of the entry, to match against smp2p_smem_item
+ * @value:	pointer to smp2p_smem_item entry value
+ * @last_value:	last handled value
+ * @domain:	irq_domain for inbound entries
+ * @irq_enabled:bitmap to track enabled irq bits
+ * @irq_rising:	bitmap to mark irq bits for rising detection
+ * @irq_falling:bitmap to mark irq bits for falling detection
+ * @state:	smem state handle
+ * @lock:	spinlock to protect read-modify-write of the value
+ */
+struct smp2p_entry {
+	struct list_head node;
+	struct qcom_smp2p *smp2p;
+
+	const char *name;
+	u32 *value;
+	u32 last_value;
+
+	struct irq_domain *domain;
+	DECLARE_BITMAP(irq_enabled, 32);
+	DECLARE_BITMAP(irq_rising, 32);
+	DECLARE_BITMAP(irq_falling, 32);
+
+	struct qcom_smem_state *state;
+
+	spinlock_t lock;
+};
+
+#define SMP2P_INBOUND	0
+#define SMP2P_OUTBOUND	1
+
+/**
+ * struct qcom_smp2p - device driver context
+ * @dev:	device driver handle
+ * @in:		pointer to the inbound smem item
+ * @smem_items:	ids of the two smem items
+ * @valid_entries: already scanned inbound entries
+ * @local_pid:	processor id of the inbound edge
+ * @remote_pid:	processor id of the outbound edge
+ * @ipc_regmap:	regmap for the outbound ipc
+ * @ipc_offset:	offset within the regmap
+ * @ipc_bit:	bit in regmap@offset to kick to signal remote processor
+ * @inbound:	list of inbound entries
+ * @outbound:	list of outbound entries
+ */
+struct qcom_smp2p {
+	struct device *dev;
+
+	struct smp2p_smem_item *in;
+	struct smp2p_smem_item *out;
+
+	unsigned smem_items[SMP2P_OUTBOUND + 1];
+
+	unsigned valid_entries;
+
+	unsigned local_pid;
+	unsigned remote_pid;
+
+	struct regmap *ipc_regmap;
+	int ipc_offset;
+	int ipc_bit;
+
+	struct list_head inbound;
+	struct list_head outbound;
+};
+
+static void qcom_smp2p_kick(struct qcom_smp2p *smp2p)
+{
+	/* Make sure any updated data is written before the kick */
+	wmb();
+	regmap_write(smp2p->ipc_regmap, smp2p->ipc_offset, BIT(smp2p->ipc_bit));
+}
+
+/**
+ * qcom_smp2p_intr() - interrupt handler for incoming notifications
+ * @irq:	unused
+ * @data:	smp2p driver context
+ *
+ * Handle notifications from the remote side to handle newly allocated entries
+ * or any changes to the state bits of existing entries.
+ */
+static irqreturn_t qcom_smp2p_intr(int irq, void *data)
+{
+	struct smp2p_smem_item *in;
+	struct smp2p_entry *entry;
+	struct qcom_smp2p *smp2p = data;
+	unsigned smem_id = smp2p->smem_items[SMP2P_INBOUND];
+	unsigned pid = smp2p->remote_pid;
+	size_t size;
+	int irq_pin;
+	u32 status;
+	char buf[SMP2P_MAX_ENTRY_NAME];
+	u32 val;
+	int i;
+
+	in = smp2p->in;
+
+	/* Acquire smem item, if not already found */
+	if (!in) {
+		in = qcom_smem_get(pid, smem_id, &size);
+		if (IS_ERR(in)) {
+			dev_err(smp2p->dev,
+				"Unable to acquire remote smp2p item\n");
+			return IRQ_HANDLED;
+		}
+
+		smp2p->in = in;
+	}
+
+	/* Match newly created entries */
+	for (i = smp2p->valid_entries; i < in->valid_entries; i++) {
+		list_for_each_entry(entry, &smp2p->inbound, node) {
+			memcpy_fromio(buf, in->entries[i].name, sizeof(buf));
+			if (!strcmp(buf, entry->name)) {
+				entry->value = &in->entries[i].value;
+				break;
+			}
+		}
+	}
+	smp2p->valid_entries = i;
+
+	/* Fire interrupts based on any value changes */
+	list_for_each_entry(entry, &smp2p->inbound, node) {
+		/* Ignore entries not yet allocated by the remote side */
+		if (!entry->value)
+			continue;
+
+		val = readl(entry->value);
+
+		status = val ^ entry->last_value;
+		entry->last_value = val;
+
+		/* No changes of this entry? */
+		if (!status)
+			continue;
+
+		for_each_set_bit(i, entry->irq_enabled, 32) {
+			if (!(status & BIT(i)))
+				continue;
+
+			if ((val & BIT(i) && test_bit(i, entry->irq_rising)) ||
+			    (!(val & BIT(i)) && test_bit(i, entry->irq_falling))) {
+				irq_pin = irq_find_mapping(entry->domain, i);
+				handle_nested_irq(irq_pin);
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void smp2p_mask_irq(struct irq_data *irqd)
+{
+	struct smp2p_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+
+	clear_bit(irq, entry->irq_enabled);
+}
+
+static void smp2p_unmask_irq(struct irq_data *irqd)
+{
+	struct smp2p_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+
+	set_bit(irq, entry->irq_enabled);
+}
+
+static int smp2p_set_irq_type(struct irq_data *irqd, unsigned int type)
+{
+	struct smp2p_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+
+	if (!(type & IRQ_TYPE_EDGE_BOTH))
+		return -EINVAL;
+
+	if (type & IRQ_TYPE_EDGE_RISING)
+		set_bit(irq, entry->irq_rising);
+	else
+		clear_bit(irq, entry->irq_rising);
+
+	if (type & IRQ_TYPE_EDGE_FALLING)
+		set_bit(irq, entry->irq_falling);
+	else
+		clear_bit(irq, entry->irq_falling);
+
+	return 0;
+}
+
+static struct irq_chip smp2p_irq_chip = {
+	.name           = "smp2p",
+	.irq_mask       = smp2p_mask_irq,
+	.irq_unmask     = smp2p_unmask_irq,
+	.irq_set_type	= smp2p_set_irq_type,
+};
+
+static int smp2p_irq_map(struct irq_domain *d,
+			 unsigned int irq,
+			 irq_hw_number_t hw)
+{
+	struct smp2p_entry *entry = d->host_data;
+
+	irq_set_chip_and_handler(irq, &smp2p_irq_chip, handle_level_irq);
+	irq_set_chip_data(irq, entry);
+	irq_set_nested_thread(irq, 1);
+	irq_set_noprobe(irq);
+
+	return 0;
+}
+
+static const struct irq_domain_ops smp2p_irq_ops = {
+	.map = smp2p_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
+static int qcom_smp2p_inbound_entry(struct qcom_smp2p *smp2p,
+				    struct smp2p_entry *entry,
+				    struct device_node *node)
+{
+	entry->domain = irq_domain_add_linear(node, 32, &smp2p_irq_ops, entry);
+	if (!entry->domain) {
+		dev_err(smp2p->dev, "failed to add irq_domain\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int smp2p_update_bits(void *data, u32 mask, u32 value)
+{
+	struct smp2p_entry *entry = data;
+	u32 orig;
+	u32 val;
+
+	spin_lock(&entry->lock);
+	val = orig = readl(entry->value);
+	val &= ~mask;
+	val |= value;
+	writel(val, entry->value);
+	spin_unlock(&entry->lock);
+
+	if (val != orig)
+		qcom_smp2p_kick(entry->smp2p);
+
+	return 0;
+}
+
+static const struct qcom_smem_state_ops smp2p_state_ops = {
+	.update_bits = smp2p_update_bits,
+};
+
+static int qcom_smp2p_outbound_entry(struct qcom_smp2p *smp2p,
+				     struct smp2p_entry *entry,
+				     struct device_node *node)
+{
+	struct smp2p_smem_item *out = smp2p->out;
+	char buf[SMP2P_MAX_ENTRY_NAME] = {};
+
+	/* Allocate an entry from the smem item */
+	strlcpy(buf, entry->name, SMP2P_MAX_ENTRY_NAME);
+	memcpy_toio(out->entries[out->valid_entries].name, buf, SMP2P_MAX_ENTRY_NAME);
+	out->valid_entries++;
+
+	/* Make the logical entry reference the physical value */
+	entry->value = &out->entries[out->valid_entries].value;
+
+	entry->state = qcom_smem_state_register(node, &smp2p_state_ops, entry);
+	if (IS_ERR(entry->state)) {
+		dev_err(smp2p->dev, "failed to register qcom_smem_state\n");
+		return PTR_ERR(entry->state);
+	}
+
+	return 0;
+}
+
+static int qcom_smp2p_alloc_outbound_item(struct qcom_smp2p *smp2p)
+{
+	struct smp2p_smem_item *out;
+	unsigned smem_id = smp2p->smem_items[SMP2P_OUTBOUND];
+	unsigned pid = smp2p->remote_pid;
+	int ret;
+
+	ret = qcom_smem_alloc(pid, smem_id, sizeof(*out));
+	if (ret < 0 && ret != -EEXIST) {
+		if (ret != -EPROBE_DEFER)
+			dev_err(smp2p->dev,
+				"unable to allocate local smp2p item\n");
+		return ret;
+	}
+
+	out = qcom_smem_get(pid, smem_id, NULL);
+	if (IS_ERR(out)) {
+		dev_err(smp2p->dev, "Unable to acquire local smp2p item\n");
+		return PTR_ERR(out);
+	}
+
+	memset(out, 0, sizeof(*out));
+	out->magic = SMP2P_MAGIC;
+	out->local_pid = smp2p->local_pid;
+	out->remote_pid = smp2p->remote_pid;
+	out->total_entries = SMP2P_MAX_ENTRY;
+	out->valid_entries = 0;
+
+	/*
+	 * Make sure the rest of the header is written before we validate the
+	 * item by writing a valid version number.
+	 */
+	wmb();
+	out->version = 1;
+
+	qcom_smp2p_kick(smp2p);
+
+	smp2p->out = out;
+
+	return 0;
+}
+
+static int smp2p_parse_ipc(struct qcom_smp2p *smp2p)
+{
+	struct device_node *syscon;
+	struct device *dev = smp2p->dev;
+	const char *key;
+	int ret;
+
+	syscon = of_parse_phandle(dev->of_node, "qcom,ipc", 0);
+	if (!syscon) {
+		dev_err(dev, "no qcom,ipc node\n");
+		return -ENODEV;
+	}
+
+	smp2p->ipc_regmap = syscon_node_to_regmap(syscon);
+	if (IS_ERR(smp2p->ipc_regmap))
+		return PTR_ERR(smp2p->ipc_regmap);
+
+	key = "qcom,ipc";
+	ret = of_property_read_u32_index(dev->of_node, key, 1, &smp2p->ipc_offset);
+	if (ret < 0) {
+		dev_err(dev, "no offset in %s\n", key);
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_index(dev->of_node, key, 2, &smp2p->ipc_bit);
+	if (ret < 0) {
+		dev_err(dev, "no bit in %s\n", key);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int qcom_smp2p_probe(struct platform_device *pdev)
+{
+	struct smp2p_entry *entry;
+	struct device_node *node;
+	struct qcom_smp2p *smp2p;
+	const char *key;
+	int irq;
+	int ret;
+
+	smp2p = devm_kzalloc(&pdev->dev, sizeof(*smp2p), GFP_KERNEL);
+	if (!smp2p)
+		return -ENOMEM;
+
+	smp2p->dev = &pdev->dev;
+	INIT_LIST_HEAD(&smp2p->inbound);
+	INIT_LIST_HEAD(&smp2p->outbound);
+
+	platform_set_drvdata(pdev, smp2p);
+
+	ret = smp2p_parse_ipc(smp2p);
+	if (ret)
+		return ret;
+
+	key = "qcom,smem";
+	ret = of_property_read_u32_array(pdev->dev.of_node, key,
+					 smp2p->smem_items, 2);
+	if (ret)
+		return ret;
+
+	key = "qcom,local-pid";
+	ret = of_property_read_u32(pdev->dev.of_node, key, &smp2p->local_pid);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to read %s\n", key);
+		return -EINVAL;
+	}
+
+	key = "qcom,remote-pid";
+	ret = of_property_read_u32(pdev->dev.of_node, key, &smp2p->remote_pid);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to read %s\n", key);
+		return -EINVAL;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "unable to acquire smp2p interrupt\n");
+		return irq;
+	}
+
+	ret = qcom_smp2p_alloc_outbound_item(smp2p);
+	if (ret < 0)
+		return ret;
+
+	for_each_available_child_of_node(pdev->dev.of_node, node) {
+		entry = devm_kzalloc(&pdev->dev, sizeof(*entry), GFP_KERNEL);
+		if (!entry) {
+			ret = -ENOMEM;
+			goto unwind_interfaces;
+		}
+
+		entry->smp2p = smp2p;
+		spin_lock_init(&entry->lock);
+
+		ret = of_property_read_string(node, "qcom,entry-name", &entry->name);
+		if (ret < 0)
+			goto unwind_interfaces;
+
+		if (of_property_read_bool(node, "interrupt-controller")) {
+			ret = qcom_smp2p_inbound_entry(smp2p, entry, node);
+			if (ret < 0)
+				goto unwind_interfaces;
+
+			list_add(&entry->node, &smp2p->inbound);
+		} else  {
+			ret = qcom_smp2p_outbound_entry(smp2p, entry, node);
+			if (ret < 0)
+				goto unwind_interfaces;
+
+			list_add(&entry->node, &smp2p->outbound);
+		}
+	}
+
+	/* Kick the outgoing edge after allocating entries */
+	qcom_smp2p_kick(smp2p);
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq,
+					NULL, qcom_smp2p_intr,
+					IRQF_ONESHOT,
+					"smp2p", (void *)smp2p);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request interrupt\n");
+		goto unwind_interfaces;
+	}
+
+
+	return 0;
+
+unwind_interfaces:
+	list_for_each_entry(entry, &smp2p->inbound, node)
+		irq_domain_remove(entry->domain);
+
+	list_for_each_entry(entry, &smp2p->outbound, node)
+		qcom_smem_state_unregister(entry->state);
+
+	smp2p->out->valid_entries = 0;
+
+	return ret;
+}
+
+static int qcom_smp2p_remove(struct platform_device *pdev)
+{
+	struct qcom_smp2p *smp2p = platform_get_drvdata(pdev);
+	struct smp2p_entry *entry;
+
+	list_for_each_entry(entry, &smp2p->inbound, node)
+		irq_domain_remove(entry->domain);
+
+	list_for_each_entry(entry, &smp2p->outbound, node)
+		qcom_smem_state_unregister(entry->state);
+
+	smp2p->out->valid_entries = 0;
+
+	return 0;
+}
+
+static const struct of_device_id qcom_smp2p_of_match[] = {
+	{ .compatible = "qcom,smp2p" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, qcom_smp2p_of_match);
+
+static struct platform_driver qcom_smp2p_driver = {
+	.probe = qcom_smp2p_probe,
+	.remove = qcom_smp2p_remove,
+	.driver  = {
+		.name  = "qcom_smp2p",
+		.of_match_table = qcom_smp2p_of_match,
+	},
+};
+module_platform_driver(qcom_smp2p_driver);
+
+MODULE_DESCRIPTION("Qualcomm Shared Memory Point to Point driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/soc/qcom/smsm.c b/drivers/soc/qcom/smsm.c
new file mode 100644
index 000000000000..6b777af1bc19
--- /dev/null
+++ b/drivers/soc/qcom/smsm.c
@@ -0,0 +1,625 @@
+/*
+ * Copyright (c) 2015, Sony Mobile Communications Inc.
+ * Copyright (c) 2012-2013, 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/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+#include <linux/regmap.h>
+#include <linux/soc/qcom/smem.h>
+#include <linux/soc/qcom/smem_state.h>
+
+/*
+ * This driver implements the Qualcomm Shared Memory State Machine, a mechanism
+ * for communicating single bit state information to remote processors.
+ *
+ * The implementation is based on two sections of shared memory; the first
+ * holding the state bits and the second holding a matrix of subscription bits.
+ *
+ * The state bits are structured in entries of 32 bits, each belonging to one
+ * system in the SoC. The entry belonging to the local system is considered
+ * read-write, while the rest should be considered read-only.
+ *
+ * The subscription matrix consists of N bitmaps per entry, denoting interest
+ * in updates of the entry for each of the N hosts. Upon updating a state bit
+ * each host's subscription bitmap should be queried and the remote system
+ * should be interrupted if they request so.
+ *
+ * The subscription matrix is laid out in entry-major order:
+ * entry0: [host0 ... hostN]
+ *	.
+ *	.
+ * entryM: [host0 ... hostN]
+ *
+ * A third, optional, shared memory region might contain information regarding
+ * the number of entries in the state bitmap as well as number of columns in
+ * the subscription matrix.
+ */
+
+/*
+ * Shared memory identifiers, used to acquire handles to respective memory
+ * region.
+ */
+#define SMEM_SMSM_SHARED_STATE		85
+#define SMEM_SMSM_CPU_INTR_MASK		333
+#define SMEM_SMSM_SIZE_INFO		419
+
+/*
+ * Default sizes, in case SMEM_SMSM_SIZE_INFO is not found.
+ */
+#define SMSM_DEFAULT_NUM_ENTRIES	8
+#define SMSM_DEFAULT_NUM_HOSTS		3
+
+struct smsm_entry;
+struct smsm_host;
+
+/**
+ * struct qcom_smsm - smsm driver context
+ * @dev:	smsm device pointer
+ * @local_host:	column in the subscription matrix representing this system
+ * @num_hosts:	number of columns in the subscription matrix
+ * @num_entries: number of entries in the state map and rows in the subscription
+ *		matrix
+ * @local_state: pointer to the local processor's state bits
+ * @subscription: pointer to local processor's row in subscription matrix
+ * @state:	smem state handle
+ * @lock:	spinlock for read-modify-write of the outgoing state
+ * @entries:	context for each of the entries
+ * @hosts:	context for each of the hosts
+ */
+struct qcom_smsm {
+	struct device *dev;
+
+	u32 local_host;
+
+	u32 num_hosts;
+	u32 num_entries;
+
+	u32 *local_state;
+	u32 *subscription;
+	struct qcom_smem_state *state;
+
+	spinlock_t lock;
+
+	struct smsm_entry *entries;
+	struct smsm_host *hosts;
+};
+
+/**
+ * struct smsm_entry - per remote processor entry context
+ * @smsm:	back-reference to driver context
+ * @domain:	IRQ domain for this entry, if representing a remote system
+ * @irq_enabled: bitmap of which state bits IRQs are enabled
+ * @irq_rising:	bitmap tracking if rising bits should be propagated
+ * @irq_falling: bitmap tracking if falling bits should be propagated
+ * @last_value:	snapshot of state bits last time the interrupts where propagated
+ * @remote_state: pointer to this entry's state bits
+ * @subscription: pointer to a row in the subscription matrix representing this
+ *		entry
+ */
+struct smsm_entry {
+	struct qcom_smsm *smsm;
+
+	struct irq_domain *domain;
+	DECLARE_BITMAP(irq_enabled, 32);
+	DECLARE_BITMAP(irq_rising, 32);
+	DECLARE_BITMAP(irq_falling, 32);
+	u32 last_value;
+
+	u32 *remote_state;
+	u32 *subscription;
+};
+
+/**
+ * struct smsm_host - representation of a remote host
+ * @ipc_regmap:	regmap for outgoing interrupt
+ * @ipc_offset:	offset in @ipc_regmap for outgoing interrupt
+ * @ipc_bit:	bit in @ipc_regmap + @ipc_offset for outgoing interrupt
+ */
+struct smsm_host {
+	struct regmap *ipc_regmap;
+	int ipc_offset;
+	int ipc_bit;
+};
+
+/**
+ * smsm_update_bits() - change bit in outgoing entry and inform subscribers
+ * @data:	smsm context pointer
+ * @offset:	bit in the entry
+ * @value:	new value
+ *
+ * Used to set and clear the bits in the outgoing/local entry and inform
+ * subscribers about the change.
+ */
+static int smsm_update_bits(void *data, u32 mask, u32 value)
+{
+	struct qcom_smsm *smsm = data;
+	struct smsm_host *hostp;
+	unsigned long flags;
+	u32 changes;
+	u32 host;
+	u32 orig;
+	u32 val;
+
+	spin_lock_irqsave(&smsm->lock, flags);
+
+	/* Update the entry */
+	val = orig = readl(smsm->local_state);
+	val &= ~mask;
+	val |= value;
+
+	/* Don't signal if we didn't change the value */
+	changes = val ^ orig;
+	if (!changes) {
+		spin_unlock_irqrestore(&smsm->lock, flags);
+		goto done;
+	}
+
+	/* Write out the new value */
+	writel(val, smsm->local_state);
+	spin_unlock_irqrestore(&smsm->lock, flags);
+
+	/* Make sure the value update is ordered before any kicks */
+	wmb();
+
+	/* Iterate over all hosts to check whom wants a kick */
+	for (host = 0; host < smsm->num_hosts; host++) {
+		hostp = &smsm->hosts[host];
+
+		val = readl(smsm->subscription + host);
+		if (val & changes && hostp->ipc_regmap) {
+			regmap_write(hostp->ipc_regmap,
+				     hostp->ipc_offset,
+				     BIT(hostp->ipc_bit));
+		}
+	}
+
+done:
+	return 0;
+}
+
+static const struct qcom_smem_state_ops smsm_state_ops = {
+	.update_bits = smsm_update_bits,
+};
+
+/**
+ * smsm_intr() - cascading IRQ handler for SMSM
+ * @irq:	unused
+ * @data:	entry related to this IRQ
+ *
+ * This function cascades an incoming interrupt from a remote system, based on
+ * the state bits and configuration.
+ */
+static irqreturn_t smsm_intr(int irq, void *data)
+{
+	struct smsm_entry *entry = data;
+	unsigned i;
+	int irq_pin;
+	u32 changed;
+	u32 val;
+
+	val = readl(entry->remote_state);
+	changed = val ^ entry->last_value;
+	entry->last_value = val;
+
+	for_each_set_bit(i, entry->irq_enabled, 32) {
+		if (!(changed & BIT(i)))
+			continue;
+
+		if (val & BIT(i)) {
+			if (test_bit(i, entry->irq_rising)) {
+				irq_pin = irq_find_mapping(entry->domain, i);
+				handle_nested_irq(irq_pin);
+			}
+		} else {
+			if (test_bit(i, entry->irq_falling)) {
+				irq_pin = irq_find_mapping(entry->domain, i);
+				handle_nested_irq(irq_pin);
+			}
+		}
+	}
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * smsm_mask_irq() - un-subscribe from cascades of IRQs of a certain staus bit
+ * @irqd:	IRQ handle to be masked
+ *
+ * This un-subscribes the local CPU from interrupts upon changes to the defines
+ * status bit. The bit is also cleared from cascading.
+ */
+static void smsm_mask_irq(struct irq_data *irqd)
+{
+	struct smsm_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+	struct qcom_smsm *smsm = entry->smsm;
+	u32 val;
+
+	if (entry->subscription) {
+		val = readl(entry->subscription + smsm->local_host);
+		val &= ~BIT(irq);
+		writel(val, entry->subscription + smsm->local_host);
+	}
+
+	clear_bit(irq, entry->irq_enabled);
+}
+
+/**
+ * smsm_unmask_irq() - subscribe to cascades of IRQs of a certain status bit
+ * @irqd:	IRQ handle to be unmasked
+ *
+
+ * This subscribes the local CPU to interrupts upon changes to the defined
+ * status bit. The bit is also marked for cascading.
+
+ */
+static void smsm_unmask_irq(struct irq_data *irqd)
+{
+	struct smsm_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+	struct qcom_smsm *smsm = entry->smsm;
+	u32 val;
+
+	set_bit(irq, entry->irq_enabled);
+
+	if (entry->subscription) {
+		val = readl(entry->subscription + smsm->local_host);
+		val |= BIT(irq);
+		writel(val, entry->subscription + smsm->local_host);
+	}
+}
+
+/**
+ * smsm_set_irq_type() - updates the requested IRQ type for the cascading
+ * @irqd:	consumer interrupt handle
+ * @type:	requested flags
+ */
+static int smsm_set_irq_type(struct irq_data *irqd, unsigned int type)
+{
+	struct smsm_entry *entry = irq_data_get_irq_chip_data(irqd);
+	irq_hw_number_t irq = irqd_to_hwirq(irqd);
+
+	if (!(type & IRQ_TYPE_EDGE_BOTH))
+		return -EINVAL;
+
+	if (type & IRQ_TYPE_EDGE_RISING)
+		set_bit(irq, entry->irq_rising);
+	else
+		clear_bit(irq, entry->irq_rising);
+
+	if (type & IRQ_TYPE_EDGE_FALLING)
+		set_bit(irq, entry->irq_falling);
+	else
+		clear_bit(irq, entry->irq_falling);
+
+	return 0;
+}
+
+static struct irq_chip smsm_irq_chip = {
+	.name           = "smsm",
+	.irq_mask       = smsm_mask_irq,
+	.irq_unmask     = smsm_unmask_irq,
+	.irq_set_type	= smsm_set_irq_type,
+};
+
+/**
+ * smsm_irq_map() - sets up a mapping for a cascaded IRQ
+ * @d:		IRQ domain representing an entry
+ * @irq:	IRQ to set up
+ * @hw:		unused
+ */
+static int smsm_irq_map(struct irq_domain *d,
+			unsigned int irq,
+			irq_hw_number_t hw)
+{
+	struct smsm_entry *entry = d->host_data;
+
+	irq_set_chip_and_handler(irq, &smsm_irq_chip, handle_level_irq);
+	irq_set_chip_data(irq, entry);
+	irq_set_nested_thread(irq, 1);
+
+	return 0;
+}
+
+static const struct irq_domain_ops smsm_irq_ops = {
+	.map = smsm_irq_map,
+	.xlate = irq_domain_xlate_twocell,
+};
+
+/**
+ * smsm_parse_ipc() - parses a qcom,ipc-%d device tree property
+ * @smsm:	smsm driver context
+ * @host_id:	index of the remote host to be resolved
+ *
+ * Parses device tree to acquire the information needed for sending the
+ * outgoing interrupts to a remote host - identified by @host_id.
+ */
+static int smsm_parse_ipc(struct qcom_smsm *smsm, unsigned host_id)
+{
+	struct device_node *syscon;
+	struct device_node *node = smsm->dev->of_node;
+	struct smsm_host *host = &smsm->hosts[host_id];
+	char key[16];
+	int ret;
+
+	snprintf(key, sizeof(key), "qcom,ipc-%d", host_id);
+	syscon = of_parse_phandle(node, key, 0);
+	if (!syscon)
+		return 0;
+
+	host->ipc_regmap = syscon_node_to_regmap(syscon);
+	if (IS_ERR(host->ipc_regmap))
+		return PTR_ERR(host->ipc_regmap);
+
+	ret = of_property_read_u32_index(node, key, 1, &host->ipc_offset);
+	if (ret < 0) {
+		dev_err(smsm->dev, "no offset in %s\n", key);
+		return -EINVAL;
+	}
+
+	ret = of_property_read_u32_index(node, key, 2, &host->ipc_bit);
+	if (ret < 0) {
+		dev_err(smsm->dev, "no bit in %s\n", key);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/**
+ * smsm_inbound_entry() - parse DT and set up an entry representing a remote system
+ * @smsm:	smsm driver context
+ * @entry:	entry context to be set up
+ * @node:	dt node containing the entry's properties
+ */
+static int smsm_inbound_entry(struct qcom_smsm *smsm,
+			      struct smsm_entry *entry,
+			      struct device_node *node)
+{
+	int ret;
+	int irq;
+
+	irq = irq_of_parse_and_map(node, 0);
+	if (!irq) {
+		dev_err(smsm->dev, "failed to parse smsm interrupt\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_threaded_irq(smsm->dev, irq,
+					NULL, smsm_intr,
+					IRQF_ONESHOT,
+					"smsm", (void *)entry);
+	if (ret) {
+		dev_err(smsm->dev, "failed to request interrupt\n");
+		return ret;
+	}
+
+	entry->domain = irq_domain_add_linear(node, 32, &smsm_irq_ops, entry);
+	if (!entry->domain) {
+		dev_err(smsm->dev, "failed to add irq_domain\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/**
+ * smsm_get_size_info() - parse the optional memory segment for sizes
+ * @smsm:	smsm driver context
+ *
+ * Attempt to acquire the number of hosts and entries from the optional shared
+ * memory location. Not being able to find this segment should indicate that
+ * we're on a older system where these values was hard coded to
+ * SMSM_DEFAULT_NUM_ENTRIES and SMSM_DEFAULT_NUM_HOSTS.
+ *
+ * Returns 0 on success, negative errno on failure.
+ */
+static int smsm_get_size_info(struct qcom_smsm *smsm)
+{
+	size_t size;
+	struct {
+		u32 num_hosts;
+		u32 num_entries;
+		u32 reserved0;
+		u32 reserved1;
+	} *info;
+
+	info = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SIZE_INFO, &size);
+	if (PTR_ERR(info) == -ENOENT || size != sizeof(*info)) {
+		dev_warn(smsm->dev, "no smsm size info, using defaults\n");
+		smsm->num_entries = SMSM_DEFAULT_NUM_ENTRIES;
+		smsm->num_hosts = SMSM_DEFAULT_NUM_HOSTS;
+		return 0;
+	} else if (IS_ERR(info)) {
+		dev_err(smsm->dev, "unable to retrieve smsm size info\n");
+		return PTR_ERR(info);
+	}
+
+	smsm->num_entries = info->num_entries;
+	smsm->num_hosts = info->num_hosts;
+
+	dev_dbg(smsm->dev,
+		"found custom size of smsm: %d entries %d hosts\n",
+		smsm->num_entries, smsm->num_hosts);
+
+	return 0;
+}
+
+static int qcom_smsm_probe(struct platform_device *pdev)
+{
+	struct device_node *local_node;
+	struct device_node *node;
+	struct smsm_entry *entry;
+	struct qcom_smsm *smsm;
+	u32 *intr_mask;
+	size_t size;
+	u32 *states;
+	u32 id;
+	int ret;
+
+	smsm = devm_kzalloc(&pdev->dev, sizeof(*smsm), GFP_KERNEL);
+	if (!smsm)
+		return -ENOMEM;
+	smsm->dev = &pdev->dev;
+	spin_lock_init(&smsm->lock);
+
+	ret = smsm_get_size_info(smsm);
+	if (ret)
+		return ret;
+
+	smsm->entries = devm_kcalloc(&pdev->dev,
+				     smsm->num_entries,
+				     sizeof(struct smsm_entry),
+				     GFP_KERNEL);
+	if (!smsm->entries)
+		return -ENOMEM;
+
+	smsm->hosts = devm_kcalloc(&pdev->dev,
+				   smsm->num_hosts,
+				   sizeof(struct smsm_host),
+				   GFP_KERNEL);
+	if (!smsm->hosts)
+		return -ENOMEM;
+
+	local_node = of_find_node_with_property(pdev->dev.of_node, "#qcom,state-cells");
+	if (!local_node) {
+		dev_err(&pdev->dev, "no state entry\n");
+		return -EINVAL;
+	}
+
+	of_property_read_u32(pdev->dev.of_node,
+			     "qcom,local-host",
+			     &smsm->local_host);
+
+	/* Parse the host properties */
+	for (id = 0; id < smsm->num_hosts; id++) {
+		ret = smsm_parse_ipc(smsm, id);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* Acquire the main SMSM state vector */
+	ret = qcom_smem_alloc(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SHARED_STATE,
+			      smsm->num_entries * sizeof(u32));
+	if (ret < 0 && ret != -EEXIST) {
+		dev_err(&pdev->dev, "unable to allocate shared state entry\n");
+		return ret;
+	}
+
+	states = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_SHARED_STATE, NULL);
+	if (IS_ERR(states)) {
+		dev_err(&pdev->dev, "Unable to acquire shared state entry\n");
+		return PTR_ERR(states);
+	}
+
+	/* Acquire the list of interrupt mask vectors */
+	size = smsm->num_entries * smsm->num_hosts * sizeof(u32);
+	ret = qcom_smem_alloc(QCOM_SMEM_HOST_ANY, SMEM_SMSM_CPU_INTR_MASK, size);
+	if (ret < 0 && ret != -EEXIST) {
+		dev_err(&pdev->dev, "unable to allocate smsm interrupt mask\n");
+		return ret;
+	}
+
+	intr_mask = qcom_smem_get(QCOM_SMEM_HOST_ANY, SMEM_SMSM_CPU_INTR_MASK, NULL);
+	if (IS_ERR(intr_mask)) {
+		dev_err(&pdev->dev, "unable to acquire shared memory interrupt mask\n");
+		return PTR_ERR(intr_mask);
+	}
+
+	/* Setup the reference to the local state bits */
+	smsm->local_state = states + smsm->local_host;
+	smsm->subscription = intr_mask + smsm->local_host * smsm->num_hosts;
+
+	/* Register the outgoing state */
+	smsm->state = qcom_smem_state_register(local_node, &smsm_state_ops, smsm);
+	if (IS_ERR(smsm->state)) {
+		dev_err(smsm->dev, "failed to register qcom_smem_state\n");
+		return PTR_ERR(smsm->state);
+	}
+
+	/* Register handlers for remote processor entries of interest. */
+	for_each_available_child_of_node(pdev->dev.of_node, node) {
+		if (!of_property_read_bool(node, "interrupt-controller"))
+			continue;
+
+		ret = of_property_read_u32(node, "reg", &id);
+		if (ret || id >= smsm->num_entries) {
+			dev_err(&pdev->dev, "invalid reg of entry\n");
+			if (!ret)
+				ret = -EINVAL;
+			goto unwind_interfaces;
+		}
+		entry = &smsm->entries[id];
+
+		entry->smsm = smsm;
+		entry->remote_state = states + id;
+
+		/* Setup subscription pointers and unsubscribe to any kicks */
+		entry->subscription = intr_mask + id * smsm->num_hosts;
+		writel(0, entry->subscription + smsm->local_host);
+
+		ret = smsm_inbound_entry(smsm, entry, node);
+		if (ret < 0)
+			goto unwind_interfaces;
+	}
+
+	platform_set_drvdata(pdev, smsm);
+
+	return 0;
+
+unwind_interfaces:
+	for (id = 0; id < smsm->num_entries; id++)
+		if (smsm->entries[id].domain)
+			irq_domain_remove(smsm->entries[id].domain);
+
+	qcom_smem_state_unregister(smsm->state);
+
+	return ret;
+}
+
+static int qcom_smsm_remove(struct platform_device *pdev)
+{
+	struct qcom_smsm *smsm = platform_get_drvdata(pdev);
+	unsigned id;
+
+	for (id = 0; id < smsm->num_entries; id++)
+		if (smsm->entries[id].domain)
+			irq_domain_remove(smsm->entries[id].domain);
+
+	qcom_smem_state_unregister(smsm->state);
+
+	return 0;
+}
+
+static const struct of_device_id qcom_smsm_of_match[] = {
+	{ .compatible = "qcom,smsm" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, qcom_smsm_of_match);
+
+static struct platform_driver qcom_smsm_driver = {
+	.probe = qcom_smsm_probe,
+	.remove = qcom_smsm_remove,
+	.driver  = {
+		.name  = "qcom-smsm",
+		.of_match_table = qcom_smsm_of_match,
+	},
+};
+module_platform_driver(qcom_smsm_driver);
+
+MODULE_DESCRIPTION("Qualcomm Shared Memory State Machine driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/soc/qcom/spm.c b/drivers/soc/qcom/spm.c
index b04b05a0904e..5548a31e1a39 100644
--- a/drivers/soc/qcom/spm.c
+++ b/drivers/soc/qcom/spm.c
@@ -116,7 +116,7 @@ static const struct spm_reg_data spm_reg_8064_cpu = {
 
 static DEFINE_PER_CPU(struct spm_driver_data *, cpu_spm_drv);
 
-typedef int (*idle_fn)(int);
+typedef int (*idle_fn)(void);
 static DEFINE_PER_CPU(idle_fn*, qcom_idle_ops);
 
 static inline void spm_register_write(struct spm_driver_data *drv,
@@ -179,10 +179,10 @@ static int qcom_pm_collapse(unsigned long int unused)
 	return -1;
 }
 
-static int qcom_cpu_spc(int cpu)
+static int qcom_cpu_spc(void)
 {
 	int ret;
-	struct spm_driver_data *drv = per_cpu(cpu_spm_drv, cpu);
+	struct spm_driver_data *drv = __this_cpu_read(cpu_spm_drv);
 
 	spm_set_low_power_mode(drv, PM_SLEEP_MODE_SPC);
 	ret = cpu_suspend(0, qcom_pm_collapse);
@@ -197,9 +197,9 @@ static int qcom_cpu_spc(int cpu)
 	return ret;
 }
 
-static int qcom_idle_enter(int cpu, unsigned long index)
+static int qcom_idle_enter(unsigned long index)
 {
-	return per_cpu(qcom_idle_ops, cpu)[index](cpu);
+	return __this_cpu_read(qcom_idle_ops)[index]();
 }
 
 static const struct of_device_id qcom_idle_state_match[] __initconst = {
@@ -288,7 +288,7 @@ static struct spm_driver_data *spm_get_drv(struct platform_device *pdev,
 	struct spm_driver_data *drv = NULL;
 	struct device_node *cpu_node, *saw_node;
 	int cpu;
-	bool found;
+	bool found = 0;
 
 	for_each_possible_cpu(cpu) {
 		cpu_node = of_cpu_device_node_get(cpu);
diff --git a/drivers/soc/qcom/wcnss_ctrl.c b/drivers/soc/qcom/wcnss_ctrl.c
new file mode 100644
index 000000000000..7a986f881d5c
--- /dev/null
+++ b/drivers/soc/qcom/wcnss_ctrl.c
@@ -0,0 +1,272 @@
+/*
+ * 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/firmware.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/soc/qcom/smd.h>
+
+#define WCNSS_REQUEST_TIMEOUT	(5 * HZ)
+
+#define NV_FRAGMENT_SIZE	3072
+#define NVBIN_FILE		"wlan/prima/WCNSS_qcom_wlan_nv.bin"
+
+/**
+ * struct wcnss_ctrl - driver context
+ * @dev:	device handle
+ * @channel:	SMD channel handle
+ * @ack:	completion for outstanding requests
+ * @ack_status:	status of the outstanding request
+ * @download_nv_work: worker for uploading nv binary
+ */
+struct wcnss_ctrl {
+	struct device *dev;
+	struct qcom_smd_channel *channel;
+
+	struct completion ack;
+	int ack_status;
+
+	struct work_struct download_nv_work;
+};
+
+/* message types */
+enum {
+	WCNSS_VERSION_REQ = 0x01000000,
+	WCNSS_VERSION_RESP,
+	WCNSS_DOWNLOAD_NV_REQ,
+	WCNSS_DOWNLOAD_NV_RESP,
+	WCNSS_UPLOAD_CAL_REQ,
+	WCNSS_UPLOAD_CAL_RESP,
+	WCNSS_DOWNLOAD_CAL_REQ,
+	WCNSS_DOWNLOAD_CAL_RESP,
+};
+
+/**
+ * struct wcnss_msg_hdr - common packet header for requests and responses
+ * @type:	packet message type
+ * @len:	total length of the packet, including this header
+ */
+struct wcnss_msg_hdr {
+	u32 type;
+	u32 len;
+} __packed;
+
+/**
+ * struct wcnss_version_resp - version request response
+ * @hdr:	common packet wcnss_msg_hdr header
+ */
+struct wcnss_version_resp {
+	struct wcnss_msg_hdr hdr;
+	u8 major;
+	u8 minor;
+	u8 version;
+	u8 revision;
+} __packed;
+
+/**
+ * struct wcnss_download_nv_req - firmware fragment request
+ * @hdr:	common packet wcnss_msg_hdr header
+ * @seq:	sequence number of this fragment
+ * @last:	boolean indicator of this being the last fragment of the binary
+ * @frag_size:	length of this fragment
+ * @fragment:	fragment data
+ */
+struct wcnss_download_nv_req {
+	struct wcnss_msg_hdr hdr;
+	u16 seq;
+	u16 last;
+	u32 frag_size;
+	u8 fragment[];
+} __packed;
+
+/**
+ * struct wcnss_download_nv_resp - firmware download response
+ * @hdr:	common packet wcnss_msg_hdr header
+ * @status:	boolean to indicate success of the download
+ */
+struct wcnss_download_nv_resp {
+	struct wcnss_msg_hdr hdr;
+	u8 status;
+} __packed;
+
+/**
+ * wcnss_ctrl_smd_callback() - handler from SMD responses
+ * @qsdev:	smd device handle
+ * @data:	pointer to the incoming data packet
+ * @count:	size of the incoming data packet
+ *
+ * Handles any incoming packets from the remote WCNSS_CTRL service.
+ */
+static int wcnss_ctrl_smd_callback(struct qcom_smd_device *qsdev,
+				   const void *data,
+				   size_t count)
+{
+	struct wcnss_ctrl *wcnss = dev_get_drvdata(&qsdev->dev);
+	const struct wcnss_download_nv_resp *nvresp;
+	const struct wcnss_version_resp *version;
+	const struct wcnss_msg_hdr *hdr = data;
+
+	switch (hdr->type) {
+	case WCNSS_VERSION_RESP:
+		if (count != sizeof(*version)) {
+			dev_err(wcnss->dev,
+				"invalid size of version response\n");
+			break;
+		}
+
+		version = data;
+		dev_info(wcnss->dev, "WCNSS Version %d.%d %d.%d\n",
+			 version->major, version->minor,
+			 version->version, version->revision);
+
+		schedule_work(&wcnss->download_nv_work);
+		break;
+	case WCNSS_DOWNLOAD_NV_RESP:
+		if (count != sizeof(*nvresp)) {
+			dev_err(wcnss->dev,
+				"invalid size of download response\n");
+			break;
+		}
+
+		nvresp = data;
+		wcnss->ack_status = nvresp->status;
+		complete(&wcnss->ack);
+		break;
+	default:
+		dev_info(wcnss->dev, "unknown message type %d\n", hdr->type);
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ * wcnss_request_version() - send a version request to WCNSS
+ * @wcnss:	wcnss ctrl driver context
+ */
+static int wcnss_request_version(struct wcnss_ctrl *wcnss)
+{
+	struct wcnss_msg_hdr msg;
+
+	msg.type = WCNSS_VERSION_REQ;
+	msg.len = sizeof(msg);
+
+	return qcom_smd_send(wcnss->channel, &msg, sizeof(msg));
+}
+
+/**
+ * wcnss_download_nv() - send nv binary to WCNSS
+ * @work:	work struct to acquire wcnss context
+ */
+static void wcnss_download_nv(struct work_struct *work)
+{
+	struct wcnss_ctrl *wcnss = container_of(work, struct wcnss_ctrl, download_nv_work);
+	struct wcnss_download_nv_req *req;
+	const struct firmware *fw;
+	const void *data;
+	ssize_t left;
+	int ret;
+
+	req = kzalloc(sizeof(*req) + NV_FRAGMENT_SIZE, GFP_KERNEL);
+	if (!req)
+		return;
+
+	ret = request_firmware(&fw, NVBIN_FILE, wcnss->dev);
+	if (ret) {
+		dev_err(wcnss->dev, "Failed to load nv file %s: %d\n",
+			NVBIN_FILE, ret);
+		goto free_req;
+	}
+
+	data = fw->data;
+	left = fw->size;
+
+	req->hdr.type = WCNSS_DOWNLOAD_NV_REQ;
+	req->hdr.len = sizeof(*req) + NV_FRAGMENT_SIZE;
+
+	req->last = 0;
+	req->frag_size = NV_FRAGMENT_SIZE;
+
+	req->seq = 0;
+	do {
+		if (left <= NV_FRAGMENT_SIZE) {
+			req->last = 1;
+			req->frag_size = left;
+			req->hdr.len = sizeof(*req) + left;
+		}
+
+		memcpy(req->fragment, data, req->frag_size);
+
+		ret = qcom_smd_send(wcnss->channel, req, req->hdr.len);
+		if (ret) {
+			dev_err(wcnss->dev, "failed to send smd packet\n");
+			goto release_fw;
+		}
+
+		/* Increment for next fragment */
+		req->seq++;
+
+		data += req->hdr.len;
+		left -= NV_FRAGMENT_SIZE;
+	} while (left > 0);
+
+	ret = wait_for_completion_timeout(&wcnss->ack, WCNSS_REQUEST_TIMEOUT);
+	if (!ret)
+		dev_err(wcnss->dev, "timeout waiting for nv upload ack\n");
+	else if (wcnss->ack_status != 1)
+		dev_err(wcnss->dev, "nv upload response failed err: %d\n",
+			wcnss->ack_status);
+
+release_fw:
+	release_firmware(fw);
+free_req:
+	kfree(req);
+}
+
+static int wcnss_ctrl_probe(struct qcom_smd_device *sdev)
+{
+	struct wcnss_ctrl *wcnss;
+
+	wcnss = devm_kzalloc(&sdev->dev, sizeof(*wcnss), GFP_KERNEL);
+	if (!wcnss)
+		return -ENOMEM;
+
+	wcnss->dev = &sdev->dev;
+	wcnss->channel = sdev->channel;
+
+	init_completion(&wcnss->ack);
+	INIT_WORK(&wcnss->download_nv_work, wcnss_download_nv);
+
+	dev_set_drvdata(&sdev->dev, wcnss);
+
+	return wcnss_request_version(wcnss);
+}
+
+static const struct qcom_smd_id wcnss_ctrl_smd_match[] = {
+	{ .name = "WCNSS_CTRL" },
+	{}
+};
+
+static struct qcom_smd_driver wcnss_ctrl_driver = {
+	.probe = wcnss_ctrl_probe,
+	.callback = wcnss_ctrl_smd_callback,
+	.smd_match_table = wcnss_ctrl_smd_match,
+	.driver  = {
+		.name  = "qcom_wcnss_ctrl",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_qcom_smd_driver(wcnss_ctrl_driver);
+
+MODULE_DESCRIPTION("Qualcomm WCNSS control driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/soc/tegra/Kconfig b/drivers/soc/tegra/Kconfig
new file mode 100644
index 000000000000..d0c3c3e085e3
--- /dev/null
+++ b/drivers/soc/tegra/Kconfig
@@ -0,0 +1,83 @@
+if ARCH_TEGRA
+
+# 32-bit ARM SoCs
+if ARM
+
+config ARCH_TEGRA_2x_SOC
+	bool "Enable support for Tegra20 family"
+	select ARCH_NEEDS_CPU_IDLE_COUPLED if SMP
+	select ARM_ERRATA_720789
+	select ARM_ERRATA_754327 if SMP
+	select ARM_ERRATA_764369 if SMP
+	select PINCTRL_TEGRA20
+	select PL310_ERRATA_727915 if CACHE_L2X0
+	select PL310_ERRATA_769419 if CACHE_L2X0
+	select TEGRA_TIMER
+	help
+	  Support for NVIDIA Tegra AP20 and T20 processors, based on the
+	  ARM CortexA9MP CPU and the ARM PL310 L2 cache controller
+
+config ARCH_TEGRA_3x_SOC
+	bool "Enable support for Tegra30 family"
+	select ARM_ERRATA_754322
+	select ARM_ERRATA_764369 if SMP
+	select PINCTRL_TEGRA30
+	select PL310_ERRATA_769419 if CACHE_L2X0
+	select TEGRA_TIMER
+	help
+	  Support for NVIDIA Tegra T30 processor family, based on the
+	  ARM CortexA9MP CPU and the ARM PL310 L2 cache controller
+
+config ARCH_TEGRA_114_SOC
+	bool "Enable support for Tegra114 family"
+	select ARM_ERRATA_798181 if SMP
+	select ARM_L1_CACHE_SHIFT_6
+	select HAVE_ARM_ARCH_TIMER
+	select PINCTRL_TEGRA114
+	select TEGRA_TIMER
+	help
+	  Support for NVIDIA Tegra T114 processor family, based on the
+	  ARM CortexA15MP CPU
+
+config ARCH_TEGRA_124_SOC
+	bool "Enable support for Tegra124 family"
+	select ARM_L1_CACHE_SHIFT_6
+	select HAVE_ARM_ARCH_TIMER
+	select PINCTRL_TEGRA124
+	select TEGRA_TIMER
+	help
+	  Support for NVIDIA Tegra T124 processor family, based on the
+	  ARM CortexA15MP CPU
+
+endif
+
+# 64-bit ARM SoCs
+if ARM64
+
+config ARCH_TEGRA_132_SOC
+	bool "NVIDIA Tegra132 SoC"
+	select PINCTRL_TEGRA124
+	help
+	  Enable support for NVIDIA Tegra132 SoC, based on the Denver
+	  ARMv8 CPU.  The Tegra132 SoC is similar to the Tegra124 SoC,
+	  but contains an NVIDIA Denver CPU complex in place of
+	  Tegra124's "4+1" Cortex-A15 CPU complex.
+
+config ARCH_TEGRA_210_SOC
+	bool "NVIDIA Tegra210 SoC"
+	select PINCTRL_TEGRA210
+	help
+	  Enable support for the NVIDIA Tegra210 SoC. Also known as Tegra X1,
+	  the Tegra210 has four Cortex-A57 cores paired with four Cortex-A53
+	  cores in a switched configuration. It features a GPU of the Maxwell
+	  architecture with support for DX11, SM4, OpenGL 4.5, OpenGL ES 3.1
+	  and providing 256 CUDA cores. It supports hardware-accelerated en-
+	  and decoding of various video standards including H.265, H.264 and
+	  VP8 at 4K resolution and up to 60 fps.
+
+	  Besides the multimedia features it also comes with a variety of I/O
+	  controllers, such as GPIO, I2C, SPI, SDHCI, PCIe, SATA and XHCI, to
+	  name only a few.
+
+endif
+endif
diff --git a/drivers/soc/ti/Kconfig b/drivers/soc/ti/Kconfig
index 7266b2165183..3557c5e32a93 100644
--- a/drivers/soc/ti/Kconfig
+++ b/drivers/soc/ti/Kconfig
@@ -28,4 +28,14 @@ config KEYSTONE_NAVIGATOR_DMA
 
 	  If unsure, say N.
 
+config WKUP_M3_IPC
+	tristate "TI AMx3 Wkup-M3 IPC Driver"
+	depends on WKUP_M3_RPROC
+	depends on OMAP2PLUS_MBOX
+	help
+	  TI AM33XX and AM43XX have a Cortex M3, the Wakeup M3, to handle
+	  low power transitions. This IPC driver provides the necessary API
+	  to communicate and use the Wakeup M3 for PM features like suspend
+	  resume and boots it using wkup_m3_rproc driver.
+
 endif # SOC_TI
diff --git a/drivers/soc/ti/Makefile b/drivers/soc/ti/Makefile
index 135bdad7a6de..48ff3a79634f 100644
--- a/drivers/soc/ti/Makefile
+++ b/drivers/soc/ti/Makefile
@@ -4,3 +4,4 @@
 obj-$(CONFIG_KEYSTONE_NAVIGATOR_QMSS)	+= knav_qmss.o
 knav_qmss-y := knav_qmss_queue.o knav_qmss_acc.o
 obj-$(CONFIG_KEYSTONE_NAVIGATOR_DMA)	+= knav_dma.o
+obj-$(CONFIG_WKUP_M3_IPC)		+= wkup_m3_ipc.o
diff --git a/drivers/soc/ti/knav_dma.c b/drivers/soc/ti/knav_dma.c
index bc1b80ec6afe..1a7b5caa127b 100644
--- a/drivers/soc/ti/knav_dma.c
+++ b/drivers/soc/ti/knav_dma.c
@@ -389,7 +389,7 @@ static int of_channel_match_helper(struct device_node *np, const char *name,
 	*dma_instance = dma_node->name;
 	index = of_property_match_string(np, "ti,navigator-dma-names", name);
 	if (index < 0) {
-		dev_err(kdev->dev, "No 'ti,navigator-dma-names' propery\n");
+		dev_err(kdev->dev, "No 'ti,navigator-dma-names' property\n");
 		return -ENODEV;
 	}
 
diff --git a/drivers/soc/ti/wkup_m3_ipc.c b/drivers/soc/ti/wkup_m3_ipc.c
new file mode 100644
index 000000000000..8823cc81ae45
--- /dev/null
+++ b/drivers/soc/ti/wkup_m3_ipc.c
@@ -0,0 +1,508 @@
+/*
+ * AMx3 Wkup M3 IPC driver
+ *
+ * Copyright (C) 2015 Texas Instruments, Inc.
+ *
+ * Dave Gerlach <d-gerlach@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/omap-mailbox.h>
+#include <linux/platform_device.h>
+#include <linux/remoteproc.h>
+#include <linux/suspend.h>
+#include <linux/wkup_m3_ipc.h>
+
+#define AM33XX_CTRL_IPC_REG_COUNT	0x8
+#define AM33XX_CTRL_IPC_REG_OFFSET(m)	(0x4 + 4 * (m))
+
+/* AM33XX M3_TXEV_EOI register */
+#define AM33XX_CONTROL_M3_TXEV_EOI	0x00
+
+#define AM33XX_M3_TXEV_ACK		(0x1 << 0)
+#define AM33XX_M3_TXEV_ENABLE		(0x0 << 0)
+
+#define IPC_CMD_DS0			0x4
+#define IPC_CMD_STANDBY			0xc
+#define IPC_CMD_IDLE			0x10
+#define IPC_CMD_RESET			0xe
+#define DS_IPC_DEFAULT			0xffffffff
+#define M3_VERSION_UNKNOWN		0x0000ffff
+#define M3_BASELINE_VERSION		0x191
+#define M3_STATUS_RESP_MASK		(0xffff << 16)
+#define M3_FW_VERSION_MASK		0xffff
+
+#define M3_STATE_UNKNOWN		0
+#define M3_STATE_RESET			1
+#define M3_STATE_INITED			2
+#define M3_STATE_MSG_FOR_LP		3
+#define M3_STATE_MSG_FOR_RESET		4
+
+static struct wkup_m3_ipc *m3_ipc_state;
+
+static void am33xx_txev_eoi(struct wkup_m3_ipc *m3_ipc)
+{
+	writel(AM33XX_M3_TXEV_ACK,
+	       m3_ipc->ipc_mem_base + AM33XX_CONTROL_M3_TXEV_EOI);
+}
+
+static void am33xx_txev_enable(struct wkup_m3_ipc *m3_ipc)
+{
+	writel(AM33XX_M3_TXEV_ENABLE,
+	       m3_ipc->ipc_mem_base + AM33XX_CONTROL_M3_TXEV_EOI);
+}
+
+static void wkup_m3_ctrl_ipc_write(struct wkup_m3_ipc *m3_ipc,
+				   u32 val, int ipc_reg_num)
+{
+	if (WARN(ipc_reg_num < 0 || ipc_reg_num > AM33XX_CTRL_IPC_REG_COUNT,
+		 "ipc register operation out of range"))
+		return;
+
+	writel(val, m3_ipc->ipc_mem_base +
+	       AM33XX_CTRL_IPC_REG_OFFSET(ipc_reg_num));
+}
+
+static unsigned int wkup_m3_ctrl_ipc_read(struct wkup_m3_ipc *m3_ipc,
+					  int ipc_reg_num)
+{
+	if (WARN(ipc_reg_num < 0 || ipc_reg_num > AM33XX_CTRL_IPC_REG_COUNT,
+		 "ipc register operation out of range"))
+		return 0;
+
+	return readl(m3_ipc->ipc_mem_base +
+		     AM33XX_CTRL_IPC_REG_OFFSET(ipc_reg_num));
+}
+
+static int wkup_m3_fw_version_read(struct wkup_m3_ipc *m3_ipc)
+{
+	int val;
+
+	val = wkup_m3_ctrl_ipc_read(m3_ipc, 2);
+
+	return val & M3_FW_VERSION_MASK;
+}
+
+static irqreturn_t wkup_m3_txev_handler(int irq, void *ipc_data)
+{
+	struct wkup_m3_ipc *m3_ipc = ipc_data;
+	struct device *dev = m3_ipc->dev;
+	int ver = 0;
+
+	am33xx_txev_eoi(m3_ipc);
+
+	switch (m3_ipc->state) {
+	case M3_STATE_RESET:
+		ver = wkup_m3_fw_version_read(m3_ipc);
+
+		if (ver == M3_VERSION_UNKNOWN ||
+		    ver < M3_BASELINE_VERSION) {
+			dev_warn(dev, "CM3 Firmware Version %x not supported\n",
+				 ver);
+		} else {
+			dev_info(dev, "CM3 Firmware Version = 0x%x\n", ver);
+		}
+
+		m3_ipc->state = M3_STATE_INITED;
+		complete(&m3_ipc->sync_complete);
+		break;
+	case M3_STATE_MSG_FOR_RESET:
+		m3_ipc->state = M3_STATE_INITED;
+		complete(&m3_ipc->sync_complete);
+		break;
+	case M3_STATE_MSG_FOR_LP:
+		complete(&m3_ipc->sync_complete);
+		break;
+	case M3_STATE_UNKNOWN:
+		dev_warn(dev, "Unknown CM3 State\n");
+	}
+
+	am33xx_txev_enable(m3_ipc);
+
+	return IRQ_HANDLED;
+}
+
+static int wkup_m3_ping(struct wkup_m3_ipc *m3_ipc)
+{
+	struct device *dev = m3_ipc->dev;
+	mbox_msg_t dummy_msg = 0;
+	int ret;
+
+	if (!m3_ipc->mbox) {
+		dev_err(dev,
+			"No IPC channel to communicate with wkup_m3!\n");
+		return -EIO;
+	}
+
+	/*
+	 * Write a dummy message to the mailbox in order to trigger the RX
+	 * interrupt to alert the M3 that data is available in the IPC
+	 * registers. We must enable the IRQ here and disable it after in
+	 * the RX callback to avoid multiple interrupts being received
+	 * by the CM3.
+	 */
+	ret = mbox_send_message(m3_ipc->mbox, &dummy_msg);
+	if (ret < 0) {
+		dev_err(dev, "%s: mbox_send_message() failed: %d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	ret = wait_for_completion_timeout(&m3_ipc->sync_complete,
+					  msecs_to_jiffies(500));
+	if (!ret) {
+		dev_err(dev, "MPU<->CM3 sync failure\n");
+		m3_ipc->state = M3_STATE_UNKNOWN;
+		return -EIO;
+	}
+
+	mbox_client_txdone(m3_ipc->mbox, 0);
+	return 0;
+}
+
+static int wkup_m3_ping_noirq(struct wkup_m3_ipc *m3_ipc)
+{
+	struct device *dev = m3_ipc->dev;
+	mbox_msg_t dummy_msg = 0;
+	int ret;
+
+	if (!m3_ipc->mbox) {
+		dev_err(dev,
+			"No IPC channel to communicate with wkup_m3!\n");
+		return -EIO;
+	}
+
+	ret = mbox_send_message(m3_ipc->mbox, &dummy_msg);
+	if (ret < 0) {
+		dev_err(dev, "%s: mbox_send_message() failed: %d\n",
+			__func__, ret);
+		return ret;
+	}
+
+	mbox_client_txdone(m3_ipc->mbox, 0);
+	return 0;
+}
+
+static int wkup_m3_is_available(struct wkup_m3_ipc *m3_ipc)
+{
+	return ((m3_ipc->state != M3_STATE_RESET) &&
+		(m3_ipc->state != M3_STATE_UNKNOWN));
+}
+
+/* Public functions */
+/**
+ * wkup_m3_set_mem_type - Pass wkup_m3 which type of memory is in use
+ * @mem_type: memory type value read directly from emif
+ *
+ * wkup_m3 must know what memory type is in use to properly suspend
+ * and resume.
+ */
+static void wkup_m3_set_mem_type(struct wkup_m3_ipc *m3_ipc, int mem_type)
+{
+	m3_ipc->mem_type = mem_type;
+}
+
+/**
+ * wkup_m3_set_resume_address - Pass wkup_m3 resume address
+ * @addr: Physical address from which resume code should execute
+ */
+static void wkup_m3_set_resume_address(struct wkup_m3_ipc *m3_ipc, void *addr)
+{
+	m3_ipc->resume_addr = (unsigned long)addr;
+}
+
+/**
+ * wkup_m3_request_pm_status - Retrieve wkup_m3 status code after suspend
+ *
+ * Returns code representing the status of a low power mode transition.
+ *	0 - Successful transition
+ *	1 - Failure to transition to low power state
+ */
+static int wkup_m3_request_pm_status(struct wkup_m3_ipc *m3_ipc)
+{
+	unsigned int i;
+	int val;
+
+	val = wkup_m3_ctrl_ipc_read(m3_ipc, 1);
+
+	i = M3_STATUS_RESP_MASK & val;
+	i >>= __ffs(M3_STATUS_RESP_MASK);
+
+	return i;
+}
+
+/**
+ * wkup_m3_prepare_low_power - Request preparation for transition to
+ *			       low power state
+ * @state: A kernel suspend state to enter, either MEM or STANDBY
+ *
+ * Returns 0 if preparation was successful, otherwise returns error code
+ */
+static int wkup_m3_prepare_low_power(struct wkup_m3_ipc *m3_ipc, int state)
+{
+	struct device *dev = m3_ipc->dev;
+	int m3_power_state;
+	int ret = 0;
+
+	if (!wkup_m3_is_available(m3_ipc))
+		return -ENODEV;
+
+	switch (state) {
+	case WKUP_M3_DEEPSLEEP:
+		m3_power_state = IPC_CMD_DS0;
+		break;
+	case WKUP_M3_STANDBY:
+		m3_power_state = IPC_CMD_STANDBY;
+		break;
+	case WKUP_M3_IDLE:
+		m3_power_state = IPC_CMD_IDLE;
+		break;
+	default:
+		return 1;
+	}
+
+	/* Program each required IPC register then write defaults to others */
+	wkup_m3_ctrl_ipc_write(m3_ipc, m3_ipc->resume_addr, 0);
+	wkup_m3_ctrl_ipc_write(m3_ipc, m3_power_state, 1);
+	wkup_m3_ctrl_ipc_write(m3_ipc, m3_ipc->mem_type, 4);
+
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 2);
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 3);
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 5);
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 6);
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 7);
+
+	m3_ipc->state = M3_STATE_MSG_FOR_LP;
+
+	if (state == WKUP_M3_IDLE)
+		ret = wkup_m3_ping_noirq(m3_ipc);
+	else
+		ret = wkup_m3_ping(m3_ipc);
+
+	if (ret) {
+		dev_err(dev, "Unable to ping CM3\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * wkup_m3_finish_low_power - Return m3 to reset state
+ *
+ * Returns 0 if reset was successful, otherwise returns error code
+ */
+static int wkup_m3_finish_low_power(struct wkup_m3_ipc *m3_ipc)
+{
+	struct device *dev = m3_ipc->dev;
+	int ret = 0;
+
+	if (!wkup_m3_is_available(m3_ipc))
+		return -ENODEV;
+
+	wkup_m3_ctrl_ipc_write(m3_ipc, IPC_CMD_RESET, 1);
+	wkup_m3_ctrl_ipc_write(m3_ipc, DS_IPC_DEFAULT, 2);
+
+	m3_ipc->state = M3_STATE_MSG_FOR_RESET;
+
+	ret = wkup_m3_ping(m3_ipc);
+	if (ret) {
+		dev_err(dev, "Unable to ping CM3\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static struct wkup_m3_ipc_ops ipc_ops = {
+	.set_mem_type = wkup_m3_set_mem_type,
+	.set_resume_address = wkup_m3_set_resume_address,
+	.prepare_low_power = wkup_m3_prepare_low_power,
+	.finish_low_power = wkup_m3_finish_low_power,
+	.request_pm_status = wkup_m3_request_pm_status,
+};
+
+/**
+ * wkup_m3_ipc_get - Return handle to wkup_m3_ipc
+ *
+ * Returns NULL if the wkup_m3 is not yet available, otherwise returns
+ * pointer to wkup_m3_ipc struct.
+ */
+struct wkup_m3_ipc *wkup_m3_ipc_get(void)
+{
+	if (m3_ipc_state)
+		get_device(m3_ipc_state->dev);
+	else
+		return NULL;
+
+	return m3_ipc_state;
+}
+EXPORT_SYMBOL_GPL(wkup_m3_ipc_get);
+
+/**
+ * wkup_m3_ipc_put - Free handle to wkup_m3_ipc returned from wkup_m3_ipc_get
+ * @m3_ipc: A pointer to wkup_m3_ipc struct returned by wkup_m3_ipc_get
+ */
+void wkup_m3_ipc_put(struct wkup_m3_ipc *m3_ipc)
+{
+	if (m3_ipc_state)
+		put_device(m3_ipc_state->dev);
+}
+EXPORT_SYMBOL_GPL(wkup_m3_ipc_put);
+
+static void wkup_m3_rproc_boot_thread(struct wkup_m3_ipc *m3_ipc)
+{
+	struct device *dev = m3_ipc->dev;
+	int ret;
+
+	wait_for_completion(&m3_ipc->rproc->firmware_loading_complete);
+
+	init_completion(&m3_ipc->sync_complete);
+
+	ret = rproc_boot(m3_ipc->rproc);
+	if (ret)
+		dev_err(dev, "rproc_boot failed\n");
+
+	do_exit(0);
+}
+
+static int wkup_m3_ipc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	int irq, ret;
+	phandle rproc_phandle;
+	struct rproc *m3_rproc;
+	struct resource *res;
+	struct task_struct *task;
+	struct wkup_m3_ipc *m3_ipc;
+
+	m3_ipc = devm_kzalloc(dev, sizeof(*m3_ipc), GFP_KERNEL);
+	if (!m3_ipc)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	m3_ipc->ipc_mem_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(m3_ipc->ipc_mem_base)) {
+		dev_err(dev, "could not ioremap ipc_mem\n");
+		return PTR_ERR(m3_ipc->ipc_mem_base);
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (!irq) {
+		dev_err(&pdev->dev, "no irq resource\n");
+		return -ENXIO;
+	}
+
+	ret = devm_request_irq(dev, irq, wkup_m3_txev_handler,
+			       0, "wkup_m3_txev", m3_ipc);
+	if (ret) {
+		dev_err(dev, "request_irq failed\n");
+		return ret;
+	}
+
+	m3_ipc->mbox_client.dev = dev;
+	m3_ipc->mbox_client.tx_done = NULL;
+	m3_ipc->mbox_client.tx_prepare = NULL;
+	m3_ipc->mbox_client.rx_callback = NULL;
+	m3_ipc->mbox_client.tx_block = false;
+	m3_ipc->mbox_client.knows_txdone = false;
+
+	m3_ipc->mbox = mbox_request_channel(&m3_ipc->mbox_client, 0);
+
+	if (IS_ERR(m3_ipc->mbox)) {
+		dev_err(dev, "IPC Request for A8->M3 Channel failed! %ld\n",
+			PTR_ERR(m3_ipc->mbox));
+		return PTR_ERR(m3_ipc->mbox);
+	}
+
+	if (of_property_read_u32(dev->of_node, "ti,rproc", &rproc_phandle)) {
+		dev_err(&pdev->dev, "could not get rproc phandle\n");
+		ret = -ENODEV;
+		goto err_free_mbox;
+	}
+
+	m3_rproc = rproc_get_by_phandle(rproc_phandle);
+	if (!m3_rproc) {
+		dev_err(&pdev->dev, "could not get rproc handle\n");
+		ret = -EPROBE_DEFER;
+		goto err_free_mbox;
+	}
+
+	m3_ipc->rproc = m3_rproc;
+	m3_ipc->dev = dev;
+	m3_ipc->state = M3_STATE_RESET;
+
+	m3_ipc->ops = &ipc_ops;
+
+	/*
+	 * Wait for firmware loading completion in a thread so we
+	 * can boot the wkup_m3 as soon as it's ready without holding
+	 * up kernel boot
+	 */
+	task = kthread_run((void *)wkup_m3_rproc_boot_thread, m3_ipc,
+			   "wkup_m3_rproc_loader");
+
+	if (IS_ERR(task)) {
+		dev_err(dev, "can't create rproc_boot thread\n");
+		goto err_put_rproc;
+	}
+
+	m3_ipc_state = m3_ipc;
+
+	return 0;
+
+err_put_rproc:
+	rproc_put(m3_rproc);
+err_free_mbox:
+	mbox_free_channel(m3_ipc->mbox);
+	return ret;
+}
+
+static int wkup_m3_ipc_remove(struct platform_device *pdev)
+{
+	mbox_free_channel(m3_ipc_state->mbox);
+
+	rproc_shutdown(m3_ipc_state->rproc);
+	rproc_put(m3_ipc_state->rproc);
+
+	m3_ipc_state = NULL;
+
+	return 0;
+}
+
+static const struct of_device_id wkup_m3_ipc_of_match[] = {
+	{ .compatible = "ti,am3352-wkup-m3-ipc", },
+	{ .compatible = "ti,am4372-wkup-m3-ipc", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, wkup_m3_ipc_of_match);
+
+static struct platform_driver wkup_m3_ipc_driver = {
+	.probe = wkup_m3_ipc_probe,
+	.remove = wkup_m3_ipc_remove,
+	.driver = {
+		.name = "wkup_m3_ipc",
+		.of_match_table = wkup_m3_ipc_of_match,
+	},
+};
+
+module_platform_driver(wkup_m3_ipc_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("wkup m3 remote processor ipc driver");
+MODULE_AUTHOR("Dave Gerlach <d-gerlach@ti.com>");
diff --git a/drivers/soc/versatile/soc-realview.c b/drivers/soc/versatile/soc-realview.c
index e642c4540dda..c337764de867 100644
--- a/drivers/soc/versatile/soc-realview.c
+++ b/drivers/soc/versatile/soc-realview.c
@@ -36,6 +36,8 @@ static const char *realview_board_str(u32 id)
 	switch ((id >> 16) & 0xfff) {
 	case 0x0147:
 		return "HBI-0147";
+	case 0x0159:
+		return "HBI-0159";
 	default:
 		return "Unknown";
 	}
@@ -44,6 +46,8 @@ static const char *realview_board_str(u32 id)
 static const char *realview_arch_str(u32 id)
 {
 	switch ((id >> 8) & 0xf) {
+	case 0x04:
+		return "AHB";
 	case 0x05:
 		return "Multi-layer AXI";
 	default:
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 8b9c2a38d1cc..77064160dd76 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -585,7 +585,7 @@ config SPI_TEGRA20_SLINK
 
 config SPI_TOPCLIFF_PCH
 	tristate "Intel EG20T PCH/LAPIS Semicon IOH(ML7213/ML7223/ML7831) SPI"
-	depends on PCI && (X86_32 || COMPILE_TEST)
+	depends on PCI && (X86_32 || MIPS || COMPILE_TEST)
 	help
 	  SPI driver for the Topcliff PCH (Platform Controller Hub) SPI bus
 	  used in some x86 embedded processors.
@@ -689,6 +689,15 @@ config SPI_SPIDEV
 	  Note that this application programming interface is EXPERIMENTAL
 	  and hence SUBJECT TO CHANGE WITHOUT NOTICE while it stabilizes.
 
+config SPI_LOOPBACK_TEST
+	tristate "spi loopback test framework support"
+	depends on m
+	help
+	  This enables the SPI loopback testing framework driver
+
+	  primarily used for development of spi_master drivers
+	  and to detect regressions
+
 config SPI_TLE62X0
 	tristate "Infineon TLE62X0 (for power switching)"
 	depends on SYSFS
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 31fb7fb2a0b6..8991ffce6e12 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -8,6 +8,7 @@ ccflags-$(CONFIG_SPI_DEBUG) := -DDEBUG
 # config declarations into driver model code
 obj-$(CONFIG_SPI_MASTER)		+= spi.o
 obj-$(CONFIG_SPI_SPIDEV)		+= spidev.o
+obj-$(CONFIG_SPI_LOOPBACK_TEST)		+= spi-loopback-test.o
 
 # SPI master controller drivers (bus)
 obj-$(CONFIG_SPI_ALTERA)		+= spi-altera.o
diff --git a/drivers/spi/spi-atmel.c b/drivers/spi/spi-atmel.c
index aebad36391c9..8feac599e9ab 100644
--- a/drivers/spi/spi-atmel.c
+++ b/drivers/spi/spi-atmel.c
@@ -1571,6 +1571,7 @@ static int atmel_spi_probe(struct platform_device *pdev)
 
 	as->use_cs_gpios = true;
 	if (atmel_spi_is_v2(as) &&
+	    pdev->dev.of_node &&
 	    !of_get_property(pdev->dev.of_node, "cs-gpios", NULL)) {
 		as->use_cs_gpios = false;
 		master->num_chipselect = 4;
diff --git a/drivers/spi/spi-bcm2835aux.c b/drivers/spi/spi-bcm2835aux.c
index 7de6f8472a81..ecc73c0a97cf 100644
--- a/drivers/spi/spi-bcm2835aux.c
+++ b/drivers/spi/spi-bcm2835aux.c
@@ -73,8 +73,8 @@
 
 /* Bitfields in CNTL1 */
 #define BCM2835_AUX_SPI_CNTL1_CSHIGH	0x00000700
-#define BCM2835_AUX_SPI_CNTL1_IDLE	0x00000080
-#define BCM2835_AUX_SPI_CNTL1_TXEMPTY	0x00000040
+#define BCM2835_AUX_SPI_CNTL1_TXEMPTY	0x00000080
+#define BCM2835_AUX_SPI_CNTL1_IDLE	0x00000040
 #define BCM2835_AUX_SPI_CNTL1_MSBF_IN	0x00000002
 #define BCM2835_AUX_SPI_CNTL1_KEEP_IN	0x00000001
 
diff --git a/drivers/spi/spi-bcm63xx.c b/drivers/spi/spi-bcm63xx.c
index bf9a610e5b89..fee747030ee6 100644
--- a/drivers/spi/spi-bcm63xx.c
+++ b/drivers/spi/spi-bcm63xx.c
@@ -207,6 +207,9 @@ static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 	u8 clk_cfg, reg;
 	int i;
 
+	/* Default to lowest clock configuration */
+	clk_cfg = SPI_CLK_0_391MHZ;
+
 	/* Find the closest clock configuration */
 	for (i = 0; i < SPI_CLK_MASK; i++) {
 		if (t->speed_hz >= bcm63xx_spi_freq_table[i][0]) {
@@ -215,10 +218,6 @@ static void bcm63xx_spi_setup_transfer(struct spi_device *spi,
 		}
 	}
 
-	/* No matching configuration found, default to lowest */
-	if (i == SPI_CLK_MASK)
-		clk_cfg = SPI_CLK_0_391MHZ;
-
 	/* clear existing clock configuration bits of the register */
 	reg = bcm_spi_readb(bs, SPI_CLK_CFG);
 	reg &= ~SPI_CLK_MASK;
diff --git a/drivers/spi/spi-butterfly.c b/drivers/spi/spi-butterfly.c
index 9a95862986c8..22a31e4a1a11 100644
--- a/drivers/spi/spi-butterfly.c
+++ b/drivers/spi/spi-butterfly.c
@@ -27,7 +27,6 @@
 
 #include <linux/mtd/partitions.h>
 
-
 /*
  * This uses SPI to talk with an "AVR Butterfly", which is a $US20 card
  * with a battery powered AVR microcontroller and lots of goodies.  You
@@ -37,7 +36,6 @@
  * and use this custom parallel port cable.
  */
 
-
 /* DATA output bits (pins 2..9 == D0..D7) */
 #define	butterfly_nreset (1 << 1)		/* pin 3 */
 
@@ -52,14 +50,11 @@
 /* CONTROL output bits */
 #define	spi_cs_bit	PARPORT_CONTROL_SELECT	/* pin 17 */
 
-
-
 static inline struct butterfly *spidev_to_pp(struct spi_device *spi)
 {
 	return spi->controller_data;
 }
 
-
 struct butterfly {
 	/* REVISIT ... for now, this must be first */
 	struct spi_bitbang	bitbang;
@@ -140,7 +135,6 @@ static void butterfly_chipselect(struct spi_device *spi, int value)
 	parport_frob_control(pp->port, spi_cs_bit, value ? spi_cs_bit : 0);
 }
 
-
 /* we only needed to implement one mode here, and choose SPI_MODE_0 */
 
 #define spidelay(X)	do { } while (0)
@@ -149,9 +143,8 @@ static void butterfly_chipselect(struct spi_device *spi, int value)
 #include "spi-bitbang-txrx.h"
 
 static u32
-butterfly_txrx_word_mode0(struct spi_device *spi,
-		unsigned nsecs,
-		u32 word, u8 bits)
+butterfly_txrx_word_mode0(struct spi_device *spi, unsigned nsecs, u32 word,
+			  u8 bits)
 {
 	return bitbang_txrx_be_cpha0(spi, nsecs, 0, 0, word, bits);
 }
@@ -186,7 +179,6 @@ static struct flash_platform_data flash = {
 	.nr_parts	= ARRAY_SIZE(partitions),
 };
 
-
 /* REVISIT remove this ugly global and its "only one" limitation */
 static struct butterfly *butterfly;
 
@@ -197,6 +189,7 @@ static void butterfly_attach(struct parport *p)
 	struct butterfly	*pp;
 	struct spi_master	*master;
 	struct device		*dev = p->physport->dev;
+	struct pardev_cb	butterfly_cb;
 
 	if (butterfly || !dev)
 		return;
@@ -229,9 +222,9 @@ static void butterfly_attach(struct parport *p)
 	 * parport hookup
 	 */
 	pp->port = p;
-	pd = parport_register_device(p, "spi_butterfly",
-			NULL, NULL, NULL,
-			0 /* FLAGS */, pp);
+	memset(&butterfly_cb, 0, sizeof(butterfly_cb));
+	butterfly_cb.private = pp;
+	pd = parport_register_dev_model(p, "spi_butterfly", &butterfly_cb, 0);
 	if (!pd) {
 		status = -ENOMEM;
 		goto clean0;
@@ -262,7 +255,6 @@ static void butterfly_attach(struct parport *p)
 	parport_write_data(pp->port, pp->lastbyte);
 	msleep(100);
 
-
 	/*
 	 * Start SPI ... for now, hide that we're two physical busses.
 	 */
@@ -283,7 +275,7 @@ static void butterfly_attach(struct parport *p)
 	pp->dataflash = spi_new_device(pp->bitbang.master, &pp->info[0]);
 	if (pp->dataflash)
 		pr_debug("%s: dataflash at %s\n", p->name,
-				dev_name(&pp->dataflash->dev));
+			 dev_name(&pp->dataflash->dev));
 
 	pr_info("%s: AVR Butterfly\n", p->name);
 	butterfly = pp;
@@ -297,7 +289,7 @@ clean2:
 clean1:
 	parport_unregister_device(pd);
 clean0:
-	(void) spi_master_put(pp->bitbang.master);
+	spi_master_put(pp->bitbang.master);
 done:
 	pr_debug("%s: butterfly probe, fail %d\n", p->name, status);
 }
@@ -325,16 +317,16 @@ static void butterfly_detach(struct parport *p)
 	parport_release(pp->pd);
 	parport_unregister_device(pp->pd);
 
-	(void) spi_master_put(pp->bitbang.master);
+	spi_master_put(pp->bitbang.master);
 }
 
 static struct parport_driver butterfly_driver = {
 	.name =		"spi_butterfly",
-	.attach =	butterfly_attach,
+	.match_port =	butterfly_attach,
 	.detach =	butterfly_detach,
+	.devmodel = true,
 };
 
-
 static int __init butterfly_init(void)
 {
 	return parport_register_driver(&butterfly_driver);
diff --git a/drivers/spi/spi-cadence.c b/drivers/spi/spi-cadence.c
index 5a6749881ff9..121a4135b540 100644
--- a/drivers/spi/spi-cadence.c
+++ b/drivers/spi/spi-cadence.c
@@ -617,8 +617,7 @@ static int cdns_spi_remove(struct platform_device *pdev)
  */
 static int __maybe_unused cdns_spi_suspend(struct device *dev)
 {
-	struct platform_device *pdev = container_of(dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spi_master *master = platform_get_drvdata(pdev);
 	struct cdns_spi *xspi = spi_master_get_devdata(master);
 
@@ -641,8 +640,7 @@ static int __maybe_unused cdns_spi_suspend(struct device *dev)
  */
 static int __maybe_unused cdns_spi_resume(struct device *dev)
 {
-	struct platform_device *pdev = container_of(dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spi_master *master = platform_get_drvdata(pdev);
 	struct cdns_spi *xspi = spi_master_get_devdata(master);
 	int ret = 0;
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 7d3af3eacf57..fddb7a3be322 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -477,33 +477,33 @@ static int davinci_spi_check_error(struct davinci_spi *dspi, int int_status)
 	struct device *sdev = dspi->bitbang.master->dev.parent;
 
 	if (int_status & SPIFLG_TIMEOUT_MASK) {
-		dev_dbg(sdev, "SPI Time-out Error\n");
+		dev_err(sdev, "SPI Time-out Error\n");
 		return -ETIMEDOUT;
 	}
 	if (int_status & SPIFLG_DESYNC_MASK) {
-		dev_dbg(sdev, "SPI Desynchronization Error\n");
+		dev_err(sdev, "SPI Desynchronization Error\n");
 		return -EIO;
 	}
 	if (int_status & SPIFLG_BITERR_MASK) {
-		dev_dbg(sdev, "SPI Bit error\n");
+		dev_err(sdev, "SPI Bit error\n");
 		return -EIO;
 	}
 
 	if (dspi->version == SPI_VERSION_2) {
 		if (int_status & SPIFLG_DLEN_ERR_MASK) {
-			dev_dbg(sdev, "SPI Data Length Error\n");
+			dev_err(sdev, "SPI Data Length Error\n");
 			return -EIO;
 		}
 		if (int_status & SPIFLG_PARERR_MASK) {
-			dev_dbg(sdev, "SPI Parity Error\n");
+			dev_err(sdev, "SPI Parity Error\n");
 			return -EIO;
 		}
 		if (int_status & SPIFLG_OVRRUN_MASK) {
-			dev_dbg(sdev, "SPI Data Overrun error\n");
+			dev_err(sdev, "SPI Data Overrun error\n");
 			return -EIO;
 		}
 		if (int_status & SPIFLG_BUF_INIT_ACTIVE_MASK) {
-			dev_dbg(sdev, "SPI Buffer Init Active\n");
+			dev_err(sdev, "SPI Buffer Init Active\n");
 			return -EBUSY;
 		}
 	}
@@ -703,7 +703,8 @@ static int davinci_spi_bufs(struct spi_device *spi, struct spi_transfer *t)
 
 	/* Wait for the transfer to complete */
 	if (spicfg->io_type != SPI_IO_TYPE_POLL) {
-		wait_for_completion_interruptible(&(dspi->done));
+		if (wait_for_completion_timeout(&dspi->done, HZ) == 0)
+			errors = SPIFLG_TIMEOUT_MASK;
 	} else {
 		while (dspi->rcount > 0 || dspi->wcount > 0) {
 			errors = davinci_spi_process_events(dspi);
diff --git a/drivers/spi/spi-dw-mid.c b/drivers/spi/spi-dw-mid.c
index bb1052e748f2..9185f6c08459 100644
--- a/drivers/spi/spi-dw-mid.c
+++ b/drivers/spi/spi-dw-mid.c
@@ -283,7 +283,7 @@ static void mid_spi_dma_stop(struct dw_spi *dws)
 	}
 }
 
-static struct dw_spi_dma_ops mid_dma_ops = {
+static const struct dw_spi_dma_ops mid_dma_ops = {
 	.dma_init	= mid_spi_dma_init,
 	.dma_exit	= mid_spi_dma_exit,
 	.dma_setup	= mid_spi_dma_setup,
diff --git a/drivers/spi/spi-dw.c b/drivers/spi/spi-dw.c
index 882cd6618cd5..c09bb745693a 100644
--- a/drivers/spi/spi-dw.c
+++ b/drivers/spi/spi-dw.c
@@ -425,7 +425,7 @@ static int dw_spi_setup(struct spi_device *spi)
 		chip->type = chip_info->type;
 	}
 
-	chip->tmode = 0; /* Tx & Rx */
+	chip->tmode = SPI_TMOD_TR;
 
 	if (gpio_is_valid(spi->cs_gpio)) {
 		ret = gpio_direction_output(spi->cs_gpio,
diff --git a/drivers/spi/spi-dw.h b/drivers/spi/spi-dw.h
index 35589a270468..61bc3cbab38d 100644
--- a/drivers/spi/spi-dw.h
+++ b/drivers/spi/spi-dw.h
@@ -130,7 +130,7 @@ struct dw_spi {
 	struct dma_chan		*rxchan;
 	unsigned long		dma_chan_busy;
 	dma_addr_t		dma_addr; /* phy address of the Data register */
-	struct dw_spi_dma_ops	*dma_ops;
+	const struct dw_spi_dma_ops *dma_ops;
 	void			*dma_tx;
 	void			*dma_rx;
 
diff --git a/drivers/spi/spi-fsl-cpm.c b/drivers/spi/spi-fsl-cpm.c
index 896add8cfd3b..8f7b26ec181e 100644
--- a/drivers/spi/spi-fsl-cpm.c
+++ b/drivers/spi/spi-fsl-cpm.c
@@ -16,7 +16,7 @@
  * option) any later version.
  */
 #include <asm/cpm.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/qe.h>
 #include <linux/dma-mapping.h>
 #include <linux/fsl_devices.h>
 #include <linux/kernel.h>
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index c27124a5ec8e..7cb0c1921495 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -84,7 +84,7 @@ struct fsl_espi_transfer {
 /* SPCOM register values */
 #define SPCOM_CS(x)		((x) << 30)
 #define SPCOM_TRANLEN(x)	((x) << 0)
-#define	SPCOM_TRANLEN_MAX	0xFFFF	/* Max transaction length */
+#define	SPCOM_TRANLEN_MAX	0x10000	/* Max transaction length */
 
 #define AUTOSUSPEND_TIMEOUT 2000
 
@@ -233,7 +233,7 @@ static int fsl_espi_bufs(struct spi_device *spi, struct spi_transfer *t)
 	reinit_completion(&mpc8xxx_spi->done);
 
 	/* Set SPCOM[CS] and SPCOM[TRANLEN] field */
-	if ((t->len - 1) > SPCOM_TRANLEN_MAX) {
+	if (t->len > SPCOM_TRANLEN_MAX) {
 		dev_err(mpc8xxx_spi->dev, "Transaction length (%d)"
 				" beyond the SPCOM[TRANLEN] field\n", t->len);
 		return -EINVAL;
@@ -643,6 +643,11 @@ static int fsl_espi_runtime_resume(struct device *dev)
 }
 #endif
 
+static size_t fsl_espi_max_transfer_size(struct spi_device *spi)
+{
+	return SPCOM_TRANLEN_MAX;
+}
+
 static struct spi_master * fsl_espi_probe(struct device *dev,
 		struct resource *mem, unsigned int irq)
 {
@@ -670,6 +675,7 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 	master->cleanup = fsl_espi_cleanup;
 	master->transfer_one_message = fsl_espi_do_one_msg;
 	master->auto_runtime_pm = true;
+	master->max_transfer_size = fsl_espi_max_transfer_size;
 
 	mpc8xxx_spi = spi_master_get_devdata(master);
 
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 0e5723ab47f0..c688efa95e29 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -104,9 +104,7 @@ struct spi_imx_data {
 	unsigned int dma_is_inited;
 	unsigned int dma_finished;
 	bool usedma;
-	u32 rx_wml;
-	u32 tx_wml;
-	u32 rxt_wml;
+	u32 wml;
 	struct completion dma_rx_completion;
 	struct completion dma_tx_completion;
 
@@ -124,9 +122,14 @@ static inline int is_imx35_cspi(struct spi_imx_data *d)
 	return d->devtype_data->devtype == IMX35_CSPI;
 }
 
+static inline int is_imx51_ecspi(struct spi_imx_data *d)
+{
+	return d->devtype_data->devtype == IMX51_ECSPI;
+}
+
 static inline unsigned spi_imx_get_fifosize(struct spi_imx_data *d)
 {
-	return (d->devtype_data->devtype == IMX51_ECSPI) ? 64 : 8;
+	return is_imx51_ecspi(d) ? 64 : 8;
 }
 
 #define MXC_SPI_BUF_RX(type)						\
@@ -201,9 +204,8 @@ static bool spi_imx_can_dma(struct spi_master *master, struct spi_device *spi,
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(master);
 
-	if (spi_imx->dma_is_inited
-	    && transfer->len > spi_imx->rx_wml * sizeof(u32)
-	    && transfer->len > spi_imx->tx_wml * sizeof(u32))
+	if (spi_imx->dma_is_inited && transfer->len >= spi_imx->wml &&
+	    (transfer->len % spi_imx->wml) == 0)
 		return true;
 	return false;
 }
@@ -244,6 +246,9 @@ static bool spi_imx_can_dma(struct spi_master *master, struct spi_device *spi,
 #define MX51_ECSPI_STAT		0x18
 #define MX51_ECSPI_STAT_RR		(1 <<  3)
 
+#define MX51_ECSPI_TESTREG	0x20
+#define MX51_ECSPI_TESTREG_LBC	BIT(31)
+
 /* MX51 eCSPI */
 static unsigned int mx51_ecspi_clkdiv(unsigned int fin, unsigned int fspi,
 				      unsigned int *fres)
@@ -313,7 +318,7 @@ static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 {
 	u32 ctrl = MX51_ECSPI_CTRL_ENABLE, cfg = 0, dma = 0;
 	u32 tx_wml_cfg, rx_wml_cfg, rxt_wml_cfg;
-	u32 clk = config->speed_hz, delay;
+	u32 clk = config->speed_hz, delay, reg;
 
 	/*
 	 * The hardware seems to have a race condition when changing modes. The
@@ -351,7 +356,16 @@ static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 	else
 		cfg &= ~MX51_ECSPI_CONFIG_SSBPOL(config->cs);
 
+	/* CTRL register always go first to bring out controller from reset */
 	writel(ctrl, spi_imx->base + MX51_ECSPI_CTRL);
+
+	reg = readl(spi_imx->base + MX51_ECSPI_TESTREG);
+	if (config->mode & SPI_LOOP)
+		reg |= MX51_ECSPI_TESTREG_LBC;
+	else
+		reg &= ~MX51_ECSPI_TESTREG_LBC;
+	writel(reg, spi_imx->base + MX51_ECSPI_TESTREG);
+
 	writel(cfg, spi_imx->base + MX51_ECSPI_CONFIG);
 
 	/*
@@ -378,10 +392,9 @@ static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 	if (spi_imx->dma_is_inited) {
 		dma = readl(spi_imx->base + MX51_ECSPI_DMA);
 
-		spi_imx->rxt_wml = spi_imx_get_fifosize(spi_imx) / 2;
-		rx_wml_cfg = spi_imx->rx_wml << MX51_ECSPI_DMA_RX_WML_OFFSET;
-		tx_wml_cfg = spi_imx->tx_wml << MX51_ECSPI_DMA_TX_WML_OFFSET;
-		rxt_wml_cfg = spi_imx->rxt_wml << MX51_ECSPI_DMA_RXT_WML_OFFSET;
+		rx_wml_cfg = spi_imx->wml << MX51_ECSPI_DMA_RX_WML_OFFSET;
+		tx_wml_cfg = spi_imx->wml << MX51_ECSPI_DMA_TX_WML_OFFSET;
+		rxt_wml_cfg = spi_imx->wml << MX51_ECSPI_DMA_RXT_WML_OFFSET;
 		dma = (dma & ~MX51_ECSPI_DMA_TX_WML_MASK
 			   & ~MX51_ECSPI_DMA_RX_WML_MASK
 			   & ~MX51_ECSPI_DMA_RXT_WML_MASK)
@@ -832,18 +845,21 @@ static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
 	if (of_machine_is_compatible("fsl,imx6dl"))
 		return 0;
 
+	spi_imx->wml = spi_imx_get_fifosize(spi_imx) / 2;
+
 	/* Prepare for TX DMA: */
-	master->dma_tx = dma_request_slave_channel(dev, "tx");
-	if (!master->dma_tx) {
-		dev_err(dev, "cannot get the TX DMA channel!\n");
-		ret = -EINVAL;
+	master->dma_tx = dma_request_slave_channel_reason(dev, "tx");
+	if (IS_ERR(master->dma_tx)) {
+		ret = PTR_ERR(master->dma_tx);
+		dev_dbg(dev, "can't get the TX DMA channel, error %d!\n", ret);
+		master->dma_tx = NULL;
 		goto err;
 	}
 
 	slave_config.direction = DMA_MEM_TO_DEV;
 	slave_config.dst_addr = res->start + MXC_CSPITXDATA;
 	slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
-	slave_config.dst_maxburst = spi_imx_get_fifosize(spi_imx) / 2;
+	slave_config.dst_maxburst = spi_imx->wml;
 	ret = dmaengine_slave_config(master->dma_tx, &slave_config);
 	if (ret) {
 		dev_err(dev, "error in TX dma configuration.\n");
@@ -851,17 +867,18 @@ static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
 	}
 
 	/* Prepare for RX : */
-	master->dma_rx = dma_request_slave_channel(dev, "rx");
-	if (!master->dma_rx) {
-		dev_dbg(dev, "cannot get the DMA channel.\n");
-		ret = -EINVAL;
+	master->dma_rx = dma_request_slave_channel_reason(dev, "rx");
+	if (IS_ERR(master->dma_rx)) {
+		ret = PTR_ERR(master->dma_rx);
+		dev_dbg(dev, "can't get the RX DMA channel, error %d\n", ret);
+		master->dma_rx = NULL;
 		goto err;
 	}
 
 	slave_config.direction = DMA_DEV_TO_MEM;
 	slave_config.src_addr = res->start + MXC_CSPIRXDATA;
 	slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
-	slave_config.src_maxburst = spi_imx_get_fifosize(spi_imx) / 2;
+	slave_config.src_maxburst = spi_imx->wml;
 	ret = dmaengine_slave_config(master->dma_rx, &slave_config);
 	if (ret) {
 		dev_err(dev, "error in RX dma configuration.\n");
@@ -874,8 +891,6 @@ static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
 	master->max_dma_len = MAX_SDMA_BD_BYTES;
 	spi_imx->bitbang.master->flags = SPI_MASTER_MUST_RX |
 					 SPI_MASTER_MUST_TX;
-	spi_imx->tx_wml = spi_imx_get_fifosize(spi_imx) / 2;
-	spi_imx->rx_wml = spi_imx_get_fifosize(spi_imx) / 2;
 	spi_imx->dma_is_inited = 1;
 
 	return 0;
@@ -904,8 +919,6 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 	struct dma_async_tx_descriptor *desc_tx = NULL, *desc_rx = NULL;
 	int ret;
 	unsigned long timeout;
-	u32 dma;
-	int left;
 	struct spi_master *master = spi_imx->bitbang.master;
 	struct sg_table *tx = &transfer->tx_sg, *rx = &transfer->rx_sg;
 
@@ -914,7 +927,7 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 					tx->sgl, tx->nents, DMA_MEM_TO_DEV,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 		if (!desc_tx)
-			goto no_dma;
+			goto tx_nodma;
 
 		desc_tx->callback = spi_imx_dma_tx_callback;
 		desc_tx->callback_param = (void *)spi_imx;
@@ -926,7 +939,7 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 					rx->sgl, rx->nents, DMA_DEV_TO_MEM,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 		if (!desc_rx)
-			goto no_dma;
+			goto rx_nodma;
 
 		desc_rx->callback = spi_imx_dma_rx_callback;
 		desc_rx->callback_param = (void *)spi_imx;
@@ -939,17 +952,18 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 	/* Trigger the cspi module. */
 	spi_imx->dma_finished = 0;
 
-	dma = readl(spi_imx->base + MX51_ECSPI_DMA);
-	dma = dma & (~MX51_ECSPI_DMA_RXT_WML_MASK);
-	/* Change RX_DMA_LENGTH trigger dma fetch tail data */
-	left = transfer->len % spi_imx->rxt_wml;
-	if (left)
-		writel(dma | (left << MX51_ECSPI_DMA_RXT_WML_OFFSET),
-				spi_imx->base + MX51_ECSPI_DMA);
+	/*
+	 * Set these order to avoid potential RX overflow. The overflow may
+	 * happen if we enable SPI HW before starting RX DMA due to rescheduling
+	 * for another task and/or interrupt.
+	 * So RX DMA enabled first to make sure data would be read out from FIFO
+	 * ASAP. TX DMA enabled next to start filling TX FIFO with new data.
+	 * And finaly SPI HW enabled to start actual data transfer.
+	 */
+	dma_async_issue_pending(master->dma_rx);
+	dma_async_issue_pending(master->dma_tx);
 	spi_imx->devtype_data->trigger(spi_imx);
 
-	dma_async_issue_pending(master->dma_tx);
-	dma_async_issue_pending(master->dma_rx);
 	/* Wait SDMA to finish the data transfer.*/
 	timeout = wait_for_completion_timeout(&spi_imx->dma_tx_completion,
 						IMX_DMA_TIMEOUT);
@@ -958,6 +972,7 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 			dev_driver_string(&master->dev),
 			dev_name(&master->dev));
 		dmaengine_terminate_all(master->dma_tx);
+		dmaengine_terminate_all(master->dma_rx);
 	} else {
 		timeout = wait_for_completion_timeout(
 				&spi_imx->dma_rx_completion, IMX_DMA_TIMEOUT);
@@ -968,9 +983,6 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 			spi_imx->devtype_data->reset(spi_imx);
 			dmaengine_terminate_all(master->dma_rx);
 		}
-		writel(dma |
-		       spi_imx->rxt_wml << MX51_ECSPI_DMA_RXT_WML_OFFSET,
-		       spi_imx->base + MX51_ECSPI_DMA);
 	}
 
 	spi_imx->dma_finished = 1;
@@ -983,7 +995,9 @@ static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
 
 	return ret;
 
-no_dma:
+rx_nodma:
+	dmaengine_terminate_all(master->dma_tx);
+tx_nodma:
 	pr_warn_once("%s %s: DMA not available, falling back to PIO\n",
 		     dev_driver_string(&master->dev),
 		     dev_name(&master->dev));
@@ -1117,6 +1131,9 @@ static int spi_imx_probe(struct platform_device *pdev)
 	spi_imx = spi_master_get_devdata(master);
 	spi_imx->bitbang.master = master;
 
+	spi_imx->devtype_data = of_id ? of_id->data :
+		(struct spi_imx_devtype_data *)pdev->id_entry->driver_data;
+
 	for (i = 0; i < master->num_chipselect; i++) {
 		int cs_gpio = of_get_named_gpio(np, "cs-gpios", i);
 		if (!gpio_is_valid(cs_gpio) && mxc_platform_info)
@@ -1142,12 +1159,11 @@ static int spi_imx_probe(struct platform_device *pdev)
 	spi_imx->bitbang.master->prepare_message = spi_imx_prepare_message;
 	spi_imx->bitbang.master->unprepare_message = spi_imx_unprepare_message;
 	spi_imx->bitbang.master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_CS_HIGH;
+	if (is_imx51_ecspi(spi_imx))
+		spi_imx->bitbang.master->mode_bits |= SPI_LOOP;
 
 	init_completion(&spi_imx->xfer_done);
 
-	spi_imx->devtype_data = of_id ? of_id->data :
-		(struct spi_imx_devtype_data *) pdev->id_entry->driver_data;
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	spi_imx->base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(spi_imx->base)) {
@@ -1193,9 +1209,15 @@ static int spi_imx_probe(struct platform_device *pdev)
 	 * Only validated on i.mx6 now, can remove the constrain if validated on
 	 * other chips.
 	 */
-	if (spi_imx->devtype_data == &imx51_ecspi_devtype_data
-	    && spi_imx_sdma_init(&pdev->dev, spi_imx, master, res))
-		dev_err(&pdev->dev, "dma setup error,use pio instead\n");
+	if (is_imx51_ecspi(spi_imx)) {
+		ret = spi_imx_sdma_init(&pdev->dev, spi_imx, master, res);
+		if (ret == -EPROBE_DEFER)
+			goto out_clk_put;
+
+		if (ret < 0)
+			dev_err(&pdev->dev, "dma setup error %d, use pio\n",
+				ret);
+	}
 
 	spi_imx->devtype_data->reset(spi_imx);
 
diff --git a/drivers/spi/spi-lm70llp.c b/drivers/spi/spi-lm70llp.c
index ba72347cb99d..61ee0f4269ae 100644
--- a/drivers/spi/spi-lm70llp.c
+++ b/drivers/spi/spi-lm70llp.c
@@ -14,6 +14,8 @@
  * GNU General Public License for more details.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -23,11 +25,9 @@
 #include <linux/sysfs.h>
 #include <linux/workqueue.h>
 
-
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 
-
 /*
  * The LM70 communicates with a host processor using a 3-wire variant of
  * the SPI/Microwire bus interface. This driver specifically supports an
@@ -88,7 +88,6 @@ struct spi_lm70llp {
 /* REVISIT : ugly global ; provides "exclusive open" facility */
 static struct spi_lm70llp *lm70llp;
 
-
 /*-------------------------------------------------------------------*/
 
 static inline struct spi_lm70llp *spidev_to_pp(struct spi_device *spi)
@@ -122,12 +121,14 @@ static inline void assertCS(struct spi_lm70llp *pp)
 static inline void clkHigh(struct spi_lm70llp *pp)
 {
 	u8 data = parport_read_data(pp->port);
+
 	parport_write_data(pp->port, data | SCLK);
 }
 
 static inline void clkLow(struct spi_lm70llp *pp)
 {
 	u8 data = parport_read_data(pp->port);
+
 	parport_write_data(pp->port, data & ~SCLK);
 }
 
@@ -166,8 +167,10 @@ static inline void setmosi(struct spi_device *s, int is_on)
 static inline int getmiso(struct spi_device *s)
 {
 	struct spi_lm70llp *pp = spidev_to_pp(s);
-	return ((SIO == (parport_read_status(pp->port) & SIO)) ? 0 : 1 );
+
+	return ((SIO == (parport_read_status(pp->port) & SIO)) ? 0 : 1);
 }
+
 /*--------------------------------------------------------------------*/
 
 #include "spi-bitbang-txrx.h"
@@ -196,11 +199,10 @@ static void spi_lm70llp_attach(struct parport *p)
 	struct spi_lm70llp	*pp;
 	struct spi_master	*master;
 	int			status;
+	struct pardev_cb	lm70llp_cb;
 
 	if (lm70llp) {
-		printk(KERN_WARNING
-			"%s: spi_lm70llp instance already loaded. Aborting.\n",
-			DRVNAME);
+		pr_warn("spi_lm70llp instance already loaded. Aborting.\n");
 		return;
 	}
 
@@ -227,9 +229,11 @@ static void spi_lm70llp_attach(struct parport *p)
 	 * Parport hookup
 	 */
 	pp->port = p;
-	pd = parport_register_device(p, DRVNAME,
-			NULL, NULL, NULL,
-			PARPORT_FLAG_EXCL, pp);
+	memset(&lm70llp_cb, 0, sizeof(lm70llp_cb));
+	lm70llp_cb.private = pp;
+	lm70llp_cb.flags = PARPORT_FLAG_EXCL;
+	pd = parport_register_dev_model(p, DRVNAME, &lm70llp_cb, 0);
+
 	if (!pd) {
 		status = -ENOMEM;
 		goto out_free_master;
@@ -245,9 +249,8 @@ static void spi_lm70llp_attach(struct parport *p)
 	 */
 	status = spi_bitbang_start(&pp->bitbang);
 	if (status < 0) {
-		printk(KERN_WARNING
-			"%s: spi_bitbang_start failed with status %d\n",
-			DRVNAME, status);
+		dev_warn(&pd->dev, "spi_bitbang_start failed with status %d\n",
+			 status);
 		goto out_off_and_release;
 	}
 
@@ -272,9 +275,9 @@ static void spi_lm70llp_attach(struct parport *p)
 	pp->spidev_lm70 = spi_new_device(pp->bitbang.master, &pp->info);
 	if (pp->spidev_lm70)
 		dev_dbg(&pp->spidev_lm70->dev, "spidev_lm70 at %s\n",
-				dev_name(&pp->spidev_lm70->dev));
+			dev_name(&pp->spidev_lm70->dev));
 	else {
-		printk(KERN_WARNING "%s: spi_new_device failed\n", DRVNAME);
+		dev_warn(&pd->dev, "spi_new_device failed\n");
 		status = -ENODEV;
 		goto out_bitbang_stop;
 	}
@@ -293,9 +296,9 @@ out_off_and_release:
 out_parport_unreg:
 	parport_unregister_device(pd);
 out_free_master:
-	(void) spi_master_put(master);
+	spi_master_put(master);
 out_fail:
-	pr_info("%s: spi_lm70llp probe fail, status %d\n", DRVNAME, status);
+	pr_info("spi_lm70llp probe fail, status %d\n", status);
 }
 
 static void spi_lm70llp_detach(struct parport *p)
@@ -314,16 +317,16 @@ static void spi_lm70llp_detach(struct parport *p)
 	parport_release(pp->pd);
 	parport_unregister_device(pp->pd);
 
-	(void) spi_master_put(pp->bitbang.master);
+	spi_master_put(pp->bitbang.master);
 
 	lm70llp = NULL;
 }
 
-
 static struct parport_driver spi_lm70llp_drv = {
 	.name =		DRVNAME,
-	.attach =	spi_lm70llp_attach,
+	.match_port =	spi_lm70llp_attach,
 	.detach =	spi_lm70llp_detach,
+	.devmodel =	true,
 };
 
 static int __init init_spi_lm70llp(void)
diff --git a/drivers/spi/spi-loopback-test.c b/drivers/spi/spi-loopback-test.c
new file mode 100644
index 000000000000..cf4bb36bee25
--- /dev/null
+++ b/drivers/spi/spi-loopback-test.c
@@ -0,0 +1,1006 @@
+/*
+ *  linux/drivers/spi/spi-loopback-test.c
+ *
+ *  (c) Martin Sperl <kernel@martin.sperl.org>
+ *
+ *  Loopback test driver to test several typical spi_message conditions
+ *  that a spi_master driver may encounter
+ *  this can also get used for regression testing
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/delay.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/list_sort.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/printk.h>
+#include <linux/spi/spi.h>
+
+#include "spi-test.h"
+
+/* flag to only simulate transfers */
+int simulate_only;
+module_param(simulate_only, int, 0);
+MODULE_PARM_DESC(simulate_only, "if not 0 do not execute the spi message");
+
+/* dump spi messages */
+int dump_messages;
+module_param(dump_messages, int, 0);
+MODULE_PARM_DESC(dump_messages,
+		 "=1 dump the basic spi_message_structure, " \
+		 "=2 dump the spi_message_structure including data, " \
+		 "=3 dump the spi_message structure before and after execution");
+/* the device is jumpered for loopback - enabling some rx_buf tests */
+int loopback;
+module_param(loopback, int, 0);
+MODULE_PARM_DESC(loopback,
+		 "if set enable loopback mode, where the rx_buf "	\
+		 "is checked to match tx_buf after the spi_message "	\
+		 "is executed");
+
+/* run only a specific test */
+int run_only_test = -1;
+module_param(run_only_test, int, 0);
+MODULE_PARM_DESC(run_only_test,
+		 "only run the test with this number (0-based !)");
+
+/* the actual tests to execute */
+static struct spi_test spi_tests[] = {
+	{
+		.description	= "tx/rx-transfer - start of page",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_rx_align = ITERATE_ALIGN,
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "tx/rx-transfer - crossing PAGE_SIZE",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_rx_align = ITERATE_ALIGN,
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(PAGE_SIZE - 4),
+				.rx_buf = RX(PAGE_SIZE - 4),
+			},
+		},
+	},
+	{
+		.description	= "tx-transfer - only",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+		},
+	},
+	{
+		.description	= "rx-transfer - only",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_rx_align = ITERATE_ALIGN,
+		.transfers		= {
+			{
+				.len = 1,
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "two tx-transfers - alter both",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0) | BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+			{
+				.len = 1,
+				/* this is why we cant use ITERATE_MAX_LEN */
+				.tx_buf = TX(SPI_TEST_MAX_SIZE_HALF),
+			},
+		},
+	},
+	{
+		.description	= "two tx-transfers - alter first",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(64),
+			},
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+		},
+	},
+	{
+		.description	= "two tx-transfers - alter second",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0),
+		.transfers		= {
+			{
+				.len = 16,
+				.tx_buf = TX(0),
+			},
+			{
+				.len = 1,
+				.tx_buf = TX(64),
+			},
+		},
+	},
+	{
+		.description	= "two transfers tx then rx - alter both",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0) | BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+			{
+				.len = 1,
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "two transfers tx then rx - alter tx",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+			{
+				.len = 1,
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "two transfers tx then rx - alter rx",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+			},
+			{
+				.len = 1,
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "two tx+rx transfers - alter both",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0) | BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+				.rx_buf = RX(0),
+			},
+			{
+				.len = 1,
+				/* making sure we align without overwrite
+				 * the reason we can not use ITERATE_MAX_LEN
+				 */
+				.tx_buf = TX(SPI_TEST_MAX_SIZE_HALF),
+				.rx_buf = RX(SPI_TEST_MAX_SIZE_HALF),
+			},
+		},
+	},
+	{
+		.description	= "two tx+rx transfers - alter first",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(0),
+		.transfers		= {
+			{
+				.len = 1,
+				/* making sure we align without overwrite */
+				.tx_buf = TX(1024),
+				.rx_buf = RX(1024),
+			},
+			{
+				.len = 1,
+				/* making sure we align without overwrite */
+				.tx_buf = TX(0),
+				.rx_buf = RX(0),
+			},
+		},
+	},
+	{
+		.description	= "two tx+rx transfers - alter second",
+		.fill_option	= FILL_COUNT_8,
+		.iterate_len    = { ITERATE_MAX_LEN },
+		.iterate_tx_align = ITERATE_ALIGN,
+		.iterate_transfer_mask = BIT(1),
+		.transfers		= {
+			{
+				.len = 1,
+				.tx_buf = TX(0),
+				.rx_buf = RX(0),
+			},
+			{
+				.len = 1,
+				/* making sure we align without overwrite */
+				.tx_buf = TX(1024),
+				.rx_buf = RX(1024),
+			},
+		},
+	},
+
+	{ /* end of tests sequence */ }
+};
+
+static int spi_loopback_test_probe(struct spi_device *spi)
+{
+	int ret;
+
+	dev_info(&spi->dev, "Executing spi-loopback-tests\n");
+
+	ret = spi_test_run_tests(spi, spi_tests);
+
+	dev_info(&spi->dev, "Finished spi-loopback-tests with return: %i\n",
+		 ret);
+
+	return ret;
+}
+
+/* non const match table to permit to change via a module parameter */
+static struct of_device_id spi_loopback_test_of_match[] = {
+	{ .compatible	= "linux,spi-loopback-test", },
+	{ }
+};
+
+/* allow to override the compatible string via a module_parameter */
+module_param_string(compatible, spi_loopback_test_of_match[0].compatible,
+		    sizeof(spi_loopback_test_of_match[0].compatible),
+		    0000);
+
+MODULE_DEVICE_TABLE(of, spi_loopback_test_of_match);
+
+static struct spi_driver spi_loopback_test_driver = {
+	.driver = {
+		.name = "spi-loopback-test",
+		.owner = THIS_MODULE,
+		.of_match_table = spi_loopback_test_of_match,
+	},
+	.probe = spi_loopback_test_probe,
+};
+
+module_spi_driver(spi_loopback_test_driver);
+
+MODULE_AUTHOR("Martin Sperl <kernel@martin.sperl.org>");
+MODULE_DESCRIPTION("test spi_driver to check core functionality");
+MODULE_LICENSE("GPL");
+
+/*-------------------------------------------------------------------------*/
+
+/* spi_test implementation */
+
+#define RANGE_CHECK(ptr, plen, start, slen) \
+	((ptr >= start) && (ptr + plen <= start + slen))
+
+/* we allocate one page more, to allow for offsets */
+#define SPI_TEST_MAX_SIZE_PLUS (SPI_TEST_MAX_SIZE + PAGE_SIZE)
+
+static void spi_test_print_hex_dump(char *pre, const void *ptr, size_t len)
+{
+	/* limit the hex_dump */
+	if (len < 1024) {
+		print_hex_dump(KERN_INFO, pre,
+			       DUMP_PREFIX_OFFSET, 16, 1,
+			       ptr, len, 0);
+		return;
+	}
+	/* print head */
+	print_hex_dump(KERN_INFO, pre,
+		       DUMP_PREFIX_OFFSET, 16, 1,
+		       ptr, 512, 0);
+	/* print tail */
+	pr_info("%s truncated - continuing at offset %04zx\n",
+		pre, len - 512);
+	print_hex_dump(KERN_INFO, pre,
+		       DUMP_PREFIX_OFFSET, 16, 1,
+		       ptr + (len - 512), 512, 0);
+}
+
+static void spi_test_dump_message(struct spi_device *spi,
+				  struct spi_message *msg,
+				  bool dump_data)
+{
+	struct spi_transfer *xfer;
+	int i;
+	u8 b;
+
+	dev_info(&spi->dev, "  spi_msg@%pK\n", msg);
+	if (msg->status)
+		dev_info(&spi->dev, "    status:        %i\n",
+			 msg->status);
+	dev_info(&spi->dev, "    frame_length:  %i\n",
+		 msg->frame_length);
+	dev_info(&spi->dev, "    actual_length: %i\n",
+		 msg->actual_length);
+
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		dev_info(&spi->dev, "    spi_transfer@%pK\n", xfer);
+		dev_info(&spi->dev, "      len:    %i\n", xfer->len);
+		dev_info(&spi->dev, "      tx_buf: %pK\n", xfer->tx_buf);
+		if (dump_data && xfer->tx_buf)
+			spi_test_print_hex_dump("          TX: ",
+						xfer->tx_buf,
+						xfer->len);
+
+		dev_info(&spi->dev, "      rx_buf: %pK\n", xfer->rx_buf);
+		if (dump_data && xfer->rx_buf)
+			spi_test_print_hex_dump("          RX: ",
+						xfer->rx_buf,
+						xfer->len);
+		/* check for unwritten test pattern on rx_buf */
+		if (xfer->rx_buf) {
+			for (i = 0 ; i < xfer->len ; i++) {
+				b = ((u8 *)xfer->rx_buf)[xfer->len - 1 - i];
+				if (b != SPI_TEST_PATTERN_UNWRITTEN)
+					break;
+			}
+			if (i)
+				dev_info(&spi->dev,
+					 "      rx_buf filled with %02x starts at offset: %i\n",
+					 SPI_TEST_PATTERN_UNWRITTEN,
+					 xfer->len - i);
+		}
+	}
+}
+
+struct rx_ranges {
+	struct list_head list;
+	u8 *start;
+	u8 *end;
+};
+
+int rx_ranges_cmp(void *priv, struct list_head *a, struct list_head *b)
+{
+	struct rx_ranges *rx_a = list_entry(a, struct rx_ranges, list);
+	struct rx_ranges *rx_b = list_entry(b, struct rx_ranges, list);
+
+	if (rx_a->start > rx_b->start)
+		return 1;
+	if (rx_a->start < rx_b->start)
+		return -1;
+	return 0;
+}
+
+static int spi_check_rx_ranges(struct spi_device *spi,
+			       struct spi_message *msg,
+			       void *rx)
+{
+	struct spi_transfer *xfer;
+	struct rx_ranges ranges[SPI_TEST_MAX_TRANSFERS], *r;
+	int i = 0;
+	LIST_HEAD(ranges_list);
+	u8 *addr;
+	int ret = 0;
+
+	/* loop over all transfers to fill in the rx_ranges */
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		/* if there is no rx, then no check is needed */
+		if (!xfer->rx_buf)
+			continue;
+		/* fill in the rx_range */
+		if (RANGE_CHECK(xfer->rx_buf, xfer->len,
+				rx, SPI_TEST_MAX_SIZE_PLUS)) {
+			ranges[i].start = xfer->rx_buf;
+			ranges[i].end = xfer->rx_buf + xfer->len;
+			list_add(&ranges[i].list, &ranges_list);
+			i++;
+		}
+	}
+
+	/* if no ranges, then we can return and avoid the checks...*/
+	if (!i)
+		return 0;
+
+	/* sort the list */
+	list_sort(NULL, &ranges_list, rx_ranges_cmp);
+
+	/* and iterate over all the rx addresses */
+	for (addr = rx; addr < (u8 *)rx + SPI_TEST_MAX_SIZE_PLUS; addr++) {
+		/* if we are the DO not write pattern,
+		 * then continue with the loop...
+		 */
+		if (*addr == SPI_TEST_PATTERN_DO_NOT_WRITE)
+			continue;
+
+		/* check if we are inside a range */
+		list_for_each_entry(r, &ranges_list, list) {
+			/* if so then set to end... */
+			if ((addr >= r->start) && (addr < r->end))
+				addr = r->end;
+		}
+		/* second test after a (hopefull) translation */
+		if (*addr == SPI_TEST_PATTERN_DO_NOT_WRITE)
+			continue;
+
+		/* if still not found then something has modified too much */
+		/* we could list the "closest" transfer here... */
+		dev_err(&spi->dev,
+			"loopback strangeness - rx changed outside of allowed range at: %pK\n",
+			addr);
+		/* do not return, only set ret,
+		 * so that we list all addresses
+		 */
+		ret = -ERANGE;
+	}
+
+	return ret;
+}
+
+static int spi_test_check_loopback_result(struct spi_device *spi,
+					  struct spi_message *msg,
+					  void *tx, void *rx)
+{
+	struct spi_transfer *xfer;
+	u8 rxb, txb;
+	size_t i;
+	int ret;
+
+	/* checks rx_buffer pattern are valid with loopback or without */
+	ret = spi_check_rx_ranges(spi, msg, rx);
+	if (ret)
+		return ret;
+
+	/* if we run without loopback, then return now */
+	if (!loopback)
+		return 0;
+
+	/* if applicable to transfer check that rx_buf is equal to tx_buf */
+	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
+		/* if there is no rx, then no check is needed */
+		if (!xfer->rx_buf)
+			continue;
+		/* so depending on tx_buf we need to handle things */
+		if (xfer->tx_buf) {
+			for (i = 1; i < xfer->len; i++) {
+				txb = ((u8 *)xfer->tx_buf)[i];
+				rxb = ((u8 *)xfer->rx_buf)[i];
+				if (txb != rxb)
+					goto mismatch_error;
+			}
+		} else {
+			/* first byte received */
+			txb = ((u8 *)xfer->rx_buf)[0];
+			/* first byte may be 0 or xff */
+			if (!((txb == 0) || (txb == 0xff))) {
+				dev_err(&spi->dev,
+					"loopback strangeness - we expect 0x00 or 0xff, but not 0x%02x\n",
+					txb);
+				return -EINVAL;
+			}
+			/* check that all bytes are identical */
+			for (i = 1; i < xfer->len; i++) {
+				rxb = ((u8 *)xfer->rx_buf)[i];
+				if (rxb != txb)
+					goto mismatch_error;
+			}
+		}
+	}
+
+	return 0;
+
+mismatch_error:
+	dev_err(&spi->dev,
+		"loopback strangeness - transfer missmatch on byte %04zx - expected 0x%02x, but got 0x%02x\n",
+		i, txb, rxb);
+
+	return -EINVAL;
+}
+
+static int spi_test_translate(struct spi_device *spi,
+			      void **ptr, size_t len,
+			      void *tx, void *rx)
+{
+	size_t off;
+
+	/* return on null */
+	if (!*ptr)
+		return 0;
+
+	/* in the MAX_SIZE_HALF case modify the pointer */
+	if (((size_t)*ptr) & SPI_TEST_MAX_SIZE_HALF)
+		/* move the pointer to the correct range */
+		*ptr += (SPI_TEST_MAX_SIZE_PLUS / 2) -
+			SPI_TEST_MAX_SIZE_HALF;
+
+	/* RX range
+	 * - we check against MAX_SIZE_PLUS to allow for automated alignment
+	 */
+	if (RANGE_CHECK(*ptr, len,  RX(0), SPI_TEST_MAX_SIZE_PLUS)) {
+		off = *ptr - RX(0);
+		*ptr = rx + off;
+
+		return 0;
+	}
+
+	/* TX range */
+	if (RANGE_CHECK(*ptr, len,  TX(0), SPI_TEST_MAX_SIZE_PLUS)) {
+		off = *ptr - TX(0);
+		*ptr = tx + off;
+
+		return 0;
+	}
+
+	dev_err(&spi->dev,
+		"PointerRange [%pK:%pK[ not in range [%pK:%pK[ or [%pK:%pK[\n",
+		*ptr, *ptr + len,
+		RX(0), RX(SPI_TEST_MAX_SIZE),
+		TX(0), TX(SPI_TEST_MAX_SIZE));
+
+	return -EINVAL;
+}
+
+static int spi_test_fill_pattern(struct spi_device *spi,
+				 struct spi_test *test)
+{
+	struct spi_transfer *xfers = test->transfers;
+	u8 *tx_buf;
+	size_t count = 0;
+	int i, j;
+
+#ifdef __BIG_ENDIAN
+#define GET_VALUE_BYTE(value, index, bytes) \
+	(value >> (8 * (bytes - 1 - count % bytes)))
+#else
+#define GET_VALUE_BYTE(value, index, bytes) \
+	(value >> (8 * (count % bytes)))
+#endif
+
+	/* fill all transfers with the pattern requested */
+	for (i = 0; i < test->transfer_count; i++) {
+		/* fill rx_buf with SPI_TEST_PATTERN_UNWRITTEN */
+		if (xfers[i].rx_buf)
+			memset(xfers[i].rx_buf, SPI_TEST_PATTERN_UNWRITTEN,
+			       xfers[i].len);
+		/* if tx_buf is NULL then skip */
+		tx_buf = (u8 *)xfers[i].tx_buf;
+		if (!tx_buf)
+			continue;
+		/* modify all the transfers */
+		for (j = 0; j < xfers[i].len; j++, tx_buf++, count++) {
+			/* fill tx */
+			switch (test->fill_option) {
+			case FILL_MEMSET_8:
+				*tx_buf = test->fill_pattern;
+				break;
+			case FILL_MEMSET_16:
+				*tx_buf = GET_VALUE_BYTE(test->fill_pattern,
+							 count, 2);
+				break;
+			case FILL_MEMSET_24:
+				*tx_buf = GET_VALUE_BYTE(test->fill_pattern,
+							 count, 3);
+				break;
+			case FILL_MEMSET_32:
+				*tx_buf = GET_VALUE_BYTE(test->fill_pattern,
+							 count, 4);
+				break;
+			case FILL_COUNT_8:
+				*tx_buf = count;
+				break;
+			case FILL_COUNT_16:
+				*tx_buf = GET_VALUE_BYTE(count, count, 2);
+				break;
+			case FILL_COUNT_24:
+				*tx_buf = GET_VALUE_BYTE(count, count, 3);
+				break;
+			case FILL_COUNT_32:
+				*tx_buf = GET_VALUE_BYTE(count, count, 4);
+				break;
+			case FILL_TRANSFER_BYTE_8:
+				*tx_buf = j;
+				break;
+			case FILL_TRANSFER_BYTE_16:
+				*tx_buf = GET_VALUE_BYTE(j, j, 2);
+				break;
+			case FILL_TRANSFER_BYTE_24:
+				*tx_buf = GET_VALUE_BYTE(j, j, 3);
+				break;
+			case FILL_TRANSFER_BYTE_32:
+				*tx_buf = GET_VALUE_BYTE(j, j, 4);
+				break;
+			case FILL_TRANSFER_NUM:
+				*tx_buf = i;
+				break;
+			default:
+				dev_err(&spi->dev,
+					"unsupported fill_option: %i\n",
+					test->fill_option);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int _spi_test_run_iter(struct spi_device *spi,
+			      struct spi_test *test,
+			      void *tx, void *rx)
+{
+	struct spi_message *msg = &test->msg;
+	struct spi_transfer *x;
+	int i, ret;
+
+	/* initialize message - zero-filled via static initialization */
+	spi_message_init_no_memset(msg);
+
+	/* fill rx with the DO_NOT_WRITE pattern */
+	memset(rx, SPI_TEST_PATTERN_DO_NOT_WRITE, SPI_TEST_MAX_SIZE_PLUS);
+
+	/* add the individual transfers */
+	for (i = 0; i < test->transfer_count; i++) {
+		x = &test->transfers[i];
+
+		/* patch the values of tx_buf */
+		ret = spi_test_translate(spi, (void **)&x->tx_buf, x->len,
+					 (void *)tx, rx);
+		if (ret)
+			return ret;
+
+		/* patch the values of rx_buf */
+		ret = spi_test_translate(spi, &x->rx_buf, x->len,
+					 (void *)tx, rx);
+		if (ret)
+			return ret;
+
+		/* and add it to the list */
+		spi_message_add_tail(x, msg);
+	}
+
+	/* fill in the transfer buffers with pattern */
+	ret = spi_test_fill_pattern(spi, test);
+	if (ret)
+		return ret;
+
+	/* and execute */
+	if (test->execute_msg)
+		ret = test->execute_msg(spi, test, tx, rx);
+	else
+		ret = spi_test_execute_msg(spi, test, tx, rx);
+
+	/* handle result */
+	if (ret == test->expected_return)
+		return 0;
+
+	dev_err(&spi->dev,
+		"test failed - test returned %i, but we expect %i\n",
+		ret, test->expected_return);
+
+	if (ret)
+		return ret;
+
+	/* if it is 0, as we expected something else,
+	 * then return something special
+	 */
+	return -EFAULT;
+}
+
+static int spi_test_run_iter(struct spi_device *spi,
+			     const struct spi_test *testtemplate,
+			     void *tx, void *rx,
+			     size_t len,
+			     size_t tx_off,
+			     size_t rx_off
+	)
+{
+	struct spi_test test;
+	int i, tx_count, rx_count;
+
+	/* copy the test template to test */
+	memcpy(&test, testtemplate, sizeof(test));
+
+	/* set up test->transfers to the correct count */
+	if (!test.transfer_count) {
+		for (i = 0;
+		    (i < SPI_TEST_MAX_TRANSFERS) && test.transfers[i].len;
+		    i++) {
+			test.transfer_count++;
+		}
+	}
+
+	/* if iterate_transfer_mask is not set,
+	 * then set it to first transfer only
+	 */
+	if (!(test.iterate_transfer_mask & (BIT(test.transfer_count) - 1)))
+		test.iterate_transfer_mask = 1;
+
+	/* count number of transfers with tx/rx_buf != NULL */
+	rx_count = tx_count = 0;
+	for (i = 0; i < test.transfer_count; i++) {
+		if (test.transfers[i].tx_buf)
+			tx_count++;
+		if (test.transfers[i].rx_buf)
+			rx_count++;
+	}
+
+	/* in some iteration cases warn and exit early,
+	 * as there is nothing to do, that has not been tested already...
+	 */
+	if (tx_off && (!tx_count)) {
+		dev_warn_once(&spi->dev,
+			      "%s: iterate_tx_off configured with tx_buf==NULL - ignoring\n",
+			      test.description);
+		return 0;
+	}
+	if (rx_off && (!rx_count)) {
+		dev_warn_once(&spi->dev,
+			      "%s: iterate_rx_off configured with rx_buf==NULL - ignoring\n",
+			      test.description);
+		return 0;
+	}
+
+	/* write out info */
+	if (!(len || tx_off || rx_off)) {
+		dev_info(&spi->dev, "Running test %s\n", test.description);
+	} else {
+		dev_info(&spi->dev,
+			 "  with iteration values: len = %zu, tx_off = %zu, rx_off = %zu\n",
+			 len, tx_off, rx_off);
+	}
+
+	/* update in the values from iteration values */
+	for (i = 0; i < test.transfer_count; i++) {
+		/* only when bit in transfer mask is set */
+		if (!(test.iterate_transfer_mask & BIT(i)))
+			continue;
+		if (len)
+			test.transfers[i].len = len;
+		if (test.transfers[i].tx_buf)
+			test.transfers[i].tx_buf += tx_off;
+		if (test.transfers[i].tx_buf)
+			test.transfers[i].rx_buf += rx_off;
+	}
+
+	/* and execute */
+	return _spi_test_run_iter(spi, &test, tx, rx);
+}
+
+/**
+ * spi_test_execute_msg - default implementation to run a test
+ *
+ * spi: @spi_device on which to run the @spi_message
+ * test: the test to execute, which already contains @msg
+ * tx:   the tx buffer allocated for the test sequence
+ * rx:   the rx buffer allocated for the test sequence
+ *
+ * Returns: error code of spi_sync as well as basic error checking
+ */
+int spi_test_execute_msg(struct spi_device *spi, struct spi_test *test,
+			 void *tx, void *rx)
+{
+	struct spi_message *msg = &test->msg;
+	int ret = 0;
+	int i;
+
+	/* only if we do not simulate */
+	if (!simulate_only) {
+		/* dump the complete message before and after the transfer */
+		if (dump_messages == 3)
+			spi_test_dump_message(spi, msg, true);
+
+		/* run spi message */
+		ret = spi_sync(spi, msg);
+		if (ret == -ETIMEDOUT) {
+			dev_info(&spi->dev,
+				 "spi-message timed out - reruning...\n");
+			/* rerun after a few explicit schedules */
+			for (i = 0; i < 16; i++)
+				schedule();
+			ret = spi_sync(spi, msg);
+		}
+		if (ret) {
+			dev_err(&spi->dev,
+				"Failed to execute spi_message: %i\n",
+				ret);
+			goto exit;
+		}
+
+		/* do some extra error checks */
+		if (msg->frame_length != msg->actual_length) {
+			dev_err(&spi->dev,
+				"actual length differs from expected\n");
+			ret = -EIO;
+			goto exit;
+		}
+
+		/* run rx-buffer tests */
+		ret = spi_test_check_loopback_result(spi, msg, tx, rx);
+	}
+
+	/* if requested or on error dump message (including data) */
+exit:
+	if (dump_messages || ret)
+		spi_test_dump_message(spi, msg,
+				      (dump_messages >= 2) || (ret));
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(spi_test_execute_msg);
+
+/**
+ * spi_test_run_test - run an individual spi_test
+ *                     including all the relevant iterations on:
+ *                     length and buffer alignment
+ *
+ * spi:  the spi_device to send the messages to
+ * test: the test which we need to execute
+ * tx:   the tx buffer allocated for the test sequence
+ * rx:   the rx buffer allocated for the test sequence
+ *
+ * Returns: status code of spi_sync or other failures
+ */
+
+int spi_test_run_test(struct spi_device *spi, const struct spi_test *test,
+		      void *tx, void *rx)
+{
+	int idx_len;
+	size_t len;
+	size_t tx_align, rx_align;
+	int ret;
+
+	/* test for transfer limits */
+	if (test->transfer_count >= SPI_TEST_MAX_TRANSFERS) {
+		dev_err(&spi->dev,
+			"%s: Exceeded max number of transfers with %i\n",
+			test->description, test->transfer_count);
+		return -E2BIG;
+	}
+
+	/* setting up some values in spi_message
+	 * based on some settings in spi_master
+	 * some of this can also get done in the run() method
+	 */
+
+	/* iterate over all the iterable values using macros
+	 * (to make it a bit more readable...
+	 */
+#define FOR_EACH_ITERATE(var, defaultvalue)				\
+	for (idx_##var = -1, var = defaultvalue;			\
+	     ((idx_##var < 0) ||					\
+		     (							\
+			     (idx_##var < SPI_TEST_MAX_ITERATE) &&	\
+			     (var = test->iterate_##var[idx_##var])	\
+		     )							\
+	     );								\
+	     idx_##var++)
+#define FOR_EACH_ALIGNMENT(var)						\
+	for (var = 0;							\
+	    var < (test->iterate_##var ?				\
+			(spi->master->dma_alignment ?			\
+			 spi->master->dma_alignment :			\
+			 test->iterate_##var) :				\
+			1);						\
+	    var++)
+
+	FOR_EACH_ITERATE(len, 0) {
+		FOR_EACH_ALIGNMENT(tx_align) {
+			FOR_EACH_ALIGNMENT(rx_align) {
+				/* and run the iteration */
+				ret = spi_test_run_iter(spi, test,
+							tx, rx,
+							len,
+							tx_align,
+							rx_align);
+				if (ret)
+					return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(spi_test_run_test);
+
+/**
+ * spi_test_run_tests - run an array of spi_messages tests
+ * @spi: the spi device on which to run the tests
+ * @tests: NULL-terminated array of @spi_test
+ *
+ * Returns: status errors as per @spi_test_run_test()
+ */
+
+int spi_test_run_tests(struct spi_device *spi,
+		       struct spi_test *tests)
+{
+	char *rx = NULL, *tx = NULL;
+	int ret = 0, count = 0;
+	struct spi_test *test;
+
+	/* allocate rx/tx buffers of 128kB size without devm
+	 * in the hope that is on a page boundary
+	 */
+	rx = kzalloc(SPI_TEST_MAX_SIZE_PLUS, GFP_KERNEL);
+	if (!rx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	tx = kzalloc(SPI_TEST_MAX_SIZE_PLUS, GFP_KERNEL);
+	if (!tx) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* now run the individual tests in the table */
+	for (test = tests, count = 0; test->description[0];
+	     test++, count++) {
+		/* only run test if requested */
+		if ((run_only_test > -1) && (count != run_only_test))
+			continue;
+		/* run custom implementation */
+		if (test->run_test)
+			ret = test->run_test(spi, test, tx, rx);
+		else
+			ret = spi_test_run_test(spi, test, tx, rx);
+		if (ret)
+			goto out;
+		/* add some delays so that we can easily
+		 * detect the individual tests when using a logic analyzer
+		 * we also add scheduling to avoid potential spi_timeouts...
+		 */
+		mdelay(100);
+		schedule();
+	}
+
+out:
+	kfree(rx);
+	kfree(tx);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(spi_test_run_tests);
diff --git a/drivers/spi/spi-mt65xx.c b/drivers/spi/spi-mt65xx.c
index 7840067062a8..0be89e052428 100644
--- a/drivers/spi/spi-mt65xx.c
+++ b/drivers/spi/spi-mt65xx.c
@@ -95,8 +95,7 @@ struct mtk_spi {
 	const struct mtk_spi_compatible *dev_comp;
 };
 
-static const struct mtk_spi_compatible mt6589_compat;
-static const struct mtk_spi_compatible mt8135_compat;
+static const struct mtk_spi_compatible mtk_common_compat;
 static const struct mtk_spi_compatible mt8173_compat = {
 	.need_pad_sel = true,
 	.must_tx = true,
@@ -112,9 +111,18 @@ static const struct mtk_chip_config mtk_default_chip_info = {
 };
 
 static const struct of_device_id mtk_spi_of_match[] = {
-	{ .compatible = "mediatek,mt6589-spi", .data = (void *)&mt6589_compat },
-	{ .compatible = "mediatek,mt8135-spi", .data = (void *)&mt8135_compat },
-	{ .compatible = "mediatek,mt8173-spi", .data = (void *)&mt8173_compat },
+	{ .compatible = "mediatek,mt2701-spi",
+		.data = (void *)&mtk_common_compat,
+	},
+	{ .compatible = "mediatek,mt6589-spi",
+		.data = (void *)&mtk_common_compat,
+	},
+	{ .compatible = "mediatek,mt8135-spi",
+		.data = (void *)&mtk_common_compat,
+	},
+	{ .compatible = "mediatek,mt8173-spi",
+		.data = (void *)&mt8173_compat,
+	},
 	{}
 };
 MODULE_DEVICE_TABLE(of, mtk_spi_of_match);
@@ -154,9 +162,6 @@ static int mtk_spi_prepare_message(struct spi_master *master,
 		reg_val |= SPI_CMD_CPOL;
 	else
 		reg_val &= ~SPI_CMD_CPOL;
-	writel(reg_val, mdata->base + SPI_CMD_REG);
-
-	reg_val = readl(mdata->base + SPI_CMD_REG);
 
 	/* set the mlsbx and mlsbtx */
 	if (chip_config->tx_mlsb)
@@ -323,7 +328,8 @@ static int mtk_spi_fifo_transfer(struct spi_master *master,
 				 struct spi_device *spi,
 				 struct spi_transfer *xfer)
 {
-	int cnt;
+	int cnt, remainder;
+	u32 reg_val;
 	struct mtk_spi *mdata = spi_master_get_devdata(master);
 
 	mdata->cur_transfer = xfer;
@@ -331,12 +337,16 @@ static int mtk_spi_fifo_transfer(struct spi_master *master,
 	mtk_spi_prepare_transfer(master, xfer);
 	mtk_spi_setup_packet(master);
 
-	if (xfer->len % 4)
-		cnt = xfer->len / 4 + 1;
-	else
-		cnt = xfer->len / 4;
+	cnt = xfer->len / 4;
 	iowrite32_rep(mdata->base + SPI_TX_DATA_REG, xfer->tx_buf, cnt);
 
+	remainder = xfer->len % 4;
+	if (remainder > 0) {
+		reg_val = 0;
+		memcpy(&reg_val, xfer->tx_buf + (cnt * 4), remainder);
+		writel(reg_val, mdata->base + SPI_TX_DATA_REG);
+	}
+
 	mtk_spi_enable_transfer(master);
 
 	return 1;
@@ -418,7 +428,7 @@ static int mtk_spi_setup(struct spi_device *spi)
 
 static irqreturn_t mtk_spi_interrupt(int irq, void *dev_id)
 {
-	u32 cmd, reg_val, cnt;
+	u32 cmd, reg_val, cnt, remainder;
 	struct spi_master *master = dev_id;
 	struct mtk_spi *mdata = spi_master_get_devdata(master);
 	struct spi_transfer *trans = mdata->cur_transfer;
@@ -431,12 +441,15 @@ static irqreturn_t mtk_spi_interrupt(int irq, void *dev_id)
 
 	if (!master->can_dma(master, master->cur_msg->spi, trans)) {
 		if (trans->rx_buf) {
-			if (mdata->xfer_len % 4)
-				cnt = mdata->xfer_len / 4 + 1;
-			else
-				cnt = mdata->xfer_len / 4;
+			cnt = mdata->xfer_len / 4;
 			ioread32_rep(mdata->base + SPI_RX_DATA_REG,
 				     trans->rx_buf, cnt);
+			remainder = mdata->xfer_len % 4;
+			if (remainder > 0) {
+				reg_val = readl(mdata->base + SPI_RX_DATA_REG);
+				memcpy(trans->rx_buf + (cnt * 4),
+					&reg_val, remainder);
+			}
 		}
 		spi_finalize_current_transfer(master);
 		return IRQ_HANDLED;
@@ -610,7 +623,8 @@ static int mtk_spi_probe(struct platform_device *pdev)
 	ret = clk_set_parent(mdata->sel_clk, mdata->parent_clk);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to clk_set_parent (%d)\n", ret);
-		goto err_disable_clk;
+		clk_disable_unprepare(mdata->spi_clk);
+		goto err_put_master;
 	}
 
 	clk_disable_unprepare(mdata->spi_clk);
@@ -620,7 +634,7 @@ static int mtk_spi_probe(struct platform_device *pdev)
 	ret = devm_spi_register_master(&pdev->dev, master);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register master (%d)\n", ret);
-		goto err_put_master;
+		goto err_disable_runtime_pm;
 	}
 
 	if (mdata->dev_comp->need_pad_sel) {
@@ -629,14 +643,14 @@ static int mtk_spi_probe(struct platform_device *pdev)
 				"pad_num does not match num_chipselect(%d != %d)\n",
 				mdata->pad_num, master->num_chipselect);
 			ret = -EINVAL;
-			goto err_put_master;
+			goto err_disable_runtime_pm;
 		}
 
 		if (!master->cs_gpios && master->num_chipselect > 1) {
 			dev_err(&pdev->dev,
 				"cs_gpios not specified and num_chipselect > 1\n");
 			ret = -EINVAL;
-			goto err_put_master;
+			goto err_disable_runtime_pm;
 		}
 
 		if (master->cs_gpios) {
@@ -647,7 +661,7 @@ static int mtk_spi_probe(struct platform_device *pdev)
 				if (ret) {
 					dev_err(&pdev->dev,
 						"can't get CS GPIO %i\n", i);
-					goto err_put_master;
+					goto err_disable_runtime_pm;
 				}
 			}
 		}
@@ -655,8 +669,8 @@ static int mtk_spi_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_disable_clk:
-	clk_disable_unprepare(mdata->spi_clk);
+err_disable_runtime_pm:
+	pm_runtime_disable(&pdev->dev);
 err_put_master:
 	spi_master_put(master);
 
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 1f8903d356e5..0caa3c8bef46 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -24,6 +24,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/dmaengine.h>
 #include <linux/omap-dma.h>
+#include <linux/pinctrl/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/err.h>
 #include <linux/clk.h>
@@ -1024,6 +1025,16 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 		spi->controller_state = cs;
 		/* Link this to context save list */
 		list_add_tail(&cs->node, &ctx->cs);
+
+		if (gpio_is_valid(spi->cs_gpio)) {
+			ret = gpio_request(spi->cs_gpio, dev_name(&spi->dev));
+			if (ret) {
+				dev_err(&spi->dev, "failed to request gpio\n");
+				return ret;
+			}
+			gpio_direction_output(spi->cs_gpio,
+					 !(spi->mode & SPI_CS_HIGH));
+		}
 	}
 
 	if (!mcspi_dma->dma_rx || !mcspi_dma->dma_tx) {
@@ -1032,15 +1043,6 @@ static int omap2_mcspi_setup(struct spi_device *spi)
 			return ret;
 	}
 
-	if (gpio_is_valid(spi->cs_gpio)) {
-		ret = gpio_request(spi->cs_gpio, dev_name(&spi->dev));
-		if (ret) {
-			dev_err(&spi->dev, "failed to request gpio\n");
-			return ret;
-		}
-		gpio_direction_output(spi->cs_gpio, !(spi->mode & SPI_CS_HIGH));
-	}
-
 	ret = pm_runtime_get_sync(mcspi->dev);
 	if (ret < 0)
 		return ret;
@@ -1488,6 +1490,8 @@ static int omap2_mcspi_probe(struct platform_device *pdev)
 	return status;
 
 disable_pm:
+	pm_runtime_dont_use_autosuspend(&pdev->dev);
+	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 free_master:
 	spi_master_put(master);
@@ -1499,6 +1503,7 @@ static int omap2_mcspi_remove(struct platform_device *pdev)
 	struct spi_master *master = platform_get_drvdata(pdev);
 	struct omap2_mcspi *mcspi = spi_master_get_devdata(master);
 
+	pm_runtime_dont_use_autosuspend(mcspi->dev);
 	pm_runtime_put_sync(mcspi->dev);
 	pm_runtime_disable(&pdev->dev);
 
@@ -1536,14 +1541,23 @@ static int omap2_mcspi_resume(struct device *dev)
 	}
 	pm_runtime_mark_last_busy(mcspi->dev);
 	pm_runtime_put_autosuspend(mcspi->dev);
-	return 0;
+
+	return pinctrl_pm_select_default_state(dev);
+}
+
+static int omap2_mcspi_suspend(struct device *dev)
+{
+	return pinctrl_pm_select_sleep_state(dev);
 }
+
 #else
+#define omap2_mcspi_suspend	NULL
 #define	omap2_mcspi_resume	NULL
 #endif
 
 static const struct dev_pm_ops omap2_mcspi_pm_ops = {
 	.resume = omap2_mcspi_resume,
+	.suspend = omap2_mcspi_suspend,
 	.runtime_resume	= omap_mcspi_runtime_resume,
 };
 
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index b25dc71b0ea9..ab9914ad8365 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -1567,9 +1567,6 @@ static int pxa2xx_spi_probe(struct platform_device *pdev)
 	if (!is_quark_x1000_ssp(drv_data))
 		pxa2xx_spi_write(drv_data, SSPSP, 0);
 
-	if (is_lpss_ssp(drv_data))
-		lpss_ssp_setup(drv_data);
-
 	if (is_lpss_ssp(drv_data)) {
 		lpss_ssp_setup(drv_data);
 		config = lpss_get_config(drv_data);
diff --git a/drivers/spi/spi-rockchip.c b/drivers/spi/spi-rockchip.c
index 79a8bc4f6cec..7cb1b2d710c1 100644
--- a/drivers/spi/spi-rockchip.c
+++ b/drivers/spi/spi-rockchip.c
@@ -749,6 +749,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 	return 0;
 
 err_register_master:
+	pm_runtime_disable(&pdev->dev);
 	if (rs->dma_tx.ch)
 		dma_release_channel(rs->dma_tx.ch);
 	if (rs->dma_rx.ch)
@@ -778,6 +779,8 @@ static int rockchip_spi_remove(struct platform_device *pdev)
 	if (rs->dma_rx.ch)
 		dma_release_channel(rs->dma_rx.ch);
 
+	spi_master_put(master);
+
 	return 0;
 }
 
diff --git a/drivers/spi/spi-s3c64xx.c b/drivers/spi/spi-s3c64xx.c
index 8e86e7f6663a..5a76a50063b5 100644
--- a/drivers/spi/spi-s3c64xx.c
+++ b/drivers/spi/spi-s3c64xx.c
@@ -133,7 +133,6 @@
 struct s3c64xx_spi_dma_data {
 	struct dma_chan *ch;
 	enum dma_transfer_direction direction;
-	unsigned int dmach;
 };
 
 /**
@@ -325,7 +324,7 @@ static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
 
 		/* Acquire DMA channels */
 		sdd->rx_dma.ch = dma_request_slave_channel_compat(mask, filter,
-				   (void *)(long)sdd->rx_dma.dmach, dev, "rx");
+				   sdd->cntrlr_info->dma_rx, dev, "rx");
 		if (!sdd->rx_dma.ch) {
 			dev_err(dev, "Failed to get RX DMA channel\n");
 			ret = -EBUSY;
@@ -334,7 +333,7 @@ static int s3c64xx_spi_prepare_transfer(struct spi_master *spi)
 		spi->dma_rx = sdd->rx_dma.ch;
 
 		sdd->tx_dma.ch = dma_request_slave_channel_compat(mask, filter,
-				   (void *)(long)sdd->tx_dma.dmach, dev, "tx");
+				   sdd->cntrlr_info->dma_tx, dev, "tx");
 		if (!sdd->tx_dma.ch) {
 			dev_err(dev, "Failed to get TX DMA channel\n");
 			ret = -EBUSY;
@@ -1028,7 +1027,6 @@ static inline struct s3c64xx_spi_port_config *s3c64xx_spi_get_port_config(
 static int s3c64xx_spi_probe(struct platform_device *pdev)
 {
 	struct resource	*mem_res;
-	struct resource	*res;
 	struct s3c64xx_spi_driver_data *sdd;
 	struct s3c64xx_spi_info *sci = dev_get_platdata(&pdev->dev);
 	struct spi_master *master;
@@ -1087,20 +1085,9 @@ static int s3c64xx_spi_probe(struct platform_device *pdev)
 
 	sdd->cur_bpw = 8;
 
-	if (!sdd->pdev->dev.of_node) {
-		res = platform_get_resource(pdev, IORESOURCE_DMA,  0);
-		if (!res) {
-			dev_warn(&pdev->dev, "Unable to get SPI tx dma resource. Switching to poll mode\n");
-			sdd->port_conf->quirks = S3C64XX_SPI_QUIRK_POLL;
-		} else
-			sdd->tx_dma.dmach = res->start;
-
-		res = platform_get_resource(pdev, IORESOURCE_DMA,  1);
-		if (!res) {
-			dev_warn(&pdev->dev, "Unable to get SPI rx dma resource. Switching to poll mode\n");
-			sdd->port_conf->quirks = S3C64XX_SPI_QUIRK_POLL;
-		} else
-			sdd->rx_dma.dmach = res->start;
+	if (!sdd->pdev->dev.of_node && (!sci->dma_tx || !sci->dma_rx)) {
+		dev_warn(&pdev->dev, "Unable to get SPI tx/rx DMA data. Switching to poll mode\n");
+		sdd->port_conf->quirks = S3C64XX_SPI_QUIRK_POLL;
 	}
 
 	sdd->tx_dma.direction = DMA_MEM_TO_DEV;
@@ -1197,9 +1184,9 @@ static int s3c64xx_spi_probe(struct platform_device *pdev)
 
 	dev_dbg(&pdev->dev, "Samsung SoC SPI Driver loaded for Bus SPI-%d with %d Slaves attached\n",
 					sdd->port_id, master->num_chipselect);
-	dev_dbg(&pdev->dev, "\tIOmem=[%pR]\tFIFO %dbytes\tDMA=[Rx-%d, Tx-%d]\n",
+	dev_dbg(&pdev->dev, "\tIOmem=[%pR]\tFIFO %dbytes\tDMA=[Rx-%p, Tx-%p]\n",
 					mem_res, (FIFO_LVL_MASK(sdd) >> 1) + 1,
-					sdd->rx_dma.dmach, sdd->tx_dma.dmach);
+					sci->dma_rx, sci->dma_tx);
 
 	pm_runtime_mark_last_busy(&pdev->dev);
 	pm_runtime_put_autosuspend(&pdev->dev);
@@ -1370,12 +1357,6 @@ static const struct platform_device_id s3c64xx_spi_driver_ids[] = {
 	}, {
 		.name		= "s3c6410-spi",
 		.driver_data	= (kernel_ulong_t)&s3c6410_spi_port_config,
-	}, {
-		.name		= "s5pv210-spi",
-		.driver_data	= (kernel_ulong_t)&s5pv210_spi_port_config,
-	}, {
-		.name		= "exynos4210-spi",
-		.driver_data	= (kernel_ulong_t)&exynos4_spi_port_config,
 	},
 	{ },
 };
diff --git a/drivers/spi/spi-sun4i.c b/drivers/spi/spi-sun4i.c
index fbb0a4d74e91..1ddd9e2309b6 100644
--- a/drivers/spi/spi-sun4i.c
+++ b/drivers/spi/spi-sun4i.c
@@ -140,6 +140,9 @@ static void sun4i_spi_set_cs(struct spi_device *spi, bool enable)
 	reg &= ~SUN4I_CTL_CS_MASK;
 	reg |= SUN4I_CTL_CS(spi->chip_select);
 
+	/* We want to control the chip select manually */
+	reg |= SUN4I_CTL_CS_MANUAL;
+
 	if (enable)
 		reg |= SUN4I_CTL_CS_LEVEL;
 	else
@@ -222,15 +225,12 @@ static int sun4i_spi_transfer_one(struct spi_master *master,
 	else
 		reg |= SUN4I_CTL_DHB;
 
-	/* We want to control the chip select manually */
-	reg |= SUN4I_CTL_CS_MANUAL;
-
 	sun4i_spi_write(sspi, SUN4I_CTL_REG, reg);
 
 	/* Ensure that we have a parent clock fast enough */
 	mclk_rate = clk_get_rate(sspi->mclk);
-	if (mclk_rate < (2 * spi->max_speed_hz)) {
-		clk_set_rate(sspi->mclk, 2 * spi->max_speed_hz);
+	if (mclk_rate < (2 * tfr->speed_hz)) {
+		clk_set_rate(sspi->mclk, 2 * tfr->speed_hz);
 		mclk_rate = clk_get_rate(sspi->mclk);
 	}
 
@@ -248,14 +248,14 @@ static int sun4i_spi_transfer_one(struct spi_master *master,
 	 * First try CDR2, and if we can't reach the expected
 	 * frequency, fall back to CDR1.
 	 */
-	div = mclk_rate / (2 * spi->max_speed_hz);
+	div = mclk_rate / (2 * tfr->speed_hz);
 	if (div <= (SUN4I_CLK_CTL_CDR2_MASK + 1)) {
 		if (div > 0)
 			div--;
 
 		reg = SUN4I_CLK_CTL_CDR2(div) | SUN4I_CLK_CTL_DRS;
 	} else {
-		div = ilog2(mclk_rate) - ilog2(spi->max_speed_hz);
+		div = ilog2(mclk_rate) - ilog2(tfr->speed_hz);
 		reg = SUN4I_CLK_CTL_CDR1(div);
 	}
 
diff --git a/drivers/spi/spi-sun6i.c b/drivers/spi/spi-sun6i.c
index ac48f59705a8..42e2c4bd690a 100644
--- a/drivers/spi/spi-sun6i.c
+++ b/drivers/spi/spi-sun6i.c
@@ -217,8 +217,8 @@ static int sun6i_spi_transfer_one(struct spi_master *master,
 
 	/* Ensure that we have a parent clock fast enough */
 	mclk_rate = clk_get_rate(sspi->mclk);
-	if (mclk_rate < (2 * spi->max_speed_hz)) {
-		clk_set_rate(sspi->mclk, 2 * spi->max_speed_hz);
+	if (mclk_rate < (2 * tfr->speed_hz)) {
+		clk_set_rate(sspi->mclk, 2 * tfr->speed_hz);
 		mclk_rate = clk_get_rate(sspi->mclk);
 	}
 
@@ -236,14 +236,14 @@ static int sun6i_spi_transfer_one(struct spi_master *master,
 	 * First try CDR2, and if we can't reach the expected
 	 * frequency, fall back to CDR1.
 	 */
-	div = mclk_rate / (2 * spi->max_speed_hz);
+	div = mclk_rate / (2 * tfr->speed_hz);
 	if (div <= (SUN6I_CLK_CTL_CDR2_MASK + 1)) {
 		if (div > 0)
 			div--;
 
 		reg = SUN6I_CLK_CTL_CDR2(div) | SUN6I_CLK_CTL_DRS;
 	} else {
-		div = ilog2(mclk_rate) - ilog2(spi->max_speed_hz);
+		div = ilog2(mclk_rate) - ilog2(tfr->speed_hz);
 		reg = SUN6I_CLK_CTL_CDR1(div);
 	}
 
diff --git a/drivers/spi/spi-test.h b/drivers/spi/spi-test.h
new file mode 100644
index 000000000000..922c52833239
--- /dev/null
+++ b/drivers/spi/spi-test.h
@@ -0,0 +1,136 @@
+/*
+ *  linux/drivers/spi/spi-test.h
+ *
+ *  (c) Martin Sperl <kernel@martin.sperl.org>
+ *
+ *  spi_test definitions
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT 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/spi/spi.h>
+
+#define SPI_TEST_MAX_TRANSFERS 4
+#define SPI_TEST_MAX_SIZE (32 * PAGE_SIZE)
+#define SPI_TEST_MAX_ITERATE 32
+
+/* the "dummy" start addresses used in spi_test
+ * these addresses get translated at a later stage
+ */
+#define RX_START	BIT(30)
+#define TX_START	BIT(31)
+#define RX(off)		((void *)(RX_START + off))
+#define TX(off)		((void *)(TX_START + off))
+
+/* some special defines for offsets */
+#define SPI_TEST_MAX_SIZE_HALF	BIT(29)
+
+/* detection pattern for unfinished reads...
+ * - 0x00 or 0xff could be valid levels for tx_buf = NULL,
+ * so we do not use either of them
+ */
+#define SPI_TEST_PATTERN_UNWRITTEN 0xAA
+#define SPI_TEST_PATTERN_DO_NOT_WRITE 0x55
+#define SPI_TEST_CHECK_DO_NOT_WRITE 64
+
+/**
+ * struct spi_test - describes a specific (set of) tests to execute
+ *
+ * @description:      description of the test
+ *
+ * @msg:              a template @spi_message usedfor the default settings
+ * @transfers:        array of @spi_transfers that are part of the
+ *                    resulting spi_message. The first transfer with len == 0
+ *                    signifies the end of the list
+ * @transfer_count:   normally computed number of transfers with len > 0
+ *
+ * @run_test:         run a specific spi_test - this allows to override
+ *                    the default implementation of @spi_test_run_transfer
+ *                    either to add some custom filters for a specific test
+ *                    or to effectively run some very custom tests...
+ * @execute_msg:      run the spi_message for real - this allows to override
+ *                    @spi_test_execute_msg to apply final modifications
+ *                    on the spi_message
+ * @expected_return:  the expected return code - in some cases we want to
+ *                    test also for error conditions
+ *
+ * @iterate_len:      list of length to iterate on (in addition to the
+ *                    explicitly set @spi_transfer.len)
+ * @iterate_tx_align: change the alignment of @spi_transfer.tx_buf
+ *                    for all values in the below range if set.
+ *                    the ranges are:
+ *                    [0 : @spi_master.dma_alignment[ if set
+ *                    [0 : iterate_tx_align[ if unset
+ * @iterate_rx_align: change the alignment of @spi_transfer.rx_buf
+ *                    see @iterate_tx_align for details
+ * @iterate_transfer_mask: the bitmask of transfers to which the iterations
+ *                         apply - if 0, then it applies to all transfer
+ *
+ * @fill_option:      define the way how tx_buf is filled
+ * @fill_pattern:     fill pattern to apply to the tx_buf
+ *                    (used in some of the @fill_options)
+ */
+
+struct spi_test {
+	char description[64];
+	struct spi_message msg;
+	struct spi_transfer transfers[SPI_TEST_MAX_TRANSFERS];
+	unsigned int transfer_count;
+	int (*run_test)(struct spi_device *spi, struct spi_test *test,
+			void *tx, void *rx);
+	int (*execute_msg)(struct spi_device *spi, struct spi_test *test,
+			   void *tx, void *rx);
+	int expected_return;
+	/* iterate over all the non-zero values */
+	int iterate_len[SPI_TEST_MAX_ITERATE];
+	int iterate_tx_align;
+	int iterate_rx_align;
+	u32 iterate_transfer_mask;
+	/* the tx-fill operation */
+	u32 fill_option;
+#define FILL_MEMSET_8	0	/* just memset with 8 bit */
+#define FILL_MEMSET_16	1	/* just memset with 16 bit */
+#define FILL_MEMSET_24	2	/* just memset with 24 bit */
+#define FILL_MEMSET_32	3	/* just memset with 32 bit */
+#define FILL_COUNT_8	4	/* fill with a 8 byte counter */
+#define FILL_COUNT_16	5	/* fill with a 16 bit counter */
+#define FILL_COUNT_24	6	/* fill with a 24 bit counter */
+#define FILL_COUNT_32	7	/* fill with a 32 bit counter */
+#define FILL_TRANSFER_BYTE_8  8	/* fill with the transfer byte - 8 bit */
+#define FILL_TRANSFER_BYTE_16 9	/* fill with the transfer byte - 16 bit */
+#define FILL_TRANSFER_BYTE_24 10 /* fill with the transfer byte - 24 bit */
+#define FILL_TRANSFER_BYTE_32 11 /* fill with the transfer byte - 32 bit */
+#define FILL_TRANSFER_NUM     16 /* fill with the transfer number */
+	u32 fill_pattern;
+};
+
+/* default implementation for @spi_test.run_test */
+int spi_test_run_test(struct spi_device *spi,
+		      const struct spi_test *test,
+		      void *tx, void *rx);
+
+/* default implementation for @spi_test.execute_msg */
+int spi_test_execute_msg(struct spi_device *spi,
+			 struct spi_test *test,
+			 void *tx, void *rx);
+
+/* function to execute a set of tests */
+int spi_test_run_tests(struct spi_device *spi,
+		       struct spi_test *tests);
+
+/* some of the default @spi_transfer.len to test */
+#define ITERATE_LEN 2, 3, 7, 11, 16, 31, 32, 64, 97, 128, 251, 256, \
+		1021, 1024, 1031, 4093, PAGE_SIZE, 4099, 65536, 65537
+
+#define ITERATE_MAX_LEN ITERATE_LEN, SPI_TEST_MAX_SIZE - 1, SPI_TEST_MAX_SIZE
+
+/* the default alignment to test */
+#define ITERATE_ALIGN sizeof(int)
diff --git a/drivers/spi/spi-zynqmp-gqspi.c b/drivers/spi/spi-zynqmp-gqspi.c
index f23f36ebaf3d..aab9b492c627 100644
--- a/drivers/spi/spi-zynqmp-gqspi.c
+++ b/drivers/spi/spi-zynqmp-gqspi.c
@@ -917,9 +917,7 @@ static int zynqmp_qspi_start_transfer(struct spi_master *master,
  */
 static int __maybe_unused zynqmp_qspi_suspend(struct device *dev)
 {
-	struct platform_device *pdev = container_of(dev,
-						    struct platform_device,
-						    dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spi_master *master = platform_get_drvdata(pdev);
 
 	spi_master_suspend(master);
@@ -940,9 +938,7 @@ static int __maybe_unused zynqmp_qspi_suspend(struct device *dev)
  */
 static int __maybe_unused zynqmp_qspi_resume(struct device *dev)
 {
-	struct platform_device *pdev = container_of(dev,
-						    struct platform_device,
-						    dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct spi_master *master = platform_get_drvdata(pdev);
 	struct zynqmp_qspi *xqspi = spi_master_get_devdata(master);
 	int ret = 0;
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index dee1cb87d24f..47eff8012a77 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -84,8 +84,7 @@ static ssize_t spi_device_##field##_show(struct device *dev,		\
 					 struct device_attribute *attr,	\
 					char *buf)			\
 {									\
-	struct spi_device *spi = container_of(dev,			\
-					      struct spi_device, dev);	\
+	struct spi_device *spi = to_spi_device(dev);			\
 	return spi_statistics_##field##_show(&spi->statistics, buf);	\
 }									\
 static struct device_attribute dev_attr_spi_device_##field = {		\
@@ -605,6 +604,24 @@ struct spi_device *spi_new_device(struct spi_master *master,
 }
 EXPORT_SYMBOL_GPL(spi_new_device);
 
+/**
+ * spi_unregister_device - unregister a single SPI device
+ * @spi: spi_device to unregister
+ *
+ * Start making the passed SPI device vanish. Normally this would be handled
+ * by spi_unregister_master().
+ */
+void spi_unregister_device(struct spi_device *spi)
+{
+	if (!spi)
+		return;
+
+	if (spi->dev.of_node)
+		of_node_clear_flag(spi->dev.of_node, OF_POPULATED);
+	device_unregister(&spi->dev);
+}
+EXPORT_SYMBOL_GPL(spi_unregister_device);
+
 static void spi_match_master_to_boardinfo(struct spi_master *master,
 				struct spi_board_info *bi)
 {
@@ -1548,6 +1565,8 @@ static void of_register_spi_devices(struct spi_master *master)
 		return;
 
 	for_each_available_child_of_node(master->dev.of_node, nc) {
+		if (of_node_test_and_set_flag(nc, OF_POPULATED))
+			continue;
 		spi = of_register_spi_device(master, nc);
 		if (IS_ERR(spi))
 			dev_warn(&master->dev, "Failed to create SPI device for %s\n",
@@ -1623,6 +1642,9 @@ static acpi_status acpi_spi_add_device(acpi_handle handle, u32 level,
 		return AE_OK;
 	}
 
+	if (spi->irq < 0)
+		spi->irq = acpi_dev_gpio_irq_get(adev, 0);
+
 	adev->power.flags.ignore_parent = true;
 	strlcpy(spi->modalias, acpi_device_hid(adev), sizeof(spi->modalias));
 	if (spi_add_device(spi)) {
@@ -2633,6 +2655,11 @@ static int of_spi_notify(struct notifier_block *nb, unsigned long action,
 		if (master == NULL)
 			return NOTIFY_OK;	/* not for us */
 
+		if (of_node_test_and_set_flag(rd->dn, OF_POPULATED)) {
+			put_device(&master->dev);
+			return NOTIFY_OK;
+		}
+
 		spi = of_register_spi_device(master, rd->dn);
 		put_device(&master->dev);
 
@@ -2644,6 +2671,10 @@ static int of_spi_notify(struct notifier_block *nb, unsigned long action,
 		break;
 
 	case OF_RECONFIG_CHANGE_REMOVE:
+		/* already depopulated? */
+		if (!of_node_check_flag(rd->dn, OF_POPULATED))
+			return NOTIFY_OK;
+
 		/* find our device by node */
 		spi = of_find_spi_device_by_node(rd->dn);
 		if (spi == NULL)
diff --git a/drivers/spi/spidev.c b/drivers/spi/spidev.c
index d0e7dfc647cf..e3c19f30f591 100644
--- a/drivers/spi/spidev.c
+++ b/drivers/spi/spidev.c
@@ -284,7 +284,7 @@ static int spidev_message(struct spidev_data *spidev,
 			k_tmp->speed_hz = spidev->speed_hz;
 #ifdef VERBOSE
 		dev_dbg(&spidev->spi->dev,
-			"  xfer len %zd %s%s%s%dbits %u usec %uHz\n",
+			"  xfer len %u %s%s%s%dbits %u usec %uHz\n",
 			u_tmp->len,
 			u_tmp->rx_buf ? "rx " : "",
 			u_tmp->tx_buf ? "tx " : "",
diff --git a/drivers/ssb/Kconfig b/drivers/ssb/Kconfig
index 149214beeda9..d8e4219c2324 100644
--- a/drivers/ssb/Kconfig
+++ b/drivers/ssb/Kconfig
@@ -82,7 +82,8 @@ config SSB_SDIOHOST
 
 config SSB_HOST_SOC
 	bool "Support for SSB bus on SoC"
-	depends on SSB
+	depends on SSB && BCM47XX_NVRAM
+	select SSB_SPROM
 	help
 	  Host interface for a SSB directly mapped into memory. This is
 	  for some Broadcom SoCs from the BCM47xx and BCM53xx lines.
diff --git a/drivers/ssb/host_soc.c b/drivers/ssb/host_soc.c
index c809f255af34..d62992dc08b2 100644
--- a/drivers/ssb/host_soc.c
+++ b/drivers/ssb/host_soc.c
@@ -8,6 +8,7 @@
  * Licensed under the GNU/GPL. See COPYING for details.
  */
 
+#include <linux/bcm47xx_nvram.h>
 #include <linux/ssb/ssb.h>
 
 #include "ssb_private.h"
@@ -171,3 +172,39 @@ const struct ssb_bus_ops ssb_host_soc_ops = {
 	.block_write	= ssb_host_soc_block_write,
 #endif
 };
+
+int ssb_host_soc_get_invariants(struct ssb_bus *bus,
+				struct ssb_init_invariants *iv)
+{
+	char buf[20];
+	int len, err;
+
+	/* Fill boardinfo structure */
+	memset(&iv->boardinfo, 0, sizeof(struct ssb_boardinfo));
+
+	len = bcm47xx_nvram_getenv("boardvendor", buf, sizeof(buf));
+	if (len > 0) {
+		err = kstrtou16(strim(buf), 0, &iv->boardinfo.vendor);
+		if (err)
+			pr_warn("Couldn't parse nvram board vendor entry with value \"%s\"\n",
+				buf);
+	}
+	if (!iv->boardinfo.vendor)
+		iv->boardinfo.vendor = SSB_BOARDVENDOR_BCM;
+
+	len = bcm47xx_nvram_getenv("boardtype", buf, sizeof(buf));
+	if (len > 0) {
+		err = kstrtou16(strim(buf), 0, &iv->boardinfo.type);
+		if (err)
+			pr_warn("Couldn't parse nvram board type entry with value \"%s\"\n",
+				buf);
+	}
+
+	memset(&iv->sprom, 0, sizeof(struct ssb_sprom));
+	ssb_fill_sprom_with_fallback(bus, &iv->sprom);
+
+	if (bcm47xx_nvram_getenv("cardbus", buf, sizeof(buf)) >= 0)
+		iv->has_cardbus_slot = !!simple_strtoul(buf, NULL, 10);
+
+	return 0;
+}
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 5d1e9a0fc389..d1a750760cf3 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -613,9 +613,10 @@ out:
 	return err;
 }
 
-static int ssb_bus_register(struct ssb_bus *bus,
-			    ssb_invariants_func_t get_invariants,
-			    unsigned long baseaddr)
+static int __maybe_unused
+ssb_bus_register(struct ssb_bus *bus,
+		 ssb_invariants_func_t get_invariants,
+		 unsigned long baseaddr)
 {
 	int err;
 
@@ -762,15 +763,14 @@ EXPORT_SYMBOL(ssb_bus_sdiobus_register);
 #endif /* CONFIG_SSB_PCMCIAHOST */
 
 #ifdef CONFIG_SSB_HOST_SOC
-int ssb_bus_ssbbus_register(struct ssb_bus *bus, unsigned long baseaddr,
-			    ssb_invariants_func_t get_invariants)
+int ssb_bus_host_soc_register(struct ssb_bus *bus, unsigned long baseaddr)
 {
 	int err;
 
 	bus->bustype = SSB_BUSTYPE_SSB;
 	bus->ops = &ssb_host_soc_ops;
 
-	err = ssb_bus_register(bus, get_invariants, baseaddr);
+	err = ssb_bus_register(bus, ssb_host_soc_get_invariants, baseaddr);
 	if (!err) {
 		ssb_info("Sonics Silicon Backplane found at address 0x%08lX\n",
 			 baseaddr);
diff --git a/drivers/ssb/ssb_private.h b/drivers/ssb/ssb_private.h
index 15bfd5c7d2d7..c2f5d3969c8b 100644
--- a/drivers/ssb/ssb_private.h
+++ b/drivers/ssb/ssb_private.h
@@ -163,6 +163,9 @@ static inline int ssb_sdio_init(struct ssb_bus *bus)
 
 #ifdef CONFIG_SSB_HOST_SOC
 extern const struct ssb_bus_ops ssb_host_soc_ops;
+
+extern int ssb_host_soc_get_invariants(struct ssb_bus *bus,
+				       struct ssb_init_invariants *iv);
 #endif
 
 /* scan.c */
diff --git a/drivers/staging/android/TODO b/drivers/staging/android/TODO
index 8f3ac37bfe12..64d8c8720960 100644
--- a/drivers/staging/android/TODO
+++ b/drivers/staging/android/TODO
@@ -25,5 +25,13 @@ ion/
    exposes existing cma regions and doesn't reserve unecessarily memory when
    booting a system which doesn't use ion.
 
+sync framework:
+ - remove CONFIG_SW_SYNC_USER, it is used only for testing/debugging and
+ should not be upstreamed.
+ - port CONFIG_SW_SYNC_USER tests interfaces to use debugfs somehow
+ - port libsync tests to kselftest
+ - clean up and ABI check for security issues
+ - move it to drivers/base/dma-buf
+
 Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
 Arve Hjønnevåg <arve@android.com> and Riley Andrews <riandrews@android.com>
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 3f2a3d611e4b..5bb1283d19cd 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -831,14 +831,14 @@ static struct miscdevice ashmem_misc = {
 
 static int __init ashmem_init(void)
 {
-	int ret;
+	int ret = -ENOMEM;
 
 	ashmem_area_cachep = kmem_cache_create("ashmem_area_cache",
 					       sizeof(struct ashmem_area),
 					       0, 0, NULL);
 	if (unlikely(!ashmem_area_cachep)) {
 		pr_err("failed to create slab cache\n");
-		return -ENOMEM;
+		goto out;
 	}
 
 	ashmem_range_cachep = kmem_cache_create("ashmem_range_cache",
@@ -846,13 +846,13 @@ static int __init ashmem_init(void)
 						0, 0, NULL);
 	if (unlikely(!ashmem_range_cachep)) {
 		pr_err("failed to create slab cache\n");
-		return -ENOMEM;
+		goto out_free1;
 	}
 
 	ret = misc_register(&ashmem_misc);
 	if (unlikely(ret)) {
 		pr_err("failed to register misc device!\n");
-		return ret;
+		goto out_free2;
 	}
 
 	register_shrinker(&ashmem_shrinker);
@@ -860,5 +860,12 @@ static int __init ashmem_init(void)
 	pr_info("initialized\n");
 
 	return 0;
+
+out_free2:
+	kmem_cache_destroy(ashmem_range_cachep);
+out_free1:
+	kmem_cache_destroy(ashmem_area_cachep);
+out:
+	return ret;
 }
 device_initcall(ashmem_init);
diff --git a/drivers/staging/android/ion/Kconfig b/drivers/staging/android/ion/Kconfig
index 345234624492..19c1572f1525 100644
--- a/drivers/staging/android/ion/Kconfig
+++ b/drivers/staging/android/ion/Kconfig
@@ -33,3 +33,10 @@ config ION_TEGRA
 	help
 	  Choose this option if you wish to use ion on an nVidia Tegra.
 
+config ION_HISI
+	tristate "Ion for Hisilicon"
+	depends on ARCH_HISI && ION
+	help
+	  Choose this option if you wish to use ion on Hisilicon Platform.
+
+source "drivers/staging/android/ion/hisilicon/Kconfig"
diff --git a/drivers/staging/android/ion/Makefile b/drivers/staging/android/ion/Makefile
index b56fd2bf2b4f..18cc2aa593c2 100644
--- a/drivers/staging/android/ion/Makefile
+++ b/drivers/staging/android/ion/Makefile
@@ -7,4 +7,5 @@ endif
 
 obj-$(CONFIG_ION_DUMMY) += ion_dummy_driver.o
 obj-$(CONFIG_ION_TEGRA) += tegra/
+obj-$(CONFIG_ION_HISI) += hisilicon/
 
diff --git a/drivers/staging/android/ion/compat_ion.c b/drivers/staging/android/ion/compat_ion.c
index a402fdaf54ca..9a978d21785e 100644
--- a/drivers/staging/android/ion/compat_ion.c
+++ b/drivers/staging/android/ion/compat_ion.c
@@ -137,7 +137,7 @@ long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
 		data32 = compat_ptr(arg);
 		data = compat_alloc_user_space(sizeof(*data));
-		if (data == NULL)
+		if (!data)
 			return -EFAULT;
 
 		err = compat_get_ion_allocation_data(data32, data);
@@ -156,7 +156,7 @@ long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
 		data32 = compat_ptr(arg);
 		data = compat_alloc_user_space(sizeof(*data));
-		if (data == NULL)
+		if (!data)
 			return -EFAULT;
 
 		err = compat_get_ion_handle_data(data32, data);
@@ -173,7 +173,7 @@ long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 
 		data32 = compat_ptr(arg);
 		data = compat_alloc_user_space(sizeof(*data));
-		if (data == NULL)
+		if (!data)
 			return -EFAULT;
 
 		err = compat_get_ion_custom_data(data32, data);
diff --git a/drivers/staging/android/ion/hisilicon/Kconfig b/drivers/staging/android/ion/hisilicon/Kconfig
new file mode 100644
index 000000000000..2b4bd0798290
--- /dev/null
+++ b/drivers/staging/android/ion/hisilicon/Kconfig
@@ -0,0 +1,5 @@
+config HI6220_ION
+        bool "Hi6220 ION Driver"
+        depends on ARCH_HISI && ION
+        help
+          Build the Hisilicon Hi6220 ion driver.
diff --git a/drivers/staging/android/ion/hisilicon/Makefile b/drivers/staging/android/ion/hisilicon/Makefile
new file mode 100644
index 000000000000..2a89414280ac
--- /dev/null
+++ b/drivers/staging/android/ion/hisilicon/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_HI6220_ION) += hi6220_ion.o
diff --git a/drivers/staging/android/ion/hisilicon/hi6220_ion.c b/drivers/staging/android/ion/hisilicon/hi6220_ion.c
new file mode 100644
index 000000000000..e3c07b2ba00e
--- /dev/null
+++ b/drivers/staging/android/ion/hisilicon/hi6220_ion.c
@@ -0,0 +1,223 @@
+/*
+ * Hisilicon Hi6220 ION Driver
+ *
+ * Copyright (c) 2015 Hisilicon Limited.
+ *
+ * Author: Chen Feng <puck.chen@hisilicon.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) "Ion: " fmt
+
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/mm.h>
+#include "../ion_priv.h"
+#include "../ion.h"
+
+struct hi6220_ion_type_table {
+	const char *name;
+	enum ion_heap_type type;
+};
+
+static struct hi6220_ion_type_table ion_type_table[] = {
+	{"ion_system", ION_HEAP_TYPE_SYSTEM},
+	{"ion_system_contig", ION_HEAP_TYPE_SYSTEM_CONTIG},
+	{"ion_carveout", ION_HEAP_TYPE_CARVEOUT},
+	{"ion_chunk", ION_HEAP_TYPE_CHUNK},
+	{"ion_dma", ION_HEAP_TYPE_DMA},
+	{"ion_custom", ION_HEAP_TYPE_CUSTOM},
+};
+
+static struct ion_device *idev;
+static int num_heaps;
+static struct ion_heap **heaps;
+static struct ion_platform_heap **heaps_data;
+
+static int get_type_by_name(const char *name, enum ion_heap_type *type)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ion_type_table); i++) {
+		if (strncmp(name, ion_type_table[i].name, strlen(name)))
+			continue;
+
+		*type = ion_type_table[i].type;
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int hi6220_set_platform_data(struct platform_device *pdev)
+{
+	unsigned int base;
+	unsigned int size;
+	unsigned int id;
+	const char *heap_name;
+	const char *type_name;
+	enum ion_heap_type type;
+	int ret;
+	struct device_node *np;
+	struct ion_platform_heap *p_data;
+	const struct device_node *dt_node = pdev->dev.of_node;
+	int index = 0;
+
+	for_each_child_of_node(dt_node, np)
+		num_heaps++;
+
+	heaps_data = devm_kzalloc(&pdev->dev,
+				  sizeof(struct ion_platform_heap *) *
+				  num_heaps,
+				  GFP_KERNEL);
+	if (!heaps_data)
+		return -ENOMEM;
+
+	for_each_child_of_node(dt_node, np) {
+		ret = of_property_read_string(np, "heap-name", &heap_name);
+		if (ret < 0) {
+			pr_err("check the name of node %s\n", np->name);
+			continue;
+		}
+
+		ret = of_property_read_u32(np, "heap-id", &id);
+		if (ret < 0) {
+			pr_err("check the id %s\n", np->name);
+			continue;
+		}
+
+		ret = of_property_read_u32(np, "heap-base", &base);
+		if (ret < 0) {
+			pr_err("check the base of node %s\n", np->name);
+			continue;
+		}
+
+		ret = of_property_read_u32(np, "heap-size", &size);
+		if (ret < 0) {
+			pr_err("check the size of node %s\n", np->name);
+			continue;
+		}
+
+		ret = of_property_read_string(np, "heap-type", &type_name);
+		if (ret < 0) {
+			pr_err("check the type of node %s\n", np->name);
+			continue;
+		}
+
+		ret = get_type_by_name(type_name, &type);
+		if (ret < 0) {
+			pr_err("type name error %s!\n", type_name);
+			continue;
+		}
+		pr_info("heap index %d : name %s base 0x%x size 0x%x id %d type %d\n",
+			index, heap_name, base, size, id, type);
+
+		p_data = devm_kzalloc(&pdev->dev,
+				      sizeof(struct ion_platform_heap),
+				      GFP_KERNEL);
+		if (!p_data)
+			return -ENOMEM;
+
+		p_data->name = heap_name;
+		p_data->base = base;
+		p_data->size = size;
+		p_data->id = id;
+		p_data->type = type;
+
+		heaps_data[index] = p_data;
+		index++;
+	}
+	return 0;
+}
+
+static int hi6220_ion_probe(struct platform_device *pdev)
+{
+	int i;
+	int err;
+	static struct ion_platform_heap *p_heap;
+
+	idev = ion_device_create(NULL);
+	err = hi6220_set_platform_data(pdev);
+	if (err) {
+		pr_err("ion set platform data error!\n");
+		goto err_free_idev;
+	}
+	heaps = devm_kzalloc(&pdev->dev,
+			     sizeof(struct ion_heap *) * num_heaps,
+			     GFP_KERNEL);
+	if (!heaps) {
+		err = -ENOMEM;
+		goto err_free_idev;
+	}
+
+	/*
+	 * create the heaps as specified in the dts file
+	 */
+	for (i = 0; i < num_heaps; i++) {
+		p_heap = heaps_data[i];
+		heaps[i] = ion_heap_create(p_heap);
+		if (IS_ERR_OR_NULL(heaps[i])) {
+			err = PTR_ERR(heaps[i]);
+			goto err_free_heaps;
+		}
+
+		ion_device_add_heap(idev, heaps[i]);
+
+		pr_info("%s: adding heap %s of type %d with %lx@%lx\n",
+			__func__, p_heap->name, p_heap->type,
+			p_heap->base, (unsigned long)p_heap->size);
+	}
+	return err;
+
+err_free_heaps:
+	for (i = 0; i < num_heaps; ++i) {
+		ion_heap_destroy(heaps[i]);
+		heaps[i] = NULL;
+	}
+err_free_idev:
+	ion_device_destroy(idev);
+
+	return err;
+}
+
+static int hi6220_ion_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < num_heaps; i++) {
+		ion_heap_destroy(heaps[i]);
+		heaps[i] = NULL;
+	}
+	ion_device_destroy(idev);
+
+	return 0;
+}
+
+static const struct of_device_id hi6220_ion_match_table[] = {
+	{.compatible = "hisilicon,hi6220-ion"},
+	{},
+};
+
+static struct platform_driver hi6220_ion_driver = {
+	.probe = hi6220_ion_probe,
+	.remove = hi6220_ion_remove,
+	.driver = {
+		.name = "ion-hi6220",
+		.of_match_table = hi6220_ion_match_table,
+	},
+};
+
+static int __init hi6220_ion_init(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&hi6220_ion_driver);
+	return ret;
+}
+
+subsys_initcall(hi6220_ion_init);
diff --git a/drivers/staging/android/lowmemorykiller.c b/drivers/staging/android/lowmemorykiller.c
index e679d8432810..8b5a4a82d8b8 100644
--- a/drivers/staging/android/lowmemorykiller.c
+++ b/drivers/staging/android/lowmemorykiller.c
@@ -43,7 +43,7 @@
 #include <linux/profile.h>
 #include <linux/notifier.h>
 
-static uint32_t lowmem_debug_level = 1;
+static u32 lowmem_debug_level = 1;
 static short lowmem_adj[6] = {
 	0,
 	1,
@@ -105,8 +105,8 @@ static unsigned long lowmem_scan(struct shrinker *s, struct shrink_control *sc)
 	}
 
 	lowmem_print(3, "lowmem_scan %lu, %x, ofree %d %d, ma %hd\n",
-			sc->nr_to_scan, sc->gfp_mask, other_free,
-			other_file, min_score_adj);
+		     sc->nr_to_scan, sc->gfp_mask, other_free,
+		     other_file, min_score_adj);
 
 	if (min_score_adj == OOM_SCORE_ADJ_MAX + 1) {
 		lowmem_print(5, "lowmem_scan %lu, %x, return 0\n",
diff --git a/drivers/staging/android/sync.c b/drivers/staging/android/sync.c
index f83e00c78051..ed43796b5b58 100644
--- a/drivers/staging/android/sync.c
+++ b/drivers/staging/android/sync.c
@@ -43,7 +43,7 @@ struct sync_timeline *sync_timeline_create(const struct sync_timeline_ops *ops,
 		return NULL;
 
 	obj = kzalloc(size, GFP_KERNEL);
-	if (obj == NULL)
+	if (!obj)
 		return NULL;
 
 	kref_init(&obj->kref);
@@ -130,7 +130,7 @@ struct sync_pt *sync_pt_create(struct sync_timeline *obj, int size)
 		return NULL;
 
 	pt = kzalloc(size, GFP_KERNEL);
-	if (pt == NULL)
+	if (!pt)
 		return NULL;
 
 	spin_lock_irqsave(&obj->child_list_lock, flags);
@@ -155,7 +155,7 @@ static struct sync_fence *sync_fence_alloc(int size, const char *name)
 	struct sync_fence *fence;
 
 	fence = kzalloc(size, GFP_KERNEL);
-	if (fence == NULL)
+	if (!fence)
 		return NULL;
 
 	fence->file = anon_inode_getfile("sync_fence", &sync_fence_fops,
@@ -188,34 +188,39 @@ static void fence_check_cb_func(struct fence *f, struct fence_cb *cb)
 }
 
 /* TODO: implement a create which takes more that one sync_pt */
-struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt)
+struct sync_fence *sync_fence_create_dma(const char *name, struct fence *pt)
 {
 	struct sync_fence *fence;
 
 	fence = sync_fence_alloc(offsetof(struct sync_fence, cbs[1]), name);
-	if (fence == NULL)
+	if (!fence)
 		return NULL;
 
 	fence->num_fences = 1;
 	atomic_set(&fence->status, 1);
 
-	fence->cbs[0].sync_pt = &pt->base;
+	fence->cbs[0].sync_pt = pt;
 	fence->cbs[0].fence = fence;
-	if (fence_add_callback(&pt->base, &fence->cbs[0].cb,
-			       fence_check_cb_func))
+	if (fence_add_callback(pt, &fence->cbs[0].cb, fence_check_cb_func))
 		atomic_dec(&fence->status);
 
 	sync_fence_debug_add(fence);
 
 	return fence;
 }
+EXPORT_SYMBOL(sync_fence_create_dma);
+
+struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt)
+{
+	return sync_fence_create_dma(name, &pt->base);
+}
 EXPORT_SYMBOL(sync_fence_create);
 
 struct sync_fence *sync_fence_fdget(int fd)
 {
 	struct file *file = fget(fd);
 
-	if (file == NULL)
+	if (!file)
 		return NULL;
 
 	if (file->f_op != &sync_fence_fops)
@@ -262,7 +267,7 @@ struct sync_fence *sync_fence_merge(const char *name,
 	unsigned long size = offsetof(struct sync_fence, cbs[num_fences]);
 
 	fence = sync_fence_alloc(size, name);
-	if (fence == NULL)
+	if (!fence)
 		return NULL;
 
 	atomic_set(&fence->status, num_fences);
@@ -313,7 +318,7 @@ struct sync_fence *sync_fence_merge(const char *name,
 EXPORT_SYMBOL(sync_fence_merge);
 
 int sync_fence_wake_up_wq(wait_queue_t *curr, unsigned mode,
-				 int wake_flags, void *key)
+			  int wake_flags, void *key)
 {
 	struct sync_fence_waiter *wait;
 
@@ -353,7 +358,7 @@ int sync_fence_wait_async(struct sync_fence *fence,
 EXPORT_SYMBOL(sync_fence_wait_async);
 
 int sync_fence_cancel_async(struct sync_fence *fence,
-			     struct sync_fence_waiter *waiter)
+			    struct sync_fence_waiter *waiter)
 {
 	unsigned long flags;
 	int ret = 0;
@@ -519,12 +524,10 @@ static const struct fence_ops android_fence_ops = {
 static void sync_fence_free(struct kref *kref)
 {
 	struct sync_fence *fence = container_of(kref, struct sync_fence, kref);
-	int i, status = atomic_read(&fence->status);
+	int i;
 
 	for (i = 0; i < fence->num_fences; ++i) {
-		if (status)
-			fence_remove_callback(fence->cbs[i].sync_pt,
-					      &fence->cbs[i].cb);
+		fence_remove_callback(fence->cbs[i].sync_pt, &fence->cbs[i].cb);
 		fence_put(fence->cbs[i].sync_pt);
 	}
 
@@ -583,14 +586,14 @@ static long sync_fence_ioctl_merge(struct sync_fence *fence, unsigned long arg)
 	}
 
 	fence2 = sync_fence_fdget(data.fd2);
-	if (fence2 == NULL) {
+	if (!fence2) {
 		err = -ENOENT;
 		goto err_put_fd;
 	}
 
 	data.name[sizeof(data.name) - 1] = '\0';
 	fence3 = sync_fence_merge(data.name, fence, fence2);
-	if (fence3 == NULL) {
+	if (!fence3) {
 		err = -ENOMEM;
 		goto err_put_fence2;
 	}
@@ -666,7 +669,7 @@ static long sync_fence_ioctl_fence_info(struct sync_fence *fence,
 		size = 4096;
 
 	data = kzalloc(size, GFP_KERNEL);
-	if (data == NULL)
+	if (!data)
 		return -ENOMEM;
 
 	strlcpy(data->name, fence->name, sizeof(data->name));
diff --git a/drivers/staging/android/sync.h b/drivers/staging/android/sync.h
index 61f8a3aede96..afa0752275a7 100644
--- a/drivers/staging/android/sync.h
+++ b/drivers/staging/android/sync.h
@@ -254,6 +254,16 @@ void sync_pt_free(struct sync_pt *pt);
  */
 struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt);
 
+/**
+ * sync_fence_create_dma() - creates a sync fence from dma-fence
+ * @name:	name of fence to create
+ * @pt:	dma-fence to add to the fence
+ *
+ * Creates a fence containg @pt.  Once this is called, the fence takes
+ * ownership of @pt.
+ */
+struct sync_fence *sync_fence_create_dma(const char *name, struct fence *pt);
+
 /*
  * API for sync_fence consumers
  */
diff --git a/drivers/staging/android/sync_debug.c b/drivers/staging/android/sync_debug.c
index 91ed2c4cff45..f45d13cdd42b 100644
--- a/drivers/staging/android/sync_debug.c
+++ b/drivers/staging/android/sync_debug.c
@@ -82,36 +82,42 @@ static const char *sync_status_str(int status)
 	return "error";
 }
 
-static void sync_print_pt(struct seq_file *s, struct sync_pt *pt, bool fence)
+static void sync_print_pt(struct seq_file *s, struct fence *pt, bool fence)
 {
 	int status = 1;
-	struct sync_timeline *parent = sync_pt_parent(pt);
 
-	if (fence_is_signaled_locked(&pt->base))
-		status = pt->base.status;
+	if (fence_is_signaled_locked(pt))
+		status = pt->status;
 
 	seq_printf(s, "  %s%spt %s",
-		   fence ? parent->name : "",
+		   fence && pt->ops->get_timeline_name ?
+		   pt->ops->get_timeline_name(pt) : "",
 		   fence ? "_" : "",
 		   sync_status_str(status));
 
 	if (status <= 0) {
 		struct timespec64 ts64 =
-			ktime_to_timespec64(pt->base.timestamp);
+			ktime_to_timespec64(pt->timestamp);
 
 		seq_printf(s, "@%lld.%09ld", (s64)ts64.tv_sec, ts64.tv_nsec);
 	}
 
-	if (parent->ops->timeline_value_str &&
-	    parent->ops->pt_value_str) {
+	if ((!fence || pt->ops->timeline_value_str) &&
+	    pt->ops->fence_value_str) {
 		char value[64];
+		bool success;
 
-		parent->ops->pt_value_str(pt, value, sizeof(value));
-		seq_printf(s, ": %s", value);
-		if (fence) {
-			parent->ops->timeline_value_str(parent, value,
-						    sizeof(value));
-			seq_printf(s, " / %s", value);
+		pt->ops->fence_value_str(pt, value, sizeof(value));
+		success = strlen(value);
+
+		if (success)
+			seq_printf(s, ": %s", value);
+
+		if (success && fence) {
+			pt->ops->timeline_value_str(pt, value, sizeof(value));
+
+			if (strlen(value))
+				seq_printf(s, " / %s", value);
 		}
 	}
 
@@ -138,7 +144,7 @@ static void sync_print_obj(struct seq_file *s, struct sync_timeline *obj)
 	list_for_each(pos, &obj->child_list_head) {
 		struct sync_pt *pt =
 			container_of(pos, struct sync_pt, child_list);
-		sync_print_pt(s, pt, false);
+		sync_print_pt(s, &pt->base, false);
 	}
 	spin_unlock_irqrestore(&obj->child_list_lock, flags);
 }
@@ -153,11 +159,7 @@ static void sync_print_fence(struct seq_file *s, struct sync_fence *fence)
 		   sync_status_str(atomic_read(&fence->status)));
 
 	for (i = 0; i < fence->num_fences; ++i) {
-		struct sync_pt *pt =
-			container_of(fence->cbs[i].sync_pt,
-				     struct sync_pt, base);
-
-		sync_print_pt(s, pt, true);
+		sync_print_pt(s, fence->cbs[i].sync_pt, true);
 	}
 
 	spin_lock_irqsave(&fence->wq.lock, flags);
diff --git a/drivers/staging/android/timed_gpio.c b/drivers/staging/android/timed_gpio.c
index ce11726f1a6c..bcd9924d4631 100644
--- a/drivers/staging/android/timed_gpio.c
+++ b/drivers/staging/android/timed_gpio.c
@@ -25,7 +25,6 @@
 #include "timed_output.h"
 #include "timed_gpio.h"
 
-
 struct timed_gpio_data {
 	struct timed_output_dev dev;
 	struct hrtimer timer;
@@ -76,8 +75,8 @@ static void gpio_enable(struct timed_output_dev *dev, int value)
 			value = data->max_timeout;
 
 		hrtimer_start(&data->timer,
-			ktime_set(value / 1000, (value % 1000) * 1000000),
-			HRTIMER_MODE_REL);
+			      ktime_set(value / 1000, (value % 1000) * 1000000),
+			      HRTIMER_MODE_REL);
 	}
 
 	spin_unlock_irqrestore(&data->lock, flags);
@@ -94,8 +93,8 @@ static int timed_gpio_probe(struct platform_device *pdev)
 		return -EBUSY;
 
 	gpio_data = devm_kzalloc(&pdev->dev,
-			sizeof(struct timed_gpio_data) * pdata->num_gpios,
-			GFP_KERNEL);
+				 sizeof(*gpio_data) * pdata->num_gpios,
+				 GFP_KERNEL);
 	if (!gpio_data)
 		return -ENOMEM;
 
@@ -104,7 +103,7 @@ static int timed_gpio_probe(struct platform_device *pdev)
 		gpio_dat = &gpio_data[i];
 
 		hrtimer_init(&gpio_dat->timer, CLOCK_MONOTONIC,
-				HRTIMER_MODE_REL);
+			     HRTIMER_MODE_REL);
 		gpio_dat->timer.function = gpio_timer_func;
 		spin_lock_init(&gpio_dat->lock);
 
diff --git a/drivers/staging/board/armadillo800eva.c b/drivers/staging/board/armadillo800eva.c
index 9c41652ee908..912c96b0536d 100644
--- a/drivers/staging/board/armadillo800eva.c
+++ b/drivers/staging/board/armadillo800eva.c
@@ -97,7 +97,7 @@ static const struct board_staging_dev armadillo800eva_devices[] __initconst = {
 
 static void __init armadillo800eva_init(void)
 {
-	board_staging_gic_setup_xlate("arm,cortex-a9-gic", 32);
+	board_staging_gic_setup_xlate("arm,pl390", 32);
 	board_staging_register_devices(armadillo800eva_devices,
 				       ARRAY_SIZE(armadillo800eva_devices));
 }
diff --git a/drivers/staging/board/kzm9d.c b/drivers/staging/board/kzm9d.c
index 8d1eb09bc66e..05a6d434d307 100644
--- a/drivers/staging/board/kzm9d.c
+++ b/drivers/staging/board/kzm9d.c
@@ -11,7 +11,7 @@ static struct resource usbs1_res[] __initdata = {
 
 static void __init kzm9d_init(void)
 {
-	board_staging_gic_setup_xlate("arm,cortex-a9-gic", 32);
+	board_staging_gic_setup_xlate("arm,pl390", 32);
 
 	if (!board_staging_dt_node_available(usbs1_res,
 					     ARRAY_SIZE(usbs1_res))) {
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index ac0f01007abd..e7255f811611 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -737,15 +737,23 @@ config COMEDI_ADL_PCI9118
 	  called adl_pci9118.
 
 config COMEDI_ADV_PCI1710
-	tristate "Advantech PCI-171x, PCI-1720 and PCI-1731 support"
+	tristate "Advantech PCI-171x and PCI-1731 support"
 	select COMEDI_8254
 	---help---
 	  Enable support for Advantech PCI-1710, PCI-1710HG, PCI-1711,
-	  PCI-1713, PCI-1720 and PCI-1731
+	  PCI-1713 and PCI-1731
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called adv_pci1710.
 
+config COMEDI_ADV_PCI1720
+	tristate "Advantech PCI-1720 support"
+	---help---
+	  Enable support for Advantech PCI-1720 Analog Output board.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called adv_pci1720.
+
 config COMEDI_ADV_PCI1723
 	tristate "Advantech PCI-1723 support"
 	---help---
@@ -764,6 +772,14 @@ config COMEDI_ADV_PCI1724
 	  To compile this driver as a module, choose M here: the module will be
 	  called adv_pci1724.
 
+config COMEDI_ADV_PCI1760
+	tristate "Advantech PCI-1760 support"
+	---help---
+	  Enable support for Advantech PCI-1760 board.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called adv_pci1760.
+
 config COMEDI_ADV_PCI_DIO
 	tristate "Advantech PCI DIO card support"
 	select COMEDI_8254
@@ -771,8 +787,8 @@ config COMEDI_ADV_PCI_DIO
 	---help---
 	  Enable support for Advantech PCI DIO cards
 	  PCI-1730, PCI-1733, PCI-1734, PCI-1735U, PCI-1736UP, PCI-1739U,
-	  PCI-1750, PCI-1751, PCI-1752, PCI-1753/E, PCI-1754, PCI-1756,
-	  PCI-1760 and PCI-1762
+	  PCI-1750, PCI-1751, PCI-1752, PCI-1753/E, PCI-1754, PCI-1756 and
+	  PCI-1762
 
 	  To compile this driver as a module, choose M here: the module will be
 	  called adv_pci_dio.
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index 66edda190b75..83bd309d011b 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -1,20 +1,20 @@
 /*
-    include/comedi.h (installed as /usr/include/comedi.h)
-    header file for comedi
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU Lesser General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT 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/comedi.h (installed as /usr/include/comedi.h)
+ * header file for comedi
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT 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 _COMEDI_H
 #define _COMEDI_H
@@ -28,9 +28,9 @@
 #define COMEDI_MAJOR 98
 
 /*
-   maximum number of minor devices.  This can be increased, although
-   kernel structures are currently statically allocated, thus you
-   don't want this to be much more than you actually use.
+ * maximum number of minor devices.  This can be increased, although
+ * kernel structures are currently statically allocated, thus you
+ * don't want this to be much more than you actually use.
  */
 #define COMEDI_NDEVICES 16
 
@@ -63,21 +63,21 @@
 /* packs and unpacks a channel/range number */
 
 #define CR_PACK(chan, rng, aref)					\
-	((((aref)&0x3)<<24) | (((rng)&0xff)<<16) | (chan))
+	((((aref) & 0x3) << 24) | (((rng) & 0xff) << 16) | (chan))
 #define CR_PACK_FLAGS(chan, range, aref, flags)				\
 	(CR_PACK(chan, range, aref) | ((flags) & CR_FLAGS_MASK))
 
-#define CR_CHAN(a)	((a)&0xffff)
-#define CR_RANGE(a)	(((a)>>16)&0xff)
-#define CR_AREF(a)	(((a)>>24)&0x03)
+#define CR_CHAN(a)	((a) & 0xffff)
+#define CR_RANGE(a)	(((a) >> 16) & 0xff)
+#define CR_AREF(a)	(((a) >> 24) & 0x03)
 
 #define CR_FLAGS_MASK	0xfc000000
-#define CR_ALT_FILTER	(1<<26)
+#define CR_ALT_FILTER	(1 << 26)
 #define CR_DITHER	CR_ALT_FILTER
 #define CR_DEGLITCH	CR_ALT_FILTER
-#define CR_ALT_SOURCE	(1<<27)
-#define CR_EDGE		(1<<30)
-#define CR_INVERT	(1<<31)
+#define CR_ALT_SOURCE	(1 << 27)
+#define CR_EDGE		(1 << 30)
+#define CR_INVERT	(1 << 31)
 
 #define AREF_GROUND	0x00	/* analog ref = analog ground */
 #define AREF_COMMON	0x01	/* analog ref = analog common */
@@ -114,11 +114,11 @@
 
 #define INSN_READ		(0 | INSN_MASK_READ)
 #define INSN_WRITE		(1 | INSN_MASK_WRITE)
-#define INSN_BITS		(2 | INSN_MASK_READ|INSN_MASK_WRITE)
-#define INSN_CONFIG		(3 | INSN_MASK_READ|INSN_MASK_WRITE)
-#define INSN_GTOD		(4 | INSN_MASK_READ|INSN_MASK_SPECIAL)
-#define INSN_WAIT		(5 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
-#define INSN_INTTRIG		(6 | INSN_MASK_WRITE|INSN_MASK_SPECIAL)
+#define INSN_BITS		(2 | INSN_MASK_READ | INSN_MASK_WRITE)
+#define INSN_CONFIG		(3 | INSN_MASK_READ | INSN_MASK_WRITE)
+#define INSN_GTOD		(4 | INSN_MASK_READ | INSN_MASK_SPECIAL)
+#define INSN_WAIT		(5 | INSN_MASK_WRITE | INSN_MASK_SPECIAL)
+#define INSN_INTTRIG		(6 | INSN_MASK_WRITE | INSN_MASK_SPECIAL)
 
 /* trigger flags */
 /* These flags are used in comedi_trig structures */
@@ -279,7 +279,8 @@ enum configuration_ids {
 	INSN_CONFIG_SET_OTHER_SRC = 2005, /* Set other source */
 	/* INSN_CONFIG_GET_OTHER_SRC = 2006,*//* Get other source */
 	/* Get size in bytes of subdevice's on-board fifos used during
-	 * streaming input/output */
+	 * streaming input/output
+	 */
 	INSN_CONFIG_GET_HARDWARE_BUFFER_SIZE = 2006,
 	INSN_CONFIG_SET_COUNTER_MODE = 4097,
 	/* INSN_CONFIG_8254_SET_MODE is deprecated */
@@ -292,7 +293,8 @@ enum configuration_ids {
 	INSN_CONFIG_PWM_GET_PERIOD = 5001,	/* gets frequency */
 	INSN_CONFIG_GET_PWM_STATUS = 5002,	/* is it running? */
 	/* sets H bridge: duty cycle and sign bit for a relay at the
-	 * same time */
+	 * same time
+	 */
 	INSN_CONFIG_PWM_SET_H_BRIDGE = 5003,
 	/* gets H bridge data: duty cycle and the sign bit */
 	INSN_CONFIG_PWM_GET_H_BRIDGE = 5004
@@ -502,13 +504,13 @@ struct comedi_bufinfo {
 
 /* range stuff */
 
-#define __RANGE(a, b)	((((a)&0xffff)<<16)|((b)&0xffff))
+#define __RANGE(a, b)	((((a) & 0xffff) << 16) | ((b) & 0xffff))
 
-#define RANGE_OFFSET(a)		(((a)>>16)&0xffff)
-#define RANGE_LENGTH(b)		((b)&0xffff)
+#define RANGE_OFFSET(a)		(((a) >> 16) & 0xffff)
+#define RANGE_LENGTH(b)		((b) & 0xffff)
 
-#define RF_UNIT(flags)		((flags)&0xff)
-#define RF_EXTERNAL		(1<<8)
+#define RF_UNIT(flags)		((flags) & 0xff)
+#define RF_EXTERNAL		(1 << 8)
 
 #define UNIT_volt		0
 #define UNIT_mA			1
@@ -521,23 +523,22 @@ struct comedi_bufinfo {
 /**********************************************************/
 
 /*
-  8254 specific configuration.
-
-  It supports two config commands:
-
-  0 ID: INSN_CONFIG_SET_COUNTER_MODE
-  1 8254 Mode
-    I8254_MODE0, I8254_MODE1, ..., I8254_MODE5
-    OR'ed with:
-    I8254_BCD, I8254_BINARY
-
-  0 ID: INSN_CONFIG_8254_READ_STATUS
-  1 <-- Status byte returned here.
-    B7 = Output
-    B6 = NULL Count
-    B5 - B0 Current mode.
-
-*/
+ * 8254 specific configuration.
+ *
+ * It supports two config commands:
+ *
+ * 0 ID: INSN_CONFIG_SET_COUNTER_MODE
+ * 1 8254 Mode
+ * I8254_MODE0, I8254_MODE1, ..., I8254_MODE5
+ * OR'ed with:
+ * I8254_BCD, I8254_BINARY
+ *
+ * 0 ID: INSN_CONFIG_8254_READ_STATUS
+ * 1 <-- Status byte returned here.
+ * B7 = Output
+ * B6 = NULL Count
+ * B5 - B0 Current mode.
+ */
 
 enum i8254_mode {
 	I8254_MODE0 = (0 << 1),	/* Interrupt on terminal count */
@@ -545,18 +546,20 @@ enum i8254_mode {
 	I8254_MODE2 = (2 << 1),	/* Rate generator */
 	I8254_MODE3 = (3 << 1),	/* Square wave mode */
 	I8254_MODE4 = (4 << 1),	/* Software triggered strobe */
-	I8254_MODE5 = (5 << 1),	/* Hardware triggered strobe
-				 * (retriggerable) */
-	I8254_BCD = 1,		/* use binary-coded decimal instead of binary
-				 * (pretty useless) */
+	/* Hardware triggered strobe (retriggerable) */
+	I8254_MODE5 = (5 << 1),
+	/* Use binary-coded decimal instead of binary (pretty useless) */
+	I8254_BCD = 1,
 	I8254_BINARY = 0
 };
 
 #define NI_USUAL_PFI_SELECT(x)	(((x) < 10) ? (0x1 + (x)) : (0xb + (x)))
 #define NI_USUAL_RTSI_SELECT(x)	(((x) < 7) ? (0xb + (x)) : 0x1b)
 
-/* mode bits for NI general-purpose counters, set with
- * INSN_CONFIG_SET_COUNTER_MODE */
+/*
+ * mode bits for NI general-purpose counters, set with
+ * INSN_CONFIG_SET_COUNTER_MODE
+ */
 #define NI_GPCT_COUNTING_MODE_SHIFT 16
 #define NI_GPCT_INDEX_PHASE_BITSHIFT 20
 #define NI_GPCT_COUNTING_DIRECTION_SHIFT 24
@@ -624,8 +627,10 @@ enum ni_gpct_mode_bits {
 	NI_GPCT_INVERT_OUTPUT_BIT = 0x20000000
 };
 
-/* Bits for setting a clock source with
- * INSN_CONFIG_SET_CLOCK_SRC when using NI general-purpose counters. */
+/*
+ * Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI general-purpose counters.
+ */
 enum ni_gpct_clock_source_bits {
 	NI_GPCT_CLOCK_SRC_SELECT_MASK = 0x3f,
 	NI_GPCT_TIMEBASE_1_CLOCK_SRC_BITS = 0x0,
@@ -656,9 +661,11 @@ enum ni_gpct_clock_source_bits {
 /* no pfi on NI 660x */
 #define NI_GPCT_PFI_CLOCK_SRC_BITS(x)		(0x20 + (x))
 
-/* Possibilities for setting a gate source with
-INSN_CONFIG_SET_GATE_SRC when using NI general-purpose counters.
-May be bitwise-or'd with CR_EDGE or CR_INVERT. */
+/*
+ * Possibilities for setting a gate source with
+ * INSN_CONFIG_SET_GATE_SRC when using NI general-purpose counters.
+ * May be bitwise-or'd with CR_EDGE or CR_INVERT.
+ */
 enum ni_gpct_gate_select {
 	/* m-series gates */
 	NI_GPCT_TIMESTAMP_MUX_GATE_SELECT = 0x0,
@@ -675,9 +682,11 @@ enum ni_gpct_gate_select {
 	/* more gates for 660x "second gate" */
 	NI_GPCT_UP_DOWN_PIN_i_GATE_SELECT = 0x201,
 	NI_GPCT_SELECTED_GATE_GATE_SELECT = 0x21e,
-	/* m-series "second gate" sources are unknown,
+	/*
+	 * m-series "second gate" sources are unknown,
 	 * we should add them here with an offset of 0x300 when
-	 * known. */
+	 * known.
+	 */
 	NI_GPCT_DISABLED_GATE_SELECT = 0x8000,
 };
 
@@ -686,8 +695,10 @@ enum ni_gpct_gate_select {
 #define NI_GPCT_PFI_GATE_SELECT(x)		NI_USUAL_PFI_SELECT(x)
 #define NI_GPCT_UP_DOWN_PIN_GATE_SELECT(x)	(0x202 + (x))
 
-/* Possibilities for setting a source with
-INSN_CONFIG_SET_OTHER_SRC when using NI general-purpose counters. */
+/*
+ * Possibilities for setting a source with
+ * INSN_CONFIG_SET_OTHER_SRC when using NI general-purpose counters.
+ */
 enum ni_gpct_other_index {
 	NI_GPCT_SOURCE_ENCODER_A,
 	NI_GPCT_SOURCE_ENCODER_B,
@@ -702,18 +713,24 @@ enum ni_gpct_other_select {
 
 #define NI_GPCT_PFI_OTHER_SELECT(x)	NI_USUAL_PFI_SELECT(x)
 
-/* start sources for ni general-purpose counters for use with
-INSN_CONFIG_ARM */
+/*
+ * start sources for ni general-purpose counters for use with
+ * INSN_CONFIG_ARM
+ */
 enum ni_gpct_arm_source {
 	NI_GPCT_ARM_IMMEDIATE = 0x0,
-	NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1, /* Start both the counter
-					     * and the adjacent paired
-					     * counter simultaneously */
-	/* NI doesn't document bits for selecting hardware arm triggers.
+	/*
+	 * Start both the counter and the adjacent pared
+	 * counter simultaneously
+	 */
+	NI_GPCT_ARM_PAIRED_IMMEDIATE = 0x1,
+	/*
+	 * NI doesn't document bits for selecting hardware arm triggers.
 	 * If the NI_GPCT_ARM_UNKNOWN bit is set, we will pass the least
 	 * significant bits (3 bits for 660x or 5 bits for m-series)
 	 * through to the hardware.  This will at least allow someone to
-	 * figure out what the bits do later. */
+	 * figure out what the bits do later.
+	 */
 	NI_GPCT_ARM_UNKNOWN = 0x1000,
 };
 
@@ -728,8 +745,10 @@ enum ni_gpct_filter_select {
 	NI_GPCT_FILTER_2x_TIMEBASE_3 = 0x6
 };
 
-/* PFI digital filtering options for ni m-series for use with
- * INSN_CONFIG_FILTER. */
+/*
+ * PFI digital filtering options for ni m-series for use with
+ * INSN_CONFIG_FILTER.
+ */
 enum ni_pfi_filter_select {
 	NI_PFI_FILTER_OFF = 0x0,
 	NI_PFI_FILTER_125ns = 0x1,
@@ -740,9 +759,11 @@ enum ni_pfi_filter_select {
 /* master clock sources for ni mio boards and INSN_CONFIG_SET_CLOCK_SRC */
 enum ni_mio_clock_source {
 	NI_MIO_INTERNAL_CLOCK = 0,
-	NI_MIO_RTSI_CLOCK = 1,	/* doesn't work for m-series, use
-				   NI_MIO_PLL_RTSI_CLOCK() */
-	/* the NI_MIO_PLL_* sources are m-series only */
+	/*
+	 * Doesn't work for m-series, use NI_MIO_PLL_RTSI_CLOCK()
+	 * the NI_MIO_PLL_* sources are m-series only
+	 */
+	NI_MIO_RTSI_CLOCK = 1,
 	NI_MIO_PLL_PXI_STAR_TRIGGER_CLOCK = 2,
 	NI_MIO_PLL_PXI10_CLOCK = 3,
 	NI_MIO_PLL_RTSI0_CLOCK = 4
@@ -750,9 +771,11 @@ enum ni_mio_clock_source {
 
 #define NI_MIO_PLL_RTSI_CLOCK(x)	(NI_MIO_PLL_RTSI0_CLOCK + (x))
 
-/* Signals which can be routed to an NI RTSI pin with INSN_CONFIG_SET_ROUTING.
- The numbers assigned are not arbitrary, they correspond to the bits required
- to program the board. */
+/*
+ * Signals which can be routed to an NI RTSI pin with INSN_CONFIG_SET_ROUTING.
+ * The numbers assigned are not arbitrary, they correspond to the bits required
+ * to program the board.
+ */
 enum ni_rtsi_routing {
 	NI_RTSI_OUTPUT_ADR_START1 = 0,
 	NI_RTSI_OUTPUT_ADR_START2 = 1,
@@ -763,17 +786,19 @@ enum ni_rtsi_routing {
 	NI_RTSI_OUTPUT_G_GATE0 = 6,
 	NI_RTSI_OUTPUT_RGOUT0 = 7,
 	NI_RTSI_OUTPUT_RTSI_BRD_0 = 8,
-	NI_RTSI_OUTPUT_RTSI_OSC = 12	/* pre-m-series always have RTSI
-					 * clock on line 7 */
+	/* Pre-m-series always have RTSI clock on line 7 */
+	NI_RTSI_OUTPUT_RTSI_OSC = 12
 };
 
 #define NI_RTSI_OUTPUT_RTSI_BRD(x)	(NI_RTSI_OUTPUT_RTSI_BRD_0 + (x))
 
-/* Signals which can be routed to an NI PFI pin on an m-series board with
+/*
+ * Signals which can be routed to an NI PFI pin on an m-series board with
  * INSN_CONFIG_SET_ROUTING.  These numbers are also returned by
  * INSN_CONFIG_GET_ROUTING on pre-m-series boards, even though their routing
  * cannot be changed.  The numbers assigned are not arbitrary, they correspond
- * to the bits required to program the board. */
+ * to the bits required to program the board.
+ */
 enum ni_pfi_routing {
 	NI_PFI_OUTPUT_PFI_DEFAULT = 0,
 	NI_PFI_OUTPUT_AI_START1 = 1,
@@ -803,20 +828,24 @@ enum ni_pfi_routing {
 
 #define NI_PFI_OUTPUT_RTSI(x)		(NI_PFI_OUTPUT_RTSI0 + (x))
 
-/* Signals which can be routed to output on a NI PFI pin on a 660x board
- with INSN_CONFIG_SET_ROUTING.  The numbers assigned are
- not arbitrary, they correspond to the bits required
- to program the board.  Lines 0 to 7 can only be set to
- NI_660X_PFI_OUTPUT_DIO.  Lines 32 to 39 can only be set to
- NI_660X_PFI_OUTPUT_COUNTER. */
+/*
+ * Signals which can be routed to output on a NI PFI pin on a 660x board
+ * with INSN_CONFIG_SET_ROUTING.  The numbers assigned are
+ * not arbitrary, they correspond to the bits required
+ * to program the board.  Lines 0 to 7 can only be set to
+ * NI_660X_PFI_OUTPUT_DIO.  Lines 32 to 39 can only be set to
+ * NI_660X_PFI_OUTPUT_COUNTER.
+ */
 enum ni_660x_pfi_routing {
 	NI_660X_PFI_OUTPUT_COUNTER = 1,	/* counter */
 	NI_660X_PFI_OUTPUT_DIO = 2,	/* static digital output */
 };
 
-/* NI External Trigger lines.  These values are not arbitrary, but are related
+/*
+ * NI External Trigger lines.  These values are not arbitrary, but are related
  * to the bits required to program the board (offset by 1 for historical
- * reasons). */
+ * reasons).
+ */
 #define NI_EXT_PFI(x)			(NI_USUAL_PFI_SELECT(x) - 1)
 #define NI_EXT_RTSI(x)			(NI_USUAL_RTSI_SELECT(x) - 1)
 
@@ -827,9 +856,11 @@ enum comedi_counter_status_flags {
 	COMEDI_COUNTER_TERMINAL_COUNT = 0x4,
 };
 
-/* Clock sources for CDIO subdevice on NI m-series boards.  Used as the
+/*
+ * Clock sources for CDIO subdevice on NI m-series boards.  Used as the
  * scan_begin_arg for a comedi_command. These sources may also be bitwise-or'd
- * with CR_INVERT to change polarity. */
+ * with CR_INVERT to change polarity.
+ */
 enum ni_m_series_cdio_scan_begin_src {
 	NI_CDIO_SCAN_BEGIN_SRC_GROUND = 0,
 	NI_CDIO_SCAN_BEGIN_SRC_AI_START = 18,
@@ -846,38 +877,50 @@ enum ni_m_series_cdio_scan_begin_src {
 #define NI_CDIO_SCAN_BEGIN_SRC_PFI(x)	NI_USUAL_PFI_SELECT(x)
 #define NI_CDIO_SCAN_BEGIN_SRC_RTSI(x)	NI_USUAL_RTSI_SELECT(x)
 
-/* scan_begin_src for scan_begin_arg==TRIG_EXT with analog output command on NI
+/*
+ * scan_begin_src for scan_begin_arg==TRIG_EXT with analog output command on NI
  * boards.  These scan begin sources can also be bitwise-or'd with CR_INVERT to
- * change polarity. */
+ * change polarity.
+ */
 #define NI_AO_SCAN_BEGIN_SRC_PFI(x)	NI_USUAL_PFI_SELECT(x)
 #define NI_AO_SCAN_BEGIN_SRC_RTSI(x)	NI_USUAL_RTSI_SELECT(x)
 
-/* Bits for setting a clock source with
- * INSN_CONFIG_SET_CLOCK_SRC when using NI frequency output subdevice. */
+/*
+ * Bits for setting a clock source with
+ * INSN_CONFIG_SET_CLOCK_SRC when using NI frequency output subdevice.
+ */
 enum ni_freq_out_clock_source_bits {
 	NI_FREQ_OUT_TIMEBASE_1_DIV_2_CLOCK_SRC,	/* 10 MHz */
 	NI_FREQ_OUT_TIMEBASE_2_CLOCK_SRC	/* 100 KHz */
 };
 
-/* Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
- * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+/*
+ * Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver).
+ */
 enum amplc_dio_clock_source {
-	AMPLC_DIO_CLK_CLKN,	/* per channel external clock
-				   input/output pin (pin is only an
-				   input when clock source set to this
-				   value, otherwise it is an output) */
+	/*
+	 * Per channel external clock
+	 * input/output pin (pin is only an
+	 * input when clock source set to this value,
+	 * otherwise it is an output)
+	 */
+	AMPLC_DIO_CLK_CLKN,
 	AMPLC_DIO_CLK_10MHZ,	/* 10 MHz internal clock */
 	AMPLC_DIO_CLK_1MHZ,	/* 1 MHz internal clock */
 	AMPLC_DIO_CLK_100KHZ,	/* 100 kHz internal clock */
 	AMPLC_DIO_CLK_10KHZ,	/* 10 kHz internal clock */
 	AMPLC_DIO_CLK_1KHZ,	/* 1 kHz internal clock */
-	AMPLC_DIO_CLK_OUTNM1,	/* output of preceding counter channel
-				   (for channel 0, preceding counter
-				   channel is channel 2 on preceding
-				   counter subdevice, for first counter
-				   subdevice, preceding counter
-				   subdevice is the last counter
-				   subdevice) */
+	/*
+	 * Output of preceding counter channel
+	 * (for channel 0, preceding counter
+	 * channel is channel 2 on preceding
+	 * counter subdevice, for first counter
+	 * subdevice, preceding counter
+	 * subdevice is the last counter
+	 * subdevice)
+	 */
+	AMPLC_DIO_CLK_OUTNM1,
 	AMPLC_DIO_CLK_EXT,	/* per chip external input pin */
 	/* the following are "enhanced" clock sources for PCIe models */
 	AMPLC_DIO_CLK_VCC,	/* clock input HIGH */
@@ -886,35 +929,39 @@ enum amplc_dio_clock_source {
 	AMPLC_DIO_CLK_20MHZ	/* 20 MHz internal clock */
 };
 
-/* Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
- * timer subdevice on some Amplicon DIO PCIe boards (amplc_dio200 driver). */
+/*
+ * Values for setting a clock source with INSN_CONFIG_SET_CLOCK_SRC for
+ * timer subdevice on some Amplicon DIO PCIe boards (amplc_dio200 driver).
+ */
 enum amplc_dio_ts_clock_src {
 	AMPLC_DIO_TS_CLK_1GHZ,	/* 1 ns period with 20 ns granularity */
 	AMPLC_DIO_TS_CLK_1MHZ,	/* 1 us period */
 	AMPLC_DIO_TS_CLK_1KHZ	/* 1 ms period */
 };
 
-/* Values for setting a gate source with INSN_CONFIG_SET_GATE_SRC for
- * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver). */
+/*
+ * Values for setting a gate source with INSN_CONFIG_SET_GATE_SRC for
+ * 8254 counter subdevices on Amplicon DIO boards (amplc_dio200 driver).
+ */
 enum amplc_dio_gate_source {
 	AMPLC_DIO_GAT_VCC,	/* internal high logic level */
 	AMPLC_DIO_GAT_GND,	/* internal low logic level */
 	AMPLC_DIO_GAT_GATN,	/* per channel external gate input */
-	AMPLC_DIO_GAT_NOUTNM2,	/* negated output of counter channel
-				   minus 2 (for channels 0 or 1,
-				   channel minus 2 is channel 1 or 2 on
-				   the preceding counter subdevice, for
-				   the first counter subdevice the
-				   preceding counter subdevice is the
-				   last counter subdevice) */
+	/*
+	 * negated output of counter channel minus 2
+	 * (for channels 0 or 1, channel minus 2 is channel 1 or 2 on
+	 * the preceding counter subdevice, for the first counter subdevice
+	 * the preceding counter subdevice is the last counter subdevice)
+	 */
+	AMPLC_DIO_GAT_NOUTNM2,
 	AMPLC_DIO_GAT_RESERVED4,
 	AMPLC_DIO_GAT_RESERVED5,
 	AMPLC_DIO_GAT_RESERVED6,
 	AMPLC_DIO_GAT_RESERVED7,
 	/* the following are "enhanced" gate sources for PCIe models */
 	AMPLC_DIO_GAT_NGATN = 6, /* negated per channel gate input */
-	AMPLC_DIO_GAT_OUTNM2,	/* non-negated output of counter
-				   channel minus 2 */
+	/* non-negated output of counter channel minus 2 */
+	AMPLC_DIO_GAT_OUTNM2,
 	AMPLC_DIO_GAT_PAT_PRESENT, /* "pattern present" signal */
 	AMPLC_DIO_GAT_PAT_OCCURRED, /* "pattern occurred" latched */
 	AMPLC_DIO_GAT_PAT_GONE,	/* "pattern gone away" latched */
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 7b4af519e17e..d57fadef47fc 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -2303,11 +2303,13 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 {
 	struct comedi_subdevice *s;
 	struct comedi_async *async;
-	int n, m, count = 0, retval = 0;
+	unsigned int n, m;
+	ssize_t count = 0;
+	int retval = 0;
 	DECLARE_WAITQUEUE(wait, current);
 	struct comedi_file *cfp = file->private_data;
 	struct comedi_device *dev = cfp->dev;
-	bool on_wait_queue = false;
+	bool become_nonbusy = false;
 	bool attach_locked;
 	unsigned int old_detach_count;
 
@@ -2329,74 +2331,33 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 	}
 
 	async = s->async;
-
-	if (!s->busy || !nbytes)
-		goto out;
-	if (s->busy != file) {
-		retval = -EACCES;
-		goto out;
-	}
-	if (!(async->cmd.flags & CMDF_WRITE)) {
+	if (s->busy != file || !(async->cmd.flags & CMDF_WRITE)) {
 		retval = -EINVAL;
 		goto out;
 	}
 
 	add_wait_queue(&async->wait_head, &wait);
-	on_wait_queue = true;
-	while (nbytes > 0 && !retval) {
+	while (count == 0 && !retval) {
 		unsigned runflags;
+		unsigned int wp, n1, n2;
 
 		set_current_state(TASK_INTERRUPTIBLE);
 
 		runflags = comedi_get_subdevice_runflags(s);
 		if (!comedi_is_runflags_running(runflags)) {
-			if (count == 0) {
-				struct comedi_subdevice *new_s;
-
-				if (comedi_is_runflags_in_error(runflags))
-					retval = -EPIPE;
-				else
-					retval = 0;
-				/*
-				 * To avoid deadlock, cannot acquire dev->mutex
-				 * while dev->attach_lock is held.  Need to
-				 * remove task from the async wait queue before
-				 * releasing dev->attach_lock, as it might not
-				 * be valid afterwards.
-				 */
-				remove_wait_queue(&async->wait_head, &wait);
-				on_wait_queue = false;
-				up_read(&dev->attach_lock);
-				attach_locked = false;
-				mutex_lock(&dev->mutex);
-				/*
-				 * Become non-busy unless things have changed
-				 * behind our back.  Checking dev->detach_count
-				 * is unchanged ought to be sufficient (unless
-				 * there have been 2**32 detaches in the
-				 * meantime!), but check the subdevice pointer
-				 * as well just in case.
-				 */
-				new_s = comedi_file_write_subdevice(file);
-				if (dev->attached &&
-				    old_detach_count == dev->detach_count &&
-				    s == new_s && new_s->async == async)
-					do_become_nonbusy(dev, s);
-				mutex_unlock(&dev->mutex);
-			}
+			if (comedi_is_runflags_in_error(runflags))
+				retval = -EPIPE;
+			if (retval || nbytes)
+				become_nonbusy = true;
 			break;
 		}
+		if (nbytes == 0)
+			break;
 
-		n = nbytes;
-
-		m = n;
-		if (async->buf_write_ptr + m > async->prealloc_bufsz)
-			m = async->prealloc_bufsz - async->buf_write_ptr;
+		/* Allocate all free buffer space. */
 		comedi_buf_write_alloc(s, async->prealloc_bufsz);
-		if (m > comedi_buf_write_n_allocated(s))
-			m = comedi_buf_write_n_allocated(s);
-		if (m < n)
-			n = m;
+		m = comedi_buf_write_n_allocated(s);
+		n = min_t(size_t, m, nbytes);
 
 		if (n == 0) {
 			if (file->f_flags & O_NONBLOCK) {
@@ -2408,21 +2369,22 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 				retval = -ERESTARTSYS;
 				break;
 			}
-			if (!s->busy)
-				break;
-			if (s->busy != file) {
-				retval = -EACCES;
-				break;
-			}
-			if (!(async->cmd.flags & CMDF_WRITE)) {
+			if (s->busy != file ||
+			    !(async->cmd.flags & CMDF_WRITE)) {
 				retval = -EINVAL;
 				break;
 			}
 			continue;
 		}
 
-		m = copy_from_user(async->prealloc_buf + async->buf_write_ptr,
-				   buf, n);
+		wp = async->buf_write_ptr;
+		n1 = min(n, async->prealloc_bufsz - wp);
+		n2 = n - n1;
+		m = copy_from_user(async->prealloc_buf + wp, buf, n1);
+		if (m)
+			m += n2;
+		else if (n2)
+			m = copy_from_user(async->prealloc_buf, buf + n1, n2);
 		if (m) {
 			n -= m;
 			retval = -EFAULT;
@@ -2433,12 +2395,38 @@ static ssize_t comedi_write(struct file *file, const char __user *buf,
 		nbytes -= n;
 
 		buf += n;
-		break;		/* makes device work like a pipe */
 	}
-out:
-	if (on_wait_queue)
-		remove_wait_queue(&async->wait_head, &wait);
+	remove_wait_queue(&async->wait_head, &wait);
 	set_current_state(TASK_RUNNING);
+	if (become_nonbusy && count == 0) {
+		struct comedi_subdevice *new_s;
+
+		/*
+		 * To avoid deadlock, cannot acquire dev->mutex
+		 * while dev->attach_lock is held.
+		 */
+		up_read(&dev->attach_lock);
+		attach_locked = false;
+		mutex_lock(&dev->mutex);
+		/*
+		 * Check device hasn't become detached behind our back.
+		 * Checking dev->detach_count is unchanged ought to be
+		 * sufficient (unless there have been 2**32 detaches in the
+		 * meantime!), but check the subdevice pointer as well just in
+		 * case.
+		 *
+		 * Also check the subdevice is still in a suitable state to
+		 * become non-busy in case it changed behind our back.
+		 */
+		new_s = comedi_file_write_subdevice(file);
+		if (dev->attached && old_detach_count == dev->detach_count &&
+		    s == new_s && new_s->async == async && s->busy == file &&
+		    (async->cmd.flags & CMDF_WRITE) &&
+		    !comedi_is_subdevice_running(s))
+			do_become_nonbusy(dev, s);
+		mutex_unlock(&dev->mutex);
+	}
+out:
 	if (attach_locked)
 		up_read(&dev->attach_lock);
 
diff --git a/drivers/staging/comedi/comedilib.h b/drivers/staging/comedi/comedilib.h
index 56baf852ecf5..f9b56396e161 100644
--- a/drivers/staging/comedi/comedilib.h
+++ b/drivers/staging/comedi/comedilib.h
@@ -1,20 +1,20 @@
 /*
-    linux/include/comedilib.h
-    header file for kcomedilib
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-*/
+ * comedilib.h
+ * Header file for kcomedilib
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998-2001 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
 
 #ifndef _LINUX_COMEDILIB_H
 #define _LINUX_COMEDILIB_H
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index c3b8f2d7611b..0c8cfa738727 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -78,8 +78,10 @@ obj-$(CONFIG_COMEDI_ADL_PCI8164)	+= adl_pci8164.o
 obj-$(CONFIG_COMEDI_ADL_PCI9111)	+= adl_pci9111.o
 obj-$(CONFIG_COMEDI_ADL_PCI9118)	+= adl_pci9118.o
 obj-$(CONFIG_COMEDI_ADV_PCI1710)	+= adv_pci1710.o
+obj-$(CONFIG_COMEDI_ADV_PCI1720)	+= adv_pci1720.o
 obj-$(CONFIG_COMEDI_ADV_PCI1723)	+= adv_pci1723.o
 obj-$(CONFIG_COMEDI_ADV_PCI1724)	+= adv_pci1724.o
+obj-$(CONFIG_COMEDI_ADV_PCI1760)	+= adv_pci1760.o
 obj-$(CONFIG_COMEDI_ADV_PCI_DIO)	+= adv_pci_dio.o
 obj-$(CONFIG_COMEDI_AMPLC_DIO200_PCI)	+= amplc_dio200_pci.o
 obj-$(CONFIG_COMEDI_AMPLC_PC236_PCI)	+= amplc_pci236.o
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index 0dff1dbb53fb..4437ea3abe8d 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -603,10 +603,11 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 	unsigned short *array = data;
 	unsigned int num_samples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned int i;
+	__be16 *barray = data;
 
 	for (i = 0; i < num_samples; i++) {
 		if (devpriv->usedma)
-			array[i] = be16_to_cpu(array[i]);
+			array[i] = be16_to_cpu(barray[i]);
 		if (s->maxdata == 0xffff)
 			array[i] ^= 0x8000;
 		else
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index 399c511cfe0a..2c1b6de30da8 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -11,8 +11,9 @@
  * Driver: adv_pci1710
  * Description: Comedi driver for Advantech PCI-1710 series boards
  * Devices: [Advantech] PCI-1710 (adv_pci1710), PCI-1710HG, PCI-1711,
- *   PCI-1713, PCI-1720, PCI-1731
+ *   PCI-1713, PCI-1731
  * Author: Michal Dobes <dobes@tesnet.cz>
+ * Updated: Fri, 29 Oct 2015 17:19:35 -0700
  * Status: works
  *
  * Configuration options: not applicable, uses PCI auto config
@@ -40,7 +41,13 @@
 #define PCI171X_AD_DATA_REG	0x00	/* R:   A/D data */
 #define PCI171X_SOFTTRG_REG	0x00	/* W:   soft trigger for A/D */
 #define PCI171X_RANGE_REG	0x02	/* W:   A/D gain/range register */
+#define PCI171X_RANGE_DIFF	BIT(5)
+#define PCI171X_RANGE_UNI	BIT(4)
+#define PCI171X_RANGE_GAIN(x)	(((x) & 0x7) << 0)
 #define PCI171X_MUX_REG		0x04	/* W:   A/D multiplexor control */
+#define PCI171X_MUX_CHANH(x)	(((x) & 0xf) << 8)
+#define PCI171X_MUX_CHANL(x)	(((x) & 0xf) << 0)
+#define PCI171X_MUX_CHAN(x)	(PCI171X_MUX_CHANH(x) | PCI171X_MUX_CHANL(x))
 #define PCI171X_STATUS_REG	0x06	/* R:   status register */
 #define PCI171X_STATUS_IRQ	BIT(11)	/* 1=IRQ occurred */
 #define PCI171X_STATUS_FF	BIT(10)	/* 1=FIFO is full, fatal error */
@@ -58,83 +65,58 @@
 #define PCI171X_CLRFIFO_REG	0x09	/* W:   clear FIFO */
 #define PCI171X_DA_REG(x)	(0x0a + ((x) * 2)) /* W:   D/A register */
 #define PCI171X_DAREF_REG	0x0e	/* W:   D/A reference control */
+#define PCI171X_DAREF(c, r)	(((r) & 0x3) << ((c) * 2))
+#define PCI171X_DAREF_MASK(c)	PCI171X_DAREF((c), 0x3)
 #define PCI171X_DI_REG		0x10	/* R:   digital inputs */
 #define PCI171X_DO_REG		0x10	/* W:   digital outputs */
 #define PCI171X_TIMER_BASE	0x18	/* R/W: 8254 timer */
 
-/*
- * PCI-1720 only has analog outputs and has a different
- * register map (dev->iobase)
- */
-#define PCI1720_DA_REG(x)	(0x00 + ((x) * 2)) /* W:   D/A registers */
-#define PCI1720_RANGE_REG	0x08	/* R/W: D/A range register */
-#define PCI1720_SYNC_REG	0x09	/* W:   D/A synchronized output */
-#define PCI1720_SYNC_CTRL_REG	0x0f	/* R/W: D/A synchronized control */
-#define PCI1720_SYNC_CTRL_SC0	BIT(0)	/* set synchronous output mode */
-
-static const struct comedi_lrange range_pci1710_3 = {
+static const struct comedi_lrange pci1710_ai_range = {
 	9, {
-		BIP_RANGE(5),
-		BIP_RANGE(2.5),
-		BIP_RANGE(1.25),
-		BIP_RANGE(0.625),
-		BIP_RANGE(10),
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2.5),
-		UNI_RANGE(1.25)
+		BIP_RANGE(5),		/* gain 1   (0x00) */
+		BIP_RANGE(2.5),		/* gain 2   (0x01) */
+		BIP_RANGE(1.25),	/* gain 4   (0x02) */
+		BIP_RANGE(0.625),	/* gain 8   (0x03) */
+		BIP_RANGE(10),		/* gain 0.5 (0x04) */
+		UNI_RANGE(10),		/* gain 1   (0x00 | UNI) */
+		UNI_RANGE(5),		/* gain 2   (0x01 | UNI) */
+		UNI_RANGE(2.5),		/* gain 4   (0x02 | UNI) */
+		UNI_RANGE(1.25)		/* gain 8   (0x03 | UNI) */
 	}
 };
 
-static const char range_codes_pci1710_3[] = { 0x00, 0x01, 0x02, 0x03, 0x04,
-					      0x10, 0x11, 0x12, 0x13 };
-
-static const struct comedi_lrange range_pci1710hg = {
+static const struct comedi_lrange pci1710hg_ai_range = {
 	12, {
-		BIP_RANGE(5),
-		BIP_RANGE(0.5),
-		BIP_RANGE(0.05),
-		BIP_RANGE(0.005),
-		BIP_RANGE(10),
-		BIP_RANGE(1),
-		BIP_RANGE(0.1),
-		BIP_RANGE(0.01),
-		UNI_RANGE(10),
-		UNI_RANGE(1),
-		UNI_RANGE(0.1),
-		UNI_RANGE(0.01)
+		BIP_RANGE(5),		/* gain 1    (0x00) */
+		BIP_RANGE(0.5),		/* gain 10   (0x01) */
+		BIP_RANGE(0.05),	/* gain 100  (0x02) */
+		BIP_RANGE(0.005),	/* gain 1000 (0x03) */
+		BIP_RANGE(10),		/* gain 0.5  (0x04) */
+		BIP_RANGE(1),		/* gain 5    (0x05) */
+		BIP_RANGE(0.1),		/* gain 50   (0x06) */
+		BIP_RANGE(0.01),	/* gain 500  (0x07) */
+		UNI_RANGE(10),		/* gain 1    (0x00 | UNI) */
+		UNI_RANGE(1),		/* gain 10   (0x01 | UNI) */
+		UNI_RANGE(0.1),		/* gain 100  (0x02 | UNI) */
+		UNI_RANGE(0.01)		/* gain 1000 (0x03 | UNI) */
 	}
 };
 
-static const char range_codes_pci1710hg[] = { 0x00, 0x01, 0x02, 0x03, 0x04,
-					      0x05, 0x06, 0x07, 0x10, 0x11,
-					      0x12, 0x13 };
-
-static const struct comedi_lrange range_pci17x1 = {
+static const struct comedi_lrange pci1711_ai_range = {
 	5, {
-		BIP_RANGE(10),
-		BIP_RANGE(5),
-		BIP_RANGE(2.5),
-		BIP_RANGE(1.25),
-		BIP_RANGE(0.625)
-	}
-};
-
-static const char range_codes_pci17x1[] = { 0x00, 0x01, 0x02, 0x03, 0x04 };
-
-static const struct comedi_lrange pci1720_ao_range = {
-	4, {
-		UNI_RANGE(5),
-		UNI_RANGE(10),
-		BIP_RANGE(5),
-		BIP_RANGE(10)
+		BIP_RANGE(10),		/* gain 1  (0x00) */
+		BIP_RANGE(5),		/* gain 2  (0x01) */
+		BIP_RANGE(2.5),		/* gain 4  (0x02) */
+		BIP_RANGE(1.25),	/* gain 8  (0x03) */
+		BIP_RANGE(0.625)	/* gain 16 (0x04) */
 	}
 };
 
 static const struct comedi_lrange pci171x_ao_range = {
-	2, {
-		UNI_RANGE(5),
-		UNI_RANGE(10)
+	3, {
+		UNI_RANGE(5),		/* internal -5V ref */
+		UNI_RANGE(10),		/* internal -10V ref */
+		RANGE_ext(0, 1)		/* external -Vref (+/-10V max) */
 	}
 };
 
@@ -143,82 +125,43 @@ enum pci1710_boardid {
 	BOARD_PCI1710HG,
 	BOARD_PCI1711,
 	BOARD_PCI1713,
-	BOARD_PCI1720,
 	BOARD_PCI1731,
 };
 
 struct boardtype {
-	const char *name;	/*  board name */
-	int n_aichan;		/*  num of A/D chans */
-	const struct comedi_lrange *rangelist_ai;	/*  rangelist for A/D */
-	const char *rangecode_ai;	/*  range codes for programming */
+	const char *name;
+	const struct comedi_lrange *ai_range;
+	unsigned int is_pci1711:1;
 	unsigned int is_pci1713:1;
-	unsigned int is_pci1720:1;
-	unsigned int has_irq:1;
-	unsigned int has_large_fifo:1;	/* 4K or 1K FIFO */
-	unsigned int has_diff_ai:1;
 	unsigned int has_ao:1;
-	unsigned int has_di_do:1;
-	unsigned int has_counter:1;
 };
 
 static const struct boardtype boardtypes[] = {
 	[BOARD_PCI1710] = {
 		.name		= "pci1710",
-		.n_aichan	= 16,
-		.rangelist_ai	= &range_pci1710_3,
-		.rangecode_ai	= range_codes_pci1710_3,
-		.has_irq	= 1,
-		.has_large_fifo	= 1,
-		.has_diff_ai	= 1,
+		.ai_range	= &pci1710_ai_range,
 		.has_ao		= 1,
-		.has_di_do	= 1,
-		.has_counter	= 1,
 	},
 	[BOARD_PCI1710HG] = {
 		.name		= "pci1710hg",
-		.n_aichan	= 16,
-		.rangelist_ai	= &range_pci1710hg,
-		.rangecode_ai	= range_codes_pci1710hg,
-		.has_irq	= 1,
-		.has_large_fifo	= 1,
-		.has_diff_ai	= 1,
+		.ai_range	= &pci1710hg_ai_range,
 		.has_ao		= 1,
-		.has_di_do	= 1,
-		.has_counter	= 1,
 	},
 	[BOARD_PCI1711] = {
 		.name		= "pci1711",
-		.n_aichan	= 16,
-		.rangelist_ai	= &range_pci17x1,
-		.rangecode_ai	= range_codes_pci17x1,
-		.has_irq	= 1,
+		.ai_range	= &pci1711_ai_range,
+		.is_pci1711	= 1,
 		.has_ao		= 1,
-		.has_di_do	= 1,
-		.has_counter	= 1,
 	},
 	[BOARD_PCI1713] = {
 		.name		= "pci1713",
-		.n_aichan	= 32,
-		.rangelist_ai	= &range_pci1710_3,
-		.rangecode_ai	= range_codes_pci1710_3,
+		.ai_range	= &pci1710_ai_range,
 		.is_pci1713	= 1,
-		.has_irq	= 1,
-		.has_large_fifo	= 1,
-		.has_diff_ai	= 1,
-	},
-	[BOARD_PCI1720] = {
-		.name		= "pci1720",
-		.is_pci1720	= 1,
-		.has_ao		= 1,
 	},
 	[BOARD_PCI1731] = {
 		.name		= "pci1731",
-		.n_aichan	= 16,
-		.rangelist_ai	= &range_pci17x1,
-		.rangecode_ai	= range_codes_pci17x1,
-		.has_irq	= 1,
-		.has_di_do	= 1,
+		.ai_range	= &pci1711_ai_range,
+		.is_pci1711	= 1,
 	},
 };
 
@@ -226,14 +169,15 @@ struct pci1710_private {
 	unsigned int max_samples;
 	unsigned int ctrl;	/* control register value */
 	unsigned int ctrl_ext;	/* used to switch from TRIG_EXT to TRIG_xxx */
-	unsigned int mux_ext;	/* used to set the channel interval to scan */
+	unsigned int mux_scan;	/* used to set the channel interval to scan */
 	unsigned char ai_et;
 	unsigned int act_chanlist[32];	/*  list of scanned channel */
 	unsigned char saved_seglen;	/* len of the non-repeating chanlist */
 	unsigned char da_ranges;	/*  copy of D/A outpit range register */
+	unsigned char unipolar_gain;	/* adjust for unipolar gain codes */
 };
 
-static int pci171x_ai_check_chanlist(struct comedi_device *dev,
+static int pci1710_ai_check_chanlist(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     struct comedi_cmd *cmd)
 {
@@ -299,13 +243,12 @@ static int pci171x_ai_check_chanlist(struct comedi_device *dev,
 	return 0;
 }
 
-static void pci171x_ai_setup_chanlist(struct comedi_device *dev,
+static void pci1710_ai_setup_chanlist(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
 				      unsigned int *chanlist,
 				      unsigned int n_chan,
 				      unsigned int seglen)
 {
-	const struct boardtype *board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 	unsigned int first_chan = CR_CHAN(chanlist[0]);
 	unsigned int last_chan = CR_CHAN(chanlist[seglen - 1]);
@@ -315,14 +258,18 @@ static void pci171x_ai_setup_chanlist(struct comedi_device *dev,
 		unsigned int chan = CR_CHAN(chanlist[i]);
 		unsigned int range = CR_RANGE(chanlist[i]);
 		unsigned int aref = CR_AREF(chanlist[i]);
-		unsigned int rangeval;
+		unsigned int rangeval = 0;
 
-		rangeval = board->rangecode_ai[range];
 		if (aref == AREF_DIFF)
-			rangeval |= 0x0020;
+			rangeval |= PCI171X_RANGE_DIFF;
+		if (comedi_range_is_unipolar(s, range)) {
+			rangeval |= PCI171X_RANGE_UNI;
+			range -= devpriv->unipolar_gain;
+		}
+		rangeval |= PCI171X_RANGE_GAIN(range);
 
 		/* select channel and set range */
-		outw(chan | (chan << 8), dev->iobase + PCI171X_MUX_REG);
+		outw(PCI171X_MUX_CHAN(chan), dev->iobase + PCI171X_MUX_REG);
 		outw(rangeval, dev->iobase + PCI171X_RANGE_REG);
 
 		devpriv->act_chanlist[i] = chan;
@@ -331,11 +278,12 @@ static void pci171x_ai_setup_chanlist(struct comedi_device *dev,
 		devpriv->act_chanlist[i] = CR_CHAN(chanlist[i]);
 
 	/* select channel interval to scan */
-	devpriv->mux_ext = first_chan | (last_chan << 8);
-	outw(devpriv->mux_ext, dev->iobase + PCI171X_MUX_REG);
+	devpriv->mux_scan = PCI171X_MUX_CHANL(first_chan) |
+			    PCI171X_MUX_CHANH(last_chan);
+	outw(devpriv->mux_scan, dev->iobase + PCI171X_MUX_REG);
 }
 
-static int pci171x_ai_eoc(struct comedi_device *dev,
+static int pci1710_ai_eoc(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  struct comedi_insn *insn,
 			  unsigned long context)
@@ -348,7 +296,7 @@ static int pci171x_ai_eoc(struct comedi_device *dev,
 	return -EBUSY;
 }
 
-static int pci171x_ai_read_sample(struct comedi_device *dev,
+static int pci1710_ai_read_sample(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  unsigned int cur_chan,
 				  unsigned int *val)
@@ -377,7 +325,7 @@ static int pci171x_ai_read_sample(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci171x_ai_insn_read(struct comedi_device *dev,
+static int pci1710_ai_insn_read(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn,
 				unsigned int *data)
@@ -386,13 +334,14 @@ static int pci171x_ai_insn_read(struct comedi_device *dev,
 	int ret = 0;
 	int i;
 
-	devpriv->ctrl &= PCI171X_CTRL_CNT0;
-	devpriv->ctrl |= PCI171X_CTRL_SW;	/*  set software trigger */
+	/* enable software trigger */
+	devpriv->ctrl |= PCI171X_CTRL_SW;
 	outw(devpriv->ctrl, dev->iobase + PCI171X_CTRL_REG);
+
 	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 
-	pci171x_ai_setup_chanlist(dev, s, &insn->chanspec, 1, 1);
+	pci1710_ai_setup_chanlist(dev, s, &insn->chanspec, 1, 1);
 
 	for (i = 0; i < insn->n; i++) {
 		unsigned int val;
@@ -400,111 +349,40 @@ static int pci171x_ai_insn_read(struct comedi_device *dev,
 		/* start conversion */
 		outw(0, dev->iobase + PCI171X_SOFTTRG_REG);
 
-		ret = comedi_timeout(dev, s, insn, pci171x_ai_eoc, 0);
+		ret = comedi_timeout(dev, s, insn, pci1710_ai_eoc, 0);
 		if (ret)
 			break;
 
-		ret = pci171x_ai_read_sample(dev, s, 0, &val);
+		ret = pci1710_ai_read_sample(dev, s, 0, &val);
 		if (ret)
 			break;
 
 		data[i] = val;
 	}
 
+	/* disable software trigger */
+	devpriv->ctrl &= ~PCI171X_CTRL_SW;
+	outw(devpriv->ctrl, dev->iobase + PCI171X_CTRL_REG);
+
 	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 
 	return ret ? ret : insn->n;
 }
 
-static int pci171x_ao_insn_write(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct pci1710_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val = s->readback[chan];
-	int i;
-
-	devpriv->da_ranges &= ~(1 << (chan << 1));
-	devpriv->da_ranges |= (range << (chan << 1));
-	outw(devpriv->da_ranges, dev->iobase + PCI171X_DAREF_REG);
-
-	for (i = 0; i < insn->n; i++) {
-		val = data[i];
-		outw(val, dev->iobase + PCI171X_DA_REG(chan));
-	}
-
-	s->readback[chan] = val;
-
-	return insn->n;
-}
-
-static int pci171x_di_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	data[1] = inw(dev->iobase + PCI171X_DI_REG);
-
-	return insn->n;
-}
-
-static int pci171x_do_insn_bits(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	if (comedi_dio_update_state(s, data))
-		outw(s->state, dev->iobase + PCI171X_DO_REG);
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
-static int pci1720_ao_insn_write(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct pci1710_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val;
-	int i;
-
-	val = devpriv->da_ranges & (~(0x03 << (chan << 1)));
-	val |= (range << (chan << 1));
-	if (val != devpriv->da_ranges) {
-		outb(val, dev->iobase + PCI1720_RANGE_REG);
-		devpriv->da_ranges = val;
-	}
-
-	val = s->readback[chan];
-	for (i = 0; i < insn->n; i++) {
-		val = data[i];
-		outw(val, dev->iobase + PCI1720_DA_REG(chan));
-		outb(0, dev->iobase + PCI1720_SYNC_REG); /* update outputs */
-	}
-
-	s->readback[chan] = val;
-
-	return insn->n;
-}
-
-static int pci171x_ai_cancel(struct comedi_device *dev,
+static int pci1710_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
 	struct pci1710_private *devpriv = dev->private;
 
-	devpriv->ctrl &= PCI171X_CTRL_CNT0;
-	devpriv->ctrl |= PCI171X_CTRL_SW;
-	/* reset any operations */
+	/* disable A/D triggers and interrupt sources */
+	devpriv->ctrl &= PCI171X_CTRL_CNT0;	/* preserve counter 0 clk src */
 	outw(devpriv->ctrl, dev->iobase + PCI171X_CTRL_REG);
+
+	/* disable pacer */
 	comedi_8254_pacer_enable(dev->pacer, 1, 2, false);
+
+	/* clear A/D FIFO and any pending interrutps */
 	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 
@@ -535,7 +413,7 @@ static void pci1710_handle_every_sample(struct comedi_device *dev,
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 
 	for (; !(inw(dev->iobase + PCI171X_STATUS_REG) & PCI171X_STATUS_FE);) {
-		ret = pci171x_ai_read_sample(dev, s, s->async->cur_chan, &val);
+		ret = pci1710_ai_read_sample(dev, s, s->async->cur_chan, &val);
 		if (ret) {
 			s->async->events |= COMEDI_CB_ERROR;
 			break;
@@ -579,7 +457,7 @@ static void pci1710_handle_fifo(struct comedi_device *dev,
 		unsigned int val;
 		int ret;
 
-		ret = pci171x_ai_read_sample(dev, s, s->async->cur_chan, &val);
+		ret = pci1710_ai_read_sample(dev, s, s->async->cur_chan, &val);
 		if (ret) {
 			s->async->events |= COMEDI_CB_ERROR;
 			break;
@@ -598,7 +476,7 @@ static void pci1710_handle_fifo(struct comedi_device *dev,
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 }
 
-static irqreturn_t interrupt_service_pci1710(int irq, void *d)
+static irqreturn_t pci1710_irq_handler(int irq, void *d)
 {
 	struct comedi_device *dev = d;
 	struct pci1710_private *devpriv = dev->private;
@@ -624,7 +502,7 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
 		outb(0, dev->iobase + PCI171X_CLRINT_REG);
 		/* no sample on this interrupt; reset the channel interval */
-		outw(devpriv->mux_ext, dev->iobase + PCI171X_MUX_REG);
+		outw(devpriv->mux_scan, dev->iobase + PCI171X_MUX_REG);
 		outw(devpriv->ctrl, dev->iobase + PCI171X_CTRL_REG);
 		comedi_8254_pacer_enable(dev->pacer, 1, 2, true);
 		return IRQ_HANDLED;
@@ -640,12 +518,12 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 	return IRQ_HANDLED;
 }
 
-static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
+static int pci1710_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
 	struct pci1710_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	pci171x_ai_setup_chanlist(dev, s, cmd->chanlist, cmd->chanlist_len,
+	pci1710_ai_setup_chanlist(dev, s, cmd->chanlist, cmd->chanlist_len,
 				  devpriv->saved_seglen);
 
 	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
@@ -681,7 +559,7 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-static int pci171x_ai_cmdtest(struct comedi_device *dev,
+static int pci1710_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
@@ -745,7 +623,7 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 
 	/* Step 5: check channel list */
 
-	err |= pci171x_ai_check_chanlist(dev, s, cmd);
+	err |= pci1710_ai_check_chanlist(dev, s, cmd);
 
 	if (err)
 		return 5;
@@ -753,7 +631,55 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci171x_insn_counter_config(struct comedi_device *dev,
+static int pci1710_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct pci1710_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int val = s->readback[chan];
+	int i;
+
+	devpriv->da_ranges &= ~PCI171X_DAREF_MASK(chan);
+	devpriv->da_ranges |= PCI171X_DAREF(chan, range);
+	outw(devpriv->da_ranges, dev->iobase + PCI171X_DAREF_REG);
+
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
+		outw(val, dev->iobase + PCI171X_DA_REG(chan));
+	}
+
+	s->readback[chan] = val;
+
+	return insn->n;
+}
+
+static int pci1710_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	data[1] = inw(dev->iobase + PCI171X_DI_REG);
+
+	return insn->n;
+}
+
+static int pci1710_do_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	if (comedi_dio_update_state(s, data))
+		outw(s->state, dev->iobase + PCI171X_DO_REG);
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int pci1710_counter_insn_config(struct comedi_device *dev,
 				       struct comedi_subdevice *s,
 				       struct comedi_insn *insn,
 				       unsigned int *data)
@@ -780,7 +706,7 @@ static int pci171x_insn_counter_config(struct comedi_device *dev,
 			data[2] = 0;
 		} else {
 			data[1] = 0;
-			data[2] = I8254_OSC_BASE_10MHZ;
+			data[2] = I8254_OSC_BASE_1MHZ;
 		}
 		break;
 	default:
@@ -790,56 +716,29 @@ static int pci171x_insn_counter_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int pci171x_reset(struct comedi_device *dev)
+static void pci1710_reset(struct comedi_device *dev)
 {
 	const struct boardtype *board = dev->board_ptr;
-	struct pci1710_private *devpriv = dev->private;
 
-	/* Software trigger, CNT0=external */
-	devpriv->ctrl = PCI171X_CTRL_SW | PCI171X_CTRL_CNT0;
-	/* reset any operations */
-	outw(devpriv->ctrl, dev->iobase + PCI171X_CTRL_REG);
+	/*
+	 * Disable A/D triggers and interrupt sources, set counter 0
+	 * to use internal 1 MHz clock.
+	 */
+	outw(0, dev->iobase + PCI171X_CTRL_REG);
+
+	/* clear A/D FIFO and any pending interrutps */
 	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
 	outb(0, dev->iobase + PCI171X_CLRINT_REG);
-	devpriv->da_ranges = 0;
+
 	if (board->has_ao) {
 		/* set DACs to 0..5V and outputs to 0V */
-		outb(devpriv->da_ranges, dev->iobase + PCI171X_DAREF_REG);
+		outb(0, dev->iobase + PCI171X_DAREF_REG);
 		outw(0, dev->iobase + PCI171X_DA_REG(0));
 		outw(0, dev->iobase + PCI171X_DA_REG(1));
 	}
-	outw(0, dev->iobase + PCI171X_DO_REG);	/*  digital outputs to 0 */
-	outb(0, dev->iobase + PCI171X_CLRFIFO_REG);
-	outb(0, dev->iobase + PCI171X_CLRINT_REG);
 
-	return 0;
-}
-
-static int pci1720_reset(struct comedi_device *dev)
-{
-	struct pci1710_private *devpriv = dev->private;
-	/* set synchronous output mode */
-	outb(PCI1720_SYNC_CTRL_SC0, dev->iobase + PCI1720_SYNC_CTRL_REG);
-	devpriv->da_ranges = 0xAA;
-	/* set all ranges to +/-5V and outputs to 0V */
-	outb(devpriv->da_ranges, dev->iobase + PCI1720_RANGE_REG);
-	outw(0x0800, dev->iobase + PCI1720_DA_REG(0));
-	outw(0x0800, dev->iobase + PCI1720_DA_REG(1));
-	outw(0x0800, dev->iobase + PCI1720_DA_REG(2));
-	outw(0x0800, dev->iobase + PCI1720_DA_REG(3));
-	outb(0, dev->iobase + PCI1720_SYNC_REG);	/* update outputs */
-
-	return 0;
-}
-
-static int pci1710_reset(struct comedi_device *dev)
-{
-	const struct boardtype *board = dev->board_ptr;
-
-	if (board->is_pci1720)
-		return pci1720_reset(dev);
-
-	return pci171x_reset(dev);
+	/* set digital outputs to 0 */
+	outw(0, dev->iobase + PCI171X_DO_REG);
 }
 
 static int pci1710_auto_attach(struct comedi_device *dev,
@@ -850,6 +749,7 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 	struct pci1710_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, subdev, n_subdevices;
+	int i;
 
 	if (context < ARRAY_SIZE(boardtypes))
 		board = &boardtypes[context];
@@ -872,15 +772,16 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 	if (!dev->pacer)
 		return -ENOMEM;
 
-	n_subdevices = 0;
-	if (board->n_aichan)
-		n_subdevices++;
+	n_subdevices = 1;	/* all boards have analog inputs */
 	if (board->has_ao)
 		n_subdevices++;
-	if (board->has_di_do)
-		n_subdevices += 2;
-	if (board->has_counter)
-		n_subdevices++;
+	if (!board->is_pci1713) {
+		/*
+		 * All other boards have digital inputs and outputs as
+		 * well as a user counter.
+		 */
+		n_subdevices += 3;
+	}
 
 	ret = comedi_alloc_subdevices(dev, n_subdevices);
 	if (ret)
@@ -888,8 +789,8 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 
 	pci1710_reset(dev);
 
-	if (board->has_irq && pcidev->irq) {
-		ret = request_irq(pcidev->irq, interrupt_service_pci1710,
+	if (pcidev->irq) {
+		ret = request_irq(pcidev->irq, pci1710_irq_handler,
 				  IRQF_SHARED, dev->board_name, dev);
 		if (ret == 0)
 			dev->irq = pcidev->irq;
@@ -897,109 +798,89 @@ static int pci1710_auto_attach(struct comedi_device *dev,
 
 	subdev = 0;
 
-	if (board->n_aichan) {
-		s = &dev->subdevices[subdev];
-		s->type		= COMEDI_SUBD_AI;
-		s->subdev_flags	= SDF_READABLE | SDF_COMMON | SDF_GROUND;
-		if (board->has_diff_ai)
-			s->subdev_flags	|= SDF_DIFF;
-		s->n_chan	= board->n_aichan;
-		s->maxdata	= 0x0fff;
-		s->range_table	= board->rangelist_ai;
-		s->insn_read	= pci171x_ai_insn_read;
-		if (dev->irq) {
-			dev->read_subdev = s;
-			s->subdev_flags	|= SDF_CMD_READ;
-			s->len_chanlist	= s->n_chan;
-			s->do_cmdtest	= pci171x_ai_cmdtest;
-			s->do_cmd	= pci171x_ai_cmd;
-			s->cancel	= pci171x_ai_cancel;
+	/* Analog Input subdevice */
+	s = &dev->subdevices[subdev++];
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND;
+	if (!board->is_pci1711)
+		s->subdev_flags	|= SDF_DIFF;
+	s->n_chan	= board->is_pci1713 ? 32 : 16;
+	s->maxdata	= 0x0fff;
+	s->range_table	= board->ai_range;
+	s->insn_read	= pci1710_ai_insn_read;
+	if (dev->irq) {
+		dev->read_subdev = s;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->len_chanlist	= s->n_chan;
+		s->do_cmdtest	= pci1710_ai_cmdtest;
+		s->do_cmd	= pci1710_ai_cmd;
+		s->cancel	= pci1710_ai_cancel;
+	}
+
+	/* find the value needed to adjust for unipolar gain codes */
+	for (i = 0; i < s->range_table->length; i++) {
+		if (comedi_range_is_unipolar(s, i)) {
+			devpriv->unipolar_gain = i;
+			break;
 		}
-		subdev++;
 	}
 
 	if (board->has_ao) {
-		s = &dev->subdevices[subdev];
+		/* Analog Output subdevice */
+		s = &dev->subdevices[subdev++];
 		s->type		= COMEDI_SUBD_AO;
-		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+		s->subdev_flags	= SDF_WRITABLE | SDF_GROUND;
+		s->n_chan	= 2;
 		s->maxdata	= 0x0fff;
-		if (board->is_pci1720) {
-			s->n_chan	= 4;
-			s->range_table	= &pci1720_ao_range;
-			s->insn_write	= pci1720_ao_insn_write;
-		} else {
-			s->n_chan	= 2;
-			s->range_table	= &pci171x_ao_range;
-			s->insn_write	= pci171x_ao_insn_write;
-		}
+		s->range_table	= &pci171x_ao_range;
+		s->insn_write	= pci1710_ao_insn_write;
 
 		ret = comedi_alloc_subdev_readback(s);
 		if (ret)
 			return ret;
-
-		/* initialize the readback values to match the board reset */
-		if (board->is_pci1720) {
-			int i;
-
-			for (i = 0; i < s->n_chan; i++)
-				s->readback[i] = 0x0800;
-		}
-
-		subdev++;
 	}
 
-	if (board->has_di_do) {
-		s = &dev->subdevices[subdev];
+	if (!board->is_pci1713) {
+		/* Digital Input subdevice */
+		s = &dev->subdevices[subdev++];
 		s->type		= COMEDI_SUBD_DI;
 		s->subdev_flags	= SDF_READABLE;
 		s->n_chan	= 16;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
-		s->insn_bits	= pci171x_di_insn_bits;
-		subdev++;
+		s->insn_bits	= pci1710_di_insn_bits;
 
-		s = &dev->subdevices[subdev];
+		/* Digital Output subdevice */
+		s = &dev->subdevices[subdev++];
 		s->type		= COMEDI_SUBD_DO;
 		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= 16;
 		s->maxdata	= 1;
 		s->range_table	= &range_digital;
-		s->insn_bits	= pci171x_do_insn_bits;
-		subdev++;
-	}
+		s->insn_bits	= pci1710_do_insn_bits;
 
-	/* Counter subdevice (8254) */
-	if (board->has_counter) {
-		s = &dev->subdevices[subdev];
+		/* Counter subdevice (8254) */
+		s = &dev->subdevices[subdev++];
 		comedi_8254_subdevice_init(s, dev->pacer);
 
-		dev->pacer->insn_config = pci171x_insn_counter_config;
+		dev->pacer->insn_config = pci1710_counter_insn_config;
 
 		/* counters 1 and 2 are used internally for the pacer */
 		comedi_8254_set_busy(dev->pacer, 1, true);
 		comedi_8254_set_busy(dev->pacer, 2, true);
-
-		subdev++;
 	}
 
 	/* max_samples is half the FIFO size (2 bytes/sample) */
-	devpriv->max_samples = (board->has_large_fifo) ? 2048 : 512;
+	devpriv->max_samples = (board->is_pci1711) ? 512 : 2048;
 
 	return 0;
 }
 
-static void pci1710_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		pci1710_reset(dev);
-	comedi_pci_detach(dev);
-}
-
 static struct comedi_driver adv_pci1710_driver = {
 	.driver_name	= "adv_pci1710",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci1710_auto_attach,
-	.detach		= pci1710_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static int adv_pci1710_pci_probe(struct pci_dev *dev,
@@ -1063,7 +944,6 @@ static const struct pci_device_id adv_pci1710_pci_table[] = {
 	},
 	{ PCI_VDEVICE(ADVANTECH, 0x1711), BOARD_PCI1711 },
 	{ PCI_VDEVICE(ADVANTECH, 0x1713), BOARD_PCI1713 },
-	{ PCI_VDEVICE(ADVANTECH, 0x1720), BOARD_PCI1720 },
 	{ PCI_VDEVICE(ADVANTECH, 0x1731), BOARD_PCI1731 },
 	{ 0 }
 };
diff --git a/drivers/staging/comedi/drivers/adv_pci1720.c b/drivers/staging/comedi/drivers/adv_pci1720.c
new file mode 100644
index 000000000000..4830a1c93d15
--- /dev/null
+++ b/drivers/staging/comedi/drivers/adv_pci1720.c
@@ -0,0 +1,195 @@
+/*
+ * COMEDI driver for Advantech PCI-1720U
+ * Copyright (c) 2015 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * Separated from the adv_pci1710 driver written by:
+ * Michal Dobes <dobes@tesnet.cz>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * Driver: adv_pci1720
+ * Description: 4-channel Isolated D/A Output board
+ * Devices: [Advantech] PCI-7120U (adv_pci1720)
+ * Author: H Hartley Sweeten <hsweeten@visionengravers.com>
+ * Updated: Fri, 29 Oct 2015 17:19:35 -0700
+ * Status: untested
+ *
+ * Configuration options: not applicable, uses PCI auto config
+ *
+ * The PCI-1720 has 4 isolated 12-bit analog output channels with multiple
+ * output ranges. It also has a BoardID switch to allow differentiating
+ * multiple boards in the system.
+ *
+ * The analog outputs can operate in two modes, immediate and synchronized.
+ * This driver currently does not support the synchronized output mode.
+ *
+ * Jumpers JP1 to JP4 are used to set the current sink ranges for each
+ * analog output channel. In order to use the current sink ranges, the
+ * unipolar 5V range must be used. The voltage output and sink output for
+ * each channel is available on the connector as separate pins.
+ *
+ * Jumper JP5 controls the "hot" reset state of the analog outputs.
+ * Depending on its setting, the analog outputs will either keep the
+ * last settings and output values or reset to the default state after
+ * a "hot" reset. The default state for all channels is uniploar 5V range
+ * and all the output values are 0V. To allow this feature to work, the
+ * analog outputs are not "reset" when the driver attaches.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+
+#include "../comedi_pci.h"
+
+/*
+ * PCI BAR2 Register map (dev->iobase)
+ */
+#define PCI1720_AO_LSB_REG(x)		(0x00 + ((x) * 2))
+#define PCI1720_AO_MSB_REG(x)		(0x01 + ((x) * 2))
+#define PCI1720_AO_RANGE_REG		0x08
+#define PCI1720_AO_RANGE(c, r)		(((r) & 0x3) << ((c) * 2))
+#define PCI1720_AO_RANGE_MASK(c)	PCI1720_AO_RANGE((c), 0x3)
+#define PCI1720_SYNC_REG		0x09
+#define PCI1720_SYNC_CTRL_REG		0x0f
+#define PCI1720_SYNC_CTRL_SC0		BIT(0)
+#define PCI1720_BOARDID_REG		0x14
+
+static const struct comedi_lrange pci1720_ao_range = {
+	4, {
+		UNI_RANGE(5),
+		UNI_RANGE(10),
+		BIP_RANGE(5),
+		BIP_RANGE(10)
+	}
+};
+
+static int pci1720_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int val;
+	int i;
+
+	/* set the channel range and polarity */
+	val = inb(dev->iobase + PCI1720_AO_RANGE_REG);
+	val &= ~PCI1720_AO_RANGE_MASK(chan);
+	val |= PCI1720_AO_RANGE(chan, range);
+	outb(val, dev->iobase + PCI1720_AO_RANGE_REG);
+
+	val = s->readback[chan];
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
+
+		outb(val & 0xff, dev->iobase + PCI1720_AO_LSB_REG(chan));
+		outb((val >> 8) & 0xff, dev->iobase + PCI1720_AO_MSB_REG(chan));
+
+		/* conversion time is 2us (500 kHz throughput) */
+		usleep_range(2, 100);
+	}
+
+	s->readback[chan] = val;
+
+	return insn->n;
+}
+
+static int pci1720_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	data[1] = inb(dev->iobase + PCI1720_BOARDID_REG);
+
+	return insn->n;
+}
+
+static int pci1720_auto_attach(struct comedi_device *dev,
+			       unsigned long context)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct comedi_subdevice *s;
+	int ret;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, 2);
+
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
+
+	/* Analog Output subdevice */
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 0x0fff;
+	s->range_table	= &pci1720_ao_range;
+	s->insn_write	= pci1720_ao_insn_write;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	/* Digital Input subdevice (BoardID SW1) */
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci1720_di_insn_bits;
+
+	/* disable synchronized output, channels update when written */
+	outb(0, dev->iobase + PCI1720_SYNC_CTRL_REG);
+
+	return 0;
+}
+
+static struct comedi_driver adv_pci1720_driver = {
+	.driver_name	= "adv_pci1720",
+	.module		= THIS_MODULE,
+	.auto_attach	= pci1720_auto_attach,
+	.detach		= comedi_pci_detach,
+};
+
+static int adv_pci1720_pci_probe(struct pci_dev *dev,
+				 const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &adv_pci1720_driver,
+				      id->driver_data);
+}
+
+static const struct pci_device_id adv_pci1720_pci_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1720) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, adv_pci1720_pci_table);
+
+static struct pci_driver adv_pci1720_pci_driver = {
+	.name		= "adv_pci1720",
+	.id_table	= adv_pci1720_pci_table,
+	.probe		= adv_pci1720_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
+};
+module_comedi_pci_driver(adv_pci1720_driver, adv_pci1720_pci_driver);
+
+MODULE_AUTHOR("H Hartley Sweeten <hsweeten@visionengravers.com>");
+MODULE_DESCRIPTION("Comedi driver for Advantech PCI-1720 Analog Output board");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adv_pci1760.c b/drivers/staging/comedi/drivers/adv_pci1760.c
new file mode 100644
index 000000000000..d7dd1e55e347
--- /dev/null
+++ b/drivers/staging/comedi/drivers/adv_pci1760.c
@@ -0,0 +1,432 @@
+/*
+ * COMEDI driver for the Advantech PCI-1760
+ * Copyright (C) 2015 H Hartley Sweeten <hsweeten@visionengravers.com>
+ *
+ * Based on the pci1760 support in the adv_pci_dio driver written by:
+ *	Michal Dobes <dobes@tesnet.cz>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * Driver: adv_pci1760
+ * Description: Advantech PCI-1760 Relay & Isolated Digital Input Card
+ * Devices: [Advantech] PCI-1760 (adv_pci1760)
+ * Author: H Hartley Sweeten <hsweeten@visionengravers.com>
+ * Updated: Fri, 13 Nov 2015 12:34:00 -0700
+ * Status: untested
+ *
+ * Configuration Options: not applicable, uses PCI auto config
+ */
+
+#include <linux/module.h>
+
+#include "../comedi_pci.h"
+
+/*
+ * PCI-1760 Register Map
+ *
+ * Outgoing Mailbox Bytes
+ * OMB3: Not used (must be 0)
+ * OMB2: The command code to the PCI-1760
+ * OMB1: The hi byte of the parameter for the command in OMB2
+ * OMB0: The lo byte of the parameter for the command in OMB2
+ *
+ * Incoming Mailbox Bytes
+ * IMB3: The Isolated Digital Input status (updated every 100us)
+ * IMB2: The current command (matches OMB2 when command is successful)
+ * IMB1: The hi byte of the feedback data for the command in OMB2
+ * IMB0: The lo byte of the feedback data for the command in OMB2
+ *
+ * Interrupt Control/Status
+ * INTCSR3: Not used (must be 0)
+ * INTCSR2: The interrupt status (read only)
+ * INTCSR1: Interrupt enable/disable
+ * INTCSR0: Not used (must be 0)
+ */
+#define PCI1760_OMB_REG(x)		(0x0c + (x))
+#define PCI1760_IMB_REG(x)		(0x1c + (x))
+#define PCI1760_INTCSR_REG(x)		(0x38 + (x))
+#define PCI1760_INTCSR1_IRQ_ENA		BIT(5)
+#define PCI1760_INTCSR2_OMB_IRQ		BIT(0)
+#define PCI1760_INTCSR2_IMB_IRQ		BIT(1)
+#define PCI1760_INTCSR2_IRQ_STATUS	BIT(6)
+#define PCI1760_INTCSR2_IRQ_ASSERTED	BIT(7)
+
+/* PCI-1760 command codes */
+#define PCI1760_CMD_CLR_IMB2		0x00	/* Clears IMB2 */
+#define PCI1760_CMD_SET_DO		0x01	/* Set output state */
+#define PCI1760_CMD_GET_DO		0x02	/* Read output status */
+#define PCI1760_CMD_GET_STATUS		0x03	/* Read current status */
+#define PCI1760_CMD_GET_FW_VER		0x0e	/* Read firware version */
+#define PCI1760_CMD_GET_HW_VER		0x0f	/* Read hardware version */
+#define PCI1760_CMD_SET_PWM_HI(x)	(0x10 + (x) * 2) /* Set "hi" period */
+#define PCI1760_CMD_SET_PWM_LO(x)	(0x11 + (x) * 2) /* Set "lo" period */
+#define PCI1760_CMD_SET_PWM_CNT(x)	(0x14 + (x)) /* Set burst count */
+#define PCI1760_CMD_ENA_PWM		0x1f	/* Enable PWM outputs */
+#define PCI1760_CMD_ENA_FILT		0x20	/* Enable input filter */
+#define PCI1760_CMD_ENA_PAT_MATCH	0x21	/* Enable input pattern match */
+#define PCI1760_CMD_SET_PAT_MATCH	0x22	/* Set input pattern match */
+#define PCI1760_CMD_ENA_RISE_EDGE	0x23	/* Enable input rising edge */
+#define PCI1760_CMD_ENA_FALL_EDGE	0x24	/* Enable input falling edge */
+#define PCI1760_CMD_ENA_CNT		0x28	/* Enable counter */
+#define PCI1760_CMD_RST_CNT		0x29	/* Reset counter */
+#define PCI1760_CMD_ENA_CNT_OFLOW	0x2a	/* Enable counter overflow */
+#define PCI1760_CMD_ENA_CNT_MATCH	0x2b	/* Enable counter match */
+#define PCI1760_CMD_SET_CNT_EDGE	0x2c	/* Set counter edge */
+#define PCI1760_CMD_GET_CNT		0x2f	/* Reads counter value */
+#define PCI1760_CMD_SET_HI_SAMP(x)	(0x30 + (x)) /* Set "hi" sample time */
+#define PCI1760_CMD_SET_LO_SAMP(x)	(0x38 + (x)) /* Set "lo" sample time */
+#define PCI1760_CMD_SET_CNT(x)		(0x40 + (x)) /* Set counter reset val */
+#define PCI1760_CMD_SET_CNT_MATCH(x)	(0x48 + (x)) /* Set counter match val */
+#define PCI1760_CMD_GET_INT_FLAGS	0x60	/* Read interrupt flags */
+#define PCI1760_CMD_GET_INT_FLAGS_MATCH	BIT(0)
+#define PCI1760_CMD_GET_INT_FLAGS_COS	BIT(1)
+#define PCI1760_CMD_GET_INT_FLAGS_OFLOW	BIT(2)
+#define PCI1760_CMD_GET_OS		0x61	/* Read edge change flags */
+#define PCI1760_CMD_GET_CNT_STATUS	0x62	/* Read counter oflow/match */
+
+#define PCI1760_CMD_TIMEOUT		250	/* 250 usec timeout */
+#define PCI1760_CMD_RETRIES		3	/* limit number of retries */
+
+#define PCI1760_PWM_TIMEBASE		100000	/* 1 unit = 100 usec */
+
+static int pci1760_send_cmd(struct comedi_device *dev,
+			    unsigned char cmd, unsigned short val)
+{
+	unsigned long timeout;
+
+	/* send the command and parameter */
+	outb(val & 0xff, dev->iobase + PCI1760_OMB_REG(0));
+	outb((val >> 8) & 0xff, dev->iobase + PCI1760_OMB_REG(1));
+	outb(cmd, dev->iobase + PCI1760_OMB_REG(2));
+	outb(0, dev->iobase + PCI1760_OMB_REG(3));
+
+	/* datasheet says to allow up to 250 usec for the command to complete */
+	timeout = jiffies + usecs_to_jiffies(PCI1760_CMD_TIMEOUT);
+	do {
+		if (inb(dev->iobase + PCI1760_IMB_REG(2)) == cmd) {
+			/* command success; return the feedback data */
+			return inb(dev->iobase + PCI1760_IMB_REG(0)) |
+			       (inb(dev->iobase + PCI1760_IMB_REG(1)) << 8);
+		}
+		cpu_relax();
+	} while (time_before(jiffies, timeout));
+
+	return -EBUSY;
+}
+
+static int pci1760_cmd(struct comedi_device *dev,
+		       unsigned char cmd, unsigned short val)
+{
+	int repeats;
+	int ret;
+
+	/* send PCI1760_CMD_CLR_IMB2 between identical commands */
+	if (inb(dev->iobase + PCI1760_IMB_REG(2)) == cmd) {
+		ret = pci1760_send_cmd(dev, PCI1760_CMD_CLR_IMB2, 0);
+		if (ret < 0) {
+			/* timeout? try it once more */
+			ret = pci1760_send_cmd(dev, PCI1760_CMD_CLR_IMB2, 0);
+			if (ret < 0)
+				return -ETIMEDOUT;
+		}
+	}
+
+	/* datasheet says to keep retrying the command */
+	for (repeats = 0; repeats < PCI1760_CMD_RETRIES; repeats++) {
+		ret = pci1760_send_cmd(dev, cmd, val);
+		if (ret >= 0)
+			return ret;
+	}
+
+	/* command failed! */
+	return -ETIMEDOUT;
+}
+
+static int pci1760_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	data[1] = inb(dev->iobase + PCI1760_IMB_REG(3));
+
+	return insn->n;
+}
+
+static int pci1760_do_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	int ret;
+
+	if (comedi_dio_update_state(s, data)) {
+		ret = pci1760_cmd(dev, PCI1760_CMD_SET_DO, s->state);
+		if (ret < 0)
+			return ret;
+	}
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static int pci1760_pwm_ns_to_div(unsigned int flags, unsigned int ns)
+{
+	unsigned int divisor;
+
+	switch (flags) {
+	case CMDF_ROUND_NEAREST:
+		divisor = DIV_ROUND_CLOSEST(ns, PCI1760_PWM_TIMEBASE);
+		break;
+	case CMDF_ROUND_UP:
+		divisor = DIV_ROUND_UP(ns, PCI1760_PWM_TIMEBASE);
+		break;
+	case CMDF_ROUND_DOWN:
+		divisor = ns / PCI1760_PWM_TIMEBASE;
+	default:
+		return -EINVAL;
+	}
+
+	if (divisor < 1)
+		divisor = 1;
+	if (divisor > 0xffff)
+		divisor = 0xffff;
+
+	return divisor;
+}
+
+static int pci1760_pwm_enable(struct comedi_device *dev,
+			      unsigned int chan, bool enable)
+{
+	int ret;
+
+	ret = pci1760_cmd(dev, PCI1760_CMD_GET_STATUS, PCI1760_CMD_ENA_PWM);
+	if (ret < 0)
+		return ret;
+
+	if (enable)
+		ret |= BIT(chan);
+	else
+		ret &= ~BIT(chan);
+
+	return pci1760_cmd(dev, PCI1760_CMD_ENA_PWM, ret);
+}
+
+static int pci1760_pwm_insn_config(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int hi_div;
+	int lo_div;
+	int ret;
+
+	switch (data[0]) {
+	case INSN_CONFIG_ARM:
+		ret = pci1760_pwm_enable(dev, chan, false);
+		if (ret < 0)
+			return ret;
+
+		if (data[1] > 0xffff)
+			return -EINVAL;
+		ret = pci1760_cmd(dev, PCI1760_CMD_SET_PWM_CNT(chan), data[1]);
+		if (ret < 0)
+			return ret;
+
+		ret = pci1760_pwm_enable(dev, chan, true);
+		if (ret < 0)
+			return ret;
+		break;
+	case INSN_CONFIG_DISARM:
+		ret = pci1760_pwm_enable(dev, chan, false);
+		if (ret < 0)
+			return ret;
+		break;
+	case INSN_CONFIG_PWM_OUTPUT:
+		ret = pci1760_pwm_enable(dev, chan, false);
+		if (ret < 0)
+			return ret;
+
+		hi_div = pci1760_pwm_ns_to_div(data[1], data[2]);
+		lo_div = pci1760_pwm_ns_to_div(data[3], data[4]);
+		if (hi_div < 0 || lo_div < 0)
+			return -EINVAL;
+		if ((hi_div * PCI1760_PWM_TIMEBASE) != data[2] ||
+		    (lo_div * PCI1760_PWM_TIMEBASE) != data[4]) {
+			data[2] = hi_div * PCI1760_PWM_TIMEBASE;
+			data[4] = lo_div * PCI1760_PWM_TIMEBASE;
+			return -EAGAIN;
+		}
+		ret = pci1760_cmd(dev, PCI1760_CMD_SET_PWM_HI(chan), hi_div);
+		if (ret < 0)
+			return ret;
+		ret = pci1760_cmd(dev, PCI1760_CMD_SET_PWM_LO(chan), lo_div);
+		if (ret < 0)
+			return ret;
+		break;
+	case INSN_CONFIG_GET_PWM_OUTPUT:
+		hi_div = pci1760_cmd(dev, PCI1760_CMD_GET_STATUS,
+				     PCI1760_CMD_SET_PWM_HI(chan));
+		lo_div = pci1760_cmd(dev, PCI1760_CMD_GET_STATUS,
+				     PCI1760_CMD_SET_PWM_LO(chan));
+		if (hi_div < 0 || lo_div < 0)
+			return -ETIMEDOUT;
+
+		data[1] = hi_div * PCI1760_PWM_TIMEBASE;
+		data[2] = lo_div * PCI1760_PWM_TIMEBASE;
+		break;
+	case INSN_CONFIG_GET_PWM_STATUS:
+		ret = pci1760_cmd(dev, PCI1760_CMD_GET_STATUS,
+				  PCI1760_CMD_ENA_PWM);
+		if (ret < 0)
+			return ret;
+
+		data[1] = (ret & BIT(chan)) ? 1 : 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return insn->n;
+}
+
+static void pci1760_reset(struct comedi_device *dev)
+{
+	int i;
+
+	/* disable interrupts (intcsr2 is read-only) */
+	outb(0, dev->iobase + PCI1760_INTCSR_REG(0));
+	outb(0, dev->iobase + PCI1760_INTCSR_REG(1));
+	outb(0, dev->iobase + PCI1760_INTCSR_REG(3));
+
+	/* disable counters */
+	pci1760_cmd(dev, PCI1760_CMD_ENA_CNT, 0);
+
+	/* disable overflow interrupts */
+	pci1760_cmd(dev, PCI1760_CMD_ENA_CNT_OFLOW, 0);
+
+	/* disable match */
+	pci1760_cmd(dev, PCI1760_CMD_ENA_CNT_MATCH, 0);
+
+	/* set match and counter reset values */
+	for (i = 0; i < 8; i++) {
+		pci1760_cmd(dev, PCI1760_CMD_SET_CNT_MATCH(i), 0x8000);
+		pci1760_cmd(dev, PCI1760_CMD_SET_CNT(i), 0x0000);
+	}
+
+	/* reset counters to reset values */
+	pci1760_cmd(dev, PCI1760_CMD_RST_CNT, 0xff);
+
+	/* set counter count edges */
+	pci1760_cmd(dev, PCI1760_CMD_SET_CNT_EDGE, 0);
+
+	/* disable input filters */
+	pci1760_cmd(dev, PCI1760_CMD_ENA_FILT, 0);
+
+	/* disable pattern matching */
+	pci1760_cmd(dev, PCI1760_CMD_ENA_PAT_MATCH, 0);
+
+	/* set pattern match value */
+	pci1760_cmd(dev, PCI1760_CMD_SET_PAT_MATCH, 0);
+}
+
+static int pci1760_auto_attach(struct comedi_device *dev,
+			       unsigned long context)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct comedi_subdevice *s;
+	int ret;
+
+	ret = comedi_pci_enable(dev);
+	if (ret)
+		return ret;
+	dev->iobase = pci_resource_start(pcidev, 0);
+
+	pci1760_reset(dev);
+
+	ret = comedi_alloc_subdevices(dev, 4);
+	if (ret)
+		return ret;
+
+	/* Digital Input subdevice */
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 8;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci1760_di_insn_bits;
+
+	/* Digital Output subdevice */
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 8;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci1760_do_insn_bits;
+
+	/* get the current state of the outputs */
+	ret = pci1760_cmd(dev, PCI1760_CMD_GET_DO, 0);
+	if (ret < 0)
+		return ret;
+	s->state	= ret;
+
+	/* PWM subdevice */
+	s = &dev->subdevices[2];
+	s->type		= COMEDI_SUBD_PWM;
+	s->subdev_flags	= SDF_PWM_COUNTER;
+	s->n_chan	= 2;
+	s->insn_config	= pci1760_pwm_insn_config;
+
+	/* Counter subdevice */
+	s = &dev->subdevices[3];
+	s->type		= COMEDI_SUBD_UNUSED;
+
+	return 0;
+}
+
+static struct comedi_driver pci1760_driver = {
+	.driver_name	= "adv_pci1760",
+	.module		= THIS_MODULE,
+	.auto_attach	= pci1760_auto_attach,
+	.detach		= comedi_pci_detach,
+};
+
+static int pci1760_pci_probe(struct pci_dev *dev,
+			     const struct pci_device_id *id)
+{
+	return comedi_pci_auto_config(dev, &pci1760_driver, id->driver_data);
+}
+
+static const struct pci_device_id pci1760_pci_table[] = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_ADVANTECH, 0x1760) },
+	{ 0 }
+};
+MODULE_DEVICE_TABLE(pci, pci1760_pci_table);
+
+static struct pci_driver pci1760_pci_driver = {
+	.name		= "adv_pci1760",
+	.id_table	= pci1760_pci_table,
+	.probe		= pci1760_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
+};
+module_comedi_pci_driver(pci1760_driver, pci1760_pci_driver);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi driver for Advantech PCI-1760");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index f1b3c5aa8d79..620cec13d74c 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -4,30 +4,21 @@
  * Author: Michal Dobes <dobes@tesnet.cz>
  *
  *  Hardware driver for Advantech PCI DIO cards.
-*/
-/*
-Driver: adv_pci_dio
-Description: Advantech PCI-1730, PCI-1733, PCI-1734, PCI-1735U,
-	PCI-1736UP, PCI-1739U, PCI-1750, PCI-1751, PCI-1752,
-	PCI-1753/E, PCI-1754, PCI-1756, PCI-1760, PCI-1762
-Author: Michal Dobes <dobes@tesnet.cz>
-Devices: [Advantech] PCI-1730 (adv_pci_dio), PCI-1733,
-  PCI-1734, PCI-1735U, PCI-1736UP, PCI-1739U, PCI-1750,
-  PCI-1751, PCI-1752, PCI-1753,
-  PCI-1753+PCI-1753E, PCI-1754, PCI-1756,
-  PCI-1760, PCI-1762
-Status: untested
-Updated: Mon, 09 Jan 2012 12:40:46 +0000
-
-This driver supports now only insn interface for DI/DO/DIO.
+ */
 
-Configuration options:
-  [0] - PCI bus of device (optional)
-  [1] - PCI slot of device (optional)
-	If bus/slot is not specified, the first available PCI
-	device will be used.
-
-*/
+/*
+ * Driver: adv_pci_dio
+ * Description: Advantech Digital I/O Cards
+ * Devices: [Advantech] PCI-1730 (adv_pci_dio), PCI-1733,
+ *   PCI-1734, PCI-1735U, PCI-1736UP, PCI-1739U, PCI-1750,
+ *   PCI-1751, PCI-1752, PCI-1753, PCI-1753+PCI-1753E,
+ *   PCI-1754, PCI-1756, PCI-1762
+ * Author: Michal Dobes <dobes@tesnet.cz>
+ * Updated: Mon, 09 Jan 2012 12:40:46 +0000
+ * Status: untested
+ *
+ * Configuration Options: not applicable, uses PCI auto config
+ */
 
 #include <linux/module.h>
 #include <linux/delay.h>
@@ -37,403 +28,199 @@ Configuration options:
 #include "8255.h"
 #include "comedi_8254.h"
 
-/* hardware types of the cards */
-enum hw_cards_id {
-	TYPE_PCI1730, TYPE_PCI1733, TYPE_PCI1734, TYPE_PCI1735, TYPE_PCI1736,
-	TYPE_PCI1739,
-	TYPE_PCI1750,
-	TYPE_PCI1751,
-	TYPE_PCI1752,
-	TYPE_PCI1753, TYPE_PCI1753E,
-	TYPE_PCI1754, TYPE_PCI1756,
-	TYPE_PCI1760,
-	TYPE_PCI1762
-};
-
-/* which I/O instructions to use */
-enum hw_io_access {
-	IO_8b, IO_16b
-};
-
-#define MAX_DI_SUBDEVS	2	/* max number of DI subdevices per card */
-#define MAX_DO_SUBDEVS	2	/* max number of DO subdevices per card */
-#define MAX_DIO_SUBDEVG	2	/* max number of DIO subdevices group per
-				 * card */
-
-#define PCIDIO_MAINREG	   2	/* main I/O region for all Advantech cards? */
-
-/* Register offset definitions */
-/*  Advantech PCI-1730/3/4 */
-#define PCI1730_IDI	   0	/* R:   Isolated digital input  0-15 */
-#define PCI1730_IDO	   0	/* W:   Isolated digital output 0-15 */
-#define PCI1730_DI	   2	/* R:   Digital input  0-15 */
-#define PCI1730_DO	   2	/* W:   Digital output 0-15 */
-#define PCI1733_IDI	   0	/* R:   Isolated digital input  0-31 */
-#define	PCI1730_3_INT_EN	0x08	/* R/W: enable/disable interrupts */
-#define	PCI1730_3_INT_RF	0x0c	/* R/W: set falling/raising edge for
-					 * interrupts */
-#define	PCI1730_3_INT_CLR	0x10	/* R/W: clear interrupts */
-#define PCI1734_IDO	   0	/* W:   Isolated digital output 0-31 */
-#define PCI173x_BOARDID	   4	/* R:   Board I/D switch for 1730/3/4 */
-
-/* Advantech PCI-1735U */
-#define PCI1735_DI	   0	/* R:   Digital input  0-31 */
-#define PCI1735_DO	   0	/* W:   Digital output 0-31 */
-#define PCI1735_C8254	   4	/* R/W: 8254 counter */
-#define PCI1735_BOARDID	   8    /* R:   Board I/D switch for 1735U */
-
-/*  Advantech PCI-1736UP */
-#define PCI1736_IDI        0	/* R:   Isolated digital input  0-15 */
-#define PCI1736_IDO        0	/* W:   Isolated digital output 0-15 */
-#define PCI1736_3_INT_EN        0x08	/* R/W: enable/disable interrupts */
-#define PCI1736_3_INT_RF        0x0c	/* R/W: set falling/raising edge for
-					 * interrupts */
-#define PCI1736_3_INT_CLR       0x10	/* R/W: clear interrupts */
-#define PCI1736_BOARDID    4	/* R:   Board I/D switch for 1736UP */
-#define PCI1736_MAINREG    0	/* Normal register (2) doesn't work */
+/*
+ * Register offset definitions
+ */
 
-/* Advantech PCI-1739U */
-#define PCI1739_DIO	   0	/* R/W: begin of 8255 registers block */
-#define PCI1739_ICR	  32	/* W:   Interrupt control register */
-#define PCI1739_ISR	  32	/* R:   Interrupt status register */
-#define PCI1739_BOARDID	   8    /* R:   Board I/D switch for 1739U */
+/* PCI-1730, PCI-1733, PCI-1736 interrupt control registers */
+#define PCI173X_INT_EN_REG	0x08	/* R/W: enable/disable */
+#define PCI173X_INT_RF_REG	0x0c	/* R/W: falling/rising edge */
+#define PCI173X_INT_CLR_REG	0x10	/* R/W: clear */
 
-/*  Advantech PCI-1750 */
-#define PCI1750_IDI	   0	/* R:   Isolated digital input  0-15 */
-#define PCI1750_IDO	   0	/* W:   Isolated digital output 0-15 */
-#define PCI1750_ICR	  32	/* W:   Interrupt control register */
-#define PCI1750_ISR	  32	/* R:   Interrupt status register */
+/* PCI-1739U, PCI-1750, PCI1751 interrupt control registers */
+#define PCI1750_INT_REG		0x20	/* R/W: status/control */
 
-/*  Advantech PCI-1751/3/3E */
-#define PCI1751_DIO	   0	/* R/W: begin of 8255 registers block */
-#define PCI1751_CNT	  24	/* R/W: begin of 8254 registers block */
-#define PCI1751_ICR	  32	/* W:   Interrupt control register */
-#define PCI1751_ISR	  32	/* R:   Interrupt status register */
-#define PCI1753_DIO	   0	/* R/W: begin of 8255 registers block */
-#define PCI1753_ICR0	  16	/* R/W: Interrupt control register group 0 */
-#define PCI1753_ICR1	  17	/* R/W: Interrupt control register group 1 */
-#define PCI1753_ICR2	  18	/* R/W: Interrupt control register group 2 */
-#define PCI1753_ICR3	  19	/* R/W: Interrupt control register group 3 */
-#define PCI1753E_DIO	  32	/* R/W: begin of 8255 registers block */
-#define PCI1753E_ICR0	  48	/* R/W: Interrupt control register group 0 */
-#define PCI1753E_ICR1	  49	/* R/W: Interrupt control register group 1 */
-#define PCI1753E_ICR2	  50	/* R/W: Interrupt control register group 2 */
-#define PCI1753E_ICR3	  51	/* R/W: Interrupt control register group 3 */
+/* PCI-1753, PCI-1753E interrupt control registers */
+#define PCI1753_INT_REG(x)	(0x10 + (x)) /* R/W: control group 0 to 3 */
+#define PCI1753E_INT_REG(x)	(0x30 + (x)) /* R/W: control group 0 to 3 */
 
-/*  Advantech PCI-1752/4/6 */
-#define PCI1752_IDO	   0	/* R/W: Digital output  0-31 */
-#define PCI1752_IDO2	   4	/* R/W: Digital output 32-63 */
-#define PCI1754_IDI	   0	/* R:   Digital input   0-31 */
-#define PCI1754_IDI2	   4	/* R:   Digital input  32-64 */
-#define PCI1756_IDI	   0	/* R:   Digital input   0-31 */
-#define PCI1756_IDO	   4	/* R/W: Digital output  0-31 */
-#define PCI1754_6_ICR0	0x08	/* R/W: Interrupt control register group 0 */
-#define PCI1754_6_ICR1	0x0a	/* R/W: Interrupt control register group 1 */
-#define PCI1754_ICR2	0x0c	/* R/W: Interrupt control register group 2 */
-#define PCI1754_ICR3	0x0e	/* R/W: Interrupt control register group 3 */
-#define PCI1752_6_CFC	0x12	/* R/W: set/read channel freeze function */
-#define PCI175x_BOARDID	0x10	/* R:   Board I/D switch for 1752/4/6 */
+/* PCI-1754, PCI-1756 interrupt control registers */
+#define PCI1754_INT_REG(x)	(0x08 + (x) * 2) /* R/W: control group 0 to 3 */
 
-/*  Advantech PCI-1762 registers */
-#define PCI1762_RO	   0	/* R/W: Relays status/output */
-#define PCI1762_IDI	   2	/* R:   Isolated input status */
-#define PCI1762_BOARDID	   4	/* R:   Board I/D switch */
-#define PCI1762_ICR	   6	/* W:   Interrupt control register */
-#define PCI1762_ISR	   6	/* R:   Interrupt status register */
+/* PCI-1752, PCI-1756 special registers */
+#define PCI1752_CFC_REG		0x12	/* R/W: channel freeze function */
 
-/*  Advantech PCI-1760 registers */
-#define OMB0		0x0c	/* W:   Mailbox outgoing registers */
-#define OMB1		0x0d
-#define OMB2		0x0e
-#define OMB3		0x0f
-#define IMB0		0x1c	/* R:   Mailbox incoming registers */
-#define IMB1		0x1d
-#define IMB2		0x1e
-#define IMB3		0x1f
-#define INTCSR0		0x38	/* R/W: Interrupt control registers */
-#define INTCSR1		0x39
-#define INTCSR2		0x3a
-#define INTCSR3		0x3b
+/* PCI-1762 interrupt control registers */
+#define PCI1762_INT_REG		0x06	/* R/W: status/control */
 
-/*  PCI-1760 mailbox commands */
-#define CMD_ClearIMB2		0x00	/* Clear IMB2 status and return actual
-					 * DI status in IMB3 */
-#define CMD_SetRelaysOutput	0x01	/* Set relay output from OMB0 */
-#define CMD_GetRelaysStatus	0x02	/* Get relay status to IMB0 */
-#define CMD_ReadCurrentStatus	0x07	/* Read the current status of the
-					 * register in OMB0, result in IMB0 */
-#define CMD_ReadFirmwareVersion	0x0e	/* Read the firmware ver., result in
-					 * IMB1.IMB0 */
-#define CMD_ReadHardwareVersion	0x0f	/* Read the hardware ver., result in
-					 * IMB1.IMB0 */
-#define CMD_EnableIDIFilters	0x20	/* Enable IDI filters based on bits in
-					 * OMB0 */
-#define CMD_EnableIDIPatternMatch 0x21	/* Enable IDI pattern match based on
-					 * bits in OMB0 */
-#define CMD_SetIDIPatternMatch	0x22	/* Enable IDI pattern match based on
-					 * bits in OMB0 */
-#define CMD_EnableIDICounters	0x28	/* Enable IDI counters based on bits in
-					 * OMB0 */
-#define CMD_ResetIDICounters	0x29	/* Reset IDI counters based on bits in
-					 * OMB0 to its reset values */
-#define CMD_OverflowIDICounters	0x2a	/* Enable IDI counters overflow
-					 * interrupts  based on bits in OMB0 */
-#define CMD_MatchIntIDICounters	0x2b	/* Enable IDI counters match value
-					 * interrupts  based on bits in OMB0 */
-#define CMD_EdgeIDICounters	0x2c	/* Set IDI up counters count edge (bit=0
-					 * - rising, =1 - falling) */
-#define CMD_GetIDICntCurValue	0x2f	/* Read IDI{OMB0} up counter current
-					 * value */
-#define CMD_SetIDI0CntResetValue 0x40	/* Set IDI0 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI1CntResetValue 0x41	/* Set IDI1 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI2CntResetValue 0x42	/* Set IDI2 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI3CntResetValue 0x43	/* Set IDI3 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI4CntResetValue 0x44	/* Set IDI4 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI5CntResetValue 0x45	/* Set IDI5 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI6CntResetValue 0x46	/* Set IDI6 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI7CntResetValue 0x47	/* Set IDI7 Counter Reset Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI0CntMatchValue 0x48	/* Set IDI0 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI1CntMatchValue 0x49	/* Set IDI1 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI2CntMatchValue 0x4a	/* Set IDI2 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI3CntMatchValue 0x4b	/* Set IDI3 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI4CntMatchValue 0x4c	/* Set IDI4 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI5CntMatchValue 0x4d	/* Set IDI5 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI6CntMatchValue 0x4e	/* Set IDI6 Counter Match Value
-					 * 256*OMB1+OMB0 */
-#define CMD_SetIDI7CntMatchValue 0x4f	/* Set IDI7 Counter Match Value
-					 * 256*OMB1+OMB0 */
+/* maximum number of subdevice descriptions in the boardinfo */
+#define PCI_DIO_MAX_DI_SUBDEVS	2	/* 2 x 8/16/32 input channels max */
+#define PCI_DIO_MAX_DO_SUBDEVS	2	/* 2 x 8/16/32 output channels max */
+#define PCI_DIO_MAX_DIO_SUBDEVG	2	/* 2 x any number of 8255 devices max */
 
-#define OMBCMD_RETRY	0x03	/* 3 times try request before error */
+enum pci_dio_boardid {
+	TYPE_PCI1730,
+	TYPE_PCI1733,
+	TYPE_PCI1734,
+	TYPE_PCI1735,
+	TYPE_PCI1736,
+	TYPE_PCI1739,
+	TYPE_PCI1750,
+	TYPE_PCI1751,
+	TYPE_PCI1752,
+	TYPE_PCI1753,
+	TYPE_PCI1753E,
+	TYPE_PCI1754,
+	TYPE_PCI1756,
+	TYPE_PCI1762
+};
 
 struct diosubd_data {
-	int chans;		/*  num of chans */
-	int addr;		/*  PCI address ofset */
-	int regs;		/*  number of registers to read or 8255
-				    subdevices */
-	unsigned int specflags;	/*  addon subdevice flags */
+	int chans;		/*  num of chans or 8255 devices */
+	unsigned long addr;	/*  PCI address ofset */
 };
 
 struct dio_boardtype {
 	const char *name;	/*  board name */
-	int main_pci_region;	/*  main I/O PCI region */
-	enum hw_cards_id cardtype;
 	int nsubdevs;
-	struct diosubd_data sdi[MAX_DI_SUBDEVS];	/*  DI chans */
-	struct diosubd_data sdo[MAX_DO_SUBDEVS];	/*  DO chans */
-	struct diosubd_data sdio[MAX_DIO_SUBDEVG];	/*  DIO 8255 chans */
-	struct diosubd_data boardid;	/*  card supports board ID switch */
+	struct diosubd_data sdi[PCI_DIO_MAX_DI_SUBDEVS];
+	struct diosubd_data sdo[PCI_DIO_MAX_DO_SUBDEVS];
+	struct diosubd_data sdio[PCI_DIO_MAX_DIO_SUBDEVG];
+	unsigned long id_reg;
 	unsigned long timer_regbase;
-	enum hw_io_access io_access;
+	unsigned int is_16bit:1;
 };
 
 static const struct dio_boardtype boardtypes[] = {
 	[TYPE_PCI1730] = {
 		.name		= "pci1730",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1730,
 		.nsubdevs	= 5,
-		.sdi[0]		= { 16, PCI1730_DI, 2, 0, },
-		.sdi[1]		= { 16, PCI1730_IDI, 2, 0, },
-		.sdo[0]		= { 16, PCI1730_DO, 2, 0, },
-		.sdo[1]		= { 16, PCI1730_IDO, 2, 0, },
-		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_8b,
+		.sdi[0]		= { 16, 0x02, },	/* DI 0-15 */
+		.sdi[1]		= { 16, 0x00, },	/* ISO DI 0-15 */
+		.sdo[0]		= { 16, 0x02, },	/* DO 0-15 */
+		.sdo[1]		= { 16, 0x00, },	/* ISO DO 0-15 */
+		.id_reg		= 0x04,
 	},
 	[TYPE_PCI1733] = {
 		.name		= "pci1733",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1733,
 		.nsubdevs	= 2,
-		.sdi[1]		= { 32, PCI1733_IDI, 4, 0, },
-		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_8b,
+		.sdi[1]		= { 32, 0x00, },	/* ISO DI 0-31 */
+		.id_reg		= 0x04,
 	},
 	[TYPE_PCI1734] = {
 		.name		= "pci1734",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1734,
 		.nsubdevs	= 2,
-		.sdo[1]		= { 32, PCI1734_IDO, 4, 0, },
-		.boardid	= { 4, PCI173x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_8b,
+		.sdo[1]		= { 32, 0x00, },	/* ISO DO 0-31 */
+		.id_reg		= 0x04,
 	},
 	[TYPE_PCI1735] = {
 		.name		= "pci1735",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1735,
 		.nsubdevs	= 4,
-		.sdi[0]		= { 32, PCI1735_DI, 4, 0, },
-		.sdo[0]		= { 32, PCI1735_DO, 4, 0, },
-		.boardid	= { 4, PCI1735_BOARDID, 1, SDF_INTERNAL, },
-		.timer_regbase	= PCI1735_C8254,
-		.io_access	= IO_8b,
+		.sdi[0]		= { 32, 0x00, },	/* DI 0-31 */
+		.sdo[0]		= { 32, 0x00, },	/* DO 0-31 */
+		.id_reg		= 0x08,
+		.timer_regbase	= 0x04,
 	},
 	[TYPE_PCI1736] = {
 		.name		= "pci1736",
-		.main_pci_region = PCI1736_MAINREG,
-		.cardtype	= TYPE_PCI1736,
 		.nsubdevs	= 3,
-		.sdi[1]		= { 16, PCI1736_IDI, 2, 0, },
-		.sdo[1]		= { 16, PCI1736_IDO, 2, 0, },
-		.boardid	= { 4, PCI1736_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_8b,
+		.sdi[1]		= { 16, 0x00, },	/* ISO DI 0-15 */
+		.sdo[1]		= { 16, 0x00, },	/* ISO DO 0-15 */
+		.id_reg		= 0x04,
 	},
 	[TYPE_PCI1739] = {
 		.name		= "pci1739",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1739,
-		.nsubdevs	= 2,
-		.sdio[0]	= { 48, PCI1739_DIO, 2, 0, },
-		.io_access	= IO_8b,
+		.nsubdevs	= 3,
+		.sdio[0]	= { 2, 0x00, },		/* 8255 DIO */
+		.id_reg		= 0x08,
 	},
 	[TYPE_PCI1750] = {
 		.name		= "pci1750",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1750,
 		.nsubdevs	= 2,
-		.sdi[1]		= { 16, PCI1750_IDI, 2, 0, },
-		.sdo[1]		= { 16, PCI1750_IDO, 2, 0, },
-		.io_access	= IO_8b,
+		.sdi[1]		= { 16, 0x00, },	/* ISO DI 0-15 */
+		.sdo[1]		= { 16, 0x00, },	/* ISO DO 0-15 */
 	},
 	[TYPE_PCI1751] = {
 		.name		= "pci1751",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1751,
 		.nsubdevs	= 3,
-		.sdio[0]	= { 48, PCI1751_DIO, 2, 0, },
-		.timer_regbase	= PCI1751_CNT,
-		.io_access	= IO_8b,
+		.sdio[0]	= { 2, 0x00, },		/* 8255 DIO */
+		.timer_regbase	= 0x18,
 	},
 	[TYPE_PCI1752] = {
 		.name		= "pci1752",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1752,
 		.nsubdevs	= 3,
-		.sdo[0]		= { 32, PCI1752_IDO, 2, 0, },
-		.sdo[1]		= { 32, PCI1752_IDO2, 2, 0, },
-		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_16b,
+		.sdo[0]		= { 32, 0x00, },	/* DO 0-31 */
+		.sdo[1]		= { 32, 0x04, },	/* DO 32-63 */
+		.id_reg		= 0x10,
+		.is_16bit	= 1,
 	},
 	[TYPE_PCI1753] = {
 		.name		= "pci1753",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1753,
 		.nsubdevs	= 4,
-		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
-		.io_access	= IO_8b,
+		.sdio[0]	= { 4, 0x00, },		/* 8255 DIO */
 	},
 	[TYPE_PCI1753E] = {
 		.name		= "pci1753e",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1753E,
 		.nsubdevs	= 8,
-		.sdio[0]	= { 96, PCI1753_DIO, 4, 0, },
-		.sdio[1]	= { 96, PCI1753E_DIO, 4, 0, },
-		.io_access	= IO_8b,
+		.sdio[0]	= { 4, 0x00, },		/* 8255 DIO */
+		.sdio[1]	= { 4, 0x20, },		/* 8255 DIO */
 	},
 	[TYPE_PCI1754] = {
 		.name		= "pci1754",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1754,
 		.nsubdevs	= 3,
-		.sdi[0]		= { 32, PCI1754_IDI, 2, 0, },
-		.sdi[1]		= { 32, PCI1754_IDI2, 2, 0, },
-		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_16b,
+		.sdi[0]		= { 32, 0x00, },	/* DI 0-31 */
+		.sdi[1]		= { 32, 0x04, },	/* DI 32-63 */
+		.id_reg		= 0x10,
+		.is_16bit	= 1,
 	},
 	[TYPE_PCI1756] = {
 		.name		= "pci1756",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1756,
 		.nsubdevs	= 3,
-		.sdi[1]		= { 32, PCI1756_IDI, 2, 0, },
-		.sdo[1]		= { 32, PCI1756_IDO, 2, 0, },
-		.boardid	= { 4, PCI175x_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_16b,
-	},
-	[TYPE_PCI1760] = {
-		/* This card has its own 'attach' */
-		.name		= "pci1760",
-		.main_pci_region = 0,
-		.cardtype	= TYPE_PCI1760,
-		.nsubdevs	= 4,
-		.io_access	= IO_8b,
+		.sdi[1]		= { 32, 0x00, },	/* DI 0-31 */
+		.sdo[1]		= { 32, 0x04, },	/* DO 0-31 */
+		.id_reg		= 0x10,
+		.is_16bit	= 1,
 	},
 	[TYPE_PCI1762] = {
 		.name		= "pci1762",
-		.main_pci_region = PCIDIO_MAINREG,
-		.cardtype	= TYPE_PCI1762,
 		.nsubdevs	= 3,
-		.sdi[1]		= { 16, PCI1762_IDI, 1, 0, },
-		.sdo[1]		= { 16, PCI1762_RO, 1, 0, },
-		.boardid	= { 4, PCI1762_BOARDID, 1, SDF_INTERNAL, },
-		.io_access	= IO_16b,
+		.sdi[1]		= { 16, 0x02, },	/* ISO DI 0-15 */
+		.sdo[1]		= { 16, 0x00, },	/* ISO DO 0-15 */
+		.id_reg		= 0x04,
+		.is_16bit	= 1,
 	},
 };
 
-struct pci_dio_private {
-	char GlobalIrqEnabled;	/*  1= any IRQ source is enabled */
-	/*  PCI-1760 specific data */
-	unsigned char IDICntEnable;	/* counter's counting enable status */
-	unsigned char IDICntOverEnable;	/* counter's overflow interrupts enable
-					 * status */
-	unsigned char IDICntMatchEnable;	/* counter's match interrupts
-						 * enable status */
-	unsigned char IDICntEdge;	/* counter's count edge value
-					 * (bit=0 - rising, =1 - falling) */
-	unsigned short CntResValue[8];	/*  counters' reset value */
-	unsigned short CntMatchValue[8]; /*  counters' match interrupt value */
-	unsigned char IDIFiltersEn; /*  IDI's digital filters enable status */
-	unsigned char IDIPatMatchEn;	/*  IDI's pattern match enable status */
-	unsigned char IDIPatMatchValue;	/*  IDI's pattern match value */
-	unsigned short IDIFiltrLow[8];	/*  IDI's filter value low signal */
-	unsigned short IDIFiltrHigh[8];	/*  IDI's filter value high signal */
-};
-
-/*
-==============================================================================
-*/
 static int pci_dio_insn_bits_di_b(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	int i;
+	unsigned long reg = (unsigned long)s->private;
+	unsigned long iobase = dev->iobase + reg;
 
-	data[1] = 0;
-	for (i = 0; i < d->regs; i++)
-		data[1] |= inb(dev->iobase + d->addr + i) << (8 * i);
+	data[1] = inb(iobase);
+	if (s->n_chan > 8)
+		data[1] |= (inb(iobase + 1) << 8);
+	if (s->n_chan > 16)
+		data[1] |= (inb(iobase + 2) << 16);
+	if (s->n_chan > 24)
+		data[1] |= (inb(iobase + 3) << 24);
 
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
 static int pci_dio_insn_bits_di_w(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	int i;
+	unsigned long reg = (unsigned long)s->private;
+	unsigned long iobase = dev->iobase + reg;
 
-	data[1] = 0;
-	for (i = 0; i < d->regs; i++)
-		data[1] |= inw(dev->iobase + d->addr + 2 * i) << (16 * i);
+	data[1] = inw(iobase);
+	if (s->n_chan > 16)
+		data[1] |= (inw(iobase + 2) << 16);
 
 	return insn->n;
 }
@@ -443,13 +230,17 @@ static int pci_dio_insn_bits_do_b(struct comedi_device *dev,
 				  struct comedi_insn *insn,
 				  unsigned int *data)
 {
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	int i;
+	unsigned long reg = (unsigned long)s->private;
+	unsigned long iobase = dev->iobase + reg;
 
 	if (comedi_dio_update_state(s, data)) {
-		for (i = 0; i < d->regs; i++)
-			outb((s->state >> (8 * i)) & 0xff,
-			     dev->iobase + d->addr + i);
+		outb(s->state & 0xff, iobase);
+		if (s->n_chan > 8)
+			outb((s->state >> 8) & 0xff, iobase + 1);
+		if (s->n_chan > 16)
+			outb((s->state >> 16) & 0xff, iobase + 2);
+		if (s->n_chan > 24)
+			outb((s->state >> 24) & 0xff, iobase + 3);
 	}
 
 	data[1] = s->state;
@@ -462,13 +253,13 @@ static int pci_dio_insn_bits_do_w(struct comedi_device *dev,
 				  struct comedi_insn *insn,
 				  unsigned int *data)
 {
-	const struct diosubd_data *d = (const struct diosubd_data *)s->private;
-	int i;
+	unsigned long reg = (unsigned long)s->private;
+	unsigned long iobase = dev->iobase + reg;
 
 	if (comedi_dio_update_state(s, data)) {
-		for (i = 0; i < d->regs; i++)
-			outw((s->state >> (16 * i)) & 0xffff,
-			     dev->iobase + d->addr + 2 * i);
+		outw(s->state & 0xffff, iobase);
+		if (s->n_chan > 16)
+			outw((s->state >> 16) & 0xffff, iobase + 2);
 	}
 
 	data[1] = s->state;
@@ -476,510 +267,64 @@ static int pci_dio_insn_bits_do_w(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
-==============================================================================
-*/
-static int pci1760_unchecked_mbxrequest(struct comedi_device *dev,
-					unsigned char *omb, unsigned char *imb,
-					int repeats)
-{
-	int cnt, tout, ok = 0;
-
-	for (cnt = 0; cnt < repeats; cnt++) {
-		outb(omb[0], dev->iobase + OMB0);
-		outb(omb[1], dev->iobase + OMB1);
-		outb(omb[2], dev->iobase + OMB2);
-		outb(omb[3], dev->iobase + OMB3);
-		for (tout = 0; tout < 251; tout++) {
-			imb[2] = inb(dev->iobase + IMB2);
-			if (imb[2] == omb[2]) {
-				imb[0] = inb(dev->iobase + IMB0);
-				imb[1] = inb(dev->iobase + IMB1);
-				imb[3] = inb(dev->iobase + IMB3);
-				ok = 1;
-				break;
-			}
-			udelay(1);
-		}
-		if (ok)
-			return 0;
-	}
-
-	dev_err(dev->class_dev, "PCI-1760 mailbox request timeout!\n");
-	return -ETIME;
-}
-
-static int pci1760_clear_imb2(struct comedi_device *dev)
-{
-	unsigned char omb[4] = { 0x0, 0x0, CMD_ClearIMB2, 0x0 };
-	unsigned char imb[4];
-	/* check if imb2 is already clear */
-	if (inb(dev->iobase + IMB2) == CMD_ClearIMB2)
-		return 0;
-	return pci1760_unchecked_mbxrequest(dev, omb, imb, OMBCMD_RETRY);
-}
-
-static int pci1760_mbxrequest(struct comedi_device *dev,
-			      unsigned char *omb, unsigned char *imb)
-{
-	if (omb[2] == CMD_ClearIMB2) {
-		dev_err(dev->class_dev,
-			"bug! this function should not be used for CMD_ClearIMB2 command\n");
-		return -EINVAL;
-	}
-	if (inb(dev->iobase + IMB2) == omb[2]) {
-		int retval;
-
-		retval = pci1760_clear_imb2(dev);
-		if (retval < 0)
-			return retval;
-	}
-	return pci1760_unchecked_mbxrequest(dev, omb, imb, OMBCMD_RETRY);
-}
-
-/*
-==============================================================================
-*/
-static int pci1760_insn_bits_di(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+static int pci_dio_reset(struct comedi_device *dev, unsigned long cardtype)
 {
-	data[1] = inb(dev->iobase + IMB3);
+	/* disable channel freeze function on the PCI-1752/1756 boards */
+	if (cardtype == TYPE_PCI1752 || cardtype == TYPE_PCI1756)
+		outw(0, dev->iobase + PCI1752_CFC_REG);
 
-	return insn->n;
-}
-
-static int pci1760_insn_bits_do(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	int ret;
-	unsigned char omb[4] = {
-		0x00,
-		0x00,
-		CMD_SetRelaysOutput,
-		0x00
-	};
-	unsigned char imb[4];
-
-	if (comedi_dio_update_state(s, data)) {
-		omb[0] = s->state;
-		ret = pci1760_mbxrequest(dev, omb, imb);
-		if (!ret)
-			return ret;
-	}
-
-	data[1] = s->state;
-
-	return insn->n;
-}
-
-/*
-==============================================================================
-*/
-static int pci1760_insn_cnt_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
-{
-	int ret, n;
-	unsigned char omb[4] = {
-		CR_CHAN(insn->chanspec) & 0x07,
-		0x00,
-		CMD_GetIDICntCurValue,
-		0x00
-	};
-	unsigned char imb[4];
-
-	for (n = 0; n < insn->n; n++) {
-		ret = pci1760_mbxrequest(dev, omb, imb);
-		if (!ret)
-			return ret;
-		data[n] = (imb[1] << 8) + imb[0];
-	}
-
-	return n;
-}
-
-/*
-==============================================================================
-*/
-static int pci1760_insn_cnt_write(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci_dio_private *devpriv = dev->private;
-	int ret;
-	unsigned char chan = CR_CHAN(insn->chanspec) & 0x07;
-	unsigned char bitmask = 1 << chan;
-	unsigned char omb[4] = {
-		data[0] & 0xff,
-		(data[0] >> 8) & 0xff,
-		CMD_SetIDI0CntResetValue + chan,
-		0x00
-	};
-	unsigned char imb[4];
-
-	/* Set reset value if different */
-	if (devpriv->CntResValue[chan] != (data[0] & 0xffff)) {
-		ret = pci1760_mbxrequest(dev, omb, imb);
-		if (!ret)
-			return ret;
-		devpriv->CntResValue[chan] = data[0] & 0xffff;
-	}
-
-	omb[0] = bitmask;	/*  reset counter to it reset value */
-	omb[2] = CMD_ResetIDICounters;
-	ret = pci1760_mbxrequest(dev, omb, imb);
-	if (!ret)
-		return ret;
-
-	/*  start counter if it don't run */
-	if (!(bitmask & devpriv->IDICntEnable)) {
-		omb[0] = bitmask;
-		omb[2] = CMD_EnableIDICounters;
-		ret = pci1760_mbxrequest(dev, omb, imb);
-		if (!ret)
-			return ret;
-		devpriv->IDICntEnable |= bitmask;
-	}
-	return 1;
-}
-
-/*
-==============================================================================
-*/
-static int pci1760_reset(struct comedi_device *dev)
-{
-	struct pci_dio_private *devpriv = dev->private;
-	int i;
-	unsigned char omb[4] = { 0x00, 0x00, 0x00, 0x00 };
-	unsigned char imb[4];
-
-	outb(0, dev->iobase + INTCSR0);	/*  disable IRQ */
-	outb(0, dev->iobase + INTCSR1);
-	outb(0, dev->iobase + INTCSR2);
-	outb(0, dev->iobase + INTCSR3);
-	devpriv->GlobalIrqEnabled = 0;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_SetRelaysOutput;	/*  reset relay outputs */
-	pci1760_mbxrequest(dev, omb, imb);
-
-	omb[0] = 0x00;
-	omb[2] = CMD_EnableIDICounters;	/*  disable IDI up counters */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDICntEnable = 0;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_OverflowIDICounters; /* disable counters overflow
-					   * interrupts */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDICntOverEnable = 0;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_MatchIntIDICounters; /* disable counters match value
-					   * interrupts */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDICntMatchEnable = 0;
-
-	omb[0] = 0x00;
-	omb[1] = 0x80;
-	for (i = 0; i < 8; i++) {	/*  set IDI up counters match value */
-		omb[2] = CMD_SetIDI0CntMatchValue + i;
-		pci1760_mbxrequest(dev, omb, imb);
-		devpriv->CntMatchValue[i] = 0x8000;
-	}
-
-	omb[0] = 0x00;
-	omb[1] = 0x00;
-	for (i = 0; i < 8; i++) {	/*  set IDI up counters reset value */
-		omb[2] = CMD_SetIDI0CntResetValue + i;
-		pci1760_mbxrequest(dev, omb, imb);
-		devpriv->CntResValue[i] = 0x0000;
-	}
-
-	omb[0] = 0xff;
-	omb[2] = CMD_ResetIDICounters; /* reset IDI up counters to reset
-					* values */
-	pci1760_mbxrequest(dev, omb, imb);
-
-	omb[0] = 0x00;
-	omb[2] = CMD_EdgeIDICounters;	/*  set IDI up counters count edge */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDICntEdge = 0x00;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_EnableIDIFilters;	/*  disable all digital in filters */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDIFiltersEn = 0x00;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_EnableIDIPatternMatch;	/*  disable pattern matching */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDIPatMatchEn = 0x00;
-
-	omb[0] = 0x00;
-	omb[2] = CMD_SetIDIPatternMatch;	/*  set pattern match value */
-	pci1760_mbxrequest(dev, omb, imb);
-	devpriv->IDIPatMatchValue = 0x00;
-
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci_dio_reset(struct comedi_device *dev)
-{
-	const struct dio_boardtype *board = dev->board_ptr;
-
-	switch (board->cardtype) {
+	/* disable and clear interrupts */
+	switch (cardtype) {
 	case TYPE_PCI1730:
-		outb(0, dev->iobase + PCI1730_DO);	/*  clear outputs */
-		outb(0, dev->iobase + PCI1730_DO + 1);
-		outb(0, dev->iobase + PCI1730_IDO);
-		outb(0, dev->iobase + PCI1730_IDO + 1);
-		/* fallthrough */
 	case TYPE_PCI1733:
-		/* disable interrupts */
-		outb(0, dev->iobase + PCI1730_3_INT_EN);
-		/* clear interrupts */
-		outb(0x0f, dev->iobase + PCI1730_3_INT_CLR);
-		/* set rising edge trigger */
-		outb(0, dev->iobase + PCI1730_3_INT_RF);
-		break;
-	case TYPE_PCI1734:
-		outb(0, dev->iobase + PCI1734_IDO);	/*  clear outputs */
-		outb(0, dev->iobase + PCI1734_IDO + 1);
-		outb(0, dev->iobase + PCI1734_IDO + 2);
-		outb(0, dev->iobase + PCI1734_IDO + 3);
-		break;
-	case TYPE_PCI1735:
-		outb(0, dev->iobase + PCI1735_DO);	/*  clear outputs */
-		outb(0, dev->iobase + PCI1735_DO + 1);
-		outb(0, dev->iobase + PCI1735_DO + 2);
-		outb(0, dev->iobase + PCI1735_DO + 3);
-		break;
-
 	case TYPE_PCI1736:
-		outb(0, dev->iobase + PCI1736_IDO);
-		outb(0, dev->iobase + PCI1736_IDO + 1);
-		/* disable interrupts */
-		outb(0, dev->iobase + PCI1736_3_INT_EN);
-		/* clear interrupts */
-		outb(0x0f, dev->iobase + PCI1736_3_INT_CLR);
-		/* set rising edge trigger */
-		outb(0, dev->iobase + PCI1736_3_INT_RF);
+		outb(0, dev->iobase + PCI173X_INT_EN_REG);
+		outb(0x0f, dev->iobase + PCI173X_INT_CLR_REG);
+		outb(0, dev->iobase + PCI173X_INT_RF_REG);
 		break;
-
 	case TYPE_PCI1739:
-		/* disable & clear interrupts */
-		outb(0x88, dev->iobase + PCI1739_ICR);
-		break;
-
 	case TYPE_PCI1750:
 	case TYPE_PCI1751:
-		/* disable & clear interrupts */
-		outb(0x88, dev->iobase + PCI1750_ICR);
-		break;
-	case TYPE_PCI1752:
-		outw(0, dev->iobase + PCI1752_6_CFC); /* disable channel freeze
-						       * function */
-		outw(0, dev->iobase + PCI1752_IDO);	/*  clear outputs */
-		outw(0, dev->iobase + PCI1752_IDO + 2);
-		outw(0, dev->iobase + PCI1752_IDO2);
-		outw(0, dev->iobase + PCI1752_IDO2 + 2);
+		outb(0x88, dev->iobase + PCI1750_INT_REG);
 		break;
-	case TYPE_PCI1753E:
-		outb(0x88, dev->iobase + PCI1753E_ICR0); /* disable & clear
-							  * interrupts */
-		outb(0x80, dev->iobase + PCI1753E_ICR1);
-		outb(0x80, dev->iobase + PCI1753E_ICR2);
-		outb(0x80, dev->iobase + PCI1753E_ICR3);
-		/* fallthrough */
 	case TYPE_PCI1753:
-		outb(0x88, dev->iobase + PCI1753_ICR0); /* disable & clear
-							 * interrupts */
-		outb(0x80, dev->iobase + PCI1753_ICR1);
-		outb(0x80, dev->iobase + PCI1753_ICR2);
-		outb(0x80, dev->iobase + PCI1753_ICR3);
+	case TYPE_PCI1753E:
+		outb(0x88, dev->iobase + PCI1753_INT_REG(0));
+		outb(0x80, dev->iobase + PCI1753_INT_REG(1));
+		outb(0x80, dev->iobase + PCI1753_INT_REG(2));
+		outb(0x80, dev->iobase + PCI1753_INT_REG(3));
+		if (cardtype == TYPE_PCI1753E) {
+			outb(0x88, dev->iobase + PCI1753E_INT_REG(0));
+			outb(0x80, dev->iobase + PCI1753E_INT_REG(1));
+			outb(0x80, dev->iobase + PCI1753E_INT_REG(2));
+			outb(0x80, dev->iobase + PCI1753E_INT_REG(3));
+		}
 		break;
 	case TYPE_PCI1754:
-		outw(0x08, dev->iobase + PCI1754_6_ICR0); /* disable and clear
-							   * interrupts */
-		outw(0x08, dev->iobase + PCI1754_6_ICR1);
-		outw(0x08, dev->iobase + PCI1754_ICR2);
-		outw(0x08, dev->iobase + PCI1754_ICR3);
-		break;
 	case TYPE_PCI1756:
-		outw(0, dev->iobase + PCI1752_6_CFC); /* disable channel freeze
-						       * function */
-		outw(0x08, dev->iobase + PCI1754_6_ICR0); /* disable and clear
-							   * interrupts */
-		outw(0x08, dev->iobase + PCI1754_6_ICR1);
-		outw(0, dev->iobase + PCI1756_IDO);	/*  clear outputs */
-		outw(0, dev->iobase + PCI1756_IDO + 2);
-		break;
-	case TYPE_PCI1760:
-		pci1760_reset(dev);
+		outw(0x08, dev->iobase + PCI1754_INT_REG(0));
+		outw(0x08, dev->iobase + PCI1754_INT_REG(1));
+		if (cardtype == TYPE_PCI1754) {
+			outw(0x08, dev->iobase + PCI1754_INT_REG(2));
+			outw(0x08, dev->iobase + PCI1754_INT_REG(3));
+		}
 		break;
 	case TYPE_PCI1762:
-		outw(0x0101, dev->iobase + PCI1762_ICR); /* disable & clear
-							  * interrupts */
-		break;
-	}
-
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci1760_attach(struct comedi_device *dev)
-{
-	struct comedi_subdevice *s;
-
-	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->len_chanlist = 8;
-	s->range_table = &range_digital;
-	s->insn_bits = pci1760_insn_bits_di;
-
-	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 8;
-	s->maxdata = 1;
-	s->len_chanlist = 8;
-	s->range_table = &range_digital;
-	s->state = 0;
-	s->insn_bits = pci1760_insn_bits_do;
-
-	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_WRITABLE | SDF_LSAMPL;
-	s->n_chan = 2;
-	s->maxdata = 0xffffffff;
-	s->len_chanlist = 2;
-/*       s->insn_config=pci1760_insn_pwm_cfg; */
-
-	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_COUNTER;
-	s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-	s->n_chan = 8;
-	s->maxdata = 0xffff;
-	s->len_chanlist = 8;
-	s->insn_read = pci1760_insn_cnt_read;
-	s->insn_write = pci1760_insn_cnt_write;
-/*       s->insn_config=pci1760_insn_cnt_cfg; */
-
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci_dio_add_di(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  const struct diosubd_data *d)
-{
-	const struct dio_boardtype *board = dev->board_ptr;
-
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | d->specflags;
-	if (d->chans > 16)
-		s->subdev_flags |= SDF_LSAMPL;
-	s->n_chan = d->chans;
-	s->maxdata = 1;
-	s->len_chanlist = d->chans;
-	s->range_table = &range_digital;
-	switch (board->io_access) {
-	case IO_8b:
-		s->insn_bits = pci_dio_insn_bits_di_b;
-		break;
-	case IO_16b:
-		s->insn_bits = pci_dio_insn_bits_di_w;
-		break;
-	}
-	s->private = (void *)d;
-
-	return 0;
-}
-
-/*
-==============================================================================
-*/
-static int pci_dio_add_do(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  const struct diosubd_data *d)
-{
-	const struct dio_boardtype *board = dev->board_ptr;
-
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE;
-	if (d->chans > 16)
-		s->subdev_flags |= SDF_LSAMPL;
-	s->n_chan = d->chans;
-	s->maxdata = 1;
-	s->len_chanlist = d->chans;
-	s->range_table = &range_digital;
-	s->state = 0;
-	switch (board->io_access) {
-	case IO_8b:
-		s->insn_bits = pci_dio_insn_bits_do_b;
+		outw(0x0101, dev->iobase + PCI1762_INT_REG);
 		break;
-	case IO_16b:
-		s->insn_bits = pci_dio_insn_bits_do_w;
+	default:
 		break;
 	}
-	s->private = (void *)d;
 
 	return 0;
 }
 
-static unsigned long pci_dio_override_cardtype(struct pci_dev *pcidev,
-					       unsigned long cardtype)
-{
-	/*
-	 * Change cardtype from TYPE_PCI1753 to TYPE_PCI1753E if expansion
-	 * board available.  Need to enable PCI device and request the main
-	 * registers PCI BAR temporarily to perform the test.
-	 */
-	if (cardtype != TYPE_PCI1753)
-		return cardtype;
-	if (pci_enable_device(pcidev) < 0)
-		return cardtype;
-	if (pci_request_region(pcidev, PCIDIO_MAINREG, "adv_pci_dio") == 0) {
-		/*
-		 * This test is based on Advantech's "advdaq" driver source
-		 * (which declares its module licence as "GPL" although the
-		 * driver source does not include a "COPYING" file).
-		 */
-		unsigned long reg =
-			pci_resource_start(pcidev, PCIDIO_MAINREG) + 53;
-
-		outb(0x05, reg);
-		if ((inb(reg) & 0x07) == 0x02) {
-			outb(0x02, reg);
-			if ((inb(reg) & 0x07) == 0x05)
-				cardtype = TYPE_PCI1753E;
-		}
-		pci_release_region(pcidev, PCIDIO_MAINREG);
-	}
-	pci_disable_device(pcidev);
-	return cardtype;
-}
-
 static int pci_dio_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct dio_boardtype *board = NULL;
-	struct pci_dio_private *devpriv;
+	const struct diosubd_data *d;
 	struct comedi_subdevice *s;
 	int ret, subdev, i, j;
 
@@ -990,54 +335,93 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 	dev->board_ptr = board;
 	dev->board_name = board->name;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = pci_resource_start(pcidev, board->main_pci_region);
+	if (context == TYPE_PCI1736)
+		dev->iobase = pci_resource_start(pcidev, 0);
+	else
+		dev->iobase = pci_resource_start(pcidev, 2);
+
+	pci_dio_reset(dev, context);
 
 	ret = comedi_alloc_subdevices(dev, board->nsubdevs);
 	if (ret)
 		return ret;
 
 	subdev = 0;
-	for (i = 0; i < MAX_DI_SUBDEVS; i++)
-		if (board->sdi[i].chans) {
-			s = &dev->subdevices[subdev];
-			pci_dio_add_di(dev, s, &board->sdi[i]);
-			subdev++;
+	for (i = 0; i < PCI_DIO_MAX_DI_SUBDEVS; i++) {
+		d = &board->sdi[i];
+		if (d->chans) {
+			s = &dev->subdevices[subdev++];
+			s->type		= COMEDI_SUBD_DI;
+			s->subdev_flags	= SDF_READABLE;
+			s->n_chan	= d->chans;
+			s->maxdata	= 1;
+			s->range_table	= &range_digital;
+			s->insn_bits	= board->is_16bit
+						? pci_dio_insn_bits_di_w
+						: pci_dio_insn_bits_di_b;
+			s->private	= (void *)d->addr;
 		}
+	}
 
-	for (i = 0; i < MAX_DO_SUBDEVS; i++)
-		if (board->sdo[i].chans) {
-			s = &dev->subdevices[subdev];
-			pci_dio_add_do(dev, s, &board->sdo[i]);
-			subdev++;
+	for (i = 0; i < PCI_DIO_MAX_DO_SUBDEVS; i++) {
+		d = &board->sdo[i];
+		if (d->chans) {
+			s = &dev->subdevices[subdev++];
+			s->type		= COMEDI_SUBD_DO;
+			s->subdev_flags	= SDF_WRITABLE;
+			s->n_chan	= d->chans;
+			s->maxdata	= 1;
+			s->range_table	= &range_digital;
+			s->insn_bits	= board->is_16bit
+						? pci_dio_insn_bits_do_w
+						: pci_dio_insn_bits_do_b;
+			s->private	= (void *)d->addr;
+
+			/* reset all outputs to 0 */
+			if (board->is_16bit) {
+				outw(0, dev->iobase + d->addr);
+				if (s->n_chan > 16)
+					outw(0, dev->iobase + d->addr + 2);
+			} else {
+				outb(0, dev->iobase + d->addr);
+				if (s->n_chan > 8)
+					outb(0, dev->iobase + d->addr + 1);
+				if (s->n_chan > 16)
+					outb(0, dev->iobase + d->addr + 2);
+				if (s->n_chan > 24)
+					outb(0, dev->iobase + d->addr + 3);
+			}
 		}
+	}
 
-	for (i = 0; i < MAX_DIO_SUBDEVG; i++)
-		for (j = 0; j < board->sdio[i].regs; j++) {
-			s = &dev->subdevices[subdev];
+	for (i = 0; i < PCI_DIO_MAX_DIO_SUBDEVG; i++) {
+		d = &board->sdio[i];
+		for (j = 0; j < d->chans; j++) {
+			s = &dev->subdevices[subdev++];
 			ret = subdev_8255_init(dev, s, NULL,
-					       board->sdio[i].addr +
-					       j * I8255_SIZE);
+					       d->addr + j * I8255_SIZE);
 			if (ret)
 				return ret;
-			subdev++;
 		}
+	}
 
-	if (board->boardid.chans) {
-		s = &dev->subdevices[subdev];
-		s->type = COMEDI_SUBD_DI;
-		pci_dio_add_di(dev, s, &board->boardid);
-		subdev++;
+	if (board->id_reg) {
+		s = &dev->subdevices[subdev++];
+		s->type		= COMEDI_SUBD_DI;
+		s->subdev_flags	= SDF_READABLE | SDF_INTERNAL;
+		s->n_chan	= 4;
+		s->maxdata	= 1;
+		s->range_table	= &range_digital;
+		s->insn_bits	= board->is_16bit ? pci_dio_insn_bits_di_w
+						  : pci_dio_insn_bits_di_b;
+		s->private	= (void *)board->id_reg;
 	}
 
 	if (board->timer_regbase) {
-		s = &dev->subdevices[subdev];
+		s = &dev->subdevices[subdev++];
 
 		dev->pacer = comedi_8254_init(dev->iobase +
 					      board->timer_regbase,
@@ -1046,32 +430,50 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 			return -ENOMEM;
 
 		comedi_8254_subdevice_init(s, dev->pacer);
-
-		subdev++;
 	}
 
-	if (board->cardtype == TYPE_PCI1760)
-		pci1760_attach(dev);
-
-	pci_dio_reset(dev);
-
 	return 0;
 }
 
-static void pci_dio_detach(struct comedi_device *dev)
-{
-	if (dev->iobase)
-		pci_dio_reset(dev);
-	comedi_pci_detach(dev);
-}
-
 static struct comedi_driver adv_pci_dio_driver = {
 	.driver_name	= "adv_pci_dio",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci_dio_auto_attach,
-	.detach		= pci_dio_detach,
+	.detach		= comedi_pci_detach,
 };
 
+static unsigned long pci_dio_override_cardtype(struct pci_dev *pcidev,
+					       unsigned long cardtype)
+{
+	/*
+	 * Change cardtype from TYPE_PCI1753 to TYPE_PCI1753E if expansion
+	 * board available.  Need to enable PCI device and request the main
+	 * registers PCI BAR temporarily to perform the test.
+	 */
+	if (cardtype != TYPE_PCI1753)
+		return cardtype;
+	if (pci_enable_device(pcidev) < 0)
+		return cardtype;
+	if (pci_request_region(pcidev, 2, "adv_pci_dio") == 0) {
+		/*
+		 * This test is based on Advantech's "advdaq" driver source
+		 * (which declares its module licence as "GPL" although the
+		 * driver source does not include a "COPYING" file).
+		 */
+		unsigned long reg = pci_resource_start(pcidev, 2) + 53;
+
+		outb(0x05, reg);
+		if ((inb(reg) & 0x07) == 0x02) {
+			outb(0x02, reg);
+			if ((inb(reg) & 0x07) == 0x05)
+				cardtype = TYPE_PCI1753E;
+		}
+		pci_release_region(pcidev, 2);
+	}
+	pci_disable_device(pcidev);
+	return cardtype;
+}
+
 static int adv_pci_dio_pci_probe(struct pci_dev *dev,
 				 const struct pci_device_id *id)
 {
@@ -1094,7 +496,6 @@ static const struct pci_device_id adv_pci_dio_pci_table[] = {
 	{ PCI_VDEVICE(ADVANTECH, 0x1753), TYPE_PCI1753 },
 	{ PCI_VDEVICE(ADVANTECH, 0x1754), TYPE_PCI1754 },
 	{ PCI_VDEVICE(ADVANTECH, 0x1756), TYPE_PCI1756 },
-	{ PCI_VDEVICE(ADVANTECH, 0x1760), TYPE_PCI1760 },
 	{ PCI_VDEVICE(ADVANTECH, 0x1762), TYPE_PCI1762 },
 	{ 0 }
 };
@@ -1109,5 +510,5 @@ static struct pci_driver adv_pci_dio_pci_driver = {
 module_comedi_pci_driver(adv_pci_dio_driver, adv_pci_dio_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for Advantech Digital I/O Cards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index b2f7679a0116..cac011fdd375 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -1022,14 +1022,17 @@ pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 	irq = pci_dev->irq;
 
 	/* Allocate buffer to hold values for AO channel scan. */
-	devpriv->ao_scan_vals = kmalloc(sizeof(devpriv->ao_scan_vals[0]) *
-					board->ao_chans, GFP_KERNEL);
+	devpriv->ao_scan_vals = kmalloc_array(board->ao_chans,
+					      sizeof(devpriv->ao_scan_vals[0]),
+					      GFP_KERNEL);
 	if (!devpriv->ao_scan_vals)
 		return -ENOMEM;
 
 	/* Allocate buffer to hold AO channel scan order. */
-	devpriv->ao_scan_order = kmalloc(sizeof(devpriv->ao_scan_order[0]) *
-					 board->ao_chans, GFP_KERNEL);
+	devpriv->ao_scan_order =
+				kmalloc_array(board->ao_chans,
+					      sizeof(devpriv->ao_scan_order[0]),
+					      GFP_KERNEL);
 	if (!devpriv->ao_scan_order)
 		return -ENOMEM;
 
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index b00a36a5cb36..ccb37d1f0f8e 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -51,13 +51,13 @@
 
 /* DAC registers */
 #define CB_DDA_DA_CTRL_REG		0x00	   /* D/A Control Register  */
-#define CB_DDA_DA_CTRL_SU		(1 << 0)   /*  Simultaneous update  */
-#define CB_DDA_DA_CTRL_EN		(1 << 1)   /*  Enable specified DAC */
+#define CB_DDA_DA_CTRL_SU		BIT(0)   /*  Simultaneous update  */
+#define CB_DDA_DA_CTRL_EN		BIT(1)   /*  Enable specified DAC */
 #define CB_DDA_DA_CTRL_DAC(x)		((x) << 2) /*  Specify DAC channel  */
 #define CB_DDA_DA_CTRL_RANGE2V5		(0 << 6)   /*  2.5V range           */
 #define CB_DDA_DA_CTRL_RANGE5V		(2 << 6)   /*  5V range             */
 #define CB_DDA_DA_CTRL_RANGE10V		(3 << 6)   /*  10V range            */
-#define CB_DDA_DA_CTRL_UNIP		(1 << 8)   /*  Unipolar range       */
+#define CB_DDA_DA_CTRL_UNIP		BIT(8)   /*  Unipolar range       */
 
 #define DACALIBRATION1	4	/*  D/A CALIBRATION REGISTER 1 */
 /* write bits */
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index 15a4093efda1..1bf8ddc6f07c 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -75,8 +75,8 @@
 #define PARPORT_DATA_REG	0x00
 #define PARPORT_STATUS_REG	0x01
 #define PARPORT_CTRL_REG	0x02
-#define PARPORT_CTRL_IRQ_ENA	(1 << 4)
-#define PARPORT_CTRL_BIDIR_ENA	(1 << 5)
+#define PARPORT_CTRL_IRQ_ENA	BIT(4)
+#define PARPORT_CTRL_BIDIR_ENA	BIT(5)
 
 static int parport_data_reg_insn_bits(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 056bca9c67d5..fd8e0b76f764 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -801,9 +801,10 @@ static void das16_ai_munge(struct comedi_device *dev,
 	unsigned short *data = array;
 	unsigned int num_samples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned int i;
+	__le16 *buf = array;
 
 	for (i = 0; i < num_samples; i++) {
-		data[i] = le16_to_cpu(data[i]);
+		data[i] = le16_to_cpu(buf[i]);
 		if (s->maxdata == 0x0fff)
 			data[i] >>= 4;
 		data[i] &= s->maxdata;
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index 62a817e4cd64..84c62e256094 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -42,24 +42,24 @@
 #define NI6527_DO_REG(x)		(0x03 + (x))
 #define NI6527_ID_REG			0x06
 #define NI6527_CLR_REG			0x07
-#define NI6527_CLR_EDGE			(1 << 3)
-#define NI6527_CLR_OVERFLOW		(1 << 2)
-#define NI6527_CLR_FILT			(1 << 1)
-#define NI6527_CLR_INTERVAL		(1 << 0)
+#define NI6527_CLR_EDGE			BIT(3)
+#define NI6527_CLR_OVERFLOW		BIT(2)
+#define NI6527_CLR_FILT			BIT(1)
+#define NI6527_CLR_INTERVAL		BIT(0)
 #define NI6527_CLR_IRQS			(NI6527_CLR_EDGE | NI6527_CLR_OVERFLOW)
 #define NI6527_CLR_RESET_FILT		(NI6527_CLR_FILT | NI6527_CLR_INTERVAL)
 #define NI6527_FILT_INTERVAL_REG(x)	(0x08 + (x))
 #define NI6527_FILT_ENA_REG(x)		(0x0c + (x))
 #define NI6527_STATUS_REG		0x14
-#define NI6527_STATUS_IRQ		(1 << 2)
-#define NI6527_STATUS_OVERFLOW		(1 << 1)
-#define NI6527_STATUS_EDGE		(1 << 0)
+#define NI6527_STATUS_IRQ		BIT(2)
+#define NI6527_STATUS_OVERFLOW		BIT(1)
+#define NI6527_STATUS_EDGE		BIT(0)
 #define NI6527_CTRL_REG			0x15
-#define NI6527_CTRL_FALLING		(1 << 4)
-#define NI6527_CTRL_RISING		(1 << 3)
-#define NI6527_CTRL_IRQ			(1 << 2)
-#define NI6527_CTRL_OVERFLOW		(1 << 1)
-#define NI6527_CTRL_EDGE		(1 << 0)
+#define NI6527_CTRL_FALLING		BIT(4)
+#define NI6527_CTRL_RISING		BIT(3)
+#define NI6527_CTRL_IRQ			BIT(2)
+#define NI6527_CTRL_OVERFLOW		BIT(1)
+#define NI6527_CTRL_EDGE		BIT(0)
 #define NI6527_CTRL_DISABLE_IRQS	0
 #define NI6527_CTRL_ENABLE_IRQS		(NI6527_CTRL_FALLING | \
 					 NI6527_CTRL_RISING | \
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 800d57426070..251117be1205 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -68,25 +68,25 @@
 /* Non-recurring Registers (8-bit except where noted) */
 #define NI_65XX_ID_REG			0x00
 #define NI_65XX_CLR_REG			0x01
-#define NI_65XX_CLR_WDOG_INT		(1 << 6)
-#define NI_65XX_CLR_WDOG_PING		(1 << 5)
-#define NI_65XX_CLR_WDOG_EXP		(1 << 4)
-#define NI_65XX_CLR_EDGE_INT		(1 << 3)
-#define NI_65XX_CLR_OVERFLOW_INT	(1 << 2)
+#define NI_65XX_CLR_WDOG_INT		BIT(6)
+#define NI_65XX_CLR_WDOG_PING		BIT(5)
+#define NI_65XX_CLR_WDOG_EXP		BIT(4)
+#define NI_65XX_CLR_EDGE_INT		BIT(3)
+#define NI_65XX_CLR_OVERFLOW_INT	BIT(2)
 #define NI_65XX_STATUS_REG		0x02
-#define NI_65XX_STATUS_WDOG_INT		(1 << 5)
-#define NI_65XX_STATUS_FALL_EDGE	(1 << 4)
-#define NI_65XX_STATUS_RISE_EDGE	(1 << 3)
-#define NI_65XX_STATUS_INT		(1 << 2)
-#define NI_65XX_STATUS_OVERFLOW_INT	(1 << 1)
-#define NI_65XX_STATUS_EDGE_INT		(1 << 0)
+#define NI_65XX_STATUS_WDOG_INT		BIT(5)
+#define NI_65XX_STATUS_FALL_EDGE	BIT(4)
+#define NI_65XX_STATUS_RISE_EDGE	BIT(3)
+#define NI_65XX_STATUS_INT		BIT(2)
+#define NI_65XX_STATUS_OVERFLOW_INT	BIT(1)
+#define NI_65XX_STATUS_EDGE_INT		BIT(0)
 #define NI_65XX_CTRL_REG		0x03
-#define NI_65XX_CTRL_WDOG_ENA		(1 << 5)
-#define NI_65XX_CTRL_FALL_EDGE_ENA	(1 << 4)
-#define NI_65XX_CTRL_RISE_EDGE_ENA	(1 << 3)
-#define NI_65XX_CTRL_INT_ENA		(1 << 2)
-#define NI_65XX_CTRL_OVERFLOW_ENA	(1 << 1)
-#define NI_65XX_CTRL_EDGE_ENA		(1 << 0)
+#define NI_65XX_CTRL_WDOG_ENA		BIT(5)
+#define NI_65XX_CTRL_FALL_EDGE_ENA	BIT(4)
+#define NI_65XX_CTRL_RISE_EDGE_ENA	BIT(3)
+#define NI_65XX_CTRL_INT_ENA		BIT(2)
+#define NI_65XX_CTRL_OVERFLOW_ENA	BIT(1)
+#define NI_65XX_CTRL_EDGE_ENA		BIT(0)
 #define NI_65XX_REV_REG			0x04 /* 32-bit */
 #define NI_65XX_FILTER_REG		0x08 /* 32-bit */
 #define NI_65XX_RTSI_ROUTE_REG		0x0c /* 16-bit */
@@ -94,24 +94,24 @@
 #define NI_65XX_RTSI_WDOG_REG		0x10 /* 16-bit */
 #define NI_65XX_RTSI_TRIG_REG		0x12 /* 16-bit */
 #define NI_65XX_AUTO_CLK_SEL_REG	0x14 /* PXI-6528 only */
-#define NI_65XX_AUTO_CLK_SEL_STATUS	(1 << 1)
-#define NI_65XX_AUTO_CLK_SEL_DISABLE	(1 << 0)
+#define NI_65XX_AUTO_CLK_SEL_STATUS	BIT(1)
+#define NI_65XX_AUTO_CLK_SEL_DISABLE	BIT(0)
 #define NI_65XX_WDOG_CTRL_REG		0x15
-#define NI_65XX_WDOG_CTRL_ENA		(1 << 0)
+#define NI_65XX_WDOG_CTRL_ENA		BIT(0)
 #define NI_65XX_RTSI_CFG_REG		0x16
-#define NI_65XX_RTSI_CFG_RISE_SENSE	(1 << 2)
-#define NI_65XX_RTSI_CFG_FALL_SENSE	(1 << 1)
-#define NI_65XX_RTSI_CFG_SYNC_DETECT	(1 << 0)
+#define NI_65XX_RTSI_CFG_RISE_SENSE	BIT(2)
+#define NI_65XX_RTSI_CFG_FALL_SENSE	BIT(1)
+#define NI_65XX_RTSI_CFG_SYNC_DETECT	BIT(0)
 #define NI_65XX_WDOG_STATUS_REG		0x17
-#define NI_65XX_WDOG_STATUS_EXP		(1 << 0)
+#define NI_65XX_WDOG_STATUS_EXP		BIT(0)
 #define NI_65XX_WDOG_INTERVAL_REG	0x18 /* 32-bit */
 
 /* Recurring port registers (8-bit) */
 #define NI_65XX_PORT(x)			((x) * 0x10)
 #define NI_65XX_IO_DATA_REG(x)		(0x40 + NI_65XX_PORT(x))
 #define NI_65XX_IO_SEL_REG(x)		(0x41 + NI_65XX_PORT(x))
-#define NI_65XX_IO_SEL_OUTPUT		(0 << 0)
-#define NI_65XX_IO_SEL_INPUT		(1 << 0)
+#define NI_65XX_IO_SEL_OUTPUT		0
+#define NI_65XX_IO_SEL_INPUT		BIT(0)
 #define NI_65XX_RISE_EDGE_ENA_REG(x)	(0x42 + NI_65XX_PORT(x))
 #define NI_65XX_FALL_EDGE_ENA_REG(x)	(0x43 + NI_65XX_PORT(x))
 #define NI_65XX_FILTER_ENA(x)		(0x44 + NI_65XX_PORT(x))
@@ -613,7 +613,7 @@ static int ni_65xx_intr_insn_config(struct comedi_device *dev,
 
 /* ripped from mite.h and mite_setup2() to avoid mite dependency */
 #define MITE_IODWBSR	0xc0	 /* IO Device Window Base Size Register */
-#define WENAB		(1 << 7) /* window enable */
+#define WENAB			BIT(7) /* window enable */
 
 static int ni_65xx_mite_init(struct pci_dev *pcidev)
 {
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index f4c580f65a89..3e72718801a9 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -214,8 +214,9 @@ static int ni_670x_auto_attach(struct comedi_device *dev,
 	if (s->n_chan == 32) {
 		const struct comedi_lrange **range_table_list;
 
-		range_table_list = kmalloc(sizeof(struct comedi_lrange *) * 32,
-					   GFP_KERNEL);
+		range_table_list = kmalloc_array(32,
+						 sizeof(struct comedi_lrange *),
+						 GFP_KERNEL);
 		if (!range_table_list)
 			return -ENOMEM;
 		s->range_table_list = range_table_list;
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index 6cc304a4c59b..5e8130a7d670 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -579,48 +579,54 @@ static inline unsigned ni_stc_dma_channel_select_bitfield(unsigned channel)
 	return 0;
 }
 
-/* negative channel means no channel */
-static inline void ni_set_ai_dma_channel(struct comedi_device *dev, int channel)
+static inline void ni_set_ai_dma_channel(struct comedi_device *dev,
+					 unsigned channel)
 {
-	unsigned bits = 0;
-
-	if (channel >= 0)
-		bits = ni_stc_dma_channel_select_bitfield(channel);
+	unsigned bits = ni_stc_dma_channel_select_bitfield(channel);
 
 	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG,
 			NI_E_DMA_AI_SEL_MASK, NI_E_DMA_AI_SEL(bits));
 }
 
-/* negative channel means no channel */
-static inline void ni_set_ao_dma_channel(struct comedi_device *dev, int channel)
+static inline void ni_set_ai_dma_no_channel(struct comedi_device *dev)
 {
-	unsigned bits = 0;
+	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG, NI_E_DMA_AI_SEL_MASK, 0);
+}
 
-	if (channel >= 0)
-		bits = ni_stc_dma_channel_select_bitfield(channel);
+static inline void ni_set_ao_dma_channel(struct comedi_device *dev,
+					 unsigned channel)
+{
+	unsigned bits = ni_stc_dma_channel_select_bitfield(channel);
 
 	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG,
 			NI_E_DMA_AO_SEL_MASK, NI_E_DMA_AO_SEL(bits));
 }
 
-/* negative channel means no channel */
+static inline void ni_set_ao_dma_no_channel(struct comedi_device *dev)
+{
+	ni_set_bitfield(dev, NI_E_DMA_AI_AO_SEL_REG, NI_E_DMA_AO_SEL_MASK, 0);
+}
+
 static inline void ni_set_gpct_dma_channel(struct comedi_device *dev,
 					   unsigned gpct_index,
-					   int channel)
+					   unsigned channel)
 {
-	unsigned bits = 0;
-
-	if (channel >= 0)
-		bits = ni_stc_dma_channel_select_bitfield(channel);
+	unsigned bits = ni_stc_dma_channel_select_bitfield(channel);
 
 	ni_set_bitfield(dev, NI_E_DMA_G0_G1_SEL_REG,
 			NI_E_DMA_G0_G1_SEL_MASK(gpct_index),
 			NI_E_DMA_G0_G1_SEL(gpct_index, bits));
 }
 
-/* negative mite_channel means no channel */
+static inline void ni_set_gpct_dma_no_channel(struct comedi_device *dev,
+					      unsigned gpct_index)
+{
+	ni_set_bitfield(dev, NI_E_DMA_G0_G1_SEL_REG,
+			NI_E_DMA_G0_G1_SEL_MASK(gpct_index), 0);
+}
+
 static inline void ni_set_cdo_dma_channel(struct comedi_device *dev,
-					  int mite_channel)
+					  unsigned mite_channel)
 {
 	struct ni_private *devpriv = dev->private;
 	unsigned long flags;
@@ -628,16 +634,26 @@ static inline void ni_set_cdo_dma_channel(struct comedi_device *dev,
 
 	spin_lock_irqsave(&devpriv->soft_reg_copy_lock, flags);
 	devpriv->cdio_dma_select_reg &= ~NI_M_CDIO_DMA_SEL_CDO_MASK;
-	if (mite_channel >= 0) {
-		/*
-		 * XXX just guessing ni_stc_dma_channel_select_bitfield()
-		 * returns the right bits, under the assumption the cdio dma
-		 * selection works just like ai/ao/gpct.
-		 * Definitely works for dma channels 0 and 1.
-		 */
-		bits = ni_stc_dma_channel_select_bitfield(mite_channel);
-		devpriv->cdio_dma_select_reg |= NI_M_CDIO_DMA_SEL_CDO(bits);
-	}
+	/*
+	 * XXX just guessing ni_stc_dma_channel_select_bitfield()
+	 * returns the right bits, under the assumption the cdio dma
+	 * selection works just like ai/ao/gpct.
+	 * Definitely works for dma channels 0 and 1.
+	 */
+	bits = ni_stc_dma_channel_select_bitfield(mite_channel);
+	devpriv->cdio_dma_select_reg |= NI_M_CDIO_DMA_SEL_CDO(bits);
+	ni_writeb(dev, devpriv->cdio_dma_select_reg, NI_M_CDIO_DMA_SEL_REG);
+	mmiowb();
+	spin_unlock_irqrestore(&devpriv->soft_reg_copy_lock, flags);
+}
+
+static inline void ni_set_cdo_dma_no_channel(struct comedi_device *dev)
+{
+	struct ni_private *devpriv = dev->private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&devpriv->soft_reg_copy_lock, flags);
+	devpriv->cdio_dma_select_reg &= ~NI_M_CDIO_DMA_SEL_CDO_MASK;
 	ni_writeb(dev, devpriv->cdio_dma_select_reg, NI_M_CDIO_DMA_SEL_REG);
 	mmiowb();
 	spin_unlock_irqrestore(&devpriv->soft_reg_copy_lock, flags);
@@ -745,7 +761,7 @@ static void ni_release_ai_mite_channel(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ai_mite_chan) {
-		ni_set_ai_dma_channel(dev, -1);
+		ni_set_ai_dma_no_channel(dev);
 		mite_release_channel(devpriv->ai_mite_chan);
 		devpriv->ai_mite_chan = NULL;
 	}
@@ -761,7 +777,7 @@ static void ni_release_ao_mite_channel(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->ao_mite_chan) {
-		ni_set_ao_dma_channel(dev, -1);
+		ni_set_ao_dma_no_channel(dev);
 		mite_release_channel(devpriv->ao_mite_chan);
 		devpriv->ao_mite_chan = NULL;
 	}
@@ -781,7 +797,7 @@ static void ni_release_gpct_mite_channel(struct comedi_device *dev,
 		struct mite_channel *mite_chan =
 		    devpriv->counter_dev->counters[gpct_index].mite_chan;
 
-		ni_set_gpct_dma_channel(dev, gpct_index, -1);
+		ni_set_gpct_dma_no_channel(dev, gpct_index);
 		ni_tio_set_mite_channel(&devpriv->
 					counter_dev->counters[gpct_index],
 					NULL);
@@ -799,7 +815,7 @@ static void ni_release_cdo_mite_channel(struct comedi_device *dev)
 
 	spin_lock_irqsave(&devpriv->mite_channel_lock, flags);
 	if (devpriv->cdo_mite_chan) {
-		ni_set_cdo_dma_channel(dev, -1);
+		ni_set_cdo_dma_no_channel(dev);
 		mite_release_channel(devpriv->cdo_mite_chan);
 		devpriv->cdo_mite_chan = NULL;
 	}
@@ -1516,13 +1532,17 @@ static void ni_ai_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	unsigned short *array = data;
 	unsigned int *larray = data;
 	unsigned int i;
+#ifdef PCIDMA
+	__le16 *barray = data;
+	__le32 *blarray = data;
+#endif
 
 	for (i = 0; i < nsamples; i++) {
 #ifdef PCIDMA
 		if (s->subdev_flags & SDF_LSAMPL)
-			larray[i] = le32_to_cpu(larray[i]);
+			larray[i] = le32_to_cpu(blarray[i]);
 		else
-			array[i] = le16_to_cpu(array[i]);
+			array[i] = le16_to_cpu(barray[i]);
 #endif
 		if (s->subdev_flags & SDF_LSAMPL)
 			larray[i] += devpriv->ai_offset[chan_index];
@@ -2574,6 +2594,9 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	unsigned int nsamples = comedi_bytes_to_samples(s, num_bytes);
 	unsigned short *array = data;
 	unsigned int i;
+#ifdef PCIDMA
+	__le16 buf, *barray = data;
+#endif
 
 	for (i = 0; i < nsamples; i++) {
 		unsigned int range = CR_RANGE(cmd->chanlist[chan_index]);
@@ -2586,10 +2609,11 @@ static void ni_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		if (comedi_range_is_bipolar(s, range))
 			val = comedi_offset_munge(s, val);
 #ifdef PCIDMA
-		val = cpu_to_le16(val);
-#endif
+		buf = cpu_to_le16(val);
+		barray[i] = buf;
+#else
 		array[i] = val;
-
+#endif
 		chan_index++;
 		chan_index %= cmd->chanlist_len;
 	}
diff --git a/drivers/staging/comedi/drivers/plx9080.h b/drivers/staging/comedi/drivers/plx9080.h
index 25706531b885..f5cd6d5004bd 100644
--- a/drivers/staging/comedi/drivers/plx9080.h
+++ b/drivers/staging/comedi/drivers/plx9080.h
@@ -1,4 +1,5 @@
-/* plx9080.h
+/*
+ * plx9080.h
  *
  * Copyright (C) 2002,2003 Frank Mori Hess <fmhess@users.sourceforge.net>
  *
@@ -33,8 +34,10 @@ struct plx_dma_desc {
 	__le32 local_start_addr;
 	/* transfer_size is in bytes, only first 23 bits of register are used */
 	__le32 transfer_size;
-	/* address of next descriptor (quad word aligned), plus some
-	 * additional bits (see PLX_DMA0_DESCRIPTOR_REG) */
+	/*
+	 * address of next descriptor (quad word aligned), plus some
+	 * additional bits (see PLX_DMA0_DESCRIPTOR_REG)
+	 */
 	__le32 next;
 };
 
@@ -46,23 +49,31 @@ struct plx_dma_desc {
 **
 **********************************************************************/
 
-#define PLX_LAS0RNG_REG         0x0000	/* L, Local Addr Space 0 Range Register */
-#define PLX_LAS1RNG_REG         0x00f0	/* L, Local Addr Space 1 Range Register */
+/* L, Local Addr Space 0 Range Register */
+#define PLX_LAS0RNG_REG         0x0000
+/* L, Local Addr Space 1 Range Register */
+#define PLX_LAS1RNG_REG         0x00f0
 #define  LRNG_IO           0x00000001	/* Map to: 1=I/O, 0=Mem */
 #define  LRNG_ANY32        0x00000000	/* Locate anywhere in 32 bit */
 #define  LRNG_LT1MB        0x00000002	/* Locate in 1st meg */
 #define  LRNG_ANY64        0x00000004	/* Locate anywhere in 64 bit */
-#define  LRNG_MEM_MASK     0xfffffff0	/*  bits that specify range for memory io */
-#define  LRNG_IO_MASK     0xfffffffa	/*  bits that specify range for normal io */
-
-#define PLX_LAS0MAP_REG         0x0004	/* L, Local Addr Space 0 Remap Register */
-#define PLX_LAS1MAP_REG         0x00f4	/* L, Local Addr Space 1 Remap Register */
+/*  bits that specify range for memory io */
+#define  LRNG_MEM_MASK     0xfffffff0
+/*  bits that specify range for normal io */
+#define  LRNG_IO_MASK     0xfffffffa
+/* L, Local Addr Space 0 Remap Register */
+#define PLX_LAS0MAP_REG         0x0004
+/* L, Local Addr Space 1 Remap Register */
+#define PLX_LAS1MAP_REG         0x00f4
 #define  LMAP_EN           0x00000001	/* Enable slave decode */
-#define  LMAP_MEM_MASK     0xfffffff0	/*  bits that specify decode for memory io */
-#define  LMAP_IO_MASK     0xfffffffa	/*  bits that specify decode bits for normal io */
+/*  bits that specify decode for memory io */
+#define  LMAP_MEM_MASK     0xfffffff0
+/*  bits that specify decode bits for normal io */
+#define  LMAP_IO_MASK     0xfffffffa
 
-/* Mode/Arbitration Register.
-*/
+/*
+ * Mode/Arbitration Register.
+ */
 #define PLX_MARB_REG         0x8	/* L, Local Arbitration Register */
 #define PLX_DMAARB_REG      0xac
 enum marb_bits {
@@ -72,35 +83,45 @@ enum marb_bits {
 	MARB_LPEN = 0x00020000,	/* Pause Timer Enable */
 	MARB_BREQ = 0x00040000,	/* Local Bus BREQ Enable */
 	MARB_DMA_PRIORITY_MASK = 0x00180000,
-	MARB_LBDS_GIVE_UP_BUS_MODE = 0x00200000,	/* local bus direct slave give up bus mode */
-	MARB_DS_LLOCK_ENABLE = 0x00400000,	/* direct slave LLOCKo# enable */
+	/* local bus direct slave give up bus mode */
+	MARB_LBDS_GIVE_UP_BUS_MODE = 0x00200000,
+	/* direct slave LLOCKo# enable */
+	MARB_DS_LLOCK_ENABLE = 0x00400000,
 	MARB_PCI_REQUEST_MODE = 0x00800000,
 	MARB_PCIv21_MODE = 0x01000000,	/* pci specification v2.1 mode */
 	MARB_PCI_READ_NO_WRITE_MODE = 0x02000000,
 	MARB_PCI_READ_WITH_WRITE_FLUSH_MODE = 0x04000000,
-	MARB_GATE_TIMER_WITH_BREQ = 0x08000000,	/* gate local bus latency timer with BREQ */
+	/* gate local bus latency timer with BREQ */
+	MARB_GATE_TIMER_WITH_BREQ = 0x08000000,
 	MARB_PCI_READ_NO_FLUSH_MODE = 0x10000000,
 	MARB_USE_SUBSYSTEM_IDS = 0x20000000,
 };
 
 #define PLX_BIGEND_REG 0xc
 enum bigend_bits {
-	BIGEND_CONFIG = 0x1,	/* use big endian ordering for configuration register accesses */
+	/* use big endian ordering for configuration register accesses */
+	BIGEND_CONFIG = 0x1,
 	BIGEND_DIRECT_MASTER = 0x2,
 	BIGEND_DIRECT_SLAVE_LOCAL0 = 0x4,
 	BIGEND_ROM = 0x8,
-	BIGEND_BYTE_LANE = 0x10,	/* use byte lane consisting of most significant bits instead of least significant */
+	/*
+	 * use byte lane consisting of most significant bits instead of
+	 * least significant
+	 */
+	BIGEND_BYTE_LANE = 0x10,
 	BIGEND_DIRECT_SLAVE_LOCAL1 = 0x20,
 	BIGEND_DMA1 = 0x40,
 	BIGEND_DMA0 = 0x80,
 };
 
-/* Note: The Expansion ROM  stuff is only relevant to the PC environment.
+/*
+** Note: The Expansion ROM  stuff is only relevant to the PC environment.
 **       This expansion ROM code is executed by the host CPU at boot time.
 **       For this reason no bit definitions are provided here.
-*/
+ */
 #define PLX_ROMRNG_REG         0x0010	/* L, Expn ROM Space Range Register */
-#define PLX_ROMMAP_REG         0x0014	/* L, Local Addr Space Range Register */
+/* L, Local Addr Space Range Register */
+#define PLX_ROMMAP_REG         0x0014
 
 #define PLX_REGION0_REG         0x0018	/* L, Local Bus Region 0 Descriptor */
 #define  RGN_WIDTH         0x00000002	/* Local bus width bits */
@@ -190,7 +211,8 @@ enum bigend_bits {
 #define  ICS_TA_DMA0       0x02000000	/* Target Abort - DMA #0 */
 #define  ICS_TA_DMA1       0x04000000	/* Target Abort - DMA #1 */
 #define  ICS_TA_RA         0x08000000	/* Target Abort - Retry Timeout */
-#define  ICS_MBIA(x)       (0x10000000 << ((x) & 0x3))	/*  mailbox x is active */
+/*  mailbox x is active */
+#define  ICS_MBIA(x)       (0x10000000 << ((x) & 0x3))
 
 #define PLX_CONTROL_REG        0x006C	/* L, EEPROM Cntl & PCI Cmd Codes */
 #define  CTL_RDMA          0x0000000E	/* DMA Read Command */
@@ -221,28 +243,38 @@ enum bigend_bits {
 #define  PLX_EN_BTERM_BIT	0x80	/*  enable BTERM# input */
 #define  PLX_DMA_LOCAL_BURST_EN_BIT	0x100	/*  enable local burst mode */
 #define  PLX_EN_CHAIN_BIT	0x200	/*  enables chaining */
-#define  PLX_EN_DMA_DONE_INTR_BIT	0x400	/*  enables interrupt on dma done */
-#define  PLX_LOCAL_ADDR_CONST_BIT	0x800	/*  hold local address constant (don't increment) */
-#define  PLX_DEMAND_MODE_BIT	0x1000	/*  enables demand-mode for dma transfer */
+/*  enables interrupt on dma done */
+#define  PLX_EN_DMA_DONE_INTR_BIT	0x400
+/*  hold local address constant (don't increment) */
+#define  PLX_LOCAL_ADDR_CONST_BIT	0x800
+/*  enables demand-mode for dma transfer */
+#define  PLX_DEMAND_MODE_BIT	0x1000
 #define  PLX_EOT_ENABLE_BIT	0x4000
 #define  PLX_STOP_MODE_BIT 0x8000
-#define  PLX_DMA_INTR_PCI_BIT	0x20000	/*  routes dma interrupt to pci bus (instead of local bus) */
+/*  routes dma interrupt to pci bus (instead of local bus) */
+#define  PLX_DMA_INTR_PCI_BIT	0x20000
 
-#define PLX_DMA0_PCI_ADDRESS_REG	0x84	/*  pci address that dma transfers start at */
+/*  pci address that dma transfers start at */
+#define PLX_DMA0_PCI_ADDRESS_REG	0x84
 #define PLX_DMA1_PCI_ADDRESS_REG	0x98
 
-#define PLX_DMA0_LOCAL_ADDRESS_REG	0x88	/*  local address that dma transfers start at */
+/*  local address that dma transfers start at */
+#define PLX_DMA0_LOCAL_ADDRESS_REG	0x88
 #define PLX_DMA1_LOCAL_ADDRESS_REG	0x9c
 
-#define PLX_DMA0_TRANSFER_SIZE_REG	0x8c	/*  number of bytes to transfer (first 23 bits) */
+/*  number of bytes to transfer (first 23 bits) */
+#define PLX_DMA0_TRANSFER_SIZE_REG	0x8c
 #define PLX_DMA1_TRANSFER_SIZE_REG	0xa0
 
 #define PLX_DMA0_DESCRIPTOR_REG	0x90	/*  descriptor pointer register */
 #define PLX_DMA1_DESCRIPTOR_REG	0xa4
-#define  PLX_DESC_IN_PCI_BIT	0x1	/*  descriptor is located in pci space (not local space) */
+/*  descriptor is located in pci space (not local space) */
+#define  PLX_DESC_IN_PCI_BIT	0x1
 #define  PLX_END_OF_CHAIN_BIT	0x2	/*  end of chain bit */
-#define  PLX_INTR_TERM_COUNT	0x4	/*  interrupt when this descriptor's transfer is finished */
-#define  PLX_XFER_LOCAL_TO_PCI 0x8	/*  transfer from local to pci bus (not pci to local) */
+/*  interrupt when this descriptor's transfer is finished */
+#define  PLX_INTR_TERM_COUNT	0x4
+/*  transfer from local to pci bus (not pci to local) */
+#define  PLX_XFER_LOCAL_TO_PCI 0x8
 
 #define PLX_DMA0_CS_REG	0xa8	/*  command status register */
 #define PLX_DMA1_CS_REG	0xa9
@@ -288,10 +320,11 @@ enum bigend_bits {
 #define MBX_STS_PCIRESET   0x00000100	/* Host issued PCI reset request */
 #define MBX_STS_BUSY       0x00000080	/* PUTS is in progress */
 #define MBX_STS_ERROR      0x00000040	/* PUTS has failed */
-#define MBX_STS_RESERVED   0x000000c0	/* Undefined -> status in transition.
-					   We are in process of changing
-					   bits; we SET Error bit before
-					   RESET of Busy bit */
+/*
+ * Undefined -> status in transition. We are in process of changing bits;
+ * we SET Error bit before RESET of Busy bit
+ */
+#define MBX_STS_RESERVED   0x000000c0
 
 #define MBX_RESERVED_5     0x00000020	/* FYI: reserved/unused bit */
 #define MBX_RESERVED_4     0x00000010	/* FYI: reserved/unused bit */
@@ -320,12 +353,12 @@ enum bigend_bits {
 #define MBX_CMD_BSWAP_0    0x8c000000	/* use scheme 0 */
 #define MBX_CMD_BSWAP_1    0x8c000001	/* use scheme 1 */
 
-#define MBX_CMD_SETHMS     0x8d000000	/* setup host memory access window
-					   size */
-#define MBX_CMD_SETHBA     0x8e000000	/* setup host memory access base
-					   address */
-#define MBX_CMD_MGO        0x8f000000	/* perform memory setup and continue
-					   (IE. Done) */
+/* setup host memory access window size */
+#define MBX_CMD_SETHMS     0x8d000000
+/* setup host memory access base address */
+#define MBX_CMD_SETHBA     0x8e000000
+/* perform memory setup and continue (IE. Done) */
+#define MBX_CMD_MGO        0x8f000000
 #define MBX_CMD_NOOP       0xFF000000	/* dummy, illegal command */
 
 /*****************************************/
@@ -348,7 +381,8 @@ enum bigend_bits {
 /***************************************/
 
 #define MBX_BTYPE_MASK          0x0000ffff	/* PUTS Board Type Register */
-#define MBX_BTYPE_FAMILY_MASK   0x0000ff00	/* PUTS Board Family Register */
+/* PUTS Board Family Register */
+#define MBX_BTYPE_FAMILY_MASK   0x0000ff00
 #define MBX_BTYPE_SUBTYPE_MASK  0x000000ff	/* PUTS Board Subtype */
 
 #define MBX_BTYPE_PLX9060       0x00000100	/* PLX family type */
@@ -378,12 +412,12 @@ enum bigend_bits {
 
 /* system allocates this many bytes for address mapping mailbox space */
 #define MBX_ADDR_SPACE_360 0x80	/* wanXL100s/200/400 */
-#define MBX_ADDR_MASK_360 (MBX_ADDR_SPACE_360-1)
+#define MBX_ADDR_MASK_360 (MBX_ADDR_SPACE_360 - 1)
 
 static inline int plx9080_abort_dma(void __iomem *iobase, unsigned int channel)
 {
 	void __iomem *dma_cs_addr;
-	uint8_t dma_status;
+	u8 dma_status;
 	const int timeout = 10000;
 	unsigned int i;
 
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index d70c97947627..c80527db9c19 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -37,7 +37,6 @@
 
 #include <linux/module.h>
 #include "../comedidev.h"
-#include <asm/byteorder.h>
 
 /*
  * Register I/O map
@@ -84,7 +83,92 @@
 #define S526_GPCT_LSB_REG(x)	(0x12 + ((x) * 8))
 #define S526_GPCT_MSB_REG(x)	(0x14 + ((x) * 8))
 #define S526_GPCT_MODE_REG(x)	(0x16 + ((x) * 8))
+#define S526_GPCT_MODE_COUT_SRC(x)	((x) << 0)
+#define S526_GPCT_MODE_COUT_SRC_MASK	S526_GPCT_MODE_COUT_SRC(0x1)
+#define S526_GPCT_MODE_COUT_SRC_RCAP	S526_GPCT_MODE_COUT_SRC(0)
+#define S526_GPCT_MODE_COUT_SRC_RTGL	S526_GPCT_MODE_COUT_SRC(1)
+#define S526_GPCT_MODE_COUT_POL(x)	((x) << 1)
+#define S526_GPCT_MODE_COUT_POL_MASK	S526_GPCT_MODE_COUT_POL(0x1)
+#define S526_GPCT_MODE_COUT_POL_NORM	S526_GPCT_MODE_COUT_POL(0)
+#define S526_GPCT_MODE_COUT_POL_INV	S526_GPCT_MODE_COUT_POL(1)
+#define S526_GPCT_MODE_AUTOLOAD(x)	((x) << 2)
+#define S526_GPCT_MODE_AUTOLOAD_MASK	S526_GPCT_MODE_AUTOLOAD(0x7)
+#define S526_GPCT_MODE_AUTOLOAD_NONE	S526_GPCT_MODE_AUTOLOAD(0)
+/* these 3 bits can be OR'ed */
+#define S526_GPCT_MODE_AUTOLOAD_RO	S526_GPCT_MODE_AUTOLOAD(0x1)
+#define S526_GPCT_MODE_AUTOLOAD_IXFALL	S526_GPCT_MODE_AUTOLOAD(0x2)
+#define S526_GPCT_MODE_AUTOLOAD_IXRISE	S526_GPCT_MODE_AUTOLOAD(0x4)
+#define S526_GPCT_MODE_HWCTEN_SRC(x)	((x) << 5)
+#define S526_GPCT_MODE_HWCTEN_SRC_MASK	S526_GPCT_MODE_HWCTEN_SRC(0x3)
+#define S526_GPCT_MODE_HWCTEN_SRC_CEN	S526_GPCT_MODE_HWCTEN_SRC(0)
+#define S526_GPCT_MODE_HWCTEN_SRC_IX	S526_GPCT_MODE_HWCTEN_SRC(1)
+#define S526_GPCT_MODE_HWCTEN_SRC_IXRF	S526_GPCT_MODE_HWCTEN_SRC(2)
+#define S526_GPCT_MODE_HWCTEN_SRC_NRCAP	S526_GPCT_MODE_HWCTEN_SRC(3)
+#define S526_GPCT_MODE_CTEN_CTRL(x)	((x) << 7)
+#define S526_GPCT_MODE_CTEN_CTRL_MASK	S526_GPCT_MODE_CTEN_CTRL(0x3)
+#define S526_GPCT_MODE_CTEN_CTRL_DIS	S526_GPCT_MODE_CTEN_CTRL(0)
+#define S526_GPCT_MODE_CTEN_CTRL_ENA	S526_GPCT_MODE_CTEN_CTRL(1)
+#define S526_GPCT_MODE_CTEN_CTRL_HW	S526_GPCT_MODE_CTEN_CTRL(2)
+#define S526_GPCT_MODE_CTEN_CTRL_INVHW	S526_GPCT_MODE_CTEN_CTRL(3)
+#define S526_GPCT_MODE_CLK_SRC(x)	((x) << 9)
+#define S526_GPCT_MODE_CLK_SRC_MASK	S526_GPCT_MODE_CLK_SRC(0x3)
+/* if count direction control set to quadrature */
+#define S526_GPCT_MODE_CLK_SRC_QUADX1	S526_GPCT_MODE_CLK_SRC(0)
+#define S526_GPCT_MODE_CLK_SRC_QUADX2	S526_GPCT_MODE_CLK_SRC(1)
+#define S526_GPCT_MODE_CLK_SRC_QUADX4	S526_GPCT_MODE_CLK_SRC(2)
+#define S526_GPCT_MODE_CLK_SRC_QUADX4_	S526_GPCT_MODE_CLK_SRC(3)
+/* if count direction control set to software control */
+#define S526_GPCT_MODE_CLK_SRC_ARISE	S526_GPCT_MODE_CLK_SRC(0)
+#define S526_GPCT_MODE_CLK_SRC_AFALL	S526_GPCT_MODE_CLK_SRC(1)
+#define S526_GPCT_MODE_CLK_SRC_INT	S526_GPCT_MODE_CLK_SRC(2)
+#define S526_GPCT_MODE_CLK_SRC_INTHALF	S526_GPCT_MODE_CLK_SRC(3)
+#define S526_GPCT_MODE_CT_DIR(x)	((x) << 11)
+#define S526_GPCT_MODE_CT_DIR_MASK	S526_GPCT_MODE_CT_DIR(0x1)
+/* if count direction control set to software control */
+#define S526_GPCT_MODE_CT_DIR_UP	S526_GPCT_MODE_CT_DIR(0)
+#define S526_GPCT_MODE_CT_DIR_DOWN	S526_GPCT_MODE_CT_DIR(1)
+#define S526_GPCT_MODE_CTDIR_CTRL(x)	((x) << 12)
+#define S526_GPCT_MODE_CTDIR_CTRL_MASK	S526_GPCT_MODE_CTDIR_CTRL(0x1)
+#define S526_GPCT_MODE_CTDIR_CTRL_QUAD	S526_GPCT_MODE_CTDIR_CTRL(0)
+#define S526_GPCT_MODE_CTDIR_CTRL_SOFT	S526_GPCT_MODE_CTDIR_CTRL(1)
+#define S526_GPCT_MODE_LATCH_CTRL(x)	((x) << 13)
+#define S526_GPCT_MODE_LATCH_CTRL_MASK	S526_GPCT_MODE_LATCH_CTRL(0x1)
+#define S526_GPCT_MODE_LATCH_CTRL_READ	S526_GPCT_MODE_LATCH_CTRL(0)
+#define S526_GPCT_MODE_LATCH_CTRL_EVENT	S526_GPCT_MODE_LATCH_CTRL(1)
+#define S526_GPCT_MODE_PR_SELECT(x)	((x) << 14)
+#define S526_GPCT_MODE_PR_SELECT_MASK	S526_GPCT_MODE_PR_SELECT(0x1)
+#define S526_GPCT_MODE_PR_SELECT_PR0	S526_GPCT_MODE_PR_SELECT(0)
+#define S526_GPCT_MODE_PR_SELECT_PR1	S526_GPCT_MODE_PR_SELECT(1)
+/* Control/Status - R = readable, W = writeable, C = write 1 to clear */
 #define S526_GPCT_CTRL_REG(x)	(0x18 + ((x) * 8))
+#define S526_GPCT_CTRL_EV_STATUS(x)	((x) << 0)		/* RC */
+#define S526_GPCT_CTRL_EV_STATUS_MASK	S526_GPCT_EV_STATUS(0xf)
+#define S526_GPCT_CTRL_EV_STATUS_NONE	S526_GPCT_EV_STATUS(0)
+/* these 4 bits can be OR'ed */
+#define S526_GPCT_CTRL_EV_STATUS_ECAP	S526_GPCT_EV_STATUS(0x1)
+#define S526_GPCT_CTRL_EV_STATUS_ICAPN	S526_GPCT_EV_STATUS(0x2)
+#define S526_GPCT_CTRL_EV_STATUS_ICAPP	S526_GPCT_EV_STATUS(0x4)
+#define S526_GPCT_CTRL_EV_STATUS_RCAP	S526_GPCT_EV_STATUS(0x8)
+#define S526_GPCT_CTRL_COUT_STATUS	BIT(4)			/* R */
+#define S526_GPCT_CTRL_INDEX_STATUS	BIT(5)			/* R */
+#define S525_GPCT_CTRL_INTEN(x)		((x) << 6)		/* W */
+#define S525_GPCT_CTRL_INTEN_MASK	S526_GPCT_CTRL_INTEN(0xf)
+#define S525_GPCT_CTRL_INTEN_NONE	S526_GPCT_CTRL_INTEN(0)
+/* these 4 bits can be OR'ed */
+#define S525_GPCT_CTRL_INTEN_ERROR	S526_GPCT_CTRL_INTEN(0x1)
+#define S525_GPCT_CTRL_INTEN_IXFALL	S526_GPCT_CTRL_INTEN(0x2)
+#define S525_GPCT_CTRL_INTEN_IXRISE	S526_GPCT_CTRL_INTEN(0x4)
+#define S525_GPCT_CTRL_INTEN_RO		S526_GPCT_CTRL_INTEN(0x8)
+#define S525_GPCT_CTRL_LATCH_SEL(x)	((x) << 10)		/* W */
+#define S525_GPCT_CTRL_LATCH_SEL_MASK	S526_GPCT_CTRL_LATCH_SEL(0x7)
+#define S525_GPCT_CTRL_LATCH_SEL_NONE	S526_GPCT_CTRL_LATCH_SEL(0)
+/* these 3 bits can be OR'ed */
+#define S525_GPCT_CTRL_LATCH_SEL_IXFALL	S526_GPCT_CTRL_LATCH_SEL(0x1)
+#define S525_GPCT_CTRL_LATCH_SEL_IXRISE	S526_GPCT_CTRL_LATCH_SEL(0x2)
+#define S525_GPCT_CTRL_LATCH_SEL_ITIMER	S526_GPCT_CTRL_LATCH_SEL(0x4)
+#define S525_GPCT_CTRL_CT_ARM		BIT(13)			/* W */
+#define S525_GPCT_CTRL_CT_LOAD		BIT(14)			/* W */
+#define S526_GPCT_CTRL_CT_RESET		BIT(15)			/* W */
 #define S526_EEPROM_DATA_REG	0x32
 #define S526_EEPROM_CTRL_REG	0x34
 #define S526_EEPROM_CTRL_ADDR(x) (((x) & 0x3f) << 3)
@@ -92,41 +176,6 @@
 #define S526_EEPROM_CTRL_READ	S526_EEPROM_CTRL(2)
 #define S526_EEPROM_CTRL_START	BIT(0)
 
-struct counter_mode_register_t {
-#if defined(__LITTLE_ENDIAN_BITFIELD)
-	unsigned short coutSource:1;
-	unsigned short coutPolarity:1;
-	unsigned short autoLoadResetRcap:3;
-	unsigned short hwCtEnableSource:2;
-	unsigned short ctEnableCtrl:2;
-	unsigned short clockSource:2;
-	unsigned short countDir:1;
-	unsigned short countDirCtrl:1;
-	unsigned short outputRegLatchCtrl:1;
-	unsigned short preloadRegSel:1;
-	unsigned short reserved:1;
- #elif defined(__BIG_ENDIAN_BITFIELD)
-	unsigned short reserved:1;
-	unsigned short preloadRegSel:1;
-	unsigned short outputRegLatchCtrl:1;
-	unsigned short countDirCtrl:1;
-	unsigned short countDir:1;
-	unsigned short clockSource:2;
-	unsigned short ctEnableCtrl:2;
-	unsigned short hwCtEnableSource:2;
-	unsigned short autoLoadResetRcap:3;
-	unsigned short coutPolarity:1;
-	unsigned short coutSource:1;
-#else
-#error Unknown bit field order
-#endif
-};
-
-union cmReg {
-	struct counter_mode_register_t reg;
-	unsigned short value;
-};
-
 struct s526_private {
 	unsigned int gpct_config[4];
 	unsigned short ai_ctrl;
@@ -174,7 +223,6 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 	struct s526_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int val;
-	union cmReg cmReg;
 
 	/*
 	 * Check what type of Counter the user requested
@@ -192,28 +240,31 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 
 #if 1
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/*  Reset the counter if it is software preload */
-		if (cmReg.reg.autoLoadResetRcap == 0) {
+		if ((val & S526_GPCT_MODE_AUTOLOAD_MASK) ==
+		    S526_GPCT_MODE_AUTOLOAD_NONE) {
 			/*  Reset the counter */
-			outw(0x8000, dev->iobase + S526_GPCT_CTRL_REG(chan));
-			/* Load the counter from PR0
-			 * outw(0x4000, dev->iobase + S526_GPCT_CTRL_REG(chan));
+			outw(S526_GPCT_CTRL_CT_RESET,
+			     dev->iobase + S526_GPCT_CTRL_REG(chan));
+			/*
+			 * Load the counter from PR0
+			 * outw(S526_GPCT_CTRL_CT_LOAD,
+			 *      dev->iobase + S526_GPCT_CTRL_REG(chan));
 			 */
 		}
 #else
-		/*  0 quadrature, 1 software control */
-		cmReg.reg.countDirCtrl = 0;
+		val = S526_GPCT_MODE_CTDIR_CTRL_QUAD;
 
 		/*  data[1] contains GPCT_X1, GPCT_X2 or GPCT_X4 */
 		if (data[1] == GPCT_X2)
-			cmReg.reg.clockSource = 1;
+			val |= S526_GPCT_MODE_CLK_SRC_QUADX2;
 		else if (data[1] == GPCT_X4)
-			cmReg.reg.clockSource = 2;
+			val |= S526_GPCT_MODE_CLK_SRC_QUADX4;
 		else
-			cmReg.reg.clockSource = 0;
+			val |= S526_GPCT_MODE_CLK_SRC_QUADX1;
 
 		/*  When to take into account the indexpulse: */
 		/*
@@ -224,13 +275,14 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		 * }
 		 */
 		/*  Take into account the index pulse? */
-		if (data[3] == GPCT_RESET_COUNTER_ON_INDEX)
+		if (data[3] == GPCT_RESET_COUNTER_ON_INDEX) {
 			/*  Auto load with INDEX^ */
-			cmReg.reg.autoLoadResetRcap = 4;
+			val |= S526_GPCT_MODE_AUTOLOAD_IXRISE;
+		}
 
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/*  Load the pre-load register */
 		s526_gpct_write(dev, chan, data[2]);
@@ -241,11 +293,14 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 			     dev->iobase + S526_GPCT_CTRL_REG(chan));
 
 		/*  Reset the counter if it is software preload */
-		if (cmReg.reg.autoLoadResetRcap == 0) {
+		if ((val & S526_GPCT_MODE_AUTOLOAD_MASK) ==
+		    S526_GPCT_MODE_AUTOLOAD_NONE) {
 			/*  Reset the counter */
-			outw(0x8000, dev->iobase + S526_GPCT_CTRL_REG(chan));
+			outw(S526_GPCT_CTRL_CT_RESET,
+			     dev->iobase + S526_GPCT_CTRL_REG(chan));
 			/*  Load the counter from PR0 */
-			outw(0x4000, dev->iobase + S526_GPCT_CTRL_REG(chan));
+			outw(S526_GPCT_CTRL_CT_LOAD,
+			     dev->iobase + S526_GPCT_CTRL_REG(chan));
 		}
 #endif
 		break;
@@ -261,17 +316,21 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		devpriv->gpct_config[chan] = data[0];
 
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		/* Select PR0 */
+		val &= ~S526_GPCT_MODE_PR_SELECT_MASK;
+		val |= S526_GPCT_MODE_PR_SELECT_PR0;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/* Load the pre-load register 0 */
 		s526_gpct_write(dev, chan, data[2]);
 
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		cmReg.reg.preloadRegSel = 1;	/*  PR1 */
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		/* Select PR1 */
+		val &= ~S526_GPCT_MODE_PR_SELECT_MASK;
+		val |= S526_GPCT_MODE_PR_SELECT_PR1;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/* Load the pre-load register 1 */
 		s526_gpct_write(dev, chan, data[3]);
@@ -294,17 +353,21 @@ static int s526_gpct_insn_config(struct comedi_device *dev,
 		devpriv->gpct_config[chan] = data[0];
 
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		cmReg.reg.preloadRegSel = 0;	/*  PR0 */
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		/* Select PR0 */
+		val &= ~S526_GPCT_MODE_PR_SELECT_MASK;
+		val |= S526_GPCT_MODE_PR_SELECT_PR0;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/* Load the pre-load register 0 */
 		s526_gpct_write(dev, chan, data[2]);
 
 		/*  Set Counter Mode Register */
-		cmReg.value = data[1] & 0xffff;
-		cmReg.reg.preloadRegSel = 1;	/*  PR1 */
-		outw(cmReg.value, dev->iobase + S526_GPCT_MODE_REG(chan));
+		val = data[1] & 0xffff;
+		/* Select PR1 */
+		val &= ~S526_GPCT_MODE_PR_SELECT_MASK;
+		val |= S526_GPCT_MODE_PR_SELECT_PR1;
+		outw(val, dev->iobase + S526_GPCT_MODE_REG(chan));
 
 		/* Load the pre-load register 1 */
 		s526_gpct_write(dev, chan, data[3]);
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index 75040daa40ce..72f0aaa6911f 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -934,7 +934,7 @@ static void cls_flush_uart_write(struct channel_t *ch)
 
 	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_XMIT),
 	       &ch->ch_cls_uart->isr_fcr);
-	udelay(10);
+	usleep_range(10, 20);
 
 	ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 }
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index 8106f5234bf5..39c76e78e56a 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -1108,9 +1108,9 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 	 * On the other hand, if the UART IS in FIFO mode, then ask
 	 * the UART to give us an approximation of data it has RX'ed.
 	 */
-	if (!(ch->ch_flags & CH_FIFO_ENABLED))
+	if (!(ch->ch_flags & CH_FIFO_ENABLED)) {
 		total = 0;
-	else {
+	} else {
 		total = readb(&ch->ch_neo_uart->rfifo);
 
 		/*
@@ -1628,7 +1628,7 @@ static void neo_uart_init(struct channel_t *ch)
 
 	/* Clear out UART and FIFO */
 	readb(&ch->ch_neo_uart->txrx);
-	writeb((UART_FCR_ENABLE_FIFO|UART_FCR_CLEAR_RCVR|UART_FCR_CLEAR_XMIT), &ch->ch_neo_uart->isr_fcr);
+	writeb((UART_FCR_ENABLE_FIFO | UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT), &ch->ch_neo_uart->isr_fcr);
 	readb(&ch->ch_neo_uart->lsr);
 	readb(&ch->ch_neo_uart->msr);
 
@@ -1779,8 +1779,8 @@ static void neo_vpd(struct dgnc_board *brd)
 	/* Store the VPD into our buffer */
 	for (i = 0; i < NEO_VPD_IMAGESIZE; i++) {
 		a = neo_read_eeprom(brd->re_map_membase, i);
-		brd->vpd[i*2] = a & 0xff;
-		brd->vpd[(i*2)+1] = (a >> 8) & 0xff;
+		brd->vpd[i * 2] = a & 0xff;
+		brd->vpd[(i * 2) + 1] = (a >> 8) & 0xff;
 	}
 
 	if  (((brd->vpd[0x08] != 0x82)	   /* long resource name tag */
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index 48e4b90578c1..b79eab084c02 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -448,7 +448,7 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
  *	dgnc_wmove - Write data to transmit queue.
  *
  *		ch	- Pointer to channel structure.
- *		buf	- Poiter to characters to be moved.
+ *		buf	- Pointer to characters to be moved.
  *		n	- Number of characters to move.
  *
  *=======================================================================*/
diff --git a/drivers/staging/dgnc/dgnc_utils.c b/drivers/staging/dgnc/dgnc_utils.c
index f76de82908d3..95272f4765fc 100644
--- a/drivers/staging/dgnc/dgnc_utils.c
+++ b/drivers/staging/dgnc/dgnc_utils.c
@@ -1,7 +1,6 @@
 #include <linux/tty.h>
 #include <linux/sched.h>
 #include "dgnc_utils.h"
-#include "digi.h"
 
 /*
  * dgnc_ms_sleep()
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index 4e6c16af40fc..beb9411658ba 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -823,7 +823,7 @@ static int _nbu2ss_out_dma(
 	u32		length
 )
 {
-	u8		*pBuffer;
+	dma_addr_t	pBuffer;
 	u32		mpkt;
 	u32		lmpkt;
 	u32		dmacnt;
@@ -836,7 +836,7 @@ static int _nbu2ss_out_dma(
 		return 1;		/* DMA is forwarded */
 
 	req->dma_flag = TRUE;
-	pBuffer = (u8 *)req->req.dma;
+	pBuffer = req->req.dma;
 	pBuffer += req->req.actual;
 
 	/* DMA Address */
@@ -1034,7 +1034,7 @@ static int _nbu2ss_in_dma(
 	u32		length
 )
 {
-	u8		*pBuffer;
+	dma_addr_t	pBuffer;
 	u32		mpkt;		/* MaxPacketSize */
 	u32		lmpkt;		/* Last Packet Data Size */
 	u32		dmacnt;		/* IN Data Size */
@@ -1080,7 +1080,7 @@ static int _nbu2ss_in_dma(
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_DCR2, data);
 
 	/* Address setting */
-	pBuffer = (u8 *)req->req.dma;
+	pBuffer = req->req.dma;
 	pBuffer += req->req.actual;
 	_nbu2ss_writel(&preg->EP_DCR[num].EP_TADR, (u32)pBuffer);
 
@@ -1285,11 +1285,7 @@ static void _nbu2ss_restert_transfer(struct nbu2ss_ep *ep)
 	bool	bflag = FALSE;
 	struct nbu2ss_req *req;
 
-	if (list_empty(&ep->queue))
-		req = NULL;
-	else
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
-
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
 	if (!req)
 		return;
 
@@ -1784,11 +1780,7 @@ static inline int _nbu2ss_ep0_in_data_stage(struct nbu2ss_udc *udc)
 	struct nbu2ss_req	*req;
 	struct nbu2ss_ep	*ep = &udc->ep[0];
 
-	if (list_empty(&ep->queue))
-		req = NULL;
-	else
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
-
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
 	if (!req)
 		req = &udc->ep0_req;
 
@@ -1811,11 +1803,7 @@ static inline int _nbu2ss_ep0_out_data_stage(struct nbu2ss_udc *udc)
 	struct nbu2ss_req	*req;
 	struct nbu2ss_ep	*ep = &udc->ep[0];
 
-	if (list_empty(&ep->queue))
-		req = NULL;
-	else
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
-
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
 	if (!req)
 		req = &udc->ep0_req;
 
@@ -1838,11 +1826,7 @@ static inline int _nbu2ss_ep0_status_stage(struct nbu2ss_udc *udc)
 	struct nbu2ss_req	*req;
 	struct nbu2ss_ep	*ep = &udc->ep[0];
 
-	if (list_empty(&ep->queue))
-		req = NULL;
-	else
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
-
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
 	if (!req) {
 		req = &udc->ep0_req;
 		if (req->req.complete)
@@ -2145,11 +2129,7 @@ static inline void _nbu2ss_epn_int(struct nbu2ss_udc *udc, u32 epnum)
 	/* Interrupt Clear */
 	_nbu2ss_writel(&udc->p_regs->EP_REGS[num].EP_STATUS, ~(u32)status);
 
-	if (list_empty(&ep->queue))
-		req = NULL;
-	else
-		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
-
+	req = list_first_entry_or_null(&ep->queue, struct nbu2ss_req, queue);
 	if (!req) {
 		/* pr_warn("=== %s(%d) req == NULL\n", __func__, epnum); */
 		return;
@@ -2728,7 +2708,7 @@ static int nbu2ss_ep_queue(
 	spin_lock_irqsave(&udc->lock, flags);
 
 #ifdef USE_DMA
-	if ((u32)req->req.buf & 0x3)
+	if ((uintptr_t)req->req.buf & 0x3)
 		req->unaligned = TRUE;
 	else
 		req->unaligned = FALSE;
diff --git a/drivers/staging/fwserial/fwserial.c b/drivers/staging/fwserial/fwserial.c
index b3ea4bb54e2c..b676c486cb18 100644
--- a/drivers/staging/fwserial/fwserial.c
+++ b/drivers/staging/fwserial/fwserial.c
@@ -893,11 +893,10 @@ static void fwserial_destroy(struct kref *kref)
 	kfree(serial);
 }
 
-void fwtty_port_put(struct fwtty_port *port)
+static void fwtty_port_put(struct fwtty_port *port)
 {
 	kref_put(&port->serial->kref, fwserial_destroy);
 }
-EXPORT_SYMBOL(fwtty_port_put);
 
 static void fwtty_port_dtr_rts(struct tty_port *tty_port, int on)
 {
diff --git a/drivers/staging/fwserial/fwserial.h b/drivers/staging/fwserial/fwserial.h
index 787aa4f3a41b..e13fe33a6897 100644
--- a/drivers/staging/fwserial/fwserial.h
+++ b/drivers/staging/fwserial/fwserial.h
@@ -342,16 +342,6 @@ static const char loop_dev_name[] = "fwloop";
 extern struct tty_driver *fwtty_driver;
 
 struct fwtty_port *fwtty_port_get(unsigned index);
-void fwtty_port_put(struct fwtty_port *port);
-
-static inline void fwtty_bind_console(struct fwtty_port *port,
-				      struct fwconsole_ops *fwcon_ops,
-				      void *data)
-{
-	port->con_data = data;
-	port->fwcon_ops = fwcon_ops;
-}
-
 /*
  * Returns the max send async payload size in bytes based on the unit device
  * link speed. Self-limiting asynchronous bandwidth (via reducing the payload)
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index 79de678807cc..17d148f6e02c 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -555,7 +555,7 @@ int gdm_lte_event_init(void)
 void gdm_lte_event_exit(void)
 {
 	if (lte_event.sock && --lte_event.ref_cnt == 0) {
-		netlink_exit(lte_event.sock);
+		sock_release(lte_event.sock->sk_socket);
 		lte_event.sock = NULL;
 	}
 }
diff --git a/drivers/staging/gdm724x/gdm_tty.c b/drivers/staging/gdm724x/gdm_tty.c
index e2c0f228f369..eb7e2523c354 100644
--- a/drivers/staging/gdm724x/gdm_tty.c
+++ b/drivers/staging/gdm724x/gdm_tty.c
@@ -64,7 +64,7 @@ static void gdm_port_destruct(struct tty_port *port)
 	kfree(gdm);
 }
 
-static struct tty_port_operations gdm_port_ops = {
+static const struct tty_port_operations gdm_port_ops = {
 	.destruct = gdm_port_destruct,
 };
 
diff --git a/drivers/staging/gdm724x/netlink_k.c b/drivers/staging/gdm724x/netlink_k.c
index 92254fdaae1e..9d8347769e88 100644
--- a/drivers/staging/gdm724x/netlink_k.c
+++ b/drivers/staging/gdm724x/netlink_k.c
@@ -107,11 +107,6 @@ struct sock *netlink_init(int unit,
 	return sock;
 }
 
-void netlink_exit(struct sock *sock)
-{
-	sock_release(sock->sk_socket);
-}
-
 int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 {
 	static u32 seq;
diff --git a/drivers/staging/gdm724x/netlink_k.h b/drivers/staging/gdm724x/netlink_k.h
index 589486d76714..7cf979b3f826 100644
--- a/drivers/staging/gdm724x/netlink_k.h
+++ b/drivers/staging/gdm724x/netlink_k.h
@@ -19,7 +19,6 @@
 
 struct sock *netlink_init(int unit,
 	void (*cb)(struct net_device *dev, u16 type, void *msg, int len));
-void netlink_exit(struct sock *sock);
 int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len);
 
 #endif /* _NETLINK_K_H_ */
diff --git a/drivers/staging/gdm72xx/gdm_qos.c b/drivers/staging/gdm72xx/gdm_qos.c
index 81feffa5784a..cad347a05d18 100644
--- a/drivers/staging/gdm72xx/gdm_qos.c
+++ b/drivers/staging/gdm72xx/gdm_qos.c
@@ -101,7 +101,7 @@ void gdm_qos_init(void *nic_ptr)
 	}
 
 	qcb->qos_list_cnt = 0;
-	qcb->qos_null_idx = QOS_MAX-1;
+	qcb->qos_null_idx = QOS_MAX - 1;
 	qcb->qos_limit_size = 255;
 
 	spin_lock_init(&qcb->qos_lock);
@@ -128,7 +128,7 @@ void gdm_qos_release_list(void *nic_ptr)
 	}
 
 	qcb->qos_list_cnt = 0;
-	qcb->qos_null_idx = QOS_MAX-1;
+	qcb->qos_null_idx = QOS_MAX - 1;
 
 	for (i = 0; i < QOS_MAX; i++) {
 		list_for_each_entry_safe(entry, n, &qcb->qos_list[i], list) {
@@ -143,18 +143,18 @@ static int chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *stream, u8 *port)
 {
 	int i;
 
-	if (csr->classifier_rule_en&IPTYPEOFSERVICE) {
+	if (csr->classifier_rule_en & IPTYPEOFSERVICE) {
 		if (((stream[1] & csr->ip2s_mask) < csr->ip2s_lo) ||
 		    ((stream[1] & csr->ip2s_mask) > csr->ip2s_hi))
 			return 1;
 	}
 
-	if (csr->classifier_rule_en&PROTOCOL) {
+	if (csr->classifier_rule_en & PROTOCOL) {
 		if (stream[9] != csr->protocol)
 			return 1;
 	}
 
-	if (csr->classifier_rule_en&IPMASKEDSRCADDRESS) {
+	if (csr->classifier_rule_en & IPMASKEDSRCADDRESS) {
 		for (i = 0; i < 4; i++) {
 			if ((stream[12 + i] & csr->ipsrc_addrmask[i]) !=
 			(csr->ipsrc_addr[i] & csr->ipsrc_addrmask[i]))
@@ -162,7 +162,7 @@ static int chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *stream, u8 *port)
 		}
 	}
 
-	if (csr->classifier_rule_en&IPMASKEDDSTADDRESS) {
+	if (csr->classifier_rule_en & IPMASKEDDSTADDRESS) {
 		for (i = 0; i < 4; i++) {
 			if ((stream[16 + i] & csr->ipdst_addrmask[i]) !=
 			(csr->ipdst_addr[i] & csr->ipdst_addrmask[i]))
@@ -170,14 +170,14 @@ static int chk_ipv4_rule(struct gdm_wimax_csr_s *csr, u8 *stream, u8 *port)
 		}
 	}
 
-	if (csr->classifier_rule_en&PROTOCOLSRCPORTRANGE) {
-		i = ((port[0]<<8)&0xff00)+port[1];
+	if (csr->classifier_rule_en & PROTOCOLSRCPORTRANGE) {
+		i = ((port[0] << 8) & 0xff00) + port[1];
 		if ((i < csr->srcport_lo) || (i > csr->srcport_hi))
 			return 1;
 	}
 
-	if (csr->classifier_rule_en&PROTOCOLDSTPORTRANGE) {
-		i = ((port[2]<<8)&0xff00)+port[3];
+	if (csr->classifier_rule_en & PROTOCOLDSTPORTRANGE) {
+		i = ((port[2] << 8) & 0xff00) + port[3];
 		if ((i < csr->dstport_lo) || (i > csr->dstport_hi))
 			return 1;
 	}
@@ -193,7 +193,7 @@ static int get_qos_index(struct nic *nic, u8 *iph, u8 *tcpudph)
 	if (!iph || !tcpudph)
 		return -1;
 
-	ip_ver = (iph[0]>>4)&0xf;
+	ip_ver = (iph[0] >> 4) & 0xf;
 
 	if (ip_ver != 4)
 		return -1;
@@ -342,17 +342,17 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 	if (sub_cmd_evt == QOS_REPORT) {
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		for (i = 0; i < qcb->qos_list_cnt; i++) {
-			sfid = ((buf[(i*5)+6]<<24)&0xff000000);
-			sfid += ((buf[(i*5)+7]<<16)&0xff0000);
-			sfid += ((buf[(i*5)+8]<<8)&0xff00);
-			sfid += (buf[(i*5)+9]);
+			sfid = ((buf[(i*5) + 6] << 24) & 0xff000000);
+			sfid += ((buf[(i*5) + 7] << 16) & 0xff0000);
+			sfid += ((buf[(i*5) + 8] << 8) & 0xff00);
+			sfid += (buf[(i*5) + 9]);
 			index = get_csr(qcb, sfid, 0);
 			if (index == -1) {
 				spin_unlock_irqrestore(&qcb->qos_lock, flags);
 				netdev_err(nic->netdev, "QoS ERROR: No SF\n");
 				return;
 			}
-			qcb->csr[index].qos_buf_count = buf[(i*5)+10];
+			qcb->csr[index].qos_buf_count = buf[(i*5) + 10];
 		}
 
 		extract_qos_list(nic, &send_list);
@@ -363,9 +363,9 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 
 	/* sub_cmd_evt == QOS_ADD || sub_cmd_evt == QOS_CHANG_DEL */
 	pos = 6;
-	sfid = ((buf[pos++]<<24)&0xff000000);
-	sfid += ((buf[pos++]<<16)&0xff0000);
-	sfid += ((buf[pos++]<<8)&0xff00);
+	sfid = ((buf[pos++] << 24) & 0xff000000);
+	sfid += ((buf[pos++] << 16) & 0xff0000);
+	sfid += ((buf[pos++] << 8) & 0xff00);
 	sfid += (buf[pos++]);
 
 	index = get_csr(qcb, sfid, 1);
@@ -382,7 +382,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		qcb->csr[index].sfid = sfid;
-		qcb->csr[index].classifier_rule_en = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].classifier_rule_en = ((buf[pos++] << 8) & 0xff00);
 		qcb->csr[index].classifier_rule_en += buf[pos++];
 		if (qcb->csr[index].classifier_rule_en == 0)
 			qcb->qos_null_idx = index;
@@ -406,16 +406,16 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		qcb->csr[index].ipdst_addr[1] = buf[pos++];
 		qcb->csr[index].ipdst_addr[2] = buf[pos++];
 		qcb->csr[index].ipdst_addr[3] = buf[pos++];
-		qcb->csr[index].srcport_lo = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].srcport_lo = ((buf[pos++] << 8) & 0xff00);
 		qcb->csr[index].srcport_lo += buf[pos++];
-		qcb->csr[index].srcport_hi = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].srcport_hi = ((buf[pos++] << 8) & 0xff00);
 		qcb->csr[index].srcport_hi += buf[pos++];
-		qcb->csr[index].dstport_lo = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].dstport_lo = ((buf[pos++] << 8) & 0xff00);
 		qcb->csr[index].dstport_lo += buf[pos++];
-		qcb->csr[index].dstport_hi = ((buf[pos++]<<8)&0xff00);
+		qcb->csr[index].dstport_hi = ((buf[pos++] << 8) & 0xff00);
 		qcb->csr[index].dstport_hi += buf[pos++];
 
-		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
+		qcb->qos_limit_size = 254 / qcb->qos_list_cnt;
 		spin_unlock_irqrestore(&qcb->qos_lock, flags);
 	} else if (sub_cmd_evt == QOS_CHANGE_DEL) {
 		netdev_dbg(nic->netdev, "QOS_CHANGE_DEL SFID = 0x%x, index=%d\n",
@@ -426,7 +426,7 @@ void gdm_recv_qos_hci_packet(void *nic_ptr, u8 *buf, int size)
 		spin_lock_irqsave(&qcb->qos_lock, flags);
 		qcb->csr[index].enabled = false;
 		qcb->qos_list_cnt--;
-		qcb->qos_limit_size = 254/qcb->qos_list_cnt;
+		qcb->qos_limit_size = 254 / qcb->qos_list_cnt;
 
 		list_for_each_entry_safe(entry, n, &qcb->qos_list[index],
 					 list) {
diff --git a/drivers/staging/gdm72xx/gdm_sdio.c b/drivers/staging/gdm72xx/gdm_sdio.c
index b0521da3c793..1f5a087723ba 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.c
+++ b/drivers/staging/gdm72xx/gdm_sdio.c
@@ -36,7 +36,7 @@
 #define RX_BUF_SIZE	(25*1024)
 
 #define TX_HZ		2000
-#define TX_INTERVAL	(1000000/TX_HZ)
+#define TX_INTERVAL	(NSEC_PER_SEC/TX_HZ)
 
 static struct sdio_tx *alloc_tx_struct(struct tx_cxt *tx)
 {
@@ -303,7 +303,7 @@ static void send_sdu(struct sdio_func *func, struct tx_cxt *tx)
 		put_tx_struct(t->tx_cxt, t);
 	}
 
-	do_gettimeofday(&tx->sdu_stamp);
+	tx->sdu_stamp = ktime_get();
 	spin_unlock_irqrestore(&tx->lock, flags);
 }
 
@@ -330,7 +330,7 @@ static void do_tx(struct work_struct *work)
 	struct sdio_func *func = sdev->func;
 	struct tx_cxt *tx = &sdev->tx;
 	struct sdio_tx *t = NULL;
-	struct timeval now, *before;
+	ktime_t now, before;
 	int is_sdu = 0;
 	long diff;
 	unsigned long flags;
@@ -346,11 +346,10 @@ static void do_tx(struct work_struct *work)
 		list_del(&t->list);
 		is_sdu = 0;
 	} else if (!tx->stop_sdu_tx && !list_empty(&tx->sdu_list)) {
-		do_gettimeofday(&now);
-		before = &tx->sdu_stamp;
+		now = ktime_get();
+		before = tx->sdu_stamp;
 
-		diff = (now.tv_sec - before->tv_sec) * 1000000 +
-			(now.tv_usec - before->tv_usec);
+		diff = ktime_to_ns(ktime_sub(now, before));
 		if (diff >= 0 && diff < TX_INTERVAL) {
 			schedule_work(&sdev->ws);
 			spin_unlock_irqrestore(&tx->lock, flags);
diff --git a/drivers/staging/gdm72xx/gdm_sdio.h b/drivers/staging/gdm72xx/gdm_sdio.h
index 77ad9d686f8e..aa7dad22a219 100644
--- a/drivers/staging/gdm72xx/gdm_sdio.h
+++ b/drivers/staging/gdm72xx/gdm_sdio.h
@@ -15,7 +15,7 @@
 #define __GDM72XX_GDM_SDIO_H__
 
 #include <linux/types.h>
-#include <linux/time.h>
+#include <linux/ktime.h>
 
 #define MAX_NR_SDU_BUF  64
 
@@ -32,7 +32,7 @@ struct tx_cxt {
 	struct list_head	free_list;
 	struct list_head	sdu_list;
 	struct list_head	hci_list;
-	struct timeval		sdu_stamp;
+	ktime_t			sdu_stamp;
 	u8			*sdu_buf;
 	spinlock_t		lock;
 	int			can_send;
diff --git a/drivers/staging/gdm72xx/gdm_wimax.c b/drivers/staging/gdm72xx/gdm_wimax.c
index d9ddced96e19..ba03f9386567 100644
--- a/drivers/staging/gdm72xx/gdm_wimax.c
+++ b/drivers/staging/gdm72xx/gdm_wimax.c
@@ -84,11 +84,6 @@ static inline struct evt_entry *alloc_event_entry(void)
 	return kmalloc(sizeof(struct evt_entry), GFP_ATOMIC);
 }
 
-static inline void free_event_entry(struct evt_entry *e)
-{
-	kfree(e);
-}
-
 static struct evt_entry *get_event_entry(void)
 {
 	struct evt_entry *e;
@@ -180,11 +175,11 @@ static void gdm_wimax_event_exit(void)
 
 		list_for_each_entry_safe(e, temp, &wm_event.evtq, list) {
 			list_del(&e->list);
-			free_event_entry(e);
+			kfree(e);
 		}
 		list_for_each_entry_safe(e, temp, &wm_event.freeq, list) {
 			list_del(&e->list);
-			free_event_entry(e);
+			kfree(e);
 		}
 
 		spin_unlock_irqrestore(&wm_event.evt_lock, flags);
@@ -368,7 +363,7 @@ static void kdelete(void **buf)
 	}
 }
 
-static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
+static int gdm_wimax_ioctl_get_data(struct udata_s *dst, struct data_s *src)
 {
 	int size;
 
@@ -384,7 +379,7 @@ static int gdm_wimax_ioctl_get_data(struct data_s *dst, struct data_s *src)
 	return 0;
 }
 
-static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct data_s *src)
+static int gdm_wimax_ioctl_set_data(struct data_s *dst, struct udata_s *src)
 {
 	if (!src->size) {
 		dst->size = 0;
@@ -460,6 +455,7 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	struct wm_req_s *req = (struct wm_req_s *)ifr;
 	struct nic *nic = netdev_priv(dev);
 	int ret;
+	struct fsm_s fsm_buf;
 
 	if (cmd != SIOCWMIOCTL)
 		return -EOPNOTSUPP;
@@ -482,8 +478,11 @@ static int gdm_wimax_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 				/* NOTE: gdm_update_fsm should be called
 				 * before gdm_wimax_ioctl_set_data is called.
 				 */
-				gdm_update_fsm(dev,
-					       req->data.buf);
+				if (copy_from_user(&fsm_buf, req->data.buf,
+						   sizeof(struct fsm_s)))
+					return -EFAULT;
+
+				gdm_update_fsm(dev, &fsm_buf);
 			}
 			ret = gdm_wimax_ioctl_set_data(
 				&nic->sdk_data[req->data_id], &req->data);
diff --git a/drivers/staging/gdm72xx/wm_ioctl.h b/drivers/staging/gdm72xx/wm_ioctl.h
index ed8f649c0042..631cb1d23c7e 100644
--- a/drivers/staging/gdm72xx/wm_ioctl.h
+++ b/drivers/staging/gdm72xx/wm_ioctl.h
@@ -78,13 +78,18 @@ struct data_s {
 	void	*buf;
 };
 
+struct udata_s {
+	int		size;
+	void __user	*buf;
+};
+
 struct wm_req_s {
 	union {
 		char ifrn_name[IFNAMSIZ];
 	} ifr_ifrn;
 	unsigned short	cmd;
 	unsigned short	data_id;
-	struct data_s	data;
+	struct udata_s	data;
 
 /* NOTE: sizeof(struct wm_req_s) must be less than sizeof(struct ifreq). */
 };
diff --git a/drivers/staging/iio/Kconfig b/drivers/staging/iio/Kconfig
index 9d7f0004d2d7..0e044cb0def8 100644
--- a/drivers/staging/iio/Kconfig
+++ b/drivers/staging/iio/Kconfig
@@ -17,33 +17,4 @@ source "drivers/staging/iio/meter/Kconfig"
 source "drivers/staging/iio/resolver/Kconfig"
 source "drivers/staging/iio/trigger/Kconfig"
 
-config IIO_DUMMY_EVGEN
-	tristate
-	select IRQ_WORK
-
-config IIO_SIMPLE_DUMMY
-       tristate "An example driver with no hardware requirements"
-       help
-	 Driver intended mainly as documentation for how to write
-	 a driver. May also be useful for testing userspace code
-	 without hardware.
-
-if IIO_SIMPLE_DUMMY
-
-config IIO_SIMPLE_DUMMY_EVENTS
-       bool "Event generation support"
-       select IIO_DUMMY_EVGEN
-       help
-         Add some dummy events to the simple dummy driver.
-
-config IIO_SIMPLE_DUMMY_BUFFER
-	bool "Buffered capture support"
-	select IIO_BUFFER
-	select IIO_TRIGGER
-	select IIO_KFIFO_BUF
-	help
-	  Add buffered data capture to the simple dummy driver.
-
-endif # IIO_SIMPLE_DUMMY
-
 endmenu
diff --git a/drivers/staging/iio/Makefile b/drivers/staging/iio/Makefile
index d87106135b27..3e616b4437f5 100644
--- a/drivers/staging/iio/Makefile
+++ b/drivers/staging/iio/Makefile
@@ -2,13 +2,6 @@
 # Makefile for the industrial I/O core.
 #
 
-obj-$(CONFIG_IIO_SIMPLE_DUMMY) += iio_dummy.o
-iio_dummy-y := iio_simple_dummy.o
-iio_dummy-$(CONFIG_IIO_SIMPLE_DUMMY_EVENTS) += iio_simple_dummy_events.o
-iio_dummy-$(CONFIG_IIO_SIMPLE_DUMMY_BUFFER) += iio_simple_dummy_buffer.o
-
-obj-$(CONFIG_IIO_DUMMY_EVGEN) += iio_dummy_evgen.o
-
 obj-y += accel/
 obj-y += adc/
 obj-y += addac/
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index 20b878d35ea2..1920dc60cf3d 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -48,7 +48,7 @@ static int sca3000_read_data(struct sca3000_state *st,
 		}
 	};
 	*rx_p = kmalloc(len, GFP_KERNEL);
-	if (*rx_p == NULL) {
+	if (!*rx_p) {
 		ret = -ENOMEM;
 		goto error_ret;
 	}
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index 94ae4232ee77..b9519be90fda 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -6,6 +6,7 @@ menu "Analog to digital converters"
 config AD7606
 	tristate "Analog Devices AD7606 ADC driver"
 	depends on GPIOLIB || COMPILE_TEST
+	depends on HAS_IOMEM
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
@@ -23,7 +24,7 @@ config AD7606_IFACE_PARALLEL
 	  ADC driver.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called ad7606_iface_parallel.
+	  module will be called ad7606_parallel.
 
 config AD7606_IFACE_SPI
 	tristate "spi interface support"
@@ -34,7 +35,7 @@ config AD7606_IFACE_SPI
 	  ADC driver.
 
 	  To compile this driver as a module, choose M here: the
-	  module will be called ad7606_iface_spi.
+	  module will be called ad7606_spi.
 
 config AD7780
 	tristate "Analog Devices AD7780 and similar ADCs driver"
diff --git a/drivers/staging/iio/adc/Makefile b/drivers/staging/iio/adc/Makefile
index 1c4277dbd318..0c87ce3530f8 100644
--- a/drivers/staging/iio/adc/Makefile
+++ b/drivers/staging/iio/adc/Makefile
@@ -2,10 +2,9 @@
 # Makefile for industrial I/O ADC drivers
 #
 
-ad7606-y := ad7606_core.o
-ad7606-$(CONFIG_IIO_BUFFER) += ad7606_ring.o
-ad7606-$(CONFIG_AD7606_IFACE_PARALLEL) += ad7606_par.o
-ad7606-$(CONFIG_AD7606_IFACE_SPI) += ad7606_spi.o
+ad7606-y := ad7606_core.o ad7606_ring.o
+obj-$(CONFIG_AD7606_IFACE_PARALLEL) += ad7606_par.o
+obj-$(CONFIG_AD7606_IFACE_SPI) += ad7606_spi.o
 obj-$(CONFIG_AD7606) += ad7606.o
 
 obj-$(CONFIG_AD7780) += ad7780.o
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index bb40f3728742..92211039ffa9 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -609,7 +609,7 @@ static const struct iio_chan_spec ad7192_channels[] = {
 
 static int ad7192_probe(struct spi_device *spi)
 {
-	const struct ad7192_platform_data *pdata = spi->dev.platform_data;
+	const struct ad7192_platform_data *pdata = dev_get_platdata(&spi->dev);
 	struct ad7192_state *st;
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
diff --git a/drivers/staging/iio/adc/ad7280a.c b/drivers/staging/iio/adc/ad7280a.c
index 35acb1a4669b..f45ebedb7a05 100644
--- a/drivers/staging/iio/adc/ad7280a.c
+++ b/drivers/staging/iio/adc/ad7280a.c
@@ -833,7 +833,7 @@ static const struct ad7280_platform_data ad7793_default_pdata = {
 
 static int ad7280_probe(struct spi_device *spi)
 {
-	const struct ad7280_platform_data *pdata = spi->dev.platform_data;
+	const struct ad7280_platform_data *pdata = dev_get_platdata(&spi->dev);
 	struct ad7280_state *st;
 	int ret;
 	const unsigned short tACQ_ns[4] = {465, 1010, 1460, 1890};
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index 5796ed2409d0..2c9d8b7de9f5 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -559,6 +559,7 @@ error_disable_reg:
 		regulator_disable(st->reg);
 	return ERR_PTR(ret);
 }
+EXPORT_SYMBOL_GPL(ad7606_probe);
 
 int ad7606_remove(struct iio_dev *indio_dev, int irq)
 {
@@ -575,6 +576,7 @@ int ad7606_remove(struct iio_dev *indio_dev, int irq)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(ad7606_remove);
 
 void ad7606_suspend(struct iio_dev *indio_dev)
 {
@@ -586,6 +588,7 @@ void ad7606_suspend(struct iio_dev *indio_dev)
 		gpio_set_value(st->pdata->gpio_stby, 0);
 	}
 }
+EXPORT_SYMBOL_GPL(ad7606_suspend);
 
 void ad7606_resume(struct iio_dev *indio_dev)
 {
@@ -600,6 +603,7 @@ void ad7606_resume(struct iio_dev *indio_dev)
 		ad7606_reset(st);
 	}
 }
+EXPORT_SYMBOL_GPL(ad7606_resume);
 
 MODULE_AUTHOR("Michael Hennerich <hennerich@blackfin.uclinux.org>");
 MODULE_DESCRIPTION("Analog Devices AD7606 ADC");
diff --git a/drivers/staging/iio/adc/ad7780.c b/drivers/staging/iio/adc/ad7780.c
index 3abc7789237f..1439cfdbb09c 100644
--- a/drivers/staging/iio/adc/ad7780.c
+++ b/drivers/staging/iio/adc/ad7780.c
@@ -15,15 +15,13 @@
 #include <linux/regulator/consumer.h>
 #include <linux/err.h>
 #include <linux/sched.h>
-#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
 #include <linux/module.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/adc/ad_sigma_delta.h>
 
-#include "ad7780.h"
-
 #define AD7780_RDY	BIT(7)
 #define AD7780_FILTER	BIT(6)
 #define AD7780_ERR	BIT(5)
@@ -42,7 +40,7 @@ struct ad7780_chip_info {
 struct ad7780_state {
 	const struct ad7780_chip_info	*chip_info;
 	struct regulator		*reg;
-	int				powerdown_gpio;
+	struct gpio_desc		*powerdown_gpio;
 	unsigned int	gain;
 	u16				int_vref_mv;
 
@@ -77,8 +75,7 @@ static int ad7780_set_mode(struct ad_sigma_delta *sigma_delta,
 		break;
 	}
 
-	if (gpio_is_valid(st->powerdown_gpio))
-		gpio_set_value(st->powerdown_gpio, val);
+	gpiod_set_value(st->powerdown_gpio, val);
 
 	return 0;
 }
@@ -163,7 +160,6 @@ static const struct iio_info ad7780_info = {
 
 static int ad7780_probe(struct spi_device *spi)
 {
-	struct ad7780_platform_data *pdata = spi->dev.platform_data;
 	struct ad7780_state *st;
 	struct iio_dev *indio_dev;
 	int ret, voltage_uv = 0;
@@ -189,12 +185,10 @@ static int ad7780_probe(struct spi_device *spi)
 	st->chip_info =
 		&ad7780_chip_info_tbl[spi_get_device_id(spi)->driver_data];
 
-	if (pdata && pdata->vref_mv)
-		st->int_vref_mv = pdata->vref_mv;
-	else if (voltage_uv)
+	if (voltage_uv)
 		st->int_vref_mv = voltage_uv / 1000;
 	else
-		dev_warn(&spi->dev, "reference voltage unspecified\n");
+		dev_warn(&spi->dev, "Reference voltage unspecified\n");
 
 	spi_set_drvdata(spi, indio_dev);
 
@@ -205,18 +199,14 @@ static int ad7780_probe(struct spi_device *spi)
 	indio_dev->num_channels = 1;
 	indio_dev->info = &ad7780_info;
 
-	if (pdata && gpio_is_valid(pdata->gpio_pdrst)) {
-		ret = devm_gpio_request_one(&spi->dev,
-					    pdata->gpio_pdrst,
-					    GPIOF_OUT_INIT_LOW,
-					    "AD7780 /PDRST");
-		if (ret) {
-			dev_err(&spi->dev, "failed to request GPIO PDRST\n");
-			goto error_disable_reg;
-		}
-		st->powerdown_gpio = pdata->gpio_pdrst;
-	} else {
-		st->powerdown_gpio = -1;
+	st->powerdown_gpio = devm_gpiod_get_optional(&spi->dev,
+						     "powerdown",
+						     GPIOD_OUT_LOW);
+	if (IS_ERR(st->powerdown_gpio)) {
+		ret = PTR_ERR(st->powerdown_gpio);
+		dev_err(&spi->dev, "Failed to request powerdown GPIO: %d\n",
+			ret);
+		goto error_disable_reg;
 	}
 
 	ret = ad_sd_setup_buffer_and_trigger(indio_dev);
diff --git a/drivers/staging/iio/adc/ad7780.h b/drivers/staging/iio/adc/ad7780.h
deleted file mode 100644
index 67e511c3d6f0..000000000000
--- a/drivers/staging/iio/adc/ad7780.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * AD7780/AD7781 SPI ADC driver
- *
- * Copyright 2011 Analog Devices Inc.
- *
- * Licensed under the GPL-2.
- */
-#ifndef IIO_ADC_AD7780_H_
-#define IIO_ADC_AD7780_H_
-
-/*
- * TODO: struct ad7780_platform_data needs to go into include/linux/iio
- */
-
-/* NOTE:
- * The AD7780 doesn't feature a dedicated SPI chip select, in addition it
- * features a dual use data out ready DOUT/RDY output.
- * In order to avoid contentions on the SPI bus, it's therefore necessary
- * to use spi bus locking combined with a dedicated GPIO to control the
- * power down reset signal of the AD7780.
- *
- * The DOUT/RDY output must also be wired to an interrupt capable GPIO.
- */
-
-struct ad7780_platform_data {
-	u16				vref_mv;
-	int				gpio_pdrst;
-};
-
-#endif /* IIO_ADC_AD7780_H_ */
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index c8e156646528..22260512cf01 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -345,7 +345,7 @@ static int ad7816_probe(struct spi_device *spi_dev)
 {
 	struct ad7816_chip_info *chip;
 	struct iio_dev *indio_dev;
-	unsigned short *pins = spi_dev->dev.platform_data;
+	unsigned short *pins = dev_get_platdata(&spi_dev->dev);
 	int ret = 0;
 	int i;
 
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index d997d9c74ca8..bb1f15224ac8 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -319,12 +319,12 @@ struct mxs_lradc {
 #define	LRADC_CH_VALUE_OFFSET			0
 
 #define	LRADC_DELAY(n)				(0xd0 + (0x10 * (n)))
-#define	LRADC_DELAY_TRIGGER_LRADCS_MASK		(0xff << 24)
+#define	LRADC_DELAY_TRIGGER_LRADCS_MASK		(0xffUL << 24)
 #define	LRADC_DELAY_TRIGGER_LRADCS_OFFSET	24
 #define	LRADC_DELAY_TRIGGER(x) \
 				(((x) << LRADC_DELAY_TRIGGER_LRADCS_OFFSET) & \
 				LRADC_DELAY_TRIGGER_LRADCS_MASK)
-#define	LRADC_DELAY_KICK			(1 << 20)
+#define	LRADC_DELAY_KICK			BIT(20)
 #define	LRADC_DELAY_TRIGGER_DELAYS_MASK		(0xf << 16)
 #define	LRADC_DELAY_TRIGGER_DELAYS_OFFSET	16
 #define	LRADC_DELAY_TRIGGER_DELAYS(x) \
diff --git a/drivers/staging/iio/frequency/ad9832.c b/drivers/staging/iio/frequency/ad9832.c
index 2b65faa6296a..18b27a1984b2 100644
--- a/drivers/staging/iio/frequency/ad9832.c
+++ b/drivers/staging/iio/frequency/ad9832.c
@@ -201,7 +201,7 @@ static const struct iio_info ad9832_info = {
 
 static int ad9832_probe(struct spi_device *spi)
 {
-	struct ad9832_platform_data *pdata = spi->dev.platform_data;
+	struct ad9832_platform_data *pdata = dev_get_platdata(&spi->dev);
 	struct iio_dev *indio_dev;
 	struct ad9832_state *st;
 	struct regulator *reg;
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index 6464f2cbe94b..6366216e4f37 100644
--- a/drivers/staging/iio/frequency/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -318,7 +318,7 @@ static const struct iio_info ad9833_info = {
 
 static int ad9834_probe(struct spi_device *spi)
 {
-	struct ad9834_platform_data *pdata = spi->dev.platform_data;
+	struct ad9834_platform_data *pdata = dev_get_platdata(&spi->dev);
 	struct ad9834_state *st;
 	struct iio_dev *indio_dev;
 	struct regulator *reg;
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index 9dfd04855a1b..5b1c1650a0e4 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -1898,7 +1898,7 @@ static int tsl2x7x_probe(struct i2c_client *clientp,
 	mutex_init(&chip->prox_mutex);
 
 	chip->tsl2x7x_chip_status = TSL2X7X_CHIP_UNKNOWN;
-	chip->pdata = clientp->dev.platform_data;
+	chip->pdata = dev_get_platdata(&clientp->dev);
 	chip->id = id->driver_data;
 	chip->chip_info =
 		&tsl2x7x_chip_info_tbl[device_channel_config[id->driver_data]];
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index f129039bece3..69287108f793 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -217,8 +217,12 @@ error_ret:
 static int ade7753_reset(struct device *dev)
 {
 	u16 val;
+	int ret;
+
+	ret = ade7753_spi_read_reg_16(dev, ADE7753_MODE, &val);
+	if (ret)
+		return ret;
 
-	ade7753_spi_read_reg_16(dev, ADE7753_MODE, &val);
 	val |= BIT(6); /* Software Chip Reset */
 
 	return ade7753_spi_write_reg_16(dev, ADE7753_MODE, val);
@@ -343,8 +347,12 @@ error_ret:
 static int ade7753_stop_device(struct device *dev)
 {
 	u16 val;
+	int ret;
+
+	ret = ade7753_spi_read_reg_16(dev, ADE7753_MODE, &val);
+	if (ret)
+		return ret;
 
-	ade7753_spi_read_reg_16(dev, ADE7753_MODE, &val);
 	val |= BIT(4);  /* AD converters can be turned off */
 
 	return ade7753_spi_write_reg_16(dev, ADE7753_MODE, val);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
index 4d74e8af5088..0d8a91ee5ffc 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -42,13 +42,6 @@
 
 #include "curproc.h"
 
-static inline int __is_po2(unsigned long long val)
-{
-	return !(val & (val - 1));
-}
-
-#define IS_PO2(val) __is_po2((unsigned long long)(val))
-
 #define LOWEST_BIT_SET(x)       ((x) & ~((x) - 1))
 
 /*
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
index 787867847483..1530b0458a61 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
@@ -22,7 +22,8 @@
  */
 /*
  * Copyright (c) 2010, Oracle and/or its affiliates. All rights reserved.
- * Copyright (c) 2012, Intel Corporation.
+ *
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
index 70b8b29e831c..c3f2332fa043 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_hash.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -41,6 +41,9 @@
 
 #ifndef __LIBCFS_HASH_H__
 #define __LIBCFS_HASH_H__
+
+#include <linux/hash.h>
+
 /*
  * Knuth recommends primes in approximately golden ratio to the maximum
  * integer representable by a machine word for multiplicative hashing.
@@ -56,22 +59,13 @@
 /*  2^63 + 2^61 - 2^57 + 2^54 - 2^51 - 2^18 + 1 */
 #define CFS_GOLDEN_RATIO_PRIME_64 0x9e37fffffffc0001ULL
 
-/*
- * Ideally we would use HAVE_HASH_LONG for this, but on linux we configure
- * the linux kernel and user space at the same time, so we need to differentiate
- * between them explicitly. If this is not needed on other architectures, then
- * we'll need to move the functions to architecture specific headers.
- */
-
-#include <linux/hash.h>
-
 /** disable debug */
-#define CFS_HASH_DEBUG_NONE	 0
+#define CFS_HASH_DEBUG_NONE	0
 /** record hash depth and output to console when it's too deep,
  *  computing overhead is low but consume more memory */
-#define CFS_HASH_DEBUG_1	    1
+#define CFS_HASH_DEBUG_1	1
 /** expensive, check key validation */
-#define CFS_HASH_DEBUG_2	    2
+#define CFS_HASH_DEBUG_2	2
 
 #define CFS_HASH_DEBUG_LEVEL	CFS_HASH_DEBUG_NONE
 
@@ -108,16 +102,18 @@ struct cfs_hash_bucket {
  * cfs_hash bucket descriptor, it's normally in stack of caller
  */
 struct cfs_hash_bd {
-	struct cfs_hash_bucket	*bd_bucket;      /**< address of bucket */
-	unsigned int		bd_offset;      /**< offset in bucket */
+	/* address of bucket */
+	struct cfs_hash_bucket	*bd_bucket;
+	/* offset in bucket */
+	unsigned int		 bd_offset;
 };
 
-#define CFS_HASH_NAME_LEN	   16      /**< default name length */
-#define CFS_HASH_BIGNAME_LEN	64      /**< bigname for param tree */
+#define CFS_HASH_NAME_LEN	16	/**< default name length */
+#define CFS_HASH_BIGNAME_LEN	64	/**< bigname for param tree */
 
-#define CFS_HASH_BKT_BITS	   3       /**< default bits of bucket */
-#define CFS_HASH_BITS_MAX	   30      /**< max bits of bucket */
-#define CFS_HASH_BITS_MIN	   CFS_HASH_BKT_BITS
+#define CFS_HASH_BKT_BITS	3	/**< default bits of bucket */
+#define CFS_HASH_BITS_MAX	30	/**< max bits of bucket */
+#define CFS_HASH_BITS_MIN	CFS_HASH_BKT_BITS
 
 /**
  * common hash attributes.
@@ -133,41 +129,41 @@ enum cfs_hash_tag {
 	 */
 	CFS_HASH_NO_LOCK	= 1 << 0,
 	/** no bucket lock, use one spinlock to protect the whole hash */
-	CFS_HASH_NO_BKTLOCK     = 1 << 1,
+	CFS_HASH_NO_BKTLOCK	= 1 << 1,
 	/** rwlock to protect bucket */
-	CFS_HASH_RW_BKTLOCK     = 1 << 2,
+	CFS_HASH_RW_BKTLOCK	= 1 << 2,
 	/** spinlock to protect bucket */
-	CFS_HASH_SPIN_BKTLOCK   = 1 << 3,
+	CFS_HASH_SPIN_BKTLOCK	= 1 << 3,
 	/** always add new item to tail */
-	CFS_HASH_ADD_TAIL       = 1 << 4,
+	CFS_HASH_ADD_TAIL	= 1 << 4,
 	/** hash-table doesn't have refcount on item */
-	CFS_HASH_NO_ITEMREF     = 1 << 5,
+	CFS_HASH_NO_ITEMREF	= 1 << 5,
 	/** big name for param-tree */
 	CFS_HASH_BIGNAME	= 1 << 6,
 	/** track global count */
 	CFS_HASH_COUNTER	= 1 << 7,
 	/** rehash item by new key */
-	CFS_HASH_REHASH_KEY     = 1 << 8,
+	CFS_HASH_REHASH_KEY	= 1 << 8,
 	/** Enable dynamic hash resizing */
-	CFS_HASH_REHASH	 = 1 << 9,
+	CFS_HASH_REHASH		= 1 << 9,
 	/** can shrink hash-size */
-	CFS_HASH_SHRINK	 = 1 << 10,
+	CFS_HASH_SHRINK		= 1 << 10,
 	/** assert hash is empty on exit */
-	CFS_HASH_ASSERT_EMPTY   = 1 << 11,
+	CFS_HASH_ASSERT_EMPTY	= 1 << 11,
 	/** record hlist depth */
-	CFS_HASH_DEPTH	  = 1 << 12,
+	CFS_HASH_DEPTH		= 1 << 12,
 	/**
 	 * rehash is always scheduled in a different thread, so current
 	 * change on hash table is non-blocking
 	 */
-	CFS_HASH_NBLK_CHANGE    = 1 << 13,
+	CFS_HASH_NBLK_CHANGE	= 1 << 13,
 	/** NB, we typed hs_flags as  __u16, please change it
 	 * if you need to extend >=16 flags */
 };
 
 /** most used attributes */
-#define CFS_HASH_DEFAULT       (CFS_HASH_RW_BKTLOCK | \
-				CFS_HASH_COUNTER | CFS_HASH_REHASH)
+#define CFS_HASH_DEFAULT	(CFS_HASH_RW_BKTLOCK | \
+				 CFS_HASH_COUNTER | CFS_HASH_REHASH)
 
 /**
  * cfs_hash is a hash-table implementation for general purpose, it can support:
@@ -211,7 +207,7 @@ enum cfs_hash_tag {
 struct cfs_hash {
 	/** serialize with rehash, or serialize all operations if
 	 * the hash-table has CFS_HASH_NO_BKTLOCK */
-	union cfs_hash_lock		 hs_lock;
+	union cfs_hash_lock		hs_lock;
 	/** hash operations */
 	struct cfs_hash_ops		*hs_ops;
 	/** hash lock operations */
@@ -219,57 +215,57 @@ struct cfs_hash {
 	/** hash list operations */
 	struct cfs_hash_hlist_ops	*hs_hops;
 	/** hash buckets-table */
-	struct cfs_hash_bucket	 **hs_buckets;
+	struct cfs_hash_bucket		**hs_buckets;
 	/** total number of items on this hash-table */
-	atomic_t		hs_count;
+	atomic_t			hs_count;
 	/** hash flags, see cfs_hash_tag for detail */
-	__u16		       hs_flags;
+	__u16				hs_flags;
 	/** # of extra-bytes for bucket, for user saving extended attributes */
-	__u16		       hs_extra_bytes;
+	__u16				hs_extra_bytes;
 	/** wants to iterate */
-	__u8			hs_iterating;
+	__u8				hs_iterating;
 	/** hash-table is dying */
-	__u8			hs_exiting;
+	__u8				hs_exiting;
 	/** current hash bits */
-	__u8			hs_cur_bits;
+	__u8				hs_cur_bits;
 	/** min hash bits */
-	__u8			hs_min_bits;
+	__u8				hs_min_bits;
 	/** max hash bits */
-	__u8			hs_max_bits;
+	__u8				hs_max_bits;
 	/** bits for rehash */
-	__u8			hs_rehash_bits;
+	__u8				hs_rehash_bits;
 	/** bits for each bucket */
-	__u8			hs_bkt_bits;
+	__u8				hs_bkt_bits;
 	/** resize min threshold */
-	__u16		       hs_min_theta;
+	__u16				hs_min_theta;
 	/** resize max threshold */
-	__u16		       hs_max_theta;
+	__u16				hs_max_theta;
 	/** resize count */
-	__u32		       hs_rehash_count;
+	__u32				hs_rehash_count;
 	/** # of iterators (caller of cfs_hash_for_each_*) */
-	__u32		       hs_iterators;
+	__u32				hs_iterators;
 	/** rehash workitem */
-	cfs_workitem_t	      hs_rehash_wi;
+	cfs_workitem_t			hs_rehash_wi;
 	/** refcount on this hash table */
-	atomic_t		hs_refcount;
+	atomic_t			hs_refcount;
 	/** rehash buckets-table */
-	struct cfs_hash_bucket	 **hs_rehash_buckets;
+	struct cfs_hash_bucket		**hs_rehash_buckets;
 #if CFS_HASH_DEBUG_LEVEL >= CFS_HASH_DEBUG_1
 	/** serialize debug members */
 	spinlock_t			hs_dep_lock;
 	/** max depth */
-	unsigned int		hs_dep_max;
+	unsigned int			hs_dep_max;
 	/** id of the deepest bucket */
-	unsigned int		hs_dep_bkt;
+	unsigned int			hs_dep_bkt;
 	/** offset in the deepest bucket */
-	unsigned int		hs_dep_off;
+	unsigned int			hs_dep_off;
 	/** bits when we found the max depth */
-	unsigned int		hs_dep_bits;
+	unsigned int			hs_dep_bits;
 	/** workitem to output max depth */
-	cfs_workitem_t	      hs_dep_wi;
+	cfs_workitem_t			hs_dep_wi;
 #endif
 	/** name of htable */
-	char			hs_name[0];
+	char				hs_name[0];
 };
 
 struct cfs_hash_lock_ops {
@@ -324,11 +320,11 @@ struct cfs_hash_ops {
 };
 
 /** total number of buckets in @hs */
-#define CFS_HASH_NBKT(hs)       \
+#define CFS_HASH_NBKT(hs)	\
 	(1U << ((hs)->hs_cur_bits - (hs)->hs_bkt_bits))
 
 /** total number of buckets in @hs while rehashing */
-#define CFS_HASH_RH_NBKT(hs)    \
+#define CFS_HASH_RH_NBKT(hs)	\
 	(1U << ((hs)->hs_rehash_bits - (hs)->hs_bkt_bits))
 
 /** number of hlist for in bucket */
@@ -433,19 +429,22 @@ cfs_hash_with_nblk_change(struct cfs_hash *hs)
 
 static inline int
 cfs_hash_is_exiting(struct cfs_hash *hs)
-{       /* cfs_hash_destroy is called */
+{
+	/* cfs_hash_destroy is called */
 	return hs->hs_exiting;
 }
 
 static inline int
 cfs_hash_is_rehashing(struct cfs_hash *hs)
-{       /* rehash is launched */
+{
+	/* rehash is launched */
 	return hs->hs_rehash_bits != 0;
 }
 
 static inline int
 cfs_hash_is_iterating(struct cfs_hash *hs)
-{       /* someone is calling cfs_hash_for_each_* */
+{
+	/* someone is calling cfs_hash_for_each_* */
 	return hs->hs_iterating || hs->hs_iterators != 0;
 }
 
@@ -641,13 +640,6 @@ cfs_hash_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 struct hlist_node *
 cfs_hash_bd_peek_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 			const void *key);
-struct hlist_node *
-cfs_hash_bd_findadd_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   const void *key, struct hlist_node *hnode,
-			   int insist_add);
-struct hlist_node *
-cfs_hash_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   const void *key, struct hlist_node *hnode);
 
 /**
  * operations on cfs_hash bucket (bd: bucket descriptor),
@@ -758,7 +750,7 @@ static inline void
 cfs_hash_bucket_validate(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 			 struct hlist_node *hnode)
 {
-	struct cfs_hash_bd   bds[2];
+	struct cfs_hash_bd bds[2];
 
 	cfs_hash_dual_bd_get(hs, cfs_hash_key(hs, hnode), bds);
 	LASSERT(bds[0].bd_bucket == bd->bd_bucket ||
@@ -777,9 +769,9 @@ cfs_hash_bucket_validate(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 
 #endif /* CFS_HASH_DEBUG_LEVEL */
 
-#define CFS_HASH_THETA_BITS  10
-#define CFS_HASH_MIN_THETA  (1U << (CFS_HASH_THETA_BITS - 1))
-#define CFS_HASH_MAX_THETA  (1U << (CFS_HASH_THETA_BITS + 1))
+#define CFS_HASH_THETA_BITS	10
+#define CFS_HASH_MIN_THETA	(1U << (CFS_HASH_THETA_BITS - 1))
+#define CFS_HASH_MAX_THETA	(1U << (CFS_HASH_THETA_BITS + 1))
 
 /* Return integer component of theta */
 static inline int __cfs_hash_theta_int(int theta)
@@ -848,20 +840,20 @@ cfs_hash_u64_hash(const __u64 key, unsigned mask)
 }
 
 /** iterate over all buckets in @bds (array of struct cfs_hash_bd) */
-#define cfs_hash_for_each_bd(bds, n, i) \
+#define cfs_hash_for_each_bd(bds, n, i)	\
 	for (i = 0; i < n && (bds)[i].bd_bucket != NULL; i++)
 
 /** iterate over all buckets of @hs */
-#define cfs_hash_for_each_bucket(hs, bd, pos)		   \
-	for (pos = 0;					   \
-	     pos < CFS_HASH_NBKT(hs) &&			 \
+#define cfs_hash_for_each_bucket(hs, bd, pos)			\
+	for (pos = 0;						\
+	     pos < CFS_HASH_NBKT(hs) &&				\
 	     ((bd)->bd_bucket = (hs)->hs_buckets[pos]) != NULL; pos++)
 
 /** iterate over all hlist of bucket @bd */
-#define cfs_hash_bd_for_each_hlist(hs, bd, hlist)	       \
-	for ((bd)->bd_offset = 0;			       \
-	     (bd)->bd_offset < CFS_HASH_BKT_NHLIST(hs) &&       \
-	     (hlist = cfs_hash_bd_hhead(hs, bd)) != NULL;       \
+#define cfs_hash_bd_for_each_hlist(hs, bd, hlist)		\
+	for ((bd)->bd_offset = 0;				\
+	     (bd)->bd_offset < CFS_HASH_BKT_NHLIST(hs) &&	\
+	     (hlist = cfs_hash_bd_hhead(hs, bd)) != NULL;	\
 	     (bd)->bd_offset++)
 
 /* !__LIBCFS__HASH_H__ */
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
index a989d2666230..41f3d810aea4 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_kernelcomm.h
@@ -91,7 +91,7 @@ typedef int (*libcfs_kkuc_cb_t)(__u32 data, void *cb_arg);
 /* Kernel methods */
 int libcfs_kkuc_msg_put(struct file *fp, void *payload);
 int libcfs_kkuc_group_put(int group, void *payload);
-int libcfs_kkuc_group_add(struct file *fp, int uid, int group,
+int libcfs_kkuc_group_add(struct file *fp, int uid, unsigned int group,
 				 __u32 data);
 int libcfs_kkuc_group_rem(int uid, int group);
 int libcfs_kkuc_group_foreach(int group, libcfs_kkuc_cb_t cb_func,
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index f0b0423a716b..a80d993b882e 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -151,16 +151,12 @@ do {									    \
 
 #define LIBCFS_FREE(ptr, size)					  \
 do {								    \
-	int s = (size);						 \
 	if (unlikely((ptr) == NULL)) {				  \
 		CERROR("LIBCFS: free NULL '" #ptr "' (%d bytes) at "    \
-		       "%s:%d\n", s, __FILE__, __LINE__);	       \
+		       "%s:%d\n", (int)(size), __FILE__, __LINE__);	\
 		break;						  \
 	}							       \
-	if (unlikely(s > LIBCFS_VMALLOC_SIZE))			  \
-		vfree(ptr);				    \
-	else							    \
-		kfree(ptr);					  \
+	kvfree(ptr);					  \
 } while (0)
 
 /******************************************************************************/
@@ -185,8 +181,6 @@ int libcfs_debug_cleanup(void);
 int libcfs_debug_clear_buffer(void);
 int libcfs_debug_mark_buffer(const char *text);
 
-void libcfs_debug_set_level(unsigned int debug_level);
-
 /*
  * allocate per-cpu-partition data, returned value is an array of pointers,
  * variable can be indexed by CPU ID.
diff --git a/drivers/staging/lustre/include/linux/lnet/api.h b/drivers/staging/lustre/include/linux/lnet/api.h
index 9493d5e236c5..75285fde15e8 100644
--- a/drivers/staging/lustre/include/linux/lnet/api.h
+++ b/drivers/staging/lustre/include/linux/lnet/api.h
@@ -161,12 +161,6 @@ int LNetEQAlloc(unsigned int       count_in,
 
 int LNetEQFree(lnet_handle_eq_t eventq_in);
 
-int LNetEQGet(lnet_handle_eq_t  eventq_in,
-	      lnet_event_t     *event_out);
-
-int LNetEQWait(lnet_handle_eq_t  eventq_in,
-	       lnet_event_t     *event_out);
-
 int LNetEQPoll(lnet_handle_eq_t *eventqs_in,
 	       int		 neq_in,
 	       int		 timeout_ms,
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index b61d5045a566..b67a6607bb3b 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -23,7 +23,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012 - 2015, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-types.h b/drivers/staging/lustre/include/linux/lnet/lib-types.h
index d792c4adb0ca..3bb9468e0b9d 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-types.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-types.h
@@ -23,7 +23,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012 - 2015, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/include/linux/lnet/nidstr.h b/drivers/staging/lustre/include/linux/lnet/nidstr.h
index 46ad9147ad2a..4fc9ddce829d 100644
--- a/drivers/staging/lustre/include/linux/lnet/nidstr.h
+++ b/drivers/staging/lustre/include/linux/lnet/nidstr.h
@@ -23,7 +23,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011 - 2015, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 #ifndef _LNET_NIDSTRINGS_H
 #define _LNET_NIDSTRINGS_H
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index 7c730e3f7453..cb74ae731b95 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -2070,32 +2070,13 @@ static int kiblnd_net_init_pools(kib_net_t *net, __u32 *cpts, int ncpts)
 
 static int kiblnd_hdev_get_attr(kib_hca_dev_t *hdev)
 {
-	struct ib_device_attr *attr;
-	int rc;
-
 	/* It's safe to assume a HCA can handle a page size
 	 * matching that of the native system */
 	hdev->ibh_page_shift = PAGE_SHIFT;
 	hdev->ibh_page_size  = 1 << PAGE_SHIFT;
 	hdev->ibh_page_mask  = ~((__u64)hdev->ibh_page_size - 1);
 
-	LIBCFS_ALLOC(attr, sizeof(*attr));
-	if (attr == NULL) {
-		CERROR("Out of memory\n");
-		return -ENOMEM;
-	}
-
-	rc = ib_query_device(hdev->ibh_ibdev, attr);
-	if (rc == 0)
-		hdev->ibh_mr_size = attr->max_mr_size;
-
-	LIBCFS_FREE(attr, sizeof(*attr));
-
-	if (rc != 0) {
-		CERROR("Failed to query IB device: %d\n", rc);
-		return rc;
-	}
-
+	hdev->ibh_mr_size = hdev->ibh_ibdev->attrs.max_mr_size;
 	if (hdev->ibh_mr_size == ~0ULL) {
 		hdev->ibh_mr_shift = 64;
 		return 0;
@@ -2865,7 +2846,7 @@ static int __init kiblnd_module_init(void)
 	return 0;
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Kernel OpenIB gen2 LND v2.00");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
index 263db37de7c8..025faa9f86b3 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 260750354a41..c7b9ccb13f1c 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
index ecfe73302350..05aa90ea597a 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -2621,8 +2621,8 @@ ksocknal_enumerate_interfaces(ksock_net_t *net)
 
 		net->ksnn_interfaces[j].ksni_ipaddr = ip;
 		net->ksnn_interfaces[j].ksni_netmask = mask;
-		strncpy(&net->ksnn_interfaces[j].ksni_name[0],
-			names[i], IFNAMSIZ);
+		strlcpy(net->ksnn_interfaces[j].ksni_name,
+			names[i], sizeof(net->ksnn_interfaces[j].ksni_name));
 		j++;
 	}
 
@@ -2805,8 +2805,9 @@ ksocknal_startup(lnet_ni_t *ni)
 				goto fail_1;
 			}
 
-			strncpy(&net->ksnn_interfaces[i].ksni_name[0],
-				ni->ni_interfaces[i], IFNAMSIZ);
+			strlcpy(net->ksnn_interfaces[i].ksni_name,
+				ni->ni_interfaces[i],
+				sizeof(net->ksnn_interfaces[i].ksni_name));
 		}
 		net->ksnn_ninterfaces = i;
 	}
@@ -2868,7 +2869,7 @@ ksocknal_module_init(void)
 	return 0;
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Kernel TCP Socket LND v3.0.0");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("3.0.0");
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index b349847f9cf9..f4fa72550657 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -679,6 +679,9 @@ int ksocknal_lib_recv_kiov(ksock_conn_t *conn);
 int ksocknal_lib_get_conn_tunables(ksock_conn_t *conn, int *txmem,
 				   int *rxmem, int *nagle);
 
+void ksocknal_read_callback(ksock_conn_t *conn);
+void ksocknal_write_callback(ksock_conn_t *conn);
+
 int ksocknal_tunables_init(void);
 
 void ksocknal_lib_csum_tx(ksock_tx_t *tx);
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
index 679785b0209c..cf8e43bd3c03 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib.c
@@ -69,7 +69,7 @@ ksocknal_lib_zc_capable(ksock_conn_t *conn)
 
 	/* ZC if the socket supports scatter/gather and doesn't need software
 	 * checksums */
-	return ((caps & NETIF_F_SG) != 0 && (caps & NETIF_F_ALL_CSUM) != 0);
+	return ((caps & NETIF_F_SG) != 0 && (caps & NETIF_F_CSUM_MASK) != 0);
 }
 
 int
@@ -580,8 +580,6 @@ ksocknal_lib_push_conn(ksock_conn_t *conn)
 	ksocknal_connsock_decref(conn);
 }
 
-extern void ksocknal_read_callback(ksock_conn_t *conn);
-extern void ksocknal_write_callback(ksock_conn_t *conn);
 /*
  * socket call back in Linux
  */
diff --git a/drivers/staging/lustre/lnet/lnet/acceptor.c b/drivers/staging/lustre/lnet/lnet/acceptor.c
index 92ca1dd64076..fed57d90028d 100644
--- a/drivers/staging/lustre/lnet/lnet/acceptor.c
+++ b/drivers/staging/lustre/lnet/lnet/acceptor.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index 395412639935..362282fa00bf 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -354,16 +354,6 @@ lnet_counters_reset(void)
 
 	lnet_net_unlock(LNET_LOCK_EX);
 }
-EXPORT_SYMBOL(lnet_counters_reset);
-
-static __u64
-lnet_create_interface_cookie(void)
-{
-	/* NB the interface cookie in wire handles guards against delayed
-	 * replies and ACKs appearing valid after reboot.
-	 */
-	return ktime_get_ns();
-}
 
 static char *
 lnet_res_type2str(int type)
@@ -553,8 +543,11 @@ lnet_prepare(lnet_pid_t requested_pid)
 	rc = lnet_create_remote_nets_table();
 	if (rc != 0)
 		goto failed;
-
-	the_lnet.ln_interface_cookie = lnet_create_interface_cookie();
+	/*
+	 * NB the interface cookie in wire handles guards against delayed
+	 * replies and ACKs appearing valid after reboot.
+	 */
+	the_lnet.ln_interface_cookie = ktime_get_ns();
 
 	the_lnet.ln_counters = cfs_percpt_alloc(lnet_cpt_table(),
 						sizeof(lnet_counters_t));
@@ -1159,7 +1152,6 @@ lnet_init(void)
 	lnet_register_lnd(&the_lolnd);
 	return 0;
 }
-EXPORT_SYMBOL(lnet_init);
 
 /**
  * Finalize LNet library.
@@ -1183,7 +1175,6 @@ lnet_fini(void)
 
 	the_lnet.ln_init = 0;
 }
-EXPORT_SYMBOL(lnet_fini);
 
 /**
  * Set LNet PID and start LNet interfaces, routing, and forwarding.
diff --git a/drivers/staging/lustre/lnet/lnet/config.c b/drivers/staging/lustre/lnet/lnet/config.c
index 1b3bc8386524..284a3c271bc6 100644
--- a/drivers/staging/lustre/lnet/lnet/config.c
+++ b/drivers/staging/lustre/lnet/lnet/config.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -650,8 +650,8 @@ lnet_parse_route(char *str, int *im_a_router)
 	INIT_LIST_HEAD(&nets);
 
 	/* save a copy of the string for error messages */
-	strncpy(cmd, str, sizeof(cmd) - 1);
-	cmd[sizeof(cmd) - 1] = 0;
+	strncpy(cmd, str, sizeof(cmd));
+	cmd[sizeof(cmd) - 1] = '\0';
 
 	sep = str;
 	for (;;) {
@@ -972,11 +972,13 @@ lnet_splitnets(char *source, struct list_head *nets)
 			return 0;
 
 		offset += (int)(sep - tb->ltb_text);
-		tb2 = lnet_new_text_buf(strlen(sep));
+		len = strlen(sep);
+		tb2 = lnet_new_text_buf(len);
 		if (tb2 == NULL)
 			return -ENOMEM;
 
-		strcpy(tb2->ltb_text, sep);
+		strncpy(tb2->ltb_text, sep, len);
+		tb2->ltb_text[len] = '\0';
 		list_add_tail(&tb2->ltb_list, nets);
 
 		tb = tb2;
@@ -1021,8 +1023,8 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 		tb = list_entry(raw_entries.next, struct lnet_text_buf_t,
 				    ltb_list);
 
-		strncpy(source, tb->ltb_text, sizeof(source)-1);
-		source[sizeof(source)-1] = 0;
+		strncpy(source, tb->ltb_text, sizeof(source));
+		source[sizeof(source)-1] = '\0';
 
 		/* replace ltb_text with the network(s) add on match */
 		rc = lnet_match_network_tokens(tb->ltb_text, ipaddrs, nip);
@@ -1103,12 +1105,6 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 	return count;
 }
 
-static void
-lnet_ipaddr_free_enumeration(__u32 *ipaddrs, int nip)
-{
-	LIBCFS_FREE(ipaddrs, nip * sizeof(*ipaddrs));
-}
-
 static int
 lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 {
@@ -1169,7 +1165,7 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 				rc = nip;
 			}
 		}
-		lnet_ipaddr_free_enumeration(ipaddrs, nif);
+		LIBCFS_FREE(ipaddrs, nip * sizeof(*ipaddrs));
 	}
 	return nip;
 }
@@ -1195,7 +1191,7 @@ lnet_parse_ip2nets(char **networksp, char *ip2nets)
 	}
 
 	rc = lnet_match_networks(networksp, ip2nets, ipaddrs, nip);
-	lnet_ipaddr_free_enumeration(ipaddrs, nip);
+	LIBCFS_FREE(ipaddrs, nip * sizeof(*ipaddrs));
 
 	if (rc < 0) {
 		LCONSOLE_ERROR_MSG(0x119, "Error %d parsing ip2nets\n", rc);
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index 60889ebd2f2b..64f94a690081 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -282,15 +282,6 @@ lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
  * at least one event between this event and the last event obtained from the
  * EQ has been dropped due to limited space in the EQ.
  */
-int
-LNetEQGet(lnet_handle_eq_t eventq, lnet_event_t *event)
-{
-	int which;
-
-	return LNetEQPoll(&eventq, 1, 0,
-			 event, &which);
-}
-EXPORT_SYMBOL(LNetEQGet);
 
 /**
  * Block the calling process until there is an event in the EQ.
@@ -308,15 +299,6 @@ EXPORT_SYMBOL(LNetEQGet);
  * at least one event between this event and the last event obtained from the
  * EQ has been dropped due to limited space in the EQ.
  */
-int
-LNetEQWait(lnet_handle_eq_t eventq, lnet_event_t *event)
-{
-	int which;
-
-	return LNetEQPoll(&eventq, 1, LNET_TIME_FOREVER,
-			 event, &which);
-}
-EXPORT_SYMBOL(LNetEQWait);
 
 static int
 lnet_eq_wait_locked(int *timeout_ms)
diff --git a/drivers/staging/lustre/lnet/lnet/lib-move.c b/drivers/staging/lustre/lnet/lnet/lib-move.c
index 5631f60a39bc..fb8f7be043ec 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-move.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-move.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -1645,7 +1645,6 @@ lnet_msgtyp2str(int type)
 		return "<UNKNOWN>";
 	}
 }
-EXPORT_SYMBOL(lnet_msgtyp2str);
 
 void
 lnet_print_hdr(lnet_hdr_t *hdr)
diff --git a/drivers/staging/lustre/lnet/lnet/lib-ptl.c b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
index b4f573ab62cc..bd7b071b2873 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-ptl.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
@@ -21,7 +21,7 @@
  * GPL HEADER END
  */
 /*
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lnet/lnet/lib-socket.c b/drivers/staging/lustre/lnet/lnet/lib-socket.c
index 6f7ef4c737cd..589ecc84d1b8 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-socket.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-socket.c
@@ -23,7 +23,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, 2015 Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lnet/lnet/module.c b/drivers/staging/lustre/lnet/lnet/module.c
index 576201a8390c..c93c00752a4c 100644
--- a/drivers/staging/lustre/lnet/lnet/module.c
+++ b/drivers/staging/lustre/lnet/lnet/module.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -146,7 +146,7 @@ fini_lnet(void)
 	lnet_fini();
 }
 
-MODULE_AUTHOR("Peter J. Braam <braam@clusterfs.com>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("LNet v3.1");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("1.0.0");
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index 4ea651c6db3a..f5faa414d250 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  *
  *   This file is part of Portals
  *   http://sourceforge.net/projects/sandiaportals/
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index 0605c651f797..1f04cc1fc31c 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -358,7 +358,7 @@ out:
 }
 
 static void
-brw_server_rpc_done(srpc_server_rpc_t *rpc)
+brw_server_rpc_done(struct srpc_server_rpc *rpc)
 {
 	srpc_bulk_t *blk = rpc->srpc_bulk;
 
@@ -378,7 +378,7 @@ brw_server_rpc_done(srpc_server_rpc_t *rpc)
 }
 
 static int
-brw_bulk_ready(srpc_server_rpc_t *rpc, int status)
+brw_bulk_ready(struct srpc_server_rpc *rpc, int status)
 {
 	__u64 magic = BRW_MAGIC;
 	srpc_brw_reply_t *reply = &rpc->srpc_replymsg.msg_body.brw_reply;
diff --git a/drivers/staging/lustre/lnet/selftest/conctl.c b/drivers/staging/lustre/lnet/selftest/conctl.c
index 556c837cf62c..a534665403e5 100644
--- a/drivers/staging/lustre/lnet/selftest/conctl.c
+++ b/drivers/staging/lustre/lnet/selftest/conctl.c
@@ -925,5 +925,3 @@ out:
 
 	return rc;
 }
-
-EXPORT_SYMBOL(lstcon_ioctl_entry);
diff --git a/drivers/staging/lustre/lnet/selftest/conrpc.c b/drivers/staging/lustre/lnet/selftest/conrpc.c
index 64a0335934f3..1066c70434b1 100644
--- a/drivers/staging/lustre/lnet/selftest/conrpc.c
+++ b/drivers/staging/lustre/lnet/selftest/conrpc.c
@@ -612,8 +612,8 @@ lstcon_sesrpc_prep(lstcon_node_t *nd, int transop,
 		msrq = &(*crpc)->crp_rpc->crpc_reqstmsg.msg_body.mksn_reqst;
 		msrq->mksn_sid     = console_session.ses_id;
 		msrq->mksn_force   = console_session.ses_force;
-		strncpy(msrq->mksn_name, console_session.ses_name,
-			strlen(console_session.ses_name));
+		strlcpy(msrq->mksn_name, console_session.ses_name,
+			sizeof(msrq->mksn_name));
 		break;
 
 	case LST_TRANS_SESEND:
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index d315dd44ae3b..5619fc430e8d 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -277,12 +277,6 @@ lstcon_group_find(const char *name, lstcon_group_t **grpp)
 	return -ENOENT;
 }
 
-static void
-lstcon_group_put(lstcon_group_t *grp)
-{
-	lstcon_group_decref(grp);
-}
-
 static int
 lstcon_group_ndlink_find(lstcon_group_t *grp, lnet_process_id_t id,
 			 lstcon_ndlink_t **ndlpp, int create)
@@ -324,8 +318,6 @@ lstcon_group_ndlink_move(lstcon_group_t *old,
 	list_add_tail(&ndl->ndl_hlink, &new->grp_ndl_hash[idx]);
 	list_add_tail(&ndl->ndl_link, &new->grp_ndl_list);
 	new->grp_nnode++;
-
-	return;
 }
 
 static void
@@ -436,7 +428,7 @@ lstcon_group_nodes_add(lstcon_group_t *grp,
 	}
 
 	if (rc != 0) {
-		lstcon_group_put(tmp);
+		lstcon_group_decref(tmp);
 		return rc;
 	}
 
@@ -445,7 +437,7 @@ lstcon_group_nodes_add(lstcon_group_t *grp,
 				     tmp, lstcon_sesrpc_condition, &trans);
 	if (rc != 0) {
 		CERROR("Can't create transaction: %d\n", rc);
-		lstcon_group_put(tmp);
+		lstcon_group_decref(tmp);
 		return rc;
 	}
 
@@ -460,7 +452,7 @@ lstcon_group_nodes_add(lstcon_group_t *grp,
 	lstcon_rpc_trans_destroy(trans);
 
 	lstcon_group_move(tmp, grp);
-	lstcon_group_put(tmp);
+	lstcon_group_decref(tmp);
 
 	return rc;
 }
@@ -510,12 +502,12 @@ lstcon_group_nodes_remove(lstcon_group_t *grp,
 
 	lstcon_rpc_trans_destroy(trans);
 	/* release nodes anyway, because we can't rollback status */
-	lstcon_group_put(tmp);
+	lstcon_group_decref(tmp);
 
 	return rc;
 error:
 	lstcon_group_move(tmp, grp);
-	lstcon_group_put(tmp);
+	lstcon_group_decref(tmp);
 
 	return rc;
 }
@@ -529,7 +521,7 @@ lstcon_group_add(char *name)
 	rc = (lstcon_group_find(name, &grp) == 0) ? -EEXIST : 0;
 	if (rc != 0) {
 		/* find a group with same name */
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return rc;
 	}
 
@@ -563,14 +555,14 @@ lstcon_nodes_add(char *name, int count, lnet_process_id_t *ids_up,
 	if (grp->grp_ref > 2) {
 		/* referred by other threads or test */
 		CDEBUG(D_NET, "Group %s is busy\n", name);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 		return -EBUSY;
 	}
 
 	rc = lstcon_group_nodes_add(grp, count, ids_up, featp, result_up);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -591,7 +583,7 @@ lstcon_group_del(char *name)
 	if (grp->grp_ref > 2) {
 		/* referred by others threads or test */
 		CDEBUG(D_NET, "Group %s is busy\n", name);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return -EBUSY;
 	}
 
@@ -600,7 +592,7 @@ lstcon_group_del(char *name)
 				     grp, lstcon_sesrpc_condition, &trans);
 	if (rc != 0) {
 		CERROR("Can't create transaction: %d\n", rc);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return rc;
 	}
 
@@ -608,10 +600,10 @@ lstcon_group_del(char *name)
 
 	lstcon_rpc_trans_destroy(trans);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 	/* -ref for session, it's destroyed,
 	 * status can't be rolled back, destroy group anyway */
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -631,7 +623,7 @@ lstcon_group_clean(char *name, int args)
 	if (grp->grp_ref > 2) {
 		/* referred by test */
 		CDEBUG(D_NET, "Group %s is busy\n", name);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return -EBUSY;
 	}
 
@@ -640,10 +632,10 @@ lstcon_group_clean(char *name, int args)
 
 	lstcon_group_drain(grp, args);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 	/* release empty group */
 	if (list_empty(&grp->grp_ndl_list))
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 	return 0;
 }
@@ -664,16 +656,16 @@ lstcon_nodes_remove(char *name, int count,
 	if (grp->grp_ref > 2) {
 		/* referred by test */
 		CDEBUG(D_NET, "Group %s is busy\n", name);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return -EBUSY;
 	}
 
 	rc = lstcon_group_nodes_remove(grp, count, ids_up, result_up);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 	/* release empty group */
 	if (list_empty(&grp->grp_ndl_list))
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -694,7 +686,7 @@ lstcon_group_refresh(char *name, struct list_head *result_up)
 	if (grp->grp_ref > 2) {
 		/* referred by test */
 		CDEBUG(D_NET, "Group %s is busy\n", name);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return -EBUSY;
 	}
 
@@ -705,7 +697,7 @@ lstcon_group_refresh(char *name, struct list_head *result_up)
 	if (rc != 0) {
 		/* local error, return */
 		CDEBUG(D_NET, "Can't create transaction: %d\n", rc);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return rc;
 	}
 
@@ -715,7 +707,7 @@ lstcon_group_refresh(char *name, struct list_head *result_up)
 
 	lstcon_rpc_trans_destroy(trans);
 	/* -ref for me */
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -797,7 +789,7 @@ lstcon_group_info(char *name, lstcon_ndlist_ent_t *gents_p,
 		/* verbose query */
 		rc = lstcon_nodes_getent(&grp->grp_ndl_list,
 					 index_p, count_p, dents_up);
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 		return rc;
 	}
@@ -806,7 +798,7 @@ lstcon_group_info(char *name, lstcon_ndlist_ent_t *gents_p,
 	LIBCFS_ALLOC(gentp, sizeof(lstcon_ndlist_ent_t));
 	if (gentp == NULL) {
 		CERROR("Can't allocate ndlist_ent\n");
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 		return -ENOMEM;
 	}
@@ -819,7 +811,7 @@ lstcon_group_info(char *name, lstcon_ndlist_ent_t *gents_p,
 
 	LIBCFS_FREE(gentp, sizeof(lstcon_ndlist_ent_t));
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return 0;
 }
@@ -1096,8 +1088,8 @@ lstcon_batch_destroy(lstcon_batch_t *bat)
 
 		list_del(&test->tes_link);
 
-		lstcon_group_put(test->tes_src_grp);
-		lstcon_group_put(test->tes_dst_grp);
+		lstcon_group_decref(test->tes_src_grp);
+		lstcon_group_decref(test->tes_dst_grp);
 
 		LIBCFS_FREE(test, offsetof(lstcon_test_t,
 					   tes_param[test->tes_paramlen]));
@@ -1352,10 +1344,10 @@ out:
 		LIBCFS_FREE(test, offsetof(lstcon_test_t, tes_param[paramlen]));
 
 	if (dst_grp != NULL)
-		lstcon_group_put(dst_grp);
+		lstcon_group_decref(dst_grp);
 
 	if (src_grp != NULL)
-		lstcon_group_put(src_grp);
+		lstcon_group_decref(src_grp);
 
 	return rc;
 }
@@ -1518,7 +1510,7 @@ lstcon_group_stat(char *grp_name, int timeout, struct list_head *result_up)
 
 	rc = lstcon_ndlist_stat(&grp->grp_ndl_list, timeout, result_up);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -1556,13 +1548,13 @@ lstcon_nodes_stat(int count, lnet_process_id_t *ids_up,
 	}
 
 	if (rc != 0) {
-		lstcon_group_put(tmp);
+		lstcon_group_decref(tmp);
 		return rc;
 	}
 
 	rc = lstcon_ndlist_stat(&tmp->grp_ndl_list, timeout, result_up);
 
-	lstcon_group_put(tmp);
+	lstcon_group_decref(tmp);
 
 	return rc;
 }
@@ -1629,7 +1621,7 @@ lstcon_group_debug(int timeout, char *name,
 
 	rc = lstcon_debug_ndlist(&grp->grp_ndl_list, NULL,
 				 timeout, result_up);
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -1666,14 +1658,14 @@ lstcon_nodes_debug(int timeout,
 	}
 
 	if (rc != 0) {
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 		return rc;
 	}
 
 	rc = lstcon_debug_ndlist(&grp->grp_ndl_list, NULL,
 				 timeout, result_up);
 
-	lstcon_group_put(grp);
+	lstcon_group_decref(grp);
 
 	return rc;
 }
@@ -1739,7 +1731,8 @@ lstcon_session_new(char *name, int key, unsigned feats,
 	console_session.ses_feats_updated = 0;
 	console_session.ses_timeout = (timeout <= 0) ?
 				      LST_CONSOLE_TIMEOUT : timeout;
-	strcpy(console_session.ses_name, name);
+	strlcpy(console_session.ses_name, name,
+		sizeof(console_session.ses_name));
 
 	rc = lstcon_batch_add(LST_DEFAULT_BATCH);
 	if (rc != 0)
@@ -1847,7 +1840,7 @@ lstcon_session_end(void)
 				     lstcon_group_t, grp_link);
 		LASSERT(grp->grp_ref == 1);
 
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 	}
 
 	/* all nodes should be released */
@@ -1891,7 +1884,7 @@ lstcon_session_feats_check(unsigned feats)
 }
 
 static int
-lstcon_acceptor_handle(srpc_server_rpc_t *rpc)
+lstcon_acceptor_handle(struct srpc_server_rpc *rpc)
 {
 	srpc_msg_t *rep  = &rpc->srpc_replymsg;
 	srpc_msg_t *req  = &rpc->srpc_reqstbuf->buf_msg;
@@ -1959,14 +1952,15 @@ lstcon_acceptor_handle(srpc_server_rpc_t *rpc)
 	if (grp->grp_userland == 0)
 		grp->grp_userland = 1;
 
-	strcpy(jrep->join_session, console_session.ses_name);
+	strlcpy(jrep->join_session, console_session.ses_name,
+		sizeof(jrep->join_session));
 	jrep->join_timeout = console_session.ses_timeout;
 	jrep->join_status  = 0;
 
 out:
 	rep->msg_ses_feats = console_session.ses_features;
 	if (grp != NULL)
-		lstcon_group_put(grp);
+		lstcon_group_decref(grp);
 
 	mutex_unlock(&console_session.ses_mutex);
 
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index f18e50036809..1a2da7430190 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -111,7 +111,7 @@ static struct smoketest_framework {
 	spinlock_t        fw_lock;            /* serialise */
 	sfw_session_t     *fw_session;        /* _the_ session */
 	int               fw_shuttingdown;    /* shutdown in progress */
-	srpc_server_rpc_t *fw_active_srpc;    /* running RPC */
+	struct srpc_server_rpc *fw_active_srpc;/* running RPC */
 } sfw_data;
 
 /* forward ref's */
@@ -722,7 +722,7 @@ sfw_unpack_addtest_req(srpc_msg_t *msg)
 }
 
 static int
-sfw_add_test_instance(sfw_batch_t *tsb, srpc_server_rpc_t *rpc)
+sfw_add_test_instance(sfw_batch_t *tsb, struct srpc_server_rpc *rpc)
 {
 	srpc_msg_t *msg = &rpc->srpc_reqstbuf->buf_msg;
 	srpc_test_reqst_t *req = &msg->msg_body.tes_reqst;
@@ -1091,7 +1091,7 @@ sfw_query_batch(sfw_batch_t *tsb, int testidx, srpc_batch_reply_t *reply)
 }
 
 void
-sfw_free_pages(srpc_server_rpc_t *rpc)
+sfw_free_pages(struct srpc_server_rpc *rpc)
 {
 	srpc_free_bulk(rpc->srpc_bulk);
 	rpc->srpc_bulk = NULL;
@@ -1112,7 +1112,7 @@ sfw_alloc_pages(struct srpc_server_rpc *rpc, int cpt, int npages, int len,
 }
 
 static int
-sfw_add_test(srpc_server_rpc_t *rpc)
+sfw_add_test(struct srpc_server_rpc *rpc)
 {
 	sfw_session_t *sn = sfw_data.fw_session;
 	srpc_test_reply_t *reply = &rpc->srpc_replymsg.msg_body.tes_reply;
diff --git a/drivers/staging/lustre/lnet/selftest/rpc.c b/drivers/staging/lustre/lnet/selftest/rpc.c
index 7005002c15da..2acf6ec717be 100644
--- a/drivers/staging/lustre/lnet/selftest/rpc.c
+++ b/drivers/staging/lustre/lnet/selftest/rpc.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -445,15 +445,6 @@ srpc_post_active_rdma(int portal, __u64 matchbits, void *buf, int len,
 }
 
 static int
-srpc_post_active_rqtbuf(lnet_process_id_t peer, int service, void *buf,
-			int len, lnet_handle_md_t *mdh, srpc_event_t *ev)
-{
-	return srpc_post_active_rdma(srpc_serv_portal(service), service,
-				     buf, len, LNET_MD_OP_PUT, peer,
-				     LNET_NID_ANY, mdh, ev);
-}
-
-static int
 srpc_post_passive_rqtbuf(int service, int local, void *buf, int len,
 			 lnet_handle_md_t *mdh, srpc_event_t *ev)
 {
@@ -798,9 +789,11 @@ srpc_send_request(srpc_client_rpc_t *rpc)
 	ev->ev_data  = rpc;
 	ev->ev_type  = SRPC_REQUEST_SENT;
 
-	rc = srpc_post_active_rqtbuf(rpc->crpc_dest, rpc->crpc_service,
-				     &rpc->crpc_reqstmsg, sizeof(srpc_msg_t),
-				     &rpc->crpc_reqstmdh, ev);
+	 rc = srpc_post_active_rdma(srpc_serv_portal(rpc->crpc_service),
+				    rpc->crpc_service, &rpc->crpc_reqstmsg,
+				    sizeof(srpc_msg_t), LNET_MD_OP_PUT,
+				    rpc->crpc_dest, LNET_NID_ANY,
+				    &rpc->crpc_reqstmdh, ev);
 	if (rc != 0) {
 		LASSERT(rc == -ENOMEM);
 		ev->ev_fired = 1;  /* no more event expected */
@@ -866,7 +859,7 @@ srpc_prepare_bulk(srpc_client_rpc_t *rpc)
 }
 
 static int
-srpc_do_bulk(srpc_server_rpc_t *rpc)
+srpc_do_bulk(struct srpc_server_rpc *rpc)
 {
 	srpc_event_t *ev = &rpc->srpc_ev;
 	srpc_bulk_t *bk = rpc->srpc_bulk;
@@ -894,7 +887,7 @@ srpc_do_bulk(srpc_server_rpc_t *rpc)
 
 /* only called from srpc_handle_rpc */
 static void
-srpc_server_rpc_done(srpc_server_rpc_t *rpc, int status)
+srpc_server_rpc_done(struct srpc_server_rpc *rpc, int status)
 {
 	struct srpc_service_cd *scd = rpc->srpc_scd;
 	struct srpc_service *sv  = scd->scd_svc;
@@ -1404,7 +1397,7 @@ srpc_lnet_ev_handler(lnet_event_t *ev)
 	struct srpc_service_cd *scd;
 	srpc_event_t *rpcev = ev->md.user_ptr;
 	srpc_client_rpc_t *crpc;
-	srpc_server_rpc_t *srpc;
+	struct srpc_server_rpc *srpc;
 	srpc_buffer_t *buffer;
 	srpc_service_t *sv;
 	srpc_msg_t *msg;
diff --git a/drivers/staging/lustre/lnet/selftest/selftest.h b/drivers/staging/lustre/lnet/selftest/selftest.h
index 8a77d3fdfa54..870498339538 100644
--- a/drivers/staging/lustre/lnet/selftest/selftest.h
+++ b/drivers/staging/lustre/lnet/selftest/selftest.h
@@ -182,7 +182,7 @@ typedef struct swi_workitem {
 } swi_workitem_t;
 
 /* server-side state of a RPC */
-typedef struct srpc_server_rpc {
+struct srpc_server_rpc {
 	/* chain on srpc_service::*_rpcq */
 	struct list_head       srpc_list;
 	struct srpc_service_cd *srpc_scd;
@@ -198,7 +198,7 @@ typedef struct srpc_server_rpc {
 	unsigned int           srpc_aborted; /* being given up */
 	int                    srpc_status;
 	void                   (*srpc_done)(struct srpc_server_rpc *);
-} srpc_server_rpc_t;
+};
 
 /* client-side state of a RPC */
 typedef struct srpc_client_rpc {
@@ -318,8 +318,8 @@ typedef struct srpc_service {
 	 * - sv_handler: process incoming RPC request
 	 * - sv_bulk_ready: notify bulk data
 	 */
-	int                     (*sv_handler) (srpc_server_rpc_t *);
-	int                     (*sv_bulk_ready) (srpc_server_rpc_t *, int);
+	int (*sv_handler)(struct srpc_server_rpc *);
+	int (*sv_bulk_ready)(struct srpc_server_rpc *, int);
 } srpc_service_t;
 
 typedef struct {
@@ -423,9 +423,9 @@ void sfw_abort_rpc(srpc_client_rpc_t *rpc);
 void sfw_post_rpc(srpc_client_rpc_t *rpc);
 void sfw_client_rpc_done(srpc_client_rpc_t *rpc);
 void sfw_unpack_message(srpc_msg_t *msg);
-void sfw_free_pages(srpc_server_rpc_t *rpc);
+void sfw_free_pages(struct srpc_server_rpc *rpc);
 void sfw_add_bulk_page(srpc_bulk_t *bk, struct page *pg, int i);
-int sfw_alloc_pages(srpc_server_rpc_t *rpc, int cpt, int npages, int len,
+int sfw_alloc_pages(struct srpc_server_rpc *rpc, int cpt, int npages, int len,
 		    int sink);
 int sfw_make_session (srpc_mksn_reqst_t *request, srpc_mksn_reply_t *reply);
 
@@ -440,7 +440,7 @@ void srpc_free_bulk(srpc_bulk_t *bk);
 srpc_bulk_t *srpc_alloc_bulk(int cpt, unsigned bulk_npg, unsigned bulk_len,
 			     int sink);
 int srpc_send_rpc(swi_workitem_t *wi);
-int srpc_send_reply(srpc_server_rpc_t *rpc);
+int srpc_send_reply(struct srpc_server_rpc *rpc);
 int srpc_add_service(srpc_service_t *sv);
 int srpc_remove_service(srpc_service_t *sv);
 void srpc_shutdown_service(srpc_service_t *sv);
@@ -585,7 +585,7 @@ swi_state2str (int state)
 do {									\
 	int __I = 2;							\
 	while (!(cond)) {						\
-		CDEBUG(IS_PO2(++__I) ? D_WARNING : D_NET,		\
+		CDEBUG(is_power_of_2(++__I) ? D_WARNING : D_NET,	\
 		       fmt, ## __VA_ARGS__);				\
 		spin_unlock(&(lock));					\
 									\
diff --git a/drivers/staging/lustre/lustre/fid/fid_internal.h b/drivers/staging/lustre/lustre/fid/fid_internal.h
index 84daee1154dc..b79a813977cf 100644
--- a/drivers/staging/lustre/lustre/fid/fid_internal.h
+++ b/drivers/staging/lustre/lustre/fid/fid_internal.h
@@ -44,8 +44,6 @@
 #include "../../include/linux/libcfs/libcfs.h"
 
 /* Functions used internally in module. */
-int seq_client_alloc_super(struct lu_client_seq *seq,
-			   const struct lu_env *env);
 
 extern struct lprocfs_vars seq_client_debugfs_list[];
 
diff --git a/drivers/staging/lustre/lustre/fid/fid_request.c b/drivers/staging/lustre/lustre/fid/fid_request.c
index 7c45e7479087..ff8f38dc10ce 100644
--- a/drivers/staging/lustre/lustre/fid/fid_request.c
+++ b/drivers/staging/lustre/lustre/fid/fid_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2013, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -142,27 +142,6 @@ out_req:
 	return rc;
 }
 
-/* Request sequence-controller node to allocate new super-sequence. */
-int seq_client_alloc_super(struct lu_client_seq *seq,
-			   const struct lu_env *env)
-{
-	int rc;
-
-	mutex_lock(&seq->lcs_mutex);
-
-	/* Check whether the connection to seq controller has been
-	 * setup (lcs_exp != NULL) */
-	if (!seq->lcs_exp) {
-		mutex_unlock(&seq->lcs_mutex);
-		return -EINPROGRESS;
-	}
-
-	rc = seq_client_rpc(seq, &seq->lcs_space,
-			    SEQ_ALLOC_SUPER, "super");
-	mutex_unlock(&seq->lcs_mutex);
-	return rc;
-}
-
 /* Request sequence-controller node to allocate new meta-sequence. */
 static int seq_client_alloc_meta(const struct lu_env *env,
 				 struct lu_client_seq *seq)
@@ -483,7 +462,7 @@ static void __exit fid_mod_exit(void)
 		ldebugfs_remove(&seq_debugfs_dir);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre FID Module");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("0.1.0");
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index ce90c1c54a63..39f2aa32e984 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/fld/fld_cache.c b/drivers/staging/lustre/lustre/fld/fld_cache.c
index 446917484637..d9459e58e2ce 100644
--- a/drivers/staging/lustre/lustre/fld/fld_cache.c
+++ b/drivers/staging/lustre/lustre/fld/fld_cache.c
@@ -121,8 +121,8 @@ void fld_cache_fini(struct fld_cache *cache)
 /**
  * delete given node from list.
  */
-void fld_cache_entry_delete(struct fld_cache *cache,
-			    struct fld_cache_entry *node)
+static void fld_cache_entry_delete(struct fld_cache *cache,
+				   struct fld_cache_entry *node)
 {
 	list_del(&node->fce_list);
 	list_del(&node->fce_lru);
@@ -227,7 +227,6 @@ static int fld_cache_shrink(struct fld_cache *cache)
 
 	while (cache->fci_cache_count + cache->fci_threshold >
 	       cache->fci_cache_size && curr != &cache->fci_lru) {
-
 		flde = list_entry(curr, struct fld_cache_entry, fce_lru);
 		curr = curr->prev;
 		fld_cache_entry_delete(cache, flde);
@@ -377,8 +376,8 @@ struct fld_cache_entry
  * This function handles all cases of merging and breaking up of
  * ranges.
  */
-int fld_cache_insert_nolock(struct fld_cache *cache,
-			    struct fld_cache_entry *f_new)
+static int fld_cache_insert_nolock(struct fld_cache *cache,
+				   struct fld_cache_entry *f_new)
 {
 	struct fld_cache_entry *f_curr;
 	struct fld_cache_entry *n;
@@ -444,36 +443,10 @@ int fld_cache_insert(struct fld_cache *cache,
 	return rc;
 }
 
-void fld_cache_delete_nolock(struct fld_cache *cache,
-		      const struct lu_seq_range *range)
-{
-	struct fld_cache_entry *flde;
-	struct fld_cache_entry *tmp;
-	struct list_head *head;
-
-	head = &cache->fci_entries_head;
-	list_for_each_entry_safe(flde, tmp, head, fce_list) {
-		/* add list if next is end of list */
-		if (range->lsr_start == flde->fce_range.lsr_start ||
-		   (range->lsr_end == flde->fce_range.lsr_end &&
-		    range->lsr_flags == flde->fce_range.lsr_flags)) {
-			fld_cache_entry_delete(cache, flde);
-			break;
-		}
-	}
-}
-
 /**
  * Delete FLD entry in FLD cache.
  *
  */
-void fld_cache_delete(struct fld_cache *cache,
-		      const struct lu_seq_range *range)
-{
-	write_lock(&cache->fci_lock);
-	fld_cache_delete_nolock(cache, range);
-	write_unlock(&cache->fci_lock);
-}
 
 struct fld_cache_entry
 *fld_cache_entry_lookup_nolock(struct fld_cache *cache,
diff --git a/drivers/staging/lustre/lustre/fld/fld_internal.h b/drivers/staging/lustre/lustre/fld/fld_internal.h
index fbb232de6c74..12eb1647b4bf 100644
--- a/drivers/staging/lustre/lustre/fld/fld_internal.h
+++ b/drivers/staging/lustre/lustre/fld/fld_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, 2013, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -156,20 +156,11 @@ int fld_cache_insert(struct fld_cache *cache,
 struct fld_cache_entry
 *fld_cache_entry_create(const struct lu_seq_range *range);
 
-int fld_cache_insert_nolock(struct fld_cache *cache,
-			    struct fld_cache_entry *f_new);
-void fld_cache_delete(struct fld_cache *cache,
-		      const struct lu_seq_range *range);
-void fld_cache_delete_nolock(struct fld_cache *cache,
-			     const struct lu_seq_range *range);
 int fld_cache_lookup(struct fld_cache *cache,
 		     const u64 seq, struct lu_seq_range *range);
 
 struct fld_cache_entry*
 fld_cache_entry_lookup(struct fld_cache *cache, struct lu_seq_range *range);
-void fld_cache_entry_delete(struct fld_cache *cache,
-			    struct fld_cache_entry *node);
-void fld_dump_cache_entries(struct fld_cache *cache);
 
 struct fld_cache_entry
 *fld_cache_entry_lookup_nolock(struct fld_cache *cache,
diff --git a/drivers/staging/lustre/lustre/fld/fld_request.c b/drivers/staging/lustre/lustre/fld/fld_request.c
index 3fd91bc77da5..d92c01b74865 100644
--- a/drivers/staging/lustre/lustre/fld/fld_request.c
+++ b/drivers/staging/lustre/lustre/fld/fld_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2013, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -501,7 +501,7 @@ static void __exit fld_mod_exit(void)
 		ldebugfs_remove(&fld_debugfs_dir);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre FLD");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lustre/fld/lproc_fld.c b/drivers/staging/lustre/lustre/fld/lproc_fld.c
index 603f56e6095b..41ceaa8198a7 100644
--- a/drivers/staging/lustre/lustre/fld/lproc_fld.c
+++ b/drivers/staging/lustre/lustre/fld/lproc_fld.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, 2013, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/cl_object.h b/drivers/staging/lustre/lustre/include/cl_object.h
index 73564f8e3884..bd7acc2a1219 100644
--- a/drivers/staging/lustre/lustre/include/cl_object.h
+++ b/drivers/staging/lustre/lustre/include/cl_object.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -3127,7 +3127,6 @@ void cl_page_list_disown (const struct lu_env *env,
 			  struct cl_io *io, struct cl_page_list *plist);
 
 void cl_2queue_init     (struct cl_2queue *queue);
-void cl_2queue_add      (struct cl_2queue *queue, struct cl_page *page);
 void cl_2queue_disown   (const struct lu_env *env,
 			 struct cl_io *io, struct cl_2queue *queue);
 void cl_2queue_discard  (const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 9e654b218ca3..0ac8e0edcc48 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -624,9 +624,6 @@ void lprocfs_stats_collect(struct lprocfs_stats *stats, int idx,
 int lprocfs_single_release(struct inode *, struct file *);
 int lprocfs_seq_release(struct inode *, struct file *);
 
-#define LPROCFS_CLIMP_EXIT(obd)		 \
-	up_read(&(obd)->u.cli.cl_sem)
-
 /* write the name##_seq_show function, call LPROC_SEQ_FOPS_RO for read-only
   proc entries; otherwise, you will define name##_seq_write function also for
   a read-write proc entry, and then call LPROC_SEQ_SEQ instead. Finally,
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index fa78689748a9..1d79341a495d 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre/ll_fiemap.h b/drivers/staging/lustre/lustre/include/lustre/ll_fiemap.h
index 06ce8c9ae9ad..09088f40ba88 100644
--- a/drivers/staging/lustre/lustre/include/lustre/ll_fiemap.h
+++ b/drivers/staging/lustre/lustre/include/lustre/ll_fiemap.h
@@ -26,6 +26,8 @@
 /*
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2014, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 0b721c65c2a3..b064b5821e3f 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index 80f8ec529424..2b4dd656d5f5 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_disk.h b/drivers/staging/lustre/lustre/include/lustre_disk.h
index 5e1ac129a681..7c6933ffc9c1 100644
--- a/drivers/staging/lustre/lustre/include/lustre_disk.h
+++ b/drivers/staging/lustre/lustre/include/lustre_disk.h
@@ -68,6 +68,7 @@
    everything as string options */
 
 #define LMD_MAGIC    0xbdacbd03
+#define LMD_PARAMS_MAXLEN	4096
 
 /* gleaned from the mount command - no persistent info here */
 struct lustre_mount_data {
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 0e75a15fe0d4..9b319f1df025 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -872,8 +872,6 @@ struct ldlm_resource {
 	 */
 	struct mutex		lr_lvb_mutex;
 	int			lr_lvb_len;
-	/** protected by lr_lock */
-	void			*lr_lvb_data;
 
 	/** When the resource was considered as contended. */
 	unsigned long		lr_contention_time;
diff --git a/drivers/staging/lustre/lustre/include/lustre_eacl.h b/drivers/staging/lustre/lustre/include/lustre_eacl.h
index fee4d2c75506..0b66593a9526 100644
--- a/drivers/staging/lustre/lustre/include/lustre_eacl.h
+++ b/drivers/staging/lustre/lustre/include/lustre_eacl.h
@@ -76,8 +76,6 @@ extern int
 lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, size_t size,
 			      posix_acl_xattr_header **out);
 extern void
-lustre_posix_acl_xattr_free(posix_acl_xattr_header *header, int size);
-extern void
 lustre_ext_acl_xattr_free(ext_acl_xattr_header *header);
 extern ext_acl_xattr_header *
 lustre_acl_xattr_merge2ext(posix_acl_xattr_header *posix_header, int size,
diff --git a/drivers/staging/lustre/lustre/include/lustre_export.h b/drivers/staging/lustre/lustre/include/lustre_export.h
index 1daf4c572415..311e5aa9b0db 100644
--- a/drivers/staging/lustre/lustre/include/lustre_export.h
+++ b/drivers/staging/lustre/lustre/include/lustre_export.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_fid.h b/drivers/staging/lustre/lustre/include/lustre_fid.h
index 47c3f3750240..9b1a9c695113 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fid.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fid.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_fld.h b/drivers/staging/lustre/lustre/include/lustre_fld.h
index d8b3db9cdeba..551162624974 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fld.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fld.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2013, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_ha.h b/drivers/staging/lustre/lustre/include/lustre_ha.h
index 49dfbb14f381..5488a698dabd 100644
--- a/drivers/staging/lustre/lustre/include/lustre_ha.h
+++ b/drivers/staging/lustre/lustre/include/lustre_ha.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_log.h b/drivers/staging/lustre/lustre/include/lustre_log.h
index 1de0c4d6f7f7..e4fc8b5e1336 100644
--- a/drivers/staging/lustre/lustre/include/lustre_log.h
+++ b/drivers/staging/lustre/lustre/include/lustre_log.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -94,9 +94,6 @@ int llog_open(const struct lu_env *env, struct llog_ctxt *ctxt,
 	      struct llog_handle **lgh, struct llog_logid *logid,
 	      char *name, enum llog_open_param open_param);
 int llog_close(const struct lu_env *env, struct llog_handle *cathandle);
-int llog_backup(const struct lu_env *env, struct obd_device *obd,
-		struct llog_ctxt *ctxt, struct llog_ctxt *bak_ctxt,
-		char *name, char *backup);
 
 /* llog_process flags */
 #define LLOG_FLAG_NODEAMON 0x0001
diff --git a/drivers/staging/lustre/lustre/include/lustre_mds.h b/drivers/staging/lustre/lustre/include/lustre_mds.h
index a16eb8b61178..95d27ddecfb3 100644
--- a/drivers/staging/lustre/lustre/include/lustre_mds.h
+++ b/drivers/staging/lustre/lustre/include/lustre_mds.h
@@ -62,12 +62,6 @@ struct mds_group_info {
 #define MDD_OBD_NAME     "mdd_obd"
 #define MDD_OBD_UUID     "mdd_obd_uuid"
 
-static inline int md_should_create(__u64 flags)
-{
-       return !(flags & MDS_OPEN_DELAY_CREATE ||
-	       !(flags & FMODE_WRITE));
-}
-
 /* these are local flags, used only on the client, private */
 #define M_CHECK_STALE	   0200000000
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index 0127f45ca0c3..d834ddd8183b 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_param.h b/drivers/staging/lustre/lustre/include/lustre_param.h
index 8f6c0b26cfab..383fe6febe4b 100644
--- a/drivers/staging/lustre/lustre/include/lustre_param.h
+++ b/drivers/staging/lustre/lustre/include/lustre_param.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/lustre_req_layout.h b/drivers/staging/lustre/lustre/include/lustre_req_layout.h
index df292f6d4a85..46a662f89322 100644
--- a/drivers/staging/lustre/lustre/include/lustre_req_layout.h
+++ b/drivers/staging/lustre/lustre/include/lustre_req_layout.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/include/obd.h b/drivers/staging/lustre/lustre/include/obd.h
index 5e93afca3435..bcbe61301713 100644
--- a/drivers/staging/lustre/lustre/include/obd.h
+++ b/drivers/staging/lustre/lustre/include/obd.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -963,123 +963,123 @@ struct md_enqueue_info {
 };
 
 struct obd_ops {
-	struct module *o_owner;
-	int (*o_iocontrol)(unsigned int cmd, struct obd_export *exp, int len,
-			   void *karg, void *uarg);
-	int (*o_get_info)(const struct lu_env *env, struct obd_export *,
-			  __u32 keylen, void *key, __u32 *vallen, void *val,
-			  struct lov_stripe_md *lsm);
-	int (*o_set_info_async)(const struct lu_env *, struct obd_export *,
-				__u32 keylen, void *key,
-				__u32 vallen, void *val,
-				struct ptlrpc_request_set *set);
-	int (*o_attach)(struct obd_device *dev, u32 len, void *data);
-	int (*o_detach)(struct obd_device *dev);
-	int (*o_setup)(struct obd_device *dev, struct lustre_cfg *cfg);
-	int (*o_precleanup)(struct obd_device *dev,
-			    enum obd_cleanup_stage cleanup_stage);
-	int (*o_cleanup)(struct obd_device *dev);
-	int (*o_process_config)(struct obd_device *dev, u32 len, void *data);
-	int (*o_postrecov)(struct obd_device *dev);
-	int (*o_add_conn)(struct obd_import *imp, struct obd_uuid *uuid,
-			  int priority);
-	int (*o_del_conn)(struct obd_import *imp, struct obd_uuid *uuid);
+	struct module *owner;
+	int (*iocontrol)(unsigned int cmd, struct obd_export *exp, int len,
+			 void *karg, void *uarg);
+	int (*get_info)(const struct lu_env *env, struct obd_export *,
+			__u32 keylen, void *key, __u32 *vallen, void *val,
+			struct lov_stripe_md *lsm);
+	int (*set_info_async)(const struct lu_env *, struct obd_export *,
+			      __u32 keylen, void *key,
+			      __u32 vallen, void *val,
+			      struct ptlrpc_request_set *set);
+	int (*attach)(struct obd_device *dev, u32 len, void *data);
+	int (*detach)(struct obd_device *dev);
+	int (*setup)(struct obd_device *dev, struct lustre_cfg *cfg);
+	int (*precleanup)(struct obd_device *dev,
+			  enum obd_cleanup_stage cleanup_stage);
+	int (*cleanup)(struct obd_device *dev);
+	int (*process_config)(struct obd_device *dev, u32 len, void *data);
+	int (*postrecov)(struct obd_device *dev);
+	int (*add_conn)(struct obd_import *imp, struct obd_uuid *uuid,
+			int priority);
+	int (*del_conn)(struct obd_import *imp, struct obd_uuid *uuid);
 	/* connect to the target device with given connection
 	 * data. @ocd->ocd_connect_flags is modified to reflect flags actually
 	 * granted by the target, which are guaranteed to be a subset of flags
 	 * asked for. If @ocd == NULL, use default parameters. */
-	int (*o_connect)(const struct lu_env *env,
-			 struct obd_export **exp, struct obd_device *src,
-			 struct obd_uuid *cluuid, struct obd_connect_data *ocd,
+	int (*connect)(const struct lu_env *env,
+		       struct obd_export **exp, struct obd_device *src,
+		       struct obd_uuid *cluuid, struct obd_connect_data *ocd,
+		       void *localdata);
+	int (*reconnect)(const struct lu_env *env,
+			 struct obd_export *exp, struct obd_device *src,
+			 struct obd_uuid *cluuid,
+			 struct obd_connect_data *ocd,
 			 void *localdata);
-	int (*o_reconnect)(const struct lu_env *env,
-			   struct obd_export *exp, struct obd_device *src,
-			   struct obd_uuid *cluuid,
-			   struct obd_connect_data *ocd,
-			   void *localdata);
-	int (*o_disconnect)(struct obd_export *exp);
+	int (*disconnect)(struct obd_export *exp);
 
 	/* Initialize/finalize fids infrastructure. */
-	int (*o_fid_init)(struct obd_device *obd,
-			  struct obd_export *exp, enum lu_cli_type type);
-	int (*o_fid_fini)(struct obd_device *obd);
+	int (*fid_init)(struct obd_device *obd,
+			struct obd_export *exp, enum lu_cli_type type);
+	int (*fid_fini)(struct obd_device *obd);
 
 	/* Allocate new fid according to passed @hint. */
-	int (*o_fid_alloc)(struct obd_export *exp, struct lu_fid *fid,
-			   struct md_op_data *op_data);
+	int (*fid_alloc)(struct obd_export *exp, struct lu_fid *fid,
+			 struct md_op_data *op_data);
 
 	/*
 	 * Object with @fid is getting deleted, we may want to do something
 	 * about this.
 	 */
-	int (*o_statfs)(const struct lu_env *, struct obd_export *exp,
-			struct obd_statfs *osfs, __u64 max_age, __u32 flags);
-	int (*o_statfs_async)(struct obd_export *exp, struct obd_info *oinfo,
-			      __u64 max_age, struct ptlrpc_request_set *set);
-	int (*o_packmd)(struct obd_export *exp, struct lov_mds_md **disk_tgt,
-			struct lov_stripe_md *mem_src);
-	int (*o_unpackmd)(struct obd_export *exp,
-			  struct lov_stripe_md **mem_tgt,
-			  struct lov_mds_md *disk_src, int disk_len);
-	int (*o_preallocate)(struct lustre_handle *, u32 *req, u64 *ids);
-	int (*o_create)(const struct lu_env *env, struct obd_export *exp,
-			struct obdo *oa, struct lov_stripe_md **ea,
-			struct obd_trans_info *oti);
-	int (*o_destroy)(const struct lu_env *env, struct obd_export *exp,
-			 struct obdo *oa, struct lov_stripe_md *ea,
-			 struct obd_trans_info *oti, struct obd_export *md_exp);
-	int (*o_setattr)(const struct lu_env *, struct obd_export *exp,
-			 struct obd_info *oinfo, struct obd_trans_info *oti);
-	int (*o_setattr_async)(struct obd_export *exp, struct obd_info *oinfo,
-			       struct obd_trans_info *oti,
-			       struct ptlrpc_request_set *rqset);
-	int (*o_getattr)(const struct lu_env *env, struct obd_export *exp,
-			 struct obd_info *oinfo);
-	int (*o_getattr_async)(struct obd_export *exp, struct obd_info *oinfo,
-			       struct ptlrpc_request_set *set);
-	int (*o_adjust_kms)(struct obd_export *exp, struct lov_stripe_md *lsm,
-			    u64 size, int shrink);
-	int (*o_preprw)(const struct lu_env *env, int cmd,
-			struct obd_export *exp, struct obdo *oa, int objcount,
-			struct obd_ioobj *obj, struct niobuf_remote *remote,
-			int *nr_pages, struct niobuf_local *local,
-			struct obd_trans_info *oti);
-	int (*o_commitrw)(const struct lu_env *env, int cmd,
-			  struct obd_export *exp, struct obdo *oa,
-			  int objcount, struct obd_ioobj *obj,
-			  struct niobuf_remote *remote, int pages,
-			  struct niobuf_local *local,
-			  struct obd_trans_info *oti, int rc);
-	int (*o_find_cbdata)(struct obd_export *, struct lov_stripe_md *,
-			     ldlm_iterator_t it, void *data);
-	int (*o_init_export)(struct obd_export *exp);
-	int (*o_destroy_export)(struct obd_export *exp);
+	int (*statfs)(const struct lu_env *, struct obd_export *exp,
+		      struct obd_statfs *osfs, __u64 max_age, __u32 flags);
+	int (*statfs_async)(struct obd_export *exp, struct obd_info *oinfo,
+			    __u64 max_age, struct ptlrpc_request_set *set);
+	int (*packmd)(struct obd_export *exp, struct lov_mds_md **disk_tgt,
+		      struct lov_stripe_md *mem_src);
+	int (*unpackmd)(struct obd_export *exp,
+			struct lov_stripe_md **mem_tgt,
+			struct lov_mds_md *disk_src, int disk_len);
+	int (*preallocate)(struct lustre_handle *, u32 *req, u64 *ids);
+	int (*create)(const struct lu_env *env, struct obd_export *exp,
+		      struct obdo *oa, struct lov_stripe_md **ea,
+		      struct obd_trans_info *oti);
+	int (*destroy)(const struct lu_env *env, struct obd_export *exp,
+		       struct obdo *oa, struct lov_stripe_md *ea,
+		       struct obd_trans_info *oti, struct obd_export *md_exp);
+	int (*setattr)(const struct lu_env *, struct obd_export *exp,
+		       struct obd_info *oinfo, struct obd_trans_info *oti);
+	int (*setattr_async)(struct obd_export *exp, struct obd_info *oinfo,
+			     struct obd_trans_info *oti,
+			     struct ptlrpc_request_set *rqset);
+	int (*getattr)(const struct lu_env *env, struct obd_export *exp,
+		       struct obd_info *oinfo);
+	int (*getattr_async)(struct obd_export *exp, struct obd_info *oinfo,
+			     struct ptlrpc_request_set *set);
+	int (*adjust_kms)(struct obd_export *exp, struct lov_stripe_md *lsm,
+			  u64 size, int shrink);
+	int (*preprw)(const struct lu_env *env, int cmd,
+		      struct obd_export *exp, struct obdo *oa, int objcount,
+		      struct obd_ioobj *obj, struct niobuf_remote *remote,
+		      int *nr_pages, struct niobuf_local *local,
+		      struct obd_trans_info *oti);
+	int (*commitrw)(const struct lu_env *env, int cmd,
+			struct obd_export *exp, struct obdo *oa,
+			int objcount, struct obd_ioobj *obj,
+			struct niobuf_remote *remote, int pages,
+			struct niobuf_local *local,
+			struct obd_trans_info *oti, int rc);
+	int (*find_cbdata)(struct obd_export *, struct lov_stripe_md *,
+			   ldlm_iterator_t it, void *data);
+	int (*init_export)(struct obd_export *exp);
+	int (*destroy_export)(struct obd_export *exp);
 
 	/* metadata-only methods */
-	int (*o_import_event)(struct obd_device *, struct obd_import *,
-			      enum obd_import_event);
+	int (*import_event)(struct obd_device *, struct obd_import *,
+			    enum obd_import_event);
 
-	int (*o_notify)(struct obd_device *obd, struct obd_device *watched,
-			enum obd_notify_event ev, void *data);
+	int (*notify)(struct obd_device *obd, struct obd_device *watched,
+		      enum obd_notify_event ev, void *data);
 
-	int (*o_health_check)(const struct lu_env *env, struct obd_device *);
-	struct obd_uuid *(*o_get_uuid)(struct obd_export *exp);
+	int (*health_check)(const struct lu_env *env, struct obd_device *);
+	struct obd_uuid *(*get_uuid)(struct obd_export *exp);
 
 	/* quota methods */
-	int (*o_quotacheck)(struct obd_device *, struct obd_export *,
-			    struct obd_quotactl *);
-	int (*o_quotactl)(struct obd_device *, struct obd_export *,
+	int (*quotacheck)(struct obd_device *, struct obd_export *,
 			  struct obd_quotactl *);
+	int (*quotactl)(struct obd_device *, struct obd_export *,
+			struct obd_quotactl *);
 
 	/* pools methods */
-	int (*o_pool_new)(struct obd_device *obd, char *poolname);
-	int (*o_pool_del)(struct obd_device *obd, char *poolname);
-	int (*o_pool_add)(struct obd_device *obd, char *poolname,
-			  char *ostname);
-	int (*o_pool_rem)(struct obd_device *obd, char *poolname,
-			  char *ostname);
-	void (*o_getref)(struct obd_device *obd);
-	void (*o_putref)(struct obd_device *obd);
+	int (*pool_new)(struct obd_device *obd, char *poolname);
+	int (*pool_del)(struct obd_device *obd, char *poolname);
+	int (*pool_add)(struct obd_device *obd, char *poolname,
+			char *ostname);
+	int (*pool_rem)(struct obd_device *obd, char *poolname,
+			char *ostname);
+	void (*getref)(struct obd_device *obd);
+	void (*putref)(struct obd_device *obd);
 	/*
 	 * NOTE: If adding ops, add another LPROCFS_OBD_OP_INIT() line
 	 * to lprocfs_alloc_obd_stats() in obdclass/lprocfs_status.c.
@@ -1124,89 +1124,89 @@ struct md_open_data {
 struct lookup_intent;
 
 struct md_ops {
-	int (*m_getstatus)(struct obd_export *, struct lu_fid *);
-	int (*m_null_inode)(struct obd_export *, const struct lu_fid *);
-	int (*m_find_cbdata)(struct obd_export *, const struct lu_fid *,
-			     ldlm_iterator_t, void *);
-	int (*m_close)(struct obd_export *, struct md_op_data *,
-		       struct md_open_data *, struct ptlrpc_request **);
-	int (*m_create)(struct obd_export *, struct md_op_data *,
-			const void *, int, int, __u32, __u32, cfs_cap_t,
-			__u64, struct ptlrpc_request **);
-	int (*m_done_writing)(struct obd_export *, struct md_op_data  *,
-			      struct md_open_data *);
-	int (*m_enqueue)(struct obd_export *, struct ldlm_enqueue_info *,
-			 struct lookup_intent *, struct md_op_data *,
-			 struct lustre_handle *, void *, int,
-			 struct ptlrpc_request **, __u64);
-	int (*m_getattr)(struct obd_export *, struct md_op_data *,
-			 struct ptlrpc_request **);
-	int (*m_getattr_name)(struct obd_export *, struct md_op_data *,
-			      struct ptlrpc_request **);
-	int (*m_intent_lock)(struct obd_export *, struct md_op_data *,
-			     void *, int, struct lookup_intent *, int,
-			     struct ptlrpc_request **,
-			     ldlm_blocking_callback, __u64);
-	int (*m_link)(struct obd_export *, struct md_op_data *,
+	int (*getstatus)(struct obd_export *, struct lu_fid *);
+	int (*null_inode)(struct obd_export *, const struct lu_fid *);
+	int (*find_cbdata)(struct obd_export *, const struct lu_fid *,
+			   ldlm_iterator_t, void *);
+	int (*close)(struct obd_export *, struct md_op_data *,
+		     struct md_open_data *, struct ptlrpc_request **);
+	int (*create)(struct obd_export *, struct md_op_data *,
+		      const void *, int, int, __u32, __u32, cfs_cap_t,
+		      __u64, struct ptlrpc_request **);
+	int (*done_writing)(struct obd_export *, struct md_op_data  *,
+			    struct md_open_data *);
+	int (*enqueue)(struct obd_export *, struct ldlm_enqueue_info *,
+		       struct lookup_intent *, struct md_op_data *,
+		       struct lustre_handle *, void *, int,
+		       struct ptlrpc_request **, __u64);
+	int (*getattr)(struct obd_export *, struct md_op_data *,
+		       struct ptlrpc_request **);
+	int (*getattr_name)(struct obd_export *, struct md_op_data *,
+			    struct ptlrpc_request **);
+	int (*intent_lock)(struct obd_export *, struct md_op_data *,
+			   void *, int, struct lookup_intent *, int,
+			   struct ptlrpc_request **,
+			   ldlm_blocking_callback, __u64);
+	int (*link)(struct obd_export *, struct md_op_data *,
+		    struct ptlrpc_request **);
+	int (*rename)(struct obd_export *, struct md_op_data *,
+		      const char *, int, const char *, int,
 		      struct ptlrpc_request **);
-	int (*m_rename)(struct obd_export *, struct md_op_data *,
-			const char *, int, const char *, int,
-			struct ptlrpc_request **);
-	int (*m_is_subdir)(struct obd_export *, const struct lu_fid *,
-			   const struct lu_fid *,
+	int (*is_subdir)(struct obd_export *, const struct lu_fid *,
+			 const struct lu_fid *,
 			   struct ptlrpc_request **);
-	int (*m_setattr)(struct obd_export *, struct md_op_data *, void *,
-			 int, void *, int, struct ptlrpc_request **,
+	int (*setattr)(struct obd_export *, struct md_op_data *, void *,
+		       int, void *, int, struct ptlrpc_request **,
 			 struct md_open_data **mod);
-	int (*m_sync)(struct obd_export *, const struct lu_fid *,
+	int (*sync)(struct obd_export *, const struct lu_fid *,
+		    struct ptlrpc_request **);
+	int (*readpage)(struct obd_export *, struct md_op_data *,
+			struct page **, struct ptlrpc_request **);
+
+	int (*unlink)(struct obd_export *, struct md_op_data *,
 		      struct ptlrpc_request **);
-	int (*m_readpage)(struct obd_export *, struct md_op_data *,
-			  struct page **, struct ptlrpc_request **);
 
-	int (*m_unlink)(struct obd_export *, struct md_op_data *,
+	int (*setxattr)(struct obd_export *, const struct lu_fid *,
+			u64, const char *, const char *, int, int, int, __u32,
 			struct ptlrpc_request **);
 
-	int (*m_setxattr)(struct obd_export *, const struct lu_fid *,
-			  u64, const char *, const char *, int, int, int, __u32,
-			  struct ptlrpc_request **);
-
-	int (*m_getxattr)(struct obd_export *, const struct lu_fid *,
-			  u64, const char *, const char *, int, int, int,
-			  struct ptlrpc_request **);
+	int (*getxattr)(struct obd_export *, const struct lu_fid *,
+			u64, const char *, const char *, int, int, int,
+			struct ptlrpc_request **);
 
-	int (*m_init_ea_size)(struct obd_export *, int, int, int, int);
+	int (*init_ea_size)(struct obd_export *, int, int, int, int);
 
-	int (*m_get_lustre_md)(struct obd_export *, struct ptlrpc_request *,
-			       struct obd_export *, struct obd_export *,
-			       struct lustre_md *);
+	int (*get_lustre_md)(struct obd_export *, struct ptlrpc_request *,
+			     struct obd_export *, struct obd_export *,
+			     struct lustre_md *);
 
-	int (*m_free_lustre_md)(struct obd_export *, struct lustre_md *);
+	int (*free_lustre_md)(struct obd_export *, struct lustre_md *);
 
-	int (*m_set_open_replay_data)(struct obd_export *,
-				      struct obd_client_handle *,
-				      struct lookup_intent *);
-	int (*m_clear_open_replay_data)(struct obd_export *,
-					struct obd_client_handle *);
-	int (*m_set_lock_data)(struct obd_export *, __u64 *, void *, __u64 *);
+	int (*set_open_replay_data)(struct obd_export *,
+				    struct obd_client_handle *,
+				    struct lookup_intent *);
+	int (*clear_open_replay_data)(struct obd_export *,
+				      struct obd_client_handle *);
+	int (*set_lock_data)(struct obd_export *, __u64 *, void *, __u64 *);
 
-	ldlm_mode_t (*m_lock_match)(struct obd_export *, __u64,
-				    const struct lu_fid *, ldlm_type_t,
-				    ldlm_policy_data_t *, ldlm_mode_t,
-				    struct lustre_handle *);
+	ldlm_mode_t (*lock_match)(struct obd_export *, __u64,
+				  const struct lu_fid *, ldlm_type_t,
+				  ldlm_policy_data_t *, ldlm_mode_t,
+				  struct lustre_handle *);
 
-	int (*m_cancel_unused)(struct obd_export *, const struct lu_fid *,
-			       ldlm_policy_data_t *, ldlm_mode_t,
-			       ldlm_cancel_flags_t flags, void *opaque);
+	int (*cancel_unused)(struct obd_export *, const struct lu_fid *,
+			     ldlm_policy_data_t *, ldlm_mode_t,
+			     ldlm_cancel_flags_t flags, void *opaque);
 
-	int (*m_get_remote_perm)(struct obd_export *, const struct lu_fid *,
-				 __u32, struct ptlrpc_request **);
+	int (*get_remote_perm)(struct obd_export *, const struct lu_fid *,
+			       __u32, struct ptlrpc_request **);
 
-	int (*m_intent_getattr_async)(struct obd_export *,
-				      struct md_enqueue_info *,
-				      struct ldlm_enqueue_info *);
+	int (*intent_getattr_async)(struct obd_export *,
+				    struct md_enqueue_info *,
+				    struct ldlm_enqueue_info *);
 
-	int (*m_revalidate_lock)(struct obd_export *, struct lookup_intent *,
-				 struct lu_fid *, __u64 *bits);
+	int (*revalidate_lock)(struct obd_export *, struct lookup_intent *,
+			       struct lu_fid *, __u64 *bits);
 
 	/*
 	 * NOTE: If adding ops, add another LPROCFS_MD_OP_INIT() line to
diff --git a/drivers/staging/lustre/lustre/include/obd_cksum.h b/drivers/staging/lustre/lustre/include/obd_cksum.h
index a0099d71773a..01db60405393 100644
--- a/drivers/staging/lustre/lustre/include/obd_cksum.h
+++ b/drivers/staging/lustre/lustre/include/obd_cksum.h
@@ -133,29 +133,6 @@ static inline cksum_type_t cksum_types_supported_client(void)
 	return ret;
 }
 
-/* Server uses algos that perform at 50% or better of the Adler */
-static inline cksum_type_t cksum_types_supported_server(void)
-{
-	int	     base_speed;
-	cksum_type_t    ret = OBD_CKSUM_ADLER;
-
-	CDEBUG(D_INFO, "Crypto hash speed: crc %d, crc32c %d, adler %d\n",
-	       cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_CRC32)),
-	       cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_CRC32C)),
-	       cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_ADLER)));
-
-	base_speed = cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_ADLER)) / 2;
-
-	if (cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_CRC32C)) >=
-	    base_speed)
-		ret |= OBD_CKSUM_CRC32C;
-	if (cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_CRC32)) >=
-	    base_speed)
-		ret |= OBD_CKSUM_CRC32;
-
-	return ret;
-}
-
 /* Select the best checksum algorithm among those supplied in the cksum_types
  * input.
  *
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index fd5f3731db92..97d80397503c 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -270,8 +270,8 @@ void obdo_to_ioobj(struct obdo *oa, struct obd_ioobj *ioobj);
 void md_from_obdo(struct md_op_data *op_data, struct obdo *oa, u32 valid);
 
 #define OBT(dev)	(dev)->obd_type
-#define OBP(dev, op)    (dev)->obd_type->typ_dt_ops->o_ ## op
-#define MDP(dev, op)    (dev)->obd_type->typ_md_ops->m_ ## op
+#define OBP(dev, op)    (dev)->obd_type->typ_dt_ops->op
+#define MDP(dev, op)    (dev)->obd_type->typ_md_ops->op
 #define CTXTP(ctxt, op) (ctxt)->loc_logops->lop_##op
 
 /* Ensure obd_setup: used for cleanup which must be called
@@ -301,9 +301,9 @@ static inline int obd_check_dev_active(struct obd_device *obd)
 }
 
 #define OBD_COUNTER_OFFSET(op)				  \
-	((offsetof(struct obd_ops, o_ ## op) -		  \
-	  offsetof(struct obd_ops, o_iocontrol))		\
-	 / sizeof(((struct obd_ops *)(0))->o_iocontrol))
+	((offsetof(struct obd_ops, op) -		  \
+	  offsetof(struct obd_ops, iocontrol))		\
+	 / sizeof(((struct obd_ops *)(0))->iocontrol))
 
 #define OBD_COUNTER_INCREMENT(obdx, op)			   \
 	if ((obdx)->obd_stats != NULL) {			  \
@@ -324,9 +324,9 @@ static inline int obd_check_dev_active(struct obd_device *obd)
 	}
 
 #define MD_COUNTER_OFFSET(op)				   \
-	((offsetof(struct md_ops, m_ ## op) -		   \
-	  offsetof(struct md_ops, m_getstatus))		 \
-	 / sizeof(((struct md_ops *)(0))->m_getstatus))
+	((offsetof(struct md_ops, op) -		   \
+	  offsetof(struct md_ops, getstatus))		 \
+	 / sizeof(((struct md_ops *)(0))->getstatus))
 
 #define MD_COUNTER_INCREMENT(obdx, op)			   \
 	if ((obd)->md_stats != NULL) {			   \
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index a22a5308fb48..d031437c0528 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index 0b8e4d2175ae..34dde7dede74 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -427,7 +427,7 @@ static void ccc_object_size_lock(struct cl_object *obj)
 {
 	struct inode *inode = ccc_object_inode(obj);
 
-	cl_isize_lock(inode);
+	ll_inode_size_lock(inode);
 	cl_object_attr_lock(obj);
 }
 
@@ -436,7 +436,7 @@ static void ccc_object_size_unlock(struct cl_object *obj)
 	struct inode *inode = ccc_object_inode(obj);
 
 	cl_object_attr_unlock(obj);
-	cl_isize_unlock(inode);
+	ll_inode_size_unlock(inode);
 }
 
 /*****************************************************************************
diff --git a/drivers/staging/lustre/lustre/ldlm/interval_tree.c b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
index 39b571721881..a2ea8e5b93d8 100644
--- a/drivers/staging/lustre/lustre/ldlm/interval_tree.c
+++ b/drivers/staging/lustre/lustre/ldlm/interval_tree.c
@@ -96,18 +96,6 @@ static inline int extent_equal(struct interval_node_extent *e1,
 	return (e1->start == e2->start) && (e1->end == e2->end);
 }
 
-static inline int node_compare(struct interval_node *n1,
-			       struct interval_node *n2)
-{
-	return extent_compare(&n1->in_extent, &n2->in_extent);
-}
-
-static inline int node_equal(struct interval_node *n1,
-			     struct interval_node *n2)
-{
-	return extent_equal(&n1->in_extent, &n2->in_extent);
-}
-
 static inline __u64 max_u64(__u64 x, __u64 y)
 {
 	return x > y ? x : y;
@@ -278,14 +266,14 @@ struct interval_node *interval_insert(struct interval_node *node,
 	p = root;
 	while (*p) {
 		parent = *p;
-		if (node_equal(parent, node))
+		if (extent_equal(&parent->in_extent, &node->in_extent))
 			return parent;
 
 		/* max_high field must be updated after each iteration */
 		if (parent->in_max_high < interval_high(node))
 			parent->in_max_high = interval_high(node);
 
-		if (node_compare(node, parent) < 0)
+		if (extent_compare(&node->in_extent, &parent->in_extent) < 0)
 			p = &parent->in_left;
 		else
 			p = &parent->in_right;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
index c787888eb8af..9c70f31ea56e 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_extent.c
@@ -149,7 +149,7 @@ static inline int lock_mode_to_index(ldlm_mode_t mode)
 	int index;
 
 	LASSERT(mode != 0);
-	LASSERT(IS_PO2(mode));
+	LASSERT(is_power_of_2(mode));
 	for (index = -1; mode; index++)
 		mode >>= 1;
 	LASSERT(index < LCK_MODE_NUM);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
index db3c9b7af7d5..849cc98df7dd 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index ccce1e503120..3c8d4413d976 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index 7f8c70056ffd..cf9ec0cfe247 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index ca115119501a..79aeb2bf6c8e 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index 1a4eef64658f..3d7c137d223a 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -176,11 +176,6 @@ enum {
 	LDLM_POOL_LAST_STAT
 };
 
-static inline struct ldlm_namespace *ldlm_pl2ns(struct ldlm_pool *pl)
-{
-	return container_of(pl, struct ldlm_namespace, ns_pool);
-}
-
 /**
  * Calculates suggested grant_step in % of available locks for passed
  * \a period. This is later used in grant_plan calculations.
@@ -213,22 +208,6 @@ static inline int ldlm_pool_t2gsp(unsigned int t)
 }
 
 /**
- * Returns current \a pl limit.
- */
-static __u32 ldlm_pool_get_limit(struct ldlm_pool *pl)
-{
-	return atomic_read(&pl->pl_limit);
-}
-
-/**
- * Sets passed \a limit to \a pl.
- */
-static void ldlm_pool_set_limit(struct ldlm_pool *pl, __u32 limit)
-{
-	atomic_set(&pl->pl_limit, limit);
-}
-
-/**
  * Recalculates next stats on passed \a pl.
  *
  * \pre ->pl_lock is locked.
@@ -254,7 +233,8 @@ static void ldlm_pool_recalc_stats(struct ldlm_pool *pl)
 }
 
 /**
- * Sets SLV and Limit from ldlm_pl2ns(pl)->ns_obd tp passed \a pl.
+ * Sets SLV and Limit from container_of(pl, struct ldlm_namespace,
+ * ns_pool)->ns_obd tp passed \a pl.
  */
 static void ldlm_cli_pool_pop_slv(struct ldlm_pool *pl)
 {
@@ -264,11 +244,12 @@ static void ldlm_cli_pool_pop_slv(struct ldlm_pool *pl)
 	 * Get new SLV and Limit from obd which is updated with coming
 	 * RPCs.
 	 */
-	obd = ldlm_pl2ns(pl)->ns_obd;
+	obd = container_of(pl, struct ldlm_namespace,
+			   ns_pool)->ns_obd;
 	LASSERT(obd != NULL);
 	read_lock(&obd->obd_pool_lock);
 	pl->pl_server_lock_volume = obd->obd_pool_slv;
-	ldlm_pool_set_limit(pl, obd->obd_pool_limit);
+	atomic_set(&pl->pl_limit, obd->obd_pool_limit);
 	read_unlock(&obd->obd_pool_lock);
 }
 
@@ -304,7 +285,8 @@ static int ldlm_cli_pool_recalc(struct ldlm_pool *pl)
 	/*
 	 * Do not cancel locks in case lru resize is disabled for this ns.
 	 */
-	if (!ns_connect_lru_resize(ldlm_pl2ns(pl))) {
+	if (!ns_connect_lru_resize(container_of(pl, struct ldlm_namespace,
+						ns_pool))) {
 		ret = 0;
 		goto out;
 	}
@@ -315,7 +297,8 @@ static int ldlm_cli_pool_recalc(struct ldlm_pool *pl)
 	 * It may be called when SLV has changed much, this is why we do not
 	 * take into account pl->pl_recalc_time here.
 	 */
-	ret = ldlm_cancel_lru(ldlm_pl2ns(pl), 0, LCF_ASYNC, LDLM_CANCEL_LRUR);
+	ret = ldlm_cancel_lru(container_of(pl, struct ldlm_namespace, ns_pool),
+			      0, LCF_ASYNC, LDLM_CANCEL_LRUR);
 
 out:
 	spin_lock(&pl->pl_lock);
@@ -341,7 +324,7 @@ static int ldlm_cli_pool_shrink(struct ldlm_pool *pl,
 	struct ldlm_namespace *ns;
 	int unused;
 
-	ns = ldlm_pl2ns(pl);
+	ns = container_of(pl, struct ldlm_namespace, ns_pool);
 
 	/*
 	 * Do not cancel locks in case lru resize is disabled for this ns.
@@ -453,7 +436,7 @@ static int lprocfs_pool_state_seq_show(struct seq_file *m, void *unused)
 	spin_lock(&pl->pl_lock);
 	slv = pl->pl_server_lock_volume;
 	clv = pl->pl_client_lock_volume;
-	limit = ldlm_pool_get_limit(pl);
+	limit = atomic_read(&pl->pl_limit);
 	granted = atomic_read(&pl->pl_granted);
 	grant_rate = atomic_read(&pl->pl_grant_rate);
 	cancel_rate = atomic_read(&pl->pl_cancel_rate);
@@ -558,7 +541,8 @@ static struct kobj_type ldlm_pl_ktype = {
 
 static int ldlm_pool_sysfs_init(struct ldlm_pool *pl)
 {
-	struct ldlm_namespace *ns = ldlm_pl2ns(pl);
+	struct ldlm_namespace *ns = container_of(pl, struct ldlm_namespace,
+						 ns_pool);
 	int err;
 
 	init_completion(&pl->pl_kobj_unregister);
@@ -570,7 +554,8 @@ static int ldlm_pool_sysfs_init(struct ldlm_pool *pl)
 
 static int ldlm_pool_debugfs_init(struct ldlm_pool *pl)
 {
-	struct ldlm_namespace *ns = ldlm_pl2ns(pl);
+	struct ldlm_namespace *ns = container_of(pl, struct ldlm_namespace,
+						 ns_pool);
 	struct dentry *debugfs_ns_parent;
 	struct lprocfs_vars pool_vars[2];
 	char *var_name = NULL;
@@ -685,7 +670,7 @@ int ldlm_pool_init(struct ldlm_pool *pl, struct ldlm_namespace *ns,
 	snprintf(pl->pl_name, sizeof(pl->pl_name), "ldlm-pool-%s-%d",
 		 ldlm_ns_name(ns), idx);
 
-	ldlm_pool_set_limit(pl, 1);
+	atomic_set(&pl->pl_limit, 1);
 	pl->pl_server_lock_volume = 0;
 	pl->pl_ops = &ldlm_cli_pool_ops;
 	pl->pl_recalc_period = LDLM_POOL_CLI_DEF_RECALC_PERIOD;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index fdf81b87aad7..b9eb37762434 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index c0a54bf406ca..0ae610015b7c 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -1154,8 +1154,6 @@ ldlm_resource_get(struct ldlm_namespace *ns, struct ldlm_resource *parent,
 			CERROR("%s: lvbo_init failed for resource %#llx:%#llx: rc = %d\n",
 			       ns->ns_obd->obd_name, name->name[0],
 			       name->name[1], rc);
-			kfree(res->lr_lvb_data);
-			res->lr_lvb_data = NULL;
 			res->lr_lvb_len = rc;
 			mutex_unlock(&res->lr_lvb_mutex);
 			ldlm_resource_putref(res);
diff --git a/drivers/staging/lustre/lustre/libcfs/debug.c b/drivers/staging/lustre/lustre/libcfs/debug.c
index 1d1c67164418..0b38dad13546 100644
--- a/drivers/staging/lustre/lustre/libcfs/debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/debug.c
@@ -94,17 +94,14 @@ static struct kernel_param_ops param_ops_debugmb = {
 static unsigned int libcfs_debug_mb;
 module_param(libcfs_debug_mb, debugmb, 0644);
 MODULE_PARM_DESC(libcfs_debug_mb, "Total debug buffer size.");
-EXPORT_SYMBOL(libcfs_debug_mb);
 
 unsigned int libcfs_printk = D_CANTMASK;
 module_param(libcfs_printk, uint, 0644);
 MODULE_PARM_DESC(libcfs_printk, "Lustre kernel debug console mask");
-EXPORT_SYMBOL(libcfs_printk);
 
 unsigned int libcfs_console_ratelimit = 1;
 module_param(libcfs_console_ratelimit, uint, 0644);
 MODULE_PARM_DESC(libcfs_console_ratelimit, "Lustre kernel debug console ratelimit (0 to disable)");
-EXPORT_SYMBOL(libcfs_console_ratelimit);
 
 static int param_set_delay_minmax(const char *val,
 				  const struct kernel_param *kp,
@@ -135,9 +132,7 @@ static int param_get_delay(char *buffer, const struct kernel_param *kp)
 }
 
 unsigned int libcfs_console_max_delay;
-EXPORT_SYMBOL(libcfs_console_max_delay);
 unsigned int libcfs_console_min_delay;
-EXPORT_SYMBOL(libcfs_console_min_delay);
 
 static int param_set_console_max_delay(const char *val,
 				       const struct kernel_param *kp)
@@ -207,10 +202,8 @@ static struct kernel_param_ops param_ops_uintpos = {
 unsigned int libcfs_console_backoff = CDEBUG_DEFAULT_BACKOFF;
 module_param(libcfs_console_backoff, uintpos, 0644);
 MODULE_PARM_DESC(libcfs_console_backoff, "Lustre kernel debug console backoff factor");
-EXPORT_SYMBOL(libcfs_console_backoff);
 
 unsigned int libcfs_debug_binary = 1;
-EXPORT_SYMBOL(libcfs_debug_binary);
 
 unsigned int libcfs_stack = 3 * THREAD_SIZE / 4;
 EXPORT_SYMBOL(libcfs_stack);
@@ -221,7 +214,6 @@ EXPORT_SYMBOL(libcfs_catastrophe);
 unsigned int libcfs_panic_on_lbug = 1;
 module_param(libcfs_panic_on_lbug, uint, 0644);
 MODULE_PARM_DESC(libcfs_panic_on_lbug, "Lustre kernel panic on LBUG");
-EXPORT_SYMBOL(libcfs_panic_on_lbug);
 
 static wait_queue_head_t debug_ctlwq;
 
@@ -512,9 +504,9 @@ int libcfs_debug_init(unsigned long bufsize)
 	}
 
 	if (libcfs_debug_file_path != NULL) {
-		strncpy(libcfs_debug_file_path_arr,
-			libcfs_debug_file_path, PATH_MAX-1);
-		libcfs_debug_file_path_arr[PATH_MAX - 1] = '\0';
+		strlcpy(libcfs_debug_file_path_arr,
+			libcfs_debug_file_path,
+			sizeof(libcfs_debug_file_path_arr));
 	}
 
 	/* If libcfs_debug_mb is set to an invalid value or uninitialized
@@ -565,12 +557,3 @@ int libcfs_debug_mark_buffer(const char *text)
 
 #undef DEBUG_SUBSYSTEM
 #define DEBUG_SUBSYSTEM S_LNET
-
-void libcfs_debug_set_level(unsigned int debug_level)
-{
-	pr_warn("Lustre: Setting portals debug level to %08x\n",
-	       debug_level);
-	libcfs_debug = debug_level;
-}
-
-EXPORT_SYMBOL(libcfs_debug_set_level);
diff --git a/drivers/staging/lustre/lustre/libcfs/fail.c b/drivers/staging/lustre/lustre/libcfs/fail.c
index d39fecebd12d..27831432d69a 100644
--- a/drivers/staging/lustre/lustre/libcfs/fail.c
+++ b/drivers/staging/lustre/lustre/libcfs/fail.c
@@ -26,7 +26,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -126,7 +126,7 @@ int __cfs_fail_timeout_set(__u32 id, __u32 value, int ms, int set)
 	int ret;
 
 	ret = __cfs_fail_check_set(id, value, set);
-	if (ret) {
+	if (ret && likely(ms > 0)) {
 		CERROR("cfs_fail_timeout id %x sleeping for %dms\n",
 		       id, ms);
 		set_current_state(TASK_UNINTERRUPTIBLE);
diff --git a/drivers/staging/lustre/lustre/libcfs/hash.c b/drivers/staging/lustre/lustre/libcfs/hash.c
index 030874428952..4d50510434be 100644
--- a/drivers/staging/lustre/lustre/libcfs/hash.c
+++ b/drivers/staging/lustre/lustre/libcfs/hash.c
@@ -106,9 +106,10 @@
  *   Now we support both locked iteration & lockless iteration of hash
  *   table. Also, user can break the iteration by return 1 in callback.
  */
+#include <linux/seq_file.h>
+#include <linux/log2.h>
 
 #include "../../include/linux/libcfs/libcfs.h"
-#include <linux/seq_file.h>
 
 #if CFS_HASH_DEBUG_LEVEL >= CFS_HASH_DEBUG_1
 static unsigned int warn_on_depth = 8;
@@ -161,49 +162,49 @@ cfs_hash_rw_unlock(union cfs_hash_lock *lock, int exclusive)
 /** No lock hash */
 static struct cfs_hash_lock_ops cfs_hash_nl_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
-	.hs_unlock      = cfs_hash_nl_unlock,
-	.hs_bkt_lock    = cfs_hash_nl_lock,
-	.hs_bkt_unlock  = cfs_hash_nl_unlock,
+	.hs_unlock	= cfs_hash_nl_unlock,
+	.hs_bkt_lock	= cfs_hash_nl_lock,
+	.hs_bkt_unlock	= cfs_hash_nl_unlock,
 };
 
 /** no bucket lock, one spinlock to protect everything */
 static struct cfs_hash_lock_ops cfs_hash_nbl_lops = {
 	.hs_lock	= cfs_hash_spin_lock,
-	.hs_unlock      = cfs_hash_spin_unlock,
-	.hs_bkt_lock    = cfs_hash_nl_lock,
-	.hs_bkt_unlock  = cfs_hash_nl_unlock,
+	.hs_unlock	= cfs_hash_spin_unlock,
+	.hs_bkt_lock	= cfs_hash_nl_lock,
+	.hs_bkt_unlock	= cfs_hash_nl_unlock,
 };
 
 /** spin bucket lock, rehash is enabled */
 static struct cfs_hash_lock_ops cfs_hash_bkt_spin_lops = {
 	.hs_lock	= cfs_hash_rw_lock,
-	.hs_unlock      = cfs_hash_rw_unlock,
-	.hs_bkt_lock    = cfs_hash_spin_lock,
-	.hs_bkt_unlock  = cfs_hash_spin_unlock,
+	.hs_unlock	= cfs_hash_rw_unlock,
+	.hs_bkt_lock	= cfs_hash_spin_lock,
+	.hs_bkt_unlock	= cfs_hash_spin_unlock,
 };
 
 /** rw bucket lock, rehash is enabled */
 static struct cfs_hash_lock_ops cfs_hash_bkt_rw_lops = {
 	.hs_lock	= cfs_hash_rw_lock,
-	.hs_unlock      = cfs_hash_rw_unlock,
-	.hs_bkt_lock    = cfs_hash_rw_lock,
-	.hs_bkt_unlock  = cfs_hash_rw_unlock,
+	.hs_unlock	= cfs_hash_rw_unlock,
+	.hs_bkt_lock	= cfs_hash_rw_lock,
+	.hs_bkt_unlock	= cfs_hash_rw_unlock,
 };
 
 /** spin bucket lock, rehash is disabled */
 static struct cfs_hash_lock_ops cfs_hash_nr_bkt_spin_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
-	.hs_unlock      = cfs_hash_nl_unlock,
-	.hs_bkt_lock    = cfs_hash_spin_lock,
-	.hs_bkt_unlock  = cfs_hash_spin_unlock,
+	.hs_unlock	= cfs_hash_nl_unlock,
+	.hs_bkt_lock	= cfs_hash_spin_lock,
+	.hs_bkt_unlock	= cfs_hash_spin_unlock,
 };
 
 /** rw bucket lock, rehash is disabled */
 static struct cfs_hash_lock_ops cfs_hash_nr_bkt_rw_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
-	.hs_unlock      = cfs_hash_nl_unlock,
-	.hs_bkt_lock    = cfs_hash_rw_lock,
-	.hs_bkt_unlock  = cfs_hash_rw_unlock,
+	.hs_unlock	= cfs_hash_nl_unlock,
+	.hs_bkt_lock	= cfs_hash_rw_lock,
+	.hs_bkt_unlock	= cfs_hash_rw_unlock,
 };
 
 static void
@@ -280,7 +281,7 @@ cfs_hash_hh_hnode_del(struct cfs_hash *hs, struct cfs_hash_bd *bd,
  */
 struct cfs_hash_head_dep {
 	struct hlist_head	hd_head;	/**< entries list */
-	unsigned int		hd_depth;       /**< list length */
+	unsigned int		hd_depth;	/**< list length */
 };
 
 static int
@@ -328,7 +329,7 @@ cfs_hash_hd_hnode_del(struct cfs_hash *hs, struct cfs_hash_bd *bd,
  */
 struct cfs_hash_dhead {
 	struct hlist_head	dh_head;	/**< entries list */
-	struct hlist_node       *dh_tail;	/**< the last entry */
+	struct hlist_node	*dh_tail;	/**< the last entry */
 };
 
 static int
@@ -384,8 +385,8 @@ cfs_hash_dh_hnode_del(struct cfs_hash *hs, struct cfs_hash_bd *bd,
  */
 struct cfs_hash_dhead_dep {
 	struct hlist_head	dd_head;	/**< entries list */
-	struct hlist_node       *dd_tail;	/**< the last entry */
-	unsigned int	    dd_depth;       /**< list length */
+	struct hlist_node	*dd_tail;	/**< the last entry */
+	unsigned int		dd_depth;	/**< list length */
 };
 
 static int
@@ -436,31 +437,31 @@ cfs_hash_dd_hnode_del(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 }
 
 static struct cfs_hash_hlist_ops cfs_hash_hh_hops = {
-	.hop_hhead      = cfs_hash_hh_hhead,
-	.hop_hhead_size = cfs_hash_hh_hhead_size,
-	.hop_hnode_add  = cfs_hash_hh_hnode_add,
-	.hop_hnode_del  = cfs_hash_hh_hnode_del,
+	.hop_hhead	= cfs_hash_hh_hhead,
+	.hop_hhead_size	= cfs_hash_hh_hhead_size,
+	.hop_hnode_add	= cfs_hash_hh_hnode_add,
+	.hop_hnode_del	= cfs_hash_hh_hnode_del,
 };
 
 static struct cfs_hash_hlist_ops cfs_hash_hd_hops = {
-	.hop_hhead      = cfs_hash_hd_hhead,
-	.hop_hhead_size = cfs_hash_hd_hhead_size,
-	.hop_hnode_add  = cfs_hash_hd_hnode_add,
-	.hop_hnode_del  = cfs_hash_hd_hnode_del,
+	.hop_hhead	= cfs_hash_hd_hhead,
+	.hop_hhead_size	= cfs_hash_hd_hhead_size,
+	.hop_hnode_add	= cfs_hash_hd_hnode_add,
+	.hop_hnode_del	= cfs_hash_hd_hnode_del,
 };
 
 static struct cfs_hash_hlist_ops cfs_hash_dh_hops = {
-	.hop_hhead      = cfs_hash_dh_hhead,
-	.hop_hhead_size = cfs_hash_dh_hhead_size,
-	.hop_hnode_add  = cfs_hash_dh_hnode_add,
-	.hop_hnode_del  = cfs_hash_dh_hnode_del,
+	.hop_hhead	= cfs_hash_dh_hhead,
+	.hop_hhead_size	= cfs_hash_dh_hhead_size,
+	.hop_hnode_add	= cfs_hash_dh_hnode_add,
+	.hop_hnode_del	= cfs_hash_dh_hnode_del,
 };
 
 static struct cfs_hash_hlist_ops cfs_hash_dd_hops = {
-	.hop_hhead      = cfs_hash_dd_hhead,
-	.hop_hhead_size = cfs_hash_dd_hhead_size,
-	.hop_hnode_add  = cfs_hash_dd_hnode_add,
-	.hop_hnode_del  = cfs_hash_dd_hnode_del,
+	.hop_hhead	= cfs_hash_dd_hhead,
+	.hop_hhead_size	= cfs_hash_dd_hhead_size,
+	.hop_hnode_add	= cfs_hash_dd_hnode_add,
+	.hop_hnode_del	= cfs_hash_dd_hnode_del,
 };
 
 static void
@@ -529,7 +530,7 @@ void
 cfs_hash_bd_add_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 		       struct hlist_node *hnode)
 {
-	int		rc;
+	int rc;
 
 	rc = hs->hs_hops->hop_hnode_add(hs, bd, hnode);
 	cfs_hash_bd_dep_record(hs, bd, rc);
@@ -572,7 +573,7 @@ cfs_hash_bd_move_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd_old,
 {
 	struct cfs_hash_bucket *obkt = bd_old->bd_bucket;
 	struct cfs_hash_bucket *nbkt = bd_new->bd_bucket;
-	int		rc;
+	int rc;
 
 	if (cfs_hash_bd_compare(bd_old, bd_new) == 0)
 		return;
@@ -593,34 +594,33 @@ cfs_hash_bd_move_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd_old,
 	if (unlikely(nbkt->hsb_version == 0))
 		nbkt->hsb_version++;
 }
-EXPORT_SYMBOL(cfs_hash_bd_move_locked);
 
 enum {
 	/** always set, for sanity (avoid ZERO intent) */
-	CFS_HS_LOOKUP_MASK_FIND     = BIT(0),
+	CFS_HS_LOOKUP_MASK_FIND	= BIT(0),
 	/** return entry with a ref */
-	CFS_HS_LOOKUP_MASK_REF      = BIT(1),
+	CFS_HS_LOOKUP_MASK_REF	= BIT(1),
 	/** add entry if not existing */
-	CFS_HS_LOOKUP_MASK_ADD      = BIT(2),
+	CFS_HS_LOOKUP_MASK_ADD	= BIT(2),
 	/** delete entry, ignore other masks */
-	CFS_HS_LOOKUP_MASK_DEL      = BIT(3),
+	CFS_HS_LOOKUP_MASK_DEL	= BIT(3),
 };
 
 enum cfs_hash_lookup_intent {
 	/** return item w/o refcount */
-	CFS_HS_LOOKUP_IT_PEEK       = CFS_HS_LOOKUP_MASK_FIND,
+	CFS_HS_LOOKUP_IT_PEEK	 = CFS_HS_LOOKUP_MASK_FIND,
 	/** return item with refcount */
-	CFS_HS_LOOKUP_IT_FIND       = (CFS_HS_LOOKUP_MASK_FIND |
-				       CFS_HS_LOOKUP_MASK_REF),
+	CFS_HS_LOOKUP_IT_FIND	 = (CFS_HS_LOOKUP_MASK_FIND |
+				    CFS_HS_LOOKUP_MASK_REF),
 	/** return item w/o refcount if existed, otherwise add */
-	CFS_HS_LOOKUP_IT_ADD	= (CFS_HS_LOOKUP_MASK_FIND |
-				       CFS_HS_LOOKUP_MASK_ADD),
+	CFS_HS_LOOKUP_IT_ADD	 = (CFS_HS_LOOKUP_MASK_FIND |
+				    CFS_HS_LOOKUP_MASK_ADD),
 	/** return item with refcount if existed, otherwise add */
-	CFS_HS_LOOKUP_IT_FINDADD    = (CFS_HS_LOOKUP_IT_FIND |
-				       CFS_HS_LOOKUP_MASK_ADD),
+	CFS_HS_LOOKUP_IT_FINDADD = (CFS_HS_LOOKUP_IT_FIND |
+				    CFS_HS_LOOKUP_MASK_ADD),
 	/** delete if existed */
-	CFS_HS_LOOKUP_IT_FINDDEL    = (CFS_HS_LOOKUP_MASK_FIND |
-				       CFS_HS_LOOKUP_MASK_DEL)
+	CFS_HS_LOOKUP_IT_FINDDEL = (CFS_HS_LOOKUP_MASK_FIND |
+				    CFS_HS_LOOKUP_MASK_DEL)
 };
 
 static struct hlist_node *
@@ -629,10 +629,10 @@ cfs_hash_bd_lookup_intent(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 			  enum cfs_hash_lookup_intent intent)
 
 {
-	struct hlist_head  *hhead = cfs_hash_bd_hhead(hs, bd);
-	struct hlist_node  *ehnode;
-	struct hlist_node  *match;
-	int  intent_add = (intent & CFS_HS_LOOKUP_MASK_ADD) != 0;
+	struct hlist_head *hhead = cfs_hash_bd_hhead(hs, bd);
+	struct hlist_node *ehnode;
+	struct hlist_node *match;
+	int intent_add = (intent & CFS_HS_LOOKUP_MASK_ADD) != 0;
 
 	/* with this function, we can avoid a lot of useless refcount ops,
 	 * which are expensive atomic operations most time. */
@@ -665,7 +665,8 @@ cfs_hash_bd_lookup_intent(struct cfs_hash *hs, struct cfs_hash_bd *bd,
 }
 
 struct hlist_node *
-cfs_hash_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd, const void *key)
+cfs_hash_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
+			  const void *key)
 {
 	return cfs_hash_bd_lookup_intent(hs, bd, key, NULL,
 					 CFS_HS_LOOKUP_IT_FIND);
@@ -673,40 +674,20 @@ cfs_hash_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd, const voi
 EXPORT_SYMBOL(cfs_hash_bd_lookup_locked);
 
 struct hlist_node *
-cfs_hash_bd_peek_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd, const void *key)
+cfs_hash_bd_peek_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
+			const void *key)
 {
 	return cfs_hash_bd_lookup_intent(hs, bd, key, NULL,
 					 CFS_HS_LOOKUP_IT_PEEK);
 }
 EXPORT_SYMBOL(cfs_hash_bd_peek_locked);
 
-struct hlist_node *
-cfs_hash_bd_findadd_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   const void *key, struct hlist_node *hnode,
-			   int noref)
-{
-	return cfs_hash_bd_lookup_intent(hs, bd, key, hnode,
-					 (!noref * CFS_HS_LOOKUP_MASK_REF) |
-					 CFS_HS_LOOKUP_IT_ADD);
-}
-EXPORT_SYMBOL(cfs_hash_bd_findadd_locked);
-
-struct hlist_node *
-cfs_hash_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bd,
-			   const void *key, struct hlist_node *hnode)
-{
-	/* hnode can be NULL, we find the first item with @key */
-	return cfs_hash_bd_lookup_intent(hs, bd, key, hnode,
-					 CFS_HS_LOOKUP_IT_FINDDEL);
-}
-EXPORT_SYMBOL(cfs_hash_bd_finddel_locked);
-
 static void
 cfs_hash_multi_bd_lock(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 		       unsigned n, int excl)
 {
 	struct cfs_hash_bucket *prev = NULL;
-	int		i;
+	int i;
 
 	/**
 	 * bds must be ascendantly ordered by bd->bd_bucket->hsb_index.
@@ -729,7 +710,7 @@ cfs_hash_multi_bd_unlock(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 			 unsigned n, int excl)
 {
 	struct cfs_hash_bucket *prev = NULL;
-	int		i;
+	int i;
 
 	cfs_hash_for_each_bd(bds, n, i) {
 		if (prev != bds[i].bd_bucket) {
@@ -743,8 +724,8 @@ static struct hlist_node *
 cfs_hash_multi_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 				unsigned n, const void *key)
 {
-	struct hlist_node  *ehnode;
-	unsigned	   i;
+	struct hlist_node *ehnode;
+	unsigned i;
 
 	cfs_hash_for_each_bd(bds, n, i) {
 		ehnode = cfs_hash_bd_lookup_intent(hs, &bds[i], key, NULL,
@@ -756,13 +737,13 @@ cfs_hash_multi_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 }
 
 static struct hlist_node *
-cfs_hash_multi_bd_findadd_locked(struct cfs_hash *hs,
-				 struct cfs_hash_bd *bds, unsigned n, const void *key,
+cfs_hash_multi_bd_findadd_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
+				 unsigned n, const void *key,
 				 struct hlist_node *hnode, int noref)
 {
-	struct hlist_node  *ehnode;
-	int		intent;
-	unsigned	   i;
+	struct hlist_node *ehnode;
+	int intent;
+	unsigned i;
 
 	LASSERT(hnode != NULL);
 	intent = (!noref * CFS_HS_LOOKUP_MASK_REF) | CFS_HS_LOOKUP_IT_PEEK;
@@ -777,7 +758,7 @@ cfs_hash_multi_bd_findadd_locked(struct cfs_hash *hs,
 	if (i == 1) { /* only one bucket */
 		cfs_hash_bd_add_locked(hs, &bds[0], hnode);
 	} else {
-		struct cfs_hash_bd      mybd;
+		struct cfs_hash_bd mybd;
 
 		cfs_hash_bd_get(hs, key, &mybd);
 		cfs_hash_bd_add_locked(hs, &mybd, hnode);
@@ -791,8 +772,8 @@ cfs_hash_multi_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 				 unsigned n, const void *key,
 				 struct hlist_node *hnode)
 {
-	struct hlist_node  *ehnode;
-	unsigned	   i;
+	struct hlist_node *ehnode;
+	unsigned int i;
 
 	cfs_hash_for_each_bd(bds, n, i) {
 		ehnode = cfs_hash_bd_lookup_intent(hs, &bds[i], key, hnode,
@@ -806,7 +787,7 @@ cfs_hash_multi_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 static void
 cfs_hash_bd_order(struct cfs_hash_bd *bd1, struct cfs_hash_bd *bd2)
 {
-	int     rc;
+	int rc;
 
 	if (bd2->bd_bucket == NULL)
 		return;
@@ -831,7 +812,8 @@ cfs_hash_bd_order(struct cfs_hash_bd *bd1, struct cfs_hash_bd *bd2)
 }
 
 void
-cfs_hash_dual_bd_get(struct cfs_hash *hs, const void *key, struct cfs_hash_bd *bds)
+cfs_hash_dual_bd_get(struct cfs_hash *hs, const void *key,
+		     struct cfs_hash_bd *bds)
 {
 	/* NB: caller should hold hs_lock.rw if REHASH is set */
 	cfs_hash_bd_from_key(hs, hs->hs_buckets,
@@ -848,21 +830,18 @@ cfs_hash_dual_bd_get(struct cfs_hash *hs, const void *key, struct cfs_hash_bd *b
 
 	cfs_hash_bd_order(&bds[0], &bds[1]);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_get);
 
 void
 cfs_hash_dual_bd_lock(struct cfs_hash *hs, struct cfs_hash_bd *bds, int excl)
 {
 	cfs_hash_multi_bd_lock(hs, bds, 2, excl);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_lock);
 
 void
 cfs_hash_dual_bd_unlock(struct cfs_hash *hs, struct cfs_hash_bd *bds, int excl)
 {
 	cfs_hash_multi_bd_unlock(hs, bds, 2, excl);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_unlock);
 
 struct hlist_node *
 cfs_hash_dual_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
@@ -870,7 +849,6 @@ cfs_hash_dual_bd_lookup_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 {
 	return cfs_hash_multi_bd_lookup_locked(hs, bds, 2, key);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_lookup_locked);
 
 struct hlist_node *
 cfs_hash_dual_bd_findadd_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
@@ -880,7 +858,6 @@ cfs_hash_dual_bd_findadd_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 	return cfs_hash_multi_bd_findadd_locked(hs, bds, 2, key,
 						hnode, noref);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_findadd_locked);
 
 struct hlist_node *
 cfs_hash_dual_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
@@ -888,13 +865,12 @@ cfs_hash_dual_bd_finddel_locked(struct cfs_hash *hs, struct cfs_hash_bd *bds,
 {
 	return cfs_hash_multi_bd_finddel_locked(hs, bds, 2, key, hnode);
 }
-EXPORT_SYMBOL(cfs_hash_dual_bd_finddel_locked);
 
 static void
 cfs_hash_buckets_free(struct cfs_hash_bucket **buckets,
 		      int bkt_size, int prev_size, int size)
 {
-	int     i;
+	int i;
 
 	for (i = prev_size; i < size; i++) {
 		if (buckets[i] != NULL)
@@ -914,7 +890,7 @@ cfs_hash_buckets_realloc(struct cfs_hash *hs, struct cfs_hash_bucket **old_bkts,
 			 unsigned int old_size, unsigned int new_size)
 {
 	struct cfs_hash_bucket **new_bkts;
-	int		 i;
+	int i;
 
 	LASSERT(old_size == 0 || old_bkts != NULL);
 
@@ -932,7 +908,7 @@ cfs_hash_buckets_realloc(struct cfs_hash *hs, struct cfs_hash_bucket **old_bkts,
 
 	for (i = old_size; i < new_size; i++) {
 		struct hlist_head *hhead;
-		struct cfs_hash_bd     bd;
+		struct cfs_hash_bd bd;
 
 		LIBCFS_ALLOC(new_bkts[i], cfs_hash_bkt_size(hs));
 		if (new_bkts[i] == NULL) {
@@ -969,7 +945,7 @@ cfs_hash_buckets_realloc(struct cfs_hash *hs, struct cfs_hash_bucket **old_bkts,
  * @max_bits - Maximum allowed hash table resize, in bits
  * @ops      - Registered hash table operations
  * @flags    - CFS_HASH_REHASH enable synamic hash resizing
- *	   - CFS_HASH_SORT enable chained hash sort
+ *	     - CFS_HASH_SORT enable chained hash sort
  */
 static int cfs_hash_rehash_worker(cfs_workitem_t *wi);
 
@@ -977,10 +953,10 @@ static int cfs_hash_rehash_worker(cfs_workitem_t *wi);
 static int cfs_hash_dep_print(cfs_workitem_t *wi)
 {
 	struct cfs_hash *hs = container_of(wi, struct cfs_hash, hs_dep_wi);
-	int	 dep;
-	int	 bkt;
-	int	 off;
-	int	 bits;
+	int dep;
+	int bkt;
+	int off;
+	int bits;
 
 	spin_lock(&hs->hs_dep_lock);
 	dep  = hs->hs_dep_max;
@@ -1031,7 +1007,7 @@ cfs_hash_create(char *name, unsigned cur_bits, unsigned max_bits,
 		struct cfs_hash_ops *ops, unsigned flags)
 {
 	struct cfs_hash *hs;
-	int	 len;
+	int len;
 
 	CLASSERT(CFS_HASH_THETA_BITS < 15);
 
@@ -1062,8 +1038,7 @@ cfs_hash_create(char *name, unsigned cur_bits, unsigned max_bits,
 	if (hs == NULL)
 		return NULL;
 
-	strncpy(hs->hs_name, name, len);
-	hs->hs_name[len - 1] = '\0';
+	strlcpy(hs->hs_name, name, len);
 	hs->hs_flags = flags;
 
 	atomic_set(&hs->hs_refcount, 1);
@@ -1077,7 +1052,7 @@ cfs_hash_create(char *name, unsigned cur_bits, unsigned max_bits,
 	hs->hs_max_bits = (__u8)max_bits;
 	hs->hs_bkt_bits = (__u8)bkt_bits;
 
-	hs->hs_ops	 = ops;
+	hs->hs_ops	   = ops;
 	hs->hs_extra_bytes = extra_bytes;
 	hs->hs_rehash_bits = 0;
 	cfs_wi_init(&hs->hs_rehash_wi, hs, cfs_hash_rehash_worker);
@@ -1102,10 +1077,10 @@ EXPORT_SYMBOL(cfs_hash_create);
 static void
 cfs_hash_destroy(struct cfs_hash *hs)
 {
-	struct hlist_node     *hnode;
-	struct hlist_node     *pos;
-	struct cfs_hash_bd	 bd;
-	int		   i;
+	struct hlist_node *hnode;
+	struct hlist_node *pos;
+	struct cfs_hash_bd bd;
+	int i;
 
 	LASSERT(hs != NULL);
 	LASSERT(!cfs_hash_is_exiting(hs) &&
@@ -1223,8 +1198,8 @@ cfs_hash_rehash_inline(struct cfs_hash *hs)
 void
 cfs_hash_add(struct cfs_hash *hs, const void *key, struct hlist_node *hnode)
 {
-	struct cfs_hash_bd   bd;
-	int	     bits;
+	struct cfs_hash_bd bd;
+	int bits;
 
 	LASSERT(hlist_unhashed(hnode));
 
@@ -1248,8 +1223,8 @@ cfs_hash_find_or_add(struct cfs_hash *hs, const void *key,
 		     struct hlist_node *hnode, int noref)
 {
 	struct hlist_node *ehnode;
-	struct cfs_hash_bd     bds[2];
-	int	       bits = 0;
+	struct cfs_hash_bd bds[2];
+	int bits = 0;
 
 	LASSERT(hlist_unhashed(hnode));
 
@@ -1261,7 +1236,7 @@ cfs_hash_find_or_add(struct cfs_hash *hs, const void *key,
 						 hnode, noref);
 	cfs_hash_dual_bd_unlock(hs, bds, 1);
 
-	if (ehnode == hnode) /* new item added */
+	if (ehnode == hnode)	/* new item added */
 		bits = cfs_hash_rehash_bits(hs);
 	cfs_hash_unlock(hs, 0);
 	if (bits > 0)
@@ -1276,7 +1251,8 @@ cfs_hash_find_or_add(struct cfs_hash *hs, const void *key,
  * Returns 0 on success or -EALREADY on key collisions.
  */
 int
-cfs_hash_add_unique(struct cfs_hash *hs, const void *key, struct hlist_node *hnode)
+cfs_hash_add_unique(struct cfs_hash *hs, const void *key,
+		    struct hlist_node *hnode)
 {
 	return cfs_hash_find_or_add(hs, key, hnode, 1) != hnode ?
 	       -EALREADY : 0;
@@ -1309,9 +1285,9 @@ EXPORT_SYMBOL(cfs_hash_findadd_unique);
 void *
 cfs_hash_del(struct cfs_hash *hs, const void *key, struct hlist_node *hnode)
 {
-	void	   *obj  = NULL;
-	int	     bits = 0;
-	struct cfs_hash_bd   bds[2];
+	void *obj = NULL;
+	int bits = 0;
+	struct cfs_hash_bd bds[2];
 
 	cfs_hash_lock(hs, 0);
 	cfs_hash_dual_bd_get_and_lock(hs, key, bds, 1);
@@ -1364,9 +1340,9 @@ EXPORT_SYMBOL(cfs_hash_del_key);
 void *
 cfs_hash_lookup(struct cfs_hash *hs, const void *key)
 {
-	void		 *obj = NULL;
-	struct hlist_node     *hnode;
-	struct cfs_hash_bd	 bds[2];
+	void *obj = NULL;
+	struct hlist_node *hnode;
+	struct cfs_hash_bd bds[2];
 
 	cfs_hash_lock(hs, 0);
 	cfs_hash_dual_bd_get_and_lock(hs, key, bds, 0);
@@ -1383,7 +1359,8 @@ cfs_hash_lookup(struct cfs_hash *hs, const void *key)
 EXPORT_SYMBOL(cfs_hash_lookup);
 
 static void
-cfs_hash_for_each_enter(struct cfs_hash *hs) {
+cfs_hash_for_each_enter(struct cfs_hash *hs)
+{
 	LASSERT(!cfs_hash_is_exiting(hs));
 
 	if (!cfs_hash_with_rehash(hs))
@@ -1408,7 +1385,8 @@ cfs_hash_for_each_enter(struct cfs_hash *hs) {
 }
 
 static void
-cfs_hash_for_each_exit(struct cfs_hash *hs) {
+cfs_hash_for_each_exit(struct cfs_hash *hs)
+{
 	int remained;
 	int bits;
 
@@ -1439,14 +1417,15 @@ cfs_hash_for_each_exit(struct cfs_hash *hs) {
  */
 static __u64
 cfs_hash_for_each_tight(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
-			void *data, int remove_safe) {
-	struct hlist_node     *hnode;
-	struct hlist_node     *pos;
-	struct cfs_hash_bd	 bd;
-	__u64		 count = 0;
-	int		   excl  = !!remove_safe;
-	int		   loop  = 0;
-	int		   i;
+			void *data, int remove_safe)
+{
+	struct hlist_node *hnode;
+	struct hlist_node *pos;
+	struct cfs_hash_bd bd;
+	__u64 count = 0;
+	int excl = !!remove_safe;
+	int loop = 0;
+	int i;
 
 	cfs_hash_for_each_enter(hs);
 
@@ -1514,8 +1493,8 @@ void
 cfs_hash_cond_del(struct cfs_hash *hs, cfs_hash_cond_opt_cb_t func, void *data)
 {
 	struct cfs_hash_cond_arg arg = {
-		.func   = func,
-		.arg    = data,
+		.func	= func,
+		.arg	= data,
 	};
 
 	cfs_hash_for_each_tight(hs, cfs_hash_cond_del_locked, &arg, 1);
@@ -1523,16 +1502,17 @@ cfs_hash_cond_del(struct cfs_hash *hs, cfs_hash_cond_opt_cb_t func, void *data)
 EXPORT_SYMBOL(cfs_hash_cond_del);
 
 void
-cfs_hash_for_each(struct cfs_hash *hs,
-		  cfs_hash_for_each_cb_t func, void *data)
+cfs_hash_for_each(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
+		  void *data)
 {
 	cfs_hash_for_each_tight(hs, func, data, 0);
 }
 EXPORT_SYMBOL(cfs_hash_for_each);
 
 void
-cfs_hash_for_each_safe(struct cfs_hash *hs,
-		       cfs_hash_for_each_cb_t func, void *data) {
+cfs_hash_for_each_safe(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
+		       void *data)
+{
 	cfs_hash_for_each_tight(hs, func, data, 1);
 }
 EXPORT_SYMBOL(cfs_hash_for_each_safe);
@@ -1581,15 +1561,16 @@ EXPORT_SYMBOL(cfs_hash_size_get);
  */
 static int
 cfs_hash_for_each_relax(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
-			void *data) {
+			void *data)
+{
 	struct hlist_node *hnode;
 	struct hlist_node *tmp;
-	struct cfs_hash_bd     bd;
-	__u32	     version;
-	int	       count = 0;
-	int	       stop_on_change;
-	int	       rc;
-	int	       i;
+	struct cfs_hash_bd bd;
+	__u32 version;
+	int count = 0;
+	int stop_on_change;
+	int rc;
+	int i;
 
 	stop_on_change = cfs_hash_with_rehash_key(hs) ||
 			 !cfs_hash_with_no_itemref(hs) ||
@@ -1645,8 +1626,9 @@ cfs_hash_for_each_relax(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
 }
 
 int
-cfs_hash_for_each_nolock(struct cfs_hash *hs,
-			 cfs_hash_for_each_cb_t func, void *data) {
+cfs_hash_for_each_nolock(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
+			 void *data)
+{
 	if (cfs_hash_with_no_lock(hs) ||
 	    cfs_hash_with_rehash_key(hs) ||
 	    !cfs_hash_with_no_itemref(hs))
@@ -1677,9 +1659,10 @@ EXPORT_SYMBOL(cfs_hash_for_each_nolock);
  * the required locking is in place to prevent concurrent insertions.
  */
 int
-cfs_hash_for_each_empty(struct cfs_hash *hs,
-			cfs_hash_for_each_cb_t func, void *data) {
-	unsigned  i = 0;
+cfs_hash_for_each_empty(struct cfs_hash *hs, cfs_hash_for_each_cb_t func,
+			void *data)
+{
+	unsigned i = 0;
 
 	if (cfs_hash_with_no_lock(hs))
 		return -EOPNOTSUPP;
@@ -1703,9 +1686,9 @@ void
 cfs_hash_hlist_for_each(struct cfs_hash *hs, unsigned hindex,
 			cfs_hash_for_each_cb_t func, void *data)
 {
-	struct hlist_head   *hhead;
-	struct hlist_node   *hnode;
-	struct cfs_hash_bd       bd;
+	struct hlist_head *hhead;
+	struct hlist_node *hnode;
+	struct cfs_hash_bd bd;
 
 	cfs_hash_for_each_enter(hs);
 	cfs_hash_lock(hs, 0);
@@ -1721,7 +1704,7 @@ cfs_hash_hlist_for_each(struct cfs_hash *hs, unsigned hindex,
 			break;
 	}
 	cfs_hash_bd_unlock(hs, &bd, 0);
- out:
+out:
 	cfs_hash_unlock(hs, 0);
 	cfs_hash_for_each_exit(hs);
 }
@@ -1736,10 +1719,11 @@ EXPORT_SYMBOL(cfs_hash_hlist_for_each);
    */
 void
 cfs_hash_for_each_key(struct cfs_hash *hs, const void *key,
-		      cfs_hash_for_each_cb_t func, void *data) {
-	struct hlist_node   *hnode;
-	struct cfs_hash_bd       bds[2];
-	unsigned	    i;
+		      cfs_hash_for_each_cb_t func, void *data)
+{
+	struct hlist_node *hnode;
+	struct cfs_hash_bd bds[2];
+	unsigned int i;
 
 	cfs_hash_lock(hs, 0);
 
@@ -1777,7 +1761,7 @@ EXPORT_SYMBOL(cfs_hash_for_each_key);
 void
 cfs_hash_rehash_cancel_locked(struct cfs_hash *hs)
 {
-	int     i;
+	int i;
 
 	/* need hold cfs_hash_lock(hs, 1) */
 	LASSERT(cfs_hash_with_rehash(hs) &&
@@ -1794,14 +1778,13 @@ cfs_hash_rehash_cancel_locked(struct cfs_hash *hs)
 	for (i = 2; cfs_hash_is_rehashing(hs); i++) {
 		cfs_hash_unlock(hs, 1);
 		/* raise console warning while waiting too long */
-		CDEBUG(IS_PO2(i >> 3) ? D_WARNING : D_INFO,
+		CDEBUG(is_power_of_2(i >> 3) ? D_WARNING : D_INFO,
 		       "hash %s is still rehashing, rescheded %d\n",
 		       hs->hs_name, i - 1);
 		cond_resched();
 		cfs_hash_lock(hs, 1);
 	}
 }
-EXPORT_SYMBOL(cfs_hash_rehash_cancel_locked);
 
 void
 cfs_hash_rehash_cancel(struct cfs_hash *hs)
@@ -1810,12 +1793,11 @@ cfs_hash_rehash_cancel(struct cfs_hash *hs)
 	cfs_hash_rehash_cancel_locked(hs);
 	cfs_hash_unlock(hs, 1);
 }
-EXPORT_SYMBOL(cfs_hash_rehash_cancel);
 
 int
 cfs_hash_rehash(struct cfs_hash *hs, int do_rehash)
 {
-	int     rc;
+	int rc;
 
 	LASSERT(cfs_hash_with_rehash(hs) && !cfs_hash_with_no_lock(hs));
 
@@ -1840,17 +1822,16 @@ cfs_hash_rehash(struct cfs_hash *hs, int do_rehash)
 
 	return cfs_hash_rehash_worker(&hs->hs_rehash_wi);
 }
-EXPORT_SYMBOL(cfs_hash_rehash);
 
 static int
 cfs_hash_rehash_bd(struct cfs_hash *hs, struct cfs_hash_bd *old)
 {
-	struct cfs_hash_bd      new;
-	struct hlist_head  *hhead;
-	struct hlist_node  *hnode;
-	struct hlist_node  *pos;
-	void	      *key;
-	int		c = 0;
+	struct cfs_hash_bd new;
+	struct hlist_head *hhead;
+	struct hlist_node *hnode;
+	struct hlist_node *pos;
+	void *key;
+	int c = 0;
 
 	/* hold cfs_hash_lock(hs, 1), so don't need any bucket lock */
 	cfs_hash_bd_for_each_hlist(hs, old, hhead) {
@@ -1876,17 +1857,17 @@ cfs_hash_rehash_bd(struct cfs_hash *hs, struct cfs_hash_bd *old)
 static int
 cfs_hash_rehash_worker(cfs_workitem_t *wi)
 {
-	struct cfs_hash	 *hs = container_of(wi, struct cfs_hash, hs_rehash_wi);
+	struct cfs_hash *hs = container_of(wi, struct cfs_hash, hs_rehash_wi);
 	struct cfs_hash_bucket **bkts;
-	struct cfs_hash_bd       bd;
-	unsigned int	old_size;
-	unsigned int	new_size;
-	int		 bsize;
-	int		 count = 0;
-	int		 rc = 0;
-	int		 i;
+	struct cfs_hash_bd bd;
+	unsigned int old_size;
+	unsigned int new_size;
+	int bsize;
+	int count = 0;
+	int rc = 0;
+	int i;
 
-	LASSERT (hs != NULL && cfs_hash_with_rehash(hs));
+	LASSERT(hs != NULL && cfs_hash_with_rehash(hs));
 
 	cfs_hash_lock(hs, 0);
 	LASSERT(cfs_hash_is_rehashing(hs));
@@ -1958,7 +1939,7 @@ cfs_hash_rehash_worker(cfs_workitem_t *wi)
 	hs->hs_rehash_buckets = NULL;
 
 	hs->hs_cur_bits = hs->hs_rehash_bits;
- out:
+out:
 	hs->hs_rehash_bits = 0;
 	if (rc == -ESRCH) /* never be scheduled again */
 		cfs_wi_exit(cfs_sched_rehash, wi);
@@ -1986,9 +1967,9 @@ cfs_hash_rehash_worker(cfs_workitem_t *wi)
 void cfs_hash_rehash_key(struct cfs_hash *hs, const void *old_key,
 			 void *new_key, struct hlist_node *hnode)
 {
-	struct cfs_hash_bd	bds[3];
-	struct cfs_hash_bd	old_bds[2];
-	struct cfs_hash_bd	new_bd;
+	struct cfs_hash_bd bds[3];
+	struct cfs_hash_bd old_bds[2];
+	struct cfs_hash_bd new_bd;
 
 	LASSERT(!hlist_unhashed(hnode));
 
@@ -2014,7 +1995,7 @@ void cfs_hash_rehash_key(struct cfs_hash *hs, const void *old_key,
 	}
 	/* overwrite key inside locks, otherwise may screw up with
 	 * other operations, i.e: rehash */
-	cfs_hash_keycpy(hs, new_key, hnode);
+	cfs_hash_keycpy(hs, hnode, new_key);
 
 	cfs_hash_multi_bd_unlock(hs, bds, 3, 1);
 	cfs_hash_unlock(hs, 0);
@@ -2054,12 +2035,12 @@ cfs_hash_full_nbkt(struct cfs_hash *hs)
 
 void cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
 {
-	int		    dist[8] = { 0, };
-	int		    maxdep  = -1;
-	int		    maxdepb = -1;
-	int		    total   = 0;
-	int		    theta;
-	int		    i;
+	int dist[8] = { 0, };
+	int maxdep = -1;
+	int maxdepb = -1;
+	int total = 0;
+	int theta;
+	int i;
 
 	cfs_hash_lock(hs, 0);
 	theta = __cfs_hash_theta(hs);
@@ -2085,11 +2066,11 @@ void cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
 	 * If you hash function results in a non-uniform hash the will
 	 * be observable by outlier bucks in the distribution histogram.
 	 *
-	 * Uniform hash distribution:      128/128/0/0/0/0/0/0
-	 * Non-Uniform hash distribution:  128/125/0/0/0/0/2/1
+	 * Uniform hash distribution:		128/128/0/0/0/0/0/0
+	 * Non-Uniform hash distribution:	128/125/0/0/0/0/2/1
 	 */
 	for (i = 0; i < cfs_hash_full_nbkt(hs); i++) {
-		struct cfs_hash_bd  bd;
+		struct cfs_hash_bd bd;
 
 		bd.bd_bucket = cfs_hash_full_bkts(hs)[i];
 		cfs_hash_bd_lock(hs, &bd, 0);
diff --git a/drivers/staging/lustre/lustre/libcfs/kernel_user_comm.c b/drivers/staging/lustre/lustre/libcfs/kernel_user_comm.c
index ad661a33a211..d8230aec9a2b 100644
--- a/drivers/staging/lustre/lustre/libcfs/kernel_user_comm.c
+++ b/drivers/staging/lustre/lustre/libcfs/kernel_user_comm.c
@@ -110,7 +110,8 @@ static DECLARE_RWSEM(kg_sem);
  * @param uid identifier for this receiver
  * @param group group number
  */
-int libcfs_kkuc_group_add(struct file *filp, int uid, int group, __u32 data)
+int libcfs_kkuc_group_add(struct file *filp, int uid, unsigned int group,
+			  __u32 data)
 {
 	struct kkuc_reg *reg;
 
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_lock.c b/drivers/staging/lustre/lustre/libcfs/libcfs_lock.c
index 94bc00785000..15782d9e6aa9 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_lock.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_lock.c
@@ -21,7 +21,7 @@
  * GPL HEADER END
  */
 /* Copyright (c) 2010, Oracle and/or its affiliates. All rights reserved.
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_mem.c b/drivers/staging/lustre/lustre/libcfs/libcfs_mem.c
index f4e08daba4d9..27cf86106363 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_mem.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_mem.c
@@ -134,7 +134,6 @@ cfs_percpt_current(void *vars)
 
 	return arr->va_ptrs[cpt];
 }
-EXPORT_SYMBOL(cfs_percpt_current);
 
 void *
 cfs_percpt_index(void *vars, int idx)
@@ -146,7 +145,6 @@ cfs_percpt_index(void *vars, int idx)
 	LASSERT(idx >= 0 && idx < arr->va_count);
 	return arr->va_ptrs[idx];
 }
-EXPORT_SYMBOL(cfs_percpt_index);
 
 /*
  * free variable array, see more detail in cfs_array_alloc
diff --git a/drivers/staging/lustre/lustre/libcfs/libcfs_string.c b/drivers/staging/lustre/lustre/libcfs/libcfs_string.c
index d40be5396769..205a3ed435a8 100644
--- a/drivers/staging/lustre/lustre/libcfs/libcfs_string.c
+++ b/drivers/staging/lustre/lustre/libcfs/libcfs_string.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
index 209736454d06..e52afe35e7ea 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-cpu.c
@@ -22,7 +22,8 @@
  */
 /*
  * Copyright (c) 2010, Oracle and/or its affiliates. All rights reserved.
- * Copyright (c) 2012, Intel Corporation.
+ *
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -78,23 +79,6 @@ struct cfs_cpt_data {
 
 static struct cfs_cpt_data	cpt_data;
 
-static void cfs_cpu_core_siblings(int cpu, cpumask_t *mask)
-{
-	/* return cpumask of cores in the same socket */
-	cpumask_copy(mask, topology_core_cpumask(cpu));
-}
-
-/* return cpumask of HTs in the same core */
-static void cfs_cpu_ht_siblings(int cpu, cpumask_t *mask)
-{
-	cpumask_copy(mask, topology_sibling_cpumask(cpu));
-}
-
-static void cfs_node_to_cpumask(int node, cpumask_t *mask)
-{
-	cpumask_copy(mask, cpumask_of_node(node));
-}
-
 void
 cfs_cpt_table_free(struct cfs_cpt_table *cptab)
 {
@@ -426,7 +410,7 @@ cfs_cpt_set_node(struct cfs_cpt_table *cptab, int cpt, int node)
 	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
-	cfs_node_to_cpumask(node, mask);
+	cpumask_copy(mask, cpumask_of_node(node));
 
 	rc = cfs_cpt_set_cpumask(cptab, cpt, mask);
 
@@ -450,7 +434,7 @@ cfs_cpt_unset_node(struct cfs_cpt_table *cptab, int cpt, int node)
 	mutex_lock(&cpt_data.cpt_mutex);
 
 	mask = cpt_data.cpt_cpumask;
-	cfs_node_to_cpumask(node, mask);
+	cpumask_copy(mask, cpumask_of_node(node));
 
 	cfs_cpt_unset_cpumask(cptab, cpt, mask);
 
@@ -643,7 +627,7 @@ cfs_cpt_choose_ncpus(struct cfs_cpt_table *cptab, int cpt,
 		cpu = cpumask_first(node);
 
 		/* get cpumask for cores in the same socket */
-		cfs_cpu_core_siblings(cpu, socket);
+		cpumask_copy(socket, topology_core_cpumask(cpu));
 		cpumask_and(socket, socket, node);
 
 		LASSERT(!cpumask_empty(socket));
@@ -652,7 +636,7 @@ cfs_cpt_choose_ncpus(struct cfs_cpt_table *cptab, int cpt,
 			int     i;
 
 			/* get cpumask for hts in the same core */
-			cfs_cpu_ht_siblings(cpu, core);
+			cpumask_copy(core, topology_sibling_cpumask(cpu));
 			cpumask_and(core, core, node);
 
 			LASSERT(!cpumask_empty(core));
@@ -769,7 +753,7 @@ cfs_cpt_table_create(int ncpt)
 	}
 
 	for_each_online_node(i) {
-		cfs_node_to_cpumask(i, mask);
+		cpumask_copy(mask, cpumask_of_node(i));
 
 		while (!cpumask_empty(mask)) {
 			struct cfs_cpu_partition *part;
@@ -968,7 +952,8 @@ cfs_cpu_notify(struct notifier_block *self, unsigned long action, void *hcpu)
 
 		mutex_lock(&cpt_data.cpt_mutex);
 		/* if all HTs in a core are offline, it may break affinity */
-		cfs_cpu_ht_siblings(cpu, cpt_data.cpt_cpumask);
+		cpumask_copy(cpt_data.cpt_cpumask,
+			     topology_sibling_cpumask(cpu));
 		warn = cpumask_any_and(cpt_data.cpt_cpumask,
 				       cpu_online_mask) >= nr_cpu_ids;
 		mutex_unlock(&cpt_data.cpt_mutex);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-crypto-adler.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-crypto-adler.c
index 5d8d8b79fa1f..db0572733712 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-crypto-adler.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-crypto-adler.c
@@ -37,11 +37,6 @@
 #define CHKSUM_BLOCK_SIZE	1
 #define CHKSUM_DIGEST_SIZE	4
 
-static u32 __adler32(u32 cksum, unsigned char const *p, size_t len)
-{
-	return zlib_adler32(cksum, p, len);
-}
-
 static int adler32_cra_init(struct crypto_tfm *tfm)
 {
 	u32 *key = crypto_tfm_ctx(tfm);
@@ -79,14 +74,14 @@ static int adler32_update(struct shash_desc *desc, const u8 *data,
 {
 	u32 *cksump = shash_desc_ctx(desc);
 
-	*cksump = __adler32(*cksump, data, len);
+	*cksump = zlib_adler32(*cksump, data, len);
 	return 0;
 }
 
 static int __adler32_finup(u32 *cksump, const u8 *data, unsigned int len,
 			   u8 *out)
 {
-	*(u32 *)out = __adler32(*cksump, data, len);
+	*(u32 *)out = zlib_adler32(*cksump, data, len);
 	return 0;
 }
 
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
index c74c80915dca..68515d9130c1 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-curproc.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
index 8689ea757c99..59c7bf3cbc1f 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
@@ -195,6 +195,5 @@ void libcfs_unregister_panic_notifier(void)
 	atomic_notifier_chain_unregister(&panic_notifier_list, &libcfs_panic_notifier);
 }
 
-EXPORT_SYMBOL(libcfs_run_upcall);
 EXPORT_SYMBOL(libcfs_run_lbug_upcall);
 EXPORT_SYMBOL(lbug_with_loc);
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.h b/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.h
deleted file mode 100644
index ba84e4ffddd1..000000000000
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tracefile.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2009, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LIBCFS_LINUX_TRACEFILE_H__
-#define __LIBCFS_LINUX_TRACEFILE_H__
-
-/**
- * three types of trace_data in linux
- */
-typedef enum {
-	CFS_TCD_TYPE_PROC = 0,
-	CFS_TCD_TYPE_SOFTIRQ,
-	CFS_TCD_TYPE_IRQ,
-	CFS_TCD_TYPE_MAX
-} cfs_trace_buf_type_t;
-
-#endif
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index e7c2b26156b9..329d78ce272d 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -62,7 +62,7 @@
 #include "../../include/linux/lnet/lnet.h"
 #include "tracefile.h"
 
-MODULE_AUTHOR("Peter J. Braam <braam@clusterfs.com>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Portals v3.1");
 MODULE_LICENSE("GPL");
 
@@ -375,7 +375,7 @@ static int __proc_dobitmasks(void *data, int write,
 	} else {
 		rc = cfs_trace_copyin_string(tmpstr, tmpstrlen, buffer, nob);
 		if (rc < 0) {
-			cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
+			kfree(tmpstr);
 			return rc;
 		}
 
@@ -385,7 +385,7 @@ static int __proc_dobitmasks(void *data, int write,
 			*mask |= D_EMERG;
 	}
 
-	cfs_trace_free_string_buffer(tmpstr, tmpstrlen);
+	kfree(tmpstr);
 	return rc;
 }
 
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index f2d018d7823c..65c4f1ab0de8 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -199,7 +199,6 @@ static void cfs_tcd_shrink(struct cfs_trace_cpu_data *tcd)
 		       pgcount + 1, tcd->tcd_cur_pages);
 
 	INIT_LIST_HEAD(&pc.pc_pages);
-	spin_lock_init(&pc.pc_lock);
 
 	list_for_each_entry_safe(tage, tmp, &tcd->tcd_pages, linkage) {
 		if (pgcount-- == 0)
@@ -451,7 +450,7 @@ console:
 		cfs_print_to_console(&header, mask,
 				     string_buf, needed, file, msgdata->msg_fn);
 
-		cfs_trace_put_console_buffer(string_buf);
+		put_cpu();
 	}
 
 	if (cdls != NULL && cdls->cdls_count != 0) {
@@ -465,7 +464,7 @@ console:
 		cfs_print_to_console(&header, mask,
 				     string_buf, needed, file, msgdata->msg_fn);
 
-		cfs_trace_put_console_buffer(string_buf);
+		put_cpu();
 		cdls->cdls_count = 0;
 	}
 
@@ -522,7 +521,6 @@ static void collect_pages_on_all_cpus(struct page_collection *pc)
 	struct cfs_trace_cpu_data *tcd;
 	int i, cpu;
 
-	spin_lock(&pc->pc_lock);
 	for_each_possible_cpu(cpu) {
 		cfs_tcd_for_each_type_lock(tcd, i, cpu) {
 			list_splice_init(&tcd->tcd_pages, &pc->pc_pages);
@@ -534,7 +532,6 @@ static void collect_pages_on_all_cpus(struct page_collection *pc)
 			}
 		}
 	}
-	spin_unlock(&pc->pc_lock);
 }
 
 static void collect_pages(struct page_collection *pc)
@@ -555,7 +552,6 @@ static void put_pages_back_on_all_cpus(struct page_collection *pc)
 	struct cfs_trace_page *tmp;
 	int i, cpu;
 
-	spin_lock(&pc->pc_lock);
 	for_each_possible_cpu(cpu) {
 		cfs_tcd_for_each_type_lock(tcd, i, cpu) {
 			cur_head = tcd->tcd_pages.next;
@@ -573,7 +569,6 @@ static void put_pages_back_on_all_cpus(struct page_collection *pc)
 			}
 		}
 	}
-	spin_unlock(&pc->pc_lock);
 }
 
 static void put_pages_back(struct page_collection *pc)
@@ -592,7 +587,6 @@ static void put_pages_on_tcd_daemon_list(struct page_collection *pc,
 	struct cfs_trace_page *tage;
 	struct cfs_trace_page *tmp;
 
-	spin_lock(&pc->pc_lock);
 	list_for_each_entry_safe(tage, tmp, &pc->pc_pages, linkage) {
 
 		__LASSERT_TAGE_INVARIANT(tage);
@@ -616,7 +610,6 @@ static void put_pages_on_tcd_daemon_list(struct page_collection *pc,
 			tcd->tcd_cur_daemon_pages--;
 		}
 	}
-	spin_unlock(&pc->pc_lock);
 }
 
 static void put_pages_on_daemon_list(struct page_collection *pc)
@@ -636,8 +629,6 @@ void cfs_trace_debug_print(void)
 	struct cfs_trace_page *tage;
 	struct cfs_trace_page *tmp;
 
-	spin_lock_init(&pc.pc_lock);
-
 	pc.pc_want_daemon_pages = 1;
 	collect_pages(&pc);
 	list_for_each_entry_safe(tage, tmp, &pc.pc_pages, linkage) {
@@ -692,7 +683,6 @@ int cfs_tracefile_dump_all_pages(char *filename)
 		goto out;
 	}
 
-	spin_lock_init(&pc.pc_lock);
 	pc.pc_want_daemon_pages = 1;
 	collect_pages(&pc);
 	if (list_empty(&pc.pc_pages)) {
@@ -739,8 +729,6 @@ void cfs_trace_flush_pages(void)
 	struct cfs_trace_page *tage;
 	struct cfs_trace_page *tmp;
 
-	spin_lock_init(&pc.pc_lock);
-
 	pc.pc_want_daemon_pages = 1;
 	collect_pages(&pc);
 	list_for_each_entry_safe(tage, tmp, &pc.pc_pages, linkage) {
@@ -817,11 +805,6 @@ int cfs_trace_allocate_string_buffer(char **str, int nob)
 	return 0;
 }
 
-void cfs_trace_free_string_buffer(char *str, int nob)
-{
-	kfree(str);
-}
-
 int cfs_trace_dump_debug_buffer_usrstr(void __user *usr_str, int usr_str_nob)
 {
 	char	 *str;
@@ -842,7 +825,7 @@ int cfs_trace_dump_debug_buffer_usrstr(void __user *usr_str, int usr_str_nob)
 	}
 	rc = cfs_tracefile_dump_all_pages(str);
 out:
-	cfs_trace_free_string_buffer(str, usr_str_nob + 1);
+	kfree(str);
 	return rc;
 }
 
@@ -898,7 +881,7 @@ int cfs_trace_daemon_command_usrstr(void __user *usr_str, int usr_str_nob)
 	if (rc == 0)
 		rc = cfs_trace_daemon_command(str);
 
-	cfs_trace_free_string_buffer(str, usr_str_nob + 1);
+	kfree(str);
 	return rc;
 }
 
@@ -970,7 +953,6 @@ static int tracefiled(void *arg)
 	/* we're started late enough that we pick up init's fs context */
 	/* this is so broken in uml?  what on earth is going on? */
 
-	spin_lock_init(&pc.pc_lock);
 	complete(&tctl->tctl_start);
 
 	while (1) {
@@ -1170,7 +1152,6 @@ static void cfs_trace_cleanup(void)
 	struct page_collection pc;
 
 	INIT_LIST_HEAD(&pc.pc_pages);
-	spin_lock_init(&pc.pc_lock);
 
 	trace_cleanup_on_all_cpus();
 
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.h b/drivers/staging/lustre/lustre/libcfs/tracefile.h
index cb7a3963589f..7bf1471a54fb 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.h
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.h
@@ -39,7 +39,12 @@
 
 #include "../../include/linux/libcfs/libcfs.h"
 
-#include "linux/linux-tracefile.h"
+typedef enum {
+	CFS_TCD_TYPE_PROC = 0,
+	CFS_TCD_TYPE_SOFTIRQ,
+	CFS_TCD_TYPE_IRQ,
+	CFS_TCD_TYPE_MAX
+} cfs_trace_buf_type_t;
 
 /* trace file lock routines */
 
@@ -70,7 +75,6 @@ int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
 int cfs_trace_copyout_string(char __user *usr_buffer, int usr_buffer_nob,
 			     const char *knl_str, char *append);
 int cfs_trace_allocate_string_buffer(char **str, int nob);
-void cfs_trace_free_string_buffer(char *str, int nob);
 int cfs_trace_dump_debug_buffer_usrstr(void __user *usr_str, int usr_str_nob);
 int cfs_trace_daemon_command(char *str);
 int cfs_trace_daemon_command_usrstr(void __user *usr_str, int usr_str_nob);
@@ -196,14 +200,6 @@ extern union cfs_trace_data_union (*cfs_trace_data[TCD_MAX_TYPES])[NR_CPUS];
 struct page_collection {
 	struct list_head	pc_pages;
 	/*
-	 * spin-lock protecting ->pc_pages. It is taken by smp_call_function()
-	 * call-back functions. XXX nikita: Which is horrible: all processors
-	 * receive NMI at the same time only to be serialized by this
-	 * lock. Probably ->pc_pages should be replaced with an array of
-	 * NR_CPUS elements accessed locklessly.
-	 */
-	spinlock_t	pc_lock;
-	/*
 	 * if this flag is set, collect_pages() will spill both
 	 * ->tcd_daemon_pages and ->tcd_pages to the ->pc_pages. Otherwise,
 	 * only ->tcd_pages are spilled.
@@ -260,13 +256,6 @@ void cfs_print_to_console(struct ptldebug_header *hdr, int mask,
 int cfs_trace_lock_tcd(struct cfs_trace_cpu_data *tcd, int walking);
 void cfs_trace_unlock_tcd(struct cfs_trace_cpu_data *tcd, int walking);
 
-/**
- * trace_buf_type_t, trace_buf_idx_get() and trace_console_buffers[][]
- * are not public libcfs API; they should be defined in
- * platform-specific tracefile include files
- * (see, for example, linux-tracefile.h).
- */
-
 extern char *cfs_trace_console_buffers[NR_CPUS][CFS_TCD_TYPE_MAX];
 cfs_trace_buf_type_t cfs_trace_buf_idx_get(void);
 
@@ -279,12 +268,6 @@ cfs_trace_get_console_buffer(void)
 	return cfs_trace_console_buffers[i][j];
 }
 
-static inline void
-cfs_trace_put_console_buffer(char *buffer)
-{
-	put_cpu();
-}
-
 static inline struct cfs_trace_cpu_data *
 cfs_trace_get_tcd(void)
 {
diff --git a/drivers/staging/lustre/lustre/libcfs/workitem.c b/drivers/staging/lustre/lustre/libcfs/workitem.c
index e1143a566ac4..60bb88a00b41 100644
--- a/drivers/staging/lustre/lustre/libcfs/workitem.c
+++ b/drivers/staging/lustre/lustre/libcfs/workitem.c
@@ -86,32 +86,20 @@ static struct cfs_workitem_data {
 	int			wi_stopping;
 } cfs_wi_data;
 
-static inline void
-cfs_wi_sched_lock(struct cfs_wi_sched *sched)
-{
-	spin_lock(&sched->ws_lock);
-}
-
-static inline void
-cfs_wi_sched_unlock(struct cfs_wi_sched *sched)
-{
-	spin_unlock(&sched->ws_lock);
-}
-
 static inline int
 cfs_wi_sched_cansleep(struct cfs_wi_sched *sched)
 {
-	cfs_wi_sched_lock(sched);
+	spin_lock(&sched->ws_lock);
 	if (sched->ws_stopping) {
-		cfs_wi_sched_unlock(sched);
+		spin_unlock(&sched->ws_lock);
 		return 0;
 	}
 
 	if (!list_empty(&sched->ws_runq)) {
-		cfs_wi_sched_unlock(sched);
+		spin_unlock(&sched->ws_lock);
 		return 0;
 	}
-	cfs_wi_sched_unlock(sched);
+	spin_unlock(&sched->ws_lock);
 	return 1;
 }
 
@@ -125,7 +113,7 @@ cfs_wi_exit(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 	LASSERT(!in_interrupt()); /* because we use plain spinlock */
 	LASSERT(!sched->ws_stopping);
 
-	cfs_wi_sched_lock(sched);
+	spin_lock(&sched->ws_lock);
 
 	LASSERT(wi->wi_running);
 	if (wi->wi_scheduled) { /* cancel pending schedules */
@@ -139,7 +127,7 @@ cfs_wi_exit(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 	LASSERT(list_empty(&wi->wi_list));
 
 	wi->wi_scheduled = 1; /* LBUG future schedule attempts */
-	cfs_wi_sched_unlock(sched);
+	spin_unlock(&sched->ws_lock);
 
 	return;
 }
@@ -161,7 +149,7 @@ cfs_wi_deschedule(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 	 * means the workitem will not be scheduled and will not have
 	 * any race with wi_action.
 	 */
-	cfs_wi_sched_lock(sched);
+	spin_lock(&sched->ws_lock);
 
 	rc = !(wi->wi_running);
 
@@ -177,7 +165,7 @@ cfs_wi_deschedule(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 
 	LASSERT (list_empty(&wi->wi_list));
 
-	cfs_wi_sched_unlock(sched);
+	spin_unlock(&sched->ws_lock);
 	return rc;
 }
 EXPORT_SYMBOL(cfs_wi_deschedule);
@@ -195,7 +183,7 @@ cfs_wi_schedule(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 	LASSERT(!in_interrupt()); /* because we use plain spinlock */
 	LASSERT(!sched->ws_stopping);
 
-	cfs_wi_sched_lock(sched);
+	spin_lock(&sched->ws_lock);
 
 	if (!wi->wi_scheduled) {
 		LASSERT (list_empty(&wi->wi_list));
@@ -211,7 +199,7 @@ cfs_wi_schedule(struct cfs_wi_sched *sched, cfs_workitem_t *wi)
 	}
 
 	LASSERT (!list_empty(&wi->wi_list));
-	cfs_wi_sched_unlock(sched);
+	spin_unlock(&sched->ws_lock);
 	return;
 }
 EXPORT_SYMBOL(cfs_wi_schedule);
@@ -225,7 +213,9 @@ cfs_wi_scheduler (void *arg)
 
 	/* CPT affinity scheduler? */
 	if (sched->ws_cptab != NULL)
-		cfs_cpt_bind(sched->ws_cptab, sched->ws_cpt);
+		if (cfs_cpt_bind(sched->ws_cptab, sched->ws_cpt) != 0)
+			CWARN("Failed to bind %s on CPT %d\n",
+			      sched->ws_name, sched->ws_cpt);
 
 	spin_lock(&cfs_wi_data.wi_glock);
 
@@ -235,7 +225,7 @@ cfs_wi_scheduler (void *arg)
 
 	spin_unlock(&cfs_wi_data.wi_glock);
 
-	cfs_wi_sched_lock(sched);
+	spin_lock(&sched->ws_lock);
 
 	while (!sched->ws_stopping) {
 		int	     nloops = 0;
@@ -256,12 +246,12 @@ cfs_wi_scheduler (void *arg)
 			wi->wi_running   = 1;
 			wi->wi_scheduled = 0;
 
-			cfs_wi_sched_unlock(sched);
+			spin_unlock(&sched->ws_lock);
 			nloops++;
 
 			rc = (*wi->wi_action) (wi);
 
-			cfs_wi_sched_lock(sched);
+			spin_lock(&sched->ws_lock);
 			if (rc != 0) /* WI should be dead, even be freed! */
 				continue;
 
@@ -276,21 +266,21 @@ cfs_wi_scheduler (void *arg)
 		}
 
 		if (!list_empty(&sched->ws_runq)) {
-			cfs_wi_sched_unlock(sched);
+			spin_unlock(&sched->ws_lock);
 			/* don't sleep because some workitems still
 			 * expect me to come back soon */
 			cond_resched();
-			cfs_wi_sched_lock(sched);
+			spin_lock(&sched->ws_lock);
 			continue;
 		}
 
-		cfs_wi_sched_unlock(sched);
+		spin_unlock(&sched->ws_lock);
 		rc = wait_event_interruptible_exclusive(sched->ws_waitq,
 						!cfs_wi_sched_cansleep(sched));
-		cfs_wi_sched_lock(sched);
+		spin_lock(&sched->ws_lock);
 	}
 
-	cfs_wi_sched_unlock(sched);
+	spin_unlock(&sched->ws_lock);
 
 	spin_lock(&cfs_wi_data.wi_glock);
 	sched->ws_nthreads--;
@@ -325,7 +315,7 @@ cfs_wi_sched_destroy(struct cfs_wi_sched *sched)
 
 	spin_lock(&cfs_wi_data.wi_glock);
 	while (sched->ws_nthreads > 0) {
-		CDEBUG(IS_PO2(++i) ? D_WARNING : D_NET,
+		CDEBUG(is_power_of_2(++i) ? D_WARNING : D_NET,
 		       "waiting for %d threads of WI sched[%s] to terminate\n",
 		       sched->ws_nthreads, sched->ws_name);
 
@@ -360,8 +350,8 @@ cfs_wi_sched_create(char *name, struct cfs_cpt_table *cptab,
 	if (sched == NULL)
 		return -ENOMEM;
 
-	strncpy(sched->ws_name, name, CFS_WS_NAME_LEN);
-	sched->ws_name[CFS_WS_NAME_LEN - 1] = '\0';
+	strlcpy(sched->ws_name, name, CFS_WS_NAME_LEN);
+
 	sched->ws_cptab = cptab;
 	sched->ws_cpt = cpt;
 
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 80cba0448499..3d6745e63fe3 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index 5c9502b5b358..8982f7d1b374 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -239,12 +239,6 @@ static int ll_dir_filler(void *_hash, struct page *page0)
 	return rc;
 }
 
-static void ll_check_page(struct inode *dir, struct page *page)
-{
-	/* XXX: check page format later */
-	SetPageChecked(page);
-}
-
 void ll_release_page(struct page *page, int remove)
 {
 	kunmap(page);
@@ -432,7 +426,8 @@ struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 		goto fail;
 	}
 	if (!PageChecked(page))
-		ll_check_page(dir, page);
+		/* XXX: check page format later */
+		SetPageChecked(page);
 	if (PageError(page)) {
 		CERROR("page error: "DFID" at %llu: rc %d\n",
 		       PFID(ll_inode2fid(dir)), hash, -5);
@@ -641,7 +636,7 @@ static int ll_send_mgc_param(struct obd_export *mgc, char *string)
 	if (!msp)
 		return -ENOMEM;
 
-	strncpy(msp->mgs_param, string, MGS_PARAM_MAXLEN);
+	strlcpy(msp->mgs_param, string, sizeof(msp->mgs_param));
 	rc = obd_set_info_async(NULL, mgc, sizeof(KEY_SET_INFO), KEY_SET_INFO,
 				sizeof(struct mgs_send_param), msp, NULL);
 	if (rc)
@@ -1863,7 +1858,7 @@ static loff_t ll_dir_seek(struct file *file, loff_t offset, int origin)
 	int api32 = ll_need_32bit_api(sbi);
 	loff_t ret = -EINVAL;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	switch (origin) {
 	case SEEK_SET:
 		break;
@@ -1901,7 +1896,7 @@ static loff_t ll_dir_seek(struct file *file, loff_t offset, int origin)
 	goto out;
 
 out:
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 	return ret;
 }
 
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index 02f27593013e..39e2ffd5f97f 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -2082,17 +2082,17 @@ putgl:
 	/* update time if requested */
 	rc = 0;
 	if (llss->ia2.ia_valid != 0) {
-		mutex_lock(&llss->inode1->i_mutex);
+		inode_lock(llss->inode1);
 		rc = ll_setattr(file1->f_path.dentry, &llss->ia2);
-		mutex_unlock(&llss->inode1->i_mutex);
+		inode_unlock(llss->inode1);
 	}
 
 	if (llss->ia1.ia_valid != 0) {
 		int rc1;
 
-		mutex_lock(&llss->inode2->i_mutex);
+		inode_lock(llss->inode2);
 		rc1 = ll_setattr(file2->f_path.dentry, &llss->ia1);
-		mutex_unlock(&llss->inode2->i_mutex);
+		inode_unlock(llss->inode2);
 		if (rc == 0)
 			rc = rc1;
 	}
@@ -2179,13 +2179,13 @@ static int ll_hsm_import(struct inode *inode, struct file *file,
 			 ATTR_MTIME | ATTR_MTIME_SET |
 			 ATTR_ATIME | ATTR_ATIME_SET;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 
 	rc = ll_setattr_raw(file->f_path.dentry, attr, true);
 	if (rc == -ENODATA)
 		rc = 0;
 
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 
 	kfree(attr);
 free_hss:
@@ -2609,7 +2609,7 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_FSYNC, 1);
 
 	rc = filemap_write_and_wait_range(inode->i_mapping, start, end);
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 
 	/* catch async errors that were recorded back when async writeback
 	 * failed for pages in this mapping. */
@@ -2641,7 +2641,7 @@ int ll_fsync(struct file *file, loff_t start, loff_t end, int datasync)
 			fd->fd_write_failed = false;
 	}
 
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 	return rc;
 }
 
@@ -3139,7 +3139,7 @@ struct file_operations ll_file_operations_noflock = {
 	.lock	   = ll_file_noflock
 };
 
-struct inode_operations ll_file_inode_operations = {
+const struct inode_operations ll_file_inode_operations = {
 	.setattr	= ll_setattr,
 	.getattr	= ll_getattr,
 	.permission	= ll_inode_permission,
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 9096d311e45d..845e992ca5fc 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -631,8 +631,6 @@ struct ll_file_data {
 
 struct lov_stripe_md;
 
-extern spinlock_t inode_lock;
-
 extern struct dentry *llite_root;
 extern struct kset *llite_kset;
 
@@ -705,7 +703,7 @@ extern const struct address_space_operations ll_aops;
 extern struct file_operations ll_file_operations;
 extern struct file_operations ll_file_operations_flock;
 extern struct file_operations ll_file_operations_noflock;
-extern struct inode_operations ll_file_inode_operations;
+extern const struct inode_operations ll_file_inode_operations;
 int ll_have_md_lock(struct inode *inode, __u64 *bits,
 		    ldlm_mode_t l_req_mode);
 ldlm_mode_t ll_take_md_lock(struct inode *inode, __u64 bits,
@@ -805,7 +803,7 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 				      const struct lu_fid *fid);
 
 /* llite/symlink.c */
-extern struct inode_operations ll_fast_symlink_inode_operations;
+extern const struct inode_operations ll_fast_symlink_inode_operations;
 
 /* llite/llite_close.c */
 struct ll_close_queue {
@@ -1285,16 +1283,6 @@ static inline struct ll_file_data *cl_iattr2fd(struct inode *inode,
 	return LUSTRE_FPRIVATE(attr->ia_file);
 }
 
-static inline void cl_isize_lock(struct inode *inode)
-{
-	ll_inode_size_lock(inode);
-}
-
-static inline void cl_isize_unlock(struct inode *inode)
-{
-	ll_inode_size_unlock(inode);
-}
-
 static inline void cl_isize_write_nolock(struct inode *inode, loff_t kms)
 {
 	LASSERT(mutex_is_locked(&ll_i2info(inode)->lli_size_mutex));
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 4a8c759fef42..b2fc5b3786ee 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -1277,7 +1277,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		return -ENOMEM;
 
 	if (!S_ISDIR(inode->i_mode))
-		mutex_unlock(&inode->i_mutex);
+		inode_unlock(inode);
 
 	memcpy(&op_data->op_attr, attr, sizeof(*attr));
 
@@ -1358,7 +1358,7 @@ out:
 	ll_finish_md_op_data(op_data);
 
 	if (!S_ISDIR(inode->i_mode)) {
-		mutex_lock(&inode->i_mutex);
+		inode_lock(inode);
 		if ((attr->ia_valid & ATTR_SIZE) && !hsm_import)
 			inode_dio_wait(inode);
 	}
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 7df978371c9a..bbae95c9feed 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index e578a1130ad1..18aab25f9cd9 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -245,9 +245,9 @@ static int ll_get_name(struct dentry *dentry, char *name,
 		goto out;
 	}
 
-	mutex_lock(&dir->i_mutex);
+	inode_lock(dir);
 	rc = ll_dir_read(dir, &lgd.ctx);
-	mutex_unlock(&dir->i_mutex);
+	inode_unlock(dir);
 	if (!rc && !lgd.lgd_found)
 		rc = -ENOENT;
 out:
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index fed50d538a41..871924b3f2e7 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -257,9 +257,9 @@ static int do_bio_lustrebacked(struct lloop_device *lo, struct bio *head)
 	 *    be asked to write less pages once, this purely depends on
 	 *    implementation. Anyway, we should be careful to avoid deadlocking.
 	 */
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	bytes = ll_direct_rw_pages(env, io, rw, inode, pvec);
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 	cl_io_fini(env, io);
 	return (bytes == pvec->ldp_size) ? 0 : (int)bytes;
 }
@@ -877,6 +877,6 @@ module_exit(lloop_exit);
 
 module_param(max_loop, int, 0444);
 MODULE_PARM_DESC(max_loop, "maximum of lloop_device");
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre virtual block device");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index 190fc44114e1..f134ad9d23f0 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 2ca22001a534..da5f443a0768 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -126,9 +126,7 @@ struct inode *ll_iget(struct super_block *sb, ino_t hash,
 				rc = cl_file_inode_init(inode, md);
 			}
 			if (rc != 0) {
-				make_bad_inode(inode);
-				unlock_new_inode(inode);
-				iput(inode);
+				iget_failed(inode);
 				inode = ERR_PTR(rc);
 			} else
 				unlock_new_inode(inode);
@@ -556,7 +554,6 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 		retval = NULL;
 	else
 		retval = dentry;
-	goto out;
  out:
 	if (req)
 		ptlrpc_req_finished(req);
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index f79193fa2fb7..f355474967d6 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -115,8 +115,8 @@ static struct ll_cl_context *ll_cl_init(struct file *file,
 		struct inode *inode = vmpage->mapping->host;
 		loff_t pos;
 
-		if (mutex_trylock(&inode->i_mutex)) {
-			mutex_unlock(&(inode)->i_mutex);
+		if (inode_trylock(inode)) {
+			inode_unlock((inode));
 
 			/* this is too bad. Someone is trying to write the
 			 * page w/o holding inode mutex. This means we can
@@ -880,14 +880,6 @@ static void ras_update_stride_detector(struct ll_readahead_state *ras,
 	return;
 }
 
-static unsigned long
-stride_page_count(struct ll_readahead_state *ras, unsigned long len)
-{
-	return stride_pg_count(ras->ras_stride_offset, ras->ras_stride_length,
-			       ras->ras_stride_pages, ras->ras_stride_offset,
-			       len);
-}
-
 /* Stride Read-ahead window will be increased inc_len according to
  * stride I/O pattern */
 static void ras_stride_increase_window(struct ll_readahead_state *ras,
@@ -921,7 +913,9 @@ static void ras_stride_increase_window(struct ll_readahead_state *ras,
 
 	window_len += step * ras->ras_stride_length + left;
 
-	if (stride_page_count(ras, window_len) <= ra->ra_max_pages_per_file)
+	if (stride_pg_count(ras->ras_stride_offset, ras->ras_stride_length,
+			    ras->ras_stride_pages, ras->ras_stride_offset,
+			    window_len) <= ra->ra_max_pages_per_file)
 		ras->ras_window_len = window_len;
 
 	RAS_CDEBUG(ras);
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 3da4c01e2159..711fda93a58d 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -298,7 +298,10 @@ ssize_t ll_direct_rw_pages(const struct lu_env *env, struct cl_io *io,
 		}
 
 		if (likely(do_io)) {
-			cl_2queue_add(queue, clp);
+			/*
+			 * Add a page to the incoming page list of 2-queue.
+			 */
+			cl_page_list_add(&queue->c2_qin, clp);
 
 			/*
 			 * Set page clip to tell transfer formation engine
@@ -400,7 +403,7 @@ static ssize_t ll_direct_IO_26(struct kiocb *iocb, struct iov_iter *iter,
 	 * 1. Need inode mutex to operate transient pages.
 	 */
 	if (iov_iter_rw(iter) == READ)
-		mutex_lock(&inode->i_mutex);
+		inode_lock(inode);
 
 	LASSERT(obj->cob_transient_pages == 0);
 	while (iov_iter_count(iter)) {
@@ -451,7 +454,7 @@ static ssize_t ll_direct_IO_26(struct kiocb *iocb, struct iov_iter *iter,
 out:
 	LASSERT(obj->cob_transient_pages == 0);
 	if (iov_iter_rw(iter) == READ)
-		mutex_unlock(&inode->i_mutex);
+		inode_unlock(inode);
 
 	if (tot_bytes > 0) {
 		if (iov_iter_rw(iter) == WRITE) {
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index 18f5f2b7e902..88ffd8e3abdb 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -87,11 +87,6 @@ struct ll_sa_entry {
 static unsigned int sai_generation;
 static DEFINE_SPINLOCK(sai_generation_lock);
 
-static inline int ll_sa_entry_unhashed(struct ll_sa_entry *entry)
-{
-	return list_empty(&entry->se_hash);
-}
-
 /*
  * The entry only can be released by the caller, it is necessary to hold lock.
  */
@@ -138,20 +133,6 @@ static inline int agl_should_run(struct ll_statahead_info *sai,
 	return (inode != NULL && S_ISREG(inode->i_mode) && sai->sai_agl_valid);
 }
 
-static inline struct ll_sa_entry *
-sa_first_received_entry(struct ll_statahead_info *sai)
-{
-	return list_entry(sai->sai_entries_received.next,
-			      struct ll_sa_entry, se_list);
-}
-
-static inline struct ll_inode_info *
-agl_first_entry(struct ll_statahead_info *sai)
-{
-	return list_entry(sai->sai_entries_agl.next,
-			      struct ll_inode_info, lli_agl_list);
-}
-
 static inline int sa_sent_full(struct ll_statahead_info *sai)
 {
 	return atomic_read(&sai->sai_cache_count) >= sai->sai_max;
@@ -331,7 +312,7 @@ static void ll_sa_entry_put(struct ll_statahead_info *sai,
 
 		LASSERT(list_empty(&entry->se_link));
 		LASSERT(list_empty(&entry->se_list));
-		LASSERT(ll_sa_entry_unhashed(entry));
+		LASSERT(list_empty(&entry->se_hash));
 
 		ll_sa_entry_cleanup(sai, entry);
 		iput(entry->se_inode);
@@ -346,7 +327,7 @@ do_sa_entry_fini(struct ll_statahead_info *sai, struct ll_sa_entry *entry)
 {
 	struct ll_inode_info *lli = ll_i2info(sai->sai_inode);
 
-	LASSERT(!ll_sa_entry_unhashed(entry));
+	LASSERT(!list_empty(&entry->se_hash));
 	LASSERT(!list_empty(&entry->se_link));
 
 	ll_sa_entry_unhash(sai, entry);
@@ -447,7 +428,7 @@ static void ll_agl_add(struct ll_statahead_info *sai,
 
 		igrab(inode);
 		spin_lock(&parent->lli_agl_lock);
-		if (agl_list_empty(sai))
+		if (list_empty(&sai->sai_entries_agl))
 			added = 1;
 		list_add_tail(&child->lli_agl_list, &sai->sai_entries_agl);
 		spin_unlock(&parent->lli_agl_lock);
@@ -537,11 +518,11 @@ static void ll_sai_put(struct ll_statahead_info *sai)
 			do_sa_entry_fini(sai, entry);
 
 		LASSERT(list_empty(&sai->sai_entries));
-		LASSERT(sa_received_empty(sai));
+		LASSERT(list_empty(&sai->sai_entries_received));
 		LASSERT(list_empty(&sai->sai_entries_stated));
 
 		LASSERT(atomic_read(&sai->sai_cache_count) == 0);
-		LASSERT(agl_list_empty(sai));
+		LASSERT(list_empty(&sai->sai_entries_agl));
 
 		iput(inode);
 		kfree(sai);
@@ -621,11 +602,12 @@ static void ll_post_statahead(struct ll_statahead_info *sai)
 	int		     rc    = 0;
 
 	spin_lock(&lli->lli_sa_lock);
-	if (unlikely(sa_received_empty(sai))) {
+	if (unlikely(list_empty(&sai->sai_entries_received))) {
 		spin_unlock(&lli->lli_sa_lock);
 		return;
 	}
-	entry = sa_first_received_entry(sai);
+	entry = list_entry(sai->sai_entries_received.next,
+			   struct ll_sa_entry, se_list);
 	atomic_inc(&entry->se_refcount);
 	list_del_init(&entry->se_list);
 	spin_unlock(&lli->lli_sa_lock);
@@ -756,7 +738,7 @@ static int ll_statahead_interpret(struct ptlrpc_request *req,
 			 * for readpage and other tries to enqueue lock on child
 			 * with parent's lock held, for example: unlink. */
 			entry->se_handle = handle;
-			wakeup = sa_received_empty(sai);
+			wakeup = list_empty(&sai->sai_entries_received);
 			list_add_tail(&entry->se_list,
 					  &sai->sai_entries_received);
 		}
@@ -973,7 +955,7 @@ static int ll_agl_thread(void *arg)
 
 	while (1) {
 		l_wait_event(thread->t_ctl_waitq,
-			     !agl_list_empty(sai) ||
+			     !list_empty(&sai->sai_entries_agl) ||
 			     !thread_is_running(thread),
 			     &lwi);
 
@@ -983,8 +965,9 @@ static int ll_agl_thread(void *arg)
 		spin_lock(&plli->lli_agl_lock);
 		/* The statahead thread maybe help to process AGL entries,
 		 * so check whether list empty again. */
-		if (!agl_list_empty(sai)) {
-			clli = agl_first_entry(sai);
+		if (!list_empty(&sai->sai_entries_agl)) {
+			clli = list_entry(sai->sai_entries_agl.next,
+					  struct ll_inode_info, lli_agl_list);
 			list_del_init(&clli->lli_agl_list);
 			spin_unlock(&plli->lli_agl_lock);
 			ll_agl_trigger(&clli->lli_vfs_inode, sai);
@@ -995,8 +978,9 @@ static int ll_agl_thread(void *arg)
 
 	spin_lock(&plli->lli_agl_lock);
 	sai->sai_agl_valid = 0;
-	while (!agl_list_empty(sai)) {
-		clli = agl_first_entry(sai);
+	while (!list_empty(&sai->sai_entries_agl)) {
+		clli = list_entry(sai->sai_entries_agl.next,
+				  struct ll_inode_info, lli_agl_list);
 		list_del_init(&clli->lli_agl_list);
 		spin_unlock(&plli->lli_agl_lock);
 		clli->lli_agl_index = 0;
@@ -1136,13 +1120,13 @@ static int ll_statahead_thread(void *arg)
 keep_it:
 			l_wait_event(thread->t_ctl_waitq,
 				     !sa_sent_full(sai) ||
-				     !sa_received_empty(sai) ||
-				     !agl_list_empty(sai) ||
+				     !list_empty(&sai->sai_entries_received) ||
+				     !list_empty(&sai->sai_entries_agl) ||
 				     !thread_is_running(thread),
 				     &lwi);
 
 interpret_it:
-			while (!sa_received_empty(sai))
+			while (!list_empty(&sai->sai_entries_received))
 				ll_post_statahead(sai);
 
 			if (unlikely(!thread_is_running(thread))) {
@@ -1156,14 +1140,15 @@ interpret_it:
 			 * to process the AGL entries. */
 			if (sa_sent_full(sai)) {
 				spin_lock(&plli->lli_agl_lock);
-				while (!agl_list_empty(sai)) {
-					clli = agl_first_entry(sai);
+				while (!list_empty(&sai->sai_entries_agl)) {
+					clli = list_entry(sai->sai_entries_agl.next,
+							  struct ll_inode_info, lli_agl_list);
 					list_del_init(&clli->lli_agl_list);
 					spin_unlock(&plli->lli_agl_lock);
 					ll_agl_trigger(&clli->lli_vfs_inode,
 						       sai);
 
-					if (!sa_received_empty(sai))
+					if (!list_empty(&sai->sai_entries_received))
 						goto interpret_it;
 
 					if (unlikely(
@@ -1194,12 +1179,12 @@ do_it:
 			ll_release_page(page, 0);
 			while (1) {
 				l_wait_event(thread->t_ctl_waitq,
-					     !sa_received_empty(sai) ||
+					     !list_empty(&sai->sai_entries_received) ||
 					     sai->sai_sent == sai->sai_replied ||
 					     !thread_is_running(thread),
 					     &lwi);
 
-				while (!sa_received_empty(sai))
+				while (!list_empty(&sai->sai_entries_received))
 					ll_post_statahead(sai);
 
 				if (unlikely(!thread_is_running(thread))) {
@@ -1208,14 +1193,15 @@ do_it:
 				}
 
 				if (sai->sai_sent == sai->sai_replied &&
-				    sa_received_empty(sai))
+				    list_empty(&sai->sai_entries_received))
 					break;
 			}
 
 			spin_lock(&plli->lli_agl_lock);
-			while (!agl_list_empty(sai) &&
+			while (!list_empty(&sai->sai_entries_agl) &&
 			       thread_is_running(thread)) {
-				clli = agl_first_entry(sai);
+				clli = list_entry(sai->sai_entries_agl.next,
+						  struct ll_inode_info, lli_agl_list);
 				list_del_init(&clli->lli_agl_list);
 				spin_unlock(&plli->lli_agl_lock);
 				ll_agl_trigger(&clli->lli_vfs_inode, sai);
@@ -1260,12 +1246,12 @@ out:
 	}
 	ll_dir_chain_fini(&chain);
 	spin_lock(&plli->lli_sa_lock);
-	if (!sa_received_empty(sai)) {
+	if (!list_empty(&sai->sai_entries_received)) {
 		thread_set_flags(thread, SVC_STOPPING);
 		spin_unlock(&plli->lli_sa_lock);
 
 		/* To release the resources held by received entries. */
-		while (!sa_received_empty(sai))
+		while (!list_empty(&sai->sai_entries_received))
 			ll_post_statahead(sai);
 
 		spin_lock(&plli->lli_sa_lock);
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index 013136860664..86c371ef71ea 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -106,7 +106,8 @@ static int __init init_lustre_lite(void)
 	rc = -ENOMEM;
 	ll_inode_cachep = kmem_cache_create("lustre_inode_cache",
 					    sizeof(struct ll_inode_info),
-					    0, SLAB_HWCACHE_ALIGN, NULL);
+					    0, SLAB_HWCACHE_ALIGN|SLAB_ACCOUNT,
+					    NULL);
 	if (ll_inode_cachep == NULL)
 		goto out_cache;
 
@@ -205,7 +206,7 @@ static void __exit exit_lustre_lite(void)
 	kmem_cache_destroy(ll_file_data_slab);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Lite Client File System");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 69b203651905..2610348f6c72 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -118,12 +118,20 @@ failed:
 	return rc;
 }
 
-static const char *ll_follow_link(struct dentry *dentry, void **cookie)
+static void ll_put_link(void *p)
+{
+	ptlrpc_req_finished(p);
+}
+
+static const char *ll_get_link(struct dentry *dentry,
+			       struct inode *inode,
+			       struct delayed_call *done)
 {
-	struct inode *inode = d_inode(dentry);
 	struct ptlrpc_request *request = NULL;
 	int rc;
 	char *symname = NULL;
+	if (!dentry)
+		return ERR_PTR(-ECHILD);
 
 	CDEBUG(D_VFSTRACE, "VFS Op\n");
 	ll_inode_size_lock(inode);
@@ -135,22 +143,16 @@ static const char *ll_follow_link(struct dentry *dentry, void **cookie)
 	}
 
 	/* symname may contain a pointer to the request message buffer,
-	 * we delay request releasing until ll_put_link then.
+	 * we delay request releasing then.
 	 */
-	*cookie = request;
+	set_delayed_call(done, ll_put_link, request);
 	return symname;
 }
 
-static void ll_put_link(struct inode *unused, void *cookie)
-{
-	ptlrpc_req_finished(cookie);
-}
-
-struct inode_operations ll_fast_symlink_inode_operations = {
+const struct inode_operations ll_fast_symlink_inode_operations = {
 	.readlink	= generic_readlink,
 	.setattr	= ll_setattr,
-	.follow_link	= ll_follow_link,
-	.put_link	= ll_put_link,
+	.get_link	= ll_get_link,
 	.getattr	= ll_getattr,
 	.permission	= ll_inode_permission,
 	.setxattr	= ll_setxattr,
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index d16d6cfce81a..fdca4ec0555d 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/vvp_internal.h b/drivers/staging/lustre/lustre/llite/vvp_internal.h
index b5a6661d47d5..2e39533a45f8 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_internal.h
+++ b/drivers/staging/lustre/lustre/llite/vvp_internal.h
@@ -26,6 +26,8 @@
 /*
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2013, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index 37773c181729..0920ac6b3003 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -439,7 +439,7 @@ static int vvp_io_setattr_start(const struct lu_env *env,
 	struct inode	*inode = ccc_object_inode(io->ci_obj);
 	int result = 0;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	if (cl_io_is_trunc(io))
 		result = vvp_io_setattr_trunc(env, ios, inode,
 					io->u.ci_setattr.sa_attr.lvb_size);
@@ -459,7 +459,7 @@ static void vvp_io_setattr_end(const struct lu_env *env,
 		 * because osc has already notified to destroy osc_extents. */
 		vvp_do_vmtruncate(inode, io->u.ci_setattr.sa_attr.lvb_size);
 
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 }
 
 static void vvp_io_setattr_fini(const struct lu_env *env,
@@ -849,7 +849,7 @@ static int vvp_io_read_page(const struct lu_env *env,
 	 * Add page into the queue even when it is marked uptodate above.
 	 * this will unlock it automatically as part of cl_page_list_disown().
 	 */
-	cl_2queue_add(queue, page);
+	cl_page_list_add(&queue->c2_qin, page);
 	if (sbi->ll_ra_info.ra_max_pages_per_file &&
 	    sbi->ll_ra_info.ra_max_pages)
 		ll_readahead(env, io, ras,
diff --git a/drivers/staging/lustre/lustre/llite/vvp_lock.c b/drivers/staging/lustre/lustre/llite/vvp_lock.c
index f7b1144aadee..ff0948043c7a 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_lock.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_lock.c
@@ -26,6 +26,8 @@
 /*
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2014, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/vvp_object.c b/drivers/staging/lustre/lustre/llite/vvp_object.c
index e13afb7e8dca..c82714ea898e 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_object.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/llite/vvp_page.c b/drivers/staging/lustre/lustre/llite/vvp_page.c
index 92f60c350f35..a133475a7c74 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_page.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_page.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -428,7 +428,7 @@ static void vvp_transient_page_verify(const struct cl_page *page)
 {
 	struct inode *inode = ccc_object_inode(page->cp_obj);
 
-	LASSERT(!mutex_trylock(&inode->i_mutex));
+	LASSERT(!inode_trylock(inode));
 }
 
 static int vvp_transient_page_own(const struct lu_env *env,
@@ -480,9 +480,9 @@ static int vvp_transient_page_is_vmlocked(const struct lu_env *env,
 	struct inode    *inode = ccc_object_inode(slice->cpl_obj);
 	int	locked;
 
-	locked = !mutex_trylock(&inode->i_mutex);
+	locked = !inode_trylock(inode);
 	if (!locked)
-		mutex_unlock(&inode->i_mutex);
+		inode_unlock(inode);
 	return locked ? -EBUSY : -ENODATA;
 }
 
@@ -502,7 +502,7 @@ static void vvp_transient_page_fini(const struct lu_env *env,
 	struct ccc_object *clobj = cl2ccc(clp->cp_obj);
 
 	vvp_page_fini_common(cp);
-	LASSERT(!mutex_trylock(&clobj->cob_inode->i_mutex));
+	LASSERT(!inode_trylock(clobj->cob_inode));
 	clobj->cob_transient_pages--;
 }
 
@@ -548,7 +548,7 @@ int vvp_page_init(const struct lu_env *env, struct cl_object *obj,
 	} else {
 		struct ccc_object *clobj = cl2ccc(obj);
 
-		LASSERT(!mutex_trylock(&clobj->cob_inode->i_mutex));
+		LASSERT(!inode_trylock(clobj->cob_inode));
 		cl_page_slice_add(page, &cpg->cpg_cl, obj,
 				&vvp_transient_page_ops);
 		clobj->cob_transient_pages++;
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index 4b7eb33f7d01..8eb43f192d1f 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -60,10 +60,10 @@
 static
 int get_xattr_type(const char *name)
 {
-	if (!strcmp(name, POSIX_ACL_XATTR_ACCESS))
+	if (!strcmp(name, XATTR_NAME_POSIX_ACL_ACCESS))
 		return XATTR_ACL_ACCESS_T;
 
-	if (!strcmp(name, POSIX_ACL_XATTR_DEFAULT))
+	if (!strcmp(name, XATTR_NAME_POSIX_ACL_DEFAULT))
 		return XATTR_ACL_DEFAULT_T;
 
 	if (!strncmp(name, XATTR_USER_PREFIX,
@@ -193,7 +193,10 @@ int ll_setxattr_common(struct inode *inode, const char *name,
 			 ll_i2suppgid(inode), &req);
 #ifdef CONFIG_FS_POSIX_ACL
 	if (new_value != NULL)
-		lustre_posix_acl_xattr_free(new_value, size);
+		/*
+		 * Release the posix ACL space.
+		 */
+		kfree(new_value);
 	if (acl != NULL)
 		lustre_ext_acl_xattr_free(acl);
 #endif
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index e1e599ceb173..d1402762a0b2 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -1,6 +1,8 @@
 /*
  * Copyright 2012 Xyratex Technology Limited
  *
+ * Copyright (c) 2013, 2015, Intel Corporation.
+ *
  * Author: Andrew Perepechko <Andrew_Perepechko@xyratex.com>
  *
  */
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_intent.c b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
index eebe45bdceb6..66de27f1d289 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_intent.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -156,11 +156,11 @@ out:
  * IT_OPEN is intended to open (and create, possible) an object. Parent (pid)
  * may be split dir.
  */
-int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
-		    void *lmm, int lmmsize, struct lookup_intent *it,
-		    int flags, struct ptlrpc_request **reqp,
-		    ldlm_blocking_callback cb_blocking,
-		    __u64 extra_lock_flags)
+static int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
+			   void *lmm, int lmmsize, struct lookup_intent *it,
+			   int flags, struct ptlrpc_request **reqp,
+			   ldlm_blocking_callback cb_blocking,
+			   __u64 extra_lock_flags)
 {
 	struct obd_device	*obd = exp->exp_obd;
 	struct lmv_obd		*lmv = &obd->u.lmv;
@@ -239,11 +239,12 @@ int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
 /*
  * Handler for: getattr, lookup and revalidate cases.
  */
-int lmv_intent_lookup(struct obd_export *exp, struct md_op_data *op_data,
-		      void *lmm, int lmmsize, struct lookup_intent *it,
-		      int flags, struct ptlrpc_request **reqp,
-		      ldlm_blocking_callback cb_blocking,
-		      __u64 extra_lock_flags)
+static int lmv_intent_lookup(struct obd_export *exp,
+			     struct md_op_data *op_data,
+			     void *lmm, int lmmsize, struct lookup_intent *it,
+			     int flags, struct ptlrpc_request **reqp,
+			     ldlm_blocking_callback cb_blocking,
+			     __u64 extra_lock_flags)
 {
 	struct obd_device      *obd = exp->exp_obd;
 	struct lmv_obd	 *lmv = &obd->u.lmv;
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_internal.h b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
index b808728daee7..eb8e673cbc3f 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_internal.h
+++ b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -56,20 +56,6 @@ int lmv_intent_lock(struct obd_export *exp, struct md_op_data *op_data,
 		    ldlm_blocking_callback cb_blocking,
 		    __u64 extra_lock_flags);
 
-int lmv_intent_lookup(struct obd_export *exp, struct md_op_data *op_data,
-		      void *lmm, int lmmsize, struct lookup_intent *it,
-		      int flags, struct ptlrpc_request **reqp,
-		      ldlm_blocking_callback cb_blocking,
-		      __u64 extra_lock_flags);
-
-int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
-		    void *lmm, int lmmsize, struct lookup_intent *it,
-		    int flags, struct ptlrpc_request **reqp,
-		    ldlm_blocking_callback cb_blocking,
-		    __u64 extra_lock_flags);
-
-int lmv_blocking_ast(struct ldlm_lock *, struct ldlm_lock_desc *,
-		     void *, int);
 int lmv_fld_lookup(struct lmv_obd *lmv, const struct lu_fid *fid, u32 *mds);
 int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid, u32 mds);
 int lmv_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index 635a93cc94de..bbafe0a710d8 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -2744,55 +2744,55 @@ static int lmv_quotacheck(struct obd_device *unused, struct obd_export *exp,
 }
 
 static struct obd_ops lmv_obd_ops = {
-	.o_owner		= THIS_MODULE,
-	.o_setup		= lmv_setup,
-	.o_cleanup	      = lmv_cleanup,
-	.o_precleanup	   = lmv_precleanup,
-	.o_process_config       = lmv_process_config,
-	.o_connect	      = lmv_connect,
-	.o_disconnect	   = lmv_disconnect,
-	.o_statfs	       = lmv_statfs,
-	.o_get_info	     = lmv_get_info,
-	.o_set_info_async       = lmv_set_info_async,
-	.o_packmd	       = lmv_packmd,
-	.o_unpackmd	     = lmv_unpackmd,
-	.o_notify	       = lmv_notify,
-	.o_get_uuid	     = lmv_get_uuid,
-	.o_iocontrol	    = lmv_iocontrol,
-	.o_quotacheck	   = lmv_quotacheck,
-	.o_quotactl	     = lmv_quotactl
+	.owner		= THIS_MODULE,
+	.setup		= lmv_setup,
+	.cleanup	= lmv_cleanup,
+	.precleanup	= lmv_precleanup,
+	.process_config	= lmv_process_config,
+	.connect	= lmv_connect,
+	.disconnect	= lmv_disconnect,
+	.statfs		= lmv_statfs,
+	.get_info	= lmv_get_info,
+	.set_info_async	= lmv_set_info_async,
+	.packmd		= lmv_packmd,
+	.unpackmd	= lmv_unpackmd,
+	.notify		= lmv_notify,
+	.get_uuid	= lmv_get_uuid,
+	.iocontrol	= lmv_iocontrol,
+	.quotacheck	= lmv_quotacheck,
+	.quotactl	= lmv_quotactl
 };
 
 static struct md_ops lmv_md_ops = {
-	.m_getstatus	    = lmv_getstatus,
-	.m_null_inode		= lmv_null_inode,
-	.m_find_cbdata	  = lmv_find_cbdata,
-	.m_close		= lmv_close,
-	.m_create	       = lmv_create,
-	.m_done_writing	 = lmv_done_writing,
-	.m_enqueue	      = lmv_enqueue,
-	.m_getattr	      = lmv_getattr,
-	.m_getxattr	     = lmv_getxattr,
-	.m_getattr_name	 = lmv_getattr_name,
-	.m_intent_lock	  = lmv_intent_lock,
-	.m_link		 = lmv_link,
-	.m_rename	       = lmv_rename,
-	.m_setattr	      = lmv_setattr,
-	.m_setxattr	     = lmv_setxattr,
-	.m_sync		 = lmv_sync,
-	.m_readpage	     = lmv_readpage,
-	.m_unlink	       = lmv_unlink,
-	.m_init_ea_size	 = lmv_init_ea_size,
-	.m_cancel_unused	= lmv_cancel_unused,
-	.m_set_lock_data	= lmv_set_lock_data,
-	.m_lock_match	   = lmv_lock_match,
-	.m_get_lustre_md	= lmv_get_lustre_md,
-	.m_free_lustre_md       = lmv_free_lustre_md,
-	.m_set_open_replay_data = lmv_set_open_replay_data,
-	.m_clear_open_replay_data = lmv_clear_open_replay_data,
-	.m_get_remote_perm      = lmv_get_remote_perm,
-	.m_intent_getattr_async = lmv_intent_getattr_async,
-	.m_revalidate_lock      = lmv_revalidate_lock
+	.getstatus		= lmv_getstatus,
+	.null_inode		= lmv_null_inode,
+	.find_cbdata		= lmv_find_cbdata,
+	.close			= lmv_close,
+	.create			= lmv_create,
+	.done_writing		= lmv_done_writing,
+	.enqueue		= lmv_enqueue,
+	.getattr		= lmv_getattr,
+	.getxattr		= lmv_getxattr,
+	.getattr_name		= lmv_getattr_name,
+	.intent_lock		= lmv_intent_lock,
+	.link			= lmv_link,
+	.rename			= lmv_rename,
+	.setattr		= lmv_setattr,
+	.setxattr		= lmv_setxattr,
+	.sync			= lmv_sync,
+	.readpage		= lmv_readpage,
+	.unlink			= lmv_unlink,
+	.init_ea_size		= lmv_init_ea_size,
+	.cancel_unused		= lmv_cancel_unused,
+	.set_lock_data		= lmv_set_lock_data,
+	.lock_match		= lmv_lock_match,
+	.get_lustre_md		= lmv_get_lustre_md,
+	.free_lustre_md		= lmv_free_lustre_md,
+	.set_open_replay_data	= lmv_set_open_replay_data,
+	.clear_open_replay_data	= lmv_clear_open_replay_data,
+	.get_remote_perm	= lmv_get_remote_perm,
+	.intent_getattr_async	= lmv_intent_getattr_async,
+	.revalidate_lock	= lmv_revalidate_lock
 };
 
 static int __init lmv_init(void)
@@ -2812,7 +2812,7 @@ static void lmv_exit(void)
 	class_unregister_type(LUSTRE_LMV_NAME);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Logical Metadata Volume OBD driver");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
index 1c0fe65243e1..66a2492c1cc3 100644
--- a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_dev.c b/drivers/staging/lustre/lustre/lov/lov_dev.c
index 2e8b566458f6..3733fdc88c8c 100644
--- a/drivers/staging/lustre/lustre/lov/lov_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lov_dev.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 34c1346f0dc7..b3c9c85aab9d 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index 515a5c147827..2d00bad58e35 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -151,23 +151,10 @@ int lov_stripe_number(struct lov_stripe_md *lsm, u64 lov_off);
 /* lov_qos.c */
 #define LOV_USES_ASSIGNED_STRIPE	0
 #define LOV_USES_DEFAULT_STRIPE	 1
-int qos_add_tgt(struct obd_device *obd, __u32 index);
-int qos_del_tgt(struct obd_device *obd, struct lov_tgt_desc *tgt);
-void qos_shrink_lsm(struct lov_request_set *set);
-int qos_prep_create(struct obd_export *exp, struct lov_request_set *set);
-void qos_update(struct lov_obd *lov);
-void qos_statfs_done(struct lov_obd *lov);
-void qos_statfs_update(struct obd_device *obd, __u64 max_age, int wait);
-int qos_remedy_create(struct lov_request_set *set, struct lov_request *req);
 
 /* lov_request.c */
-void lov_set_add_req(struct lov_request *req, struct lov_request_set *set);
-int lov_set_finished(struct lov_request_set *set, int idempotent);
-void lov_update_set(struct lov_request_set *set,
-		    struct lov_request *req, int rc);
 int lov_update_common_set(struct lov_request_set *set,
 			  struct lov_request *req, int rc);
-int lov_check_and_wait_active(struct lov_obd *lov, int ost_idx);
 int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 			 struct lov_request_set **reqset);
 int lov_fini_getattr_set(struct lov_request_set *set);
@@ -184,8 +171,6 @@ int lov_update_setattr_set(struct lov_request_set *set,
 int lov_fini_setattr_set(struct lov_request_set *set);
 int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 			struct lov_request_set **reqset);
-void lov_update_statfs(struct obd_statfs *osfs, struct obd_statfs *lov_sfs,
-		       int success);
 int lov_fini_statfs(struct obd_device *obd, struct obd_statfs *osfs,
 		    int success);
 int lov_fini_statfs_set(struct lov_request_set *set);
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index 5e6228b9ca01..93fe69eb2560 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_merge.c b/drivers/staging/lustre/lustre/lov/lov_merge.c
index dd1cf3d2d039..97115bec7cca 100644
--- a/drivers/staging/lustre/lustre/lov/lov_merge.c
+++ b/drivers/staging/lustre/lustre/lov/lov_merge.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index 7abe484c07c0..6c2bdfe9cdcf 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -2277,35 +2277,35 @@ out:
 }
 
 static struct obd_ops lov_obd_ops = {
-	.o_owner	       = THIS_MODULE,
-	.o_setup	       = lov_setup,
-	.o_precleanup	  = lov_precleanup,
-	.o_cleanup	     = lov_cleanup,
-	/*.o_process_config      = lov_process_config,*/
-	.o_connect	     = lov_connect,
-	.o_disconnect	  = lov_disconnect,
-	.o_statfs	      = lov_statfs,
-	.o_statfs_async	= lov_statfs_async,
-	.o_packmd	      = lov_packmd,
-	.o_unpackmd	    = lov_unpackmd,
-	.o_create	      = lov_create,
-	.o_destroy	     = lov_destroy,
-	.o_getattr_async       = lov_getattr_async,
-	.o_setattr_async       = lov_setattr_async,
-	.o_adjust_kms	  = lov_adjust_kms,
-	.o_find_cbdata	 = lov_find_cbdata,
-	.o_iocontrol	   = lov_iocontrol,
-	.o_get_info	    = lov_get_info,
-	.o_set_info_async      = lov_set_info_async,
-	.o_notify	      = lov_notify,
-	.o_pool_new	    = lov_pool_new,
-	.o_pool_rem	    = lov_pool_remove,
-	.o_pool_add	    = lov_pool_add,
-	.o_pool_del	    = lov_pool_del,
-	.o_getref	      = lov_getref,
-	.o_putref	      = lov_putref,
-	.o_quotactl	    = lov_quotactl,
-	.o_quotacheck	  = lov_quotacheck,
+	.owner          = THIS_MODULE,
+	.setup          = lov_setup,
+	.precleanup     = lov_precleanup,
+	.cleanup        = lov_cleanup,
+	/*.process_config       = lov_process_config,*/
+	.connect        = lov_connect,
+	.disconnect     = lov_disconnect,
+	.statfs         = lov_statfs,
+	.statfs_async   = lov_statfs_async,
+	.packmd         = lov_packmd,
+	.unpackmd       = lov_unpackmd,
+	.create         = lov_create,
+	.destroy        = lov_destroy,
+	.getattr_async  = lov_getattr_async,
+	.setattr_async  = lov_setattr_async,
+	.adjust_kms     = lov_adjust_kms,
+	.find_cbdata    = lov_find_cbdata,
+	.iocontrol      = lov_iocontrol,
+	.get_info       = lov_get_info,
+	.set_info_async = lov_set_info_async,
+	.notify         = lov_notify,
+	.pool_new       = lov_pool_new,
+	.pool_rem       = lov_pool_remove,
+	.pool_add       = lov_pool_add,
+	.pool_del       = lov_pool_del,
+	.getref         = lov_getref,
+	.putref         = lov_putref,
+	.quotactl       = lov_quotactl,
+	.quotacheck     = lov_quotacheck,
 };
 
 struct kmem_cache *lov_oinfo_slab;
@@ -2352,7 +2352,7 @@ static void /*__exit*/ lov_exit(void)
 	lu_kmem_fini(lov_caches);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Logical Object Volume OBD driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(LUSTRE_VERSION_STRING);
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index c7ff817bb6fb..3b79ebc8eccf 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_offset.c b/drivers/staging/lustre/lustre/lov/lov_offset.c
index 9c8c77c05a8a..aa520aa76e09 100644
--- a/drivers/staging/lustre/lustre/lov/lov_offset.c
+++ b/drivers/staging/lustre/lustre/lov/lov_offset.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index 2fb1e974cc70..6b2d1007192b 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -258,22 +258,9 @@ static int lov_verify_lmm(void *lmm, int lmm_bytes, __u16 *stripe_count)
 	int rc;
 
 	if (lsm_op_find(le32_to_cpu(*(__u32 *)lmm)) == NULL) {
-		char *buffer;
-		int sz;
-
 		CERROR("bad disk LOV MAGIC: 0x%08X; dumping LMM (size=%d):\n",
 		       le32_to_cpu(*(__u32 *)lmm), lmm_bytes);
-		sz = lmm_bytes * 2 + 1;
-		buffer = libcfs_kvzalloc(sz, GFP_NOFS);
-		if (buffer != NULL) {
-			int i;
-
-			for (i = 0; i < lmm_bytes; i++)
-				sprintf(buffer+2*i, "%.2X", ((char *)lmm)[i]);
-			buffer[sz - 1] = '\0';
-			CERROR("%s\n", buffer);
-			kvfree(buffer);
-		}
+		CERROR("%*phN\n", lmm_bytes, lmm);
 		return -EINVAL;
 	}
 	rc = lsm_op_find(le32_to_cpu(*(__u32 *)lmm))->lsm_lmm_verify(lmm,
diff --git a/drivers/staging/lustre/lustre/lov/lov_page.c b/drivers/staging/lustre/lustre/lov/lov_page.c
index 463cadbd9d40..037ae91b74e7 100644
--- a/drivers/staging/lustre/lustre/lov/lov_page.c
+++ b/drivers/staging/lustre/lustre/lov/lov_page.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lov_pool.c b/drivers/staging/lustre/lustre/lov/lov_pool.c
index b03827ef6514..b43ce6cd64c2 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pool.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pool.c
@@ -412,8 +412,7 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 	if (!new_pool)
 		return -ENOMEM;
 
-	strncpy(new_pool->pool_name, poolname, LOV_MAXPOOLNAME);
-	new_pool->pool_name[LOV_MAXPOOLNAME] = '\0';
+	strlcpy(new_pool->pool_name, poolname, sizeof(new_pool->pool_name));
 	new_pool->pool_lobd = obd;
 	/* ref count init to 1 because when created a pool is always used
 	 * up to deletion
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index 1a150c26798d..42deda71f577 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -74,7 +74,7 @@ void lov_finish_set(struct lov_request_set *set)
 	kfree(set);
 }
 
-int lov_set_finished(struct lov_request_set *set, int idempotent)
+static int lov_set_finished(struct lov_request_set *set, int idempotent)
 {
 	int completes = atomic_read(&set->set_completes);
 
@@ -89,8 +89,8 @@ int lov_set_finished(struct lov_request_set *set, int idempotent)
 	return 0;
 }
 
-void lov_update_set(struct lov_request_set *set,
-		    struct lov_request *req, int rc)
+static void lov_update_set(struct lov_request_set *set,
+			   struct lov_request *req, int rc)
 {
 	req->rq_complete = 1;
 	req->rq_rc = rc;
@@ -118,7 +118,8 @@ int lov_update_common_set(struct lov_request_set *set,
 	return rc;
 }
 
-void lov_set_add_req(struct lov_request *req, struct lov_request_set *set)
+static void lov_set_add_req(struct lov_request *req,
+			    struct lov_request_set *set)
 {
 	list_add_tail(&req->rq_link, &set->set_list);
 	set->set_count++;
@@ -144,7 +145,7 @@ static int lov_check_set(struct lov_obd *lov, int idx)
  * If the OSC has not yet had a chance to connect to the OST the first time,
  * wait once for it to connect instead of returning an error.
  */
-int lov_check_and_wait_active(struct lov_obd *lov, int ost_idx)
+static int lov_check_and_wait_active(struct lov_obd *lov, int ost_idx)
 {
 	wait_queue_head_t waitq;
 	struct l_wait_info lwi;
@@ -591,8 +592,9 @@ int lov_fini_statfs_set(struct lov_request_set *set)
 	return rc;
 }
 
-void lov_update_statfs(struct obd_statfs *osfs, struct obd_statfs *lov_sfs,
-		       int success)
+static void lov_update_statfs(struct obd_statfs *osfs,
+			      struct obd_statfs *lov_sfs,
+			      int success)
 {
 	int shift = 0, quit = 0;
 	__u64 tmp;
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_dev.c b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
index 8bc04c8d3d60..f1795c3e2db5 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
@@ -26,6 +26,8 @@
 /*
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2013, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_object.c b/drivers/staging/lustre/lustre/lov/lovsub_object.c
index d775e28d4097..5ba5ee1b8681 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_object.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/lov/lproc_lov.c b/drivers/staging/lustre/lustre/lov/lproc_lov.c
index a0be15c6b55a..337241d84980 100644
--- a/drivers/staging/lustre/lustre/lov/lproc_lov.c
+++ b/drivers/staging/lustre/lustre/lov/lproc_lov.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
index 1c95f87a0e2a..38f267a60f59 100644
--- a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
+++ b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -82,82 +82,6 @@ static ssize_t max_rpcs_in_flight_store(struct kobject *kobj,
 }
 LUSTRE_RW_ATTR(max_rpcs_in_flight);
 
-static int mdc_kuc_open(struct inode *inode, struct file *file)
-{
-	return single_open(file, NULL, inode->i_private);
-}
-
-/* temporary for testing */
-static ssize_t mdc_kuc_write(struct file *file,
-				const char __user *buffer,
-				size_t count, loff_t *off)
-{
-	struct obd_device *obd =
-			((struct seq_file *)file->private_data)->private;
-	struct kuc_hdr		*lh;
-	struct hsm_action_list	*hal;
-	struct hsm_action_item	*hai;
-	int			 len;
-	int			 fd, rc;
-
-	rc = lprocfs_write_helper(buffer, count, &fd);
-	if (rc)
-		return rc;
-
-	if (fd < 0)
-		return -ERANGE;
-	CWARN("message to fd %d\n", fd);
-
-	len = sizeof(*lh) + sizeof(*hal) + MTI_NAME_MAXLEN +
-		/* for mockup below */ 2 * cfs_size_round(sizeof(*hai));
-
-	lh = kzalloc(len, GFP_NOFS);
-	if (!lh)
-		return -ENOMEM;
-
-	lh->kuc_magic = KUC_MAGIC;
-	lh->kuc_transport = KUC_TRANSPORT_HSM;
-	lh->kuc_msgtype = HMT_ACTION_LIST;
-	lh->kuc_msglen = len;
-
-	hal = (struct hsm_action_list *)(lh + 1);
-	hal->hal_version = HAL_VERSION;
-	hal->hal_archive_id = 1;
-	hal->hal_flags = 0;
-	obd_uuid2fsname(hal->hal_fsname, obd->obd_name, MTI_NAME_MAXLEN);
-
-	/* mock up an action list */
-	hal->hal_count = 2;
-	hai = hai_zero(hal);
-	hai->hai_action = HSMA_ARCHIVE;
-	hai->hai_fid.f_oid = 5;
-	hai->hai_len = sizeof(*hai);
-	hai = hai_next(hai);
-	hai->hai_action = HSMA_RESTORE;
-	hai->hai_fid.f_oid = 10;
-	hai->hai_len = sizeof(*hai);
-
-	/* This works for either broadcast or unicast to a single fd */
-	if (fd == 0) {
-		rc = libcfs_kkuc_group_put(KUC_GRP_HSM, lh);
-	} else {
-		struct file *fp = fget(fd);
-
-		rc = libcfs_kkuc_msg_put(fp, lh);
-		fput(fp);
-	}
-	kfree(lh);
-	if (rc < 0)
-		return rc;
-	return count;
-}
-
-static struct file_operations mdc_kuc_fops = {
-	.open		= mdc_kuc_open,
-	.write		= mdc_kuc_write,
-	.release	= single_release,
-};
-
 LPROC_SEQ_FOPS_WR_ONLY(mdc, ping);
 
 LPROC_SEQ_FOPS_RO_TYPE(mdc, connect_flags);
@@ -196,7 +120,6 @@ static struct lprocfs_vars lprocfs_mdc_obd_vars[] = {
 	{ "timeouts",		&mdc_timeouts_fops,		NULL, 0 },
 	{ "import",		&mdc_import_fops,		NULL, 0 },
 	{ "state",		&mdc_state_fops,		NULL, 0 },
-	{ "hsm_nl",		&mdc_kuc_fops,			NULL, 0200 },
 	{ "pinger_recov",	&mdc_pinger_recov_fops,		NULL, 0 },
 	{ NULL }
 };
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_internal.h b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
index 29b46f754726..3d2997a161b6 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_internal.h
+++ b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -44,7 +44,6 @@ void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars);
 
 void mdc_pack_body(struct ptlrpc_request *req, const struct lu_fid *fid,
 		   __u64 valid, int ea_size, __u32 suppgid, int flags);
-int mdc_pack_req(struct ptlrpc_request *req, int version, int opc);
 void mdc_is_subdir_pack(struct ptlrpc_request *req, const struct lu_fid *pfid,
 			const struct lu_fid *cfid, int flags);
 void mdc_swap_layouts_pack(struct ptlrpc_request *req,
@@ -62,7 +61,6 @@ void mdc_open_pack(struct ptlrpc_request *req, struct md_op_data *op_data,
 		   __u32 mode, __u64 rdev, __u64 flags, const void *data,
 		   int datalen);
 void mdc_unlink_pack(struct ptlrpc_request *req, struct md_op_data *op_data);
-void mdc_getxattr_pack(struct ptlrpc_request *req, struct md_op_data *op_data);
 void mdc_link_pack(struct ptlrpc_request *req, struct md_op_data *op_data);
 void mdc_rename_pack(struct ptlrpc_request *req, struct md_op_data *op_data,
 		     const char *old, int oldlen, const char *new, int newlen);
@@ -97,25 +95,12 @@ int mdc_resource_get_unused(struct obd_export *exp, const struct lu_fid *fid,
 /* mdc/mdc_request.c */
 int mdc_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 		  struct md_op_data *op_data);
-
-int mdc_open(struct obd_export *exp, u64 ino, int type, int flags,
-	     struct lov_mds_md *lmm, int lmm_size, struct lustre_handle *fh,
-	     struct ptlrpc_request **);
-
 struct obd_client_handle;
 
-int mdc_get_lustre_md(struct obd_export *md_exp, struct ptlrpc_request *req,
-		      struct obd_export *dt_exp, struct obd_export *lmv_exp,
-		      struct lustre_md *md);
-
-int mdc_free_lustre_md(struct obd_export *exp, struct lustre_md *md);
-
 int mdc_set_open_replay_data(struct obd_export *exp,
 			     struct obd_client_handle *och,
 			     struct lookup_intent *it);
 
-int mdc_clear_open_replay_data(struct obd_export *exp,
-			       struct obd_client_handle *och);
 void mdc_commit_open(struct ptlrpc_request *req);
 void mdc_replay_open(struct ptlrpc_request *req);
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_lib.c b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
index 227fc9ee0dcf..7218532ffea3 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_lib.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index d4bf34b61f3a..ef9a1e124ea4 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_reint.c b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
index c87c7d8efa07..ac7695a10753 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_reint.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index 16a5a10d371e..57e0fc1e8549 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2001, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -447,9 +447,11 @@ static int mdc_unpack_acl(struct ptlrpc_request *req, struct lustre_md *md)
 #define mdc_unpack_acl(req, md) 0
 #endif
 
-int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
-		      struct obd_export *dt_exp, struct obd_export *md_exp,
-		      struct lustre_md *md)
+static int mdc_get_lustre_md(struct obd_export *exp,
+			     struct ptlrpc_request *req,
+			     struct obd_export *dt_exp,
+			     struct obd_export *md_exp,
+			     struct lustre_md *md)
 {
 	struct req_capsule *pill = &req->rq_pill;
 	int rc;
@@ -573,7 +575,7 @@ out:
 	return rc;
 }
 
-int mdc_free_lustre_md(struct obd_export *exp, struct lustre_md *md)
+static int mdc_free_lustre_md(struct obd_export *exp, struct lustre_md *md)
 {
 	return 0;
 }
@@ -737,8 +739,8 @@ static void mdc_free_open(struct md_open_data *mod)
 		ptlrpc_request_committed(mod->mod_close_req, committed);
 }
 
-int mdc_clear_open_replay_data(struct obd_export *exp,
-			       struct obd_client_handle *och)
+static int mdc_clear_open_replay_data(struct obd_export *exp,
+				      struct obd_client_handle *och)
 {
 	struct md_open_data *mod = och->och_mod;
 
@@ -1179,7 +1181,6 @@ static int mdc_ioc_hsm_progress(struct obd_export *exp,
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1214,7 +1215,6 @@ static int mdc_ioc_hsm_ct_register(struct obd_import *imp, __u32 archives)
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1280,7 +1280,6 @@ static int mdc_ioc_hsm_ct_unregister(struct obd_import *imp)
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1360,8 +1359,6 @@ static int mdc_ioc_hsm_state_set(struct obd_export *exp,
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	goto out;
-
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1425,8 +1422,6 @@ static int mdc_ioc_hsm_request(struct obd_export *exp,
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	goto out;
-
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -2035,17 +2030,6 @@ static int mdc_hsm_ct_reregister(__u32 data, void *cb_arg)
 	return ((rc != 0) && (rc != -EEXIST)) ? rc : 0;
 }
 
-/**
- * Re-establish all kuc contexts with MDT
- * after MDT shutdown/recovery.
- */
-static int mdc_kuc_reregister(struct obd_import *imp)
-{
-	/* re-register HSM agents */
-	return libcfs_kkuc_group_foreach(KUC_GRP_HSM, mdc_hsm_ct_reregister,
-					 (void *)imp);
-}
-
 static int mdc_set_info_async(const struct lu_env *env,
 			      struct obd_export *exp,
 			      u32 keylen, void *key,
@@ -2208,7 +2192,10 @@ static int mdc_import_event(struct obd_device *obd, struct obd_import *imp,
 		rc = obd_notify_observer(obd, obd, OBD_NOTIFY_ACTIVE, NULL);
 		/* redo the kuc registration after reconnecting */
 		if (rc == 0)
-			rc = mdc_kuc_reregister(imp);
+			/* re-register HSM agents */
+			rc = libcfs_kkuc_group_foreach(KUC_GRP_HSM,
+						       mdc_hsm_ct_reregister,
+						       (void *)imp);
 		break;
 	case IMP_EVENT_OCD:
 		rc = obd_notify_observer(obd, obd, OBD_NOTIFY_OCD, NULL);
@@ -2460,59 +2447,59 @@ static int mdc_get_remote_perm(struct obd_export *exp, const struct lu_fid *fid,
 }
 
 static struct obd_ops mdc_obd_ops = {
-	.o_owner	    = THIS_MODULE,
-	.o_setup	    = mdc_setup,
-	.o_precleanup       = mdc_precleanup,
-	.o_cleanup	  = mdc_cleanup,
-	.o_add_conn	 = client_import_add_conn,
-	.o_del_conn	 = client_import_del_conn,
-	.o_connect          = client_connect_import,
-	.o_disconnect       = client_disconnect_export,
-	.o_iocontrol	= mdc_iocontrol,
-	.o_set_info_async   = mdc_set_info_async,
-	.o_statfs	   = mdc_statfs,
-	.o_fid_init	    = client_fid_init,
-	.o_fid_fini	    = client_fid_fini,
-	.o_fid_alloc	= mdc_fid_alloc,
-	.o_import_event     = mdc_import_event,
-	.o_get_info	 = mdc_get_info,
-	.o_process_config   = mdc_process_config,
-	.o_get_uuid	 = mdc_get_uuid,
-	.o_quotactl	 = mdc_quotactl,
-	.o_quotacheck       = mdc_quotacheck
+	.owner          = THIS_MODULE,
+	.setup          = mdc_setup,
+	.precleanup     = mdc_precleanup,
+	.cleanup        = mdc_cleanup,
+	.add_conn       = client_import_add_conn,
+	.del_conn       = client_import_del_conn,
+	.connect        = client_connect_import,
+	.disconnect     = client_disconnect_export,
+	.iocontrol      = mdc_iocontrol,
+	.set_info_async = mdc_set_info_async,
+	.statfs         = mdc_statfs,
+	.fid_init       = client_fid_init,
+	.fid_fini       = client_fid_fini,
+	.fid_alloc      = mdc_fid_alloc,
+	.import_event   = mdc_import_event,
+	.get_info       = mdc_get_info,
+	.process_config = mdc_process_config,
+	.get_uuid       = mdc_get_uuid,
+	.quotactl       = mdc_quotactl,
+	.quotacheck     = mdc_quotacheck
 };
 
 static struct md_ops mdc_md_ops = {
-	.m_getstatus	= mdc_getstatus,
-	.m_null_inode	    = mdc_null_inode,
-	.m_find_cbdata      = mdc_find_cbdata,
-	.m_close	    = mdc_close,
-	.m_create	   = mdc_create,
-	.m_done_writing     = mdc_done_writing,
-	.m_enqueue	  = mdc_enqueue,
-	.m_getattr	  = mdc_getattr,
-	.m_getattr_name     = mdc_getattr_name,
-	.m_intent_lock      = mdc_intent_lock,
-	.m_link	     = mdc_link,
-	.m_is_subdir	= mdc_is_subdir,
-	.m_rename	   = mdc_rename,
-	.m_setattr	  = mdc_setattr,
-	.m_setxattr	 = mdc_setxattr,
-	.m_getxattr	 = mdc_getxattr,
-	.m_sync	     = mdc_sync,
-	.m_readpage	 = mdc_readpage,
-	.m_unlink	   = mdc_unlink,
-	.m_cancel_unused    = mdc_cancel_unused,
-	.m_init_ea_size     = mdc_init_ea_size,
-	.m_set_lock_data    = mdc_set_lock_data,
-	.m_lock_match       = mdc_lock_match,
-	.m_get_lustre_md    = mdc_get_lustre_md,
-	.m_free_lustre_md   = mdc_free_lustre_md,
-	.m_set_open_replay_data = mdc_set_open_replay_data,
-	.m_clear_open_replay_data = mdc_clear_open_replay_data,
-	.m_get_remote_perm  = mdc_get_remote_perm,
-	.m_intent_getattr_async = mdc_intent_getattr_async,
-	.m_revalidate_lock      = mdc_revalidate_lock
+	.getstatus		= mdc_getstatus,
+	.null_inode		= mdc_null_inode,
+	.find_cbdata		= mdc_find_cbdata,
+	.close			= mdc_close,
+	.create			= mdc_create,
+	.done_writing		= mdc_done_writing,
+	.enqueue		= mdc_enqueue,
+	.getattr		= mdc_getattr,
+	.getattr_name		= mdc_getattr_name,
+	.intent_lock		= mdc_intent_lock,
+	.link			= mdc_link,
+	.is_subdir		= mdc_is_subdir,
+	.rename			= mdc_rename,
+	.setattr		= mdc_setattr,
+	.setxattr		= mdc_setxattr,
+	.getxattr		= mdc_getxattr,
+	.sync			= mdc_sync,
+	.readpage		= mdc_readpage,
+	.unlink			= mdc_unlink,
+	.cancel_unused		= mdc_cancel_unused,
+	.init_ea_size		= mdc_init_ea_size,
+	.set_lock_data		= mdc_set_lock_data,
+	.lock_match		= mdc_lock_match,
+	.get_lustre_md		= mdc_get_lustre_md,
+	.free_lustre_md		= mdc_free_lustre_md,
+	.set_open_replay_data	= mdc_set_open_replay_data,
+	.clear_open_replay_data	= mdc_clear_open_replay_data,
+	.get_remote_perm	= mdc_get_remote_perm,
+	.intent_getattr_async	= mdc_intent_getattr_async,
+	.revalidate_lock	= mdc_revalidate_lock
 };
 
 static int __init mdc_init(void)
@@ -2530,7 +2517,7 @@ static void /*__exit*/ mdc_exit(void)
 	class_unregister_type(LUSTRE_MDC_NAME);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Metadata Client");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index 5f53f3b7ceff..ab4800c20a95 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -463,7 +463,7 @@ int lprocfs_mgc_rd_ir_state(struct seq_file *m, void *data)
 	}
 	spin_unlock(&config_list_lock);
 
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
 
@@ -1293,7 +1293,7 @@ static int mgc_process_recover_log(struct obd_device *obd,
 	struct page **pages;
 	int nrpages;
 	bool eof = true;
-	bool mne_swab = false;
+	bool mne_swab;
 	int i;
 	int ealen;
 	int rc;
@@ -1698,20 +1698,20 @@ out:
 }
 
 static struct obd_ops mgc_obd_ops = {
-	.o_owner	= THIS_MODULE,
-	.o_setup	= mgc_setup,
-	.o_precleanup   = mgc_precleanup,
-	.o_cleanup      = mgc_cleanup,
-	.o_add_conn     = client_import_add_conn,
-	.o_del_conn     = client_import_del_conn,
-	.o_connect      = client_connect_import,
-	.o_disconnect   = client_disconnect_export,
-	/* .o_enqueue      = mgc_enqueue, */
-	/* .o_iocontrol    = mgc_iocontrol, */
-	.o_set_info_async = mgc_set_info_async,
-	.o_get_info       = mgc_get_info,
-	.o_import_event = mgc_import_event,
-	.o_process_config = mgc_process_config,
+	.owner          = THIS_MODULE,
+	.setup          = mgc_setup,
+	.precleanup     = mgc_precleanup,
+	.cleanup        = mgc_cleanup,
+	.add_conn       = client_import_add_conn,
+	.del_conn       = client_import_del_conn,
+	.connect        = client_connect_import,
+	.disconnect     = client_disconnect_export,
+	/* .enqueue     = mgc_enqueue, */
+	/* .iocontrol   = mgc_iocontrol, */
+	.set_info_async = mgc_set_info_async,
+	.get_info       = mgc_get_info,
+	.import_event   = mgc_import_event,
+	.process_config = mgc_process_config,
 };
 
 static int __init mgc_init(void)
@@ -1725,7 +1725,7 @@ static void /*__exit*/ mgc_exit(void)
 	class_unregister_type(LUSTRE_MGC_NAME);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Management Client");
 MODULE_LICENSE("GPL");
 
diff --git a/drivers/staging/lustre/lustre/obdclass/acl.c b/drivers/staging/lustre/lustre/obdclass/acl.c
index 2e20cf635b27..49ba8851c8ac 100644
--- a/drivers/staging/lustre/lustre/obdclass/acl.c
+++ b/drivers/staging/lustre/lustre/obdclass/acl.c
@@ -236,15 +236,6 @@ _out:
 EXPORT_SYMBOL(lustre_posix_acl_xattr_filter);
 
 /*
- * Release the posix ACL space.
- */
-void lustre_posix_acl_xattr_free(posix_acl_xattr_header *header, int size)
-{
-	kfree(header);
-}
-EXPORT_SYMBOL(lustre_posix_acl_xattr_free);
-
-/*
  * Release the extended ACL space.
  */
 void lustre_ext_acl_xattr_free(ext_acl_xattr_header *header)
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_io.c b/drivers/staging/lustre/lustre/obdclass/cl_io.c
index e67cea758405..63246ba36798 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_io.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_io.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -236,16 +236,11 @@ int cl_io_rw_init(const struct lu_env *env, struct cl_io *io,
 }
 EXPORT_SYMBOL(cl_io_rw_init);
 
-static inline const struct lu_fid *
-cl_lock_descr_fid(const struct cl_lock_descr *descr)
-{
-	return lu_object_fid(&descr->cld_obj->co_lu);
-}
-
 static int cl_lock_descr_sort(const struct cl_lock_descr *d0,
 			      const struct cl_lock_descr *d1)
 {
-	return lu_fid_cmp(cl_lock_descr_fid(d0), cl_lock_descr_fid(d1)) ?:
+	return lu_fid_cmp(lu_object_fid(&d0->cld_obj->co_lu),
+			  lu_object_fid(&d1->cld_obj->co_lu)) ?:
 		__diff_normalize(d0->cld_start, d1->cld_start);
 }
 
@@ -254,7 +249,8 @@ static int cl_lock_descr_cmp(const struct cl_lock_descr *d0,
 {
 	int ret;
 
-	ret = lu_fid_cmp(cl_lock_descr_fid(d0), cl_lock_descr_fid(d1));
+	ret = lu_fid_cmp(lu_object_fid(&d0->cld_obj->co_lu),
+			 lu_object_fid(&d1->cld_obj->co_lu));
 	if (ret)
 		return ret;
 	if (d0->cld_end < d1->cld_start)
@@ -1216,15 +1212,6 @@ void cl_2queue_init(struct cl_2queue *queue)
 EXPORT_SYMBOL(cl_2queue_init);
 
 /**
- * Add a page to the incoming page list of 2-queue.
- */
-void cl_2queue_add(struct cl_2queue *queue, struct cl_page *page)
-{
-	cl_page_list_add(&queue->c2_qin, page);
-}
-EXPORT_SYMBOL(cl_2queue_add);
-
-/**
  * Disown pages in both lists of a 2-queue.
  */
 void cl_2queue_disown(const struct lu_env *env,
@@ -1262,7 +1249,10 @@ EXPORT_SYMBOL(cl_2queue_fini);
 void cl_2queue_init_page(struct cl_2queue *queue, struct cl_page *page)
 {
 	cl_2queue_init(queue);
-	cl_2queue_add(queue, page);
+	/*
+	 * Add a page to the incoming page list of 2-queue.
+	 */
+	cl_page_list_add(&queue->c2_qin, page);
 }
 EXPORT_SYMBOL(cl_2queue_init_page);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index 5621bebf33a9..1836dc01499a 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -687,7 +687,7 @@ EXPORT_SYMBOL(cl_lock_mutex_get);
  *
  * \see cl_lock_mutex_get()
  */
-int cl_lock_mutex_try(const struct lu_env *env, struct cl_lock *lock)
+static int cl_lock_mutex_try(const struct lu_env *env, struct cl_lock *lock)
 {
 	int result;
 
@@ -705,7 +705,6 @@ int cl_lock_mutex_try(const struct lu_env *env, struct cl_lock *lock)
 		result = -EBUSY;
 	return result;
 }
-EXPORT_SYMBOL(cl_lock_mutex_try);
 
 /**
  {* Unlocks cl_lock object.
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_object.c b/drivers/staging/lustre/lustre/obdclass/cl_object.c
index a1a6024220ff..57c8d5412bbd 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -704,7 +704,7 @@ static inline struct cl_env *cl_env_container(struct lu_env *env)
 	return container_of(env, struct cl_env, ce_lu);
 }
 
-struct lu_env *cl_env_peek(int *refcheck)
+static struct lu_env *cl_env_peek(int *refcheck)
 {
 	struct lu_env *env;
 	struct cl_env *cle;
@@ -724,7 +724,6 @@ struct lu_env *cl_env_peek(int *refcheck)
 	CDEBUG(D_OTHER, "%d@%p\n", cle ? cle->ce_ref : 0, cle);
 	return env;
 }
-EXPORT_SYMBOL(cl_env_peek);
 
 /**
  * Returns lu_env: if there already is an environment associated with the
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_page.c b/drivers/staging/lustre/lustre/obdclass/cl_page.c
index 2f569edd0811..61f28ebfc058 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_page.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_page.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index 3e9c24684690..0975e443057c 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 1999, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -576,7 +576,7 @@ static void cleanup_obdclass(void)
 	obd_zombie_impexp_stop();
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Class Driver Build Version: " BUILD_VERSION);
 MODULE_LICENSE("GPL");
 MODULE_VERSION(LUSTRE_VERSION_STRING);
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index 6477aeb88028..228c44c37c4a 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -27,7 +27,7 @@
  * Copyright (c) 1999, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -132,7 +132,7 @@ static struct obd_type *class_get_type(const char *name)
 	if (type) {
 		spin_lock(&type->obd_type_lock);
 		type->typ_refcnt++;
-		try_module_get(type->typ_dt_ops->o_owner);
+		try_module_get(type->typ_dt_ops->owner);
 		spin_unlock(&type->obd_type_lock);
 	}
 	return type;
@@ -143,7 +143,7 @@ void class_put_type(struct obd_type *type)
 	LASSERT(type);
 	spin_lock(&type->obd_type_lock);
 	type->typ_refcnt--;
-	module_put(type->typ_dt_ops->o_owner);
+	module_put(type->typ_dt_ops->owner);
 	spin_unlock(&type->obd_type_lock);
 }
 EXPORT_SYMBOL(class_put_type);
@@ -155,7 +155,7 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 			struct lu_device_type *ldt)
 {
 	struct obd_type *type;
-	int rc = 0;
+	int rc;
 
 	/* sanity check */
 	LASSERT(strnlen(name, CLASS_MAX_NAME) < CLASS_MAX_NAME);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
index 518288df4d53..42fc26f4ae25 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-sysctl.c
@@ -27,7 +27,7 @@
  * Copyright (c) 1999, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/llog.c b/drivers/staging/lustre/lustre/obdclass/llog.c
index 7cb55ef79737..f956d7ed6785 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index c442cae5fd37..0f05e9c4a5b2 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_internal.h b/drivers/staging/lustre/lustre/obdclass/llog_internal.h
index b9fe4b01c690..7fb48dda355e 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_internal.h
+++ b/drivers/staging/lustre/lustre/obdclass/llog_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index 3900b9d4007e..9bc51998c05c 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_swab.c b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
index 9354f75b5cab..3aa7393b20c3 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_swab.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index 333ac7d269b7..51fe15f5d687 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -502,7 +502,7 @@ int lprocfs_rd_server_uuid(struct seq_file *m, void *data)
 		   obd2cli_tgt(obd), imp_state_name,
 		   imp->imp_deactive ? "\tDEACTIVATED" : "");
 
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 
 	return 0;
 }
@@ -526,7 +526,7 @@ int lprocfs_rd_conn_uuid(struct seq_file *m, void *data)
 	else
 		seq_puts(m, "<none>\n");
 
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 
 	return 0;
 }
@@ -665,15 +665,18 @@ int lprocfs_rd_import(struct seq_file *m, void *data)
 		seq_printf(m, "%s%s", j ? ", " : "", nidstr);
 		j++;
 	}
-	libcfs_nid2str_r(imp->imp_connection->c_peer.nid,
-			 nidstr, sizeof(nidstr));
+	if (imp->imp_connection != NULL)
+		libcfs_nid2str_r(imp->imp_connection->c_peer.nid,
+				 nidstr, sizeof(nidstr));
+	else
+		strncpy(nidstr, "<none>", sizeof(nidstr));
 	seq_printf(m,
 		      "]\n"
 		      "       current_connection: %s\n"
 		      "       connection_attempts: %u\n"
 		      "       generation: %u\n"
 		      "       in-progress_invalidations: %u\n",
-		      imp->imp_connection == NULL ? "<none>" : nidstr,
+		      nidstr,
 		      imp->imp_conn_cnt,
 		      imp->imp_generation,
 		      atomic_read(&imp->imp_inval_count));
@@ -765,7 +768,7 @@ int lprocfs_rd_import(struct seq_file *m, void *data)
 	}
 
 out_climp:
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_import);
@@ -796,7 +799,7 @@ int lprocfs_rd_state(struct seq_file *m, void *data)
 			   ptlrpc_import_state_name(ish->ish_state));
 	}
 
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_state);
@@ -857,7 +860,7 @@ int lprocfs_rd_timeouts(struct seq_file *m, void *data)
 		lprocfs_at_hist_helper(m, &imp->imp_at.iat_service_estimate[i]);
 	}
 
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_timeouts);
@@ -876,7 +879,7 @@ int lprocfs_rd_connect_flags(struct seq_file *m, void *data)
 	seq_printf(m, "flags=%#llx\n", flags);
 	obd_connect_seq_flags2str(m, flags, "\n");
 	seq_printf(m, "\n");
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	return 0;
 }
 EXPORT_SYMBOL(lprocfs_rd_connect_flags);
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_object.c b/drivers/staging/lustre/lustre/obdclass/lu_object.c
index 0193608a930a..ce248f4072c2 100644
--- a/drivers/staging/lustre/lustre/obdclass/lu_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/lu_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -916,7 +916,7 @@ static void lu_obj_hop_put_locked(struct cfs_hash *hs, struct hlist_node *hnode)
 	LBUG(); /* we should never called it */
 }
 
-struct cfs_hash_ops lu_site_hash_ops = {
+static struct cfs_hash_ops lu_site_hash_ops = {
 	.hs_hash	= lu_obj_hop_hash,
 	.hs_key		= lu_obj_hop_key,
 	.hs_keycmp      = lu_obj_hop_keycmp,
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_config.c b/drivers/staging/lustre/lustre/obdclass/obd_config.c
index c231e0da0e2a..49cdc647910c 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_config.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_config.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index 48003d5325e3..b5aa8168dbff 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -892,7 +892,7 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 	}
 	lmd->lmd_magic = LMD_MAGIC;
 
-	lmd->lmd_params = kzalloc(4096, GFP_NOFS);
+	lmd->lmd_params = kzalloc(LMD_PARAMS_MAXLEN, GFP_NOFS);
 	if (!lmd->lmd_params)
 		return -ENOMEM;
 	lmd->lmd_params[0] = '\0';
@@ -978,7 +978,7 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 				goto invalid;
 			clear++;
 		} else if (strncmp(s1, "param=", 6) == 0) {
-			int length;
+			size_t length, params_length;
 			char *tail = strchr(s1 + 6, ',');
 
 			if (tail == NULL)
@@ -986,8 +986,12 @@ static int lmd_parse(char *options, struct lustre_mount_data *lmd)
 			else
 				length = tail - s1;
 			length -= 6;
+			params_length = strlen(lmd->lmd_params);
+			if (params_length + length + 1 >= LMD_PARAMS_MAXLEN)
+				return -E2BIG;
 			strncat(lmd->lmd_params, s1 + 6, length);
-			strcat(lmd->lmd_params, " ");
+			lmd->lmd_params[params_length + length] = '\0';
+			strlcat(lmd->lmd_params, " ", LMD_PARAMS_MAXLEN);
 			clear++;
 		} else if (strncmp(s1, "osd=", 4) == 0) {
 			rc = lmd_parse_string(&lmd->lmd_osd_type, s1 + 4);
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index a4a9a763ff02..7b53f7dd1797 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -1228,8 +1228,10 @@ static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 			cl_page_put(env, clp);
 			break;
 		}
-
-		cl_2queue_add(queue, clp);
+		/*
+		 * Add a page to the incoming page list of 2-queue.
+		 */
+		cl_page_list_add(&queue->c2_qin, clp);
 
 		/* drop the reference count for cl_page_find, so that the page
 		 * will be freed in cl_2queue_fini. */
@@ -2130,10 +2132,10 @@ static int echo_client_disconnect(struct obd_export *exp)
 }
 
 static struct obd_ops echo_client_obd_ops = {
-	.o_owner       = THIS_MODULE,
-	.o_iocontrol   = echo_client_iocontrol,
-	.o_connect     = echo_client_connect,
-	.o_disconnect  = echo_client_disconnect
+	.owner          = THIS_MODULE,
+	.iocontrol      = echo_client_iocontrol,
+	.connect        = echo_client_connect,
+	.disconnect     = echo_client_disconnect
 };
 
 static int echo_client_init(void)
@@ -2172,7 +2174,7 @@ static void /*__exit*/ obdecho_exit(void)
 
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Testing Echo OBD driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(LUSTRE_VERSION_STRING);
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index c4d44e70f1d7..1091536fc90d 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index b1d1a87f05e3..2229419b7184 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  *
  */
 /*
@@ -619,9 +619,9 @@ static inline int overlapped(struct osc_extent *ex1, struct osc_extent *ex2)
  * Find or create an extent which includes @index, core function to manage
  * extent tree.
  */
-struct osc_extent *osc_extent_find(const struct lu_env *env,
-				   struct osc_object *obj, pgoff_t index,
-				   int *grants)
+static struct osc_extent *osc_extent_find(const struct lu_env *env,
+					  struct osc_object *obj, pgoff_t index,
+					  int *grants)
 
 {
 	struct client_obd *cli = osc_cli(obj);
@@ -1420,8 +1420,8 @@ static void __osc_unreserve_grant(struct client_obd *cli,
 	}
 }
 
-void osc_unreserve_grant(struct client_obd *cli,
-			 unsigned int reserved, unsigned int unused)
+static void osc_unreserve_grant(struct client_obd *cli,
+				unsigned int reserved, unsigned int unused)
 {
 	client_obd_list_lock(&cli->cl_loi_list_lock);
 	__osc_unreserve_grant(cli, reserved, unused);
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index d2d68452d382..415c27e4ab66 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_dev.c b/drivers/staging/lustre/lustre/osc/osc_dev.c
index 69b523c0f570..7078cc57d8b9 100644
--- a/drivers/staging/lustre/lustre/osc/osc_dev.c
+++ b/drivers/staging/lustre/lustre/osc/osc_dev.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_internal.h b/drivers/staging/lustre/lustre/osc/osc_internal.h
index 5ed30ecc84e3..a4c61463b1c7 100644
--- a/drivers/staging/lustre/lustre/osc/osc_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -89,11 +89,6 @@ struct osc_cache_waiter {
 	int		     ocw_rc;
 };
 
-int osc_create(const struct lu_env *env, struct obd_export *exp,
-	       struct obdo *oa, struct lov_stripe_md **ea,
-	       struct obd_trans_info *oti);
-int osc_real_create(struct obd_export *exp, struct obdo *oa,
-		    struct lov_stripe_md **ea, struct obd_trans_info *oti);
 void osc_wake_cache_waiters(struct client_obd *cli);
 int osc_shrink_grant_to_target(struct client_obd *cli, __u64 target_bytes);
 void osc_update_next_shrink(struct client_obd *cli);
@@ -137,7 +132,6 @@ int osc_lru_shrink(struct client_obd *cli, int target);
 
 extern spinlock_t osc_ast_guard;
 
-int osc_cleanup(struct obd_device *obd);
 int osc_setup(struct obd_device *obd, struct lustre_cfg *lcfg);
 
 int lproc_osc_attach_seqstat(struct obd_device *dev);
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index d413496c0f63..abd0beb483fe 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_lock.c b/drivers/staging/lustre/lustre/osc/osc_lock.c
index 194490dcaca9..71f2810d18b9 100644
--- a/drivers/staging/lustre/lustre/osc/osc_lock.c
+++ b/drivers/staging/lustre/lustre/osc/osc_lock.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_object.c b/drivers/staging/lustre/lustre/osc/osc_object.c
index ba57f8df5c7f..fdd6219aacf6 100644
--- a/drivers/staging/lustre/lustre/osc/osc_object.c
+++ b/drivers/staging/lustre/lustre/osc/osc_object.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -158,8 +158,8 @@ static int osc_attr_get(const struct lu_env *env, struct cl_object *obj,
 	return 0;
 }
 
-int osc_attr_set(const struct lu_env *env, struct cl_object *obj,
-		 const struct cl_attr *attr, unsigned valid)
+static int osc_attr_set(const struct lu_env *env, struct cl_object *obj,
+			const struct cl_attr *attr, unsigned valid)
 {
 	struct lov_oinfo *oinfo = cl2osc(obj)->oo_oinfo;
 	struct ost_lvb *lvb = &oinfo->loi_lvb;
diff --git a/drivers/staging/lustre/lustre/osc/osc_page.c b/drivers/staging/lustre/lustre/osc/osc_page.c
index 61eaf7172fdf..2439d804fe75 100644
--- a/drivers/staging/lustre/lustre/osc/osc_page.c
+++ b/drivers/staging/lustre/lustre/osc/osc_page.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/osc/osc_quota.c b/drivers/staging/lustre/lustre/osc/osc_quota.c
index 199783103f71..e70e7961d763 100644
--- a/drivers/staging/lustre/lustre/osc/osc_quota.c
+++ b/drivers/staging/lustre/lustre/osc/osc_quota.c
@@ -23,7 +23,7 @@
 /*
  * Copyright (c) 2009, 2010, Oracle and/or its affiliates. All rights reserved.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  *
  * Code originally extracted from quota directory
  */
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index 367f83af12c0..7034f0a942c5 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -104,7 +104,7 @@ struct osc_enqueue_args {
 static void osc_release_ppga(struct brw_page **ppga, u32 count);
 static int brw_interpret(const struct lu_env *env,
 			 struct ptlrpc_request *req, void *data, int rc);
-int osc_cleanup(struct obd_device *obd);
+static int osc_cleanup(struct obd_device *obd);
 
 /* Pack OSC object metadata for disk storage (LE byte order). */
 static int osc_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
@@ -431,8 +431,9 @@ static int osc_setattr_async(struct obd_export *exp, struct obd_info *oinfo,
 				      oinfo->oi_cb_up, oinfo, rqset);
 }
 
-int osc_real_create(struct obd_export *exp, struct obdo *oa,
-		    struct lov_stripe_md **ea, struct obd_trans_info *oti)
+static int osc_real_create(struct obd_export *exp, struct obdo *oa,
+			   struct lov_stripe_md **ea,
+			   struct obd_trans_info *oti)
 {
 	struct ptlrpc_request *req;
 	struct ost_body *body;
@@ -689,9 +690,9 @@ static int osc_can_send_destroy(struct client_obd *cli)
 	return 0;
 }
 
-int osc_create(const struct lu_env *env, struct obd_export *exp,
-	       struct obdo *oa, struct lov_stripe_md **ea,
-	       struct obd_trans_info *oti)
+static int osc_create(const struct lu_env *env, struct obd_export *exp,
+		      struct obdo *oa, struct lov_stripe_md **ea,
+		      struct obd_trans_info *oti)
 {
 	int rc = 0;
 
@@ -2726,7 +2727,7 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 		}
 
 		*((u64 *)val) = *reply;
-	out:
+out:
 		ptlrpc_req_finished(req);
 		return rc;
 	} else if (KEY_IS(KEY_FIEMAP)) {
@@ -3255,33 +3256,33 @@ static int osc_process_config(struct obd_device *obd, u32 len, void *buf)
 }
 
 struct obd_ops osc_obd_ops = {
-	.o_owner		= THIS_MODULE,
-	.o_setup		= osc_setup,
-	.o_precleanup	   = osc_precleanup,
-	.o_cleanup	      = osc_cleanup,
-	.o_add_conn	     = client_import_add_conn,
-	.o_del_conn	     = client_import_del_conn,
-	.o_connect	      = client_connect_import,
-	.o_reconnect	    = osc_reconnect,
-	.o_disconnect	   = osc_disconnect,
-	.o_statfs	       = osc_statfs,
-	.o_statfs_async	 = osc_statfs_async,
-	.o_packmd	       = osc_packmd,
-	.o_unpackmd	     = osc_unpackmd,
-	.o_create	       = osc_create,
-	.o_destroy	      = osc_destroy,
-	.o_getattr	      = osc_getattr,
-	.o_getattr_async	= osc_getattr_async,
-	.o_setattr	      = osc_setattr,
-	.o_setattr_async	= osc_setattr_async,
-	.o_find_cbdata	  = osc_find_cbdata,
-	.o_iocontrol	    = osc_iocontrol,
-	.o_get_info	     = osc_get_info,
-	.o_set_info_async       = osc_set_info_async,
-	.o_import_event	 = osc_import_event,
-	.o_process_config       = osc_process_config,
-	.o_quotactl	     = osc_quotactl,
-	.o_quotacheck	   = osc_quotacheck,
+	.owner          = THIS_MODULE,
+	.setup          = osc_setup,
+	.precleanup     = osc_precleanup,
+	.cleanup        = osc_cleanup,
+	.add_conn       = client_import_add_conn,
+	.del_conn       = client_import_del_conn,
+	.connect        = client_connect_import,
+	.reconnect      = osc_reconnect,
+	.disconnect     = osc_disconnect,
+	.statfs         = osc_statfs,
+	.statfs_async   = osc_statfs_async,
+	.packmd         = osc_packmd,
+	.unpackmd       = osc_unpackmd,
+	.create         = osc_create,
+	.destroy        = osc_destroy,
+	.getattr        = osc_getattr,
+	.getattr_async  = osc_getattr_async,
+	.setattr        = osc_setattr,
+	.setattr_async  = osc_setattr_async,
+	.find_cbdata    = osc_find_cbdata,
+	.iocontrol      = osc_iocontrol,
+	.get_info       = osc_get_info,
+	.set_info_async = osc_set_info_async,
+	.import_event   = osc_import_event,
+	.process_config = osc_process_config,
+	.quotactl       = osc_quotactl,
+	.quotacheck     = osc_quotacheck,
 };
 
 extern struct lu_kmem_descr osc_caches[];
@@ -3357,7 +3358,7 @@ static void /*__exit*/ osc_exit(void)
 	ptlrpc_free_rq_pool(osc_rq_pool);
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Object Storage Client (OSC)");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(LUSTRE_VERSION_STRING);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index a9f1bf536da9..efdda09507bf 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index 9c2fd34e2eb9..990156986986 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015 Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index bfa410f7e773..f752c789bda0 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index d7c4f47808bd..c0e613c23854 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
index 5122205cbb99..e87702073f1f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index afab0dee7a5c..cc55b7973721 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -1197,7 +1197,7 @@ int lprocfs_wr_ping(struct file *file, const char __user *buffer,
 		return rc;
 
 	req = ptlrpc_prep_ping(obd->u.cli.cl_import);
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 	if (req == NULL)
 		return -ENOMEM;
 
@@ -1291,7 +1291,7 @@ int lprocfs_rd_pinger_recov(struct seq_file *m, void *n)
 		return rc;
 
 	seq_printf(m, "%d\n", !imp->imp_no_pinger_recover);
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 
 	return 0;
 }
@@ -1319,7 +1319,7 @@ int lprocfs_wr_pinger_recov(struct file *file, const char __user *buffer,
 	spin_lock(&imp->imp_lock);
 	imp->imp_no_pinger_recover = !val;
 	spin_unlock(&imp->imp_lock);
-	LPROCFS_CLIMP_EXIT(obd);
+	up_read(&obd->u.cli.cl_sem);
 
 	return count;
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index 09ddeef6ba48..c5d7ff5cbd73 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pers.c b/drivers/staging/lustre/lustre/ptlrpc/pers.c
index 2a2a9fb6549c..ec3af109a1d7 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pers.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pers.c
@@ -26,6 +26,8 @@
 /*
  * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
+ *
+ * Copyright (c) 2014, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 5c719f175657..fb2d5236a971 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index ab6c4580f91c..8f67e0562b73 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -73,7 +73,6 @@ void ptlrpc_request_handle_notconn(struct ptlrpc_request *);
 void lustre_assert_wire_constants(void);
 int ptlrpc_import_in_recovery(struct obd_import *imp);
 int ptlrpc_set_import_discon(struct obd_import *imp, __u32 conn_cnt);
-void ptlrpc_handle_failed_import(struct obd_import *imp);
 int ptlrpc_replay_next(struct obd_import *imp, int *inflight);
 void ptlrpc_initiate_recovery(struct obd_import *imp);
 
@@ -88,8 +87,6 @@ void ptlrpc_ldebugfs_register_service(struct dentry *debugfs_entry,
 				      struct ptlrpc_service *svc);
 void ptlrpc_lprocfs_unregister_service(struct ptlrpc_service *svc);
 void ptlrpc_lprocfs_rpc_sent(struct ptlrpc_request *req, long amount);
-void ptlrpc_lprocfs_do_request_stat(struct ptlrpc_request *req,
-				     long q_usec, long work_usec);
 
 /* NRS */
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
index 9deeb244166f..c4f1d0f5deb2 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
@@ -160,7 +160,7 @@ static void __exit ptlrpc_exit(void)
 	ptlrpc_connection_fini();
 }
 
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
+MODULE_AUTHOR("OpenSFS, Inc. <http://www.lustre.org/>");
 MODULE_DESCRIPTION("Lustre Request Processor and Lock Management");
 MODULE_LICENSE("GPL");
 MODULE_VERSION("1.0.0");
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
index ce036a1ac466..60fb0ced7137 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -422,6 +422,7 @@ static int ptlrpcd(void *arg)
 	complete(&pc->pc_starting);
 
 	/*
+
 	 * This mainloop strongly resembles ptlrpc_set_wait() except that our
 	 * set never completes.  ptlrpcd_check() calls ptlrpc_check_set() when
 	 * there are requests in the set. New requests come in on the set's
diff --git a/drivers/staging/lustre/lustre/ptlrpc/recover.c b/drivers/staging/lustre/lustre/ptlrpc/recover.c
index 7b1d72947330..db6626cab6f2 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/recover.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/recover.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index cd8a9987f7ac..6152c1b766c3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
index 7ff948fe1424..4b0b81c115ee 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
@@ -83,8 +83,7 @@ int sptlrpc_parse_flavor(const char *str, struct sptlrpc_flavor *flvr)
 		return 0;
 	}
 
-	strncpy(buf, str, sizeof(buf));
-	buf[sizeof(buf) - 1] = '\0';
+	strlcpy(buf, str, sizeof(buf));
 
 	bulk = strchr(buf, '-');
 	if (bulk)
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
index f448b4567af0..905a41451ca3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_plain.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, 2012, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index f45898f17793..8598300a61d1 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2010, 2012, Intel Corporation.
+ * Copyright (c) 2010, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/lustre/lustre/ptlrpc/wiretest.c b/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
index 40f720ca3b14..61d9ca93c53a 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
@@ -27,7 +27,7 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2011, Intel Corporation.
+ * Copyright (c) 2011, 2015, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
diff --git a/drivers/staging/media/bcm2048/radio-bcm2048.c b/drivers/staging/media/bcm2048/radio-bcm2048.c
index b10d6016b993..8fdf0ac4f287 100644
--- a/drivers/staging/media/bcm2048/radio-bcm2048.c
+++ b/drivers/staging/media/bcm2048/radio-bcm2048.c
@@ -179,14 +179,13 @@
 #define BCM2048_DEFAULT_TIMEOUT		1500
 #define BCM2048_AUTO_SEARCH_TIMEOUT	3000
 
-
 #define BCM2048_FREQDEV_UNIT		10000
 #define BCM2048_FREQV4L2_MULTI		625
 #define dev_to_v4l2(f)	((f * BCM2048_FREQDEV_UNIT) / BCM2048_FREQV4L2_MULTI)
 #define v4l2_to_dev(f)	((f * BCM2048_FREQV4L2_MULTI) / BCM2048_FREQDEV_UNIT)
 
-#define msb(x)                  ((u8)((u16) x >> 8))
-#define lsb(x)                  ((u8)((u16) x &  0x00FF))
+#define msb(x)                  ((u8)((u16)x >> 8))
+#define lsb(x)                  ((u8)((u16)x &  0x00FF))
 #define compose_u16(msb, lsb)	(((u16)msb << 8) | lsb)
 
 #define BCM2048_DEFAULT_POWERING_DELAY	20
@@ -348,7 +347,7 @@ static struct region_info region_configs[] = {
  *	I2C Interface read / write
  */
 static int bcm2048_send_command(struct bcm2048_device *bdev, unsigned int reg,
-					unsigned int value)
+				unsigned int value)
 {
 	struct i2c_client *client = bdev->client;
 	u8 data[2];
@@ -370,7 +369,7 @@ static int bcm2048_send_command(struct bcm2048_device *bdev, unsigned int reg,
 }
 
 static int bcm2048_recv_command(struct bcm2048_device *bdev, unsigned int reg,
-			u8 *value)
+				u8 *value)
 {
 	struct i2c_client *client = bdev->client;
 
@@ -385,7 +384,7 @@ static int bcm2048_recv_command(struct bcm2048_device *bdev, unsigned int reg,
 }
 
 static int bcm2048_recv_duples(struct bcm2048_device *bdev, unsigned int reg,
-			u8 *value, u8 duples)
+			       u8 *value, u8 duples)
 {
 	struct i2c_client *client = bdev->client;
 	struct i2c_adapter *adap = client->adapter;
@@ -436,7 +435,7 @@ static int bcm2048_set_power_state(struct bcm2048_device *bdev, u8 power)
 	 */
 	if (power)
 		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM,
-					bdev->cache_fm_rds_system);
+					   bdev->cache_fm_rds_system);
 	msleep(BCM2048_DEFAULT_POWERING_DELAY);
 
 	if (!power)
@@ -475,17 +474,17 @@ static int bcm2048_set_rds_no_lock(struct bcm2048_device *bdev, u8 rds_on)
 		bdev->rds_state = BCM2048_RDS_ON;
 		flags =	BCM2048_RDS_FLAG_FIFO_WLINE;
 		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
-						flags);
+					   flags);
 	} else {
 		flags =	0;
 		bdev->rds_state = 0;
 		err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
-						flags);
+					   flags);
 		memset(&bdev->rds_info, 0, sizeof(bdev->rds_info));
 	}
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_SYSTEM,
-					bdev->cache_fm_rds_system);
+				   bdev->cache_fm_rds_system);
 
 	return err;
 }
@@ -545,14 +544,14 @@ static int bcm2048_set_fm_automatic_stereo_mono(struct bcm2048_device *bdev,
 		bdev->cache_fm_ctrl |= BCM2048_STEREO_MONO_AUTO_SELECT;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_CTRL,
-					bdev->cache_fm_ctrl);
+				   bdev->cache_fm_ctrl);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
 }
 
 static int bcm2048_set_fm_hi_lo_injection(struct bcm2048_device *bdev,
-						u8 hi_lo)
+					  u8 hi_lo)
 {
 	int err;
 
@@ -564,7 +563,7 @@ static int bcm2048_set_fm_hi_lo_injection(struct bcm2048_device *bdev,
 		bdev->cache_fm_ctrl |= BCM2048_HI_LO_INJECTION;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_CTRL,
-					bdev->cache_fm_ctrl);
+				   bdev->cache_fm_ctrl);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -592,7 +591,7 @@ static int bcm2048_set_fm_frequency(struct bcm2048_device *bdev, u32 frequency)
 	int err;
 
 	if (frequency < bdev->region_info.bottom_frequency ||
-		frequency > bdev->region_info.top_frequency)
+	    frequency > bdev->region_info.top_frequency)
 		return -EDOM;
 
 	frequency -= BCM2048_FREQUENCY_BASE;
@@ -601,7 +600,7 @@ static int bcm2048_set_fm_frequency(struct bcm2048_device *bdev, u32 frequency)
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_FREQ0, lsb(frequency));
 	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_FREQ1,
-					msb(frequency));
+				    msb(frequency));
 
 	if (!err)
 		bdev->frequency = frequency;
@@ -632,12 +631,12 @@ static int bcm2048_get_fm_frequency(struct bcm2048_device *bdev)
 }
 
 static int bcm2048_set_fm_af_frequency(struct bcm2048_device *bdev,
-						u32 frequency)
+				       u32 frequency)
 {
 	int err;
 
 	if (frequency < bdev->region_info.bottom_frequency ||
-		frequency > bdev->region_info.top_frequency)
+	    frequency > bdev->region_info.top_frequency)
 		return -EDOM;
 
 	frequency -= BCM2048_FREQUENCY_BASE;
@@ -645,9 +644,9 @@ static int bcm2048_set_fm_af_frequency(struct bcm2048_device *bdev,
 	mutex_lock(&bdev->mutex);
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AF_FREQ0,
-					lsb(frequency));
+				   lsb(frequency));
 	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_AF_FREQ1,
-					msb(frequency));
+				    msb(frequency));
 	if (!err)
 		bdev->frequency = frequency;
 
@@ -692,7 +691,7 @@ static int bcm2048_set_fm_deemphasis(struct bcm2048_device *bdev, int d)
 	bdev->cache_fm_audio_ctrl0 |= deemphasis;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
-		bdev->cache_fm_audio_ctrl0);
+				   bdev->cache_fm_audio_ctrl0);
 
 	if (!err)
 		bdev->region_info.deemphasis = d;
@@ -740,7 +739,7 @@ static int bcm2048_set_region(struct bcm2048_device *bdev, u8 region)
 		bdev->cache_fm_ctrl &= ~BCM2048_BAND_SELECT;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_CTRL,
-					bdev->cache_fm_ctrl);
+				   bdev->cache_fm_ctrl);
 	if (err) {
 		mutex_unlock(&bdev->mutex);
 		goto done;
@@ -748,7 +747,7 @@ static int bcm2048_set_region(struct bcm2048_device *bdev, u8 region)
 	mutex_unlock(&bdev->mutex);
 
 	if (bdev->frequency < region_configs[region].bottom_frequency ||
-		bdev->frequency > region_configs[region].top_frequency)
+	    bdev->frequency > region_configs[region].top_frequency)
 		new_frequency = region_configs[region].bottom_frequency;
 
 	if (new_frequency > 0) {
@@ -759,7 +758,7 @@ static int bcm2048_set_region(struct bcm2048_device *bdev, u8 region)
 	}
 
 	err = bcm2048_set_fm_deemphasis(bdev,
-			region_configs[region].deemphasis);
+					region_configs[region].deemphasis);
 
 done:
 	return err;
@@ -786,10 +785,10 @@ static int bcm2048_set_mute(struct bcm2048_device *bdev, u16 mute)
 
 	if (mute)
 		bdev->cache_fm_audio_ctrl0 |= (BCM2048_RF_MUTE |
-						BCM2048_MANUAL_MUTE);
+					       BCM2048_MANUAL_MUTE);
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
-					bdev->cache_fm_audio_ctrl0);
+				   bdev->cache_fm_audio_ctrl0);
 
 	if (!err)
 		bdev->mute_state = mute;
@@ -807,7 +806,7 @@ static int bcm2048_get_mute(struct bcm2048_device *bdev)
 
 	if (bdev->power_state) {
 		err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
-						&value);
+					   &value);
 		if (!err)
 			err = value & (BCM2048_RF_MUTE | BCM2048_MANUAL_MUTE);
 	} else {
@@ -826,11 +825,11 @@ static int bcm2048_set_audio_route(struct bcm2048_device *bdev, u8 route)
 
 	route &= (BCM2048_AUDIO_ROUTE_DAC | BCM2048_AUDIO_ROUTE_I2S);
 	bdev->cache_fm_audio_ctrl0 &= ~(BCM2048_AUDIO_ROUTE_DAC |
-		BCM2048_AUDIO_ROUTE_I2S);
+					BCM2048_AUDIO_ROUTE_I2S);
 	bdev->cache_fm_audio_ctrl0 |= route;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
-					bdev->cache_fm_audio_ctrl0);
+				   bdev->cache_fm_audio_ctrl0);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -849,7 +848,7 @@ static int bcm2048_get_audio_route(struct bcm2048_device *bdev)
 
 	if (!err)
 		return value & (BCM2048_AUDIO_ROUTE_DAC |
-			BCM2048_AUDIO_ROUTE_I2S);
+				BCM2048_AUDIO_ROUTE_I2S);
 
 	return err;
 }
@@ -865,7 +864,7 @@ static int bcm2048_set_dac_output(struct bcm2048_device *bdev, u8 channels)
 	bdev->cache_fm_audio_ctrl0 |= channels;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_AUDIO_CTRL0,
-					bdev->cache_fm_audio_ctrl0);
+				   bdev->cache_fm_audio_ctrl0);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -884,13 +883,13 @@ static int bcm2048_get_dac_output(struct bcm2048_device *bdev)
 
 	if (!err)
 		return value & (BCM2048_DAC_OUTPUT_LEFT |
-			BCM2048_DAC_OUTPUT_RIGHT);
+				BCM2048_DAC_OUTPUT_RIGHT);
 
 	return err;
 }
 
 static int bcm2048_set_fm_search_rssi_threshold(struct bcm2048_device *bdev,
-							u8 threshold)
+						u8 threshold)
 {
 	int err;
 
@@ -901,7 +900,7 @@ static int bcm2048_set_fm_search_rssi_threshold(struct bcm2048_device *bdev,
 	bdev->cache_fm_search_ctrl0 |= threshold;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0,
-					bdev->cache_fm_search_ctrl0);
+				   bdev->cache_fm_search_ctrl0);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -937,7 +936,7 @@ static int bcm2048_set_fm_search_mode_direction(struct bcm2048_device *bdev,
 		bdev->cache_fm_search_ctrl0 |= BCM2048_SEARCH_DIRECTION;
 
 	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_SEARCH_CTRL0,
-					bdev->cache_fm_search_ctrl0);
+				   bdev->cache_fm_search_ctrl0);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -961,7 +960,7 @@ static int bcm2048_get_fm_search_mode_direction(struct bcm2048_device *bdev)
 }
 
 static int bcm2048_set_fm_search_tune_mode(struct bcm2048_device *bdev,
-						u8 mode)
+					   u8 mode)
 {
 	int err, timeout, restart_rds = 0;
 	u8 value, flags;
@@ -1000,8 +999,8 @@ static int bcm2048_set_fm_search_tune_mode(struct bcm2048_device *bdev,
 		timeout = BCM2048_AUTO_SEARCH_TIMEOUT;
 
 	if (!wait_for_completion_timeout(&bdev->compl,
-			msecs_to_jiffies(timeout)))
-			dev_err(&bdev->client->dev, "IRQ timeout.\n");
+		msecs_to_jiffies(timeout)))
+		dev_err(&bdev->client->dev, "IRQ timeout.\n");
 
 	if (value)
 		if (!bdev->scan_state)
@@ -1024,7 +1023,7 @@ static int bcm2048_get_fm_search_tune_mode(struct bcm2048_device *bdev)
 	mutex_lock(&bdev->mutex);
 
 	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_SEARCH_TUNE_MODE,
-					&value);
+				   &value);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1040,10 +1039,10 @@ static int bcm2048_set_rds_b_block_mask(struct bcm2048_device *bdev, u16 mask)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MASK0, lsb(mask));
-	err |= bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MASK1, msb(mask));
+	err = bcm2048_send_command(bdev, BCM2048_I2C_RDS_BLKB_MASK0,
+				   lsb(mask));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_RDS_BLKB_MASK1,
+				    msb(mask));
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1056,10 +1055,8 @@ static int bcm2048_get_rds_b_block_mask(struct bcm2048_device *bdev)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MASK0, &lsb);
-	err |= bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MASK1, &msb);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_RDS_BLKB_MASK0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_RDS_BLKB_MASK1, &msb);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1070,16 +1067,16 @@ static int bcm2048_get_rds_b_block_mask(struct bcm2048_device *bdev)
 }
 
 static int bcm2048_set_rds_b_block_match(struct bcm2048_device *bdev,
-						u16 match)
+					 u16 match)
 {
 	int err;
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MATCH0, lsb(match));
-	err |= bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MATCH1, msb(match));
+	err = bcm2048_send_command(bdev, BCM2048_I2C_RDS_BLKB_MATCH0,
+				   lsb(match));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_RDS_BLKB_MATCH1,
+				    msb(match));
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1092,10 +1089,8 @@ static int bcm2048_get_rds_b_block_match(struct bcm2048_device *bdev)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MATCH0, &lsb);
-	err |= bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_BLKB_MATCH1, &msb);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_RDS_BLKB_MATCH0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_RDS_BLKB_MATCH1, &msb);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1111,10 +1106,8 @@ static int bcm2048_set_rds_pi_mask(struct bcm2048_device *bdev, u16 mask)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_PI_MASK0, lsb(mask));
-	err |= bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_PI_MASK1, msb(mask));
+	err = bcm2048_send_command(bdev, BCM2048_I2C_RDS_PI_MASK0, lsb(mask));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_RDS_PI_MASK1, msb(mask));
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1127,10 +1120,8 @@ static int bcm2048_get_rds_pi_mask(struct bcm2048_device *bdev)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_PI_MASK0, &lsb);
-	err |= bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_PI_MASK1, &msb);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_RDS_PI_MASK0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_RDS_PI_MASK1, &msb);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1146,10 +1137,10 @@ static int bcm2048_set_rds_pi_match(struct bcm2048_device *bdev, u16 match)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_PI_MATCH0, lsb(match));
-	err |= bcm2048_send_command(bdev,
-			BCM2048_I2C_RDS_PI_MATCH1, msb(match));
+	err = bcm2048_send_command(bdev, BCM2048_I2C_RDS_PI_MATCH0,
+				   lsb(match));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_RDS_PI_MATCH1,
+				    msb(match));
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1162,10 +1153,8 @@ static int bcm2048_get_rds_pi_match(struct bcm2048_device *bdev)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_PI_MATCH0, &lsb);
-	err |= bcm2048_recv_command(bdev,
-			BCM2048_I2C_RDS_PI_MATCH1, &msb);
+	err = bcm2048_recv_command(bdev, BCM2048_I2C_RDS_PI_MATCH0, &lsb);
+	err |= bcm2048_recv_command(bdev, BCM2048_I2C_RDS_PI_MATCH1, &msb);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1181,10 +1170,8 @@ static int bcm2048_set_fm_rds_mask(struct bcm2048_device *bdev, u16 mask)
 
 	mutex_lock(&bdev->mutex);
 
-	err = bcm2048_send_command(bdev,
-			BCM2048_I2C_FM_RDS_MASK0, lsb(mask));
-	err |= bcm2048_send_command(bdev,
-			BCM2048_I2C_FM_RDS_MASK1, msb(mask));
+	err = bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK0, lsb(mask));
+	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1, msb(mask));
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1245,13 +1232,13 @@ static int bcm2048_set_fm_best_tune_mode(struct bcm2048_device *bdev, u8 mode)
 
 	/* Perform read as the manual indicates */
 	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
-					&value);
+				   &value);
 	value &= ~BCM2048_BEST_TUNE_MODE;
 
 	if (mode)
 		value |= BCM2048_BEST_TUNE_MODE;
 	err |= bcm2048_send_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
-					value);
+				    value);
 
 	mutex_unlock(&bdev->mutex);
 	return err;
@@ -1265,7 +1252,7 @@ static int bcm2048_get_fm_best_tune_mode(struct bcm2048_device *bdev)
 	mutex_lock(&bdev->mutex);
 
 	err = bcm2048_recv_command(bdev, BCM2048_I2C_FM_BEST_TUNE_MODE,
-					&value);
+				   &value);
 
 	mutex_unlock(&bdev->mutex);
 
@@ -1352,7 +1339,7 @@ static int bcm2048_checkrev(struct bcm2048_device *bdev)
 
 	if (!err) {
 		dev_info(&bdev->client->dev, "BCM2048 Version 0x%x\n",
-			version);
+			 version);
 		return version;
 	}
 
@@ -1362,7 +1349,7 @@ static int bcm2048_checkrev(struct bcm2048_device *bdev)
 static int bcm2048_get_rds_rt(struct bcm2048_device *bdev, char *data)
 {
 	int err = 0, i, j = 0, ce = 0, cr = 0;
-	char data_buffer[BCM2048_MAX_RDS_RT+1];
+	char data_buffer[BCM2048_MAX_RDS_RT + 1];
 
 	mutex_lock(&bdev->mutex);
 
@@ -1412,7 +1399,7 @@ unlock:
 static int bcm2048_get_rds_ps(struct bcm2048_device *bdev, char *data)
 {
 	int err = 0, i, j = 0;
-	char data_buffer[BCM2048_MAX_RDS_PS+1];
+	char data_buffer[BCM2048_MAX_RDS_PS + 1];
 
 	mutex_lock(&bdev->mutex);
 
@@ -1448,12 +1435,10 @@ static void bcm2048_parse_rds_pi(struct bcm2048_device *bdev)
 	u16 pi;
 
 	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
-
 		/* Block A match, only data without crc errors taken */
 		if (bdev->rds_info.radio_text[i] == BCM2048_RDS_BLOCK_A) {
-
-			pi = (bdev->rds_info.radio_text[i+1] << 8) +
-				bdev->rds_info.radio_text[i+2];
+			pi = (bdev->rds_info.radio_text[i + 1] << 8) +
+				bdev->rds_info.radio_text[i + 2];
 
 			if (!bdev->rds_info.rds_pi) {
 				bdev->rds_info.rds_pi = pi;
@@ -1478,20 +1463,21 @@ static int bcm2048_rds_block_crc(struct bcm2048_device *bdev, int i)
 }
 
 static void bcm2048_parse_rds_rt_block(struct bcm2048_device *bdev, int i,
-					int index, int crc)
+				       int index, int crc)
 {
 	/* Good data will overwrite poor data */
 	if (crc) {
 		if (!bdev->rds_info.rds_rt[index])
 			bdev->rds_info.rds_rt[index] =
-				bdev->rds_info.radio_text[i+1];
-		if (!bdev->rds_info.rds_rt[index+1])
-			bdev->rds_info.rds_rt[index+1] =
-				bdev->rds_info.radio_text[i+2];
+				bdev->rds_info.radio_text[i + 1];
+		if (!bdev->rds_info.rds_rt[index + 1])
+			bdev->rds_info.rds_rt[index + 1] =
+				bdev->rds_info.radio_text[i + 2];
 	} else {
-		bdev->rds_info.rds_rt[index] = bdev->rds_info.radio_text[i+1];
-		bdev->rds_info.rds_rt[index+1] =
-			bdev->rds_info.radio_text[i+2];
+		bdev->rds_info.rds_rt[index] =
+			bdev->rds_info.radio_text[i + 1];
+		bdev->rds_info.rds_rt[index + 1] =
+			bdev->rds_info.radio_text[i + 2];
 	}
 }
 
@@ -1505,18 +1491,17 @@ static int bcm2048_parse_rt_match_b(struct bcm2048_device *bdev, int i)
 		return -EIO;
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
-		BCM2048_RDS_BLOCK_B) {
-
-		rt_id = bdev->rds_info.radio_text[i+1] &
+	    BCM2048_RDS_BLOCK_B) {
+		rt_id = bdev->rds_info.radio_text[i + 1] &
 			BCM2048_RDS_BLOCK_MASK;
-		rt_group_b = bdev->rds_info.radio_text[i+1] &
+		rt_group_b = bdev->rds_info.radio_text[i + 1] &
 			BCM2048_RDS_GROUP_AB_MASK;
-		rt_ab = bdev->rds_info.radio_text[i+2] &
+		rt_ab = bdev->rds_info.radio_text[i + 2] &
 				BCM2048_RDS_RT_AB_MASK;
 
 		if (rt_group_b != bdev->rds_info.rds_rt_group_b) {
 			memset(bdev->rds_info.rds_rt, 0,
-				sizeof(bdev->rds_info.rds_rt));
+			       sizeof(bdev->rds_info.rds_rt));
 			bdev->rds_info.rds_rt_group_b = rt_group_b;
 		}
 
@@ -1524,11 +1509,11 @@ static int bcm2048_parse_rt_match_b(struct bcm2048_device *bdev, int i)
 			/* A to B or (vice versa), means: clear screen */
 			if (rt_ab != bdev->rds_info.rds_rt_ab) {
 				memset(bdev->rds_info.rds_rt, 0,
-					sizeof(bdev->rds_info.rds_rt));
+				       sizeof(bdev->rds_info.rds_rt));
 				bdev->rds_info.rds_rt_ab = rt_ab;
 			}
 
-			index = bdev->rds_info.radio_text[i+2] &
+			index = bdev->rds_info.radio_text[i + 2] &
 					BCM2048_RDS_RT_INDEX;
 
 			if (bdev->rds_info.rds_rt_group_b)
@@ -1544,7 +1529,7 @@ static int bcm2048_parse_rt_match_b(struct bcm2048_device *bdev, int i)
 }
 
 static int bcm2048_parse_rt_match_c(struct bcm2048_device *bdev, int i,
-					int index)
+				    int index)
 {
 	int crc;
 
@@ -1567,7 +1552,7 @@ static int bcm2048_parse_rt_match_c(struct bcm2048_device *bdev, int i,
 }
 
 static void bcm2048_parse_rt_match_d(struct bcm2048_device *bdev, int i,
-					int index)
+				     int index)
 {
 	int crc;
 
@@ -1579,8 +1564,8 @@ static void bcm2048_parse_rt_match_d(struct bcm2048_device *bdev, int i,
 	BUG_ON((index+4) >= BCM2048_MAX_RDS_RT);
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
-		BCM2048_RDS_BLOCK_D)
-		bcm2048_parse_rds_rt_block(bdev, i, index+2, crc);
+	    BCM2048_RDS_BLOCK_D)
+		bcm2048_parse_rds_rt_block(bdev, i, index + 2, crc);
 }
 
 static void bcm2048_parse_rds_rt(struct bcm2048_device *bdev)
@@ -1588,7 +1573,6 @@ static void bcm2048_parse_rds_rt(struct bcm2048_device *bdev)
 	int i, index = 0, crc, match_b = 0, match_c = 0, match_d = 0;
 
 	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
-
 		if (match_b) {
 			match_b = 0;
 			index = bcm2048_parse_rt_match_b(bdev, i);
@@ -1607,40 +1591,41 @@ static void bcm2048_parse_rds_rt(struct bcm2048_device *bdev)
 		}
 
 		/* Skip erroneous blocks due to messed up A block altogether */
-		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK)
-			== BCM2048_RDS_BLOCK_A) {
+		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		    BCM2048_RDS_BLOCK_A) {
 			crc = bcm2048_rds_block_crc(bdev, i);
 			if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
 				continue;
-			/* Syncronize to a good RDS PI */
-			if (((bdev->rds_info.radio_text[i+1] << 8) +
-				bdev->rds_info.radio_text[i+2]) ==
-				bdev->rds_info.rds_pi)
-					match_b = 1;
+			/* Synchronize to a good RDS PI */
+			if (((bdev->rds_info.radio_text[i + 1] << 8) +
+			    bdev->rds_info.radio_text[i + 2]) ==
+			    bdev->rds_info.rds_pi)
+				match_b = 1;
 		}
 	}
 }
 
 static void bcm2048_parse_rds_ps_block(struct bcm2048_device *bdev, int i,
-					int index, int crc)
+				       int index, int crc)
 {
 	/* Good data will overwrite poor data */
 	if (crc) {
 		if (!bdev->rds_info.rds_ps[index])
 			bdev->rds_info.rds_ps[index] =
-				bdev->rds_info.radio_text[i+1];
-		if (!bdev->rds_info.rds_ps[index+1])
-			bdev->rds_info.rds_ps[index+1] =
-				bdev->rds_info.radio_text[i+2];
+				bdev->rds_info.radio_text[i + 1];
+		if (!bdev->rds_info.rds_ps[index + 1])
+			bdev->rds_info.rds_ps[index + 1] =
+				bdev->rds_info.radio_text[i + 2];
 	} else {
-		bdev->rds_info.rds_ps[index] = bdev->rds_info.radio_text[i+1];
-		bdev->rds_info.rds_ps[index+1] =
-			bdev->rds_info.radio_text[i+2];
+		bdev->rds_info.rds_ps[index] =
+			bdev->rds_info.radio_text[i + 1];
+		bdev->rds_info.rds_ps[index + 1] =
+			bdev->rds_info.radio_text[i + 2];
 	}
 }
 
 static int bcm2048_parse_ps_match_c(struct bcm2048_device *bdev, int i,
-					int index)
+				    int index)
 {
 	int crc;
 
@@ -1650,14 +1635,14 @@ static int bcm2048_parse_ps_match_c(struct bcm2048_device *bdev, int i,
 		return 0;
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
-		BCM2048_RDS_BLOCK_C)
+	    BCM2048_RDS_BLOCK_C)
 		return 1;
 
 	return 0;
 }
 
 static void bcm2048_parse_ps_match_d(struct bcm2048_device *bdev, int i,
-					int index)
+				     int index)
 {
 	int crc;
 
@@ -1667,7 +1652,7 @@ static void bcm2048_parse_ps_match_d(struct bcm2048_device *bdev, int i,
 		return;
 
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
-		BCM2048_RDS_BLOCK_D)
+	    BCM2048_RDS_BLOCK_D)
 		bcm2048_parse_rds_ps_block(bdev, i, index, crc);
 }
 
@@ -1682,10 +1667,10 @@ static int bcm2048_parse_ps_match_b(struct bcm2048_device *bdev, int i)
 
 	/* Block B Radio PS match */
 	if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
-		BCM2048_RDS_BLOCK_B) {
-		ps_id = bdev->rds_info.radio_text[i+1] &
+	    BCM2048_RDS_BLOCK_B) {
+		ps_id = bdev->rds_info.radio_text[i + 1] &
 			BCM2048_RDS_BLOCK_MASK;
-		ps_group = bdev->rds_info.radio_text[i+1] &
+		ps_group = bdev->rds_info.radio_text[i + 1] &
 			BCM2048_RDS_GROUP_AB_MASK;
 
 		/*
@@ -1709,7 +1694,7 @@ static int bcm2048_parse_ps_match_b(struct bcm2048_device *bdev, int i)
 		}
 
 		if (ps_id == BCM2048_RDS_PS) {
-			index = bdev->rds_info.radio_text[i+2] &
+			index = bdev->rds_info.radio_text[i + 2] &
 				BCM2048_RDS_PS_INDEX;
 			index <<= 1;
 			return index;
@@ -1724,7 +1709,6 @@ static void bcm2048_parse_rds_ps(struct bcm2048_device *bdev)
 	int i, index = 0, crc, match_b = 0, match_c = 0, match_d = 0;
 
 	for (i = 0; i < bdev->fifo_size; i += BCM2048_RDS_FIFO_DUPLE_SIZE) {
-
 		if (match_b) {
 			match_b = 0;
 			index = bcm2048_parse_ps_match_b(bdev, i);
@@ -1743,16 +1727,16 @@ static void bcm2048_parse_rds_ps(struct bcm2048_device *bdev)
 		}
 
 		/* Skip erroneous blocks due to messed up A block altogether */
-		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK)
-			== BCM2048_RDS_BLOCK_A) {
+		if ((bdev->rds_info.radio_text[i] & BCM2048_RDS_BLOCK_MASK) ==
+		    BCM2048_RDS_BLOCK_A) {
 			crc = bcm2048_rds_block_crc(bdev, i);
 			if (crc == BCM2048_RDS_CRC_UNRECOVARABLE)
 				continue;
-			/* Syncronize to a good RDS PI */
-			if (((bdev->rds_info.radio_text[i+1] << 8) +
-				bdev->rds_info.radio_text[i+2]) ==
-				bdev->rds_info.rds_pi)
-					match_b = 1;
+			/* Synchronize to a good RDS PI */
+			if (((bdev->rds_info.radio_text[i + 1] << 8) +
+			    bdev->rds_info.radio_text[i + 2]) ==
+			    bdev->rds_info.rds_pi)
+				match_b = 1;
 		}
 	}
 }
@@ -1764,7 +1748,7 @@ static void bcm2048_rds_fifo_receive(struct bcm2048_device *bdev)
 	mutex_lock(&bdev->mutex);
 
 	err = bcm2048_recv_duples(bdev, BCM2048_I2C_RDS_DATA,
-				bdev->rds_info.radio_text, bdev->fifo_size);
+				  bdev->rds_info.radio_text, bdev->fifo_size);
 	if (err != 2) {
 		dev_err(&bdev->client->dev, "RDS Read problem\n");
 		mutex_unlock(&bdev->mutex);
@@ -1801,8 +1785,8 @@ static int bcm2048_get_rds_data(struct bcm2048_device *bdev, char *data)
 	}
 
 	for (i = 0; i < bdev->rds_info.text_len; i++) {
-		p += sprintf(data_buffer+p, "%x ",
-			bdev->rds_info.radio_text[i]);
+		p += sprintf(data_buffer + p, "%x ",
+			     bdev->rds_info.radio_text[i]);
 	}
 
 	memcpy(data, data_buffer, p);
@@ -1829,7 +1813,7 @@ static int bcm2048_init(struct bcm2048_device *bdev)
 		goto exit;
 
 	err = bcm2048_set_dac_output(bdev, BCM2048_DAC_OUTPUT_LEFT |
-		BCM2048_DAC_OUTPUT_RIGHT);
+				     BCM2048_DAC_OUTPUT_RIGHT);
 
 exit:
 	return err;
@@ -1921,7 +1905,6 @@ static void bcm2048_work(struct work_struct *work)
 
 	if (flag_lsb & (BCM2048_FM_FLAG_SEARCH_TUNE_FINISHED |
 			BCM2048_FM_FLAG_SEARCH_TUNE_FAIL)) {
-
 		if (flag_lsb & BCM2048_FM_FLAG_SEARCH_TUNE_FAIL)
 			bdev->scan_state = BCM2048_SCAN_FAIL;
 		else
@@ -1935,7 +1918,7 @@ static void bcm2048_work(struct work_struct *work)
 		if (bdev->rds_state) {
 			flags =	BCM2048_RDS_FLAG_FIFO_WLINE;
 			bcm2048_send_command(bdev, BCM2048_I2C_FM_RDS_MASK1,
-						flags);
+					     flags);
 		}
 		bdev->rds_data_available = 1;
 		bdev->rd_index = 0; /* new data, new start */
@@ -2074,7 +2057,7 @@ property_str_read(rds_rt, (BCM2048_MAX_RDS_RT + 1))
 property_str_read(rds_ps, (BCM2048_MAX_RDS_PS + 1))
 
 property_read(fm_rds_flags, unsigned int, "%u")
-property_str_read(rds_data, BCM2048_MAX_RDS_RADIO_TEXT*5)
+property_str_read(rds_data, BCM2048_MAX_RDS_RADIO_TEXT * 5)
 
 property_read(region_bottom_frequency, unsigned int, "%u")
 property_read(region_top_frequency, unsigned int, "%u")
@@ -2084,70 +2067,70 @@ DEFINE_SYSFS_PROPERTY(region, unsigned, int, "%u", 0)
 
 static struct device_attribute attrs[] = {
 	__ATTR(power_state, S_IRUGO | S_IWUSR, bcm2048_power_state_read,
-		bcm2048_power_state_write),
+	       bcm2048_power_state_write),
 	__ATTR(mute, S_IRUGO | S_IWUSR, bcm2048_mute_read,
-		bcm2048_mute_write),
+	       bcm2048_mute_write),
 	__ATTR(audio_route, S_IRUGO | S_IWUSR, bcm2048_audio_route_read,
-		bcm2048_audio_route_write),
+	       bcm2048_audio_route_write),
 	__ATTR(dac_output, S_IRUGO | S_IWUSR, bcm2048_dac_output_read,
-		bcm2048_dac_output_write),
+	       bcm2048_dac_output_write),
 	__ATTR(fm_hi_lo_injection, S_IRUGO | S_IWUSR,
-		bcm2048_fm_hi_lo_injection_read,
-		bcm2048_fm_hi_lo_injection_write),
+	       bcm2048_fm_hi_lo_injection_read,
+	       bcm2048_fm_hi_lo_injection_write),
 	__ATTR(fm_frequency, S_IRUGO | S_IWUSR, bcm2048_fm_frequency_read,
-		bcm2048_fm_frequency_write),
+	       bcm2048_fm_frequency_write),
 	__ATTR(fm_af_frequency, S_IRUGO | S_IWUSR,
-		bcm2048_fm_af_frequency_read,
-		bcm2048_fm_af_frequency_write),
+	       bcm2048_fm_af_frequency_read,
+	       bcm2048_fm_af_frequency_write),
 	__ATTR(fm_deemphasis, S_IRUGO | S_IWUSR, bcm2048_fm_deemphasis_read,
-		bcm2048_fm_deemphasis_write),
+	       bcm2048_fm_deemphasis_write),
 	__ATTR(fm_rds_mask, S_IRUGO | S_IWUSR, bcm2048_fm_rds_mask_read,
-		bcm2048_fm_rds_mask_write),
+	       bcm2048_fm_rds_mask_write),
 	__ATTR(fm_best_tune_mode, S_IRUGO | S_IWUSR,
-		bcm2048_fm_best_tune_mode_read,
-		bcm2048_fm_best_tune_mode_write),
+	       bcm2048_fm_best_tune_mode_read,
+	       bcm2048_fm_best_tune_mode_write),
 	__ATTR(fm_search_rssi_threshold, S_IRUGO | S_IWUSR,
-		bcm2048_fm_search_rssi_threshold_read,
-		bcm2048_fm_search_rssi_threshold_write),
+	       bcm2048_fm_search_rssi_threshold_read,
+	       bcm2048_fm_search_rssi_threshold_write),
 	__ATTR(fm_search_mode_direction, S_IRUGO | S_IWUSR,
-		bcm2048_fm_search_mode_direction_read,
-		bcm2048_fm_search_mode_direction_write),
+	       bcm2048_fm_search_mode_direction_read,
+	       bcm2048_fm_search_mode_direction_write),
 	__ATTR(fm_search_tune_mode, S_IRUGO | S_IWUSR,
-		bcm2048_fm_search_tune_mode_read,
-		bcm2048_fm_search_tune_mode_write),
+	       bcm2048_fm_search_tune_mode_read,
+	       bcm2048_fm_search_tune_mode_write),
 	__ATTR(rds, S_IRUGO | S_IWUSR, bcm2048_rds_read,
-		bcm2048_rds_write),
+	       bcm2048_rds_write),
 	__ATTR(rds_b_block_mask, S_IRUGO | S_IWUSR,
-		bcm2048_rds_b_block_mask_read,
-		bcm2048_rds_b_block_mask_write),
+	       bcm2048_rds_b_block_mask_read,
+	       bcm2048_rds_b_block_mask_write),
 	__ATTR(rds_b_block_match, S_IRUGO | S_IWUSR,
-		bcm2048_rds_b_block_match_read,
-		bcm2048_rds_b_block_match_write),
+	       bcm2048_rds_b_block_match_read,
+	       bcm2048_rds_b_block_match_write),
 	__ATTR(rds_pi_mask, S_IRUGO | S_IWUSR, bcm2048_rds_pi_mask_read,
-		bcm2048_rds_pi_mask_write),
+	       bcm2048_rds_pi_mask_write),
 	__ATTR(rds_pi_match, S_IRUGO | S_IWUSR, bcm2048_rds_pi_match_read,
-		bcm2048_rds_pi_match_write),
+	       bcm2048_rds_pi_match_write),
 	__ATTR(rds_wline, S_IRUGO | S_IWUSR, bcm2048_rds_wline_read,
-		bcm2048_rds_wline_write),
+	       bcm2048_rds_wline_write),
 	__ATTR(rds_pi, S_IRUGO, bcm2048_rds_pi_read, NULL),
 	__ATTR(rds_rt, S_IRUGO, bcm2048_rds_rt_read, NULL),
 	__ATTR(rds_ps, S_IRUGO, bcm2048_rds_ps_read, NULL),
 	__ATTR(fm_rds_flags, S_IRUGO, bcm2048_fm_rds_flags_read, NULL),
 	__ATTR(region_bottom_frequency, S_IRUGO,
-		bcm2048_region_bottom_frequency_read, NULL),
+	       bcm2048_region_bottom_frequency_read, NULL),
 	__ATTR(region_top_frequency, S_IRUGO,
-		bcm2048_region_top_frequency_read, NULL),
+	       bcm2048_region_top_frequency_read, NULL),
 	__ATTR(fm_carrier_error, S_IRUGO,
-		bcm2048_fm_carrier_error_read, NULL),
+	       bcm2048_fm_carrier_error_read, NULL),
 	__ATTR(fm_rssi, S_IRUGO,
-		bcm2048_fm_rssi_read, NULL),
+	       bcm2048_fm_rssi_read, NULL),
 	__ATTR(region, S_IRUGO | S_IWUSR, bcm2048_region_read,
-		bcm2048_region_write),
+	       bcm2048_region_write),
 	__ATTR(rds_data, S_IRUGO, bcm2048_rds_data_read, NULL),
 };
 
 static int bcm2048_sysfs_unregister_properties(struct bcm2048_device *bdev,
-						int size)
+					       int size)
 {
 	int i;
 
@@ -2165,7 +2148,7 @@ static int bcm2048_sysfs_register_properties(struct bcm2048_device *bdev)
 	for (i = 0; i < ARRAY_SIZE(attrs); i++) {
 		if (device_create_file(&bdev->client->dev, &attrs[i]) != 0) {
 			dev_err(&bdev->client->dev,
-					"could not register sysfs entry\n");
+				"could not register sysfs entry\n");
 			err = -EBUSY;
 			bcm2048_sysfs_unregister_properties(bdev, i);
 			break;
@@ -2175,7 +2158,6 @@ static int bcm2048_sysfs_register_properties(struct bcm2048_device *bdev)
 	return err;
 }
 
-
 static int bcm2048_fops_open(struct file *file)
 {
 	struct bcm2048_device *bdev = video_drvdata(file);
@@ -2197,7 +2179,7 @@ static int bcm2048_fops_release(struct file *file)
 }
 
 static unsigned int bcm2048_fops_poll(struct file *file,
-		struct poll_table_struct *pts)
+				      struct poll_table_struct *pts)
 {
 	struct bcm2048_device *bdev = video_drvdata(file);
 	int retval = 0;
@@ -2211,7 +2193,7 @@ static unsigned int bcm2048_fops_poll(struct file *file,
 }
 
 static ssize_t bcm2048_fops_read(struct file *file, char __user *buf,
-	size_t count, loff_t *ppos)
+				 size_t count, loff_t *ppos)
 {
 	struct bcm2048_device *bdev = video_drvdata(file);
 	int i;
@@ -2229,7 +2211,7 @@ static ssize_t bcm2048_fops_read(struct file *file, char __user *buf,
 		}
 		/* interruptible_sleep_on(&bdev->read_queue); */
 		if (wait_event_interruptible(bdev->read_queue,
-			bdev->rds_data_available) < 0) {
+		    bdev->rds_data_available) < 0) {
 			retval = -EINTR;
 			goto done;
 		}
@@ -2245,13 +2227,16 @@ static ssize_t bcm2048_fops_read(struct file *file, char __user *buf,
 	while (i < count) {
 		unsigned char tmpbuf[3];
 
-		tmpbuf[i] = bdev->rds_info.radio_text[bdev->rd_index+i+2];
-		tmpbuf[i+1] = bdev->rds_info.radio_text[bdev->rd_index+i+1];
-		tmpbuf[i+2] = (bdev->rds_info.radio_text[bdev->rd_index + i] & 0xf0) >> 4;
-		if ((bdev->rds_info.radio_text[bdev->rd_index+i] &
-			BCM2048_RDS_CRC_MASK) == BCM2048_RDS_CRC_UNRECOVARABLE)
-			tmpbuf[i+2] |= 0x80;
-		if (copy_to_user(buf+i, tmpbuf, 3)) {
+		tmpbuf[i] = bdev->rds_info.radio_text[bdev->rd_index + i + 2];
+		tmpbuf[i + 1] =
+			bdev->rds_info.radio_text[bdev->rd_index + i + 1];
+		tmpbuf[i + 2] =
+			(bdev->rds_info.radio_text[bdev->rd_index + i] &
+			 0xf0) >> 4;
+		if ((bdev->rds_info.radio_text[bdev->rd_index + i] &
+		    BCM2048_RDS_CRC_MASK) == BCM2048_RDS_CRC_UNRECOVARABLE)
+			tmpbuf[i + 2] |= 0x80;
+		if (copy_to_user(buf + i, tmpbuf, 3)) {
 			retval = -EFAULT;
 			break;
 		}
@@ -2319,7 +2304,7 @@ static struct v4l2_queryctrl bcm2048_v4l2_queryctrl[] = {
 };
 
 static int bcm2048_vidioc_querycap(struct file *file, void *priv,
-		struct v4l2_capability *capability)
+				   struct v4l2_capability *capability)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 
@@ -2337,7 +2322,7 @@ static int bcm2048_vidioc_querycap(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_g_input(struct file *filp, void *priv,
-		unsigned int *i)
+				  unsigned int *i)
 {
 	*i = 0;
 
@@ -2345,7 +2330,7 @@ static int bcm2048_vidioc_g_input(struct file *filp, void *priv,
 }
 
 static int bcm2048_vidioc_s_input(struct file *filp, void *priv,
-					unsigned int i)
+				  unsigned int i)
 {
 	if (i)
 		return -EINVAL;
@@ -2354,7 +2339,7 @@ static int bcm2048_vidioc_s_input(struct file *filp, void *priv,
 }
 
 static int bcm2048_vidioc_queryctrl(struct file *file, void *priv,
-		struct v4l2_queryctrl *qc)
+				    struct v4l2_queryctrl *qc)
 {
 	int i;
 
@@ -2369,7 +2354,7 @@ static int bcm2048_vidioc_queryctrl(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_g_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+				 struct v4l2_control *ctrl)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	int err = 0;
@@ -2389,7 +2374,7 @@ static int bcm2048_vidioc_g_ctrl(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_s_ctrl(struct file *file, void *priv,
-		struct v4l2_control *ctrl)
+				 struct v4l2_control *ctrl)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	int err = 0;
@@ -2417,7 +2402,7 @@ static int bcm2048_vidioc_s_ctrl(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_g_audio(struct file *file, void *priv,
-		struct v4l2_audio *audio)
+				  struct v4l2_audio *audio)
 {
 	if (audio->index > 1)
 		return -EINVAL;
@@ -2429,7 +2414,7 @@ static int bcm2048_vidioc_g_audio(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_s_audio(struct file *file, void *priv,
-		const struct v4l2_audio *audio)
+				  const struct v4l2_audio *audio)
 {
 	if (audio->index != 0)
 		return -EINVAL;
@@ -2438,7 +2423,7 @@ static int bcm2048_vidioc_s_audio(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_g_tuner(struct file *file, void *priv,
-		struct v4l2_tuner *tuner)
+				  struct v4l2_tuner *tuner)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	s8 f_error;
@@ -2493,7 +2478,7 @@ static int bcm2048_vidioc_g_tuner(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_s_tuner(struct file *file, void *priv,
-		const struct v4l2_tuner *tuner)
+				  const struct v4l2_tuner *tuner)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 
@@ -2507,7 +2492,7 @@ static int bcm2048_vidioc_s_tuner(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_g_frequency(struct file *file, void *priv,
-		struct v4l2_frequency *freq)
+				      struct v4l2_frequency *freq)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	int err = 0;
@@ -2528,7 +2513,7 @@ static int bcm2048_vidioc_g_frequency(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_s_frequency(struct file *file, void *priv,
-		const struct v4l2_frequency *freq)
+				      const struct v4l2_frequency *freq)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	int err;
@@ -2546,7 +2531,7 @@ static int bcm2048_vidioc_s_frequency(struct file *file, void *priv,
 }
 
 static int bcm2048_vidioc_s_hw_freq_seek(struct file *file, void *priv,
-					const struct v4l2_hw_freq_seek *seek)
+					 const struct v4l2_hw_freq_seek *seek)
 {
 	struct bcm2048_device *bdev = video_get_drvdata(video_devdata(file));
 	int err;
@@ -2559,7 +2544,7 @@ static int bcm2048_vidioc_s_hw_freq_seek(struct file *file, void *priv,
 
 	err = bcm2048_set_fm_search_mode_direction(bdev, seek->seek_upward);
 	err |= bcm2048_set_fm_search_tune_mode(bdev,
-			BCM2048_FM_AUTO_SEARCH_MODE);
+					       BCM2048_FM_AUTO_SEARCH_MODE);
 
 	return err;
 }
@@ -2594,7 +2579,7 @@ static struct video_device bcm2048_viddev_template = {
  *	I2C driver interface
  */
 static int bcm2048_i2c_driver_probe(struct i2c_client *client,
-					const struct i2c_device_id *id)
+				    const struct i2c_device_id *id)
 {
 	struct bcm2048_device *bdev;
 	int err;
@@ -2613,8 +2598,8 @@ static int bcm2048_i2c_driver_probe(struct i2c_client *client,
 
 	if (client->irq) {
 		err = request_irq(client->irq,
-			bcm2048_handler, IRQF_TRIGGER_FALLING,
-			client->name, bdev);
+				  bcm2048_handler, IRQF_TRIGGER_FALLING,
+				  client->name, bdev);
 		if (err < 0) {
 			dev_err(&client->dev, "Could not request IRQ\n");
 			goto free_bdev;
diff --git a/drivers/staging/media/davinci_vpfe/Kconfig b/drivers/staging/media/davinci_vpfe/Kconfig
index 4de2f082491d..f40a06954a92 100644
--- a/drivers/staging/media/davinci_vpfe/Kconfig
+++ b/drivers/staging/media/davinci_vpfe/Kconfig
@@ -2,6 +2,8 @@ config VIDEO_DM365_VPFE
 	tristate "DM365 VPFE Media Controller Capture Driver"
 	depends on VIDEO_V4L2 && ARCH_DAVINCI_DM365 && !VIDEO_DM365_ISIF
 	depends on HAS_DMA
+	depends on VIDEO_V4L2_SUBDEV_API
+	depends on VIDEO_DAVINCI_VPBE_DISPLAY
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Support for DM365 VPFE based Media Controller Capture driver.
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
index b1dfa2ccc4ef..ac78ed2f8bcc 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe.c
@@ -1536,8 +1536,9 @@ ipipe_get_format(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
  * @fse: pointer to v4l2_subdev_frame_size_enum structure.
  */
 static int
-ipipe_enum_frame_size(struct v4l2_subdev *sd, struct v4l2_subdev_pad_config *cfg,
-			  struct v4l2_subdev_frame_size_enum *fse)
+ipipe_enum_frame_size(struct v4l2_subdev *sd,
+		       struct v4l2_subdev_pad_config *cfg,
+		       struct v4l2_subdev_frame_size_enum *fse)
 {
 	struct vpfe_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
 	struct v4l2_mbus_framefmt format;
@@ -1711,8 +1712,11 @@ ipipe_link_setup(struct media_entity *entity, const struct media_pad *local,
 	struct vpfe_device *vpfe_dev = to_vpfe_device(ipipe);
 	u16 ipipeif_sink = vpfe_dev->vpfe_ipipeif.input;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case IPIPE_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	if (!is_media_entity_v4l2_subdev(remote->entity))
+		return -EINVAL;
+
+	switch (local->index) {
+	case IPIPE_PAD_SINK:
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipe->input = IPIPE_INPUT_NONE;
 			break;
@@ -1725,7 +1729,7 @@ ipipe_link_setup(struct media_entity *entity, const struct media_pad *local,
 			ipipe->input = IPIPE_INPUT_CCDC;
 		break;
 
-	case IPIPE_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case IPIPE_PAD_SOURCE:
 		/* out to RESIZER */
 		if (flags & MEDIA_LNK_FL_ENABLED)
 			ipipe->output = IPIPE_OUTPUT_RESIZER;
@@ -1839,7 +1843,7 @@ vpfe_ipipe_init(struct vpfe_ipipe_device *ipipe, struct platform_device *pdev)
 	v4l2_ctrl_handler_setup(&ipipe->ctrls);
 	sd->ctrl_handler = &ipipe->ctrls;
 
-	return media_entity_init(me, IPIPE_PADS_NUM, pads, 0);
+	return media_entity_pads_init(me, IPIPE_PADS_NUM, pads);
 }
 
 /*
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
index 2a3a56b88de1..b1d5e23ae6e0 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipe_hw.c
@@ -254,7 +254,7 @@ int config_ipipe_hw(struct vpfe_ipipe_device *ipipe)
 	void __iomem *ipipe_base = ipipe->base_addr;
 	struct v4l2_mbus_framefmt *outformat;
 	u32 color_pat;
-	u32 ipipe_mode;
+	int ipipe_mode;
 	u32 data_path;
 
 	/* enable clock to IPIPE */
diff --git a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
index 8b230541b1d1..633d6456fdce 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_ipipeif.c
@@ -885,9 +885,14 @@ ipipeif_link_setup(struct media_entity *entity, const struct media_pad *local,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct vpfe_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
 	struct vpfe_device *vpfe = to_vpfe_device(ipipeif);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case IPIPEIF_PAD_SINK | MEDIA_ENT_T_DEVNODE:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case IPIPEIF_PAD_SINK:
 		/* Single shot mode */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipeif->input = IPIPEIF_INPUT_NONE;
@@ -896,7 +901,7 @@ ipipeif_link_setup(struct media_entity *entity, const struct media_pad *local,
 		ipipeif->input = IPIPEIF_INPUT_MEMORY;
 		break;
 
-	case IPIPEIF_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	case IPIPEIF_PAD_SINK | 2 << 16:
 		/* read from isif */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipeif->input = IPIPEIF_INPUT_NONE;
@@ -908,7 +913,7 @@ ipipeif_link_setup(struct media_entity *entity, const struct media_pad *local,
 		ipipeif->input = IPIPEIF_INPUT_ISIF;
 		break;
 
-	case IPIPEIF_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case IPIPEIF_PAD_SOURCE | 2 << 16:
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipeif->output = IPIPEIF_OUTPUT_NONE;
 			break;
@@ -971,7 +976,7 @@ vpfe_ipipeif_register_entities(struct vpfe_ipipeif_device *ipipeif,
 	ipipeif->video_in.vpfe_dev = vpfe_dev;
 
 	flags = 0;
-	ret = media_entity_create_link(&ipipeif->video_in.video_dev.entity, 0,
+	ret = media_create_pad_link(&ipipeif->video_in.video_dev.entity, 0,
 					&ipipeif->subdev.entity, 0, flags);
 	if (ret < 0)
 		goto fail;
@@ -1026,7 +1031,7 @@ int vpfe_ipipeif_init(struct vpfe_ipipeif_device *ipipeif,
 	ipipeif->output = IPIPEIF_OUTPUT_NONE;
 	me->ops = &ipipeif_media_ops;
 
-	ret = media_entity_init(me, IPIPEIF_NUM_PADS, pads, 0);
+	ret = media_entity_pads_init(me, IPIPEIF_NUM_PADS, pads);
 	if (ret)
 		goto fail;
 
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index 80907b464412..99057892d88d 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -1707,9 +1707,14 @@ isif_link_setup(struct media_entity *entity, const struct media_pad *local,
 {
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct vpfe_isif_device *isif = v4l2_get_subdevdata(sd);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case ISIF_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case ISIF_PAD_SINK | 2 << 16:
 		/* read from decoder/sensor */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			isif->input = ISIF_INPUT_NONE;
@@ -1720,7 +1725,7 @@ isif_link_setup(struct media_entity *entity, const struct media_pad *local,
 		isif->input = ISIF_INPUT_PARALLEL;
 		break;
 
-	case ISIF_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+	case ISIF_PAD_SOURCE:
 		/* write to memory */
 		if (flags & MEDIA_LNK_FL_ENABLED)
 			isif->output = ISIF_OUTPUT_MEMORY;
@@ -1728,7 +1733,7 @@ isif_link_setup(struct media_entity *entity, const struct media_pad *local,
 			isif->output = ISIF_OUTPUT_NONE;
 		break;
 
-	case ISIF_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case ISIF_PAD_SOURCE | 2 << 16:
 		if (flags & MEDIA_LNK_FL_ENABLED)
 			isif->output = ISIF_OUTPUT_IPIPEIF;
 		else
@@ -1817,7 +1822,7 @@ int vpfe_isif_register_entities(struct vpfe_isif_device *isif,
 	isif->video_out.vpfe_dev = vpfe_dev;
 	flags = 0;
 	/* connect isif to video node */
-	ret = media_entity_create_link(&isif->subdev.entity, 1,
+	ret = media_create_pad_link(&isif->subdev.entity, 1,
 				       &isif->video_out.video_dev.entity,
 				       0, flags);
 	if (ret < 0)
@@ -2052,7 +2057,7 @@ int vpfe_isif_init(struct vpfe_isif_device *isif, struct platform_device *pdev)
 	isif->input = ISIF_INPUT_NONE;
 	isif->output = ISIF_OUTPUT_NONE;
 	me->ops = &isif_media_ops;
-	status = media_entity_init(me, ISIF_PADS_NUM, pads, 0);
+	status = media_entity_pads_init(me, ISIF_PADS_NUM, pads);
 	if (status)
 		goto isif_fail;
 	isif->video_out.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
diff --git a/drivers/staging/media/davinci_vpfe/dm365_resizer.c b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
index acb293ed9c91..a91395ce91e1 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_resizer.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_resizer.c
@@ -63,16 +63,11 @@ resizer_calculate_line_length(u32 pix, int width, int height,
 	if (pix == MEDIA_BUS_FMT_UYVY8_2X8 ||
 	    pix == MEDIA_BUS_FMT_SGRBG12_1X12) {
 		*line_len = width << 1;
-	} else if (pix == MEDIA_BUS_FMT_Y8_1X8 ||
-		   pix == MEDIA_BUS_FMT_UV8_1X8) {
-		*line_len = width;
-		*line_len_c = width;
 	} else {
-		/* YUV 420 */
-		/* round width to upper 32 byte boundary */
 		*line_len = width;
 		*line_len_c = width;
 	}
+
 	/* adjust the line len to be a multiple of 32 */
 	*line_len += 31;
 	*line_len &= ~0x1f;
@@ -1653,10 +1648,15 @@ static int resizer_link_setup(struct media_entity *entity,
 	struct vpfe_device *vpfe_dev = to_vpfe_device(resizer);
 	u16 ipipeif_source = vpfe_dev->vpfe_ipipeif.output;
 	u16 ipipe_source = vpfe_dev->vpfe_ipipe.output;
+	unsigned int index = local->index;
+
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
 
 	if (&resizer->crop_resizer.subdev == sd) {
-		switch (local->index | media_entity_type(remote->entity)) {
-		case RESIZER_CROP_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		switch (index) {
+		case RESIZER_CROP_PAD_SINK | 2 << 16:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->crop_resizer.input =
 					RESIZER_CROP_INPUT_NONE;
@@ -1676,7 +1676,7 @@ static int resizer_link_setup(struct media_entity *entity,
 				return -EINVAL;
 			break;
 
-		case RESIZER_CROP_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+		case RESIZER_CROP_PAD_SOURCE | 2 << 16:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->crop_resizer.output =
 				RESIZER_CROP_OUTPUT_NONE;
@@ -1688,7 +1688,7 @@ static int resizer_link_setup(struct media_entity *entity,
 			resizer->crop_resizer.output = RESIZER_A;
 			break;
 
-		case RESIZER_CROP_PAD_SOURCE2 | MEDIA_ENT_T_V4L2_SUBDEV:
+		case RESIZER_CROP_PAD_SOURCE2 | 2 << 16:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->crop_resizer.output2 =
 					RESIZER_CROP_OUTPUT_NONE;
@@ -1704,8 +1704,8 @@ static int resizer_link_setup(struct media_entity *entity,
 			return -EINVAL;
 		}
 	} else if (&resizer->resizer_a.subdev == sd) {
-		switch (local->index | media_entity_type(remote->entity)) {
-		case RESIZER_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		switch (index) {
+		case RESIZER_PAD_SINK | 2 << 16:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->resizer_a.input = RESIZER_INPUT_NONE;
 				break;
@@ -1715,7 +1715,7 @@ static int resizer_link_setup(struct media_entity *entity,
 			resizer->resizer_a.input = RESIZER_INPUT_CROP_RESIZER;
 			break;
 
-		case RESIZER_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		case RESIZER_PAD_SOURCE:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->resizer_a.output = RESIZER_OUTPUT_NONE;
 				break;
@@ -1729,8 +1729,8 @@ static int resizer_link_setup(struct media_entity *entity,
 			return -EINVAL;
 		}
 	} else if (&resizer->resizer_b.subdev == sd) {
-		switch (local->index | media_entity_type(remote->entity)) {
-		case RESIZER_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+		switch (index) {
+		case RESIZER_PAD_SINK | 2 << 16:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->resizer_b.input = RESIZER_INPUT_NONE;
 				break;
@@ -1740,7 +1740,7 @@ static int resizer_link_setup(struct media_entity *entity,
 			resizer->resizer_b.input = RESIZER_INPUT_CROP_RESIZER;
 			break;
 
-		case RESIZER_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+		case RESIZER_PAD_SOURCE:
 			if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 				resizer->resizer_b.output = RESIZER_OUTPUT_NONE;
 				break;
@@ -1831,27 +1831,27 @@ int vpfe_resizer_register_entities(struct vpfe_resizer_device *resizer,
 	resizer->resizer_b.video_out.vpfe_dev = vpfe_dev;
 
 	/* create link between Resizer Crop----> Resizer A*/
-	ret = media_entity_create_link(&resizer->crop_resizer.subdev.entity, 1,
+	ret = media_create_pad_link(&resizer->crop_resizer.subdev.entity, 1,
 				&resizer->resizer_a.subdev.entity,
 				0, flags);
 	if (ret < 0)
 		goto out_create_link;
 
 	/* create link between Resizer Crop----> Resizer B*/
-	ret = media_entity_create_link(&resizer->crop_resizer.subdev.entity, 2,
+	ret = media_create_pad_link(&resizer->crop_resizer.subdev.entity, 2,
 				&resizer->resizer_b.subdev.entity,
 				0, flags);
 	if (ret < 0)
 		goto out_create_link;
 
 	/* create link between Resizer A ----> video out */
-	ret = media_entity_create_link(&resizer->resizer_a.subdev.entity, 1,
+	ret = media_create_pad_link(&resizer->resizer_a.subdev.entity, 1,
 		&resizer->resizer_a.video_out.video_dev.entity, 0, flags);
 	if (ret < 0)
 		goto out_create_link;
 
 	/* create link between Resizer B ----> video out */
-	ret = media_entity_create_link(&resizer->resizer_b.subdev.entity, 1,
+	ret = media_create_pad_link(&resizer->resizer_b.subdev.entity, 1,
 		&resizer->resizer_b.video_out.video_dev.entity, 0, flags);
 	if (ret < 0)
 		goto out_create_link;
@@ -1915,7 +1915,7 @@ int vpfe_resizer_init(struct vpfe_resizer_device *vpfe_rsz,
 	vpfe_rsz->crop_resizer.output2 = RESIZER_CROP_OUTPUT_NONE;
 	vpfe_rsz->crop_resizer.rsz_device = vpfe_rsz;
 	me->ops = &resizer_media_ops;
-	ret = media_entity_init(me, RESIZER_CROP_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, RESIZER_CROP_PADS_NUM, pads);
 	if (ret)
 		return ret;
 
@@ -1937,7 +1937,7 @@ int vpfe_resizer_init(struct vpfe_resizer_device *vpfe_rsz,
 	vpfe_rsz->resizer_a.output = RESIZER_OUTPUT_NONE;
 	vpfe_rsz->resizer_a.rsz_device = vpfe_rsz;
 	me->ops = &resizer_media_ops;
-	ret = media_entity_init(me, RESIZER_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, RESIZER_PADS_NUM, pads);
 	if (ret)
 		return ret;
 
@@ -1959,7 +1959,7 @@ int vpfe_resizer_init(struct vpfe_resizer_device *vpfe_rsz,
 	vpfe_rsz->resizer_b.output = RESIZER_OUTPUT_NONE;
 	vpfe_rsz->resizer_b.rsz_device = vpfe_rsz;
 	me->ops = &resizer_media_ops;
-	ret = media_entity_init(me, RESIZER_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, RESIZER_PADS_NUM, pads);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index 01df0683e950..ec46f366dd17 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -227,7 +227,7 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 		return 0;
 
 	vpfe_dev->clks = kcalloc(vpfe_cfg->num_clocks,
-				 sizeof(struct clock *), GFP_KERNEL);
+				 sizeof(*vpfe_dev->clks), GFP_KERNEL);
 	if (vpfe_dev->clks == NULL)
 		return -ENOMEM;
 
@@ -445,32 +445,32 @@ static int vpfe_register_entities(struct vpfe_device *vpfe_dev)
 		/* if entity has no pads (ex: amplifier),
 		   cant establish link */
 		if (vpfe_dev->sd[i]->entity.num_pads) {
-			ret = media_entity_create_link(&vpfe_dev->sd[i]->entity,
+			ret = media_create_pad_link(&vpfe_dev->sd[i]->entity,
 				0, &vpfe_dev->vpfe_isif.subdev.entity,
 				0, flags);
 			if (ret < 0)
 				goto out_resizer_register;
 		}
 
-	ret = media_entity_create_link(&vpfe_dev->vpfe_isif.subdev.entity, 1,
+	ret = media_create_pad_link(&vpfe_dev->vpfe_isif.subdev.entity, 1,
 				       &vpfe_dev->vpfe_ipipeif.subdev.entity,
 				       0, flags);
 	if (ret < 0)
 		goto out_resizer_register;
 
-	ret = media_entity_create_link(&vpfe_dev->vpfe_ipipeif.subdev.entity, 1,
+	ret = media_create_pad_link(&vpfe_dev->vpfe_ipipeif.subdev.entity, 1,
 				       &vpfe_dev->vpfe_ipipe.subdev.entity,
 				       0, flags);
 	if (ret < 0)
 		goto out_resizer_register;
 
-	ret = media_entity_create_link(&vpfe_dev->vpfe_ipipe.subdev.entity,
+	ret = media_create_pad_link(&vpfe_dev->vpfe_ipipe.subdev.entity,
 			1, &vpfe_dev->vpfe_resizer.crop_resizer.subdev.entity,
 			0, flags);
 	if (ret < 0)
 		goto out_resizer_register;
 
-	ret = media_entity_create_link(&vpfe_dev->vpfe_ipipeif.subdev.entity, 1,
+	ret = media_create_pad_link(&vpfe_dev->vpfe_ipipeif.subdev.entity, 1,
 			&vpfe_dev->vpfe_resizer.crop_resizer.subdev.entity,
 			0, flags);
 	if (ret < 0)
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.c b/drivers/staging/media/davinci_vpfe/vpfe_video.c
index 0fdff91624fd..db49af90217e 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.c
@@ -88,7 +88,7 @@ vpfe_video_remote_subdev(struct vpfe_video_device *video, u32 *pad)
 {
 	struct media_pad *remote = media_entity_remote_pad(&video->pad);
 
-	if (remote == NULL || remote->entity->type != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 		return NULL;
 	if (pad)
 		*pad = remote->index;
@@ -127,13 +127,14 @@ __vpfe_video_get_format(struct vpfe_video_device *video,
 }
 
 /* make a note of pipeline details */
-static void vpfe_prepare_pipeline(struct vpfe_video_device *video)
+static int vpfe_prepare_pipeline(struct vpfe_video_device *video)
 {
+	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video_dev.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 	struct vpfe_pipeline *pipe = &video->pipe;
 	struct vpfe_video_device *far_end = NULL;
-	struct media_entity_graph graph;
+	int ret;
 
 	pipe->input_num = 0;
 	pipe->output_num = 0;
@@ -144,11 +145,16 @@ static void vpfe_prepare_pipeline(struct vpfe_video_device *video)
 		pipe->outputs[pipe->output_num++] = video;
 
 	mutex_lock(&mdev->graph_mutex);
+	ret = media_entity_graph_walk_init(&graph, entity->graph_obj.mdev);
+	if (ret) {
+		mutex_unlock(&mdev->graph_mutex);
+		return -ENOMEM;
+	}
 	media_entity_graph_walk_start(&graph, entity);
 	while ((entity = media_entity_graph_walk_next(&graph))) {
 		if (entity == &video->video_dev.entity)
 			continue;
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+		if (!is_media_entity_v4l2_io(entity))
 			continue;
 		far_end = to_vpfe_video(media_entity_to_video_device(entity));
 		if (far_end->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
@@ -156,7 +162,10 @@ static void vpfe_prepare_pipeline(struct vpfe_video_device *video)
 		else
 			pipe->outputs[pipe->output_num++] = far_end;
 	}
+	media_entity_graph_walk_cleanup(&graph);
 	mutex_unlock(&mdev->graph_mutex);
+
+	return 0;
 }
 
 /* update pipe state selected by user */
@@ -165,7 +174,9 @@ static int vpfe_update_pipe_state(struct vpfe_video_device *video)
 	struct vpfe_pipeline *pipe = &video->pipe;
 	int ret;
 
-	vpfe_prepare_pipeline(video);
+	ret = vpfe_prepare_pipeline(video);
+	if (ret)
+		return ret;
 
 	/* Find out if there is any input video
 	  if yes, it is single shot.
@@ -243,8 +254,7 @@ static int vpfe_video_validate_pipeline(struct vpfe_pipeline *pipe)
 
 		/* Retrieve the source format */
 		pad = media_entity_remote_pad(pad);
-		if (pad == NULL ||
-			pad->entity->type != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		subdev = media_entity_to_v4l2_subdev(pad->entity);
@@ -277,29 +287,35 @@ static int vpfe_video_validate_pipeline(struct vpfe_pipeline *pipe)
  */
 static int vpfe_pipeline_enable(struct vpfe_pipeline *pipe)
 {
-	struct media_entity_graph graph;
 	struct media_entity *entity;
 	struct v4l2_subdev *subdev;
 	struct media_device *mdev;
-	int ret = 0;
+	int ret;
 
 	if (pipe->state == VPFE_PIPELINE_STREAM_CONTINUOUS)
 		entity = vpfe_get_input_entity(pipe->outputs[0]);
 	else
 		entity = &pipe->inputs[0]->video_dev.entity;
 
-	mdev = entity->parent;
+	mdev = entity->graph_obj.mdev;
 	mutex_lock(&mdev->graph_mutex);
-	media_entity_graph_walk_start(&graph, entity);
-	while ((entity = media_entity_graph_walk_next(&graph))) {
+	ret = media_entity_graph_walk_init(&pipe->graph,
+					   entity->graph_obj.mdev);
+	if (ret)
+		goto out;
+	media_entity_graph_walk_start(&pipe->graph, entity);
+	while ((entity = media_entity_graph_walk_next(&pipe->graph))) {
 
-		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+		if (!is_media_entity_v4l2_subdev(entity))
 			continue;
 		subdev = media_entity_to_v4l2_subdev(entity);
 		ret = v4l2_subdev_call(subdev, video, s_stream, 1);
 		if (ret < 0 && ret != -ENOIOCTLCMD)
 			break;
 	}
+out:
+	if (ret)
+		media_entity_graph_walk_cleanup(&pipe->graph);
 	mutex_unlock(&mdev->graph_mutex);
 	return ret;
 }
@@ -317,7 +333,6 @@ static int vpfe_pipeline_enable(struct vpfe_pipeline *pipe)
  */
 static int vpfe_pipeline_disable(struct vpfe_pipeline *pipe)
 {
-	struct media_entity_graph graph;
 	struct media_entity *entity;
 	struct v4l2_subdev *subdev;
 	struct media_device *mdev;
@@ -328,13 +343,13 @@ static int vpfe_pipeline_disable(struct vpfe_pipeline *pipe)
 	else
 		entity = &pipe->inputs[0]->video_dev.entity;
 
-	mdev = entity->parent;
+	mdev = entity->graph_obj.mdev;
 	mutex_lock(&mdev->graph_mutex);
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(&pipe->graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
+	while ((entity = media_entity_graph_walk_next(&pipe->graph))) {
 
-		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+		if (!is_media_entity_v4l2_subdev(entity))
 			continue;
 		subdev = media_entity_to_v4l2_subdev(entity);
 		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
@@ -343,6 +358,7 @@ static int vpfe_pipeline_disable(struct vpfe_pipeline *pipe)
 	}
 	mutex_unlock(&mdev->graph_mutex);
 
+	media_entity_graph_walk_cleanup(&pipe->graph);
 	return ret ? -ETIMEDOUT : 0;
 }
 
@@ -470,7 +486,7 @@ void vpfe_video_process_buffer_complete(struct vpfe_video_device *video)
 {
 	struct vpfe_pipeline *pipe = &video->pipe;
 
-	v4l2_get_timestamp(&video->cur_frm->vb.timestamp);
+	video->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns();
 	vb2_buffer_done(&video->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE);
 	if (pipe->state == VPFE_PIPELINE_STREAM_CONTINUOUS)
 		video->cur_frm = video->next_frm;
@@ -1078,7 +1094,7 @@ vpfe_g_dv_timings(struct file *file, void *fh,
  * the buffer nbuffers and buffer size
  */
 static int
-vpfe_buffer_queue_setup(struct vb2_queue *vq, const void *parg,
+vpfe_buffer_queue_setup(struct vb2_queue *vq,
 			unsigned int *nbuffers, unsigned int *nplanes,
 			unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -1600,8 +1616,8 @@ int vpfe_video_init(struct vpfe_video_device *video, const char *name)
 	spin_lock_init(&video->irqlock);
 	spin_lock_init(&video->dma_queue_lock);
 	mutex_init(&video->lock);
-	ret = media_entity_init(&video->video_dev.entity,
-				1, &video->pad, 0);
+	ret = media_entity_pads_init(&video->video_dev.entity,
+				1, &video->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_video.h b/drivers/staging/media/davinci_vpfe/vpfe_video.h
index 673cefe3ef61..653334d537d3 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_video.h
+++ b/drivers/staging/media/davinci_vpfe/vpfe_video.h
@@ -52,6 +52,7 @@ enum vpfe_video_state {
 struct vpfe_pipeline {
 	/* media pipeline */
 	struct media_pipeline		*pipe;
+	struct media_entity_graph	graph;
 	/* state of the pipeline, continuous,
 	 * single-shot or stopped
 	 */
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 534b8103ae80..ff1926ca1f96 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -885,12 +885,14 @@ static int imon_probe(struct usb_interface *interface,
 		vendor, product, ifnum, usbdev->bus->busnum, usbdev->devnum);
 
 	/* Everything went fine. Just unlock and return retval (with is 0) */
+	mutex_unlock(&context->ctx_lock);
 	goto driver_unlock;
 
 unregister_lirc:
 	lirc_unregister_driver(driver->minor);
 
 free_tx_urb:
+	mutex_unlock(&context->ctx_lock);
 	usb_free_urb(tx_urb);
 
 free_rx_urb:
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index c1408342b1d0..d009bcb439f0 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -33,7 +33,7 @@
 #include <linux/fs.h>
 #include <linux/kernel.h>
 #include <linux/ioport.h>
-#include <linux/time.h>
+#include <linux/ktime.h>
 #include <linux/mm.h>
 #include <linux/delay.h>
 
@@ -144,25 +144,22 @@ static void lirc_off(void)
 
 static unsigned int init_lirc_timer(void)
 {
-	struct timeval tv, now;
+	ktime_t kt, now, timeout;
 	unsigned int level, newlevel, timeelapsed, newtimer;
 	int count = 0;
 
-	do_gettimeofday(&tv);
-	tv.tv_sec++;                     /* wait max. 1 sec. */
+	kt = ktime_get();
+	/* wait max. 1 sec. */
+	timeout = ktime_add_ns(kt, NSEC_PER_SEC);
 	level = lirc_get_timer();
 	do {
 		newlevel = lirc_get_timer();
 		if (level == 0 && newlevel != 0)
 			count++;
 		level = newlevel;
-		do_gettimeofday(&now);
-	} while (count < 1000 && (now.tv_sec < tv.tv_sec
-			     || (now.tv_sec == tv.tv_sec
-				 && now.tv_usec < tv.tv_usec)));
-
-	timeelapsed = (now.tv_sec + 1 - tv.tv_sec)*1000000
-		     + (now.tv_usec - tv.tv_usec);
+		now = ktime_get();
+	} while (count < 1000 && (ktime_before(now, timeout)));
+	timeelapsed = ktime_us_delta(now, kt);
 	if (count >= 1000 && timeelapsed > 0) {
 		if (default_timer == 0) {
 			/* autodetect timer */
@@ -220,8 +217,8 @@ static void rbuf_write(int signal)
 
 static void lirc_lirc_irq_handler(void *blah)
 {
-	struct timeval tv;
-	static struct timeval lasttv;
+	ktime_t kt, delkt;
+	static ktime_t lastkt;
 	static int init;
 	long signal;
 	int data;
@@ -244,16 +241,14 @@ static void lirc_lirc_irq_handler(void *blah)
 
 #ifdef LIRC_TIMER
 	if (init) {
-		do_gettimeofday(&tv);
+		kt = ktime_get();
 
-		signal = tv.tv_sec - lasttv.tv_sec;
-		if (signal > 15)
+		delkt = ktime_sub(kt, lastkt);
+		if (ktime_compare(delkt, ktime_set(15, 0)) > 0)
 			/* really long time */
 			data = PULSE_MASK;
 		else
-			data = (int) (signal*1000000 +
-					 tv.tv_usec - lasttv.tv_usec +
-					 LIRC_SFH506_DELAY);
+			data = (int)(ktime_to_us(delkt) + LIRC_SFH506_DELAY);
 
 		rbuf_write(data); /* space */
 	} else {
@@ -301,7 +296,7 @@ static void lirc_lirc_irq_handler(void *blah)
 			data = 1;
 		rbuf_write(PULSE_BIT|data); /* pulse */
 	}
-	do_gettimeofday(&lasttv);
+	lastkt = ktime_get();
 #else
 	/* add your code here */
 #endif
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index f2dca69c2bc0..2218d0042030 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -42,6 +42,7 @@
 #include <linux/slab.h>
 #include <linux/uaccess.h>
 #include <linux/usb.h>
+#include <linux/ktime.h>
 
 #include <media/lirc.h>
 #include <media/lirc_dev.h>
@@ -111,7 +112,7 @@ struct sasem_context {
 	} tx;
 
 	/* for dealing with repeat codes (wish there was a toggle bit!) */
-	struct timeval presstime;
+	ktime_t presstime;
 	char lastcode[8];
 	int codesaved;
 };
@@ -566,8 +567,8 @@ static void incoming_packet(struct sasem_context *context,
 {
 	int len = urb->actual_length;
 	unsigned char *buf = urb->transfer_buffer;
-	long ms;
-	struct timeval tv;
+	u64 ns;
+	ktime_t kt;
 
 	if (len != 8) {
 		dev_warn(&context->dev->dev,
@@ -584,9 +585,8 @@ static void incoming_packet(struct sasem_context *context,
 	 */
 
 	/* get the time since the last button press */
-	do_gettimeofday(&tv);
-	ms = (tv.tv_sec - context->presstime.tv_sec) * 1000 +
-	     (tv.tv_usec - context->presstime.tv_usec) / 1000;
+	kt = ktime_get();
+	ns = ktime_to_ns(ktime_sub(kt, context->presstime));
 
 	if (memcmp(buf, "\x08\0\0\0\0\0\0\0", 8) == 0) {
 		/*
@@ -600,10 +600,9 @@ static void incoming_packet(struct sasem_context *context,
 		 *   in that time and then get a false repeat of the previous
 		 *   press but it is long enough for a genuine repeat
 		 */
-		if ((ms < 250) && (context->codesaved != 0)) {
+		if ((ns < 250 * NSEC_PER_MSEC) && (context->codesaved != 0)) {
 			memcpy(buf, &context->lastcode, 8);
-			context->presstime.tv_sec = tv.tv_sec;
-			context->presstime.tv_usec = tv.tv_usec;
+			context->presstime = kt;
 		}
 	} else {
 		/* save the current valid code for repeats */
@@ -613,8 +612,7 @@ static void incoming_packet(struct sasem_context *context,
 		 * just for safety reasons
 		 */
 		context->codesaved = 1;
-		context->presstime.tv_sec = tv.tv_sec;
-		context->presstime.tv_usec = tv.tv_usec;
+		context->presstime = kt;
 	}
 
 	lirc_buffer_write(context->driver->rbuf, buf);
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index 64a7b2fc5289..b798b311d32c 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -59,7 +59,7 @@
 #include <linux/ioport.h>
 #include <linux/kernel.h>
 #include <linux/serial_reg.h>
-#include <linux/time.h>
+#include <linux/ktime.h>
 #include <linux/string.h>
 #include <linux/types.h>
 #include <linux/wait.h>
@@ -204,7 +204,7 @@ static struct lirc_serial hardware[] = {
 
 #define RBUF_LEN 256
 
-static struct timeval lasttv = {0, 0};
+static ktime_t lastkt;
 
 static struct lirc_buffer rbuf;
 
@@ -542,10 +542,10 @@ static void frbwrite(int l)
 
 static irqreturn_t lirc_irq_handler(int i, void *blah)
 {
-	struct timeval tv;
+	ktime_t kt;
 	int counter, dcd;
 	u8 status;
-	long deltv;
+	ktime_t delkt;
 	int data;
 	static int last_dcd = -1;
 
@@ -565,7 +565,7 @@ static irqreturn_t lirc_irq_handler(int i, void *blah)
 		if ((status & hardware[type].signal_pin_change)
 		    && sense != -1) {
 			/* get current time */
-			do_gettimeofday(&tv);
+			kt = ktime_get();
 
 			/* New mode, written by Trent Piepho
 			   <xyzzy@u.washington.edu>. */
@@ -594,34 +594,20 @@ static irqreturn_t lirc_irq_handler(int i, void *blah)
 			dcd = (status & hardware[type].signal_pin) ? 1 : 0;
 
 			if (dcd == last_dcd) {
-				pr_warn("ignoring spike: %d %d %lx %lx %lx %lx\n",
-					dcd, sense,
-					tv.tv_sec, lasttv.tv_sec,
-					(unsigned long)tv.tv_usec,
-					(unsigned long)lasttv.tv_usec);
+				pr_warn("ignoring spike: %d %d %llx %llx\n",
+					dcd, sense, ktime_to_us(kt),
+					ktime_to_us(lastkt));
 				continue;
 			}
 
-			deltv = tv.tv_sec-lasttv.tv_sec;
-			if (tv.tv_sec < lasttv.tv_sec ||
-			    (tv.tv_sec == lasttv.tv_sec &&
-			     tv.tv_usec < lasttv.tv_usec)) {
-				pr_warn("AIEEEE: your clock just jumped backwards\n");
-				pr_warn("%d %d %lx %lx %lx %lx\n",
-					dcd, sense,
-					tv.tv_sec, lasttv.tv_sec,
-					(unsigned long)tv.tv_usec,
-					(unsigned long)lasttv.tv_usec);
-				data = PULSE_MASK;
-			} else if (deltv > 15) {
+			delkt = ktime_sub(kt, lastkt);
+			if (ktime_compare(delkt, ktime_set(15, 0)) > 0) {
 				data = PULSE_MASK; /* really long time */
 				if (!(dcd^sense)) {
 					/* sanity check */
-					pr_warn("AIEEEE: %d %d %lx %lx %lx %lx\n",
-						dcd, sense,
-						tv.tv_sec, lasttv.tv_sec,
-						(unsigned long)tv.tv_usec,
-						(unsigned long)lasttv.tv_usec);
+					pr_warn("AIEEEE: %d %d %llx %llx\n",
+						dcd, sense, ktime_to_us(kt),
+						ktime_to_us(lastkt));
 					/*
 					 * detecting pulse while this
 					 * MUST be a space!
@@ -629,11 +615,9 @@ static irqreturn_t lirc_irq_handler(int i, void *blah)
 					sense = sense ? 0 : 1;
 				}
 			} else
-				data = (int) (deltv*1000000 +
-					       tv.tv_usec -
-					       lasttv.tv_usec);
+				data = (int) ktime_to_us(delkt);
 			frbwrite(dcd^sense ? data : (data|PULSE_BIT));
-			lasttv = tv;
+			lastkt = kt;
 			last_dcd = dcd;
 			wake_up_interruptible(&rbuf.wait_poll);
 		}
@@ -790,7 +774,7 @@ static int set_use_inc(void *data)
 	unsigned long flags;
 
 	/* initialize timestamp */
-	do_gettimeofday(&lasttv);
+	lastkt = ktime_get();
 
 	spin_lock_irqsave(&hardware[type].lock, flags);
 
@@ -979,7 +963,7 @@ static int lirc_serial_resume(struct platform_device *dev)
 
 	spin_lock_irqsave(&hardware[type].lock, flags);
 	/* Enable Interrupt */
-	do_gettimeofday(&lasttv);
+	lastkt = ktime_get();
 	soutp(UART_IER, sinp(UART_IER)|UART_IER_MSI);
 	off();
 
diff --git a/drivers/staging/media/omap4iss/Kconfig b/drivers/staging/media/omap4iss/Kconfig
index 8d4e3bd1bfe1..46183464ee79 100644
--- a/drivers/staging/media/omap4iss/Kconfig
+++ b/drivers/staging/media/omap4iss/Kconfig
@@ -1,6 +1,6 @@
 config VIDEO_OMAP4
 	tristate "OMAP 4 Camera support"
-	depends on VIDEO_V4L2=y && VIDEO_V4L2_SUBDEV_API && I2C=y && ARCH_OMAP4
+	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && I2C && ARCH_OMAP4
 	depends on HAS_DMA
 	select MFD_SYSCON
 	select VIDEOBUF2_DMA_CONTIG
diff --git a/drivers/staging/media/omap4iss/iss.c b/drivers/staging/media/omap4iss/iss.c
index aa76ccda5b42..30b473cfb020 100644
--- a/drivers/staging/media/omap4iss/iss.c
+++ b/drivers/staging/media/omap4iss/iss.c
@@ -389,15 +389,15 @@ static irqreturn_t iss_isr(int irq, void *_iss)
  *
  * Return the total number of users of all video device nodes in the pipeline.
  */
-static int iss_pipeline_pm_use_count(struct media_entity *entity)
+static int iss_pipeline_pm_use_count(struct media_entity *entity,
+				     struct media_entity_graph *graph)
 {
-	struct media_entity_graph graph;
 	int use = 0;
 
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(graph, entity);
 
-	while ((entity = media_entity_graph_walk_next(&graph))) {
-		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
+	while ((entity = media_entity_graph_walk_next(graph))) {
+		if (is_media_entity_v4l2_io(entity))
 			use += entity->use_count;
 	}
 
@@ -419,7 +419,7 @@ static int iss_pipeline_pm_power_one(struct media_entity *entity, int change)
 {
 	struct v4l2_subdev *subdev;
 
-	subdev = media_entity_type(entity) == MEDIA_ENT_T_V4L2_SUBDEV
+	subdev = is_media_entity_v4l2_subdev(entity)
 	       ? media_entity_to_v4l2_subdev(entity) : NULL;
 
 	if (entity->use_count == 0 && change > 0 && subdev) {
@@ -449,29 +449,29 @@ static int iss_pipeline_pm_power_one(struct media_entity *entity, int change)
  *
  * Return 0 on success or a negative error code on failure.
  */
-static int iss_pipeline_pm_power(struct media_entity *entity, int change)
+static int iss_pipeline_pm_power(struct media_entity *entity, int change,
+				 struct media_entity_graph *graph)
 {
-	struct media_entity_graph graph;
 	struct media_entity *first = entity;
 	int ret = 0;
 
 	if (!change)
 		return 0;
 
-	media_entity_graph_walk_start(&graph, entity);
+	media_entity_graph_walk_start(graph, entity);
 
-	while (!ret && (entity = media_entity_graph_walk_next(&graph)))
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+	while (!ret && (entity = media_entity_graph_walk_next(graph)))
+		if (is_media_entity_v4l2_subdev(entity))
 			ret = iss_pipeline_pm_power_one(entity, change);
 
 	if (!ret)
 		return 0;
 
-	media_entity_graph_walk_start(&graph, first);
+	media_entity_graph_walk_start(graph, first);
 
-	while ((first = media_entity_graph_walk_next(&graph)) &&
+	while ((first = media_entity_graph_walk_next(graph)) &&
 	       first != entity)
-		if (media_entity_type(first) != MEDIA_ENT_T_DEVNODE)
+		if (is_media_entity_v4l2_subdev(first))
 			iss_pipeline_pm_power_one(first, -change);
 
 	return ret;
@@ -489,23 +489,24 @@ static int iss_pipeline_pm_power(struct media_entity *entity, int change)
  * off is assumed to never fail. No failure can occur when the use parameter is
  * set to 0.
  */
-int omap4iss_pipeline_pm_use(struct media_entity *entity, int use)
+int omap4iss_pipeline_pm_use(struct media_entity *entity, int use,
+			     struct media_entity_graph *graph)
 {
 	int change = use ? 1 : -1;
 	int ret;
 
-	mutex_lock(&entity->parent->graph_mutex);
+	mutex_lock(&entity->graph_obj.mdev->graph_mutex);
 
 	/* Apply use count to node. */
 	entity->use_count += change;
 	WARN_ON(entity->use_count < 0);
 
 	/* Apply power change to connected non-nodes. */
-	ret = iss_pipeline_pm_power(entity, change);
+	ret = iss_pipeline_pm_power(entity, change, graph);
 	if (ret < 0)
 		entity->use_count -= change;
 
-	mutex_unlock(&entity->parent->graph_mutex);
+	mutex_unlock(&entity->graph_obj.mdev->graph_mutex);
 
 	return ret;
 }
@@ -526,34 +527,48 @@ int omap4iss_pipeline_pm_use(struct media_entity *entity, int use)
 static int iss_pipeline_link_notify(struct media_link *link, u32 flags,
 				    unsigned int notification)
 {
+	struct media_entity_graph *graph =
+		&container_of(link->graph_obj.mdev, struct iss_device,
+			      media_dev)->pm_count_graph;
 	struct media_entity *source = link->source->entity;
 	struct media_entity *sink = link->sink->entity;
-	int source_use = iss_pipeline_pm_use_count(source);
-	int sink_use = iss_pipeline_pm_use_count(sink);
+	int source_use;
+	int sink_use;
 	int ret;
 
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH) {
+		ret = media_entity_graph_walk_init(graph,
+						   link->graph_obj.mdev);
+		if (ret)
+			return ret;
+	}
+
+	source_use = iss_pipeline_pm_use_count(source, graph);
+	sink_use = iss_pipeline_pm_use_count(sink, graph);
+
 	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
-	    !(link->flags & MEDIA_LNK_FL_ENABLED)) {
+	    !(flags & MEDIA_LNK_FL_ENABLED)) {
 		/* Powering off entities is assumed to never fail. */
-		iss_pipeline_pm_power(source, -sink_use);
-		iss_pipeline_pm_power(sink, -source_use);
+		iss_pipeline_pm_power(source, -sink_use, graph);
+		iss_pipeline_pm_power(sink, -source_use, graph);
 		return 0;
 	}
 
-	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
+	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH &&
 	    (flags & MEDIA_LNK_FL_ENABLED)) {
-		ret = iss_pipeline_pm_power(source, sink_use);
+		ret = iss_pipeline_pm_power(source, sink_use, graph);
 		if (ret < 0)
 			return ret;
 
-		ret = iss_pipeline_pm_power(sink, source_use);
+		ret = iss_pipeline_pm_power(sink, source_use, graph);
 		if (ret < 0)
-			iss_pipeline_pm_power(source, -sink_use);
-
-		return ret;
+			iss_pipeline_pm_power(source, -sink_use, graph);
 	}
 
-	return 0;
+	if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH)
+		media_entity_graph_walk_cleanup(graph);
+
+	return ret;
 }
 
 /* -----------------------------------------------------------------------------
@@ -590,8 +605,7 @@ static int iss_pipeline_disable(struct iss_pipeline *pipe,
 			break;
 
 		pad = media_entity_remote_pad(pad);
-		if (!pad ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = pad->entity;
@@ -601,13 +615,13 @@ static int iss_pipeline_disable(struct iss_pipeline *pipe,
 		subdev = media_entity_to_v4l2_subdev(entity);
 		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
 		if (ret < 0) {
-			dev_dbg(iss->dev, "%s: module stop timeout.\n",
-				subdev->name);
+			dev_warn(iss->dev, "%s: module stop timeout.\n",
+				 subdev->name);
 			/* If the entity failed to stopped, assume it has
 			 * crashed. Mark it as such, the ISS will be reset when
 			 * applications will release it.
 			 */
-			iss->crashed |= 1U << subdev->entity.id;
+			media_entity_enum_set(&iss->crashed, &subdev->entity);
 			failure = -ETIMEDOUT;
 		}
 	}
@@ -642,7 +656,7 @@ static int iss_pipeline_enable(struct iss_pipeline *pipe,
 	 * pipeline won't start anyway (those entities would then likely fail to
 	 * stop, making the problem worse).
 	 */
-	if (pipe->entities & iss->crashed)
+	if (media_entity_enum_intersects(&pipe->ent_enum, &iss->crashed))
 		return -EIO;
 
 	spin_lock_irqsave(&pipe->lock, flags);
@@ -658,8 +672,7 @@ static int iss_pipeline_enable(struct iss_pipeline *pipe,
 			break;
 
 		pad = media_entity_remote_pad(pad);
-		if (!pad ||
-		    media_entity_type(pad->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 			break;
 
 		entity = pad->entity;
@@ -763,7 +776,8 @@ static int iss_reset(struct iss_device *iss)
 		return -ETIMEDOUT;
 	}
 
-	iss->crashed = 0;
+	media_entity_enum_zero(&iss->crashed);
+
 	return 0;
 }
 
@@ -1092,7 +1106,7 @@ void omap4iss_put(struct iss_device *iss)
 		 * be worth investigating whether resetting the ISP only can't
 		 * fix the problem in some cases.
 		 */
-		if (iss->crashed)
+		if (!media_entity_enum_empty(&iss->crashed))
 			iss_reset(iss);
 		iss_disable_clocks(iss);
 	}
@@ -1259,7 +1273,7 @@ static int iss_register_entities(struct iss_device *iss)
 			goto done;
 		}
 
-		ret = media_entity_create_link(&sensor->entity, 0, input, pad,
+		ret = media_create_pad_link(&sensor->entity, 0, input, pad,
 					       flags);
 		if (ret < 0)
 			goto done;
@@ -1274,6 +1288,68 @@ done:
 	return ret;
 }
 
+/*
+ * iss_create_links() - Pads links creation for the subdevices
+ * @iss : Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+static int iss_create_links(struct iss_device *iss)
+{
+	int ret;
+
+	ret = omap4iss_csi2_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "CSI2 pads links creation failed\n");
+		return ret;
+	}
+
+	ret = omap4iss_ipipeif_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP IPIPEIF pads links creation failed\n");
+		return ret;
+	}
+
+	ret = omap4iss_resizer_create_links(iss);
+	if (ret < 0) {
+		dev_err(iss->dev, "ISP RESIZER pads links creation failed\n");
+		return ret;
+	}
+
+	/* Connect the submodules. */
+	ret = media_create_pad_link(
+			&iss->csi2a.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->csi2b.subdev.entity, CSI2_PAD_SOURCE,
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	ret = media_create_pad_link(
+			&iss->ipipe.subdev.entity, IPIPE_PAD_SOURCE_VP,
+			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+};
+
 static void iss_cleanup_modules(struct iss_device *iss)
 {
 	omap4iss_csi2_cleanup(iss);
@@ -1316,41 +1392,8 @@ static int iss_initialize_modules(struct iss_device *iss)
 		goto error_resizer;
 	}
 
-	/* Connect the submodules. */
-	ret = media_entity_create_link(
-			&iss->csi2a.subdev.entity, CSI2_PAD_SOURCE,
-			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&iss->csi2b.subdev.entity, CSI2_PAD_SOURCE,
-			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
-			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&iss->ipipeif.subdev.entity, IPIPEIF_PAD_SOURCE_VP,
-			&iss->ipipe.subdev.entity, IPIPE_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
-	ret = media_entity_create_link(
-			&iss->ipipe.subdev.entity, IPIPE_PAD_SOURCE_VP,
-			&iss->resizer.subdev.entity, RESIZER_PAD_SINK, 0);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap4iss_resizer_cleanup(iss);
 error_resizer:
 	omap4iss_ipipe_cleanup(iss);
 error_ipipe:
@@ -1464,10 +1507,21 @@ static int iss_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto error_modules;
 
+	ret = media_entity_enum_init(&iss->crashed, &iss->media_dev);
+	if (ret)
+		goto error_entities;
+
+	ret = iss_create_links(iss);
+	if (ret < 0)
+		goto error_entities;
+
 	omap4iss_put(iss);
 
 	return 0;
 
+error_entities:
+	iss_unregister_entities(iss);
+	media_entity_enum_cleanup(&iss->crashed);
 error_modules:
 	iss_cleanup_modules(iss);
 error_iss:
@@ -1485,6 +1539,7 @@ static int iss_remove(struct platform_device *pdev)
 	struct iss_device *iss = platform_get_drvdata(pdev);
 
 	iss_unregister_entities(iss);
+	media_entity_enum_cleanup(&iss->crashed);
 	iss_cleanup_modules(iss);
 
 	return 0;
diff --git a/drivers/staging/media/omap4iss/iss.h b/drivers/staging/media/omap4iss/iss.h
index 35df8b4709e6..05f08a3caa19 100644
--- a/drivers/staging/media/omap4iss/iss.h
+++ b/drivers/staging/media/omap4iss/iss.h
@@ -20,7 +20,7 @@
 #include <linux/platform_device.h>
 #include <linux/wait.h>
 
-#include <media/omap4iss.h>
+#include <linux/platform_data/media/omap4iss.h>
 
 #include "iss_regs.h"
 #include "iss_csiphy.h"
@@ -82,11 +82,12 @@ struct iss_reg {
 /*
  * struct iss_device - ISS device structure.
  * @syscon: Regmap for the syscon register space
- * @crashed: Bitmask of crashed entities (indexed by entity ID)
+ * @crashed: Crashed entities
  */
 struct iss_device {
 	struct v4l2_device v4l2_dev;
 	struct media_device media_dev;
+	struct media_entity_graph pm_count_graph;
 	struct device *dev;
 	u32 revision;
 
@@ -101,7 +102,7 @@ struct iss_device {
 	u64 raw_dmamask;
 
 	struct mutex iss_mutex;	/* For handling ref_count field */
-	unsigned int crashed;
+	struct media_entity_enum crashed;
 	int has_context;
 	int ref_count;
 
@@ -151,7 +152,8 @@ void omap4iss_isp_subclk_enable(struct iss_device *iss,
 void omap4iss_isp_subclk_disable(struct iss_device *iss,
 				 enum iss_isp_subclk_resource res);
 
-int omap4iss_pipeline_pm_use(struct media_entity *entity, int use);
+int omap4iss_pipeline_pm_use(struct media_entity *entity, int use,
+			     struct media_entity_graph *graph);
 
 int omap4iss_register_entities(struct platform_device *pdev,
 			       struct v4l2_device *v4l2_dev);
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index c6e6d47ac57f..aaca39d751a5 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -674,6 +674,9 @@ static void csi2_isr_ctx(struct iss_csi2_device *csi2,
 	status = iss_reg_read(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n));
 	iss_reg_write(csi2->iss, csi2->regs1, CSI2_CTX_IRQSTATUS(n), status);
 
+	if (omap4iss_module_sync_is_stopping(&csi2->wait, &csi2->stopping))
+		return;
+
 	/* Propagate frame number */
 	if (status & CSI2_CTX_IRQ_FS) {
 		struct iss_pipeline *pipe =
@@ -776,9 +779,6 @@ void omap4iss_csi2_isr(struct iss_csi2_device *csi2)
 		pipe->error = true;
 	}
 
-	if (omap4iss_module_sync_is_stopping(&csi2->wait, &csi2->stopping))
-		return;
-
 	/* Successful cases */
 	if (csi2_irqstatus & CSI2_IRQ_CONTEXT0)
 		csi2_isr_ctx(csi2, &csi2->contexts[0]);
@@ -1170,14 +1170,19 @@ static int csi2_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct iss_csi2_device *csi2 = v4l2_get_subdevdata(sd);
 	struct iss_csi2_ctrl_cfg *ctrl = &csi2->ctrl;
+	unsigned int index = local->index;
+
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
 
 	/*
 	 * The ISS core doesn't support pipelines with multiple video outputs.
 	 * Revisit this when it will be implemented, and return -EBUSY for now.
 	 */
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case CSI2_PAD_SOURCE | MEDIA_ENT_T_DEVNODE:
+	switch (index) {
+	case CSI2_PAD_SOURCE:
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (csi2->output & ~CSI2_OUTPUT_MEMORY)
 				return -EBUSY;
@@ -1187,7 +1192,7 @@ static int csi2_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case CSI2_PAD_SOURCE | MEDIA_ENT_T_V4L2_SUBDEV:
+	case CSI2_PAD_SOURCE | 2 << 16:
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (csi2->output & ~CSI2_OUTPUT_IPIPEIF)
 				return -EBUSY;
@@ -1271,7 +1276,7 @@ static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
 	pads[CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
 
 	me->ops = &csi2_media_ops;
-	ret = media_entity_init(me, CSI2_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, CSI2_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -1290,16 +1295,8 @@ static int csi2_init_entities(struct iss_csi2_device *csi2, const char *subname)
 	if (ret < 0)
 		goto error_video;
 
-	/* Connect the CSI2 subdev to the video node. */
-	ret = media_entity_create_link(&csi2->subdev.entity, CSI2_PAD_SOURCE,
-				       &csi2->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		goto error_link;
-
 	return 0;
 
-error_link:
-	omap4iss_video_cleanup(&csi2->video_out);
 error_video:
 	media_entity_cleanup(&csi2->subdev.entity);
 	return ret;
@@ -1342,6 +1339,33 @@ int omap4iss_csi2_init(struct iss_device *iss)
 }
 
 /*
+ * omap4iss_csi2_create_links() - CSI2 pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_csi2_create_links(struct iss_device *iss)
+{
+	struct iss_csi2_device *csi2a = &iss->csi2a;
+	struct iss_csi2_device *csi2b = &iss->csi2b;
+	int ret;
+
+	/* Connect the CSI2a subdev to the video node. */
+	ret = media_create_pad_link(&csi2a->subdev.entity, CSI2_PAD_SOURCE,
+				    &csi2a->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Connect the CSI2b subdev to the video node. */
+	ret = media_create_pad_link(&csi2b->subdev.entity, CSI2_PAD_SOURCE,
+				    &csi2b->video_out.video.entity, 0, 0);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+/*
  * omap4iss_csi2_cleanup - Routine for module driver cleanup
  */
 void omap4iss_csi2_cleanup(struct iss_device *iss)
diff --git a/drivers/staging/media/omap4iss/iss_csi2.h b/drivers/staging/media/omap4iss/iss_csi2.h
index f2f5343b4a80..24ab378d469f 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.h
+++ b/drivers/staging/media/omap4iss/iss_csi2.h
@@ -151,6 +151,7 @@ struct iss_csi2_device {
 void omap4iss_csi2_isr(struct iss_csi2_device *csi2);
 int omap4iss_csi2_reset(struct iss_csi2_device *csi2);
 int omap4iss_csi2_init(struct iss_device *iss);
+int omap4iss_csi2_create_links(struct iss_device *iss);
 void omap4iss_csi2_cleanup(struct iss_device *iss);
 void omap4iss_csi2_unregister_entities(struct iss_csi2_device *csi2);
 int omap4iss_csi2_register_entities(struct iss_csi2_device *csi2,
diff --git a/drivers/staging/media/omap4iss/iss_csiphy.h b/drivers/staging/media/omap4iss/iss_csiphy.h
index e9ca43955654..a0f2d974daeb 100644
--- a/drivers/staging/media/omap4iss/iss_csiphy.h
+++ b/drivers/staging/media/omap4iss/iss_csiphy.h
@@ -14,7 +14,7 @@
 #ifndef OMAP4_ISS_CSI_PHY_H
 #define OMAP4_ISS_CSI_PHY_H
 
-#include <media/omap4iss.h>
+#include <linux/platform_data/media/omap4iss.h>
 
 struct iss_csi2_device;
 
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
index dd0abeffd893..d38782e8e84c 100644
--- a/drivers/staging/media/omap4iss/iss_ipipe.c
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -447,8 +447,11 @@ static int ipipe_link_setup(struct media_entity *entity,
 	struct iss_ipipe_device *ipipe = v4l2_get_subdevdata(sd);
 	struct iss_device *iss = to_iss_device(ipipe);
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case IPIPE_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	if (!is_media_entity_v4l2_subdev(remote->entity))
+		return -EINVAL;
+
+	switch (local->index) {
+	case IPIPE_PAD_SINK:
 		/* Read from IPIPEIF. */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipe->input = IPIPE_INPUT_NONE;
@@ -463,7 +466,7 @@ static int ipipe_link_setup(struct media_entity *entity,
 
 		break;
 
-	case IPIPE_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+	case IPIPE_PAD_SOURCE_VP:
 		/* Send to RESIZER */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ipipe->output & ~IPIPE_OUTPUT_VP)
@@ -513,7 +516,7 @@ static int ipipe_init_entities(struct iss_ipipe_device *ipipe)
 	pads[IPIPE_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &ipipe_media_ops;
-	ret = media_entity_init(me, IPIPE_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, IPIPE_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
index 5f9e449e7007..23de8330731d 100644
--- a/drivers/staging/media/omap4iss/iss_ipipeif.c
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -662,9 +662,14 @@ static int ipipeif_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct iss_ipipeif_device *ipipeif = v4l2_get_subdevdata(sd);
 	struct iss_device *iss = to_iss_device(ipipeif);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case IPIPEIF_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case IPIPEIF_PAD_SINK | 2 << 16:
 		/* Read from the sensor CSI2a or CSI2b. */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			ipipeif->input = IPIPEIF_INPUT_NONE;
@@ -681,7 +686,7 @@ static int ipipeif_link_setup(struct media_entity *entity,
 
 		break;
 
-	case IPIPEIF_PAD_SOURCE_ISIF_SF | MEDIA_ENT_T_DEVNODE:
+	case IPIPEIF_PAD_SOURCE_ISIF_SF:
 		/* Write to memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ipipeif->output & ~IPIPEIF_OUTPUT_MEMORY)
@@ -692,7 +697,7 @@ static int ipipeif_link_setup(struct media_entity *entity,
 		}
 		break;
 
-	case IPIPEIF_PAD_SOURCE_VP | MEDIA_ENT_T_V4L2_SUBDEV:
+	case IPIPEIF_PAD_SOURCE_VP | 2 << 16:
 		/* Send to IPIPE/RESIZER */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (ipipeif->output & ~IPIPEIF_OUTPUT_VP)
@@ -743,7 +748,7 @@ static int ipipeif_init_entities(struct iss_ipipeif_device *ipipeif)
 	pads[IPIPEIF_PAD_SOURCE_VP].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &ipipeif_media_ops;
-	ret = media_entity_init(me, IPIPEIF_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, IPIPEIF_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -757,18 +762,7 @@ static int ipipeif_init_entities(struct iss_ipipeif_device *ipipeif)
 	ipipeif->video_out.bpl_zero_padding = 1;
 	ipipeif->video_out.bpl_max = 0x1ffe0;
 
-	ret = omap4iss_video_init(&ipipeif->video_out, "ISP IPIPEIF");
-	if (ret < 0)
-		return ret;
-
-	/* Connect the IPIPEIF subdev to the video node. */
-	ret = media_entity_create_link(&ipipeif->subdev.entity,
-				       IPIPEIF_PAD_SOURCE_ISIF_SF,
-				       &ipipeif->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		return ret;
-
-	return 0;
+	return omap4iss_video_init(&ipipeif->video_out, "ISP IPIPEIF");
 }
 
 void omap4iss_ipipeif_unregister_entities(struct iss_ipipeif_device *ipipeif)
@@ -821,6 +815,22 @@ int omap4iss_ipipeif_init(struct iss_device *iss)
 }
 
 /*
+ * omap4iss_ipipeif_create_links() - IPIPEIF pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_ipipeif_create_links(struct iss_device *iss)
+{
+	struct iss_ipipeif_device *ipipeif = &iss->ipipeif;
+
+	/* Connect the IPIPEIF subdev to the video node. */
+	return media_create_pad_link(&ipipeif->subdev.entity,
+				     IPIPEIF_PAD_SOURCE_ISIF_SF,
+				     &ipipeif->video_out.video.entity, 0, 0);
+}
+
+/*
  * omap4iss_ipipeif_cleanup - IPIPEIF module cleanup.
  * @iss: Device pointer specific to the OMAP4 ISS.
  */
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.h b/drivers/staging/media/omap4iss/iss_ipipeif.h
index c6bd96d9656c..bad32b1d6ad8 100644
--- a/drivers/staging/media/omap4iss/iss_ipipeif.h
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.h
@@ -78,6 +78,7 @@ struct iss_ipipeif_device {
 struct iss_device;
 
 int omap4iss_ipipeif_init(struct iss_device *iss);
+int omap4iss_ipipeif_create_links(struct iss_device *iss);
 void omap4iss_ipipeif_cleanup(struct iss_device *iss);
 int omap4iss_ipipeif_register_entities(struct iss_ipipeif_device *ipipeif,
 				       struct v4l2_device *vdev);
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
index 9c8180bba77e..f1d352c711d5 100644
--- a/drivers/staging/media/omap4iss/iss_resizer.c
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -158,8 +158,8 @@ static void resizer_set_outaddr(struct iss_resizer_device *resizer, u32 addr)
 	/* Program UV buffer address... Hardcoded to be contiguous! */
 	if ((informat->code == MEDIA_BUS_FMT_UYVY8_1X16) &&
 	    (outformat->code == MEDIA_BUS_FMT_YUYV8_1_5X8)) {
-		u32 c_addr = addr + (resizer->video_out.bpl_value *
-				     (outformat->height - 1));
+		u32 c_addr = addr + resizer->video_out.bpl_value
+			   * outformat->height;
 
 		/* Ensure Y_BAD_L[6:0] = C_BAD_L[6:0]*/
 		if ((c_addr ^ addr) & 0x7f) {
@@ -716,9 +716,14 @@ static int resizer_link_setup(struct media_entity *entity,
 	struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);
 	struct iss_resizer_device *resizer = v4l2_get_subdevdata(sd);
 	struct iss_device *iss = to_iss_device(resizer);
+	unsigned int index = local->index;
 
-	switch (local->index | media_entity_type(remote->entity)) {
-	case RESIZER_PAD_SINK | MEDIA_ENT_T_V4L2_SUBDEV:
+	/* FIXME: this is actually a hack! */
+	if (is_media_entity_v4l2_subdev(remote->entity))
+		index |= 2 << 16;
+
+	switch (index) {
+	case RESIZER_PAD_SINK | 2 << 16:
 		/* Read from IPIPE or IPIPEIF. */
 		if (!(flags & MEDIA_LNK_FL_ENABLED)) {
 			resizer->input = RESIZER_INPUT_NONE;
@@ -735,7 +740,7 @@ static int resizer_link_setup(struct media_entity *entity,
 
 		break;
 
-	case RESIZER_PAD_SOURCE_MEM | MEDIA_ENT_T_DEVNODE:
+	case RESIZER_PAD_SOURCE_MEM:
 		/* Write to memory */
 		if (flags & MEDIA_LNK_FL_ENABLED) {
 			if (resizer->output & ~RESIZER_OUTPUT_MEMORY)
@@ -785,7 +790,7 @@ static int resizer_init_entities(struct iss_resizer_device *resizer)
 	pads[RESIZER_PAD_SOURCE_MEM].flags = MEDIA_PAD_FL_SOURCE;
 
 	me->ops = &resizer_media_ops;
-	ret = media_entity_init(me, RESIZER_PADS_NUM, pads, 0);
+	ret = media_entity_pads_init(me, RESIZER_PADS_NUM, pads);
 	if (ret < 0)
 		return ret;
 
@@ -799,18 +804,7 @@ static int resizer_init_entities(struct iss_resizer_device *resizer)
 	resizer->video_out.bpl_zero_padding = 1;
 	resizer->video_out.bpl_max = 0x1ffe0;
 
-	ret = omap4iss_video_init(&resizer->video_out, "ISP resizer a");
-	if (ret < 0)
-		return ret;
-
-	/* Connect the RESIZER subdev to the video node. */
-	ret = media_entity_create_link(&resizer->subdev.entity,
-				       RESIZER_PAD_SOURCE_MEM,
-				       &resizer->video_out.video.entity, 0, 0);
-	if (ret < 0)
-		return ret;
-
-	return 0;
+	return omap4iss_video_init(&resizer->video_out, "ISP resizer a");
 }
 
 void omap4iss_resizer_unregister_entities(struct iss_resizer_device *resizer)
@@ -863,6 +857,22 @@ int omap4iss_resizer_init(struct iss_device *iss)
 }
 
 /*
+ * omap4iss_resizer_create_links() - RESIZER pads links creation
+ * @iss: Pointer to ISS device
+ *
+ * return negative error code or zero on success
+ */
+int omap4iss_resizer_create_links(struct iss_device *iss)
+{
+	struct iss_resizer_device *resizer = &iss->resizer;
+
+	/* Connect the RESIZER subdev to the video node. */
+	return media_create_pad_link(&resizer->subdev.entity,
+				     RESIZER_PAD_SOURCE_MEM,
+				     &resizer->video_out.video.entity, 0, 0);
+}
+
+/*
  * omap4iss_resizer_cleanup - RESIZER module cleanup.
  * @iss: Device pointer specific to the OMAP4 ISS.
  */
diff --git a/drivers/staging/media/omap4iss/iss_resizer.h b/drivers/staging/media/omap4iss/iss_resizer.h
index 1e145abafc65..8b7c5fe9ffed 100644
--- a/drivers/staging/media/omap4iss/iss_resizer.h
+++ b/drivers/staging/media/omap4iss/iss_resizer.h
@@ -61,6 +61,7 @@ struct iss_resizer_device {
 struct iss_device;
 
 int omap4iss_resizer_init(struct iss_device *iss);
+int omap4iss_resizer_create_links(struct iss_device *iss);
 void omap4iss_resizer_cleanup(struct iss_device *iss);
 int omap4iss_resizer_register_entities(struct iss_resizer_device *resizer,
 				       struct v4l2_device *vdev);
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
index 2a0158bb4974..058233a9de67 100644
--- a/drivers/staging/media/omap4iss/iss_video.c
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -190,8 +190,7 @@ iss_video_remote_subdev(struct iss_video *video, u32 *pad)
 
 	remote = media_entity_remote_pad(&video->pad);
 
-	if (!remote ||
-	    media_entity_type(remote->entity) != MEDIA_ENT_T_V4L2_SUBDEV)
+	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 		return NULL;
 
 	if (pad)
@@ -206,17 +205,23 @@ iss_video_far_end(struct iss_video *video)
 {
 	struct media_entity_graph graph;
 	struct media_entity *entity = &video->video.entity;
-	struct media_device *mdev = entity->parent;
+	struct media_device *mdev = entity->graph_obj.mdev;
 	struct iss_video *far_end = NULL;
 
 	mutex_lock(&mdev->graph_mutex);
+
+	if (media_entity_graph_walk_init(&graph, mdev)) {
+		mutex_unlock(&mdev->graph_mutex);
+		return NULL;
+	}
+
 	media_entity_graph_walk_start(&graph, entity);
 
 	while ((entity = media_entity_graph_walk_next(&graph))) {
 		if (entity == &video->video.entity)
 			continue;
 
-		if (media_entity_type(entity) != MEDIA_ENT_T_DEVNODE)
+		if (!is_media_entity_v4l2_io(entity))
 			continue;
 
 		far_end = to_iss_video(media_entity_to_video_device(entity));
@@ -227,6 +232,9 @@ iss_video_far_end(struct iss_video *video)
 	}
 
 	mutex_unlock(&mdev->graph_mutex);
+
+	media_entity_graph_walk_cleanup(&graph);
+
 	return far_end;
 }
 
@@ -287,7 +295,6 @@ iss_video_check_format(struct iss_video *video, struct iss_video_fh *vfh)
  */
 
 static int iss_video_queue_setup(struct vb2_queue *vq,
-				 const void *parg,
 				 unsigned int *count, unsigned int *num_planes,
 				 unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -434,7 +441,7 @@ struct iss_buffer *omap4iss_video_buffer_next(struct iss_video *video)
 	list_del(&buf->list);
 	spin_unlock_irqrestore(&video->qlock, flags);
 
-	v4l2_get_timestamp(&buf->vb.timestamp);
+	buf->vb.vb2_buf.timestamp = ktime_get_ns();
 
 	/* Do frame number propagation only if this is the output video node.
 	 * Frame number either comes from the CSI receivers or it gets
@@ -751,7 +758,7 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	struct iss_video_fh *vfh = to_iss_video_fh(fh);
 	struct iss_video *video = video_drvdata(file);
 	struct media_entity_graph graph;
-	struct media_entity *entity;
+	struct media_entity *entity = &video->video.entity;
 	enum iss_pipeline_state state;
 	struct iss_pipeline *pipe;
 	struct iss_video *far_end;
@@ -766,24 +773,30 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	/* Start streaming on the pipeline. No link touching an entity in the
 	 * pipeline can be activated or deactivated once streaming is started.
 	 */
-	pipe = video->video.entity.pipe
-	     ? to_iss_pipeline(&video->video.entity) : &video->pipe;
+	pipe = entity->pipe
+	     ? to_iss_pipeline(entity) : &video->pipe;
 	pipe->external = NULL;
 	pipe->external_rate = 0;
 	pipe->external_bpp = 0;
-	pipe->entities = 0;
+
+	ret = media_entity_enum_init(&pipe->ent_enum, entity->graph_obj.mdev);
+	if (ret)
+		goto err_graph_walk_init;
+
+	ret = media_entity_graph_walk_init(&graph, entity->graph_obj.mdev);
+	if (ret)
+		goto err_graph_walk_init;
 
 	if (video->iss->pdata->set_constraints)
 		video->iss->pdata->set_constraints(video->iss, true);
 
-	ret = media_entity_pipeline_start(&video->video.entity, &pipe->pipe);
+	ret = media_entity_pipeline_start(entity, &pipe->pipe);
 	if (ret < 0)
 		goto err_media_entity_pipeline_start;
 
-	entity = &video->video.entity;
 	media_entity_graph_walk_start(&graph, entity);
 	while ((entity = media_entity_graph_walk_next(&graph)))
-		pipe->entities |= 1 << entity->id;
+		media_entity_enum_set(&pipe->ent_enum, entity);
 
 	/* Verify that the currently configured format matches the output of
 	 * the connected subdev.
@@ -853,7 +866,10 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 		spin_unlock_irqrestore(&video->qlock, flags);
 	}
 
+	media_entity_graph_walk_cleanup(&graph);
+
 	mutex_unlock(&video->stream_lock);
+
 	return 0;
 
 err_omap4iss_set_stream:
@@ -865,7 +881,13 @@ err_media_entity_pipeline_start:
 		video->iss->pdata->set_constraints(video->iss, false);
 	video->queue = NULL;
 
+	media_entity_graph_walk_cleanup(&graph);
+
+err_graph_walk_init:
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
 	mutex_unlock(&video->stream_lock);
+
 	return ret;
 }
 
@@ -903,6 +925,8 @@ iss_video_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
 	vb2_streamoff(&vfh->queue, type);
 	video->queue = NULL;
 
+	media_entity_enum_cleanup(&pipe->ent_enum);
+
 	if (video->iss->pdata->set_constraints)
 		video->iss->pdata->set_constraints(video->iss, false);
 	media_entity_pipeline_stop(&video->video.entity);
@@ -985,7 +1009,13 @@ static int iss_video_open(struct file *file)
 		goto done;
 	}
 
-	ret = omap4iss_pipeline_pm_use(&video->video.entity, 1);
+	ret = media_entity_graph_walk_init(&handle->graph,
+					   &video->iss->media_dev);
+	if (ret)
+		goto done;
+
+	ret = omap4iss_pipeline_pm_use(&video->video.entity, 1,
+				       &handle->graph);
 	if (ret < 0) {
 		omap4iss_put(video->iss);
 		goto done;
@@ -1024,6 +1054,7 @@ static int iss_video_open(struct file *file)
 done:
 	if (ret < 0) {
 		v4l2_fh_del(&handle->vfh);
+		media_entity_graph_walk_cleanup(&handle->graph);
 		kfree(handle);
 	}
 
@@ -1039,12 +1070,13 @@ static int iss_video_release(struct file *file)
 	/* Disable streaming and free the buffers queue resources. */
 	iss_video_streamoff(file, vfh, video->type);
 
-	omap4iss_pipeline_pm_use(&video->video.entity, 0);
+	omap4iss_pipeline_pm_use(&video->video.entity, 0, &handle->graph);
 
 	/* Release the videobuf2 queue */
 	vb2_queue_release(&handle->queue);
 
 	/* Release the file handle. */
+	media_entity_graph_walk_cleanup(&handle->graph);
 	v4l2_fh_del(vfh);
 	kfree(handle);
 	file->private_data = NULL;
@@ -1103,7 +1135,7 @@ int omap4iss_video_init(struct iss_video *video, const char *name)
 		return -EINVAL;
 	}
 
-	ret = media_entity_init(&video->video.entity, 1, &video->pad, 0);
+	ret = media_entity_pads_init(&video->video.entity, 1, &video->pad);
 	if (ret < 0)
 		return ret;
 
diff --git a/drivers/staging/media/omap4iss/iss_video.h b/drivers/staging/media/omap4iss/iss_video.h
index 41532eda1277..34588b7176ca 100644
--- a/drivers/staging/media/omap4iss/iss_video.h
+++ b/drivers/staging/media/omap4iss/iss_video.h
@@ -77,7 +77,7 @@ enum iss_pipeline_state {
 
 /*
  * struct iss_pipeline - An OMAP4 ISS hardware pipeline
- * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
+ * @ent_enum: Entities in the pipeline
  * @error: A hardware error occurred during capture
  */
 struct iss_pipeline {
@@ -87,7 +87,7 @@ struct iss_pipeline {
 	enum iss_pipeline_stream_state stream_state;
 	struct iss_video *input;
 	struct iss_video *output;
-	unsigned int entities;
+	struct media_entity_enum ent_enum;
 	atomic_t frame_number;
 	bool do_propagation; /* of frame number */
 	bool error;
@@ -183,6 +183,7 @@ struct iss_video_fh {
 	struct vb2_queue queue;
 	struct v4l2_format format;
 	struct v4l2_fract timeperframe;
+	struct media_entity_graph graph;
 };
 
 #define to_iss_video_fh(fh)	container_of(fh, struct iss_video_fh, vfh)
diff --git a/drivers/staging/most/aim-network/networking.h b/drivers/staging/most/aim-network/networking.h
index 1b8b434fabb0..6f346d410525 100644
--- a/drivers/staging/most/aim-network/networking.h
+++ b/drivers/staging/most/aim-network/networking.h
@@ -15,9 +15,7 @@
 
 #include "mostcore.h"
 
-
 void most_deliver_netinfo(struct most_interface *iface,
 			  unsigned char link_stat, unsigned char *mac_addr);
 
-
 #endif
diff --git a/drivers/staging/most/hdm-dim2/dim2_errors.h b/drivers/staging/most/hdm-dim2/dim2_errors.h
index 314f7de2be73..5a713df1d1d4 100644
--- a/drivers/staging/most/hdm-dim2/dim2_errors.h
+++ b/drivers/staging/most/hdm-dim2/dim2_errors.h
@@ -19,7 +19,6 @@
 extern "C" {
 #endif
 
-
 /**
  * MOST DIM errors.
  */
@@ -59,7 +58,6 @@ enum dim_errors_t {
 	DIM_ERR_OVERFLOW,
 };
 
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/drivers/staging/most/hdm-dim2/dim2_hal.c b/drivers/staging/most/hdm-dim2/dim2_hal.c
index c915c44f025e..172257596f1f 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hal.c
+++ b/drivers/staging/most/hdm-dim2/dim2_hal.c
@@ -74,7 +74,7 @@ static inline u32 bit_mask(u8 position)
 
 static inline bool dim_on_error(u8 error_id, const char *error_message)
 {
-	DIMCB_OnError(error_id, error_message);
+	dimcb_on_error(error_id, error_message);
 	return false;
 }
 
@@ -151,44 +151,44 @@ static void free_dbr(int offs, int size)
 
 static u32 dim2_read_ctr(u32 ctr_addr, u16 mdat_idx)
 {
-	DIMCB_IoWrite(&g.dim2->MADR, ctr_addr);
+	dimcb_io_write(&g.dim2->MADR, ctr_addr);
 
 	/* wait till transfer is completed */
-	while ((DIMCB_IoRead(&g.dim2->MCTL) & 1) != 1)
+	while ((dimcb_io_read(&g.dim2->MCTL) & 1) != 1)
 		continue;
 
-	DIMCB_IoWrite(&g.dim2->MCTL, 0);   /* clear transfer complete */
+	dimcb_io_write(&g.dim2->MCTL, 0);   /* clear transfer complete */
 
-	return DIMCB_IoRead((&g.dim2->MDAT0) + mdat_idx);
+	return dimcb_io_read((&g.dim2->MDAT0) + mdat_idx);
 }
 
 static void dim2_write_ctr_mask(u32 ctr_addr, const u32 *mask, const u32 *value)
 {
 	enum { MADR_WNR_BIT = 31 };
 
-	DIMCB_IoWrite(&g.dim2->MCTL, 0);   /* clear transfer complete */
+	dimcb_io_write(&g.dim2->MCTL, 0);   /* clear transfer complete */
 
 	if (mask[0] != 0)
-		DIMCB_IoWrite(&g.dim2->MDAT0, value[0]);
+		dimcb_io_write(&g.dim2->MDAT0, value[0]);
 	if (mask[1] != 0)
-		DIMCB_IoWrite(&g.dim2->MDAT1, value[1]);
+		dimcb_io_write(&g.dim2->MDAT1, value[1]);
 	if (mask[2] != 0)
-		DIMCB_IoWrite(&g.dim2->MDAT2, value[2]);
+		dimcb_io_write(&g.dim2->MDAT2, value[2]);
 	if (mask[3] != 0)
-		DIMCB_IoWrite(&g.dim2->MDAT3, value[3]);
+		dimcb_io_write(&g.dim2->MDAT3, value[3]);
 
-	DIMCB_IoWrite(&g.dim2->MDWE0, mask[0]);
-	DIMCB_IoWrite(&g.dim2->MDWE1, mask[1]);
-	DIMCB_IoWrite(&g.dim2->MDWE2, mask[2]);
-	DIMCB_IoWrite(&g.dim2->MDWE3, mask[3]);
+	dimcb_io_write(&g.dim2->MDWE0, mask[0]);
+	dimcb_io_write(&g.dim2->MDWE1, mask[1]);
+	dimcb_io_write(&g.dim2->MDWE2, mask[2]);
+	dimcb_io_write(&g.dim2->MDWE3, mask[3]);
 
-	DIMCB_IoWrite(&g.dim2->MADR, bit_mask(MADR_WNR_BIT) | ctr_addr);
+	dimcb_io_write(&g.dim2->MADR, bit_mask(MADR_WNR_BIT) | ctr_addr);
 
 	/* wait till transfer is completed */
-	while ((DIMCB_IoRead(&g.dim2->MCTL) & 1) != 1)
+	while ((dimcb_io_read(&g.dim2->MCTL) & 1) != 1)
 		continue;
 
-	DIMCB_IoWrite(&g.dim2->MCTL, 0);   /* clear transfer complete */
+	dimcb_io_write(&g.dim2->MCTL, 0);   /* clear transfer complete */
 }
 
 static inline void dim2_write_ctr(u32 ctr_addr, const u32 *value)
@@ -341,15 +341,15 @@ static void dim2_configure_channel(
 	dim2_configure_cat(AHB_CAT, ch_addr, type, is_tx ? 0 : 1, sync_mfe);
 
 	/* unmask interrupt for used channel, enable mlb_sys_int[0] interrupt */
-	DIMCB_IoWrite(&g.dim2->ACMR0,
-		      DIMCB_IoRead(&g.dim2->ACMR0) | bit_mask(ch_addr));
+	dimcb_io_write(&g.dim2->ACMR0,
+		       dimcb_io_read(&g.dim2->ACMR0) | bit_mask(ch_addr));
 }
 
 static void dim2_clear_channel(u8 ch_addr)
 {
 	/* mask interrupt for used channel, disable mlb_sys_int[0] interrupt */
-	DIMCB_IoWrite(&g.dim2->ACMR0,
-		      DIMCB_IoRead(&g.dim2->ACMR0) & ~bit_mask(ch_addr));
+	dimcb_io_write(&g.dim2->ACMR0,
+		       dimcb_io_read(&g.dim2->ACMR0) & ~bit_mask(ch_addr));
 
 	dim2_clear_cat(AHB_CAT, ch_addr);
 	dim2_clear_adt(ch_addr);
@@ -455,20 +455,20 @@ static inline u16 norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame)
 static void dim2_cleanup(void)
 {
 	/* disable MediaLB */
-	DIMCB_IoWrite(&g.dim2->MLBC0, false << MLBC0_MLBEN_BIT);
+	dimcb_io_write(&g.dim2->MLBC0, false << MLBC0_MLBEN_BIT);
 
 	dim2_clear_ctram();
 
 	/* disable mlb_int interrupt */
-	DIMCB_IoWrite(&g.dim2->MIEN, 0);
+	dimcb_io_write(&g.dim2->MIEN, 0);
 
 	/* clear status for all dma channels */
-	DIMCB_IoWrite(&g.dim2->ACSR0, 0xFFFFFFFF);
-	DIMCB_IoWrite(&g.dim2->ACSR1, 0xFFFFFFFF);
+	dimcb_io_write(&g.dim2->ACSR0, 0xFFFFFFFF);
+	dimcb_io_write(&g.dim2->ACSR1, 0xFFFFFFFF);
 
 	/* mask interrupts for all channels */
-	DIMCB_IoWrite(&g.dim2->ACMR0, 0);
-	DIMCB_IoWrite(&g.dim2->ACMR1, 0);
+	dimcb_io_write(&g.dim2->ACMR0, 0);
+	dimcb_io_write(&g.dim2->ACMR1, 0);
 }
 
 static void dim2_initialize(bool enable_6pin, u8 mlb_clock)
@@ -476,23 +476,23 @@ static void dim2_initialize(bool enable_6pin, u8 mlb_clock)
 	dim2_cleanup();
 
 	/* configure and enable MediaLB */
-	DIMCB_IoWrite(&g.dim2->MLBC0,
-		      enable_6pin << MLBC0_MLBPEN_BIT |
-		      mlb_clock << MLBC0_MLBCLK_SHIFT |
-		      MLBC0_FCNT_VAL(FRAMES_PER_SUBBUFF) << MLBC0_FCNT_SHIFT |
-		      true << MLBC0_MLBEN_BIT);
+	dimcb_io_write(&g.dim2->MLBC0,
+		       enable_6pin << MLBC0_MLBPEN_BIT |
+		       mlb_clock << MLBC0_MLBCLK_SHIFT |
+		       MLBC0_FCNT_VAL(FRAMES_PER_SUBBUFF) << MLBC0_FCNT_SHIFT |
+		       true << MLBC0_MLBEN_BIT);
 
 	/* activate all HBI channels */
-	DIMCB_IoWrite(&g.dim2->HCMR0, 0xFFFFFFFF);
-	DIMCB_IoWrite(&g.dim2->HCMR1, 0xFFFFFFFF);
+	dimcb_io_write(&g.dim2->HCMR0, 0xFFFFFFFF);
+	dimcb_io_write(&g.dim2->HCMR1, 0xFFFFFFFF);
 
 	/* enable HBI */
-	DIMCB_IoWrite(&g.dim2->HCTL, bit_mask(HCTL_EN_BIT));
+	dimcb_io_write(&g.dim2->HCTL, bit_mask(HCTL_EN_BIT));
 
 	/* configure DMA */
-	DIMCB_IoWrite(&g.dim2->ACTL,
-		      ACTL_DMA_MODE_VAL_DMA_MODE_1 << ACTL_DMA_MODE_BIT |
-		      true << ACTL_SCE_BIT);
+	dimcb_io_write(&g.dim2->ACTL,
+		       ACTL_DMA_MODE_VAL_DMA_MODE_1 << ACTL_DMA_MODE_BIT |
+		       true << ACTL_SCE_BIT);
 }
 
 static bool dim2_is_mlb_locked(void)
@@ -500,12 +500,12 @@ static bool dim2_is_mlb_locked(void)
 	u32 const mask0 = bit_mask(MLBC0_MLBLK_BIT);
 	u32 const mask1 = bit_mask(MLBC1_CLKMERR_BIT) |
 			  bit_mask(MLBC1_LOCKERR_BIT);
-	u32 const c1 = DIMCB_IoRead(&g.dim2->MLBC1);
+	u32 const c1 = dimcb_io_read(&g.dim2->MLBC1);
 	u32 const nda_mask = (u32)MLBC1_NDA_MASK << MLBC1_NDA_SHIFT;
 
-	DIMCB_IoWrite(&g.dim2->MLBC1, c1 & nda_mask);
-	return (DIMCB_IoRead(&g.dim2->MLBC1) & mask1) == 0 &&
-	       (DIMCB_IoRead(&g.dim2->MLBC0) & mask0) != 0;
+	dimcb_io_write(&g.dim2->MLBC1, c1 & nda_mask);
+	return (dimcb_io_read(&g.dim2->MLBC1) & mask1) == 0 &&
+	       (dimcb_io_read(&g.dim2->MLBC0) & mask0) != 0;
 }
 
 /* -------------------------------------------------------------------------- */
@@ -531,7 +531,7 @@ static inline bool service_channel(u8 ch_addr, u8 idx)
 	}
 
 	/* clear channel status bit */
-	DIMCB_IoWrite(&g.dim2->ACSR0, bit_mask(ch_addr));
+	dimcb_io_write(&g.dim2->ACSR0, bit_mask(ch_addr));
 
 	return true;
 }
@@ -650,7 +650,7 @@ static bool channel_detach_buffers(struct dim_channel *ch, u16 buffers_number)
 /* -------------------------------------------------------------------------- */
 /* API */
 
-u8 DIM_Startup(void *dim_base_address, u32 mlb_clock)
+u8 dim_startup(void *dim_base_address, u32 mlb_clock)
 {
 	g.dim_is_initialized = false;
 
@@ -673,13 +673,13 @@ u8 DIM_Startup(void *dim_base_address, u32 mlb_clock)
 	return DIM_NO_ERROR;
 }
 
-void DIM_Shutdown(void)
+void dim_shutdown(void)
 {
 	g.dim_is_initialized = false;
 	dim2_cleanup();
 }
 
-bool DIM_GetLockState(void)
+bool dim_get_lock_state(void)
 {
 	return dim2_is_mlb_locked();
 }
@@ -706,7 +706,7 @@ static u8 init_ctrl_async(struct dim_channel *ch, u8 type, u8 is_tx,
 	return DIM_NO_ERROR;
 }
 
-u16 DIM_NormCtrlAsyncBufferSize(u16 buf_size)
+u16 dim_norm_ctrl_async_buffer_size(u16 buf_size)
 {
 	return norm_ctrl_async_buffer_size(buf_size);
 }
@@ -717,7 +717,7 @@ u16 DIM_NormCtrlAsyncBufferSize(u16 buf_size)
  *
  * Returns non-zero correct buffer size or zero by error.
  */
-u16 DIM_NormIsocBufferSize(u16 buf_size, u16 packet_length)
+u16 dim_norm_isoc_buffer_size(u16 buf_size, u16 packet_length)
 {
 	if (!check_packet_length(packet_length))
 		return 0;
@@ -731,7 +731,7 @@ u16 DIM_NormIsocBufferSize(u16 buf_size, u16 packet_length)
  *
  * Returns non-zero correct buffer size or zero by error.
  */
-u16 DIM_NormSyncBufferSize(u16 buf_size, u16 bytes_per_frame)
+u16 dim_norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame)
 {
 	if (!check_bytes_per_frame(bytes_per_frame))
 		return 0;
@@ -739,22 +739,22 @@ u16 DIM_NormSyncBufferSize(u16 buf_size, u16 bytes_per_frame)
 	return norm_sync_buffer_size(buf_size, bytes_per_frame);
 }
 
-u8 DIM_InitControl(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		   u16 max_buffer_size)
+u8 dim_init_control(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		    u16 max_buffer_size)
 {
 	return init_ctrl_async(ch, CAT_CT_VAL_CONTROL, is_tx, ch_address,
 			       max_buffer_size);
 }
 
-u8 DIM_InitAsync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		 u16 max_buffer_size)
+u8 dim_init_async(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		  u16 max_buffer_size)
 {
 	return init_ctrl_async(ch, CAT_CT_VAL_ASYNC, is_tx, ch_address,
 			       max_buffer_size);
 }
 
-u8 DIM_InitIsoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		u16 packet_length)
+u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 packet_length)
 {
 	if (!g.dim_is_initialized || !ch)
 		return DIM_ERR_DRIVER_NOT_INITIALIZED;
@@ -778,8 +778,8 @@ u8 DIM_InitIsoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
 	return DIM_NO_ERROR;
 }
 
-u8 DIM_InitSync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		u16 bytes_per_frame)
+u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 bytes_per_frame)
 {
 	if (!g.dim_is_initialized || !ch)
 		return DIM_ERR_DRIVER_NOT_INITIALIZED;
@@ -803,7 +803,7 @@ u8 DIM_InitSync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
 	return DIM_NO_ERROR;
 }
 
-u8 DIM_DestroyChannel(struct dim_channel *ch)
+u8 dim_destroy_channel(struct dim_channel *ch)
 {
 	if (!g.dim_is_initialized || !ch)
 		return DIM_ERR_DRIVER_NOT_INITIALIZED;
@@ -816,7 +816,7 @@ u8 DIM_DestroyChannel(struct dim_channel *ch)
 	return DIM_NO_ERROR;
 }
 
-void DIM_ServiceIrq(struct dim_channel *const *channels)
+void dim_service_irq(struct dim_channel *const *channels)
 {
 	bool state_changed;
 
@@ -850,11 +850,11 @@ void DIM_ServiceIrq(struct dim_channel *const *channels)
 	} while (state_changed);
 
 	/* clear pending Interrupts */
-	DIMCB_IoWrite(&g.dim2->MS0, 0);
-	DIMCB_IoWrite(&g.dim2->MS1, 0);
+	dimcb_io_write(&g.dim2->MS0, 0);
+	dimcb_io_write(&g.dim2->MS1, 0);
 }
 
-u8 DIM_ServiceChannel(struct dim_channel *ch)
+u8 dim_service_channel(struct dim_channel *ch)
 {
 	if (!g.dim_is_initialized || !ch)
 		return DIM_ERR_DRIVER_NOT_INITIALIZED;
@@ -862,8 +862,8 @@ u8 DIM_ServiceChannel(struct dim_channel *ch)
 	return channel_service(ch);
 }
 
-struct dim_ch_state_t *DIM_GetChannelState(struct dim_channel *ch,
-					   struct dim_ch_state_t *state_ptr)
+struct dim_ch_state_t *dim_get_channel_state(struct dim_channel *ch,
+					     struct dim_ch_state_t *state_ptr)
 {
 	if (!ch || !state_ptr)
 		return NULL;
@@ -874,7 +874,8 @@ struct dim_ch_state_t *DIM_GetChannelState(struct dim_channel *ch,
 	return state_ptr;
 }
 
-bool DIM_EnqueueBuffer(struct dim_channel *ch, u32 buffer_addr, u16 buffer_size)
+bool dim_enqueue_buffer(struct dim_channel *ch, u32 buffer_addr,
+			u16 buffer_size)
 {
 	if (!ch)
 		return dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED,
@@ -883,7 +884,7 @@ bool DIM_EnqueueBuffer(struct dim_channel *ch, u32 buffer_addr, u16 buffer_size)
 	return channel_start(ch, buffer_addr, buffer_size);
 }
 
-bool DIM_DetachBuffers(struct dim_channel *ch, u16 buffers_number)
+bool dim_detach_buffers(struct dim_channel *ch, u16 buffers_number)
 {
 	if (!ch)
 		return dim_on_error(DIM_ERR_DRIVER_NOT_INITIALIZED,
diff --git a/drivers/staging/most/hdm-dim2/dim2_hal.h b/drivers/staging/most/hdm-dim2/dim2_hal.h
index ebb7d87a45fc..48cdd9c8cde1 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hal.h
+++ b/drivers/staging/most/hdm-dim2/dim2_hal.h
@@ -17,7 +17,6 @@
 
 #include <linux/types.h>
 
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -66,51 +65,49 @@ struct dim_channel {
 	u16 done_sw_buffers_number; /*< Done software buffers number. */
 };
 
+u8 dim_startup(void *dim_base_address, u32 mlb_clock);
 
-u8 DIM_Startup(void *dim_base_address, u32 mlb_clock);
-
-void DIM_Shutdown(void);
-
-bool DIM_GetLockState(void);
+void dim_shutdown(void);
 
-u16 DIM_NormCtrlAsyncBufferSize(u16 buf_size);
+bool dim_get_lock_state(void);
 
-u16 DIM_NormIsocBufferSize(u16 buf_size, u16 packet_length);
+u16 dim_norm_ctrl_async_buffer_size(u16 buf_size);
 
-u16 DIM_NormSyncBufferSize(u16 buf_size, u16 bytes_per_frame);
+u16 dim_norm_isoc_buffer_size(u16 buf_size, u16 packet_length);
 
-u8 DIM_InitControl(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		   u16 max_buffer_size);
+u16 dim_norm_sync_buffer_size(u16 buf_size, u16 bytes_per_frame);
 
-u8 DIM_InitAsync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		 u16 max_buffer_size);
+u8 dim_init_control(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		    u16 max_buffer_size);
 
-u8 DIM_InitIsoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		u16 packet_length);
+u8 dim_init_async(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		  u16 max_buffer_size);
 
-u8 DIM_InitSync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
-		u16 bytes_per_frame);
+u8 dim_init_isoc(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 packet_length);
 
-u8 DIM_DestroyChannel(struct dim_channel *ch);
+u8 dim_init_sync(struct dim_channel *ch, u8 is_tx, u16 ch_address,
+		 u16 bytes_per_frame);
 
-void DIM_ServiceIrq(struct dim_channel *const *channels);
+u8 dim_destroy_channel(struct dim_channel *ch);
 
-u8 DIM_ServiceChannel(struct dim_channel *ch);
+void dim_service_irq(struct dim_channel *const *channels);
 
-struct dim_ch_state_t *DIM_GetChannelState(struct dim_channel *ch,
-		struct dim_ch_state_t *dim_ch_state_ptr);
+u8 dim_service_channel(struct dim_channel *ch);
 
-bool DIM_EnqueueBuffer(struct dim_channel *ch, u32 buffer_addr,
-		       u16 buffer_size);
+struct dim_ch_state_t *dim_get_channel_state(struct dim_channel *ch,
+					     struct dim_ch_state_t *state_ptr);
 
-bool DIM_DetachBuffers(struct dim_channel *ch, u16 buffers_number);
+bool dim_enqueue_buffer(struct dim_channel *ch, u32 buffer_addr,
+			u16 buffer_size);
 
-u32 DIMCB_IoRead(u32 *ptr32);
+bool dim_detach_buffers(struct dim_channel *ch, u16 buffers_number);
 
-void DIMCB_IoWrite(u32 *ptr32, u32 value);
+u32 dimcb_io_read(u32 *ptr32);
 
-void DIMCB_OnError(u8 error_id, const char *error_message);
+void dimcb_io_write(u32 *ptr32, u32 value);
 
+void dimcb_on_error(u8 error_id, const char *error_message);
 
 #ifdef __cplusplus
 }
diff --git a/drivers/staging/most/hdm-dim2/dim2_hdm.c b/drivers/staging/most/hdm-dim2/dim2_hdm.c
index b6fe346f73b0..327d738c7194 100644
--- a/drivers/staging/most/hdm-dim2/dim2_hdm.c
+++ b/drivers/staging/most/hdm-dim2/dim2_hdm.c
@@ -73,8 +73,8 @@ struct hdm_channel {
 	char name[sizeof "caNNN"];
 	bool is_initialized;
 	struct dim_channel ch;
-	struct list_head pending_list;	/* before DIM_EnqueueBuffer() */
-	struct list_head started_list;	/* after DIM_EnqueueBuffer() */
+	struct list_head pending_list;	/* before dim_enqueue_buffer() */
+	struct list_head started_list;	/* after dim_enqueue_buffer() */
 	enum most_channel_direction direction;
 	enum most_channel_data_type data_type;
 };
@@ -128,40 +128,40 @@ bool dim2_sysfs_get_state_cb(void)
 	unsigned long flags;
 
 	spin_lock_irqsave(&dim_lock, flags);
-	state = DIM_GetLockState();
+	state = dim_get_lock_state();
 	spin_unlock_irqrestore(&dim_lock, flags);
 
 	return state;
 }
 
 /**
- * DIMCB_IoRead - callback from HAL to read an I/O register
+ * dimcb_io_read - callback from HAL to read an I/O register
  * @ptr32: register address
  */
-u32 DIMCB_IoRead(u32 *ptr32)
+u32 dimcb_io_read(u32 *ptr32)
 {
 	return __raw_readl(ptr32);
 }
 
 /**
- * DIMCB_IoWrite - callback from HAL to write value to an I/O register
+ * dimcb_io_write - callback from HAL to write value to an I/O register
  * @ptr32: register address
  * @value: value to write
  */
-void DIMCB_IoWrite(u32 *ptr32, u32 value)
+void dimcb_io_write(u32 *ptr32, u32 value)
 {
 	__raw_writel(value, ptr32);
 }
 
 /**
- * DIMCB_OnError - callback from HAL to report miscommunication between
+ * dimcb_on_error - callback from HAL to report miscommunication between
  * HDM and HAL
  * @error_id: Error ID
  * @error_message: Error message. Some text in a free format
  */
-void DIMCB_OnError(u8 error_id, const char *error_message)
+void dimcb_on_error(u8 error_id, const char *error_message)
 {
-	pr_err("DIMCB_OnError: error_id - %d, error_message - %s\n", error_id,
+	pr_err("dimcb_on_error: error_id - %d, error_message - %s\n", error_id,
 	       error_message);
 }
 
@@ -212,9 +212,9 @@ static int startup_dim(struct platform_device *pdev)
 			return ret;
 	}
 
-	hal_ret = DIM_Startup(dev->io_base, dev->clk_speed);
+	hal_ret = dim_startup(dev->io_base, dev->clk_speed);
 	if (hal_ret != DIM_NO_ERROR) {
-		pr_err("DIM_Startup failed: %d\n", hal_ret);
+		pr_err("dim_startup failed: %d\n", hal_ret);
 		if (pdata && pdata->destroy)
 			pdata->destroy(pdata);
 		return -ENODEV;
@@ -246,7 +246,7 @@ static int try_start_dim_transfer(struct hdm_channel *hdm_ch)
 		return -EAGAIN;
 	}
 
-	if (!DIM_GetChannelState(&hdm_ch->ch, &st)->ready) {
+	if (!dim_get_channel_state(&hdm_ch->ch, &st)->ready) {
 		spin_unlock_irqrestore(&dim_lock, flags);
 		return -EAGAIN;
 	}
@@ -255,7 +255,7 @@ static int try_start_dim_transfer(struct hdm_channel *hdm_ch)
 	buf_size = mbo->buffer_length;
 
 	BUG_ON(mbo->bus_address == 0);
-	if (!DIM_EnqueueBuffer(&hdm_ch->ch, mbo->bus_address, buf_size)) {
+	if (!dim_enqueue_buffer(&hdm_ch->ch, mbo->bus_address, buf_size)) {
 		list_del(head->next);
 		spin_unlock_irqrestore(&dim_lock, flags);
 		mbo->processed_length = 0;
@@ -340,13 +340,13 @@ static void service_done_flag(struct dim2_hdm *dev, int ch_idx)
 
 	spin_lock_irqsave(&dim_lock, flags);
 
-	done_buffers = DIM_GetChannelState(&hdm_ch->ch, &st)->done_buffers;
+	done_buffers = dim_get_channel_state(&hdm_ch->ch, &st)->done_buffers;
 	if (!done_buffers) {
 		spin_unlock_irqrestore(&dim_lock, flags);
 		return;
 	}
 
-	if (!DIM_DetachBuffers(&hdm_ch->ch, done_buffers)) {
+	if (!dim_detach_buffers(&hdm_ch->ch, done_buffers)) {
 		spin_unlock_irqrestore(&dim_lock, flags);
 		return;
 	}
@@ -371,7 +371,6 @@ static void service_done_flag(struct dim2_hdm *dev, int ch_idx)
 		if (hdm_ch->data_type == MOST_CH_ASYNC &&
 		    hdm_ch->direction == MOST_CH_RX &&
 		    PACKET_IS_NET_INFO(data)) {
-
 			retrieve_netinfo(dev, mbo);
 
 			spin_lock_irqsave(&dim_lock, flags);
@@ -380,7 +379,6 @@ static void service_done_flag(struct dim2_hdm *dev, int ch_idx)
 		} else {
 			if (hdm_ch->data_type == MOST_CH_CONTROL ||
 			    hdm_ch->data_type == MOST_CH_ASYNC) {
-
 				u32 const data_size =
 					(u32)data[0] * 256 + data[1] + 2;
 
@@ -430,7 +428,7 @@ static void dim2_tasklet_fn(unsigned long data)
 			continue;
 
 		spin_lock_irqsave(&dim_lock, flags);
-		DIM_ServiceChannel(&dev->hch[ch_idx].ch);
+		dim_service_channel(&dev->hch[ch_idx].ch);
 		spin_unlock_irqrestore(&dim_lock, flags);
 
 		service_done_flag(dev, ch_idx);
@@ -454,7 +452,7 @@ static irqreturn_t dim2_ahb_isr(int irq, void *_dev)
 	unsigned long flags;
 
 	spin_lock_irqsave(&dim_lock, flags);
-	DIM_ServiceIrq(get_active_channels(dev, buffer));
+	dim_service_irq(get_active_channels(dev, buffer));
 	spin_unlock_irqrestore(&dim_lock, flags);
 
 #if !defined(ENABLE_HDM_TEST)
@@ -536,7 +534,7 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
 
 	switch (ccfg->data_type) {
 	case MOST_CH_CONTROL:
-		new_size = DIM_NormCtrlAsyncBufferSize(buf_size);
+		new_size = dim_norm_ctrl_async_buffer_size(buf_size);
 		if (new_size == 0) {
 			pr_err("%s: too small buffer size\n", hdm_ch->name);
 			return -EINVAL;
@@ -546,11 +544,11 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
 			pr_warn("%s: fixed buffer size (%d -> %d)\n",
 				hdm_ch->name, buf_size, new_size);
 		spin_lock_irqsave(&dim_lock, flags);
-		hal_ret = DIM_InitControl(&hdm_ch->ch, is_tx, ch_addr,
-					  buf_size);
+		hal_ret = dim_init_control(&hdm_ch->ch, is_tx, ch_addr,
+					   buf_size);
 		break;
 	case MOST_CH_ASYNC:
-		new_size = DIM_NormCtrlAsyncBufferSize(buf_size);
+		new_size = dim_norm_ctrl_async_buffer_size(buf_size);
 		if (new_size == 0) {
 			pr_err("%s: too small buffer size\n", hdm_ch->name);
 			return -EINVAL;
@@ -560,10 +558,10 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
 			pr_warn("%s: fixed buffer size (%d -> %d)\n",
 				hdm_ch->name, buf_size, new_size);
 		spin_lock_irqsave(&dim_lock, flags);
-		hal_ret = DIM_InitAsync(&hdm_ch->ch, is_tx, ch_addr, buf_size);
+		hal_ret = dim_init_async(&hdm_ch->ch, is_tx, ch_addr, buf_size);
 		break;
 	case MOST_CH_ISOC_AVP:
-		new_size = DIM_NormIsocBufferSize(buf_size, sub_size);
+		new_size = dim_norm_isoc_buffer_size(buf_size, sub_size);
 		if (new_size == 0) {
 			pr_err("%s: invalid sub-buffer size or too small buffer size\n",
 			       hdm_ch->name);
@@ -574,10 +572,10 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
 			pr_warn("%s: fixed buffer size (%d -> %d)\n",
 				hdm_ch->name, buf_size, new_size);
 		spin_lock_irqsave(&dim_lock, flags);
-		hal_ret = DIM_InitIsoc(&hdm_ch->ch, is_tx, ch_addr, sub_size);
+		hal_ret = dim_init_isoc(&hdm_ch->ch, is_tx, ch_addr, sub_size);
 		break;
 	case MOST_CH_SYNC:
-		new_size = DIM_NormSyncBufferSize(buf_size, sub_size);
+		new_size = dim_norm_sync_buffer_size(buf_size, sub_size);
 		if (new_size == 0) {
 			pr_err("%s: invalid sub-buffer size or too small buffer size\n",
 			       hdm_ch->name);
@@ -588,7 +586,7 @@ static int configure_channel(struct most_interface *most_iface, int ch_idx,
 			pr_warn("%s: fixed buffer size (%d -> %d)\n",
 				hdm_ch->name, buf_size, new_size);
 		spin_lock_irqsave(&dim_lock, flags);
-		hal_ret = DIM_InitSync(&hdm_ch->ch, is_tx, ch_addr, sub_size);
+		hal_ret = dim_init_sync(&hdm_ch->ch, is_tx, ch_addr, sub_size);
 		break;
 	default:
 		pr_err("%s: configure failed, bad channel type: %d\n",
@@ -708,7 +706,7 @@ static int poison_channel(struct most_interface *most_iface, int ch_idx)
 		return -EPERM;
 
 	spin_lock_irqsave(&dim_lock, flags);
-	hal_ret = DIM_DestroyChannel(&hdm_ch->ch);
+	hal_ret = dim_destroy_channel(&hdm_ch->ch);
 	hdm_ch->is_initialized = false;
 	if (ch_idx == dev->atx_idx)
 		dev->atx_idx = -1;
@@ -885,7 +883,7 @@ static int dim2_remove(struct platform_device *pdev)
 	unsigned long flags;
 
 	spin_lock_irqsave(&dim_lock, flags);
-	DIM_Shutdown();
+	dim_shutdown();
 	spin_unlock_irqrestore(&dim_lock, flags);
 
 	if (pdata && pdata->destroy)
diff --git a/drivers/staging/most/hdm-dim2/dim2_reg.h b/drivers/staging/most/hdm-dim2/dim2_reg.h
index 476f66f4c566..bcf6a79f6744 100644
--- a/drivers/staging/most/hdm-dim2/dim2_reg.h
+++ b/drivers/staging/most/hdm-dim2/dim2_reg.h
@@ -21,7 +21,6 @@
 extern "C" {
 #endif
 
-
 struct dim2_regs {
 	/* 0x00 */ u32 MLBC0;
 	/* 0x01 */ u32 rsvd0[1];
@@ -67,8 +66,7 @@ struct dim2_regs {
 	/* 0xF7 */ u32 ACMR1;
 };
 
-
-#define DIM2_MASK(n)  (~((~(u32)0)<<(n)))
+#define DIM2_MASK(n)  (~((~(u32)0) << (n)))
 
 enum {
 	MLBC0_MLBLK_BIT = 7,
@@ -168,7 +166,6 @@ enum {
 	CAT_CL_MASK = DIM2_MASK(6)
 };
 
-
 #ifdef	__cplusplus
 }
 #endif
diff --git a/drivers/staging/most/hdm-dim2/dim2_sysfs.h b/drivers/staging/most/hdm-dim2/dim2_sysfs.h
index e719691035b0..b71dd027ebc7 100644
--- a/drivers/staging/most/hdm-dim2/dim2_sysfs.h
+++ b/drivers/staging/most/hdm-dim2/dim2_sysfs.h
@@ -16,10 +16,8 @@
 #ifndef DIM2_SYSFS_H
 #define	DIM2_SYSFS_H
 
-
 #include <linux/kobject.h>
 
-
 struct medialb_bus {
 	struct kobject kobj_group;
 };
@@ -35,5 +33,4 @@ void dim2_sysfs_destroy(struct medialb_bus *bus);
  */
 bool dim2_sysfs_get_state_cb(void);
 
-
 #endif	/* DIM2_SYSFS_H */
diff --git a/drivers/staging/most/mostcore/core.c b/drivers/staging/most/mostcore/core.c
index 19852ca7e59c..ed1ed25b6d1d 100644
--- a/drivers/staging/most/mostcore/core.c
+++ b/drivers/staging/most/mostcore/core.c
@@ -1587,8 +1587,7 @@ out:
 	return 0;
 
 error:
-	if (iface->mod)
-		module_put(iface->mod);
+	module_put(iface->mod);
 	modref--;
 	mutex_unlock(&c->start_mutex);
 	return ret;
diff --git a/drivers/staging/most/mostcore/mostcore.h b/drivers/staging/most/mostcore/mostcore.h
index e148b324331a..bda3850d5435 100644
--- a/drivers/staging/most/mostcore/mostcore.h
+++ b/drivers/staging/most/mostcore/mostcore.h
@@ -60,7 +60,6 @@ enum most_channel_data_type {
 	MOST_CH_SYNC = 1 << 5,
 };
 
-
 enum mbo_status_flags {
 	/* MBO was processed successfully (data was send or received )*/
 	MBO_SUCCESS = 0,
@@ -317,5 +316,4 @@ int most_start_channel(struct most_interface *iface, int channel_idx,
 int most_stop_channel(struct most_interface *iface, int channel_idx,
 		      struct most_aim *);
 
-
 #endif /* MOST_CORE_H_ */
diff --git a/drivers/staging/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index 47bb56f1f8c0..197d1124733d 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -31,8 +31,8 @@
 
 static inline struct spinand_state *mtd_to_state(struct mtd_info *mtd)
 {
-	struct nand_chip *chip = (struct nand_chip *)mtd->priv;
-	struct spinand_info *info = (struct spinand_info *)chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct spinand_info *info = nand_get_controller_data(chip);
 	struct spinand_state *state = (struct spinand_state *)info->priv;
 
 	return state;
@@ -633,7 +633,7 @@ static int spinand_read_page_hwecc(struct mtd_info *mtd, struct nand_chip *chip,
 	u8 *p = buf;
 	int eccsize = chip->ecc.size;
 	int eccsteps = chip->ecc.steps;
-	struct spinand_info *info = (struct spinand_info *)chip->priv;
+	struct spinand_info *info = nand_get_controller_data(chip);
 
 	enable_read_hw_ecc = 1;
 
@@ -679,7 +679,7 @@ static u8 spinand_read_byte(struct mtd_info *mtd)
 
 static int spinand_wait(struct mtd_info *mtd, struct nand_chip *chip)
 {
-	struct spinand_info *info = (struct spinand_info *)chip->priv;
+	struct spinand_info *info = nand_get_controller_data(chip);
 
 	unsigned long timeo = jiffies;
 	int retval, state = chip->state;
@@ -744,8 +744,8 @@ static void spinand_reset(struct spi_device *spi_nand)
 static void spinand_cmdfunc(struct mtd_info *mtd, unsigned int command,
 			    int column, int page)
 {
-	struct nand_chip *chip = (struct nand_chip *)mtd->priv;
-	struct spinand_info *info = (struct spinand_info *)chip->priv;
+	struct nand_chip *chip = mtd_to_nand(mtd);
+	struct spinand_info *info = nand_get_controller_data(chip);
 	struct spinand_state *state = (struct spinand_state *)info->priv;
 
 	switch (command) {
@@ -850,7 +850,6 @@ static int spinand_probe(struct spi_device *spi_nand)
 	struct nand_chip *chip;
 	struct spinand_info *info;
 	struct spinand_state *state;
-	struct mtd_part_parser_data ppdata;
 
 	info  = devm_kzalloc(&spi_nand->dev, sizeof(struct spinand_info),
 			     GFP_KERNEL);
@@ -894,7 +893,8 @@ static int spinand_probe(struct spi_device *spi_nand)
 		pr_info("%s: disable ecc failed!\n", __func__);
 #endif
 
-	chip->priv	= info;
+	nand_set_flash_node(chip, spi_nand->dev.of_node);
+	nand_set_controller_data(chip, info);
 	chip->read_buf	= spinand_read_buf;
 	chip->write_buf	= spinand_write_buf;
 	chip->read_byte	= spinand_read_byte;
@@ -903,21 +903,17 @@ static int spinand_probe(struct spi_device *spi_nand)
 	chip->options	|= NAND_CACHEPRG;
 	chip->select_chip = spinand_select_chip;
 
-	mtd = devm_kzalloc(&spi_nand->dev, sizeof(struct mtd_info), GFP_KERNEL);
-	if (!mtd)
-		return -ENOMEM;
+	mtd = nand_to_mtd(chip);
 
 	dev_set_drvdata(&spi_nand->dev, mtd);
 
-	mtd->priv = chip;
 	mtd->dev.parent = &spi_nand->dev;
 	mtd->oobsize = 64;
 
 	if (nand_scan(mtd, 1))
 		return -ENXIO;
 
-	ppdata.of_node = spi_nand->dev.of_node;
-	return mtd_device_parse_register(mtd, NULL, &ppdata, NULL, 0);
+	return mtd_device_register(mtd, NULL, 0);
 }
 
 /*
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index 8ae01753b011..0b4e819f5164 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -165,13 +165,18 @@ static void xlr_net_fmn_handler(int bkt, int src_stnid, int size,
 	}
 }
 
+static struct phy_device *xlr_get_phydev(struct xlr_net_priv *priv)
+{
+	return mdiobus_get_phy(priv->mii_bus, priv->phy_addr);
+}
+
 /*
  * Ethtool operation
  */
 static int xlr_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 {
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	if (!phydev)
 		return -ENODEV;
@@ -181,7 +186,7 @@ static int xlr_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 static int xlr_set_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 {
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	if (!phydev)
 		return -ENODEV;
@@ -218,7 +223,7 @@ static int xlr_net_open(struct net_device *ndev)
 {
 	u32 err;
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	/* schedule a link state check */
 	phy_start(phydev);
@@ -239,7 +244,7 @@ static int xlr_net_open(struct net_device *ndev)
 static int xlr_net_stop(struct net_device *ndev)
 {
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	phy_stop(phydev);
 	netif_tx_stop_all_queues(ndev);
@@ -268,7 +273,7 @@ static void __maybe_unused xlr_wakeup_queue(unsigned long dev)
 {
 	struct net_device *ndev = (struct net_device *) dev;
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	if (phydev->link)
 		netif_tx_wake_queue(netdev_get_tx_queue(ndev, priv->wakeup_q));
@@ -771,7 +776,7 @@ static void xlr_sgmii_init(struct xlr_net_priv *priv)
 
 void xlr_set_gmac_speed(struct xlr_net_priv *priv)
 {
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 	int speed;
 
 	if (phydev->interface == PHY_INTERFACE_MODE_SGMII)
@@ -813,7 +818,7 @@ void xlr_set_gmac_speed(struct xlr_net_priv *priv)
 static void xlr_gmac_link_adjust(struct net_device *ndev)
 {
 	struct xlr_net_priv *priv = netdev_priv(ndev);
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 	u32 intreg;
 
 	intreg = xlr_nae_rdreg(priv->base_addr, R_INTREG);
@@ -830,7 +835,7 @@ static void xlr_gmac_link_adjust(struct net_device *ndev)
 
 static int xlr_mii_probe(struct xlr_net_priv *priv)
 {
-	struct phy_device *phydev = priv->mii_bus->phy_map[priv->phy_addr];
+	struct phy_device *phydev = xlr_get_phydev(priv);
 
 	if (!phydev) {
 		pr_err("no PHY found on phy_addr %d\n", priv->phy_addr);
@@ -838,8 +843,8 @@ static int xlr_mii_probe(struct xlr_net_priv *priv)
 	}
 
 	/* Attach MAC to PHY */
-	phydev = phy_connect(priv->ndev, dev_name(&phydev->dev),
-			&xlr_gmac_link_adjust, priv->nd->phy_interface);
+	phydev = phy_connect(priv->ndev, phydev_name(phydev),
+			     &xlr_gmac_link_adjust, priv->nd->phy_interface);
 
 	if (IS_ERR(phydev)) {
 		pr_err("could not attach PHY\n");
@@ -854,8 +859,7 @@ static int xlr_mii_probe(struct xlr_net_priv *priv)
 				| ADVERTISED_MII);
 
 	phydev->advertising = phydev->supported;
-	pr_info("attached PHY driver [%s] (mii_bus:phy_addr=%s\n",
-		phydev->drv->name, dev_name(&phydev->dev));
+	phy_attached_info(phydev);
 	return 0;
 }
 
@@ -877,14 +881,6 @@ static int xlr_setup_mdio(struct xlr_net_priv *priv,
 	priv->mii_bus->read = xlr_mii_read;
 	priv->mii_bus->write = xlr_mii_write;
 	priv->mii_bus->parent = &pdev->dev;
-	priv->mii_bus->irq = kmalloc(sizeof(int)*PHY_MAX_ADDR, GFP_KERNEL);
-	if (priv->mii_bus->irq == NULL) {
-		pr_err("irq alloc failed\n");
-		mdiobus_free(priv->mii_bus);
-		return -ENOMEM;
-	}
-
-	priv->mii_bus->irq[priv->phy_addr] = priv->ndev->irq;
 
 	/* Scan only the enabled address */
 	priv->mii_bus->phy_mask = ~(1 << priv->phy_addr);
diff --git a/drivers/staging/nvec/README b/drivers/staging/nvec/README
index 9a320b7fdbe6..0e2d5c4c875f 100644
--- a/drivers/staging/nvec/README
+++ b/drivers/staging/nvec/README
@@ -1,4 +1,4 @@
-NVEC: An NVidia compliant Embedded Controller Protocol Implemenation
+NVEC: An NVidia compliant Embedded Controller Protocol Implementation
 
 This is an implementation of the NVEC protocol used to communicate with an
 embedded controller (EC) via I2C bus. The EC is an I2C master while the host
diff --git a/drivers/staging/nvec/nvec.c b/drivers/staging/nvec/nvec.c
index 802c9597d421..4ae44a5168f9 100644
--- a/drivers/staging/nvec/nvec.c
+++ b/drivers/staging/nvec/nvec.c
@@ -40,18 +40,18 @@
 #include "nvec.h"
 
 #define I2C_CNFG			0x00
-#define I2C_CNFG_PACKET_MODE_EN		(1<<10)
-#define I2C_CNFG_NEW_MASTER_SFM		(1<<11)
+#define I2C_CNFG_PACKET_MODE_EN		(1 << 10)
+#define I2C_CNFG_NEW_MASTER_SFM		(1 << 11)
 #define I2C_CNFG_DEBOUNCE_CNT_SHIFT	12
 
 #define I2C_SL_CNFG		0x20
-#define I2C_SL_NEWSL		(1<<2)
-#define I2C_SL_NACK		(1<<1)
-#define I2C_SL_RESP		(1<<0)
-#define I2C_SL_IRQ		(1<<3)
-#define END_TRANS		(1<<4)
-#define RCVD			(1<<2)
-#define RNW			(1<<1)
+#define I2C_SL_NEWSL		(1 << 2)
+#define I2C_SL_NACK		(1 << 1)
+#define I2C_SL_RESP		(1 << 0)
+#define I2C_SL_IRQ		(1 << 3)
+#define END_TRANS		(1 << 4)
+#define RCVD			(1 << 2)
+#define RNW			(1 << 1)
 
 #define I2C_SL_RCVD		0x24
 #define I2C_SL_STATUS		0x28
@@ -740,7 +740,7 @@ static void tegra_init_i2c_slave(struct nvec_chip *nvec)
 	writel(I2C_SL_NEWSL, nvec->base + I2C_SL_CNFG);
 	writel(0x1E, nvec->base + I2C_SL_DELAY_COUNT);
 
-	writel(nvec->i2c_addr>>1, nvec->base + I2C_SL_ADDR1);
+	writel(nvec->i2c_addr >> 1, nvec->base + I2C_SL_ADDR1);
 	writel(0, nvec->base + I2C_SL_ADDR2);
 
 	enable_irq(nvec->irq);
diff --git a/drivers/staging/octeon/ethernet-defines.h b/drivers/staging/octeon/ethernet-defines.h
index 13e4cee1f86d..07bd2b87f6a0 100644
--- a/drivers/staging/octeon/ethernet-defines.h
+++ b/drivers/staging/octeon/ethernet-defines.h
@@ -40,6 +40,6 @@
 #define FAU_TOTAL_TX_TO_CLEAN (CVMX_FAU_REG_END - sizeof(u32))
 #define FAU_NUM_PACKET_BUFFERS_TO_FREE (FAU_TOTAL_TX_TO_CLEAN - sizeof(u32))
 
-#define TOTAL_NUMBER_OF_PORTS       (CVMX_PIP_NUM_INPUT_PORTS+1)
+#define TOTAL_NUMBER_OF_PORTS       (CVMX_PIP_NUM_INPUT_PORTS + 1)
 
 #endif /* __ETHERNET_DEFINES_H__ */
diff --git a/drivers/staging/octeon/ethernet-rgmii.c b/drivers/staging/octeon/ethernet-rgmii.c
index 613344b886e1..1055ee14b66a 100644
--- a/drivers/staging/octeon/ethernet-rgmii.c
+++ b/drivers/staging/octeon/ethernet-rgmii.c
@@ -78,7 +78,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 		 */
 		spin_lock_irqsave(&global_register_lock, flags);
 	} else {
-		mutex_lock(&priv->phydev->bus->mdio_lock);
+		mutex_lock(&priv->phydev->mdio.bus->mdio_lock);
 	}
 
 	link_info = cvmx_helper_link_get(priv->port);
@@ -113,7 +113,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 		if (use_global_register_lock)
 			spin_unlock_irqrestore(&global_register_lock, flags);
 		else
-			mutex_unlock(&priv->phydev->bus->mdio_lock);
+			mutex_unlock(&priv->phydev->mdio.bus->mdio_lock);
 		return;
 	}
 
@@ -132,7 +132,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 	if (use_global_register_lock)
 		spin_unlock_irqrestore(&global_register_lock, flags);
 	else
-		mutex_unlock(&priv->phydev->bus->mdio_lock);
+		mutex_unlock(&priv->phydev->mdio.bus->mdio_lock);
 
 	if (priv->phydev == NULL) {
 		/* Tell core. */
diff --git a/drivers/staging/panel/panel.c b/drivers/staging/panel/panel.c
index 79ac19246548..70b8f4fabfad 100644
--- a/drivers/staging/panel/panel.c
+++ b/drivers/staging/panel/panel.c
@@ -825,8 +825,7 @@ static void lcd_write_cmd_s(int cmd)
 	lcd_send_serial(0x1F);	/* R/W=W, RS=0 */
 	lcd_send_serial(cmd & 0x0F);
 	lcd_send_serial((cmd >> 4) & 0x0F);
-	/* the shortest command takes at least 40 us */
-	usleep_range(40, 100);
+	udelay(40);		/* the shortest command takes at least 40 us */
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -837,8 +836,7 @@ static void lcd_write_data_s(int data)
 	lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
 	lcd_send_serial(data & 0x0F);
 	lcd_send_serial((data >> 4) & 0x0F);
-	/* the shortest data takes at least 40 us */
-	usleep_range(40, 100);
+	udelay(40);		/* the shortest data takes at least 40 us */
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -848,20 +846,19 @@ static void lcd_write_cmd_p8(int cmd)
 	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, cmd);
-	/* maintain the data during 20 us before the strobe */
-	usleep_range(20, 100);
+	udelay(20);	/* maintain the data during 20 us before the strobe */
 
 	bits.e = BIT_SET;
 	bits.rs = BIT_CLR;
 	bits.rw = BIT_CLR;
 	set_ctrl_bits();
 
-	usleep_range(40, 100);	/* maintain the strobe during 40 us */
+	udelay(40);	/* maintain the strobe during 40 us */
 
 	bits.e = BIT_CLR;
 	set_ctrl_bits();
 
-	usleep_range(120, 500);	/* the shortest command takes at least 120 us */
+	udelay(120);	/* the shortest command takes at least 120 us */
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -871,20 +868,19 @@ static void lcd_write_data_p8(int data)
 	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, data);
-	/* maintain the data during 20 us before the strobe */
-	usleep_range(20, 100);
+	udelay(20);	/* maintain the data during 20 us before the strobe */
 
 	bits.e = BIT_SET;
 	bits.rs = BIT_SET;
 	bits.rw = BIT_CLR;
 	set_ctrl_bits();
 
-	usleep_range(40, 100);	/* maintain the strobe during 40 us */
+	udelay(40);	/* maintain the strobe during 40 us */
 
 	bits.e = BIT_CLR;
 	set_ctrl_bits();
 
-	usleep_range(45, 100);	/* the shortest data takes at least 45 us */
+	udelay(45);	/* the shortest data takes at least 45 us */
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -894,7 +890,7 @@ static void lcd_write_cmd_tilcd(int cmd)
 	spin_lock_irq(&pprt_lock);
 	/* present the data to the control port */
 	w_ctr(pprt, cmd);
-	usleep_range(60, 120);
+	udelay(60);
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -904,7 +900,7 @@ static void lcd_write_data_tilcd(int data)
 	spin_lock_irq(&pprt_lock);
 	/* present the data to the data port */
 	w_dtr(pprt, data);
-	usleep_range(60, 120);
+	udelay(60);
 	spin_unlock_irq(&pprt_lock);
 }
 
@@ -947,7 +943,7 @@ static void lcd_clear_fast_s(void)
 		lcd_send_serial(0x5F);	/* R/W=W, RS=1 */
 		lcd_send_serial(' ' & 0x0F);
 		lcd_send_serial((' ' >> 4) & 0x0F);
-		usleep_range(40, 100);	/* the shortest data takes at least 40 us */
+		udelay(40);	/* the shortest data takes at least 40 us */
 	}
 	spin_unlock_irq(&pprt_lock);
 
@@ -971,7 +967,7 @@ static void lcd_clear_fast_p8(void)
 		w_dtr(pprt, ' ');
 
 		/* maintain the data during 20 us before the strobe */
-		usleep_range(20, 100);
+		udelay(20);
 
 		bits.e = BIT_SET;
 		bits.rs = BIT_SET;
@@ -979,13 +975,13 @@ static void lcd_clear_fast_p8(void)
 		set_ctrl_bits();
 
 		/* maintain the strobe during 40 us */
-		usleep_range(40, 100);
+		udelay(40);
 
 		bits.e = BIT_CLR;
 		set_ctrl_bits();
 
 		/* the shortest data takes at least 45 us */
-		usleep_range(45, 100);
+		udelay(45);
 	}
 	spin_unlock_irq(&pprt_lock);
 
@@ -1007,7 +1003,7 @@ static void lcd_clear_fast_tilcd(void)
 	for (pos = 0; pos < lcd.height * lcd.hwidth; pos++) {
 		/* present the data to the data port */
 		w_dtr(pprt, ' ');
-		usleep_range(60, 120);
+		udelay(60);
 	}
 
 	spin_unlock_irq(&pprt_lock);
diff --git a/drivers/staging/rdma/Kconfig b/drivers/staging/rdma/Kconfig
index ba8765063174..f1f3ecadf0fb 100644
--- a/drivers/staging/rdma/Kconfig
+++ b/drivers/staging/rdma/Kconfig
@@ -22,12 +22,6 @@ menuconfig STAGING_RDMA
 # Please keep entries in alphabetic order
 if STAGING_RDMA
 
-source "drivers/staging/rdma/amso1100/Kconfig"
-
-source "drivers/staging/rdma/ehca/Kconfig"
-
 source "drivers/staging/rdma/hfi1/Kconfig"
 
-source "drivers/staging/rdma/ipath/Kconfig"
-
 endif
diff --git a/drivers/staging/rdma/Makefile b/drivers/staging/rdma/Makefile
index 139d78ef2c24..8c7fc1de48a7 100644
--- a/drivers/staging/rdma/Makefile
+++ b/drivers/staging/rdma/Makefile
@@ -1,5 +1,2 @@
 # Entries for RDMA_STAGING tree
-obj-$(CONFIG_INFINIBAND_AMSO1100)	+= amso1100/
-obj-$(CONFIG_INFINIBAND_EHCA)	+= ehca/
 obj-$(CONFIG_INFINIBAND_HFI1)	+= hfi1/
-obj-$(CONFIG_INFINIBAND_IPATH)	+= ipath/
diff --git a/drivers/staging/rdma/amso1100/Kbuild b/drivers/staging/rdma/amso1100/Kbuild
deleted file mode 100644
index 950dfabcd89d..000000000000
--- a/drivers/staging/rdma/amso1100/Kbuild
+++ /dev/null
@@ -1,6 +0,0 @@
-ccflags-$(CONFIG_INFINIBAND_AMSO1100_DEBUG) := -DDEBUG
-
-obj-$(CONFIG_INFINIBAND_AMSO1100) += iw_c2.o
-
-iw_c2-y := c2.o c2_provider.o c2_rnic.o c2_alloc.o c2_mq.o c2_ae.o c2_vq.o \
-	c2_intr.o c2_cq.o c2_qp.o c2_cm.o c2_mm.o c2_pd.o
diff --git a/drivers/staging/rdma/amso1100/Kconfig b/drivers/staging/rdma/amso1100/Kconfig
deleted file mode 100644
index e6ce5f209e47..000000000000
--- a/drivers/staging/rdma/amso1100/Kconfig
+++ /dev/null
@@ -1,15 +0,0 @@
-config INFINIBAND_AMSO1100
-	tristate "Ammasso 1100 HCA support"
-	depends on PCI && INET
-	---help---
-	  This is a low-level driver for the Ammasso 1100 host
-	  channel adapter (HCA).
-
-config INFINIBAND_AMSO1100_DEBUG
-	bool "Verbose debugging output"
-	depends on INFINIBAND_AMSO1100
-	default n
-	---help---
-	  This option causes the amso1100 driver to produce a bunch of
-	  debug messages.  Select this if you are developing the driver
-	  or trying to diagnose a problem.
diff --git a/drivers/staging/rdma/amso1100/TODO b/drivers/staging/rdma/amso1100/TODO
deleted file mode 100644
index 18b00a5cb549..000000000000
--- a/drivers/staging/rdma/amso1100/TODO
+++ /dev/null
@@ -1,4 +0,0 @@
-7/2015
-
-The amso1100 driver has been deprecated and moved to drivers/staging.
-It will be removed in the 4.6 merge window.
diff --git a/drivers/staging/rdma/amso1100/c2.c b/drivers/staging/rdma/amso1100/c2.c
deleted file mode 100644
index 35ac536b2d45..000000000000
--- a/drivers/staging/rdma/amso1100/c2.c
+++ /dev/null
@@ -1,1241 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/pci.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/inetdevice.h>
-#include <linux/interrupt.h>
-#include <linux/delay.h>
-#include <linux/ethtool.h>
-#include <linux/mii.h>
-#include <linux/if_vlan.h>
-#include <linux/crc32.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/tcp.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/slab.h>
-#include <linux/prefetch.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/byteorder.h>
-
-#include <rdma/ib_smi.h>
-#include "c2.h"
-#include "c2_provider.h"
-
-MODULE_AUTHOR("Tom Tucker <tom@opengridcomputing.com>");
-MODULE_DESCRIPTION("Ammasso AMSO1100 Low-level iWARP Driver");
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_VERSION(DRV_VERSION);
-
-static const u32 default_msg = NETIF_MSG_DRV | NETIF_MSG_PROBE | NETIF_MSG_LINK
-    | NETIF_MSG_IFUP | NETIF_MSG_IFDOWN;
-
-static int debug = -1;		/* defaults above */
-module_param(debug, int, 0);
-MODULE_PARM_DESC(debug, "Debug level (0=none,...,16=all)");
-
-static int c2_up(struct net_device *netdev);
-static int c2_down(struct net_device *netdev);
-static int c2_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
-static void c2_tx_interrupt(struct net_device *netdev);
-static void c2_rx_interrupt(struct net_device *netdev);
-static irqreturn_t c2_interrupt(int irq, void *dev_id);
-static void c2_tx_timeout(struct net_device *netdev);
-static int c2_change_mtu(struct net_device *netdev, int new_mtu);
-static void c2_reset(struct c2_port *c2_port);
-
-static struct pci_device_id c2_pci_table[] = {
-	{ PCI_DEVICE(0x18b8, 0xb001) },
-	{ 0 }
-};
-
-MODULE_DEVICE_TABLE(pci, c2_pci_table);
-
-static void c2_print_macaddr(struct net_device *netdev)
-{
-	pr_debug("%s: MAC %pM, IRQ %u\n", netdev->name, netdev->dev_addr, netdev->irq);
-}
-
-static void c2_set_rxbufsize(struct c2_port *c2_port)
-{
-	struct net_device *netdev = c2_port->netdev;
-
-	if (netdev->mtu > RX_BUF_SIZE)
-		c2_port->rx_buf_size =
-		    netdev->mtu + ETH_HLEN + sizeof(struct c2_rxp_hdr) +
-		    NET_IP_ALIGN;
-	else
-		c2_port->rx_buf_size = sizeof(struct c2_rxp_hdr) + RX_BUF_SIZE;
-}
-
-/*
- * Allocate TX ring elements and chain them together.
- * One-to-one association of adapter descriptors with ring elements.
- */
-static int c2_tx_ring_alloc(struct c2_ring *tx_ring, void *vaddr,
-			    dma_addr_t base, void __iomem * mmio_txp_ring)
-{
-	struct c2_tx_desc *tx_desc;
-	struct c2_txp_desc __iomem *txp_desc;
-	struct c2_element *elem;
-	int i;
-
-	tx_ring->start = kmalloc(sizeof(*elem) * tx_ring->count, GFP_KERNEL);
-	if (!tx_ring->start)
-		return -ENOMEM;
-
-	elem = tx_ring->start;
-	tx_desc = vaddr;
-	txp_desc = mmio_txp_ring;
-	for (i = 0; i < tx_ring->count; i++, elem++, tx_desc++, txp_desc++) {
-		tx_desc->len = 0;
-		tx_desc->status = 0;
-
-		/* Set TXP_HTXD_UNINIT */
-		__raw_writeq((__force u64) cpu_to_be64(0x1122334455667788ULL),
-			     (void __iomem *) txp_desc + C2_TXP_ADDR);
-		__raw_writew(0, (void __iomem *) txp_desc + C2_TXP_LEN);
-		__raw_writew((__force u16) cpu_to_be16(TXP_HTXD_UNINIT),
-			     (void __iomem *) txp_desc + C2_TXP_FLAGS);
-
-		elem->skb = NULL;
-		elem->ht_desc = tx_desc;
-		elem->hw_desc = txp_desc;
-
-		if (i == tx_ring->count - 1) {
-			elem->next = tx_ring->start;
-			tx_desc->next_offset = base;
-		} else {
-			elem->next = elem + 1;
-			tx_desc->next_offset =
-			    base + (i + 1) * sizeof(*tx_desc);
-		}
-	}
-
-	tx_ring->to_use = tx_ring->to_clean = tx_ring->start;
-
-	return 0;
-}
-
-/*
- * Allocate RX ring elements and chain them together.
- * One-to-one association of adapter descriptors with ring elements.
- */
-static int c2_rx_ring_alloc(struct c2_ring *rx_ring, void *vaddr,
-			    dma_addr_t base, void __iomem * mmio_rxp_ring)
-{
-	struct c2_rx_desc *rx_desc;
-	struct c2_rxp_desc __iomem *rxp_desc;
-	struct c2_element *elem;
-	int i;
-
-	rx_ring->start = kmalloc(sizeof(*elem) * rx_ring->count, GFP_KERNEL);
-	if (!rx_ring->start)
-		return -ENOMEM;
-
-	elem = rx_ring->start;
-	rx_desc = vaddr;
-	rxp_desc = mmio_rxp_ring;
-	for (i = 0; i < rx_ring->count; i++, elem++, rx_desc++, rxp_desc++) {
-		rx_desc->len = 0;
-		rx_desc->status = 0;
-
-		/* Set RXP_HRXD_UNINIT */
-		__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_OK),
-		       (void __iomem *) rxp_desc + C2_RXP_STATUS);
-		__raw_writew(0, (void __iomem *) rxp_desc + C2_RXP_COUNT);
-		__raw_writew(0, (void __iomem *) rxp_desc + C2_RXP_LEN);
-		__raw_writeq((__force u64) cpu_to_be64(0x99aabbccddeeffULL),
-			     (void __iomem *) rxp_desc + C2_RXP_ADDR);
-		__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_UNINIT),
-			     (void __iomem *) rxp_desc + C2_RXP_FLAGS);
-
-		elem->skb = NULL;
-		elem->ht_desc = rx_desc;
-		elem->hw_desc = rxp_desc;
-
-		if (i == rx_ring->count - 1) {
-			elem->next = rx_ring->start;
-			rx_desc->next_offset = base;
-		} else {
-			elem->next = elem + 1;
-			rx_desc->next_offset =
-			    base + (i + 1) * sizeof(*rx_desc);
-		}
-	}
-
-	rx_ring->to_use = rx_ring->to_clean = rx_ring->start;
-
-	return 0;
-}
-
-/* Setup buffer for receiving */
-static inline int c2_rx_alloc(struct c2_port *c2_port, struct c2_element *elem)
-{
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_rx_desc *rx_desc = elem->ht_desc;
-	struct sk_buff *skb;
-	dma_addr_t mapaddr;
-	u32 maplen;
-	struct c2_rxp_hdr *rxp_hdr;
-
-	skb = dev_alloc_skb(c2_port->rx_buf_size);
-	if (unlikely(!skb)) {
-		pr_debug("%s: out of memory for receive\n",
-			c2_port->netdev->name);
-		return -ENOMEM;
-	}
-
-	/* Zero out the rxp hdr in the sk_buff */
-	memset(skb->data, 0, sizeof(*rxp_hdr));
-
-	skb->dev = c2_port->netdev;
-
-	maplen = c2_port->rx_buf_size;
-	mapaddr =
-	    pci_map_single(c2dev->pcidev, skb->data, maplen,
-			   PCI_DMA_FROMDEVICE);
-
-	/* Set the sk_buff RXP_header to RXP_HRXD_READY */
-	rxp_hdr = (struct c2_rxp_hdr *) skb->data;
-	rxp_hdr->flags = RXP_HRXD_READY;
-
-	__raw_writew(0, elem->hw_desc + C2_RXP_STATUS);
-	__raw_writew((__force u16) cpu_to_be16((u16) maplen - sizeof(*rxp_hdr)),
-		     elem->hw_desc + C2_RXP_LEN);
-	__raw_writeq((__force u64) cpu_to_be64(mapaddr), elem->hw_desc + C2_RXP_ADDR);
-	__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_READY),
-		     elem->hw_desc + C2_RXP_FLAGS);
-
-	elem->skb = skb;
-	elem->mapaddr = mapaddr;
-	elem->maplen = maplen;
-	rx_desc->len = maplen;
-
-	return 0;
-}
-
-/*
- * Allocate buffers for the Rx ring
- * For receive:  rx_ring.to_clean is next received frame
- */
-static int c2_rx_fill(struct c2_port *c2_port)
-{
-	struct c2_ring *rx_ring = &c2_port->rx_ring;
-	struct c2_element *elem;
-	int ret = 0;
-
-	elem = rx_ring->start;
-	do {
-		if (c2_rx_alloc(c2_port, elem)) {
-			ret = 1;
-			break;
-		}
-	} while ((elem = elem->next) != rx_ring->start);
-
-	rx_ring->to_clean = rx_ring->start;
-	return ret;
-}
-
-/* Free all buffers in RX ring, assumes receiver stopped */
-static void c2_rx_clean(struct c2_port *c2_port)
-{
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_ring *rx_ring = &c2_port->rx_ring;
-	struct c2_element *elem;
-	struct c2_rx_desc *rx_desc;
-
-	elem = rx_ring->start;
-	do {
-		rx_desc = elem->ht_desc;
-		rx_desc->len = 0;
-
-		__raw_writew(0, elem->hw_desc + C2_RXP_STATUS);
-		__raw_writew(0, elem->hw_desc + C2_RXP_COUNT);
-		__raw_writew(0, elem->hw_desc + C2_RXP_LEN);
-		__raw_writeq((__force u64) cpu_to_be64(0x99aabbccddeeffULL),
-			     elem->hw_desc + C2_RXP_ADDR);
-		__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_UNINIT),
-			     elem->hw_desc + C2_RXP_FLAGS);
-
-		if (elem->skb) {
-			pci_unmap_single(c2dev->pcidev, elem->mapaddr,
-					 elem->maplen, PCI_DMA_FROMDEVICE);
-			dev_kfree_skb(elem->skb);
-			elem->skb = NULL;
-		}
-	} while ((elem = elem->next) != rx_ring->start);
-}
-
-static inline int c2_tx_free(struct c2_dev *c2dev, struct c2_element *elem)
-{
-	struct c2_tx_desc *tx_desc = elem->ht_desc;
-
-	tx_desc->len = 0;
-
-	pci_unmap_single(c2dev->pcidev, elem->mapaddr, elem->maplen,
-			 PCI_DMA_TODEVICE);
-
-	if (elem->skb) {
-		dev_kfree_skb_any(elem->skb);
-		elem->skb = NULL;
-	}
-
-	return 0;
-}
-
-/* Free all buffers in TX ring, assumes transmitter stopped */
-static void c2_tx_clean(struct c2_port *c2_port)
-{
-	struct c2_ring *tx_ring = &c2_port->tx_ring;
-	struct c2_element *elem;
-	struct c2_txp_desc txp_htxd;
-	int retry;
-	unsigned long flags;
-
-	spin_lock_irqsave(&c2_port->tx_lock, flags);
-
-	elem = tx_ring->start;
-
-	do {
-		retry = 0;
-		do {
-			txp_htxd.flags =
-			    readw(elem->hw_desc + C2_TXP_FLAGS);
-
-			if (txp_htxd.flags == TXP_HTXD_READY) {
-				retry = 1;
-				__raw_writew(0,
-					     elem->hw_desc + C2_TXP_LEN);
-				__raw_writeq(0,
-					     elem->hw_desc + C2_TXP_ADDR);
-				__raw_writew((__force u16) cpu_to_be16(TXP_HTXD_DONE),
-					     elem->hw_desc + C2_TXP_FLAGS);
-				c2_port->netdev->stats.tx_dropped++;
-				break;
-			} else {
-				__raw_writew(0,
-					     elem->hw_desc + C2_TXP_LEN);
-				__raw_writeq((__force u64) cpu_to_be64(0x1122334455667788ULL),
-					     elem->hw_desc + C2_TXP_ADDR);
-				__raw_writew((__force u16) cpu_to_be16(TXP_HTXD_UNINIT),
-					     elem->hw_desc + C2_TXP_FLAGS);
-			}
-
-			c2_tx_free(c2_port->c2dev, elem);
-
-		} while ((elem = elem->next) != tx_ring->start);
-	} while (retry);
-
-	c2_port->tx_avail = c2_port->tx_ring.count - 1;
-	c2_port->c2dev->cur_tx = tx_ring->to_use - tx_ring->start;
-
-	if (c2_port->tx_avail > MAX_SKB_FRAGS + 1)
-		netif_wake_queue(c2_port->netdev);
-
-	spin_unlock_irqrestore(&c2_port->tx_lock, flags);
-}
-
-/*
- * Process transmit descriptors marked 'DONE' by the firmware,
- * freeing up their unneeded sk_buffs.
- */
-static void c2_tx_interrupt(struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_ring *tx_ring = &c2_port->tx_ring;
-	struct c2_element *elem;
-	struct c2_txp_desc txp_htxd;
-
-	spin_lock(&c2_port->tx_lock);
-
-	for (elem = tx_ring->to_clean; elem != tx_ring->to_use;
-	     elem = elem->next) {
-		txp_htxd.flags =
-		    be16_to_cpu((__force __be16) readw(elem->hw_desc + C2_TXP_FLAGS));
-
-		if (txp_htxd.flags != TXP_HTXD_DONE)
-			break;
-
-		if (netif_msg_tx_done(c2_port)) {
-			/* PCI reads are expensive in fast path */
-			txp_htxd.len =
-			    be16_to_cpu((__force __be16) readw(elem->hw_desc + C2_TXP_LEN));
-			pr_debug("%s: tx done slot %3Zu status 0x%x len "
-				"%5u bytes\n",
-				netdev->name, elem - tx_ring->start,
-				txp_htxd.flags, txp_htxd.len);
-		}
-
-		c2_tx_free(c2dev, elem);
-		++(c2_port->tx_avail);
-	}
-
-	tx_ring->to_clean = elem;
-
-	if (netif_queue_stopped(netdev)
-	    && c2_port->tx_avail > MAX_SKB_FRAGS + 1)
-		netif_wake_queue(netdev);
-
-	spin_unlock(&c2_port->tx_lock);
-}
-
-static void c2_rx_error(struct c2_port *c2_port, struct c2_element *elem)
-{
-	struct c2_rx_desc *rx_desc = elem->ht_desc;
-	struct c2_rxp_hdr *rxp_hdr = (struct c2_rxp_hdr *) elem->skb->data;
-
-	if (rxp_hdr->status != RXP_HRXD_OK ||
-	    rxp_hdr->len > (rx_desc->len - sizeof(*rxp_hdr))) {
-		pr_debug("BAD RXP_HRXD\n");
-		pr_debug("  rx_desc : %p\n", rx_desc);
-		pr_debug("    index : %Zu\n",
-			elem - c2_port->rx_ring.start);
-		pr_debug("    len   : %u\n", rx_desc->len);
-		pr_debug("  rxp_hdr : %p [PA %p]\n", rxp_hdr,
-			(void *) __pa((unsigned long) rxp_hdr));
-		pr_debug("    flags : 0x%x\n", rxp_hdr->flags);
-		pr_debug("    status: 0x%x\n", rxp_hdr->status);
-		pr_debug("    len   : %u\n", rxp_hdr->len);
-		pr_debug("    rsvd  : 0x%x\n", rxp_hdr->rsvd);
-	}
-
-	/* Setup the skb for reuse since we're dropping this pkt */
-	elem->skb->data = elem->skb->head;
-	skb_reset_tail_pointer(elem->skb);
-
-	/* Zero out the rxp hdr in the sk_buff */
-	memset(elem->skb->data, 0, sizeof(*rxp_hdr));
-
-	/* Write the descriptor to the adapter's rx ring */
-	__raw_writew(0, elem->hw_desc + C2_RXP_STATUS);
-	__raw_writew(0, elem->hw_desc + C2_RXP_COUNT);
-	__raw_writew((__force u16) cpu_to_be16((u16) elem->maplen - sizeof(*rxp_hdr)),
-		     elem->hw_desc + C2_RXP_LEN);
-	__raw_writeq((__force u64) cpu_to_be64(elem->mapaddr),
-		     elem->hw_desc + C2_RXP_ADDR);
-	__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_READY),
-		     elem->hw_desc + C2_RXP_FLAGS);
-
-	pr_debug("packet dropped\n");
-	c2_port->netdev->stats.rx_dropped++;
-}
-
-static void c2_rx_interrupt(struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_ring *rx_ring = &c2_port->rx_ring;
-	struct c2_element *elem;
-	struct c2_rx_desc *rx_desc;
-	struct c2_rxp_hdr *rxp_hdr;
-	struct sk_buff *skb;
-	dma_addr_t mapaddr;
-	u32 maplen, buflen;
-	unsigned long flags;
-
-	spin_lock_irqsave(&c2dev->lock, flags);
-
-	/* Begin where we left off */
-	rx_ring->to_clean = rx_ring->start + c2dev->cur_rx;
-
-	for (elem = rx_ring->to_clean; elem->next != rx_ring->to_clean;
-	     elem = elem->next) {
-		rx_desc = elem->ht_desc;
-		mapaddr = elem->mapaddr;
-		maplen = elem->maplen;
-		skb = elem->skb;
-		rxp_hdr = (struct c2_rxp_hdr *) skb->data;
-
-		if (rxp_hdr->flags != RXP_HRXD_DONE)
-			break;
-		buflen = rxp_hdr->len;
-
-		/* Sanity check the RXP header */
-		if (rxp_hdr->status != RXP_HRXD_OK ||
-		    buflen > (rx_desc->len - sizeof(*rxp_hdr))) {
-			c2_rx_error(c2_port, elem);
-			continue;
-		}
-
-		/*
-		 * Allocate and map a new skb for replenishing the host
-		 * RX desc
-		 */
-		if (c2_rx_alloc(c2_port, elem)) {
-			c2_rx_error(c2_port, elem);
-			continue;
-		}
-
-		/* Unmap the old skb */
-		pci_unmap_single(c2dev->pcidev, mapaddr, maplen,
-				 PCI_DMA_FROMDEVICE);
-
-		prefetch(skb->data);
-
-		/*
-		 * Skip past the leading 8 bytes comprising of the
-		 * "struct c2_rxp_hdr", prepended by the adapter
-		 * to the usual Ethernet header ("struct ethhdr"),
-		 * to the start of the raw Ethernet packet.
-		 *
-		 * Fix up the various fields in the sk_buff before
-		 * passing it up to netif_rx(). The transfer size
-		 * (in bytes) specified by the adapter len field of
-		 * the "struct rxp_hdr_t" does NOT include the
-		 * "sizeof(struct c2_rxp_hdr)".
-		 */
-		skb->data += sizeof(*rxp_hdr);
-		skb_set_tail_pointer(skb, buflen);
-		skb->len = buflen;
-		skb->protocol = eth_type_trans(skb, netdev);
-
-		netif_rx(skb);
-
-		netdev->stats.rx_packets++;
-		netdev->stats.rx_bytes += buflen;
-	}
-
-	/* Save where we left off */
-	rx_ring->to_clean = elem;
-	c2dev->cur_rx = elem - rx_ring->start;
-	C2_SET_CUR_RX(c2dev, c2dev->cur_rx);
-
-	spin_unlock_irqrestore(&c2dev->lock, flags);
-}
-
-/*
- * Handle netisr0 TX & RX interrupts.
- */
-static irqreturn_t c2_interrupt(int irq, void *dev_id)
-{
-	unsigned int netisr0, dmaisr;
-	int handled = 0;
-	struct c2_dev *c2dev = dev_id;
-
-	/* Process CCILNET interrupts */
-	netisr0 = readl(c2dev->regs + C2_NISR0);
-	if (netisr0) {
-
-		/*
-		 * There is an issue with the firmware that always
-		 * provides the status of RX for both TX & RX
-		 * interrupts.  So process both queues here.
-		 */
-		c2_rx_interrupt(c2dev->netdev);
-		c2_tx_interrupt(c2dev->netdev);
-
-		/* Clear the interrupt */
-		writel(netisr0, c2dev->regs + C2_NISR0);
-		handled++;
-	}
-
-	/* Process RNIC interrupts */
-	dmaisr = readl(c2dev->regs + C2_DISR);
-	if (dmaisr) {
-		writel(dmaisr, c2dev->regs + C2_DISR);
-		c2_rnic_interrupt(c2dev);
-		handled++;
-	}
-
-	if (handled) {
-		return IRQ_HANDLED;
-	} else {
-		return IRQ_NONE;
-	}
-}
-
-static int c2_up(struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_element *elem;
-	struct c2_rxp_hdr *rxp_hdr;
-	struct in_device *in_dev;
-	size_t rx_size, tx_size;
-	int ret, i;
-	unsigned int netimr0;
-
-	if (netif_msg_ifup(c2_port))
-		pr_debug("%s: enabling interface\n", netdev->name);
-
-	/* Set the Rx buffer size based on MTU */
-	c2_set_rxbufsize(c2_port);
-
-	/* Allocate DMA'able memory for Tx/Rx host descriptor rings */
-	rx_size = c2_port->rx_ring.count * sizeof(struct c2_rx_desc);
-	tx_size = c2_port->tx_ring.count * sizeof(struct c2_tx_desc);
-
-	c2_port->mem_size = tx_size + rx_size;
-	c2_port->mem = pci_zalloc_consistent(c2dev->pcidev, c2_port->mem_size,
-					     &c2_port->dma);
-	if (c2_port->mem == NULL) {
-		pr_debug("Unable to allocate memory for "
-			"host descriptor rings\n");
-		return -ENOMEM;
-	}
-
-	/* Create the Rx host descriptor ring */
-	if ((ret =
-	     c2_rx_ring_alloc(&c2_port->rx_ring, c2_port->mem, c2_port->dma,
-			      c2dev->mmio_rxp_ring))) {
-		pr_debug("Unable to create RX ring\n");
-		goto bail0;
-	}
-
-	/* Allocate Rx buffers for the host descriptor ring */
-	if (c2_rx_fill(c2_port)) {
-		pr_debug("Unable to fill RX ring\n");
-		goto bail1;
-	}
-
-	/* Create the Tx host descriptor ring */
-	if ((ret = c2_tx_ring_alloc(&c2_port->tx_ring, c2_port->mem + rx_size,
-				    c2_port->dma + rx_size,
-				    c2dev->mmio_txp_ring))) {
-		pr_debug("Unable to create TX ring\n");
-		goto bail1;
-	}
-
-	/* Set the TX pointer to where we left off */
-	c2_port->tx_avail = c2_port->tx_ring.count - 1;
-	c2_port->tx_ring.to_use = c2_port->tx_ring.to_clean =
-	    c2_port->tx_ring.start + c2dev->cur_tx;
-
-	/* missing: Initialize MAC */
-
-	BUG_ON(c2_port->tx_ring.to_use != c2_port->tx_ring.to_clean);
-
-	/* Reset the adapter, ensures the driver is in sync with the RXP */
-	c2_reset(c2_port);
-
-	/* Reset the READY bit in the sk_buff RXP headers & adapter HRXDQ */
-	for (i = 0, elem = c2_port->rx_ring.start; i < c2_port->rx_ring.count;
-	     i++, elem++) {
-		rxp_hdr = (struct c2_rxp_hdr *) elem->skb->data;
-		rxp_hdr->flags = 0;
-		__raw_writew((__force u16) cpu_to_be16(RXP_HRXD_READY),
-			     elem->hw_desc + C2_RXP_FLAGS);
-	}
-
-	/* Enable network packets */
-	netif_start_queue(netdev);
-
-	/* Enable IRQ */
-	writel(0, c2dev->regs + C2_IDIS);
-	netimr0 = readl(c2dev->regs + C2_NIMR0);
-	netimr0 &= ~(C2_PCI_HTX_INT | C2_PCI_HRX_INT);
-	writel(netimr0, c2dev->regs + C2_NIMR0);
-
-	/* Tell the stack to ignore arp requests for ipaddrs bound to
-	 * other interfaces.  This is needed to prevent the host stack
-	 * from responding to arp requests to the ipaddr bound on the
-	 * rdma interface.
-	 */
-	in_dev = in_dev_get(netdev);
-	IN_DEV_CONF_SET(in_dev, ARP_IGNORE, 1);
-	in_dev_put(in_dev);
-
-	return 0;
-
-bail1:
-	c2_rx_clean(c2_port);
-	kfree(c2_port->rx_ring.start);
-
-bail0:
-	pci_free_consistent(c2dev->pcidev, c2_port->mem_size, c2_port->mem,
-			    c2_port->dma);
-
-	return ret;
-}
-
-static int c2_down(struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-	struct c2_dev *c2dev = c2_port->c2dev;
-
-	if (netif_msg_ifdown(c2_port))
-		pr_debug("%s: disabling interface\n",
-			netdev->name);
-
-	/* Wait for all the queued packets to get sent */
-	c2_tx_interrupt(netdev);
-
-	/* Disable network packets */
-	netif_stop_queue(netdev);
-
-	/* Disable IRQs by clearing the interrupt mask */
-	writel(1, c2dev->regs + C2_IDIS);
-	writel(0, c2dev->regs + C2_NIMR0);
-
-	/* missing: Stop transmitter */
-
-	/* missing: Stop receiver */
-
-	/* Reset the adapter, ensures the driver is in sync with the RXP */
-	c2_reset(c2_port);
-
-	/* missing: Turn off LEDs here */
-
-	/* Free all buffers in the host descriptor rings */
-	c2_tx_clean(c2_port);
-	c2_rx_clean(c2_port);
-
-	/* Free the host descriptor rings */
-	kfree(c2_port->rx_ring.start);
-	kfree(c2_port->tx_ring.start);
-	pci_free_consistent(c2dev->pcidev, c2_port->mem_size, c2_port->mem,
-			    c2_port->dma);
-
-	return 0;
-}
-
-static void c2_reset(struct c2_port *c2_port)
-{
-	struct c2_dev *c2dev = c2_port->c2dev;
-	unsigned int cur_rx = c2dev->cur_rx;
-
-	/* Tell the hardware to quiesce */
-	C2_SET_CUR_RX(c2dev, cur_rx | C2_PCI_HRX_QUI);
-
-	/*
-	 * The hardware will reset the C2_PCI_HRX_QUI bit once
-	 * the RXP is quiesced.  Wait 2 seconds for this.
-	 */
-	ssleep(2);
-
-	cur_rx = C2_GET_CUR_RX(c2dev);
-
-	if (cur_rx & C2_PCI_HRX_QUI)
-		pr_debug("c2_reset: failed to quiesce the hardware!\n");
-
-	cur_rx &= ~C2_PCI_HRX_QUI;
-
-	c2dev->cur_rx = cur_rx;
-
-	pr_debug("Current RX: %u\n", c2dev->cur_rx);
-}
-
-static int c2_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-	struct c2_dev *c2dev = c2_port->c2dev;
-	struct c2_ring *tx_ring = &c2_port->tx_ring;
-	struct c2_element *elem;
-	dma_addr_t mapaddr;
-	u32 maplen;
-	unsigned long flags;
-	unsigned int i;
-
-	spin_lock_irqsave(&c2_port->tx_lock, flags);
-
-	if (unlikely(c2_port->tx_avail < (skb_shinfo(skb)->nr_frags + 1))) {
-		netif_stop_queue(netdev);
-		spin_unlock_irqrestore(&c2_port->tx_lock, flags);
-
-		pr_debug("%s: Tx ring full when queue awake!\n",
-			netdev->name);
-		return NETDEV_TX_BUSY;
-	}
-
-	maplen = skb_headlen(skb);
-	mapaddr =
-	    pci_map_single(c2dev->pcidev, skb->data, maplen, PCI_DMA_TODEVICE);
-
-	elem = tx_ring->to_use;
-	elem->skb = skb;
-	elem->mapaddr = mapaddr;
-	elem->maplen = maplen;
-
-	/* Tell HW to xmit */
-	__raw_writeq((__force u64) cpu_to_be64(mapaddr),
-		     elem->hw_desc + C2_TXP_ADDR);
-	__raw_writew((__force u16) cpu_to_be16(maplen),
-		     elem->hw_desc + C2_TXP_LEN);
-	__raw_writew((__force u16) cpu_to_be16(TXP_HTXD_READY),
-		     elem->hw_desc + C2_TXP_FLAGS);
-
-	netdev->stats.tx_packets++;
-	netdev->stats.tx_bytes += maplen;
-
-	/* Loop thru additional data fragments and queue them */
-	if (skb_shinfo(skb)->nr_frags) {
-		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
-			const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
-			maplen = skb_frag_size(frag);
-			mapaddr = skb_frag_dma_map(&c2dev->pcidev->dev, frag,
-						   0, maplen, DMA_TO_DEVICE);
-			elem = elem->next;
-			elem->skb = NULL;
-			elem->mapaddr = mapaddr;
-			elem->maplen = maplen;
-
-			/* Tell HW to xmit */
-			__raw_writeq((__force u64) cpu_to_be64(mapaddr),
-				     elem->hw_desc + C2_TXP_ADDR);
-			__raw_writew((__force u16) cpu_to_be16(maplen),
-				     elem->hw_desc + C2_TXP_LEN);
-			__raw_writew((__force u16) cpu_to_be16(TXP_HTXD_READY),
-				     elem->hw_desc + C2_TXP_FLAGS);
-
-			netdev->stats.tx_packets++;
-			netdev->stats.tx_bytes += maplen;
-		}
-	}
-
-	tx_ring->to_use = elem->next;
-	c2_port->tx_avail -= (skb_shinfo(skb)->nr_frags + 1);
-
-	if (c2_port->tx_avail <= MAX_SKB_FRAGS + 1) {
-		netif_stop_queue(netdev);
-		if (netif_msg_tx_queued(c2_port))
-			pr_debug("%s: transmit queue full\n",
-				netdev->name);
-	}
-
-	spin_unlock_irqrestore(&c2_port->tx_lock, flags);
-
-	netdev->trans_start = jiffies;
-
-	return NETDEV_TX_OK;
-}
-
-static void c2_tx_timeout(struct net_device *netdev)
-{
-	struct c2_port *c2_port = netdev_priv(netdev);
-
-	if (netif_msg_timer(c2_port))
-		pr_debug("%s: tx timeout\n", netdev->name);
-
-	c2_tx_clean(c2_port);
-}
-
-static int c2_change_mtu(struct net_device *netdev, int new_mtu)
-{
-	int ret = 0;
-
-	if (new_mtu < ETH_ZLEN || new_mtu > ETH_JUMBO_MTU)
-		return -EINVAL;
-
-	netdev->mtu = new_mtu;
-
-	if (netif_running(netdev)) {
-		c2_down(netdev);
-
-		c2_up(netdev);
-	}
-
-	return ret;
-}
-
-static const struct net_device_ops c2_netdev = {
-	.ndo_open 		= c2_up,
-	.ndo_stop 		= c2_down,
-	.ndo_start_xmit		= c2_xmit_frame,
-	.ndo_tx_timeout		= c2_tx_timeout,
-	.ndo_change_mtu		= c2_change_mtu,
-	.ndo_set_mac_address 	= eth_mac_addr,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-/* Initialize network device */
-static struct net_device *c2_devinit(struct c2_dev *c2dev,
-				     void __iomem * mmio_addr)
-{
-	struct c2_port *c2_port = NULL;
-	struct net_device *netdev = alloc_etherdev(sizeof(*c2_port));
-
-	if (!netdev) {
-		pr_debug("c2_port etherdev alloc failed");
-		return NULL;
-	}
-
-	SET_NETDEV_DEV(netdev, &c2dev->pcidev->dev);
-
-	netdev->netdev_ops = &c2_netdev;
-	netdev->watchdog_timeo = C2_TX_TIMEOUT;
-	netdev->irq = c2dev->pcidev->irq;
-
-	c2_port = netdev_priv(netdev);
-	c2_port->netdev = netdev;
-	c2_port->c2dev = c2dev;
-	c2_port->msg_enable = netif_msg_init(debug, default_msg);
-	c2_port->tx_ring.count = C2_NUM_TX_DESC;
-	c2_port->rx_ring.count = C2_NUM_RX_DESC;
-
-	spin_lock_init(&c2_port->tx_lock);
-
-	/* Copy our 48-bit ethernet hardware address */
-	memcpy_fromio(netdev->dev_addr, mmio_addr + C2_REGS_ENADDR, 6);
-
-	/* Validate the MAC address */
-	if (!is_valid_ether_addr(netdev->dev_addr)) {
-		pr_debug("Invalid MAC Address\n");
-		c2_print_macaddr(netdev);
-		free_netdev(netdev);
-		return NULL;
-	}
-
-	c2dev->netdev = netdev;
-
-	return netdev;
-}
-
-static int c2_probe(struct pci_dev *pcidev, const struct pci_device_id *ent)
-{
-	int ret = 0, i;
-	unsigned long reg0_start, reg0_flags, reg0_len;
-	unsigned long reg2_start, reg2_flags, reg2_len;
-	unsigned long reg4_start, reg4_flags, reg4_len;
-	unsigned kva_map_size;
-	struct net_device *netdev = NULL;
-	struct c2_dev *c2dev = NULL;
-	void __iomem *mmio_regs = NULL;
-
-	printk(KERN_INFO PFX "AMSO1100 Gigabit Ethernet driver v%s loaded\n",
-		DRV_VERSION);
-
-	/* Enable PCI device */
-	ret = pci_enable_device(pcidev);
-	if (ret) {
-		printk(KERN_ERR PFX "%s: Unable to enable PCI device\n",
-			pci_name(pcidev));
-		goto bail0;
-	}
-
-	reg0_start = pci_resource_start(pcidev, BAR_0);
-	reg0_len = pci_resource_len(pcidev, BAR_0);
-	reg0_flags = pci_resource_flags(pcidev, BAR_0);
-
-	reg2_start = pci_resource_start(pcidev, BAR_2);
-	reg2_len = pci_resource_len(pcidev, BAR_2);
-	reg2_flags = pci_resource_flags(pcidev, BAR_2);
-
-	reg4_start = pci_resource_start(pcidev, BAR_4);
-	reg4_len = pci_resource_len(pcidev, BAR_4);
-	reg4_flags = pci_resource_flags(pcidev, BAR_4);
-
-	pr_debug("BAR0 size = 0x%lX bytes\n", reg0_len);
-	pr_debug("BAR2 size = 0x%lX bytes\n", reg2_len);
-	pr_debug("BAR4 size = 0x%lX bytes\n", reg4_len);
-
-	/* Make sure PCI base addr are MMIO */
-	if (!(reg0_flags & IORESOURCE_MEM) ||
-	    !(reg2_flags & IORESOURCE_MEM) || !(reg4_flags & IORESOURCE_MEM)) {
-		printk(KERN_ERR PFX "PCI regions not an MMIO resource\n");
-		ret = -ENODEV;
-		goto bail1;
-	}
-
-	/* Check for weird/broken PCI region reporting */
-	if ((reg0_len < C2_REG0_SIZE) ||
-	    (reg2_len < C2_REG2_SIZE) || (reg4_len < C2_REG4_SIZE)) {
-		printk(KERN_ERR PFX "Invalid PCI region sizes\n");
-		ret = -ENODEV;
-		goto bail1;
-	}
-
-	/* Reserve PCI I/O and memory resources */
-	ret = pci_request_regions(pcidev, DRV_NAME);
-	if (ret) {
-		printk(KERN_ERR PFX "%s: Unable to request regions\n",
-			pci_name(pcidev));
-		goto bail1;
-	}
-
-	if ((sizeof(dma_addr_t) > 4)) {
-		ret = pci_set_dma_mask(pcidev, DMA_BIT_MASK(64));
-		if (ret < 0) {
-			printk(KERN_ERR PFX "64b DMA configuration failed\n");
-			goto bail2;
-		}
-	} else {
-		ret = pci_set_dma_mask(pcidev, DMA_BIT_MASK(32));
-		if (ret < 0) {
-			printk(KERN_ERR PFX "32b DMA configuration failed\n");
-			goto bail2;
-		}
-	}
-
-	/* Enables bus-mastering on the device */
-	pci_set_master(pcidev);
-
-	/* Remap the adapter PCI registers in BAR4 */
-	mmio_regs = ioremap_nocache(reg4_start + C2_PCI_REGS_OFFSET,
-				    sizeof(struct c2_adapter_pci_regs));
-	if (!mmio_regs) {
-		printk(KERN_ERR PFX
-			"Unable to remap adapter PCI registers in BAR4\n");
-		ret = -EIO;
-		goto bail2;
-	}
-
-	/* Validate PCI regs magic */
-	for (i = 0; i < sizeof(c2_magic); i++) {
-		if (c2_magic[i] != readb(mmio_regs + C2_REGS_MAGIC + i)) {
-			printk(KERN_ERR PFX "Downlevel Firmware boot loader "
-				"[%d/%Zd: got 0x%x, exp 0x%x]. Use the cc_flash "
-			       "utility to update your boot loader\n",
-				i + 1, sizeof(c2_magic),
-				readb(mmio_regs + C2_REGS_MAGIC + i),
-				c2_magic[i]);
-			printk(KERN_ERR PFX "Adapter not claimed\n");
-			iounmap(mmio_regs);
-			ret = -EIO;
-			goto bail2;
-		}
-	}
-
-	/* Validate the adapter version */
-	if (be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_VERS)) != C2_VERSION) {
-		printk(KERN_ERR PFX "Version mismatch "
-			"[fw=%u, c2=%u], Adapter not claimed\n",
-			be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_VERS)),
-			C2_VERSION);
-		ret = -EINVAL;
-		iounmap(mmio_regs);
-		goto bail2;
-	}
-
-	/* Validate the adapter IVN */
-	if (be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_IVN)) != C2_IVN) {
-		printk(KERN_ERR PFX "Downlevel FIrmware level. You should be using "
-		       "the OpenIB device support kit. "
-		       "[fw=0x%x, c2=0x%x], Adapter not claimed\n",
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_IVN)),
-		       C2_IVN);
-		ret = -EINVAL;
-		iounmap(mmio_regs);
-		goto bail2;
-	}
-
-	/* Allocate hardware structure */
-	c2dev = (struct c2_dev *) ib_alloc_device(sizeof(*c2dev));
-	if (!c2dev) {
-		printk(KERN_ERR PFX "%s: Unable to alloc hardware struct\n",
-			pci_name(pcidev));
-		ret = -ENOMEM;
-		iounmap(mmio_regs);
-		goto bail2;
-	}
-
-	memset(c2dev, 0, sizeof(*c2dev));
-	spin_lock_init(&c2dev->lock);
-	c2dev->pcidev = pcidev;
-	c2dev->cur_tx = 0;
-
-	/* Get the last RX index */
-	c2dev->cur_rx =
-	    (be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_HRX_CUR)) -
-	     0xffffc000) / sizeof(struct c2_rxp_desc);
-
-	/* Request an interrupt line for the driver */
-	ret = request_irq(pcidev->irq, c2_interrupt, IRQF_SHARED, DRV_NAME, c2dev);
-	if (ret) {
-		printk(KERN_ERR PFX "%s: requested IRQ %u is busy\n",
-			pci_name(pcidev), pcidev->irq);
-		iounmap(mmio_regs);
-		goto bail3;
-	}
-
-	/* Set driver specific data */
-	pci_set_drvdata(pcidev, c2dev);
-
-	/* Initialize network device */
-	if ((netdev = c2_devinit(c2dev, mmio_regs)) == NULL) {
-		ret = -ENOMEM;
-		iounmap(mmio_regs);
-		goto bail4;
-	}
-
-	/* Save off the actual size prior to unmapping mmio_regs */
-	kva_map_size = be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_PCI_WINSIZE));
-
-	/* Unmap the adapter PCI registers in BAR4 */
-	iounmap(mmio_regs);
-
-	/* Register network device */
-	ret = register_netdev(netdev);
-	if (ret) {
-		printk(KERN_ERR PFX "Unable to register netdev, ret = %d\n",
-			ret);
-		goto bail5;
-	}
-
-	/* Disable network packets */
-	netif_stop_queue(netdev);
-
-	/* Remap the adapter HRXDQ PA space to kernel VA space */
-	c2dev->mmio_rxp_ring = ioremap_nocache(reg4_start + C2_RXP_HRXDQ_OFFSET,
-					       C2_RXP_HRXDQ_SIZE);
-	if (!c2dev->mmio_rxp_ring) {
-		printk(KERN_ERR PFX "Unable to remap MMIO HRXDQ region\n");
-		ret = -EIO;
-		goto bail6;
-	}
-
-	/* Remap the adapter HTXDQ PA space to kernel VA space */
-	c2dev->mmio_txp_ring = ioremap_nocache(reg4_start + C2_TXP_HTXDQ_OFFSET,
-					       C2_TXP_HTXDQ_SIZE);
-	if (!c2dev->mmio_txp_ring) {
-		printk(KERN_ERR PFX "Unable to remap MMIO HTXDQ region\n");
-		ret = -EIO;
-		goto bail7;
-	}
-
-	/* Save off the current RX index in the last 4 bytes of the TXP Ring */
-	C2_SET_CUR_RX(c2dev, c2dev->cur_rx);
-
-	/* Remap the PCI registers in adapter BAR0 to kernel VA space */
-	c2dev->regs = ioremap_nocache(reg0_start, reg0_len);
-	if (!c2dev->regs) {
-		printk(KERN_ERR PFX "Unable to remap BAR0\n");
-		ret = -EIO;
-		goto bail8;
-	}
-
-	/* Remap the PCI registers in adapter BAR4 to kernel VA space */
-	c2dev->pa = reg4_start + C2_PCI_REGS_OFFSET;
-	c2dev->kva = ioremap_nocache(reg4_start + C2_PCI_REGS_OFFSET,
-				     kva_map_size);
-	if (!c2dev->kva) {
-		printk(KERN_ERR PFX "Unable to remap BAR4\n");
-		ret = -EIO;
-		goto bail9;
-	}
-
-	/* Print out the MAC address */
-	c2_print_macaddr(netdev);
-
-	ret = c2_rnic_init(c2dev);
-	if (ret) {
-		printk(KERN_ERR PFX "c2_rnic_init failed: %d\n", ret);
-		goto bail10;
-	}
-
-	ret = c2_register_device(c2dev);
-	if (ret)
-		goto bail10;
-
-	return 0;
-
- bail10:
-	iounmap(c2dev->kva);
-
- bail9:
-	iounmap(c2dev->regs);
-
- bail8:
-	iounmap(c2dev->mmio_txp_ring);
-
- bail7:
-	iounmap(c2dev->mmio_rxp_ring);
-
- bail6:
-	unregister_netdev(netdev);
-
- bail5:
-	free_netdev(netdev);
-
- bail4:
-	free_irq(pcidev->irq, c2dev);
-
- bail3:
-	ib_dealloc_device(&c2dev->ibdev);
-
- bail2:
-	pci_release_regions(pcidev);
-
- bail1:
-	pci_disable_device(pcidev);
-
- bail0:
-	return ret;
-}
-
-static void c2_remove(struct pci_dev *pcidev)
-{
-	struct c2_dev *c2dev = pci_get_drvdata(pcidev);
-	struct net_device *netdev = c2dev->netdev;
-
-	/* Unregister with OpenIB */
-	c2_unregister_device(c2dev);
-
-	/* Clean up the RNIC resources */
-	c2_rnic_term(c2dev);
-
-	/* Remove network device from the kernel */
-	unregister_netdev(netdev);
-
-	/* Free network device */
-	free_netdev(netdev);
-
-	/* Free the interrupt line */
-	free_irq(pcidev->irq, c2dev);
-
-	/* missing: Turn LEDs off here */
-
-	/* Unmap adapter PA space */
-	iounmap(c2dev->kva);
-	iounmap(c2dev->regs);
-	iounmap(c2dev->mmio_txp_ring);
-	iounmap(c2dev->mmio_rxp_ring);
-
-	/* Free the hardware structure */
-	ib_dealloc_device(&c2dev->ibdev);
-
-	/* Release reserved PCI I/O and memory resources */
-	pci_release_regions(pcidev);
-
-	/* Disable PCI device */
-	pci_disable_device(pcidev);
-
-	/* Clear driver specific data */
-	pci_set_drvdata(pcidev, NULL);
-}
-
-static struct pci_driver c2_pci_driver = {
-	.name = DRV_NAME,
-	.id_table = c2_pci_table,
-	.probe = c2_probe,
-	.remove = c2_remove,
-};
-
-module_pci_driver(c2_pci_driver);
diff --git a/drivers/staging/rdma/amso1100/c2.h b/drivers/staging/rdma/amso1100/c2.h
deleted file mode 100644
index d619d735838b..000000000000
--- a/drivers/staging/rdma/amso1100/c2.h
+++ /dev/null
@@ -1,547 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 __C2_H
-#define __C2_H
-
-#include <linux/netdevice.h>
-#include <linux/spinlock.h>
-#include <linux/kernel.h>
-#include <linux/pci.h>
-#include <linux/dma-mapping.h>
-#include <linux/idr.h>
-
-#include "c2_provider.h"
-#include "c2_mq.h"
-#include "c2_status.h"
-
-#define DRV_NAME     "c2"
-#define DRV_VERSION  "1.1"
-#define PFX          DRV_NAME ": "
-
-#define BAR_0                0
-#define BAR_2                2
-#define BAR_4                4
-
-#define RX_BUF_SIZE         (1536 + 8)
-#define ETH_JUMBO_MTU        9000
-#define C2_MAGIC            "CEPHEUS"
-#define C2_VERSION           4
-#define C2_IVN              (18 & 0x7fffffff)
-
-#define C2_REG0_SIZE        (16 * 1024)
-#define C2_REG2_SIZE        (2 * 1024 * 1024)
-#define C2_REG4_SIZE        (256 * 1024 * 1024)
-#define C2_NUM_TX_DESC       341
-#define C2_NUM_RX_DESC       256
-#define C2_PCI_REGS_OFFSET  (0x10000)
-#define C2_RXP_HRXDQ_OFFSET (((C2_REG4_SIZE)/2))
-#define C2_RXP_HRXDQ_SIZE   (4096)
-#define C2_TXP_HTXDQ_OFFSET (((C2_REG4_SIZE)/2) + C2_RXP_HRXDQ_SIZE)
-#define C2_TXP_HTXDQ_SIZE   (4096)
-#define C2_TX_TIMEOUT	    (6*HZ)
-
-/* CEPHEUS */
-static const u8 c2_magic[] = {
-	0x43, 0x45, 0x50, 0x48, 0x45, 0x55, 0x53
-};
-
-enum adapter_pci_regs {
-	C2_REGS_MAGIC = 0x0000,
-	C2_REGS_VERS = 0x0008,
-	C2_REGS_IVN = 0x000C,
-	C2_REGS_PCI_WINSIZE = 0x0010,
-	C2_REGS_Q0_QSIZE = 0x0014,
-	C2_REGS_Q0_MSGSIZE = 0x0018,
-	C2_REGS_Q0_POOLSTART = 0x001C,
-	C2_REGS_Q0_SHARED = 0x0020,
-	C2_REGS_Q1_QSIZE = 0x0024,
-	C2_REGS_Q1_MSGSIZE = 0x0028,
-	C2_REGS_Q1_SHARED = 0x0030,
-	C2_REGS_Q2_QSIZE = 0x0034,
-	C2_REGS_Q2_MSGSIZE = 0x0038,
-	C2_REGS_Q2_SHARED = 0x0040,
-	C2_REGS_ENADDR = 0x004C,
-	C2_REGS_RDMA_ENADDR = 0x0054,
-	C2_REGS_HRX_CUR = 0x006C,
-};
-
-struct c2_adapter_pci_regs {
-	char reg_magic[8];
-	u32 version;
-	u32 ivn;
-	u32 pci_window_size;
-	u32 q0_q_size;
-	u32 q0_msg_size;
-	u32 q0_pool_start;
-	u32 q0_shared;
-	u32 q1_q_size;
-	u32 q1_msg_size;
-	u32 q1_pool_start;
-	u32 q1_shared;
-	u32 q2_q_size;
-	u32 q2_msg_size;
-	u32 q2_pool_start;
-	u32 q2_shared;
-	u32 log_start;
-	u32 log_size;
-	u8 host_enaddr[8];
-	u8 rdma_enaddr[8];
-	u32 crash_entry;
-	u32 crash_ready[2];
-	u32 fw_txd_cur;
-	u32 fw_hrxd_cur;
-	u32 fw_rxd_cur;
-};
-
-enum pci_regs {
-	C2_HISR = 0x0000,
-	C2_DISR = 0x0004,
-	C2_HIMR = 0x0008,
-	C2_DIMR = 0x000C,
-	C2_NISR0 = 0x0010,
-	C2_NISR1 = 0x0014,
-	C2_NIMR0 = 0x0018,
-	C2_NIMR1 = 0x001C,
-	C2_IDIS = 0x0020,
-};
-
-enum {
-	C2_PCI_HRX_INT = 1 << 8,
-	C2_PCI_HTX_INT = 1 << 17,
-	C2_PCI_HRX_QUI = 1 << 31,
-};
-
-/*
- * Cepheus registers in BAR0.
- */
-struct c2_pci_regs {
-	u32 hostisr;
-	u32 dmaisr;
-	u32 hostimr;
-	u32 dmaimr;
-	u32 netisr0;
-	u32 netisr1;
-	u32 netimr0;
-	u32 netimr1;
-	u32 int_disable;
-};
-
-/* TXP flags */
-enum c2_txp_flags {
-	TXP_HTXD_DONE = 0,
-	TXP_HTXD_READY = 1 << 0,
-	TXP_HTXD_UNINIT = 1 << 1,
-};
-
-/* RXP flags */
-enum c2_rxp_flags {
-	RXP_HRXD_UNINIT = 0,
-	RXP_HRXD_READY = 1 << 0,
-	RXP_HRXD_DONE = 1 << 1,
-};
-
-/* RXP status */
-enum c2_rxp_status {
-	RXP_HRXD_ZERO = 0,
-	RXP_HRXD_OK = 1 << 0,
-	RXP_HRXD_BUF_OV = 1 << 1,
-};
-
-/* TXP descriptor fields */
-enum txp_desc {
-	C2_TXP_FLAGS = 0x0000,
-	C2_TXP_LEN = 0x0002,
-	C2_TXP_ADDR = 0x0004,
-};
-
-/* RXP descriptor fields */
-enum rxp_desc {
-	C2_RXP_FLAGS = 0x0000,
-	C2_RXP_STATUS = 0x0002,
-	C2_RXP_COUNT = 0x0004,
-	C2_RXP_LEN = 0x0006,
-	C2_RXP_ADDR = 0x0008,
-};
-
-struct c2_txp_desc {
-	u16 flags;
-	u16 len;
-	u64 addr;
-} __attribute__ ((packed));
-
-struct c2_rxp_desc {
-	u16 flags;
-	u16 status;
-	u16 count;
-	u16 len;
-	u64 addr;
-} __attribute__ ((packed));
-
-struct c2_rxp_hdr {
-	u16 flags;
-	u16 status;
-	u16 len;
-	u16 rsvd;
-} __attribute__ ((packed));
-
-struct c2_tx_desc {
-	u32 len;
-	u32 status;
-	dma_addr_t next_offset;
-};
-
-struct c2_rx_desc {
-	u32 len;
-	u32 status;
-	dma_addr_t next_offset;
-};
-
-struct c2_alloc {
-	u32 last;
-	u32 max;
-	spinlock_t lock;
-	unsigned long *table;
-};
-
-struct c2_array {
-	struct {
-		void **page;
-		int used;
-	} *page_list;
-};
-
-/*
- * The MQ shared pointer pool is organized as a linked list of
- * chunks. Each chunk contains a linked list of free shared pointers
- * that can be allocated to a given user mode client.
- *
- */
-struct sp_chunk {
-	struct sp_chunk *next;
-	dma_addr_t dma_addr;
-	DEFINE_DMA_UNMAP_ADDR(mapping);
-	u16 head;
-	u16 shared_ptr[0];
-};
-
-struct c2_pd_table {
-	u32 last;
-	u32 max;
-	spinlock_t lock;
-	unsigned long *table;
-};
-
-struct c2_qp_table {
-	struct idr idr;
-	spinlock_t lock;
-};
-
-struct c2_element {
-	struct c2_element *next;
-	void *ht_desc;		/* host     descriptor */
-	void __iomem *hw_desc;	/* hardware descriptor */
-	struct sk_buff *skb;
-	dma_addr_t mapaddr;
-	u32 maplen;
-};
-
-struct c2_ring {
-	struct c2_element *to_clean;
-	struct c2_element *to_use;
-	struct c2_element *start;
-	unsigned long count;
-};
-
-struct c2_dev {
-	struct ib_device ibdev;
-	void __iomem *regs;
-	void __iomem *mmio_txp_ring; /* remapped adapter memory for hw rings */
-	void __iomem *mmio_rxp_ring;
-	spinlock_t lock;
-	struct pci_dev *pcidev;
-	struct net_device *netdev;
-	struct net_device *pseudo_netdev;
-	unsigned int cur_tx;
-	unsigned int cur_rx;
-	u32 adapter_handle;
-	int device_cap_flags;
-	void __iomem *kva;	/* KVA device memory */
-	unsigned long pa;	/* PA device memory */
-	void **qptr_array;
-
-	struct kmem_cache *host_msg_cache;
-
-	struct list_head cca_link;		/* adapter list */
-	struct list_head eh_wakeup_list;	/* event wakeup list */
-	wait_queue_head_t req_vq_wo;
-
-	/* Cached RNIC properties */
-	struct ib_device_attr props;
-
-	struct c2_pd_table pd_table;
-	struct c2_qp_table qp_table;
-	int ports;		/* num of GigE ports */
-	int devnum;
-	spinlock_t vqlock;	/* sync vbs req MQ */
-
-	/* Verbs Queues */
-	struct c2_mq req_vq;	/* Verbs Request MQ */
-	struct c2_mq rep_vq;	/* Verbs Reply MQ */
-	struct c2_mq aeq;	/* Async Events MQ */
-
-	/* Kernel client MQs */
-	struct sp_chunk *kern_mqsp_pool;
-
-	/* Device updates these values when posting messages to a host
-	 * target queue */
-	u16 req_vq_shared;
-	u16 rep_vq_shared;
-	u16 aeq_shared;
-	u16 irq_claimed;
-
-	/*
-	 * Shared host target pages for user-accessible MQs.
-	 */
-	int hthead;		/* index of first free entry */
-	void *htpages;		/* kernel vaddr */
-	int htlen;		/* length of htpages memory */
-	void *htuva;		/* user mapped vaddr */
-	spinlock_t htlock;	/* serialize allocation */
-
-	u64 adapter_hint_uva;	/* access to the activity FIFO */
-
-	//	spinlock_t aeq_lock;
-	//	spinlock_t rnic_lock;
-
-	__be16 *hint_count;
-	dma_addr_t hint_count_dma;
-	u16 hints_read;
-
-	int init;		/* TRUE if it's ready */
-	char ae_cache_name[16];
-	char vq_cache_name[16];
-};
-
-struct c2_port {
-	u32 msg_enable;
-	struct c2_dev *c2dev;
-	struct net_device *netdev;
-
-	spinlock_t tx_lock;
-	u32 tx_avail;
-	struct c2_ring tx_ring;
-	struct c2_ring rx_ring;
-
-	void *mem;		/* PCI memory for host rings */
-	dma_addr_t dma;
-	unsigned long mem_size;
-
-	u32 rx_buf_size;
-};
-
-/*
- * Activity FIFO registers in BAR0.
- */
-#define PCI_BAR0_HOST_HINT	0x100
-#define PCI_BAR0_ADAPTER_HINT	0x2000
-
-/*
- * Ammasso PCI vendor id and Cepheus PCI device id.
- */
-#define CQ_ARMED 	0x01
-#define CQ_WAIT_FOR_DMA	0x80
-
-/*
- * The format of a hint is as follows:
- * Lower 16 bits are the count of hints for the queue.
- * Next 15 bits are the qp_index
- * Upper most bit depends on who reads it:
- *    If read by producer, then it means Full (1) or Not-Full (0)
- *    If read by consumer, then it means Empty (1) or Not-Empty (0)
- */
-#define C2_HINT_MAKE(q_index, hint_count) (((q_index) << 16) | hint_count)
-#define C2_HINT_GET_INDEX(hint) (((hint) & 0x7FFF0000) >> 16)
-#define C2_HINT_GET_COUNT(hint) ((hint) & 0x0000FFFF)
-
-
-/*
- * The following defines the offset in SDRAM for the c2_adapter_pci_regs_t
- * struct.
- */
-#define C2_ADAPTER_PCI_REGS_OFFSET 0x10000
-
-#ifndef readq
-static inline u64 readq(const void __iomem * addr)
-{
-	u64 ret = readl(addr + 4);
-	ret <<= 32;
-	ret |= readl(addr);
-
-	return ret;
-}
-#endif
-
-#ifndef writeq
-static inline void __raw_writeq(u64 val, void __iomem * addr)
-{
-	__raw_writel((u32) (val), addr);
-	__raw_writel((u32) (val >> 32), (addr + 4));
-}
-#endif
-
-#define C2_SET_CUR_RX(c2dev, cur_rx) \
-	__raw_writel((__force u32) cpu_to_be32(cur_rx), c2dev->mmio_txp_ring + 4092)
-
-#define C2_GET_CUR_RX(c2dev) \
-	be32_to_cpu((__force __be32) readl(c2dev->mmio_txp_ring + 4092))
-
-static inline struct c2_dev *to_c2dev(struct ib_device *ibdev)
-{
-	return container_of(ibdev, struct c2_dev, ibdev);
-}
-
-static inline int c2_errno(void *reply)
-{
-	switch (c2_wr_get_result(reply)) {
-	case C2_OK:
-		return 0;
-	case CCERR_NO_BUFS:
-	case CCERR_INSUFFICIENT_RESOURCES:
-	case CCERR_ZERO_RDMA_READ_RESOURCES:
-		return -ENOMEM;
-	case CCERR_MR_IN_USE:
-	case CCERR_QP_IN_USE:
-		return -EBUSY;
-	case CCERR_ADDR_IN_USE:
-		return -EADDRINUSE;
-	case CCERR_ADDR_NOT_AVAIL:
-		return -EADDRNOTAVAIL;
-	case CCERR_CONN_RESET:
-		return -ECONNRESET;
-	case CCERR_NOT_IMPLEMENTED:
-	case CCERR_INVALID_WQE:
-		return -ENOSYS;
-	case CCERR_QP_NOT_PRIVILEGED:
-		return -EPERM;
-	case CCERR_STACK_ERROR:
-		return -EPROTO;
-	case CCERR_ACCESS_VIOLATION:
-	case CCERR_BASE_AND_BOUNDS_VIOLATION:
-		return -EFAULT;
-	case CCERR_STAG_STATE_NOT_INVALID:
-	case CCERR_INVALID_ADDRESS:
-	case CCERR_INVALID_CQ:
-	case CCERR_INVALID_EP:
-	case CCERR_INVALID_MODIFIER:
-	case CCERR_INVALID_MTU:
-	case CCERR_INVALID_PD_ID:
-	case CCERR_INVALID_QP:
-	case CCERR_INVALID_RNIC:
-	case CCERR_INVALID_STAG:
-		return -EINVAL;
-	default:
-		return -EAGAIN;
-	}
-}
-
-/* Device */
-extern int c2_register_device(struct c2_dev *c2dev);
-extern void c2_unregister_device(struct c2_dev *c2dev);
-extern int c2_rnic_init(struct c2_dev *c2dev);
-extern void c2_rnic_term(struct c2_dev *c2dev);
-extern void c2_rnic_interrupt(struct c2_dev *c2dev);
-extern int c2_del_addr(struct c2_dev *c2dev, __be32 inaddr, __be32 inmask);
-extern int c2_add_addr(struct c2_dev *c2dev, __be32 inaddr, __be32 inmask);
-
-/* QPs */
-extern int c2_alloc_qp(struct c2_dev *c2dev, struct c2_pd *pd,
-		       struct ib_qp_init_attr *qp_attrs, struct c2_qp *qp);
-extern void c2_free_qp(struct c2_dev *c2dev, struct c2_qp *qp);
-extern struct ib_qp *c2_get_qp(struct ib_device *device, int qpn);
-extern int c2_qp_modify(struct c2_dev *c2dev, struct c2_qp *qp,
-			struct ib_qp_attr *attr, int attr_mask);
-extern int c2_qp_set_read_limits(struct c2_dev *c2dev, struct c2_qp *qp,
-				 int ord, int ird);
-extern int c2_post_send(struct ib_qp *ibqp, struct ib_send_wr *ib_wr,
-			struct ib_send_wr **bad_wr);
-extern int c2_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *ib_wr,
-			   struct ib_recv_wr **bad_wr);
-extern void c2_init_qp_table(struct c2_dev *c2dev);
-extern void c2_cleanup_qp_table(struct c2_dev *c2dev);
-extern void c2_set_qp_state(struct c2_qp *, int);
-extern struct c2_qp *c2_find_qpn(struct c2_dev *c2dev, int qpn);
-
-/* PDs */
-extern int c2_pd_alloc(struct c2_dev *c2dev, int privileged, struct c2_pd *pd);
-extern void c2_pd_free(struct c2_dev *c2dev, struct c2_pd *pd);
-extern int c2_init_pd_table(struct c2_dev *c2dev);
-extern void c2_cleanup_pd_table(struct c2_dev *c2dev);
-
-/* CQs */
-extern int c2_init_cq(struct c2_dev *c2dev, int entries,
-		      struct c2_ucontext *ctx, struct c2_cq *cq);
-extern void c2_free_cq(struct c2_dev *c2dev, struct c2_cq *cq);
-extern void c2_cq_event(struct c2_dev *c2dev, u32 mq_index);
-extern void c2_cq_clean(struct c2_dev *c2dev, struct c2_qp *qp, u32 mq_index);
-extern int c2_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry);
-extern int c2_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags flags);
-
-/* CM */
-extern int c2_llp_connect(struct iw_cm_id *cm_id,
-			  struct iw_cm_conn_param *iw_param);
-extern int c2_llp_accept(struct iw_cm_id *cm_id,
-			 struct iw_cm_conn_param *iw_param);
-extern int c2_llp_reject(struct iw_cm_id *cm_id, const void *pdata,
-			 u8 pdata_len);
-extern int c2_llp_service_create(struct iw_cm_id *cm_id, int backlog);
-extern int c2_llp_service_destroy(struct iw_cm_id *cm_id);
-
-/* MM */
-extern int c2_nsmr_register_phys_kern(struct c2_dev *c2dev, u64 *addr_list,
- 				      int page_size, int pbl_depth, u32 length,
- 				      u32 off, u64 *va, enum c2_acf acf,
-				      struct c2_mr *mr);
-extern int c2_stag_dealloc(struct c2_dev *c2dev, u32 stag_index);
-
-/* AE */
-extern void c2_ae_event(struct c2_dev *c2dev, u32 mq_index);
-
-/* MQSP Allocator */
-extern int c2_init_mqsp_pool(struct c2_dev *c2dev, gfp_t gfp_mask,
-			     struct sp_chunk **root);
-extern void c2_free_mqsp_pool(struct c2_dev *c2dev, struct sp_chunk *root);
-extern __be16 *c2_alloc_mqsp(struct c2_dev *c2dev, struct sp_chunk *head,
-			     dma_addr_t *dma_addr, gfp_t gfp_mask);
-extern void c2_free_mqsp(__be16* mqsp);
-#endif
diff --git a/drivers/staging/rdma/amso1100/c2_ae.c b/drivers/staging/rdma/amso1100/c2_ae.c
deleted file mode 100644
index eb7a92b2692f..000000000000
--- a/drivers/staging/rdma/amso1100/c2_ae.c
+++ /dev/null
@@ -1,327 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include "c2.h"
-#include <rdma/iw_cm.h>
-#include "c2_status.h"
-#include "c2_ae.h"
-
-static int c2_convert_cm_status(u32 c2_status)
-{
-	switch (c2_status) {
-	case C2_CONN_STATUS_SUCCESS:
-		return 0;
-	case C2_CONN_STATUS_REJECTED:
-		return -ENETRESET;
-	case C2_CONN_STATUS_REFUSED:
-		return -ECONNREFUSED;
-	case C2_CONN_STATUS_TIMEDOUT:
-		return -ETIMEDOUT;
-	case C2_CONN_STATUS_NETUNREACH:
-		return -ENETUNREACH;
-	case C2_CONN_STATUS_HOSTUNREACH:
-		return -EHOSTUNREACH;
-	case C2_CONN_STATUS_INVALID_RNIC:
-		return -EINVAL;
-	case C2_CONN_STATUS_INVALID_QP:
-		return -EINVAL;
-	case C2_CONN_STATUS_INVALID_QP_STATE:
-		return -EINVAL;
-	case C2_CONN_STATUS_ADDR_NOT_AVAIL:
-		return -EADDRNOTAVAIL;
-	default:
-		printk(KERN_ERR PFX
-		       "%s - Unable to convert CM status: %d\n",
-		       __func__, c2_status);
-		return -EIO;
-	}
-}
-
-static const char* to_event_str(int event)
-{
-	static const char* event_str[] = {
-		"CCAE_REMOTE_SHUTDOWN",
-		"CCAE_ACTIVE_CONNECT_RESULTS",
-		"CCAE_CONNECTION_REQUEST",
-		"CCAE_LLP_CLOSE_COMPLETE",
-		"CCAE_TERMINATE_MESSAGE_RECEIVED",
-		"CCAE_LLP_CONNECTION_RESET",
-		"CCAE_LLP_CONNECTION_LOST",
-		"CCAE_LLP_SEGMENT_SIZE_INVALID",
-		"CCAE_LLP_INVALID_CRC",
-		"CCAE_LLP_BAD_FPDU",
-		"CCAE_INVALID_DDP_VERSION",
-		"CCAE_INVALID_RDMA_VERSION",
-		"CCAE_UNEXPECTED_OPCODE",
-		"CCAE_INVALID_DDP_QUEUE_NUMBER",
-		"CCAE_RDMA_READ_NOT_ENABLED",
-		"CCAE_RDMA_WRITE_NOT_ENABLED",
-		"CCAE_RDMA_READ_TOO_SMALL",
-		"CCAE_NO_L_BIT",
-		"CCAE_TAGGED_INVALID_STAG",
-		"CCAE_TAGGED_BASE_BOUNDS_VIOLATION",
-		"CCAE_TAGGED_ACCESS_RIGHTS_VIOLATION",
-		"CCAE_TAGGED_INVALID_PD",
-		"CCAE_WRAP_ERROR",
-		"CCAE_BAD_CLOSE",
-		"CCAE_BAD_LLP_CLOSE",
-		"CCAE_INVALID_MSN_RANGE",
-		"CCAE_INVALID_MSN_GAP",
-		"CCAE_IRRQ_OVERFLOW",
-		"CCAE_IRRQ_MSN_GAP",
-		"CCAE_IRRQ_MSN_RANGE",
-		"CCAE_IRRQ_INVALID_STAG",
-		"CCAE_IRRQ_BASE_BOUNDS_VIOLATION",
-		"CCAE_IRRQ_ACCESS_RIGHTS_VIOLATION",
-		"CCAE_IRRQ_INVALID_PD",
-		"CCAE_IRRQ_WRAP_ERROR",
-		"CCAE_CQ_SQ_COMPLETION_OVERFLOW",
-		"CCAE_CQ_RQ_COMPLETION_ERROR",
-		"CCAE_QP_SRQ_WQE_ERROR",
-		"CCAE_QP_LOCAL_CATASTROPHIC_ERROR",
-		"CCAE_CQ_OVERFLOW",
-		"CCAE_CQ_OPERATION_ERROR",
-		"CCAE_SRQ_LIMIT_REACHED",
-		"CCAE_QP_RQ_LIMIT_REACHED",
-		"CCAE_SRQ_CATASTROPHIC_ERROR",
-		"CCAE_RNIC_CATASTROPHIC_ERROR"
-	};
-
-	if (event < CCAE_REMOTE_SHUTDOWN ||
-	    event > CCAE_RNIC_CATASTROPHIC_ERROR)
-		return "<invalid event>";
-
-	event -= CCAE_REMOTE_SHUTDOWN;
-	return event_str[event];
-}
-
-static const char *to_qp_state_str(int state)
-{
-	switch (state) {
-	case C2_QP_STATE_IDLE:
-		return "C2_QP_STATE_IDLE";
-	case C2_QP_STATE_CONNECTING:
-		return "C2_QP_STATE_CONNECTING";
-	case C2_QP_STATE_RTS:
-		return "C2_QP_STATE_RTS";
-	case C2_QP_STATE_CLOSING:
-		return "C2_QP_STATE_CLOSING";
-	case C2_QP_STATE_TERMINATE:
-		return "C2_QP_STATE_TERMINATE";
-	case C2_QP_STATE_ERROR:
-		return "C2_QP_STATE_ERROR";
-	default:
-		return "<invalid QP state>";
-	}
-}
-
-void c2_ae_event(struct c2_dev *c2dev, u32 mq_index)
-{
-	struct c2_mq *mq = c2dev->qptr_array[mq_index];
-	union c2wr *wr;
-	void *resource_user_context;
-	struct iw_cm_event cm_event;
-	struct ib_event ib_event;
-	enum c2_resource_indicator resource_indicator;
-	enum c2_event_id event_id;
-	unsigned long flags;
-	int status;
-	struct sockaddr_in *laddr = (struct sockaddr_in *)&cm_event.local_addr;
-	struct sockaddr_in *raddr = (struct sockaddr_in *)&cm_event.remote_addr;
-
-	/*
-	 * retrieve the message
-	 */
-	wr = c2_mq_consume(mq);
-	if (!wr)
-		return;
-
-	memset(&ib_event, 0, sizeof(ib_event));
-	memset(&cm_event, 0, sizeof(cm_event));
-
-	event_id = c2_wr_get_id(wr);
-	resource_indicator = be32_to_cpu(wr->ae.ae_generic.resource_type);
-	resource_user_context =
-	    (void *) (unsigned long) wr->ae.ae_generic.user_context;
-
-	status = cm_event.status = c2_convert_cm_status(c2_wr_get_result(wr));
-
-	pr_debug("event received c2_dev=%p, event_id=%d, "
-		"resource_indicator=%d, user_context=%p, status = %d\n",
-		c2dev, event_id, resource_indicator, resource_user_context,
-		status);
-
-	switch (resource_indicator) {
-	case C2_RES_IND_QP:{
-
-		struct c2_qp *qp = resource_user_context;
-		struct iw_cm_id *cm_id = qp->cm_id;
-		struct c2wr_ae_active_connect_results *res;
-
-		if (!cm_id) {
-			pr_debug("event received, but cm_id is <nul>, qp=%p!\n",
-				qp);
-			goto ignore_it;
-		}
-		pr_debug("%s: event = %s, user_context=%llx, "
-			"resource_type=%x, "
-			"resource=%x, qp_state=%s\n",
-			__func__,
-			to_event_str(event_id),
-			(unsigned long long) wr->ae.ae_generic.user_context,
-			be32_to_cpu(wr->ae.ae_generic.resource_type),
-			be32_to_cpu(wr->ae.ae_generic.resource),
-			to_qp_state_str(be32_to_cpu(wr->ae.ae_generic.qp_state)));
-
-		c2_set_qp_state(qp, be32_to_cpu(wr->ae.ae_generic.qp_state));
-
-		switch (event_id) {
-		case CCAE_ACTIVE_CONNECT_RESULTS:
-			res = &wr->ae.ae_active_connect_results;
-			cm_event.event = IW_CM_EVENT_CONNECT_REPLY;
-			laddr->sin_addr.s_addr = res->laddr;
-			raddr->sin_addr.s_addr = res->raddr;
-			laddr->sin_port = res->lport;
-			raddr->sin_port = res->rport;
-			if (status == 0) {
-				cm_event.private_data_len =
-					be32_to_cpu(res->private_data_length);
-				cm_event.private_data = res->private_data;
-			} else {
-				spin_lock_irqsave(&qp->lock, flags);
-				if (qp->cm_id) {
-					qp->cm_id->rem_ref(qp->cm_id);
-					qp->cm_id = NULL;
-				}
-				spin_unlock_irqrestore(&qp->lock, flags);
-				cm_event.private_data_len = 0;
-				cm_event.private_data = NULL;
-			}
-			if (cm_id->event_handler)
-				cm_id->event_handler(cm_id, &cm_event);
-			break;
-		case CCAE_TERMINATE_MESSAGE_RECEIVED:
-		case CCAE_CQ_SQ_COMPLETION_OVERFLOW:
-			ib_event.device = &c2dev->ibdev;
-			ib_event.element.qp = &qp->ibqp;
-			ib_event.event = IB_EVENT_QP_REQ_ERR;
-
-			if (qp->ibqp.event_handler)
-				qp->ibqp.event_handler(&ib_event,
-						       qp->ibqp.
-						       qp_context);
-			break;
-		case CCAE_BAD_CLOSE:
-		case CCAE_LLP_CLOSE_COMPLETE:
-		case CCAE_LLP_CONNECTION_RESET:
-		case CCAE_LLP_CONNECTION_LOST:
-			BUG_ON(cm_id->event_handler==(void*)0x6b6b6b6b);
-
-			spin_lock_irqsave(&qp->lock, flags);
-			if (qp->cm_id) {
-				qp->cm_id->rem_ref(qp->cm_id);
-				qp->cm_id = NULL;
-			}
-			spin_unlock_irqrestore(&qp->lock, flags);
-			cm_event.event = IW_CM_EVENT_CLOSE;
-			cm_event.status = 0;
-			if (cm_id->event_handler)
-				cm_id->event_handler(cm_id, &cm_event);
-			break;
-		default:
-			BUG_ON(1);
-			pr_debug("%s:%d Unexpected event_id=%d on QP=%p, "
-				"CM_ID=%p\n",
-				__func__, __LINE__,
-				event_id, qp, cm_id);
-			break;
-		}
-		break;
-	}
-
-	case C2_RES_IND_EP:{
-
-		struct c2wr_ae_connection_request *req =
-			&wr->ae.ae_connection_request;
-		struct iw_cm_id *cm_id =
-			resource_user_context;
-
-		pr_debug("C2_RES_IND_EP event_id=%d\n", event_id);
-		if (event_id != CCAE_CONNECTION_REQUEST) {
-			pr_debug("%s: Invalid event_id: %d\n",
-				__func__, event_id);
-			break;
-		}
-		cm_event.event = IW_CM_EVENT_CONNECT_REQUEST;
-		cm_event.provider_data = (void*)(unsigned long)req->cr_handle;
-		laddr->sin_addr.s_addr = req->laddr;
-		raddr->sin_addr.s_addr = req->raddr;
-		laddr->sin_port = req->lport;
-		raddr->sin_port = req->rport;
-		cm_event.private_data_len =
-			be32_to_cpu(req->private_data_length);
-		cm_event.private_data = req->private_data;
-		/*
-		 * Until ird/ord negotiation via MPAv2 support is added, send
-		 * max supported values
-		 */
-		cm_event.ird = cm_event.ord = 128;
-
-		if (cm_id->event_handler)
-			cm_id->event_handler(cm_id, &cm_event);
-		break;
-	}
-
-	case C2_RES_IND_CQ:{
-		struct c2_cq *cq =
-		    resource_user_context;
-
-		pr_debug("IB_EVENT_CQ_ERR\n");
-		ib_event.device = &c2dev->ibdev;
-		ib_event.element.cq = &cq->ibcq;
-		ib_event.event = IB_EVENT_CQ_ERR;
-
-		if (cq->ibcq.event_handler)
-			cq->ibcq.event_handler(&ib_event,
-					       cq->ibcq.cq_context);
-		break;
-	}
-
-	default:
-		printk("Bad resource indicator = %d\n",
-		       resource_indicator);
-		break;
-	}
-
- ignore_it:
-	c2_mq_free(mq);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_ae.h b/drivers/staging/rdma/amso1100/c2_ae.h
deleted file mode 100644
index 3a065c33b83b..000000000000
--- a/drivers/staging/rdma/amso1100/c2_ae.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 _C2_AE_H_
-#define _C2_AE_H_
-
-/*
- * WARNING: If you change this file, also bump C2_IVN_BASE
- * in common/include/clustercore/c2_ivn.h.
- */
-
-/*
- * Asynchronous Event Identifiers
- *
- * These start at 0x80 only so it's obvious from inspection that
- * they are not work-request statuses.  This isn't critical.
- *
- * NOTE: these event id's must fit in eight bits.
- */
-enum c2_event_id {
-	CCAE_REMOTE_SHUTDOWN = 0x80,
-	CCAE_ACTIVE_CONNECT_RESULTS,
-	CCAE_CONNECTION_REQUEST,
-	CCAE_LLP_CLOSE_COMPLETE,
-	CCAE_TERMINATE_MESSAGE_RECEIVED,
-	CCAE_LLP_CONNECTION_RESET,
-	CCAE_LLP_CONNECTION_LOST,
-	CCAE_LLP_SEGMENT_SIZE_INVALID,
-	CCAE_LLP_INVALID_CRC,
-	CCAE_LLP_BAD_FPDU,
-	CCAE_INVALID_DDP_VERSION,
-	CCAE_INVALID_RDMA_VERSION,
-	CCAE_UNEXPECTED_OPCODE,
-	CCAE_INVALID_DDP_QUEUE_NUMBER,
-	CCAE_RDMA_READ_NOT_ENABLED,
-	CCAE_RDMA_WRITE_NOT_ENABLED,
-	CCAE_RDMA_READ_TOO_SMALL,
-	CCAE_NO_L_BIT,
-	CCAE_TAGGED_INVALID_STAG,
-	CCAE_TAGGED_BASE_BOUNDS_VIOLATION,
-	CCAE_TAGGED_ACCESS_RIGHTS_VIOLATION,
-	CCAE_TAGGED_INVALID_PD,
-	CCAE_WRAP_ERROR,
-	CCAE_BAD_CLOSE,
-	CCAE_BAD_LLP_CLOSE,
-	CCAE_INVALID_MSN_RANGE,
-	CCAE_INVALID_MSN_GAP,
-	CCAE_IRRQ_OVERFLOW,
-	CCAE_IRRQ_MSN_GAP,
-	CCAE_IRRQ_MSN_RANGE,
-	CCAE_IRRQ_INVALID_STAG,
-	CCAE_IRRQ_BASE_BOUNDS_VIOLATION,
-	CCAE_IRRQ_ACCESS_RIGHTS_VIOLATION,
-	CCAE_IRRQ_INVALID_PD,
-	CCAE_IRRQ_WRAP_ERROR,
-	CCAE_CQ_SQ_COMPLETION_OVERFLOW,
-	CCAE_CQ_RQ_COMPLETION_ERROR,
-	CCAE_QP_SRQ_WQE_ERROR,
-	CCAE_QP_LOCAL_CATASTROPHIC_ERROR,
-	CCAE_CQ_OVERFLOW,
-	CCAE_CQ_OPERATION_ERROR,
-	CCAE_SRQ_LIMIT_REACHED,
-	CCAE_QP_RQ_LIMIT_REACHED,
-	CCAE_SRQ_CATASTROPHIC_ERROR,
-	CCAE_RNIC_CATASTROPHIC_ERROR
-/* WARNING If you add more id's, make sure their values fit in eight bits. */
-};
-
-/*
- * Resource Indicators and Identifiers
- */
-enum c2_resource_indicator {
-	C2_RES_IND_QP = 1,
-	C2_RES_IND_EP,
-	C2_RES_IND_CQ,
-	C2_RES_IND_SRQ,
-};
-
-#endif /* _C2_AE_H_ */
diff --git a/drivers/staging/rdma/amso1100/c2_alloc.c b/drivers/staging/rdma/amso1100/c2_alloc.c
deleted file mode 100644
index 039872dfabbc..000000000000
--- a/drivers/staging/rdma/amso1100/c2_alloc.c
+++ /dev/null
@@ -1,142 +0,0 @@
-/*
- * Copyright (c) 2004 Topspin Communications.  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.
- */
-
-#include <linux/errno.h>
-#include <linux/bitmap.h>
-
-#include "c2.h"
-
-static int c2_alloc_mqsp_chunk(struct c2_dev *c2dev, gfp_t gfp_mask,
-			       struct sp_chunk **head)
-{
-	int i;
-	struct sp_chunk *new_head;
-	dma_addr_t dma_addr;
-
-	new_head = dma_alloc_coherent(&c2dev->pcidev->dev, PAGE_SIZE,
-				      &dma_addr, gfp_mask);
-	if (new_head == NULL)
-		return -ENOMEM;
-
-	new_head->dma_addr = dma_addr;
-	dma_unmap_addr_set(new_head, mapping, new_head->dma_addr);
-
-	new_head->next = NULL;
-	new_head->head = 0;
-
-	/* build list where each index is the next free slot */
-	for (i = 0;
-	     i < (PAGE_SIZE - sizeof(struct sp_chunk) -
-		  sizeof(u16)) / sizeof(u16) - 1;
-	     i++) {
-		new_head->shared_ptr[i] = i + 1;
-	}
-	/* terminate list */
-	new_head->shared_ptr[i] = 0xFFFF;
-
-	*head = new_head;
-	return 0;
-}
-
-int c2_init_mqsp_pool(struct c2_dev *c2dev, gfp_t gfp_mask,
-		      struct sp_chunk **root)
-{
-	return c2_alloc_mqsp_chunk(c2dev, gfp_mask, root);
-}
-
-void c2_free_mqsp_pool(struct c2_dev *c2dev, struct sp_chunk *root)
-{
-	struct sp_chunk *next;
-
-	while (root) {
-		next = root->next;
-		dma_free_coherent(&c2dev->pcidev->dev, PAGE_SIZE, root,
-				  dma_unmap_addr(root, mapping));
-		root = next;
-	}
-}
-
-__be16 *c2_alloc_mqsp(struct c2_dev *c2dev, struct sp_chunk *head,
-		      dma_addr_t *dma_addr, gfp_t gfp_mask)
-{
-	u16 mqsp;
-
-	while (head) {
-		mqsp = head->head;
-		if (mqsp != 0xFFFF) {
-			head->head = head->shared_ptr[mqsp];
-			break;
-		} else if (head->next == NULL) {
-			if (c2_alloc_mqsp_chunk(c2dev, gfp_mask, &head->next) ==
-			    0) {
-				head = head->next;
-				mqsp = head->head;
-				head->head = head->shared_ptr[mqsp];
-				break;
-			} else
-				return NULL;
-		} else
-			head = head->next;
-	}
-	if (head) {
-		*dma_addr = head->dma_addr +
-			    ((unsigned long) &(head->shared_ptr[mqsp]) -
-			     (unsigned long) head);
-		pr_debug("%s addr %p dma_addr %llx\n", __func__,
-			 &(head->shared_ptr[mqsp]), (unsigned long long) *dma_addr);
-		return (__force __be16 *) &(head->shared_ptr[mqsp]);
-	}
-	return NULL;
-}
-
-void c2_free_mqsp(__be16 *mqsp)
-{
-	struct sp_chunk *head;
-	u16 idx;
-
-	/* The chunk containing this ptr begins at the page boundary */
-	head = (struct sp_chunk *) ((unsigned long) mqsp & PAGE_MASK);
-
-	/* Link head to new mqsp */
-	*mqsp = (__force __be16) head->head;
-
-	/* Compute the shared_ptr index */
-	idx = (offset_in_page(mqsp)) >> 1;
-	idx -= (unsigned long) &(((struct sp_chunk *) 0)->shared_ptr[0]) >> 1;
-
-	/* Point this index at the head */
-	head->shared_ptr[idx] = head->head;
-
-	/* Point head at this index */
-	head->head = idx;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_cm.c b/drivers/staging/rdma/amso1100/c2_cm.c
deleted file mode 100644
index f8dbdb9e0f66..000000000000
--- a/drivers/staging/rdma/amso1100/c2_cm.c
+++ /dev/null
@@ -1,458 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc.  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.
- *
- */
-#include <linux/slab.h>
-
-#include "c2.h"
-#include "c2_wr.h"
-#include "c2_vq.h"
-#include <rdma/iw_cm.h>
-
-int c2_llp_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *iw_param)
-{
-	struct c2_dev *c2dev = to_c2dev(cm_id->device);
-	struct ib_qp *ibqp;
-	struct c2_qp *qp;
-	struct c2wr_qp_connect_req *wr;	/* variable size needs a malloc. */
-	struct c2_vq_req *vq_req;
-	int err;
-	struct sockaddr_in *raddr = (struct sockaddr_in *)&cm_id->remote_addr;
-
-	if (cm_id->remote_addr.ss_family != AF_INET)
-		return -ENOSYS;
-
-	ibqp = c2_get_qp(cm_id->device, iw_param->qpn);
-	if (!ibqp)
-		return -EINVAL;
-	qp = to_c2qp(ibqp);
-
-	/* Associate QP <--> CM_ID */
-	cm_id->provider_data = qp;
-	cm_id->add_ref(cm_id);
-	qp->cm_id = cm_id;
-
-	/*
-	 * only support the max private_data length
-	 */
-	if (iw_param->private_data_len > C2_MAX_PRIVATE_DATA_SIZE) {
-		err = -EINVAL;
-		goto bail0;
-	}
-	/*
-	 * Set the rdma read limits
-	 */
-	err = c2_qp_set_read_limits(c2dev, qp, iw_param->ord, iw_param->ird);
-	if (err)
-		goto bail0;
-
-	/*
-	 * Create and send a WR_QP_CONNECT...
-	 */
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	c2_wr_set_id(wr, CCWR_QP_CONNECT);
-	wr->hdr.context = 0;
-	wr->rnic_handle = c2dev->adapter_handle;
-	wr->qp_handle = qp->adapter_handle;
-
-	wr->remote_addr = raddr->sin_addr.s_addr;
-	wr->remote_port = raddr->sin_port;
-
-	/*
-	 * Move any private data from the callers's buf into
-	 * the WR.
-	 */
-	if (iw_param->private_data) {
-		wr->private_data_length =
-			cpu_to_be32(iw_param->private_data_len);
-		memcpy(&wr->private_data[0], iw_param->private_data,
-		       iw_param->private_data_len);
-	} else
-		wr->private_data_length = 0;
-
-	/*
-	 * Send WR to adapter.  NOTE: There is no synch reply from
-	 * the adapter.
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) wr);
-	vq_req_free(c2dev, vq_req);
-
- bail1:
-	kfree(wr);
- bail0:
-	if (err) {
-		/*
-		 * If we fail, release reference on QP and
-		 * disassociate QP from CM_ID
-		 */
-		cm_id->provider_data = NULL;
-		qp->cm_id = NULL;
-		cm_id->rem_ref(cm_id);
-	}
-	return err;
-}
-
-int c2_llp_service_create(struct iw_cm_id *cm_id, int backlog)
-{
-	struct c2_dev *c2dev;
-	struct c2wr_ep_listen_create_req wr;
-	struct c2wr_ep_listen_create_rep *reply;
-	struct c2_vq_req *vq_req;
-	int err;
-	struct sockaddr_in *laddr = (struct sockaddr_in *)&cm_id->local_addr;
-
-	if (cm_id->local_addr.ss_family != AF_INET)
-		return -ENOSYS;
-
-	c2dev = to_c2dev(cm_id->device);
-	if (c2dev == NULL)
-		return -EINVAL;
-
-	/*
-	 * Allocate verbs request.
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	/*
-	 * Build the WR
-	 */
-	c2_wr_set_id(&wr, CCWR_EP_LISTEN_CREATE);
-	wr.hdr.context = (u64) (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.local_addr = laddr->sin_addr.s_addr;
-	wr.local_port = laddr->sin_port;
-	wr.backlog = cpu_to_be32(backlog);
-	wr.user_context = (u64) (unsigned long) cm_id;
-
-	/*
-	 * Reference the request struct.  Dereferenced in the int handler.
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	/*
-	 * Send WR to adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	/*
-	 * Wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail0;
-
-	/*
-	 * Process reply
-	 */
-	reply =
-	    (struct c2wr_ep_listen_create_rep *) (unsigned long) vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	if ((err = c2_errno(reply)) != 0)
-		goto bail1;
-
-	/*
-	 * Keep the adapter handle. Used in subsequent destroy
-	 */
-	cm_id->provider_data = (void*)(unsigned long) reply->ep_handle;
-
-	/*
-	 * free vq stuff
-	 */
-	vq_repbuf_free(c2dev, reply);
-	vq_req_free(c2dev, vq_req);
-
-	return 0;
-
- bail1:
-	vq_repbuf_free(c2dev, reply);
- bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-
-int c2_llp_service_destroy(struct iw_cm_id *cm_id)
-{
-
-	struct c2_dev *c2dev;
-	struct c2wr_ep_listen_destroy_req wr;
-	struct c2wr_ep_listen_destroy_rep *reply;
-	struct c2_vq_req *vq_req;
-	int err;
-
-	c2dev = to_c2dev(cm_id->device);
-	if (c2dev == NULL)
-		return -EINVAL;
-
-	/*
-	 * Allocate verbs request.
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	/*
-	 * Build the WR
-	 */
-	c2_wr_set_id(&wr, CCWR_EP_LISTEN_DESTROY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.ep_handle = (u32)(unsigned long)cm_id->provider_data;
-
-	/*
-	 * reference the request struct.  dereferenced in the int handler.
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	/*
-	 * Send WR to adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	/*
-	 * Wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail0;
-
-	/*
-	 * Process reply
-	 */
-	reply=(struct c2wr_ep_listen_destroy_rep *)(unsigned long)vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	vq_repbuf_free(c2dev, reply);
- bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-int c2_llp_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *iw_param)
-{
-	struct c2_dev *c2dev = to_c2dev(cm_id->device);
-	struct c2_qp *qp;
-	struct ib_qp *ibqp;
-	struct c2wr_cr_accept_req *wr;	/* variable length WR */
-	struct c2_vq_req *vq_req;
-	struct c2wr_cr_accept_rep *reply;	/* VQ Reply msg ptr. */
-	int err;
-
-	ibqp = c2_get_qp(cm_id->device, iw_param->qpn);
-	if (!ibqp)
-		return -EINVAL;
-	qp = to_c2qp(ibqp);
-
-	/* Set the RDMA read limits */
-	err = c2_qp_set_read_limits(c2dev, qp, iw_param->ord, iw_param->ird);
-	if (err)
-		goto bail0;
-
-	/* Allocate verbs request. */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-	vq_req->qp = qp;
-	vq_req->cm_id = cm_id;
-	vq_req->event = IW_CM_EVENT_ESTABLISHED;
-
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	/* Build the WR */
-	c2_wr_set_id(wr, CCWR_CR_ACCEPT);
-	wr->hdr.context = (unsigned long) vq_req;
-	wr->rnic_handle = c2dev->adapter_handle;
-	wr->ep_handle = (u32) (unsigned long) cm_id->provider_data;
-	wr->qp_handle = qp->adapter_handle;
-
-	/* Replace the cr_handle with the QP after accept */
-	cm_id->provider_data = qp;
-	cm_id->add_ref(cm_id);
-	qp->cm_id = cm_id;
-
-	cm_id->provider_data = qp;
-
-	/* Validate private_data length */
-	if (iw_param->private_data_len > C2_MAX_PRIVATE_DATA_SIZE) {
-		err = -EINVAL;
-		goto bail1;
-	}
-
-	if (iw_param->private_data) {
-		wr->private_data_length = cpu_to_be32(iw_param->private_data_len);
-		memcpy(&wr->private_data[0],
-		       iw_param->private_data, iw_param->private_data_len);
-	} else
-		wr->private_data_length = 0;
-
-	/* Reference the request struct.  Dereferenced in the int handler. */
-	vq_req_get(c2dev, vq_req);
-
-	/* Send WR to adapter */
-	err = vq_send_wr(c2dev, (union c2wr *) wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	/* Wait for reply from adapter */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail1;
-
-	/* Check that reply is present */
-	reply = (struct c2wr_cr_accept_rep *) (unsigned long) vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	err = c2_errno(reply);
-	vq_repbuf_free(c2dev, reply);
-
-	if (!err)
-		c2_set_qp_state(qp, C2_QP_STATE_RTS);
- bail1:
-	kfree(wr);
-	vq_req_free(c2dev, vq_req);
- bail0:
-	if (err) {
-		/*
-		 * If we fail, release reference on QP and
-		 * disassociate QP from CM_ID
-		 */
-		cm_id->provider_data = NULL;
-		qp->cm_id = NULL;
-		cm_id->rem_ref(cm_id);
-	}
-	return err;
-}
-
-int c2_llp_reject(struct iw_cm_id *cm_id, const void *pdata, u8 pdata_len)
-{
-	struct c2_dev *c2dev;
-	struct c2wr_cr_reject_req wr;
-	struct c2_vq_req *vq_req;
-	struct c2wr_cr_reject_rep *reply;
-	int err;
-
-	c2dev = to_c2dev(cm_id->device);
-
-	/*
-	 * Allocate verbs request.
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	/*
-	 * Build the WR
-	 */
-	c2_wr_set_id(&wr, CCWR_CR_REJECT);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.ep_handle = (u32) (unsigned long) cm_id->provider_data;
-
-	/*
-	 * reference the request struct.  dereferenced in the int handler.
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	/*
-	 * Send WR to adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	/*
-	 * Wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail0;
-
-	/*
-	 * Process reply
-	 */
-	reply = (struct c2wr_cr_reject_rep *) (unsigned long)
-		vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-	err = c2_errno(reply);
-	/*
-	 * free vq stuff
-	 */
-	vq_repbuf_free(c2dev, reply);
-
- bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_cq.c b/drivers/staging/rdma/amso1100/c2_cq.c
deleted file mode 100644
index 3ef881f2da0f..000000000000
--- a/drivers/staging/rdma/amso1100/c2_cq.c
+++ /dev/null
@@ -1,440 +0,0 @@
-/*
- * Copyright (c) 2004, 2005 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved.
- * Copyright (c) 2005 Cisco Systems, Inc. All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
- * Copyright (c) 2004 Voltaire, Inc. 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.
- *
- */
-#include <linux/gfp.h>
-
-#include "c2.h"
-#include "c2_vq.h"
-#include "c2_status.h"
-
-#define C2_CQ_MSG_SIZE ((sizeof(struct c2wr_ce) + 32-1) & ~(32-1))
-
-static struct c2_cq *c2_cq_get(struct c2_dev *c2dev, int cqn)
-{
-	struct c2_cq *cq;
-	unsigned long flags;
-
-	spin_lock_irqsave(&c2dev->lock, flags);
-	cq = c2dev->qptr_array[cqn];
-	if (!cq) {
-		spin_unlock_irqrestore(&c2dev->lock, flags);
-		return NULL;
-	}
-	atomic_inc(&cq->refcount);
-	spin_unlock_irqrestore(&c2dev->lock, flags);
-	return cq;
-}
-
-static void c2_cq_put(struct c2_cq *cq)
-{
-	if (atomic_dec_and_test(&cq->refcount))
-		wake_up(&cq->wait);
-}
-
-void c2_cq_event(struct c2_dev *c2dev, u32 mq_index)
-{
-	struct c2_cq *cq;
-
-	cq = c2_cq_get(c2dev, mq_index);
-	if (!cq) {
-		printk("discarding events on destroyed CQN=%d\n", mq_index);
-		return;
-	}
-
-	(*cq->ibcq.comp_handler) (&cq->ibcq, cq->ibcq.cq_context);
-	c2_cq_put(cq);
-}
-
-void c2_cq_clean(struct c2_dev *c2dev, struct c2_qp *qp, u32 mq_index)
-{
-	struct c2_cq *cq;
-	struct c2_mq *q;
-
-	cq = c2_cq_get(c2dev, mq_index);
-	if (!cq)
-		return;
-
-	spin_lock_irq(&cq->lock);
-	q = &cq->mq;
-	if (q && !c2_mq_empty(q)) {
-		u16 priv = q->priv;
-		struct c2wr_ce *msg;
-
-		while (priv != be16_to_cpu(*q->shared)) {
-			msg = (struct c2wr_ce *)
-				(q->msg_pool.host + priv * q->msg_size);
-			if (msg->qp_user_context == (u64) (unsigned long) qp) {
-				msg->qp_user_context = (u64) 0;
-			}
-			priv = (priv + 1) % q->q_size;
-		}
-	}
-	spin_unlock_irq(&cq->lock);
-	c2_cq_put(cq);
-}
-
-static inline enum ib_wc_status c2_cqe_status_to_openib(u8 status)
-{
-	switch (status) {
-	case C2_OK:
-		return IB_WC_SUCCESS;
-	case CCERR_FLUSHED:
-		return IB_WC_WR_FLUSH_ERR;
-	case CCERR_BASE_AND_BOUNDS_VIOLATION:
-		return IB_WC_LOC_PROT_ERR;
-	case CCERR_ACCESS_VIOLATION:
-		return IB_WC_LOC_ACCESS_ERR;
-	case CCERR_TOTAL_LENGTH_TOO_BIG:
-		return IB_WC_LOC_LEN_ERR;
-	case CCERR_INVALID_WINDOW:
-		return IB_WC_MW_BIND_ERR;
-	default:
-		return IB_WC_GENERAL_ERR;
-	}
-}
-
-
-static inline int c2_poll_one(struct c2_dev *c2dev,
-			      struct c2_cq *cq, struct ib_wc *entry)
-{
-	struct c2wr_ce *ce;
-	struct c2_qp *qp;
-	int is_recv = 0;
-
-	ce = c2_mq_consume(&cq->mq);
-	if (!ce) {
-		return -EAGAIN;
-	}
-
-	/*
-	 * if the qp returned is null then this qp has already
-	 * been freed and we are unable process the completion.
-	 * try pulling the next message
-	 */
-	while ((qp =
-		(struct c2_qp *) (unsigned long) ce->qp_user_context) == NULL) {
-		c2_mq_free(&cq->mq);
-		ce = c2_mq_consume(&cq->mq);
-		if (!ce)
-			return -EAGAIN;
-	}
-
-	entry->status = c2_cqe_status_to_openib(c2_wr_get_result(ce));
-	entry->wr_id = ce->hdr.context;
-	entry->qp = &qp->ibqp;
-	entry->wc_flags = 0;
-	entry->slid = 0;
-	entry->sl = 0;
-	entry->src_qp = 0;
-	entry->dlid_path_bits = 0;
-	entry->pkey_index = 0;
-
-	switch (c2_wr_get_id(ce)) {
-	case C2_WR_TYPE_SEND:
-		entry->opcode = IB_WC_SEND;
-		break;
-	case C2_WR_TYPE_RDMA_WRITE:
-		entry->opcode = IB_WC_RDMA_WRITE;
-		break;
-	case C2_WR_TYPE_RDMA_READ:
-		entry->opcode = IB_WC_RDMA_READ;
-		break;
-	case C2_WR_TYPE_BIND_MW:
-		entry->opcode = IB_WC_BIND_MW;
-		break;
-	case C2_WR_TYPE_RECV:
-		entry->byte_len = be32_to_cpu(ce->bytes_rcvd);
-		entry->opcode = IB_WC_RECV;
-		is_recv = 1;
-		break;
-	default:
-		break;
-	}
-
-	/* consume the WQEs */
-	if (is_recv)
-		c2_mq_lconsume(&qp->rq_mq, 1);
-	else
-		c2_mq_lconsume(&qp->sq_mq,
-			       be32_to_cpu(c2_wr_get_wqe_count(ce)) + 1);
-
-	/* free the message */
-	c2_mq_free(&cq->mq);
-
-	return 0;
-}
-
-int c2_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry)
-{
-	struct c2_dev *c2dev = to_c2dev(ibcq->device);
-	struct c2_cq *cq = to_c2cq(ibcq);
-	unsigned long flags;
-	int npolled, err;
-
-	spin_lock_irqsave(&cq->lock, flags);
-
-	for (npolled = 0; npolled < num_entries; ++npolled) {
-
-		err = c2_poll_one(c2dev, cq, entry + npolled);
-		if (err)
-			break;
-	}
-
-	spin_unlock_irqrestore(&cq->lock, flags);
-
-	return npolled;
-}
-
-int c2_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags notify_flags)
-{
-	struct c2_mq_shared __iomem *shared;
-	struct c2_cq *cq;
-	unsigned long flags;
-	int ret = 0;
-
-	cq = to_c2cq(ibcq);
-	shared = cq->mq.peer;
-
-	if ((notify_flags & IB_CQ_SOLICITED_MASK) == IB_CQ_NEXT_COMP)
-		writeb(C2_CQ_NOTIFICATION_TYPE_NEXT, &shared->notification_type);
-	else if ((notify_flags & IB_CQ_SOLICITED_MASK) == IB_CQ_SOLICITED)
-		writeb(C2_CQ_NOTIFICATION_TYPE_NEXT_SE, &shared->notification_type);
-	else
-		return -EINVAL;
-
-	writeb(CQ_WAIT_FOR_DMA | CQ_ARMED, &shared->armed);
-
-	/*
-	 * Now read back shared->armed to make the PCI
-	 * write synchronous.  This is necessary for
-	 * correct cq notification semantics.
-	 */
-	readb(&shared->armed);
-
-	if (notify_flags & IB_CQ_REPORT_MISSED_EVENTS) {
-		spin_lock_irqsave(&cq->lock, flags);
-		ret = !c2_mq_empty(&cq->mq);
-		spin_unlock_irqrestore(&cq->lock, flags);
-	}
-
-	return ret;
-}
-
-static void c2_free_cq_buf(struct c2_dev *c2dev, struct c2_mq *mq)
-{
-	dma_free_coherent(&c2dev->pcidev->dev, mq->q_size * mq->msg_size,
-			  mq->msg_pool.host, dma_unmap_addr(mq, mapping));
-}
-
-static int c2_alloc_cq_buf(struct c2_dev *c2dev, struct c2_mq *mq,
-			   size_t q_size, size_t msg_size)
-{
-	u8 *pool_start;
-
-	if (q_size > SIZE_MAX / msg_size)
-		return -EINVAL;
-
-	pool_start = dma_alloc_coherent(&c2dev->pcidev->dev, q_size * msg_size,
-					&mq->host_dma, GFP_KERNEL);
-	if (!pool_start)
-		return -ENOMEM;
-
-	c2_mq_rep_init(mq,
-		       0,		/* index (currently unknown) */
-		       q_size,
-		       msg_size,
-		       pool_start,
-		       NULL,	/* peer (currently unknown) */
-		       C2_MQ_HOST_TARGET);
-
-	dma_unmap_addr_set(mq, mapping, mq->host_dma);
-
-	return 0;
-}
-
-int c2_init_cq(struct c2_dev *c2dev, int entries,
-	       struct c2_ucontext *ctx, struct c2_cq *cq)
-{
-	struct c2wr_cq_create_req wr;
-	struct c2wr_cq_create_rep *reply;
-	unsigned long peer_pa;
-	struct c2_vq_req *vq_req;
-	int err;
-
-	might_sleep();
-
-	cq->ibcq.cqe = entries - 1;
-	cq->is_kernel = !ctx;
-
-	/* Allocate a shared pointer */
-	cq->mq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-				      &cq->mq.shared_dma, GFP_KERNEL);
-	if (!cq->mq.shared)
-		return -ENOMEM;
-
-	/* Allocate pages for the message pool */
-	err = c2_alloc_cq_buf(c2dev, &cq->mq, entries + 1, C2_CQ_MSG_SIZE);
-	if (err)
-		goto bail0;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_CQ_CREATE);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.msg_size = cpu_to_be32(cq->mq.msg_size);
-	wr.depth = cpu_to_be32(cq->mq.q_size);
-	wr.shared_ht = cpu_to_be64(cq->mq.shared_dma);
-	wr.msg_pool = cpu_to_be64(cq->mq.host_dma);
-	wr.user_context = (u64) (unsigned long) (cq);
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail2;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail2;
-
-	reply = (struct c2wr_cq_create_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail2;
-	}
-
-	if ((err = c2_errno(reply)) != 0)
-		goto bail3;
-
-	cq->adapter_handle = reply->cq_handle;
-	cq->mq.index = be32_to_cpu(reply->mq_index);
-
-	peer_pa = c2dev->pa + be32_to_cpu(reply->adapter_shared);
-	cq->mq.peer = ioremap_nocache(peer_pa, PAGE_SIZE);
-	if (!cq->mq.peer) {
-		err = -ENOMEM;
-		goto bail3;
-	}
-
-	vq_repbuf_free(c2dev, reply);
-	vq_req_free(c2dev, vq_req);
-
-	spin_lock_init(&cq->lock);
-	atomic_set(&cq->refcount, 1);
-	init_waitqueue_head(&cq->wait);
-
-	/*
-	 * Use the MQ index allocated by the adapter to
-	 * store the CQ in the qptr_array
-	 */
-	cq->cqn = cq->mq.index;
-	c2dev->qptr_array[cq->cqn] = cq;
-
-	return 0;
-
-bail3:
-	vq_repbuf_free(c2dev, reply);
-bail2:
-	vq_req_free(c2dev, vq_req);
-bail1:
-	c2_free_cq_buf(c2dev, &cq->mq);
-bail0:
-	c2_free_mqsp(cq->mq.shared);
-
-	return err;
-}
-
-void c2_free_cq(struct c2_dev *c2dev, struct c2_cq *cq)
-{
-	int err;
-	struct c2_vq_req *vq_req;
-	struct c2wr_cq_destroy_req wr;
-	struct c2wr_cq_destroy_rep *reply;
-
-	might_sleep();
-
-	/* Clear CQ from the qptr array */
-	spin_lock_irq(&c2dev->lock);
-	c2dev->qptr_array[cq->mq.index] = NULL;
-	atomic_dec(&cq->refcount);
-	spin_unlock_irq(&c2dev->lock);
-
-	wait_event(cq->wait, !atomic_read(&cq->refcount));
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		goto bail0;
-	}
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_CQ_DESTROY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.cq_handle = cq->adapter_handle;
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail1;
-
-	reply = (struct c2wr_cq_destroy_rep *) (unsigned long) (vq_req->reply_msg);
-	if (reply)
-		vq_repbuf_free(c2dev, reply);
-bail1:
-	vq_req_free(c2dev, vq_req);
-bail0:
-	if (cq->is_kernel) {
-		c2_free_cq_buf(c2dev, &cq->mq);
-	}
-
-	return;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_intr.c b/drivers/staging/rdma/amso1100/c2_intr.c
deleted file mode 100644
index 74b32a971124..000000000000
--- a/drivers/staging/rdma/amso1100/c2_intr.c
+++ /dev/null
@@ -1,219 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include "c2.h"
-#include <rdma/iw_cm.h>
-#include "c2_vq.h"
-
-static void handle_mq(struct c2_dev *c2dev, u32 index);
-static void handle_vq(struct c2_dev *c2dev, u32 mq_index);
-
-/*
- * Handle RNIC interrupts
- */
-void c2_rnic_interrupt(struct c2_dev *c2dev)
-{
-	unsigned int mq_index;
-
-	while (c2dev->hints_read != be16_to_cpu(*c2dev->hint_count)) {
-		mq_index = readl(c2dev->regs + PCI_BAR0_HOST_HINT);
-		if (mq_index & 0x80000000) {
-			break;
-		}
-
-		c2dev->hints_read++;
-		handle_mq(c2dev, mq_index);
-	}
-
-}
-
-/*
- * Top level MQ handler
- */
-static void handle_mq(struct c2_dev *c2dev, u32 mq_index)
-{
-	if (c2dev->qptr_array[mq_index] == NULL) {
-		pr_debug("handle_mq: stray activity for mq_index=%d\n",
-			 mq_index);
-		return;
-	}
-
-	switch (mq_index) {
-	case (0):
-		/*
-		 * An index of 0 in the activity queue
-		 * indicates the req vq now has messages
-		 * available...
-		 *
-		 * Wake up any waiters waiting on req VQ
-		 * message availability.
-		 */
-		wake_up(&c2dev->req_vq_wo);
-		break;
-	case (1):
-		handle_vq(c2dev, mq_index);
-		break;
-	case (2):
-		/* We have to purge the VQ in case there are pending
-		 * accept reply requests that would result in the
-		 * generation of an ESTABLISHED event. If we don't
-		 * generate these first, a CLOSE event could end up
-		 * being delivered before the ESTABLISHED event.
-		 */
-		handle_vq(c2dev, 1);
-
-		c2_ae_event(c2dev, mq_index);
-		break;
-	default:
-		/* There is no event synchronization between CQ events
-		 * and AE or CM events. In fact, CQE could be
-		 * delivered for all of the I/O up to and including the
-		 * FLUSH for a peer disconenct prior to the ESTABLISHED
-		 * event being delivered to the app. The reason for this
-		 * is that CM events are delivered on a thread, while AE
-		 * and CM events are delivered on interrupt context.
-		 */
-		c2_cq_event(c2dev, mq_index);
-		break;
-	}
-
-	return;
-}
-
-/*
- * Handles verbs WR replies.
- */
-static void handle_vq(struct c2_dev *c2dev, u32 mq_index)
-{
-	void *adapter_msg, *reply_msg;
-	struct c2wr_hdr *host_msg;
-	struct c2wr_hdr tmp;
-	struct c2_mq *reply_vq;
-	struct c2_vq_req *req;
-	struct iw_cm_event cm_event;
-	int err;
-
-	reply_vq = c2dev->qptr_array[mq_index];
-
-	/*
-	 * get next msg from mq_index into adapter_msg.
-	 * don't free it yet.
-	 */
-	adapter_msg = c2_mq_consume(reply_vq);
-	if (adapter_msg == NULL) {
-		return;
-	}
-
-	host_msg = vq_repbuf_alloc(c2dev);
-
-	/*
-	 * If we can't get a host buffer, then we'll still
-	 * wakeup the waiter, we just won't give him the msg.
-	 * It is assumed the waiter will deal with this...
-	 */
-	if (!host_msg) {
-		pr_debug("handle_vq: no repbufs!\n");
-
-		/*
-		 * just copy the WR header into a local variable.
-		 * this allows us to still demux on the context
-		 */
-		host_msg = &tmp;
-		memcpy(host_msg, adapter_msg, sizeof(tmp));
-		reply_msg = NULL;
-	} else {
-		memcpy(host_msg, adapter_msg, reply_vq->msg_size);
-		reply_msg = host_msg;
-	}
-
-	/*
-	 * consume the msg from the MQ
-	 */
-	c2_mq_free(reply_vq);
-
-	/*
-	 * wakeup the waiter.
-	 */
-	req = (struct c2_vq_req *) (unsigned long) host_msg->context;
-	if (req == NULL) {
-		/*
-		 * We should never get here, as the adapter should
-		 * never send us a reply that we're not expecting.
-		 */
-		if (reply_msg != NULL)
-			vq_repbuf_free(c2dev, host_msg);
-		pr_debug("handle_vq: UNEXPECTEDLY got NULL req\n");
-		return;
-	}
-
-	if (reply_msg)
-		err = c2_errno(reply_msg);
-	else
-		err = -ENOMEM;
-
-	if (!err) switch (req->event) {
-	case IW_CM_EVENT_ESTABLISHED:
-		c2_set_qp_state(req->qp,
-				C2_QP_STATE_RTS);
-		/*
-		 * Until ird/ord negotiation via MPAv2 support is added, send
-		 * max supported values
-		 */
-		cm_event.ird = cm_event.ord = 128;
-	case IW_CM_EVENT_CLOSE:
-
-		/*
-		 * Move the QP to RTS if this is
-		 * the established event
-		 */
-		cm_event.event = req->event;
-		cm_event.status = 0;
-		cm_event.local_addr = req->cm_id->local_addr;
-		cm_event.remote_addr = req->cm_id->remote_addr;
-		cm_event.private_data = NULL;
-		cm_event.private_data_len = 0;
-		req->cm_id->event_handler(req->cm_id, &cm_event);
-		break;
-	default:
-		break;
-	}
-
-	req->reply_msg = (u64) (unsigned long) (reply_msg);
-	atomic_set(&req->reply_ready, 1);
-	wake_up(&req->wait_object);
-
-	/*
-	 * If the request was cancelled, then this put will
-	 * free the vq_req memory...and reply_msg!!!
-	 */
-	vq_req_put(c2dev, req);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_mm.c b/drivers/staging/rdma/amso1100/c2_mm.c
deleted file mode 100644
index 25081e2913de..000000000000
--- a/drivers/staging/rdma/amso1100/c2_mm.c
+++ /dev/null
@@ -1,377 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include <linux/slab.h>
-
-#include "c2.h"
-#include "c2_vq.h"
-
-#define PBL_VIRT 1
-#define PBL_PHYS 2
-
-/*
- * Send all the PBL messages to convey the remainder of the PBL
- * Wait for the adapter's reply on the last one.
- * This is indicated by setting the MEM_PBL_COMPLETE in the flags.
- *
- * NOTE:  vq_req is _not_ freed by this function.  The VQ Host
- *	  Reply buffer _is_ freed by this function.
- */
-static int
-send_pbl_messages(struct c2_dev *c2dev, __be32 stag_index,
-		  unsigned long va, u32 pbl_depth,
-		  struct c2_vq_req *vq_req, int pbl_type)
-{
-	u32 pbe_count;		/* amt that fits in a PBL msg */
-	u32 count;		/* amt in this PBL MSG. */
-	struct c2wr_nsmr_pbl_req *wr;	/* PBL WR ptr */
-	struct c2wr_nsmr_pbl_rep *reply;	/* reply ptr */
- 	int err, pbl_virt, pbl_index, i;
-
-	switch (pbl_type) {
-	case PBL_VIRT:
-		pbl_virt = 1;
-		break;
-	case PBL_PHYS:
-		pbl_virt = 0;
-		break;
-	default:
-		return -EINVAL;
-		break;
-	}
-
-	pbe_count = (c2dev->req_vq.msg_size -
-		     sizeof(struct c2wr_nsmr_pbl_req)) / sizeof(u64);
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		return -ENOMEM;
-	}
-	c2_wr_set_id(wr, CCWR_NSMR_PBL);
-
-	/*
-	 * Only the last PBL message will generate a reply from the verbs,
-	 * so we set the context to 0 indicating there is no kernel verbs
-	 * handler blocked awaiting this reply.
-	 */
-	wr->hdr.context = 0;
-	wr->rnic_handle = c2dev->adapter_handle;
-	wr->stag_index = stag_index;	/* already swapped */
-	wr->flags = 0;
-	pbl_index = 0;
-	while (pbl_depth) {
-		count = min(pbe_count, pbl_depth);
-		wr->addrs_length = cpu_to_be32(count);
-
-		/*
-		 *  If this is the last message, then reference the
-		 *  vq request struct cuz we're gonna wait for a reply.
-		 *  also make this PBL msg as the last one.
-		 */
-		if (count == pbl_depth) {
-			/*
-			 * reference the request struct.  dereferenced in the
-			 * int handler.
-			 */
-			vq_req_get(c2dev, vq_req);
-			wr->flags = cpu_to_be32(MEM_PBL_COMPLETE);
-
-			/*
-			 * This is the last PBL message.
-			 * Set the context to our VQ Request Object so we can
-			 * wait for the reply.
-			 */
-			wr->hdr.context = (unsigned long) vq_req;
-		}
-
-		/*
-		 * If pbl_virt is set then va is a virtual address
-		 * that describes a virtually contiguous memory
-		 * allocation. The wr needs the start of each virtual page
-		 * to be converted to the corresponding physical address
-		 * of the page. If pbl_virt is not set then va is an array
-		 * of physical addresses and there is no conversion to do.
-		 * Just fill in the wr with what is in the array.
-		 */
-		for (i = 0; i < count; i++) {
-			if (pbl_virt) {
-				va += PAGE_SIZE;
-			} else {
- 				wr->paddrs[i] =
-				    cpu_to_be64(((u64 *)va)[pbl_index + i]);
-			}
-		}
-
-		/*
-		 * Send WR to adapter
-		 */
-		err = vq_send_wr(c2dev, (union c2wr *) wr);
-		if (err) {
-			if (count <= pbe_count) {
-				vq_req_put(c2dev, vq_req);
-			}
-			goto bail0;
-		}
-		pbl_depth -= count;
-		pbl_index += count;
-	}
-
-	/*
-	 *  Now wait for the reply...
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail0;
-	}
-
-	/*
-	 * Process reply
-	 */
-	reply = (struct c2wr_nsmr_pbl_rep *) (unsigned long) vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	err = c2_errno(reply);
-
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	kfree(wr);
-	return err;
-}
-
-#define C2_PBL_MAX_DEPTH 131072
-int
-c2_nsmr_register_phys_kern(struct c2_dev *c2dev, u64 *addr_list,
- 			   int page_size, int pbl_depth, u32 length,
- 			   u32 offset, u64 *va, enum c2_acf acf,
-			   struct c2_mr *mr)
-{
-	struct c2_vq_req *vq_req;
-	struct c2wr_nsmr_register_req *wr;
-	struct c2wr_nsmr_register_rep *reply;
-	u16 flags;
-	int i, pbe_count, count;
-	int err;
-
-	if (!va || !length || !addr_list || !pbl_depth)
-		return -EINTR;
-
-	/*
-	 * Verify PBL depth is within rnic max
-	 */
-	if (pbl_depth > C2_PBL_MAX_DEPTH) {
-		return -EINTR;
-	}
-
-	/*
-	 * allocate verbs request object
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	/*
-	 * build the WR
-	 */
-	c2_wr_set_id(wr, CCWR_NSMR_REGISTER);
-	wr->hdr.context = (unsigned long) vq_req;
-	wr->rnic_handle = c2dev->adapter_handle;
-
-	flags = (acf | MEM_VA_BASED | MEM_REMOTE);
-
-	/*
-	 * compute how many pbes can fit in the message
-	 */
-	pbe_count = (c2dev->req_vq.msg_size -
-		     sizeof(struct c2wr_nsmr_register_req)) / sizeof(u64);
-
-	if (pbl_depth <= pbe_count) {
-		flags |= MEM_PBL_COMPLETE;
-	}
-	wr->flags = cpu_to_be16(flags);
-	wr->stag_key = 0;	//stag_key;
-	wr->va = cpu_to_be64(*va);
-	wr->pd_id = mr->pd->pd_id;
-	wr->pbe_size = cpu_to_be32(page_size);
-	wr->length = cpu_to_be32(length);
-	wr->pbl_depth = cpu_to_be32(pbl_depth);
-	wr->fbo = cpu_to_be32(offset);
-	count = min(pbl_depth, pbe_count);
-	wr->addrs_length = cpu_to_be32(count);
-
-	/*
-	 * fill out the PBL for this message
-	 */
-	for (i = 0; i < count; i++) {
-		wr->paddrs[i] = cpu_to_be64(addr_list[i]);
-	}
-
-	/*
-	 * regerence the request struct
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	/*
-	 * send the WR to the adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	/*
-	 * wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail1;
-	}
-
-	/*
-	 * process reply
-	 */
-	reply =
-	    (struct c2wr_nsmr_register_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-	if ((err = c2_errno(reply))) {
-		goto bail2;
-	}
-	//*p_pb_entries = be32_to_cpu(reply->pbl_depth);
-	mr->ibmr.lkey = mr->ibmr.rkey = be32_to_cpu(reply->stag_index);
-	vq_repbuf_free(c2dev, reply);
-
-	/*
-	 * if there are still more PBEs we need to send them to
-	 * the adapter and wait for a reply on the final one.
-	 * reuse vq_req for this purpose.
-	 */
-	pbl_depth -= count;
-	if (pbl_depth) {
-
-		vq_req->reply_msg = (unsigned long) NULL;
-		atomic_set(&vq_req->reply_ready, 0);
-		err = send_pbl_messages(c2dev,
-					cpu_to_be32(mr->ibmr.lkey),
-					(unsigned long) &addr_list[i],
-					pbl_depth, vq_req, PBL_PHYS);
-		if (err) {
-			goto bail1;
-		}
-	}
-
-	vq_req_free(c2dev, vq_req);
-	kfree(wr);
-
-	return err;
-
-bail2:
-	vq_repbuf_free(c2dev, reply);
-bail1:
-	kfree(wr);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-int c2_stag_dealloc(struct c2_dev *c2dev, u32 stag_index)
-{
-	struct c2_vq_req *vq_req;	/* verbs request object */
-	struct c2wr_stag_dealloc_req wr;	/* work request */
-	struct c2wr_stag_dealloc_rep *reply;	/* WR reply  */
-	int err;
-
-
-	/*
-	 * allocate verbs request object
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		return -ENOMEM;
-	}
-
-	/*
-	 * Build the WR
-	 */
-	c2_wr_set_id(&wr, CCWR_STAG_DEALLOC);
-	wr.hdr.context = (u64) (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.stag_index = cpu_to_be32(stag_index);
-
-	/*
-	 * reference the request struct.  dereferenced in the int handler.
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	/*
-	 * Send WR to adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	/*
-	 * Wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail0;
-	}
-
-	/*
-	 * Process reply
-	 */
-	reply = (struct c2wr_stag_dealloc_rep *) (unsigned long) vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	err = c2_errno(reply);
-
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_mq.c b/drivers/staging/rdma/amso1100/c2_mq.c
deleted file mode 100644
index 7827fb8bdb10..000000000000
--- a/drivers/staging/rdma/amso1100/c2_mq.c
+++ /dev/null
@@ -1,175 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include "c2.h"
-#include "c2_mq.h"
-
-void *c2_mq_alloc(struct c2_mq *q)
-{
-	BUG_ON(q->magic != C2_MQ_MAGIC);
-	BUG_ON(q->type != C2_MQ_ADAPTER_TARGET);
-
-	if (c2_mq_full(q)) {
-		return NULL;
-	} else {
-#ifdef DEBUG
-		struct c2wr_hdr *m =
-		    (struct c2wr_hdr *) (q->msg_pool.host + q->priv * q->msg_size);
-#ifdef CCMSGMAGIC
-		BUG_ON(m->magic != be32_to_cpu(~CCWR_MAGIC));
-		m->magic = cpu_to_be32(CCWR_MAGIC);
-#endif
-		return m;
-#else
-		return q->msg_pool.host + q->priv * q->msg_size;
-#endif
-	}
-}
-
-void c2_mq_produce(struct c2_mq *q)
-{
-	BUG_ON(q->magic != C2_MQ_MAGIC);
-	BUG_ON(q->type != C2_MQ_ADAPTER_TARGET);
-
-	if (!c2_mq_full(q)) {
-		q->priv = (q->priv + 1) % q->q_size;
-		q->hint_count++;
-		/* Update peer's offset. */
-		__raw_writew((__force u16) cpu_to_be16(q->priv), &q->peer->shared);
-	}
-}
-
-void *c2_mq_consume(struct c2_mq *q)
-{
-	BUG_ON(q->magic != C2_MQ_MAGIC);
-	BUG_ON(q->type != C2_MQ_HOST_TARGET);
-
-	if (c2_mq_empty(q)) {
-		return NULL;
-	} else {
-#ifdef DEBUG
-		struct c2wr_hdr *m = (struct c2wr_hdr *)
-		    (q->msg_pool.host + q->priv * q->msg_size);
-#ifdef CCMSGMAGIC
-		BUG_ON(m->magic != be32_to_cpu(CCWR_MAGIC));
-#endif
-		return m;
-#else
-		return q->msg_pool.host + q->priv * q->msg_size;
-#endif
-	}
-}
-
-void c2_mq_free(struct c2_mq *q)
-{
-	BUG_ON(q->magic != C2_MQ_MAGIC);
-	BUG_ON(q->type != C2_MQ_HOST_TARGET);
-
-	if (!c2_mq_empty(q)) {
-
-#ifdef CCMSGMAGIC
-		{
-			struct c2wr_hdr __iomem *m = (struct c2wr_hdr __iomem *)
-			    (q->msg_pool.adapter + q->priv * q->msg_size);
-			__raw_writel(cpu_to_be32(~CCWR_MAGIC), &m->magic);
-		}
-#endif
-		q->priv = (q->priv + 1) % q->q_size;
-		/* Update peer's offset. */
-		__raw_writew((__force u16) cpu_to_be16(q->priv), &q->peer->shared);
-	}
-}
-
-
-void c2_mq_lconsume(struct c2_mq *q, u32 wqe_count)
-{
-	BUG_ON(q->magic != C2_MQ_MAGIC);
-	BUG_ON(q->type != C2_MQ_ADAPTER_TARGET);
-
-	while (wqe_count--) {
-		BUG_ON(c2_mq_empty(q));
-		*q->shared = cpu_to_be16((be16_to_cpu(*q->shared)+1) % q->q_size);
-	}
-}
-
-#if 0
-u32 c2_mq_count(struct c2_mq *q)
-{
-	s32 count;
-
-	if (q->type == C2_MQ_HOST_TARGET)
-		count = be16_to_cpu(*q->shared) - q->priv;
-	else
-		count = q->priv - be16_to_cpu(*q->shared);
-
-	if (count < 0)
-		count += q->q_size;
-
-	return (u32) count;
-}
-#endif  /*  0  */
-
-void c2_mq_req_init(struct c2_mq *q, u32 index, u32 q_size, u32 msg_size,
-		    u8 __iomem *pool_start, u16 __iomem *peer, u32 type)
-{
-	BUG_ON(!q->shared);
-
-	/* This code assumes the byte swapping has already been done! */
-	q->index = index;
-	q->q_size = q_size;
-	q->msg_size = msg_size;
-	q->msg_pool.adapter = pool_start;
-	q->peer = (struct c2_mq_shared __iomem *) peer;
-	q->magic = C2_MQ_MAGIC;
-	q->type = type;
-	q->priv = 0;
-	q->hint_count = 0;
-	return;
-}
-
-void c2_mq_rep_init(struct c2_mq *q, u32 index, u32 q_size, u32 msg_size,
-		    u8 *pool_start, u16 __iomem *peer, u32 type)
-{
-	BUG_ON(!q->shared);
-
-	/* This code assumes the byte swapping has already been done! */
-	q->index = index;
-	q->q_size = q_size;
-	q->msg_size = msg_size;
-	q->msg_pool.host = pool_start;
-	q->peer = (struct c2_mq_shared __iomem *) peer;
-	q->magic = C2_MQ_MAGIC;
-	q->type = type;
-	q->priv = 0;
-	q->hint_count = 0;
-	return;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_mq.h b/drivers/staging/rdma/amso1100/c2_mq.h
deleted file mode 100644
index fc1b9a7cec4b..000000000000
--- a/drivers/staging/rdma/amso1100/c2_mq.h
+++ /dev/null
@@ -1,106 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 _C2_MQ_H_
-#define _C2_MQ_H_
-#include <linux/kernel.h>
-#include <linux/dma-mapping.h>
-#include "c2_wr.h"
-
-enum c2_shared_regs {
-
-	C2_SHARED_ARMED = 0x10,
-	C2_SHARED_NOTIFY = 0x18,
-	C2_SHARED_SHARED = 0x40,
-};
-
-struct c2_mq_shared {
-	u16 unused1;
-	u8 armed;
-	u8 notification_type;
-	u32 unused2;
-	u16 shared;
-	/* Pad to 64 bytes. */
-	u8 pad[64 - sizeof(u16) - 2 * sizeof(u8) - sizeof(u32) - sizeof(u16)];
-};
-
-enum c2_mq_type {
-	C2_MQ_HOST_TARGET = 1,
-	C2_MQ_ADAPTER_TARGET = 2,
-};
-
-/*
- * c2_mq_t is for kernel-mode MQs like the VQs Cand the AEQ.
- * c2_user_mq_t (which is the same format) is for user-mode MQs...
- */
-#define C2_MQ_MAGIC 0x4d512020	/* 'MQ  ' */
-struct c2_mq {
-	u32 magic;
-	union {
-		u8 *host;
-		u8 __iomem *adapter;
-	} msg_pool;
-	dma_addr_t host_dma;
-	DEFINE_DMA_UNMAP_ADDR(mapping);
-	u16 hint_count;
-	u16 priv;
-	struct c2_mq_shared __iomem *peer;
-	__be16 *shared;
-	dma_addr_t shared_dma;
-	u32 q_size;
-	u32 msg_size;
-	u32 index;
-	enum c2_mq_type type;
-};
-
-static __inline__ int c2_mq_empty(struct c2_mq *q)
-{
-	return q->priv == be16_to_cpu(*q->shared);
-}
-
-static __inline__ int c2_mq_full(struct c2_mq *q)
-{
-	return q->priv == (be16_to_cpu(*q->shared) + q->q_size - 1) % q->q_size;
-}
-
-extern void c2_mq_lconsume(struct c2_mq *q, u32 wqe_count);
-extern void *c2_mq_alloc(struct c2_mq *q);
-extern void c2_mq_produce(struct c2_mq *q);
-extern void *c2_mq_consume(struct c2_mq *q);
-extern void c2_mq_free(struct c2_mq *q);
-extern void c2_mq_req_init(struct c2_mq *q, u32 index, u32 q_size, u32 msg_size,
-		       u8 __iomem *pool_start, u16 __iomem *peer, u32 type);
-extern void c2_mq_rep_init(struct c2_mq *q, u32 index, u32 q_size, u32 msg_size,
-			   u8 *pool_start, u16 __iomem *peer, u32 type);
-
-#endif				/* _C2_MQ_H_ */
diff --git a/drivers/staging/rdma/amso1100/c2_pd.c b/drivers/staging/rdma/amso1100/c2_pd.c
deleted file mode 100644
index f3e81dc357bb..000000000000
--- a/drivers/staging/rdma/amso1100/c2_pd.c
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- * Copyright (c) 2004 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Cisco Systems.  All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. 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.
- */
-
-#include <linux/init.h>
-#include <linux/slab.h>
-#include <linux/errno.h>
-
-#include "c2.h"
-#include "c2_provider.h"
-
-int c2_pd_alloc(struct c2_dev *c2dev, int privileged, struct c2_pd *pd)
-{
-	u32 obj;
-	int ret = 0;
-
-	spin_lock(&c2dev->pd_table.lock);
-	obj = find_next_zero_bit(c2dev->pd_table.table, c2dev->pd_table.max,
-				 c2dev->pd_table.last);
-	if (obj >= c2dev->pd_table.max)
-		obj = find_first_zero_bit(c2dev->pd_table.table,
-					  c2dev->pd_table.max);
-	if (obj < c2dev->pd_table.max) {
-		pd->pd_id = obj;
-		__set_bit(obj, c2dev->pd_table.table);
-		c2dev->pd_table.last = obj+1;
-		if (c2dev->pd_table.last >= c2dev->pd_table.max)
-			c2dev->pd_table.last = 0;
-	} else
-		ret = -ENOMEM;
-	spin_unlock(&c2dev->pd_table.lock);
-	return ret;
-}
-
-void c2_pd_free(struct c2_dev *c2dev, struct c2_pd *pd)
-{
-	spin_lock(&c2dev->pd_table.lock);
-	__clear_bit(pd->pd_id, c2dev->pd_table.table);
-	spin_unlock(&c2dev->pd_table.lock);
-}
-
-int c2_init_pd_table(struct c2_dev *c2dev)
-{
-
-	c2dev->pd_table.last = 0;
-	c2dev->pd_table.max = c2dev->props.max_pd;
-	spin_lock_init(&c2dev->pd_table.lock);
-	c2dev->pd_table.table = kmalloc(BITS_TO_LONGS(c2dev->props.max_pd) *
-					sizeof(long), GFP_KERNEL);
-	if (!c2dev->pd_table.table)
-		return -ENOMEM;
-	bitmap_zero(c2dev->pd_table.table, c2dev->props.max_pd);
-	return 0;
-}
-
-void c2_cleanup_pd_table(struct c2_dev *c2dev)
-{
-	kfree(c2dev->pd_table.table);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_provider.c b/drivers/staging/rdma/amso1100/c2_provider.c
deleted file mode 100644
index c707e45887c2..000000000000
--- a/drivers/staging/rdma/amso1100/c2_provider.c
+++ /dev/null
@@ -1,912 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- *
- */
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/pci.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/inetdevice.h>
-#include <linux/delay.h>
-#include <linux/ethtool.h>
-#include <linux/mii.h>
-#include <linux/if_vlan.h>
-#include <linux/crc32.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/tcp.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/if_arp.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/byteorder.h>
-
-#include <rdma/ib_smi.h>
-#include <rdma/ib_umem.h>
-#include <rdma/ib_user_verbs.h>
-#include "c2.h"
-#include "c2_provider.h"
-#include "c2_user.h"
-
-static int c2_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
-			   struct ib_udata *uhw)
-{
-	struct c2_dev *c2dev = to_c2dev(ibdev);
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	if (uhw->inlen || uhw->outlen)
-		return -EINVAL;
-
-	*props = c2dev->props;
-	return 0;
-}
-
-static int c2_query_port(struct ib_device *ibdev,
-			 u8 port, struct ib_port_attr *props)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	props->max_mtu = IB_MTU_4096;
-	props->lid = 0;
-	props->lmc = 0;
-	props->sm_lid = 0;
-	props->sm_sl = 0;
-	props->state = IB_PORT_ACTIVE;
-	props->phys_state = 0;
-	props->port_cap_flags =
-	    IB_PORT_CM_SUP |
-	    IB_PORT_REINIT_SUP |
-	    IB_PORT_VENDOR_CLASS_SUP | IB_PORT_BOOT_MGMT_SUP;
-	props->gid_tbl_len = 1;
-	props->pkey_tbl_len = 1;
-	props->qkey_viol_cntr = 0;
-	props->active_width = 1;
-	props->active_speed = IB_SPEED_SDR;
-
-	return 0;
-}
-
-static int c2_query_pkey(struct ib_device *ibdev,
-			 u8 port, u16 index, u16 * pkey)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	*pkey = 0;
-	return 0;
-}
-
-static int c2_query_gid(struct ib_device *ibdev, u8 port,
-			int index, union ib_gid *gid)
-{
-	struct c2_dev *c2dev = to_c2dev(ibdev);
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	memset(&(gid->raw[0]), 0, sizeof(gid->raw));
-	memcpy(&(gid->raw[0]), c2dev->pseudo_netdev->dev_addr, 6);
-
-	return 0;
-}
-
-/* Allocate the user context data structure. This keeps track
- * of all objects associated with a particular user-mode client.
- */
-static struct ib_ucontext *c2_alloc_ucontext(struct ib_device *ibdev,
-					     struct ib_udata *udata)
-{
-	struct c2_ucontext *context;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	context = kmalloc(sizeof(*context), GFP_KERNEL);
-	if (!context)
-		return ERR_PTR(-ENOMEM);
-
-	return &context->ibucontext;
-}
-
-static int c2_dealloc_ucontext(struct ib_ucontext *context)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	kfree(context);
-	return 0;
-}
-
-static int c2_mmap_uar(struct ib_ucontext *context, struct vm_area_struct *vma)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return -ENOSYS;
-}
-
-static struct ib_pd *c2_alloc_pd(struct ib_device *ibdev,
-				 struct ib_ucontext *context,
-				 struct ib_udata *udata)
-{
-	struct c2_pd *pd;
-	int err;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	pd = kmalloc(sizeof(*pd), GFP_KERNEL);
-	if (!pd)
-		return ERR_PTR(-ENOMEM);
-
-	err = c2_pd_alloc(to_c2dev(ibdev), !context, pd);
-	if (err) {
-		kfree(pd);
-		return ERR_PTR(err);
-	}
-
-	if (context) {
-		if (ib_copy_to_udata(udata, &pd->pd_id, sizeof(__u32))) {
-			c2_pd_free(to_c2dev(ibdev), pd);
-			kfree(pd);
-			return ERR_PTR(-EFAULT);
-		}
-	}
-
-	return &pd->ibpd;
-}
-
-static int c2_dealloc_pd(struct ib_pd *pd)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	c2_pd_free(to_c2dev(pd->device), to_c2pd(pd));
-	kfree(pd);
-
-	return 0;
-}
-
-static struct ib_ah *c2_ah_create(struct ib_pd *pd, struct ib_ah_attr *ah_attr)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return ERR_PTR(-ENOSYS);
-}
-
-static int c2_ah_destroy(struct ib_ah *ah)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return -ENOSYS;
-}
-
-static void c2_add_ref(struct ib_qp *ibqp)
-{
-	struct c2_qp *qp;
-	BUG_ON(!ibqp);
-	qp = to_c2qp(ibqp);
-	atomic_inc(&qp->refcount);
-}
-
-static void c2_rem_ref(struct ib_qp *ibqp)
-{
-	struct c2_qp *qp;
-	BUG_ON(!ibqp);
-	qp = to_c2qp(ibqp);
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-}
-
-struct ib_qp *c2_get_qp(struct ib_device *device, int qpn)
-{
-	struct c2_dev* c2dev = to_c2dev(device);
-	struct c2_qp *qp;
-
-	qp = c2_find_qpn(c2dev, qpn);
-	pr_debug("%s Returning QP=%p for QPN=%d, device=%p, refcount=%d\n",
-		__func__, qp, qpn, device,
-		(qp?atomic_read(&qp->refcount):0));
-
-	return (qp?&qp->ibqp:NULL);
-}
-
-static struct ib_qp *c2_create_qp(struct ib_pd *pd,
-				  struct ib_qp_init_attr *init_attr,
-				  struct ib_udata *udata)
-{
-	struct c2_qp *qp;
-	int err;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	if (init_attr->create_flags)
-		return ERR_PTR(-EINVAL);
-
-	switch (init_attr->qp_type) {
-	case IB_QPT_RC:
-		qp = kzalloc(sizeof(*qp), GFP_KERNEL);
-		if (!qp) {
-			pr_debug("%s: Unable to allocate QP\n", __func__);
-			return ERR_PTR(-ENOMEM);
-		}
-		spin_lock_init(&qp->lock);
-		if (pd->uobject) {
-			/* userspace specific */
-		}
-
-		err = c2_alloc_qp(to_c2dev(pd->device),
-				  to_c2pd(pd), init_attr, qp);
-
-		if (err && pd->uobject) {
-			/* userspace specific */
-		}
-
-		break;
-	default:
-		pr_debug("%s: Invalid QP type: %d\n", __func__,
-			init_attr->qp_type);
-		return ERR_PTR(-EINVAL);
-	}
-
-	if (err) {
-		kfree(qp);
-		return ERR_PTR(err);
-	}
-
-	return &qp->ibqp;
-}
-
-static int c2_destroy_qp(struct ib_qp *ib_qp)
-{
-	struct c2_qp *qp = to_c2qp(ib_qp);
-
-	pr_debug("%s:%u qp=%p,qp->state=%d\n",
-		__func__, __LINE__, ib_qp, qp->state);
-	c2_free_qp(to_c2dev(ib_qp->device), qp);
-	kfree(qp);
-	return 0;
-}
-
-static struct ib_cq *c2_create_cq(struct ib_device *ibdev,
-				  const struct ib_cq_init_attr *attr,
-				  struct ib_ucontext *context,
-				  struct ib_udata *udata)
-{
-	int entries = attr->cqe;
-	struct c2_cq *cq;
-	int err;
-
-	if (attr->flags)
-		return ERR_PTR(-EINVAL);
-
-	cq = kmalloc(sizeof(*cq), GFP_KERNEL);
-	if (!cq) {
-		pr_debug("%s: Unable to allocate CQ\n", __func__);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	err = c2_init_cq(to_c2dev(ibdev), entries, NULL, cq);
-	if (err) {
-		pr_debug("%s: error initializing CQ\n", __func__);
-		kfree(cq);
-		return ERR_PTR(err);
-	}
-
-	return &cq->ibcq;
-}
-
-static int c2_destroy_cq(struct ib_cq *ib_cq)
-{
-	struct c2_cq *cq = to_c2cq(ib_cq);
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	c2_free_cq(to_c2dev(ib_cq->device), cq);
-	kfree(cq);
-
-	return 0;
-}
-
-static inline u32 c2_convert_access(int acc)
-{
-	return (acc & IB_ACCESS_REMOTE_WRITE ? C2_ACF_REMOTE_WRITE : 0) |
-	    (acc & IB_ACCESS_REMOTE_READ ? C2_ACF_REMOTE_READ : 0) |
-	    (acc & IB_ACCESS_LOCAL_WRITE ? C2_ACF_LOCAL_WRITE : 0) |
-	    C2_ACF_LOCAL_READ | C2_ACF_WINDOW_BIND;
-}
-
-static struct ib_mr *c2_reg_phys_mr(struct ib_pd *ib_pd,
-				    struct ib_phys_buf *buffer_list,
-				    int num_phys_buf, int acc, u64 * iova_start)
-{
-	struct c2_mr *mr;
-	u64 *page_list;
-	u32 total_len;
-	int err, i, j, k, page_shift, pbl_depth;
-
-	pbl_depth = 0;
-	total_len = 0;
-
-	page_shift = PAGE_SHIFT;
-	/*
-	 * If there is only 1 buffer we assume this could
-	 * be a map of all phy mem...use a 32k page_shift.
-	 */
-	if (num_phys_buf == 1)
-		page_shift += 3;
-
-	for (i = 0; i < num_phys_buf; i++) {
-
-		if (offset_in_page(buffer_list[i].addr)) {
-			pr_debug("Unaligned Memory Buffer: 0x%x\n",
-				(unsigned int) buffer_list[i].addr);
-			return ERR_PTR(-EINVAL);
-		}
-
-		if (!buffer_list[i].size) {
-			pr_debug("Invalid Buffer Size\n");
-			return ERR_PTR(-EINVAL);
-		}
-
-		total_len += buffer_list[i].size;
-		pbl_depth += ALIGN(buffer_list[i].size,
-				   BIT(page_shift)) >> page_shift;
-	}
-
-	page_list = vmalloc(sizeof(u64) * pbl_depth);
-	if (!page_list) {
-		pr_debug("couldn't vmalloc page_list of size %zd\n",
-			(sizeof(u64) * pbl_depth));
-		return ERR_PTR(-ENOMEM);
-	}
-
-	for (i = 0, j = 0; i < num_phys_buf; i++) {
-
-		int naddrs;
-
- 		naddrs = ALIGN(buffer_list[i].size,
-			       BIT(page_shift)) >> page_shift;
-		for (k = 0; k < naddrs; k++)
-			page_list[j++] = (buffer_list[i].addr +
-						     (k << page_shift));
-	}
-
-	mr = kmalloc(sizeof(*mr), GFP_KERNEL);
-	if (!mr) {
-		vfree(page_list);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	mr->pd = to_c2pd(ib_pd);
-	mr->umem = NULL;
-	pr_debug("%s - page shift %d, pbl_depth %d, total_len %u, "
-		"*iova_start %llx, first pa %llx, last pa %llx\n",
-		__func__, page_shift, pbl_depth, total_len,
-		(unsigned long long) *iova_start,
-	       	(unsigned long long) page_list[0],
-	       	(unsigned long long) page_list[pbl_depth-1]);
-  	err = c2_nsmr_register_phys_kern(to_c2dev(ib_pd->device), page_list,
-					 BIT(page_shift), pbl_depth,
-					 total_len, 0, iova_start,
-					 c2_convert_access(acc), mr);
-	vfree(page_list);
-	if (err) {
-		kfree(mr);
-		return ERR_PTR(err);
-	}
-
-	return &mr->ibmr;
-}
-
-static struct ib_mr *c2_get_dma_mr(struct ib_pd *pd, int acc)
-{
-	struct ib_phys_buf bl;
-	u64 kva = 0;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	/* AMSO1100 limit */
-	bl.size = 0xffffffff;
-	bl.addr = 0;
-	return c2_reg_phys_mr(pd, &bl, 1, acc, &kva);
-}
-
-static struct ib_mr *c2_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
-				    u64 virt, int acc, struct ib_udata *udata)
-{
-	u64 *pages;
-	u64 kva = 0;
-	int shift, n, len;
-	int i, k, entry;
-	int err = 0;
-	struct scatterlist *sg;
-	struct c2_pd *c2pd = to_c2pd(pd);
-	struct c2_mr *c2mr;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	c2mr = kmalloc(sizeof(*c2mr), GFP_KERNEL);
-	if (!c2mr)
-		return ERR_PTR(-ENOMEM);
-	c2mr->pd = c2pd;
-
-	c2mr->umem = ib_umem_get(pd->uobject->context, start, length, acc, 0);
-	if (IS_ERR(c2mr->umem)) {
-		err = PTR_ERR(c2mr->umem);
-		kfree(c2mr);
-		return ERR_PTR(err);
-	}
-
-	shift = ffs(c2mr->umem->page_size) - 1;
-	n = c2mr->umem->nmap;
-
-	pages = kmalloc_array(n, sizeof(u64), GFP_KERNEL);
-	if (!pages) {
-		err = -ENOMEM;
-		goto err;
-	}
-
-	i = 0;
-	for_each_sg(c2mr->umem->sg_head.sgl, sg, c2mr->umem->nmap, entry) {
-		len = sg_dma_len(sg) >> shift;
-		for (k = 0; k < len; ++k) {
-			pages[i++] =
-				sg_dma_address(sg) +
-				(c2mr->umem->page_size * k);
-		}
-	}
-
-	kva = virt;
-  	err = c2_nsmr_register_phys_kern(to_c2dev(pd->device),
-					 pages,
-					 c2mr->umem->page_size,
-					 i,
-					 length,
-					 ib_umem_offset(c2mr->umem),
-					 &kva,
-					 c2_convert_access(acc),
-					 c2mr);
-	kfree(pages);
-	if (err)
-		goto err;
-	return &c2mr->ibmr;
-
-err:
-	ib_umem_release(c2mr->umem);
-	kfree(c2mr);
-	return ERR_PTR(err);
-}
-
-static int c2_dereg_mr(struct ib_mr *ib_mr)
-{
-	struct c2_mr *mr = to_c2mr(ib_mr);
-	int err;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	err = c2_stag_dealloc(to_c2dev(ib_mr->device), ib_mr->lkey);
-	if (err)
-		pr_debug("c2_stag_dealloc failed: %d\n", err);
-	else {
-		if (mr->umem)
-			ib_umem_release(mr->umem);
-		kfree(mr);
-	}
-
-	return err;
-}
-
-static ssize_t show_rev(struct device *dev, struct device_attribute *attr,
-			char *buf)
-{
-	struct c2_dev *c2dev = container_of(dev, struct c2_dev, ibdev.dev);
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return sprintf(buf, "%x\n", c2dev->props.hw_ver);
-}
-
-static ssize_t show_fw_ver(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	struct c2_dev *c2dev = container_of(dev, struct c2_dev, ibdev.dev);
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return sprintf(buf, "%x.%x.%x\n",
-		       (int) (c2dev->props.fw_ver >> 32),
-		       (int) (c2dev->props.fw_ver >> 16) & 0xffff,
-		       (int) (c2dev->props.fw_ver & 0xffff));
-}
-
-static ssize_t show_hca(struct device *dev, struct device_attribute *attr,
-			char *buf)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return sprintf(buf, "AMSO1100\n");
-}
-
-static ssize_t show_board(struct device *dev, struct device_attribute *attr,
-			  char *buf)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return sprintf(buf, "%.*s\n", 32, "AMSO1100 Board ID");
-}
-
-static DEVICE_ATTR(hw_rev, S_IRUGO, show_rev, NULL);
-static DEVICE_ATTR(fw_ver, S_IRUGO, show_fw_ver, NULL);
-static DEVICE_ATTR(hca_type, S_IRUGO, show_hca, NULL);
-static DEVICE_ATTR(board_id, S_IRUGO, show_board, NULL);
-
-static struct device_attribute *c2_dev_attributes[] = {
-	&dev_attr_hw_rev,
-	&dev_attr_fw_ver,
-	&dev_attr_hca_type,
-	&dev_attr_board_id
-};
-
-static int c2_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-			int attr_mask, struct ib_udata *udata)
-{
-	int err;
-
-	err =
-	    c2_qp_modify(to_c2dev(ibqp->device), to_c2qp(ibqp), attr,
-			 attr_mask);
-
-	return err;
-}
-
-static int c2_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return -ENOSYS;
-}
-
-static int c2_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return -ENOSYS;
-}
-
-static int c2_process_mad(struct ib_device *ibdev,
-			  int 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)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	return -ENOSYS;
-}
-
-static int c2_connect(struct iw_cm_id *cm_id, struct iw_cm_conn_param *iw_param)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	/* Request a connection */
-	return c2_llp_connect(cm_id, iw_param);
-}
-
-static int c2_accept(struct iw_cm_id *cm_id, struct iw_cm_conn_param *iw_param)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	/* Accept the new connection */
-	return c2_llp_accept(cm_id, iw_param);
-}
-
-static int c2_reject(struct iw_cm_id *cm_id, const void *pdata, u8 pdata_len)
-{
-	int err;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	err = c2_llp_reject(cm_id, pdata, pdata_len);
-	return err;
-}
-
-static int c2_service_create(struct iw_cm_id *cm_id, int backlog)
-{
-	int err;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	err = c2_llp_service_create(cm_id, backlog);
-	pr_debug("%s:%u err=%d\n",
-		__func__, __LINE__,
-		err);
-	return err;
-}
-
-static int c2_service_destroy(struct iw_cm_id *cm_id)
-{
-	int err;
-	pr_debug("%s:%u\n", __func__, __LINE__);
-
-	err = c2_llp_service_destroy(cm_id);
-
-	return err;
-}
-
-static int c2_pseudo_up(struct net_device *netdev)
-{
-	struct in_device *ind;
-	struct c2_dev *c2dev = netdev->ml_priv;
-
-	ind = in_dev_get(netdev);
-	if (!ind)
-		return 0;
-
-	pr_debug("adding...\n");
-	for_ifa(ind) {
-#ifdef DEBUG
-		u8 *ip = (u8 *) & ifa->ifa_address;
-
-		pr_debug("%s: %d.%d.%d.%d\n",
-		       ifa->ifa_label, ip[0], ip[1], ip[2], ip[3]);
-#endif
-		c2_add_addr(c2dev, ifa->ifa_address, ifa->ifa_mask);
-	}
-	endfor_ifa(ind);
-	in_dev_put(ind);
-
-	return 0;
-}
-
-static int c2_pseudo_down(struct net_device *netdev)
-{
-	struct in_device *ind;
-	struct c2_dev *c2dev = netdev->ml_priv;
-
-	ind = in_dev_get(netdev);
-	if (!ind)
-		return 0;
-
-	pr_debug("deleting...\n");
-	for_ifa(ind) {
-#ifdef DEBUG
-		u8 *ip = (u8 *) & ifa->ifa_address;
-
-		pr_debug("%s: %d.%d.%d.%d\n",
-		       ifa->ifa_label, ip[0], ip[1], ip[2], ip[3]);
-#endif
-		c2_del_addr(c2dev, ifa->ifa_address, ifa->ifa_mask);
-	}
-	endfor_ifa(ind);
-	in_dev_put(ind);
-
-	return 0;
-}
-
-static int c2_pseudo_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
-{
-	kfree_skb(skb);
-	return NETDEV_TX_OK;
-}
-
-static int c2_pseudo_change_mtu(struct net_device *netdev, int new_mtu)
-{
-	if (new_mtu < ETH_ZLEN || new_mtu > ETH_JUMBO_MTU)
-		return -EINVAL;
-
-	netdev->mtu = new_mtu;
-
-	/* TODO: Tell rnic about new rmda interface mtu */
-	return 0;
-}
-
-static const struct net_device_ops c2_pseudo_netdev_ops = {
-	.ndo_open 		= c2_pseudo_up,
-	.ndo_stop 		= c2_pseudo_down,
-	.ndo_start_xmit 	= c2_pseudo_xmit_frame,
-	.ndo_change_mtu 	= c2_pseudo_change_mtu,
-	.ndo_validate_addr	= eth_validate_addr,
-};
-
-static void setup(struct net_device *netdev)
-{
-	netdev->netdev_ops = &c2_pseudo_netdev_ops;
-
-	netdev->watchdog_timeo = 0;
-	netdev->type = ARPHRD_ETHER;
-	netdev->mtu = 1500;
-	netdev->hard_header_len = ETH_HLEN;
-	netdev->addr_len = ETH_ALEN;
-	netdev->tx_queue_len = 0;
-	netdev->flags |= IFF_NOARP;
-}
-
-static struct net_device *c2_pseudo_netdev_init(struct c2_dev *c2dev)
-{
-	char name[IFNAMSIZ];
-	struct net_device *netdev;
-
-	/* change ethxxx to iwxxx */
-	strcpy(name, "iw");
-	strcat(name, &c2dev->netdev->name[3]);
-	netdev = alloc_netdev(0, name, NET_NAME_UNKNOWN, setup);
-	if (!netdev) {
-		printk(KERN_ERR PFX "%s -  etherdev alloc failed",
-			__func__);
-		return NULL;
-	}
-
-	netdev->ml_priv = c2dev;
-
-	SET_NETDEV_DEV(netdev, &c2dev->pcidev->dev);
-
-	memcpy_fromio(netdev->dev_addr, c2dev->kva + C2_REGS_RDMA_ENADDR, 6);
-
-	/* Print out the MAC address */
-	pr_debug("%s: MAC %pM\n", netdev->name, netdev->dev_addr);
-
-#if 0
-	/* Disable network packets */
-	netif_stop_queue(netdev);
-#endif
-	return netdev;
-}
-
-static int c2_port_immutable(struct ib_device *ibdev, u8 port_num,
-			     struct ib_port_immutable *immutable)
-{
-	struct ib_port_attr attr;
-	int err;
-
-	err = c2_query_port(ibdev, port_num, &attr);
-	if (err)
-		return err;
-
-	immutable->pkey_tbl_len = attr.pkey_tbl_len;
-	immutable->gid_tbl_len = attr.gid_tbl_len;
-	immutable->core_cap_flags = RDMA_CORE_PORT_IWARP;
-
-	return 0;
-}
-
-int c2_register_device(struct c2_dev *dev)
-{
-	int ret = -ENOMEM;
-	int i;
-
-	/* Register pseudo network device */
-	dev->pseudo_netdev = c2_pseudo_netdev_init(dev);
-	if (!dev->pseudo_netdev)
-		goto out;
-
-	ret = register_netdev(dev->pseudo_netdev);
-	if (ret)
-		goto out_free_netdev;
-
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	strlcpy(dev->ibdev.name, "amso%d", IB_DEVICE_NAME_MAX);
-	dev->ibdev.owner = THIS_MODULE;
-	dev->ibdev.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_REQ_NOTIFY_CQ) |
-	    (1ull << IB_USER_VERBS_CMD_CREATE_QP) |
-	    (1ull << IB_USER_VERBS_CMD_MODIFY_QP) |
-	    (1ull << IB_USER_VERBS_CMD_POLL_CQ) |
-	    (1ull << IB_USER_VERBS_CMD_DESTROY_QP) |
-	    (1ull << IB_USER_VERBS_CMD_POST_SEND) |
-	    (1ull << IB_USER_VERBS_CMD_POST_RECV);
-
-	dev->ibdev.node_type = RDMA_NODE_RNIC;
-	memset(&dev->ibdev.node_guid, 0, sizeof(dev->ibdev.node_guid));
-	memcpy(&dev->ibdev.node_guid, dev->pseudo_netdev->dev_addr, 6);
-	dev->ibdev.phys_port_cnt = 1;
-	dev->ibdev.num_comp_vectors = 1;
-	dev->ibdev.dma_device = &dev->pcidev->dev;
-	dev->ibdev.query_device = c2_query_device;
-	dev->ibdev.query_port = c2_query_port;
-	dev->ibdev.query_pkey = c2_query_pkey;
-	dev->ibdev.query_gid = c2_query_gid;
-	dev->ibdev.alloc_ucontext = c2_alloc_ucontext;
-	dev->ibdev.dealloc_ucontext = c2_dealloc_ucontext;
-	dev->ibdev.mmap = c2_mmap_uar;
-	dev->ibdev.alloc_pd = c2_alloc_pd;
-	dev->ibdev.dealloc_pd = c2_dealloc_pd;
-	dev->ibdev.create_ah = c2_ah_create;
-	dev->ibdev.destroy_ah = c2_ah_destroy;
-	dev->ibdev.create_qp = c2_create_qp;
-	dev->ibdev.modify_qp = c2_modify_qp;
-	dev->ibdev.destroy_qp = c2_destroy_qp;
-	dev->ibdev.create_cq = c2_create_cq;
-	dev->ibdev.destroy_cq = c2_destroy_cq;
-	dev->ibdev.poll_cq = c2_poll_cq;
-	dev->ibdev.get_dma_mr = c2_get_dma_mr;
-	dev->ibdev.reg_phys_mr = c2_reg_phys_mr;
-	dev->ibdev.reg_user_mr = c2_reg_user_mr;
-	dev->ibdev.dereg_mr = c2_dereg_mr;
-	dev->ibdev.get_port_immutable = c2_port_immutable;
-
-	dev->ibdev.alloc_fmr = NULL;
-	dev->ibdev.unmap_fmr = NULL;
-	dev->ibdev.dealloc_fmr = NULL;
-	dev->ibdev.map_phys_fmr = NULL;
-
-	dev->ibdev.attach_mcast = c2_multicast_attach;
-	dev->ibdev.detach_mcast = c2_multicast_detach;
-	dev->ibdev.process_mad = c2_process_mad;
-
-	dev->ibdev.req_notify_cq = c2_arm_cq;
-	dev->ibdev.post_send = c2_post_send;
-	dev->ibdev.post_recv = c2_post_receive;
-
-	dev->ibdev.iwcm = kmalloc(sizeof(*dev->ibdev.iwcm), GFP_KERNEL);
-	if (dev->ibdev.iwcm == NULL) {
-		ret = -ENOMEM;
-		goto out_unregister_netdev;
-	}
-	dev->ibdev.iwcm->add_ref = c2_add_ref;
-	dev->ibdev.iwcm->rem_ref = c2_rem_ref;
-	dev->ibdev.iwcm->get_qp = c2_get_qp;
-	dev->ibdev.iwcm->connect = c2_connect;
-	dev->ibdev.iwcm->accept = c2_accept;
-	dev->ibdev.iwcm->reject = c2_reject;
-	dev->ibdev.iwcm->create_listen = c2_service_create;
-	dev->ibdev.iwcm->destroy_listen = c2_service_destroy;
-
-	ret = ib_register_device(&dev->ibdev, NULL);
-	if (ret)
-		goto out_free_iwcm;
-
-	for (i = 0; i < ARRAY_SIZE(c2_dev_attributes); ++i) {
-		ret = device_create_file(&dev->ibdev.dev,
-					       c2_dev_attributes[i]);
-		if (ret)
-			goto out_unregister_ibdev;
-	}
-	goto out;
-
-out_unregister_ibdev:
-	ib_unregister_device(&dev->ibdev);
-out_free_iwcm:
-	kfree(dev->ibdev.iwcm);
-out_unregister_netdev:
-	unregister_netdev(dev->pseudo_netdev);
-out_free_netdev:
-	free_netdev(dev->pseudo_netdev);
-out:
-	pr_debug("%s:%u ret=%d\n", __func__, __LINE__, ret);
-	return ret;
-}
-
-void c2_unregister_device(struct c2_dev *dev)
-{
-	pr_debug("%s:%u\n", __func__, __LINE__);
-	unregister_netdev(dev->pseudo_netdev);
-	free_netdev(dev->pseudo_netdev);
-	ib_unregister_device(&dev->ibdev);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_provider.h b/drivers/staging/rdma/amso1100/c2_provider.h
deleted file mode 100644
index bf189987711f..000000000000
--- a/drivers/staging/rdma/amso1100/c2_provider.h
+++ /dev/null
@@ -1,182 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 C2_PROVIDER_H
-#define C2_PROVIDER_H
-#include <linux/inetdevice.h>
-
-#include <rdma/ib_verbs.h>
-#include <rdma/ib_pack.h>
-
-#include "c2_mq.h"
-#include <rdma/iw_cm.h>
-
-#define C2_MPT_FLAG_ATOMIC        (1 << 14)
-#define C2_MPT_FLAG_REMOTE_WRITE  (1 << 13)
-#define C2_MPT_FLAG_REMOTE_READ   (1 << 12)
-#define C2_MPT_FLAG_LOCAL_WRITE   (1 << 11)
-#define C2_MPT_FLAG_LOCAL_READ    (1 << 10)
-
-struct c2_buf_list {
-	void *buf;
-	DEFINE_DMA_UNMAP_ADDR(mapping);
-};
-
-
-/* The user context keeps track of objects allocated for a
- * particular user-mode client. */
-struct c2_ucontext {
-	struct ib_ucontext ibucontext;
-};
-
-struct c2_mtt;
-
-/* All objects associated with a PD are kept in the
- * associated user context if present.
- */
-struct c2_pd {
-	struct ib_pd ibpd;
-	u32 pd_id;
-};
-
-struct c2_mr {
-	struct ib_mr ibmr;
-	struct c2_pd *pd;
-	struct ib_umem *umem;
-};
-
-struct c2_av;
-
-enum c2_ah_type {
-	C2_AH_ON_HCA,
-	C2_AH_PCI_POOL,
-	C2_AH_KMALLOC
-};
-
-struct c2_ah {
-	struct ib_ah ibah;
-};
-
-struct c2_cq {
-	struct ib_cq ibcq;
-	spinlock_t lock;
-	atomic_t refcount;
-	int cqn;
-	int is_kernel;
-	wait_queue_head_t wait;
-
-	u32 adapter_handle;
-	struct c2_mq mq;
-};
-
-struct c2_wq {
-	spinlock_t lock;
-};
-struct iw_cm_id;
-struct c2_qp {
-	struct ib_qp ibqp;
-	struct iw_cm_id *cm_id;
-	spinlock_t lock;
-	atomic_t refcount;
-	wait_queue_head_t wait;
-	int qpn;
-
-	u32 adapter_handle;
-	u32 send_sgl_depth;
-	u32 recv_sgl_depth;
-	u32 rdma_write_sgl_depth;
-	u8 state;
-
-	struct c2_mq sq_mq;
-	struct c2_mq rq_mq;
-};
-
-struct c2_cr_query_attrs {
-	u32 local_addr;
-	u32 remote_addr;
-	u16 local_port;
-	u16 remote_port;
-};
-
-static inline struct c2_pd *to_c2pd(struct ib_pd *ibpd)
-{
-	return container_of(ibpd, struct c2_pd, ibpd);
-}
-
-static inline struct c2_ucontext *to_c2ucontext(struct ib_ucontext *ibucontext)
-{
-	return container_of(ibucontext, struct c2_ucontext, ibucontext);
-}
-
-static inline struct c2_mr *to_c2mr(struct ib_mr *ibmr)
-{
-	return container_of(ibmr, struct c2_mr, ibmr);
-}
-
-
-static inline struct c2_ah *to_c2ah(struct ib_ah *ibah)
-{
-	return container_of(ibah, struct c2_ah, ibah);
-}
-
-static inline struct c2_cq *to_c2cq(struct ib_cq *ibcq)
-{
-	return container_of(ibcq, struct c2_cq, ibcq);
-}
-
-static inline struct c2_qp *to_c2qp(struct ib_qp *ibqp)
-{
-	return container_of(ibqp, struct c2_qp, ibqp);
-}
-
-static inline int is_rnic_addr(struct net_device *netdev, u32 addr)
-{
-	struct in_device *ind;
-	int ret = 0;
-
-	ind = in_dev_get(netdev);
-	if (!ind)
-		return 0;
-
-	for_ifa(ind) {
-		if (ifa->ifa_address == addr) {
-			ret = 1;
-			break;
-		}
-	}
-	endfor_ifa(ind);
-	in_dev_put(ind);
-	return ret;
-}
-#endif				/* C2_PROVIDER_H */
diff --git a/drivers/staging/rdma/amso1100/c2_qp.c b/drivers/staging/rdma/amso1100/c2_qp.c
deleted file mode 100644
index ca364dbe369c..000000000000
--- a/drivers/staging/rdma/amso1100/c2_qp.c
+++ /dev/null
@@ -1,1024 +0,0 @@
-/*
- * Copyright (c) 2004 Topspin Communications.  All rights reserved.
- * Copyright (c) 2005 Cisco Systems. All rights reserved.
- * Copyright (c) 2005 Mellanox Technologies. All rights reserved.
- * Copyright (c) 2004 Voltaire, Inc. 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.
- *
- */
-
-#include <linux/delay.h>
-#include <linux/gfp.h>
-
-#include "c2.h"
-#include "c2_vq.h"
-#include "c2_status.h"
-
-#define C2_MAX_ORD_PER_QP 128
-#define C2_MAX_IRD_PER_QP 128
-
-#define C2_HINT_MAKE(q_index, hint_count) (((q_index) << 16) | hint_count)
-#define C2_HINT_GET_INDEX(hint) (((hint) & 0x7FFF0000) >> 16)
-#define C2_HINT_GET_COUNT(hint) ((hint) & 0x0000FFFF)
-
-#define NO_SUPPORT -1
-static const u8 c2_opcode[] = {
-	[IB_WR_SEND] = C2_WR_TYPE_SEND,
-	[IB_WR_SEND_WITH_IMM] = NO_SUPPORT,
-	[IB_WR_RDMA_WRITE] = C2_WR_TYPE_RDMA_WRITE,
-	[IB_WR_RDMA_WRITE_WITH_IMM] = NO_SUPPORT,
-	[IB_WR_RDMA_READ] = C2_WR_TYPE_RDMA_READ,
-	[IB_WR_ATOMIC_CMP_AND_SWP] = NO_SUPPORT,
-	[IB_WR_ATOMIC_FETCH_AND_ADD] = NO_SUPPORT,
-};
-
-static int to_c2_state(enum ib_qp_state ib_state)
-{
-	switch (ib_state) {
-	case IB_QPS_RESET:
-		return C2_QP_STATE_IDLE;
-	case IB_QPS_RTS:
-		return C2_QP_STATE_RTS;
-	case IB_QPS_SQD:
-		return C2_QP_STATE_CLOSING;
-	case IB_QPS_SQE:
-		return C2_QP_STATE_CLOSING;
-	case IB_QPS_ERR:
-		return C2_QP_STATE_ERROR;
-	default:
-		return -1;
-	}
-}
-
-static int to_ib_state(enum c2_qp_state c2_state)
-{
-	switch (c2_state) {
-	case C2_QP_STATE_IDLE:
-		return IB_QPS_RESET;
-	case C2_QP_STATE_CONNECTING:
-		return IB_QPS_RTR;
-	case C2_QP_STATE_RTS:
-		return IB_QPS_RTS;
-	case C2_QP_STATE_CLOSING:
-		return IB_QPS_SQD;
-	case C2_QP_STATE_ERROR:
-		return IB_QPS_ERR;
-	case C2_QP_STATE_TERMINATE:
-		return IB_QPS_SQE;
-	default:
-		return -1;
-	}
-}
-
-static const char *to_ib_state_str(int ib_state)
-{
-	static const char *state_str[] = {
-		"IB_QPS_RESET",
-		"IB_QPS_INIT",
-		"IB_QPS_RTR",
-		"IB_QPS_RTS",
-		"IB_QPS_SQD",
-		"IB_QPS_SQE",
-		"IB_QPS_ERR"
-	};
-	if (ib_state < IB_QPS_RESET ||
-	    ib_state > IB_QPS_ERR)
-		return "<invalid IB QP state>";
-
-	ib_state -= IB_QPS_RESET;
-	return state_str[ib_state];
-}
-
-void c2_set_qp_state(struct c2_qp *qp, int c2_state)
-{
-	int new_state = to_ib_state(c2_state);
-
-	pr_debug("%s: qp[%p] state modify %s --> %s\n",
-	       __func__,
-		qp,
-		to_ib_state_str(qp->state),
-		to_ib_state_str(new_state));
-	qp->state = new_state;
-}
-
-#define C2_QP_NO_ATTR_CHANGE 0xFFFFFFFF
-
-int c2_qp_modify(struct c2_dev *c2dev, struct c2_qp *qp,
-		 struct ib_qp_attr *attr, int attr_mask)
-{
-	struct c2wr_qp_modify_req wr;
-	struct c2wr_qp_modify_rep *reply;
-	struct c2_vq_req *vq_req;
-	unsigned long flags;
-	u8 next_state;
-	int err;
-
-	pr_debug("%s:%d qp=%p, %s --> %s\n",
-		__func__, __LINE__,
-		qp,
-		to_ib_state_str(qp->state),
-		to_ib_state_str(attr->qp_state));
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	c2_wr_set_id(&wr, CCWR_QP_MODIFY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.qp_handle = qp->adapter_handle;
-	wr.ord = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-	wr.ird = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-	wr.sq_depth = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-	wr.rq_depth = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-
-	if (attr_mask & IB_QP_STATE) {
-		/* Ensure the state is valid */
-		if (attr->qp_state < 0 || attr->qp_state > IB_QPS_ERR) {
-			err = -EINVAL;
-			goto bail0;
-		}
-
-		wr.next_qp_state = cpu_to_be32(to_c2_state(attr->qp_state));
-
-		if (attr->qp_state == IB_QPS_ERR) {
-			spin_lock_irqsave(&qp->lock, flags);
-			if (qp->cm_id && qp->state == IB_QPS_RTS) {
-				pr_debug("Generating CLOSE event for QP-->ERR, "
-					"qp=%p, cm_id=%p\n",qp,qp->cm_id);
-				/* Generate an CLOSE event */
-				vq_req->cm_id = qp->cm_id;
-				vq_req->event = IW_CM_EVENT_CLOSE;
-			}
-			spin_unlock_irqrestore(&qp->lock, flags);
-		}
-		next_state =  attr->qp_state;
-
-	} else if (attr_mask & IB_QP_CUR_STATE) {
-
-		if (attr->cur_qp_state != IB_QPS_RTR &&
-		    attr->cur_qp_state != IB_QPS_RTS &&
-		    attr->cur_qp_state != IB_QPS_SQD &&
-		    attr->cur_qp_state != IB_QPS_SQE) {
-			err = -EINVAL;
-			goto bail0;
-		} else
-			wr.next_qp_state =
-			    cpu_to_be32(to_c2_state(attr->cur_qp_state));
-
-		next_state = attr->cur_qp_state;
-
-	} else {
-		err = 0;
-		goto bail0;
-	}
-
-	/* reference the request struct */
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail0;
-
-	reply = (struct c2wr_qp_modify_rep *) (unsigned long) vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	err = c2_errno(reply);
-	if (!err)
-		qp->state = next_state;
-#ifdef DEBUG
-	else
-		pr_debug("%s: c2_errno=%d\n", __func__, err);
-#endif
-	/*
-	 * If we're going to error and generating the event here, then
-	 * we need to remove the reference because there will be no
-	 * close event generated by the adapter
-	*/
-	spin_lock_irqsave(&qp->lock, flags);
-	if (vq_req->event==IW_CM_EVENT_CLOSE && qp->cm_id) {
-		qp->cm_id->rem_ref(qp->cm_id);
-		qp->cm_id = NULL;
-	}
-	spin_unlock_irqrestore(&qp->lock, flags);
-
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-
-	pr_debug("%s:%d qp=%p, cur_state=%s\n",
-		__func__, __LINE__,
-		qp,
-		to_ib_state_str(qp->state));
-	return err;
-}
-
-int c2_qp_set_read_limits(struct c2_dev *c2dev, struct c2_qp *qp,
-			  int ord, int ird)
-{
-	struct c2wr_qp_modify_req wr;
-	struct c2wr_qp_modify_rep *reply;
-	struct c2_vq_req *vq_req;
-	int err;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	c2_wr_set_id(&wr, CCWR_QP_MODIFY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.qp_handle = qp->adapter_handle;
-	wr.ord = cpu_to_be32(ord);
-	wr.ird = cpu_to_be32(ird);
-	wr.sq_depth = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-	wr.rq_depth = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-	wr.next_qp_state = cpu_to_be32(C2_QP_NO_ATTR_CHANGE);
-
-	/* reference the request struct */
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail0;
-
-	reply = (struct c2wr_qp_modify_rep *) (unsigned long)
-		vq_req->reply_msg;
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	err = c2_errno(reply);
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-static int destroy_qp(struct c2_dev *c2dev, struct c2_qp *qp)
-{
-	struct c2_vq_req *vq_req;
-	struct c2wr_qp_destroy_req wr;
-	struct c2wr_qp_destroy_rep *reply;
-	unsigned long flags;
-	int err;
-
-	/*
-	 * Allocate a verb request message
-	 */
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req) {
-		return -ENOMEM;
-	}
-
-	/*
-	 * Initialize the WR
-	 */
-	c2_wr_set_id(&wr, CCWR_QP_DESTROY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.qp_handle = qp->adapter_handle;
-
-	/*
-	 * reference the request struct.  dereferenced in the int handler.
-	 */
-	vq_req_get(c2dev, vq_req);
-
-	spin_lock_irqsave(&qp->lock, flags);
-	if (qp->cm_id && qp->state == IB_QPS_RTS) {
-		pr_debug("destroy_qp: generating CLOSE event for QP-->ERR, "
-			"qp=%p, cm_id=%p\n",qp,qp->cm_id);
-		/* Generate an CLOSE event */
-		vq_req->qp = qp;
-		vq_req->cm_id = qp->cm_id;
-		vq_req->event = IW_CM_EVENT_CLOSE;
-	}
-	spin_unlock_irqrestore(&qp->lock, flags);
-
-	/*
-	 * Send WR to adapter
-	 */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	/*
-	 * Wait for reply from adapter
-	 */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail0;
-	}
-
-	/*
-	 * Process reply
-	 */
-	reply = (struct c2wr_qp_destroy_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	spin_lock_irqsave(&qp->lock, flags);
-	if (qp->cm_id) {
-		qp->cm_id->rem_ref(qp->cm_id);
-		qp->cm_id = NULL;
-	}
-	spin_unlock_irqrestore(&qp->lock, flags);
-
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-static int c2_alloc_qpn(struct c2_dev *c2dev, struct c2_qp *qp)
-{
-	int ret;
-
-	idr_preload(GFP_KERNEL);
-	spin_lock_irq(&c2dev->qp_table.lock);
-
-	ret = idr_alloc_cyclic(&c2dev->qp_table.idr, qp, 0, 0, GFP_NOWAIT);
-	if (ret >= 0)
-		qp->qpn = ret;
-
-	spin_unlock_irq(&c2dev->qp_table.lock);
-	idr_preload_end();
-	return ret < 0 ? ret : 0;
-}
-
-static void c2_free_qpn(struct c2_dev *c2dev, int qpn)
-{
-	spin_lock_irq(&c2dev->qp_table.lock);
-	idr_remove(&c2dev->qp_table.idr, qpn);
-	spin_unlock_irq(&c2dev->qp_table.lock);
-}
-
-struct c2_qp *c2_find_qpn(struct c2_dev *c2dev, int qpn)
-{
-	unsigned long flags;
-	struct c2_qp *qp;
-
-	spin_lock_irqsave(&c2dev->qp_table.lock, flags);
-	qp = idr_find(&c2dev->qp_table.idr, qpn);
-	spin_unlock_irqrestore(&c2dev->qp_table.lock, flags);
-	return qp;
-}
-
-int c2_alloc_qp(struct c2_dev *c2dev,
-		struct c2_pd *pd,
-		struct ib_qp_init_attr *qp_attrs, struct c2_qp *qp)
-{
-	struct c2wr_qp_create_req wr;
-	struct c2wr_qp_create_rep *reply;
-	struct c2_vq_req *vq_req;
-	struct c2_cq *send_cq = to_c2cq(qp_attrs->send_cq);
-	struct c2_cq *recv_cq = to_c2cq(qp_attrs->recv_cq);
-	unsigned long peer_pa;
-	u32 q_size, msg_size, mmap_size;
-	void __iomem *mmap;
-	int err;
-
-	err = c2_alloc_qpn(c2dev, qp);
-	if (err)
-		return err;
-	qp->ibqp.qp_num = qp->qpn;
-	qp->ibqp.qp_type = IB_QPT_RC;
-
-	/* Allocate the SQ and RQ shared pointers */
-	qp->sq_mq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					 &qp->sq_mq.shared_dma, GFP_KERNEL);
-	if (!qp->sq_mq.shared) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	qp->rq_mq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					 &qp->rq_mq.shared_dma, GFP_KERNEL);
-	if (!qp->rq_mq.shared) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	/* Allocate the verbs request */
-	vq_req = vq_req_alloc(c2dev);
-	if (vq_req == NULL) {
-		err = -ENOMEM;
-		goto bail2;
-	}
-
-	/* Initialize the work request */
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_QP_CREATE);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-	wr.sq_cq_handle = send_cq->adapter_handle;
-	wr.rq_cq_handle = recv_cq->adapter_handle;
-	wr.sq_depth = cpu_to_be32(qp_attrs->cap.max_send_wr + 1);
-	wr.rq_depth = cpu_to_be32(qp_attrs->cap.max_recv_wr + 1);
-	wr.srq_handle = 0;
-	wr.flags = cpu_to_be32(QP_RDMA_READ | QP_RDMA_WRITE | QP_MW_BIND |
-			       QP_ZERO_STAG | QP_RDMA_READ_RESPONSE);
-	wr.send_sgl_depth = cpu_to_be32(qp_attrs->cap.max_send_sge);
-	wr.recv_sgl_depth = cpu_to_be32(qp_attrs->cap.max_recv_sge);
-	wr.rdma_write_sgl_depth = cpu_to_be32(qp_attrs->cap.max_send_sge);
-	wr.shared_sq_ht = cpu_to_be64(qp->sq_mq.shared_dma);
-	wr.shared_rq_ht = cpu_to_be64(qp->rq_mq.shared_dma);
-	wr.ord = cpu_to_be32(C2_MAX_ORD_PER_QP);
-	wr.ird = cpu_to_be32(C2_MAX_IRD_PER_QP);
-	wr.pd_id = pd->pd_id;
-	wr.user_context = (unsigned long) qp;
-
-	vq_req_get(c2dev, vq_req);
-
-	/* Send the WR to the adapter */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail3;
-	}
-
-	/* Wait for the verb reply  */
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail3;
-	}
-
-	/* Process the reply */
-	reply = (struct c2wr_qp_create_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail3;
-	}
-
-	if ((err = c2_wr_get_result(reply)) != 0) {
-		goto bail4;
-	}
-
-	/* Fill in the kernel QP struct */
-	atomic_set(&qp->refcount, 1);
-	qp->adapter_handle = reply->qp_handle;
-	qp->state = IB_QPS_RESET;
-	qp->send_sgl_depth = qp_attrs->cap.max_send_sge;
-	qp->rdma_write_sgl_depth = qp_attrs->cap.max_send_sge;
-	qp->recv_sgl_depth = qp_attrs->cap.max_recv_sge;
-	init_waitqueue_head(&qp->wait);
-
-	/* Initialize the SQ MQ */
-	q_size = be32_to_cpu(reply->sq_depth);
-	msg_size = be32_to_cpu(reply->sq_msg_size);
-	peer_pa = c2dev->pa + be32_to_cpu(reply->sq_mq_start);
-	mmap_size = PAGE_ALIGN(sizeof(struct c2_mq_shared) + msg_size * q_size);
-	mmap = ioremap_nocache(peer_pa, mmap_size);
-	if (!mmap) {
-		err = -ENOMEM;
-		goto bail5;
-	}
-
-	c2_mq_req_init(&qp->sq_mq,
-		       be32_to_cpu(reply->sq_mq_index),
-		       q_size,
-		       msg_size,
-		       mmap + sizeof(struct c2_mq_shared),	/* pool start */
-		       mmap,				/* peer */
-		       C2_MQ_ADAPTER_TARGET);
-
-	/* Initialize the RQ mq */
-	q_size = be32_to_cpu(reply->rq_depth);
-	msg_size = be32_to_cpu(reply->rq_msg_size);
-	peer_pa = c2dev->pa + be32_to_cpu(reply->rq_mq_start);
-	mmap_size = PAGE_ALIGN(sizeof(struct c2_mq_shared) + msg_size * q_size);
-	mmap = ioremap_nocache(peer_pa, mmap_size);
-	if (!mmap) {
-		err = -ENOMEM;
-		goto bail6;
-	}
-
-	c2_mq_req_init(&qp->rq_mq,
-		       be32_to_cpu(reply->rq_mq_index),
-		       q_size,
-		       msg_size,
-		       mmap + sizeof(struct c2_mq_shared),	/* pool start */
-		       mmap,				/* peer */
-		       C2_MQ_ADAPTER_TARGET);
-
-	vq_repbuf_free(c2dev, reply);
-	vq_req_free(c2dev, vq_req);
-
-	return 0;
-
-bail6:
-	iounmap(qp->sq_mq.peer);
-bail5:
-	destroy_qp(c2dev, qp);
-bail4:
-	vq_repbuf_free(c2dev, reply);
-bail3:
-	vq_req_free(c2dev, vq_req);
-bail2:
-	c2_free_mqsp(qp->rq_mq.shared);
-bail1:
-	c2_free_mqsp(qp->sq_mq.shared);
-bail0:
-	c2_free_qpn(c2dev, qp->qpn);
-	return err;
-}
-
-static inline void c2_lock_cqs(struct c2_cq *send_cq, struct c2_cq *recv_cq)
-{
-	if (send_cq == recv_cq)
-		spin_lock_irq(&send_cq->lock);
-	else if (send_cq > recv_cq) {
-		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);
-	}
-}
-
-static inline void c2_unlock_cqs(struct c2_cq *send_cq, struct c2_cq *recv_cq)
-{
-	if (send_cq == recv_cq)
-		spin_unlock_irq(&send_cq->lock);
-	else if (send_cq > recv_cq) {
-		spin_unlock(&recv_cq->lock);
-		spin_unlock_irq(&send_cq->lock);
-	} else {
-		spin_unlock(&send_cq->lock);
-		spin_unlock_irq(&recv_cq->lock);
-	}
-}
-
-void c2_free_qp(struct c2_dev *c2dev, struct c2_qp *qp)
-{
-	struct c2_cq *send_cq;
-	struct c2_cq *recv_cq;
-
-	send_cq = to_c2cq(qp->ibqp.send_cq);
-	recv_cq = to_c2cq(qp->ibqp.recv_cq);
-
-	/*
-	 * Lock CQs here, so that CQ polling code can do QP lookup
-	 * without taking a lock.
-	 */
-	c2_lock_cqs(send_cq, recv_cq);
-	c2_free_qpn(c2dev, qp->qpn);
-	c2_unlock_cqs(send_cq, recv_cq);
-
-	/*
-	 * Destroy qp in the rnic...
-	 */
-	destroy_qp(c2dev, qp);
-
-	/*
-	 * Mark any unreaped CQEs as null and void.
-	 */
-	c2_cq_clean(c2dev, qp, send_cq->cqn);
-	if (send_cq != recv_cq)
-		c2_cq_clean(c2dev, qp, recv_cq->cqn);
-	/*
-	 * Unmap the MQs and return the shared pointers
-	 * to the message pool.
-	 */
-	iounmap(qp->sq_mq.peer);
-	iounmap(qp->rq_mq.peer);
-	c2_free_mqsp(qp->sq_mq.shared);
-	c2_free_mqsp(qp->rq_mq.shared);
-
-	atomic_dec(&qp->refcount);
-	wait_event(qp->wait, !atomic_read(&qp->refcount));
-}
-
-/*
- * Function: move_sgl
- *
- * Description:
- * Move an SGL from the user's work request struct into a CCIL Work Request
- * message, swapping to WR byte order and ensure the total length doesn't
- * overflow.
- *
- * IN:
- * dst		- ptr to CCIL Work Request message SGL memory.
- * src		- ptr to the consumers SGL memory.
- *
- * OUT: none
- *
- * Return:
- * CCIL status codes.
- */
-static int
-move_sgl(struct c2_data_addr * dst, struct ib_sge *src, int count, u32 * p_len,
-	 u8 * actual_count)
-{
-	u32 tot = 0;		/* running total */
-	u8 acount = 0;		/* running total non-0 len sge's */
-
-	while (count > 0) {
-		/*
-		 * If the addition of this SGE causes the
-		 * total SGL length to exceed 2^32-1, then
-		 * fail-n-bail.
-		 *
-		 * If the current total plus the next element length
-		 * wraps, then it will go negative and be less than the
-		 * current total...
-		 */
-		if ((tot + src->length) < tot) {
-			return -EINVAL;
-		}
-		/*
-		 * Bug: 1456 (as well as 1498 & 1643)
-		 * Skip over any sge's supplied with len=0
-		 */
-		if (src->length) {
-			tot += src->length;
-			dst->stag = cpu_to_be32(src->lkey);
-			dst->to = cpu_to_be64(src->addr);
-			dst->length = cpu_to_be32(src->length);
-			dst++;
-			acount++;
-		}
-		src++;
-		count--;
-	}
-
-	if (acount == 0) {
-		/*
-		 * Bug: 1476 (as well as 1498, 1456 and 1643)
-		 * Setup the SGL in the WR to make it easier for the RNIC.
-		 * This way, the FW doesn't have to deal with special cases.
-		 * Setting length=0 should be sufficient.
-		 */
-		dst->stag = 0;
-		dst->to = 0;
-		dst->length = 0;
-	}
-
-	*p_len = tot;
-	*actual_count = acount;
-	return 0;
-}
-
-/*
- * Function: c2_activity (private function)
- *
- * Description:
- * Post an mq index to the host->adapter activity fifo.
- *
- * IN:
- * c2dev	- ptr to c2dev structure
- * mq_index	- mq index to post
- * shared	- value most recently written to shared
- *
- * OUT:
- *
- * Return:
- * none
- */
-static inline void c2_activity(struct c2_dev *c2dev, u32 mq_index, u16 shared)
-{
-	/*
-	 * First read the register to see if the FIFO is full, and if so,
-	 * spin until it's not.  This isn't perfect -- there is no
-	 * synchronization among the clients of the register, but in
-	 * practice it prevents multiple CPU from hammering the bus
-	 * with PCI RETRY. Note that when this does happen, the card
-	 * cannot get on the bus and the card and system hang in a
-	 * deadlock -- thus the need for this code. [TOT]
-	 */
-	while (readl(c2dev->regs + PCI_BAR0_ADAPTER_HINT) & 0x80000000)
-		udelay(10);
-
-	__raw_writel(C2_HINT_MAKE(mq_index, shared),
-		     c2dev->regs + PCI_BAR0_ADAPTER_HINT);
-}
-
-/*
- * Function: qp_wr_post
- *
- * Description:
- * This in-line function allocates a MQ msg, then moves the host-copy of
- * the completed WR into msg.  Then it posts the message.
- *
- * IN:
- * q		- ptr to user MQ.
- * wr		- ptr to host-copy of the WR.
- * qp		- ptr to user qp
- * size		- Number of bytes to post.  Assumed to be divisible by 4.
- *
- * OUT: none
- *
- * Return:
- * CCIL status codes.
- */
-static int qp_wr_post(struct c2_mq *q, union c2wr * wr, struct c2_qp *qp, u32 size)
-{
-	union c2wr *msg;
-
-	msg = c2_mq_alloc(q);
-	if (msg == NULL) {
-		return -EINVAL;
-	}
-#ifdef CCMSGMAGIC
-	((c2wr_hdr_t *) wr)->magic = cpu_to_be32(CCWR_MAGIC);
-#endif
-
-	/*
-	 * Since all header fields in the WR are the same as the
-	 * CQE, set the following so the adapter need not.
-	 */
-	c2_wr_set_result(wr, CCERR_PENDING);
-
-	/*
-	 * Copy the wr down to the adapter
-	 */
-	memcpy((void *) msg, (void *) wr, size);
-
-	c2_mq_produce(q);
-	return 0;
-}
-
-
-int c2_post_send(struct ib_qp *ibqp, struct ib_send_wr *ib_wr,
-		 struct ib_send_wr **bad_wr)
-{
-	struct c2_dev *c2dev = to_c2dev(ibqp->device);
-	struct c2_qp *qp = to_c2qp(ibqp);
-	union c2wr wr;
-	unsigned long lock_flags;
-	int err = 0;
-
-	u32 flags;
-	u32 tot_len;
-	u8 actual_sge_count;
-	u32 msg_size;
-
-	if (qp->state > IB_QPS_RTS) {
-		err = -EINVAL;
-		goto out;
-	}
-
-	while (ib_wr) {
-
-		flags = 0;
-		wr.sqwr.sq_hdr.user_hdr.hdr.context = ib_wr->wr_id;
-		if (ib_wr->send_flags & IB_SEND_SIGNALED) {
-			flags |= SQ_SIGNALED;
-		}
-
-		switch (ib_wr->opcode) {
-		case IB_WR_SEND:
-		case IB_WR_SEND_WITH_INV:
-			if (ib_wr->opcode == IB_WR_SEND) {
-				if (ib_wr->send_flags & IB_SEND_SOLICITED)
-					c2_wr_set_id(&wr, C2_WR_TYPE_SEND_SE);
-				else
-					c2_wr_set_id(&wr, C2_WR_TYPE_SEND);
-				wr.sqwr.send.remote_stag = 0;
-			} else {
-				if (ib_wr->send_flags & IB_SEND_SOLICITED)
-					c2_wr_set_id(&wr, C2_WR_TYPE_SEND_SE_INV);
-				else
-					c2_wr_set_id(&wr, C2_WR_TYPE_SEND_INV);
-				wr.sqwr.send.remote_stag =
-					cpu_to_be32(ib_wr->ex.invalidate_rkey);
-			}
-
-			msg_size = sizeof(struct c2wr_send_req) +
-				sizeof(struct c2_data_addr) * ib_wr->num_sge;
-			if (ib_wr->num_sge > qp->send_sgl_depth) {
-				err = -EINVAL;
-				break;
-			}
-			if (ib_wr->send_flags & IB_SEND_FENCE) {
-				flags |= SQ_READ_FENCE;
-			}
-			err = move_sgl((struct c2_data_addr *) & (wr.sqwr.send.data),
-				       ib_wr->sg_list,
-				       ib_wr->num_sge,
-				       &tot_len, &actual_sge_count);
-			wr.sqwr.send.sge_len = cpu_to_be32(tot_len);
-			c2_wr_set_sge_count(&wr, actual_sge_count);
-			break;
-		case IB_WR_RDMA_WRITE:
-			c2_wr_set_id(&wr, C2_WR_TYPE_RDMA_WRITE);
-			msg_size = sizeof(struct c2wr_rdma_write_req) +
-			    (sizeof(struct c2_data_addr) * ib_wr->num_sge);
-			if (ib_wr->num_sge > qp->rdma_write_sgl_depth) {
-				err = -EINVAL;
-				break;
-			}
-			if (ib_wr->send_flags & IB_SEND_FENCE) {
-				flags |= SQ_READ_FENCE;
-			}
-			wr.sqwr.rdma_write.remote_stag =
-			    cpu_to_be32(rdma_wr(ib_wr)->rkey);
-			wr.sqwr.rdma_write.remote_to =
-			    cpu_to_be64(rdma_wr(ib_wr)->remote_addr);
-			err = move_sgl((struct c2_data_addr *)
-				       & (wr.sqwr.rdma_write.data),
-				       ib_wr->sg_list,
-				       ib_wr->num_sge,
-				       &tot_len, &actual_sge_count);
-			wr.sqwr.rdma_write.sge_len = cpu_to_be32(tot_len);
-			c2_wr_set_sge_count(&wr, actual_sge_count);
-			break;
-		case IB_WR_RDMA_READ:
-			c2_wr_set_id(&wr, C2_WR_TYPE_RDMA_READ);
-			msg_size = sizeof(struct c2wr_rdma_read_req);
-
-			/* IWarp only suppots 1 sge for RDMA reads */
-			if (ib_wr->num_sge > 1) {
-				err = -EINVAL;
-				break;
-			}
-
-			/*
-			 * Move the local and remote stag/to/len into the WR.
-			 */
-			wr.sqwr.rdma_read.local_stag =
-			    cpu_to_be32(ib_wr->sg_list->lkey);
-			wr.sqwr.rdma_read.local_to =
-			    cpu_to_be64(ib_wr->sg_list->addr);
-			wr.sqwr.rdma_read.remote_stag =
-			    cpu_to_be32(rdma_wr(ib_wr)->rkey);
-			wr.sqwr.rdma_read.remote_to =
-			    cpu_to_be64(rdma_wr(ib_wr)->remote_addr);
-			wr.sqwr.rdma_read.length =
-			    cpu_to_be32(ib_wr->sg_list->length);
-			break;
-		default:
-			/* error */
-			msg_size = 0;
-			err = -EINVAL;
-			break;
-		}
-
-		/*
-		 * If we had an error on the last wr build, then
-		 * break out.  Possible errors include bogus WR
-		 * type, and a bogus SGL length...
-		 */
-		if (err) {
-			break;
-		}
-
-		/*
-		 * Store flags
-		 */
-		c2_wr_set_flags(&wr, flags);
-
-		/*
-		 * Post the puppy!
-		 */
-		spin_lock_irqsave(&qp->lock, lock_flags);
-		err = qp_wr_post(&qp->sq_mq, &wr, qp, msg_size);
-		if (err) {
-			spin_unlock_irqrestore(&qp->lock, lock_flags);
-			break;
-		}
-
-		/*
-		 * Enqueue mq index to activity FIFO.
-		 */
-		c2_activity(c2dev, qp->sq_mq.index, qp->sq_mq.hint_count);
-		spin_unlock_irqrestore(&qp->lock, lock_flags);
-
-		ib_wr = ib_wr->next;
-	}
-
-out:
-	if (err)
-		*bad_wr = ib_wr;
-	return err;
-}
-
-int c2_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *ib_wr,
-		    struct ib_recv_wr **bad_wr)
-{
-	struct c2_dev *c2dev = to_c2dev(ibqp->device);
-	struct c2_qp *qp = to_c2qp(ibqp);
-	union c2wr wr;
-	unsigned long lock_flags;
-	int err = 0;
-
-	if (qp->state > IB_QPS_RTS) {
-		err = -EINVAL;
-		goto out;
-	}
-
-	/*
-	 * Try and post each work request
-	 */
-	while (ib_wr) {
-		u32 tot_len;
-		u8 actual_sge_count;
-
-		if (ib_wr->num_sge > qp->recv_sgl_depth) {
-			err = -EINVAL;
-			break;
-		}
-
-		/*
-		 * Create local host-copy of the WR
-		 */
-		wr.rqwr.rq_hdr.user_hdr.hdr.context = ib_wr->wr_id;
-		c2_wr_set_id(&wr, CCWR_RECV);
-		c2_wr_set_flags(&wr, 0);
-
-		/* sge_count is limited to eight bits. */
-		BUG_ON(ib_wr->num_sge >= 256);
-		err = move_sgl((struct c2_data_addr *) & (wr.rqwr.data),
-			       ib_wr->sg_list,
-			       ib_wr->num_sge, &tot_len, &actual_sge_count);
-		c2_wr_set_sge_count(&wr, actual_sge_count);
-
-		/*
-		 * If we had an error on the last wr build, then
-		 * break out.  Possible errors include bogus WR
-		 * type, and a bogus SGL length...
-		 */
-		if (err) {
-			break;
-		}
-
-		spin_lock_irqsave(&qp->lock, lock_flags);
-		err = qp_wr_post(&qp->rq_mq, &wr, qp, qp->rq_mq.msg_size);
-		if (err) {
-			spin_unlock_irqrestore(&qp->lock, lock_flags);
-			break;
-		}
-
-		/*
-		 * Enqueue mq index to activity FIFO
-		 */
-		c2_activity(c2dev, qp->rq_mq.index, qp->rq_mq.hint_count);
-		spin_unlock_irqrestore(&qp->lock, lock_flags);
-
-		ib_wr = ib_wr->next;
-	}
-
-out:
-	if (err)
-		*bad_wr = ib_wr;
-	return err;
-}
-
-void c2_init_qp_table(struct c2_dev *c2dev)
-{
-	spin_lock_init(&c2dev->qp_table.lock);
-	idr_init(&c2dev->qp_table.idr);
-}
-
-void c2_cleanup_qp_table(struct c2_dev *c2dev)
-{
-	idr_destroy(&c2dev->qp_table.idr);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_rnic.c b/drivers/staging/rdma/amso1100/c2_rnic.c
deleted file mode 100644
index d3c0f77767d9..000000000000
--- a/drivers/staging/rdma/amso1100/c2_rnic.c
+++ /dev/null
@@ -1,655 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- *
- */
-
-
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/pci.h>
-#include <linux/netdevice.h>
-#include <linux/etherdevice.h>
-#include <linux/delay.h>
-#include <linux/ethtool.h>
-#include <linux/mii.h>
-#include <linux/if_vlan.h>
-#include <linux/crc32.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/tcp.h>
-#include <linux/init.h>
-#include <linux/dma-mapping.h>
-#include <linux/mm.h>
-#include <linux/inet.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-
-#include <linux/route.h>
-
-#include <asm/io.h>
-#include <asm/irq.h>
-#include <asm/byteorder.h>
-#include <rdma/ib_smi.h>
-#include "c2.h"
-#include "c2_vq.h"
-
-/* Device capabilities */
-#define C2_MIN_PAGESIZE  1024
-
-#define C2_MAX_MRS       32768
-#define C2_MAX_QPS       16000
-#define C2_MAX_WQE_SZ    256
-#define C2_MAX_QP_WR     ((128*1024)/C2_MAX_WQE_SZ)
-#define C2_MAX_SGES      4
-#define C2_MAX_SGE_RD    1
-#define C2_MAX_CQS       32768
-#define C2_MAX_CQES      4096
-#define C2_MAX_PDS       16384
-
-/*
- * Send the adapter INIT message to the amso1100
- */
-static int c2_adapter_init(struct c2_dev *c2dev)
-{
-	struct c2wr_init_req wr;
-	int err;
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_INIT);
-	wr.hdr.context = 0;
-	wr.hint_count = cpu_to_be64(c2dev->hint_count_dma);
-	wr.q0_host_shared = cpu_to_be64(c2dev->req_vq.shared_dma);
-	wr.q1_host_shared = cpu_to_be64(c2dev->rep_vq.shared_dma);
-	wr.q1_host_msg_pool = cpu_to_be64(c2dev->rep_vq.host_dma);
-	wr.q2_host_shared = cpu_to_be64(c2dev->aeq.shared_dma);
-	wr.q2_host_msg_pool = cpu_to_be64(c2dev->aeq.host_dma);
-
-	/* Post the init message */
-	err = vq_send_wr(c2dev, (union c2wr *) & wr);
-
-	return err;
-}
-
-/*
- * Send the adapter TERM message to the amso1100
- */
-static void c2_adapter_term(struct c2_dev *c2dev)
-{
-	struct c2wr_init_req wr;
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_TERM);
-	wr.hdr.context = 0;
-
-	/* Post the init message */
-	vq_send_wr(c2dev, (union c2wr *) & wr);
-	c2dev->init = 0;
-
-	return;
-}
-
-/*
- * Query the adapter
- */
-static int c2_rnic_query(struct c2_dev *c2dev, struct ib_device_attr *props)
-{
-	struct c2_vq_req *vq_req;
-	struct c2wr_rnic_query_req wr;
-	struct c2wr_rnic_query_rep *reply;
-	int err;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	c2_wr_set_id(&wr, CCWR_RNIC_QUERY);
-	wr.hdr.context = (unsigned long) vq_req;
-	wr.rnic_handle = c2dev->adapter_handle;
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) &wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail1;
-
-	reply =
-	    (struct c2wr_rnic_query_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply)
-		err = -ENOMEM;
-	else
-		err = c2_errno(reply);
-	if (err)
-		goto bail2;
-
-	props->fw_ver =
-		((u64)be32_to_cpu(reply->fw_ver_major) << 32) |
-		((be32_to_cpu(reply->fw_ver_minor) & 0xFFFF) << 16) |
-		(be32_to_cpu(reply->fw_ver_patch) & 0xFFFF);
-	memcpy(&props->sys_image_guid, c2dev->netdev->dev_addr, 6);
-	props->max_mr_size         = 0xFFFFFFFF;
-	props->page_size_cap       = ~(C2_MIN_PAGESIZE-1);
-	props->vendor_id           = be32_to_cpu(reply->vendor_id);
-	props->vendor_part_id      = be32_to_cpu(reply->part_number);
-	props->hw_ver              = be32_to_cpu(reply->hw_version);
-	props->max_qp              = be32_to_cpu(reply->max_qps);
-	props->max_qp_wr           = be32_to_cpu(reply->max_qp_depth);
-	props->device_cap_flags    = c2dev->device_cap_flags;
-	props->max_sge             = C2_MAX_SGES;
-	props->max_sge_rd          = C2_MAX_SGE_RD;
-	props->max_cq              = be32_to_cpu(reply->max_cqs);
-	props->max_cqe             = be32_to_cpu(reply->max_cq_depth);
-	props->max_mr              = be32_to_cpu(reply->max_mrs);
-	props->max_pd              = be32_to_cpu(reply->max_pds);
-	props->max_qp_rd_atom      = be32_to_cpu(reply->max_qp_ird);
-	props->max_ee_rd_atom      = 0;
-	props->max_res_rd_atom     = be32_to_cpu(reply->max_global_ird);
-	props->max_qp_init_rd_atom = be32_to_cpu(reply->max_qp_ord);
-	props->max_ee_init_rd_atom = 0;
-	props->atomic_cap          = IB_ATOMIC_NONE;
-	props->max_ee              = 0;
-	props->max_rdd             = 0;
-	props->max_mw              = be32_to_cpu(reply->max_mws);
-	props->max_raw_ipv6_qp     = 0;
-	props->max_raw_ethy_qp     = 0;
-	props->max_mcast_grp       = 0;
-	props->max_mcast_qp_attach = 0;
-	props->max_total_mcast_qp_attach = 0;
-	props->max_ah              = 0;
-	props->max_fmr             = 0;
-	props->max_map_per_fmr     = 0;
-	props->max_srq             = 0;
-	props->max_srq_wr          = 0;
-	props->max_srq_sge         = 0;
-	props->max_pkeys           = 0;
-	props->local_ca_ack_delay  = 0;
-
- bail2:
-	vq_repbuf_free(c2dev, reply);
-
- bail1:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-/*
- * Add an IP address to the RNIC interface
- */
-int c2_add_addr(struct c2_dev *c2dev, __be32 inaddr, __be32 inmask)
-{
-	struct c2_vq_req *vq_req;
-	struct c2wr_rnic_setconfig_req *wr;
-	struct c2wr_rnic_setconfig_rep *reply;
-	struct c2_netaddr netaddr;
-	int err, len;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	len = sizeof(struct c2_netaddr);
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	c2_wr_set_id(wr, CCWR_RNIC_SETCONFIG);
-	wr->hdr.context = (unsigned long) vq_req;
-	wr->rnic_handle = c2dev->adapter_handle;
-	wr->option = cpu_to_be32(C2_CFG_ADD_ADDR);
-
-	netaddr.ip_addr = inaddr;
-	netaddr.netmask = inmask;
-	netaddr.mtu = 0;
-
-	memcpy(wr->data, &netaddr, len);
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail1;
-
-	reply =
-	    (struct c2wr_rnic_setconfig_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	err = c2_errno(reply);
-	vq_repbuf_free(c2dev, reply);
-
-bail1:
-	kfree(wr);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-/*
- * Delete an IP address from the RNIC interface
- */
-int c2_del_addr(struct c2_dev *c2dev, __be32 inaddr, __be32 inmask)
-{
-	struct c2_vq_req *vq_req;
-	struct c2wr_rnic_setconfig_req *wr;
-	struct c2wr_rnic_setconfig_rep *reply;
-	struct c2_netaddr netaddr;
-	int err, len;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (!vq_req)
-		return -ENOMEM;
-
-	len = sizeof(struct c2_netaddr);
-	wr = kmalloc(c2dev->req_vq.msg_size, GFP_KERNEL);
-	if (!wr) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	c2_wr_set_id(wr, CCWR_RNIC_SETCONFIG);
-	wr->hdr.context = (unsigned long) vq_req;
-	wr->rnic_handle = c2dev->adapter_handle;
-	wr->option = cpu_to_be32(C2_CFG_DEL_ADDR);
-
-	netaddr.ip_addr = inaddr;
-	netaddr.netmask = inmask;
-	netaddr.mtu = 0;
-
-	memcpy(wr->data, &netaddr, len);
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, (union c2wr *) wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail1;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err)
-		goto bail1;
-
-	reply =
-	    (struct c2wr_rnic_setconfig_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	err = c2_errno(reply);
-	vq_repbuf_free(c2dev, reply);
-
-bail1:
-	kfree(wr);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-/*
- * Open a single RNIC instance to use with all
- * low level openib calls
- */
-static int c2_rnic_open(struct c2_dev *c2dev)
-{
-	struct c2_vq_req *vq_req;
-	union c2wr wr;
-	struct c2wr_rnic_open_rep *reply;
-	int err;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (vq_req == NULL) {
-		return -ENOMEM;
-	}
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_RNIC_OPEN);
-	wr.rnic_open.req.hdr.context = (unsigned long) (vq_req);
-	wr.rnic_open.req.flags = cpu_to_be16(RNIC_PRIV_MODE);
-	wr.rnic_open.req.port_num = cpu_to_be16(0);
-	wr.rnic_open.req.user_context = (unsigned long) c2dev;
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, &wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail0;
-	}
-
-	reply = (struct c2wr_rnic_open_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	if ((err = c2_errno(reply)) != 0) {
-		goto bail1;
-	}
-
-	c2dev->adapter_handle = reply->rnic_handle;
-
-bail1:
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-/*
- * Close the RNIC instance
- */
-static int c2_rnic_close(struct c2_dev *c2dev)
-{
-	struct c2_vq_req *vq_req;
-	union c2wr wr;
-	struct c2wr_rnic_close_rep *reply;
-	int err;
-
-	vq_req = vq_req_alloc(c2dev);
-	if (vq_req == NULL) {
-		return -ENOMEM;
-	}
-
-	memset(&wr, 0, sizeof(wr));
-	c2_wr_set_id(&wr, CCWR_RNIC_CLOSE);
-	wr.rnic_close.req.hdr.context = (unsigned long) vq_req;
-	wr.rnic_close.req.rnic_handle = c2dev->adapter_handle;
-
-	vq_req_get(c2dev, vq_req);
-
-	err = vq_send_wr(c2dev, &wr);
-	if (err) {
-		vq_req_put(c2dev, vq_req);
-		goto bail0;
-	}
-
-	err = vq_wait_for_reply(c2dev, vq_req);
-	if (err) {
-		goto bail0;
-	}
-
-	reply = (struct c2wr_rnic_close_rep *) (unsigned long) (vq_req->reply_msg);
-	if (!reply) {
-		err = -ENOMEM;
-		goto bail0;
-	}
-
-	if ((err = c2_errno(reply)) != 0) {
-		goto bail1;
-	}
-
-	c2dev->adapter_handle = 0;
-
-bail1:
-	vq_repbuf_free(c2dev, reply);
-bail0:
-	vq_req_free(c2dev, vq_req);
-	return err;
-}
-
-/*
- * Called by c2_probe to initialize the RNIC. This principally
- * involves initializing the various limits and resource pools that
- * comprise the RNIC instance.
- */
-int c2_rnic_init(struct c2_dev *c2dev)
-{
-	int err;
-	u32 qsize, msgsize;
-	void *q1_pages;
-	void *q2_pages;
-	void __iomem *mmio_regs;
-
-	/* Device capabilities */
-	c2dev->device_cap_flags =
-	    (IB_DEVICE_RESIZE_MAX_WR |
-	     IB_DEVICE_CURR_QP_STATE_MOD |
-	     IB_DEVICE_SYS_IMAGE_GUID |
-	     IB_DEVICE_LOCAL_DMA_LKEY |
-	     IB_DEVICE_MEM_WINDOW);
-
-	/* Allocate the qptr_array */
-	c2dev->qptr_array = vzalloc(C2_MAX_CQS * sizeof(void *));
-	if (!c2dev->qptr_array) {
-		return -ENOMEM;
-	}
-
-	/* Initialize the qptr_array */
-	c2dev->qptr_array[0] = (void *) &c2dev->req_vq;
-	c2dev->qptr_array[1] = (void *) &c2dev->rep_vq;
-	c2dev->qptr_array[2] = (void *) &c2dev->aeq;
-
-	/* Initialize data structures */
-	init_waitqueue_head(&c2dev->req_vq_wo);
-	spin_lock_init(&c2dev->vqlock);
-	spin_lock_init(&c2dev->lock);
-
-	/* Allocate MQ shared pointer pool for kernel clients. User
-	 * mode client pools are hung off the user context
-	 */
-	err = c2_init_mqsp_pool(c2dev, GFP_KERNEL, &c2dev->kern_mqsp_pool);
-	if (err) {
-		goto bail0;
-	}
-
-	/* Allocate shared pointers for Q0, Q1, and Q2 from
-	 * the shared pointer pool.
-	 */
-
-	c2dev->hint_count = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					     &c2dev->hint_count_dma,
-					     GFP_KERNEL);
-	c2dev->req_vq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					     &c2dev->req_vq.shared_dma,
-					     GFP_KERNEL);
-	c2dev->rep_vq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					     &c2dev->rep_vq.shared_dma,
-					     GFP_KERNEL);
-	c2dev->aeq.shared = c2_alloc_mqsp(c2dev, c2dev->kern_mqsp_pool,
-					  &c2dev->aeq.shared_dma, GFP_KERNEL);
-	if (!c2dev->hint_count || !c2dev->req_vq.shared ||
-	    !c2dev->rep_vq.shared || !c2dev->aeq.shared) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-
-	mmio_regs = c2dev->kva;
-	/* Initialize the Verbs Request Queue */
-	c2_mq_req_init(&c2dev->req_vq, 0,
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q0_QSIZE)),
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q0_MSGSIZE)),
-		       mmio_regs +
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q0_POOLSTART)),
-		       mmio_regs +
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q0_SHARED)),
-		       C2_MQ_ADAPTER_TARGET);
-
-	/* Initialize the Verbs Reply Queue */
-	qsize = be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q1_QSIZE));
-	msgsize = be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q1_MSGSIZE));
-	q1_pages = dma_alloc_coherent(&c2dev->pcidev->dev, qsize * msgsize,
-				      &c2dev->rep_vq.host_dma, GFP_KERNEL);
-	if (!q1_pages) {
-		err = -ENOMEM;
-		goto bail1;
-	}
-	dma_unmap_addr_set(&c2dev->rep_vq, mapping, c2dev->rep_vq.host_dma);
-	pr_debug("%s rep_vq va %p dma %llx\n", __func__, q1_pages,
-		 (unsigned long long) c2dev->rep_vq.host_dma);
-	c2_mq_rep_init(&c2dev->rep_vq,
-		   1,
-		   qsize,
-		   msgsize,
-		   q1_pages,
-		   mmio_regs +
-		   be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q1_SHARED)),
-		   C2_MQ_HOST_TARGET);
-
-	/* Initialize the Asynchronus Event Queue */
-	qsize = be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q2_QSIZE));
-	msgsize = be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q2_MSGSIZE));
-	q2_pages = dma_alloc_coherent(&c2dev->pcidev->dev, qsize * msgsize,
-				      &c2dev->aeq.host_dma, GFP_KERNEL);
-	if (!q2_pages) {
-		err = -ENOMEM;
-		goto bail2;
-	}
-	dma_unmap_addr_set(&c2dev->aeq, mapping, c2dev->aeq.host_dma);
-	pr_debug("%s aeq va %p dma %llx\n", __func__, q2_pages,
-		 (unsigned long long) c2dev->aeq.host_dma);
-	c2_mq_rep_init(&c2dev->aeq,
-		       2,
-		       qsize,
-		       msgsize,
-		       q2_pages,
-		       mmio_regs +
-		       be32_to_cpu((__force __be32) readl(mmio_regs + C2_REGS_Q2_SHARED)),
-		       C2_MQ_HOST_TARGET);
-
-	/* Initialize the verbs request allocator */
-	err = vq_init(c2dev);
-	if (err)
-		goto bail3;
-
-	/* Enable interrupts on the adapter */
-	writel(0, c2dev->regs + C2_IDIS);
-
-	/* create the WR init message */
-	err = c2_adapter_init(c2dev);
-	if (err)
-		goto bail4;
-	c2dev->init++;
-
-	/* open an adapter instance */
-	err = c2_rnic_open(c2dev);
-	if (err)
-		goto bail4;
-
-	/* Initialize cached the adapter limits */
-	err = c2_rnic_query(c2dev, &c2dev->props);
-	if (err)
-		goto bail5;
-
-	/* Initialize the PD pool */
-	err = c2_init_pd_table(c2dev);
-	if (err)
-		goto bail5;
-
-	/* Initialize the QP pool */
-	c2_init_qp_table(c2dev);
-	return 0;
-
-bail5:
-	c2_rnic_close(c2dev);
-bail4:
-	vq_term(c2dev);
-bail3:
-	dma_free_coherent(&c2dev->pcidev->dev,
-			  c2dev->aeq.q_size * c2dev->aeq.msg_size,
-			  q2_pages, dma_unmap_addr(&c2dev->aeq, mapping));
-bail2:
-	dma_free_coherent(&c2dev->pcidev->dev,
-			  c2dev->rep_vq.q_size * c2dev->rep_vq.msg_size,
-			  q1_pages, dma_unmap_addr(&c2dev->rep_vq, mapping));
-bail1:
-	c2_free_mqsp_pool(c2dev, c2dev->kern_mqsp_pool);
-bail0:
-	vfree(c2dev->qptr_array);
-
-	return err;
-}
-
-/*
- * Called by c2_remove to cleanup the RNIC resources.
- */
-void c2_rnic_term(struct c2_dev *c2dev)
-{
-
-	/* Close the open adapter instance */
-	c2_rnic_close(c2dev);
-
-	/* Send the TERM message to the adapter */
-	c2_adapter_term(c2dev);
-
-	/* Disable interrupts on the adapter */
-	writel(1, c2dev->regs + C2_IDIS);
-
-	/* Free the QP pool */
-	c2_cleanup_qp_table(c2dev);
-
-	/* Free the PD pool */
-	c2_cleanup_pd_table(c2dev);
-
-	/* Free the verbs request allocator */
-	vq_term(c2dev);
-
-	/* Free the asynchronus event queue */
-	dma_free_coherent(&c2dev->pcidev->dev,
-			  c2dev->aeq.q_size * c2dev->aeq.msg_size,
-			  c2dev->aeq.msg_pool.host,
-			  dma_unmap_addr(&c2dev->aeq, mapping));
-
-	/* Free the verbs reply queue */
-	dma_free_coherent(&c2dev->pcidev->dev,
-			  c2dev->rep_vq.q_size * c2dev->rep_vq.msg_size,
-			  c2dev->rep_vq.msg_pool.host,
-			  dma_unmap_addr(&c2dev->rep_vq, mapping));
-
-	/* Free the MQ shared pointer pool */
-	c2_free_mqsp_pool(c2dev, c2dev->kern_mqsp_pool);
-
-	/* Free the qptr_array */
-	vfree(c2dev->qptr_array);
-
-	return;
-}
diff --git a/drivers/staging/rdma/amso1100/c2_status.h b/drivers/staging/rdma/amso1100/c2_status.h
deleted file mode 100644
index 6ee4aa92d875..000000000000
--- a/drivers/staging/rdma/amso1100/c2_status.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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	_C2_STATUS_H_
-#define _C2_STATUS_H_
-
-/*
- * Verbs Status Codes
- */
-enum c2_status {
-	C2_OK = 0,		/* This must be zero */
-	CCERR_INSUFFICIENT_RESOURCES = 1,
-	CCERR_INVALID_MODIFIER = 2,
-	CCERR_INVALID_MODE = 3,
-	CCERR_IN_USE = 4,
-	CCERR_INVALID_RNIC = 5,
-	CCERR_INTERRUPTED_OPERATION = 6,
-	CCERR_INVALID_EH = 7,
-	CCERR_INVALID_CQ = 8,
-	CCERR_CQ_EMPTY = 9,
-	CCERR_NOT_IMPLEMENTED = 10,
-	CCERR_CQ_DEPTH_TOO_SMALL = 11,
-	CCERR_PD_IN_USE = 12,
-	CCERR_INVALID_PD = 13,
-	CCERR_INVALID_SRQ = 14,
-	CCERR_INVALID_ADDRESS = 15,
-	CCERR_INVALID_NETMASK = 16,
-	CCERR_INVALID_QP = 17,
-	CCERR_INVALID_QP_STATE = 18,
-	CCERR_TOO_MANY_WRS_POSTED = 19,
-	CCERR_INVALID_WR_TYPE = 20,
-	CCERR_INVALID_SGL_LENGTH = 21,
-	CCERR_INVALID_SQ_DEPTH = 22,
-	CCERR_INVALID_RQ_DEPTH = 23,
-	CCERR_INVALID_ORD = 24,
-	CCERR_INVALID_IRD = 25,
-	CCERR_QP_ATTR_CANNOT_CHANGE = 26,
-	CCERR_INVALID_STAG = 27,
-	CCERR_QP_IN_USE = 28,
-	CCERR_OUTSTANDING_WRS = 29,
-	CCERR_STAG_IN_USE = 30,
-	CCERR_INVALID_STAG_INDEX = 31,
-	CCERR_INVALID_SGL_FORMAT = 32,
-	CCERR_ADAPTER_TIMEOUT = 33,
-	CCERR_INVALID_CQ_DEPTH = 34,
-	CCERR_INVALID_PRIVATE_DATA_LENGTH = 35,
-	CCERR_INVALID_EP = 36,
-	CCERR_MR_IN_USE = CCERR_STAG_IN_USE,
-	CCERR_FLUSHED = 38,
-	CCERR_INVALID_WQE = 39,
-	CCERR_LOCAL_QP_CATASTROPHIC_ERROR = 40,
-	CCERR_REMOTE_TERMINATION_ERROR = 41,
-	CCERR_BASE_AND_BOUNDS_VIOLATION = 42,
-	CCERR_ACCESS_VIOLATION = 43,
-	CCERR_INVALID_PD_ID = 44,
-	CCERR_WRAP_ERROR = 45,
-	CCERR_INV_STAG_ACCESS_ERROR = 46,
-	CCERR_ZERO_RDMA_READ_RESOURCES = 47,
-	CCERR_QP_NOT_PRIVILEGED = 48,
-	CCERR_STAG_STATE_NOT_INVALID = 49,
-	CCERR_INVALID_PAGE_SIZE = 50,
-	CCERR_INVALID_BUFFER_SIZE = 51,
-	CCERR_INVALID_PBE = 52,
-	CCERR_INVALID_FBO = 53,
-	CCERR_INVALID_LENGTH = 54,
-	CCERR_INVALID_ACCESS_RIGHTS = 55,
-	CCERR_PBL_TOO_BIG = 56,
-	CCERR_INVALID_VA = 57,
-	CCERR_INVALID_REGION = 58,
-	CCERR_INVALID_WINDOW = 59,
-	CCERR_TOTAL_LENGTH_TOO_BIG = 60,
-	CCERR_INVALID_QP_ID = 61,
-	CCERR_ADDR_IN_USE = 62,
-	CCERR_ADDR_NOT_AVAIL = 63,
-	CCERR_NET_DOWN = 64,
-	CCERR_NET_UNREACHABLE = 65,
-	CCERR_CONN_ABORTED = 66,
-	CCERR_CONN_RESET = 67,
-	CCERR_NO_BUFS = 68,
-	CCERR_CONN_TIMEDOUT = 69,
-	CCERR_CONN_REFUSED = 70,
-	CCERR_HOST_UNREACHABLE = 71,
-	CCERR_INVALID_SEND_SGL_DEPTH = 72,
-	CCERR_INVALID_RECV_SGL_DEPTH = 73,
-	CCERR_INVALID_RDMA_WRITE_SGL_DEPTH = 74,
-	CCERR_INSUFFICIENT_PRIVILEGES = 75,
-	CCERR_STACK_ERROR = 76,
-	CCERR_INVALID_VERSION = 77,
-	CCERR_INVALID_MTU = 78,
-	CCERR_INVALID_IMAGE = 79,
-	CCERR_PENDING = 98,	/* not an error; user internally by adapter */
-	CCERR_DEFER = 99,	/* not an error; used internally by adapter */
-	CCERR_FAILED_WRITE = 100,
-	CCERR_FAILED_ERASE = 101,
-	CCERR_FAILED_VERIFICATION = 102,
-	CCERR_NOT_FOUND = 103,
-
-};
-
-/*
- * CCAE_ACTIVE_CONNECT_RESULTS status result codes.
- */
-enum c2_connect_status {
-	C2_CONN_STATUS_SUCCESS = C2_OK,
-	C2_CONN_STATUS_NO_MEM = CCERR_INSUFFICIENT_RESOURCES,
-	C2_CONN_STATUS_TIMEDOUT = CCERR_CONN_TIMEDOUT,
-	C2_CONN_STATUS_REFUSED = CCERR_CONN_REFUSED,
-	C2_CONN_STATUS_NETUNREACH = CCERR_NET_UNREACHABLE,
-	C2_CONN_STATUS_HOSTUNREACH = CCERR_HOST_UNREACHABLE,
-	C2_CONN_STATUS_INVALID_RNIC = CCERR_INVALID_RNIC,
-	C2_CONN_STATUS_INVALID_QP = CCERR_INVALID_QP,
-	C2_CONN_STATUS_INVALID_QP_STATE = CCERR_INVALID_QP_STATE,
-	C2_CONN_STATUS_REJECTED = CCERR_CONN_RESET,
-	C2_CONN_STATUS_ADDR_NOT_AVAIL = CCERR_ADDR_NOT_AVAIL,
-};
-
-/*
- * Flash programming status codes.
- */
-enum c2_flash_status {
-	C2_FLASH_STATUS_SUCCESS = 0x0000,
-	C2_FLASH_STATUS_VERIFY_ERR = 0x0002,
-	C2_FLASH_STATUS_IMAGE_ERR = 0x0004,
-	C2_FLASH_STATUS_ECLBS = 0x0400,
-	C2_FLASH_STATUS_PSLBS = 0x0800,
-	C2_FLASH_STATUS_VPENS = 0x1000,
-};
-
-#endif				/* _C2_STATUS_H_ */
diff --git a/drivers/staging/rdma/amso1100/c2_vq.c b/drivers/staging/rdma/amso1100/c2_vq.c
deleted file mode 100644
index 2ec716fb2edb..000000000000
--- a/drivers/staging/rdma/amso1100/c2_vq.c
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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.
- */
-#include <linux/slab.h>
-#include <linux/spinlock.h>
-
-#include "c2_vq.h"
-#include "c2_provider.h"
-
-/*
- * Verbs Request Objects:
- *
- * VQ Request Objects are allocated by the kernel verbs handlers.
- * They contain a wait object, a refcnt, an atomic bool indicating that the
- * adapter has replied, and a copy of the verb reply work request.
- * A pointer to the VQ Request Object is passed down in the context
- * field of the work request message, and reflected back by the adapter
- * in the verbs reply message.  The function handle_vq() in the interrupt
- * path will use this pointer to:
- * 	1) append a copy of the verbs reply message
- * 	2) mark that the reply is ready
- * 	3) wake up the kernel verbs handler blocked awaiting the reply.
- *
- *
- * The kernel verbs handlers do a "get" to put a 2nd reference on the
- * VQ Request object.  If the kernel verbs handler exits before the adapter
- * can respond, this extra reference will keep the VQ Request object around
- * until the adapter's reply can be processed.  The reason we need this is
- * because a pointer to this object is stuffed into the context field of
- * the verbs work request message, and reflected back in the reply message.
- * It is used in the interrupt handler (handle_vq()) to wake up the appropriate
- * kernel verb handler that is blocked awaiting the verb reply.
- * So handle_vq() will do a "put" on the object when it's done accessing it.
- * NOTE:  If we guarantee that the kernel verb handler will never bail before
- *        getting the reply, then we don't need these refcnts.
- *
- *
- * VQ Request objects are freed by the kernel verbs handlers only
- * after the verb has been processed, or when the adapter fails and
- * does not reply.
- *
- *
- * Verbs Reply Buffers:
- *
- * VQ Reply bufs are local host memory copies of a
- * outstanding Verb Request reply
- * message.  The are always allocated by the kernel verbs handlers, and _may_ be
- * freed by either the kernel verbs handler -or- the interrupt handler.  The
- * kernel verbs handler _must_ free the repbuf, then free the vq request object
- * in that order.
- */
-
-int vq_init(struct c2_dev *c2dev)
-{
-	sprintf(c2dev->vq_cache_name, "c2-vq:dev%c",
-		(char) ('0' + c2dev->devnum));
-	c2dev->host_msg_cache =
-	    kmem_cache_create(c2dev->vq_cache_name, c2dev->rep_vq.msg_size, 0,
-			      SLAB_HWCACHE_ALIGN, NULL);
-	if (c2dev->host_msg_cache == NULL) {
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-void vq_term(struct c2_dev *c2dev)
-{
-	kmem_cache_destroy(c2dev->host_msg_cache);
-}
-
-/* vq_req_alloc - allocate a VQ Request Object and initialize it.
- * The refcnt is set to 1.
- */
-struct c2_vq_req *vq_req_alloc(struct c2_dev *c2dev)
-{
-	struct c2_vq_req *r;
-
-	r = kmalloc(sizeof(struct c2_vq_req), GFP_KERNEL);
-	if (r) {
-		init_waitqueue_head(&r->wait_object);
-		r->reply_msg = 0;
-		r->event = 0;
-		r->cm_id = NULL;
-		r->qp = NULL;
-		atomic_set(&r->refcnt, 1);
-		atomic_set(&r->reply_ready, 0);
-	}
-	return r;
-}
-
-
-/* vq_req_free - free the VQ Request Object.  It is assumed the verbs handler
- * has already free the VQ Reply Buffer if it existed.
- */
-void vq_req_free(struct c2_dev *c2dev, struct c2_vq_req *r)
-{
-	r->reply_msg = 0;
-	if (atomic_dec_and_test(&r->refcnt)) {
-		kfree(r);
-	}
-}
-
-/* vq_req_get - reference a VQ Request Object.  Done
- * only in the kernel verbs handlers.
- */
-void vq_req_get(struct c2_dev *c2dev, struct c2_vq_req *r)
-{
-	atomic_inc(&r->refcnt);
-}
-
-
-/* vq_req_put - dereference and potentially free a VQ Request Object.
- *
- * This is only called by handle_vq() on the
- * interrupt when it is done processing
- * a verb reply message.  If the associated
- * kernel verbs handler has already bailed,
- * then this put will actually free the VQ
- * Request object _and_ the VQ Reply Buffer
- * if it exists.
- */
-void vq_req_put(struct c2_dev *c2dev, struct c2_vq_req *r)
-{
-	if (atomic_dec_and_test(&r->refcnt)) {
-		if (r->reply_msg != 0)
-			vq_repbuf_free(c2dev,
-				       (void *) (unsigned long) r->reply_msg);
-		kfree(r);
-	}
-}
-
-
-/*
- * vq_repbuf_alloc - allocate a VQ Reply Buffer.
- */
-void *vq_repbuf_alloc(struct c2_dev *c2dev)
-{
-	return kmem_cache_alloc(c2dev->host_msg_cache, GFP_ATOMIC);
-}
-
-/*
- * vq_send_wr - post a verbs request message to the Verbs Request Queue.
- * If a message is not available in the MQ, then block until one is available.
- * NOTE: handle_mq() on the interrupt context will wake up threads blocked here.
- * When the adapter drains the Verbs Request Queue,
- * it inserts MQ index 0 in to the
- * adapter->host activity fifo and interrupts the host.
- */
-int vq_send_wr(struct c2_dev *c2dev, union c2wr *wr)
-{
-	void *msg;
-	wait_queue_t __wait;
-
-	/*
-	 * grab adapter vq lock
-	 */
-	spin_lock(&c2dev->vqlock);
-
-	/*
-	 * allocate msg
-	 */
-	msg = c2_mq_alloc(&c2dev->req_vq);
-
-	/*
-	 * If we cannot get a msg, then we'll wait
-	 * When a messages are available, the int handler will wake_up()
-	 * any waiters.
-	 */
-	while (msg == NULL) {
-		pr_debug("%s:%d no available msg in VQ, waiting...\n",
-		       __func__, __LINE__);
-		init_waitqueue_entry(&__wait, current);
-		add_wait_queue(&c2dev->req_vq_wo, &__wait);
-		spin_unlock(&c2dev->vqlock);
-		for (;;) {
-			set_current_state(TASK_INTERRUPTIBLE);
-			if (!c2_mq_full(&c2dev->req_vq)) {
-				break;
-			}
-			if (!signal_pending(current)) {
-				schedule_timeout(1 * HZ);	/* 1 second... */
-				continue;
-			}
-			set_current_state(TASK_RUNNING);
-			remove_wait_queue(&c2dev->req_vq_wo, &__wait);
-			return -EINTR;
-		}
-		set_current_state(TASK_RUNNING);
-		remove_wait_queue(&c2dev->req_vq_wo, &__wait);
-		spin_lock(&c2dev->vqlock);
-		msg = c2_mq_alloc(&c2dev->req_vq);
-	}
-
-	/*
-	 * copy wr into adapter msg
-	 */
-	memcpy(msg, wr, c2dev->req_vq.msg_size);
-
-	/*
-	 * post msg
-	 */
-	c2_mq_produce(&c2dev->req_vq);
-
-	/*
-	 * release adapter vq lock
-	 */
-	spin_unlock(&c2dev->vqlock);
-	return 0;
-}
-
-
-/*
- * vq_wait_for_reply - block until the adapter posts a Verb Reply Message.
- */
-int vq_wait_for_reply(struct c2_dev *c2dev, struct c2_vq_req *req)
-{
-	if (!wait_event_timeout(req->wait_object,
-				atomic_read(&req->reply_ready),
-				60*HZ))
-		return -ETIMEDOUT;
-
-	return 0;
-}
-
-/*
- * vq_repbuf_free - Free a Verbs Reply Buffer.
- */
-void vq_repbuf_free(struct c2_dev *c2dev, void *reply)
-{
-	kmem_cache_free(c2dev->host_msg_cache, reply);
-}
diff --git a/drivers/staging/rdma/amso1100/c2_vq.h b/drivers/staging/rdma/amso1100/c2_vq.h
deleted file mode 100644
index 33805627a607..000000000000
--- a/drivers/staging/rdma/amso1100/c2_vq.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 _C2_VQ_H_
-#define _C2_VQ_H_
-#include <linux/sched.h>
-#include "c2.h"
-#include "c2_wr.h"
-#include "c2_provider.h"
-
-struct c2_vq_req {
-	u64 reply_msg;		/* ptr to reply msg */
-	wait_queue_head_t wait_object;	/* wait object for vq reqs */
-	atomic_t reply_ready;	/* set when reply is ready */
-	atomic_t refcnt;	/* used to cancel WRs... */
-	int event;
-	struct iw_cm_id *cm_id;
-	struct c2_qp *qp;
-};
-
-extern int vq_init(struct c2_dev *c2dev);
-extern void vq_term(struct c2_dev *c2dev);
-
-extern struct c2_vq_req *vq_req_alloc(struct c2_dev *c2dev);
-extern void vq_req_free(struct c2_dev *c2dev, struct c2_vq_req *req);
-extern void vq_req_get(struct c2_dev *c2dev, struct c2_vq_req *req);
-extern void vq_req_put(struct c2_dev *c2dev, struct c2_vq_req *req);
-extern int vq_send_wr(struct c2_dev *c2dev, union c2wr * wr);
-
-extern void *vq_repbuf_alloc(struct c2_dev *c2dev);
-extern void vq_repbuf_free(struct c2_dev *c2dev, void *reply);
-
-extern int vq_wait_for_reply(struct c2_dev *c2dev, struct c2_vq_req *req);
-#endif				/* _C2_VQ_H_ */
diff --git a/drivers/staging/rdma/amso1100/c2_wr.h b/drivers/staging/rdma/amso1100/c2_wr.h
deleted file mode 100644
index 8d4b4ca463ca..000000000000
--- a/drivers/staging/rdma/amso1100/c2_wr.h
+++ /dev/null
@@ -1,1520 +0,0 @@
-/*
- * Copyright (c) 2005 Ammasso, Inc. 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 _C2_WR_H_
-#define _C2_WR_H_
-
-#ifdef CCDEBUG
-#define CCWR_MAGIC		0xb07700b0
-#endif
-
-#define C2_QP_NO_ATTR_CHANGE 0xFFFFFFFF
-
-/* Maximum allowed size in bytes of private_data exchange
- * on connect.
- */
-#define C2_MAX_PRIVATE_DATA_SIZE 200
-
-/*
- * These types are shared among the adapter, host, and CCIL consumer.
- */
-enum c2_cq_notification_type {
-	C2_CQ_NOTIFICATION_TYPE_NONE = 1,
-	C2_CQ_NOTIFICATION_TYPE_NEXT,
-	C2_CQ_NOTIFICATION_TYPE_NEXT_SE
-};
-
-enum c2_setconfig_cmd {
-	C2_CFG_ADD_ADDR = 1,
-	C2_CFG_DEL_ADDR = 2,
-	C2_CFG_ADD_ROUTE = 3,
-	C2_CFG_DEL_ROUTE = 4
-};
-
-enum c2_getconfig_cmd {
-	C2_GETCONFIG_ROUTES = 1,
-	C2_GETCONFIG_ADDRS
-};
-
-/*
- *  CCIL Work Request Identifiers
- */
-enum c2wr_ids {
-	CCWR_RNIC_OPEN = 1,
-	CCWR_RNIC_QUERY,
-	CCWR_RNIC_SETCONFIG,
-	CCWR_RNIC_GETCONFIG,
-	CCWR_RNIC_CLOSE,
-	CCWR_CQ_CREATE,
-	CCWR_CQ_QUERY,
-	CCWR_CQ_MODIFY,
-	CCWR_CQ_DESTROY,
-	CCWR_QP_CONNECT,
-	CCWR_PD_ALLOC,
-	CCWR_PD_DEALLOC,
-	CCWR_SRQ_CREATE,
-	CCWR_SRQ_QUERY,
-	CCWR_SRQ_MODIFY,
-	CCWR_SRQ_DESTROY,
-	CCWR_QP_CREATE,
-	CCWR_QP_QUERY,
-	CCWR_QP_MODIFY,
-	CCWR_QP_DESTROY,
-	CCWR_NSMR_STAG_ALLOC,
-	CCWR_NSMR_REGISTER,
-	CCWR_NSMR_PBL,
-	CCWR_STAG_DEALLOC,
-	CCWR_NSMR_REREGISTER,
-	CCWR_SMR_REGISTER,
-	CCWR_MR_QUERY,
-	CCWR_MW_ALLOC,
-	CCWR_MW_QUERY,
-	CCWR_EP_CREATE,
-	CCWR_EP_GETOPT,
-	CCWR_EP_SETOPT,
-	CCWR_EP_DESTROY,
-	CCWR_EP_BIND,
-	CCWR_EP_CONNECT,
-	CCWR_EP_LISTEN,
-	CCWR_EP_SHUTDOWN,
-	CCWR_EP_LISTEN_CREATE,
-	CCWR_EP_LISTEN_DESTROY,
-	CCWR_EP_QUERY,
-	CCWR_CR_ACCEPT,
-	CCWR_CR_REJECT,
-	CCWR_CONSOLE,
-	CCWR_TERM,
-	CCWR_FLASH_INIT,
-	CCWR_FLASH,
-	CCWR_BUF_ALLOC,
-	CCWR_BUF_FREE,
-	CCWR_FLASH_WRITE,
-	CCWR_INIT,		/* WARNING: Don't move this ever again! */
-
-
-
-	/* Add new IDs here */
-
-
-
-	/*
-	 * WARNING: CCWR_LAST must always be the last verbs id defined!
-	 *          All the preceding IDs are fixed, and must not change.
-	 *          You can add new IDs, but must not remove or reorder
-	 *          any IDs. If you do, YOU will ruin any hope of
-	 *          compatibility between versions.
-	 */
-	CCWR_LAST,
-
-	/*
-	 * Start over at 1 so that arrays indexed by user wr id's
-	 * begin at 1.  This is OK since the verbs and user wr id's
-	 * are always used on disjoint sets of queues.
-	 */
-	/*
-	 * The order of the CCWR_SEND_XX verbs must
-	 * match the order of the RDMA_OPs
-	 */
-	CCWR_SEND = 1,
-	CCWR_SEND_INV,
-	CCWR_SEND_SE,
-	CCWR_SEND_SE_INV,
-	CCWR_RDMA_WRITE,
-	CCWR_RDMA_READ,
-	CCWR_RDMA_READ_INV,
-	CCWR_MW_BIND,
-	CCWR_NSMR_FASTREG,
-	CCWR_STAG_INVALIDATE,
-	CCWR_RECV,
-	CCWR_NOP,
-	CCWR_UNIMPL,
-/* WARNING: This must always be the last user wr id defined! */
-};
-#define RDMA_SEND_OPCODE_FROM_WR_ID(x)   (x+2)
-
-/*
- * SQ/RQ Work Request Types
- */
-enum c2_wr_type {
-	C2_WR_TYPE_SEND = CCWR_SEND,
-	C2_WR_TYPE_SEND_SE = CCWR_SEND_SE,
-	C2_WR_TYPE_SEND_INV = CCWR_SEND_INV,
-	C2_WR_TYPE_SEND_SE_INV = CCWR_SEND_SE_INV,
-	C2_WR_TYPE_RDMA_WRITE = CCWR_RDMA_WRITE,
-	C2_WR_TYPE_RDMA_READ = CCWR_RDMA_READ,
-	C2_WR_TYPE_RDMA_READ_INV_STAG = CCWR_RDMA_READ_INV,
-	C2_WR_TYPE_BIND_MW = CCWR_MW_BIND,
-	C2_WR_TYPE_FASTREG_NSMR = CCWR_NSMR_FASTREG,
-	C2_WR_TYPE_INV_STAG = CCWR_STAG_INVALIDATE,
-	C2_WR_TYPE_RECV = CCWR_RECV,
-	C2_WR_TYPE_NOP = CCWR_NOP,
-};
-
-struct c2_netaddr {
-	__be32 ip_addr;
-	__be32 netmask;
-	u32 mtu;
-};
-
-struct c2_route {
-	u32 ip_addr;		/* 0 indicates the default route */
-	u32 netmask;		/* netmask associated with dst */
-	u32 flags;
-	union {
-		u32 ipaddr;	/* address of the nexthop interface */
-		u8 enaddr[6];
-	} nexthop;
-};
-
-/*
- * A Scatter Gather Entry.
- */
-struct c2_data_addr {
-	__be32 stag;
-	__be32 length;
-	__be64 to;
-};
-
-/*
- * MR and MW flags used by the consumer, RI, and RNIC.
- */
-enum c2_mm_flags {
-	MEM_REMOTE = 0x0001,	/* allow mw binds with remote access. */
-	MEM_VA_BASED = 0x0002,	/* Not Zero-based */
-	MEM_PBL_COMPLETE = 0x0004,	/* PBL array is complete in this msg */
-	MEM_LOCAL_READ = 0x0008,	/* allow local reads */
-	MEM_LOCAL_WRITE = 0x0010,	/* allow local writes */
-	MEM_REMOTE_READ = 0x0020,	/* allow remote reads */
-	MEM_REMOTE_WRITE = 0x0040,	/* allow remote writes */
-	MEM_WINDOW_BIND = 0x0080,	/* binds allowed */
-	MEM_SHARED = 0x0100,	/* set if MR is shared */
-	MEM_STAG_VALID = 0x0200	/* set if STAG is in valid state */
-};
-
-/*
- * CCIL API ACF flags defined in terms of the low level mem flags.
- * This minimizes translation needed in the user API
- */
-enum c2_acf {
-	C2_ACF_LOCAL_READ = MEM_LOCAL_READ,
-	C2_ACF_LOCAL_WRITE = MEM_LOCAL_WRITE,
-	C2_ACF_REMOTE_READ = MEM_REMOTE_READ,
-	C2_ACF_REMOTE_WRITE = MEM_REMOTE_WRITE,
-	C2_ACF_WINDOW_BIND = MEM_WINDOW_BIND
-};
-
-/*
- * Image types of objects written to flash
- */
-#define C2_FLASH_IMG_BITFILE 1
-#define C2_FLASH_IMG_OPTION_ROM 2
-#define C2_FLASH_IMG_VPD 3
-
-/*
- *  to fix bug 1815 we define the max size allowable of the
- *  terminate message (per the IETF spec).Refer to the IETF
- *  protocol specification, section 12.1.6, page 64)
- *  The message is prefixed by 20 types of DDP info.
- *
- *  Then the message has 6 bytes for the terminate control
- *  and DDP segment length info plus a DDP header (either
- *  14 or 18 byts) plus 28 bytes for the RDMA header.
- *  Thus the max size in:
- *  20 + (6 + 18 + 28) = 72
- */
-#define C2_MAX_TERMINATE_MESSAGE_SIZE (72)
-
-/*
- * Build String Length.  It must be the same as C2_BUILD_STR_LEN in ccil_api.h
- */
-#define WR_BUILD_STR_LEN 64
-
-/*
- * WARNING:  All of these structs need to align any 64bit types on
- * 64 bit boundaries!  64bit types include u64 and u64.
- */
-
-/*
- * Clustercore Work Request Header.  Be sensitive to field layout
- * and alignment.
- */
-struct c2wr_hdr {
-	/* wqe_count is part of the cqe.  It is put here so the
-	 * adapter can write to it while the wr is pending without
-	 * clobbering part of the wr.  This word need not be dma'd
-	 * from the host to adapter by libccil, but we copy it anyway
-	 * to make the memcpy to the adapter better aligned.
-	 */
-	__be32 wqe_count;
-
-	/* Put these fields next so that later 32- and 64-bit
-	 * quantities are naturally aligned.
-	 */
-	u8 id;
-	u8 result;		/* adapter -> host */
-	u8 sge_count;		/* host -> adapter */
-	u8 flags;		/* host -> adapter */
-
-	u64 context;
-#ifdef CCMSGMAGIC
-	u32 magic;
-	u32 pad;
-#endif
-} __attribute__((packed));
-
-/*
- *------------------------ RNIC ------------------------
- */
-
-/*
- * WR_RNIC_OPEN
- */
-
-/*
- * Flags for the RNIC WRs
- */
-enum c2_rnic_flags {
-	RNIC_IRD_STATIC = 0x0001,
-	RNIC_ORD_STATIC = 0x0002,
-	RNIC_QP_STATIC = 0x0004,
-	RNIC_SRQ_SUPPORTED = 0x0008,
-	RNIC_PBL_BLOCK_MODE = 0x0010,
-	RNIC_SRQ_MODEL_ARRIVAL = 0x0020,
-	RNIC_CQ_OVF_DETECTED = 0x0040,
-	RNIC_PRIV_MODE = 0x0080
-};
-
-struct c2wr_rnic_open_req {
-	struct c2wr_hdr hdr;
-	u64 user_context;
-	__be16 flags;		/* See enum c2_rnic_flags */
-	__be16 port_num;
-} __attribute__((packed));
-
-struct c2wr_rnic_open_rep {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-} __attribute__((packed));
-
-union c2wr_rnic_open {
-	struct c2wr_rnic_open_req req;
-	struct c2wr_rnic_open_rep rep;
-} __attribute__((packed));
-
-struct c2wr_rnic_query_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-} __attribute__((packed));
-
-/*
- * WR_RNIC_QUERY
- */
-struct c2wr_rnic_query_rep {
-	struct c2wr_hdr hdr;
-	u64 user_context;
-	__be32 vendor_id;
-	__be32 part_number;
-	__be32 hw_version;
-	__be32 fw_ver_major;
-	__be32 fw_ver_minor;
-	__be32 fw_ver_patch;
-	char fw_ver_build_str[WR_BUILD_STR_LEN];
-	__be32 max_qps;
-	__be32 max_qp_depth;
-	u32 max_srq_depth;
-	u32 max_send_sgl_depth;
-	u32 max_rdma_sgl_depth;
-	__be32 max_cqs;
-	__be32 max_cq_depth;
-	u32 max_cq_event_handlers;
-	__be32 max_mrs;
-	u32 max_pbl_depth;
-	__be32 max_pds;
-	__be32 max_global_ird;
-	u32 max_global_ord;
-	__be32 max_qp_ird;
-	__be32 max_qp_ord;
-	u32 flags;
-	__be32 max_mws;
-	u32 pbe_range_low;
-	u32 pbe_range_high;
-	u32 max_srqs;
-	u32 page_size;
-} __attribute__((packed));
-
-union c2wr_rnic_query {
-	struct c2wr_rnic_query_req req;
-	struct c2wr_rnic_query_rep rep;
-} __attribute__((packed));
-
-/*
- * WR_RNIC_GETCONFIG
- */
-
-struct c2wr_rnic_getconfig_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 option;		/* see c2_getconfig_cmd_t */
-	u64 reply_buf;
-	u32 reply_buf_len;
-} __attribute__((packed)) ;
-
-struct c2wr_rnic_getconfig_rep {
-	struct c2wr_hdr hdr;
-	u32 option;		/* see c2_getconfig_cmd_t */
-	u32 count_len;		/* length of the number of addresses configured */
-} __attribute__((packed)) ;
-
-union c2wr_rnic_getconfig {
-	struct c2wr_rnic_getconfig_req req;
-	struct c2wr_rnic_getconfig_rep rep;
-} __attribute__((packed)) ;
-
-/*
- * WR_RNIC_SETCONFIG
- */
-struct c2wr_rnic_setconfig_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	__be32 option;		/* See c2_setconfig_cmd_t */
-	/* variable data and pad. See c2_netaddr and c2_route */
-	u8 data[0];
-} __attribute__((packed)) ;
-
-struct c2wr_rnic_setconfig_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_rnic_setconfig {
-	struct c2wr_rnic_setconfig_req req;
-	struct c2wr_rnic_setconfig_rep rep;
-} __attribute__((packed)) ;
-
-/*
- * WR_RNIC_CLOSE
- */
-struct c2wr_rnic_close_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_rnic_close_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_rnic_close {
-	struct c2wr_rnic_close_req req;
-	struct c2wr_rnic_close_rep rep;
-} __attribute__((packed)) ;
-
-/*
- *------------------------ CQ ------------------------
- */
-struct c2wr_cq_create_req {
-	struct c2wr_hdr hdr;
-	__be64 shared_ht;
-	u64 user_context;
-	__be64 msg_pool;
-	u32 rnic_handle;
-	__be32 msg_size;
-	__be32 depth;
-} __attribute__((packed)) ;
-
-struct c2wr_cq_create_rep {
-	struct c2wr_hdr hdr;
-	__be32 mq_index;
-	__be32 adapter_shared;
-	u32 cq_handle;
-} __attribute__((packed)) ;
-
-union c2wr_cq_create {
-	struct c2wr_cq_create_req req;
-	struct c2wr_cq_create_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_cq_modify_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 cq_handle;
-	u32 new_depth;
-	u64 new_msg_pool;
-} __attribute__((packed)) ;
-
-struct c2wr_cq_modify_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_cq_modify {
-	struct c2wr_cq_modify_req req;
-	struct c2wr_cq_modify_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_cq_destroy_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 cq_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_cq_destroy_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_cq_destroy {
-	struct c2wr_cq_destroy_req req;
-	struct c2wr_cq_destroy_rep rep;
-} __attribute__((packed)) ;
-
-/*
- *------------------------ PD ------------------------
- */
-struct c2wr_pd_alloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_pd_alloc_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_pd_alloc {
-	struct c2wr_pd_alloc_req req;
-	struct c2wr_pd_alloc_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_pd_dealloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_pd_dealloc_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_pd_dealloc {
-	struct c2wr_pd_dealloc_req req;
-	struct c2wr_pd_dealloc_rep rep;
-} __attribute__((packed)) ;
-
-/*
- *------------------------ SRQ ------------------------
- */
-struct c2wr_srq_create_req {
-	struct c2wr_hdr hdr;
-	u64 shared_ht;
-	u64 user_context;
-	u32 rnic_handle;
-	u32 srq_depth;
-	u32 srq_limit;
-	u32 sgl_depth;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_srq_create_rep {
-	struct c2wr_hdr hdr;
-	u32 srq_depth;
-	u32 sgl_depth;
-	u32 msg_size;
-	u32 mq_index;
-	u32 mq_start;
-	u32 srq_handle;
-} __attribute__((packed)) ;
-
-union c2wr_srq_create {
-	struct c2wr_srq_create_req req;
-	struct c2wr_srq_create_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_srq_destroy_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 srq_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_srq_destroy_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_srq_destroy {
-	struct c2wr_srq_destroy_req req;
-	struct c2wr_srq_destroy_rep rep;
-} __attribute__((packed)) ;
-
-/*
- *------------------------ QP ------------------------
- */
-enum c2wr_qp_flags {
-	QP_RDMA_READ = 0x00000001,	/* RDMA read enabled? */
-	QP_RDMA_WRITE = 0x00000002,	/* RDMA write enabled? */
-	QP_MW_BIND = 0x00000004,	/* MWs enabled */
-	QP_ZERO_STAG = 0x00000008,	/* enabled? */
-	QP_REMOTE_TERMINATION = 0x00000010,	/* remote end terminated */
-	QP_RDMA_READ_RESPONSE = 0x00000020	/* Remote RDMA read  */
-	    /* enabled? */
-};
-
-struct c2wr_qp_create_req {
-	struct c2wr_hdr hdr;
-	__be64 shared_sq_ht;
-	__be64 shared_rq_ht;
-	u64 user_context;
-	u32 rnic_handle;
-	u32 sq_cq_handle;
-	u32 rq_cq_handle;
-	__be32 sq_depth;
-	__be32 rq_depth;
-	u32 srq_handle;
-	u32 srq_limit;
-	__be32 flags;		/* see enum c2wr_qp_flags */
-	__be32 send_sgl_depth;
-	__be32 recv_sgl_depth;
-	__be32 rdma_write_sgl_depth;
-	__be32 ord;
-	__be32 ird;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_create_rep {
-	struct c2wr_hdr hdr;
-	__be32 sq_depth;
-	__be32 rq_depth;
-	u32 send_sgl_depth;
-	u32 recv_sgl_depth;
-	u32 rdma_write_sgl_depth;
-	u32 ord;
-	u32 ird;
-	__be32 sq_msg_size;
-	__be32 sq_mq_index;
-	__be32 sq_mq_start;
-	__be32 rq_msg_size;
-	__be32 rq_mq_index;
-	__be32 rq_mq_start;
-	u32 qp_handle;
-} __attribute__((packed)) ;
-
-union c2wr_qp_create {
-	struct c2wr_qp_create_req req;
-	struct c2wr_qp_create_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_query_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 qp_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_query_rep {
-	struct c2wr_hdr hdr;
-	u64 user_context;
-	u32 rnic_handle;
-	u32 sq_depth;
-	u32 rq_depth;
-	u32 send_sgl_depth;
-	u32 rdma_write_sgl_depth;
-	u32 recv_sgl_depth;
-	u32 ord;
-	u32 ird;
-	u16 qp_state;
-	u16 flags;		/* see c2wr_qp_flags_t */
-	u32 qp_id;
-	u32 local_addr;
-	u32 remote_addr;
-	u16 local_port;
-	u16 remote_port;
-	u32 terminate_msg_length;	/* 0 if not present */
-	u8 data[0];
-	/* Terminate Message in-line here. */
-} __attribute__((packed)) ;
-
-union c2wr_qp_query {
-	struct c2wr_qp_query_req req;
-	struct c2wr_qp_query_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_modify_req {
-	struct c2wr_hdr hdr;
-	u64 stream_msg;
-	u32 stream_msg_length;
-	u32 rnic_handle;
-	u32 qp_handle;
-	__be32 next_qp_state;
-	__be32 ord;
-	__be32 ird;
-	__be32 sq_depth;
-	__be32 rq_depth;
-	u32 llp_ep_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_modify_rep {
-	struct c2wr_hdr hdr;
-	u32 ord;
-	u32 ird;
-	u32 sq_depth;
-	u32 rq_depth;
-	u32 sq_msg_size;
-	u32 sq_mq_index;
-	u32 sq_mq_start;
-	u32 rq_msg_size;
-	u32 rq_mq_index;
-	u32 rq_mq_start;
-} __attribute__((packed)) ;
-
-union c2wr_qp_modify {
-	struct c2wr_qp_modify_req req;
-	struct c2wr_qp_modify_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_destroy_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 qp_handle;
-} __attribute__((packed)) ;
-
-struct c2wr_qp_destroy_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_qp_destroy {
-	struct c2wr_qp_destroy_req req;
-	struct c2wr_qp_destroy_rep rep;
-} __attribute__((packed)) ;
-
-/*
- * The CCWR_QP_CONNECT msg is posted on the verbs request queue.  It can
- * only be posted when a QP is in IDLE state.  After the connect request is
- * submitted to the LLP, the adapter moves the QP to CONNECT_PENDING state.
- * No synchronous reply from adapter to this WR.  The results of
- * connection are passed back in an async event CCAE_ACTIVE_CONNECT_RESULTS
- * See c2wr_ae_active_connect_results_t
- */
-struct c2wr_qp_connect_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 qp_handle;
-	__be32 remote_addr;
-	__be16 remote_port;
-	u16 pad;
-	__be32 private_data_length;
-	u8 private_data[0];	/* Private data in-line. */
-} __attribute__((packed)) ;
-
-struct c2wr_qp_connect {
-	struct c2wr_qp_connect_req req;
-	/* no synchronous reply.         */
-} __attribute__((packed)) ;
-
-
-/*
- *------------------------ MM ------------------------
- */
-
-struct c2wr_nsmr_stag_alloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 pbl_depth;
-	u32 pd_id;
-	u32 flags;
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_stag_alloc_rep {
-	struct c2wr_hdr hdr;
-	u32 pbl_depth;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-union c2wr_nsmr_stag_alloc {
-	struct c2wr_nsmr_stag_alloc_req req;
-	struct c2wr_nsmr_stag_alloc_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_register_req {
-	struct c2wr_hdr hdr;
-	__be64 va;
-	u32 rnic_handle;
-	__be16 flags;
-	u8 stag_key;
-	u8 pad;
-	u32 pd_id;
-	__be32 pbl_depth;
-	__be32 pbe_size;
-	__be32 fbo;
-	__be32 length;
-	__be32 addrs_length;
-	/* array of paddrs (must be aligned on a 64bit boundary) */
-	__be64 paddrs[0];
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_register_rep {
-	struct c2wr_hdr hdr;
-	u32 pbl_depth;
-	__be32 stag_index;
-} __attribute__((packed)) ;
-
-union c2wr_nsmr_register {
-	struct c2wr_nsmr_register_req req;
-	struct c2wr_nsmr_register_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_pbl_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	__be32 flags;
-	__be32 stag_index;
-	__be32 addrs_length;
-	/* array of paddrs (must be aligned on a 64bit boundary) */
-	__be64 paddrs[0];
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_pbl_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_nsmr_pbl {
-	struct c2wr_nsmr_pbl_req req;
-	struct c2wr_nsmr_pbl_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_mr_query_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-struct c2wr_mr_query_rep {
-	struct c2wr_hdr hdr;
-	u8 stag_key;
-	u8 pad[3];
-	u32 pd_id;
-	u32 flags;
-	u32 pbl_depth;
-} __attribute__((packed)) ;
-
-union c2wr_mr_query {
-	struct c2wr_mr_query_req req;
-	struct c2wr_mr_query_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_mw_query_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-struct c2wr_mw_query_rep {
-	struct c2wr_hdr hdr;
-	u8 stag_key;
-	u8 pad[3];
-	u32 pd_id;
-	u32 flags;
-} __attribute__((packed)) ;
-
-union c2wr_mw_query {
-	struct c2wr_mw_query_req req;
-	struct c2wr_mw_query_rep rep;
-} __attribute__((packed)) ;
-
-
-struct c2wr_stag_dealloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	__be32 stag_index;
-} __attribute__((packed)) ;
-
-struct c2wr_stag_dealloc_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed)) ;
-
-union c2wr_stag_dealloc {
-	struct c2wr_stag_dealloc_req req;
-	struct c2wr_stag_dealloc_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_reregister_req {
-	struct c2wr_hdr hdr;
-	u64 va;
-	u32 rnic_handle;
-	u16 flags;
-	u8 stag_key;
-	u8 pad;
-	u32 stag_index;
-	u32 pd_id;
-	u32 pbl_depth;
-	u32 pbe_size;
-	u32 fbo;
-	u32 length;
-	u32 addrs_length;
-	u32 pad1;
-	/* array of paddrs (must be aligned on a 64bit boundary) */
-	u64 paddrs[0];
-} __attribute__((packed)) ;
-
-struct c2wr_nsmr_reregister_rep {
-	struct c2wr_hdr hdr;
-	u32 pbl_depth;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-union c2wr_nsmr_reregister {
-	struct c2wr_nsmr_reregister_req req;
-	struct c2wr_nsmr_reregister_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_smr_register_req {
-	struct c2wr_hdr hdr;
-	u64 va;
-	u32 rnic_handle;
-	u16 flags;
-	u8 stag_key;
-	u8 pad;
-	u32 stag_index;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_smr_register_rep {
-	struct c2wr_hdr hdr;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-union c2wr_smr_register {
-	struct c2wr_smr_register_req req;
-	struct c2wr_smr_register_rep rep;
-} __attribute__((packed)) ;
-
-struct c2wr_mw_alloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 pd_id;
-} __attribute__((packed)) ;
-
-struct c2wr_mw_alloc_rep {
-	struct c2wr_hdr hdr;
-	u32 stag_index;
-} __attribute__((packed)) ;
-
-union c2wr_mw_alloc {
-	struct c2wr_mw_alloc_req req;
-	struct c2wr_mw_alloc_rep rep;
-} __attribute__((packed)) ;
-
-/*
- *------------------------ WRs -----------------------
- */
-
-struct c2wr_user_hdr {
-	struct c2wr_hdr hdr;		/* Has status and WR Type */
-} __attribute__((packed)) ;
-
-enum c2_qp_state {
-	C2_QP_STATE_IDLE = 0x01,
-	C2_QP_STATE_CONNECTING = 0x02,
-	C2_QP_STATE_RTS = 0x04,
-	C2_QP_STATE_CLOSING = 0x08,
-	C2_QP_STATE_TERMINATE = 0x10,
-	C2_QP_STATE_ERROR = 0x20,
-};
-
-/* Completion queue entry. */
-struct c2wr_ce {
-	struct c2wr_hdr hdr;		/* Has status and WR Type */
-	u64 qp_user_context;	/* c2_user_qp_t * */
-	u32 qp_state;		/* Current QP State */
-	u32 handle;		/* QPID or EP Handle */
-	__be32 bytes_rcvd;		/* valid for RECV WCs */
-	u32 stag;
-} __attribute__((packed)) ;
-
-
-/*
- * Flags used for all post-sq WRs.  These must fit in the flags
- * field of the struct c2wr_hdr (eight bits).
- */
-enum {
-	SQ_SIGNALED = 0x01,
-	SQ_READ_FENCE = 0x02,
-	SQ_FENCE = 0x04,
-};
-
-/*
- * Common fields for all post-sq WRs.  Namely the standard header and a
- * secondary header with fields common to all post-sq WRs.
- */
-struct c2_sq_hdr {
-	struct c2wr_user_hdr user_hdr;
-} __attribute__((packed));
-
-/*
- * Same as above but for post-rq WRs.
- */
-struct c2_rq_hdr {
-	struct c2wr_user_hdr user_hdr;
-} __attribute__((packed));
-
-/*
- * use the same struct for all sends.
- */
-struct c2wr_send_req {
-	struct c2_sq_hdr sq_hdr;
-	__be32 sge_len;
-	__be32 remote_stag;
-	u8 data[0];		/* SGE array */
-} __attribute__((packed));
-
-union c2wr_send {
-	struct c2wr_send_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_rdma_write_req {
-	struct c2_sq_hdr sq_hdr;
-	__be64 remote_to;
-	__be32 remote_stag;
-	__be32 sge_len;
-	u8 data[0];		/* SGE array */
-} __attribute__((packed));
-
-union c2wr_rdma_write {
-	struct c2wr_rdma_write_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_rdma_read_req {
-	struct c2_sq_hdr sq_hdr;
-	__be64 local_to;
-	__be64 remote_to;
-	__be32 local_stag;
-	__be32 remote_stag;
-	__be32 length;
-} __attribute__((packed));
-
-union c2wr_rdma_read {
-	struct c2wr_rdma_read_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_mw_bind_req {
-	struct c2_sq_hdr sq_hdr;
-	u64 va;
-	u8 stag_key;
-	u8 pad[3];
-	u32 mw_stag_index;
-	u32 mr_stag_index;
-	u32 length;
-	u32 flags;
-} __attribute__((packed));
-
-union c2wr_mw_bind {
-	struct c2wr_mw_bind_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_nsmr_fastreg_req {
-	struct c2_sq_hdr sq_hdr;
-	u64 va;
-	u8 stag_key;
-	u8 pad[3];
-	u32 stag_index;
-	u32 pbe_size;
-	u32 fbo;
-	u32 length;
-	u32 addrs_length;
-	/* array of paddrs (must be aligned on a 64bit boundary) */
-	u64 paddrs[0];
-} __attribute__((packed));
-
-union c2wr_nsmr_fastreg {
-	struct c2wr_nsmr_fastreg_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_stag_invalidate_req {
-	struct c2_sq_hdr sq_hdr;
-	u8 stag_key;
-	u8 pad[3];
-	u32 stag_index;
-} __attribute__((packed));
-
-union c2wr_stag_invalidate {
-	struct c2wr_stag_invalidate_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-union c2wr_sqwr {
-	struct c2_sq_hdr sq_hdr;
-	struct c2wr_send_req send;
-	struct c2wr_send_req send_se;
-	struct c2wr_send_req send_inv;
-	struct c2wr_send_req send_se_inv;
-	struct c2wr_rdma_write_req rdma_write;
-	struct c2wr_rdma_read_req rdma_read;
-	struct c2wr_mw_bind_req mw_bind;
-	struct c2wr_nsmr_fastreg_req nsmr_fastreg;
-	struct c2wr_stag_invalidate_req stag_inv;
-} __attribute__((packed));
-
-
-/*
- * RQ WRs
- */
-struct c2wr_rqwr {
-	struct c2_rq_hdr rq_hdr;
-	u8 data[0];		/* array of SGEs */
-} __attribute__((packed));
-
-union c2wr_recv {
-	struct c2wr_rqwr req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-/*
- * All AEs start with this header.  Most AEs only need to convey the
- * information in the header.  Some, like LLP connection events, need
- * more info.  The union typdef c2wr_ae_t has all the possible AEs.
- *
- * hdr.context is the user_context from the rnic_open WR.  NULL If this
- * is not affiliated with an rnic
- *
- * hdr.id is the AE identifier (eg;  CCAE_REMOTE_SHUTDOWN,
- * CCAE_LLP_CLOSE_COMPLETE)
- *
- * resource_type is one of:  C2_RES_IND_QP, C2_RES_IND_CQ, C2_RES_IND_SRQ
- *
- * user_context is the context passed down when the host created the resource.
- */
-struct c2wr_ae_hdr {
-	struct c2wr_hdr hdr;
-	u64 user_context;	/* user context for this res. */
-	__be32 resource_type;	/* see enum c2_resource_indicator */
-	__be32 resource;	/* handle for resource */
-	__be32 qp_state;	/* current QP State */
-} __attribute__((packed));
-
-/*
- * After submitting the CCAE_ACTIVE_CONNECT_RESULTS message on the AEQ,
- * the adapter moves the QP into RTS state
- */
-struct c2wr_ae_active_connect_results {
-	struct c2wr_ae_hdr ae_hdr;
-	__be32 laddr;
-	__be32 raddr;
-	__be16 lport;
-	__be16 rport;
-	__be32 private_data_length;
-	u8 private_data[0];	/* data is in-line in the msg. */
-} __attribute__((packed));
-
-/*
- * When connections are established by the stack (and the private data
- * MPA frame is received), the adapter will generate an event to the host.
- * The details of the connection, any private data, and the new connection
- * request handle is passed up via the CCAE_CONNECTION_REQUEST msg on the
- * AE queue:
- */
-struct c2wr_ae_connection_request {
-	struct c2wr_ae_hdr ae_hdr;
-	u32 cr_handle;		/* connreq handle (sock ptr) */
-	__be32 laddr;
-	__be32 raddr;
-	__be16 lport;
-	__be16 rport;
-	__be32 private_data_length;
-	u8 private_data[0];	/* data is in-line in the msg. */
-} __attribute__((packed));
-
-union c2wr_ae {
-	struct c2wr_ae_hdr ae_generic;
-	struct c2wr_ae_active_connect_results ae_active_connect_results;
-	struct c2wr_ae_connection_request ae_connection_request;
-} __attribute__((packed));
-
-struct c2wr_init_req {
-	struct c2wr_hdr hdr;
-	__be64 hint_count;
-	__be64 q0_host_shared;
-	__be64 q1_host_shared;
-	__be64 q1_host_msg_pool;
-	__be64 q2_host_shared;
-	__be64 q2_host_msg_pool;
-} __attribute__((packed));
-
-struct c2wr_init_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed));
-
-union c2wr_init {
-	struct c2wr_init_req req;
-	struct c2wr_init_rep rep;
-} __attribute__((packed));
-
-/*
- * For upgrading flash.
- */
-
-struct c2wr_flash_init_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-} __attribute__((packed));
-
-struct c2wr_flash_init_rep {
-	struct c2wr_hdr hdr;
-	u32 adapter_flash_buf_offset;
-	u32 adapter_flash_len;
-} __attribute__((packed));
-
-union c2wr_flash_init {
-	struct c2wr_flash_init_req req;
-	struct c2wr_flash_init_rep rep;
-} __attribute__((packed));
-
-struct c2wr_flash_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 len;
-} __attribute__((packed));
-
-struct c2wr_flash_rep {
-	struct c2wr_hdr hdr;
-	u32 status;
-} __attribute__((packed));
-
-union c2wr_flash {
-	struct c2wr_flash_req req;
-	struct c2wr_flash_rep rep;
-} __attribute__((packed));
-
-struct c2wr_buf_alloc_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 size;
-} __attribute__((packed));
-
-struct c2wr_buf_alloc_rep {
-	struct c2wr_hdr hdr;
-	u32 offset;		/* 0 if mem not available */
-	u32 size;		/* 0 if mem not available */
-} __attribute__((packed));
-
-union c2wr_buf_alloc {
-	struct c2wr_buf_alloc_req req;
-	struct c2wr_buf_alloc_rep rep;
-} __attribute__((packed));
-
-struct c2wr_buf_free_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 offset;		/* Must match value from alloc */
-	u32 size;		/* Must match value from alloc */
-} __attribute__((packed));
-
-struct c2wr_buf_free_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed));
-
-union c2wr_buf_free {
-	struct c2wr_buf_free_req req;
-	struct c2wr_ce rep;
-} __attribute__((packed));
-
-struct c2wr_flash_write_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 offset;
-	u32 size;
-	u32 type;
-	u32 flags;
-} __attribute__((packed));
-
-struct c2wr_flash_write_rep {
-	struct c2wr_hdr hdr;
-	u32 status;
-} __attribute__((packed));
-
-union c2wr_flash_write {
-	struct c2wr_flash_write_req req;
-	struct c2wr_flash_write_rep rep;
-} __attribute__((packed));
-
-/*
- * Messages for LLP connection setup.
- */
-
-/*
- * Listen Request.  This allocates a listening endpoint to allow passive
- * connection setup.  Newly established LLP connections are passed up
- * via an AE.  See c2wr_ae_connection_request_t
- */
-struct c2wr_ep_listen_create_req {
-	struct c2wr_hdr hdr;
-	u64 user_context;	/* returned in AEs. */
-	u32 rnic_handle;
-	__be32 local_addr;		/* local addr, or 0  */
-	__be16 local_port;		/* 0 means "pick one" */
-	u16 pad;
-	__be32 backlog;		/* tradional tcp listen bl */
-} __attribute__((packed));
-
-struct c2wr_ep_listen_create_rep {
-	struct c2wr_hdr hdr;
-	u32 ep_handle;		/* handle to new listening ep */
-	u16 local_port;		/* resulting port... */
-	u16 pad;
-} __attribute__((packed));
-
-union c2wr_ep_listen_create {
-	struct c2wr_ep_listen_create_req req;
-	struct c2wr_ep_listen_create_rep rep;
-} __attribute__((packed));
-
-struct c2wr_ep_listen_destroy_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 ep_handle;
-} __attribute__((packed));
-
-struct c2wr_ep_listen_destroy_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed));
-
-union c2wr_ep_listen_destroy {
-	struct c2wr_ep_listen_destroy_req req;
-	struct c2wr_ep_listen_destroy_rep rep;
-} __attribute__((packed));
-
-struct c2wr_ep_query_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 ep_handle;
-} __attribute__((packed));
-
-struct c2wr_ep_query_rep {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 local_addr;
-	u32 remote_addr;
-	u16 local_port;
-	u16 remote_port;
-} __attribute__((packed));
-
-union c2wr_ep_query {
-	struct c2wr_ep_query_req req;
-	struct c2wr_ep_query_rep rep;
-} __attribute__((packed));
-
-
-/*
- * The host passes this down to indicate acceptance of a pending iWARP
- * connection.  The cr_handle was obtained from the CONNECTION_REQUEST
- * AE passed up by the adapter.  See c2wr_ae_connection_request_t.
- */
-struct c2wr_cr_accept_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 qp_handle;		/* QP to bind to this LLP conn */
-	u32 ep_handle;		/* LLP  handle to accept */
-	__be32 private_data_length;
-	u8 private_data[0];	/* data in-line in msg. */
-} __attribute__((packed));
-
-/*
- * adapter sends reply when private data is successfully submitted to
- * the LLP.
- */
-struct c2wr_cr_accept_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed));
-
-union c2wr_cr_accept {
-	struct c2wr_cr_accept_req req;
-	struct c2wr_cr_accept_rep rep;
-} __attribute__((packed));
-
-/*
- * The host sends this down if a given iWARP connection request was
- * rejected by the consumer.  The cr_handle was obtained from a
- * previous c2wr_ae_connection_request_t AE sent by the adapter.
- */
-struct  c2wr_cr_reject_req {
-	struct c2wr_hdr hdr;
-	u32 rnic_handle;
-	u32 ep_handle;		/* LLP handle to reject */
-} __attribute__((packed));
-
-/*
- * Dunno if this is needed, but we'll add it for now.  The adapter will
- * send the reject_reply after the LLP endpoint has been destroyed.
- */
-struct  c2wr_cr_reject_rep {
-	struct c2wr_hdr hdr;
-} __attribute__((packed));
-
-union c2wr_cr_reject {
-	struct c2wr_cr_reject_req req;
-	struct c2wr_cr_reject_rep rep;
-} __attribute__((packed));
-
-/*
- * console command.  Used to implement a debug console over the verbs
- * request and reply queues.
- */
-
-/*
- * Console request message.  It contains:
- *	- message hdr with id = CCWR_CONSOLE
- *	- the physaddr/len of host memory to be used for the reply.
- *	- the command string.  eg:  "netstat -s" or "zoneinfo"
- */
-struct c2wr_console_req {
-	struct c2wr_hdr hdr;		/* id = CCWR_CONSOLE */
-	u64 reply_buf;		/* pinned host buf for reply */
-	u32 reply_buf_len;	/* length of reply buffer */
-	u8 command[0];		/* NUL terminated ascii string */
-	/* containing the command req */
-} __attribute__((packed));
-
-/*
- * flags used in the console reply.
- */
-enum c2_console_flags {
-	CONS_REPLY_TRUNCATED = 0x00000001	/* reply was truncated */
-} __attribute__((packed));
-
-/*
- * Console reply message.
- * hdr.result contains the c2_status_t error if the reply was _not_ generated,
- * or C2_OK if the reply was generated.
- */
-struct c2wr_console_rep {
-	struct c2wr_hdr hdr;		/* id = CCWR_CONSOLE */
-	u32 flags;
-} __attribute__((packed));
-
-union c2wr_console {
-	struct c2wr_console_req req;
-	struct c2wr_console_rep rep;
-} __attribute__((packed));
-
-
-/*
- * Giant union with all WRs.  Makes life easier...
- */
-union c2wr {
-	struct c2wr_hdr hdr;
-	struct c2wr_user_hdr user_hdr;
-	union c2wr_rnic_open rnic_open;
-	union c2wr_rnic_query rnic_query;
-	union c2wr_rnic_getconfig rnic_getconfig;
-	union c2wr_rnic_setconfig rnic_setconfig;
-	union c2wr_rnic_close rnic_close;
-	union c2wr_cq_create cq_create;
-	union c2wr_cq_modify cq_modify;
-	union c2wr_cq_destroy cq_destroy;
-	union c2wr_pd_alloc pd_alloc;
-	union c2wr_pd_dealloc pd_dealloc;
-	union c2wr_srq_create srq_create;
-	union c2wr_srq_destroy srq_destroy;
-	union c2wr_qp_create qp_create;
-	union c2wr_qp_query qp_query;
-	union c2wr_qp_modify qp_modify;
-	union c2wr_qp_destroy qp_destroy;
-	struct c2wr_qp_connect qp_connect;
-	union c2wr_nsmr_stag_alloc nsmr_stag_alloc;
-	union c2wr_nsmr_register nsmr_register;
-	union c2wr_nsmr_pbl nsmr_pbl;
-	union c2wr_mr_query mr_query;
-	union c2wr_mw_query mw_query;
-	union c2wr_stag_dealloc stag_dealloc;
-	union c2wr_sqwr sqwr;
-	struct c2wr_rqwr rqwr;
-	struct c2wr_ce ce;
-	union c2wr_ae ae;
-	union c2wr_init init;
-	union c2wr_ep_listen_create ep_listen_create;
-	union c2wr_ep_listen_destroy ep_listen_destroy;
-	union c2wr_cr_accept cr_accept;
-	union c2wr_cr_reject cr_reject;
-	union c2wr_console console;
-	union c2wr_flash_init flash_init;
-	union c2wr_flash flash;
-	union c2wr_buf_alloc buf_alloc;
-	union c2wr_buf_free buf_free;
-	union c2wr_flash_write flash_write;
-} __attribute__((packed));
-
-
-/*
- * Accessors for the wr fields that are packed together tightly to
- * reduce the wr message size.  The wr arguments are void* so that
- * either a struct c2wr*, a struct c2wr_hdr*, or a pointer to any of the types
- * in the struct c2wr union can be passed in.
- */
-static __inline__ u8 c2_wr_get_id(void *wr)
-{
-	return ((struct c2wr_hdr *) wr)->id;
-}
-static __inline__ void c2_wr_set_id(void *wr, u8 id)
-{
-	((struct c2wr_hdr *) wr)->id = id;
-}
-static __inline__ u8 c2_wr_get_result(void *wr)
-{
-	return ((struct c2wr_hdr *) wr)->result;
-}
-static __inline__ void c2_wr_set_result(void *wr, u8 result)
-{
-	((struct c2wr_hdr *) wr)->result = result;
-}
-static __inline__ u8 c2_wr_get_flags(void *wr)
-{
-	return ((struct c2wr_hdr *) wr)->flags;
-}
-static __inline__ void c2_wr_set_flags(void *wr, u8 flags)
-{
-	((struct c2wr_hdr *) wr)->flags = flags;
-}
-static __inline__ u8 c2_wr_get_sge_count(void *wr)
-{
-	return ((struct c2wr_hdr *) wr)->sge_count;
-}
-static __inline__ void c2_wr_set_sge_count(void *wr, u8 sge_count)
-{
-	((struct c2wr_hdr *) wr)->sge_count = sge_count;
-}
-static __inline__ __be32 c2_wr_get_wqe_count(void *wr)
-{
-	return ((struct c2wr_hdr *) wr)->wqe_count;
-}
-static __inline__ void c2_wr_set_wqe_count(void *wr, u32 wqe_count)
-{
-	((struct c2wr_hdr *) wr)->wqe_count = wqe_count;
-}
-
-#endif				/* _C2_WR_H_ */
diff --git a/drivers/staging/rdma/ehca/Kconfig b/drivers/staging/rdma/ehca/Kconfig
deleted file mode 100644
index 3fadd2ad6426..000000000000
--- a/drivers/staging/rdma/ehca/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-config INFINIBAND_EHCA
-	tristate "eHCA support"
-	depends on IBMEBUS
-	---help---
-	This driver supports the deprecated IBM pSeries eHCA InfiniBand
-	adapter.
-
-	To compile the driver as a module, choose M here. The module
-	will be called ib_ehca.
-
diff --git a/drivers/staging/rdma/ehca/Makefile b/drivers/staging/rdma/ehca/Makefile
deleted file mode 100644
index 74d284e46a40..000000000000
--- a/drivers/staging/rdma/ehca/Makefile
+++ /dev/null
@@ -1,16 +0,0 @@
-#  Authors: Heiko J Schick <schickhj@de.ibm.com>
-#           Christoph Raisch <raisch@de.ibm.com>
-#           Joachim Fenkes <fenkes@de.ibm.com>
-#
-#  Copyright (c) 2005 IBM Corporation
-#
-#  All rights reserved.
-#
-#  This source code is distributed under a dual license of GPL v2.0 and OpenIB BSD.
-
-obj-$(CONFIG_INFINIBAND_EHCA) += ib_ehca.o
-
-ib_ehca-objs  = ehca_main.o ehca_hca.o ehca_mcast.o ehca_pd.o ehca_av.o ehca_eq.o \
-		ehca_cq.o ehca_qp.o ehca_sqp.o ehca_mrmw.o ehca_reqs.o ehca_irq.o \
-		ehca_uverbs.o ipz_pt_fn.o hcp_if.o hcp_phyp.o
-
diff --git a/drivers/staging/rdma/ehca/TODO b/drivers/staging/rdma/ehca/TODO
deleted file mode 100644
index 199a4a600142..000000000000
--- a/drivers/staging/rdma/ehca/TODO
+++ /dev/null
@@ -1,4 +0,0 @@
-9/2015
-
-The ehca driver has been deprecated and moved to drivers/staging/rdma.
-It will be removed in the 4.6 merge window.
diff --git a/drivers/staging/rdma/ehca/ehca_av.c b/drivers/staging/rdma/ehca/ehca_av.c
deleted file mode 100644
index 465926319f3d..000000000000
--- a/drivers/staging/rdma/ehca/ehca_av.c
+++ /dev/null
@@ -1,277 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  address vector functions
- *
- *  Authors: Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Khadija Souissi <souissik@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_tools.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-
-static struct kmem_cache *av_cache;
-
-int ehca_calc_ipd(struct ehca_shca *shca, int port,
-		  enum ib_rate path_rate, u32 *ipd)
-{
-	int path = ib_rate_to_mult(path_rate);
-	int link, ret;
-	struct ib_port_attr pa;
-
-	if (path_rate == IB_RATE_PORT_CURRENT) {
-		*ipd = 0;
-		return 0;
-	}
-
-	if (unlikely(path < 0)) {
-		ehca_err(&shca->ib_device, "Invalid static rate! path_rate=%x",
-			 path_rate);
-		return -EINVAL;
-	}
-
-	ret = ehca_query_port(&shca->ib_device, port, &pa);
-	if (unlikely(ret < 0)) {
-		ehca_err(&shca->ib_device, "Failed to query port  ret=%i", ret);
-		return ret;
-	}
-
-	link = ib_width_enum_to_int(pa.active_width) * pa.active_speed;
-
-	if (path >= link)
-		/* no need to throttle if path faster than link */
-		*ipd = 0;
-	else
-		/* IPD = round((link / path) - 1) */
-		*ipd = ((link + (path >> 1)) / path) - 1;
-
-	return 0;
-}
-
-struct ib_ah *ehca_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr)
-{
-	int ret;
-	struct ehca_av *av;
-	struct ehca_shca *shca = container_of(pd->device, struct ehca_shca,
-					      ib_device);
-
-	av = kmem_cache_alloc(av_cache, GFP_KERNEL);
-	if (!av) {
-		ehca_err(pd->device, "Out of memory pd=%p ah_attr=%p",
-			 pd, ah_attr);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	av->av.sl = ah_attr->sl;
-	av->av.dlid = ah_attr->dlid;
-	av->av.slid_path_bits = ah_attr->src_path_bits;
-
-	if (ehca_static_rate < 0) {
-		u32 ipd;
-		if (ehca_calc_ipd(shca, ah_attr->port_num,
-				  ah_attr->static_rate, &ipd)) {
-			ret = -EINVAL;
-			goto create_ah_exit1;
-		}
-		av->av.ipd = ipd;
-	} else
-		av->av.ipd = ehca_static_rate;
-
-	av->av.lnh = ah_attr->ah_flags;
-	av->av.grh.word_0 = EHCA_BMASK_SET(GRH_IPVERSION_MASK, 6);
-	av->av.grh.word_0 |= EHCA_BMASK_SET(GRH_TCLASS_MASK,
-					    ah_attr->grh.traffic_class);
-	av->av.grh.word_0 |= EHCA_BMASK_SET(GRH_FLOWLABEL_MASK,
-					    ah_attr->grh.flow_label);
-	av->av.grh.word_0 |= EHCA_BMASK_SET(GRH_HOPLIMIT_MASK,
-					    ah_attr->grh.hop_limit);
-	av->av.grh.word_0 |= EHCA_BMASK_SET(GRH_NEXTHEADER_MASK, 0x1B);
-	/* set sgid in grh.word_1 */
-	if (ah_attr->ah_flags & IB_AH_GRH) {
-		int rc;
-		struct ib_port_attr port_attr;
-		union ib_gid gid;
-		memset(&port_attr, 0, sizeof(port_attr));
-		rc = ehca_query_port(pd->device, ah_attr->port_num,
-				     &port_attr);
-		if (rc) { /* invalid port number */
-			ret = -EINVAL;
-			ehca_err(pd->device, "Invalid port number "
-				 "ehca_query_port() returned %x "
-				 "pd=%p ah_attr=%p", rc, pd, ah_attr);
-			goto create_ah_exit1;
-		}
-		memset(&gid, 0, sizeof(gid));
-		rc = ehca_query_gid(pd->device,
-				    ah_attr->port_num,
-				    ah_attr->grh.sgid_index, &gid);
-		if (rc) {
-			ret = -EINVAL;
-			ehca_err(pd->device, "Failed to retrieve sgid "
-				 "ehca_query_gid() returned %x "
-				 "pd=%p ah_attr=%p", rc, pd, ah_attr);
-			goto create_ah_exit1;
-		}
-		memcpy(&av->av.grh.word_1, &gid, sizeof(gid));
-	}
-	av->av.pmtu = shca->max_mtu;
-
-	/* dgid comes in grh.word_3 */
-	memcpy(&av->av.grh.word_3, &ah_attr->grh.dgid,
-	       sizeof(ah_attr->grh.dgid));
-
-	return &av->ib_ah;
-
-create_ah_exit1:
-	kmem_cache_free(av_cache, av);
-
-	return ERR_PTR(ret);
-}
-
-int ehca_modify_ah(struct ib_ah *ah, struct ib_ah_attr *ah_attr)
-{
-	struct ehca_av *av;
-	struct ehca_ud_av new_ehca_av;
-	struct ehca_shca *shca = container_of(ah->pd->device, struct ehca_shca,
-					      ib_device);
-
-	memset(&new_ehca_av, 0, sizeof(new_ehca_av));
-	new_ehca_av.sl = ah_attr->sl;
-	new_ehca_av.dlid = ah_attr->dlid;
-	new_ehca_av.slid_path_bits = ah_attr->src_path_bits;
-	new_ehca_av.ipd = ah_attr->static_rate;
-	new_ehca_av.lnh = EHCA_BMASK_SET(GRH_FLAG_MASK,
-					 (ah_attr->ah_flags & IB_AH_GRH) > 0);
-	new_ehca_av.grh.word_0 = EHCA_BMASK_SET(GRH_TCLASS_MASK,
-						ah_attr->grh.traffic_class);
-	new_ehca_av.grh.word_0 |= EHCA_BMASK_SET(GRH_FLOWLABEL_MASK,
-						 ah_attr->grh.flow_label);
-	new_ehca_av.grh.word_0 |= EHCA_BMASK_SET(GRH_HOPLIMIT_MASK,
-						 ah_attr->grh.hop_limit);
-	new_ehca_av.grh.word_0 |= EHCA_BMASK_SET(GRH_NEXTHEADER_MASK, 0x1b);
-
-	/* set sgid in grh.word_1 */
-	if (ah_attr->ah_flags & IB_AH_GRH) {
-		int rc;
-		struct ib_port_attr port_attr;
-		union ib_gid gid;
-		memset(&port_attr, 0, sizeof(port_attr));
-		rc = ehca_query_port(ah->device, ah_attr->port_num,
-				     &port_attr);
-		if (rc) { /* invalid port number */
-			ehca_err(ah->device, "Invalid port number "
-				 "ehca_query_port() returned %x "
-				 "ah=%p ah_attr=%p port_num=%x",
-				 rc, ah, ah_attr, ah_attr->port_num);
-			return -EINVAL;
-		}
-		memset(&gid, 0, sizeof(gid));
-		rc = ehca_query_gid(ah->device,
-				    ah_attr->port_num,
-				    ah_attr->grh.sgid_index, &gid);
-		if (rc) {
-			ehca_err(ah->device, "Failed to retrieve sgid "
-				 "ehca_query_gid() returned %x "
-				 "ah=%p ah_attr=%p port_num=%x "
-				 "sgid_index=%x",
-				 rc, ah, ah_attr, ah_attr->port_num,
-				 ah_attr->grh.sgid_index);
-			return -EINVAL;
-		}
-		memcpy(&new_ehca_av.grh.word_1, &gid, sizeof(gid));
-	}
-
-	new_ehca_av.pmtu = shca->max_mtu;
-
-	memcpy(&new_ehca_av.grh.word_3, &ah_attr->grh.dgid,
-	       sizeof(ah_attr->grh.dgid));
-
-	av = container_of(ah, struct ehca_av, ib_ah);
-	av->av = new_ehca_av;
-
-	return 0;
-}
-
-int ehca_query_ah(struct ib_ah *ah, struct ib_ah_attr *ah_attr)
-{
-	struct ehca_av *av = container_of(ah, struct ehca_av, ib_ah);
-
-	memcpy(&ah_attr->grh.dgid, &av->av.grh.word_3,
-	       sizeof(ah_attr->grh.dgid));
-	ah_attr->sl = av->av.sl;
-
-	ah_attr->dlid = av->av.dlid;
-
-	ah_attr->src_path_bits = av->av.slid_path_bits;
-	ah_attr->static_rate = av->av.ipd;
-	ah_attr->ah_flags = EHCA_BMASK_GET(GRH_FLAG_MASK, av->av.lnh);
-	ah_attr->grh.traffic_class = EHCA_BMASK_GET(GRH_TCLASS_MASK,
-						    av->av.grh.word_0);
-	ah_attr->grh.hop_limit = EHCA_BMASK_GET(GRH_HOPLIMIT_MASK,
-						av->av.grh.word_0);
-	ah_attr->grh.flow_label = EHCA_BMASK_GET(GRH_FLOWLABEL_MASK,
-						 av->av.grh.word_0);
-
-	return 0;
-}
-
-int ehca_destroy_ah(struct ib_ah *ah)
-{
-	kmem_cache_free(av_cache, container_of(ah, struct ehca_av, ib_ah));
-
-	return 0;
-}
-
-int ehca_init_av_cache(void)
-{
-	av_cache = kmem_cache_create("ehca_cache_av",
-				   sizeof(struct ehca_av), 0,
-				   SLAB_HWCACHE_ALIGN,
-				   NULL);
-	if (!av_cache)
-		return -ENOMEM;
-	return 0;
-}
-
-void ehca_cleanup_av_cache(void)
-{
-	if (av_cache)
-		kmem_cache_destroy(av_cache);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_classes.h b/drivers/staging/rdma/ehca/ehca_classes.h
deleted file mode 100644
index bd45e0f3923f..000000000000
--- a/drivers/staging/rdma/ehca/ehca_classes.h
+++ /dev/null
@@ -1,482 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Struct definition for eHCA internal structures
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *           Joachim Fenkes <fenkes@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __EHCA_CLASSES_H__
-#define __EHCA_CLASSES_H__
-
-struct ehca_module;
-struct ehca_qp;
-struct ehca_cq;
-struct ehca_eq;
-struct ehca_mr;
-struct ehca_mw;
-struct ehca_pd;
-struct ehca_av;
-
-#include <linux/wait.h>
-#include <linux/mutex.h>
-
-#include <rdma/ib_verbs.h>
-#include <rdma/ib_user_verbs.h>
-
-#ifdef CONFIG_PPC64
-#include "ehca_classes_pSeries.h"
-#endif
-#include "ipz_pt_fn.h"
-#include "ehca_qes.h"
-#include "ehca_irq.h"
-
-#define EHCA_EQE_CACHE_SIZE 20
-#define EHCA_MAX_NUM_QUEUES 0xffff
-
-struct ehca_eqe_cache_entry {
-	struct ehca_eqe *eqe;
-	struct ehca_cq *cq;
-};
-
-struct ehca_eq {
-	u32 length;
-	struct ipz_queue ipz_queue;
-	struct ipz_eq_handle ipz_eq_handle;
-	struct work_struct work;
-	struct h_galpas galpas;
-	int is_initialized;
-	struct ehca_pfeq pf;
-	spinlock_t spinlock;
-	struct tasklet_struct interrupt_task;
-	u32 ist;
-	spinlock_t irq_spinlock;
-	struct ehca_eqe_cache_entry eqe_cache[EHCA_EQE_CACHE_SIZE];
-};
-
-struct ehca_sma_attr {
-	u16 lid, lmc, sm_sl, sm_lid;
-	u16 pkey_tbl_len, pkeys[16];
-};
-
-struct ehca_sport {
-	struct ib_cq *ibcq_aqp1;
-	struct ib_qp *ibqp_sqp[2];
-	/* lock to serialze modify_qp() calls for sqp in normal
-	 * and irq path (when event PORT_ACTIVE is received first time)
-	 */
-	spinlock_t mod_sqp_lock;
-	enum ib_port_state port_state;
-	struct ehca_sma_attr saved_attr;
-	u32 pma_qp_nr;
-};
-
-#define HCA_CAP_MR_PGSIZE_4K  0x80000000
-#define HCA_CAP_MR_PGSIZE_64K 0x40000000
-#define HCA_CAP_MR_PGSIZE_1M  0x20000000
-#define HCA_CAP_MR_PGSIZE_16M 0x10000000
-
-struct ehca_shca {
-	struct ib_device ib_device;
-	struct platform_device *ofdev;
-	u8 num_ports;
-	int hw_level;
-	struct list_head shca_list;
-	struct ipz_adapter_handle ipz_hca_handle;
-	struct ehca_sport sport[2];
-	struct ehca_eq eq;
-	struct ehca_eq neq;
-	struct ehca_mr *maxmr;
-	struct ehca_pd *pd;
-	struct h_galpas galpas;
-	struct mutex modify_mutex;
-	u64 hca_cap;
-	/* MR pgsize: bit 0-3 means 4K, 64K, 1M, 16M respectively */
-	u32 hca_cap_mr_pgsize;
-	int max_mtu;
-	int max_num_qps;
-	int max_num_cqs;
-	atomic_t num_cqs;
-	atomic_t num_qps;
-};
-
-struct ehca_pd {
-	struct ib_pd ib_pd;
-	struct ipz_pd fw_pd;
-	/* small queue mgmt */
-	struct mutex lock;
-	struct list_head free[2];
-	struct list_head full[2];
-};
-
-enum ehca_ext_qp_type {
-	EQPT_NORMAL    = 0,
-	EQPT_LLQP      = 1,
-	EQPT_SRQBASE   = 2,
-	EQPT_SRQ       = 3,
-};
-
-/* struct to cache modify_qp()'s parms for GSI/SMI qp */
-struct ehca_mod_qp_parm {
-	int mask;
-	struct ib_qp_attr attr;
-};
-
-#define EHCA_MOD_QP_PARM_MAX 4
-
-#define QMAP_IDX_MASK 0xFFFFULL
-
-/* struct for tracking if cqes have been reported to the application */
-struct ehca_qmap_entry {
-	u16 app_wr_id;
-	u8 reported;
-	u8 cqe_req;
-};
-
-struct ehca_queue_map {
-	struct ehca_qmap_entry *map;
-	unsigned int entries;
-	unsigned int tail;
-	unsigned int left_to_poll;
-	unsigned int next_wqe_idx;   /* Idx to first wqe to be flushed */
-};
-
-/* function to calculate the next index for the qmap */
-static inline unsigned int next_index(unsigned int cur_index, unsigned int limit)
-{
-	unsigned int temp = cur_index + 1;
-	return (temp == limit) ? 0 : temp;
-}
-
-struct ehca_qp {
-	union {
-		struct ib_qp ib_qp;
-		struct ib_srq ib_srq;
-	};
-	u32 qp_type;
-	enum ehca_ext_qp_type ext_type;
-	enum ib_qp_state state;
-	struct ipz_queue ipz_squeue;
-	struct ehca_queue_map sq_map;
-	struct ipz_queue ipz_rqueue;
-	struct ehca_queue_map rq_map;
-	struct h_galpas galpas;
-	u32 qkey;
-	u32 real_qp_num;
-	u32 token;
-	spinlock_t spinlock_s;
-	spinlock_t spinlock_r;
-	u32 sq_max_inline_data_size;
-	struct ipz_qp_handle ipz_qp_handle;
-	struct ehca_pfqp pf;
-	struct ib_qp_init_attr init_attr;
-	struct ehca_cq *send_cq;
-	struct ehca_cq *recv_cq;
-	unsigned int sqerr_purgeflag;
-	struct hlist_node list_entries;
-	/* array to cache modify_qp()'s parms for GSI/SMI qp */
-	struct ehca_mod_qp_parm *mod_qp_parm;
-	int mod_qp_parm_idx;
-	/* mmap counter for resources mapped into user space */
-	u32 mm_count_squeue;
-	u32 mm_count_rqueue;
-	u32 mm_count_galpa;
-	/* unsolicited ack circumvention */
-	int unsol_ack_circ;
-	int mtu_shift;
-	u32 message_count;
-	u32 packet_count;
-	atomic_t nr_events; /* events seen */
-	wait_queue_head_t wait_completion;
-	int mig_armed;
-	struct list_head sq_err_node;
-	struct list_head rq_err_node;
-};
-
-#define IS_SRQ(qp) (qp->ext_type == EQPT_SRQ)
-#define HAS_SQ(qp) (qp->ext_type != EQPT_SRQ)
-#define HAS_RQ(qp) (qp->ext_type != EQPT_SRQBASE)
-
-/* must be power of 2 */
-#define QP_HASHTAB_LEN 8
-
-struct ehca_cq {
-	struct ib_cq ib_cq;
-	struct ipz_queue ipz_queue;
-	struct h_galpas galpas;
-	spinlock_t spinlock;
-	u32 cq_number;
-	u32 token;
-	u32 nr_of_entries;
-	struct ipz_cq_handle ipz_cq_handle;
-	struct ehca_pfcq pf;
-	spinlock_t cb_lock;
-	struct hlist_head qp_hashtab[QP_HASHTAB_LEN];
-	struct list_head entry;
-	u32 nr_callbacks;   /* #events assigned to cpu by scaling code */
-	atomic_t nr_events; /* #events seen */
-	wait_queue_head_t wait_completion;
-	spinlock_t task_lock;
-	/* mmap counter for resources mapped into user space */
-	u32 mm_count_queue;
-	u32 mm_count_galpa;
-	struct list_head sqp_err_list;
-	struct list_head rqp_err_list;
-};
-
-enum ehca_mr_flag {
-	EHCA_MR_FLAG_FMR = 0x80000000,	 /* FMR, created with ehca_alloc_fmr */
-	EHCA_MR_FLAG_MAXMR = 0x40000000, /* max-MR                           */
-};
-
-struct ehca_mr {
-	union {
-		struct ib_mr ib_mr;	/* must always be first in ehca_mr */
-		struct ib_fmr ib_fmr;	/* must always be first in ehca_mr */
-	} ib;
-	struct ib_umem *umem;
-	spinlock_t mrlock;
-
-	enum ehca_mr_flag flags;
-	u32 num_kpages;		/* number of kernel pages */
-	u32 num_hwpages;	/* number of hw pages to form MR */
-	u64 hwpage_size;	/* hw page size used for this MR */
-	int acl;		/* ACL (stored here for usage in reregister) */
-	u64 *start;		/* virtual start address (stored here for */
-				/* usage in reregister) */
-	u64 size;		/* size (stored here for usage in reregister) */
-	u32 fmr_page_size;	/* page size for FMR */
-	u32 fmr_max_pages;	/* max pages for FMR */
-	u32 fmr_max_maps;	/* max outstanding maps for FMR */
-	u32 fmr_map_cnt;	/* map counter for FMR */
-	/* fw specific data */
-	struct ipz_mrmw_handle ipz_mr_handle;	/* MR handle for h-calls */
-	struct h_galpas galpas;
-};
-
-struct ehca_mw {
-	struct ib_mw ib_mw;	/* gen2 mw, must always be first in ehca_mw */
-	spinlock_t mwlock;
-
-	u8 never_bound;		/* indication MW was never bound */
-	struct ipz_mrmw_handle ipz_mw_handle;	/* MW handle for h-calls */
-	struct h_galpas galpas;
-};
-
-enum ehca_mr_pgi_type {
-	EHCA_MR_PGI_PHYS   = 1,  /* type of ehca_reg_phys_mr,
-				  * ehca_rereg_phys_mr,
-				  * ehca_reg_internal_maxmr */
-	EHCA_MR_PGI_USER   = 2,  /* type of ehca_reg_user_mr */
-	EHCA_MR_PGI_FMR    = 3   /* type of ehca_map_phys_fmr */
-};
-
-struct ehca_mr_pginfo {
-	enum ehca_mr_pgi_type type;
-	u64 num_kpages;
-	u64 kpage_cnt;
-	u64 hwpage_size;     /* hw page size used for this MR */
-	u64 num_hwpages;     /* number of hw pages */
-	u64 hwpage_cnt;      /* counter for hw pages */
-	u64 next_hwpage;     /* next hw page in buffer/chunk/listelem */
-
-	union {
-		struct { /* type EHCA_MR_PGI_PHYS section */
-			int num_phys_buf;
-			struct ib_phys_buf *phys_buf_array;
-			u64 next_buf;
-		} phy;
-		struct { /* type EHCA_MR_PGI_USER section */
-			struct ib_umem *region;
-			struct scatterlist *next_sg;
-			u64 next_nmap;
-		} usr;
-		struct { /* type EHCA_MR_PGI_FMR section */
-			u64 fmr_pgsize;
-			u64 *page_list;
-			u64 next_listelem;
-		} fmr;
-	} u;
-};
-
-/* output parameters for MR/FMR hipz calls */
-struct ehca_mr_hipzout_parms {
-	struct ipz_mrmw_handle handle;
-	u32 lkey;
-	u32 rkey;
-	u64 len;
-	u64 vaddr;
-	u32 acl;
-};
-
-/* output parameters for MW hipz calls */
-struct ehca_mw_hipzout_parms {
-	struct ipz_mrmw_handle handle;
-	u32 rkey;
-};
-
-struct ehca_av {
-	struct ib_ah ib_ah;
-	struct ehca_ud_av av;
-};
-
-struct ehca_ucontext {
-	struct ib_ucontext ib_ucontext;
-};
-
-int ehca_init_pd_cache(void);
-void ehca_cleanup_pd_cache(void);
-int ehca_init_cq_cache(void);
-void ehca_cleanup_cq_cache(void);
-int ehca_init_qp_cache(void);
-void ehca_cleanup_qp_cache(void);
-int ehca_init_av_cache(void);
-void ehca_cleanup_av_cache(void);
-int ehca_init_mrmw_cache(void);
-void ehca_cleanup_mrmw_cache(void);
-int ehca_init_small_qp_cache(void);
-void ehca_cleanup_small_qp_cache(void);
-
-extern rwlock_t ehca_qp_idr_lock;
-extern rwlock_t ehca_cq_idr_lock;
-extern struct idr ehca_qp_idr;
-extern struct idr ehca_cq_idr;
-extern spinlock_t shca_list_lock;
-
-extern int ehca_static_rate;
-extern int ehca_port_act_time;
-extern bool ehca_use_hp_mr;
-extern bool ehca_scaling_code;
-extern int ehca_lock_hcalls;
-extern int ehca_nr_ports;
-extern int ehca_max_cq;
-extern int ehca_max_qp;
-
-struct ipzu_queue_resp {
-	u32 qe_size;      /* queue entry size */
-	u32 act_nr_of_sg;
-	u32 queue_length; /* queue length allocated in bytes */
-	u32 pagesize;
-	u32 toggle_state;
-	u32 offset; /* save offset within a page for small_qp */
-};
-
-struct ehca_create_cq_resp {
-	u32 cq_number;
-	u32 token;
-	struct ipzu_queue_resp ipz_queue;
-	u32 fw_handle_ofs;
-	u32 dummy;
-};
-
-struct ehca_create_qp_resp {
-	u32 qp_num;
-	u32 token;
-	u32 qp_type;
-	u32 ext_type;
-	u32 qkey;
-	/* qp_num assigned by ehca: sqp0/1 may have got different numbers */
-	u32 real_qp_num;
-	u32 fw_handle_ofs;
-	u32 dummy;
-	struct ipzu_queue_resp ipz_squeue;
-	struct ipzu_queue_resp ipz_rqueue;
-};
-
-struct ehca_alloc_cq_parms {
-	u32 nr_cqe;
-	u32 act_nr_of_entries;
-	u32 act_pages;
-	struct ipz_eq_handle eq_handle;
-};
-
-enum ehca_service_type {
-	ST_RC  = 0,
-	ST_UC  = 1,
-	ST_RD  = 2,
-	ST_UD  = 3,
-};
-
-enum ehca_ll_comp_flags {
-	LLQP_SEND_COMP = 0x20,
-	LLQP_RECV_COMP = 0x40,
-	LLQP_COMP_MASK = 0x60,
-};
-
-struct ehca_alloc_queue_parms {
-	/* input parameters */
-	int max_wr;
-	int max_sge;
-	int page_size;
-	int is_small;
-
-	/* output parameters */
-	u16 act_nr_wqes;
-	u8  act_nr_sges;
-	u32 queue_size; /* bytes for small queues, pages otherwise */
-};
-
-struct ehca_alloc_qp_parms {
-	struct ehca_alloc_queue_parms squeue;
-	struct ehca_alloc_queue_parms rqueue;
-
-	/* input parameters */
-	enum ehca_service_type servicetype;
-	int qp_storage;
-	int sigtype;
-	enum ehca_ext_qp_type ext_type;
-	enum ehca_ll_comp_flags ll_comp_flags;
-	int ud_av_l_key_ctl;
-
-	u32 token;
-	struct ipz_eq_handle eq_handle;
-	struct ipz_pd pd;
-	struct ipz_cq_handle send_cq_handle, recv_cq_handle;
-
-	u32 srq_qpn, srq_token, srq_limit;
-
-	/* output parameters */
-	u32 real_qp_num;
-	struct ipz_qp_handle qp_handle;
-	struct h_galpas galpas;
-};
-
-int ehca_cq_assign_qp(struct ehca_cq *cq, struct ehca_qp *qp);
-int ehca_cq_unassign_qp(struct ehca_cq *cq, unsigned int qp_num);
-struct ehca_qp *ehca_cq_get_qp(struct ehca_cq *cq, int qp_num);
-
-#endif
diff --git a/drivers/staging/rdma/ehca/ehca_classes_pSeries.h b/drivers/staging/rdma/ehca/ehca_classes_pSeries.h
deleted file mode 100644
index 689c35786dd2..000000000000
--- a/drivers/staging/rdma/ehca/ehca_classes_pSeries.h
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  pSeries interface definitions
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __EHCA_CLASSES_PSERIES_H__
-#define __EHCA_CLASSES_PSERIES_H__
-
-#include "hcp_phyp.h"
-#include "ipz_pt_fn.h"
-
-
-struct ehca_pfqp {
-	struct ipz_qpt sqpt;
-	struct ipz_qpt rqpt;
-};
-
-struct ehca_pfcq {
-	struct ipz_qpt qpt;
-	u32 cqnr;
-};
-
-struct ehca_pfeq {
-	struct ipz_qpt qpt;
-	struct h_galpa galpa;
-	u32 eqnr;
-};
-
-struct ipz_adapter_handle {
-	u64 handle;
-};
-
-struct ipz_cq_handle {
-	u64 handle;
-};
-
-struct ipz_eq_handle {
-	u64 handle;
-};
-
-struct ipz_qp_handle {
-	u64 handle;
-};
-struct ipz_mrmw_handle {
-	u64 handle;
-};
-
-struct ipz_pd {
-	u32 value;
-};
-
-struct hcp_modify_qp_control_block {
-	u32 qkey;                      /* 00 */
-	u32 rdd;                       /* reliable datagram domain */
-	u32 send_psn;                  /* 02 */
-	u32 receive_psn;               /* 03 */
-	u32 prim_phys_port;            /* 04 */
-	u32 alt_phys_port;             /* 05 */
-	u32 prim_p_key_idx;            /* 06 */
-	u32 alt_p_key_idx;             /* 07 */
-	u32 rdma_atomic_ctrl;          /* 08 */
-	u32 qp_state;                  /* 09 */
-	u32 reserved_10;               /* 10 */
-	u32 rdma_nr_atomic_resp_res;   /* 11 */
-	u32 path_migration_state;      /* 12 */
-	u32 rdma_atomic_outst_dest_qp; /* 13 */
-	u32 dest_qp_nr;                /* 14 */
-	u32 min_rnr_nak_timer_field;   /* 15 */
-	u32 service_level;             /* 16 */
-	u32 send_grh_flag;             /* 17 */
-	u32 retry_count;               /* 18 */
-	u32 timeout;                   /* 19 */
-	u32 path_mtu;                  /* 20 */
-	u32 max_static_rate;           /* 21 */
-	u32 dlid;                      /* 22 */
-	u32 rnr_retry_count;           /* 23 */
-	u32 source_path_bits;          /* 24 */
-	u32 traffic_class;             /* 25 */
-	u32 hop_limit;                 /* 26 */
-	u32 source_gid_idx;            /* 27 */
-	u32 flow_label;                /* 28 */
-	u32 reserved_29;               /* 29 */
-	union {                        /* 30 */
-		u64 dw[2];
-		u8 byte[16];
-	} dest_gid;
-	u32 service_level_al;          /* 34 */
-	u32 send_grh_flag_al;          /* 35 */
-	u32 retry_count_al;            /* 36 */
-	u32 timeout_al;                /* 37 */
-	u32 max_static_rate_al;        /* 38 */
-	u32 dlid_al;                   /* 39 */
-	u32 rnr_retry_count_al;        /* 40 */
-	u32 source_path_bits_al;       /* 41 */
-	u32 traffic_class_al;          /* 42 */
-	u32 hop_limit_al;              /* 43 */
-	u32 source_gid_idx_al;         /* 44 */
-	u32 flow_label_al;             /* 45 */
-	u32 reserved_46;               /* 46 */
-	u32 reserved_47;               /* 47 */
-	union {                        /* 48 */
-		u64 dw[2];
-		u8 byte[16];
-	} dest_gid_al;
-	u32 max_nr_outst_send_wr;      /* 52 */
-	u32 max_nr_outst_recv_wr;      /* 53 */
-	u32 disable_ete_credit_check;  /* 54 */
-	u32 qp_number;                 /* 55 */
-	u64 send_queue_handle;         /* 56 */
-	u64 recv_queue_handle;         /* 58 */
-	u32 actual_nr_sges_in_sq_wqe;  /* 60 */
-	u32 actual_nr_sges_in_rq_wqe;  /* 61 */
-	u32 qp_enable;                 /* 62 */
-	u32 curr_srq_limit;            /* 63 */
-	u64 qp_aff_asyn_ev_log_reg;    /* 64 */
-	u64 shared_rq_hndl;            /* 66 */
-	u64 trigg_doorbell_qp_hndl;    /* 68 */
-	u32 reserved_70_127[58];       /* 70 */
-};
-
-#define MQPCB_MASK_QKEY                         EHCA_BMASK_IBM( 0,  0)
-#define MQPCB_MASK_SEND_PSN                     EHCA_BMASK_IBM( 2,  2)
-#define MQPCB_MASK_RECEIVE_PSN                  EHCA_BMASK_IBM( 3,  3)
-#define MQPCB_MASK_PRIM_PHYS_PORT               EHCA_BMASK_IBM( 4,  4)
-#define MQPCB_PRIM_PHYS_PORT                    EHCA_BMASK_IBM(24, 31)
-#define MQPCB_MASK_ALT_PHYS_PORT                EHCA_BMASK_IBM( 5,  5)
-#define MQPCB_MASK_PRIM_P_KEY_IDX               EHCA_BMASK_IBM( 6,  6)
-#define MQPCB_PRIM_P_KEY_IDX                    EHCA_BMASK_IBM(24, 31)
-#define MQPCB_MASK_ALT_P_KEY_IDX                EHCA_BMASK_IBM( 7,  7)
-#define MQPCB_MASK_RDMA_ATOMIC_CTRL             EHCA_BMASK_IBM( 8,  8)
-#define MQPCB_MASK_QP_STATE                     EHCA_BMASK_IBM( 9,  9)
-#define MQPCB_MASK_RDMA_NR_ATOMIC_RESP_RES      EHCA_BMASK_IBM(11, 11)
-#define MQPCB_MASK_PATH_MIGRATION_STATE         EHCA_BMASK_IBM(12, 12)
-#define MQPCB_MASK_RDMA_ATOMIC_OUTST_DEST_QP    EHCA_BMASK_IBM(13, 13)
-#define MQPCB_MASK_DEST_QP_NR                   EHCA_BMASK_IBM(14, 14)
-#define MQPCB_MASK_MIN_RNR_NAK_TIMER_FIELD      EHCA_BMASK_IBM(15, 15)
-#define MQPCB_MASK_SERVICE_LEVEL                EHCA_BMASK_IBM(16, 16)
-#define MQPCB_MASK_SEND_GRH_FLAG                EHCA_BMASK_IBM(17, 17)
-#define MQPCB_MASK_RETRY_COUNT                  EHCA_BMASK_IBM(18, 18)
-#define MQPCB_MASK_TIMEOUT                      EHCA_BMASK_IBM(19, 19)
-#define MQPCB_MASK_PATH_MTU                     EHCA_BMASK_IBM(20, 20)
-#define MQPCB_MASK_MAX_STATIC_RATE              EHCA_BMASK_IBM(21, 21)
-#define MQPCB_MASK_DLID                         EHCA_BMASK_IBM(22, 22)
-#define MQPCB_MASK_RNR_RETRY_COUNT              EHCA_BMASK_IBM(23, 23)
-#define MQPCB_MASK_SOURCE_PATH_BITS             EHCA_BMASK_IBM(24, 24)
-#define MQPCB_MASK_TRAFFIC_CLASS                EHCA_BMASK_IBM(25, 25)
-#define MQPCB_MASK_HOP_LIMIT                    EHCA_BMASK_IBM(26, 26)
-#define MQPCB_MASK_SOURCE_GID_IDX               EHCA_BMASK_IBM(27, 27)
-#define MQPCB_MASK_FLOW_LABEL                   EHCA_BMASK_IBM(28, 28)
-#define MQPCB_MASK_DEST_GID                     EHCA_BMASK_IBM(30, 30)
-#define MQPCB_MASK_SERVICE_LEVEL_AL             EHCA_BMASK_IBM(31, 31)
-#define MQPCB_MASK_SEND_GRH_FLAG_AL             EHCA_BMASK_IBM(32, 32)
-#define MQPCB_MASK_RETRY_COUNT_AL               EHCA_BMASK_IBM(33, 33)
-#define MQPCB_MASK_TIMEOUT_AL                   EHCA_BMASK_IBM(34, 34)
-#define MQPCB_MASK_MAX_STATIC_RATE_AL           EHCA_BMASK_IBM(35, 35)
-#define MQPCB_MASK_DLID_AL                      EHCA_BMASK_IBM(36, 36)
-#define MQPCB_MASK_RNR_RETRY_COUNT_AL           EHCA_BMASK_IBM(37, 37)
-#define MQPCB_MASK_SOURCE_PATH_BITS_AL          EHCA_BMASK_IBM(38, 38)
-#define MQPCB_MASK_TRAFFIC_CLASS_AL             EHCA_BMASK_IBM(39, 39)
-#define MQPCB_MASK_HOP_LIMIT_AL                 EHCA_BMASK_IBM(40, 40)
-#define MQPCB_MASK_SOURCE_GID_IDX_AL            EHCA_BMASK_IBM(41, 41)
-#define MQPCB_MASK_FLOW_LABEL_AL                EHCA_BMASK_IBM(42, 42)
-#define MQPCB_MASK_DEST_GID_AL                  EHCA_BMASK_IBM(44, 44)
-#define MQPCB_MASK_MAX_NR_OUTST_SEND_WR         EHCA_BMASK_IBM(45, 45)
-#define MQPCB_MASK_MAX_NR_OUTST_RECV_WR         EHCA_BMASK_IBM(46, 46)
-#define MQPCB_MASK_DISABLE_ETE_CREDIT_CHECK     EHCA_BMASK_IBM(47, 47)
-#define MQPCB_MASK_QP_ENABLE                    EHCA_BMASK_IBM(48, 48)
-#define MQPCB_MASK_CURR_SRQ_LIMIT               EHCA_BMASK_IBM(49, 49)
-#define MQPCB_MASK_QP_AFF_ASYN_EV_LOG_REG       EHCA_BMASK_IBM(50, 50)
-#define MQPCB_MASK_SHARED_RQ_HNDL               EHCA_BMASK_IBM(51, 51)
-
-#endif /* __EHCA_CLASSES_PSERIES_H__ */
diff --git a/drivers/staging/rdma/ehca/ehca_cq.c b/drivers/staging/rdma/ehca/ehca_cq.c
deleted file mode 100644
index ea1b5c1203b4..000000000000
--- a/drivers/staging/rdma/ehca/ehca_cq.c
+++ /dev/null
@@ -1,398 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Completion queue handling
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Khadija Souissi <souissi@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_iverbs.h"
-#include "ehca_classes.h"
-#include "ehca_irq.h"
-#include "hcp_if.h"
-
-static struct kmem_cache *cq_cache;
-
-int ehca_cq_assign_qp(struct ehca_cq *cq, struct ehca_qp *qp)
-{
-	unsigned int qp_num = qp->real_qp_num;
-	unsigned int key = qp_num & (QP_HASHTAB_LEN-1);
-	unsigned long flags;
-
-	spin_lock_irqsave(&cq->spinlock, flags);
-	hlist_add_head(&qp->list_entries, &cq->qp_hashtab[key]);
-	spin_unlock_irqrestore(&cq->spinlock, flags);
-
-	ehca_dbg(cq->ib_cq.device, "cq_num=%x real_qp_num=%x",
-		 cq->cq_number, qp_num);
-
-	return 0;
-}
-
-int ehca_cq_unassign_qp(struct ehca_cq *cq, unsigned int real_qp_num)
-{
-	int ret = -EINVAL;
-	unsigned int key = real_qp_num & (QP_HASHTAB_LEN-1);
-	struct hlist_node *iter;
-	struct ehca_qp *qp;
-	unsigned long flags;
-
-	spin_lock_irqsave(&cq->spinlock, flags);
-	hlist_for_each(iter, &cq->qp_hashtab[key]) {
-		qp = hlist_entry(iter, struct ehca_qp, list_entries);
-		if (qp->real_qp_num == real_qp_num) {
-			hlist_del(iter);
-			ehca_dbg(cq->ib_cq.device,
-				 "removed qp from cq .cq_num=%x real_qp_num=%x",
-				 cq->cq_number, real_qp_num);
-			ret = 0;
-			break;
-		}
-	}
-	spin_unlock_irqrestore(&cq->spinlock, flags);
-	if (ret)
-		ehca_err(cq->ib_cq.device,
-			 "qp not found cq_num=%x real_qp_num=%x",
-			 cq->cq_number, real_qp_num);
-
-	return ret;
-}
-
-struct ehca_qp *ehca_cq_get_qp(struct ehca_cq *cq, int real_qp_num)
-{
-	struct ehca_qp *ret = NULL;
-	unsigned int key = real_qp_num & (QP_HASHTAB_LEN-1);
-	struct hlist_node *iter;
-	struct ehca_qp *qp;
-	hlist_for_each(iter, &cq->qp_hashtab[key]) {
-		qp = hlist_entry(iter, struct ehca_qp, list_entries);
-		if (qp->real_qp_num == real_qp_num) {
-			ret = qp;
-			break;
-		}
-	}
-	return ret;
-}
-
-struct ib_cq *ehca_create_cq(struct ib_device *device,
-			     const struct ib_cq_init_attr *attr,
-			     struct ib_ucontext *context,
-			     struct ib_udata *udata)
-{
-	int cqe = attr->cqe;
-	static const u32 additional_cqe = 20;
-	struct ib_cq *cq;
-	struct ehca_cq *my_cq;
-	struct ehca_shca *shca =
-		container_of(device, struct ehca_shca, ib_device);
-	struct ipz_adapter_handle adapter_handle;
-	struct ehca_alloc_cq_parms param; /* h_call's out parameters */
-	struct h_galpa gal;
-	void *vpage;
-	u32 counter;
-	u64 rpage, cqx_fec, h_ret;
-	int rc, i;
-	unsigned long flags;
-
-	if (attr->flags)
-		return ERR_PTR(-EINVAL);
-
-	if (cqe >= 0xFFFFFFFF - 64 - additional_cqe)
-		return ERR_PTR(-EINVAL);
-
-	if (!atomic_add_unless(&shca->num_cqs, 1, shca->max_num_cqs)) {
-		ehca_err(device, "Unable to create CQ, max number of %i "
-			"CQs reached.", shca->max_num_cqs);
-		ehca_err(device, "To increase the maximum number of CQs "
-			"use the number_of_cqs module parameter.\n");
-		return ERR_PTR(-ENOSPC);
-	}
-
-	my_cq = kmem_cache_zalloc(cq_cache, GFP_KERNEL);
-	if (!my_cq) {
-		ehca_err(device, "Out of memory for ehca_cq struct device=%p",
-			 device);
-		atomic_dec(&shca->num_cqs);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	memset(&param, 0, sizeof(struct ehca_alloc_cq_parms));
-
-	spin_lock_init(&my_cq->spinlock);
-	spin_lock_init(&my_cq->cb_lock);
-	spin_lock_init(&my_cq->task_lock);
-	atomic_set(&my_cq->nr_events, 0);
-	init_waitqueue_head(&my_cq->wait_completion);
-
-	cq = &my_cq->ib_cq;
-
-	adapter_handle = shca->ipz_hca_handle;
-	param.eq_handle = shca->eq.ipz_eq_handle;
-
-	idr_preload(GFP_KERNEL);
-	write_lock_irqsave(&ehca_cq_idr_lock, flags);
-	rc = idr_alloc(&ehca_cq_idr, my_cq, 0, 0x2000000, GFP_NOWAIT);
-	write_unlock_irqrestore(&ehca_cq_idr_lock, flags);
-	idr_preload_end();
-
-	if (rc < 0) {
-		cq = ERR_PTR(-ENOMEM);
-		ehca_err(device, "Can't allocate new idr entry. device=%p",
-			 device);
-		goto create_cq_exit1;
-	}
-	my_cq->token = rc;
-
-	/*
-	 * CQs maximum depth is 4GB-64, but we need additional 20 as buffer
-	 * for receiving errors CQEs.
-	 */
-	param.nr_cqe = cqe + additional_cqe;
-	h_ret = hipz_h_alloc_resource_cq(adapter_handle, my_cq, &param);
-
-	if (h_ret != H_SUCCESS) {
-		ehca_err(device, "hipz_h_alloc_resource_cq() failed "
-			 "h_ret=%lli device=%p", h_ret, device);
-		cq = ERR_PTR(ehca2ib_return_code(h_ret));
-		goto create_cq_exit2;
-	}
-
-	rc = ipz_queue_ctor(NULL, &my_cq->ipz_queue, param.act_pages,
-				EHCA_PAGESIZE, sizeof(struct ehca_cqe), 0, 0);
-	if (!rc) {
-		ehca_err(device, "ipz_queue_ctor() failed ipz_rc=%i device=%p",
-			 rc, device);
-		cq = ERR_PTR(-EINVAL);
-		goto create_cq_exit3;
-	}
-
-	for (counter = 0; counter < param.act_pages; counter++) {
-		vpage = ipz_qpageit_get_inc(&my_cq->ipz_queue);
-		if (!vpage) {
-			ehca_err(device, "ipz_qpageit_get_inc() "
-				 "returns NULL device=%p", device);
-			cq = ERR_PTR(-EAGAIN);
-			goto create_cq_exit4;
-		}
-		rpage = __pa(vpage);
-
-		h_ret = hipz_h_register_rpage_cq(adapter_handle,
-						 my_cq->ipz_cq_handle,
-						 &my_cq->pf,
-						 0,
-						 0,
-						 rpage,
-						 1,
-						 my_cq->galpas.
-						 kernel);
-
-		if (h_ret < H_SUCCESS) {
-			ehca_err(device, "hipz_h_register_rpage_cq() failed "
-				 "ehca_cq=%p cq_num=%x h_ret=%lli counter=%i "
-				 "act_pages=%i", my_cq, my_cq->cq_number,
-				 h_ret, counter, param.act_pages);
-			cq = ERR_PTR(-EINVAL);
-			goto create_cq_exit4;
-		}
-
-		if (counter == (param.act_pages - 1)) {
-			vpage = ipz_qpageit_get_inc(&my_cq->ipz_queue);
-			if ((h_ret != H_SUCCESS) || vpage) {
-				ehca_err(device, "Registration of pages not "
-					 "complete ehca_cq=%p cq_num=%x "
-					 "h_ret=%lli", my_cq, my_cq->cq_number,
-					 h_ret);
-				cq = ERR_PTR(-EAGAIN);
-				goto create_cq_exit4;
-			}
-		} else {
-			if (h_ret != H_PAGE_REGISTERED) {
-				ehca_err(device, "Registration of page failed "
-					 "ehca_cq=%p cq_num=%x h_ret=%lli "
-					 "counter=%i act_pages=%i",
-					 my_cq, my_cq->cq_number,
-					 h_ret, counter, param.act_pages);
-				cq = ERR_PTR(-ENOMEM);
-				goto create_cq_exit4;
-			}
-		}
-	}
-
-	ipz_qeit_reset(&my_cq->ipz_queue);
-
-	gal = my_cq->galpas.kernel;
-	cqx_fec = hipz_galpa_load(gal, CQTEMM_OFFSET(cqx_fec));
-	ehca_dbg(device, "ehca_cq=%p cq_num=%x CQX_FEC=%llx",
-		 my_cq, my_cq->cq_number, cqx_fec);
-
-	my_cq->ib_cq.cqe = my_cq->nr_of_entries =
-		param.act_nr_of_entries - additional_cqe;
-	my_cq->cq_number = (my_cq->ipz_cq_handle.handle) & 0xffff;
-
-	for (i = 0; i < QP_HASHTAB_LEN; i++)
-		INIT_HLIST_HEAD(&my_cq->qp_hashtab[i]);
-
-	INIT_LIST_HEAD(&my_cq->sqp_err_list);
-	INIT_LIST_HEAD(&my_cq->rqp_err_list);
-
-	if (context) {
-		struct ipz_queue *ipz_queue = &my_cq->ipz_queue;
-		struct ehca_create_cq_resp resp;
-		memset(&resp, 0, sizeof(resp));
-		resp.cq_number = my_cq->cq_number;
-		resp.token = my_cq->token;
-		resp.ipz_queue.qe_size = ipz_queue->qe_size;
-		resp.ipz_queue.act_nr_of_sg = ipz_queue->act_nr_of_sg;
-		resp.ipz_queue.queue_length = ipz_queue->queue_length;
-		resp.ipz_queue.pagesize = ipz_queue->pagesize;
-		resp.ipz_queue.toggle_state = ipz_queue->toggle_state;
-		resp.fw_handle_ofs = (u32)
-			(my_cq->galpas.user.fw_handle & (PAGE_SIZE - 1));
-		if (ib_copy_to_udata(udata, &resp, sizeof(resp))) {
-			ehca_err(device, "Copy to udata failed.");
-			cq = ERR_PTR(-EFAULT);
-			goto create_cq_exit4;
-		}
-	}
-
-	return cq;
-
-create_cq_exit4:
-	ipz_queue_dtor(NULL, &my_cq->ipz_queue);
-
-create_cq_exit3:
-	h_ret = hipz_h_destroy_cq(adapter_handle, my_cq, 1);
-	if (h_ret != H_SUCCESS)
-		ehca_err(device, "hipz_h_destroy_cq() failed ehca_cq=%p "
-			 "cq_num=%x h_ret=%lli", my_cq, my_cq->cq_number, h_ret);
-
-create_cq_exit2:
-	write_lock_irqsave(&ehca_cq_idr_lock, flags);
-	idr_remove(&ehca_cq_idr, my_cq->token);
-	write_unlock_irqrestore(&ehca_cq_idr_lock, flags);
-
-create_cq_exit1:
-	kmem_cache_free(cq_cache, my_cq);
-
-	atomic_dec(&shca->num_cqs);
-	return cq;
-}
-
-int ehca_destroy_cq(struct ib_cq *cq)
-{
-	u64 h_ret;
-	struct ehca_cq *my_cq = container_of(cq, struct ehca_cq, ib_cq);
-	int cq_num = my_cq->cq_number;
-	struct ib_device *device = cq->device;
-	struct ehca_shca *shca = container_of(device, struct ehca_shca,
-					      ib_device);
-	struct ipz_adapter_handle adapter_handle = shca->ipz_hca_handle;
-	unsigned long flags;
-
-	if (cq->uobject) {
-		if (my_cq->mm_count_galpa || my_cq->mm_count_queue) {
-			ehca_err(device, "Resources still referenced in "
-				 "user space cq_num=%x", my_cq->cq_number);
-			return -EINVAL;
-		}
-	}
-
-	/*
-	 * remove the CQ from the idr first to make sure
-	 * no more interrupt tasklets will touch this CQ
-	 */
-	write_lock_irqsave(&ehca_cq_idr_lock, flags);
-	idr_remove(&ehca_cq_idr, my_cq->token);
-	write_unlock_irqrestore(&ehca_cq_idr_lock, flags);
-
-	/* now wait until all pending events have completed */
-	wait_event(my_cq->wait_completion, !atomic_read(&my_cq->nr_events));
-
-	/* nobody's using our CQ any longer -- we can destroy it */
-	h_ret = hipz_h_destroy_cq(adapter_handle, my_cq, 0);
-	if (h_ret == H_R_STATE) {
-		/* cq in err: read err data and destroy it forcibly */
-		ehca_dbg(device, "ehca_cq=%p cq_num=%x resource=%llx in err "
-			 "state. Try to delete it forcibly.",
-			 my_cq, cq_num, my_cq->ipz_cq_handle.handle);
-		ehca_error_data(shca, my_cq, my_cq->ipz_cq_handle.handle);
-		h_ret = hipz_h_destroy_cq(adapter_handle, my_cq, 1);
-		if (h_ret == H_SUCCESS)
-			ehca_dbg(device, "cq_num=%x deleted successfully.",
-				 cq_num);
-	}
-	if (h_ret != H_SUCCESS) {
-		ehca_err(device, "hipz_h_destroy_cq() failed h_ret=%lli "
-			 "ehca_cq=%p cq_num=%x", h_ret, my_cq, cq_num);
-		return ehca2ib_return_code(h_ret);
-	}
-	ipz_queue_dtor(NULL, &my_cq->ipz_queue);
-	kmem_cache_free(cq_cache, my_cq);
-
-	atomic_dec(&shca->num_cqs);
-	return 0;
-}
-
-int ehca_resize_cq(struct ib_cq *cq, int cqe, struct ib_udata *udata)
-{
-	/* TODO: proper resize needs to be done */
-	ehca_err(cq->device, "not implemented yet");
-
-	return -EFAULT;
-}
-
-int ehca_init_cq_cache(void)
-{
-	cq_cache = kmem_cache_create("ehca_cache_cq",
-				     sizeof(struct ehca_cq), 0,
-				     SLAB_HWCACHE_ALIGN,
-				     NULL);
-	if (!cq_cache)
-		return -ENOMEM;
-	return 0;
-}
-
-void ehca_cleanup_cq_cache(void)
-{
-	if (cq_cache)
-		kmem_cache_destroy(cq_cache);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_eq.c b/drivers/staging/rdma/ehca/ehca_eq.c
deleted file mode 100644
index 90da6747d395..000000000000
--- a/drivers/staging/rdma/ehca/ehca_eq.c
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Event queue handling
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Khadija Souissi <souissi@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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 "ehca_classes.h"
-#include "ehca_irq.h"
-#include "ehca_iverbs.h"
-#include "ehca_qes.h"
-#include "hcp_if.h"
-#include "ipz_pt_fn.h"
-
-int ehca_create_eq(struct ehca_shca *shca,
-		   struct ehca_eq *eq,
-		   const enum ehca_eq_type type, const u32 length)
-{
-	int ret;
-	u64 h_ret;
-	u32 nr_pages;
-	u32 i;
-	void *vpage;
-	struct ib_device *ib_dev = &shca->ib_device;
-
-	spin_lock_init(&eq->spinlock);
-	spin_lock_init(&eq->irq_spinlock);
-	eq->is_initialized = 0;
-
-	if (type != EHCA_EQ && type != EHCA_NEQ) {
-		ehca_err(ib_dev, "Invalid EQ type %x. eq=%p", type, eq);
-		return -EINVAL;
-	}
-	if (!length) {
-		ehca_err(ib_dev, "EQ length must not be zero. eq=%p", eq);
-		return -EINVAL;
-	}
-
-	h_ret = hipz_h_alloc_resource_eq(shca->ipz_hca_handle,
-					 &eq->pf,
-					 type,
-					 length,
-					 &eq->ipz_eq_handle,
-					 &eq->length,
-					 &nr_pages, &eq->ist);
-
-	if (h_ret != H_SUCCESS) {
-		ehca_err(ib_dev, "Can't allocate EQ/NEQ. eq=%p", eq);
-		return -EINVAL;
-	}
-
-	ret = ipz_queue_ctor(NULL, &eq->ipz_queue, nr_pages,
-			     EHCA_PAGESIZE, sizeof(struct ehca_eqe), 0, 0);
-	if (!ret) {
-		ehca_err(ib_dev, "Can't allocate EQ pages eq=%p", eq);
-		goto create_eq_exit1;
-	}
-
-	for (i = 0; i < nr_pages; i++) {
-		u64 rpage;
-
-		vpage = ipz_qpageit_get_inc(&eq->ipz_queue);
-		if (!vpage)
-			goto create_eq_exit2;
-
-		rpage = __pa(vpage);
-		h_ret = hipz_h_register_rpage_eq(shca->ipz_hca_handle,
-						 eq->ipz_eq_handle,
-						 &eq->pf,
-						 0, 0, rpage, 1);
-
-		if (i == (nr_pages - 1)) {
-			/* last page */
-			vpage = ipz_qpageit_get_inc(&eq->ipz_queue);
-			if (h_ret != H_SUCCESS || vpage)
-				goto create_eq_exit2;
-		} else {
-			if (h_ret != H_PAGE_REGISTERED)
-				goto create_eq_exit2;
-		}
-	}
-
-	ipz_qeit_reset(&eq->ipz_queue);
-
-	/* register interrupt handlers and initialize work queues */
-	if (type == EHCA_EQ) {
-		tasklet_init(&eq->interrupt_task, ehca_tasklet_eq, (long)shca);
-
-		ret = ibmebus_request_irq(eq->ist, ehca_interrupt_eq,
-					  0, "ehca_eq",
-					  (void *)shca);
-		if (ret < 0)
-			ehca_err(ib_dev, "Can't map interrupt handler.");
-	} else if (type == EHCA_NEQ) {
-		tasklet_init(&eq->interrupt_task, ehca_tasklet_neq, (long)shca);
-
-		ret = ibmebus_request_irq(eq->ist, ehca_interrupt_neq,
-					  0, "ehca_neq",
-					  (void *)shca);
-		if (ret < 0)
-			ehca_err(ib_dev, "Can't map interrupt handler.");
-	}
-
-	eq->is_initialized = 1;
-
-	return 0;
-
-create_eq_exit2:
-	ipz_queue_dtor(NULL, &eq->ipz_queue);
-
-create_eq_exit1:
-	hipz_h_destroy_eq(shca->ipz_hca_handle, eq);
-
-	return -EINVAL;
-}
-
-void *ehca_poll_eq(struct ehca_shca *shca, struct ehca_eq *eq)
-{
-	unsigned long flags;
-	void *eqe;
-
-	spin_lock_irqsave(&eq->spinlock, flags);
-	eqe = ipz_eqit_eq_get_inc_valid(&eq->ipz_queue);
-	spin_unlock_irqrestore(&eq->spinlock, flags);
-
-	return eqe;
-}
-
-int ehca_destroy_eq(struct ehca_shca *shca, struct ehca_eq *eq)
-{
-	unsigned long flags;
-	u64 h_ret;
-
-	ibmebus_free_irq(eq->ist, (void *)shca);
-
-	spin_lock_irqsave(&shca_list_lock, flags);
-	eq->is_initialized = 0;
-	spin_unlock_irqrestore(&shca_list_lock, flags);
-
-	tasklet_kill(&eq->interrupt_task);
-
-	h_ret = hipz_h_destroy_eq(shca->ipz_hca_handle, eq);
-
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't free EQ resources.");
-		return -EINVAL;
-	}
-	ipz_queue_dtor(NULL, &eq->ipz_queue);
-
-	return 0;
-}
diff --git a/drivers/staging/rdma/ehca/ehca_hca.c b/drivers/staging/rdma/ehca/ehca_hca.c
deleted file mode 100644
index e8b1bb65797a..000000000000
--- a/drivers/staging/rdma/ehca/ehca_hca.c
+++ /dev/null
@@ -1,414 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  HCA query functions
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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/gfp.h>
-
-#include "ehca_tools.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-
-static unsigned int limit_uint(unsigned int value)
-{
-	return min_t(unsigned int, value, INT_MAX);
-}
-
-int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
-		      struct ib_udata *uhw)
-{
-	int i, ret = 0;
-	struct ehca_shca *shca = container_of(ibdev, struct ehca_shca,
-					      ib_device);
-	struct hipz_query_hca *rblock;
-
-	static const u32 cap_mapping[] = {
-		IB_DEVICE_RESIZE_MAX_WR,      HCA_CAP_WQE_RESIZE,
-		IB_DEVICE_BAD_PKEY_CNTR,      HCA_CAP_BAD_P_KEY_CTR,
-		IB_DEVICE_BAD_QKEY_CNTR,      HCA_CAP_Q_KEY_VIOL_CTR,
-		IB_DEVICE_RAW_MULTI,          HCA_CAP_RAW_PACKET_MCAST,
-		IB_DEVICE_AUTO_PATH_MIG,      HCA_CAP_AUTO_PATH_MIG,
-		IB_DEVICE_CHANGE_PHY_PORT,    HCA_CAP_SQD_RTS_PORT_CHANGE,
-		IB_DEVICE_UD_AV_PORT_ENFORCE, HCA_CAP_AH_PORT_NR_CHECK,
-		IB_DEVICE_CURR_QP_STATE_MOD,  HCA_CAP_CUR_QP_STATE_MOD,
-		IB_DEVICE_SHUTDOWN_PORT,      HCA_CAP_SHUTDOWN_PORT,
-		IB_DEVICE_INIT_TYPE,          HCA_CAP_INIT_TYPE,
-		IB_DEVICE_PORT_ACTIVE_EVENT,  HCA_CAP_PORT_ACTIVE_EVENT,
-	};
-
-	if (uhw->inlen || uhw->outlen)
-		return -EINVAL;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	if (hipz_h_query_hca(shca->ipz_hca_handle, rblock) != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query device properties");
-		ret = -EINVAL;
-		goto query_device1;
-	}
-
-	memset(props, 0, sizeof(struct ib_device_attr));
-	props->page_size_cap   = shca->hca_cap_mr_pgsize;
-	props->fw_ver          = rblock->hw_ver;
-	props->max_mr_size     = rblock->max_mr_size;
-	props->vendor_id       = rblock->vendor_id >> 8;
-	props->vendor_part_id  = rblock->vendor_part_id >> 16;
-	props->hw_ver          = rblock->hw_ver;
-	props->max_qp          = limit_uint(rblock->max_qp);
-	props->max_qp_wr       = limit_uint(rblock->max_wqes_wq);
-	props->max_sge         = limit_uint(rblock->max_sge);
-	props->max_sge_rd      = limit_uint(rblock->max_sge_rd);
-	props->max_cq          = limit_uint(rblock->max_cq);
-	props->max_cqe         = limit_uint(rblock->max_cqe);
-	props->max_mr          = limit_uint(rblock->max_mr);
-	props->max_mw          = limit_uint(rblock->max_mw);
-	props->max_pd          = limit_uint(rblock->max_pd);
-	props->max_ah          = limit_uint(rblock->max_ah);
-	props->max_ee          = limit_uint(rblock->max_rd_ee_context);
-	props->max_rdd         = limit_uint(rblock->max_rd_domain);
-	props->max_fmr         = limit_uint(rblock->max_mr);
-	props->max_qp_rd_atom  = limit_uint(rblock->max_rr_qp);
-	props->max_ee_rd_atom  = limit_uint(rblock->max_rr_ee_context);
-	props->max_res_rd_atom = limit_uint(rblock->max_rr_hca);
-	props->max_qp_init_rd_atom = limit_uint(rblock->max_act_wqs_qp);
-	props->max_ee_init_rd_atom = limit_uint(rblock->max_act_wqs_ee_context);
-
-	if (EHCA_BMASK_GET(HCA_CAP_SRQ, shca->hca_cap)) {
-		props->max_srq         = limit_uint(props->max_qp);
-		props->max_srq_wr      = limit_uint(props->max_qp_wr);
-		props->max_srq_sge     = 3;
-	}
-
-	props->max_pkeys           = 16;
-	/* Some FW versions say 0 here; insert sensible value in that case */
-	props->local_ca_ack_delay  = rblock->local_ca_ack_delay ?
-		min_t(u8, rblock->local_ca_ack_delay, 255) : 12;
-	props->max_raw_ipv6_qp     = limit_uint(rblock->max_raw_ipv6_qp);
-	props->max_raw_ethy_qp     = limit_uint(rblock->max_raw_ethy_qp);
-	props->max_mcast_grp       = limit_uint(rblock->max_mcast_grp);
-	props->max_mcast_qp_attach = limit_uint(rblock->max_mcast_qp_attach);
-	props->max_total_mcast_qp_attach
-		= limit_uint(rblock->max_total_mcast_qp_attach);
-
-	/* translate device capabilities */
-	props->device_cap_flags = IB_DEVICE_SYS_IMAGE_GUID |
-		IB_DEVICE_RC_RNR_NAK_GEN | IB_DEVICE_N_NOTIFY_CQ;
-	for (i = 0; i < ARRAY_SIZE(cap_mapping); i += 2)
-		if (rblock->hca_cap_indicators & cap_mapping[i + 1])
-			props->device_cap_flags |= cap_mapping[i];
-
-query_device1:
-	ehca_free_fw_ctrlblock(rblock);
-
-	return ret;
-}
-
-static enum ib_mtu map_mtu(struct ehca_shca *shca, u32 fw_mtu)
-{
-	switch (fw_mtu) {
-	case 0x1:
-		return IB_MTU_256;
-	case 0x2:
-		return IB_MTU_512;
-	case 0x3:
-		return IB_MTU_1024;
-	case 0x4:
-		return IB_MTU_2048;
-	case 0x5:
-		return IB_MTU_4096;
-	default:
-		ehca_err(&shca->ib_device, "Unknown MTU size: %x.",
-			 fw_mtu);
-		return 0;
-	}
-}
-
-static u8 map_number_of_vls(struct ehca_shca *shca, u32 vl_cap)
-{
-	switch (vl_cap) {
-	case 0x1:
-		return 1;
-	case 0x2:
-		return 2;
-	case 0x3:
-		return 4;
-	case 0x4:
-		return 8;
-	case 0x5:
-		return 15;
-	default:
-		ehca_err(&shca->ib_device, "invalid Vl Capability: %x.",
-			 vl_cap);
-		return 0;
-	}
-}
-
-int ehca_query_port(struct ib_device *ibdev,
-		    u8 port, struct ib_port_attr *props)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_shca *shca = container_of(ibdev, struct ehca_shca,
-					      ib_device);
-	struct hipz_query_port *rblock;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_port(shca->ipz_hca_handle, port, rblock);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query port properties");
-		ret = -EINVAL;
-		goto query_port1;
-	}
-
-	memset(props, 0, sizeof(struct ib_port_attr));
-
-	props->active_mtu = props->max_mtu = map_mtu(shca, rblock->max_mtu);
-	props->port_cap_flags  = rblock->capability_mask;
-	props->gid_tbl_len     = rblock->gid_tbl_len;
-	if (rblock->max_msg_sz)
-		props->max_msg_sz      = rblock->max_msg_sz;
-	else
-		props->max_msg_sz      = 0x1 << 31;
-	props->bad_pkey_cntr   = rblock->bad_pkey_cntr;
-	props->qkey_viol_cntr  = rblock->qkey_viol_cntr;
-	props->pkey_tbl_len    = rblock->pkey_tbl_len;
-	props->lid             = rblock->lid;
-	props->sm_lid          = rblock->sm_lid;
-	props->lmc             = rblock->lmc;
-	props->sm_sl           = rblock->sm_sl;
-	props->subnet_timeout  = rblock->subnet_timeout;
-	props->init_type_reply = rblock->init_type_reply;
-	props->max_vl_num      = map_number_of_vls(shca, rblock->vl_cap);
-
-	if (rblock->state && rblock->phys_width) {
-		props->phys_state      = rblock->phys_pstate;
-		props->state           = rblock->phys_state;
-		props->active_width    = rblock->phys_width;
-		props->active_speed    = rblock->phys_speed;
-	} else {
-		/* old firmware releases don't report physical
-		 * port info, so use default values
-		 */
-		props->phys_state      = 5;
-		props->state           = rblock->state;
-		props->active_width    = IB_WIDTH_12X;
-		props->active_speed    = IB_SPEED_SDR;
-	}
-
-query_port1:
-	ehca_free_fw_ctrlblock(rblock);
-
-	return ret;
-}
-
-int ehca_query_sma_attr(struct ehca_shca *shca,
-			u8 port, struct ehca_sma_attr *attr)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct hipz_query_port *rblock;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_ATOMIC);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_port(shca->ipz_hca_handle, port, rblock);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query port properties");
-		ret = -EINVAL;
-		goto query_sma_attr1;
-	}
-
-	memset(attr, 0, sizeof(struct ehca_sma_attr));
-
-	attr->lid    = rblock->lid;
-	attr->lmc    = rblock->lmc;
-	attr->sm_sl  = rblock->sm_sl;
-	attr->sm_lid = rblock->sm_lid;
-
-	attr->pkey_tbl_len = rblock->pkey_tbl_len;
-	memcpy(attr->pkeys, rblock->pkey_entries, sizeof(attr->pkeys));
-
-query_sma_attr1:
-	ehca_free_fw_ctrlblock(rblock);
-
-	return ret;
-}
-
-int ehca_query_pkey(struct ib_device *ibdev, u8 port, u16 index, u16 *pkey)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_shca *shca;
-	struct hipz_query_port *rblock;
-
-	shca = container_of(ibdev, struct ehca_shca, ib_device);
-	if (index > 16) {
-		ehca_err(&shca->ib_device, "Invalid index: %x.", index);
-		return -EINVAL;
-	}
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device,  "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_port(shca->ipz_hca_handle, port, rblock);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query port properties");
-		ret = -EINVAL;
-		goto query_pkey1;
-	}
-
-	memcpy(pkey, &rblock->pkey_entries + index, sizeof(u16));
-
-query_pkey1:
-	ehca_free_fw_ctrlblock(rblock);
-
-	return ret;
-}
-
-int ehca_query_gid(struct ib_device *ibdev, u8 port,
-		   int index, union ib_gid *gid)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_shca *shca = container_of(ibdev, struct ehca_shca,
-					      ib_device);
-	struct hipz_query_port *rblock;
-
-	if (index < 0 || index > 255) {
-		ehca_err(&shca->ib_device, "Invalid index: %x.", index);
-		return -EINVAL;
-	}
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_port(shca->ipz_hca_handle, port, rblock);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query port properties");
-		ret = -EINVAL;
-		goto query_gid1;
-	}
-
-	memcpy(&gid->raw[0], &rblock->gid_prefix, sizeof(u64));
-	memcpy(&gid->raw[8], &rblock->guid_entries[index], sizeof(u64));
-
-query_gid1:
-	ehca_free_fw_ctrlblock(rblock);
-
-	return ret;
-}
-
-static const u32 allowed_port_caps = (
-	IB_PORT_SM | IB_PORT_LED_INFO_SUP | IB_PORT_CM_SUP |
-	IB_PORT_SNMP_TUNNEL_SUP | IB_PORT_DEVICE_MGMT_SUP |
-	IB_PORT_VENDOR_CLASS_SUP);
-
-int ehca_modify_port(struct ib_device *ibdev,
-		     u8 port, int port_modify_mask,
-		     struct ib_port_modify *props)
-{
-	int ret = 0;
-	struct ehca_shca *shca;
-	struct hipz_query_port *rblock;
-	u32 cap;
-	u64 hret;
-
-	shca = container_of(ibdev, struct ehca_shca, ib_device);
-	if ((props->set_port_cap_mask | props->clr_port_cap_mask)
-	    & ~allowed_port_caps) {
-		ehca_err(&shca->ib_device, "Non-changeable bits set in masks  "
-			 "set=%x  clr=%x  allowed=%x", props->set_port_cap_mask,
-			 props->clr_port_cap_mask, allowed_port_caps);
-		return -EINVAL;
-	}
-
-	if (mutex_lock_interruptible(&shca->modify_mutex))
-		return -ERESTARTSYS;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device,  "Can't allocate rblock memory.");
-		ret = -ENOMEM;
-		goto modify_port1;
-	}
-
-	hret = hipz_h_query_port(shca->ipz_hca_handle, port, rblock);
-	if (hret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query port properties");
-		ret = -EINVAL;
-		goto modify_port2;
-	}
-
-	cap = (rblock->capability_mask | props->set_port_cap_mask)
-		& ~props->clr_port_cap_mask;
-
-	hret = hipz_h_modify_port(shca->ipz_hca_handle, port,
-				  cap, props->init_type, port_modify_mask);
-	if (hret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Modify port failed  h_ret=%lli",
-			 hret);
-		ret = -EINVAL;
-	}
-
-modify_port2:
-	ehca_free_fw_ctrlblock(rblock);
-
-modify_port1:
-	mutex_unlock(&shca->modify_mutex);
-
-	return ret;
-}
diff --git a/drivers/staging/rdma/ehca/ehca_irq.c b/drivers/staging/rdma/ehca/ehca_irq.c
deleted file mode 100644
index 8615d7cf7e01..000000000000
--- a/drivers/staging/rdma/ehca/ehca_irq.c
+++ /dev/null
@@ -1,870 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Functions for EQs, NEQs and interrupts
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Khadija Souissi <souissi@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Joachim Fenkes <fenkes@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-#include <linux/smpboot.h>
-
-#include "ehca_classes.h"
-#include "ehca_irq.h"
-#include "ehca_iverbs.h"
-#include "ehca_tools.h"
-#include "hcp_if.h"
-#include "hipz_fns.h"
-#include "ipz_pt_fn.h"
-
-#define EQE_COMPLETION_EVENT   EHCA_BMASK_IBM( 1,  1)
-#define EQE_CQ_QP_NUMBER       EHCA_BMASK_IBM( 8, 31)
-#define EQE_EE_IDENTIFIER      EHCA_BMASK_IBM( 2,  7)
-#define EQE_CQ_NUMBER          EHCA_BMASK_IBM( 8, 31)
-#define EQE_QP_NUMBER          EHCA_BMASK_IBM( 8, 31)
-#define EQE_QP_TOKEN           EHCA_BMASK_IBM(32, 63)
-#define EQE_CQ_TOKEN           EHCA_BMASK_IBM(32, 63)
-
-#define NEQE_COMPLETION_EVENT  EHCA_BMASK_IBM( 1,  1)
-#define NEQE_EVENT_CODE        EHCA_BMASK_IBM( 2,  7)
-#define NEQE_PORT_NUMBER       EHCA_BMASK_IBM( 8, 15)
-#define NEQE_PORT_AVAILABILITY EHCA_BMASK_IBM(16, 16)
-#define NEQE_DISRUPTIVE        EHCA_BMASK_IBM(16, 16)
-#define NEQE_SPECIFIC_EVENT    EHCA_BMASK_IBM(16, 23)
-
-#define ERROR_DATA_LENGTH      EHCA_BMASK_IBM(52, 63)
-#define ERROR_DATA_TYPE        EHCA_BMASK_IBM( 0,  7)
-
-static void queue_comp_task(struct ehca_cq *__cq);
-
-static struct ehca_comp_pool *pool;
-
-static inline void comp_event_callback(struct ehca_cq *cq)
-{
-	if (!cq->ib_cq.comp_handler)
-		return;
-
-	spin_lock(&cq->cb_lock);
-	cq->ib_cq.comp_handler(&cq->ib_cq, cq->ib_cq.cq_context);
-	spin_unlock(&cq->cb_lock);
-
-	return;
-}
-
-static void print_error_data(struct ehca_shca *shca, void *data,
-			     u64 *rblock, int length)
-{
-	u64 type = EHCA_BMASK_GET(ERROR_DATA_TYPE, rblock[2]);
-	u64 resource = rblock[1];
-
-	switch (type) {
-	case 0x1: /* Queue Pair */
-	{
-		struct ehca_qp *qp = (struct ehca_qp *)data;
-
-		/* only print error data if AER is set */
-		if (rblock[6] == 0)
-			return;
-
-		ehca_err(&shca->ib_device,
-			 "QP 0x%x (resource=%llx) has errors.",
-			 qp->ib_qp.qp_num, resource);
-		break;
-	}
-	case 0x4: /* Completion Queue */
-	{
-		struct ehca_cq *cq = (struct ehca_cq *)data;
-
-		ehca_err(&shca->ib_device,
-			 "CQ 0x%x (resource=%llx) has errors.",
-			 cq->cq_number, resource);
-		break;
-	}
-	default:
-		ehca_err(&shca->ib_device,
-			 "Unknown error type: %llx on %s.",
-			 type, shca->ib_device.name);
-		break;
-	}
-
-	ehca_err(&shca->ib_device, "Error data is available: %llx.", resource);
-	ehca_err(&shca->ib_device, "EHCA ----- error data begin "
-		 "---------------------------------------------------");
-	ehca_dmp(rblock, length, "resource=%llx", resource);
-	ehca_err(&shca->ib_device, "EHCA ----- error data end "
-		 "----------------------------------------------------");
-
-	return;
-}
-
-int ehca_error_data(struct ehca_shca *shca, void *data,
-		    u64 resource)
-{
-
-	unsigned long ret;
-	u64 *rblock;
-	unsigned long block_count;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_ATOMIC);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Cannot allocate rblock memory.");
-		ret = -ENOMEM;
-		goto error_data1;
-	}
-
-	/* rblock must be 4K aligned and should be 4K large */
-	ret = hipz_h_error_data(shca->ipz_hca_handle,
-				resource,
-				rblock,
-				&block_count);
-
-	if (ret == H_R_STATE)
-		ehca_err(&shca->ib_device,
-			 "No error data is available: %llx.", resource);
-	else if (ret == H_SUCCESS) {
-		int length;
-
-		length = EHCA_BMASK_GET(ERROR_DATA_LENGTH, rblock[0]);
-
-		if (length > EHCA_PAGESIZE)
-			length = EHCA_PAGESIZE;
-
-		print_error_data(shca, data, rblock, length);
-	} else
-		ehca_err(&shca->ib_device,
-			 "Error data could not be fetched: %llx", resource);
-
-	ehca_free_fw_ctrlblock(rblock);
-
-error_data1:
-	return ret;
-
-}
-
-static void dispatch_qp_event(struct ehca_shca *shca, struct ehca_qp *qp,
-			      enum ib_event_type event_type)
-{
-	struct ib_event event;
-
-	/* PATH_MIG without the QP ever having been armed is false alarm */
-	if (event_type == IB_EVENT_PATH_MIG && !qp->mig_armed)
-		return;
-
-	event.device = &shca->ib_device;
-	event.event = event_type;
-
-	if (qp->ext_type == EQPT_SRQ) {
-		if (!qp->ib_srq.event_handler)
-			return;
-
-		event.element.srq = &qp->ib_srq;
-		qp->ib_srq.event_handler(&event, qp->ib_srq.srq_context);
-	} else {
-		if (!qp->ib_qp.event_handler)
-			return;
-
-		event.element.qp = &qp->ib_qp;
-		qp->ib_qp.event_handler(&event, qp->ib_qp.qp_context);
-	}
-}
-
-static void qp_event_callback(struct ehca_shca *shca, u64 eqe,
-			      enum ib_event_type event_type, int fatal)
-{
-	struct ehca_qp *qp;
-	u32 token = EHCA_BMASK_GET(EQE_QP_TOKEN, eqe);
-
-	read_lock(&ehca_qp_idr_lock);
-	qp = idr_find(&ehca_qp_idr, token);
-	if (qp)
-		atomic_inc(&qp->nr_events);
-	read_unlock(&ehca_qp_idr_lock);
-
-	if (!qp)
-		return;
-
-	if (fatal)
-		ehca_error_data(shca, qp, qp->ipz_qp_handle.handle);
-
-	dispatch_qp_event(shca, qp, fatal && qp->ext_type == EQPT_SRQ ?
-			  IB_EVENT_SRQ_ERR : event_type);
-
-	/*
-	 * eHCA only processes one WQE at a time for SRQ base QPs,
-	 * so the last WQE has been processed as soon as the QP enters
-	 * error state.
-	 */
-	if (fatal && qp->ext_type == EQPT_SRQBASE)
-		dispatch_qp_event(shca, qp, IB_EVENT_QP_LAST_WQE_REACHED);
-
-	if (atomic_dec_and_test(&qp->nr_events))
-		wake_up(&qp->wait_completion);
-	return;
-}
-
-static void cq_event_callback(struct ehca_shca *shca,
-			      u64 eqe)
-{
-	struct ehca_cq *cq;
-	u32 token = EHCA_BMASK_GET(EQE_CQ_TOKEN, eqe);
-
-	read_lock(&ehca_cq_idr_lock);
-	cq = idr_find(&ehca_cq_idr, token);
-	if (cq)
-		atomic_inc(&cq->nr_events);
-	read_unlock(&ehca_cq_idr_lock);
-
-	if (!cq)
-		return;
-
-	ehca_error_data(shca, cq, cq->ipz_cq_handle.handle);
-
-	if (atomic_dec_and_test(&cq->nr_events))
-		wake_up(&cq->wait_completion);
-
-	return;
-}
-
-static void parse_identifier(struct ehca_shca *shca, u64 eqe)
-{
-	u8 identifier = EHCA_BMASK_GET(EQE_EE_IDENTIFIER, eqe);
-
-	switch (identifier) {
-	case 0x02: /* path migrated */
-		qp_event_callback(shca, eqe, IB_EVENT_PATH_MIG, 0);
-		break;
-	case 0x03: /* communication established */
-		qp_event_callback(shca, eqe, IB_EVENT_COMM_EST, 0);
-		break;
-	case 0x04: /* send queue drained */
-		qp_event_callback(shca, eqe, IB_EVENT_SQ_DRAINED, 0);
-		break;
-	case 0x05: /* QP error */
-	case 0x06: /* QP error */
-		qp_event_callback(shca, eqe, IB_EVENT_QP_FATAL, 1);
-		break;
-	case 0x07: /* CQ error */
-	case 0x08: /* CQ error */
-		cq_event_callback(shca, eqe);
-		break;
-	case 0x09: /* MRMWPTE error */
-		ehca_err(&shca->ib_device, "MRMWPTE error.");
-		break;
-	case 0x0A: /* port event */
-		ehca_err(&shca->ib_device, "Port event.");
-		break;
-	case 0x0B: /* MR access error */
-		ehca_err(&shca->ib_device, "MR access error.");
-		break;
-	case 0x0C: /* EQ error */
-		ehca_err(&shca->ib_device, "EQ error.");
-		break;
-	case 0x0D: /* P/Q_Key mismatch */
-		ehca_err(&shca->ib_device, "P/Q_Key mismatch.");
-		break;
-	case 0x10: /* sampling complete */
-		ehca_err(&shca->ib_device, "Sampling complete.");
-		break;
-	case 0x11: /* unaffiliated access error */
-		ehca_err(&shca->ib_device, "Unaffiliated access error.");
-		break;
-	case 0x12: /* path migrating */
-		ehca_err(&shca->ib_device, "Path migrating.");
-		break;
-	case 0x13: /* interface trace stopped */
-		ehca_err(&shca->ib_device, "Interface trace stopped.");
-		break;
-	case 0x14: /* first error capture info available */
-		ehca_info(&shca->ib_device, "First error capture available");
-		break;
-	case 0x15: /* SRQ limit reached */
-		qp_event_callback(shca, eqe, IB_EVENT_SRQ_LIMIT_REACHED, 0);
-		break;
-	default:
-		ehca_err(&shca->ib_device, "Unknown identifier: %x on %s.",
-			 identifier, shca->ib_device.name);
-		break;
-	}
-
-	return;
-}
-
-static void dispatch_port_event(struct ehca_shca *shca, int port_num,
-				enum ib_event_type type, const char *msg)
-{
-	struct ib_event event;
-
-	ehca_info(&shca->ib_device, "port %d %s.", port_num, msg);
-	event.device = &shca->ib_device;
-	event.event = type;
-	event.element.port_num = port_num;
-	ib_dispatch_event(&event);
-}
-
-static void notify_port_conf_change(struct ehca_shca *shca, int port_num)
-{
-	struct ehca_sma_attr  new_attr;
-	struct ehca_sma_attr *old_attr = &shca->sport[port_num - 1].saved_attr;
-
-	ehca_query_sma_attr(shca, port_num, &new_attr);
-
-	if (new_attr.sm_sl  != old_attr->sm_sl ||
-	    new_attr.sm_lid != old_attr->sm_lid)
-		dispatch_port_event(shca, port_num, IB_EVENT_SM_CHANGE,
-				    "SM changed");
-
-	if (new_attr.lid != old_attr->lid ||
-	    new_attr.lmc != old_attr->lmc)
-		dispatch_port_event(shca, port_num, IB_EVENT_LID_CHANGE,
-				    "LID changed");
-
-	if (new_attr.pkey_tbl_len != old_attr->pkey_tbl_len ||
-	    memcmp(new_attr.pkeys, old_attr->pkeys,
-		   sizeof(u16) * new_attr.pkey_tbl_len))
-		dispatch_port_event(shca, port_num, IB_EVENT_PKEY_CHANGE,
-				    "P_Key changed");
-
-	*old_attr = new_attr;
-}
-
-/* replay modify_qp for sqps -- return 0 if all is well, 1 if AQP1 destroyed */
-static int replay_modify_qp(struct ehca_sport *sport)
-{
-	int aqp1_destroyed;
-	unsigned long flags;
-
-	spin_lock_irqsave(&sport->mod_sqp_lock, flags);
-
-	aqp1_destroyed = !sport->ibqp_sqp[IB_QPT_GSI];
-
-	if (sport->ibqp_sqp[IB_QPT_SMI])
-		ehca_recover_sqp(sport->ibqp_sqp[IB_QPT_SMI]);
-	if (!aqp1_destroyed)
-		ehca_recover_sqp(sport->ibqp_sqp[IB_QPT_GSI]);
-
-	spin_unlock_irqrestore(&sport->mod_sqp_lock, flags);
-
-	return aqp1_destroyed;
-}
-
-static void parse_ec(struct ehca_shca *shca, u64 eqe)
-{
-	u8 ec   = EHCA_BMASK_GET(NEQE_EVENT_CODE, eqe);
-	u8 port = EHCA_BMASK_GET(NEQE_PORT_NUMBER, eqe);
-	u8 spec_event;
-	struct ehca_sport *sport = &shca->sport[port - 1];
-
-	switch (ec) {
-	case 0x30: /* port availability change */
-		if (EHCA_BMASK_GET(NEQE_PORT_AVAILABILITY, eqe)) {
-			/* only replay modify_qp calls in autodetect mode;
-			 * if AQP1 was destroyed, the port is already down
-			 * again and we can drop the event.
-			 */
-			if (ehca_nr_ports < 0)
-				if (replay_modify_qp(sport))
-					break;
-
-			sport->port_state = IB_PORT_ACTIVE;
-			dispatch_port_event(shca, port, IB_EVENT_PORT_ACTIVE,
-					    "is active");
-			ehca_query_sma_attr(shca, port, &sport->saved_attr);
-		} else {
-			sport->port_state = IB_PORT_DOWN;
-			dispatch_port_event(shca, port, IB_EVENT_PORT_ERR,
-					    "is inactive");
-		}
-		break;
-	case 0x31:
-		/* port configuration change
-		 * disruptive change is caused by
-		 * LID, PKEY or SM change
-		 */
-		if (EHCA_BMASK_GET(NEQE_DISRUPTIVE, eqe)) {
-			ehca_warn(&shca->ib_device, "disruptive port "
-				  "%d configuration change", port);
-
-			sport->port_state = IB_PORT_DOWN;
-			dispatch_port_event(shca, port, IB_EVENT_PORT_ERR,
-					    "is inactive");
-
-			sport->port_state = IB_PORT_ACTIVE;
-			dispatch_port_event(shca, port, IB_EVENT_PORT_ACTIVE,
-					    "is active");
-			ehca_query_sma_attr(shca, port,
-					    &sport->saved_attr);
-		} else
-			notify_port_conf_change(shca, port);
-		break;
-	case 0x32: /* adapter malfunction */
-		ehca_err(&shca->ib_device, "Adapter malfunction.");
-		break;
-	case 0x33:  /* trace stopped */
-		ehca_err(&shca->ib_device, "Traced stopped.");
-		break;
-	case 0x34: /* util async event */
-		spec_event = EHCA_BMASK_GET(NEQE_SPECIFIC_EVENT, eqe);
-		if (spec_event == 0x80) /* client reregister required */
-			dispatch_port_event(shca, port,
-					    IB_EVENT_CLIENT_REREGISTER,
-					    "client reregister req.");
-		else
-			ehca_warn(&shca->ib_device, "Unknown util async "
-				  "event %x on port %x", spec_event, port);
-		break;
-	default:
-		ehca_err(&shca->ib_device, "Unknown event code: %x on %s.",
-			 ec, shca->ib_device.name);
-		break;
-	}
-
-	return;
-}
-
-static inline void reset_eq_pending(struct ehca_cq *cq)
-{
-	u64 CQx_EP;
-	struct h_galpa gal = cq->galpas.kernel;
-
-	hipz_galpa_store_cq(gal, cqx_ep, 0x0);
-	CQx_EP = hipz_galpa_load(gal, CQTEMM_OFFSET(cqx_ep));
-
-	return;
-}
-
-irqreturn_t ehca_interrupt_neq(int irq, void *dev_id)
-{
-	struct ehca_shca *shca = (struct ehca_shca*)dev_id;
-
-	tasklet_hi_schedule(&shca->neq.interrupt_task);
-
-	return IRQ_HANDLED;
-}
-
-void ehca_tasklet_neq(unsigned long data)
-{
-	struct ehca_shca *shca = (struct ehca_shca*)data;
-	struct ehca_eqe *eqe;
-	u64 ret;
-
-	eqe = ehca_poll_eq(shca, &shca->neq);
-
-	while (eqe) {
-		if (!EHCA_BMASK_GET(NEQE_COMPLETION_EVENT, eqe->entry))
-			parse_ec(shca, eqe->entry);
-
-		eqe = ehca_poll_eq(shca, &shca->neq);
-	}
-
-	ret = hipz_h_reset_event(shca->ipz_hca_handle,
-				 shca->neq.ipz_eq_handle, 0xFFFFFFFFFFFFFFFFL);
-
-	if (ret != H_SUCCESS)
-		ehca_err(&shca->ib_device, "Can't clear notification events.");
-
-	return;
-}
-
-irqreturn_t ehca_interrupt_eq(int irq, void *dev_id)
-{
-	struct ehca_shca *shca = (struct ehca_shca*)dev_id;
-
-	tasklet_hi_schedule(&shca->eq.interrupt_task);
-
-	return IRQ_HANDLED;
-}
-
-
-static inline void process_eqe(struct ehca_shca *shca, struct ehca_eqe *eqe)
-{
-	u64 eqe_value;
-	u32 token;
-	struct ehca_cq *cq;
-
-	eqe_value = eqe->entry;
-	ehca_dbg(&shca->ib_device, "eqe_value=%llx", eqe_value);
-	if (EHCA_BMASK_GET(EQE_COMPLETION_EVENT, eqe_value)) {
-		ehca_dbg(&shca->ib_device, "Got completion event");
-		token = EHCA_BMASK_GET(EQE_CQ_TOKEN, eqe_value);
-		read_lock(&ehca_cq_idr_lock);
-		cq = idr_find(&ehca_cq_idr, token);
-		if (cq)
-			atomic_inc(&cq->nr_events);
-		read_unlock(&ehca_cq_idr_lock);
-		if (cq == NULL) {
-			ehca_err(&shca->ib_device,
-				 "Invalid eqe for non-existing cq token=%x",
-				 token);
-			return;
-		}
-		reset_eq_pending(cq);
-		if (ehca_scaling_code)
-			queue_comp_task(cq);
-		else {
-			comp_event_callback(cq);
-			if (atomic_dec_and_test(&cq->nr_events))
-				wake_up(&cq->wait_completion);
-		}
-	} else {
-		ehca_dbg(&shca->ib_device, "Got non completion event");
-		parse_identifier(shca, eqe_value);
-	}
-}
-
-void ehca_process_eq(struct ehca_shca *shca, int is_irq)
-{
-	struct ehca_eq *eq = &shca->eq;
-	struct ehca_eqe_cache_entry *eqe_cache = eq->eqe_cache;
-	u64 eqe_value, ret;
-	int eqe_cnt, i;
-	int eq_empty = 0;
-
-	spin_lock(&eq->irq_spinlock);
-	if (is_irq) {
-		const int max_query_cnt = 100;
-		int query_cnt = 0;
-		int int_state = 1;
-		do {
-			int_state = hipz_h_query_int_state(
-				shca->ipz_hca_handle, eq->ist);
-			query_cnt++;
-			iosync();
-		} while (int_state && query_cnt < max_query_cnt);
-		if (unlikely((query_cnt == max_query_cnt)))
-			ehca_dbg(&shca->ib_device, "int_state=%x query_cnt=%x",
-				 int_state, query_cnt);
-	}
-
-	/* read out all eqes */
-	eqe_cnt = 0;
-	do {
-		u32 token;
-		eqe_cache[eqe_cnt].eqe = ehca_poll_eq(shca, eq);
-		if (!eqe_cache[eqe_cnt].eqe)
-			break;
-		eqe_value = eqe_cache[eqe_cnt].eqe->entry;
-		if (EHCA_BMASK_GET(EQE_COMPLETION_EVENT, eqe_value)) {
-			token = EHCA_BMASK_GET(EQE_CQ_TOKEN, eqe_value);
-			read_lock(&ehca_cq_idr_lock);
-			eqe_cache[eqe_cnt].cq = idr_find(&ehca_cq_idr, token);
-			if (eqe_cache[eqe_cnt].cq)
-				atomic_inc(&eqe_cache[eqe_cnt].cq->nr_events);
-			read_unlock(&ehca_cq_idr_lock);
-			if (!eqe_cache[eqe_cnt].cq) {
-				ehca_err(&shca->ib_device,
-					 "Invalid eqe for non-existing cq "
-					 "token=%x", token);
-				continue;
-			}
-		} else
-			eqe_cache[eqe_cnt].cq = NULL;
-		eqe_cnt++;
-	} while (eqe_cnt < EHCA_EQE_CACHE_SIZE);
-	if (!eqe_cnt) {
-		if (is_irq)
-			ehca_dbg(&shca->ib_device,
-				 "No eqe found for irq event");
-		goto unlock_irq_spinlock;
-	} else if (!is_irq) {
-		ret = hipz_h_eoi(eq->ist);
-		if (ret != H_SUCCESS)
-			ehca_err(&shca->ib_device,
-				 "bad return code EOI -rc = %lld\n", ret);
-		ehca_dbg(&shca->ib_device, "deadman found %x eqe", eqe_cnt);
-	}
-	if (unlikely(eqe_cnt == EHCA_EQE_CACHE_SIZE))
-		ehca_dbg(&shca->ib_device, "too many eqes for one irq event");
-	/* enable irq for new packets */
-	for (i = 0; i < eqe_cnt; i++) {
-		if (eq->eqe_cache[i].cq)
-			reset_eq_pending(eq->eqe_cache[i].cq);
-	}
-	/* check eq */
-	spin_lock(&eq->spinlock);
-	eq_empty = (!ipz_eqit_eq_peek_valid(&shca->eq.ipz_queue));
-	spin_unlock(&eq->spinlock);
-	/* call completion handler for cached eqes */
-	for (i = 0; i < eqe_cnt; i++)
-		if (eq->eqe_cache[i].cq) {
-			if (ehca_scaling_code)
-				queue_comp_task(eq->eqe_cache[i].cq);
-			else {
-				struct ehca_cq *cq = eq->eqe_cache[i].cq;
-				comp_event_callback(cq);
-				if (atomic_dec_and_test(&cq->nr_events))
-					wake_up(&cq->wait_completion);
-			}
-		} else {
-			ehca_dbg(&shca->ib_device, "Got non completion event");
-			parse_identifier(shca, eq->eqe_cache[i].eqe->entry);
-		}
-	/* poll eq if not empty */
-	if (eq_empty)
-		goto unlock_irq_spinlock;
-	do {
-		struct ehca_eqe *eqe;
-		eqe = ehca_poll_eq(shca, &shca->eq);
-		if (!eqe)
-			break;
-		process_eqe(shca, eqe);
-	} while (1);
-
-unlock_irq_spinlock:
-	spin_unlock(&eq->irq_spinlock);
-}
-
-void ehca_tasklet_eq(unsigned long data)
-{
-	ehca_process_eq((struct ehca_shca*)data, 1);
-}
-
-static int find_next_online_cpu(struct ehca_comp_pool *pool)
-{
-	int cpu;
-	unsigned long flags;
-
-	WARN_ON_ONCE(!in_interrupt());
-	if (ehca_debug_level >= 3)
-		ehca_dmp(cpu_online_mask, cpumask_size(), "");
-
-	spin_lock_irqsave(&pool->last_cpu_lock, flags);
-	do {
-		cpu = cpumask_next(pool->last_cpu, cpu_online_mask);
-		if (cpu >= nr_cpu_ids)
-			cpu = cpumask_first(cpu_online_mask);
-		pool->last_cpu = cpu;
-	} while (!per_cpu_ptr(pool->cpu_comp_tasks, cpu)->active);
-	spin_unlock_irqrestore(&pool->last_cpu_lock, flags);
-
-	return cpu;
-}
-
-static void __queue_comp_task(struct ehca_cq *__cq,
-			      struct ehca_cpu_comp_task *cct,
-			      struct task_struct *thread)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&cct->task_lock, flags);
-	spin_lock(&__cq->task_lock);
-
-	if (__cq->nr_callbacks == 0) {
-		__cq->nr_callbacks++;
-		list_add_tail(&__cq->entry, &cct->cq_list);
-		cct->cq_jobs++;
-		wake_up_process(thread);
-	} else
-		__cq->nr_callbacks++;
-
-	spin_unlock(&__cq->task_lock);
-	spin_unlock_irqrestore(&cct->task_lock, flags);
-}
-
-static void queue_comp_task(struct ehca_cq *__cq)
-{
-	int cpu_id;
-	struct ehca_cpu_comp_task *cct;
-	struct task_struct *thread;
-	int cq_jobs;
-	unsigned long flags;
-
-	cpu_id = find_next_online_cpu(pool);
-	BUG_ON(!cpu_online(cpu_id));
-
-	cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu_id);
-	thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu_id);
-	BUG_ON(!cct || !thread);
-
-	spin_lock_irqsave(&cct->task_lock, flags);
-	cq_jobs = cct->cq_jobs;
-	spin_unlock_irqrestore(&cct->task_lock, flags);
-	if (cq_jobs > 0) {
-		cpu_id = find_next_online_cpu(pool);
-		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu_id);
-		thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu_id);
-		BUG_ON(!cct || !thread);
-	}
-	__queue_comp_task(__cq, cct, thread);
-}
-
-static void run_comp_task(struct ehca_cpu_comp_task *cct)
-{
-	struct ehca_cq *cq;
-
-	while (!list_empty(&cct->cq_list)) {
-		cq = list_entry(cct->cq_list.next, struct ehca_cq, entry);
-		spin_unlock_irq(&cct->task_lock);
-
-		comp_event_callback(cq);
-		if (atomic_dec_and_test(&cq->nr_events))
-			wake_up(&cq->wait_completion);
-
-		spin_lock_irq(&cct->task_lock);
-		spin_lock(&cq->task_lock);
-		cq->nr_callbacks--;
-		if (!cq->nr_callbacks) {
-			list_del_init(cct->cq_list.next);
-			cct->cq_jobs--;
-		}
-		spin_unlock(&cq->task_lock);
-	}
-}
-
-static void comp_task_park(unsigned int cpu)
-{
-	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-	struct ehca_cpu_comp_task *target;
-	struct task_struct *thread;
-	struct ehca_cq *cq, *tmp;
-	LIST_HEAD(list);
-
-	spin_lock_irq(&cct->task_lock);
-	cct->cq_jobs = 0;
-	cct->active = 0;
-	list_splice_init(&cct->cq_list, &list);
-	spin_unlock_irq(&cct->task_lock);
-
-	cpu = find_next_online_cpu(pool);
-	target = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-	thread = *per_cpu_ptr(pool->cpu_comp_threads, cpu);
-	spin_lock_irq(&target->task_lock);
-	list_for_each_entry_safe(cq, tmp, &list, entry) {
-		list_del(&cq->entry);
-		__queue_comp_task(cq, target, thread);
-	}
-	spin_unlock_irq(&target->task_lock);
-}
-
-static void comp_task_stop(unsigned int cpu, bool online)
-{
-	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-
-	spin_lock_irq(&cct->task_lock);
-	cct->cq_jobs = 0;
-	cct->active = 0;
-	WARN_ON(!list_empty(&cct->cq_list));
-	spin_unlock_irq(&cct->task_lock);
-}
-
-static int comp_task_should_run(unsigned int cpu)
-{
-	struct ehca_cpu_comp_task *cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-
-	return cct->cq_jobs;
-}
-
-static void comp_task(unsigned int cpu)
-{
-	struct ehca_cpu_comp_task *cct = this_cpu_ptr(pool->cpu_comp_tasks);
-	int cql_empty;
-
-	spin_lock_irq(&cct->task_lock);
-	cql_empty = list_empty(&cct->cq_list);
-	if (!cql_empty) {
-		__set_current_state(TASK_RUNNING);
-		run_comp_task(cct);
-	}
-	spin_unlock_irq(&cct->task_lock);
-}
-
-static struct smp_hotplug_thread comp_pool_threads = {
-	.thread_should_run	= comp_task_should_run,
-	.thread_fn		= comp_task,
-	.thread_comm		= "ehca_comp/%u",
-	.cleanup		= comp_task_stop,
-	.park			= comp_task_park,
-};
-
-int ehca_create_comp_pool(void)
-{
-	int cpu, ret = -ENOMEM;
-
-	if (!ehca_scaling_code)
-		return 0;
-
-	pool = kzalloc(sizeof(struct ehca_comp_pool), GFP_KERNEL);
-	if (pool == NULL)
-		return -ENOMEM;
-
-	spin_lock_init(&pool->last_cpu_lock);
-	pool->last_cpu = cpumask_any(cpu_online_mask);
-
-	pool->cpu_comp_tasks = alloc_percpu(struct ehca_cpu_comp_task);
-	if (!pool->cpu_comp_tasks)
-		goto out_pool;
-
-	pool->cpu_comp_threads = alloc_percpu(struct task_struct *);
-	if (!pool->cpu_comp_threads)
-		goto out_tasks;
-
-	for_each_present_cpu(cpu) {
-		struct ehca_cpu_comp_task *cct;
-
-		cct = per_cpu_ptr(pool->cpu_comp_tasks, cpu);
-		spin_lock_init(&cct->task_lock);
-		INIT_LIST_HEAD(&cct->cq_list);
-	}
-
-	comp_pool_threads.store = pool->cpu_comp_threads;
-	ret = smpboot_register_percpu_thread(&comp_pool_threads);
-	if (ret)
-		goto out_threads;
-
-	pr_info("eHCA scaling code enabled\n");
-	return ret;
-
-out_threads:
-	free_percpu(pool->cpu_comp_threads);
-out_tasks:
-	free_percpu(pool->cpu_comp_tasks);
-out_pool:
-	kfree(pool);
-	return ret;
-}
-
-void ehca_destroy_comp_pool(void)
-{
-	if (!ehca_scaling_code)
-		return;
-
-	smpboot_unregister_percpu_thread(&comp_pool_threads);
-
-	free_percpu(pool->cpu_comp_threads);
-	free_percpu(pool->cpu_comp_tasks);
-	kfree(pool);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_irq.h b/drivers/staging/rdma/ehca/ehca_irq.h
deleted file mode 100644
index 5370199f08c7..000000000000
--- a/drivers/staging/rdma/ehca/ehca_irq.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Function definitions and structs for EQs, NEQs and interrupts
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Khadija Souissi <souissi@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __EHCA_IRQ_H
-#define __EHCA_IRQ_H
-
-
-struct ehca_shca;
-
-#include <linux/interrupt.h>
-#include <linux/types.h>
-
-int ehca_error_data(struct ehca_shca *shca, void *data, u64 resource);
-
-irqreturn_t ehca_interrupt_neq(int irq, void *dev_id);
-void ehca_tasklet_neq(unsigned long data);
-
-irqreturn_t ehca_interrupt_eq(int irq, void *dev_id);
-void ehca_tasklet_eq(unsigned long data);
-void ehca_process_eq(struct ehca_shca *shca, int is_irq);
-
-struct ehca_cpu_comp_task {
-	struct list_head cq_list;
-	spinlock_t task_lock;
-	int cq_jobs;
-	int active;
-};
-
-struct ehca_comp_pool {
-	struct ehca_cpu_comp_task __percpu *cpu_comp_tasks;
-	struct task_struct * __percpu *cpu_comp_threads;
-	int last_cpu;
-	spinlock_t last_cpu_lock;
-};
-
-int ehca_create_comp_pool(void);
-void ehca_destroy_comp_pool(void);
-
-#endif
diff --git a/drivers/staging/rdma/ehca/ehca_iverbs.h b/drivers/staging/rdma/ehca/ehca_iverbs.h
deleted file mode 100644
index 80e6a3d5df3e..000000000000
--- a/drivers/staging/rdma/ehca/ehca_iverbs.h
+++ /dev/null
@@ -1,218 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Function definitions for internal functions
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Dietmar Decker <ddecker@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __EHCA_IVERBS_H__
-#define __EHCA_IVERBS_H__
-
-#include "ehca_classes.h"
-
-int ehca_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
-		      struct ib_udata *uhw);
-
-int ehca_query_port(struct ib_device *ibdev, u8 port,
-		    struct ib_port_attr *props);
-
-enum rdma_protocol_type
-ehca_query_protocol(struct ib_device *device, u8 port_num);
-
-int ehca_query_sma_attr(struct ehca_shca *shca, u8 port,
-			struct ehca_sma_attr *attr);
-
-int ehca_query_pkey(struct ib_device *ibdev, u8 port, u16 index, u16 * pkey);
-
-int ehca_query_gid(struct ib_device *ibdev, u8 port, int index,
-		   union ib_gid *gid);
-
-int ehca_modify_port(struct ib_device *ibdev, u8 port, int port_modify_mask,
-		     struct ib_port_modify *props);
-
-struct ib_pd *ehca_alloc_pd(struct ib_device *device,
-			    struct ib_ucontext *context,
-			    struct ib_udata *udata);
-
-int ehca_dealloc_pd(struct ib_pd *pd);
-
-struct ib_ah *ehca_create_ah(struct ib_pd *pd, struct ib_ah_attr *ah_attr);
-
-int ehca_modify_ah(struct ib_ah *ah, struct ib_ah_attr *ah_attr);
-
-int ehca_query_ah(struct ib_ah *ah, struct ib_ah_attr *ah_attr);
-
-int ehca_destroy_ah(struct ib_ah *ah);
-
-struct ib_mr *ehca_get_dma_mr(struct ib_pd *pd, int mr_access_flags);
-
-struct ib_mr *ehca_reg_phys_mr(struct ib_pd *pd,
-			       struct ib_phys_buf *phys_buf_array,
-			       int num_phys_buf,
-			       int mr_access_flags, u64 *iova_start);
-
-struct ib_mr *ehca_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
-			       u64 virt, int mr_access_flags,
-			       struct ib_udata *udata);
-
-int ehca_rereg_phys_mr(struct ib_mr *mr,
-		       int mr_rereg_mask,
-		       struct ib_pd *pd,
-		       struct ib_phys_buf *phys_buf_array,
-		       int num_phys_buf, int mr_access_flags, u64 *iova_start);
-
-int ehca_query_mr(struct ib_mr *mr, struct ib_mr_attr *mr_attr);
-
-int ehca_dereg_mr(struct ib_mr *mr);
-
-struct ib_mw *ehca_alloc_mw(struct ib_pd *pd, enum ib_mw_type type);
-
-int ehca_bind_mw(struct ib_qp *qp, struct ib_mw *mw,
-		 struct ib_mw_bind *mw_bind);
-
-int ehca_dealloc_mw(struct ib_mw *mw);
-
-struct ib_fmr *ehca_alloc_fmr(struct ib_pd *pd,
-			      int mr_access_flags,
-			      struct ib_fmr_attr *fmr_attr);
-
-int ehca_map_phys_fmr(struct ib_fmr *fmr,
-		      u64 *page_list, int list_len, u64 iova);
-
-int ehca_unmap_fmr(struct list_head *fmr_list);
-
-int ehca_dealloc_fmr(struct ib_fmr *fmr);
-
-enum ehca_eq_type {
-	EHCA_EQ = 0, /* Event Queue              */
-	EHCA_NEQ     /* Notification Event Queue */
-};
-
-int ehca_create_eq(struct ehca_shca *shca, struct ehca_eq *eq,
-		   enum ehca_eq_type type, const u32 length);
-
-int ehca_destroy_eq(struct ehca_shca *shca, struct ehca_eq *eq);
-
-void *ehca_poll_eq(struct ehca_shca *shca, struct ehca_eq *eq);
-
-
-struct ib_cq *ehca_create_cq(struct ib_device *device,
-			     const struct ib_cq_init_attr *attr,
-			     struct ib_ucontext *context,
-			     struct ib_udata *udata);
-
-int ehca_destroy_cq(struct ib_cq *cq);
-
-int ehca_resize_cq(struct ib_cq *cq, int cqe, struct ib_udata *udata);
-
-int ehca_poll_cq(struct ib_cq *cq, int num_entries, struct ib_wc *wc);
-
-int ehca_peek_cq(struct ib_cq *cq, int wc_cnt);
-
-int ehca_req_notify_cq(struct ib_cq *cq, enum ib_cq_notify_flags notify_flags);
-
-struct ib_qp *ehca_create_qp(struct ib_pd *pd,
-			     struct ib_qp_init_attr *init_attr,
-			     struct ib_udata *udata);
-
-int ehca_destroy_qp(struct ib_qp *qp);
-
-int ehca_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr, int attr_mask,
-		   struct ib_udata *udata);
-
-int ehca_query_qp(struct ib_qp *qp, struct ib_qp_attr *qp_attr,
-		  int qp_attr_mask, struct ib_qp_init_attr *qp_init_attr);
-
-int ehca_post_send(struct ib_qp *qp, struct ib_send_wr *send_wr,
-		   struct ib_send_wr **bad_send_wr);
-
-int ehca_post_recv(struct ib_qp *qp, struct ib_recv_wr *recv_wr,
-		   struct ib_recv_wr **bad_recv_wr);
-
-int ehca_post_srq_recv(struct ib_srq *srq,
-		       struct ib_recv_wr *recv_wr,
-		       struct ib_recv_wr **bad_recv_wr);
-
-struct ib_srq *ehca_create_srq(struct ib_pd *pd,
-			       struct ib_srq_init_attr *init_attr,
-			       struct ib_udata *udata);
-
-int ehca_modify_srq(struct ib_srq *srq, struct ib_srq_attr *attr,
-		    enum ib_srq_attr_mask attr_mask, struct ib_udata *udata);
-
-int ehca_query_srq(struct ib_srq *srq, struct ib_srq_attr *srq_attr);
-
-int ehca_destroy_srq(struct ib_srq *srq);
-
-u64 ehca_define_sqp(struct ehca_shca *shca, struct ehca_qp *ibqp,
-		    struct ib_qp_init_attr *qp_init_attr);
-
-int ehca_attach_mcast(struct ib_qp *qp, union ib_gid *gid, u16 lid);
-
-int ehca_detach_mcast(struct ib_qp *qp, union ib_gid *gid, u16 lid);
-
-struct ib_ucontext *ehca_alloc_ucontext(struct ib_device *device,
-					struct ib_udata *udata);
-
-int ehca_dealloc_ucontext(struct ib_ucontext *context);
-
-int ehca_mmap(struct ib_ucontext *context, struct vm_area_struct *vma);
-
-int ehca_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
-		     const struct ib_mad_hdr *in, size_t in_mad_size,
-		     struct ib_mad_hdr *out, size_t *out_mad_size,
-		     u16 *out_mad_pkey_index);
-
-void ehca_poll_eqs(unsigned long data);
-
-int ehca_calc_ipd(struct ehca_shca *shca, int port,
-		  enum ib_rate path_rate, u32 *ipd);
-
-void ehca_add_to_err_list(struct ehca_qp *qp, int on_sq);
-
-#ifdef CONFIG_PPC_64K_PAGES
-void *ehca_alloc_fw_ctrlblock(gfp_t flags);
-void ehca_free_fw_ctrlblock(void *ptr);
-#else
-#define ehca_alloc_fw_ctrlblock(flags) ((void *)get_zeroed_page(flags))
-#define ehca_free_fw_ctrlblock(ptr) free_page((unsigned long)(ptr))
-#endif
-
-void ehca_recover_sqp(struct ib_qp *sqp);
-
-#endif
diff --git a/drivers/staging/rdma/ehca/ehca_main.c b/drivers/staging/rdma/ehca/ehca_main.c
deleted file mode 100644
index 8246418cd4e0..000000000000
--- a/drivers/staging/rdma/ehca/ehca_main.c
+++ /dev/null
@@ -1,1123 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  module start stop, hca detection
- *
- *  Authors: Heiko J Schick <schickhj@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Joachim Fenkes <fenkes@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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.
- */
-
-#ifdef CONFIG_PPC_64K_PAGES
-#include <linux/slab.h>
-#endif
-
-#include <linux/notifier.h>
-#include <linux/memory.h>
-#include <rdma/ib_mad.h>
-#include "ehca_classes.h"
-#include "ehca_iverbs.h"
-#include "ehca_mrmw.h"
-#include "ehca_tools.h"
-#include "hcp_if.h"
-
-#define HCAD_VERSION "0029"
-
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_AUTHOR("Christoph Raisch <raisch@de.ibm.com>");
-MODULE_DESCRIPTION("IBM eServer HCA InfiniBand Device Driver");
-MODULE_VERSION(HCAD_VERSION);
-
-static bool ehca_open_aqp1    = 0;
-static int ehca_hw_level      = 0;
-static bool ehca_poll_all_eqs = 1;
-
-int ehca_debug_level   = 0;
-int ehca_nr_ports      = -1;
-bool ehca_use_hp_mr    = 0;
-int ehca_port_act_time = 30;
-int ehca_static_rate   = -1;
-bool ehca_scaling_code = 0;
-int ehca_lock_hcalls   = -1;
-int ehca_max_cq        = -1;
-int ehca_max_qp        = -1;
-
-module_param_named(open_aqp1,     ehca_open_aqp1,     bool, S_IRUGO);
-module_param_named(debug_level,   ehca_debug_level,   int,  S_IRUGO);
-module_param_named(hw_level,      ehca_hw_level,      int,  S_IRUGO);
-module_param_named(nr_ports,      ehca_nr_ports,      int,  S_IRUGO);
-module_param_named(use_hp_mr,     ehca_use_hp_mr,     bool, S_IRUGO);
-module_param_named(port_act_time, ehca_port_act_time, int,  S_IRUGO);
-module_param_named(poll_all_eqs,  ehca_poll_all_eqs,  bool, S_IRUGO);
-module_param_named(static_rate,   ehca_static_rate,   int,  S_IRUGO);
-module_param_named(scaling_code,  ehca_scaling_code,  bool, S_IRUGO);
-module_param_named(lock_hcalls,   ehca_lock_hcalls,   bint, S_IRUGO);
-module_param_named(number_of_cqs, ehca_max_cq,        int,  S_IRUGO);
-module_param_named(number_of_qps, ehca_max_qp,        int,  S_IRUGO);
-
-MODULE_PARM_DESC(open_aqp1,
-		 "Open AQP1 on startup (default: no)");
-MODULE_PARM_DESC(debug_level,
-		 "Amount of debug output (0: none (default), 1: traces, "
-		 "2: some dumps, 3: lots)");
-MODULE_PARM_DESC(hw_level,
-		 "Hardware level (0: autosensing (default), "
-		 "0x10..0x14: eHCA, 0x20..0x23: eHCA2)");
-MODULE_PARM_DESC(nr_ports,
-		 "number of connected ports (-1: autodetect (default), "
-		 "1: port one only, 2: two ports)");
-MODULE_PARM_DESC(use_hp_mr,
-		 "Use high performance MRs (default: no)");
-MODULE_PARM_DESC(port_act_time,
-		 "Time to wait for port activation (default: 30 sec)");
-MODULE_PARM_DESC(poll_all_eqs,
-		 "Poll all event queues periodically (default: yes)");
-MODULE_PARM_DESC(static_rate,
-		 "Set permanent static rate (default: no static rate)");
-MODULE_PARM_DESC(scaling_code,
-		 "Enable scaling code (default: no)");
-MODULE_PARM_DESC(lock_hcalls,
-		 "Serialize all hCalls made by the driver "
-		 "(default: autodetect)");
-MODULE_PARM_DESC(number_of_cqs,
-		"Max number of CQs which can be allocated "
-		"(default: autodetect)");
-MODULE_PARM_DESC(number_of_qps,
-		"Max number of QPs which can be allocated "
-		"(default: autodetect)");
-
-DEFINE_RWLOCK(ehca_qp_idr_lock);
-DEFINE_RWLOCK(ehca_cq_idr_lock);
-DEFINE_IDR(ehca_qp_idr);
-DEFINE_IDR(ehca_cq_idr);
-
-static LIST_HEAD(shca_list); /* list of all registered ehcas */
-DEFINE_SPINLOCK(shca_list_lock);
-
-static struct timer_list poll_eqs_timer;
-
-#ifdef CONFIG_PPC_64K_PAGES
-static struct kmem_cache *ctblk_cache;
-
-void *ehca_alloc_fw_ctrlblock(gfp_t flags)
-{
-	void *ret = kmem_cache_zalloc(ctblk_cache, flags);
-	if (!ret)
-		ehca_gen_err("Out of memory for ctblk");
-	return ret;
-}
-
-void ehca_free_fw_ctrlblock(void *ptr)
-{
-	if (ptr)
-		kmem_cache_free(ctblk_cache, ptr);
-
-}
-#endif
-
-int ehca2ib_return_code(u64 ehca_rc)
-{
-	switch (ehca_rc) {
-	case H_SUCCESS:
-		return 0;
-	case H_RESOURCE:             /* Resource in use */
-	case H_BUSY:
-		return -EBUSY;
-	case H_NOT_ENOUGH_RESOURCES: /* insufficient resources */
-	case H_CONSTRAINED:          /* resource constraint */
-	case H_NO_MEM:
-		return -ENOMEM;
-	default:
-		return -EINVAL;
-	}
-}
-
-static int ehca_create_slab_caches(void)
-{
-	int ret;
-
-	ret = ehca_init_pd_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create PD SLAB cache.");
-		return ret;
-	}
-
-	ret = ehca_init_cq_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create CQ SLAB cache.");
-		goto create_slab_caches2;
-	}
-
-	ret = ehca_init_qp_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create QP SLAB cache.");
-		goto create_slab_caches3;
-	}
-
-	ret = ehca_init_av_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create AV SLAB cache.");
-		goto create_slab_caches4;
-	}
-
-	ret = ehca_init_mrmw_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create MR&MW SLAB cache.");
-		goto create_slab_caches5;
-	}
-
-	ret = ehca_init_small_qp_cache();
-	if (ret) {
-		ehca_gen_err("Cannot create small queue SLAB cache.");
-		goto create_slab_caches6;
-	}
-
-#ifdef CONFIG_PPC_64K_PAGES
-	ctblk_cache = kmem_cache_create("ehca_cache_ctblk",
-					EHCA_PAGESIZE, H_CB_ALIGNMENT,
-					SLAB_HWCACHE_ALIGN,
-					NULL);
-	if (!ctblk_cache) {
-		ehca_gen_err("Cannot create ctblk SLAB cache.");
-		ehca_cleanup_small_qp_cache();
-		ret = -ENOMEM;
-		goto create_slab_caches6;
-	}
-#endif
-	return 0;
-
-create_slab_caches6:
-	ehca_cleanup_mrmw_cache();
-
-create_slab_caches5:
-	ehca_cleanup_av_cache();
-
-create_slab_caches4:
-	ehca_cleanup_qp_cache();
-
-create_slab_caches3:
-	ehca_cleanup_cq_cache();
-
-create_slab_caches2:
-	ehca_cleanup_pd_cache();
-
-	return ret;
-}
-
-static void ehca_destroy_slab_caches(void)
-{
-	ehca_cleanup_small_qp_cache();
-	ehca_cleanup_mrmw_cache();
-	ehca_cleanup_av_cache();
-	ehca_cleanup_qp_cache();
-	ehca_cleanup_cq_cache();
-	ehca_cleanup_pd_cache();
-#ifdef CONFIG_PPC_64K_PAGES
-	if (ctblk_cache)
-		kmem_cache_destroy(ctblk_cache);
-#endif
-}
-
-#define EHCA_HCAAVER  EHCA_BMASK_IBM(32, 39)
-#define EHCA_REVID    EHCA_BMASK_IBM(40, 63)
-
-static struct cap_descr {
-	u64 mask;
-	char *descr;
-} hca_cap_descr[] = {
-	{ HCA_CAP_AH_PORT_NR_CHECK, "HCA_CAP_AH_PORT_NR_CHECK" },
-	{ HCA_CAP_ATOMIC, "HCA_CAP_ATOMIC" },
-	{ HCA_CAP_AUTO_PATH_MIG, "HCA_CAP_AUTO_PATH_MIG" },
-	{ HCA_CAP_BAD_P_KEY_CTR, "HCA_CAP_BAD_P_KEY_CTR" },
-	{ HCA_CAP_SQD_RTS_PORT_CHANGE, "HCA_CAP_SQD_RTS_PORT_CHANGE" },
-	{ HCA_CAP_CUR_QP_STATE_MOD, "HCA_CAP_CUR_QP_STATE_MOD" },
-	{ HCA_CAP_INIT_TYPE, "HCA_CAP_INIT_TYPE" },
-	{ HCA_CAP_PORT_ACTIVE_EVENT, "HCA_CAP_PORT_ACTIVE_EVENT" },
-	{ HCA_CAP_Q_KEY_VIOL_CTR, "HCA_CAP_Q_KEY_VIOL_CTR" },
-	{ HCA_CAP_WQE_RESIZE, "HCA_CAP_WQE_RESIZE" },
-	{ HCA_CAP_RAW_PACKET_MCAST, "HCA_CAP_RAW_PACKET_MCAST" },
-	{ HCA_CAP_SHUTDOWN_PORT, "HCA_CAP_SHUTDOWN_PORT" },
-	{ HCA_CAP_RC_LL_QP, "HCA_CAP_RC_LL_QP" },
-	{ HCA_CAP_SRQ, "HCA_CAP_SRQ" },
-	{ HCA_CAP_UD_LL_QP, "HCA_CAP_UD_LL_QP" },
-	{ HCA_CAP_RESIZE_MR, "HCA_CAP_RESIZE_MR" },
-	{ HCA_CAP_MINI_QP, "HCA_CAP_MINI_QP" },
-	{ HCA_CAP_H_ALLOC_RES_SYNC, "HCA_CAP_H_ALLOC_RES_SYNC" },
-};
-
-static int ehca_sense_attributes(struct ehca_shca *shca)
-{
-	int i, ret = 0;
-	u64 h_ret;
-	struct hipz_query_hca *rblock;
-	struct hipz_query_port *port;
-	const char *loc_code;
-
-	static const u32 pgsize_map[] = {
-		HCA_CAP_MR_PGSIZE_4K,  0x1000,
-		HCA_CAP_MR_PGSIZE_64K, 0x10000,
-		HCA_CAP_MR_PGSIZE_1M,  0x100000,
-		HCA_CAP_MR_PGSIZE_16M, 0x1000000,
-	};
-
-	ehca_gen_dbg("Probing adapter %s...",
-		     shca->ofdev->dev.of_node->full_name);
-	loc_code = of_get_property(shca->ofdev->dev.of_node, "ibm,loc-code",
-				   NULL);
-	if (loc_code)
-		ehca_gen_dbg(" ... location lode=%s", loc_code);
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_gen_err("Cannot allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_hca(shca->ipz_hca_handle, rblock);
-	if (h_ret != H_SUCCESS) {
-		ehca_gen_err("Cannot query device properties. h_ret=%lli",
-			     h_ret);
-		ret = -EPERM;
-		goto sense_attributes1;
-	}
-
-	if (ehca_nr_ports == 1)
-		shca->num_ports = 1;
-	else
-		shca->num_ports = (u8)rblock->num_ports;
-
-	ehca_gen_dbg(" ... found %x ports", rblock->num_ports);
-
-	if (ehca_hw_level == 0) {
-		u32 hcaaver;
-		u32 revid;
-
-		hcaaver = EHCA_BMASK_GET(EHCA_HCAAVER, rblock->hw_ver);
-		revid   = EHCA_BMASK_GET(EHCA_REVID, rblock->hw_ver);
-
-		ehca_gen_dbg(" ... hardware version=%x:%x", hcaaver, revid);
-
-		if (hcaaver == 1) {
-			if (revid <= 3)
-				shca->hw_level = 0x10 | (revid + 1);
-			else
-				shca->hw_level = 0x14;
-		} else if (hcaaver == 2) {
-			if (revid == 0)
-				shca->hw_level = 0x21;
-			else if (revid == 0x10)
-				shca->hw_level = 0x22;
-			else if (revid == 0x20 || revid == 0x21)
-				shca->hw_level = 0x23;
-		}
-
-		if (!shca->hw_level) {
-			ehca_gen_warn("unknown hardware version"
-				      " - assuming default level");
-			shca->hw_level = 0x22;
-		}
-	} else
-		shca->hw_level = ehca_hw_level;
-	ehca_gen_dbg(" ... hardware level=%x", shca->hw_level);
-
-	shca->hca_cap = rblock->hca_cap_indicators;
-	ehca_gen_dbg(" ... HCA capabilities:");
-	for (i = 0; i < ARRAY_SIZE(hca_cap_descr); i++)
-		if (EHCA_BMASK_GET(hca_cap_descr[i].mask, shca->hca_cap))
-			ehca_gen_dbg("   %s", hca_cap_descr[i].descr);
-
-	/* Autodetect hCall locking -- the "H_ALLOC_RESOURCE synced" flag is
-	 * a firmware property, so it's valid across all adapters
-	 */
-	if (ehca_lock_hcalls == -1)
-		ehca_lock_hcalls = !EHCA_BMASK_GET(HCA_CAP_H_ALLOC_RES_SYNC,
-					shca->hca_cap);
-
-	/* translate supported MR page sizes; always support 4K */
-	shca->hca_cap_mr_pgsize = EHCA_PAGESIZE;
-	for (i = 0; i < ARRAY_SIZE(pgsize_map); i += 2)
-		if (rblock->memory_page_size_supported & pgsize_map[i])
-			shca->hca_cap_mr_pgsize |= pgsize_map[i + 1];
-
-	/* Set maximum number of CQs and QPs to calculate EQ size */
-	if (shca->max_num_qps == -1)
-		shca->max_num_qps = min_t(int, rblock->max_qp,
-					  EHCA_MAX_NUM_QUEUES);
-	else if (shca->max_num_qps < 1 || shca->max_num_qps > rblock->max_qp) {
-		ehca_gen_warn("The requested number of QPs is out of range "
-			      "(1 - %i) specified by HW. Value is set to %i",
-			      rblock->max_qp, rblock->max_qp);
-		shca->max_num_qps = rblock->max_qp;
-	}
-
-	if (shca->max_num_cqs == -1)
-		shca->max_num_cqs = min_t(int, rblock->max_cq,
-					  EHCA_MAX_NUM_QUEUES);
-	else if (shca->max_num_cqs < 1 || shca->max_num_cqs > rblock->max_cq) {
-		ehca_gen_warn("The requested number of CQs is out of range "
-			      "(1 - %i) specified by HW. Value is set to %i",
-			      rblock->max_cq, rblock->max_cq);
-	}
-
-	/* query max MTU from first port -- it's the same for all ports */
-	port = (struct hipz_query_port *)rblock;
-	h_ret = hipz_h_query_port(shca->ipz_hca_handle, 1, port);
-	if (h_ret != H_SUCCESS) {
-		ehca_gen_err("Cannot query port properties. h_ret=%lli",
-			     h_ret);
-		ret = -EPERM;
-		goto sense_attributes1;
-	}
-
-	shca->max_mtu = port->max_mtu;
-
-sense_attributes1:
-	ehca_free_fw_ctrlblock(rblock);
-	return ret;
-}
-
-static int init_node_guid(struct ehca_shca *shca)
-{
-	int ret = 0;
-	struct hipz_query_hca *rblock;
-
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!rblock) {
-		ehca_err(&shca->ib_device, "Can't allocate rblock memory.");
-		return -ENOMEM;
-	}
-
-	if (hipz_h_query_hca(shca->ipz_hca_handle, rblock) != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "Can't query device properties");
-		ret = -EINVAL;
-		goto init_node_guid1;
-	}
-
-	memcpy(&shca->ib_device.node_guid, &rblock->node_guid, sizeof(u64));
-
-init_node_guid1:
-	ehca_free_fw_ctrlblock(rblock);
-	return ret;
-}
-
-static int ehca_port_immutable(struct ib_device *ibdev, u8 port_num,
-			       struct ib_port_immutable *immutable)
-{
-	struct ib_port_attr attr;
-	int err;
-
-	err = ehca_query_port(ibdev, port_num, &attr);
-	if (err)
-		return err;
-
-	immutable->pkey_tbl_len = attr.pkey_tbl_len;
-	immutable->gid_tbl_len = attr.gid_tbl_len;
-	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
-	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
-
-	return 0;
-}
-
-static int ehca_init_device(struct ehca_shca *shca)
-{
-	int ret;
-
-	ret = init_node_guid(shca);
-	if (ret)
-		return ret;
-
-	strlcpy(shca->ib_device.name, "ehca%d", IB_DEVICE_NAME_MAX);
-	shca->ib_device.owner               = THIS_MODULE;
-
-	shca->ib_device.uverbs_abi_ver	    = 8;
-	shca->ib_device.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)		|
-		(1ull << IB_USER_VERBS_CMD_ATTACH_MCAST)	|
-		(1ull << IB_USER_VERBS_CMD_DETACH_MCAST);
-
-	shca->ib_device.node_type           = RDMA_NODE_IB_CA;
-	shca->ib_device.phys_port_cnt       = shca->num_ports;
-	shca->ib_device.num_comp_vectors    = 1;
-	shca->ib_device.dma_device          = &shca->ofdev->dev;
-	shca->ib_device.query_device        = ehca_query_device;
-	shca->ib_device.query_port          = ehca_query_port;
-	shca->ib_device.query_gid           = ehca_query_gid;
-	shca->ib_device.query_pkey          = ehca_query_pkey;
-	/* shca->in_device.modify_device    = ehca_modify_device    */
-	shca->ib_device.modify_port         = ehca_modify_port;
-	shca->ib_device.alloc_ucontext      = ehca_alloc_ucontext;
-	shca->ib_device.dealloc_ucontext    = ehca_dealloc_ucontext;
-	shca->ib_device.alloc_pd            = ehca_alloc_pd;
-	shca->ib_device.dealloc_pd          = ehca_dealloc_pd;
-	shca->ib_device.create_ah	    = ehca_create_ah;
-	/* shca->ib_device.modify_ah	    = ehca_modify_ah;	    */
-	shca->ib_device.query_ah	    = ehca_query_ah;
-	shca->ib_device.destroy_ah	    = ehca_destroy_ah;
-	shca->ib_device.create_qp	    = ehca_create_qp;
-	shca->ib_device.modify_qp	    = ehca_modify_qp;
-	shca->ib_device.query_qp	    = ehca_query_qp;
-	shca->ib_device.destroy_qp	    = ehca_destroy_qp;
-	shca->ib_device.post_send	    = ehca_post_send;
-	shca->ib_device.post_recv	    = ehca_post_recv;
-	shca->ib_device.create_cq	    = ehca_create_cq;
-	shca->ib_device.destroy_cq	    = ehca_destroy_cq;
-	shca->ib_device.resize_cq	    = ehca_resize_cq;
-	shca->ib_device.poll_cq		    = ehca_poll_cq;
-	/* shca->ib_device.peek_cq	    = ehca_peek_cq;	    */
-	shca->ib_device.req_notify_cq	    = ehca_req_notify_cq;
-	/* shca->ib_device.req_ncomp_notif  = ehca_req_ncomp_notif; */
-	shca->ib_device.get_dma_mr	    = ehca_get_dma_mr;
-	shca->ib_device.reg_phys_mr	    = ehca_reg_phys_mr;
-	shca->ib_device.reg_user_mr	    = ehca_reg_user_mr;
-	shca->ib_device.query_mr	    = ehca_query_mr;
-	shca->ib_device.dereg_mr	    = ehca_dereg_mr;
-	shca->ib_device.rereg_phys_mr	    = ehca_rereg_phys_mr;
-	shca->ib_device.alloc_mw	    = ehca_alloc_mw;
-	shca->ib_device.bind_mw		    = ehca_bind_mw;
-	shca->ib_device.dealloc_mw	    = ehca_dealloc_mw;
-	shca->ib_device.alloc_fmr	    = ehca_alloc_fmr;
-	shca->ib_device.map_phys_fmr	    = ehca_map_phys_fmr;
-	shca->ib_device.unmap_fmr	    = ehca_unmap_fmr;
-	shca->ib_device.dealloc_fmr	    = ehca_dealloc_fmr;
-	shca->ib_device.attach_mcast	    = ehca_attach_mcast;
-	shca->ib_device.detach_mcast	    = ehca_detach_mcast;
-	shca->ib_device.process_mad	    = ehca_process_mad;
-	shca->ib_device.mmap		    = ehca_mmap;
-	shca->ib_device.dma_ops		    = &ehca_dma_mapping_ops;
-	shca->ib_device.get_port_immutable  = ehca_port_immutable;
-
-	if (EHCA_BMASK_GET(HCA_CAP_SRQ, shca->hca_cap)) {
-		shca->ib_device.uverbs_cmd_mask |=
-			(1ull << IB_USER_VERBS_CMD_CREATE_SRQ) |
-			(1ull << IB_USER_VERBS_CMD_MODIFY_SRQ) |
-			(1ull << IB_USER_VERBS_CMD_QUERY_SRQ) |
-			(1ull << IB_USER_VERBS_CMD_DESTROY_SRQ);
-
-		shca->ib_device.create_srq          = ehca_create_srq;
-		shca->ib_device.modify_srq          = ehca_modify_srq;
-		shca->ib_device.query_srq           = ehca_query_srq;
-		shca->ib_device.destroy_srq         = ehca_destroy_srq;
-		shca->ib_device.post_srq_recv       = ehca_post_srq_recv;
-	}
-
-	return ret;
-}
-
-static int ehca_create_aqp1(struct ehca_shca *shca, u32 port)
-{
-	struct ehca_sport *sport = &shca->sport[port - 1];
-	struct ib_cq *ibcq;
-	struct ib_qp *ibqp;
-	struct ib_qp_init_attr qp_init_attr;
-	struct ib_cq_init_attr cq_attr = {};
-	int ret;
-
-	if (sport->ibcq_aqp1) {
-		ehca_err(&shca->ib_device, "AQP1 CQ is already created.");
-		return -EPERM;
-	}
-
-	cq_attr.cqe = 10;
-	ibcq = ib_create_cq(&shca->ib_device, NULL, NULL, (void *)(-1),
-			    &cq_attr);
-	if (IS_ERR(ibcq)) {
-		ehca_err(&shca->ib_device, "Cannot create AQP1 CQ.");
-		return PTR_ERR(ibcq);
-	}
-	sport->ibcq_aqp1 = ibcq;
-
-	if (sport->ibqp_sqp[IB_QPT_GSI]) {
-		ehca_err(&shca->ib_device, "AQP1 QP is already created.");
-		ret = -EPERM;
-		goto create_aqp1;
-	}
-
-	memset(&qp_init_attr, 0, sizeof(struct ib_qp_init_attr));
-	qp_init_attr.send_cq          = ibcq;
-	qp_init_attr.recv_cq          = ibcq;
-	qp_init_attr.sq_sig_type      = IB_SIGNAL_ALL_WR;
-	qp_init_attr.cap.max_send_wr  = 100;
-	qp_init_attr.cap.max_recv_wr  = 100;
-	qp_init_attr.cap.max_send_sge = 2;
-	qp_init_attr.cap.max_recv_sge = 1;
-	qp_init_attr.qp_type          = IB_QPT_GSI;
-	qp_init_attr.port_num         = port;
-	qp_init_attr.qp_context       = NULL;
-	qp_init_attr.event_handler    = NULL;
-	qp_init_attr.srq              = NULL;
-
-	ibqp = ib_create_qp(&shca->pd->ib_pd, &qp_init_attr);
-	if (IS_ERR(ibqp)) {
-		ehca_err(&shca->ib_device, "Cannot create AQP1 QP.");
-		ret = PTR_ERR(ibqp);
-		goto create_aqp1;
-	}
-	sport->ibqp_sqp[IB_QPT_GSI] = ibqp;
-
-	return 0;
-
-create_aqp1:
-	ib_destroy_cq(sport->ibcq_aqp1);
-	return ret;
-}
-
-static int ehca_destroy_aqp1(struct ehca_sport *sport)
-{
-	int ret;
-
-	ret = ib_destroy_qp(sport->ibqp_sqp[IB_QPT_GSI]);
-	if (ret) {
-		ehca_gen_err("Cannot destroy AQP1 QP. ret=%i", ret);
-		return ret;
-	}
-
-	ret = ib_destroy_cq(sport->ibcq_aqp1);
-	if (ret)
-		ehca_gen_err("Cannot destroy AQP1 CQ. ret=%i", ret);
-
-	return ret;
-}
-
-static ssize_t ehca_show_debug_level(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%d\n", ehca_debug_level);
-}
-
-static ssize_t ehca_store_debug_level(struct device_driver *ddp,
-				      const char *buf, size_t count)
-{
-	int value = (*buf) - '0';
-	if (value >= 0 && value <= 9)
-		ehca_debug_level = value;
-	return 1;
-}
-
-static DRIVER_ATTR(debug_level, S_IRUSR | S_IWUSR,
-		   ehca_show_debug_level, ehca_store_debug_level);
-
-static struct attribute *ehca_drv_attrs[] = {
-	&driver_attr_debug_level.attr,
-	NULL
-};
-
-static struct attribute_group ehca_drv_attr_grp = {
-	.attrs = ehca_drv_attrs
-};
-
-static const struct attribute_group *ehca_drv_attr_groups[] = {
-	&ehca_drv_attr_grp,
-	NULL,
-};
-
-#define EHCA_RESOURCE_ATTR(name)                                           \
-static ssize_t  ehca_show_##name(struct device *dev,                       \
-				 struct device_attribute *attr,            \
-				 char *buf)                                \
-{									   \
-	struct ehca_shca *shca;						   \
-	struct hipz_query_hca *rblock;				           \
-	int data;                                                          \
-									   \
-	shca = dev_get_drvdata(dev);					   \
-									   \
-	rblock = ehca_alloc_fw_ctrlblock(GFP_KERNEL);			   \
-	if (!rblock) {						           \
-		dev_err(dev, "Can't allocate rblock memory.\n");           \
-		return 0;						   \
-	}								   \
-									   \
-	if (hipz_h_query_hca(shca->ipz_hca_handle, rblock) != H_SUCCESS) { \
-		dev_err(dev, "Can't query device properties\n");           \
-		ehca_free_fw_ctrlblock(rblock);			   	   \
-		return 0;					   	   \
-	}								   \
-									   \
-	data = rblock->name;                                               \
-	ehca_free_fw_ctrlblock(rblock);                                    \
-									   \
-	if ((strcmp(#name, "num_ports") == 0) && (ehca_nr_ports == 1))	   \
-		return snprintf(buf, 256, "1\n");			   \
-	else								   \
-		return snprintf(buf, 256, "%d\n", data);		   \
-									   \
-}									   \
-static DEVICE_ATTR(name, S_IRUGO, ehca_show_##name, NULL);
-
-EHCA_RESOURCE_ATTR(num_ports);
-EHCA_RESOURCE_ATTR(hw_ver);
-EHCA_RESOURCE_ATTR(max_eq);
-EHCA_RESOURCE_ATTR(cur_eq);
-EHCA_RESOURCE_ATTR(max_cq);
-EHCA_RESOURCE_ATTR(cur_cq);
-EHCA_RESOURCE_ATTR(max_qp);
-EHCA_RESOURCE_ATTR(cur_qp);
-EHCA_RESOURCE_ATTR(max_mr);
-EHCA_RESOURCE_ATTR(cur_mr);
-EHCA_RESOURCE_ATTR(max_mw);
-EHCA_RESOURCE_ATTR(cur_mw);
-EHCA_RESOURCE_ATTR(max_pd);
-EHCA_RESOURCE_ATTR(max_ah);
-
-static ssize_t ehca_show_adapter_handle(struct device *dev,
-					struct device_attribute *attr,
-					char *buf)
-{
-	struct ehca_shca *shca = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%llx\n", shca->ipz_hca_handle.handle);
-
-}
-static DEVICE_ATTR(adapter_handle, S_IRUGO, ehca_show_adapter_handle, NULL);
-
-static struct attribute *ehca_dev_attrs[] = {
-	&dev_attr_adapter_handle.attr,
-	&dev_attr_num_ports.attr,
-	&dev_attr_hw_ver.attr,
-	&dev_attr_max_eq.attr,
-	&dev_attr_cur_eq.attr,
-	&dev_attr_max_cq.attr,
-	&dev_attr_cur_cq.attr,
-	&dev_attr_max_qp.attr,
-	&dev_attr_cur_qp.attr,
-	&dev_attr_max_mr.attr,
-	&dev_attr_cur_mr.attr,
-	&dev_attr_max_mw.attr,
-	&dev_attr_cur_mw.attr,
-	&dev_attr_max_pd.attr,
-	&dev_attr_max_ah.attr,
-	NULL
-};
-
-static struct attribute_group ehca_dev_attr_grp = {
-	.attrs = ehca_dev_attrs
-};
-
-static int ehca_probe(struct platform_device *dev)
-{
-	struct ehca_shca *shca;
-	const u64 *handle;
-	struct ib_pd *ibpd;
-	int ret, i, eq_size;
-	unsigned long flags;
-
-	handle = of_get_property(dev->dev.of_node, "ibm,hca-handle", NULL);
-	if (!handle) {
-		ehca_gen_err("Cannot get eHCA handle for adapter: %s.",
-			     dev->dev.of_node->full_name);
-		return -ENODEV;
-	}
-
-	if (!(*handle)) {
-		ehca_gen_err("Wrong eHCA handle for adapter: %s.",
-			     dev->dev.of_node->full_name);
-		return -ENODEV;
-	}
-
-	shca = (struct ehca_shca *)ib_alloc_device(sizeof(*shca));
-	if (!shca) {
-		ehca_gen_err("Cannot allocate shca memory.");
-		return -ENOMEM;
-	}
-
-	mutex_init(&shca->modify_mutex);
-	atomic_set(&shca->num_cqs, 0);
-	atomic_set(&shca->num_qps, 0);
-	shca->max_num_qps = ehca_max_qp;
-	shca->max_num_cqs = ehca_max_cq;
-
-	for (i = 0; i < ARRAY_SIZE(shca->sport); i++)
-		spin_lock_init(&shca->sport[i].mod_sqp_lock);
-
-	shca->ofdev = dev;
-	shca->ipz_hca_handle.handle = *handle;
-	dev_set_drvdata(&dev->dev, shca);
-
-	ret = ehca_sense_attributes(shca);
-	if (ret < 0) {
-		ehca_gen_err("Cannot sense eHCA attributes.");
-		goto probe1;
-	}
-
-	ret = ehca_init_device(shca);
-	if (ret) {
-		ehca_gen_err("Cannot init ehca  device struct");
-		goto probe1;
-	}
-
-	eq_size = 2 * shca->max_num_cqs + 4 * shca->max_num_qps;
-	/* create event queues */
-	ret = ehca_create_eq(shca, &shca->eq, EHCA_EQ, eq_size);
-	if (ret) {
-		ehca_err(&shca->ib_device, "Cannot create EQ.");
-		goto probe1;
-	}
-
-	ret = ehca_create_eq(shca, &shca->neq, EHCA_NEQ, 513);
-	if (ret) {
-		ehca_err(&shca->ib_device, "Cannot create NEQ.");
-		goto probe3;
-	}
-
-	/* create internal protection domain */
-	ibpd = ehca_alloc_pd(&shca->ib_device, (void *)(-1), NULL);
-	if (IS_ERR(ibpd)) {
-		ehca_err(&shca->ib_device, "Cannot create internal PD.");
-		ret = PTR_ERR(ibpd);
-		goto probe4;
-	}
-
-	shca->pd = container_of(ibpd, struct ehca_pd, ib_pd);
-	shca->pd->ib_pd.device = &shca->ib_device;
-
-	/* create internal max MR */
-	ret = ehca_reg_internal_maxmr(shca, shca->pd, &shca->maxmr);
-
-	if (ret) {
-		ehca_err(&shca->ib_device, "Cannot create internal MR ret=%i",
-			 ret);
-		goto probe5;
-	}
-
-	ret = ib_register_device(&shca->ib_device, NULL);
-	if (ret) {
-		ehca_err(&shca->ib_device,
-			 "ib_register_device() failed ret=%i", ret);
-		goto probe6;
-	}
-
-	/* create AQP1 for port 1 */
-	if (ehca_open_aqp1 == 1) {
-		shca->sport[0].port_state = IB_PORT_DOWN;
-		ret = ehca_create_aqp1(shca, 1);
-		if (ret) {
-			ehca_err(&shca->ib_device,
-				 "Cannot create AQP1 for port 1.");
-			goto probe7;
-		}
-	}
-
-	/* create AQP1 for port 2 */
-	if ((ehca_open_aqp1 == 1) && (shca->num_ports == 2)) {
-		shca->sport[1].port_state = IB_PORT_DOWN;
-		ret = ehca_create_aqp1(shca, 2);
-		if (ret) {
-			ehca_err(&shca->ib_device,
-				 "Cannot create AQP1 for port 2.");
-			goto probe8;
-		}
-	}
-
-	ret = sysfs_create_group(&dev->dev.kobj, &ehca_dev_attr_grp);
-	if (ret) /* only complain; we can live without attributes */
-		ehca_err(&shca->ib_device,
-			 "Cannot create device attributes  ret=%d", ret);
-
-	spin_lock_irqsave(&shca_list_lock, flags);
-	list_add(&shca->shca_list, &shca_list);
-	spin_unlock_irqrestore(&shca_list_lock, flags);
-
-	return 0;
-
-probe8:
-	ret = ehca_destroy_aqp1(&shca->sport[0]);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy AQP1 for port 1. ret=%i", ret);
-
-probe7:
-	ib_unregister_device(&shca->ib_device);
-
-probe6:
-	ret = ehca_dereg_internal_maxmr(shca);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy internal MR. ret=%x", ret);
-
-probe5:
-	ret = ehca_dealloc_pd(&shca->pd->ib_pd);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy internal PD. ret=%x", ret);
-
-probe4:
-	ret = ehca_destroy_eq(shca, &shca->neq);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy NEQ. ret=%x", ret);
-
-probe3:
-	ret = ehca_destroy_eq(shca, &shca->eq);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy EQ. ret=%x", ret);
-
-probe1:
-	ib_dealloc_device(&shca->ib_device);
-
-	return -EINVAL;
-}
-
-static int ehca_remove(struct platform_device *dev)
-{
-	struct ehca_shca *shca = dev_get_drvdata(&dev->dev);
-	unsigned long flags;
-	int ret;
-
-	sysfs_remove_group(&dev->dev.kobj, &ehca_dev_attr_grp);
-
-	if (ehca_open_aqp1 == 1) {
-		int i;
-		for (i = 0; i < shca->num_ports; i++) {
-			ret = ehca_destroy_aqp1(&shca->sport[i]);
-			if (ret)
-				ehca_err(&shca->ib_device,
-					 "Cannot destroy AQP1 for port %x "
-					 "ret=%i", ret, i);
-		}
-	}
-
-	ib_unregister_device(&shca->ib_device);
-
-	ret = ehca_dereg_internal_maxmr(shca);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy internal MR. ret=%i", ret);
-
-	ret = ehca_dealloc_pd(&shca->pd->ib_pd);
-	if (ret)
-		ehca_err(&shca->ib_device,
-			 "Cannot destroy internal PD. ret=%i", ret);
-
-	ret = ehca_destroy_eq(shca, &shca->eq);
-	if (ret)
-		ehca_err(&shca->ib_device, "Cannot destroy EQ. ret=%i", ret);
-
-	ret = ehca_destroy_eq(shca, &shca->neq);
-	if (ret)
-		ehca_err(&shca->ib_device, "Canot destroy NEQ. ret=%i", ret);
-
-	ib_dealloc_device(&shca->ib_device);
-
-	spin_lock_irqsave(&shca_list_lock, flags);
-	list_del(&shca->shca_list);
-	spin_unlock_irqrestore(&shca_list_lock, flags);
-
-	return ret;
-}
-
-static struct of_device_id ehca_device_table[] =
-{
-	{
-		.name       = "lhca",
-		.compatible = "IBM,lhca",
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, ehca_device_table);
-
-static struct platform_driver ehca_driver = {
-	.probe       = ehca_probe,
-	.remove      = ehca_remove,
-	.driver = {
-		.name = "ehca",
-		.owner = THIS_MODULE,
-		.groups = ehca_drv_attr_groups,
-		.of_match_table = ehca_device_table,
-	},
-};
-
-void ehca_poll_eqs(unsigned long data)
-{
-	struct ehca_shca *shca;
-
-	spin_lock(&shca_list_lock);
-	list_for_each_entry(shca, &shca_list, shca_list) {
-		if (shca->eq.is_initialized) {
-			/* call deadman proc only if eq ptr does not change */
-			struct ehca_eq *eq = &shca->eq;
-			int max = 3;
-			volatile u64 q_ofs, q_ofs2;
-			unsigned long flags;
-			spin_lock_irqsave(&eq->spinlock, flags);
-			q_ofs = eq->ipz_queue.current_q_offset;
-			spin_unlock_irqrestore(&eq->spinlock, flags);
-			do {
-				spin_lock_irqsave(&eq->spinlock, flags);
-				q_ofs2 = eq->ipz_queue.current_q_offset;
-				spin_unlock_irqrestore(&eq->spinlock, flags);
-				max--;
-			} while (q_ofs == q_ofs2 && max > 0);
-			if (q_ofs == q_ofs2)
-				ehca_process_eq(shca, 0);
-		}
-	}
-	mod_timer(&poll_eqs_timer, round_jiffies(jiffies + HZ));
-	spin_unlock(&shca_list_lock);
-}
-
-static int ehca_mem_notifier(struct notifier_block *nb,
-			     unsigned long action, void *data)
-{
-	static unsigned long ehca_dmem_warn_time;
-	unsigned long flags;
-
-	switch (action) {
-	case MEM_CANCEL_OFFLINE:
-	case MEM_CANCEL_ONLINE:
-	case MEM_ONLINE:
-	case MEM_OFFLINE:
-		return NOTIFY_OK;
-	case MEM_GOING_ONLINE:
-	case MEM_GOING_OFFLINE:
-		/* only ok if no hca is attached to the lpar */
-		spin_lock_irqsave(&shca_list_lock, flags);
-		if (list_empty(&shca_list)) {
-			spin_unlock_irqrestore(&shca_list_lock, flags);
-			return NOTIFY_OK;
-		} else {
-			spin_unlock_irqrestore(&shca_list_lock, flags);
-			if (printk_timed_ratelimit(&ehca_dmem_warn_time,
-						   30 * 1000))
-				ehca_gen_err("DMEM operations are not allowed"
-					     "in conjunction with eHCA");
-			return NOTIFY_BAD;
-		}
-	}
-	return NOTIFY_OK;
-}
-
-static struct notifier_block ehca_mem_nb = {
-	.notifier_call = ehca_mem_notifier,
-};
-
-static int __init ehca_module_init(void)
-{
-	int ret;
-
-	printk(KERN_INFO "eHCA Infiniband Device Driver "
-	       "(Version " HCAD_VERSION ")\n");
-
-	ret = ehca_create_comp_pool();
-	if (ret) {
-		ehca_gen_err("Cannot create comp pool.");
-		return ret;
-	}
-
-	ret = ehca_create_slab_caches();
-	if (ret) {
-		ehca_gen_err("Cannot create SLAB caches");
-		ret = -ENOMEM;
-		goto module_init1;
-	}
-
-	ret = ehca_create_busmap();
-	if (ret) {
-		ehca_gen_err("Cannot create busmap.");
-		goto module_init2;
-	}
-
-	ret = ibmebus_register_driver(&ehca_driver);
-	if (ret) {
-		ehca_gen_err("Cannot register eHCA device driver");
-		ret = -EINVAL;
-		goto module_init3;
-	}
-
-	ret = register_memory_notifier(&ehca_mem_nb);
-	if (ret) {
-		ehca_gen_err("Failed registering memory add/remove notifier");
-		goto module_init4;
-	}
-
-	if (ehca_poll_all_eqs != 1) {
-		ehca_gen_err("WARNING!!!");
-		ehca_gen_err("It is possible to lose interrupts.");
-	} else {
-		init_timer(&poll_eqs_timer);
-		poll_eqs_timer.function = ehca_poll_eqs;
-		poll_eqs_timer.expires = jiffies + HZ;
-		add_timer(&poll_eqs_timer);
-	}
-
-	return 0;
-
-module_init4:
-	ibmebus_unregister_driver(&ehca_driver);
-
-module_init3:
-	ehca_destroy_busmap();
-
-module_init2:
-	ehca_destroy_slab_caches();
-
-module_init1:
-	ehca_destroy_comp_pool();
-	return ret;
-};
-
-static void __exit ehca_module_exit(void)
-{
-	if (ehca_poll_all_eqs == 1)
-		del_timer_sync(&poll_eqs_timer);
-
-	ibmebus_unregister_driver(&ehca_driver);
-
-	unregister_memory_notifier(&ehca_mem_nb);
-
-	ehca_destroy_busmap();
-
-	ehca_destroy_slab_caches();
-
-	ehca_destroy_comp_pool();
-
-	idr_destroy(&ehca_cq_idr);
-	idr_destroy(&ehca_qp_idr);
-};
-
-module_init(ehca_module_init);
-module_exit(ehca_module_exit);
diff --git a/drivers/staging/rdma/ehca/ehca_mcast.c b/drivers/staging/rdma/ehca/ehca_mcast.c
deleted file mode 100644
index cec181532924..000000000000
--- a/drivers/staging/rdma/ehca/ehca_mcast.c
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  mcast  functions
- *
- *  Authors: Khadija Souissi <souissik@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/module.h>
-#include <linux/err.h>
-#include "ehca_classes.h"
-#include "ehca_tools.h"
-#include "ehca_qes.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-
-#define MAX_MC_LID 0xFFFE
-#define MIN_MC_LID 0xC000	/* Multicast limits */
-#define EHCA_VALID_MULTICAST_GID(gid)  ((gid)[0] == 0xFF)
-#define EHCA_VALID_MULTICAST_LID(lid) \
-	(((lid) >= MIN_MC_LID) && ((lid) <= MAX_MC_LID))
-
-int ehca_attach_mcast(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	struct ehca_qp *my_qp = container_of(ibqp, struct ehca_qp, ib_qp);
-	struct ehca_shca *shca = container_of(ibqp->device, struct ehca_shca,
-					      ib_device);
-	union ib_gid my_gid;
-	u64 subnet_prefix, interface_id, h_ret;
-
-	if (ibqp->qp_type != IB_QPT_UD) {
-		ehca_err(ibqp->device, "invalid qp_type=%x", ibqp->qp_type);
-		return -EINVAL;
-	}
-
-	if (!(EHCA_VALID_MULTICAST_GID(gid->raw))) {
-		ehca_err(ibqp->device, "invalid mulitcast gid");
-		return -EINVAL;
-	} else if ((lid < MIN_MC_LID) || (lid > MAX_MC_LID)) {
-		ehca_err(ibqp->device, "invalid mulitcast lid=%x", lid);
-		return -EINVAL;
-	}
-
-	memcpy(&my_gid, gid->raw, sizeof(union ib_gid));
-
-	subnet_prefix = be64_to_cpu(my_gid.global.subnet_prefix);
-	interface_id = be64_to_cpu(my_gid.global.interface_id);
-	h_ret = hipz_h_attach_mcqp(shca->ipz_hca_handle,
-				   my_qp->ipz_qp_handle,
-				   my_qp->galpas.kernel,
-				   lid, subnet_prefix, interface_id);
-	if (h_ret != H_SUCCESS)
-		ehca_err(ibqp->device,
-			 "ehca_qp=%p qp_num=%x hipz_h_attach_mcqp() failed "
-			 "h_ret=%lli", my_qp, ibqp->qp_num, h_ret);
-
-	return ehca2ib_return_code(h_ret);
-}
-
-int ehca_detach_mcast(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	struct ehca_qp *my_qp = container_of(ibqp, struct ehca_qp, ib_qp);
-	struct ehca_shca *shca = container_of(ibqp->pd->device,
-					      struct ehca_shca, ib_device);
-	union ib_gid my_gid;
-	u64 subnet_prefix, interface_id, h_ret;
-
-	if (ibqp->qp_type != IB_QPT_UD) {
-		ehca_err(ibqp->device, "invalid qp_type %x", ibqp->qp_type);
-		return -EINVAL;
-	}
-
-	if (!(EHCA_VALID_MULTICAST_GID(gid->raw))) {
-		ehca_err(ibqp->device, "invalid mulitcast gid");
-		return -EINVAL;
-	} else if ((lid < MIN_MC_LID) || (lid > MAX_MC_LID)) {
-		ehca_err(ibqp->device, "invalid mulitcast lid=%x", lid);
-		return -EINVAL;
-	}
-
-	memcpy(&my_gid, gid->raw, sizeof(union ib_gid));
-
-	subnet_prefix = be64_to_cpu(my_gid.global.subnet_prefix);
-	interface_id = be64_to_cpu(my_gid.global.interface_id);
-	h_ret = hipz_h_detach_mcqp(shca->ipz_hca_handle,
-				   my_qp->ipz_qp_handle,
-				   my_qp->galpas.kernel,
-				   lid, subnet_prefix, interface_id);
-	if (h_ret != H_SUCCESS)
-		ehca_err(ibqp->device,
-			 "ehca_qp=%p qp_num=%x hipz_h_detach_mcqp() failed "
-			 "h_ret=%lli", my_qp, ibqp->qp_num, h_ret);
-
-	return ehca2ib_return_code(h_ret);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_mrmw.c b/drivers/staging/rdma/ehca/ehca_mrmw.c
deleted file mode 100644
index f914b30999f8..000000000000
--- a/drivers/staging/rdma/ehca/ehca_mrmw.c
+++ /dev/null
@@ -1,2593 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  MR/MW functions
- *
- *  Authors: Dietmar Decker <ddecker@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-#include <rdma/ib_umem.h>
-
-#include "ehca_iverbs.h"
-#include "ehca_mrmw.h"
-#include "hcp_if.h"
-#include "hipz_hw.h"
-
-#define NUM_CHUNKS(length, chunk_size) \
-	(((length) + (chunk_size - 1)) / (chunk_size))
-
-/* max number of rpages (per hcall register_rpages) */
-#define MAX_RPAGES 512
-
-/* DMEM toleration management */
-#define EHCA_SECTSHIFT        SECTION_SIZE_BITS
-#define EHCA_SECTSIZE          (1UL << EHCA_SECTSHIFT)
-#define EHCA_HUGEPAGESHIFT     34
-#define EHCA_HUGEPAGE_SIZE     (1UL << EHCA_HUGEPAGESHIFT)
-#define EHCA_HUGEPAGE_PFN_MASK ((EHCA_HUGEPAGE_SIZE - 1) >> PAGE_SHIFT)
-#define EHCA_INVAL_ADDR        0xFFFFFFFFFFFFFFFFULL
-#define EHCA_DIR_INDEX_SHIFT 13                   /* 8k Entries in 64k block */
-#define EHCA_TOP_INDEX_SHIFT (EHCA_DIR_INDEX_SHIFT * 2)
-#define EHCA_MAP_ENTRIES (1 << EHCA_DIR_INDEX_SHIFT)
-#define EHCA_TOP_MAP_SIZE (0x10000)               /* currently fixed map size */
-#define EHCA_DIR_MAP_SIZE (0x10000)
-#define EHCA_ENT_MAP_SIZE (0x10000)
-#define EHCA_INDEX_MASK (EHCA_MAP_ENTRIES - 1)
-
-static unsigned long ehca_mr_len;
-
-/*
- * Memory map data structures
- */
-struct ehca_dir_bmap {
-	u64 ent[EHCA_MAP_ENTRIES];
-};
-struct ehca_top_bmap {
-	struct ehca_dir_bmap *dir[EHCA_MAP_ENTRIES];
-};
-struct ehca_bmap {
-	struct ehca_top_bmap *top[EHCA_MAP_ENTRIES];
-};
-
-static struct ehca_bmap *ehca_bmap;
-
-static struct kmem_cache *mr_cache;
-static struct kmem_cache *mw_cache;
-
-enum ehca_mr_pgsize {
-	EHCA_MR_PGSIZE4K  = 0x1000L,
-	EHCA_MR_PGSIZE64K = 0x10000L,
-	EHCA_MR_PGSIZE1M  = 0x100000L,
-	EHCA_MR_PGSIZE16M = 0x1000000L
-};
-
-#define EHCA_MR_PGSHIFT4K  12
-#define EHCA_MR_PGSHIFT64K 16
-#define EHCA_MR_PGSHIFT1M  20
-#define EHCA_MR_PGSHIFT16M 24
-
-static u64 ehca_map_vaddr(void *caddr);
-
-static u32 ehca_encode_hwpage_size(u32 pgsize)
-{
-	int log = ilog2(pgsize);
-	WARN_ON(log < 12 || log > 24 || log & 3);
-	return (log - 12) / 4;
-}
-
-static u64 ehca_get_max_hwpage_size(struct ehca_shca *shca)
-{
-	return rounddown_pow_of_two(shca->hca_cap_mr_pgsize);
-}
-
-static struct ehca_mr *ehca_mr_new(void)
-{
-	struct ehca_mr *me;
-
-	me = kmem_cache_zalloc(mr_cache, GFP_KERNEL);
-	if (me)
-		spin_lock_init(&me->mrlock);
-	else
-		ehca_gen_err("alloc failed");
-
-	return me;
-}
-
-static void ehca_mr_delete(struct ehca_mr *me)
-{
-	kmem_cache_free(mr_cache, me);
-}
-
-static struct ehca_mw *ehca_mw_new(void)
-{
-	struct ehca_mw *me;
-
-	me = kmem_cache_zalloc(mw_cache, GFP_KERNEL);
-	if (me)
-		spin_lock_init(&me->mwlock);
-	else
-		ehca_gen_err("alloc failed");
-
-	return me;
-}
-
-static void ehca_mw_delete(struct ehca_mw *me)
-{
-	kmem_cache_free(mw_cache, me);
-}
-
-/*----------------------------------------------------------------------*/
-
-struct ib_mr *ehca_get_dma_mr(struct ib_pd *pd, int mr_access_flags)
-{
-	struct ib_mr *ib_mr;
-	int ret;
-	struct ehca_mr *e_maxmr;
-	struct ehca_pd *e_pd = container_of(pd, struct ehca_pd, ib_pd);
-	struct ehca_shca *shca =
-		container_of(pd->device, struct ehca_shca, ib_device);
-
-	if (shca->maxmr) {
-		e_maxmr = ehca_mr_new();
-		if (!e_maxmr) {
-			ehca_err(&shca->ib_device, "out of memory");
-			ib_mr = ERR_PTR(-ENOMEM);
-			goto get_dma_mr_exit0;
-		}
-
-		ret = ehca_reg_maxmr(shca, e_maxmr,
-				     (void *)ehca_map_vaddr((void *)(KERNELBASE + PHYSICAL_START)),
-				     mr_access_flags, e_pd,
-				     &e_maxmr->ib.ib_mr.lkey,
-				     &e_maxmr->ib.ib_mr.rkey);
-		if (ret) {
-			ehca_mr_delete(e_maxmr);
-			ib_mr = ERR_PTR(ret);
-			goto get_dma_mr_exit0;
-		}
-		ib_mr = &e_maxmr->ib.ib_mr;
-	} else {
-		ehca_err(&shca->ib_device, "no internal max-MR exist!");
-		ib_mr = ERR_PTR(-EINVAL);
-		goto get_dma_mr_exit0;
-	}
-
-get_dma_mr_exit0:
-	if (IS_ERR(ib_mr))
-		ehca_err(&shca->ib_device, "h_ret=%li pd=%p mr_access_flags=%x",
-			 PTR_ERR(ib_mr), pd, mr_access_flags);
-	return ib_mr;
-} /* end ehca_get_dma_mr() */
-
-/*----------------------------------------------------------------------*/
-
-struct ib_mr *ehca_reg_phys_mr(struct ib_pd *pd,
-			       struct ib_phys_buf *phys_buf_array,
-			       int num_phys_buf,
-			       int mr_access_flags,
-			       u64 *iova_start)
-{
-	struct ib_mr *ib_mr;
-	int ret;
-	struct ehca_mr *e_mr;
-	struct ehca_shca *shca =
-		container_of(pd->device, struct ehca_shca, ib_device);
-	struct ehca_pd *e_pd = container_of(pd, struct ehca_pd, ib_pd);
-
-	u64 size;
-
-	if ((num_phys_buf <= 0) || !phys_buf_array) {
-		ehca_err(pd->device, "bad input values: num_phys_buf=%x "
-			 "phys_buf_array=%p", num_phys_buf, phys_buf_array);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_phys_mr_exit0;
-	}
-	if (((mr_access_flags & IB_ACCESS_REMOTE_WRITE) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE)) ||
-	    ((mr_access_flags & IB_ACCESS_REMOTE_ATOMIC) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE))) {
-		/*
-		 * Remote Write Access requires Local Write Access
-		 * Remote Atomic Access requires Local Write Access
-		 */
-		ehca_err(pd->device, "bad input values: mr_access_flags=%x",
-			 mr_access_flags);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_phys_mr_exit0;
-	}
-
-	/* check physical buffer list and calculate size */
-	ret = ehca_mr_chk_buf_and_calc_size(phys_buf_array, num_phys_buf,
-					    iova_start, &size);
-	if (ret) {
-		ib_mr = ERR_PTR(ret);
-		goto reg_phys_mr_exit0;
-	}
-	if ((size == 0) ||
-	    (((u64)iova_start + size) < (u64)iova_start)) {
-		ehca_err(pd->device, "bad input values: size=%llx iova_start=%p",
-			 size, iova_start);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_phys_mr_exit0;
-	}
-
-	e_mr = ehca_mr_new();
-	if (!e_mr) {
-		ehca_err(pd->device, "out of memory");
-		ib_mr = ERR_PTR(-ENOMEM);
-		goto reg_phys_mr_exit0;
-	}
-
-	/* register MR on HCA */
-	if (ehca_mr_is_maxmr(size, iova_start)) {
-		e_mr->flags |= EHCA_MR_FLAG_MAXMR;
-		ret = ehca_reg_maxmr(shca, e_mr, iova_start, mr_access_flags,
-				     e_pd, &e_mr->ib.ib_mr.lkey,
-				     &e_mr->ib.ib_mr.rkey);
-		if (ret) {
-			ib_mr = ERR_PTR(ret);
-			goto reg_phys_mr_exit1;
-		}
-	} else {
-		struct ehca_mr_pginfo pginfo;
-		u32 num_kpages;
-		u32 num_hwpages;
-		u64 hw_pgsize;
-
-		num_kpages = NUM_CHUNKS(((u64)iova_start % PAGE_SIZE) + size,
-					PAGE_SIZE);
-		/* for kernel space we try most possible pgsize */
-		hw_pgsize = ehca_get_max_hwpage_size(shca);
-		num_hwpages = NUM_CHUNKS(((u64)iova_start % hw_pgsize) + size,
-					 hw_pgsize);
-		memset(&pginfo, 0, sizeof(pginfo));
-		pginfo.type = EHCA_MR_PGI_PHYS;
-		pginfo.num_kpages = num_kpages;
-		pginfo.hwpage_size = hw_pgsize;
-		pginfo.num_hwpages = num_hwpages;
-		pginfo.u.phy.num_phys_buf = num_phys_buf;
-		pginfo.u.phy.phys_buf_array = phys_buf_array;
-		pginfo.next_hwpage =
-			((u64)iova_start & ~PAGE_MASK) / hw_pgsize;
-
-		ret = ehca_reg_mr(shca, e_mr, iova_start, size, mr_access_flags,
-				  e_pd, &pginfo, &e_mr->ib.ib_mr.lkey,
-				  &e_mr->ib.ib_mr.rkey, EHCA_REG_MR);
-		if (ret) {
-			ib_mr = ERR_PTR(ret);
-			goto reg_phys_mr_exit1;
-		}
-	}
-
-	/* successful registration of all pages */
-	return &e_mr->ib.ib_mr;
-
-reg_phys_mr_exit1:
-	ehca_mr_delete(e_mr);
-reg_phys_mr_exit0:
-	if (IS_ERR(ib_mr))
-		ehca_err(pd->device, "h_ret=%li pd=%p phys_buf_array=%p "
-			 "num_phys_buf=%x mr_access_flags=%x iova_start=%p",
-			 PTR_ERR(ib_mr), pd, phys_buf_array,
-			 num_phys_buf, mr_access_flags, iova_start);
-	return ib_mr;
-} /* end ehca_reg_phys_mr() */
-
-/*----------------------------------------------------------------------*/
-
-struct ib_mr *ehca_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
-			       u64 virt, int mr_access_flags,
-			       struct ib_udata *udata)
-{
-	struct ib_mr *ib_mr;
-	struct ehca_mr *e_mr;
-	struct ehca_shca *shca =
-		container_of(pd->device, struct ehca_shca, ib_device);
-	struct ehca_pd *e_pd = container_of(pd, struct ehca_pd, ib_pd);
-	struct ehca_mr_pginfo pginfo;
-	int ret, page_shift;
-	u32 num_kpages;
-	u32 num_hwpages;
-	u64 hwpage_size;
-
-	if (!pd) {
-		ehca_gen_err("bad pd=%p", pd);
-		return ERR_PTR(-EFAULT);
-	}
-
-	if (((mr_access_flags & IB_ACCESS_REMOTE_WRITE) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE)) ||
-	    ((mr_access_flags & IB_ACCESS_REMOTE_ATOMIC) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE))) {
-		/*
-		 * Remote Write Access requires Local Write Access
-		 * Remote Atomic Access requires Local Write Access
-		 */
-		ehca_err(pd->device, "bad input values: mr_access_flags=%x",
-			 mr_access_flags);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_user_mr_exit0;
-	}
-
-	if (length == 0 || virt + length < virt) {
-		ehca_err(pd->device, "bad input values: length=%llx "
-			 "virt_base=%llx", length, virt);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_user_mr_exit0;
-	}
-
-	e_mr = ehca_mr_new();
-	if (!e_mr) {
-		ehca_err(pd->device, "out of memory");
-		ib_mr = ERR_PTR(-ENOMEM);
-		goto reg_user_mr_exit0;
-	}
-
-	e_mr->umem = ib_umem_get(pd->uobject->context, start, length,
-				 mr_access_flags, 0);
-	if (IS_ERR(e_mr->umem)) {
-		ib_mr = (void *)e_mr->umem;
-		goto reg_user_mr_exit1;
-	}
-
-	if (e_mr->umem->page_size != PAGE_SIZE) {
-		ehca_err(pd->device, "page size not supported, "
-			 "e_mr->umem->page_size=%x", e_mr->umem->page_size);
-		ib_mr = ERR_PTR(-EINVAL);
-		goto reg_user_mr_exit2;
-	}
-
-	/* determine number of MR pages */
-	num_kpages = NUM_CHUNKS((virt % PAGE_SIZE) + length, PAGE_SIZE);
-	/* select proper hw_pgsize */
-	page_shift = PAGE_SHIFT;
-	if (e_mr->umem->hugetlb) {
-		/* determine page_shift, clamp between 4K and 16M */
-		page_shift = (fls64(length - 1) + 3) & ~3;
-		page_shift = min(max(page_shift, EHCA_MR_PGSHIFT4K),
-				 EHCA_MR_PGSHIFT16M);
-	}
-	hwpage_size = 1UL << page_shift;
-
-	/* now that we have the desired page size, shift until it's
-	 * supported, too. 4K is always supported, so this terminates.
-	 */
-	while (!(hwpage_size & shca->hca_cap_mr_pgsize))
-		hwpage_size >>= 4;
-
-reg_user_mr_fallback:
-	num_hwpages = NUM_CHUNKS((virt % hwpage_size) + length, hwpage_size);
-	/* register MR on HCA */
-	memset(&pginfo, 0, sizeof(pginfo));
-	pginfo.type = EHCA_MR_PGI_USER;
-	pginfo.hwpage_size = hwpage_size;
-	pginfo.num_kpages = num_kpages;
-	pginfo.num_hwpages = num_hwpages;
-	pginfo.u.usr.region = e_mr->umem;
-	pginfo.next_hwpage = ib_umem_offset(e_mr->umem) / hwpage_size;
-	pginfo.u.usr.next_sg = pginfo.u.usr.region->sg_head.sgl;
-	ret = ehca_reg_mr(shca, e_mr, (u64 *)virt, length, mr_access_flags,
-			  e_pd, &pginfo, &e_mr->ib.ib_mr.lkey,
-			  &e_mr->ib.ib_mr.rkey, EHCA_REG_MR);
-	if (ret == -EINVAL && pginfo.hwpage_size > PAGE_SIZE) {
-		ehca_warn(pd->device, "failed to register mr "
-			  "with hwpage_size=%llx", hwpage_size);
-		ehca_info(pd->device, "try to register mr with "
-			  "kpage_size=%lx", PAGE_SIZE);
-		/*
-		 * this means kpages are not contiguous for a hw page
-		 * try kernel page size as fallback solution
-		 */
-		hwpage_size = PAGE_SIZE;
-		goto reg_user_mr_fallback;
-	}
-	if (ret) {
-		ib_mr = ERR_PTR(ret);
-		goto reg_user_mr_exit2;
-	}
-
-	/* successful registration of all pages */
-	return &e_mr->ib.ib_mr;
-
-reg_user_mr_exit2:
-	ib_umem_release(e_mr->umem);
-reg_user_mr_exit1:
-	ehca_mr_delete(e_mr);
-reg_user_mr_exit0:
-	if (IS_ERR(ib_mr))
-		ehca_err(pd->device, "rc=%li pd=%p mr_access_flags=%x udata=%p",
-			 PTR_ERR(ib_mr), pd, mr_access_flags, udata);
-	return ib_mr;
-} /* end ehca_reg_user_mr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_rereg_phys_mr(struct ib_mr *mr,
-		       int mr_rereg_mask,
-		       struct ib_pd *pd,
-		       struct ib_phys_buf *phys_buf_array,
-		       int num_phys_buf,
-		       int mr_access_flags,
-		       u64 *iova_start)
-{
-	int ret;
-
-	struct ehca_shca *shca =
-		container_of(mr->device, struct ehca_shca, ib_device);
-	struct ehca_mr *e_mr = container_of(mr, struct ehca_mr, ib.ib_mr);
-	u64 new_size;
-	u64 *new_start;
-	u32 new_acl;
-	struct ehca_pd *new_pd;
-	u32 tmp_lkey, tmp_rkey;
-	unsigned long sl_flags;
-	u32 num_kpages = 0;
-	u32 num_hwpages = 0;
-	struct ehca_mr_pginfo pginfo;
-
-	if (!(mr_rereg_mask & IB_MR_REREG_TRANS)) {
-		/* TODO not supported, because PHYP rereg hCall needs pages */
-		ehca_err(mr->device, "rereg without IB_MR_REREG_TRANS not "
-			 "supported yet, mr_rereg_mask=%x", mr_rereg_mask);
-		ret = -EINVAL;
-		goto rereg_phys_mr_exit0;
-	}
-
-	if (mr_rereg_mask & IB_MR_REREG_PD) {
-		if (!pd) {
-			ehca_err(mr->device, "rereg with bad pd, pd=%p "
-				 "mr_rereg_mask=%x", pd, mr_rereg_mask);
-			ret = -EINVAL;
-			goto rereg_phys_mr_exit0;
-		}
-	}
-
-	if ((mr_rereg_mask &
-	     ~(IB_MR_REREG_TRANS | IB_MR_REREG_PD | IB_MR_REREG_ACCESS)) ||
-	    (mr_rereg_mask == 0)) {
-		ret = -EINVAL;
-		goto rereg_phys_mr_exit0;
-	}
-
-	/* check other parameters */
-	if (e_mr == shca->maxmr) {
-		/* should be impossible, however reject to be sure */
-		ehca_err(mr->device, "rereg internal max-MR impossible, mr=%p "
-			 "shca->maxmr=%p mr->lkey=%x",
-			 mr, shca->maxmr, mr->lkey);
-		ret = -EINVAL;
-		goto rereg_phys_mr_exit0;
-	}
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) { /* transl., i.e. addr/size */
-		if (e_mr->flags & EHCA_MR_FLAG_FMR) {
-			ehca_err(mr->device, "not supported for FMR, mr=%p "
-				 "flags=%x", mr, e_mr->flags);
-			ret = -EINVAL;
-			goto rereg_phys_mr_exit0;
-		}
-		if (!phys_buf_array || num_phys_buf <= 0) {
-			ehca_err(mr->device, "bad input values mr_rereg_mask=%x"
-				 " phys_buf_array=%p num_phys_buf=%x",
-				 mr_rereg_mask, phys_buf_array, num_phys_buf);
-			ret = -EINVAL;
-			goto rereg_phys_mr_exit0;
-		}
-	}
-	if ((mr_rereg_mask & IB_MR_REREG_ACCESS) &&	/* change ACL */
-	    (((mr_access_flags & IB_ACCESS_REMOTE_WRITE) &&
-	      !(mr_access_flags & IB_ACCESS_LOCAL_WRITE)) ||
-	     ((mr_access_flags & IB_ACCESS_REMOTE_ATOMIC) &&
-	      !(mr_access_flags & IB_ACCESS_LOCAL_WRITE)))) {
-		/*
-		 * Remote Write Access requires Local Write Access
-		 * Remote Atomic Access requires Local Write Access
-		 */
-		ehca_err(mr->device, "bad input values: mr_rereg_mask=%x "
-			 "mr_access_flags=%x", mr_rereg_mask, mr_access_flags);
-		ret = -EINVAL;
-		goto rereg_phys_mr_exit0;
-	}
-
-	/* set requested values dependent on rereg request */
-	spin_lock_irqsave(&e_mr->mrlock, sl_flags);
-	new_start = e_mr->start;
-	new_size = e_mr->size;
-	new_acl = e_mr->acl;
-	new_pd = container_of(mr->pd, struct ehca_pd, ib_pd);
-
-	if (mr_rereg_mask & IB_MR_REREG_TRANS) {
-		u64 hw_pgsize = ehca_get_max_hwpage_size(shca);
-
-		new_start = iova_start;	/* change address */
-		/* check physical buffer list and calculate size */
-		ret = ehca_mr_chk_buf_and_calc_size(phys_buf_array,
-						    num_phys_buf, iova_start,
-						    &new_size);
-		if (ret)
-			goto rereg_phys_mr_exit1;
-		if ((new_size == 0) ||
-		    (((u64)iova_start + new_size) < (u64)iova_start)) {
-			ehca_err(mr->device, "bad input values: new_size=%llx "
-				 "iova_start=%p", new_size, iova_start);
-			ret = -EINVAL;
-			goto rereg_phys_mr_exit1;
-		}
-		num_kpages = NUM_CHUNKS(((u64)new_start % PAGE_SIZE) +
-					new_size, PAGE_SIZE);
-		num_hwpages = NUM_CHUNKS(((u64)new_start % hw_pgsize) +
-					 new_size, hw_pgsize);
-		memset(&pginfo, 0, sizeof(pginfo));
-		pginfo.type = EHCA_MR_PGI_PHYS;
-		pginfo.num_kpages = num_kpages;
-		pginfo.hwpage_size = hw_pgsize;
-		pginfo.num_hwpages = num_hwpages;
-		pginfo.u.phy.num_phys_buf = num_phys_buf;
-		pginfo.u.phy.phys_buf_array = phys_buf_array;
-		pginfo.next_hwpage =
-			((u64)iova_start & ~PAGE_MASK) / hw_pgsize;
-	}
-	if (mr_rereg_mask & IB_MR_REREG_ACCESS)
-		new_acl = mr_access_flags;
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		new_pd = container_of(pd, struct ehca_pd, ib_pd);
-
-	ret = ehca_rereg_mr(shca, e_mr, new_start, new_size, new_acl,
-			    new_pd, &pginfo, &tmp_lkey, &tmp_rkey);
-	if (ret)
-		goto rereg_phys_mr_exit1;
-
-	/* successful reregistration */
-	if (mr_rereg_mask & IB_MR_REREG_PD)
-		mr->pd = pd;
-	mr->lkey = tmp_lkey;
-	mr->rkey = tmp_rkey;
-
-rereg_phys_mr_exit1:
-	spin_unlock_irqrestore(&e_mr->mrlock, sl_flags);
-rereg_phys_mr_exit0:
-	if (ret)
-		ehca_err(mr->device, "ret=%i mr=%p mr_rereg_mask=%x pd=%p "
-			 "phys_buf_array=%p num_phys_buf=%x mr_access_flags=%x "
-			 "iova_start=%p",
-			 ret, mr, mr_rereg_mask, pd, phys_buf_array,
-			 num_phys_buf, mr_access_flags, iova_start);
-	return ret;
-} /* end ehca_rereg_phys_mr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_query_mr(struct ib_mr *mr, struct ib_mr_attr *mr_attr)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_shca *shca =
-		container_of(mr->device, struct ehca_shca, ib_device);
-	struct ehca_mr *e_mr = container_of(mr, struct ehca_mr, ib.ib_mr);
-	unsigned long sl_flags;
-	struct ehca_mr_hipzout_parms hipzout;
-
-	if ((e_mr->flags & EHCA_MR_FLAG_FMR)) {
-		ehca_err(mr->device, "not supported for FMR, mr=%p e_mr=%p "
-			 "e_mr->flags=%x", mr, e_mr, e_mr->flags);
-		ret = -EINVAL;
-		goto query_mr_exit0;
-	}
-
-	memset(mr_attr, 0, sizeof(struct ib_mr_attr));
-	spin_lock_irqsave(&e_mr->mrlock, sl_flags);
-
-	h_ret = hipz_h_query_mr(shca->ipz_hca_handle, e_mr, &hipzout);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(mr->device, "hipz_mr_query failed, h_ret=%lli mr=%p "
-			 "hca_hndl=%llx mr_hndl=%llx lkey=%x",
-			 h_ret, mr, shca->ipz_hca_handle.handle,
-			 e_mr->ipz_mr_handle.handle, mr->lkey);
-		ret = ehca2ib_return_code(h_ret);
-		goto query_mr_exit1;
-	}
-	mr_attr->pd = mr->pd;
-	mr_attr->device_virt_addr = hipzout.vaddr;
-	mr_attr->size = hipzout.len;
-	mr_attr->lkey = hipzout.lkey;
-	mr_attr->rkey = hipzout.rkey;
-	ehca_mrmw_reverse_map_acl(&hipzout.acl, &mr_attr->mr_access_flags);
-
-query_mr_exit1:
-	spin_unlock_irqrestore(&e_mr->mrlock, sl_flags);
-query_mr_exit0:
-	if (ret)
-		ehca_err(mr->device, "ret=%i mr=%p mr_attr=%p",
-			 ret, mr, mr_attr);
-	return ret;
-} /* end ehca_query_mr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_dereg_mr(struct ib_mr *mr)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_shca *shca =
-		container_of(mr->device, struct ehca_shca, ib_device);
-	struct ehca_mr *e_mr = container_of(mr, struct ehca_mr, ib.ib_mr);
-
-	if ((e_mr->flags & EHCA_MR_FLAG_FMR)) {
-		ehca_err(mr->device, "not supported for FMR, mr=%p e_mr=%p "
-			 "e_mr->flags=%x", mr, e_mr, e_mr->flags);
-		ret = -EINVAL;
-		goto dereg_mr_exit0;
-	} else if (e_mr == shca->maxmr) {
-		/* should be impossible, however reject to be sure */
-		ehca_err(mr->device, "dereg internal max-MR impossible, mr=%p "
-			 "shca->maxmr=%p mr->lkey=%x",
-			 mr, shca->maxmr, mr->lkey);
-		ret = -EINVAL;
-		goto dereg_mr_exit0;
-	}
-
-	/* TODO: BUSY: MR still has bound window(s) */
-	h_ret = hipz_h_free_resource_mr(shca->ipz_hca_handle, e_mr);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(mr->device, "hipz_free_mr failed, h_ret=%lli shca=%p "
-			 "e_mr=%p hca_hndl=%llx mr_hndl=%llx mr->lkey=%x",
-			 h_ret, shca, e_mr, shca->ipz_hca_handle.handle,
-			 e_mr->ipz_mr_handle.handle, mr->lkey);
-		ret = ehca2ib_return_code(h_ret);
-		goto dereg_mr_exit0;
-	}
-
-	if (e_mr->umem)
-		ib_umem_release(e_mr->umem);
-
-	/* successful deregistration */
-	ehca_mr_delete(e_mr);
-
-dereg_mr_exit0:
-	if (ret)
-		ehca_err(mr->device, "ret=%i mr=%p", ret, mr);
-	return ret;
-} /* end ehca_dereg_mr() */
-
-/*----------------------------------------------------------------------*/
-
-struct ib_mw *ehca_alloc_mw(struct ib_pd *pd, enum ib_mw_type type)
-{
-	struct ib_mw *ib_mw;
-	u64 h_ret;
-	struct ehca_mw *e_mw;
-	struct ehca_pd *e_pd = container_of(pd, struct ehca_pd, ib_pd);
-	struct ehca_shca *shca =
-		container_of(pd->device, struct ehca_shca, ib_device);
-	struct ehca_mw_hipzout_parms hipzout;
-
-	if (type != IB_MW_TYPE_1)
-		return ERR_PTR(-EINVAL);
-
-	e_mw = ehca_mw_new();
-	if (!e_mw) {
-		ib_mw = ERR_PTR(-ENOMEM);
-		goto alloc_mw_exit0;
-	}
-
-	h_ret = hipz_h_alloc_resource_mw(shca->ipz_hca_handle, e_mw,
-					 e_pd->fw_pd, &hipzout);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(pd->device, "hipz_mw_allocate failed, h_ret=%lli "
-			 "shca=%p hca_hndl=%llx mw=%p",
-			 h_ret, shca, shca->ipz_hca_handle.handle, e_mw);
-		ib_mw = ERR_PTR(ehca2ib_return_code(h_ret));
-		goto alloc_mw_exit1;
-	}
-	/* successful MW allocation */
-	e_mw->ipz_mw_handle = hipzout.handle;
-	e_mw->ib_mw.rkey    = hipzout.rkey;
-	return &e_mw->ib_mw;
-
-alloc_mw_exit1:
-	ehca_mw_delete(e_mw);
-alloc_mw_exit0:
-	if (IS_ERR(ib_mw))
-		ehca_err(pd->device, "h_ret=%li pd=%p", PTR_ERR(ib_mw), pd);
-	return ib_mw;
-} /* end ehca_alloc_mw() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_bind_mw(struct ib_qp *qp,
-		 struct ib_mw *mw,
-		 struct ib_mw_bind *mw_bind)
-{
-	/* TODO: not supported up to now */
-	ehca_gen_err("bind MW currently not supported by HCAD");
-
-	return -EPERM;
-} /* end ehca_bind_mw() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_dealloc_mw(struct ib_mw *mw)
-{
-	u64 h_ret;
-	struct ehca_shca *shca =
-		container_of(mw->device, struct ehca_shca, ib_device);
-	struct ehca_mw *e_mw = container_of(mw, struct ehca_mw, ib_mw);
-
-	h_ret = hipz_h_free_resource_mw(shca->ipz_hca_handle, e_mw);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(mw->device, "hipz_free_mw failed, h_ret=%lli shca=%p "
-			 "mw=%p rkey=%x hca_hndl=%llx mw_hndl=%llx",
-			 h_ret, shca, mw, mw->rkey, shca->ipz_hca_handle.handle,
-			 e_mw->ipz_mw_handle.handle);
-		return ehca2ib_return_code(h_ret);
-	}
-	/* successful deallocation */
-	ehca_mw_delete(e_mw);
-	return 0;
-} /* end ehca_dealloc_mw() */
-
-/*----------------------------------------------------------------------*/
-
-struct ib_fmr *ehca_alloc_fmr(struct ib_pd *pd,
-			      int mr_access_flags,
-			      struct ib_fmr_attr *fmr_attr)
-{
-	struct ib_fmr *ib_fmr;
-	struct ehca_shca *shca =
-		container_of(pd->device, struct ehca_shca, ib_device);
-	struct ehca_pd *e_pd = container_of(pd, struct ehca_pd, ib_pd);
-	struct ehca_mr *e_fmr;
-	int ret;
-	u32 tmp_lkey, tmp_rkey;
-	struct ehca_mr_pginfo pginfo;
-	u64 hw_pgsize;
-
-	/* check other parameters */
-	if (((mr_access_flags & IB_ACCESS_REMOTE_WRITE) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE)) ||
-	    ((mr_access_flags & IB_ACCESS_REMOTE_ATOMIC) &&
-	     !(mr_access_flags & IB_ACCESS_LOCAL_WRITE))) {
-		/*
-		 * Remote Write Access requires Local Write Access
-		 * Remote Atomic Access requires Local Write Access
-		 */
-		ehca_err(pd->device, "bad input values: mr_access_flags=%x",
-			 mr_access_flags);
-		ib_fmr = ERR_PTR(-EINVAL);
-		goto alloc_fmr_exit0;
-	}
-	if (mr_access_flags & IB_ACCESS_MW_BIND) {
-		ehca_err(pd->device, "bad input values: mr_access_flags=%x",
-			 mr_access_flags);
-		ib_fmr = ERR_PTR(-EINVAL);
-		goto alloc_fmr_exit0;
-	}
-	if ((fmr_attr->max_pages == 0) || (fmr_attr->max_maps == 0)) {
-		ehca_err(pd->device, "bad input values: fmr_attr->max_pages=%x "
-			 "fmr_attr->max_maps=%x fmr_attr->page_shift=%x",
-			 fmr_attr->max_pages, fmr_attr->max_maps,
-			 fmr_attr->page_shift);
-		ib_fmr = ERR_PTR(-EINVAL);
-		goto alloc_fmr_exit0;
-	}
-
-	hw_pgsize = 1 << fmr_attr->page_shift;
-	if (!(hw_pgsize & shca->hca_cap_mr_pgsize)) {
-		ehca_err(pd->device, "unsupported fmr_attr->page_shift=%x",
-			 fmr_attr->page_shift);
-		ib_fmr = ERR_PTR(-EINVAL);
-		goto alloc_fmr_exit0;
-	}
-
-	e_fmr = ehca_mr_new();
-	if (!e_fmr) {
-		ib_fmr = ERR_PTR(-ENOMEM);
-		goto alloc_fmr_exit0;
-	}
-	e_fmr->flags |= EHCA_MR_FLAG_FMR;
-
-	/* register MR on HCA */
-	memset(&pginfo, 0, sizeof(pginfo));
-	pginfo.hwpage_size = hw_pgsize;
-	/*
-	 * pginfo.num_hwpages==0, ie register_rpages() will not be called
-	 * but deferred to map_phys_fmr()
-	 */
-	ret = ehca_reg_mr(shca, e_fmr, NULL,
-			  fmr_attr->max_pages * (1 << fmr_attr->page_shift),
-			  mr_access_flags, e_pd, &pginfo,
-			  &tmp_lkey, &tmp_rkey, EHCA_REG_MR);
-	if (ret) {
-		ib_fmr = ERR_PTR(ret);
-		goto alloc_fmr_exit1;
-	}
-
-	/* successful */
-	e_fmr->hwpage_size = hw_pgsize;
-	e_fmr->fmr_page_size = 1 << fmr_attr->page_shift;
-	e_fmr->fmr_max_pages = fmr_attr->max_pages;
-	e_fmr->fmr_max_maps = fmr_attr->max_maps;
-	e_fmr->fmr_map_cnt = 0;
-	return &e_fmr->ib.ib_fmr;
-
-alloc_fmr_exit1:
-	ehca_mr_delete(e_fmr);
-alloc_fmr_exit0:
-	return ib_fmr;
-} /* end ehca_alloc_fmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_map_phys_fmr(struct ib_fmr *fmr,
-		      u64 *page_list,
-		      int list_len,
-		      u64 iova)
-{
-	int ret;
-	struct ehca_shca *shca =
-		container_of(fmr->device, struct ehca_shca, ib_device);
-	struct ehca_mr *e_fmr = container_of(fmr, struct ehca_mr, ib.ib_fmr);
-	struct ehca_pd *e_pd = container_of(fmr->pd, struct ehca_pd, ib_pd);
-	struct ehca_mr_pginfo pginfo;
-	u32 tmp_lkey, tmp_rkey;
-
-	if (!(e_fmr->flags & EHCA_MR_FLAG_FMR)) {
-		ehca_err(fmr->device, "not a FMR, e_fmr=%p e_fmr->flags=%x",
-			 e_fmr, e_fmr->flags);
-		ret = -EINVAL;
-		goto map_phys_fmr_exit0;
-	}
-	ret = ehca_fmr_check_page_list(e_fmr, page_list, list_len);
-	if (ret)
-		goto map_phys_fmr_exit0;
-	if (iova % e_fmr->fmr_page_size) {
-		/* only whole-numbered pages */
-		ehca_err(fmr->device, "bad iova, iova=%llx fmr_page_size=%x",
-			 iova, e_fmr->fmr_page_size);
-		ret = -EINVAL;
-		goto map_phys_fmr_exit0;
-	}
-	if (e_fmr->fmr_map_cnt >= e_fmr->fmr_max_maps) {
-		/* HCAD does not limit the maps, however trace this anyway */
-		ehca_info(fmr->device, "map limit exceeded, fmr=%p "
-			  "e_fmr->fmr_map_cnt=%x e_fmr->fmr_max_maps=%x",
-			  fmr, e_fmr->fmr_map_cnt, e_fmr->fmr_max_maps);
-	}
-
-	memset(&pginfo, 0, sizeof(pginfo));
-	pginfo.type = EHCA_MR_PGI_FMR;
-	pginfo.num_kpages = list_len;
-	pginfo.hwpage_size = e_fmr->hwpage_size;
-	pginfo.num_hwpages =
-		list_len * e_fmr->fmr_page_size / pginfo.hwpage_size;
-	pginfo.u.fmr.page_list = page_list;
-	pginfo.next_hwpage =
-		(iova & (e_fmr->fmr_page_size-1)) / pginfo.hwpage_size;
-	pginfo.u.fmr.fmr_pgsize = e_fmr->fmr_page_size;
-
-	ret = ehca_rereg_mr(shca, e_fmr, (u64 *)iova,
-			    list_len * e_fmr->fmr_page_size,
-			    e_fmr->acl, e_pd, &pginfo, &tmp_lkey, &tmp_rkey);
-	if (ret)
-		goto map_phys_fmr_exit0;
-
-	/* successful reregistration */
-	e_fmr->fmr_map_cnt++;
-	e_fmr->ib.ib_fmr.lkey = tmp_lkey;
-	e_fmr->ib.ib_fmr.rkey = tmp_rkey;
-	return 0;
-
-map_phys_fmr_exit0:
-	if (ret)
-		ehca_err(fmr->device, "ret=%i fmr=%p page_list=%p list_len=%x "
-			 "iova=%llx", ret, fmr, page_list, list_len, iova);
-	return ret;
-} /* end ehca_map_phys_fmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_unmap_fmr(struct list_head *fmr_list)
-{
-	int ret = 0;
-	struct ib_fmr *ib_fmr;
-	struct ehca_shca *shca = NULL;
-	struct ehca_shca *prev_shca;
-	struct ehca_mr *e_fmr;
-	u32 num_fmr = 0;
-	u32 unmap_fmr_cnt = 0;
-
-	/* check all FMR belong to same SHCA, and check internal flag */
-	list_for_each_entry(ib_fmr, fmr_list, list) {
-		prev_shca = shca;
-		shca = container_of(ib_fmr->device, struct ehca_shca,
-				    ib_device);
-		e_fmr = container_of(ib_fmr, struct ehca_mr, ib.ib_fmr);
-		if ((shca != prev_shca) && prev_shca) {
-			ehca_err(&shca->ib_device, "SHCA mismatch, shca=%p "
-				 "prev_shca=%p e_fmr=%p",
-				 shca, prev_shca, e_fmr);
-			ret = -EINVAL;
-			goto unmap_fmr_exit0;
-		}
-		if (!(e_fmr->flags & EHCA_MR_FLAG_FMR)) {
-			ehca_err(&shca->ib_device, "not a FMR, e_fmr=%p "
-				 "e_fmr->flags=%x", e_fmr, e_fmr->flags);
-			ret = -EINVAL;
-			goto unmap_fmr_exit0;
-		}
-		num_fmr++;
-	}
-
-	/* loop over all FMRs to unmap */
-	list_for_each_entry(ib_fmr, fmr_list, list) {
-		unmap_fmr_cnt++;
-		e_fmr = container_of(ib_fmr, struct ehca_mr, ib.ib_fmr);
-		shca = container_of(ib_fmr->device, struct ehca_shca,
-				    ib_device);
-		ret = ehca_unmap_one_fmr(shca, e_fmr);
-		if (ret) {
-			/* unmap failed, stop unmapping of rest of FMRs */
-			ehca_err(&shca->ib_device, "unmap of one FMR failed, "
-				 "stop rest, e_fmr=%p num_fmr=%x "
-				 "unmap_fmr_cnt=%x lkey=%x", e_fmr, num_fmr,
-				 unmap_fmr_cnt, e_fmr->ib.ib_fmr.lkey);
-			goto unmap_fmr_exit0;
-		}
-	}
-
-unmap_fmr_exit0:
-	if (ret)
-		ehca_gen_err("ret=%i fmr_list=%p num_fmr=%x unmap_fmr_cnt=%x",
-			     ret, fmr_list, num_fmr, unmap_fmr_cnt);
-	return ret;
-} /* end ehca_unmap_fmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_dealloc_fmr(struct ib_fmr *fmr)
-{
-	int ret;
-	u64 h_ret;
-	struct ehca_shca *shca =
-		container_of(fmr->device, struct ehca_shca, ib_device);
-	struct ehca_mr *e_fmr = container_of(fmr, struct ehca_mr, ib.ib_fmr);
-
-	if (!(e_fmr->flags & EHCA_MR_FLAG_FMR)) {
-		ehca_err(fmr->device, "not a FMR, e_fmr=%p e_fmr->flags=%x",
-			 e_fmr, e_fmr->flags);
-		ret = -EINVAL;
-		goto free_fmr_exit0;
-	}
-
-	h_ret = hipz_h_free_resource_mr(shca->ipz_hca_handle, e_fmr);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(fmr->device, "hipz_free_mr failed, h_ret=%lli e_fmr=%p "
-			 "hca_hndl=%llx fmr_hndl=%llx fmr->lkey=%x",
-			 h_ret, e_fmr, shca->ipz_hca_handle.handle,
-			 e_fmr->ipz_mr_handle.handle, fmr->lkey);
-		ret = ehca2ib_return_code(h_ret);
-		goto free_fmr_exit0;
-	}
-	/* successful deregistration */
-	ehca_mr_delete(e_fmr);
-	return 0;
-
-free_fmr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i fmr=%p", ret, fmr);
-	return ret;
-} /* end ehca_dealloc_fmr() */
-
-/*----------------------------------------------------------------------*/
-
-static int ehca_reg_bmap_mr_rpages(struct ehca_shca *shca,
-				   struct ehca_mr *e_mr,
-				   struct ehca_mr_pginfo *pginfo);
-
-int ehca_reg_mr(struct ehca_shca *shca,
-		struct ehca_mr *e_mr,
-		u64 *iova_start,
-		u64 size,
-		int acl,
-		struct ehca_pd *e_pd,
-		struct ehca_mr_pginfo *pginfo,
-		u32 *lkey, /*OUT*/
-		u32 *rkey, /*OUT*/
-		enum ehca_reg_type reg_type)
-{
-	int ret;
-	u64 h_ret;
-	u32 hipz_acl;
-	struct ehca_mr_hipzout_parms hipzout;
-
-	ehca_mrmw_map_acl(acl, &hipz_acl);
-	ehca_mrmw_set_pgsize_hipz_acl(pginfo->hwpage_size, &hipz_acl);
-	if (ehca_use_hp_mr == 1)
-		hipz_acl |= 0x00000001;
-
-	h_ret = hipz_h_alloc_resource_mr(shca->ipz_hca_handle, e_mr,
-					 (u64)iova_start, size, hipz_acl,
-					 e_pd->fw_pd, &hipzout);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "hipz_alloc_mr failed, h_ret=%lli "
-			 "hca_hndl=%llx", h_ret, shca->ipz_hca_handle.handle);
-		ret = ehca2ib_return_code(h_ret);
-		goto ehca_reg_mr_exit0;
-	}
-
-	e_mr->ipz_mr_handle = hipzout.handle;
-
-	if (reg_type == EHCA_REG_BUSMAP_MR)
-		ret = ehca_reg_bmap_mr_rpages(shca, e_mr, pginfo);
-	else if (reg_type == EHCA_REG_MR)
-		ret = ehca_reg_mr_rpages(shca, e_mr, pginfo);
-	else
-		ret = -EINVAL;
-
-	if (ret)
-		goto ehca_reg_mr_exit1;
-
-	/* successful registration */
-	e_mr->num_kpages = pginfo->num_kpages;
-	e_mr->num_hwpages = pginfo->num_hwpages;
-	e_mr->hwpage_size = pginfo->hwpage_size;
-	e_mr->start = iova_start;
-	e_mr->size = size;
-	e_mr->acl = acl;
-	*lkey = hipzout.lkey;
-	*rkey = hipzout.rkey;
-	return 0;
-
-ehca_reg_mr_exit1:
-	h_ret = hipz_h_free_resource_mr(shca->ipz_hca_handle, e_mr);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "h_ret=%lli shca=%p e_mr=%p "
-			 "iova_start=%p size=%llx acl=%x e_pd=%p lkey=%x "
-			 "pginfo=%p num_kpages=%llx num_hwpages=%llx ret=%i",
-			 h_ret, shca, e_mr, iova_start, size, acl, e_pd,
-			 hipzout.lkey, pginfo, pginfo->num_kpages,
-			 pginfo->num_hwpages, ret);
-		ehca_err(&shca->ib_device, "internal error in ehca_reg_mr, "
-			 "not recoverable");
-	}
-ehca_reg_mr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p e_mr=%p "
-			 "iova_start=%p size=%llx acl=%x e_pd=%p pginfo=%p "
-			 "num_kpages=%llx num_hwpages=%llx",
-			 ret, shca, e_mr, iova_start, size, acl, e_pd, pginfo,
-			 pginfo->num_kpages, pginfo->num_hwpages);
-	return ret;
-} /* end ehca_reg_mr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_reg_mr_rpages(struct ehca_shca *shca,
-		       struct ehca_mr *e_mr,
-		       struct ehca_mr_pginfo *pginfo)
-{
-	int ret = 0;
-	u64 h_ret;
-	u32 rnum;
-	u64 rpage;
-	u32 i;
-	u64 *kpage;
-
-	if (!pginfo->num_hwpages) /* in case of fmr */
-		return 0;
-
-	kpage = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!kpage) {
-		ehca_err(&shca->ib_device, "kpage alloc failed");
-		ret = -ENOMEM;
-		goto ehca_reg_mr_rpages_exit0;
-	}
-
-	/* max MAX_RPAGES ehca mr pages per register call */
-	for (i = 0; i < NUM_CHUNKS(pginfo->num_hwpages, MAX_RPAGES); i++) {
-
-		if (i == NUM_CHUNKS(pginfo->num_hwpages, MAX_RPAGES) - 1) {
-			rnum = pginfo->num_hwpages % MAX_RPAGES; /* last shot */
-			if (rnum == 0)
-				rnum = MAX_RPAGES;      /* last shot is full */
-		} else
-			rnum = MAX_RPAGES;
-
-		ret = ehca_set_pagebuf(pginfo, rnum, kpage);
-		if (ret) {
-			ehca_err(&shca->ib_device, "ehca_set_pagebuf "
-				 "bad rc, ret=%i rnum=%x kpage=%p",
-				 ret, rnum, kpage);
-			goto ehca_reg_mr_rpages_exit1;
-		}
-
-		if (rnum > 1) {
-			rpage = __pa(kpage);
-			if (!rpage) {
-				ehca_err(&shca->ib_device, "kpage=%p i=%x",
-					 kpage, i);
-				ret = -EFAULT;
-				goto ehca_reg_mr_rpages_exit1;
-			}
-		} else
-			rpage = *kpage;
-
-		h_ret = hipz_h_register_rpage_mr(
-			shca->ipz_hca_handle, e_mr,
-			ehca_encode_hwpage_size(pginfo->hwpage_size),
-			0, rpage, rnum);
-
-		if (i == NUM_CHUNKS(pginfo->num_hwpages, MAX_RPAGES) - 1) {
-			/*
-			 * check for 'registration complete'==H_SUCCESS
-			 * and for 'page registered'==H_PAGE_REGISTERED
-			 */
-			if (h_ret != H_SUCCESS) {
-				ehca_err(&shca->ib_device, "last "
-					 "hipz_reg_rpage_mr failed, h_ret=%lli "
-					 "e_mr=%p i=%x hca_hndl=%llx mr_hndl=%llx"
-					 " lkey=%x", h_ret, e_mr, i,
-					 shca->ipz_hca_handle.handle,
-					 e_mr->ipz_mr_handle.handle,
-					 e_mr->ib.ib_mr.lkey);
-				ret = ehca2ib_return_code(h_ret);
-				break;
-			} else
-				ret = 0;
-		} else if (h_ret != H_PAGE_REGISTERED) {
-			ehca_err(&shca->ib_device, "hipz_reg_rpage_mr failed, "
-				 "h_ret=%lli e_mr=%p i=%x lkey=%x hca_hndl=%llx "
-				 "mr_hndl=%llx", h_ret, e_mr, i,
-				 e_mr->ib.ib_mr.lkey,
-				 shca->ipz_hca_handle.handle,
-				 e_mr->ipz_mr_handle.handle);
-			ret = ehca2ib_return_code(h_ret);
-			break;
-		} else
-			ret = 0;
-	} /* end for(i) */
-
-
-ehca_reg_mr_rpages_exit1:
-	ehca_free_fw_ctrlblock(kpage);
-ehca_reg_mr_rpages_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p e_mr=%p pginfo=%p "
-			 "num_kpages=%llx num_hwpages=%llx", ret, shca, e_mr,
-			 pginfo, pginfo->num_kpages, pginfo->num_hwpages);
-	return ret;
-} /* end ehca_reg_mr_rpages() */
-
-/*----------------------------------------------------------------------*/
-
-inline int ehca_rereg_mr_rereg1(struct ehca_shca *shca,
-				struct ehca_mr *e_mr,
-				u64 *iova_start,
-				u64 size,
-				u32 acl,
-				struct ehca_pd *e_pd,
-				struct ehca_mr_pginfo *pginfo,
-				u32 *lkey, /*OUT*/
-				u32 *rkey) /*OUT*/
-{
-	int ret;
-	u64 h_ret;
-	u32 hipz_acl;
-	u64 *kpage;
-	u64 rpage;
-	struct ehca_mr_pginfo pginfo_save;
-	struct ehca_mr_hipzout_parms hipzout;
-
-	ehca_mrmw_map_acl(acl, &hipz_acl);
-	ehca_mrmw_set_pgsize_hipz_acl(pginfo->hwpage_size, &hipz_acl);
-
-	kpage = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!kpage) {
-		ehca_err(&shca->ib_device, "kpage alloc failed");
-		ret = -ENOMEM;
-		goto ehca_rereg_mr_rereg1_exit0;
-	}
-
-	pginfo_save = *pginfo;
-	ret = ehca_set_pagebuf(pginfo, pginfo->num_hwpages, kpage);
-	if (ret) {
-		ehca_err(&shca->ib_device, "set pagebuf failed, e_mr=%p "
-			 "pginfo=%p type=%x num_kpages=%llx num_hwpages=%llx "
-			 "kpage=%p", e_mr, pginfo, pginfo->type,
-			 pginfo->num_kpages, pginfo->num_hwpages, kpage);
-		goto ehca_rereg_mr_rereg1_exit1;
-	}
-	rpage = __pa(kpage);
-	if (!rpage) {
-		ehca_err(&shca->ib_device, "kpage=%p", kpage);
-		ret = -EFAULT;
-		goto ehca_rereg_mr_rereg1_exit1;
-	}
-	h_ret = hipz_h_reregister_pmr(shca->ipz_hca_handle, e_mr,
-				      (u64)iova_start, size, hipz_acl,
-				      e_pd->fw_pd, rpage, &hipzout);
-	if (h_ret != H_SUCCESS) {
-		/*
-		 * reregistration unsuccessful, try it again with the 3 hCalls,
-		 * e.g. this is required in case H_MR_CONDITION
-		 * (MW bound or MR is shared)
-		 */
-		ehca_warn(&shca->ib_device, "hipz_h_reregister_pmr failed "
-			  "(Rereg1), h_ret=%lli e_mr=%p", h_ret, e_mr);
-		*pginfo = pginfo_save;
-		ret = -EAGAIN;
-	} else if ((u64 *)hipzout.vaddr != iova_start) {
-		ehca_err(&shca->ib_device, "PHYP changed iova_start in "
-			 "rereg_pmr, iova_start=%p iova_start_out=%llx e_mr=%p "
-			 "mr_handle=%llx lkey=%x lkey_out=%x", iova_start,
-			 hipzout.vaddr, e_mr, e_mr->ipz_mr_handle.handle,
-			 e_mr->ib.ib_mr.lkey, hipzout.lkey);
-		ret = -EFAULT;
-	} else {
-		/*
-		 * successful reregistration
-		 * note: start and start_out are identical for eServer HCAs
-		 */
-		e_mr->num_kpages = pginfo->num_kpages;
-		e_mr->num_hwpages = pginfo->num_hwpages;
-		e_mr->hwpage_size = pginfo->hwpage_size;
-		e_mr->start = iova_start;
-		e_mr->size = size;
-		e_mr->acl = acl;
-		*lkey = hipzout.lkey;
-		*rkey = hipzout.rkey;
-	}
-
-ehca_rereg_mr_rereg1_exit1:
-	ehca_free_fw_ctrlblock(kpage);
-ehca_rereg_mr_rereg1_exit0:
-	if ( ret && (ret != -EAGAIN) )
-		ehca_err(&shca->ib_device, "ret=%i lkey=%x rkey=%x "
-			 "pginfo=%p num_kpages=%llx num_hwpages=%llx",
-			 ret, *lkey, *rkey, pginfo, pginfo->num_kpages,
-			 pginfo->num_hwpages);
-	return ret;
-} /* end ehca_rereg_mr_rereg1() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_rereg_mr(struct ehca_shca *shca,
-		  struct ehca_mr *e_mr,
-		  u64 *iova_start,
-		  u64 size,
-		  int acl,
-		  struct ehca_pd *e_pd,
-		  struct ehca_mr_pginfo *pginfo,
-		  u32 *lkey,
-		  u32 *rkey)
-{
-	int ret = 0;
-	u64 h_ret;
-	int rereg_1_hcall = 1; /* 1: use hipz_h_reregister_pmr directly */
-	int rereg_3_hcall = 0; /* 1: use 3 hipz calls for reregistration */
-
-	/* first determine reregistration hCall(s) */
-	if ((pginfo->num_hwpages > MAX_RPAGES) ||
-	    (e_mr->num_hwpages > MAX_RPAGES) ||
-	    (pginfo->num_hwpages > e_mr->num_hwpages)) {
-		ehca_dbg(&shca->ib_device, "Rereg3 case, "
-			 "pginfo->num_hwpages=%llx e_mr->num_hwpages=%x",
-			 pginfo->num_hwpages, e_mr->num_hwpages);
-		rereg_1_hcall = 0;
-		rereg_3_hcall = 1;
-	}
-
-	if (e_mr->flags & EHCA_MR_FLAG_MAXMR) {	/* check for max-MR */
-		rereg_1_hcall = 0;
-		rereg_3_hcall = 1;
-		e_mr->flags &= ~EHCA_MR_FLAG_MAXMR;
-		ehca_err(&shca->ib_device, "Rereg MR for max-MR! e_mr=%p",
-			 e_mr);
-	}
-
-	if (rereg_1_hcall) {
-		ret = ehca_rereg_mr_rereg1(shca, e_mr, iova_start, size,
-					   acl, e_pd, pginfo, lkey, rkey);
-		if (ret) {
-			if (ret == -EAGAIN)
-				rereg_3_hcall = 1;
-			else
-				goto ehca_rereg_mr_exit0;
-		}
-	}
-
-	if (rereg_3_hcall) {
-		struct ehca_mr save_mr;
-
-		/* first deregister old MR */
-		h_ret = hipz_h_free_resource_mr(shca->ipz_hca_handle, e_mr);
-		if (h_ret != H_SUCCESS) {
-			ehca_err(&shca->ib_device, "hipz_free_mr failed, "
-				 "h_ret=%lli e_mr=%p hca_hndl=%llx mr_hndl=%llx "
-				 "mr->lkey=%x",
-				 h_ret, e_mr, shca->ipz_hca_handle.handle,
-				 e_mr->ipz_mr_handle.handle,
-				 e_mr->ib.ib_mr.lkey);
-			ret = ehca2ib_return_code(h_ret);
-			goto ehca_rereg_mr_exit0;
-		}
-		/* clean ehca_mr_t, without changing struct ib_mr and lock */
-		save_mr = *e_mr;
-		ehca_mr_deletenew(e_mr);
-
-		/* set some MR values */
-		e_mr->flags = save_mr.flags;
-		e_mr->hwpage_size = save_mr.hwpage_size;
-		e_mr->fmr_page_size = save_mr.fmr_page_size;
-		e_mr->fmr_max_pages = save_mr.fmr_max_pages;
-		e_mr->fmr_max_maps = save_mr.fmr_max_maps;
-		e_mr->fmr_map_cnt = save_mr.fmr_map_cnt;
-
-		ret = ehca_reg_mr(shca, e_mr, iova_start, size, acl,
-				  e_pd, pginfo, lkey, rkey, EHCA_REG_MR);
-		if (ret) {
-			u32 offset = (u64)(&e_mr->flags) - (u64)e_mr;
-			memcpy(&e_mr->flags, &(save_mr.flags),
-			       sizeof(struct ehca_mr) - offset);
-			goto ehca_rereg_mr_exit0;
-		}
-	}
-
-ehca_rereg_mr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p e_mr=%p "
-			 "iova_start=%p size=%llx acl=%x e_pd=%p pginfo=%p "
-			 "num_kpages=%llx lkey=%x rkey=%x rereg_1_hcall=%x "
-			 "rereg_3_hcall=%x", ret, shca, e_mr, iova_start, size,
-			 acl, e_pd, pginfo, pginfo->num_kpages, *lkey, *rkey,
-			 rereg_1_hcall, rereg_3_hcall);
-	return ret;
-} /* end ehca_rereg_mr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_unmap_one_fmr(struct ehca_shca *shca,
-		       struct ehca_mr *e_fmr)
-{
-	int ret = 0;
-	u64 h_ret;
-	struct ehca_pd *e_pd =
-		container_of(e_fmr->ib.ib_fmr.pd, struct ehca_pd, ib_pd);
-	struct ehca_mr save_fmr;
-	u32 tmp_lkey, tmp_rkey;
-	struct ehca_mr_pginfo pginfo;
-	struct ehca_mr_hipzout_parms hipzout;
-	struct ehca_mr save_mr;
-
-	if (e_fmr->fmr_max_pages <= MAX_RPAGES) {
-		/*
-		 * note: after using rereg hcall with len=0,
-		 * rereg hcall must be used again for registering pages
-		 */
-		h_ret = hipz_h_reregister_pmr(shca->ipz_hca_handle, e_fmr, 0,
-					      0, 0, e_pd->fw_pd, 0, &hipzout);
-		if (h_ret == H_SUCCESS) {
-			/* successful reregistration */
-			e_fmr->start = NULL;
-			e_fmr->size = 0;
-			tmp_lkey = hipzout.lkey;
-			tmp_rkey = hipzout.rkey;
-			return 0;
-		}
-		/*
-		 * should not happen, because length checked above,
-		 * FMRs are not shared and no MW bound to FMRs
-		 */
-		ehca_err(&shca->ib_device, "hipz_reregister_pmr failed "
-			 "(Rereg1), h_ret=%lli e_fmr=%p hca_hndl=%llx "
-			 "mr_hndl=%llx lkey=%x lkey_out=%x",
-			 h_ret, e_fmr, shca->ipz_hca_handle.handle,
-			 e_fmr->ipz_mr_handle.handle,
-			 e_fmr->ib.ib_fmr.lkey, hipzout.lkey);
-		/* try free and rereg */
-	}
-
-	/* first free old FMR */
-	h_ret = hipz_h_free_resource_mr(shca->ipz_hca_handle, e_fmr);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "hipz_free_mr failed, "
-			 "h_ret=%lli e_fmr=%p hca_hndl=%llx mr_hndl=%llx "
-			 "lkey=%x",
-			 h_ret, e_fmr, shca->ipz_hca_handle.handle,
-			 e_fmr->ipz_mr_handle.handle,
-			 e_fmr->ib.ib_fmr.lkey);
-		ret = ehca2ib_return_code(h_ret);
-		goto ehca_unmap_one_fmr_exit0;
-	}
-	/* clean ehca_mr_t, without changing lock */
-	save_fmr = *e_fmr;
-	ehca_mr_deletenew(e_fmr);
-
-	/* set some MR values */
-	e_fmr->flags = save_fmr.flags;
-	e_fmr->hwpage_size = save_fmr.hwpage_size;
-	e_fmr->fmr_page_size = save_fmr.fmr_page_size;
-	e_fmr->fmr_max_pages = save_fmr.fmr_max_pages;
-	e_fmr->fmr_max_maps = save_fmr.fmr_max_maps;
-	e_fmr->fmr_map_cnt = save_fmr.fmr_map_cnt;
-	e_fmr->acl = save_fmr.acl;
-
-	memset(&pginfo, 0, sizeof(pginfo));
-	pginfo.type = EHCA_MR_PGI_FMR;
-	ret = ehca_reg_mr(shca, e_fmr, NULL,
-			  (e_fmr->fmr_max_pages * e_fmr->fmr_page_size),
-			  e_fmr->acl, e_pd, &pginfo, &tmp_lkey,
-			  &tmp_rkey, EHCA_REG_MR);
-	if (ret) {
-		u32 offset = (u64)(&e_fmr->flags) - (u64)e_fmr;
-		memcpy(&e_fmr->flags, &(save_mr.flags),
-		       sizeof(struct ehca_mr) - offset);
-	}
-
-ehca_unmap_one_fmr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i tmp_lkey=%x tmp_rkey=%x "
-			 "fmr_max_pages=%x",
-			 ret, tmp_lkey, tmp_rkey, e_fmr->fmr_max_pages);
-	return ret;
-} /* end ehca_unmap_one_fmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_reg_smr(struct ehca_shca *shca,
-		 struct ehca_mr *e_origmr,
-		 struct ehca_mr *e_newmr,
-		 u64 *iova_start,
-		 int acl,
-		 struct ehca_pd *e_pd,
-		 u32 *lkey, /*OUT*/
-		 u32 *rkey) /*OUT*/
-{
-	int ret = 0;
-	u64 h_ret;
-	u32 hipz_acl;
-	struct ehca_mr_hipzout_parms hipzout;
-
-	ehca_mrmw_map_acl(acl, &hipz_acl);
-	ehca_mrmw_set_pgsize_hipz_acl(e_origmr->hwpage_size, &hipz_acl);
-
-	h_ret = hipz_h_register_smr(shca->ipz_hca_handle, e_newmr, e_origmr,
-				    (u64)iova_start, hipz_acl, e_pd->fw_pd,
-				    &hipzout);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "hipz_reg_smr failed, h_ret=%lli "
-			 "shca=%p e_origmr=%p e_newmr=%p iova_start=%p acl=%x "
-			 "e_pd=%p hca_hndl=%llx mr_hndl=%llx lkey=%x",
-			 h_ret, shca, e_origmr, e_newmr, iova_start, acl, e_pd,
-			 shca->ipz_hca_handle.handle,
-			 e_origmr->ipz_mr_handle.handle,
-			 e_origmr->ib.ib_mr.lkey);
-		ret = ehca2ib_return_code(h_ret);
-		goto ehca_reg_smr_exit0;
-	}
-	/* successful registration */
-	e_newmr->num_kpages = e_origmr->num_kpages;
-	e_newmr->num_hwpages = e_origmr->num_hwpages;
-	e_newmr->hwpage_size   = e_origmr->hwpage_size;
-	e_newmr->start = iova_start;
-	e_newmr->size = e_origmr->size;
-	e_newmr->acl = acl;
-	e_newmr->ipz_mr_handle = hipzout.handle;
-	*lkey = hipzout.lkey;
-	*rkey = hipzout.rkey;
-	return 0;
-
-ehca_reg_smr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p e_origmr=%p "
-			 "e_newmr=%p iova_start=%p acl=%x e_pd=%p",
-			 ret, shca, e_origmr, e_newmr, iova_start, acl, e_pd);
-	return ret;
-} /* end ehca_reg_smr() */
-
-/*----------------------------------------------------------------------*/
-static inline void *ehca_calc_sectbase(int top, int dir, int idx)
-{
-	unsigned long ret = idx;
-	ret |= dir << EHCA_DIR_INDEX_SHIFT;
-	ret |= top << EHCA_TOP_INDEX_SHIFT;
-	return __va(ret << SECTION_SIZE_BITS);
-}
-
-#define ehca_bmap_valid(entry) \
-	((u64)entry != (u64)EHCA_INVAL_ADDR)
-
-static u64 ehca_reg_mr_section(int top, int dir, int idx, u64 *kpage,
-			       struct ehca_shca *shca, struct ehca_mr *mr,
-			       struct ehca_mr_pginfo *pginfo)
-{
-	u64 h_ret = 0;
-	unsigned long page = 0;
-	u64 rpage = __pa(kpage);
-	int page_count;
-
-	void *sectbase = ehca_calc_sectbase(top, dir, idx);
-	if ((unsigned long)sectbase & (pginfo->hwpage_size - 1)) {
-		ehca_err(&shca->ib_device, "reg_mr_section will probably fail:"
-					   "hwpage_size does not fit to "
-					   "section start address");
-	}
-	page_count = EHCA_SECTSIZE / pginfo->hwpage_size;
-
-	while (page < page_count) {
-		u64 rnum;
-		for (rnum = 0; (rnum < MAX_RPAGES) && (page < page_count);
-		     rnum++) {
-			void *pg = sectbase + ((page++) * pginfo->hwpage_size);
-			kpage[rnum] = __pa(pg);
-		}
-
-		h_ret = hipz_h_register_rpage_mr(shca->ipz_hca_handle, mr,
-			ehca_encode_hwpage_size(pginfo->hwpage_size),
-			0, rpage, rnum);
-
-		if ((h_ret != H_SUCCESS) && (h_ret != H_PAGE_REGISTERED)) {
-			ehca_err(&shca->ib_device, "register_rpage_mr failed");
-			return h_ret;
-		}
-	}
-	return h_ret;
-}
-
-static u64 ehca_reg_mr_sections(int top, int dir, u64 *kpage,
-				struct ehca_shca *shca, struct ehca_mr *mr,
-				struct ehca_mr_pginfo *pginfo)
-{
-	u64 hret = H_SUCCESS;
-	int idx;
-
-	for (idx = 0; idx < EHCA_MAP_ENTRIES; idx++) {
-		if (!ehca_bmap_valid(ehca_bmap->top[top]->dir[dir]->ent[idx]))
-			continue;
-
-		hret = ehca_reg_mr_section(top, dir, idx, kpage, shca, mr,
-					   pginfo);
-		if ((hret != H_SUCCESS) && (hret != H_PAGE_REGISTERED))
-				return hret;
-	}
-	return hret;
-}
-
-static u64 ehca_reg_mr_dir_sections(int top, u64 *kpage, struct ehca_shca *shca,
-				    struct ehca_mr *mr,
-				    struct ehca_mr_pginfo *pginfo)
-{
-	u64 hret = H_SUCCESS;
-	int dir;
-
-	for (dir = 0; dir < EHCA_MAP_ENTRIES; dir++) {
-		if (!ehca_bmap_valid(ehca_bmap->top[top]->dir[dir]))
-			continue;
-
-		hret = ehca_reg_mr_sections(top, dir, kpage, shca, mr, pginfo);
-		if ((hret != H_SUCCESS) && (hret != H_PAGE_REGISTERED))
-				return hret;
-	}
-	return hret;
-}
-
-/* register internal max-MR to internal SHCA */
-int ehca_reg_internal_maxmr(
-	struct ehca_shca *shca,
-	struct ehca_pd *e_pd,
-	struct ehca_mr **e_maxmr)  /*OUT*/
-{
-	int ret;
-	struct ehca_mr *e_mr;
-	u64 *iova_start;
-	u64 size_maxmr;
-	struct ehca_mr_pginfo pginfo;
-	struct ib_phys_buf ib_pbuf;
-	u32 num_kpages;
-	u32 num_hwpages;
-	u64 hw_pgsize;
-
-	if (!ehca_bmap) {
-		ret = -EFAULT;
-		goto ehca_reg_internal_maxmr_exit0;
-	}
-
-	e_mr = ehca_mr_new();
-	if (!e_mr) {
-		ehca_err(&shca->ib_device, "out of memory");
-		ret = -ENOMEM;
-		goto ehca_reg_internal_maxmr_exit0;
-	}
-	e_mr->flags |= EHCA_MR_FLAG_MAXMR;
-
-	/* register internal max-MR on HCA */
-	size_maxmr = ehca_mr_len;
-	iova_start = (u64 *)ehca_map_vaddr((void *)(KERNELBASE + PHYSICAL_START));
-	ib_pbuf.addr = 0;
-	ib_pbuf.size = size_maxmr;
-	num_kpages = NUM_CHUNKS(((u64)iova_start % PAGE_SIZE) + size_maxmr,
-				PAGE_SIZE);
-	hw_pgsize = ehca_get_max_hwpage_size(shca);
-	num_hwpages = NUM_CHUNKS(((u64)iova_start % hw_pgsize) + size_maxmr,
-				 hw_pgsize);
-
-	memset(&pginfo, 0, sizeof(pginfo));
-	pginfo.type = EHCA_MR_PGI_PHYS;
-	pginfo.num_kpages = num_kpages;
-	pginfo.num_hwpages = num_hwpages;
-	pginfo.hwpage_size = hw_pgsize;
-	pginfo.u.phy.num_phys_buf = 1;
-	pginfo.u.phy.phys_buf_array = &ib_pbuf;
-
-	ret = ehca_reg_mr(shca, e_mr, iova_start, size_maxmr, 0, e_pd,
-			  &pginfo, &e_mr->ib.ib_mr.lkey,
-			  &e_mr->ib.ib_mr.rkey, EHCA_REG_BUSMAP_MR);
-	if (ret) {
-		ehca_err(&shca->ib_device, "reg of internal max MR failed, "
-			 "e_mr=%p iova_start=%p size_maxmr=%llx num_kpages=%x "
-			 "num_hwpages=%x", e_mr, iova_start, size_maxmr,
-			 num_kpages, num_hwpages);
-		goto ehca_reg_internal_maxmr_exit1;
-	}
-
-	/* successful registration of all pages */
-	e_mr->ib.ib_mr.device = e_pd->ib_pd.device;
-	e_mr->ib.ib_mr.pd = &e_pd->ib_pd;
-	e_mr->ib.ib_mr.uobject = NULL;
-	atomic_inc(&(e_pd->ib_pd.usecnt));
-	atomic_set(&(e_mr->ib.ib_mr.usecnt), 0);
-	*e_maxmr = e_mr;
-	return 0;
-
-ehca_reg_internal_maxmr_exit1:
-	ehca_mr_delete(e_mr);
-ehca_reg_internal_maxmr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p e_pd=%p e_maxmr=%p",
-			 ret, shca, e_pd, e_maxmr);
-	return ret;
-} /* end ehca_reg_internal_maxmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_reg_maxmr(struct ehca_shca *shca,
-		   struct ehca_mr *e_newmr,
-		   u64 *iova_start,
-		   int acl,
-		   struct ehca_pd *e_pd,
-		   u32 *lkey,
-		   u32 *rkey)
-{
-	u64 h_ret;
-	struct ehca_mr *e_origmr = shca->maxmr;
-	u32 hipz_acl;
-	struct ehca_mr_hipzout_parms hipzout;
-
-	ehca_mrmw_map_acl(acl, &hipz_acl);
-	ehca_mrmw_set_pgsize_hipz_acl(e_origmr->hwpage_size, &hipz_acl);
-
-	h_ret = hipz_h_register_smr(shca->ipz_hca_handle, e_newmr, e_origmr,
-				    (u64)iova_start, hipz_acl, e_pd->fw_pd,
-				    &hipzout);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "hipz_reg_smr failed, h_ret=%lli "
-			 "e_origmr=%p hca_hndl=%llx mr_hndl=%llx lkey=%x",
-			 h_ret, e_origmr, shca->ipz_hca_handle.handle,
-			 e_origmr->ipz_mr_handle.handle,
-			 e_origmr->ib.ib_mr.lkey);
-		return ehca2ib_return_code(h_ret);
-	}
-	/* successful registration */
-	e_newmr->num_kpages = e_origmr->num_kpages;
-	e_newmr->num_hwpages = e_origmr->num_hwpages;
-	e_newmr->hwpage_size = e_origmr->hwpage_size;
-	e_newmr->start = iova_start;
-	e_newmr->size = e_origmr->size;
-	e_newmr->acl = acl;
-	e_newmr->ipz_mr_handle = hipzout.handle;
-	*lkey = hipzout.lkey;
-	*rkey = hipzout.rkey;
-	return 0;
-} /* end ehca_reg_maxmr() */
-
-/*----------------------------------------------------------------------*/
-
-int ehca_dereg_internal_maxmr(struct ehca_shca *shca)
-{
-	int ret;
-	struct ehca_mr *e_maxmr;
-	struct ib_pd *ib_pd;
-
-	if (!shca->maxmr) {
-		ehca_err(&shca->ib_device, "bad call, shca=%p", shca);
-		ret = -EINVAL;
-		goto ehca_dereg_internal_maxmr_exit0;
-	}
-
-	e_maxmr = shca->maxmr;
-	ib_pd = e_maxmr->ib.ib_mr.pd;
-	shca->maxmr = NULL; /* remove internal max-MR indication from SHCA */
-
-	ret = ehca_dereg_mr(&e_maxmr->ib.ib_mr);
-	if (ret) {
-		ehca_err(&shca->ib_device, "dereg internal max-MR failed, "
-			 "ret=%i e_maxmr=%p shca=%p lkey=%x",
-			 ret, e_maxmr, shca, e_maxmr->ib.ib_mr.lkey);
-		shca->maxmr = e_maxmr;
-		goto ehca_dereg_internal_maxmr_exit0;
-	}
-
-	atomic_dec(&ib_pd->usecnt);
-
-ehca_dereg_internal_maxmr_exit0:
-	if (ret)
-		ehca_err(&shca->ib_device, "ret=%i shca=%p shca->maxmr=%p",
-			 ret, shca, shca->maxmr);
-	return ret;
-} /* end ehca_dereg_internal_maxmr() */
-
-/*----------------------------------------------------------------------*/
-
-/*
- * check physical buffer array of MR verbs for validness and
- * calculates MR size
- */
-int ehca_mr_chk_buf_and_calc_size(struct ib_phys_buf *phys_buf_array,
-				  int num_phys_buf,
-				  u64 *iova_start,
-				  u64 *size)
-{
-	struct ib_phys_buf *pbuf = phys_buf_array;
-	u64 size_count = 0;
-	u32 i;
-
-	if (num_phys_buf == 0) {
-		ehca_gen_err("bad phys buf array len, num_phys_buf=0");
-		return -EINVAL;
-	}
-	/* check first buffer */
-	if (((u64)iova_start & ~PAGE_MASK) != (pbuf->addr & ~PAGE_MASK)) {
-		ehca_gen_err("iova_start/addr mismatch, iova_start=%p "
-			     "pbuf->addr=%llx pbuf->size=%llx",
-			     iova_start, pbuf->addr, pbuf->size);
-		return -EINVAL;
-	}
-	if (((pbuf->addr + pbuf->size) % PAGE_SIZE) &&
-	    (num_phys_buf > 1)) {
-		ehca_gen_err("addr/size mismatch in 1st buf, pbuf->addr=%llx "
-			     "pbuf->size=%llx", pbuf->addr, pbuf->size);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < num_phys_buf; i++) {
-		if ((i > 0) && (pbuf->addr % PAGE_SIZE)) {
-			ehca_gen_err("bad address, i=%x pbuf->addr=%llx "
-				     "pbuf->size=%llx",
-				     i, pbuf->addr, pbuf->size);
-			return -EINVAL;
-		}
-		if (((i > 0) &&	/* not 1st */
-		     (i < (num_phys_buf - 1)) &&	/* not last */
-		     (pbuf->size % PAGE_SIZE)) || (pbuf->size == 0)) {
-			ehca_gen_err("bad size, i=%x pbuf->size=%llx",
-				     i, pbuf->size);
-			return -EINVAL;
-		}
-		size_count += pbuf->size;
-		pbuf++;
-	}
-
-	*size = size_count;
-	return 0;
-} /* end ehca_mr_chk_buf_and_calc_size() */
-
-/*----------------------------------------------------------------------*/
-
-/* check page list of map FMR verb for validness */
-int ehca_fmr_check_page_list(struct ehca_mr *e_fmr,
-			     u64 *page_list,
-			     int list_len)
-{
-	u32 i;
-	u64 *page;
-
-	if ((list_len == 0) || (list_len > e_fmr->fmr_max_pages)) {
-		ehca_gen_err("bad list_len, list_len=%x "
-			     "e_fmr->fmr_max_pages=%x fmr=%p",
-			     list_len, e_fmr->fmr_max_pages, e_fmr);
-		return -EINVAL;
-	}
-
-	/* each page must be aligned */
-	page = page_list;
-	for (i = 0; i < list_len; i++) {
-		if (*page % e_fmr->fmr_page_size) {
-			ehca_gen_err("bad page, i=%x *page=%llx page=%p fmr=%p "
-				     "fmr_page_size=%x", i, *page, page, e_fmr,
-				     e_fmr->fmr_page_size);
-			return -EINVAL;
-		}
-		page++;
-	}
-
-	return 0;
-} /* end ehca_fmr_check_page_list() */
-
-/*----------------------------------------------------------------------*/
-
-/* PAGE_SIZE >= pginfo->hwpage_size */
-static int ehca_set_pagebuf_user1(struct ehca_mr_pginfo *pginfo,
-				  u32 number,
-				  u64 *kpage)
-{
-	int ret = 0;
-	u64 pgaddr;
-	u32 j = 0;
-	int hwpages_per_kpage = PAGE_SIZE / pginfo->hwpage_size;
-	struct scatterlist **sg = &pginfo->u.usr.next_sg;
-
-	while (*sg != NULL) {
-		pgaddr = page_to_pfn(sg_page(*sg))
-			<< PAGE_SHIFT;
-		*kpage = pgaddr + (pginfo->next_hwpage *
-				   pginfo->hwpage_size);
-		if (!(*kpage)) {
-			ehca_gen_err("pgaddr=%llx "
-				     "sg_dma_address=%llx "
-				     "entry=%llx next_hwpage=%llx",
-				     pgaddr, (u64)sg_dma_address(*sg),
-				     pginfo->u.usr.next_nmap,
-				     pginfo->next_hwpage);
-			return -EFAULT;
-		}
-		(pginfo->hwpage_cnt)++;
-		(pginfo->next_hwpage)++;
-		kpage++;
-		if (pginfo->next_hwpage % hwpages_per_kpage == 0) {
-			(pginfo->kpage_cnt)++;
-			(pginfo->u.usr.next_nmap)++;
-			pginfo->next_hwpage = 0;
-			*sg = sg_next(*sg);
-		}
-		j++;
-		if (j >= number)
-			break;
-	}
-
-	return ret;
-}
-
-/*
- * check given pages for contiguous layout
- * last page addr is returned in prev_pgaddr for further check
- */
-static int ehca_check_kpages_per_ate(struct scatterlist **sg,
-				     int num_pages,
-				     u64 *prev_pgaddr)
-{
-	for (; *sg && num_pages > 0; *sg = sg_next(*sg), num_pages--) {
-		u64 pgaddr = page_to_pfn(sg_page(*sg)) << PAGE_SHIFT;
-		if (ehca_debug_level >= 3)
-			ehca_gen_dbg("chunk_page=%llx value=%016llx", pgaddr,
-				     *(u64 *)__va(pgaddr));
-		if (pgaddr - PAGE_SIZE != *prev_pgaddr) {
-			ehca_gen_err("uncontiguous page found pgaddr=%llx "
-				     "prev_pgaddr=%llx entries_left_in_hwpage=%x",
-				     pgaddr, *prev_pgaddr, num_pages);
-			return -EINVAL;
-		}
-		*prev_pgaddr = pgaddr;
-	}
-	return 0;
-}
-
-/* PAGE_SIZE < pginfo->hwpage_size */
-static int ehca_set_pagebuf_user2(struct ehca_mr_pginfo *pginfo,
-				  u32 number,
-				  u64 *kpage)
-{
-	int ret = 0;
-	u64 pgaddr, prev_pgaddr;
-	u32 j = 0;
-	int kpages_per_hwpage = pginfo->hwpage_size / PAGE_SIZE;
-	int nr_kpages = kpages_per_hwpage;
-	struct scatterlist **sg = &pginfo->u.usr.next_sg;
-
-	while (*sg != NULL) {
-
-		if (nr_kpages == kpages_per_hwpage) {
-			pgaddr = (page_to_pfn(sg_page(*sg))
-				   << PAGE_SHIFT);
-			*kpage = pgaddr;
-			if (!(*kpage)) {
-				ehca_gen_err("pgaddr=%llx entry=%llx",
-					     pgaddr, pginfo->u.usr.next_nmap);
-				ret = -EFAULT;
-				return ret;
-			}
-			/*
-			 * The first page in a hwpage must be aligned;
-			 * the first MR page is exempt from this rule.
-			 */
-			if (pgaddr & (pginfo->hwpage_size - 1)) {
-				if (pginfo->hwpage_cnt) {
-					ehca_gen_err(
-						"invalid alignment "
-						"pgaddr=%llx entry=%llx "
-						"mr_pgsize=%llx",
-						pgaddr, pginfo->u.usr.next_nmap,
-						pginfo->hwpage_size);
-					ret = -EFAULT;
-					return ret;
-				}
-				/* first MR page */
-				pginfo->kpage_cnt =
-					(pgaddr &
-					 (pginfo->hwpage_size - 1)) >>
-					PAGE_SHIFT;
-				nr_kpages -= pginfo->kpage_cnt;
-				*kpage = pgaddr &
-					 ~(pginfo->hwpage_size - 1);
-			}
-			if (ehca_debug_level >= 3) {
-				u64 val = *(u64 *)__va(pgaddr);
-				ehca_gen_dbg("kpage=%llx page=%llx "
-					     "value=%016llx",
-					     *kpage, pgaddr, val);
-			}
-			prev_pgaddr = pgaddr;
-			*sg = sg_next(*sg);
-			pginfo->kpage_cnt++;
-			pginfo->u.usr.next_nmap++;
-			nr_kpages--;
-			if (!nr_kpages)
-				goto next_kpage;
-			continue;
-		}
-
-		ret = ehca_check_kpages_per_ate(sg, nr_kpages,
-						&prev_pgaddr);
-		if (ret)
-			return ret;
-		pginfo->kpage_cnt += nr_kpages;
-		pginfo->u.usr.next_nmap += nr_kpages;
-
-next_kpage:
-		nr_kpages = kpages_per_hwpage;
-		(pginfo->hwpage_cnt)++;
-		kpage++;
-		j++;
-		if (j >= number)
-			break;
-	}
-
-	return ret;
-}
-
-static int ehca_set_pagebuf_phys(struct ehca_mr_pginfo *pginfo,
-				 u32 number, u64 *kpage)
-{
-	int ret = 0;
-	struct ib_phys_buf *pbuf;
-	u64 num_hw, offs_hw;
-	u32 i = 0;
-
-	/* loop over desired phys_buf_array entries */
-	while (i < number) {
-		pbuf   = pginfo->u.phy.phys_buf_array + pginfo->u.phy.next_buf;
-		num_hw  = NUM_CHUNKS((pbuf->addr % pginfo->hwpage_size) +
-				     pbuf->size, pginfo->hwpage_size);
-		offs_hw = (pbuf->addr & ~(pginfo->hwpage_size - 1)) /
-			pginfo->hwpage_size;
-		while (pginfo->next_hwpage < offs_hw + num_hw) {
-			/* sanity check */
-			if ((pginfo->kpage_cnt >= pginfo->num_kpages) ||
-			    (pginfo->hwpage_cnt >= pginfo->num_hwpages)) {
-				ehca_gen_err("kpage_cnt >= num_kpages, "
-					     "kpage_cnt=%llx num_kpages=%llx "
-					     "hwpage_cnt=%llx "
-					     "num_hwpages=%llx i=%x",
-					     pginfo->kpage_cnt,
-					     pginfo->num_kpages,
-					     pginfo->hwpage_cnt,
-					     pginfo->num_hwpages, i);
-				return -EFAULT;
-			}
-			*kpage = (pbuf->addr & ~(pginfo->hwpage_size - 1)) +
-				 (pginfo->next_hwpage * pginfo->hwpage_size);
-			if ( !(*kpage) && pbuf->addr ) {
-				ehca_gen_err("pbuf->addr=%llx pbuf->size=%llx "
-					     "next_hwpage=%llx", pbuf->addr,
-					     pbuf->size, pginfo->next_hwpage);
-				return -EFAULT;
-			}
-			(pginfo->hwpage_cnt)++;
-			(pginfo->next_hwpage)++;
-			if (PAGE_SIZE >= pginfo->hwpage_size) {
-				if (pginfo->next_hwpage %
-				    (PAGE_SIZE / pginfo->hwpage_size) == 0)
-					(pginfo->kpage_cnt)++;
-			} else
-				pginfo->kpage_cnt += pginfo->hwpage_size /
-					PAGE_SIZE;
-			kpage++;
-			i++;
-			if (i >= number) break;
-		}
-		if (pginfo->next_hwpage >= offs_hw + num_hw) {
-			(pginfo->u.phy.next_buf)++;
-			pginfo->next_hwpage = 0;
-		}
-	}
-	return ret;
-}
-
-static int ehca_set_pagebuf_fmr(struct ehca_mr_pginfo *pginfo,
-				u32 number, u64 *kpage)
-{
-	int ret = 0;
-	u64 *fmrlist;
-	u32 i;
-
-	/* loop over desired page_list entries */
-	fmrlist = pginfo->u.fmr.page_list + pginfo->u.fmr.next_listelem;
-	for (i = 0; i < number; i++) {
-		*kpage = (*fmrlist & ~(pginfo->hwpage_size - 1)) +
-			   pginfo->next_hwpage * pginfo->hwpage_size;
-		if ( !(*kpage) ) {
-			ehca_gen_err("*fmrlist=%llx fmrlist=%p "
-				     "next_listelem=%llx next_hwpage=%llx",
-				     *fmrlist, fmrlist,
-				     pginfo->u.fmr.next_listelem,
-				     pginfo->next_hwpage);
-			return -EFAULT;
-		}
-		(pginfo->hwpage_cnt)++;
-		if (pginfo->u.fmr.fmr_pgsize >= pginfo->hwpage_size) {
-			if (pginfo->next_hwpage %
-			    (pginfo->u.fmr.fmr_pgsize /
-			     pginfo->hwpage_size) == 0) {
-				(pginfo->kpage_cnt)++;
-				(pginfo->u.fmr.next_listelem)++;
-				fmrlist++;
-				pginfo->next_hwpage = 0;
-			} else
-				(pginfo->next_hwpage)++;
-		} else {
-			unsigned int cnt_per_hwpage = pginfo->hwpage_size /
-				pginfo->u.fmr.fmr_pgsize;
-			unsigned int j;
-			u64 prev = *kpage;
-			/* check if adrs are contiguous */
-			for (j = 1; j < cnt_per_hwpage; j++) {
-				u64 p = fmrlist[j] & ~(pginfo->hwpage_size - 1);
-				if (prev + pginfo->u.fmr.fmr_pgsize != p) {
-					ehca_gen_err("uncontiguous fmr pages "
-						     "found prev=%llx p=%llx "
-						     "idx=%x", prev, p, i + j);
-					return -EINVAL;
-				}
-				prev = p;
-			}
-			pginfo->kpage_cnt += cnt_per_hwpage;
-			pginfo->u.fmr.next_listelem += cnt_per_hwpage;
-			fmrlist += cnt_per_hwpage;
-		}
-		kpage++;
-	}
-	return ret;
-}
-
-/* setup page buffer from page info */
-int ehca_set_pagebuf(struct ehca_mr_pginfo *pginfo,
-		     u32 number,
-		     u64 *kpage)
-{
-	int ret;
-
-	switch (pginfo->type) {
-	case EHCA_MR_PGI_PHYS:
-		ret = ehca_set_pagebuf_phys(pginfo, number, kpage);
-		break;
-	case EHCA_MR_PGI_USER:
-		ret = PAGE_SIZE >= pginfo->hwpage_size ?
-			ehca_set_pagebuf_user1(pginfo, number, kpage) :
-			ehca_set_pagebuf_user2(pginfo, number, kpage);
-		break;
-	case EHCA_MR_PGI_FMR:
-		ret = ehca_set_pagebuf_fmr(pginfo, number, kpage);
-		break;
-	default:
-		ehca_gen_err("bad pginfo->type=%x", pginfo->type);
-		ret = -EFAULT;
-		break;
-	}
-	return ret;
-} /* end ehca_set_pagebuf() */
-
-/*----------------------------------------------------------------------*/
-
-/*
- * check MR if it is a max-MR, i.e. uses whole memory
- * in case it's a max-MR 1 is returned, else 0
- */
-int ehca_mr_is_maxmr(u64 size,
-		     u64 *iova_start)
-{
-	/* a MR is treated as max-MR only if it fits following: */
-	if ((size == ehca_mr_len) &&
-	    (iova_start == (void *)ehca_map_vaddr((void *)(KERNELBASE + PHYSICAL_START)))) {
-		ehca_gen_dbg("this is a max-MR");
-		return 1;
-	} else
-		return 0;
-} /* end ehca_mr_is_maxmr() */
-
-/*----------------------------------------------------------------------*/
-
-/* map access control for MR/MW. This routine is used for MR and MW. */
-void ehca_mrmw_map_acl(int ib_acl,
-		       u32 *hipz_acl)
-{
-	*hipz_acl = 0;
-	if (ib_acl & IB_ACCESS_REMOTE_READ)
-		*hipz_acl |= HIPZ_ACCESSCTRL_R_READ;
-	if (ib_acl & IB_ACCESS_REMOTE_WRITE)
-		*hipz_acl |= HIPZ_ACCESSCTRL_R_WRITE;
-	if (ib_acl & IB_ACCESS_REMOTE_ATOMIC)
-		*hipz_acl |= HIPZ_ACCESSCTRL_R_ATOMIC;
-	if (ib_acl & IB_ACCESS_LOCAL_WRITE)
-		*hipz_acl |= HIPZ_ACCESSCTRL_L_WRITE;
-	if (ib_acl & IB_ACCESS_MW_BIND)
-		*hipz_acl |= HIPZ_ACCESSCTRL_MW_BIND;
-} /* end ehca_mrmw_map_acl() */
-
-/*----------------------------------------------------------------------*/
-
-/* sets page size in hipz access control for MR/MW. */
-void ehca_mrmw_set_pgsize_hipz_acl(u32 pgsize, u32 *hipz_acl) /*INOUT*/
-{
-	*hipz_acl |= (ehca_encode_hwpage_size(pgsize) << 24);
-} /* end ehca_mrmw_set_pgsize_hipz_acl() */
-
-/*----------------------------------------------------------------------*/
-
-/*
- * reverse map access control for MR/MW.
- * This routine is used for MR and MW.
- */
-void ehca_mrmw_reverse_map_acl(const u32 *hipz_acl,
-			       int *ib_acl) /*OUT*/
-{
-	*ib_acl = 0;
-	if (*hipz_acl & HIPZ_ACCESSCTRL_R_READ)
-		*ib_acl |= IB_ACCESS_REMOTE_READ;
-	if (*hipz_acl & HIPZ_ACCESSCTRL_R_WRITE)
-		*ib_acl |= IB_ACCESS_REMOTE_WRITE;
-	if (*hipz_acl & HIPZ_ACCESSCTRL_R_ATOMIC)
-		*ib_acl |= IB_ACCESS_REMOTE_ATOMIC;
-	if (*hipz_acl & HIPZ_ACCESSCTRL_L_WRITE)
-		*ib_acl |= IB_ACCESS_LOCAL_WRITE;
-	if (*hipz_acl & HIPZ_ACCESSCTRL_MW_BIND)
-		*ib_acl |= IB_ACCESS_MW_BIND;
-} /* end ehca_mrmw_reverse_map_acl() */
-
-
-/*----------------------------------------------------------------------*/
-
-/*
- * MR destructor and constructor
- * used in Reregister MR verb, sets all fields in ehca_mr_t to 0,
- * except struct ib_mr and spinlock
- */
-void ehca_mr_deletenew(struct ehca_mr *mr)
-{
-	mr->flags = 0;
-	mr->num_kpages = 0;
-	mr->num_hwpages = 0;
-	mr->acl = 0;
-	mr->start = NULL;
-	mr->fmr_page_size = 0;
-	mr->fmr_max_pages = 0;
-	mr->fmr_max_maps = 0;
-	mr->fmr_map_cnt = 0;
-	memset(&mr->ipz_mr_handle, 0, sizeof(mr->ipz_mr_handle));
-	memset(&mr->galpas, 0, sizeof(mr->galpas));
-} /* end ehca_mr_deletenew() */
-
-int ehca_init_mrmw_cache(void)
-{
-	mr_cache = kmem_cache_create("ehca_cache_mr",
-				     sizeof(struct ehca_mr), 0,
-				     SLAB_HWCACHE_ALIGN,
-				     NULL);
-	if (!mr_cache)
-		return -ENOMEM;
-	mw_cache = kmem_cache_create("ehca_cache_mw",
-				     sizeof(struct ehca_mw), 0,
-				     SLAB_HWCACHE_ALIGN,
-				     NULL);
-	if (!mw_cache) {
-		kmem_cache_destroy(mr_cache);
-		mr_cache = NULL;
-		return -ENOMEM;
-	}
-	return 0;
-}
-
-void ehca_cleanup_mrmw_cache(void)
-{
-	if (mr_cache)
-		kmem_cache_destroy(mr_cache);
-	if (mw_cache)
-		kmem_cache_destroy(mw_cache);
-}
-
-static inline int ehca_init_top_bmap(struct ehca_top_bmap *ehca_top_bmap,
-				     int dir)
-{
-	if (!ehca_bmap_valid(ehca_top_bmap->dir[dir])) {
-		ehca_top_bmap->dir[dir] =
-			kmalloc(sizeof(struct ehca_dir_bmap), GFP_KERNEL);
-		if (!ehca_top_bmap->dir[dir])
-			return -ENOMEM;
-		/* Set map block to 0xFF according to EHCA_INVAL_ADDR */
-		memset(ehca_top_bmap->dir[dir], 0xFF, EHCA_ENT_MAP_SIZE);
-	}
-	return 0;
-}
-
-static inline int ehca_init_bmap(struct ehca_bmap *ehca_bmap, int top, int dir)
-{
-	if (!ehca_bmap_valid(ehca_bmap->top[top])) {
-		ehca_bmap->top[top] =
-			kmalloc(sizeof(struct ehca_top_bmap), GFP_KERNEL);
-		if (!ehca_bmap->top[top])
-			return -ENOMEM;
-		/* Set map block to 0xFF according to EHCA_INVAL_ADDR */
-		memset(ehca_bmap->top[top], 0xFF, EHCA_DIR_MAP_SIZE);
-	}
-	return ehca_init_top_bmap(ehca_bmap->top[top], dir);
-}
-
-static inline int ehca_calc_index(unsigned long i, unsigned long s)
-{
-	return (i >> s) & EHCA_INDEX_MASK;
-}
-
-void ehca_destroy_busmap(void)
-{
-	int top, dir;
-
-	if (!ehca_bmap)
-		return;
-
-	for (top = 0; top < EHCA_MAP_ENTRIES; top++) {
-		if (!ehca_bmap_valid(ehca_bmap->top[top]))
-			continue;
-		for (dir = 0; dir < EHCA_MAP_ENTRIES; dir++) {
-			if (!ehca_bmap_valid(ehca_bmap->top[top]->dir[dir]))
-				continue;
-
-			kfree(ehca_bmap->top[top]->dir[dir]);
-		}
-
-		kfree(ehca_bmap->top[top]);
-	}
-
-	kfree(ehca_bmap);
-	ehca_bmap = NULL;
-}
-
-static int ehca_update_busmap(unsigned long pfn, unsigned long nr_pages)
-{
-	unsigned long i, start_section, end_section;
-	int top, dir, idx;
-
-	if (!nr_pages)
-		return 0;
-
-	if (!ehca_bmap) {
-		ehca_bmap = kmalloc(sizeof(struct ehca_bmap), GFP_KERNEL);
-		if (!ehca_bmap)
-			return -ENOMEM;
-		/* Set map block to 0xFF according to EHCA_INVAL_ADDR */
-		memset(ehca_bmap, 0xFF, EHCA_TOP_MAP_SIZE);
-	}
-
-	start_section = (pfn * PAGE_SIZE) / EHCA_SECTSIZE;
-	end_section = ((pfn + nr_pages) * PAGE_SIZE) / EHCA_SECTSIZE;
-	for (i = start_section; i < end_section; i++) {
-		int ret;
-		top = ehca_calc_index(i, EHCA_TOP_INDEX_SHIFT);
-		dir = ehca_calc_index(i, EHCA_DIR_INDEX_SHIFT);
-		idx = i & EHCA_INDEX_MASK;
-
-		ret = ehca_init_bmap(ehca_bmap, top, dir);
-		if (ret) {
-			ehca_destroy_busmap();
-			return ret;
-		}
-		ehca_bmap->top[top]->dir[dir]->ent[idx] = ehca_mr_len;
-		ehca_mr_len += EHCA_SECTSIZE;
-	}
-	return 0;
-}
-
-static int ehca_is_hugepage(unsigned long pfn)
-{
-	int page_order;
-
-	if (pfn & EHCA_HUGEPAGE_PFN_MASK)
-		return 0;
-
-	page_order = compound_order(pfn_to_page(pfn));
-	if (page_order + PAGE_SHIFT != EHCA_HUGEPAGESHIFT)
-		return 0;
-
-	return 1;
-}
-
-static int ehca_create_busmap_callback(unsigned long initial_pfn,
-				       unsigned long total_nr_pages, void *arg)
-{
-	int ret;
-	unsigned long pfn, start_pfn, end_pfn, nr_pages;
-
-	if ((total_nr_pages * PAGE_SIZE) < EHCA_HUGEPAGE_SIZE)
-		return ehca_update_busmap(initial_pfn, total_nr_pages);
-
-	/* Given chunk is >= 16GB -> check for hugepages */
-	start_pfn = initial_pfn;
-	end_pfn = initial_pfn + total_nr_pages;
-	pfn = start_pfn;
-
-	while (pfn < end_pfn) {
-		if (ehca_is_hugepage(pfn)) {
-			/* Add mem found in front of the hugepage */
-			nr_pages = pfn - start_pfn;
-			ret = ehca_update_busmap(start_pfn, nr_pages);
-			if (ret)
-				return ret;
-			/* Skip the hugepage */
-			pfn += (EHCA_HUGEPAGE_SIZE / PAGE_SIZE);
-			start_pfn = pfn;
-		} else
-			pfn += (EHCA_SECTSIZE / PAGE_SIZE);
-	}
-
-	/* Add mem found behind the hugepage(s)  */
-	nr_pages = pfn - start_pfn;
-	return ehca_update_busmap(start_pfn, nr_pages);
-}
-
-int ehca_create_busmap(void)
-{
-	int ret;
-
-	ehca_mr_len = 0;
-	ret = walk_system_ram_range(0, 1ULL << MAX_PHYSMEM_BITS, NULL,
-				   ehca_create_busmap_callback);
-	return ret;
-}
-
-static int ehca_reg_bmap_mr_rpages(struct ehca_shca *shca,
-				   struct ehca_mr *e_mr,
-				   struct ehca_mr_pginfo *pginfo)
-{
-	int top;
-	u64 hret, *kpage;
-
-	kpage = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!kpage) {
-		ehca_err(&shca->ib_device, "kpage alloc failed");
-		return -ENOMEM;
-	}
-	for (top = 0; top < EHCA_MAP_ENTRIES; top++) {
-		if (!ehca_bmap_valid(ehca_bmap->top[top]))
-			continue;
-		hret = ehca_reg_mr_dir_sections(top, kpage, shca, e_mr, pginfo);
-		if ((hret != H_PAGE_REGISTERED) && (hret != H_SUCCESS))
-			break;
-	}
-
-	ehca_free_fw_ctrlblock(kpage);
-
-	if (hret == H_SUCCESS)
-		return 0; /* Everything is fine */
-	else {
-		ehca_err(&shca->ib_device, "ehca_reg_bmap_mr_rpages failed, "
-				 "h_ret=%lli e_mr=%p top=%x lkey=%x "
-				 "hca_hndl=%llx mr_hndl=%llx", hret, e_mr, top,
-				 e_mr->ib.ib_mr.lkey,
-				 shca->ipz_hca_handle.handle,
-				 e_mr->ipz_mr_handle.handle);
-		return ehca2ib_return_code(hret);
-	}
-}
-
-static u64 ehca_map_vaddr(void *caddr)
-{
-	int top, dir, idx;
-	unsigned long abs_addr, offset;
-	u64 entry;
-
-	if (!ehca_bmap)
-		return EHCA_INVAL_ADDR;
-
-	abs_addr = __pa(caddr);
-	top = ehca_calc_index(abs_addr, EHCA_TOP_INDEX_SHIFT + EHCA_SECTSHIFT);
-	if (!ehca_bmap_valid(ehca_bmap->top[top]))
-		return EHCA_INVAL_ADDR;
-
-	dir = ehca_calc_index(abs_addr, EHCA_DIR_INDEX_SHIFT + EHCA_SECTSHIFT);
-	if (!ehca_bmap_valid(ehca_bmap->top[top]->dir[dir]))
-		return EHCA_INVAL_ADDR;
-
-	idx = ehca_calc_index(abs_addr, EHCA_SECTSHIFT);
-
-	entry = ehca_bmap->top[top]->dir[dir]->ent[idx];
-	if (ehca_bmap_valid(entry)) {
-		offset = (unsigned long)caddr & (EHCA_SECTSIZE - 1);
-		return entry | offset;
-	} else
-		return EHCA_INVAL_ADDR;
-}
-
-static int ehca_dma_mapping_error(struct ib_device *dev, u64 dma_addr)
-{
-	return dma_addr == EHCA_INVAL_ADDR;
-}
-
-static u64 ehca_dma_map_single(struct ib_device *dev, void *cpu_addr,
-			       size_t size, enum dma_data_direction direction)
-{
-	if (cpu_addr)
-		return ehca_map_vaddr(cpu_addr);
-	else
-		return EHCA_INVAL_ADDR;
-}
-
-static void ehca_dma_unmap_single(struct ib_device *dev, u64 addr, size_t size,
-				  enum dma_data_direction direction)
-{
-	/* This is only a stub; nothing to be done here */
-}
-
-static u64 ehca_dma_map_page(struct ib_device *dev, struct page *page,
-			     unsigned long offset, size_t size,
-			     enum dma_data_direction direction)
-{
-	u64 addr;
-
-	if (offset + size > PAGE_SIZE)
-		return EHCA_INVAL_ADDR;
-
-	addr = ehca_map_vaddr(page_address(page));
-	if (!ehca_dma_mapping_error(dev, addr))
-		addr += offset;
-
-	return addr;
-}
-
-static void ehca_dma_unmap_page(struct ib_device *dev, u64 addr, size_t size,
-				enum dma_data_direction direction)
-{
-	/* This is only a stub; nothing to be done here */
-}
-
-static int ehca_dma_map_sg(struct ib_device *dev, struct scatterlist *sgl,
-			   int nents, enum dma_data_direction direction)
-{
-	struct scatterlist *sg;
-	int i;
-
-	for_each_sg(sgl, sg, nents, i) {
-		u64 addr;
-		addr = ehca_map_vaddr(sg_virt(sg));
-		if (ehca_dma_mapping_error(dev, addr))
-			return 0;
-
-		sg->dma_address = addr;
-		sg->dma_length = sg->length;
-	}
-	return nents;
-}
-
-static void ehca_dma_unmap_sg(struct ib_device *dev, struct scatterlist *sg,
-			      int nents, enum dma_data_direction direction)
-{
-	/* This is only a stub; nothing to be done here */
-}
-
-static void ehca_dma_sync_single_for_cpu(struct ib_device *dev, u64 addr,
-					 size_t size,
-					 enum dma_data_direction dir)
-{
-	dma_sync_single_for_cpu(dev->dma_device, addr, size, dir);
-}
-
-static void ehca_dma_sync_single_for_device(struct ib_device *dev, u64 addr,
-					    size_t size,
-					    enum dma_data_direction dir)
-{
-	dma_sync_single_for_device(dev->dma_device, addr, size, dir);
-}
-
-static void *ehca_dma_alloc_coherent(struct ib_device *dev, size_t size,
-				     u64 *dma_handle, gfp_t flag)
-{
-	struct page *p;
-	void *addr = NULL;
-	u64 dma_addr;
-
-	p = alloc_pages(flag, get_order(size));
-	if (p) {
-		addr = page_address(p);
-		dma_addr = ehca_map_vaddr(addr);
-		if (ehca_dma_mapping_error(dev, dma_addr)) {
-			free_pages((unsigned long)addr,	get_order(size));
-			return NULL;
-		}
-		if (dma_handle)
-			*dma_handle = dma_addr;
-		return addr;
-	}
-	return NULL;
-}
-
-static void ehca_dma_free_coherent(struct ib_device *dev, size_t size,
-				   void *cpu_addr, u64 dma_handle)
-{
-	if (cpu_addr && size)
-		free_pages((unsigned long)cpu_addr, get_order(size));
-}
-
-
-struct ib_dma_mapping_ops ehca_dma_mapping_ops = {
-	.mapping_error          = ehca_dma_mapping_error,
-	.map_single             = ehca_dma_map_single,
-	.unmap_single           = ehca_dma_unmap_single,
-	.map_page               = ehca_dma_map_page,
-	.unmap_page             = ehca_dma_unmap_page,
-	.map_sg                 = ehca_dma_map_sg,
-	.unmap_sg               = ehca_dma_unmap_sg,
-	.sync_single_for_cpu    = ehca_dma_sync_single_for_cpu,
-	.sync_single_for_device = ehca_dma_sync_single_for_device,
-	.alloc_coherent         = ehca_dma_alloc_coherent,
-	.free_coherent          = ehca_dma_free_coherent,
-};
diff --git a/drivers/staging/rdma/ehca/ehca_mrmw.h b/drivers/staging/rdma/ehca/ehca_mrmw.h
deleted file mode 100644
index 50d8b51306dd..000000000000
--- a/drivers/staging/rdma/ehca/ehca_mrmw.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  MR/MW declarations and inline functions
- *
- *  Authors: Dietmar Decker <ddecker@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef _EHCA_MRMW_H_
-#define _EHCA_MRMW_H_
-
-enum ehca_reg_type {
-	EHCA_REG_MR,
-	EHCA_REG_BUSMAP_MR
-};
-
-int ehca_reg_mr(struct ehca_shca *shca,
-		struct ehca_mr *e_mr,
-		u64 *iova_start,
-		u64 size,
-		int acl,
-		struct ehca_pd *e_pd,
-		struct ehca_mr_pginfo *pginfo,
-		u32 *lkey,
-		u32 *rkey,
-		enum ehca_reg_type reg_type);
-
-int ehca_reg_mr_rpages(struct ehca_shca *shca,
-		       struct ehca_mr *e_mr,
-		       struct ehca_mr_pginfo *pginfo);
-
-int ehca_rereg_mr(struct ehca_shca *shca,
-		  struct ehca_mr *e_mr,
-		  u64 *iova_start,
-		  u64 size,
-		  int mr_access_flags,
-		  struct ehca_pd *e_pd,
-		  struct ehca_mr_pginfo *pginfo,
-		  u32 *lkey,
-		  u32 *rkey);
-
-int ehca_unmap_one_fmr(struct ehca_shca *shca,
-		       struct ehca_mr *e_fmr);
-
-int ehca_reg_smr(struct ehca_shca *shca,
-		 struct ehca_mr *e_origmr,
-		 struct ehca_mr *e_newmr,
-		 u64 *iova_start,
-		 int acl,
-		 struct ehca_pd *e_pd,
-		 u32 *lkey,
-		 u32 *rkey);
-
-int ehca_reg_internal_maxmr(struct ehca_shca *shca,
-			    struct ehca_pd *e_pd,
-			    struct ehca_mr **maxmr);
-
-int ehca_reg_maxmr(struct ehca_shca *shca,
-		   struct ehca_mr *e_newmr,
-		   u64 *iova_start,
-		   int acl,
-		   struct ehca_pd *e_pd,
-		   u32 *lkey,
-		   u32 *rkey);
-
-int ehca_dereg_internal_maxmr(struct ehca_shca *shca);
-
-int ehca_mr_chk_buf_and_calc_size(struct ib_phys_buf *phys_buf_array,
-				  int num_phys_buf,
-				  u64 *iova_start,
-				  u64 *size);
-
-int ehca_fmr_check_page_list(struct ehca_mr *e_fmr,
-			     u64 *page_list,
-			     int list_len);
-
-int ehca_set_pagebuf(struct ehca_mr_pginfo *pginfo,
-		     u32 number,
-		     u64 *kpage);
-
-int ehca_mr_is_maxmr(u64 size,
-		     u64 *iova_start);
-
-void ehca_mrmw_map_acl(int ib_acl,
-		       u32 *hipz_acl);
-
-void ehca_mrmw_set_pgsize_hipz_acl(u32 pgsize, u32 *hipz_acl);
-
-void ehca_mrmw_reverse_map_acl(const u32 *hipz_acl,
-			       int *ib_acl);
-
-void ehca_mr_deletenew(struct ehca_mr *mr);
-
-int ehca_create_busmap(void);
-
-void ehca_destroy_busmap(void);
-
-extern struct ib_dma_mapping_ops ehca_dma_mapping_ops;
-#endif  /*_EHCA_MRMW_H_*/
diff --git a/drivers/staging/rdma/ehca/ehca_pd.c b/drivers/staging/rdma/ehca/ehca_pd.c
deleted file mode 100644
index 351577a6670a..000000000000
--- a/drivers/staging/rdma/ehca/ehca_pd.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  PD functions
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_tools.h"
-#include "ehca_iverbs.h"
-
-static struct kmem_cache *pd_cache;
-
-struct ib_pd *ehca_alloc_pd(struct ib_device *device,
-			    struct ib_ucontext *context, struct ib_udata *udata)
-{
-	struct ehca_pd *pd;
-	int i;
-
-	pd = kmem_cache_zalloc(pd_cache, GFP_KERNEL);
-	if (!pd) {
-		ehca_err(device, "device=%p context=%p out of memory",
-			 device, context);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	for (i = 0; i < 2; i++) {
-		INIT_LIST_HEAD(&pd->free[i]);
-		INIT_LIST_HEAD(&pd->full[i]);
-	}
-	mutex_init(&pd->lock);
-
-	/*
-	 * Kernel PD: when device = -1, 0
-	 * User   PD: when context != -1
-	 */
-	if (!context) {
-		/*
-		 * Kernel PDs after init reuses always
-		 * the one created in ehca_shca_reopen()
-		 */
-		struct ehca_shca *shca = container_of(device, struct ehca_shca,
-						      ib_device);
-		pd->fw_pd.value = shca->pd->fw_pd.value;
-	} else
-		pd->fw_pd.value = (u64)pd;
-
-	return &pd->ib_pd;
-}
-
-int ehca_dealloc_pd(struct ib_pd *pd)
-{
-	struct ehca_pd *my_pd = container_of(pd, struct ehca_pd, ib_pd);
-	int i, leftovers = 0;
-	struct ipz_small_queue_page *page, *tmp;
-
-	for (i = 0; i < 2; i++) {
-		list_splice(&my_pd->full[i], &my_pd->free[i]);
-		list_for_each_entry_safe(page, tmp, &my_pd->free[i], list) {
-			leftovers = 1;
-			free_page(page->page);
-			kmem_cache_free(small_qp_cache, page);
-		}
-	}
-
-	if (leftovers)
-		ehca_warn(pd->device,
-			  "Some small queue pages were not freed");
-
-	kmem_cache_free(pd_cache, my_pd);
-
-	return 0;
-}
-
-int ehca_init_pd_cache(void)
-{
-	pd_cache = kmem_cache_create("ehca_cache_pd",
-				     sizeof(struct ehca_pd), 0,
-				     SLAB_HWCACHE_ALIGN,
-				     NULL);
-	if (!pd_cache)
-		return -ENOMEM;
-	return 0;
-}
-
-void ehca_cleanup_pd_cache(void)
-{
-	if (pd_cache)
-		kmem_cache_destroy(pd_cache);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_qes.h b/drivers/staging/rdma/ehca/ehca_qes.h
deleted file mode 100644
index 90c4efa67586..000000000000
--- a/drivers/staging/rdma/ehca/ehca_qes.h
+++ /dev/null
@@ -1,260 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Hardware request structures
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-
-#ifndef _EHCA_QES_H_
-#define _EHCA_QES_H_
-
-#include "ehca_tools.h"
-
-/* virtual scatter gather entry to specify remote addresses with length */
-struct ehca_vsgentry {
-	u64 vaddr;
-	u32 lkey;
-	u32 length;
-};
-
-#define GRH_FLAG_MASK        EHCA_BMASK_IBM( 7,  7)
-#define GRH_IPVERSION_MASK   EHCA_BMASK_IBM( 0,  3)
-#define GRH_TCLASS_MASK      EHCA_BMASK_IBM( 4, 12)
-#define GRH_FLOWLABEL_MASK   EHCA_BMASK_IBM(13, 31)
-#define GRH_PAYLEN_MASK      EHCA_BMASK_IBM(32, 47)
-#define GRH_NEXTHEADER_MASK  EHCA_BMASK_IBM(48, 55)
-#define GRH_HOPLIMIT_MASK    EHCA_BMASK_IBM(56, 63)
-
-/*
- * Unreliable Datagram Address Vector Format
- * see IBTA Vol1 chapter 8.3 Global Routing Header
- */
-struct ehca_ud_av {
-	u8 sl;
-	u8 lnh;
-	u16 dlid;
-	u8 reserved1;
-	u8 reserved2;
-	u8 reserved3;
-	u8 slid_path_bits;
-	u8 reserved4;
-	u8 ipd;
-	u8 reserved5;
-	u8 pmtu;
-	u32 reserved6;
-	u64 reserved7;
-	union {
-		struct {
-			u64 word_0; /* always set to 6  */
-			/*should be 0x1B for IB transport */
-			u64 word_1;
-			u64 word_2;
-			u64 word_3;
-			u64 word_4;
-		} grh;
-		struct {
-			u32 wd_0;
-			u32 wd_1;
-			/* DWord_1 --> SGID */
-
-			u32 sgid_wd3;
-			u32 sgid_wd2;
-
-			u32 sgid_wd1;
-			u32 sgid_wd0;
-			/* DWord_3 --> DGID */
-
-			u32 dgid_wd3;
-			u32 dgid_wd2;
-
-			u32 dgid_wd1;
-			u32 dgid_wd0;
-		} grh_l;
-	};
-};
-
-/* maximum number of sg entries allowed in a WQE */
-#define MAX_WQE_SG_ENTRIES 252
-
-#define WQE_OPTYPE_SEND             0x80
-#define WQE_OPTYPE_RDMAREAD         0x40
-#define WQE_OPTYPE_RDMAWRITE        0x20
-#define WQE_OPTYPE_CMPSWAP          0x10
-#define WQE_OPTYPE_FETCHADD         0x08
-#define WQE_OPTYPE_BIND             0x04
-
-#define WQE_WRFLAG_REQ_SIGNAL_COM   0x80
-#define WQE_WRFLAG_FENCE            0x40
-#define WQE_WRFLAG_IMM_DATA_PRESENT 0x20
-#define WQE_WRFLAG_SOLIC_EVENT      0x10
-
-#define WQEF_CACHE_HINT             0x80
-#define WQEF_CACHE_HINT_RD_WR       0x40
-#define WQEF_TIMED_WQE              0x20
-#define WQEF_PURGE                  0x08
-#define WQEF_HIGH_NIBBLE            0xF0
-
-#define MW_BIND_ACCESSCTRL_R_WRITE   0x40
-#define MW_BIND_ACCESSCTRL_R_READ    0x20
-#define MW_BIND_ACCESSCTRL_R_ATOMIC  0x10
-
-struct ehca_wqe {
-	u64 work_request_id;
-	u8 optype;
-	u8 wr_flag;
-	u16 pkeyi;
-	u8 wqef;
-	u8 nr_of_data_seg;
-	u16 wqe_provided_slid;
-	u32 destination_qp_number;
-	u32 resync_psn_sqp;
-	u32 local_ee_context_qkey;
-	u32 immediate_data;
-	union {
-		struct {
-			u64 remote_virtual_address;
-			u32 rkey;
-			u32 reserved;
-			u64 atomic_1st_op_dma_len;
-			u64 atomic_2nd_op;
-			struct ehca_vsgentry sg_list[MAX_WQE_SG_ENTRIES];
-
-		} nud;
-		struct {
-			u64 ehca_ud_av_ptr;
-			u64 reserved1;
-			u64 reserved2;
-			u64 reserved3;
-			struct ehca_vsgentry sg_list[MAX_WQE_SG_ENTRIES];
-		} ud_avp;
-		struct {
-			struct ehca_ud_av ud_av;
-			struct ehca_vsgentry sg_list[MAX_WQE_SG_ENTRIES -
-						     2];
-		} ud_av;
-		struct {
-			u64 reserved0;
-			u64 reserved1;
-			u64 reserved2;
-			u64 reserved3;
-			struct ehca_vsgentry sg_list[MAX_WQE_SG_ENTRIES];
-		} all_rcv;
-
-		struct {
-			u64 reserved;
-			u32 rkey;
-			u32 old_rkey;
-			u64 reserved1;
-			u64 reserved2;
-			u64 virtual_address;
-			u32 reserved3;
-			u32 length;
-			u32 reserved4;
-			u16 reserved5;
-			u8 reserved6;
-			u8 lr_ctl;
-			u32 lkey;
-			u32 reserved7;
-			u64 reserved8;
-			u64 reserved9;
-			u64 reserved10;
-			u64 reserved11;
-		} bind;
-		struct {
-			u64 reserved12;
-			u64 reserved13;
-			u32 size;
-			u32 start;
-		} inline_data;
-	} u;
-
-};
-
-#define WC_SEND_RECEIVE EHCA_BMASK_IBM(0, 0)
-#define WC_IMM_DATA     EHCA_BMASK_IBM(1, 1)
-#define WC_GRH_PRESENT  EHCA_BMASK_IBM(2, 2)
-#define WC_SE_BIT       EHCA_BMASK_IBM(3, 3)
-#define WC_STATUS_ERROR_BIT 0x80000000
-#define WC_STATUS_REMOTE_ERROR_FLAGS 0x0000F800
-#define WC_STATUS_PURGE_BIT 0x10
-#define WC_SEND_RECEIVE_BIT 0x80
-
-struct ehca_cqe {
-	u64 work_request_id;
-	u8 optype;
-	u8 w_completion_flags;
-	u16 reserved1;
-	u32 nr_bytes_transferred;
-	u32 immediate_data;
-	u32 local_qp_number;
-	u8 freed_resource_count;
-	u8 service_level;
-	u16 wqe_count;
-	u32 qp_token;
-	u32 qkey_ee_token;
-	u32 remote_qp_number;
-	u16 dlid;
-	u16 rlid;
-	u16 reserved2;
-	u16 pkey_index;
-	u32 cqe_timestamp;
-	u32 wqe_timestamp;
-	u8 wqe_timestamp_valid;
-	u8 reserved3;
-	u8 reserved4;
-	u8 cqe_flags;
-	u32 status;
-};
-
-struct ehca_eqe {
-	u64 entry;
-};
-
-struct ehca_mrte {
-	u64 starting_va;
-	u64 length; /* length of memory region in bytes*/
-	u32 pd;
-	u8 key_instance;
-	u8 pagesize;
-	u8 mr_control;
-	u8 local_remote_access_ctrl;
-	u8 reserved[0x20 - 0x18];
-	u64 at_pointer[4];
-};
-#endif /*_EHCA_QES_H_*/
diff --git a/drivers/staging/rdma/ehca/ehca_qp.c b/drivers/staging/rdma/ehca/ehca_qp.c
deleted file mode 100644
index 2e89356c46fa..000000000000
--- a/drivers/staging/rdma/ehca/ehca_qp.c
+++ /dev/null
@@ -1,2257 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  QP functions
- *
- *  Authors: Joachim Fenkes <fenkes@de.ibm.com>
- *           Stefan Roscher <stefan.roscher@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_classes.h"
-#include "ehca_tools.h"
-#include "ehca_qes.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-#include "hipz_fns.h"
-
-static struct kmem_cache *qp_cache;
-
-/*
- * attributes not supported by query qp
- */
-#define QP_ATTR_QUERY_NOT_SUPPORTED (IB_QP_ACCESS_FLAGS       | \
-				     IB_QP_EN_SQD_ASYNC_NOTIFY)
-
-/*
- * ehca (internal) qp state values
- */
-enum ehca_qp_state {
-	EHCA_QPS_RESET = 1,
-	EHCA_QPS_INIT = 2,
-	EHCA_QPS_RTR = 3,
-	EHCA_QPS_RTS = 5,
-	EHCA_QPS_SQD = 6,
-	EHCA_QPS_SQE = 8,
-	EHCA_QPS_ERR = 128
-};
-
-/*
- * qp state transitions as defined by IB Arch Rel 1.1 page 431
- */
-enum ib_qp_statetrans {
-	IB_QPST_ANY2RESET,
-	IB_QPST_ANY2ERR,
-	IB_QPST_RESET2INIT,
-	IB_QPST_INIT2RTR,
-	IB_QPST_INIT2INIT,
-	IB_QPST_RTR2RTS,
-	IB_QPST_RTS2SQD,
-	IB_QPST_RTS2RTS,
-	IB_QPST_SQD2RTS,
-	IB_QPST_SQE2RTS,
-	IB_QPST_SQD2SQD,
-	IB_QPST_MAX	/* nr of transitions, this must be last!!! */
-};
-
-/*
- * ib2ehca_qp_state maps IB to ehca qp_state
- * returns ehca qp state corresponding to given ib qp state
- */
-static inline enum ehca_qp_state ib2ehca_qp_state(enum ib_qp_state ib_qp_state)
-{
-	switch (ib_qp_state) {
-	case IB_QPS_RESET:
-		return EHCA_QPS_RESET;
-	case IB_QPS_INIT:
-		return EHCA_QPS_INIT;
-	case IB_QPS_RTR:
-		return EHCA_QPS_RTR;
-	case IB_QPS_RTS:
-		return EHCA_QPS_RTS;
-	case IB_QPS_SQD:
-		return EHCA_QPS_SQD;
-	case IB_QPS_SQE:
-		return EHCA_QPS_SQE;
-	case IB_QPS_ERR:
-		return EHCA_QPS_ERR;
-	default:
-		ehca_gen_err("invalid ib_qp_state=%x", ib_qp_state);
-		return -EINVAL;
-	}
-}
-
-/*
- * ehca2ib_qp_state maps ehca to IB qp_state
- * returns ib qp state corresponding to given ehca qp state
- */
-static inline enum ib_qp_state ehca2ib_qp_state(enum ehca_qp_state
-						ehca_qp_state)
-{
-	switch (ehca_qp_state) {
-	case EHCA_QPS_RESET:
-		return IB_QPS_RESET;
-	case EHCA_QPS_INIT:
-		return IB_QPS_INIT;
-	case EHCA_QPS_RTR:
-		return IB_QPS_RTR;
-	case EHCA_QPS_RTS:
-		return IB_QPS_RTS;
-	case EHCA_QPS_SQD:
-		return IB_QPS_SQD;
-	case EHCA_QPS_SQE:
-		return IB_QPS_SQE;
-	case EHCA_QPS_ERR:
-		return IB_QPS_ERR;
-	default:
-		ehca_gen_err("invalid ehca_qp_state=%x", ehca_qp_state);
-		return -EINVAL;
-	}
-}
-
-/*
- * ehca_qp_type used as index for req_attr and opt_attr of
- * struct ehca_modqp_statetrans
- */
-enum ehca_qp_type {
-	QPT_RC = 0,
-	QPT_UC = 1,
-	QPT_UD = 2,
-	QPT_SQP = 3,
-	QPT_MAX
-};
-
-/*
- * ib2ehcaqptype maps Ib to ehca qp_type
- * returns ehca qp type corresponding to ib qp type
- */
-static inline enum ehca_qp_type ib2ehcaqptype(enum ib_qp_type ibqptype)
-{
-	switch (ibqptype) {
-	case IB_QPT_SMI:
-	case IB_QPT_GSI:
-		return QPT_SQP;
-	case IB_QPT_RC:
-		return QPT_RC;
-	case IB_QPT_UC:
-		return QPT_UC;
-	case IB_QPT_UD:
-		return QPT_UD;
-	default:
-		ehca_gen_err("Invalid ibqptype=%x", ibqptype);
-		return -EINVAL;
-	}
-}
-
-static inline enum ib_qp_statetrans get_modqp_statetrans(int ib_fromstate,
-							 int ib_tostate)
-{
-	int index = -EINVAL;
-	switch (ib_tostate) {
-	case IB_QPS_RESET:
-		index = IB_QPST_ANY2RESET;
-		break;
-	case IB_QPS_INIT:
-		switch (ib_fromstate) {
-		case IB_QPS_RESET:
-			index = IB_QPST_RESET2INIT;
-			break;
-		case IB_QPS_INIT:
-			index = IB_QPST_INIT2INIT;
-			break;
-		}
-		break;
-	case IB_QPS_RTR:
-		if (ib_fromstate == IB_QPS_INIT)
-			index = IB_QPST_INIT2RTR;
-		break;
-	case IB_QPS_RTS:
-		switch (ib_fromstate) {
-		case IB_QPS_RTR:
-			index = IB_QPST_RTR2RTS;
-			break;
-		case IB_QPS_RTS:
-			index = IB_QPST_RTS2RTS;
-			break;
-		case IB_QPS_SQD:
-			index = IB_QPST_SQD2RTS;
-			break;
-		case IB_QPS_SQE:
-			index = IB_QPST_SQE2RTS;
-			break;
-		}
-		break;
-	case IB_QPS_SQD:
-		if (ib_fromstate == IB_QPS_RTS)
-			index = IB_QPST_RTS2SQD;
-		break;
-	case IB_QPS_SQE:
-		break;
-	case IB_QPS_ERR:
-		index = IB_QPST_ANY2ERR;
-		break;
-	default:
-		break;
-	}
-	return index;
-}
-
-/*
- * ibqptype2servicetype returns hcp service type corresponding to given
- * ib qp type used by create_qp()
- */
-static inline int ibqptype2servicetype(enum ib_qp_type ibqptype)
-{
-	switch (ibqptype) {
-	case IB_QPT_SMI:
-	case IB_QPT_GSI:
-		return ST_UD;
-	case IB_QPT_RC:
-		return ST_RC;
-	case IB_QPT_UC:
-		return ST_UC;
-	case IB_QPT_UD:
-		return ST_UD;
-	case IB_QPT_RAW_IPV6:
-		return -EINVAL;
-	case IB_QPT_RAW_ETHERTYPE:
-		return -EINVAL;
-	default:
-		ehca_gen_err("Invalid ibqptype=%x", ibqptype);
-		return -EINVAL;
-	}
-}
-
-/*
- * init userspace queue info from ipz_queue data
- */
-static inline void queue2resp(struct ipzu_queue_resp *resp,
-			      struct ipz_queue *queue)
-{
-	resp->qe_size = queue->qe_size;
-	resp->act_nr_of_sg = queue->act_nr_of_sg;
-	resp->queue_length = queue->queue_length;
-	resp->pagesize = queue->pagesize;
-	resp->toggle_state = queue->toggle_state;
-	resp->offset = queue->offset;
-}
-
-/*
- * init_qp_queue initializes/constructs r/squeue and registers queue pages.
- */
-static inline int init_qp_queue(struct ehca_shca *shca,
-				struct ehca_pd *pd,
-				struct ehca_qp *my_qp,
-				struct ipz_queue *queue,
-				int q_type,
-				u64 expected_hret,
-				struct ehca_alloc_queue_parms *parms,
-				int wqe_size)
-{
-	int ret, cnt, ipz_rc, nr_q_pages;
-	void *vpage;
-	u64 rpage, h_ret;
-	struct ib_device *ib_dev = &shca->ib_device;
-	struct ipz_adapter_handle ipz_hca_handle = shca->ipz_hca_handle;
-
-	if (!parms->queue_size)
-		return 0;
-
-	if (parms->is_small) {
-		nr_q_pages = 1;
-		ipz_rc = ipz_queue_ctor(pd, queue, nr_q_pages,
-					128 << parms->page_size,
-					wqe_size, parms->act_nr_sges, 1);
-	} else {
-		nr_q_pages = parms->queue_size;
-		ipz_rc = ipz_queue_ctor(pd, queue, nr_q_pages,
-					EHCA_PAGESIZE, wqe_size,
-					parms->act_nr_sges, 0);
-	}
-
-	if (!ipz_rc) {
-		ehca_err(ib_dev, "Cannot allocate page for queue. ipz_rc=%i",
-			 ipz_rc);
-		return -EBUSY;
-	}
-
-	/* register queue pages */
-	for (cnt = 0; cnt < nr_q_pages; cnt++) {
-		vpage = ipz_qpageit_get_inc(queue);
-		if (!vpage) {
-			ehca_err(ib_dev, "ipz_qpageit_get_inc() "
-				 "failed p_vpage= %p", vpage);
-			ret = -EINVAL;
-			goto init_qp_queue1;
-		}
-		rpage = __pa(vpage);
-
-		h_ret = hipz_h_register_rpage_qp(ipz_hca_handle,
-						 my_qp->ipz_qp_handle,
-						 NULL, 0, q_type,
-						 rpage, parms->is_small ? 0 : 1,
-						 my_qp->galpas.kernel);
-		if (cnt == (nr_q_pages - 1)) {	/* last page! */
-			if (h_ret != expected_hret) {
-				ehca_err(ib_dev, "hipz_qp_register_rpage() "
-					 "h_ret=%lli", h_ret);
-				ret = ehca2ib_return_code(h_ret);
-				goto init_qp_queue1;
-			}
-			vpage = ipz_qpageit_get_inc(&my_qp->ipz_rqueue);
-			if (vpage) {
-				ehca_err(ib_dev, "ipz_qpageit_get_inc() "
-					 "should not succeed vpage=%p", vpage);
-				ret = -EINVAL;
-				goto init_qp_queue1;
-			}
-		} else {
-			if (h_ret != H_PAGE_REGISTERED) {
-				ehca_err(ib_dev, "hipz_qp_register_rpage() "
-					 "h_ret=%lli", h_ret);
-				ret = ehca2ib_return_code(h_ret);
-				goto init_qp_queue1;
-			}
-		}
-	}
-
-	ipz_qeit_reset(queue);
-
-	return 0;
-
-init_qp_queue1:
-	ipz_queue_dtor(pd, queue);
-	return ret;
-}
-
-static inline int ehca_calc_wqe_size(int act_nr_sge, int is_llqp)
-{
-	if (is_llqp)
-		return 128 << act_nr_sge;
-	else
-		return offsetof(struct ehca_wqe,
-				u.nud.sg_list[act_nr_sge]);
-}
-
-static void ehca_determine_small_queue(struct ehca_alloc_queue_parms *queue,
-				       int req_nr_sge, int is_llqp)
-{
-	u32 wqe_size, q_size;
-	int act_nr_sge = req_nr_sge;
-
-	if (!is_llqp)
-		/* round up #SGEs so WQE size is a power of 2 */
-		for (act_nr_sge = 4; act_nr_sge <= 252;
-		     act_nr_sge = 4 + 2 * act_nr_sge)
-			if (act_nr_sge >= req_nr_sge)
-				break;
-
-	wqe_size = ehca_calc_wqe_size(act_nr_sge, is_llqp);
-	q_size = wqe_size * (queue->max_wr + 1);
-
-	if (q_size <= 512)
-		queue->page_size = 2;
-	else if (q_size <= 1024)
-		queue->page_size = 3;
-	else
-		queue->page_size = 0;
-
-	queue->is_small = (queue->page_size != 0);
-}
-
-/* needs to be called with cq->spinlock held */
-void ehca_add_to_err_list(struct ehca_qp *qp, int on_sq)
-{
-	struct list_head *list, *node;
-
-	/* TODO: support low latency QPs */
-	if (qp->ext_type == EQPT_LLQP)
-		return;
-
-	if (on_sq) {
-		list = &qp->send_cq->sqp_err_list;
-		node = &qp->sq_err_node;
-	} else {
-		list = &qp->recv_cq->rqp_err_list;
-		node = &qp->rq_err_node;
-	}
-
-	if (list_empty(node))
-		list_add_tail(node, list);
-
-	return;
-}
-
-static void del_from_err_list(struct ehca_cq *cq, struct list_head *node)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&cq->spinlock, flags);
-
-	if (!list_empty(node))
-		list_del_init(node);
-
-	spin_unlock_irqrestore(&cq->spinlock, flags);
-}
-
-static void reset_queue_map(struct ehca_queue_map *qmap)
-{
-	int i;
-
-	qmap->tail = qmap->entries - 1;
-	qmap->left_to_poll = 0;
-	qmap->next_wqe_idx = 0;
-	for (i = 0; i < qmap->entries; i++) {
-		qmap->map[i].reported = 1;
-		qmap->map[i].cqe_req = 0;
-	}
-}
-
-/*
- * Create an ib_qp struct that is either a QP or an SRQ, depending on
- * the value of the is_srq parameter. If init_attr and srq_init_attr share
- * fields, the field out of init_attr is used.
- */
-static struct ehca_qp *internal_create_qp(
-	struct ib_pd *pd,
-	struct ib_qp_init_attr *init_attr,
-	struct ib_srq_init_attr *srq_init_attr,
-	struct ib_udata *udata, int is_srq)
-{
-	struct ehca_qp *my_qp, *my_srq = NULL;
-	struct ehca_pd *my_pd = container_of(pd, struct ehca_pd, ib_pd);
-	struct ehca_shca *shca = container_of(pd->device, struct ehca_shca,
-					      ib_device);
-	struct ib_ucontext *context = NULL;
-	u64 h_ret;
-	int is_llqp = 0, has_srq = 0, is_user = 0;
-	int qp_type, max_send_sge, max_recv_sge, ret;
-
-	/* h_call's out parameters */
-	struct ehca_alloc_qp_parms parms;
-	u32 swqe_size = 0, rwqe_size = 0, ib_qp_num;
-	unsigned long flags;
-
-	if (!atomic_add_unless(&shca->num_qps, 1, shca->max_num_qps)) {
-		ehca_err(pd->device, "Unable to create QP, max number of %i "
-			 "QPs reached.", shca->max_num_qps);
-		ehca_err(pd->device, "To increase the maximum number of QPs "
-			 "use the number_of_qps module parameter.\n");
-		return ERR_PTR(-ENOSPC);
-	}
-
-	if (init_attr->create_flags) {
-		atomic_dec(&shca->num_qps);
-		return ERR_PTR(-EINVAL);
-	}
-
-	memset(&parms, 0, sizeof(parms));
-	qp_type = init_attr->qp_type;
-
-	if (init_attr->sq_sig_type != IB_SIGNAL_REQ_WR &&
-		init_attr->sq_sig_type != IB_SIGNAL_ALL_WR) {
-		ehca_err(pd->device, "init_attr->sg_sig_type=%x not allowed",
-			 init_attr->sq_sig_type);
-		atomic_dec(&shca->num_qps);
-		return ERR_PTR(-EINVAL);
-	}
-
-	/* save LLQP info */
-	if (qp_type & 0x80) {
-		is_llqp = 1;
-		parms.ext_type = EQPT_LLQP;
-		parms.ll_comp_flags = qp_type & LLQP_COMP_MASK;
-	}
-	qp_type &= 0x1F;
-	init_attr->qp_type &= 0x1F;
-
-	/* handle SRQ base QPs */
-	if (init_attr->srq) {
-		my_srq = container_of(init_attr->srq, struct ehca_qp, ib_srq);
-
-		if (qp_type == IB_QPT_UC) {
-			ehca_err(pd->device, "UC with SRQ not supported");
-			atomic_dec(&shca->num_qps);
-			return ERR_PTR(-EINVAL);
-		}
-
-		has_srq = 1;
-		parms.ext_type = EQPT_SRQBASE;
-		parms.srq_qpn = my_srq->real_qp_num;
-	}
-
-	if (is_llqp && has_srq) {
-		ehca_err(pd->device, "LLQPs can't have an SRQ");
-		atomic_dec(&shca->num_qps);
-		return ERR_PTR(-EINVAL);
-	}
-
-	/* handle SRQs */
-	if (is_srq) {
-		parms.ext_type = EQPT_SRQ;
-		parms.srq_limit = srq_init_attr->attr.srq_limit;
-		if (init_attr->cap.max_recv_sge > 3) {
-			ehca_err(pd->device, "no more than three SGEs "
-				 "supported for SRQ  pd=%p  max_sge=%x",
-				 pd, init_attr->cap.max_recv_sge);
-			atomic_dec(&shca->num_qps);
-			return ERR_PTR(-EINVAL);
-		}
-	}
-
-	/* check QP type */
-	if (qp_type != IB_QPT_UD &&
-	    qp_type != IB_QPT_UC &&
-	    qp_type != IB_QPT_RC &&
-	    qp_type != IB_QPT_SMI &&
-	    qp_type != IB_QPT_GSI) {
-		ehca_err(pd->device, "wrong QP Type=%x", qp_type);
-		atomic_dec(&shca->num_qps);
-		return ERR_PTR(-EINVAL);
-	}
-
-	if (is_llqp) {
-		switch (qp_type) {
-		case IB_QPT_RC:
-			if ((init_attr->cap.max_send_wr > 255) ||
-			    (init_attr->cap.max_recv_wr > 255)) {
-				ehca_err(pd->device,
-					 "Invalid Number of max_sq_wr=%x "
-					 "or max_rq_wr=%x for RC LLQP",
-					 init_attr->cap.max_send_wr,
-					 init_attr->cap.max_recv_wr);
-				atomic_dec(&shca->num_qps);
-				return ERR_PTR(-EINVAL);
-			}
-			break;
-		case IB_QPT_UD:
-			if (!EHCA_BMASK_GET(HCA_CAP_UD_LL_QP, shca->hca_cap)) {
-				ehca_err(pd->device, "UD LLQP not supported "
-					 "by this adapter");
-				atomic_dec(&shca->num_qps);
-				return ERR_PTR(-ENOSYS);
-			}
-			if (!(init_attr->cap.max_send_sge <= 5
-			    && init_attr->cap.max_send_sge >= 1
-			    && init_attr->cap.max_recv_sge <= 5
-			    && init_attr->cap.max_recv_sge >= 1)) {
-				ehca_err(pd->device,
-					 "Invalid Number of max_send_sge=%x "
-					 "or max_recv_sge=%x for UD LLQP",
-					 init_attr->cap.max_send_sge,
-					 init_attr->cap.max_recv_sge);
-				atomic_dec(&shca->num_qps);
-				return ERR_PTR(-EINVAL);
-			} else if (init_attr->cap.max_send_wr > 255) {
-				ehca_err(pd->device,
-					 "Invalid Number of "
-					 "max_send_wr=%x for UD QP_TYPE=%x",
-					 init_attr->cap.max_send_wr, qp_type);
-				atomic_dec(&shca->num_qps);
-				return ERR_PTR(-EINVAL);
-			}
-			break;
-		default:
-			ehca_err(pd->device, "unsupported LL QP Type=%x",
-				 qp_type);
-			atomic_dec(&shca->num_qps);
-			return ERR_PTR(-EINVAL);
-		}
-	} else {
-		int max_sge = (qp_type == IB_QPT_UD || qp_type == IB_QPT_SMI
-			       || qp_type == IB_QPT_GSI) ? 250 : 252;
-
-		if (init_attr->cap.max_send_sge > max_sge
-		    || init_attr->cap.max_recv_sge > max_sge) {
-			ehca_err(pd->device, "Invalid number of SGEs requested "
-				 "send_sge=%x recv_sge=%x max_sge=%x",
-				 init_attr->cap.max_send_sge,
-				 init_attr->cap.max_recv_sge, max_sge);
-			atomic_dec(&shca->num_qps);
-			return ERR_PTR(-EINVAL);
-		}
-	}
-
-	my_qp = kmem_cache_zalloc(qp_cache, GFP_KERNEL);
-	if (!my_qp) {
-		ehca_err(pd->device, "pd=%p not enough memory to alloc qp", pd);
-		atomic_dec(&shca->num_qps);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	if (pd->uobject && udata) {
-		is_user = 1;
-		context = pd->uobject->context;
-	}
-
-	atomic_set(&my_qp->nr_events, 0);
-	init_waitqueue_head(&my_qp->wait_completion);
-	spin_lock_init(&my_qp->spinlock_s);
-	spin_lock_init(&my_qp->spinlock_r);
-	my_qp->qp_type = qp_type;
-	my_qp->ext_type = parms.ext_type;
-	my_qp->state = IB_QPS_RESET;
-
-	if (init_attr->recv_cq)
-		my_qp->recv_cq =
-			container_of(init_attr->recv_cq, struct ehca_cq, ib_cq);
-	if (init_attr->send_cq)
-		my_qp->send_cq =
-			container_of(init_attr->send_cq, struct ehca_cq, ib_cq);
-
-	idr_preload(GFP_KERNEL);
-	write_lock_irqsave(&ehca_qp_idr_lock, flags);
-
-	ret = idr_alloc(&ehca_qp_idr, my_qp, 0, 0x2000000, GFP_NOWAIT);
-	if (ret >= 0)
-		my_qp->token = ret;
-
-	write_unlock_irqrestore(&ehca_qp_idr_lock, flags);
-	idr_preload_end();
-	if (ret < 0) {
-		if (ret == -ENOSPC) {
-			ret = -EINVAL;
-			ehca_err(pd->device, "Invalid number of qp");
-		} else {
-			ret = -ENOMEM;
-			ehca_err(pd->device, "Can't allocate new idr entry.");
-		}
-		goto create_qp_exit0;
-	}
-
-	if (has_srq)
-		parms.srq_token = my_qp->token;
-
-	parms.servicetype = ibqptype2servicetype(qp_type);
-	if (parms.servicetype < 0) {
-		ret = -EINVAL;
-		ehca_err(pd->device, "Invalid qp_type=%x", qp_type);
-		goto create_qp_exit1;
-	}
-
-	/* Always signal by WQE so we can hide circ. WQEs */
-	parms.sigtype = HCALL_SIGT_BY_WQE;
-
-	/* UD_AV CIRCUMVENTION */
-	max_send_sge = init_attr->cap.max_send_sge;
-	max_recv_sge = init_attr->cap.max_recv_sge;
-	if (parms.servicetype == ST_UD && !is_llqp) {
-		max_send_sge += 2;
-		max_recv_sge += 2;
-	}
-
-	parms.token = my_qp->token;
-	parms.eq_handle = shca->eq.ipz_eq_handle;
-	parms.pd = my_pd->fw_pd;
-	if (my_qp->send_cq)
-		parms.send_cq_handle = my_qp->send_cq->ipz_cq_handle;
-	if (my_qp->recv_cq)
-		parms.recv_cq_handle = my_qp->recv_cq->ipz_cq_handle;
-
-	parms.squeue.max_wr = init_attr->cap.max_send_wr;
-	parms.rqueue.max_wr = init_attr->cap.max_recv_wr;
-	parms.squeue.max_sge = max_send_sge;
-	parms.rqueue.max_sge = max_recv_sge;
-
-	/* RC QPs need one more SWQE for unsolicited ack circumvention */
-	if (qp_type == IB_QPT_RC)
-		parms.squeue.max_wr++;
-
-	if (EHCA_BMASK_GET(HCA_CAP_MINI_QP, shca->hca_cap)) {
-		if (HAS_SQ(my_qp))
-			ehca_determine_small_queue(
-				&parms.squeue, max_send_sge, is_llqp);
-		if (HAS_RQ(my_qp))
-			ehca_determine_small_queue(
-				&parms.rqueue, max_recv_sge, is_llqp);
-		parms.qp_storage =
-			(parms.squeue.is_small || parms.rqueue.is_small);
-	}
-
-	h_ret = hipz_h_alloc_resource_qp(shca->ipz_hca_handle, &parms, is_user);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(pd->device, "h_alloc_resource_qp() failed h_ret=%lli",
-			 h_ret);
-		ret = ehca2ib_return_code(h_ret);
-		goto create_qp_exit1;
-	}
-
-	ib_qp_num = my_qp->real_qp_num = parms.real_qp_num;
-	my_qp->ipz_qp_handle = parms.qp_handle;
-	my_qp->galpas = parms.galpas;
-
-	swqe_size = ehca_calc_wqe_size(parms.squeue.act_nr_sges, is_llqp);
-	rwqe_size = ehca_calc_wqe_size(parms.rqueue.act_nr_sges, is_llqp);
-
-	switch (qp_type) {
-	case IB_QPT_RC:
-		if (is_llqp) {
-			parms.squeue.act_nr_sges = 1;
-			parms.rqueue.act_nr_sges = 1;
-		}
-		/* hide the extra WQE */
-		parms.squeue.act_nr_wqes--;
-		break;
-	case IB_QPT_UD:
-	case IB_QPT_GSI:
-	case IB_QPT_SMI:
-		/* UD circumvention */
-		if (is_llqp) {
-			parms.squeue.act_nr_sges = 1;
-			parms.rqueue.act_nr_sges = 1;
-		} else {
-			parms.squeue.act_nr_sges -= 2;
-			parms.rqueue.act_nr_sges -= 2;
-		}
-
-		if (IB_QPT_GSI == qp_type || IB_QPT_SMI == qp_type) {
-			parms.squeue.act_nr_wqes = init_attr->cap.max_send_wr;
-			parms.rqueue.act_nr_wqes = init_attr->cap.max_recv_wr;
-			parms.squeue.act_nr_sges = init_attr->cap.max_send_sge;
-			parms.rqueue.act_nr_sges = init_attr->cap.max_recv_sge;
-			ib_qp_num = (qp_type == IB_QPT_SMI) ? 0 : 1;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	/* initialize r/squeue and register queue pages */
-	if (HAS_SQ(my_qp)) {
-		ret = init_qp_queue(
-			shca, my_pd, my_qp, &my_qp->ipz_squeue, 0,
-			HAS_RQ(my_qp) ? H_PAGE_REGISTERED : H_SUCCESS,
-			&parms.squeue, swqe_size);
-		if (ret) {
-			ehca_err(pd->device, "Couldn't initialize squeue "
-				 "and pages ret=%i", ret);
-			goto create_qp_exit2;
-		}
-
-		if (!is_user) {
-			my_qp->sq_map.entries = my_qp->ipz_squeue.queue_length /
-				my_qp->ipz_squeue.qe_size;
-			my_qp->sq_map.map = vmalloc(my_qp->sq_map.entries *
-						    sizeof(struct ehca_qmap_entry));
-			if (!my_qp->sq_map.map) {
-				ehca_err(pd->device, "Couldn't allocate squeue "
-					 "map ret=%i", ret);
-				goto create_qp_exit3;
-			}
-			INIT_LIST_HEAD(&my_qp->sq_err_node);
-			/* to avoid the generation of bogus flush CQEs */
-			reset_queue_map(&my_qp->sq_map);
-		}
-	}
-
-	if (HAS_RQ(my_qp)) {
-		ret = init_qp_queue(
-			shca, my_pd, my_qp, &my_qp->ipz_rqueue, 1,
-			H_SUCCESS, &parms.rqueue, rwqe_size);
-		if (ret) {
-			ehca_err(pd->device, "Couldn't initialize rqueue "
-				 "and pages ret=%i", ret);
-			goto create_qp_exit4;
-		}
-		if (!is_user) {
-			my_qp->rq_map.entries = my_qp->ipz_rqueue.queue_length /
-				my_qp->ipz_rqueue.qe_size;
-			my_qp->rq_map.map = vmalloc(my_qp->rq_map.entries *
-						    sizeof(struct ehca_qmap_entry));
-			if (!my_qp->rq_map.map) {
-				ehca_err(pd->device, "Couldn't allocate squeue "
-					 "map ret=%i", ret);
-				goto create_qp_exit5;
-			}
-			INIT_LIST_HEAD(&my_qp->rq_err_node);
-			/* to avoid the generation of bogus flush CQEs */
-			reset_queue_map(&my_qp->rq_map);
-		}
-	} else if (init_attr->srq && !is_user) {
-		/* this is a base QP, use the queue map of the SRQ */
-		my_qp->rq_map = my_srq->rq_map;
-		INIT_LIST_HEAD(&my_qp->rq_err_node);
-
-		my_qp->ipz_rqueue = my_srq->ipz_rqueue;
-	}
-
-	if (is_srq) {
-		my_qp->ib_srq.pd = &my_pd->ib_pd;
-		my_qp->ib_srq.device = my_pd->ib_pd.device;
-
-		my_qp->ib_srq.srq_context = init_attr->qp_context;
-		my_qp->ib_srq.event_handler = init_attr->event_handler;
-	} else {
-		my_qp->ib_qp.qp_num = ib_qp_num;
-		my_qp->ib_qp.pd = &my_pd->ib_pd;
-		my_qp->ib_qp.device = my_pd->ib_pd.device;
-
-		my_qp->ib_qp.recv_cq = init_attr->recv_cq;
-		my_qp->ib_qp.send_cq = init_attr->send_cq;
-
-		my_qp->ib_qp.qp_type = qp_type;
-		my_qp->ib_qp.srq = init_attr->srq;
-
-		my_qp->ib_qp.qp_context = init_attr->qp_context;
-		my_qp->ib_qp.event_handler = init_attr->event_handler;
-	}
-
-	init_attr->cap.max_inline_data = 0; /* not supported yet */
-	init_attr->cap.max_recv_sge = parms.rqueue.act_nr_sges;
-	init_attr->cap.max_recv_wr = parms.rqueue.act_nr_wqes;
-	init_attr->cap.max_send_sge = parms.squeue.act_nr_sges;
-	init_attr->cap.max_send_wr = parms.squeue.act_nr_wqes;
-	my_qp->init_attr = *init_attr;
-
-	if (qp_type == IB_QPT_SMI || qp_type == IB_QPT_GSI) {
-		shca->sport[init_attr->port_num - 1].ibqp_sqp[qp_type] =
-			&my_qp->ib_qp;
-		if (ehca_nr_ports < 0) {
-			/* alloc array to cache subsequent modify qp parms
-			 * for autodetect mode
-			 */
-			my_qp->mod_qp_parm =
-				kzalloc(EHCA_MOD_QP_PARM_MAX *
-					sizeof(*my_qp->mod_qp_parm),
-					GFP_KERNEL);
-			if (!my_qp->mod_qp_parm) {
-				ehca_err(pd->device,
-					 "Could not alloc mod_qp_parm");
-				goto create_qp_exit5;
-			}
-		}
-	}
-
-	/* NOTE: define_apq0() not supported yet */
-	if (qp_type == IB_QPT_GSI) {
-		h_ret = ehca_define_sqp(shca, my_qp, init_attr);
-		if (h_ret != H_SUCCESS) {
-			kfree(my_qp->mod_qp_parm);
-			my_qp->mod_qp_parm = NULL;
-			/* the QP pointer is no longer valid */
-			shca->sport[init_attr->port_num - 1].ibqp_sqp[qp_type] =
-				NULL;
-			ret = ehca2ib_return_code(h_ret);
-			goto create_qp_exit6;
-		}
-	}
-
-	if (my_qp->send_cq) {
-		ret = ehca_cq_assign_qp(my_qp->send_cq, my_qp);
-		if (ret) {
-			ehca_err(pd->device,
-				 "Couldn't assign qp to send_cq ret=%i", ret);
-			goto create_qp_exit7;
-		}
-	}
-
-	/* copy queues, galpa data to user space */
-	if (context && udata) {
-		struct ehca_create_qp_resp resp;
-		memset(&resp, 0, sizeof(resp));
-
-		resp.qp_num = my_qp->real_qp_num;
-		resp.token = my_qp->token;
-		resp.qp_type = my_qp->qp_type;
-		resp.ext_type = my_qp->ext_type;
-		resp.qkey = my_qp->qkey;
-		resp.real_qp_num = my_qp->real_qp_num;
-
-		if (HAS_SQ(my_qp))
-			queue2resp(&resp.ipz_squeue, &my_qp->ipz_squeue);
-		if (HAS_RQ(my_qp))
-			queue2resp(&resp.ipz_rqueue, &my_qp->ipz_rqueue);
-		resp.fw_handle_ofs = (u32)
-			(my_qp->galpas.user.fw_handle & (PAGE_SIZE - 1));
-
-		if (ib_copy_to_udata(udata, &resp, sizeof resp)) {
-			ehca_err(pd->device, "Copy to udata failed");
-			ret = -EINVAL;
-			goto create_qp_exit8;
-		}
-	}
-
-	return my_qp;
-
-create_qp_exit8:
-	ehca_cq_unassign_qp(my_qp->send_cq, my_qp->real_qp_num);
-
-create_qp_exit7:
-	kfree(my_qp->mod_qp_parm);
-
-create_qp_exit6:
-	if (HAS_RQ(my_qp) && !is_user)
-		vfree(my_qp->rq_map.map);
-
-create_qp_exit5:
-	if (HAS_RQ(my_qp))
-		ipz_queue_dtor(my_pd, &my_qp->ipz_rqueue);
-
-create_qp_exit4:
-	if (HAS_SQ(my_qp) && !is_user)
-		vfree(my_qp->sq_map.map);
-
-create_qp_exit3:
-	if (HAS_SQ(my_qp))
-		ipz_queue_dtor(my_pd, &my_qp->ipz_squeue);
-
-create_qp_exit2:
-	hipz_h_destroy_qp(shca->ipz_hca_handle, my_qp);
-
-create_qp_exit1:
-	write_lock_irqsave(&ehca_qp_idr_lock, flags);
-	idr_remove(&ehca_qp_idr, my_qp->token);
-	write_unlock_irqrestore(&ehca_qp_idr_lock, flags);
-
-create_qp_exit0:
-	kmem_cache_free(qp_cache, my_qp);
-	atomic_dec(&shca->num_qps);
-	return ERR_PTR(ret);
-}
-
-struct ib_qp *ehca_create_qp(struct ib_pd *pd,
-			     struct ib_qp_init_attr *qp_init_attr,
-			     struct ib_udata *udata)
-{
-	struct ehca_qp *ret;
-
-	ret = internal_create_qp(pd, qp_init_attr, NULL, udata, 0);
-	return IS_ERR(ret) ? (struct ib_qp *)ret : &ret->ib_qp;
-}
-
-static int internal_destroy_qp(struct ib_device *dev, struct ehca_qp *my_qp,
-			       struct ib_uobject *uobject);
-
-struct ib_srq *ehca_create_srq(struct ib_pd *pd,
-			       struct ib_srq_init_attr *srq_init_attr,
-			       struct ib_udata *udata)
-{
-	struct ib_qp_init_attr qp_init_attr;
-	struct ehca_qp *my_qp;
-	struct ib_srq *ret;
-	struct ehca_shca *shca = container_of(pd->device, struct ehca_shca,
-					      ib_device);
-	struct hcp_modify_qp_control_block *mqpcb;
-	u64 hret, update_mask;
-
-	if (srq_init_attr->srq_type != IB_SRQT_BASIC)
-		return ERR_PTR(-ENOSYS);
-
-	/* For common attributes, internal_create_qp() takes its info
-	 * out of qp_init_attr, so copy all common attrs there.
-	 */
-	memset(&qp_init_attr, 0, sizeof(qp_init_attr));
-	qp_init_attr.event_handler = srq_init_attr->event_handler;
-	qp_init_attr.qp_context = srq_init_attr->srq_context;
-	qp_init_attr.sq_sig_type = IB_SIGNAL_ALL_WR;
-	qp_init_attr.qp_type = IB_QPT_RC;
-	qp_init_attr.cap.max_recv_wr = srq_init_attr->attr.max_wr;
-	qp_init_attr.cap.max_recv_sge = srq_init_attr->attr.max_sge;
-
-	my_qp = internal_create_qp(pd, &qp_init_attr, srq_init_attr, udata, 1);
-	if (IS_ERR(my_qp))
-		return (struct ib_srq *)my_qp;
-
-	/* copy back return values */
-	srq_init_attr->attr.max_wr = qp_init_attr.cap.max_recv_wr;
-	srq_init_attr->attr.max_sge = 3;
-
-	/* drive SRQ into RTR state */
-	mqpcb = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!mqpcb) {
-		ehca_err(pd->device, "Could not get zeroed page for mqpcb "
-			 "ehca_qp=%p qp_num=%x ", my_qp, my_qp->real_qp_num);
-		ret = ERR_PTR(-ENOMEM);
-		goto create_srq1;
-	}
-
-	mqpcb->qp_state = EHCA_QPS_INIT;
-	mqpcb->prim_phys_port = 1;
-	update_mask = EHCA_BMASK_SET(MQPCB_MASK_QP_STATE, 1);
-	hret = hipz_h_modify_qp(shca->ipz_hca_handle,
-				my_qp->ipz_qp_handle,
-				&my_qp->pf,
-				update_mask,
-				mqpcb, my_qp->galpas.kernel);
-	if (hret != H_SUCCESS) {
-		ehca_err(pd->device, "Could not modify SRQ to INIT "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, my_qp->real_qp_num, hret);
-		goto create_srq2;
-	}
-
-	mqpcb->qp_enable = 1;
-	update_mask = EHCA_BMASK_SET(MQPCB_MASK_QP_ENABLE, 1);
-	hret = hipz_h_modify_qp(shca->ipz_hca_handle,
-				my_qp->ipz_qp_handle,
-				&my_qp->pf,
-				update_mask,
-				mqpcb, my_qp->galpas.kernel);
-	if (hret != H_SUCCESS) {
-		ehca_err(pd->device, "Could not enable SRQ "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, my_qp->real_qp_num, hret);
-		goto create_srq2;
-	}
-
-	mqpcb->qp_state  = EHCA_QPS_RTR;
-	update_mask = EHCA_BMASK_SET(MQPCB_MASK_QP_STATE, 1);
-	hret = hipz_h_modify_qp(shca->ipz_hca_handle,
-				my_qp->ipz_qp_handle,
-				&my_qp->pf,
-				update_mask,
-				mqpcb, my_qp->galpas.kernel);
-	if (hret != H_SUCCESS) {
-		ehca_err(pd->device, "Could not modify SRQ to RTR "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, my_qp->real_qp_num, hret);
-		goto create_srq2;
-	}
-
-	ehca_free_fw_ctrlblock(mqpcb);
-
-	return &my_qp->ib_srq;
-
-create_srq2:
-	ret = ERR_PTR(ehca2ib_return_code(hret));
-	ehca_free_fw_ctrlblock(mqpcb);
-
-create_srq1:
-	internal_destroy_qp(pd->device, my_qp, my_qp->ib_srq.uobject);
-
-	return ret;
-}
-
-/*
- * prepare_sqe_rts called by internal_modify_qp() at trans sqe -> rts
- * set purge bit of bad wqe and subsequent wqes to avoid reentering sqe
- * returns total number of bad wqes in bad_wqe_cnt
- */
-static int prepare_sqe_rts(struct ehca_qp *my_qp, struct ehca_shca *shca,
-			   int *bad_wqe_cnt)
-{
-	u64 h_ret;
-	struct ipz_queue *squeue;
-	void *bad_send_wqe_p, *bad_send_wqe_v;
-	u64 q_ofs;
-	struct ehca_wqe *wqe;
-	int qp_num = my_qp->ib_qp.qp_num;
-
-	/* get send wqe pointer */
-	h_ret = hipz_h_disable_and_get_wqe(shca->ipz_hca_handle,
-					   my_qp->ipz_qp_handle, &my_qp->pf,
-					   &bad_send_wqe_p, NULL, 2);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(&shca->ib_device, "hipz_h_disable_and_get_wqe() failed"
-			 " ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, qp_num, h_ret);
-		return ehca2ib_return_code(h_ret);
-	}
-	bad_send_wqe_p = (void *)((u64)bad_send_wqe_p & (~(1L << 63)));
-	ehca_dbg(&shca->ib_device, "qp_num=%x bad_send_wqe_p=%p",
-		 qp_num, bad_send_wqe_p);
-	/* convert wqe pointer to vadr */
-	bad_send_wqe_v = __va((u64)bad_send_wqe_p);
-	if (ehca_debug_level >= 2)
-		ehca_dmp(bad_send_wqe_v, 32, "qp_num=%x bad_wqe", qp_num);
-	squeue = &my_qp->ipz_squeue;
-	if (ipz_queue_abs_to_offset(squeue, (u64)bad_send_wqe_p, &q_ofs)) {
-		ehca_err(&shca->ib_device, "failed to get wqe offset qp_num=%x"
-			 " bad_send_wqe_p=%p", qp_num, bad_send_wqe_p);
-		return -EFAULT;
-	}
-
-	/* loop sets wqe's purge bit */
-	wqe = (struct ehca_wqe *)ipz_qeit_calc(squeue, q_ofs);
-	*bad_wqe_cnt = 0;
-	while (wqe->optype != 0xff && wqe->wqef != 0xff) {
-		if (ehca_debug_level >= 2)
-			ehca_dmp(wqe, 32, "qp_num=%x wqe", qp_num);
-		wqe->nr_of_data_seg = 0; /* suppress data access */
-		wqe->wqef = WQEF_PURGE; /* WQE to be purged */
-		q_ofs = ipz_queue_advance_offset(squeue, q_ofs);
-		wqe = (struct ehca_wqe *)ipz_qeit_calc(squeue, q_ofs);
-		*bad_wqe_cnt = (*bad_wqe_cnt)+1;
-	}
-	/*
-	 * bad wqe will be reprocessed and ignored when pol_cq() is called,
-	 *  i.e. nr of wqes with flush error status is one less
-	 */
-	ehca_dbg(&shca->ib_device, "qp_num=%x flusherr_wqe_cnt=%x",
-		 qp_num, (*bad_wqe_cnt)-1);
-	wqe->wqef = 0;
-
-	return 0;
-}
-
-static int calc_left_cqes(u64 wqe_p, struct ipz_queue *ipz_queue,
-			  struct ehca_queue_map *qmap)
-{
-	void *wqe_v;
-	u64 q_ofs;
-	u32 wqe_idx;
-	unsigned int tail_idx;
-
-	/* convert real to abs address */
-	wqe_p = wqe_p & (~(1UL << 63));
-
-	wqe_v = __va(wqe_p);
-
-	if (ipz_queue_abs_to_offset(ipz_queue, wqe_p, &q_ofs)) {
-		ehca_gen_err("Invalid offset for calculating left cqes "
-				"wqe_p=%#llx wqe_v=%p\n", wqe_p, wqe_v);
-		return -EFAULT;
-	}
-
-	tail_idx = next_index(qmap->tail, qmap->entries);
-	wqe_idx = q_ofs / ipz_queue->qe_size;
-
-	/* check all processed wqes, whether a cqe is requested or not */
-	while (tail_idx != wqe_idx) {
-		if (qmap->map[tail_idx].cqe_req)
-			qmap->left_to_poll++;
-		tail_idx = next_index(tail_idx, qmap->entries);
-	}
-	/* save index in queue, where we have to start flushing */
-	qmap->next_wqe_idx = wqe_idx;
-	return 0;
-}
-
-static int check_for_left_cqes(struct ehca_qp *my_qp, struct ehca_shca *shca)
-{
-	u64 h_ret;
-	void *send_wqe_p, *recv_wqe_p;
-	int ret;
-	unsigned long flags;
-	int qp_num = my_qp->ib_qp.qp_num;
-
-	/* this hcall is not supported on base QPs */
-	if (my_qp->ext_type != EQPT_SRQBASE) {
-		/* get send and receive wqe pointer */
-		h_ret = hipz_h_disable_and_get_wqe(shca->ipz_hca_handle,
-				my_qp->ipz_qp_handle, &my_qp->pf,
-				&send_wqe_p, &recv_wqe_p, 4);
-		if (h_ret != H_SUCCESS) {
-			ehca_err(&shca->ib_device, "disable_and_get_wqe() "
-				 "failed ehca_qp=%p qp_num=%x h_ret=%lli",
-				 my_qp, qp_num, h_ret);
-			return ehca2ib_return_code(h_ret);
-		}
-
-		/*
-		 * acquire lock to ensure that nobody is polling the cq which
-		 * could mean that the qmap->tail pointer is in an
-		 * inconsistent state.
-		 */
-		spin_lock_irqsave(&my_qp->send_cq->spinlock, flags);
-		ret = calc_left_cqes((u64)send_wqe_p, &my_qp->ipz_squeue,
-				&my_qp->sq_map);
-		spin_unlock_irqrestore(&my_qp->send_cq->spinlock, flags);
-		if (ret)
-			return ret;
-
-
-		spin_lock_irqsave(&my_qp->recv_cq->spinlock, flags);
-		ret = calc_left_cqes((u64)recv_wqe_p, &my_qp->ipz_rqueue,
-				&my_qp->rq_map);
-		spin_unlock_irqrestore(&my_qp->recv_cq->spinlock, flags);
-		if (ret)
-			return ret;
-	} else {
-		spin_lock_irqsave(&my_qp->send_cq->spinlock, flags);
-		my_qp->sq_map.left_to_poll = 0;
-		my_qp->sq_map.next_wqe_idx = next_index(my_qp->sq_map.tail,
-							my_qp->sq_map.entries);
-		spin_unlock_irqrestore(&my_qp->send_cq->spinlock, flags);
-
-		spin_lock_irqsave(&my_qp->recv_cq->spinlock, flags);
-		my_qp->rq_map.left_to_poll = 0;
-		my_qp->rq_map.next_wqe_idx = next_index(my_qp->rq_map.tail,
-							my_qp->rq_map.entries);
-		spin_unlock_irqrestore(&my_qp->recv_cq->spinlock, flags);
-	}
-
-	/* this assures flush cqes being generated only for pending wqes */
-	if ((my_qp->sq_map.left_to_poll == 0) &&
-				(my_qp->rq_map.left_to_poll == 0)) {
-		spin_lock_irqsave(&my_qp->send_cq->spinlock, flags);
-		ehca_add_to_err_list(my_qp, 1);
-		spin_unlock_irqrestore(&my_qp->send_cq->spinlock, flags);
-
-		if (HAS_RQ(my_qp)) {
-			spin_lock_irqsave(&my_qp->recv_cq->spinlock, flags);
-			ehca_add_to_err_list(my_qp, 0);
-			spin_unlock_irqrestore(&my_qp->recv_cq->spinlock,
-					flags);
-		}
-	}
-
-	return 0;
-}
-
-/*
- * internal_modify_qp with circumvention to handle aqp0 properly
- * smi_reset2init indicates if this is an internal reset-to-init-call for
- * smi. This flag must always be zero if called from ehca_modify_qp()!
- * This internal func was intorduced to avoid recursion of ehca_modify_qp()!
- */
-static int internal_modify_qp(struct ib_qp *ibqp,
-			      struct ib_qp_attr *attr,
-			      int attr_mask, int smi_reset2init)
-{
-	enum ib_qp_state qp_cur_state, qp_new_state;
-	int cnt, qp_attr_idx, ret = 0;
-	enum ib_qp_statetrans statetrans;
-	struct hcp_modify_qp_control_block *mqpcb;
-	struct ehca_qp *my_qp = container_of(ibqp, struct ehca_qp, ib_qp);
-	struct ehca_shca *shca =
-		container_of(ibqp->pd->device, struct ehca_shca, ib_device);
-	u64 update_mask;
-	u64 h_ret;
-	int bad_wqe_cnt = 0;
-	int is_user = 0;
-	int squeue_locked = 0;
-	unsigned long flags = 0;
-
-	/* do query_qp to obtain current attr values */
-	mqpcb = ehca_alloc_fw_ctrlblock(GFP_ATOMIC);
-	if (!mqpcb) {
-		ehca_err(ibqp->device, "Could not get zeroed page for mqpcb "
-			 "ehca_qp=%p qp_num=%x ", my_qp, ibqp->qp_num);
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_qp(shca->ipz_hca_handle,
-				my_qp->ipz_qp_handle,
-				&my_qp->pf,
-				mqpcb, my_qp->galpas.kernel);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(ibqp->device, "hipz_h_query_qp() failed "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, ibqp->qp_num, h_ret);
-		ret = ehca2ib_return_code(h_ret);
-		goto modify_qp_exit1;
-	}
-	if (ibqp->uobject)
-		is_user = 1;
-
-	qp_cur_state = ehca2ib_qp_state(mqpcb->qp_state);
-
-	if (qp_cur_state == -EINVAL) {	/* invalid qp state */
-		ret = -EINVAL;
-		ehca_err(ibqp->device, "Invalid current ehca_qp_state=%x "
-			 "ehca_qp=%p qp_num=%x",
-			 mqpcb->qp_state, my_qp, ibqp->qp_num);
-		goto modify_qp_exit1;
-	}
-	/*
-	 * circumvention to set aqp0 initial state to init
-	 * as expected by IB spec
-	 */
-	if (smi_reset2init == 0 &&
-	    ibqp->qp_type == IB_QPT_SMI &&
-	    qp_cur_state == IB_QPS_RESET &&
-	    (attr_mask & IB_QP_STATE) &&
-	    attr->qp_state == IB_QPS_INIT) { /* RESET -> INIT */
-		struct ib_qp_attr smiqp_attr = {
-			.qp_state = IB_QPS_INIT,
-			.port_num = my_qp->init_attr.port_num,
-			.pkey_index = 0,
-			.qkey = 0
-		};
-		int smiqp_attr_mask = IB_QP_STATE | IB_QP_PORT |
-			IB_QP_PKEY_INDEX | IB_QP_QKEY;
-		int smirc = internal_modify_qp(
-			ibqp, &smiqp_attr, smiqp_attr_mask, 1);
-		if (smirc) {
-			ehca_err(ibqp->device, "SMI RESET -> INIT failed. "
-				 "ehca_modify_qp() rc=%i", smirc);
-			ret = H_PARAMETER;
-			goto modify_qp_exit1;
-		}
-		qp_cur_state = IB_QPS_INIT;
-		ehca_dbg(ibqp->device, "SMI RESET -> INIT succeeded");
-	}
-	/* is transmitted current state  equal to "real" current state */
-	if ((attr_mask & IB_QP_CUR_STATE) &&
-	    qp_cur_state != attr->cur_qp_state) {
-		ret = -EINVAL;
-		ehca_err(ibqp->device,
-			 "Invalid IB_QP_CUR_STATE attr->curr_qp_state=%x <>"
-			 " actual cur_qp_state=%x. ehca_qp=%p qp_num=%x",
-			 attr->cur_qp_state, qp_cur_state, my_qp, ibqp->qp_num);
-		goto modify_qp_exit1;
-	}
-
-	ehca_dbg(ibqp->device, "ehca_qp=%p qp_num=%x current qp_state=%x "
-		 "new qp_state=%x attribute_mask=%x",
-		 my_qp, ibqp->qp_num, qp_cur_state, attr->qp_state, attr_mask);
-
-	qp_new_state = attr_mask & IB_QP_STATE ? attr->qp_state : qp_cur_state;
-	if (!smi_reset2init &&
-	    !ib_modify_qp_is_ok(qp_cur_state, qp_new_state, ibqp->qp_type,
-				attr_mask, IB_LINK_LAYER_UNSPECIFIED)) {
-		ret = -EINVAL;
-		ehca_err(ibqp->device,
-			 "Invalid qp transition new_state=%x cur_state=%x "
-			 "ehca_qp=%p qp_num=%x attr_mask=%x", qp_new_state,
-			 qp_cur_state, my_qp, ibqp->qp_num, attr_mask);
-		goto modify_qp_exit1;
-	}
-
-	mqpcb->qp_state = ib2ehca_qp_state(qp_new_state);
-	if (mqpcb->qp_state)
-		update_mask = EHCA_BMASK_SET(MQPCB_MASK_QP_STATE, 1);
-	else {
-		ret = -EINVAL;
-		ehca_err(ibqp->device, "Invalid new qp state=%x "
-			 "ehca_qp=%p qp_num=%x",
-			 qp_new_state, my_qp, ibqp->qp_num);
-		goto modify_qp_exit1;
-	}
-
-	/* retrieve state transition struct to get req and opt attrs */
-	statetrans = get_modqp_statetrans(qp_cur_state, qp_new_state);
-	if (statetrans < 0) {
-		ret = -EINVAL;
-		ehca_err(ibqp->device, "<INVALID STATE CHANGE> qp_cur_state=%x "
-			 "new_qp_state=%x State_xsition=%x ehca_qp=%p "
-			 "qp_num=%x", qp_cur_state, qp_new_state,
-			 statetrans, my_qp, ibqp->qp_num);
-		goto modify_qp_exit1;
-	}
-
-	qp_attr_idx = ib2ehcaqptype(ibqp->qp_type);
-
-	if (qp_attr_idx < 0) {
-		ret = qp_attr_idx;
-		ehca_err(ibqp->device,
-			 "Invalid QP type=%x ehca_qp=%p qp_num=%x",
-			 ibqp->qp_type, my_qp, ibqp->qp_num);
-		goto modify_qp_exit1;
-	}
-
-	ehca_dbg(ibqp->device,
-		 "ehca_qp=%p qp_num=%x <VALID STATE CHANGE> qp_state_xsit=%x",
-		 my_qp, ibqp->qp_num, statetrans);
-
-	/* eHCA2 rev2 and higher require the SEND_GRH_FLAG to be set
-	 * in non-LL UD QPs.
-	 */
-	if ((my_qp->qp_type == IB_QPT_UD) &&
-	    (my_qp->ext_type != EQPT_LLQP) &&
-	    (statetrans == IB_QPST_INIT2RTR) &&
-	    (shca->hw_level >= 0x22)) {
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SEND_GRH_FLAG, 1);
-		mqpcb->send_grh_flag = 1;
-	}
-
-	/* sqe -> rts: set purge bit of bad wqe before actual trans */
-	if ((my_qp->qp_type == IB_QPT_UD ||
-	     my_qp->qp_type == IB_QPT_GSI ||
-	     my_qp->qp_type == IB_QPT_SMI) &&
-	    statetrans == IB_QPST_SQE2RTS) {
-		/* mark next free wqe if kernel */
-		if (!ibqp->uobject) {
-			struct ehca_wqe *wqe;
-			/* lock send queue */
-			spin_lock_irqsave(&my_qp->spinlock_s, flags);
-			squeue_locked = 1;
-			/* mark next free wqe */
-			wqe = (struct ehca_wqe *)
-				ipz_qeit_get(&my_qp->ipz_squeue);
-			wqe->optype = wqe->wqef = 0xff;
-			ehca_dbg(ibqp->device, "qp_num=%x next_free_wqe=%p",
-				 ibqp->qp_num, wqe);
-		}
-		ret = prepare_sqe_rts(my_qp, shca, &bad_wqe_cnt);
-		if (ret) {
-			ehca_err(ibqp->device, "prepare_sqe_rts() failed "
-				 "ehca_qp=%p qp_num=%x ret=%i",
-				 my_qp, ibqp->qp_num, ret);
-			goto modify_qp_exit2;
-		}
-	}
-
-	/*
-	 * enable RDMA_Atomic_Control if reset->init und reliable con
-	 * this is necessary since gen2 does not provide that flag,
-	 * but pHyp requires it
-	 */
-	if (statetrans == IB_QPST_RESET2INIT &&
-	    (ibqp->qp_type == IB_QPT_RC || ibqp->qp_type == IB_QPT_UC)) {
-		mqpcb->rdma_atomic_ctrl = 3;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_RDMA_ATOMIC_CTRL, 1);
-	}
-	/* circ. pHyp requires #RDMA/Atomic Resp Res for UC INIT -> RTR */
-	if (statetrans == IB_QPST_INIT2RTR &&
-	    (ibqp->qp_type == IB_QPT_UC) &&
-	    !(attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)) {
-		mqpcb->rdma_nr_atomic_resp_res = 1; /* default to 1 */
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_RDMA_NR_ATOMIC_RESP_RES, 1);
-	}
-
-	if (attr_mask & IB_QP_PKEY_INDEX) {
-		if (attr->pkey_index >= 16) {
-			ret = -EINVAL;
-			ehca_err(ibqp->device, "Invalid pkey_index=%x. "
-				 "ehca_qp=%p qp_num=%x max_pkey_index=f",
-				 attr->pkey_index, my_qp, ibqp->qp_num);
-			goto modify_qp_exit2;
-		}
-		mqpcb->prim_p_key_idx = attr->pkey_index;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_PRIM_P_KEY_IDX, 1);
-	}
-	if (attr_mask & IB_QP_PORT) {
-		struct ehca_sport *sport;
-		struct ehca_qp *aqp1;
-		if (attr->port_num < 1 || attr->port_num > shca->num_ports) {
-			ret = -EINVAL;
-			ehca_err(ibqp->device, "Invalid port=%x. "
-				 "ehca_qp=%p qp_num=%x num_ports=%x",
-				 attr->port_num, my_qp, ibqp->qp_num,
-				 shca->num_ports);
-			goto modify_qp_exit2;
-		}
-		sport = &shca->sport[attr->port_num - 1];
-		if (!sport->ibqp_sqp[IB_QPT_GSI]) {
-			/* should not occur */
-			ret = -EFAULT;
-			ehca_err(ibqp->device, "AQP1 was not created for "
-				 "port=%x", attr->port_num);
-			goto modify_qp_exit2;
-		}
-		aqp1 = container_of(sport->ibqp_sqp[IB_QPT_GSI],
-				    struct ehca_qp, ib_qp);
-		if (ibqp->qp_type != IB_QPT_GSI &&
-		    ibqp->qp_type != IB_QPT_SMI &&
-		    aqp1->mod_qp_parm) {
-			/*
-			 * firmware will reject this modify_qp() because
-			 * port is not activated/initialized fully
-			 */
-			ret = -EFAULT;
-			ehca_warn(ibqp->device, "Couldn't modify qp port=%x: "
-				  "either port is being activated (try again) "
-				  "or cabling issue", attr->port_num);
-			goto modify_qp_exit2;
-		}
-		mqpcb->prim_phys_port = attr->port_num;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_PRIM_PHYS_PORT, 1);
-	}
-	if (attr_mask & IB_QP_QKEY) {
-		mqpcb->qkey = attr->qkey;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_QKEY, 1);
-	}
-	if (attr_mask & IB_QP_AV) {
-		mqpcb->dlid = attr->ah_attr.dlid;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_DLID, 1);
-		mqpcb->source_path_bits = attr->ah_attr.src_path_bits;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SOURCE_PATH_BITS, 1);
-		mqpcb->service_level = attr->ah_attr.sl;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SERVICE_LEVEL, 1);
-
-		if (ehca_calc_ipd(shca, mqpcb->prim_phys_port,
-				  attr->ah_attr.static_rate,
-				  &mqpcb->max_static_rate)) {
-			ret = -EINVAL;
-			goto modify_qp_exit2;
-		}
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_MAX_STATIC_RATE, 1);
-
-		/*
-		 * Always supply the GRH flag, even if it's zero, to give the
-		 * hypervisor a clear "yes" or "no" instead of a "perhaps"
-		 */
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SEND_GRH_FLAG, 1);
-
-		/*
-		 * only if GRH is TRUE we might consider SOURCE_GID_IDX
-		 * and DEST_GID otherwise phype will return H_ATTR_PARM!!!
-		 */
-		if (attr->ah_attr.ah_flags == IB_AH_GRH) {
-			mqpcb->send_grh_flag = 1;
-
-			mqpcb->source_gid_idx = attr->ah_attr.grh.sgid_index;
-			update_mask |=
-				EHCA_BMASK_SET(MQPCB_MASK_SOURCE_GID_IDX, 1);
-
-			for (cnt = 0; cnt < 16; cnt++)
-				mqpcb->dest_gid.byte[cnt] =
-					attr->ah_attr.grh.dgid.raw[cnt];
-
-			update_mask |= EHCA_BMASK_SET(MQPCB_MASK_DEST_GID, 1);
-			mqpcb->flow_label = attr->ah_attr.grh.flow_label;
-			update_mask |= EHCA_BMASK_SET(MQPCB_MASK_FLOW_LABEL, 1);
-			mqpcb->hop_limit = attr->ah_attr.grh.hop_limit;
-			update_mask |= EHCA_BMASK_SET(MQPCB_MASK_HOP_LIMIT, 1);
-			mqpcb->traffic_class = attr->ah_attr.grh.traffic_class;
-			update_mask |=
-				EHCA_BMASK_SET(MQPCB_MASK_TRAFFIC_CLASS, 1);
-		}
-	}
-
-	if (attr_mask & IB_QP_PATH_MTU) {
-		/* store ld(MTU) */
-		my_qp->mtu_shift = attr->path_mtu + 7;
-		mqpcb->path_mtu = attr->path_mtu;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_PATH_MTU, 1);
-	}
-	if (attr_mask & IB_QP_TIMEOUT) {
-		mqpcb->timeout = attr->timeout;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_TIMEOUT, 1);
-	}
-	if (attr_mask & IB_QP_RETRY_CNT) {
-		mqpcb->retry_count = attr->retry_cnt;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_RETRY_COUNT, 1);
-	}
-	if (attr_mask & IB_QP_RNR_RETRY) {
-		mqpcb->rnr_retry_count = attr->rnr_retry;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_RNR_RETRY_COUNT, 1);
-	}
-	if (attr_mask & IB_QP_RQ_PSN) {
-		mqpcb->receive_psn = attr->rq_psn;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_RECEIVE_PSN, 1);
-	}
-	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC) {
-		mqpcb->rdma_nr_atomic_resp_res = attr->max_dest_rd_atomic < 3 ?
-			attr->max_dest_rd_atomic : 2;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_RDMA_NR_ATOMIC_RESP_RES, 1);
-	}
-	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC) {
-		mqpcb->rdma_atomic_outst_dest_qp = attr->max_rd_atomic < 3 ?
-			attr->max_rd_atomic : 2;
-		update_mask |=
-			EHCA_BMASK_SET
-			(MQPCB_MASK_RDMA_ATOMIC_OUTST_DEST_QP, 1);
-	}
-	if (attr_mask & IB_QP_ALT_PATH) {
-		if (attr->alt_port_num < 1
-		    || attr->alt_port_num > shca->num_ports) {
-			ret = -EINVAL;
-			ehca_err(ibqp->device, "Invalid alt_port=%x. "
-				 "ehca_qp=%p qp_num=%x num_ports=%x",
-				 attr->alt_port_num, my_qp, ibqp->qp_num,
-				 shca->num_ports);
-			goto modify_qp_exit2;
-		}
-		mqpcb->alt_phys_port = attr->alt_port_num;
-
-		if (attr->alt_pkey_index >= 16) {
-			ret = -EINVAL;
-			ehca_err(ibqp->device, "Invalid alt_pkey_index=%x. "
-				 "ehca_qp=%p qp_num=%x max_pkey_index=f",
-				 attr->pkey_index, my_qp, ibqp->qp_num);
-			goto modify_qp_exit2;
-		}
-		mqpcb->alt_p_key_idx = attr->alt_pkey_index;
-
-		mqpcb->timeout_al = attr->alt_timeout;
-		mqpcb->dlid_al = attr->alt_ah_attr.dlid;
-		mqpcb->source_path_bits_al = attr->alt_ah_attr.src_path_bits;
-		mqpcb->service_level_al = attr->alt_ah_attr.sl;
-
-		if (ehca_calc_ipd(shca, mqpcb->alt_phys_port,
-				  attr->alt_ah_attr.static_rate,
-				  &mqpcb->max_static_rate_al)) {
-			ret = -EINVAL;
-			goto modify_qp_exit2;
-		}
-
-		/* OpenIB doesn't support alternate retry counts - copy them */
-		mqpcb->retry_count_al = mqpcb->retry_count;
-		mqpcb->rnr_retry_count_al = mqpcb->rnr_retry_count;
-
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_ALT_PHYS_PORT, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_ALT_P_KEY_IDX, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_TIMEOUT_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_DLID_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_SOURCE_PATH_BITS_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_SERVICE_LEVEL_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_MAX_STATIC_RATE_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_RETRY_COUNT_AL, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_RNR_RETRY_COUNT_AL, 1);
-
-		/*
-		 * Always supply the GRH flag, even if it's zero, to give the
-		 * hypervisor a clear "yes" or "no" instead of a "perhaps"
-		 */
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SEND_GRH_FLAG_AL, 1);
-
-		/*
-		 * only if GRH is TRUE we might consider SOURCE_GID_IDX
-		 * and DEST_GID otherwise phype will return H_ATTR_PARM!!!
-		 */
-		if (attr->alt_ah_attr.ah_flags == IB_AH_GRH) {
-			mqpcb->send_grh_flag_al = 1;
-
-			for (cnt = 0; cnt < 16; cnt++)
-				mqpcb->dest_gid_al.byte[cnt] =
-					attr->alt_ah_attr.grh.dgid.raw[cnt];
-			mqpcb->source_gid_idx_al =
-				attr->alt_ah_attr.grh.sgid_index;
-			mqpcb->flow_label_al = attr->alt_ah_attr.grh.flow_label;
-			mqpcb->hop_limit_al = attr->alt_ah_attr.grh.hop_limit;
-			mqpcb->traffic_class_al =
-				attr->alt_ah_attr.grh.traffic_class;
-
-			update_mask |=
-				EHCA_BMASK_SET(MQPCB_MASK_SOURCE_GID_IDX_AL, 1)
-				| EHCA_BMASK_SET(MQPCB_MASK_DEST_GID_AL, 1)
-				| EHCA_BMASK_SET(MQPCB_MASK_FLOW_LABEL_AL, 1)
-				| EHCA_BMASK_SET(MQPCB_MASK_HOP_LIMIT_AL, 1) |
-				EHCA_BMASK_SET(MQPCB_MASK_TRAFFIC_CLASS_AL, 1);
-		}
-	}
-
-	if (attr_mask & IB_QP_MIN_RNR_TIMER) {
-		mqpcb->min_rnr_nak_timer_field = attr->min_rnr_timer;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_MIN_RNR_NAK_TIMER_FIELD, 1);
-	}
-
-	if (attr_mask & IB_QP_SQ_PSN) {
-		mqpcb->send_psn = attr->sq_psn;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_SEND_PSN, 1);
-	}
-
-	if (attr_mask & IB_QP_DEST_QPN) {
-		mqpcb->dest_qp_nr = attr->dest_qp_num;
-		update_mask |= EHCA_BMASK_SET(MQPCB_MASK_DEST_QP_NR, 1);
-	}
-
-	if (attr_mask & IB_QP_PATH_MIG_STATE) {
-		if (attr->path_mig_state != IB_MIG_REARM
-		    && attr->path_mig_state != IB_MIG_MIGRATED) {
-			ret = -EINVAL;
-			ehca_err(ibqp->device, "Invalid mig_state=%x",
-				 attr->path_mig_state);
-			goto modify_qp_exit2;
-		}
-		mqpcb->path_migration_state = attr->path_mig_state + 1;
-		if (attr->path_mig_state == IB_MIG_REARM)
-			my_qp->mig_armed = 1;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_PATH_MIGRATION_STATE, 1);
-	}
-
-	if (attr_mask & IB_QP_CAP) {
-		mqpcb->max_nr_outst_send_wr = attr->cap.max_send_wr+1;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_MAX_NR_OUTST_SEND_WR, 1);
-		mqpcb->max_nr_outst_recv_wr = attr->cap.max_recv_wr+1;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_MAX_NR_OUTST_RECV_WR, 1);
-		/* no support for max_send/recv_sge yet */
-	}
-
-	if (ehca_debug_level >= 2)
-		ehca_dmp(mqpcb, 4*70, "qp_num=%x", ibqp->qp_num);
-
-	h_ret = hipz_h_modify_qp(shca->ipz_hca_handle,
-				 my_qp->ipz_qp_handle,
-				 &my_qp->pf,
-				 update_mask,
-				 mqpcb, my_qp->galpas.kernel);
-
-	if (h_ret != H_SUCCESS) {
-		ret = ehca2ib_return_code(h_ret);
-		ehca_err(ibqp->device, "hipz_h_modify_qp() failed h_ret=%lli "
-			 "ehca_qp=%p qp_num=%x", h_ret, my_qp, ibqp->qp_num);
-		goto modify_qp_exit2;
-	}
-
-	if ((my_qp->qp_type == IB_QPT_UD ||
-	     my_qp->qp_type == IB_QPT_GSI ||
-	     my_qp->qp_type == IB_QPT_SMI) &&
-	    statetrans == IB_QPST_SQE2RTS) {
-		/* doorbell to reprocessing wqes */
-		iosync(); /* serialize GAL register access */
-		hipz_update_sqa(my_qp, bad_wqe_cnt-1);
-		ehca_gen_dbg("doorbell for %x wqes", bad_wqe_cnt);
-	}
-
-	if (statetrans == IB_QPST_RESET2INIT ||
-	    statetrans == IB_QPST_INIT2INIT) {
-		mqpcb->qp_enable = 1;
-		mqpcb->qp_state = EHCA_QPS_INIT;
-		update_mask = 0;
-		update_mask = EHCA_BMASK_SET(MQPCB_MASK_QP_ENABLE, 1);
-
-		h_ret = hipz_h_modify_qp(shca->ipz_hca_handle,
-					 my_qp->ipz_qp_handle,
-					 &my_qp->pf,
-					 update_mask,
-					 mqpcb,
-					 my_qp->galpas.kernel);
-
-		if (h_ret != H_SUCCESS) {
-			ret = ehca2ib_return_code(h_ret);
-			ehca_err(ibqp->device, "ENABLE in context of "
-				 "RESET_2_INIT failed! Maybe you didn't get "
-				 "a LID h_ret=%lli ehca_qp=%p qp_num=%x",
-				 h_ret, my_qp, ibqp->qp_num);
-			goto modify_qp_exit2;
-		}
-	}
-	if ((qp_new_state == IB_QPS_ERR) && (qp_cur_state != IB_QPS_ERR)
-	    && !is_user) {
-		ret = check_for_left_cqes(my_qp, shca);
-		if (ret)
-			goto modify_qp_exit2;
-	}
-
-	if (statetrans == IB_QPST_ANY2RESET) {
-		ipz_qeit_reset(&my_qp->ipz_rqueue);
-		ipz_qeit_reset(&my_qp->ipz_squeue);
-
-		if (qp_cur_state == IB_QPS_ERR && !is_user) {
-			del_from_err_list(my_qp->send_cq, &my_qp->sq_err_node);
-
-			if (HAS_RQ(my_qp))
-				del_from_err_list(my_qp->recv_cq,
-						  &my_qp->rq_err_node);
-		}
-		if (!is_user)
-			reset_queue_map(&my_qp->sq_map);
-
-		if (HAS_RQ(my_qp) && !is_user)
-			reset_queue_map(&my_qp->rq_map);
-	}
-
-	if (attr_mask & IB_QP_QKEY)
-		my_qp->qkey = attr->qkey;
-
-modify_qp_exit2:
-	if (squeue_locked) { /* this means: sqe -> rts */
-		spin_unlock_irqrestore(&my_qp->spinlock_s, flags);
-		my_qp->sqerr_purgeflag = 1;
-	}
-
-modify_qp_exit1:
-	ehca_free_fw_ctrlblock(mqpcb);
-
-	return ret;
-}
-
-int ehca_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr, int attr_mask,
-		   struct ib_udata *udata)
-{
-	int ret = 0;
-
-	struct ehca_shca *shca = container_of(ibqp->device, struct ehca_shca,
-					      ib_device);
-	struct ehca_qp *my_qp = container_of(ibqp, struct ehca_qp, ib_qp);
-
-	/* The if-block below caches qp_attr to be modified for GSI and SMI
-	 * qps during the initialization by ib_mad. When the respective port
-	 * is activated, ie we got an event PORT_ACTIVE, we'll replay the
-	 * cached modify calls sequence, see ehca_recover_sqs() below.
-	 * Why that is required:
-	 * 1) If one port is connected, older code requires that port one
-	 *    to be connected and module option nr_ports=1 to be given by
-	 *    user, which is very inconvenient for end user.
-	 * 2) Firmware accepts modify_qp() only if respective port has become
-	 *    active. Older code had a wait loop of 30sec create_qp()/
-	 *    define_aqp1(), which is not appropriate in practice. This
-	 *    code now removes that wait loop, see define_aqp1(), and always
-	 *    reports all ports to ib_mad resp. users. Only activated ports
-	 *    will then usable for the users.
-	 */
-	if (ibqp->qp_type == IB_QPT_GSI || ibqp->qp_type == IB_QPT_SMI) {
-		int port = my_qp->init_attr.port_num;
-		struct ehca_sport *sport = &shca->sport[port - 1];
-		unsigned long flags;
-		spin_lock_irqsave(&sport->mod_sqp_lock, flags);
-		/* cache qp_attr only during init */
-		if (my_qp->mod_qp_parm) {
-			struct ehca_mod_qp_parm *p;
-			if (my_qp->mod_qp_parm_idx >= EHCA_MOD_QP_PARM_MAX) {
-				ehca_err(&shca->ib_device,
-					 "mod_qp_parm overflow state=%x port=%x"
-					 " type=%x", attr->qp_state,
-					 my_qp->init_attr.port_num,
-					 ibqp->qp_type);
-				spin_unlock_irqrestore(&sport->mod_sqp_lock,
-						       flags);
-				return -EINVAL;
-			}
-			p = &my_qp->mod_qp_parm[my_qp->mod_qp_parm_idx];
-			p->mask = attr_mask;
-			p->attr = *attr;
-			my_qp->mod_qp_parm_idx++;
-			ehca_dbg(&shca->ib_device,
-				 "Saved qp_attr for state=%x port=%x type=%x",
-				 attr->qp_state, my_qp->init_attr.port_num,
-				 ibqp->qp_type);
-			spin_unlock_irqrestore(&sport->mod_sqp_lock, flags);
-			goto out;
-		}
-		spin_unlock_irqrestore(&sport->mod_sqp_lock, flags);
-	}
-
-	ret = internal_modify_qp(ibqp, attr, attr_mask, 0);
-
-out:
-	if ((ret == 0) && (attr_mask & IB_QP_STATE))
-		my_qp->state = attr->qp_state;
-
-	return ret;
-}
-
-void ehca_recover_sqp(struct ib_qp *sqp)
-{
-	struct ehca_qp *my_sqp = container_of(sqp, struct ehca_qp, ib_qp);
-	int port = my_sqp->init_attr.port_num;
-	struct ib_qp_attr attr;
-	struct ehca_mod_qp_parm *qp_parm;
-	int i, qp_parm_idx, ret;
-	unsigned long flags, wr_cnt;
-
-	if (!my_sqp->mod_qp_parm)
-		return;
-	ehca_dbg(sqp->device, "SQP port=%x qp_num=%x", port, sqp->qp_num);
-
-	qp_parm = my_sqp->mod_qp_parm;
-	qp_parm_idx = my_sqp->mod_qp_parm_idx;
-	for (i = 0; i < qp_parm_idx; i++) {
-		attr = qp_parm[i].attr;
-		ret = internal_modify_qp(sqp, &attr, qp_parm[i].mask, 0);
-		if (ret) {
-			ehca_err(sqp->device, "Could not modify SQP port=%x "
-				 "qp_num=%x ret=%x", port, sqp->qp_num, ret);
-			goto free_qp_parm;
-		}
-		ehca_dbg(sqp->device, "SQP port=%x qp_num=%x in state=%x",
-			 port, sqp->qp_num, attr.qp_state);
-	}
-
-	/* re-trigger posted recv wrs */
-	wr_cnt =  my_sqp->ipz_rqueue.current_q_offset /
-		my_sqp->ipz_rqueue.qe_size;
-	if (wr_cnt) {
-		spin_lock_irqsave(&my_sqp->spinlock_r, flags);
-		hipz_update_rqa(my_sqp, wr_cnt);
-		spin_unlock_irqrestore(&my_sqp->spinlock_r, flags);
-		ehca_dbg(sqp->device, "doorbell port=%x qp_num=%x wr_cnt=%lx",
-			 port, sqp->qp_num, wr_cnt);
-	}
-
-free_qp_parm:
-	kfree(qp_parm);
-	/* this prevents subsequent calls to modify_qp() to cache qp_attr */
-	my_sqp->mod_qp_parm = NULL;
-}
-
-int ehca_query_qp(struct ib_qp *qp,
-		  struct ib_qp_attr *qp_attr,
-		  int qp_attr_mask, struct ib_qp_init_attr *qp_init_attr)
-{
-	struct ehca_qp *my_qp = container_of(qp, struct ehca_qp, ib_qp);
-	struct ehca_shca *shca = container_of(qp->device, struct ehca_shca,
-					      ib_device);
-	struct ipz_adapter_handle adapter_handle = shca->ipz_hca_handle;
-	struct hcp_modify_qp_control_block *qpcb;
-	int cnt, ret = 0;
-	u64 h_ret;
-
-	if (qp_attr_mask & QP_ATTR_QUERY_NOT_SUPPORTED) {
-		ehca_err(qp->device, "Invalid attribute mask "
-			 "ehca_qp=%p qp_num=%x qp_attr_mask=%x ",
-			 my_qp, qp->qp_num, qp_attr_mask);
-		return -EINVAL;
-	}
-
-	qpcb = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!qpcb) {
-		ehca_err(qp->device, "Out of memory for qpcb "
-			 "ehca_qp=%p qp_num=%x", my_qp, qp->qp_num);
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_qp(adapter_handle,
-				my_qp->ipz_qp_handle,
-				&my_qp->pf,
-				qpcb, my_qp->galpas.kernel);
-
-	if (h_ret != H_SUCCESS) {
-		ret = ehca2ib_return_code(h_ret);
-		ehca_err(qp->device, "hipz_h_query_qp() failed "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, qp->qp_num, h_ret);
-		goto query_qp_exit1;
-	}
-
-	qp_attr->cur_qp_state = ehca2ib_qp_state(qpcb->qp_state);
-	qp_attr->qp_state = qp_attr->cur_qp_state;
-
-	if (qp_attr->cur_qp_state == -EINVAL) {
-		ret = -EINVAL;
-		ehca_err(qp->device, "Got invalid ehca_qp_state=%x "
-			 "ehca_qp=%p qp_num=%x",
-			 qpcb->qp_state, my_qp, qp->qp_num);
-		goto query_qp_exit1;
-	}
-
-	if (qp_attr->qp_state == IB_QPS_SQD)
-		qp_attr->sq_draining = 1;
-
-	qp_attr->qkey = qpcb->qkey;
-	qp_attr->path_mtu = qpcb->path_mtu;
-	qp_attr->path_mig_state = qpcb->path_migration_state - 1;
-	qp_attr->rq_psn = qpcb->receive_psn;
-	qp_attr->sq_psn = qpcb->send_psn;
-	qp_attr->min_rnr_timer = qpcb->min_rnr_nak_timer_field;
-	qp_attr->cap.max_send_wr = qpcb->max_nr_outst_send_wr-1;
-	qp_attr->cap.max_recv_wr = qpcb->max_nr_outst_recv_wr-1;
-	/* UD_AV CIRCUMVENTION */
-	if (my_qp->qp_type == IB_QPT_UD) {
-		qp_attr->cap.max_send_sge =
-			qpcb->actual_nr_sges_in_sq_wqe - 2;
-		qp_attr->cap.max_recv_sge =
-			qpcb->actual_nr_sges_in_rq_wqe - 2;
-	} else {
-		qp_attr->cap.max_send_sge =
-			qpcb->actual_nr_sges_in_sq_wqe;
-		qp_attr->cap.max_recv_sge =
-			qpcb->actual_nr_sges_in_rq_wqe;
-	}
-
-	qp_attr->cap.max_inline_data = my_qp->sq_max_inline_data_size;
-	qp_attr->dest_qp_num = qpcb->dest_qp_nr;
-
-	qp_attr->pkey_index = qpcb->prim_p_key_idx;
-	qp_attr->port_num = qpcb->prim_phys_port;
-	qp_attr->timeout = qpcb->timeout;
-	qp_attr->retry_cnt = qpcb->retry_count;
-	qp_attr->rnr_retry = qpcb->rnr_retry_count;
-
-	qp_attr->alt_pkey_index = qpcb->alt_p_key_idx;
-	qp_attr->alt_port_num = qpcb->alt_phys_port;
-	qp_attr->alt_timeout = qpcb->timeout_al;
-
-	qp_attr->max_dest_rd_atomic = qpcb->rdma_nr_atomic_resp_res;
-	qp_attr->max_rd_atomic = qpcb->rdma_atomic_outst_dest_qp;
-
-	/* primary av */
-	qp_attr->ah_attr.sl = qpcb->service_level;
-
-	if (qpcb->send_grh_flag) {
-		qp_attr->ah_attr.ah_flags = IB_AH_GRH;
-	}
-
-	qp_attr->ah_attr.static_rate = qpcb->max_static_rate;
-	qp_attr->ah_attr.dlid = qpcb->dlid;
-	qp_attr->ah_attr.src_path_bits = qpcb->source_path_bits;
-	qp_attr->ah_attr.port_num = qp_attr->port_num;
-
-	/* primary GRH */
-	qp_attr->ah_attr.grh.traffic_class = qpcb->traffic_class;
-	qp_attr->ah_attr.grh.hop_limit = qpcb->hop_limit;
-	qp_attr->ah_attr.grh.sgid_index = qpcb->source_gid_idx;
-	qp_attr->ah_attr.grh.flow_label = qpcb->flow_label;
-
-	for (cnt = 0; cnt < 16; cnt++)
-		qp_attr->ah_attr.grh.dgid.raw[cnt] =
-			qpcb->dest_gid.byte[cnt];
-
-	/* alternate AV */
-	qp_attr->alt_ah_attr.sl = qpcb->service_level_al;
-	if (qpcb->send_grh_flag_al) {
-		qp_attr->alt_ah_attr.ah_flags = IB_AH_GRH;
-	}
-
-	qp_attr->alt_ah_attr.static_rate = qpcb->max_static_rate_al;
-	qp_attr->alt_ah_attr.dlid = qpcb->dlid_al;
-	qp_attr->alt_ah_attr.src_path_bits = qpcb->source_path_bits_al;
-
-	/* alternate GRH */
-	qp_attr->alt_ah_attr.grh.traffic_class = qpcb->traffic_class_al;
-	qp_attr->alt_ah_attr.grh.hop_limit = qpcb->hop_limit_al;
-	qp_attr->alt_ah_attr.grh.sgid_index = qpcb->source_gid_idx_al;
-	qp_attr->alt_ah_attr.grh.flow_label = qpcb->flow_label_al;
-
-	for (cnt = 0; cnt < 16; cnt++)
-		qp_attr->alt_ah_attr.grh.dgid.raw[cnt] =
-			qpcb->dest_gid_al.byte[cnt];
-
-	/* return init attributes given in ehca_create_qp */
-	if (qp_init_attr)
-		*qp_init_attr = my_qp->init_attr;
-
-	if (ehca_debug_level >= 2)
-		ehca_dmp(qpcb, 4*70, "qp_num=%x", qp->qp_num);
-
-query_qp_exit1:
-	ehca_free_fw_ctrlblock(qpcb);
-
-	return ret;
-}
-
-int ehca_modify_srq(struct ib_srq *ibsrq, struct ib_srq_attr *attr,
-		    enum ib_srq_attr_mask attr_mask, struct ib_udata *udata)
-{
-	struct ehca_qp *my_qp =
-		container_of(ibsrq, struct ehca_qp, ib_srq);
-	struct ehca_shca *shca =
-		container_of(ibsrq->pd->device, struct ehca_shca, ib_device);
-	struct hcp_modify_qp_control_block *mqpcb;
-	u64 update_mask;
-	u64 h_ret;
-	int ret = 0;
-
-	mqpcb = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!mqpcb) {
-		ehca_err(ibsrq->device, "Could not get zeroed page for mqpcb "
-			 "ehca_qp=%p qp_num=%x ", my_qp, my_qp->real_qp_num);
-		return -ENOMEM;
-	}
-
-	update_mask = 0;
-	if (attr_mask & IB_SRQ_LIMIT) {
-		attr_mask &= ~IB_SRQ_LIMIT;
-		update_mask |=
-			EHCA_BMASK_SET(MQPCB_MASK_CURR_SRQ_LIMIT, 1)
-			| EHCA_BMASK_SET(MQPCB_MASK_QP_AFF_ASYN_EV_LOG_REG, 1);
-		mqpcb->curr_srq_limit = attr->srq_limit;
-		mqpcb->qp_aff_asyn_ev_log_reg =
-			EHCA_BMASK_SET(QPX_AAELOG_RESET_SRQ_LIMIT, 1);
-	}
-
-	/* by now, all bits in attr_mask should have been cleared */
-	if (attr_mask) {
-		ehca_err(ibsrq->device, "invalid attribute mask bits set  "
-			 "attr_mask=%x", attr_mask);
-		ret = -EINVAL;
-		goto modify_srq_exit0;
-	}
-
-	if (ehca_debug_level >= 2)
-		ehca_dmp(mqpcb, 4*70, "qp_num=%x", my_qp->real_qp_num);
-
-	h_ret = hipz_h_modify_qp(shca->ipz_hca_handle, my_qp->ipz_qp_handle,
-				 NULL, update_mask, mqpcb,
-				 my_qp->galpas.kernel);
-
-	if (h_ret != H_SUCCESS) {
-		ret = ehca2ib_return_code(h_ret);
-		ehca_err(ibsrq->device, "hipz_h_modify_qp() failed h_ret=%lli "
-			 "ehca_qp=%p qp_num=%x",
-			 h_ret, my_qp, my_qp->real_qp_num);
-	}
-
-modify_srq_exit0:
-	ehca_free_fw_ctrlblock(mqpcb);
-
-	return ret;
-}
-
-int ehca_query_srq(struct ib_srq *srq, struct ib_srq_attr *srq_attr)
-{
-	struct ehca_qp *my_qp = container_of(srq, struct ehca_qp, ib_srq);
-	struct ehca_shca *shca = container_of(srq->device, struct ehca_shca,
-					      ib_device);
-	struct ipz_adapter_handle adapter_handle = shca->ipz_hca_handle;
-	struct hcp_modify_qp_control_block *qpcb;
-	int ret = 0;
-	u64 h_ret;
-
-	qpcb = ehca_alloc_fw_ctrlblock(GFP_KERNEL);
-	if (!qpcb) {
-		ehca_err(srq->device, "Out of memory for qpcb "
-			 "ehca_qp=%p qp_num=%x", my_qp, my_qp->real_qp_num);
-		return -ENOMEM;
-	}
-
-	h_ret = hipz_h_query_qp(adapter_handle, my_qp->ipz_qp_handle,
-				NULL, qpcb, my_qp->galpas.kernel);
-
-	if (h_ret != H_SUCCESS) {
-		ret = ehca2ib_return_code(h_ret);
-		ehca_err(srq->device, "hipz_h_query_qp() failed "
-			 "ehca_qp=%p qp_num=%x h_ret=%lli",
-			 my_qp, my_qp->real_qp_num, h_ret);
-		goto query_srq_exit1;
-	}
-
-	srq_attr->max_wr = qpcb->max_nr_outst_recv_wr - 1;
-	srq_attr->max_sge = 3;
-	srq_attr->srq_limit = qpcb->curr_srq_limit;
-
-	if (ehca_debug_level >= 2)
-		ehca_dmp(qpcb, 4*70, "qp_num=%x", my_qp->real_qp_num);
-
-query_srq_exit1:
-	ehca_free_fw_ctrlblock(qpcb);
-
-	return ret;
-}
-
-static int internal_destroy_qp(struct ib_device *dev, struct ehca_qp *my_qp,
-			       struct ib_uobject *uobject)
-{
-	struct ehca_shca *shca = container_of(dev, struct ehca_shca, ib_device);
-	struct ehca_pd *my_pd = container_of(my_qp->ib_qp.pd, struct ehca_pd,
-					     ib_pd);
-	struct ehca_sport *sport = &shca->sport[my_qp->init_attr.port_num - 1];
-	u32 qp_num = my_qp->real_qp_num;
-	int ret;
-	u64 h_ret;
-	u8 port_num;
-	int is_user = 0;
-	enum ib_qp_type	qp_type;
-	unsigned long flags;
-
-	if (uobject) {
-		is_user = 1;
-		if (my_qp->mm_count_galpa ||
-		    my_qp->mm_count_rqueue || my_qp->mm_count_squeue) {
-			ehca_err(dev, "Resources still referenced in "
-				 "user space qp_num=%x", qp_num);
-			return -EINVAL;
-		}
-	}
-
-	if (my_qp->send_cq) {
-		ret = ehca_cq_unassign_qp(my_qp->send_cq, qp_num);
-		if (ret) {
-			ehca_err(dev, "Couldn't unassign qp from "
-				 "send_cq ret=%i qp_num=%x cq_num=%x", ret,
-				 qp_num, my_qp->send_cq->cq_number);
-			return ret;
-		}
-	}
-
-	write_lock_irqsave(&ehca_qp_idr_lock, flags);
-	idr_remove(&ehca_qp_idr, my_qp->token);
-	write_unlock_irqrestore(&ehca_qp_idr_lock, flags);
-
-	/*
-	 * SRQs will never get into an error list and do not have a recv_cq,
-	 * so we need to skip them here.
-	 */
-	if (HAS_RQ(my_qp) && !IS_SRQ(my_qp) && !is_user)
-		del_from_err_list(my_qp->recv_cq, &my_qp->rq_err_node);
-
-	if (HAS_SQ(my_qp) && !is_user)
-		del_from_err_list(my_qp->send_cq, &my_qp->sq_err_node);
-
-	/* now wait until all pending events have completed */
-	wait_event(my_qp->wait_completion, !atomic_read(&my_qp->nr_events));
-
-	h_ret = hipz_h_destroy_qp(shca->ipz_hca_handle, my_qp);
-	if (h_ret != H_SUCCESS) {
-		ehca_err(dev, "hipz_h_destroy_qp() failed h_ret=%lli "
-			 "ehca_qp=%p qp_num=%x", h_ret, my_qp, qp_num);
-		return ehca2ib_return_code(h_ret);
-	}
-
-	port_num = my_qp->init_attr.port_num;
-	qp_type  = my_qp->init_attr.qp_type;
-
-	if (qp_type == IB_QPT_SMI || qp_type == IB_QPT_GSI) {
-		spin_lock_irqsave(&sport->mod_sqp_lock, flags);
-		kfree(my_qp->mod_qp_parm);
-		my_qp->mod_qp_parm = NULL;
-		shca->sport[port_num - 1].ibqp_sqp[qp_type] = NULL;
-		spin_unlock_irqrestore(&sport->mod_sqp_lock, flags);
-	}
-
-	/* no support for IB_QPT_SMI yet */
-	if (qp_type == IB_QPT_GSI) {
-		struct ib_event event;
-		ehca_info(dev, "device %s: port %x is inactive.",
-				shca->ib_device.name, port_num);
-		event.device = &shca->ib_device;
-		event.event = IB_EVENT_PORT_ERR;
-		event.element.port_num = port_num;
-		shca->sport[port_num - 1].port_state = IB_PORT_DOWN;
-		ib_dispatch_event(&event);
-	}
-
-	if (HAS_RQ(my_qp)) {
-		ipz_queue_dtor(my_pd, &my_qp->ipz_rqueue);
-		if (!is_user)
-			vfree(my_qp->rq_map.map);
-	}
-	if (HAS_SQ(my_qp)) {
-		ipz_queue_dtor(my_pd, &my_qp->ipz_squeue);
-		if (!is_user)
-			vfree(my_qp->sq_map.map);
-	}
-	kmem_cache_free(qp_cache, my_qp);
-	atomic_dec(&shca->num_qps);
-	return 0;
-}
-
-int ehca_destroy_qp(struct ib_qp *qp)
-{
-	return internal_destroy_qp(qp->device,
-				   container_of(qp, struct ehca_qp, ib_qp),
-				   qp->uobject);
-}
-
-int ehca_destroy_srq(struct ib_srq *srq)
-{
-	return internal_destroy_qp(srq->device,
-				   container_of(srq, struct ehca_qp, ib_srq),
-				   srq->uobject);
-}
-
-int ehca_init_qp_cache(void)
-{
-	qp_cache = kmem_cache_create("ehca_cache_qp",
-				     sizeof(struct ehca_qp), 0,
-				     SLAB_HWCACHE_ALIGN,
-				     NULL);
-	if (!qp_cache)
-		return -ENOMEM;
-	return 0;
-}
-
-void ehca_cleanup_qp_cache(void)
-{
-	if (qp_cache)
-		kmem_cache_destroy(qp_cache);
-}
diff --git a/drivers/staging/rdma/ehca/ehca_reqs.c b/drivers/staging/rdma/ehca/ehca_reqs.c
deleted file mode 100644
index 10e2074384f5..000000000000
--- a/drivers/staging/rdma/ehca/ehca_reqs.c
+++ /dev/null
@@ -1,954 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  post_send/recv, poll_cq, req_notify
- *
- *  Authors: Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *           Joachim Fenkes <fenkes@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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 "ehca_classes.h"
-#include "ehca_tools.h"
-#include "ehca_qes.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-#include "hipz_fns.h"
-
-/* in RC traffic, insert an empty RDMA READ every this many packets */
-#define ACK_CIRC_THRESHOLD 2000000
-
-static u64 replace_wr_id(u64 wr_id, u16 idx)
-{
-	u64 ret;
-
-	ret = wr_id & ~QMAP_IDX_MASK;
-	ret |= idx & QMAP_IDX_MASK;
-
-	return ret;
-}
-
-static u16 get_app_wr_id(u64 wr_id)
-{
-	return wr_id & QMAP_IDX_MASK;
-}
-
-static inline int ehca_write_rwqe(struct ipz_queue *ipz_rqueue,
-				  struct ehca_wqe *wqe_p,
-				  struct ib_recv_wr *recv_wr,
-				  u32 rq_map_idx)
-{
-	u8 cnt_ds;
-	if (unlikely((recv_wr->num_sge < 0) ||
-		     (recv_wr->num_sge > ipz_rqueue->act_nr_of_sg))) {
-		ehca_gen_err("Invalid number of WQE SGE. "
-			 "num_sqe=%x max_nr_of_sg=%x",
-			 recv_wr->num_sge, ipz_rqueue->act_nr_of_sg);
-		return -EINVAL; /* invalid SG list length */
-	}
-
-	/* clear wqe header until sglist */
-	memset(wqe_p, 0, offsetof(struct ehca_wqe, u.ud_av.sg_list));
-
-	wqe_p->work_request_id = replace_wr_id(recv_wr->wr_id, rq_map_idx);
-	wqe_p->nr_of_data_seg = recv_wr->num_sge;
-
-	for (cnt_ds = 0; cnt_ds < recv_wr->num_sge; cnt_ds++) {
-		wqe_p->u.all_rcv.sg_list[cnt_ds].vaddr =
-			recv_wr->sg_list[cnt_ds].addr;
-		wqe_p->u.all_rcv.sg_list[cnt_ds].lkey =
-			recv_wr->sg_list[cnt_ds].lkey;
-		wqe_p->u.all_rcv.sg_list[cnt_ds].length =
-			recv_wr->sg_list[cnt_ds].length;
-	}
-
-	if (ehca_debug_level >= 3) {
-		ehca_gen_dbg("RECEIVE WQE written into ipz_rqueue=%p",
-			     ipz_rqueue);
-		ehca_dmp(wqe_p, 16*(6 + wqe_p->nr_of_data_seg), "recv wqe");
-	}
-
-	return 0;
-}
-
-#if defined(DEBUG_GSI_SEND_WR)
-
-/* need ib_mad struct */
-#include <rdma/ib_mad.h>
-
-static void trace_ud_wr(const struct ib_ud_wr *ud_wr)
-{
-	int idx;
-	int j;
-	while (ud_wr) {
-		struct ib_mad_hdr *mad_hdr = ud_wrmad_hdr;
-		struct ib_sge *sge = ud_wr->wr.sg_list;
-		ehca_gen_dbg("ud_wr#%x wr_id=%lx num_sge=%x "
-			     "send_flags=%x opcode=%x", idx, ud_wr->wr.wr_id,
-			     ud_wr->wr.num_sge, ud_wr->wr.send_flags,
-			     ud_wr->.wr.opcode);
-		if (mad_hdr) {
-			ehca_gen_dbg("ud_wr#%x mad_hdr base_version=%x "
-				     "mgmt_class=%x class_version=%x method=%x "
-				     "status=%x class_specific=%x tid=%lx "
-				     "attr_id=%x resv=%x attr_mod=%x",
-				     idx, mad_hdr->base_version,
-				     mad_hdr->mgmt_class,
-				     mad_hdr->class_version, mad_hdr->method,
-				     mad_hdr->status, mad_hdr->class_specific,
-				     mad_hdr->tid, mad_hdr->attr_id,
-				     mad_hdr->resv,
-				     mad_hdr->attr_mod);
-		}
-		for (j = 0; j < ud_wr->wr.num_sge; j++) {
-			u8 *data = __va(sge->addr);
-			ehca_gen_dbg("ud_wr#%x sge#%x addr=%p length=%x "
-				     "lkey=%x",
-				     idx, j, data, sge->length, sge->lkey);
-			/* assume length is n*16 */
-			ehca_dmp(data, sge->length, "ud_wr#%x sge#%x",
-				 idx, j);
-			sge++;
-		} /* eof for j */
-		idx++;
-		ud_wr = ud_wr(ud_wr->wr.next);
-	} /* eof while ud_wr */
-}
-
-#endif /* DEBUG_GSI_SEND_WR */
-
-static inline int ehca_write_swqe(struct ehca_qp *qp,
-				  struct ehca_wqe *wqe_p,
-				  struct ib_send_wr *send_wr,
-				  u32 sq_map_idx,
-				  int hidden)
-{
-	u32 idx;
-	u64 dma_length;
-	struct ehca_av *my_av;
-	u32 remote_qkey;
-	struct ehca_qmap_entry *qmap_entry = &qp->sq_map.map[sq_map_idx];
-
-	if (unlikely((send_wr->num_sge < 0) ||
-		     (send_wr->num_sge > qp->ipz_squeue.act_nr_of_sg))) {
-		ehca_gen_err("Invalid number of WQE SGE. "
-			 "num_sqe=%x max_nr_of_sg=%x",
-			 send_wr->num_sge, qp->ipz_squeue.act_nr_of_sg);
-		return -EINVAL; /* invalid SG list length */
-	}
-
-	/* clear wqe header until sglist */
-	memset(wqe_p, 0, offsetof(struct ehca_wqe, u.ud_av.sg_list));
-
-	wqe_p->work_request_id = replace_wr_id(send_wr->wr_id, sq_map_idx);
-
-	qmap_entry->app_wr_id = get_app_wr_id(send_wr->wr_id);
-	qmap_entry->reported = 0;
-	qmap_entry->cqe_req = 0;
-
-	switch (send_wr->opcode) {
-	case IB_WR_SEND:
-	case IB_WR_SEND_WITH_IMM:
-		wqe_p->optype = WQE_OPTYPE_SEND;
-		break;
-	case IB_WR_RDMA_WRITE:
-	case IB_WR_RDMA_WRITE_WITH_IMM:
-		wqe_p->optype = WQE_OPTYPE_RDMAWRITE;
-		break;
-	case IB_WR_RDMA_READ:
-		wqe_p->optype = WQE_OPTYPE_RDMAREAD;
-		break;
-	default:
-		ehca_gen_err("Invalid opcode=%x", send_wr->opcode);
-		return -EINVAL; /* invalid opcode */
-	}
-
-	wqe_p->wqef = (send_wr->opcode) & WQEF_HIGH_NIBBLE;
-
-	wqe_p->wr_flag = 0;
-
-	if ((send_wr->send_flags & IB_SEND_SIGNALED ||
-	    qp->init_attr.sq_sig_type == IB_SIGNAL_ALL_WR)
-	    && !hidden) {
-		wqe_p->wr_flag |= WQE_WRFLAG_REQ_SIGNAL_COM;
-		qmap_entry->cqe_req = 1;
-	}
-
-	if (send_wr->opcode == IB_WR_SEND_WITH_IMM ||
-	    send_wr->opcode == IB_WR_RDMA_WRITE_WITH_IMM) {
-		/* this might not work as long as HW does not support it */
-		wqe_p->immediate_data = be32_to_cpu(send_wr->ex.imm_data);
-		wqe_p->wr_flag |= WQE_WRFLAG_IMM_DATA_PRESENT;
-	}
-
-	wqe_p->nr_of_data_seg = send_wr->num_sge;
-
-	switch (qp->qp_type) {
-	case IB_QPT_SMI:
-	case IB_QPT_GSI:
-		/* no break is intential here */
-	case IB_QPT_UD:
-		/* IB 1.2 spec C10-15 compliance */
-		remote_qkey = ud_wr(send_wr)->remote_qkey;
-		if (remote_qkey & 0x80000000)
-			remote_qkey = qp->qkey;
-
-		wqe_p->destination_qp_number = ud_wr(send_wr)->remote_qpn << 8;
-		wqe_p->local_ee_context_qkey = remote_qkey;
-		if (unlikely(!ud_wr(send_wr)->ah)) {
-			ehca_gen_err("ud_wr(send_wr) is NULL. qp=%p", qp);
-			return -EINVAL;
-		}
-		if (unlikely(ud_wr(send_wr)->remote_qpn == 0)) {
-			ehca_gen_err("dest QP# is 0. qp=%x", qp->real_qp_num);
-			return -EINVAL;
-		}
-		my_av = container_of(ud_wr(send_wr)->ah, struct ehca_av, ib_ah);
-		wqe_p->u.ud_av.ud_av = my_av->av;
-
-		/*
-		 * omitted check of IB_SEND_INLINE
-		 * since HW does not support it
-		 */
-		for (idx = 0; idx < send_wr->num_sge; idx++) {
-			wqe_p->u.ud_av.sg_list[idx].vaddr =
-				send_wr->sg_list[idx].addr;
-			wqe_p->u.ud_av.sg_list[idx].lkey =
-				send_wr->sg_list[idx].lkey;
-			wqe_p->u.ud_av.sg_list[idx].length =
-				send_wr->sg_list[idx].length;
-		} /* eof for idx */
-		if (qp->qp_type == IB_QPT_SMI ||
-		    qp->qp_type == IB_QPT_GSI)
-			wqe_p->u.ud_av.ud_av.pmtu = 1;
-		if (qp->qp_type == IB_QPT_GSI) {
-			wqe_p->pkeyi = ud_wr(send_wr)->pkey_index;
-#ifdef DEBUG_GSI_SEND_WR
-			trace_ud_wr(ud_wr(send_wr));
-#endif /* DEBUG_GSI_SEND_WR */
-		}
-		break;
-
-	case IB_QPT_UC:
-		if (send_wr->send_flags & IB_SEND_FENCE)
-			wqe_p->wr_flag |= WQE_WRFLAG_FENCE;
-		/* no break is intentional here */
-	case IB_QPT_RC:
-		/* TODO: atomic not implemented */
-		wqe_p->u.nud.remote_virtual_address =
-			rdma_wr(send_wr)->remote_addr;
-		wqe_p->u.nud.rkey = rdma_wr(send_wr)->rkey;
-
-		/*
-		 * omitted checking of IB_SEND_INLINE
-		 * since HW does not support it
-		 */
-		dma_length = 0;
-		for (idx = 0; idx < send_wr->num_sge; idx++) {
-			wqe_p->u.nud.sg_list[idx].vaddr =
-				send_wr->sg_list[idx].addr;
-			wqe_p->u.nud.sg_list[idx].lkey =
-				send_wr->sg_list[idx].lkey;
-			wqe_p->u.nud.sg_list[idx].length =
-				send_wr->sg_list[idx].length;
-			dma_length += send_wr->sg_list[idx].length;
-		} /* eof idx */
-		wqe_p->u.nud.atomic_1st_op_dma_len = dma_length;
-
-		/* unsolicited ack circumvention */
-		if (send_wr->opcode == IB_WR_RDMA_READ) {
-			/* on RDMA read, switch on and reset counters */
-			qp->message_count = qp->packet_count = 0;
-			qp->unsol_ack_circ = 1;
-		} else
-			/* else estimate #packets */
-			qp->packet_count += (dma_length >> qp->mtu_shift) + 1;
-
-		break;
-
-	default:
-		ehca_gen_err("Invalid qptype=%x", qp->qp_type);
-		return -EINVAL;
-	}
-
-	if (ehca_debug_level >= 3) {
-		ehca_gen_dbg("SEND WQE written into queue qp=%p ", qp);
-		ehca_dmp( wqe_p, 16*(6 + wqe_p->nr_of_data_seg), "send wqe");
-	}
-	return 0;
-}
-
-/* map_ib_wc_status converts raw cqe_status to ib_wc_status */
-static inline void map_ib_wc_status(u32 cqe_status,
-				    enum ib_wc_status *wc_status)
-{
-	if (unlikely(cqe_status & WC_STATUS_ERROR_BIT)) {
-		switch (cqe_status & 0x3F) {
-		case 0x01:
-		case 0x21:
-			*wc_status = IB_WC_LOC_LEN_ERR;
-			break;
-		case 0x02:
-		case 0x22:
-			*wc_status = IB_WC_LOC_QP_OP_ERR;
-			break;
-		case 0x03:
-		case 0x23:
-			*wc_status = IB_WC_LOC_EEC_OP_ERR;
-			break;
-		case 0x04:
-		case 0x24:
-			*wc_status = IB_WC_LOC_PROT_ERR;
-			break;
-		case 0x05:
-		case 0x25:
-			*wc_status = IB_WC_WR_FLUSH_ERR;
-			break;
-		case 0x06:
-			*wc_status = IB_WC_MW_BIND_ERR;
-			break;
-		case 0x07: /* remote error - look into bits 20:24 */
-			switch ((cqe_status
-				 & WC_STATUS_REMOTE_ERROR_FLAGS) >> 11) {
-			case 0x0:
-				/*
-				 * PSN Sequence Error!
-				 * couldn't find a matching status!
-				 */
-				*wc_status = IB_WC_GENERAL_ERR;
-				break;
-			case 0x1:
-				*wc_status = IB_WC_REM_INV_REQ_ERR;
-				break;
-			case 0x2:
-				*wc_status = IB_WC_REM_ACCESS_ERR;
-				break;
-			case 0x3:
-				*wc_status = IB_WC_REM_OP_ERR;
-				break;
-			case 0x4:
-				*wc_status = IB_WC_REM_INV_RD_REQ_ERR;
-				break;
-			}
-			break;
-		case 0x08:
-			*wc_status = IB_WC_RETRY_EXC_ERR;
-			break;
-		case 0x09:
-			*wc_status = IB_WC_RNR_RETRY_EXC_ERR;
-			break;
-		case 0x0A:
-		case 0x2D:
-			*wc_status = IB_WC_REM_ABORT_ERR;
-			break;
-		case 0x0B:
-		case 0x2E:
-			*wc_status = IB_WC_INV_EECN_ERR;
-			break;
-		case 0x0C:
-		case 0x2F:
-			*wc_status = IB_WC_INV_EEC_STATE_ERR;
-			break;
-		case 0x0D:
-			*wc_status = IB_WC_BAD_RESP_ERR;
-			break;
-		case 0x10:
-			/* WQE purged */
-			*wc_status = IB_WC_WR_FLUSH_ERR;
-			break;
-		default:
-			*wc_status = IB_WC_FATAL_ERR;
-
-		}
-	} else
-		*wc_status = IB_WC_SUCCESS;
-}
-
-static inline int post_one_send(struct ehca_qp *my_qp,
-			 struct ib_send_wr *cur_send_wr,
-			 int hidden)
-{
-	struct ehca_wqe *wqe_p;
-	int ret;
-	u32 sq_map_idx;
-	u64 start_offset = my_qp->ipz_squeue.current_q_offset;
-
-	/* get pointer next to free WQE */
-	wqe_p = ipz_qeit_get_inc(&my_qp->ipz_squeue);
-	if (unlikely(!wqe_p)) {
-		/* too many posted work requests: queue overflow */
-		ehca_err(my_qp->ib_qp.device, "Too many posted WQEs "
-			 "qp_num=%x", my_qp->ib_qp.qp_num);
-		return -ENOMEM;
-	}
-
-	/*
-	 * Get the index of the WQE in the send queue. The same index is used
-	 * for writing into the sq_map.
-	 */
-	sq_map_idx = start_offset / my_qp->ipz_squeue.qe_size;
-
-	/* write a SEND WQE into the QUEUE */
-	ret = ehca_write_swqe(my_qp, wqe_p, cur_send_wr, sq_map_idx, hidden);
-	/*
-	 * if something failed,
-	 * reset the free entry pointer to the start value
-	 */
-	if (unlikely(ret)) {
-		my_qp->ipz_squeue.current_q_offset = start_offset;
-		ehca_err(my_qp->ib_qp.device, "Could not write WQE "
-			 "qp_num=%x", my_qp->ib_qp.qp_num);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int ehca_post_send(struct ib_qp *qp,
-		   struct ib_send_wr *send_wr,
-		   struct ib_send_wr **bad_send_wr)
-{
-	struct ehca_qp *my_qp = container_of(qp, struct ehca_qp, ib_qp);
-	int wqe_cnt = 0;
-	int ret = 0;
-	unsigned long flags;
-
-	/* Reject WR if QP is in RESET, INIT or RTR state */
-	if (unlikely(my_qp->state < IB_QPS_RTS)) {
-		ehca_err(qp->device, "Invalid QP state  qp_state=%d qpn=%x",
-			 my_qp->state, qp->qp_num);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	/* LOCK the QUEUE */
-	spin_lock_irqsave(&my_qp->spinlock_s, flags);
-
-	/* Send an empty extra RDMA read if:
-	 *  1) there has been an RDMA read on this connection before
-	 *  2) no RDMA read occurred for ACK_CIRC_THRESHOLD link packets
-	 *  3) we can be sure that any previous extra RDMA read has been
-	 *     processed so we don't overflow the SQ
-	 */
-	if (unlikely(my_qp->unsol_ack_circ &&
-		     my_qp->packet_count > ACK_CIRC_THRESHOLD &&
-		     my_qp->message_count > my_qp->init_attr.cap.max_send_wr)) {
-		/* insert an empty RDMA READ to fix up the remote QP state */
-		struct ib_send_wr circ_wr;
-		memset(&circ_wr, 0, sizeof(circ_wr));
-		circ_wr.opcode = IB_WR_RDMA_READ;
-		post_one_send(my_qp, &circ_wr, 1); /* ignore retcode */
-		wqe_cnt++;
-		ehca_dbg(qp->device, "posted circ wr  qp_num=%x", qp->qp_num);
-		my_qp->message_count = my_qp->packet_count = 0;
-	}
-
-	/* loop processes list of send reqs */
-	while (send_wr) {
-		ret = post_one_send(my_qp, send_wr, 0);
-		if (unlikely(ret)) {
-			goto post_send_exit0;
-		}
-		wqe_cnt++;
-		send_wr = send_wr->next;
-	}
-
-post_send_exit0:
-	iosync(); /* serialize GAL register access */
-	hipz_update_sqa(my_qp, wqe_cnt);
-	if (unlikely(ret || ehca_debug_level >= 2))
-		ehca_dbg(qp->device, "ehca_qp=%p qp_num=%x wqe_cnt=%d ret=%i",
-			 my_qp, qp->qp_num, wqe_cnt, ret);
-	my_qp->message_count += wqe_cnt;
-	spin_unlock_irqrestore(&my_qp->spinlock_s, flags);
-
-out:
-	if (ret)
-		*bad_send_wr = send_wr;
-	return ret;
-}
-
-static int internal_post_recv(struct ehca_qp *my_qp,
-			      struct ib_device *dev,
-			      struct ib_recv_wr *recv_wr,
-			      struct ib_recv_wr **bad_recv_wr)
-{
-	struct ehca_wqe *wqe_p;
-	int wqe_cnt = 0;
-	int ret = 0;
-	u32 rq_map_idx;
-	unsigned long flags;
-	struct ehca_qmap_entry *qmap_entry;
-
-	if (unlikely(!HAS_RQ(my_qp))) {
-		ehca_err(dev, "QP has no RQ  ehca_qp=%p qp_num=%x ext_type=%d",
-			 my_qp, my_qp->real_qp_num, my_qp->ext_type);
-		ret = -ENODEV;
-		goto out;
-	}
-
-	/* LOCK the QUEUE */
-	spin_lock_irqsave(&my_qp->spinlock_r, flags);
-
-	/* loop processes list of recv reqs */
-	while (recv_wr) {
-		u64 start_offset = my_qp->ipz_rqueue.current_q_offset;
-		/* get pointer next to free WQE */
-		wqe_p = ipz_qeit_get_inc(&my_qp->ipz_rqueue);
-		if (unlikely(!wqe_p)) {
-			/* too many posted work requests: queue overflow */
-			ret = -ENOMEM;
-			ehca_err(dev, "Too many posted WQEs "
-				"qp_num=%x", my_qp->real_qp_num);
-			goto post_recv_exit0;
-		}
-		/*
-		 * Get the index of the WQE in the recv queue. The same index
-		 * is used for writing into the rq_map.
-		 */
-		rq_map_idx = start_offset / my_qp->ipz_rqueue.qe_size;
-
-		/* write a RECV WQE into the QUEUE */
-		ret = ehca_write_rwqe(&my_qp->ipz_rqueue, wqe_p, recv_wr,
-				rq_map_idx);
-		/*
-		 * if something failed,
-		 * reset the free entry pointer to the start value
-		 */
-		if (unlikely(ret)) {
-			my_qp->ipz_rqueue.current_q_offset = start_offset;
-			ret = -EINVAL;
-			ehca_err(dev, "Could not write WQE "
-				"qp_num=%x", my_qp->real_qp_num);
-			goto post_recv_exit0;
-		}
-
-		qmap_entry = &my_qp->rq_map.map[rq_map_idx];
-		qmap_entry->app_wr_id = get_app_wr_id(recv_wr->wr_id);
-		qmap_entry->reported = 0;
-		qmap_entry->cqe_req = 1;
-
-		wqe_cnt++;
-		recv_wr = recv_wr->next;
-	} /* eof for recv_wr */
-
-post_recv_exit0:
-	iosync(); /* serialize GAL register access */
-	hipz_update_rqa(my_qp, wqe_cnt);
-	if (unlikely(ret || ehca_debug_level >= 2))
-	    ehca_dbg(dev, "ehca_qp=%p qp_num=%x wqe_cnt=%d ret=%i",
-		     my_qp, my_qp->real_qp_num, wqe_cnt, ret);
-	spin_unlock_irqrestore(&my_qp->spinlock_r, flags);
-
-out:
-	if (ret)
-		*bad_recv_wr = recv_wr;
-
-	return ret;
-}
-
-int ehca_post_recv(struct ib_qp *qp,
-		   struct ib_recv_wr *recv_wr,
-		   struct ib_recv_wr **bad_recv_wr)
-{
-	struct ehca_qp *my_qp = container_of(qp, struct ehca_qp, ib_qp);
-
-	/* Reject WR if QP is in RESET state */
-	if (unlikely(my_qp->state == IB_QPS_RESET)) {
-		ehca_err(qp->device, "Invalid QP state  qp_state=%d qpn=%x",
-			 my_qp->state, qp->qp_num);
-		*bad_recv_wr = recv_wr;
-		return -EINVAL;
-	}
-
-	return internal_post_recv(my_qp, qp->device, recv_wr, bad_recv_wr);
-}
-
-int ehca_post_srq_recv(struct ib_srq *srq,
-		       struct ib_recv_wr *recv_wr,
-		       struct ib_recv_wr **bad_recv_wr)
-{
-	return internal_post_recv(container_of(srq, struct ehca_qp, ib_srq),
-				  srq->device, recv_wr, bad_recv_wr);
-}
-
-/*
- * ib_wc_opcode table converts ehca wc opcode to ib
- * Since we use zero to indicate invalid opcode, the actual ib opcode must
- * be decremented!!!
- */
-static const u8 ib_wc_opcode[255] = {
-	[0x01] = IB_WC_RECV+1,
-	[0x02] = IB_WC_RECV_RDMA_WITH_IMM+1,
-	[0x04] = IB_WC_BIND_MW+1,
-	[0x08] = IB_WC_FETCH_ADD+1,
-	[0x10] = IB_WC_COMP_SWAP+1,
-	[0x20] = IB_WC_RDMA_WRITE+1,
-	[0x40] = IB_WC_RDMA_READ+1,
-	[0x80] = IB_WC_SEND+1
-};
-
-/* internal function to poll one entry of cq */
-static inline int ehca_poll_cq_one(struct ib_cq *cq, struct ib_wc *wc)
-{
-	int ret = 0, qmap_tail_idx;
-	struct ehca_cq *my_cq = container_of(cq, struct ehca_cq, ib_cq);
-	struct ehca_cqe *cqe;
-	struct ehca_qp *my_qp;
-	struct ehca_qmap_entry *qmap_entry;
-	struct ehca_queue_map *qmap;
-	int cqe_count = 0, is_error;
-
-repoll:
-	cqe = (struct ehca_cqe *)
-		ipz_qeit_get_inc_valid(&my_cq->ipz_queue);
-	if (!cqe) {
-		ret = -EAGAIN;
-		if (ehca_debug_level >= 3)
-			ehca_dbg(cq->device, "Completion queue is empty  "
-				 "my_cq=%p cq_num=%x", my_cq, my_cq->cq_number);
-		goto poll_cq_one_exit0;
-	}
-
-	/* prevents loads being reordered across this point */
-	rmb();
-
-	cqe_count++;
-	if (unlikely(cqe->status & WC_STATUS_PURGE_BIT)) {
-		struct ehca_qp *qp;
-		int purgeflag;
-		unsigned long flags;
-
-		qp = ehca_cq_get_qp(my_cq, cqe->local_qp_number);
-		if (!qp) {
-			ehca_err(cq->device, "cq_num=%x qp_num=%x "
-				 "could not find qp -> ignore cqe",
-				 my_cq->cq_number, cqe->local_qp_number);
-			ehca_dmp(cqe, 64, "cq_num=%x qp_num=%x",
-				 my_cq->cq_number, cqe->local_qp_number);
-			/* ignore this purged cqe */
-			goto repoll;
-		}
-		spin_lock_irqsave(&qp->spinlock_s, flags);
-		purgeflag = qp->sqerr_purgeflag;
-		spin_unlock_irqrestore(&qp->spinlock_s, flags);
-
-		if (purgeflag) {
-			ehca_dbg(cq->device,
-				 "Got CQE with purged bit qp_num=%x src_qp=%x",
-				 cqe->local_qp_number, cqe->remote_qp_number);
-			if (ehca_debug_level >= 2)
-				ehca_dmp(cqe, 64, "qp_num=%x src_qp=%x",
-					 cqe->local_qp_number,
-					 cqe->remote_qp_number);
-			/*
-			 * ignore this to avoid double cqes of bad wqe
-			 * that caused sqe and turn off purge flag
-			 */
-			qp->sqerr_purgeflag = 0;
-			goto repoll;
-		}
-	}
-
-	is_error = cqe->status & WC_STATUS_ERROR_BIT;
-
-	/* trace error CQEs if debug_level >= 1, trace all CQEs if >= 3 */
-	if (unlikely(ehca_debug_level >= 3 || (ehca_debug_level && is_error))) {
-		ehca_dbg(cq->device,
-			 "Received %sCOMPLETION ehca_cq=%p cq_num=%x -----",
-			 is_error ? "ERROR " : "", my_cq, my_cq->cq_number);
-		ehca_dmp(cqe, 64, "ehca_cq=%p cq_num=%x",
-			 my_cq, my_cq->cq_number);
-		ehca_dbg(cq->device,
-			 "ehca_cq=%p cq_num=%x -------------------------",
-			 my_cq, my_cq->cq_number);
-	}
-
-	read_lock(&ehca_qp_idr_lock);
-	my_qp = idr_find(&ehca_qp_idr, cqe->qp_token);
-	read_unlock(&ehca_qp_idr_lock);
-	if (!my_qp)
-		goto repoll;
-	wc->qp = &my_qp->ib_qp;
-
-	qmap_tail_idx = get_app_wr_id(cqe->work_request_id);
-	if (!(cqe->w_completion_flags & WC_SEND_RECEIVE_BIT))
-		/* We got a send completion. */
-		qmap = &my_qp->sq_map;
-	else
-		/* We got a receive completion. */
-		qmap = &my_qp->rq_map;
-
-	/* advance the tail pointer */
-	qmap->tail = qmap_tail_idx;
-
-	if (is_error) {
-		/*
-		 * set left_to_poll to 0 because in error state, we will not
-		 * get any additional CQEs
-		 */
-		my_qp->sq_map.next_wqe_idx = next_index(my_qp->sq_map.tail,
-							my_qp->sq_map.entries);
-		my_qp->sq_map.left_to_poll = 0;
-		ehca_add_to_err_list(my_qp, 1);
-
-		my_qp->rq_map.next_wqe_idx = next_index(my_qp->rq_map.tail,
-							my_qp->rq_map.entries);
-		my_qp->rq_map.left_to_poll = 0;
-		if (HAS_RQ(my_qp))
-			ehca_add_to_err_list(my_qp, 0);
-	}
-
-	qmap_entry = &qmap->map[qmap_tail_idx];
-	if (qmap_entry->reported) {
-		ehca_warn(cq->device, "Double cqe on qp_num=%#x",
-				my_qp->real_qp_num);
-		/* found a double cqe, discard it and read next one */
-		goto repoll;
-	}
-
-	wc->wr_id = replace_wr_id(cqe->work_request_id, qmap_entry->app_wr_id);
-	qmap_entry->reported = 1;
-
-	/* if left_to_poll is decremented to 0, add the QP to the error list */
-	if (qmap->left_to_poll > 0) {
-		qmap->left_to_poll--;
-		if ((my_qp->sq_map.left_to_poll == 0) &&
-				(my_qp->rq_map.left_to_poll == 0)) {
-			ehca_add_to_err_list(my_qp, 1);
-			if (HAS_RQ(my_qp))
-				ehca_add_to_err_list(my_qp, 0);
-		}
-	}
-
-	/* eval ib_wc_opcode */
-	wc->opcode = ib_wc_opcode[cqe->optype]-1;
-	if (unlikely(wc->opcode == -1)) {
-		ehca_err(cq->device, "Invalid cqe->OPType=%x cqe->status=%x "
-			 "ehca_cq=%p cq_num=%x",
-			 cqe->optype, cqe->status, my_cq, my_cq->cq_number);
-		/* dump cqe for other infos */
-		ehca_dmp(cqe, 64, "ehca_cq=%p cq_num=%x",
-			 my_cq, my_cq->cq_number);
-		/* update also queue adder to throw away this entry!!! */
-		goto repoll;
-	}
-
-	/* eval ib_wc_status */
-	if (unlikely(is_error)) {
-		/* complete with errors */
-		map_ib_wc_status(cqe->status, &wc->status);
-		wc->vendor_err = wc->status;
-	} else
-		wc->status = IB_WC_SUCCESS;
-
-	wc->byte_len = cqe->nr_bytes_transferred;
-	wc->pkey_index = cqe->pkey_index;
-	wc->slid = cqe->rlid;
-	wc->dlid_path_bits = cqe->dlid;
-	wc->src_qp = cqe->remote_qp_number;
-	/*
-	 * HW has "Immed data present" and "GRH present" in bits 6 and 5.
-	 * SW defines those in bits 1 and 0, so we can just shift and mask.
-	 */
-	wc->wc_flags = (cqe->w_completion_flags >> 5) & 3;
-	wc->ex.imm_data = cpu_to_be32(cqe->immediate_data);
-	wc->sl = cqe->service_level;
-
-poll_cq_one_exit0:
-	if (cqe_count > 0)
-		hipz_update_feca(my_cq, cqe_count);
-
-	return ret;
-}
-
-static int generate_flush_cqes(struct ehca_qp *my_qp, struct ib_cq *cq,
-			       struct ib_wc *wc, int num_entries,
-			       struct ipz_queue *ipz_queue, int on_sq)
-{
-	int nr = 0;
-	struct ehca_wqe *wqe;
-	u64 offset;
-	struct ehca_queue_map *qmap;
-	struct ehca_qmap_entry *qmap_entry;
-
-	if (on_sq)
-		qmap = &my_qp->sq_map;
-	else
-		qmap = &my_qp->rq_map;
-
-	qmap_entry = &qmap->map[qmap->next_wqe_idx];
-
-	while ((nr < num_entries) && (qmap_entry->reported == 0)) {
-		/* generate flush CQE */
-
-		memset(wc, 0, sizeof(*wc));
-
-		offset = qmap->next_wqe_idx * ipz_queue->qe_size;
-		wqe = (struct ehca_wqe *)ipz_qeit_calc(ipz_queue, offset);
-		if (!wqe) {
-			ehca_err(cq->device, "Invalid wqe offset=%#llx on "
-				 "qp_num=%#x", offset, my_qp->real_qp_num);
-			return nr;
-		}
-
-		wc->wr_id = replace_wr_id(wqe->work_request_id,
-					  qmap_entry->app_wr_id);
-
-		if (on_sq) {
-			switch (wqe->optype) {
-			case WQE_OPTYPE_SEND:
-				wc->opcode = IB_WC_SEND;
-				break;
-			case WQE_OPTYPE_RDMAWRITE:
-				wc->opcode = IB_WC_RDMA_WRITE;
-				break;
-			case WQE_OPTYPE_RDMAREAD:
-				wc->opcode = IB_WC_RDMA_READ;
-				break;
-			default:
-				ehca_err(cq->device, "Invalid optype=%x",
-						wqe->optype);
-				return nr;
-			}
-		} else
-			wc->opcode = IB_WC_RECV;
-
-		if (wqe->wr_flag & WQE_WRFLAG_IMM_DATA_PRESENT) {
-			wc->ex.imm_data = wqe->immediate_data;
-			wc->wc_flags |= IB_WC_WITH_IMM;
-		}
-
-		wc->status = IB_WC_WR_FLUSH_ERR;
-
-		wc->qp = &my_qp->ib_qp;
-
-		/* mark as reported and advance next_wqe pointer */
-		qmap_entry->reported = 1;
-		qmap->next_wqe_idx = next_index(qmap->next_wqe_idx,
-						qmap->entries);
-		qmap_entry = &qmap->map[qmap->next_wqe_idx];
-
-		wc++; nr++;
-	}
-
-	return nr;
-
-}
-
-int ehca_poll_cq(struct ib_cq *cq, int num_entries, struct ib_wc *wc)
-{
-	struct ehca_cq *my_cq = container_of(cq, struct ehca_cq, ib_cq);
-	int nr;
-	struct ehca_qp *err_qp;
-	struct ib_wc *current_wc = wc;
-	int ret = 0;
-	unsigned long flags;
-	int entries_left = num_entries;
-
-	if (num_entries < 1) {
-		ehca_err(cq->device, "Invalid num_entries=%d ehca_cq=%p "
-			 "cq_num=%x", num_entries, my_cq, my_cq->cq_number);
-		ret = -EINVAL;
-		goto poll_cq_exit0;
-	}
-
-	spin_lock_irqsave(&my_cq->spinlock, flags);
-
-	/* generate flush cqes for send queues */
-	list_for_each_entry(err_qp, &my_cq->sqp_err_list, sq_err_node) {
-		nr = generate_flush_cqes(err_qp, cq, current_wc, entries_left,
-				&err_qp->ipz_squeue, 1);
-		entries_left -= nr;
-		current_wc += nr;
-
-		if (entries_left == 0)
-			break;
-	}
-
-	/* generate flush cqes for receive queues */
-	list_for_each_entry(err_qp, &my_cq->rqp_err_list, rq_err_node) {
-		nr = generate_flush_cqes(err_qp, cq, current_wc, entries_left,
-				&err_qp->ipz_rqueue, 0);
-		entries_left -= nr;
-		current_wc += nr;
-
-		if (entries_left == 0)
-			break;
-	}
-
-	for (nr = 0; nr < entries_left; nr++) {
-		ret = ehca_poll_cq_one(cq, current_wc);
-		if (ret)
-			break;
-		current_wc++;
-	} /* eof for nr */
-	entries_left -= nr;
-
-	spin_unlock_irqrestore(&my_cq->spinlock, flags);
-	if (ret == -EAGAIN  || !ret)
-		ret = num_entries - entries_left;
-
-poll_cq_exit0:
-	return ret;
-}
-
-int ehca_req_notify_cq(struct ib_cq *cq, enum ib_cq_notify_flags notify_flags)
-{
-	struct ehca_cq *my_cq = container_of(cq, struct ehca_cq, ib_cq);
-	int ret = 0;
-
-	switch (notify_flags & IB_CQ_SOLICITED_MASK) {
-	case IB_CQ_SOLICITED:
-		hipz_set_cqx_n0(my_cq, 1);
-		break;
-	case IB_CQ_NEXT_COMP:
-		hipz_set_cqx_n1(my_cq, 1);
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	if (notify_flags & IB_CQ_REPORT_MISSED_EVENTS) {
-		unsigned long spl_flags;
-		spin_lock_irqsave(&my_cq->spinlock, spl_flags);
-		ret = ipz_qeit_is_valid(&my_cq->ipz_queue);
-		spin_unlock_irqrestore(&my_cq->spinlock, spl_flags);
-	}
-
-	return ret;
-}
diff --git a/drivers/staging/rdma/ehca/ehca_sqp.c b/drivers/staging/rdma/ehca/ehca_sqp.c
deleted file mode 100644
index 376b031c2c7f..000000000000
--- a/drivers/staging/rdma/ehca/ehca_sqp.c
+++ /dev/null
@@ -1,245 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  SQP functions
- *
- *  Authors: Khadija Souissi <souissi@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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 <rdma/ib_mad.h>
-
-#include "ehca_classes.h"
-#include "ehca_tools.h"
-#include "ehca_iverbs.h"
-#include "hcp_if.h"
-
-#define IB_MAD_STATUS_REDIRECT		cpu_to_be16(0x0002)
-#define IB_MAD_STATUS_UNSUP_VERSION	cpu_to_be16(0x0004)
-#define IB_MAD_STATUS_UNSUP_METHOD	cpu_to_be16(0x0008)
-
-#define IB_PMA_CLASS_PORT_INFO		cpu_to_be16(0x0001)
-
-/**
- * ehca_define_sqp - Defines special queue pair 1 (GSI QP). When special queue
- * pair is created successfully, the corresponding port gets active.
- *
- * Define Special Queue pair 0 (SMI QP) is still not supported.
- *
- * @qp_init_attr: Queue pair init attributes with port and queue pair type
- */
-
-u64 ehca_define_sqp(struct ehca_shca *shca,
-		    struct ehca_qp *ehca_qp,
-		    struct ib_qp_init_attr *qp_init_attr)
-{
-	u32 pma_qp_nr, bma_qp_nr;
-	u64 ret;
-	u8 port = qp_init_attr->port_num;
-	int counter;
-
-	shca->sport[port - 1].port_state = IB_PORT_DOWN;
-
-	switch (qp_init_attr->qp_type) {
-	case IB_QPT_SMI:
-		/* function not supported yet */
-		break;
-	case IB_QPT_GSI:
-		ret = hipz_h_define_aqp1(shca->ipz_hca_handle,
-					 ehca_qp->ipz_qp_handle,
-					 ehca_qp->galpas.kernel,
-					 (u32) qp_init_attr->port_num,
-					 &pma_qp_nr, &bma_qp_nr);
-
-		if (ret != H_SUCCESS) {
-			ehca_err(&shca->ib_device,
-				 "Can't define AQP1 for port %x. h_ret=%lli",
-				 port, ret);
-			return ret;
-		}
-		shca->sport[port - 1].pma_qp_nr = pma_qp_nr;
-		ehca_dbg(&shca->ib_device, "port=%x pma_qp_nr=%x",
-			 port, pma_qp_nr);
-		break;
-	default:
-		ehca_err(&shca->ib_device, "invalid qp_type=%x",
-			 qp_init_attr->qp_type);
-		return H_PARAMETER;
-	}
-
-	if (ehca_nr_ports < 0) /* autodetect mode */
-		return H_SUCCESS;
-
-	for (counter = 0;
-	     shca->sport[port - 1].port_state != IB_PORT_ACTIVE &&
-		     counter < ehca_port_act_time;
-	     counter++) {
-		ehca_dbg(&shca->ib_device, "... wait until port %x is active",
-			 port);
-		msleep_interruptible(1000);
-	}
-
-	if (counter == ehca_port_act_time) {
-		ehca_err(&shca->ib_device, "Port %x is not active.", port);
-		return H_HARDWARE;
-	}
-
-	return H_SUCCESS;
-}
-
-struct ib_perf {
-	struct ib_mad_hdr mad_hdr;
-	u8 reserved[40];
-	u8 data[192];
-} __attribute__ ((packed));
-
-/* TC/SL/FL packed into 32 bits, as in ClassPortInfo */
-struct tcslfl {
-	u32 tc:8;
-	u32 sl:4;
-	u32 fl:20;
-} __attribute__ ((packed));
-
-/* IP Version/TC/FL packed into 32 bits, as in GRH */
-struct vertcfl {
-	u32 ver:4;
-	u32 tc:8;
-	u32 fl:20;
-} __attribute__ ((packed));
-
-static int ehca_process_perf(struct ib_device *ibdev, u8 port_num,
-			     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
-			     const struct ib_mad *in_mad, struct ib_mad *out_mad)
-{
-	const struct ib_perf *in_perf = (const struct ib_perf *)in_mad;
-	struct ib_perf *out_perf = (struct ib_perf *)out_mad;
-	struct ib_class_port_info *poi =
-		(struct ib_class_port_info *)out_perf->data;
-	struct tcslfl *tcslfl =
-		(struct tcslfl *)&poi->redirect_tcslfl;
-	struct ehca_shca *shca =
-		container_of(ibdev, struct ehca_shca, ib_device);
-	struct ehca_sport *sport = &shca->sport[port_num - 1];
-
-	ehca_dbg(ibdev, "method=%x", in_perf->mad_hdr.method);
-
-	*out_mad = *in_mad;
-
-	if (in_perf->mad_hdr.class_version != 1) {
-		ehca_warn(ibdev, "Unsupported class_version=%x",
-			  in_perf->mad_hdr.class_version);
-		out_perf->mad_hdr.status = IB_MAD_STATUS_UNSUP_VERSION;
-		goto perf_reply;
-	}
-
-	switch (in_perf->mad_hdr.method) {
-	case IB_MGMT_METHOD_GET:
-	case IB_MGMT_METHOD_SET:
-		/* set class port info for redirection */
-		out_perf->mad_hdr.attr_id = IB_PMA_CLASS_PORT_INFO;
-		out_perf->mad_hdr.status = IB_MAD_STATUS_REDIRECT;
-		memset(poi, 0, sizeof(*poi));
-		poi->base_version = 1;
-		poi->class_version = 1;
-		poi->resp_time_value = 18;
-
-		/* copy local routing information from WC where applicable */
-		tcslfl->sl         = in_wc->sl;
-		poi->redirect_lid  =
-			sport->saved_attr.lid | in_wc->dlid_path_bits;
-		poi->redirect_qp   = sport->pma_qp_nr;
-		poi->redirect_qkey = IB_QP1_QKEY;
-
-		ehca_query_pkey(ibdev, port_num, in_wc->pkey_index,
-				&poi->redirect_pkey);
-
-		/* if request was globally routed, copy route info */
-		if (in_grh) {
-			const struct vertcfl *vertcfl =
-				(const struct vertcfl *)&in_grh->version_tclass_flow;
-			memcpy(poi->redirect_gid, in_grh->dgid.raw,
-			       sizeof(poi->redirect_gid));
-			tcslfl->tc        = vertcfl->tc;
-			tcslfl->fl        = vertcfl->fl;
-		} else
-			/* else only fill in default GID */
-			ehca_query_gid(ibdev, port_num, 0,
-				       (union ib_gid *)&poi->redirect_gid);
-
-		ehca_dbg(ibdev, "ehca_pma_lid=%x ehca_pma_qp=%x",
-			 sport->saved_attr.lid, sport->pma_qp_nr);
-		break;
-
-	case IB_MGMT_METHOD_GET_RESP:
-		return IB_MAD_RESULT_FAILURE;
-
-	default:
-		out_perf->mad_hdr.status = IB_MAD_STATUS_UNSUP_METHOD;
-		break;
-	}
-
-perf_reply:
-	out_perf->mad_hdr.method = IB_MGMT_METHOD_GET_RESP;
-
-	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
-}
-
-int ehca_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		     const struct ib_wc *in_wc, const struct ib_grh *in_grh,
-		     const struct ib_mad_hdr *in, size_t in_mad_size,
-		     struct ib_mad_hdr *out, size_t *out_mad_size,
-		     u16 *out_mad_pkey_index)
-{
-	int ret;
-	const struct ib_mad *in_mad = (const struct ib_mad *)in;
-	struct ib_mad *out_mad = (struct ib_mad *)out;
-
-	if (WARN_ON_ONCE(in_mad_size != sizeof(*in_mad) ||
-			 *out_mad_size != sizeof(*out_mad)))
-		return IB_MAD_RESULT_FAILURE;
-
-	if (!port_num || port_num > ibdev->phys_port_cnt || !in_wc)
-		return IB_MAD_RESULT_FAILURE;
-
-	/* accept only pma request */
-	if (in_mad->mad_hdr.mgmt_class != IB_MGMT_CLASS_PERF_MGMT)
-		return IB_MAD_RESULT_SUCCESS;
-
-	ehca_dbg(ibdev, "port_num=%x src_qp=%x", port_num, in_wc->src_qp);
-	ret = ehca_process_perf(ibdev, port_num, in_wc, in_grh,
-				in_mad, out_mad);
-
-	return ret;
-}
diff --git a/drivers/staging/rdma/ehca/ehca_tools.h b/drivers/staging/rdma/ehca/ehca_tools.h
deleted file mode 100644
index d280b12aae64..000000000000
--- a/drivers/staging/rdma/ehca/ehca_tools.h
+++ /dev/null
@@ -1,155 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  auxiliary functions
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Khadija Souissi <souissik@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-
-#ifndef EHCA_TOOLS_H
-#define EHCA_TOOLS_H
-
-#include <linux/kernel.h>
-#include <linux/spinlock.h>
-#include <linux/delay.h>
-#include <linux/idr.h>
-#include <linux/kthread.h>
-#include <linux/mm.h>
-#include <linux/mman.h>
-#include <linux/module.h>
-#include <linux/moduleparam.h>
-#include <linux/vmalloc.h>
-#include <linux/notifier.h>
-#include <linux/cpu.h>
-#include <linux/device.h>
-
-#include <linux/atomic.h>
-#include <asm/ibmebus.h>
-#include <asm/io.h>
-#include <asm/pgtable.h>
-#include <asm/hvcall.h>
-
-extern int ehca_debug_level;
-
-#define ehca_dbg(ib_dev, format, arg...) \
-	do { \
-		if (unlikely(ehca_debug_level)) \
-			dev_printk(KERN_DEBUG, (ib_dev)->dma_device, \
-				   "PU%04x EHCA_DBG:%s " format "\n", \
-				   raw_smp_processor_id(), __func__, \
-				   ## arg); \
-	} while (0)
-
-#define ehca_info(ib_dev, format, arg...) \
-	dev_info((ib_dev)->dma_device, "PU%04x EHCA_INFO:%s " format "\n", \
-		 raw_smp_processor_id(), __func__, ## arg)
-
-#define ehca_warn(ib_dev, format, arg...) \
-	dev_warn((ib_dev)->dma_device, "PU%04x EHCA_WARN:%s " format "\n", \
-		 raw_smp_processor_id(), __func__, ## arg)
-
-#define ehca_err(ib_dev, format, arg...) \
-	dev_err((ib_dev)->dma_device, "PU%04x EHCA_ERR:%s " format "\n", \
-		raw_smp_processor_id(), __func__, ## arg)
-
-/* use this one only if no ib_dev available */
-#define ehca_gen_dbg(format, arg...) \
-	do { \
-		if (unlikely(ehca_debug_level)) \
-			printk(KERN_DEBUG "PU%04x EHCA_DBG:%s " format "\n", \
-			       raw_smp_processor_id(), __func__, ## arg); \
-	} while (0)
-
-#define ehca_gen_warn(format, arg...) \
-	printk(KERN_INFO "PU%04x EHCA_WARN:%s " format "\n", \
-	       raw_smp_processor_id(), __func__, ## arg)
-
-#define ehca_gen_err(format, arg...) \
-	printk(KERN_ERR "PU%04x EHCA_ERR:%s " format "\n", \
-	       raw_smp_processor_id(), __func__, ## arg)
-
-/**
- * ehca_dmp - printk a memory block, whose length is n*8 bytes.
- * Each line has the following layout:
- * <format string> adr=X ofs=Y <8 bytes hex> <8 bytes hex>
- */
-#define ehca_dmp(adr, len, format, args...) \
-	do { \
-		unsigned int x; \
-		unsigned int l = (unsigned int)(len); \
-		unsigned char *deb = (unsigned char *)(adr); \
-		for (x = 0; x < l; x += 16) { \
-			printk(KERN_INFO "EHCA_DMP:%s " format \
-			       " adr=%p ofs=%04x %016llx %016llx\n", \
-			       __func__, ##args, deb, x, \
-			       *((u64 *)&deb[0]), *((u64 *)&deb[8])); \
-			deb += 16; \
-		} \
-	} while (0)
-
-/* define a bitmask, little endian version */
-#define EHCA_BMASK(pos, length) (((pos) << 16) + (length))
-
-/* define a bitmask, the ibm way... */
-#define EHCA_BMASK_IBM(from, to) (((63 - to) << 16) + ((to) - (from) + 1))
-
-/* internal function, don't use */
-#define EHCA_BMASK_SHIFTPOS(mask) (((mask) >> 16) & 0xffff)
-
-/* internal function, don't use */
-#define EHCA_BMASK_MASK(mask) (~0ULL >> ((64 - (mask)) & 0xffff))
-
-/**
- * EHCA_BMASK_SET - return value shifted and masked by mask
- * variable|=EHCA_BMASK_SET(MY_MASK,0x4711) ORs the bits in variable
- * variable&=~EHCA_BMASK_SET(MY_MASK,-1) clears the bits from the mask
- * in variable
- */
-#define EHCA_BMASK_SET(mask, value) \
-	((EHCA_BMASK_MASK(mask) & ((u64)(value))) << EHCA_BMASK_SHIFTPOS(mask))
-
-/**
- * EHCA_BMASK_GET - extract a parameter from value by mask
- */
-#define EHCA_BMASK_GET(mask, value) \
-	(EHCA_BMASK_MASK(mask) & (((u64)(value)) >> EHCA_BMASK_SHIFTPOS(mask)))
-
-/* Converts ehca to ib return code */
-int ehca2ib_return_code(u64 ehca_rc);
-
-#endif /* EHCA_TOOLS_H */
diff --git a/drivers/staging/rdma/ehca/ehca_uverbs.c b/drivers/staging/rdma/ehca/ehca_uverbs.c
deleted file mode 100644
index 1a1d5d99fcf9..000000000000
--- a/drivers/staging/rdma/ehca/ehca_uverbs.c
+++ /dev/null
@@ -1,309 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  userspace support verbs
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_classes.h"
-#include "ehca_iverbs.h"
-#include "ehca_mrmw.h"
-#include "ehca_tools.h"
-#include "hcp_if.h"
-
-struct ib_ucontext *ehca_alloc_ucontext(struct ib_device *device,
-					struct ib_udata *udata)
-{
-	struct ehca_ucontext *my_context;
-
-	my_context = kzalloc(sizeof *my_context, GFP_KERNEL);
-	if (!my_context) {
-		ehca_err(device, "Out of memory device=%p", device);
-		return ERR_PTR(-ENOMEM);
-	}
-
-	return &my_context->ib_ucontext;
-}
-
-int ehca_dealloc_ucontext(struct ib_ucontext *context)
-{
-	kfree(container_of(context, struct ehca_ucontext, ib_ucontext));
-	return 0;
-}
-
-static void ehca_mm_open(struct vm_area_struct *vma)
-{
-	u32 *count = (u32 *)vma->vm_private_data;
-	if (!count) {
-		ehca_gen_err("Invalid vma struct vm_start=%lx vm_end=%lx",
-			     vma->vm_start, vma->vm_end);
-		return;
-	}
-	(*count)++;
-	if (!(*count))
-		ehca_gen_err("Use count overflow vm_start=%lx vm_end=%lx",
-			     vma->vm_start, vma->vm_end);
-	ehca_gen_dbg("vm_start=%lx vm_end=%lx count=%x",
-		     vma->vm_start, vma->vm_end, *count);
-}
-
-static void ehca_mm_close(struct vm_area_struct *vma)
-{
-	u32 *count = (u32 *)vma->vm_private_data;
-	if (!count) {
-		ehca_gen_err("Invalid vma struct vm_start=%lx vm_end=%lx",
-			     vma->vm_start, vma->vm_end);
-		return;
-	}
-	(*count)--;
-	ehca_gen_dbg("vm_start=%lx vm_end=%lx count=%x",
-		     vma->vm_start, vma->vm_end, *count);
-}
-
-static const struct vm_operations_struct vm_ops = {
-	.open =	ehca_mm_open,
-	.close = ehca_mm_close,
-};
-
-static int ehca_mmap_fw(struct vm_area_struct *vma, struct h_galpas *galpas,
-			u32 *mm_count)
-{
-	int ret;
-	u64 vsize, physical;
-
-	vsize = vma->vm_end - vma->vm_start;
-	if (vsize < EHCA_PAGESIZE) {
-		ehca_gen_err("invalid vsize=%lx", vma->vm_end - vma->vm_start);
-		return -EINVAL;
-	}
-
-	physical = galpas->user.fw_handle;
-	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-	ehca_gen_dbg("vsize=%llx physical=%llx", vsize, physical);
-	/* VM_IO | VM_DONTEXPAND | VM_DONTDUMP are set by remap_pfn_range() */
-	ret = remap_4k_pfn(vma, vma->vm_start, physical >> EHCA_PAGESHIFT,
-			   vma->vm_page_prot);
-	if (unlikely(ret)) {
-		ehca_gen_err("remap_pfn_range() failed ret=%i", ret);
-		return -ENOMEM;
-	}
-
-	vma->vm_private_data = mm_count;
-	(*mm_count)++;
-	vma->vm_ops = &vm_ops;
-
-	return 0;
-}
-
-static int ehca_mmap_queue(struct vm_area_struct *vma, struct ipz_queue *queue,
-			   u32 *mm_count)
-{
-	int ret;
-	u64 start, ofs;
-	struct page *page;
-
-	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
-	start = vma->vm_start;
-	for (ofs = 0; ofs < queue->queue_length; ofs += PAGE_SIZE) {
-		u64 virt_addr = (u64)ipz_qeit_calc(queue, ofs);
-		page = virt_to_page(virt_addr);
-		ret = vm_insert_page(vma, start, page);
-		if (unlikely(ret)) {
-			ehca_gen_err("vm_insert_page() failed rc=%i", ret);
-			return ret;
-		}
-		start += PAGE_SIZE;
-	}
-	vma->vm_private_data = mm_count;
-	(*mm_count)++;
-	vma->vm_ops = &vm_ops;
-
-	return 0;
-}
-
-static int ehca_mmap_cq(struct vm_area_struct *vma, struct ehca_cq *cq,
-			u32 rsrc_type)
-{
-	int ret;
-
-	switch (rsrc_type) {
-	case 0: /* galpa fw handle */
-		ehca_dbg(cq->ib_cq.device, "cq_num=%x fw", cq->cq_number);
-		ret = ehca_mmap_fw(vma, &cq->galpas, &cq->mm_count_galpa);
-		if (unlikely(ret)) {
-			ehca_err(cq->ib_cq.device,
-				 "ehca_mmap_fw() failed rc=%i cq_num=%x",
-				 ret, cq->cq_number);
-			return ret;
-		}
-		break;
-
-	case 1: /* cq queue_addr */
-		ehca_dbg(cq->ib_cq.device, "cq_num=%x queue", cq->cq_number);
-		ret = ehca_mmap_queue(vma, &cq->ipz_queue, &cq->mm_count_queue);
-		if (unlikely(ret)) {
-			ehca_err(cq->ib_cq.device,
-				 "ehca_mmap_queue() failed rc=%i cq_num=%x",
-				 ret, cq->cq_number);
-			return ret;
-		}
-		break;
-
-	default:
-		ehca_err(cq->ib_cq.device, "bad resource type=%x cq_num=%x",
-			 rsrc_type, cq->cq_number);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int ehca_mmap_qp(struct vm_area_struct *vma, struct ehca_qp *qp,
-			u32 rsrc_type)
-{
-	int ret;
-
-	switch (rsrc_type) {
-	case 0: /* galpa fw handle */
-		ehca_dbg(qp->ib_qp.device, "qp_num=%x fw", qp->ib_qp.qp_num);
-		ret = ehca_mmap_fw(vma, &qp->galpas, &qp->mm_count_galpa);
-		if (unlikely(ret)) {
-			ehca_err(qp->ib_qp.device,
-				 "remap_pfn_range() failed ret=%i qp_num=%x",
-				 ret, qp->ib_qp.qp_num);
-			return -ENOMEM;
-		}
-		break;
-
-	case 1: /* qp rqueue_addr */
-		ehca_dbg(qp->ib_qp.device, "qp_num=%x rq", qp->ib_qp.qp_num);
-		ret = ehca_mmap_queue(vma, &qp->ipz_rqueue,
-				      &qp->mm_count_rqueue);
-		if (unlikely(ret)) {
-			ehca_err(qp->ib_qp.device,
-				 "ehca_mmap_queue(rq) failed rc=%i qp_num=%x",
-				 ret, qp->ib_qp.qp_num);
-			return ret;
-		}
-		break;
-
-	case 2: /* qp squeue_addr */
-		ehca_dbg(qp->ib_qp.device, "qp_num=%x sq", qp->ib_qp.qp_num);
-		ret = ehca_mmap_queue(vma, &qp->ipz_squeue,
-				      &qp->mm_count_squeue);
-		if (unlikely(ret)) {
-			ehca_err(qp->ib_qp.device,
-				 "ehca_mmap_queue(sq) failed rc=%i qp_num=%x",
-				 ret, qp->ib_qp.qp_num);
-			return ret;
-		}
-		break;
-
-	default:
-		ehca_err(qp->ib_qp.device, "bad resource type=%x qp=num=%x",
-			 rsrc_type, qp->ib_qp.qp_num);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-int ehca_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
-{
-	u64 fileoffset = vma->vm_pgoff;
-	u32 idr_handle = fileoffset & 0x1FFFFFF;
-	u32 q_type = (fileoffset >> 27) & 0x1;	  /* CQ, QP,...        */
-	u32 rsrc_type = (fileoffset >> 25) & 0x3; /* sq,rq,cmnd_window */
-	u32 ret;
-	struct ehca_cq *cq;
-	struct ehca_qp *qp;
-	struct ib_uobject *uobject;
-
-	switch (q_type) {
-	case  0: /* CQ */
-		read_lock(&ehca_cq_idr_lock);
-		cq = idr_find(&ehca_cq_idr, idr_handle);
-		read_unlock(&ehca_cq_idr_lock);
-
-		/* make sure this mmap really belongs to the authorized user */
-		if (!cq)
-			return -EINVAL;
-
-		if (!cq->ib_cq.uobject || cq->ib_cq.uobject->context != context)
-			return -EINVAL;
-
-		ret = ehca_mmap_cq(vma, cq, rsrc_type);
-		if (unlikely(ret)) {
-			ehca_err(cq->ib_cq.device,
-				 "ehca_mmap_cq() failed rc=%i cq_num=%x",
-				 ret, cq->cq_number);
-			return ret;
-		}
-		break;
-
-	case 1: /* QP */
-		read_lock(&ehca_qp_idr_lock);
-		qp = idr_find(&ehca_qp_idr, idr_handle);
-		read_unlock(&ehca_qp_idr_lock);
-
-		/* make sure this mmap really belongs to the authorized user */
-		if (!qp)
-			return -EINVAL;
-
-		uobject = IS_SRQ(qp) ? qp->ib_srq.uobject : qp->ib_qp.uobject;
-		if (!uobject || uobject->context != context)
-			return -EINVAL;
-
-		ret = ehca_mmap_qp(vma, qp, rsrc_type);
-		if (unlikely(ret)) {
-			ehca_err(qp->ib_qp.device,
-				 "ehca_mmap_qp() failed rc=%i qp_num=%x",
-				 ret, qp->ib_qp.qp_num);
-			return ret;
-		}
-		break;
-
-	default:
-		ehca_gen_err("bad queue type %x", q_type);
-		return -EINVAL;
-	}
-
-	return 0;
-}
diff --git a/drivers/staging/rdma/ehca/hcp_if.c b/drivers/staging/rdma/ehca/hcp_if.c
deleted file mode 100644
index 89517ffb4389..000000000000
--- a/drivers/staging/rdma/ehca/hcp_if.c
+++ /dev/null
@@ -1,949 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Firmware Infiniband Interface code for POWER
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Joachim Fenkes <fenkes@de.ibm.com>
- *           Gerd Bayer <gerd.bayer@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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 <asm/hvcall.h>
-#include "ehca_tools.h"
-#include "hcp_if.h"
-#include "hcp_phyp.h"
-#include "hipz_fns.h"
-#include "ipz_pt_fn.h"
-
-#define H_ALL_RES_QP_ENHANCED_OPS       EHCA_BMASK_IBM(9, 11)
-#define H_ALL_RES_QP_PTE_PIN            EHCA_BMASK_IBM(12, 12)
-#define H_ALL_RES_QP_SERVICE_TYPE       EHCA_BMASK_IBM(13, 15)
-#define H_ALL_RES_QP_STORAGE            EHCA_BMASK_IBM(16, 17)
-#define H_ALL_RES_QP_LL_RQ_CQE_POSTING  EHCA_BMASK_IBM(18, 18)
-#define H_ALL_RES_QP_LL_SQ_CQE_POSTING  EHCA_BMASK_IBM(19, 21)
-#define H_ALL_RES_QP_SIGNALING_TYPE     EHCA_BMASK_IBM(22, 23)
-#define H_ALL_RES_QP_UD_AV_LKEY_CTRL    EHCA_BMASK_IBM(31, 31)
-#define H_ALL_RES_QP_SMALL_SQ_PAGE_SIZE EHCA_BMASK_IBM(32, 35)
-#define H_ALL_RES_QP_SMALL_RQ_PAGE_SIZE EHCA_BMASK_IBM(36, 39)
-#define H_ALL_RES_QP_RESOURCE_TYPE      EHCA_BMASK_IBM(56, 63)
-
-#define H_ALL_RES_QP_MAX_OUTST_SEND_WR  EHCA_BMASK_IBM(0, 15)
-#define H_ALL_RES_QP_MAX_OUTST_RECV_WR  EHCA_BMASK_IBM(16, 31)
-#define H_ALL_RES_QP_MAX_SEND_SGE       EHCA_BMASK_IBM(32, 39)
-#define H_ALL_RES_QP_MAX_RECV_SGE       EHCA_BMASK_IBM(40, 47)
-
-#define H_ALL_RES_QP_UD_AV_LKEY         EHCA_BMASK_IBM(32, 63)
-#define H_ALL_RES_QP_SRQ_QP_TOKEN       EHCA_BMASK_IBM(0, 31)
-#define H_ALL_RES_QP_SRQ_QP_HANDLE      EHCA_BMASK_IBM(0, 64)
-#define H_ALL_RES_QP_SRQ_LIMIT          EHCA_BMASK_IBM(48, 63)
-#define H_ALL_RES_QP_SRQ_QPN            EHCA_BMASK_IBM(40, 63)
-
-#define H_ALL_RES_QP_ACT_OUTST_SEND_WR  EHCA_BMASK_IBM(16, 31)
-#define H_ALL_RES_QP_ACT_OUTST_RECV_WR  EHCA_BMASK_IBM(48, 63)
-#define H_ALL_RES_QP_ACT_SEND_SGE       EHCA_BMASK_IBM(8, 15)
-#define H_ALL_RES_QP_ACT_RECV_SGE       EHCA_BMASK_IBM(24, 31)
-
-#define H_ALL_RES_QP_SQUEUE_SIZE_PAGES  EHCA_BMASK_IBM(0, 31)
-#define H_ALL_RES_QP_RQUEUE_SIZE_PAGES  EHCA_BMASK_IBM(32, 63)
-
-#define H_MP_INIT_TYPE                  EHCA_BMASK_IBM(44, 47)
-#define H_MP_SHUTDOWN                   EHCA_BMASK_IBM(48, 48)
-#define H_MP_RESET_QKEY_CTR             EHCA_BMASK_IBM(49, 49)
-
-#define HCALL4_REGS_FORMAT "r4=%lx r5=%lx r6=%lx r7=%lx"
-#define HCALL7_REGS_FORMAT HCALL4_REGS_FORMAT " r8=%lx r9=%lx r10=%lx"
-#define HCALL9_REGS_FORMAT HCALL7_REGS_FORMAT " r11=%lx r12=%lx"
-
-static DEFINE_SPINLOCK(hcall_lock);
-
-static long ehca_plpar_hcall_norets(unsigned long opcode,
-				    unsigned long arg1,
-				    unsigned long arg2,
-				    unsigned long arg3,
-				    unsigned long arg4,
-				    unsigned long arg5,
-				    unsigned long arg6,
-				    unsigned long arg7)
-{
-	long ret;
-	int i, sleep_msecs;
-	unsigned long flags = 0;
-
-	if (unlikely(ehca_debug_level >= 2))
-		ehca_gen_dbg("opcode=%lx " HCALL7_REGS_FORMAT,
-			     opcode, arg1, arg2, arg3, arg4, arg5, arg6, arg7);
-
-	for (i = 0; i < 5; i++) {
-		/* serialize hCalls to work around firmware issue */
-		if (ehca_lock_hcalls)
-			spin_lock_irqsave(&hcall_lock, flags);
-
-		ret = plpar_hcall_norets(opcode, arg1, arg2, arg3, arg4,
-					 arg5, arg6, arg7);
-
-		if (ehca_lock_hcalls)
-			spin_unlock_irqrestore(&hcall_lock, flags);
-
-		if (H_IS_LONG_BUSY(ret)) {
-			sleep_msecs = get_longbusy_msecs(ret);
-			msleep_interruptible(sleep_msecs);
-			continue;
-		}
-
-		if (ret < H_SUCCESS)
-			ehca_gen_err("opcode=%lx ret=%li " HCALL7_REGS_FORMAT,
-				     opcode, ret, arg1, arg2, arg3,
-				     arg4, arg5, arg6, arg7);
-		else
-			if (unlikely(ehca_debug_level >= 2))
-				ehca_gen_dbg("opcode=%lx ret=%li", opcode, ret);
-
-		return ret;
-	}
-
-	return H_BUSY;
-}
-
-static long ehca_plpar_hcall9(unsigned long opcode,
-			      unsigned long *outs, /* array of 9 outputs */
-			      unsigned long arg1,
-			      unsigned long arg2,
-			      unsigned long arg3,
-			      unsigned long arg4,
-			      unsigned long arg5,
-			      unsigned long arg6,
-			      unsigned long arg7,
-			      unsigned long arg8,
-			      unsigned long arg9)
-{
-	long ret;
-	int i, sleep_msecs;
-	unsigned long flags = 0;
-
-	if (unlikely(ehca_debug_level >= 2))
-		ehca_gen_dbg("INPUT -- opcode=%lx " HCALL9_REGS_FORMAT, opcode,
-			     arg1, arg2, arg3, arg4, arg5,
-			     arg6, arg7, arg8, arg9);
-
-	for (i = 0; i < 5; i++) {
-		/* serialize hCalls to work around firmware issue */
-		if (ehca_lock_hcalls)
-			spin_lock_irqsave(&hcall_lock, flags);
-
-		ret = plpar_hcall9(opcode, outs,
-				   arg1, arg2, arg3, arg4, arg5,
-				   arg6, arg7, arg8, arg9);
-
-		if (ehca_lock_hcalls)
-			spin_unlock_irqrestore(&hcall_lock, flags);
-
-		if (H_IS_LONG_BUSY(ret)) {
-			sleep_msecs = get_longbusy_msecs(ret);
-			msleep_interruptible(sleep_msecs);
-			continue;
-		}
-
-		if (ret < H_SUCCESS) {
-			ehca_gen_err("INPUT -- opcode=%lx " HCALL9_REGS_FORMAT,
-				     opcode, arg1, arg2, arg3, arg4, arg5,
-				     arg6, arg7, arg8, arg9);
-			ehca_gen_err("OUTPUT -- ret=%li " HCALL9_REGS_FORMAT,
-				     ret, outs[0], outs[1], outs[2], outs[3],
-				     outs[4], outs[5], outs[6], outs[7],
-				     outs[8]);
-		} else if (unlikely(ehca_debug_level >= 2))
-			ehca_gen_dbg("OUTPUT -- ret=%li " HCALL9_REGS_FORMAT,
-				     ret, outs[0], outs[1], outs[2], outs[3],
-				     outs[4], outs[5], outs[6], outs[7],
-				     outs[8]);
-		return ret;
-	}
-
-	return H_BUSY;
-}
-
-u64 hipz_h_alloc_resource_eq(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_pfeq *pfeq,
-			     const u32 neq_control,
-			     const u32 number_of_entries,
-			     struct ipz_eq_handle *eq_handle,
-			     u32 *act_nr_of_entries,
-			     u32 *act_pages,
-			     u32 *eq_ist)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-	u64 allocate_controls;
-
-	/* resource type */
-	allocate_controls = 3ULL;
-
-	/* ISN is associated */
-	if (neq_control != 1)
-		allocate_controls = (1ULL << (63 - 7)) | allocate_controls;
-	else /* notification event queue */
-		allocate_controls = (1ULL << 63) | allocate_controls;
-
-	ret = ehca_plpar_hcall9(H_ALLOC_RESOURCE, outs,
-				adapter_handle.handle,  /* r4 */
-				allocate_controls,      /* r5 */
-				number_of_entries,      /* r6 */
-				0, 0, 0, 0, 0, 0);
-	eq_handle->handle = outs[0];
-	*act_nr_of_entries = (u32)outs[3];
-	*act_pages = (u32)outs[4];
-	*eq_ist = (u32)outs[5];
-
-	if (ret == H_NOT_ENOUGH_RESOURCES)
-		ehca_gen_err("Not enough resource - ret=%lli ", ret);
-
-	return ret;
-}
-
-u64 hipz_h_reset_event(const struct ipz_adapter_handle adapter_handle,
-		       struct ipz_eq_handle eq_handle,
-		       const u64 event_mask)
-{
-	return ehca_plpar_hcall_norets(H_RESET_EVENTS,
-				       adapter_handle.handle, /* r4 */
-				       eq_handle.handle,      /* r5 */
-				       event_mask,	      /* r6 */
-				       0, 0, 0, 0);
-}
-
-u64 hipz_h_alloc_resource_cq(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_cq *cq,
-			     struct ehca_alloc_cq_parms *param)
-{
-	int rc;
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_ALLOC_RESOURCE, outs,
-				adapter_handle.handle,   /* r4  */
-				2,	                 /* r5  */
-				param->eq_handle.handle, /* r6  */
-				cq->token,	         /* r7  */
-				param->nr_cqe,           /* r8  */
-				0, 0, 0, 0);
-	cq->ipz_cq_handle.handle = outs[0];
-	param->act_nr_of_entries = (u32)outs[3];
-	param->act_pages = (u32)outs[4];
-
-	if (ret == H_SUCCESS) {
-		rc = hcp_galpas_ctor(&cq->galpas, 0, outs[5], outs[6]);
-		if (rc) {
-			ehca_gen_err("Could not establish HW access. rc=%d paddr=%#lx",
-				     rc, outs[5]);
-
-			ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-						adapter_handle.handle,     /* r4 */
-						cq->ipz_cq_handle.handle,  /* r5 */
-						0, 0, 0, 0, 0);
-			ret = H_NO_MEM;
-		}
-	}
-
-	if (ret == H_NOT_ENOUGH_RESOURCES)
-		ehca_gen_err("Not enough resources. ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_alloc_resource_qp(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_alloc_qp_parms *parms, int is_user)
-{
-	int rc;
-	u64 ret;
-	u64 allocate_controls, max_r10_reg, r11, r12;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	allocate_controls =
-		EHCA_BMASK_SET(H_ALL_RES_QP_ENHANCED_OPS, parms->ext_type)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_PTE_PIN, 0)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_SERVICE_TYPE, parms->servicetype)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_SIGNALING_TYPE, parms->sigtype)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_STORAGE, parms->qp_storage)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_SMALL_SQ_PAGE_SIZE,
-				 parms->squeue.page_size)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_SMALL_RQ_PAGE_SIZE,
-				 parms->rqueue.page_size)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_LL_RQ_CQE_POSTING,
-				 !!(parms->ll_comp_flags & LLQP_RECV_COMP))
-		| EHCA_BMASK_SET(H_ALL_RES_QP_LL_SQ_CQE_POSTING,
-				 !!(parms->ll_comp_flags & LLQP_SEND_COMP))
-		| EHCA_BMASK_SET(H_ALL_RES_QP_UD_AV_LKEY_CTRL,
-				 parms->ud_av_l_key_ctl)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_RESOURCE_TYPE, 1);
-
-	max_r10_reg =
-		EHCA_BMASK_SET(H_ALL_RES_QP_MAX_OUTST_SEND_WR,
-			       parms->squeue.max_wr + 1)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_MAX_OUTST_RECV_WR,
-				 parms->rqueue.max_wr + 1)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_MAX_SEND_SGE,
-				 parms->squeue.max_sge)
-		| EHCA_BMASK_SET(H_ALL_RES_QP_MAX_RECV_SGE,
-				 parms->rqueue.max_sge);
-
-	r11 = EHCA_BMASK_SET(H_ALL_RES_QP_SRQ_QP_TOKEN, parms->srq_token);
-
-	if (parms->ext_type == EQPT_SRQ)
-		r12 = EHCA_BMASK_SET(H_ALL_RES_QP_SRQ_LIMIT, parms->srq_limit);
-	else
-		r12 = EHCA_BMASK_SET(H_ALL_RES_QP_SRQ_QPN, parms->srq_qpn);
-
-	ret = ehca_plpar_hcall9(H_ALLOC_RESOURCE, outs,
-				adapter_handle.handle,	           /* r4  */
-				allocate_controls,	           /* r5  */
-				parms->send_cq_handle.handle,
-				parms->recv_cq_handle.handle,
-				parms->eq_handle.handle,
-				((u64)parms->token << 32) | parms->pd.value,
-				max_r10_reg, r11, r12);
-
-	parms->qp_handle.handle = outs[0];
-	parms->real_qp_num = (u32)outs[1];
-	parms->squeue.act_nr_wqes =
-		(u16)EHCA_BMASK_GET(H_ALL_RES_QP_ACT_OUTST_SEND_WR, outs[2]);
-	parms->rqueue.act_nr_wqes =
-		(u16)EHCA_BMASK_GET(H_ALL_RES_QP_ACT_OUTST_RECV_WR, outs[2]);
-	parms->squeue.act_nr_sges =
-		(u8)EHCA_BMASK_GET(H_ALL_RES_QP_ACT_SEND_SGE, outs[3]);
-	parms->rqueue.act_nr_sges =
-		(u8)EHCA_BMASK_GET(H_ALL_RES_QP_ACT_RECV_SGE, outs[3]);
-	parms->squeue.queue_size =
-		(u32)EHCA_BMASK_GET(H_ALL_RES_QP_SQUEUE_SIZE_PAGES, outs[4]);
-	parms->rqueue.queue_size =
-		(u32)EHCA_BMASK_GET(H_ALL_RES_QP_RQUEUE_SIZE_PAGES, outs[4]);
-
-	if (ret == H_SUCCESS) {
-		rc = hcp_galpas_ctor(&parms->galpas, is_user, outs[6], outs[6]);
-		if (rc) {
-			ehca_gen_err("Could not establish HW access. rc=%d paddr=%#lx",
-				     rc, outs[6]);
-
-			ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-						adapter_handle.handle,     /* r4 */
-						parms->qp_handle.handle,  /* r5 */
-						0, 0, 0, 0, 0);
-			ret = H_NO_MEM;
-		}
-	}
-
-	if (ret == H_NOT_ENOUGH_RESOURCES)
-		ehca_gen_err("Not enough resources. ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_query_port(const struct ipz_adapter_handle adapter_handle,
-		      const u8 port_id,
-		      struct hipz_query_port *query_port_response_block)
-{
-	u64 ret;
-	u64 r_cb = __pa(query_port_response_block);
-
-	if (r_cb & (EHCA_PAGESIZE-1)) {
-		ehca_gen_err("response block not page aligned");
-		return H_PARAMETER;
-	}
-
-	ret = ehca_plpar_hcall_norets(H_QUERY_PORT,
-				      adapter_handle.handle, /* r4 */
-				      port_id,	             /* r5 */
-				      r_cb,	             /* r6 */
-				      0, 0, 0, 0);
-
-	if (ehca_debug_level >= 2)
-		ehca_dmp(query_port_response_block, 64, "response_block");
-
-	return ret;
-}
-
-u64 hipz_h_modify_port(const struct ipz_adapter_handle adapter_handle,
-		       const u8 port_id, const u32 port_cap,
-		       const u8 init_type, const int modify_mask)
-{
-	u64 port_attributes = port_cap;
-
-	if (modify_mask & IB_PORT_SHUTDOWN)
-		port_attributes |= EHCA_BMASK_SET(H_MP_SHUTDOWN, 1);
-	if (modify_mask & IB_PORT_INIT_TYPE)
-		port_attributes |= EHCA_BMASK_SET(H_MP_INIT_TYPE, init_type);
-	if (modify_mask & IB_PORT_RESET_QKEY_CNTR)
-		port_attributes |= EHCA_BMASK_SET(H_MP_RESET_QKEY_CTR, 1);
-
-	return ehca_plpar_hcall_norets(H_MODIFY_PORT,
-				       adapter_handle.handle, /* r4 */
-				       port_id,               /* r5 */
-				       port_attributes,       /* r6 */
-				       0, 0, 0, 0);
-}
-
-u64 hipz_h_query_hca(const struct ipz_adapter_handle adapter_handle,
-		     struct hipz_query_hca *query_hca_rblock)
-{
-	u64 r_cb = __pa(query_hca_rblock);
-
-	if (r_cb & (EHCA_PAGESIZE-1)) {
-		ehca_gen_err("response_block=%p not page aligned",
-			     query_hca_rblock);
-		return H_PARAMETER;
-	}
-
-	return ehca_plpar_hcall_norets(H_QUERY_HCA,
-				       adapter_handle.handle, /* r4 */
-				       r_cb,                  /* r5 */
-				       0, 0, 0, 0, 0);
-}
-
-u64 hipz_h_register_rpage(const struct ipz_adapter_handle adapter_handle,
-			  const u8 pagesize,
-			  const u8 queue_type,
-			  const u64 resource_handle,
-			  const u64 logical_address_of_page,
-			  u64 count)
-{
-	return ehca_plpar_hcall_norets(H_REGISTER_RPAGES,
-				       adapter_handle.handle,      /* r4  */
-				       (u64)queue_type | ((u64)pagesize) << 8,
-				       /* r5  */
-				       resource_handle,	           /* r6  */
-				       logical_address_of_page,    /* r7  */
-				       count,	                   /* r8  */
-				       0, 0);
-}
-
-u64 hipz_h_register_rpage_eq(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_eq_handle eq_handle,
-			     struct ehca_pfeq *pfeq,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count)
-{
-	if (count != 1) {
-		ehca_gen_err("Ppage counter=%llx", count);
-		return H_PARAMETER;
-	}
-	return hipz_h_register_rpage(adapter_handle,
-				     pagesize,
-				     queue_type,
-				     eq_handle.handle,
-				     logical_address_of_page, count);
-}
-
-u64 hipz_h_query_int_state(const struct ipz_adapter_handle adapter_handle,
-			   u32 ist)
-{
-	u64 ret;
-	ret = ehca_plpar_hcall_norets(H_QUERY_INT_STATE,
-				      adapter_handle.handle, /* r4 */
-				      ist,                   /* r5 */
-				      0, 0, 0, 0, 0);
-
-	if (ret != H_SUCCESS && ret != H_BUSY)
-		ehca_gen_err("Could not query interrupt state.");
-
-	return ret;
-}
-
-u64 hipz_h_register_rpage_cq(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_cq_handle cq_handle,
-			     struct ehca_pfcq *pfcq,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count,
-			     const struct h_galpa gal)
-{
-	if (count != 1) {
-		ehca_gen_err("Page counter=%llx", count);
-		return H_PARAMETER;
-	}
-
-	return hipz_h_register_rpage(adapter_handle, pagesize, queue_type,
-				     cq_handle.handle, logical_address_of_page,
-				     count);
-}
-
-u64 hipz_h_register_rpage_qp(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_qp_handle qp_handle,
-			     struct ehca_pfqp *pfqp,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count,
-			     const struct h_galpa galpa)
-{
-	if (count > 1) {
-		ehca_gen_err("Page counter=%llx", count);
-		return H_PARAMETER;
-	}
-
-	return hipz_h_register_rpage(adapter_handle, pagesize, queue_type,
-				     qp_handle.handle, logical_address_of_page,
-				     count);
-}
-
-u64 hipz_h_disable_and_get_wqe(const struct ipz_adapter_handle adapter_handle,
-			       const struct ipz_qp_handle qp_handle,
-			       struct ehca_pfqp *pfqp,
-			       void **log_addr_next_sq_wqe2processed,
-			       void **log_addr_next_rq_wqe2processed,
-			       int dis_and_get_function_code)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_DISABLE_AND_GETC, outs,
-				adapter_handle.handle,     /* r4 */
-				dis_and_get_function_code, /* r5 */
-				qp_handle.handle,	   /* r6 */
-				0, 0, 0, 0, 0, 0);
-	if (log_addr_next_sq_wqe2processed)
-		*log_addr_next_sq_wqe2processed = (void *)outs[0];
-	if (log_addr_next_rq_wqe2processed)
-		*log_addr_next_rq_wqe2processed = (void *)outs[1];
-
-	return ret;
-}
-
-u64 hipz_h_modify_qp(const struct ipz_adapter_handle adapter_handle,
-		     const struct ipz_qp_handle qp_handle,
-		     struct ehca_pfqp *pfqp,
-		     const u64 update_mask,
-		     struct hcp_modify_qp_control_block *mqpcb,
-		     struct h_galpa gal)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-	ret = ehca_plpar_hcall9(H_MODIFY_QP, outs,
-				adapter_handle.handle, /* r4 */
-				qp_handle.handle,      /* r5 */
-				update_mask,	       /* r6 */
-				__pa(mqpcb),	       /* r7 */
-				0, 0, 0, 0, 0);
-
-	if (ret == H_NOT_ENOUGH_RESOURCES)
-		ehca_gen_err("Insufficient resources ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_query_qp(const struct ipz_adapter_handle adapter_handle,
-		    const struct ipz_qp_handle qp_handle,
-		    struct ehca_pfqp *pfqp,
-		    struct hcp_modify_qp_control_block *qqpcb,
-		    struct h_galpa gal)
-{
-	return ehca_plpar_hcall_norets(H_QUERY_QP,
-				       adapter_handle.handle, /* r4 */
-				       qp_handle.handle,      /* r5 */
-				       __pa(qqpcb),	      /* r6 */
-				       0, 0, 0, 0);
-}
-
-u64 hipz_h_destroy_qp(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_qp *qp)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = hcp_galpas_dtor(&qp->galpas);
-	if (ret) {
-		ehca_gen_err("Could not destruct qp->galpas");
-		return H_RESOURCE;
-	}
-	ret = ehca_plpar_hcall9(H_DISABLE_AND_GETC, outs,
-				adapter_handle.handle,     /* r4 */
-				/* function code */
-				1,	                   /* r5 */
-				qp->ipz_qp_handle.handle,  /* r6 */
-				0, 0, 0, 0, 0, 0);
-	if (ret == H_HARDWARE)
-		ehca_gen_err("HCA not operational. ret=%lli", ret);
-
-	ret = ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-				      adapter_handle.handle,     /* r4 */
-				      qp->ipz_qp_handle.handle,  /* r5 */
-				      0, 0, 0, 0, 0);
-
-	if (ret == H_RESOURCE)
-		ehca_gen_err("Resource still in use. ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_define_aqp0(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u32 port)
-{
-	return ehca_plpar_hcall_norets(H_DEFINE_AQP0,
-				       adapter_handle.handle, /* r4 */
-				       qp_handle.handle,      /* r5 */
-				       port,                  /* r6 */
-				       0, 0, 0, 0);
-}
-
-u64 hipz_h_define_aqp1(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u32 port, u32 * pma_qp_nr,
-		       u32 * bma_qp_nr)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_DEFINE_AQP1, outs,
-				adapter_handle.handle, /* r4 */
-				qp_handle.handle,      /* r5 */
-				port,	               /* r6 */
-				0, 0, 0, 0, 0, 0);
-	*pma_qp_nr = (u32)outs[0];
-	*bma_qp_nr = (u32)outs[1];
-
-	if (ret == H_ALIAS_EXIST)
-		ehca_gen_err("AQP1 already exists. ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_attach_mcqp(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u16 mcg_dlid,
-		       u64 subnet_prefix, u64 interface_id)
-{
-	u64 ret;
-
-	ret = ehca_plpar_hcall_norets(H_ATTACH_MCQP,
-				      adapter_handle.handle,  /* r4 */
-				      qp_handle.handle,       /* r5 */
-				      mcg_dlid,               /* r6 */
-				      interface_id,           /* r7 */
-				      subnet_prefix,          /* r8 */
-				      0, 0);
-
-	if (ret == H_NOT_ENOUGH_RESOURCES)
-		ehca_gen_err("Not enough resources. ret=%lli", ret);
-
-	return ret;
-}
-
-u64 hipz_h_detach_mcqp(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u16 mcg_dlid,
-		       u64 subnet_prefix, u64 interface_id)
-{
-	return ehca_plpar_hcall_norets(H_DETACH_MCQP,
-				       adapter_handle.handle, /* r4 */
-				       qp_handle.handle,      /* r5 */
-				       mcg_dlid,              /* r6 */
-				       interface_id,          /* r7 */
-				       subnet_prefix,         /* r8 */
-				       0, 0);
-}
-
-u64 hipz_h_destroy_cq(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_cq *cq,
-		      u8 force_flag)
-{
-	u64 ret;
-
-	ret = hcp_galpas_dtor(&cq->galpas);
-	if (ret) {
-		ehca_gen_err("Could not destruct cp->galpas");
-		return H_RESOURCE;
-	}
-
-	ret = ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-				      adapter_handle.handle,     /* r4 */
-				      cq->ipz_cq_handle.handle,  /* r5 */
-				      force_flag != 0 ? 1L : 0L, /* r6 */
-				      0, 0, 0, 0);
-
-	if (ret == H_RESOURCE)
-		ehca_gen_err("H_FREE_RESOURCE failed ret=%lli ", ret);
-
-	return ret;
-}
-
-u64 hipz_h_destroy_eq(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_eq *eq)
-{
-	u64 ret;
-
-	ret = hcp_galpas_dtor(&eq->galpas);
-	if (ret) {
-		ehca_gen_err("Could not destruct eq->galpas");
-		return H_RESOURCE;
-	}
-
-	ret = ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-				      adapter_handle.handle,     /* r4 */
-				      eq->ipz_eq_handle.handle,  /* r5 */
-				      0, 0, 0, 0, 0);
-
-	if (ret == H_RESOURCE)
-		ehca_gen_err("Resource in use. ret=%lli ", ret);
-
-	return ret;
-}
-
-u64 hipz_h_alloc_resource_mr(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mr *mr,
-			     const u64 vaddr,
-			     const u64 length,
-			     const u32 access_ctrl,
-			     const struct ipz_pd pd,
-			     struct ehca_mr_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_ALLOC_RESOURCE, outs,
-				adapter_handle.handle,            /* r4 */
-				5,                                /* r5 */
-				vaddr,                            /* r6 */
-				length,                           /* r7 */
-				(((u64)access_ctrl) << 32ULL),    /* r8 */
-				pd.value,                         /* r9 */
-				0, 0, 0);
-	outparms->handle.handle = outs[0];
-	outparms->lkey = (u32)outs[2];
-	outparms->rkey = (u32)outs[3];
-
-	return ret;
-}
-
-u64 hipz_h_register_rpage_mr(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mr *mr,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count)
-{
-	u64 ret;
-
-	if (unlikely(ehca_debug_level >= 3)) {
-		if (count > 1) {
-			u64 *kpage;
-			int i;
-			kpage = __va(logical_address_of_page);
-			for (i = 0; i < count; i++)
-				ehca_gen_dbg("kpage[%d]=%p",
-					     i, (void *)kpage[i]);
-		} else
-			ehca_gen_dbg("kpage=%p",
-				     (void *)logical_address_of_page);
-	}
-
-	if ((count > 1) && (logical_address_of_page & (EHCA_PAGESIZE-1))) {
-		ehca_gen_err("logical_address_of_page not on a 4k boundary "
-			     "adapter_handle=%llx mr=%p mr_handle=%llx "
-			     "pagesize=%x queue_type=%x "
-			     "logical_address_of_page=%llx count=%llx",
-			     adapter_handle.handle, mr,
-			     mr->ipz_mr_handle.handle, pagesize, queue_type,
-			     logical_address_of_page, count);
-		ret = H_PARAMETER;
-	} else
-		ret = hipz_h_register_rpage(adapter_handle, pagesize,
-					    queue_type,
-					    mr->ipz_mr_handle.handle,
-					    logical_address_of_page, count);
-	return ret;
-}
-
-u64 hipz_h_query_mr(const struct ipz_adapter_handle adapter_handle,
-		    const struct ehca_mr *mr,
-		    struct ehca_mr_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_QUERY_MR, outs,
-				adapter_handle.handle,     /* r4 */
-				mr->ipz_mr_handle.handle,  /* r5 */
-				0, 0, 0, 0, 0, 0, 0);
-	outparms->len = outs[0];
-	outparms->vaddr = outs[1];
-	outparms->acl  = outs[4] >> 32;
-	outparms->lkey = (u32)(outs[5] >> 32);
-	outparms->rkey = (u32)(outs[5] & (0xffffffff));
-
-	return ret;
-}
-
-u64 hipz_h_free_resource_mr(const struct ipz_adapter_handle adapter_handle,
-			    const struct ehca_mr *mr)
-{
-	return ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-				       adapter_handle.handle,    /* r4 */
-				       mr->ipz_mr_handle.handle, /* r5 */
-				       0, 0, 0, 0, 0);
-}
-
-u64 hipz_h_reregister_pmr(const struct ipz_adapter_handle adapter_handle,
-			  const struct ehca_mr *mr,
-			  const u64 vaddr_in,
-			  const u64 length,
-			  const u32 access_ctrl,
-			  const struct ipz_pd pd,
-			  const u64 mr_addr_cb,
-			  struct ehca_mr_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_REREGISTER_PMR, outs,
-				adapter_handle.handle,    /* r4 */
-				mr->ipz_mr_handle.handle, /* r5 */
-				vaddr_in,	          /* r6 */
-				length,                   /* r7 */
-				/* r8 */
-				((((u64)access_ctrl) << 32ULL) | pd.value),
-				mr_addr_cb,               /* r9 */
-				0, 0, 0);
-	outparms->vaddr = outs[1];
-	outparms->lkey = (u32)outs[2];
-	outparms->rkey = (u32)outs[3];
-
-	return ret;
-}
-
-u64 hipz_h_register_smr(const struct ipz_adapter_handle adapter_handle,
-			const struct ehca_mr *mr,
-			const struct ehca_mr *orig_mr,
-			const u64 vaddr_in,
-			const u32 access_ctrl,
-			const struct ipz_pd pd,
-			struct ehca_mr_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_REGISTER_SMR, outs,
-				adapter_handle.handle,            /* r4 */
-				orig_mr->ipz_mr_handle.handle,    /* r5 */
-				vaddr_in,                         /* r6 */
-				(((u64)access_ctrl) << 32ULL),    /* r7 */
-				pd.value,                         /* r8 */
-				0, 0, 0, 0);
-	outparms->handle.handle = outs[0];
-	outparms->lkey = (u32)outs[2];
-	outparms->rkey = (u32)outs[3];
-
-	return ret;
-}
-
-u64 hipz_h_alloc_resource_mw(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mw *mw,
-			     const struct ipz_pd pd,
-			     struct ehca_mw_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_ALLOC_RESOURCE, outs,
-				adapter_handle.handle,      /* r4 */
-				6,                          /* r5 */
-				pd.value,                   /* r6 */
-				0, 0, 0, 0, 0, 0);
-	outparms->handle.handle = outs[0];
-	outparms->rkey = (u32)outs[3];
-
-	return ret;
-}
-
-u64 hipz_h_query_mw(const struct ipz_adapter_handle adapter_handle,
-		    const struct ehca_mw *mw,
-		    struct ehca_mw_hipzout_parms *outparms)
-{
-	u64 ret;
-	unsigned long outs[PLPAR_HCALL9_BUFSIZE];
-
-	ret = ehca_plpar_hcall9(H_QUERY_MW, outs,
-				adapter_handle.handle,    /* r4 */
-				mw->ipz_mw_handle.handle, /* r5 */
-				0, 0, 0, 0, 0, 0, 0);
-	outparms->rkey = (u32)outs[3];
-
-	return ret;
-}
-
-u64 hipz_h_free_resource_mw(const struct ipz_adapter_handle adapter_handle,
-			    const struct ehca_mw *mw)
-{
-	return ehca_plpar_hcall_norets(H_FREE_RESOURCE,
-				       adapter_handle.handle,    /* r4 */
-				       mw->ipz_mw_handle.handle, /* r5 */
-				       0, 0, 0, 0, 0);
-}
-
-u64 hipz_h_error_data(const struct ipz_adapter_handle adapter_handle,
-		      const u64 ressource_handle,
-		      void *rblock,
-		      unsigned long *byte_count)
-{
-	u64 r_cb = __pa(rblock);
-
-	if (r_cb & (EHCA_PAGESIZE-1)) {
-		ehca_gen_err("rblock not page aligned.");
-		return H_PARAMETER;
-	}
-
-	return ehca_plpar_hcall_norets(H_ERROR_DATA,
-				       adapter_handle.handle,
-				       ressource_handle,
-				       r_cb,
-				       0, 0, 0, 0);
-}
-
-u64 hipz_h_eoi(int irq)
-{
-	unsigned long xirr;
-
-	iosync();
-	xirr = (0xffULL << 24) | irq;
-
-	return plpar_hcall_norets(H_EOI, xirr);
-}
diff --git a/drivers/staging/rdma/ehca/hcp_if.h b/drivers/staging/rdma/ehca/hcp_if.h
deleted file mode 100644
index a46e514c367b..000000000000
--- a/drivers/staging/rdma/ehca/hcp_if.h
+++ /dev/null
@@ -1,265 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Firmware Infiniband Interface code for POWER
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Gerd Bayer <gerd.bayer@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __HCP_IF_H__
-#define __HCP_IF_H__
-
-#include "ehca_classes.h"
-#include "ehca_tools.h"
-#include "hipz_hw.h"
-
-/*
- * hipz_h_alloc_resource_eq allocates EQ resources in HW and FW, initialize
- * resources, create the empty EQPT (ring).
- */
-u64 hipz_h_alloc_resource_eq(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_pfeq *pfeq,
-			     const u32 neq_control,
-			     const u32 number_of_entries,
-			     struct ipz_eq_handle *eq_handle,
-			     u32 * act_nr_of_entries,
-			     u32 * act_pages,
-			     u32 * eq_ist);
-
-u64 hipz_h_reset_event(const struct ipz_adapter_handle adapter_handle,
-		       struct ipz_eq_handle eq_handle,
-		       const u64 event_mask);
-/*
- * hipz_h_allocate_resource_cq allocates CQ resources in HW and FW, initialize
- * resources, create the empty CQPT (ring).
- */
-u64 hipz_h_alloc_resource_cq(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_cq *cq,
-			     struct ehca_alloc_cq_parms *param);
-
-
-/*
- * hipz_h_alloc_resource_qp allocates QP resources in HW and FW,
- * initialize resources, create empty QPPTs (2 rings).
- */
-u64 hipz_h_alloc_resource_qp(const struct ipz_adapter_handle adapter_handle,
-			     struct ehca_alloc_qp_parms *parms, int is_user);
-
-u64 hipz_h_query_port(const struct ipz_adapter_handle adapter_handle,
-		      const u8 port_id,
-		      struct hipz_query_port *query_port_response_block);
-
-u64 hipz_h_modify_port(const struct ipz_adapter_handle adapter_handle,
-		       const u8 port_id, const u32 port_cap,
-		       const u8 init_type, const int modify_mask);
-
-u64 hipz_h_query_hca(const struct ipz_adapter_handle adapter_handle,
-		     struct hipz_query_hca *query_hca_rblock);
-
-/*
- * hipz_h_register_rpage internal function in hcp_if.h for all
- * hcp_H_REGISTER_RPAGE calls.
- */
-u64 hipz_h_register_rpage(const struct ipz_adapter_handle adapter_handle,
-			  const u8 pagesize,
-			  const u8 queue_type,
-			  const u64 resource_handle,
-			  const u64 logical_address_of_page,
-			  u64 count);
-
-u64 hipz_h_register_rpage_eq(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_eq_handle eq_handle,
-			     struct ehca_pfeq *pfeq,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count);
-
-u64 hipz_h_query_int_state(const struct ipz_adapter_handle
-			   hcp_adapter_handle,
-			   u32 ist);
-
-u64 hipz_h_register_rpage_cq(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_cq_handle cq_handle,
-			     struct ehca_pfcq *pfcq,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count,
-			     const struct h_galpa gal);
-
-u64 hipz_h_register_rpage_qp(const struct ipz_adapter_handle adapter_handle,
-			     const struct ipz_qp_handle qp_handle,
-			     struct ehca_pfqp *pfqp,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count,
-			     const struct h_galpa galpa);
-
-u64 hipz_h_disable_and_get_wqe(const struct ipz_adapter_handle adapter_handle,
-			       const struct ipz_qp_handle qp_handle,
-			       struct ehca_pfqp *pfqp,
-			       void **log_addr_next_sq_wqe_tb_processed,
-			       void **log_addr_next_rq_wqe_tb_processed,
-			       int dis_and_get_function_code);
-enum hcall_sigt {
-	HCALL_SIGT_NO_CQE = 0,
-	HCALL_SIGT_BY_WQE = 1,
-	HCALL_SIGT_EVERY = 2
-};
-
-u64 hipz_h_modify_qp(const struct ipz_adapter_handle adapter_handle,
-		     const struct ipz_qp_handle qp_handle,
-		     struct ehca_pfqp *pfqp,
-		     const u64 update_mask,
-		     struct hcp_modify_qp_control_block *mqpcb,
-		     struct h_galpa gal);
-
-u64 hipz_h_query_qp(const struct ipz_adapter_handle adapter_handle,
-		    const struct ipz_qp_handle qp_handle,
-		    struct ehca_pfqp *pfqp,
-		    struct hcp_modify_qp_control_block *qqpcb,
-		    struct h_galpa gal);
-
-u64 hipz_h_destroy_qp(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_qp *qp);
-
-u64 hipz_h_define_aqp0(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u32 port);
-
-u64 hipz_h_define_aqp1(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u32 port, u32 * pma_qp_nr,
-		       u32 * bma_qp_nr);
-
-u64 hipz_h_attach_mcqp(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u16 mcg_dlid,
-		       u64 subnet_prefix, u64 interface_id);
-
-u64 hipz_h_detach_mcqp(const struct ipz_adapter_handle adapter_handle,
-		       const struct ipz_qp_handle qp_handle,
-		       struct h_galpa gal,
-		       u16 mcg_dlid,
-		       u64 subnet_prefix, u64 interface_id);
-
-u64 hipz_h_destroy_cq(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_cq *cq,
-		      u8 force_flag);
-
-u64 hipz_h_destroy_eq(const struct ipz_adapter_handle adapter_handle,
-		      struct ehca_eq *eq);
-
-/*
- * hipz_h_alloc_resource_mr allocates MR resources in HW and FW, initialize
- * resources.
- */
-u64 hipz_h_alloc_resource_mr(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mr *mr,
-			     const u64 vaddr,
-			     const u64 length,
-			     const u32 access_ctrl,
-			     const struct ipz_pd pd,
-			     struct ehca_mr_hipzout_parms *outparms);
-
-/* hipz_h_register_rpage_mr registers MR resource pages in HW and FW */
-u64 hipz_h_register_rpage_mr(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mr *mr,
-			     const u8 pagesize,
-			     const u8 queue_type,
-			     const u64 logical_address_of_page,
-			     const u64 count);
-
-/* hipz_h_query_mr queries MR in HW and FW */
-u64 hipz_h_query_mr(const struct ipz_adapter_handle adapter_handle,
-		    const struct ehca_mr *mr,
-		    struct ehca_mr_hipzout_parms *outparms);
-
-/* hipz_h_free_resource_mr frees MR resources in HW and FW */
-u64 hipz_h_free_resource_mr(const struct ipz_adapter_handle adapter_handle,
-			    const struct ehca_mr *mr);
-
-/* hipz_h_reregister_pmr reregisters MR in HW and FW */
-u64 hipz_h_reregister_pmr(const struct ipz_adapter_handle adapter_handle,
-			  const struct ehca_mr *mr,
-			  const u64 vaddr_in,
-			  const u64 length,
-			  const u32 access_ctrl,
-			  const struct ipz_pd pd,
-			  const u64 mr_addr_cb,
-			  struct ehca_mr_hipzout_parms *outparms);
-
-/* hipz_h_register_smr register shared MR in HW and FW */
-u64 hipz_h_register_smr(const struct ipz_adapter_handle adapter_handle,
-			const struct ehca_mr *mr,
-			const struct ehca_mr *orig_mr,
-			const u64 vaddr_in,
-			const u32 access_ctrl,
-			const struct ipz_pd pd,
-			struct ehca_mr_hipzout_parms *outparms);
-
-/*
- * hipz_h_alloc_resource_mw allocates MW resources in HW and FW, initialize
- * resources.
- */
-u64 hipz_h_alloc_resource_mw(const struct ipz_adapter_handle adapter_handle,
-			     const struct ehca_mw *mw,
-			     const struct ipz_pd pd,
-			     struct ehca_mw_hipzout_parms *outparms);
-
-/* hipz_h_query_mw queries MW in HW and FW */
-u64 hipz_h_query_mw(const struct ipz_adapter_handle adapter_handle,
-		    const struct ehca_mw *mw,
-		    struct ehca_mw_hipzout_parms *outparms);
-
-/* hipz_h_free_resource_mw frees MW resources in HW and FW */
-u64 hipz_h_free_resource_mw(const struct ipz_adapter_handle adapter_handle,
-			    const struct ehca_mw *mw);
-
-u64 hipz_h_error_data(const struct ipz_adapter_handle adapter_handle,
-		      const u64 ressource_handle,
-		      void *rblock,
-		      unsigned long *byte_count);
-u64 hipz_h_eoi(int irq);
-
-#endif /* __HCP_IF_H__ */
diff --git a/drivers/staging/rdma/ehca/hcp_phyp.c b/drivers/staging/rdma/ehca/hcp_phyp.c
deleted file mode 100644
index 077376ff3d28..000000000000
--- a/drivers/staging/rdma/ehca/hcp_phyp.c
+++ /dev/null
@@ -1,82 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *   load store abstraction for ehca register access with tracing
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 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 "ehca_classes.h"
-#include "hipz_hw.h"
-
-u64 hcall_map_page(u64 physaddr)
-{
-	return (u64)ioremap(physaddr, EHCA_PAGESIZE);
-}
-
-int hcall_unmap_page(u64 mapaddr)
-{
-	iounmap((volatile void __iomem *) mapaddr);
-	return 0;
-}
-
-int hcp_galpas_ctor(struct h_galpas *galpas, int is_user,
-		    u64 paddr_kernel, u64 paddr_user)
-{
-	if (!is_user) {
-		galpas->kernel.fw_handle = hcall_map_page(paddr_kernel);
-		if (!galpas->kernel.fw_handle)
-			return -ENOMEM;
-	} else
-		galpas->kernel.fw_handle = 0;
-
-	galpas->user.fw_handle = paddr_user;
-
-	return 0;
-}
-
-int hcp_galpas_dtor(struct h_galpas *galpas)
-{
-	if (galpas->kernel.fw_handle) {
-		int ret = hcall_unmap_page(galpas->kernel.fw_handle);
-		if (ret)
-			return ret;
-	}
-
-	galpas->user.fw_handle = galpas->kernel.fw_handle = 0;
-
-	return 0;
-}
diff --git a/drivers/staging/rdma/ehca/hcp_phyp.h b/drivers/staging/rdma/ehca/hcp_phyp.h
deleted file mode 100644
index d1b029910249..000000000000
--- a/drivers/staging/rdma/ehca/hcp_phyp.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  Firmware calls
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Waleri Fomin <fomin@de.ibm.com>
- *           Gerd Bayer <gerd.bayer@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __HCP_PHYP_H__
-#define __HCP_PHYP_H__
-
-
-/*
- * eHCA page (mapped into memory)
- * resource to access eHCA register pages in CPU address space
-*/
-struct h_galpa {
-	u64 fw_handle;
-	/* for pSeries this is a 64bit memory address where
-	   I/O memory is mapped into CPU address space (kv) */
-};
-
-/*
- * resource to access eHCA address space registers, all types
- */
-struct h_galpas {
-	u32 pid;		/*PID of userspace galpa checking */
-	struct h_galpa user;	/* user space accessible resource,
-				   set to 0 if unused */
-	struct h_galpa kernel;	/* kernel space accessible resource,
-				   set to 0 if unused */
-};
-
-static inline u64 hipz_galpa_load(struct h_galpa galpa, u32 offset)
-{
-	u64 addr = galpa.fw_handle + offset;
-	return *(volatile u64 __force *)addr;
-}
-
-static inline void hipz_galpa_store(struct h_galpa galpa, u32 offset, u64 value)
-{
-	u64 addr = galpa.fw_handle + offset;
-	*(volatile u64 __force *)addr = value;
-}
-
-int hcp_galpas_ctor(struct h_galpas *galpas, int is_user,
-		    u64 paddr_kernel, u64 paddr_user);
-
-int hcp_galpas_dtor(struct h_galpas *galpas);
-
-u64 hcall_map_page(u64 physaddr);
-
-int hcall_unmap_page(u64 mapaddr);
-
-#endif
diff --git a/drivers/staging/rdma/ehca/hipz_fns.h b/drivers/staging/rdma/ehca/hipz_fns.h
deleted file mode 100644
index 9dac93d02140..000000000000
--- a/drivers/staging/rdma/ehca/hipz_fns.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  HW abstraction register functions
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __HIPZ_FNS_H__
-#define __HIPZ_FNS_H__
-
-#include "ehca_classes.h"
-#include "hipz_hw.h"
-
-#include "hipz_fns_core.h"
-
-#define hipz_galpa_store_eq(gal, offset, value) \
-	hipz_galpa_store(gal, EQTEMM_OFFSET(offset), value)
-
-#define hipz_galpa_load_eq(gal, offset) \
-	hipz_galpa_load(gal, EQTEMM_OFFSET(offset))
-
-#define hipz_galpa_store_qped(gal, offset, value) \
-	hipz_galpa_store(gal, QPEDMM_OFFSET(offset), value)
-
-#define hipz_galpa_load_qped(gal, offset) \
-	hipz_galpa_load(gal, QPEDMM_OFFSET(offset))
-
-#define hipz_galpa_store_mrmw(gal, offset, value) \
-	hipz_galpa_store(gal, MRMWMM_OFFSET(offset), value)
-
-#define hipz_galpa_load_mrmw(gal, offset) \
-	hipz_galpa_load(gal, MRMWMM_OFFSET(offset))
-
-#endif
diff --git a/drivers/staging/rdma/ehca/hipz_fns_core.h b/drivers/staging/rdma/ehca/hipz_fns_core.h
deleted file mode 100644
index 868735fd3187..000000000000
--- a/drivers/staging/rdma/ehca/hipz_fns_core.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  HW abstraction register functions
- *
- *  Authors: Christoph Raisch <raisch@de.ibm.com>
- *           Heiko J Schick <schickhj@de.ibm.com>
- *           Hoang-Nam Nguyen <hnguyen@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __HIPZ_FNS_CORE_H__
-#define __HIPZ_FNS_CORE_H__
-
-#include "hcp_phyp.h"
-#include "hipz_hw.h"
-
-#define hipz_galpa_store_cq(gal, offset, value) \
-	hipz_galpa_store(gal, CQTEMM_OFFSET(offset), value)
-
-#define hipz_galpa_load_cq(gal, offset) \
-	hipz_galpa_load(gal, CQTEMM_OFFSET(offset))
-
-#define hipz_galpa_store_qp(gal, offset, value) \
-	hipz_galpa_store(gal, QPTEMM_OFFSET(offset), value)
-#define hipz_galpa_load_qp(gal, offset) \
-	hipz_galpa_load(gal, QPTEMM_OFFSET(offset))
-
-static inline void hipz_update_sqa(struct ehca_qp *qp, u16 nr_wqes)
-{
-	/*  ringing doorbell :-) */
-	hipz_galpa_store_qp(qp->galpas.kernel, qpx_sqa,
-			    EHCA_BMASK_SET(QPX_SQADDER, nr_wqes));
-}
-
-static inline void hipz_update_rqa(struct ehca_qp *qp, u16 nr_wqes)
-{
-	/*  ringing doorbell :-) */
-	hipz_galpa_store_qp(qp->galpas.kernel, qpx_rqa,
-			    EHCA_BMASK_SET(QPX_RQADDER, nr_wqes));
-}
-
-static inline void hipz_update_feca(struct ehca_cq *cq, u32 nr_cqes)
-{
-	hipz_galpa_store_cq(cq->galpas.kernel, cqx_feca,
-			    EHCA_BMASK_SET(CQX_FECADDER, nr_cqes));
-}
-
-static inline void hipz_set_cqx_n0(struct ehca_cq *cq, u32 value)
-{
-	u64 cqx_n0_reg;
-
-	hipz_galpa_store_cq(cq->galpas.kernel, cqx_n0,
-			    EHCA_BMASK_SET(CQX_N0_GENERATE_SOLICITED_COMP_EVENT,
-					   value));
-	cqx_n0_reg = hipz_galpa_load_cq(cq->galpas.kernel, cqx_n0);
-}
-
-static inline void hipz_set_cqx_n1(struct ehca_cq *cq, u32 value)
-{
-	u64 cqx_n1_reg;
-
-	hipz_galpa_store_cq(cq->galpas.kernel, cqx_n1,
-			    EHCA_BMASK_SET(CQX_N1_GENERATE_COMP_EVENT, value));
-	cqx_n1_reg = hipz_galpa_load_cq(cq->galpas.kernel, cqx_n1);
-}
-
-#endif /* __HIPZ_FNC_CORE_H__ */
diff --git a/drivers/staging/rdma/ehca/hipz_hw.h b/drivers/staging/rdma/ehca/hipz_hw.h
deleted file mode 100644
index bf996c7acc42..000000000000
--- a/drivers/staging/rdma/ehca/hipz_hw.h
+++ /dev/null
@@ -1,414 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  eHCA register definitions
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __HIPZ_HW_H__
-#define __HIPZ_HW_H__
-
-#include "ehca_tools.h"
-
-#define EHCA_MAX_MTU 4
-
-/* QP Table Entry Memory Map */
-struct hipz_qptemm {
-	u64 qpx_hcr;
-	u64 qpx_c;
-	u64 qpx_herr;
-	u64 qpx_aer;
-/* 0x20*/
-	u64 qpx_sqa;
-	u64 qpx_sqc;
-	u64 qpx_rqa;
-	u64 qpx_rqc;
-/* 0x40*/
-	u64 qpx_st;
-	u64 qpx_pmstate;
-	u64 qpx_pmfa;
-	u64 qpx_pkey;
-/* 0x60*/
-	u64 qpx_pkeya;
-	u64 qpx_pkeyb;
-	u64 qpx_pkeyc;
-	u64 qpx_pkeyd;
-/* 0x80*/
-	u64 qpx_qkey;
-	u64 qpx_dqp;
-	u64 qpx_dlidp;
-	u64 qpx_portp;
-/* 0xa0*/
-	u64 qpx_slidp;
-	u64 qpx_slidpp;
-	u64 qpx_dlida;
-	u64 qpx_porta;
-/* 0xc0*/
-	u64 qpx_slida;
-	u64 qpx_slidpa;
-	u64 qpx_slvl;
-	u64 qpx_ipd;
-/* 0xe0*/
-	u64 qpx_mtu;
-	u64 qpx_lato;
-	u64 qpx_rlimit;
-	u64 qpx_rnrlimit;
-/* 0x100*/
-	u64 qpx_t;
-	u64 qpx_sqhp;
-	u64 qpx_sqptp;
-	u64 qpx_nspsn;
-/* 0x120*/
-	u64 qpx_nspsnhwm;
-	u64 reserved1;
-	u64 qpx_sdsi;
-	u64 qpx_sdsbc;
-/* 0x140*/
-	u64 qpx_sqwsize;
-	u64 qpx_sqwts;
-	u64 qpx_lsn;
-	u64 qpx_nssn;
-/* 0x160 */
-	u64 qpx_mor;
-	u64 qpx_cor;
-	u64 qpx_sqsize;
-	u64 qpx_erc;
-/* 0x180*/
-	u64 qpx_rnrrc;
-	u64 qpx_ernrwt;
-	u64 qpx_rnrresp;
-	u64 qpx_lmsna;
-/* 0x1a0 */
-	u64 qpx_sqhpc;
-	u64 qpx_sqcptp;
-	u64 qpx_sigt;
-	u64 qpx_wqecnt;
-/* 0x1c0*/
-	u64 qpx_rqhp;
-	u64 qpx_rqptp;
-	u64 qpx_rqsize;
-	u64 qpx_nrr;
-/* 0x1e0*/
-	u64 qpx_rdmac;
-	u64 qpx_nrpsn;
-	u64 qpx_lapsn;
-	u64 qpx_lcr;
-/* 0x200*/
-	u64 qpx_rwc;
-	u64 qpx_rwva;
-	u64 qpx_rdsi;
-	u64 qpx_rdsbc;
-/* 0x220*/
-	u64 qpx_rqwsize;
-	u64 qpx_crmsn;
-	u64 qpx_rdd;
-	u64 qpx_larpsn;
-/* 0x240*/
-	u64 qpx_pd;
-	u64 qpx_scqn;
-	u64 qpx_rcqn;
-	u64 qpx_aeqn;
-/* 0x260*/
-	u64 qpx_aaelog;
-	u64 qpx_ram;
-	u64 qpx_rdmaqe0;
-	u64 qpx_rdmaqe1;
-/* 0x280*/
-	u64 qpx_rdmaqe2;
-	u64 qpx_rdmaqe3;
-	u64 qpx_nrpsnhwm;
-/* 0x298*/
-	u64 reserved[(0x400 - 0x298) / 8];
-/* 0x400 extended data */
-	u64 reserved_ext[(0x500 - 0x400) / 8];
-/* 0x500 */
-	u64 reserved2[(0x1000 - 0x500) / 8];
-/* 0x1000      */
-};
-
-#define QPX_SQADDER EHCA_BMASK_IBM(48, 63)
-#define QPX_RQADDER EHCA_BMASK_IBM(48, 63)
-#define QPX_AAELOG_RESET_SRQ_LIMIT EHCA_BMASK_IBM(3, 3)
-
-#define QPTEMM_OFFSET(x) offsetof(struct hipz_qptemm, x)
-
-/* MRMWPT Entry Memory Map */
-struct hipz_mrmwmm {
-	/* 0x00 */
-	u64 mrx_hcr;
-
-	u64 mrx_c;
-	u64 mrx_herr;
-	u64 mrx_aer;
-	/* 0x20 */
-	u64 mrx_pp;
-	u64 reserved1;
-	u64 reserved2;
-	u64 reserved3;
-	/* 0x40 */
-	u64 reserved4[(0x200 - 0x40) / 8];
-	/* 0x200 */
-	u64 mrx_ctl[64];
-
-};
-
-#define MRMWMM_OFFSET(x) offsetof(struct hipz_mrmwmm, x)
-
-struct hipz_qpedmm {
-	/* 0x00 */
-	u64 reserved0[(0x400) / 8];
-	/* 0x400 */
-	u64 qpedx_phh;
-	u64 qpedx_ppsgp;
-	/* 0x410 */
-	u64 qpedx_ppsgu;
-	u64 qpedx_ppdgp;
-	/* 0x420 */
-	u64 qpedx_ppdgu;
-	u64 qpedx_aph;
-	/* 0x430 */
-	u64 qpedx_apsgp;
-	u64 qpedx_apsgu;
-	/* 0x440 */
-	u64 qpedx_apdgp;
-	u64 qpedx_apdgu;
-	/* 0x450 */
-	u64 qpedx_apav;
-	u64 qpedx_apsav;
-	/* 0x460  */
-	u64 qpedx_hcr;
-	u64 reserved1[4];
-	/* 0x488 */
-	u64 qpedx_rrl0;
-	/* 0x490 */
-	u64 qpedx_rrrkey0;
-	u64 qpedx_rrva0;
-	/* 0x4a0 */
-	u64 reserved2;
-	u64 qpedx_rrl1;
-	/* 0x4b0 */
-	u64 qpedx_rrrkey1;
-	u64 qpedx_rrva1;
-	/* 0x4c0 */
-	u64 reserved3;
-	u64 qpedx_rrl2;
-	/* 0x4d0 */
-	u64 qpedx_rrrkey2;
-	u64 qpedx_rrva2;
-	/* 0x4e0 */
-	u64 reserved4;
-	u64 qpedx_rrl3;
-	/* 0x4f0 */
-	u64 qpedx_rrrkey3;
-	u64 qpedx_rrva3;
-};
-
-#define QPEDMM_OFFSET(x) offsetof(struct hipz_qpedmm, x)
-
-/* CQ Table Entry Memory Map */
-struct hipz_cqtemm {
-	u64 cqx_hcr;
-	u64 cqx_c;
-	u64 cqx_herr;
-	u64 cqx_aer;
-/* 0x20  */
-	u64 cqx_ptp;
-	u64 cqx_tp;
-	u64 cqx_fec;
-	u64 cqx_feca;
-/* 0x40  */
-	u64 cqx_ep;
-	u64 cqx_eq;
-/* 0x50  */
-	u64 reserved1;
-	u64 cqx_n0;
-/* 0x60  */
-	u64 cqx_n1;
-	u64 reserved2[(0x1000 - 0x60) / 8];
-/* 0x1000 */
-};
-
-#define CQX_FEC_CQE_CNT           EHCA_BMASK_IBM(32, 63)
-#define CQX_FECADDER              EHCA_BMASK_IBM(32, 63)
-#define CQX_N0_GENERATE_SOLICITED_COMP_EVENT EHCA_BMASK_IBM(0, 0)
-#define CQX_N1_GENERATE_COMP_EVENT EHCA_BMASK_IBM(0, 0)
-
-#define CQTEMM_OFFSET(x) offsetof(struct hipz_cqtemm, x)
-
-/* EQ Table Entry Memory Map */
-struct hipz_eqtemm {
-	u64 eqx_hcr;
-	u64 eqx_c;
-
-	u64 eqx_herr;
-	u64 eqx_aer;
-/* 0x20 */
-	u64 eqx_ptp;
-	u64 eqx_tp;
-	u64 eqx_ssba;
-	u64 eqx_psba;
-
-/* 0x40 */
-	u64 eqx_cec;
-	u64 eqx_meql;
-	u64 eqx_xisbi;
-	u64 eqx_xisc;
-/* 0x60 */
-	u64 eqx_it;
-
-};
-
-#define EQTEMM_OFFSET(x) offsetof(struct hipz_eqtemm, x)
-
-/* access control defines for MR/MW */
-#define HIPZ_ACCESSCTRL_L_WRITE  0x00800000
-#define HIPZ_ACCESSCTRL_R_WRITE  0x00400000
-#define HIPZ_ACCESSCTRL_R_READ   0x00200000
-#define HIPZ_ACCESSCTRL_R_ATOMIC 0x00100000
-#define HIPZ_ACCESSCTRL_MW_BIND  0x00080000
-
-/* query hca response block */
-struct hipz_query_hca {
-	u32 cur_reliable_dg;
-	u32 cur_qp;
-	u32 cur_cq;
-	u32 cur_eq;
-	u32 cur_mr;
-	u32 cur_mw;
-	u32 cur_ee_context;
-	u32 cur_mcast_grp;
-	u32 cur_qp_attached_mcast_grp;
-	u32 reserved1;
-	u32 cur_ipv6_qp;
-	u32 cur_eth_qp;
-	u32 cur_hp_mr;
-	u32 reserved2[3];
-	u32 max_rd_domain;
-	u32 max_qp;
-	u32 max_cq;
-	u32 max_eq;
-	u32 max_mr;
-	u32 max_hp_mr;
-	u32 max_mw;
-	u32 max_mrwpte;
-	u32 max_special_mrwpte;
-	u32 max_rd_ee_context;
-	u32 max_mcast_grp;
-	u32 max_total_mcast_qp_attach;
-	u32 max_mcast_qp_attach;
-	u32 max_raw_ipv6_qp;
-	u32 max_raw_ethy_qp;
-	u32 internal_clock_frequency;
-	u32 max_pd;
-	u32 max_ah;
-	u32 max_cqe;
-	u32 max_wqes_wq;
-	u32 max_partitions;
-	u32 max_rr_ee_context;
-	u32 max_rr_qp;
-	u32 max_rr_hca;
-	u32 max_act_wqs_ee_context;
-	u32 max_act_wqs_qp;
-	u32 max_sge;
-	u32 max_sge_rd;
-	u32 memory_page_size_supported;
-	u64 max_mr_size;
-	u32 local_ca_ack_delay;
-	u32 num_ports;
-	u32 vendor_id;
-	u32 vendor_part_id;
-	u32 hw_ver;
-	u64 node_guid;
-	u64 hca_cap_indicators;
-	u32 data_counter_register_size;
-	u32 max_shared_rq;
-	u32 max_isns_eq;
-	u32 max_neq;
-} __attribute__ ((packed));
-
-#define HCA_CAP_AH_PORT_NR_CHECK      EHCA_BMASK_IBM( 0,  0)
-#define HCA_CAP_ATOMIC                EHCA_BMASK_IBM( 1,  1)
-#define HCA_CAP_AUTO_PATH_MIG         EHCA_BMASK_IBM( 2,  2)
-#define HCA_CAP_BAD_P_KEY_CTR         EHCA_BMASK_IBM( 3,  3)
-#define HCA_CAP_SQD_RTS_PORT_CHANGE   EHCA_BMASK_IBM( 4,  4)
-#define HCA_CAP_CUR_QP_STATE_MOD      EHCA_BMASK_IBM( 5,  5)
-#define HCA_CAP_INIT_TYPE             EHCA_BMASK_IBM( 6,  6)
-#define HCA_CAP_PORT_ACTIVE_EVENT     EHCA_BMASK_IBM( 7,  7)
-#define HCA_CAP_Q_KEY_VIOL_CTR        EHCA_BMASK_IBM( 8,  8)
-#define HCA_CAP_WQE_RESIZE            EHCA_BMASK_IBM( 9,  9)
-#define HCA_CAP_RAW_PACKET_MCAST      EHCA_BMASK_IBM(10, 10)
-#define HCA_CAP_SHUTDOWN_PORT         EHCA_BMASK_IBM(11, 11)
-#define HCA_CAP_RC_LL_QP              EHCA_BMASK_IBM(12, 12)
-#define HCA_CAP_SRQ                   EHCA_BMASK_IBM(13, 13)
-#define HCA_CAP_UD_LL_QP              EHCA_BMASK_IBM(16, 16)
-#define HCA_CAP_RESIZE_MR             EHCA_BMASK_IBM(17, 17)
-#define HCA_CAP_MINI_QP               EHCA_BMASK_IBM(18, 18)
-#define HCA_CAP_H_ALLOC_RES_SYNC      EHCA_BMASK_IBM(19, 19)
-
-/* query port response block */
-struct hipz_query_port {
-	u32 state;
-	u32 bad_pkey_cntr;
-	u32 lmc;
-	u32 lid;
-	u32 subnet_timeout;
-	u32 qkey_viol_cntr;
-	u32 sm_sl;
-	u32 sm_lid;
-	u32 capability_mask;
-	u32 init_type_reply;
-	u32 pkey_tbl_len;
-	u32 gid_tbl_len;
-	u64 gid_prefix;
-	u32 port_nr;
-	u16 pkey_entries[16];
-	u8  reserved1[32];
-	u32 trent_size;
-	u32 trbuf_size;
-	u64 max_msg_sz;
-	u32 max_mtu;
-	u32 vl_cap;
-	u32 phys_pstate;
-	u32 phys_state;
-	u32 phys_speed;
-	u32 phys_width;
-	u8  reserved2[1884];
-	u64 guid_entries[255];
-} __attribute__ ((packed));
-
-#endif
diff --git a/drivers/staging/rdma/ehca/ipz_pt_fn.c b/drivers/staging/rdma/ehca/ipz_pt_fn.c
deleted file mode 100644
index 7ffc748cb973..000000000000
--- a/drivers/staging/rdma/ehca/ipz_pt_fn.c
+++ /dev/null
@@ -1,289 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  internal queue handling
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <linux/slab.h>
-
-#include "ehca_tools.h"
-#include "ipz_pt_fn.h"
-#include "ehca_classes.h"
-
-#define PAGES_PER_KPAGE (PAGE_SIZE >> EHCA_PAGESHIFT)
-
-struct kmem_cache *small_qp_cache;
-
-void *ipz_qpageit_get_inc(struct ipz_queue *queue)
-{
-	void *ret = ipz_qeit_get(queue);
-	queue->current_q_offset += queue->pagesize;
-	if (queue->current_q_offset > queue->queue_length) {
-		queue->current_q_offset -= queue->pagesize;
-		ret = NULL;
-	}
-	if (((u64)ret) % queue->pagesize) {
-		ehca_gen_err("ERROR!! not at PAGE-Boundary");
-		return NULL;
-	}
-	return ret;
-}
-
-void *ipz_qeit_eq_get_inc(struct ipz_queue *queue)
-{
-	void *ret = ipz_qeit_get(queue);
-	u64 last_entry_in_q = queue->queue_length - queue->qe_size;
-
-	queue->current_q_offset += queue->qe_size;
-	if (queue->current_q_offset > last_entry_in_q) {
-		queue->current_q_offset = 0;
-		queue->toggle_state = (~queue->toggle_state) & 1;
-	}
-
-	return ret;
-}
-
-int ipz_queue_abs_to_offset(struct ipz_queue *queue, u64 addr, u64 *q_offset)
-{
-	int i;
-	for (i = 0; i < queue->queue_length / queue->pagesize; i++) {
-		u64 page = __pa(queue->queue_pages[i]);
-		if (addr >= page && addr < page + queue->pagesize) {
-			*q_offset = addr - page + i * queue->pagesize;
-			return 0;
-		}
-	}
-	return -EINVAL;
-}
-
-#if PAGE_SHIFT < EHCA_PAGESHIFT
-#error Kernel pages must be at least as large than eHCA pages (4K) !
-#endif
-
-/*
- * allocate pages for queue:
- * outer loop allocates whole kernel pages (page aligned) and
- * inner loop divides a kernel page into smaller hca queue pages
- */
-static int alloc_queue_pages(struct ipz_queue *queue, const u32 nr_of_pages)
-{
-	int k, f = 0;
-	u8 *kpage;
-
-	while (f < nr_of_pages) {
-		kpage = (u8 *)get_zeroed_page(GFP_KERNEL);
-		if (!kpage)
-			goto out;
-
-		for (k = 0; k < PAGES_PER_KPAGE && f < nr_of_pages; k++) {
-			queue->queue_pages[f] = (struct ipz_page *)kpage;
-			kpage += EHCA_PAGESIZE;
-			f++;
-		}
-	}
-	return 1;
-
-out:
-	for (f = 0; f < nr_of_pages && queue->queue_pages[f];
-	     f += PAGES_PER_KPAGE)
-		free_page((unsigned long)(queue->queue_pages)[f]);
-	return 0;
-}
-
-static int alloc_small_queue_page(struct ipz_queue *queue, struct ehca_pd *pd)
-{
-	int order = ilog2(queue->pagesize) - 9;
-	struct ipz_small_queue_page *page;
-	unsigned long bit;
-
-	mutex_lock(&pd->lock);
-
-	if (!list_empty(&pd->free[order]))
-		page = list_entry(pd->free[order].next,
-				  struct ipz_small_queue_page, list);
-	else {
-		page = kmem_cache_zalloc(small_qp_cache, GFP_KERNEL);
-		if (!page)
-			goto out;
-
-		page->page = get_zeroed_page(GFP_KERNEL);
-		if (!page->page) {
-			kmem_cache_free(small_qp_cache, page);
-			goto out;
-		}
-
-		list_add(&page->list, &pd->free[order]);
-	}
-
-	bit = find_first_zero_bit(page->bitmap, IPZ_SPAGE_PER_KPAGE >> order);
-	__set_bit(bit, page->bitmap);
-	page->fill++;
-
-	if (page->fill == IPZ_SPAGE_PER_KPAGE >> order)
-		list_move(&page->list, &pd->full[order]);
-
-	mutex_unlock(&pd->lock);
-
-	queue->queue_pages[0] = (void *)(page->page | (bit << (order + 9)));
-	queue->small_page = page;
-	queue->offset = bit << (order + 9);
-	return 1;
-
-out:
-	ehca_err(pd->ib_pd.device, "failed to allocate small queue page");
-	mutex_unlock(&pd->lock);
-	return 0;
-}
-
-static void free_small_queue_page(struct ipz_queue *queue, struct ehca_pd *pd)
-{
-	int order = ilog2(queue->pagesize) - 9;
-	struct ipz_small_queue_page *page = queue->small_page;
-	unsigned long bit;
-	int free_page = 0;
-
-	bit = ((unsigned long)queue->queue_pages[0] & ~PAGE_MASK)
-		>> (order + 9);
-
-	mutex_lock(&pd->lock);
-
-	__clear_bit(bit, page->bitmap);
-	page->fill--;
-
-	if (page->fill == 0) {
-		list_del(&page->list);
-		free_page = 1;
-	}
-
-	if (page->fill == (IPZ_SPAGE_PER_KPAGE >> order) - 1)
-		/* the page was full until we freed the chunk */
-		list_move_tail(&page->list, &pd->free[order]);
-
-	mutex_unlock(&pd->lock);
-
-	if (free_page) {
-		free_page(page->page);
-		kmem_cache_free(small_qp_cache, page);
-	}
-}
-
-int ipz_queue_ctor(struct ehca_pd *pd, struct ipz_queue *queue,
-		   const u32 nr_of_pages, const u32 pagesize,
-		   const u32 qe_size, const u32 nr_of_sg,
-		   int is_small)
-{
-	if (pagesize > PAGE_SIZE) {
-		ehca_gen_err("FATAL ERROR: pagesize=%x "
-			     "is greater than kernel page size", pagesize);
-		return 0;
-	}
-
-	/* init queue fields */
-	queue->queue_length = nr_of_pages * pagesize;
-	queue->pagesize = pagesize;
-	queue->qe_size = qe_size;
-	queue->act_nr_of_sg = nr_of_sg;
-	queue->current_q_offset = 0;
-	queue->toggle_state = 1;
-	queue->small_page = NULL;
-
-	/* allocate queue page pointers */
-	queue->queue_pages = kzalloc(nr_of_pages * sizeof(void *),
-				     GFP_KERNEL | __GFP_NOWARN);
-	if (!queue->queue_pages) {
-		queue->queue_pages = vzalloc(nr_of_pages * sizeof(void *));
-		if (!queue->queue_pages) {
-			ehca_gen_err("Couldn't allocate queue page list");
-			return 0;
-		}
-	}
-
-	/* allocate actual queue pages */
-	if (is_small) {
-		if (!alloc_small_queue_page(queue, pd))
-			goto ipz_queue_ctor_exit0;
-	} else
-		if (!alloc_queue_pages(queue, nr_of_pages))
-			goto ipz_queue_ctor_exit0;
-
-	return 1;
-
-ipz_queue_ctor_exit0:
-	ehca_gen_err("Couldn't alloc pages queue=%p "
-		 "nr_of_pages=%x",  queue, nr_of_pages);
-	kvfree(queue->queue_pages);
-
-	return 0;
-}
-
-int ipz_queue_dtor(struct ehca_pd *pd, struct ipz_queue *queue)
-{
-	int i, nr_pages;
-
-	if (!queue || !queue->queue_pages) {
-		ehca_gen_dbg("queue or queue_pages is NULL");
-		return 0;
-	}
-
-	if (queue->small_page)
-		free_small_queue_page(queue, pd);
-	else {
-		nr_pages = queue->queue_length / queue->pagesize;
-		for (i = 0; i < nr_pages; i += PAGES_PER_KPAGE)
-			free_page((unsigned long)queue->queue_pages[i]);
-	}
-
-	kvfree(queue->queue_pages);
-
-	return 1;
-}
-
-int ehca_init_small_qp_cache(void)
-{
-	small_qp_cache = kmem_cache_create("ehca_cache_small_qp",
-					   sizeof(struct ipz_small_queue_page),
-					   0, SLAB_HWCACHE_ALIGN, NULL);
-	if (!small_qp_cache)
-		return -ENOMEM;
-
-	return 0;
-}
-
-void ehca_cleanup_small_qp_cache(void)
-{
-	kmem_cache_destroy(small_qp_cache);
-}
diff --git a/drivers/staging/rdma/ehca/ipz_pt_fn.h b/drivers/staging/rdma/ehca/ipz_pt_fn.h
deleted file mode 100644
index a801274ea337..000000000000
--- a/drivers/staging/rdma/ehca/ipz_pt_fn.h
+++ /dev/null
@@ -1,289 +0,0 @@
-/*
- *  IBM eServer eHCA Infiniband device driver for Linux on POWER
- *
- *  internal queue handling
- *
- *  Authors: Waleri Fomin <fomin@de.ibm.com>
- *           Reinhard Ernst <rernst@de.ibm.com>
- *           Christoph Raisch <raisch@de.ibm.com>
- *
- *  Copyright (c) 2005 IBM Corporation
- *
- *  All rights reserved.
- *
- *  This source code is distributed under a dual license of GPL v2.0 and OpenIB
- *  BSD.
- *
- * OpenIB BSD License
- *
- * 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 OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
- * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef __IPZ_PT_FN_H__
-#define __IPZ_PT_FN_H__
-
-#define EHCA_PAGESHIFT   12
-#define EHCA_PAGESIZE   4096UL
-#define EHCA_PAGEMASK   (~(EHCA_PAGESIZE-1))
-#define EHCA_PT_ENTRIES 512UL
-
-#include "ehca_tools.h"
-#include "ehca_qes.h"
-
-struct ehca_pd;
-struct ipz_small_queue_page;
-
-extern struct kmem_cache *small_qp_cache;
-
-/* struct generic ehca page */
-struct ipz_page {
-	u8 entries[EHCA_PAGESIZE];
-};
-
-#define IPZ_SPAGE_PER_KPAGE (PAGE_SIZE / 512)
-
-struct ipz_small_queue_page {
-	unsigned long page;
-	unsigned long bitmap[IPZ_SPAGE_PER_KPAGE / BITS_PER_LONG];
-	int fill;
-	void *mapped_addr;
-	u32 mmap_count;
-	struct list_head list;
-};
-
-/* struct generic queue in linux kernel virtual memory (kv) */
-struct ipz_queue {
-	u64 current_q_offset;	/* current queue entry */
-
-	struct ipz_page **queue_pages;	/* array of pages belonging to queue */
-	u32 qe_size;		/* queue entry size */
-	u32 act_nr_of_sg;
-	u32 queue_length;	/* queue length allocated in bytes */
-	u32 pagesize;
-	u32 toggle_state;	/* toggle flag - per page */
-	u32 offset; /* save offset within page for small_qp */
-	struct ipz_small_queue_page *small_page;
-};
-
-/*
- * return current Queue Entry for a certain q_offset
- * returns address (kv) of Queue Entry
- */
-static inline void *ipz_qeit_calc(struct ipz_queue *queue, u64 q_offset)
-{
-	struct ipz_page *current_page;
-	if (q_offset >= queue->queue_length)
-		return NULL;
-	current_page = (queue->queue_pages)[q_offset >> EHCA_PAGESHIFT];
-	return &current_page->entries[q_offset & (EHCA_PAGESIZE - 1)];
-}
-
-/*
- * return current Queue Entry
- * returns address (kv) of Queue Entry
- */
-static inline void *ipz_qeit_get(struct ipz_queue *queue)
-{
-	return ipz_qeit_calc(queue, queue->current_q_offset);
-}
-
-/*
- * return current Queue Page , increment Queue Page iterator from
- * page to page in struct ipz_queue, last increment will return 0! and
- * NOT wrap
- * returns address (kv) of Queue Page
- * warning don't use in parallel with ipz_QE_get_inc()
- */
-void *ipz_qpageit_get_inc(struct ipz_queue *queue);
-
-/*
- * return current Queue Entry, increment Queue Entry iterator by one
- * step in struct ipz_queue, will wrap in ringbuffer
- * returns address (kv) of Queue Entry BEFORE increment
- * warning don't use in parallel with ipz_qpageit_get_inc()
- */
-static inline void *ipz_qeit_get_inc(struct ipz_queue *queue)
-{
-	void *ret = ipz_qeit_get(queue);
-	queue->current_q_offset += queue->qe_size;
-	if (queue->current_q_offset >= queue->queue_length) {
-		queue->current_q_offset = 0;
-		/* toggle the valid flag */
-		queue->toggle_state = (~queue->toggle_state) & 1;
-	}
-
-	return ret;
-}
-
-/*
- * return a bool indicating whether current Queue Entry is valid
- */
-static inline int ipz_qeit_is_valid(struct ipz_queue *queue)
-{
-	struct ehca_cqe *cqe = ipz_qeit_get(queue);
-	return ((cqe->cqe_flags >> 7) == (queue->toggle_state & 1));
-}
-
-/*
- * return current Queue Entry, increment Queue Entry iterator by one
- * step in struct ipz_queue, will wrap in ringbuffer
- * returns address (kv) of Queue Entry BEFORE increment
- * returns 0 and does not increment, if wrong valid state
- * warning don't use in parallel with ipz_qpageit_get_inc()
- */
-static inline void *ipz_qeit_get_inc_valid(struct ipz_queue *queue)
-{
-	return ipz_qeit_is_valid(queue) ? ipz_qeit_get_inc(queue) : NULL;
-}
-
-/*
- * returns and resets Queue Entry iterator
- * returns address (kv) of first Queue Entry
- */
-static inline void *ipz_qeit_reset(struct ipz_queue *queue)
-{
-	queue->current_q_offset = 0;
-	return ipz_qeit_get(queue);
-}
-
-/*
- * return the q_offset corresponding to an absolute address
- */
-int ipz_queue_abs_to_offset(struct ipz_queue *queue, u64 addr, u64 *q_offset);
-
-/*
- * return the next queue offset. don't modify the queue.
- */
-static inline u64 ipz_queue_advance_offset(struct ipz_queue *queue, u64 offset)
-{
-	offset += queue->qe_size;
-	if (offset >= queue->queue_length) offset = 0;
-	return offset;
-}
-
-/* struct generic page table */
-struct ipz_pt {
-	u64 entries[EHCA_PT_ENTRIES];
-};
-
-/* struct page table for a queue, only to be used in pf */
-struct ipz_qpt {
-	/* queue page tables (kv), use u64 because we know the element length */
-	u64 *qpts;
-	u32 n_qpts;
-	u32 n_ptes;       /*  number of page table entries */
-	u64 *current_pte_addr;
-};
-
-/*
- * constructor for a ipz_queue_t, placement new for ipz_queue_t,
- * new for all dependent datastructors
- * all QP Tables are the same
- * flow:
- *    allocate+pin queue
- * see ipz_qpt_ctor()
- * returns true if ok, false if out of memory
- */
-int ipz_queue_ctor(struct ehca_pd *pd, struct ipz_queue *queue,
-		   const u32 nr_of_pages, const u32 pagesize,
-		   const u32 qe_size, const u32 nr_of_sg,
-		   int is_small);
-
-/*
- * destructor for a ipz_queue_t
- *  -# free queue
- *  see ipz_queue_ctor()
- *  returns true if ok, false if queue was NULL-ptr of free failed
- */
-int ipz_queue_dtor(struct ehca_pd *pd, struct ipz_queue *queue);
-
-/*
- * constructor for a ipz_qpt_t,
- * placement new for struct ipz_queue, new for all dependent datastructors
- * all QP Tables are the same,
- * flow:
- * -# allocate+pin queue
- * -# initialise ptcb
- * -# allocate+pin PTs
- * -# link PTs to a ring, according to HCA Arch, set bit62 id needed
- * -# the ring must have room for exactly nr_of_PTEs
- * see ipz_qpt_ctor()
- */
-void ipz_qpt_ctor(struct ipz_qpt *qpt,
-		  const u32 nr_of_qes,
-		  const u32 pagesize,
-		  const u32 qe_size,
-		  const u8 lowbyte, const u8 toggle,
-		  u32 * act_nr_of_QEs, u32 * act_nr_of_pages);
-
-/*
- * return current Queue Entry, increment Queue Entry iterator by one
- * step in struct ipz_queue, will wrap in ringbuffer
- * returns address (kv) of Queue Entry BEFORE increment
- * warning don't use in parallel with ipz_qpageit_get_inc()
- * warning unpredictable results may occur if steps>act_nr_of_queue_entries
- * fix EQ page problems
- */
-void *ipz_qeit_eq_get_inc(struct ipz_queue *queue);
-
-/*
- * return current Event Queue Entry, increment Queue Entry iterator
- * by one step in struct ipz_queue if valid, will wrap in ringbuffer
- * returns address (kv) of Queue Entry BEFORE increment
- * returns 0 and does not increment, if wrong valid state
- * warning don't use in parallel with ipz_queue_QPageit_get_inc()
- * warning unpredictable results may occur if steps>act_nr_of_queue_entries
- */
-static inline void *ipz_eqit_eq_get_inc_valid(struct ipz_queue *queue)
-{
-	void *ret = ipz_qeit_get(queue);
-	u32 qe = *(u8 *)ret;
-	if ((qe >> 7) != (queue->toggle_state & 1))
-		return NULL;
-	ipz_qeit_eq_get_inc(queue); /* this is a good one */
-	return ret;
-}
-
-static inline void *ipz_eqit_eq_peek_valid(struct ipz_queue *queue)
-{
-	void *ret = ipz_qeit_get(queue);
-	u32 qe = *(u8 *)ret;
-	if ((qe >> 7) != (queue->toggle_state & 1))
-		return NULL;
-	return ret;
-}
-
-/* returns address (GX) of first queue entry */
-static inline u64 ipz_qpt_get_firstpage(struct ipz_qpt *qpt)
-{
-	return be64_to_cpu(qpt->qpts[0]);
-}
-
-/* returns address (kv) of first page of queue page table */
-static inline void *ipz_qpt_get_qpt(struct ipz_qpt *qpt)
-{
-	return qpt->qpts;
-}
-
-#endif				/* __IPZ_PT_FN_H__ */
diff --git a/drivers/staging/rdma/hfi1/Makefile b/drivers/staging/rdma/hfi1/Makefile
index 2e5daa6cdcc2..68c5a315e557 100644
--- a/drivers/staging/rdma/hfi1/Makefile
+++ b/drivers/staging/rdma/hfi1/Makefile
@@ -7,7 +7,7 @@
 #
 obj-$(CONFIG_INFINIBAND_HFI1) += hfi1.o
 
-hfi1-y := chip.o cq.o device.o diag.o dma.o driver.o eprom.o file_ops.o firmware.o \
+hfi1-y := chip.o cq.o device.o diag.o dma.o driver.o efivar.o eprom.o file_ops.o firmware.o \
 	init.o intr.o keys.o mad.o mmap.o mr.o pcie.o pio.o pio_copy.o \
 	qp.o qsfp.o rc.o ruc.o sdma.o srq.o sysfs.o trace.o twsi.o \
 	uc.o ud.o user_pages.o user_sdma.o verbs_mcast.o verbs.o
diff --git a/drivers/staging/rdma/hfi1/chip.c b/drivers/staging/rdma/hfi1/chip.c
index e48981994b10..bbe5ad85cec0 100644
--- a/drivers/staging/rdma/hfi1/chip.c
+++ b/drivers/staging/rdma/hfi1/chip.c
@@ -63,6 +63,7 @@
 #include "pio.h"
 #include "sdma.h"
 #include "eprom.h"
+#include "efivar.h"
 
 #define NUM_IB_PORTS 1
 
@@ -121,8 +122,8 @@ struct flag_table {
 #define SEC_SC_HALTED		0x4	/* per-context only */
 #define SEC_SPC_FREEZE		0x8	/* per-HFI only */
 
-#define VL15CTXT                  1
 #define MIN_KERNEL_KCTXTS         2
+#define FIRST_KERNEL_KCTXT        1
 #define NUM_MAP_REGS             32
 
 /* Bit offset into the GUID which carries HFI id information */
@@ -663,7 +664,7 @@ static struct flag_table egress_err_info_flags[] = {
  */
 #define SES(name) SEND_ERR_STATUS_SEND_##name##_ERR_SMASK
 static struct flag_table send_err_status_flags[] = {
-/* 0*/	FLAG_ENTRY0("SDmaRpyTagErr", SES(CSR_PARITY)),
+/* 0*/	FLAG_ENTRY0("SendCsrParityErr", SES(CSR_PARITY)),
 /* 1*/	FLAG_ENTRY0("SendCsrReadBadAddrErr", SES(CSR_READ_BAD_ADDR)),
 /* 2*/	FLAG_ENTRY0("SendCsrWriteBadAddrErr", SES(CSR_WRITE_BAD_ADDR))
 };
@@ -1417,6 +1418,17 @@ static u64 access_sw_link_up_cnt(const struct cntr_entry *entry, void *context,
 	return read_write_sw(ppd->dd, &ppd->link_up, mode, data);
 }
 
+static u64 access_sw_unknown_frame_cnt(const struct cntr_entry *entry,
+				       void *context, int vl, int mode,
+				       u64 data)
+{
+	struct hfi1_pportdata *ppd = (struct hfi1_pportdata *)context;
+
+	if (vl != CNTR_INVALID_VL)
+		return 0;
+	return read_write_sw(ppd->dd, &ppd->unknown_frame_count, mode, data);
+}
+
 static u64 access_sw_xmit_discards(const struct cntr_entry *entry,
 				    void *context, int vl, int mode, u64 data)
 {
@@ -1538,6 +1550,2336 @@ static u64 access_sw_send_schedule(const struct cntr_entry *entry,
 	return dd->verbs_dev.n_send_schedule;
 }
 
+/* Software counters for the error status bits within MISC_ERR_STATUS */
+static u64 access_misc_pll_lock_fail_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[12];
+}
+
+static u64 access_misc_mbist_fail_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[11];
+}
+
+static u64 access_misc_invalid_eep_cmd_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[10];
+}
+
+static u64 access_misc_efuse_done_parity_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[9];
+}
+
+static u64 access_misc_efuse_write_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[8];
+}
+
+static u64 access_misc_efuse_read_bad_addr_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[7];
+}
+
+static u64 access_misc_efuse_csr_parity_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[6];
+}
+
+static u64 access_misc_fw_auth_failed_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[5];
+}
+
+static u64 access_misc_key_mismatch_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[4];
+}
+
+static u64 access_misc_sbus_write_failed_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[3];
+}
+
+static u64 access_misc_csr_write_bad_addr_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[2];
+}
+
+static u64 access_misc_csr_read_bad_addr_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[1];
+}
+
+static u64 access_misc_csr_parity_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->misc_err_status_cnt[0];
+}
+
+/*
+ * Software counter for the aggregate of
+ * individual CceErrStatus counters
+ */
+static u64 access_sw_cce_err_status_aggregated_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_cce_err_status_aggregate;
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within CceErrStatus
+ */
+static u64 access_cce_msix_csr_parity_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[40];
+}
+
+static u64 access_cce_int_map_unc_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[39];
+}
+
+static u64 access_cce_int_map_cor_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[38];
+}
+
+static u64 access_cce_msix_table_unc_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[37];
+}
+
+static u64 access_cce_msix_table_cor_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[36];
+}
+
+static u64 access_cce_rxdma_conv_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[35];
+}
+
+static u64 access_cce_rcpl_async_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[34];
+}
+
+static u64 access_cce_seg_write_bad_addr_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[33];
+}
+
+static u64 access_cce_seg_read_bad_addr_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl, int mode,
+						u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[32];
+}
+
+static u64 access_la_triggered_cnt(const struct cntr_entry *entry,
+				   void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[31];
+}
+
+static u64 access_cce_trgt_cpl_timeout_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[30];
+}
+
+static u64 access_pcic_receive_parity_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[29];
+}
+
+static u64 access_pcic_transmit_back_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[28];
+}
+
+static u64 access_pcic_transmit_front_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[27];
+}
+
+static u64 access_pcic_cpl_dat_q_unc_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[26];
+}
+
+static u64 access_pcic_cpl_hd_q_unc_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[25];
+}
+
+static u64 access_pcic_post_dat_q_unc_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[24];
+}
+
+static u64 access_pcic_post_hd_q_unc_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[23];
+}
+
+static u64 access_pcic_retry_sot_mem_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[22];
+}
+
+static u64 access_pcic_retry_mem_unc_err(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[21];
+}
+
+static u64 access_pcic_n_post_dat_q_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[20];
+}
+
+static u64 access_pcic_n_post_h_q_parity_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[19];
+}
+
+static u64 access_pcic_cpl_dat_q_cor_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[18];
+}
+
+static u64 access_pcic_cpl_hd_q_cor_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[17];
+}
+
+static u64 access_pcic_post_dat_q_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[16];
+}
+
+static u64 access_pcic_post_hd_q_cor_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[15];
+}
+
+static u64 access_pcic_retry_sot_mem_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[14];
+}
+
+static u64 access_pcic_retry_mem_cor_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[13];
+}
+
+static u64 access_cce_cli1_async_fifo_dbg_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[12];
+}
+
+static u64 access_cce_cli1_async_fifo_rxdma_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[11];
+}
+
+static u64 access_cce_cli1_async_fifo_sdma_hd_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[10];
+}
+
+static u64 access_cce_cl1_async_fifo_pio_crdt_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[9];
+}
+
+static u64 access_cce_cli2_async_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[8];
+}
+
+static u64 access_cce_csr_cfg_bus_parity_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[7];
+}
+
+static u64 access_cce_cli0_async_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[6];
+}
+
+static u64 access_cce_rspd_data_parity_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[5];
+}
+
+static u64 access_cce_trgt_access_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[4];
+}
+
+static u64 access_cce_trgt_async_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[3];
+}
+
+static u64 access_cce_csr_write_bad_addr_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[2];
+}
+
+static u64 access_cce_csr_read_bad_addr_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[1];
+}
+
+static u64 access_ccs_csr_parity_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->cce_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within RcvErrStatus
+ */
+static u64 access_rx_csr_parity_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[63];
+}
+
+static u64 access_rx_csr_write_bad_addr_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[62];
+}
+
+static u64 access_rx_csr_read_bad_addr_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[61];
+}
+
+static u64 access_rx_dma_csr_unc_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[60];
+}
+
+static u64 access_rx_dma_dq_fsm_encoding_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[59];
+}
+
+static u64 access_rx_dma_eq_fsm_encoding_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[58];
+}
+
+static u64 access_rx_dma_csr_parity_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[57];
+}
+
+static u64 access_rx_rbuf_data_cor_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[56];
+}
+
+static u64 access_rx_rbuf_data_unc_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[55];
+}
+
+static u64 access_rx_dma_data_fifo_rd_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[54];
+}
+
+static u64 access_rx_dma_data_fifo_rd_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[53];
+}
+
+static u64 access_rx_dma_hdr_fifo_rd_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[52];
+}
+
+static u64 access_rx_dma_hdr_fifo_rd_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[51];
+}
+
+static u64 access_rx_rbuf_desc_part2_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[50];
+}
+
+static u64 access_rx_rbuf_desc_part2_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[49];
+}
+
+static u64 access_rx_rbuf_desc_part1_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[48];
+}
+
+static u64 access_rx_rbuf_desc_part1_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[47];
+}
+
+static u64 access_rx_hq_intr_fsm_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[46];
+}
+
+static u64 access_rx_hq_intr_csr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[45];
+}
+
+static u64 access_rx_lookup_csr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[44];
+}
+
+static u64 access_rx_lookup_rcv_array_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[43];
+}
+
+static u64 access_rx_lookup_rcv_array_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[42];
+}
+
+static u64 access_rx_lookup_des_part2_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[41];
+}
+
+static u64 access_rx_lookup_des_part1_unc_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[40];
+}
+
+static u64 access_rx_lookup_des_part1_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[39];
+}
+
+static u64 access_rx_rbuf_next_free_buf_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[38];
+}
+
+static u64 access_rx_rbuf_next_free_buf_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[37];
+}
+
+static u64 access_rbuf_fl_init_wr_addr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[36];
+}
+
+static u64 access_rx_rbuf_fl_initdone_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[35];
+}
+
+static u64 access_rx_rbuf_fl_write_addr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[34];
+}
+
+static u64 access_rx_rbuf_fl_rd_addr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[33];
+}
+
+static u64 access_rx_rbuf_empty_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[32];
+}
+
+static u64 access_rx_rbuf_full_err_cnt(const struct cntr_entry *entry,
+				       void *context, int vl, int mode,
+				       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[31];
+}
+
+static u64 access_rbuf_bad_lookup_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[30];
+}
+
+static u64 access_rbuf_ctx_id_parity_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[29];
+}
+
+static u64 access_rbuf_csr_qeopdw_parity_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[28];
+}
+
+static u64 access_rx_rbuf_csr_q_num_of_pkt_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[27];
+}
+
+static u64 access_rx_rbuf_csr_q_t1_ptr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[26];
+}
+
+static u64 access_rx_rbuf_csr_q_hd_ptr_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[25];
+}
+
+static u64 access_rx_rbuf_csr_q_vld_bit_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[24];
+}
+
+static u64 access_rx_rbuf_csr_q_next_buf_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[23];
+}
+
+static u64 access_rx_rbuf_csr_q_ent_cnt_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[22];
+}
+
+static u64 access_rx_rbuf_csr_q_head_buf_num_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[21];
+}
+
+static u64 access_rx_rbuf_block_list_read_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[20];
+}
+
+static u64 access_rx_rbuf_block_list_read_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[19];
+}
+
+static u64 access_rx_rbuf_lookup_des_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[18];
+}
+
+static u64 access_rx_rbuf_lookup_des_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[17];
+}
+
+static u64 access_rx_rbuf_lookup_des_reg_unc_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[16];
+}
+
+static u64 access_rx_rbuf_lookup_des_reg_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[15];
+}
+
+static u64 access_rx_rbuf_free_list_cor_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[14];
+}
+
+static u64 access_rx_rbuf_free_list_unc_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[13];
+}
+
+static u64 access_rx_rcv_fsm_encoding_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[12];
+}
+
+static u64 access_rx_dma_flag_cor_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[11];
+}
+
+static u64 access_rx_dma_flag_unc_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[10];
+}
+
+static u64 access_rx_dc_sop_eop_parity_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[9];
+}
+
+static u64 access_rx_rcv_csr_parity_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[8];
+}
+
+static u64 access_rx_rcv_qp_map_table_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[7];
+}
+
+static u64 access_rx_rcv_qp_map_table_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[6];
+}
+
+static u64 access_rx_rcv_data_cor_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[5];
+}
+
+static u64 access_rx_rcv_data_unc_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[4];
+}
+
+static u64 access_rx_rcv_hdr_cor_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[3];
+}
+
+static u64 access_rx_rcv_hdr_unc_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[2];
+}
+
+static u64 access_rx_dc_intf_parity_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[1];
+}
+
+static u64 access_rx_dma_csr_cor_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->rcv_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendPioErrStatus
+ */
+static u64 access_pio_pec_sop_head_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[35];
+}
+
+static u64 access_pio_pcc_sop_head_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[34];
+}
+
+static u64 access_pio_last_returned_cnt_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[33];
+}
+
+static u64 access_pio_current_free_cnt_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[32];
+}
+
+static u64 access_pio_reserved_31_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[31];
+}
+
+static u64 access_pio_reserved_30_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[30];
+}
+
+static u64 access_pio_ppmc_sop_len_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[29];
+}
+
+static u64 access_pio_ppmc_bqc_mem_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[28];
+}
+
+static u64 access_pio_vl_fifo_parity_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[27];
+}
+
+static u64 access_pio_vlf_sop_parity_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[26];
+}
+
+static u64 access_pio_vlf_v1_len_parity_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[25];
+}
+
+static u64 access_pio_block_qw_count_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[24];
+}
+
+static u64 access_pio_write_qw_valid_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[23];
+}
+
+static u64 access_pio_state_machine_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[22];
+}
+
+static u64 access_pio_write_data_parity_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[21];
+}
+
+static u64 access_pio_host_addr_mem_cor_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[20];
+}
+
+static u64 access_pio_host_addr_mem_unc_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[19];
+}
+
+static u64 access_pio_pkt_evict_sm_or_arb_sm_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[18];
+}
+
+static u64 access_pio_init_sm_in_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[17];
+}
+
+static u64 access_pio_ppmc_pbl_fifo_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[16];
+}
+
+static u64 access_pio_credit_ret_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[15];
+}
+
+static u64 access_pio_v1_len_mem_bank1_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[14];
+}
+
+static u64 access_pio_v1_len_mem_bank0_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[13];
+}
+
+static u64 access_pio_v1_len_mem_bank1_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[12];
+}
+
+static u64 access_pio_v1_len_mem_bank0_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[11];
+}
+
+static u64 access_pio_sm_pkt_reset_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[10];
+}
+
+static u64 access_pio_pkt_evict_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[9];
+}
+
+static u64 access_pio_sbrdctrl_crrel_fifo_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[8];
+}
+
+static u64 access_pio_sbrdctl_crrel_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[7];
+}
+
+static u64 access_pio_pec_fifo_parity_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[6];
+}
+
+static u64 access_pio_pcc_fifo_parity_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[5];
+}
+
+static u64 access_pio_sb_mem_fifo1_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[4];
+}
+
+static u64 access_pio_sb_mem_fifo0_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[3];
+}
+
+static u64 access_pio_csr_parity_err_cnt(const struct cntr_entry *entry,
+					 void *context, int vl, int mode,
+					 u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[2];
+}
+
+static u64 access_pio_write_addr_parity_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[1];
+}
+
+static u64 access_pio_write_bad_ctxt_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_pio_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendDmaErrStatus
+ */
+static u64 access_sdma_pcie_req_tracking_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_dma_err_status_cnt[3];
+}
+
+static u64 access_sdma_pcie_req_tracking_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_dma_err_status_cnt[2];
+}
+
+static u64 access_sdma_csr_parity_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_dma_err_status_cnt[1];
+}
+
+static u64 access_sdma_rpy_tag_err_cnt(const struct cntr_entry *entry,
+				       void *context, int vl, int mode,
+				       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_dma_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendEgressErrStatus
+ */
+static u64 access_tx_read_pio_memory_csr_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[63];
+}
+
+static u64 access_tx_read_sdma_memory_csr_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[62];
+}
+
+static u64 access_tx_egress_fifo_cor_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[61];
+}
+
+static u64 access_tx_read_pio_memory_cor_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[60];
+}
+
+static u64 access_tx_read_sdma_memory_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[59];
+}
+
+static u64 access_tx_sb_hdr_cor_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[58];
+}
+
+static u64 access_tx_credit_overrun_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[57];
+}
+
+static u64 access_tx_launch_fifo8_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[56];
+}
+
+static u64 access_tx_launch_fifo7_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[55];
+}
+
+static u64 access_tx_launch_fifo6_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[54];
+}
+
+static u64 access_tx_launch_fifo5_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[53];
+}
+
+static u64 access_tx_launch_fifo4_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[52];
+}
+
+static u64 access_tx_launch_fifo3_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[51];
+}
+
+static u64 access_tx_launch_fifo2_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[50];
+}
+
+static u64 access_tx_launch_fifo1_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[49];
+}
+
+static u64 access_tx_launch_fifo0_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[48];
+}
+
+static u64 access_tx_credit_return_vl_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[47];
+}
+
+static u64 access_tx_hcrc_insertion_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[46];
+}
+
+static u64 access_tx_egress_fifo_unc_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[45];
+}
+
+static u64 access_tx_read_pio_memory_unc_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[44];
+}
+
+static u64 access_tx_read_sdma_memory_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[43];
+}
+
+static u64 access_tx_sb_hdr_unc_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[42];
+}
+
+static u64 access_tx_credit_return_partiy_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[41];
+}
+
+static u64 access_tx_launch_fifo8_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[40];
+}
+
+static u64 access_tx_launch_fifo7_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[39];
+}
+
+static u64 access_tx_launch_fifo6_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[38];
+}
+
+static u64 access_tx_launch_fifo5_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[37];
+}
+
+static u64 access_tx_launch_fifo4_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[36];
+}
+
+static u64 access_tx_launch_fifo3_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[35];
+}
+
+static u64 access_tx_launch_fifo2_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[34];
+}
+
+static u64 access_tx_launch_fifo1_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[33];
+}
+
+static u64 access_tx_launch_fifo0_unc_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[32];
+}
+
+static u64 access_tx_sdma15_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[31];
+}
+
+static u64 access_tx_sdma14_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[30];
+}
+
+static u64 access_tx_sdma13_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[29];
+}
+
+static u64 access_tx_sdma12_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[28];
+}
+
+static u64 access_tx_sdma11_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[27];
+}
+
+static u64 access_tx_sdma10_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[26];
+}
+
+static u64 access_tx_sdma9_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[25];
+}
+
+static u64 access_tx_sdma8_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[24];
+}
+
+static u64 access_tx_sdma7_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[23];
+}
+
+static u64 access_tx_sdma6_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[22];
+}
+
+static u64 access_tx_sdma5_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[21];
+}
+
+static u64 access_tx_sdma4_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[20];
+}
+
+static u64 access_tx_sdma3_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[19];
+}
+
+static u64 access_tx_sdma2_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[18];
+}
+
+static u64 access_tx_sdma1_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[17];
+}
+
+static u64 access_tx_sdma0_disallowed_packet_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[16];
+}
+
+static u64 access_tx_config_parity_err_cnt(const struct cntr_entry *entry,
+					   void *context, int vl, int mode,
+					   u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[15];
+}
+
+static u64 access_tx_sbrd_ctl_csr_parity_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[14];
+}
+
+static u64 access_tx_launch_csr_parity_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[13];
+}
+
+static u64 access_tx_illegal_vl_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[12];
+}
+
+static u64 access_tx_sbrd_ctl_state_machine_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[11];
+}
+
+static u64 access_egress_reserved_10_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[10];
+}
+
+static u64 access_egress_reserved_9_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[9];
+}
+
+static u64 access_tx_sdma_launch_intf_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[8];
+}
+
+static u64 access_tx_pio_launch_intf_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[7];
+}
+
+static u64 access_egress_reserved_6_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[6];
+}
+
+static u64 access_tx_incorrect_link_state_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[5];
+}
+
+static u64 access_tx_linkdown_err_cnt(const struct cntr_entry *entry,
+				      void *context, int vl, int mode,
+				      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[4];
+}
+
+static u64 access_tx_egress_fifi_underrun_or_parity_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[3];
+}
+
+static u64 access_egress_reserved_2_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[2];
+}
+
+static u64 access_tx_pkt_integrity_mem_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[1];
+}
+
+static u64 access_tx_pkt_integrity_mem_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_egress_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendErrStatus
+ */
+static u64 access_send_csr_write_bad_addr_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_err_status_cnt[2];
+}
+
+static u64 access_send_csr_read_bad_addr_err_cnt(const struct cntr_entry *entry,
+						 void *context, int vl,
+						 int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_err_status_cnt[1];
+}
+
+static u64 access_send_csr_parity_cnt(const struct cntr_entry *entry,
+				      void *context, int vl, int mode,
+				      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->send_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendCtxtErrStatus
+ */
+static u64 access_pio_write_out_of_bounds_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_ctxt_err_status_cnt[4];
+}
+
+static u64 access_pio_write_overflow_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_ctxt_err_status_cnt[3];
+}
+
+static u64 access_pio_write_crosses_boundary_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_ctxt_err_status_cnt[2];
+}
+
+static u64 access_pio_disallowed_packet_err_cnt(const struct cntr_entry *entry,
+						void *context, int vl,
+						int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_ctxt_err_status_cnt[1];
+}
+
+static u64 access_pio_inconsistent_sop_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl, int mode,
+					       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_ctxt_err_status_cnt[0];
+}
+
+/*
+ * Software counters corresponding to each of the
+ * error status bits within SendDmaEngErrStatus
+ */
+static u64 access_sdma_header_request_fifo_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[23];
+}
+
+static u64 access_sdma_header_storage_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[22];
+}
+
+static u64 access_sdma_packet_tracking_cor_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[21];
+}
+
+static u64 access_sdma_assembly_cor_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[20];
+}
+
+static u64 access_sdma_desc_table_cor_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[19];
+}
+
+static u64 access_sdma_header_request_fifo_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[18];
+}
+
+static u64 access_sdma_header_storage_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[17];
+}
+
+static u64 access_sdma_packet_tracking_unc_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[16];
+}
+
+static u64 access_sdma_assembly_unc_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[15];
+}
+
+static u64 access_sdma_desc_table_unc_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[14];
+}
+
+static u64 access_sdma_timeout_err_cnt(const struct cntr_entry *entry,
+				       void *context, int vl, int mode,
+				       u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[13];
+}
+
+static u64 access_sdma_header_length_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[12];
+}
+
+static u64 access_sdma_header_address_err_cnt(const struct cntr_entry *entry,
+					      void *context, int vl, int mode,
+					      u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[11];
+}
+
+static u64 access_sdma_header_select_err_cnt(const struct cntr_entry *entry,
+					     void *context, int vl, int mode,
+					     u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[10];
+}
+
+static u64 access_sdma_reserved_9_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[9];
+}
+
+static u64 access_sdma_packet_desc_overflow_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[8];
+}
+
+static u64 access_sdma_length_mismatch_err_cnt(const struct cntr_entry *entry,
+					       void *context, int vl,
+					       int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[7];
+}
+
+static u64 access_sdma_halt_err_cnt(const struct cntr_entry *entry,
+				    void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[6];
+}
+
+static u64 access_sdma_mem_read_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[5];
+}
+
+static u64 access_sdma_first_desc_err_cnt(const struct cntr_entry *entry,
+					  void *context, int vl, int mode,
+					  u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[4];
+}
+
+static u64 access_sdma_tail_out_of_bounds_err_cnt(
+				const struct cntr_entry *entry,
+				void *context, int vl, int mode, u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[3];
+}
+
+static u64 access_sdma_too_long_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[2];
+}
+
+static u64 access_sdma_gen_mismatch_err_cnt(const struct cntr_entry *entry,
+					    void *context, int vl, int mode,
+					    u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[1];
+}
+
+static u64 access_sdma_wrong_dw_err_cnt(const struct cntr_entry *entry,
+					void *context, int vl, int mode,
+					u64 data)
+{
+	struct hfi1_devdata *dd = (struct hfi1_devdata *)context;
+
+	return dd->sw_send_dma_eng_err_status_cnt[0];
+}
+
 #define def_access_sw_cpu(cntr) \
 static u64 access_sw_cpu_##cntr(const struct cntr_entry *entry,		      \
 			      void *context, int vl, int mode, u64 data)      \
@@ -1587,8 +3929,6 @@ static struct cntr_entry dev_cntrs[DEV_CNTR_LAST] = {
 [C_RX_TID_FLGMS] = RXE32_DEV_CNTR_ELEM(RxTidFLGMs,
 			RCV_TID_FLOW_GEN_MISMATCH_CNT,
 			CNTR_NORMAL),
-[C_RX_CTX_RHQS] = RXE32_DEV_CNTR_ELEM(RxCtxRHQS, RCV_CONTEXT_RHQ_STALL,
-			CNTR_NORMAL),
 [C_RX_CTX_EGRS] = RXE32_DEV_CNTR_ELEM(RxCtxEgrS, RCV_CONTEXT_EGR_STALL,
 			CNTR_NORMAL),
 [C_RCV_TID_FLSMS] = RXE32_DEV_CNTR_ELEM(RxTidFLSMs,
@@ -1730,6 +4070,794 @@ static struct cntr_entry dev_cntrs[DEV_CNTR_LAST] = {
 			    access_sw_kmem_wait),
 [C_SW_SEND_SCHED] = CNTR_ELEM("SendSched", 0, 0, CNTR_NORMAL,
 			    access_sw_send_schedule),
+/* MISC_ERR_STATUS */
+[C_MISC_PLL_LOCK_FAIL_ERR] = CNTR_ELEM("MISC_PLL_LOCK_FAIL_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_pll_lock_fail_err_cnt),
+[C_MISC_MBIST_FAIL_ERR] = CNTR_ELEM("MISC_MBIST_FAIL_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_mbist_fail_err_cnt),
+[C_MISC_INVALID_EEP_CMD_ERR] = CNTR_ELEM("MISC_INVALID_EEP_CMD_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_invalid_eep_cmd_err_cnt),
+[C_MISC_EFUSE_DONE_PARITY_ERR] = CNTR_ELEM("MISC_EFUSE_DONE_PARITY_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_efuse_done_parity_err_cnt),
+[C_MISC_EFUSE_WRITE_ERR] = CNTR_ELEM("MISC_EFUSE_WRITE_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_efuse_write_err_cnt),
+[C_MISC_EFUSE_READ_BAD_ADDR_ERR] = CNTR_ELEM("MISC_EFUSE_READ_BAD_ADDR_ERR", 0,
+				0, CNTR_NORMAL,
+				access_misc_efuse_read_bad_addr_err_cnt),
+[C_MISC_EFUSE_CSR_PARITY_ERR] = CNTR_ELEM("MISC_EFUSE_CSR_PARITY_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_efuse_csr_parity_err_cnt),
+[C_MISC_FW_AUTH_FAILED_ERR] = CNTR_ELEM("MISC_FW_AUTH_FAILED_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_fw_auth_failed_err_cnt),
+[C_MISC_KEY_MISMATCH_ERR] = CNTR_ELEM("MISC_KEY_MISMATCH_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_key_mismatch_err_cnt),
+[C_MISC_SBUS_WRITE_FAILED_ERR] = CNTR_ELEM("MISC_SBUS_WRITE_FAILED_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_sbus_write_failed_err_cnt),
+[C_MISC_CSR_WRITE_BAD_ADDR_ERR] = CNTR_ELEM("MISC_CSR_WRITE_BAD_ADDR_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_csr_write_bad_addr_err_cnt),
+[C_MISC_CSR_READ_BAD_ADDR_ERR] = CNTR_ELEM("MISC_CSR_READ_BAD_ADDR_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_csr_read_bad_addr_err_cnt),
+[C_MISC_CSR_PARITY_ERR] = CNTR_ELEM("MISC_CSR_PARITY_ERR", 0, 0,
+				CNTR_NORMAL,
+				access_misc_csr_parity_err_cnt),
+/* CceErrStatus */
+[C_CCE_ERR_STATUS_AGGREGATED_CNT] = CNTR_ELEM("CceErrStatusAggregatedCnt", 0, 0,
+				CNTR_NORMAL,
+				access_sw_cce_err_status_aggregated_cnt),
+[C_CCE_MSIX_CSR_PARITY_ERR] = CNTR_ELEM("CceMsixCsrParityErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_msix_csr_parity_err_cnt),
+[C_CCE_INT_MAP_UNC_ERR] = CNTR_ELEM("CceIntMapUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_int_map_unc_err_cnt),
+[C_CCE_INT_MAP_COR_ERR] = CNTR_ELEM("CceIntMapCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_int_map_cor_err_cnt),
+[C_CCE_MSIX_TABLE_UNC_ERR] = CNTR_ELEM("CceMsixTableUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_msix_table_unc_err_cnt),
+[C_CCE_MSIX_TABLE_COR_ERR] = CNTR_ELEM("CceMsixTableCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_msix_table_cor_err_cnt),
+[C_CCE_RXDMA_CONV_FIFO_PARITY_ERR] = CNTR_ELEM("CceRxdmaConvFifoParityErr", 0,
+				0, CNTR_NORMAL,
+				access_cce_rxdma_conv_fifo_parity_err_cnt),
+[C_CCE_RCPL_ASYNC_FIFO_PARITY_ERR] = CNTR_ELEM("CceRcplAsyncFifoParityErr", 0,
+				0, CNTR_NORMAL,
+				access_cce_rcpl_async_fifo_parity_err_cnt),
+[C_CCE_SEG_WRITE_BAD_ADDR_ERR] = CNTR_ELEM("CceSegWriteBadAddrErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_seg_write_bad_addr_err_cnt),
+[C_CCE_SEG_READ_BAD_ADDR_ERR] = CNTR_ELEM("CceSegReadBadAddrErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_seg_read_bad_addr_err_cnt),
+[C_LA_TRIGGERED] = CNTR_ELEM("Cce LATriggered", 0, 0,
+				CNTR_NORMAL,
+				access_la_triggered_cnt),
+[C_CCE_TRGT_CPL_TIMEOUT_ERR] = CNTR_ELEM("CceTrgtCplTimeoutErr", 0, 0,
+				CNTR_NORMAL,
+				access_cce_trgt_cpl_timeout_err_cnt),
+[C_PCIC_RECEIVE_PARITY_ERR] = CNTR_ELEM("PcicReceiveParityErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_receive_parity_err_cnt),
+[C_PCIC_TRANSMIT_BACK_PARITY_ERR] = CNTR_ELEM("PcicTransmitBackParityErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_transmit_back_parity_err_cnt),
+[C_PCIC_TRANSMIT_FRONT_PARITY_ERR] = CNTR_ELEM("PcicTransmitFrontParityErr", 0,
+				0, CNTR_NORMAL,
+				access_pcic_transmit_front_parity_err_cnt),
+[C_PCIC_CPL_DAT_Q_UNC_ERR] = CNTR_ELEM("PcicCplDatQUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_cpl_dat_q_unc_err_cnt),
+[C_PCIC_CPL_HD_Q_UNC_ERR] = CNTR_ELEM("PcicCplHdQUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_cpl_hd_q_unc_err_cnt),
+[C_PCIC_POST_DAT_Q_UNC_ERR] = CNTR_ELEM("PcicPostDatQUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_post_dat_q_unc_err_cnt),
+[C_PCIC_POST_HD_Q_UNC_ERR] = CNTR_ELEM("PcicPostHdQUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_post_hd_q_unc_err_cnt),
+[C_PCIC_RETRY_SOT_MEM_UNC_ERR] = CNTR_ELEM("PcicRetrySotMemUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_retry_sot_mem_unc_err_cnt),
+[C_PCIC_RETRY_MEM_UNC_ERR] = CNTR_ELEM("PcicRetryMemUncErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_retry_mem_unc_err),
+[C_PCIC_N_POST_DAT_Q_PARITY_ERR] = CNTR_ELEM("PcicNPostDatQParityErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_n_post_dat_q_parity_err_cnt),
+[C_PCIC_N_POST_H_Q_PARITY_ERR] = CNTR_ELEM("PcicNPostHQParityErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_n_post_h_q_parity_err_cnt),
+[C_PCIC_CPL_DAT_Q_COR_ERR] = CNTR_ELEM("PcicCplDatQCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_cpl_dat_q_cor_err_cnt),
+[C_PCIC_CPL_HD_Q_COR_ERR] = CNTR_ELEM("PcicCplHdQCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_cpl_hd_q_cor_err_cnt),
+[C_PCIC_POST_DAT_Q_COR_ERR] = CNTR_ELEM("PcicPostDatQCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_post_dat_q_cor_err_cnt),
+[C_PCIC_POST_HD_Q_COR_ERR] = CNTR_ELEM("PcicPostHdQCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_post_hd_q_cor_err_cnt),
+[C_PCIC_RETRY_SOT_MEM_COR_ERR] = CNTR_ELEM("PcicRetrySotMemCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_retry_sot_mem_cor_err_cnt),
+[C_PCIC_RETRY_MEM_COR_ERR] = CNTR_ELEM("PcicRetryMemCorErr", 0, 0,
+				CNTR_NORMAL,
+				access_pcic_retry_mem_cor_err_cnt),
+[C_CCE_CLI1_ASYNC_FIFO_DBG_PARITY_ERR] = CNTR_ELEM(
+				"CceCli1AsyncFifoDbgParityError", 0, 0,
+				CNTR_NORMAL,
+				access_cce_cli1_async_fifo_dbg_parity_err_cnt),
+[C_CCE_CLI1_ASYNC_FIFO_RXDMA_PARITY_ERR] = CNTR_ELEM(
+				"CceCli1AsyncFifoRxdmaParityError", 0, 0,
+				CNTR_NORMAL,
+				access_cce_cli1_async_fifo_rxdma_parity_err_cnt
+				),
+[C_CCE_CLI1_ASYNC_FIFO_SDMA_HD_PARITY_ERR] = CNTR_ELEM(
+			"CceCli1AsyncFifoSdmaHdParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_cli1_async_fifo_sdma_hd_parity_err_cnt),
+[C_CCE_CLI1_ASYNC_FIFO_PIO_CRDT_PARITY_ERR] = CNTR_ELEM(
+			"CceCli1AsyncFifoPioCrdtParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_cl1_async_fifo_pio_crdt_parity_err_cnt),
+[C_CCE_CLI2_ASYNC_FIFO_PARITY_ERR] = CNTR_ELEM("CceCli2AsyncFifoParityErr", 0,
+			0, CNTR_NORMAL,
+			access_cce_cli2_async_fifo_parity_err_cnt),
+[C_CCE_CSR_CFG_BUS_PARITY_ERR] = CNTR_ELEM("CceCsrCfgBusParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_csr_cfg_bus_parity_err_cnt),
+[C_CCE_CLI0_ASYNC_FIFO_PARTIY_ERR] = CNTR_ELEM("CceCli0AsyncFifoParityErr", 0,
+			0, CNTR_NORMAL,
+			access_cce_cli0_async_fifo_parity_err_cnt),
+[C_CCE_RSPD_DATA_PARITY_ERR] = CNTR_ELEM("CceRspdDataParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_rspd_data_parity_err_cnt),
+[C_CCE_TRGT_ACCESS_ERR] = CNTR_ELEM("CceTrgtAccessErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_trgt_access_err_cnt),
+[C_CCE_TRGT_ASYNC_FIFO_PARITY_ERR] = CNTR_ELEM("CceTrgtAsyncFifoParityErr", 0,
+			0, CNTR_NORMAL,
+			access_cce_trgt_async_fifo_parity_err_cnt),
+[C_CCE_CSR_WRITE_BAD_ADDR_ERR] = CNTR_ELEM("CceCsrWriteBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_csr_write_bad_addr_err_cnt),
+[C_CCE_CSR_READ_BAD_ADDR_ERR] = CNTR_ELEM("CceCsrReadBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_cce_csr_read_bad_addr_err_cnt),
+[C_CCE_CSR_PARITY_ERR] = CNTR_ELEM("CceCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_ccs_csr_parity_err_cnt),
+
+/* RcvErrStatus */
+[C_RX_CSR_PARITY_ERR] = CNTR_ELEM("RxCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_csr_parity_err_cnt),
+[C_RX_CSR_WRITE_BAD_ADDR_ERR] = CNTR_ELEM("RxCsrWriteBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_csr_write_bad_addr_err_cnt),
+[C_RX_CSR_READ_BAD_ADDR_ERR] = CNTR_ELEM("RxCsrReadBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_csr_read_bad_addr_err_cnt),
+[C_RX_DMA_CSR_UNC_ERR] = CNTR_ELEM("RxDmaCsrUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_csr_unc_err_cnt),
+[C_RX_DMA_DQ_FSM_ENCODING_ERR] = CNTR_ELEM("RxDmaDqFsmEncodingErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_dq_fsm_encoding_err_cnt),
+[C_RX_DMA_EQ_FSM_ENCODING_ERR] = CNTR_ELEM("RxDmaEqFsmEncodingErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_eq_fsm_encoding_err_cnt),
+[C_RX_DMA_CSR_PARITY_ERR] = CNTR_ELEM("RxDmaCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_csr_parity_err_cnt),
+[C_RX_RBUF_DATA_COR_ERR] = CNTR_ELEM("RxRbufDataCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_data_cor_err_cnt),
+[C_RX_RBUF_DATA_UNC_ERR] = CNTR_ELEM("RxRbufDataUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_data_unc_err_cnt),
+[C_RX_DMA_DATA_FIFO_RD_COR_ERR] = CNTR_ELEM("RxDmaDataFifoRdCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_data_fifo_rd_cor_err_cnt),
+[C_RX_DMA_DATA_FIFO_RD_UNC_ERR] = CNTR_ELEM("RxDmaDataFifoRdUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_data_fifo_rd_unc_err_cnt),
+[C_RX_DMA_HDR_FIFO_RD_COR_ERR] = CNTR_ELEM("RxDmaHdrFifoRdCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_hdr_fifo_rd_cor_err_cnt),
+[C_RX_DMA_HDR_FIFO_RD_UNC_ERR] = CNTR_ELEM("RxDmaHdrFifoRdUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_hdr_fifo_rd_unc_err_cnt),
+[C_RX_RBUF_DESC_PART2_COR_ERR] = CNTR_ELEM("RxRbufDescPart2CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_desc_part2_cor_err_cnt),
+[C_RX_RBUF_DESC_PART2_UNC_ERR] = CNTR_ELEM("RxRbufDescPart2UncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_desc_part2_unc_err_cnt),
+[C_RX_RBUF_DESC_PART1_COR_ERR] = CNTR_ELEM("RxRbufDescPart1CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_desc_part1_cor_err_cnt),
+[C_RX_RBUF_DESC_PART1_UNC_ERR] = CNTR_ELEM("RxRbufDescPart1UncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_desc_part1_unc_err_cnt),
+[C_RX_HQ_INTR_FSM_ERR] = CNTR_ELEM("RxHqIntrFsmErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_hq_intr_fsm_err_cnt),
+[C_RX_HQ_INTR_CSR_PARITY_ERR] = CNTR_ELEM("RxHqIntrCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_hq_intr_csr_parity_err_cnt),
+[C_RX_LOOKUP_CSR_PARITY_ERR] = CNTR_ELEM("RxLookupCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_lookup_csr_parity_err_cnt),
+[C_RX_LOOKUP_RCV_ARRAY_COR_ERR] = CNTR_ELEM("RxLookupRcvArrayCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_lookup_rcv_array_cor_err_cnt),
+[C_RX_LOOKUP_RCV_ARRAY_UNC_ERR] = CNTR_ELEM("RxLookupRcvArrayUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_lookup_rcv_array_unc_err_cnt),
+[C_RX_LOOKUP_DES_PART2_PARITY_ERR] = CNTR_ELEM("RxLookupDesPart2ParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_lookup_des_part2_parity_err_cnt),
+[C_RX_LOOKUP_DES_PART1_UNC_COR_ERR] = CNTR_ELEM("RxLookupDesPart1UncCorErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_lookup_des_part1_unc_cor_err_cnt),
+[C_RX_LOOKUP_DES_PART1_UNC_ERR] = CNTR_ELEM("RxLookupDesPart1UncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_lookup_des_part1_unc_err_cnt),
+[C_RX_RBUF_NEXT_FREE_BUF_COR_ERR] = CNTR_ELEM("RxRbufNextFreeBufCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_next_free_buf_cor_err_cnt),
+[C_RX_RBUF_NEXT_FREE_BUF_UNC_ERR] = CNTR_ELEM("RxRbufNextFreeBufUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_next_free_buf_unc_err_cnt),
+[C_RX_RBUF_FL_INIT_WR_ADDR_PARITY_ERR] = CNTR_ELEM(
+			"RxRbufFlInitWrAddrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rbuf_fl_init_wr_addr_parity_err_cnt),
+[C_RX_RBUF_FL_INITDONE_PARITY_ERR] = CNTR_ELEM("RxRbufFlInitdoneParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_fl_initdone_parity_err_cnt),
+[C_RX_RBUF_FL_WRITE_ADDR_PARITY_ERR] = CNTR_ELEM("RxRbufFlWrAddrParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_fl_write_addr_parity_err_cnt),
+[C_RX_RBUF_FL_RD_ADDR_PARITY_ERR] = CNTR_ELEM("RxRbufFlRdAddrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_fl_rd_addr_parity_err_cnt),
+[C_RX_RBUF_EMPTY_ERR] = CNTR_ELEM("RxRbufEmptyErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_empty_err_cnt),
+[C_RX_RBUF_FULL_ERR] = CNTR_ELEM("RxRbufFullErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_full_err_cnt),
+[C_RX_RBUF_BAD_LOOKUP_ERR] = CNTR_ELEM("RxRBufBadLookupErr", 0, 0,
+			CNTR_NORMAL,
+			access_rbuf_bad_lookup_err_cnt),
+[C_RX_RBUF_CTX_ID_PARITY_ERR] = CNTR_ELEM("RxRbufCtxIdParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rbuf_ctx_id_parity_err_cnt),
+[C_RX_RBUF_CSR_QEOPDW_PARITY_ERR] = CNTR_ELEM("RxRbufCsrQEOPDWParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rbuf_csr_qeopdw_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_NUM_OF_PKT_PARITY_ERR] = CNTR_ELEM(
+			"RxRbufCsrQNumOfPktParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_csr_q_num_of_pkt_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_T1_PTR_PARITY_ERR] = CNTR_ELEM(
+			"RxRbufCsrQTlPtrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_csr_q_t1_ptr_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_HD_PTR_PARITY_ERR] = CNTR_ELEM("RxRbufCsrQHdPtrParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_csr_q_hd_ptr_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_VLD_BIT_PARITY_ERR] = CNTR_ELEM("RxRbufCsrQVldBitParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_csr_q_vld_bit_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_NEXT_BUF_PARITY_ERR] = CNTR_ELEM("RxRbufCsrQNextBufParityErr",
+			0, 0, CNTR_NORMAL,
+			access_rx_rbuf_csr_q_next_buf_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_ENT_CNT_PARITY_ERR] = CNTR_ELEM("RxRbufCsrQEntCntParityErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_csr_q_ent_cnt_parity_err_cnt),
+[C_RX_RBUF_CSR_Q_HEAD_BUF_NUM_PARITY_ERR] = CNTR_ELEM(
+			"RxRbufCsrQHeadBufNumParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_csr_q_head_buf_num_parity_err_cnt),
+[C_RX_RBUF_BLOCK_LIST_READ_COR_ERR] = CNTR_ELEM("RxRbufBlockListReadCorErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_block_list_read_cor_err_cnt),
+[C_RX_RBUF_BLOCK_LIST_READ_UNC_ERR] = CNTR_ELEM("RxRbufBlockListReadUncErr", 0,
+			0, CNTR_NORMAL,
+			access_rx_rbuf_block_list_read_unc_err_cnt),
+[C_RX_RBUF_LOOKUP_DES_COR_ERR] = CNTR_ELEM("RxRbufLookupDesCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_lookup_des_cor_err_cnt),
+[C_RX_RBUF_LOOKUP_DES_UNC_ERR] = CNTR_ELEM("RxRbufLookupDesUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_lookup_des_unc_err_cnt),
+[C_RX_RBUF_LOOKUP_DES_REG_UNC_COR_ERR] = CNTR_ELEM(
+			"RxRbufLookupDesRegUncCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_lookup_des_reg_unc_cor_err_cnt),
+[C_RX_RBUF_LOOKUP_DES_REG_UNC_ERR] = CNTR_ELEM("RxRbufLookupDesRegUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_lookup_des_reg_unc_err_cnt),
+[C_RX_RBUF_FREE_LIST_COR_ERR] = CNTR_ELEM("RxRbufFreeListCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_free_list_cor_err_cnt),
+[C_RX_RBUF_FREE_LIST_UNC_ERR] = CNTR_ELEM("RxRbufFreeListUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rbuf_free_list_unc_err_cnt),
+[C_RX_RCV_FSM_ENCODING_ERR] = CNTR_ELEM("RxRcvFsmEncodingErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_fsm_encoding_err_cnt),
+[C_RX_DMA_FLAG_COR_ERR] = CNTR_ELEM("RxDmaFlagCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_flag_cor_err_cnt),
+[C_RX_DMA_FLAG_UNC_ERR] = CNTR_ELEM("RxDmaFlagUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_flag_unc_err_cnt),
+[C_RX_DC_SOP_EOP_PARITY_ERR] = CNTR_ELEM("RxDcSopEopParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dc_sop_eop_parity_err_cnt),
+[C_RX_RCV_CSR_PARITY_ERR] = CNTR_ELEM("RxRcvCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_csr_parity_err_cnt),
+[C_RX_RCV_QP_MAP_TABLE_COR_ERR] = CNTR_ELEM("RxRcvQpMapTableCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_qp_map_table_cor_err_cnt),
+[C_RX_RCV_QP_MAP_TABLE_UNC_ERR] = CNTR_ELEM("RxRcvQpMapTableUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_qp_map_table_unc_err_cnt),
+[C_RX_RCV_DATA_COR_ERR] = CNTR_ELEM("RxRcvDataCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_data_cor_err_cnt),
+[C_RX_RCV_DATA_UNC_ERR] = CNTR_ELEM("RxRcvDataUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_data_unc_err_cnt),
+[C_RX_RCV_HDR_COR_ERR] = CNTR_ELEM("RxRcvHdrCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_hdr_cor_err_cnt),
+[C_RX_RCV_HDR_UNC_ERR] = CNTR_ELEM("RxRcvHdrUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_rcv_hdr_unc_err_cnt),
+[C_RX_DC_INTF_PARITY_ERR] = CNTR_ELEM("RxDcIntfParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dc_intf_parity_err_cnt),
+[C_RX_DMA_CSR_COR_ERR] = CNTR_ELEM("RxDmaCsrCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_rx_dma_csr_cor_err_cnt),
+/* SendPioErrStatus */
+[C_PIO_PEC_SOP_HEAD_PARITY_ERR] = CNTR_ELEM("PioPecSopHeadParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pec_sop_head_parity_err_cnt),
+[C_PIO_PCC_SOP_HEAD_PARITY_ERR] = CNTR_ELEM("PioPccSopHeadParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pcc_sop_head_parity_err_cnt),
+[C_PIO_LAST_RETURNED_CNT_PARITY_ERR] = CNTR_ELEM("PioLastReturnedCntParityErr",
+			0, 0, CNTR_NORMAL,
+			access_pio_last_returned_cnt_parity_err_cnt),
+[C_PIO_CURRENT_FREE_CNT_PARITY_ERR] = CNTR_ELEM("PioCurrentFreeCntParityErr", 0,
+			0, CNTR_NORMAL,
+			access_pio_current_free_cnt_parity_err_cnt),
+[C_PIO_RSVD_31_ERR] = CNTR_ELEM("Pio Reserved 31", 0, 0,
+			CNTR_NORMAL,
+			access_pio_reserved_31_err_cnt),
+[C_PIO_RSVD_30_ERR] = CNTR_ELEM("Pio Reserved 30", 0, 0,
+			CNTR_NORMAL,
+			access_pio_reserved_30_err_cnt),
+[C_PIO_PPMC_SOP_LEN_ERR] = CNTR_ELEM("PioPpmcSopLenErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_ppmc_sop_len_err_cnt),
+[C_PIO_PPMC_BQC_MEM_PARITY_ERR] = CNTR_ELEM("PioPpmcBqcMemParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_ppmc_bqc_mem_parity_err_cnt),
+[C_PIO_VL_FIFO_PARITY_ERR] = CNTR_ELEM("PioVlFifoParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_vl_fifo_parity_err_cnt),
+[C_PIO_VLF_SOP_PARITY_ERR] = CNTR_ELEM("PioVlfSopParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_vlf_sop_parity_err_cnt),
+[C_PIO_VLF_V1_LEN_PARITY_ERR] = CNTR_ELEM("PioVlfVlLenParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_vlf_v1_len_parity_err_cnt),
+[C_PIO_BLOCK_QW_COUNT_PARITY_ERR] = CNTR_ELEM("PioBlockQwCountParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_block_qw_count_parity_err_cnt),
+[C_PIO_WRITE_QW_VALID_PARITY_ERR] = CNTR_ELEM("PioWriteQwValidParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_qw_valid_parity_err_cnt),
+[C_PIO_STATE_MACHINE_ERR] = CNTR_ELEM("PioStateMachineErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_state_machine_err_cnt),
+[C_PIO_WRITE_DATA_PARITY_ERR] = CNTR_ELEM("PioWriteDataParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_data_parity_err_cnt),
+[C_PIO_HOST_ADDR_MEM_COR_ERR] = CNTR_ELEM("PioHostAddrMemCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_host_addr_mem_cor_err_cnt),
+[C_PIO_HOST_ADDR_MEM_UNC_ERR] = CNTR_ELEM("PioHostAddrMemUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_host_addr_mem_unc_err_cnt),
+[C_PIO_PKT_EVICT_SM_OR_ARM_SM_ERR] = CNTR_ELEM("PioPktEvictSmOrArbSmErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pkt_evict_sm_or_arb_sm_err_cnt),
+[C_PIO_INIT_SM_IN_ERR] = CNTR_ELEM("PioInitSmInErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_init_sm_in_err_cnt),
+[C_PIO_PPMC_PBL_FIFO_ERR] = CNTR_ELEM("PioPpmcPblFifoErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_ppmc_pbl_fifo_err_cnt),
+[C_PIO_CREDIT_RET_FIFO_PARITY_ERR] = CNTR_ELEM("PioCreditRetFifoParityErr", 0,
+			0, CNTR_NORMAL,
+			access_pio_credit_ret_fifo_parity_err_cnt),
+[C_PIO_V1_LEN_MEM_BANK1_COR_ERR] = CNTR_ELEM("PioVlLenMemBank1CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_v1_len_mem_bank1_cor_err_cnt),
+[C_PIO_V1_LEN_MEM_BANK0_COR_ERR] = CNTR_ELEM("PioVlLenMemBank0CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_v1_len_mem_bank0_cor_err_cnt),
+[C_PIO_V1_LEN_MEM_BANK1_UNC_ERR] = CNTR_ELEM("PioVlLenMemBank1UncErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_v1_len_mem_bank1_unc_err_cnt),
+[C_PIO_V1_LEN_MEM_BANK0_UNC_ERR] = CNTR_ELEM("PioVlLenMemBank0UncErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_v1_len_mem_bank0_unc_err_cnt),
+[C_PIO_SM_PKT_RESET_PARITY_ERR] = CNTR_ELEM("PioSmPktResetParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_sm_pkt_reset_parity_err_cnt),
+[C_PIO_PKT_EVICT_FIFO_PARITY_ERR] = CNTR_ELEM("PioPktEvictFifoParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pkt_evict_fifo_parity_err_cnt),
+[C_PIO_SBRDCTRL_CRREL_FIFO_PARITY_ERR] = CNTR_ELEM(
+			"PioSbrdctrlCrrelFifoParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_sbrdctrl_crrel_fifo_parity_err_cnt),
+[C_PIO_SBRDCTL_CRREL_PARITY_ERR] = CNTR_ELEM("PioSbrdctlCrrelParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_sbrdctl_crrel_parity_err_cnt),
+[C_PIO_PEC_FIFO_PARITY_ERR] = CNTR_ELEM("PioPecFifoParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pec_fifo_parity_err_cnt),
+[C_PIO_PCC_FIFO_PARITY_ERR] = CNTR_ELEM("PioPccFifoParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_pcc_fifo_parity_err_cnt),
+[C_PIO_SB_MEM_FIFO1_ERR] = CNTR_ELEM("PioSbMemFifo1Err", 0, 0,
+			CNTR_NORMAL,
+			access_pio_sb_mem_fifo1_err_cnt),
+[C_PIO_SB_MEM_FIFO0_ERR] = CNTR_ELEM("PioSbMemFifo0Err", 0, 0,
+			CNTR_NORMAL,
+			access_pio_sb_mem_fifo0_err_cnt),
+[C_PIO_CSR_PARITY_ERR] = CNTR_ELEM("PioCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_csr_parity_err_cnt),
+[C_PIO_WRITE_ADDR_PARITY_ERR] = CNTR_ELEM("PioWriteAddrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_addr_parity_err_cnt),
+[C_PIO_WRITE_BAD_CTXT_ERR] = CNTR_ELEM("PioWriteBadCtxtErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_bad_ctxt_err_cnt),
+/* SendDmaErrStatus */
+[C_SDMA_PCIE_REQ_TRACKING_COR_ERR] = CNTR_ELEM("SDmaPcieReqTrackingCorErr", 0,
+			0, CNTR_NORMAL,
+			access_sdma_pcie_req_tracking_cor_err_cnt),
+[C_SDMA_PCIE_REQ_TRACKING_UNC_ERR] = CNTR_ELEM("SDmaPcieReqTrackingUncErr", 0,
+			0, CNTR_NORMAL,
+			access_sdma_pcie_req_tracking_unc_err_cnt),
+[C_SDMA_CSR_PARITY_ERR] = CNTR_ELEM("SDmaCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_csr_parity_err_cnt),
+[C_SDMA_RPY_TAG_ERR] = CNTR_ELEM("SDmaRpyTagErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_rpy_tag_err_cnt),
+/* SendEgressErrStatus */
+[C_TX_READ_PIO_MEMORY_CSR_UNC_ERR] = CNTR_ELEM("TxReadPioMemoryCsrUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_read_pio_memory_csr_unc_err_cnt),
+[C_TX_READ_SDMA_MEMORY_CSR_UNC_ERR] = CNTR_ELEM("TxReadSdmaMemoryCsrUncErr", 0,
+			0, CNTR_NORMAL,
+			access_tx_read_sdma_memory_csr_err_cnt),
+[C_TX_EGRESS_FIFO_COR_ERR] = CNTR_ELEM("TxEgressFifoCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_egress_fifo_cor_err_cnt),
+[C_TX_READ_PIO_MEMORY_COR_ERR] = CNTR_ELEM("TxReadPioMemoryCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_read_pio_memory_cor_err_cnt),
+[C_TX_READ_SDMA_MEMORY_COR_ERR] = CNTR_ELEM("TxReadSdmaMemoryCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_read_sdma_memory_cor_err_cnt),
+[C_TX_SB_HDR_COR_ERR] = CNTR_ELEM("TxSbHdrCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_sb_hdr_cor_err_cnt),
+[C_TX_CREDIT_OVERRUN_ERR] = CNTR_ELEM("TxCreditOverrunErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_credit_overrun_err_cnt),
+[C_TX_LAUNCH_FIFO8_COR_ERR] = CNTR_ELEM("TxLaunchFifo8CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo8_cor_err_cnt),
+[C_TX_LAUNCH_FIFO7_COR_ERR] = CNTR_ELEM("TxLaunchFifo7CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo7_cor_err_cnt),
+[C_TX_LAUNCH_FIFO6_COR_ERR] = CNTR_ELEM("TxLaunchFifo6CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo6_cor_err_cnt),
+[C_TX_LAUNCH_FIFO5_COR_ERR] = CNTR_ELEM("TxLaunchFifo5CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo5_cor_err_cnt),
+[C_TX_LAUNCH_FIFO4_COR_ERR] = CNTR_ELEM("TxLaunchFifo4CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo4_cor_err_cnt),
+[C_TX_LAUNCH_FIFO3_COR_ERR] = CNTR_ELEM("TxLaunchFifo3CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo3_cor_err_cnt),
+[C_TX_LAUNCH_FIFO2_COR_ERR] = CNTR_ELEM("TxLaunchFifo2CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo2_cor_err_cnt),
+[C_TX_LAUNCH_FIFO1_COR_ERR] = CNTR_ELEM("TxLaunchFifo1CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo1_cor_err_cnt),
+[C_TX_LAUNCH_FIFO0_COR_ERR] = CNTR_ELEM("TxLaunchFifo0CorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_fifo0_cor_err_cnt),
+[C_TX_CREDIT_RETURN_VL_ERR] = CNTR_ELEM("TxCreditReturnVLErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_credit_return_vl_err_cnt),
+[C_TX_HCRC_INSERTION_ERR] = CNTR_ELEM("TxHcrcInsertionErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_hcrc_insertion_err_cnt),
+[C_TX_EGRESS_FIFI_UNC_ERR] = CNTR_ELEM("TxEgressFifoUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_egress_fifo_unc_err_cnt),
+[C_TX_READ_PIO_MEMORY_UNC_ERR] = CNTR_ELEM("TxReadPioMemoryUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_read_pio_memory_unc_err_cnt),
+[C_TX_READ_SDMA_MEMORY_UNC_ERR] = CNTR_ELEM("TxReadSdmaMemoryUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_read_sdma_memory_unc_err_cnt),
+[C_TX_SB_HDR_UNC_ERR] = CNTR_ELEM("TxSbHdrUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_sb_hdr_unc_err_cnt),
+[C_TX_CREDIT_RETURN_PARITY_ERR] = CNTR_ELEM("TxCreditReturnParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_credit_return_partiy_err_cnt),
+[C_TX_LAUNCH_FIFO8_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo8UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo8_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO7_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo7UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo7_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO6_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo6UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo6_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO5_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo5UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo5_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO4_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo4UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo4_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO3_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo3UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo3_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO2_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo2UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo2_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO1_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo1UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo1_unc_or_parity_err_cnt),
+[C_TX_LAUNCH_FIFO0_UNC_OR_PARITY_ERR] = CNTR_ELEM("TxLaunchFifo0UncOrParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_launch_fifo0_unc_or_parity_err_cnt),
+[C_TX_SDMA15_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma15DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma15_disallowed_packet_err_cnt),
+[C_TX_SDMA14_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma14DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma14_disallowed_packet_err_cnt),
+[C_TX_SDMA13_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma13DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma13_disallowed_packet_err_cnt),
+[C_TX_SDMA12_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma12DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma12_disallowed_packet_err_cnt),
+[C_TX_SDMA11_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma11DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma11_disallowed_packet_err_cnt),
+[C_TX_SDMA10_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma10DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma10_disallowed_packet_err_cnt),
+[C_TX_SDMA9_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma9DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma9_disallowed_packet_err_cnt),
+[C_TX_SDMA8_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma8DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma8_disallowed_packet_err_cnt),
+[C_TX_SDMA7_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma7DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma7_disallowed_packet_err_cnt),
+[C_TX_SDMA6_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma6DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma6_disallowed_packet_err_cnt),
+[C_TX_SDMA5_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma5DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma5_disallowed_packet_err_cnt),
+[C_TX_SDMA4_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma4DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma4_disallowed_packet_err_cnt),
+[C_TX_SDMA3_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma3DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma3_disallowed_packet_err_cnt),
+[C_TX_SDMA2_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma2DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma2_disallowed_packet_err_cnt),
+[C_TX_SDMA1_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma1DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma1_disallowed_packet_err_cnt),
+[C_TX_SDMA0_DISALLOWED_PACKET_ERR] = CNTR_ELEM("TxSdma0DisallowedPacketErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma0_disallowed_packet_err_cnt),
+[C_TX_CONFIG_PARITY_ERR] = CNTR_ELEM("TxConfigParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_config_parity_err_cnt),
+[C_TX_SBRD_CTL_CSR_PARITY_ERR] = CNTR_ELEM("TxSbrdCtlCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_sbrd_ctl_csr_parity_err_cnt),
+[C_TX_LAUNCH_CSR_PARITY_ERR] = CNTR_ELEM("TxLaunchCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_launch_csr_parity_err_cnt),
+[C_TX_ILLEGAL_CL_ERR] = CNTR_ELEM("TxIllegalVLErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_illegal_vl_err_cnt),
+[C_TX_SBRD_CTL_STATE_MACHINE_PARITY_ERR] = CNTR_ELEM(
+			"TxSbrdCtlStateMachineParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_sbrd_ctl_state_machine_parity_err_cnt),
+[C_TX_RESERVED_10] = CNTR_ELEM("Tx Egress Reserved 10", 0, 0,
+			CNTR_NORMAL,
+			access_egress_reserved_10_err_cnt),
+[C_TX_RESERVED_9] = CNTR_ELEM("Tx Egress Reserved 9", 0, 0,
+			CNTR_NORMAL,
+			access_egress_reserved_9_err_cnt),
+[C_TX_SDMA_LAUNCH_INTF_PARITY_ERR] = CNTR_ELEM("TxSdmaLaunchIntfParityErr",
+			0, 0, CNTR_NORMAL,
+			access_tx_sdma_launch_intf_parity_err_cnt),
+[C_TX_PIO_LAUNCH_INTF_PARITY_ERR] = CNTR_ELEM("TxPioLaunchIntfParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_pio_launch_intf_parity_err_cnt),
+[C_TX_RESERVED_6] = CNTR_ELEM("Tx Egress Reserved 6", 0, 0,
+			CNTR_NORMAL,
+			access_egress_reserved_6_err_cnt),
+[C_TX_INCORRECT_LINK_STATE_ERR] = CNTR_ELEM("TxIncorrectLinkStateErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_incorrect_link_state_err_cnt),
+[C_TX_LINK_DOWN_ERR] = CNTR_ELEM("TxLinkdownErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_linkdown_err_cnt),
+[C_TX_EGRESS_FIFO_UNDERRUN_OR_PARITY_ERR] = CNTR_ELEM(
+			"EgressFifoUnderrunOrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_egress_fifi_underrun_or_parity_err_cnt),
+[C_TX_RESERVED_2] = CNTR_ELEM("Tx Egress Reserved 2", 0, 0,
+			CNTR_NORMAL,
+			access_egress_reserved_2_err_cnt),
+[C_TX_PKT_INTEGRITY_MEM_UNC_ERR] = CNTR_ELEM("TxPktIntegrityMemUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_pkt_integrity_mem_unc_err_cnt),
+[C_TX_PKT_INTEGRITY_MEM_COR_ERR] = CNTR_ELEM("TxPktIntegrityMemCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_tx_pkt_integrity_mem_cor_err_cnt),
+/* SendErrStatus */
+[C_SEND_CSR_WRITE_BAD_ADDR_ERR] = CNTR_ELEM("SendCsrWriteBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_send_csr_write_bad_addr_err_cnt),
+[C_SEND_CSR_READ_BAD_ADD_ERR] = CNTR_ELEM("SendCsrReadBadAddrErr", 0, 0,
+			CNTR_NORMAL,
+			access_send_csr_read_bad_addr_err_cnt),
+[C_SEND_CSR_PARITY_ERR] = CNTR_ELEM("SendCsrParityErr", 0, 0,
+			CNTR_NORMAL,
+			access_send_csr_parity_cnt),
+/* SendCtxtErrStatus */
+[C_PIO_WRITE_OUT_OF_BOUNDS_ERR] = CNTR_ELEM("PioWriteOutOfBoundsErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_out_of_bounds_err_cnt),
+[C_PIO_WRITE_OVERFLOW_ERR] = CNTR_ELEM("PioWriteOverflowErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_write_overflow_err_cnt),
+[C_PIO_WRITE_CROSSES_BOUNDARY_ERR] = CNTR_ELEM("PioWriteCrossesBoundaryErr",
+			0, 0, CNTR_NORMAL,
+			access_pio_write_crosses_boundary_err_cnt),
+[C_PIO_DISALLOWED_PACKET_ERR] = CNTR_ELEM("PioDisallowedPacketErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_disallowed_packet_err_cnt),
+[C_PIO_INCONSISTENT_SOP_ERR] = CNTR_ELEM("PioInconsistentSopErr", 0, 0,
+			CNTR_NORMAL,
+			access_pio_inconsistent_sop_err_cnt),
+/* SendDmaEngErrStatus */
+[C_SDMA_HEADER_REQUEST_FIFO_COR_ERR] = CNTR_ELEM("SDmaHeaderRequestFifoCorErr",
+			0, 0, CNTR_NORMAL,
+			access_sdma_header_request_fifo_cor_err_cnt),
+[C_SDMA_HEADER_STORAGE_COR_ERR] = CNTR_ELEM("SDmaHeaderStorageCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_header_storage_cor_err_cnt),
+[C_SDMA_PACKET_TRACKING_COR_ERR] = CNTR_ELEM("SDmaPacketTrackingCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_packet_tracking_cor_err_cnt),
+[C_SDMA_ASSEMBLY_COR_ERR] = CNTR_ELEM("SDmaAssemblyCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_assembly_cor_err_cnt),
+[C_SDMA_DESC_TABLE_COR_ERR] = CNTR_ELEM("SDmaDescTableCorErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_desc_table_cor_err_cnt),
+[C_SDMA_HEADER_REQUEST_FIFO_UNC_ERR] = CNTR_ELEM("SDmaHeaderRequestFifoUncErr",
+			0, 0, CNTR_NORMAL,
+			access_sdma_header_request_fifo_unc_err_cnt),
+[C_SDMA_HEADER_STORAGE_UNC_ERR] = CNTR_ELEM("SDmaHeaderStorageUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_header_storage_unc_err_cnt),
+[C_SDMA_PACKET_TRACKING_UNC_ERR] = CNTR_ELEM("SDmaPacketTrackingUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_packet_tracking_unc_err_cnt),
+[C_SDMA_ASSEMBLY_UNC_ERR] = CNTR_ELEM("SDmaAssemblyUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_assembly_unc_err_cnt),
+[C_SDMA_DESC_TABLE_UNC_ERR] = CNTR_ELEM("SDmaDescTableUncErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_desc_table_unc_err_cnt),
+[C_SDMA_TIMEOUT_ERR] = CNTR_ELEM("SDmaTimeoutErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_timeout_err_cnt),
+[C_SDMA_HEADER_LENGTH_ERR] = CNTR_ELEM("SDmaHeaderLengthErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_header_length_err_cnt),
+[C_SDMA_HEADER_ADDRESS_ERR] = CNTR_ELEM("SDmaHeaderAddressErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_header_address_err_cnt),
+[C_SDMA_HEADER_SELECT_ERR] = CNTR_ELEM("SDmaHeaderSelectErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_header_select_err_cnt),
+[C_SMDA_RESERVED_9] = CNTR_ELEM("SDma Reserved 9", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_reserved_9_err_cnt),
+[C_SDMA_PACKET_DESC_OVERFLOW_ERR] = CNTR_ELEM("SDmaPacketDescOverflowErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_packet_desc_overflow_err_cnt),
+[C_SDMA_LENGTH_MISMATCH_ERR] = CNTR_ELEM("SDmaLengthMismatchErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_length_mismatch_err_cnt),
+[C_SDMA_HALT_ERR] = CNTR_ELEM("SDmaHaltErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_halt_err_cnt),
+[C_SDMA_MEM_READ_ERR] = CNTR_ELEM("SDmaMemReadErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_mem_read_err_cnt),
+[C_SDMA_FIRST_DESC_ERR] = CNTR_ELEM("SDmaFirstDescErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_first_desc_err_cnt),
+[C_SDMA_TAIL_OUT_OF_BOUNDS_ERR] = CNTR_ELEM("SDmaTailOutOfBoundsErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_tail_out_of_bounds_err_cnt),
+[C_SDMA_TOO_LONG_ERR] = CNTR_ELEM("SDmaTooLongErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_too_long_err_cnt),
+[C_SDMA_GEN_MISMATCH_ERR] = CNTR_ELEM("SDmaGenMismatchErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_gen_mismatch_err_cnt),
+[C_SDMA_WRONG_DW_ERR] = CNTR_ELEM("SDmaWrongDwErr", 0, 0,
+			CNTR_NORMAL,
+			access_sdma_wrong_dw_err_cnt),
 };
 
 static struct cntr_entry port_cntrs[PORT_CNTR_LAST] = {
@@ -1762,6 +4890,8 @@ static struct cntr_entry port_cntrs[PORT_CNTR_LAST] = {
 			access_sw_link_dn_cnt),
 [C_SW_LINK_UP] = CNTR_ELEM("SwLinkUp", 0, 0, CNTR_SYNTH | CNTR_32BIT,
 			access_sw_link_up_cnt),
+[C_SW_UNKNOWN_FRAME] = CNTR_ELEM("UnknownFrame", 0, 0, CNTR_NORMAL,
+				 access_sw_unknown_frame_cnt),
 [C_SW_XMIT_DSCD] = CNTR_ELEM("XmitDscd", 0, 0, CNTR_SYNTH | CNTR_32BIT,
 			access_sw_xmit_discards),
 [C_SW_XMIT_DSCD_VL] = CNTR_ELEM("XmitDscdVl", 0, 0,
@@ -1873,13 +5003,6 @@ static struct cntr_entry port_cntrs[PORT_CNTR_LAST] = {
 
 /* ======================================================================== */
 
-/* return true if this is chip revision revision a0 */
-int is_a0(struct hfi1_devdata *dd)
-{
-	return ((dd->revision >> CCE_REVISION_CHIP_REV_MINOR_SHIFT)
-			& CCE_REVISION_CHIP_REV_MINOR_MASK) == 0;
-}
-
 /* return true if this is chip revision revision a */
 int is_ax(struct hfi1_devdata *dd)
 {
@@ -1895,7 +5018,7 @@ int is_bx(struct hfi1_devdata *dd)
 	u8 chip_rev_minor =
 		dd->revision >> CCE_REVISION_CHIP_REV_MINOR_SHIFT
 			& CCE_REVISION_CHIP_REV_MINOR_MASK;
-	return !!(chip_rev_minor & 0x10);
+	return (chip_rev_minor & 0xF0) == 0x10;
 }
 
 /*
@@ -2182,6 +5305,7 @@ static char *send_err_status_string(char *buf, int buf_len, u64 flags)
 static void handle_cce_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	/*
 	 * For most these errors, there is nothing that can be done except
@@ -2190,13 +5314,20 @@ static void handle_cce_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 	dd_dev_info(dd, "CCE Error: %s\n",
 		cce_err_status_string(buf, sizeof(buf), reg));
 
-	if ((reg & CCE_ERR_STATUS_CCE_CLI2_ASYNC_FIFO_PARITY_ERR_SMASK)
-			&& is_a0(dd)
-			&& (dd->icode != ICODE_FUNCTIONAL_SIMULATOR)) {
+	if ((reg & CCE_ERR_STATUS_CCE_CLI2_ASYNC_FIFO_PARITY_ERR_SMASK) &&
+	    is_ax(dd) && (dd->icode != ICODE_FUNCTIONAL_SIMULATOR)) {
 		/* this error requires a manual drop into SPC freeze mode */
 		/* then a fix up */
 		start_freeze_handling(dd->pport, FREEZE_SELF);
 	}
+
+	for (i = 0; i < NUM_CCE_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i)) {
+			incr_cntr64(&dd->cce_err_status_cnt[i]);
+			/* maintain a counter over all cce_err_status errors */
+			incr_cntr64(&dd->sw_cce_err_status_aggregate);
+		}
+	}
 }
 
 /*
@@ -2241,6 +5372,7 @@ static void free_rcverr(struct hfi1_devdata *dd)
 static void handle_rxe_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	dd_dev_info(dd, "Receive Error: %s\n",
 		rxe_err_status_string(buf, sizeof(buf), reg));
@@ -2252,41 +5384,63 @@ static void handle_rxe_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 		 * Freeze mode recovery is disabled for the errors
 		 * in RXE_FREEZE_ABORT_MASK
 		 */
-		if (is_a0(dd) && (reg & RXE_FREEZE_ABORT_MASK))
+		if (is_ax(dd) && (reg & RXE_FREEZE_ABORT_MASK))
 			flags = FREEZE_ABORT;
 
 		start_freeze_handling(dd->pport, flags);
 	}
+
+	for (i = 0; i < NUM_RCV_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->rcv_err_status_cnt[i]);
+	}
 }
 
 static void handle_misc_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	dd_dev_info(dd, "Misc Error: %s",
 		misc_err_status_string(buf, sizeof(buf), reg));
+	for (i = 0; i < NUM_MISC_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->misc_err_status_cnt[i]);
+	}
 }
 
 static void handle_pio_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	dd_dev_info(dd, "PIO Error: %s\n",
 		pio_err_status_string(buf, sizeof(buf), reg));
 
 	if (reg & ALL_PIO_FREEZE_ERR)
 		start_freeze_handling(dd->pport, 0);
+
+	for (i = 0; i < NUM_SEND_PIO_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->send_pio_err_status_cnt[i]);
+	}
 }
 
 static void handle_sdma_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	dd_dev_info(dd, "SDMA Error: %s\n",
 		sdma_err_status_string(buf, sizeof(buf), reg));
 
 	if (reg & ALL_SDMA_FREEZE_ERR)
 		start_freeze_handling(dd->pport, 0);
+
+	for (i = 0; i < NUM_SEND_DMA_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->send_dma_err_status_cnt[i]);
+	}
 }
 
 static void count_port_inactive(struct hfi1_devdata *dd)
@@ -2352,10 +5506,11 @@ static void handle_egress_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	u64 reg_copy = reg, handled = 0;
 	char buf[96];
+	int i = 0;
 
 	if (reg & ALL_TXE_EGRESS_FREEZE_ERR)
 		start_freeze_handling(dd->pport, 0);
-	if (is_a0(dd) && (reg &
+	if (is_ax(dd) && (reg &
 		    SEND_EGRESS_ERR_STATUS_TX_CREDIT_RETURN_VL_ERR_SMASK)
 		    && (dd->icode != ICODE_FUNCTIONAL_SIMULATOR))
 		start_freeze_handling(dd->pport, 0);
@@ -2383,15 +5538,25 @@ static void handle_egress_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 	if (reg)
 		dd_dev_info(dd, "Egress Error: %s\n",
 			egress_err_status_string(buf, sizeof(buf), reg));
+
+	for (i = 0; i < NUM_SEND_EGRESS_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->send_egress_err_status_cnt[i]);
+	}
 }
 
 static void handle_txe_err(struct hfi1_devdata *dd, u32 unused, u64 reg)
 {
 	char buf[96];
+	int i = 0;
 
 	dd_dev_info(dd, "Send Error: %s\n",
 		send_err_status_string(buf, sizeof(buf), reg));
 
+	for (i = 0; i < NUM_SEND_ERR_STATUS_COUNTERS; i++) {
+		if (reg & (1ull << i))
+			incr_cntr64(&dd->send_err_status_cnt[i]);
+	}
 }
 
 /*
@@ -2483,6 +5648,7 @@ static void is_sendctxt_err_int(struct hfi1_devdata *dd,
 	char flags[96];
 	u64 status;
 	u32 sw_index;
+	int i = 0;
 
 	sw_index = dd->hw_to_sw[hw_context];
 	if (sw_index >= dd->num_send_contexts) {
@@ -2516,12 +5682,23 @@ static void is_sendctxt_err_int(struct hfi1_devdata *dd,
 	 */
 	if (sc->type != SC_USER)
 		queue_work(dd->pport->hfi1_wq, &sc->halt_work);
+
+	/*
+	 * Update the counters for the corresponding status bits.
+	 * Note that these particular counters are aggregated over all
+	 * 160 contexts.
+	 */
+	for (i = 0; i < NUM_SEND_CTXT_ERR_STATUS_COUNTERS; i++) {
+		if (status & (1ull << i))
+			incr_cntr64(&dd->sw_ctxt_err_status_cnt[i]);
+	}
 }
 
 static void handle_sdma_eng_err(struct hfi1_devdata *dd,
 				unsigned int source, u64 status)
 {
 	struct sdma_engine *sde;
+	int i = 0;
 
 	sde = &dd->per_sdma[source];
 #ifdef CONFIG_SDMA_VERBOSITY
@@ -2531,6 +5708,16 @@ static void handle_sdma_eng_err(struct hfi1_devdata *dd,
 		   sde->this_idx, source, (unsigned long long)status);
 #endif
 	sdma_engine_error(sde, status);
+
+	/*
+	* Update the counters for the corresponding status bits.
+	* Note that these particular counters are aggregated over
+	* all 16 DMA engines.
+	*/
+	for (i = 0; i < NUM_SEND_DMA_ENG_ERR_STATUS_COUNTERS; i++) {
+		if (status & (1ull << i))
+			incr_cntr64(&dd->sw_send_dma_eng_err_status_cnt[i]);
+	}
 }
 
 /*
@@ -3050,7 +6237,7 @@ static void adjust_lcb_for_fpga_serdes(struct hfi1_devdata *dd)
 	/* else this is _p */
 
 	version = emulator_rev(dd);
-	if (!is_a0(dd))
+	if (!is_ax(dd))
 		version = 0x2d;	/* all B0 use 0x2d or higher settings */
 
 	if (version <= 0x12) {
@@ -3313,7 +6500,6 @@ void handle_freeze(struct work_struct *work)
 	struct hfi1_devdata *dd = ppd->dd;
 
 	/* wait for freeze indicators on all affected blocks */
-	dd_dev_info(dd, "Entering SPC freeze\n");
 	wait_for_freeze_status(dd, 1);
 
 	/* SPC is now frozen */
@@ -3336,7 +6522,7 @@ void handle_freeze(struct work_struct *work)
 	write_csr(dd, CCE_CTRL, CCE_CTRL_SPC_UNFREEZE_SMASK);
 	wait_for_freeze_status(dd, 0);
 
-	if (is_a0(dd)) {
+	if (is_ax(dd)) {
 		write_csr(dd, CCE_CTRL, CCE_CTRL_SPC_FREEZE_SMASK);
 		wait_for_freeze_status(dd, 1);
 		write_csr(dd, CCE_CTRL, CCE_CTRL_SPC_UNFREEZE_SMASK);
@@ -3371,7 +6557,6 @@ void handle_freeze(struct work_struct *work)
 	wake_up(&dd->event_queue);
 
 	/* no longer frozen */
-	dd_dev_err(dd, "Exiting SPC freeze\n");
 }
 
 /*
@@ -3542,10 +6727,10 @@ static void add_full_mgmt_pkey(struct hfi1_pportdata *ppd)
 {
 	struct hfi1_devdata *dd = ppd->dd;
 
-	/* Sanity check - ppd->pkeys[2] should be 0 */
-	if (ppd->pkeys[2] != 0)
-		dd_dev_err(dd, "%s pkey[2] already set to 0x%x, resetting it to 0x%x\n",
-			   __func__, ppd->pkeys[2], FULL_MGMT_P_KEY);
+	/* Sanity check - ppd->pkeys[2] should be 0, or already initalized */
+	if (!((ppd->pkeys[2] == 0) || (ppd->pkeys[2] == FULL_MGMT_P_KEY)))
+		dd_dev_warn(dd, "%s pkey[2] already set to 0x%x, resetting it to 0x%x\n",
+			    __func__, ppd->pkeys[2], FULL_MGMT_P_KEY);
 	ppd->pkeys[2] = FULL_MGMT_P_KEY;
 	(void)hfi1_set_ib_cfg(ppd, HFI1_IB_CFG_PKEYS, 0);
 }
@@ -3864,7 +7049,7 @@ void handle_verify_cap(struct work_struct *work)
 	 *	REPLAY_BUF_MBE_SMASK
 	 *	FLIT_INPUT_BUF_MBE_SMASK
 	 */
-	if (is_a0(dd)) {			/* fixed in B0 */
+	if (is_ax(dd)) {			/* fixed in B0 */
 		reg = read_csr(dd, DC_LCB_CFG_LINK_KILL_EN);
 		reg |= DC_LCB_CFG_LINK_KILL_EN_REPLAY_BUF_MBE_SMASK
 			| DC_LCB_CFG_LINK_KILL_EN_FLIT_INPUT_BUF_MBE_SMASK;
@@ -3907,18 +7092,32 @@ void handle_verify_cap(struct work_struct *work)
  */
 void apply_link_downgrade_policy(struct hfi1_pportdata *ppd, int refresh_widths)
 {
-	int skip = 1;
 	int do_bounce = 0;
-	u16 lwde = ppd->link_width_downgrade_enabled;
+	int tries;
+	u16 lwde;
 	u16 tx, rx;
 
+	/* use the hls lock to avoid a race with actual link up */
+	tries = 0;
+retry:
 	mutex_lock(&ppd->hls_lock);
 	/* only apply if the link is up */
-	if (ppd->host_link_state & HLS_UP)
-		skip = 0;
-	mutex_unlock(&ppd->hls_lock);
-	if (skip)
-		return;
+	if (!(ppd->host_link_state & HLS_UP)) {
+		/* still going up..wait and retry */
+		if (ppd->host_link_state & HLS_GOING_UP) {
+			if (++tries < 1000) {
+				mutex_unlock(&ppd->hls_lock);
+				usleep_range(100, 120); /* arbitrary */
+				goto retry;
+			}
+			dd_dev_err(ppd->dd,
+				   "%s: giving up waiting for link state change\n",
+				   __func__);
+		}
+		goto done;
+	}
+
+	lwde = ppd->link_width_downgrade_enabled;
 
 	if (refresh_widths) {
 		get_link_widths(ppd->dd, &tx, &rx);
@@ -3956,6 +7155,9 @@ void apply_link_downgrade_policy(struct hfi1_pportdata *ppd, int refresh_widths)
 		do_bounce = 1;
 	}
 
+done:
+	mutex_unlock(&ppd->hls_lock);
+
 	if (do_bounce) {
 		set_link_down_reason(ppd, OPA_LINKDOWN_REASON_WIDTH_POLICY, 0,
 		  OPA_LINKDOWN_REASON_WIDTH_POLICY);
@@ -4046,6 +7248,11 @@ static void handle_8051_interrupt(struct hfi1_devdata *dd, u32 unused, u64 reg)
 			}
 			err &= ~(u64)FAILED_LNI;
 		}
+		/* unknown frames can happen durning LNI, just count */
+		if (err & UNKNOWN_FRAME) {
+			ppd->unknown_frame_count++;
+			err &= ~(u64)UNKNOWN_FRAME;
+		}
 		if (err) {
 			/* report remaining errors, but do not do anything */
 			dd_dev_err(dd, "8051 info error: %s\n",
@@ -4774,13 +7981,25 @@ int read_lcb_csr(struct hfi1_devdata *dd, u32 addr, u64 *data)
  */
 static int write_lcb_via_8051(struct hfi1_devdata *dd, u32 addr, u64 data)
 {
+	u32 regno;
+	int ret;
 
-	if (acquire_lcb_access(dd, 0) == 0) {
-		write_csr(dd, addr, data);
-		release_lcb_access(dd, 0);
-		return 0;
+	if (dd->icode == ICODE_FUNCTIONAL_SIMULATOR ||
+	    (dd->dc8051_ver < dc8051_ver(0, 20))) {
+		if (acquire_lcb_access(dd, 0) == 0) {
+			write_csr(dd, addr, data);
+			release_lcb_access(dd, 0);
+			return 0;
+		}
+		return -EBUSY;
 	}
-	return -EBUSY;
+
+	/* register is an index of LCB registers: (offset - base) / 8 */
+	regno = (addr - DC_LCB_CFG_RUN) >> 3;
+	ret = do_8051_command(dd, HCMD_WRITE_LCB_CSR, regno, &data);
+	if (ret != HCMD_SUCCESS)
+		return -EBUSY;
+	return 0;
 }
 
 /*
@@ -4862,6 +8081,26 @@ static int do_8051_command(
 	 */
 
 	/*
+	 * When writing a LCB CSR, out_data contains the full value to
+	 * to be written, while in_data contains the relative LCB
+	 * address in 7:0.  Do the work here, rather than the caller,
+	 * of distrubting the write data to where it needs to go:
+	 *
+	 * Write data
+	 *   39:00 -> in_data[47:8]
+	 *   47:40 -> DC8051_CFG_EXT_DEV_0.RETURN_CODE
+	 *   63:48 -> DC8051_CFG_EXT_DEV_0.RSP_DATA
+	 */
+	if (type == HCMD_WRITE_LCB_CSR) {
+		in_data |= ((*out_data) & 0xffffffffffull) << 8;
+		reg = ((((*out_data) >> 40) & 0xff) <<
+				DC_DC8051_CFG_EXT_DEV_0_RETURN_CODE_SHIFT)
+		      | ((((*out_data) >> 48) & 0xffff) <<
+				DC_DC8051_CFG_EXT_DEV_0_RSP_DATA_SHIFT);
+		write_csr(dd, DC_DC8051_CFG_EXT_DEV_0, reg);
+	}
+
+	/*
 	 * Do two writes: the first to stabilize the type and req_data, the
 	 * second to activate.
 	 */
@@ -5856,6 +9095,23 @@ void init_qsfp(struct hfi1_pportdata *ppd)
 	}
 }
 
+/*
+ * Do a one-time initialize of the LCB block.
+ */
+static void init_lcb(struct hfi1_devdata *dd)
+{
+	/* the DC has been reset earlier in the driver load */
+
+	/* set LCB for cclk loopback on the port */
+	write_csr(dd, DC_LCB_CFG_TX_FIFOS_RESET, 0x01);
+	write_csr(dd, DC_LCB_CFG_LANE_WIDTH, 0x00);
+	write_csr(dd, DC_LCB_CFG_REINIT_AS_SLAVE, 0x00);
+	write_csr(dd, DC_LCB_CFG_CNT_FOR_SKIP_STALL, 0x110);
+	write_csr(dd, DC_LCB_CFG_CLK_CNTR, 0x08);
+	write_csr(dd, DC_LCB_CFG_LOOPBACK, 0x02);
+	write_csr(dd, DC_LCB_CFG_TX_FIFOS_RESET, 0x00);
+}
+
 int bringup_serdes(struct hfi1_pportdata *ppd)
 {
 	struct hfi1_devdata *dd = ppd->dd;
@@ -5877,6 +9133,9 @@ int bringup_serdes(struct hfi1_pportdata *ppd)
 	/* Set linkinit_reason on power up per OPA spec */
 	ppd->linkinit_reason = OPA_LINKINIT_REASON_LINKUP;
 
+	/* one-time init of the LCB */
+	init_lcb(dd);
+
 	if (loopback) {
 		ret = init_loopback(dd);
 		if (ret < 0)
@@ -6148,7 +9407,8 @@ u32 lrh_max_header_bytes(struct hfi1_devdata *dd)
 static void set_send_length(struct hfi1_pportdata *ppd)
 {
 	struct hfi1_devdata *dd = ppd->dd;
-	u32 max_hb = lrh_max_header_bytes(dd), maxvlmtu = 0, dcmtu;
+	u32 max_hb = lrh_max_header_bytes(dd), dcmtu;
+	u32 maxvlmtu = dd->vld[15].mtu;
 	u64 len1 = 0, len2 = (((dd->vld[15].mtu + max_hb) >> 2)
 			      & SEND_LEN_CHECK1_LEN_VL15_MASK) <<
 		SEND_LEN_CHECK1_LEN_VL15_SHIFT;
@@ -6319,9 +9579,10 @@ static int goto_offline(struct hfi1_pportdata *ppd, u8 rem_reason)
 	 * depending on how the link went down.  The 8051 firmware
 	 * will observe the needed wait time and only move to ready
 	 * when that is completed.  The largest of the quiet timeouts
-	 * is 2.5s, so wait that long and then a bit more.
+	 * is 6s, so wait that long and then at least 0.5s more for
+	 * other transitions, and another 0.5s for a buffer.
 	 */
-	ret = wait_fm_ready(dd, 3000);
+	ret = wait_fm_ready(dd, 7000);
 	if (ret) {
 		dd_dev_err(dd,
 			"After going offline, timed out waiting for the 8051 to become ready to accept host requests\n");
@@ -7235,8 +10496,7 @@ static int set_buffer_control(struct hfi1_devdata *dd,
 		new_bc->vl[i].shared = 0;
 	}
 	new_total += be16_to_cpu(new_bc->overall_shared_limit);
-	if (new_total > (u32)dd->link_credits)
-		return -EINVAL;
+
 	/* fetch the current values */
 	get_buffer_control(dd, &cur_bc, &cur_total);
 
@@ -7282,8 +10542,8 @@ static int set_buffer_control(struct hfi1_devdata *dd,
 	 */
 	use_all_mask = 0;
 	if ((be16_to_cpu(new_bc->overall_shared_limit) <
-				be16_to_cpu(cur_bc.overall_shared_limit))
-			|| (is_a0(dd) && any_shared_limit_changing)) {
+	     be16_to_cpu(cur_bc.overall_shared_limit)) ||
+	    (is_ax(dd) && any_shared_limit_changing)) {
 		set_global_shared(dd, 0);
 		cur_bc.overall_shared_limit = 0;
 		use_all_mask = 1;
@@ -7457,7 +10717,7 @@ int fm_set_table(struct hfi1_pportdata *ppd, int which, void *t)
  */
 static int disable_data_vls(struct hfi1_devdata *dd)
 {
-	if (is_a0(dd))
+	if (is_ax(dd))
 		return 1;
 
 	pio_send_control(dd, PSC_DATA_VL_DISABLE);
@@ -7475,7 +10735,7 @@ static int disable_data_vls(struct hfi1_devdata *dd)
  */
 int open_fill_data_vls(struct hfi1_devdata *dd)
 {
-	if (is_a0(dd))
+	if (is_ax(dd))
 		return 1;
 
 	pio_send_control(dd, PSC_DATA_VL_ENABLE);
@@ -7748,11 +11008,22 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 					& RCV_TID_CTRL_TID_BASE_INDEX_MASK)
 				<< RCV_TID_CTRL_TID_BASE_INDEX_SHIFT);
 		write_kctxt_csr(dd, ctxt, RCV_TID_CTRL, reg);
-		if (ctxt == VL15CTXT)
-			write_csr(dd, RCV_VL15, VL15CTXT);
+		if (ctxt == HFI1_CTRL_CTXT)
+			write_csr(dd, RCV_VL15, HFI1_CTRL_CTXT);
 	}
 	if (op & HFI1_RCVCTRL_CTXT_DIS) {
 		write_csr(dd, RCV_VL15, 0);
+		/*
+		 * When receive context is being disabled turn on tail
+		 * update with a dummy tail address and then disable
+		 * receive context.
+		 */
+		if (dd->rcvhdrtail_dummy_physaddr) {
+			write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR,
+					dd->rcvhdrtail_dummy_physaddr);
+			rcvctrl |= RCV_CTXT_CTRL_TAIL_UPD_SMASK;
+		}
+
 		rcvctrl &= ~RCV_CTXT_CTRL_ENABLE_SMASK;
 	}
 	if (op & HFI1_RCVCTRL_INTRAVAIL_ENB)
@@ -7822,10 +11093,11 @@ void hfi1_rcvctrl(struct hfi1_devdata *dd, unsigned int op, int ctxt)
 	if (op & (HFI1_RCVCTRL_TAILUPD_DIS | HFI1_RCVCTRL_CTXT_DIS))
 		/*
 		 * If the context has been disabled and the Tail Update has
-		 * been cleared, clear the RCV_HDR_TAIL_ADDR CSR so
-		 * it doesn't contain an address that is invalid.
+		 * been cleared, set the RCV_HDR_TAIL_ADDR CSR to dummy address
+		 * so it doesn't contain an address that is invalid.
 		 */
-		write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR, 0);
+		write_kctxt_csr(dd, ctxt, RCV_HDR_TAIL_ADDR,
+				dd->rcvhdrtail_dummy_physaddr);
 }
 
 u32 hfi1_read_cntrs(struct hfi1_devdata *dd, loff_t pos, char **namep,
@@ -8785,7 +12057,7 @@ static void clean_up_interrupts(struct hfi1_devdata *dd)
 	/* turn off interrupts */
 	if (dd->num_msix_entries) {
 		/* MSI-X */
-		hfi1_nomsix(dd);
+		pci_disable_msix(dd->pcidev);
 	} else {
 		/* INTx */
 		disable_intx(dd->pcidev);
@@ -8840,18 +12112,12 @@ static void remap_sdma_interrupts(struct hfi1_devdata *dd,
 		msix_intr);
 }
 
-static void remap_receive_available_interrupt(struct hfi1_devdata *dd,
-					      int rx, int msix_intr)
-{
-	remap_intr(dd, IS_RCVAVAIL_START + rx, msix_intr);
-}
-
 static int request_intx_irq(struct hfi1_devdata *dd)
 {
 	int ret;
 
-	snprintf(dd->intx_name, sizeof(dd->intx_name), DRIVER_NAME"_%d",
-		dd->unit);
+	snprintf(dd->intx_name, sizeof(dd->intx_name), DRIVER_NAME "_%d",
+		 dd->unit);
 	ret = request_irq(dd->pcidev->irq, general_interrupt,
 				  IRQF_SHARED, dd->intx_name, dd);
 	if (ret)
@@ -8870,7 +12136,7 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 	int first_general, last_general;
 	int first_sdma, last_sdma;
 	int first_rx, last_rx;
-	int first_cpu, restart_cpu, curr_cpu;
+	int first_cpu, curr_cpu;
 	int rcv_cpu, sdma_cpu;
 	int i, ret = 0, possible;
 	int ht;
@@ -8909,22 +12175,19 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 			topology_sibling_cpumask(cpumask_first(local_mask)));
 	for (i = possible/ht; i < possible; i++)
 		cpumask_clear_cpu(i, def);
-	/* reset possible */
-	possible = cpumask_weight(def);
 	/* def now has full cores on chosen node*/
 	first_cpu = cpumask_first(def);
 	if (nr_cpu_ids >= first_cpu)
 		first_cpu++;
-	restart_cpu = first_cpu;
-	curr_cpu = restart_cpu;
+	curr_cpu = first_cpu;
 
-	for (i = first_cpu; i < dd->n_krcv_queues + first_cpu; i++) {
+	/*  One context is reserved as control context */
+	for (i = first_cpu; i < dd->n_krcv_queues + first_cpu - 1; i++) {
 		cpumask_clear_cpu(curr_cpu, def);
 		cpumask_set_cpu(curr_cpu, rcv);
-		if (curr_cpu >= possible)
-			curr_cpu = restart_cpu;
-		else
-			curr_cpu++;
+		curr_cpu = cpumask_next(curr_cpu, def);
+		if (curr_cpu >= nr_cpu_ids)
+			break;
 	}
 	/* def mask has non-rcv, rcv has recv mask */
 	rcv_cpu = cpumask_first(rcv);
@@ -8953,7 +12216,7 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 			handler = general_interrupt;
 			arg = dd;
 			snprintf(me->name, sizeof(me->name),
-				DRIVER_NAME"_%d", dd->unit);
+				 DRIVER_NAME "_%d", dd->unit);
 			err_info = "general";
 		} else if (first_sdma <= i && i < last_sdma) {
 			idx = i - first_sdma;
@@ -8961,7 +12224,7 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 			handler = sdma_interrupt;
 			arg = sde;
 			snprintf(me->name, sizeof(me->name),
-				DRIVER_NAME"_%d sdma%d", dd->unit, idx);
+				 DRIVER_NAME "_%d sdma%d", dd->unit, idx);
 			err_info = "sdma";
 			remap_sdma_interrupts(dd, idx, i);
 		} else if (first_rx <= i && i < last_rx) {
@@ -8981,9 +12244,9 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 			thread = receive_context_thread;
 			arg = rcd;
 			snprintf(me->name, sizeof(me->name),
-				DRIVER_NAME"_%d kctxt%d", dd->unit, idx);
+				 DRIVER_NAME "_%d kctxt%d", dd->unit, idx);
 			err_info = "receive context";
-			remap_receive_available_interrupt(dd, idx, i);
+			remap_intr(dd, IS_RCVAVAIL_START + idx, i);
 		} else {
 			/* not in our expected range - complain, then
 			   ignore it */
@@ -9018,17 +12281,26 @@ static int request_msix_irqs(struct hfi1_devdata *dd)
 		if (handler == sdma_interrupt) {
 			dd_dev_info(dd, "sdma engine %d cpu %d\n",
 				sde->this_idx, sdma_cpu);
+			sde->cpu = sdma_cpu;
 			cpumask_set_cpu(sdma_cpu, dd->msix_entries[i].mask);
 			sdma_cpu = cpumask_next(sdma_cpu, def);
 			if (sdma_cpu >= nr_cpu_ids)
 				sdma_cpu = cpumask_first(def);
 		} else if (handler == receive_context_interrupt) {
-			dd_dev_info(dd, "rcv ctxt %d cpu %d\n",
-				rcd->ctxt, rcv_cpu);
-			cpumask_set_cpu(rcv_cpu, dd->msix_entries[i].mask);
-			rcv_cpu = cpumask_next(rcv_cpu, rcv);
-			if (rcv_cpu >= nr_cpu_ids)
-				rcv_cpu = cpumask_first(rcv);
+			dd_dev_info(dd, "rcv ctxt %d cpu %d\n", rcd->ctxt,
+				    (rcd->ctxt == HFI1_CTRL_CTXT) ?
+					    cpumask_first(def) : rcv_cpu);
+			if (rcd->ctxt == HFI1_CTRL_CTXT) {
+				/* map to first default */
+				cpumask_set_cpu(cpumask_first(def),
+						dd->msix_entries[i].mask);
+			} else {
+				cpumask_set_cpu(rcv_cpu,
+						dd->msix_entries[i].mask);
+				rcv_cpu = cpumask_next(rcv_cpu, rcv);
+				if (rcv_cpu >= nr_cpu_ids)
+					rcv_cpu = cpumask_first(rcv);
+			}
 		} else {
 			/* otherwise first def */
 			dd_dev_info(dd, "%s cpu %d\n",
@@ -9153,7 +12425,6 @@ fail:
 static int set_up_context_variables(struct hfi1_devdata *dd)
 {
 	int num_kernel_contexts;
-	int num_user_contexts;
 	int total_contexts;
 	int ret;
 	unsigned ngroups;
@@ -9161,11 +12432,18 @@ static int set_up_context_variables(struct hfi1_devdata *dd)
 	/*
 	 * Kernel contexts: (to be fixed later):
 	 * - min or 2 or 1 context/numa
-	 * - Context 0 - default/errors
-	 * - Context 1 - VL15
+	 * - Context 0 - control context (VL15/multicast/error)
+	 * - Context 1 - default context
 	 */
 	if (n_krcvqs)
-		num_kernel_contexts = n_krcvqs + MIN_KERNEL_KCTXTS;
+		/*
+		 * Don't count context 0 in n_krcvqs since
+		 * is isn't used for normal verbs traffic.
+		 *
+		 * krcvqs will reflect number of kernel
+		 * receive contexts above 0.
+		 */
+		num_kernel_contexts = n_krcvqs + MIN_KERNEL_KCTXTS - 1;
 	else
 		num_kernel_contexts = num_online_nodes();
 	num_kernel_contexts =
@@ -9183,12 +12461,10 @@ static int set_up_context_variables(struct hfi1_devdata *dd)
 	}
 	/*
 	 * User contexts: (to be fixed later)
-	 *	- set to num_rcv_contexts if non-zero
-	 *	- default to 1 user context per CPU
+	 *	- default to 1 user context per CPU if num_user_contexts is
+	 *	  negative
 	 */
-	if (num_rcv_contexts)
-		num_user_contexts = num_rcv_contexts;
-	else
+	if (num_user_contexts < 0)
 		num_user_contexts = num_online_cpus();
 
 	total_contexts = num_kernel_contexts + num_user_contexts;
@@ -9455,7 +12731,7 @@ static void reset_asic_csrs(struct hfi1_devdata *dd)
 	/* We might want to retain this state across FLR if we ever use it */
 	write_csr(dd, ASIC_CFG_DRV_STR, 0);
 
-	write_csr(dd, ASIC_CFG_THERM_POLL_EN, 0);
+	/* ASIC_CFG_THERM_POLL_EN leave alone */
 	/* ASIC_STS_THERM read-only */
 	/* ASIC_CFG_RESET leave alone */
 
@@ -9906,7 +13182,7 @@ static void init_chip(struct hfi1_devdata *dd)
 		/* restore command and BARs */
 		restore_pci_variables(dd);
 
-		if (is_a0(dd)) {
+		if (is_ax(dd)) {
 			dd_dev_info(dd, "Resetting CSRs with FLR\n");
 			hfi1_pcie_flr(dd);
 			restore_pci_variables(dd);
@@ -9925,23 +13201,20 @@ static void init_chip(struct hfi1_devdata *dd)
 	write_csr(dd, CCE_DC_CTRL, 0);
 
 	/* Set the LED off */
-	if (is_a0(dd))
+	if (is_ax(dd))
 		setextled(dd, 0);
 	/*
 	 * Clear the QSFP reset.
-	 * A0 leaves the out lines floating on power on, then on an FLR
-	 * enforces a 0 on all out pins.  The driver does not touch
+	 * An FLR enforces a 0 on all out pins. The driver does not touch
 	 * ASIC_QSFPn_OUT otherwise.  This leaves RESET_N low and
-	 * anything  plugged constantly in reset, if it pays attention
+	 * anything plugged constantly in reset, if it pays attention
 	 * to RESET_N.
-	 * A prime example of this is SiPh. For now, set all pins high.
+	 * Prime examples of this are optical cables. Set all pins high.
 	 * I2CCLK and I2CDAT will change per direction, and INT_N and
 	 * MODPRS_N are input only and their value is ignored.
 	 */
-	if (is_a0(dd)) {
-		write_csr(dd, ASIC_QSFP1_OUT, 0x1f);
-		write_csr(dd, ASIC_QSFP2_OUT, 0x1f);
-	}
+	write_csr(dd, ASIC_QSFP1_OUT, 0x1f);
+	write_csr(dd, ASIC_QSFP2_OUT, 0x1f);
 }
 
 static void init_early_variables(struct hfi1_devdata *dd)
@@ -9951,7 +13224,7 @@ static void init_early_variables(struct hfi1_devdata *dd)
 	/* assign link credit variables */
 	dd->vau = CM_VAU;
 	dd->link_credits = CM_GLOBAL_CREDITS;
-	if (is_a0(dd))
+	if (is_ax(dd))
 		dd->link_credits--;
 	dd->vcu = cu_to_vcu(hfi1_cu);
 	/* enough room for 8 MAD packets plus header - 17K */
@@ -10017,12 +13290,6 @@ static void init_qpmap_table(struct hfi1_devdata *dd,
 	u64 ctxt = first_ctxt;
 
 	for (i = 0; i < 256;) {
-		if (ctxt == VL15CTXT) {
-			ctxt++;
-			if (ctxt > last_ctxt)
-				ctxt = first_ctxt;
-			continue;
-		}
 		reg |= ctxt << (8 * (i % 8));
 		i++;
 		ctxt++;
@@ -10065,7 +13332,7 @@ static void init_qos(struct hfi1_devdata *dd, u32 first_ctxt)
 	unsigned qpns_per_vl, ctxt, i, qpn, n = 1, m;
 	u64 *rsmmap;
 	u64 reg;
-	u8  rxcontext = is_a0(dd) ? 0 : 0xff;  /* 0 is default if a0 ver. */
+	u8  rxcontext = is_ax(dd) ? 0 : 0xff;  /* 0 is default if a0 ver. */
 
 	/* validate */
 	if (dd->n_krcv_queues <= MIN_KERNEL_KCTXTS ||
@@ -10087,6 +13354,8 @@ static void init_qos(struct hfi1_devdata *dd, u32 first_ctxt)
 	if (num_vls * qpns_per_vl > dd->chip_rcv_contexts)
 		goto bail;
 	rsmmap = kmalloc_array(NUM_MAP_REGS, sizeof(u64), GFP_KERNEL);
+	if (!rsmmap)
+		goto bail;
 	memset(rsmmap, rxcontext, NUM_MAP_REGS * sizeof(u64));
 	/* init the local copy of the table */
 	for (i = 0, ctxt = first_ctxt; i < num_vls; i++) {
@@ -10135,19 +13404,13 @@ static void init_qos(struct hfi1_devdata *dd, u32 first_ctxt)
 	/* Enable RSM */
 	add_rcvctrl(dd, RCV_CTRL_RCV_RSM_ENABLE_SMASK);
 	kfree(rsmmap);
-	/* map everything else (non-VL15) to context 0 */
-	init_qpmap_table(
-		dd,
-		0,
-		0);
+	/* map everything else to first context */
+	init_qpmap_table(dd, FIRST_KERNEL_KCTXT, MIN_KERNEL_KCTXTS - 1);
 	dd->qos_shift = n + 1;
 	return;
 bail:
 	dd->qos_shift = 1;
-	init_qpmap_table(
-		dd,
-		dd->n_krcv_queues > MIN_KERNEL_KCTXTS ? MIN_KERNEL_KCTXTS : 0,
-		dd->n_krcv_queues - 1);
+	init_qpmap_table(dd, FIRST_KERNEL_KCTXT, dd->n_krcv_queues - 1);
 }
 
 static void init_rxe(struct hfi1_devdata *dd)
@@ -10276,7 +13539,7 @@ int hfi1_set_ctxt_jkey(struct hfi1_devdata *dd, unsigned ctxt, u16 jkey)
 	 * Enable send-side J_KEY integrity check, unless this is A0 h/w
 	 * (due to A0 erratum).
 	 */
-	if (!is_a0(dd)) {
+	if (!is_ax(dd)) {
 		reg = read_kctxt_csr(dd, sctxt, SEND_CTXT_CHECK_ENABLE);
 		reg |= SEND_CTXT_CHECK_ENABLE_CHECK_JOB_KEY_SMASK;
 		write_kctxt_csr(dd, sctxt, SEND_CTXT_CHECK_ENABLE, reg);
@@ -10309,7 +13572,7 @@ int hfi1_clear_ctxt_jkey(struct hfi1_devdata *dd, unsigned ctxt)
 	 * This check would not have been enabled for A0 h/w, see
 	 * set_ctxt_jkey().
 	 */
-	if (!is_a0(dd)) {
+	if (!is_ax(dd)) {
 		reg = read_kctxt_csr(dd, sctxt, SEND_CTXT_CHECK_ENABLE);
 		reg &= ~SEND_CTXT_CHECK_ENABLE_CHECK_JOB_KEY_SMASK;
 		write_kctxt_csr(dd, sctxt, SEND_CTXT_CHECK_ENABLE, reg);
@@ -10418,6 +13681,32 @@ static void asic_should_init(struct hfi1_devdata *dd)
 	spin_unlock_irqrestore(&hfi1_devs_lock, flags);
 }
 
+/*
+ * Set dd->boardname.  Use a generic name if a name is not returned from
+ * EFI variable space.
+ *
+ * Return 0 on success, -ENOMEM if space could not be allocated.
+ */
+static int obtain_boardname(struct hfi1_devdata *dd)
+{
+	/* generic board description */
+	const char generic[] =
+		"Intel Omni-Path Host Fabric Interface Adapter 100 Series";
+	unsigned long size;
+	int ret;
+
+	ret = read_hfi1_efi_var(dd, "description", &size,
+				(void **)&dd->boardname);
+	if (ret) {
+		dd_dev_err(dd, "Board description not found\n");
+		/* use generic description */
+		dd->boardname = kstrdup(generic, GFP_KERNEL);
+		if (!dd->boardname)
+			return -ENOMEM;
+	}
+	return 0;
+}
+
 /**
  * Allocate and initialize the device structure for the hfi.
  * @dev: the pci_dev for hfi1_ib device
@@ -10554,9 +13843,9 @@ struct hfi1_devdata *hfi1_init_dd(struct pci_dev *pdev,
 	/* insure num_vls isn't larger than number of sdma engines */
 	if (HFI1_CAP_IS_KSET(SDMA) && num_vls > dd->chip_sdma_engines) {
 		dd_dev_err(dd, "num_vls %u too large, using %u VLs\n",
-				num_vls, HFI1_MAX_VLS_SUPPORTED);
-		ppd->vls_supported = num_vls = HFI1_MAX_VLS_SUPPORTED;
-		ppd->vls_operational = ppd->vls_supported;
+			   num_vls, dd->chip_sdma_engines);
+		num_vls = dd->chip_sdma_engines;
+		ppd->vls_supported = dd->chip_sdma_engines;
 	}
 
 	/*
@@ -10615,18 +13904,13 @@ struct hfi1_devdata *hfi1_init_dd(struct pci_dev *pdev,
 
 	parse_platform_config(dd);
 
-	/* add board names as they are defined */
-	dd->boardname = kmalloc(64, GFP_KERNEL);
-	if (!dd->boardname)
+	ret = obtain_boardname(dd);
+	if (ret)
 		goto bail_cleanup;
-	snprintf(dd->boardname, 64, "Board ID 0x%llx",
-		 dd->revision >> CCE_REVISION_BOARD_ID_LOWER_NIBBLE_SHIFT
-		    & CCE_REVISION_BOARD_ID_LOWER_NIBBLE_MASK);
 
 	snprintf(dd->boardversion, BOARD_VERS_MAX,
-		 "ChipABI %u.%u, %s, ChipRev %u.%u, SW Compat %llu\n",
+		 "ChipABI %u.%u, ChipRev %u.%u, SW Compat %llu\n",
 		 HFI1_CHIP_VERS_MAJ, HFI1_CHIP_VERS_MIN,
-		 dd->boardname,
 		 (u32)dd->majrev,
 		 (u32)dd->minrev,
 		 (dd->revision >> CCE_REVISION_SW_SHIFT)
@@ -10803,7 +14087,9 @@ static int thermal_init(struct hfi1_devdata *dd)
 
 	acquire_hw_mutex(dd);
 	dd_dev_info(dd, "Initializing thermal sensor\n");
-
+	/* Disable polling of thermal readings */
+	write_csr(dd, ASIC_CFG_THERM_POLL_EN, 0x0);
+	msleep(100);
 	/* Thermal Sensor Initialization */
 	/*    Step 1: Reset the Thermal SBus Receiver */
 	ret = sbus_request_slow(dd, SBUS_THERMAL, 0x0,
diff --git a/drivers/staging/rdma/hfi1/chip.h b/drivers/staging/rdma/hfi1/chip.h
index ebf9041a1c5e..5b375ddc345d 100644
--- a/drivers/staging/rdma/hfi1/chip.h
+++ b/drivers/staging/rdma/hfi1/chip.h
@@ -235,6 +235,7 @@
 #define HCMD_MISC		   0x05
 #define HCMD_READ_LCB_IDLE_MSG 0x06
 #define HCMD_READ_LCB_CSR      0x07
+#define HCMD_WRITE_LCB_CSR     0x08
 #define HCMD_INTERFACE_TEST	   0xff
 
 /* DC_DC8051_CFG_HOST_CMD_1.RETURN_CODE - 8051 host command return */
@@ -663,7 +664,6 @@ void get_linkup_link_widths(struct hfi1_pportdata *ppd);
 void read_ltp_rtt(struct hfi1_devdata *dd);
 void clear_linkup_counters(struct hfi1_devdata *dd);
 u32 hdrqempty(struct hfi1_ctxtdata *rcd);
-int is_a0(struct hfi1_devdata *dd);
 int is_ax(struct hfi1_devdata *dd);
 int is_bx(struct hfi1_devdata *dd);
 u32 read_physical_state(struct hfi1_devdata *dd);
@@ -721,7 +721,6 @@ enum {
 	C_RX_TID_FULL,
 	C_RX_TID_INVALID,
 	C_RX_TID_FLGMS,
-	C_RX_CTX_RHQS,
 	C_RX_CTX_EGRS,
 	C_RCV_TID_FLSMS,
 	C_CCE_PCI_CR_ST,
@@ -788,6 +787,275 @@ enum {
 	C_SW_PIO_WAIT,
 	C_SW_KMEM_WAIT,
 	C_SW_SEND_SCHED,
+/* MISC_ERR_STATUS */
+	C_MISC_PLL_LOCK_FAIL_ERR,
+	C_MISC_MBIST_FAIL_ERR,
+	C_MISC_INVALID_EEP_CMD_ERR,
+	C_MISC_EFUSE_DONE_PARITY_ERR,
+	C_MISC_EFUSE_WRITE_ERR,
+	C_MISC_EFUSE_READ_BAD_ADDR_ERR,
+	C_MISC_EFUSE_CSR_PARITY_ERR,
+	C_MISC_FW_AUTH_FAILED_ERR,
+	C_MISC_KEY_MISMATCH_ERR,
+	C_MISC_SBUS_WRITE_FAILED_ERR,
+	C_MISC_CSR_WRITE_BAD_ADDR_ERR,
+	C_MISC_CSR_READ_BAD_ADDR_ERR,
+	C_MISC_CSR_PARITY_ERR,
+/* CceErrStatus */
+	/*
+	* A special counter that is the aggregate count
+	* of all the cce_err_status errors.  The remainder
+	* are actual bits in the CceErrStatus register.
+	*/
+	C_CCE_ERR_STATUS_AGGREGATED_CNT,
+	C_CCE_MSIX_CSR_PARITY_ERR,
+	C_CCE_INT_MAP_UNC_ERR,
+	C_CCE_INT_MAP_COR_ERR,
+	C_CCE_MSIX_TABLE_UNC_ERR,
+	C_CCE_MSIX_TABLE_COR_ERR,
+	C_CCE_RXDMA_CONV_FIFO_PARITY_ERR,
+	C_CCE_RCPL_ASYNC_FIFO_PARITY_ERR,
+	C_CCE_SEG_WRITE_BAD_ADDR_ERR,
+	C_CCE_SEG_READ_BAD_ADDR_ERR,
+	C_LA_TRIGGERED,
+	C_CCE_TRGT_CPL_TIMEOUT_ERR,
+	C_PCIC_RECEIVE_PARITY_ERR,
+	C_PCIC_TRANSMIT_BACK_PARITY_ERR,
+	C_PCIC_TRANSMIT_FRONT_PARITY_ERR,
+	C_PCIC_CPL_DAT_Q_UNC_ERR,
+	C_PCIC_CPL_HD_Q_UNC_ERR,
+	C_PCIC_POST_DAT_Q_UNC_ERR,
+	C_PCIC_POST_HD_Q_UNC_ERR,
+	C_PCIC_RETRY_SOT_MEM_UNC_ERR,
+	C_PCIC_RETRY_MEM_UNC_ERR,
+	C_PCIC_N_POST_DAT_Q_PARITY_ERR,
+	C_PCIC_N_POST_H_Q_PARITY_ERR,
+	C_PCIC_CPL_DAT_Q_COR_ERR,
+	C_PCIC_CPL_HD_Q_COR_ERR,
+	C_PCIC_POST_DAT_Q_COR_ERR,
+	C_PCIC_POST_HD_Q_COR_ERR,
+	C_PCIC_RETRY_SOT_MEM_COR_ERR,
+	C_PCIC_RETRY_MEM_COR_ERR,
+	C_CCE_CLI1_ASYNC_FIFO_DBG_PARITY_ERR,
+	C_CCE_CLI1_ASYNC_FIFO_RXDMA_PARITY_ERR,
+	C_CCE_CLI1_ASYNC_FIFO_SDMA_HD_PARITY_ERR,
+	C_CCE_CLI1_ASYNC_FIFO_PIO_CRDT_PARITY_ERR,
+	C_CCE_CLI2_ASYNC_FIFO_PARITY_ERR,
+	C_CCE_CSR_CFG_BUS_PARITY_ERR,
+	C_CCE_CLI0_ASYNC_FIFO_PARTIY_ERR,
+	C_CCE_RSPD_DATA_PARITY_ERR,
+	C_CCE_TRGT_ACCESS_ERR,
+	C_CCE_TRGT_ASYNC_FIFO_PARITY_ERR,
+	C_CCE_CSR_WRITE_BAD_ADDR_ERR,
+	C_CCE_CSR_READ_BAD_ADDR_ERR,
+	C_CCE_CSR_PARITY_ERR,
+/* RcvErrStatus */
+	C_RX_CSR_PARITY_ERR,
+	C_RX_CSR_WRITE_BAD_ADDR_ERR,
+	C_RX_CSR_READ_BAD_ADDR_ERR,
+	C_RX_DMA_CSR_UNC_ERR,
+	C_RX_DMA_DQ_FSM_ENCODING_ERR,
+	C_RX_DMA_EQ_FSM_ENCODING_ERR,
+	C_RX_DMA_CSR_PARITY_ERR,
+	C_RX_RBUF_DATA_COR_ERR,
+	C_RX_RBUF_DATA_UNC_ERR,
+	C_RX_DMA_DATA_FIFO_RD_COR_ERR,
+	C_RX_DMA_DATA_FIFO_RD_UNC_ERR,
+	C_RX_DMA_HDR_FIFO_RD_COR_ERR,
+	C_RX_DMA_HDR_FIFO_RD_UNC_ERR,
+	C_RX_RBUF_DESC_PART2_COR_ERR,
+	C_RX_RBUF_DESC_PART2_UNC_ERR,
+	C_RX_RBUF_DESC_PART1_COR_ERR,
+	C_RX_RBUF_DESC_PART1_UNC_ERR,
+	C_RX_HQ_INTR_FSM_ERR,
+	C_RX_HQ_INTR_CSR_PARITY_ERR,
+	C_RX_LOOKUP_CSR_PARITY_ERR,
+	C_RX_LOOKUP_RCV_ARRAY_COR_ERR,
+	C_RX_LOOKUP_RCV_ARRAY_UNC_ERR,
+	C_RX_LOOKUP_DES_PART2_PARITY_ERR,
+	C_RX_LOOKUP_DES_PART1_UNC_COR_ERR,
+	C_RX_LOOKUP_DES_PART1_UNC_ERR,
+	C_RX_RBUF_NEXT_FREE_BUF_COR_ERR,
+	C_RX_RBUF_NEXT_FREE_BUF_UNC_ERR,
+	C_RX_RBUF_FL_INIT_WR_ADDR_PARITY_ERR,
+	C_RX_RBUF_FL_INITDONE_PARITY_ERR,
+	C_RX_RBUF_FL_WRITE_ADDR_PARITY_ERR,
+	C_RX_RBUF_FL_RD_ADDR_PARITY_ERR,
+	C_RX_RBUF_EMPTY_ERR,
+	C_RX_RBUF_FULL_ERR,
+	C_RX_RBUF_BAD_LOOKUP_ERR,
+	C_RX_RBUF_CTX_ID_PARITY_ERR,
+	C_RX_RBUF_CSR_QEOPDW_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_NUM_OF_PKT_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_T1_PTR_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_HD_PTR_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_VLD_BIT_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_NEXT_BUF_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_ENT_CNT_PARITY_ERR,
+	C_RX_RBUF_CSR_Q_HEAD_BUF_NUM_PARITY_ERR,
+	C_RX_RBUF_BLOCK_LIST_READ_COR_ERR,
+	C_RX_RBUF_BLOCK_LIST_READ_UNC_ERR,
+	C_RX_RBUF_LOOKUP_DES_COR_ERR,
+	C_RX_RBUF_LOOKUP_DES_UNC_ERR,
+	C_RX_RBUF_LOOKUP_DES_REG_UNC_COR_ERR,
+	C_RX_RBUF_LOOKUP_DES_REG_UNC_ERR,
+	C_RX_RBUF_FREE_LIST_COR_ERR,
+	C_RX_RBUF_FREE_LIST_UNC_ERR,
+	C_RX_RCV_FSM_ENCODING_ERR,
+	C_RX_DMA_FLAG_COR_ERR,
+	C_RX_DMA_FLAG_UNC_ERR,
+	C_RX_DC_SOP_EOP_PARITY_ERR,
+	C_RX_RCV_CSR_PARITY_ERR,
+	C_RX_RCV_QP_MAP_TABLE_COR_ERR,
+	C_RX_RCV_QP_MAP_TABLE_UNC_ERR,
+	C_RX_RCV_DATA_COR_ERR,
+	C_RX_RCV_DATA_UNC_ERR,
+	C_RX_RCV_HDR_COR_ERR,
+	C_RX_RCV_HDR_UNC_ERR,
+	C_RX_DC_INTF_PARITY_ERR,
+	C_RX_DMA_CSR_COR_ERR,
+/* SendPioErrStatus */
+	C_PIO_PEC_SOP_HEAD_PARITY_ERR,
+	C_PIO_PCC_SOP_HEAD_PARITY_ERR,
+	C_PIO_LAST_RETURNED_CNT_PARITY_ERR,
+	C_PIO_CURRENT_FREE_CNT_PARITY_ERR,
+	C_PIO_RSVD_31_ERR,
+	C_PIO_RSVD_30_ERR,
+	C_PIO_PPMC_SOP_LEN_ERR,
+	C_PIO_PPMC_BQC_MEM_PARITY_ERR,
+	C_PIO_VL_FIFO_PARITY_ERR,
+	C_PIO_VLF_SOP_PARITY_ERR,
+	C_PIO_VLF_V1_LEN_PARITY_ERR,
+	C_PIO_BLOCK_QW_COUNT_PARITY_ERR,
+	C_PIO_WRITE_QW_VALID_PARITY_ERR,
+	C_PIO_STATE_MACHINE_ERR,
+	C_PIO_WRITE_DATA_PARITY_ERR,
+	C_PIO_HOST_ADDR_MEM_COR_ERR,
+	C_PIO_HOST_ADDR_MEM_UNC_ERR,
+	C_PIO_PKT_EVICT_SM_OR_ARM_SM_ERR,
+	C_PIO_INIT_SM_IN_ERR,
+	C_PIO_PPMC_PBL_FIFO_ERR,
+	C_PIO_CREDIT_RET_FIFO_PARITY_ERR,
+	C_PIO_V1_LEN_MEM_BANK1_COR_ERR,
+	C_PIO_V1_LEN_MEM_BANK0_COR_ERR,
+	C_PIO_V1_LEN_MEM_BANK1_UNC_ERR,
+	C_PIO_V1_LEN_MEM_BANK0_UNC_ERR,
+	C_PIO_SM_PKT_RESET_PARITY_ERR,
+	C_PIO_PKT_EVICT_FIFO_PARITY_ERR,
+	C_PIO_SBRDCTRL_CRREL_FIFO_PARITY_ERR,
+	C_PIO_SBRDCTL_CRREL_PARITY_ERR,
+	C_PIO_PEC_FIFO_PARITY_ERR,
+	C_PIO_PCC_FIFO_PARITY_ERR,
+	C_PIO_SB_MEM_FIFO1_ERR,
+	C_PIO_SB_MEM_FIFO0_ERR,
+	C_PIO_CSR_PARITY_ERR,
+	C_PIO_WRITE_ADDR_PARITY_ERR,
+	C_PIO_WRITE_BAD_CTXT_ERR,
+/* SendDmaErrStatus */
+	C_SDMA_PCIE_REQ_TRACKING_COR_ERR,
+	C_SDMA_PCIE_REQ_TRACKING_UNC_ERR,
+	C_SDMA_CSR_PARITY_ERR,
+	C_SDMA_RPY_TAG_ERR,
+/* SendEgressErrStatus */
+	C_TX_READ_PIO_MEMORY_CSR_UNC_ERR,
+	C_TX_READ_SDMA_MEMORY_CSR_UNC_ERR,
+	C_TX_EGRESS_FIFO_COR_ERR,
+	C_TX_READ_PIO_MEMORY_COR_ERR,
+	C_TX_READ_SDMA_MEMORY_COR_ERR,
+	C_TX_SB_HDR_COR_ERR,
+	C_TX_CREDIT_OVERRUN_ERR,
+	C_TX_LAUNCH_FIFO8_COR_ERR,
+	C_TX_LAUNCH_FIFO7_COR_ERR,
+	C_TX_LAUNCH_FIFO6_COR_ERR,
+	C_TX_LAUNCH_FIFO5_COR_ERR,
+	C_TX_LAUNCH_FIFO4_COR_ERR,
+	C_TX_LAUNCH_FIFO3_COR_ERR,
+	C_TX_LAUNCH_FIFO2_COR_ERR,
+	C_TX_LAUNCH_FIFO1_COR_ERR,
+	C_TX_LAUNCH_FIFO0_COR_ERR,
+	C_TX_CREDIT_RETURN_VL_ERR,
+	C_TX_HCRC_INSERTION_ERR,
+	C_TX_EGRESS_FIFI_UNC_ERR,
+	C_TX_READ_PIO_MEMORY_UNC_ERR,
+	C_TX_READ_SDMA_MEMORY_UNC_ERR,
+	C_TX_SB_HDR_UNC_ERR,
+	C_TX_CREDIT_RETURN_PARITY_ERR,
+	C_TX_LAUNCH_FIFO8_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO7_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO6_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO5_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO4_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO3_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO2_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO1_UNC_OR_PARITY_ERR,
+	C_TX_LAUNCH_FIFO0_UNC_OR_PARITY_ERR,
+	C_TX_SDMA15_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA14_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA13_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA12_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA11_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA10_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA9_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA8_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA7_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA6_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA5_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA4_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA3_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA2_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA1_DISALLOWED_PACKET_ERR,
+	C_TX_SDMA0_DISALLOWED_PACKET_ERR,
+	C_TX_CONFIG_PARITY_ERR,
+	C_TX_SBRD_CTL_CSR_PARITY_ERR,
+	C_TX_LAUNCH_CSR_PARITY_ERR,
+	C_TX_ILLEGAL_CL_ERR,
+	C_TX_SBRD_CTL_STATE_MACHINE_PARITY_ERR,
+	C_TX_RESERVED_10,
+	C_TX_RESERVED_9,
+	C_TX_SDMA_LAUNCH_INTF_PARITY_ERR,
+	C_TX_PIO_LAUNCH_INTF_PARITY_ERR,
+	C_TX_RESERVED_6,
+	C_TX_INCORRECT_LINK_STATE_ERR,
+	C_TX_LINK_DOWN_ERR,
+	C_TX_EGRESS_FIFO_UNDERRUN_OR_PARITY_ERR,
+	C_TX_RESERVED_2,
+	C_TX_PKT_INTEGRITY_MEM_UNC_ERR,
+	C_TX_PKT_INTEGRITY_MEM_COR_ERR,
+/* SendErrStatus */
+	C_SEND_CSR_WRITE_BAD_ADDR_ERR,
+	C_SEND_CSR_READ_BAD_ADD_ERR,
+	C_SEND_CSR_PARITY_ERR,
+/* SendCtxtErrStatus */
+	C_PIO_WRITE_OUT_OF_BOUNDS_ERR,
+	C_PIO_WRITE_OVERFLOW_ERR,
+	C_PIO_WRITE_CROSSES_BOUNDARY_ERR,
+	C_PIO_DISALLOWED_PACKET_ERR,
+	C_PIO_INCONSISTENT_SOP_ERR,
+/*SendDmaEngErrStatus */
+	C_SDMA_HEADER_REQUEST_FIFO_COR_ERR,
+	C_SDMA_HEADER_STORAGE_COR_ERR,
+	C_SDMA_PACKET_TRACKING_COR_ERR,
+	C_SDMA_ASSEMBLY_COR_ERR,
+	C_SDMA_DESC_TABLE_COR_ERR,
+	C_SDMA_HEADER_REQUEST_FIFO_UNC_ERR,
+	C_SDMA_HEADER_STORAGE_UNC_ERR,
+	C_SDMA_PACKET_TRACKING_UNC_ERR,
+	C_SDMA_ASSEMBLY_UNC_ERR,
+	C_SDMA_DESC_TABLE_UNC_ERR,
+	C_SDMA_TIMEOUT_ERR,
+	C_SDMA_HEADER_LENGTH_ERR,
+	C_SDMA_HEADER_ADDRESS_ERR,
+	C_SDMA_HEADER_SELECT_ERR,
+	C_SMDA_RESERVED_9,
+	C_SDMA_PACKET_DESC_OVERFLOW_ERR,
+	C_SDMA_LENGTH_MISMATCH_ERR,
+	C_SDMA_HALT_ERR,
+	C_SDMA_MEM_READ_ERR,
+	C_SDMA_FIRST_DESC_ERR,
+	C_SDMA_TAIL_OUT_OF_BOUNDS_ERR,
+	C_SDMA_TOO_LONG_ERR,
+	C_SDMA_GEN_MISMATCH_ERR,
+	C_SDMA_WRONG_DW_ERR,
 	DEV_CNTR_LAST  /* Must be kept last */
 };
 
@@ -810,6 +1078,7 @@ enum {
 	C_RX_WORDS,
 	C_SW_LINK_DOWN,
 	C_SW_LINK_UP,
+	C_SW_UNKNOWN_FRAME,
 	C_SW_XMIT_DSCD,
 	C_SW_XMIT_DSCD_VL,
 	C_SW_XMIT_CSTR_ERR,
diff --git a/drivers/staging/rdma/hfi1/chip_registers.h b/drivers/staging/rdma/hfi1/chip_registers.h
index bf45de29d8bd..701e9e1012a6 100644
--- a/drivers/staging/rdma/hfi1/chip_registers.h
+++ b/drivers/staging/rdma/hfi1/chip_registers.h
@@ -318,6 +318,9 @@
 #define DC_LCB_CFG_TX_FIFOS_RADR_RST_VAL_SHIFT 0
 #define DC_LCB_CFG_TX_FIFOS_RESET (DC_LCB_CSRS + 0x000000000008)
 #define DC_LCB_CFG_TX_FIFOS_RESET_VAL_SHIFT 0
+#define DC_LCB_CFG_REINIT_AS_SLAVE (DC_LCB_CSRS + 0x000000000030)
+#define DC_LCB_CFG_CNT_FOR_SKIP_STALL (DC_LCB_CSRS + 0x000000000040)
+#define DC_LCB_CFG_CLK_CNTR (DC_LCB_CSRS + 0x000000000110)
 #define DC_LCB_ERR_CLR (DC_LCB_CSRS + 0x000000000308)
 #define DC_LCB_ERR_EN (DC_LCB_CSRS + 0x000000000310)
 #define DC_LCB_ERR_FLG (DC_LCB_CSRS + 0x000000000300)
@@ -379,7 +382,6 @@
 #define DC_LCB_STS_ROUND_TRIP_LTP_CNT (DC_LCB_CSRS + 0x0000000004B0)
 #define RCV_BUF_OVFL_CNT 10
 #define RCV_CONTEXT_EGR_STALL 22
-#define RCV_CONTEXT_RHQ_STALL 21
 #define RCV_DATA_PKT_CNT 0
 #define RCV_DWORD_CNT 1
 #define RCV_TID_FLOW_GEN_MISMATCH_CNT 20
diff --git a/drivers/staging/rdma/hfi1/common.h b/drivers/staging/rdma/hfi1/common.h
index 5e203239c5b0..5dd92720faae 100644
--- a/drivers/staging/rdma/hfi1/common.h
+++ b/drivers/staging/rdma/hfi1/common.h
@@ -132,13 +132,14 @@
  * HFI1_CAP_RESERVED_MASK bits.
  */
 #define HFI1_CAP_WRITABLE_MASK   (HFI1_CAP_SDMA_AHG |			\
-				 HFI1_CAP_HDRSUPP |			\
-				 HFI1_CAP_MULTI_PKT_EGR |		\
-				 HFI1_CAP_NODROP_RHQ_FULL |		\
-				 HFI1_CAP_NODROP_EGR_FULL |		\
-				 HFI1_CAP_ALLOW_PERM_JKEY |		\
-				 HFI1_CAP_STATIC_RATE_CTRL |		\
-				 HFI1_CAP_PRINT_UNIMPL)
+				  HFI1_CAP_HDRSUPP |			\
+				  HFI1_CAP_MULTI_PKT_EGR |		\
+				  HFI1_CAP_NODROP_RHQ_FULL |		\
+				  HFI1_CAP_NODROP_EGR_FULL |		\
+				  HFI1_CAP_ALLOW_PERM_JKEY |		\
+				  HFI1_CAP_STATIC_RATE_CTRL |		\
+				  HFI1_CAP_PRINT_UNIMPL |		\
+				  HFI1_CAP_TID_UNMAP)
 /*
  * A set of capability bits that are "global" and are not allowed to be
  * set in the user bitmask.
diff --git a/drivers/staging/rdma/hfi1/diag.c b/drivers/staging/rdma/hfi1/diag.c
index 88414d720469..0c8831705664 100644
--- a/drivers/staging/rdma/hfi1/diag.c
+++ b/drivers/staging/rdma/hfi1/diag.c
@@ -78,8 +78,8 @@
 	hfi1_cdbg(SNOOP, fmt, ##__VA_ARGS__)
 
 /* Snoop option mask */
-#define SNOOP_DROP_SEND	(1 << 0)
-#define SNOOP_USE_METADATA	(1 << 1)
+#define SNOOP_DROP_SEND		BIT(0)
+#define SNOOP_USE_METADATA	BIT(1)
 
 static u8 snoop_flags;
 
@@ -135,7 +135,7 @@ static struct cdev diagpkt_cdev;
 static struct device *diagpkt_device;
 
 static ssize_t diagpkt_write(struct file *fp, const char __user *data,
-				 size_t count, loff_t *off);
+			     size_t count, loff_t *off);
 
 static const struct file_operations diagpkt_file_ops = {
 	.owner = THIS_MODULE,
@@ -177,37 +177,37 @@ struct hfi1_link_info {
 #define HFI1_SNOOP_IOCGETLINKSTATE \
 	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ)
 #define HFI1_SNOOP_IOCSETLINKSTATE \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+1)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 1)
 #define HFI1_SNOOP_IOCCLEARQUEUE \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+2)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 2)
 #define HFI1_SNOOP_IOCCLEARFILTER \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+3)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 3)
 #define HFI1_SNOOP_IOCSETFILTER \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+4)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 4)
 #define HFI1_SNOOP_IOCGETVERSION \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+5)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 5)
 #define HFI1_SNOOP_IOCSET_OPTS \
-	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+6)
+	_IO(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 6)
 
 /*
  * These offsets +6/+7 could change, but these are already known and used
  * IOCTL numbers so don't change them without a good reason.
  */
 #define HFI1_SNOOP_IOCGETLINKSTATE_EXTRA \
-	_IOWR(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+6, \
+	_IOWR(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 6, \
 		struct hfi1_link_info)
 #define HFI1_SNOOP_IOCSETLINKSTATE_EXTRA \
-	_IOWR(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ+7, \
+	_IOWR(HFI1_SNOOP_IOC_MAGIC, HFI1_SNOOP_IOC_BASE_SEQ + 7, \
 		struct hfi1_link_info)
 
 static int hfi1_snoop_open(struct inode *in, struct file *fp);
 static ssize_t hfi1_snoop_read(struct file *fp, char __user *data,
-				size_t pkt_len, loff_t *off);
+			       size_t pkt_len, loff_t *off);
 static ssize_t hfi1_snoop_write(struct file *fp, const char __user *data,
-				 size_t count, loff_t *off);
+				size_t count, loff_t *off);
 static long hfi1_ioctl(struct file *fp, unsigned int cmd, unsigned long arg);
 static unsigned int hfi1_snoop_poll(struct file *fp,
-					struct poll_table_struct *wait);
+				    struct poll_table_struct *wait);
 static int hfi1_snoop_release(struct inode *in, struct file *fp);
 
 struct hfi1_packet_filter_command {
@@ -323,14 +323,12 @@ static void hfi1_snoop_remove(struct hfi1_devdata *dd)
 
 void hfi1_diag_remove(struct hfi1_devdata *dd)
 {
-
 	hfi1_snoop_remove(dd);
 	if (atomic_dec_and_test(&diagpkt_count))
 		hfi1_cdev_cleanup(&diagpkt_cdev, &diagpkt_device);
 	hfi1_cdev_cleanup(&dd->diag_cdev, &dd->diag_device);
 }
 
-
 /*
  * Allocated structure shared between the credit return mechanism and
  * diagpkt_send().
@@ -379,6 +377,7 @@ static ssize_t diagpkt_send(struct diag_pkt *dp)
 	pio_release_cb credit_cb = NULL;
 	void *credit_arg = NULL;
 	struct diagpkt_wait *wait = NULL;
+	int trycount = 0;
 
 	dd = hfi1_lookup(dp->unit);
 	if (!dd || !(dd->flags & HFI1_PRESENT) || !dd->kregbase) {
@@ -393,7 +392,7 @@ static ssize_t diagpkt_send(struct diag_pkt *dp)
 
 	if (dp->version != _DIAG_PKT_VERS) {
 		dd_dev_err(dd, "Invalid version %u for diagpkt_write\n",
-			    dp->version);
+			   dp->version);
 		ret = -EINVAL;
 		goto bail;
 	}
@@ -440,7 +439,7 @@ static ssize_t diagpkt_send(struct diag_pkt *dp)
 	}
 
 	if (copy_from_user(tmpbuf,
-			   (const void __user *) (unsigned long) dp->data,
+			   (const void __user *)(unsigned long)dp->data,
 			   dp->len)) {
 		ret = -EFAULT;
 		goto bail;
@@ -493,8 +492,15 @@ static ssize_t diagpkt_send(struct diag_pkt *dp)
 		credit_arg = wait;
 	}
 
+retry:
 	pbuf = sc_buffer_alloc(sc, total_len, credit_cb, credit_arg);
 	if (!pbuf) {
+		if (trycount == 0) {
+			/* force a credit return and try again */
+			sc_return_credits(sc);
+			trycount = 1;
+			goto retry;
+		}
 		/*
 		 * No send buffer means no credit callback.  Undo
 		 * the wait set-up that was done above.  We free wait
@@ -530,9 +536,9 @@ static ssize_t diagpkt_send(struct diag_pkt *dp)
 		 * NOTE: PRC_FILL_ERR is at best informational and cannot
 		 * be depended on.
 		 */
-		if (!ret && (((wait->code & PRC_STATUS_ERR)
-				|| (wait->code & PRC_FILL_ERR)
-				|| (wait->code & PRC_SC_DISABLE))))
+		if (!ret && (((wait->code & PRC_STATUS_ERR) ||
+			      (wait->code & PRC_FILL_ERR) ||
+			      (wait->code & PRC_SC_DISABLE))))
 			ret = -EIO;
 
 		put_diagpkt_wait(wait);	/* finished with the structure */
@@ -545,7 +551,7 @@ bail:
 }
 
 static ssize_t diagpkt_write(struct file *fp, const char __user *data,
-				 size_t count, loff_t *off)
+			     size_t count, loff_t *off)
 {
 	struct hfi1_devdata *dd;
 	struct send_context *sc;
@@ -565,7 +571,7 @@ static ssize_t diagpkt_write(struct file *fp, const char __user *data,
 	*/
 	if (dp.pbc) {
 		dd = hfi1_lookup(dp.unit);
-		if (dd == NULL)
+		if (!dd)
 			return -ENODEV;
 		vl = (dp.pbc >> PBC_VL_SHIFT) & PBC_VL_MASK;
 		sc = dd->vld[vl].sc;
@@ -598,7 +604,7 @@ static int hfi1_snoop_add(struct hfi1_devdata *dd, const char *name)
 	if (ret) {
 		dd_dev_err(dd, "Couldn't create %s device: %d", name, ret);
 		hfi1_cdev_cleanup(&dd->hfi1_snoop.cdev,
-				 &dd->hfi1_snoop.class_dev);
+				  &dd->hfi1_snoop.class_dev);
 	}
 
 	return ret;
@@ -611,7 +617,6 @@ static struct hfi1_devdata *hfi1_dd_from_sc_inode(struct inode *in)
 
 	dd = hfi1_lookup(unit);
 	return dd;
-
 }
 
 /* clear or restore send context integrity checks */
@@ -652,7 +657,7 @@ static int hfi1_snoop_open(struct inode *in, struct file *fp)
 	mutex_lock(&hfi1_mutex);
 
 	dd = hfi1_dd_from_sc_inode(in);
-	if (dd == NULL) {
+	if (!dd) {
 		ret = -ENODEV;
 		goto bail;
 	}
@@ -739,7 +744,7 @@ static int hfi1_snoop_release(struct inode *in, struct file *fp)
 	int mode_flag;
 
 	dd = hfi1_dd_from_sc_inode(in);
-	if (dd == NULL)
+	if (!dd)
 		return -ENODEV;
 
 	spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
@@ -794,7 +799,7 @@ static unsigned int hfi1_snoop_poll(struct file *fp,
 	struct hfi1_devdata *dd;
 
 	dd = hfi1_dd_from_sc_inode(fp->f_inode);
-	if (dd == NULL)
+	if (!dd)
 		return -ENODEV;
 
 	spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
@@ -805,7 +810,6 @@ static unsigned int hfi1_snoop_poll(struct file *fp,
 
 	spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
 	return ret;
-
 }
 
 static ssize_t hfi1_snoop_write(struct file *fp, const char __user *data,
@@ -822,7 +826,7 @@ static ssize_t hfi1_snoop_write(struct file *fp, const char __user *data,
 	struct hfi1_pportdata *ppd;
 
 	dd = hfi1_dd_from_sc_inode(fp->f_inode);
-	if (dd == NULL)
+	if (!dd)
 		return -ENODEV;
 
 	ppd = dd->pport;
@@ -847,7 +851,7 @@ static ssize_t hfi1_snoop_write(struct file *fp, const char __user *data,
 		if (copy_from_user(&byte_one, data, 1))
 			return -EINVAL;
 
-		if (copy_from_user(&byte_two, data+1, 1))
+		if (copy_from_user(&byte_two, data + 1, 1))
 			return -EINVAL;
 
 		sc4 = (byte_one >> 4) & 0xf;
@@ -920,7 +924,7 @@ static ssize_t hfi1_snoop_read(struct file *fp, char __user *data,
 	struct hfi1_devdata *dd;
 
 	dd = hfi1_dd_from_sc_inode(fp->f_inode);
-	if (dd == NULL)
+	if (!dd)
 		return -ENODEV;
 
 	spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
@@ -946,16 +950,18 @@ static ssize_t hfi1_snoop_read(struct file *fp, char __user *data,
 		spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
 		if (pkt_len >= packet->total_len) {
 			if (copy_to_user(data, packet->data,
-				packet->total_len))
+					 packet->total_len))
 				ret = -EFAULT;
 			else
 				ret = packet->total_len;
-		} else
+		} else {
 			ret = -EINVAL;
+		}
 
 		kfree(packet);
-	} else
+	} else {
 		spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
+	}
 
 	return ret;
 }
@@ -966,9 +972,9 @@ static long hfi1_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
 	void *filter_value = NULL;
 	long ret = 0;
 	int value = 0;
-	u8 physState = 0;
-	u8 linkState = 0;
-	u16 devState = 0;
+	u8 phys_state = 0;
+	u8 link_state = 0;
+	u16 dev_state = 0;
 	unsigned long flags = 0;
 	unsigned long *argp = NULL;
 	struct hfi1_packet_filter_command filter_cmd = {0};
@@ -976,237 +982,221 @@ static long hfi1_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
 	struct hfi1_pportdata *ppd = NULL;
 	unsigned int index;
 	struct hfi1_link_info link_info;
+	int read_cmd, write_cmd, read_ok, write_ok;
 
 	dd = hfi1_dd_from_sc_inode(fp->f_inode);
-	if (dd == NULL)
+	if (!dd)
 		return -ENODEV;
 
-	spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
-
 	mode_flag = dd->hfi1_snoop.mode_flag;
+	read_cmd = _IOC_DIR(cmd) & _IOC_READ;
+	write_cmd = _IOC_DIR(cmd) & _IOC_WRITE;
+	write_ok = access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd));
+	read_ok = access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd));
 
-	if (((_IOC_DIR(cmd) & _IOC_READ)
-	    && !access_ok(VERIFY_WRITE, (void __user *)arg, _IOC_SIZE(cmd)))
-	    || ((_IOC_DIR(cmd) & _IOC_WRITE)
-	    && !access_ok(VERIFY_READ, (void __user *)arg, _IOC_SIZE(cmd)))) {
-		ret = -EFAULT;
-	} else if (!capable(CAP_SYS_ADMIN)) {
-		ret = -EPERM;
-	} else if ((mode_flag & HFI1_PORT_CAPTURE_MODE) &&
-		   (cmd != HFI1_SNOOP_IOCCLEARQUEUE) &&
-		   (cmd != HFI1_SNOOP_IOCCLEARFILTER) &&
-		   (cmd != HFI1_SNOOP_IOCSETFILTER)) {
+	if ((read_cmd && !write_ok) || (write_cmd && !read_ok))
+		return -EFAULT;
+
+	if (!capable(CAP_SYS_ADMIN))
+		return -EPERM;
+
+	if ((mode_flag & HFI1_PORT_CAPTURE_MODE) &&
+	    (cmd != HFI1_SNOOP_IOCCLEARQUEUE) &&
+	    (cmd != HFI1_SNOOP_IOCCLEARFILTER) &&
+	    (cmd != HFI1_SNOOP_IOCSETFILTER))
 		/* Capture devices are allowed only 3 operations
 		 * 1.Clear capture queue
 		 * 2.Clear capture filter
 		 * 3.Set capture filter
 		 * Other are invalid.
 		 */
-		ret = -EINVAL;
-	} else {
-		switch (cmd) {
-		case HFI1_SNOOP_IOCSETLINKSTATE:
-			snoop_dbg("HFI1_SNOOP_IOCSETLINKSTATE is not valid");
-			ret = -EINVAL;
-			break;
+		return -EINVAL;
 
-		case HFI1_SNOOP_IOCSETLINKSTATE_EXTRA:
-			memset(&link_info, 0, sizeof(link_info));
+	switch (cmd) {
+	case HFI1_SNOOP_IOCSETLINKSTATE_EXTRA:
+		memset(&link_info, 0, sizeof(link_info));
 
-			if (copy_from_user(&link_info,
-				(struct hfi1_link_info __user *)arg,
-				sizeof(link_info)))
-				ret = -EFAULT;
+		if (copy_from_user(&link_info,
+				   (struct hfi1_link_info __user *)arg,
+				   sizeof(link_info)))
+			return -EFAULT;
 
-			value = link_info.port_state;
-			index = link_info.port_number;
-			if (index > dd->num_pports - 1) {
-				ret = -EINVAL;
-				break;
-			}
+		value = link_info.port_state;
+		index = link_info.port_number;
+		if (index > dd->num_pports - 1)
+			return -EINVAL;
 
-			ppd = &dd->pport[index];
-			if (!ppd) {
-				ret = -EINVAL;
-				break;
-			}
+		ppd = &dd->pport[index];
+		if (!ppd)
+			return -EINVAL;
 
-			/* What we want to transition to */
-			physState = (value >> 4) & 0xF;
-			linkState = value & 0xF;
-			snoop_dbg("Setting link state 0x%x", value);
-
-			switch (linkState) {
-			case IB_PORT_NOP:
-				if (physState == 0)
-					break;
-					/* fall through */
-			case IB_PORT_DOWN:
-				switch (physState) {
-				case 0:
-					devState = HLS_DN_DOWNDEF;
-					break;
-				case 2:
-					devState = HLS_DN_POLL;
-					break;
-				case 3:
-					devState = HLS_DN_DISABLE;
-					break;
-				default:
-					ret = -EINVAL;
-					goto done;
-				}
-				ret = set_link_state(ppd, devState);
-				break;
-			case IB_PORT_ARMED:
-				ret = set_link_state(ppd, HLS_UP_ARMED);
-				if (!ret)
-					send_idle_sma(dd, SMA_IDLE_ARM);
-				break;
-			case IB_PORT_ACTIVE:
-				ret = set_link_state(ppd, HLS_UP_ACTIVE);
-				if (!ret)
-					send_idle_sma(dd, SMA_IDLE_ACTIVE);
-				break;
-			default:
-				ret = -EINVAL;
-				break;
-			}
+		/* What we want to transition to */
+		phys_state = (value >> 4) & 0xF;
+		link_state = value & 0xF;
+		snoop_dbg("Setting link state 0x%x", value);
 
-			if (ret)
+		switch (link_state) {
+		case IB_PORT_NOP:
+			if (phys_state == 0)
 				break;
-			/* fall through */
-		case HFI1_SNOOP_IOCGETLINKSTATE:
-		case HFI1_SNOOP_IOCGETLINKSTATE_EXTRA:
-			if (cmd == HFI1_SNOOP_IOCGETLINKSTATE_EXTRA) {
-				memset(&link_info, 0, sizeof(link_info));
-				if (copy_from_user(&link_info,
-					(struct hfi1_link_info __user *)arg,
-					sizeof(link_info)))
-					ret = -EFAULT;
-				index = link_info.port_number;
-			} else {
-				ret = __get_user(index, (int __user *) arg);
-				if (ret !=  0)
-					break;
-			}
-
-			if (index > dd->num_pports - 1) {
-				ret = -EINVAL;
+				/* fall through */
+		case IB_PORT_DOWN:
+			switch (phys_state) {
+			case 0:
+				dev_state = HLS_DN_DOWNDEF;
 				break;
-			}
-
-			ppd = &dd->pport[index];
-			if (!ppd) {
-				ret = -EINVAL;
+			case 2:
+				dev_state = HLS_DN_POLL;
 				break;
+			case 3:
+				dev_state = HLS_DN_DISABLE;
+				break;
+			default:
+				return -EINVAL;
 			}
-			value = hfi1_ibphys_portstate(ppd);
-			value <<= 4;
-			value |= driver_lstate(ppd);
-
-			snoop_dbg("Link port | Link State: %d", value);
-
-			if ((cmd == HFI1_SNOOP_IOCGETLINKSTATE_EXTRA) ||
-			    (cmd == HFI1_SNOOP_IOCSETLINKSTATE_EXTRA)) {
-				link_info.port_state = value;
-				link_info.node_guid = cpu_to_be64(ppd->guid);
-				link_info.link_speed_active =
-							ppd->link_speed_active;
-				link_info.link_width_active =
-							ppd->link_width_active;
-				if (copy_to_user(
-					(struct hfi1_link_info __user *)arg,
-					&link_info, sizeof(link_info)))
-					ret = -EFAULT;
-			} else {
-				ret = __put_user(value, (int __user *)arg);
-			}
+			ret = set_link_state(ppd, dev_state);
 			break;
-
-		case HFI1_SNOOP_IOCCLEARQUEUE:
-			snoop_dbg("Clearing snoop queue");
-			drain_snoop_list(&dd->hfi1_snoop.queue);
+		case IB_PORT_ARMED:
+			ret = set_link_state(ppd, HLS_UP_ARMED);
+			if (!ret)
+				send_idle_sma(dd, SMA_IDLE_ARM);
 			break;
-
-		case HFI1_SNOOP_IOCCLEARFILTER:
-			snoop_dbg("Clearing filter");
-			if (dd->hfi1_snoop.filter_callback) {
-				/* Drain packets first */
-				drain_snoop_list(&dd->hfi1_snoop.queue);
-				dd->hfi1_snoop.filter_callback = NULL;
-			}
-			kfree(dd->hfi1_snoop.filter_value);
-			dd->hfi1_snoop.filter_value = NULL;
+		case IB_PORT_ACTIVE:
+			ret = set_link_state(ppd, HLS_UP_ACTIVE);
+			if (!ret)
+				send_idle_sma(dd, SMA_IDLE_ACTIVE);
 			break;
+		default:
+			return -EINVAL;
+		}
 
-		case HFI1_SNOOP_IOCSETFILTER:
-			snoop_dbg("Setting filter");
-			/* just copy command structure */
-			argp = (unsigned long *)arg;
-			if (copy_from_user(&filter_cmd, (void __user *)argp,
-					     sizeof(filter_cmd))) {
-				ret = -EFAULT;
-				break;
-			}
-			if (filter_cmd.opcode >= HFI1_MAX_FILTERS) {
-				pr_alert("Invalid opcode in request\n");
-				ret = -EINVAL;
+		if (ret)
+			break;
+		/* fall through */
+	case HFI1_SNOOP_IOCGETLINKSTATE:
+	case HFI1_SNOOP_IOCGETLINKSTATE_EXTRA:
+		if (cmd == HFI1_SNOOP_IOCGETLINKSTATE_EXTRA) {
+			memset(&link_info, 0, sizeof(link_info));
+			if (copy_from_user(&link_info,
+					   (struct hfi1_link_info __user *)arg,
+					   sizeof(link_info)))
+				return -EFAULT;
+			index = link_info.port_number;
+		} else {
+			ret = __get_user(index, (int __user *)arg);
+			if (ret !=  0)
 				break;
-			}
+		}
 
-			snoop_dbg("Opcode %d Len %d Ptr %p",
-				   filter_cmd.opcode, filter_cmd.length,
-				   filter_cmd.value_ptr);
+		if (index > dd->num_pports - 1)
+			return -EINVAL;
 
-			filter_value = kcalloc(filter_cmd.length, sizeof(u8),
-					       GFP_KERNEL);
-			if (!filter_value) {
-				pr_alert("Not enough memory\n");
-				ret = -ENOMEM;
-				break;
-			}
-			/* copy remaining data from userspace */
-			if (copy_from_user((u8 *)filter_value,
-					(void __user *)filter_cmd.value_ptr,
-					filter_cmd.length)) {
-				kfree(filter_value);
-				ret = -EFAULT;
-				break;
-			}
+		ppd = &dd->pport[index];
+		if (!ppd)
+			return -EINVAL;
+
+		value = hfi1_ibphys_portstate(ppd);
+		value <<= 4;
+		value |= driver_lstate(ppd);
+
+		snoop_dbg("Link port | Link State: %d", value);
+
+		if ((cmd == HFI1_SNOOP_IOCGETLINKSTATE_EXTRA) ||
+		    (cmd == HFI1_SNOOP_IOCSETLINKSTATE_EXTRA)) {
+			link_info.port_state = value;
+			link_info.node_guid = cpu_to_be64(ppd->guid);
+			link_info.link_speed_active =
+						ppd->link_speed_active;
+			link_info.link_width_active =
+						ppd->link_width_active;
+			if (copy_to_user((struct hfi1_link_info __user *)arg,
+					 &link_info, sizeof(link_info)))
+				return -EFAULT;
+		} else {
+			ret = __put_user(value, (int __user *)arg);
+		}
+		break;
+
+	case HFI1_SNOOP_IOCCLEARQUEUE:
+		snoop_dbg("Clearing snoop queue");
+		spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
+		drain_snoop_list(&dd->hfi1_snoop.queue);
+		spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
+		break;
+
+	case HFI1_SNOOP_IOCCLEARFILTER:
+		snoop_dbg("Clearing filter");
+		spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
+		if (dd->hfi1_snoop.filter_callback) {
 			/* Drain packets first */
 			drain_snoop_list(&dd->hfi1_snoop.queue);
-			dd->hfi1_snoop.filter_callback =
-				hfi1_filters[filter_cmd.opcode].filter;
-			/* just in case we see back to back sets */
-			kfree(dd->hfi1_snoop.filter_value);
-			dd->hfi1_snoop.filter_value = filter_value;
+			dd->hfi1_snoop.filter_callback = NULL;
+		}
+		kfree(dd->hfi1_snoop.filter_value);
+		dd->hfi1_snoop.filter_value = NULL;
+		spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
+		break;
 
-			break;
-		case HFI1_SNOOP_IOCGETVERSION:
-			value = SNOOP_CAPTURE_VERSION;
-			snoop_dbg("Getting version: %d", value);
-			ret = __put_user(value, (int __user *)arg);
-			break;
-		case HFI1_SNOOP_IOCSET_OPTS:
-			snoop_flags = 0;
-			ret = __get_user(value, (int __user *) arg);
-			if (ret != 0)
-				break;
+	case HFI1_SNOOP_IOCSETFILTER:
+		snoop_dbg("Setting filter");
+		/* just copy command structure */
+		argp = (unsigned long *)arg;
+		if (copy_from_user(&filter_cmd, (void __user *)argp,
+				   sizeof(filter_cmd)))
+			return -EFAULT;
 
-			snoop_dbg("Setting snoop option %d", value);
-			if (value & SNOOP_DROP_SEND)
-				snoop_flags |= SNOOP_DROP_SEND;
-			if (value & SNOOP_USE_METADATA)
-				snoop_flags |= SNOOP_USE_METADATA;
-			break;
-		default:
-			ret = -ENOTTY;
-			break;
+		if (filter_cmd.opcode >= HFI1_MAX_FILTERS) {
+			pr_alert("Invalid opcode in request\n");
+			return -EINVAL;
+		}
+
+		snoop_dbg("Opcode %d Len %d Ptr %p",
+			  filter_cmd.opcode, filter_cmd.length,
+			  filter_cmd.value_ptr);
+
+		filter_value = kcalloc(filter_cmd.length, sizeof(u8),
+				       GFP_KERNEL);
+		if (!filter_value)
+			return -ENOMEM;
+
+		/* copy remaining data from userspace */
+		if (copy_from_user((u8 *)filter_value,
+				   (void __user *)filter_cmd.value_ptr,
+				   filter_cmd.length)) {
+			kfree(filter_value);
+			return -EFAULT;
 		}
+		/* Drain packets first */
+		spin_lock_irqsave(&dd->hfi1_snoop.snoop_lock, flags);
+		drain_snoop_list(&dd->hfi1_snoop.queue);
+		dd->hfi1_snoop.filter_callback =
+			hfi1_filters[filter_cmd.opcode].filter;
+		/* just in case we see back to back sets */
+		kfree(dd->hfi1_snoop.filter_value);
+		dd->hfi1_snoop.filter_value = filter_value;
+		spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
+		break;
+	case HFI1_SNOOP_IOCGETVERSION:
+		value = SNOOP_CAPTURE_VERSION;
+		snoop_dbg("Getting version: %d", value);
+		ret = __put_user(value, (int __user *)arg);
+		break;
+	case HFI1_SNOOP_IOCSET_OPTS:
+		snoop_flags = 0;
+		ret = __get_user(value, (int __user *)arg);
+		if (ret != 0)
+			break;
+
+		snoop_dbg("Setting snoop option %d", value);
+		if (value & SNOOP_DROP_SEND)
+			snoop_flags |= SNOOP_DROP_SEND;
+		if (value & SNOOP_USE_METADATA)
+			snoop_flags |= SNOOP_USE_METADATA;
+		break;
+	default:
+		return -ENOTTY;
 	}
-done:
-	spin_unlock_irqrestore(&dd->hfi1_snoop.snoop_lock, flags);
+
 	return ret;
 }
 
@@ -1321,7 +1311,6 @@ static int hfi1_filter_mad_mgmt_class(void *ibhdr, void *packet_data,
 
 static int hfi1_filter_qp_number(void *ibhdr, void *packet_data, void *value)
 {
-
 	struct hfi1_ib_header *hdr;
 	struct hfi1_other_headers *ohdr = NULL;
 	int ret;
@@ -1404,7 +1393,6 @@ static int hfi1_filter_ib_service_level(void *ibhdr, void *packet_data,
 
 static int hfi1_filter_ib_pkey(void *ibhdr, void *packet_data, void *value)
 {
-
 	u32 lnh = 0;
 	struct hfi1_ib_header *hdr;
 	struct hfi1_other_headers *ohdr = NULL;
@@ -1476,16 +1464,14 @@ static struct snoop_packet *allocate_snoop_packet(u32 hdr_len,
 						  u32 data_len,
 						  u32 md_len)
 {
-
 	struct snoop_packet *packet;
 
-	packet = kzalloc(sizeof(struct snoop_packet) + hdr_len + data_len
+	packet = kzalloc(sizeof(*packet) + hdr_len + data_len
 			 + md_len,
 			 GFP_ATOMIC | __GFP_NOWARN);
 	if (likely(packet))
 		INIT_LIST_HEAD(&packet->list);
 
-
 	return packet;
 }
 
@@ -1542,12 +1528,11 @@ int snoop_recv_handler(struct hfi1_packet *packet)
 		    unlikely(snoop_flags & SNOOP_USE_METADATA))
 			md_len = sizeof(struct capture_md);
 
-
 		s_packet = allocate_snoop_packet(header_size,
 						 tlen - header_size,
 						 md_len);
 
-		if (unlikely(s_packet == NULL)) {
+		if (unlikely(!s_packet)) {
 			dd_dev_warn_ratelimited(ppd->dd, "Unable to allocate snoop/capture packet\n");
 			break;
 		}
@@ -1618,14 +1603,12 @@ int snoop_recv_handler(struct hfi1_packet *packet)
 /*
  * Handle snooping and capturing packets when sdma is being used.
  */
-int snoop_send_dma_handler(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
-			   u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			   u32 plen, u32 dwords, u64 pbc)
+int snoop_send_dma_handler(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			   u64 pbc)
 {
-	pr_alert("Snooping/Capture of  Send DMA Packets Is Not Supported!\n");
+	pr_alert("Snooping/Capture of Send DMA Packets Is Not Supported!\n");
 	snoop_dbg("Unsupported Operation");
-	return hfi1_verbs_send_dma(qp, ibhdr, hdrwords, ss, len, plen, dwords,
-				  0);
+	return hfi1_verbs_send_dma(qp, ps, 0);
 }
 
 /*
@@ -1633,12 +1616,16 @@ int snoop_send_dma_handler(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
  * bypass packets. The only way to send a bypass packet currently is to use the
  * diagpkt interface. When that interface is enable snoop/capture is not.
  */
-int snoop_send_pio_handler(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
-			   u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			   u32 plen, u32 dwords, u64 pbc)
+int snoop_send_pio_handler(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			   u64 pbc)
 {
-	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct ahg_ib_header *ahdr = qp->s_hdr;
+	u32 hdrwords = qp->s_hdrwords;
+	struct hfi1_sge_state *ss = qp->s_cur_sge;
+	u32 len = qp->s_cur_size;
+	u32 dwords = (len + 3) >> 2;
+	u32 plen = hdrwords + dwords + 2; /* includes pbc */
+	struct hfi1_pportdata *ppd = ps->ppd;
 	struct snoop_packet *s_packet = NULL;
 	u32 *hdr = (u32 *)&ahdr->ibh;
 	u32 length = 0;
@@ -1666,7 +1653,7 @@ int snoop_send_pio_handler(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
 	/* not using ss->total_len as arg 2 b/c that does not count CRC */
 	s_packet = allocate_snoop_packet(hdr_len, tlen - hdr_len, md_len);
 
-	if (unlikely(s_packet == NULL)) {
+	if (unlikely(!s_packet)) {
 		dd_dev_warn_ratelimited(ppd->dd, "Unable to allocate snoop/capture packet\n");
 		goto out;
 	}
@@ -1783,8 +1770,7 @@ int snoop_send_pio_handler(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
 		break;
 	}
 out:
-	return hfi1_verbs_send_pio(qp, ahdr, hdrwords, ss, len, plen, dwords,
-				  md.u.pbc);
+	return hfi1_verbs_send_pio(qp, ps, md.u.pbc);
 }
 
 /*
@@ -1836,7 +1822,7 @@ void snoop_inline_pio_send(struct hfi1_devdata *dd, struct pio_buf *pbuf,
 
 		s_packet = allocate_snoop_packet(packet_len, 0, md_len);
 
-		if (unlikely(s_packet == NULL)) {
+		if (unlikely(!s_packet)) {
 			dd_dev_warn_ratelimited(dd, "Unable to allocate snoop/capture packet\n");
 			goto inline_pio_out;
 		}
@@ -1868,5 +1854,4 @@ void snoop_inline_pio_send(struct hfi1_devdata *dd, struct pio_buf *pbuf,
 
 inline_pio_out:
 	pio_copy(dd, pbuf, pbc, from, count);
-
 }
diff --git a/drivers/staging/rdma/hfi1/driver.c b/drivers/staging/rdma/hfi1/driver.c
index ce69141b56cb..8485de1fce08 100644
--- a/drivers/staging/rdma/hfi1/driver.c
+++ b/drivers/staging/rdma/hfi1/driver.c
@@ -158,7 +158,7 @@ const char *get_unit_name(int unit)
 {
 	static char iname[16];
 
-	snprintf(iname, sizeof(iname), DRIVER_NAME"_%u", unit);
+	snprintf(iname, sizeof(iname), DRIVER_NAME "_%u", unit);
 	return iname;
 }
 
@@ -436,59 +436,58 @@ static inline void init_packet(struct hfi1_ctxtdata *rcd,
 
 #ifndef CONFIG_PRESCAN_RXQ
 static void prescan_rxq(struct hfi1_packet *packet) {}
-#else /* CONFIG_PRESCAN_RXQ */
+#else /* !CONFIG_PRESCAN_RXQ */
 static int prescan_receive_queue;
 
 static void process_ecn(struct hfi1_qp *qp, struct hfi1_ib_header *hdr,
 			struct hfi1_other_headers *ohdr,
-			u64 rhf, struct ib_grh *grh)
+			u64 rhf, u32 bth1, struct ib_grh *grh)
 {
 	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	u32 bth1;
+	u32 rqpn = 0;
+	u16 rlid;
 	u8 sc5, svc_type;
-	int is_fecn, is_becn;
 
 	switch (qp->ibqp.qp_type) {
+	case IB_QPT_SMI:
+	case IB_QPT_GSI:
 	case IB_QPT_UD:
+		rlid = be16_to_cpu(hdr->lrh[3]);
+		rqpn = be32_to_cpu(ohdr->u.ud.deth[1]) & HFI1_QPN_MASK;
 		svc_type = IB_CC_SVCTYPE_UD;
 		break;
-	case IB_QPT_UC:	/* LATER */
-	case IB_QPT_RC:	/* LATER */
+	case IB_QPT_UC:
+		rlid = qp->remote_ah_attr.dlid;
+		rqpn = qp->remote_qpn;
+		svc_type = IB_CC_SVCTYPE_UC;
+		break;
+	case IB_QPT_RC:
+		rlid = qp->remote_ah_attr.dlid;
+		rqpn = qp->remote_qpn;
+		svc_type = IB_CC_SVCTYPE_RC;
+		break;
 	default:
 		return;
 	}
 
-	is_fecn = (be32_to_cpu(ohdr->bth[1]) >> HFI1_FECN_SHIFT) &
-			HFI1_FECN_MASK;
-	is_becn = (be32_to_cpu(ohdr->bth[1]) >> HFI1_BECN_SHIFT) &
-			HFI1_BECN_MASK;
-
 	sc5 = (be16_to_cpu(hdr->lrh[0]) >> 12) & 0xf;
 	if (rhf_dc_info(rhf))
 		sc5 |= 0x10;
 
-	if (is_fecn) {
-		u32 src_qpn = be32_to_cpu(ohdr->u.ud.deth[1]) & HFI1_QPN_MASK;
+	if (bth1 & HFI1_FECN_SMASK) {
 		u16 pkey = (u16)be32_to_cpu(ohdr->bth[0]);
 		u16 dlid = be16_to_cpu(hdr->lrh[1]);
-		u16 slid = be16_to_cpu(hdr->lrh[3]);
 
-		return_cnp(ibp, qp, src_qpn, pkey, dlid, slid, sc5, grh);
+		return_cnp(ibp, qp, rqpn, pkey, dlid, rlid, sc5, grh);
 	}
 
-	if (is_becn) {
+	if (bth1 & HFI1_BECN_SMASK) {
 		struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
-		u32 lqpn =  be32_to_cpu(ohdr->bth[1]) & HFI1_QPN_MASK;
+		u32 lqpn = bth1 & HFI1_QPN_MASK;
 		u8 sl = ibp->sc_to_sl[sc5];
 
-		process_becn(ppd, sl, 0, lqpn, 0, svc_type);
+		process_becn(ppd, sl, rlid, lqpn, rqpn, svc_type);
 	}
-
-	/* turn off BECN, or FECN */
-	bth1 = be32_to_cpu(ohdr->bth[1]);
-	bth1 &= ~(HFI1_FECN_MASK << HFI1_FECN_SHIFT);
-	bth1 &= ~(HFI1_BECN_MASK << HFI1_BECN_SHIFT);
-	ohdr->bth[1] = cpu_to_be32(bth1);
 }
 
 struct ps_mdata {
@@ -508,38 +507,51 @@ static inline void init_ps_mdata(struct ps_mdata *mdata,
 	mdata->rcd = rcd;
 	mdata->rsize = packet->rsize;
 	mdata->maxcnt = packet->maxcnt;
+	mdata->ps_head = packet->rhqoff;
 
-	if (rcd->ps_state.initialized == 0) {
-		mdata->ps_head = packet->rhqoff;
-		rcd->ps_state.initialized++;
-	} else
-		mdata->ps_head = rcd->ps_state.ps_head;
-
-	if (HFI1_CAP_IS_KSET(DMA_RTAIL)) {
-		mdata->ps_tail = packet->hdrqtail;
-		mdata->ps_seq = 0; /* not used with DMA_RTAIL */
+	if (HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL)) {
+		mdata->ps_tail = get_rcvhdrtail(rcd);
+		if (rcd->ctxt == HFI1_CTRL_CTXT)
+			mdata->ps_seq = rcd->seq_cnt;
+		else
+			mdata->ps_seq = 0; /* not used with DMA_RTAIL */
 	} else {
 		mdata->ps_tail = 0; /* used only with DMA_RTAIL*/
 		mdata->ps_seq = rcd->seq_cnt;
 	}
 }
 
-static inline int ps_done(struct ps_mdata *mdata, u64 rhf)
+static inline int ps_done(struct ps_mdata *mdata, u64 rhf,
+			  struct hfi1_ctxtdata *rcd)
 {
-	if (HFI1_CAP_IS_KSET(DMA_RTAIL))
+	if (HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL))
 		return mdata->ps_head == mdata->ps_tail;
 	return mdata->ps_seq != rhf_rcv_seq(rhf);
 }
 
-static inline void update_ps_mdata(struct ps_mdata *mdata)
+static inline int ps_skip(struct ps_mdata *mdata, u64 rhf,
+			  struct hfi1_ctxtdata *rcd)
 {
-	struct hfi1_ctxtdata *rcd = mdata->rcd;
+	/*
+	 * Control context can potentially receive an invalid rhf.
+	 * Drop such packets.
+	 */
+	if ((rcd->ctxt == HFI1_CTRL_CTXT) && (mdata->ps_head != mdata->ps_tail))
+		return mdata->ps_seq != rhf_rcv_seq(rhf);
+
+	return 0;
+}
 
+static inline void update_ps_mdata(struct ps_mdata *mdata,
+				   struct hfi1_ctxtdata *rcd)
+{
 	mdata->ps_head += mdata->rsize;
-	if (mdata->ps_head > mdata->maxcnt)
+	if (mdata->ps_head >= mdata->maxcnt)
 		mdata->ps_head = 0;
-	rcd->ps_state.ps_head = mdata->ps_head;
-	if (!HFI1_CAP_IS_KSET(DMA_RTAIL)) {
+
+	/* Control context must do seq counting */
+	if (!HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL) ||
+	    (rcd->ctxt == HFI1_CTRL_CTXT)) {
 		if (++mdata->ps_seq > 13)
 			mdata->ps_seq = 1;
 	}
@@ -571,13 +583,16 @@ static void prescan_rxq(struct hfi1_packet *packet)
 		struct hfi1_other_headers *ohdr;
 		struct ib_grh *grh = NULL;
 		u64 rhf = rhf_to_cpu(rhf_addr);
-		u32 etype = rhf_rcv_type(rhf), qpn;
+		u32 etype = rhf_rcv_type(rhf), qpn, bth1;
 		int is_ecn = 0;
 		u8 lnh;
 
-		if (ps_done(&mdata, rhf))
+		if (ps_done(&mdata, rhf, rcd))
 			break;
 
+		if (ps_skip(&mdata, rhf, rcd))
+			goto next;
+
 		if (etype != RHF_RCV_TYPE_IB)
 			goto next;
 
@@ -593,15 +608,13 @@ static void prescan_rxq(struct hfi1_packet *packet)
 		} else
 			goto next; /* just in case */
 
-		is_ecn |= be32_to_cpu(ohdr->bth[1]) &
-			(HFI1_FECN_MASK << HFI1_FECN_SHIFT);
-		is_ecn |= be32_to_cpu(ohdr->bth[1]) &
-			(HFI1_BECN_MASK << HFI1_BECN_SHIFT);
+		bth1 = be32_to_cpu(ohdr->bth[1]);
+		is_ecn = !!(bth1 & (HFI1_FECN_SMASK | HFI1_BECN_SMASK));
 
 		if (!is_ecn)
 			goto next;
 
-		qpn = be32_to_cpu(ohdr->bth[1]) & HFI1_QPN_MASK;
+		qpn = bth1 & HFI1_QPN_MASK;
 		rcu_read_lock();
 		qp = hfi1_lookup_qpn(ibp, qpn);
 
@@ -610,14 +623,44 @@ static void prescan_rxq(struct hfi1_packet *packet)
 			goto next;
 		}
 
-		process_ecn(qp, hdr, ohdr, rhf, grh);
+		process_ecn(qp, hdr, ohdr, rhf, bth1, grh);
 		rcu_read_unlock();
+
+		/* turn off BECN, FECN */
+		bth1 &= ~(HFI1_FECN_SMASK | HFI1_BECN_SMASK);
+		ohdr->bth[1] = cpu_to_be32(bth1);
 next:
-		update_ps_mdata(&mdata);
+		update_ps_mdata(&mdata, rcd);
 	}
 }
 #endif /* CONFIG_PRESCAN_RXQ */
 
+static inline int skip_rcv_packet(struct hfi1_packet *packet, int thread)
+{
+	int ret = RCV_PKT_OK;
+
+	/* Set up for the next packet */
+	packet->rhqoff += packet->rsize;
+	if (packet->rhqoff >= packet->maxcnt)
+		packet->rhqoff = 0;
+
+	packet->numpkt++;
+	if (unlikely((packet->numpkt & (MAX_PKT_RECV - 1)) == 0)) {
+		if (thread) {
+			cond_resched();
+		} else {
+			ret = RCV_PKT_LIMIT;
+			this_cpu_inc(*packet->rcd->dd->rcv_limit);
+		}
+	}
+
+	packet->rhf_addr = (__le32 *)packet->rcd->rcvhdrq + packet->rhqoff +
+				     packet->rcd->dd->rhf_offset;
+	packet->rhf = rhf_to_cpu(packet->rhf_addr);
+
+	return ret;
+}
+
 static inline int process_rcv_packet(struct hfi1_packet *packet, int thread)
 {
 	int ret = RCV_PKT_OK;
@@ -721,8 +764,8 @@ static inline void process_rcv_qp_work(struct hfi1_packet *packet)
 	 */
 	list_for_each_entry_safe(qp, nqp, &rcd->qp_wait_list, rspwait) {
 		list_del_init(&qp->rspwait);
-		if (qp->r_flags & HFI1_R_RSP_NAK) {
-			qp->r_flags &= ~HFI1_R_RSP_NAK;
+		if (qp->r_flags & HFI1_R_RSP_DEFERED_ACK) {
+			qp->r_flags &= ~HFI1_R_RSP_DEFERED_ACK;
 			hfi1_send_rc_ack(rcd, qp, 0);
 		}
 		if (qp->r_flags & HFI1_R_RSP_SEND) {
@@ -791,7 +834,6 @@ int handle_receive_interrupt_dma_rtail(struct hfi1_ctxtdata *rcd, int thread)
 
 	while (last == RCV_PKT_OK) {
 		last = process_rcv_packet(&packet, thread);
-		hdrqtail = get_rcvhdrtail(rcd);
 		if (packet.rhqoff == hdrqtail)
 			last = RCV_PKT_DONE;
 		process_rcv_update(last, &packet);
@@ -806,7 +848,7 @@ static inline void set_all_nodma_rtail(struct hfi1_devdata *dd)
 {
 	int i;
 
-	for (i = 0; i < dd->first_user_ctxt; i++)
+	for (i = HFI1_CTRL_CTXT + 1; i < dd->first_user_ctxt; i++)
 		dd->rcd[i]->do_interrupt =
 			&handle_receive_interrupt_nodma_rtail;
 }
@@ -815,7 +857,7 @@ static inline void set_all_dma_rtail(struct hfi1_devdata *dd)
 {
 	int i;
 
-	for (i = 0; i < dd->first_user_ctxt; i++)
+	for (i = HFI1_CTRL_CTXT + 1; i < dd->first_user_ctxt; i++)
 		dd->rcd[i]->do_interrupt =
 			&handle_receive_interrupt_dma_rtail;
 }
@@ -831,12 +873,16 @@ int handle_receive_interrupt(struct hfi1_ctxtdata *rcd, int thread)
 {
 	struct hfi1_devdata *dd = rcd->dd;
 	u32 hdrqtail;
-	int last = RCV_PKT_OK, needset = 1;
+	int needset, last = RCV_PKT_OK;
 	struct hfi1_packet packet;
+	int skip_pkt = 0;
+
+	/* Control context will always use the slow path interrupt handler */
+	needset = (rcd->ctxt == HFI1_CTRL_CTXT) ? 0 : 1;
 
 	init_packet(rcd, &packet);
 
-	if (!HFI1_CAP_IS_KSET(DMA_RTAIL)) {
+	if (!HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL)) {
 		u32 seq = rhf_rcv_seq(packet.rhf);
 
 		if (seq != rcd->seq_cnt) {
@@ -851,6 +897,17 @@ int handle_receive_interrupt(struct hfi1_ctxtdata *rcd, int thread)
 			goto bail;
 		}
 		smp_rmb();  /* prevent speculative reads of dma'ed hdrq */
+
+		/*
+		 * Control context can potentially receive an invalid
+		 * rhf. Drop such packets.
+		 */
+		if (rcd->ctxt == HFI1_CTRL_CTXT) {
+			u32 seq = rhf_rcv_seq(packet.rhf);
+
+			if (seq != rcd->seq_cnt)
+				skip_pkt = 1;
+		}
 	}
 
 	prescan_rxq(&packet);
@@ -868,11 +925,14 @@ int handle_receive_interrupt(struct hfi1_ctxtdata *rcd, int thread)
 					  dd->rhf_offset;
 			packet.rhf = rhf_to_cpu(packet.rhf_addr);
 
+		} else if (skip_pkt) {
+			last = skip_rcv_packet(&packet, thread);
+			skip_pkt = 0;
 		} else {
 			last = process_rcv_packet(&packet, thread);
 		}
 
-		if (!HFI1_CAP_IS_KSET(DMA_RTAIL)) {
+		if (!HFI1_CAP_KGET_MASK(rcd->flags, DMA_RTAIL)) {
 			u32 seq = rhf_rcv_seq(packet.rhf);
 
 			if (++rcd->seq_cnt > 13)
@@ -888,6 +948,19 @@ int handle_receive_interrupt(struct hfi1_ctxtdata *rcd, int thread)
 		} else {
 			if (packet.rhqoff == hdrqtail)
 				last = RCV_PKT_DONE;
+			/*
+			 * Control context can potentially receive an invalid
+			 * rhf. Drop such packets.
+			 */
+			if (rcd->ctxt == HFI1_CTRL_CTXT) {
+				u32 seq = rhf_rcv_seq(packet.rhf);
+
+				if (++rcd->seq_cnt > 13)
+					rcd->seq_cnt = 1;
+				if (!last && (seq != rcd->seq_cnt))
+					skip_pkt = 1;
+			}
+
 			if (needset) {
 				dd_dev_info(dd,
 					    "Switching to DMA_RTAIL\n");
@@ -1163,20 +1236,20 @@ void handle_eflags(struct hfi1_packet *packet)
 	struct hfi1_ctxtdata *rcd = packet->rcd;
 	u32 rte = rhf_rcv_type_err(packet->rhf);
 
-	dd_dev_err(rcd->dd,
-		"receive context %d: rhf 0x%016llx, errs [ %s%s%s%s%s%s%s%s] rte 0x%x\n",
-		rcd->ctxt, packet->rhf,
-		packet->rhf & RHF_K_HDR_LEN_ERR ? "k_hdr_len " : "",
-		packet->rhf & RHF_DC_UNC_ERR ? "dc_unc " : "",
-		packet->rhf & RHF_DC_ERR ? "dc " : "",
-		packet->rhf & RHF_TID_ERR ? "tid " : "",
-		packet->rhf & RHF_LEN_ERR ? "len " : "",
-		packet->rhf & RHF_ECC_ERR ? "ecc " : "",
-		packet->rhf & RHF_VCRC_ERR ? "vcrc " : "",
-		packet->rhf & RHF_ICRC_ERR ? "icrc " : "",
-		rte);
-
 	rcv_hdrerr(rcd, rcd->ppd, packet);
+	if (rhf_err_flags(packet->rhf))
+		dd_dev_err(rcd->dd,
+			   "receive context %d: rhf 0x%016llx, errs [ %s%s%s%s%s%s%s%s] rte 0x%x\n",
+			   rcd->ctxt, packet->rhf,
+			   packet->rhf & RHF_K_HDR_LEN_ERR ? "k_hdr_len " : "",
+			   packet->rhf & RHF_DC_UNC_ERR ? "dc_unc " : "",
+			   packet->rhf & RHF_DC_ERR ? "dc " : "",
+			   packet->rhf & RHF_TID_ERR ? "tid " : "",
+			   packet->rhf & RHF_LEN_ERR ? "len " : "",
+			   packet->rhf & RHF_ECC_ERR ? "ecc " : "",
+			   packet->rhf & RHF_VCRC_ERR ? "vcrc " : "",
+			   packet->rhf & RHF_ICRC_ERR ? "icrc " : "",
+			   rte);
 }
 
 /*
diff --git a/drivers/staging/rdma/hfi1/efivar.c b/drivers/staging/rdma/hfi1/efivar.c
new file mode 100644
index 000000000000..7dc5bae220e0
--- /dev/null
+++ b/drivers/staging/rdma/hfi1/efivar.c
@@ -0,0 +1,169 @@
+/*
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  - Neither the name of Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include "efivar.h"
+
+/* GUID for HFI1 variables in EFI */
+#define HFI1_EFIVAR_GUID EFI_GUID(0xc50a953e, 0xa8b2, 0x42a6, \
+		0xbf, 0x89, 0xd3, 0x33, 0xa6, 0xe9, 0xe6, 0xd4)
+/* largest EFI data size we expect */
+#define EFI_DATA_SIZE 4096
+
+/*
+ * Read the named EFI variable.  Return the size of the actual data in *size
+ * and a kmalloc'ed buffer in *return_data.  The caller must free the
+ * data.  It is guaranteed that *return_data will be NULL and *size = 0
+ * if this routine fails.
+ *
+ * Return 0 on success, -errno on failure.
+ */
+static int read_efi_var(const char *name, unsigned long *size,
+			void **return_data)
+{
+	efi_status_t status;
+	efi_char16_t *uni_name;
+	efi_guid_t guid;
+	unsigned long temp_size;
+	void *temp_buffer;
+	void *data;
+	int i;
+	int ret;
+
+	/* set failure return values */
+	*size = 0;
+	*return_data = NULL;
+
+	if (!efi_enabled(EFI_RUNTIME_SERVICES))
+		return -EOPNOTSUPP;
+
+	uni_name = kzalloc(sizeof(efi_char16_t) * (strlen(name) + 1),
+			   GFP_KERNEL);
+	temp_buffer = kzalloc(EFI_DATA_SIZE, GFP_KERNEL);
+
+	if (!uni_name || !temp_buffer) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	/* input: the size of the buffer */
+	temp_size = EFI_DATA_SIZE;
+
+	/* convert ASCII to unicode - it is a 1:1 mapping */
+	for (i = 0; name[i]; i++)
+		uni_name[i] = name[i];
+
+	/* need a variable for our GUID */
+	guid = HFI1_EFIVAR_GUID;
+
+	/* call into EFI runtime services */
+	status = efi.get_variable(
+			uni_name,
+			&guid,
+			NULL,
+			&temp_size,
+			temp_buffer);
+
+	/*
+	 * It would be nice to call efi_status_to_err() here, but that
+	 * is in the EFIVAR_FS code and may not be compiled in.
+	 * However, even that is insufficient since it does not cover
+	 * EFI_BUFFER_TOO_SMALL which could be an important return.
+	 * For now, just split out succces or not found.
+	 */
+	ret = status == EFI_SUCCESS   ? 0 :
+	      status == EFI_NOT_FOUND ? -ENOENT :
+					-EINVAL;
+	if (ret)
+		goto fail;
+
+	/*
+	 * We have successfully read the EFI variable into our
+	 * temporary buffer.  Now allocate a correctly sized
+	 * buffer.
+	 */
+	data = kmalloc(temp_size, GFP_KERNEL);
+	if (!data) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	memcpy(data, temp_buffer, temp_size);
+	*size = temp_size;
+	*return_data = data;
+
+fail:
+	kfree(uni_name);
+	kfree(temp_buffer);
+
+	return ret;
+}
+
+/*
+ * Read an HFI1 EFI variable of the form:
+ *	<PCIe address>-<kind>
+ * Return an kalloc'ed array and size of the data.
+ *
+ * Returns 0 on success, -errno on failure.
+ */
+int read_hfi1_efi_var(struct hfi1_devdata *dd, const char *kind,
+		      unsigned long *size, void **return_data)
+{
+	char name[64];
+
+	/* create a common prefix */
+	snprintf(name, sizeof(name), "%04x:%02x:%02x.%x-%s",
+		 pci_domain_nr(dd->pcidev->bus),
+		 dd->pcidev->bus->number,
+		 PCI_SLOT(dd->pcidev->devfn),
+		 PCI_FUNC(dd->pcidev->devfn),
+		 kind);
+
+	return read_efi_var(name, size, return_data);
+}
diff --git a/drivers/staging/rdma/hfi1/efivar.h b/drivers/staging/rdma/hfi1/efivar.h
new file mode 100644
index 000000000000..070706225c51
--- /dev/null
+++ b/drivers/staging/rdma/hfi1/efivar.h
@@ -0,0 +1,60 @@
+/*
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  - Neither the name of Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+#ifndef _HFI1_EFIVAR_H
+#define _HFI1_EFIVAR_H
+
+#include <linux/efi.h>
+
+#include "hfi.h"
+
+int read_hfi1_efi_var(struct hfi1_devdata *dd, const char *kind,
+		      unsigned long *size, void **return_data);
+
+#endif /* _HFI1_EFIVAR_H */
diff --git a/drivers/staging/rdma/hfi1/eprom.c b/drivers/staging/rdma/hfi1/eprom.c
index b61d3ae93ed1..fb620c97f592 100644
--- a/drivers/staging/rdma/hfi1/eprom.c
+++ b/drivers/staging/rdma/hfi1/eprom.c
@@ -53,17 +53,26 @@
 #include "eprom.h"
 
 /*
- * The EPROM is logically divided into two partitions:
+ * The EPROM is logically divided into three partitions:
  *	partition 0: the first 128K, visible from PCI ROM BAR
- *	partition 1: the rest
+ *	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)
+
+/* erase sizes supported by the controller */
+#define SIZE_4KB (4 * 1024)
+#define MASK_4KB (SIZE_4KB - 1)
 
-/* largest erase size supported by the controller */
 #define SIZE_32KB (32 * 1024)
 #define MASK_32KB (SIZE_32KB - 1)
 
+#define SIZE_64KB (64 * 1024)
+#define MASK_64KB (SIZE_64KB - 1)
+
 /* controller page size, in bytes */
 #define EP_PAGE_SIZE 256
 #define EEP_PAGE_MASK (EP_PAGE_SIZE - 1)
@@ -75,10 +84,12 @@
 #define CMD_READ_DATA(addr)	    ((0x03 << CMD_SHIFT) | addr)
 #define CMD_READ_SR1		    ((0x05 << CMD_SHIFT))
 #define CMD_WRITE_ENABLE	    ((0x06 << CMD_SHIFT))
+#define CMD_SECTOR_ERASE_4KB(addr)  ((0x20 << CMD_SHIFT) | addr)
 #define CMD_SECTOR_ERASE_32KB(addr) ((0x52 << CMD_SHIFT) | addr)
 #define CMD_CHIP_ERASE		    ((0x60 << CMD_SHIFT))
 #define CMD_READ_MANUF_DEV_ID	    ((0x90 << CMD_SHIFT))
 #define CMD_RELEASE_POWERDOWN_NOID  ((0xab << CMD_SHIFT))
+#define CMD_SECTOR_ERASE_64KB(addr) ((0xd8 << CMD_SHIFT) | addr)
 
 /* controller interface speeds */
 #define EP_SPEED_FULL 0x2	/* full speed */
@@ -188,28 +199,43 @@ static int erase_chip(struct hfi1_devdata *dd)
 }
 
 /*
- * Erase a range using the 32KB erase command.
+ * Erase a range.
  */
-static int erase_32kb_range(struct hfi1_devdata *dd, u32 start, u32 end)
+static int erase_range(struct hfi1_devdata *dd, u32 start, u32 len)
 {
+	u32 end = start + len;
 	int ret = 0;
 
 	if (end < start)
 		return -EINVAL;
 
-	if ((start & MASK_32KB) || (end & MASK_32KB)) {
+	/* check the end points for the minimum erase */
+	if ((start & MASK_4KB) || (end & MASK_4KB)) {
 		dd_dev_err(dd,
-			"%s: non-aligned range (0x%x,0x%x) for a 32KB erase\n",
+			"%s: non-aligned range (0x%x,0x%x) for a 4KB erase\n",
 			__func__, start, end);
 		return -EINVAL;
 	}
 
 	write_enable(dd);
 
-	for (; start < end; start += SIZE_32KB) {
+	while (start < end) {
 		write_csr(dd, ASIC_EEP_ADDR_CMD, CMD_WRITE_ENABLE);
-		write_csr(dd, ASIC_EEP_ADDR_CMD,
-						CMD_SECTOR_ERASE_32KB(start));
+		/* check in order of largest to smallest */
+		if (((start & MASK_64KB) == 0) && (start + SIZE_64KB <= end)) {
+			write_csr(dd, ASIC_EEP_ADDR_CMD,
+				  CMD_SECTOR_ERASE_64KB(start));
+			start += SIZE_64KB;
+		} else if (((start & MASK_32KB) == 0) &&
+			   (start + SIZE_32KB <= end)) {
+			write_csr(dd, ASIC_EEP_ADDR_CMD,
+				  CMD_SECTOR_ERASE_32KB(start));
+			start += SIZE_32KB;
+		} else {	/* 4KB will work */
+			write_csr(dd, ASIC_EEP_ADDR_CMD,
+				  CMD_SECTOR_ERASE_4KB(start));
+			start += SIZE_4KB;
+		}
 		ret = wait_for_not_busy(dd);
 		if (ret)
 			goto done;
@@ -309,6 +335,18 @@ done:
 	return ret;
 }
 
+/* convert an range composite to a length, in bytes */
+static inline u32 extract_rlen(u32 composite)
+{
+	return (composite & 0xffff) * EP_PAGE_SIZE;
+}
+
+/* convert an range composite to a start, in bytes */
+static inline u32 extract_rstart(u32 composite)
+{
+	return (composite >> 16) * EP_PAGE_SIZE;
+}
+
 /*
  * Perform the given operation on the EPROM.  Called from user space.  The
  * user credentials have already been checked.
@@ -319,6 +357,8 @@ int handle_eprom_command(const struct hfi1_cmd *cmd)
 {
 	struct hfi1_devdata *dd;
 	u32 dev_id;
+	u32 rlen;	/* range length */
+	u32 rstart;	/* range start */
 	int ret = 0;
 
 	/*
@@ -364,54 +404,29 @@ int handle_eprom_command(const struct hfi1_cmd *cmd)
 								sizeof(u32)))
 			ret = -EFAULT;
 		break;
+
 	case HFI1_CMD_EP_ERASE_CHIP:
 		ret = erase_chip(dd);
 		break;
-	case HFI1_CMD_EP_ERASE_P0:
-		if (cmd->len != P0_SIZE) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = erase_32kb_range(dd, 0, cmd->len);
-		break;
-	case HFI1_CMD_EP_ERASE_P1:
-		/* check for overflow */
-		if (P1_START + cmd->len > ASIC_EEP_ADDR_CMD_EP_ADDR_MASK) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = erase_32kb_range(dd, P1_START, P1_START + cmd->len);
-		break;
-	case HFI1_CMD_EP_READ_P0:
-		if (cmd->len != P0_SIZE) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = read_length(dd, 0, cmd->len, cmd->addr);
-		break;
-	case HFI1_CMD_EP_READ_P1:
-		/* check for overflow */
-		if (P1_START + cmd->len > ASIC_EEP_ADDR_CMD_EP_ADDR_MASK) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = read_length(dd, P1_START, cmd->len, cmd->addr);
+
+	case HFI1_CMD_EP_ERASE_RANGE:
+		rlen = extract_rlen(cmd->len);
+		rstart = extract_rstart(cmd->len);
+		ret = erase_range(dd, rstart, rlen);
 		break;
-	case HFI1_CMD_EP_WRITE_P0:
-		if (cmd->len > P0_SIZE) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = write_length(dd, 0, cmd->len, cmd->addr);
+
+	case HFI1_CMD_EP_READ_RANGE:
+		rlen = extract_rlen(cmd->len);
+		rstart = extract_rstart(cmd->len);
+		ret = read_length(dd, rstart, rlen, cmd->addr);
 		break;
-	case HFI1_CMD_EP_WRITE_P1:
-		/* check for overflow */
-		if (P1_START + cmd->len > ASIC_EEP_ADDR_CMD_EP_ADDR_MASK) {
-			ret = -ERANGE;
-			break;
-		}
-		ret = write_length(dd, P1_START, cmd->len, cmd->addr);
+
+	case HFI1_CMD_EP_WRITE_RANGE:
+		rlen = extract_rlen(cmd->len);
+		rstart = extract_rstart(cmd->len);
+		ret = write_length(dd, rstart, rlen, cmd->addr);
 		break;
+
 	default:
 		dd_dev_err(dd, "%s: unexpected command %d\n",
 			__func__, cmd->type);
diff --git a/drivers/staging/rdma/hfi1/file_ops.c b/drivers/staging/rdma/hfi1/file_ops.c
index aae9826ec62b..d57d549052c8 100644
--- a/drivers/staging/rdma/hfi1/file_ops.c
+++ b/drivers/staging/rdma/hfi1/file_ops.c
@@ -47,20 +47,10 @@
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  */
-#include <linux/pci.h>
 #include <linux/poll.h>
 #include <linux/cdev.h>
-#include <linux/swap.h>
 #include <linux/vmalloc.h>
-#include <linux/highmem.h>
 #include <linux/io.h>
-#include <linux/jiffies.h>
-#include <asm/pgtable.h>
-#include <linux/delay.h>
-#include <linux/export.h>
-#include <linux/module.h>
-#include <linux/cred.h>
-#include <linux/uio.h>
 
 #include "hfi.h"
 #include "pio.h"
@@ -68,6 +58,7 @@
 #include "common.h"
 #include "trace.h"
 #include "user_sdma.h"
+#include "user_exp_rcv.h"
 #include "eprom.h"
 
 #undef pr_fmt
@@ -170,18 +161,6 @@ enum mmap_types {
 	HFI1_MMAP_TOKEN_SET(SUBCTXT, subctxt) | \
 	HFI1_MMAP_TOKEN_SET(OFFSET, (offset_in_page(addr))))
 
-#define EXP_TID_SET(field, value)			\
-	(((value) & EXP_TID_TID##field##_MASK) <<	\
-	 EXP_TID_TID##field##_SHIFT)
-#define EXP_TID_CLEAR(tid, field) {					\
-		(tid) &= ~(EXP_TID_TID##field##_MASK <<			\
-			   EXP_TID_TID##field##_SHIFT);			\
-			}
-#define EXP_TID_RESET(tid, field, value) do {				\
-		EXP_TID_CLEAR(tid, field);				\
-		(tid) |= EXP_TID_SET(field, value);			\
-	} while (0)
-
 #define dbg(fmt, ...)				\
 	pr_info(fmt, ##__VA_ARGS__)
 
@@ -204,7 +183,8 @@ static ssize_t hfi1_file_write(struct file *fp, const char __user *data,
 			       size_t count, loff_t *offset)
 {
 	const struct hfi1_cmd __user *ucmd;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_cmd cmd;
 	struct hfi1_user_info uinfo;
 	struct hfi1_tid_info tinfo;
@@ -254,12 +234,9 @@ static ssize_t hfi1_file_write(struct file *fp, const char __user *data,
 		break;
 	case HFI1_CMD_EP_INFO:
 	case HFI1_CMD_EP_ERASE_CHIP:
-	case HFI1_CMD_EP_ERASE_P0:
-	case HFI1_CMD_EP_ERASE_P1:
-	case HFI1_CMD_EP_READ_P0:
-	case HFI1_CMD_EP_READ_P1:
-	case HFI1_CMD_EP_WRITE_P0:
-	case HFI1_CMD_EP_WRITE_P1:
+	case HFI1_CMD_EP_ERASE_RANGE:
+	case HFI1_CMD_EP_READ_RANGE:
+	case HFI1_CMD_EP_WRITE_RANGE:
 		uctxt_required = 0;	/* assigned user context not required */
 		must_be_root = 1;	/* validate user */
 		copy = 0;
@@ -338,17 +315,17 @@ static ssize_t hfi1_file_write(struct file *fp, const char __user *data,
 		ret = exp_tid_free(fp, &tinfo);
 		break;
 	case HFI1_CMD_RECV_CTRL:
-		ret = manage_rcvq(uctxt, subctxt_fp(fp), (int)user_val);
+		ret = manage_rcvq(uctxt, fd->subctxt, (int)user_val);
 		break;
 	case HFI1_CMD_POLL_TYPE:
 		uctxt->poll_type = (typeof(uctxt->poll_type))user_val;
 		break;
 	case HFI1_CMD_ACK_EVENT:
-		ret = user_event_ack(uctxt, subctxt_fp(fp), user_val);
+		ret = user_event_ack(uctxt, fd->subctxt, user_val);
 		break;
 	case HFI1_CMD_SET_PKEY:
 		if (HFI1_CAP_IS_USET(PKEY_CHECK))
-			ret = set_ctxt_pkey(uctxt, subctxt_fp(fp), user_val);
+			ret = set_ctxt_pkey(uctxt, fd->subctxt, user_val);
 		else
 			ret = -EPERM;
 		break;
@@ -413,12 +390,9 @@ static ssize_t hfi1_file_write(struct file *fp, const char __user *data,
 	}
 	case HFI1_CMD_EP_INFO:
 	case HFI1_CMD_EP_ERASE_CHIP:
-	case HFI1_CMD_EP_ERASE_P0:
-	case HFI1_CMD_EP_ERASE_P1:
-	case HFI1_CMD_EP_READ_P0:
-	case HFI1_CMD_EP_READ_P1:
-	case HFI1_CMD_EP_WRITE_P0:
-	case HFI1_CMD_EP_WRITE_P1:
+	case HFI1_CMD_EP_ERASE_RANGE:
+	case HFI1_CMD_EP_READ_RANGE:
+	case HFI1_CMD_EP_WRITE_RANGE:
 		ret = handle_eprom_command(&cmd);
 		break;
 	}
@@ -431,13 +405,13 @@ bail:
 
 static ssize_t hfi1_write_iter(struct kiocb *kiocb, struct iov_iter *from)
 {
-	struct hfi1_user_sdma_pkt_q *pq;
-	struct hfi1_user_sdma_comp_q *cq;
+	struct hfi1_filedata *fd = kiocb->ki_filp->private_data;
+	struct hfi1_user_sdma_pkt_q *pq = fd->pq;
+	struct hfi1_user_sdma_comp_q *cq = fd->cq;
 	int ret = 0, done = 0, reqs = 0;
 	unsigned long dim = from->nr_segs;
 
-	if (!user_sdma_comp_fp(kiocb->ki_filp) ||
-	    !user_sdma_pkt_fp(kiocb->ki_filp)) {
+	if (!cq || !pq) {
 		ret = -EIO;
 		goto done;
 	}
@@ -448,10 +422,7 @@ static ssize_t hfi1_write_iter(struct kiocb *kiocb, struct iov_iter *from)
 	}
 
 	hfi1_cdbg(SDMA, "SDMA request from %u:%u (%lu)",
-		  ctxt_fp(kiocb->ki_filp)->ctxt, subctxt_fp(kiocb->ki_filp),
-		  dim);
-	pq = user_sdma_pkt_fp(kiocb->ki_filp);
-	cq = user_sdma_comp_fp(kiocb->ki_filp);
+		  fd->uctxt->ctxt, fd->subctxt, dim);
 
 	if (atomic_read(&pq->n_reqs) == pq->n_max_reqs) {
 		ret = -ENOSPC;
@@ -476,7 +447,8 @@ done:
 
 static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 {
-	struct hfi1_ctxtdata *uctxt;
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd;
 	unsigned long flags, pfn;
 	u64 token = vma->vm_pgoff << PAGE_SHIFT,
@@ -486,7 +458,6 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 	int ret = 0;
 	u16 ctxt;
 
-	uctxt = ctxt_fp(fp);
 	if (!is_valid_mmap(token) || !uctxt ||
 	    !(vma->vm_flags & VM_SHARED)) {
 		ret = -EINVAL;
@@ -496,7 +467,7 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 	ctxt = HFI1_MMAP_TOKEN_GET(CTXT, token);
 	subctxt = HFI1_MMAP_TOKEN_GET(SUBCTXT, token);
 	type = HFI1_MMAP_TOKEN_GET(TYPE, token);
-	if (ctxt != uctxt->ctxt || subctxt != subctxt_fp(fp)) {
+	if (ctxt != uctxt->ctxt || subctxt != fd->subctxt) {
 		ret = -EINVAL;
 		goto done;
 	}
@@ -660,13 +631,12 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 		vmf = 1;
 		break;
 	case SDMA_COMP: {
-		struct hfi1_user_sdma_comp_q *cq;
+		struct hfi1_user_sdma_comp_q *cq = fd->cq;
 
-		if (!user_sdma_comp_fp(fp)) {
+		if (!cq) {
 			ret = -EFAULT;
 			goto done;
 		}
-		cq = user_sdma_comp_fp(fp);
 		memaddr = (u64)cq->comps;
 		memlen = ALIGN(sizeof(*cq->comps) * cq->nentries, PAGE_SIZE);
 		flags |= VM_IO | VM_DONTEXPAND;
@@ -680,16 +650,16 @@ static int hfi1_file_mmap(struct file *fp, struct vm_area_struct *vma)
 
 	if ((vma->vm_end - vma->vm_start) != memlen) {
 		hfi1_cdbg(PROC, "%u:%u Memory size mismatch %lu:%lu",
-			  uctxt->ctxt, subctxt_fp(fp),
+			  uctxt->ctxt, fd->subctxt,
 			  (vma->vm_end - vma->vm_start), memlen);
 		ret = -EINVAL;
 		goto done;
 	}
 
 	vma->vm_flags = flags;
-	dd_dev_info(dd,
-		    "%s: %u:%u type:%u io/vf:%d/%d, addr:0x%llx, len:%lu(%lu), flags:0x%lx\n",
-		    __func__, ctxt, subctxt, type, mapio, vmf, memaddr, memlen,
+	hfi1_cdbg(PROC,
+		  "%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) {
@@ -730,7 +700,7 @@ static unsigned int hfi1_poll(struct file *fp, struct poll_table_struct *pt)
 	struct hfi1_ctxtdata *uctxt;
 	unsigned pollflag;
 
-	uctxt = ctxt_fp(fp);
+	uctxt = ((struct hfi1_filedata *)fp->private_data)->uctxt;
 	if (!uctxt)
 		pollflag = POLLERR;
 	else if (uctxt->poll_type == HFI1_POLL_TYPE_URGENT)
@@ -761,8 +731,7 @@ static int hfi1_file_close(struct inode *inode, struct file *fp)
 
 	flush_wc();
 	/* drain user sdma queue */
-	if (fdata->pq)
-		hfi1_user_sdma_free_queues(fdata);
+	hfi1_user_sdma_free_queues(fdata);
 
 	/*
 	 * Clear any left over, unhandled events so the next process that
@@ -874,6 +843,14 @@ done:
 	return ret;
 }
 
+/* return true if the device available for general use */
+static int usable_device(struct hfi1_devdata *dd)
+{
+	struct hfi1_pportdata *ppd = dd->pport;
+
+	return driver_lstate(ppd) == IB_PORT_ACTIVE;
+}
+
 static int get_user_context(struct file *fp, struct hfi1_user_info *uinfo,
 			    int devno, unsigned alg)
 {
@@ -903,7 +880,11 @@ static int get_user_context(struct file *fp, struct hfi1_user_info *uinfo,
 
 			for (dev = 0; dev < devmax; dev++) {
 				pdd = hfi1_lookup(dev);
-				if (pdd && pdd->freectxts &&
+				if (!pdd)
+					continue;
+				if (!usable_device(pdd))
+					continue;
+				if (pdd->freectxts &&
 				    pdd->freectxts > free) {
 					dd = pdd;
 					free = pdd->freectxts;
@@ -912,7 +893,11 @@ static int get_user_context(struct file *fp, struct hfi1_user_info *uinfo,
 		} else {
 			for (dev = 0; dev < devmax; dev++) {
 				pdd = hfi1_lookup(dev);
-				if (pdd && pdd->freectxts) {
+				if (!pdd)
+					continue;
+				if (!usable_device(pdd))
+					continue;
+				if (pdd->freectxts) {
 					dd = pdd;
 					break;
 				}
@@ -930,13 +915,13 @@ static int find_shared_ctxt(struct file *fp,
 {
 	int devmax, ndev, i;
 	int ret = 0;
+	struct hfi1_filedata *fd = fp->private_data;
 
 	devmax = hfi1_count_units(NULL, NULL);
 
 	for (ndev = 0; ndev < devmax; ndev++) {
 		struct hfi1_devdata *dd = hfi1_lookup(ndev);
 
-		/* device portion of usable() */
 		if (!(dd && (dd->flags & HFI1_PRESENT) && dd->kregbase))
 			continue;
 		for (i = dd->first_user_ctxt; i < dd->num_rcv_contexts; i++) {
@@ -959,10 +944,10 @@ static int find_shared_ctxt(struct file *fp,
 				ret = -EINVAL;
 				goto done;
 			}
-			ctxt_fp(fp) = uctxt;
-			subctxt_fp(fp) = uctxt->cnt++;
-			uctxt->subpid[subctxt_fp(fp)] = current->pid;
-			uctxt->active_slaves |= 1 << subctxt_fp(fp);
+			fd->uctxt = uctxt;
+			fd->subctxt  = uctxt->cnt++;
+			uctxt->subpid[fd->subctxt] = current->pid;
+			uctxt->active_slaves |= 1 << fd->subctxt;
 			ret = 1;
 			goto done;
 		}
@@ -975,6 +960,7 @@ done:
 static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 			 struct hfi1_user_info *uinfo)
 {
+	struct hfi1_filedata *fd = fp->private_data;
 	struct hfi1_ctxtdata *uctxt;
 	unsigned ctxt;
 	int ret;
@@ -1011,8 +997,8 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 	if (!uctxt->sc)
 		return -ENOMEM;
 
-	dbg("allocated send context %u(%u)\n", uctxt->sc->sw_index,
-		uctxt->sc->hw_context);
+	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;
@@ -1022,7 +1008,7 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 	 * This has to be done here so the rest of the sub-contexts find the
 	 * proper master.
 	 */
-	if (uinfo->subctxt_cnt && !subctxt_fp(fp)) {
+	if (uinfo->subctxt_cnt && !fd->subctxt) {
 		ret = init_subctxts(uctxt, uinfo);
 		/*
 		 * On error, we don't need to disable and de-allocate the
@@ -1042,7 +1028,7 @@ static int allocate_ctxt(struct file *fp, struct hfi1_devdata *dd,
 	spin_lock_init(&uctxt->sdma_qlock);
 	hfi1_stats.sps_ctxts++;
 	dd->freectxts--;
-	ctxt_fp(fp) = uctxt;
+	fd->uctxt = uctxt;
 
 	return 0;
 }
@@ -1106,7 +1092,8 @@ static int user_init(struct file *fp)
 {
 	int ret;
 	unsigned int rcvctrl_ops = 0;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 
 	/* make sure that the context has already been setup */
 	if (!test_bit(HFI1_CTXT_SETUP_DONE, &uctxt->event_flags)) {
@@ -1118,7 +1105,7 @@ static int user_init(struct file *fp)
 	 * Subctxts don't need to initialize anything since master
 	 * has done it.
 	 */
-	if (subctxt_fp(fp)) {
+	if (fd->subctxt) {
 		ret = wait_event_interruptible(uctxt->wait,
 			!test_bit(HFI1_CTXT_MASTER_UNINIT,
 			&uctxt->event_flags));
@@ -1178,8 +1165,8 @@ done:
 static int get_ctxt_info(struct file *fp, void __user *ubase, __u32 len)
 {
 	struct hfi1_ctxt_info cinfo;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
 	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	int ret = 0;
 
 	memset(&cinfo, 0, sizeof(cinfo));
@@ -1189,7 +1176,7 @@ static int get_ctxt_info(struct file *fp, void __user *ubase, __u32 len)
 	cinfo.num_active = hfi1_count_active_units();
 	cinfo.unit = uctxt->dd->unit;
 	cinfo.ctxt = uctxt->ctxt;
-	cinfo.subctxt = subctxt_fp(fp);
+	cinfo.subctxt = fd->subctxt;
 	cinfo.rcvtids = roundup(uctxt->egrbufs.alloced,
 				uctxt->dd->rcv_entries.group_size) +
 		uctxt->expected_count;
@@ -1201,10 +1188,10 @@ static int get_ctxt_info(struct file *fp, void __user *ubase, __u32 len)
 	cinfo.egrtids = uctxt->egrbufs.alloced;
 	cinfo.rcvhdrq_cnt = uctxt->rcvhdrq_cnt;
 	cinfo.rcvhdrq_entsize = uctxt->rcvhdrqentsize << 2;
-	cinfo.sdma_ring_size = user_sdma_comp_fp(fp)->nentries;
+	cinfo.sdma_ring_size = fd->cq->nentries;
 	cinfo.rcvegr_size = uctxt->egrbufs.rcvtid_size;
 
-	trace_hfi1_ctxt_info(uctxt->dd, uctxt->ctxt, subctxt_fp(fp), cinfo);
+	trace_hfi1_ctxt_info(uctxt->dd, uctxt->ctxt, fd->subctxt, cinfo);
 	if (copy_to_user(ubase, &cinfo, sizeof(cinfo)))
 		ret = -EFAULT;
 done:
@@ -1213,7 +1200,8 @@ done:
 
 static int setup_ctxt(struct file *fp)
 {
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	int ret = 0;
 
@@ -1222,7 +1210,7 @@ static int setup_ctxt(struct file *fp)
 	 * programming of eager buffers. This is done if context sharing
 	 * is not requested or by the master process.
 	 */
-	if (!uctxt->subctxt_cnt || !subctxt_fp(fp)) {
+	if (!uctxt->subctxt_cnt || !fd->subctxt) {
 		ret = hfi1_init_ctxt(uctxt->sc);
 		if (ret)
 			goto done;
@@ -1234,7 +1222,7 @@ static int setup_ctxt(struct file *fp)
 		ret = hfi1_setup_eagerbufs(uctxt);
 		if (ret)
 			goto done;
-		if (uctxt->subctxt_cnt && !subctxt_fp(fp)) {
+		if (uctxt->subctxt_cnt && !fd->subctxt) {
 			ret = setup_subctxt(uctxt);
 			if (ret)
 				goto done;
@@ -1277,7 +1265,7 @@ static int setup_ctxt(struct file *fp)
 			uctxt->tidusemap[uctxt->tidmapcnt - 1] =
 				~((1ULL << (uctxt->numtidgroups %
 					    BITS_PER_LONG)) - 1);
-		trace_hfi1_exp_tid_map(uctxt->ctxt, subctxt_fp(fp), 0,
+		trace_hfi1_exp_tid_map(uctxt->ctxt, fd->subctxt, 0,
 				       uctxt->tidusemap, uctxt->tidmapcnt);
 	}
 	ret = hfi1_user_sdma_alloc_queues(uctxt, fp);
@@ -1292,7 +1280,8 @@ done:
 static int get_base_info(struct file *fp, void __user *ubase, __u32 len)
 {
 	struct hfi1_base_info binfo;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	ssize_t sz;
 	unsigned offset;
@@ -1314,50 +1303,50 @@ static int get_base_info(struct file *fp, void __user *ubase, __u32 len)
 	offset = ((u64)uctxt->sc->hw_free -
 		  (u64)dd->cr_base[uctxt->numa_id].va) % PAGE_SIZE;
 	binfo.sc_credits_addr = HFI1_MMAP_TOKEN(PIO_CRED, uctxt->ctxt,
-					       subctxt_fp(fp), offset);
+						fd->subctxt, offset);
 	binfo.pio_bufbase = HFI1_MMAP_TOKEN(PIO_BUFS, uctxt->ctxt,
-					    subctxt_fp(fp),
+					    fd->subctxt,
 					    uctxt->sc->base_addr);
 	binfo.pio_bufbase_sop = HFI1_MMAP_TOKEN(PIO_BUFS_SOP,
 						uctxt->ctxt,
-						subctxt_fp(fp),
+						fd->subctxt,
 						uctxt->sc->base_addr);
 	binfo.rcvhdr_bufbase = HFI1_MMAP_TOKEN(RCV_HDRQ, uctxt->ctxt,
-					       subctxt_fp(fp),
+					       fd->subctxt,
 					       uctxt->rcvhdrq);
 	binfo.rcvegr_bufbase = HFI1_MMAP_TOKEN(RCV_EGRBUF, uctxt->ctxt,
-					       subctxt_fp(fp),
+					       fd->subctxt,
 					       uctxt->egrbufs.rcvtids[0].phys);
 	binfo.sdma_comp_bufbase = HFI1_MMAP_TOKEN(SDMA_COMP, uctxt->ctxt,
-						 subctxt_fp(fp), 0);
+						 fd->subctxt, 0);
 	/*
 	 * user regs are at
 	 * (RXE_PER_CONTEXT_USER + (ctxt * RXE_PER_CONTEXT_SIZE))
 	 */
 	binfo.user_regbase = HFI1_MMAP_TOKEN(UREGS, uctxt->ctxt,
-					    subctxt_fp(fp), 0);
+					    fd->subctxt, 0);
 	offset = offset_in_page((((uctxt->ctxt - dd->first_user_ctxt) *
-		    HFI1_MAX_SHARED_CTXTS) + subctxt_fp(fp)) *
+		    HFI1_MAX_SHARED_CTXTS) + fd->subctxt) *
 		  sizeof(*dd->events));
 	binfo.events_bufbase = HFI1_MMAP_TOKEN(EVENTS, uctxt->ctxt,
-					      subctxt_fp(fp),
+					      fd->subctxt,
 					      offset);
 	binfo.status_bufbase = HFI1_MMAP_TOKEN(STATUS, uctxt->ctxt,
-					      subctxt_fp(fp),
+					      fd->subctxt,
 					      dd->status);
 	if (HFI1_CAP_IS_USET(DMA_RTAIL))
 		binfo.rcvhdrtail_base = HFI1_MMAP_TOKEN(RTAIL, uctxt->ctxt,
-						       subctxt_fp(fp), 0);
+						       fd->subctxt, 0);
 	if (uctxt->subctxt_cnt) {
 		binfo.subctxt_uregbase = HFI1_MMAP_TOKEN(SUBCTXT_UREGS,
 							uctxt->ctxt,
-							subctxt_fp(fp), 0);
+							fd->subctxt, 0);
 		binfo.subctxt_rcvhdrbuf = HFI1_MMAP_TOKEN(SUBCTXT_RCV_HDRQ,
 							 uctxt->ctxt,
-							 subctxt_fp(fp), 0);
+							 fd->subctxt, 0);
 		binfo.subctxt_rcvegrbuf = HFI1_MMAP_TOKEN(SUBCTXT_EGRBUF,
 							 uctxt->ctxt,
-							 subctxt_fp(fp), 0);
+							 fd->subctxt, 0);
 	}
 	sz = (len < sizeof(binfo)) ? len : sizeof(binfo);
 	if (copy_to_user(ubase, &binfo, sz))
@@ -1368,7 +1357,8 @@ static int get_base_info(struct file *fp, void __user *ubase, __u32 len)
 static unsigned int poll_urgent(struct file *fp,
 				struct poll_table_struct *pt)
 {
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	unsigned pollflag;
 
@@ -1390,7 +1380,8 @@ static unsigned int poll_urgent(struct file *fp,
 static unsigned int poll_next(struct file *fp,
 			      struct poll_table_struct *pt)
 {
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	unsigned pollflag;
 
@@ -1562,7 +1553,8 @@ static inline unsigned num_free_groups(unsigned long map, u16 *start)
 static int exp_tid_setup(struct file *fp, struct hfi1_tid_info *tinfo)
 {
 	int ret = 0;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	unsigned tid, mapped = 0, npages, ngroups, exp_groups,
 		tidpairs = uctxt->expected_count / 2;
@@ -1671,8 +1663,8 @@ static int exp_tid_setup(struct file *fp, struct hfi1_tid_info *tinfo)
 		 * Now that we know how many free RcvArray entries we have,
 		 * we can pin that many user pages.
 		 */
-		ret = hfi1_get_user_pages(vaddr + (mapped * PAGE_SIZE),
-					  pinned, pages);
+		ret = hfi1_acquire_user_pages(vaddr + (mapped * PAGE_SIZE),
+					      pinned, true, pages);
 		if (ret) {
 			/*
 			 * We can't continue because the pages array won't be
@@ -1718,7 +1710,7 @@ static int exp_tid_setup(struct file *fp, struct hfi1_tid_info *tinfo)
 						   pages[pmapped], 0,
 						   tidsize, PCI_DMA_FROMDEVICE);
 				trace_hfi1_exp_rcv_set(uctxt->ctxt,
-						       subctxt_fp(fp),
+						       fd->subctxt,
 						       tid, vaddr,
 						       phys[pmapped],
 						       pages[pmapped]);
@@ -1763,7 +1755,7 @@ static int exp_tid_setup(struct file *fp, struct hfi1_tid_info *tinfo)
 			   (((useidx & 0xffffff) << 16) |
 			    ((bitidx + bits_used) & 0xffffff)));
 	}
-	trace_hfi1_exp_tid_map(uctxt->ctxt, subctxt_fp(fp), 0, uctxt->tidusemap,
+	trace_hfi1_exp_tid_map(uctxt->ctxt, fd->subctxt, 0, uctxt->tidusemap,
 			       uctxt->tidmapcnt);
 
 done:
@@ -1792,7 +1784,8 @@ bail:
 
 static int exp_tid_free(struct file *fp, struct hfi1_tid_info *tinfo)
 {
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
 	struct hfi1_devdata *dd = uctxt->dd;
 	unsigned long tidmap[uctxt->tidmapcnt];
 	struct page **pages;
@@ -1828,7 +1821,7 @@ static int exp_tid_free(struct file *fp, struct hfi1_tid_info *tinfo)
 					hfi1_put_tid(dd, tid, PT_INVALID,
 						      0, 0);
 					trace_hfi1_exp_rcv_free(uctxt->ctxt,
-								subctxt_fp(fp),
+								fd->subctxt,
 								tid, phys[i],
 								pages[i]);
 					pci_unmap_page(dd->pcidev, phys[i],
@@ -1840,12 +1833,12 @@ static int exp_tid_free(struct file *fp, struct hfi1_tid_info *tinfo)
 				}
 			}
 			flush_wc();
-			hfi1_release_user_pages(pshadow, pcount);
+			hfi1_release_user_pages(pshadow, pcount, true);
 			clear_bit(bitidx, &uctxt->tidusemap[idx]);
 			map &= ~(1ULL<<bitidx);
 		}
 	}
-	trace_hfi1_exp_tid_map(uctxt->ctxt, subctxt_fp(fp), 1, uctxt->tidusemap,
+	trace_hfi1_exp_tid_map(uctxt->ctxt, fd->subctxt, 1, uctxt->tidusemap,
 			       uctxt->tidmapcnt);
 done:
 	return ret;
@@ -1869,7 +1862,7 @@ static void unlock_exp_tids(struct hfi1_ctxtdata *uctxt)
 		uctxt->physshadow[tid] = 0;
 		uctxt->tid_pg_list[tid] = NULL;
 		pci_unmap_page(dd->pcidev, phys, PAGE_SIZE, PCI_DMA_FROMDEVICE);
-		hfi1_release_user_pages(&p, 1);
+		hfi1_release_user_pages(&p, 1, true);
 	}
 }
 
diff --git a/drivers/staging/rdma/hfi1/firmware.c b/drivers/staging/rdma/hfi1/firmware.c
index b4bdcf341aac..28ae42faa018 100644
--- a/drivers/staging/rdma/hfi1/firmware.c
+++ b/drivers/staging/rdma/hfi1/firmware.c
@@ -68,6 +68,10 @@
 #define DEFAULT_FW_SBUS_NAME "hfi1_sbus.fw"
 #define DEFAULT_FW_PCIE_NAME "hfi1_pcie.fw"
 #define DEFAULT_PLATFORM_CONFIG_NAME "hfi1_platform.dat"
+#define ALT_FW_8051_NAME_ASIC "hfi1_dc8051_d.fw"
+#define ALT_FW_FABRIC_NAME "hfi1_fabric_d.fw"
+#define ALT_FW_SBUS_NAME "hfi1_sbus_d.fw"
+#define ALT_FW_PCIE_NAME "hfi1_pcie_d.fw"
 
 static uint fw_8051_load = 1;
 static uint fw_fabric_serdes_load = 1;
@@ -158,7 +162,8 @@ struct firmware_details {
 static DEFINE_MUTEX(fw_mutex);
 enum fw_state {
 	FW_EMPTY,
-	FW_ACQUIRED,
+	FW_TRY,
+	FW_FINAL,
 	FW_ERR
 };
 static enum fw_state fw_state = FW_EMPTY;
@@ -428,8 +433,8 @@ static int obtain_one_firmware(struct hfi1_devdata *dd, const char *name,
 
 	ret = request_firmware(&fdet->fw, name, &dd->pcidev->dev);
 	if (ret) {
-		dd_dev_err(dd, "cannot load firmware \"%s\", err %d\n",
-			name, ret);
+		dd_dev_err(dd, "cannot find firmware \"%s\", err %d\n",
+			   name, ret);
 		return ret;
 	}
 
@@ -539,28 +544,53 @@ done:
 static void dispose_one_firmware(struct firmware_details *fdet)
 {
 	release_firmware(fdet->fw);
-	fdet->fw = NULL;
+	/* erase all previous information */
+	memset(fdet, 0, sizeof(*fdet));
 }
 
 /*
- * Called by all HFIs when loading their firmware - i.e. device probe time.
- * The first one will do the actual firmware load.  Use a mutex to resolve
- * any possible race condition.
+ * Obtain the 4 firmwares from the OS.  All must be obtained at once or not
+ * at all.  If called with the firmware state in FW_TRY, use alternate names.
+ * On exit, this routine will have set the firmware state to one of FW_TRY,
+ * FW_FINAL, or FW_ERR.
  *
- * The call to this routine cannot be moved to driver load because the kernel
- * call request_firmware() requires a device which is only available after
- * the first device probe.
+ * Must be holding fw_mutex.
  */
-static int obtain_firmware(struct hfi1_devdata *dd)
+static void __obtain_firmware(struct hfi1_devdata *dd)
 {
 	int err = 0;
 
-	mutex_lock(&fw_mutex);
-	if (fw_state == FW_ACQUIRED) {
-		goto done;	/* already acquired */
-	} else if (fw_state == FW_ERR) {
-		err = fw_err;
-		goto done;	/* already tried and failed */
+	if (fw_state == FW_FINAL)	/* nothing more to obtain */
+		return;
+	if (fw_state == FW_ERR)		/* already in error */
+		return;
+
+	/* fw_state is FW_EMPTY or FW_TRY */
+retry:
+	if (fw_state == FW_TRY) {
+		/*
+		 * We tried the original and it failed.  Move to the
+		 * alternate.
+		 */
+		dd_dev_info(dd, "using alternate firmware names\n");
+		/*
+		 * Let others run.  Some systems, when missing firmware, does
+		 * something that holds for 30 seconds.  If we do that twice
+		 * in a row it triggers task blocked warning.
+		 */
+		cond_resched();
+		if (fw_8051_load)
+			dispose_one_firmware(&fw_8051);
+		if (fw_fabric_serdes_load)
+			dispose_one_firmware(&fw_fabric);
+		if (fw_sbus_load)
+			dispose_one_firmware(&fw_sbus);
+		if (fw_pcie_serdes_load)
+			dispose_one_firmware(&fw_pcie);
+		fw_8051_name = ALT_FW_8051_NAME_ASIC;
+		fw_fabric_serdes_name = ALT_FW_FABRIC_NAME;
+		fw_sbus_name = ALT_FW_SBUS_NAME;
+		fw_pcie_serdes_name = ALT_FW_PCIE_NAME;
 	}
 
 	if (fw_8051_load) {
@@ -588,27 +618,82 @@ static int obtain_firmware(struct hfi1_devdata *dd)
 			goto done;
 	}
 
+done:
+	if (err) {
+		/* oops, had problems obtaining a firmware */
+		if (fw_state == FW_EMPTY) {
+			/* retry with alternate */
+			fw_state = FW_TRY;
+			goto retry;
+		}
+		fw_state = FW_ERR;
+		fw_err = -ENOENT;
+	} else {
+		/* success */
+		if (fw_state == FW_EMPTY)
+			fw_state = FW_TRY;	/* may retry later */
+		else
+			fw_state = FW_FINAL;	/* cannot try again */
+	}
+}
+
+/*
+ * Called by all HFIs when loading their firmware - i.e. device probe time.
+ * The first one will do the actual firmware load.  Use a mutex to resolve
+ * any possible race condition.
+ *
+ * The call to this routine cannot be moved to driver load because the kernel
+ * call request_firmware() requires a device which is only available after
+ * the first device probe.
+ */
+static int obtain_firmware(struct hfi1_devdata *dd)
+{
+	unsigned long timeout;
+	int err = 0;
+
+	mutex_lock(&fw_mutex);
+
+	/* 40s delay due to long delay on missing firmware on some systems */
+	timeout = jiffies + msecs_to_jiffies(40000);
+	while (fw_state == FW_TRY) {
+		/*
+		 * Another device is trying the firmware.  Wait until it
+		 * decides what works (or not).
+		 */
+		if (time_after(jiffies, timeout)) {
+			/* waited too long */
+			dd_dev_err(dd, "Timeout waiting for firmware try");
+			fw_state = FW_ERR;
+			fw_err = -ETIMEDOUT;
+			break;
+		}
+		mutex_unlock(&fw_mutex);
+		msleep(20);	/* arbitrary delay */
+		mutex_lock(&fw_mutex);
+	}
+	/* not in FW_TRY state */
+
+	if (fw_state == FW_FINAL)
+		goto done;	/* already acquired */
+	else if (fw_state == FW_ERR)
+		goto done;	/* already tried and failed */
+	/* fw_state is FW_EMPTY */
+
+	/* set fw_state to FW_TRY, FW_FINAL, or FW_ERR, and fw_err */
+	__obtain_firmware(dd);
+
 	if (platform_config_load) {
 		platform_config = NULL;
 		err = request_firmware(&platform_config, platform_config_name,
 						&dd->pcidev->dev);
-		if (err) {
-			err = 0;
+		if (err)
 			platform_config = NULL;
-		}
 	}
 
-	/* success */
-	fw_state = FW_ACQUIRED;
-
 done:
-	if (err) {
-		fw_err = err;
-		fw_state = FW_ERR;
-	}
 	mutex_unlock(&fw_mutex);
 
-	return err;
+	return fw_err;
 }
 
 /*
@@ -638,6 +723,38 @@ void dispose_firmware(void)
 }
 
 /*
+ * Called with the result of a firmware download.
+ *
+ * Return 1 to retry loading the firmware, 0 to stop.
+ */
+static int retry_firmware(struct hfi1_devdata *dd, int load_result)
+{
+	int retry;
+
+	mutex_lock(&fw_mutex);
+
+	if (load_result == 0) {
+		/*
+		 * The load succeeded, so expect all others to do the same.
+		 * Do not retry again.
+		 */
+		if (fw_state == FW_TRY)
+			fw_state = FW_FINAL;
+		retry = 0;	/* do NOT retry */
+	} else if (fw_state == FW_TRY) {
+		/* load failed, obtain alternate firmware */
+		__obtain_firmware(dd);
+		retry = (fw_state == FW_FINAL);
+	} else {
+		/* else in FW_FINAL or FW_ERR, no retry in either case */
+		retry = 0;
+	}
+
+	mutex_unlock(&fw_mutex);
+	return retry;
+}
+
+/*
  * Write a block of data to a given array CSR.  All calls will be in
  * multiples of 8 bytes.
  */
@@ -951,7 +1068,7 @@ void sbus_request(struct hfi1_devdata *dd,
 static void turn_off_spicos(struct hfi1_devdata *dd, int flags)
 {
 	/* only needed on A0 */
-	if (!is_a0(dd))
+	if (!is_ax(dd))
 		return;
 
 	dd_dev_info(dd, "Turning off spicos:%s%s\n",
@@ -1248,7 +1365,9 @@ int load_firmware(struct hfi1_devdata *dd)
 				fabric_serdes_addrs[dd->hfi1_id],
 				NUM_FABRIC_SERDES);
 		turn_off_spicos(dd, SPICO_FABRIC);
-		ret = load_fabric_serdes_firmware(dd, &fw_fabric);
+		do {
+			ret = load_fabric_serdes_firmware(dd, &fw_fabric);
+		} while (retry_firmware(dd, ret));
 
 		clear_sbus_fast_mode(dd);
 		release_hw_mutex(dd);
@@ -1257,7 +1376,9 @@ int load_firmware(struct hfi1_devdata *dd)
 	}
 
 	if (fw_8051_load) {
-		ret = load_8051_firmware(dd, &fw_8051);
+		do {
+			ret = load_8051_firmware(dd, &fw_8051);
+		} while (retry_firmware(dd, ret));
 		if (ret)
 			return ret;
 	}
@@ -1568,9 +1689,11 @@ int load_pcie_firmware(struct hfi1_devdata *dd)
 	/* both firmware loads below use the SBus */
 	set_sbus_fast_mode(dd);
 
-	if (fw_sbus_load && (dd->flags & HFI1_DO_INIT_ASIC)) {
+	if (fw_sbus_load) {
 		turn_off_spicos(dd, SPICO_SBUS);
-		ret = load_sbus_firmware(dd, &fw_sbus);
+		do {
+			ret = load_sbus_firmware(dd, &fw_sbus);
+		} while (retry_firmware(dd, ret));
 		if (ret)
 			goto done;
 	}
@@ -1581,7 +1704,9 @@ int load_pcie_firmware(struct hfi1_devdata *dd)
 					pcie_serdes_broadcast[dd->hfi1_id],
 					pcie_serdes_addrs[dd->hfi1_id],
 					NUM_PCIE_SERDES);
-		ret = load_pcie_serdes_firmware(dd, &fw_pcie);
+		do {
+			ret = load_pcie_serdes_firmware(dd, &fw_pcie);
+		} while (retry_firmware(dd, ret));
 		if (ret)
 			goto done;
 	}
diff --git a/drivers/staging/rdma/hfi1/hfi.h b/drivers/staging/rdma/hfi1/hfi.h
index 190f7a2f6773..2611bb2e764d 100644
--- a/drivers/staging/rdma/hfi1/hfi.h
+++ b/drivers/staging/rdma/hfi1/hfi.h
@@ -100,6 +100,26 @@ extern unsigned long hfi1_cap_mask;
 			HFI1_CAP_MISC_MASK)
 
 /*
+ * Control context is always 0 and handles the error packets.
+ * It also handles the VL15 and multicast packets.
+ */
+#define HFI1_CTRL_CTXT    0
+
+/*
+ * Driver context will store software counters for each of the events
+ * associated with these status registers
+ */
+#define NUM_CCE_ERR_STATUS_COUNTERS 41
+#define NUM_RCV_ERR_STATUS_COUNTERS 64
+#define NUM_MISC_ERR_STATUS_COUNTERS 13
+#define NUM_SEND_PIO_ERR_STATUS_COUNTERS 36
+#define NUM_SEND_DMA_ERR_STATUS_COUNTERS 4
+#define NUM_SEND_EGRESS_ERR_STATUS_COUNTERS 64
+#define NUM_SEND_ERR_STATUS_COUNTERS 3
+#define NUM_SEND_CTXT_ERR_STATUS_COUNTERS 5
+#define NUM_SEND_DMA_ENG_ERR_STATUS_COUNTERS 24
+
+/*
  * per driver stats, either not device nor port-specific, or
  * summed over all of the devices and ports.
  * They are described by name via ipathfs filesystem, so layout
@@ -139,15 +159,6 @@ extern const struct pci_error_handlers hfi1_pci_err_handler;
 struct hfi1_opcode_stats_perctx;
 #endif
 
-/*
- * struct ps_state keeps state associated with RX queue "prescanning"
- * (prescanning for FECNs, and BECNs), if prescanning is in use.
- */
-struct ps_state {
-	u32 ps_head;
-	int initialized;
-};
-
 struct ctxt_eager_bufs {
 	ssize_t size;            /* total size of eager buffers */
 	u32 count;               /* size of buffers array */
@@ -243,7 +254,7 @@ struct hfi1_ctxtdata {
 	/* chip offset of PIO buffers for this ctxt */
 	u32 piobufs;
 	/* per-context configuration flags */
-	u16 flags;
+	u32 flags;
 	/* per-context event flags for fileops/intr communication */
 	unsigned long event_flags;
 	/* WAIT_RCV that timed out, no interrupt */
@@ -302,10 +313,6 @@ struct hfi1_ctxtdata {
 	struct list_head sdma_queues;
 	spinlock_t sdma_qlock;
 
-#ifdef CONFIG_PRESCAN_RXQ
-	struct ps_state ps_state;
-#endif /* CONFIG_PRESCAN_RXQ */
-
 	/*
 	 * The interrupt handler for a particular receive context can vary
 	 * throughout it's lifetime. This is not a lock protected data member so
@@ -706,6 +713,8 @@ struct hfi1_pportdata {
 	u64 link_downed;
 	/* number of times link retrained successfully */
 	u64 link_up;
+	/* number of times a link unknown frame was reported */
+	u64 unknown_frame_count;
 	/* port_ltp_crc_mode is returned in 'portinfo' MADs */
 	u16 port_ltp_crc_mode;
 	/* port_crc_mode_enabled is the crc we support */
@@ -1053,6 +1062,26 @@ struct hfi1_devdata {
 	atomic_t drop_packet;
 	u8 do_drop;
 
+	/*
+	 * Software counters for the status bits defined by the
+	 * associated error status registers
+	 */
+	u64 cce_err_status_cnt[NUM_CCE_ERR_STATUS_COUNTERS];
+	u64 rcv_err_status_cnt[NUM_RCV_ERR_STATUS_COUNTERS];
+	u64 misc_err_status_cnt[NUM_MISC_ERR_STATUS_COUNTERS];
+	u64 send_pio_err_status_cnt[NUM_SEND_PIO_ERR_STATUS_COUNTERS];
+	u64 send_dma_err_status_cnt[NUM_SEND_DMA_ERR_STATUS_COUNTERS];
+	u64 send_egress_err_status_cnt[NUM_SEND_EGRESS_ERR_STATUS_COUNTERS];
+	u64 send_err_status_cnt[NUM_SEND_ERR_STATUS_COUNTERS];
+
+	/* Software counter that spans all contexts */
+	u64 sw_ctxt_err_status_cnt[NUM_SEND_CTXT_ERR_STATUS_COUNTERS];
+	/* Software counter that spans all DMA engines */
+	u64 sw_send_dma_eng_err_status_cnt[
+		NUM_SEND_DMA_ENG_ERR_STATUS_COUNTERS];
+	/* Software counter that aggregates all cce_err_status errors */
+	u64 sw_cce_err_status_aggregate;
+
 	/* receive interrupt functions */
 	rhf_rcv_function_ptr *rhf_rcv_function_map;
 	rhf_rcv_function_ptr normal_rhf_rcv_functions[8];
@@ -1061,12 +1090,10 @@ struct hfi1_devdata {
 	 * Handlers for outgoing data so that snoop/capture does not
 	 * have to have its hooks in the send path
 	 */
-	int (*process_pio_send)(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
-				u32 hdrwords, struct hfi1_sge_state *ss,
-				u32 len, u32 plen, u32 dwords, u64 pbc);
-	int (*process_dma_send)(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
-				u32 hdrwords, struct hfi1_sge_state *ss,
-				u32 len, u32 plen, u32 dwords, u64 pbc);
+	int (*process_pio_send)(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+				u64 pbc);
+	int (*process_dma_send)(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+				u64 pbc);
 	void (*pio_inline_send)(struct hfi1_devdata *dd, struct pio_buf *pbuf,
 				u64 pbc, const void *from, size_t count);
 
@@ -1084,6 +1111,10 @@ struct hfi1_devdata {
 	/* Save the enabled LCB error bits */
 	u64 lcb_err_en;
 	u8 dc_shutdown;
+
+	/* receive context tail dummy address */
+	__le64 *rcvhdrtail_dummy_kvaddr;
+	dma_addr_t rcvhdrtail_dummy_physaddr;
 };
 
 /* 8051 firmware version helper */
@@ -1414,27 +1445,13 @@ void reset_link_credits(struct hfi1_devdata *dd);
 void assign_remote_cm_au_table(struct hfi1_devdata *dd, u8 vcu);
 
 int snoop_recv_handler(struct hfi1_packet *packet);
-int snoop_send_dma_handler(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
-			   u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			   u32 plen, u32 dwords, u64 pbc);
-int snoop_send_pio_handler(struct hfi1_qp *qp, struct ahg_ib_header *ibhdr,
-			   u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			   u32 plen, u32 dwords, u64 pbc);
+int snoop_send_dma_handler(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			   u64 pbc);
+int snoop_send_pio_handler(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			   u64 pbc);
 void snoop_inline_pio_send(struct hfi1_devdata *dd, struct pio_buf *pbuf,
 			   u64 pbc, const void *from, size_t count);
 
-/* for use in system calls, where we want to know device type, etc. */
-#define ctxt_fp(fp) \
-	(((struct hfi1_filedata *)(fp)->private_data)->uctxt)
-#define subctxt_fp(fp) \
-	(((struct hfi1_filedata *)(fp)->private_data)->subctxt)
-#define tidcursor_fp(fp) \
-	(((struct hfi1_filedata *)(fp)->private_data)->tidcursor)
-#define user_sdma_pkt_fp(fp) \
-	(((struct hfi1_filedata *)(fp)->private_data)->pq)
-#define user_sdma_comp_fp(fp) \
-	(((struct hfi1_filedata *)(fp)->private_data)->cq)
-
 static inline struct hfi1_devdata *dd_from_ppd(struct hfi1_pportdata *ppd)
 {
 	return ppd->dd;
@@ -1570,8 +1587,8 @@ void hfi1_set_led_override(struct hfi1_pportdata *ppd, unsigned int val);
  */
 #define DEFAULT_RCVHDR_ENTSIZE 32
 
-int hfi1_get_user_pages(unsigned long, size_t, struct page **);
-void hfi1_release_user_pages(struct page **, size_t);
+int hfi1_acquire_user_pages(unsigned long, size_t, bool, struct page **);
+void hfi1_release_user_pages(struct page **, size_t, bool);
 
 static inline void clear_rcvhdrtail(const struct hfi1_ctxtdata *rcd)
 {
@@ -1612,7 +1629,6 @@ void hfi1_pcie_flr(struct hfi1_devdata *);
 int pcie_speeds(struct hfi1_devdata *);
 void request_msix(struct hfi1_devdata *, u32 *, struct hfi1_msix_entry *);
 void hfi1_enable_intx(struct pci_dev *);
-void hfi1_nomsix(struct hfi1_devdata *);
 void restore_pci_variables(struct hfi1_devdata *dd);
 int do_pcie_gen3_transition(struct hfi1_devdata *dd);
 int parse_platform_config(struct hfi1_devdata *dd);
@@ -1649,7 +1665,7 @@ void update_sge(struct hfi1_sge_state *ss, u32 length);
 extern unsigned int hfi1_max_mtu;
 extern unsigned int hfi1_cu;
 extern unsigned int user_credit_return_threshold;
-extern uint num_rcv_contexts;
+extern int num_user_contexts;
 extern unsigned n_krcvqs;
 extern u8 krcvqs[];
 extern int krcvqsset;
@@ -1713,7 +1729,7 @@ static inline u64 hfi1_pkt_default_send_ctxt_mask(struct hfi1_devdata *dd,
 	else
 		base_sc_integrity |= HFI1_PKT_KERNEL_SC_INTEGRITY;
 
-	if (is_a0(dd))
+	if (is_ax(dd))
 		/* turn off send-side job key checks - A0 erratum */
 		return base_sc_integrity &
 		       ~SEND_CTXT_CHECK_ENABLE_CHECK_JOB_KEY_SMASK;
@@ -1740,7 +1756,7 @@ static inline u64 hfi1_pkt_base_sdma_integrity(struct hfi1_devdata *dd)
 	| SEND_DMA_CHECK_ENABLE_CHECK_VL_SMASK
 	| SEND_DMA_CHECK_ENABLE_CHECK_ENABLE_SMASK;
 
-	if (is_a0(dd))
+	if (is_ax(dd))
 		/* turn off send-side job key checks - A0 erratum */
 		return base_sdma_integrity &
 		       ~SEND_DMA_CHECK_ENABLE_CHECK_JOB_KEY_SMASK;
diff --git a/drivers/staging/rdma/hfi1/init.c b/drivers/staging/rdma/hfi1/init.c
index 8666f3ad24e9..4dd8051aba7e 100644
--- a/drivers/staging/rdma/hfi1/init.c
+++ b/drivers/staging/rdma/hfi1/init.c
@@ -60,6 +60,7 @@
 #include "hfi.h"
 #include "device.h"
 #include "common.h"
+#include "trace.h"
 #include "mad.h"
 #include "sdma.h"
 #include "debugfs.h"
@@ -81,15 +82,15 @@
  * Number of user receive contexts we are configured to use (to allow for more
  * pio buffers per ctxt, etc.)  Zero means use one user context per CPU.
  */
-uint num_rcv_contexts;
-module_param_named(num_rcv_contexts, num_rcv_contexts, uint, S_IRUGO);
+int num_user_contexts = -1;
+module_param_named(num_user_contexts, num_user_contexts, uint, S_IRUGO);
 MODULE_PARM_DESC(
-	num_rcv_contexts, "Set max number of user receive contexts to use");
+	num_user_contexts, "Set max number of user contexts to use");
 
 u8 krcvqs[RXE_NUM_DATA_VL];
 int krcvqsset;
 module_param_array(krcvqs, byte, &krcvqsset, S_IRUGO);
-MODULE_PARM_DESC(krcvqs, "Array of the number of kernel receive queues by VL");
+MODULE_PARM_DESC(krcvqs, "Array of the number of non-control kernel receive queues by VL");
 
 /* computed based on above array */
 unsigned n_krcvqs;
@@ -112,7 +113,7 @@ MODULE_PARM_DESC(hdrq_entsize, "Size of header queue entries: 2 - 8B, 16 - 64B (
 
 unsigned int user_credit_return_threshold = 33;	/* default is 33% */
 module_param(user_credit_return_threshold, uint, S_IRUGO);
-MODULE_PARM_DESC(user_credit_return_theshold, "Credit return threshold for user send contexts, return when unreturned credits passes this many blocks (in percent of allocated blocks, 0 is off)");
+MODULE_PARM_DESC(user_credit_return_threshold, "Credit return threshold for user send contexts, return when unreturned credits passes this many blocks (in percent of allocated blocks, 0 is off)");
 
 static inline u64 encode_rcv_header_entry_size(u16);
 
@@ -129,6 +130,9 @@ int hfi1_create_ctxts(struct hfi1_devdata *dd)
 	int ret;
 	int local_node_id = pcibus_to_node(dd->pcidev->bus);
 
+	/* Control context has to be always 0 */
+	BUILD_BUG_ON(HFI1_CTRL_CTXT != 0);
+
 	if (local_node_id < 0)
 		local_node_id = numa_node_id();
 	dd->assigned_node_id = local_node_id;
@@ -158,6 +162,10 @@ int hfi1_create_ctxts(struct hfi1_devdata *dd)
 			HFI1_CAP_KGET(NODROP_RHQ_FULL) |
 			HFI1_CAP_KGET(NODROP_EGR_FULL) |
 			HFI1_CAP_KGET(DMA_RTAIL);
+
+		/* Control context must use DMA_RTAIL */
+		if (rcd->ctxt == HFI1_CTRL_CTXT)
+			rcd->flags |= HFI1_CAP_DMA_RTAIL;
 		rcd->seq_cnt = 1;
 
 		rcd->sc = sc_alloc(dd, SC_ACK, rcd->rcvhdrqentsize, dd->node);
@@ -208,7 +216,7 @@ struct hfi1_ctxtdata *hfi1_create_ctxtdata(struct hfi1_pportdata *ppd, u32 ctxt)
 	if (rcd) {
 		u32 rcvtids, max_entries;
 
-		dd_dev_info(dd, "%s: setting up context %u\n", __func__, ctxt);
+		hfi1_cdbg(PROC, "setting up context %u\n", ctxt);
 
 		INIT_LIST_HEAD(&rcd->qp_wait_list);
 		rcd->ppd = ppd;
@@ -279,8 +287,9 @@ struct hfi1_ctxtdata *hfi1_create_ctxtdata(struct hfi1_pportdata *ppd, u32 ctxt)
 				   rcd->ctxt);
 			rcd->egrbufs.count = MAX_EAGER_ENTRIES;
 		}
-		dd_dev_info(dd, "ctxt%u: max Eager buffer RcvArray entries: %u\n",
-			    rcd->ctxt, rcd->egrbufs.count);
+		hfi1_cdbg(PROC,
+			  "ctxt%u: max Eager buffer RcvArray entries: %u\n",
+			  rcd->ctxt, rcd->egrbufs.count);
 
 		/*
 		 * Allocate array that will hold the eager buffer accounting
@@ -308,8 +317,8 @@ struct hfi1_ctxtdata *hfi1_create_ctxtdata(struct hfi1_pportdata *ppd, u32 ctxt)
 		 */
 		if (rcd->egrbufs.size < hfi1_max_mtu) {
 			rcd->egrbufs.size = __roundup_pow_of_two(hfi1_max_mtu);
-			dd_dev_info(dd,
-				    "ctxt%u: eager bufs size too small. Adjusting to %zu\n",
+			hfi1_cdbg(PROC,
+				  "ctxt%u: eager bufs size too small. Adjusting to %zu\n",
 				    rcd->ctxt, rcd->egrbufs.size);
 		}
 		rcd->egrbufs.rcvtid_size = HFI1_MAX_EAGER_BUFFER_SIZE;
@@ -601,20 +610,19 @@ static int create_workqueues(struct hfi1_devdata *dd)
 	for (pidx = 0; pidx < dd->num_pports; ++pidx) {
 		ppd = dd->pport + pidx;
 		if (!ppd->hfi1_wq) {
-			char wq_name[8]; /* 3 + 2 + 1 + 1 + 1 */
-
-			snprintf(wq_name, sizeof(wq_name), "hfi%d_%d",
-				 dd->unit, pidx);
 			ppd->hfi1_wq =
-				create_singlethread_workqueue(wq_name);
+				alloc_workqueue(
+				    "hfi%d_%d",
+				    WQ_SYSFS | WQ_HIGHPRI | WQ_CPU_INTENSIVE,
+				    dd->num_sdma,
+				    dd->unit, pidx);
 			if (!ppd->hfi1_wq)
 				goto wq_error;
 		}
 	}
 	return 0;
 wq_error:
-	pr_err("create_singlethread_workqueue failed for port %d\n",
-		pidx + 1);
+	pr_err("alloc_workqueue failed for port %d\n", pidx + 1);
 	for (pidx = 0; pidx < dd->num_pports; ++pidx) {
 		ppd = dd->pport + pidx;
 		if (ppd->hfi1_wq) {
@@ -670,7 +678,7 @@ int hfi1_init(struct hfi1_devdata *dd, int reinit)
 	dd->process_dma_send = hfi1_verbs_send_dma;
 	dd->pio_inline_send = pio_copy;
 
-	if (is_a0(dd)) {
+	if (is_ax(dd)) {
 		atomic_set(&dd->drop_packet, DROP_PACKET_ON);
 		dd->do_drop = 1;
 	} else {
@@ -691,6 +699,18 @@ int hfi1_init(struct hfi1_devdata *dd, int reinit)
 	if (ret)
 		goto done;
 
+	/* allocate dummy tail memory for all receive contexts */
+	dd->rcvhdrtail_dummy_kvaddr = dma_zalloc_coherent(
+		&dd->pcidev->dev, sizeof(u64),
+		&dd->rcvhdrtail_dummy_physaddr,
+		GFP_KERNEL);
+
+	if (!dd->rcvhdrtail_dummy_kvaddr) {
+		dd_dev_err(dd, "cannot allocate dummy tail memory\n");
+		ret = -ENOMEM;
+		goto done;
+	}
+
 	/* dd->rcd can be NULL if early initialization failed */
 	for (i = 0; dd->rcd && i < dd->first_user_ctxt; ++i) {
 		/*
@@ -1266,6 +1286,14 @@ static void cleanup_device_data(struct hfi1_devdata *dd)
 	tmp = dd->rcd;
 	dd->rcd = NULL;
 	spin_unlock_irqrestore(&dd->uctxt_lock, flags);
+
+	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_kvaddr = NULL;
+	}
+
 	for (ctxt = 0; tmp && ctxt < dd->num_rcv_contexts; ctxt++) {
 		struct hfi1_ctxtdata *rcd = tmp[ctxt];
 
@@ -1308,6 +1336,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	int ret = 0, j, pidx, initfail;
 	struct hfi1_devdata *dd = NULL;
+	struct hfi1_pportdata *ppd;
 
 	/* First, lock the non-writable module parameters */
 	HFI1_CAP_LOCK();
@@ -1322,6 +1351,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (!encode_rcv_header_entry_size(hfi1_hdrq_entsize)) {
 		hfi1_early_err(&pdev->dev, "Invalid HdrQ Entry size %u\n",
 			       hfi1_hdrq_entsize);
+		ret = -EINVAL;
 		goto bail;
 	}
 
@@ -1403,8 +1433,14 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (initfail || ret) {
 		stop_timers(dd);
 		flush_workqueue(ib_wq);
-		for (pidx = 0; pidx < dd->num_pports; ++pidx)
+		for (pidx = 0; pidx < dd->num_pports; ++pidx) {
 			hfi1_quiet_serdes(dd->pport + pidx);
+			ppd = dd->pport + pidx;
+			if (ppd->hfi1_wq) {
+				destroy_workqueue(ppd->hfi1_wq);
+				ppd->hfi1_wq = NULL;
+			}
+		}
 		if (!j)
 			hfi1_device_remove(dd);
 		if (!ret)
@@ -1521,6 +1557,14 @@ int hfi1_create_rcvhdrq(struct hfi1_devdata *dd, struct hfi1_ctxtdata *rcd)
 	reg = (dd->rcvhdrsize & RCV_HDR_SIZE_HDR_SIZE_MASK)
 		<< RCV_HDR_SIZE_HDR_SIZE_SHIFT;
 	write_kctxt_csr(dd, rcd->ctxt, RCV_HDR_SIZE, reg);
+
+	/*
+	 * Program dummy tail address for every receive context
+	 * before enabling any receive context
+	 */
+	write_kctxt_csr(dd, rcd->ctxt, RCV_HDR_TAIL_ADDR,
+			dd->rcvhdrtail_dummy_physaddr);
+
 	return 0;
 
 bail_free:
@@ -1660,9 +1704,11 @@ int hfi1_setup_eagerbufs(struct hfi1_ctxtdata *rcd)
 	rcd->egrbufs.numbufs = idx;
 	rcd->egrbufs.size = alloced_bytes;
 
-	dd_dev_info(dd, "ctxt%u: Alloced %u rcv tid entries @ %uKB, total %zuKB\n",
-		rcd->ctxt, rcd->egrbufs.alloced, rcd->egrbufs.rcvtid_size,
-		rcd->egrbufs.size);
+	hfi1_cdbg(PROC,
+		  "ctxt%u: Alloced %u rcv tid entries @ %uKB, total %zuKB\n",
+		  rcd->ctxt, rcd->egrbufs.alloced, rcd->egrbufs.rcvtid_size,
+		  rcd->egrbufs.size);
+
 
 	/*
 	 * Set the contexts rcv array head update threshold to the closest
@@ -1683,13 +1729,14 @@ int hfi1_setup_eagerbufs(struct hfi1_ctxtdata *rcd)
 		rcd->expected_count = MAX_TID_PAIR_ENTRIES * 2;
 
 	rcd->expected_base = rcd->eager_base + egrtop;
-	dd_dev_info(dd, "ctxt%u: eager:%u, exp:%u, egrbase:%u, expbase:%u\n",
-		    rcd->ctxt, rcd->egrbufs.alloced, rcd->expected_count,
-		    rcd->eager_base, rcd->expected_base);
+	hfi1_cdbg(PROC, "ctxt%u: eager:%u, exp:%u, egrbase:%u, expbase:%u\n",
+		  rcd->ctxt, rcd->egrbufs.alloced, rcd->expected_count,
+		  rcd->eager_base, rcd->expected_base);
 
 	if (!hfi1_rcvbuf_validate(rcd->egrbufs.rcvtid_size, PT_EAGER, &order)) {
-		dd_dev_err(dd, "ctxt%u: current Eager buffer size is invalid %u\n",
-			   rcd->ctxt, rcd->egrbufs.rcvtid_size);
+		hfi1_cdbg(PROC,
+			  "ctxt%u: current Eager buffer size is invalid %u\n",
+			  rcd->ctxt, rcd->egrbufs.rcvtid_size);
 		ret = -EINVAL;
 		goto bail;
 	}
diff --git a/drivers/staging/rdma/hfi1/iowait.h b/drivers/staging/rdma/hfi1/iowait.h
index fa361b405851..e8ba5606d08d 100644
--- a/drivers/staging/rdma/hfi1/iowait.h
+++ b/drivers/staging/rdma/hfi1/iowait.h
@@ -150,12 +150,14 @@ static inline void iowait_init(
  * iowait_schedule() - initialize wait structure
  * @wait: wait struct to schedule
  * @wq: workqueue for schedule
+ * @cpu: cpu
  */
 static inline void iowait_schedule(
 	struct iowait *wait,
-	struct workqueue_struct *wq)
+	struct workqueue_struct *wq,
+	int cpu)
 {
-	queue_work(wq, &wait->iowork);
+	queue_work_on(cpu, wq, &wait->iowork);
 }
 
 /**
diff --git a/drivers/staging/rdma/hfi1/mad.c b/drivers/staging/rdma/hfi1/mad.c
index 32f703736185..4f5dbd14b5de 100644
--- a/drivers/staging/rdma/hfi1/mad.c
+++ b/drivers/staging/rdma/hfi1/mad.c
@@ -84,7 +84,7 @@ static void send_trap(struct hfi1_ibport *ibp, void *data, unsigned len)
 {
 	struct ib_mad_send_buf *send_buf;
 	struct ib_mad_agent *agent;
-	struct ib_smp *smp;
+	struct opa_smp *smp;
 	int ret;
 	unsigned long flags;
 	unsigned long timeout;
@@ -117,15 +117,15 @@ static void send_trap(struct hfi1_ibport *ibp, void *data, unsigned len)
 		return;
 
 	smp = send_buf->mad;
-	smp->base_version = IB_MGMT_BASE_VERSION;
+	smp->base_version = OPA_MGMT_BASE_VERSION;
 	smp->mgmt_class = IB_MGMT_CLASS_SUBN_LID_ROUTED;
-	smp->class_version = 1;
+	smp->class_version = OPA_SMI_CLASS_VERSION;
 	smp->method = IB_MGMT_METHOD_TRAP;
 	ibp->tid++;
 	smp->tid = cpu_to_be64(ibp->tid);
 	smp->attr_id = IB_SMP_ATTR_NOTICE;
 	/* o14-1: smp->mkey = 0; */
-	memcpy(smp->data, data, len);
+	memcpy(smp->route.lid.data, data, len);
 
 	spin_lock_irqsave(&ibp->lock, flags);
 	if (!ibp->sm_ah) {
@@ -164,11 +164,16 @@ static void send_trap(struct hfi1_ibport *ibp, void *data, unsigned len)
  * Send a bad [PQ]_Key trap (ch. 14.3.8).
  */
 void hfi1_bad_pqkey(struct hfi1_ibport *ibp, __be16 trap_num, u32 key, u32 sl,
-		    u32 qp1, u32 qp2, __be16 lid1, __be16 lid2)
+		    u32 qp1, u32 qp2, u16 lid1, u16 lid2)
 {
-	struct ib_mad_notice_attr data;
+	struct opa_mad_notice_attr data;
+	u32 lid = ppd_from_ibp(ibp)->lid;
+	u32 _lid1 = lid1;
+	u32 _lid2 = lid2;
 
-	if (trap_num == IB_NOTICE_TRAP_BAD_PKEY)
+	memset(&data, 0, sizeof(data));
+
+	if (trap_num == OPA_TRAP_BAD_P_KEY)
 		ibp->pkey_violations++;
 	else
 		ibp->qkey_violations++;
@@ -176,17 +181,15 @@ void hfi1_bad_pqkey(struct hfi1_ibport *ibp, __be16 trap_num, u32 key, u32 sl,
 
 	/* Send violation trap */
 	data.generic_type = IB_NOTICE_TYPE_SECURITY;
-	data.prod_type_msb = 0;
 	data.prod_type_lsb = IB_NOTICE_PROD_CA;
 	data.trap_num = trap_num;
-	data.issuer_lid = cpu_to_be16(ppd_from_ibp(ibp)->lid);
-	data.toggle_count = 0;
-	memset(&data.details, 0, sizeof(data.details));
-	data.details.ntc_257_258.lid1 = lid1;
-	data.details.ntc_257_258.lid2 = lid2;
-	data.details.ntc_257_258.key = cpu_to_be32(key);
-	data.details.ntc_257_258.sl_qp1 = cpu_to_be32((sl << 28) | qp1);
-	data.details.ntc_257_258.qp2 = cpu_to_be32(qp2);
+	data.issuer_lid = cpu_to_be32(lid);
+	data.ntc_257_258.lid1 = cpu_to_be32(_lid1);
+	data.ntc_257_258.lid2 = cpu_to_be32(_lid2);
+	data.ntc_257_258.key = cpu_to_be32(key);
+	data.ntc_257_258.sl = sl << 3;
+	data.ntc_257_258.qp1 = cpu_to_be32(qp1);
+	data.ntc_257_258.qp2 = cpu_to_be32(qp2);
 
 	send_trap(ibp, &data, sizeof(data));
 }
@@ -197,32 +200,30 @@ void hfi1_bad_pqkey(struct hfi1_ibport *ibp, __be16 trap_num, u32 key, u32 sl,
 static void bad_mkey(struct hfi1_ibport *ibp, struct ib_mad_hdr *mad,
 		     __be64 mkey, __be32 dr_slid, u8 return_path[], u8 hop_cnt)
 {
-	struct ib_mad_notice_attr data;
+	struct opa_mad_notice_attr data;
+	u32 lid = ppd_from_ibp(ibp)->lid;
 
+	memset(&data, 0, sizeof(data));
 	/* Send violation trap */
 	data.generic_type = IB_NOTICE_TYPE_SECURITY;
-	data.prod_type_msb = 0;
 	data.prod_type_lsb = IB_NOTICE_PROD_CA;
-	data.trap_num = IB_NOTICE_TRAP_BAD_MKEY;
-	data.issuer_lid = cpu_to_be16(ppd_from_ibp(ibp)->lid);
-	data.toggle_count = 0;
-	memset(&data.details, 0, sizeof(data.details));
-	data.details.ntc_256.lid = data.issuer_lid;
-	data.details.ntc_256.method = mad->method;
-	data.details.ntc_256.attr_id = mad->attr_id;
-	data.details.ntc_256.attr_mod = mad->attr_mod;
-	data.details.ntc_256.mkey = mkey;
+	data.trap_num = OPA_TRAP_BAD_M_KEY;
+	data.issuer_lid = cpu_to_be32(lid);
+	data.ntc_256.lid = data.issuer_lid;
+	data.ntc_256.method = mad->method;
+	data.ntc_256.attr_id = mad->attr_id;
+	data.ntc_256.attr_mod = mad->attr_mod;
+	data.ntc_256.mkey = mkey;
 	if (mad->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
-
-		data.details.ntc_256.dr_slid = (__force __be16)dr_slid;
-		data.details.ntc_256.dr_trunc_hop = IB_NOTICE_TRAP_DR_NOTICE;
-		if (hop_cnt > ARRAY_SIZE(data.details.ntc_256.dr_rtn_path)) {
-			data.details.ntc_256.dr_trunc_hop |=
+		data.ntc_256.dr_slid = dr_slid;
+		data.ntc_256.dr_trunc_hop = IB_NOTICE_TRAP_DR_NOTICE;
+		if (hop_cnt > ARRAY_SIZE(data.ntc_256.dr_rtn_path)) {
+			data.ntc_256.dr_trunc_hop |=
 				IB_NOTICE_TRAP_DR_TRUNC;
-			hop_cnt = ARRAY_SIZE(data.details.ntc_256.dr_rtn_path);
+			hop_cnt = ARRAY_SIZE(data.ntc_256.dr_rtn_path);
 		}
-		data.details.ntc_256.dr_trunc_hop |= hop_cnt;
-		memcpy(data.details.ntc_256.dr_rtn_path, return_path,
+		data.ntc_256.dr_trunc_hop |= hop_cnt;
+		memcpy(data.ntc_256.dr_rtn_path, return_path,
 		       hop_cnt);
 	}
 
@@ -234,17 +235,17 @@ static void bad_mkey(struct hfi1_ibport *ibp, struct ib_mad_hdr *mad,
  */
 void hfi1_cap_mask_chg(struct hfi1_ibport *ibp)
 {
-	struct ib_mad_notice_attr data;
+	struct opa_mad_notice_attr data;
+	u32 lid = ppd_from_ibp(ibp)->lid;
+
+	memset(&data, 0, sizeof(data));
 
 	data.generic_type = IB_NOTICE_TYPE_INFO;
-	data.prod_type_msb = 0;
 	data.prod_type_lsb = IB_NOTICE_PROD_CA;
-	data.trap_num = IB_NOTICE_TRAP_CAP_MASK_CHG;
-	data.issuer_lid = cpu_to_be16(ppd_from_ibp(ibp)->lid);
-	data.toggle_count = 0;
-	memset(&data.details, 0, sizeof(data.details));
-	data.details.ntc_144.lid = data.issuer_lid;
-	data.details.ntc_144.new_cap_mask = cpu_to_be32(ibp->port_cap_flags);
+	data.trap_num = OPA_TRAP_CHANGE_CAPABILITY;
+	data.issuer_lid = cpu_to_be32(lid);
+	data.ntc_144.lid = data.issuer_lid;
+	data.ntc_144.new_cap_mask = cpu_to_be32(ibp->port_cap_flags);
 
 	send_trap(ibp, &data, sizeof(data));
 }
@@ -254,17 +255,17 @@ void hfi1_cap_mask_chg(struct hfi1_ibport *ibp)
  */
 void hfi1_sys_guid_chg(struct hfi1_ibport *ibp)
 {
-	struct ib_mad_notice_attr data;
+	struct opa_mad_notice_attr data;
+	u32 lid = ppd_from_ibp(ibp)->lid;
+
+	memset(&data, 0, sizeof(data));
 
 	data.generic_type = IB_NOTICE_TYPE_INFO;
-	data.prod_type_msb = 0;
 	data.prod_type_lsb = IB_NOTICE_PROD_CA;
-	data.trap_num = IB_NOTICE_TRAP_SYS_GUID_CHG;
-	data.issuer_lid = cpu_to_be16(ppd_from_ibp(ibp)->lid);
-	data.toggle_count = 0;
-	memset(&data.details, 0, sizeof(data.details));
-	data.details.ntc_145.lid = data.issuer_lid;
-	data.details.ntc_145.new_sys_guid = ib_hfi1_sys_image_guid;
+	data.trap_num = OPA_TRAP_CHANGE_SYSGUID;
+	data.issuer_lid = cpu_to_be32(lid);
+	data.ntc_145.new_sys_guid = ib_hfi1_sys_image_guid;
+	data.ntc_145.lid = data.issuer_lid;
 
 	send_trap(ibp, &data, sizeof(data));
 }
@@ -274,18 +275,18 @@ void hfi1_sys_guid_chg(struct hfi1_ibport *ibp)
  */
 void hfi1_node_desc_chg(struct hfi1_ibport *ibp)
 {
-	struct ib_mad_notice_attr data;
+	struct opa_mad_notice_attr data;
+	u32 lid = ppd_from_ibp(ibp)->lid;
+
+	memset(&data, 0, sizeof(data));
 
 	data.generic_type = IB_NOTICE_TYPE_INFO;
-	data.prod_type_msb = 0;
 	data.prod_type_lsb = IB_NOTICE_PROD_CA;
-	data.trap_num = IB_NOTICE_TRAP_CAP_MASK_CHG;
-	data.issuer_lid = cpu_to_be16(ppd_from_ibp(ibp)->lid);
-	data.toggle_count = 0;
-	memset(&data.details, 0, sizeof(data.details));
-	data.details.ntc_144.lid = data.issuer_lid;
-	data.details.ntc_144.local_changes = 1;
-	data.details.ntc_144.change_flags = IB_NOTICE_TRAP_NODE_DESC_CHG;
+	data.trap_num = OPA_TRAP_CHANGE_CAPABILITY;
+	data.issuer_lid = cpu_to_be32(lid);
+	data.ntc_144.lid = data.issuer_lid;
+	data.ntc_144.change_flags =
+		cpu_to_be16(OPA_NOTICE_TRAP_NODE_DESC_CHG);
 
 	send_trap(ibp, &data, sizeof(data));
 }
@@ -2076,13 +2077,20 @@ struct opa_aggregate {
 	u8 data[0];
 };
 
-/* Request contains first two fields, response contains those plus the rest */
+#define MSK_LLI 0x000000f0
+#define MSK_LLI_SFT 4
+#define MSK_LER 0x0000000f
+#define MSK_LER_SFT 0
+#define ADD_LLI 8
+#define ADD_LER 2
+
+/* Request contains first three fields, response contains those plus the rest */
 struct opa_port_data_counters_msg {
 	__be64 port_select_mask[4];
 	__be32 vl_select_mask;
+	__be32 resolution;
 
 	/* Response fields follow */
-	__be32 reserved1;
 	struct _port_dctrs {
 		u8 port_number;
 		u8 reserved2[3];
@@ -2271,34 +2279,8 @@ static void a0_portstatus(struct hfi1_pportdata *ppd,
 {
 	if (!is_bx(ppd->dd)) {
 		unsigned long vl;
-		int vfi = 0;
 		u64 max_vl_xmit_wait = 0, tmp;
 		u32 vl_all_mask = VL_MASK_ALL;
-		u64 rcv_data, rcv_bubble;
-
-		rcv_data = be64_to_cpu(rsp->port_rcv_data);
-		rcv_bubble = be64_to_cpu(rsp->port_rcv_bubble);
-		/* In the measured time period, calculate the total number
-		 * of flits that were received. Subtract out one false
-		 * rcv_bubble increment for every 32 received flits but
-		 * don't let the number go negative.
-		 */
-		if (rcv_bubble >= (rcv_data>>5)) {
-			rcv_bubble -= (rcv_data>>5);
-			rsp->port_rcv_bubble = cpu_to_be64(rcv_bubble);
-		}
-		for_each_set_bit(vl, (unsigned long *)&(vl_select_mask),
-				 8 * sizeof(vl_select_mask)) {
-			rcv_data = be64_to_cpu(rsp->vls[vfi].port_vl_rcv_data);
-			rcv_bubble =
-				be64_to_cpu(rsp->vls[vfi].port_vl_rcv_bubble);
-			if (rcv_bubble >= (rcv_data>>5)) {
-				rcv_bubble -= (rcv_data>>5);
-				rsp->vls[vfi].port_vl_rcv_bubble =
-							cpu_to_be64(rcv_bubble);
-			}
-			vfi++;
-		}
 
 		for_each_set_bit(vl, (unsigned long *)&(vl_all_mask),
 				 8 * sizeof(vl_all_mask)) {
@@ -2363,8 +2345,6 @@ static int pma_get_opa_portstatus(struct opa_pma_mad *pmp,
 					  CNTR_INVALID_VL));
 	rsp->port_rcv_data = cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_FLITS,
 					 CNTR_INVALID_VL));
-	rsp->port_rcv_bubble =
-		cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_BBL, CNTR_INVALID_VL));
 	rsp->port_xmit_pkts = cpu_to_be64(read_dev_cntr(dd, C_DC_XMIT_PKTS,
 					  CNTR_INVALID_VL));
 	rsp->port_rcv_pkts = cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_PKTS,
@@ -2434,9 +2414,6 @@ static int pma_get_opa_portstatus(struct opa_pma_mad *pmp,
 
 		tmp = read_dev_cntr(dd, C_DC_RX_FLIT_VL, idx_from_vl(vl));
 		rsp->vls[vfi].port_vl_rcv_data = cpu_to_be64(tmp);
-		rsp->vls[vfi].port_vl_rcv_bubble =
-			cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_BBL_VL,
-					idx_from_vl(vl)));
 
 		rsp->vls[vfi].port_vl_rcv_pkts =
 			cpu_to_be64(read_dev_cntr(dd, C_DC_RX_PKT_VL,
@@ -2474,7 +2451,8 @@ static int pma_get_opa_portstatus(struct opa_pma_mad *pmp,
 	return reply((struct ib_mad_hdr *)pmp);
 }
 
-static u64 get_error_counter_summary(struct ib_device *ibdev, u8 port)
+static u64 get_error_counter_summary(struct ib_device *ibdev, u8 port,
+				     u8 res_lli, u8 res_ler)
 {
 	struct hfi1_devdata *dd = dd_from_ibdev(ibdev);
 	struct hfi1_ibport *ibp = to_iport(ibdev, port);
@@ -2490,14 +2468,14 @@ static u64 get_error_counter_summary(struct ib_device *ibdev, u8 port)
 						CNTR_INVALID_VL);
 	error_counter_summary += read_dev_cntr(dd, C_DC_RMT_PHY_ERR,
 						CNTR_INVALID_VL);
-	error_counter_summary += read_dev_cntr(dd, C_DC_TX_REPLAY,
-						CNTR_INVALID_VL);
-	error_counter_summary += read_dev_cntr(dd, C_DC_RX_REPLAY,
-						CNTR_INVALID_VL);
-	error_counter_summary += read_dev_cntr(dd, C_DC_SEQ_CRC_CNT,
-						CNTR_INVALID_VL);
-	error_counter_summary += read_dev_cntr(dd, C_DC_REINIT_FROM_PEER_CNT,
-						CNTR_INVALID_VL);
+	/* local link integrity must be right-shifted by the lli resolution */
+	tmp = read_dev_cntr(dd, C_DC_RX_REPLAY, CNTR_INVALID_VL);
+	tmp += read_dev_cntr(dd, C_DC_TX_REPLAY, CNTR_INVALID_VL);
+	error_counter_summary += (tmp >> res_lli);
+	/* link error recovery must b right-shifted by the ler resolution */
+	tmp = read_dev_cntr(dd, C_DC_SEQ_CRC_CNT, CNTR_INVALID_VL);
+	tmp += read_dev_cntr(dd, C_DC_REINIT_FROM_PEER_CNT, CNTR_INVALID_VL);
+	error_counter_summary += (tmp >> res_ler);
 	error_counter_summary += read_dev_cntr(dd, C_DC_RCV_ERR,
 						CNTR_INVALID_VL);
 	error_counter_summary += read_dev_cntr(dd, C_RCV_OVF, CNTR_INVALID_VL);
@@ -2519,32 +2497,8 @@ static void a0_datacounters(struct hfi1_devdata *dd, struct _port_dctrs *rsp,
 	if (!is_bx(dd)) {
 		unsigned long vl;
 		int vfi = 0;
-		u64 rcv_data, rcv_bubble, sum_vl_xmit_wait = 0;
-
-		rcv_data = be64_to_cpu(rsp->port_rcv_data);
-		rcv_bubble = be64_to_cpu(rsp->port_rcv_bubble);
-		/* In the measured time period, calculate the total number
-		 * of flits that were received. Subtract out one false
-		 * rcv_bubble increment for every 32 received flits but
-		 * don't let the number go negative.
-		 */
-		if (rcv_bubble >= (rcv_data>>5)) {
-			rcv_bubble -= (rcv_data>>5);
-			rsp->port_rcv_bubble = cpu_to_be64(rcv_bubble);
-		}
-		for_each_set_bit(vl, (unsigned long *)&(vl_select_mask),
-				8 * sizeof(vl_select_mask)) {
-			rcv_data = be64_to_cpu(rsp->vls[vfi].port_vl_rcv_data);
-			rcv_bubble =
-				be64_to_cpu(rsp->vls[vfi].port_vl_rcv_bubble);
-			if (rcv_bubble >= (rcv_data>>5)) {
-				rcv_bubble -= (rcv_data>>5);
-				rsp->vls[vfi].port_vl_rcv_bubble =
-							cpu_to_be64(rcv_bubble);
-			}
-			vfi++;
-		}
-		vfi = 0;
+		u64 sum_vl_xmit_wait = 0;
+
 		for_each_set_bit(vl, (unsigned long *)&(vl_select_mask),
 				8 * sizeof(vl_select_mask)) {
 			u64 tmp = sum_vl_xmit_wait +
@@ -2575,6 +2529,7 @@ static int pma_get_opa_datacounters(struct opa_pma_mad *pmp,
 	u32 num_ports;
 	u8 num_pslm;
 	u8 lq, num_vls;
+	u8 res_lli, res_ler;
 	u64 port_mask;
 	unsigned long port_num;
 	unsigned long vl;
@@ -2585,6 +2540,10 @@ static int pma_get_opa_datacounters(struct opa_pma_mad *pmp,
 	num_pslm = hweight64(be64_to_cpu(req->port_select_mask[3]));
 	num_vls = hweight32(be32_to_cpu(req->vl_select_mask));
 	vl_select_mask = be32_to_cpu(req->vl_select_mask);
+	res_lli = (u8)(be32_to_cpu(req->resolution) & MSK_LLI) >> MSK_LLI_SFT;
+	res_lli = res_lli ? res_lli + ADD_LLI : 0;
+	res_ler = (u8)(be32_to_cpu(req->resolution) & MSK_LER) >> MSK_LER_SFT;
+	res_ler = res_ler ? res_ler + ADD_LER : 0;
 
 	if (num_ports != 1 || (vl_select_mask & ~VL_MASK_ALL)) {
 		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
@@ -2635,8 +2594,6 @@ static int pma_get_opa_datacounters(struct opa_pma_mad *pmp,
 						CNTR_INVALID_VL));
 	rsp->port_rcv_data = cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_FLITS,
 						CNTR_INVALID_VL));
-	rsp->port_rcv_bubble =
-		cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_BBL, CNTR_INVALID_VL));
 	rsp->port_xmit_pkts = cpu_to_be64(read_dev_cntr(dd, C_DC_XMIT_PKTS,
 						CNTR_INVALID_VL));
 	rsp->port_rcv_pkts = cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_PKTS,
@@ -2655,7 +2612,8 @@ static int pma_get_opa_datacounters(struct opa_pma_mad *pmp,
 		cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_BCN, CNTR_INVALID_VL));
 
 	rsp->port_error_counter_summary =
-		cpu_to_be64(get_error_counter_summary(ibdev, port));
+		cpu_to_be64(get_error_counter_summary(ibdev, port,
+						      res_lli, res_ler));
 
 	vlinfo = &(rsp->vls[0]);
 	vfi = 0;
@@ -2675,9 +2633,6 @@ static int pma_get_opa_datacounters(struct opa_pma_mad *pmp,
 		rsp->vls[vfi].port_vl_rcv_data =
 			cpu_to_be64(read_dev_cntr(dd, C_DC_RX_FLIT_VL,
 							idx_from_vl(vl)));
-		rsp->vls[vfi].port_vl_rcv_bubble =
-			cpu_to_be64(read_dev_cntr(dd, C_DC_RCV_BBL_VL,
-					idx_from_vl(vl)));
 
 		rsp->vls[vfi].port_vl_xmit_pkts =
 			cpu_to_be64(read_port_cntr(ppd, C_TX_PKT_VL,
@@ -3458,7 +3413,7 @@ static int __subn_get_opa_led_info(struct opa_smp *smp, u32 am, u8 *data,
 	u32 nport = OPA_AM_NPORT(am);
 	u64 reg;
 
-	if (nport != 1 || OPA_AM_PORTNUM(am)) {
+	if (nport != 1) {
 		smp->status |= IB_SMP_INVALID_FIELD;
 		return reply((struct ib_mad_hdr *)smp);
 	}
@@ -3483,7 +3438,7 @@ static int __subn_set_opa_led_info(struct opa_smp *smp, u32 am, u8 *data,
 	u32 nport = OPA_AM_NPORT(am);
 	int on = !!(be32_to_cpu(p->rsvd_led_mask) & OPA_LED_MASK);
 
-	if (nport != 1 || OPA_AM_PORTNUM(am)) {
+	if (nport != 1) {
 		smp->status |= IB_SMP_INVALID_FIELD;
 		return reply((struct ib_mad_hdr *)smp);
 	}
diff --git a/drivers/staging/rdma/hfi1/mad.h b/drivers/staging/rdma/hfi1/mad.h
index 47457501c044..f0317750e2fc 100644
--- a/drivers/staging/rdma/hfi1/mad.h
+++ b/drivers/staging/rdma/hfi1/mad.h
@@ -60,7 +60,121 @@
 #endif
 #include "opa_compat.h"
 
+/*
+ * OPA Traps
+ */
+#define OPA_TRAP_GID_NOW_IN_SERVICE             cpu_to_be16(64)
+#define OPA_TRAP_GID_OUT_OF_SERVICE             cpu_to_be16(65)
+#define OPA_TRAP_ADD_MULTICAST_GROUP            cpu_to_be16(66)
+#define OPA_TRAL_DEL_MULTICAST_GROUP            cpu_to_be16(67)
+#define OPA_TRAP_UNPATH                         cpu_to_be16(68)
+#define OPA_TRAP_REPATH                         cpu_to_be16(69)
+#define OPA_TRAP_PORT_CHANGE_STATE              cpu_to_be16(128)
+#define OPA_TRAP_LINK_INTEGRITY                 cpu_to_be16(129)
+#define OPA_TRAP_EXCESSIVE_BUFFER_OVERRUN       cpu_to_be16(130)
+#define OPA_TRAP_FLOW_WATCHDOG                  cpu_to_be16(131)
+#define OPA_TRAP_CHANGE_CAPABILITY              cpu_to_be16(144)
+#define OPA_TRAP_CHANGE_SYSGUID                 cpu_to_be16(145)
+#define OPA_TRAP_BAD_M_KEY                      cpu_to_be16(256)
+#define OPA_TRAP_BAD_P_KEY                      cpu_to_be16(257)
+#define OPA_TRAP_BAD_Q_KEY                      cpu_to_be16(258)
+#define OPA_TRAP_SWITCH_BAD_PKEY                cpu_to_be16(259)
+#define OPA_SMA_TRAP_DATA_LINK_WIDTH            cpu_to_be16(2048)
 
+/*
+ * Generic trap/notice other local changes flags (trap 144).
+ */
+#define	OPA_NOTICE_TRAP_LWDE_CHG        0x08 /* Link Width Downgrade Enable
+					      * changed
+					      */
+#define OPA_NOTICE_TRAP_LSE_CHG         0x04 /* Link Speed Enable changed */
+#define OPA_NOTICE_TRAP_LWE_CHG         0x02 /* Link Width Enable changed */
+#define OPA_NOTICE_TRAP_NODE_DESC_CHG   0x01
+
+struct opa_mad_notice_attr {
+	u8 generic_type;
+	u8 prod_type_msb;
+	__be16 prod_type_lsb;
+	__be16 trap_num;
+	__be16 toggle_count;
+	__be32 issuer_lid;
+	__be32 reserved1;
+	union ib_gid issuer_gid;
+
+	union {
+		struct {
+			u8	details[64];
+		} raw_data;
+
+		struct {
+			union ib_gid	gid;
+		} __packed ntc_64_65_66_67;
+
+		struct {
+			__be32	lid;
+		} __packed ntc_128;
+
+		struct {
+			__be32	lid;		/* where violation happened */
+			u8	port_num;	/* where violation happened */
+		} __packed ntc_129_130_131;
+
+		struct {
+			__be32	lid;		/* LID where change occurred */
+			__be32	new_cap_mask;	/* new capability mask */
+			__be16	reserved2;
+			__be16	cap_mask;
+			__be16	change_flags;	/* low 4 bits only */
+		} __packed ntc_144;
+
+		struct {
+			__be64	new_sys_guid;
+			__be32	lid;		/* lid where sys guid changed */
+		} __packed ntc_145;
+
+		struct {
+			__be32	lid;
+			__be32	dr_slid;
+			u8	method;
+			u8	dr_trunc_hop;
+			__be16	attr_id;
+			__be32	attr_mod;
+			__be64	mkey;
+			u8	dr_rtn_path[30];
+		} __packed ntc_256;
+
+		struct {
+			__be32		lid1;
+			__be32		lid2;
+			__be32		key;
+			u8		sl;	/* SL: high 5 bits */
+			u8		reserved3[3];
+			union ib_gid	gid1;
+			union ib_gid	gid2;
+			__be32		qp1;	/* high 8 bits reserved */
+			__be32		qp2;	/* high 8 bits reserved */
+		} __packed ntc_257_258;
+
+		struct {
+			__be16		flags;	/* low 8 bits reserved */
+			__be16		pkey;
+			__be32		lid1;
+			__be32		lid2;
+			u8		sl;	/* SL: high 5 bits */
+			u8		reserved4[3];
+			union ib_gid	gid1;
+			union ib_gid	gid2;
+			__be32		qp1;	/* high 8 bits reserved */
+			__be32		qp2;	/* high 8 bits reserved */
+		} __packed ntc_259;
+
+		struct {
+			__be32	lid;
+		} __packed ntc_2048;
+
+	};
+	u8	class_data[0];
+};
 
 #define IB_VLARB_LOWPRI_0_31    1
 #define IB_VLARB_LOWPRI_32_63   2
diff --git a/drivers/staging/rdma/hfi1/mr.c b/drivers/staging/rdma/hfi1/mr.c
index 568f185a022d..a3f8b884fdd6 100644
--- a/drivers/staging/rdma/hfi1/mr.c
+++ b/drivers/staging/rdma/hfi1/mr.c
@@ -167,10 +167,7 @@ static struct hfi1_mr *alloc_mr(int count, struct ib_pd *pd)
 	rval = init_mregion(&mr->mr, pd, count);
 	if (rval)
 		goto bail;
-	/*
-	 * ib_reg_phys_mr() will initialize mr->ibmr except for
-	 * lkey and rkey.
-	 */
+
 	rval = hfi1_alloc_lkey(&mr->mr, 0);
 	if (rval)
 		goto bail_mregion;
@@ -188,52 +185,6 @@ bail:
 }
 
 /**
- * hfi1_reg_phys_mr - register a physical memory region
- * @pd: protection domain for this memory region
- * @buffer_list: pointer to the list of physical buffers to register
- * @num_phys_buf: the number of physical buffers to register
- * @iova_start: the starting address passed over IB which maps to this MR
- *
- * Returns the memory region on success, otherwise returns an errno.
- */
-struct ib_mr *hfi1_reg_phys_mr(struct ib_pd *pd,
-			       struct ib_phys_buf *buffer_list,
-			       int num_phys_buf, int acc, u64 *iova_start)
-{
-	struct hfi1_mr *mr;
-	int n, m, i;
-	struct ib_mr *ret;
-
-	mr = alloc_mr(num_phys_buf, pd);
-	if (IS_ERR(mr)) {
-		ret = (struct ib_mr *)mr;
-		goto bail;
-	}
-
-	mr->mr.user_base = *iova_start;
-	mr->mr.iova = *iova_start;
-	mr->mr.access_flags = acc;
-
-	m = 0;
-	n = 0;
-	for (i = 0; i < num_phys_buf; i++) {
-		mr->mr.map[m]->segs[n].vaddr = (void *) buffer_list[i].addr;
-		mr->mr.map[m]->segs[n].length = buffer_list[i].size;
-		mr->mr.length += buffer_list[i].size;
-		n++;
-		if (n == HFI1_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-
-	ret = &mr->ibmr;
-
-bail:
-	return ret;
-}
-
-/**
  * hfi1_reg_user_mr - register a userspace memory region
  * @pd: protection domain for this memory region
  * @start: starting userspace address
diff --git a/drivers/staging/rdma/hfi1/pcie.c b/drivers/staging/rdma/hfi1/pcie.c
index a956044459a2..8317b07d722a 100644
--- a/drivers/staging/rdma/hfi1/pcie.c
+++ b/drivers/staging/rdma/hfi1/pcie.c
@@ -426,14 +426,6 @@ void request_msix(struct hfi1_devdata *dd, u32 *nent,
 	tune_pcie_caps(dd);
 }
 
-/*
- * Disable MSI-X.
- */
-void hfi1_nomsix(struct hfi1_devdata *dd)
-{
-	pci_disable_msix(dd->pcidev);
-}
-
 void hfi1_enable_intx(struct pci_dev *pdev)
 {
 	/* first, turn on INTx */
@@ -475,8 +467,18 @@ static void tune_pcie_caps(struct hfi1_devdata *dd)
 {
 	struct pci_dev *parent;
 	u16 rc_mpss, rc_mps, ep_mpss, ep_mps;
-	u16 rc_mrrs, ep_mrrs, max_mrrs;
+	u16 rc_mrrs, ep_mrrs, max_mrrs, ectl;
 
+	/*
+	 * Turn on extended tags in DevCtl in case the BIOS has turned it off
+	 * to improve WFR SDMA bandwidth
+	 */
+	pcie_capability_read_word(dd->pcidev, PCI_EXP_DEVCTL, &ectl);
+	if (!(ectl & PCI_EXP_DEVCTL_EXT_TAG)) {
+		dd_dev_info(dd, "Enabling PCIe extended tags\n");
+		ectl |= PCI_EXP_DEVCTL_EXT_TAG;
+		pcie_capability_write_word(dd->pcidev, PCI_EXP_DEVCTL, ectl);
+	}
 	/* Find out supported and configured values for parent (root) */
 	parent = dd->pcidev->bus->self;
 	if (!pci_is_root_bus(parent->bus)) {
@@ -915,7 +917,7 @@ int do_pcie_gen3_transition(struct hfi1_devdata *dd)
 	/*
 	 * A0 needs an additional SBR
 	 */
-	if (is_a0(dd))
+	if (is_ax(dd))
 		nsbr++;
 
 	/*
@@ -947,17 +949,7 @@ int do_pcie_gen3_transition(struct hfi1_devdata *dd)
 	}
 
 retry:
-
-	if (therm) {
-		/*
-		 * toggle SPICO_ENABLE to get back to the state
-		 * just after the firmware load
-		 */
-		sbus_request(dd, SBUS_MASTER_BROADCAST, 0x01,
-			WRITE_SBUS_RECEIVER, 0x00000040);
-		sbus_request(dd, SBUS_MASTER_BROADCAST, 0x01,
-			WRITE_SBUS_RECEIVER, 0x00000140);
-	}
+	/* the SBus download will reset the spico for thermal */
 
 	/* step 3: download SBus Master firmware */
 	/* step 4: download PCIe Gen3 SerDes firmware */
@@ -1201,7 +1193,7 @@ retry:
 	write_csr(dd, CCE_DC_CTRL, 0);
 
 	/* Set the LED off */
-	if (is_a0(dd))
+	if (is_ax(dd))
 		setextled(dd, 0);
 
 	/* check for any per-lane errors */
diff --git a/drivers/staging/rdma/hfi1/pio.c b/drivers/staging/rdma/hfi1/pio.c
index e5c32db4bc67..b51a4416312b 100644
--- a/drivers/staging/rdma/hfi1/pio.c
+++ b/drivers/staging/rdma/hfi1/pio.c
@@ -660,6 +660,24 @@ void set_pio_integrity(struct send_context *sc)
 	write_kctxt_csr(dd, hw_context, SC(CHECK_ENABLE), reg);
 }
 
+static u32 get_buffers_allocated(struct send_context *sc)
+{
+	int cpu;
+	u32 ret = 0;
+
+	for_each_possible_cpu(cpu)
+		ret += *per_cpu_ptr(sc->buffers_allocated, cpu);
+	return ret;
+}
+
+static void reset_buffers_allocated(struct send_context *sc)
+{
+	int cpu;
+
+	for_each_possible_cpu(cpu)
+		(*per_cpu_ptr(sc->buffers_allocated, cpu)) = 0;
+}
+
 /*
  * Allocate a NUMA relative send context structure of the given type along
  * with a HW context.
@@ -668,7 +686,7 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 			      uint hdrqentsize, int numa)
 {
 	struct send_context_info *sci;
-	struct send_context *sc;
+	struct send_context *sc = NULL;
 	dma_addr_t pa;
 	unsigned long flags;
 	u64 reg;
@@ -686,10 +704,20 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 	if (!sc)
 		return NULL;
 
+	sc->buffers_allocated = alloc_percpu(u32);
+	if (!sc->buffers_allocated) {
+		kfree(sc);
+		dd_dev_err(dd,
+			   "Cannot allocate buffers_allocated per cpu counters\n"
+			  );
+		return NULL;
+	}
+
 	spin_lock_irqsave(&dd->sc_lock, flags);
 	ret = sc_hw_alloc(dd, type, &sw_index, &hw_context);
 	if (ret) {
 		spin_unlock_irqrestore(&dd->sc_lock, flags);
+		free_percpu(sc->buffers_allocated);
 		kfree(sc);
 		return NULL;
 	}
@@ -705,7 +733,6 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 	spin_lock_init(&sc->credit_ctrl_lock);
 	INIT_LIST_HEAD(&sc->piowait);
 	INIT_WORK(&sc->halt_work, sc_halted);
-	atomic_set(&sc->buffers_allocated, 0);
 	init_waitqueue_head(&sc->halt_wait);
 
 	/* grouping is always single context for now */
@@ -815,15 +842,16 @@ struct send_context *sc_alloc(struct hfi1_devdata *dd, int type,
 		}
 	}
 
-	dd_dev_info(dd,
-		"Send context %u(%u) %s group %u credits %u credit_ctrl 0x%llx threshold %u\n",
-		sw_index,
-		hw_context,
-		sc_type_name(type),
-		sc->group,
-		sc->credits,
-		sc->credit_ctrl,
-		thresh);
+	hfi1_cdbg(PIO,
+		  "Send context %u(%u) %s group %u credits %u credit_ctrl 0x%llx threshold %u\n",
+		  sw_index,
+		  hw_context,
+		  sc_type_name(type),
+		  sc->group,
+		  sc->credits,
+		  sc->credit_ctrl,
+		  thresh);
+
 
 	return sc;
 }
@@ -865,6 +893,7 @@ void sc_free(struct send_context *sc)
 	spin_unlock_irqrestore(&dd->sc_lock, flags);
 
 	kfree(sc->sr);
+	free_percpu(sc->buffers_allocated);
 	kfree(sc);
 }
 
@@ -1028,7 +1057,7 @@ int sc_restart(struct send_context *sc)
 		/* kernel context */
 		loop = 0;
 		while (1) {
-			count = atomic_read(&sc->buffers_allocated);
+			count = get_buffers_allocated(sc);
 			if (count == 0)
 				break;
 			if (loop > 100) {
@@ -1196,7 +1225,8 @@ int sc_enable(struct send_context *sc)
 	sc->sr_head = 0;
 	sc->sr_tail = 0;
 	sc->flags = 0;
-	atomic_set(&sc->buffers_allocated, 0);
+	/* the alloc lock insures no fast path allocation */
+	reset_buffers_allocated(sc);
 
 	/*
 	 * Clear all per-context errors.  Some of these will be set when
@@ -1372,7 +1402,8 @@ retry:
 
 	/* there is enough room */
 
-	atomic_inc(&sc->buffers_allocated);
+	preempt_disable();
+	this_cpu_inc(*sc->buffers_allocated);
 
 	/* read this once */
 	head = sc->sr_head;
@@ -1564,6 +1595,7 @@ void sc_release_update(struct send_context *sc)
 	u64 hw_free;
 	u32 head, tail;
 	unsigned long old_free;
+	unsigned long free;
 	unsigned long extra;
 	unsigned long flags;
 	int code;
@@ -1578,7 +1610,7 @@ void sc_release_update(struct send_context *sc)
 	extra = (((hw_free & CR_COUNTER_SMASK) >> CR_COUNTER_SHIFT)
 			- (old_free & CR_COUNTER_MASK))
 				& CR_COUNTER_MASK;
-	sc->free = old_free + extra;
+	free = old_free + extra;
 	trace_hfi1_piofree(sc, extra);
 
 	/* call sent buffer callbacks */
@@ -1588,7 +1620,7 @@ void sc_release_update(struct send_context *sc)
 	while (head != tail) {
 		pbuf = &sc->sr[tail].pbuf;
 
-		if (sent_before(sc->free, pbuf->sent_at)) {
+		if (sent_before(free, pbuf->sent_at)) {
 			/* not sent yet */
 			break;
 		}
@@ -1602,8 +1634,10 @@ void sc_release_update(struct send_context *sc)
 		if (tail >= sc->sr_size)
 			tail = 0;
 	}
-	/* update tail, in case we moved it */
 	sc->sr_tail = tail;
+	/* make sure tail is updated before free */
+	smp_wmb();
+	sc->free = free;
 	spin_unlock_irqrestore(&sc->release_lock, flags);
 	sc_piobufavail(sc);
 }
diff --git a/drivers/staging/rdma/hfi1/pio.h b/drivers/staging/rdma/hfi1/pio.h
index 0bb885ca3cfb..53d3e0a79375 100644
--- a/drivers/staging/rdma/hfi1/pio.h
+++ b/drivers/staging/rdma/hfi1/pio.h
@@ -130,7 +130,7 @@ struct send_context {
 	spinlock_t credit_ctrl_lock ____cacheline_aligned_in_smp;
 	u64 credit_ctrl;		/* cache for credit control */
 	u32 credit_intr_count;		/* count of credit intr users */
-	atomic_t buffers_allocated;	/* count of buffers allocated */
+	u32 __percpu *buffers_allocated;/* count of buffers allocated */
 	wait_queue_head_t halt_wait;    /* wait until kernel sees interrupt */
 };
 
diff --git a/drivers/staging/rdma/hfi1/pio_copy.c b/drivers/staging/rdma/hfi1/pio_copy.c
index 8972bbc02038..ebb0bafc68cb 100644
--- a/drivers/staging/rdma/hfi1/pio_copy.c
+++ b/drivers/staging/rdma/hfi1/pio_copy.c
@@ -160,7 +160,8 @@ void pio_copy(struct hfi1_devdata *dd, struct pio_buf *pbuf, u64 pbc,
 	}
 
 	/* finished with this buffer */
-	atomic_dec(&pbuf->sc->buffers_allocated);
+	this_cpu_dec(*pbuf->sc->buffers_allocated);
+	preempt_enable();
 }
 
 /* USE_SHIFTS is faster in user-space tests on a Xeon X5570 @ 2.93GHz */
@@ -854,5 +855,6 @@ void seg_pio_copy_end(struct pio_buf *pbuf)
 	}
 
 	/* finished with this buffer */
-	atomic_dec(&pbuf->sc->buffers_allocated);
+	this_cpu_dec(*pbuf->sc->buffers_allocated);
+	preempt_enable();
 }
diff --git a/drivers/staging/rdma/hfi1/qp.c b/drivers/staging/rdma/hfi1/qp.c
index f8c36166962f..ce036810d576 100644
--- a/drivers/staging/rdma/hfi1/qp.c
+++ b/drivers/staging/rdma/hfi1/qp.c
@@ -378,6 +378,7 @@ static void reset_qp(struct hfi1_qp *qp, enum ib_qp_type type)
 	}
 	qp->s_ack_state = IB_OPCODE_RC_ACKNOWLEDGE;
 	qp->r_nak_state = 0;
+	qp->r_adefered = 0;
 	qp->r_aflags = 0;
 	qp->r_flags = 0;
 	qp->s_head = 0;
@@ -617,7 +618,7 @@ int hfi1_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	int mig = 0;
 	int ret;
 	u32 pmtu = 0; /* for gcc warning only */
-	struct hfi1_devdata *dd;
+	struct hfi1_devdata *dd = dd_from_dev(dev);
 
 	spin_lock_irq(&qp->r_lock);
 	spin_lock(&qp->s_lock);
@@ -631,23 +632,35 @@ int hfi1_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 		goto inval;
 
 	if (attr_mask & IB_QP_AV) {
+		u8 sc;
+
 		if (attr->ah_attr.dlid >= HFI1_MULTICAST_LID_BASE)
 			goto inval;
 		if (hfi1_check_ah(qp->ibqp.device, &attr->ah_attr))
 			goto inval;
+		sc = ah_to_sc(ibqp->device, &attr->ah_attr);
+		if (!qp_to_sdma_engine(qp, sc) &&
+		    dd->flags & HFI1_HAS_SEND_DMA)
+			goto inval;
 	}
 
 	if (attr_mask & IB_QP_ALT_PATH) {
+		u8 sc;
+
 		if (attr->alt_ah_attr.dlid >= HFI1_MULTICAST_LID_BASE)
 			goto inval;
 		if (hfi1_check_ah(qp->ibqp.device, &attr->alt_ah_attr))
 			goto inval;
-		if (attr->alt_pkey_index >= hfi1_get_npkeys(dd_from_dev(dev)))
+		if (attr->alt_pkey_index >= hfi1_get_npkeys(dd))
+			goto inval;
+		sc = ah_to_sc(ibqp->device, &attr->alt_ah_attr);
+		if (!qp_to_sdma_engine(qp, sc) &&
+		    dd->flags & HFI1_HAS_SEND_DMA)
 			goto inval;
 	}
 
 	if (attr_mask & IB_QP_PKEY_INDEX)
-		if (attr->pkey_index >= hfi1_get_npkeys(dd_from_dev(dev)))
+		if (attr->pkey_index >= hfi1_get_npkeys(dd))
 			goto inval;
 
 	if (attr_mask & IB_QP_MIN_RNR_TIMER)
@@ -792,6 +805,8 @@ int hfi1_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 		qp->remote_ah_attr = attr->ah_attr;
 		qp->s_srate = attr->ah_attr.static_rate;
 		qp->srate_mbps = ib_rate_to_mbps(qp->s_srate);
+		qp->s_sc = ah_to_sc(ibqp->device, &qp->remote_ah_attr);
+		qp->s_sde = qp_to_sdma_engine(qp, qp->s_sc);
 	}
 
 	if (attr_mask & IB_QP_ALT_PATH) {
@@ -806,6 +821,8 @@ int hfi1_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 			qp->port_num = qp->alt_ah_attr.port_num;
 			qp->s_pkey_index = qp->s_alt_pkey_index;
 			qp->s_flags |= HFI1_S_AHG_CLEAR;
+			qp->s_sc = ah_to_sc(ibqp->device, &qp->remote_ah_attr);
+			qp->s_sde = qp_to_sdma_engine(qp, qp->s_sc);
 		}
 	}
 
@@ -1528,9 +1545,6 @@ struct sdma_engine *qp_to_sdma_engine(struct hfi1_qp *qp, u8 sc5)
 	if (!(dd->flags & HFI1_HAS_SEND_DMA))
 		return NULL;
 	switch (qp->ibqp.qp_type) {
-	case IB_QPT_UC:
-	case IB_QPT_RC:
-		break;
 	case IB_QPT_SMI:
 		return NULL;
 	default:
@@ -1685,3 +1699,25 @@ void qp_comm_est(struct hfi1_qp *qp)
 		qp->ibqp.event_handler(&ev, qp->ibqp.qp_context);
 	}
 }
+
+/*
+ * Switch to alternate path.
+ * The QP s_lock should be held and interrupts disabled.
+ */
+void hfi1_migrate_qp(struct hfi1_qp *qp)
+{
+	struct ib_event ev;
+
+	qp->s_mig_state = IB_MIG_MIGRATED;
+	qp->remote_ah_attr = qp->alt_ah_attr;
+	qp->port_num = qp->alt_ah_attr.port_num;
+	qp->s_pkey_index = qp->s_alt_pkey_index;
+	qp->s_flags |= HFI1_S_AHG_CLEAR;
+	qp->s_sc = ah_to_sc(qp->ibqp.device, &qp->remote_ah_attr);
+	qp->s_sde = qp_to_sdma_engine(qp, qp->s_sc);
+
+	ev.device = qp->ibqp.device;
+	ev.element.qp = &qp->ibqp;
+	ev.event = IB_EVENT_PATH_MIG;
+	qp->ibqp.event_handler(&ev, qp->ibqp.qp_context);
+}
diff --git a/drivers/staging/rdma/hfi1/qp.h b/drivers/staging/rdma/hfi1/qp.h
index b9c1575990aa..62a94c5d7dca 100644
--- a/drivers/staging/rdma/hfi1/qp.h
+++ b/drivers/staging/rdma/hfi1/qp.h
@@ -128,7 +128,6 @@ static inline void clear_ahg(struct hfi1_qp *qp)
 	if (qp->s_sde && qp->s_ahgidx >= 0)
 		sdma_ahg_free(qp->s_sde, qp->s_ahgidx);
 	qp->s_ahgidx = -1;
-	qp->s_sde = NULL;
 }
 
 /**
@@ -212,7 +211,7 @@ int hfi1_qp_init(struct hfi1_ibdev *dev);
 void hfi1_qp_exit(struct hfi1_ibdev *dev);
 
 /**
- * hfi1_qp_waitup - wake up on the indicated event
+ * hfi1_qp_wakeup - wake up on the indicated event
  * @qp: the QP
  * @flag: flag the qp on which the qp is stalled
  */
@@ -223,19 +222,19 @@ struct sdma_engine *qp_to_sdma_engine(struct hfi1_qp *qp, u8 sc5);
 struct qp_iter;
 
 /**
- * qp_iter_init - wake up on the indicated event
+ * qp_iter_init - initialize the iterator for the qp hash list
  * @dev: the hfi1_ibdev
  */
 struct qp_iter *qp_iter_init(struct hfi1_ibdev *dev);
 
 /**
- * qp_iter_next - wakeup on the indicated event
+ * qp_iter_next - Find the next qp in the hash list
  * @iter: the iterator for the qp hash list
  */
 int qp_iter_next(struct qp_iter *iter);
 
 /**
- * qp_iter_next - wake up on the indicated event
+ * qp_iter_print - print the qp information to seq_file
  * @s: the seq_file to emit the qp information on
  * @iter: the iterator for the qp hash list
  */
@@ -247,4 +246,41 @@ void qp_iter_print(struct seq_file *s, struct qp_iter *iter);
  */
 void qp_comm_est(struct hfi1_qp *qp);
 
+/**
+ * _hfi1_schedule_send - schedule progress
+ * @qp: the QP
+ *
+ * This schedules qp progress w/o regard to the s_flags.
+ *
+ * It is only used in the post send, which doesn't hold
+ * the s_lock.
+ */
+static inline void _hfi1_schedule_send(struct hfi1_qp *qp)
+{
+	struct hfi1_ibport *ibp =
+		to_iport(qp->ibqp.device, qp->port_num);
+	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct hfi1_devdata *dd = dd_from_ibdev(qp->ibqp.device);
+
+	iowait_schedule(&qp->s_iowait, ppd->hfi1_wq,
+			qp->s_sde ?
+			qp->s_sde->cpu :
+			cpumask_first(cpumask_of_node(dd->assigned_node_id)));
+}
+
+/**
+ * hfi1_schedule_send - schedule progress
+ * @qp: the QP
+ *
+ * This schedules qp progress and caller should hold
+ * the s_lock.
+ */
+static inline void hfi1_schedule_send(struct hfi1_qp *qp)
+{
+	if (hfi1_send_ok(qp))
+		_hfi1_schedule_send(qp);
+}
+
+void hfi1_migrate_qp(struct hfi1_qp *qp);
+
 #endif /* _QP_H */
diff --git a/drivers/staging/rdma/hfi1/qsfp.c b/drivers/staging/rdma/hfi1/qsfp.c
index ffdb1d787a80..6326a915d7fd 100644
--- a/drivers/staging/rdma/hfi1/qsfp.c
+++ b/drivers/staging/rdma/hfi1/qsfp.c
@@ -475,7 +475,7 @@ int qsfp_dump(struct hfi1_pportdata *ppd, char *buf, int len)
 	u8 *cache = &ppd->qsfp_info.cache[0];
 	u8 bin_buff[QSFP_DUMP_CHUNK];
 	char lenstr[6];
-	int sofar, ret;
+	int sofar;
 	int bidx = 0;
 	u8 *atten = &cache[QSFP_ATTEN_OFFS];
 	u8 *vendor_oui = &cache[QSFP_VOUI_OFFS];
@@ -536,6 +536,5 @@ int qsfp_dump(struct hfi1_pportdata *ppd, char *buf, int len)
 			bidx += QSFP_DUMP_CHUNK;
 		}
 	}
-	ret = sofar;
-	return ret;
+	return sofar;
 }
diff --git a/drivers/staging/rdma/hfi1/rc.c b/drivers/staging/rdma/hfi1/rc.c
index 5fc93bb312f1..6f4a155f7931 100644
--- a/drivers/staging/rdma/hfi1/rc.c
+++ b/drivers/staging/rdma/hfi1/rc.c
@@ -1608,6 +1608,27 @@ bail:
 	return;
 }
 
+static inline void rc_defered_ack(struct hfi1_ctxtdata *rcd,
+				  struct hfi1_qp *qp)
+{
+	if (list_empty(&qp->rspwait)) {
+		qp->r_flags |= HFI1_R_RSP_DEFERED_ACK;
+		atomic_inc(&qp->refcount);
+		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
+	}
+}
+
+static inline void rc_cancel_ack(struct hfi1_qp *qp)
+{
+	qp->r_adefered = 0;
+	if (list_empty(&qp->rspwait))
+		return;
+	list_del_init(&qp->rspwait);
+	qp->r_flags &= ~HFI1_R_RSP_DEFERED_ACK;
+	if (atomic_dec_and_test(&qp->refcount))
+		wake_up(&qp->wait);
+}
+
 /**
  * rc_rcv_error - process an incoming duplicate or error RC packet
  * @ohdr: the other headers for this packet
@@ -1650,11 +1671,7 @@ static noinline int rc_rcv_error(struct hfi1_other_headers *ohdr, void *data,
 			 * in the receive queue have been processed.
 			 * Otherwise, we end up propagating congestion.
 			 */
-			if (list_empty(&qp->rspwait)) {
-				qp->r_flags |= HFI1_R_RSP_NAK;
-				atomic_inc(&qp->refcount);
-				list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
-			}
+			rc_defered_ack(rcd, qp);
 		}
 		goto done;
 	}
@@ -1978,7 +1995,7 @@ void hfi1_rc_rcv(struct hfi1_packet *packet)
 	}
 
 	psn = be32_to_cpu(ohdr->bth[2]);
-	opcode = bth0 >> 24;
+	opcode = (bth0 >> 24) & 0xff;
 
 	/*
 	 * Process responses (ACKs) before anything else.  Note that the
@@ -2329,19 +2346,29 @@ send_last:
 	qp->r_ack_psn = psn;
 	qp->r_nak_state = 0;
 	/* Send an ACK if requested or required. */
-	if (psn & (1 << 31))
-		goto send_ack;
+	if (psn & IB_BTH_REQ_ACK) {
+		if (packet->numpkt == 0) {
+			rc_cancel_ack(qp);
+			goto send_ack;
+		}
+		if (qp->r_adefered >= HFI1_PSN_CREDIT) {
+			rc_cancel_ack(qp);
+			goto send_ack;
+		}
+		if (unlikely(is_fecn)) {
+			rc_cancel_ack(qp);
+			goto send_ack;
+		}
+		qp->r_adefered++;
+		rc_defered_ack(rcd, qp);
+	}
 	return;
 
 rnr_nak:
 	qp->r_nak_state = IB_RNR_NAK | qp->r_min_rnr_timer;
 	qp->r_ack_psn = qp->r_psn;
 	/* Queue RNR NAK for later */
-	if (list_empty(&qp->rspwait)) {
-		qp->r_flags |= HFI1_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
-		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
-	}
+	rc_defered_ack(rcd, qp);
 	return;
 
 nack_op_err:
@@ -2349,11 +2376,7 @@ nack_op_err:
 	qp->r_nak_state = IB_NAK_REMOTE_OPERATIONAL_ERROR;
 	qp->r_ack_psn = qp->r_psn;
 	/* Queue NAK for later */
-	if (list_empty(&qp->rspwait)) {
-		qp->r_flags |= HFI1_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
-		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
-	}
+	rc_defered_ack(rcd, qp);
 	return;
 
 nack_inv_unlck:
@@ -2363,11 +2386,7 @@ nack_inv:
 	qp->r_nak_state = IB_NAK_INVALID_REQUEST;
 	qp->r_ack_psn = qp->r_psn;
 	/* Queue NAK for later */
-	if (list_empty(&qp->rspwait)) {
-		qp->r_flags |= HFI1_R_RSP_NAK;
-		atomic_inc(&qp->refcount);
-		list_add_tail(&qp->rspwait, &rcd->qp_wait_list);
-	}
+	rc_defered_ack(rcd, qp);
 	return;
 
 nack_acc_unlck:
@@ -2391,19 +2410,19 @@ void hfi1_rc_hdrerr(
 	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
 	int diff;
 	u32 opcode;
-	u32 psn;
+	u32 psn, bth0;
 
 	/* Check for GRH */
 	ohdr = &hdr->u.oth;
 	if (has_grh)
 		ohdr = &hdr->u.l.oth;
 
-	opcode = be32_to_cpu(ohdr->bth[0]);
-	if (hfi1_ruc_check_hdr(ibp, hdr, has_grh, qp, opcode))
+	bth0 = be32_to_cpu(ohdr->bth[0]);
+	if (hfi1_ruc_check_hdr(ibp, hdr, has_grh, qp, bth0))
 		return;
 
 	psn = be32_to_cpu(ohdr->bth[2]);
-	opcode >>= 24;
+	opcode = (bth0 >> 24) & 0xff;
 
 	/* Only deal with RDMA Writes for now */
 	if (opcode < IB_OPCODE_RC_RDMA_READ_RESPONSE_FIRST) {
@@ -2421,13 +2440,7 @@ void hfi1_rc_hdrerr(
 			 * Otherwise, we end up
 			 * propagating congestion.
 			 */
-			if (list_empty(&qp->rspwait)) {
-				qp->r_flags |= HFI1_R_RSP_NAK;
-				atomic_inc(&qp->refcount);
-				list_add_tail(
-					&qp->rspwait,
-					&rcd->qp_wait_list);
-				}
+			rc_defered_ack(rcd, qp);
 		} /* Out of sequence NAK */
 	} /* QP Request NAKs */
 }
diff --git a/drivers/staging/rdma/hfi1/ruc.c b/drivers/staging/rdma/hfi1/ruc.c
index 49bc9fd7a51a..4a91975b68d7 100644
--- a/drivers/staging/rdma/hfi1/ruc.c
+++ b/drivers/staging/rdma/hfi1/ruc.c
@@ -241,26 +241,6 @@ bail:
 	return ret;
 }
 
-/*
- * Switch to alternate path.
- * The QP s_lock should be held and interrupts disabled.
- */
-void hfi1_migrate_qp(struct hfi1_qp *qp)
-{
-	struct ib_event ev;
-
-	qp->s_mig_state = IB_MIG_MIGRATED;
-	qp->remote_ah_attr = qp->alt_ah_attr;
-	qp->port_num = qp->alt_ah_attr.port_num;
-	qp->s_pkey_index = qp->s_alt_pkey_index;
-	qp->s_flags |= HFI1_S_AHG_CLEAR;
-
-	ev.device = qp->ibqp.device;
-	ev.element.qp = &qp->ibqp;
-	ev.event = IB_EVENT_PATH_MIG;
-	qp->ibqp.event_handler(&ev, qp->ibqp.qp_context);
-}
-
 static __be64 get_sguid(struct hfi1_ibport *ibp, unsigned index)
 {
 	if (!index) {
@@ -308,11 +288,12 @@ int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct hfi1_ib_header *hdr,
 		}
 		if (unlikely(rcv_pkey_check(ppd_from_ibp(ibp), (u16)bth0,
 					    sc5, be16_to_cpu(hdr->lrh[3])))) {
-			hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_PKEY,
+			hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_P_KEY,
 				       (u16)bth0,
 				       (be16_to_cpu(hdr->lrh[0]) >> 4) & 0xF,
 				       0, qp->ibqp.qp_num,
-				       hdr->lrh[3], hdr->lrh[1]);
+				       be16_to_cpu(hdr->lrh[3]),
+				       be16_to_cpu(hdr->lrh[1]));
 			goto err;
 		}
 		/* Validate the SLID. See Ch. 9.6.1.5 and 17.2.8 */
@@ -340,11 +321,12 @@ int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct hfi1_ib_header *hdr,
 		}
 		if (unlikely(rcv_pkey_check(ppd_from_ibp(ibp), (u16)bth0,
 					    sc5, be16_to_cpu(hdr->lrh[3])))) {
-			hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_PKEY,
+			hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_P_KEY,
 				       (u16)bth0,
 				       (be16_to_cpu(hdr->lrh[0]) >> 4) & 0xF,
 				       0, qp->ibqp.qp_num,
-				       hdr->lrh[3], hdr->lrh[1]);
+				       be16_to_cpu(hdr->lrh[3]),
+				       be16_to_cpu(hdr->lrh[1]));
 			goto err;
 		}
 		/* Validate the SLID. See Ch. 9.6.1.5 */
@@ -714,11 +696,8 @@ static inline void build_ahg(struct hfi1_qp *qp, u32 npsn)
 		clear_ahg(qp);
 	if (!(qp->s_flags & HFI1_S_AHG_VALID)) {
 		/* first middle that needs copy  */
-		if (qp->s_ahgidx < 0) {
-			if (!qp->s_sde)
-				qp->s_sde = qp_to_sdma_engine(qp, qp->s_sc);
+		if (qp->s_ahgidx < 0)
 			qp->s_ahgidx = sdma_ahg_alloc(qp->s_sde);
-		}
 		if (qp->s_ahgidx >= 0) {
 			qp->s_ahgpsn = npsn;
 			qp->s_hdr->tx_flags |= SDMA_TXREQ_F_AHG_COPY;
@@ -761,7 +740,6 @@ void hfi1_make_ruc_header(struct hfi1_qp *qp, struct hfi1_other_headers *ohdr,
 	u16 lrh0;
 	u32 nwords;
 	u32 extra_bytes;
-	u8 sc5;
 	u32 bth1;
 
 	/* Construct the header. */
@@ -775,9 +753,7 @@ void hfi1_make_ruc_header(struct hfi1_qp *qp, struct hfi1_other_headers *ohdr,
 		lrh0 = HFI1_LRH_GRH;
 		middle = 0;
 	}
-	sc5 = ibp->sl_to_sc[qp->remote_ah_attr.sl];
-	lrh0 |= (sc5 & 0xf) << 12 | (qp->remote_ah_attr.sl & 0xf) << 4;
-	qp->s_sc = sc5;
+	lrh0 |= (qp->s_sc & 0xf) << 12 | (qp->remote_ah_attr.sl & 0xf) << 4;
 	/*
 	 * reset s_hdr/AHG fields
 	 *
@@ -835,16 +811,20 @@ void hfi1_do_send(struct work_struct *work)
 {
 	struct iowait *wait = container_of(work, struct iowait, iowork);
 	struct hfi1_qp *qp = container_of(wait, struct hfi1_qp, s_iowait);
-	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct hfi1_pkt_state ps;
 	int (*make_req)(struct hfi1_qp *qp);
 	unsigned long flags;
 	unsigned long timeout;
 
+	ps.dev = to_idev(qp->ibqp.device);
+	ps.ibp = to_iport(qp->ibqp.device, qp->port_num);
+	ps.ppd = ppd_from_ibp(ps.ibp);
+
 	if ((qp->ibqp.qp_type == IB_QPT_RC ||
 	     qp->ibqp.qp_type == IB_QPT_UC) &&
 	    !loopback &&
-	    (qp->remote_ah_attr.dlid & ~((1 << ppd->lmc) - 1)) == ppd->lid) {
+	    (qp->remote_ah_attr.dlid & ~((1 << ps.ppd->lmc) - 1)) ==
+	    ps.ppd->lid) {
 		ruc_loopback(qp);
 		return;
 	}
@@ -876,8 +856,7 @@ void hfi1_do_send(struct work_struct *work)
 			 * If the packet cannot be sent now, return and
 			 * the send tasklet will be woken up later.
 			 */
-			if (hfi1_verbs_send(qp, qp->s_hdr, qp->s_hdrwords,
-					    qp->s_cur_sge, qp->s_cur_size))
+			if (hfi1_verbs_send(qp, &ps))
 				break;
 			/* Record that s_hdr is empty. */
 			qp->s_hdrwords = 0;
@@ -886,7 +865,7 @@ void hfi1_do_send(struct work_struct *work)
 		/* allow other tasks to run */
 		if (unlikely(time_after(jiffies, timeout))) {
 			cond_resched();
-			ppd->dd->verbs_dev.n_send_schedule++;
+			ps.ppd->dd->verbs_dev.n_send_schedule++;
 			timeout = jiffies + SEND_RESCHED_TIMEOUT;
 		}
 	} while (make_req(qp));
diff --git a/drivers/staging/rdma/hfi1/sdma.c b/drivers/staging/rdma/hfi1/sdma.c
index 2a1da2189900..9a15f1f32b45 100644
--- a/drivers/staging/rdma/hfi1/sdma.c
+++ b/drivers/staging/rdma/hfi1/sdma.c
@@ -236,7 +236,6 @@ static void sdma_hw_clean_up_task(unsigned long);
 static void sdma_put(struct sdma_state *);
 static void sdma_set_state(struct sdma_engine *, enum sdma_states);
 static void sdma_start_hw_clean_up(struct sdma_engine *);
-static void sdma_start_sw_clean_up(struct sdma_engine *);
 static void sdma_sw_clean_up_task(unsigned long);
 static void sdma_sendctrl(struct sdma_engine *, unsigned);
 static void init_sdma_regs(struct sdma_engine *, u32, uint);
@@ -470,12 +469,6 @@ static void sdma_err_halt_wait(struct work_struct *work)
 	sdma_process_event(sde, sdma_event_e15_hw_halt_done);
 }
 
-static void sdma_start_err_halt_wait(struct sdma_engine *sde)
-{
-	schedule_work(&sde->err_halt_worker);
-}
-
-
 static void sdma_err_progress_check_schedule(struct sdma_engine *sde)
 {
 	if (!is_bx(sde->dd) && HFI1_CAP_IS_KSET(SDMA_AHG)) {
@@ -682,11 +675,6 @@ static void sdma_start_hw_clean_up(struct sdma_engine *sde)
 	tasklet_hi_schedule(&sde->sdma_hw_clean_up_task);
 }
 
-static void sdma_start_sw_clean_up(struct sdma_engine *sde)
-{
-	tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
-}
-
 static void sdma_set_state(struct sdma_engine *sde,
 	enum sdma_states next_state)
 {
@@ -777,19 +765,27 @@ struct sdma_engine *sdma_select_engine_vl(
 	struct sdma_map_elem *e;
 	struct sdma_engine *rval;
 
-	if (WARN_ON(vl > 8))
-		return NULL;
+	/* NOTE This should only happen if SC->VL changed after the initial
+	 *      checks on the QP/AH
+	 *      Default will return engine 0 below
+	 */
+	if (vl >= num_vls) {
+		rval = NULL;
+		goto done;
+	}
 
 	rcu_read_lock();
 	m = rcu_dereference(dd->sdma_map);
 	if (unlikely(!m)) {
 		rcu_read_unlock();
-		return NULL;
+		return &dd->per_sdma[0];
 	}
 	e = m->map[vl & m->mask];
 	rval = e->sde[selector & e->mask];
 	rcu_read_unlock();
 
+done:
+	rval =  !rval ? &dd->per_sdma[0] : rval;
 	trace_hfi1_sdma_engine_select(dd, selector, vl, rval->this_idx);
 	return rval;
 }
@@ -1874,7 +1870,7 @@ static void dump_sdma_state(struct sdma_engine *sde)
 }
 
 #define SDE_FMT \
-	"SDE %u STE %s C 0x%llx S 0x%016llx E 0x%llx T(HW) 0x%llx T(SW) 0x%x H(HW) 0x%llx H(SW) 0x%x H(D) 0x%llx DM 0x%llx GL 0x%llx R 0x%llx LIS 0x%llx AHGI 0x%llx TXT %u TXH %u DT %u DH %u FLNE %d DQF %u SLC 0x%llx\n"
+	"SDE %u CPU %d STE %s C 0x%llx S 0x%016llx E 0x%llx T(HW) 0x%llx T(SW) 0x%x H(HW) 0x%llx H(SW) 0x%x H(D) 0x%llx DM 0x%llx GL 0x%llx R 0x%llx LIS 0x%llx AHGI 0x%llx TXT %u TXH %u DT %u DH %u FLNE %d DQF %u SLC 0x%llx\n"
 /**
  * sdma_seqfile_dump_sde() - debugfs dump of sde
  * @s: seq file
@@ -1894,6 +1890,7 @@ void sdma_seqfile_dump_sde(struct seq_file *s, struct sdma_engine *sde)
 	head = sde->descq_head & sde->sdma_mask;
 	tail = ACCESS_ONCE(sde->descq_tail) & sde->sdma_mask;
 	seq_printf(s, SDE_FMT, sde->this_idx,
+		sde->cpu,
 		sdma_state_name(sde->state.current_state),
 		(unsigned long long)read_sde_csr(sde, SD(CTRL)),
 		(unsigned long long)read_sde_csr(sde, SD(STATUS)),
@@ -2308,7 +2305,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		case sdma_event_e50_hw_cleaned:
 			break;
 		case sdma_event_e60_hw_halted:
-			sdma_start_err_halt_wait(sde);
+			schedule_work(&sde->err_halt_worker);
 			break;
 		case sdma_event_e70_go_idle:
 			ss->go_s99_running = 0;
@@ -2389,7 +2386,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 			break;
 		case sdma_event_e60_hw_halted:
 			sdma_set_state(sde, sdma_state_s50_hw_halt_wait);
-			sdma_start_err_halt_wait(sde);
+			schedule_work(&sde->err_halt_worker);
 			break;
 		case sdma_event_e70_go_idle:
 			break;
@@ -2452,7 +2449,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
@@ -2494,13 +2491,13 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
 		case sdma_event_e15_hw_halt_done:
 			sdma_set_state(sde, sdma_state_s30_sw_clean_up_wait);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e25_hw_clean_up_done:
 			break;
@@ -2512,7 +2509,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		case sdma_event_e50_hw_cleaned:
 			break;
 		case sdma_event_e60_hw_halted:
-			sdma_start_err_halt_wait(sde);
+			schedule_work(&sde->err_halt_worker);
 			break;
 		case sdma_event_e70_go_idle:
 			ss->go_s99_running = 0;
@@ -2535,13 +2532,13 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
 		case sdma_event_e15_hw_halt_done:
 			sdma_set_state(sde, sdma_state_s30_sw_clean_up_wait);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e25_hw_clean_up_done:
 			break;
@@ -2553,7 +2550,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		case sdma_event_e50_hw_cleaned:
 			break;
 		case sdma_event_e60_hw_halted:
-			sdma_start_err_halt_wait(sde);
+			schedule_work(&sde->err_halt_worker);
 			break;
 		case sdma_event_e70_go_idle:
 			ss->go_s99_running = 0;
@@ -2575,7 +2572,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
@@ -2599,7 +2596,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 			break;
 		case sdma_event_e81_hw_frozen:
 			sdma_set_state(sde, sdma_state_s82_freeze_sw_clean);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e82_hw_unfreeze:
 			break;
@@ -2614,7 +2611,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
@@ -2658,7 +2655,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 		switch (event) {
 		case sdma_event_e00_go_hw_down:
 			sdma_set_state(sde, sdma_state_s00_hw_down);
-			sdma_start_sw_clean_up(sde);
+			tasklet_hi_schedule(&sde->sdma_sw_clean_up_task);
 			break;
 		case sdma_event_e10_go_hw_start:
 			break;
@@ -2681,7 +2678,7 @@ static void __sdma_process_event(struct sdma_engine *sde,
 			* progress check
 			*/
 			sdma_set_state(sde, sdma_state_s50_hw_halt_wait);
-			sdma_start_err_halt_wait(sde);
+			schedule_work(&sde->err_halt_worker);
 			break;
 		case sdma_event_e70_go_idle:
 			sdma_set_state(sde, sdma_state_s60_idle_halt_wait);
@@ -2734,22 +2731,21 @@ static int _extend_sdma_tx_descs(struct hfi1_devdata *dd, struct sdma_txreq *tx)
 			tx->coalesce_buf = kmalloc(tx->tlen + sizeof(u32),
 						   GFP_ATOMIC);
 			if (!tx->coalesce_buf)
-				return -ENOMEM;
-
+				goto enomem;
 			tx->coalesce_idx = 0;
 		}
 		return 0;
 	}
 
 	if (unlikely(tx->num_desc == MAX_DESC))
-		return -ENOMEM;
+		goto enomem;
 
 	tx->descp = kmalloc_array(
 			MAX_DESC,
 			sizeof(struct sdma_desc),
 			GFP_ATOMIC);
 	if (!tx->descp)
-		return -ENOMEM;
+		goto enomem;
 
 	/* reserve last descriptor for coalescing */
 	tx->desc_limit = MAX_DESC - 1;
@@ -2757,6 +2753,9 @@ static int _extend_sdma_tx_descs(struct hfi1_devdata *dd, struct sdma_txreq *tx)
 	for (i = 0; i < tx->num_desc; i++)
 		tx->descp[i] = tx->descs[i];
 	return 0;
+enomem:
+	sdma_txclean(dd, tx);
+	return -ENOMEM;
 }
 
 /*
diff --git a/drivers/staging/rdma/hfi1/sdma.h b/drivers/staging/rdma/hfi1/sdma.h
index cc22d2ee2054..da89e6458162 100644
--- a/drivers/staging/rdma/hfi1/sdma.h
+++ b/drivers/staging/rdma/hfi1/sdma.h
@@ -410,8 +410,6 @@ struct sdma_engine {
 	u64 idle_mask;
 	u64 progress_mask;
 	/* private: */
-	struct workqueue_struct *wq;
-	/* private: */
 	volatile __le64      *head_dma; /* DMA'ed by chip */
 	/* private: */
 	dma_addr_t            head_phys;
@@ -426,6 +424,8 @@ struct sdma_engine {
 	u32 sdma_mask;
 	/* private */
 	struct sdma_state state;
+	/* private */
+	int cpu;
 	/* private: */
 	u8 sdma_shift;
 	/* private: */
@@ -774,10 +774,13 @@ static inline int _sdma_txadd_daddr(
 	tx->tlen -= len;
 	/* special cases for last */
 	if (!tx->tlen) {
-		if (tx->packet_len & (sizeof(u32) - 1))
+		if (tx->packet_len & (sizeof(u32) - 1)) {
 			rval = _pad_sdma_tx_descs(dd, tx);
-		else
+			if (rval)
+				return rval;
+		} else {
 			_sdma_close_tx(dd, tx);
+		}
 	}
 	tx->num_desc++;
 	return rval;
@@ -990,7 +993,9 @@ static inline void sdma_iowait_schedule(
 	struct sdma_engine *sde,
 	struct iowait *wait)
 {
-	iowait_schedule(wait, sde->wq);
+	struct hfi1_pportdata *ppd = sde->dd->pport;
+
+	iowait_schedule(wait, ppd->hfi1_wq, sde->cpu);
 }
 
 /* for use by interrupt handling */
diff --git a/drivers/staging/rdma/hfi1/trace.c b/drivers/staging/rdma/hfi1/trace.c
index f55b75194847..10122e84cb2f 100644
--- a/drivers/staging/rdma/hfi1/trace.c
+++ b/drivers/staging/rdma/hfi1/trace.c
@@ -67,7 +67,7 @@ u8 ibhdr_exhdr_len(struct hfi1_ib_header *hdr)
 
 #define IMM_PRN  "imm %d"
 #define RETH_PRN "reth vaddr 0x%.16llx rkey 0x%.8x dlen 0x%.8x"
-#define AETH_PRN "aeth syn 0x%.2x msn 0x%.8x"
+#define AETH_PRN "aeth syn 0x%.2x %s msn 0x%.8x"
 #define DETH_PRN "deth qkey 0x%.8x sqpn 0x%.6x"
 #define ATOMICACKETH_PRN "origdata %lld"
 #define ATOMICETH_PRN "vaddr 0x%llx rkey 0x%.8x sdata %lld cdata %lld"
@@ -79,6 +79,19 @@ 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) {
+	case 0:
+		return "ACK";
+	case 1:
+		return "RNRNAK";
+	case 3:
+		return "NAK";
+	}
+	return "";
+}
+
 const char *parse_everbs_hdrs(
 	struct trace_seq *p,
 	u8 opcode,
@@ -124,16 +137,18 @@ const char *parse_everbs_hdrs(
 	case OP(RC, RDMA_READ_RESPONSE_LAST):
 	case OP(RC, RDMA_READ_RESPONSE_ONLY):
 	case OP(RC, ACKNOWLEDGE):
-		trace_seq_printf(p, AETH_PRN,
-			be32_to_cpu(eh->aeth) >> 24,
-			be32_to_cpu(eh->aeth) & HFI1_MSN_MASK);
+		trace_seq_printf(p, AETH_PRN, be32_to_cpu(eh->aeth) >> 24,
+				 parse_syndrome(be32_to_cpu(eh->aeth) >> 24),
+				 be32_to_cpu(eh->aeth) & HFI1_MSN_MASK);
 		break;
 	/* aeth + atomicacketh */
 	case OP(RC, ATOMIC_ACKNOWLEDGE):
 		trace_seq_printf(p, AETH_PRN " " ATOMICACKETH_PRN,
-			(be32_to_cpu(eh->at.aeth) >> 24) & 0xff,
-			be32_to_cpu(eh->at.aeth) & HFI1_MSN_MASK,
-			(unsigned long long)ib_u64_get(eh->at.atomic_ack_eth));
+				 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));
 		break;
 	/* atomiceth */
 	case OP(RC, COMPARE_SWAP):
diff --git a/drivers/staging/rdma/hfi1/trace.h b/drivers/staging/rdma/hfi1/trace.h
index 57430295c404..86c12ebfd4f0 100644
--- a/drivers/staging/rdma/hfi1/trace.h
+++ b/drivers/staging/rdma/hfi1/trace.h
@@ -417,7 +417,8 @@ __print_symbolic(opcode,                                   \
 	ib_opcode_name(UC_RDMA_WRITE_ONLY),                \
 	ib_opcode_name(UC_RDMA_WRITE_ONLY_WITH_IMMEDIATE), \
 	ib_opcode_name(UD_SEND_ONLY),                      \
-	ib_opcode_name(UD_SEND_ONLY_WITH_IMMEDIATE))
+	ib_opcode_name(UD_SEND_ONLY_WITH_IMMEDIATE),       \
+	ib_opcode_name(CNP))
 
 
 #define LRH_PRN "vl %d lver %d sl %d lnh %d,%s dlid %.4x len %d slid %.4x"
diff --git a/drivers/staging/rdma/hfi1/uc.c b/drivers/staging/rdma/hfi1/uc.c
index 6095039c4485..4f2a7889a852 100644
--- a/drivers/staging/rdma/hfi1/uc.c
+++ b/drivers/staging/rdma/hfi1/uc.c
@@ -268,7 +268,7 @@ void hfi1_uc_rcv(struct hfi1_packet *packet)
 	u32 tlen = packet->tlen;
 	struct hfi1_qp *qp = packet->qp;
 	struct hfi1_other_headers *ohdr = packet->ohdr;
-	u32 opcode;
+	u32 bth0, opcode;
 	u32 hdrsize = packet->hlen;
 	u32 psn;
 	u32 pad;
@@ -278,10 +278,9 @@ void hfi1_uc_rcv(struct hfi1_packet *packet)
 	int has_grh = rcv_flags & HFI1_HAS_GRH;
 	int ret;
 	u32 bth1;
-	struct ib_grh *grh = NULL;
 
-	opcode = be32_to_cpu(ohdr->bth[0]);
-	if (hfi1_ruc_check_hdr(ibp, hdr, has_grh, qp, opcode))
+	bth0 = be32_to_cpu(ohdr->bth[0]);
+	if (hfi1_ruc_check_hdr(ibp, hdr, has_grh, qp, bth0))
 		return;
 
 	bth1 = be32_to_cpu(ohdr->bth[1]);
@@ -303,6 +302,7 @@ void hfi1_uc_rcv(struct hfi1_packet *packet)
 		}
 
 		if (bth1 & HFI1_FECN_SMASK) {
+			struct ib_grh *grh = NULL;
 			u16 pkey = (u16)be32_to_cpu(ohdr->bth[0]);
 			u16 slid = be16_to_cpu(hdr->lrh[3]);
 			u16 dlid = be16_to_cpu(hdr->lrh[1]);
@@ -310,13 +310,16 @@ void hfi1_uc_rcv(struct hfi1_packet *packet)
 			u8 sc5;
 
 			sc5 = ibp->sl_to_sc[qp->remote_ah_attr.sl];
+			if (has_grh)
+				grh = &hdr->u.l.grh;
 
-			return_cnp(ibp, qp, src_qp, pkey, dlid, slid, sc5, grh);
+			return_cnp(ibp, qp, src_qp, pkey, dlid, slid, sc5,
+				   grh);
 		}
 	}
 
 	psn = be32_to_cpu(ohdr->bth[2]);
-	opcode >>= 24;
+	opcode = (bth0 >> 24) & 0xff;
 
 	/* Compare the PSN verses the expected PSN. */
 	if (unlikely(cmp_psn(psn, qp->r_psn) != 0)) {
diff --git a/drivers/staging/rdma/hfi1/ud.c b/drivers/staging/rdma/hfi1/ud.c
index 5a9c784bec04..bd1b402c1e14 100644
--- a/drivers/staging/rdma/hfi1/ud.c
+++ b/drivers/staging/rdma/hfi1/ud.c
@@ -111,11 +111,10 @@ static void ud_loopback(struct hfi1_qp *sqp, struct hfi1_swqe *swqe)
 				   ((1 << ppd->lmc) - 1));
 		if (unlikely(ingress_pkey_check(ppd, pkey, sc5,
 						qp->s_pkey_index, slid))) {
-			hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_PKEY, pkey,
+			hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_P_KEY, pkey,
 				       ah_attr->sl,
 				       sqp->ibqp.qp_num, qp->ibqp.qp_num,
-				       cpu_to_be16(slid),
-				       cpu_to_be16(ah_attr->dlid));
+				       slid, ah_attr->dlid);
 			goto drop;
 		}
 	}
@@ -135,11 +134,11 @@ static void ud_loopback(struct hfi1_qp *sqp, struct hfi1_swqe *swqe)
 
 			lid = ppd->lid | (ah_attr->src_path_bits &
 					  ((1 << ppd->lmc) - 1));
-			hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_QKEY, qkey,
+			hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_Q_KEY, qkey,
 				       ah_attr->sl,
 				       sqp->ibqp.qp_num, qp->ibqp.qp_num,
-				       cpu_to_be16(lid),
-				       cpu_to_be16(ah_attr->dlid));
+				       lid,
+				       ah_attr->dlid);
 			goto drop;
 		}
 	}
@@ -383,6 +382,7 @@ int hfi1_make_ud_req(struct hfi1_qp *qp)
 		lrh0 |= (sc5 & 0xf) << 12;
 		qp->s_sc = sc5;
 	}
+	qp->s_sde = qp_to_sdma_engine(qp, qp->s_sc);
 	qp->s_hdr->ibh.lrh[0] = cpu_to_be16(lrh0);
 	qp->s_hdr->ibh.lrh[1] = cpu_to_be16(ah_attr->dlid);  /* DEST LID */
 	qp->s_hdr->ibh.lrh[2] =
@@ -736,12 +736,13 @@ void hfi1_ud_rcv(struct hfi1_packet *packet)
 				 * for invalid pkeys is optional according to
 				 * IB spec (release 1.3, section 10.9.4)
 				 */
-				hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_PKEY,
+				hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_P_KEY,
 					       pkey,
 					       (be16_to_cpu(hdr->lrh[0]) >> 4) &
 						0xF,
 					       src_qp, qp->ibqp.qp_num,
-					       hdr->lrh[3], hdr->lrh[1]);
+					       be16_to_cpu(hdr->lrh[3]),
+					       be16_to_cpu(hdr->lrh[1]));
 				return;
 			}
 		} else {
@@ -752,10 +753,11 @@ void hfi1_ud_rcv(struct hfi1_packet *packet)
 
 		}
 		if (unlikely(qkey != qp->qkey)) {
-			hfi1_bad_pqkey(ibp, IB_NOTICE_TRAP_BAD_QKEY, qkey,
+			hfi1_bad_pqkey(ibp, OPA_TRAP_BAD_Q_KEY, qkey,
 				       (be16_to_cpu(hdr->lrh[0]) >> 4) & 0xF,
 				       src_qp, qp->ibqp.qp_num,
-				       hdr->lrh[3], hdr->lrh[1]);
+				       be16_to_cpu(hdr->lrh[3]),
+				       be16_to_cpu(hdr->lrh[1]));
 			return;
 		}
 		/* Drop invalid MAD packets (see 13.5.3.1). */
diff --git a/drivers/staging/rdma/hfi1/user_exp_rcv.h b/drivers/staging/rdma/hfi1/user_exp_rcv.h
new file mode 100644
index 000000000000..4f4876e1d353
--- /dev/null
+++ b/drivers/staging/rdma/hfi1/user_exp_rcv.h
@@ -0,0 +1,74 @@
+#ifndef _HFI1_USER_EXP_RCV_H
+#define _HFI1_USER_EXP_RCV_H
+/*
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2015 Intel Corporation.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  - Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  - Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  - Neither the name of Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#define EXP_TID_TIDLEN_MASK   0x7FFULL
+#define EXP_TID_TIDLEN_SHIFT  0
+#define EXP_TID_TIDCTRL_MASK  0x3ULL
+#define EXP_TID_TIDCTRL_SHIFT 20
+#define EXP_TID_TIDIDX_MASK   0x3FFULL
+#define EXP_TID_TIDIDX_SHIFT  22
+#define EXP_TID_GET(tid, field)	\
+	(((tid) >> EXP_TID_TID##field##_SHIFT) & EXP_TID_TID##field##_MASK)
+
+#define EXP_TID_SET(field, value)			\
+	(((value) & EXP_TID_TID##field##_MASK) <<	\
+	 EXP_TID_TID##field##_SHIFT)
+#define EXP_TID_CLEAR(tid, field) ({					\
+		(tid) &= ~(EXP_TID_TID##field##_MASK <<			\
+			   EXP_TID_TID##field##_SHIFT);			\
+		})
+#define EXP_TID_RESET(tid, field, value) do {				\
+		EXP_TID_CLEAR(tid, field);				\
+		(tid) |= EXP_TID_SET(field, (value));			\
+	} while (0)
+
+#endif /* _HFI1_USER_EXP_RCV_H */
diff --git a/drivers/staging/rdma/hfi1/user_pages.c b/drivers/staging/rdma/hfi1/user_pages.c
index 9071afbd7bf4..692de658f0dc 100644
--- a/drivers/staging/rdma/hfi1/user_pages.c
+++ b/drivers/staging/rdma/hfi1/user_pages.c
@@ -49,59 +49,11 @@
  */
 
 #include <linux/mm.h>
+#include <linux/sched.h>
 #include <linux/device.h>
 
 #include "hfi.h"
 
-static void __hfi1_release_user_pages(struct page **p, size_t num_pages,
-				      int dirty)
-{
-	size_t i;
-
-	for (i = 0; i < num_pages; i++) {
-		if (dirty)
-			set_page_dirty_lock(p[i]);
-		put_page(p[i]);
-	}
-}
-
-/*
- * Call with current->mm->mmap_sem held.
- */
-static int __hfi1_get_user_pages(unsigned long start_page, size_t num_pages,
-				 struct page **p)
-{
-	unsigned long lock_limit;
-	size_t got;
-	int ret;
-
-	lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
-
-	if (num_pages > lock_limit && !capable(CAP_IPC_LOCK)) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	for (got = 0; got < num_pages; got += ret) {
-		ret = get_user_pages(current, current->mm,
-				     start_page + got * PAGE_SIZE,
-				     num_pages - got, 1, 1,
-				     p + got, NULL);
-		if (ret < 0)
-			goto bail_release;
-	}
-
-	current->mm->pinned_vm += num_pages;
-
-	ret = 0;
-	goto bail;
-
-bail_release:
-	__hfi1_release_user_pages(p, got, 0);
-bail:
-	return ret;
-}
-
 /**
  * hfi1_map_page - a safety wrapper around pci_map_page()
  *
@@ -116,41 +68,44 @@ dma_addr_t hfi1_map_page(struct pci_dev *hwdev, struct page *page,
 	return phys;
 }
 
-/**
- * hfi1_get_user_pages - lock user pages into memory
- * @start_page: the start page
- * @num_pages: the number of pages
- * @p: the output page structures
- *
- * This function takes a given start page (page aligned user virtual
- * address) and pins it and the following specified number of pages.  For
- * now, num_pages is always 1, but that will probably change at some point
- * (because caller is doing expected sends on a single virtually contiguous
- * buffer, so we can do all pages at once).
- */
-int hfi1_get_user_pages(unsigned long start_page, size_t num_pages,
-			struct page **p)
+int hfi1_acquire_user_pages(unsigned long vaddr, size_t npages, bool writable,
+			    struct page **pages)
 {
+	unsigned long pinned, lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
+	bool can_lock = capable(CAP_IPC_LOCK);
 	int ret;
 
-	down_write(&current->mm->mmap_sem);
+	down_read(&current->mm->mmap_sem);
+	pinned = current->mm->pinned_vm;
+	up_read(&current->mm->mmap_sem);
 
-	ret = __hfi1_get_user_pages(start_page, num_pages, p);
+	if (pinned + npages > lock_limit && !can_lock)
+		return -ENOMEM;
 
+	ret = get_user_pages_fast(vaddr, npages, writable, pages);
+	if (ret < 0)
+		return ret;
+
+	down_write(&current->mm->mmap_sem);
+	current->mm->pinned_vm += ret;
 	up_write(&current->mm->mmap_sem);
 
 	return ret;
 }
 
-void hfi1_release_user_pages(struct page **p, size_t num_pages)
+void hfi1_release_user_pages(struct page **p, size_t npages, bool dirty)
 {
-	if (current->mm) /* during close after signal, mm can be NULL */
-		down_write(&current->mm->mmap_sem);
+	size_t i;
 
-	__hfi1_release_user_pages(p, num_pages, 1);
+	for (i = 0; i < npages; i++) {
+		if (dirty)
+			set_page_dirty_lock(p[i]);
+		put_page(p[i]);
+	}
 
-	if (current->mm) {
-		current->mm->pinned_vm -= num_pages;
+	if (current->mm) { /* during close after signal, mm can be NULL */
+		down_write(&current->mm->mmap_sem);
+		current->mm->pinned_vm -= npages;
 		up_write(&current->mm->mmap_sem);
 	}
 }
diff --git a/drivers/staging/rdma/hfi1/user_sdma.c b/drivers/staging/rdma/hfi1/user_sdma.c
index 36c838dcf023..d3de771a0770 100644
--- a/drivers/staging/rdma/hfi1/user_sdma.c
+++ b/drivers/staging/rdma/hfi1/user_sdma.c
@@ -146,8 +146,8 @@ MODULE_PARM_DESC(sdma_comp_size, "Size of User SDMA completion ring. Default: 12
 #define KDETH_OM_MAX_SIZE  (1 << ((KDETH_OM_LARGE / KDETH_OM_SMALL) + 1))
 
 /* Last packet in the request */
-#define TXREQ_FLAGS_REQ_LAST_PKT   (1 << 0)
-#define TXREQ_FLAGS_IOVEC_LAST_PKT (1 << 0)
+#define TXREQ_FLAGS_REQ_LAST_PKT BIT(0)
+#define TXREQ_FLAGS_IOVEC_LAST_PKT BIT(0)
 
 #define SDMA_REQ_IN_USE     0
 #define SDMA_REQ_FOR_THREAD 1
@@ -156,9 +156,9 @@ MODULE_PARM_DESC(sdma_comp_size, "Size of User SDMA completion ring. Default: 12
 #define SDMA_REQ_HAS_ERROR  4
 #define SDMA_REQ_DONE_ERROR 5
 
-#define SDMA_PKT_Q_INACTIVE (1 << 0)
-#define SDMA_PKT_Q_ACTIVE   (1 << 1)
-#define SDMA_PKT_Q_DEFERRED (1 << 2)
+#define SDMA_PKT_Q_INACTIVE BIT(0)
+#define SDMA_PKT_Q_ACTIVE   BIT(1)
+#define SDMA_PKT_Q_DEFERRED BIT(2)
 
 /*
  * Maximum retry attempts to submit a TX request
@@ -214,12 +214,6 @@ struct user_sdma_request {
 	 */
 	u8 omfactor;
 	/*
-	 * pointer to the user's task_struct. We are going to
-	 * get a reference to it so we can process io vectors
-	 * at a later time.
-	 */
-	struct task_struct *user_proc;
-	/*
 	 * pointer to the user's mm_struct. We are going to
 	 * get a reference to it so it doesn't get freed
 	 * since we might not be in process context when we
@@ -245,9 +239,13 @@ struct user_sdma_request {
 	u16 tididx;
 	u32 sent;
 	u64 seqnum;
-	spinlock_t list_lock;
 	struct list_head txps;
+	spinlock_t txcmp_lock;  /* protect txcmp list */
+	struct list_head txcmp;
 	unsigned long flags;
+	/* status of the last txreq completed */
+	int status;
+	struct work_struct worker;
 };
 
 /*
@@ -260,6 +258,7 @@ struct user_sdma_txreq {
 	/* Packet header for the txreq */
 	struct hfi1_pkt_header hdr;
 	struct sdma_txreq txreq;
+	struct list_head list;
 	struct user_sdma_request *req;
 	struct {
 		struct user_sdma_iovec *vec;
@@ -282,10 +281,12 @@ struct user_sdma_txreq {
 static int user_sdma_send_pkts(struct user_sdma_request *, unsigned);
 static int num_user_pages(const struct iovec *);
 static void user_sdma_txreq_cb(struct sdma_txreq *, int, int);
+static void user_sdma_delayed_completion(struct work_struct *);
 static void user_sdma_free_request(struct user_sdma_request *);
 static int pin_vector_pages(struct user_sdma_request *,
 			    struct user_sdma_iovec *);
-static void unpin_vector_pages(struct user_sdma_iovec *);
+static void unpin_vector_pages(struct user_sdma_request *,
+			       struct user_sdma_iovec *);
 static int check_header_template(struct user_sdma_request *,
 				 struct hfi1_pkt_header *, u32, u32);
 static int set_txreq_header(struct user_sdma_request *,
@@ -352,6 +353,7 @@ static void sdma_kmem_cache_ctor(void *obj)
 
 int hfi1_user_sdma_alloc_queues(struct hfi1_ctxtdata *uctxt, struct file *fp)
 {
+	struct hfi1_filedata *fd;
 	int ret = 0;
 	unsigned memsize;
 	char buf[64];
@@ -365,6 +367,8 @@ int hfi1_user_sdma_alloc_queues(struct hfi1_ctxtdata *uctxt, struct file *fp)
 		goto done;
 	}
 
+	fd = fp->private_data;
+
 	if (!hfi1_sdma_comp_ring_size) {
 		ret = -EINVAL;
 		goto done;
@@ -384,16 +388,17 @@ int hfi1_user_sdma_alloc_queues(struct hfi1_ctxtdata *uctxt, struct file *fp)
 	INIT_LIST_HEAD(&pq->list);
 	pq->dd = dd;
 	pq->ctxt = uctxt->ctxt;
-	pq->subctxt = subctxt_fp(fp);
+	pq->subctxt = fd->subctxt;
 	pq->n_max_reqs = hfi1_sdma_comp_ring_size;
 	pq->state = SDMA_PKT_Q_INACTIVE;
 	atomic_set(&pq->n_reqs, 0);
+	init_waitqueue_head(&pq->wait);
 
 	iowait_init(&pq->busy, 0, NULL, defer_packet_queue,
 		    activate_packet_queue);
 	pq->reqidx = 0;
 	snprintf(buf, 64, "txreq-kmem-cache-%u-%u-%u", dd->unit, uctxt->ctxt,
-		 subctxt_fp(fp));
+		 fd->subctxt);
 	pq->txreq_cache = kmem_cache_create(buf,
 			       sizeof(struct user_sdma_txreq),
 					    L1_CACHE_BYTES,
@@ -404,7 +409,7 @@ int hfi1_user_sdma_alloc_queues(struct hfi1_ctxtdata *uctxt, struct file *fp)
 			   uctxt->ctxt);
 		goto pq_txreq_nomem;
 	}
-	user_sdma_pkt_fp(fp) = pq;
+	fd->pq = pq;
 	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
 	if (!cq)
 		goto cq_nomem;
@@ -416,7 +421,7 @@ int hfi1_user_sdma_alloc_queues(struct hfi1_ctxtdata *uctxt, struct file *fp)
 		goto cq_comps_nomem;
 
 	cq->nentries = hfi1_sdma_comp_ring_size;
-	user_sdma_comp_fp(fp) = cq;
+	fd->cq = cq;
 
 	spin_lock_irqsave(&uctxt->sdma_qlock, flags);
 	list_add(&pq->list, &uctxt->sdma_queues);
@@ -431,7 +436,7 @@ pq_txreq_nomem:
 	kfree(pq->reqs);
 pq_reqs_nomem:
 	kfree(pq);
-	user_sdma_pkt_fp(fp) = NULL;
+	fd->pq = NULL;
 pq_nomem:
 	ret = -ENOMEM;
 done:
@@ -448,26 +453,16 @@ int hfi1_user_sdma_free_queues(struct hfi1_filedata *fd)
 		  uctxt->ctxt, fd->subctxt);
 	pq = fd->pq;
 	if (pq) {
-		u16 i, j;
-
 		spin_lock_irqsave(&uctxt->sdma_qlock, flags);
 		if (!list_empty(&pq->list))
 			list_del_init(&pq->list);
 		spin_unlock_irqrestore(&uctxt->sdma_qlock, flags);
 		iowait_sdma_drain(&pq->busy);
-		if (pq->reqs) {
-			for (i = 0, j = 0; i < atomic_read(&pq->n_reqs) &&
-				     j < pq->n_max_reqs; j++) {
-				struct user_sdma_request *req = &pq->reqs[j];
-
-				if (test_bit(SDMA_REQ_IN_USE, &req->flags)) {
-					set_comp_state(req, ERROR, -ECOMM);
-					user_sdma_free_request(req);
-					i++;
-				}
-			}
-			kfree(pq->reqs);
-		}
+		/* Wait until all requests have been freed. */
+		wait_event_interruptible(
+			pq->wait,
+			(ACCESS_ONCE(pq->state) == SDMA_PKT_Q_INACTIVE));
+		kfree(pq->reqs);
 		kmem_cache_destroy(pq->txreq_cache);
 		kfree(pq);
 		fd->pq = NULL;
@@ -485,9 +480,10 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 				   unsigned long dim, unsigned long *count)
 {
 	int ret = 0, i = 0, sent;
-	struct hfi1_ctxtdata *uctxt = ctxt_fp(fp);
-	struct hfi1_user_sdma_pkt_q *pq = user_sdma_pkt_fp(fp);
-	struct hfi1_user_sdma_comp_q *cq = user_sdma_comp_fp(fp);
+	struct hfi1_filedata *fd = fp->private_data;
+	struct hfi1_ctxtdata *uctxt = fd->uctxt;
+	struct hfi1_user_sdma_pkt_q *pq = fd->pq;
+	struct hfi1_user_sdma_comp_q *cq = fd->cq;
 	struct hfi1_devdata *dd = pq->dd;
 	unsigned long idx = 0;
 	u8 pcount = initial_pkt_count;
@@ -499,40 +495,36 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 		hfi1_cdbg(
 		   SDMA,
 		   "[%u:%u:%u] First vector not big enough for header %lu/%lu",
-		   dd->unit, uctxt->ctxt, subctxt_fp(fp),
+		   dd->unit, uctxt->ctxt, fd->subctxt,
 		   iovec[idx].iov_len, sizeof(info) + sizeof(req->hdr));
-		ret = -EINVAL;
-		goto done;
+		return -EINVAL;
 	}
 	ret = copy_from_user(&info, iovec[idx].iov_base, sizeof(info));
 	if (ret) {
 		hfi1_cdbg(SDMA, "[%u:%u:%u] Failed to copy info QW (%d)",
-			  dd->unit, uctxt->ctxt, subctxt_fp(fp), ret);
-		ret = -EFAULT;
-		goto done;
+			  dd->unit, uctxt->ctxt, fd->subctxt, ret);
+		return -EFAULT;
 	}
-	trace_hfi1_sdma_user_reqinfo(dd, uctxt->ctxt, subctxt_fp(fp),
+	trace_hfi1_sdma_user_reqinfo(dd, uctxt->ctxt, fd->subctxt,
 				     (u16 *)&info);
 	if (cq->comps[info.comp_idx].status == QUEUED) {
 		hfi1_cdbg(SDMA, "[%u:%u:%u] Entry %u is in QUEUED state",
-			  dd->unit, uctxt->ctxt, subctxt_fp(fp),
+			  dd->unit, uctxt->ctxt, fd->subctxt,
 			  info.comp_idx);
-		ret = -EBADSLT;
-		goto done;
+		return -EBADSLT;
 	}
 	if (!info.fragsize) {
 		hfi1_cdbg(SDMA,
 			  "[%u:%u:%u:%u] Request does not specify fragsize",
-			  dd->unit, uctxt->ctxt, subctxt_fp(fp), info.comp_idx);
-		ret = -EINVAL;
-		goto done;
+			  dd->unit, uctxt->ctxt, fd->subctxt, info.comp_idx);
+		return -EINVAL;
 	}
 	/*
 	 * We've done all the safety checks that we can up to this point,
 	 * "allocate" the request entry.
 	 */
 	hfi1_cdbg(SDMA, "[%u:%u:%u] Using req/comp entry %u\n", dd->unit,
-		  uctxt->ctxt, subctxt_fp(fp), info.comp_idx);
+		  uctxt->ctxt, fd->subctxt, info.comp_idx);
 	req = pq->reqs + info.comp_idx;
 	memset(req, 0, sizeof(*req));
 	/* Mark the request as IN_USE before we start filling it in. */
@@ -540,8 +532,12 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 	req->data_iovs = req_iovcnt(info.ctrl) - 1;
 	req->pq = pq;
 	req->cq = cq;
+	req->status = -1;
 	INIT_LIST_HEAD(&req->txps);
-	spin_lock_init(&req->list_lock);
+	INIT_LIST_HEAD(&req->txcmp);
+	INIT_WORK(&req->worker, user_sdma_delayed_completion);
+
+	spin_lock_init(&req->txcmp_lock);
 	memcpy(&req->info, &info, sizeof(info));
 
 	if (req_opcode(info.ctrl) == EXPECTED)
@@ -550,8 +546,7 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 	if (!info.npkts || req->data_iovs > MAX_VECTORS_PER_REQ) {
 		SDMA_DBG(req, "Too many vectors (%u/%u)", req->data_iovs,
 			 MAX_VECTORS_PER_REQ);
-		ret = -EINVAL;
-		goto done;
+		return -EINVAL;
 	}
 	/* Copy the header from the user buffer */
 	ret = copy_from_user(&req->hdr, iovec[idx].iov_base + sizeof(info),
@@ -659,7 +654,7 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 
 	/* Have to select the engine */
 	req->sde = sdma_select_engine_vl(dd,
-					 (u32)(uctxt->ctxt + subctxt_fp(fp)),
+					 (u32)(uctxt->ctxt + fd->subctxt),
 					 vl);
 	if (!req->sde || !sdma_running(req->sde)) {
 		ret = -ECOMM;
@@ -681,18 +676,16 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 	sent = user_sdma_send_pkts(req, pcount);
 	if (unlikely(sent < 0)) {
 		if (sent != -EBUSY) {
-			ret = sent;
-			goto send_err;
+			req->status = sent;
+			set_comp_state(req, ERROR, req->status);
+			return sent;
 		} else
 			sent = 0;
 	}
 	atomic_inc(&pq->n_reqs);
+	xchg(&pq->state, SDMA_PKT_Q_ACTIVE);
 
 	if (sent < req->info.npkts) {
-		/* Take the references to the user's task and mm_struct */
-		get_task_struct(current);
-		req->user_proc = current;
-
 		/*
 		 * This is a somewhat blocking send implementation.
 		 * The driver will block the caller until all packets of the
@@ -702,8 +695,10 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 		while (!test_bit(SDMA_REQ_SEND_DONE, &req->flags)) {
 			ret = user_sdma_send_pkts(req, pcount);
 			if (ret < 0) {
-				if (ret != -EBUSY)
-					goto send_err;
+				if (ret != -EBUSY) {
+					req->status = ret;
+					return ret;
+				}
 				wait_event_interruptible_timeout(
 					pq->busy.wait_dma,
 					(pq->state == SDMA_PKT_Q_ACTIVE),
@@ -713,14 +708,10 @@ int hfi1_user_sdma_process_request(struct file *fp, struct iovec *iovec,
 		}
 
 	}
-	ret = 0;
 	*count += idx;
-	goto done;
-send_err:
-	set_comp_state(req, ERROR, ret);
+	return 0;
 free_req:
 	user_sdma_free_request(req);
-done:
 	return ret;
 }
 
@@ -778,20 +769,24 @@ static int user_sdma_send_pkts(struct user_sdma_request *req, unsigned maxpkts)
 	struct hfi1_user_sdma_pkt_q *pq = NULL;
 	struct user_sdma_iovec *iovec = NULL;
 
-	if (!req->pq) {
-		ret = -EINVAL;
-		goto done;
-	}
+	if (!req->pq)
+		return -EINVAL;
 
 	pq = req->pq;
 
+	/* If tx completion has reported an error, we are done. */
+	if (test_bit(SDMA_REQ_HAS_ERROR, &req->flags)) {
+		set_bit(SDMA_REQ_DONE_ERROR, &req->flags);
+		return -EFAULT;
+	}
+
 	/*
 	 * Check if we might have sent the entire request already
 	 */
 	if (unlikely(req->seqnum == req->info.npkts)) {
 		if (!list_empty(&req->txps))
 			goto dosend;
-		goto done;
+		return ret;
 	}
 
 	if (!maxpkts || maxpkts > req->info.npkts - req->seqnum)
@@ -808,19 +803,18 @@ static int user_sdma_send_pkts(struct user_sdma_request *req, unsigned maxpkts)
 		 */
 		if (test_bit(SDMA_REQ_HAS_ERROR, &req->flags)) {
 			set_bit(SDMA_REQ_DONE_ERROR, &req->flags);
-			ret = -EFAULT;
-			goto done;
+			return -EFAULT;
 		}
 
 		tx = kmem_cache_alloc(pq->txreq_cache, GFP_KERNEL);
-		if (!tx) {
-			ret = -ENOMEM;
-			goto done;
-		}
+		if (!tx)
+			return -ENOMEM;
+
 		tx->flags = 0;
 		tx->req = req;
 		tx->busycount = 0;
 		tx->idx = -1;
+		INIT_LIST_HEAD(&tx->list);
 		memset(tx->iovecs, 0, sizeof(tx->iovecs));
 
 		if (req->seqnum == req->info.npkts - 1)
@@ -945,9 +939,8 @@ static int user_sdma_send_pkts(struct user_sdma_request *req, unsigned maxpkts)
 			if (ret) {
 				int i;
 
-				dd_dev_err(pq->dd,
-					   "SDMA txreq add page failed %d\n",
-					   ret);
+				SDMA_DBG(req, "SDMA txreq add page failed %d\n",
+					 ret);
 				/* Mark all assigned vectors as complete so they
 				 * are unpinned in the callback. */
 				for (i = tx->idx; i >= 0; i--) {
@@ -1017,12 +1010,12 @@ dosend:
 			if (test_bit(SDMA_REQ_HAVE_AHG, &req->flags))
 				sdma_ahg_free(req->sde, req->ahg_idx);
 		}
-	goto done;
+	return ret;
+
 free_txreq:
 	sdma_txclean(pq->dd, &tx->txreq);
 free_tx:
 	kmem_cache_free(pq->txreq_cache, tx);
-done:
 	return ret;
 }
 
@@ -1041,52 +1034,58 @@ static inline int num_user_pages(const struct iovec *iov)
 
 static int pin_vector_pages(struct user_sdma_request *req,
 			    struct user_sdma_iovec *iovec) {
-	int ret = 0;
-	unsigned pinned;
+	int pinned, npages;
 
-	iovec->npages = num_user_pages(&iovec->iov);
-	iovec->pages = kcalloc(iovec->npages, sizeof(*iovec->pages),
-			       GFP_KERNEL);
+	npages = num_user_pages(&iovec->iov);
+	iovec->pages = kcalloc(npages, sizeof(*iovec->pages), GFP_KERNEL);
 	if (!iovec->pages) {
 		SDMA_DBG(req, "Failed page array alloc");
-		ret = -ENOMEM;
-		goto done;
+		return -ENOMEM;
 	}
-	/* If called by the kernel thread, use the user's mm */
-	if (current->flags & PF_KTHREAD)
-		use_mm(req->user_proc->mm);
-	pinned = get_user_pages_fast(
-		(unsigned long)iovec->iov.iov_base,
-		iovec->npages, 0, iovec->pages);
-	/* If called by the kernel thread, unuse the user's mm */
-	if (current->flags & PF_KTHREAD)
-		unuse_mm(req->user_proc->mm);
-	if (pinned != iovec->npages) {
-		SDMA_DBG(req, "Failed to pin pages (%u/%u)", pinned,
-			 iovec->npages);
-		ret = -EFAULT;
-		goto pfree;
+
+	/*
+	 * Get a reference to the process's mm so we can use it when
+	 * unpinning the io vectors.
+	 */
+	req->pq->user_mm = get_task_mm(current);
+
+	pinned = hfi1_acquire_user_pages((unsigned long)iovec->iov.iov_base,
+					 npages, 0, iovec->pages);
+
+	if (pinned < 0)
+		return pinned;
+
+	iovec->npages = pinned;
+	if (pinned != npages) {
+		SDMA_DBG(req, "Failed to pin pages (%d/%u)", pinned, npages);
+		unpin_vector_pages(req, iovec);
+		return -EFAULT;
 	}
-	goto done;
-pfree:
-	unpin_vector_pages(iovec);
-done:
-	return ret;
+	return 0;
 }
 
-static void unpin_vector_pages(struct user_sdma_iovec *iovec)
+static void unpin_vector_pages(struct user_sdma_request *req,
+			       struct user_sdma_iovec *iovec)
 {
-	unsigned i;
+	/*
+	 * Unpinning is done through the workqueue so use the
+	 * process's mm if we have a reference to it.
+	 */
+	if ((current->flags & PF_KTHREAD) && req->pq->user_mm)
+		use_mm(req->pq->user_mm);
 
-	if (ACCESS_ONCE(iovec->offset) != iovec->iov.iov_len) {
-		hfi1_cdbg(SDMA,
-			  "the complete vector has not been sent yet %llu %zu",
-			  iovec->offset, iovec->iov.iov_len);
-		return;
+	hfi1_release_user_pages(iovec->pages, iovec->npages, 0);
+
+	/*
+	 * Unuse the user's mm (see above) and release the
+	 * reference to it.
+	 */
+	if (req->pq->user_mm) {
+		if (current->flags & PF_KTHREAD)
+			unuse_mm(req->pq->user_mm);
+		mmput(req->pq->user_mm);
 	}
-	for (i = 0; i < iovec->npages; i++)
-		if (iovec->pages[i])
-			put_page(iovec->pages[i]);
+
 	kfree(iovec->pages);
 	iovec->pages = NULL;
 	iovec->npages = 0;
@@ -1354,54 +1353,116 @@ static int set_txreq_header_ahg(struct user_sdma_request *req,
 	return diff;
 }
 
+/*
+ * SDMA tx request completion callback. Called when the SDMA progress
+ * state machine gets notification that the SDMA descriptors for this
+ * tx request have been processed by the DMA engine. Called in
+ * interrupt context.
+ */
 static void user_sdma_txreq_cb(struct sdma_txreq *txreq, int status,
 			       int drain)
 {
 	struct user_sdma_txreq *tx =
 		container_of(txreq, struct user_sdma_txreq, txreq);
-	struct user_sdma_request *req = tx->req;
-	struct hfi1_user_sdma_pkt_q *pq = req ? req->pq : NULL;
-	u64 tx_seqnum;
+	struct user_sdma_request *req;
+	bool defer;
+	int i;
 
-	if (unlikely(!req || !pq))
+	if (!tx->req)
 		return;
 
-	/* If we have any io vectors associated with this txreq,
-	 * check whether they need to be 'freed'. */
-	if (tx->idx != -1) {
-		int i;
+	req = tx->req;
+	/*
+	 * If this is the callback for the last packet of the request,
+	 * queue up the request for clean up.
+	 */
+	defer = (tx->seqnum == req->info.npkts - 1);
 
-		for (i = tx->idx; i >= 0; i--) {
-			if (tx->iovecs[i].flags & TXREQ_FLAGS_IOVEC_LAST_PKT)
-				unpin_vector_pages(tx->iovecs[i].vec);
+	/*
+	 * If we have any io vectors associated with this txreq,
+	 * check whether they need to be 'freed'. We can't free them
+	 * here because the unpin function needs to be able to sleep.
+	 */
+	for (i = tx->idx; i >= 0; i--) {
+		if (tx->iovecs[i].flags & TXREQ_FLAGS_IOVEC_LAST_PKT) {
+			defer = true;
+			break;
 		}
 	}
 
-	tx_seqnum = tx->seqnum;
-	kmem_cache_free(pq->txreq_cache, tx);
-
+	req->status = status;
 	if (status != SDMA_TXREQ_S_OK) {
-		dd_dev_err(pq->dd, "SDMA completion with error %d", status);
-		set_comp_state(req, ERROR, status);
+		SDMA_DBG(req, "SDMA completion with error %d",
+			 status);
 		set_bit(SDMA_REQ_HAS_ERROR, &req->flags);
-		/* Do not free the request until the sender loop has ack'ed
-		 * the error and we've seen all txreqs. */
-		if (tx_seqnum == ACCESS_ONCE(req->seqnum) &&
-		    test_bit(SDMA_REQ_DONE_ERROR, &req->flags)) {
-			atomic_dec(&pq->n_reqs);
-			user_sdma_free_request(req);
-		}
+		defer = true;
+	}
+
+	/*
+	 * Defer the clean up of the iovectors and the request until later
+	 * so it can be done outside of interrupt context.
+	 */
+	if (defer) {
+		spin_lock(&req->txcmp_lock);
+		list_add_tail(&tx->list, &req->txcmp);
+		spin_unlock(&req->txcmp_lock);
+		schedule_work(&req->worker);
 	} else {
-		if (tx_seqnum == req->info.npkts - 1) {
-			/* We've sent and completed all packets in this
-			 * request. Signal completion to the user */
-			atomic_dec(&pq->n_reqs);
-			set_comp_state(req, COMPLETE, 0);
-			user_sdma_free_request(req);
+		kmem_cache_free(req->pq->txreq_cache, tx);
+	}
+}
+
+static void user_sdma_delayed_completion(struct work_struct *work)
+{
+	struct user_sdma_request *req =
+		container_of(work, struct user_sdma_request, worker);
+	struct hfi1_user_sdma_pkt_q *pq = req->pq;
+	struct user_sdma_txreq *tx = NULL;
+	unsigned long flags;
+	u64 seqnum;
+	int i;
+
+	while (1) {
+		spin_lock_irqsave(&req->txcmp_lock, flags);
+		if (!list_empty(&req->txcmp)) {
+			tx = list_first_entry(&req->txcmp,
+					      struct user_sdma_txreq, list);
+			list_del(&tx->list);
+		}
+		spin_unlock_irqrestore(&req->txcmp_lock, flags);
+		if (!tx)
+			break;
+
+		for (i = tx->idx; i >= 0; i--)
+			if (tx->iovecs[i].flags & TXREQ_FLAGS_IOVEC_LAST_PKT)
+				unpin_vector_pages(req, tx->iovecs[i].vec);
+
+		seqnum = tx->seqnum;
+		kmem_cache_free(pq->txreq_cache, tx);
+		tx = NULL;
+
+		if (req->status != SDMA_TXREQ_S_OK) {
+			if (seqnum == ACCESS_ONCE(req->seqnum) &&
+			    test_bit(SDMA_REQ_DONE_ERROR, &req->flags)) {
+				atomic_dec(&pq->n_reqs);
+				set_comp_state(req, ERROR, req->status);
+				user_sdma_free_request(req);
+				break;
+			}
+		} else {
+			if (seqnum == req->info.npkts - 1) {
+				atomic_dec(&pq->n_reqs);
+				set_comp_state(req, COMPLETE, 0);
+				user_sdma_free_request(req);
+				break;
+			}
 		}
 	}
-	if (!atomic_read(&pq->n_reqs))
+
+	if (!atomic_read(&pq->n_reqs)) {
 		xchg(&pq->state, SDMA_PKT_Q_INACTIVE);
+		wake_up(&pq->wait);
+	}
 }
 
 static void user_sdma_free_request(struct user_sdma_request *req)
@@ -1422,10 +1483,8 @@ static void user_sdma_free_request(struct user_sdma_request *req)
 
 		for (i = 0; i < req->data_iovs; i++)
 			if (req->iovs[i].npages && req->iovs[i].pages)
-				unpin_vector_pages(&req->iovs[i]);
+				unpin_vector_pages(req, &req->iovs[i]);
 	}
-	if (req->user_proc)
-		put_task_struct(req->user_proc);
 	kfree(req->tids);
 	clear_bit(SDMA_REQ_IN_USE, &req->flags);
 }
diff --git a/drivers/staging/rdma/hfi1/user_sdma.h b/drivers/staging/rdma/hfi1/user_sdma.h
index fa4422553e23..0afa28508a8a 100644
--- a/drivers/staging/rdma/hfi1/user_sdma.h
+++ b/drivers/staging/rdma/hfi1/user_sdma.h
@@ -52,15 +52,7 @@
 
 #include "common.h"
 #include "iowait.h"
-
-#define EXP_TID_TIDLEN_MASK   0x7FFULL
-#define EXP_TID_TIDLEN_SHIFT  0
-#define EXP_TID_TIDCTRL_MASK  0x3ULL
-#define EXP_TID_TIDCTRL_SHIFT 20
-#define EXP_TID_TIDIDX_MASK   0x7FFULL
-#define EXP_TID_TIDIDX_SHIFT  22
-#define EXP_TID_GET(tid, field)	\
-	(((tid) >> EXP_TID_TID##field##_SHIFT) & EXP_TID_TID##field##_MASK)
+#include "user_exp_rcv.h"
 
 extern uint extended_psn;
 
@@ -76,6 +68,8 @@ struct hfi1_user_sdma_pkt_q {
 	struct user_sdma_request *reqs;
 	struct iowait busy;
 	unsigned state;
+	wait_queue_head_t wait;
+	struct mm_struct *user_mm;
 };
 
 struct hfi1_user_sdma_comp_q {
diff --git a/drivers/staging/rdma/hfi1/verbs.c b/drivers/staging/rdma/hfi1/verbs.c
index 9beb0aa876f0..09b8d412ee90 100644
--- a/drivers/staging/rdma/hfi1/verbs.c
+++ b/drivers/staging/rdma/hfi1/verbs.c
@@ -162,6 +162,8 @@ static inline struct hfi1_ucontext *to_iucontext(struct ib_ucontext
 	return container_of(ibucontext, struct hfi1_ucontext, ibucontext);
 }
 
+static inline void _hfi1_schedule_send(struct hfi1_qp *qp);
+
 /*
  * Translate ib_wr_opcode into ib_wc_opcode.
  */
@@ -509,9 +511,9 @@ static int post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
 		nreq++;
 	}
 bail:
-	if (nreq && !call_send)
-		hfi1_schedule_send(qp);
 	spin_unlock_irqrestore(&qp->s_lock, flags);
+	if (nreq && !call_send)
+		_hfi1_schedule_send(qp);
 	if (nreq && call_send)
 		hfi1_do_send(&qp->s_iowait.iowork);
 	return err;
@@ -999,17 +1001,19 @@ bail_txadd:
 	return ret;
 }
 
-int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
-			u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			u32 plen, u32 dwords, u64 pbc)
+int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			u64 pbc)
 {
-	struct hfi1_ibdev *dev = to_idev(qp->ibqp.device);
-	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct ahg_ib_header *ahdr = qp->s_hdr;
+	u32 hdrwords = qp->s_hdrwords;
+	struct hfi1_sge_state *ss = qp->s_cur_sge;
+	u32 len = qp->s_cur_size;
+	u32 plen = hdrwords + ((len + 3) >> 2) + 2; /* includes pbc */
+	struct hfi1_ibdev *dev = ps->dev;
+	struct hfi1_pportdata *ppd = ps->ppd;
 	struct verbs_txreq *tx;
 	struct sdma_txreq *stx;
 	u64 pbc_flags = 0;
-	struct sdma_engine *sde;
 	u8 sc5 = qp->s_sc;
 	int ret;
 
@@ -1030,12 +1034,7 @@ int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
 	if (IS_ERR(tx))
 		goto bail_tx;
 
-	if (!qp->s_hdr->sde) {
-		tx->sde = sde = qp_to_sdma_engine(qp, sc5);
-		if (!sde)
-			goto bail_no_sde;
-	} else
-		tx->sde = sde = qp->s_hdr->sde;
+	tx->sde = qp->s_sde;
 
 	if (likely(pbc == 0)) {
 		u32 vl = sc_to_vlt(dd_from_ibdev(qp->ibqp.device), sc5);
@@ -1050,17 +1049,15 @@ int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
 	if (qp->s_rdma_mr)
 		qp->s_rdma_mr = NULL;
 	tx->hdr_dwords = hdrwords + 2;
-	ret = build_verbs_tx_desc(sde, ss, len, tx, ahdr, pbc);
+	ret = build_verbs_tx_desc(tx->sde, ss, len, tx, ahdr, pbc);
 	if (unlikely(ret))
 		goto bail_build;
 	trace_output_ibhdr(dd_from_ibdev(qp->ibqp.device), &ahdr->ibh);
-	ret =  sdma_send_txreq(sde, &qp->s_iowait, &tx->txreq);
+	ret =  sdma_send_txreq(tx->sde, &qp->s_iowait, &tx->txreq);
 	if (unlikely(ret == -ECOMM))
 		goto bail_ecomm;
 	return ret;
 
-bail_no_sde:
-	hfi1_put_txreq(tx);
 bail_ecomm:
 	/* The current one got "sent" */
 	return 0;
@@ -1126,12 +1123,16 @@ struct send_context *qp_to_send_context(struct hfi1_qp *qp, u8 sc5)
 	return dd->vld[vl].sc;
 }
 
-int hfi1_verbs_send_pio(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
-			u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			u32 plen, u32 dwords, u64 pbc)
+int hfi1_verbs_send_pio(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			u64 pbc)
 {
-	struct hfi1_ibport *ibp = to_iport(qp->ibqp.device, qp->port_num);
-	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct ahg_ib_header *ahdr = qp->s_hdr;
+	u32 hdrwords = qp->s_hdrwords;
+	struct hfi1_sge_state *ss = qp->s_cur_sge;
+	u32 len = qp->s_cur_size;
+	u32 dwords = (len + 3) >> 2;
+	u32 plen = hdrwords + dwords + 2; /* includes pbc */
+	struct hfi1_pportdata *ppd = ps->ppd;
 	u32 *hdr = (u32 *)&ahdr->ibh;
 	u64 pbc_flags = 0;
 	u32 sc5;
@@ -1303,23 +1304,18 @@ bad:
 /**
  * hfi1_verbs_send - send a packet
  * @qp: the QP to send on
- * @ahdr: the packet header
- * @hdrwords: the number of 32-bit words in the header
- * @ss: the SGE to send
- * @len: the length of the packet in bytes
+ * @ps: the state of the packet to send
  *
  * Return zero if packet is sent or queued OK.
  * Return non-zero and clear qp->s_flags HFI1_S_BUSY otherwise.
  */
-int hfi1_verbs_send(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
-		    u32 hdrwords, struct hfi1_sge_state *ss, u32 len)
+int hfi1_verbs_send(struct hfi1_qp *qp, struct hfi1_pkt_state *ps)
 {
 	struct hfi1_devdata *dd = dd_from_ibdev(qp->ibqp.device);
-	u32 plen;
+	struct ahg_ib_header *ahdr = qp->s_hdr;
 	int ret;
 	int pio = 0;
 	unsigned long flags = 0;
-	u32 dwords = (len + 3) >> 2;
 
 	/*
 	 * VL15 packets (IB_QPT_SMI) will always use PIO, so we
@@ -1350,23 +1346,16 @@ int hfi1_verbs_send(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
 		return -EINVAL;
 	}
 
-	/*
-	 * Calculate the send buffer trigger address.
-	 * The +2 counts for the pbc control qword
-	 */
-	plen = hdrwords + dwords + 2;
-
 	if (pio) {
-		ret = dd->process_pio_send(
-			qp, ahdr, hdrwords, ss, len, plen, dwords, 0);
+		ret = dd->process_pio_send(qp, ps, 0);
 	} else {
 #ifdef CONFIG_SDMA_VERBOSITY
 		dd_dev_err(dd, "CONFIG SDMA %s:%d %s()\n",
 			   slashstrip(__FILE__), __LINE__, __func__);
-		dd_dev_err(dd, "SDMA hdrwords = %u, len = %u\n", hdrwords, len);
+		dd_dev_err(dd, "SDMA hdrwords = %u, len = %u\n", qp->s_hdrwords,
+			   qp->s_cur_size);
 #endif
-		ret = dd->process_dma_send(
-			qp, ahdr, hdrwords, ss, len, plen, dwords, 0);
+		ret = dd->process_dma_send(qp, ps, 0);
 	}
 
 	return ret;
@@ -2063,7 +2052,6 @@ int hfi1_register_ib_device(struct hfi1_devdata *dd)
 	ibdev->poll_cq = hfi1_poll_cq;
 	ibdev->req_notify_cq = hfi1_req_notify_cq;
 	ibdev->get_dma_mr = hfi1_get_dma_mr;
-	ibdev->reg_phys_mr = hfi1_reg_phys_mr;
 	ibdev->reg_user_mr = hfi1_reg_user_mr;
 	ibdev->dereg_mr = hfi1_dereg_mr;
 	ibdev->alloc_mr = hfi1_alloc_mr;
@@ -2135,28 +2123,43 @@ void hfi1_unregister_ib_device(struct hfi1_devdata *dd)
 	vfree(dev->lk_table.table);
 }
 
-/*
- * This must be called with s_lock held.
- */
-void hfi1_schedule_send(struct hfi1_qp *qp)
-{
-	if (hfi1_send_ok(qp)) {
-		struct hfi1_ibport *ibp =
-			to_iport(qp->ibqp.device, qp->port_num);
-		struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
-
-		iowait_schedule(&qp->s_iowait, ppd->hfi1_wq);
-	}
-}
-
 void hfi1_cnp_rcv(struct hfi1_packet *packet)
 {
 	struct hfi1_ibport *ibp = &packet->rcd->ppd->ibport_data;
-
-	if (packet->qp->ibqp.qp_type == IB_QPT_UC)
-		hfi1_uc_rcv(packet);
-	else if (packet->qp->ibqp.qp_type == IB_QPT_UD)
-		hfi1_ud_rcv(packet);
-	else
+	struct hfi1_pportdata *ppd = ppd_from_ibp(ibp);
+	struct hfi1_ib_header *hdr = packet->hdr;
+	struct hfi1_qp *qp = packet->qp;
+	u32 lqpn, rqpn = 0;
+	u16 rlid = 0;
+	u8 sl, sc5, sc4_bit, svc_type;
+	bool sc4_set = has_sc4_bit(packet);
+
+	switch (packet->qp->ibqp.qp_type) {
+	case IB_QPT_UC:
+		rlid = qp->remote_ah_attr.dlid;
+		rqpn = qp->remote_qpn;
+		svc_type = IB_CC_SVCTYPE_UC;
+		break;
+	case IB_QPT_RC:
+		rlid = qp->remote_ah_attr.dlid;
+		rqpn = qp->remote_qpn;
+		svc_type = IB_CC_SVCTYPE_RC;
+		break;
+	case IB_QPT_SMI:
+	case IB_QPT_GSI:
+	case IB_QPT_UD:
+		svc_type = IB_CC_SVCTYPE_UD;
+		break;
+	default:
 		ibp->n_pkt_drops++;
+		return;
+	}
+
+	sc4_bit = sc4_set << 4;
+	sc5 = (be16_to_cpu(hdr->lrh[0]) >> 12) & 0xf;
+	sc5 |= sc4_bit;
+	sl = ibp->sc_to_sl[sc5];
+	lqpn = qp->ibqp.qp_num;
+
+	process_becn(ppd, sl, rlid, lqpn, rqpn, svc_type);
 }
diff --git a/drivers/staging/rdma/hfi1/verbs.h b/drivers/staging/rdma/hfi1/verbs.h
index 041ad07ee699..286e468b0479 100644
--- a/drivers/staging/rdma/hfi1/verbs.h
+++ b/drivers/staging/rdma/hfi1/verbs.h
@@ -120,9 +120,9 @@ struct hfi1_packet;
 
 #define HFI1_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)
+#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
@@ -441,7 +441,8 @@ struct hfi1_qp {
 	struct hfi1_swqe *s_wq;  /* send work queue */
 	struct hfi1_mmap_info *ip;
 	struct ahg_ib_header *s_hdr;     /* next packet header to send */
-	u8 s_sc;			/* SC[0..4] for next packet */
+	/* sc for UC/RC QPs - based on ah for UD */
+	u8 s_sc;
 	unsigned long timeout_jiffies;  /* computed from timeout */
 
 	enum ib_mtu path_mtu;
@@ -489,6 +490,7 @@ struct hfi1_qp {
 	u32 r_psn;              /* expected rcv packet sequence number */
 	u32 r_msn;              /* message sequence number */
 
+	u8 r_adefered;         /* number of acks defered */
 	u8 r_state;             /* opcode of last packet received */
 	u8 r_flags;
 	u8 r_head_ack_queue;    /* index into s_ack_queue[] */
@@ -544,6 +546,16 @@ struct hfi1_qp {
 };
 
 /*
+ * This structure is used to hold commonly lookedup and computed values during
+ * the send engine progress.
+ */
+struct hfi1_pkt_state {
+	struct hfi1_ibdev *dev;
+	struct hfi1_ibport *ibp;
+	struct hfi1_pportdata *ppd;
+};
+
+/*
  * Atomic bit definitions for r_aflags.
  */
 #define HFI1_R_WRID_VALID        0
@@ -552,11 +564,13 @@ struct hfi1_qp {
 /*
  * Bit definitions for r_flags.
  */
-#define HFI1_R_REUSE_SGE 0x01
-#define HFI1_R_RDMAR_SEQ 0x02
-#define HFI1_R_RSP_NAK   0x04
-#define HFI1_R_RSP_SEND  0x08
-#define HFI1_R_COMM_EST  0x10
+#define HFI1_R_REUSE_SGE       0x01
+#define HFI1_R_RDMAR_SEQ       0x02
+/* defer ack until end of interrupt session */
+#define HFI1_R_RSP_DEFERED_ACK 0x04
+/* relay ack to send engine */
+#define HFI1_R_RSP_SEND        0x08
+#define HFI1_R_COMM_EST        0x10
 
 /*
  * Bit definitions for s_flags.
@@ -846,9 +860,8 @@ static inline int hfi1_send_ok(struct hfi1_qp *qp)
 /*
  * This must be called with s_lock held.
  */
-void hfi1_schedule_send(struct hfi1_qp *qp);
 void hfi1_bad_pqkey(struct hfi1_ibport *ibp, __be16 trap_num, u32 key, u32 sl,
-		    u32 qp1, u32 qp2, __be16 lid1, __be16 lid2);
+		    u32 qp1, u32 qp2, u16 lid1, u16 lid2);
 void hfi1_cap_mask_chg(struct hfi1_ibport *ibp);
 void hfi1_sys_guid_chg(struct hfi1_ibport *ibp);
 void hfi1_node_desc_chg(struct hfi1_ibport *ibp);
@@ -927,8 +940,7 @@ int hfi1_mcast_tree_empty(struct hfi1_ibport *ibp);
 struct verbs_txreq;
 void hfi1_put_txreq(struct verbs_txreq *tx);
 
-int hfi1_verbs_send(struct hfi1_qp *qp, struct ahg_ib_header *ahdr,
-		    u32 hdrwords, struct hfi1_sge_state *ss, u32 len);
+int hfi1_verbs_send(struct hfi1_qp *qp, struct hfi1_pkt_state *ps);
 
 void hfi1_copy_sge(struct hfi1_sge_state *ss, void *data, u32 length,
 		   int release);
@@ -1012,10 +1024,6 @@ int hfi1_resize_cq(struct ib_cq *ibcq, int cqe, struct ib_udata *udata);
 
 struct ib_mr *hfi1_get_dma_mr(struct ib_pd *pd, int acc);
 
-struct ib_mr *hfi1_reg_phys_mr(struct ib_pd *pd,
-			       struct ib_phys_buf *buffer_list,
-			       int num_phys_buf, int acc, u64 *iova_start);
-
 struct ib_mr *hfi1_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 			       u64 virt_addr, int mr_access_flags,
 			       struct ib_udata *udata);
@@ -1069,8 +1077,6 @@ int hfi1_mmap(struct ib_ucontext *context, struct vm_area_struct *vma);
 
 int hfi1_get_rwqe(struct hfi1_qp *qp, int wr_id_only);
 
-void hfi1_migrate_qp(struct hfi1_qp *qp);
-
 int hfi1_ruc_check_hdr(struct hfi1_ibport *ibp, struct hfi1_ib_header *hdr,
 		       int has_grh, struct hfi1_qp *qp, u32 bth0);
 
@@ -1101,13 +1107,11 @@ void hfi1_ib_rcv(struct hfi1_packet *packet);
 
 unsigned hfi1_get_npkeys(struct hfi1_devdata *);
 
-int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct ahg_ib_header *hdr,
-			u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			u32 plen, u32 dwords, u64 pbc);
+int hfi1_verbs_send_dma(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			u64 pbc);
 
-int hfi1_verbs_send_pio(struct hfi1_qp *qp, struct ahg_ib_header *hdr,
-			u32 hdrwords, struct hfi1_sge_state *ss, u32 len,
-			u32 plen, u32 dwords, u64 pbc);
+int hfi1_verbs_send_pio(struct hfi1_qp *qp, struct hfi1_pkt_state *ps,
+			u64 pbc);
 
 struct send_context *qp_to_send_context(struct hfi1_qp *qp, u8 sc5);
 
diff --git a/drivers/staging/rdma/ipath/Kconfig b/drivers/staging/rdma/ipath/Kconfig
deleted file mode 100644
index 041ce0634968..000000000000
--- a/drivers/staging/rdma/ipath/Kconfig
+++ /dev/null
@@ -1,16 +0,0 @@
-config INFINIBAND_IPATH
-	tristate "QLogic HTX HCA support"
-	depends on 64BIT && NET && HT_IRQ
-	---help---
-	This is a driver for the deprecated QLogic Hyper-Transport
-	IB host channel adapter (model QHT7140),
-	including InfiniBand verbs support.  This driver allows these
-	devices to be used with both kernel upper level protocols such
-	as IP-over-InfiniBand as well as with userspace applications
-	(in conjunction with InfiniBand userspace access).
-	For QLogic PCIe QLE based cards, use the QIB driver instead.
-
-	If you have this hardware you will need to boot with PAT disabled
-	on your x86-64 systems, use the nopat kernel parameter.
-
-	Note that this driver will soon be removed entirely from the kernel.
diff --git a/drivers/staging/rdma/ipath/Makefile b/drivers/staging/rdma/ipath/Makefile
deleted file mode 100644
index 4496f2820c92..000000000000
--- a/drivers/staging/rdma/ipath/Makefile
+++ /dev/null
@@ -1,37 +0,0 @@
-ccflags-y := -DIPATH_IDSTR='"QLogic kernel.org driver"' \
-	-DIPATH_KERN_TYPE=0
-
-obj-$(CONFIG_INFINIBAND_IPATH) += ib_ipath.o
-
-ib_ipath-y := \
-	ipath_cq.o \
-	ipath_diag.o \
-	ipath_dma.o \
-	ipath_driver.o \
-	ipath_eeprom.o \
-	ipath_file_ops.o \
-	ipath_fs.o \
-	ipath_init_chip.o \
-	ipath_intr.o \
-	ipath_keys.o \
-	ipath_mad.o \
-	ipath_mmap.o \
-	ipath_mr.o \
-	ipath_qp.o \
-	ipath_rc.o \
-	ipath_ruc.o \
-	ipath_sdma.o \
-	ipath_srq.o \
-	ipath_stats.o \
-	ipath_sysfs.o \
-	ipath_uc.o \
-	ipath_ud.o \
-	ipath_user_pages.o \
-	ipath_user_sdma.o \
-	ipath_verbs_mcast.o \
-	ipath_verbs.o
-
-ib_ipath-$(CONFIG_HT_IRQ) += ipath_iba6110.o
-
-ib_ipath-$(CONFIG_X86_64) += ipath_wc_x86_64.o
-ib_ipath-$(CONFIG_PPC64) += ipath_wc_ppc64.o
diff --git a/drivers/staging/rdma/ipath/TODO b/drivers/staging/rdma/ipath/TODO
deleted file mode 100644
index cb00158d64c8..000000000000
--- a/drivers/staging/rdma/ipath/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-The ipath driver has been moved to staging in preparation for its removal in a
-few releases. The driver will be deleted during the 4.6 merge window.
-
-Contact Dennis Dalessandro <dennis.dalessandro@intel.com> and
-Cc: linux-rdma@vger.kernel.org
diff --git a/drivers/staging/rdma/ipath/ipath_common.h b/drivers/staging/rdma/ipath/ipath_common.h
deleted file mode 100644
index 28cfe97cf1e9..000000000000
--- a/drivers/staging/rdma/ipath/ipath_common.h
+++ /dev/null
@@ -1,851 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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 _IPATH_COMMON_H
-#define _IPATH_COMMON_H
-
-/*
- * This file contains defines, structures, etc. that are used
- * to communicate between kernel and user code.
- */
-
-
-/* This is the IEEE-assigned OUI for QLogic Inc. InfiniPath */
-#define IPATH_SRC_OUI_1 0x00
-#define IPATH_SRC_OUI_2 0x11
-#define IPATH_SRC_OUI_3 0x75
-
-/* version of protocol header (known to chip also). In the long run,
- * we should be able to generate and accept a range of version numbers;
- * for now we only accept one, and it's compiled in.
- */
-#define IPS_PROTO_VERSION 2
-
-/*
- * These are compile time constants that you may want to enable or disable
- * if you are trying to debug problems with code or performance.
- * IPATH_VERBOSE_TRACING define as 1 if you want additional tracing in
- * fastpath code
- * IPATH_TRACE_REGWRITES define as 1 if you want register writes to be
- * traced in faspath code
- * _IPATH_TRACING define as 0 if you want to remove all tracing in a
- * compilation unit
- * _IPATH_DEBUGGING define as 0 if you want to remove debug prints
- */
-
-/*
- * The value in the BTH QP field that InfiniPath uses to differentiate
- * an infinipath protocol IB packet vs standard IB transport
- */
-#define IPATH_KD_QP 0x656b79
-
-/*
- * valid states passed to ipath_set_linkstate() user call
- */
-#define IPATH_IB_LINKDOWN		0
-#define IPATH_IB_LINKARM		1
-#define IPATH_IB_LINKACTIVE		2
-#define IPATH_IB_LINKDOWN_ONLY		3
-#define IPATH_IB_LINKDOWN_SLEEP		4
-#define IPATH_IB_LINKDOWN_DISABLE	5
-#define IPATH_IB_LINK_LOOPBACK	6 /* enable local loopback */
-#define IPATH_IB_LINK_EXTERNAL	7 /* normal, disable local loopback */
-#define IPATH_IB_LINK_NO_HRTBT	8 /* disable Heartbeat, e.g. for loopback */
-#define IPATH_IB_LINK_HRTBT	9 /* enable heartbeat, normal, non-loopback */
-
-/*
- * These 3 values (SDR and DDR may be ORed for auto-speed
- * negotiation) are used for the 3rd argument to path_f_set_ib_cfg
- * with cmd IPATH_IB_CFG_SPD_ENB, by direct calls or via sysfs.  They
- * are also the the possible values for ipath_link_speed_enabled and active
- * The values were chosen to match values used within the IB spec.
- */
-#define IPATH_IB_SDR 1
-#define IPATH_IB_DDR 2
-
-/*
- * stats maintained by the driver.  For now, at least, this is global
- * to all minor devices.
- */
-struct infinipath_stats {
-	/* number of interrupts taken */
-	__u64 sps_ints;
-	/* number of interrupts for errors */
-	__u64 sps_errints;
-	/* number of errors from chip (not incl. packet errors or CRC) */
-	__u64 sps_errs;
-	/* number of packet errors from chip other than CRC */
-	__u64 sps_pkterrs;
-	/* number of packets with CRC errors (ICRC and VCRC) */
-	__u64 sps_crcerrs;
-	/* number of hardware errors reported (parity, etc.) */
-	__u64 sps_hwerrs;
-	/* number of times IB link changed state unexpectedly */
-	__u64 sps_iblink;
-	__u64 sps_unused; /* was fastrcvint, no longer implemented */
-	/* number of kernel (port0) packets received */
-	__u64 sps_port0pkts;
-	/* number of "ethernet" packets sent by driver */
-	__u64 sps_ether_spkts;
-	/* number of "ethernet" packets received by driver */
-	__u64 sps_ether_rpkts;
-	/* number of SMA packets sent by driver. Obsolete. */
-	__u64 sps_sma_spkts;
-	/* number of SMA packets received by driver. Obsolete. */
-	__u64 sps_sma_rpkts;
-	/* number of times all ports rcvhdrq was full and packet dropped */
-	__u64 sps_hdrqfull;
-	/* number of times all ports egrtid was full and packet dropped */
-	__u64 sps_etidfull;
-	/*
-	 * number of times we tried to send from driver, but no pio buffers
-	 * avail
-	 */
-	__u64 sps_nopiobufs;
-	/* number of ports currently open */
-	__u64 sps_ports;
-	/* list of pkeys (other than default) accepted (0 means not set) */
-	__u16 sps_pkeys[4];
-	__u16 sps_unused16[4]; /* available; maintaining compatible layout */
-	/* number of user ports per chip (not IB ports) */
-	__u32 sps_nports;
-	/* not our interrupt, or already handled */
-	__u32 sps_nullintr;
-	/* max number of packets handled per receive call */
-	__u32 sps_maxpkts_call;
-	/* avg number of packets handled per receive call */
-	__u32 sps_avgpkts_call;
-	/* total number of pages locked */
-	__u64 sps_pagelocks;
-	/* total number of pages unlocked */
-	__u64 sps_pageunlocks;
-	/*
-	 * Number of packets dropped in kernel other than errors (ether
-	 * packets if ipath not configured, etc.)
-	 */
-	__u64 sps_krdrops;
-	__u64 sps_txeparity; /* PIO buffer parity error, recovered */
-	/* pad for future growth */
-	__u64 __sps_pad[45];
-};
-
-/*
- * These are the status bits readable (in ascii form, 64bit value)
- * from the "status" sysfs file.
- */
-#define IPATH_STATUS_INITTED       0x1	/* basic initialization done */
-#define IPATH_STATUS_DISABLED      0x2	/* hardware disabled */
-/* Device has been disabled via admin request */
-#define IPATH_STATUS_ADMIN_DISABLED    0x4
-/* Chip has been found and initted */
-#define IPATH_STATUS_CHIP_PRESENT 0x20
-/* IB link is at ACTIVE, usable for data traffic */
-#define IPATH_STATUS_IB_READY     0x40
-/* link is configured, LID, MTU, etc. have been set */
-#define IPATH_STATUS_IB_CONF      0x80
-/* no link established, probably no cable */
-#define IPATH_STATUS_IB_NOCABLE  0x100
-/* A Fatal hardware error has occurred. */
-#define IPATH_STATUS_HWERROR     0x200
-
-/*
- * The list of usermode accessible registers.  Also see Reg_* later in file.
- */
-typedef enum _ipath_ureg {
-	/* (RO)  DMA RcvHdr to be used next. */
-	ur_rcvhdrtail = 0,
-	/* (RW)  RcvHdr entry to be processed next by host. */
-	ur_rcvhdrhead = 1,
-	/* (RO)  Index of next Eager index to use. */
-	ur_rcvegrindextail = 2,
-	/* (RW)  Eager TID to be processed next */
-	ur_rcvegrindexhead = 3,
-	/* For internal use only; max register number. */
-	_IPATH_UregMax
-} ipath_ureg;
-
-/* bit values for spi_runtime_flags */
-#define IPATH_RUNTIME_HT	0x1
-#define IPATH_RUNTIME_PCIE	0x2
-#define IPATH_RUNTIME_FORCE_WC_ORDER	0x4
-#define IPATH_RUNTIME_RCVHDR_COPY	0x8
-#define IPATH_RUNTIME_MASTER	0x10
-#define IPATH_RUNTIME_NODMA_RTAIL 0x80
-#define IPATH_RUNTIME_SDMA	      0x200
-#define IPATH_RUNTIME_FORCE_PIOAVAIL 0x400
-#define IPATH_RUNTIME_PIO_REGSWAPPED 0x800
-
-/*
- * This structure is returned by ipath_userinit() immediately after
- * open to get implementation-specific info, and info specific to this
- * instance.
- *
- * This struct must have explict pad fields where type sizes
- * may result in different alignments between 32 and 64 bit
- * programs, since the 64 bit * bit kernel requires the user code
- * to have matching offsets
- */
-struct ipath_base_info {
-	/* version of hardware, for feature checking. */
-	__u32 spi_hw_version;
-	/* version of software, for feature checking. */
-	__u32 spi_sw_version;
-	/* InfiniPath port assigned, goes into sent packets */
-	__u16 spi_port;
-	__u16 spi_subport;
-	/*
-	 * IB MTU, packets IB data must be less than this.
-	 * The MTU is in bytes, and will be a multiple of 4 bytes.
-	 */
-	__u32 spi_mtu;
-	/*
-	 * Size of a PIO buffer.  Any given packet's total size must be less
-	 * than this (in words).  Included is the starting control word, so
-	 * if 513 is returned, then total pkt size is 512 words or less.
-	 */
-	__u32 spi_piosize;
-	/* size of the TID cache in infinipath, in entries */
-	__u32 spi_tidcnt;
-	/* size of the TID Eager list in infinipath, in entries */
-	__u32 spi_tidegrcnt;
-	/* size of a single receive header queue entry in words. */
-	__u32 spi_rcvhdrent_size;
-	/*
-	 * Count of receive header queue entries allocated.
-	 * This may be less than the spu_rcvhdrcnt passed in!.
-	 */
-	__u32 spi_rcvhdr_cnt;
-
-	/* per-chip and other runtime features bitmap (IPATH_RUNTIME_*) */
-	__u32 spi_runtime_flags;
-
-	/* address where receive buffer queue is mapped into */
-	__u64 spi_rcvhdr_base;
-
-	/* user program. */
-
-	/* base address of eager TID receive buffers. */
-	__u64 spi_rcv_egrbufs;
-
-	/* Allocated by initialization code, not by protocol. */
-
-	/*
-	 * Size of each TID buffer in host memory, starting at
-	 * spi_rcv_egrbufs.  The buffers are virtually contiguous.
-	 */
-	__u32 spi_rcv_egrbufsize;
-	/*
-	 * The special QP (queue pair) value that identifies an infinipath
-	 * protocol packet from standard IB packets.  More, probably much
-	 * more, to be added.
-	 */
-	__u32 spi_qpair;
-
-	/*
-	 * User register base for init code, not to be used directly by
-	 * protocol or applications.
-	 */
-	__u64 __spi_uregbase;
-	/*
-	 * Maximum buffer size in bytes that can be used in a single TID
-	 * entry (assuming the buffer is aligned to this boundary).  This is
-	 * the minimum of what the hardware and software support Guaranteed
-	 * to be a power of 2.
-	 */
-	__u32 spi_tid_maxsize;
-	/*
-	 * alignment of each pio send buffer (byte count
-	 * to add to spi_piobufbase to get to second buffer)
-	 */
-	__u32 spi_pioalign;
-	/*
-	 * The index of the first pio buffer available to this process;
-	 * needed to do lookup in spi_pioavailaddr; not added to
-	 * spi_piobufbase.
-	 */
-	__u32 spi_pioindex;
-	 /* number of buffers mapped for this process */
-	__u32 spi_piocnt;
-
-	/*
-	 * Base address of writeonly pio buffers for this process.
-	 * Each buffer has spi_piosize words, and is aligned on spi_pioalign
-	 * boundaries.  spi_piocnt buffers are mapped from this address
-	 */
-	__u64 spi_piobufbase;
-
-	/*
-	 * Base address of readonly memory copy of the pioavail registers.
-	 * There are 2 bits for each buffer.
-	 */
-	__u64 spi_pioavailaddr;
-
-	/*
-	 * Address where driver updates a copy of the interface and driver
-	 * status (IPATH_STATUS_*) as a 64 bit value.  It's followed by a
-	 * string indicating hardware error, if there was one.
-	 */
-	__u64 spi_status;
-
-	/* number of chip ports available to user processes */
-	__u32 spi_nports;
-	/* unit number of chip we are using */
-	__u32 spi_unit;
-	/* num bufs in each contiguous set */
-	__u32 spi_rcv_egrperchunk;
-	/* size in bytes of each contiguous set */
-	__u32 spi_rcv_egrchunksize;
-	/* total size of mmap to cover full rcvegrbuffers */
-	__u32 spi_rcv_egrbuftotlen;
-	__u32 spi_filler_for_align;
-	/* address of readonly memory copy of the rcvhdrq tail register. */
-	__u64 spi_rcvhdr_tailaddr;
-
-	/* shared memory pages for subports if port is shared */
-	__u64 spi_subport_uregbase;
-	__u64 spi_subport_rcvegrbuf;
-	__u64 spi_subport_rcvhdr_base;
-
-	/* shared memory page for hardware port if it is shared */
-	__u64 spi_port_uregbase;
-	__u64 spi_port_rcvegrbuf;
-	__u64 spi_port_rcvhdr_base;
-	__u64 spi_port_rcvhdr_tailaddr;
-
-} __attribute__ ((aligned(8)));
-
-
-/*
- * This version number is given to the driver by the user code during
- * initialization in the spu_userversion field of ipath_user_info, so
- * the driver can check for compatibility with user code.
- *
- * The major version changes when data structures
- * change in an incompatible way.  The driver must be the same or higher
- * for initialization to succeed.  In some cases, a higher version
- * driver will not interoperate with older software, and initialization
- * will return an error.
- */
-#define IPATH_USER_SWMAJOR 1
-
-/*
- * Minor version differences are always compatible
- * a within a major version, however if user software is larger
- * than driver software, some new features and/or structure fields
- * may not be implemented; the user code must deal with this if it
- * cares, or it must abort after initialization reports the difference.
- */
-#define IPATH_USER_SWMINOR 6
-
-#define IPATH_USER_SWVERSION ((IPATH_USER_SWMAJOR<<16) | IPATH_USER_SWMINOR)
-
-#define IPATH_KERN_TYPE 0
-
-/*
- * Similarly, this is the kernel version going back to the user.  It's
- * slightly different, in that we want to tell if the driver was built as
- * part of a QLogic release, or from the driver from openfabrics.org,
- * kernel.org, or a standard distribution, for support reasons.
- * The high bit is 0 for non-QLogic and 1 for QLogic-built/supplied.
- *
- * It's returned by the driver to the user code during initialization in the
- * spi_sw_version field of ipath_base_info, so the user code can in turn
- * check for compatibility with the kernel.
-*/
-#define IPATH_KERN_SWVERSION ((IPATH_KERN_TYPE<<31) | IPATH_USER_SWVERSION)
-
-/*
- * This structure is passed to ipath_userinit() to tell the driver where
- * user code buffers are, sizes, etc.   The offsets and sizes of the
- * fields must remain unchanged, for binary compatibility.  It can
- * be extended, if userversion is changed so user code can tell, if needed
- */
-struct ipath_user_info {
-	/*
-	 * version of user software, to detect compatibility issues.
-	 * Should be set to IPATH_USER_SWVERSION.
-	 */
-	__u32 spu_userversion;
-
-	/* desired number of receive header queue entries */
-	__u32 spu_rcvhdrcnt;
-
-	/* size of struct base_info to write to */
-	__u32 spu_base_info_size;
-
-	/*
-	 * number of words in KD protocol header
-	 * This tells InfiniPath how many words to copy to rcvhdrq.  If 0,
-	 * kernel uses a default.  Once set, attempts to set any other value
-	 * are an error (EAGAIN) until driver is reloaded.
-	 */
-	__u32 spu_rcvhdrsize;
-
-	/*
-	 * If two or more processes wish to share a port, each process
-	 * must set the spu_subport_cnt and spu_subport_id to the same
-	 * values.  The only restriction on the spu_subport_id is that
-	 * it be unique for a given node.
-	 */
-	__u16 spu_subport_cnt;
-	__u16 spu_subport_id;
-
-	__u32 spu_unused; /* kept for compatible layout */
-
-	/*
-	 * address of struct base_info to write to
-	 */
-	__u64 spu_base_info;
-
-} __attribute__ ((aligned(8)));
-
-/* User commands. */
-
-#define IPATH_CMD_MIN		16
-
-#define __IPATH_CMD_USER_INIT	16	/* old set up userspace (for old user code) */
-#define IPATH_CMD_PORT_INFO	17	/* find out what resources we got */
-#define IPATH_CMD_RECV_CTRL	18	/* control receipt of packets */
-#define IPATH_CMD_TID_UPDATE	19	/* update expected TID entries */
-#define IPATH_CMD_TID_FREE	20	/* free expected TID entries */
-#define IPATH_CMD_SET_PART_KEY	21	/* add partition key */
-#define __IPATH_CMD_SLAVE_INFO	22	/* return info on slave processes (for old user code) */
-#define IPATH_CMD_ASSIGN_PORT	23	/* allocate HCA and port */
-#define IPATH_CMD_USER_INIT 	24	/* set up userspace */
-#define IPATH_CMD_UNUSED_1	25
-#define IPATH_CMD_UNUSED_2	26
-#define IPATH_CMD_PIOAVAILUPD	27	/* force an update of PIOAvail reg */
-#define IPATH_CMD_POLL_TYPE	28	/* set the kind of polling we want */
-#define IPATH_CMD_ARMLAUNCH_CTRL	29 /* armlaunch detection control */
-/* 30 is unused */
-#define IPATH_CMD_SDMA_INFLIGHT 31	/* sdma inflight counter request */
-#define IPATH_CMD_SDMA_COMPLETE 32	/* sdma completion counter request */
-
-/*
- * Poll types
- */
-#define IPATH_POLL_TYPE_URGENT	 0x01
-#define IPATH_POLL_TYPE_OVERFLOW 0x02
-
-struct ipath_port_info {
-	__u32 num_active;	/* number of active units */
-	__u32 unit;		/* unit (chip) assigned to caller */
-	__u16 port;		/* port on unit assigned to caller */
-	__u16 subport;		/* subport on unit assigned to caller */
-	__u16 num_ports;	/* number of ports available on unit */
-	__u16 num_subports;	/* number of subports opened on port */
-};
-
-struct ipath_tid_info {
-	__u32 tidcnt;
-	/* make structure same size in 32 and 64 bit */
-	__u32 tid__unused;
-	/* virtual address of first page in transfer */
-	__u64 tidvaddr;
-	/* pointer (same size 32/64 bit) to __u16 tid array */
-	__u64 tidlist;
-
-	/*
-	 * pointer (same size 32/64 bit) to bitmap of TIDs used
-	 * for this call; checked for being large enough at open
-	 */
-	__u64 tidmap;
-};
-
-struct ipath_cmd {
-	__u32 type;			/* command type */
-	union {
-		struct ipath_tid_info tid_info;
-		struct ipath_user_info user_info;
-
-		/*
-		 * address in userspace where we should put the sdma
-		 * inflight counter
-		 */
-		__u64 sdma_inflight;
-		/*
-		 * address in userspace where we should put the sdma
-		 * completion counter
-		 */
-		__u64 sdma_complete;
-		/* address in userspace of struct ipath_port_info to
-		   write result to */
-		__u64 port_info;
-		/* enable/disable receipt of packets */
-		__u32 recv_ctrl;
-		/* enable/disable armlaunch errors (non-zero to enable) */
-		__u32 armlaunch_ctrl;
-		/* partition key to set */
-		__u16 part_key;
-		/* user address of __u32 bitmask of active slaves */
-		__u64 slave_mask_addr;
-		/* type of polling we want */
-		__u16 poll_type;
-	} cmd;
-};
-
-struct ipath_iovec {
-	/* Pointer to data, but same size 32 and 64 bit */
-	__u64 iov_base;
-
-	/*
-	 * Length of data; don't need 64 bits, but want
-	 * ipath_sendpkt to remain same size as before 32 bit changes, so...
-	 */
-	__u64 iov_len;
-};
-
-/*
- * Describes a single packet for send.  Each packet can have one or more
- * buffers, but the total length (exclusive of IB headers) must be less
- * than the MTU, and if using the PIO method, entire packet length,
- * including IB headers, must be less than the ipath_piosize value (words).
- * Use of this necessitates including sys/uio.h
- */
-struct __ipath_sendpkt {
-	__u32 sps_flags;	/* flags for packet (TBD) */
-	__u32 sps_cnt;		/* number of entries to use in sps_iov */
-	/* array of iov's describing packet. TEMPORARY */
-	struct ipath_iovec sps_iov[4];
-};
-
-/*
- * diagnostics can send a packet by "writing" one of the following
- * two structs to diag data special file
- * The first is the legacy version for backward compatibility
- */
-struct ipath_diag_pkt {
-	__u32 unit;
-	__u64 data;
-	__u32 len;
-};
-
-/* The second diag_pkt struct is the expanded version that allows
- * more control over the packet, specifically, by allowing a custom
- * pbc (+ static rate) qword, so that special modes and deliberate
- * changes to CRCs can be used. The elements were also re-ordered
- * for better alignment and to avoid padding issues.
- */
-struct ipath_diag_xpkt {
-	__u64 data;
-	__u64 pbc_wd;
-	__u32 unit;
-	__u32 len;
-};
-
-/*
- * Data layout in I2C flash (for GUID, etc.)
- * All fields are little-endian binary unless otherwise stated
- */
-#define IPATH_FLASH_VERSION 2
-struct ipath_flash {
-	/* flash layout version (IPATH_FLASH_VERSION) */
-	__u8 if_fversion;
-	/* checksum protecting if_length bytes */
-	__u8 if_csum;
-	/*
-	 * valid length (in use, protected by if_csum), including
-	 * if_fversion and if_csum themselves)
-	 */
-	__u8 if_length;
-	/* the GUID, in network order */
-	__u8 if_guid[8];
-	/* number of GUIDs to use, starting from if_guid */
-	__u8 if_numguid;
-	/* the (last 10 characters of) board serial number, in ASCII */
-	char if_serial[12];
-	/* board mfg date (YYYYMMDD ASCII) */
-	char if_mfgdate[8];
-	/* last board rework/test date (YYYYMMDD ASCII) */
-	char if_testdate[8];
-	/* logging of error counts, TBD */
-	__u8 if_errcntp[4];
-	/* powered on hours, updated at driver unload */
-	__u8 if_powerhour[2];
-	/* ASCII free-form comment field */
-	char if_comment[32];
-	/* Backwards compatible prefix for longer QLogic Serial Numbers */
-	char if_sprefix[4];
-	/* 82 bytes used, min flash size is 128 bytes */
-	__u8 if_future[46];
-};
-
-/*
- * These are the counters implemented in the chip, and are listed in order.
- * The InterCaps naming is taken straight from the chip spec.
- */
-struct infinipath_counters {
-	__u64 LBIntCnt;
-	__u64 LBFlowStallCnt;
-	__u64 TxSDmaDescCnt;	/* was Reserved1 */
-	__u64 TxUnsupVLErrCnt;
-	__u64 TxDataPktCnt;
-	__u64 TxFlowPktCnt;
-	__u64 TxDwordCnt;
-	__u64 TxLenErrCnt;
-	__u64 TxMaxMinLenErrCnt;
-	__u64 TxUnderrunCnt;
-	__u64 TxFlowStallCnt;
-	__u64 TxDroppedPktCnt;
-	__u64 RxDroppedPktCnt;
-	__u64 RxDataPktCnt;
-	__u64 RxFlowPktCnt;
-	__u64 RxDwordCnt;
-	__u64 RxLenErrCnt;
-	__u64 RxMaxMinLenErrCnt;
-	__u64 RxICRCErrCnt;
-	__u64 RxVCRCErrCnt;
-	__u64 RxFlowCtrlErrCnt;
-	__u64 RxBadFormatCnt;
-	__u64 RxLinkProblemCnt;
-	__u64 RxEBPCnt;
-	__u64 RxLPCRCErrCnt;
-	__u64 RxBufOvflCnt;
-	__u64 RxTIDFullErrCnt;
-	__u64 RxTIDValidErrCnt;
-	__u64 RxPKeyMismatchCnt;
-	__u64 RxP0HdrEgrOvflCnt;
-	__u64 RxP1HdrEgrOvflCnt;
-	__u64 RxP2HdrEgrOvflCnt;
-	__u64 RxP3HdrEgrOvflCnt;
-	__u64 RxP4HdrEgrOvflCnt;
-	__u64 RxP5HdrEgrOvflCnt;
-	__u64 RxP6HdrEgrOvflCnt;
-	__u64 RxP7HdrEgrOvflCnt;
-	__u64 RxP8HdrEgrOvflCnt;
-	__u64 RxP9HdrEgrOvflCnt;	/* was Reserved6 */
-	__u64 RxP10HdrEgrOvflCnt;	/* was Reserved7 */
-	__u64 RxP11HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 RxP12HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 RxP13HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 RxP14HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 RxP15HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 RxP16HdrEgrOvflCnt;	/* new for IBA7220 */
-	__u64 IBStatusChangeCnt;
-	__u64 IBLinkErrRecoveryCnt;
-	__u64 IBLinkDownedCnt;
-	__u64 IBSymbolErrCnt;
-	/* The following are new for IBA7220 */
-	__u64 RxVL15DroppedPktCnt;
-	__u64 RxOtherLocalPhyErrCnt;
-	__u64 PcieRetryBufDiagQwordCnt;
-	__u64 ExcessBufferOvflCnt;
-	__u64 LocalLinkIntegrityErrCnt;
-	__u64 RxVlErrCnt;
-	__u64 RxDlidFltrCnt;
-};
-
-/*
- * The next set of defines are for packet headers, and chip register
- * and memory bits that are visible to and/or used by user-mode software
- * The other bits that are used only by the driver or diags are in
- * ipath_registers.h
- */
-
-/* RcvHdrFlags bits */
-#define INFINIPATH_RHF_LENGTH_MASK 0x7FF
-#define INFINIPATH_RHF_LENGTH_SHIFT 0
-#define INFINIPATH_RHF_RCVTYPE_MASK 0x7
-#define INFINIPATH_RHF_RCVTYPE_SHIFT 11
-#define INFINIPATH_RHF_EGRINDEX_MASK 0xFFF
-#define INFINIPATH_RHF_EGRINDEX_SHIFT 16
-#define INFINIPATH_RHF_SEQ_MASK 0xF
-#define INFINIPATH_RHF_SEQ_SHIFT 0
-#define INFINIPATH_RHF_HDRQ_OFFSET_MASK 0x7FF
-#define INFINIPATH_RHF_HDRQ_OFFSET_SHIFT 4
-#define INFINIPATH_RHF_H_ICRCERR   0x80000000
-#define INFINIPATH_RHF_H_VCRCERR   0x40000000
-#define INFINIPATH_RHF_H_PARITYERR 0x20000000
-#define INFINIPATH_RHF_H_LENERR    0x10000000
-#define INFINIPATH_RHF_H_MTUERR    0x08000000
-#define INFINIPATH_RHF_H_IHDRERR   0x04000000
-#define INFINIPATH_RHF_H_TIDERR    0x02000000
-#define INFINIPATH_RHF_H_MKERR     0x01000000
-#define INFINIPATH_RHF_H_IBERR     0x00800000
-#define INFINIPATH_RHF_H_ERR_MASK  0xFF800000
-#define INFINIPATH_RHF_L_USE_EGR   0x80000000
-#define INFINIPATH_RHF_L_SWA       0x00008000
-#define INFINIPATH_RHF_L_SWB       0x00004000
-
-/* infinipath header fields */
-#define INFINIPATH_I_VERS_MASK 0xF
-#define INFINIPATH_I_VERS_SHIFT 28
-#define INFINIPATH_I_PORT_MASK 0xF
-#define INFINIPATH_I_PORT_SHIFT 24
-#define INFINIPATH_I_TID_MASK 0x7FF
-#define INFINIPATH_I_TID_SHIFT 13
-#define INFINIPATH_I_OFFSET_MASK 0x1FFF
-#define INFINIPATH_I_OFFSET_SHIFT 0
-
-/* K_PktFlags bits */
-#define INFINIPATH_KPF_INTR 0x1
-#define INFINIPATH_KPF_SUBPORT_MASK 0x3
-#define INFINIPATH_KPF_SUBPORT_SHIFT 1
-
-#define INFINIPATH_MAX_SUBPORT	4
-
-/* SendPIO per-buffer control */
-#define INFINIPATH_SP_TEST    0x40
-#define INFINIPATH_SP_TESTEBP 0x20
-#define INFINIPATH_SP_TRIGGER_SHIFT  15
-
-/* SendPIOAvail bits */
-#define INFINIPATH_SENDPIOAVAIL_BUSY_SHIFT 1
-#define INFINIPATH_SENDPIOAVAIL_CHECK_SHIFT 0
-
-/* infinipath header format */
-struct ipath_header {
-	/*
-	 * Version - 4 bits, Port - 4 bits, TID - 10 bits and Offset -
-	 * 14 bits before ECO change ~28 Dec 03.  After that, Vers 4,
-	 * Port 4, TID 11, offset 13.
-	 */
-	__le32 ver_port_tid_offset;
-	__le16 chksum;
-	__le16 pkt_flags;
-};
-
-/* infinipath user message header format.
- * This structure contains the first 4 fields common to all protocols
- * that employ infinipath.
- */
-struct ipath_message_header {
-	__be16 lrh[4];
-	__be32 bth[3];
-	/* fields below this point are in host byte order */
-	struct ipath_header iph;
-	__u8 sub_opcode;
-};
-
-/* infinipath ethernet header format */
-struct ether_header {
-	__be16 lrh[4];
-	__be32 bth[3];
-	struct ipath_header iph;
-	__u8 sub_opcode;
-	__u8 cmd;
-	__be16 lid;
-	__u16 mac[3];
-	__u8 frag_num;
-	__u8 seq_num;
-	__le32 len;
-	/* MUST be of word size due to PIO write requirements */
-	__le32 csum;
-	__le16 csum_offset;
-	__le16 flags;
-	__u16 first_2_bytes;
-	__u8 unused[2];		/* currently unused */
-};
-
-
-/* IB - LRH header consts */
-#define IPATH_LRH_GRH 0x0003	/* 1. word of IB LRH - next header: GRH */
-#define IPATH_LRH_BTH 0x0002	/* 1. word of IB LRH - next header: BTH */
-
-/* misc. */
-#define SIZE_OF_CRC 1
-
-#define IPATH_DEFAULT_P_KEY 0xFFFF
-#define IPATH_PERMISSIVE_LID 0xFFFF
-#define IPATH_AETH_CREDIT_SHIFT 24
-#define IPATH_AETH_CREDIT_MASK 0x1F
-#define IPATH_AETH_CREDIT_INVAL 0x1F
-#define IPATH_PSN_MASK 0xFFFFFF
-#define IPATH_MSN_MASK 0xFFFFFF
-#define IPATH_QPN_MASK 0xFFFFFF
-#define IPATH_MULTICAST_LID_BASE 0xC000
-#define IPATH_EAGER_TID_ID INFINIPATH_I_TID_MASK
-#define IPATH_MULTICAST_QPN 0xFFFFFF
-
-/* Receive Header Queue: receive type (from infinipath) */
-#define RCVHQ_RCV_TYPE_EXPECTED  0
-#define RCVHQ_RCV_TYPE_EAGER     1
-#define RCVHQ_RCV_TYPE_NON_KD    2
-#define RCVHQ_RCV_TYPE_ERROR     3
-
-
-/* sub OpCodes - ith4x  */
-#define IPATH_ITH4X_OPCODE_ENCAP 0x81
-#define IPATH_ITH4X_OPCODE_LID_ARP 0x82
-
-#define IPATH_HEADER_QUEUE_WORDS 9
-
-/* functions for extracting fields from rcvhdrq entries for the driver.
- */
-static inline __u32 ipath_hdrget_err_flags(const __le32 * rbuf)
-{
-	return __le32_to_cpu(rbuf[1]) & INFINIPATH_RHF_H_ERR_MASK;
-}
-
-static inline __u32 ipath_hdrget_rcv_type(const __le32 * rbuf)
-{
-	return (__le32_to_cpu(rbuf[0]) >> INFINIPATH_RHF_RCVTYPE_SHIFT)
-	    & INFINIPATH_RHF_RCVTYPE_MASK;
-}
-
-static inline __u32 ipath_hdrget_length_in_bytes(const __le32 * rbuf)
-{
-	return ((__le32_to_cpu(rbuf[0]) >> INFINIPATH_RHF_LENGTH_SHIFT)
-		& INFINIPATH_RHF_LENGTH_MASK) << 2;
-}
-
-static inline __u32 ipath_hdrget_index(const __le32 * rbuf)
-{
-	return (__le32_to_cpu(rbuf[0]) >> INFINIPATH_RHF_EGRINDEX_SHIFT)
-	    & INFINIPATH_RHF_EGRINDEX_MASK;
-}
-
-static inline __u32 ipath_hdrget_seq(const __le32 *rbuf)
-{
-	return (__le32_to_cpu(rbuf[1]) >> INFINIPATH_RHF_SEQ_SHIFT)
-		& INFINIPATH_RHF_SEQ_MASK;
-}
-
-static inline __u32 ipath_hdrget_offset(const __le32 *rbuf)
-{
-	return (__le32_to_cpu(rbuf[1]) >> INFINIPATH_RHF_HDRQ_OFFSET_SHIFT)
-		& INFINIPATH_RHF_HDRQ_OFFSET_MASK;
-}
-
-static inline __u32 ipath_hdrget_use_egr_buf(const __le32 *rbuf)
-{
-	return __le32_to_cpu(rbuf[0]) & INFINIPATH_RHF_L_USE_EGR;
-}
-
-static inline __u32 ipath_hdrget_ipath_ver(__le32 hdrword)
-{
-	return (__le32_to_cpu(hdrword) >> INFINIPATH_I_VERS_SHIFT)
-	    & INFINIPATH_I_VERS_MASK;
-}
-
-#endif				/* _IPATH_COMMON_H */
diff --git a/drivers/staging/rdma/ipath/ipath_cq.c b/drivers/staging/rdma/ipath/ipath_cq.c
deleted file mode 100644
index e9dd9112e718..000000000000
--- a/drivers/staging/rdma/ipath/ipath_cq.c
+++ /dev/null
@@ -1,483 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/err.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-
-#include "ipath_verbs.h"
-
-/**
- * ipath_cq_enter - add a new entry to the completion queue
- * @cq: completion queue
- * @entry: work completion entry to add
- * @sig: true if @entry is a solicitated entry
- *
- * This may be called with qp->s_lock held.
- */
-void ipath_cq_enter(struct ipath_cq *cq, struct ib_wc *entry, int solicited)
-{
-	struct ipath_cq_wc *wc;
-	unsigned long flags;
-	u32 head;
-	u32 next;
-
-	spin_lock_irqsave(&cq->lock, flags);
-
-	/*
-	 * Note that the head pointer might be writable by user processes.
-	 * Take care to verify it is a sane value.
-	 */
-	wc = cq->queue;
-	head = wc->head;
-	if (head >= (unsigned) cq->ibcq.cqe) {
-		head = cq->ibcq.cqe;
-		next = 0;
-	} else
-		next = head + 1;
-	if (unlikely(next == wc->tail)) {
-		spin_unlock_irqrestore(&cq->lock, flags);
-		if (cq->ibcq.event_handler) {
-			struct ib_event ev;
-
-			ev.device = cq->ibcq.device;
-			ev.element.cq = &cq->ibcq;
-			ev.event = IB_EVENT_CQ_ERR;
-			cq->ibcq.event_handler(&ev, cq->ibcq.cq_context);
-		}
-		return;
-	}
-	if (cq->ip) {
-		wc->uqueue[head].wr_id = entry->wr_id;
-		wc->uqueue[head].status = entry->status;
-		wc->uqueue[head].opcode = entry->opcode;
-		wc->uqueue[head].vendor_err = entry->vendor_err;
-		wc->uqueue[head].byte_len = entry->byte_len;
-		wc->uqueue[head].ex.imm_data = (__u32 __force) entry->ex.imm_data;
-		wc->uqueue[head].qp_num = entry->qp->qp_num;
-		wc->uqueue[head].src_qp = entry->src_qp;
-		wc->uqueue[head].wc_flags = entry->wc_flags;
-		wc->uqueue[head].pkey_index = entry->pkey_index;
-		wc->uqueue[head].slid = entry->slid;
-		wc->uqueue[head].sl = entry->sl;
-		wc->uqueue[head].dlid_path_bits = entry->dlid_path_bits;
-		wc->uqueue[head].port_num = entry->port_num;
-		/* Make sure entry is written before the head index. */
-		smp_wmb();
-	} else
-		wc->kqueue[head] = *entry;
-	wc->head = next;
-
-	if (cq->notify == IB_CQ_NEXT_COMP ||
-	    (cq->notify == IB_CQ_SOLICITED && solicited)) {
-		cq->notify = IB_CQ_NONE;
-		cq->triggered++;
-		/*
-		 * This will cause send_complete() to be called in
-		 * another thread.
-		 */
-		tasklet_hi_schedule(&cq->comptask);
-	}
-
-	spin_unlock_irqrestore(&cq->lock, flags);
-
-	if (entry->status != IB_WC_SUCCESS)
-		to_idev(cq->ibcq.device)->n_wqe_errs++;
-}
-
-/**
- * ipath_poll_cq - poll for work completion entries
- * @ibcq: the completion queue to poll
- * @num_entries: the maximum number of entries to return
- * @entry: pointer to array where work completions are placed
- *
- * Returns the number of completion entries polled.
- *
- * This may be called from interrupt context.  Also called by ib_poll_cq()
- * in the generic verbs code.
- */
-int ipath_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry)
-{
-	struct ipath_cq *cq = to_icq(ibcq);
-	struct ipath_cq_wc *wc;
-	unsigned long flags;
-	int npolled;
-	u32 tail;
-
-	/* The kernel can only poll a kernel completion queue */
-	if (cq->ip) {
-		npolled = -EINVAL;
-		goto bail;
-	}
-
-	spin_lock_irqsave(&cq->lock, flags);
-
-	wc = cq->queue;
-	tail = wc->tail;
-	if (tail > (u32) cq->ibcq.cqe)
-		tail = (u32) cq->ibcq.cqe;
-	for (npolled = 0; npolled < num_entries; ++npolled, ++entry) {
-		if (tail == wc->head)
-			break;
-		/* The kernel doesn't need a RMB since it has the lock. */
-		*entry = wc->kqueue[tail];
-		if (tail >= cq->ibcq.cqe)
-			tail = 0;
-		else
-			tail++;
-	}
-	wc->tail = tail;
-
-	spin_unlock_irqrestore(&cq->lock, flags);
-
-bail:
-	return npolled;
-}
-
-static void send_complete(unsigned long data)
-{
-	struct ipath_cq *cq = (struct ipath_cq *)data;
-
-	/*
-	 * The completion handler will most likely rearm the notification
-	 * and poll for all pending entries.  If a new completion entry
-	 * is added while we are in this routine, tasklet_hi_schedule()
-	 * won't call us again until we return so we check triggered to
-	 * see if we need to call the handler again.
-	 */
-	for (;;) {
-		u8 triggered = cq->triggered;
-
-		cq->ibcq.comp_handler(&cq->ibcq, cq->ibcq.cq_context);
-
-		if (cq->triggered == triggered)
-			return;
-	}
-}
-
-/**
- * ipath_create_cq - create a completion queue
- * @ibdev: the device this completion queue is attached to
- * @attr: creation attributes
- * @context: unused by the InfiniPath driver
- * @udata: unused by the InfiniPath driver
- *
- * Returns a pointer to the completion queue or negative errno values
- * for failure.
- *
- * Called by ib_create_cq() in the generic verbs code.
- */
-struct ib_cq *ipath_create_cq(struct ib_device *ibdev,
-			      const struct ib_cq_init_attr *attr,
-			      struct ib_ucontext *context,
-			      struct ib_udata *udata)
-{
-	int entries = attr->cqe;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_cq *cq;
-	struct ipath_cq_wc *wc;
-	struct ib_cq *ret;
-	u32 sz;
-
-	if (attr->flags)
-		return ERR_PTR(-EINVAL);
-
-	if (entries < 1 || entries > ib_ipath_max_cqes) {
-		ret = ERR_PTR(-EINVAL);
-		goto done;
-	}
-
-	/* Allocate the completion queue structure. */
-	cq = kmalloc(sizeof(*cq), GFP_KERNEL);
-	if (!cq) {
-		ret = ERR_PTR(-ENOMEM);
-		goto done;
-	}
-
-	/*
-	 * Allocate the completion queue entries and head/tail pointers.
-	 * This is allocated separately so that it can be resized and
-	 * also mapped into user space.
-	 * We need to use vmalloc() in order to support mmap and large
-	 * numbers of entries.
-	 */
-	sz = sizeof(*wc);
-	if (udata && udata->outlen >= sizeof(__u64))
-		sz += sizeof(struct ib_uverbs_wc) * (entries + 1);
-	else
-		sz += sizeof(struct ib_wc) * (entries + 1);
-	wc = vmalloc_user(sz);
-	if (!wc) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail_cq;
-	}
-
-	/*
-	 * Return the address of the WC as the offset to mmap.
-	 * See ipath_mmap() for details.
-	 */
-	if (udata && udata->outlen >= sizeof(__u64)) {
-		int err;
-
-		cq->ip = ipath_create_mmap_info(dev, sz, context, wc);
-		if (!cq->ip) {
-			ret = ERR_PTR(-ENOMEM);
-			goto bail_wc;
-		}
-
-		err = ib_copy_to_udata(udata, &cq->ip->offset,
-				       sizeof(cq->ip->offset));
-		if (err) {
-			ret = ERR_PTR(err);
-			goto bail_ip;
-		}
-	} else
-		cq->ip = NULL;
-
-	spin_lock(&dev->n_cqs_lock);
-	if (dev->n_cqs_allocated == ib_ipath_max_cqs) {
-		spin_unlock(&dev->n_cqs_lock);
-		ret = ERR_PTR(-ENOMEM);
-		goto bail_ip;
-	}
-
-	dev->n_cqs_allocated++;
-	spin_unlock(&dev->n_cqs_lock);
-
-	if (cq->ip) {
-		spin_lock_irq(&dev->pending_lock);
-		list_add(&cq->ip->pending_mmaps, &dev->pending_mmaps);
-		spin_unlock_irq(&dev->pending_lock);
-	}
-
-	/*
-	 * ib_create_cq() will initialize cq->ibcq except for cq->ibcq.cqe.
-	 * The number of entries should be >= the number requested or return
-	 * an error.
-	 */
-	cq->ibcq.cqe = entries;
-	cq->notify = IB_CQ_NONE;
-	cq->triggered = 0;
-	spin_lock_init(&cq->lock);
-	tasklet_init(&cq->comptask, send_complete, (unsigned long)cq);
-	wc->head = 0;
-	wc->tail = 0;
-	cq->queue = wc;
-
-	ret = &cq->ibcq;
-
-	goto done;
-
-bail_ip:
-	kfree(cq->ip);
-bail_wc:
-	vfree(wc);
-bail_cq:
-	kfree(cq);
-done:
-	return ret;
-}
-
-/**
- * ipath_destroy_cq - destroy a completion queue
- * @ibcq: the completion queue to destroy.
- *
- * Returns 0 for success.
- *
- * Called by ib_destroy_cq() in the generic verbs code.
- */
-int ipath_destroy_cq(struct ib_cq *ibcq)
-{
-	struct ipath_ibdev *dev = to_idev(ibcq->device);
-	struct ipath_cq *cq = to_icq(ibcq);
-
-	tasklet_kill(&cq->comptask);
-	spin_lock(&dev->n_cqs_lock);
-	dev->n_cqs_allocated--;
-	spin_unlock(&dev->n_cqs_lock);
-	if (cq->ip)
-		kref_put(&cq->ip->ref, ipath_release_mmap_info);
-	else
-		vfree(cq->queue);
-	kfree(cq);
-
-	return 0;
-}
-
-/**
- * ipath_req_notify_cq - change the notification type for a completion queue
- * @ibcq: the completion queue
- * @notify_flags: the type of notification to request
- *
- * Returns 0 for success.
- *
- * This may be called from interrupt context.  Also called by
- * ib_req_notify_cq() in the generic verbs code.
- */
-int ipath_req_notify_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags notify_flags)
-{
-	struct ipath_cq *cq = to_icq(ibcq);
-	unsigned long flags;
-	int ret = 0;
-
-	spin_lock_irqsave(&cq->lock, flags);
-	/*
-	 * Don't change IB_CQ_NEXT_COMP to IB_CQ_SOLICITED but allow
-	 * any other transitions (see C11-31 and C11-32 in ch. 11.4.2.2).
-	 */
-	if (cq->notify != IB_CQ_NEXT_COMP)
-		cq->notify = notify_flags & IB_CQ_SOLICITED_MASK;
-
-	if ((notify_flags & IB_CQ_REPORT_MISSED_EVENTS) &&
-	    cq->queue->head != cq->queue->tail)
-		ret = 1;
-
-	spin_unlock_irqrestore(&cq->lock, flags);
-
-	return ret;
-}
-
-/**
- * ipath_resize_cq - change the size of the CQ
- * @ibcq: the completion queue
- *
- * Returns 0 for success.
- */
-int ipath_resize_cq(struct ib_cq *ibcq, int cqe, struct ib_udata *udata)
-{
-	struct ipath_cq *cq = to_icq(ibcq);
-	struct ipath_cq_wc *old_wc;
-	struct ipath_cq_wc *wc;
-	u32 head, tail, n;
-	int ret;
-	u32 sz;
-
-	if (cqe < 1 || cqe > ib_ipath_max_cqes) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	/*
-	 * Need to use vmalloc() if we want to support large #s of entries.
-	 */
-	sz = sizeof(*wc);
-	if (udata && udata->outlen >= sizeof(__u64))
-		sz += sizeof(struct ib_uverbs_wc) * (cqe + 1);
-	else
-		sz += sizeof(struct ib_wc) * (cqe + 1);
-	wc = vmalloc_user(sz);
-	if (!wc) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	/* Check that we can write the offset to mmap. */
-	if (udata && udata->outlen >= sizeof(__u64)) {
-		__u64 offset = 0;
-
-		ret = ib_copy_to_udata(udata, &offset, sizeof(offset));
-		if (ret)
-			goto bail_free;
-	}
-
-	spin_lock_irq(&cq->lock);
-	/*
-	 * Make sure head and tail are sane since they
-	 * might be user writable.
-	 */
-	old_wc = cq->queue;
-	head = old_wc->head;
-	if (head > (u32) cq->ibcq.cqe)
-		head = (u32) cq->ibcq.cqe;
-	tail = old_wc->tail;
-	if (tail > (u32) cq->ibcq.cqe)
-		tail = (u32) cq->ibcq.cqe;
-	if (head < tail)
-		n = cq->ibcq.cqe + 1 + head - tail;
-	else
-		n = head - tail;
-	if (unlikely((u32)cqe < n)) {
-		ret = -EINVAL;
-		goto bail_unlock;
-	}
-	for (n = 0; tail != head; n++) {
-		if (cq->ip)
-			wc->uqueue[n] = old_wc->uqueue[tail];
-		else
-			wc->kqueue[n] = old_wc->kqueue[tail];
-		if (tail == (u32) cq->ibcq.cqe)
-			tail = 0;
-		else
-			tail++;
-	}
-	cq->ibcq.cqe = cqe;
-	wc->head = n;
-	wc->tail = 0;
-	cq->queue = wc;
-	spin_unlock_irq(&cq->lock);
-
-	vfree(old_wc);
-
-	if (cq->ip) {
-		struct ipath_ibdev *dev = to_idev(ibcq->device);
-		struct ipath_mmap_info *ip = cq->ip;
-
-		ipath_update_mmap_info(dev, ip, sz, wc);
-
-		/*
-		 * Return the offset to mmap.
-		 * See ipath_mmap() for details.
-		 */
-		if (udata && udata->outlen >= sizeof(__u64)) {
-			ret = ib_copy_to_udata(udata, &ip->offset,
-					       sizeof(ip->offset));
-			if (ret)
-				goto bail;
-		}
-
-		spin_lock_irq(&dev->pending_lock);
-		if (list_empty(&ip->pending_mmaps))
-			list_add(&ip->pending_mmaps, &dev->pending_mmaps);
-		spin_unlock_irq(&dev->pending_lock);
-	}
-
-	ret = 0;
-	goto bail;
-
-bail_unlock:
-	spin_unlock_irq(&cq->lock);
-bail_free:
-	vfree(wc);
-bail:
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_debug.h b/drivers/staging/rdma/ipath/ipath_debug.h
deleted file mode 100644
index 65926cd35759..000000000000
--- a/drivers/staging/rdma/ipath/ipath_debug.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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 _IPATH_DEBUG_H
-#define _IPATH_DEBUG_H
-
-#ifndef _IPATH_DEBUGGING	/* debugging enabled or not */
-#define _IPATH_DEBUGGING 1
-#endif
-
-#if _IPATH_DEBUGGING
-
-/*
- * Mask values for debugging.  The scheme allows us to compile out any
- * of the debug tracing stuff, and if compiled in, to enable or disable
- * dynamically.  This can be set at modprobe time also:
- *      modprobe infinipath.ko infinipath_debug=7
- */
-
-#define __IPATH_INFO        0x1	/* generic low verbosity stuff */
-#define __IPATH_DBG         0x2	/* generic debug */
-#define __IPATH_TRSAMPLE    0x8	/* generate trace buffer sample entries */
-/* leave some low verbosity spots open */
-#define __IPATH_VERBDBG     0x40	/* very verbose debug */
-#define __IPATH_PKTDBG      0x80	/* print packet data */
-/* print process startup (init)/exit messages */
-#define __IPATH_PROCDBG     0x100
-/* print mmap/fault stuff, not using VDBG any more */
-#define __IPATH_MMDBG       0x200
-#define __IPATH_ERRPKTDBG   0x400
-#define __IPATH_USER_SEND   0x1000	/* use user mode send */
-#define __IPATH_KERNEL_SEND 0x2000	/* use kernel mode send */
-#define __IPATH_EPKTDBG     0x4000	/* print ethernet packet data */
-#define __IPATH_IPATHDBG    0x10000	/* Ethernet (IPATH) gen debug */
-#define __IPATH_IPATHWARN   0x20000	/* Ethernet (IPATH) warnings */
-#define __IPATH_IPATHERR    0x40000	/* Ethernet (IPATH) errors */
-#define __IPATH_IPATHPD     0x80000	/* Ethernet (IPATH) packet dump */
-#define __IPATH_IPATHTABLE  0x100000	/* Ethernet (IPATH) table dump */
-#define __IPATH_LINKVERBDBG 0x200000	/* very verbose linkchange debug */
-
-#else				/* _IPATH_DEBUGGING */
-
-/*
- * define all of these even with debugging off, for the few places that do
- * if(infinipath_debug & _IPATH_xyzzy), but in a way that will make the
- * compiler eliminate the code
- */
-
-#define __IPATH_INFO      0x0	/* generic low verbosity stuff */
-#define __IPATH_DBG       0x0	/* generic debug */
-#define __IPATH_TRSAMPLE  0x0	/* generate trace buffer sample entries */
-#define __IPATH_VERBDBG   0x0	/* very verbose debug */
-#define __IPATH_PKTDBG    0x0	/* print packet data */
-#define __IPATH_PROCDBG   0x0	/* process startup (init)/exit messages */
-/* print mmap/fault stuff, not using VDBG any more */
-#define __IPATH_MMDBG     0x0
-#define __IPATH_EPKTDBG   0x0	/* print ethernet packet data */
-#define __IPATH_IPATHDBG  0x0	/* Ethernet (IPATH) table dump on */
-#define __IPATH_IPATHWARN 0x0	/* Ethernet (IPATH) warnings on   */
-#define __IPATH_IPATHERR  0x0	/* Ethernet (IPATH) errors on   */
-#define __IPATH_IPATHPD   0x0	/* Ethernet (IPATH) packet dump on   */
-#define __IPATH_IPATHTABLE 0x0	/* Ethernet (IPATH) packet dump on   */
-#define __IPATH_LINKVERBDBG 0x0	/* very verbose linkchange debug */
-
-#endif				/* _IPATH_DEBUGGING */
-
-#define __IPATH_VERBOSEDBG __IPATH_VERBDBG
-
-#endif				/* _IPATH_DEBUG_H */
diff --git a/drivers/staging/rdma/ipath/ipath_diag.c b/drivers/staging/rdma/ipath/ipath_diag.c
deleted file mode 100644
index 45802e97332e..000000000000
--- a/drivers/staging/rdma/ipath/ipath_diag.c
+++ /dev/null
@@ -1,551 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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.
- */
-
-/*
- * This file contains support for diagnostic functions.  It is accessed by
- * opening the ipath_diag device, normally minor number 129.  Diagnostic use
- * of the InfiniPath chip may render the chip or board unusable until the
- * driver is unloaded, or in some cases, until the system is rebooted.
- *
- * Accesses to the chip through this interface are not similar to going
- * through the /sys/bus/pci resource mmap interface.
- */
-
-#include <linux/io.h>
-#include <linux/pci.h>
-#include <linux/vmalloc.h>
-#include <linux/fs.h>
-#include <linux/export.h>
-#include <asm/uaccess.h>
-
-#include "ipath_kernel.h"
-#include "ipath_common.h"
-
-int ipath_diag_inuse;
-static int diag_set_link;
-
-static int ipath_diag_open(struct inode *in, struct file *fp);
-static int ipath_diag_release(struct inode *in, struct file *fp);
-static ssize_t ipath_diag_read(struct file *fp, char __user *data,
-			       size_t count, loff_t *off);
-static ssize_t ipath_diag_write(struct file *fp, const char __user *data,
-				size_t count, loff_t *off);
-
-static const struct file_operations diag_file_ops = {
-	.owner = THIS_MODULE,
-	.write = ipath_diag_write,
-	.read = ipath_diag_read,
-	.open = ipath_diag_open,
-	.release = ipath_diag_release,
-	.llseek = default_llseek,
-};
-
-static ssize_t ipath_diagpkt_write(struct file *fp,
-				   const char __user *data,
-				   size_t count, loff_t *off);
-
-static const struct file_operations diagpkt_file_ops = {
-	.owner = THIS_MODULE,
-	.write = ipath_diagpkt_write,
-	.llseek = noop_llseek,
-};
-
-static atomic_t diagpkt_count = ATOMIC_INIT(0);
-static struct cdev *diagpkt_cdev;
-static struct device *diagpkt_dev;
-
-int ipath_diag_add(struct ipath_devdata *dd)
-{
-	char name[16];
-	int ret = 0;
-
-	if (atomic_inc_return(&diagpkt_count) == 1) {
-		ret = ipath_cdev_init(IPATH_DIAGPKT_MINOR,
-				      "ipath_diagpkt", &diagpkt_file_ops,
-				      &diagpkt_cdev, &diagpkt_dev);
-
-		if (ret) {
-			ipath_dev_err(dd, "Couldn't create ipath_diagpkt "
-				      "device: %d", ret);
-			goto done;
-		}
-	}
-
-	snprintf(name, sizeof(name), "ipath_diag%d", dd->ipath_unit);
-
-	ret = ipath_cdev_init(IPATH_DIAG_MINOR_BASE + dd->ipath_unit, name,
-			      &diag_file_ops, &dd->diag_cdev,
-			      &dd->diag_dev);
-	if (ret)
-		ipath_dev_err(dd, "Couldn't create %s device: %d",
-			      name, ret);
-
-done:
-	return ret;
-}
-
-void ipath_diag_remove(struct ipath_devdata *dd)
-{
-	if (atomic_dec_and_test(&diagpkt_count))
-		ipath_cdev_cleanup(&diagpkt_cdev, &diagpkt_dev);
-
-	ipath_cdev_cleanup(&dd->diag_cdev, &dd->diag_dev);
-}
-
-/**
- * ipath_read_umem64 - read a 64-bit quantity from the chip into user space
- * @dd: the infinipath device
- * @uaddr: the location to store the data in user memory
- * @caddr: the source chip address (full pointer, not offset)
- * @count: number of bytes to copy (multiple of 32 bits)
- *
- * This function also localizes all chip memory accesses.
- * The copy should be written such that we read full cacheline packets
- * from the chip.  This is usually used for a single qword
- *
- * NOTE:  This assumes the chip address is 64-bit aligned.
- */
-static int ipath_read_umem64(struct ipath_devdata *dd, void __user *uaddr,
-			     const void __iomem *caddr, size_t count)
-{
-	const u64 __iomem *reg_addr = caddr;
-	const u64 __iomem *reg_end = reg_addr + (count / sizeof(u64));
-	int ret;
-
-	/* not very efficient, but it works for now */
-	if (reg_addr < dd->ipath_kregbase || reg_end > dd->ipath_kregend) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	while (reg_addr < reg_end) {
-		u64 data = readq(reg_addr);
-		if (copy_to_user(uaddr, &data, sizeof(u64))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-		reg_addr++;
-		uaddr += sizeof(u64);
-	}
-	ret = 0;
-bail:
-	return ret;
-}
-
-/**
- * ipath_write_umem64 - write a 64-bit quantity to the chip from user space
- * @dd: the infinipath device
- * @caddr: the destination chip address (full pointer, not offset)
- * @uaddr: the source of the data in user memory
- * @count: the number of bytes to copy (multiple of 32 bits)
- *
- * This is usually used for a single qword
- * NOTE:  This assumes the chip address is 64-bit aligned.
- */
-
-static int ipath_write_umem64(struct ipath_devdata *dd, void __iomem *caddr,
-			      const void __user *uaddr, size_t count)
-{
-	u64 __iomem *reg_addr = caddr;
-	const u64 __iomem *reg_end = reg_addr + (count / sizeof(u64));
-	int ret;
-
-	/* not very efficient, but it works for now */
-	if (reg_addr < dd->ipath_kregbase || reg_end > dd->ipath_kregend) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	while (reg_addr < reg_end) {
-		u64 data;
-		if (copy_from_user(&data, uaddr, sizeof(data))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-		writeq(data, reg_addr);
-
-		reg_addr++;
-		uaddr += sizeof(u64);
-	}
-	ret = 0;
-bail:
-	return ret;
-}
-
-/**
- * ipath_read_umem32 - read a 32-bit quantity from the chip into user space
- * @dd: the infinipath device
- * @uaddr: the location to store the data in user memory
- * @caddr: the source chip address (full pointer, not offset)
- * @count: number of bytes to copy
- *
- * read 32 bit values, not 64 bit; for memories that only
- * support 32 bit reads; usually a single dword.
- */
-static int ipath_read_umem32(struct ipath_devdata *dd, void __user *uaddr,
-			     const void __iomem *caddr, size_t count)
-{
-	const u32 __iomem *reg_addr = caddr;
-	const u32 __iomem *reg_end = reg_addr + (count / sizeof(u32));
-	int ret;
-
-	if (reg_addr < (u32 __iomem *) dd->ipath_kregbase ||
-	    reg_end > (u32 __iomem *) dd->ipath_kregend) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	/* not very efficient, but it works for now */
-	while (reg_addr < reg_end) {
-		u32 data = readl(reg_addr);
-		if (copy_to_user(uaddr, &data, sizeof(data))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-
-		reg_addr++;
-		uaddr += sizeof(u32);
-
-	}
-	ret = 0;
-bail:
-	return ret;
-}
-
-/**
- * ipath_write_umem32 - write a 32-bit quantity to the chip from user space
- * @dd: the infinipath device
- * @caddr: the destination chip address (full pointer, not offset)
- * @uaddr: the source of the data in user memory
- * @count: number of bytes to copy
- *
- * write 32 bit values, not 64 bit; for memories that only
- * support 32 bit write; usually a single dword.
- */
-
-static int ipath_write_umem32(struct ipath_devdata *dd, void __iomem *caddr,
-			      const void __user *uaddr, size_t count)
-{
-	u32 __iomem *reg_addr = caddr;
-	const u32 __iomem *reg_end = reg_addr + (count / sizeof(u32));
-	int ret;
-
-	if (reg_addr < (u32 __iomem *) dd->ipath_kregbase ||
-	    reg_end > (u32 __iomem *) dd->ipath_kregend) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	while (reg_addr < reg_end) {
-		u32 data;
-		if (copy_from_user(&data, uaddr, sizeof(data))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-		writel(data, reg_addr);
-
-		reg_addr++;
-		uaddr += sizeof(u32);
-	}
-	ret = 0;
-bail:
-	return ret;
-}
-
-static int ipath_diag_open(struct inode *in, struct file *fp)
-{
-	int unit = iminor(in) - IPATH_DIAG_MINOR_BASE;
-	struct ipath_devdata *dd;
-	int ret;
-
-	mutex_lock(&ipath_mutex);
-
-	if (ipath_diag_inuse) {
-		ret = -EBUSY;
-		goto bail;
-	}
-
-	dd = ipath_lookup(unit);
-
-	if (dd == NULL || !(dd->ipath_flags & IPATH_PRESENT) ||
-	    !dd->ipath_kregbase) {
-		ret = -ENODEV;
-		goto bail;
-	}
-
-	fp->private_data = dd;
-	ipath_diag_inuse = -2;
-	diag_set_link = 0;
-	ret = 0;
-
-	/* Only expose a way to reset the device if we
-	   make it into diag mode. */
-	ipath_expose_reset(&dd->pcidev->dev);
-
-bail:
-	mutex_unlock(&ipath_mutex);
-
-	return ret;
-}
-
-/**
- * ipath_diagpkt_write - write an IB packet
- * @fp: the diag data device file pointer
- * @data: ipath_diag_pkt structure saying where to get the packet
- * @count: size of data to write
- * @off: unused by this code
- */
-static ssize_t ipath_diagpkt_write(struct file *fp,
-				   const char __user *data,
-				   size_t count, loff_t *off)
-{
-	u32 __iomem *piobuf;
-	u32 plen, pbufn, maxlen_reserve;
-	struct ipath_diag_pkt odp;
-	struct ipath_diag_xpkt dp;
-	u32 *tmpbuf = NULL;
-	struct ipath_devdata *dd;
-	ssize_t ret = 0;
-	u64 val;
-	u32 l_state, lt_state; /* LinkState, LinkTrainingState */
-
-
-	if (count == sizeof(dp)) {
-		if (copy_from_user(&dp, data, sizeof(dp))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-	} else if (count == sizeof(odp)) {
-		if (copy_from_user(&odp, data, sizeof(odp))) {
-			ret = -EFAULT;
-			goto bail;
-		}
-		dp.len = odp.len;
-		dp.unit = odp.unit;
-		dp.data = odp.data;
-		dp.pbc_wd = 0;
-	} else {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	/* send count must be an exact number of dwords */
-	if (dp.len & 3) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	plen = dp.len >> 2;
-
-	dd = ipath_lookup(dp.unit);
-	if (!dd || !(dd->ipath_flags & IPATH_PRESENT) ||
-	    !dd->ipath_kregbase) {
-		ipath_cdbg(VERBOSE, "illegal unit %u for diag data send\n",
-			   dp.unit);
-		ret = -ENODEV;
-		goto bail;
-	}
-
-	if (ipath_diag_inuse && !diag_set_link &&
-	    !(dd->ipath_flags & IPATH_LINKACTIVE)) {
-		diag_set_link = 1;
-		ipath_cdbg(VERBOSE, "Trying to set to set link active for "
-			   "diag pkt\n");
-		ipath_set_linkstate(dd, IPATH_IB_LINKARM);
-		ipath_set_linkstate(dd, IPATH_IB_LINKACTIVE);
-	}
-
-	if (!(dd->ipath_flags & IPATH_INITTED)) {
-		/* no hardware, freeze, etc. */
-		ipath_cdbg(VERBOSE, "unit %u not usable\n", dd->ipath_unit);
-		ret = -ENODEV;
-		goto bail;
-	}
-	/*
-	 * Want to skip check for l_state if using custom PBC,
-	 * because we might be trying to force an SM packet out.
-	 * first-cut, skip _all_ state checking in that case.
-	 */
-	val = ipath_ib_state(dd, dd->ipath_lastibcstat);
-	lt_state = ipath_ib_linktrstate(dd, dd->ipath_lastibcstat);
-	l_state = ipath_ib_linkstate(dd, dd->ipath_lastibcstat);
-	if (!dp.pbc_wd && (lt_state != INFINIPATH_IBCS_LT_STATE_LINKUP ||
-	    (val != dd->ib_init && val != dd->ib_arm &&
-	    val != dd->ib_active))) {
-		ipath_cdbg(VERBOSE, "unit %u not ready (state %llx)\n",
-			   dd->ipath_unit, (unsigned long long) val);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	/*
-	 * need total length before first word written, plus 2 Dwords. One Dword
-	 * is for padding so we get the full user data when not aligned on
-	 * a word boundary. The other Dword is to make sure we have room for the
-	 * ICRC which gets tacked on later.
-	 */
-	maxlen_reserve = 2 * sizeof(u32);
-	if (dp.len > dd->ipath_ibmaxlen - maxlen_reserve) {
-		ipath_dbg("Pkt len 0x%x > ibmaxlen %x\n",
-			  dp.len, dd->ipath_ibmaxlen);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	plen = sizeof(u32) + dp.len;
-
-	tmpbuf = vmalloc(plen);
-	if (!tmpbuf) {
-		dev_info(&dd->pcidev->dev, "Unable to allocate tmp buffer, "
-			 "failing\n");
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	if (copy_from_user(tmpbuf,
-			   (const void __user *) (unsigned long) dp.data,
-			   dp.len)) {
-		ret = -EFAULT;
-		goto bail;
-	}
-
-	plen >>= 2;		/* in dwords */
-
-	piobuf = ipath_getpiobuf(dd, plen, &pbufn);
-	if (!piobuf) {
-		ipath_cdbg(VERBOSE, "No PIO buffers avail unit for %u\n",
-			   dd->ipath_unit);
-		ret = -EBUSY;
-		goto bail;
-	}
-	/* disarm it just to be extra sure */
-	ipath_disarm_piobufs(dd, pbufn, 1);
-
-	if (ipath_debug & __IPATH_PKTDBG)
-		ipath_cdbg(VERBOSE, "unit %u 0x%x+1w pio%d\n",
-			   dd->ipath_unit, plen - 1, pbufn);
-
-	if (dp.pbc_wd == 0)
-		dp.pbc_wd = plen;
-	writeq(dp.pbc_wd, piobuf);
-	/*
-	 * Copy all by the trigger word, then flush, so it's written
-	 * to chip before trigger word, then write trigger word, then
-	 * flush again, so packet is sent.
-	 */
-	if (dd->ipath_flags & IPATH_PIO_FLUSH_WC) {
-		ipath_flush_wc();
-		__iowrite32_copy(piobuf + 2, tmpbuf, plen - 1);
-		ipath_flush_wc();
-		__raw_writel(tmpbuf[plen - 1], piobuf + plen + 1);
-	} else
-		__iowrite32_copy(piobuf + 2, tmpbuf, plen);
-
-	ipath_flush_wc();
-
-	ret = sizeof(dp);
-
-bail:
-	vfree(tmpbuf);
-	return ret;
-}
-
-static int ipath_diag_release(struct inode *in, struct file *fp)
-{
-	mutex_lock(&ipath_mutex);
-	ipath_diag_inuse = 0;
-	fp->private_data = NULL;
-	mutex_unlock(&ipath_mutex);
-	return 0;
-}
-
-static ssize_t ipath_diag_read(struct file *fp, char __user *data,
-			       size_t count, loff_t *off)
-{
-	struct ipath_devdata *dd = fp->private_data;
-	void __iomem *kreg_base;
-	ssize_t ret;
-
-	kreg_base = dd->ipath_kregbase;
-
-	if (count == 0)
-		ret = 0;
-	else if ((count % 4) || (*off % 4))
-		/* address or length is not 32-bit aligned, hence invalid */
-		ret = -EINVAL;
-	else if (ipath_diag_inuse < 1 && (*off || count != 8))
-		ret = -EINVAL;  /* prevent cat /dev/ipath_diag* */
-	else if ((count % 8) || (*off % 8))
-		/* address or length not 64-bit aligned; do 32-bit reads */
-		ret = ipath_read_umem32(dd, data, kreg_base + *off, count);
-	else
-		ret = ipath_read_umem64(dd, data, kreg_base + *off, count);
-
-	if (ret >= 0) {
-		*off += count;
-		ret = count;
-		if (ipath_diag_inuse == -2)
-			ipath_diag_inuse++;
-	}
-
-	return ret;
-}
-
-static ssize_t ipath_diag_write(struct file *fp, const char __user *data,
-				size_t count, loff_t *off)
-{
-	struct ipath_devdata *dd = fp->private_data;
-	void __iomem *kreg_base;
-	ssize_t ret;
-
-	kreg_base = dd->ipath_kregbase;
-
-	if (count == 0)
-		ret = 0;
-	else if ((count % 4) || (*off % 4))
-		/* address or length is not 32-bit aligned, hence invalid */
-		ret = -EINVAL;
-	else if ((ipath_diag_inuse == -1 && (*off || count != 8)) ||
-		 ipath_diag_inuse == -2)  /* read qw off 0, write qw off 0 */
-		ret = -EINVAL;  /* before any other write allowed */
-	else if ((count % 8) || (*off % 8))
-		/* address or length not 64-bit aligned; do 32-bit writes */
-		ret = ipath_write_umem32(dd, kreg_base + *off, data, count);
-	else
-		ret = ipath_write_umem64(dd, kreg_base + *off, data, count);
-
-	if (ret >= 0) {
-		*off += count;
-		ret = count;
-		if (ipath_diag_inuse == -1)
-			ipath_diag_inuse = 1; /* all read/write OK now */
-	}
-
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_dma.c b/drivers/staging/rdma/ipath/ipath_dma.c
deleted file mode 100644
index 123a8c053539..000000000000
--- a/drivers/staging/rdma/ipath/ipath_dma.c
+++ /dev/null
@@ -1,179 +0,0 @@
-/*
- * Copyright (c) 2006 QLogic, Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <linux/scatterlist.h>
-#include <linux/gfp.h>
-#include <rdma/ib_verbs.h>
-
-#include "ipath_verbs.h"
-
-#define BAD_DMA_ADDRESS ((u64) 0)
-
-/*
- * The following functions implement driver specific replacements
- * for the ib_dma_*() functions.
- *
- * These functions return kernel virtual addresses instead of
- * device bus addresses since the driver uses the CPU to copy
- * data instead of using hardware DMA.
- */
-
-static int ipath_mapping_error(struct ib_device *dev, u64 dma_addr)
-{
-	return dma_addr == BAD_DMA_ADDRESS;
-}
-
-static u64 ipath_dma_map_single(struct ib_device *dev,
-			        void *cpu_addr, size_t size,
-			        enum dma_data_direction direction)
-{
-	BUG_ON(!valid_dma_direction(direction));
-	return (u64) cpu_addr;
-}
-
-static void ipath_dma_unmap_single(struct ib_device *dev,
-				   u64 addr, size_t size,
-				   enum dma_data_direction direction)
-{
-	BUG_ON(!valid_dma_direction(direction));
-}
-
-static u64 ipath_dma_map_page(struct ib_device *dev,
-			      struct page *page,
-			      unsigned long offset,
-			      size_t size,
-			      enum dma_data_direction direction)
-{
-	u64 addr;
-
-	BUG_ON(!valid_dma_direction(direction));
-
-	if (offset + size > PAGE_SIZE) {
-		addr = BAD_DMA_ADDRESS;
-		goto done;
-	}
-
-	addr = (u64) page_address(page);
-	if (addr)
-		addr += offset;
-	/* TODO: handle highmem pages */
-
-done:
-	return addr;
-}
-
-static void ipath_dma_unmap_page(struct ib_device *dev,
-				 u64 addr, size_t size,
-				 enum dma_data_direction direction)
-{
-	BUG_ON(!valid_dma_direction(direction));
-}
-
-static int ipath_map_sg(struct ib_device *dev, struct scatterlist *sgl,
-			int nents, enum dma_data_direction direction)
-{
-	struct scatterlist *sg;
-	u64 addr;
-	int i;
-	int ret = nents;
-
-	BUG_ON(!valid_dma_direction(direction));
-
-	for_each_sg(sgl, sg, nents, i) {
-		addr = (u64) page_address(sg_page(sg));
-		/* TODO: handle highmem pages */
-		if (!addr) {
-			ret = 0;
-			break;
-		}
-		sg->dma_address = addr + sg->offset;
-#ifdef CONFIG_NEED_SG_DMA_LENGTH
-		sg->dma_length = sg->length;
-#endif
-	}
-	return ret;
-}
-
-static void ipath_unmap_sg(struct ib_device *dev,
-			   struct scatterlist *sg, int nents,
-			   enum dma_data_direction direction)
-{
-	BUG_ON(!valid_dma_direction(direction));
-}
-
-static void ipath_sync_single_for_cpu(struct ib_device *dev,
-				      u64 addr,
-				      size_t size,
-				      enum dma_data_direction dir)
-{
-}
-
-static void ipath_sync_single_for_device(struct ib_device *dev,
-					 u64 addr,
-					 size_t size,
-					 enum dma_data_direction dir)
-{
-}
-
-static void *ipath_dma_alloc_coherent(struct ib_device *dev, size_t size,
-				      u64 *dma_handle, gfp_t flag)
-{
-	struct page *p;
-	void *addr = NULL;
-
-	p = alloc_pages(flag, get_order(size));
-	if (p)
-		addr = page_address(p);
-	if (dma_handle)
-		*dma_handle = (u64) addr;
-	return addr;
-}
-
-static void ipath_dma_free_coherent(struct ib_device *dev, size_t size,
-				    void *cpu_addr, u64 dma_handle)
-{
-	free_pages((unsigned long) cpu_addr, get_order(size));
-}
-
-struct ib_dma_mapping_ops ipath_dma_mapping_ops = {
-	.mapping_error = ipath_mapping_error,
-	.map_single = ipath_dma_map_single,
-	.unmap_single = ipath_dma_unmap_single,
-	.map_page = ipath_dma_map_page,
-	.unmap_page = ipath_dma_unmap_page,
-	.map_sg = ipath_map_sg,
-	.unmap_sg = ipath_unmap_sg,
-	.sync_single_for_cpu = ipath_sync_single_for_cpu,
-	.sync_single_for_device = ipath_sync_single_for_device,
-	.alloc_coherent = ipath_dma_alloc_coherent,
-	.free_coherent = ipath_dma_free_coherent
-};
diff --git a/drivers/staging/rdma/ipath/ipath_driver.c b/drivers/staging/rdma/ipath/ipath_driver.c
deleted file mode 100644
index 2ab22f98e3ba..000000000000
--- a/drivers/staging/rdma/ipath/ipath_driver.c
+++ /dev/null
@@ -1,2784 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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.
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/spinlock.h>
-#include <linux/idr.h>
-#include <linux/pci.h>
-#include <linux/io.h>
-#include <linux/delay.h>
-#include <linux/netdevice.h>
-#include <linux/vmalloc.h>
-#include <linux/bitmap.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#ifdef CONFIG_X86_64
-#include <asm/pat.h>
-#endif
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-
-static void ipath_update_pio_bufs(struct ipath_devdata *);
-
-const char *ipath_get_unit_name(int unit)
-{
-	static char iname[16];
-	snprintf(iname, sizeof iname, "infinipath%u", unit);
-	return iname;
-}
-
-#define DRIVER_LOAD_MSG "QLogic " IPATH_DRV_NAME " loaded: "
-#define PFX IPATH_DRV_NAME ": "
-
-/*
- * The size has to be longer than this string, so we can append
- * board/chip information to it in the init code.
- */
-const char ib_ipath_version[] = IPATH_IDSTR "\n";
-
-static struct idr unit_table;
-DEFINE_SPINLOCK(ipath_devs_lock);
-LIST_HEAD(ipath_dev_list);
-
-wait_queue_head_t ipath_state_wait;
-
-unsigned ipath_debug = __IPATH_INFO;
-
-module_param_named(debug, ipath_debug, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(debug, "mask for debug prints");
-EXPORT_SYMBOL_GPL(ipath_debug);
-
-unsigned ipath_mtu4096 = 1; /* max 4KB IB mtu by default, if supported */
-module_param_named(mtu4096, ipath_mtu4096, uint, S_IRUGO);
-MODULE_PARM_DESC(mtu4096, "enable MTU of 4096 bytes, if supported");
-
-static unsigned ipath_hol_timeout_ms = 13000;
-module_param_named(hol_timeout_ms, ipath_hol_timeout_ms, uint, S_IRUGO);
-MODULE_PARM_DESC(hol_timeout_ms,
-	"duration of user app suspension after link failure");
-
-unsigned ipath_linkrecovery = 1;
-module_param_named(linkrecovery, ipath_linkrecovery, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(linkrecovery, "enable workaround for link recovery issue");
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("QLogic <support@qlogic.com>");
-MODULE_DESCRIPTION("QLogic InfiniPath driver");
-
-/*
- * Table to translate the LINKTRAININGSTATE portion of
- * IBCStatus to a human-readable form.
- */
-const char *ipath_ibcstatus_str[] = {
-	"Disabled",
-	"LinkUp",
-	"PollActive",
-	"PollQuiet",
-	"SleepDelay",
-	"SleepQuiet",
-	"LState6",		/* unused */
-	"LState7",		/* unused */
-	"CfgDebounce",
-	"CfgRcvfCfg",
-	"CfgWaitRmt",
-	"CfgIdle",
-	"RecovRetrain",
-	"CfgTxRevLane",		/* unused before IBA7220 */
-	"RecovWaitRmt",
-	"RecovIdle",
-	/* below were added for IBA7220 */
-	"CfgEnhanced",
-	"CfgTest",
-	"CfgWaitRmtTest",
-	"CfgWaitCfgEnhanced",
-	"SendTS_T",
-	"SendTstIdles",
-	"RcvTS_T",
-	"SendTst_TS1s",
-	"LTState18", "LTState19", "LTState1A", "LTState1B",
-	"LTState1C", "LTState1D", "LTState1E", "LTState1F"
-};
-
-static void ipath_remove_one(struct pci_dev *);
-static int ipath_init_one(struct pci_dev *, const struct pci_device_id *);
-
-/* Only needed for registration, nothing else needs this info */
-#define PCI_VENDOR_ID_PATHSCALE 0x1fc1
-#define PCI_DEVICE_ID_INFINIPATH_HT 0xd
-
-/* Number of seconds before our card status check...  */
-#define STATUS_TIMEOUT 60
-
-static const struct pci_device_id ipath_pci_tbl[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_PATHSCALE, PCI_DEVICE_ID_INFINIPATH_HT) },
-	{ 0, }
-};
-
-MODULE_DEVICE_TABLE(pci, ipath_pci_tbl);
-
-static struct pci_driver ipath_driver = {
-	.name = IPATH_DRV_NAME,
-	.probe = ipath_init_one,
-	.remove = ipath_remove_one,
-	.id_table = ipath_pci_tbl,
-	.driver = {
-		.groups = ipath_driver_attr_groups,
-	},
-};
-
-static inline void read_bars(struct ipath_devdata *dd, struct pci_dev *dev,
-			     u32 *bar0, u32 *bar1)
-{
-	int ret;
-
-	ret = pci_read_config_dword(dev, PCI_BASE_ADDRESS_0, bar0);
-	if (ret)
-		ipath_dev_err(dd, "failed to read bar0 before enable: "
-			      "error %d\n", -ret);
-
-	ret = pci_read_config_dword(dev, PCI_BASE_ADDRESS_1, bar1);
-	if (ret)
-		ipath_dev_err(dd, "failed to read bar1 before enable: "
-			      "error %d\n", -ret);
-
-	ipath_dbg("Read bar0 %x bar1 %x\n", *bar0, *bar1);
-}
-
-static void ipath_free_devdata(struct pci_dev *pdev,
-			       struct ipath_devdata *dd)
-{
-	unsigned long flags;
-
-	pci_set_drvdata(pdev, NULL);
-
-	if (dd->ipath_unit != -1) {
-		spin_lock_irqsave(&ipath_devs_lock, flags);
-		idr_remove(&unit_table, dd->ipath_unit);
-		list_del(&dd->ipath_list);
-		spin_unlock_irqrestore(&ipath_devs_lock, flags);
-	}
-	vfree(dd);
-}
-
-static struct ipath_devdata *ipath_alloc_devdata(struct pci_dev *pdev)
-{
-	unsigned long flags;
-	struct ipath_devdata *dd;
-	int ret;
-
-	dd = vzalloc(sizeof(*dd));
-	if (!dd) {
-		dd = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-	dd->ipath_unit = -1;
-
-	idr_preload(GFP_KERNEL);
-	spin_lock_irqsave(&ipath_devs_lock, flags);
-
-	ret = idr_alloc(&unit_table, dd, 0, 0, GFP_NOWAIT);
-	if (ret < 0) {
-		printk(KERN_ERR IPATH_DRV_NAME
-		       ": Could not allocate unit ID: error %d\n", -ret);
-		ipath_free_devdata(pdev, dd);
-		dd = ERR_PTR(ret);
-		goto bail_unlock;
-	}
-	dd->ipath_unit = ret;
-
-	dd->pcidev = pdev;
-	pci_set_drvdata(pdev, dd);
-
-	list_add(&dd->ipath_list, &ipath_dev_list);
-
-bail_unlock:
-	spin_unlock_irqrestore(&ipath_devs_lock, flags);
-	idr_preload_end();
-bail:
-	return dd;
-}
-
-static inline struct ipath_devdata *__ipath_lookup(int unit)
-{
-	return idr_find(&unit_table, unit);
-}
-
-struct ipath_devdata *ipath_lookup(int unit)
-{
-	struct ipath_devdata *dd;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ipath_devs_lock, flags);
-	dd = __ipath_lookup(unit);
-	spin_unlock_irqrestore(&ipath_devs_lock, flags);
-
-	return dd;
-}
-
-int ipath_count_units(int *npresentp, int *nupp, int *maxportsp)
-{
-	int nunits, npresent, nup;
-	struct ipath_devdata *dd;
-	unsigned long flags;
-	int maxports;
-
-	nunits = npresent = nup = maxports = 0;
-
-	spin_lock_irqsave(&ipath_devs_lock, flags);
-
-	list_for_each_entry(dd, &ipath_dev_list, ipath_list) {
-		nunits++;
-		if ((dd->ipath_flags & IPATH_PRESENT) && dd->ipath_kregbase)
-			npresent++;
-		if (dd->ipath_lid &&
-		    !(dd->ipath_flags & (IPATH_DISABLED | IPATH_LINKDOWN
-					 | IPATH_LINKUNK)))
-			nup++;
-		if (dd->ipath_cfgports > maxports)
-			maxports = dd->ipath_cfgports;
-	}
-
-	spin_unlock_irqrestore(&ipath_devs_lock, flags);
-
-	if (npresentp)
-		*npresentp = npresent;
-	if (nupp)
-		*nupp = nup;
-	if (maxportsp)
-		*maxportsp = maxports;
-
-	return nunits;
-}
-
-/*
- * These next two routines are placeholders in case we don't have per-arch
- * code for controlling write combining.  If explicit control of write
- * combining is not available, performance will probably be awful.
- */
-
-int __attribute__((weak)) ipath_enable_wc(struct ipath_devdata *dd)
-{
-	return -EOPNOTSUPP;
-}
-
-void __attribute__((weak)) ipath_disable_wc(struct ipath_devdata *dd)
-{
-}
-
-/*
- * Perform a PIO buffer bandwidth write test, to verify proper system
- * configuration.  Even when all the setup calls work, occasionally
- * BIOS or other issues can prevent write combining from working, or
- * can cause other bandwidth problems to the chip.
- *
- * This test simply writes the same buffer over and over again, and
- * measures close to the peak bandwidth to the chip (not testing
- * data bandwidth to the wire).   On chips that use an address-based
- * trigger to send packets to the wire, this is easy.  On chips that
- * use a count to trigger, we want to make sure that the packet doesn't
- * go out on the wire, or trigger flow control checks.
- */
-static void ipath_verify_pioperf(struct ipath_devdata *dd)
-{
-	u32 pbnum, cnt, lcnt;
-	u32 __iomem *piobuf;
-	u32 *addr;
-	u64 msecs, emsecs;
-
-	piobuf = ipath_getpiobuf(dd, 0, &pbnum);
-	if (!piobuf) {
-		dev_info(&dd->pcidev->dev,
-			"No PIObufs for checking perf, skipping\n");
-		return;
-	}
-
-	/*
-	 * Enough to give us a reasonable test, less than piobuf size, and
-	 * likely multiple of store buffer length.
-	 */
-	cnt = 1024;
-
-	addr = vmalloc(cnt);
-	if (!addr) {
-		dev_info(&dd->pcidev->dev,
-			"Couldn't get memory for checking PIO perf,"
-			" skipping\n");
-		goto done;
-	}
-
-	preempt_disable();  /* we want reasonably accurate elapsed time */
-	msecs = 1 + jiffies_to_msecs(jiffies);
-	for (lcnt = 0; lcnt < 10000U; lcnt++) {
-		/* wait until we cross msec boundary */
-		if (jiffies_to_msecs(jiffies) >= msecs)
-			break;
-		udelay(1);
-	}
-
-	ipath_disable_armlaunch(dd);
-
-	/*
-	 * length 0, no dwords actually sent, and mark as VL15
-	 * on chips where that may matter (due to IB flowcontrol)
-	 */
-	if ((dd->ipath_flags & IPATH_HAS_PBC_CNT))
-		writeq(1UL << 63, piobuf);
-	else
-		writeq(0, piobuf);
-	ipath_flush_wc();
-
-	/*
-	 * this is only roughly accurate, since even with preempt we
-	 * still take interrupts that could take a while.   Running for
-	 * >= 5 msec seems to get us "close enough" to accurate values
-	 */
-	msecs = jiffies_to_msecs(jiffies);
-	for (emsecs = lcnt = 0; emsecs <= 5UL; lcnt++) {
-		__iowrite32_copy(piobuf + 64, addr, cnt >> 2);
-		emsecs = jiffies_to_msecs(jiffies) - msecs;
-	}
-
-	/* 1 GiB/sec, slightly over IB SDR line rate */
-	if (lcnt < (emsecs * 1024U))
-		ipath_dev_err(dd,
-			"Performance problem: bandwidth to PIO buffers is "
-			"only %u MiB/sec\n",
-			lcnt / (u32) emsecs);
-	else
-		ipath_dbg("PIO buffer bandwidth %u MiB/sec is OK\n",
-			lcnt / (u32) emsecs);
-
-	preempt_enable();
-
-	vfree(addr);
-
-done:
-	/* disarm piobuf, so it's available again */
-	ipath_disarm_piobufs(dd, pbnum, 1);
-	ipath_enable_armlaunch(dd);
-}
-
-static void cleanup_device(struct ipath_devdata *dd);
-
-static int ipath_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
-{
-	int ret, len, j;
-	struct ipath_devdata *dd;
-	unsigned long long addr;
-	u32 bar0 = 0, bar1 = 0;
-
-#ifdef CONFIG_X86_64
-	if (pat_enabled()) {
-		pr_warn("ipath needs PAT disabled, boot with nopat kernel parameter\n");
-		ret = -ENODEV;
-		goto bail;
-	}
-#endif
-
-	dd = ipath_alloc_devdata(pdev);
-	if (IS_ERR(dd)) {
-		ret = PTR_ERR(dd);
-		printk(KERN_ERR IPATH_DRV_NAME
-		       ": Could not allocate devdata: error %d\n", -ret);
-		goto bail;
-	}
-
-	ipath_cdbg(VERBOSE, "initializing unit #%u\n", dd->ipath_unit);
-
-	ret = pci_enable_device(pdev);
-	if (ret) {
-		/* This can happen iff:
-		 *
-		 * We did a chip reset, and then failed to reprogram the
-		 * BAR, or the chip reset due to an internal error.  We then
-		 * unloaded the driver and reloaded it.
-		 *
-		 * Both reset cases set the BAR back to initial state.  For
-		 * the latter case, the AER sticky error bit at offset 0x718
-		 * should be set, but the Linux kernel doesn't yet know
-		 * about that, it appears.  If the original BAR was retained
-		 * in the kernel data structures, this may be OK.
-		 */
-		ipath_dev_err(dd, "enable unit %d failed: error %d\n",
-			      dd->ipath_unit, -ret);
-		goto bail_devdata;
-	}
-	addr = pci_resource_start(pdev, 0);
-	len = pci_resource_len(pdev, 0);
-	ipath_cdbg(VERBOSE, "regbase (0) %llx len %d irq %d, vend %x/%x "
-		   "driver_data %lx\n", addr, len, pdev->irq, ent->vendor,
-		   ent->device, ent->driver_data);
-
-	read_bars(dd, pdev, &bar0, &bar1);
-
-	if (!bar1 && !(bar0 & ~0xf)) {
-		if (addr) {
-			dev_info(&pdev->dev, "BAR is 0 (probable RESET), "
-				 "rewriting as %llx\n", addr);
-			ret = pci_write_config_dword(
-				pdev, PCI_BASE_ADDRESS_0, addr);
-			if (ret) {
-				ipath_dev_err(dd, "rewrite of BAR0 "
-					      "failed: err %d\n", -ret);
-				goto bail_disable;
-			}
-			ret = pci_write_config_dword(
-				pdev, PCI_BASE_ADDRESS_1, addr >> 32);
-			if (ret) {
-				ipath_dev_err(dd, "rewrite of BAR1 "
-					      "failed: err %d\n", -ret);
-				goto bail_disable;
-			}
-		} else {
-			ipath_dev_err(dd, "BAR is 0 (probable RESET), "
-				      "not usable until reboot\n");
-			ret = -ENODEV;
-			goto bail_disable;
-		}
-	}
-
-	ret = pci_request_regions(pdev, IPATH_DRV_NAME);
-	if (ret) {
-		dev_info(&pdev->dev, "pci_request_regions unit %u fails: "
-			 "err %d\n", dd->ipath_unit, -ret);
-		goto bail_disable;
-	}
-
-	ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
-	if (ret) {
-		/*
-		 * if the 64 bit setup fails, try 32 bit.  Some systems
-		 * do not setup 64 bit maps on systems with 2GB or less
-		 * memory installed.
-		 */
-		ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
-		if (ret) {
-			dev_info(&pdev->dev,
-				"Unable to set DMA mask for unit %u: %d\n",
-				dd->ipath_unit, ret);
-			goto bail_regions;
-		} else {
-			ipath_dbg("No 64bit DMA mask, used 32 bit mask\n");
-			ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
-			if (ret)
-				dev_info(&pdev->dev,
-					"Unable to set DMA consistent mask "
-					"for unit %u: %d\n",
-					dd->ipath_unit, ret);
-
-		}
-	} else {
-		ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
-		if (ret)
-			dev_info(&pdev->dev,
-				"Unable to set DMA consistent mask "
-				"for unit %u: %d\n",
-				dd->ipath_unit, ret);
-	}
-
-	pci_set_master(pdev);
-
-	/*
-	 * Save BARs to rewrite after device reset.  Save all 64 bits of
-	 * BAR, just in case.
-	 */
-	dd->ipath_pcibar0 = addr;
-	dd->ipath_pcibar1 = addr >> 32;
-	dd->ipath_deviceid = ent->device;	/* save for later use */
-	dd->ipath_vendorid = ent->vendor;
-
-	/* setup the chip-specific functions, as early as possible. */
-	switch (ent->device) {
-	case PCI_DEVICE_ID_INFINIPATH_HT:
-		ipath_init_iba6110_funcs(dd);
-		break;
-
-	default:
-		ipath_dev_err(dd, "Found unknown QLogic deviceid 0x%x, "
-			      "failing\n", ent->device);
-		return -ENODEV;
-	}
-
-	for (j = 0; j < 6; j++) {
-		if (!pdev->resource[j].start)
-			continue;
-		ipath_cdbg(VERBOSE, "BAR %d %pR, len %llx\n",
-			   j, &pdev->resource[j],
-			   (unsigned long long)pci_resource_len(pdev, j));
-	}
-
-	if (!addr) {
-		ipath_dev_err(dd, "No valid address in BAR 0!\n");
-		ret = -ENODEV;
-		goto bail_regions;
-	}
-
-	dd->ipath_pcirev = pdev->revision;
-
-#if defined(__powerpc__)
-	/* There isn't a generic way to specify writethrough mappings */
-	dd->ipath_kregbase = __ioremap(addr, len,
-		(_PAGE_NO_CACHE|_PAGE_WRITETHRU));
-#else
-	/* XXX: split this properly to enable on PAT */
-	dd->ipath_kregbase = ioremap_nocache(addr, len);
-#endif
-
-	if (!dd->ipath_kregbase) {
-		ipath_dbg("Unable to map io addr %llx to kvirt, failing\n",
-			  addr);
-		ret = -ENOMEM;
-		goto bail_iounmap;
-	}
-	dd->ipath_kregend = (u64 __iomem *)
-		((void __iomem *)dd->ipath_kregbase + len);
-	dd->ipath_physaddr = addr;	/* used for io_remap, etc. */
-	/* for user mmap */
-	ipath_cdbg(VERBOSE, "mapped io addr %llx to kregbase %p\n",
-		   addr, dd->ipath_kregbase);
-
-	if (dd->ipath_f_bus(dd, pdev))
-		ipath_dev_err(dd, "Failed to setup config space; "
-			      "continuing anyway\n");
-
-	/*
-	 * set up our interrupt handler; IRQF_SHARED probably not needed,
-	 * since MSI interrupts shouldn't be shared but won't  hurt for now.
-	 * check 0 irq after we return from chip-specific bus setup, since
-	 * that can affect this due to setup
-	 */
-	if (!dd->ipath_irq)
-		ipath_dev_err(dd, "irq is 0, BIOS error?  Interrupts won't "
-			      "work\n");
-	else {
-		ret = request_irq(dd->ipath_irq, ipath_intr, IRQF_SHARED,
-				  IPATH_DRV_NAME, dd);
-		if (ret) {
-			ipath_dev_err(dd, "Couldn't setup irq handler, "
-				      "irq=%d: %d\n", dd->ipath_irq, ret);
-			goto bail_iounmap;
-		}
-	}
-
-	ret = ipath_init_chip(dd, 0);	/* do the chip-specific init */
-	if (ret)
-		goto bail_irqsetup;
-
-	ret = ipath_enable_wc(dd);
-
-	if (ret)
-		ret = 0;
-
-	ipath_verify_pioperf(dd);
-
-	ipath_device_create_group(&pdev->dev, dd);
-	ipathfs_add_device(dd);
-	ipath_user_add(dd);
-	ipath_diag_add(dd);
-	ipath_register_ib_device(dd);
-
-	goto bail;
-
-bail_irqsetup:
-	cleanup_device(dd);
-
-	if (dd->ipath_irq)
-		dd->ipath_f_free_irq(dd);
-
-	if (dd->ipath_f_cleanup)
-		dd->ipath_f_cleanup(dd);
-
-bail_iounmap:
-	iounmap((volatile void __iomem *) dd->ipath_kregbase);
-
-bail_regions:
-	pci_release_regions(pdev);
-
-bail_disable:
-	pci_disable_device(pdev);
-
-bail_devdata:
-	ipath_free_devdata(pdev, dd);
-
-bail:
-	return ret;
-}
-
-static void cleanup_device(struct ipath_devdata *dd)
-{
-	int port;
-	struct ipath_portdata **tmp;
-	unsigned long flags;
-
-	if (*dd->ipath_statusp & IPATH_STATUS_CHIP_PRESENT) {
-		/* can't do anything more with chip; needs re-init */
-		*dd->ipath_statusp &= ~IPATH_STATUS_CHIP_PRESENT;
-		if (dd->ipath_kregbase) {
-			/*
-			 * if we haven't already cleaned up before these are
-			 * to ensure any register reads/writes "fail" until
-			 * re-init
-			 */
-			dd->ipath_kregbase = NULL;
-			dd->ipath_uregbase = 0;
-			dd->ipath_sregbase = 0;
-			dd->ipath_cregbase = 0;
-			dd->ipath_kregsize = 0;
-		}
-		ipath_disable_wc(dd);
-	}
-
-	if (dd->ipath_spectriggerhit)
-		dev_info(&dd->pcidev->dev, "%lu special trigger hits\n",
-			 dd->ipath_spectriggerhit);
-
-	if (dd->ipath_pioavailregs_dma) {
-		dma_free_coherent(&dd->pcidev->dev, PAGE_SIZE,
-				  (void *) dd->ipath_pioavailregs_dma,
-				  dd->ipath_pioavailregs_phys);
-		dd->ipath_pioavailregs_dma = NULL;
-	}
-	if (dd->ipath_dummy_hdrq) {
-		dma_free_coherent(&dd->pcidev->dev,
-			dd->ipath_pd[0]->port_rcvhdrq_size,
-			dd->ipath_dummy_hdrq, dd->ipath_dummy_hdrq_phys);
-		dd->ipath_dummy_hdrq = NULL;
-	}
-
-	if (dd->ipath_pageshadow) {
-		struct page **tmpp = dd->ipath_pageshadow;
-		dma_addr_t *tmpd = dd->ipath_physshadow;
-		int i, cnt = 0;
-
-		ipath_cdbg(VERBOSE, "Unlocking any expTID pages still "
-			   "locked\n");
-		for (port = 0; port < dd->ipath_cfgports; port++) {
-			int port_tidbase = port * dd->ipath_rcvtidcnt;
-			int maxtid = port_tidbase + dd->ipath_rcvtidcnt;
-			for (i = port_tidbase; i < maxtid; i++) {
-				if (!tmpp[i])
-					continue;
-				pci_unmap_page(dd->pcidev, tmpd[i],
-					PAGE_SIZE, PCI_DMA_FROMDEVICE);
-				ipath_release_user_pages(&tmpp[i], 1);
-				tmpp[i] = NULL;
-				cnt++;
-			}
-		}
-		if (cnt) {
-			ipath_stats.sps_pageunlocks += cnt;
-			ipath_cdbg(VERBOSE, "There were still %u expTID "
-				   "entries locked\n", cnt);
-		}
-		if (ipath_stats.sps_pagelocks ||
-		    ipath_stats.sps_pageunlocks)
-			ipath_cdbg(VERBOSE, "%llu pages locked, %llu "
-				   "unlocked via ipath_m{un}lock\n",
-				   (unsigned long long)
-				   ipath_stats.sps_pagelocks,
-				   (unsigned long long)
-				   ipath_stats.sps_pageunlocks);
-
-		ipath_cdbg(VERBOSE, "Free shadow page tid array at %p\n",
-			   dd->ipath_pageshadow);
-		tmpp = dd->ipath_pageshadow;
-		dd->ipath_pageshadow = NULL;
-		vfree(tmpp);
-
-		dd->ipath_egrtidbase = NULL;
-	}
-
-	/*
-	 * free any resources still in use (usually just kernel ports)
-	 * at unload; we do for portcnt, because that's what we allocate.
-	 * We acquire lock to be really paranoid that ipath_pd isn't being
-	 * accessed from some interrupt-related code (that should not happen,
-	 * but best to be sure).
-	 */
-	spin_lock_irqsave(&dd->ipath_uctxt_lock, flags);
-	tmp = dd->ipath_pd;
-	dd->ipath_pd = NULL;
-	spin_unlock_irqrestore(&dd->ipath_uctxt_lock, flags);
-	for (port = 0; port < dd->ipath_portcnt; port++) {
-		struct ipath_portdata *pd = tmp[port];
-		tmp[port] = NULL; /* debugging paranoia */
-		ipath_free_pddata(dd, pd);
-	}
-	kfree(tmp);
-}
-
-static void ipath_remove_one(struct pci_dev *pdev)
-{
-	struct ipath_devdata *dd = pci_get_drvdata(pdev);
-
-	ipath_cdbg(VERBOSE, "removing, pdev=%p, dd=%p\n", pdev, dd);
-
-	/*
-	 * disable the IB link early, to be sure no new packets arrive, which
-	 * complicates the shutdown process
-	 */
-	ipath_shutdown_device(dd);
-
-	flush_workqueue(ib_wq);
-
-	if (dd->verbs_dev)
-		ipath_unregister_ib_device(dd->verbs_dev);
-
-	ipath_diag_remove(dd);
-	ipath_user_remove(dd);
-	ipathfs_remove_device(dd);
-	ipath_device_remove_group(&pdev->dev, dd);
-
-	ipath_cdbg(VERBOSE, "Releasing pci memory regions, dd %p, "
-		   "unit %u\n", dd, (u32) dd->ipath_unit);
-
-	cleanup_device(dd);
-
-	/*
-	 * turn off rcv, send, and interrupts for all ports, all drivers
-	 * should also hard reset the chip here?
-	 * free up port 0 (kernel) rcvhdr, egr bufs, and eventually tid bufs
-	 * for all versions of the driver, if they were allocated
-	 */
-	if (dd->ipath_irq) {
-		ipath_cdbg(VERBOSE, "unit %u free irq %d\n",
-			   dd->ipath_unit, dd->ipath_irq);
-		dd->ipath_f_free_irq(dd);
-	} else
-		ipath_dbg("irq is 0, not doing free_irq "
-			  "for unit %u\n", dd->ipath_unit);
-	/*
-	 * we check for NULL here, because it's outside
-	 * the kregbase check, and we need to call it
-	 * after the free_irq.	Thus it's possible that
-	 * the function pointers were never initialized.
-	 */
-	if (dd->ipath_f_cleanup)
-		/* clean up chip-specific stuff */
-		dd->ipath_f_cleanup(dd);
-
-	ipath_cdbg(VERBOSE, "Unmapping kregbase %p\n", dd->ipath_kregbase);
-	iounmap((volatile void __iomem *) dd->ipath_kregbase);
-	pci_release_regions(pdev);
-	ipath_cdbg(VERBOSE, "calling pci_disable_device\n");
-	pci_disable_device(pdev);
-
-	ipath_free_devdata(pdev, dd);
-}
-
-/* general driver use */
-DEFINE_MUTEX(ipath_mutex);
-
-static DEFINE_SPINLOCK(ipath_pioavail_lock);
-
-/**
- * ipath_disarm_piobufs - cancel a range of PIO buffers
- * @dd: the infinipath device
- * @first: the first PIO buffer to cancel
- * @cnt: the number of PIO buffers to cancel
- *
- * cancel a range of PIO buffers, used when they might be armed, but
- * not triggered.  Used at init to ensure buffer state, and also user
- * process close, in case it died while writing to a PIO buffer
- * Also after errors.
- */
-void ipath_disarm_piobufs(struct ipath_devdata *dd, unsigned first,
-			  unsigned cnt)
-{
-	unsigned i, last = first + cnt;
-	unsigned long flags;
-
-	ipath_cdbg(PKT, "disarm %u PIObufs first=%u\n", cnt, first);
-	for (i = first; i < last; i++) {
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		/*
-		 * The disarm-related bits are write-only, so it
-		 * is ok to OR them in with our copy of sendctrl
-		 * while we hold the lock.
-		 */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			dd->ipath_sendctrl | INFINIPATH_S_DISARM |
-			(i << INFINIPATH_S_DISARMPIOBUF_SHIFT));
-		/* can't disarm bufs back-to-back per iba7220 spec */
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-	}
-	/* on some older chips, update may not happen after cancel */
-	ipath_force_pio_avail_update(dd);
-}
-
-/**
- * ipath_wait_linkstate - wait for an IB link state change to occur
- * @dd: the infinipath device
- * @state: the state to wait for
- * @msecs: the number of milliseconds to wait
- *
- * wait up to msecs milliseconds for IB link state change to occur for
- * now, take the easy polling route.  Currently used only by
- * ipath_set_linkstate.  Returns 0 if state reached, otherwise
- * -ETIMEDOUT state can have multiple states set, for any of several
- * transitions.
- */
-int ipath_wait_linkstate(struct ipath_devdata *dd, u32 state, int msecs)
-{
-	dd->ipath_state_wanted = state;
-	wait_event_interruptible_timeout(ipath_state_wait,
-					 (dd->ipath_flags & state),
-					 msecs_to_jiffies(msecs));
-	dd->ipath_state_wanted = 0;
-
-	if (!(dd->ipath_flags & state)) {
-		u64 val;
-		ipath_cdbg(VERBOSE, "Didn't reach linkstate %s within %u"
-			   " ms\n",
-			   /* test INIT ahead of DOWN, both can be set */
-			   (state & IPATH_LINKINIT) ? "INIT" :
-			   ((state & IPATH_LINKDOWN) ? "DOWN" :
-			    ((state & IPATH_LINKARMED) ? "ARM" : "ACTIVE")),
-			   msecs);
-		val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_ibcstatus);
-		ipath_cdbg(VERBOSE, "ibcc=%llx ibcstatus=%llx (%s)\n",
-			   (unsigned long long) ipath_read_kreg64(
-				   dd, dd->ipath_kregs->kr_ibcctrl),
-			   (unsigned long long) val,
-			   ipath_ibcstatus_str[val & dd->ibcs_lts_mask]);
-	}
-	return (dd->ipath_flags & state) ? 0 : -ETIMEDOUT;
-}
-
-static void decode_sdma_errs(struct ipath_devdata *dd, ipath_err_t err,
-	char *buf, size_t blen)
-{
-	static const struct {
-		ipath_err_t err;
-		const char *msg;
-	} errs[] = {
-		{ INFINIPATH_E_SDMAGENMISMATCH, "SDmaGenMismatch" },
-		{ INFINIPATH_E_SDMAOUTOFBOUND, "SDmaOutOfBound" },
-		{ INFINIPATH_E_SDMATAILOUTOFBOUND, "SDmaTailOutOfBound" },
-		{ INFINIPATH_E_SDMABASE, "SDmaBase" },
-		{ INFINIPATH_E_SDMA1STDESC, "SDma1stDesc" },
-		{ INFINIPATH_E_SDMARPYTAG, "SDmaRpyTag" },
-		{ INFINIPATH_E_SDMADWEN, "SDmaDwEn" },
-		{ INFINIPATH_E_SDMAMISSINGDW, "SDmaMissingDw" },
-		{ INFINIPATH_E_SDMAUNEXPDATA, "SDmaUnexpData" },
-		{ INFINIPATH_E_SDMADESCADDRMISALIGN, "SDmaDescAddrMisalign" },
-		{ INFINIPATH_E_SENDBUFMISUSE, "SendBufMisuse" },
-		{ INFINIPATH_E_SDMADISABLED, "SDmaDisabled" },
-	};
-	int i;
-	int expected;
-	size_t bidx = 0;
-
-	for (i = 0; i < ARRAY_SIZE(errs); i++) {
-		expected = (errs[i].err != INFINIPATH_E_SDMADISABLED) ? 0 :
-			test_bit(IPATH_SDMA_ABORTING, &dd->ipath_sdma_status);
-		if ((err & errs[i].err) && !expected)
-			bidx += snprintf(buf + bidx, blen - bidx,
-					 "%s ", errs[i].msg);
-	}
-}
-
-/*
- * Decode the error status into strings, deciding whether to always
- * print * it or not depending on "normal packet errors" vs everything
- * else.   Return 1 if "real" errors, otherwise 0 if only packet
- * errors, so caller can decide what to print with the string.
- */
-int ipath_decode_err(struct ipath_devdata *dd, char *buf, size_t blen,
-	ipath_err_t err)
-{
-	int iserr = 1;
-	*buf = '\0';
-	if (err & INFINIPATH_E_PKTERRS) {
-		if (!(err & ~INFINIPATH_E_PKTERRS))
-			iserr = 0; // if only packet errors.
-		if (ipath_debug & __IPATH_ERRPKTDBG) {
-			if (err & INFINIPATH_E_REBP)
-				strlcat(buf, "EBP ", blen);
-			if (err & INFINIPATH_E_RVCRC)
-				strlcat(buf, "VCRC ", blen);
-			if (err & INFINIPATH_E_RICRC) {
-				strlcat(buf, "CRC ", blen);
-				// clear for check below, so only once
-				err &= INFINIPATH_E_RICRC;
-			}
-			if (err & INFINIPATH_E_RSHORTPKTLEN)
-				strlcat(buf, "rshortpktlen ", blen);
-			if (err & INFINIPATH_E_SDROPPEDDATAPKT)
-				strlcat(buf, "sdroppeddatapkt ", blen);
-			if (err & INFINIPATH_E_SPKTLEN)
-				strlcat(buf, "spktlen ", blen);
-		}
-		if ((err & INFINIPATH_E_RICRC) &&
-			!(err&(INFINIPATH_E_RVCRC|INFINIPATH_E_REBP)))
-			strlcat(buf, "CRC ", blen);
-		if (!iserr)
-			goto done;
-	}
-	if (err & INFINIPATH_E_RHDRLEN)
-		strlcat(buf, "rhdrlen ", blen);
-	if (err & INFINIPATH_E_RBADTID)
-		strlcat(buf, "rbadtid ", blen);
-	if (err & INFINIPATH_E_RBADVERSION)
-		strlcat(buf, "rbadversion ", blen);
-	if (err & INFINIPATH_E_RHDR)
-		strlcat(buf, "rhdr ", blen);
-	if (err & INFINIPATH_E_SENDSPECIALTRIGGER)
-		strlcat(buf, "sendspecialtrigger ", blen);
-	if (err & INFINIPATH_E_RLONGPKTLEN)
-		strlcat(buf, "rlongpktlen ", blen);
-	if (err & INFINIPATH_E_RMAXPKTLEN)
-		strlcat(buf, "rmaxpktlen ", blen);
-	if (err & INFINIPATH_E_RMINPKTLEN)
-		strlcat(buf, "rminpktlen ", blen);
-	if (err & INFINIPATH_E_SMINPKTLEN)
-		strlcat(buf, "sminpktlen ", blen);
-	if (err & INFINIPATH_E_RFORMATERR)
-		strlcat(buf, "rformaterr ", blen);
-	if (err & INFINIPATH_E_RUNSUPVL)
-		strlcat(buf, "runsupvl ", blen);
-	if (err & INFINIPATH_E_RUNEXPCHAR)
-		strlcat(buf, "runexpchar ", blen);
-	if (err & INFINIPATH_E_RIBFLOW)
-		strlcat(buf, "ribflow ", blen);
-	if (err & INFINIPATH_E_SUNDERRUN)
-		strlcat(buf, "sunderrun ", blen);
-	if (err & INFINIPATH_E_SPIOARMLAUNCH)
-		strlcat(buf, "spioarmlaunch ", blen);
-	if (err & INFINIPATH_E_SUNEXPERRPKTNUM)
-		strlcat(buf, "sunexperrpktnum ", blen);
-	if (err & INFINIPATH_E_SDROPPEDSMPPKT)
-		strlcat(buf, "sdroppedsmppkt ", blen);
-	if (err & INFINIPATH_E_SMAXPKTLEN)
-		strlcat(buf, "smaxpktlen ", blen);
-	if (err & INFINIPATH_E_SUNSUPVL)
-		strlcat(buf, "sunsupVL ", blen);
-	if (err & INFINIPATH_E_INVALIDADDR)
-		strlcat(buf, "invalidaddr ", blen);
-	if (err & INFINIPATH_E_RRCVEGRFULL)
-		strlcat(buf, "rcvegrfull ", blen);
-	if (err & INFINIPATH_E_RRCVHDRFULL)
-		strlcat(buf, "rcvhdrfull ", blen);
-	if (err & INFINIPATH_E_IBSTATUSCHANGED)
-		strlcat(buf, "ibcstatuschg ", blen);
-	if (err & INFINIPATH_E_RIBLOSTLINK)
-		strlcat(buf, "riblostlink ", blen);
-	if (err & INFINIPATH_E_HARDWARE)
-		strlcat(buf, "hardware ", blen);
-	if (err & INFINIPATH_E_RESET)
-		strlcat(buf, "reset ", blen);
-	if (err & INFINIPATH_E_SDMAERRS)
-		decode_sdma_errs(dd, err, buf, blen);
-	if (err & INFINIPATH_E_INVALIDEEPCMD)
-		strlcat(buf, "invalideepromcmd ", blen);
-done:
-	return iserr;
-}
-
-/**
- * get_rhf_errstring - decode RHF errors
- * @err: the err number
- * @msg: the output buffer
- * @len: the length of the output buffer
- *
- * only used one place now, may want more later
- */
-static void get_rhf_errstring(u32 err, char *msg, size_t len)
-{
-	/* if no errors, and so don't need to check what's first */
-	*msg = '\0';
-
-	if (err & INFINIPATH_RHF_H_ICRCERR)
-		strlcat(msg, "icrcerr ", len);
-	if (err & INFINIPATH_RHF_H_VCRCERR)
-		strlcat(msg, "vcrcerr ", len);
-	if (err & INFINIPATH_RHF_H_PARITYERR)
-		strlcat(msg, "parityerr ", len);
-	if (err & INFINIPATH_RHF_H_LENERR)
-		strlcat(msg, "lenerr ", len);
-	if (err & INFINIPATH_RHF_H_MTUERR)
-		strlcat(msg, "mtuerr ", len);
-	if (err & INFINIPATH_RHF_H_IHDRERR)
-		/* infinipath hdr checksum error */
-		strlcat(msg, "ipathhdrerr ", len);
-	if (err & INFINIPATH_RHF_H_TIDERR)
-		strlcat(msg, "tiderr ", len);
-	if (err & INFINIPATH_RHF_H_MKERR)
-		/* bad port, offset, etc. */
-		strlcat(msg, "invalid ipathhdr ", len);
-	if (err & INFINIPATH_RHF_H_IBERR)
-		strlcat(msg, "iberr ", len);
-	if (err & INFINIPATH_RHF_L_SWA)
-		strlcat(msg, "swA ", len);
-	if (err & INFINIPATH_RHF_L_SWB)
-		strlcat(msg, "swB ", len);
-}
-
-/**
- * ipath_get_egrbuf - get an eager buffer
- * @dd: the infinipath device
- * @bufnum: the eager buffer to get
- *
- * must only be called if ipath_pd[port] is known to be allocated
- */
-static inline void *ipath_get_egrbuf(struct ipath_devdata *dd, u32 bufnum)
-{
-	return dd->ipath_port0_skbinfo ?
-		(void *) dd->ipath_port0_skbinfo[bufnum].skb->data : NULL;
-}
-
-/**
- * ipath_alloc_skb - allocate an skb and buffer with possible constraints
- * @dd: the infinipath device
- * @gfp_mask: the sk_buff SFP mask
- */
-struct sk_buff *ipath_alloc_skb(struct ipath_devdata *dd,
-				gfp_t gfp_mask)
-{
-	struct sk_buff *skb;
-	u32 len;
-
-	/*
-	 * Only fully supported way to handle this is to allocate lots
-	 * extra, align as needed, and then do skb_reserve().  That wastes
-	 * a lot of memory...  I'll have to hack this into infinipath_copy
-	 * also.
-	 */
-
-	/*
-	 * We need 2 extra bytes for ipath_ether data sent in the
-	 * key header.  In order to keep everything dword aligned,
-	 * we'll reserve 4 bytes.
-	 */
-	len = dd->ipath_ibmaxlen + 4;
-
-	if (dd->ipath_flags & IPATH_4BYTE_TID) {
-		/* We need a 2KB multiple alignment, and there is no way
-		 * to do it except to allocate extra and then skb_reserve
-		 * enough to bring it up to the right alignment.
-		 */
-		len += 2047;
-	}
-
-	skb = __dev_alloc_skb(len, gfp_mask);
-	if (!skb) {
-		ipath_dev_err(dd, "Failed to allocate skbuff, length %u\n",
-			      len);
-		goto bail;
-	}
-
-	skb_reserve(skb, 4);
-
-	if (dd->ipath_flags & IPATH_4BYTE_TID) {
-		u32 una = (unsigned long)skb->data & 2047;
-		if (una)
-			skb_reserve(skb, 2048 - una);
-	}
-
-bail:
-	return skb;
-}
-
-static void ipath_rcv_hdrerr(struct ipath_devdata *dd,
-			     u32 eflags,
-			     u32 l,
-			     u32 etail,
-			     __le32 *rhf_addr,
-			     struct ipath_message_header *hdr)
-{
-	char emsg[128];
-
-	get_rhf_errstring(eflags, emsg, sizeof emsg);
-	ipath_cdbg(PKT, "RHFerrs %x hdrqtail=%x typ=%u "
-		   "tlen=%x opcode=%x egridx=%x: %s\n",
-		   eflags, l,
-		   ipath_hdrget_rcv_type(rhf_addr),
-		   ipath_hdrget_length_in_bytes(rhf_addr),
-		   be32_to_cpu(hdr->bth[0]) >> 24,
-		   etail, emsg);
-
-	/* Count local link integrity errors. */
-	if (eflags & (INFINIPATH_RHF_H_ICRCERR | INFINIPATH_RHF_H_VCRCERR)) {
-		u8 n = (dd->ipath_ibcctrl >>
-			INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT) &
-			INFINIPATH_IBCC_PHYERRTHRESHOLD_MASK;
-
-		if (++dd->ipath_lli_counter > n) {
-			dd->ipath_lli_counter = 0;
-			dd->ipath_lli_errors++;
-		}
-	}
-}
-
-/*
- * ipath_kreceive - receive a packet
- * @pd: the infinipath port
- *
- * called from interrupt handler for errors or receive interrupt
- */
-void ipath_kreceive(struct ipath_portdata *pd)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	__le32 *rhf_addr;
-	void *ebuf;
-	const u32 rsize = dd->ipath_rcvhdrentsize;	/* words */
-	const u32 maxcnt = dd->ipath_rcvhdrcnt * rsize;	/* words */
-	u32 etail = -1, l, hdrqtail;
-	struct ipath_message_header *hdr;
-	u32 eflags, i, etype, tlen, pkttot = 0, updegr = 0, reloop = 0;
-	static u64 totcalls;	/* stats, may eventually remove */
-	int last;
-
-	l = pd->port_head;
-	rhf_addr = (__le32 *) pd->port_rcvhdrq + l + dd->ipath_rhf_offset;
-	if (dd->ipath_flags & IPATH_NODMA_RTAIL) {
-		u32 seq = ipath_hdrget_seq(rhf_addr);
-
-		if (seq != pd->port_seq_cnt)
-			goto bail;
-		hdrqtail = 0;
-	} else {
-		hdrqtail = ipath_get_rcvhdrtail(pd);
-		if (l == hdrqtail)
-			goto bail;
-		smp_rmb();
-	}
-
-reloop:
-	for (last = 0, i = 1; !last; i += !last) {
-		hdr = dd->ipath_f_get_msgheader(dd, rhf_addr);
-		eflags = ipath_hdrget_err_flags(rhf_addr);
-		etype = ipath_hdrget_rcv_type(rhf_addr);
-		/* total length */
-		tlen = ipath_hdrget_length_in_bytes(rhf_addr);
-		ebuf = NULL;
-		if ((dd->ipath_flags & IPATH_NODMA_RTAIL) ?
-		    ipath_hdrget_use_egr_buf(rhf_addr) :
-		    (etype != RCVHQ_RCV_TYPE_EXPECTED)) {
-			/*
-			 * It turns out that the chip uses an eager buffer
-			 * for all non-expected packets, whether it "needs"
-			 * one or not.  So always get the index, but don't
-			 * set ebuf (so we try to copy data) unless the
-			 * length requires it.
-			 */
-			etail = ipath_hdrget_index(rhf_addr);
-			updegr = 1;
-			if (tlen > sizeof(*hdr) ||
-			    etype == RCVHQ_RCV_TYPE_NON_KD)
-				ebuf = ipath_get_egrbuf(dd, etail);
-		}
-
-		/*
-		 * both tiderr and ipathhdrerr are set for all plain IB
-		 * packets; only ipathhdrerr should be set.
-		 */
-
-		if (etype != RCVHQ_RCV_TYPE_NON_KD &&
-		    etype != RCVHQ_RCV_TYPE_ERROR &&
-		    ipath_hdrget_ipath_ver(hdr->iph.ver_port_tid_offset) !=
-		    IPS_PROTO_VERSION)
-			ipath_cdbg(PKT, "Bad InfiniPath protocol version "
-				   "%x\n", etype);
-
-		if (unlikely(eflags))
-			ipath_rcv_hdrerr(dd, eflags, l, etail, rhf_addr, hdr);
-		else if (etype == RCVHQ_RCV_TYPE_NON_KD) {
-			ipath_ib_rcv(dd->verbs_dev, (u32 *)hdr, ebuf, tlen);
-			if (dd->ipath_lli_counter)
-				dd->ipath_lli_counter--;
-		} else if (etype == RCVHQ_RCV_TYPE_EAGER) {
-			u8 opcode = be32_to_cpu(hdr->bth[0]) >> 24;
-			u32 qp = be32_to_cpu(hdr->bth[1]) & 0xffffff;
-			ipath_cdbg(PKT, "typ %x, opcode %x (eager, "
-				   "qp=%x), len %x; ignored\n",
-				   etype, opcode, qp, tlen);
-		} else if (etype == RCVHQ_RCV_TYPE_EXPECTED) {
-			ipath_dbg("Bug: Expected TID, opcode %x; ignored\n",
-				  be32_to_cpu(hdr->bth[0]) >> 24);
-		} else {
-			/*
-			 * error packet, type of error unknown.
-			 * Probably type 3, but we don't know, so don't
-			 * even try to print the opcode, etc.
-			 * Usually caused by a "bad packet", that has no
-			 * BTH, when the LRH says it should.
-			 */
-			ipath_cdbg(ERRPKT, "Error Pkt, but no eflags! egrbuf"
-				  " %x, len %x hdrq+%x rhf: %Lx\n",
-				  etail, tlen, l, (unsigned long long)
-				  le64_to_cpu(*(__le64 *) rhf_addr));
-			if (ipath_debug & __IPATH_ERRPKTDBG) {
-				u32 j, *d, dw = rsize-2;
-				if (rsize > (tlen>>2))
-					dw = tlen>>2;
-				d = (u32 *)hdr;
-				printk(KERN_DEBUG "EPkt rcvhdr(%x dw):\n",
-					dw);
-				for (j = 0; j < dw; j++)
-					printk(KERN_DEBUG "%8x%s", d[j],
-						(j%8) == 7 ? "\n" : " ");
-				printk(KERN_DEBUG ".\n");
-			}
-		}
-		l += rsize;
-		if (l >= maxcnt)
-			l = 0;
-		rhf_addr = (__le32 *) pd->port_rcvhdrq +
-			l + dd->ipath_rhf_offset;
-		if (dd->ipath_flags & IPATH_NODMA_RTAIL) {
-			u32 seq = ipath_hdrget_seq(rhf_addr);
-
-			if (++pd->port_seq_cnt > 13)
-				pd->port_seq_cnt = 1;
-			if (seq != pd->port_seq_cnt)
-				last = 1;
-		} else if (l == hdrqtail) {
-			last = 1;
-		}
-		/*
-		 * update head regs on last packet, and every 16 packets.
-		 * Reduce bus traffic, while still trying to prevent
-		 * rcvhdrq overflows, for when the queue is nearly full
-		 */
-		if (last || !(i & 0xf)) {
-			u64 lval = l;
-
-			/* request IBA6120 and 7220 interrupt only on last */
-			if (last)
-				lval |= dd->ipath_rhdrhead_intr_off;
-			ipath_write_ureg(dd, ur_rcvhdrhead, lval,
-				pd->port_port);
-			if (updegr) {
-				ipath_write_ureg(dd, ur_rcvegrindexhead,
-						 etail, pd->port_port);
-				updegr = 0;
-			}
-		}
-	}
-
-	if (!dd->ipath_rhdrhead_intr_off && !reloop &&
-	    !(dd->ipath_flags & IPATH_NODMA_RTAIL)) {
-		/* IBA6110 workaround; we can have a race clearing chip
-		 * interrupt with another interrupt about to be delivered,
-		 * and can clear it before it is delivered on the GPIO
-		 * workaround.  By doing the extra check here for the
-		 * in-memory tail register updating while we were doing
-		 * earlier packets, we "almost" guarantee we have covered
-		 * that case.
-		 */
-		u32 hqtail = ipath_get_rcvhdrtail(pd);
-		if (hqtail != hdrqtail) {
-			hdrqtail = hqtail;
-			reloop = 1; /* loop 1 extra time at most */
-			goto reloop;
-		}
-	}
-
-	pkttot += i;
-
-	pd->port_head = l;
-
-	if (pkttot > ipath_stats.sps_maxpkts_call)
-		ipath_stats.sps_maxpkts_call = pkttot;
-	ipath_stats.sps_port0pkts += pkttot;
-	ipath_stats.sps_avgpkts_call =
-		ipath_stats.sps_port0pkts / ++totcalls;
-
-bail:;
-}
-
-/**
- * ipath_update_pio_bufs - update shadow copy of the PIO availability map
- * @dd: the infinipath device
- *
- * called whenever our local copy indicates we have run out of send buffers
- * NOTE: This can be called from interrupt context by some code
- * and from non-interrupt context by ipath_getpiobuf().
- */
-
-static void ipath_update_pio_bufs(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-	int i;
-	const unsigned piobregs = (unsigned)dd->ipath_pioavregs;
-
-	/* If the generation (check) bits have changed, then we update the
-	 * busy bit for the corresponding PIO buffer.  This algorithm will
-	 * modify positions to the value they already have in some cases
-	 * (i.e., no change), but it's faster than changing only the bits
-	 * that have changed.
-	 *
-	 * We would like to do this atomicly, to avoid spinlocks in the
-	 * critical send path, but that's not really possible, given the
-	 * type of changes, and that this routine could be called on
-	 * multiple cpu's simultaneously, so we lock in this routine only,
-	 * to avoid conflicting updates; all we change is the shadow, and
-	 * it's a single 64 bit memory location, so by definition the update
-	 * is atomic in terms of what other cpu's can see in testing the
-	 * bits.  The spin_lock overhead isn't too bad, since it only
-	 * happens when all buffers are in use, so only cpu overhead, not
-	 * latency or bandwidth is affected.
-	 */
-	if (!dd->ipath_pioavailregs_dma) {
-		ipath_dbg("Update shadow pioavail, but regs_dma NULL!\n");
-		return;
-	}
-	if (ipath_debug & __IPATH_VERBDBG) {
-		/* only if packet debug and verbose */
-		volatile __le64 *dma = dd->ipath_pioavailregs_dma;
-		unsigned long *shadow = dd->ipath_pioavailshadow;
-
-		ipath_cdbg(PKT, "Refill avail, dma0=%llx shad0=%lx, "
-			   "d1=%llx s1=%lx, d2=%llx s2=%lx, d3=%llx "
-			   "s3=%lx\n",
-			   (unsigned long long) le64_to_cpu(dma[0]),
-			   shadow[0],
-			   (unsigned long long) le64_to_cpu(dma[1]),
-			   shadow[1],
-			   (unsigned long long) le64_to_cpu(dma[2]),
-			   shadow[2],
-			   (unsigned long long) le64_to_cpu(dma[3]),
-			   shadow[3]);
-		if (piobregs > 4)
-			ipath_cdbg(
-				PKT, "2nd group, dma4=%llx shad4=%lx, "
-				"d5=%llx s5=%lx, d6=%llx s6=%lx, "
-				"d7=%llx s7=%lx\n",
-				(unsigned long long) le64_to_cpu(dma[4]),
-				shadow[4],
-				(unsigned long long) le64_to_cpu(dma[5]),
-				shadow[5],
-				(unsigned long long) le64_to_cpu(dma[6]),
-				shadow[6],
-				(unsigned long long) le64_to_cpu(dma[7]),
-				shadow[7]);
-	}
-	spin_lock_irqsave(&ipath_pioavail_lock, flags);
-	for (i = 0; i < piobregs; i++) {
-		u64 pchbusy, pchg, piov, pnew;
-		/*
-		 * Chip Errata: bug 6641; even and odd qwords>3 are swapped
-		 */
-		if (i > 3 && (dd->ipath_flags & IPATH_SWAP_PIOBUFS))
-			piov = le64_to_cpu(dd->ipath_pioavailregs_dma[i ^ 1]);
-		else
-			piov = le64_to_cpu(dd->ipath_pioavailregs_dma[i]);
-		pchg = dd->ipath_pioavailkernel[i] &
-			~(dd->ipath_pioavailshadow[i] ^ piov);
-		pchbusy = pchg << INFINIPATH_SENDPIOAVAIL_BUSY_SHIFT;
-		if (pchg && (pchbusy & dd->ipath_pioavailshadow[i])) {
-			pnew = dd->ipath_pioavailshadow[i] & ~pchbusy;
-			pnew |= piov & pchbusy;
-			dd->ipath_pioavailshadow[i] = pnew;
-		}
-	}
-	spin_unlock_irqrestore(&ipath_pioavail_lock, flags);
-}
-
-/*
- * used to force update of pioavailshadow if we can't get a pio buffer.
- * Needed primarily due to exitting freeze mode after recovering
- * from errors.  Done lazily, because it's safer (known to not
- * be writing pio buffers).
- */
-static void ipath_reset_availshadow(struct ipath_devdata *dd)
-{
-	int i, im;
-	unsigned long flags;
-
-	spin_lock_irqsave(&ipath_pioavail_lock, flags);
-	for (i = 0; i < dd->ipath_pioavregs; i++) {
-		u64 val, oldval;
-		/* deal with 6110 chip bug on high register #s */
-		im = (i > 3 && (dd->ipath_flags & IPATH_SWAP_PIOBUFS)) ?
-			i ^ 1 : i;
-		val = le64_to_cpu(dd->ipath_pioavailregs_dma[im]);
-		/*
-		 * busy out the buffers not in the kernel avail list,
-		 * without changing the generation bits.
-		 */
-		oldval = dd->ipath_pioavailshadow[i];
-		dd->ipath_pioavailshadow[i] = val |
-			((~dd->ipath_pioavailkernel[i] <<
-			INFINIPATH_SENDPIOAVAIL_BUSY_SHIFT) &
-			0xaaaaaaaaaaaaaaaaULL); /* All BUSY bits in qword */
-		if (oldval != dd->ipath_pioavailshadow[i])
-			ipath_dbg("shadow[%d] was %Lx, now %lx\n",
-				i, (unsigned long long) oldval,
-				dd->ipath_pioavailshadow[i]);
-	}
-	spin_unlock_irqrestore(&ipath_pioavail_lock, flags);
-}
-
-/**
- * ipath_setrcvhdrsize - set the receive header size
- * @dd: the infinipath device
- * @rhdrsize: the receive header size
- *
- * called from user init code, and also layered driver init
- */
-int ipath_setrcvhdrsize(struct ipath_devdata *dd, unsigned rhdrsize)
-{
-	int ret = 0;
-
-	if (dd->ipath_flags & IPATH_RCVHDRSZ_SET) {
-		if (dd->ipath_rcvhdrsize != rhdrsize) {
-			dev_info(&dd->pcidev->dev,
-				 "Error: can't set protocol header "
-				 "size %u, already %u\n",
-				 rhdrsize, dd->ipath_rcvhdrsize);
-			ret = -EAGAIN;
-		} else
-			ipath_cdbg(VERBOSE, "Reuse same protocol header "
-				   "size %u\n", dd->ipath_rcvhdrsize);
-	} else if (rhdrsize > (dd->ipath_rcvhdrentsize -
-			       (sizeof(u64) / sizeof(u32)))) {
-		ipath_dbg("Error: can't set protocol header size %u "
-			  "(> max %u)\n", rhdrsize,
-			  dd->ipath_rcvhdrentsize -
-			  (u32) (sizeof(u64) / sizeof(u32)));
-		ret = -EOVERFLOW;
-	} else {
-		dd->ipath_flags |= IPATH_RCVHDRSZ_SET;
-		dd->ipath_rcvhdrsize = rhdrsize;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvhdrsize,
-				 dd->ipath_rcvhdrsize);
-		ipath_cdbg(VERBOSE, "Set protocol header size to %u\n",
-			   dd->ipath_rcvhdrsize);
-	}
-	return ret;
-}
-
-/*
- * debugging code and stats updates if no pio buffers available.
- */
-static noinline void no_pio_bufs(struct ipath_devdata *dd)
-{
-	unsigned long *shadow = dd->ipath_pioavailshadow;
-	__le64 *dma = (__le64 *)dd->ipath_pioavailregs_dma;
-
-	dd->ipath_upd_pio_shadow = 1;
-
-	/*
-	 * not atomic, but if we lose a stat count in a while, that's OK
-	 */
-	ipath_stats.sps_nopiobufs++;
-	if (!(++dd->ipath_consec_nopiobuf % 100000)) {
-		ipath_force_pio_avail_update(dd); /* at start */
-		ipath_dbg("%u tries no piobufavail ts%lx; dmacopy: "
-			"%llx %llx %llx %llx\n"
-			"ipath  shadow:  %lx %lx %lx %lx\n",
-			dd->ipath_consec_nopiobuf,
-			(unsigned long)get_cycles(),
-			(unsigned long long) le64_to_cpu(dma[0]),
-			(unsigned long long) le64_to_cpu(dma[1]),
-			(unsigned long long) le64_to_cpu(dma[2]),
-			(unsigned long long) le64_to_cpu(dma[3]),
-			shadow[0], shadow[1], shadow[2], shadow[3]);
-		/*
-		 * 4 buffers per byte, 4 registers above, cover rest
-		 * below
-		 */
-		if ((dd->ipath_piobcnt2k + dd->ipath_piobcnt4k) >
-		    (sizeof(shadow[0]) * 4 * 4))
-			ipath_dbg("2nd group: dmacopy: "
-				  "%llx %llx %llx %llx\n"
-				  "ipath  shadow:  %lx %lx %lx %lx\n",
-				  (unsigned long long)le64_to_cpu(dma[4]),
-				  (unsigned long long)le64_to_cpu(dma[5]),
-				  (unsigned long long)le64_to_cpu(dma[6]),
-				  (unsigned long long)le64_to_cpu(dma[7]),
-				  shadow[4], shadow[5], shadow[6], shadow[7]);
-
-		/* at end, so update likely happened */
-		ipath_reset_availshadow(dd);
-	}
-}
-
-/*
- * common code for normal driver pio buffer allocation, and reserved
- * allocation.
- *
- * do appropriate marking as busy, etc.
- * returns buffer number if one found (>=0), negative number is error.
- */
-static u32 __iomem *ipath_getpiobuf_range(struct ipath_devdata *dd,
-	u32 *pbufnum, u32 first, u32 last, u32 firsti)
-{
-	int i, j, updated = 0;
-	unsigned piobcnt;
-	unsigned long flags;
-	unsigned long *shadow = dd->ipath_pioavailshadow;
-	u32 __iomem *buf;
-
-	piobcnt = last - first;
-	if (dd->ipath_upd_pio_shadow) {
-		/*
-		 * Minor optimization.  If we had no buffers on last call,
-		 * start out by doing the update; continue and do scan even
-		 * if no buffers were updated, to be paranoid
-		 */
-		ipath_update_pio_bufs(dd);
-		updated++;
-		i = first;
-	} else
-		i = firsti;
-rescan:
-	/*
-	 * while test_and_set_bit() is atomic, we do that and then the
-	 * change_bit(), and the pair is not.  See if this is the cause
-	 * of the remaining armlaunch errors.
-	 */
-	spin_lock_irqsave(&ipath_pioavail_lock, flags);
-	for (j = 0; j < piobcnt; j++, i++) {
-		if (i >= last)
-			i = first;
-		if (__test_and_set_bit((2 * i) + 1, shadow))
-			continue;
-		/* flip generation bit */
-		__change_bit(2 * i, shadow);
-		break;
-	}
-	spin_unlock_irqrestore(&ipath_pioavail_lock, flags);
-
-	if (j == piobcnt) {
-		if (!updated) {
-			/*
-			 * first time through; shadow exhausted, but may be
-			 * buffers available, try an update and then rescan.
-			 */
-			ipath_update_pio_bufs(dd);
-			updated++;
-			i = first;
-			goto rescan;
-		} else if (updated == 1 && piobcnt <=
-			((dd->ipath_sendctrl
-			>> INFINIPATH_S_UPDTHRESH_SHIFT) &
-			INFINIPATH_S_UPDTHRESH_MASK)) {
-			/*
-			 * for chips supporting and using the update
-			 * threshold we need to force an update of the
-			 * in-memory copy if the count is less than the
-			 * thershold, then check one more time.
-			 */
-			ipath_force_pio_avail_update(dd);
-			ipath_update_pio_bufs(dd);
-			updated++;
-			i = first;
-			goto rescan;
-		}
-
-		no_pio_bufs(dd);
-		buf = NULL;
-	} else {
-		if (i < dd->ipath_piobcnt2k)
-			buf = (u32 __iomem *) (dd->ipath_pio2kbase +
-					       i * dd->ipath_palign);
-		else
-			buf = (u32 __iomem *)
-				(dd->ipath_pio4kbase +
-				 (i - dd->ipath_piobcnt2k) * dd->ipath_4kalign);
-		if (pbufnum)
-			*pbufnum = i;
-	}
-
-	return buf;
-}
-
-/**
- * ipath_getpiobuf - find an available pio buffer
- * @dd: the infinipath device
- * @plen: the size of the PIO buffer needed in 32-bit words
- * @pbufnum: the buffer number is placed here
- */
-u32 __iomem *ipath_getpiobuf(struct ipath_devdata *dd, u32 plen, u32 *pbufnum)
-{
-	u32 __iomem *buf;
-	u32 pnum, nbufs;
-	u32 first, lasti;
-
-	if (plen + 1 >= IPATH_SMALLBUF_DWORDS) {
-		first = dd->ipath_piobcnt2k;
-		lasti = dd->ipath_lastpioindexl;
-	} else {
-		first = 0;
-		lasti = dd->ipath_lastpioindex;
-	}
-	nbufs = dd->ipath_piobcnt2k + dd->ipath_piobcnt4k;
-	buf = ipath_getpiobuf_range(dd, &pnum, first, nbufs, lasti);
-
-	if (buf) {
-		/*
-		 * Set next starting place.  It's just an optimization,
-		 * it doesn't matter who wins on this, so no locking
-		 */
-		if (plen + 1 >= IPATH_SMALLBUF_DWORDS)
-			dd->ipath_lastpioindexl = pnum + 1;
-		else
-			dd->ipath_lastpioindex = pnum + 1;
-		if (dd->ipath_upd_pio_shadow)
-			dd->ipath_upd_pio_shadow = 0;
-		if (dd->ipath_consec_nopiobuf)
-			dd->ipath_consec_nopiobuf = 0;
-		ipath_cdbg(VERBOSE, "Return piobuf%u %uk @ %p\n",
-			   pnum, (pnum < dd->ipath_piobcnt2k) ? 2 : 4, buf);
-		if (pbufnum)
-			*pbufnum = pnum;
-
-	}
-	return buf;
-}
-
-/**
- * ipath_chg_pioavailkernel - change which send buffers are available for kernel
- * @dd: the infinipath device
- * @start: the starting send buffer number
- * @len: the number of send buffers
- * @avail: true if the buffers are available for kernel use, false otherwise
- */
-void ipath_chg_pioavailkernel(struct ipath_devdata *dd, unsigned start,
-			      unsigned len, int avail)
-{
-	unsigned long flags;
-	unsigned end, cnt = 0;
-
-	/* There are two bits per send buffer (busy and generation) */
-	start *= 2;
-	end = start + len * 2;
-
-	spin_lock_irqsave(&ipath_pioavail_lock, flags);
-	/* Set or clear the busy bit in the shadow. */
-	while (start < end) {
-		if (avail) {
-			unsigned long dma;
-			int i, im;
-			/*
-			 * the BUSY bit will never be set, because we disarm
-			 * the user buffers before we hand them back to the
-			 * kernel.  We do have to make sure the generation
-			 * bit is set correctly in shadow, since it could
-			 * have changed many times while allocated to user.
-			 * We can't use the bitmap functions on the full
-			 * dma array because it is always little-endian, so
-			 * we have to flip to host-order first.
-			 * BITS_PER_LONG is slightly wrong, since it's
-			 * always 64 bits per register in chip...
-			 * We only work on 64 bit kernels, so that's OK.
-			 */
-			/* deal with 6110 chip bug on high register #s */
-			i = start / BITS_PER_LONG;
-			im = (i > 3 && (dd->ipath_flags & IPATH_SWAP_PIOBUFS)) ?
-				i ^ 1 : i;
-			__clear_bit(INFINIPATH_SENDPIOAVAIL_BUSY_SHIFT
-				+ start, dd->ipath_pioavailshadow);
-			dma = (unsigned long) le64_to_cpu(
-				dd->ipath_pioavailregs_dma[im]);
-			if (test_bit((INFINIPATH_SENDPIOAVAIL_CHECK_SHIFT
-				+ start) % BITS_PER_LONG, &dma))
-				__set_bit(INFINIPATH_SENDPIOAVAIL_CHECK_SHIFT
-					+ start, dd->ipath_pioavailshadow);
-			else
-				__clear_bit(INFINIPATH_SENDPIOAVAIL_CHECK_SHIFT
-					+ start, dd->ipath_pioavailshadow);
-			__set_bit(start, dd->ipath_pioavailkernel);
-		} else {
-			__set_bit(start + INFINIPATH_SENDPIOAVAIL_BUSY_SHIFT,
-				dd->ipath_pioavailshadow);
-			__clear_bit(start, dd->ipath_pioavailkernel);
-		}
-		start += 2;
-	}
-
-	if (dd->ipath_pioupd_thresh) {
-		end = 2 * (dd->ipath_piobcnt2k + dd->ipath_piobcnt4k);
-		cnt = bitmap_weight(dd->ipath_pioavailkernel, end);
-	}
-	spin_unlock_irqrestore(&ipath_pioavail_lock, flags);
-
-	/*
-	 * When moving buffers from kernel to user, if number assigned to
-	 * the user is less than the pio update threshold, and threshold
-	 * is supported (cnt was computed > 0), drop the update threshold
-	 * so we update at least once per allocated number of buffers.
-	 * In any case, if the kernel buffers are less than the threshold,
-	 * drop the threshold.  We don't bother increasing it, having once
-	 * decreased it, since it would typically just cycle back and forth.
-	 * If we don't decrease below buffers in use, we can wait a long
-	 * time for an update, until some other context uses PIO buffers.
-	 */
-	if (!avail && len < cnt)
-		cnt = len;
-	if (cnt < dd->ipath_pioupd_thresh) {
-		dd->ipath_pioupd_thresh = cnt;
-		ipath_dbg("Decreased pio update threshold to %u\n",
-			dd->ipath_pioupd_thresh);
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		dd->ipath_sendctrl &= ~(INFINIPATH_S_UPDTHRESH_MASK
-			<< INFINIPATH_S_UPDTHRESH_SHIFT);
-		dd->ipath_sendctrl |= dd->ipath_pioupd_thresh
-			<< INFINIPATH_S_UPDTHRESH_SHIFT;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			dd->ipath_sendctrl);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-	}
-}
-
-/**
- * ipath_create_rcvhdrq - create a receive header queue
- * @dd: the infinipath device
- * @pd: the port data
- *
- * this must be contiguous memory (from an i/o perspective), and must be
- * DMA'able (which means for some systems, it will go through an IOMMU,
- * or be forced into a low address range).
- */
-int ipath_create_rcvhdrq(struct ipath_devdata *dd,
-			 struct ipath_portdata *pd)
-{
-	int ret = 0;
-
-	if (!pd->port_rcvhdrq) {
-		dma_addr_t phys_hdrqtail;
-		gfp_t gfp_flags = GFP_USER | __GFP_COMP;
-		int amt = ALIGN(dd->ipath_rcvhdrcnt * dd->ipath_rcvhdrentsize *
-				sizeof(u32), PAGE_SIZE);
-
-		pd->port_rcvhdrq = dma_alloc_coherent(
-			&dd->pcidev->dev, amt, &pd->port_rcvhdrq_phys,
-			gfp_flags);
-
-		if (!pd->port_rcvhdrq) {
-			ipath_dev_err(dd, "attempt to allocate %d bytes "
-				      "for port %u rcvhdrq failed\n",
-				      amt, pd->port_port);
-			ret = -ENOMEM;
-			goto bail;
-		}
-
-		if (!(dd->ipath_flags & IPATH_NODMA_RTAIL)) {
-			pd->port_rcvhdrtail_kvaddr = dma_alloc_coherent(
-				&dd->pcidev->dev, PAGE_SIZE, &phys_hdrqtail,
-				GFP_KERNEL);
-			if (!pd->port_rcvhdrtail_kvaddr) {
-				ipath_dev_err(dd, "attempt to allocate 1 page "
-					"for port %u rcvhdrqtailaddr "
-					"failed\n", pd->port_port);
-				ret = -ENOMEM;
-				dma_free_coherent(&dd->pcidev->dev, amt,
-					pd->port_rcvhdrq,
-					pd->port_rcvhdrq_phys);
-				pd->port_rcvhdrq = NULL;
-				goto bail;
-			}
-			pd->port_rcvhdrqtailaddr_phys = phys_hdrqtail;
-			ipath_cdbg(VERBOSE, "port %d hdrtailaddr, %llx "
-				   "physical\n", pd->port_port,
-				   (unsigned long long) phys_hdrqtail);
-		}
-
-		pd->port_rcvhdrq_size = amt;
-
-		ipath_cdbg(VERBOSE, "%d pages at %p (phys %lx) size=%lu "
-			   "for port %u rcvhdr Q\n",
-			   amt >> PAGE_SHIFT, pd->port_rcvhdrq,
-			   (unsigned long) pd->port_rcvhdrq_phys,
-			   (unsigned long) pd->port_rcvhdrq_size,
-			   pd->port_port);
-	} else {
-		ipath_cdbg(VERBOSE, "reuse port %d rcvhdrq @%p %llx phys; "
-			   "hdrtailaddr@%p %llx physical\n",
-			   pd->port_port, pd->port_rcvhdrq,
-			   (unsigned long long) pd->port_rcvhdrq_phys,
-			   pd->port_rcvhdrtail_kvaddr, (unsigned long long)
-			   pd->port_rcvhdrqtailaddr_phys);
-	}
-	/* clear for security and sanity on each use */
-	memset(pd->port_rcvhdrq, 0, pd->port_rcvhdrq_size);
-	if (pd->port_rcvhdrtail_kvaddr)
-		memset(pd->port_rcvhdrtail_kvaddr, 0, PAGE_SIZE);
-
-	/*
-	 * tell chip each time we init it, even if we are re-using previous
-	 * memory (we zero the register at process close)
-	 */
-	ipath_write_kreg_port(dd, dd->ipath_kregs->kr_rcvhdrtailaddr,
-			      pd->port_port, pd->port_rcvhdrqtailaddr_phys);
-	ipath_write_kreg_port(dd, dd->ipath_kregs->kr_rcvhdraddr,
-			      pd->port_port, pd->port_rcvhdrq_phys);
-
-bail:
-	return ret;
-}
-
-
-/*
- * Flush all sends that might be in the ready to send state, as well as any
- * that are in the process of being sent.   Used whenever we need to be
- * sure the send side is idle.  Cleans up all buffer state by canceling
- * all pio buffers, and issuing an abort, which cleans up anything in the
- * launch fifo.  The cancel is superfluous on some chip versions, but
- * it's safer to always do it.
- * PIOAvail bits are updated by the chip as if normal send had happened.
- */
-void ipath_cancel_sends(struct ipath_devdata *dd, int restore_sendctrl)
-{
-	unsigned long flags;
-
-	if (dd->ipath_flags & IPATH_IB_AUTONEG_INPROG) {
-		ipath_cdbg(VERBOSE, "Ignore while in autonegotiation\n");
-		goto bail;
-	}
-	/*
-	 * If we have SDMA, and it's not disabled, we have to kick off the
-	 * abort state machine, provided we aren't already aborting.
-	 * If we are in the process of aborting SDMA (!DISABLED, but ABORTING),
-	 * we skip the rest of this routine. It is already "in progress"
-	 */
-	if (dd->ipath_flags & IPATH_HAS_SEND_DMA) {
-		int skip_cancel;
-		unsigned long *statp = &dd->ipath_sdma_status;
-
-		spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-		skip_cancel =
-			test_and_set_bit(IPATH_SDMA_ABORTING, statp)
-			&& !test_bit(IPATH_SDMA_DISABLED, statp);
-		spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-		if (skip_cancel)
-			goto bail;
-	}
-
-	ipath_dbg("Cancelling all in-progress send buffers\n");
-
-	/* skip armlaunch errs for a while */
-	dd->ipath_lastcancel = jiffies + HZ / 2;
-
-	/*
-	 * The abort bit is auto-clearing.  We also don't want pioavail
-	 * update happening during this, and we don't want any other
-	 * sends going out, so turn those off for the duration.  We read
-	 * the scratch register to be sure that cancels and the abort
-	 * have taken effect in the chip.  Otherwise two parts are same
-	 * as ipath_force_pio_avail_update()
-	 */
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl &= ~(INFINIPATH_S_PIOBUFAVAILUPD
-		| INFINIPATH_S_PIOENABLE);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-		dd->ipath_sendctrl | INFINIPATH_S_ABORT);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	/* disarm all send buffers */
-	ipath_disarm_piobufs(dd, 0,
-		dd->ipath_piobcnt2k + dd->ipath_piobcnt4k);
-
-	if (dd->ipath_flags & IPATH_HAS_SEND_DMA)
-		set_bit(IPATH_SDMA_DISARMED, &dd->ipath_sdma_status);
-
-	if (restore_sendctrl) {
-		/* else done by caller later if needed */
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		dd->ipath_sendctrl |= INFINIPATH_S_PIOBUFAVAILUPD |
-			INFINIPATH_S_PIOENABLE;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			dd->ipath_sendctrl);
-		/* and again, be sure all have hit the chip */
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-	}
-
-	if ((dd->ipath_flags & IPATH_HAS_SEND_DMA) &&
-	    !test_bit(IPATH_SDMA_DISABLED, &dd->ipath_sdma_status) &&
-	    test_bit(IPATH_SDMA_RUNNING, &dd->ipath_sdma_status)) {
-		spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-		/* only wait so long for intr */
-		dd->ipath_sdma_abort_intr_timeout = jiffies + HZ;
-		dd->ipath_sdma_reset_wait = 200;
-		if (!test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-			tasklet_hi_schedule(&dd->ipath_sdma_abort_task);
-		spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-	}
-bail:;
-}
-
-/*
- * Force an update of in-memory copy of the pioavail registers, when
- * needed for any of a variety of reasons.  We read the scratch register
- * to make it highly likely that the update will have happened by the
- * time we return.  If already off (as in cancel_sends above), this
- * routine is a nop, on the assumption that the caller will "do the
- * right thing".
- */
-void ipath_force_pio_avail_update(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	if (dd->ipath_sendctrl & INFINIPATH_S_PIOBUFAVAILUPD) {
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			dd->ipath_sendctrl & ~INFINIPATH_S_PIOBUFAVAILUPD);
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			dd->ipath_sendctrl);
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	}
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-}
-
-static void ipath_set_ib_lstate(struct ipath_devdata *dd, int linkcmd,
-				int linitcmd)
-{
-	u64 mod_wd;
-	static const char *what[4] = {
-		[0] = "NOP",
-		[INFINIPATH_IBCC_LINKCMD_DOWN] = "DOWN",
-		[INFINIPATH_IBCC_LINKCMD_ARMED] = "ARMED",
-		[INFINIPATH_IBCC_LINKCMD_ACTIVE] = "ACTIVE"
-	};
-
-	if (linitcmd == INFINIPATH_IBCC_LINKINITCMD_DISABLE) {
-		/*
-		 * If we are told to disable, note that so link-recovery
-		 * code does not attempt to bring us back up.
-		 */
-		preempt_disable();
-		dd->ipath_flags |= IPATH_IB_LINK_DISABLED;
-		preempt_enable();
-	} else if (linitcmd) {
-		/*
-		 * Any other linkinitcmd will lead to LINKDOWN and then
-		 * to INIT (if all is well), so clear flag to let
-		 * link-recovery code attempt to bring us back up.
-		 */
-		preempt_disable();
-		dd->ipath_flags &= ~IPATH_IB_LINK_DISABLED;
-		preempt_enable();
-	}
-
-	mod_wd = (linkcmd << dd->ibcc_lc_shift) |
-		(linitcmd << INFINIPATH_IBCC_LINKINITCMD_SHIFT);
-	ipath_cdbg(VERBOSE,
-		"Moving unit %u to %s (initcmd=0x%x), current ltstate is %s\n",
-		dd->ipath_unit, what[linkcmd], linitcmd,
-		ipath_ibcstatus_str[ipath_ib_linktrstate(dd,
-			ipath_read_kreg64(dd, dd->ipath_kregs->kr_ibcstatus))]);
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-			 dd->ipath_ibcctrl | mod_wd);
-	/* read from chip so write is flushed */
-	(void) ipath_read_kreg64(dd, dd->ipath_kregs->kr_ibcstatus);
-}
-
-int ipath_set_linkstate(struct ipath_devdata *dd, u8 newstate)
-{
-	u32 lstate;
-	int ret;
-
-	switch (newstate) {
-	case IPATH_IB_LINKDOWN_ONLY:
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_DOWN, 0);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	case IPATH_IB_LINKDOWN:
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_DOWN,
-					INFINIPATH_IBCC_LINKINITCMD_POLL);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	case IPATH_IB_LINKDOWN_SLEEP:
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_DOWN,
-					INFINIPATH_IBCC_LINKINITCMD_SLEEP);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	case IPATH_IB_LINKDOWN_DISABLE:
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_DOWN,
-					INFINIPATH_IBCC_LINKINITCMD_DISABLE);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	case IPATH_IB_LINKARM:
-		if (dd->ipath_flags & IPATH_LINKARMED) {
-			ret = 0;
-			goto bail;
-		}
-		if (!(dd->ipath_flags &
-		      (IPATH_LINKINIT | IPATH_LINKACTIVE))) {
-			ret = -EINVAL;
-			goto bail;
-		}
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_ARMED, 0);
-
-		/*
-		 * Since the port can transition to ACTIVE by receiving
-		 * a non VL 15 packet, wait for either state.
-		 */
-		lstate = IPATH_LINKARMED | IPATH_LINKACTIVE;
-		break;
-
-	case IPATH_IB_LINKACTIVE:
-		if (dd->ipath_flags & IPATH_LINKACTIVE) {
-			ret = 0;
-			goto bail;
-		}
-		if (!(dd->ipath_flags & IPATH_LINKARMED)) {
-			ret = -EINVAL;
-			goto bail;
-		}
-		ipath_set_ib_lstate(dd, INFINIPATH_IBCC_LINKCMD_ACTIVE, 0);
-		lstate = IPATH_LINKACTIVE;
-		break;
-
-	case IPATH_IB_LINK_LOOPBACK:
-		dev_info(&dd->pcidev->dev, "Enabling IB local loopback\n");
-		dd->ipath_ibcctrl |= INFINIPATH_IBCC_LOOPBACK;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-				 dd->ipath_ibcctrl);
-
-		/* turn heartbeat off, as it causes loopback to fail */
-		dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_HRTBT,
-				       IPATH_IB_HRTBT_OFF);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	case IPATH_IB_LINK_EXTERNAL:
-		dev_info(&dd->pcidev->dev,
-			"Disabling IB local loopback (normal)\n");
-		dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_HRTBT,
-				       IPATH_IB_HRTBT_ON);
-		dd->ipath_ibcctrl &= ~INFINIPATH_IBCC_LOOPBACK;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-				 dd->ipath_ibcctrl);
-		/* don't wait */
-		ret = 0;
-		goto bail;
-
-	/*
-	 * Heartbeat can be explicitly enabled by the user via
-	 * "hrtbt_enable" "file", and if disabled, trying to enable here
-	 * will have no effect.  Implicit changes (heartbeat off when
-	 * loopback on, and vice versa) are included to ease testing.
-	 */
-	case IPATH_IB_LINK_HRTBT:
-		ret = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_HRTBT,
-			IPATH_IB_HRTBT_ON);
-		goto bail;
-
-	case IPATH_IB_LINK_NO_HRTBT:
-		ret = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_HRTBT,
-			IPATH_IB_HRTBT_OFF);
-		goto bail;
-
-	default:
-		ipath_dbg("Invalid linkstate 0x%x requested\n", newstate);
-		ret = -EINVAL;
-		goto bail;
-	}
-	ret = ipath_wait_linkstate(dd, lstate, 2000);
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_set_mtu - set the MTU
- * @dd: the infinipath device
- * @arg: the new MTU
- *
- * we can handle "any" incoming size, the issue here is whether we
- * need to restrict our outgoing size.   For now, we don't do any
- * sanity checking on this, and we don't deal with what happens to
- * programs that are already running when the size changes.
- * NOTE: changing the MTU will usually cause the IBC to go back to
- * link INIT state...
- */
-int ipath_set_mtu(struct ipath_devdata *dd, u16 arg)
-{
-	u32 piosize;
-	int changed = 0;
-	int ret;
-
-	/*
-	 * mtu is IB data payload max.  It's the largest power of 2 less
-	 * than piosize (or even larger, since it only really controls the
-	 * largest we can receive; we can send the max of the mtu and
-	 * piosize).  We check that it's one of the valid IB sizes.
-	 */
-	if (arg != 256 && arg != 512 && arg != 1024 && arg != 2048 &&
-	    (arg != 4096 || !ipath_mtu4096)) {
-		ipath_dbg("Trying to set invalid mtu %u, failing\n", arg);
-		ret = -EINVAL;
-		goto bail;
-	}
-	if (dd->ipath_ibmtu == arg) {
-		ret = 0;        /* same as current */
-		goto bail;
-	}
-
-	piosize = dd->ipath_ibmaxlen;
-	dd->ipath_ibmtu = arg;
-
-	if (arg >= (piosize - IPATH_PIO_MAXIBHDR)) {
-		/* Only if it's not the initial value (or reset to it) */
-		if (piosize != dd->ipath_init_ibmaxlen) {
-			if (arg > piosize && arg <= dd->ipath_init_ibmaxlen)
-				piosize = dd->ipath_init_ibmaxlen;
-			dd->ipath_ibmaxlen = piosize;
-			changed = 1;
-		}
-	} else if ((arg + IPATH_PIO_MAXIBHDR) != dd->ipath_ibmaxlen) {
-		piosize = arg + IPATH_PIO_MAXIBHDR;
-		ipath_cdbg(VERBOSE, "ibmaxlen was 0x%x, setting to 0x%x "
-			   "(mtu 0x%x)\n", dd->ipath_ibmaxlen, piosize,
-			   arg);
-		dd->ipath_ibmaxlen = piosize;
-		changed = 1;
-	}
-
-	if (changed) {
-		u64 ibc = dd->ipath_ibcctrl, ibdw;
-		/*
-		 * update our housekeeping variables, and set IBC max
-		 * size, same as init code; max IBC is max we allow in
-		 * buffer, less the qword pbc, plus 1 for ICRC, in dwords
-		 */
-		dd->ipath_ibmaxlen = piosize - 2 * sizeof(u32);
-		ibdw = (dd->ipath_ibmaxlen >> 2) + 1;
-		ibc &= ~(INFINIPATH_IBCC_MAXPKTLEN_MASK <<
-			 dd->ibcc_mpl_shift);
-		ibc |= ibdw << dd->ibcc_mpl_shift;
-		dd->ipath_ibcctrl = ibc;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-				 dd->ipath_ibcctrl);
-		dd->ipath_f_tidtemplate(dd);
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-int ipath_set_lid(struct ipath_devdata *dd, u32 lid, u8 lmc)
-{
-	dd->ipath_lid = lid;
-	dd->ipath_lmc = lmc;
-
-	dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_LIDLMC, lid |
-		(~((1U << lmc) - 1)) << 16);
-
-	dev_info(&dd->pcidev->dev, "We got a lid: 0x%x\n", lid);
-
-	return 0;
-}
-
-
-/**
- * ipath_write_kreg_port - write a device's per-port 64-bit kernel register
- * @dd: the infinipath device
- * @regno: the register number to write
- * @port: the port containing the register
- * @value: the value to write
- *
- * Registers that vary with the chip implementation constants (port)
- * use this routine.
- */
-void ipath_write_kreg_port(const struct ipath_devdata *dd, ipath_kreg regno,
-			  unsigned port, u64 value)
-{
-	u16 where;
-
-	if (port < dd->ipath_portcnt &&
-	    (regno == dd->ipath_kregs->kr_rcvhdraddr ||
-	     regno == dd->ipath_kregs->kr_rcvhdrtailaddr))
-		where = regno + port;
-	else
-		where = -1;
-
-	ipath_write_kreg(dd, where, value);
-}
-
-/*
- * Following deal with the "obviously simple" task of overriding the state
- * of the LEDS, which normally indicate link physical and logical status.
- * The complications arise in dealing with different hardware mappings
- * and the board-dependent routine being called from interrupts.
- * and then there's the requirement to _flash_ them.
- */
-#define LED_OVER_FREQ_SHIFT 8
-#define LED_OVER_FREQ_MASK (0xFF<<LED_OVER_FREQ_SHIFT)
-/* Below is "non-zero" to force override, but both actual LEDs are off */
-#define LED_OVER_BOTH_OFF (8)
-
-static void ipath_run_led_override(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *)opaque;
-	int timeoff;
-	int pidx;
-	u64 lstate, ltstate, val;
-
-	if (!(dd->ipath_flags & IPATH_INITTED))
-		return;
-
-	pidx = dd->ipath_led_override_phase++ & 1;
-	dd->ipath_led_override = dd->ipath_led_override_vals[pidx];
-	timeoff = dd->ipath_led_override_timeoff;
-
-	/*
-	 * below potentially restores the LED values per current status,
-	 * should also possibly setup the traffic-blink register,
-	 * but leave that to per-chip functions.
-	 */
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_ibcstatus);
-	ltstate = ipath_ib_linktrstate(dd, val);
-	lstate = ipath_ib_linkstate(dd, val);
-
-	dd->ipath_f_setextled(dd, lstate, ltstate);
-	mod_timer(&dd->ipath_led_override_timer, jiffies + timeoff);
-}
-
-void ipath_set_led_override(struct ipath_devdata *dd, unsigned int val)
-{
-	int timeoff, freq;
-
-	if (!(dd->ipath_flags & IPATH_INITTED))
-		return;
-
-	/* First check if we are blinking. If not, use 1HZ polling */
-	timeoff = HZ;
-	freq = (val & LED_OVER_FREQ_MASK) >> LED_OVER_FREQ_SHIFT;
-
-	if (freq) {
-		/* For blink, set each phase from one nybble of val */
-		dd->ipath_led_override_vals[0] = val & 0xF;
-		dd->ipath_led_override_vals[1] = (val >> 4) & 0xF;
-		timeoff = (HZ << 4)/freq;
-	} else {
-		/* Non-blink set both phases the same. */
-		dd->ipath_led_override_vals[0] = val & 0xF;
-		dd->ipath_led_override_vals[1] = val & 0xF;
-	}
-	dd->ipath_led_override_timeoff = timeoff;
-
-	/*
-	 * If the timer has not already been started, do so. Use a "quick"
-	 * timeout so the function will be called soon, to look at our request.
-	 */
-	if (atomic_inc_return(&dd->ipath_led_override_timer_active) == 1) {
-		/* Need to start timer */
-		setup_timer(&dd->ipath_led_override_timer,
-				ipath_run_led_override, (unsigned long)dd);
-
-		dd->ipath_led_override_timer.expires = jiffies + 1;
-		add_timer(&dd->ipath_led_override_timer);
-	} else
-		atomic_dec(&dd->ipath_led_override_timer_active);
-}
-
-/**
- * ipath_shutdown_device - shut down a device
- * @dd: the infinipath device
- *
- * This is called to make the device quiet when we are about to
- * unload the driver, and also when the device is administratively
- * disabled.   It does not free any data structures.
- * Everything it does has to be setup again by ipath_init_chip(dd,1)
- */
-void ipath_shutdown_device(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-
-	ipath_dbg("Shutting down the device\n");
-
-	ipath_hol_up(dd); /* make sure user processes aren't suspended */
-
-	dd->ipath_flags |= IPATH_LINKUNK;
-	dd->ipath_flags &= ~(IPATH_INITTED | IPATH_LINKDOWN |
-			     IPATH_LINKINIT | IPATH_LINKARMED |
-			     IPATH_LINKACTIVE);
-	*dd->ipath_statusp &= ~(IPATH_STATUS_IB_CONF |
-				IPATH_STATUS_IB_READY);
-
-	/* mask interrupts, but not errors */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask, 0ULL);
-
-	dd->ipath_rcvctrl = 0;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			 dd->ipath_rcvctrl);
-
-	if (dd->ipath_flags & IPATH_HAS_SEND_DMA)
-		teardown_sdma(dd);
-
-	/*
-	 * gracefully stop all sends allowing any in progress to trickle out
-	 * first.
-	 */
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl = 0;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	/* flush it */
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	/*
-	 * enough for anything that's going to trickle out to have actually
-	 * done so.
-	 */
-	udelay(5);
-
-	dd->ipath_f_setextled(dd, 0, 0); /* make sure LEDs are off */
-
-	ipath_set_ib_lstate(dd, 0, INFINIPATH_IBCC_LINKINITCMD_DISABLE);
-	ipath_cancel_sends(dd, 0);
-
-	/*
-	 * we are shutting down, so tell components that care.  We don't do
-	 * this on just a link state change, much like ethernet, a cable
-	 * unplug, etc. doesn't change driver state
-	 */
-	signal_ib_event(dd, IB_EVENT_PORT_ERR);
-
-	/* disable IBC */
-	dd->ipath_control &= ~INFINIPATH_C_LINKENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-			 dd->ipath_control | INFINIPATH_C_FREEZEMODE);
-
-	/*
-	 * clear SerdesEnable and turn the leds off; do this here because
-	 * we are unloading, so don't count on interrupts to move along
-	 * Turn the LEDs off explicitly for the same reason.
-	 */
-	dd->ipath_f_quiet_serdes(dd);
-
-	/* stop all the timers that might still be running */
-	del_timer_sync(&dd->ipath_hol_timer);
-	if (dd->ipath_stats_timer_active) {
-		del_timer_sync(&dd->ipath_stats_timer);
-		dd->ipath_stats_timer_active = 0;
-	}
-	if (dd->ipath_intrchk_timer.data) {
-		del_timer_sync(&dd->ipath_intrchk_timer);
-		dd->ipath_intrchk_timer.data = 0;
-	}
-	if (atomic_read(&dd->ipath_led_override_timer_active)) {
-		del_timer_sync(&dd->ipath_led_override_timer);
-		atomic_set(&dd->ipath_led_override_timer_active, 0);
-	}
-
-	/*
-	 * clear all interrupts and errors, so that the next time the driver
-	 * is loaded or device is enabled, we know that whatever is set
-	 * happened while we were unloaded
-	 */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear,
-			 ~0ULL & ~INFINIPATH_HWE_MEMBISTFAILED);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear, -1LL);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, -1LL);
-
-	ipath_cdbg(VERBOSE, "Flush time and errors to EEPROM\n");
-	ipath_update_eeprom_log(dd);
-}
-
-/**
- * ipath_free_pddata - free a port's allocated data
- * @dd: the infinipath device
- * @pd: the portdata structure
- *
- * free up any allocated data for a port
- * This should not touch anything that would affect a simultaneous
- * re-allocation of port data, because it is called after ipath_mutex
- * is released (and can be called from reinit as well).
- * It should never change any chip state, or global driver state.
- * (The only exception to global state is freeing the port0 port0_skbs.)
- */
-void ipath_free_pddata(struct ipath_devdata *dd, struct ipath_portdata *pd)
-{
-	if (!pd)
-		return;
-
-	if (pd->port_rcvhdrq) {
-		ipath_cdbg(VERBOSE, "free closed port %d rcvhdrq @ %p "
-			   "(size=%lu)\n", pd->port_port, pd->port_rcvhdrq,
-			   (unsigned long) pd->port_rcvhdrq_size);
-		dma_free_coherent(&dd->pcidev->dev, pd->port_rcvhdrq_size,
-				  pd->port_rcvhdrq, pd->port_rcvhdrq_phys);
-		pd->port_rcvhdrq = NULL;
-		if (pd->port_rcvhdrtail_kvaddr) {
-			dma_free_coherent(&dd->pcidev->dev, PAGE_SIZE,
-					 pd->port_rcvhdrtail_kvaddr,
-					 pd->port_rcvhdrqtailaddr_phys);
-			pd->port_rcvhdrtail_kvaddr = NULL;
-		}
-	}
-	if (pd->port_port && pd->port_rcvegrbuf) {
-		unsigned e;
-
-		for (e = 0; e < pd->port_rcvegrbuf_chunks; e++) {
-			void *base = pd->port_rcvegrbuf[e];
-			size_t size = pd->port_rcvegrbuf_size;
-
-			ipath_cdbg(VERBOSE, "egrbuf free(%p, %lu), "
-				   "chunk %u/%u\n", base,
-				   (unsigned long) size,
-				   e, pd->port_rcvegrbuf_chunks);
-			dma_free_coherent(&dd->pcidev->dev, size,
-				base, pd->port_rcvegrbuf_phys[e]);
-		}
-		kfree(pd->port_rcvegrbuf);
-		pd->port_rcvegrbuf = NULL;
-		kfree(pd->port_rcvegrbuf_phys);
-		pd->port_rcvegrbuf_phys = NULL;
-		pd->port_rcvegrbuf_chunks = 0;
-	} else if (pd->port_port == 0 && dd->ipath_port0_skbinfo) {
-		unsigned e;
-		struct ipath_skbinfo *skbinfo = dd->ipath_port0_skbinfo;
-
-		dd->ipath_port0_skbinfo = NULL;
-		ipath_cdbg(VERBOSE, "free closed port %d "
-			   "ipath_port0_skbinfo @ %p\n", pd->port_port,
-			   skbinfo);
-		for (e = 0; e < dd->ipath_p0_rcvegrcnt; e++)
-			if (skbinfo[e].skb) {
-				pci_unmap_single(dd->pcidev, skbinfo[e].phys,
-						 dd->ipath_ibmaxlen,
-						 PCI_DMA_FROMDEVICE);
-				dev_kfree_skb(skbinfo[e].skb);
-			}
-		vfree(skbinfo);
-	}
-	kfree(pd->port_tid_pg_list);
-	vfree(pd->subport_uregbase);
-	vfree(pd->subport_rcvegrbuf);
-	vfree(pd->subport_rcvhdr_base);
-	kfree(pd);
-}
-
-static int __init infinipath_init(void)
-{
-	int ret;
-
-	if (ipath_debug & __IPATH_DBG)
-		printk(KERN_INFO DRIVER_LOAD_MSG "%s", ib_ipath_version);
-
-	/*
-	 * These must be called before the driver is registered with
-	 * the PCI subsystem.
-	 */
-	idr_init(&unit_table);
-
-	ret = pci_register_driver(&ipath_driver);
-	if (ret < 0) {
-		printk(KERN_ERR IPATH_DRV_NAME
-		       ": Unable to register driver: error %d\n", -ret);
-		goto bail_unit;
-	}
-
-	ret = ipath_init_ipathfs();
-	if (ret < 0) {
-		printk(KERN_ERR IPATH_DRV_NAME ": Unable to create "
-		       "ipathfs: error %d\n", -ret);
-		goto bail_pci;
-	}
-
-	goto bail;
-
-bail_pci:
-	pci_unregister_driver(&ipath_driver);
-
-bail_unit:
-	idr_destroy(&unit_table);
-
-bail:
-	return ret;
-}
-
-static void __exit infinipath_cleanup(void)
-{
-	ipath_exit_ipathfs();
-
-	ipath_cdbg(VERBOSE, "Unregistering pci driver\n");
-	pci_unregister_driver(&ipath_driver);
-
-	idr_destroy(&unit_table);
-}
-
-/**
- * ipath_reset_device - reset the chip if possible
- * @unit: the device to reset
- *
- * Whether or not reset is successful, we attempt to re-initialize the chip
- * (that is, much like a driver unload/reload).  We clear the INITTED flag
- * so that the various entry points will fail until we reinitialize.  For
- * now, we only allow this if no user ports are open that use chip resources
- */
-int ipath_reset_device(int unit)
-{
-	int ret, i;
-	struct ipath_devdata *dd = ipath_lookup(unit);
-	unsigned long flags;
-
-	if (!dd) {
-		ret = -ENODEV;
-		goto bail;
-	}
-
-	if (atomic_read(&dd->ipath_led_override_timer_active)) {
-		/* Need to stop LED timer, _then_ shut off LEDs */
-		del_timer_sync(&dd->ipath_led_override_timer);
-		atomic_set(&dd->ipath_led_override_timer_active, 0);
-	}
-
-	/* Shut off LEDs after we are sure timer is not running */
-	dd->ipath_led_override = LED_OVER_BOTH_OFF;
-	dd->ipath_f_setextled(dd, 0, 0);
-
-	dev_info(&dd->pcidev->dev, "Reset on unit %u requested\n", unit);
-
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT)) {
-		dev_info(&dd->pcidev->dev, "Invalid unit number %u or "
-			 "not initialized or not present\n", unit);
-		ret = -ENXIO;
-		goto bail;
-	}
-
-	spin_lock_irqsave(&dd->ipath_uctxt_lock, flags);
-	if (dd->ipath_pd)
-		for (i = 1; i < dd->ipath_cfgports; i++) {
-			if (!dd->ipath_pd[i] || !dd->ipath_pd[i]->port_cnt)
-				continue;
-			spin_unlock_irqrestore(&dd->ipath_uctxt_lock, flags);
-			ipath_dbg("unit %u port %d is in use "
-				  "(PID %u cmd %s), can't reset\n",
-				  unit, i,
-				  pid_nr(dd->ipath_pd[i]->port_pid),
-				  dd->ipath_pd[i]->port_comm);
-			ret = -EBUSY;
-			goto bail;
-		}
-	spin_unlock_irqrestore(&dd->ipath_uctxt_lock, flags);
-
-	if (dd->ipath_flags & IPATH_HAS_SEND_DMA)
-		teardown_sdma(dd);
-
-	dd->ipath_flags &= ~IPATH_INITTED;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask, 0ULL);
-	ret = dd->ipath_f_reset(dd);
-	if (ret == 1) {
-		ipath_dbg("Reinitializing unit %u after reset attempt\n",
-			  unit);
-		ret = ipath_init_chip(dd, 1);
-	} else
-		ret = -EAGAIN;
-	if (ret)
-		ipath_dev_err(dd, "Reinitialize unit %u after "
-			      "reset failed with %d\n", unit, ret);
-	else
-		dev_info(&dd->pcidev->dev, "Reinitialized unit %u after "
-			 "resetting\n", unit);
-
-bail:
-	return ret;
-}
-
-/*
- * send a signal to all the processes that have the driver open
- * through the normal interfaces (i.e., everything other than diags
- * interface).  Returns number of signalled processes.
- */
-static int ipath_signal_procs(struct ipath_devdata *dd, int sig)
-{
-	int i, sub, any = 0;
-	struct pid *pid;
-	unsigned long flags;
-
-	if (!dd->ipath_pd)
-		return 0;
-
-	spin_lock_irqsave(&dd->ipath_uctxt_lock, flags);
-	for (i = 1; i < dd->ipath_cfgports; i++) {
-		if (!dd->ipath_pd[i] || !dd->ipath_pd[i]->port_cnt)
-			continue;
-		pid = dd->ipath_pd[i]->port_pid;
-		if (!pid)
-			continue;
-
-		dev_info(&dd->pcidev->dev, "context %d in use "
-			  "(PID %u), sending signal %d\n",
-			  i, pid_nr(pid), sig);
-		kill_pid(pid, sig, 1);
-		any++;
-		for (sub = 0; sub < INFINIPATH_MAX_SUBPORT; sub++) {
-			pid = dd->ipath_pd[i]->port_subpid[sub];
-			if (!pid)
-				continue;
-			dev_info(&dd->pcidev->dev, "sub-context "
-				"%d:%d in use (PID %u), sending "
-				"signal %d\n", i, sub, pid_nr(pid), sig);
-			kill_pid(pid, sig, 1);
-			any++;
-		}
-	}
-	spin_unlock_irqrestore(&dd->ipath_uctxt_lock, flags);
-	return any;
-}
-
-static void ipath_hol_signal_down(struct ipath_devdata *dd)
-{
-	if (ipath_signal_procs(dd, SIGSTOP))
-		ipath_dbg("Stopped some processes\n");
-	ipath_cancel_sends(dd, 1);
-}
-
-
-static void ipath_hol_signal_up(struct ipath_devdata *dd)
-{
-	if (ipath_signal_procs(dd, SIGCONT))
-		ipath_dbg("Continued some processes\n");
-}
-
-/*
- * link is down, stop any users processes, and flush pending sends
- * to prevent HoL blocking, then start the HoL timer that
- * periodically continues, then stop procs, so they can detect
- * link down if they want, and do something about it.
- * Timer may already be running, so use mod_timer, not add_timer.
- */
-void ipath_hol_down(struct ipath_devdata *dd)
-{
-	dd->ipath_hol_state = IPATH_HOL_DOWN;
-	ipath_hol_signal_down(dd);
-	dd->ipath_hol_next = IPATH_HOL_DOWNCONT;
-	dd->ipath_hol_timer.expires = jiffies +
-		msecs_to_jiffies(ipath_hol_timeout_ms);
-	mod_timer(&dd->ipath_hol_timer, dd->ipath_hol_timer.expires);
-}
-
-/*
- * link is up, continue any user processes, and ensure timer
- * is a nop, if running.  Let timer keep running, if set; it
- * will nop when it sees the link is up
- */
-void ipath_hol_up(struct ipath_devdata *dd)
-{
-	ipath_hol_signal_up(dd);
-	dd->ipath_hol_state = IPATH_HOL_UP;
-}
-
-/*
- * toggle the running/not running state of user proceses
- * to prevent HoL blocking on chip resources, but still allow
- * user processes to do link down special case handling.
- * Should only be called via the timer
- */
-void ipath_hol_event(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *)opaque;
-
-	if (dd->ipath_hol_next == IPATH_HOL_DOWNSTOP
-		&& dd->ipath_hol_state != IPATH_HOL_UP) {
-		dd->ipath_hol_next = IPATH_HOL_DOWNCONT;
-		ipath_dbg("Stopping processes\n");
-		ipath_hol_signal_down(dd);
-	} else { /* may do "extra" if also in ipath_hol_up() */
-		dd->ipath_hol_next = IPATH_HOL_DOWNSTOP;
-		ipath_dbg("Continuing processes\n");
-		ipath_hol_signal_up(dd);
-	}
-	if (dd->ipath_hol_state == IPATH_HOL_UP)
-		ipath_dbg("link's up, don't resched timer\n");
-	else {
-		dd->ipath_hol_timer.expires = jiffies +
-			msecs_to_jiffies(ipath_hol_timeout_ms);
-		mod_timer(&dd->ipath_hol_timer,
-			dd->ipath_hol_timer.expires);
-	}
-}
-
-int ipath_set_rx_pol_inv(struct ipath_devdata *dd, u8 new_pol_inv)
-{
-	u64 val;
-
-	if (new_pol_inv > INFINIPATH_XGXS_RX_POL_MASK)
-		return -1;
-	if (dd->ipath_rx_pol_inv != new_pol_inv) {
-		dd->ipath_rx_pol_inv = new_pol_inv;
-		val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_xgxsconfig);
-		val &= ~(INFINIPATH_XGXS_RX_POL_MASK <<
-			 INFINIPATH_XGXS_RX_POL_SHIFT);
-		val |= ((u64)dd->ipath_rx_pol_inv) <<
-			INFINIPATH_XGXS_RX_POL_SHIFT;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_xgxsconfig, val);
-	}
-	return 0;
-}
-
-/*
- * Disable and enable the armlaunch error.  Used for PIO bandwidth testing on
- * the 7220, which is count-based, rather than trigger-based.  Safe for the
- * driver check, since it's at init.   Not completely safe when used for
- * user-mode checking, since some error checking can be lost, but not
- * particularly risky, and only has problematic side-effects in the face of
- * very buggy user code.  There is no reference counting, but that's also
- * fine, given the intended use.
- */
-void ipath_enable_armlaunch(struct ipath_devdata *dd)
-{
-	dd->ipath_lasterror &= ~INFINIPATH_E_SPIOARMLAUNCH;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear,
-		INFINIPATH_E_SPIOARMLAUNCH);
-	dd->ipath_errormask |= INFINIPATH_E_SPIOARMLAUNCH;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-		dd->ipath_errormask);
-}
-
-void ipath_disable_armlaunch(struct ipath_devdata *dd)
-{
-	/* so don't re-enable if already set */
-	dd->ipath_maskederrs &= ~INFINIPATH_E_SPIOARMLAUNCH;
-	dd->ipath_errormask &= ~INFINIPATH_E_SPIOARMLAUNCH;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-		dd->ipath_errormask);
-}
-
-module_init(infinipath_init);
-module_exit(infinipath_cleanup);
diff --git a/drivers/staging/rdma/ipath/ipath_eeprom.c b/drivers/staging/rdma/ipath/ipath_eeprom.c
deleted file mode 100644
index ef84107c7ce0..000000000000
--- a/drivers/staging/rdma/ipath/ipath_eeprom.c
+++ /dev/null
@@ -1,1183 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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/delay.h>
-#include <linux/pci.h>
-#include <linux/vmalloc.h>
-
-#include "ipath_kernel.h"
-
-/*
- * InfiniPath I2C driver for a serial eeprom.  This is not a generic
- * I2C interface.  For a start, the device we're using (Atmel AT24C11)
- * doesn't work like a regular I2C device.  It looks like one
- * electrically, but not logically.  Normal I2C devices have a single
- * 7-bit or 10-bit I2C address that they respond to.  Valid 7-bit
- * addresses range from 0x03 to 0x77.  Addresses 0x00 to 0x02 and 0x78
- * to 0x7F are special reserved addresses (e.g. 0x00 is the "general
- * call" address.)  The Atmel device, on the other hand, responds to ALL
- * 7-bit addresses.  It's designed to be the only device on a given I2C
- * bus.  A 7-bit address corresponds to the memory address within the
- * Atmel device itself.
- *
- * Also, the timing requirements mean more than simple software
- * bitbanging, with readbacks from chip to ensure timing (simple udelay
- * is not enough).
- *
- * This all means that accessing the device is specialized enough
- * that using the standard kernel I2C bitbanging interface would be
- * impossible.  For example, the core I2C eeprom driver expects to find
- * a device at one or more of a limited set of addresses only.  It doesn't
- * allow writing to an eeprom.  It also doesn't provide any means of
- * accessing eeprom contents from within the kernel, only via sysfs.
- */
-
-/* Added functionality for IBA7220-based cards */
-#define IPATH_EEPROM_DEV_V1 0xA0
-#define IPATH_EEPROM_DEV_V2 0xA2
-#define IPATH_TEMP_DEV 0x98
-#define IPATH_BAD_DEV (IPATH_EEPROM_DEV_V2+2)
-#define IPATH_NO_DEV (0xFF)
-
-/*
- * The number of I2C chains is proliferating. Table below brings
- * some order to the madness. The basic principle is that the
- * table is scanned from the top, and a "probe" is made to the
- * device probe_dev. If that succeeds, the chain is considered
- * to be of that type, and dd->i2c_chain_type is set to the index+1
- * of the entry.
- * The +1 is so static initialization can mean "unknown, do probe."
- */
-static struct i2c_chain_desc {
-	u8 probe_dev;	/* If seen at probe, chain is this type */
-	u8 eeprom_dev;	/* Dev addr (if any) for EEPROM */
-	u8 temp_dev;	/* Dev Addr (if any) for Temp-sense */
-} i2c_chains[] = {
-	{ IPATH_BAD_DEV, IPATH_NO_DEV, IPATH_NO_DEV }, /* pre-iba7220 bds */
-	{ IPATH_EEPROM_DEV_V1, IPATH_EEPROM_DEV_V1, IPATH_TEMP_DEV}, /* V1 */
-	{ IPATH_EEPROM_DEV_V2, IPATH_EEPROM_DEV_V2, IPATH_TEMP_DEV}, /* V2 */
-	{ IPATH_NO_DEV }
-};
-
-enum i2c_type {
-	i2c_line_scl = 0,
-	i2c_line_sda
-};
-
-enum i2c_state {
-	i2c_line_low = 0,
-	i2c_line_high
-};
-
-#define READ_CMD 1
-#define WRITE_CMD 0
-
-/**
- * i2c_gpio_set - set a GPIO line
- * @dd: the infinipath device
- * @line: the line to set
- * @new_line_state: the state to set
- *
- * Returns 0 if the line was set to the new state successfully, non-zero
- * on error.
- */
-static int i2c_gpio_set(struct ipath_devdata *dd,
-			enum i2c_type line,
-			enum i2c_state new_line_state)
-{
-	u64 out_mask, dir_mask, *gpioval;
-	unsigned long flags = 0;
-
-	gpioval = &dd->ipath_gpio_out;
-
-	if (line == i2c_line_scl) {
-		dir_mask = dd->ipath_gpio_scl;
-		out_mask = (1UL << dd->ipath_gpio_scl_num);
-	} else {
-		dir_mask = dd->ipath_gpio_sda;
-		out_mask = (1UL << dd->ipath_gpio_sda_num);
-	}
-
-	spin_lock_irqsave(&dd->ipath_gpio_lock, flags);
-	if (new_line_state == i2c_line_high) {
-		/* tri-state the output rather than force high */
-		dd->ipath_extctrl &= ~dir_mask;
-	} else {
-		/* config line to be an output */
-		dd->ipath_extctrl |= dir_mask;
-	}
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_extctrl, dd->ipath_extctrl);
-
-	/* set output as well (no real verify) */
-	if (new_line_state == i2c_line_high)
-		*gpioval |= out_mask;
-	else
-		*gpioval &= ~out_mask;
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_gpio_out, *gpioval);
-	spin_unlock_irqrestore(&dd->ipath_gpio_lock, flags);
-
-	return 0;
-}
-
-/**
- * i2c_gpio_get - get a GPIO line state
- * @dd: the infinipath device
- * @line: the line to get
- * @curr_statep: where to put the line state
- *
- * Returns 0 if the line was set to the new state successfully, non-zero
- * on error.  curr_state is not set on error.
- */
-static int i2c_gpio_get(struct ipath_devdata *dd,
-			enum i2c_type line,
-			enum i2c_state *curr_statep)
-{
-	u64 read_val, mask;
-	int ret;
-	unsigned long flags = 0;
-
-	/* check args */
-	if (curr_statep == NULL) {
-		ret = 1;
-		goto bail;
-	}
-
-	/* config line to be an input */
-	if (line == i2c_line_scl)
-		mask = dd->ipath_gpio_scl;
-	else
-		mask = dd->ipath_gpio_sda;
-
-	spin_lock_irqsave(&dd->ipath_gpio_lock, flags);
-	dd->ipath_extctrl &= ~mask;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_extctrl, dd->ipath_extctrl);
-	/*
-	 * Below is very unlikely to reflect true input state if Output
-	 * Enable actually changed.
-	 */
-	read_val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_extstatus);
-	spin_unlock_irqrestore(&dd->ipath_gpio_lock, flags);
-
-	if (read_val & mask)
-		*curr_statep = i2c_line_high;
-	else
-		*curr_statep = i2c_line_low;
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * i2c_wait_for_writes - wait for a write
- * @dd: the infinipath device
- *
- * We use this instead of udelay directly, so we can make sure
- * that previous register writes have been flushed all the way
- * to the chip.  Since we are delaying anyway, the cost doesn't
- * hurt, and makes the bit twiddling more regular
- */
-static void i2c_wait_for_writes(struct ipath_devdata *dd)
-{
-	(void)ipath_read_kreg32(dd, dd->ipath_kregs->kr_scratch);
-	rmb();
-}
-
-static void scl_out(struct ipath_devdata *dd, u8 bit)
-{
-	udelay(1);
-	i2c_gpio_set(dd, i2c_line_scl, bit ? i2c_line_high : i2c_line_low);
-
-	i2c_wait_for_writes(dd);
-}
-
-static void sda_out(struct ipath_devdata *dd, u8 bit)
-{
-	i2c_gpio_set(dd, i2c_line_sda, bit ? i2c_line_high : i2c_line_low);
-
-	i2c_wait_for_writes(dd);
-}
-
-static u8 sda_in(struct ipath_devdata *dd, int wait)
-{
-	enum i2c_state bit;
-
-	if (i2c_gpio_get(dd, i2c_line_sda, &bit))
-		ipath_dbg("get bit failed!\n");
-
-	if (wait)
-		i2c_wait_for_writes(dd);
-
-	return bit == i2c_line_high ? 1U : 0;
-}
-
-/**
- * i2c_ackrcv - see if ack following write is true
- * @dd: the infinipath device
- */
-static int i2c_ackrcv(struct ipath_devdata *dd)
-{
-	u8 ack_received;
-
-	/* AT ENTRY SCL = LOW */
-	/* change direction, ignore data */
-	ack_received = sda_in(dd, 1);
-	scl_out(dd, i2c_line_high);
-	ack_received = sda_in(dd, 1) == 0;
-	scl_out(dd, i2c_line_low);
-	return ack_received;
-}
-
-/**
- * rd_byte - read a byte, leaving ACK, STOP, etc up to caller
- * @dd: the infinipath device
- *
- * Returns byte shifted out of device
- */
-static int rd_byte(struct ipath_devdata *dd)
-{
-	int bit_cntr, data;
-
-	data = 0;
-
-	for (bit_cntr = 7; bit_cntr >= 0; --bit_cntr) {
-		data <<= 1;
-		scl_out(dd, i2c_line_high);
-		data |= sda_in(dd, 0);
-		scl_out(dd, i2c_line_low);
-	}
-	return data;
-}
-
-/**
- * wr_byte - write a byte, one bit at a time
- * @dd: the infinipath device
- * @data: the byte to write
- *
- * Returns 0 if we got the following ack, otherwise 1
- */
-static int wr_byte(struct ipath_devdata *dd, u8 data)
-{
-	int bit_cntr;
-	u8 bit;
-
-	for (bit_cntr = 7; bit_cntr >= 0; bit_cntr--) {
-		bit = (data >> bit_cntr) & 1;
-		sda_out(dd, bit);
-		scl_out(dd, i2c_line_high);
-		scl_out(dd, i2c_line_low);
-	}
-	return (!i2c_ackrcv(dd)) ? 1 : 0;
-}
-
-static void send_ack(struct ipath_devdata *dd)
-{
-	sda_out(dd, i2c_line_low);
-	scl_out(dd, i2c_line_high);
-	scl_out(dd, i2c_line_low);
-	sda_out(dd, i2c_line_high);
-}
-
-/**
- * i2c_startcmd - transmit the start condition, followed by address/cmd
- * @dd: the infinipath device
- * @offset_dir: direction byte
- *
- *      (both clock/data high, clock high, data low while clock is high)
- */
-static int i2c_startcmd(struct ipath_devdata *dd, u8 offset_dir)
-{
-	int res;
-
-	/* issue start sequence */
-	sda_out(dd, i2c_line_high);
-	scl_out(dd, i2c_line_high);
-	sda_out(dd, i2c_line_low);
-	scl_out(dd, i2c_line_low);
-
-	/* issue length and direction byte */
-	res = wr_byte(dd, offset_dir);
-
-	if (res)
-		ipath_cdbg(VERBOSE, "No ack to complete start\n");
-
-	return res;
-}
-
-/**
- * stop_cmd - transmit the stop condition
- * @dd: the infinipath device
- *
- * (both clock/data low, clock high, data high while clock is high)
- */
-static void stop_cmd(struct ipath_devdata *dd)
-{
-	scl_out(dd, i2c_line_low);
-	sda_out(dd, i2c_line_low);
-	scl_out(dd, i2c_line_high);
-	sda_out(dd, i2c_line_high);
-	udelay(2);
-}
-
-/**
- * eeprom_reset - reset I2C communication
- * @dd: the infinipath device
- */
-
-static int eeprom_reset(struct ipath_devdata *dd)
-{
-	int clock_cycles_left = 9;
-	u64 *gpioval = &dd->ipath_gpio_out;
-	int ret;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dd->ipath_gpio_lock, flags);
-	/* Make sure shadows are consistent */
-	dd->ipath_extctrl = ipath_read_kreg64(dd, dd->ipath_kregs->kr_extctrl);
-	*gpioval = ipath_read_kreg64(dd, dd->ipath_kregs->kr_gpio_out);
-	spin_unlock_irqrestore(&dd->ipath_gpio_lock, flags);
-
-	ipath_cdbg(VERBOSE, "Resetting i2c eeprom; initial gpioout reg "
-		   "is %llx\n", (unsigned long long) *gpioval);
-
-	/*
-	 * This is to get the i2c into a known state, by first going low,
-	 * then tristate sda (and then tristate scl as first thing
-	 * in loop)
-	 */
-	scl_out(dd, i2c_line_low);
-	sda_out(dd, i2c_line_high);
-
-	/* Clock up to 9 cycles looking for SDA hi, then issue START and STOP */
-	while (clock_cycles_left--) {
-		scl_out(dd, i2c_line_high);
-
-		/* SDA seen high, issue START by dropping it while SCL high */
-		if (sda_in(dd, 0)) {
-			sda_out(dd, i2c_line_low);
-			scl_out(dd, i2c_line_low);
-			/* ATMEL spec says must be followed by STOP. */
-			scl_out(dd, i2c_line_high);
-			sda_out(dd, i2c_line_high);
-			ret = 0;
-			goto bail;
-		}
-
-		scl_out(dd, i2c_line_low);
-	}
-
-	ret = 1;
-
-bail:
-	return ret;
-}
-
-/*
- * Probe for I2C device at specified address. Returns 0 for "success"
- * to match rest of this file.
- * Leave bus in "reasonable" state for further commands.
- */
-static int i2c_probe(struct ipath_devdata *dd, int devaddr)
-{
-	int ret;
-
-	ret = eeprom_reset(dd);
-	if (ret) {
-		ipath_dev_err(dd, "Failed reset probing device 0x%02X\n",
-			      devaddr);
-		return ret;
-	}
-	/*
-	 * Reset no longer leaves bus in start condition, so normal
-	 * i2c_startcmd() will do.
-	 */
-	ret = i2c_startcmd(dd, devaddr | READ_CMD);
-	if (ret)
-		ipath_cdbg(VERBOSE, "Failed startcmd for device 0x%02X\n",
-			   devaddr);
-	else {
-		/*
-		 * Device did respond. Complete a single-byte read, because some
-		 * devices apparently cannot handle STOP immediately after they
-		 * ACK the start-cmd.
-		 */
-		int data;
-		data = rd_byte(dd);
-		stop_cmd(dd);
-		ipath_cdbg(VERBOSE, "Response from device 0x%02X\n", devaddr);
-	}
-	return ret;
-}
-
-/*
- * Returns the "i2c type". This is a pointer to a struct that describes
- * the I2C chain on this board. To minimize impact on struct ipath_devdata,
- * the (small integer) index into the table is actually memoized, rather
- * then the pointer.
- * Memoization is because the type is determined on the first call per chip.
- * An alternative would be to move type determination to early
- * init code.
- */
-static struct i2c_chain_desc *ipath_i2c_type(struct ipath_devdata *dd)
-{
-	int idx;
-
-	/* Get memoized index, from previous successful probes */
-	idx = dd->ipath_i2c_chain_type - 1;
-	if (idx >= 0 && idx < (ARRAY_SIZE(i2c_chains) - 1))
-		goto done;
-
-	idx = 0;
-	while (i2c_chains[idx].probe_dev != IPATH_NO_DEV) {
-		/* if probe succeeds, this is type */
-		if (!i2c_probe(dd, i2c_chains[idx].probe_dev))
-			break;
-		++idx;
-	}
-
-	/*
-	 * Old EEPROM (first entry) may require a reset after probe,
-	 * rather than being able to "start" after "stop"
-	 */
-	if (idx == 0)
-		eeprom_reset(dd);
-
-	if (i2c_chains[idx].probe_dev == IPATH_NO_DEV)
-		idx = -1;
-	else
-		dd->ipath_i2c_chain_type = idx + 1;
-done:
-	return (idx >= 0) ? i2c_chains + idx : NULL;
-}
-
-static int ipath_eeprom_internal_read(struct ipath_devdata *dd,
-					u8 eeprom_offset, void *buffer, int len)
-{
-	int ret;
-	struct i2c_chain_desc *icd;
-	u8 *bp = buffer;
-
-	ret = 1;
-	icd = ipath_i2c_type(dd);
-	if (!icd)
-		goto bail;
-
-	if (icd->eeprom_dev == IPATH_NO_DEV) {
-		/* legacy not-really-I2C */
-		ipath_cdbg(VERBOSE, "Start command only address\n");
-		eeprom_offset = (eeprom_offset << 1) | READ_CMD;
-		ret = i2c_startcmd(dd, eeprom_offset);
-	} else {
-		/* Actual I2C */
-		ipath_cdbg(VERBOSE, "Start command uses devaddr\n");
-		if (i2c_startcmd(dd, icd->eeprom_dev | WRITE_CMD)) {
-			ipath_dbg("Failed EEPROM startcmd\n");
-			stop_cmd(dd);
-			ret = 1;
-			goto bail;
-		}
-		ret = wr_byte(dd, eeprom_offset);
-		stop_cmd(dd);
-		if (ret) {
-			ipath_dev_err(dd, "Failed to write EEPROM address\n");
-			ret = 1;
-			goto bail;
-		}
-		ret = i2c_startcmd(dd, icd->eeprom_dev | READ_CMD);
-	}
-	if (ret) {
-		ipath_dbg("Failed startcmd for dev %02X\n", icd->eeprom_dev);
-		stop_cmd(dd);
-		ret = 1;
-		goto bail;
-	}
-
-	/*
-	 * eeprom keeps clocking data out as long as we ack, automatically
-	 * incrementing the address.
-	 */
-	while (len-- > 0) {
-		/* get and store data */
-		*bp++ = rd_byte(dd);
-		/* send ack if not the last byte */
-		if (len)
-			send_ack(dd);
-	}
-
-	stop_cmd(dd);
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-static int ipath_eeprom_internal_write(struct ipath_devdata *dd, u8 eeprom_offset,
-				       const void *buffer, int len)
-{
-	int sub_len;
-	const u8 *bp = buffer;
-	int max_wait_time, i;
-	int ret;
-	struct i2c_chain_desc *icd;
-
-	ret = 1;
-	icd = ipath_i2c_type(dd);
-	if (!icd)
-		goto bail;
-
-	while (len > 0) {
-		if (icd->eeprom_dev == IPATH_NO_DEV) {
-			if (i2c_startcmd(dd,
-					 (eeprom_offset << 1) | WRITE_CMD)) {
-				ipath_dbg("Failed to start cmd offset %u\n",
-					eeprom_offset);
-				goto failed_write;
-			}
-		} else {
-			/* Real I2C */
-			if (i2c_startcmd(dd, icd->eeprom_dev | WRITE_CMD)) {
-				ipath_dbg("Failed EEPROM startcmd\n");
-				goto failed_write;
-			}
-			ret = wr_byte(dd, eeprom_offset);
-			if (ret) {
-				ipath_dev_err(dd, "Failed to write EEPROM "
-					      "address\n");
-				goto failed_write;
-			}
-		}
-
-		sub_len = min(len, 4);
-		eeprom_offset += sub_len;
-		len -= sub_len;
-
-		for (i = 0; i < sub_len; i++) {
-			if (wr_byte(dd, *bp++)) {
-				ipath_dbg("no ack after byte %u/%u (%u "
-					  "total remain)\n", i, sub_len,
-					  len + sub_len - i);
-				goto failed_write;
-			}
-		}
-
-		stop_cmd(dd);
-
-		/*
-		 * wait for write complete by waiting for a successful
-		 * read (the chip replies with a zero after the write
-		 * cmd completes, and before it writes to the eeprom.
-		 * The startcmd for the read will fail the ack until
-		 * the writes have completed.   We do this inline to avoid
-		 * the debug prints that are in the real read routine
-		 * if the startcmd fails.
-		 * We also use the proper device address, so it doesn't matter
-		 * whether we have real eeprom_dev. legacy likes any address.
-		 */
-		max_wait_time = 100;
-		while (i2c_startcmd(dd, icd->eeprom_dev | READ_CMD)) {
-			stop_cmd(dd);
-			if (!--max_wait_time) {
-				ipath_dbg("Did not get successful read to "
-					  "complete write\n");
-				goto failed_write;
-			}
-		}
-		/* now read (and ignore) the resulting byte */
-		rd_byte(dd);
-		stop_cmd(dd);
-	}
-
-	ret = 0;
-	goto bail;
-
-failed_write:
-	stop_cmd(dd);
-	ret = 1;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_eeprom_read - receives bytes from the eeprom via I2C
- * @dd: the infinipath device
- * @eeprom_offset: address to read from
- * @buffer: where to store result
- * @len: number of bytes to receive
- */
-int ipath_eeprom_read(struct ipath_devdata *dd, u8 eeprom_offset,
-			void *buff, int len)
-{
-	int ret;
-
-	ret = mutex_lock_interruptible(&dd->ipath_eep_lock);
-	if (!ret) {
-		ret = ipath_eeprom_internal_read(dd, eeprom_offset, buff, len);
-		mutex_unlock(&dd->ipath_eep_lock);
-	}
-
-	return ret;
-}
-
-/**
- * ipath_eeprom_write - writes data to the eeprom via I2C
- * @dd: the infinipath device
- * @eeprom_offset: where to place data
- * @buffer: data to write
- * @len: number of bytes to write
- */
-int ipath_eeprom_write(struct ipath_devdata *dd, u8 eeprom_offset,
-			const void *buff, int len)
-{
-	int ret;
-
-	ret = mutex_lock_interruptible(&dd->ipath_eep_lock);
-	if (!ret) {
-		ret = ipath_eeprom_internal_write(dd, eeprom_offset, buff, len);
-		mutex_unlock(&dd->ipath_eep_lock);
-	}
-
-	return ret;
-}
-
-static u8 flash_csum(struct ipath_flash *ifp, int adjust)
-{
-	u8 *ip = (u8 *) ifp;
-	u8 csum = 0, len;
-
-	/*
-	 * Limit length checksummed to max length of actual data.
-	 * Checksum of erased eeprom will still be bad, but we avoid
-	 * reading past the end of the buffer we were passed.
-	 */
-	len = ifp->if_length;
-	if (len > sizeof(struct ipath_flash))
-		len = sizeof(struct ipath_flash);
-	while (len--)
-		csum += *ip++;
-	csum -= ifp->if_csum;
-	csum = ~csum;
-	if (adjust)
-		ifp->if_csum = csum;
-
-	return csum;
-}
-
-/**
- * ipath_get_guid - get the GUID from the i2c device
- * @dd: the infinipath device
- *
- * We have the capability to use the ipath_nguid field, and get
- * the guid from the first chip's flash, to use for all of them.
- */
-void ipath_get_eeprom_info(struct ipath_devdata *dd)
-{
-	void *buf;
-	struct ipath_flash *ifp;
-	__be64 guid;
-	int len, eep_stat;
-	u8 csum, *bguid;
-	int t = dd->ipath_unit;
-	struct ipath_devdata *dd0 = ipath_lookup(0);
-
-	if (t && dd0->ipath_nguid > 1 && t <= dd0->ipath_nguid) {
-		u8 oguid;
-		dd->ipath_guid = dd0->ipath_guid;
-		bguid = (u8 *) & dd->ipath_guid;
-
-		oguid = bguid[7];
-		bguid[7] += t;
-		if (oguid > bguid[7]) {
-			if (bguid[6] == 0xff) {
-				if (bguid[5] == 0xff) {
-					ipath_dev_err(
-						dd,
-						"Can't set %s GUID from "
-						"base, wraps to OUI!\n",
-						ipath_get_unit_name(t));
-					dd->ipath_guid = 0;
-					goto bail;
-				}
-				bguid[5]++;
-			}
-			bguid[6]++;
-		}
-		dd->ipath_nguid = 1;
-
-		ipath_dbg("nguid %u, so adding %u to device 0 guid, "
-			  "for %llx\n",
-			  dd0->ipath_nguid, t,
-			  (unsigned long long) be64_to_cpu(dd->ipath_guid));
-		goto bail;
-	}
-
-	/*
-	 * read full flash, not just currently used part, since it may have
-	 * been written with a newer definition
-	 * */
-	len = sizeof(struct ipath_flash);
-	buf = vmalloc(len);
-	if (!buf) {
-		ipath_dev_err(dd, "Couldn't allocate memory to read %u "
-			      "bytes from eeprom for GUID\n", len);
-		goto bail;
-	}
-
-	mutex_lock(&dd->ipath_eep_lock);
-	eep_stat = ipath_eeprom_internal_read(dd, 0, buf, len);
-	mutex_unlock(&dd->ipath_eep_lock);
-
-	if (eep_stat) {
-		ipath_dev_err(dd, "Failed reading GUID from eeprom\n");
-		goto done;
-	}
-	ifp = (struct ipath_flash *)buf;
-
-	csum = flash_csum(ifp, 0);
-	if (csum != ifp->if_csum) {
-		dev_info(&dd->pcidev->dev, "Bad I2C flash checksum: "
-			 "0x%x, not 0x%x\n", csum, ifp->if_csum);
-		goto done;
-	}
-	if (*(__be64 *) ifp->if_guid == cpu_to_be64(0) ||
-	    *(__be64 *) ifp->if_guid == ~cpu_to_be64(0)) {
-		ipath_dev_err(dd, "Invalid GUID %llx from flash; "
-			      "ignoring\n",
-			      *(unsigned long long *) ifp->if_guid);
-		/* don't allow GUID if all 0 or all 1's */
-		goto done;
-	}
-
-	/* complain, but allow it */
-	if (*(u64 *) ifp->if_guid == 0x100007511000000ULL)
-		dev_info(&dd->pcidev->dev, "Warning, GUID %llx is "
-			 "default, probably not correct!\n",
-			 *(unsigned long long *) ifp->if_guid);
-
-	bguid = ifp->if_guid;
-	if (!bguid[0] && !bguid[1] && !bguid[2]) {
-		/* original incorrect GUID format in flash; fix in
-		 * core copy, by shifting up 2 octets; don't need to
-		 * change top octet, since both it and shifted are
-		 * 0.. */
-		bguid[1] = bguid[3];
-		bguid[2] = bguid[4];
-		bguid[3] = bguid[4] = 0;
-		guid = *(__be64 *) ifp->if_guid;
-		ipath_cdbg(VERBOSE, "Old GUID format in flash, top 3 zero, "
-			   "shifting 2 octets\n");
-	} else
-		guid = *(__be64 *) ifp->if_guid;
-	dd->ipath_guid = guid;
-	dd->ipath_nguid = ifp->if_numguid;
-	/*
-	 * Things are slightly complicated by the desire to transparently
-	 * support both the Pathscale 10-digit serial number and the QLogic
-	 * 13-character version.
-	 */
-	if ((ifp->if_fversion > 1) && ifp->if_sprefix[0]
-		&& ((u8 *)ifp->if_sprefix)[0] != 0xFF) {
-		/* This board has a Serial-prefix, which is stored
-		 * elsewhere for backward-compatibility.
-		 */
-		char *snp = dd->ipath_serial;
-		memcpy(snp, ifp->if_sprefix, sizeof ifp->if_sprefix);
-		snp[sizeof ifp->if_sprefix] = '\0';
-		len = strlen(snp);
-		snp += len;
-		len = (sizeof dd->ipath_serial) - len;
-		if (len > sizeof ifp->if_serial) {
-			len = sizeof ifp->if_serial;
-		}
-		memcpy(snp, ifp->if_serial, len);
-	} else
-		memcpy(dd->ipath_serial, ifp->if_serial,
-		       sizeof ifp->if_serial);
-	if (!strstr(ifp->if_comment, "Tested successfully"))
-		ipath_dev_err(dd, "Board SN %s did not pass functional "
-			"test: %s\n", dd->ipath_serial,
-			ifp->if_comment);
-
-	ipath_cdbg(VERBOSE, "Initted GUID to %llx from eeprom\n",
-		   (unsigned long long) be64_to_cpu(dd->ipath_guid));
-
-	memcpy(&dd->ipath_eep_st_errs, &ifp->if_errcntp, IPATH_EEP_LOG_CNT);
-	/*
-	 * Power-on (actually "active") hours are kept as little-endian value
-	 * in EEPROM, but as seconds in a (possibly as small as 24-bit)
-	 * atomic_t while running.
-	 */
-	atomic_set(&dd->ipath_active_time, 0);
-	dd->ipath_eep_hrs = ifp->if_powerhour[0] | (ifp->if_powerhour[1] << 8);
-
-done:
-	vfree(buf);
-
-bail:;
-}
-
-/**
- * ipath_update_eeprom_log - copy active-time and error counters to eeprom
- * @dd: the infinipath device
- *
- * Although the time is kept as seconds in the ipath_devdata struct, it is
- * rounded to hours for re-write, as we have only 16 bits in EEPROM.
- * First-cut code reads whole (expected) struct ipath_flash, modifies,
- * re-writes. Future direction: read/write only what we need, assuming
- * that the EEPROM had to have been "good enough" for driver init, and
- * if not, we aren't making it worse.
- *
- */
-
-int ipath_update_eeprom_log(struct ipath_devdata *dd)
-{
-	void *buf;
-	struct ipath_flash *ifp;
-	int len, hi_water;
-	uint32_t new_time, new_hrs;
-	u8 csum;
-	int ret, idx;
-	unsigned long flags;
-
-	/* first, check if we actually need to do anything. */
-	ret = 0;
-	for (idx = 0; idx < IPATH_EEP_LOG_CNT; ++idx) {
-		if (dd->ipath_eep_st_new_errs[idx]) {
-			ret = 1;
-			break;
-		}
-	}
-	new_time = atomic_read(&dd->ipath_active_time);
-
-	if (ret == 0 && new_time < 3600)
-		return 0;
-
-	/*
-	 * The quick-check above determined that there is something worthy
-	 * of logging, so get current contents and do a more detailed idea.
-	 * read full flash, not just currently used part, since it may have
-	 * been written with a newer definition
-	 */
-	len = sizeof(struct ipath_flash);
-	buf = vmalloc(len);
-	ret = 1;
-	if (!buf) {
-		ipath_dev_err(dd, "Couldn't allocate memory to read %u "
-				"bytes from eeprom for logging\n", len);
-		goto bail;
-	}
-
-	/* Grab semaphore and read current EEPROM. If we get an
-	 * error, let go, but if not, keep it until we finish write.
-	 */
-	ret = mutex_lock_interruptible(&dd->ipath_eep_lock);
-	if (ret) {
-		ipath_dev_err(dd, "Unable to acquire EEPROM for logging\n");
-		goto free_bail;
-	}
-	ret = ipath_eeprom_internal_read(dd, 0, buf, len);
-	if (ret) {
-		mutex_unlock(&dd->ipath_eep_lock);
-		ipath_dev_err(dd, "Unable read EEPROM for logging\n");
-		goto free_bail;
-	}
-	ifp = (struct ipath_flash *)buf;
-
-	csum = flash_csum(ifp, 0);
-	if (csum != ifp->if_csum) {
-		mutex_unlock(&dd->ipath_eep_lock);
-		ipath_dev_err(dd, "EEPROM cks err (0x%02X, S/B 0x%02X)\n",
-				csum, ifp->if_csum);
-		ret = 1;
-		goto free_bail;
-	}
-	hi_water = 0;
-	spin_lock_irqsave(&dd->ipath_eep_st_lock, flags);
-	for (idx = 0; idx < IPATH_EEP_LOG_CNT; ++idx) {
-		int new_val = dd->ipath_eep_st_new_errs[idx];
-		if (new_val) {
-			/*
-			 * If we have seen any errors, add to EEPROM values
-			 * We need to saturate at 0xFF (255) and we also
-			 * would need to adjust the checksum if we were
-			 * trying to minimize EEPROM traffic
-			 * Note that we add to actual current count in EEPROM,
-			 * in case it was altered while we were running.
-			 */
-			new_val += ifp->if_errcntp[idx];
-			if (new_val > 0xFF)
-				new_val = 0xFF;
-			if (ifp->if_errcntp[idx] != new_val) {
-				ifp->if_errcntp[idx] = new_val;
-				hi_water = offsetof(struct ipath_flash,
-						if_errcntp) + idx;
-			}
-			/*
-			 * update our shadow (used to minimize EEPROM
-			 * traffic), to match what we are about to write.
-			 */
-			dd->ipath_eep_st_errs[idx] = new_val;
-			dd->ipath_eep_st_new_errs[idx] = 0;
-		}
-	}
-	/*
-	 * now update active-time. We would like to round to the nearest hour
-	 * but unless atomic_t are sure to be proper signed ints we cannot,
-	 * because we need to account for what we "transfer" to EEPROM and
-	 * if we log an hour at 31 minutes, then we would need to set
-	 * active_time to -29 to accurately count the _next_ hour.
-	 */
-	if (new_time >= 3600) {
-		new_hrs = new_time / 3600;
-		atomic_sub((new_hrs * 3600), &dd->ipath_active_time);
-		new_hrs += dd->ipath_eep_hrs;
-		if (new_hrs > 0xFFFF)
-			new_hrs = 0xFFFF;
-		dd->ipath_eep_hrs = new_hrs;
-		if ((new_hrs & 0xFF) != ifp->if_powerhour[0]) {
-			ifp->if_powerhour[0] = new_hrs & 0xFF;
-			hi_water = offsetof(struct ipath_flash, if_powerhour);
-		}
-		if ((new_hrs >> 8) != ifp->if_powerhour[1]) {
-			ifp->if_powerhour[1] = new_hrs >> 8;
-			hi_water = offsetof(struct ipath_flash, if_powerhour)
-					+ 1;
-		}
-	}
-	/*
-	 * There is a tiny possibility that we could somehow fail to write
-	 * the EEPROM after updating our shadows, but problems from holding
-	 * the spinlock too long are a much bigger issue.
-	 */
-	spin_unlock_irqrestore(&dd->ipath_eep_st_lock, flags);
-	if (hi_water) {
-		/* we made some change to the data, uopdate cksum and write */
-		csum = flash_csum(ifp, 1);
-		ret = ipath_eeprom_internal_write(dd, 0, buf, hi_water + 1);
-	}
-	mutex_unlock(&dd->ipath_eep_lock);
-	if (ret)
-		ipath_dev_err(dd, "Failed updating EEPROM\n");
-
-free_bail:
-	vfree(buf);
-bail:
-	return ret;
-
-}
-
-/**
- * ipath_inc_eeprom_err - increment one of the four error counters
- * that are logged to EEPROM.
- * @dd: the infinipath device
- * @eidx: 0..3, the counter to increment
- * @incr: how much to add
- *
- * Each counter is 8-bits, and saturates at 255 (0xFF). They
- * are copied to the EEPROM (aka flash) whenever ipath_update_eeprom_log()
- * is called, but it can only be called in a context that allows sleep.
- * This function can be called even at interrupt level.
- */
-
-void ipath_inc_eeprom_err(struct ipath_devdata *dd, u32 eidx, u32 incr)
-{
-	uint new_val;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dd->ipath_eep_st_lock, flags);
-	new_val = dd->ipath_eep_st_new_errs[eidx] + incr;
-	if (new_val > 255)
-		new_val = 255;
-	dd->ipath_eep_st_new_errs[eidx] = new_val;
-	spin_unlock_irqrestore(&dd->ipath_eep_st_lock, flags);
-	return;
-}
-
-static int ipath_tempsense_internal_read(struct ipath_devdata *dd, u8 regnum)
-{
-	int ret;
-	struct i2c_chain_desc *icd;
-
-	ret = -ENOENT;
-
-	icd = ipath_i2c_type(dd);
-	if (!icd)
-		goto bail;
-
-	if (icd->temp_dev == IPATH_NO_DEV) {
-		/* tempsense only exists on new, real-I2C boards */
-		ret = -ENXIO;
-		goto bail;
-	}
-
-	if (i2c_startcmd(dd, icd->temp_dev | WRITE_CMD)) {
-		ipath_dbg("Failed tempsense startcmd\n");
-		stop_cmd(dd);
-		ret = -ENXIO;
-		goto bail;
-	}
-	ret = wr_byte(dd, regnum);
-	stop_cmd(dd);
-	if (ret) {
-		ipath_dev_err(dd, "Failed tempsense WR command %02X\n",
-			      regnum);
-		ret = -ENXIO;
-		goto bail;
-	}
-	if (i2c_startcmd(dd, icd->temp_dev | READ_CMD)) {
-		ipath_dbg("Failed tempsense RD startcmd\n");
-		stop_cmd(dd);
-		ret = -ENXIO;
-		goto bail;
-	}
-	/*
-	 * We can only clock out one byte per command, sensibly
-	 */
-	ret = rd_byte(dd);
-	stop_cmd(dd);
-
-bail:
-	return ret;
-}
-
-#define VALID_TS_RD_REG_MASK 0xBF
-
-/**
- * ipath_tempsense_read - read register of temp sensor via I2C
- * @dd: the infinipath device
- * @regnum: register to read from
- *
- * returns reg contents (0..255) or < 0 for error
- */
-int ipath_tempsense_read(struct ipath_devdata *dd, u8 regnum)
-{
-	int ret;
-
-	if (regnum > 7)
-		return -EINVAL;
-
-	/* return a bogus value for (the one) register we do not have */
-	if (!((1 << regnum) & VALID_TS_RD_REG_MASK))
-		return 0;
-
-	ret = mutex_lock_interruptible(&dd->ipath_eep_lock);
-	if (!ret) {
-		ret = ipath_tempsense_internal_read(dd, regnum);
-		mutex_unlock(&dd->ipath_eep_lock);
-	}
-
-	/*
-	 * There are three possibilities here:
-	 * ret is actual value (0..255)
-	 * ret is -ENXIO or -EINVAL from code in this file
-	 * ret is -EINTR from mutex_lock_interruptible.
-	 */
-	return ret;
-}
-
-static int ipath_tempsense_internal_write(struct ipath_devdata *dd,
-					  u8 regnum, u8 data)
-{
-	int ret = -ENOENT;
-	struct i2c_chain_desc *icd;
-
-	icd = ipath_i2c_type(dd);
-	if (!icd)
-		goto bail;
-
-	if (icd->temp_dev == IPATH_NO_DEV) {
-		/* tempsense only exists on new, real-I2C boards */
-		ret = -ENXIO;
-		goto bail;
-	}
-	if (i2c_startcmd(dd, icd->temp_dev | WRITE_CMD)) {
-		ipath_dbg("Failed tempsense startcmd\n");
-		stop_cmd(dd);
-		ret = -ENXIO;
-		goto bail;
-	}
-	ret = wr_byte(dd, regnum);
-	if (ret) {
-		stop_cmd(dd);
-		ipath_dev_err(dd, "Failed to write tempsense command %02X\n",
-			      regnum);
-		ret = -ENXIO;
-		goto bail;
-	}
-	ret = wr_byte(dd, data);
-	stop_cmd(dd);
-	ret = i2c_startcmd(dd, icd->temp_dev | READ_CMD);
-	if (ret) {
-		ipath_dev_err(dd, "Failed tempsense data wrt to %02X\n",
-			      regnum);
-		ret = -ENXIO;
-	}
-
-bail:
-	return ret;
-}
-
-#define VALID_TS_WR_REG_MASK ((1 << 9) | (1 << 0xB) | (1 << 0xD))
-
-/**
- * ipath_tempsense_write - write register of temp sensor via I2C
- * @dd: the infinipath device
- * @regnum: register to write
- * @data: data to write
- *
- * returns 0 for success or < 0 for error
- */
-int ipath_tempsense_write(struct ipath_devdata *dd, u8 regnum, u8 data)
-{
-	int ret;
-
-	if (regnum > 15 || !((1 << regnum) & VALID_TS_WR_REG_MASK))
-		return -EINVAL;
-
-	ret = mutex_lock_interruptible(&dd->ipath_eep_lock);
-	if (!ret) {
-		ret = ipath_tempsense_internal_write(dd, regnum, data);
-		mutex_unlock(&dd->ipath_eep_lock);
-	}
-
-	/*
-	 * There are three possibilities here:
-	 * ret is 0 for success
-	 * ret is -ENXIO or -EINVAL from code in this file
-	 * ret is -EINTR from mutex_lock_interruptible.
-	 */
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_file_ops.c b/drivers/staging/rdma/ipath/ipath_file_ops.c
deleted file mode 100644
index 13c3cd11ab92..000000000000
--- a/drivers/staging/rdma/ipath/ipath_file_ops.c
+++ /dev/null
@@ -1,2619 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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/pci.h>
-#include <linux/poll.h>
-#include <linux/cdev.h>
-#include <linux/swap.h>
-#include <linux/export.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-#include <linux/highmem.h>
-#include <linux/io.h>
-#include <linux/jiffies.h>
-#include <linux/cpu.h>
-#include <linux/uio.h>
-#include <asm/pgtable.h>
-
-#include "ipath_kernel.h"
-#include "ipath_common.h"
-#include "ipath_user_sdma.h"
-
-static int ipath_open(struct inode *, struct file *);
-static int ipath_close(struct inode *, struct file *);
-static ssize_t ipath_write(struct file *, const char __user *, size_t,
-			   loff_t *);
-static ssize_t ipath_write_iter(struct kiocb *, struct iov_iter *from);
-static unsigned int ipath_poll(struct file *, struct poll_table_struct *);
-static int ipath_mmap(struct file *, struct vm_area_struct *);
-
-/*
- * This is really, really weird shit - write() and writev() here
- * have completely unrelated semantics.  Sucky userland ABI,
- * film at 11.
- */
-static const struct file_operations ipath_file_ops = {
-	.owner = THIS_MODULE,
-	.write = ipath_write,
-	.write_iter = ipath_write_iter,
-	.open = ipath_open,
-	.release = ipath_close,
-	.poll = ipath_poll,
-	.mmap = ipath_mmap,
-	.llseek = noop_llseek,
-};
-
-/*
- * Convert kernel virtual addresses to physical addresses so they don't
- * potentially conflict with the chip addresses used as mmap offsets.
- * It doesn't really matter what mmap offset we use as long as we can
- * interpret it correctly.
- */
-static u64 cvt_kvaddr(void *p)
-{
-	struct page *page;
-	u64 paddr = 0;
-
-	page = vmalloc_to_page(p);
-	if (page)
-		paddr = page_to_pfn(page) << PAGE_SHIFT;
-
-	return paddr;
-}
-
-static int ipath_get_base_info(struct file *fp,
-			       void __user *ubase, size_t ubase_size)
-{
-	struct ipath_portdata *pd = port_fp(fp);
-	int ret = 0;
-	struct ipath_base_info *kinfo = NULL;
-	struct ipath_devdata *dd = pd->port_dd;
-	unsigned subport_cnt;
-	int shared, master;
-	size_t sz;
-
-	subport_cnt = pd->port_subport_cnt;
-	if (!subport_cnt) {
-		shared = 0;
-		master = 0;
-		subport_cnt = 1;
-	} else {
-		shared = 1;
-		master = !subport_fp(fp);
-	}
-
-	sz = sizeof(*kinfo);
-	/* If port sharing is not requested, allow the old size structure */
-	if (!shared)
-		sz -= 7 * sizeof(u64);
-	if (ubase_size < sz) {
-		ipath_cdbg(PROC,
-			   "Base size %zu, need %zu (version mismatch?)\n",
-			   ubase_size, sz);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	kinfo = kzalloc(sizeof(*kinfo), GFP_KERNEL);
-	if (kinfo == NULL) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	ret = dd->ipath_f_get_base_info(pd, kinfo);
-	if (ret < 0)
-		goto bail;
-
-	kinfo->spi_rcvhdr_cnt = dd->ipath_rcvhdrcnt;
-	kinfo->spi_rcvhdrent_size = dd->ipath_rcvhdrentsize;
-	kinfo->spi_tidegrcnt = dd->ipath_rcvegrcnt;
-	kinfo->spi_rcv_egrbufsize = dd->ipath_rcvegrbufsize;
-	/*
-	 * have to mmap whole thing
-	 */
-	kinfo->spi_rcv_egrbuftotlen =
-		pd->port_rcvegrbuf_chunks * pd->port_rcvegrbuf_size;
-	kinfo->spi_rcv_egrperchunk = pd->port_rcvegrbufs_perchunk;
-	kinfo->spi_rcv_egrchunksize = kinfo->spi_rcv_egrbuftotlen /
-		pd->port_rcvegrbuf_chunks;
-	kinfo->spi_tidcnt = dd->ipath_rcvtidcnt / subport_cnt;
-	if (master)
-		kinfo->spi_tidcnt += dd->ipath_rcvtidcnt % subport_cnt;
-	/*
-	 * for this use, may be ipath_cfgports summed over all chips that
-	 * are are configured and present
-	 */
-	kinfo->spi_nports = dd->ipath_cfgports;
-	/* unit (chip/board) our port is on */
-	kinfo->spi_unit = dd->ipath_unit;
-	/* for now, only a single page */
-	kinfo->spi_tid_maxsize = PAGE_SIZE;
-
-	/*
-	 * Doing this per port, and based on the skip value, etc.  This has
-	 * to be the actual buffer size, since the protocol code treats it
-	 * as an array.
-	 *
-	 * These have to be set to user addresses in the user code via mmap.
-	 * These values are used on return to user code for the mmap target
-	 * addresses only.  For 32 bit, same 44 bit address problem, so use
-	 * the physical address, not virtual.  Before 2.6.11, using the
-	 * page_address() macro worked, but in 2.6.11, even that returns the
-	 * full 64 bit address (upper bits all 1's).  So far, using the
-	 * physical addresses (or chip offsets, for chip mapping) works, but
-	 * no doubt some future kernel release will change that, and we'll be
-	 * on to yet another method of dealing with this.
-	 */
-	kinfo->spi_rcvhdr_base = (u64) pd->port_rcvhdrq_phys;
-	kinfo->spi_rcvhdr_tailaddr = (u64) pd->port_rcvhdrqtailaddr_phys;
-	kinfo->spi_rcv_egrbufs = (u64) pd->port_rcvegr_phys;
-	kinfo->spi_pioavailaddr = (u64) dd->ipath_pioavailregs_phys;
-	kinfo->spi_status = (u64) kinfo->spi_pioavailaddr +
-		(void *) dd->ipath_statusp -
-		(void *) dd->ipath_pioavailregs_dma;
-	if (!shared) {
-		kinfo->spi_piocnt = pd->port_piocnt;
-		kinfo->spi_piobufbase = (u64) pd->port_piobufs;
-		kinfo->__spi_uregbase = (u64) dd->ipath_uregbase +
-			dd->ipath_ureg_align * pd->port_port;
-	} else if (master) {
-		kinfo->spi_piocnt = (pd->port_piocnt / subport_cnt) +
-				    (pd->port_piocnt % subport_cnt);
-		/* Master's PIO buffers are after all the slave's */
-		kinfo->spi_piobufbase = (u64) pd->port_piobufs +
-			dd->ipath_palign *
-			(pd->port_piocnt - kinfo->spi_piocnt);
-	} else {
-		unsigned slave = subport_fp(fp) - 1;
-
-		kinfo->spi_piocnt = pd->port_piocnt / subport_cnt;
-		kinfo->spi_piobufbase = (u64) pd->port_piobufs +
-			dd->ipath_palign * kinfo->spi_piocnt * slave;
-	}
-
-	if (shared) {
-		kinfo->spi_port_uregbase = (u64) dd->ipath_uregbase +
-			dd->ipath_ureg_align * pd->port_port;
-		kinfo->spi_port_rcvegrbuf = kinfo->spi_rcv_egrbufs;
-		kinfo->spi_port_rcvhdr_base = kinfo->spi_rcvhdr_base;
-		kinfo->spi_port_rcvhdr_tailaddr = kinfo->spi_rcvhdr_tailaddr;
-
-		kinfo->__spi_uregbase = cvt_kvaddr(pd->subport_uregbase +
-			PAGE_SIZE * subport_fp(fp));
-
-		kinfo->spi_rcvhdr_base = cvt_kvaddr(pd->subport_rcvhdr_base +
-			pd->port_rcvhdrq_size * subport_fp(fp));
-		kinfo->spi_rcvhdr_tailaddr = 0;
-		kinfo->spi_rcv_egrbufs = cvt_kvaddr(pd->subport_rcvegrbuf +
-			pd->port_rcvegrbuf_chunks * pd->port_rcvegrbuf_size *
-			subport_fp(fp));
-
-		kinfo->spi_subport_uregbase =
-			cvt_kvaddr(pd->subport_uregbase);
-		kinfo->spi_subport_rcvegrbuf =
-			cvt_kvaddr(pd->subport_rcvegrbuf);
-		kinfo->spi_subport_rcvhdr_base =
-			cvt_kvaddr(pd->subport_rcvhdr_base);
-		ipath_cdbg(PROC, "port %u flags %x %llx %llx %llx\n",
-			kinfo->spi_port, kinfo->spi_runtime_flags,
-			(unsigned long long) kinfo->spi_subport_uregbase,
-			(unsigned long long) kinfo->spi_subport_rcvegrbuf,
-			(unsigned long long) kinfo->spi_subport_rcvhdr_base);
-	}
-
-	/*
-	 * All user buffers are 2KB buffers.  If we ever support
-	 * giving 4KB buffers to user processes, this will need some
-	 * work.
-	 */
-	kinfo->spi_pioindex = (kinfo->spi_piobufbase -
-		(dd->ipath_piobufbase & 0xffffffff)) / dd->ipath_palign;
-	kinfo->spi_pioalign = dd->ipath_palign;
-
-	kinfo->spi_qpair = IPATH_KD_QP;
-	/*
-	 * user mode PIO buffers are always 2KB, even when 4KB can
-	 * be received, and sent via the kernel; this is ibmaxlen
-	 * for 2K MTU.
-	 */
-	kinfo->spi_piosize = dd->ipath_piosize2k - 2 * sizeof(u32);
-	kinfo->spi_mtu = dd->ipath_ibmaxlen;	/* maxlen, not ibmtu */
-	kinfo->spi_port = pd->port_port;
-	kinfo->spi_subport = subport_fp(fp);
-	kinfo->spi_sw_version = IPATH_KERN_SWVERSION;
-	kinfo->spi_hw_version = dd->ipath_revision;
-
-	if (master) {
-		kinfo->spi_runtime_flags |= IPATH_RUNTIME_MASTER;
-	}
-
-	sz = (ubase_size < sizeof(*kinfo)) ? ubase_size : sizeof(*kinfo);
-	if (copy_to_user(ubase, kinfo, sz))
-		ret = -EFAULT;
-
-bail:
-	kfree(kinfo);
-	return ret;
-}
-
-/**
- * ipath_tid_update - update a port TID
- * @pd: the port
- * @fp: the ipath device file
- * @ti: the TID information
- *
- * The new implementation as of Oct 2004 is that the driver assigns
- * the tid and returns it to the caller.   To make it easier to
- * catch bugs, and to reduce search time, we keep a cursor for
- * each port, walking the shadow tid array to find one that's not
- * in use.
- *
- * For now, if we can't allocate the full list, we fail, although
- * in the long run, we'll allocate as many as we can, and the
- * caller will deal with that by trying the remaining pages later.
- * That means that when we fail, we have to mark the tids as not in
- * use again, in our shadow copy.
- *
- * It's up to the caller to free the tids when they are done.
- * We'll unlock the pages as they free them.
- *
- * Also, right now we are locking one page at a time, but since
- * the intended use of this routine is for a single group of
- * virtually contiguous pages, that should change to improve
- * performance.
- */
-static int ipath_tid_update(struct ipath_portdata *pd, struct file *fp,
-			    const struct ipath_tid_info *ti)
-{
-	int ret = 0, ntids;
-	u32 tid, porttid, cnt, i, tidcnt, tidoff;
-	u16 *tidlist;
-	struct ipath_devdata *dd = pd->port_dd;
-	u64 physaddr;
-	unsigned long vaddr;
-	u64 __iomem *tidbase;
-	unsigned long tidmap[8];
-	struct page **pagep = NULL;
-	unsigned subport = subport_fp(fp);
-
-	if (!dd->ipath_pageshadow) {
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	cnt = ti->tidcnt;
-	if (!cnt) {
-		ipath_dbg("After copyin, tidcnt 0, tidlist %llx\n",
-			  (unsigned long long) ti->tidlist);
-		/*
-		 * Should we treat as success?  likely a bug
-		 */
-		ret = -EFAULT;
-		goto done;
-	}
-	porttid = pd->port_port * dd->ipath_rcvtidcnt;
-	if (!pd->port_subport_cnt) {
-		tidcnt = dd->ipath_rcvtidcnt;
-		tid = pd->port_tidcursor;
-		tidoff = 0;
-	} else if (!subport) {
-		tidcnt = (dd->ipath_rcvtidcnt / pd->port_subport_cnt) +
-			 (dd->ipath_rcvtidcnt % pd->port_subport_cnt);
-		tidoff = dd->ipath_rcvtidcnt - tidcnt;
-		porttid += tidoff;
-		tid = tidcursor_fp(fp);
-	} else {
-		tidcnt = dd->ipath_rcvtidcnt / pd->port_subport_cnt;
-		tidoff = tidcnt * (subport - 1);
-		porttid += tidoff;
-		tid = tidcursor_fp(fp);
-	}
-	if (cnt > tidcnt) {
-		/* make sure it all fits in port_tid_pg_list */
-		dev_info(&dd->pcidev->dev, "Process tried to allocate %u "
-			 "TIDs, only trying max (%u)\n", cnt, tidcnt);
-		cnt = tidcnt;
-	}
-	pagep = &((struct page **) pd->port_tid_pg_list)[tidoff];
-	tidlist = &((u16 *) &pagep[dd->ipath_rcvtidcnt])[tidoff];
-
-	memset(tidmap, 0, sizeof(tidmap));
-	/* before decrement; chip actual # */
-	ntids = tidcnt;
-	tidbase = (u64 __iomem *) (((char __iomem *) dd->ipath_kregbase) +
-				   dd->ipath_rcvtidbase +
-				   porttid * sizeof(*tidbase));
-
-	ipath_cdbg(VERBOSE, "Port%u %u tids, cursor %u, tidbase %p\n",
-		   pd->port_port, cnt, tid, tidbase);
-
-	/* virtual address of first page in transfer */
-	vaddr = ti->tidvaddr;
-	if (!access_ok(VERIFY_WRITE, (void __user *) vaddr,
-		       cnt * PAGE_SIZE)) {
-		ipath_dbg("Fail vaddr %p, %u pages, !access_ok\n",
-			  (void *)vaddr, cnt);
-		ret = -EFAULT;
-		goto done;
-	}
-	ret = ipath_get_user_pages(vaddr, cnt, pagep);
-	if (ret) {
-		if (ret == -EBUSY) {
-			ipath_dbg("Failed to lock addr %p, %u pages "
-				  "(already locked)\n",
-				  (void *) vaddr, cnt);
-			/*
-			 * for now, continue, and see what happens but with
-			 * the new implementation, this should never happen,
-			 * unless perhaps the user has mpin'ed the pages
-			 * themselves (something we need to test)
-			 */
-			ret = 0;
-		} else {
-			dev_info(&dd->pcidev->dev,
-				 "Failed to lock addr %p, %u pages: "
-				 "errno %d\n", (void *) vaddr, cnt, -ret);
-			goto done;
-		}
-	}
-	for (i = 0; i < cnt; i++, vaddr += PAGE_SIZE) {
-		for (; ntids--; tid++) {
-			if (tid == tidcnt)
-				tid = 0;
-			if (!dd->ipath_pageshadow[porttid + tid])
-				break;
-		}
-		if (ntids < 0) {
-			/*
-			 * oops, wrapped all the way through their TIDs,
-			 * and didn't have enough free; see comments at
-			 * start of routine
-			 */
-			ipath_dbg("Not enough free TIDs for %u pages "
-				  "(index %d), failing\n", cnt, i);
-			i--;	/* last tidlist[i] not filled in */
-			ret = -ENOMEM;
-			break;
-		}
-		tidlist[i] = tid + tidoff;
-		ipath_cdbg(VERBOSE, "Updating idx %u to TID %u, "
-			   "vaddr %lx\n", i, tid + tidoff, vaddr);
-		/* we "know" system pages and TID pages are same size */
-		dd->ipath_pageshadow[porttid + tid] = pagep[i];
-		dd->ipath_physshadow[porttid + tid] = ipath_map_page(
-			dd->pcidev, pagep[i], 0, PAGE_SIZE,
-			PCI_DMA_FROMDEVICE);
-		/*
-		 * don't need atomic or it's overhead
-		 */
-		__set_bit(tid, tidmap);
-		physaddr = dd->ipath_physshadow[porttid + tid];
-		ipath_stats.sps_pagelocks++;
-		ipath_cdbg(VERBOSE,
-			   "TID %u, vaddr %lx, physaddr %llx pgp %p\n",
-			   tid, vaddr, (unsigned long long) physaddr,
-			   pagep[i]);
-		dd->ipath_f_put_tid(dd, &tidbase[tid], RCVHQ_RCV_TYPE_EXPECTED,
-				    physaddr);
-		/*
-		 * don't check this tid in ipath_portshadow, since we
-		 * just filled it in; start with the next one.
-		 */
-		tid++;
-	}
-
-	if (ret) {
-		u32 limit;
-	cleanup:
-		/* jump here if copy out of updated info failed... */
-		ipath_dbg("After failure (ret=%d), undo %d of %d entries\n",
-			  -ret, i, cnt);
-		/* same code that's in ipath_free_tid() */
-		limit = sizeof(tidmap) * BITS_PER_BYTE;
-		if (limit > tidcnt)
-			/* just in case size changes in future */
-			limit = tidcnt;
-		tid = find_first_bit((const unsigned long *)tidmap, limit);
-		for (; tid < limit; tid++) {
-			if (!test_bit(tid, tidmap))
-				continue;
-			if (dd->ipath_pageshadow[porttid + tid]) {
-				ipath_cdbg(VERBOSE, "Freeing TID %u\n",
-					   tid);
-				dd->ipath_f_put_tid(dd, &tidbase[tid],
-						    RCVHQ_RCV_TYPE_EXPECTED,
-						    dd->ipath_tidinvalid);
-				pci_unmap_page(dd->pcidev,
-					dd->ipath_physshadow[porttid + tid],
-					PAGE_SIZE, PCI_DMA_FROMDEVICE);
-				dd->ipath_pageshadow[porttid + tid] = NULL;
-				ipath_stats.sps_pageunlocks++;
-			}
-		}
-		ipath_release_user_pages(pagep, cnt);
-	} else {
-		/*
-		 * Copy the updated array, with ipath_tid's filled in, back
-		 * to user.  Since we did the copy in already, this "should
-		 * never fail" If it does, we have to clean up...
-		 */
-		if (copy_to_user((void __user *)
-				 (unsigned long) ti->tidlist,
-				 tidlist, cnt * sizeof(*tidlist))) {
-			ret = -EFAULT;
-			goto cleanup;
-		}
-		if (copy_to_user((void __user *) (unsigned long) ti->tidmap,
-				 tidmap, sizeof tidmap)) {
-			ret = -EFAULT;
-			goto cleanup;
-		}
-		if (tid == tidcnt)
-			tid = 0;
-		if (!pd->port_subport_cnt)
-			pd->port_tidcursor = tid;
-		else
-			tidcursor_fp(fp) = tid;
-	}
-
-done:
-	if (ret)
-		ipath_dbg("Failed to map %u TID pages, failing with %d\n",
-			  ti->tidcnt, -ret);
-	return ret;
-}
-
-/**
- * ipath_tid_free - free a port TID
- * @pd: the port
- * @subport: the subport
- * @ti: the TID info
- *
- * right now we are unlocking one page at a time, but since
- * the intended use of this routine is for a single group of
- * virtually contiguous pages, that should change to improve
- * performance.  We check that the TID is in range for this port
- * but otherwise don't check validity; if user has an error and
- * frees the wrong tid, it's only their own data that can thereby
- * be corrupted.  We do check that the TID was in use, for sanity
- * We always use our idea of the saved address, not the address that
- * they pass in to us.
- */
-
-static int ipath_tid_free(struct ipath_portdata *pd, unsigned subport,
-			  const struct ipath_tid_info *ti)
-{
-	int ret = 0;
-	u32 tid, porttid, cnt, limit, tidcnt;
-	struct ipath_devdata *dd = pd->port_dd;
-	u64 __iomem *tidbase;
-	unsigned long tidmap[8];
-
-	if (!dd->ipath_pageshadow) {
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	if (copy_from_user(tidmap, (void __user *)(unsigned long)ti->tidmap,
-			   sizeof tidmap)) {
-		ret = -EFAULT;
-		goto done;
-	}
-
-	porttid = pd->port_port * dd->ipath_rcvtidcnt;
-	if (!pd->port_subport_cnt)
-		tidcnt = dd->ipath_rcvtidcnt;
-	else if (!subport) {
-		tidcnt = (dd->ipath_rcvtidcnt / pd->port_subport_cnt) +
-			 (dd->ipath_rcvtidcnt % pd->port_subport_cnt);
-		porttid += dd->ipath_rcvtidcnt - tidcnt;
-	} else {
-		tidcnt = dd->ipath_rcvtidcnt / pd->port_subport_cnt;
-		porttid += tidcnt * (subport - 1);
-	}
-	tidbase = (u64 __iomem *) ((char __iomem *)(dd->ipath_kregbase) +
-				   dd->ipath_rcvtidbase +
-				   porttid * sizeof(*tidbase));
-
-	limit = sizeof(tidmap) * BITS_PER_BYTE;
-	if (limit > tidcnt)
-		/* just in case size changes in future */
-		limit = tidcnt;
-	tid = find_first_bit(tidmap, limit);
-	ipath_cdbg(VERBOSE, "Port%u free %u tids; first bit (max=%d) "
-		   "set is %d, porttid %u\n", pd->port_port, ti->tidcnt,
-		   limit, tid, porttid);
-	for (cnt = 0; tid < limit; tid++) {
-		/*
-		 * small optimization; if we detect a run of 3 or so without
-		 * any set, use find_first_bit again.  That's mainly to
-		 * accelerate the case where we wrapped, so we have some at
-		 * the beginning, and some at the end, and a big gap
-		 * in the middle.
-		 */
-		if (!test_bit(tid, tidmap))
-			continue;
-		cnt++;
-		if (dd->ipath_pageshadow[porttid + tid]) {
-			struct page *p;
-			p = dd->ipath_pageshadow[porttid + tid];
-			dd->ipath_pageshadow[porttid + tid] = NULL;
-			ipath_cdbg(VERBOSE, "PID %u freeing TID %u\n",
-				   pid_nr(pd->port_pid), tid);
-			dd->ipath_f_put_tid(dd, &tidbase[tid],
-					    RCVHQ_RCV_TYPE_EXPECTED,
-					    dd->ipath_tidinvalid);
-			pci_unmap_page(dd->pcidev,
-				dd->ipath_physshadow[porttid + tid],
-				PAGE_SIZE, PCI_DMA_FROMDEVICE);
-			ipath_release_user_pages(&p, 1);
-			ipath_stats.sps_pageunlocks++;
-		} else
-			ipath_dbg("Unused tid %u, ignoring\n", tid);
-	}
-	if (cnt != ti->tidcnt)
-		ipath_dbg("passed in tidcnt %d, only %d bits set in map\n",
-			  ti->tidcnt, cnt);
-done:
-	if (ret)
-		ipath_dbg("Failed to unmap %u TID pages, failing with %d\n",
-			  ti->tidcnt, -ret);
-	return ret;
-}
-
-/**
- * ipath_set_part_key - set a partition key
- * @pd: the port
- * @key: the key
- *
- * We can have up to 4 active at a time (other than the default, which is
- * always allowed).  This is somewhat tricky, since multiple ports may set
- * the same key, so we reference count them, and clean up at exit.  All 4
- * partition keys are packed into a single infinipath register.  It's an
- * error for a process to set the same pkey multiple times.  We provide no
- * mechanism to de-allocate a pkey at this time, we may eventually need to
- * do that.  I've used the atomic operations, and no locking, and only make
- * a single pass through what's available.  This should be more than
- * adequate for some time. I'll think about spinlocks or the like if and as
- * it's necessary.
- */
-static int ipath_set_part_key(struct ipath_portdata *pd, u16 key)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	int i, any = 0, pidx = -1;
-	u16 lkey = key & 0x7FFF;
-	int ret;
-
-	if (lkey == (IPATH_DEFAULT_P_KEY & 0x7FFF)) {
-		/* nothing to do; this key always valid */
-		ret = 0;
-		goto bail;
-	}
-
-	ipath_cdbg(VERBOSE, "p%u try to set pkey %hx, current keys "
-		   "%hx:%x %hx:%x %hx:%x %hx:%x\n",
-		   pd->port_port, key, dd->ipath_pkeys[0],
-		   atomic_read(&dd->ipath_pkeyrefs[0]), dd->ipath_pkeys[1],
-		   atomic_read(&dd->ipath_pkeyrefs[1]), dd->ipath_pkeys[2],
-		   atomic_read(&dd->ipath_pkeyrefs[2]), dd->ipath_pkeys[3],
-		   atomic_read(&dd->ipath_pkeyrefs[3]));
-
-	if (!lkey) {
-		ipath_cdbg(PROC, "p%u tries to set key 0, not allowed\n",
-			   pd->port_port);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	/*
-	 * Set the full membership bit, because it has to be
-	 * set in the register or the packet, and it seems
-	 * cleaner to set in the register than to force all
-	 * callers to set it. (see bug 4331)
-	 */
-	key |= 0x8000;
-
-	for (i = 0; i < ARRAY_SIZE(pd->port_pkeys); i++) {
-		if (!pd->port_pkeys[i] && pidx == -1)
-			pidx = i;
-		if (pd->port_pkeys[i] == key) {
-			ipath_cdbg(VERBOSE, "p%u tries to set same pkey "
-				   "(%x) more than once\n",
-				   pd->port_port, key);
-			ret = -EEXIST;
-			goto bail;
-		}
-	}
-	if (pidx == -1) {
-		ipath_dbg("All pkeys for port %u already in use, "
-			  "can't set %x\n", pd->port_port, key);
-		ret = -EBUSY;
-		goto bail;
-	}
-	for (any = i = 0; i < ARRAY_SIZE(dd->ipath_pkeys); i++) {
-		if (!dd->ipath_pkeys[i]) {
-			any++;
-			continue;
-		}
-		if (dd->ipath_pkeys[i] == key) {
-			atomic_t *pkrefs = &dd->ipath_pkeyrefs[i];
-
-			if (atomic_inc_return(pkrefs) > 1) {
-				pd->port_pkeys[pidx] = key;
-				ipath_cdbg(VERBOSE, "p%u set key %x "
-					   "matches #%d, count now %d\n",
-					   pd->port_port, key, i,
-					   atomic_read(pkrefs));
-				ret = 0;
-				goto bail;
-			} else {
-				/*
-				 * lost race, decrement count, catch below
-				 */
-				atomic_dec(pkrefs);
-				ipath_cdbg(VERBOSE, "Lost race, count was "
-					   "0, after dec, it's %d\n",
-					   atomic_read(pkrefs));
-				any++;
-			}
-		}
-		if ((dd->ipath_pkeys[i] & 0x7FFF) == lkey) {
-			/*
-			 * It makes no sense to have both the limited and
-			 * full membership PKEY set at the same time since
-			 * the unlimited one will disable the limited one.
-			 */
-			ret = -EEXIST;
-			goto bail;
-		}
-	}
-	if (!any) {
-		ipath_dbg("port %u, all pkeys already in use, "
-			  "can't set %x\n", pd->port_port, key);
-		ret = -EBUSY;
-		goto bail;
-	}
-	for (any = i = 0; i < ARRAY_SIZE(dd->ipath_pkeys); i++) {
-		if (!dd->ipath_pkeys[i] &&
-		    atomic_inc_return(&dd->ipath_pkeyrefs[i]) == 1) {
-			u64 pkey;
-
-			/* for ipathstats, etc. */
-			ipath_stats.sps_pkeys[i] = lkey;
-			pd->port_pkeys[pidx] = dd->ipath_pkeys[i] = key;
-			pkey =
-				(u64) dd->ipath_pkeys[0] |
-				((u64) dd->ipath_pkeys[1] << 16) |
-				((u64) dd->ipath_pkeys[2] << 32) |
-				((u64) dd->ipath_pkeys[3] << 48);
-			ipath_cdbg(PROC, "p%u set key %x in #%d, "
-				   "portidx %d, new pkey reg %llx\n",
-				   pd->port_port, key, i, pidx,
-				   (unsigned long long) pkey);
-			ipath_write_kreg(
-				dd, dd->ipath_kregs->kr_partitionkey, pkey);
-
-			ret = 0;
-			goto bail;
-		}
-	}
-	ipath_dbg("port %u, all pkeys already in use 2nd pass, "
-		  "can't set %x\n", pd->port_port, key);
-	ret = -EBUSY;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_manage_rcvq - manage a port's receive queue
- * @pd: the port
- * @subport: the subport
- * @start_stop: action to carry out
- *
- * start_stop == 0 disables receive on the port, for use in queue
- * overflow conditions.  start_stop==1 re-enables, to be used to
- * re-init the software copy of the head register
- */
-static int ipath_manage_rcvq(struct ipath_portdata *pd, unsigned subport,
-			     int start_stop)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-
-	ipath_cdbg(PROC, "%sabling rcv for unit %u port %u:%u\n",
-		   start_stop ? "en" : "dis", dd->ipath_unit,
-		   pd->port_port, subport);
-	if (subport)
-		goto bail;
-	/* atomically clear receive enable port. */
-	if (start_stop) {
-		/*
-		 * On enable, force in-memory copy of the tail register to
-		 * 0, so that protocol code doesn't have to worry about
-		 * whether or not the chip has yet updated the in-memory
-		 * copy or not on return from the system call. The chip
-		 * always resets it's tail register back to 0 on a
-		 * transition from disabled to enabled.  This could cause a
-		 * problem if software was broken, and did the enable w/o
-		 * the disable, but eventually the in-memory copy will be
-		 * updated and correct itself, even in the face of software
-		 * bugs.
-		 */
-		if (pd->port_rcvhdrtail_kvaddr)
-			ipath_clear_rcvhdrtail(pd);
-		set_bit(dd->ipath_r_portenable_shift + pd->port_port,
-			&dd->ipath_rcvctrl);
-	} else
-		clear_bit(dd->ipath_r_portenable_shift + pd->port_port,
-			  &dd->ipath_rcvctrl);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			 dd->ipath_rcvctrl);
-	/* now be sure chip saw it before we return */
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	if (start_stop) {
-		/*
-		 * And try to be sure that tail reg update has happened too.
-		 * This should in theory interlock with the RXE changes to
-		 * the tail register.  Don't assign it to the tail register
-		 * in memory copy, since we could overwrite an update by the
-		 * chip if we did.
-		 */
-		ipath_read_ureg32(dd, ur_rcvhdrtail, pd->port_port);
-	}
-	/* always; new head should be equal to new tail; see above */
-bail:
-	return 0;
-}
-
-static void ipath_clean_part_key(struct ipath_portdata *pd,
-				 struct ipath_devdata *dd)
-{
-	int i, j, pchanged = 0;
-	u64 oldpkey;
-
-	/* for debugging only */
-	oldpkey = (u64) dd->ipath_pkeys[0] |
-		((u64) dd->ipath_pkeys[1] << 16) |
-		((u64) dd->ipath_pkeys[2] << 32) |
-		((u64) dd->ipath_pkeys[3] << 48);
-
-	for (i = 0; i < ARRAY_SIZE(pd->port_pkeys); i++) {
-		if (!pd->port_pkeys[i])
-			continue;
-		ipath_cdbg(VERBOSE, "look for key[%d] %hx in pkeys\n", i,
-			   pd->port_pkeys[i]);
-		for (j = 0; j < ARRAY_SIZE(dd->ipath_pkeys); j++) {
-			/* check for match independent of the global bit */
-			if ((dd->ipath_pkeys[j] & 0x7fff) !=
-			    (pd->port_pkeys[i] & 0x7fff))
-				continue;
-			if (atomic_dec_and_test(&dd->ipath_pkeyrefs[j])) {
-				ipath_cdbg(VERBOSE, "p%u clear key "
-					   "%x matches #%d\n",
-					   pd->port_port,
-					   pd->port_pkeys[i], j);
-				ipath_stats.sps_pkeys[j] =
-					dd->ipath_pkeys[j] = 0;
-				pchanged++;
-			} else {
-				ipath_cdbg(VERBOSE, "p%u key %x matches #%d, "
-					   "but ref still %d\n", pd->port_port,
-					   pd->port_pkeys[i], j,
-					   atomic_read(&dd->ipath_pkeyrefs[j]));
-				break;
-			}
-		}
-		pd->port_pkeys[i] = 0;
-	}
-	if (pchanged) {
-		u64 pkey = (u64) dd->ipath_pkeys[0] |
-			((u64) dd->ipath_pkeys[1] << 16) |
-			((u64) dd->ipath_pkeys[2] << 32) |
-			((u64) dd->ipath_pkeys[3] << 48);
-		ipath_cdbg(VERBOSE, "p%u old pkey reg %llx, "
-			   "new pkey reg %llx\n", pd->port_port,
-			   (unsigned long long) oldpkey,
-			   (unsigned long long) pkey);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_partitionkey,
-				 pkey);
-	}
-}
-
-/*
- * Initialize the port data with the receive buffer sizes
- * so this can be done while the master port is locked.
- * Otherwise, there is a race with a slave opening the port
- * and seeing these fields uninitialized.
- */
-static void init_user_egr_sizes(struct ipath_portdata *pd)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	unsigned egrperchunk, egrcnt, size;
-
-	/*
-	 * to avoid wasting a lot of memory, we allocate 32KB chunks of
-	 * physically contiguous memory, advance through it until used up
-	 * and then allocate more.  Of course, we need memory to store those
-	 * extra pointers, now.  Started out with 256KB, but under heavy
-	 * memory pressure (creating large files and then copying them over
-	 * NFS while doing lots of MPI jobs), we hit some allocation
-	 * failures, even though we can sleep...  (2.6.10) Still get
-	 * failures at 64K.  32K is the lowest we can go without wasting
-	 * additional memory.
-	 */
-	size = 0x8000;
-	egrperchunk = size / dd->ipath_rcvegrbufsize;
-	egrcnt = dd->ipath_rcvegrcnt;
-	pd->port_rcvegrbuf_chunks = (egrcnt + egrperchunk - 1) / egrperchunk;
-	pd->port_rcvegrbufs_perchunk = egrperchunk;
-	pd->port_rcvegrbuf_size = size;
-}
-
-/**
- * ipath_create_user_egr - allocate eager TID buffers
- * @pd: the port to allocate TID buffers for
- *
- * This routine is now quite different for user and kernel, because
- * the kernel uses skb's, for the accelerated network performance
- * This is the user port version
- *
- * Allocate the eager TID buffers and program them into infinipath
- * They are no longer completely contiguous, we do multiple allocation
- * calls.
- */
-static int ipath_create_user_egr(struct ipath_portdata *pd)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	unsigned e, egrcnt, egrperchunk, chunk, egrsize, egroff;
-	size_t size;
-	int ret;
-	gfp_t gfp_flags;
-
-	/*
-	 * GFP_USER, but without GFP_FS, so buffer cache can be
-	 * coalesced (we hope); otherwise, even at order 4,
-	 * heavy filesystem activity makes these fail, and we can
-	 * use compound pages.
-	 */
-	gfp_flags = __GFP_RECLAIM | __GFP_IO | __GFP_COMP;
-
-	egrcnt = dd->ipath_rcvegrcnt;
-	/* TID number offset for this port */
-	egroff = (pd->port_port - 1) * egrcnt + dd->ipath_p0_rcvegrcnt;
-	egrsize = dd->ipath_rcvegrbufsize;
-	ipath_cdbg(VERBOSE, "Allocating %d egr buffers, at egrtid "
-		   "offset %x, egrsize %u\n", egrcnt, egroff, egrsize);
-
-	chunk = pd->port_rcvegrbuf_chunks;
-	egrperchunk = pd->port_rcvegrbufs_perchunk;
-	size = pd->port_rcvegrbuf_size;
-	pd->port_rcvegrbuf = kmalloc(chunk * sizeof(pd->port_rcvegrbuf[0]),
-				     GFP_KERNEL);
-	if (!pd->port_rcvegrbuf) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-	pd->port_rcvegrbuf_phys =
-		kmalloc(chunk * sizeof(pd->port_rcvegrbuf_phys[0]),
-			GFP_KERNEL);
-	if (!pd->port_rcvegrbuf_phys) {
-		ret = -ENOMEM;
-		goto bail_rcvegrbuf;
-	}
-	for (e = 0; e < pd->port_rcvegrbuf_chunks; e++) {
-
-		pd->port_rcvegrbuf[e] = dma_alloc_coherent(
-			&dd->pcidev->dev, size, &pd->port_rcvegrbuf_phys[e],
-			gfp_flags);
-
-		if (!pd->port_rcvegrbuf[e]) {
-			ret = -ENOMEM;
-			goto bail_rcvegrbuf_phys;
-		}
-	}
-
-	pd->port_rcvegr_phys = pd->port_rcvegrbuf_phys[0];
-
-	for (e = chunk = 0; chunk < pd->port_rcvegrbuf_chunks; chunk++) {
-		dma_addr_t pa = pd->port_rcvegrbuf_phys[chunk];
-		unsigned i;
-
-		for (i = 0; e < egrcnt && i < egrperchunk; e++, i++) {
-			dd->ipath_f_put_tid(dd, e + egroff +
-					    (u64 __iomem *)
-					    ((char __iomem *)
-					     dd->ipath_kregbase +
-					     dd->ipath_rcvegrbase),
-					    RCVHQ_RCV_TYPE_EAGER, pa);
-			pa += egrsize;
-		}
-		cond_resched();	/* don't hog the cpu */
-	}
-
-	ret = 0;
-	goto bail;
-
-bail_rcvegrbuf_phys:
-	for (e = 0; e < pd->port_rcvegrbuf_chunks &&
-		pd->port_rcvegrbuf[e]; e++) {
-		dma_free_coherent(&dd->pcidev->dev, size,
-				  pd->port_rcvegrbuf[e],
-				  pd->port_rcvegrbuf_phys[e]);
-
-	}
-	kfree(pd->port_rcvegrbuf_phys);
-	pd->port_rcvegrbuf_phys = NULL;
-bail_rcvegrbuf:
-	kfree(pd->port_rcvegrbuf);
-	pd->port_rcvegrbuf = NULL;
-bail:
-	return ret;
-}
-
-
-/* common code for the mappings on dma_alloc_coherent mem */
-static int ipath_mmap_mem(struct vm_area_struct *vma,
-	struct ipath_portdata *pd, unsigned len, int write_ok,
-	void *kvaddr, char *what)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	unsigned long pfn;
-	int ret;
-
-	if ((vma->vm_end - vma->vm_start) > len) {
-		dev_info(&dd->pcidev->dev,
-		         "FAIL on %s: len %lx > %x\n", what,
-			 vma->vm_end - vma->vm_start, len);
-		ret = -EFAULT;
-		goto bail;
-	}
-
-	if (!write_ok) {
-		if (vma->vm_flags & VM_WRITE) {
-			dev_info(&dd->pcidev->dev,
-				 "%s must be mapped readonly\n", what);
-			ret = -EPERM;
-			goto bail;
-		}
-
-		/* don't allow them to later change with mprotect */
-		vma->vm_flags &= ~VM_MAYWRITE;
-	}
-
-	pfn = virt_to_phys(kvaddr) >> PAGE_SHIFT;
-	ret = remap_pfn_range(vma, vma->vm_start, pfn,
-			      len, vma->vm_page_prot);
-	if (ret)
-		dev_info(&dd->pcidev->dev, "%s port%u mmap of %lx, %x "
-			 "bytes r%c failed: %d\n", what, pd->port_port,
-			 pfn, len, write_ok?'w':'o', ret);
-	else
-		ipath_cdbg(VERBOSE, "%s port%u mmaped %lx, %x bytes "
-			   "r%c\n", what, pd->port_port, pfn, len,
-			   write_ok?'w':'o');
-bail:
-	return ret;
-}
-
-static int mmap_ureg(struct vm_area_struct *vma, struct ipath_devdata *dd,
-		     u64 ureg)
-{
-	unsigned long phys;
-	int ret;
-
-	/*
-	 * This is real hardware, so use io_remap.  This is the mechanism
-	 * for the user process to update the head registers for their port
-	 * in the chip.
-	 */
-	if ((vma->vm_end - vma->vm_start) > PAGE_SIZE) {
-		dev_info(&dd->pcidev->dev, "FAIL mmap userreg: reqlen "
-			 "%lx > PAGE\n", vma->vm_end - vma->vm_start);
-		ret = -EFAULT;
-	} else {
-		phys = dd->ipath_physaddr + ureg;
-		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
-
-		vma->vm_flags |= VM_DONTCOPY | VM_DONTEXPAND;
-		ret = io_remap_pfn_range(vma, vma->vm_start,
-					 phys >> PAGE_SHIFT,
-					 vma->vm_end - vma->vm_start,
-					 vma->vm_page_prot);
-	}
-	return ret;
-}
-
-static int mmap_piobufs(struct vm_area_struct *vma,
-			struct ipath_devdata *dd,
-			struct ipath_portdata *pd,
-			unsigned piobufs, unsigned piocnt)
-{
-	unsigned long phys;
-	int ret;
-
-	/*
-	 * When we map the PIO buffers in the chip, we want to map them as
-	 * writeonly, no read possible.   This prevents access to previous
-	 * process data, and catches users who might try to read the i/o
-	 * space due to a bug.
-	 */
-	if ((vma->vm_end - vma->vm_start) > (piocnt * dd->ipath_palign)) {
-		dev_info(&dd->pcidev->dev, "FAIL mmap piobufs: "
-			 "reqlen %lx > PAGE\n",
-			 vma->vm_end - vma->vm_start);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	phys = dd->ipath_physaddr + piobufs;
-
-#if defined(__powerpc__)
-	/* There isn't a generic way to specify writethrough mappings */
-	pgprot_val(vma->vm_page_prot) |= _PAGE_NO_CACHE;
-	pgprot_val(vma->vm_page_prot) |= _PAGE_WRITETHRU;
-	pgprot_val(vma->vm_page_prot) &= ~_PAGE_GUARDED;
-#endif
-
-	/*
-	 * don't allow them to later change to readable with mprotect (for when
-	 * not initially mapped readable, as is normally the case)
-	 */
-	vma->vm_flags &= ~VM_MAYREAD;
-	vma->vm_flags |= VM_DONTCOPY | VM_DONTEXPAND;
-
-	ret = io_remap_pfn_range(vma, vma->vm_start, phys >> PAGE_SHIFT,
-				 vma->vm_end - vma->vm_start,
-				 vma->vm_page_prot);
-bail:
-	return ret;
-}
-
-static int mmap_rcvegrbufs(struct vm_area_struct *vma,
-			   struct ipath_portdata *pd)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	unsigned long start, size;
-	size_t total_size, i;
-	unsigned long pfn;
-	int ret;
-
-	size = pd->port_rcvegrbuf_size;
-	total_size = pd->port_rcvegrbuf_chunks * size;
-	if ((vma->vm_end - vma->vm_start) > total_size) {
-		dev_info(&dd->pcidev->dev, "FAIL on egr bufs: "
-			 "reqlen %lx > actual %lx\n",
-			 vma->vm_end - vma->vm_start,
-			 (unsigned long) total_size);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	if (vma->vm_flags & VM_WRITE) {
-		dev_info(&dd->pcidev->dev, "Can't map eager buffers as "
-			 "writable (flags=%lx)\n", vma->vm_flags);
-		ret = -EPERM;
-		goto bail;
-	}
-	/* don't allow them to later change to writeable with mprotect */
-	vma->vm_flags &= ~VM_MAYWRITE;
-
-	start = vma->vm_start;
-
-	for (i = 0; i < pd->port_rcvegrbuf_chunks; i++, start += size) {
-		pfn = virt_to_phys(pd->port_rcvegrbuf[i]) >> PAGE_SHIFT;
-		ret = remap_pfn_range(vma, start, pfn, size,
-				      vma->vm_page_prot);
-		if (ret < 0)
-			goto bail;
-	}
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/*
- * ipath_file_vma_fault - handle a VMA page fault.
- */
-static int ipath_file_vma_fault(struct vm_area_struct *vma,
-					struct vm_fault *vmf)
-{
-	struct page *page;
-
-	page = vmalloc_to_page((void *)(vmf->pgoff << PAGE_SHIFT));
-	if (!page)
-		return VM_FAULT_SIGBUS;
-	get_page(page);
-	vmf->page = page;
-
-	return 0;
-}
-
-static const struct vm_operations_struct ipath_file_vm_ops = {
-	.fault = ipath_file_vma_fault,
-};
-
-static int mmap_kvaddr(struct vm_area_struct *vma, u64 pgaddr,
-		       struct ipath_portdata *pd, unsigned subport)
-{
-	unsigned long len;
-	struct ipath_devdata *dd;
-	void *addr;
-	size_t size;
-	int ret = 0;
-
-	/* If the port is not shared, all addresses should be physical */
-	if (!pd->port_subport_cnt)
-		goto bail;
-
-	dd = pd->port_dd;
-	size = pd->port_rcvegrbuf_chunks * pd->port_rcvegrbuf_size;
-
-	/*
-	 * Each process has all the subport uregbase, rcvhdrq, and
-	 * rcvegrbufs mmapped - as an array for all the processes,
-	 * and also separately for this process.
-	 */
-	if (pgaddr == cvt_kvaddr(pd->subport_uregbase)) {
-		addr = pd->subport_uregbase;
-		size = PAGE_SIZE * pd->port_subport_cnt;
-	} else if (pgaddr == cvt_kvaddr(pd->subport_rcvhdr_base)) {
-		addr = pd->subport_rcvhdr_base;
-		size = pd->port_rcvhdrq_size * pd->port_subport_cnt;
-	} else if (pgaddr == cvt_kvaddr(pd->subport_rcvegrbuf)) {
-		addr = pd->subport_rcvegrbuf;
-		size *= pd->port_subport_cnt;
-        } else if (pgaddr == cvt_kvaddr(pd->subport_uregbase +
-                                        PAGE_SIZE * subport)) {
-                addr = pd->subport_uregbase + PAGE_SIZE * subport;
-                size = PAGE_SIZE;
-        } else if (pgaddr == cvt_kvaddr(pd->subport_rcvhdr_base +
-                                pd->port_rcvhdrq_size * subport)) {
-                addr = pd->subport_rcvhdr_base +
-                        pd->port_rcvhdrq_size * subport;
-                size = pd->port_rcvhdrq_size;
-        } else if (pgaddr == cvt_kvaddr(pd->subport_rcvegrbuf +
-                               size * subport)) {
-                addr = pd->subport_rcvegrbuf + size * subport;
-                /* rcvegrbufs are read-only on the slave */
-                if (vma->vm_flags & VM_WRITE) {
-                        dev_info(&dd->pcidev->dev,
-                                 "Can't map eager buffers as "
-                                 "writable (flags=%lx)\n", vma->vm_flags);
-                        ret = -EPERM;
-                        goto bail;
-                }
-                /*
-                 * Don't allow permission to later change to writeable
-                 * with mprotect.
-                 */
-                vma->vm_flags &= ~VM_MAYWRITE;
-	} else {
-		goto bail;
-	}
-	len = vma->vm_end - vma->vm_start;
-	if (len > size) {
-		ipath_cdbg(MM, "FAIL: reqlen %lx > %zx\n", len, size);
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	vma->vm_pgoff = (unsigned long) addr >> PAGE_SHIFT;
-	vma->vm_ops = &ipath_file_vm_ops;
-	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
-	ret = 1;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_mmap - mmap various structures into user space
- * @fp: the file pointer
- * @vma: the VM area
- *
- * We use this to have a shared buffer between the kernel and the user code
- * for the rcvhdr queue, egr buffers, and the per-port user regs and pio
- * buffers in the chip.  We have the open and close entries so we can bump
- * the ref count and keep the driver from being unloaded while still mapped.
- */
-static int ipath_mmap(struct file *fp, struct vm_area_struct *vma)
-{
-	struct ipath_portdata *pd;
-	struct ipath_devdata *dd;
-	u64 pgaddr, ureg;
-	unsigned piobufs, piocnt;
-	int ret;
-
-	pd = port_fp(fp);
-	if (!pd) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	dd = pd->port_dd;
-
-	/*
-	 * This is the ipath_do_user_init() code, mapping the shared buffers
-	 * into the user process. The address referred to by vm_pgoff is the
-	 * file offset passed via mmap().  For shared ports, this is the
-	 * kernel vmalloc() address of the pages to share with the master.
-	 * For non-shared or master ports, this is a physical address.
-	 * We only do one mmap for each space mapped.
-	 */
-	pgaddr = vma->vm_pgoff << PAGE_SHIFT;
-
-	/*
-	 * Check for 0 in case one of the allocations failed, but user
-	 * called mmap anyway.
-	 */
-	if (!pgaddr)  {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	ipath_cdbg(MM, "pgaddr %llx vm_start=%lx len %lx port %u:%u:%u\n",
-		   (unsigned long long) pgaddr, vma->vm_start,
-		   vma->vm_end - vma->vm_start, dd->ipath_unit,
-		   pd->port_port, subport_fp(fp));
-
-	/*
-	 * Physical addresses must fit in 40 bits for our hardware.
-	 * Check for kernel virtual addresses first, anything else must
-	 * match a HW or memory address.
-	 */
-	ret = mmap_kvaddr(vma, pgaddr, pd, subport_fp(fp));
-	if (ret) {
-		if (ret > 0)
-			ret = 0;
-		goto bail;
-	}
-
-	ureg = dd->ipath_uregbase + dd->ipath_ureg_align * pd->port_port;
-	if (!pd->port_subport_cnt) {
-		/* port is not shared */
-		piocnt = pd->port_piocnt;
-		piobufs = pd->port_piobufs;
-	} else if (!subport_fp(fp)) {
-		/* caller is the master */
-		piocnt = (pd->port_piocnt / pd->port_subport_cnt) +
-			 (pd->port_piocnt % pd->port_subport_cnt);
-		piobufs = pd->port_piobufs +
-			dd->ipath_palign * (pd->port_piocnt - piocnt);
-	} else {
-		unsigned slave = subport_fp(fp) - 1;
-
-		/* caller is a slave */
-		piocnt = pd->port_piocnt / pd->port_subport_cnt;
-		piobufs = pd->port_piobufs + dd->ipath_palign * piocnt * slave;
-	}
-
-	if (pgaddr == ureg)
-		ret = mmap_ureg(vma, dd, ureg);
-	else if (pgaddr == piobufs)
-		ret = mmap_piobufs(vma, dd, pd, piobufs, piocnt);
-	else if (pgaddr == dd->ipath_pioavailregs_phys)
-		/* in-memory copy of pioavail registers */
-		ret = ipath_mmap_mem(vma, pd, PAGE_SIZE, 0,
-			      	     (void *) dd->ipath_pioavailregs_dma,
-				     "pioavail registers");
-	else if (pgaddr == pd->port_rcvegr_phys)
-		ret = mmap_rcvegrbufs(vma, pd);
-	else if (pgaddr == (u64) pd->port_rcvhdrq_phys)
-		/*
-		 * The rcvhdrq itself; readonly except on HT (so have
-		 * to allow writable mapping), multiple pages, contiguous
-		 * from an i/o perspective.
-		 */
-		ret = ipath_mmap_mem(vma, pd, pd->port_rcvhdrq_size, 1,
-				     pd->port_rcvhdrq,
-				     "rcvhdrq");
-	else if (pgaddr == (u64) pd->port_rcvhdrqtailaddr_phys)
-		/* in-memory copy of rcvhdrq tail register */
-		ret = ipath_mmap_mem(vma, pd, PAGE_SIZE, 0,
-				     pd->port_rcvhdrtail_kvaddr,
-				     "rcvhdrq tail");
-	else
-		ret = -EINVAL;
-
-	vma->vm_private_data = NULL;
-
-	if (ret < 0)
-		dev_info(&dd->pcidev->dev,
-			 "Failure %d on off %llx len %lx\n",
-			 -ret, (unsigned long long)pgaddr,
-			 vma->vm_end - vma->vm_start);
-bail:
-	return ret;
-}
-
-static unsigned ipath_poll_hdrqfull(struct ipath_portdata *pd)
-{
-	unsigned pollflag = 0;
-
-	if ((pd->poll_type & IPATH_POLL_TYPE_OVERFLOW) &&
-	    pd->port_hdrqfull != pd->port_hdrqfull_poll) {
-		pollflag |= POLLIN | POLLRDNORM;
-		pd->port_hdrqfull_poll = pd->port_hdrqfull;
-	}
-
-	return pollflag;
-}
-
-static unsigned int ipath_poll_urgent(struct ipath_portdata *pd,
-				      struct file *fp,
-				      struct poll_table_struct *pt)
-{
-	unsigned pollflag = 0;
-	struct ipath_devdata *dd;
-
-	dd = pd->port_dd;
-
-	/* variable access in ipath_poll_hdrqfull() needs this */
-	rmb();
-	pollflag = ipath_poll_hdrqfull(pd);
-
-	if (pd->port_urgent != pd->port_urgent_poll) {
-		pollflag |= POLLIN | POLLRDNORM;
-		pd->port_urgent_poll = pd->port_urgent;
-	}
-
-	if (!pollflag) {
-		/* this saves a spin_lock/unlock in interrupt handler... */
-		set_bit(IPATH_PORT_WAITING_URG, &pd->port_flag);
-		/* flush waiting flag so don't miss an event... */
-		wmb();
-		poll_wait(fp, &pd->port_wait, pt);
-	}
-
-	return pollflag;
-}
-
-static unsigned int ipath_poll_next(struct ipath_portdata *pd,
-				    struct file *fp,
-				    struct poll_table_struct *pt)
-{
-	u32 head;
-	u32 tail;
-	unsigned pollflag = 0;
-	struct ipath_devdata *dd;
-
-	dd = pd->port_dd;
-
-	/* variable access in ipath_poll_hdrqfull() needs this */
-	rmb();
-	pollflag = ipath_poll_hdrqfull(pd);
-
-	head = ipath_read_ureg32(dd, ur_rcvhdrhead, pd->port_port);
-	if (pd->port_rcvhdrtail_kvaddr)
-		tail = ipath_get_rcvhdrtail(pd);
-	else
-		tail = ipath_read_ureg32(dd, ur_rcvhdrtail, pd->port_port);
-
-	if (head != tail)
-		pollflag |= POLLIN | POLLRDNORM;
-	else {
-		/* this saves a spin_lock/unlock in interrupt handler */
-		set_bit(IPATH_PORT_WAITING_RCV, &pd->port_flag);
-		/* flush waiting flag so we don't miss an event */
-		wmb();
-
-		set_bit(pd->port_port + dd->ipath_r_intravail_shift,
-			&dd->ipath_rcvctrl);
-
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-				 dd->ipath_rcvctrl);
-
-		if (dd->ipath_rhdrhead_intr_off) /* arm rcv interrupt */
-			ipath_write_ureg(dd, ur_rcvhdrhead,
-					 dd->ipath_rhdrhead_intr_off | head,
-					 pd->port_port);
-
-		poll_wait(fp, &pd->port_wait, pt);
-	}
-
-	return pollflag;
-}
-
-static unsigned int ipath_poll(struct file *fp,
-			       struct poll_table_struct *pt)
-{
-	struct ipath_portdata *pd;
-	unsigned pollflag;
-
-	pd = port_fp(fp);
-	if (!pd)
-		pollflag = 0;
-	else if (pd->poll_type & IPATH_POLL_TYPE_URGENT)
-		pollflag = ipath_poll_urgent(pd, fp, pt);
-	else
-		pollflag = ipath_poll_next(pd, fp, pt);
-
-	return pollflag;
-}
-
-static int ipath_supports_subports(int user_swmajor, int user_swminor)
-{
-	/* no subport implementation prior to software version 1.3 */
-	return (user_swmajor > 1) || (user_swminor >= 3);
-}
-
-static int ipath_compatible_subports(int user_swmajor, int user_swminor)
-{
-	/* this code is written long-hand for clarity */
-	if (IPATH_USER_SWMAJOR != user_swmajor) {
-		/* no promise of compatibility if major mismatch */
-		return 0;
-	}
-	if (IPATH_USER_SWMAJOR == 1) {
-		switch (IPATH_USER_SWMINOR) {
-		case 0:
-		case 1:
-		case 2:
-			/* no subport implementation so cannot be compatible */
-			return 0;
-		case 3:
-			/* 3 is only compatible with itself */
-			return user_swminor == 3;
-		default:
-			/* >= 4 are compatible (or are expected to be) */
-			return user_swminor >= 4;
-		}
-	}
-	/* make no promises yet for future major versions */
-	return 0;
-}
-
-static int init_subports(struct ipath_devdata *dd,
-			 struct ipath_portdata *pd,
-			 const struct ipath_user_info *uinfo)
-{
-	int ret = 0;
-	unsigned num_subports;
-	size_t size;
-
-	/*
-	 * If the user is requesting zero subports,
-	 * skip the subport allocation.
-	 */
-	if (uinfo->spu_subport_cnt <= 0)
-		goto bail;
-
-	/* Self-consistency check for ipath_compatible_subports() */
-	if (ipath_supports_subports(IPATH_USER_SWMAJOR, IPATH_USER_SWMINOR) &&
-	    !ipath_compatible_subports(IPATH_USER_SWMAJOR,
-				       IPATH_USER_SWMINOR)) {
-		dev_info(&dd->pcidev->dev,
-			 "Inconsistent ipath_compatible_subports()\n");
-		goto bail;
-	}
-
-	/* Check for subport compatibility */
-	if (!ipath_compatible_subports(uinfo->spu_userversion >> 16,
-				       uinfo->spu_userversion & 0xffff)) {
-		dev_info(&dd->pcidev->dev,
-			 "Mismatched user version (%d.%d) and driver "
-			 "version (%d.%d) while port sharing. Ensure "
-                         "that driver and library are from the same "
-                         "release.\n",
-			 (int) (uinfo->spu_userversion >> 16),
-                         (int) (uinfo->spu_userversion & 0xffff),
-			 IPATH_USER_SWMAJOR,
-	                 IPATH_USER_SWMINOR);
-		goto bail;
-	}
-	if (uinfo->spu_subport_cnt > INFINIPATH_MAX_SUBPORT) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	num_subports = uinfo->spu_subport_cnt;
-	pd->subport_uregbase = vzalloc(PAGE_SIZE * num_subports);
-	if (!pd->subport_uregbase) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-	/* Note: pd->port_rcvhdrq_size isn't initialized yet. */
-	size = ALIGN(dd->ipath_rcvhdrcnt * dd->ipath_rcvhdrentsize *
-		     sizeof(u32), PAGE_SIZE) * num_subports;
-	pd->subport_rcvhdr_base = vzalloc(size);
-	if (!pd->subport_rcvhdr_base) {
-		ret = -ENOMEM;
-		goto bail_ureg;
-	}
-
-	pd->subport_rcvegrbuf = vzalloc(pd->port_rcvegrbuf_chunks *
-					pd->port_rcvegrbuf_size *
-					num_subports);
-	if (!pd->subport_rcvegrbuf) {
-		ret = -ENOMEM;
-		goto bail_rhdr;
-	}
-
-	pd->port_subport_cnt = uinfo->spu_subport_cnt;
-	pd->port_subport_id = uinfo->spu_subport_id;
-	pd->active_slaves = 1;
-	set_bit(IPATH_PORT_MASTER_UNINIT, &pd->port_flag);
-	goto bail;
-
-bail_rhdr:
-	vfree(pd->subport_rcvhdr_base);
-bail_ureg:
-	vfree(pd->subport_uregbase);
-	pd->subport_uregbase = NULL;
-bail:
-	return ret;
-}
-
-static int try_alloc_port(struct ipath_devdata *dd, int port,
-			  struct file *fp,
-			  const struct ipath_user_info *uinfo)
-{
-	struct ipath_portdata *pd;
-	int ret;
-
-	if (!(pd = dd->ipath_pd[port])) {
-		void *ptmp;
-
-		pd = kzalloc(sizeof(struct ipath_portdata), GFP_KERNEL);
-
-		/*
-		 * Allocate memory for use in ipath_tid_update() just once
-		 * at open, not per call.  Reduces cost of expected send
-		 * setup.
-		 */
-		ptmp = kmalloc(dd->ipath_rcvtidcnt * sizeof(u16) +
-			       dd->ipath_rcvtidcnt * sizeof(struct page **),
-			       GFP_KERNEL);
-		if (!pd || !ptmp) {
-			ipath_dev_err(dd, "Unable to allocate portdata "
-				      "memory, failing open\n");
-			ret = -ENOMEM;
-			kfree(pd);
-			kfree(ptmp);
-			goto bail;
-		}
-		dd->ipath_pd[port] = pd;
-		dd->ipath_pd[port]->port_port = port;
-		dd->ipath_pd[port]->port_dd = dd;
-		dd->ipath_pd[port]->port_tid_pg_list = ptmp;
-		init_waitqueue_head(&dd->ipath_pd[port]->port_wait);
-	}
-	if (!pd->port_cnt) {
-		pd->userversion = uinfo->spu_userversion;
-		init_user_egr_sizes(pd);
-		if ((ret = init_subports(dd, pd, uinfo)) != 0)
-			goto bail;
-		ipath_cdbg(PROC, "%s[%u] opened unit:port %u:%u\n",
-			   current->comm, current->pid, dd->ipath_unit,
-			   port);
-		pd->port_cnt = 1;
-		port_fp(fp) = pd;
-		pd->port_pid = get_pid(task_pid(current));
-		strlcpy(pd->port_comm, current->comm, sizeof(pd->port_comm));
-		ipath_stats.sps_ports++;
-		ret = 0;
-	} else
-		ret = -EBUSY;
-
-bail:
-	return ret;
-}
-
-static inline int usable(struct ipath_devdata *dd)
-{
-	return dd &&
-		(dd->ipath_flags & IPATH_PRESENT) &&
-		dd->ipath_kregbase &&
-		dd->ipath_lid &&
-		!(dd->ipath_flags & (IPATH_LINKDOWN | IPATH_DISABLED
-				     | IPATH_LINKUNK));
-}
-
-static int find_free_port(int unit, struct file *fp,
-			  const struct ipath_user_info *uinfo)
-{
-	struct ipath_devdata *dd = ipath_lookup(unit);
-	int ret, i;
-
-	if (!dd) {
-		ret = -ENODEV;
-		goto bail;
-	}
-
-	if (!usable(dd)) {
-		ret = -ENETDOWN;
-		goto bail;
-	}
-
-	for (i = 1; i < dd->ipath_cfgports; i++) {
-		ret = try_alloc_port(dd, i, fp, uinfo);
-		if (ret != -EBUSY)
-			goto bail;
-	}
-	ret = -EBUSY;
-
-bail:
-	return ret;
-}
-
-static int find_best_unit(struct file *fp,
-			  const struct ipath_user_info *uinfo)
-{
-	int ret = 0, i, prefunit = -1, devmax;
-	int maxofallports, npresent, nup;
-	int ndev;
-
-	devmax = ipath_count_units(&npresent, &nup, &maxofallports);
-
-	/*
-	 * This code is present to allow a knowledgeable person to
-	 * specify the layout of processes to processors before opening
-	 * this driver, and then we'll assign the process to the "closest"
-	 * InfiniPath chip to that processor (we assume reasonable connectivity,
-	 * for now).  This code assumes that if affinity has been set
-	 * before this point, that at most one cpu is set; for now this
-	 * is reasonable.  I check for both cpumask_empty() and cpumask_full(),
-	 * in case some kernel variant sets none of the bits when no
-	 * affinity is set.  2.6.11 and 12 kernels have all present
-	 * cpus set.  Some day we'll have to fix it up further to handle
-	 * a cpu subset.  This algorithm fails for two HT chips connected
-	 * in tunnel fashion.  Eventually this needs real topology
-	 * information.  There may be some issues with dual core numbering
-	 * as well.  This needs more work prior to release.
-	 */
-	if (!cpumask_empty(tsk_cpus_allowed(current)) &&
-	    !cpumask_full(tsk_cpus_allowed(current))) {
-		int ncpus = num_online_cpus(), curcpu = -1, nset = 0;
-		get_online_cpus();
-		for_each_online_cpu(i)
-			if (cpumask_test_cpu(i, tsk_cpus_allowed(current))) {
-				ipath_cdbg(PROC, "%s[%u] affinity set for "
-					   "cpu %d/%d\n", current->comm,
-					   current->pid, i, ncpus);
-				curcpu = i;
-				nset++;
-			}
-		put_online_cpus();
-		if (curcpu != -1 && nset != ncpus) {
-			if (npresent) {
-				prefunit = curcpu / (ncpus / npresent);
-				ipath_cdbg(PROC,"%s[%u] %d chips, %d cpus, "
-					  "%d cpus/chip, select unit %d\n",
-					  current->comm, current->pid,
-					  npresent, ncpus, ncpus / npresent,
-					  prefunit);
-			}
-		}
-	}
-
-	/*
-	 * user ports start at 1, kernel port is 0
-	 * For now, we do round-robin access across all chips
-	 */
-
-	if (prefunit != -1)
-		devmax = prefunit + 1;
-recheck:
-	for (i = 1; i < maxofallports; i++) {
-		for (ndev = prefunit != -1 ? prefunit : 0; ndev < devmax;
-		     ndev++) {
-			struct ipath_devdata *dd = ipath_lookup(ndev);
-
-			if (!usable(dd))
-				continue; /* can't use this unit */
-			if (i >= dd->ipath_cfgports)
-				/*
-				 * Maxed out on users of this unit. Try
-				 * next.
-				 */
-				continue;
-			ret = try_alloc_port(dd, i, fp, uinfo);
-			if (!ret)
-				goto done;
-		}
-	}
-
-	if (npresent) {
-		if (nup == 0) {
-			ret = -ENETDOWN;
-			ipath_dbg("No ports available (none initialized "
-				  "and ready)\n");
-		} else {
-			if (prefunit > 0) {
-				/* if started above 0, retry from 0 */
-				ipath_cdbg(PROC,
-					   "%s[%u] no ports on prefunit "
-					   "%d, clear and re-check\n",
-					   current->comm, current->pid,
-					   prefunit);
-				devmax = ipath_count_units(NULL, NULL,
-							   NULL);
-				prefunit = -1;
-				goto recheck;
-			}
-			ret = -EBUSY;
-			ipath_dbg("No ports available\n");
-		}
-	} else {
-		ret = -ENXIO;
-		ipath_dbg("No boards found\n");
-	}
-
-done:
-	return ret;
-}
-
-static int find_shared_port(struct file *fp,
-			    const struct ipath_user_info *uinfo)
-{
-	int devmax, ndev, i;
-	int ret = 0;
-
-	devmax = ipath_count_units(NULL, NULL, NULL);
-
-	for (ndev = 0; ndev < devmax; ndev++) {
-		struct ipath_devdata *dd = ipath_lookup(ndev);
-
-		if (!usable(dd))
-			continue;
-		for (i = 1; i < dd->ipath_cfgports; i++) {
-			struct ipath_portdata *pd = dd->ipath_pd[i];
-
-			/* Skip ports which are not yet open */
-			if (!pd || !pd->port_cnt)
-				continue;
-			/* Skip port if it doesn't match the requested one */
-			if (pd->port_subport_id != uinfo->spu_subport_id)
-				continue;
-			/* Verify the sharing process matches the master */
-			if (pd->port_subport_cnt != uinfo->spu_subport_cnt ||
-			    pd->userversion != uinfo->spu_userversion ||
-			    pd->port_cnt >= pd->port_subport_cnt) {
-				ret = -EINVAL;
-				goto done;
-			}
-			port_fp(fp) = pd;
-			subport_fp(fp) = pd->port_cnt++;
-			pd->port_subpid[subport_fp(fp)] =
-				get_pid(task_pid(current));
-			tidcursor_fp(fp) = 0;
-			pd->active_slaves |= 1 << subport_fp(fp);
-			ipath_cdbg(PROC,
-				   "%s[%u] %u sharing %s[%u] unit:port %u:%u\n",
-				   current->comm, current->pid,
-				   subport_fp(fp),
-				   pd->port_comm, pid_nr(pd->port_pid),
-				   dd->ipath_unit, pd->port_port);
-			ret = 1;
-			goto done;
-		}
-	}
-
-done:
-	return ret;
-}
-
-static int ipath_open(struct inode *in, struct file *fp)
-{
-	/* The real work is performed later in ipath_assign_port() */
-	fp->private_data = kzalloc(sizeof(struct ipath_filedata), GFP_KERNEL);
-	return fp->private_data ? 0 : -ENOMEM;
-}
-
-/* Get port early, so can set affinity prior to memory allocation */
-static int ipath_assign_port(struct file *fp,
-			      const struct ipath_user_info *uinfo)
-{
-	int ret;
-	int i_minor;
-	unsigned swmajor, swminor;
-
-	/* Check to be sure we haven't already initialized this file */
-	if (port_fp(fp)) {
-		ret = -EINVAL;
-		goto done;
-	}
-
-	/* for now, if major version is different, bail */
-	swmajor = uinfo->spu_userversion >> 16;
-	if (swmajor != IPATH_USER_SWMAJOR) {
-		ipath_dbg("User major version %d not same as driver "
-			  "major %d\n", uinfo->spu_userversion >> 16,
-			  IPATH_USER_SWMAJOR);
-		ret = -ENODEV;
-		goto done;
-	}
-
-	swminor = uinfo->spu_userversion & 0xffff;
-	if (swminor != IPATH_USER_SWMINOR)
-		ipath_dbg("User minor version %d not same as driver "
-			  "minor %d\n", swminor, IPATH_USER_SWMINOR);
-
-	mutex_lock(&ipath_mutex);
-
-	if (ipath_compatible_subports(swmajor, swminor) &&
-	    uinfo->spu_subport_cnt &&
-	    (ret = find_shared_port(fp, uinfo))) {
-		if (ret > 0)
-			ret = 0;
-		goto done_chk_sdma;
-	}
-
-	i_minor = iminor(file_inode(fp)) - IPATH_USER_MINOR_BASE;
-	ipath_cdbg(VERBOSE, "open on dev %lx (minor %d)\n",
-		   (long)file_inode(fp)->i_rdev, i_minor);
-
-	if (i_minor)
-		ret = find_free_port(i_minor - 1, fp, uinfo);
-	else
-		ret = find_best_unit(fp, uinfo);
-
-done_chk_sdma:
-	if (!ret) {
-		struct ipath_filedata *fd = fp->private_data;
-		const struct ipath_portdata *pd = fd->pd;
-		const struct ipath_devdata *dd = pd->port_dd;
-
-		fd->pq = ipath_user_sdma_queue_create(&dd->pcidev->dev,
-						      dd->ipath_unit,
-						      pd->port_port,
-						      fd->subport);
-
-		if (!fd->pq)
-			ret = -ENOMEM;
-	}
-
-	mutex_unlock(&ipath_mutex);
-
-done:
-	return ret;
-}
-
-
-static int ipath_do_user_init(struct file *fp,
-			      const struct ipath_user_info *uinfo)
-{
-	int ret;
-	struct ipath_portdata *pd = port_fp(fp);
-	struct ipath_devdata *dd;
-	u32 head32;
-
-	/* Subports don't need to initialize anything since master did it. */
-	if (subport_fp(fp)) {
-		ret = wait_event_interruptible(pd->port_wait,
-			!test_bit(IPATH_PORT_MASTER_UNINIT, &pd->port_flag));
-		goto done;
-	}
-
-	dd = pd->port_dd;
-
-	if (uinfo->spu_rcvhdrsize) {
-		ret = ipath_setrcvhdrsize(dd, uinfo->spu_rcvhdrsize);
-		if (ret)
-			goto done;
-	}
-
-	/* for now we do nothing with rcvhdrcnt: uinfo->spu_rcvhdrcnt */
-
-	/* some ports may get extra buffers, calculate that here */
-	if (pd->port_port <= dd->ipath_ports_extrabuf)
-		pd->port_piocnt = dd->ipath_pbufsport + 1;
-	else
-		pd->port_piocnt = dd->ipath_pbufsport;
-
-	/* for right now, kernel piobufs are at end, so port 1 is at 0 */
-	if (pd->port_port <= dd->ipath_ports_extrabuf)
-		pd->port_pio_base = (dd->ipath_pbufsport + 1)
-			* (pd->port_port - 1);
-	else
-		pd->port_pio_base = dd->ipath_ports_extrabuf +
-			dd->ipath_pbufsport * (pd->port_port - 1);
-	pd->port_piobufs = dd->ipath_piobufbase +
-		pd->port_pio_base * dd->ipath_palign;
-	ipath_cdbg(VERBOSE, "piobuf base for port %u is 0x%x, piocnt %u,"
-		" first pio %u\n", pd->port_port, pd->port_piobufs,
-		pd->port_piocnt, pd->port_pio_base);
-	ipath_chg_pioavailkernel(dd, pd->port_pio_base, pd->port_piocnt, 0);
-
-	/*
-	 * Now allocate the rcvhdr Q and eager TIDs; skip the TID
-	 * array for time being.  If pd->port_port > chip-supported,
-	 * we need to do extra stuff here to handle by handling overflow
-	 * through port 0, someday
-	 */
-	ret = ipath_create_rcvhdrq(dd, pd);
-	if (!ret)
-		ret = ipath_create_user_egr(pd);
-	if (ret)
-		goto done;
-
-	/*
-	 * set the eager head register for this port to the current values
-	 * of the tail pointers, since we don't know if they were
-	 * updated on last use of the port.
-	 */
-	head32 = ipath_read_ureg32(dd, ur_rcvegrindextail, pd->port_port);
-	ipath_write_ureg(dd, ur_rcvegrindexhead, head32, pd->port_port);
-	pd->port_lastrcvhdrqtail = -1;
-	ipath_cdbg(VERBOSE, "Wrote port%d egrhead %x from tail regs\n",
-		pd->port_port, head32);
-	pd->port_tidcursor = 0;	/* start at beginning after open */
-
-	/* initialize poll variables... */
-	pd->port_urgent = 0;
-	pd->port_urgent_poll = 0;
-	pd->port_hdrqfull_poll = pd->port_hdrqfull;
-
-	/*
-	 * Now enable the port for receive.
-	 * For chips that are set to DMA the tail register to memory
-	 * when they change (and when the update bit transitions from
-	 * 0 to 1.  So for those chips, we turn it off and then back on.
-	 * This will (very briefly) affect any other open ports, but the
-	 * duration is very short, and therefore isn't an issue.  We
-	 * explicitly set the in-memory tail copy to 0 beforehand, so we
-	 * don't have to wait to be sure the DMA update has happened
-	 * (chip resets head/tail to 0 on transition to enable).
-	 */
-	set_bit(dd->ipath_r_portenable_shift + pd->port_port,
-		&dd->ipath_rcvctrl);
-	if (!(dd->ipath_flags & IPATH_NODMA_RTAIL)) {
-		if (pd->port_rcvhdrtail_kvaddr)
-			ipath_clear_rcvhdrtail(pd);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			dd->ipath_rcvctrl &
-			~(1ULL << dd->ipath_r_tailupd_shift));
-	}
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			 dd->ipath_rcvctrl);
-	/* Notify any waiting slaves */
-	if (pd->port_subport_cnt) {
-		clear_bit(IPATH_PORT_MASTER_UNINIT, &pd->port_flag);
-		wake_up(&pd->port_wait);
-	}
-done:
-	return ret;
-}
-
-/**
- * unlock_exptid - unlock any expected TID entries port still had in use
- * @pd: port
- *
- * We don't actually update the chip here, because we do a bulk update
- * below, using ipath_f_clear_tids.
- */
-static void unlock_expected_tids(struct ipath_portdata *pd)
-{
-	struct ipath_devdata *dd = pd->port_dd;
-	int port_tidbase = pd->port_port * dd->ipath_rcvtidcnt;
-	int i, cnt = 0, maxtid = port_tidbase + dd->ipath_rcvtidcnt;
-
-	ipath_cdbg(VERBOSE, "Port %u unlocking any locked expTID pages\n",
-		   pd->port_port);
-	for (i = port_tidbase; i < maxtid; i++) {
-		struct page *ps = dd->ipath_pageshadow[i];
-
-		if (!ps)
-			continue;
-
-		dd->ipath_pageshadow[i] = NULL;
-		pci_unmap_page(dd->pcidev, dd->ipath_physshadow[i],
-			PAGE_SIZE, PCI_DMA_FROMDEVICE);
-		ipath_release_user_pages_on_close(&ps, 1);
-		cnt++;
-		ipath_stats.sps_pageunlocks++;
-	}
-	if (cnt)
-		ipath_cdbg(VERBOSE, "Port %u locked %u expTID entries\n",
-			   pd->port_port, cnt);
-
-	if (ipath_stats.sps_pagelocks || ipath_stats.sps_pageunlocks)
-		ipath_cdbg(VERBOSE, "%llu pages locked, %llu unlocked\n",
-			   (unsigned long long) ipath_stats.sps_pagelocks,
-			   (unsigned long long)
-			   ipath_stats.sps_pageunlocks);
-}
-
-static int ipath_close(struct inode *in, struct file *fp)
-{
-	struct ipath_filedata *fd;
-	struct ipath_portdata *pd;
-	struct ipath_devdata *dd;
-	unsigned long flags;
-	unsigned port;
-	struct pid *pid;
-
-	ipath_cdbg(VERBOSE, "close on dev %lx, private data %p\n",
-		   (long)in->i_rdev, fp->private_data);
-
-	mutex_lock(&ipath_mutex);
-
-	fd = fp->private_data;
-	fp->private_data = NULL;
-	pd = fd->pd;
-	if (!pd) {
-		mutex_unlock(&ipath_mutex);
-		goto bail;
-	}
-
-	dd = pd->port_dd;
-
-	/* drain user sdma queue */
-	ipath_user_sdma_queue_drain(dd, fd->pq);
-	ipath_user_sdma_queue_destroy(fd->pq);
-
-	if (--pd->port_cnt) {
-		/*
-		 * XXX If the master closes the port before the slave(s),
-		 * revoke the mmap for the eager receive queue so
-		 * the slave(s) don't wait for receive data forever.
-		 */
-		pd->active_slaves &= ~(1 << fd->subport);
-		put_pid(pd->port_subpid[fd->subport]);
-		pd->port_subpid[fd->subport] = NULL;
-		mutex_unlock(&ipath_mutex);
-		goto bail;
-	}
-	/* early; no interrupt users after this */
-	spin_lock_irqsave(&dd->ipath_uctxt_lock, flags);
-	port = pd->port_port;
-	dd->ipath_pd[port] = NULL;
-	pid = pd->port_pid;
-	pd->port_pid = NULL;
-	spin_unlock_irqrestore(&dd->ipath_uctxt_lock, flags);
-
-	if (pd->port_rcvwait_to || pd->port_piowait_to
-	    || pd->port_rcvnowait || pd->port_pionowait) {
-		ipath_cdbg(VERBOSE, "port%u, %u rcv, %u pio wait timeo; "
-			   "%u rcv %u, pio already\n",
-			   pd->port_port, pd->port_rcvwait_to,
-			   pd->port_piowait_to, pd->port_rcvnowait,
-			   pd->port_pionowait);
-		pd->port_rcvwait_to = pd->port_piowait_to =
-			pd->port_rcvnowait = pd->port_pionowait = 0;
-	}
-	if (pd->port_flag) {
-		ipath_cdbg(PROC, "port %u port_flag set: 0x%lx\n",
-			  pd->port_port, pd->port_flag);
-		pd->port_flag = 0;
-	}
-
-	if (dd->ipath_kregbase) {
-		/* atomically clear receive enable port and intr avail. */
-		clear_bit(dd->ipath_r_portenable_shift + port,
-			  &dd->ipath_rcvctrl);
-		clear_bit(pd->port_port + dd->ipath_r_intravail_shift,
-			  &dd->ipath_rcvctrl);
-		ipath_write_kreg( dd, dd->ipath_kregs->kr_rcvctrl,
-			dd->ipath_rcvctrl);
-		/* and read back from chip to be sure that nothing
-		 * else is in flight when we do the rest */
-		(void)ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-
-		/* clean up the pkeys for this port user */
-		ipath_clean_part_key(pd, dd);
-		/*
-		 * be paranoid, and never write 0's to these, just use an
-		 * unused part of the port 0 tail page.  Of course,
-		 * rcvhdraddr points to a large chunk of memory, so this
-		 * could still trash things, but at least it won't trash
-		 * page 0, and by disabling the port, it should stop "soon",
-		 * even if a packet or two is in already in flight after we
-		 * disabled the port.
-		 */
-		ipath_write_kreg_port(dd,
-		        dd->ipath_kregs->kr_rcvhdrtailaddr, port,
-			dd->ipath_dummy_hdrq_phys);
-		ipath_write_kreg_port(dd, dd->ipath_kregs->kr_rcvhdraddr,
-			pd->port_port, dd->ipath_dummy_hdrq_phys);
-
-		ipath_disarm_piobufs(dd, pd->port_pio_base, pd->port_piocnt);
-		ipath_chg_pioavailkernel(dd, pd->port_pio_base,
-			pd->port_piocnt, 1);
-
-		dd->ipath_f_clear_tids(dd, pd->port_port);
-
-		if (dd->ipath_pageshadow)
-			unlock_expected_tids(pd);
-		ipath_stats.sps_ports--;
-		ipath_cdbg(PROC, "%s[%u] closed port %u:%u\n",
-			   pd->port_comm, pid_nr(pid),
-			   dd->ipath_unit, port);
-	}
-
-	put_pid(pid);
-	mutex_unlock(&ipath_mutex);
-	ipath_free_pddata(dd, pd); /* after releasing the mutex */
-
-bail:
-	kfree(fd);
-	return 0;
-}
-
-static int ipath_port_info(struct ipath_portdata *pd, u16 subport,
-			   struct ipath_port_info __user *uinfo)
-{
-	struct ipath_port_info info;
-	int nup;
-	int ret;
-	size_t sz;
-
-	(void) ipath_count_units(NULL, &nup, NULL);
-	info.num_active = nup;
-	info.unit = pd->port_dd->ipath_unit;
-	info.port = pd->port_port;
-	info.subport = subport;
-	/* Don't return new fields if old library opened the port. */
-	if (ipath_supports_subports(pd->userversion >> 16,
-				    pd->userversion & 0xffff)) {
-		/* Number of user ports available for this device. */
-		info.num_ports = pd->port_dd->ipath_cfgports - 1;
-		info.num_subports = pd->port_subport_cnt;
-		sz = sizeof(info);
-	} else
-		sz = sizeof(info) - 2 * sizeof(u16);
-
-	if (copy_to_user(uinfo, &info, sz)) {
-		ret = -EFAULT;
-		goto bail;
-	}
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-static int ipath_get_slave_info(struct ipath_portdata *pd,
-				void __user *slave_mask_addr)
-{
-	int ret = 0;
-
-	if (copy_to_user(slave_mask_addr, &pd->active_slaves, sizeof(u32)))
-		ret = -EFAULT;
-	return ret;
-}
-
-static int ipath_sdma_get_inflight(struct ipath_user_sdma_queue *pq,
-				   u32 __user *inflightp)
-{
-	const u32 val = ipath_user_sdma_inflight_counter(pq);
-
-	if (put_user(val, inflightp))
-		return -EFAULT;
-
-	return 0;
-}
-
-static int ipath_sdma_get_complete(struct ipath_devdata *dd,
-				   struct ipath_user_sdma_queue *pq,
-				   u32 __user *completep)
-{
-	u32 val;
-	int err;
-
-	err = ipath_user_sdma_make_progress(dd, pq);
-	if (err < 0)
-		return err;
-
-	val = ipath_user_sdma_complete_counter(pq);
-	if (put_user(val, completep))
-		return -EFAULT;
-
-	return 0;
-}
-
-static ssize_t ipath_write(struct file *fp, const char __user *data,
-			   size_t count, loff_t *off)
-{
-	const struct ipath_cmd __user *ucmd;
-	struct ipath_portdata *pd;
-	const void __user *src;
-	size_t consumed, copy;
-	struct ipath_cmd cmd;
-	ssize_t ret = 0;
-	void *dest;
-
-	if (count < sizeof(cmd.type)) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	ucmd = (const struct ipath_cmd __user *) data;
-
-	if (copy_from_user(&cmd.type, &ucmd->type, sizeof(cmd.type))) {
-		ret = -EFAULT;
-		goto bail;
-	}
-
-	consumed = sizeof(cmd.type);
-
-	switch (cmd.type) {
-	case IPATH_CMD_ASSIGN_PORT:
-	case __IPATH_CMD_USER_INIT:
-	case IPATH_CMD_USER_INIT:
-		copy = sizeof(cmd.cmd.user_info);
-		dest = &cmd.cmd.user_info;
-		src = &ucmd->cmd.user_info;
-		break;
-	case IPATH_CMD_RECV_CTRL:
-		copy = sizeof(cmd.cmd.recv_ctrl);
-		dest = &cmd.cmd.recv_ctrl;
-		src = &ucmd->cmd.recv_ctrl;
-		break;
-	case IPATH_CMD_PORT_INFO:
-		copy = sizeof(cmd.cmd.port_info);
-		dest = &cmd.cmd.port_info;
-		src = &ucmd->cmd.port_info;
-		break;
-	case IPATH_CMD_TID_UPDATE:
-	case IPATH_CMD_TID_FREE:
-		copy = sizeof(cmd.cmd.tid_info);
-		dest = &cmd.cmd.tid_info;
-		src = &ucmd->cmd.tid_info;
-		break;
-	case IPATH_CMD_SET_PART_KEY:
-		copy = sizeof(cmd.cmd.part_key);
-		dest = &cmd.cmd.part_key;
-		src = &ucmd->cmd.part_key;
-		break;
-	case __IPATH_CMD_SLAVE_INFO:
-		copy = sizeof(cmd.cmd.slave_mask_addr);
-		dest = &cmd.cmd.slave_mask_addr;
-		src = &ucmd->cmd.slave_mask_addr;
-		break;
-	case IPATH_CMD_PIOAVAILUPD:	// force an update of PIOAvail reg
-		copy = 0;
-		src = NULL;
-		dest = NULL;
-		break;
-	case IPATH_CMD_POLL_TYPE:
-		copy = sizeof(cmd.cmd.poll_type);
-		dest = &cmd.cmd.poll_type;
-		src = &ucmd->cmd.poll_type;
-		break;
-	case IPATH_CMD_ARMLAUNCH_CTRL:
-		copy = sizeof(cmd.cmd.armlaunch_ctrl);
-		dest = &cmd.cmd.armlaunch_ctrl;
-		src = &ucmd->cmd.armlaunch_ctrl;
-		break;
-	case IPATH_CMD_SDMA_INFLIGHT:
-		copy = sizeof(cmd.cmd.sdma_inflight);
-		dest = &cmd.cmd.sdma_inflight;
-		src = &ucmd->cmd.sdma_inflight;
-		break;
-	case IPATH_CMD_SDMA_COMPLETE:
-		copy = sizeof(cmd.cmd.sdma_complete);
-		dest = &cmd.cmd.sdma_complete;
-		src = &ucmd->cmd.sdma_complete;
-		break;
-	default:
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	if (copy) {
-		if ((count - consumed) < copy) {
-			ret = -EINVAL;
-			goto bail;
-		}
-
-		if (copy_from_user(dest, src, copy)) {
-			ret = -EFAULT;
-			goto bail;
-		}
-
-		consumed += copy;
-	}
-
-	pd = port_fp(fp);
-	if (!pd && cmd.type != __IPATH_CMD_USER_INIT &&
-		cmd.type != IPATH_CMD_ASSIGN_PORT) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	switch (cmd.type) {
-	case IPATH_CMD_ASSIGN_PORT:
-		ret = ipath_assign_port(fp, &cmd.cmd.user_info);
-		if (ret)
-			goto bail;
-		break;
-	case __IPATH_CMD_USER_INIT:
-		/* backwards compatibility, get port first */
-		ret = ipath_assign_port(fp, &cmd.cmd.user_info);
-		if (ret)
-			goto bail;
-		/* and fall through to current version. */
-	case IPATH_CMD_USER_INIT:
-		ret = ipath_do_user_init(fp, &cmd.cmd.user_info);
-		if (ret)
-			goto bail;
-		ret = ipath_get_base_info(
-			fp, (void __user *) (unsigned long)
-			cmd.cmd.user_info.spu_base_info,
-			cmd.cmd.user_info.spu_base_info_size);
-		break;
-	case IPATH_CMD_RECV_CTRL:
-		ret = ipath_manage_rcvq(pd, subport_fp(fp), cmd.cmd.recv_ctrl);
-		break;
-	case IPATH_CMD_PORT_INFO:
-		ret = ipath_port_info(pd, subport_fp(fp),
-				      (struct ipath_port_info __user *)
-				      (unsigned long) cmd.cmd.port_info);
-		break;
-	case IPATH_CMD_TID_UPDATE:
-		ret = ipath_tid_update(pd, fp, &cmd.cmd.tid_info);
-		break;
-	case IPATH_CMD_TID_FREE:
-		ret = ipath_tid_free(pd, subport_fp(fp), &cmd.cmd.tid_info);
-		break;
-	case IPATH_CMD_SET_PART_KEY:
-		ret = ipath_set_part_key(pd, cmd.cmd.part_key);
-		break;
-	case __IPATH_CMD_SLAVE_INFO:
-		ret = ipath_get_slave_info(pd,
-					   (void __user *) (unsigned long)
-					   cmd.cmd.slave_mask_addr);
-		break;
-	case IPATH_CMD_PIOAVAILUPD:
-		ipath_force_pio_avail_update(pd->port_dd);
-		break;
-	case IPATH_CMD_POLL_TYPE:
-		pd->poll_type = cmd.cmd.poll_type;
-		break;
-	case IPATH_CMD_ARMLAUNCH_CTRL:
-		if (cmd.cmd.armlaunch_ctrl)
-			ipath_enable_armlaunch(pd->port_dd);
-		else
-			ipath_disable_armlaunch(pd->port_dd);
-		break;
-	case IPATH_CMD_SDMA_INFLIGHT:
-		ret = ipath_sdma_get_inflight(user_sdma_queue_fp(fp),
-					      (u32 __user *) (unsigned long)
-					      cmd.cmd.sdma_inflight);
-		break;
-	case IPATH_CMD_SDMA_COMPLETE:
-		ret = ipath_sdma_get_complete(pd->port_dd,
-					      user_sdma_queue_fp(fp),
-					      (u32 __user *) (unsigned long)
-					      cmd.cmd.sdma_complete);
-		break;
-	}
-
-	if (ret >= 0)
-		ret = consumed;
-
-bail:
-	return ret;
-}
-
-static ssize_t ipath_write_iter(struct kiocb *iocb, struct iov_iter *from)
-{
-	struct file *filp = iocb->ki_filp;
-	struct ipath_filedata *fp = filp->private_data;
-	struct ipath_portdata *pd = port_fp(filp);
-	struct ipath_user_sdma_queue *pq = fp->pq;
-
-	if (!iter_is_iovec(from) || !from->nr_segs)
-		return -EINVAL;
-
-	return ipath_user_sdma_writev(pd->port_dd, pq, from->iov, from->nr_segs);
-}
-
-static struct class *ipath_class;
-
-static int init_cdev(int minor, char *name, const struct file_operations *fops,
-		     struct cdev **cdevp, struct device **devp)
-{
-	const dev_t dev = MKDEV(IPATH_MAJOR, minor);
-	struct cdev *cdev = NULL;
-	struct device *device = NULL;
-	int ret;
-
-	cdev = cdev_alloc();
-	if (!cdev) {
-		printk(KERN_ERR IPATH_DRV_NAME
-		       ": Could not allocate cdev for minor %d, %s\n",
-		       minor, name);
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	cdev->owner = THIS_MODULE;
-	cdev->ops = fops;
-	kobject_set_name(&cdev->kobj, name);
-
-	ret = cdev_add(cdev, dev, 1);
-	if (ret < 0) {
-		printk(KERN_ERR IPATH_DRV_NAME
-		       ": Could not add cdev for minor %d, %s (err %d)\n",
-		       minor, name, -ret);
-		goto err_cdev;
-	}
-
-	device = device_create(ipath_class, NULL, dev, NULL, name);
-
-	if (IS_ERR(device)) {
-		ret = PTR_ERR(device);
-		printk(KERN_ERR IPATH_DRV_NAME ": Could not create "
-		       "device for minor %d, %s (err %d)\n",
-		       minor, name, -ret);
-		goto err_cdev;
-	}
-
-	goto done;
-
-err_cdev:
-	cdev_del(cdev);
-	cdev = NULL;
-
-done:
-	if (ret >= 0) {
-		*cdevp = cdev;
-		*devp = device;
-	} else {
-		*cdevp = NULL;
-		*devp = NULL;
-	}
-
-	return ret;
-}
-
-int ipath_cdev_init(int minor, char *name, const struct file_operations *fops,
-		    struct cdev **cdevp, struct device **devp)
-{
-	return init_cdev(minor, name, fops, cdevp, devp);
-}
-
-static void cleanup_cdev(struct cdev **cdevp,
-			 struct device **devp)
-{
-	struct device *dev = *devp;
-
-	if (dev) {
-		device_unregister(dev);
-		*devp = NULL;
-	}
-
-	if (*cdevp) {
-		cdev_del(*cdevp);
-		*cdevp = NULL;
-	}
-}
-
-void ipath_cdev_cleanup(struct cdev **cdevp,
-			struct device **devp)
-{
-	cleanup_cdev(cdevp, devp);
-}
-
-static struct cdev *wildcard_cdev;
-static struct device *wildcard_dev;
-
-static const dev_t dev = MKDEV(IPATH_MAJOR, 0);
-
-static int user_init(void)
-{
-	int ret;
-
-	ret = register_chrdev_region(dev, IPATH_NMINORS, IPATH_DRV_NAME);
-	if (ret < 0) {
-		printk(KERN_ERR IPATH_DRV_NAME ": Could not register "
-		       "chrdev region (err %d)\n", -ret);
-		goto done;
-	}
-
-	ipath_class = class_create(THIS_MODULE, IPATH_DRV_NAME);
-
-	if (IS_ERR(ipath_class)) {
-		ret = PTR_ERR(ipath_class);
-		printk(KERN_ERR IPATH_DRV_NAME ": Could not create "
-		       "device class (err %d)\n", -ret);
-		goto bail;
-	}
-
-	goto done;
-bail:
-	unregister_chrdev_region(dev, IPATH_NMINORS);
-done:
-	return ret;
-}
-
-static void user_cleanup(void)
-{
-	if (ipath_class) {
-		class_destroy(ipath_class);
-		ipath_class = NULL;
-	}
-
-	unregister_chrdev_region(dev, IPATH_NMINORS);
-}
-
-static atomic_t user_count = ATOMIC_INIT(0);
-static atomic_t user_setup = ATOMIC_INIT(0);
-
-int ipath_user_add(struct ipath_devdata *dd)
-{
-	char name[10];
-	int ret;
-
-	if (atomic_inc_return(&user_count) == 1) {
-		ret = user_init();
-		if (ret < 0) {
-			ipath_dev_err(dd, "Unable to set up user support: "
-				      "error %d\n", -ret);
-			goto bail;
-		}
-		ret = init_cdev(0, "ipath", &ipath_file_ops, &wildcard_cdev,
-				&wildcard_dev);
-		if (ret < 0) {
-			ipath_dev_err(dd, "Could not create wildcard "
-				      "minor: error %d\n", -ret);
-			goto bail_user;
-		}
-
-		atomic_set(&user_setup, 1);
-	}
-
-	snprintf(name, sizeof(name), "ipath%d", dd->ipath_unit);
-
-	ret = init_cdev(dd->ipath_unit + 1, name, &ipath_file_ops,
-			&dd->user_cdev, &dd->user_dev);
-	if (ret < 0)
-		ipath_dev_err(dd, "Could not create user minor %d, %s\n",
-			      dd->ipath_unit + 1, name);
-
-	goto bail;
-
-bail_user:
-	user_cleanup();
-bail:
-	return ret;
-}
-
-void ipath_user_remove(struct ipath_devdata *dd)
-{
-	cleanup_cdev(&dd->user_cdev, &dd->user_dev);
-
-	if (atomic_dec_return(&user_count) == 0) {
-		if (atomic_read(&user_setup) == 0)
-			goto bail;
-
-		cleanup_cdev(&wildcard_cdev, &wildcard_dev);
-		user_cleanup();
-
-		atomic_set(&user_setup, 0);
-	}
-bail:
-	return;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_fs.c b/drivers/staging/rdma/ipath/ipath_fs.c
deleted file mode 100644
index 796af6867007..000000000000
--- a/drivers/staging/rdma/ipath/ipath_fs.c
+++ /dev/null
@@ -1,415 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2006 PathScale, 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/fs.h>
-#include <linux/mount.h>
-#include <linux/pagemap.h>
-#include <linux/init.h>
-#include <linux/namei.h>
-#include <linux/slab.h>
-
-#include "ipath_kernel.h"
-
-#define IPATHFS_MAGIC 0x726a77
-
-static struct super_block *ipath_super;
-
-static int ipathfs_mknod(struct inode *dir, struct dentry *dentry,
-			 umode_t mode, const struct file_operations *fops,
-			 void *data)
-{
-	int error;
-	struct inode *inode = new_inode(dir->i_sb);
-
-	if (!inode) {
-		error = -EPERM;
-		goto bail;
-	}
-
-	inode->i_ino = get_next_ino();
-	inode->i_mode = mode;
-	inode->i_atime = inode->i_mtime = inode->i_ctime = CURRENT_TIME;
-	inode->i_private = data;
-	if (S_ISDIR(mode)) {
-		inode->i_op = &simple_dir_inode_operations;
-		inc_nlink(inode);
-		inc_nlink(dir);
-	}
-
-	inode->i_fop = fops;
-
-	d_instantiate(dentry, inode);
-	error = 0;
-
-bail:
-	return error;
-}
-
-static int create_file(const char *name, umode_t mode,
-		       struct dentry *parent, struct dentry **dentry,
-		       const struct file_operations *fops, void *data)
-{
-	int error;
-
-	mutex_lock(&d_inode(parent)->i_mutex);
-	*dentry = lookup_one_len(name, parent, strlen(name));
-	if (!IS_ERR(*dentry))
-		error = ipathfs_mknod(d_inode(parent), *dentry,
-				      mode, fops, data);
-	else
-		error = PTR_ERR(*dentry);
-	mutex_unlock(&d_inode(parent)->i_mutex);
-
-	return error;
-}
-
-static ssize_t atomic_stats_read(struct file *file, char __user *buf,
-				 size_t count, loff_t *ppos)
-{
-	return simple_read_from_buffer(buf, count, ppos, &ipath_stats,
-				       sizeof ipath_stats);
-}
-
-static const struct file_operations atomic_stats_ops = {
-	.read = atomic_stats_read,
-	.llseek = default_llseek,
-};
-
-static ssize_t atomic_counters_read(struct file *file, char __user *buf,
-				    size_t count, loff_t *ppos)
-{
-	struct infinipath_counters counters;
-	struct ipath_devdata *dd;
-
-	dd = file_inode(file)->i_private;
-	dd->ipath_f_read_counters(dd, &counters);
-
-	return simple_read_from_buffer(buf, count, ppos, &counters,
-				       sizeof counters);
-}
-
-static const struct file_operations atomic_counters_ops = {
-	.read = atomic_counters_read,
-	.llseek = default_llseek,
-};
-
-static ssize_t flash_read(struct file *file, char __user *buf,
-			  size_t count, loff_t *ppos)
-{
-	struct ipath_devdata *dd;
-	ssize_t ret;
-	loff_t pos;
-	char *tmp;
-
-	pos = *ppos;
-
-	if ( pos < 0) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	if (pos >= sizeof(struct ipath_flash)) {
-		ret = 0;
-		goto bail;
-	}
-
-	if (count > sizeof(struct ipath_flash) - pos)
-		count = sizeof(struct ipath_flash) - pos;
-
-	tmp = kmalloc(count, GFP_KERNEL);
-	if (!tmp) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	dd = file_inode(file)->i_private;
-	if (ipath_eeprom_read(dd, pos, tmp, count)) {
-		ipath_dev_err(dd, "failed to read from flash\n");
-		ret = -ENXIO;
-		goto bail_tmp;
-	}
-
-	if (copy_to_user(buf, tmp, count)) {
-		ret = -EFAULT;
-		goto bail_tmp;
-	}
-
-	*ppos = pos + count;
-	ret = count;
-
-bail_tmp:
-	kfree(tmp);
-
-bail:
-	return ret;
-}
-
-static ssize_t flash_write(struct file *file, const char __user *buf,
-			   size_t count, loff_t *ppos)
-{
-	struct ipath_devdata *dd;
-	ssize_t ret;
-	loff_t pos;
-	char *tmp;
-
-	pos = *ppos;
-
-	if (pos != 0) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	if (count != sizeof(struct ipath_flash)) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	tmp = memdup_user(buf, count);
-	if (IS_ERR(tmp))
-		return PTR_ERR(tmp);
-
-	dd = file_inode(file)->i_private;
-	if (ipath_eeprom_write(dd, pos, tmp, count)) {
-		ret = -ENXIO;
-		ipath_dev_err(dd, "failed to write to flash\n");
-		goto bail_tmp;
-	}
-
-	*ppos = pos + count;
-	ret = count;
-
-bail_tmp:
-	kfree(tmp);
-
-bail:
-	return ret;
-}
-
-static const struct file_operations flash_ops = {
-	.read = flash_read,
-	.write = flash_write,
-	.llseek = default_llseek,
-};
-
-static int create_device_files(struct super_block *sb,
-			       struct ipath_devdata *dd)
-{
-	struct dentry *dir, *tmp;
-	char unit[10];
-	int ret;
-
-	snprintf(unit, sizeof unit, "%02d", dd->ipath_unit);
-	ret = create_file(unit, S_IFDIR|S_IRUGO|S_IXUGO, sb->s_root, &dir,
-			  &simple_dir_operations, dd);
-	if (ret) {
-		printk(KERN_ERR "create_file(%s) failed: %d\n", unit, ret);
-		goto bail;
-	}
-
-	ret = create_file("atomic_counters", S_IFREG|S_IRUGO, dir, &tmp,
-			  &atomic_counters_ops, dd);
-	if (ret) {
-		printk(KERN_ERR "create_file(%s/atomic_counters) "
-		       "failed: %d\n", unit, ret);
-		goto bail;
-	}
-
-	ret = create_file("flash", S_IFREG|S_IWUSR|S_IRUGO, dir, &tmp,
-			  &flash_ops, dd);
-	if (ret) {
-		printk(KERN_ERR "create_file(%s/flash) "
-		       "failed: %d\n", unit, ret);
-		goto bail;
-	}
-
-bail:
-	return ret;
-}
-
-static int remove_file(struct dentry *parent, char *name)
-{
-	struct dentry *tmp;
-	int ret;
-
-	tmp = lookup_one_len(name, parent, strlen(name));
-
-	if (IS_ERR(tmp)) {
-		ret = PTR_ERR(tmp);
-		goto bail;
-	}
-
-	spin_lock(&tmp->d_lock);
-	if (simple_positive(tmp)) {
-		dget_dlock(tmp);
-		__d_drop(tmp);
-		spin_unlock(&tmp->d_lock);
-		simple_unlink(d_inode(parent), tmp);
-	} else
-		spin_unlock(&tmp->d_lock);
-
-	ret = 0;
-bail:
-	/*
-	 * We don't expect clients to care about the return value, but
-	 * it's there if they need it.
-	 */
-	return ret;
-}
-
-static int remove_device_files(struct super_block *sb,
-			       struct ipath_devdata *dd)
-{
-	struct dentry *dir, *root;
-	char unit[10];
-	int ret;
-
-	root = dget(sb->s_root);
-	mutex_lock(&d_inode(root)->i_mutex);
-	snprintf(unit, sizeof unit, "%02d", dd->ipath_unit);
-	dir = lookup_one_len(unit, root, strlen(unit));
-
-	if (IS_ERR(dir)) {
-		ret = PTR_ERR(dir);
-		printk(KERN_ERR "Lookup of %s failed\n", unit);
-		goto bail;
-	}
-
-	remove_file(dir, "flash");
-	remove_file(dir, "atomic_counters");
-	d_delete(dir);
-	ret = simple_rmdir(d_inode(root), dir);
-
-bail:
-	mutex_unlock(&d_inode(root)->i_mutex);
-	dput(root);
-	return ret;
-}
-
-static int ipathfs_fill_super(struct super_block *sb, void *data,
-			      int silent)
-{
-	struct ipath_devdata *dd, *tmp;
-	unsigned long flags;
-	int ret;
-
-	static struct tree_descr files[] = {
-		[2] = {"atomic_stats", &atomic_stats_ops, S_IRUGO},
-		{""},
-	};
-
-	ret = simple_fill_super(sb, IPATHFS_MAGIC, files);
-	if (ret) {
-		printk(KERN_ERR "simple_fill_super failed: %d\n", ret);
-		goto bail;
-	}
-
-	spin_lock_irqsave(&ipath_devs_lock, flags);
-
-	list_for_each_entry_safe(dd, tmp, &ipath_dev_list, ipath_list) {
-		spin_unlock_irqrestore(&ipath_devs_lock, flags);
-		ret = create_device_files(sb, dd);
-		if (ret)
-			goto bail;
-		spin_lock_irqsave(&ipath_devs_lock, flags);
-	}
-
-	spin_unlock_irqrestore(&ipath_devs_lock, flags);
-
-bail:
-	return ret;
-}
-
-static struct dentry *ipathfs_mount(struct file_system_type *fs_type,
-			int flags, const char *dev_name, void *data)
-{
-	struct dentry *ret;
-	ret = mount_single(fs_type, flags, data, ipathfs_fill_super);
-	if (!IS_ERR(ret))
-		ipath_super = ret->d_sb;
-	return ret;
-}
-
-static void ipathfs_kill_super(struct super_block *s)
-{
-	kill_litter_super(s);
-	ipath_super = NULL;
-}
-
-int ipathfs_add_device(struct ipath_devdata *dd)
-{
-	int ret;
-
-	if (ipath_super == NULL) {
-		ret = 0;
-		goto bail;
-	}
-
-	ret = create_device_files(ipath_super, dd);
-
-bail:
-	return ret;
-}
-
-int ipathfs_remove_device(struct ipath_devdata *dd)
-{
-	int ret;
-
-	if (ipath_super == NULL) {
-		ret = 0;
-		goto bail;
-	}
-
-	ret = remove_device_files(ipath_super, dd);
-
-bail:
-	return ret;
-}
-
-static struct file_system_type ipathfs_fs_type = {
-	.owner =	THIS_MODULE,
-	.name =		"ipathfs",
-	.mount =	ipathfs_mount,
-	.kill_sb =	ipathfs_kill_super,
-};
-MODULE_ALIAS_FS("ipathfs");
-
-int __init ipath_init_ipathfs(void)
-{
-	return register_filesystem(&ipathfs_fs_type);
-}
-
-void __exit ipath_exit_ipathfs(void)
-{
-	unregister_filesystem(&ipathfs_fs_type);
-}
diff --git a/drivers/staging/rdma/ipath/ipath_iba6110.c b/drivers/staging/rdma/ipath/ipath_iba6110.c
deleted file mode 100644
index 5f13572a5e24..000000000000
--- a/drivers/staging/rdma/ipath/ipath_iba6110.c
+++ /dev/null
@@ -1,1939 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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.
- */
-
-/*
- * This file contains all of the code that is specific to the InfiniPath
- * HT chip.
- */
-
-#include <linux/vmalloc.h>
-#include <linux/pci.h>
-#include <linux/delay.h>
-#include <linux/htirq.h>
-#include <rdma/ib_verbs.h>
-
-#include "ipath_kernel.h"
-#include "ipath_registers.h"
-
-static void ipath_setup_ht_setextled(struct ipath_devdata *, u64, u64);
-
-
-/*
- * This lists the InfiniPath registers, in the actual chip layout.
- * This structure should never be directly accessed.
- *
- * The names are in InterCap form because they're taken straight from
- * the chip specification.  Since they're only used in this file, they
- * don't pollute the rest of the source.
-*/
-
-struct _infinipath_do_not_use_kernel_regs {
-	unsigned long long Revision;
-	unsigned long long Control;
-	unsigned long long PageAlign;
-	unsigned long long PortCnt;
-	unsigned long long DebugPortSelect;
-	unsigned long long DebugPort;
-	unsigned long long SendRegBase;
-	unsigned long long UserRegBase;
-	unsigned long long CounterRegBase;
-	unsigned long long Scratch;
-	unsigned long long ReservedMisc1;
-	unsigned long long InterruptConfig;
-	unsigned long long IntBlocked;
-	unsigned long long IntMask;
-	unsigned long long IntStatus;
-	unsigned long long IntClear;
-	unsigned long long ErrorMask;
-	unsigned long long ErrorStatus;
-	unsigned long long ErrorClear;
-	unsigned long long HwErrMask;
-	unsigned long long HwErrStatus;
-	unsigned long long HwErrClear;
-	unsigned long long HwDiagCtrl;
-	unsigned long long MDIO;
-	unsigned long long IBCStatus;
-	unsigned long long IBCCtrl;
-	unsigned long long ExtStatus;
-	unsigned long long ExtCtrl;
-	unsigned long long GPIOOut;
-	unsigned long long GPIOMask;
-	unsigned long long GPIOStatus;
-	unsigned long long GPIOClear;
-	unsigned long long RcvCtrl;
-	unsigned long long RcvBTHQP;
-	unsigned long long RcvHdrSize;
-	unsigned long long RcvHdrCnt;
-	unsigned long long RcvHdrEntSize;
-	unsigned long long RcvTIDBase;
-	unsigned long long RcvTIDCnt;
-	unsigned long long RcvEgrBase;
-	unsigned long long RcvEgrCnt;
-	unsigned long long RcvBufBase;
-	unsigned long long RcvBufSize;
-	unsigned long long RxIntMemBase;
-	unsigned long long RxIntMemSize;
-	unsigned long long RcvPartitionKey;
-	unsigned long long ReservedRcv[10];
-	unsigned long long SendCtrl;
-	unsigned long long SendPIOBufBase;
-	unsigned long long SendPIOSize;
-	unsigned long long SendPIOBufCnt;
-	unsigned long long SendPIOAvailAddr;
-	unsigned long long TxIntMemBase;
-	unsigned long long TxIntMemSize;
-	unsigned long long ReservedSend[9];
-	unsigned long long SendBufferError;
-	unsigned long long SendBufferErrorCONT1;
-	unsigned long long SendBufferErrorCONT2;
-	unsigned long long SendBufferErrorCONT3;
-	unsigned long long ReservedSBE[4];
-	unsigned long long RcvHdrAddr0;
-	unsigned long long RcvHdrAddr1;
-	unsigned long long RcvHdrAddr2;
-	unsigned long long RcvHdrAddr3;
-	unsigned long long RcvHdrAddr4;
-	unsigned long long RcvHdrAddr5;
-	unsigned long long RcvHdrAddr6;
-	unsigned long long RcvHdrAddr7;
-	unsigned long long RcvHdrAddr8;
-	unsigned long long ReservedRHA[7];
-	unsigned long long RcvHdrTailAddr0;
-	unsigned long long RcvHdrTailAddr1;
-	unsigned long long RcvHdrTailAddr2;
-	unsigned long long RcvHdrTailAddr3;
-	unsigned long long RcvHdrTailAddr4;
-	unsigned long long RcvHdrTailAddr5;
-	unsigned long long RcvHdrTailAddr6;
-	unsigned long long RcvHdrTailAddr7;
-	unsigned long long RcvHdrTailAddr8;
-	unsigned long long ReservedRHTA[7];
-	unsigned long long Sync;	/* Software only */
-	unsigned long long Dump;	/* Software only */
-	unsigned long long SimVer;	/* Software only */
-	unsigned long long ReservedSW[5];
-	unsigned long long SerdesConfig0;
-	unsigned long long SerdesConfig1;
-	unsigned long long SerdesStatus;
-	unsigned long long XGXSConfig;
-	unsigned long long ReservedSW2[4];
-};
-
-struct _infinipath_do_not_use_counters {
-	__u64 LBIntCnt;
-	__u64 LBFlowStallCnt;
-	__u64 Reserved1;
-	__u64 TxUnsupVLErrCnt;
-	__u64 TxDataPktCnt;
-	__u64 TxFlowPktCnt;
-	__u64 TxDwordCnt;
-	__u64 TxLenErrCnt;
-	__u64 TxMaxMinLenErrCnt;
-	__u64 TxUnderrunCnt;
-	__u64 TxFlowStallCnt;
-	__u64 TxDroppedPktCnt;
-	__u64 RxDroppedPktCnt;
-	__u64 RxDataPktCnt;
-	__u64 RxFlowPktCnt;
-	__u64 RxDwordCnt;
-	__u64 RxLenErrCnt;
-	__u64 RxMaxMinLenErrCnt;
-	__u64 RxICRCErrCnt;
-	__u64 RxVCRCErrCnt;
-	__u64 RxFlowCtrlErrCnt;
-	__u64 RxBadFormatCnt;
-	__u64 RxLinkProblemCnt;
-	__u64 RxEBPCnt;
-	__u64 RxLPCRCErrCnt;
-	__u64 RxBufOvflCnt;
-	__u64 RxTIDFullErrCnt;
-	__u64 RxTIDValidErrCnt;
-	__u64 RxPKeyMismatchCnt;
-	__u64 RxP0HdrEgrOvflCnt;
-	__u64 RxP1HdrEgrOvflCnt;
-	__u64 RxP2HdrEgrOvflCnt;
-	__u64 RxP3HdrEgrOvflCnt;
-	__u64 RxP4HdrEgrOvflCnt;
-	__u64 RxP5HdrEgrOvflCnt;
-	__u64 RxP6HdrEgrOvflCnt;
-	__u64 RxP7HdrEgrOvflCnt;
-	__u64 RxP8HdrEgrOvflCnt;
-	__u64 Reserved6;
-	__u64 Reserved7;
-	__u64 IBStatusChangeCnt;
-	__u64 IBLinkErrRecoveryCnt;
-	__u64 IBLinkDownedCnt;
-	__u64 IBSymbolErrCnt;
-};
-
-#define IPATH_KREG_OFFSET(field) (offsetof( \
-	struct _infinipath_do_not_use_kernel_regs, field) / sizeof(u64))
-#define IPATH_CREG_OFFSET(field) (offsetof( \
-	struct _infinipath_do_not_use_counters, field) / sizeof(u64))
-
-static const struct ipath_kregs ipath_ht_kregs = {
-	.kr_control = IPATH_KREG_OFFSET(Control),
-	.kr_counterregbase = IPATH_KREG_OFFSET(CounterRegBase),
-	.kr_debugport = IPATH_KREG_OFFSET(DebugPort),
-	.kr_debugportselect = IPATH_KREG_OFFSET(DebugPortSelect),
-	.kr_errorclear = IPATH_KREG_OFFSET(ErrorClear),
-	.kr_errormask = IPATH_KREG_OFFSET(ErrorMask),
-	.kr_errorstatus = IPATH_KREG_OFFSET(ErrorStatus),
-	.kr_extctrl = IPATH_KREG_OFFSET(ExtCtrl),
-	.kr_extstatus = IPATH_KREG_OFFSET(ExtStatus),
-	.kr_gpio_clear = IPATH_KREG_OFFSET(GPIOClear),
-	.kr_gpio_mask = IPATH_KREG_OFFSET(GPIOMask),
-	.kr_gpio_out = IPATH_KREG_OFFSET(GPIOOut),
-	.kr_gpio_status = IPATH_KREG_OFFSET(GPIOStatus),
-	.kr_hwdiagctrl = IPATH_KREG_OFFSET(HwDiagCtrl),
-	.kr_hwerrclear = IPATH_KREG_OFFSET(HwErrClear),
-	.kr_hwerrmask = IPATH_KREG_OFFSET(HwErrMask),
-	.kr_hwerrstatus = IPATH_KREG_OFFSET(HwErrStatus),
-	.kr_ibcctrl = IPATH_KREG_OFFSET(IBCCtrl),
-	.kr_ibcstatus = IPATH_KREG_OFFSET(IBCStatus),
-	.kr_intblocked = IPATH_KREG_OFFSET(IntBlocked),
-	.kr_intclear = IPATH_KREG_OFFSET(IntClear),
-	.kr_interruptconfig = IPATH_KREG_OFFSET(InterruptConfig),
-	.kr_intmask = IPATH_KREG_OFFSET(IntMask),
-	.kr_intstatus = IPATH_KREG_OFFSET(IntStatus),
-	.kr_mdio = IPATH_KREG_OFFSET(MDIO),
-	.kr_pagealign = IPATH_KREG_OFFSET(PageAlign),
-	.kr_partitionkey = IPATH_KREG_OFFSET(RcvPartitionKey),
-	.kr_portcnt = IPATH_KREG_OFFSET(PortCnt),
-	.kr_rcvbthqp = IPATH_KREG_OFFSET(RcvBTHQP),
-	.kr_rcvbufbase = IPATH_KREG_OFFSET(RcvBufBase),
-	.kr_rcvbufsize = IPATH_KREG_OFFSET(RcvBufSize),
-	.kr_rcvctrl = IPATH_KREG_OFFSET(RcvCtrl),
-	.kr_rcvegrbase = IPATH_KREG_OFFSET(RcvEgrBase),
-	.kr_rcvegrcnt = IPATH_KREG_OFFSET(RcvEgrCnt),
-	.kr_rcvhdrcnt = IPATH_KREG_OFFSET(RcvHdrCnt),
-	.kr_rcvhdrentsize = IPATH_KREG_OFFSET(RcvHdrEntSize),
-	.kr_rcvhdrsize = IPATH_KREG_OFFSET(RcvHdrSize),
-	.kr_rcvintmembase = IPATH_KREG_OFFSET(RxIntMemBase),
-	.kr_rcvintmemsize = IPATH_KREG_OFFSET(RxIntMemSize),
-	.kr_rcvtidbase = IPATH_KREG_OFFSET(RcvTIDBase),
-	.kr_rcvtidcnt = IPATH_KREG_OFFSET(RcvTIDCnt),
-	.kr_revision = IPATH_KREG_OFFSET(Revision),
-	.kr_scratch = IPATH_KREG_OFFSET(Scratch),
-	.kr_sendbuffererror = IPATH_KREG_OFFSET(SendBufferError),
-	.kr_sendctrl = IPATH_KREG_OFFSET(SendCtrl),
-	.kr_sendpioavailaddr = IPATH_KREG_OFFSET(SendPIOAvailAddr),
-	.kr_sendpiobufbase = IPATH_KREG_OFFSET(SendPIOBufBase),
-	.kr_sendpiobufcnt = IPATH_KREG_OFFSET(SendPIOBufCnt),
-	.kr_sendpiosize = IPATH_KREG_OFFSET(SendPIOSize),
-	.kr_sendregbase = IPATH_KREG_OFFSET(SendRegBase),
-	.kr_txintmembase = IPATH_KREG_OFFSET(TxIntMemBase),
-	.kr_txintmemsize = IPATH_KREG_OFFSET(TxIntMemSize),
-	.kr_userregbase = IPATH_KREG_OFFSET(UserRegBase),
-	.kr_serdesconfig0 = IPATH_KREG_OFFSET(SerdesConfig0),
-	.kr_serdesconfig1 = IPATH_KREG_OFFSET(SerdesConfig1),
-	.kr_serdesstatus = IPATH_KREG_OFFSET(SerdesStatus),
-	.kr_xgxsconfig = IPATH_KREG_OFFSET(XGXSConfig),
-	/*
-	 * These should not be used directly via ipath_write_kreg64(),
-	 * use them with ipath_write_kreg64_port(),
-	 */
-	.kr_rcvhdraddr = IPATH_KREG_OFFSET(RcvHdrAddr0),
-	.kr_rcvhdrtailaddr = IPATH_KREG_OFFSET(RcvHdrTailAddr0)
-};
-
-static const struct ipath_cregs ipath_ht_cregs = {
-	.cr_badformatcnt = IPATH_CREG_OFFSET(RxBadFormatCnt),
-	.cr_erricrccnt = IPATH_CREG_OFFSET(RxICRCErrCnt),
-	.cr_errlinkcnt = IPATH_CREG_OFFSET(RxLinkProblemCnt),
-	.cr_errlpcrccnt = IPATH_CREG_OFFSET(RxLPCRCErrCnt),
-	.cr_errpkey = IPATH_CREG_OFFSET(RxPKeyMismatchCnt),
-	.cr_errrcvflowctrlcnt = IPATH_CREG_OFFSET(RxFlowCtrlErrCnt),
-	.cr_err_rlencnt = IPATH_CREG_OFFSET(RxLenErrCnt),
-	.cr_errslencnt = IPATH_CREG_OFFSET(TxLenErrCnt),
-	.cr_errtidfull = IPATH_CREG_OFFSET(RxTIDFullErrCnt),
-	.cr_errtidvalid = IPATH_CREG_OFFSET(RxTIDValidErrCnt),
-	.cr_errvcrccnt = IPATH_CREG_OFFSET(RxVCRCErrCnt),
-	.cr_ibstatuschange = IPATH_CREG_OFFSET(IBStatusChangeCnt),
-	/* calc from Reg_CounterRegBase + offset */
-	.cr_intcnt = IPATH_CREG_OFFSET(LBIntCnt),
-	.cr_invalidrlencnt = IPATH_CREG_OFFSET(RxMaxMinLenErrCnt),
-	.cr_invalidslencnt = IPATH_CREG_OFFSET(TxMaxMinLenErrCnt),
-	.cr_lbflowstallcnt = IPATH_CREG_OFFSET(LBFlowStallCnt),
-	.cr_pktrcvcnt = IPATH_CREG_OFFSET(RxDataPktCnt),
-	.cr_pktrcvflowctrlcnt = IPATH_CREG_OFFSET(RxFlowPktCnt),
-	.cr_pktsendcnt = IPATH_CREG_OFFSET(TxDataPktCnt),
-	.cr_pktsendflowcnt = IPATH_CREG_OFFSET(TxFlowPktCnt),
-	.cr_portovflcnt = IPATH_CREG_OFFSET(RxP0HdrEgrOvflCnt),
-	.cr_rcvebpcnt = IPATH_CREG_OFFSET(RxEBPCnt),
-	.cr_rcvovflcnt = IPATH_CREG_OFFSET(RxBufOvflCnt),
-	.cr_senddropped = IPATH_CREG_OFFSET(TxDroppedPktCnt),
-	.cr_sendstallcnt = IPATH_CREG_OFFSET(TxFlowStallCnt),
-	.cr_sendunderruncnt = IPATH_CREG_OFFSET(TxUnderrunCnt),
-	.cr_wordrcvcnt = IPATH_CREG_OFFSET(RxDwordCnt),
-	.cr_wordsendcnt = IPATH_CREG_OFFSET(TxDwordCnt),
-	.cr_unsupvlcnt = IPATH_CREG_OFFSET(TxUnsupVLErrCnt),
-	.cr_rxdroppktcnt = IPATH_CREG_OFFSET(RxDroppedPktCnt),
-	.cr_iblinkerrrecovcnt = IPATH_CREG_OFFSET(IBLinkErrRecoveryCnt),
-	.cr_iblinkdowncnt = IPATH_CREG_OFFSET(IBLinkDownedCnt),
-	.cr_ibsymbolerrcnt = IPATH_CREG_OFFSET(IBSymbolErrCnt)
-};
-
-/* kr_intstatus, kr_intclear, kr_intmask bits */
-#define INFINIPATH_I_RCVURG_MASK ((1U<<9)-1)
-#define INFINIPATH_I_RCVURG_SHIFT 0
-#define INFINIPATH_I_RCVAVAIL_MASK ((1U<<9)-1)
-#define INFINIPATH_I_RCVAVAIL_SHIFT 12
-
-/* kr_hwerrclear, kr_hwerrmask, kr_hwerrstatus, bits */
-#define INFINIPATH_HWE_HTCMEMPARITYERR_SHIFT 0
-#define INFINIPATH_HWE_HTCMEMPARITYERR_MASK 0x3FFFFFULL
-#define INFINIPATH_HWE_HTCLNKABYTE0CRCERR   0x0000000000800000ULL
-#define INFINIPATH_HWE_HTCLNKABYTE1CRCERR   0x0000000001000000ULL
-#define INFINIPATH_HWE_HTCLNKBBYTE0CRCERR   0x0000000002000000ULL
-#define INFINIPATH_HWE_HTCLNKBBYTE1CRCERR   0x0000000004000000ULL
-#define INFINIPATH_HWE_HTCMISCERR4          0x0000000008000000ULL
-#define INFINIPATH_HWE_HTCMISCERR5          0x0000000010000000ULL
-#define INFINIPATH_HWE_HTCMISCERR6          0x0000000020000000ULL
-#define INFINIPATH_HWE_HTCMISCERR7          0x0000000040000000ULL
-#define INFINIPATH_HWE_HTCBUSTREQPARITYERR  0x0000000080000000ULL
-#define INFINIPATH_HWE_HTCBUSTRESPPARITYERR 0x0000000100000000ULL
-#define INFINIPATH_HWE_HTCBUSIREQPARITYERR  0x0000000200000000ULL
-#define INFINIPATH_HWE_COREPLL_FBSLIP       0x0080000000000000ULL
-#define INFINIPATH_HWE_COREPLL_RFSLIP       0x0100000000000000ULL
-#define INFINIPATH_HWE_HTBPLL_FBSLIP        0x0200000000000000ULL
-#define INFINIPATH_HWE_HTBPLL_RFSLIP        0x0400000000000000ULL
-#define INFINIPATH_HWE_HTAPLL_FBSLIP        0x0800000000000000ULL
-#define INFINIPATH_HWE_HTAPLL_RFSLIP        0x1000000000000000ULL
-#define INFINIPATH_HWE_SERDESPLLFAILED      0x2000000000000000ULL
-
-#define IBA6110_IBCS_LINKTRAININGSTATE_MASK 0xf
-#define IBA6110_IBCS_LINKSTATE_SHIFT 4
-
-/* kr_extstatus bits */
-#define INFINIPATH_EXTS_FREQSEL 0x2
-#define INFINIPATH_EXTS_SERDESSEL 0x4
-#define INFINIPATH_EXTS_MEMBIST_ENDTEST     0x0000000000004000
-#define INFINIPATH_EXTS_MEMBIST_CORRECT     0x0000000000008000
-
-
-/* TID entries (memory), HT-only */
-#define INFINIPATH_RT_ADDR_MASK 0xFFFFFFFFFFULL	/* 40 bits valid */
-#define INFINIPATH_RT_VALID 0x8000000000000000ULL
-#define INFINIPATH_RT_ADDR_SHIFT 0
-#define INFINIPATH_RT_BUFSIZE_MASK 0x3FFFULL
-#define INFINIPATH_RT_BUFSIZE_SHIFT 48
-
-#define INFINIPATH_R_INTRAVAIL_SHIFT 16
-#define INFINIPATH_R_TAILUPD_SHIFT 31
-
-/* kr_xgxsconfig bits */
-#define INFINIPATH_XGXS_RESET          0x7ULL
-
-/*
- * masks and bits that are different in different chips, or present only
- * in one
- */
-static const ipath_err_t infinipath_hwe_htcmemparityerr_mask =
-    INFINIPATH_HWE_HTCMEMPARITYERR_MASK;
-static const ipath_err_t infinipath_hwe_htcmemparityerr_shift =
-    INFINIPATH_HWE_HTCMEMPARITYERR_SHIFT;
-
-static const ipath_err_t infinipath_hwe_htclnkabyte0crcerr =
-    INFINIPATH_HWE_HTCLNKABYTE0CRCERR;
-static const ipath_err_t infinipath_hwe_htclnkabyte1crcerr =
-    INFINIPATH_HWE_HTCLNKABYTE1CRCERR;
-static const ipath_err_t infinipath_hwe_htclnkbbyte0crcerr =
-    INFINIPATH_HWE_HTCLNKBBYTE0CRCERR;
-static const ipath_err_t infinipath_hwe_htclnkbbyte1crcerr =
-    INFINIPATH_HWE_HTCLNKBBYTE1CRCERR;
-
-#define _IPATH_GPIO_SDA_NUM 1
-#define _IPATH_GPIO_SCL_NUM 0
-
-#define IPATH_GPIO_SDA \
-	(1ULL << (_IPATH_GPIO_SDA_NUM+INFINIPATH_EXTC_GPIOOE_SHIFT))
-#define IPATH_GPIO_SCL \
-	(1ULL << (_IPATH_GPIO_SCL_NUM+INFINIPATH_EXTC_GPIOOE_SHIFT))
-
-/* keep the code below somewhat more readable; not used elsewhere */
-#define _IPATH_HTLINK0_CRCBITS (infinipath_hwe_htclnkabyte0crcerr |	\
-				infinipath_hwe_htclnkabyte1crcerr)
-#define _IPATH_HTLINK1_CRCBITS (infinipath_hwe_htclnkbbyte0crcerr |	\
-				infinipath_hwe_htclnkbbyte1crcerr)
-#define _IPATH_HTLANE0_CRCBITS (infinipath_hwe_htclnkabyte0crcerr |	\
-				infinipath_hwe_htclnkbbyte0crcerr)
-#define _IPATH_HTLANE1_CRCBITS (infinipath_hwe_htclnkabyte1crcerr |	\
-				infinipath_hwe_htclnkbbyte1crcerr)
-
-static void hwerr_crcbits(struct ipath_devdata *dd, ipath_err_t hwerrs,
-			  char *msg, size_t msgl)
-{
-	char bitsmsg[64];
-	ipath_err_t crcbits = hwerrs &
-		(_IPATH_HTLINK0_CRCBITS | _IPATH_HTLINK1_CRCBITS);
-	/* don't check if 8bit HT */
-	if (dd->ipath_flags & IPATH_8BIT_IN_HT0)
-		crcbits &= ~infinipath_hwe_htclnkabyte1crcerr;
-	/* don't check if 8bit HT */
-	if (dd->ipath_flags & IPATH_8BIT_IN_HT1)
-		crcbits &= ~infinipath_hwe_htclnkbbyte1crcerr;
-	/*
-	 * we'll want to ignore link errors on link that is
-	 * not in use, if any.  For now, complain about both
-	 */
-	if (crcbits) {
-		u16 ctrl0, ctrl1;
-		snprintf(bitsmsg, sizeof bitsmsg,
-			 "[HT%s lane %s CRC (%llx); powercycle to completely clear]",
-			 !(crcbits & _IPATH_HTLINK1_CRCBITS) ?
-			 "0 (A)" : (!(crcbits & _IPATH_HTLINK0_CRCBITS)
-				    ? "1 (B)" : "0+1 (A+B)"),
-			 !(crcbits & _IPATH_HTLANE1_CRCBITS) ? "0"
-			 : (!(crcbits & _IPATH_HTLANE0_CRCBITS) ? "1" :
-			    "0+1"), (unsigned long long) crcbits);
-		strlcat(msg, bitsmsg, msgl);
-
-		/*
-		 * print extra info for debugging.  slave/primary
-		 * config word 4, 8 (link control 0, 1)
-		 */
-
-		if (pci_read_config_word(dd->pcidev,
-					 dd->ipath_ht_slave_off + 0x4,
-					 &ctrl0))
-			dev_info(&dd->pcidev->dev, "Couldn't read "
-				 "linkctrl0 of slave/primary "
-				 "config block\n");
-		else if (!(ctrl0 & 1 << 6))
-			/* not if EOC bit set */
-			ipath_dbg("HT linkctrl0 0x%x%s%s\n", ctrl0,
-				  ((ctrl0 >> 8) & 7) ? " CRC" : "",
-				  ((ctrl0 >> 4) & 1) ? "linkfail" :
-				  "");
-		if (pci_read_config_word(dd->pcidev,
-					 dd->ipath_ht_slave_off + 0x8,
-					 &ctrl1))
-			dev_info(&dd->pcidev->dev, "Couldn't read "
-				 "linkctrl1 of slave/primary "
-				 "config block\n");
-		else if (!(ctrl1 & 1 << 6))
-			/* not if EOC bit set */
-			ipath_dbg("HT linkctrl1 0x%x%s%s\n", ctrl1,
-				  ((ctrl1 >> 8) & 7) ? " CRC" : "",
-				  ((ctrl1 >> 4) & 1) ? "linkfail" :
-				  "");
-
-		/* disable until driver reloaded */
-		dd->ipath_hwerrmask &= ~crcbits;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask,
-				 dd->ipath_hwerrmask);
-		ipath_dbg("HT crc errs: %s\n", msg);
-	} else
-		ipath_dbg("ignoring HT crc errors 0x%llx, "
-			  "not in use\n", (unsigned long long)
-			  (hwerrs & (_IPATH_HTLINK0_CRCBITS |
-				     _IPATH_HTLINK1_CRCBITS)));
-}
-
-/* 6110 specific hardware errors... */
-static const struct ipath_hwerror_msgs ipath_6110_hwerror_msgs[] = {
-	INFINIPATH_HWE_MSG(HTCBUSIREQPARITYERR, "HTC Ireq Parity"),
-	INFINIPATH_HWE_MSG(HTCBUSTREQPARITYERR, "HTC Treq Parity"),
-	INFINIPATH_HWE_MSG(HTCBUSTRESPPARITYERR, "HTC Tresp Parity"),
-	INFINIPATH_HWE_MSG(HTCMISCERR5, "HT core Misc5"),
-	INFINIPATH_HWE_MSG(HTCMISCERR6, "HT core Misc6"),
-	INFINIPATH_HWE_MSG(HTCMISCERR7, "HT core Misc7"),
-	INFINIPATH_HWE_MSG(RXDSYNCMEMPARITYERR, "Rx Dsync"),
-	INFINIPATH_HWE_MSG(SERDESPLLFAILED, "SerDes PLL"),
-};
-
-#define TXE_PIO_PARITY ((INFINIPATH_HWE_TXEMEMPARITYERR_PIOBUF | \
-		        INFINIPATH_HWE_TXEMEMPARITYERR_PIOPBC) \
-		        << INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT)
-#define RXE_EAGER_PARITY (INFINIPATH_HWE_RXEMEMPARITYERR_EAGERTID \
-			  << INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT)
-
-static void ipath_ht_txe_recover(struct ipath_devdata *dd)
-{
-	++ipath_stats.sps_txeparity;
-	dev_info(&dd->pcidev->dev,
-		"Recovering from TXE PIO parity error\n");
-}
-
-
-/**
- * ipath_ht_handle_hwerrors - display hardware errors.
- * @dd: the infinipath device
- * @msg: the output buffer
- * @msgl: the size of the output buffer
- *
- * Use same msg buffer as regular errors to avoid excessive stack
- * use.  Most hardware errors are catastrophic, but for right now,
- * we'll print them and continue.  We reuse the same message buffer as
- * ipath_handle_errors() to avoid excessive stack usage.
- */
-static void ipath_ht_handle_hwerrors(struct ipath_devdata *dd, char *msg,
-				     size_t msgl)
-{
-	ipath_err_t hwerrs;
-	u32 bits, ctrl;
-	int isfatal = 0;
-	char bitsmsg[64];
-	int log_idx;
-
-	hwerrs = ipath_read_kreg64(dd, dd->ipath_kregs->kr_hwerrstatus);
-
-	if (!hwerrs) {
-		ipath_cdbg(VERBOSE, "Called but no hardware errors set\n");
-		/*
-		 * better than printing cofusing messages
-		 * This seems to be related to clearing the crc error, or
-		 * the pll error during init.
-		 */
-		goto bail;
-	} else if (hwerrs == -1LL) {
-		ipath_dev_err(dd, "Read of hardware error status failed "
-			      "(all bits set); ignoring\n");
-		goto bail;
-	}
-	ipath_stats.sps_hwerrs++;
-
-	/* Always clear the error status register, except MEMBISTFAIL,
-	 * regardless of whether we continue or stop using the chip.
-	 * We want that set so we know it failed, even across driver reload.
-	 * We'll still ignore it in the hwerrmask.  We do this partly for
-	 * diagnostics, but also for support */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear,
-			 hwerrs&~INFINIPATH_HWE_MEMBISTFAILED);
-
-	hwerrs &= dd->ipath_hwerrmask;
-
-	/* We log some errors to EEPROM, check if we have any of those. */
-	for (log_idx = 0; log_idx < IPATH_EEP_LOG_CNT; ++log_idx)
-		if (hwerrs & dd->ipath_eep_st_masks[log_idx].hwerrs_to_log)
-			ipath_inc_eeprom_err(dd, log_idx, 1);
-
-	/*
-	 * make sure we get this much out, unless told to be quiet,
-	 * it's a parity error we may recover from,
-	 * or it's occurred within the last 5 seconds
-	 */
-	if ((hwerrs & ~(dd->ipath_lasthwerror | TXE_PIO_PARITY |
-		RXE_EAGER_PARITY)) ||
-		(ipath_debug & __IPATH_VERBDBG))
-		dev_info(&dd->pcidev->dev, "Hardware error: hwerr=0x%llx "
-			 "(cleared)\n", (unsigned long long) hwerrs);
-	dd->ipath_lasthwerror |= hwerrs;
-
-	if (hwerrs & ~dd->ipath_hwe_bitsextant)
-		ipath_dev_err(dd, "hwerror interrupt with unknown errors "
-			      "%llx set\n", (unsigned long long)
-			      (hwerrs & ~dd->ipath_hwe_bitsextant));
-
-	ctrl = ipath_read_kreg32(dd, dd->ipath_kregs->kr_control);
-	if ((ctrl & INFINIPATH_C_FREEZEMODE) && !ipath_diag_inuse) {
-		/*
-		 * parity errors in send memory are recoverable,
-		 * just cancel the send (if indicated in * sendbuffererror),
-		 * count the occurrence, unfreeze (if no other handled
-		 * hardware error bits are set), and continue. They can
-		 * occur if a processor speculative read is done to the PIO
-		 * buffer while we are sending a packet, for example.
-		 */
-		if (hwerrs & TXE_PIO_PARITY) {
-			ipath_ht_txe_recover(dd);
-			hwerrs &= ~TXE_PIO_PARITY;
-		}
-
-		if (!hwerrs) {
-			ipath_dbg("Clearing freezemode on ignored or "
-				  "recovered hardware error\n");
-			ipath_clear_freeze(dd);
-		}
-	}
-
-	*msg = '\0';
-
-	/*
-	 * may someday want to decode into which bits are which
-	 * functional area for parity errors, etc.
-	 */
-	if (hwerrs & (infinipath_hwe_htcmemparityerr_mask
-		      << INFINIPATH_HWE_HTCMEMPARITYERR_SHIFT)) {
-		bits = (u32) ((hwerrs >>
-			       INFINIPATH_HWE_HTCMEMPARITYERR_SHIFT) &
-			      INFINIPATH_HWE_HTCMEMPARITYERR_MASK);
-		snprintf(bitsmsg, sizeof bitsmsg, "[HTC Parity Errs %x] ",
-			 bits);
-		strlcat(msg, bitsmsg, msgl);
-	}
-
-	ipath_format_hwerrors(hwerrs,
-			      ipath_6110_hwerror_msgs,
-			      ARRAY_SIZE(ipath_6110_hwerror_msgs),
-			      msg, msgl);
-
-	if (hwerrs & (_IPATH_HTLINK0_CRCBITS | _IPATH_HTLINK1_CRCBITS))
-		hwerr_crcbits(dd, hwerrs, msg, msgl);
-
-	if (hwerrs & INFINIPATH_HWE_MEMBISTFAILED) {
-		strlcat(msg, "[Memory BIST test failed, InfiniPath hardware unusable]",
-			msgl);
-		/* ignore from now on, so disable until driver reloaded */
-		dd->ipath_hwerrmask &= ~INFINIPATH_HWE_MEMBISTFAILED;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask,
-				 dd->ipath_hwerrmask);
-	}
-#define _IPATH_PLL_FAIL (INFINIPATH_HWE_COREPLL_FBSLIP |	\
-			 INFINIPATH_HWE_COREPLL_RFSLIP |	\
-			 INFINIPATH_HWE_HTBPLL_FBSLIP |		\
-			 INFINIPATH_HWE_HTBPLL_RFSLIP |		\
-			 INFINIPATH_HWE_HTAPLL_FBSLIP |		\
-			 INFINIPATH_HWE_HTAPLL_RFSLIP)
-
-	if (hwerrs & _IPATH_PLL_FAIL) {
-		snprintf(bitsmsg, sizeof bitsmsg,
-			 "[PLL failed (%llx), InfiniPath hardware unusable]",
-			 (unsigned long long) (hwerrs & _IPATH_PLL_FAIL));
-		strlcat(msg, bitsmsg, msgl);
-		/* ignore from now on, so disable until driver reloaded */
-		dd->ipath_hwerrmask &= ~(hwerrs & _IPATH_PLL_FAIL);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask,
-				 dd->ipath_hwerrmask);
-	}
-
-	if (hwerrs & INFINIPATH_HWE_SERDESPLLFAILED) {
-		/*
-		 * If it occurs, it is left masked since the eternal
-		 * interface is unused
-		 */
-		dd->ipath_hwerrmask &= ~INFINIPATH_HWE_SERDESPLLFAILED;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask,
-				 dd->ipath_hwerrmask);
-	}
-
-	if (hwerrs) {
-		/*
-		 * if any set that we aren't ignoring; only
-		 * make the complaint once, in case it's stuck
-		 * or recurring, and we get here multiple
-		 * times.
-		 * force link down, so switch knows, and
-		 * LEDs are turned off
-		 */
-		if (dd->ipath_flags & IPATH_INITTED) {
-			ipath_set_linkstate(dd, IPATH_IB_LINKDOWN);
-			ipath_setup_ht_setextled(dd,
-				INFINIPATH_IBCS_L_STATE_DOWN,
-				INFINIPATH_IBCS_LT_STATE_DISABLED);
-			ipath_dev_err(dd, "Fatal Hardware Error (freeze "
-					  "mode), no longer usable, SN %.16s\n",
-					  dd->ipath_serial);
-			isfatal = 1;
-		}
-		*dd->ipath_statusp &= ~IPATH_STATUS_IB_READY;
-		/* mark as having had error */
-		*dd->ipath_statusp |= IPATH_STATUS_HWERROR;
-		/*
-		 * mark as not usable, at a minimum until driver
-		 * is reloaded, probably until reboot, since no
-		 * other reset is possible.
-		 */
-		dd->ipath_flags &= ~IPATH_INITTED;
-	} else {
-		*msg = 0; /* recovered from all of them */
-	}
-	if (*msg)
-		ipath_dev_err(dd, "%s hardware error\n", msg);
-	if (isfatal && !ipath_diag_inuse && dd->ipath_freezemsg)
-		/*
-		 * for status file; if no trailing brace is copied,
-		 * we'll know it was truncated.
-		 */
-		snprintf(dd->ipath_freezemsg,
-			 dd->ipath_freezelen, "{%s}", msg);
-
-bail:;
-}
-
-/**
- * ipath_ht_boardname - fill in the board name
- * @dd: the infinipath device
- * @name: the output buffer
- * @namelen: the size of the output buffer
- *
- * fill in the board name, based on the board revision register
- */
-static int ipath_ht_boardname(struct ipath_devdata *dd, char *name,
-			      size_t namelen)
-{
-	char *n = NULL;
-	u8 boardrev = dd->ipath_boardrev;
-	int ret = 0;
-
-	switch (boardrev) {
-	case 5:
-		/*
-		 * original production board; two production levels, with
-		 * different serial number ranges.   See ipath_ht_early_init() for
-		 * case where we enable IPATH_GPIO_INTR for later serial # range.
-		 * Original 112* serial number is no longer supported.
-		 */
-		n = "InfiniPath_QHT7040";
-		break;
-	case 7:
-		/* small form factor production board */
-		n = "InfiniPath_QHT7140";
-		break;
-	default:		/* don't know, just print the number */
-		ipath_dev_err(dd, "Don't yet know about board "
-			      "with ID %u\n", boardrev);
-		snprintf(name, namelen, "Unknown_InfiniPath_QHT7xxx_%u",
-			 boardrev);
-		break;
-	}
-	if (n)
-		snprintf(name, namelen, "%s", n);
-
-	if (ret) {
-		ipath_dev_err(dd, "Unsupported InfiniPath board %s!\n", name);
-		goto bail;
-	}
-	if (dd->ipath_majrev != 3 || (dd->ipath_minrev < 2 ||
-		dd->ipath_minrev > 4)) {
-		/*
-		 * This version of the driver only supports Rev 3.2 - 3.4
-		 */
-		ipath_dev_err(dd,
-			      "Unsupported InfiniPath hardware revision %u.%u!\n",
-			      dd->ipath_majrev, dd->ipath_minrev);
-		ret = 1;
-		goto bail;
-	}
-	/*
-	 * pkt/word counters are 32 bit, and therefore wrap fast enough
-	 * that we snapshot them from a timer, and maintain 64 bit shadow
-	 * copies
-	 */
-	dd->ipath_flags |= IPATH_32BITCOUNTERS;
-	dd->ipath_flags |= IPATH_GPIO_INTR;
-	if (dd->ipath_lbus_speed != 800)
-		ipath_dev_err(dd,
-			      "Incorrectly configured for HT @ %uMHz\n",
-			      dd->ipath_lbus_speed);
-
-	/*
-	 * set here, not in ipath_init_*_funcs because we have to do
-	 * it after we can read chip registers.
-	 */
-	dd->ipath_ureg_align =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_pagealign);
-
-bail:
-	return ret;
-}
-
-static void ipath_check_htlink(struct ipath_devdata *dd)
-{
-	u8 linkerr, link_off, i;
-
-	for (i = 0; i < 2; i++) {
-		link_off = dd->ipath_ht_slave_off + i * 4 + 0xd;
-		if (pci_read_config_byte(dd->pcidev, link_off, &linkerr))
-			dev_info(&dd->pcidev->dev, "Couldn't read "
-				 "linkerror%d of HT slave/primary block\n",
-				 i);
-		else if (linkerr & 0xf0) {
-			ipath_cdbg(VERBOSE, "HT linkerr%d bits 0x%x set, "
-				   "clearing\n", linkerr >> 4, i);
-			/*
-			 * writing the linkerr bits that are set should
-			 * clear them
-			 */
-			if (pci_write_config_byte(dd->pcidev, link_off,
-						  linkerr))
-				ipath_dbg("Failed write to clear HT "
-					  "linkerror%d\n", i);
-			if (pci_read_config_byte(dd->pcidev, link_off,
-						 &linkerr))
-				dev_info(&dd->pcidev->dev,
-					 "Couldn't reread linkerror%d of "
-					 "HT slave/primary block\n", i);
-			else if (linkerr & 0xf0)
-				dev_info(&dd->pcidev->dev,
-					 "HT linkerror%d bits 0x%x "
-					 "couldn't be cleared\n",
-					 i, linkerr >> 4);
-		}
-	}
-}
-
-static int ipath_setup_ht_reset(struct ipath_devdata *dd)
-{
-	ipath_dbg("No reset possible for this InfiniPath hardware\n");
-	return 0;
-}
-
-#define HT_INTR_DISC_CONFIG  0x80	/* HT interrupt and discovery cap */
-#define HT_INTR_REG_INDEX    2	/* intconfig requires indirect accesses */
-
-/*
- * Bits 13-15 of command==0 is slave/primary block.  Clear any HT CRC
- * errors.  We only bother to do this at load time, because it's OK if
- * it happened before we were loaded (first time after boot/reset),
- * but any time after that, it's fatal anyway.  Also need to not check
- * for upper byte errors if we are in 8 bit mode, so figure out
- * our width.  For now, at least, also complain if it's 8 bit.
- */
-static void slave_or_pri_blk(struct ipath_devdata *dd, struct pci_dev *pdev,
-			     int pos, u8 cap_type)
-{
-	u8 linkwidth = 0, linkerr, link_a_b_off, link_off;
-	u16 linkctrl = 0;
-	int i;
-
-	dd->ipath_ht_slave_off = pos;
-	/* command word, master_host bit */
-	/* master host || slave */
-	if ((cap_type >> 2) & 1)
-		link_a_b_off = 4;
-	else
-		link_a_b_off = 0;
-	ipath_cdbg(VERBOSE, "HT%u (Link %c) connected to processor\n",
-		   link_a_b_off ? 1 : 0,
-		   link_a_b_off ? 'B' : 'A');
-
-	link_a_b_off += pos;
-
-	/*
-	 * check both link control registers; clear both HT CRC sets if
-	 * necessary.
-	 */
-	for (i = 0; i < 2; i++) {
-		link_off = pos + i * 4 + 0x4;
-		if (pci_read_config_word(pdev, link_off, &linkctrl))
-			ipath_dev_err(dd, "Couldn't read HT link control%d "
-				      "register\n", i);
-		else if (linkctrl & (0xf << 8)) {
-			ipath_cdbg(VERBOSE, "Clear linkctrl%d CRC Error "
-				   "bits %x\n", i, linkctrl & (0xf << 8));
-			/*
-			 * now write them back to clear the error.
-			 */
-			pci_write_config_word(pdev, link_off,
-					      linkctrl & (0xf << 8));
-		}
-	}
-
-	/*
-	 * As with HT CRC bits, same for protocol errors that might occur
-	 * during boot.
-	 */
-	for (i = 0; i < 2; i++) {
-		link_off = pos + i * 4 + 0xd;
-		if (pci_read_config_byte(pdev, link_off, &linkerr))
-			dev_info(&pdev->dev, "Couldn't read linkerror%d "
-				 "of HT slave/primary block\n", i);
-		else if (linkerr & 0xf0) {
-			ipath_cdbg(VERBOSE, "HT linkerr%d bits 0x%x set, "
-				   "clearing\n", linkerr >> 4, i);
-			/*
-			 * writing the linkerr bits that are set will clear
-			 * them
-			 */
-			if (pci_write_config_byte
-			    (pdev, link_off, linkerr))
-				ipath_dbg("Failed write to clear HT "
-					  "linkerror%d\n", i);
-			if (pci_read_config_byte(pdev, link_off, &linkerr))
-				dev_info(&pdev->dev, "Couldn't reread "
-					 "linkerror%d of HT slave/primary "
-					 "block\n", i);
-			else if (linkerr & 0xf0)
-				dev_info(&pdev->dev, "HT linkerror%d bits "
-					 "0x%x couldn't be cleared\n",
-					 i, linkerr >> 4);
-		}
-	}
-
-	/*
-	 * this is just for our link to the host, not devices connected
-	 * through tunnel.
-	 */
-
-	if (pci_read_config_byte(pdev, link_a_b_off + 7, &linkwidth))
-		ipath_dev_err(dd, "Couldn't read HT link width "
-			      "config register\n");
-	else {
-		u32 width;
-		switch (linkwidth & 7) {
-		case 5:
-			width = 4;
-			break;
-		case 4:
-			width = 2;
-			break;
-		case 3:
-			width = 32;
-			break;
-		case 1:
-			width = 16;
-			break;
-		case 0:
-		default:	/* if wrong, assume 8 bit */
-			width = 8;
-			break;
-		}
-
-		dd->ipath_lbus_width = width;
-
-		if (linkwidth != 0x11) {
-			ipath_dev_err(dd, "Not configured for 16 bit HT "
-				      "(%x)\n", linkwidth);
-			if (!(linkwidth & 0xf)) {
-				ipath_dbg("Will ignore HT lane1 errors\n");
-				dd->ipath_flags |= IPATH_8BIT_IN_HT0;
-			}
-		}
-	}
-
-	/*
-	 * this is just for our link to the host, not devices connected
-	 * through tunnel.
-	 */
-	if (pci_read_config_byte(pdev, link_a_b_off + 0xd, &linkwidth))
-		ipath_dev_err(dd, "Couldn't read HT link frequency "
-			      "config register\n");
-	else {
-		u32 speed;
-		switch (linkwidth & 0xf) {
-		case 6:
-			speed = 1000;
-			break;
-		case 5:
-			speed = 800;
-			break;
-		case 4:
-			speed = 600;
-			break;
-		case 3:
-			speed = 500;
-			break;
-		case 2:
-			speed = 400;
-			break;
-		case 1:
-			speed = 300;
-			break;
-		default:
-			/*
-			 * assume reserved and vendor-specific are 200...
-			 */
-		case 0:
-			speed = 200;
-			break;
-		}
-		dd->ipath_lbus_speed = speed;
-	}
-
-	snprintf(dd->ipath_lbus_info, sizeof(dd->ipath_lbus_info),
-		"HyperTransport,%uMHz,x%u\n",
-		dd->ipath_lbus_speed,
-		dd->ipath_lbus_width);
-}
-
-static int ipath_ht_intconfig(struct ipath_devdata *dd)
-{
-	int ret;
-
-	if (dd->ipath_intconfig) {
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_interruptconfig,
-				 dd->ipath_intconfig);	/* interrupt address */
-		ret = 0;
-	} else {
-		ipath_dev_err(dd, "No interrupts enabled, couldn't setup "
-			      "interrupt address\n");
-		ret = -EINVAL;
-	}
-
-	return ret;
-}
-
-static void ipath_ht_irq_update(struct pci_dev *dev, int irq,
-				struct ht_irq_msg *msg)
-{
-	struct ipath_devdata *dd = pci_get_drvdata(dev);
-	u64 prev_intconfig = dd->ipath_intconfig;
-
-	dd->ipath_intconfig = msg->address_lo;
-	dd->ipath_intconfig |= ((u64) msg->address_hi) << 32;
-
-	/*
-	 * If the previous value of dd->ipath_intconfig is zero, we're
-	 * getting configured for the first time, and must not program the
-	 * intconfig register here (it will be programmed later, when the
-	 * hardware is ready).  Otherwise, we should.
-	 */
-	if (prev_intconfig)
-		ipath_ht_intconfig(dd);
-}
-
-/**
- * ipath_setup_ht_config - setup the interruptconfig register
- * @dd: the infinipath device
- * @pdev: the PCI device
- *
- * setup the interruptconfig register from the HT config info.
- * Also clear CRC errors in HT linkcontrol, if necessary.
- * This is done only for the real hardware.  It is done before
- * chip address space is initted, so can't touch infinipath registers
- */
-static int ipath_setup_ht_config(struct ipath_devdata *dd,
-				 struct pci_dev *pdev)
-{
-	int pos, ret;
-
-	ret = __ht_create_irq(pdev, 0, ipath_ht_irq_update);
-	if (ret < 0) {
-		ipath_dev_err(dd, "Couldn't create interrupt handler: "
-			      "err %d\n", ret);
-		goto bail;
-	}
-	dd->ipath_irq = ret;
-	ret = 0;
-
-	/*
-	 * Handle clearing CRC errors in linkctrl register if necessary.  We
-	 * do this early, before we ever enable errors or hardware errors,
-	 * mostly to avoid causing the chip to enter freeze mode.
-	 */
-	pos = pci_find_capability(pdev, PCI_CAP_ID_HT);
-	if (!pos) {
-		ipath_dev_err(dd, "Couldn't find HyperTransport "
-			      "capability; no interrupts\n");
-		ret = -ENODEV;
-		goto bail;
-	}
-	do {
-		u8 cap_type;
-
-		/*
-		 * The HT capability type byte is 3 bytes after the
-		 * capability byte.
-		 */
-		if (pci_read_config_byte(pdev, pos + 3, &cap_type)) {
-			dev_info(&pdev->dev, "Couldn't read config "
-				 "command @ %d\n", pos);
-			continue;
-		}
-		if (!(cap_type & 0xE0))
-			slave_or_pri_blk(dd, pdev, pos, cap_type);
-	} while ((pos = pci_find_next_capability(pdev, pos,
-						 PCI_CAP_ID_HT)));
-
-	dd->ipath_flags |= IPATH_SWAP_PIOBUFS;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_setup_ht_cleanup - clean up any per-chip chip-specific stuff
- * @dd: the infinipath device
- *
- * Called during driver unload.
- * This is currently a nop for the HT chip, not for all chips
- */
-static void ipath_setup_ht_cleanup(struct ipath_devdata *dd)
-{
-}
-
-/**
- * ipath_setup_ht_setextled - set the state of the two external LEDs
- * @dd: the infinipath device
- * @lst: the L state
- * @ltst: the LT state
- *
- * Set the state of the two external LEDs, to indicate physical and
- * logical state of IB link.   For this chip (at least with recommended
- * board pinouts), LED1 is Green (physical state), and LED2 is Yellow
- * (logical state)
- *
- * Note:  We try to match the Mellanox HCA LED behavior as best
- * we can.  Green indicates physical link state is OK (something is
- * plugged in, and we can train).
- * Amber indicates the link is logically up (ACTIVE).
- * Mellanox further blinks the amber LED to indicate data packet
- * activity, but we have no hardware support for that, so it would
- * require waking up every 10-20 msecs and checking the counters
- * on the chip, and then turning the LED off if appropriate.  That's
- * visible overhead, so not something we will do.
- *
- */
-static void ipath_setup_ht_setextled(struct ipath_devdata *dd,
-				     u64 lst, u64 ltst)
-{
-	u64 extctl;
-	unsigned long flags = 0;
-
-	/* the diags use the LED to indicate diag info, so we leave
-	 * the external LED alone when the diags are running */
-	if (ipath_diag_inuse)
-		return;
-
-	/* Allow override of LED display for, e.g. Locating system in rack */
-	if (dd->ipath_led_override) {
-		ltst = (dd->ipath_led_override & IPATH_LED_PHYS)
-			? INFINIPATH_IBCS_LT_STATE_LINKUP
-			: INFINIPATH_IBCS_LT_STATE_DISABLED;
-		lst = (dd->ipath_led_override & IPATH_LED_LOG)
-			? INFINIPATH_IBCS_L_STATE_ACTIVE
-			: INFINIPATH_IBCS_L_STATE_DOWN;
-	}
-
-	spin_lock_irqsave(&dd->ipath_gpio_lock, flags);
-	/*
-	 * start by setting both LED control bits to off, then turn
-	 * on the appropriate bit(s).
-	 */
-	if (dd->ipath_boardrev == 8) { /* LS/X-1 uses different pins */
-		/*
-		 * major difference is that INFINIPATH_EXTC_LEDGBLERR_OFF
-		 * is inverted,  because it is normally used to indicate
-		 * a hardware fault at reset, if there were errors
-		 */
-		extctl = (dd->ipath_extctrl & ~INFINIPATH_EXTC_LEDGBLOK_ON)
-			| INFINIPATH_EXTC_LEDGBLERR_OFF;
-		if (ltst == INFINIPATH_IBCS_LT_STATE_LINKUP)
-			extctl &= ~INFINIPATH_EXTC_LEDGBLERR_OFF;
-		if (lst == INFINIPATH_IBCS_L_STATE_ACTIVE)
-			extctl |= INFINIPATH_EXTC_LEDGBLOK_ON;
-	} else {
-		extctl = dd->ipath_extctrl &
-			~(INFINIPATH_EXTC_LED1PRIPORT_ON |
-			  INFINIPATH_EXTC_LED2PRIPORT_ON);
-		if (ltst == INFINIPATH_IBCS_LT_STATE_LINKUP)
-			extctl |= INFINIPATH_EXTC_LED1PRIPORT_ON;
-		if (lst == INFINIPATH_IBCS_L_STATE_ACTIVE)
-			extctl |= INFINIPATH_EXTC_LED2PRIPORT_ON;
-	}
-	dd->ipath_extctrl = extctl;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_extctrl, extctl);
-	spin_unlock_irqrestore(&dd->ipath_gpio_lock, flags);
-}
-
-static void ipath_init_ht_variables(struct ipath_devdata *dd)
-{
-	/*
-	 * setup the register offsets, since they are different for each
-	 * chip
-	 */
-	dd->ipath_kregs = &ipath_ht_kregs;
-	dd->ipath_cregs = &ipath_ht_cregs;
-
-	dd->ipath_gpio_sda_num = _IPATH_GPIO_SDA_NUM;
-	dd->ipath_gpio_scl_num = _IPATH_GPIO_SCL_NUM;
-	dd->ipath_gpio_sda = IPATH_GPIO_SDA;
-	dd->ipath_gpio_scl = IPATH_GPIO_SCL;
-
-	/*
-	 * Fill in data for field-values that change in newer chips.
-	 * We dynamically specify only the mask for LINKTRAININGSTATE
-	 * and only the shift for LINKSTATE, as they are the only ones
-	 * that change.  Also precalculate the 3 link states of interest
-	 * and the combined mask.
-	 */
-	dd->ibcs_ls_shift = IBA6110_IBCS_LINKSTATE_SHIFT;
-	dd->ibcs_lts_mask = IBA6110_IBCS_LINKTRAININGSTATE_MASK;
-	dd->ibcs_mask = (INFINIPATH_IBCS_LINKSTATE_MASK <<
-		dd->ibcs_ls_shift) | dd->ibcs_lts_mask;
-	dd->ib_init = (INFINIPATH_IBCS_LT_STATE_LINKUP <<
-		INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) |
-		(INFINIPATH_IBCS_L_STATE_INIT << dd->ibcs_ls_shift);
-	dd->ib_arm = (INFINIPATH_IBCS_LT_STATE_LINKUP <<
-		INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) |
-		(INFINIPATH_IBCS_L_STATE_ARM << dd->ibcs_ls_shift);
-	dd->ib_active = (INFINIPATH_IBCS_LT_STATE_LINKUP <<
-		INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) |
-		(INFINIPATH_IBCS_L_STATE_ACTIVE << dd->ibcs_ls_shift);
-
-	/*
-	 * Fill in data for ibcc field-values that change in newer chips.
-	 * We dynamically specify only the mask for LINKINITCMD
-	 * and only the shift for LINKCMD and MAXPKTLEN, as they are
-	 * the only ones that change.
-	 */
-	dd->ibcc_lic_mask = INFINIPATH_IBCC_LINKINITCMD_MASK;
-	dd->ibcc_lc_shift = INFINIPATH_IBCC_LINKCMD_SHIFT;
-	dd->ibcc_mpl_shift = INFINIPATH_IBCC_MAXPKTLEN_SHIFT;
-
-	/* Fill in shifts for RcvCtrl. */
-	dd->ipath_r_portenable_shift = INFINIPATH_R_PORTENABLE_SHIFT;
-	dd->ipath_r_intravail_shift = INFINIPATH_R_INTRAVAIL_SHIFT;
-	dd->ipath_r_tailupd_shift = INFINIPATH_R_TAILUPD_SHIFT;
-	dd->ipath_r_portcfg_shift = 0; /* Not on IBA6110 */
-
-	dd->ipath_i_bitsextant =
-		(INFINIPATH_I_RCVURG_MASK << INFINIPATH_I_RCVURG_SHIFT) |
-		(INFINIPATH_I_RCVAVAIL_MASK <<
-		 INFINIPATH_I_RCVAVAIL_SHIFT) |
-		INFINIPATH_I_ERROR | INFINIPATH_I_SPIOSENT |
-		INFINIPATH_I_SPIOBUFAVAIL | INFINIPATH_I_GPIO;
-
-	dd->ipath_e_bitsextant =
-		INFINIPATH_E_RFORMATERR | INFINIPATH_E_RVCRC |
-		INFINIPATH_E_RICRC | INFINIPATH_E_RMINPKTLEN |
-		INFINIPATH_E_RMAXPKTLEN | INFINIPATH_E_RLONGPKTLEN |
-		INFINIPATH_E_RSHORTPKTLEN | INFINIPATH_E_RUNEXPCHAR |
-		INFINIPATH_E_RUNSUPVL | INFINIPATH_E_REBP |
-		INFINIPATH_E_RIBFLOW | INFINIPATH_E_RBADVERSION |
-		INFINIPATH_E_RRCVEGRFULL | INFINIPATH_E_RRCVHDRFULL |
-		INFINIPATH_E_RBADTID | INFINIPATH_E_RHDRLEN |
-		INFINIPATH_E_RHDR | INFINIPATH_E_RIBLOSTLINK |
-		INFINIPATH_E_SMINPKTLEN | INFINIPATH_E_SMAXPKTLEN |
-		INFINIPATH_E_SUNDERRUN | INFINIPATH_E_SPKTLEN |
-		INFINIPATH_E_SDROPPEDSMPPKT | INFINIPATH_E_SDROPPEDDATAPKT |
-		INFINIPATH_E_SPIOARMLAUNCH | INFINIPATH_E_SUNEXPERRPKTNUM |
-		INFINIPATH_E_SUNSUPVL | INFINIPATH_E_IBSTATUSCHANGED |
-		INFINIPATH_E_INVALIDADDR | INFINIPATH_E_RESET |
-		INFINIPATH_E_HARDWARE;
-
-	dd->ipath_hwe_bitsextant =
-		(INFINIPATH_HWE_HTCMEMPARITYERR_MASK <<
-		 INFINIPATH_HWE_HTCMEMPARITYERR_SHIFT) |
-		(INFINIPATH_HWE_TXEMEMPARITYERR_MASK <<
-		 INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT) |
-		(INFINIPATH_HWE_RXEMEMPARITYERR_MASK <<
-		 INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT) |
-		INFINIPATH_HWE_HTCLNKABYTE0CRCERR |
-		INFINIPATH_HWE_HTCLNKABYTE1CRCERR |
-		INFINIPATH_HWE_HTCLNKBBYTE0CRCERR |
-		INFINIPATH_HWE_HTCLNKBBYTE1CRCERR |
-		INFINIPATH_HWE_HTCMISCERR4 |
-		INFINIPATH_HWE_HTCMISCERR5 | INFINIPATH_HWE_HTCMISCERR6 |
-		INFINIPATH_HWE_HTCMISCERR7 |
-		INFINIPATH_HWE_HTCBUSTREQPARITYERR |
-		INFINIPATH_HWE_HTCBUSTRESPPARITYERR |
-		INFINIPATH_HWE_HTCBUSIREQPARITYERR |
-		INFINIPATH_HWE_RXDSYNCMEMPARITYERR |
-		INFINIPATH_HWE_MEMBISTFAILED |
-		INFINIPATH_HWE_COREPLL_FBSLIP |
-		INFINIPATH_HWE_COREPLL_RFSLIP |
-		INFINIPATH_HWE_HTBPLL_FBSLIP |
-		INFINIPATH_HWE_HTBPLL_RFSLIP |
-		INFINIPATH_HWE_HTAPLL_FBSLIP |
-		INFINIPATH_HWE_HTAPLL_RFSLIP |
-		INFINIPATH_HWE_SERDESPLLFAILED |
-		INFINIPATH_HWE_IBCBUSTOSPCPARITYERR |
-		INFINIPATH_HWE_IBCBUSFRSPCPARITYERR;
-
-	dd->ipath_i_rcvavail_mask = INFINIPATH_I_RCVAVAIL_MASK;
-	dd->ipath_i_rcvurg_mask = INFINIPATH_I_RCVURG_MASK;
-	dd->ipath_i_rcvavail_shift = INFINIPATH_I_RCVAVAIL_SHIFT;
-	dd->ipath_i_rcvurg_shift = INFINIPATH_I_RCVURG_SHIFT;
-
-	/*
-	 * EEPROM error log 0 is TXE Parity errors. 1 is RXE Parity.
-	 * 2 is Some Misc, 3 is reserved for future.
-	 */
-	dd->ipath_eep_st_masks[0].hwerrs_to_log =
-		INFINIPATH_HWE_TXEMEMPARITYERR_MASK <<
-		INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT;
-
-	dd->ipath_eep_st_masks[1].hwerrs_to_log =
-		INFINIPATH_HWE_RXEMEMPARITYERR_MASK <<
-		INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT;
-
-	dd->ipath_eep_st_masks[2].errs_to_log = INFINIPATH_E_RESET;
-
-	dd->delay_mult = 2; /* SDR, 4X, can't change */
-
-	dd->ipath_link_width_supported = IB_WIDTH_1X | IB_WIDTH_4X;
-	dd->ipath_link_speed_supported = IPATH_IB_SDR;
-	dd->ipath_link_width_enabled = IB_WIDTH_4X;
-	dd->ipath_link_speed_enabled = dd->ipath_link_speed_supported;
-	/* these can't change for this chip, so set once */
-	dd->ipath_link_width_active = dd->ipath_link_width_enabled;
-	dd->ipath_link_speed_active = dd->ipath_link_speed_enabled;
-}
-
-/**
- * ipath_ht_init_hwerrors - enable hardware errors
- * @dd: the infinipath device
- *
- * now that we have finished initializing everything that might reasonably
- * cause a hardware error, and cleared those errors bits as they occur,
- * we can enable hardware errors in the mask (potentially enabling
- * freeze mode), and enable hardware errors as errors (along with
- * everything else) in errormask
- */
-static void ipath_ht_init_hwerrors(struct ipath_devdata *dd)
-{
-	ipath_err_t val;
-	u64 extsval;
-
-	extsval = ipath_read_kreg64(dd, dd->ipath_kregs->kr_extstatus);
-
-	if (!(extsval & INFINIPATH_EXTS_MEMBIST_ENDTEST))
-		ipath_dev_err(dd, "MemBIST did not complete!\n");
-	if (extsval & INFINIPATH_EXTS_MEMBIST_CORRECT)
-		ipath_dbg("MemBIST corrected\n");
-
-	ipath_check_htlink(dd);
-
-	/* barring bugs, all hwerrors become interrupts, which can */
-	val = -1LL;
-	/* don't look at crc lane1 if 8 bit */
-	if (dd->ipath_flags & IPATH_8BIT_IN_HT0)
-		val &= ~infinipath_hwe_htclnkabyte1crcerr;
-	/* don't look at crc lane1 if 8 bit */
-	if (dd->ipath_flags & IPATH_8BIT_IN_HT1)
-		val &= ~infinipath_hwe_htclnkbbyte1crcerr;
-
-	/*
-	 * disable RXDSYNCMEMPARITY because external serdes is unused,
-	 * and therefore the logic will never be used or initialized,
-	 * and uninitialized state will normally result in this error
-	 * being asserted.  Similarly for the external serdess pll
-	 * lock signal.
-	 */
-	val &= ~(INFINIPATH_HWE_SERDESPLLFAILED |
-		 INFINIPATH_HWE_RXDSYNCMEMPARITYERR);
-
-	/*
-	 * Disable MISCERR4 because of an inversion in the HT core
-	 * logic checking for errors that cause this bit to be set.
-	 * The errata can also cause the protocol error bit to be set
-	 * in the HT config space linkerror register(s).
-	 */
-	val &= ~INFINIPATH_HWE_HTCMISCERR4;
-
-	/*
-	 * PLL ignored because unused MDIO interface has a logic problem
-	 */
-	if (dd->ipath_boardrev == 4 || dd->ipath_boardrev == 9)
-		val &= ~INFINIPATH_HWE_SERDESPLLFAILED;
-	dd->ipath_hwerrmask = val;
-}
-
-
-
-
-/**
- * ipath_ht_bringup_serdes - bring up the serdes
- * @dd: the infinipath device
- */
-static int ipath_ht_bringup_serdes(struct ipath_devdata *dd)
-{
-	u64 val, config1;
-	int ret = 0, change = 0;
-
-	ipath_dbg("Trying to bringup serdes\n");
-
-	if (ipath_read_kreg64(dd, dd->ipath_kregs->kr_hwerrstatus) &
-	    INFINIPATH_HWE_SERDESPLLFAILED)
-	{
-		ipath_dbg("At start, serdes PLL failed bit set in "
-			  "hwerrstatus, clearing and continuing\n");
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear,
-				 INFINIPATH_HWE_SERDESPLLFAILED);
-	}
-
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesconfig0);
-	config1 = ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesconfig1);
-
-	ipath_cdbg(VERBOSE, "Initial serdes status is config0=%llx "
-		   "config1=%llx, sstatus=%llx xgxs %llx\n",
-		   (unsigned long long) val, (unsigned long long) config1,
-		   (unsigned long long)
-		   ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesstatus),
-		   (unsigned long long)
-		   ipath_read_kreg64(dd, dd->ipath_kregs->kr_xgxsconfig));
-
-	/* force reset on */
-	val |= INFINIPATH_SERDC0_RESET_PLL
-		/* | INFINIPATH_SERDC0_RESET_MASK */
-		;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_serdesconfig0, val);
-	udelay(15);		/* need pll reset set at least for a bit */
-
-	if (val & INFINIPATH_SERDC0_RESET_PLL) {
-		u64 val2 = val &= ~INFINIPATH_SERDC0_RESET_PLL;
-		/* set lane resets, and tx idle, during pll reset */
-		val2 |= INFINIPATH_SERDC0_RESET_MASK |
-			INFINIPATH_SERDC0_TXIDLE;
-		ipath_cdbg(VERBOSE, "Clearing serdes PLL reset (writing "
-			   "%llx)\n", (unsigned long long) val2);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_serdesconfig0,
-				 val2);
-		/*
-		 * be sure chip saw it
-		 */
-		val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		/*
-		 * need pll reset clear at least 11 usec before lane
-		 * resets cleared; give it a few more
-		 */
-		udelay(15);
-		val = val2;	/* for check below */
-	}
-
-	if (val & (INFINIPATH_SERDC0_RESET_PLL |
-		   INFINIPATH_SERDC0_RESET_MASK |
-		   INFINIPATH_SERDC0_TXIDLE)) {
-		val &= ~(INFINIPATH_SERDC0_RESET_PLL |
-			 INFINIPATH_SERDC0_RESET_MASK |
-			 INFINIPATH_SERDC0_TXIDLE);
-		/* clear them */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_serdesconfig0,
-				 val);
-	}
-
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_xgxsconfig);
-	if (val & INFINIPATH_XGXS_RESET) {
-		/* normally true after boot */
-		val &= ~INFINIPATH_XGXS_RESET;
-		change = 1;
-	}
-	if (((val >> INFINIPATH_XGXS_RX_POL_SHIFT) &
-	     INFINIPATH_XGXS_RX_POL_MASK) != dd->ipath_rx_pol_inv ) {
-		/* need to compensate for Tx inversion in partner */
-		val &= ~(INFINIPATH_XGXS_RX_POL_MASK <<
-		         INFINIPATH_XGXS_RX_POL_SHIFT);
-		val |= dd->ipath_rx_pol_inv <<
-			INFINIPATH_XGXS_RX_POL_SHIFT;
-		change = 1;
-	}
-	if (change)
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_xgxsconfig, val);
-
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesconfig0);
-
-	/* clear current and de-emphasis bits */
-	config1 &= ~0x0ffffffff00ULL;
-	/* set current to 20ma */
-	config1 |= 0x00000000000ULL;
-	/* set de-emphasis to -5.68dB */
-	config1 |= 0x0cccc000000ULL;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_serdesconfig1, config1);
-
-	ipath_cdbg(VERBOSE, "After setup: serdes status is config0=%llx "
-		   "config1=%llx, sstatus=%llx xgxs %llx\n",
-		   (unsigned long long) val, (unsigned long long) config1,
-		   (unsigned long long)
-		   ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesstatus),
-		   (unsigned long long)
-		   ipath_read_kreg64(dd, dd->ipath_kregs->kr_xgxsconfig));
-
-	return ret;		/* for now, say we always succeeded */
-}
-
-/**
- * ipath_ht_quiet_serdes - set serdes to txidle
- * @dd: the infinipath device
- * driver is being unloaded
- */
-static void ipath_ht_quiet_serdes(struct ipath_devdata *dd)
-{
-	u64 val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_serdesconfig0);
-
-	val |= INFINIPATH_SERDC0_TXIDLE;
-	ipath_dbg("Setting TxIdleEn on serdes (config0 = %llx)\n",
-		  (unsigned long long) val);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_serdesconfig0, val);
-}
-
-/**
- * ipath_pe_put_tid - write a TID in chip
- * @dd: the infinipath device
- * @tidptr: pointer to the expected TID (in chip) to update
- * @tidtype: RCVHQ_RCV_TYPE_EAGER (1) for eager, RCVHQ_RCV_TYPE_EXPECTED (0) for expected
- * @pa: physical address of in memory buffer; ipath_tidinvalid if freeing
- *
- * This exists as a separate routine to allow for special locking etc.
- * It's used for both the full cleanup on exit, as well as the normal
- * setup and teardown.
- */
-static void ipath_ht_put_tid(struct ipath_devdata *dd,
-			     u64 __iomem *tidptr, u32 type,
-			     unsigned long pa)
-{
-	if (!dd->ipath_kregbase)
-		return;
-
-	if (pa != dd->ipath_tidinvalid) {
-		if (unlikely((pa & ~INFINIPATH_RT_ADDR_MASK))) {
-			dev_info(&dd->pcidev->dev,
-				 "physaddr %lx has more than "
-				 "40 bits, using only 40!!!\n", pa);
-			pa &= INFINIPATH_RT_ADDR_MASK;
-		}
-		if (type == RCVHQ_RCV_TYPE_EAGER)
-			pa |= dd->ipath_tidtemplate;
-		else {
-			/* in words (fixed, full page).  */
-			u64 lenvalid = PAGE_SIZE >> 2;
-			lenvalid <<= INFINIPATH_RT_BUFSIZE_SHIFT;
-			pa |= lenvalid | INFINIPATH_RT_VALID;
-		}
-	}
-
-	writeq(pa, tidptr);
-}
-
-
-/**
- * ipath_ht_clear_tid - clear all TID entries for a port, expected and eager
- * @dd: the infinipath device
- * @port: the port
- *
- * Used from ipath_close(), and at chip initialization.
- */
-static void ipath_ht_clear_tids(struct ipath_devdata *dd, unsigned port)
-{
-	u64 __iomem *tidbase;
-	int i;
-
-	if (!dd->ipath_kregbase)
-		return;
-
-	ipath_cdbg(VERBOSE, "Invalidate TIDs for port %u\n", port);
-
-	/*
-	 * need to invalidate all of the expected TID entries for this
-	 * port, so we don't have valid entries that might somehow get
-	 * used (early in next use of this port, or through some bug)
-	 */
-	tidbase = (u64 __iomem *) ((char __iomem *)(dd->ipath_kregbase) +
-				   dd->ipath_rcvtidbase +
-				   port * dd->ipath_rcvtidcnt *
-				   sizeof(*tidbase));
-	for (i = 0; i < dd->ipath_rcvtidcnt; i++)
-		ipath_ht_put_tid(dd, &tidbase[i], RCVHQ_RCV_TYPE_EXPECTED,
-				 dd->ipath_tidinvalid);
-
-	tidbase = (u64 __iomem *) ((char __iomem *)(dd->ipath_kregbase) +
-				   dd->ipath_rcvegrbase +
-				   port * dd->ipath_rcvegrcnt *
-				   sizeof(*tidbase));
-
-	for (i = 0; i < dd->ipath_rcvegrcnt; i++)
-		ipath_ht_put_tid(dd, &tidbase[i], RCVHQ_RCV_TYPE_EAGER,
-				 dd->ipath_tidinvalid);
-}
-
-/**
- * ipath_ht_tidtemplate - setup constants for TID updates
- * @dd: the infinipath device
- *
- * We setup stuff that we use a lot, to avoid calculating each time
- */
-static void ipath_ht_tidtemplate(struct ipath_devdata *dd)
-{
-	dd->ipath_tidtemplate = dd->ipath_ibmaxlen >> 2;
-	dd->ipath_tidtemplate <<= INFINIPATH_RT_BUFSIZE_SHIFT;
-	dd->ipath_tidtemplate |= INFINIPATH_RT_VALID;
-
-	/*
-	 * work around chip errata bug 7358, by marking invalid tids
-	 * as having max length
-	 */
-	dd->ipath_tidinvalid = (-1LL & INFINIPATH_RT_BUFSIZE_MASK) <<
-		INFINIPATH_RT_BUFSIZE_SHIFT;
-}
-
-static int ipath_ht_early_init(struct ipath_devdata *dd)
-{
-	u32 __iomem *piobuf;
-	u32 pioincr, val32;
-	int i;
-
-	/*
-	 * one cache line; long IB headers will spill over into received
-	 * buffer
-	 */
-	dd->ipath_rcvhdrentsize = 16;
-	dd->ipath_rcvhdrsize = IPATH_DFLT_RCVHDRSIZE;
-
-	/*
-	 * For HT, we allocate a somewhat overly large eager buffer,
-	 * such that we can guarantee that we can receive the largest
-	 * packet that we can send out.  To truly support a 4KB MTU,
-	 * we need to bump this to a large value.  To date, other than
-	 * testing, we have never encountered an HCA that can really
-	 * send 4KB MTU packets, so we do not handle that (we'll get
-	 * errors interrupts if we ever see one).
-	 */
-	dd->ipath_rcvegrbufsize = dd->ipath_piosize2k;
-
-	/*
-	 * the min() check here is currently a nop, but it may not
-	 * always be, depending on just how we do ipath_rcvegrbufsize
-	 */
-	dd->ipath_ibmaxlen = min(dd->ipath_piosize2k,
-				 dd->ipath_rcvegrbufsize);
-	dd->ipath_init_ibmaxlen = dd->ipath_ibmaxlen;
-	ipath_ht_tidtemplate(dd);
-
-	/*
-	 * zero all the TID entries at startup.  We do this for sanity,
-	 * in case of a previous driver crash of some kind, and also
-	 * because the chip powers up with these memories in an unknown
-	 * state.  Use portcnt, not cfgports, since this is for the
-	 * full chip, not for current (possibly different) configuration
-	 * value.
-	 * Chip Errata bug 6447
-	 */
-	for (val32 = 0; val32 < dd->ipath_portcnt; val32++)
-		ipath_ht_clear_tids(dd, val32);
-
-	/*
-	 * write the pbc of each buffer, to be sure it's initialized, then
-	 * cancel all the buffers, and also abort any packets that might
-	 * have been in flight for some reason (the latter is for driver
-	 * unload/reload, but isn't a bad idea at first init).	PIO send
-	 * isn't enabled at this point, so there is no danger of sending
-	 * these out on the wire.
-	 * Chip Errata bug 6610
-	 */
-	piobuf = (u32 __iomem *) (((char __iomem *)(dd->ipath_kregbase)) +
-				  dd->ipath_piobufbase);
-	pioincr = dd->ipath_palign / sizeof(*piobuf);
-	for (i = 0; i < dd->ipath_piobcnt2k; i++) {
-		/*
-		 * reasonable word count, just to init pbc
-		 */
-		writel(16, piobuf);
-		piobuf += pioincr;
-	}
-
-	ipath_get_eeprom_info(dd);
-	if (dd->ipath_boardrev == 5) {
-		/*
-		 * Later production QHT7040 has same changes as QHT7140, so
-		 * can use GPIO interrupts.  They have serial #'s starting
-		 * with 128, rather than 112.
-		 */
-		if (dd->ipath_serial[0] == '1' &&
-		    dd->ipath_serial[1] == '2' &&
-		    dd->ipath_serial[2] == '8')
-			dd->ipath_flags |= IPATH_GPIO_INTR;
-		else {
-			ipath_dev_err(dd, "Unsupported InfiniPath board "
-				"(serial number %.16s)!\n",
-				dd->ipath_serial);
-			return 1;
-		}
-	}
-
-	if (dd->ipath_minrev >= 4) {
-		/* Rev4+ reports extra errors via internal GPIO pins */
-		dd->ipath_flags |= IPATH_GPIO_ERRINTRS;
-		dd->ipath_gpio_mask |= IPATH_GPIO_ERRINTR_MASK;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_gpio_mask,
-				 dd->ipath_gpio_mask);
-	}
-
-	return 0;
-}
-
-
-/**
- * ipath_init_ht_get_base_info - set chip-specific flags for user code
- * @dd: the infinipath device
- * @kbase: ipath_base_info pointer
- *
- * We set the PCIE flag because the lower bandwidth on PCIe vs
- * HyperTransport can affect some user packet algorithms.
- */
-static int ipath_ht_get_base_info(struct ipath_portdata *pd, void *kbase)
-{
-	struct ipath_base_info *kinfo = kbase;
-
-	kinfo->spi_runtime_flags |= IPATH_RUNTIME_HT |
-		IPATH_RUNTIME_PIO_REGSWAPPED;
-
-	if (pd->port_dd->ipath_minrev < 4)
-		kinfo->spi_runtime_flags |= IPATH_RUNTIME_RCVHDR_COPY;
-
-	return 0;
-}
-
-static void ipath_ht_free_irq(struct ipath_devdata *dd)
-{
-	free_irq(dd->ipath_irq, dd);
-	ht_destroy_irq(dd->ipath_irq);
-	dd->ipath_irq = 0;
-	dd->ipath_intconfig = 0;
-}
-
-static struct ipath_message_header *
-ipath_ht_get_msgheader(struct ipath_devdata *dd, __le32 *rhf_addr)
-{
-	return (struct ipath_message_header *)
-		&rhf_addr[sizeof(u64) / sizeof(u32)];
-}
-
-static void ipath_ht_config_ports(struct ipath_devdata *dd, ushort cfgports)
-{
-	dd->ipath_portcnt =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_portcnt);
-	dd->ipath_p0_rcvegrcnt =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvegrcnt);
-}
-
-static void ipath_ht_read_counters(struct ipath_devdata *dd,
-				   struct infinipath_counters *cntrs)
-{
-	cntrs->LBIntCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(LBIntCnt));
-	cntrs->LBFlowStallCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(LBFlowStallCnt));
-	cntrs->TxSDmaDescCnt = 0;
-	cntrs->TxUnsupVLErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxUnsupVLErrCnt));
-	cntrs->TxDataPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxDataPktCnt));
-	cntrs->TxFlowPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxFlowPktCnt));
-	cntrs->TxDwordCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxDwordCnt));
-	cntrs->TxLenErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxLenErrCnt));
-	cntrs->TxMaxMinLenErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxMaxMinLenErrCnt));
-	cntrs->TxUnderrunCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxUnderrunCnt));
-	cntrs->TxFlowStallCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxFlowStallCnt));
-	cntrs->TxDroppedPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(TxDroppedPktCnt));
-	cntrs->RxDroppedPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxDroppedPktCnt));
-	cntrs->RxDataPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxDataPktCnt));
-	cntrs->RxFlowPktCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxFlowPktCnt));
-	cntrs->RxDwordCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxDwordCnt));
-	cntrs->RxLenErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxLenErrCnt));
-	cntrs->RxMaxMinLenErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxMaxMinLenErrCnt));
-	cntrs->RxICRCErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxICRCErrCnt));
-	cntrs->RxVCRCErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxVCRCErrCnt));
-	cntrs->RxFlowCtrlErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxFlowCtrlErrCnt));
-	cntrs->RxBadFormatCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxBadFormatCnt));
-	cntrs->RxLinkProblemCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxLinkProblemCnt));
-	cntrs->RxEBPCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxEBPCnt));
-	cntrs->RxLPCRCErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxLPCRCErrCnt));
-	cntrs->RxBufOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxBufOvflCnt));
-	cntrs->RxTIDFullErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxTIDFullErrCnt));
-	cntrs->RxTIDValidErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxTIDValidErrCnt));
-	cntrs->RxPKeyMismatchCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxPKeyMismatchCnt));
-	cntrs->RxP0HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP0HdrEgrOvflCnt));
-	cntrs->RxP1HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP1HdrEgrOvflCnt));
-	cntrs->RxP2HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP2HdrEgrOvflCnt));
-	cntrs->RxP3HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP3HdrEgrOvflCnt));
-	cntrs->RxP4HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP4HdrEgrOvflCnt));
-	cntrs->RxP5HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP5HdrEgrOvflCnt));
-	cntrs->RxP6HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP6HdrEgrOvflCnt));
-	cntrs->RxP7HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP7HdrEgrOvflCnt));
-	cntrs->RxP8HdrEgrOvflCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(RxP8HdrEgrOvflCnt));
-	cntrs->RxP9HdrEgrOvflCnt = 0;
-	cntrs->RxP10HdrEgrOvflCnt = 0;
-	cntrs->RxP11HdrEgrOvflCnt = 0;
-	cntrs->RxP12HdrEgrOvflCnt = 0;
-	cntrs->RxP13HdrEgrOvflCnt = 0;
-	cntrs->RxP14HdrEgrOvflCnt = 0;
-	cntrs->RxP15HdrEgrOvflCnt = 0;
-	cntrs->RxP16HdrEgrOvflCnt = 0;
-	cntrs->IBStatusChangeCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(IBStatusChangeCnt));
-	cntrs->IBLinkErrRecoveryCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(IBLinkErrRecoveryCnt));
-	cntrs->IBLinkDownedCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(IBLinkDownedCnt));
-	cntrs->IBSymbolErrCnt =
-		ipath_snap_cntr(dd, IPATH_CREG_OFFSET(IBSymbolErrCnt));
-	cntrs->RxVL15DroppedPktCnt = 0;
-	cntrs->RxOtherLocalPhyErrCnt = 0;
-	cntrs->PcieRetryBufDiagQwordCnt = 0;
-	cntrs->ExcessBufferOvflCnt = dd->ipath_overrun_thresh_errs;
-	cntrs->LocalLinkIntegrityErrCnt =
-		(dd->ipath_flags & IPATH_GPIO_ERRINTRS) ?
-		dd->ipath_lli_errs : dd->ipath_lli_errors;
-	cntrs->RxVlErrCnt = 0;
-	cntrs->RxDlidFltrCnt = 0;
-}
-
-
-/* no interrupt fallback for these chips */
-static int ipath_ht_nointr_fallback(struct ipath_devdata *dd)
-{
-	return 0;
-}
-
-
-/*
- * reset the XGXS (between serdes and IBC).  Slightly less intrusive
- * than resetting the IBC or external link state, and useful in some
- * cases to cause some retraining.  To do this right, we reset IBC
- * as well.
- */
-static void ipath_ht_xgxs_reset(struct ipath_devdata *dd)
-{
-	u64 val, prev_val;
-
-	prev_val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_xgxsconfig);
-	val = prev_val | INFINIPATH_XGXS_RESET;
-	prev_val &= ~INFINIPATH_XGXS_RESET; /* be sure */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-			 dd->ipath_control & ~INFINIPATH_C_LINKENABLE);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_xgxsconfig, val);
-	ipath_read_kreg32(dd, dd->ipath_kregs->kr_scratch);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_xgxsconfig, prev_val);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-			 dd->ipath_control);
-}
-
-
-static int ipath_ht_get_ib_cfg(struct ipath_devdata *dd, int which)
-{
-	int ret;
-
-	switch (which) {
-	case IPATH_IB_CFG_LWID:
-		ret = dd->ipath_link_width_active;
-		break;
-	case IPATH_IB_CFG_SPD:
-		ret = dd->ipath_link_speed_active;
-		break;
-	case IPATH_IB_CFG_LWID_ENB:
-		ret = dd->ipath_link_width_enabled;
-		break;
-	case IPATH_IB_CFG_SPD_ENB:
-		ret = dd->ipath_link_speed_enabled;
-		break;
-	default:
-		ret =  -ENOTSUPP;
-		break;
-	}
-	return ret;
-}
-
-
-/* we assume range checking is already done, if needed */
-static int ipath_ht_set_ib_cfg(struct ipath_devdata *dd, int which, u32 val)
-{
-	int ret = 0;
-
-	if (which == IPATH_IB_CFG_LWID_ENB)
-		dd->ipath_link_width_enabled = val;
-	else if (which == IPATH_IB_CFG_SPD_ENB)
-		dd->ipath_link_speed_enabled = val;
-	else
-		ret = -ENOTSUPP;
-	return ret;
-}
-
-
-static void ipath_ht_config_jint(struct ipath_devdata *dd, u16 a, u16 b)
-{
-}
-
-
-static int ipath_ht_ib_updown(struct ipath_devdata *dd, int ibup, u64 ibcs)
-{
-	ipath_setup_ht_setextled(dd, ipath_ib_linkstate(dd, ibcs),
-		ipath_ib_linktrstate(dd, ibcs));
-	return 0;
-}
-
-
-/**
- * ipath_init_iba6110_funcs - set up the chip-specific function pointers
- * @dd: the infinipath device
- *
- * This is global, and is called directly at init to set up the
- * chip-specific function pointers for later use.
- */
-void ipath_init_iba6110_funcs(struct ipath_devdata *dd)
-{
-	dd->ipath_f_intrsetup = ipath_ht_intconfig;
-	dd->ipath_f_bus = ipath_setup_ht_config;
-	dd->ipath_f_reset = ipath_setup_ht_reset;
-	dd->ipath_f_get_boardname = ipath_ht_boardname;
-	dd->ipath_f_init_hwerrors = ipath_ht_init_hwerrors;
-	dd->ipath_f_early_init = ipath_ht_early_init;
-	dd->ipath_f_handle_hwerrors = ipath_ht_handle_hwerrors;
-	dd->ipath_f_quiet_serdes = ipath_ht_quiet_serdes;
-	dd->ipath_f_bringup_serdes = ipath_ht_bringup_serdes;
-	dd->ipath_f_clear_tids = ipath_ht_clear_tids;
-	dd->ipath_f_put_tid = ipath_ht_put_tid;
-	dd->ipath_f_cleanup = ipath_setup_ht_cleanup;
-	dd->ipath_f_setextled = ipath_setup_ht_setextled;
-	dd->ipath_f_get_base_info = ipath_ht_get_base_info;
-	dd->ipath_f_free_irq = ipath_ht_free_irq;
-	dd->ipath_f_tidtemplate = ipath_ht_tidtemplate;
-	dd->ipath_f_intr_fallback = ipath_ht_nointr_fallback;
-	dd->ipath_f_get_msgheader = ipath_ht_get_msgheader;
-	dd->ipath_f_config_ports = ipath_ht_config_ports;
-	dd->ipath_f_read_counters = ipath_ht_read_counters;
-	dd->ipath_f_xgxs_reset = ipath_ht_xgxs_reset;
-	dd->ipath_f_get_ib_cfg = ipath_ht_get_ib_cfg;
-	dd->ipath_f_set_ib_cfg = ipath_ht_set_ib_cfg;
-	dd->ipath_f_config_jint = ipath_ht_config_jint;
-	dd->ipath_f_ib_updown = ipath_ht_ib_updown;
-
-	/*
-	 * initialize chip-specific variables
-	 */
-	ipath_init_ht_variables(dd);
-}
diff --git a/drivers/staging/rdma/ipath/ipath_init_chip.c b/drivers/staging/rdma/ipath/ipath_init_chip.c
deleted file mode 100644
index a5eea199f733..000000000000
--- a/drivers/staging/rdma/ipath/ipath_init_chip.c
+++ /dev/null
@@ -1,1062 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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/pci.h>
-#include <linux/netdevice.h>
-#include <linux/moduleparam.h>
-#include <linux/slab.h>
-#include <linux/stat.h>
-#include <linux/vmalloc.h>
-
-#include "ipath_kernel.h"
-#include "ipath_common.h"
-
-/*
- * min buffers we want to have per port, after driver
- */
-#define IPATH_MIN_USER_PORT_BUFCNT 7
-
-/*
- * Number of ports we are configured to use (to allow for more pio
- * buffers per port, etc.)  Zero means use chip value.
- */
-static ushort ipath_cfgports;
-
-module_param_named(cfgports, ipath_cfgports, ushort, S_IRUGO);
-MODULE_PARM_DESC(cfgports, "Set max number of ports to use");
-
-/*
- * Number of buffers reserved for driver (verbs and layered drivers.)
- * Initialized based on number of PIO buffers if not set via module interface.
- * The problem with this is that it's global, but we'll use different
- * numbers for different chip types.
- */
-static ushort ipath_kpiobufs;
-
-static int ipath_set_kpiobufs(const char *val, struct kernel_param *kp);
-
-module_param_call(kpiobufs, ipath_set_kpiobufs, param_get_ushort,
-		  &ipath_kpiobufs, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(kpiobufs, "Set number of PIO buffers for driver");
-
-/**
- * create_port0_egr - allocate the eager TID buffers
- * @dd: the infinipath device
- *
- * This code is now quite different for user and kernel, because
- * the kernel uses skb's, for the accelerated network performance.
- * This is the kernel (port0) version.
- *
- * Allocate the eager TID buffers and program them into infinipath.
- * We use the network layer alloc_skb() allocator to allocate the
- * memory, and either use the buffers as is for things like verbs
- * packets, or pass the buffers up to the ipath layered driver and
- * thence the network layer, replacing them as we do so (see
- * ipath_rcv_layer()).
- */
-static int create_port0_egr(struct ipath_devdata *dd)
-{
-	unsigned e, egrcnt;
-	struct ipath_skbinfo *skbinfo;
-	int ret;
-
-	egrcnt = dd->ipath_p0_rcvegrcnt;
-
-	skbinfo = vmalloc(sizeof(*dd->ipath_port0_skbinfo) * egrcnt);
-	if (skbinfo == NULL) {
-		ipath_dev_err(dd, "allocation error for eager TID "
-			      "skb array\n");
-		ret = -ENOMEM;
-		goto bail;
-	}
-	for (e = 0; e < egrcnt; e++) {
-		/*
-		 * This is a bit tricky in that we allocate extra
-		 * space for 2 bytes of the 14 byte ethernet header.
-		 * These two bytes are passed in the ipath header so
-		 * the rest of the data is word aligned.  We allocate
-		 * 4 bytes so that the data buffer stays word aligned.
-		 * See ipath_kreceive() for more details.
-		 */
-		skbinfo[e].skb = ipath_alloc_skb(dd, GFP_KERNEL);
-		if (!skbinfo[e].skb) {
-			ipath_dev_err(dd, "SKB allocation error for "
-				      "eager TID %u\n", e);
-			while (e != 0)
-				dev_kfree_skb(skbinfo[--e].skb);
-			vfree(skbinfo);
-			ret = -ENOMEM;
-			goto bail;
-		}
-	}
-	/*
-	 * After loop above, so we can test non-NULL to see if ready
-	 * to use at receive, etc.
-	 */
-	dd->ipath_port0_skbinfo = skbinfo;
-
-	for (e = 0; e < egrcnt; e++) {
-		dd->ipath_port0_skbinfo[e].phys =
-		  ipath_map_single(dd->pcidev,
-				   dd->ipath_port0_skbinfo[e].skb->data,
-				   dd->ipath_ibmaxlen, PCI_DMA_FROMDEVICE);
-		dd->ipath_f_put_tid(dd, e + (u64 __iomem *)
-				    ((char __iomem *) dd->ipath_kregbase +
-				     dd->ipath_rcvegrbase),
-				    RCVHQ_RCV_TYPE_EAGER,
-				    dd->ipath_port0_skbinfo[e].phys);
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-static int bringup_link(struct ipath_devdata *dd)
-{
-	u64 val, ibc;
-	int ret = 0;
-
-	/* hold IBC in reset */
-	dd->ipath_control &= ~INFINIPATH_C_LINKENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-			 dd->ipath_control);
-
-	/*
-	 * set initial max size pkt IBC will send, including ICRC; it's the
-	 * PIO buffer size in dwords, less 1; also see ipath_set_mtu()
-	 */
-	val = (dd->ipath_ibmaxlen >> 2) + 1;
-	ibc = val << dd->ibcc_mpl_shift;
-
-	/* flowcontrolwatermark is in units of KBytes */
-	ibc |= 0x5ULL << INFINIPATH_IBCC_FLOWCTRLWATERMARK_SHIFT;
-	/*
-	 * How often flowctrl sent.  More or less in usecs; balance against
-	 * watermark value, so that in theory senders always get a flow
-	 * control update in time to not let the IB link go idle.
-	 */
-	ibc |= 0x3ULL << INFINIPATH_IBCC_FLOWCTRLPERIOD_SHIFT;
-	/* max error tolerance */
-	ibc |= 0xfULL << INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT;
-	/* use "real" buffer space for */
-	ibc |= 4ULL << INFINIPATH_IBCC_CREDITSCALE_SHIFT;
-	/* IB credit flow control. */
-	ibc |= 0xfULL << INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT;
-	/* initially come up waiting for TS1, without sending anything. */
-	dd->ipath_ibcctrl = ibc;
-	/*
-	 * Want to start out with both LINKCMD and LINKINITCMD in NOP
-	 * (0 and 0).  Don't put linkinitcmd in ipath_ibcctrl, want that
-	 * to stay a NOP. Flag that we are disabled, for the (unlikely)
-	 * case that some recovery path is trying to bring the link up
-	 * before we are ready.
-	 */
-	ibc |= INFINIPATH_IBCC_LINKINITCMD_DISABLE <<
-		INFINIPATH_IBCC_LINKINITCMD_SHIFT;
-	dd->ipath_flags |= IPATH_IB_LINK_DISABLED;
-	ipath_cdbg(VERBOSE, "Writing 0x%llx to ibcctrl\n",
-		   (unsigned long long) ibc);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl, ibc);
-
-	// be sure chip saw it
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-
-	ret = dd->ipath_f_bringup_serdes(dd);
-
-	if (ret)
-		dev_info(&dd->pcidev->dev, "Could not initialize SerDes, "
-			 "not usable\n");
-	else {
-		/* enable IBC */
-		dd->ipath_control |= INFINIPATH_C_LINKENABLE;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-				 dd->ipath_control);
-	}
-
-	return ret;
-}
-
-static struct ipath_portdata *create_portdata0(struct ipath_devdata *dd)
-{
-	struct ipath_portdata *pd;
-
-	pd = kzalloc(sizeof(*pd), GFP_KERNEL);
-	if (pd) {
-		pd->port_dd = dd;
-		pd->port_cnt = 1;
-		/* The port 0 pkey table is used by the layer interface. */
-		pd->port_pkeys[0] = IPATH_DEFAULT_P_KEY;
-		pd->port_seq_cnt = 1;
-	}
-	return pd;
-}
-
-static int init_chip_first(struct ipath_devdata *dd)
-{
-	struct ipath_portdata *pd;
-	int ret = 0;
-	u64 val;
-
-	spin_lock_init(&dd->ipath_kernel_tid_lock);
-	spin_lock_init(&dd->ipath_user_tid_lock);
-	spin_lock_init(&dd->ipath_sendctrl_lock);
-	spin_lock_init(&dd->ipath_uctxt_lock);
-	spin_lock_init(&dd->ipath_sdma_lock);
-	spin_lock_init(&dd->ipath_gpio_lock);
-	spin_lock_init(&dd->ipath_eep_st_lock);
-	spin_lock_init(&dd->ipath_sdepb_lock);
-	mutex_init(&dd->ipath_eep_lock);
-
-	/*
-	 * skip cfgports stuff because we are not allocating memory,
-	 * and we don't want problems if the portcnt changed due to
-	 * cfgports.  We do still check and report a difference, if
-	 * not same (should be impossible).
-	 */
-	dd->ipath_f_config_ports(dd, ipath_cfgports);
-	if (!ipath_cfgports)
-		dd->ipath_cfgports = dd->ipath_portcnt;
-	else if (ipath_cfgports <= dd->ipath_portcnt) {
-		dd->ipath_cfgports = ipath_cfgports;
-		ipath_dbg("Configured to use %u ports out of %u in chip\n",
-			  dd->ipath_cfgports, ipath_read_kreg32(dd,
-			  dd->ipath_kregs->kr_portcnt));
-	} else {
-		dd->ipath_cfgports = dd->ipath_portcnt;
-		ipath_dbg("Tried to configured to use %u ports; chip "
-			  "only supports %u\n", ipath_cfgports,
-			  ipath_read_kreg32(dd,
-				  dd->ipath_kregs->kr_portcnt));
-	}
-	/*
-	 * Allocate full portcnt array, rather than just cfgports, because
-	 * cleanup iterates across all possible ports.
-	 */
-	dd->ipath_pd = kcalloc(dd->ipath_portcnt, sizeof(*dd->ipath_pd),
-			       GFP_KERNEL);
-
-	if (!dd->ipath_pd) {
-		ipath_dev_err(dd, "Unable to allocate portdata array, "
-			      "failing\n");
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	pd = create_portdata0(dd);
-	if (!pd) {
-		ipath_dev_err(dd, "Unable to allocate portdata for port "
-			      "0, failing\n");
-		ret = -ENOMEM;
-		goto done;
-	}
-	dd->ipath_pd[0] = pd;
-
-	dd->ipath_rcvtidcnt =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvtidcnt);
-	dd->ipath_rcvtidbase =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvtidbase);
-	dd->ipath_rcvegrcnt =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvegrcnt);
-	dd->ipath_rcvegrbase =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvegrbase);
-	dd->ipath_palign =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_pagealign);
-	dd->ipath_piobufbase =
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_sendpiobufbase);
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_sendpiosize);
-	dd->ipath_piosize2k = val & ~0U;
-	dd->ipath_piosize4k = val >> 32;
-	if (dd->ipath_piosize4k == 0 && ipath_mtu4096)
-		ipath_mtu4096 = 0; /* 4KB not supported by this chip */
-	dd->ipath_ibmtu = ipath_mtu4096 ? 4096 : 2048;
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_sendpiobufcnt);
-	dd->ipath_piobcnt2k = val & ~0U;
-	dd->ipath_piobcnt4k = val >> 32;
-	dd->ipath_pio2kbase =
-		(u32 __iomem *) (((char __iomem *) dd->ipath_kregbase) +
-				 (dd->ipath_piobufbase & 0xffffffff));
-	if (dd->ipath_piobcnt4k) {
-		dd->ipath_pio4kbase = (u32 __iomem *)
-			(((char __iomem *) dd->ipath_kregbase) +
-			 (dd->ipath_piobufbase >> 32));
-		/*
-		 * 4K buffers take 2 pages; we use roundup just to be
-		 * paranoid; we calculate it once here, rather than on
-		 * ever buf allocate
-		 */
-		dd->ipath_4kalign = ALIGN(dd->ipath_piosize4k,
-					  dd->ipath_palign);
-		ipath_dbg("%u 2k(%x) piobufs @ %p, %u 4k(%x) @ %p "
-			  "(%x aligned)\n",
-			  dd->ipath_piobcnt2k, dd->ipath_piosize2k,
-			  dd->ipath_pio2kbase, dd->ipath_piobcnt4k,
-			  dd->ipath_piosize4k, dd->ipath_pio4kbase,
-			  dd->ipath_4kalign);
-	} else {
-		ipath_dbg("%u 2k piobufs @ %p\n",
-			  dd->ipath_piobcnt2k, dd->ipath_pio2kbase);
-	}
-done:
-	return ret;
-}
-
-/**
- * init_chip_reset - re-initialize after a reset, or enable
- * @dd: the infinipath device
- *
- * sanity check at least some of the values after reset, and
- * ensure no receive or transmit (explicitly, in case reset
- * failed
- */
-static int init_chip_reset(struct ipath_devdata *dd)
-{
-	u32 rtmp;
-	int i;
-	unsigned long flags;
-
-	/*
-	 * ensure chip does no sends or receives, tail updates, or
-	 * pioavail updates while we re-initialize
-	 */
-	dd->ipath_rcvctrl &= ~(1ULL << dd->ipath_r_tailupd_shift);
-	for (i = 0; i < dd->ipath_portcnt; i++) {
-		clear_bit(dd->ipath_r_portenable_shift + i,
-			  &dd->ipath_rcvctrl);
-		clear_bit(dd->ipath_r_intravail_shift + i,
-			  &dd->ipath_rcvctrl);
-	}
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-		dd->ipath_rcvctrl);
-
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl = 0U; /* no sdma, etc */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control, 0ULL);
-
-	rtmp = ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvtidcnt);
-	if (rtmp != dd->ipath_rcvtidcnt)
-		dev_info(&dd->pcidev->dev, "tidcnt was %u before "
-			 "reset, now %u, using original\n",
-			 dd->ipath_rcvtidcnt, rtmp);
-	rtmp = ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvtidbase);
-	if (rtmp != dd->ipath_rcvtidbase)
-		dev_info(&dd->pcidev->dev, "tidbase was %u before "
-			 "reset, now %u, using original\n",
-			 dd->ipath_rcvtidbase, rtmp);
-	rtmp = ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvegrcnt);
-	if (rtmp != dd->ipath_rcvegrcnt)
-		dev_info(&dd->pcidev->dev, "egrcnt was %u before "
-			 "reset, now %u, using original\n",
-			 dd->ipath_rcvegrcnt, rtmp);
-	rtmp = ipath_read_kreg32(dd, dd->ipath_kregs->kr_rcvegrbase);
-	if (rtmp != dd->ipath_rcvegrbase)
-		dev_info(&dd->pcidev->dev, "egrbase was %u before "
-			 "reset, now %u, using original\n",
-			 dd->ipath_rcvegrbase, rtmp);
-
-	return 0;
-}
-
-static int init_pioavailregs(struct ipath_devdata *dd)
-{
-	int ret;
-
-	dd->ipath_pioavailregs_dma = dma_alloc_coherent(
-		&dd->pcidev->dev, PAGE_SIZE, &dd->ipath_pioavailregs_phys,
-		GFP_KERNEL);
-	if (!dd->ipath_pioavailregs_dma) {
-		ipath_dev_err(dd, "failed to allocate PIOavail reg area "
-			      "in memory\n");
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	/*
-	 * we really want L2 cache aligned, but for current CPUs of
-	 * interest, they are the same.
-	 */
-	dd->ipath_statusp = (u64 *)
-		((char *)dd->ipath_pioavailregs_dma +
-		 ((2 * L1_CACHE_BYTES +
-		   dd->ipath_pioavregs * sizeof(u64)) & ~L1_CACHE_BYTES));
-	/* copy the current value now that it's really allocated */
-	*dd->ipath_statusp = dd->_ipath_status;
-	/*
-	 * setup buffer to hold freeze msg, accessible to apps,
-	 * following statusp
-	 */
-	dd->ipath_freezemsg = (char *)&dd->ipath_statusp[1];
-	/* and its length */
-	dd->ipath_freezelen = L1_CACHE_BYTES - sizeof(dd->ipath_statusp[0]);
-
-	ret = 0;
-
-done:
-	return ret;
-}
-
-/**
- * init_shadow_tids - allocate the shadow TID array
- * @dd: the infinipath device
- *
- * allocate the shadow TID array, so we can ipath_munlock previous
- * entries.  It may make more sense to move the pageshadow to the
- * port data structure, so we only allocate memory for ports actually
- * in use, since we at 8k per port, now.
- */
-static void init_shadow_tids(struct ipath_devdata *dd)
-{
-	struct page **pages;
-	dma_addr_t *addrs;
-
-	pages = vzalloc(dd->ipath_cfgports * dd->ipath_rcvtidcnt *
-			sizeof(struct page *));
-	if (!pages) {
-		ipath_dev_err(dd, "failed to allocate shadow page * "
-			      "array, no expected sends!\n");
-		dd->ipath_pageshadow = NULL;
-		return;
-	}
-
-	addrs = vmalloc(dd->ipath_cfgports * dd->ipath_rcvtidcnt *
-			sizeof(dma_addr_t));
-	if (!addrs) {
-		ipath_dev_err(dd, "failed to allocate shadow dma handle "
-			      "array, no expected sends!\n");
-		vfree(pages);
-		dd->ipath_pageshadow = NULL;
-		return;
-	}
-
-	dd->ipath_pageshadow = pages;
-	dd->ipath_physshadow = addrs;
-}
-
-static void enable_chip(struct ipath_devdata *dd, int reinit)
-{
-	u32 val;
-	u64 rcvmask;
-	unsigned long flags;
-	int i;
-
-	if (!reinit)
-		init_waitqueue_head(&ipath_state_wait);
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			 dd->ipath_rcvctrl);
-
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	/* Enable PIO send, and update of PIOavail regs to memory. */
-	dd->ipath_sendctrl = INFINIPATH_S_PIOENABLE |
-		INFINIPATH_S_PIOBUFAVAILUPD;
-
-	/*
-	 * Set the PIO avail update threshold to host memory
-	 * on chips that support it.
-	 */
-	if (dd->ipath_pioupd_thresh)
-		dd->ipath_sendctrl |= dd->ipath_pioupd_thresh
-			<< INFINIPATH_S_UPDTHRESH_SHIFT;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	/*
-	 * Enable kernel ports' receive and receive interrupt.
-	 * Other ports done as user opens and inits them.
-	 */
-	rcvmask = 1ULL;
-	dd->ipath_rcvctrl |= (rcvmask << dd->ipath_r_portenable_shift) |
-		(rcvmask << dd->ipath_r_intravail_shift);
-	if (!(dd->ipath_flags & IPATH_NODMA_RTAIL))
-		dd->ipath_rcvctrl |= (1ULL << dd->ipath_r_tailupd_shift);
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-			 dd->ipath_rcvctrl);
-
-	/*
-	 * now ready for use.  this should be cleared whenever we
-	 * detect a reset, or initiate one.
-	 */
-	dd->ipath_flags |= IPATH_INITTED;
-
-	/*
-	 * Init our shadow copies of head from tail values,
-	 * and write head values to match.
-	 */
-	val = ipath_read_ureg32(dd, ur_rcvegrindextail, 0);
-	ipath_write_ureg(dd, ur_rcvegrindexhead, val, 0);
-
-	/* Initialize so we interrupt on next packet received */
-	ipath_write_ureg(dd, ur_rcvhdrhead,
-			 dd->ipath_rhdrhead_intr_off |
-			 dd->ipath_pd[0]->port_head, 0);
-
-	/*
-	 * by now pioavail updates to memory should have occurred, so
-	 * copy them into our working/shadow registers; this is in
-	 * case something went wrong with abort, but mostly to get the
-	 * initial values of the generation bit correct.
-	 */
-	for (i = 0; i < dd->ipath_pioavregs; i++) {
-		__le64 pioavail;
-
-		/*
-		 * Chip Errata bug 6641; even and odd qwords>3 are swapped.
-		 */
-		if (i > 3 && (dd->ipath_flags & IPATH_SWAP_PIOBUFS))
-			pioavail = dd->ipath_pioavailregs_dma[i ^ 1];
-		else
-			pioavail = dd->ipath_pioavailregs_dma[i];
-		/*
-		 * don't need to worry about ipath_pioavailkernel here
-		 * because we will call ipath_chg_pioavailkernel() later
-		 * in initialization, to busy out buffers as needed
-		 */
-		dd->ipath_pioavailshadow[i] = le64_to_cpu(pioavail);
-	}
-	/* can get counters, stats, etc. */
-	dd->ipath_flags |= IPATH_PRESENT;
-}
-
-static int init_housekeeping(struct ipath_devdata *dd, int reinit)
-{
-	char boardn[40];
-	int ret = 0;
-
-	/*
-	 * have to clear shadow copies of registers at init that are
-	 * not otherwise set here, or all kinds of bizarre things
-	 * happen with driver on chip reset
-	 */
-	dd->ipath_rcvhdrsize = 0;
-
-	/*
-	 * Don't clear ipath_flags as 8bit mode was set before
-	 * entering this func. However, we do set the linkstate to
-	 * unknown, so we can watch for a transition.
-	 * PRESENT is set because we want register reads to work,
-	 * and the kernel infrastructure saw it in config space;
-	 * We clear it if we have failures.
-	 */
-	dd->ipath_flags |= IPATH_LINKUNK | IPATH_PRESENT;
-	dd->ipath_flags &= ~(IPATH_LINKACTIVE | IPATH_LINKARMED |
-			     IPATH_LINKDOWN | IPATH_LINKINIT);
-
-	ipath_cdbg(VERBOSE, "Try to read spc chip revision\n");
-	dd->ipath_revision =
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_revision);
-
-	/*
-	 * set up fundamental info we need to use the chip; we assume
-	 * if the revision reg and these regs are OK, we don't need to
-	 * special case the rest
-	 */
-	dd->ipath_sregbase =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_sendregbase);
-	dd->ipath_cregbase =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_counterregbase);
-	dd->ipath_uregbase =
-		ipath_read_kreg32(dd, dd->ipath_kregs->kr_userregbase);
-	ipath_cdbg(VERBOSE, "ipath_kregbase %p, sendbase %x usrbase %x, "
-		   "cntrbase %x\n", dd->ipath_kregbase, dd->ipath_sregbase,
-		   dd->ipath_uregbase, dd->ipath_cregbase);
-	if ((dd->ipath_revision & 0xffffffff) == 0xffffffff
-	    || (dd->ipath_sregbase & 0xffffffff) == 0xffffffff
-	    || (dd->ipath_cregbase & 0xffffffff) == 0xffffffff
-	    || (dd->ipath_uregbase & 0xffffffff) == 0xffffffff) {
-		ipath_dev_err(dd, "Register read failures from chip, "
-			      "giving up initialization\n");
-		dd->ipath_flags &= ~IPATH_PRESENT;
-		ret = -ENODEV;
-		goto done;
-	}
-
-
-	/* clear diagctrl register, in case diags were running and crashed */
-	ipath_write_kreg (dd, dd->ipath_kregs->kr_hwdiagctrl, 0);
-
-	/* clear the initial reset flag, in case first driver load */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear,
-			 INFINIPATH_E_RESET);
-
-	ipath_cdbg(VERBOSE, "Revision %llx (PCI %x)\n",
-		   (unsigned long long) dd->ipath_revision,
-		   dd->ipath_pcirev);
-
-	if (((dd->ipath_revision >> INFINIPATH_R_SOFTWARE_SHIFT) &
-	     INFINIPATH_R_SOFTWARE_MASK) != IPATH_CHIP_SWVERSION) {
-		ipath_dev_err(dd, "Driver only handles version %d, "
-			      "chip swversion is %d (%llx), failng\n",
-			      IPATH_CHIP_SWVERSION,
-			      (int)(dd->ipath_revision >>
-				    INFINIPATH_R_SOFTWARE_SHIFT) &
-			      INFINIPATH_R_SOFTWARE_MASK,
-			      (unsigned long long) dd->ipath_revision);
-		ret = -ENOSYS;
-		goto done;
-	}
-	dd->ipath_majrev = (u8) ((dd->ipath_revision >>
-				  INFINIPATH_R_CHIPREVMAJOR_SHIFT) &
-				 INFINIPATH_R_CHIPREVMAJOR_MASK);
-	dd->ipath_minrev = (u8) ((dd->ipath_revision >>
-				  INFINIPATH_R_CHIPREVMINOR_SHIFT) &
-				 INFINIPATH_R_CHIPREVMINOR_MASK);
-	dd->ipath_boardrev = (u8) ((dd->ipath_revision >>
-				    INFINIPATH_R_BOARDID_SHIFT) &
-				   INFINIPATH_R_BOARDID_MASK);
-
-	ret = dd->ipath_f_get_boardname(dd, boardn, sizeof boardn);
-
-	snprintf(dd->ipath_boardversion, sizeof(dd->ipath_boardversion),
-		 "ChipABI %u.%u, %s, InfiniPath%u %u.%u, PCI %u, "
-		 "SW Compat %u\n",
-		 IPATH_CHIP_VERS_MAJ, IPATH_CHIP_VERS_MIN, boardn,
-		 (unsigned)(dd->ipath_revision >> INFINIPATH_R_ARCH_SHIFT) &
-		 INFINIPATH_R_ARCH_MASK,
-		 dd->ipath_majrev, dd->ipath_minrev, dd->ipath_pcirev,
-		 (unsigned)(dd->ipath_revision >>
-			    INFINIPATH_R_SOFTWARE_SHIFT) &
-		 INFINIPATH_R_SOFTWARE_MASK);
-
-	ipath_dbg("%s", dd->ipath_boardversion);
-
-	if (ret)
-		goto done;
-
-	if (reinit)
-		ret = init_chip_reset(dd);
-	else
-		ret = init_chip_first(dd);
-
-done:
-	return ret;
-}
-
-static void verify_interrupt(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *) opaque;
-
-	if (!dd)
-		return; /* being torn down */
-
-	/*
-	 * If we don't have any interrupts, let the user know and
-	 * don't bother checking again.
-	 */
-	if (dd->ipath_int_counter == 0) {
-		if (!dd->ipath_f_intr_fallback(dd))
-			dev_err(&dd->pcidev->dev, "No interrupts detected, "
-				"not usable.\n");
-		else /* re-arm the timer to see if fallback works */
-			mod_timer(&dd->ipath_intrchk_timer, jiffies + HZ/2);
-	} else
-		ipath_cdbg(VERBOSE, "%u interrupts at timer check\n",
-			dd->ipath_int_counter);
-}
-
-/**
- * ipath_init_chip - do the actual initialization sequence on the chip
- * @dd: the infinipath device
- * @reinit: reinitializing, so don't allocate new memory
- *
- * Do the actual initialization sequence on the chip.  This is done
- * both from the init routine called from the PCI infrastructure, and
- * when we reset the chip, or detect that it was reset internally,
- * or it's administratively re-enabled.
- *
- * Memory allocation here and in called routines is only done in
- * the first case (reinit == 0).  We have to be careful, because even
- * without memory allocation, we need to re-write all the chip registers
- * TIDs, etc. after the reset or enable has completed.
- */
-int ipath_init_chip(struct ipath_devdata *dd, int reinit)
-{
-	int ret = 0;
-	u32 kpiobufs, defkbufs;
-	u32 piobufs, uports;
-	u64 val;
-	struct ipath_portdata *pd;
-	gfp_t gfp_flags = GFP_USER | __GFP_COMP;
-
-	ret = init_housekeeping(dd, reinit);
-	if (ret)
-		goto done;
-
-	/*
-	 * We could bump this to allow for full rcvegrcnt + rcvtidcnt,
-	 * but then it no longer nicely fits power of two, and since
-	 * we now use routines that backend onto __get_free_pages, the
-	 * rest would be wasted.
-	 */
-	dd->ipath_rcvhdrcnt = max(dd->ipath_p0_rcvegrcnt, dd->ipath_rcvegrcnt);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvhdrcnt,
-			 dd->ipath_rcvhdrcnt);
-
-	/*
-	 * Set up the shadow copies of the piobufavail registers,
-	 * which we compare against the chip registers for now, and
-	 * the in memory DMA'ed copies of the registers.  This has to
-	 * be done early, before we calculate lastport, etc.
-	 */
-	piobufs = dd->ipath_piobcnt2k + dd->ipath_piobcnt4k;
-	/*
-	 * calc number of pioavail registers, and save it; we have 2
-	 * bits per buffer.
-	 */
-	dd->ipath_pioavregs = ALIGN(piobufs, sizeof(u64) * BITS_PER_BYTE / 2)
-		/ (sizeof(u64) * BITS_PER_BYTE / 2);
-	uports = dd->ipath_cfgports ? dd->ipath_cfgports - 1 : 0;
-	if (piobufs > 144)
-		defkbufs = 32 + dd->ipath_pioreserved;
-	else
-		defkbufs = 16 + dd->ipath_pioreserved;
-
-	if (ipath_kpiobufs && (ipath_kpiobufs +
-		(uports * IPATH_MIN_USER_PORT_BUFCNT)) > piobufs) {
-		int i = (int) piobufs -
-			(int) (uports * IPATH_MIN_USER_PORT_BUFCNT);
-		if (i < 1)
-			i = 1;
-		dev_info(&dd->pcidev->dev, "Allocating %d PIO bufs of "
-			 "%d for kernel leaves too few for %d user ports "
-			 "(%d each); using %u\n", ipath_kpiobufs,
-			 piobufs, uports, IPATH_MIN_USER_PORT_BUFCNT, i);
-		/*
-		 * shouldn't change ipath_kpiobufs, because could be
-		 * different for different devices...
-		 */
-		kpiobufs = i;
-	} else if (ipath_kpiobufs)
-		kpiobufs = ipath_kpiobufs;
-	else
-		kpiobufs = defkbufs;
-	dd->ipath_lastport_piobuf = piobufs - kpiobufs;
-	dd->ipath_pbufsport =
-		uports ? dd->ipath_lastport_piobuf / uports : 0;
-	/* if not an even divisor, some user ports get extra buffers */
-	dd->ipath_ports_extrabuf = dd->ipath_lastport_piobuf -
-		(dd->ipath_pbufsport * uports);
-	if (dd->ipath_ports_extrabuf)
-		ipath_dbg("%u pbufs/port leaves some unused, add 1 buffer to "
-			"ports <= %u\n", dd->ipath_pbufsport,
-			dd->ipath_ports_extrabuf);
-	dd->ipath_lastpioindex = 0;
-	dd->ipath_lastpioindexl = dd->ipath_piobcnt2k;
-	/* ipath_pioavailshadow initialized earlier */
-	ipath_cdbg(VERBOSE, "%d PIO bufs for kernel out of %d total %u "
-		   "each for %u user ports\n", kpiobufs,
-		   piobufs, dd->ipath_pbufsport, uports);
-	ret = dd->ipath_f_early_init(dd);
-	if (ret) {
-		ipath_dev_err(dd, "Early initialization failure\n");
-		goto done;
-	}
-
-	/*
-	 * Early_init sets rcvhdrentsize and rcvhdrsize, so this must be
-	 * done after early_init.
-	 */
-	dd->ipath_hdrqlast =
-		dd->ipath_rcvhdrentsize * (dd->ipath_rcvhdrcnt - 1);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvhdrentsize,
-			 dd->ipath_rcvhdrentsize);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvhdrsize,
-			 dd->ipath_rcvhdrsize);
-
-	if (!reinit) {
-		ret = init_pioavailregs(dd);
-		init_shadow_tids(dd);
-		if (ret)
-			goto done;
-	}
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendpioavailaddr,
-			 dd->ipath_pioavailregs_phys);
-
-	/*
-	 * this is to detect s/w errors, which the h/w works around by
-	 * ignoring the low 6 bits of address, if it wasn't aligned.
-	 */
-	val = ipath_read_kreg64(dd, dd->ipath_kregs->kr_sendpioavailaddr);
-	if (val != dd->ipath_pioavailregs_phys) {
-		ipath_dev_err(dd, "Catastrophic software error, "
-			      "SendPIOAvailAddr written as %lx, "
-			      "read back as %llx\n",
-			      (unsigned long) dd->ipath_pioavailregs_phys,
-			      (unsigned long long) val);
-		ret = -EINVAL;
-		goto done;
-	}
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvbthqp, IPATH_KD_QP);
-
-	/*
-	 * make sure we are not in freeze, and PIO send enabled, so
-	 * writes to pbc happen
-	 */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask, 0ULL);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear,
-			 ~0ULL&~INFINIPATH_HWE_MEMBISTFAILED);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control, 0ULL);
-
-	/*
-	 * before error clears, since we expect serdes pll errors during
-	 * this, the first time after reset
-	 */
-	if (bringup_link(dd)) {
-		dev_info(&dd->pcidev->dev, "Failed to bringup IB link\n");
-		ret = -ENETDOWN;
-		goto done;
-	}
-
-	/*
-	 * clear any "expected" hwerrs from reset and/or initialization
-	 * clear any that aren't enabled (at least this once), and then
-	 * set the enable mask
-	 */
-	dd->ipath_f_init_hwerrors(dd);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear,
-			 ~0ULL&~INFINIPATH_HWE_MEMBISTFAILED);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrmask,
-			 dd->ipath_hwerrmask);
-
-	/* clear all */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear, -1LL);
-	/* enable errors that are masked, at least this first time. */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-			 ~dd->ipath_maskederrs);
-	dd->ipath_maskederrs = 0; /* don't re-enable ignored in timer */
-	dd->ipath_errormask =
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_errormask);
-	/* clear any interrupts up to this point (ints still not enabled) */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, -1LL);
-
-	dd->ipath_f_tidtemplate(dd);
-
-	/*
-	 * Set up the port 0 (kernel) rcvhdr q and egr TIDs.  If doing
-	 * re-init, the simplest way to handle this is to free
-	 * existing, and re-allocate.
-	 * Need to re-create rest of port 0 portdata as well.
-	 */
-	pd = dd->ipath_pd[0];
-	if (reinit) {
-		struct ipath_portdata *npd;
-
-		/*
-		 * Alloc and init new ipath_portdata for port0,
-		 * Then free old pd. Could lead to fragmentation, but also
-		 * makes later support for hot-swap easier.
-		 */
-		npd = create_portdata0(dd);
-		if (npd) {
-			ipath_free_pddata(dd, pd);
-			dd->ipath_pd[0] = npd;
-			pd = npd;
-		} else {
-			ipath_dev_err(dd, "Unable to allocate portdata"
-				      " for port 0, failing\n");
-			ret = -ENOMEM;
-			goto done;
-		}
-	}
-	ret = ipath_create_rcvhdrq(dd, pd);
-	if (!ret)
-		ret = create_port0_egr(dd);
-	if (ret) {
-		ipath_dev_err(dd, "failed to allocate kernel port's "
-			      "rcvhdrq and/or egr bufs\n");
-		goto done;
-	} else {
-		enable_chip(dd, reinit);
-	}
-
-	/* after enable_chip, so pioavailshadow setup */
-	ipath_chg_pioavailkernel(dd, 0, piobufs, 1);
-
-	/*
-	 * Cancel any possible active sends from early driver load.
-	 * Follows early_init because some chips have to initialize
-	 * PIO buffers in early_init to avoid false parity errors.
-	 * After enable and ipath_chg_pioavailkernel so we can safely
-	 * enable pioavail updates and PIOENABLE; packets are now
-	 * ready to go out.
-	 */
-	ipath_cancel_sends(dd, 1);
-
-	if (!reinit) {
-		/*
-		 * Used when we close a port, for DMA already in flight
-		 * at close.
-		 */
-		dd->ipath_dummy_hdrq = dma_alloc_coherent(
-			&dd->pcidev->dev, dd->ipath_pd[0]->port_rcvhdrq_size,
-			&dd->ipath_dummy_hdrq_phys,
-			gfp_flags);
-		if (!dd->ipath_dummy_hdrq) {
-			dev_info(&dd->pcidev->dev,
-				"Couldn't allocate 0x%lx bytes for dummy hdrq\n",
-				dd->ipath_pd[0]->port_rcvhdrq_size);
-			/* fallback to just 0'ing */
-			dd->ipath_dummy_hdrq_phys = 0UL;
-		}
-	}
-
-	/*
-	 * cause retrigger of pending interrupts ignored during init,
-	 * even if we had errors
-	 */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, 0ULL);
-
-	if (!dd->ipath_stats_timer_active) {
-		/*
-		 * first init, or after an admin disable/enable
-		 * set up stats retrieval timer, even if we had errors
-		 * in last portion of setup
-		 */
-		setup_timer(&dd->ipath_stats_timer, ipath_get_faststats,
-				(unsigned long)dd);
-		/* every 5 seconds; */
-		dd->ipath_stats_timer.expires = jiffies + 5 * HZ;
-		/* takes ~16 seconds to overflow at full IB 4x bandwdith */
-		add_timer(&dd->ipath_stats_timer);
-		dd->ipath_stats_timer_active = 1;
-	}
-
-	/* Set up SendDMA if chip supports it */
-	if (dd->ipath_flags & IPATH_HAS_SEND_DMA)
-		ret = setup_sdma(dd);
-
-	/* Set up HoL state */
-	setup_timer(&dd->ipath_hol_timer, ipath_hol_event, (unsigned long)dd);
-
-	dd->ipath_hol_state = IPATH_HOL_UP;
-
-done:
-	if (!ret) {
-		*dd->ipath_statusp |= IPATH_STATUS_CHIP_PRESENT;
-		if (!dd->ipath_f_intrsetup(dd)) {
-			/* now we can enable all interrupts from the chip */
-			ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask,
-					 -1LL);
-			/* force re-interrupt of any pending interrupts. */
-			ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear,
-					 0ULL);
-			/* chip is usable; mark it as initialized */
-			*dd->ipath_statusp |= IPATH_STATUS_INITTED;
-
-			/*
-			 * setup to verify we get an interrupt, and fallback
-			 * to an alternate if necessary and possible
-			 */
-			if (!reinit) {
-				setup_timer(&dd->ipath_intrchk_timer,
-						verify_interrupt,
-						(unsigned long)dd);
-			}
-			dd->ipath_intrchk_timer.expires = jiffies + HZ/2;
-			add_timer(&dd->ipath_intrchk_timer);
-		} else
-			ipath_dev_err(dd, "No interrupts enabled, couldn't "
-				      "setup interrupt address\n");
-
-		if (dd->ipath_cfgports > ipath_stats.sps_nports)
-			/*
-			 * sps_nports is a global, so, we set it to
-			 * the highest number of ports of any of the
-			 * chips we find; we never decrement it, at
-			 * least for now.  Since this might have changed
-			 * over disable/enable or prior to reset, always
-			 * do the check and potentially adjust.
-			 */
-			ipath_stats.sps_nports = dd->ipath_cfgports;
-	} else
-		ipath_dbg("Failed (%d) to initialize chip\n", ret);
-
-	/* if ret is non-zero, we probably should do some cleanup
-	   here... */
-	return ret;
-}
-
-static int ipath_set_kpiobufs(const char *str, struct kernel_param *kp)
-{
-	struct ipath_devdata *dd;
-	unsigned long flags;
-	unsigned short val;
-	int ret;
-
-	ret = ipath_parse_ushort(str, &val);
-
-	spin_lock_irqsave(&ipath_devs_lock, flags);
-
-	if (ret < 0)
-		goto bail;
-
-	if (val == 0) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	list_for_each_entry(dd, &ipath_dev_list, ipath_list) {
-		if (dd->ipath_kregbase)
-			continue;
-		if (val > (dd->ipath_piobcnt2k + dd->ipath_piobcnt4k -
-			   (dd->ipath_cfgports *
-			    IPATH_MIN_USER_PORT_BUFCNT)))
-		{
-			ipath_dev_err(
-				dd,
-				"Allocating %d PIO bufs for kernel leaves "
-				"too few for %d user ports (%d each)\n",
-				val, dd->ipath_cfgports - 1,
-				IPATH_MIN_USER_PORT_BUFCNT);
-			ret = -EINVAL;
-			goto bail;
-		}
-		dd->ipath_lastport_piobuf =
-			dd->ipath_piobcnt2k + dd->ipath_piobcnt4k - val;
-	}
-
-	ipath_kpiobufs = val;
-	ret = 0;
-bail:
-	spin_unlock_irqrestore(&ipath_devs_lock, flags);
-
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_intr.c b/drivers/staging/rdma/ipath/ipath_intr.c
deleted file mode 100644
index 0403fa28ed8d..000000000000
--- a/drivers/staging/rdma/ipath/ipath_intr.c
+++ /dev/null
@@ -1,1271 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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/pci.h>
-#include <linux/delay.h>
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-#include "ipath_common.h"
-
-
-/*
- * Called when we might have an error that is specific to a particular
- * PIO buffer, and may need to cancel that buffer, so it can be re-used.
- */
-void ipath_disarm_senderrbufs(struct ipath_devdata *dd)
-{
-	u32 piobcnt;
-	unsigned long sbuf[4];
-	/*
-	 * it's possible that sendbuffererror could have bits set; might
-	 * have already done this as a result of hardware error handling
-	 */
-	piobcnt = dd->ipath_piobcnt2k + dd->ipath_piobcnt4k;
-	/* read these before writing errorclear */
-	sbuf[0] = ipath_read_kreg64(
-		dd, dd->ipath_kregs->kr_sendbuffererror);
-	sbuf[1] = ipath_read_kreg64(
-		dd, dd->ipath_kregs->kr_sendbuffererror + 1);
-	if (piobcnt > 128)
-		sbuf[2] = ipath_read_kreg64(
-			dd, dd->ipath_kregs->kr_sendbuffererror + 2);
-	if (piobcnt > 192)
-		sbuf[3] = ipath_read_kreg64(
-			dd, dd->ipath_kregs->kr_sendbuffererror + 3);
-	else
-		sbuf[3] = 0;
-
-	if (sbuf[0] || sbuf[1] || (piobcnt > 128 && (sbuf[2] || sbuf[3]))) {
-		int i;
-		if (ipath_debug & (__IPATH_PKTDBG|__IPATH_DBG) &&
-			time_after(dd->ipath_lastcancel, jiffies)) {
-			__IPATH_DBG_WHICH(__IPATH_PKTDBG|__IPATH_DBG,
-					  "SendbufErrs %lx %lx", sbuf[0],
-					  sbuf[1]);
-			if (ipath_debug & __IPATH_PKTDBG && piobcnt > 128)
-				printk(" %lx %lx ", sbuf[2], sbuf[3]);
-			printk("\n");
-		}
-
-		for (i = 0; i < piobcnt; i++)
-			if (test_bit(i, sbuf))
-				ipath_disarm_piobufs(dd, i, 1);
-		/* ignore armlaunch errs for a bit */
-		dd->ipath_lastcancel = jiffies+3;
-	}
-}
-
-
-/* These are all rcv-related errors which we want to count for stats */
-#define E_SUM_PKTERRS \
-	(INFINIPATH_E_RHDRLEN | INFINIPATH_E_RBADTID | \
-	 INFINIPATH_E_RBADVERSION | INFINIPATH_E_RHDR | \
-	 INFINIPATH_E_RLONGPKTLEN | INFINIPATH_E_RSHORTPKTLEN | \
-	 INFINIPATH_E_RMAXPKTLEN | INFINIPATH_E_RMINPKTLEN | \
-	 INFINIPATH_E_RFORMATERR | INFINIPATH_E_RUNSUPVL | \
-	 INFINIPATH_E_RUNEXPCHAR | INFINIPATH_E_REBP)
-
-/* These are all send-related errors which we want to count for stats */
-#define E_SUM_ERRS \
-	(INFINIPATH_E_SPIOARMLAUNCH | INFINIPATH_E_SUNEXPERRPKTNUM | \
-	 INFINIPATH_E_SDROPPEDDATAPKT | INFINIPATH_E_SDROPPEDSMPPKT | \
-	 INFINIPATH_E_SMAXPKTLEN | INFINIPATH_E_SUNSUPVL | \
-	 INFINIPATH_E_SMINPKTLEN | INFINIPATH_E_SPKTLEN | \
-	 INFINIPATH_E_INVALIDADDR)
-
-/*
- * this is similar to E_SUM_ERRS, but can't ignore armlaunch, don't ignore
- * errors not related to freeze and cancelling buffers.  Can't ignore
- * armlaunch because could get more while still cleaning up, and need
- * to cancel those as they happen.
- */
-#define E_SPKT_ERRS_IGNORE \
-	 (INFINIPATH_E_SDROPPEDDATAPKT | INFINIPATH_E_SDROPPEDSMPPKT | \
-	 INFINIPATH_E_SMAXPKTLEN | INFINIPATH_E_SMINPKTLEN | \
-	 INFINIPATH_E_SPKTLEN)
-
-/*
- * these are errors that can occur when the link changes state while
- * a packet is being sent or received.  This doesn't cover things
- * like EBP or VCRC that can be the result of a sending having the
- * link change state, so we receive a "known bad" packet.
- */
-#define E_SUM_LINK_PKTERRS \
-	(INFINIPATH_E_SDROPPEDDATAPKT | INFINIPATH_E_SDROPPEDSMPPKT | \
-	 INFINIPATH_E_SMINPKTLEN | INFINIPATH_E_SPKTLEN | \
-	 INFINIPATH_E_RSHORTPKTLEN | INFINIPATH_E_RMINPKTLEN | \
-	 INFINIPATH_E_RUNEXPCHAR)
-
-static u64 handle_e_sum_errs(struct ipath_devdata *dd, ipath_err_t errs)
-{
-	u64 ignore_this_time = 0;
-
-	ipath_disarm_senderrbufs(dd);
-	if ((errs & E_SUM_LINK_PKTERRS) &&
-	    !(dd->ipath_flags & IPATH_LINKACTIVE)) {
-		/*
-		 * This can happen when SMA is trying to bring the link
-		 * up, but the IB link changes state at the "wrong" time.
-		 * The IB logic then complains that the packet isn't
-		 * valid.  We don't want to confuse people, so we just
-		 * don't print them, except at debug
-		 */
-		ipath_dbg("Ignoring packet errors %llx, because link not "
-			  "ACTIVE\n", (unsigned long long) errs);
-		ignore_this_time = errs & E_SUM_LINK_PKTERRS;
-	}
-
-	return ignore_this_time;
-}
-
-/* generic hw error messages... */
-#define INFINIPATH_HWE_TXEMEMPARITYERR_MSG(a) \
-	{ \
-		.mask = ( INFINIPATH_HWE_TXEMEMPARITYERR_##a <<    \
-			  INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT ),   \
-		.msg = "TXE " #a " Memory Parity"	     \
-	}
-#define INFINIPATH_HWE_RXEMEMPARITYERR_MSG(a) \
-	{ \
-		.mask = ( INFINIPATH_HWE_RXEMEMPARITYERR_##a <<    \
-			  INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT ),   \
-		.msg = "RXE " #a " Memory Parity"	     \
-	}
-
-static const struct ipath_hwerror_msgs ipath_generic_hwerror_msgs[] = {
-	INFINIPATH_HWE_MSG(IBCBUSFRSPCPARITYERR, "IPATH2IB Parity"),
-	INFINIPATH_HWE_MSG(IBCBUSTOSPCPARITYERR, "IB2IPATH Parity"),
-
-	INFINIPATH_HWE_TXEMEMPARITYERR_MSG(PIOBUF),
-	INFINIPATH_HWE_TXEMEMPARITYERR_MSG(PIOPBC),
-	INFINIPATH_HWE_TXEMEMPARITYERR_MSG(PIOLAUNCHFIFO),
-
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(RCVBUF),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(LOOKUPQ),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(EAGERTID),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(EXPTID),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(FLAGBUF),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(DATAINFO),
-	INFINIPATH_HWE_RXEMEMPARITYERR_MSG(HDRINFO),
-};
-
-/**
- * ipath_format_hwmsg - format a single hwerror message
- * @msg message buffer
- * @msgl length of message buffer
- * @hwmsg message to add to message buffer
- */
-static void ipath_format_hwmsg(char *msg, size_t msgl, const char *hwmsg)
-{
-	strlcat(msg, "[", msgl);
-	strlcat(msg, hwmsg, msgl);
-	strlcat(msg, "]", msgl);
-}
-
-/**
- * ipath_format_hwerrors - format hardware error messages for display
- * @hwerrs hardware errors bit vector
- * @hwerrmsgs hardware error descriptions
- * @nhwerrmsgs number of hwerrmsgs
- * @msg message buffer
- * @msgl message buffer length
- */
-void ipath_format_hwerrors(u64 hwerrs,
-			   const struct ipath_hwerror_msgs *hwerrmsgs,
-			   size_t nhwerrmsgs,
-			   char *msg, size_t msgl)
-{
-	int i;
-	const int glen =
-	    ARRAY_SIZE(ipath_generic_hwerror_msgs);
-
-	for (i=0; i<glen; i++) {
-		if (hwerrs & ipath_generic_hwerror_msgs[i].mask) {
-			ipath_format_hwmsg(msg, msgl,
-					   ipath_generic_hwerror_msgs[i].msg);
-		}
-	}
-
-	for (i=0; i<nhwerrmsgs; i++) {
-		if (hwerrs & hwerrmsgs[i].mask) {
-			ipath_format_hwmsg(msg, msgl, hwerrmsgs[i].msg);
-		}
-	}
-}
-
-/* return the strings for the most common link states */
-static char *ib_linkstate(struct ipath_devdata *dd, u64 ibcs)
-{
-	char *ret;
-	u32 state;
-
-	state = ipath_ib_state(dd, ibcs);
-	if (state == dd->ib_init)
-		ret = "Init";
-	else if (state == dd->ib_arm)
-		ret = "Arm";
-	else if (state == dd->ib_active)
-		ret = "Active";
-	else
-		ret = "Down";
-	return ret;
-}
-
-void signal_ib_event(struct ipath_devdata *dd, enum ib_event_type ev)
-{
-	struct ib_event event;
-
-	event.device = &dd->verbs_dev->ibdev;
-	event.element.port_num = 1;
-	event.event = ev;
-	ib_dispatch_event(&event);
-}
-
-static void handle_e_ibstatuschanged(struct ipath_devdata *dd,
-				     ipath_err_t errs)
-{
-	u32 ltstate, lstate, ibstate, lastlstate;
-	u32 init = dd->ib_init;
-	u32 arm = dd->ib_arm;
-	u32 active = dd->ib_active;
-	const u64 ibcs = ipath_read_kreg64(dd, dd->ipath_kregs->kr_ibcstatus);
-
-	lstate = ipath_ib_linkstate(dd, ibcs); /* linkstate */
-	ibstate = ipath_ib_state(dd, ibcs);
-	/* linkstate at last interrupt */
-	lastlstate = ipath_ib_linkstate(dd, dd->ipath_lastibcstat);
-	ltstate = ipath_ib_linktrstate(dd, ibcs); /* linktrainingtate */
-
-	/*
-	 * Since going into a recovery state causes the link state to go
-	 * down and since recovery is transitory, it is better if we "miss"
-	 * ever seeing the link training state go into recovery (i.e.,
-	 * ignore this transition for link state special handling purposes)
-	 * without even updating ipath_lastibcstat.
-	 */
-	if ((ltstate == INFINIPATH_IBCS_LT_STATE_RECOVERRETRAIN) ||
-	    (ltstate == INFINIPATH_IBCS_LT_STATE_RECOVERWAITRMT) ||
-	    (ltstate == INFINIPATH_IBCS_LT_STATE_RECOVERIDLE))
-		goto done;
-
-	/*
-	 * if linkstate transitions into INIT from any of the various down
-	 * states, or if it transitions from any of the up (INIT or better)
-	 * states into any of the down states (except link recovery), then
-	 * call the chip-specific code to take appropriate actions.
-	 */
-	if (lstate >= INFINIPATH_IBCS_L_STATE_INIT &&
-		lastlstate == INFINIPATH_IBCS_L_STATE_DOWN) {
-		/* transitioned to UP */
-		if (dd->ipath_f_ib_updown(dd, 1, ibcs)) {
-			/* link came up, so we must no longer be disabled */
-			dd->ipath_flags &= ~IPATH_IB_LINK_DISABLED;
-			ipath_cdbg(LINKVERB, "LinkUp handled, skipped\n");
-			goto skip_ibchange; /* chip-code handled */
-		}
-	} else if ((lastlstate >= INFINIPATH_IBCS_L_STATE_INIT ||
-		(dd->ipath_flags & IPATH_IB_FORCE_NOTIFY)) &&
-		ltstate <= INFINIPATH_IBCS_LT_STATE_CFGWAITRMT &&
-		ltstate != INFINIPATH_IBCS_LT_STATE_LINKUP) {
-		int handled;
-		handled = dd->ipath_f_ib_updown(dd, 0, ibcs);
-		dd->ipath_flags &= ~IPATH_IB_FORCE_NOTIFY;
-		if (handled) {
-			ipath_cdbg(LINKVERB, "LinkDown handled, skipped\n");
-			goto skip_ibchange; /* chip-code handled */
-		}
-	}
-
-	/*
-	 * Significant enough to always print and get into logs, if it was
-	 * unexpected.  If it was a requested state change, we'll have
-	 * already cleared the flags, so we won't print this warning
-	 */
-	if ((ibstate != arm && ibstate != active) &&
-	    (dd->ipath_flags & (IPATH_LINKARMED | IPATH_LINKACTIVE))) {
-		dev_info(&dd->pcidev->dev, "Link state changed from %s "
-			 "to %s\n", (dd->ipath_flags & IPATH_LINKARMED) ?
-			 "ARM" : "ACTIVE", ib_linkstate(dd, ibcs));
-	}
-
-	if (ltstate == INFINIPATH_IBCS_LT_STATE_POLLACTIVE ||
-	    ltstate == INFINIPATH_IBCS_LT_STATE_POLLQUIET) {
-		u32 lastlts;
-		lastlts = ipath_ib_linktrstate(dd, dd->ipath_lastibcstat);
-		/*
-		 * Ignore cycling back and forth from Polling.Active to
-		 * Polling.Quiet while waiting for the other end of the link
-		 * to come up, except to try and decide if we are connected
-		 * to a live IB device or not.  We will cycle back and
-		 * forth between them if no cable is plugged in, the other
-		 * device is powered off or disabled, etc.
-		 */
-		if (lastlts == INFINIPATH_IBCS_LT_STATE_POLLACTIVE ||
-		    lastlts == INFINIPATH_IBCS_LT_STATE_POLLQUIET) {
-			if (!(dd->ipath_flags & IPATH_IB_AUTONEG_INPROG) &&
-			     (++dd->ipath_ibpollcnt == 40)) {
-				dd->ipath_flags |= IPATH_NOCABLE;
-				*dd->ipath_statusp |=
-					IPATH_STATUS_IB_NOCABLE;
-				ipath_cdbg(LINKVERB, "Set NOCABLE\n");
-			}
-			ipath_cdbg(LINKVERB, "POLL change to %s (%x)\n",
-				ipath_ibcstatus_str[ltstate], ibstate);
-			goto skip_ibchange;
-		}
-	}
-
-	dd->ipath_ibpollcnt = 0; /* not poll*, now */
-	ipath_stats.sps_iblink++;
-
-	if (ibstate != init && dd->ipath_lastlinkrecov && ipath_linkrecovery) {
-		u64 linkrecov;
-		linkrecov = ipath_snap_cntr(dd,
-			dd->ipath_cregs->cr_iblinkerrrecovcnt);
-		if (linkrecov != dd->ipath_lastlinkrecov) {
-			ipath_dbg("IB linkrecov up %Lx (%s %s) recov %Lu\n",
-				(unsigned long long) ibcs,
-				ib_linkstate(dd, ibcs),
-				ipath_ibcstatus_str[ltstate],
-				(unsigned long long) linkrecov);
-			/* and no more until active again */
-			dd->ipath_lastlinkrecov = 0;
-			ipath_set_linkstate(dd, IPATH_IB_LINKDOWN);
-			goto skip_ibchange;
-		}
-	}
-
-	if (ibstate == init || ibstate == arm || ibstate == active) {
-		*dd->ipath_statusp &= ~IPATH_STATUS_IB_NOCABLE;
-		if (ibstate == init || ibstate == arm) {
-			*dd->ipath_statusp &= ~IPATH_STATUS_IB_READY;
-			if (dd->ipath_flags & IPATH_LINKACTIVE)
-				signal_ib_event(dd, IB_EVENT_PORT_ERR);
-		}
-		if (ibstate == arm) {
-			dd->ipath_flags |= IPATH_LINKARMED;
-			dd->ipath_flags &= ~(IPATH_LINKUNK |
-				IPATH_LINKINIT | IPATH_LINKDOWN |
-				IPATH_LINKACTIVE | IPATH_NOCABLE);
-			ipath_hol_down(dd);
-		} else  if (ibstate == init) {
-			/*
-			 * set INIT and DOWN.  Down is checked by
-			 * most of the other code, but INIT is
-			 * useful to know in a few places.
-			 */
-			dd->ipath_flags |= IPATH_LINKINIT |
-				IPATH_LINKDOWN;
-			dd->ipath_flags &= ~(IPATH_LINKUNK |
-				IPATH_LINKARMED | IPATH_LINKACTIVE |
-				IPATH_NOCABLE);
-			ipath_hol_down(dd);
-		} else {  /* active */
-			dd->ipath_lastlinkrecov = ipath_snap_cntr(dd,
-				dd->ipath_cregs->cr_iblinkerrrecovcnt);
-			*dd->ipath_statusp |=
-				IPATH_STATUS_IB_READY | IPATH_STATUS_IB_CONF;
-			dd->ipath_flags |= IPATH_LINKACTIVE;
-			dd->ipath_flags &= ~(IPATH_LINKUNK | IPATH_LINKINIT
-				| IPATH_LINKDOWN | IPATH_LINKARMED |
-				IPATH_NOCABLE);
-			if (dd->ipath_flags & IPATH_HAS_SEND_DMA)
-				ipath_restart_sdma(dd);
-			signal_ib_event(dd, IB_EVENT_PORT_ACTIVE);
-			/* LED active not handled in chip _f_updown */
-			dd->ipath_f_setextled(dd, lstate, ltstate);
-			ipath_hol_up(dd);
-		}
-
-		/*
-		 * print after we've already done the work, so as not to
-		 * delay the state changes and notifications, for debugging
-		 */
-		if (lstate == lastlstate)
-			ipath_cdbg(LINKVERB, "Unchanged from last: %s "
-				"(%x)\n", ib_linkstate(dd, ibcs), ibstate);
-		else
-			ipath_cdbg(VERBOSE, "Unit %u: link up to %s %s (%x)\n",
-				  dd->ipath_unit, ib_linkstate(dd, ibcs),
-				  ipath_ibcstatus_str[ltstate],  ibstate);
-	} else { /* down */
-		if (dd->ipath_flags & IPATH_LINKACTIVE)
-			signal_ib_event(dd, IB_EVENT_PORT_ERR);
-		dd->ipath_flags |= IPATH_LINKDOWN;
-		dd->ipath_flags &= ~(IPATH_LINKUNK | IPATH_LINKINIT
-				     | IPATH_LINKACTIVE |
-				     IPATH_LINKARMED);
-		*dd->ipath_statusp &= ~IPATH_STATUS_IB_READY;
-		dd->ipath_lli_counter = 0;
-
-		if (lastlstate != INFINIPATH_IBCS_L_STATE_DOWN)
-			ipath_cdbg(VERBOSE, "Unit %u link state down "
-				   "(state 0x%x), from %s\n",
-				   dd->ipath_unit, lstate,
-				   ib_linkstate(dd, dd->ipath_lastibcstat));
-		else
-			ipath_cdbg(LINKVERB, "Unit %u link state changed "
-				   "to %s (0x%x) from down (%x)\n",
-				   dd->ipath_unit,
-				   ipath_ibcstatus_str[ltstate],
-				   ibstate, lastlstate);
-	}
-
-skip_ibchange:
-	dd->ipath_lastibcstat = ibcs;
-done:
-	return;
-}
-
-static void handle_supp_msgs(struct ipath_devdata *dd,
-			     unsigned supp_msgs, char *msg, u32 msgsz)
-{
-	/*
-	 * Print the message unless it's ibc status change only, which
-	 * happens so often we never want to count it.
-	 */
-	if (dd->ipath_lasterror & ~INFINIPATH_E_IBSTATUSCHANGED) {
-		int iserr;
-		ipath_err_t mask;
-		iserr = ipath_decode_err(dd, msg, msgsz,
-					 dd->ipath_lasterror &
-					 ~INFINIPATH_E_IBSTATUSCHANGED);
-
-		mask = INFINIPATH_E_RRCVEGRFULL | INFINIPATH_E_RRCVHDRFULL |
-			INFINIPATH_E_PKTERRS | INFINIPATH_E_SDMADISABLED;
-
-		/* if we're in debug, then don't mask SDMADISABLED msgs */
-		if (ipath_debug & __IPATH_DBG)
-			mask &= ~INFINIPATH_E_SDMADISABLED;
-
-		if (dd->ipath_lasterror & ~mask)
-			ipath_dev_err(dd, "Suppressed %u messages for "
-				      "fast-repeating errors (%s) (%llx)\n",
-				      supp_msgs, msg,
-				      (unsigned long long)
-				      dd->ipath_lasterror);
-		else {
-			/*
-			 * rcvegrfull and rcvhdrqfull are "normal", for some
-			 * types of processes (mostly benchmarks) that send
-			 * huge numbers of messages, while not processing
-			 * them. So only complain about these at debug
-			 * level.
-			 */
-			if (iserr)
-				ipath_dbg("Suppressed %u messages for %s\n",
-					  supp_msgs, msg);
-			else
-				ipath_cdbg(ERRPKT,
-					"Suppressed %u messages for %s\n",
-					  supp_msgs, msg);
-		}
-	}
-}
-
-static unsigned handle_frequent_errors(struct ipath_devdata *dd,
-				       ipath_err_t errs, char *msg,
-				       u32 msgsz, int *noprint)
-{
-	unsigned long nc;
-	static unsigned long nextmsg_time;
-	static unsigned nmsgs, supp_msgs;
-
-	/*
-	 * Throttle back "fast" messages to no more than 10 per 5 seconds.
-	 * This isn't perfect, but it's a reasonable heuristic. If we get
-	 * more than 10, give a 6x longer delay.
-	 */
-	nc = jiffies;
-	if (nmsgs > 10) {
-		if (time_before(nc, nextmsg_time)) {
-			*noprint = 1;
-			if (!supp_msgs++)
-				nextmsg_time = nc + HZ * 3;
-		} else if (supp_msgs) {
-			handle_supp_msgs(dd, supp_msgs, msg, msgsz);
-			supp_msgs = 0;
-			nmsgs = 0;
-		}
-	} else if (!nmsgs++ || time_after(nc, nextmsg_time)) {
-		nextmsg_time = nc + HZ / 2;
-	}
-
-	return supp_msgs;
-}
-
-static void handle_sdma_errors(struct ipath_devdata *dd, ipath_err_t errs)
-{
-	unsigned long flags;
-	int expected;
-
-	if (ipath_debug & __IPATH_DBG) {
-		char msg[128];
-		ipath_decode_err(dd, msg, sizeof msg, errs &
-			INFINIPATH_E_SDMAERRS);
-		ipath_dbg("errors %lx (%s)\n", (unsigned long)errs, msg);
-	}
-	if (ipath_debug & __IPATH_VERBDBG) {
-		unsigned long tl, hd, status, lengen;
-		tl = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmatail);
-		hd = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmahead);
-		status = ipath_read_kreg64(dd
-			, dd->ipath_kregs->kr_senddmastatus);
-		lengen = ipath_read_kreg64(dd,
-			dd->ipath_kregs->kr_senddmalengen);
-		ipath_cdbg(VERBOSE, "sdma tl 0x%lx hd 0x%lx status 0x%lx "
-			"lengen 0x%lx\n", tl, hd, status, lengen);
-	}
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-	__set_bit(IPATH_SDMA_DISABLED, &dd->ipath_sdma_status);
-	expected = test_bit(IPATH_SDMA_ABORTING, &dd->ipath_sdma_status);
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-	if (!expected)
-		ipath_cancel_sends(dd, 1);
-}
-
-static void handle_sdma_intr(struct ipath_devdata *dd, u64 istat)
-{
-	unsigned long flags;
-	int expected;
-
-	if ((istat & INFINIPATH_I_SDMAINT) &&
-	    !test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-		ipath_sdma_intr(dd);
-
-	if (istat & INFINIPATH_I_SDMADISABLED) {
-		expected = test_bit(IPATH_SDMA_ABORTING,
-			&dd->ipath_sdma_status);
-		ipath_dbg("%s SDmaDisabled intr\n",
-			expected ? "expected" : "unexpected");
-		spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-		__set_bit(IPATH_SDMA_DISABLED, &dd->ipath_sdma_status);
-		spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-		if (!expected)
-			ipath_cancel_sends(dd, 1);
-		if (!test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-			tasklet_hi_schedule(&dd->ipath_sdma_abort_task);
-	}
-}
-
-static int handle_hdrq_full(struct ipath_devdata *dd)
-{
-	int chkerrpkts = 0;
-	u32 hd, tl;
-	u32 i;
-
-	ipath_stats.sps_hdrqfull++;
-	for (i = 0; i < dd->ipath_cfgports; i++) {
-		struct ipath_portdata *pd = dd->ipath_pd[i];
-
-		if (i == 0) {
-			/*
-			 * For kernel receive queues, we just want to know
-			 * if there are packets in the queue that we can
-			 * process.
-			 */
-			if (pd->port_head != ipath_get_hdrqtail(pd))
-				chkerrpkts |= 1 << i;
-			continue;
-		}
-
-		/* Skip if user context is not open */
-		if (!pd || !pd->port_cnt)
-			continue;
-
-		/* Don't report the same point multiple times. */
-		if (dd->ipath_flags & IPATH_NODMA_RTAIL)
-			tl = ipath_read_ureg32(dd, ur_rcvhdrtail, i);
-		else
-			tl = ipath_get_rcvhdrtail(pd);
-		if (tl == pd->port_lastrcvhdrqtail)
-			continue;
-
-		hd = ipath_read_ureg32(dd, ur_rcvhdrhead, i);
-		if (hd == (tl + 1) || (!hd && tl == dd->ipath_hdrqlast)) {
-			pd->port_lastrcvhdrqtail = tl;
-			pd->port_hdrqfull++;
-			/* flush hdrqfull so that poll() sees it */
-			wmb();
-			wake_up_interruptible(&pd->port_wait);
-		}
-	}
-
-	return chkerrpkts;
-}
-
-static int handle_errors(struct ipath_devdata *dd, ipath_err_t errs)
-{
-	char msg[128];
-	u64 ignore_this_time = 0;
-	u64 iserr = 0;
-	int chkerrpkts = 0, noprint = 0;
-	unsigned supp_msgs;
-	int log_idx;
-
-	/*
-	 * don't report errors that are masked, either at init
-	 * (not set in ipath_errormask), or temporarily (set in
-	 * ipath_maskederrs)
-	 */
-	errs &= dd->ipath_errormask & ~dd->ipath_maskederrs;
-
-	supp_msgs = handle_frequent_errors(dd, errs, msg, (u32)sizeof msg,
-		&noprint);
-
-	/* do these first, they are most important */
-	if (errs & INFINIPATH_E_HARDWARE) {
-		/* reuse same msg buf */
-		dd->ipath_f_handle_hwerrors(dd, msg, sizeof msg);
-	} else {
-		u64 mask;
-		for (log_idx = 0; log_idx < IPATH_EEP_LOG_CNT; ++log_idx) {
-			mask = dd->ipath_eep_st_masks[log_idx].errs_to_log;
-			if (errs & mask)
-				ipath_inc_eeprom_err(dd, log_idx, 1);
-		}
-	}
-
-	if (errs & INFINIPATH_E_SDMAERRS)
-		handle_sdma_errors(dd, errs);
-
-	if (!noprint && (errs & ~dd->ipath_e_bitsextant))
-		ipath_dev_err(dd, "error interrupt with unknown errors "
-			      "%llx set\n", (unsigned long long)
-			      (errs & ~dd->ipath_e_bitsextant));
-
-	if (errs & E_SUM_ERRS)
-		ignore_this_time = handle_e_sum_errs(dd, errs);
-	else if ((errs & E_SUM_LINK_PKTERRS) &&
-	    !(dd->ipath_flags & IPATH_LINKACTIVE)) {
-		/*
-		 * This can happen when SMA is trying to bring the link
-		 * up, but the IB link changes state at the "wrong" time.
-		 * The IB logic then complains that the packet isn't
-		 * valid.  We don't want to confuse people, so we just
-		 * don't print them, except at debug
-		 */
-		ipath_dbg("Ignoring packet errors %llx, because link not "
-			  "ACTIVE\n", (unsigned long long) errs);
-		ignore_this_time = errs & E_SUM_LINK_PKTERRS;
-	}
-
-	if (supp_msgs == 250000) {
-		int s_iserr;
-		/*
-		 * It's not entirely reasonable assuming that the errors set
-		 * in the last clear period are all responsible for the
-		 * problem, but the alternative is to assume it's the only
-		 * ones on this particular interrupt, which also isn't great
-		 */
-		dd->ipath_maskederrs |= dd->ipath_lasterror | errs;
-
-		dd->ipath_errormask &= ~dd->ipath_maskederrs;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-				 dd->ipath_errormask);
-		s_iserr = ipath_decode_err(dd, msg, sizeof msg,
-					   dd->ipath_maskederrs);
-
-		if (dd->ipath_maskederrs &
-		    ~(INFINIPATH_E_RRCVEGRFULL |
-		      INFINIPATH_E_RRCVHDRFULL | INFINIPATH_E_PKTERRS))
-			ipath_dev_err(dd, "Temporarily disabling "
-			    "error(s) %llx reporting; too frequent (%s)\n",
-				(unsigned long long) dd->ipath_maskederrs,
-				msg);
-		else {
-			/*
-			 * rcvegrfull and rcvhdrqfull are "normal",
-			 * for some types of processes (mostly benchmarks)
-			 * that send huge numbers of messages, while not
-			 * processing them.  So only complain about
-			 * these at debug level.
-			 */
-			if (s_iserr)
-				ipath_dbg("Temporarily disabling reporting "
-				    "too frequent queue full errors (%s)\n",
-				    msg);
-			else
-				ipath_cdbg(ERRPKT,
-				    "Temporarily disabling reporting too"
-				    " frequent packet errors (%s)\n",
-				    msg);
-		}
-
-		/*
-		 * Re-enable the masked errors after around 3 minutes.  in
-		 * ipath_get_faststats().  If we have a series of fast
-		 * repeating but different errors, the interval will keep
-		 * stretching out, but that's OK, as that's pretty
-		 * catastrophic.
-		 */
-		dd->ipath_unmasktime = jiffies + HZ * 180;
-	}
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear, errs);
-	if (ignore_this_time)
-		errs &= ~ignore_this_time;
-	if (errs & ~dd->ipath_lasterror) {
-		errs &= ~dd->ipath_lasterror;
-		/* never suppress duplicate hwerrors or ibstatuschange */
-		dd->ipath_lasterror |= errs &
-			~(INFINIPATH_E_HARDWARE |
-			  INFINIPATH_E_IBSTATUSCHANGED);
-	}
-
-	if (errs & INFINIPATH_E_SENDSPECIALTRIGGER) {
-		dd->ipath_spectriggerhit++;
-		ipath_dbg("%lu special trigger hits\n",
-			dd->ipath_spectriggerhit);
-	}
-
-	/* likely due to cancel; so suppress message unless verbose */
-	if ((errs & (INFINIPATH_E_SPKTLEN | INFINIPATH_E_SPIOARMLAUNCH)) &&
-		time_after(dd->ipath_lastcancel, jiffies)) {
-		/* armlaunch takes precedence; it often causes both. */
-		ipath_cdbg(VERBOSE,
-			"Suppressed %s error (%llx) after sendbuf cancel\n",
-			(errs &  INFINIPATH_E_SPIOARMLAUNCH) ?
-			"armlaunch" : "sendpktlen", (unsigned long long)errs);
-		errs &= ~(INFINIPATH_E_SPIOARMLAUNCH | INFINIPATH_E_SPKTLEN);
-	}
-
-	if (!errs)
-		return 0;
-
-	if (!noprint) {
-		ipath_err_t mask;
-		/*
-		 * The ones we mask off are handled specially below
-		 * or above.  Also mask SDMADISABLED by default as it
-		 * is too chatty.
-		 */
-		mask = INFINIPATH_E_IBSTATUSCHANGED |
-			INFINIPATH_E_RRCVEGRFULL | INFINIPATH_E_RRCVHDRFULL |
-			INFINIPATH_E_HARDWARE | INFINIPATH_E_SDMADISABLED;
-
-		/* if we're in debug, then don't mask SDMADISABLED msgs */
-		if (ipath_debug & __IPATH_DBG)
-			mask &= ~INFINIPATH_E_SDMADISABLED;
-
-		ipath_decode_err(dd, msg, sizeof msg, errs & ~mask);
-	} else
-		/* so we don't need if (!noprint) at strlcat's below */
-		*msg = 0;
-
-	if (errs & E_SUM_PKTERRS) {
-		ipath_stats.sps_pkterrs++;
-		chkerrpkts = 1;
-	}
-	if (errs & E_SUM_ERRS)
-		ipath_stats.sps_errs++;
-
-	if (errs & (INFINIPATH_E_RICRC | INFINIPATH_E_RVCRC)) {
-		ipath_stats.sps_crcerrs++;
-		chkerrpkts = 1;
-	}
-	iserr = errs & ~(E_SUM_PKTERRS | INFINIPATH_E_PKTERRS);
-
-
-	/*
-	 * We don't want to print these two as they happen, or we can make
-	 * the situation even worse, because it takes so long to print
-	 * messages to serial consoles.  Kernel ports get printed from
-	 * fast_stats, no more than every 5 seconds, user ports get printed
-	 * on close
-	 */
-	if (errs & INFINIPATH_E_RRCVHDRFULL)
-		chkerrpkts |= handle_hdrq_full(dd);
-	if (errs & INFINIPATH_E_RRCVEGRFULL) {
-		struct ipath_portdata *pd = dd->ipath_pd[0];
-
-		/*
-		 * since this is of less importance and not likely to
-		 * happen without also getting hdrfull, only count
-		 * occurrences; don't check each port (or even the kernel
-		 * vs user)
-		 */
-		ipath_stats.sps_etidfull++;
-		if (pd->port_head != ipath_get_hdrqtail(pd))
-			chkerrpkts |= 1;
-	}
-
-	/*
-	 * do this before IBSTATUSCHANGED, in case both bits set in a single
-	 * interrupt; we want the STATUSCHANGE to "win", so we do our
-	 * internal copy of state machine correctly
-	 */
-	if (errs & INFINIPATH_E_RIBLOSTLINK) {
-		/*
-		 * force through block below
-		 */
-		errs |= INFINIPATH_E_IBSTATUSCHANGED;
-		ipath_stats.sps_iblink++;
-		dd->ipath_flags |= IPATH_LINKDOWN;
-		dd->ipath_flags &= ~(IPATH_LINKUNK | IPATH_LINKINIT
-				     | IPATH_LINKARMED | IPATH_LINKACTIVE);
-		*dd->ipath_statusp &= ~IPATH_STATUS_IB_READY;
-
-		ipath_dbg("Lost link, link now down (%s)\n",
-			ipath_ibcstatus_str[ipath_read_kreg64(dd,
-			dd->ipath_kregs->kr_ibcstatus) & 0xf]);
-	}
-	if (errs & INFINIPATH_E_IBSTATUSCHANGED)
-		handle_e_ibstatuschanged(dd, errs);
-
-	if (errs & INFINIPATH_E_RESET) {
-		if (!noprint)
-			ipath_dev_err(dd, "Got reset, requires re-init "
-				      "(unload and reload driver)\n");
-		dd->ipath_flags &= ~IPATH_INITTED;	/* needs re-init */
-		/* mark as having had error */
-		*dd->ipath_statusp |= IPATH_STATUS_HWERROR;
-		*dd->ipath_statusp &= ~IPATH_STATUS_IB_CONF;
-	}
-
-	if (!noprint && *msg) {
-		if (iserr)
-			ipath_dev_err(dd, "%s error\n", msg);
-	}
-	if (dd->ipath_state_wanted & dd->ipath_flags) {
-		ipath_cdbg(VERBOSE, "driver wanted state %x, iflags now %x, "
-			   "waking\n", dd->ipath_state_wanted,
-			   dd->ipath_flags);
-		wake_up_interruptible(&ipath_state_wait);
-	}
-
-	return chkerrpkts;
-}
-
-/*
- * try to cleanup as much as possible for anything that might have gone
- * wrong while in freeze mode, such as pio buffers being written by user
- * processes (causing armlaunch), send errors due to going into freeze mode,
- * etc., and try to avoid causing extra interrupts while doing so.
- * Forcibly update the in-memory pioavail register copies after cleanup
- * because the chip won't do it while in freeze mode (the register values
- * themselves are kept correct).
- * Make sure that we don't lose any important interrupts by using the chip
- * feature that says that writing 0 to a bit in *clear that is set in
- * *status will cause an interrupt to be generated again (if allowed by
- * the *mask value).
- */
-void ipath_clear_freeze(struct ipath_devdata *dd)
-{
-	/* disable error interrupts, to avoid confusion */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask, 0ULL);
-
-	/* also disable interrupts; errormask is sometimes overwriten */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask, 0ULL);
-
-	ipath_cancel_sends(dd, 1);
-
-	/* clear the freeze, and be sure chip saw it */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_control,
-			 dd->ipath_control);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-
-	/* force in-memory update now we are out of freeze */
-	ipath_force_pio_avail_update(dd);
-
-	/*
-	 * force new interrupt if any hwerr, error or interrupt bits are
-	 * still set, and clear "safe" send packet errors related to freeze
-	 * and cancelling sends.  Re-enable error interrupts before possible
-	 * force of re-interrupt on pending interrupts.
-	 */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear, 0ULL);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear,
-		E_SPKT_ERRS_IGNORE);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-		dd->ipath_errormask);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask, -1LL);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, 0ULL);
-}
-
-
-/* this is separate to allow for better optimization of ipath_intr() */
-
-static noinline void ipath_bad_intr(struct ipath_devdata *dd, u32 *unexpectp)
-{
-	/*
-	 * sometimes happen during driver init and unload, don't want
-	 * to process any interrupts at that point
-	 */
-
-	/* this is just a bandaid, not a fix, if something goes badly
-	 * wrong */
-	if (++*unexpectp > 100) {
-		if (++*unexpectp > 105) {
-			/*
-			 * ok, we must be taking somebody else's interrupts,
-			 * due to a messed up mptable and/or PIRQ table, so
-			 * unregister the interrupt.  We've seen this during
-			 * linuxbios development work, and it may happen in
-			 * the future again.
-			 */
-			if (dd->pcidev && dd->ipath_irq) {
-				ipath_dev_err(dd, "Now %u unexpected "
-					      "interrupts, unregistering "
-					      "interrupt handler\n",
-					      *unexpectp);
-				ipath_dbg("free_irq of irq %d\n",
-					  dd->ipath_irq);
-				dd->ipath_f_free_irq(dd);
-			}
-		}
-		if (ipath_read_ireg(dd, dd->ipath_kregs->kr_intmask)) {
-			ipath_dev_err(dd, "%u unexpected interrupts, "
-				      "disabling interrupts completely\n",
-				      *unexpectp);
-			/*
-			 * disable all interrupts, something is very wrong
-			 */
-			ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask,
-					 0ULL);
-		}
-	} else if (*unexpectp > 1)
-		ipath_dbg("Interrupt when not ready, should not happen, "
-			  "ignoring\n");
-}
-
-static noinline void ipath_bad_regread(struct ipath_devdata *dd)
-{
-	static int allbits;
-
-	/* separate routine, for better optimization of ipath_intr() */
-
-	/*
-	 * We print the message and disable interrupts, in hope of
-	 * having a better chance of debugging the problem.
-	 */
-	ipath_dev_err(dd,
-		      "Read of interrupt status failed (all bits set)\n");
-	if (allbits++) {
-		/* disable all interrupts, something is very wrong */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_intmask, 0ULL);
-		if (allbits == 2) {
-			ipath_dev_err(dd, "Still bad interrupt status, "
-				      "unregistering interrupt\n");
-			dd->ipath_f_free_irq(dd);
-		} else if (allbits > 2) {
-			if ((allbits % 10000) == 0)
-				printk(".");
-		} else
-			ipath_dev_err(dd, "Disabling interrupts, "
-				      "multiple errors\n");
-	}
-}
-
-static void handle_layer_pioavail(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-	int ret;
-
-	ret = ipath_ib_piobufavail(dd->verbs_dev);
-	if (ret > 0)
-		goto set;
-
-	return;
-set:
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl |= INFINIPATH_S_PIOINTBUFAVAIL;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-			 dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-}
-
-/*
- * Handle receive interrupts for user ports; this means a user
- * process was waiting for a packet to arrive, and didn't want
- * to poll
- */
-static void handle_urcv(struct ipath_devdata *dd, u64 istat)
-{
-	u64 portr;
-	int i;
-	int rcvdint = 0;
-
-	/*
-	 * test_and_clear_bit(IPATH_PORT_WAITING_RCV) and
-	 * test_and_clear_bit(IPATH_PORT_WAITING_URG) below
-	 * would both like timely updates of the bits so that
-	 * we don't pass them by unnecessarily.  the rmb()
-	 * here ensures that we see them promptly -- the
-	 * corresponding wmb()'s are in ipath_poll_urgent()
-	 * and ipath_poll_next()...
-	 */
-	rmb();
-	portr = ((istat >> dd->ipath_i_rcvavail_shift) &
-		 dd->ipath_i_rcvavail_mask) |
-		((istat >> dd->ipath_i_rcvurg_shift) &
-		 dd->ipath_i_rcvurg_mask);
-	for (i = 1; i < dd->ipath_cfgports; i++) {
-		struct ipath_portdata *pd = dd->ipath_pd[i];
-
-		if (portr & (1 << i) && pd && pd->port_cnt) {
-			if (test_and_clear_bit(IPATH_PORT_WAITING_RCV,
-					       &pd->port_flag)) {
-				clear_bit(i + dd->ipath_r_intravail_shift,
-					  &dd->ipath_rcvctrl);
-				wake_up_interruptible(&pd->port_wait);
-				rcvdint = 1;
-			} else if (test_and_clear_bit(IPATH_PORT_WAITING_URG,
-						      &pd->port_flag)) {
-				pd->port_urgent++;
-				wake_up_interruptible(&pd->port_wait);
-			}
-		}
-	}
-	if (rcvdint) {
-		/* only want to take one interrupt, so turn off the rcv
-		 * interrupt for all the ports that we set the rcv_waiting
-		 * (but never for kernel port)
-		 */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_rcvctrl,
-				 dd->ipath_rcvctrl);
-	}
-}
-
-irqreturn_t ipath_intr(int irq, void *data)
-{
-	struct ipath_devdata *dd = data;
-	u64 istat, chk0rcv = 0;
-	ipath_err_t estat = 0;
-	irqreturn_t ret;
-	static unsigned unexpected = 0;
-	u64 kportrbits;
-
-	ipath_stats.sps_ints++;
-
-	if (dd->ipath_int_counter != (u32) -1)
-		dd->ipath_int_counter++;
-
-	if (!(dd->ipath_flags & IPATH_PRESENT)) {
-		/*
-		 * This return value is not great, but we do not want the
-		 * interrupt core code to remove our interrupt handler
-		 * because we don't appear to be handling an interrupt
-		 * during a chip reset.
-		 */
-		return IRQ_HANDLED;
-	}
-
-	/*
-	 * this needs to be flags&initted, not statusp, so we keep
-	 * taking interrupts even after link goes down, etc.
-	 * Also, we *must* clear the interrupt at some point, or we won't
-	 * take it again, which can be real bad for errors, etc...
-	 */
-
-	if (!(dd->ipath_flags & IPATH_INITTED)) {
-		ipath_bad_intr(dd, &unexpected);
-		ret = IRQ_NONE;
-		goto bail;
-	}
-
-	istat = ipath_read_ireg(dd, dd->ipath_kregs->kr_intstatus);
-
-	if (unlikely(!istat)) {
-		ipath_stats.sps_nullintr++;
-		ret = IRQ_NONE; /* not our interrupt, or already handled */
-		goto bail;
-	}
-	if (unlikely(istat == -1)) {
-		ipath_bad_regread(dd);
-		/* don't know if it was our interrupt or not */
-		ret = IRQ_NONE;
-		goto bail;
-	}
-
-	if (unexpected)
-		unexpected = 0;
-
-	if (unlikely(istat & ~dd->ipath_i_bitsextant))
-		ipath_dev_err(dd,
-			      "interrupt with unknown interrupts %Lx set\n",
-			      (unsigned long long)
-			      istat & ~dd->ipath_i_bitsextant);
-	else if (istat & ~INFINIPATH_I_ERROR) /* errors do own printing */
-		ipath_cdbg(VERBOSE, "intr stat=0x%Lx\n",
-			(unsigned long long) istat);
-
-	if (istat & INFINIPATH_I_ERROR) {
-		ipath_stats.sps_errints++;
-		estat = ipath_read_kreg64(dd,
-					  dd->ipath_kregs->kr_errorstatus);
-		if (!estat)
-			dev_info(&dd->pcidev->dev, "error interrupt (%Lx), "
-				 "but no error bits set!\n",
-				 (unsigned long long) istat);
-		else if (estat == -1LL)
-			/*
-			 * should we try clearing all, or hope next read
-			 * works?
-			 */
-			ipath_dev_err(dd, "Read of error status failed "
-				      "(all bits set); ignoring\n");
-		else
-			chk0rcv |= handle_errors(dd, estat);
-	}
-
-	if (istat & INFINIPATH_I_GPIO) {
-		/*
-		 * GPIO interrupts fall in two broad classes:
-		 * GPIO_2 indicates (on some HT4xx boards) that a packet
-		 *        has arrived for Port 0. Checking for this
-		 *        is controlled by flag IPATH_GPIO_INTR.
-		 * GPIO_3..5 on IBA6120 Rev2 and IBA6110 Rev4 chips indicate
-		 *        errors that we need to count. Checking for this
-		 *        is controlled by flag IPATH_GPIO_ERRINTRS.
-		 */
-		u32 gpiostatus;
-		u32 to_clear = 0;
-
-		gpiostatus = ipath_read_kreg32(
-			dd, dd->ipath_kregs->kr_gpio_status);
-		/* First the error-counter case. */
-		if ((gpiostatus & IPATH_GPIO_ERRINTR_MASK) &&
-		    (dd->ipath_flags & IPATH_GPIO_ERRINTRS)) {
-			/* want to clear the bits we see asserted. */
-			to_clear |= (gpiostatus & IPATH_GPIO_ERRINTR_MASK);
-
-			/*
-			 * Count appropriately, clear bits out of our copy,
-			 * as they have been "handled".
-			 */
-			if (gpiostatus & (1 << IPATH_GPIO_RXUVL_BIT)) {
-				ipath_dbg("FlowCtl on UnsupVL\n");
-				dd->ipath_rxfc_unsupvl_errs++;
-			}
-			if (gpiostatus & (1 << IPATH_GPIO_OVRUN_BIT)) {
-				ipath_dbg("Overrun Threshold exceeded\n");
-				dd->ipath_overrun_thresh_errs++;
-			}
-			if (gpiostatus & (1 << IPATH_GPIO_LLI_BIT)) {
-				ipath_dbg("Local Link Integrity error\n");
-				dd->ipath_lli_errs++;
-			}
-			gpiostatus &= ~IPATH_GPIO_ERRINTR_MASK;
-		}
-		/* Now the Port0 Receive case */
-		if ((gpiostatus & (1 << IPATH_GPIO_PORT0_BIT)) &&
-		    (dd->ipath_flags & IPATH_GPIO_INTR)) {
-			/*
-			 * GPIO status bit 2 is set, and we expected it.
-			 * clear it and indicate in p0bits.
-			 * This probably only happens if a Port0 pkt
-			 * arrives at _just_ the wrong time, and we
-			 * handle that by seting chk0rcv;
-			 */
-			to_clear |= (1 << IPATH_GPIO_PORT0_BIT);
-			gpiostatus &= ~(1 << IPATH_GPIO_PORT0_BIT);
-			chk0rcv = 1;
-		}
-		if (gpiostatus) {
-			/*
-			 * Some unexpected bits remain. If they could have
-			 * caused the interrupt, complain and clear.
-			 * To avoid repetition of this condition, also clear
-			 * the mask. It is almost certainly due to error.
-			 */
-			const u32 mask = (u32) dd->ipath_gpio_mask;
-
-			if (mask & gpiostatus) {
-				ipath_dbg("Unexpected GPIO IRQ bits %x\n",
-				  gpiostatus & mask);
-				to_clear |= (gpiostatus & mask);
-				dd->ipath_gpio_mask &= ~(gpiostatus & mask);
-				ipath_write_kreg(dd,
-					dd->ipath_kregs->kr_gpio_mask,
-					dd->ipath_gpio_mask);
-			}
-		}
-		if (to_clear) {
-			ipath_write_kreg(dd, dd->ipath_kregs->kr_gpio_clear,
-					(u64) to_clear);
-		}
-	}
-
-	/*
-	 * Clear the interrupt bits we found set, unless they are receive
-	 * related, in which case we already cleared them above, and don't
-	 * want to clear them again, because we might lose an interrupt.
-	 * Clear it early, so we "know" know the chip will have seen this by
-	 * the time we process the queue, and will re-interrupt if necessary.
-	 * The processor itself won't take the interrupt again until we return.
-	 */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, istat);
-
-	/*
-	 * Handle kernel receive queues before checking for pio buffers
-	 * available since receives can overflow; piobuf waiters can afford
-	 * a few extra cycles, since they were waiting anyway, and user's
-	 * waiting for receive are at the bottom.
-	 */
-	kportrbits = (1ULL << dd->ipath_i_rcvavail_shift) |
-		(1ULL << dd->ipath_i_rcvurg_shift);
-	if (chk0rcv || (istat & kportrbits)) {
-		istat &= ~kportrbits;
-		ipath_kreceive(dd->ipath_pd[0]);
-	}
-
-	if (istat & ((dd->ipath_i_rcvavail_mask << dd->ipath_i_rcvavail_shift) |
-		     (dd->ipath_i_rcvurg_mask << dd->ipath_i_rcvurg_shift)))
-		handle_urcv(dd, istat);
-
-	if (istat & (INFINIPATH_I_SDMAINT | INFINIPATH_I_SDMADISABLED))
-		handle_sdma_intr(dd, istat);
-
-	if (istat & INFINIPATH_I_SPIOBUFAVAIL) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		dd->ipath_sendctrl &= ~INFINIPATH_S_PIOINTBUFAVAIL;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-				 dd->ipath_sendctrl);
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-		/* always process; sdma verbs uses PIO for acks and VL15  */
-		handle_layer_pioavail(dd);
-	}
-
-	ret = IRQ_HANDLED;
-
-bail:
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_kernel.h b/drivers/staging/rdma/ipath/ipath_kernel.h
deleted file mode 100644
index 66c934a5f839..000000000000
--- a/drivers/staging/rdma/ipath/ipath_kernel.h
+++ /dev/null
@@ -1,1374 +0,0 @@
-#ifndef _IPATH_KERNEL_H
-#define _IPATH_KERNEL_H
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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.
- */
-
-/*
- * This header file is the base header file for infinipath kernel code
- * ipath_user.h serves a similar purpose for user code.
- */
-
-#include <linux/interrupt.h>
-#include <linux/pci.h>
-#include <linux/dma-mapping.h>
-#include <linux/mutex.h>
-#include <linux/list.h>
-#include <linux/scatterlist.h>
-#include <linux/sched.h>
-#include <asm/io.h>
-#include <rdma/ib_verbs.h>
-
-#include "ipath_common.h"
-#include "ipath_debug.h"
-#include "ipath_registers.h"
-
-/* only s/w major version of InfiniPath we can handle */
-#define IPATH_CHIP_VERS_MAJ 2U
-
-/* don't care about this except printing */
-#define IPATH_CHIP_VERS_MIN 0U
-
-/* temporary, maybe always */
-extern struct infinipath_stats ipath_stats;
-
-#define IPATH_CHIP_SWVERSION IPATH_CHIP_VERS_MAJ
-/*
- * First-cut critierion for "device is active" is
- * two thousand dwords combined Tx, Rx traffic per
- * 5-second interval. SMA packets are 64 dwords,
- * and occur "a few per second", presumably each way.
- */
-#define IPATH_TRAFFIC_ACTIVE_THRESHOLD (2000)
-/*
- * Struct used to indicate which errors are logged in each of the
- * error-counters that are logged to EEPROM. A counter is incremented
- * _once_ (saturating at 255) for each event with any bits set in
- * the error or hwerror register masks below.
- */
-#define IPATH_EEP_LOG_CNT (4)
-struct ipath_eep_log_mask {
-	u64 errs_to_log;
-	u64 hwerrs_to_log;
-};
-
-struct ipath_portdata {
-	void **port_rcvegrbuf;
-	dma_addr_t *port_rcvegrbuf_phys;
-	/* rcvhdrq base, needs mmap before useful */
-	void *port_rcvhdrq;
-	/* kernel virtual address where hdrqtail is updated */
-	void *port_rcvhdrtail_kvaddr;
-	/*
-	 * temp buffer for expected send setup, allocated at open, instead
-	 * of each setup call
-	 */
-	void *port_tid_pg_list;
-	/* when waiting for rcv or pioavail */
-	wait_queue_head_t port_wait;
-	/*
-	 * rcvegr bufs base, physical, must fit
-	 * in 44 bits so 32 bit programs mmap64 44 bit works)
-	 */
-	dma_addr_t port_rcvegr_phys;
-	/* mmap of hdrq, must fit in 44 bits */
-	dma_addr_t port_rcvhdrq_phys;
-	dma_addr_t port_rcvhdrqtailaddr_phys;
-	/*
-	 * number of opens (including slave subports) on this instance
-	 * (ignoring forks, dup, etc. for now)
-	 */
-	int port_cnt;
-	/*
-	 * how much space to leave at start of eager TID entries for
-	 * protocol use, on each TID
-	 */
-	/* instead of calculating it */
-	unsigned port_port;
-	/* non-zero if port is being shared. */
-	u16 port_subport_cnt;
-	/* non-zero if port is being shared. */
-	u16 port_subport_id;
-	/* number of pio bufs for this port (all procs, if shared) */
-	u32 port_piocnt;
-	/* first pio buffer for this port */
-	u32 port_pio_base;
-	/* chip offset of PIO buffers for this port */
-	u32 port_piobufs;
-	/* how many alloc_pages() chunks in port_rcvegrbuf_pages */
-	u32 port_rcvegrbuf_chunks;
-	/* how many egrbufs per chunk */
-	u32 port_rcvegrbufs_perchunk;
-	/* order for port_rcvegrbuf_pages */
-	size_t port_rcvegrbuf_size;
-	/* rcvhdrq size (for freeing) */
-	size_t port_rcvhdrq_size;
-	/* next expected TID to check when looking for free */
-	u32 port_tidcursor;
-	/* next expected TID to check */
-	unsigned long port_flag;
-	/* what happened */
-	unsigned long int_flag;
-	/* WAIT_RCV that timed out, no interrupt */
-	u32 port_rcvwait_to;
-	/* WAIT_PIO that timed out, no interrupt */
-	u32 port_piowait_to;
-	/* WAIT_RCV already happened, no wait */
-	u32 port_rcvnowait;
-	/* WAIT_PIO already happened, no wait */
-	u32 port_pionowait;
-	/* total number of rcvhdrqfull errors */
-	u32 port_hdrqfull;
-	/*
-	 * Used to suppress multiple instances of same
-	 * port staying stuck at same point.
-	 */
-	u32 port_lastrcvhdrqtail;
-	/* saved total number of rcvhdrqfull errors for poll edge trigger */
-	u32 port_hdrqfull_poll;
-	/* total number of polled urgent packets */
-	u32 port_urgent;
-	/* saved total number of polled urgent packets for poll edge trigger */
-	u32 port_urgent_poll;
-	/* pid of process using this port */
-	struct pid *port_pid;
-	struct pid *port_subpid[INFINIPATH_MAX_SUBPORT];
-	/* same size as task_struct .comm[] */
-	char port_comm[TASK_COMM_LEN];
-	/* pkeys set by this use of this port */
-	u16 port_pkeys[4];
-	/* so file ops can get at unit */
-	struct ipath_devdata *port_dd;
-	/* A page of memory for rcvhdrhead, rcvegrhead, rcvegrtail * N */
-	void *subport_uregbase;
-	/* An array of pages for the eager receive buffers * N */
-	void *subport_rcvegrbuf;
-	/* An array of pages for the eager header queue entries * N */
-	void *subport_rcvhdr_base;
-	/* The version of the library which opened this port */
-	u32 userversion;
-	/* Bitmask of active slaves */
-	u32 active_slaves;
-	/* Type of packets or conditions we want to poll for */
-	u16 poll_type;
-	/* port rcvhdrq head offset */
-	u32 port_head;
-	/* receive packet sequence counter */
-	u32 port_seq_cnt;
-};
-
-struct sk_buff;
-struct ipath_sge_state;
-struct ipath_verbs_txreq;
-
-/*
- * control information for layered drivers
- */
-struct _ipath_layer {
-	void *l_arg;
-};
-
-struct ipath_skbinfo {
-	struct sk_buff *skb;
-	dma_addr_t phys;
-};
-
-struct ipath_sdma_txreq {
-	int                 flags;
-	int                 sg_count;
-	union {
-		struct scatterlist *sg;
-		void *map_addr;
-	};
-	void              (*callback)(void *, int);
-	void               *callback_cookie;
-	int                 callback_status;
-	u16                 start_idx;  /* sdma private */
-	u16                 next_descq_idx;  /* sdma private */
-	struct list_head    list;       /* sdma private */
-};
-
-struct ipath_sdma_desc {
-	__le64 qw[2];
-};
-
-#define IPATH_SDMA_TXREQ_F_USELARGEBUF  0x1
-#define IPATH_SDMA_TXREQ_F_HEADTOHOST   0x2
-#define IPATH_SDMA_TXREQ_F_INTREQ       0x4
-#define IPATH_SDMA_TXREQ_F_FREEBUF      0x8
-#define IPATH_SDMA_TXREQ_F_FREEDESC     0x10
-#define IPATH_SDMA_TXREQ_F_VL15         0x20
-
-#define IPATH_SDMA_TXREQ_S_OK        0
-#define IPATH_SDMA_TXREQ_S_SENDERROR 1
-#define IPATH_SDMA_TXREQ_S_ABORTED   2
-#define IPATH_SDMA_TXREQ_S_SHUTDOWN  3
-
-#define IPATH_SDMA_STATUS_SCORE_BOARD_DRAIN_IN_PROG	(1ull << 63)
-#define IPATH_SDMA_STATUS_ABORT_IN_PROG			(1ull << 62)
-#define IPATH_SDMA_STATUS_INTERNAL_SDMA_ENABLE		(1ull << 61)
-#define IPATH_SDMA_STATUS_SCB_EMPTY			(1ull << 30)
-
-/* max dwords in small buffer packet */
-#define IPATH_SMALLBUF_DWORDS (dd->ipath_piosize2k >> 2)
-
-/*
- * Possible IB config parameters for ipath_f_get/set_ib_cfg()
- */
-#define IPATH_IB_CFG_LIDLMC 0 /* Get/set LID (LS16b) and Mask (MS16b) */
-#define IPATH_IB_CFG_HRTBT 1 /* Get/set Heartbeat off/enable/auto */
-#define IPATH_IB_HRTBT_ON 3 /* Heartbeat enabled, sent every 100msec */
-#define IPATH_IB_HRTBT_OFF 0 /* Heartbeat off */
-#define IPATH_IB_CFG_LWID_ENB 2 /* Get/set allowed Link-width */
-#define IPATH_IB_CFG_LWID 3 /* Get currently active Link-width */
-#define IPATH_IB_CFG_SPD_ENB 4 /* Get/set allowed Link speeds */
-#define IPATH_IB_CFG_SPD 5 /* Get current Link spd */
-#define IPATH_IB_CFG_RXPOL_ENB 6 /* Get/set Auto-RX-polarity enable */
-#define IPATH_IB_CFG_LREV_ENB 7 /* Get/set Auto-Lane-reversal enable */
-#define IPATH_IB_CFG_LINKLATENCY 8 /* Get Auto-Lane-reversal enable */
-
-
-struct ipath_devdata {
-	struct list_head ipath_list;
-
-	struct ipath_kregs const *ipath_kregs;
-	struct ipath_cregs const *ipath_cregs;
-
-	/* mem-mapped pointer to base of chip regs */
-	u64 __iomem *ipath_kregbase;
-	/* end of mem-mapped chip space; range checking */
-	u64 __iomem *ipath_kregend;
-	/* physical address of chip for io_remap, etc. */
-	unsigned long ipath_physaddr;
-	/* base of memory alloced for ipath_kregbase, for free */
-	u64 *ipath_kregalloc;
-	/* ipath_cfgports pointers */
-	struct ipath_portdata **ipath_pd;
-	/* sk_buffs used by port 0 eager receive queue */
-	struct ipath_skbinfo *ipath_port0_skbinfo;
-	/* kvirt address of 1st 2k pio buffer */
-	void __iomem *ipath_pio2kbase;
-	/* kvirt address of 1st 4k pio buffer */
-	void __iomem *ipath_pio4kbase;
-	/*
-	 * points to area where PIOavail registers will be DMA'ed.
-	 * Has to be on a page of it's own, because the page will be
-	 * mapped into user program space.  This copy is *ONLY* ever
-	 * written by DMA, not by the driver!  Need a copy per device
-	 * when we get to multiple devices
-	 */
-	volatile __le64 *ipath_pioavailregs_dma;
-	/* physical address where updates occur */
-	dma_addr_t ipath_pioavailregs_phys;
-	struct _ipath_layer ipath_layer;
-	/* setup intr */
-	int (*ipath_f_intrsetup)(struct ipath_devdata *);
-	/* fallback to alternate interrupt type if possible */
-	int (*ipath_f_intr_fallback)(struct ipath_devdata *);
-	/* setup on-chip bus config */
-	int (*ipath_f_bus)(struct ipath_devdata *, struct pci_dev *);
-	/* hard reset chip */
-	int (*ipath_f_reset)(struct ipath_devdata *);
-	int (*ipath_f_get_boardname)(struct ipath_devdata *, char *,
-				     size_t);
-	void (*ipath_f_init_hwerrors)(struct ipath_devdata *);
-	void (*ipath_f_handle_hwerrors)(struct ipath_devdata *, char *,
-					size_t);
-	void (*ipath_f_quiet_serdes)(struct ipath_devdata *);
-	int (*ipath_f_bringup_serdes)(struct ipath_devdata *);
-	int (*ipath_f_early_init)(struct ipath_devdata *);
-	void (*ipath_f_clear_tids)(struct ipath_devdata *, unsigned);
-	void (*ipath_f_put_tid)(struct ipath_devdata *, u64 __iomem*,
-				u32, unsigned long);
-	void (*ipath_f_tidtemplate)(struct ipath_devdata *);
-	void (*ipath_f_cleanup)(struct ipath_devdata *);
-	void (*ipath_f_setextled)(struct ipath_devdata *, u64, u64);
-	/* fill out chip-specific fields */
-	int (*ipath_f_get_base_info)(struct ipath_portdata *, void *);
-	/* free irq */
-	void (*ipath_f_free_irq)(struct ipath_devdata *);
-	struct ipath_message_header *(*ipath_f_get_msgheader)
-					(struct ipath_devdata *, __le32 *);
-	void (*ipath_f_config_ports)(struct ipath_devdata *, ushort);
-	int (*ipath_f_get_ib_cfg)(struct ipath_devdata *, int);
-	int (*ipath_f_set_ib_cfg)(struct ipath_devdata *, int, u32);
-	void (*ipath_f_config_jint)(struct ipath_devdata *, u16 , u16);
-	void (*ipath_f_read_counters)(struct ipath_devdata *,
-					struct infinipath_counters *);
-	void (*ipath_f_xgxs_reset)(struct ipath_devdata *);
-	/* per chip actions needed for IB Link up/down changes */
-	int (*ipath_f_ib_updown)(struct ipath_devdata *, int, u64);
-
-	unsigned ipath_lastegr_idx;
-	struct ipath_ibdev *verbs_dev;
-	struct timer_list verbs_timer;
-	/* total dwords sent (summed from counter) */
-	u64 ipath_sword;
-	/* total dwords rcvd (summed from counter) */
-	u64 ipath_rword;
-	/* total packets sent (summed from counter) */
-	u64 ipath_spkts;
-	/* total packets rcvd (summed from counter) */
-	u64 ipath_rpkts;
-	/* ipath_statusp initially points to this. */
-	u64 _ipath_status;
-	/* GUID for this interface, in network order */
-	__be64 ipath_guid;
-	/*
-	 * aggregrate of error bits reported since last cleared, for
-	 * limiting of error reporting
-	 */
-	ipath_err_t ipath_lasterror;
-	/*
-	 * aggregrate of error bits reported since last cleared, for
-	 * limiting of hwerror reporting
-	 */
-	ipath_err_t ipath_lasthwerror;
-	/* errors masked because they occur too fast */
-	ipath_err_t ipath_maskederrs;
-	u64 ipath_lastlinkrecov; /* link recoveries at last ACTIVE */
-	/* these 5 fields are used to establish deltas for IB Symbol
-	 * errors and linkrecovery errors. They can be reported on
-	 * some chips during link negotiation prior to INIT, and with
-	 * DDR when faking DDR negotiations with non-IBTA switches.
-	 * The chip counters are adjusted at driver unload if there is
-	 * a non-zero delta.
-	 */
-	u64 ibdeltainprog;
-	u64 ibsymdelta;
-	u64 ibsymsnap;
-	u64 iblnkerrdelta;
-	u64 iblnkerrsnap;
-
-	/* time in jiffies at which to re-enable maskederrs */
-	unsigned long ipath_unmasktime;
-	/* count of egrfull errors, combined for all ports */
-	u64 ipath_last_tidfull;
-	/* for ipath_qcheck() */
-	u64 ipath_lastport0rcv_cnt;
-	/* template for writing TIDs  */
-	u64 ipath_tidtemplate;
-	/* value to write to free TIDs */
-	u64 ipath_tidinvalid;
-	/* IBA6120 rcv interrupt setup */
-	u64 ipath_rhdrhead_intr_off;
-
-	/* size of memory at ipath_kregbase */
-	u32 ipath_kregsize;
-	/* number of registers used for pioavail */
-	u32 ipath_pioavregs;
-	/* IPATH_POLL, etc. */
-	u32 ipath_flags;
-	/* ipath_flags driver is waiting for */
-	u32 ipath_state_wanted;
-	/* last buffer for user use, first buf for kernel use is this
-	 * index. */
-	u32 ipath_lastport_piobuf;
-	/* is a stats timer active */
-	u32 ipath_stats_timer_active;
-	/* number of interrupts for this device -- saturates... */
-	u32 ipath_int_counter;
-	/* dwords sent read from counter */
-	u32 ipath_lastsword;
-	/* dwords received read from counter */
-	u32 ipath_lastrword;
-	/* sent packets read from counter */
-	u32 ipath_lastspkts;
-	/* received packets read from counter */
-	u32 ipath_lastrpkts;
-	/* pio bufs allocated per port */
-	u32 ipath_pbufsport;
-	/* if remainder on bufs/port, ports < extrabuf get 1 extra */
-	u32 ipath_ports_extrabuf;
-	u32 ipath_pioupd_thresh; /* update threshold, some chips */
-	/*
-	 * number of ports configured as max; zero is set to number chip
-	 * supports, less gives more pio bufs/port, etc.
-	 */
-	u32 ipath_cfgports;
-	/* count of port 0 hdrqfull errors */
-	u32 ipath_p0_hdrqfull;
-	/* port 0 number of receive eager buffers */
-	u32 ipath_p0_rcvegrcnt;
-
-	/*
-	 * index of last piobuffer we used.  Speeds up searching, by
-	 * starting at this point.  Doesn't matter if multiple cpu's use and
-	 * update, last updater is only write that matters.  Whenever it
-	 * wraps, we update shadow copies.  Need a copy per device when we
-	 * get to multiple devices
-	 */
-	u32 ipath_lastpioindex;
-	u32 ipath_lastpioindexl;
-	/* max length of freezemsg */
-	u32 ipath_freezelen;
-	/*
-	 * consecutive times we wanted a PIO buffer but were unable to
-	 * get one
-	 */
-	u32 ipath_consec_nopiobuf;
-	/*
-	 * hint that we should update ipath_pioavailshadow before
-	 * looking for a PIO buffer
-	 */
-	u32 ipath_upd_pio_shadow;
-	/* so we can rewrite it after a chip reset */
-	u32 ipath_pcibar0;
-	/* so we can rewrite it after a chip reset */
-	u32 ipath_pcibar1;
-	u32 ipath_x1_fix_tries;
-	u32 ipath_autoneg_tries;
-	u32 serdes_first_init_done;
-
-	struct ipath_relock {
-		atomic_t ipath_relock_timer_active;
-		struct timer_list ipath_relock_timer;
-		unsigned int ipath_relock_interval; /* in jiffies */
-	} ipath_relock_singleton;
-
-	/* interrupt number */
-	int ipath_irq;
-	/* HT/PCI Vendor ID (here for NodeInfo) */
-	u16 ipath_vendorid;
-	/* HT/PCI Device ID (here for NodeInfo) */
-	u16 ipath_deviceid;
-	/* offset in HT config space of slave/primary interface block */
-	u8 ipath_ht_slave_off;
-	/* for write combining settings */
-	int wc_cookie;
-	/* ref count for each pkey */
-	atomic_t ipath_pkeyrefs[4];
-	/* shadow copy of struct page *'s for exp tid pages */
-	struct page **ipath_pageshadow;
-	/* shadow copy of dma handles for exp tid pages */
-	dma_addr_t *ipath_physshadow;
-	u64 __iomem *ipath_egrtidbase;
-	/* lock to workaround chip bug 9437 and others */
-	spinlock_t ipath_kernel_tid_lock;
-	spinlock_t ipath_user_tid_lock;
-	spinlock_t ipath_sendctrl_lock;
-	/* around ipath_pd and (user ports) port_cnt use (intr vs free) */
-	spinlock_t ipath_uctxt_lock;
-
-	/*
-	 * IPATH_STATUS_*,
-	 * this address is mapped readonly into user processes so they can
-	 * get status cheaply, whenever they want.
-	 */
-	u64 *ipath_statusp;
-	/* freeze msg if hw error put chip in freeze */
-	char *ipath_freezemsg;
-	/* pci access data structure */
-	struct pci_dev *pcidev;
-	struct cdev *user_cdev;
-	struct cdev *diag_cdev;
-	struct device *user_dev;
-	struct device *diag_dev;
-	/* timer used to prevent stats overflow, error throttling, etc. */
-	struct timer_list ipath_stats_timer;
-	/* timer to verify interrupts work, and fallback if possible */
-	struct timer_list ipath_intrchk_timer;
-	void *ipath_dummy_hdrq;	/* used after port close */
-	dma_addr_t ipath_dummy_hdrq_phys;
-
-	/* SendDMA related entries */
-	spinlock_t            ipath_sdma_lock;
-	unsigned long         ipath_sdma_status;
-	unsigned long         ipath_sdma_abort_jiffies;
-	unsigned long         ipath_sdma_abort_intr_timeout;
-	unsigned long         ipath_sdma_buf_jiffies;
-	struct ipath_sdma_desc *ipath_sdma_descq;
-	u64		      ipath_sdma_descq_added;
-	u64		      ipath_sdma_descq_removed;
-	int		      ipath_sdma_desc_nreserved;
-	u16                   ipath_sdma_descq_cnt;
-	u16                   ipath_sdma_descq_tail;
-	u16                   ipath_sdma_descq_head;
-	u16                   ipath_sdma_next_intr;
-	u16                   ipath_sdma_reset_wait;
-	u8                    ipath_sdma_generation;
-	struct tasklet_struct ipath_sdma_abort_task;
-	struct tasklet_struct ipath_sdma_notify_task;
-	struct list_head      ipath_sdma_activelist;
-	struct list_head      ipath_sdma_notifylist;
-	atomic_t              ipath_sdma_vl15_count;
-	struct timer_list     ipath_sdma_vl15_timer;
-
-	dma_addr_t       ipath_sdma_descq_phys;
-	volatile __le64 *ipath_sdma_head_dma;
-	dma_addr_t       ipath_sdma_head_phys;
-
-	unsigned long ipath_ureg_align; /* user register alignment */
-
-	struct delayed_work ipath_autoneg_work;
-	wait_queue_head_t ipath_autoneg_wait;
-
-	/* HoL blocking / user app forward-progress state */
-	unsigned          ipath_hol_state;
-	unsigned          ipath_hol_next;
-	struct timer_list ipath_hol_timer;
-
-	/*
-	 * Shadow copies of registers; size indicates read access size.
-	 * Most of them are readonly, but some are write-only register,
-	 * where we manipulate the bits in the shadow copy, and then write
-	 * the shadow copy to infinipath.
-	 *
-	 * We deliberately make most of these 32 bits, since they have
-	 * restricted range.  For any that we read, we won't to generate 32
-	 * bit accesses, since Opteron will generate 2 separate 32 bit HT
-	 * transactions for a 64 bit read, and we want to avoid unnecessary
-	 * HT transactions.
-	 */
-
-	/* This is the 64 bit group */
-
-	/*
-	 * shadow of pioavail, check to be sure it's large enough at
-	 * init time.
-	 */
-	unsigned long ipath_pioavailshadow[8];
-	/* bitmap of send buffers available for the kernel to use with PIO. */
-	unsigned long ipath_pioavailkernel[8];
-	/* shadow of kr_gpio_out, for rmw ops */
-	u64 ipath_gpio_out;
-	/* shadow the gpio mask register */
-	u64 ipath_gpio_mask;
-	/* shadow the gpio output enable, etc... */
-	u64 ipath_extctrl;
-	/* kr_revision shadow */
-	u64 ipath_revision;
-	/*
-	 * shadow of ibcctrl, for interrupt handling of link changes,
-	 * etc.
-	 */
-	u64 ipath_ibcctrl;
-	/*
-	 * last ibcstatus, to suppress "duplicate" status change messages,
-	 * mostly from 2 to 3
-	 */
-	u64 ipath_lastibcstat;
-	/* hwerrmask shadow */
-	ipath_err_t ipath_hwerrmask;
-	ipath_err_t ipath_errormask; /* errormask shadow */
-	/* interrupt config reg shadow */
-	u64 ipath_intconfig;
-	/* kr_sendpiobufbase value */
-	u64 ipath_piobufbase;
-	/* kr_ibcddrctrl shadow */
-	u64 ipath_ibcddrctrl;
-
-	/* these are the "32 bit" regs */
-
-	/*
-	 * number of GUIDs in the flash for this interface; may need some
-	 * rethinking for setting on other ifaces
-	 */
-	u32 ipath_nguid;
-	/*
-	 * the following two are 32-bit bitmasks, but {test,clear,set}_bit
-	 * all expect bit fields to be "unsigned long"
-	 */
-	/* shadow kr_rcvctrl */
-	unsigned long ipath_rcvctrl;
-	/* shadow kr_sendctrl */
-	unsigned long ipath_sendctrl;
-	/* to not count armlaunch after cancel */
-	unsigned long ipath_lastcancel;
-	/* count cases where special trigger was needed (double write) */
-	unsigned long ipath_spectriggerhit;
-
-	/* value we put in kr_rcvhdrcnt */
-	u32 ipath_rcvhdrcnt;
-	/* value we put in kr_rcvhdrsize */
-	u32 ipath_rcvhdrsize;
-	/* value we put in kr_rcvhdrentsize */
-	u32 ipath_rcvhdrentsize;
-	/* offset of last entry in rcvhdrq */
-	u32 ipath_hdrqlast;
-	/* kr_portcnt value */
-	u32 ipath_portcnt;
-	/* kr_pagealign value */
-	u32 ipath_palign;
-	/* number of "2KB" PIO buffers */
-	u32 ipath_piobcnt2k;
-	/* size in bytes of "2KB" PIO buffers */
-	u32 ipath_piosize2k;
-	/* number of "4KB" PIO buffers */
-	u32 ipath_piobcnt4k;
-	/* size in bytes of "4KB" PIO buffers */
-	u32 ipath_piosize4k;
-	u32 ipath_pioreserved; /* reserved special-inkernel; */
-	/* kr_rcvegrbase value */
-	u32 ipath_rcvegrbase;
-	/* kr_rcvegrcnt value */
-	u32 ipath_rcvegrcnt;
-	/* kr_rcvtidbase value */
-	u32 ipath_rcvtidbase;
-	/* kr_rcvtidcnt value */
-	u32 ipath_rcvtidcnt;
-	/* kr_sendregbase */
-	u32 ipath_sregbase;
-	/* kr_userregbase */
-	u32 ipath_uregbase;
-	/* kr_counterregbase */
-	u32 ipath_cregbase;
-	/* shadow the control register contents */
-	u32 ipath_control;
-	/* PCI revision register (HTC rev on FPGA) */
-	u32 ipath_pcirev;
-
-	/* chip address space used by 4k pio buffers */
-	u32 ipath_4kalign;
-	/* The MTU programmed for this unit */
-	u32 ipath_ibmtu;
-	/*
-	 * The max size IB packet, included IB headers that we can send.
-	 * Starts same as ipath_piosize, but is affected when ibmtu is
-	 * changed, or by size of eager buffers
-	 */
-	u32 ipath_ibmaxlen;
-	/*
-	 * ibmaxlen at init time, limited by chip and by receive buffer
-	 * size.  Not changed after init.
-	 */
-	u32 ipath_init_ibmaxlen;
-	/* size of each rcvegrbuffer */
-	u32 ipath_rcvegrbufsize;
-	/* localbus width (1, 2,4,8,16,32) from config space  */
-	u32 ipath_lbus_width;
-	/* localbus speed (HT: 200,400,800,1000; PCIe 2500) */
-	u32 ipath_lbus_speed;
-	/*
-	 * number of sequential ibcstatus change for polling active/quiet
-	 * (i.e., link not coming up).
-	 */
-	u32 ipath_ibpollcnt;
-	/* low and high portions of MSI capability/vector */
-	u32 ipath_msi_lo;
-	/* saved after PCIe init for restore after reset */
-	u32 ipath_msi_hi;
-	/* MSI data (vector) saved for restore */
-	u16 ipath_msi_data;
-	/* MLID programmed for this instance */
-	u16 ipath_mlid;
-	/* LID programmed for this instance */
-	u16 ipath_lid;
-	/* list of pkeys programmed; 0 if not set */
-	u16 ipath_pkeys[4];
-	/*
-	 * ASCII serial number, from flash, large enough for original
-	 * all digit strings, and longer QLogic serial number format
-	 */
-	u8 ipath_serial[16];
-	/* human readable board version */
-	u8 ipath_boardversion[96];
-	u8 ipath_lbus_info[32]; /* human readable localbus info */
-	/* chip major rev, from ipath_revision */
-	u8 ipath_majrev;
-	/* chip minor rev, from ipath_revision */
-	u8 ipath_minrev;
-	/* board rev, from ipath_revision */
-	u8 ipath_boardrev;
-	/* saved for restore after reset */
-	u8 ipath_pci_cacheline;
-	/* LID mask control */
-	u8 ipath_lmc;
-	/* link width supported */
-	u8 ipath_link_width_supported;
-	/* link speed supported */
-	u8 ipath_link_speed_supported;
-	u8 ipath_link_width_enabled;
-	u8 ipath_link_speed_enabled;
-	u8 ipath_link_width_active;
-	u8 ipath_link_speed_active;
-	/* Rx Polarity inversion (compensate for ~tx on partner) */
-	u8 ipath_rx_pol_inv;
-
-	u8 ipath_r_portenable_shift;
-	u8 ipath_r_intravail_shift;
-	u8 ipath_r_tailupd_shift;
-	u8 ipath_r_portcfg_shift;
-
-	/* unit # of this chip, if present */
-	int ipath_unit;
-
-	/* local link integrity counter */
-	u32 ipath_lli_counter;
-	/* local link integrity errors */
-	u32 ipath_lli_errors;
-	/*
-	 * Above counts only cases where _successive_ LocalLinkIntegrity
-	 * errors were seen in the receive headers of kern-packets.
-	 * Below are the three (monotonically increasing) counters
-	 * maintained via GPIO interrupts on iba6120-rev2.
-	 */
-	u32 ipath_rxfc_unsupvl_errs;
-	u32 ipath_overrun_thresh_errs;
-	u32 ipath_lli_errs;
-
-	/*
-	 * Not all devices managed by a driver instance are the same
-	 * type, so these fields must be per-device.
-	 */
-	u64 ipath_i_bitsextant;
-	ipath_err_t ipath_e_bitsextant;
-	ipath_err_t ipath_hwe_bitsextant;
-
-	/*
-	 * Below should be computable from number of ports,
-	 * since they are never modified.
-	 */
-	u64 ipath_i_rcvavail_mask;
-	u64 ipath_i_rcvurg_mask;
-	u16 ipath_i_rcvurg_shift;
-	u16 ipath_i_rcvavail_shift;
-
-	/*
-	 * Register bits for selecting i2c direction and values, used for
-	 * I2C serial flash.
-	 */
-	u8 ipath_gpio_sda_num;
-	u8 ipath_gpio_scl_num;
-	u8 ipath_i2c_chain_type;
-	u64 ipath_gpio_sda;
-	u64 ipath_gpio_scl;
-
-	/* lock for doing RMW of shadows/regs for ExtCtrl and GPIO */
-	spinlock_t ipath_gpio_lock;
-
-	/*
-	 * IB link and linktraining states and masks that vary per chip in
-	 * some way.  Set at init, to avoid each IB status change interrupt
-	 */
-	u8 ibcs_ls_shift;
-	u8 ibcs_lts_mask;
-	u32 ibcs_mask;
-	u32 ib_init;
-	u32 ib_arm;
-	u32 ib_active;
-
-	u16 ipath_rhf_offset; /* offset of RHF within receive header entry */
-
-	/*
-	 * shift/mask for linkcmd, linkinitcmd, maxpktlen in ibccontol
-	 * reg. Changes for IBA7220
-	 */
-	u8 ibcc_lic_mask; /* LinkInitCmd */
-	u8 ibcc_lc_shift; /* LinkCmd */
-	u8 ibcc_mpl_shift; /* Maxpktlen */
-
-	u8 delay_mult;
-
-	/* used to override LED behavior */
-	u8 ipath_led_override;  /* Substituted for normal value, if non-zero */
-	u16 ipath_led_override_timeoff; /* delta to next timer event */
-	u8 ipath_led_override_vals[2]; /* Alternates per blink-frame */
-	u8 ipath_led_override_phase; /* Just counts, LSB picks from vals[] */
-	atomic_t ipath_led_override_timer_active;
-	/* Used to flash LEDs in override mode */
-	struct timer_list ipath_led_override_timer;
-
-	/* Support (including locks) for EEPROM logging of errors and time */
-	/* control access to actual counters, timer */
-	spinlock_t ipath_eep_st_lock;
-	/* control high-level access to EEPROM */
-	struct mutex ipath_eep_lock;
-	/* Below inc'd by ipath_snap_cntrs(), locked by ipath_eep_st_lock */
-	uint64_t ipath_traffic_wds;
-	/* active time is kept in seconds, but logged in hours */
-	atomic_t ipath_active_time;
-	/* Below are nominal shadow of EEPROM, new since last EEPROM update */
-	uint8_t ipath_eep_st_errs[IPATH_EEP_LOG_CNT];
-	uint8_t ipath_eep_st_new_errs[IPATH_EEP_LOG_CNT];
-	uint16_t ipath_eep_hrs;
-	/*
-	 * masks for which bits of errs, hwerrs that cause
-	 * each of the counters to increment.
-	 */
-	struct ipath_eep_log_mask ipath_eep_st_masks[IPATH_EEP_LOG_CNT];
-
-	/* interrupt mitigation reload register info */
-	u16 ipath_jint_idle_ticks;	/* idle clock ticks */
-	u16 ipath_jint_max_packets;	/* max packets across all ports */
-
-	/*
-	 * lock for access to SerDes, and flags to sequence preset
-	 * versus steady-state. 7220-only at the moment.
-	 */
-	spinlock_t ipath_sdepb_lock;
-	u8 ipath_presets_needed; /* Set if presets to be restored next DOWN */
-};
-
-/* ipath_hol_state values (stopping/starting user proc, send flushing) */
-#define IPATH_HOL_UP       0
-#define IPATH_HOL_DOWN     1
-/* ipath_hol_next toggle values, used when hol_state IPATH_HOL_DOWN */
-#define IPATH_HOL_DOWNSTOP 0
-#define IPATH_HOL_DOWNCONT 1
-
-/* bit positions for sdma_status */
-#define IPATH_SDMA_ABORTING  0
-#define IPATH_SDMA_DISARMED  1
-#define IPATH_SDMA_DISABLED  2
-#define IPATH_SDMA_LAYERBUF  3
-#define IPATH_SDMA_RUNNING  30
-#define IPATH_SDMA_SHUTDOWN 31
-
-/* bit combinations that correspond to abort states */
-#define IPATH_SDMA_ABORT_NONE 0
-#define IPATH_SDMA_ABORT_ABORTING (1UL << IPATH_SDMA_ABORTING)
-#define IPATH_SDMA_ABORT_DISARMED ((1UL << IPATH_SDMA_ABORTING) | \
-	(1UL << IPATH_SDMA_DISARMED))
-#define IPATH_SDMA_ABORT_DISABLED ((1UL << IPATH_SDMA_ABORTING) | \
-	(1UL << IPATH_SDMA_DISABLED))
-#define IPATH_SDMA_ABORT_ABORTED ((1UL << IPATH_SDMA_ABORTING) | \
-	(1UL << IPATH_SDMA_DISARMED) | (1UL << IPATH_SDMA_DISABLED))
-#define IPATH_SDMA_ABORT_MASK ((1UL<<IPATH_SDMA_ABORTING) | \
-	(1UL << IPATH_SDMA_DISARMED) | (1UL << IPATH_SDMA_DISABLED))
-
-#define IPATH_SDMA_BUF_NONE 0
-#define IPATH_SDMA_BUF_MASK (1UL<<IPATH_SDMA_LAYERBUF)
-
-/* Private data for file operations */
-struct ipath_filedata {
-	struct ipath_portdata *pd;
-	unsigned subport;
-	unsigned tidcursor;
-	struct ipath_user_sdma_queue *pq;
-};
-extern struct list_head ipath_dev_list;
-extern spinlock_t ipath_devs_lock;
-extern struct ipath_devdata *ipath_lookup(int unit);
-
-int ipath_init_chip(struct ipath_devdata *, int);
-int ipath_enable_wc(struct ipath_devdata *dd);
-void ipath_disable_wc(struct ipath_devdata *dd);
-int ipath_count_units(int *npresentp, int *nupp, int *maxportsp);
-void ipath_shutdown_device(struct ipath_devdata *);
-void ipath_clear_freeze(struct ipath_devdata *);
-
-struct file_operations;
-int ipath_cdev_init(int minor, char *name, const struct file_operations *fops,
-		    struct cdev **cdevp, struct device **devp);
-void ipath_cdev_cleanup(struct cdev **cdevp,
-			struct device **devp);
-
-int ipath_diag_add(struct ipath_devdata *);
-void ipath_diag_remove(struct ipath_devdata *);
-
-extern wait_queue_head_t ipath_state_wait;
-
-int ipath_user_add(struct ipath_devdata *dd);
-void ipath_user_remove(struct ipath_devdata *dd);
-
-struct sk_buff *ipath_alloc_skb(struct ipath_devdata *dd, gfp_t);
-
-extern int ipath_diag_inuse;
-
-irqreturn_t ipath_intr(int irq, void *devid);
-int ipath_decode_err(struct ipath_devdata *dd, char *buf, size_t blen,
-		     ipath_err_t err);
-#if __IPATH_INFO || __IPATH_DBG
-extern const char *ipath_ibcstatus_str[];
-#endif
-
-/* clean up any per-chip chip-specific stuff */
-void ipath_chip_cleanup(struct ipath_devdata *);
-/* clean up any chip type-specific stuff */
-void ipath_chip_done(void);
-
-void ipath_disarm_piobufs(struct ipath_devdata *, unsigned first,
-			  unsigned cnt);
-void ipath_cancel_sends(struct ipath_devdata *, int);
-
-int ipath_create_rcvhdrq(struct ipath_devdata *, struct ipath_portdata *);
-void ipath_free_pddata(struct ipath_devdata *, struct ipath_portdata *);
-
-int ipath_parse_ushort(const char *str, unsigned short *valp);
-
-void ipath_kreceive(struct ipath_portdata *);
-int ipath_setrcvhdrsize(struct ipath_devdata *, unsigned);
-int ipath_reset_device(int);
-void ipath_get_faststats(unsigned long);
-int ipath_wait_linkstate(struct ipath_devdata *, u32, int);
-int ipath_set_linkstate(struct ipath_devdata *, u8);
-int ipath_set_mtu(struct ipath_devdata *, u16);
-int ipath_set_lid(struct ipath_devdata *, u32, u8);
-int ipath_set_rx_pol_inv(struct ipath_devdata *dd, u8 new_pol_inv);
-void ipath_enable_armlaunch(struct ipath_devdata *);
-void ipath_disable_armlaunch(struct ipath_devdata *);
-void ipath_hol_down(struct ipath_devdata *);
-void ipath_hol_up(struct ipath_devdata *);
-void ipath_hol_event(unsigned long);
-void ipath_toggle_rclkrls(struct ipath_devdata *);
-void ipath_sd7220_clr_ibpar(struct ipath_devdata *);
-void ipath_set_relock_poll(struct ipath_devdata *, int);
-void ipath_shutdown_relock_poll(struct ipath_devdata *);
-
-/* for use in system calls, where we want to know device type, etc. */
-#define port_fp(fp) ((struct ipath_filedata *)(fp)->private_data)->pd
-#define subport_fp(fp) \
-	((struct ipath_filedata *)(fp)->private_data)->subport
-#define tidcursor_fp(fp) \
-	((struct ipath_filedata *)(fp)->private_data)->tidcursor
-#define user_sdma_queue_fp(fp) \
-	((struct ipath_filedata *)(fp)->private_data)->pq
-
-/*
- * values for ipath_flags
- */
-		/* chip can report link latency (IB 1.2) */
-#define IPATH_HAS_LINK_LATENCY 0x1
-		/* The chip is up and initted */
-#define IPATH_INITTED       0x2
-		/* set if any user code has set kr_rcvhdrsize */
-#define IPATH_RCVHDRSZ_SET  0x4
-		/* The chip is present and valid for accesses */
-#define IPATH_PRESENT       0x8
-		/* HT link0 is only 8 bits wide, ignore upper byte crc
-		 * errors, etc. */
-#define IPATH_8BIT_IN_HT0   0x10
-		/* HT link1 is only 8 bits wide, ignore upper byte crc
-		 * errors, etc. */
-#define IPATH_8BIT_IN_HT1   0x20
-		/* The link is down */
-#define IPATH_LINKDOWN      0x40
-		/* The link level is up (0x11) */
-#define IPATH_LINKINIT      0x80
-		/* The link is in the armed (0x21) state */
-#define IPATH_LINKARMED     0x100
-		/* The link is in the active (0x31) state */
-#define IPATH_LINKACTIVE    0x200
-		/* link current state is unknown */
-#define IPATH_LINKUNK       0x400
-		/* Write combining flush needed for PIO */
-#define IPATH_PIO_FLUSH_WC  0x1000
-		/* DMA Receive tail pointer */
-#define IPATH_NODMA_RTAIL   0x2000
-		/* no IB cable, or no device on IB cable */
-#define IPATH_NOCABLE       0x4000
-		/* Supports port zero per packet receive interrupts via
-		 * GPIO */
-#define IPATH_GPIO_INTR     0x8000
-		/* uses the coded 4byte TID, not 8 byte */
-#define IPATH_4BYTE_TID     0x10000
-		/* packet/word counters are 32 bit, else those 4 counters
-		 * are 64bit */
-#define IPATH_32BITCOUNTERS 0x20000
-		/* Interrupt register is 64 bits */
-#define IPATH_INTREG_64     0x40000
-		/* can miss port0 rx interrupts */
-#define IPATH_DISABLED      0x80000 /* administratively disabled */
-		/* Use GPIO interrupts for new counters */
-#define IPATH_GPIO_ERRINTRS 0x100000
-#define IPATH_SWAP_PIOBUFS  0x200000
-		/* Supports Send DMA */
-#define IPATH_HAS_SEND_DMA  0x400000
-		/* Supports Send Count (not just word count) in PBC */
-#define IPATH_HAS_PBC_CNT   0x800000
-		/* Suppress heartbeat, even if turning off loopback */
-#define IPATH_NO_HRTBT      0x1000000
-#define IPATH_HAS_THRESH_UPDATE 0x4000000
-#define IPATH_HAS_MULT_IB_SPEED 0x8000000
-#define IPATH_IB_AUTONEG_INPROG 0x10000000
-#define IPATH_IB_AUTONEG_FAILED 0x20000000
-		/* Linkdown-disable intentionally, Do not attempt to bring up */
-#define IPATH_IB_LINK_DISABLED 0x40000000
-#define IPATH_IB_FORCE_NOTIFY 0x80000000 /* force notify on next ib change */
-
-/* Bits in GPIO for the added interrupts */
-#define IPATH_GPIO_PORT0_BIT 2
-#define IPATH_GPIO_RXUVL_BIT 3
-#define IPATH_GPIO_OVRUN_BIT 4
-#define IPATH_GPIO_LLI_BIT 5
-#define IPATH_GPIO_ERRINTR_MASK 0x38
-
-/* portdata flag bit offsets */
-		/* waiting for a packet to arrive */
-#define IPATH_PORT_WAITING_RCV   2
-		/* master has not finished initializing */
-#define IPATH_PORT_MASTER_UNINIT 4
-		/* waiting for an urgent packet to arrive */
-#define IPATH_PORT_WAITING_URG 5
-
-/* free up any allocated data at closes */
-void ipath_free_data(struct ipath_portdata *dd);
-u32 __iomem *ipath_getpiobuf(struct ipath_devdata *, u32, u32 *);
-void ipath_chg_pioavailkernel(struct ipath_devdata *dd, unsigned start,
-				unsigned len, int avail);
-void ipath_init_iba6110_funcs(struct ipath_devdata *);
-void ipath_get_eeprom_info(struct ipath_devdata *);
-int ipath_update_eeprom_log(struct ipath_devdata *dd);
-void ipath_inc_eeprom_err(struct ipath_devdata *dd, u32 eidx, u32 incr);
-u64 ipath_snap_cntr(struct ipath_devdata *, ipath_creg);
-void ipath_disarm_senderrbufs(struct ipath_devdata *);
-void ipath_force_pio_avail_update(struct ipath_devdata *);
-void signal_ib_event(struct ipath_devdata *dd, enum ib_event_type ev);
-
-/*
- * Set LED override, only the two LSBs have "public" meaning, but
- * any non-zero value substitutes them for the Link and LinkTrain
- * LED states.
- */
-#define IPATH_LED_PHYS 1 /* Physical (linktraining) GREEN LED */
-#define IPATH_LED_LOG 2  /* Logical (link) YELLOW LED */
-void ipath_set_led_override(struct ipath_devdata *dd, unsigned int val);
-
-/* send dma routines */
-int setup_sdma(struct ipath_devdata *);
-void teardown_sdma(struct ipath_devdata *);
-void ipath_restart_sdma(struct ipath_devdata *);
-void ipath_sdma_intr(struct ipath_devdata *);
-int ipath_sdma_verbs_send(struct ipath_devdata *, struct ipath_sge_state *,
-			  u32, struct ipath_verbs_txreq *);
-/* ipath_sdma_lock should be locked before calling this. */
-int ipath_sdma_make_progress(struct ipath_devdata *dd);
-
-/* must be called under ipath_sdma_lock */
-static inline u16 ipath_sdma_descq_freecnt(const struct ipath_devdata *dd)
-{
-	return dd->ipath_sdma_descq_cnt -
-		(dd->ipath_sdma_descq_added - dd->ipath_sdma_descq_removed) -
-		1 - dd->ipath_sdma_desc_nreserved;
-}
-
-static inline void ipath_sdma_desc_reserve(struct ipath_devdata *dd, u16 cnt)
-{
-	dd->ipath_sdma_desc_nreserved += cnt;
-}
-
-static inline void ipath_sdma_desc_unreserve(struct ipath_devdata *dd, u16 cnt)
-{
-	dd->ipath_sdma_desc_nreserved -= cnt;
-}
-
-/*
- * number of words used for protocol header if not set by ipath_userinit();
- */
-#define IPATH_DFLT_RCVHDRSIZE 9
-
-int ipath_get_user_pages(unsigned long, size_t, struct page **);
-void ipath_release_user_pages(struct page **, size_t);
-void ipath_release_user_pages_on_close(struct page **, size_t);
-int ipath_eeprom_read(struct ipath_devdata *, u8, void *, int);
-int ipath_eeprom_write(struct ipath_devdata *, u8, const void *, int);
-int ipath_tempsense_read(struct ipath_devdata *, u8 regnum);
-int ipath_tempsense_write(struct ipath_devdata *, u8 regnum, u8 data);
-
-/* these are used for the registers that vary with port */
-void ipath_write_kreg_port(const struct ipath_devdata *, ipath_kreg,
-			   unsigned, u64);
-
-/*
- * We could have a single register get/put routine, that takes a group type,
- * but this is somewhat clearer and cleaner.  It also gives us some error
- * checking.  64 bit register reads should always work, but are inefficient
- * on opteron (the northbridge always generates 2 separate HT 32 bit reads),
- * so we use kreg32 wherever possible.  User register and counter register
- * reads are always 32 bit reads, so only one form of those routines.
- */
-
-/*
- * At the moment, none of the s-registers are writable, so no
- * ipath_write_sreg().
- */
-
-/**
- * ipath_read_ureg32 - read 32-bit virtualized per-port register
- * @dd: device
- * @regno: register number
- * @port: port number
- *
- * Return the contents of a register that is virtualized to be per port.
- * Returns -1 on errors (not distinguishable from valid contents at
- * runtime; we may add a separate error variable at some point).
- */
-static inline u32 ipath_read_ureg32(const struct ipath_devdata *dd,
-				    ipath_ureg regno, int port)
-{
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT))
-		return 0;
-
-	return readl(regno + (u64 __iomem *)
-		     (dd->ipath_uregbase +
-		      (char __iomem *)dd->ipath_kregbase +
-		      dd->ipath_ureg_align * port));
-}
-
-/**
- * ipath_write_ureg - write 32-bit virtualized per-port register
- * @dd: device
- * @regno: register number
- * @value: value
- * @port: port
- *
- * Write the contents of a register that is virtualized to be per port.
- */
-static inline void ipath_write_ureg(const struct ipath_devdata *dd,
-				    ipath_ureg regno, u64 value, int port)
-{
-	u64 __iomem *ubase = (u64 __iomem *)
-		(dd->ipath_uregbase + (char __iomem *) dd->ipath_kregbase +
-		 dd->ipath_ureg_align * port);
-	if (dd->ipath_kregbase)
-		writeq(value, &ubase[regno]);
-}
-
-static inline u32 ipath_read_kreg32(const struct ipath_devdata *dd,
-				    ipath_kreg regno)
-{
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT))
-		return -1;
-	return readl((u32 __iomem *) & dd->ipath_kregbase[regno]);
-}
-
-static inline u64 ipath_read_kreg64(const struct ipath_devdata *dd,
-				    ipath_kreg regno)
-{
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT))
-		return -1;
-
-	return readq(&dd->ipath_kregbase[regno]);
-}
-
-static inline void ipath_write_kreg(const struct ipath_devdata *dd,
-				    ipath_kreg regno, u64 value)
-{
-	if (dd->ipath_kregbase)
-		writeq(value, &dd->ipath_kregbase[regno]);
-}
-
-static inline u64 ipath_read_creg(const struct ipath_devdata *dd,
-				  ipath_sreg regno)
-{
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT))
-		return 0;
-
-	return readq(regno + (u64 __iomem *)
-		     (dd->ipath_cregbase +
-		      (char __iomem *)dd->ipath_kregbase));
-}
-
-static inline u32 ipath_read_creg32(const struct ipath_devdata *dd,
-					 ipath_sreg regno)
-{
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_PRESENT))
-		return 0;
-	return readl(regno + (u64 __iomem *)
-		     (dd->ipath_cregbase +
-		      (char __iomem *)dd->ipath_kregbase));
-}
-
-static inline void ipath_write_creg(const struct ipath_devdata *dd,
-				    ipath_creg regno, u64 value)
-{
-	if (dd->ipath_kregbase)
-		writeq(value, regno + (u64 __iomem *)
-		       (dd->ipath_cregbase +
-			(char __iomem *)dd->ipath_kregbase));
-}
-
-static inline void ipath_clear_rcvhdrtail(const struct ipath_portdata *pd)
-{
-	*((u64 *) pd->port_rcvhdrtail_kvaddr) = 0ULL;
-}
-
-static inline u32 ipath_get_rcvhdrtail(const struct ipath_portdata *pd)
-{
-	return (u32) le64_to_cpu(*((volatile __le64 *)
-				pd->port_rcvhdrtail_kvaddr));
-}
-
-static inline u32 ipath_get_hdrqtail(const struct ipath_portdata *pd)
-{
-	const struct ipath_devdata *dd = pd->port_dd;
-	u32 hdrqtail;
-
-	if (dd->ipath_flags & IPATH_NODMA_RTAIL) {
-		__le32 *rhf_addr;
-		u32 seq;
-
-		rhf_addr = (__le32 *) pd->port_rcvhdrq +
-			pd->port_head + dd->ipath_rhf_offset;
-		seq = ipath_hdrget_seq(rhf_addr);
-		hdrqtail = pd->port_head;
-		if (seq == pd->port_seq_cnt)
-			hdrqtail++;
-	} else
-		hdrqtail = ipath_get_rcvhdrtail(pd);
-
-	return hdrqtail;
-}
-
-static inline u64 ipath_read_ireg(const struct ipath_devdata *dd, ipath_kreg r)
-{
-	return (dd->ipath_flags & IPATH_INTREG_64) ?
-		ipath_read_kreg64(dd, r) : ipath_read_kreg32(dd, r);
-}
-
-/*
- * from contents of IBCStatus (or a saved copy), return linkstate
- * Report ACTIVE_DEFER as ACTIVE, because we treat them the same
- * everywhere, anyway (and should be, for almost all purposes).
- */
-static inline u32 ipath_ib_linkstate(struct ipath_devdata *dd, u64 ibcs)
-{
-	u32 state = (u32)(ibcs >> dd->ibcs_ls_shift) &
-		INFINIPATH_IBCS_LINKSTATE_MASK;
-	if (state == INFINIPATH_IBCS_L_STATE_ACT_DEFER)
-		state = INFINIPATH_IBCS_L_STATE_ACTIVE;
-	return state;
-}
-
-/* from contents of IBCStatus (or a saved copy), return linktrainingstate */
-static inline u32 ipath_ib_linktrstate(struct ipath_devdata *dd, u64 ibcs)
-{
-	return (u32)(ibcs >> INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) &
-		dd->ibcs_lts_mask;
-}
-
-/*
- * from contents of IBCStatus (or a saved copy), return logical link state
- * combination of link state and linktraining state (down, active, init,
- * arm, etc.
- */
-static inline u32 ipath_ib_state(struct ipath_devdata *dd, u64 ibcs)
-{
-	u32 ibs;
-	ibs = (u32)(ibcs >> INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) &
-		dd->ibcs_lts_mask;
-	ibs |= (u32)(ibcs &
-		(INFINIPATH_IBCS_LINKSTATE_MASK << dd->ibcs_ls_shift));
-	return ibs;
-}
-
-/*
- * sysfs interface.
- */
-
-struct device_driver;
-
-extern const char ib_ipath_version[];
-
-extern const struct attribute_group *ipath_driver_attr_groups[];
-
-int ipath_device_create_group(struct device *, struct ipath_devdata *);
-void ipath_device_remove_group(struct device *, struct ipath_devdata *);
-int ipath_expose_reset(struct device *);
-
-int ipath_init_ipathfs(void);
-void ipath_exit_ipathfs(void);
-int ipathfs_add_device(struct ipath_devdata *);
-int ipathfs_remove_device(struct ipath_devdata *);
-
-/*
- * dma_addr wrappers - all 0's invalid for hw
- */
-dma_addr_t ipath_map_page(struct pci_dev *, struct page *, unsigned long,
-			  size_t, int);
-dma_addr_t ipath_map_single(struct pci_dev *, void *, size_t, int);
-const char *ipath_get_unit_name(int unit);
-
-/*
- * Flush write combining store buffers (if present) and perform a write
- * barrier.
- */
-#if defined(CONFIG_X86_64)
-#define ipath_flush_wc() asm volatile("sfence" ::: "memory")
-#else
-#define ipath_flush_wc() wmb()
-#endif
-
-extern unsigned ipath_debug; /* debugging bit mask */
-extern unsigned ipath_linkrecovery;
-extern unsigned ipath_mtu4096;
-extern struct mutex ipath_mutex;
-
-#define IPATH_DRV_NAME		"ib_ipath"
-#define IPATH_MAJOR		233
-#define IPATH_USER_MINOR_BASE	0
-#define IPATH_DIAGPKT_MINOR	127
-#define IPATH_DIAG_MINOR_BASE	129
-#define IPATH_NMINORS		255
-
-#define ipath_dev_err(dd,fmt,...) \
-	do { \
-		const struct ipath_devdata *__dd = (dd); \
-		if (__dd->pcidev) \
-			dev_err(&__dd->pcidev->dev, "%s: " fmt, \
-				ipath_get_unit_name(__dd->ipath_unit), \
-				##__VA_ARGS__); \
-		else \
-			printk(KERN_ERR IPATH_DRV_NAME ": %s: " fmt, \
-			       ipath_get_unit_name(__dd->ipath_unit), \
-			       ##__VA_ARGS__); \
-	} while (0)
-
-#if _IPATH_DEBUGGING
-
-# define __IPATH_DBG_WHICH(which,fmt,...) \
-	do { \
-		if (unlikely(ipath_debug & (which))) \
-			printk(KERN_DEBUG IPATH_DRV_NAME ": %s: " fmt, \
-			       __func__,##__VA_ARGS__); \
-	} while(0)
-
-# define ipath_dbg(fmt,...) \
-	__IPATH_DBG_WHICH(__IPATH_DBG,fmt,##__VA_ARGS__)
-# define ipath_cdbg(which,fmt,...) \
-	__IPATH_DBG_WHICH(__IPATH_##which##DBG,fmt,##__VA_ARGS__)
-
-#else /* ! _IPATH_DEBUGGING */
-
-# define ipath_dbg(fmt,...)
-# define ipath_cdbg(which,fmt,...)
-
-#endif /* _IPATH_DEBUGGING */
-
-/*
- * this is used for formatting hw error messages...
- */
-struct ipath_hwerror_msgs {
-	u64 mask;
-	const char *msg;
-};
-
-#define INFINIPATH_HWE_MSG(a, b) { .mask = INFINIPATH_HWE_##a, .msg = b }
-
-/* in ipath_intr.c... */
-void ipath_format_hwerrors(u64 hwerrs,
-			   const struct ipath_hwerror_msgs *hwerrmsgs,
-			   size_t nhwerrmsgs,
-			   char *msg, size_t lmsg);
-
-#endif				/* _IPATH_KERNEL_H */
diff --git a/drivers/staging/rdma/ipath/ipath_keys.c b/drivers/staging/rdma/ipath/ipath_keys.c
deleted file mode 100644
index c0e933fec218..000000000000
--- a/drivers/staging/rdma/ipath/ipath_keys.c
+++ /dev/null
@@ -1,270 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 <asm/io.h>
-
-#include "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-/**
- * ipath_alloc_lkey - allocate an lkey
- * @rkt: lkey table in which to allocate the lkey
- * @mr: memory region that this lkey protects
- *
- * Returns 1 if successful, otherwise returns 0.
- */
-
-int ipath_alloc_lkey(struct ipath_lkey_table *rkt, struct ipath_mregion *mr)
-{
-	unsigned long flags;
-	u32 r;
-	u32 n;
-	int ret;
-
-	spin_lock_irqsave(&rkt->lock, flags);
-
-	/* Find the next available LKEY */
-	r = n = rkt->next;
-	for (;;) {
-		if (rkt->table[r] == NULL)
-			break;
-		r = (r + 1) & (rkt->max - 1);
-		if (r == n) {
-			spin_unlock_irqrestore(&rkt->lock, flags);
-			ipath_dbg("LKEY table full\n");
-			ret = 0;
-			goto bail;
-		}
-	}
-	rkt->next = (r + 1) & (rkt->max - 1);
-	/*
-	 * Make sure lkey is never zero which is reserved to indicate an
-	 * unrestricted LKEY.
-	 */
-	rkt->gen++;
-	mr->lkey = (r << (32 - ib_ipath_lkey_table_size)) |
-		((((1 << (24 - ib_ipath_lkey_table_size)) - 1) & rkt->gen)
-		 << 8);
-	if (mr->lkey == 0) {
-		mr->lkey |= 1 << 8;
-		rkt->gen++;
-	}
-	rkt->table[r] = mr;
-	spin_unlock_irqrestore(&rkt->lock, flags);
-
-	ret = 1;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_free_lkey - free an lkey
- * @rkt: table from which to free the lkey
- * @lkey: lkey id to free
- */
-void ipath_free_lkey(struct ipath_lkey_table *rkt, u32 lkey)
-{
-	unsigned long flags;
-	u32 r;
-
-	if (lkey == 0)
-		return;
-	r = lkey >> (32 - ib_ipath_lkey_table_size);
-	spin_lock_irqsave(&rkt->lock, flags);
-	rkt->table[r] = NULL;
-	spin_unlock_irqrestore(&rkt->lock, flags);
-}
-
-/**
- * ipath_lkey_ok - check IB SGE for validity and initialize
- * @rkt: table containing lkey to check SGE against
- * @isge: outgoing internal SGE
- * @sge: SGE to check
- * @acc: access flags
- *
- * Return 1 if valid and successful, otherwise returns 0.
- *
- * Check the IB SGE for validity and initialize our internal version
- * of it.
- */
-int ipath_lkey_ok(struct ipath_qp *qp, struct ipath_sge *isge,
-		  struct ib_sge *sge, int acc)
-{
-	struct ipath_lkey_table *rkt = &to_idev(qp->ibqp.device)->lk_table;
-	struct ipath_mregion *mr;
-	unsigned n, m;
-	size_t off;
-	int ret;
-
-	/*
-	 * We use LKEY == zero for kernel virtual addresses
-	 * (see ipath_get_dma_mr and ipath_dma.c).
-	 */
-	if (sge->lkey == 0) {
-		/* always a kernel port, no locking needed */
-		struct ipath_pd *pd = to_ipd(qp->ibqp.pd);
-
-		if (pd->user) {
-			ret = 0;
-			goto bail;
-		}
-		isge->mr = NULL;
-		isge->vaddr = (void *) sge->addr;
-		isge->length = sge->length;
-		isge->sge_length = sge->length;
-		ret = 1;
-		goto bail;
-	}
-	mr = rkt->table[(sge->lkey >> (32 - ib_ipath_lkey_table_size))];
-	if (unlikely(mr == NULL || mr->lkey != sge->lkey ||
-		     qp->ibqp.pd != mr->pd)) {
-		ret = 0;
-		goto bail;
-	}
-
-	off = sge->addr - mr->user_base;
-	if (unlikely(sge->addr < mr->user_base ||
-		     off + sge->length > mr->length ||
-		     (mr->access_flags & acc) != acc)) {
-		ret = 0;
-		goto bail;
-	}
-
-	off += mr->offset;
-	m = 0;
-	n = 0;
-	while (off >= mr->map[m]->segs[n].length) {
-		off -= mr->map[m]->segs[n].length;
-		n++;
-		if (n >= IPATH_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-	isge->mr = mr;
-	isge->vaddr = mr->map[m]->segs[n].vaddr + off;
-	isge->length = mr->map[m]->segs[n].length - off;
-	isge->sge_length = sge->length;
-	isge->m = m;
-	isge->n = n;
-
-	ret = 1;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_rkey_ok - check the IB virtual address, length, and RKEY
- * @dev: infiniband device
- * @ss: SGE state
- * @len: length of data
- * @vaddr: virtual address to place data
- * @rkey: rkey to check
- * @acc: access flags
- *
- * Return 1 if successful, otherwise 0.
- */
-int ipath_rkey_ok(struct ipath_qp *qp, struct ipath_sge_state *ss,
-		  u32 len, u64 vaddr, u32 rkey, int acc)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ipath_lkey_table *rkt = &dev->lk_table;
-	struct ipath_sge *sge = &ss->sge;
-	struct ipath_mregion *mr;
-	unsigned n, m;
-	size_t off;
-	int ret;
-
-	/*
-	 * We use RKEY == zero for kernel virtual addresses
-	 * (see ipath_get_dma_mr and ipath_dma.c).
-	 */
-	if (rkey == 0) {
-		/* always a kernel port, no locking needed */
-		struct ipath_pd *pd = to_ipd(qp->ibqp.pd);
-
-		if (pd->user) {
-			ret = 0;
-			goto bail;
-		}
-		sge->mr = NULL;
-		sge->vaddr = (void *) vaddr;
-		sge->length = len;
-		sge->sge_length = len;
-		ss->sg_list = NULL;
-		ss->num_sge = 1;
-		ret = 1;
-		goto bail;
-	}
-
-	mr = rkt->table[(rkey >> (32 - ib_ipath_lkey_table_size))];
-	if (unlikely(mr == NULL || mr->lkey != rkey ||
-		     qp->ibqp.pd != mr->pd)) {
-		ret = 0;
-		goto bail;
-	}
-
-	off = vaddr - mr->iova;
-	if (unlikely(vaddr < mr->iova || off + len > mr->length ||
-		     (mr->access_flags & acc) == 0)) {
-		ret = 0;
-		goto bail;
-	}
-
-	off += mr->offset;
-	m = 0;
-	n = 0;
-	while (off >= mr->map[m]->segs[n].length) {
-		off -= mr->map[m]->segs[n].length;
-		n++;
-		if (n >= IPATH_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-	sge->mr = mr;
-	sge->vaddr = mr->map[m]->segs[n].vaddr + off;
-	sge->length = mr->map[m]->segs[n].length - off;
-	sge->sge_length = len;
-	sge->m = m;
-	sge->n = n;
-	ss->sg_list = NULL;
-	ss->num_sge = 1;
-
-	ret = 1;
-
-bail:
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_mad.c b/drivers/staging/rdma/ipath/ipath_mad.c
deleted file mode 100644
index ad3a926ab3c5..000000000000
--- a/drivers/staging/rdma/ipath/ipath_mad.c
+++ /dev/null
@@ -1,1521 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 <rdma/ib_smi.h>
-#include <rdma/ib_pma.h>
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-#include "ipath_common.h"
-
-#define IB_SMP_UNSUP_VERSION	cpu_to_be16(0x0004)
-#define IB_SMP_UNSUP_METHOD	cpu_to_be16(0x0008)
-#define IB_SMP_UNSUP_METH_ATTR	cpu_to_be16(0x000C)
-#define IB_SMP_INVALID_FIELD	cpu_to_be16(0x001C)
-
-static int reply(struct ib_smp *smp)
-{
-	/*
-	 * The verbs framework will handle the directed/LID route
-	 * packet changes.
-	 */
-	smp->method = IB_MGMT_METHOD_GET_RESP;
-	if (smp->mgmt_class == IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE)
-		smp->status |= IB_SMP_DIRECTION;
-	return IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_REPLY;
-}
-
-static int recv_subn_get_nodedescription(struct ib_smp *smp,
-					 struct ib_device *ibdev)
-{
-	if (smp->attr_mod)
-		smp->status |= IB_SMP_INVALID_FIELD;
-
-	memcpy(smp->data, ibdev->node_desc, sizeof(smp->data));
-
-	return reply(smp);
-}
-
-struct nodeinfo {
-	u8 base_version;
-	u8 class_version;
-	u8 node_type;
-	u8 num_ports;
-	__be64 sys_guid;
-	__be64 node_guid;
-	__be64 port_guid;
-	__be16 partition_cap;
-	__be16 device_id;
-	__be32 revision;
-	u8 local_port_num;
-	u8 vendor_id[3];
-} __attribute__ ((packed));
-
-static int recv_subn_get_nodeinfo(struct ib_smp *smp,
-				  struct ib_device *ibdev, u8 port)
-{
-	struct nodeinfo *nip = (struct nodeinfo *)&smp->data;
-	struct ipath_devdata *dd = to_idev(ibdev)->dd;
-	u32 vendor, majrev, minrev;
-
-	/* GUID 0 is illegal */
-	if (smp->attr_mod || (dd->ipath_guid == 0))
-		smp->status |= IB_SMP_INVALID_FIELD;
-
-	nip->base_version = 1;
-	nip->class_version = 1;
-	nip->node_type = 1;	/* channel adapter */
-	/*
-	 * XXX The num_ports value will need a layer function to get
-	 * the value if we ever have more than one IB port on a chip.
-	 * We will also need to get the GUID for the port.
-	 */
-	nip->num_ports = ibdev->phys_port_cnt;
-	/* This is already in network order */
-	nip->sys_guid = to_idev(ibdev)->sys_image_guid;
-	nip->node_guid = dd->ipath_guid;
-	nip->port_guid = dd->ipath_guid;
-	nip->partition_cap = cpu_to_be16(ipath_get_npkeys(dd));
-	nip->device_id = cpu_to_be16(dd->ipath_deviceid);
-	majrev = dd->ipath_majrev;
-	minrev = dd->ipath_minrev;
-	nip->revision = cpu_to_be32((majrev << 16) | minrev);
-	nip->local_port_num = port;
-	vendor = dd->ipath_vendorid;
-	nip->vendor_id[0] = IPATH_SRC_OUI_1;
-	nip->vendor_id[1] = IPATH_SRC_OUI_2;
-	nip->vendor_id[2] = IPATH_SRC_OUI_3;
-
-	return reply(smp);
-}
-
-static int recv_subn_get_guidinfo(struct ib_smp *smp,
-				  struct ib_device *ibdev)
-{
-	u32 startgx = 8 * be32_to_cpu(smp->attr_mod);
-	__be64 *p = (__be64 *) smp->data;
-
-	/* 32 blocks of 8 64-bit GUIDs per block */
-
-	memset(smp->data, 0, sizeof(smp->data));
-
-	/*
-	 * We only support one GUID for now.  If this changes, the
-	 * portinfo.guid_cap field needs to be updated too.
-	 */
-	if (startgx == 0) {
-		__be64 g = to_idev(ibdev)->dd->ipath_guid;
-		if (g == 0)
-			/* GUID 0 is illegal */
-			smp->status |= IB_SMP_INVALID_FIELD;
-		else
-			/* The first is a copy of the read-only HW GUID. */
-			*p = g;
-	} else
-		smp->status |= IB_SMP_INVALID_FIELD;
-
-	return reply(smp);
-}
-
-static void set_link_width_enabled(struct ipath_devdata *dd, u32 w)
-{
-	(void) dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_LWID_ENB, w);
-}
-
-static void set_link_speed_enabled(struct ipath_devdata *dd, u32 s)
-{
-	(void) dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_SPD_ENB, s);
-}
-
-static int get_overrunthreshold(struct ipath_devdata *dd)
-{
-	return (dd->ipath_ibcctrl >>
-		INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT) &
-		INFINIPATH_IBCC_OVERRUNTHRESHOLD_MASK;
-}
-
-/**
- * set_overrunthreshold - set the overrun threshold
- * @dd: the infinipath device
- * @n: the new threshold
- *
- * Note that this will only take effect when the link state changes.
- */
-static int set_overrunthreshold(struct ipath_devdata *dd, unsigned n)
-{
-	unsigned v;
-
-	v = (dd->ipath_ibcctrl >> INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT) &
-		INFINIPATH_IBCC_OVERRUNTHRESHOLD_MASK;
-	if (v != n) {
-		dd->ipath_ibcctrl &=
-			~(INFINIPATH_IBCC_OVERRUNTHRESHOLD_MASK <<
-			  INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT);
-		dd->ipath_ibcctrl |=
-			(u64) n << INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-				 dd->ipath_ibcctrl);
-	}
-	return 0;
-}
-
-static int get_phyerrthreshold(struct ipath_devdata *dd)
-{
-	return (dd->ipath_ibcctrl >>
-		INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT) &
-		INFINIPATH_IBCC_PHYERRTHRESHOLD_MASK;
-}
-
-/**
- * set_phyerrthreshold - set the physical error threshold
- * @dd: the infinipath device
- * @n: the new threshold
- *
- * Note that this will only take effect when the link state changes.
- */
-static int set_phyerrthreshold(struct ipath_devdata *dd, unsigned n)
-{
-	unsigned v;
-
-	v = (dd->ipath_ibcctrl >> INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT) &
-		INFINIPATH_IBCC_PHYERRTHRESHOLD_MASK;
-	if (v != n) {
-		dd->ipath_ibcctrl &=
-			~(INFINIPATH_IBCC_PHYERRTHRESHOLD_MASK <<
-			  INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT);
-		dd->ipath_ibcctrl |=
-			(u64) n << INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-				 dd->ipath_ibcctrl);
-	}
-	return 0;
-}
-
-/**
- * get_linkdowndefaultstate - get the default linkdown state
- * @dd: the infinipath device
- *
- * Returns zero if the default is POLL, 1 if the default is SLEEP.
- */
-static int get_linkdowndefaultstate(struct ipath_devdata *dd)
-{
-	return !!(dd->ipath_ibcctrl & INFINIPATH_IBCC_LINKDOWNDEFAULTSTATE);
-}
-
-static int recv_subn_get_portinfo(struct ib_smp *smp,
-				  struct ib_device *ibdev, u8 port)
-{
-	struct ipath_ibdev *dev;
-	struct ipath_devdata *dd;
-	struct ib_port_info *pip = (struct ib_port_info *)smp->data;
-	u16 lid;
-	u8 ibcstat;
-	u8 mtu;
-	int ret;
-
-	if (be32_to_cpu(smp->attr_mod) > ibdev->phys_port_cnt) {
-		smp->status |= IB_SMP_INVALID_FIELD;
-		ret = reply(smp);
-		goto bail;
-	}
-
-	dev = to_idev(ibdev);
-	dd = dev->dd;
-
-	/* Clear all fields.  Only set the non-zero fields. */
-	memset(smp->data, 0, sizeof(smp->data));
-
-	/* Only return the mkey if the protection field allows it. */
-	if (smp->method == IB_MGMT_METHOD_SET || dev->mkey == smp->mkey ||
-	    dev->mkeyprot == 0)
-		pip->mkey = dev->mkey;
-	pip->gid_prefix = dev->gid_prefix;
-	lid = dd->ipath_lid;
-	pip->lid = lid ? cpu_to_be16(lid) : IB_LID_PERMISSIVE;
-	pip->sm_lid = cpu_to_be16(dev->sm_lid);
-	pip->cap_mask = cpu_to_be32(dev->port_cap_flags);
-	/* pip->diag_code; */
-	pip->mkey_lease_period = cpu_to_be16(dev->mkey_lease_period);
-	pip->local_port_num = port;
-	pip->link_width_enabled = dd->ipath_link_width_enabled;
-	pip->link_width_supported = dd->ipath_link_width_supported;
-	pip->link_width_active = dd->ipath_link_width_active;
-	pip->linkspeed_portstate = dd->ipath_link_speed_supported << 4;
-	ibcstat = dd->ipath_lastibcstat;
-	/* map LinkState to IB portinfo values.  */
-	pip->linkspeed_portstate |= ipath_ib_linkstate(dd, ibcstat) + 1;
-
-	pip->portphysstate_linkdown =
-		(ipath_cvt_physportstate[ibcstat & dd->ibcs_lts_mask] << 4) |
-		(get_linkdowndefaultstate(dd) ? 1 : 2);
-	pip->mkeyprot_resv_lmc = (dev->mkeyprot << 6) | dd->ipath_lmc;
-	pip->linkspeedactive_enabled = (dd->ipath_link_speed_active << 4) |
-		dd->ipath_link_speed_enabled;
-	switch (dd->ipath_ibmtu) {
-	case 4096:
-		mtu = IB_MTU_4096;
-		break;
-	case 2048:
-		mtu = IB_MTU_2048;
-		break;
-	case 1024:
-		mtu = IB_MTU_1024;
-		break;
-	case 512:
-		mtu = IB_MTU_512;
-		break;
-	case 256:
-		mtu = IB_MTU_256;
-		break;
-	default:		/* oops, something is wrong */
-		mtu = IB_MTU_2048;
-		break;
-	}
-	pip->neighbormtu_mastersmsl = (mtu << 4) | dev->sm_sl;
-	pip->vlcap_inittype = 0x10;	/* VLCap = VL0, InitType = 0 */
-	pip->vl_high_limit = dev->vl_high_limit;
-	/* pip->vl_arb_high_cap; // only one VL */
-	/* pip->vl_arb_low_cap; // only one VL */
-	/* InitTypeReply = 0 */
-	/* our mtu cap depends on whether 4K MTU enabled or not */
-	pip->inittypereply_mtucap = ipath_mtu4096 ? IB_MTU_4096 : IB_MTU_2048;
-	/* HCAs ignore VLStallCount and HOQLife */
-	/* pip->vlstallcnt_hoqlife; */
-	pip->operationalvl_pei_peo_fpi_fpo = 0x10;	/* OVLs = 1 */
-	pip->mkey_violations = cpu_to_be16(dev->mkey_violations);
-	/* P_KeyViolations are counted by hardware. */
-	pip->pkey_violations =
-		cpu_to_be16((ipath_get_cr_errpkey(dd) -
-			     dev->z_pkey_violations) & 0xFFFF);
-	pip->qkey_violations = cpu_to_be16(dev->qkey_violations);
-	/* Only the hardware GUID is supported for now */
-	pip->guid_cap = 1;
-	pip->clientrereg_resv_subnetto = dev->subnet_timeout;
-	/* 32.768 usec. response time (guessing) */
-	pip->resv_resptimevalue = 3;
-	pip->localphyerrors_overrunerrors =
-		(get_phyerrthreshold(dd) << 4) |
-		get_overrunthreshold(dd);
-	/* pip->max_credit_hint; */
-	if (dev->port_cap_flags & IB_PORT_LINK_LATENCY_SUP) {
-		u32 v;
-
-		v = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_LINKLATENCY);
-		pip->link_roundtrip_latency[0] = v >> 16;
-		pip->link_roundtrip_latency[1] = v >> 8;
-		pip->link_roundtrip_latency[2] = v;
-	}
-
-	ret = reply(smp);
-
-bail:
-	return ret;
-}
-
-/**
- * get_pkeys - return the PKEY table for port 0
- * @dd: the infinipath device
- * @pkeys: the pkey table is placed here
- */
-static int get_pkeys(struct ipath_devdata *dd, u16 * pkeys)
-{
-	/* always a kernel port, no locking needed */
-	struct ipath_portdata *pd = dd->ipath_pd[0];
-
-	memcpy(pkeys, pd->port_pkeys, sizeof(pd->port_pkeys));
-
-	return 0;
-}
-
-static int recv_subn_get_pkeytable(struct ib_smp *smp,
-				   struct ib_device *ibdev)
-{
-	u32 startpx = 32 * (be32_to_cpu(smp->attr_mod) & 0xffff);
-	u16 *p = (u16 *) smp->data;
-	__be16 *q = (__be16 *) smp->data;
-
-	/* 64 blocks of 32 16-bit P_Key entries */
-
-	memset(smp->data, 0, sizeof(smp->data));
-	if (startpx == 0) {
-		struct ipath_ibdev *dev = to_idev(ibdev);
-		unsigned i, n = ipath_get_npkeys(dev->dd);
-
-		get_pkeys(dev->dd, p);
-
-		for (i = 0; i < n; i++)
-			q[i] = cpu_to_be16(p[i]);
-	} else
-		smp->status |= IB_SMP_INVALID_FIELD;
-
-	return reply(smp);
-}
-
-static int recv_subn_set_guidinfo(struct ib_smp *smp,
-				  struct ib_device *ibdev)
-{
-	/* The only GUID we support is the first read-only entry. */
-	return recv_subn_get_guidinfo(smp, ibdev);
-}
-
-/**
- * set_linkdowndefaultstate - set the default linkdown state
- * @dd: the infinipath device
- * @sleep: the new state
- *
- * Note that this will only take effect when the link state changes.
- */
-static int set_linkdowndefaultstate(struct ipath_devdata *dd, int sleep)
-{
-	if (sleep)
-		dd->ipath_ibcctrl |= INFINIPATH_IBCC_LINKDOWNDEFAULTSTATE;
-	else
-		dd->ipath_ibcctrl &= ~INFINIPATH_IBCC_LINKDOWNDEFAULTSTATE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_ibcctrl,
-			 dd->ipath_ibcctrl);
-	return 0;
-}
-
-/**
- * recv_subn_set_portinfo - set port information
- * @smp: the incoming SM packet
- * @ibdev: the infiniband device
- * @port: the port on the device
- *
- * Set Portinfo (see ch. 14.2.5.6).
- */
-static int recv_subn_set_portinfo(struct ib_smp *smp,
-				  struct ib_device *ibdev, u8 port)
-{
-	struct ib_port_info *pip = (struct ib_port_info *)smp->data;
-	struct ib_event event;
-	struct ipath_ibdev *dev;
-	struct ipath_devdata *dd;
-	char clientrereg = 0;
-	u16 lid, smlid;
-	u8 lwe;
-	u8 lse;
-	u8 state;
-	u16 lstate;
-	u32 mtu;
-	int ret, ore;
-
-	if (be32_to_cpu(smp->attr_mod) > ibdev->phys_port_cnt)
-		goto err;
-
-	dev = to_idev(ibdev);
-	dd = dev->dd;
-	event.device = ibdev;
-	event.element.port_num = port;
-
-	dev->mkey = pip->mkey;
-	dev->gid_prefix = pip->gid_prefix;
-	dev->mkey_lease_period = be16_to_cpu(pip->mkey_lease_period);
-
-	lid = be16_to_cpu(pip->lid);
-	if (dd->ipath_lid != lid ||
-	    dd->ipath_lmc != (pip->mkeyprot_resv_lmc & 7)) {
-		/* Must be a valid unicast LID address. */
-		if (lid == 0 || lid >= IPATH_MULTICAST_LID_BASE)
-			goto err;
-		ipath_set_lid(dd, lid, pip->mkeyprot_resv_lmc & 7);
-		event.event = IB_EVENT_LID_CHANGE;
-		ib_dispatch_event(&event);
-	}
-
-	smlid = be16_to_cpu(pip->sm_lid);
-	if (smlid != dev->sm_lid) {
-		/* Must be a valid unicast LID address. */
-		if (smlid == 0 || smlid >= IPATH_MULTICAST_LID_BASE)
-			goto err;
-		dev->sm_lid = smlid;
-		event.event = IB_EVENT_SM_CHANGE;
-		ib_dispatch_event(&event);
-	}
-
-	/* Allow 1x or 4x to be set (see 14.2.6.6). */
-	lwe = pip->link_width_enabled;
-	if (lwe) {
-		if (lwe == 0xFF)
-			lwe = dd->ipath_link_width_supported;
-		else if (lwe >= 16 || (lwe & ~dd->ipath_link_width_supported))
-			goto err;
-		set_link_width_enabled(dd, lwe);
-	}
-
-	/* Allow 2.5 or 5.0 Gbs. */
-	lse = pip->linkspeedactive_enabled & 0xF;
-	if (lse) {
-		if (lse == 15)
-			lse = dd->ipath_link_speed_supported;
-		else if (lse >= 8 || (lse & ~dd->ipath_link_speed_supported))
-			goto err;
-		set_link_speed_enabled(dd, lse);
-	}
-
-	/* Set link down default state. */
-	switch (pip->portphysstate_linkdown & 0xF) {
-	case 0: /* NOP */
-		break;
-	case 1: /* SLEEP */
-		if (set_linkdowndefaultstate(dd, 1))
-			goto err;
-		break;
-	case 2: /* POLL */
-		if (set_linkdowndefaultstate(dd, 0))
-			goto err;
-		break;
-	default:
-		goto err;
-	}
-
-	dev->mkeyprot = pip->mkeyprot_resv_lmc >> 6;
-	dev->vl_high_limit = pip->vl_high_limit;
-
-	switch ((pip->neighbormtu_mastersmsl >> 4) & 0xF) {
-	case IB_MTU_256:
-		mtu = 256;
-		break;
-	case IB_MTU_512:
-		mtu = 512;
-		break;
-	case IB_MTU_1024:
-		mtu = 1024;
-		break;
-	case IB_MTU_2048:
-		mtu = 2048;
-		break;
-	case IB_MTU_4096:
-		if (!ipath_mtu4096)
-			goto err;
-		mtu = 4096;
-		break;
-	default:
-		/* XXX We have already partially updated our state! */
-		goto err;
-	}
-	ipath_set_mtu(dd, mtu);
-
-	dev->sm_sl = pip->neighbormtu_mastersmsl & 0xF;
-
-	/* We only support VL0 */
-	if (((pip->operationalvl_pei_peo_fpi_fpo >> 4) & 0xF) > 1)
-		goto err;
-
-	if (pip->mkey_violations == 0)
-		dev->mkey_violations = 0;
-
-	/*
-	 * Hardware counter can't be reset so snapshot and subtract
-	 * later.
-	 */
-	if (pip->pkey_violations == 0)
-		dev->z_pkey_violations = ipath_get_cr_errpkey(dd);
-
-	if (pip->qkey_violations == 0)
-		dev->qkey_violations = 0;
-
-	ore = pip->localphyerrors_overrunerrors;
-	if (set_phyerrthreshold(dd, (ore >> 4) & 0xF))
-		goto err;
-
-	if (set_overrunthreshold(dd, (ore & 0xF)))
-		goto err;
-
-	dev->subnet_timeout = pip->clientrereg_resv_subnetto & 0x1F;
-
-	if (pip->clientrereg_resv_subnetto & 0x80) {
-		clientrereg = 1;
-		event.event = IB_EVENT_CLIENT_REREGISTER;
-		ib_dispatch_event(&event);
-	}
-
-	/*
-	 * Do the port state change now that the other link parameters
-	 * have been set.
-	 * Changing the port physical state only makes sense if the link
-	 * is down or is being set to down.
-	 */
-	state = pip->linkspeed_portstate & 0xF;
-	lstate = (pip->portphysstate_linkdown >> 4) & 0xF;
-	if (lstate && !(state == IB_PORT_DOWN || state == IB_PORT_NOP))
-		goto err;
-
-	/*
-	 * Only state changes of DOWN, ARM, and ACTIVE are valid
-	 * and must be in the correct state to take effect (see 7.2.6).
-	 */
-	switch (state) {
-	case IB_PORT_NOP:
-		if (lstate == 0)
-			break;
-		/* FALLTHROUGH */
-	case IB_PORT_DOWN:
-		if (lstate == 0)
-			lstate = IPATH_IB_LINKDOWN_ONLY;
-		else if (lstate == 1)
-			lstate = IPATH_IB_LINKDOWN_SLEEP;
-		else if (lstate == 2)
-			lstate = IPATH_IB_LINKDOWN;
-		else if (lstate == 3)
-			lstate = IPATH_IB_LINKDOWN_DISABLE;
-		else
-			goto err;
-		ipath_set_linkstate(dd, lstate);
-		if (lstate == IPATH_IB_LINKDOWN_DISABLE) {
-			ret = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED;
-			goto done;
-		}
-		ipath_wait_linkstate(dd, IPATH_LINKINIT | IPATH_LINKARMED |
-				IPATH_LINKACTIVE, 1000);
-		break;
-	case IB_PORT_ARMED:
-		ipath_set_linkstate(dd, IPATH_IB_LINKARM);
-		break;
-	case IB_PORT_ACTIVE:
-		ipath_set_linkstate(dd, IPATH_IB_LINKACTIVE);
-		break;
-	default:
-		/* XXX We have already partially updated our state! */
-		goto err;
-	}
-
-	ret = recv_subn_get_portinfo(smp, ibdev, port);
-
-	if (clientrereg)
-		pip->clientrereg_resv_subnetto |= 0x80;
-
-	goto done;
-
-err:
-	smp->status |= IB_SMP_INVALID_FIELD;
-	ret = recv_subn_get_portinfo(smp, ibdev, port);
-
-done:
-	return ret;
-}
-
-/**
- * rm_pkey - decrecment the reference count for the given PKEY
- * @dd: the infinipath device
- * @key: the PKEY index
- *
- * Return true if this was the last reference and the hardware table entry
- * needs to be changed.
- */
-static int rm_pkey(struct ipath_devdata *dd, u16 key)
-{
-	int i;
-	int ret;
-
-	for (i = 0; i < ARRAY_SIZE(dd->ipath_pkeys); i++) {
-		if (dd->ipath_pkeys[i] != key)
-			continue;
-		if (atomic_dec_and_test(&dd->ipath_pkeyrefs[i])) {
-			dd->ipath_pkeys[i] = 0;
-			ret = 1;
-			goto bail;
-		}
-		break;
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * add_pkey - add the given PKEY to the hardware table
- * @dd: the infinipath device
- * @key: the PKEY
- *
- * Return an error code if unable to add the entry, zero if no change,
- * or 1 if the hardware PKEY register needs to be updated.
- */
-static int add_pkey(struct ipath_devdata *dd, u16 key)
-{
-	int i;
-	u16 lkey = key & 0x7FFF;
-	int any = 0;
-	int ret;
-
-	if (lkey == 0x7FFF) {
-		ret = 0;
-		goto bail;
-	}
-
-	/* Look for an empty slot or a matching PKEY. */
-	for (i = 0; i < ARRAY_SIZE(dd->ipath_pkeys); i++) {
-		if (!dd->ipath_pkeys[i]) {
-			any++;
-			continue;
-		}
-		/* If it matches exactly, try to increment the ref count */
-		if (dd->ipath_pkeys[i] == key) {
-			if (atomic_inc_return(&dd->ipath_pkeyrefs[i]) > 1) {
-				ret = 0;
-				goto bail;
-			}
-			/* Lost the race. Look for an empty slot below. */
-			atomic_dec(&dd->ipath_pkeyrefs[i]);
-			any++;
-		}
-		/*
-		 * It makes no sense to have both the limited and unlimited
-		 * PKEY set at the same time since the unlimited one will
-		 * disable the limited one.
-		 */
-		if ((dd->ipath_pkeys[i] & 0x7FFF) == lkey) {
-			ret = -EEXIST;
-			goto bail;
-		}
-	}
-	if (!any) {
-		ret = -EBUSY;
-		goto bail;
-	}
-	for (i = 0; i < ARRAY_SIZE(dd->ipath_pkeys); i++) {
-		if (!dd->ipath_pkeys[i] &&
-		    atomic_inc_return(&dd->ipath_pkeyrefs[i]) == 1) {
-			/* for ipathstats, etc. */
-			ipath_stats.sps_pkeys[i] = lkey;
-			dd->ipath_pkeys[i] = key;
-			ret = 1;
-			goto bail;
-		}
-	}
-	ret = -EBUSY;
-
-bail:
-	return ret;
-}
-
-/**
- * set_pkeys - set the PKEY table for port 0
- * @dd: the infinipath device
- * @pkeys: the PKEY table
- */
-static int set_pkeys(struct ipath_devdata *dd, u16 *pkeys, u8 port)
-{
-	struct ipath_portdata *pd;
-	int i;
-	int changed = 0;
-
-	/* always a kernel port, no locking needed */
-	pd = dd->ipath_pd[0];
-
-	for (i = 0; i < ARRAY_SIZE(pd->port_pkeys); i++) {
-		u16 key = pkeys[i];
-		u16 okey = pd->port_pkeys[i];
-
-		if (key == okey)
-			continue;
-		/*
-		 * The value of this PKEY table entry is changing.
-		 * Remove the old entry in the hardware's array of PKEYs.
-		 */
-		if (okey & 0x7FFF)
-			changed |= rm_pkey(dd, okey);
-		if (key & 0x7FFF) {
-			int ret = add_pkey(dd, key);
-
-			if (ret < 0)
-				key = 0;
-			else
-				changed |= ret;
-		}
-		pd->port_pkeys[i] = key;
-	}
-	if (changed) {
-		u64 pkey;
-		struct ib_event event;
-
-		pkey = (u64) dd->ipath_pkeys[0] |
-			((u64) dd->ipath_pkeys[1] << 16) |
-			((u64) dd->ipath_pkeys[2] << 32) |
-			((u64) dd->ipath_pkeys[3] << 48);
-		ipath_cdbg(VERBOSE, "p0 new pkey reg %llx\n",
-			   (unsigned long long) pkey);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_partitionkey,
-				 pkey);
-
-		event.event = IB_EVENT_PKEY_CHANGE;
-		event.device = &dd->verbs_dev->ibdev;
-		event.element.port_num = port;
-		ib_dispatch_event(&event);
-	}
-	return 0;
-}
-
-static int recv_subn_set_pkeytable(struct ib_smp *smp,
-				   struct ib_device *ibdev, u8 port)
-{
-	u32 startpx = 32 * (be32_to_cpu(smp->attr_mod) & 0xffff);
-	__be16 *p = (__be16 *) smp->data;
-	u16 *q = (u16 *) smp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	unsigned i, n = ipath_get_npkeys(dev->dd);
-
-	for (i = 0; i < n; i++)
-		q[i] = be16_to_cpu(p[i]);
-
-	if (startpx != 0 || set_pkeys(dev->dd, q, port) != 0)
-		smp->status |= IB_SMP_INVALID_FIELD;
-
-	return recv_subn_get_pkeytable(smp, ibdev);
-}
-
-static int recv_pma_get_classportinfo(struct ib_pma_mad *pmp)
-{
-	struct ib_class_port_info *p =
-		(struct ib_class_port_info *)pmp->data;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-
-	if (pmp->mad_hdr.attr_mod != 0)
-		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
-
-	/* Indicate AllPortSelect is valid (only one port anyway) */
-	p->capability_mask = cpu_to_be16(1 << 8);
-	p->base_version = 1;
-	p->class_version = 1;
-	/*
-	 * Expected response time is 4.096 usec. * 2^18 == 1.073741824
-	 * sec.
-	 */
-	p->resp_time_value = 18;
-
-	return reply((struct ib_smp *) pmp);
-}
-
-/*
- * The PortSamplesControl.CounterMasks field is an array of 3 bit fields
- * which specify the N'th counter's capabilities. See ch. 16.1.3.2.
- * We support 5 counters which only count the mandatory quantities.
- */
-#define COUNTER_MASK(q, n) (q << ((9 - n) * 3))
-#define COUNTER_MASK0_9 cpu_to_be32(COUNTER_MASK(1, 0) | \
-				    COUNTER_MASK(1, 1) | \
-				    COUNTER_MASK(1, 2) | \
-				    COUNTER_MASK(1, 3) | \
-				    COUNTER_MASK(1, 4))
-
-static int recv_pma_get_portsamplescontrol(struct ib_pma_mad *pmp,
-					   struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portsamplescontrol *p =
-		(struct ib_pma_portsamplescontrol *)pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_cregs const *crp = dev->dd->ipath_cregs;
-	unsigned long flags;
-	u8 port_select = p->port_select;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-
-	p->port_select = port_select;
-	if (pmp->mad_hdr.attr_mod != 0 ||
-	    (port_select != port && port_select != 0xFF))
-		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
-	/*
-	 * Ticks are 10x the link transfer period which for 2.5Gbs is 4
-	 * nsec.  0 == 4 nsec., 1 == 8 nsec., ..., 255 == 1020 nsec.  Sample
-	 * intervals are counted in ticks.  Since we use Linux timers, that
-	 * count in jiffies, we can't sample for less than 1000 ticks if HZ
-	 * == 1000 (4000 ticks if HZ is 250).  link_speed_active returns 2 for
-	 * DDR, 1 for SDR, set the tick to 1 for DDR, 0 for SDR on chips that
-	 * have hardware support for delaying packets.
-	 */
-	if (crp->cr_psstat)
-		p->tick = dev->dd->ipath_link_speed_active - 1;
-	else
-		p->tick = 250;		/* 1 usec. */
-	p->counter_width = 4;	/* 32 bit counters */
-	p->counter_mask0_9 = COUNTER_MASK0_9;
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	if (crp->cr_psstat)
-		p->sample_status = ipath_read_creg32(dev->dd, crp->cr_psstat);
-	else
-		p->sample_status = dev->pma_sample_status;
-	p->sample_start = cpu_to_be32(dev->pma_sample_start);
-	p->sample_interval = cpu_to_be32(dev->pma_sample_interval);
-	p->tag = cpu_to_be16(dev->pma_tag);
-	p->counter_select[0] = dev->pma_counter_select[0];
-	p->counter_select[1] = dev->pma_counter_select[1];
-	p->counter_select[2] = dev->pma_counter_select[2];
-	p->counter_select[3] = dev->pma_counter_select[3];
-	p->counter_select[4] = dev->pma_counter_select[4];
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-
-	return reply((struct ib_smp *) pmp);
-}
-
-static int recv_pma_set_portsamplescontrol(struct ib_pma_mad *pmp,
-					   struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portsamplescontrol *p =
-		(struct ib_pma_portsamplescontrol *)pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_cregs const *crp = dev->dd->ipath_cregs;
-	unsigned long flags;
-	u8 status;
-	int ret;
-
-	if (pmp->mad_hdr.attr_mod != 0 ||
-	    (p->port_select != port && p->port_select != 0xFF)) {
-		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
-		ret = reply((struct ib_smp *) pmp);
-		goto bail;
-	}
-
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	if (crp->cr_psstat)
-		status = ipath_read_creg32(dev->dd, crp->cr_psstat);
-	else
-		status = dev->pma_sample_status;
-	if (status == IB_PMA_SAMPLE_STATUS_DONE) {
-		dev->pma_sample_start = be32_to_cpu(p->sample_start);
-		dev->pma_sample_interval = be32_to_cpu(p->sample_interval);
-		dev->pma_tag = be16_to_cpu(p->tag);
-		dev->pma_counter_select[0] = p->counter_select[0];
-		dev->pma_counter_select[1] = p->counter_select[1];
-		dev->pma_counter_select[2] = p->counter_select[2];
-		dev->pma_counter_select[3] = p->counter_select[3];
-		dev->pma_counter_select[4] = p->counter_select[4];
-		if (crp->cr_psstat) {
-			ipath_write_creg(dev->dd, crp->cr_psinterval,
-					 dev->pma_sample_interval);
-			ipath_write_creg(dev->dd, crp->cr_psstart,
-					 dev->pma_sample_start);
-		} else
-			dev->pma_sample_status = IB_PMA_SAMPLE_STATUS_STARTED;
-	}
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-
-	ret = recv_pma_get_portsamplescontrol(pmp, ibdev, port);
-
-bail:
-	return ret;
-}
-
-static u64 get_counter(struct ipath_ibdev *dev,
-		       struct ipath_cregs const *crp,
-		       __be16 sel)
-{
-	u64 ret;
-
-	switch (sel) {
-	case IB_PMA_PORT_XMIT_DATA:
-		ret = (crp->cr_psxmitdatacount) ?
-			ipath_read_creg32(dev->dd, crp->cr_psxmitdatacount) :
-			dev->ipath_sword;
-		break;
-	case IB_PMA_PORT_RCV_DATA:
-		ret = (crp->cr_psrcvdatacount) ?
-			ipath_read_creg32(dev->dd, crp->cr_psrcvdatacount) :
-			dev->ipath_rword;
-		break;
-	case IB_PMA_PORT_XMIT_PKTS:
-		ret = (crp->cr_psxmitpktscount) ?
-			ipath_read_creg32(dev->dd, crp->cr_psxmitpktscount) :
-			dev->ipath_spkts;
-		break;
-	case IB_PMA_PORT_RCV_PKTS:
-		ret = (crp->cr_psrcvpktscount) ?
-			ipath_read_creg32(dev->dd, crp->cr_psrcvpktscount) :
-			dev->ipath_rpkts;
-		break;
-	case IB_PMA_PORT_XMIT_WAIT:
-		ret = (crp->cr_psxmitwaitcount) ?
-			ipath_read_creg32(dev->dd, crp->cr_psxmitwaitcount) :
-			dev->ipath_xmit_wait;
-		break;
-	default:
-		ret = 0;
-	}
-
-	return ret;
-}
-
-static int recv_pma_get_portsamplesresult(struct ib_pma_mad *pmp,
-					  struct ib_device *ibdev)
-{
-	struct ib_pma_portsamplesresult *p =
-		(struct ib_pma_portsamplesresult *)pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_cregs const *crp = dev->dd->ipath_cregs;
-	u8 status;
-	int i;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-	p->tag = cpu_to_be16(dev->pma_tag);
-	if (crp->cr_psstat)
-		status = ipath_read_creg32(dev->dd, crp->cr_psstat);
-	else
-		status = dev->pma_sample_status;
-	p->sample_status = cpu_to_be16(status);
-	for (i = 0; i < ARRAY_SIZE(dev->pma_counter_select); i++)
-		p->counter[i] = (status != IB_PMA_SAMPLE_STATUS_DONE) ? 0 :
-		    cpu_to_be32(
-			get_counter(dev, crp, dev->pma_counter_select[i]));
-
-	return reply((struct ib_smp *) pmp);
-}
-
-static int recv_pma_get_portsamplesresult_ext(struct ib_pma_mad *pmp,
-					      struct ib_device *ibdev)
-{
-	struct ib_pma_portsamplesresult_ext *p =
-		(struct ib_pma_portsamplesresult_ext *)pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_cregs const *crp = dev->dd->ipath_cregs;
-	u8 status;
-	int i;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-	p->tag = cpu_to_be16(dev->pma_tag);
-	if (crp->cr_psstat)
-		status = ipath_read_creg32(dev->dd, crp->cr_psstat);
-	else
-		status = dev->pma_sample_status;
-	p->sample_status = cpu_to_be16(status);
-	/* 64 bits */
-	p->extended_width = cpu_to_be32(0x80000000);
-	for (i = 0; i < ARRAY_SIZE(dev->pma_counter_select); i++)
-		p->counter[i] = (status != IB_PMA_SAMPLE_STATUS_DONE) ? 0 :
-		    cpu_to_be64(
-			get_counter(dev, crp, dev->pma_counter_select[i]));
-
-	return reply((struct ib_smp *) pmp);
-}
-
-static int recv_pma_get_portcounters(struct ib_pma_mad *pmp,
-				     struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portcounters *p = (struct ib_pma_portcounters *)
-		pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_verbs_counters cntrs;
-	u8 port_select = p->port_select;
-
-	ipath_get_counters(dev->dd, &cntrs);
-
-	/* Adjust counters for any resets done. */
-	cntrs.symbol_error_counter -= dev->z_symbol_error_counter;
-	cntrs.link_error_recovery_counter -=
-		dev->z_link_error_recovery_counter;
-	cntrs.link_downed_counter -= dev->z_link_downed_counter;
-	cntrs.port_rcv_errors += dev->rcv_errors;
-	cntrs.port_rcv_errors -= dev->z_port_rcv_errors;
-	cntrs.port_rcv_remphys_errors -= dev->z_port_rcv_remphys_errors;
-	cntrs.port_xmit_discards -= dev->z_port_xmit_discards;
-	cntrs.port_xmit_data -= dev->z_port_xmit_data;
-	cntrs.port_rcv_data -= dev->z_port_rcv_data;
-	cntrs.port_xmit_packets -= dev->z_port_xmit_packets;
-	cntrs.port_rcv_packets -= dev->z_port_rcv_packets;
-	cntrs.local_link_integrity_errors -=
-		dev->z_local_link_integrity_errors;
-	cntrs.excessive_buffer_overrun_errors -=
-		dev->z_excessive_buffer_overrun_errors;
-	cntrs.vl15_dropped -= dev->z_vl15_dropped;
-	cntrs.vl15_dropped += dev->n_vl15_dropped;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-
-	p->port_select = port_select;
-	if (pmp->mad_hdr.attr_mod != 0 ||
-	    (port_select != port && port_select != 0xFF))
-		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
-
-	if (cntrs.symbol_error_counter > 0xFFFFUL)
-		p->symbol_error_counter = cpu_to_be16(0xFFFF);
-	else
-		p->symbol_error_counter =
-			cpu_to_be16((u16)cntrs.symbol_error_counter);
-	if (cntrs.link_error_recovery_counter > 0xFFUL)
-		p->link_error_recovery_counter = 0xFF;
-	else
-		p->link_error_recovery_counter =
-			(u8)cntrs.link_error_recovery_counter;
-	if (cntrs.link_downed_counter > 0xFFUL)
-		p->link_downed_counter = 0xFF;
-	else
-		p->link_downed_counter = (u8)cntrs.link_downed_counter;
-	if (cntrs.port_rcv_errors > 0xFFFFUL)
-		p->port_rcv_errors = cpu_to_be16(0xFFFF);
-	else
-		p->port_rcv_errors =
-			cpu_to_be16((u16) cntrs.port_rcv_errors);
-	if (cntrs.port_rcv_remphys_errors > 0xFFFFUL)
-		p->port_rcv_remphys_errors = cpu_to_be16(0xFFFF);
-	else
-		p->port_rcv_remphys_errors =
-			cpu_to_be16((u16)cntrs.port_rcv_remphys_errors);
-	if (cntrs.port_xmit_discards > 0xFFFFUL)
-		p->port_xmit_discards = cpu_to_be16(0xFFFF);
-	else
-		p->port_xmit_discards =
-			cpu_to_be16((u16)cntrs.port_xmit_discards);
-	if (cntrs.local_link_integrity_errors > 0xFUL)
-		cntrs.local_link_integrity_errors = 0xFUL;
-	if (cntrs.excessive_buffer_overrun_errors > 0xFUL)
-		cntrs.excessive_buffer_overrun_errors = 0xFUL;
-	p->link_overrun_errors = (cntrs.local_link_integrity_errors << 4) |
-		cntrs.excessive_buffer_overrun_errors;
-	if (cntrs.vl15_dropped > 0xFFFFUL)
-		p->vl15_dropped = cpu_to_be16(0xFFFF);
-	else
-		p->vl15_dropped = cpu_to_be16((u16)cntrs.vl15_dropped);
-	if (cntrs.port_xmit_data > 0xFFFFFFFFUL)
-		p->port_xmit_data = cpu_to_be32(0xFFFFFFFF);
-	else
-		p->port_xmit_data = cpu_to_be32((u32)cntrs.port_xmit_data);
-	if (cntrs.port_rcv_data > 0xFFFFFFFFUL)
-		p->port_rcv_data = cpu_to_be32(0xFFFFFFFF);
-	else
-		p->port_rcv_data = cpu_to_be32((u32)cntrs.port_rcv_data);
-	if (cntrs.port_xmit_packets > 0xFFFFFFFFUL)
-		p->port_xmit_packets = cpu_to_be32(0xFFFFFFFF);
-	else
-		p->port_xmit_packets =
-			cpu_to_be32((u32)cntrs.port_xmit_packets);
-	if (cntrs.port_rcv_packets > 0xFFFFFFFFUL)
-		p->port_rcv_packets = cpu_to_be32(0xFFFFFFFF);
-	else
-		p->port_rcv_packets =
-			cpu_to_be32((u32) cntrs.port_rcv_packets);
-
-	return reply((struct ib_smp *) pmp);
-}
-
-static int recv_pma_get_portcounters_ext(struct ib_pma_mad *pmp,
-					 struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portcounters_ext *p =
-		(struct ib_pma_portcounters_ext *)pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	u64 swords, rwords, spkts, rpkts, xwait;
-	u8 port_select = p->port_select;
-
-	ipath_snapshot_counters(dev->dd, &swords, &rwords, &spkts,
-				&rpkts, &xwait);
-
-	/* Adjust counters for any resets done. */
-	swords -= dev->z_port_xmit_data;
-	rwords -= dev->z_port_rcv_data;
-	spkts -= dev->z_port_xmit_packets;
-	rpkts -= dev->z_port_rcv_packets;
-
-	memset(pmp->data, 0, sizeof(pmp->data));
-
-	p->port_select = port_select;
-	if (pmp->mad_hdr.attr_mod != 0 ||
-	    (port_select != port && port_select != 0xFF))
-		pmp->mad_hdr.status |= IB_SMP_INVALID_FIELD;
-
-	p->port_xmit_data = cpu_to_be64(swords);
-	p->port_rcv_data = cpu_to_be64(rwords);
-	p->port_xmit_packets = cpu_to_be64(spkts);
-	p->port_rcv_packets = cpu_to_be64(rpkts);
-	p->port_unicast_xmit_packets = cpu_to_be64(dev->n_unicast_xmit);
-	p->port_unicast_rcv_packets = cpu_to_be64(dev->n_unicast_rcv);
-	p->port_multicast_xmit_packets = cpu_to_be64(dev->n_multicast_xmit);
-	p->port_multicast_rcv_packets = cpu_to_be64(dev->n_multicast_rcv);
-
-	return reply((struct ib_smp *) pmp);
-}
-
-static int recv_pma_set_portcounters(struct ib_pma_mad *pmp,
-				     struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portcounters *p = (struct ib_pma_portcounters *)
-		pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_verbs_counters cntrs;
-
-	/*
-	 * Since the HW doesn't support clearing counters, we save the
-	 * current count and subtract it from future responses.
-	 */
-	ipath_get_counters(dev->dd, &cntrs);
-
-	if (p->counter_select & IB_PMA_SEL_SYMBOL_ERROR)
-		dev->z_symbol_error_counter = cntrs.symbol_error_counter;
-
-	if (p->counter_select & IB_PMA_SEL_LINK_ERROR_RECOVERY)
-		dev->z_link_error_recovery_counter =
-			cntrs.link_error_recovery_counter;
-
-	if (p->counter_select & IB_PMA_SEL_LINK_DOWNED)
-		dev->z_link_downed_counter = cntrs.link_downed_counter;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_RCV_ERRORS)
-		dev->z_port_rcv_errors =
-			cntrs.port_rcv_errors + dev->rcv_errors;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_RCV_REMPHYS_ERRORS)
-		dev->z_port_rcv_remphys_errors =
-			cntrs.port_rcv_remphys_errors;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_XMIT_DISCARDS)
-		dev->z_port_xmit_discards = cntrs.port_xmit_discards;
-
-	if (p->counter_select & IB_PMA_SEL_LOCAL_LINK_INTEGRITY_ERRORS)
-		dev->z_local_link_integrity_errors =
-			cntrs.local_link_integrity_errors;
-
-	if (p->counter_select & IB_PMA_SEL_EXCESSIVE_BUFFER_OVERRUNS)
-		dev->z_excessive_buffer_overrun_errors =
-			cntrs.excessive_buffer_overrun_errors;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_VL15_DROPPED) {
-		dev->n_vl15_dropped = 0;
-		dev->z_vl15_dropped = cntrs.vl15_dropped;
-	}
-
-	if (p->counter_select & IB_PMA_SEL_PORT_XMIT_DATA)
-		dev->z_port_xmit_data = cntrs.port_xmit_data;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_RCV_DATA)
-		dev->z_port_rcv_data = cntrs.port_rcv_data;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_XMIT_PACKETS)
-		dev->z_port_xmit_packets = cntrs.port_xmit_packets;
-
-	if (p->counter_select & IB_PMA_SEL_PORT_RCV_PACKETS)
-		dev->z_port_rcv_packets = cntrs.port_rcv_packets;
-
-	return recv_pma_get_portcounters(pmp, ibdev, port);
-}
-
-static int recv_pma_set_portcounters_ext(struct ib_pma_mad *pmp,
-					 struct ib_device *ibdev, u8 port)
-{
-	struct ib_pma_portcounters *p = (struct ib_pma_portcounters *)
-		pmp->data;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	u64 swords, rwords, spkts, rpkts, xwait;
-
-	ipath_snapshot_counters(dev->dd, &swords, &rwords, &spkts,
-				&rpkts, &xwait);
-
-	if (p->counter_select & IB_PMA_SELX_PORT_XMIT_DATA)
-		dev->z_port_xmit_data = swords;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_RCV_DATA)
-		dev->z_port_rcv_data = rwords;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_XMIT_PACKETS)
-		dev->z_port_xmit_packets = spkts;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_RCV_PACKETS)
-		dev->z_port_rcv_packets = rpkts;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_UNI_XMIT_PACKETS)
-		dev->n_unicast_xmit = 0;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_UNI_RCV_PACKETS)
-		dev->n_unicast_rcv = 0;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_MULTI_XMIT_PACKETS)
-		dev->n_multicast_xmit = 0;
-
-	if (p->counter_select & IB_PMA_SELX_PORT_MULTI_RCV_PACKETS)
-		dev->n_multicast_rcv = 0;
-
-	return recv_pma_get_portcounters_ext(pmp, ibdev, port);
-}
-
-static int process_subn(struct ib_device *ibdev, int mad_flags,
-			u8 port_num, const struct ib_mad *in_mad,
-			struct ib_mad *out_mad)
-{
-	struct ib_smp *smp = (struct ib_smp *)out_mad;
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	int ret;
-
-	*out_mad = *in_mad;
-	if (smp->class_version != 1) {
-		smp->status |= IB_SMP_UNSUP_VERSION;
-		ret = reply(smp);
-		goto bail;
-	}
-
-	/* Is the mkey in the process of expiring? */
-	if (dev->mkey_lease_timeout &&
-	    time_after_eq(jiffies, dev->mkey_lease_timeout)) {
-		/* Clear timeout and mkey protection field. */
-		dev->mkey_lease_timeout = 0;
-		dev->mkeyprot = 0;
-	}
-
-	/*
-	 * M_Key checking depends on
-	 * Portinfo:M_Key_protect_bits
-	 */
-	if ((mad_flags & IB_MAD_IGNORE_MKEY) == 0 && dev->mkey != 0 &&
-	    dev->mkey != smp->mkey &&
-	    (smp->method == IB_MGMT_METHOD_SET ||
-	     (smp->method == IB_MGMT_METHOD_GET &&
-	      dev->mkeyprot >= 2))) {
-		if (dev->mkey_violations != 0xFFFF)
-			++dev->mkey_violations;
-		if (dev->mkey_lease_timeout ||
-		    dev->mkey_lease_period == 0) {
-			ret = IB_MAD_RESULT_SUCCESS |
-				IB_MAD_RESULT_CONSUMED;
-			goto bail;
-		}
-		dev->mkey_lease_timeout = jiffies +
-			dev->mkey_lease_period * HZ;
-		/* Future: Generate a trap notice. */
-		ret = IB_MAD_RESULT_SUCCESS | IB_MAD_RESULT_CONSUMED;
-		goto bail;
-	} else if (dev->mkey_lease_timeout)
-		dev->mkey_lease_timeout = 0;
-
-	switch (smp->method) {
-	case IB_MGMT_METHOD_GET:
-		switch (smp->attr_id) {
-		case IB_SMP_ATTR_NODE_DESC:
-			ret = recv_subn_get_nodedescription(smp, ibdev);
-			goto bail;
-		case IB_SMP_ATTR_NODE_INFO:
-			ret = recv_subn_get_nodeinfo(smp, ibdev, port_num);
-			goto bail;
-		case IB_SMP_ATTR_GUID_INFO:
-			ret = recv_subn_get_guidinfo(smp, ibdev);
-			goto bail;
-		case IB_SMP_ATTR_PORT_INFO:
-			ret = recv_subn_get_portinfo(smp, ibdev, port_num);
-			goto bail;
-		case IB_SMP_ATTR_PKEY_TABLE:
-			ret = recv_subn_get_pkeytable(smp, ibdev);
-			goto bail;
-		case IB_SMP_ATTR_SM_INFO:
-			if (dev->port_cap_flags & IB_PORT_SM_DISABLED) {
-				ret = IB_MAD_RESULT_SUCCESS |
-					IB_MAD_RESULT_CONSUMED;
-				goto bail;
-			}
-			if (dev->port_cap_flags & IB_PORT_SM) {
-				ret = IB_MAD_RESULT_SUCCESS;
-				goto bail;
-			}
-			/* FALLTHROUGH */
-		default:
-			smp->status |= IB_SMP_UNSUP_METH_ATTR;
-			ret = reply(smp);
-			goto bail;
-		}
-
-	case IB_MGMT_METHOD_SET:
-		switch (smp->attr_id) {
-		case IB_SMP_ATTR_GUID_INFO:
-			ret = recv_subn_set_guidinfo(smp, ibdev);
-			goto bail;
-		case IB_SMP_ATTR_PORT_INFO:
-			ret = recv_subn_set_portinfo(smp, ibdev, port_num);
-			goto bail;
-		case IB_SMP_ATTR_PKEY_TABLE:
-			ret = recv_subn_set_pkeytable(smp, ibdev, port_num);
-			goto bail;
-		case IB_SMP_ATTR_SM_INFO:
-			if (dev->port_cap_flags & IB_PORT_SM_DISABLED) {
-				ret = IB_MAD_RESULT_SUCCESS |
-					IB_MAD_RESULT_CONSUMED;
-				goto bail;
-			}
-			if (dev->port_cap_flags & IB_PORT_SM) {
-				ret = IB_MAD_RESULT_SUCCESS;
-				goto bail;
-			}
-			/* FALLTHROUGH */
-		default:
-			smp->status |= IB_SMP_UNSUP_METH_ATTR;
-			ret = reply(smp);
-			goto bail;
-		}
-
-	case IB_MGMT_METHOD_TRAP:
-	case IB_MGMT_METHOD_REPORT:
-	case IB_MGMT_METHOD_REPORT_RESP:
-	case IB_MGMT_METHOD_TRAP_REPRESS:
-	case IB_MGMT_METHOD_GET_RESP:
-		/*
-		 * The ib_mad module will call us to process responses
-		 * before checking for other consumers.
-		 * Just tell the caller to process it normally.
-		 */
-		ret = IB_MAD_RESULT_SUCCESS;
-		goto bail;
-	default:
-		smp->status |= IB_SMP_UNSUP_METHOD;
-		ret = reply(smp);
-	}
-
-bail:
-	return ret;
-}
-
-static int process_perf(struct ib_device *ibdev, u8 port_num,
-			const struct ib_mad *in_mad,
-			struct ib_mad *out_mad)
-{
-	struct ib_pma_mad *pmp = (struct ib_pma_mad *)out_mad;
-	int ret;
-
-	*out_mad = *in_mad;
-	if (pmp->mad_hdr.class_version != 1) {
-		pmp->mad_hdr.status |= IB_SMP_UNSUP_VERSION;
-		ret = reply((struct ib_smp *) pmp);
-		goto bail;
-	}
-
-	switch (pmp->mad_hdr.method) {
-	case IB_MGMT_METHOD_GET:
-		switch (pmp->mad_hdr.attr_id) {
-		case IB_PMA_CLASS_PORT_INFO:
-			ret = recv_pma_get_classportinfo(pmp);
-			goto bail;
-		case IB_PMA_PORT_SAMPLES_CONTROL:
-			ret = recv_pma_get_portsamplescontrol(pmp, ibdev,
-							      port_num);
-			goto bail;
-		case IB_PMA_PORT_SAMPLES_RESULT:
-			ret = recv_pma_get_portsamplesresult(pmp, ibdev);
-			goto bail;
-		case IB_PMA_PORT_SAMPLES_RESULT_EXT:
-			ret = recv_pma_get_portsamplesresult_ext(pmp,
-								 ibdev);
-			goto bail;
-		case IB_PMA_PORT_COUNTERS:
-			ret = recv_pma_get_portcounters(pmp, ibdev,
-							port_num);
-			goto bail;
-		case IB_PMA_PORT_COUNTERS_EXT:
-			ret = recv_pma_get_portcounters_ext(pmp, ibdev,
-							    port_num);
-			goto bail;
-		default:
-			pmp->mad_hdr.status |= IB_SMP_UNSUP_METH_ATTR;
-			ret = reply((struct ib_smp *) pmp);
-			goto bail;
-		}
-
-	case IB_MGMT_METHOD_SET:
-		switch (pmp->mad_hdr.attr_id) {
-		case IB_PMA_PORT_SAMPLES_CONTROL:
-			ret = recv_pma_set_portsamplescontrol(pmp, ibdev,
-							      port_num);
-			goto bail;
-		case IB_PMA_PORT_COUNTERS:
-			ret = recv_pma_set_portcounters(pmp, ibdev,
-							port_num);
-			goto bail;
-		case IB_PMA_PORT_COUNTERS_EXT:
-			ret = recv_pma_set_portcounters_ext(pmp, ibdev,
-							    port_num);
-			goto bail;
-		default:
-			pmp->mad_hdr.status |= IB_SMP_UNSUP_METH_ATTR;
-			ret = reply((struct ib_smp *) pmp);
-			goto bail;
-		}
-
-	case IB_MGMT_METHOD_GET_RESP:
-		/*
-		 * The ib_mad module will call us to process responses
-		 * before checking for other consumers.
-		 * Just tell the caller to process it normally.
-		 */
-		ret = IB_MAD_RESULT_SUCCESS;
-		goto bail;
-	default:
-		pmp->mad_hdr.status |= IB_SMP_UNSUP_METHOD;
-		ret = reply((struct ib_smp *) pmp);
-	}
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_process_mad - process an incoming MAD packet
- * @ibdev: the infiniband device this packet came in on
- * @mad_flags: MAD flags
- * @port_num: the port number this packet came in on
- * @in_wc: the work completion entry for this packet
- * @in_grh: the global route header for this packet
- * @in_mad: the incoming MAD
- * @out_mad: any outgoing MAD reply
- *
- * Returns IB_MAD_RESULT_SUCCESS if this is a MAD that we are not
- * interested in processing.
- *
- * Note that the verbs framework has already done the MAD sanity checks,
- * and hop count/pointer updating for IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE
- * MADs.
- *
- * This is called by the ib_mad module.
- */
-int ipath_process_mad(struct ib_device *ibdev, int mad_flags, u8 port_num,
-		      const struct ib_wc *in_wc, const struct ib_grh *in_grh,
-		      const struct ib_mad_hdr *in, size_t in_mad_size,
-		      struct ib_mad_hdr *out, size_t *out_mad_size,
-		      u16 *out_mad_pkey_index)
-{
-	int ret;
-	const struct ib_mad *in_mad = (const struct ib_mad *)in;
-	struct ib_mad *out_mad = (struct ib_mad *)out;
-
-	if (WARN_ON_ONCE(in_mad_size != sizeof(*in_mad) ||
-			 *out_mad_size != sizeof(*out_mad)))
-		return IB_MAD_RESULT_FAILURE;
-
-	switch (in_mad->mad_hdr.mgmt_class) {
-	case IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE:
-	case IB_MGMT_CLASS_SUBN_LID_ROUTED:
-		ret = process_subn(ibdev, mad_flags, port_num,
-				   in_mad, out_mad);
-		goto bail;
-	case IB_MGMT_CLASS_PERF_MGMT:
-		ret = process_perf(ibdev, port_num, in_mad, out_mad);
-		goto bail;
-	default:
-		ret = IB_MAD_RESULT_SUCCESS;
-	}
-
-bail:
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_mmap.c b/drivers/staging/rdma/ipath/ipath_mmap.c
deleted file mode 100644
index e73274229404..000000000000
--- a/drivers/staging/rdma/ipath/ipath_mmap.c
+++ /dev/null
@@ -1,174 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <linux/module.h>
-#include <linux/vmalloc.h>
-#include <linux/slab.h>
-#include <linux/mm.h>
-#include <linux/errno.h>
-#include <asm/pgtable.h>
-
-#include "ipath_verbs.h"
-
-/**
- * ipath_release_mmap_info - free mmap info structure
- * @ref: a pointer to the kref within struct ipath_mmap_info
- */
-void ipath_release_mmap_info(struct kref *ref)
-{
-	struct ipath_mmap_info *ip =
-		container_of(ref, struct ipath_mmap_info, ref);
-	struct ipath_ibdev *dev = to_idev(ip->context->device);
-
-	spin_lock_irq(&dev->pending_lock);
-	list_del(&ip->pending_mmaps);
-	spin_unlock_irq(&dev->pending_lock);
-
-	vfree(ip->obj);
-	kfree(ip);
-}
-
-/*
- * open and close keep track of how many times the CQ is mapped,
- * to avoid releasing it.
- */
-static void ipath_vma_open(struct vm_area_struct *vma)
-{
-	struct ipath_mmap_info *ip = vma->vm_private_data;
-
-	kref_get(&ip->ref);
-}
-
-static void ipath_vma_close(struct vm_area_struct *vma)
-{
-	struct ipath_mmap_info *ip = vma->vm_private_data;
-
-	kref_put(&ip->ref, ipath_release_mmap_info);
-}
-
-static const struct vm_operations_struct ipath_vm_ops = {
-	.open =     ipath_vma_open,
-	.close =    ipath_vma_close,
-};
-
-/**
- * ipath_mmap - create a new mmap region
- * @context: the IB user context of the process making the mmap() call
- * @vma: the VMA to be initialized
- * Return zero if the mmap is OK. Otherwise, return an errno.
- */
-int ipath_mmap(struct ib_ucontext *context, struct vm_area_struct *vma)
-{
-	struct ipath_ibdev *dev = to_idev(context->device);
-	unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
-	unsigned long size = vma->vm_end - vma->vm_start;
-	struct ipath_mmap_info *ip, *pp;
-	int ret = -EINVAL;
-
-	/*
-	 * Search the device's list of objects waiting for a mmap call.
-	 * Normally, this list is very short since a call to create a
-	 * CQ, QP, or SRQ is soon followed by a call to mmap().
-	 */
-	spin_lock_irq(&dev->pending_lock);
-	list_for_each_entry_safe(ip, pp, &dev->pending_mmaps,
-				 pending_mmaps) {
-		/* Only the creator is allowed to mmap the object */
-		if (context != ip->context || (__u64) offset != ip->offset)
-			continue;
-		/* Don't allow a mmap larger than the object. */
-		if (size > ip->size)
-			break;
-
-		list_del_init(&ip->pending_mmaps);
-		spin_unlock_irq(&dev->pending_lock);
-
-		ret = remap_vmalloc_range(vma, ip->obj, 0);
-		if (ret)
-			goto done;
-		vma->vm_ops = &ipath_vm_ops;
-		vma->vm_private_data = ip;
-		ipath_vma_open(vma);
-		goto done;
-	}
-	spin_unlock_irq(&dev->pending_lock);
-done:
-	return ret;
-}
-
-/*
- * Allocate information for ipath_mmap
- */
-struct ipath_mmap_info *ipath_create_mmap_info(struct ipath_ibdev *dev,
-					       u32 size,
-					       struct ib_ucontext *context,
-					       void *obj) {
-	struct ipath_mmap_info *ip;
-
-	ip = kmalloc(sizeof *ip, GFP_KERNEL);
-	if (!ip)
-		goto bail;
-
-	size = PAGE_ALIGN(size);
-
-	spin_lock_irq(&dev->mmap_offset_lock);
-	if (dev->mmap_offset == 0)
-		dev->mmap_offset = PAGE_SIZE;
-	ip->offset = dev->mmap_offset;
-	dev->mmap_offset += size;
-	spin_unlock_irq(&dev->mmap_offset_lock);
-
-	INIT_LIST_HEAD(&ip->pending_mmaps);
-	ip->size = size;
-	ip->context = context;
-	ip->obj = obj;
-	kref_init(&ip->ref);
-
-bail:
-	return ip;
-}
-
-void ipath_update_mmap_info(struct ipath_ibdev *dev,
-			    struct ipath_mmap_info *ip,
-			    u32 size, void *obj) {
-	size = PAGE_ALIGN(size);
-
-	spin_lock_irq(&dev->mmap_offset_lock);
-	if (dev->mmap_offset == 0)
-		dev->mmap_offset = PAGE_SIZE;
-	ip->offset = dev->mmap_offset;
-	dev->mmap_offset += size;
-	spin_unlock_irq(&dev->mmap_offset_lock);
-
-	ip->size = size;
-	ip->obj = obj;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_mr.c b/drivers/staging/rdma/ipath/ipath_mr.c
deleted file mode 100644
index c7278f6a8217..000000000000
--- a/drivers/staging/rdma/ipath/ipath_mr.c
+++ /dev/null
@@ -1,425 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/slab.h>
-
-#include <rdma/ib_umem.h>
-#include <rdma/ib_pack.h>
-#include <rdma/ib_smi.h>
-
-#include "ipath_verbs.h"
-
-/* Fast memory region */
-struct ipath_fmr {
-	struct ib_fmr ibfmr;
-	u8 page_shift;
-	struct ipath_mregion mr;        /* must be last */
-};
-
-static inline struct ipath_fmr *to_ifmr(struct ib_fmr *ibfmr)
-{
-	return container_of(ibfmr, struct ipath_fmr, ibfmr);
-}
-
-/**
- * ipath_get_dma_mr - get a DMA memory region
- * @pd: protection domain for this memory region
- * @acc: access flags
- *
- * Returns the memory region on success, otherwise returns an errno.
- * Note that all DMA addresses should be created via the
- * struct ib_dma_mapping_ops functions (see ipath_dma.c).
- */
-struct ib_mr *ipath_get_dma_mr(struct ib_pd *pd, int acc)
-{
-	struct ipath_mr *mr;
-	struct ib_mr *ret;
-
-	mr = kzalloc(sizeof *mr, GFP_KERNEL);
-	if (!mr) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	mr->mr.access_flags = acc;
-	ret = &mr->ibmr;
-
-bail:
-	return ret;
-}
-
-static struct ipath_mr *alloc_mr(int count,
-				 struct ipath_lkey_table *lk_table)
-{
-	struct ipath_mr *mr;
-	int m, i = 0;
-
-	/* Allocate struct plus pointers to first level page tables. */
-	m = (count + IPATH_SEGSZ - 1) / IPATH_SEGSZ;
-	mr = kmalloc(sizeof *mr + m * sizeof mr->mr.map[0], GFP_KERNEL);
-	if (!mr)
-		goto done;
-
-	/* Allocate first level page tables. */
-	for (; i < m; i++) {
-		mr->mr.map[i] = kmalloc(sizeof *mr->mr.map[0], GFP_KERNEL);
-		if (!mr->mr.map[i])
-			goto bail;
-	}
-	mr->mr.mapsz = m;
-
-	/*
-	 * ib_reg_phys_mr() will initialize mr->ibmr except for
-	 * lkey and rkey.
-	 */
-	if (!ipath_alloc_lkey(lk_table, &mr->mr))
-		goto bail;
-	mr->ibmr.rkey = mr->ibmr.lkey = mr->mr.lkey;
-
-	goto done;
-
-bail:
-	while (i) {
-		i--;
-		kfree(mr->mr.map[i]);
-	}
-	kfree(mr);
-	mr = NULL;
-
-done:
-	return mr;
-}
-
-/**
- * ipath_reg_phys_mr - register a physical memory region
- * @pd: protection domain for this memory region
- * @buffer_list: pointer to the list of physical buffers to register
- * @num_phys_buf: the number of physical buffers to register
- * @iova_start: the starting address passed over IB which maps to this MR
- *
- * Returns the memory region on success, otherwise returns an errno.
- */
-struct ib_mr *ipath_reg_phys_mr(struct ib_pd *pd,
-				struct ib_phys_buf *buffer_list,
-				int num_phys_buf, int acc, u64 *iova_start)
-{
-	struct ipath_mr *mr;
-	int n, m, i;
-	struct ib_mr *ret;
-
-	mr = alloc_mr(num_phys_buf, &to_idev(pd->device)->lk_table);
-	if (mr == NULL) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	mr->mr.pd = pd;
-	mr->mr.user_base = *iova_start;
-	mr->mr.iova = *iova_start;
-	mr->mr.length = 0;
-	mr->mr.offset = 0;
-	mr->mr.access_flags = acc;
-	mr->mr.max_segs = num_phys_buf;
-	mr->umem = NULL;
-
-	m = 0;
-	n = 0;
-	for (i = 0; i < num_phys_buf; i++) {
-		mr->mr.map[m]->segs[n].vaddr = (void *) buffer_list[i].addr;
-		mr->mr.map[m]->segs[n].length = buffer_list[i].size;
-		mr->mr.length += buffer_list[i].size;
-		n++;
-		if (n == IPATH_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-
-	ret = &mr->ibmr;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_reg_user_mr - register a userspace memory region
- * @pd: protection domain for this memory region
- * @start: starting userspace address
- * @length: length of region to register
- * @virt_addr: virtual address to use (from HCA's point of view)
- * @mr_access_flags: access flags for this memory region
- * @udata: unused by the InfiniPath driver
- *
- * Returns the memory region on success, otherwise returns an errno.
- */
-struct ib_mr *ipath_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
-				u64 virt_addr, int mr_access_flags,
-				struct ib_udata *udata)
-{
-	struct ipath_mr *mr;
-	struct ib_umem *umem;
-	int n, m, entry;
-	struct scatterlist *sg;
-	struct ib_mr *ret;
-
-	if (length == 0) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	umem = ib_umem_get(pd->uobject->context, start, length,
-			   mr_access_flags, 0);
-	if (IS_ERR(umem))
-		return (void *) umem;
-
-	n = umem->nmap;
-	mr = alloc_mr(n, &to_idev(pd->device)->lk_table);
-	if (!mr) {
-		ret = ERR_PTR(-ENOMEM);
-		ib_umem_release(umem);
-		goto bail;
-	}
-
-	mr->mr.pd = pd;
-	mr->mr.user_base = start;
-	mr->mr.iova = virt_addr;
-	mr->mr.length = length;
-	mr->mr.offset = ib_umem_offset(umem);
-	mr->mr.access_flags = mr_access_flags;
-	mr->mr.max_segs = n;
-	mr->umem = umem;
-
-	m = 0;
-	n = 0;
-	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
-		void *vaddr;
-
-		vaddr = page_address(sg_page(sg));
-		if (!vaddr) {
-			ret = ERR_PTR(-EINVAL);
-			goto bail;
-		}
-		mr->mr.map[m]->segs[n].vaddr = vaddr;
-		mr->mr.map[m]->segs[n].length = umem->page_size;
-		n++;
-		if (n == IPATH_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-	ret = &mr->ibmr;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_dereg_mr - unregister and free a memory region
- * @ibmr: the memory region to free
- *
- * Returns 0 on success.
- *
- * Note that this is called to free MRs created by ipath_get_dma_mr()
- * or ipath_reg_user_mr().
- */
-int ipath_dereg_mr(struct ib_mr *ibmr)
-{
-	struct ipath_mr *mr = to_imr(ibmr);
-	int i;
-
-	ipath_free_lkey(&to_idev(ibmr->device)->lk_table, ibmr->lkey);
-	i = mr->mr.mapsz;
-	while (i) {
-		i--;
-		kfree(mr->mr.map[i]);
-	}
-
-	if (mr->umem)
-		ib_umem_release(mr->umem);
-
-	kfree(mr);
-	return 0;
-}
-
-/**
- * ipath_alloc_fmr - allocate a fast memory region
- * @pd: the protection domain for this memory region
- * @mr_access_flags: access flags for this memory region
- * @fmr_attr: fast memory region attributes
- *
- * Returns the memory region on success, otherwise returns an errno.
- */
-struct ib_fmr *ipath_alloc_fmr(struct ib_pd *pd, int mr_access_flags,
-			       struct ib_fmr_attr *fmr_attr)
-{
-	struct ipath_fmr *fmr;
-	int m, i = 0;
-	struct ib_fmr *ret;
-
-	/* Allocate struct plus pointers to first level page tables. */
-	m = (fmr_attr->max_pages + IPATH_SEGSZ - 1) / IPATH_SEGSZ;
-	fmr = kmalloc(sizeof *fmr + m * sizeof fmr->mr.map[0], GFP_KERNEL);
-	if (!fmr)
-		goto bail;
-
-	/* Allocate first level page tables. */
-	for (; i < m; i++) {
-		fmr->mr.map[i] = kmalloc(sizeof *fmr->mr.map[0],
-					 GFP_KERNEL);
-		if (!fmr->mr.map[i])
-			goto bail;
-	}
-	fmr->mr.mapsz = m;
-
-	/*
-	 * ib_alloc_fmr() will initialize fmr->ibfmr except for lkey &
-	 * rkey.
-	 */
-	if (!ipath_alloc_lkey(&to_idev(pd->device)->lk_table, &fmr->mr))
-		goto bail;
-	fmr->ibfmr.rkey = fmr->ibfmr.lkey = fmr->mr.lkey;
-	/*
-	 * Resources are allocated but no valid mapping (RKEY can't be
-	 * used).
-	 */
-	fmr->mr.pd = pd;
-	fmr->mr.user_base = 0;
-	fmr->mr.iova = 0;
-	fmr->mr.length = 0;
-	fmr->mr.offset = 0;
-	fmr->mr.access_flags = mr_access_flags;
-	fmr->mr.max_segs = fmr_attr->max_pages;
-	fmr->page_shift = fmr_attr->page_shift;
-
-	ret = &fmr->ibfmr;
-	goto done;
-
-bail:
-	while (i)
-		kfree(fmr->mr.map[--i]);
-	kfree(fmr);
-	ret = ERR_PTR(-ENOMEM);
-
-done:
-	return ret;
-}
-
-/**
- * ipath_map_phys_fmr - set up a fast memory region
- * @ibmfr: the fast memory region to set up
- * @page_list: the list of pages to associate with the fast memory region
- * @list_len: the number of pages to associate with the fast memory region
- * @iova: the virtual address of the start of the fast memory region
- *
- * This may be called from interrupt context.
- */
-
-int ipath_map_phys_fmr(struct ib_fmr *ibfmr, u64 * page_list,
-		       int list_len, u64 iova)
-{
-	struct ipath_fmr *fmr = to_ifmr(ibfmr);
-	struct ipath_lkey_table *rkt;
-	unsigned long flags;
-	int m, n, i;
-	u32 ps;
-	int ret;
-
-	if (list_len > fmr->mr.max_segs) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	rkt = &to_idev(ibfmr->device)->lk_table;
-	spin_lock_irqsave(&rkt->lock, flags);
-	fmr->mr.user_base = iova;
-	fmr->mr.iova = iova;
-	ps = 1 << fmr->page_shift;
-	fmr->mr.length = list_len * ps;
-	m = 0;
-	n = 0;
-	ps = 1 << fmr->page_shift;
-	for (i = 0; i < list_len; i++) {
-		fmr->mr.map[m]->segs[n].vaddr = (void *) page_list[i];
-		fmr->mr.map[m]->segs[n].length = ps;
-		if (++n == IPATH_SEGSZ) {
-			m++;
-			n = 0;
-		}
-	}
-	spin_unlock_irqrestore(&rkt->lock, flags);
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_unmap_fmr - unmap fast memory regions
- * @fmr_list: the list of fast memory regions to unmap
- *
- * Returns 0 on success.
- */
-int ipath_unmap_fmr(struct list_head *fmr_list)
-{
-	struct ipath_fmr *fmr;
-	struct ipath_lkey_table *rkt;
-	unsigned long flags;
-
-	list_for_each_entry(fmr, fmr_list, ibfmr.list) {
-		rkt = &to_idev(fmr->ibfmr.device)->lk_table;
-		spin_lock_irqsave(&rkt->lock, flags);
-		fmr->mr.user_base = 0;
-		fmr->mr.iova = 0;
-		fmr->mr.length = 0;
-		spin_unlock_irqrestore(&rkt->lock, flags);
-	}
-	return 0;
-}
-
-/**
- * ipath_dealloc_fmr - deallocate a fast memory region
- * @ibfmr: the fast memory region to deallocate
- *
- * Returns 0 on success.
- */
-int ipath_dealloc_fmr(struct ib_fmr *ibfmr)
-{
-	struct ipath_fmr *fmr = to_ifmr(ibfmr);
-	int i;
-
-	ipath_free_lkey(&to_idev(ibfmr->device)->lk_table, ibfmr->lkey);
-	i = fmr->mr.mapsz;
-	while (i)
-		kfree(fmr->mr.map[--i]);
-	kfree(fmr);
-	return 0;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_qp.c b/drivers/staging/rdma/ipath/ipath_qp.c
deleted file mode 100644
index 280cd2d638e4..000000000000
--- a/drivers/staging/rdma/ipath/ipath_qp.c
+++ /dev/null
@@ -1,1079 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/err.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-
-#include "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-#define BITS_PER_PAGE		(PAGE_SIZE*BITS_PER_BYTE)
-#define BITS_PER_PAGE_MASK	(BITS_PER_PAGE-1)
-#define mk_qpn(qpt, map, off)	(((map) - (qpt)->map) * BITS_PER_PAGE + \
-				 (off))
-#define find_next_offset(map, off) find_next_zero_bit((map)->page, \
-						      BITS_PER_PAGE, off)
-
-/*
- * Convert the AETH credit code into the number of credits.
- */
-static u32 credit_table[31] = {
-	0,			/* 0 */
-	1,			/* 1 */
-	2,			/* 2 */
-	3,			/* 3 */
-	4,			/* 4 */
-	6,			/* 5 */
-	8,			/* 6 */
-	12,			/* 7 */
-	16,			/* 8 */
-	24,			/* 9 */
-	32,			/* A */
-	48,			/* B */
-	64,			/* C */
-	96,			/* D */
-	128,			/* E */
-	192,			/* F */
-	256,			/* 10 */
-	384,			/* 11 */
-	512,			/* 12 */
-	768,			/* 13 */
-	1024,			/* 14 */
-	1536,			/* 15 */
-	2048,			/* 16 */
-	3072,			/* 17 */
-	4096,			/* 18 */
-	6144,			/* 19 */
-	8192,			/* 1A */
-	12288,			/* 1B */
-	16384,			/* 1C */
-	24576,			/* 1D */
-	32768			/* 1E */
-};
-
-
-static void get_map_page(struct ipath_qp_table *qpt, struct qpn_map *map)
-{
-	unsigned long page = get_zeroed_page(GFP_KERNEL);
-	unsigned long flags;
-
-	/*
-	 * Free the page if someone raced with us installing it.
-	 */
-
-	spin_lock_irqsave(&qpt->lock, flags);
-	if (map->page)
-		free_page(page);
-	else
-		map->page = (void *)page;
-	spin_unlock_irqrestore(&qpt->lock, flags);
-}
-
-
-static int alloc_qpn(struct ipath_qp_table *qpt, enum ib_qp_type type)
-{
-	u32 i, offset, max_scan, qpn;
-	struct qpn_map *map;
-	u32 ret = -1;
-
-	if (type == IB_QPT_SMI)
-		ret = 0;
-	else if (type == IB_QPT_GSI)
-		ret = 1;
-
-	if (ret != -1) {
-		map = &qpt->map[0];
-		if (unlikely(!map->page)) {
-			get_map_page(qpt, map);
-			if (unlikely(!map->page)) {
-				ret = -ENOMEM;
-				goto bail;
-			}
-		}
-		if (!test_and_set_bit(ret, map->page))
-			atomic_dec(&map->n_free);
-		else
-			ret = -EBUSY;
-		goto bail;
-	}
-
-	qpn = qpt->last + 1;
-	if (qpn >= QPN_MAX)
-		qpn = 2;
-	offset = qpn & BITS_PER_PAGE_MASK;
-	map = &qpt->map[qpn / BITS_PER_PAGE];
-	max_scan = qpt->nmaps - !offset;
-	for (i = 0;;) {
-		if (unlikely(!map->page)) {
-			get_map_page(qpt, map);
-			if (unlikely(!map->page))
-				break;
-		}
-		if (likely(atomic_read(&map->n_free))) {
-			do {
-				if (!test_and_set_bit(offset, map->page)) {
-					atomic_dec(&map->n_free);
-					qpt->last = qpn;
-					ret = qpn;
-					goto bail;
-				}
-				offset = find_next_offset(map, offset);
-				qpn = mk_qpn(qpt, map, offset);
-				/*
-				 * This test differs from alloc_pidmap().
-				 * If find_next_offset() does find a zero
-				 * bit, we don't need to check for QPN
-				 * wrapping around past our starting QPN.
-				 * We just need to be sure we don't loop
-				 * forever.
-				 */
-			} while (offset < BITS_PER_PAGE && qpn < QPN_MAX);
-		}
-		/*
-		 * In order to keep the number of pages allocated to a
-		 * minimum, we scan the all existing pages before increasing
-		 * the size of the bitmap table.
-		 */
-		if (++i > max_scan) {
-			if (qpt->nmaps == QPNMAP_ENTRIES)
-				break;
-			map = &qpt->map[qpt->nmaps++];
-			offset = 0;
-		} else if (map < &qpt->map[qpt->nmaps]) {
-			++map;
-			offset = 0;
-		} else {
-			map = &qpt->map[0];
-			offset = 2;
-		}
-		qpn = mk_qpn(qpt, map, offset);
-	}
-
-	ret = -ENOMEM;
-
-bail:
-	return ret;
-}
-
-static void free_qpn(struct ipath_qp_table *qpt, u32 qpn)
-{
-	struct qpn_map *map;
-
-	map = qpt->map + qpn / BITS_PER_PAGE;
-	if (map->page)
-		clear_bit(qpn & BITS_PER_PAGE_MASK, map->page);
-	atomic_inc(&map->n_free);
-}
-
-/**
- * ipath_alloc_qpn - allocate a QP number
- * @qpt: the QP table
- * @qp: the QP
- * @type: the QP type (IB_QPT_SMI and IB_QPT_GSI are special)
- *
- * Allocate the next available QPN and put the QP into the hash table.
- * The hash table holds a reference to the QP.
- */
-static int ipath_alloc_qpn(struct ipath_qp_table *qpt, struct ipath_qp *qp,
-			   enum ib_qp_type type)
-{
-	unsigned long flags;
-	int ret;
-
-	ret = alloc_qpn(qpt, type);
-	if (ret < 0)
-		goto bail;
-	qp->ibqp.qp_num = ret;
-
-	/* Add the QP to the hash table. */
-	spin_lock_irqsave(&qpt->lock, flags);
-
-	ret %= qpt->max;
-	qp->next = qpt->table[ret];
-	qpt->table[ret] = qp;
-	atomic_inc(&qp->refcount);
-
-	spin_unlock_irqrestore(&qpt->lock, flags);
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_free_qp - remove a QP from the QP table
- * @qpt: the QP table
- * @qp: the QP to remove
- *
- * Remove the QP from the table so it can't be found asynchronously by
- * the receive interrupt routine.
- */
-static void ipath_free_qp(struct ipath_qp_table *qpt, struct ipath_qp *qp)
-{
-	struct ipath_qp *q, **qpp;
-	unsigned long flags;
-
-	spin_lock_irqsave(&qpt->lock, flags);
-
-	/* Remove QP from the hash table. */
-	qpp = &qpt->table[qp->ibqp.qp_num % qpt->max];
-	for (; (q = *qpp) != NULL; qpp = &q->next) {
-		if (q == qp) {
-			*qpp = qp->next;
-			qp->next = NULL;
-			atomic_dec(&qp->refcount);
-			break;
-		}
-	}
-
-	spin_unlock_irqrestore(&qpt->lock, flags);
-}
-
-/**
- * ipath_free_all_qps - check for QPs still in use
- * @qpt: the QP table to empty
- *
- * There should not be any QPs still in use.
- * Free memory for table.
- */
-unsigned ipath_free_all_qps(struct ipath_qp_table *qpt)
-{
-	unsigned long flags;
-	struct ipath_qp *qp;
-	u32 n, qp_inuse = 0;
-
-	spin_lock_irqsave(&qpt->lock, flags);
-	for (n = 0; n < qpt->max; n++) {
-		qp = qpt->table[n];
-		qpt->table[n] = NULL;
-
-		for (; qp; qp = qp->next)
-			qp_inuse++;
-	}
-	spin_unlock_irqrestore(&qpt->lock, flags);
-
-	for (n = 0; n < ARRAY_SIZE(qpt->map); n++)
-		if (qpt->map[n].page)
-			free_page((unsigned long) qpt->map[n].page);
-	return qp_inuse;
-}
-
-/**
- * ipath_lookup_qpn - return the QP with the given QPN
- * @qpt: the QP table
- * @qpn: the QP number to look up
- *
- * The caller is responsible for decrementing the QP reference count
- * when done.
- */
-struct ipath_qp *ipath_lookup_qpn(struct ipath_qp_table *qpt, u32 qpn)
-{
-	unsigned long flags;
-	struct ipath_qp *qp;
-
-	spin_lock_irqsave(&qpt->lock, flags);
-
-	for (qp = qpt->table[qpn % qpt->max]; qp; qp = qp->next) {
-		if (qp->ibqp.qp_num == qpn) {
-			atomic_inc(&qp->refcount);
-			break;
-		}
-	}
-
-	spin_unlock_irqrestore(&qpt->lock, flags);
-	return qp;
-}
-
-/**
- * ipath_reset_qp - initialize the QP state to the reset state
- * @qp: the QP to reset
- * @type: the QP type
- */
-static void ipath_reset_qp(struct ipath_qp *qp, enum ib_qp_type type)
-{
-	qp->remote_qpn = 0;
-	qp->qkey = 0;
-	qp->qp_access_flags = 0;
-	atomic_set(&qp->s_dma_busy, 0);
-	qp->s_flags &= IPATH_S_SIGNAL_REQ_WR;
-	qp->s_hdrwords = 0;
-	qp->s_wqe = NULL;
-	qp->s_pkt_delay = 0;
-	qp->s_draining = 0;
-	qp->s_psn = 0;
-	qp->r_psn = 0;
-	qp->r_msn = 0;
-	if (type == IB_QPT_RC) {
-		qp->s_state = IB_OPCODE_RC_SEND_LAST;
-		qp->r_state = IB_OPCODE_RC_SEND_LAST;
-	} else {
-		qp->s_state = IB_OPCODE_UC_SEND_LAST;
-		qp->r_state = IB_OPCODE_UC_SEND_LAST;
-	}
-	qp->s_ack_state = IB_OPCODE_RC_ACKNOWLEDGE;
-	qp->r_nak_state = 0;
-	qp->r_aflags = 0;
-	qp->r_flags = 0;
-	qp->s_rnr_timeout = 0;
-	qp->s_head = 0;
-	qp->s_tail = 0;
-	qp->s_cur = 0;
-	qp->s_last = 0;
-	qp->s_ssn = 1;
-	qp->s_lsn = 0;
-	memset(qp->s_ack_queue, 0, sizeof(qp->s_ack_queue));
-	qp->r_head_ack_queue = 0;
-	qp->s_tail_ack_queue = 0;
-	qp->s_num_rd_atomic = 0;
-	if (qp->r_rq.wq) {
-		qp->r_rq.wq->head = 0;
-		qp->r_rq.wq->tail = 0;
-	}
-}
-
-/**
- * ipath_error_qp - put a QP into the error state
- * @qp: the QP to put into the error state
- * @err: the receive completion error to signal if a RWQE is active
- *
- * Flushes both send and receive work queues.
- * Returns true if last WQE event should be generated.
- * The QP s_lock should be held and interrupts disabled.
- * If we are already in error state, just return.
- */
-
-int ipath_error_qp(struct ipath_qp *qp, enum ib_wc_status err)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ib_wc wc;
-	int ret = 0;
-
-	if (qp->state == IB_QPS_ERR)
-		goto bail;
-
-	qp->state = IB_QPS_ERR;
-
-	spin_lock(&dev->pending_lock);
-	if (!list_empty(&qp->timerwait))
-		list_del_init(&qp->timerwait);
-	if (!list_empty(&qp->piowait))
-		list_del_init(&qp->piowait);
-	spin_unlock(&dev->pending_lock);
-
-	/* Schedule the sending tasklet to drain the send work queue. */
-	if (qp->s_last != qp->s_head)
-		ipath_schedule_send(qp);
-
-	memset(&wc, 0, sizeof(wc));
-	wc.qp = &qp->ibqp;
-	wc.opcode = IB_WC_RECV;
-
-	if (test_and_clear_bit(IPATH_R_WRID_VALID, &qp->r_aflags)) {
-		wc.wr_id = qp->r_wr_id;
-		wc.status = err;
-		ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc, 1);
-	}
-	wc.status = IB_WC_WR_FLUSH_ERR;
-
-	if (qp->r_rq.wq) {
-		struct ipath_rwq *wq;
-		u32 head;
-		u32 tail;
-
-		spin_lock(&qp->r_rq.lock);
-
-		/* sanity check pointers before trusting them */
-		wq = qp->r_rq.wq;
-		head = wq->head;
-		if (head >= qp->r_rq.size)
-			head = 0;
-		tail = wq->tail;
-		if (tail >= qp->r_rq.size)
-			tail = 0;
-		while (tail != head) {
-			wc.wr_id = get_rwqe_ptr(&qp->r_rq, tail)->wr_id;
-			if (++tail >= qp->r_rq.size)
-				tail = 0;
-			ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc, 1);
-		}
-		wq->tail = tail;
-
-		spin_unlock(&qp->r_rq.lock);
-	} else if (qp->ibqp.event_handler)
-		ret = 1;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_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
- * @udata: user data for ipathverbs.so
- *
- * Returns 0 on success, otherwise returns an errno.
- */
-int ipath_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-		    int attr_mask, struct ib_udata *udata)
-{
-	struct ipath_ibdev *dev = to_idev(ibqp->device);
-	struct ipath_qp *qp = to_iqp(ibqp);
-	enum ib_qp_state cur_state, new_state;
-	int lastwqe = 0;
-	int ret;
-
-	spin_lock_irq(&qp->s_lock);
-
-	cur_state = attr_mask & IB_QP_CUR_STATE ?
-		attr->cur_qp_state : 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_UNSPECIFIED))
-		goto inval;
-
-	if (attr_mask & IB_QP_AV) {
-		if (attr->ah_attr.dlid == 0 ||
-		    attr->ah_attr.dlid >= IPATH_MULTICAST_LID_BASE)
-			goto inval;
-
-		if ((attr->ah_attr.ah_flags & IB_AH_GRH) &&
-		    (attr->ah_attr.grh.sgid_index > 1))
-			goto inval;
-	}
-
-	if (attr_mask & IB_QP_PKEY_INDEX)
-		if (attr->pkey_index >= ipath_get_npkeys(dev->dd))
-			goto inval;
-
-	if (attr_mask & IB_QP_MIN_RNR_TIMER)
-		if (attr->min_rnr_timer > 31)
-			goto inval;
-
-	if (attr_mask & IB_QP_PORT)
-		if (attr->port_num == 0 ||
-		    attr->port_num > ibqp->device->phys_port_cnt)
-			goto inval;
-
-	/*
-	 * don't allow invalid Path MTU values or greater than 2048
-	 * unless we are configured for a 4KB MTU
-	 */
-	if ((attr_mask & IB_QP_PATH_MTU) &&
-		(ib_mtu_enum_to_int(attr->path_mtu) == -1 ||
-		(attr->path_mtu > IB_MTU_2048 && !ipath_mtu4096)))
-		goto inval;
-
-	if (attr_mask & IB_QP_PATH_MIG_STATE)
-		if (attr->path_mig_state != IB_MIG_MIGRATED &&
-		    attr->path_mig_state != IB_MIG_REARM)
-			goto inval;
-
-	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)
-		if (attr->max_dest_rd_atomic > IPATH_MAX_RDMA_ATOMIC)
-			goto inval;
-
-	switch (new_state) {
-	case IB_QPS_RESET:
-		if (qp->state != IB_QPS_RESET) {
-			qp->state = IB_QPS_RESET;
-			spin_lock(&dev->pending_lock);
-			if (!list_empty(&qp->timerwait))
-				list_del_init(&qp->timerwait);
-			if (!list_empty(&qp->piowait))
-				list_del_init(&qp->piowait);
-			spin_unlock(&dev->pending_lock);
-			qp->s_flags &= ~IPATH_S_ANY_WAIT;
-			spin_unlock_irq(&qp->s_lock);
-			/* Stop the sending tasklet */
-			tasklet_kill(&qp->s_task);
-			wait_event(qp->wait_dma, !atomic_read(&qp->s_dma_busy));
-			spin_lock_irq(&qp->s_lock);
-		}
-		ipath_reset_qp(qp, ibqp->qp_type);
-		break;
-
-	case IB_QPS_SQD:
-		qp->s_draining = qp->s_last != qp->s_cur;
-		qp->state = new_state;
-		break;
-
-	case IB_QPS_SQE:
-		if (qp->ibqp.qp_type == IB_QPT_RC)
-			goto inval;
-		qp->state = new_state;
-		break;
-
-	case IB_QPS_ERR:
-		lastwqe = ipath_error_qp(qp, IB_WC_WR_FLUSH_ERR);
-		break;
-
-	default:
-		qp->state = new_state;
-		break;
-	}
-
-	if (attr_mask & IB_QP_PKEY_INDEX)
-		qp->s_pkey_index = attr->pkey_index;
-
-	if (attr_mask & IB_QP_DEST_QPN)
-		qp->remote_qpn = attr->dest_qp_num;
-
-	if (attr_mask & IB_QP_SQ_PSN) {
-		qp->s_psn = qp->s_next_psn = attr->sq_psn;
-		qp->s_last_psn = qp->s_next_psn - 1;
-	}
-
-	if (attr_mask & IB_QP_RQ_PSN)
-		qp->r_psn = attr->rq_psn;
-
-	if (attr_mask & IB_QP_ACCESS_FLAGS)
-		qp->qp_access_flags = attr->qp_access_flags;
-
-	if (attr_mask & IB_QP_AV) {
-		qp->remote_ah_attr = attr->ah_attr;
-		qp->s_dmult = ipath_ib_rate_to_mult(attr->ah_attr.static_rate);
-	}
-
-	if (attr_mask & IB_QP_PATH_MTU)
-		qp->path_mtu = attr->path_mtu;
-
-	if (attr_mask & IB_QP_RETRY_CNT)
-		qp->s_retry = qp->s_retry_cnt = attr->retry_cnt;
-
-	if (attr_mask & IB_QP_RNR_RETRY) {
-		qp->s_rnr_retry = attr->rnr_retry;
-		if (qp->s_rnr_retry > 7)
-			qp->s_rnr_retry = 7;
-		qp->s_rnr_retry_cnt = qp->s_rnr_retry;
-	}
-
-	if (attr_mask & IB_QP_MIN_RNR_TIMER)
-		qp->r_min_rnr_timer = attr->min_rnr_timer;
-
-	if (attr_mask & IB_QP_TIMEOUT)
-		qp->timeout = attr->timeout;
-
-	if (attr_mask & IB_QP_QKEY)
-		qp->qkey = attr->qkey;
-
-	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC)
-		qp->r_max_rd_atomic = attr->max_dest_rd_atomic;
-
-	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC)
-		qp->s_max_rd_atomic = attr->max_rd_atomic;
-
-	spin_unlock_irq(&qp->s_lock);
-
-	if (lastwqe) {
-		struct ib_event ev;
-
-		ev.device = qp->ibqp.device;
-		ev.element.qp = &qp->ibqp;
-		ev.event = IB_EVENT_QP_LAST_WQE_REACHED;
-		qp->ibqp.event_handler(&ev, qp->ibqp.qp_context);
-	}
-	ret = 0;
-	goto bail;
-
-inval:
-	spin_unlock_irq(&qp->s_lock);
-	ret = -EINVAL;
-
-bail:
-	return ret;
-}
-
-int ipath_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-		   int attr_mask, struct ib_qp_init_attr *init_attr)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-
-	attr->qp_state = qp->state;
-	attr->cur_qp_state = attr->qp_state;
-	attr->path_mtu = qp->path_mtu;
-	attr->path_mig_state = 0;
-	attr->qkey = qp->qkey;
-	attr->rq_psn = qp->r_psn;
-	attr->sq_psn = qp->s_next_psn;
-	attr->dest_qp_num = qp->remote_qpn;
-	attr->qp_access_flags = qp->qp_access_flags;
-	attr->cap.max_send_wr = qp->s_size - 1;
-	attr->cap.max_recv_wr = qp->ibqp.srq ? 0 : qp->r_rq.size - 1;
-	attr->cap.max_send_sge = qp->s_max_sge;
-	attr->cap.max_recv_sge = qp->r_rq.max_sge;
-	attr->cap.max_inline_data = 0;
-	attr->ah_attr = qp->remote_ah_attr;
-	memset(&attr->alt_ah_attr, 0, sizeof(attr->alt_ah_attr));
-	attr->pkey_index = qp->s_pkey_index;
-	attr->alt_pkey_index = 0;
-	attr->en_sqd_async_notify = 0;
-	attr->sq_draining = qp->s_draining;
-	attr->max_rd_atomic = qp->s_max_rd_atomic;
-	attr->max_dest_rd_atomic = qp->r_max_rd_atomic;
-	attr->min_rnr_timer = qp->r_min_rnr_timer;
-	attr->port_num = 1;
-	attr->timeout = qp->timeout;
-	attr->retry_cnt = qp->s_retry_cnt;
-	attr->rnr_retry = qp->s_rnr_retry_cnt;
-	attr->alt_port_num = 0;
-	attr->alt_timeout = 0;
-
-	init_attr->event_handler = qp->ibqp.event_handler;
-	init_attr->qp_context = qp->ibqp.qp_context;
-	init_attr->send_cq = qp->ibqp.send_cq;
-	init_attr->recv_cq = qp->ibqp.recv_cq;
-	init_attr->srq = qp->ibqp.srq;
-	init_attr->cap = attr->cap;
-	if (qp->s_flags & IPATH_S_SIGNAL_REQ_WR)
-		init_attr->sq_sig_type = IB_SIGNAL_REQ_WR;
-	else
-		init_attr->sq_sig_type = IB_SIGNAL_ALL_WR;
-	init_attr->qp_type = qp->ibqp.qp_type;
-	init_attr->port_num = 1;
-	return 0;
-}
-
-/**
- * ipath_compute_aeth - compute the AETH (syndrome + MSN)
- * @qp: the queue pair to compute the AETH for
- *
- * Returns the AETH.
- */
-__be32 ipath_compute_aeth(struct ipath_qp *qp)
-{
-	u32 aeth = qp->r_msn & IPATH_MSN_MASK;
-
-	if (qp->ibqp.srq) {
-		/*
-		 * Shared receive queues don't generate credits.
-		 * Set the credit field to the invalid value.
-		 */
-		aeth |= IPATH_AETH_CREDIT_INVAL << IPATH_AETH_CREDIT_SHIFT;
-	} else {
-		u32 min, max, x;
-		u32 credits;
-		struct ipath_rwq *wq = qp->r_rq.wq;
-		u32 head;
-		u32 tail;
-
-		/* sanity check pointers before trusting them */
-		head = wq->head;
-		if (head >= qp->r_rq.size)
-			head = 0;
-		tail = wq->tail;
-		if (tail >= qp->r_rq.size)
-			tail = 0;
-		/*
-		 * Compute the number of credits available (RWQEs).
-		 * XXX Not holding the r_rq.lock here so there is a small
-		 * chance that the pair of reads are not atomic.
-		 */
-		credits = head - tail;
-		if ((int)credits < 0)
-			credits += qp->r_rq.size;
-		/*
-		 * Binary search the credit table to find the code to
-		 * use.
-		 */
-		min = 0;
-		max = 31;
-		for (;;) {
-			x = (min + max) / 2;
-			if (credit_table[x] == credits)
-				break;
-			if (credit_table[x] > credits)
-				max = x;
-			else if (min == x)
-				break;
-			else
-				min = x;
-		}
-		aeth |= x << IPATH_AETH_CREDIT_SHIFT;
-	}
-	return cpu_to_be32(aeth);
-}
-
-/**
- * ipath_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
- * @udata: unused by InfiniPath
- *
- * Returns the queue pair on success, otherwise returns an errno.
- *
- * Called by the ib_create_qp() core verbs function.
- */
-struct ib_qp *ipath_create_qp(struct ib_pd *ibpd,
-			      struct ib_qp_init_attr *init_attr,
-			      struct ib_udata *udata)
-{
-	struct ipath_qp *qp;
-	int err;
-	struct ipath_swqe *swq = NULL;
-	struct ipath_ibdev *dev;
-	size_t sz;
-	size_t sg_list_sz;
-	struct ib_qp *ret;
-
-	if (init_attr->create_flags) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	if (init_attr->cap.max_send_sge > ib_ipath_max_sges ||
-	    init_attr->cap.max_send_wr > ib_ipath_max_qp_wrs) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	/* Check receive queue parameters if no SRQ is specified. */
-	if (!init_attr->srq) {
-		if (init_attr->cap.max_recv_sge > ib_ipath_max_sges ||
-		    init_attr->cap.max_recv_wr > ib_ipath_max_qp_wrs) {
-			ret = ERR_PTR(-EINVAL);
-			goto bail;
-		}
-		if (init_attr->cap.max_send_sge +
-		    init_attr->cap.max_send_wr +
-		    init_attr->cap.max_recv_sge +
-		    init_attr->cap.max_recv_wr == 0) {
-			ret = ERR_PTR(-EINVAL);
-			goto bail;
-		}
-	}
-
-	switch (init_attr->qp_type) {
-	case IB_QPT_UC:
-	case IB_QPT_RC:
-	case IB_QPT_UD:
-	case IB_QPT_SMI:
-	case IB_QPT_GSI:
-		sz = sizeof(struct ipath_sge) *
-			init_attr->cap.max_send_sge +
-			sizeof(struct ipath_swqe);
-		swq = vmalloc((init_attr->cap.max_send_wr + 1) * sz);
-		if (swq == NULL) {
-			ret = ERR_PTR(-ENOMEM);
-			goto bail;
-		}
-		sz = sizeof(*qp);
-		sg_list_sz = 0;
-		if (init_attr->srq) {
-			struct ipath_srq *srq = to_isrq(init_attr->srq);
-
-			if (srq->rq.max_sge > 1)
-				sg_list_sz = sizeof(*qp->r_sg_list) *
-					(srq->rq.max_sge - 1);
-		} else if (init_attr->cap.max_recv_sge > 1)
-			sg_list_sz = sizeof(*qp->r_sg_list) *
-				(init_attr->cap.max_recv_sge - 1);
-		qp = kmalloc(sz + sg_list_sz, GFP_KERNEL);
-		if (!qp) {
-			ret = ERR_PTR(-ENOMEM);
-			goto bail_swq;
-		}
-		if (sg_list_sz && (init_attr->qp_type == IB_QPT_UD ||
-		    init_attr->qp_type == IB_QPT_SMI ||
-		    init_attr->qp_type == IB_QPT_GSI)) {
-			qp->r_ud_sg_list = kmalloc(sg_list_sz, GFP_KERNEL);
-			if (!qp->r_ud_sg_list) {
-				ret = ERR_PTR(-ENOMEM);
-				goto bail_qp;
-			}
-		} else
-			qp->r_ud_sg_list = NULL;
-		if (init_attr->srq) {
-			sz = 0;
-			qp->r_rq.size = 0;
-			qp->r_rq.max_sge = 0;
-			qp->r_rq.wq = NULL;
-			init_attr->cap.max_recv_wr = 0;
-			init_attr->cap.max_recv_sge = 0;
-		} else {
-			qp->r_rq.size = init_attr->cap.max_recv_wr + 1;
-			qp->r_rq.max_sge = init_attr->cap.max_recv_sge;
-			sz = (sizeof(struct ib_sge) * qp->r_rq.max_sge) +
-				sizeof(struct ipath_rwqe);
-			qp->r_rq.wq = vmalloc_user(sizeof(struct ipath_rwq) +
-					      qp->r_rq.size * sz);
-			if (!qp->r_rq.wq) {
-				ret = ERR_PTR(-ENOMEM);
-				goto bail_sg_list;
-			}
-		}
-
-		/*
-		 * ib_create_qp() will initialize qp->ibqp
-		 * except for qp->ibqp.qp_num.
-		 */
-		spin_lock_init(&qp->s_lock);
-		spin_lock_init(&qp->r_rq.lock);
-		atomic_set(&qp->refcount, 0);
-		init_waitqueue_head(&qp->wait);
-		init_waitqueue_head(&qp->wait_dma);
-		tasklet_init(&qp->s_task, ipath_do_send, (unsigned long)qp);
-		INIT_LIST_HEAD(&qp->piowait);
-		INIT_LIST_HEAD(&qp->timerwait);
-		qp->state = IB_QPS_RESET;
-		qp->s_wq = swq;
-		qp->s_size = init_attr->cap.max_send_wr + 1;
-		qp->s_max_sge = init_attr->cap.max_send_sge;
-		if (init_attr->sq_sig_type == IB_SIGNAL_REQ_WR)
-			qp->s_flags = IPATH_S_SIGNAL_REQ_WR;
-		else
-			qp->s_flags = 0;
-		dev = to_idev(ibpd->device);
-		err = ipath_alloc_qpn(&dev->qp_table, qp,
-				      init_attr->qp_type);
-		if (err) {
-			ret = ERR_PTR(err);
-			vfree(qp->r_rq.wq);
-			goto bail_sg_list;
-		}
-		qp->ip = NULL;
-		qp->s_tx = NULL;
-		ipath_reset_qp(qp, init_attr->qp_type);
-		break;
-
-	default:
-		/* Don't support raw QPs */
-		ret = ERR_PTR(-ENOSYS);
-		goto bail;
-	}
-
-	init_attr->cap.max_inline_data = 0;
-
-	/*
-	 * Return the address of the RWQ as the offset to mmap.
-	 * See ipath_mmap() for details.
-	 */
-	if (udata && udata->outlen >= sizeof(__u64)) {
-		if (!qp->r_rq.wq) {
-			__u64 offset = 0;
-
-			err = ib_copy_to_udata(udata, &offset,
-					       sizeof(offset));
-			if (err) {
-				ret = ERR_PTR(err);
-				goto bail_ip;
-			}
-		} else {
-			u32 s = sizeof(struct ipath_rwq) +
-				qp->r_rq.size * sz;
-
-			qp->ip =
-			    ipath_create_mmap_info(dev, s,
-						   ibpd->uobject->context,
-						   qp->r_rq.wq);
-			if (!qp->ip) {
-				ret = ERR_PTR(-ENOMEM);
-				goto bail_ip;
-			}
-
-			err = ib_copy_to_udata(udata, &(qp->ip->offset),
-					       sizeof(qp->ip->offset));
-			if (err) {
-				ret = ERR_PTR(err);
-				goto bail_ip;
-			}
-		}
-	}
-
-	spin_lock(&dev->n_qps_lock);
-	if (dev->n_qps_allocated == ib_ipath_max_qps) {
-		spin_unlock(&dev->n_qps_lock);
-		ret = ERR_PTR(-ENOMEM);
-		goto bail_ip;
-	}
-
-	dev->n_qps_allocated++;
-	spin_unlock(&dev->n_qps_lock);
-
-	if (qp->ip) {
-		spin_lock_irq(&dev->pending_lock);
-		list_add(&qp->ip->pending_mmaps, &dev->pending_mmaps);
-		spin_unlock_irq(&dev->pending_lock);
-	}
-
-	ret = &qp->ibqp;
-	goto bail;
-
-bail_ip:
-	if (qp->ip)
-		kref_put(&qp->ip->ref, ipath_release_mmap_info);
-	else
-		vfree(qp->r_rq.wq);
-	ipath_free_qp(&dev->qp_table, qp);
-	free_qpn(&dev->qp_table, qp->ibqp.qp_num);
-bail_sg_list:
-	kfree(qp->r_ud_sg_list);
-bail_qp:
-	kfree(qp);
-bail_swq:
-	vfree(swq);
-bail:
-	return ret;
-}
-
-/**
- * ipath_destroy_qp - destroy a queue pair
- * @ibqp: the queue pair to destroy
- *
- * Returns 0 on success.
- *
- * Note that this can be called while the QP is actively sending or
- * receiving!
- */
-int ipath_destroy_qp(struct ib_qp *ibqp)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-	struct ipath_ibdev *dev = to_idev(ibqp->device);
-
-	/* Make sure HW and driver activity is stopped. */
-	spin_lock_irq(&qp->s_lock);
-	if (qp->state != IB_QPS_RESET) {
-		qp->state = IB_QPS_RESET;
-		spin_lock(&dev->pending_lock);
-		if (!list_empty(&qp->timerwait))
-			list_del_init(&qp->timerwait);
-		if (!list_empty(&qp->piowait))
-			list_del_init(&qp->piowait);
-		spin_unlock(&dev->pending_lock);
-		qp->s_flags &= ~IPATH_S_ANY_WAIT;
-		spin_unlock_irq(&qp->s_lock);
-		/* Stop the sending tasklet */
-		tasklet_kill(&qp->s_task);
-		wait_event(qp->wait_dma, !atomic_read(&qp->s_dma_busy));
-	} else
-		spin_unlock_irq(&qp->s_lock);
-
-	ipath_free_qp(&dev->qp_table, qp);
-
-	if (qp->s_tx) {
-		atomic_dec(&qp->refcount);
-		if (qp->s_tx->txreq.flags & IPATH_SDMA_TXREQ_F_FREEBUF)
-			kfree(qp->s_tx->txreq.map_addr);
-		spin_lock_irq(&dev->pending_lock);
-		list_add(&qp->s_tx->txreq.list, &dev->txreq_free);
-		spin_unlock_irq(&dev->pending_lock);
-		qp->s_tx = NULL;
-	}
-
-	wait_event(qp->wait, !atomic_read(&qp->refcount));
-
-	/* all user's cleaned up, mark it available */
-	free_qpn(&dev->qp_table, qp->ibqp.qp_num);
-	spin_lock(&dev->n_qps_lock);
-	dev->n_qps_allocated--;
-	spin_unlock(&dev->n_qps_lock);
-
-	if (qp->ip)
-		kref_put(&qp->ip->ref, ipath_release_mmap_info);
-	else
-		vfree(qp->r_rq.wq);
-	kfree(qp->r_ud_sg_list);
-	vfree(qp->s_wq);
-	kfree(qp);
-	return 0;
-}
-
-/**
- * ipath_init_qp_table - initialize the QP table for a device
- * @idev: the device who's QP table we're initializing
- * @size: the size of the QP table
- *
- * Returns 0 on success, otherwise returns an errno.
- */
-int ipath_init_qp_table(struct ipath_ibdev *idev, int size)
-{
-	int i;
-	int ret;
-
-	idev->qp_table.last = 1;	/* QPN 0 and 1 are special. */
-	idev->qp_table.max = size;
-	idev->qp_table.nmaps = 1;
-	idev->qp_table.table = kcalloc(size, sizeof(*idev->qp_table.table),
-				       GFP_KERNEL);
-	if (idev->qp_table.table == NULL) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	for (i = 0; i < ARRAY_SIZE(idev->qp_table.map); i++) {
-		atomic_set(&idev->qp_table.map[i].n_free, BITS_PER_PAGE);
-		idev->qp_table.map[i].page = NULL;
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_get_credit - flush the send work queue of a QP
- * @qp: the qp who's send work queue to flush
- * @aeth: the Acknowledge Extended Transport Header
- *
- * The QP s_lock should be held.
- */
-void ipath_get_credit(struct ipath_qp *qp, u32 aeth)
-{
-	u32 credit = (aeth >> IPATH_AETH_CREDIT_SHIFT) & IPATH_AETH_CREDIT_MASK;
-
-	/*
-	 * If the credit is invalid, we can send
-	 * as many packets as we like.  Otherwise, we have to
-	 * honor the credit field.
-	 */
-	if (credit == IPATH_AETH_CREDIT_INVAL)
-		qp->s_lsn = (u32) -1;
-	else if (qp->s_lsn != (u32) -1) {
-		/* Compute new LSN (i.e., MSN + credit) */
-		credit = (aeth + credit_table[credit]) & IPATH_MSN_MASK;
-		if (ipath_cmp24(credit, qp->s_lsn) > 0)
-			qp->s_lsn = credit;
-	}
-
-	/* Restart sending if it was blocked due to lack of credits. */
-	if ((qp->s_flags & IPATH_S_WAIT_SSN_CREDIT) &&
-	    qp->s_cur != qp->s_head &&
-	    (qp->s_lsn == (u32) -1 ||
-	     ipath_cmp24(get_swqe_ptr(qp, qp->s_cur)->ssn,
-			 qp->s_lsn + 1) <= 0))
-		ipath_schedule_send(qp);
-}
diff --git a/drivers/staging/rdma/ipath/ipath_rc.c b/drivers/staging/rdma/ipath/ipath_rc.c
deleted file mode 100644
index d4aa53574e57..000000000000
--- a/drivers/staging/rdma/ipath/ipath_rc.c
+++ /dev/null
@@ -1,1969 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/io.h>
-
-#include "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-/* cut down ridiculously long IB macro names */
-#define OP(x) IB_OPCODE_RC_##x
-
-static u32 restart_sge(struct ipath_sge_state *ss, struct ipath_swqe *wqe,
-		       u32 psn, u32 pmtu)
-{
-	u32 len;
-
-	len = ((psn - wqe->psn) & IPATH_PSN_MASK) * pmtu;
-	ss->sge = wqe->sg_list[0];
-	ss->sg_list = wqe->sg_list + 1;
-	ss->num_sge = wqe->wr.num_sge;
-	ipath_skip_sge(ss, len);
-	return wqe->length - len;
-}
-
-/**
- * ipath_init_restart- initialize the qp->s_sge after a restart
- * @qp: the QP who's SGE we're restarting
- * @wqe: the work queue to initialize the QP's SGE from
- *
- * The QP s_lock should be held and interrupts disabled.
- */
-static void ipath_init_restart(struct ipath_qp *qp, struct ipath_swqe *wqe)
-{
-	struct ipath_ibdev *dev;
-
-	qp->s_len = restart_sge(&qp->s_sge, wqe, qp->s_psn,
-				ib_mtu_enum_to_int(qp->path_mtu));
-	dev = to_idev(qp->ibqp.device);
-	spin_lock(&dev->pending_lock);
-	if (list_empty(&qp->timerwait))
-		list_add_tail(&qp->timerwait,
-			      &dev->pending[dev->pending_index]);
-	spin_unlock(&dev->pending_lock);
-}
-
-/**
- * ipath_make_rc_ack - construct a response packet (ACK, NAK, or RDMA read)
- * @qp: a pointer to the QP
- * @ohdr: a pointer to the IB header being constructed
- * @pmtu: the path MTU
- *
- * Return 1 if constructed; otherwise, return 0.
- * Note that we are in the responder's side of the QP context.
- * Note the QP s_lock must be held.
- */
-static int ipath_make_rc_ack(struct ipath_ibdev *dev, struct ipath_qp *qp,
-			     struct ipath_other_headers *ohdr, u32 pmtu)
-{
-	struct ipath_ack_entry *e;
-	u32 hwords;
-	u32 len;
-	u32 bth0;
-	u32 bth2;
-
-	/* Don't send an ACK if we aren't supposed to. */
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK))
-		goto bail;
-
-	/* header size in 32-bit words LRH+BTH = (8+12)/4. */
-	hwords = 5;
-
-	switch (qp->s_ack_state) {
-	case OP(RDMA_READ_RESPONSE_LAST):
-	case OP(RDMA_READ_RESPONSE_ONLY):
-	case OP(ATOMIC_ACKNOWLEDGE):
-		/*
-		 * We can increment the tail pointer now that the last
-		 * response has been sent instead of only being
-		 * constructed.
-		 */
-		if (++qp->s_tail_ack_queue > IPATH_MAX_RDMA_ATOMIC)
-			qp->s_tail_ack_queue = 0;
-		/* FALLTHROUGH */
-	case OP(SEND_ONLY):
-	case OP(ACKNOWLEDGE):
-		/* Check for no next entry in the queue. */
-		if (qp->r_head_ack_queue == qp->s_tail_ack_queue) {
-			if (qp->s_flags & IPATH_S_ACK_PENDING)
-				goto normal;
-			qp->s_ack_state = OP(ACKNOWLEDGE);
-			goto bail;
-		}
-
-		e = &qp->s_ack_queue[qp->s_tail_ack_queue];
-		if (e->opcode == OP(RDMA_READ_REQUEST)) {
-			/* Copy SGE state in case we need to resend */
-			qp->s_ack_rdma_sge = e->rdma_sge;
-			qp->s_cur_sge = &qp->s_ack_rdma_sge;
-			len = e->rdma_sge.sge.sge_length;
-			if (len > pmtu) {
-				len = pmtu;
-				qp->s_ack_state = OP(RDMA_READ_RESPONSE_FIRST);
-			} else {
-				qp->s_ack_state = OP(RDMA_READ_RESPONSE_ONLY);
-				e->sent = 1;
-			}
-			ohdr->u.aeth = ipath_compute_aeth(qp);
-			hwords++;
-			qp->s_ack_rdma_psn = e->psn;
-			bth2 = qp->s_ack_rdma_psn++ & IPATH_PSN_MASK;
-		} else {
-			/* COMPARE_SWAP or FETCH_ADD */
-			qp->s_cur_sge = NULL;
-			len = 0;
-			qp->s_ack_state = OP(ATOMIC_ACKNOWLEDGE);
-			ohdr->u.at.aeth = ipath_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);
-			hwords += sizeof(ohdr->u.at) / sizeof(u32);
-			bth2 = e->psn;
-			e->sent = 1;
-		}
-		bth0 = qp->s_ack_state << 24;
-		break;
-
-	case OP(RDMA_READ_RESPONSE_FIRST):
-		qp->s_ack_state = OP(RDMA_READ_RESPONSE_MIDDLE);
-		/* FALLTHROUGH */
-	case OP(RDMA_READ_RESPONSE_MIDDLE):
-		len = qp->s_ack_rdma_sge.sge.sge_length;
-		if (len > pmtu)
-			len = pmtu;
-		else {
-			ohdr->u.aeth = ipath_compute_aeth(qp);
-			hwords++;
-			qp->s_ack_state = OP(RDMA_READ_RESPONSE_LAST);
-			qp->s_ack_queue[qp->s_tail_ack_queue].sent = 1;
-		}
-		bth0 = qp->s_ack_state << 24;
-		bth2 = qp->s_ack_rdma_psn++ & IPATH_PSN_MASK;
-		break;
-
-	default:
-	normal:
-		/*
-		 * Send a regular ACK.
-		 * Set the s_ack_state so we wait until after sending
-		 * the ACK before setting s_ack_state to ACKNOWLEDGE
-		 * (see above).
-		 */
-		qp->s_ack_state = OP(SEND_ONLY);
-		qp->s_flags &= ~IPATH_S_ACK_PENDING;
-		qp->s_cur_sge = NULL;
-		if (qp->s_nak_state)
-			ohdr->u.aeth =
-				cpu_to_be32((qp->r_msn & IPATH_MSN_MASK) |
-					    (qp->s_nak_state <<
-					     IPATH_AETH_CREDIT_SHIFT));
-		else
-			ohdr->u.aeth = ipath_compute_aeth(qp);
-		hwords++;
-		len = 0;
-		bth0 = OP(ACKNOWLEDGE) << 24;
-		bth2 = qp->s_ack_psn & IPATH_PSN_MASK;
-	}
-	qp->s_hdrwords = hwords;
-	qp->s_cur_size = len;
-	ipath_make_ruc_header(dev, qp, ohdr, bth0, bth2);
-	return 1;
-
-bail:
-	return 0;
-}
-
-/**
- * ipath_make_rc_req - construct a request packet (SEND, RDMA r/w, ATOMIC)
- * @qp: a pointer to the QP
- *
- * Return 1 if constructed; otherwise, return 0.
- */
-int ipath_make_rc_req(struct ipath_qp *qp)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ipath_other_headers *ohdr;
-	struct ipath_sge_state *ss;
-	struct ipath_swqe *wqe;
-	u32 hwords;
-	u32 len;
-	u32 bth0;
-	u32 bth2;
-	u32 pmtu = ib_mtu_enum_to_int(qp->path_mtu);
-	char newreq;
-	unsigned long flags;
-	int ret = 0;
-
-	ohdr = &qp->s_hdr.u.oth;
-	if (qp->remote_ah_attr.ah_flags & IB_AH_GRH)
-		ohdr = &qp->s_hdr.u.l.oth;
-
-	/*
-	 * The lock is needed to synchronize between the sending tasklet,
-	 * the receive interrupt handler, and timeout resends.
-	 */
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	/* Sending responses has higher priority over sending requests. */
-	if ((qp->r_head_ack_queue != qp->s_tail_ack_queue ||
-	     (qp->s_flags & IPATH_S_ACK_PENDING) ||
-	     qp->s_ack_state != OP(ACKNOWLEDGE)) &&
-	    ipath_make_rc_ack(dev, qp, ohdr, pmtu))
-		goto done;
-
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK)) {
-		if (!(ib_ipath_state_ops[qp->state] & IPATH_FLUSH_SEND))
-			goto bail;
-		/* We are in the error state, flush the work request. */
-		if (qp->s_last == qp->s_head)
-			goto bail;
-		/* If DMAs are in progress, we can't flush immediately. */
-		if (atomic_read(&qp->s_dma_busy)) {
-			qp->s_flags |= IPATH_S_WAIT_DMA;
-			goto bail;
-		}
-		wqe = get_swqe_ptr(qp, qp->s_last);
-		ipath_send_complete(qp, wqe, IB_WC_WR_FLUSH_ERR);
-		goto done;
-	}
-
-	/* Leave BUSY set until RNR timeout. */
-	if (qp->s_rnr_timeout) {
-		qp->s_flags |= IPATH_S_WAITING;
-		goto bail;
-	}
-
-	/* header size in 32-bit words LRH+BTH = (8+12)/4. */
-	hwords = 5;
-	bth0 = 1 << 22; /* Set M bit */
-
-	/* Send a request. */
-	wqe = get_swqe_ptr(qp, qp->s_cur);
-	switch (qp->s_state) {
-	default:
-		if (!(ib_ipath_state_ops[qp->state] &
-		    IPATH_PROCESS_NEXT_SEND_OK))
-			goto bail;
-		/*
-		 * Resend an old request or start a new one.
-		 *
-		 * We keep track of the current SWQE so that
-		 * we don't reset the "furthest progress" state
-		 * if we need to back up.
-		 */
-		newreq = 0;
-		if (qp->s_cur == qp->s_tail) {
-			/* Check if send work queue is empty. */
-			if (qp->s_tail == qp->s_head)
-				goto bail;
-			/*
-			 * If a fence is requested, wait for previous
-			 * RDMA read and atomic operations to finish.
-			 */
-			if ((wqe->wr.send_flags & IB_SEND_FENCE) &&
-			    qp->s_num_rd_atomic) {
-				qp->s_flags |= IPATH_S_FENCE_PENDING;
-				goto bail;
-			}
-			wqe->psn = qp->s_next_psn;
-			newreq = 1;
-		}
-		/*
-		 * Note that we have to be careful not to modify the
-		 * original work request since we may need to resend
-		 * it.
-		 */
-		len = wqe->length;
-		ss = &qp->s_sge;
-		bth2 = 0;
-		switch (wqe->wr.opcode) {
-		case IB_WR_SEND:
-		case IB_WR_SEND_WITH_IMM:
-			/* If no credit, return. */
-			if (qp->s_lsn != (u32) -1 &&
-			    ipath_cmp24(wqe->ssn, qp->s_lsn + 1) > 0) {
-				qp->s_flags |= IPATH_S_WAIT_SSN_CREDIT;
-				goto bail;
-			}
-			wqe->lpsn = wqe->psn;
-			if (len > pmtu) {
-				wqe->lpsn += (len - 1) / pmtu;
-				qp->s_state = OP(SEND_FIRST);
-				len = pmtu;
-				break;
-			}
-			if (wqe->wr.opcode == IB_WR_SEND)
-				qp->s_state = OP(SEND_ONLY);
-			else {
-				qp->s_state = OP(SEND_ONLY_WITH_IMMEDIATE);
-				/* Immediate data comes after the BTH */
-				ohdr->u.imm_data = wqe->wr.ex.imm_data;
-				hwords += 1;
-			}
-			if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-				bth0 |= 1 << 23;
-			bth2 = 1 << 31;	/* Request ACK. */
-			if (++qp->s_cur == qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		case IB_WR_RDMA_WRITE:
-			if (newreq && qp->s_lsn != (u32) -1)
-				qp->s_lsn++;
-			/* FALLTHROUGH */
-		case IB_WR_RDMA_WRITE_WITH_IMM:
-			/* If no credit, return. */
-			if (qp->s_lsn != (u32) -1 &&
-			    ipath_cmp24(wqe->ssn, qp->s_lsn + 1) > 0) {
-				qp->s_flags |= IPATH_S_WAIT_SSN_CREDIT;
-				goto bail;
-			}
-			ohdr->u.rc.reth.vaddr =
-				cpu_to_be64(wqe->rdma_wr.remote_addr);
-			ohdr->u.rc.reth.rkey =
-				cpu_to_be32(wqe->rdma_wr.rkey);
-			ohdr->u.rc.reth.length = cpu_to_be32(len);
-			hwords += sizeof(struct ib_reth) / sizeof(u32);
-			wqe->lpsn = wqe->psn;
-			if (len > pmtu) {
-				wqe->lpsn += (len - 1) / pmtu;
-				qp->s_state = OP(RDMA_WRITE_FIRST);
-				len = pmtu;
-				break;
-			}
-			if (wqe->wr.opcode == IB_WR_RDMA_WRITE)
-				qp->s_state = OP(RDMA_WRITE_ONLY);
-			else {
-				qp->s_state =
-					OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE);
-				/* Immediate data comes after RETH */
-				ohdr->u.rc.imm_data = wqe->wr.ex.imm_data;
-				hwords += 1;
-				if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-					bth0 |= 1 << 23;
-			}
-			bth2 = 1 << 31;	/* Request ACK. */
-			if (++qp->s_cur == qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		case IB_WR_RDMA_READ:
-			/*
-			 * Don't allow more operations to be started
-			 * than the QP limits allow.
-			 */
-			if (newreq) {
-				if (qp->s_num_rd_atomic >=
-				    qp->s_max_rd_atomic) {
-					qp->s_flags |= IPATH_S_RDMAR_PENDING;
-					goto bail;
-				}
-				qp->s_num_rd_atomic++;
-				if (qp->s_lsn != (u32) -1)
-					qp->s_lsn++;
-				/*
-				 * Adjust s_next_psn to count the
-				 * expected number of responses.
-				 */
-				if (len > pmtu)
-					qp->s_next_psn += (len - 1) / pmtu;
-				wqe->lpsn = qp->s_next_psn++;
-			}
-			ohdr->u.rc.reth.vaddr =
-				cpu_to_be64(wqe->rdma_wr.remote_addr);
-			ohdr->u.rc.reth.rkey =
-				cpu_to_be32(wqe->rdma_wr.rkey);
-			ohdr->u.rc.reth.length = cpu_to_be32(len);
-			qp->s_state = OP(RDMA_READ_REQUEST);
-			hwords += sizeof(ohdr->u.rc.reth) / sizeof(u32);
-			ss = NULL;
-			len = 0;
-			if (++qp->s_cur == qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		case IB_WR_ATOMIC_CMP_AND_SWP:
-		case IB_WR_ATOMIC_FETCH_AND_ADD:
-			/*
-			 * Don't allow more operations to be started
-			 * than the QP limits allow.
-			 */
-			if (newreq) {
-				if (qp->s_num_rd_atomic >=
-				    qp->s_max_rd_atomic) {
-					qp->s_flags |= IPATH_S_RDMAR_PENDING;
-					goto bail;
-				}
-				qp->s_num_rd_atomic++;
-				if (qp->s_lsn != (u32) -1)
-					qp->s_lsn++;
-				wqe->lpsn = wqe->psn;
-			}
-			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);
-			} 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;
-			}
-			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);
-			ohdr->u.atomic_eth.rkey = cpu_to_be32(
-				wqe->atomic_wr.rkey);
-			hwords += sizeof(struct ib_atomic_eth) / sizeof(u32);
-			ss = NULL;
-			len = 0;
-			if (++qp->s_cur == qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		default:
-			goto bail;
-		}
-		qp->s_sge.sge = wqe->sg_list[0];
-		qp->s_sge.sg_list = wqe->sg_list + 1;
-		qp->s_sge.num_sge = wqe->wr.num_sge;
-		qp->s_len = wqe->length;
-		if (newreq) {
-			qp->s_tail++;
-			if (qp->s_tail >= qp->s_size)
-				qp->s_tail = 0;
-		}
-		bth2 |= qp->s_psn & IPATH_PSN_MASK;
-		if (wqe->wr.opcode == IB_WR_RDMA_READ)
-			qp->s_psn = wqe->lpsn + 1;
-		else {
-			qp->s_psn++;
-			if (ipath_cmp24(qp->s_psn, qp->s_next_psn) > 0)
-				qp->s_next_psn = qp->s_psn;
-		}
-		/*
-		 * Put the QP on the pending list so lost ACKs will cause
-		 * a retry.  More than one request can be pending so the
-		 * QP may already be on the dev->pending list.
-		 */
-		spin_lock(&dev->pending_lock);
-		if (list_empty(&qp->timerwait))
-			list_add_tail(&qp->timerwait,
-				      &dev->pending[dev->pending_index]);
-		spin_unlock(&dev->pending_lock);
-		break;
-
-	case OP(RDMA_READ_RESPONSE_FIRST):
-		/*
-		 * This case can only happen if a send is restarted.
-		 * See ipath_restart_rc().
-		 */
-		ipath_init_restart(qp, wqe);
-		/* FALLTHROUGH */
-	case OP(SEND_FIRST):
-		qp->s_state = OP(SEND_MIDDLE);
-		/* FALLTHROUGH */
-	case OP(SEND_MIDDLE):
-		bth2 = qp->s_psn++ & IPATH_PSN_MASK;
-		if (ipath_cmp24(qp->s_psn, qp->s_next_psn) > 0)
-			qp->s_next_psn = qp->s_psn;
-		ss = &qp->s_sge;
-		len = qp->s_len;
-		if (len > pmtu) {
-			len = pmtu;
-			break;
-		}
-		if (wqe->wr.opcode == IB_WR_SEND)
-			qp->s_state = OP(SEND_LAST);
-		else {
-			qp->s_state = OP(SEND_LAST_WITH_IMMEDIATE);
-			/* Immediate data comes after the BTH */
-			ohdr->u.imm_data = wqe->wr.ex.imm_data;
-			hwords += 1;
-		}
-		if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-			bth0 |= 1 << 23;
-		bth2 |= 1 << 31;	/* Request ACK. */
-		qp->s_cur++;
-		if (qp->s_cur >= qp->s_size)
-			qp->s_cur = 0;
-		break;
-
-	case OP(RDMA_READ_RESPONSE_LAST):
-		/*
-		 * This case can only happen if a RDMA write is restarted.
-		 * See ipath_restart_rc().
-		 */
-		ipath_init_restart(qp, wqe);
-		/* FALLTHROUGH */
-	case OP(RDMA_WRITE_FIRST):
-		qp->s_state = OP(RDMA_WRITE_MIDDLE);
-		/* FALLTHROUGH */
-	case OP(RDMA_WRITE_MIDDLE):
-		bth2 = qp->s_psn++ & IPATH_PSN_MASK;
-		if (ipath_cmp24(qp->s_psn, qp->s_next_psn) > 0)
-			qp->s_next_psn = qp->s_psn;
-		ss = &qp->s_sge;
-		len = qp->s_len;
-		if (len > pmtu) {
-			len = pmtu;
-			break;
-		}
-		if (wqe->wr.opcode == IB_WR_RDMA_WRITE)
-			qp->s_state = OP(RDMA_WRITE_LAST);
-		else {
-			qp->s_state = OP(RDMA_WRITE_LAST_WITH_IMMEDIATE);
-			/* Immediate data comes after the BTH */
-			ohdr->u.imm_data = wqe->wr.ex.imm_data;
-			hwords += 1;
-			if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-				bth0 |= 1 << 23;
-		}
-		bth2 |= 1 << 31;	/* Request ACK. */
-		qp->s_cur++;
-		if (qp->s_cur >= qp->s_size)
-			qp->s_cur = 0;
-		break;
-
-	case OP(RDMA_READ_RESPONSE_MIDDLE):
-		/*
-		 * This case can only happen if a RDMA read is restarted.
-		 * See ipath_restart_rc().
-		 */
-		ipath_init_restart(qp, wqe);
-		len = ((qp->s_psn - wqe->psn) & IPATH_PSN_MASK) * pmtu;
-		ohdr->u.rc.reth.vaddr =
-			cpu_to_be64(wqe->rdma_wr.remote_addr + len);
-		ohdr->u.rc.reth.rkey =
-			cpu_to_be32(wqe->rdma_wr.rkey);
-		ohdr->u.rc.reth.length = cpu_to_be32(qp->s_len);
-		qp->s_state = OP(RDMA_READ_REQUEST);
-		hwords += sizeof(ohdr->u.rc.reth) / sizeof(u32);
-		bth2 = qp->s_psn & IPATH_PSN_MASK;
-		qp->s_psn = wqe->lpsn + 1;
-		ss = NULL;
-		len = 0;
-		qp->s_cur++;
-		if (qp->s_cur == qp->s_size)
-			qp->s_cur = 0;
-		break;
-	}
-	if (ipath_cmp24(qp->s_psn, qp->s_last_psn + IPATH_PSN_CREDIT - 1) >= 0)
-		bth2 |= 1 << 31;	/* Request ACK. */
-	qp->s_len -= len;
-	qp->s_hdrwords = hwords;
-	qp->s_cur_sge = ss;
-	qp->s_cur_size = len;
-	ipath_make_ruc_header(dev, qp, ohdr, bth0 | (qp->s_state << 24), bth2);
-done:
-	ret = 1;
-	goto unlock;
-
-bail:
-	qp->s_flags &= ~IPATH_S_BUSY;
-unlock:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-	return ret;
-}
-
-/**
- * send_rc_ack - Construct an ACK packet and send it
- * @qp: a pointer to the QP
- *
- * This is called from ipath_rc_rcv() and only uses the receive
- * side QP state.
- * Note that RDMA reads and atomics are handled in the
- * send side QP state and tasklet.
- */
-static void send_rc_ack(struct ipath_qp *qp)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ipath_devdata *dd;
-	u16 lrh0;
-	u32 bth0;
-	u32 hwords;
-	u32 __iomem *piobuf;
-	struct ipath_ib_header hdr;
-	struct ipath_other_headers *ohdr;
-	unsigned long flags;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	/* Don't send ACK or NAK if a RDMA read or atomic is pending. */
-	if (qp->r_head_ack_queue != qp->s_tail_ack_queue ||
-	    (qp->s_flags & IPATH_S_ACK_PENDING) ||
-	    qp->s_ack_state != OP(ACKNOWLEDGE))
-		goto queue_ack;
-
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-
-	/* Don't try to send ACKs if the link isn't ACTIVE */
-	dd = dev->dd;
-	if (!(dd->ipath_flags & IPATH_LINKACTIVE))
-		goto done;
-
-	piobuf = ipath_getpiobuf(dd, 0, NULL);
-	if (!piobuf) {
-		/*
-		 * We are out of PIO buffers at the moment.
-		 * Pass responsibility for sending the ACK to the
-		 * send tasklet so that when a PIO buffer becomes
-		 * available, the ACK is sent ahead of other outgoing
-		 * packets.
-		 */
-		spin_lock_irqsave(&qp->s_lock, flags);
-		goto queue_ack;
-	}
-
-	/* Construct the header. */
-	ohdr = &hdr.u.oth;
-	lrh0 = IPATH_LRH_BTH;
-	/* header size in 32-bit words LRH+BTH+AETH = (8+12+4)/4. */
-	hwords = 6;
-	if (unlikely(qp->remote_ah_attr.ah_flags & IB_AH_GRH)) {
-		hwords += ipath_make_grh(dev, &hdr.u.l.grh,
-					 &qp->remote_ah_attr.grh,
-					 hwords, 0);
-		ohdr = &hdr.u.l.oth;
-		lrh0 = IPATH_LRH_GRH;
-	}
-	/* read pkey_index w/o lock (its atomic) */
-	bth0 = ipath_get_pkey(dd, qp->s_pkey_index) |
-		(OP(ACKNOWLEDGE) << 24) | (1 << 22);
-	if (qp->r_nak_state)
-		ohdr->u.aeth = cpu_to_be32((qp->r_msn & IPATH_MSN_MASK) |
-					    (qp->r_nak_state <<
-					     IPATH_AETH_CREDIT_SHIFT));
-	else
-		ohdr->u.aeth = ipath_compute_aeth(qp);
-	lrh0 |= qp->remote_ah_attr.sl << 4;
-	hdr.lrh[0] = cpu_to_be16(lrh0);
-	hdr.lrh[1] = cpu_to_be16(qp->remote_ah_attr.dlid);
-	hdr.lrh[2] = cpu_to_be16(hwords + SIZE_OF_CRC);
-	hdr.lrh[3] = cpu_to_be16(dd->ipath_lid |
-				 qp->remote_ah_attr.src_path_bits);
-	ohdr->bth[0] = cpu_to_be32(bth0);
-	ohdr->bth[1] = cpu_to_be32(qp->remote_qpn);
-	ohdr->bth[2] = cpu_to_be32(qp->r_ack_psn & IPATH_PSN_MASK);
-
-	writeq(hwords + 1, piobuf);
-
-	if (dd->ipath_flags & IPATH_PIO_FLUSH_WC) {
-		u32 *hdrp = (u32 *) &hdr;
-
-		ipath_flush_wc();
-		__iowrite32_copy(piobuf + 2, hdrp, hwords - 1);
-		ipath_flush_wc();
-		__raw_writel(hdrp[hwords - 1], piobuf + hwords + 1);
-	} else
-		__iowrite32_copy(piobuf + 2, (u32 *) &hdr, hwords);
-
-	ipath_flush_wc();
-
-	dev->n_unicast_xmit++;
-	goto done;
-
-queue_ack:
-	if (ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK) {
-		dev->n_rc_qacks++;
-		qp->s_flags |= IPATH_S_ACK_PENDING;
-		qp->s_nak_state = qp->r_nak_state;
-		qp->s_ack_psn = qp->r_ack_psn;
-
-		/* Schedule the send tasklet. */
-		ipath_schedule_send(qp);
-	}
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-done:
-	return;
-}
-
-/**
- * reset_psn - reset the QP state to send starting from PSN
- * @qp: the QP
- * @psn: the packet sequence number to restart at
- *
- * This is called from ipath_rc_rcv() to process an incoming RC ACK
- * for the given QP.
- * Called at interrupt level with the QP s_lock held.
- */
-static void reset_psn(struct ipath_qp *qp, u32 psn)
-{
-	u32 n = qp->s_last;
-	struct ipath_swqe *wqe = get_swqe_ptr(qp, n);
-	u32 opcode;
-
-	qp->s_cur = n;
-
-	/*
-	 * If we are starting the request from the beginning,
-	 * let the normal send code handle initialization.
-	 */
-	if (ipath_cmp24(psn, wqe->psn) <= 0) {
-		qp->s_state = OP(SEND_LAST);
-		goto done;
-	}
-
-	/* Find the work request opcode corresponding to the given PSN. */
-	opcode = wqe->wr.opcode;
-	for (;;) {
-		int diff;
-
-		if (++n == qp->s_size)
-			n = 0;
-		if (n == qp->s_tail)
-			break;
-		wqe = get_swqe_ptr(qp, n);
-		diff = ipath_cmp24(psn, wqe->psn);
-		if (diff < 0)
-			break;
-		qp->s_cur = n;
-		/*
-		 * If we are starting the request from the beginning,
-		 * let the normal send code handle initialization.
-		 */
-		if (diff == 0) {
-			qp->s_state = OP(SEND_LAST);
-			goto done;
-		}
-		opcode = wqe->wr.opcode;
-	}
-
-	/*
-	 * Set the state to restart in the middle of a request.
-	 * Don't change the s_sge, s_cur_sge, or s_cur_size.
-	 * See ipath_make_rc_req().
-	 */
-	switch (opcode) {
-	case IB_WR_SEND:
-	case IB_WR_SEND_WITH_IMM:
-		qp->s_state = OP(RDMA_READ_RESPONSE_FIRST);
-		break;
-
-	case IB_WR_RDMA_WRITE:
-	case IB_WR_RDMA_WRITE_WITH_IMM:
-		qp->s_state = OP(RDMA_READ_RESPONSE_LAST);
-		break;
-
-	case IB_WR_RDMA_READ:
-		qp->s_state = OP(RDMA_READ_RESPONSE_MIDDLE);
-		break;
-
-	default:
-		/*
-		 * This case shouldn't happen since its only
-		 * one PSN per req.
-		 */
-		qp->s_state = OP(SEND_LAST);
-	}
-done:
-	qp->s_psn = psn;
-}
-
-/**
- * ipath_restart_rc - back up requester to resend the last un-ACKed request
- * @qp: the QP to restart
- * @psn: packet sequence number for the request
- * @wc: the work completion request
- *
- * The QP s_lock should be held and interrupts disabled.
- */
-void ipath_restart_rc(struct ipath_qp *qp, u32 psn)
-{
-	struct ipath_swqe *wqe = get_swqe_ptr(qp, qp->s_last);
-	struct ipath_ibdev *dev;
-
-	if (qp->s_retry == 0) {
-		ipath_send_complete(qp, wqe, IB_WC_RETRY_EXC_ERR);
-		ipath_error_qp(qp, IB_WC_WR_FLUSH_ERR);
-		goto bail;
-	}
-	qp->s_retry--;
-
-	/*
-	 * Remove the QP from the timeout queue.
-	 * Note: it may already have been removed by ipath_ib_timer().
-	 */
-	dev = to_idev(qp->ibqp.device);
-	spin_lock(&dev->pending_lock);
-	if (!list_empty(&qp->timerwait))
-		list_del_init(&qp->timerwait);
-	if (!list_empty(&qp->piowait))
-		list_del_init(&qp->piowait);
-	spin_unlock(&dev->pending_lock);
-
-	if (wqe->wr.opcode == IB_WR_RDMA_READ)
-		dev->n_rc_resends++;
-	else
-		dev->n_rc_resends += (qp->s_psn - psn) & IPATH_PSN_MASK;
-
-	reset_psn(qp, psn);
-	ipath_schedule_send(qp);
-
-bail:
-	return;
-}
-
-static inline void update_last_psn(struct ipath_qp *qp, u32 psn)
-{
-	qp->s_last_psn = psn;
-}
-
-/**
- * do_rc_ack - process an incoming RC ACK
- * @qp: the QP the ACK came in on
- * @psn: the packet sequence number of the ACK
- * @opcode: the opcode of the request that resulted in the ACK
- *
- * This is called from ipath_rc_rcv_resp() to process an incoming RC ACK
- * for the given QP.
- * Called at interrupt level with the QP s_lock held and interrupts disabled.
- * Returns 1 if OK, 0 if current operation should be aborted (NAK).
- */
-static int do_rc_ack(struct ipath_qp *qp, u32 aeth, u32 psn, int opcode,
-		     u64 val)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ib_wc wc;
-	enum ib_wc_status status;
-	struct ipath_swqe *wqe;
-	int ret = 0;
-	u32 ack_psn;
-	int diff;
-
-	/*
-	 * Remove the QP from the timeout queue (or RNR timeout queue).
-	 * If ipath_ib_timer() has already removed it,
-	 * it's OK since we hold the QP s_lock and ipath_restart_rc()
-	 * just won't find anything to restart if we ACK everything.
-	 */
-	spin_lock(&dev->pending_lock);
-	if (!list_empty(&qp->timerwait))
-		list_del_init(&qp->timerwait);
-	spin_unlock(&dev->pending_lock);
-
-	/*
-	 * Note that NAKs implicitly ACK outstanding SEND and RDMA write
-	 * requests and implicitly NAK RDMA read and atomic requests issued
-	 * before the NAK'ed request.  The MSN won't include the NAK'ed
-	 * request but will include an ACK'ed request(s).
-	 */
-	ack_psn = psn;
-	if (aeth >> 29)
-		ack_psn--;
-	wqe = get_swqe_ptr(qp, qp->s_last);
-
-	/*
-	 * The MSN might be for a later WQE than the PSN indicates so
-	 * only complete WQEs that the PSN finishes.
-	 */
-	while ((diff = ipath_cmp24(ack_psn, wqe->lpsn)) >= 0) {
-		/*
-		 * RDMA_READ_RESPONSE_ONLY is a special case since
-		 * we want to generate completion events for everything
-		 * before the RDMA read, copy the data, then generate
-		 * the completion for the read.
-		 */
-		if (wqe->wr.opcode == IB_WR_RDMA_READ &&
-		    opcode == OP(RDMA_READ_RESPONSE_ONLY) &&
-		    diff == 0) {
-			ret = 1;
-			goto bail;
-		}
-		/*
-		 * If this request is a RDMA read or atomic, and the ACK is
-		 * for a later operation, this ACK NAKs the RDMA read or
-		 * atomic.  In other words, only a RDMA_READ_LAST or ONLY
-		 * can ACK a RDMA read and likewise for atomic ops.  Note
-		 * that the NAK case can only happen if relaxed ordering is
-		 * used and requests are sent after an RDMA read or atomic
-		 * is sent but before the response is received.
-		 */
-		if ((wqe->wr.opcode == IB_WR_RDMA_READ &&
-		     (opcode != OP(RDMA_READ_RESPONSE_LAST) || diff != 0)) ||
-		    ((wqe->wr.opcode == IB_WR_ATOMIC_CMP_AND_SWP ||
-		      wqe->wr.opcode == IB_WR_ATOMIC_FETCH_AND_ADD) &&
-		     (opcode != OP(ATOMIC_ACKNOWLEDGE) || diff != 0))) {
-			/*
-			 * The last valid PSN seen is the previous
-			 * request's.
-			 */
-			update_last_psn(qp, wqe->psn - 1);
-			/* Retry this request. */
-			ipath_restart_rc(qp, wqe->psn);
-			/*
-			 * No need to process the ACK/NAK since we are
-			 * restarting an earlier request.
-			 */
-			goto bail;
-		}
-		if (wqe->wr.opcode == IB_WR_ATOMIC_CMP_AND_SWP ||
-		    wqe->wr.opcode == IB_WR_ATOMIC_FETCH_AND_ADD)
-			*(u64 *) wqe->sg_list[0].vaddr = val;
-		if (qp->s_num_rd_atomic &&
-		    (wqe->wr.opcode == IB_WR_RDMA_READ ||
-		     wqe->wr.opcode == IB_WR_ATOMIC_CMP_AND_SWP ||
-		     wqe->wr.opcode == IB_WR_ATOMIC_FETCH_AND_ADD)) {
-			qp->s_num_rd_atomic--;
-			/* Restart sending task if fence is complete */
-			if (((qp->s_flags & IPATH_S_FENCE_PENDING) &&
-			     !qp->s_num_rd_atomic) ||
-			    qp->s_flags & IPATH_S_RDMAR_PENDING)
-				ipath_schedule_send(qp);
-		}
-		/* Post a send completion queue entry if requested. */
-		if (!(qp->s_flags & IPATH_S_SIGNAL_REQ_WR) ||
-		    (wqe->wr.send_flags & IB_SEND_SIGNALED)) {
-			memset(&wc, 0, sizeof wc);
-			wc.wr_id = wqe->wr.wr_id;
-			wc.status = IB_WC_SUCCESS;
-			wc.opcode = ib_ipath_wc_opcode[wqe->wr.opcode];
-			wc.byte_len = wqe->length;
-			wc.qp = &qp->ibqp;
-			wc.src_qp = qp->remote_qpn;
-			wc.slid = qp->remote_ah_attr.dlid;
-			wc.sl = qp->remote_ah_attr.sl;
-			ipath_cq_enter(to_icq(qp->ibqp.send_cq), &wc, 0);
-		}
-		qp->s_retry = qp->s_retry_cnt;
-		/*
-		 * If we are completing a request which is in the process of
-		 * being resent, we can stop resending it since we know the
-		 * responder has already seen it.
-		 */
-		if (qp->s_last == qp->s_cur) {
-			if (++qp->s_cur >= qp->s_size)
-				qp->s_cur = 0;
-			qp->s_last = qp->s_cur;
-			if (qp->s_last == qp->s_tail)
-				break;
-			wqe = get_swqe_ptr(qp, qp->s_cur);
-			qp->s_state = OP(SEND_LAST);
-			qp->s_psn = wqe->psn;
-		} else {
-			if (++qp->s_last >= qp->s_size)
-				qp->s_last = 0;
-			if (qp->state == IB_QPS_SQD && qp->s_last == qp->s_cur)
-				qp->s_draining = 0;
-			if (qp->s_last == qp->s_tail)
-				break;
-			wqe = get_swqe_ptr(qp, qp->s_last);
-		}
-	}
-
-	switch (aeth >> 29) {
-	case 0:		/* ACK */
-		dev->n_rc_acks++;
-		/* If this is a partial ACK, reset the retransmit timer. */
-		if (qp->s_last != qp->s_tail) {
-			spin_lock(&dev->pending_lock);
-			if (list_empty(&qp->timerwait))
-				list_add_tail(&qp->timerwait,
-					&dev->pending[dev->pending_index]);
-			spin_unlock(&dev->pending_lock);
-			/*
-			 * If we get a partial ACK for a resent operation,
-			 * we can stop resending the earlier packets and
-			 * continue with the next packet the receiver wants.
-			 */
-			if (ipath_cmp24(qp->s_psn, psn) <= 0) {
-				reset_psn(qp, psn + 1);
-				ipath_schedule_send(qp);
-			}
-		} else if (ipath_cmp24(qp->s_psn, psn) <= 0) {
-			qp->s_state = OP(SEND_LAST);
-			qp->s_psn = psn + 1;
-		}
-		ipath_get_credit(qp, aeth);
-		qp->s_rnr_retry = qp->s_rnr_retry_cnt;
-		qp->s_retry = qp->s_retry_cnt;
-		update_last_psn(qp, psn);
-		ret = 1;
-		goto bail;
-
-	case 1:		/* RNR NAK */
-		dev->n_rnr_naks++;
-		if (qp->s_last == qp->s_tail)
-			goto bail;
-		if (qp->s_rnr_retry == 0) {
-			status = IB_WC_RNR_RETRY_EXC_ERR;
-			goto class_b;
-		}
-		if (qp->s_rnr_retry_cnt < 7)
-			qp->s_rnr_retry--;
-
-		/* The last valid PSN is the previous PSN. */
-		update_last_psn(qp, psn - 1);
-
-		if (wqe->wr.opcode == IB_WR_RDMA_READ)
-			dev->n_rc_resends++;
-		else
-			dev->n_rc_resends +=
-				(qp->s_psn - psn) & IPATH_PSN_MASK;
-
-		reset_psn(qp, psn);
-
-		qp->s_rnr_timeout =
-			ib_ipath_rnr_table[(aeth >> IPATH_AETH_CREDIT_SHIFT) &
-					   IPATH_AETH_CREDIT_MASK];
-		ipath_insert_rnr_queue(qp);
-		ipath_schedule_send(qp);
-		goto bail;
-
-	case 3:		/* NAK */
-		if (qp->s_last == qp->s_tail)
-			goto bail;
-		/* The last valid PSN is the previous PSN. */
-		update_last_psn(qp, psn - 1);
-		switch ((aeth >> IPATH_AETH_CREDIT_SHIFT) &
-			IPATH_AETH_CREDIT_MASK) {
-		case 0:	/* PSN sequence error */
-			dev->n_seq_naks++;
-			/*
-			 * Back up to the responder's expected PSN.
-			 * Note that we might get a NAK in the middle of an
-			 * RDMA READ response which terminates the RDMA
-			 * READ.
-			 */
-			ipath_restart_rc(qp, psn);
-			break;
-
-		case 1:	/* Invalid Request */
-			status = IB_WC_REM_INV_REQ_ERR;
-			dev->n_other_naks++;
-			goto class_b;
-
-		case 2:	/* Remote Access Error */
-			status = IB_WC_REM_ACCESS_ERR;
-			dev->n_other_naks++;
-			goto class_b;
-
-		case 3:	/* Remote Operation Error */
-			status = IB_WC_REM_OP_ERR;
-			dev->n_other_naks++;
-		class_b:
-			ipath_send_complete(qp, wqe, status);
-			ipath_error_qp(qp, IB_WC_WR_FLUSH_ERR);
-			break;
-
-		default:
-			/* Ignore other reserved NAK error codes */
-			goto reserved;
-		}
-		qp->s_rnr_retry = qp->s_rnr_retry_cnt;
-		goto bail;
-
-	default:		/* 2: reserved */
-	reserved:
-		/* Ignore reserved NAK codes. */
-		goto bail;
-	}
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_rc_rcv_resp - process an incoming RC response packet
- * @dev: the device this packet came in on
- * @ohdr: the other headers for this packet
- * @data: the packet data
- * @tlen: the packet length
- * @qp: the QP for this packet
- * @opcode: the opcode for this packet
- * @psn: the packet sequence number for this packet
- * @hdrsize: the header length
- * @pmtu: the path MTU
- * @header_in_data: true if part of the header data is in the data buffer
- *
- * This is called from ipath_rc_rcv() to process an incoming RC response
- * packet for the given QP.
- * Called at interrupt level.
- */
-static inline void ipath_rc_rcv_resp(struct ipath_ibdev *dev,
-				     struct ipath_other_headers *ohdr,
-				     void *data, u32 tlen,
-				     struct ipath_qp *qp,
-				     u32 opcode,
-				     u32 psn, u32 hdrsize, u32 pmtu,
-				     int header_in_data)
-{
-	struct ipath_swqe *wqe;
-	enum ib_wc_status status;
-	unsigned long flags;
-	int diff;
-	u32 pad;
-	u32 aeth;
-	u64 val;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	/* Double check we can process this now that we hold the s_lock. */
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK))
-		goto ack_done;
-
-	/* Ignore invalid responses. */
-	if (ipath_cmp24(psn, qp->s_next_psn) >= 0)
-		goto ack_done;
-
-	/* Ignore duplicate responses. */
-	diff = ipath_cmp24(psn, qp->s_last_psn);
-	if (unlikely(diff <= 0)) {
-		/* Update credits for "ghost" ACKs */
-		if (diff == 0 && opcode == OP(ACKNOWLEDGE)) {
-			if (!header_in_data)
-				aeth = be32_to_cpu(ohdr->u.aeth);
-			else {
-				aeth = be32_to_cpu(((__be32 *) data)[0]);
-				data += sizeof(__be32);
-			}
-			if ((aeth >> 29) == 0)
-				ipath_get_credit(qp, aeth);
-		}
-		goto ack_done;
-	}
-
-	if (unlikely(qp->s_last == qp->s_tail))
-		goto ack_done;
-	wqe = get_swqe_ptr(qp, qp->s_last);
-	status = IB_WC_SUCCESS;
-
-	switch (opcode) {
-	case OP(ACKNOWLEDGE):
-	case OP(ATOMIC_ACKNOWLEDGE):
-	case OP(RDMA_READ_RESPONSE_FIRST):
-		if (!header_in_data)
-			aeth = be32_to_cpu(ohdr->u.aeth);
-		else {
-			aeth = be32_to_cpu(((__be32 *) data)[0]);
-			data += sizeof(__be32);
-		}
-		if (opcode == OP(ATOMIC_ACKNOWLEDGE)) {
-			if (!header_in_data) {
-				__be32 *p = ohdr->u.at.atomic_ack_eth;
-
-				val = ((u64) be32_to_cpu(p[0]) << 32) |
-					be32_to_cpu(p[1]);
-			} else
-				val = be64_to_cpu(((__be64 *) data)[0]);
-		} else
-			val = 0;
-		if (!do_rc_ack(qp, aeth, psn, opcode, val) ||
-		    opcode != OP(RDMA_READ_RESPONSE_FIRST))
-			goto ack_done;
-		hdrsize += 4;
-		wqe = get_swqe_ptr(qp, qp->s_last);
-		if (unlikely(wqe->wr.opcode != IB_WR_RDMA_READ))
-			goto ack_op_err;
-		qp->r_flags &= ~IPATH_R_RDMAR_SEQ;
-		/*
-		 * If this is a response to a resent RDMA read, we
-		 * have to be careful to copy the data to the right
-		 * location.
-		 */
-		qp->s_rdma_read_len = restart_sge(&qp->s_rdma_read_sge,
-						  wqe, psn, pmtu);
-		goto read_middle;
-
-	case OP(RDMA_READ_RESPONSE_MIDDLE):
-		/* no AETH, no ACK */
-		if (unlikely(ipath_cmp24(psn, qp->s_last_psn + 1))) {
-			dev->n_rdma_seq++;
-			if (qp->r_flags & IPATH_R_RDMAR_SEQ)
-				goto ack_done;
-			qp->r_flags |= IPATH_R_RDMAR_SEQ;
-			ipath_restart_rc(qp, qp->s_last_psn + 1);
-			goto ack_done;
-		}
-		if (unlikely(wqe->wr.opcode != IB_WR_RDMA_READ))
-			goto ack_op_err;
-	read_middle:
-		if (unlikely(tlen != (hdrsize + pmtu + 4)))
-			goto ack_len_err;
-		if (unlikely(pmtu >= qp->s_rdma_read_len))
-			goto ack_len_err;
-
-		/* We got a response so update the timeout. */
-		spin_lock(&dev->pending_lock);
-		if (qp->s_rnr_timeout == 0 && !list_empty(&qp->timerwait))
-			list_move_tail(&qp->timerwait,
-				       &dev->pending[dev->pending_index]);
-		spin_unlock(&dev->pending_lock);
-
-		if (opcode == OP(RDMA_READ_RESPONSE_MIDDLE))
-			qp->s_retry = qp->s_retry_cnt;
-
-		/*
-		 * Update the RDMA receive state but do the copy w/o
-		 * holding the locks and blocking interrupts.
-		 */
-		qp->s_rdma_read_len -= pmtu;
-		update_last_psn(qp, psn);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-		ipath_copy_sge(&qp->s_rdma_read_sge, data, pmtu);
-		goto bail;
-
-	case OP(RDMA_READ_RESPONSE_ONLY):
-		if (!header_in_data)
-			aeth = be32_to_cpu(ohdr->u.aeth);
-		else
-			aeth = be32_to_cpu(((__be32 *) data)[0]);
-		if (!do_rc_ack(qp, aeth, psn, opcode, 0))
-			goto ack_done;
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/*
-		 * Check that the data size is >= 0 && <= pmtu.
-		 * Remember to account for the AETH header (4) and
-		 * ICRC (4).
-		 */
-		if (unlikely(tlen < (hdrsize + pad + 8)))
-			goto ack_len_err;
-		/*
-		 * If this is a response to a resent RDMA read, we
-		 * have to be careful to copy the data to the right
-		 * location.
-		 */
-		wqe = get_swqe_ptr(qp, qp->s_last);
-		qp->s_rdma_read_len = restart_sge(&qp->s_rdma_read_sge,
-						  wqe, psn, pmtu);
-		goto read_last;
-
-	case OP(RDMA_READ_RESPONSE_LAST):
-		/* ACKs READ req. */
-		if (unlikely(ipath_cmp24(psn, qp->s_last_psn + 1))) {
-			dev->n_rdma_seq++;
-			if (qp->r_flags & IPATH_R_RDMAR_SEQ)
-				goto ack_done;
-			qp->r_flags |= IPATH_R_RDMAR_SEQ;
-			ipath_restart_rc(qp, qp->s_last_psn + 1);
-			goto ack_done;
-		}
-		if (unlikely(wqe->wr.opcode != IB_WR_RDMA_READ))
-			goto ack_op_err;
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/*
-		 * Check that the data size is >= 1 && <= pmtu.
-		 * Remember to account for the AETH header (4) and
-		 * ICRC (4).
-		 */
-		if (unlikely(tlen <= (hdrsize + pad + 8)))
-			goto ack_len_err;
-	read_last:
-		tlen -= hdrsize + pad + 8;
-		if (unlikely(tlen != qp->s_rdma_read_len))
-			goto ack_len_err;
-		if (!header_in_data)
-			aeth = be32_to_cpu(ohdr->u.aeth);
-		else {
-			aeth = be32_to_cpu(((__be32 *) data)[0]);
-			data += sizeof(__be32);
-		}
-		ipath_copy_sge(&qp->s_rdma_read_sge, data, tlen);
-		(void) do_rc_ack(qp, aeth, psn,
-				 OP(RDMA_READ_RESPONSE_LAST), 0);
-		goto ack_done;
-	}
-
-ack_op_err:
-	status = IB_WC_LOC_QP_OP_ERR;
-	goto ack_err;
-
-ack_len_err:
-	status = IB_WC_LOC_LEN_ERR;
-ack_err:
-	ipath_send_complete(qp, wqe, status);
-	ipath_error_qp(qp, IB_WC_WR_FLUSH_ERR);
-ack_done:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-bail:
-	return;
-}
-
-/**
- * ipath_rc_rcv_error - process an incoming duplicate or error RC packet
- * @dev: the device this packet came in on
- * @ohdr: the other headers for this packet
- * @data: the packet data
- * @qp: the QP for this packet
- * @opcode: the opcode for this packet
- * @psn: the packet sequence number for this packet
- * @diff: the difference between the PSN and the expected PSN
- * @header_in_data: true if part of the header data is in the data buffer
- *
- * This is called from ipath_rc_rcv() to process an unexpected
- * incoming RC packet for the given QP.
- * Called at interrupt level.
- * Return 1 if no more processing is needed; otherwise return 0 to
- * schedule a response to be sent.
- */
-static inline int ipath_rc_rcv_error(struct ipath_ibdev *dev,
-				     struct ipath_other_headers *ohdr,
-				     void *data,
-				     struct ipath_qp *qp,
-				     u32 opcode,
-				     u32 psn,
-				     int diff,
-				     int header_in_data)
-{
-	struct ipath_ack_entry *e;
-	u8 i, prev;
-	int old_req;
-	unsigned long flags;
-
-	if (diff > 0) {
-		/*
-		 * Packet sequence error.
-		 * A NAK will ACK earlier sends and RDMA writes.
-		 * Don't queue the NAK if we already sent one.
-		 */
-		if (!qp->r_nak_state) {
-			qp->r_nak_state = IB_NAK_PSN_ERROR;
-			/* Use the expected PSN. */
-			qp->r_ack_psn = qp->r_psn;
-			goto send_ack;
-		}
-		goto done;
-	}
-
-	/*
-	 * Handle a duplicate request.  Don't re-execute SEND, RDMA
-	 * write or atomic op.  Don't NAK errors, just silently drop
-	 * the duplicate request.  Note that r_sge, r_len, and
-	 * r_rcv_len may be in use so don't modify them.
-	 *
-	 * We are supposed to ACK the earliest duplicate PSN but we
-	 * can coalesce an outstanding duplicate ACK.  We have to
-	 * send the earliest so that RDMA reads can be restarted at
-	 * the requester's expected PSN.
-	 *
-	 * First, find where this duplicate PSN falls within the
-	 * ACKs previously sent.
-	 */
-	psn &= IPATH_PSN_MASK;
-	e = NULL;
-	old_req = 1;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-	/* Double check we can process this now that we hold the s_lock. */
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK))
-		goto unlock_done;
-
-	for (i = qp->r_head_ack_queue; ; i = prev) {
-		if (i == qp->s_tail_ack_queue)
-			old_req = 0;
-		if (i)
-			prev = i - 1;
-		else
-			prev = IPATH_MAX_RDMA_ATOMIC;
-		if (prev == qp->r_head_ack_queue) {
-			e = NULL;
-			break;
-		}
-		e = &qp->s_ack_queue[prev];
-		if (!e->opcode) {
-			e = NULL;
-			break;
-		}
-		if (ipath_cmp24(psn, e->psn) >= 0) {
-			if (prev == qp->s_tail_ack_queue)
-				old_req = 0;
-			break;
-		}
-	}
-	switch (opcode) {
-	case OP(RDMA_READ_REQUEST): {
-		struct ib_reth *reth;
-		u32 offset;
-		u32 len;
-
-		/*
-		 * If we didn't find the RDMA read request in the ack queue,
-		 * or the send tasklet is already backed up to send an
-		 * earlier entry, we can ignore this request.
-		 */
-		if (!e || e->opcode != OP(RDMA_READ_REQUEST) || old_req)
-			goto unlock_done;
-		/* RETH comes after BTH */
-		if (!header_in_data)
-			reth = &ohdr->u.rc.reth;
-		else {
-			reth = (struct ib_reth *)data;
-			data += sizeof(*reth);
-		}
-		/*
-		 * Address range must be a subset of the original
-		 * request and start on pmtu boundaries.
-		 * We reuse the old ack_queue slot since the requester
-		 * should not back up and request an earlier PSN for the
-		 * same request.
-		 */
-		offset = ((psn - e->psn) & IPATH_PSN_MASK) *
-			ib_mtu_enum_to_int(qp->path_mtu);
-		len = be32_to_cpu(reth->length);
-		if (unlikely(offset + len > e->rdma_sge.sge.sge_length))
-			goto unlock_done;
-		if (len != 0) {
-			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
-			int ok;
-
-			ok = ipath_rkey_ok(qp, &e->rdma_sge,
-					   len, vaddr, rkey,
-					   IB_ACCESS_REMOTE_READ);
-			if (unlikely(!ok))
-				goto unlock_done;
-		} else {
-			e->rdma_sge.sg_list = NULL;
-			e->rdma_sge.num_sge = 0;
-			e->rdma_sge.sge.mr = NULL;
-			e->rdma_sge.sge.vaddr = NULL;
-			e->rdma_sge.sge.length = 0;
-			e->rdma_sge.sge.sge_length = 0;
-		}
-		e->psn = psn;
-		qp->s_ack_state = OP(ACKNOWLEDGE);
-		qp->s_tail_ack_queue = prev;
-		break;
-	}
-
-	case OP(COMPARE_SWAP):
-	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
-		 * earlier entry, we can ignore this request.
-		 */
-		if (!e || e->opcode != (u8) opcode || old_req)
-			goto unlock_done;
-		qp->s_ack_state = OP(ACKNOWLEDGE);
-		qp->s_tail_ack_queue = prev;
-		break;
-	}
-
-	default:
-		if (old_req)
-			goto unlock_done;
-		/*
-		 * Resend the most recent ACK if this request is
-		 * after all the previous RDMA reads and atomics.
-		 */
-		if (i == qp->r_head_ack_queue) {
-			spin_unlock_irqrestore(&qp->s_lock, flags);
-			qp->r_nak_state = 0;
-			qp->r_ack_psn = qp->r_psn - 1;
-			goto send_ack;
-		}
-		/*
-		 * Try to send a simple ACK to work around a Mellanox bug
-		 * which doesn't accept a RDMA read response or atomic
-		 * response as an ACK for earlier SENDs or RDMA writes.
-		 */
-		if (qp->r_head_ack_queue == qp->s_tail_ack_queue &&
-		    !(qp->s_flags & IPATH_S_ACK_PENDING) &&
-		    qp->s_ack_state == OP(ACKNOWLEDGE)) {
-			spin_unlock_irqrestore(&qp->s_lock, flags);
-			qp->r_nak_state = 0;
-			qp->r_ack_psn = qp->s_ack_queue[i].psn - 1;
-			goto send_ack;
-		}
-		/*
-		 * Resend the RDMA read or atomic op which
-		 * ACKs this duplicate request.
-		 */
-		qp->s_ack_state = OP(ACKNOWLEDGE);
-		qp->s_tail_ack_queue = i;
-		break;
-	}
-	qp->r_nak_state = 0;
-	ipath_schedule_send(qp);
-
-unlock_done:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-done:
-	return 1;
-
-send_ack:
-	return 0;
-}
-
-void ipath_rc_error(struct ipath_qp *qp, enum ib_wc_status err)
-{
-	unsigned long flags;
-	int lastwqe;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-	lastwqe = ipath_error_qp(qp, err);
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-
-	if (lastwqe) {
-		struct ib_event ev;
-
-		ev.device = qp->ibqp.device;
-		ev.element.qp = &qp->ibqp;
-		ev.event = IB_EVENT_QP_LAST_WQE_REACHED;
-		qp->ibqp.event_handler(&ev, qp->ibqp.qp_context);
-	}
-}
-
-static inline void ipath_update_ack_queue(struct ipath_qp *qp, unsigned n)
-{
-	unsigned next;
-
-	next = n + 1;
-	if (next > IPATH_MAX_RDMA_ATOMIC)
-		next = 0;
-	if (n == qp->s_tail_ack_queue) {
-		qp->s_tail_ack_queue = next;
-		qp->s_ack_state = OP(ACKNOWLEDGE);
-	}
-}
-
-/**
- * ipath_rc_rcv - process an incoming RC packet
- * @dev: the device this packet came in on
- * @hdr: the header of this packet
- * @has_grh: true if the header has a GRH
- * @data: the packet data
- * @tlen: the packet length
- * @qp: the QP for this packet
- *
- * This is called from ipath_qp_rcv() to process an incoming RC packet
- * for the given QP.
- * Called at interrupt level.
- */
-void ipath_rc_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp)
-{
-	struct ipath_other_headers *ohdr;
-	u32 opcode;
-	u32 hdrsize;
-	u32 psn;
-	u32 pad;
-	struct ib_wc wc;
-	u32 pmtu = ib_mtu_enum_to_int(qp->path_mtu);
-	int diff;
-	struct ib_reth *reth;
-	int header_in_data;
-	unsigned long flags;
-
-	/* Validate the SLID. See Ch. 9.6.1.5 */
-	if (unlikely(be16_to_cpu(hdr->lrh[3]) != qp->remote_ah_attr.dlid))
-		goto done;
-
-	/* Check for GRH */
-	if (!has_grh) {
-		ohdr = &hdr->u.oth;
-		hdrsize = 8 + 12;	/* LRH + BTH */
-		psn = be32_to_cpu(ohdr->bth[2]);
-		header_in_data = 0;
-	} else {
-		ohdr = &hdr->u.l.oth;
-		hdrsize = 8 + 40 + 12;	/* LRH + GRH + BTH */
-		/*
-		 * The header with GRH is 60 bytes and the core driver sets
-		 * the eager header buffer size to 56 bytes so the last 4
-		 * bytes of the BTH header (PSN) is in the data buffer.
-		 */
-		header_in_data = dev->dd->ipath_rcvhdrentsize == 16;
-		if (header_in_data) {
-			psn = be32_to_cpu(((__be32 *) data)[0]);
-			data += sizeof(__be32);
-		} else
-			psn = be32_to_cpu(ohdr->bth[2]);
-	}
-
-	/*
-	 * Process responses (ACKs) before anything else.  Note that the
-	 * packet sequence number will be for something in the send work
-	 * queue rather than the expected receive packet sequence number.
-	 * In other words, this QP is the requester.
-	 */
-	opcode = be32_to_cpu(ohdr->bth[0]) >> 24;
-	if (opcode >= OP(RDMA_READ_RESPONSE_FIRST) &&
-	    opcode <= OP(ATOMIC_ACKNOWLEDGE)) {
-		ipath_rc_rcv_resp(dev, ohdr, data, tlen, qp, opcode, psn,
-				  hdrsize, pmtu, header_in_data);
-		goto done;
-	}
-
-	/* Compute 24 bits worth of difference. */
-	diff = ipath_cmp24(psn, qp->r_psn);
-	if (unlikely(diff)) {
-		if (ipath_rc_rcv_error(dev, ohdr, data, qp, opcode,
-				       psn, diff, header_in_data))
-			goto done;
-		goto send_ack;
-	}
-
-	/* Check for opcode sequence errors. */
-	switch (qp->r_state) {
-	case OP(SEND_FIRST):
-	case OP(SEND_MIDDLE):
-		if (opcode == OP(SEND_MIDDLE) ||
-		    opcode == OP(SEND_LAST) ||
-		    opcode == OP(SEND_LAST_WITH_IMMEDIATE))
-			break;
-		goto nack_inv;
-
-	case OP(RDMA_WRITE_FIRST):
-	case OP(RDMA_WRITE_MIDDLE):
-		if (opcode == OP(RDMA_WRITE_MIDDLE) ||
-		    opcode == OP(RDMA_WRITE_LAST) ||
-		    opcode == OP(RDMA_WRITE_LAST_WITH_IMMEDIATE))
-			break;
-		goto nack_inv;
-
-	default:
-		if (opcode == OP(SEND_MIDDLE) ||
-		    opcode == OP(SEND_LAST) ||
-		    opcode == OP(SEND_LAST_WITH_IMMEDIATE) ||
-		    opcode == OP(RDMA_WRITE_MIDDLE) ||
-		    opcode == OP(RDMA_WRITE_LAST) ||
-		    opcode == OP(RDMA_WRITE_LAST_WITH_IMMEDIATE))
-			goto nack_inv;
-		/*
-		 * Note that it is up to the requester to not send a new
-		 * RDMA read or atomic operation before receiving an ACK
-		 * for the previous operation.
-		 */
-		break;
-	}
-
-	memset(&wc, 0, sizeof wc);
-
-	/* OK, process the packet. */
-	switch (opcode) {
-	case OP(SEND_FIRST):
-		if (!ipath_get_rwqe(qp, 0))
-			goto rnr_nak;
-		qp->r_rcv_len = 0;
-		/* FALLTHROUGH */
-	case OP(SEND_MIDDLE):
-	case OP(RDMA_WRITE_MIDDLE):
-	send_middle:
-		/* Check for invalid length PMTU or posted rwqe len. */
-		if (unlikely(tlen != (hdrsize + pmtu + 4)))
-			goto nack_inv;
-		qp->r_rcv_len += pmtu;
-		if (unlikely(qp->r_rcv_len > qp->r_len))
-			goto nack_inv;
-		ipath_copy_sge(&qp->r_sge, data, pmtu);
-		break;
-
-	case OP(RDMA_WRITE_LAST_WITH_IMMEDIATE):
-		/* consume RWQE */
-		if (!ipath_get_rwqe(qp, 1))
-			goto rnr_nak;
-		goto send_last_imm;
-
-	case OP(SEND_ONLY):
-	case OP(SEND_ONLY_WITH_IMMEDIATE):
-		if (!ipath_get_rwqe(qp, 0))
-			goto rnr_nak;
-		qp->r_rcv_len = 0;
-		if (opcode == OP(SEND_ONLY))
-			goto send_last;
-		/* FALLTHROUGH */
-	case OP(SEND_LAST_WITH_IMMEDIATE):
-	send_last_imm:
-		if (header_in_data) {
-			wc.ex.imm_data = *(__be32 *) data;
-			data += sizeof(__be32);
-		} else {
-			/* Immediate data comes after BTH */
-			wc.ex.imm_data = ohdr->u.imm_data;
-		}
-		hdrsize += 4;
-		wc.wc_flags = IB_WC_WITH_IMM;
-		/* FALLTHROUGH */
-	case OP(SEND_LAST):
-	case OP(RDMA_WRITE_LAST):
-	send_last:
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/* Check for invalid length. */
-		/* XXX LAST len should be >= 1 */
-		if (unlikely(tlen < (hdrsize + pad + 4)))
-			goto nack_inv;
-		/* Don't count the CRC. */
-		tlen -= (hdrsize + pad + 4);
-		wc.byte_len = tlen + qp->r_rcv_len;
-		if (unlikely(wc.byte_len > qp->r_len))
-			goto nack_inv;
-		ipath_copy_sge(&qp->r_sge, data, tlen);
-		qp->r_msn++;
-		if (!test_and_clear_bit(IPATH_R_WRID_VALID, &qp->r_aflags))
-			break;
-		wc.wr_id = qp->r_wr_id;
-		wc.status = IB_WC_SUCCESS;
-		if (opcode == OP(RDMA_WRITE_LAST_WITH_IMMEDIATE) ||
-		    opcode == OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE))
-			wc.opcode = IB_WC_RECV_RDMA_WITH_IMM;
-		else
-			wc.opcode = IB_WC_RECV;
-		wc.qp = &qp->ibqp;
-		wc.src_qp = qp->remote_qpn;
-		wc.slid = qp->remote_ah_attr.dlid;
-		wc.sl = qp->remote_ah_attr.sl;
-		/* Signal completion event if the solicited bit is set. */
-		ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc,
-			       (ohdr->bth[0] &
-				cpu_to_be32(1 << 23)) != 0);
-		break;
-
-	case OP(RDMA_WRITE_FIRST):
-	case OP(RDMA_WRITE_ONLY):
-	case OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE):
-		if (unlikely(!(qp->qp_access_flags &
-			       IB_ACCESS_REMOTE_WRITE)))
-			goto nack_inv;
-		/* consume RWQE */
-		/* RETH comes after BTH */
-		if (!header_in_data)
-			reth = &ohdr->u.rc.reth;
-		else {
-			reth = (struct ib_reth *)data;
-			data += sizeof(*reth);
-		}
-		hdrsize += sizeof(*reth);
-		qp->r_len = be32_to_cpu(reth->length);
-		qp->r_rcv_len = 0;
-		if (qp->r_len != 0) {
-			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
-			int ok;
-
-			/* Check rkey & NAK */
-			ok = ipath_rkey_ok(qp, &qp->r_sge,
-					   qp->r_len, vaddr, rkey,
-					   IB_ACCESS_REMOTE_WRITE);
-			if (unlikely(!ok))
-				goto nack_acc;
-		} else {
-			qp->r_sge.sg_list = NULL;
-			qp->r_sge.sge.mr = NULL;
-			qp->r_sge.sge.vaddr = NULL;
-			qp->r_sge.sge.length = 0;
-			qp->r_sge.sge.sge_length = 0;
-		}
-		if (opcode == OP(RDMA_WRITE_FIRST))
-			goto send_middle;
-		else if (opcode == OP(RDMA_WRITE_ONLY))
-			goto send_last;
-		if (!ipath_get_rwqe(qp, 1))
-			goto rnr_nak;
-		goto send_last_imm;
-
-	case OP(RDMA_READ_REQUEST): {
-		struct ipath_ack_entry *e;
-		u32 len;
-		u8 next;
-
-		if (unlikely(!(qp->qp_access_flags &
-			       IB_ACCESS_REMOTE_READ)))
-			goto nack_inv;
-		next = qp->r_head_ack_queue + 1;
-		if (next > IPATH_MAX_RDMA_ATOMIC)
-			next = 0;
-		spin_lock_irqsave(&qp->s_lock, flags);
-		/* Double check we can process this while holding the s_lock. */
-		if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK))
-			goto unlock;
-		if (unlikely(next == qp->s_tail_ack_queue)) {
-			if (!qp->s_ack_queue[next].sent)
-				goto nack_inv_unlck;
-			ipath_update_ack_queue(qp, next);
-		}
-		e = &qp->s_ack_queue[qp->r_head_ack_queue];
-		/* RETH comes after BTH */
-		if (!header_in_data)
-			reth = &ohdr->u.rc.reth;
-		else {
-			reth = (struct ib_reth *)data;
-			data += sizeof(*reth);
-		}
-		len = be32_to_cpu(reth->length);
-		if (len) {
-			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
-			int ok;
-
-			/* Check rkey & NAK */
-			ok = ipath_rkey_ok(qp, &e->rdma_sge, len, vaddr,
-					   rkey, IB_ACCESS_REMOTE_READ);
-			if (unlikely(!ok))
-				goto nack_acc_unlck;
-			/*
-			 * Update the next expected PSN.  We add 1 later
-			 * below, so only add the remainder here.
-			 */
-			if (len > pmtu)
-				qp->r_psn += (len - 1) / pmtu;
-		} else {
-			e->rdma_sge.sg_list = NULL;
-			e->rdma_sge.num_sge = 0;
-			e->rdma_sge.sge.mr = NULL;
-			e->rdma_sge.sge.vaddr = NULL;
-			e->rdma_sge.sge.length = 0;
-			e->rdma_sge.sge.sge_length = 0;
-		}
-		e->opcode = opcode;
-		e->sent = 0;
-		e->psn = psn;
-		/*
-		 * We need to increment the MSN here instead of when we
-		 * finish sending the result since a duplicate request would
-		 * increment it more than once.
-		 */
-		qp->r_msn++;
-		qp->r_psn++;
-		qp->r_state = opcode;
-		qp->r_nak_state = 0;
-		qp->r_head_ack_queue = next;
-
-		/* Schedule the send tasklet. */
-		ipath_schedule_send(qp);
-
-		goto unlock;
-	}
-
-	case OP(COMPARE_SWAP):
-	case OP(FETCH_ADD): {
-		struct ib_atomic_eth *ateth;
-		struct ipath_ack_entry *e;
-		u64 vaddr;
-		atomic64_t *maddr;
-		u64 sdata;
-		u32 rkey;
-		u8 next;
-
-		if (unlikely(!(qp->qp_access_flags &
-			       IB_ACCESS_REMOTE_ATOMIC)))
-			goto nack_inv;
-		next = qp->r_head_ack_queue + 1;
-		if (next > IPATH_MAX_RDMA_ATOMIC)
-			next = 0;
-		spin_lock_irqsave(&qp->s_lock, flags);
-		/* Double check we can process this while holding the s_lock. */
-		if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK))
-			goto unlock;
-		if (unlikely(next == qp->s_tail_ack_queue)) {
-			if (!qp->s_ack_queue[next].sent)
-				goto nack_inv_unlck;
-			ipath_update_ack_queue(qp, next);
-		}
-		if (!header_in_data)
-			ateth = &ohdr->u.atomic_eth;
-		else
-			ateth = (struct ib_atomic_eth *)data;
-		vaddr = ((u64) be32_to_cpu(ateth->vaddr[0]) << 32) |
-			be32_to_cpu(ateth->vaddr[1]);
-		if (unlikely(vaddr & (sizeof(u64) - 1)))
-			goto nack_inv_unlck;
-		rkey = be32_to_cpu(ateth->rkey);
-		/* Check rkey & NAK */
-		if (unlikely(!ipath_rkey_ok(qp, &qp->r_sge,
-					    sizeof(u64), vaddr, rkey,
-					    IB_ACCESS_REMOTE_ATOMIC)))
-			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);
-		e = &qp->s_ack_queue[qp->r_head_ack_queue];
-		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),
-				      sdata);
-		e->opcode = opcode;
-		e->sent = 0;
-		e->psn = psn & IPATH_PSN_MASK;
-		qp->r_msn++;
-		qp->r_psn++;
-		qp->r_state = opcode;
-		qp->r_nak_state = 0;
-		qp->r_head_ack_queue = next;
-
-		/* Schedule the send tasklet. */
-		ipath_schedule_send(qp);
-
-		goto unlock;
-	}
-
-	default:
-		/* NAK unknown opcodes. */
-		goto nack_inv;
-	}
-	qp->r_psn++;
-	qp->r_state = opcode;
-	qp->r_ack_psn = psn;
-	qp->r_nak_state = 0;
-	/* Send an ACK if requested or required. */
-	if (psn & (1 << 31))
-		goto send_ack;
-	goto done;
-
-rnr_nak:
-	qp->r_nak_state = IB_RNR_NAK | qp->r_min_rnr_timer;
-	qp->r_ack_psn = qp->r_psn;
-	goto send_ack;
-
-nack_inv_unlck:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-nack_inv:
-	ipath_rc_error(qp, IB_WC_LOC_QP_OP_ERR);
-	qp->r_nak_state = IB_NAK_INVALID_REQUEST;
-	qp->r_ack_psn = qp->r_psn;
-	goto send_ack;
-
-nack_acc_unlck:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-nack_acc:
-	ipath_rc_error(qp, IB_WC_LOC_PROT_ERR);
-	qp->r_nak_state = IB_NAK_REMOTE_ACCESS_ERROR;
-	qp->r_ack_psn = qp->r_psn;
-send_ack:
-	send_rc_ack(qp);
-	goto done;
-
-unlock:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-done:
-	return;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_registers.h b/drivers/staging/rdma/ipath/ipath_registers.h
deleted file mode 100644
index 8f44d0cf3833..000000000000
--- a/drivers/staging/rdma/ipath/ipath_registers.h
+++ /dev/null
@@ -1,512 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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 _IPATH_REGISTERS_H
-#define _IPATH_REGISTERS_H
-
-/*
- * This file should only be included by kernel source, and by the diags.  It
- * defines the registers, and their contents, for InfiniPath chips.
- */
-
-/*
- * These are the InfiniPath register and buffer bit definitions,
- * that are visible to software, and needed only by the kernel
- * and diag code.  A few, that are visible to protocol and user
- * code are in ipath_common.h.  Some bits are specific
- * to a given chip implementation, and have been moved to the
- * chip-specific source file
- */
-
-/* kr_revision bits */
-#define INFINIPATH_R_CHIPREVMINOR_MASK 0xFF
-#define INFINIPATH_R_CHIPREVMINOR_SHIFT 0
-#define INFINIPATH_R_CHIPREVMAJOR_MASK 0xFF
-#define INFINIPATH_R_CHIPREVMAJOR_SHIFT 8
-#define INFINIPATH_R_ARCH_MASK 0xFF
-#define INFINIPATH_R_ARCH_SHIFT 16
-#define INFINIPATH_R_SOFTWARE_MASK 0xFF
-#define INFINIPATH_R_SOFTWARE_SHIFT 24
-#define INFINIPATH_R_BOARDID_MASK 0xFF
-#define INFINIPATH_R_BOARDID_SHIFT 32
-
-/* kr_control bits */
-#define INFINIPATH_C_FREEZEMODE 0x00000002
-#define INFINIPATH_C_LINKENABLE 0x00000004
-
-/* kr_sendctrl bits */
-#define INFINIPATH_S_DISARMPIOBUF_SHIFT 16
-#define INFINIPATH_S_UPDTHRESH_SHIFT 24
-#define INFINIPATH_S_UPDTHRESH_MASK 0x1f
-
-#define IPATH_S_ABORT		0
-#define IPATH_S_PIOINTBUFAVAIL	1
-#define IPATH_S_PIOBUFAVAILUPD	2
-#define IPATH_S_PIOENABLE	3
-#define IPATH_S_SDMAINTENABLE	9
-#define IPATH_S_SDMASINGLEDESCRIPTOR	10
-#define IPATH_S_SDMAENABLE	11
-#define IPATH_S_SDMAHALT	12
-#define IPATH_S_DISARM		31
-
-#define INFINIPATH_S_ABORT		(1U << IPATH_S_ABORT)
-#define INFINIPATH_S_PIOINTBUFAVAIL	(1U << IPATH_S_PIOINTBUFAVAIL)
-#define INFINIPATH_S_PIOBUFAVAILUPD	(1U << IPATH_S_PIOBUFAVAILUPD)
-#define INFINIPATH_S_PIOENABLE		(1U << IPATH_S_PIOENABLE)
-#define INFINIPATH_S_SDMAINTENABLE	(1U << IPATH_S_SDMAINTENABLE)
-#define INFINIPATH_S_SDMASINGLEDESCRIPTOR \
-					(1U << IPATH_S_SDMASINGLEDESCRIPTOR)
-#define INFINIPATH_S_SDMAENABLE		(1U << IPATH_S_SDMAENABLE)
-#define INFINIPATH_S_SDMAHALT		(1U << IPATH_S_SDMAHALT)
-#define INFINIPATH_S_DISARM		(1U << IPATH_S_DISARM)
-
-/* kr_rcvctrl bits that are the same on multiple chips */
-#define INFINIPATH_R_PORTENABLE_SHIFT 0
-#define INFINIPATH_R_QPMAP_ENABLE (1ULL << 38)
-
-/* kr_intstatus, kr_intclear, kr_intmask bits */
-#define INFINIPATH_I_SDMAINT		0x8000000000000000ULL
-#define INFINIPATH_I_SDMADISABLED	0x4000000000000000ULL
-#define INFINIPATH_I_ERROR		0x0000000080000000ULL
-#define INFINIPATH_I_SPIOSENT		0x0000000040000000ULL
-#define INFINIPATH_I_SPIOBUFAVAIL	0x0000000020000000ULL
-#define INFINIPATH_I_GPIO		0x0000000010000000ULL
-#define INFINIPATH_I_JINT		0x0000000004000000ULL
-
-/* kr_errorstatus, kr_errorclear, kr_errormask bits */
-#define INFINIPATH_E_RFORMATERR			0x0000000000000001ULL
-#define INFINIPATH_E_RVCRC			0x0000000000000002ULL
-#define INFINIPATH_E_RICRC			0x0000000000000004ULL
-#define INFINIPATH_E_RMINPKTLEN			0x0000000000000008ULL
-#define INFINIPATH_E_RMAXPKTLEN			0x0000000000000010ULL
-#define INFINIPATH_E_RLONGPKTLEN		0x0000000000000020ULL
-#define INFINIPATH_E_RSHORTPKTLEN		0x0000000000000040ULL
-#define INFINIPATH_E_RUNEXPCHAR			0x0000000000000080ULL
-#define INFINIPATH_E_RUNSUPVL			0x0000000000000100ULL
-#define INFINIPATH_E_REBP			0x0000000000000200ULL
-#define INFINIPATH_E_RIBFLOW			0x0000000000000400ULL
-#define INFINIPATH_E_RBADVERSION		0x0000000000000800ULL
-#define INFINIPATH_E_RRCVEGRFULL		0x0000000000001000ULL
-#define INFINIPATH_E_RRCVHDRFULL		0x0000000000002000ULL
-#define INFINIPATH_E_RBADTID			0x0000000000004000ULL
-#define INFINIPATH_E_RHDRLEN			0x0000000000008000ULL
-#define INFINIPATH_E_RHDR			0x0000000000010000ULL
-#define INFINIPATH_E_RIBLOSTLINK		0x0000000000020000ULL
-#define INFINIPATH_E_SENDSPECIALTRIGGER		0x0000000008000000ULL
-#define INFINIPATH_E_SDMADISABLED		0x0000000010000000ULL
-#define INFINIPATH_E_SMINPKTLEN			0x0000000020000000ULL
-#define INFINIPATH_E_SMAXPKTLEN			0x0000000040000000ULL
-#define INFINIPATH_E_SUNDERRUN			0x0000000080000000ULL
-#define INFINIPATH_E_SPKTLEN			0x0000000100000000ULL
-#define INFINIPATH_E_SDROPPEDSMPPKT		0x0000000200000000ULL
-#define INFINIPATH_E_SDROPPEDDATAPKT		0x0000000400000000ULL
-#define INFINIPATH_E_SPIOARMLAUNCH		0x0000000800000000ULL
-#define INFINIPATH_E_SUNEXPERRPKTNUM		0x0000001000000000ULL
-#define INFINIPATH_E_SUNSUPVL			0x0000002000000000ULL
-#define INFINIPATH_E_SENDBUFMISUSE		0x0000004000000000ULL
-#define INFINIPATH_E_SDMAGENMISMATCH		0x0000008000000000ULL
-#define INFINIPATH_E_SDMAOUTOFBOUND		0x0000010000000000ULL
-#define INFINIPATH_E_SDMATAILOUTOFBOUND		0x0000020000000000ULL
-#define INFINIPATH_E_SDMABASE			0x0000040000000000ULL
-#define INFINIPATH_E_SDMA1STDESC		0x0000080000000000ULL
-#define INFINIPATH_E_SDMARPYTAG			0x0000100000000000ULL
-#define INFINIPATH_E_SDMADWEN			0x0000200000000000ULL
-#define INFINIPATH_E_SDMAMISSINGDW		0x0000400000000000ULL
-#define INFINIPATH_E_SDMAUNEXPDATA		0x0000800000000000ULL
-#define INFINIPATH_E_IBSTATUSCHANGED		0x0001000000000000ULL
-#define INFINIPATH_E_INVALIDADDR		0x0002000000000000ULL
-#define INFINIPATH_E_RESET			0x0004000000000000ULL
-#define INFINIPATH_E_HARDWARE			0x0008000000000000ULL
-#define INFINIPATH_E_SDMADESCADDRMISALIGN	0x0010000000000000ULL
-#define INFINIPATH_E_INVALIDEEPCMD		0x0020000000000000ULL
-
-/*
- * this is used to print "common" packet errors only when the
- * __IPATH_ERRPKTDBG bit is set in ipath_debug.
- */
-#define INFINIPATH_E_PKTERRS ( INFINIPATH_E_SPKTLEN \
-		| INFINIPATH_E_SDROPPEDDATAPKT | INFINIPATH_E_RVCRC \
-		| INFINIPATH_E_RICRC | INFINIPATH_E_RSHORTPKTLEN \
-		| INFINIPATH_E_REBP )
-
-/* Convenience for decoding Send DMA errors */
-#define INFINIPATH_E_SDMAERRS ( \
-	INFINIPATH_E_SDMAGENMISMATCH | INFINIPATH_E_SDMAOUTOFBOUND | \
-	INFINIPATH_E_SDMATAILOUTOFBOUND | INFINIPATH_E_SDMABASE | \
-	INFINIPATH_E_SDMA1STDESC | INFINIPATH_E_SDMARPYTAG | \
-	INFINIPATH_E_SDMADWEN | INFINIPATH_E_SDMAMISSINGDW | \
-	INFINIPATH_E_SDMAUNEXPDATA | \
-	INFINIPATH_E_SDMADESCADDRMISALIGN | \
-	INFINIPATH_E_SDMADISABLED | \
-	INFINIPATH_E_SENDBUFMISUSE)
-
-/* kr_hwerrclear, kr_hwerrmask, kr_hwerrstatus, bits */
-/* TXEMEMPARITYERR bit 0: PIObuf, 1: PIOpbc, 2: launchfifo
- * RXEMEMPARITYERR bit 0: rcvbuf, 1: lookupq, 2:  expTID, 3: eagerTID
- * 		bit 4: flag buffer, 5: datainfo, 6: header info */
-#define INFINIPATH_HWE_TXEMEMPARITYERR_MASK 0xFULL
-#define INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT 40
-#define INFINIPATH_HWE_RXEMEMPARITYERR_MASK 0x7FULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT 44
-#define INFINIPATH_HWE_IBCBUSTOSPCPARITYERR 0x4000000000000000ULL
-#define INFINIPATH_HWE_IBCBUSFRSPCPARITYERR 0x8000000000000000ULL
-/* txe mem parity errors (shift by INFINIPATH_HWE_TXEMEMPARITYERR_SHIFT) */
-#define INFINIPATH_HWE_TXEMEMPARITYERR_PIOBUF	0x1ULL
-#define INFINIPATH_HWE_TXEMEMPARITYERR_PIOPBC	0x2ULL
-#define INFINIPATH_HWE_TXEMEMPARITYERR_PIOLAUNCHFIFO 0x4ULL
-/* rxe mem parity errors (shift by INFINIPATH_HWE_RXEMEMPARITYERR_SHIFT) */
-#define INFINIPATH_HWE_RXEMEMPARITYERR_RCVBUF   0x01ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_LOOKUPQ  0x02ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_EXPTID   0x04ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_EAGERTID 0x08ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_FLAGBUF  0x10ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_DATAINFO 0x20ULL
-#define INFINIPATH_HWE_RXEMEMPARITYERR_HDRINFO  0x40ULL
-/* waldo specific -- find the rest in ipath_6110.c */
-#define INFINIPATH_HWE_RXDSYNCMEMPARITYERR  0x0000000400000000ULL
-/* 6120/7220 specific -- find the rest in ipath_6120.c and ipath_7220.c */
-#define INFINIPATH_HWE_MEMBISTFAILED	0x0040000000000000ULL
-
-/* kr_hwdiagctrl bits */
-#define INFINIPATH_DC_FORCETXEMEMPARITYERR_MASK 0xFULL
-#define INFINIPATH_DC_FORCETXEMEMPARITYERR_SHIFT 40
-#define INFINIPATH_DC_FORCERXEMEMPARITYERR_MASK 0x7FULL
-#define INFINIPATH_DC_FORCERXEMEMPARITYERR_SHIFT 44
-#define INFINIPATH_DC_FORCERXDSYNCMEMPARITYERR  0x0000000400000000ULL
-#define INFINIPATH_DC_COUNTERDISABLE            0x1000000000000000ULL
-#define INFINIPATH_DC_COUNTERWREN               0x2000000000000000ULL
-#define INFINIPATH_DC_FORCEIBCBUSTOSPCPARITYERR 0x4000000000000000ULL
-#define INFINIPATH_DC_FORCEIBCBUSFRSPCPARITYERR 0x8000000000000000ULL
-
-/* kr_ibcctrl bits */
-#define INFINIPATH_IBCC_FLOWCTRLPERIOD_MASK 0xFFULL
-#define INFINIPATH_IBCC_FLOWCTRLPERIOD_SHIFT 0
-#define INFINIPATH_IBCC_FLOWCTRLWATERMARK_MASK 0xFFULL
-#define INFINIPATH_IBCC_FLOWCTRLWATERMARK_SHIFT 8
-#define INFINIPATH_IBCC_LINKINITCMD_MASK 0x3ULL
-#define INFINIPATH_IBCC_LINKINITCMD_DISABLE 1
-/* cycle through TS1/TS2 till OK */
-#define INFINIPATH_IBCC_LINKINITCMD_POLL 2
-/* wait for TS1, then go on */
-#define INFINIPATH_IBCC_LINKINITCMD_SLEEP 3
-#define INFINIPATH_IBCC_LINKINITCMD_SHIFT 16
-#define INFINIPATH_IBCC_LINKCMD_MASK 0x3ULL
-#define INFINIPATH_IBCC_LINKCMD_DOWN 1		/* move to 0x11 */
-#define INFINIPATH_IBCC_LINKCMD_ARMED 2		/* move to 0x21 */
-#define INFINIPATH_IBCC_LINKCMD_ACTIVE 3	/* move to 0x31 */
-#define INFINIPATH_IBCC_LINKCMD_SHIFT 18
-#define INFINIPATH_IBCC_MAXPKTLEN_MASK 0x7FFULL
-#define INFINIPATH_IBCC_MAXPKTLEN_SHIFT 20
-#define INFINIPATH_IBCC_PHYERRTHRESHOLD_MASK 0xFULL
-#define INFINIPATH_IBCC_PHYERRTHRESHOLD_SHIFT 32
-#define INFINIPATH_IBCC_OVERRUNTHRESHOLD_MASK 0xFULL
-#define INFINIPATH_IBCC_OVERRUNTHRESHOLD_SHIFT 36
-#define INFINIPATH_IBCC_CREDITSCALE_MASK 0x7ULL
-#define INFINIPATH_IBCC_CREDITSCALE_SHIFT 40
-#define INFINIPATH_IBCC_LOOPBACK             0x8000000000000000ULL
-#define INFINIPATH_IBCC_LINKDOWNDEFAULTSTATE 0x4000000000000000ULL
-
-/* kr_ibcstatus bits */
-#define INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT 0
-#define INFINIPATH_IBCS_LINKSTATE_MASK 0x7
-
-#define INFINIPATH_IBCS_TXREADY       0x40000000
-#define INFINIPATH_IBCS_TXCREDITOK    0x80000000
-/* link training states (shift by
-   INFINIPATH_IBCS_LINKTRAININGSTATE_SHIFT) */
-#define INFINIPATH_IBCS_LT_STATE_DISABLED	0x00
-#define INFINIPATH_IBCS_LT_STATE_LINKUP		0x01
-#define INFINIPATH_IBCS_LT_STATE_POLLACTIVE	0x02
-#define INFINIPATH_IBCS_LT_STATE_POLLQUIET	0x03
-#define INFINIPATH_IBCS_LT_STATE_SLEEPDELAY	0x04
-#define INFINIPATH_IBCS_LT_STATE_SLEEPQUIET	0x05
-#define INFINIPATH_IBCS_LT_STATE_CFGDEBOUNCE	0x08
-#define INFINIPATH_IBCS_LT_STATE_CFGRCVFCFG	0x09
-#define INFINIPATH_IBCS_LT_STATE_CFGWAITRMT	0x0a
-#define INFINIPATH_IBCS_LT_STATE_CFGIDLE	0x0b
-#define INFINIPATH_IBCS_LT_STATE_RECOVERRETRAIN	0x0c
-#define INFINIPATH_IBCS_LT_STATE_RECOVERWAITRMT	0x0e
-#define INFINIPATH_IBCS_LT_STATE_RECOVERIDLE	0x0f
-/* link state machine states (shift by ibcs_ls_shift) */
-#define INFINIPATH_IBCS_L_STATE_DOWN		0x0
-#define INFINIPATH_IBCS_L_STATE_INIT		0x1
-#define INFINIPATH_IBCS_L_STATE_ARM		0x2
-#define INFINIPATH_IBCS_L_STATE_ACTIVE		0x3
-#define INFINIPATH_IBCS_L_STATE_ACT_DEFER	0x4
-
-
-/* kr_extstatus bits */
-#define INFINIPATH_EXTS_SERDESPLLLOCK 0x1
-#define INFINIPATH_EXTS_GPIOIN_MASK 0xFFFFULL
-#define INFINIPATH_EXTS_GPIOIN_SHIFT 48
-
-/* kr_extctrl bits */
-#define INFINIPATH_EXTC_GPIOINVERT_MASK 0xFFFFULL
-#define INFINIPATH_EXTC_GPIOINVERT_SHIFT 32
-#define INFINIPATH_EXTC_GPIOOE_MASK 0xFFFFULL
-#define INFINIPATH_EXTC_GPIOOE_SHIFT 48
-#define INFINIPATH_EXTC_SERDESENABLE         0x80000000ULL
-#define INFINIPATH_EXTC_SERDESCONNECT        0x40000000ULL
-#define INFINIPATH_EXTC_SERDESENTRUNKING     0x20000000ULL
-#define INFINIPATH_EXTC_SERDESDISRXFIFO      0x10000000ULL
-#define INFINIPATH_EXTC_SERDESENPLPBK1       0x08000000ULL
-#define INFINIPATH_EXTC_SERDESENPLPBK2       0x04000000ULL
-#define INFINIPATH_EXTC_SERDESENENCDEC       0x02000000ULL
-#define INFINIPATH_EXTC_LED1SECPORT_ON       0x00000020ULL
-#define INFINIPATH_EXTC_LED2SECPORT_ON       0x00000010ULL
-#define INFINIPATH_EXTC_LED1PRIPORT_ON       0x00000008ULL
-#define INFINIPATH_EXTC_LED2PRIPORT_ON       0x00000004ULL
-#define INFINIPATH_EXTC_LEDGBLOK_ON          0x00000002ULL
-#define INFINIPATH_EXTC_LEDGBLERR_OFF        0x00000001ULL
-
-/* kr_partitionkey bits */
-#define INFINIPATH_PKEY_SIZE 16
-#define INFINIPATH_PKEY_MASK 0xFFFF
-#define INFINIPATH_PKEY_DEFAULT_PKEY 0xFFFF
-
-/* kr_serdesconfig0 bits */
-#define INFINIPATH_SERDC0_RESET_MASK  0xfULL	/* overal reset bits */
-#define INFINIPATH_SERDC0_RESET_PLL   0x10000000ULL	/* pll reset */
-/* tx idle enables (per lane) */
-#define INFINIPATH_SERDC0_TXIDLE      0xF000ULL
-/* rx detect enables (per lane) */
-#define INFINIPATH_SERDC0_RXDETECT_EN 0xF0000ULL
-/* L1 Power down; use with RXDETECT, Otherwise not used on IB side */
-#define INFINIPATH_SERDC0_L1PWR_DN	 0xF0ULL
-
-/* common kr_xgxsconfig bits (or safe in all, even if not implemented) */
-#define INFINIPATH_XGXS_RX_POL_SHIFT 19
-#define INFINIPATH_XGXS_RX_POL_MASK 0xfULL
-
-
-/*
- * IPATH_PIO_MAXIBHDR is the max IB header size allowed for in our
- * PIO send buffers.  This is well beyond anything currently
- * defined in the InfiniBand spec.
- */
-#define IPATH_PIO_MAXIBHDR 128
-
-typedef u64 ipath_err_t;
-
-/* The following change with the type of device, so
- * need to be part of the ipath_devdata struct, or
- * we could have problems plugging in devices of
- * different types (e.g. one HT, one PCIE)
- * in one system, to be managed by one driver.
- * On the other hand, this file is may also be included
- * by other code, so leave the declarations here
- * temporarily. Minor footprint issue if common-model
- * linker used, none if C89+ linker used.
- */
-
-/* mask of defined bits for various registers */
-extern u64 infinipath_i_bitsextant;
-extern ipath_err_t infinipath_e_bitsextant, infinipath_hwe_bitsextant;
-
-/* masks that are different in various chips, or only exist in some chips */
-extern u32 infinipath_i_rcvavail_mask, infinipath_i_rcvurg_mask;
-
-/*
- * These are the infinipath general register numbers (not offsets).
- * The kernel registers are used directly, those beyond the kernel
- * registers are calculated from one of the base registers.  The use of
- * an integer type doesn't allow type-checking as thorough as, say,
- * an enum but allows for better hiding of chip differences.
- */
-typedef const u16 ipath_kreg,	/* infinipath general registers */
- ipath_creg,			/* infinipath counter registers */
- ipath_sreg;			/* kernel-only, infinipath send registers */
-
-/*
- * These are the chip registers common to all infinipath chips, and
- * used both by the kernel and the diagnostics or other user code.
- * They are all implemented such that 64 bit accesses work.
- * Some implement no more than 32 bits.  Because 64 bit reads
- * require 2 HT cmds on opteron, we access those with 32 bit
- * reads for efficiency (they are written as 64 bits, since
- * the extra 32 bits are nearly free on writes, and it slightly reduces
- * complexity).  The rest are all accessed as 64 bits.
- */
-struct ipath_kregs {
-	/* These are the 32 bit group */
-	ipath_kreg kr_control;
-	ipath_kreg kr_counterregbase;
-	ipath_kreg kr_intmask;
-	ipath_kreg kr_intstatus;
-	ipath_kreg kr_pagealign;
-	ipath_kreg kr_portcnt;
-	ipath_kreg kr_rcvtidbase;
-	ipath_kreg kr_rcvtidcnt;
-	ipath_kreg kr_rcvegrbase;
-	ipath_kreg kr_rcvegrcnt;
-	ipath_kreg kr_scratch;
-	ipath_kreg kr_sendctrl;
-	ipath_kreg kr_sendpiobufbase;
-	ipath_kreg kr_sendpiobufcnt;
-	ipath_kreg kr_sendpiosize;
-	ipath_kreg kr_sendregbase;
-	ipath_kreg kr_userregbase;
-	/* These are the 64 bit group */
-	ipath_kreg kr_debugport;
-	ipath_kreg kr_debugportselect;
-	ipath_kreg kr_errorclear;
-	ipath_kreg kr_errormask;
-	ipath_kreg kr_errorstatus;
-	ipath_kreg kr_extctrl;
-	ipath_kreg kr_extstatus;
-	ipath_kreg kr_gpio_clear;
-	ipath_kreg kr_gpio_mask;
-	ipath_kreg kr_gpio_out;
-	ipath_kreg kr_gpio_status;
-	ipath_kreg kr_hwdiagctrl;
-	ipath_kreg kr_hwerrclear;
-	ipath_kreg kr_hwerrmask;
-	ipath_kreg kr_hwerrstatus;
-	ipath_kreg kr_ibcctrl;
-	ipath_kreg kr_ibcstatus;
-	ipath_kreg kr_intblocked;
-	ipath_kreg kr_intclear;
-	ipath_kreg kr_interruptconfig;
-	ipath_kreg kr_mdio;
-	ipath_kreg kr_partitionkey;
-	ipath_kreg kr_rcvbthqp;
-	ipath_kreg kr_rcvbufbase;
-	ipath_kreg kr_rcvbufsize;
-	ipath_kreg kr_rcvctrl;
-	ipath_kreg kr_rcvhdrcnt;
-	ipath_kreg kr_rcvhdrentsize;
-	ipath_kreg kr_rcvhdrsize;
-	ipath_kreg kr_rcvintmembase;
-	ipath_kreg kr_rcvintmemsize;
-	ipath_kreg kr_revision;
-	ipath_kreg kr_sendbuffererror;
-	ipath_kreg kr_sendpioavailaddr;
-	ipath_kreg kr_serdesconfig0;
-	ipath_kreg kr_serdesconfig1;
-	ipath_kreg kr_serdesstatus;
-	ipath_kreg kr_txintmembase;
-	ipath_kreg kr_txintmemsize;
-	ipath_kreg kr_xgxsconfig;
-	ipath_kreg kr_ibpllcfg;
-	/* use these two (and the following N ports) only with
-	 * ipath_k*_kreg64_port(); not *kreg64() */
-	ipath_kreg kr_rcvhdraddr;
-	ipath_kreg kr_rcvhdrtailaddr;
-
-	/* remaining registers are not present on all types of infinipath
-	   chips  */
-	ipath_kreg kr_rcvpktledcnt;
-	ipath_kreg kr_pcierbuftestreg0;
-	ipath_kreg kr_pcierbuftestreg1;
-	ipath_kreg kr_pcieq0serdesconfig0;
-	ipath_kreg kr_pcieq0serdesconfig1;
-	ipath_kreg kr_pcieq0serdesstatus;
-	ipath_kreg kr_pcieq1serdesconfig0;
-	ipath_kreg kr_pcieq1serdesconfig1;
-	ipath_kreg kr_pcieq1serdesstatus;
-	ipath_kreg kr_hrtbt_guid;
-	ipath_kreg kr_ibcddrctrl;
-	ipath_kreg kr_ibcddrstatus;
-	ipath_kreg kr_jintreload;
-
-	/* send dma related regs */
-	ipath_kreg kr_senddmabase;
-	ipath_kreg kr_senddmalengen;
-	ipath_kreg kr_senddmatail;
-	ipath_kreg kr_senddmahead;
-	ipath_kreg kr_senddmaheadaddr;
-	ipath_kreg kr_senddmabufmask0;
-	ipath_kreg kr_senddmabufmask1;
-	ipath_kreg kr_senddmabufmask2;
-	ipath_kreg kr_senddmastatus;
-
-	/* SerDes related regs (IBA7220-only) */
-	ipath_kreg kr_ibserdesctrl;
-	ipath_kreg kr_ib_epbacc;
-	ipath_kreg kr_ib_epbtrans;
-	ipath_kreg kr_pcie_epbacc;
-	ipath_kreg kr_pcie_epbtrans;
-	ipath_kreg kr_ib_ddsrxeq;
-};
-
-struct ipath_cregs {
-	ipath_creg cr_badformatcnt;
-	ipath_creg cr_erricrccnt;
-	ipath_creg cr_errlinkcnt;
-	ipath_creg cr_errlpcrccnt;
-	ipath_creg cr_errpkey;
-	ipath_creg cr_errrcvflowctrlcnt;
-	ipath_creg cr_err_rlencnt;
-	ipath_creg cr_errslencnt;
-	ipath_creg cr_errtidfull;
-	ipath_creg cr_errtidvalid;
-	ipath_creg cr_errvcrccnt;
-	ipath_creg cr_ibstatuschange;
-	ipath_creg cr_intcnt;
-	ipath_creg cr_invalidrlencnt;
-	ipath_creg cr_invalidslencnt;
-	ipath_creg cr_lbflowstallcnt;
-	ipath_creg cr_iblinkdowncnt;
-	ipath_creg cr_iblinkerrrecovcnt;
-	ipath_creg cr_ibsymbolerrcnt;
-	ipath_creg cr_pktrcvcnt;
-	ipath_creg cr_pktrcvflowctrlcnt;
-	ipath_creg cr_pktsendcnt;
-	ipath_creg cr_pktsendflowcnt;
-	ipath_creg cr_portovflcnt;
-	ipath_creg cr_rcvebpcnt;
-	ipath_creg cr_rcvovflcnt;
-	ipath_creg cr_rxdroppktcnt;
-	ipath_creg cr_senddropped;
-	ipath_creg cr_sendstallcnt;
-	ipath_creg cr_sendunderruncnt;
-	ipath_creg cr_unsupvlcnt;
-	ipath_creg cr_wordrcvcnt;
-	ipath_creg cr_wordsendcnt;
-	ipath_creg cr_vl15droppedpktcnt;
-	ipath_creg cr_rxotherlocalphyerrcnt;
-	ipath_creg cr_excessbufferovflcnt;
-	ipath_creg cr_locallinkintegrityerrcnt;
-	ipath_creg cr_rxvlerrcnt;
-	ipath_creg cr_rxdlidfltrcnt;
-	ipath_creg cr_psstat;
-	ipath_creg cr_psstart;
-	ipath_creg cr_psinterval;
-	ipath_creg cr_psrcvdatacount;
-	ipath_creg cr_psrcvpktscount;
-	ipath_creg cr_psxmitdatacount;
-	ipath_creg cr_psxmitpktscount;
-	ipath_creg cr_psxmitwaitcount;
-};
-
-#endif				/* _IPATH_REGISTERS_H */
diff --git a/drivers/staging/rdma/ipath/ipath_ruc.c b/drivers/staging/rdma/ipath/ipath_ruc.c
deleted file mode 100644
index e541a01f1f61..000000000000
--- a/drivers/staging/rdma/ipath/ipath_ruc.c
+++ /dev/null
@@ -1,733 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/spinlock.h>
-
-#include "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-/*
- * Convert the AETH RNR timeout code into the number of milliseconds.
- */
-const u32 ib_ipath_rnr_table[32] = {
-	656,			/* 0 */
-	1,			/* 1 */
-	1,			/* 2 */
-	1,			/* 3 */
-	1,			/* 4 */
-	1,			/* 5 */
-	1,			/* 6 */
-	1,			/* 7 */
-	1,			/* 8 */
-	1,			/* 9 */
-	1,			/* A */
-	1,			/* B */
-	1,			/* C */
-	1,			/* D */
-	2,			/* E */
-	2,			/* F */
-	3,			/* 10 */
-	4,			/* 11 */
-	6,			/* 12 */
-	8,			/* 13 */
-	11,			/* 14 */
-	16,			/* 15 */
-	21,			/* 16 */
-	31,			/* 17 */
-	41,			/* 18 */
-	62,			/* 19 */
-	82,			/* 1A */
-	123,			/* 1B */
-	164,			/* 1C */
-	246,			/* 1D */
-	328,			/* 1E */
-	492			/* 1F */
-};
-
-/**
- * ipath_insert_rnr_queue - put QP on the RNR timeout list for the device
- * @qp: the QP
- *
- * Called with the QP s_lock held and interrupts disabled.
- * XXX Use a simple list for now.  We might need a priority
- * queue if we have lots of QPs waiting for RNR timeouts
- * but that should be rare.
- */
-void ipath_insert_rnr_queue(struct ipath_qp *qp)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-
-	/* We already did a spin_lock_irqsave(), so just use spin_lock */
-	spin_lock(&dev->pending_lock);
-	if (list_empty(&dev->rnrwait))
-		list_add(&qp->timerwait, &dev->rnrwait);
-	else {
-		struct list_head *l = &dev->rnrwait;
-		struct ipath_qp *nqp = list_entry(l->next, struct ipath_qp,
-						  timerwait);
-
-		while (qp->s_rnr_timeout >= nqp->s_rnr_timeout) {
-			qp->s_rnr_timeout -= nqp->s_rnr_timeout;
-			l = l->next;
-			if (l->next == &dev->rnrwait) {
-				nqp = NULL;
-				break;
-			}
-			nqp = list_entry(l->next, struct ipath_qp,
-					 timerwait);
-		}
-		if (nqp)
-			nqp->s_rnr_timeout -= qp->s_rnr_timeout;
-		list_add(&qp->timerwait, l);
-	}
-	spin_unlock(&dev->pending_lock);
-}
-
-/**
- * ipath_init_sge - Validate a RWQE and fill in the SGE state
- * @qp: the QP
- *
- * Return 1 if OK.
- */
-int ipath_init_sge(struct ipath_qp *qp, struct ipath_rwqe *wqe,
-		   u32 *lengthp, struct ipath_sge_state *ss)
-{
-	int i, j, ret;
-	struct ib_wc wc;
-
-	*lengthp = 0;
-	for (i = j = 0; i < wqe->num_sge; i++) {
-		if (wqe->sg_list[i].length == 0)
-			continue;
-		/* Check LKEY */
-		if (!ipath_lkey_ok(qp, j ? &ss->sg_list[j - 1] : &ss->sge,
-				   &wqe->sg_list[i], IB_ACCESS_LOCAL_WRITE))
-			goto bad_lkey;
-		*lengthp += wqe->sg_list[i].length;
-		j++;
-	}
-	ss->num_sge = j;
-	ret = 1;
-	goto bail;
-
-bad_lkey:
-	memset(&wc, 0, sizeof(wc));
-	wc.wr_id = wqe->wr_id;
-	wc.status = IB_WC_LOC_PROT_ERR;
-	wc.opcode = IB_WC_RECV;
-	wc.qp = &qp->ibqp;
-	/* Signal solicited completion event. */
-	ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc, 1);
-	ret = 0;
-bail:
-	return ret;
-}
-
-/**
- * ipath_get_rwqe - copy the next RWQE into the QP's RWQE
- * @qp: the QP
- * @wr_id_only: update qp->r_wr_id only, not qp->r_sge
- *
- * Return 0 if no RWQE is available, otherwise return 1.
- *
- * Can be called from interrupt level.
- */
-int ipath_get_rwqe(struct ipath_qp *qp, int wr_id_only)
-{
-	unsigned long flags;
-	struct ipath_rq *rq;
-	struct ipath_rwq *wq;
-	struct ipath_srq *srq;
-	struct ipath_rwqe *wqe;
-	void (*handler)(struct ib_event *, void *);
-	u32 tail;
-	int ret;
-
-	if (qp->ibqp.srq) {
-		srq = to_isrq(qp->ibqp.srq);
-		handler = srq->ibsrq.event_handler;
-		rq = &srq->rq;
-	} else {
-		srq = NULL;
-		handler = NULL;
-		rq = &qp->r_rq;
-	}
-
-	spin_lock_irqsave(&rq->lock, flags);
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK)) {
-		ret = 0;
-		goto unlock;
-	}
-
-	wq = rq->wq;
-	tail = wq->tail;
-	/* Validate tail before using it since it is user writable. */
-	if (tail >= rq->size)
-		tail = 0;
-	do {
-		if (unlikely(tail == wq->head)) {
-			ret = 0;
-			goto unlock;
-		}
-		/* Make sure entry is read after head index is read. */
-		smp_rmb();
-		wqe = get_rwqe_ptr(rq, tail);
-		if (++tail >= rq->size)
-			tail = 0;
-		if (wr_id_only)
-			break;
-		qp->r_sge.sg_list = qp->r_sg_list;
-	} while (!ipath_init_sge(qp, wqe, &qp->r_len, &qp->r_sge));
-	qp->r_wr_id = wqe->wr_id;
-	wq->tail = tail;
-
-	ret = 1;
-	set_bit(IPATH_R_WRID_VALID, &qp->r_aflags);
-	if (handler) {
-		u32 n;
-
-		/*
-		 * validate head pointer value and compute
-		 * the number of remaining WQEs.
-		 */
-		n = wq->head;
-		if (n >= rq->size)
-			n = 0;
-		if (n < tail)
-			n += rq->size - tail;
-		else
-			n -= tail;
-		if (n < srq->limit) {
-			struct ib_event ev;
-
-			srq->limit = 0;
-			spin_unlock_irqrestore(&rq->lock, flags);
-			ev.device = qp->ibqp.device;
-			ev.element.srq = qp->ibqp.srq;
-			ev.event = IB_EVENT_SRQ_LIMIT_REACHED;
-			handler(&ev, srq->ibsrq.srq_context);
-			goto bail;
-		}
-	}
-unlock:
-	spin_unlock_irqrestore(&rq->lock, flags);
-bail:
-	return ret;
-}
-
-/**
- * ipath_ruc_loopback - handle UC and RC lookback requests
- * @sqp: the sending QP
- *
- * This is called from ipath_do_send() to
- * forward a WQE addressed to the same HCA.
- * Note that although we are single threaded due to the tasklet, 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.
- */
-static void ipath_ruc_loopback(struct ipath_qp *sqp)
-{
-	struct ipath_ibdev *dev = to_idev(sqp->ibqp.device);
-	struct ipath_qp *qp;
-	struct ipath_swqe *wqe;
-	struct ipath_sge *sge;
-	unsigned long flags;
-	struct ib_wc wc;
-	u64 sdata;
-	atomic64_t *maddr;
-	enum ib_wc_status send_status;
-
-	/*
-	 * Note that we check the responder QP state after
-	 * checking the requester's state.
-	 */
-	qp = ipath_lookup_qpn(&dev->qp_table, sqp->remote_qpn);
-
-	spin_lock_irqsave(&sqp->s_lock, flags);
-
-	/* Return if we are already busy processing a work request. */
-	if ((sqp->s_flags & (IPATH_S_BUSY | IPATH_S_ANY_WAIT)) ||
-	    !(ib_ipath_state_ops[sqp->state] & IPATH_PROCESS_OR_FLUSH_SEND))
-		goto unlock;
-
-	sqp->s_flags |= IPATH_S_BUSY;
-
-again:
-	if (sqp->s_last == sqp->s_head)
-		goto clr_busy;
-	wqe = get_swqe_ptr(sqp, sqp->s_last);
-
-	/* Return if it is not OK to start a new work reqeust. */
-	if (!(ib_ipath_state_ops[sqp->state] & IPATH_PROCESS_NEXT_SEND_OK)) {
-		if (!(ib_ipath_state_ops[sqp->state] & IPATH_FLUSH_SEND))
-			goto clr_busy;
-		/* We are in the error state, flush the work request. */
-		send_status = IB_WC_WR_FLUSH_ERR;
-		goto flush_send;
-	}
-
-	/*
-	 * We can rely on the entry not changing without the s_lock
-	 * being held until we update s_last.
-	 * We increment s_cur to indicate s_last is in progress.
-	 */
-	if (sqp->s_last == sqp->s_cur) {
-		if (++sqp->s_cur >= sqp->s_size)
-			sqp->s_cur = 0;
-	}
-	spin_unlock_irqrestore(&sqp->s_lock, flags);
-
-	if (!qp || !(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK)) {
-		dev->n_pkt_drops++;
-		/*
-		 * For RC, the requester would timeout and retry so
-		 * shortcut the timeouts and just signal too many retries.
-		 */
-		if (sqp->ibqp.qp_type == IB_QPT_RC)
-			send_status = IB_WC_RETRY_EXC_ERR;
-		else
-			send_status = IB_WC_SUCCESS;
-		goto serr;
-	}
-
-	memset(&wc, 0, sizeof wc);
-	send_status = IB_WC_SUCCESS;
-
-	sqp->s_sge.sge = wqe->sg_list[0];
-	sqp->s_sge.sg_list = wqe->sg_list + 1;
-	sqp->s_sge.num_sge = wqe->wr.num_sge;
-	sqp->s_len = wqe->length;
-	switch (wqe->wr.opcode) {
-	case IB_WR_SEND_WITH_IMM:
-		wc.wc_flags = IB_WC_WITH_IMM;
-		wc.ex.imm_data = wqe->wr.ex.imm_data;
-		/* FALLTHROUGH */
-	case IB_WR_SEND:
-		if (!ipath_get_rwqe(qp, 0))
-			goto rnr_nak;
-		break;
-
-	case IB_WR_RDMA_WRITE_WITH_IMM:
-		if (unlikely(!(qp->qp_access_flags & IB_ACCESS_REMOTE_WRITE)))
-			goto inv_err;
-		wc.wc_flags = IB_WC_WITH_IMM;
-		wc.ex.imm_data = wqe->wr.ex.imm_data;
-		if (!ipath_get_rwqe(qp, 1))
-			goto rnr_nak;
-		/* FALLTHROUGH */
-	case IB_WR_RDMA_WRITE:
-		if (unlikely(!(qp->qp_access_flags & IB_ACCESS_REMOTE_WRITE)))
-			goto inv_err;
-		if (wqe->length == 0)
-			break;
-		if (unlikely(!ipath_rkey_ok(qp, &qp->r_sge, wqe->length,
-					    wqe->rdma_wr.remote_addr,
-					    wqe->rdma_wr.rkey,
-					    IB_ACCESS_REMOTE_WRITE)))
-			goto acc_err;
-		break;
-
-	case IB_WR_RDMA_READ:
-		if (unlikely(!(qp->qp_access_flags & IB_ACCESS_REMOTE_READ)))
-			goto inv_err;
-		if (unlikely(!ipath_rkey_ok(qp, &sqp->s_sge, wqe->length,
-					    wqe->rdma_wr.remote_addr,
-					    wqe->rdma_wr.rkey,
-					    IB_ACCESS_REMOTE_READ)))
-			goto acc_err;
-		qp->r_sge.sge = wqe->sg_list[0];
-		qp->r_sge.sg_list = wqe->sg_list + 1;
-		qp->r_sge.num_sge = wqe->wr.num_sge;
-		break;
-
-	case IB_WR_ATOMIC_CMP_AND_SWP:
-	case IB_WR_ATOMIC_FETCH_AND_ADD:
-		if (unlikely(!(qp->qp_access_flags & IB_ACCESS_REMOTE_ATOMIC)))
-			goto inv_err;
-		if (unlikely(!ipath_rkey_ok(qp, &qp->r_sge, sizeof(u64),
-					    wqe->atomic_wr.remote_addr,
-					    wqe->atomic_wr.rkey,
-					    IB_ACCESS_REMOTE_ATOMIC)))
-			goto acc_err;
-		/* Perform atomic OP and save result. */
-		maddr = (atomic64_t *) qp->r_sge.sge.vaddr;
-		sdata = wqe->atomic_wr.compare_add;
-		*(u64 *) sqp->s_sge.sge.vaddr =
-			(wqe->wr.opcode == IB_WR_ATOMIC_FETCH_AND_ADD) ?
-			(u64) atomic64_add_return(sdata, maddr) - sdata :
-			(u64) cmpxchg((u64 *) qp->r_sge.sge.vaddr,
-				      sdata, wqe->atomic_wr.swap);
-		goto send_comp;
-
-	default:
-		send_status = IB_WC_LOC_QP_OP_ERR;
-		goto serr;
-	}
-
-	sge = &sqp->s_sge.sge;
-	while (sqp->s_len) {
-		u32 len = sqp->s_len;
-
-		if (len > sge->length)
-			len = sge->length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		ipath_copy_sge(&qp->r_sge, sge->vaddr, len);
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--sqp->s_sge.num_sge)
-				*sge = *sqp->s_sge.sg_list++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-		sqp->s_len -= len;
-	}
-
-	if (!test_and_clear_bit(IPATH_R_WRID_VALID, &qp->r_aflags))
-		goto send_comp;
-
-	if (wqe->wr.opcode == IB_WR_RDMA_WRITE_WITH_IMM)
-		wc.opcode = IB_WC_RECV_RDMA_WITH_IMM;
-	else
-		wc.opcode = IB_WC_RECV;
-	wc.wr_id = qp->r_wr_id;
-	wc.status = IB_WC_SUCCESS;
-	wc.byte_len = wqe->length;
-	wc.qp = &qp->ibqp;
-	wc.src_qp = qp->remote_qpn;
-	wc.slid = qp->remote_ah_attr.dlid;
-	wc.sl = qp->remote_ah_attr.sl;
-	wc.port_num = 1;
-	/* Signal completion event if the solicited bit is set. */
-	ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc,
-		       wqe->wr.send_flags & IB_SEND_SOLICITED);
-
-send_comp:
-	spin_lock_irqsave(&sqp->s_lock, flags);
-flush_send:
-	sqp->s_rnr_retry = sqp->s_rnr_retry_cnt;
-	ipath_send_complete(sqp, wqe, send_status);
-	goto again;
-
-rnr_nak:
-	/* Handle RNR NAK */
-	if (qp->ibqp.qp_type == IB_QPT_UC)
-		goto send_comp;
-	/*
-	 * Note: we don't need the s_lock held since the BUSY flag
-	 * makes this single threaded.
-	 */
-	if (sqp->s_rnr_retry == 0) {
-		send_status = IB_WC_RNR_RETRY_EXC_ERR;
-		goto serr;
-	}
-	if (sqp->s_rnr_retry_cnt < 7)
-		sqp->s_rnr_retry--;
-	spin_lock_irqsave(&sqp->s_lock, flags);
-	if (!(ib_ipath_state_ops[sqp->state] & IPATH_PROCESS_RECV_OK))
-		goto clr_busy;
-	sqp->s_flags |= IPATH_S_WAITING;
-	dev->n_rnr_naks++;
-	sqp->s_rnr_timeout = ib_ipath_rnr_table[qp->r_min_rnr_timer];
-	ipath_insert_rnr_queue(sqp);
-	goto clr_busy;
-
-inv_err:
-	send_status = IB_WC_REM_INV_REQ_ERR;
-	wc.status = IB_WC_LOC_QP_OP_ERR;
-	goto err;
-
-acc_err:
-	send_status = IB_WC_REM_ACCESS_ERR;
-	wc.status = IB_WC_LOC_PROT_ERR;
-err:
-	/* responder goes to error state */
-	ipath_rc_error(qp, wc.status);
-
-serr:
-	spin_lock_irqsave(&sqp->s_lock, flags);
-	ipath_send_complete(sqp, wqe, send_status);
-	if (sqp->ibqp.qp_type == IB_QPT_RC) {
-		int lastwqe = ipath_error_qp(sqp, IB_WC_WR_FLUSH_ERR);
-
-		sqp->s_flags &= ~IPATH_S_BUSY;
-		spin_unlock_irqrestore(&sqp->s_lock, flags);
-		if (lastwqe) {
-			struct ib_event ev;
-
-			ev.device = sqp->ibqp.device;
-			ev.element.qp = &sqp->ibqp;
-			ev.event = IB_EVENT_QP_LAST_WQE_REACHED;
-			sqp->ibqp.event_handler(&ev, sqp->ibqp.qp_context);
-		}
-		goto done;
-	}
-clr_busy:
-	sqp->s_flags &= ~IPATH_S_BUSY;
-unlock:
-	spin_unlock_irqrestore(&sqp->s_lock, flags);
-done:
-	if (qp && atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-}
-
-static void want_buffer(struct ipath_devdata *dd, struct ipath_qp *qp)
-{
-	if (!(dd->ipath_flags & IPATH_HAS_SEND_DMA) ||
-	    qp->ibqp.qp_type == IB_QPT_SMI) {
-		unsigned long flags;
-
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		dd->ipath_sendctrl |= INFINIPATH_S_PIOINTBUFAVAIL;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-				 dd->ipath_sendctrl);
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-	}
-}
-
-/**
- * ipath_no_bufs_available - tell the layer driver we need buffers
- * @qp: the QP that caused the problem
- * @dev: the device we ran out of buffers on
- *
- * Called when we run out of PIO buffers.
- * If we are now in the error state, return zero to flush the
- * send work request.
- */
-static int ipath_no_bufs_available(struct ipath_qp *qp,
-				    struct ipath_ibdev *dev)
-{
-	unsigned long flags;
-	int ret = 1;
-
-	/*
-	 * Note that as soon as want_buffer() is called and
-	 * possibly before it returns, ipath_ib_piobufavail()
-	 * could be called. Therefore, put QP on the piowait list before
-	 * enabling the PIO avail interrupt.
-	 */
-	spin_lock_irqsave(&qp->s_lock, flags);
-	if (ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK) {
-		dev->n_piowait++;
-		qp->s_flags |= IPATH_S_WAITING;
-		qp->s_flags &= ~IPATH_S_BUSY;
-		spin_lock(&dev->pending_lock);
-		if (list_empty(&qp->piowait))
-			list_add_tail(&qp->piowait, &dev->piowait);
-		spin_unlock(&dev->pending_lock);
-	} else
-		ret = 0;
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-	if (ret)
-		want_buffer(dev->dd, qp);
-	return ret;
-}
-
-/**
- * ipath_make_grh - construct a GRH header
- * @dev: a pointer to the ipath device
- * @hdr: a pointer to the GRH header being constructed
- * @grh: the global route address to send to
- * @hwords: the number of 32 bit words of header being sent
- * @nwords: the number of 32 bit words of data being sent
- *
- * Return the size of the header in 32 bit words.
- */
-u32 ipath_make_grh(struct ipath_ibdev *dev, struct ib_grh *hdr,
-		   struct ib_global_route *grh, u32 hwords, u32 nwords)
-{
-	hdr->version_tclass_flow =
-		cpu_to_be32((6 << 28) |
-			    (grh->traffic_class << 20) |
-			    grh->flow_label);
-	hdr->paylen = cpu_to_be16((hwords - 2 + nwords + SIZE_OF_CRC) << 2);
-	/* next_hdr is defined by C8-7 in ch. 8.4.1 */
-	hdr->next_hdr = 0x1B;
-	hdr->hop_limit = grh->hop_limit;
-	/* The SGID is 32-bit aligned. */
-	hdr->sgid.global.subnet_prefix = dev->gid_prefix;
-	hdr->sgid.global.interface_id = dev->dd->ipath_guid;
-	hdr->dgid = grh->dgid;
-
-	/* GRH header size in 32-bit words. */
-	return sizeof(struct ib_grh) / sizeof(u32);
-}
-
-void ipath_make_ruc_header(struct ipath_ibdev *dev, struct ipath_qp *qp,
-			   struct ipath_other_headers *ohdr,
-			   u32 bth0, u32 bth2)
-{
-	u16 lrh0;
-	u32 nwords;
-	u32 extra_bytes;
-
-	/* Construct the header. */
-	extra_bytes = -qp->s_cur_size & 3;
-	nwords = (qp->s_cur_size + extra_bytes) >> 2;
-	lrh0 = IPATH_LRH_BTH;
-	if (unlikely(qp->remote_ah_attr.ah_flags & IB_AH_GRH)) {
-		qp->s_hdrwords += ipath_make_grh(dev, &qp->s_hdr.u.l.grh,
-						 &qp->remote_ah_attr.grh,
-						 qp->s_hdrwords, nwords);
-		lrh0 = IPATH_LRH_GRH;
-	}
-	lrh0 |= qp->remote_ah_attr.sl << 4;
-	qp->s_hdr.lrh[0] = cpu_to_be16(lrh0);
-	qp->s_hdr.lrh[1] = cpu_to_be16(qp->remote_ah_attr.dlid);
-	qp->s_hdr.lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords + SIZE_OF_CRC);
-	qp->s_hdr.lrh[3] = cpu_to_be16(dev->dd->ipath_lid |
-				       qp->remote_ah_attr.src_path_bits);
-	bth0 |= ipath_get_pkey(dev->dd, qp->s_pkey_index);
-	bth0 |= extra_bytes << 20;
-	ohdr->bth[0] = cpu_to_be32(bth0 | (1 << 22));
-	ohdr->bth[1] = cpu_to_be32(qp->remote_qpn);
-	ohdr->bth[2] = cpu_to_be32(bth2);
-}
-
-/**
- * ipath_do_send - perform a send on a QP
- * @data: 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).
- * Otherwise, two threads could send packets out of order.
- */
-void ipath_do_send(unsigned long data)
-{
-	struct ipath_qp *qp = (struct ipath_qp *)data;
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	int (*make_req)(struct ipath_qp *qp);
-	unsigned long flags;
-
-	if ((qp->ibqp.qp_type == IB_QPT_RC ||
-	     qp->ibqp.qp_type == IB_QPT_UC) &&
-	    qp->remote_ah_attr.dlid == dev->dd->ipath_lid) {
-		ipath_ruc_loopback(qp);
-		goto bail;
-	}
-
-	if (qp->ibqp.qp_type == IB_QPT_RC)
-	       make_req = ipath_make_rc_req;
-	else if (qp->ibqp.qp_type == IB_QPT_UC)
-	       make_req = ipath_make_uc_req;
-	else
-	       make_req = ipath_make_ud_req;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	/* Return if we are already busy processing a work request. */
-	if ((qp->s_flags & (IPATH_S_BUSY | IPATH_S_ANY_WAIT)) ||
-	    !(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_OR_FLUSH_SEND)) {
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-		goto bail;
-	}
-
-	qp->s_flags |= IPATH_S_BUSY;
-
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-
-again:
-	/* Check for a constructed packet to be sent. */
-	if (qp->s_hdrwords != 0) {
-		/*
-		 * If no PIO bufs are available, return.  An interrupt will
-		 * call ipath_ib_piobufavail() when one is available.
-		 */
-		if (ipath_verbs_send(qp, &qp->s_hdr, qp->s_hdrwords,
-				     qp->s_cur_sge, qp->s_cur_size)) {
-			if (ipath_no_bufs_available(qp, dev))
-				goto bail;
-		}
-		dev->n_unicast_xmit++;
-		/* Record that we sent the packet and s_hdr is empty. */
-		qp->s_hdrwords = 0;
-	}
-
-	if (make_req(qp))
-		goto again;
-
-bail:;
-}
-
-/*
- * This should be called with s_lock held.
- */
-void ipath_send_complete(struct ipath_qp *qp, struct ipath_swqe *wqe,
-			 enum ib_wc_status status)
-{
-	u32 old_last, last;
-
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_OR_FLUSH_SEND))
-		return;
-
-	/* See ch. 11.2.4.1 and 10.7.3.1 */
-	if (!(qp->s_flags & IPATH_S_SIGNAL_REQ_WR) ||
-	    (wqe->wr.send_flags & IB_SEND_SIGNALED) ||
-	    status != IB_WC_SUCCESS) {
-		struct ib_wc wc;
-
-		memset(&wc, 0, sizeof wc);
-		wc.wr_id = wqe->wr.wr_id;
-		wc.status = status;
-		wc.opcode = ib_ipath_wc_opcode[wqe->wr.opcode];
-		wc.qp = &qp->ibqp;
-		if (status == IB_WC_SUCCESS)
-			wc.byte_len = wqe->length;
-		ipath_cq_enter(to_icq(qp->ibqp.send_cq), &wc,
-			       status != IB_WC_SUCCESS);
-	}
-
-	old_last = last = qp->s_last;
-	if (++last >= qp->s_size)
-		last = 0;
-	qp->s_last = last;
-	if (qp->s_cur == old_last)
-		qp->s_cur = last;
-	if (qp->s_tail == old_last)
-		qp->s_tail = last;
-	if (qp->state == IB_QPS_SQD && last == qp->s_cur)
-		qp->s_draining = 0;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_sdma.c b/drivers/staging/rdma/ipath/ipath_sdma.c
deleted file mode 100644
index 1ffc06abf9da..000000000000
--- a/drivers/staging/rdma/ipath/ipath_sdma.c
+++ /dev/null
@@ -1,818 +0,0 @@
-/*
- * Copyright (c) 2007, 2008 QLogic Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include <linux/spinlock.h>
-#include <linux/gfp.h>
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-#include "ipath_common.h"
-
-#define SDMA_DESCQ_SZ PAGE_SIZE /* 256 entries per 4KB page */
-
-static void vl15_watchdog_enq(struct ipath_devdata *dd)
-{
-	/* ipath_sdma_lock must already be held */
-	if (atomic_inc_return(&dd->ipath_sdma_vl15_count) == 1) {
-		unsigned long interval = (HZ + 19) / 20;
-		dd->ipath_sdma_vl15_timer.expires = jiffies + interval;
-		add_timer(&dd->ipath_sdma_vl15_timer);
-	}
-}
-
-static void vl15_watchdog_deq(struct ipath_devdata *dd)
-{
-	/* ipath_sdma_lock must already be held */
-	if (atomic_dec_return(&dd->ipath_sdma_vl15_count) != 0) {
-		unsigned long interval = (HZ + 19) / 20;
-		mod_timer(&dd->ipath_sdma_vl15_timer, jiffies + interval);
-	} else {
-		del_timer(&dd->ipath_sdma_vl15_timer);
-	}
-}
-
-static void vl15_watchdog_timeout(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *)opaque;
-
-	if (atomic_read(&dd->ipath_sdma_vl15_count) != 0) {
-		ipath_dbg("vl15 watchdog timeout - clearing\n");
-		ipath_cancel_sends(dd, 1);
-		ipath_hol_down(dd);
-	} else {
-		ipath_dbg("vl15 watchdog timeout - "
-			  "condition already cleared\n");
-	}
-}
-
-static void unmap_desc(struct ipath_devdata *dd, unsigned head)
-{
-	__le64 *descqp = &dd->ipath_sdma_descq[head].qw[0];
-	u64 desc[2];
-	dma_addr_t addr;
-	size_t len;
-
-	desc[0] = le64_to_cpu(descqp[0]);
-	desc[1] = le64_to_cpu(descqp[1]);
-
-	addr = (desc[1] << 32) | (desc[0] >> 32);
-	len = (desc[0] >> 14) & (0x7ffULL << 2);
-	dma_unmap_single(&dd->pcidev->dev, addr, len, DMA_TO_DEVICE);
-}
-
-/*
- * ipath_sdma_lock should be locked before calling this.
- */
-int ipath_sdma_make_progress(struct ipath_devdata *dd)
-{
-	struct list_head *lp = NULL;
-	struct ipath_sdma_txreq *txp = NULL;
-	u16 dmahead;
-	u16 start_idx = 0;
-	int progress = 0;
-
-	if (!list_empty(&dd->ipath_sdma_activelist)) {
-		lp = dd->ipath_sdma_activelist.next;
-		txp = list_entry(lp, struct ipath_sdma_txreq, list);
-		start_idx = txp->start_idx;
-	}
-
-	/*
-	 * Read the SDMA head register in order to know that the
-	 * interrupt clear has been written to the chip.
-	 * Otherwise, we may not get an interrupt for the last
-	 * descriptor in the queue.
-	 */
-	dmahead = (u16)ipath_read_kreg32(dd, dd->ipath_kregs->kr_senddmahead);
-	/* sanity check return value for error handling (chip reset, etc.) */
-	if (dmahead >= dd->ipath_sdma_descq_cnt)
-		goto done;
-
-	while (dd->ipath_sdma_descq_head != dmahead) {
-		if (txp && txp->flags & IPATH_SDMA_TXREQ_F_FREEDESC &&
-		    dd->ipath_sdma_descq_head == start_idx) {
-			unmap_desc(dd, dd->ipath_sdma_descq_head);
-			start_idx++;
-			if (start_idx == dd->ipath_sdma_descq_cnt)
-				start_idx = 0;
-		}
-
-		/* increment free count and head */
-		dd->ipath_sdma_descq_removed++;
-		if (++dd->ipath_sdma_descq_head == dd->ipath_sdma_descq_cnt)
-			dd->ipath_sdma_descq_head = 0;
-
-		if (txp && txp->next_descq_idx == dd->ipath_sdma_descq_head) {
-			/* move to notify list */
-			if (txp->flags & IPATH_SDMA_TXREQ_F_VL15)
-				vl15_watchdog_deq(dd);
-			list_move_tail(lp, &dd->ipath_sdma_notifylist);
-			if (!list_empty(&dd->ipath_sdma_activelist)) {
-				lp = dd->ipath_sdma_activelist.next;
-				txp = list_entry(lp, struct ipath_sdma_txreq,
-						 list);
-				start_idx = txp->start_idx;
-			} else {
-				lp = NULL;
-				txp = NULL;
-			}
-		}
-		progress = 1;
-	}
-
-	if (progress)
-		tasklet_hi_schedule(&dd->ipath_sdma_notify_task);
-
-done:
-	return progress;
-}
-
-static void ipath_sdma_notify(struct ipath_devdata *dd, struct list_head *list)
-{
-	struct ipath_sdma_txreq *txp, *txp_next;
-
-	list_for_each_entry_safe(txp, txp_next, list, list) {
-		list_del_init(&txp->list);
-
-		if (txp->callback)
-			(*txp->callback)(txp->callback_cookie,
-					 txp->callback_status);
-	}
-}
-
-static void sdma_notify_taskbody(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-	struct list_head list;
-
-	INIT_LIST_HEAD(&list);
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-	list_splice_init(&dd->ipath_sdma_notifylist, &list);
-
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	ipath_sdma_notify(dd, &list);
-
-	/*
-	 * The IB verbs layer needs to see the callback before getting
-	 * the call to ipath_ib_piobufavail() because the callback
-	 * handles releasing resources the next send will need.
-	 * Otherwise, we could do these calls in
-	 * ipath_sdma_make_progress().
-	 */
-	ipath_ib_piobufavail(dd->verbs_dev);
-}
-
-static void sdma_notify_task(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *)opaque;
-
-	if (!test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-		sdma_notify_taskbody(dd);
-}
-
-static void dump_sdma_state(struct ipath_devdata *dd)
-{
-	unsigned long reg;
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmastatus);
-	ipath_cdbg(VERBOSE, "kr_senddmastatus: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_sendctrl);
-	ipath_cdbg(VERBOSE, "kr_sendctrl: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmabufmask0);
-	ipath_cdbg(VERBOSE, "kr_senddmabufmask0: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmabufmask1);
-	ipath_cdbg(VERBOSE, "kr_senddmabufmask1: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmabufmask2);
-	ipath_cdbg(VERBOSE, "kr_senddmabufmask2: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmatail);
-	ipath_cdbg(VERBOSE, "kr_senddmatail: 0x%016lx\n", reg);
-
-	reg = ipath_read_kreg64(dd, dd->ipath_kregs->kr_senddmahead);
-	ipath_cdbg(VERBOSE, "kr_senddmahead: 0x%016lx\n", reg);
-}
-
-static void sdma_abort_task(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *) opaque;
-	u64 status;
-	unsigned long flags;
-
-	if (test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-		return;
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-	status = dd->ipath_sdma_status & IPATH_SDMA_ABORT_MASK;
-
-	/* nothing to do */
-	if (status == IPATH_SDMA_ABORT_NONE)
-		goto unlock;
-
-	/* ipath_sdma_abort() is done, waiting for interrupt */
-	if (status == IPATH_SDMA_ABORT_DISARMED) {
-		if (time_before(jiffies, dd->ipath_sdma_abort_intr_timeout))
-			goto resched_noprint;
-		/* give up, intr got lost somewhere */
-		ipath_dbg("give up waiting for SDMADISABLED intr\n");
-		__set_bit(IPATH_SDMA_DISABLED, &dd->ipath_sdma_status);
-		status = IPATH_SDMA_ABORT_ABORTED;
-	}
-
-	/* everything is stopped, time to clean up and restart */
-	if (status == IPATH_SDMA_ABORT_ABORTED) {
-		struct ipath_sdma_txreq *txp, *txpnext;
-		u64 hwstatus;
-		int notify = 0;
-
-		hwstatus = ipath_read_kreg64(dd,
-				dd->ipath_kregs->kr_senddmastatus);
-
-		if ((hwstatus & (IPATH_SDMA_STATUS_SCORE_BOARD_DRAIN_IN_PROG |
-				 IPATH_SDMA_STATUS_ABORT_IN_PROG	     |
-				 IPATH_SDMA_STATUS_INTERNAL_SDMA_ENABLE)) ||
-		    !(hwstatus & IPATH_SDMA_STATUS_SCB_EMPTY)) {
-			if (dd->ipath_sdma_reset_wait > 0) {
-				/* not done shutting down sdma */
-				--dd->ipath_sdma_reset_wait;
-				goto resched;
-			}
-			ipath_cdbg(VERBOSE, "gave up waiting for quiescent "
-				"status after SDMA reset, continuing\n");
-			dump_sdma_state(dd);
-		}
-
-		/* dequeue all "sent" requests */
-		list_for_each_entry_safe(txp, txpnext,
-					 &dd->ipath_sdma_activelist, list) {
-			txp->callback_status = IPATH_SDMA_TXREQ_S_ABORTED;
-			if (txp->flags & IPATH_SDMA_TXREQ_F_VL15)
-				vl15_watchdog_deq(dd);
-			list_move_tail(&txp->list, &dd->ipath_sdma_notifylist);
-			notify = 1;
-		}
-		if (notify)
-			tasklet_hi_schedule(&dd->ipath_sdma_notify_task);
-
-		/* reset our notion of head and tail */
-		dd->ipath_sdma_descq_tail = 0;
-		dd->ipath_sdma_descq_head = 0;
-		dd->ipath_sdma_head_dma[0] = 0;
-		dd->ipath_sdma_generation = 0;
-		dd->ipath_sdma_descq_removed = dd->ipath_sdma_descq_added;
-
-		/* Reset SendDmaLenGen */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmalengen,
-			(u64) dd->ipath_sdma_descq_cnt | (1ULL << 18));
-
-		/* done with sdma state for a bit */
-		spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-		/*
-		 * Don't restart sdma here (with the exception
-		 * below). Wait until link is up to ACTIVE.  VL15 MADs
-		 * used to bring the link up use PIO, and multiple link
-		 * transitions otherwise cause the sdma engine to be
-		 * stopped and started multiple times.
-		 * The disable is done here, including the shadow,
-		 * so the state is kept consistent.
-		 * See ipath_restart_sdma() for the actual starting
-		 * of sdma.
-		 */
-		spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-		dd->ipath_sendctrl &= ~INFINIPATH_S_SDMAENABLE;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-				 dd->ipath_sendctrl);
-		ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-		spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-		/* make sure I see next message */
-		dd->ipath_sdma_abort_jiffies = 0;
-
-		/*
-		 * Not everything that takes SDMA offline is a link
-		 * status change.  If the link was up, restart SDMA.
-		 */
-		if (dd->ipath_flags & IPATH_LINKACTIVE)
-			ipath_restart_sdma(dd);
-
-		goto done;
-	}
-
-resched:
-	/*
-	 * for now, keep spinning
-	 * JAG - this is bad to just have default be a loop without
-	 * state change
-	 */
-	if (time_after(jiffies, dd->ipath_sdma_abort_jiffies)) {
-		ipath_dbg("looping with status 0x%08lx\n",
-			  dd->ipath_sdma_status);
-		dd->ipath_sdma_abort_jiffies = jiffies + 5 * HZ;
-	}
-resched_noprint:
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-	if (!test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-		tasklet_hi_schedule(&dd->ipath_sdma_abort_task);
-	return;
-
-unlock:
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-done:
-	return;
-}
-
-/*
- * This is called from interrupt context.
- */
-void ipath_sdma_intr(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-	(void) ipath_sdma_make_progress(dd);
-
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-}
-
-static int alloc_sdma(struct ipath_devdata *dd)
-{
-	int ret = 0;
-
-	/* Allocate memory for SendDMA descriptor FIFO */
-	dd->ipath_sdma_descq = dma_alloc_coherent(&dd->pcidev->dev,
-		SDMA_DESCQ_SZ, &dd->ipath_sdma_descq_phys, GFP_KERNEL);
-
-	if (!dd->ipath_sdma_descq) {
-		ipath_dev_err(dd, "failed to allocate SendDMA descriptor "
-			"FIFO memory\n");
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	dd->ipath_sdma_descq_cnt =
-		SDMA_DESCQ_SZ / sizeof(struct ipath_sdma_desc);
-
-	/* Allocate memory for DMA of head register to memory */
-	dd->ipath_sdma_head_dma = dma_alloc_coherent(&dd->pcidev->dev,
-		PAGE_SIZE, &dd->ipath_sdma_head_phys, GFP_KERNEL);
-	if (!dd->ipath_sdma_head_dma) {
-		ipath_dev_err(dd, "failed to allocate SendDMA head memory\n");
-		ret = -ENOMEM;
-		goto cleanup_descq;
-	}
-	dd->ipath_sdma_head_dma[0] = 0;
-
-	setup_timer(&dd->ipath_sdma_vl15_timer, vl15_watchdog_timeout,
-			(unsigned long)dd);
-
-	atomic_set(&dd->ipath_sdma_vl15_count, 0);
-
-	goto done;
-
-cleanup_descq:
-	dma_free_coherent(&dd->pcidev->dev, SDMA_DESCQ_SZ,
-		(void *)dd->ipath_sdma_descq, dd->ipath_sdma_descq_phys);
-	dd->ipath_sdma_descq = NULL;
-	dd->ipath_sdma_descq_phys = 0;
-done:
-	return ret;
-}
-
-int setup_sdma(struct ipath_devdata *dd)
-{
-	int ret = 0;
-	unsigned i, n;
-	u64 tmp64;
-	u64 senddmabufmask[3] = { 0 };
-	unsigned long flags;
-
-	ret = alloc_sdma(dd);
-	if (ret)
-		goto done;
-
-	if (!dd->ipath_sdma_descq) {
-		ipath_dev_err(dd, "SendDMA memory not allocated\n");
-		goto done;
-	}
-
-	/*
-	 * Set initial status as if we had been up, then gone down.
-	 * This lets initial start on transition to ACTIVE be the
-	 * same as restart after link flap.
-	 */
-	dd->ipath_sdma_status = IPATH_SDMA_ABORT_ABORTED;
-	dd->ipath_sdma_abort_jiffies = 0;
-	dd->ipath_sdma_generation = 0;
-	dd->ipath_sdma_descq_tail = 0;
-	dd->ipath_sdma_descq_head = 0;
-	dd->ipath_sdma_descq_removed = 0;
-	dd->ipath_sdma_descq_added = 0;
-
-	/* Set SendDmaBase */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabase,
-			 dd->ipath_sdma_descq_phys);
-	/* Set SendDmaLenGen */
-	tmp64 = dd->ipath_sdma_descq_cnt;
-	tmp64 |= 1<<18; /* enable generation checking */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmalengen, tmp64);
-	/* Set SendDmaTail */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmatail,
-			 dd->ipath_sdma_descq_tail);
-	/* Set SendDmaHeadAddr */
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmaheadaddr,
-			 dd->ipath_sdma_head_phys);
-
-	/*
-	 * Reserve all the former "kernel" piobufs, using high number range
-	 * so we get as many 4K buffers as possible
-	 */
-	n = dd->ipath_piobcnt2k + dd->ipath_piobcnt4k;
-	i = dd->ipath_lastport_piobuf + dd->ipath_pioreserved;
-	ipath_chg_pioavailkernel(dd, i, n - i , 0);
-	for (; i < n; ++i) {
-		unsigned word = i / 64;
-		unsigned bit = i & 63;
-		BUG_ON(word >= 3);
-		senddmabufmask[word] |= 1ULL << bit;
-	}
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask0,
-			 senddmabufmask[0]);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask1,
-			 senddmabufmask[1]);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask2,
-			 senddmabufmask[2]);
-
-	INIT_LIST_HEAD(&dd->ipath_sdma_activelist);
-	INIT_LIST_HEAD(&dd->ipath_sdma_notifylist);
-
-	tasklet_init(&dd->ipath_sdma_notify_task, sdma_notify_task,
-		     (unsigned long) dd);
-	tasklet_init(&dd->ipath_sdma_abort_task, sdma_abort_task,
-		     (unsigned long) dd);
-
-	/*
-	 * No use to turn on SDMA here, as link is probably not ACTIVE
-	 * Just mark it RUNNING and enable the interrupt, and let the
-	 * ipath_restart_sdma() on link transition to ACTIVE actually
-	 * enable it.
-	 */
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl |= INFINIPATH_S_SDMAINTENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	__set_bit(IPATH_SDMA_RUNNING, &dd->ipath_sdma_status);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-done:
-	return ret;
-}
-
-void teardown_sdma(struct ipath_devdata *dd)
-{
-	struct ipath_sdma_txreq *txp, *txpnext;
-	unsigned long flags;
-	dma_addr_t sdma_head_phys = 0;
-	dma_addr_t sdma_descq_phys = 0;
-	void *sdma_descq = NULL;
-	void *sdma_head_dma = NULL;
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-	__clear_bit(IPATH_SDMA_RUNNING, &dd->ipath_sdma_status);
-	__set_bit(IPATH_SDMA_ABORTING, &dd->ipath_sdma_status);
-	__set_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status);
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	tasklet_kill(&dd->ipath_sdma_abort_task);
-	tasklet_kill(&dd->ipath_sdma_notify_task);
-
-	/* turn off sdma */
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	dd->ipath_sendctrl &= ~INFINIPATH_S_SDMAENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl,
-		dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-	/* dequeue all "sent" requests */
-	list_for_each_entry_safe(txp, txpnext, &dd->ipath_sdma_activelist,
-				 list) {
-		txp->callback_status = IPATH_SDMA_TXREQ_S_SHUTDOWN;
-		if (txp->flags & IPATH_SDMA_TXREQ_F_VL15)
-			vl15_watchdog_deq(dd);
-		list_move_tail(&txp->list, &dd->ipath_sdma_notifylist);
-	}
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	sdma_notify_taskbody(dd);
-
-	del_timer_sync(&dd->ipath_sdma_vl15_timer);
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-	dd->ipath_sdma_abort_jiffies = 0;
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabase, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmalengen, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmatail, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmaheadaddr, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask0, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask1, 0);
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmabufmask2, 0);
-
-	if (dd->ipath_sdma_head_dma) {
-		sdma_head_dma = (void *) dd->ipath_sdma_head_dma;
-		sdma_head_phys = dd->ipath_sdma_head_phys;
-		dd->ipath_sdma_head_dma = NULL;
-		dd->ipath_sdma_head_phys = 0;
-	}
-
-	if (dd->ipath_sdma_descq) {
-		sdma_descq = dd->ipath_sdma_descq;
-		sdma_descq_phys = dd->ipath_sdma_descq_phys;
-		dd->ipath_sdma_descq = NULL;
-		dd->ipath_sdma_descq_phys = 0;
-	}
-
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	if (sdma_head_dma)
-		dma_free_coherent(&dd->pcidev->dev, PAGE_SIZE,
-				  sdma_head_dma, sdma_head_phys);
-
-	if (sdma_descq)
-		dma_free_coherent(&dd->pcidev->dev, SDMA_DESCQ_SZ,
-				  sdma_descq, sdma_descq_phys);
-}
-
-/*
- * [Re]start SDMA, if we use it, and it's not already OK.
- * This is called on transition to link ACTIVE, either the first or
- * subsequent times.
- */
-void ipath_restart_sdma(struct ipath_devdata *dd)
-{
-	unsigned long flags;
-	int needed = 1;
-
-	if (!(dd->ipath_flags & IPATH_HAS_SEND_DMA))
-		goto bail;
-
-	/*
-	 * First, make sure we should, which is to say,
-	 * check that we are "RUNNING" (not in teardown)
-	 * and not "SHUTDOWN"
-	 */
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-	if (!test_bit(IPATH_SDMA_RUNNING, &dd->ipath_sdma_status)
-		|| test_bit(IPATH_SDMA_SHUTDOWN, &dd->ipath_sdma_status))
-			needed = 0;
-	else {
-		__clear_bit(IPATH_SDMA_DISABLED, &dd->ipath_sdma_status);
-		__clear_bit(IPATH_SDMA_DISARMED, &dd->ipath_sdma_status);
-		__clear_bit(IPATH_SDMA_ABORTING, &dd->ipath_sdma_status);
-	}
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-	if (!needed) {
-		ipath_dbg("invalid attempt to restart SDMA, status 0x%08lx\n",
-			dd->ipath_sdma_status);
-		goto bail;
-	}
-	spin_lock_irqsave(&dd->ipath_sendctrl_lock, flags);
-	/*
-	 * First clear, just to be safe. Enable is only done
-	 * in chip on 0->1 transition
-	 */
-	dd->ipath_sendctrl &= ~INFINIPATH_S_SDMAENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	dd->ipath_sendctrl |= INFINIPATH_S_SDMAENABLE;
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_sendctrl, dd->ipath_sendctrl);
-	ipath_read_kreg64(dd, dd->ipath_kregs->kr_scratch);
-	spin_unlock_irqrestore(&dd->ipath_sendctrl_lock, flags);
-
-	/* notify upper layers */
-	ipath_ib_piobufavail(dd->verbs_dev);
-
-bail:
-	return;
-}
-
-static inline void make_sdma_desc(struct ipath_devdata *dd,
-	u64 *sdmadesc, u64 addr, u64 dwlen, u64 dwoffset)
-{
-	WARN_ON(addr & 3);
-	/* SDmaPhyAddr[47:32] */
-	sdmadesc[1] = addr >> 32;
-	/* SDmaPhyAddr[31:0] */
-	sdmadesc[0] = (addr & 0xfffffffcULL) << 32;
-	/* SDmaGeneration[1:0] */
-	sdmadesc[0] |= (dd->ipath_sdma_generation & 3ULL) << 30;
-	/* SDmaDwordCount[10:0] */
-	sdmadesc[0] |= (dwlen & 0x7ffULL) << 16;
-	/* SDmaBufOffset[12:2] */
-	sdmadesc[0] |= dwoffset & 0x7ffULL;
-}
-
-/*
- * This function queues one IB packet onto the send DMA queue per call.
- * The caller is responsible for checking:
- * 1) The number of send DMA descriptor entries is less than the size of
- *    the descriptor queue.
- * 2) The IB SGE addresses and lengths are 32-bit aligned
- *    (except possibly the last SGE's length)
- * 3) The SGE addresses are suitable for passing to dma_map_single().
- */
-int ipath_sdma_verbs_send(struct ipath_devdata *dd,
-	struct ipath_sge_state *ss, u32 dwords,
-	struct ipath_verbs_txreq *tx)
-{
-
-	unsigned long flags;
-	struct ipath_sge *sge;
-	int ret = 0;
-	u16 tail;
-	__le64 *descqp;
-	u64 sdmadesc[2];
-	u32 dwoffset;
-	dma_addr_t addr;
-
-	if ((tx->map_len + (dwords<<2)) > dd->ipath_ibmaxlen) {
-		ipath_dbg("packet size %X > ibmax %X, fail\n",
-			tx->map_len + (dwords<<2), dd->ipath_ibmaxlen);
-		ret = -EMSGSIZE;
-		goto fail;
-	}
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-retry:
-	if (unlikely(test_bit(IPATH_SDMA_ABORTING, &dd->ipath_sdma_status))) {
-		ret = -EBUSY;
-		goto unlock;
-	}
-
-	if (tx->txreq.sg_count > ipath_sdma_descq_freecnt(dd)) {
-		if (ipath_sdma_make_progress(dd))
-			goto retry;
-		ret = -ENOBUFS;
-		goto unlock;
-	}
-
-	addr = dma_map_single(&dd->pcidev->dev, tx->txreq.map_addr,
-			      tx->map_len, DMA_TO_DEVICE);
-	if (dma_mapping_error(&dd->pcidev->dev, addr))
-		goto ioerr;
-
-	dwoffset = tx->map_len >> 2;
-	make_sdma_desc(dd, sdmadesc, (u64) addr, dwoffset, 0);
-
-	/* SDmaFirstDesc */
-	sdmadesc[0] |= 1ULL << 12;
-	if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_USELARGEBUF)
-		sdmadesc[0] |= 1ULL << 14;	/* SDmaUseLargeBuf */
-
-	/* write to the descq */
-	tail = dd->ipath_sdma_descq_tail;
-	descqp = &dd->ipath_sdma_descq[tail].qw[0];
-	*descqp++ = cpu_to_le64(sdmadesc[0]);
-	*descqp++ = cpu_to_le64(sdmadesc[1]);
-
-	if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_FREEDESC)
-		tx->txreq.start_idx = tail;
-
-	/* increment the tail */
-	if (++tail == dd->ipath_sdma_descq_cnt) {
-		tail = 0;
-		descqp = &dd->ipath_sdma_descq[0].qw[0];
-		++dd->ipath_sdma_generation;
-	}
-
-	sge = &ss->sge;
-	while (dwords) {
-		u32 dw;
-		u32 len;
-
-		len = dwords << 2;
-		if (len > sge->length)
-			len = sge->length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		dw = (len + 3) >> 2;
-		addr = dma_map_single(&dd->pcidev->dev, sge->vaddr, dw << 2,
-				      DMA_TO_DEVICE);
-		if (dma_mapping_error(&dd->pcidev->dev, addr))
-			goto unmap;
-		make_sdma_desc(dd, sdmadesc, (u64) addr, dw, dwoffset);
-		/* SDmaUseLargeBuf has to be set in every descriptor */
-		if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_USELARGEBUF)
-			sdmadesc[0] |= 1ULL << 14;
-		/* write to the descq */
-		*descqp++ = cpu_to_le64(sdmadesc[0]);
-		*descqp++ = cpu_to_le64(sdmadesc[1]);
-
-		/* increment the tail */
-		if (++tail == dd->ipath_sdma_descq_cnt) {
-			tail = 0;
-			descqp = &dd->ipath_sdma_descq[0].qw[0];
-			++dd->ipath_sdma_generation;
-		}
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--ss->num_sge)
-				*sge = *ss->sg_list++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-
-		dwoffset += dw;
-		dwords -= dw;
-	}
-
-	if (!tail)
-		descqp = &dd->ipath_sdma_descq[dd->ipath_sdma_descq_cnt].qw[0];
-	descqp -= 2;
-	/* SDmaLastDesc */
-	descqp[0] |= cpu_to_le64(1ULL << 11);
-	if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_INTREQ) {
-		/* SDmaIntReq */
-		descqp[0] |= cpu_to_le64(1ULL << 15);
-	}
-
-	/* Commit writes to memory and advance the tail on the chip */
-	wmb();
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmatail, tail);
-
-	tx->txreq.next_descq_idx = tail;
-	tx->txreq.callback_status = IPATH_SDMA_TXREQ_S_OK;
-	dd->ipath_sdma_descq_tail = tail;
-	dd->ipath_sdma_descq_added += tx->txreq.sg_count;
-	list_add_tail(&tx->txreq.list, &dd->ipath_sdma_activelist);
-	if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_VL15)
-		vl15_watchdog_enq(dd);
-	goto unlock;
-
-unmap:
-	while (tail != dd->ipath_sdma_descq_tail) {
-		if (!tail)
-			tail = dd->ipath_sdma_descq_cnt - 1;
-		else
-			tail--;
-		unmap_desc(dd, tail);
-	}
-ioerr:
-	ret = -EIO;
-unlock:
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-fail:
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_srq.c b/drivers/staging/rdma/ipath/ipath_srq.c
deleted file mode 100644
index 26271984b717..000000000000
--- a/drivers/staging/rdma/ipath/ipath_srq.c
+++ /dev/null
@@ -1,380 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/err.h>
-#include <linux/slab.h>
-#include <linux/vmalloc.h>
-
-#include "ipath_verbs.h"
-
-/**
- * ipath_post_srq_receive - post a receive on a shared receive queue
- * @ibsrq: the SRQ to post the receive on
- * @wr: the list of work requests to post
- * @bad_wr: the first WR to cause a problem is put here
- *
- * This may be called from interrupt context.
- */
-int ipath_post_srq_receive(struct ib_srq *ibsrq, struct ib_recv_wr *wr,
-			   struct ib_recv_wr **bad_wr)
-{
-	struct ipath_srq *srq = to_isrq(ibsrq);
-	struct ipath_rwq *wq;
-	unsigned long flags;
-	int ret;
-
-	for (; wr; wr = wr->next) {
-		struct ipath_rwqe *wqe;
-		u32 next;
-		int i;
-
-		if ((unsigned) wr->num_sge > srq->rq.max_sge) {
-			*bad_wr = wr;
-			ret = -EINVAL;
-			goto bail;
-		}
-
-		spin_lock_irqsave(&srq->rq.lock, flags);
-		wq = srq->rq.wq;
-		next = wq->head + 1;
-		if (next >= srq->rq.size)
-			next = 0;
-		if (next == wq->tail) {
-			spin_unlock_irqrestore(&srq->rq.lock, flags);
-			*bad_wr = wr;
-			ret = -ENOMEM;
-			goto bail;
-		}
-
-		wqe = get_rwqe_ptr(&srq->rq, wq->head);
-		wqe->wr_id = wr->wr_id;
-		wqe->num_sge = wr->num_sge;
-		for (i = 0; i < wr->num_sge; i++)
-			wqe->sg_list[i] = wr->sg_list[i];
-		/* Make sure queue entry is written before the head index. */
-		smp_wmb();
-		wq->head = next;
-		spin_unlock_irqrestore(&srq->rq.lock, flags);
-	}
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_create_srq - create a shared receive queue
- * @ibpd: the protection domain of the SRQ to create
- * @srq_init_attr: the attributes of the SRQ
- * @udata: data from libipathverbs when creating a user SRQ
- */
-struct ib_srq *ipath_create_srq(struct ib_pd *ibpd,
-				struct ib_srq_init_attr *srq_init_attr,
-				struct ib_udata *udata)
-{
-	struct ipath_ibdev *dev = to_idev(ibpd->device);
-	struct ipath_srq *srq;
-	u32 sz;
-	struct ib_srq *ret;
-
-	if (srq_init_attr->srq_type != IB_SRQT_BASIC) {
-		ret = ERR_PTR(-ENOSYS);
-		goto done;
-	}
-
-	if (srq_init_attr->attr.max_wr == 0) {
-		ret = ERR_PTR(-EINVAL);
-		goto done;
-	}
-
-	if ((srq_init_attr->attr.max_sge > ib_ipath_max_srq_sges) ||
-	    (srq_init_attr->attr.max_wr > ib_ipath_max_srq_wrs)) {
-		ret = ERR_PTR(-EINVAL);
-		goto done;
-	}
-
-	srq = kmalloc(sizeof(*srq), GFP_KERNEL);
-	if (!srq) {
-		ret = ERR_PTR(-ENOMEM);
-		goto done;
-	}
-
-	/*
-	 * Need to use vmalloc() if we want to support large #s of entries.
-	 */
-	srq->rq.size = srq_init_attr->attr.max_wr + 1;
-	srq->rq.max_sge = srq_init_attr->attr.max_sge;
-	sz = sizeof(struct ib_sge) * srq->rq.max_sge +
-		sizeof(struct ipath_rwqe);
-	srq->rq.wq = vmalloc_user(sizeof(struct ipath_rwq) + srq->rq.size * sz);
-	if (!srq->rq.wq) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail_srq;
-	}
-
-	/*
-	 * Return the address of the RWQ as the offset to mmap.
-	 * See ipath_mmap() for details.
-	 */
-	if (udata && udata->outlen >= sizeof(__u64)) {
-		int err;
-		u32 s = sizeof(struct ipath_rwq) + srq->rq.size * sz;
-
-		srq->ip =
-		    ipath_create_mmap_info(dev, s,
-					   ibpd->uobject->context,
-					   srq->rq.wq);
-		if (!srq->ip) {
-			ret = ERR_PTR(-ENOMEM);
-			goto bail_wq;
-		}
-
-		err = ib_copy_to_udata(udata, &srq->ip->offset,
-				       sizeof(srq->ip->offset));
-		if (err) {
-			ret = ERR_PTR(err);
-			goto bail_ip;
-		}
-	} else
-		srq->ip = NULL;
-
-	/*
-	 * ib_create_srq() will initialize srq->ibsrq.
-	 */
-	spin_lock_init(&srq->rq.lock);
-	srq->rq.wq->head = 0;
-	srq->rq.wq->tail = 0;
-	srq->limit = srq_init_attr->attr.srq_limit;
-
-	spin_lock(&dev->n_srqs_lock);
-	if (dev->n_srqs_allocated == ib_ipath_max_srqs) {
-		spin_unlock(&dev->n_srqs_lock);
-		ret = ERR_PTR(-ENOMEM);
-		goto bail_ip;
-	}
-
- 	dev->n_srqs_allocated++;
-	spin_unlock(&dev->n_srqs_lock);
-
-	if (srq->ip) {
-		spin_lock_irq(&dev->pending_lock);
-		list_add(&srq->ip->pending_mmaps, &dev->pending_mmaps);
-		spin_unlock_irq(&dev->pending_lock);
-	}
-
-	ret = &srq->ibsrq;
-	goto done;
-
-bail_ip:
-	kfree(srq->ip);
-bail_wq:
-	vfree(srq->rq.wq);
-bail_srq:
-	kfree(srq);
-done:
-	return ret;
-}
-
-/**
- * ipath_modify_srq - modify a shared receive queue
- * @ibsrq: the SRQ to modify
- * @attr: the new attributes of the SRQ
- * @attr_mask: indicates which attributes to modify
- * @udata: user data for ipathverbs.so
- */
-int ipath_modify_srq(struct ib_srq *ibsrq, struct ib_srq_attr *attr,
-		     enum ib_srq_attr_mask attr_mask,
-		     struct ib_udata *udata)
-{
-	struct ipath_srq *srq = to_isrq(ibsrq);
-	struct ipath_rwq *wq;
-	int ret = 0;
-
-	if (attr_mask & IB_SRQ_MAX_WR) {
-		struct ipath_rwq *owq;
-		struct ipath_rwqe *p;
-		u32 sz, size, n, head, tail;
-
-		/* Check that the requested sizes are below the limits. */
-		if ((attr->max_wr > ib_ipath_max_srq_wrs) ||
-		    ((attr_mask & IB_SRQ_LIMIT) ?
-		     attr->srq_limit : srq->limit) > attr->max_wr) {
-			ret = -EINVAL;
-			goto bail;
-		}
-
-		sz = sizeof(struct ipath_rwqe) +
-			srq->rq.max_sge * sizeof(struct ib_sge);
-		size = attr->max_wr + 1;
-		wq = vmalloc_user(sizeof(struct ipath_rwq) + size * sz);
-		if (!wq) {
-			ret = -ENOMEM;
-			goto bail;
-		}
-
-		/* Check that we can write the offset to mmap. */
-		if (udata && udata->inlen >= sizeof(__u64)) {
-			__u64 offset_addr;
-			__u64 offset = 0;
-
-			ret = ib_copy_from_udata(&offset_addr, udata,
-						 sizeof(offset_addr));
-			if (ret)
-				goto bail_free;
-			udata->outbuf =
-				(void __user *) (unsigned long) offset_addr;
-			ret = ib_copy_to_udata(udata, &offset,
-					       sizeof(offset));
-			if (ret)
-				goto bail_free;
-		}
-
-		spin_lock_irq(&srq->rq.lock);
-		/*
-		 * validate head pointer value and compute
-		 * the number of remaining WQEs.
-		 */
-		owq = srq->rq.wq;
-		head = owq->head;
-		if (head >= srq->rq.size)
-			head = 0;
-		tail = owq->tail;
-		if (tail >= srq->rq.size)
-			tail = 0;
-		n = head;
-		if (n < tail)
-			n += srq->rq.size - tail;
-		else
-			n -= tail;
-		if (size <= n) {
-			ret = -EINVAL;
-			goto bail_unlock;
-		}
-		n = 0;
-		p = wq->wq;
-		while (tail != head) {
-			struct ipath_rwqe *wqe;
-			int i;
-
-			wqe = get_rwqe_ptr(&srq->rq, tail);
-			p->wr_id = wqe->wr_id;
-			p->num_sge = wqe->num_sge;
-			for (i = 0; i < wqe->num_sge; i++)
-				p->sg_list[i] = wqe->sg_list[i];
-			n++;
-			p = (struct ipath_rwqe *)((char *) p + sz);
-			if (++tail >= srq->rq.size)
-				tail = 0;
-		}
-		srq->rq.wq = wq;
-		srq->rq.size = size;
-		wq->head = n;
-		wq->tail = 0;
-		if (attr_mask & IB_SRQ_LIMIT)
-			srq->limit = attr->srq_limit;
-		spin_unlock_irq(&srq->rq.lock);
-
-		vfree(owq);
-
-		if (srq->ip) {
-			struct ipath_mmap_info *ip = srq->ip;
-			struct ipath_ibdev *dev = to_idev(srq->ibsrq.device);
-			u32 s = sizeof(struct ipath_rwq) + size * sz;
-
-			ipath_update_mmap_info(dev, ip, s, wq);
-
-			/*
-			 * Return the offset to mmap.
-			 * See ipath_mmap() for details.
-			 */
-			if (udata && udata->inlen >= sizeof(__u64)) {
-				ret = ib_copy_to_udata(udata, &ip->offset,
-						       sizeof(ip->offset));
-				if (ret)
-					goto bail;
-			}
-
-			spin_lock_irq(&dev->pending_lock);
-			if (list_empty(&ip->pending_mmaps))
-				list_add(&ip->pending_mmaps,
-					 &dev->pending_mmaps);
-			spin_unlock_irq(&dev->pending_lock);
-		}
-	} else if (attr_mask & IB_SRQ_LIMIT) {
-		spin_lock_irq(&srq->rq.lock);
-		if (attr->srq_limit >= srq->rq.size)
-			ret = -EINVAL;
-		else
-			srq->limit = attr->srq_limit;
-		spin_unlock_irq(&srq->rq.lock);
-	}
-	goto bail;
-
-bail_unlock:
-	spin_unlock_irq(&srq->rq.lock);
-bail_free:
-	vfree(wq);
-bail:
-	return ret;
-}
-
-int ipath_query_srq(struct ib_srq *ibsrq, struct ib_srq_attr *attr)
-{
-	struct ipath_srq *srq = to_isrq(ibsrq);
-
-	attr->max_wr = srq->rq.size - 1;
-	attr->max_sge = srq->rq.max_sge;
-	attr->srq_limit = srq->limit;
-	return 0;
-}
-
-/**
- * ipath_destroy_srq - destroy a shared receive queue
- * @ibsrq: the SRQ to destroy
- */
-int ipath_destroy_srq(struct ib_srq *ibsrq)
-{
-	struct ipath_srq *srq = to_isrq(ibsrq);
-	struct ipath_ibdev *dev = to_idev(ibsrq->device);
-
-	spin_lock(&dev->n_srqs_lock);
-	dev->n_srqs_allocated--;
-	spin_unlock(&dev->n_srqs_lock);
-	if (srq->ip)
-		kref_put(&srq->ip->ref, ipath_release_mmap_info);
-	else
-		vfree(srq->rq.wq);
-	kfree(srq);
-
-	return 0;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_stats.c b/drivers/staging/rdma/ipath/ipath_stats.c
deleted file mode 100644
index f63e143e3292..000000000000
--- a/drivers/staging/rdma/ipath/ipath_stats.c
+++ /dev/null
@@ -1,347 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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 "ipath_kernel.h"
-
-struct infinipath_stats ipath_stats;
-
-/**
- * ipath_snap_cntr - snapshot a chip counter
- * @dd: the infinipath device
- * @creg: the counter to snapshot
- *
- * called from add_timer and user counter read calls, to deal with
- * counters that wrap in "human time".  The words sent and received, and
- * the packets sent and received are all that we worry about.  For now,
- * at least, we don't worry about error counters, because if they wrap
- * that quickly, we probably don't care.  We may eventually just make this
- * handle all the counters.  word counters can wrap in about 20 seconds
- * of full bandwidth traffic, packet counters in a few hours.
- */
-
-u64 ipath_snap_cntr(struct ipath_devdata *dd, ipath_creg creg)
-{
-	u32 val, reg64 = 0;
-	u64 val64;
-	unsigned long t0, t1;
-	u64 ret;
-
-	t0 = jiffies;
-	/* If fast increment counters are only 32 bits, snapshot them,
-	 * and maintain them as 64bit values in the driver */
-	if (!(dd->ipath_flags & IPATH_32BITCOUNTERS) &&
-	    (creg == dd->ipath_cregs->cr_wordsendcnt ||
-	     creg == dd->ipath_cregs->cr_wordrcvcnt ||
-	     creg == dd->ipath_cregs->cr_pktsendcnt ||
-	     creg == dd->ipath_cregs->cr_pktrcvcnt)) {
-		val64 = ipath_read_creg(dd, creg);
-		val = val64 == ~0ULL ? ~0U : 0;
-		reg64 = 1;
-	} else			/* val64 just to keep gcc quiet... */
-		val64 = val = ipath_read_creg32(dd, creg);
-	/*
-	 * See if a second has passed.  This is just a way to detect things
-	 * that are quite broken.  Normally this should take just a few
-	 * cycles (the check is for long enough that we don't care if we get
-	 * pre-empted.)  An Opteron HT O read timeout is 4 seconds with
-	 * normal NB values
-	 */
-	t1 = jiffies;
-	if (time_before(t0 + HZ, t1) && val == -1) {
-		ipath_dev_err(dd, "Error!  Read counter 0x%x timed out\n",
-			      creg);
-		ret = 0ULL;
-		goto bail;
-	}
-	if (reg64) {
-		ret = val64;
-		goto bail;
-	}
-
-	if (creg == dd->ipath_cregs->cr_wordsendcnt) {
-		if (val != dd->ipath_lastsword) {
-			dd->ipath_sword += val - dd->ipath_lastsword;
-			dd->ipath_lastsword = val;
-		}
-		val64 = dd->ipath_sword;
-	} else if (creg == dd->ipath_cregs->cr_wordrcvcnt) {
-		if (val != dd->ipath_lastrword) {
-			dd->ipath_rword += val - dd->ipath_lastrword;
-			dd->ipath_lastrword = val;
-		}
-		val64 = dd->ipath_rword;
-	} else if (creg == dd->ipath_cregs->cr_pktsendcnt) {
-		if (val != dd->ipath_lastspkts) {
-			dd->ipath_spkts += val - dd->ipath_lastspkts;
-			dd->ipath_lastspkts = val;
-		}
-		val64 = dd->ipath_spkts;
-	} else if (creg == dd->ipath_cregs->cr_pktrcvcnt) {
-		if (val != dd->ipath_lastrpkts) {
-			dd->ipath_rpkts += val - dd->ipath_lastrpkts;
-			dd->ipath_lastrpkts = val;
-		}
-		val64 = dd->ipath_rpkts;
-	} else if (creg == dd->ipath_cregs->cr_ibsymbolerrcnt) {
-		if (dd->ibdeltainprog)
-			val64 -= val64 - dd->ibsymsnap;
-		val64 -= dd->ibsymdelta;
-	} else if (creg == dd->ipath_cregs->cr_iblinkerrrecovcnt) {
-		if (dd->ibdeltainprog)
-			val64 -= val64 - dd->iblnkerrsnap;
-		val64 -= dd->iblnkerrdelta;
-	} else
-		val64 = (u64) val;
-
-	ret = val64;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_qcheck - print delta of egrfull/hdrqfull errors for kernel ports
- * @dd: the infinipath device
- *
- * print the delta of egrfull/hdrqfull errors for kernel ports no more than
- * every 5 seconds.  User processes are printed at close, but kernel doesn't
- * close, so...  Separate routine so may call from other places someday, and
- * so function name when printed by _IPATH_INFO is meaningfull
- */
-static void ipath_qcheck(struct ipath_devdata *dd)
-{
-	static u64 last_tot_hdrqfull;
-	struct ipath_portdata *pd = dd->ipath_pd[0];
-	size_t blen = 0;
-	char buf[128];
-	u32 hdrqtail;
-
-	*buf = 0;
-	if (pd->port_hdrqfull != dd->ipath_p0_hdrqfull) {
-		blen = snprintf(buf, sizeof buf, "port 0 hdrqfull %u",
-				pd->port_hdrqfull -
-				dd->ipath_p0_hdrqfull);
-		dd->ipath_p0_hdrqfull = pd->port_hdrqfull;
-	}
-	if (ipath_stats.sps_etidfull != dd->ipath_last_tidfull) {
-		blen += snprintf(buf + blen, sizeof buf - blen,
-				 "%srcvegrfull %llu",
-				 blen ? ", " : "",
-				 (unsigned long long)
-				 (ipath_stats.sps_etidfull -
-				  dd->ipath_last_tidfull));
-		dd->ipath_last_tidfull = ipath_stats.sps_etidfull;
-	}
-
-	/*
-	 * this is actually the number of hdrq full interrupts, not actual
-	 * events, but at the moment that's mostly what I'm interested in.
-	 * Actual count, etc. is in the counters, if needed.  For production
-	 * users this won't ordinarily be printed.
-	 */
-
-	if ((ipath_debug & (__IPATH_PKTDBG | __IPATH_DBG)) &&
-	    ipath_stats.sps_hdrqfull != last_tot_hdrqfull) {
-		blen += snprintf(buf + blen, sizeof buf - blen,
-				 "%shdrqfull %llu (all ports)",
-				 blen ? ", " : "",
-				 (unsigned long long)
-				 (ipath_stats.sps_hdrqfull -
-				  last_tot_hdrqfull));
-		last_tot_hdrqfull = ipath_stats.sps_hdrqfull;
-	}
-	if (blen)
-		ipath_dbg("%s\n", buf);
-
-	hdrqtail = ipath_get_hdrqtail(pd);
-	if (pd->port_head != hdrqtail) {
-		if (dd->ipath_lastport0rcv_cnt ==
-		    ipath_stats.sps_port0pkts) {
-			ipath_cdbg(PKT, "missing rcv interrupts? "
-				   "port0 hd=%x tl=%x; port0pkts %llx; write"
-				   " hd (w/intr)\n",
-				   pd->port_head, hdrqtail,
-				   (unsigned long long)
-				   ipath_stats.sps_port0pkts);
-			ipath_write_ureg(dd, ur_rcvhdrhead, hdrqtail |
-				dd->ipath_rhdrhead_intr_off, pd->port_port);
-		}
-		dd->ipath_lastport0rcv_cnt = ipath_stats.sps_port0pkts;
-	}
-}
-
-static void ipath_chk_errormask(struct ipath_devdata *dd)
-{
-	static u32 fixed;
-	u32 ctrl;
-	unsigned long errormask;
-	unsigned long hwerrs;
-
-	if (!dd->ipath_errormask || !(dd->ipath_flags & IPATH_INITTED))
-		return;
-
-	errormask = ipath_read_kreg64(dd, dd->ipath_kregs->kr_errormask);
-
-	if (errormask == dd->ipath_errormask)
-		return;
-	fixed++;
-
-	hwerrs = ipath_read_kreg64(dd, dd->ipath_kregs->kr_hwerrstatus);
-	ctrl = ipath_read_kreg32(dd, dd->ipath_kregs->kr_control);
-
-	ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-		dd->ipath_errormask);
-
-	if ((hwerrs & dd->ipath_hwerrmask) ||
-		(ctrl & INFINIPATH_C_FREEZEMODE)) {
-		/* force re-interrupt of pending events, just in case */
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_hwerrclear, 0ULL);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_errorclear, 0ULL);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_intclear, 0ULL);
-		dev_info(&dd->pcidev->dev,
-			"errormask fixed(%u) %lx -> %lx, ctrl %x hwerr %lx\n",
-			fixed, errormask, (unsigned long)dd->ipath_errormask,
-			ctrl, hwerrs);
-	} else
-		ipath_dbg("errormask fixed(%u) %lx -> %lx, no freeze\n",
-			fixed, errormask,
-			(unsigned long)dd->ipath_errormask);
-}
-
-
-/**
- * ipath_get_faststats - get word counters from chip before they overflow
- * @opaque - contains a pointer to the infinipath device ipath_devdata
- *
- * called from add_timer
- */
-void ipath_get_faststats(unsigned long opaque)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *) opaque;
-	int i;
-	static unsigned cnt;
-	unsigned long flags;
-	u64 traffic_wds;
-
-	/*
-	 * don't access the chip while running diags, or memory diags can
-	 * fail
-	 */
-	if (!dd->ipath_kregbase || !(dd->ipath_flags & IPATH_INITTED) ||
-	    ipath_diag_inuse)
-		/* but re-arm the timer, for diags case; won't hurt other */
-		goto done;
-
-	/*
-	 * We now try to maintain a "active timer", based on traffic
-	 * exceeding a threshold, so we need to check the word-counts
-	 * even if they are 64-bit.
-	 */
-	traffic_wds = ipath_snap_cntr(dd, dd->ipath_cregs->cr_wordsendcnt) +
-		ipath_snap_cntr(dd, dd->ipath_cregs->cr_wordrcvcnt);
-	spin_lock_irqsave(&dd->ipath_eep_st_lock, flags);
-	traffic_wds -= dd->ipath_traffic_wds;
-	dd->ipath_traffic_wds += traffic_wds;
-	if (traffic_wds  >= IPATH_TRAFFIC_ACTIVE_THRESHOLD)
-		atomic_add(5, &dd->ipath_active_time); /* S/B #define */
-	spin_unlock_irqrestore(&dd->ipath_eep_st_lock, flags);
-
-	if (dd->ipath_flags & IPATH_32BITCOUNTERS) {
-		ipath_snap_cntr(dd, dd->ipath_cregs->cr_pktsendcnt);
-		ipath_snap_cntr(dd, dd->ipath_cregs->cr_pktrcvcnt);
-	}
-
-	ipath_qcheck(dd);
-
-	/*
-	 * deal with repeat error suppression.  Doesn't really matter if
-	 * last error was almost a full interval ago, or just a few usecs
-	 * ago; still won't get more than 2 per interval.  We may want
-	 * longer intervals for this eventually, could do with mod, counter
-	 * or separate timer.  Also see code in ipath_handle_errors() and
-	 * ipath_handle_hwerrors().
-	 */
-
-	if (dd->ipath_lasterror)
-		dd->ipath_lasterror = 0;
-	if (dd->ipath_lasthwerror)
-		dd->ipath_lasthwerror = 0;
-	if (dd->ipath_maskederrs
-	    && time_after(jiffies, dd->ipath_unmasktime)) {
-		char ebuf[256];
-		int iserr;
-		iserr = ipath_decode_err(dd, ebuf, sizeof ebuf,
-					 dd->ipath_maskederrs);
-		if (dd->ipath_maskederrs &
-		    ~(INFINIPATH_E_RRCVEGRFULL | INFINIPATH_E_RRCVHDRFULL |
-		      INFINIPATH_E_PKTERRS))
-			ipath_dev_err(dd, "Re-enabling masked errors "
-				      "(%s)\n", ebuf);
-		else {
-			/*
-			 * rcvegrfull and rcvhdrqfull are "normal", for some
-			 * types of processes (mostly benchmarks) that send
-			 * huge numbers of messages, while not processing
-			 * them.  So only complain about these at debug
-			 * level.
-			 */
-			if (iserr)
-				ipath_dbg(
-					"Re-enabling queue full errors (%s)\n",
-					ebuf);
-			else
-				ipath_cdbg(ERRPKT, "Re-enabling packet"
-					" problem interrupt (%s)\n", ebuf);
-		}
-
-		/* re-enable masked errors */
-		dd->ipath_errormask |= dd->ipath_maskederrs;
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_errormask,
-				 dd->ipath_errormask);
-		dd->ipath_maskederrs = 0;
-	}
-
-	/* limit qfull messages to ~one per minute per port */
-	if ((++cnt & 0x10)) {
-		for (i = (int) dd->ipath_cfgports; --i >= 0; ) {
-			struct ipath_portdata *pd = dd->ipath_pd[i];
-
-			if (pd && pd->port_lastrcvhdrqtail != -1)
-				pd->port_lastrcvhdrqtail = -1;
-		}
-	}
-
-	ipath_chk_errormask(dd);
-done:
-	mod_timer(&dd->ipath_stats_timer, jiffies + HZ * 5);
-}
diff --git a/drivers/staging/rdma/ipath/ipath_sysfs.c b/drivers/staging/rdma/ipath/ipath_sysfs.c
deleted file mode 100644
index b12b1f6caf59..000000000000
--- a/drivers/staging/rdma/ipath/ipath_sysfs.c
+++ /dev/null
@@ -1,1237 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2006 PathScale, 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/ctype.h>
-#include <linux/stat.h>
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-#include "ipath_common.h"
-
-/**
- * ipath_parse_ushort - parse an unsigned short value in an arbitrary base
- * @str: the string containing the number
- * @valp: where to put the result
- *
- * returns the number of bytes consumed, or negative value on error
- */
-int ipath_parse_ushort(const char *str, unsigned short *valp)
-{
-	unsigned long val;
-	char *end;
-	int ret;
-
-	if (!isdigit(str[0])) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	val = simple_strtoul(str, &end, 0);
-
-	if (val > 0xffff) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	*valp = val;
-
-	ret = end + 1 - str;
-	if (ret == 0)
-		ret = -EINVAL;
-
-bail:
-	return ret;
-}
-
-static ssize_t show_version(struct device_driver *dev, char *buf)
-{
-	/* The string printed here is already newline-terminated. */
-	return scnprintf(buf, PAGE_SIZE, "%s", ib_ipath_version);
-}
-
-static ssize_t show_num_units(struct device_driver *dev, char *buf)
-{
-	return scnprintf(buf, PAGE_SIZE, "%d\n",
-			 ipath_count_units(NULL, NULL, NULL));
-}
-
-static ssize_t show_status(struct device *dev,
-			   struct device_attribute *attr,
-			   char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	ssize_t ret;
-
-	if (!dd->ipath_statusp) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	ret = scnprintf(buf, PAGE_SIZE, "0x%llx\n",
-			(unsigned long long) *(dd->ipath_statusp));
-
-bail:
-	return ret;
-}
-
-static const char *ipath_status_str[] = {
-	"Initted",
-	"Disabled",
-	"Admin_Disabled",
-	"", /* This used to be the old "OIB_SMA" status. */
-	"", /* This used to be the old "SMA" status. */
-	"Present",
-	"IB_link_up",
-	"IB_configured",
-	"NoIBcable",
-	"Fatal_Hardware_Error",
-	NULL,
-};
-
-static ssize_t show_status_str(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int i, any;
-	u64 s;
-	ssize_t ret;
-
-	if (!dd->ipath_statusp) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	s = *(dd->ipath_statusp);
-	*buf = '\0';
-	for (any = i = 0; s && ipath_status_str[i]; i++) {
-		if (s & 1) {
-			if (any && strlcat(buf, " ", PAGE_SIZE) >=
-			    PAGE_SIZE)
-				/* overflow */
-				break;
-			if (strlcat(buf, ipath_status_str[i],
-				    PAGE_SIZE) >= PAGE_SIZE)
-				break;
-			any = 1;
-		}
-		s >>= 1;
-	}
-	if (any)
-		strlcat(buf, "\n", PAGE_SIZE);
-
-	ret = strlen(buf);
-
-bail:
-	return ret;
-}
-
-static ssize_t show_boardversion(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	/* The string printed here is already newline-terminated. */
-	return scnprintf(buf, PAGE_SIZE, "%s", dd->ipath_boardversion);
-}
-
-static ssize_t show_localbus_info(struct device *dev,
-			       struct device_attribute *attr,
-			       char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	/* The string printed here is already newline-terminated. */
-	return scnprintf(buf, PAGE_SIZE, "%s", dd->ipath_lbus_info);
-}
-
-static ssize_t show_lmc(struct device *dev,
-			struct device_attribute *attr,
-			char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->ipath_lmc);
-}
-
-static ssize_t store_lmc(struct device *dev,
-			 struct device_attribute *attr,
-			 const char *buf,
-			 size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u16 lmc = 0;
-	int ret;
-
-	ret = ipath_parse_ushort(buf, &lmc);
-	if (ret < 0)
-		goto invalid;
-
-	if (lmc > 7) {
-		ret = -EINVAL;
-		goto invalid;
-	}
-
-	ipath_set_lid(dd, dd->ipath_lid, lmc);
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid LMC %u\n", lmc);
-bail:
-	return ret;
-}
-
-static ssize_t show_lid(struct device *dev,
-			struct device_attribute *attr,
-			char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "0x%x\n", dd->ipath_lid);
-}
-
-static ssize_t store_lid(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u16 lid = 0;
-	int ret;
-
-	ret = ipath_parse_ushort(buf, &lid);
-	if (ret < 0)
-		goto invalid;
-
-	if (lid == 0 || lid >= IPATH_MULTICAST_LID_BASE) {
-		ret = -EINVAL;
-		goto invalid;
-	}
-
-	ipath_set_lid(dd, lid, dd->ipath_lmc);
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid LID 0x%x\n", lid);
-bail:
-	return ret;
-}
-
-static ssize_t show_mlid(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "0x%x\n", dd->ipath_mlid);
-}
-
-static ssize_t store_mlid(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u16 mlid;
-	int ret;
-
-	ret = ipath_parse_ushort(buf, &mlid);
-	if (ret < 0 || mlid < IPATH_MULTICAST_LID_BASE)
-		goto invalid;
-
-	dd->ipath_mlid = mlid;
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid MLID\n");
-bail:
-	return ret;
-}
-
-static ssize_t show_guid(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u8 *guid;
-
-	guid = (u8 *) & (dd->ipath_guid);
-
-	return scnprintf(buf, PAGE_SIZE,
-			 "%02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x\n",
-			 guid[0], guid[1], guid[2], guid[3],
-			 guid[4], guid[5], guid[6], guid[7]);
-}
-
-static ssize_t store_guid(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	ssize_t ret;
-	unsigned short guid[8];
-	__be64 new_guid;
-	u8 *ng;
-	int i;
-
-	if (sscanf(buf, "%hx:%hx:%hx:%hx:%hx:%hx:%hx:%hx",
-		   &guid[0], &guid[1], &guid[2], &guid[3],
-		   &guid[4], &guid[5], &guid[6], &guid[7]) != 8)
-		goto invalid;
-
-	ng = (u8 *) &new_guid;
-
-	for (i = 0; i < 8; i++) {
-		if (guid[i] > 0xff)
-			goto invalid;
-		ng[i] = guid[i];
-	}
-
-	if (new_guid == 0)
-		goto invalid;
-
-	dd->ipath_guid = new_guid;
-	dd->ipath_nguid = 1;
-	if (dd->verbs_dev)
-		dd->verbs_dev->ibdev.node_guid = new_guid;
-
-	ret = strlen(buf);
-	goto bail;
-
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid GUID\n");
-	ret = -EINVAL;
-
-bail:
-	return ret;
-}
-
-static ssize_t show_nguid(struct device *dev,
-			  struct device_attribute *attr,
-			  char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->ipath_nguid);
-}
-
-static ssize_t show_nports(struct device *dev,
-			   struct device_attribute *attr,
-			   char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	/* Return the number of user ports available. */
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->ipath_cfgports - 1);
-}
-
-static ssize_t show_serial(struct device *dev,
-			   struct device_attribute *attr,
-			   char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	buf[sizeof dd->ipath_serial] = '\0';
-	memcpy(buf, dd->ipath_serial, sizeof dd->ipath_serial);
-	strcat(buf, "\n");
-	return strlen(buf);
-}
-
-static ssize_t show_unit(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->ipath_unit);
-}
-
-static ssize_t show_jint_max_packets(struct device *dev,
-				     struct device_attribute *attr,
-				     char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "%hu\n", dd->ipath_jint_max_packets);
-}
-
-static ssize_t store_jint_max_packets(struct device *dev,
-				      struct device_attribute *attr,
-				      const char *buf,
-				      size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u16 v = 0;
-	int ret;
-
-	ret = ipath_parse_ushort(buf, &v);
-	if (ret < 0)
-		ipath_dev_err(dd, "invalid jint_max_packets.\n");
-	else
-		dd->ipath_f_config_jint(dd, dd->ipath_jint_idle_ticks, v);
-
-	return ret;
-}
-
-static ssize_t show_jint_idle_ticks(struct device *dev,
-				    struct device_attribute *attr,
-				    char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-
-	return scnprintf(buf, PAGE_SIZE, "%hu\n", dd->ipath_jint_idle_ticks);
-}
-
-static ssize_t store_jint_idle_ticks(struct device *dev,
-				     struct device_attribute *attr,
-				     const char *buf,
-				     size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	u16 v = 0;
-	int ret;
-
-	ret = ipath_parse_ushort(buf, &v);
-	if (ret < 0)
-		ipath_dev_err(dd, "invalid jint_idle_ticks.\n");
-	else
-		dd->ipath_f_config_jint(dd, v, dd->ipath_jint_max_packets);
-
-	return ret;
-}
-
-#define DEVICE_COUNTER(name, attr) \
-	static ssize_t show_counter_##name(struct device *dev, \
-					   struct device_attribute *attr, \
-					   char *buf) \
-	{ \
-		struct ipath_devdata *dd = dev_get_drvdata(dev); \
-		return scnprintf(\
-			buf, PAGE_SIZE, "%llu\n", (unsigned long long) \
-			ipath_snap_cntr( \
-				dd, offsetof(struct infinipath_counters, \
-					     attr) / sizeof(u64)));	\
-	} \
-	static DEVICE_ATTR(name, S_IRUGO, show_counter_##name, NULL);
-
-DEVICE_COUNTER(ib_link_downeds, IBLinkDownedCnt);
-DEVICE_COUNTER(ib_link_err_recoveries, IBLinkErrRecoveryCnt);
-DEVICE_COUNTER(ib_status_changes, IBStatusChangeCnt);
-DEVICE_COUNTER(ib_symbol_errs, IBSymbolErrCnt);
-DEVICE_COUNTER(lb_flow_stalls, LBFlowStallCnt);
-DEVICE_COUNTER(lb_ints, LBIntCnt);
-DEVICE_COUNTER(rx_bad_formats, RxBadFormatCnt);
-DEVICE_COUNTER(rx_buf_ovfls, RxBufOvflCnt);
-DEVICE_COUNTER(rx_data_pkts, RxDataPktCnt);
-DEVICE_COUNTER(rx_dropped_pkts, RxDroppedPktCnt);
-DEVICE_COUNTER(rx_dwords, RxDwordCnt);
-DEVICE_COUNTER(rx_ebps, RxEBPCnt);
-DEVICE_COUNTER(rx_flow_ctrl_errs, RxFlowCtrlErrCnt);
-DEVICE_COUNTER(rx_flow_pkts, RxFlowPktCnt);
-DEVICE_COUNTER(rx_icrc_errs, RxICRCErrCnt);
-DEVICE_COUNTER(rx_len_errs, RxLenErrCnt);
-DEVICE_COUNTER(rx_link_problems, RxLinkProblemCnt);
-DEVICE_COUNTER(rx_lpcrc_errs, RxLPCRCErrCnt);
-DEVICE_COUNTER(rx_max_min_len_errs, RxMaxMinLenErrCnt);
-DEVICE_COUNTER(rx_p0_hdr_egr_ovfls, RxP0HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p1_hdr_egr_ovfls, RxP1HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p2_hdr_egr_ovfls, RxP2HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p3_hdr_egr_ovfls, RxP3HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p4_hdr_egr_ovfls, RxP4HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p5_hdr_egr_ovfls, RxP5HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p6_hdr_egr_ovfls, RxP6HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p7_hdr_egr_ovfls, RxP7HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_p8_hdr_egr_ovfls, RxP8HdrEgrOvflCnt);
-DEVICE_COUNTER(rx_pkey_mismatches, RxPKeyMismatchCnt);
-DEVICE_COUNTER(rx_tid_full_errs, RxTIDFullErrCnt);
-DEVICE_COUNTER(rx_tid_valid_errs, RxTIDValidErrCnt);
-DEVICE_COUNTER(rx_vcrc_errs, RxVCRCErrCnt);
-DEVICE_COUNTER(tx_data_pkts, TxDataPktCnt);
-DEVICE_COUNTER(tx_dropped_pkts, TxDroppedPktCnt);
-DEVICE_COUNTER(tx_dwords, TxDwordCnt);
-DEVICE_COUNTER(tx_flow_pkts, TxFlowPktCnt);
-DEVICE_COUNTER(tx_flow_stalls, TxFlowStallCnt);
-DEVICE_COUNTER(tx_len_errs, TxLenErrCnt);
-DEVICE_COUNTER(tx_max_min_len_errs, TxMaxMinLenErrCnt);
-DEVICE_COUNTER(tx_underruns, TxUnderrunCnt);
-DEVICE_COUNTER(tx_unsup_vl_errs, TxUnsupVLErrCnt);
-
-static struct attribute *dev_counter_attributes[] = {
-	&dev_attr_ib_link_downeds.attr,
-	&dev_attr_ib_link_err_recoveries.attr,
-	&dev_attr_ib_status_changes.attr,
-	&dev_attr_ib_symbol_errs.attr,
-	&dev_attr_lb_flow_stalls.attr,
-	&dev_attr_lb_ints.attr,
-	&dev_attr_rx_bad_formats.attr,
-	&dev_attr_rx_buf_ovfls.attr,
-	&dev_attr_rx_data_pkts.attr,
-	&dev_attr_rx_dropped_pkts.attr,
-	&dev_attr_rx_dwords.attr,
-	&dev_attr_rx_ebps.attr,
-	&dev_attr_rx_flow_ctrl_errs.attr,
-	&dev_attr_rx_flow_pkts.attr,
-	&dev_attr_rx_icrc_errs.attr,
-	&dev_attr_rx_len_errs.attr,
-	&dev_attr_rx_link_problems.attr,
-	&dev_attr_rx_lpcrc_errs.attr,
-	&dev_attr_rx_max_min_len_errs.attr,
-	&dev_attr_rx_p0_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p1_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p2_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p3_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p4_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p5_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p6_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p7_hdr_egr_ovfls.attr,
-	&dev_attr_rx_p8_hdr_egr_ovfls.attr,
-	&dev_attr_rx_pkey_mismatches.attr,
-	&dev_attr_rx_tid_full_errs.attr,
-	&dev_attr_rx_tid_valid_errs.attr,
-	&dev_attr_rx_vcrc_errs.attr,
-	&dev_attr_tx_data_pkts.attr,
-	&dev_attr_tx_dropped_pkts.attr,
-	&dev_attr_tx_dwords.attr,
-	&dev_attr_tx_flow_pkts.attr,
-	&dev_attr_tx_flow_stalls.attr,
-	&dev_attr_tx_len_errs.attr,
-	&dev_attr_tx_max_min_len_errs.attr,
-	&dev_attr_tx_underruns.attr,
-	&dev_attr_tx_unsup_vl_errs.attr,
-	NULL
-};
-
-static struct attribute_group dev_counter_attr_group = {
-	.name = "counters",
-	.attrs = dev_counter_attributes
-};
-
-static ssize_t store_reset(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	if (count < 5 || memcmp(buf, "reset", 5)) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	if (dd->ipath_flags & IPATH_DISABLED) {
-		/*
-		 * post-reset init would re-enable interrupts, etc.
-		 * so don't allow reset on disabled devices.  Not
-		 * perfect error, but about the best choice.
-		 */
-		dev_info(dev,"Unit %d is disabled, can't reset\n",
-			 dd->ipath_unit);
-		ret = -EINVAL;
-		goto bail;
-	}
-	ret = ipath_reset_device(dd->ipath_unit);
-bail:
-	return ret<0 ? ret : count;
-}
-
-static ssize_t store_link_state(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 state;
-
-	ret = ipath_parse_ushort(buf, &state);
-	if (ret < 0)
-		goto invalid;
-
-	r = ipath_set_linkstate(dd, state);
-	if (r < 0) {
-		ret = r;
-		goto bail;
-	}
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid link state\n");
-bail:
-	return ret;
-}
-
-static ssize_t show_mtu(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	return scnprintf(buf, PAGE_SIZE, "%u\n", dd->ipath_ibmtu);
-}
-
-static ssize_t store_mtu(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	ssize_t ret;
-	u16 mtu = 0;
-	int r;
-
-	ret = ipath_parse_ushort(buf, &mtu);
-	if (ret < 0)
-		goto invalid;
-
-	r = ipath_set_mtu(dd, mtu);
-	if (r < 0)
-		ret = r;
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid MTU\n");
-bail:
-	return ret;
-}
-
-static ssize_t show_enabled(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	return scnprintf(buf, PAGE_SIZE, "%u\n",
-			 (dd->ipath_flags & IPATH_DISABLED) ? 0 : 1);
-}
-
-static ssize_t store_enabled(struct device *dev,
-			 struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	ssize_t ret;
-	u16 enable = 0;
-
-	ret = ipath_parse_ushort(buf, &enable);
-	if (ret < 0) {
-		ipath_dev_err(dd, "attempt to use non-numeric on enable\n");
-		goto bail;
-	}
-
-	if (enable) {
-		if (!(dd->ipath_flags & IPATH_DISABLED))
-			goto bail;
-
-		dev_info(dev, "Enabling unit %d\n", dd->ipath_unit);
-		/* same as post-reset */
-		ret = ipath_init_chip(dd, 1);
-		if (ret)
-			ipath_dev_err(dd, "Failed to enable unit %d\n",
-				      dd->ipath_unit);
-		else {
-			dd->ipath_flags &= ~IPATH_DISABLED;
-			*dd->ipath_statusp &= ~IPATH_STATUS_ADMIN_DISABLED;
-		}
-	} else if (!(dd->ipath_flags & IPATH_DISABLED)) {
-		dev_info(dev, "Disabling unit %d\n", dd->ipath_unit);
-		ipath_shutdown_device(dd);
-		dd->ipath_flags |= IPATH_DISABLED;
-		*dd->ipath_statusp |= IPATH_STATUS_ADMIN_DISABLED;
-	}
-
-bail:
-	return ret;
-}
-
-static ssize_t store_rx_pol_inv(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret < 0)
-		goto invalid;
-
-	r = ipath_set_rx_pol_inv(dd, val);
-	if (r < 0) {
-		ret = r;
-		goto bail;
-	}
-
-	goto bail;
-invalid:
-	ipath_dev_err(dd, "attempt to set invalid Rx Polarity invert\n");
-bail:
-	return ret;
-}
-
-static ssize_t store_led_override(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret > 0)
-		ipath_set_led_override(dd, val);
-	else
-		ipath_dev_err(dd, "attempt to set invalid LED override\n");
-	return ret;
-}
-
-static ssize_t show_logged_errs(struct device *dev,
-				struct device_attribute *attr,
-				char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int idx, count;
-
-	/* force consistency with actual EEPROM */
-	if (ipath_update_eeprom_log(dd) != 0)
-		return -ENXIO;
-
-	count = 0;
-	for (idx = 0; idx < IPATH_EEP_LOG_CNT; ++idx) {
-		count += scnprintf(buf + count, PAGE_SIZE - count, "%d%c",
-			dd->ipath_eep_st_errs[idx],
-			idx == (IPATH_EEP_LOG_CNT - 1) ? '\n' : ' ');
-	}
-
-	return count;
-}
-
-/*
- * New sysfs entries to control various IB config. These all turn into
- * accesses via ipath_f_get/set_ib_cfg.
- *
- * Get/Set heartbeat enable. Or of 1=enabled, 2=auto
- */
-static ssize_t show_hrtbt_enb(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_HRTBT);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-static ssize_t store_hrtbt_enb(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret >= 0 && val > 3)
-		ret = -EINVAL;
-	if (ret < 0) {
-		ipath_dev_err(dd, "attempt to set invalid Heartbeat enable\n");
-		goto bail;
-	}
-
-	/*
-	 * Set the "intentional" heartbeat enable per either of
-	 * "Enable" and "Auto", as these are normally set together.
-	 * This bit is consulted when leaving loopback mode,
-	 * because entering loopback mode overrides it and automatically
-	 * disables heartbeat.
-	 */
-	r = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_HRTBT, val);
-	if (r < 0)
-		ret = r;
-	else if (val == IPATH_IB_HRTBT_OFF)
-		dd->ipath_flags |= IPATH_NO_HRTBT;
-	else
-		dd->ipath_flags &= ~IPATH_NO_HRTBT;
-
-bail:
-	return ret;
-}
-
-/*
- * Get/Set Link-widths enabled. Or of 1=1x, 2=4x (this is human/IB centric,
- * _not_ the particular encoding of any given chip)
- */
-static ssize_t show_lwid_enb(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_LWID_ENB);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-static ssize_t store_lwid_enb(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret >= 0 && (val == 0 || val > 3))
-		ret = -EINVAL;
-	if (ret < 0) {
-		ipath_dev_err(dd,
-			"attempt to set invalid Link Width (enable)\n");
-		goto bail;
-	}
-
-	r = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_LWID_ENB, val);
-	if (r < 0)
-		ret = r;
-
-bail:
-	return ret;
-}
-
-/* Get current link width */
-static ssize_t show_lwid(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_LWID);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-/*
- * Get/Set Link-speeds enabled. Or of 1=SDR 2=DDR.
- */
-static ssize_t show_spd_enb(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_SPD_ENB);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-static ssize_t store_spd_enb(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret >= 0 && (val == 0 || val > (IPATH_IB_SDR | IPATH_IB_DDR)))
-		ret = -EINVAL;
-	if (ret < 0) {
-		ipath_dev_err(dd,
-			"attempt to set invalid Link Speed (enable)\n");
-		goto bail;
-	}
-
-	r = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_SPD_ENB, val);
-	if (r < 0)
-		ret = r;
-
-bail:
-	return ret;
-}
-
-/* Get current link speed */
-static ssize_t show_spd(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_SPD);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-/*
- * Get/Set RX polarity-invert enable. 0=no, 1=yes.
- */
-static ssize_t show_rx_polinv_enb(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_RXPOL_ENB);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-static ssize_t store_rx_polinv_enb(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret >= 0 && val > 1) {
-		ipath_dev_err(dd,
-			"attempt to set invalid Rx Polarity (enable)\n");
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	r = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_RXPOL_ENB, val);
-	if (r < 0)
-		ret = r;
-
-bail:
-	return ret;
-}
-
-/*
- * Get/Set RX lane-reversal enable. 0=no, 1=yes.
- */
-static ssize_t show_lanerev_enb(struct device *dev,
-			 struct device_attribute *attr,
-			 char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-
-	ret = dd->ipath_f_get_ib_cfg(dd, IPATH_IB_CFG_LREV_ENB);
-	if (ret >= 0)
-		ret = scnprintf(buf, PAGE_SIZE, "%d\n", ret);
-	return ret;
-}
-
-static ssize_t store_lanerev_enb(struct device *dev,
-			  struct device_attribute *attr,
-			  const char *buf,
-			  size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, r;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret >= 0 && val > 1) {
-		ret = -EINVAL;
-		ipath_dev_err(dd,
-			"attempt to set invalid Lane reversal (enable)\n");
-		goto bail;
-	}
-
-	r = dd->ipath_f_set_ib_cfg(dd, IPATH_IB_CFG_LREV_ENB, val);
-	if (r < 0)
-		ret = r;
-
-bail:
-	return ret;
-}
-
-static DRIVER_ATTR(num_units, S_IRUGO, show_num_units, NULL);
-static DRIVER_ATTR(version, S_IRUGO, show_version, NULL);
-
-static struct attribute *driver_attributes[] = {
-	&driver_attr_num_units.attr,
-	&driver_attr_version.attr,
-	NULL
-};
-
-static struct attribute_group driver_attr_group = {
-	.attrs = driver_attributes
-};
-
-static ssize_t store_tempsense(struct device *dev,
-			       struct device_attribute *attr,
-			       const char *buf,
-			       size_t count)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret, stat;
-	u16 val;
-
-	ret = ipath_parse_ushort(buf, &val);
-	if (ret <= 0) {
-		ipath_dev_err(dd, "attempt to set invalid tempsense config\n");
-		goto bail;
-	}
-	/* If anything but the highest limit, enable T_CRIT_A "interrupt" */
-	stat = ipath_tempsense_write(dd, 9, (val == 0x7f7f) ? 0x80 : 0);
-	if (stat) {
-		ipath_dev_err(dd, "Unable to set tempsense config\n");
-		ret = -1;
-		goto bail;
-	}
-	stat = ipath_tempsense_write(dd, 0xB, (u8) (val & 0xFF));
-	if (stat) {
-		ipath_dev_err(dd, "Unable to set local Tcrit\n");
-		ret = -1;
-		goto bail;
-	}
-	stat = ipath_tempsense_write(dd, 0xD, (u8) (val >> 8));
-	if (stat) {
-		ipath_dev_err(dd, "Unable to set remote Tcrit\n");
-		ret = -1;
-		goto bail;
-	}
-
-bail:
-	return ret;
-}
-
-/*
- * dump tempsense regs. in decimal, to ease shell-scripts.
- */
-static ssize_t show_tempsense(struct device *dev,
-			      struct device_attribute *attr,
-			      char *buf)
-{
-	struct ipath_devdata *dd = dev_get_drvdata(dev);
-	int ret;
-	int idx;
-	u8 regvals[8];
-
-	ret = -ENXIO;
-	for (idx = 0; idx < 8; ++idx) {
-		if (idx == 6)
-			continue;
-		ret = ipath_tempsense_read(dd, idx);
-		if (ret < 0)
-			break;
-		regvals[idx] = ret;
-	}
-	if (idx == 8)
-		ret = scnprintf(buf, PAGE_SIZE, "%d %d %02X %02X %d %d\n",
-			*(signed char *)(regvals),
-			*(signed char *)(regvals + 1),
-			regvals[2], regvals[3],
-			*(signed char *)(regvals + 5),
-			*(signed char *)(regvals + 7));
-	return ret;
-}
-
-const struct attribute_group *ipath_driver_attr_groups[] = {
-	&driver_attr_group,
-	NULL,
-};
-
-static DEVICE_ATTR(guid, S_IWUSR | S_IRUGO, show_guid, store_guid);
-static DEVICE_ATTR(lmc, S_IWUSR | S_IRUGO, show_lmc, store_lmc);
-static DEVICE_ATTR(lid, S_IWUSR | S_IRUGO, show_lid, store_lid);
-static DEVICE_ATTR(link_state, S_IWUSR, NULL, store_link_state);
-static DEVICE_ATTR(mlid, S_IWUSR | S_IRUGO, show_mlid, store_mlid);
-static DEVICE_ATTR(mtu, S_IWUSR | S_IRUGO, show_mtu, store_mtu);
-static DEVICE_ATTR(enabled, S_IWUSR | S_IRUGO, show_enabled, store_enabled);
-static DEVICE_ATTR(nguid, S_IRUGO, show_nguid, NULL);
-static DEVICE_ATTR(nports, S_IRUGO, show_nports, NULL);
-static DEVICE_ATTR(reset, S_IWUSR, NULL, store_reset);
-static DEVICE_ATTR(serial, S_IRUGO, show_serial, NULL);
-static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
-static DEVICE_ATTR(status_str, S_IRUGO, show_status_str, NULL);
-static DEVICE_ATTR(boardversion, S_IRUGO, show_boardversion, NULL);
-static DEVICE_ATTR(unit, S_IRUGO, show_unit, NULL);
-static DEVICE_ATTR(rx_pol_inv, S_IWUSR, NULL, store_rx_pol_inv);
-static DEVICE_ATTR(led_override, S_IWUSR, NULL, store_led_override);
-static DEVICE_ATTR(logged_errors, S_IRUGO, show_logged_errs, NULL);
-static DEVICE_ATTR(localbus_info, S_IRUGO, show_localbus_info, NULL);
-static DEVICE_ATTR(jint_max_packets, S_IWUSR | S_IRUGO,
-		   show_jint_max_packets, store_jint_max_packets);
-static DEVICE_ATTR(jint_idle_ticks, S_IWUSR | S_IRUGO,
-		   show_jint_idle_ticks, store_jint_idle_ticks);
-static DEVICE_ATTR(tempsense, S_IWUSR | S_IRUGO,
-		   show_tempsense, store_tempsense);
-
-static struct attribute *dev_attributes[] = {
-	&dev_attr_guid.attr,
-	&dev_attr_lmc.attr,
-	&dev_attr_lid.attr,
-	&dev_attr_link_state.attr,
-	&dev_attr_mlid.attr,
-	&dev_attr_mtu.attr,
-	&dev_attr_nguid.attr,
-	&dev_attr_nports.attr,
-	&dev_attr_serial.attr,
-	&dev_attr_status.attr,
-	&dev_attr_status_str.attr,
-	&dev_attr_boardversion.attr,
-	&dev_attr_unit.attr,
-	&dev_attr_enabled.attr,
-	&dev_attr_rx_pol_inv.attr,
-	&dev_attr_led_override.attr,
-	&dev_attr_logged_errors.attr,
-	&dev_attr_tempsense.attr,
-	&dev_attr_localbus_info.attr,
-	NULL
-};
-
-static struct attribute_group dev_attr_group = {
-	.attrs = dev_attributes
-};
-
-static DEVICE_ATTR(hrtbt_enable, S_IWUSR | S_IRUGO, show_hrtbt_enb,
-		   store_hrtbt_enb);
-static DEVICE_ATTR(link_width_enable, S_IWUSR | S_IRUGO, show_lwid_enb,
-		   store_lwid_enb);
-static DEVICE_ATTR(link_width, S_IRUGO, show_lwid, NULL);
-static DEVICE_ATTR(link_speed_enable, S_IWUSR | S_IRUGO, show_spd_enb,
-		   store_spd_enb);
-static DEVICE_ATTR(link_speed, S_IRUGO, show_spd, NULL);
-static DEVICE_ATTR(rx_pol_inv_enable, S_IWUSR | S_IRUGO, show_rx_polinv_enb,
-		   store_rx_polinv_enb);
-static DEVICE_ATTR(rx_lane_rev_enable, S_IWUSR | S_IRUGO, show_lanerev_enb,
-		   store_lanerev_enb);
-
-static struct attribute *dev_ibcfg_attributes[] = {
-	&dev_attr_hrtbt_enable.attr,
-	&dev_attr_link_width_enable.attr,
-	&dev_attr_link_width.attr,
-	&dev_attr_link_speed_enable.attr,
-	&dev_attr_link_speed.attr,
-	&dev_attr_rx_pol_inv_enable.attr,
-	&dev_attr_rx_lane_rev_enable.attr,
-	NULL
-};
-
-static struct attribute_group dev_ibcfg_attr_group = {
-	.attrs = dev_ibcfg_attributes
-};
-
-/**
- * ipath_expose_reset - create a device reset file
- * @dev: the device structure
- *
- * Only expose a file that lets us reset the device after someone
- * enters diag mode.  A device reset is quite likely to crash the
- * machine entirely, so we don't want to normally make it
- * available.
- *
- * Called with ipath_mutex held.
- */
-int ipath_expose_reset(struct device *dev)
-{
-	static int exposed;
-	int ret;
-
-	if (!exposed) {
-		ret = device_create_file(dev, &dev_attr_reset);
-		exposed = 1;
-	} else {
-		ret = 0;
-	}
-
-	return ret;
-}
-
-int ipath_device_create_group(struct device *dev, struct ipath_devdata *dd)
-{
-	int ret;
-
-	ret = sysfs_create_group(&dev->kobj, &dev_attr_group);
-	if (ret)
-		goto bail;
-
-	ret = sysfs_create_group(&dev->kobj, &dev_counter_attr_group);
-	if (ret)
-		goto bail_attrs;
-
-	if (dd->ipath_flags & IPATH_HAS_MULT_IB_SPEED) {
-		ret = device_create_file(dev, &dev_attr_jint_idle_ticks);
-		if (ret)
-			goto bail_counter;
-		ret = device_create_file(dev, &dev_attr_jint_max_packets);
-		if (ret)
-			goto bail_idle;
-
-		ret = sysfs_create_group(&dev->kobj, &dev_ibcfg_attr_group);
-		if (ret)
-			goto bail_max;
-	}
-
-	return 0;
-
-bail_max:
-	device_remove_file(dev, &dev_attr_jint_max_packets);
-bail_idle:
-	device_remove_file(dev, &dev_attr_jint_idle_ticks);
-bail_counter:
-	sysfs_remove_group(&dev->kobj, &dev_counter_attr_group);
-bail_attrs:
-	sysfs_remove_group(&dev->kobj, &dev_attr_group);
-bail:
-	return ret;
-}
-
-void ipath_device_remove_group(struct device *dev, struct ipath_devdata *dd)
-{
-	sysfs_remove_group(&dev->kobj, &dev_counter_attr_group);
-
-	if (dd->ipath_flags & IPATH_HAS_MULT_IB_SPEED) {
-		sysfs_remove_group(&dev->kobj, &dev_ibcfg_attr_group);
-		device_remove_file(dev, &dev_attr_jint_idle_ticks);
-		device_remove_file(dev, &dev_attr_jint_max_packets);
-	}
-
-	sysfs_remove_group(&dev->kobj, &dev_attr_group);
-
-	device_remove_file(dev, &dev_attr_reset);
-}
diff --git a/drivers/staging/rdma/ipath/ipath_uc.c b/drivers/staging/rdma/ipath/ipath_uc.c
deleted file mode 100644
index 0246b30280b9..000000000000
--- a/drivers/staging/rdma/ipath/ipath_uc.c
+++ /dev/null
@@ -1,547 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-/* cut down ridiculously long IB macro names */
-#define OP(x) IB_OPCODE_UC_##x
-
-/**
- * ipath_make_uc_req - construct a request packet (SEND, RDMA write)
- * @qp: a pointer to the QP
- *
- * Return 1 if constructed; otherwise, return 0.
- */
-int ipath_make_uc_req(struct ipath_qp *qp)
-{
-	struct ipath_other_headers *ohdr;
-	struct ipath_swqe *wqe;
-	unsigned long flags;
-	u32 hwords;
-	u32 bth0;
-	u32 len;
-	u32 pmtu = ib_mtu_enum_to_int(qp->path_mtu);
-	int ret = 0;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK)) {
-		if (!(ib_ipath_state_ops[qp->state] & IPATH_FLUSH_SEND))
-			goto bail;
-		/* We are in the error state, flush the work request. */
-		if (qp->s_last == qp->s_head)
-			goto bail;
-		/* If DMAs are in progress, we can't flush immediately. */
-		if (atomic_read(&qp->s_dma_busy)) {
-			qp->s_flags |= IPATH_S_WAIT_DMA;
-			goto bail;
-		}
-		wqe = get_swqe_ptr(qp, qp->s_last);
-		ipath_send_complete(qp, wqe, IB_WC_WR_FLUSH_ERR);
-		goto done;
-	}
-
-	ohdr = &qp->s_hdr.u.oth;
-	if (qp->remote_ah_attr.ah_flags & IB_AH_GRH)
-		ohdr = &qp->s_hdr.u.l.oth;
-
-	/* header size in 32-bit words LRH+BTH = (8+12)/4. */
-	hwords = 5;
-	bth0 = 1 << 22; /* Set M bit */
-
-	/* Get the next send request. */
-	wqe = get_swqe_ptr(qp, qp->s_cur);
-	qp->s_wqe = NULL;
-	switch (qp->s_state) {
-	default:
-		if (!(ib_ipath_state_ops[qp->state] &
-		    IPATH_PROCESS_NEXT_SEND_OK))
-			goto bail;
-		/* Check if send work queue is empty. */
-		if (qp->s_cur == qp->s_head)
-			goto bail;
-		/*
-		 * Start a new request.
-		 */
-		qp->s_psn = wqe->psn = qp->s_next_psn;
-		qp->s_sge.sge = wqe->sg_list[0];
-		qp->s_sge.sg_list = wqe->sg_list + 1;
-		qp->s_sge.num_sge = wqe->wr.num_sge;
-		qp->s_len = len = wqe->length;
-		switch (wqe->wr.opcode) {
-		case IB_WR_SEND:
-		case IB_WR_SEND_WITH_IMM:
-			if (len > pmtu) {
-				qp->s_state = OP(SEND_FIRST);
-				len = pmtu;
-				break;
-			}
-			if (wqe->wr.opcode == IB_WR_SEND)
-				qp->s_state = OP(SEND_ONLY);
-			else {
-				qp->s_state =
-					OP(SEND_ONLY_WITH_IMMEDIATE);
-				/* Immediate data comes after the BTH */
-				ohdr->u.imm_data = wqe->wr.ex.imm_data;
-				hwords += 1;
-			}
-			if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-				bth0 |= 1 << 23;
-			qp->s_wqe = wqe;
-			if (++qp->s_cur >= qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		case IB_WR_RDMA_WRITE:
-		case IB_WR_RDMA_WRITE_WITH_IMM:
-			ohdr->u.rc.reth.vaddr =
-				cpu_to_be64(wqe->rdma_wr.remote_addr);
-			ohdr->u.rc.reth.rkey =
-				cpu_to_be32(wqe->rdma_wr.rkey);
-			ohdr->u.rc.reth.length = cpu_to_be32(len);
-			hwords += sizeof(struct ib_reth) / 4;
-			if (len > pmtu) {
-				qp->s_state = OP(RDMA_WRITE_FIRST);
-				len = pmtu;
-				break;
-			}
-			if (wqe->wr.opcode == IB_WR_RDMA_WRITE)
-				qp->s_state = OP(RDMA_WRITE_ONLY);
-			else {
-				qp->s_state =
-					OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE);
-				/* Immediate data comes after the RETH */
-				ohdr->u.rc.imm_data = wqe->wr.ex.imm_data;
-				hwords += 1;
-				if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-					bth0 |= 1 << 23;
-			}
-			qp->s_wqe = wqe;
-			if (++qp->s_cur >= qp->s_size)
-				qp->s_cur = 0;
-			break;
-
-		default:
-			goto bail;
-		}
-		break;
-
-	case OP(SEND_FIRST):
-		qp->s_state = OP(SEND_MIDDLE);
-		/* FALLTHROUGH */
-	case OP(SEND_MIDDLE):
-		len = qp->s_len;
-		if (len > pmtu) {
-			len = pmtu;
-			break;
-		}
-		if (wqe->wr.opcode == IB_WR_SEND)
-			qp->s_state = OP(SEND_LAST);
-		else {
-			qp->s_state = OP(SEND_LAST_WITH_IMMEDIATE);
-			/* Immediate data comes after the BTH */
-			ohdr->u.imm_data = wqe->wr.ex.imm_data;
-			hwords += 1;
-		}
-		if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-			bth0 |= 1 << 23;
-		qp->s_wqe = wqe;
-		if (++qp->s_cur >= qp->s_size)
-			qp->s_cur = 0;
-		break;
-
-	case OP(RDMA_WRITE_FIRST):
-		qp->s_state = OP(RDMA_WRITE_MIDDLE);
-		/* FALLTHROUGH */
-	case OP(RDMA_WRITE_MIDDLE):
-		len = qp->s_len;
-		if (len > pmtu) {
-			len = pmtu;
-			break;
-		}
-		if (wqe->wr.opcode == IB_WR_RDMA_WRITE)
-			qp->s_state = OP(RDMA_WRITE_LAST);
-		else {
-			qp->s_state =
-				OP(RDMA_WRITE_LAST_WITH_IMMEDIATE);
-			/* Immediate data comes after the BTH */
-			ohdr->u.imm_data = wqe->wr.ex.imm_data;
-			hwords += 1;
-			if (wqe->wr.send_flags & IB_SEND_SOLICITED)
-				bth0 |= 1 << 23;
-		}
-		qp->s_wqe = wqe;
-		if (++qp->s_cur >= qp->s_size)
-			qp->s_cur = 0;
-		break;
-	}
-	qp->s_len -= len;
-	qp->s_hdrwords = hwords;
-	qp->s_cur_sge = &qp->s_sge;
-	qp->s_cur_size = len;
-	ipath_make_ruc_header(to_idev(qp->ibqp.device),
-			      qp, ohdr, bth0 | (qp->s_state << 24),
-			      qp->s_next_psn++ & IPATH_PSN_MASK);
-done:
-	ret = 1;
-	goto unlock;
-
-bail:
-	qp->s_flags &= ~IPATH_S_BUSY;
-unlock:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-	return ret;
-}
-
-/**
- * ipath_uc_rcv - handle an incoming UC packet
- * @dev: the device the packet came in on
- * @hdr: the header of the packet
- * @has_grh: true if the packet has a GRH
- * @data: the packet data
- * @tlen: the length of the packet
- * @qp: the QP for this packet.
- *
- * This is called from ipath_qp_rcv() to process an incoming UC packet
- * for the given QP.
- * Called at interrupt level.
- */
-void ipath_uc_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp)
-{
-	struct ipath_other_headers *ohdr;
-	int opcode;
-	u32 hdrsize;
-	u32 psn;
-	u32 pad;
-	struct ib_wc wc;
-	u32 pmtu = ib_mtu_enum_to_int(qp->path_mtu);
-	struct ib_reth *reth;
-	int header_in_data;
-
-	/* Validate the SLID. See Ch. 9.6.1.5 */
-	if (unlikely(be16_to_cpu(hdr->lrh[3]) != qp->remote_ah_attr.dlid))
-		goto done;
-
-	/* Check for GRH */
-	if (!has_grh) {
-		ohdr = &hdr->u.oth;
-		hdrsize = 8 + 12;	/* LRH + BTH */
-		psn = be32_to_cpu(ohdr->bth[2]);
-		header_in_data = 0;
-	} else {
-		ohdr = &hdr->u.l.oth;
-		hdrsize = 8 + 40 + 12;	/* LRH + GRH + BTH */
-		/*
-		 * The header with GRH is 60 bytes and the
-		 * core driver sets the eager header buffer
-		 * size to 56 bytes so the last 4 bytes of
-		 * the BTH header (PSN) is in the data buffer.
-		 */
-		header_in_data = dev->dd->ipath_rcvhdrentsize == 16;
-		if (header_in_data) {
-			psn = be32_to_cpu(((__be32 *) data)[0]);
-			data += sizeof(__be32);
-		} else
-			psn = be32_to_cpu(ohdr->bth[2]);
-	}
-	/*
-	 * The opcode is in the low byte when its in network order
-	 * (top byte when in host order).
-	 */
-	opcode = be32_to_cpu(ohdr->bth[0]) >> 24;
-
-	memset(&wc, 0, sizeof wc);
-
-	/* Compare the PSN verses the expected PSN. */
-	if (unlikely(ipath_cmp24(psn, qp->r_psn) != 0)) {
-		/*
-		 * Handle a sequence error.
-		 * Silently drop any current message.
-		 */
-		qp->r_psn = psn;
-	inv:
-		qp->r_state = OP(SEND_LAST);
-		switch (opcode) {
-		case OP(SEND_FIRST):
-		case OP(SEND_ONLY):
-		case OP(SEND_ONLY_WITH_IMMEDIATE):
-			goto send_first;
-
-		case OP(RDMA_WRITE_FIRST):
-		case OP(RDMA_WRITE_ONLY):
-		case OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE):
-			goto rdma_first;
-
-		default:
-			dev->n_pkt_drops++;
-			goto done;
-		}
-	}
-
-	/* Check for opcode sequence errors. */
-	switch (qp->r_state) {
-	case OP(SEND_FIRST):
-	case OP(SEND_MIDDLE):
-		if (opcode == OP(SEND_MIDDLE) ||
-		    opcode == OP(SEND_LAST) ||
-		    opcode == OP(SEND_LAST_WITH_IMMEDIATE))
-			break;
-		goto inv;
-
-	case OP(RDMA_WRITE_FIRST):
-	case OP(RDMA_WRITE_MIDDLE):
-		if (opcode == OP(RDMA_WRITE_MIDDLE) ||
-		    opcode == OP(RDMA_WRITE_LAST) ||
-		    opcode == OP(RDMA_WRITE_LAST_WITH_IMMEDIATE))
-			break;
-		goto inv;
-
-	default:
-		if (opcode == OP(SEND_FIRST) ||
-		    opcode == OP(SEND_ONLY) ||
-		    opcode == OP(SEND_ONLY_WITH_IMMEDIATE) ||
-		    opcode == OP(RDMA_WRITE_FIRST) ||
-		    opcode == OP(RDMA_WRITE_ONLY) ||
-		    opcode == OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE))
-			break;
-		goto inv;
-	}
-
-	/* OK, process the packet. */
-	switch (opcode) {
-	case OP(SEND_FIRST):
-	case OP(SEND_ONLY):
-	case OP(SEND_ONLY_WITH_IMMEDIATE):
-	send_first:
-		if (qp->r_flags & IPATH_R_REUSE_SGE) {
-			qp->r_flags &= ~IPATH_R_REUSE_SGE;
-			qp->r_sge = qp->s_rdma_read_sge;
-		} else if (!ipath_get_rwqe(qp, 0)) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		/* Save the WQE so we can reuse it in case of an error. */
-		qp->s_rdma_read_sge = qp->r_sge;
-		qp->r_rcv_len = 0;
-		if (opcode == OP(SEND_ONLY))
-			goto send_last;
-		else if (opcode == OP(SEND_ONLY_WITH_IMMEDIATE))
-			goto send_last_imm;
-		/* FALLTHROUGH */
-	case OP(SEND_MIDDLE):
-		/* Check for invalid length PMTU or posted rwqe len. */
-		if (unlikely(tlen != (hdrsize + pmtu + 4))) {
-			qp->r_flags |= IPATH_R_REUSE_SGE;
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		qp->r_rcv_len += pmtu;
-		if (unlikely(qp->r_rcv_len > qp->r_len)) {
-			qp->r_flags |= IPATH_R_REUSE_SGE;
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		ipath_copy_sge(&qp->r_sge, data, pmtu);
-		break;
-
-	case OP(SEND_LAST_WITH_IMMEDIATE):
-	send_last_imm:
-		if (header_in_data) {
-			wc.ex.imm_data = *(__be32 *) data;
-			data += sizeof(__be32);
-		} else {
-			/* Immediate data comes after BTH */
-			wc.ex.imm_data = ohdr->u.imm_data;
-		}
-		hdrsize += 4;
-		wc.wc_flags = IB_WC_WITH_IMM;
-		/* FALLTHROUGH */
-	case OP(SEND_LAST):
-	send_last:
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/* Check for invalid length. */
-		/* XXX LAST len should be >= 1 */
-		if (unlikely(tlen < (hdrsize + pad + 4))) {
-			qp->r_flags |= IPATH_R_REUSE_SGE;
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		/* Don't count the CRC. */
-		tlen -= (hdrsize + pad + 4);
-		wc.byte_len = tlen + qp->r_rcv_len;
-		if (unlikely(wc.byte_len > qp->r_len)) {
-			qp->r_flags |= IPATH_R_REUSE_SGE;
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		wc.opcode = IB_WC_RECV;
-	last_imm:
-		ipath_copy_sge(&qp->r_sge, data, tlen);
-		wc.wr_id = qp->r_wr_id;
-		wc.status = IB_WC_SUCCESS;
-		wc.qp = &qp->ibqp;
-		wc.src_qp = qp->remote_qpn;
-		wc.slid = qp->remote_ah_attr.dlid;
-		wc.sl = qp->remote_ah_attr.sl;
-		/* Signal completion event if the solicited bit is set. */
-		ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc,
-			       (ohdr->bth[0] &
-				cpu_to_be32(1 << 23)) != 0);
-		break;
-
-	case OP(RDMA_WRITE_FIRST):
-	case OP(RDMA_WRITE_ONLY):
-	case OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE): /* consume RWQE */
-	rdma_first:
-		/* RETH comes after BTH */
-		if (!header_in_data)
-			reth = &ohdr->u.rc.reth;
-		else {
-			reth = (struct ib_reth *)data;
-			data += sizeof(*reth);
-		}
-		hdrsize += sizeof(*reth);
-		qp->r_len = be32_to_cpu(reth->length);
-		qp->r_rcv_len = 0;
-		if (qp->r_len != 0) {
-			u32 rkey = be32_to_cpu(reth->rkey);
-			u64 vaddr = be64_to_cpu(reth->vaddr);
-			int ok;
-
-			/* Check rkey */
-			ok = ipath_rkey_ok(qp, &qp->r_sge, qp->r_len,
-					   vaddr, rkey,
-					   IB_ACCESS_REMOTE_WRITE);
-			if (unlikely(!ok)) {
-				dev->n_pkt_drops++;
-				goto done;
-			}
-		} else {
-			qp->r_sge.sg_list = NULL;
-			qp->r_sge.sge.mr = NULL;
-			qp->r_sge.sge.vaddr = NULL;
-			qp->r_sge.sge.length = 0;
-			qp->r_sge.sge.sge_length = 0;
-		}
-		if (unlikely(!(qp->qp_access_flags &
-			       IB_ACCESS_REMOTE_WRITE))) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		if (opcode == OP(RDMA_WRITE_ONLY))
-			goto rdma_last;
-		else if (opcode == OP(RDMA_WRITE_ONLY_WITH_IMMEDIATE))
-			goto rdma_last_imm;
-		/* FALLTHROUGH */
-	case OP(RDMA_WRITE_MIDDLE):
-		/* Check for invalid length PMTU or posted rwqe len. */
-		if (unlikely(tlen != (hdrsize + pmtu + 4))) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		qp->r_rcv_len += pmtu;
-		if (unlikely(qp->r_rcv_len > qp->r_len)) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		ipath_copy_sge(&qp->r_sge, data, pmtu);
-		break;
-
-	case OP(RDMA_WRITE_LAST_WITH_IMMEDIATE):
-	rdma_last_imm:
-		if (header_in_data) {
-			wc.ex.imm_data = *(__be32 *) data;
-			data += sizeof(__be32);
-		} else {
-			/* Immediate data comes after BTH */
-			wc.ex.imm_data = ohdr->u.imm_data;
-		}
-		hdrsize += 4;
-		wc.wc_flags = IB_WC_WITH_IMM;
-
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/* Check for invalid length. */
-		/* XXX LAST len should be >= 1 */
-		if (unlikely(tlen < (hdrsize + pad + 4))) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		/* Don't count the CRC. */
-		tlen -= (hdrsize + pad + 4);
-		if (unlikely(tlen + qp->r_rcv_len != qp->r_len)) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		if (qp->r_flags & IPATH_R_REUSE_SGE)
-			qp->r_flags &= ~IPATH_R_REUSE_SGE;
-		else if (!ipath_get_rwqe(qp, 1)) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		wc.byte_len = qp->r_len;
-		wc.opcode = IB_WC_RECV_RDMA_WITH_IMM;
-		goto last_imm;
-
-	case OP(RDMA_WRITE_LAST):
-	rdma_last:
-		/* Get the number of bytes the message was padded by. */
-		pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-		/* Check for invalid length. */
-		/* XXX LAST len should be >= 1 */
-		if (unlikely(tlen < (hdrsize + pad + 4))) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		/* Don't count the CRC. */
-		tlen -= (hdrsize + pad + 4);
-		if (unlikely(tlen + qp->r_rcv_len != qp->r_len)) {
-			dev->n_pkt_drops++;
-			goto done;
-		}
-		ipath_copy_sge(&qp->r_sge, data, tlen);
-		break;
-
-	default:
-		/* Drop packet for unknown opcodes. */
-		dev->n_pkt_drops++;
-		goto done;
-	}
-	qp->r_psn++;
-	qp->r_state = opcode;
-done:
-	return;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_ud.c b/drivers/staging/rdma/ipath/ipath_ud.c
deleted file mode 100644
index 385d9410a51e..000000000000
--- a/drivers/staging/rdma/ipath/ipath_ud.c
+++ /dev/null
@@ -1,579 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 <rdma/ib_smi.h>
-
-#include "ipath_verbs.h"
-#include "ipath_kernel.h"
-
-/**
- * ipath_ud_loopback - handle send on loopback QPs
- * @sqp: the sending QP
- * @swqe: the send work request
- *
- * This is called from ipath_make_ud_req() to forward a WQE addressed
- * to the same HCA.
- * Note that the receive interrupt handler may be calling ipath_ud_rcv()
- * while this is being called.
- */
-static void ipath_ud_loopback(struct ipath_qp *sqp, struct ipath_swqe *swqe)
-{
-	struct ipath_ibdev *dev = to_idev(sqp->ibqp.device);
-	struct ipath_qp *qp;
-	struct ib_ah_attr *ah_attr;
-	unsigned long flags;
-	struct ipath_rq *rq;
-	struct ipath_srq *srq;
-	struct ipath_sge_state rsge;
-	struct ipath_sge *sge;
-	struct ipath_rwq *wq;
-	struct ipath_rwqe *wqe;
-	void (*handler)(struct ib_event *, void *);
-	struct ib_wc wc;
-	u32 tail;
-	u32 rlen;
-	u32 length;
-
-	qp = ipath_lookup_qpn(&dev->qp_table, swqe->ud_wr.remote_qpn);
-	if (!qp || !(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK)) {
-		dev->n_pkt_drops++;
-		goto done;
-	}
-
-	/*
-	 * Check that the qkey matches (except for QP0, see 9.6.1.4.1).
-	 * Qkeys with the high order bit set mean use the
-	 * qkey from the QP context instead of the WR (see 10.2.5).
-	 */
-	if (unlikely(qp->ibqp.qp_num &&
-		     ((int) swqe->ud_wr.remote_qkey < 0 ?
-		      sqp->qkey : swqe->ud_wr.remote_qkey) != qp->qkey)) {
-		/* XXX OK to lose a count once in a while. */
-		dev->qkey_violations++;
-		dev->n_pkt_drops++;
-		goto drop;
-	}
-
-	/*
-	 * A GRH is expected to precede the data even if not
-	 * present on the wire.
-	 */
-	length = swqe->length;
-	memset(&wc, 0, sizeof wc);
-	wc.byte_len = length + sizeof(struct ib_grh);
-
-	if (swqe->wr.opcode == IB_WR_SEND_WITH_IMM) {
-		wc.wc_flags = IB_WC_WITH_IMM;
-		wc.ex.imm_data = swqe->wr.ex.imm_data;
-	}
-
-	/*
-	 * This would be a lot simpler if we could call ipath_get_rwqe()
-	 * but that uses state that the receive interrupt handler uses
-	 * so we would need to lock out receive interrupts while doing
-	 * local loopback.
-	 */
-	if (qp->ibqp.srq) {
-		srq = to_isrq(qp->ibqp.srq);
-		handler = srq->ibsrq.event_handler;
-		rq = &srq->rq;
-	} else {
-		srq = NULL;
-		handler = NULL;
-		rq = &qp->r_rq;
-	}
-
-	/*
-	 * Get the next work request entry to find where to put the data.
-	 * Note that it is safe to drop the lock after changing rq->tail
-	 * since ipath_post_receive() won't fill the empty slot.
-	 */
-	spin_lock_irqsave(&rq->lock, flags);
-	wq = rq->wq;
-	tail = wq->tail;
-	/* Validate tail before using it since it is user writable. */
-	if (tail >= rq->size)
-		tail = 0;
-	if (unlikely(tail == wq->head)) {
-		spin_unlock_irqrestore(&rq->lock, flags);
-		dev->n_pkt_drops++;
-		goto drop;
-	}
-	wqe = get_rwqe_ptr(rq, tail);
-	rsge.sg_list = qp->r_ud_sg_list;
-	if (!ipath_init_sge(qp, wqe, &rlen, &rsge)) {
-		spin_unlock_irqrestore(&rq->lock, flags);
-		dev->n_pkt_drops++;
-		goto drop;
-	}
-	/* Silently drop packets which are too big. */
-	if (wc.byte_len > rlen) {
-		spin_unlock_irqrestore(&rq->lock, flags);
-		dev->n_pkt_drops++;
-		goto drop;
-	}
-	if (++tail >= rq->size)
-		tail = 0;
-	wq->tail = tail;
-	wc.wr_id = wqe->wr_id;
-	if (handler) {
-		u32 n;
-
-		/*
-		 * validate head pointer value and compute
-		 * the number of remaining WQEs.
-		 */
-		n = wq->head;
-		if (n >= rq->size)
-			n = 0;
-		if (n < tail)
-			n += rq->size - tail;
-		else
-			n -= tail;
-		if (n < srq->limit) {
-			struct ib_event ev;
-
-			srq->limit = 0;
-			spin_unlock_irqrestore(&rq->lock, flags);
-			ev.device = qp->ibqp.device;
-			ev.element.srq = qp->ibqp.srq;
-			ev.event = IB_EVENT_SRQ_LIMIT_REACHED;
-			handler(&ev, srq->ibsrq.srq_context);
-		} else
-			spin_unlock_irqrestore(&rq->lock, flags);
-	} else
-		spin_unlock_irqrestore(&rq->lock, flags);
-
-	ah_attr = &to_iah(swqe->ud_wr.ah)->attr;
-	if (ah_attr->ah_flags & IB_AH_GRH) {
-		ipath_copy_sge(&rsge, &ah_attr->grh, sizeof(struct ib_grh));
-		wc.wc_flags |= IB_WC_GRH;
-	} else
-		ipath_skip_sge(&rsge, sizeof(struct ib_grh));
-	sge = swqe->sg_list;
-	while (length) {
-		u32 len = sge->length;
-
-		if (len > length)
-			len = length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		ipath_copy_sge(&rsge, sge->vaddr, len);
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--swqe->wr.num_sge)
-				sge++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-		length -= len;
-	}
-	wc.status = IB_WC_SUCCESS;
-	wc.opcode = IB_WC_RECV;
-	wc.qp = &qp->ibqp;
-	wc.src_qp = sqp->ibqp.qp_num;
-	/* XXX do we know which pkey matched? Only needed for GSI. */
-	wc.pkey_index = 0;
-	wc.slid = dev->dd->ipath_lid |
-		(ah_attr->src_path_bits &
-		 ((1 << dev->dd->ipath_lmc) - 1));
-	wc.sl = ah_attr->sl;
-	wc.dlid_path_bits =
-		ah_attr->dlid & ((1 << dev->dd->ipath_lmc) - 1);
-	wc.port_num = 1;
-	/* Signal completion event if the solicited bit is set. */
-	ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc,
-		       swqe->ud_wr.wr.send_flags & IB_SEND_SOLICITED);
-drop:
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-done:;
-}
-
-/**
- * ipath_make_ud_req - construct a UD request packet
- * @qp: the QP
- *
- * Return 1 if constructed; otherwise, return 0.
- */
-int ipath_make_ud_req(struct ipath_qp *qp)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ipath_other_headers *ohdr;
-	struct ib_ah_attr *ah_attr;
-	struct ipath_swqe *wqe;
-	unsigned long flags;
-	u32 nwords;
-	u32 extra_bytes;
-	u32 bth0;
-	u16 lrh0;
-	u16 lid;
-	int ret = 0;
-	int next_cur;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_NEXT_SEND_OK)) {
-		if (!(ib_ipath_state_ops[qp->state] & IPATH_FLUSH_SEND))
-			goto bail;
-		/* We are in the error state, flush the work request. */
-		if (qp->s_last == qp->s_head)
-			goto bail;
-		/* If DMAs are in progress, we can't flush immediately. */
-		if (atomic_read(&qp->s_dma_busy)) {
-			qp->s_flags |= IPATH_S_WAIT_DMA;
-			goto bail;
-		}
-		wqe = get_swqe_ptr(qp, qp->s_last);
-		ipath_send_complete(qp, wqe, IB_WC_WR_FLUSH_ERR);
-		goto done;
-	}
-
-	if (qp->s_cur == qp->s_head)
-		goto bail;
-
-	wqe = get_swqe_ptr(qp, qp->s_cur);
-	next_cur = qp->s_cur + 1;
-	if (next_cur >= qp->s_size)
-		next_cur = 0;
-
-	/* Construct the header. */
-	ah_attr = &to_iah(wqe->ud_wr.ah)->attr;
-	if (ah_attr->dlid >= IPATH_MULTICAST_LID_BASE) {
-		if (ah_attr->dlid != IPATH_PERMISSIVE_LID)
-			dev->n_multicast_xmit++;
-		else
-			dev->n_unicast_xmit++;
-	} else {
-		dev->n_unicast_xmit++;
-		lid = ah_attr->dlid & ~((1 << dev->dd->ipath_lmc) - 1);
-		if (unlikely(lid == dev->dd->ipath_lid)) {
-			/*
-			 * If DMAs are in progress, we can't generate
-			 * a completion for the loopback packet since
-			 * it would be out of order.
-			 * XXX Instead of waiting, we could queue a
-			 * zero length descriptor so we get a callback.
-			 */
-			if (atomic_read(&qp->s_dma_busy)) {
-				qp->s_flags |= IPATH_S_WAIT_DMA;
-				goto bail;
-			}
-			qp->s_cur = next_cur;
-			spin_unlock_irqrestore(&qp->s_lock, flags);
-			ipath_ud_loopback(qp, wqe);
-			spin_lock_irqsave(&qp->s_lock, flags);
-			ipath_send_complete(qp, wqe, IB_WC_SUCCESS);
-			goto done;
-		}
-	}
-
-	qp->s_cur = next_cur;
-	extra_bytes = -wqe->length & 3;
-	nwords = (wqe->length + extra_bytes) >> 2;
-
-	/* header size in 32-bit words LRH+BTH+DETH = (8+12+8)/4. */
-	qp->s_hdrwords = 7;
-	qp->s_cur_size = wqe->length;
-	qp->s_cur_sge = &qp->s_sge;
-	qp->s_dmult = ah_attr->static_rate;
-	qp->s_wqe = wqe;
-	qp->s_sge.sge = wqe->sg_list[0];
-	qp->s_sge.sg_list = wqe->sg_list + 1;
-	qp->s_sge.num_sge = wqe->ud_wr.wr.num_sge;
-
-	if (ah_attr->ah_flags & IB_AH_GRH) {
-		/* Header size in 32-bit words. */
-		qp->s_hdrwords += ipath_make_grh(dev, &qp->s_hdr.u.l.grh,
-						 &ah_attr->grh,
-						 qp->s_hdrwords, nwords);
-		lrh0 = IPATH_LRH_GRH;
-		ohdr = &qp->s_hdr.u.l.oth;
-		/*
-		 * Don't worry about sending to locally attached multicast
-		 * QPs.  It is unspecified by the spec. what happens.
-		 */
-	} else {
-		/* Header size in 32-bit words. */
-		lrh0 = IPATH_LRH_BTH;
-		ohdr = &qp->s_hdr.u.oth;
-	}
-	if (wqe->ud_wr.wr.opcode == IB_WR_SEND_WITH_IMM) {
-		qp->s_hdrwords++;
-		ohdr->u.ud.imm_data = wqe->ud_wr.wr.ex.imm_data;
-		bth0 = IB_OPCODE_UD_SEND_ONLY_WITH_IMMEDIATE << 24;
-	} else
-		bth0 = IB_OPCODE_UD_SEND_ONLY << 24;
-	lrh0 |= ah_attr->sl << 4;
-	if (qp->ibqp.qp_type == IB_QPT_SMI)
-		lrh0 |= 0xF000;	/* Set VL (see ch. 13.5.3.1) */
-	qp->s_hdr.lrh[0] = cpu_to_be16(lrh0);
-	qp->s_hdr.lrh[1] = cpu_to_be16(ah_attr->dlid);	/* DEST LID */
-	qp->s_hdr.lrh[2] = cpu_to_be16(qp->s_hdrwords + nwords +
-					   SIZE_OF_CRC);
-	lid = dev->dd->ipath_lid;
-	if (lid) {
-		lid |= ah_attr->src_path_bits &
-			((1 << dev->dd->ipath_lmc) - 1);
-		qp->s_hdr.lrh[3] = cpu_to_be16(lid);
-	} else
-		qp->s_hdr.lrh[3] = IB_LID_PERMISSIVE;
-	if (wqe->ud_wr.wr.send_flags & IB_SEND_SOLICITED)
-		bth0 |= 1 << 23;
-	bth0 |= extra_bytes << 20;
-	bth0 |= qp->ibqp.qp_type == IB_QPT_SMI ? IPATH_DEFAULT_P_KEY :
-		ipath_get_pkey(dev->dd, qp->s_pkey_index);
-	ohdr->bth[0] = cpu_to_be32(bth0);
-	/*
-	 * Use the multicast QP if the destination LID is a multicast LID.
-	 */
-	ohdr->bth[1] = ah_attr->dlid >= IPATH_MULTICAST_LID_BASE &&
-		ah_attr->dlid != IPATH_PERMISSIVE_LID ?
-		cpu_to_be32(IPATH_MULTICAST_QPN) :
-		cpu_to_be32(wqe->ud_wr.remote_qpn);
-	ohdr->bth[2] = cpu_to_be32(qp->s_next_psn++ & IPATH_PSN_MASK);
-	/*
-	 * Qkeys with the high order bit set mean use the
-	 * qkey from the QP context instead of the WR (see 10.2.5).
-	 */
-	ohdr->u.ud.deth[0] = cpu_to_be32((int)wqe->ud_wr.remote_qkey < 0 ?
-					 qp->qkey : wqe->ud_wr.remote_qkey);
-	ohdr->u.ud.deth[1] = cpu_to_be32(qp->ibqp.qp_num);
-
-done:
-	ret = 1;
-	goto unlock;
-
-bail:
-	qp->s_flags &= ~IPATH_S_BUSY;
-unlock:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-	return ret;
-}
-
-/**
- * ipath_ud_rcv - receive an incoming UD packet
- * @dev: the device the packet came in on
- * @hdr: the packet header
- * @has_grh: true if the packet has a GRH
- * @data: the packet data
- * @tlen: the packet length
- * @qp: the QP the packet came on
- *
- * This is called from ipath_qp_rcv() to process an incoming UD packet
- * for the given QP.
- * Called at interrupt level.
- */
-void ipath_ud_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp)
-{
-	struct ipath_other_headers *ohdr;
-	int opcode;
-	u32 hdrsize;
-	u32 pad;
-	struct ib_wc wc;
-	u32 qkey;
-	u32 src_qp;
-	u16 dlid;
-	int header_in_data;
-
-	/* Check for GRH */
-	if (!has_grh) {
-		ohdr = &hdr->u.oth;
-		hdrsize = 8 + 12 + 8;	/* LRH + BTH + DETH */
-		qkey = be32_to_cpu(ohdr->u.ud.deth[0]);
-		src_qp = be32_to_cpu(ohdr->u.ud.deth[1]);
-		header_in_data = 0;
-	} else {
-		ohdr = &hdr->u.l.oth;
-		hdrsize = 8 + 40 + 12 + 8; /* LRH + GRH + BTH + DETH */
-		/*
-		 * The header with GRH is 68 bytes and the core driver sets
-		 * the eager header buffer size to 56 bytes so the last 12
-		 * bytes of the IB header is in the data buffer.
-		 */
-		header_in_data = dev->dd->ipath_rcvhdrentsize == 16;
-		if (header_in_data) {
-			qkey = be32_to_cpu(((__be32 *) data)[1]);
-			src_qp = be32_to_cpu(((__be32 *) data)[2]);
-			data += 12;
-		} else {
-			qkey = be32_to_cpu(ohdr->u.ud.deth[0]);
-			src_qp = be32_to_cpu(ohdr->u.ud.deth[1]);
-		}
-	}
-	src_qp &= IPATH_QPN_MASK;
-
-	/*
-	 * Check that the permissive LID is only used on QP0
-	 * and the QKEY matches (see 9.6.1.4.1 and 9.6.1.5.1).
-	 */
-	if (qp->ibqp.qp_num) {
-		if (unlikely(hdr->lrh[1] == IB_LID_PERMISSIVE ||
-			     hdr->lrh[3] == IB_LID_PERMISSIVE)) {
-			dev->n_pkt_drops++;
-			goto bail;
-		}
-		if (unlikely(qkey != qp->qkey)) {
-			/* XXX OK to lose a count once in a while. */
-			dev->qkey_violations++;
-			dev->n_pkt_drops++;
-			goto bail;
-		}
-	} else if (hdr->lrh[1] == IB_LID_PERMISSIVE ||
-		   hdr->lrh[3] == IB_LID_PERMISSIVE) {
-		struct ib_smp *smp = (struct ib_smp *) data;
-
-		if (smp->mgmt_class != IB_MGMT_CLASS_SUBN_DIRECTED_ROUTE) {
-			dev->n_pkt_drops++;
-			goto bail;
-		}
-	}
-
-	/*
-	 * The opcode is in the low byte when its in network order
-	 * (top byte when in host order).
-	 */
-	opcode = be32_to_cpu(ohdr->bth[0]) >> 24;
-	if (qp->ibqp.qp_num > 1 &&
-	    opcode == IB_OPCODE_UD_SEND_ONLY_WITH_IMMEDIATE) {
-		if (header_in_data) {
-			wc.ex.imm_data = *(__be32 *) data;
-			data += sizeof(__be32);
-		} else
-			wc.ex.imm_data = ohdr->u.ud.imm_data;
-		wc.wc_flags = IB_WC_WITH_IMM;
-		hdrsize += sizeof(u32);
-	} else if (opcode == IB_OPCODE_UD_SEND_ONLY) {
-		wc.ex.imm_data = 0;
-		wc.wc_flags = 0;
-	} else {
-		dev->n_pkt_drops++;
-		goto bail;
-	}
-
-	/* Get the number of bytes the message was padded by. */
-	pad = (be32_to_cpu(ohdr->bth[0]) >> 20) & 3;
-	if (unlikely(tlen < (hdrsize + pad + 4))) {
-		/* Drop incomplete packets. */
-		dev->n_pkt_drops++;
-		goto bail;
-	}
-	tlen -= hdrsize + pad + 4;
-
-	/* Drop invalid MAD packets (see 13.5.3.1). */
-	if (unlikely((qp->ibqp.qp_num == 0 &&
-		      (tlen != 256 ||
-		       (be16_to_cpu(hdr->lrh[0]) >> 12) != 15)) ||
-		     (qp->ibqp.qp_num == 1 &&
-		      (tlen != 256 ||
-		       (be16_to_cpu(hdr->lrh[0]) >> 12) == 15)))) {
-		dev->n_pkt_drops++;
-		goto bail;
-	}
-
-	/*
-	 * A GRH is expected to precede the data even if not
-	 * present on the wire.
-	 */
-	wc.byte_len = tlen + sizeof(struct ib_grh);
-
-	/*
-	 * Get the next work request entry to find where to put the data.
-	 */
-	if (qp->r_flags & IPATH_R_REUSE_SGE)
-		qp->r_flags &= ~IPATH_R_REUSE_SGE;
-	else if (!ipath_get_rwqe(qp, 0)) {
-		/*
-		 * Count VL15 packets dropped due to no receive buffer.
-		 * Otherwise, count them as buffer overruns since usually,
-		 * the HW will be able to receive packets even if there are
-		 * no QPs with posted receive buffers.
-		 */
-		if (qp->ibqp.qp_num == 0)
-			dev->n_vl15_dropped++;
-		else
-			dev->rcv_errors++;
-		goto bail;
-	}
-	/* Silently drop packets which are too big. */
-	if (wc.byte_len > qp->r_len) {
-		qp->r_flags |= IPATH_R_REUSE_SGE;
-		dev->n_pkt_drops++;
-		goto bail;
-	}
-	if (has_grh) {
-		ipath_copy_sge(&qp->r_sge, &hdr->u.l.grh,
-			       sizeof(struct ib_grh));
-		wc.wc_flags |= IB_WC_GRH;
-	} else
-		ipath_skip_sge(&qp->r_sge, sizeof(struct ib_grh));
-	ipath_copy_sge(&qp->r_sge, data,
-		       wc.byte_len - sizeof(struct ib_grh));
-	if (!test_and_clear_bit(IPATH_R_WRID_VALID, &qp->r_aflags))
-		goto bail;
-	wc.wr_id = qp->r_wr_id;
-	wc.status = IB_WC_SUCCESS;
-	wc.opcode = IB_WC_RECV;
-	wc.vendor_err = 0;
-	wc.qp = &qp->ibqp;
-	wc.src_qp = src_qp;
-	/* XXX do we know which pkey matched? Only needed for GSI. */
-	wc.pkey_index = 0;
-	wc.slid = be16_to_cpu(hdr->lrh[3]);
-	wc.sl = (be16_to_cpu(hdr->lrh[0]) >> 4) & 0xF;
-	dlid = be16_to_cpu(hdr->lrh[1]);
-	/*
-	 * Save the LMC lower bits if the destination LID is a unicast LID.
-	 */
-	wc.dlid_path_bits = dlid >= IPATH_MULTICAST_LID_BASE ? 0 :
-		dlid & ((1 << dev->dd->ipath_lmc) - 1);
-	wc.port_num = 1;
-	/* Signal completion event if the solicited bit is set. */
-	ipath_cq_enter(to_icq(qp->ibqp.recv_cq), &wc,
-		       (ohdr->bth[0] &
-			cpu_to_be32(1 << 23)) != 0);
-
-bail:;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_user_pages.c b/drivers/staging/rdma/ipath/ipath_user_pages.c
deleted file mode 100644
index d29b4daf61f8..000000000000
--- a/drivers/staging/rdma/ipath/ipath_user_pages.c
+++ /dev/null
@@ -1,228 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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/mm.h>
-#include <linux/device.h>
-#include <linux/slab.h>
-
-#include "ipath_kernel.h"
-
-static void __ipath_release_user_pages(struct page **p, size_t num_pages,
-				   int dirty)
-{
-	size_t i;
-
-	for (i = 0; i < num_pages; i++) {
-		ipath_cdbg(MM, "%lu/%lu put_page %p\n", (unsigned long) i,
-			   (unsigned long) num_pages, p[i]);
-		if (dirty)
-			set_page_dirty_lock(p[i]);
-		put_page(p[i]);
-	}
-}
-
-/* call with current->mm->mmap_sem held */
-static int __ipath_get_user_pages(unsigned long start_page, size_t num_pages,
-				  struct page **p)
-{
-	unsigned long lock_limit;
-	size_t got;
-	int ret;
-
-	lock_limit = rlimit(RLIMIT_MEMLOCK) >> PAGE_SHIFT;
-
-	if (num_pages > lock_limit) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	ipath_cdbg(VERBOSE, "pin %lx pages from vaddr %lx\n",
-		   (unsigned long) num_pages, start_page);
-
-	for (got = 0; got < num_pages; got += ret) {
-		ret = get_user_pages(current, current->mm,
-				     start_page + got * PAGE_SIZE,
-				     num_pages - got, 1, 1,
-				     p + got, NULL);
-		if (ret < 0)
-			goto bail_release;
-	}
-
-	current->mm->pinned_vm += num_pages;
-
-	ret = 0;
-	goto bail;
-
-bail_release:
-	__ipath_release_user_pages(p, got, 0);
-bail:
-	return ret;
-}
-
-/**
- * ipath_map_page - a safety wrapper around pci_map_page()
- *
- * A dma_addr of all 0's is interpreted by the chip as "disabled".
- * Unfortunately, it can also be a valid dma_addr returned on some
- * architectures.
- *
- * The powerpc iommu assigns dma_addrs in ascending order, so we don't
- * have to bother with retries or mapping a dummy page to insure we
- * don't just get the same mapping again.
- *
- * I'm sure we won't be so lucky with other iommu's, so FIXME.
- */
-dma_addr_t ipath_map_page(struct pci_dev *hwdev, struct page *page,
-	unsigned long offset, size_t size, int direction)
-{
-	dma_addr_t phys;
-
-	phys = pci_map_page(hwdev, page, offset, size, direction);
-
-	if (phys == 0) {
-		pci_unmap_page(hwdev, phys, size, direction);
-		phys = pci_map_page(hwdev, page, offset, size, direction);
-		/*
-		 * FIXME: If we get 0 again, we should keep this page,
-		 * map another, then free the 0 page.
-		 */
-	}
-
-	return phys;
-}
-
-/**
- * ipath_map_single - a safety wrapper around pci_map_single()
- *
- * Same idea as ipath_map_page().
- */
-dma_addr_t ipath_map_single(struct pci_dev *hwdev, void *ptr, size_t size,
-	int direction)
-{
-	dma_addr_t phys;
-
-	phys = pci_map_single(hwdev, ptr, size, direction);
-
-	if (phys == 0) {
-		pci_unmap_single(hwdev, phys, size, direction);
-		phys = pci_map_single(hwdev, ptr, size, direction);
-		/*
-		 * FIXME: If we get 0 again, we should keep this page,
-		 * map another, then free the 0 page.
-		 */
-	}
-
-	return phys;
-}
-
-/**
- * ipath_get_user_pages - lock user pages into memory
- * @start_page: the start page
- * @num_pages: the number of pages
- * @p: the output page structures
- *
- * This function takes a given start page (page aligned user virtual
- * address) and pins it and the following specified number of pages.  For
- * now, num_pages is always 1, but that will probably change at some point
- * (because caller is doing expected sends on a single virtually contiguous
- * buffer, so we can do all pages at once).
- */
-int ipath_get_user_pages(unsigned long start_page, size_t num_pages,
-			 struct page **p)
-{
-	int ret;
-
-	down_write(&current->mm->mmap_sem);
-
-	ret = __ipath_get_user_pages(start_page, num_pages, p);
-
-	up_write(&current->mm->mmap_sem);
-
-	return ret;
-}
-
-void ipath_release_user_pages(struct page **p, size_t num_pages)
-{
-	down_write(&current->mm->mmap_sem);
-
-	__ipath_release_user_pages(p, num_pages, 1);
-
-	current->mm->pinned_vm -= num_pages;
-
-	up_write(&current->mm->mmap_sem);
-}
-
-struct ipath_user_pages_work {
-	struct work_struct work;
-	struct mm_struct *mm;
-	unsigned long num_pages;
-};
-
-static void user_pages_account(struct work_struct *_work)
-{
-	struct ipath_user_pages_work *work =
-		container_of(_work, struct ipath_user_pages_work, work);
-
-	down_write(&work->mm->mmap_sem);
-	work->mm->pinned_vm -= work->num_pages;
-	up_write(&work->mm->mmap_sem);
-	mmput(work->mm);
-	kfree(work);
-}
-
-void ipath_release_user_pages_on_close(struct page **p, size_t num_pages)
-{
-	struct ipath_user_pages_work *work;
-	struct mm_struct *mm;
-
-	__ipath_release_user_pages(p, num_pages, 1);
-
-	mm = get_task_mm(current);
-	if (!mm)
-		return;
-
-	work = kmalloc(sizeof(*work), GFP_KERNEL);
-	if (!work)
-		goto bail_mm;
-
-	INIT_WORK(&work->work, user_pages_account);
-	work->mm = mm;
-	work->num_pages = num_pages;
-
-	queue_work(ib_wq, &work->work);
-	return;
-
-bail_mm:
-	mmput(mm);
-	return;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_user_sdma.c b/drivers/staging/rdma/ipath/ipath_user_sdma.c
deleted file mode 100644
index 8c12e3cccc58..000000000000
--- a/drivers/staging/rdma/ipath/ipath_user_sdma.c
+++ /dev/null
@@ -1,874 +0,0 @@
-/*
- * Copyright (c) 2007, 2008 QLogic Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-#include <linux/mm.h>
-#include <linux/types.h>
-#include <linux/device.h>
-#include <linux/dmapool.h>
-#include <linux/slab.h>
-#include <linux/list.h>
-#include <linux/highmem.h>
-#include <linux/io.h>
-#include <linux/uio.h>
-#include <linux/rbtree.h>
-#include <linux/spinlock.h>
-#include <linux/delay.h>
-
-#include "ipath_kernel.h"
-#include "ipath_user_sdma.h"
-
-/* minimum size of header */
-#define IPATH_USER_SDMA_MIN_HEADER_LENGTH	64
-/* expected size of headers (for dma_pool) */
-#define IPATH_USER_SDMA_EXP_HEADER_LENGTH	64
-/* length mask in PBC (lower 11 bits) */
-#define IPATH_PBC_LENGTH_MASK			((1 << 11) - 1)
-
-struct ipath_user_sdma_pkt {
-	u8 naddr;		/* dimension of addr (1..3) ... */
-	u32 counter;		/* sdma pkts queued counter for this entry */
-	u64 added;		/* global descq number of entries */
-
-	struct {
-		u32 offset;			/* offset for kvaddr, addr */
-		u32 length;			/* length in page */
-		u8  put_page;			/* should we put_page? */
-		u8  dma_mapped;			/* is page dma_mapped? */
-		struct page *page;		/* may be NULL (coherent mem) */
-		void *kvaddr;			/* FIXME: only for pio hack */
-		dma_addr_t addr;
-	} addr[4];   /* max pages, any more and we coalesce */
-	struct list_head list;	/* list element */
-};
-
-struct ipath_user_sdma_queue {
-	/*
-	 * pkts sent to dma engine are queued on this
-	 * list head.  the type of the elements of this
-	 * list are struct ipath_user_sdma_pkt...
-	 */
-	struct list_head sent;
-
-	/* headers with expected length are allocated from here... */
-	char header_cache_name[64];
-	struct dma_pool *header_cache;
-
-	/* packets are allocated from the slab cache... */
-	char pkt_slab_name[64];
-	struct kmem_cache *pkt_slab;
-
-	/* as packets go on the queued queue, they are counted... */
-	u32 counter;
-	u32 sent_counter;
-
-	/* dma page table */
-	struct rb_root dma_pages_root;
-
-	/* protect everything above... */
-	struct mutex lock;
-};
-
-struct ipath_user_sdma_queue *
-ipath_user_sdma_queue_create(struct device *dev, int unit, int port, int sport)
-{
-	struct ipath_user_sdma_queue *pq =
-		kmalloc(sizeof(struct ipath_user_sdma_queue), GFP_KERNEL);
-
-	if (!pq)
-		goto done;
-
-	pq->counter = 0;
-	pq->sent_counter = 0;
-	INIT_LIST_HEAD(&pq->sent);
-
-	mutex_init(&pq->lock);
-
-	snprintf(pq->pkt_slab_name, sizeof(pq->pkt_slab_name),
-		 "ipath-user-sdma-pkts-%u-%02u.%02u", unit, port, sport);
-	pq->pkt_slab = kmem_cache_create(pq->pkt_slab_name,
-					 sizeof(struct ipath_user_sdma_pkt),
-					 0, 0, NULL);
-
-	if (!pq->pkt_slab)
-		goto err_kfree;
-
-	snprintf(pq->header_cache_name, sizeof(pq->header_cache_name),
-		 "ipath-user-sdma-headers-%u-%02u.%02u", unit, port, sport);
-	pq->header_cache = dma_pool_create(pq->header_cache_name,
-					   dev,
-					   IPATH_USER_SDMA_EXP_HEADER_LENGTH,
-					   4, 0);
-	if (!pq->header_cache)
-		goto err_slab;
-
-	pq->dma_pages_root = RB_ROOT;
-
-	goto done;
-
-err_slab:
-	kmem_cache_destroy(pq->pkt_slab);
-err_kfree:
-	kfree(pq);
-	pq = NULL;
-
-done:
-	return pq;
-}
-
-static void ipath_user_sdma_init_frag(struct ipath_user_sdma_pkt *pkt,
-				      int i, size_t offset, size_t len,
-				      int put_page, int dma_mapped,
-				      struct page *page,
-				      void *kvaddr, dma_addr_t dma_addr)
-{
-	pkt->addr[i].offset = offset;
-	pkt->addr[i].length = len;
-	pkt->addr[i].put_page = put_page;
-	pkt->addr[i].dma_mapped = dma_mapped;
-	pkt->addr[i].page = page;
-	pkt->addr[i].kvaddr = kvaddr;
-	pkt->addr[i].addr = dma_addr;
-}
-
-static void ipath_user_sdma_init_header(struct ipath_user_sdma_pkt *pkt,
-					u32 counter, size_t offset,
-					size_t len, int dma_mapped,
-					struct page *page,
-					void *kvaddr, dma_addr_t dma_addr)
-{
-	pkt->naddr = 1;
-	pkt->counter = counter;
-	ipath_user_sdma_init_frag(pkt, 0, offset, len, 0, dma_mapped, page,
-				  kvaddr, dma_addr);
-}
-
-/* we've too many pages in the iovec, coalesce to a single page */
-static int ipath_user_sdma_coalesce(const struct ipath_devdata *dd,
-				    struct ipath_user_sdma_pkt *pkt,
-				    const struct iovec *iov,
-				    unsigned long niov) {
-	int ret = 0;
-	struct page *page = alloc_page(GFP_KERNEL);
-	void *mpage_save;
-	char *mpage;
-	int i;
-	int len = 0;
-	dma_addr_t dma_addr;
-
-	if (!page) {
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	mpage = kmap(page);
-	mpage_save = mpage;
-	for (i = 0; i < niov; i++) {
-		int cfur;
-
-		cfur = copy_from_user(mpage,
-				      iov[i].iov_base, iov[i].iov_len);
-		if (cfur) {
-			ret = -EFAULT;
-			goto free_unmap;
-		}
-
-		mpage += iov[i].iov_len;
-		len += iov[i].iov_len;
-	}
-
-	dma_addr = dma_map_page(&dd->pcidev->dev, page, 0, len,
-				DMA_TO_DEVICE);
-	if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
-		ret = -ENOMEM;
-		goto free_unmap;
-	}
-
-	ipath_user_sdma_init_frag(pkt, 1, 0, len, 0, 1, page, mpage_save,
-				  dma_addr);
-	pkt->naddr = 2;
-
-	goto done;
-
-free_unmap:
-	kunmap(page);
-	__free_page(page);
-done:
-	return ret;
-}
-
-/* how many pages in this iovec element? */
-static int ipath_user_sdma_num_pages(const struct iovec *iov)
-{
-	const unsigned long addr  = (unsigned long) iov->iov_base;
-	const unsigned long  len  = iov->iov_len;
-	const unsigned long spage = addr & PAGE_MASK;
-	const unsigned long epage = (addr + len - 1) & PAGE_MASK;
-
-	return 1 + ((epage - spage) >> PAGE_SHIFT);
-}
-
-/* truncate length to page boundary */
-static int ipath_user_sdma_page_length(unsigned long addr, unsigned long len)
-{
-	const unsigned long offset = offset_in_page(addr);
-
-	return ((offset + len) > PAGE_SIZE) ? (PAGE_SIZE - offset) : len;
-}
-
-static void ipath_user_sdma_free_pkt_frag(struct device *dev,
-					  struct ipath_user_sdma_queue *pq,
-					  struct ipath_user_sdma_pkt *pkt,
-					  int frag)
-{
-	const int i = frag;
-
-	if (pkt->addr[i].page) {
-		if (pkt->addr[i].dma_mapped)
-			dma_unmap_page(dev,
-				       pkt->addr[i].addr,
-				       pkt->addr[i].length,
-				       DMA_TO_DEVICE);
-
-		if (pkt->addr[i].kvaddr)
-			kunmap(pkt->addr[i].page);
-
-		if (pkt->addr[i].put_page)
-			put_page(pkt->addr[i].page);
-		else
-			__free_page(pkt->addr[i].page);
-	} else if (pkt->addr[i].kvaddr)
-		/* free coherent mem from cache... */
-		dma_pool_free(pq->header_cache,
-			      pkt->addr[i].kvaddr, pkt->addr[i].addr);
-}
-
-/* return number of pages pinned... */
-static int ipath_user_sdma_pin_pages(const struct ipath_devdata *dd,
-				     struct ipath_user_sdma_pkt *pkt,
-				     unsigned long addr, int tlen, int npages)
-{
-	struct page *pages[2];
-	int j;
-	int ret;
-
-	ret = get_user_pages_fast(addr, npages, 0, pages);
-	if (ret != npages) {
-		int i;
-
-		for (i = 0; i < ret; i++)
-			put_page(pages[i]);
-
-		ret = -ENOMEM;
-		goto done;
-	}
-
-	for (j = 0; j < npages; j++) {
-		/* map the pages... */
-		const int flen =
-			ipath_user_sdma_page_length(addr, tlen);
-		dma_addr_t dma_addr =
-			dma_map_page(&dd->pcidev->dev,
-				     pages[j], 0, flen, DMA_TO_DEVICE);
-		unsigned long fofs = offset_in_page(addr);
-
-		if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
-			ret = -ENOMEM;
-			goto done;
-		}
-
-		ipath_user_sdma_init_frag(pkt, pkt->naddr, fofs, flen, 1, 1,
-					  pages[j], kmap(pages[j]),
-					  dma_addr);
-
-		pkt->naddr++;
-		addr += flen;
-		tlen -= flen;
-	}
-
-done:
-	return ret;
-}
-
-static int ipath_user_sdma_pin_pkt(const struct ipath_devdata *dd,
-				   struct ipath_user_sdma_queue *pq,
-				   struct ipath_user_sdma_pkt *pkt,
-				   const struct iovec *iov,
-				   unsigned long niov)
-{
-	int ret = 0;
-	unsigned long idx;
-
-	for (idx = 0; idx < niov; idx++) {
-		const int npages = ipath_user_sdma_num_pages(iov + idx);
-		const unsigned long addr = (unsigned long) iov[idx].iov_base;
-
-		ret = ipath_user_sdma_pin_pages(dd, pkt,
-						addr, iov[idx].iov_len,
-						npages);
-		if (ret < 0)
-			goto free_pkt;
-	}
-
-	goto done;
-
-free_pkt:
-	for (idx = 0; idx < pkt->naddr; idx++)
-		ipath_user_sdma_free_pkt_frag(&dd->pcidev->dev, pq, pkt, idx);
-
-done:
-	return ret;
-}
-
-static int ipath_user_sdma_init_payload(const struct ipath_devdata *dd,
-					struct ipath_user_sdma_queue *pq,
-					struct ipath_user_sdma_pkt *pkt,
-					const struct iovec *iov,
-					unsigned long niov, int npages)
-{
-	int ret = 0;
-
-	if (npages >= ARRAY_SIZE(pkt->addr))
-		ret = ipath_user_sdma_coalesce(dd, pkt, iov, niov);
-	else
-		ret = ipath_user_sdma_pin_pkt(dd, pq, pkt, iov, niov);
-
-	return ret;
-}
-
-/* free a packet list -- return counter value of last packet */
-static void ipath_user_sdma_free_pkt_list(struct device *dev,
-					  struct ipath_user_sdma_queue *pq,
-					  struct list_head *list)
-{
-	struct ipath_user_sdma_pkt *pkt, *pkt_next;
-
-	list_for_each_entry_safe(pkt, pkt_next, list, list) {
-		int i;
-
-		for (i = 0; i < pkt->naddr; i++)
-			ipath_user_sdma_free_pkt_frag(dev, pq, pkt, i);
-
-		kmem_cache_free(pq->pkt_slab, pkt);
-	}
-}
-
-/*
- * copy headers, coalesce etc -- pq->lock must be held
- *
- * we queue all the packets to list, returning the
- * number of bytes total.  list must be empty initially,
- * as, if there is an error we clean it...
- */
-static int ipath_user_sdma_queue_pkts(const struct ipath_devdata *dd,
-				      struct ipath_user_sdma_queue *pq,
-				      struct list_head *list,
-				      const struct iovec *iov,
-				      unsigned long niov,
-				      int maxpkts)
-{
-	unsigned long idx = 0;
-	int ret = 0;
-	int npkts = 0;
-	struct page *page = NULL;
-	__le32 *pbc;
-	dma_addr_t dma_addr;
-	struct ipath_user_sdma_pkt *pkt = NULL;
-	size_t len;
-	size_t nw;
-	u32 counter = pq->counter;
-	int dma_mapped = 0;
-
-	while (idx < niov && npkts < maxpkts) {
-		const unsigned long addr = (unsigned long) iov[idx].iov_base;
-		const unsigned long idx_save = idx;
-		unsigned pktnw;
-		unsigned pktnwc;
-		int nfrags = 0;
-		int npages = 0;
-		int cfur;
-
-		dma_mapped = 0;
-		len = iov[idx].iov_len;
-		nw = len >> 2;
-		page = NULL;
-
-		pkt = kmem_cache_alloc(pq->pkt_slab, GFP_KERNEL);
-		if (!pkt) {
-			ret = -ENOMEM;
-			goto free_list;
-		}
-
-		if (len < IPATH_USER_SDMA_MIN_HEADER_LENGTH ||
-		    len > PAGE_SIZE || len & 3 || addr & 3) {
-			ret = -EINVAL;
-			goto free_pkt;
-		}
-
-		if (len == IPATH_USER_SDMA_EXP_HEADER_LENGTH)
-			pbc = dma_pool_alloc(pq->header_cache, GFP_KERNEL,
-					     &dma_addr);
-		else
-			pbc = NULL;
-
-		if (!pbc) {
-			page = alloc_page(GFP_KERNEL);
-			if (!page) {
-				ret = -ENOMEM;
-				goto free_pkt;
-			}
-			pbc = kmap(page);
-		}
-
-		cfur = copy_from_user(pbc, iov[idx].iov_base, len);
-		if (cfur) {
-			ret = -EFAULT;
-			goto free_pbc;
-		}
-
-		/*
-		 * this assignment is a bit strange.  it's because the
-		 * the pbc counts the number of 32 bit words in the full
-		 * packet _except_ the first word of the pbc itself...
-		 */
-		pktnwc = nw - 1;
-
-		/*
-		 * pktnw computation yields the number of 32 bit words
-		 * that the caller has indicated in the PBC.  note that
-		 * this is one less than the total number of words that
-		 * goes to the send DMA engine as the first 32 bit word
-		 * of the PBC itself is not counted.  Armed with this count,
-		 * we can verify that the packet is consistent with the
-		 * iovec lengths.
-		 */
-		pktnw = le32_to_cpu(*pbc) & IPATH_PBC_LENGTH_MASK;
-		if (pktnw < pktnwc || pktnw > pktnwc + (PAGE_SIZE >> 2)) {
-			ret = -EINVAL;
-			goto free_pbc;
-		}
-
-
-		idx++;
-		while (pktnwc < pktnw && idx < niov) {
-			const size_t slen = iov[idx].iov_len;
-			const unsigned long faddr =
-				(unsigned long) iov[idx].iov_base;
-
-			if (slen & 3 || faddr & 3 || !slen ||
-			    slen > PAGE_SIZE) {
-				ret = -EINVAL;
-				goto free_pbc;
-			}
-
-			npages++;
-			if ((faddr & PAGE_MASK) !=
-			    ((faddr + slen - 1) & PAGE_MASK))
-				npages++;
-
-			pktnwc += slen >> 2;
-			idx++;
-			nfrags++;
-		}
-
-		if (pktnwc != pktnw) {
-			ret = -EINVAL;
-			goto free_pbc;
-		}
-
-		if (page) {
-			dma_addr = dma_map_page(&dd->pcidev->dev,
-						page, 0, len, DMA_TO_DEVICE);
-			if (dma_mapping_error(&dd->pcidev->dev, dma_addr)) {
-				ret = -ENOMEM;
-				goto free_pbc;
-			}
-
-			dma_mapped = 1;
-		}
-
-		ipath_user_sdma_init_header(pkt, counter, 0, len, dma_mapped,
-					    page, pbc, dma_addr);
-
-		if (nfrags) {
-			ret = ipath_user_sdma_init_payload(dd, pq, pkt,
-							   iov + idx_save + 1,
-							   nfrags, npages);
-			if (ret < 0)
-				goto free_pbc_dma;
-		}
-
-		counter++;
-		npkts++;
-
-		list_add_tail(&pkt->list, list);
-	}
-
-	ret = idx;
-	goto done;
-
-free_pbc_dma:
-	if (dma_mapped)
-		dma_unmap_page(&dd->pcidev->dev, dma_addr, len, DMA_TO_DEVICE);
-free_pbc:
-	if (page) {
-		kunmap(page);
-		__free_page(page);
-	} else
-		dma_pool_free(pq->header_cache, pbc, dma_addr);
-free_pkt:
-	kmem_cache_free(pq->pkt_slab, pkt);
-free_list:
-	ipath_user_sdma_free_pkt_list(&dd->pcidev->dev, pq, list);
-done:
-	return ret;
-}
-
-static void ipath_user_sdma_set_complete_counter(struct ipath_user_sdma_queue *pq,
-						 u32 c)
-{
-	pq->sent_counter = c;
-}
-
-/* try to clean out queue -- needs pq->lock */
-static int ipath_user_sdma_queue_clean(const struct ipath_devdata *dd,
-				       struct ipath_user_sdma_queue *pq)
-{
-	struct list_head free_list;
-	struct ipath_user_sdma_pkt *pkt;
-	struct ipath_user_sdma_pkt *pkt_prev;
-	int ret = 0;
-
-	INIT_LIST_HEAD(&free_list);
-
-	list_for_each_entry_safe(pkt, pkt_prev, &pq->sent, list) {
-		s64 descd = dd->ipath_sdma_descq_removed - pkt->added;
-
-		if (descd < 0)
-			break;
-
-		list_move_tail(&pkt->list, &free_list);
-
-		/* one more packet cleaned */
-		ret++;
-	}
-
-	if (!list_empty(&free_list)) {
-		u32 counter;
-
-		pkt = list_entry(free_list.prev,
-				 struct ipath_user_sdma_pkt, list);
-		counter = pkt->counter;
-
-		ipath_user_sdma_free_pkt_list(&dd->pcidev->dev, pq, &free_list);
-		ipath_user_sdma_set_complete_counter(pq, counter);
-	}
-
-	return ret;
-}
-
-void ipath_user_sdma_queue_destroy(struct ipath_user_sdma_queue *pq)
-{
-	if (!pq)
-		return;
-
-	kmem_cache_destroy(pq->pkt_slab);
-	dma_pool_destroy(pq->header_cache);
-	kfree(pq);
-}
-
-/* clean descriptor queue, returns > 0 if some elements cleaned */
-static int ipath_user_sdma_hwqueue_clean(struct ipath_devdata *dd)
-{
-	int ret;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-	ret = ipath_sdma_make_progress(dd);
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	return ret;
-}
-
-/* we're in close, drain packets so that we can cleanup successfully... */
-void ipath_user_sdma_queue_drain(struct ipath_devdata *dd,
-				 struct ipath_user_sdma_queue *pq)
-{
-	int i;
-
-	if (!pq)
-		return;
-
-	for (i = 0; i < 100; i++) {
-		mutex_lock(&pq->lock);
-		if (list_empty(&pq->sent)) {
-			mutex_unlock(&pq->lock);
-			break;
-		}
-		ipath_user_sdma_hwqueue_clean(dd);
-		ipath_user_sdma_queue_clean(dd, pq);
-		mutex_unlock(&pq->lock);
-		msleep(10);
-	}
-
-	if (!list_empty(&pq->sent)) {
-		struct list_head free_list;
-
-		printk(KERN_INFO "drain: lists not empty: forcing!\n");
-		INIT_LIST_HEAD(&free_list);
-		mutex_lock(&pq->lock);
-		list_splice_init(&pq->sent, &free_list);
-		ipath_user_sdma_free_pkt_list(&dd->pcidev->dev, pq, &free_list);
-		mutex_unlock(&pq->lock);
-	}
-}
-
-static inline __le64 ipath_sdma_make_desc0(struct ipath_devdata *dd,
-					   u64 addr, u64 dwlen, u64 dwoffset)
-{
-	return cpu_to_le64(/* SDmaPhyAddr[31:0] */
-			   ((addr & 0xfffffffcULL) << 32) |
-			   /* SDmaGeneration[1:0] */
-			   ((dd->ipath_sdma_generation & 3ULL) << 30) |
-			   /* SDmaDwordCount[10:0] */
-			   ((dwlen & 0x7ffULL) << 16) |
-			   /* SDmaBufOffset[12:2] */
-			   (dwoffset & 0x7ffULL));
-}
-
-static inline __le64 ipath_sdma_make_first_desc0(__le64 descq)
-{
-	return descq | cpu_to_le64(1ULL << 12);
-}
-
-static inline __le64 ipath_sdma_make_last_desc0(__le64 descq)
-{
-					      /* last */  /* dma head */
-	return descq | cpu_to_le64(1ULL << 11 | 1ULL << 13);
-}
-
-static inline __le64 ipath_sdma_make_desc1(u64 addr)
-{
-	/* SDmaPhyAddr[47:32] */
-	return cpu_to_le64(addr >> 32);
-}
-
-static void ipath_user_sdma_send_frag(struct ipath_devdata *dd,
-				      struct ipath_user_sdma_pkt *pkt, int idx,
-				      unsigned ofs, u16 tail)
-{
-	const u64 addr = (u64) pkt->addr[idx].addr +
-		(u64) pkt->addr[idx].offset;
-	const u64 dwlen = (u64) pkt->addr[idx].length / 4;
-	__le64 *descqp;
-	__le64 descq0;
-
-	descqp = &dd->ipath_sdma_descq[tail].qw[0];
-
-	descq0 = ipath_sdma_make_desc0(dd, addr, dwlen, ofs);
-	if (idx == 0)
-		descq0 = ipath_sdma_make_first_desc0(descq0);
-	if (idx == pkt->naddr - 1)
-		descq0 = ipath_sdma_make_last_desc0(descq0);
-
-	descqp[0] = descq0;
-	descqp[1] = ipath_sdma_make_desc1(addr);
-}
-
-/* pq->lock must be held, get packets on the wire... */
-static int ipath_user_sdma_push_pkts(struct ipath_devdata *dd,
-				     struct ipath_user_sdma_queue *pq,
-				     struct list_head *pktlist)
-{
-	int ret = 0;
-	unsigned long flags;
-	u16 tail;
-
-	if (list_empty(pktlist))
-		return 0;
-
-	if (unlikely(!(dd->ipath_flags & IPATH_LINKACTIVE)))
-		return -ECOMM;
-
-	spin_lock_irqsave(&dd->ipath_sdma_lock, flags);
-
-	if (unlikely(dd->ipath_sdma_status & IPATH_SDMA_ABORT_MASK)) {
-		ret = -ECOMM;
-		goto unlock;
-	}
-
-	tail = dd->ipath_sdma_descq_tail;
-	while (!list_empty(pktlist)) {
-		struct ipath_user_sdma_pkt *pkt =
-			list_entry(pktlist->next, struct ipath_user_sdma_pkt,
-				   list);
-		int i;
-		unsigned ofs = 0;
-		u16 dtail = tail;
-
-		if (pkt->naddr > ipath_sdma_descq_freecnt(dd))
-			goto unlock_check_tail;
-
-		for (i = 0; i < pkt->naddr; i++) {
-			ipath_user_sdma_send_frag(dd, pkt, i, ofs, tail);
-			ofs += pkt->addr[i].length >> 2;
-
-			if (++tail == dd->ipath_sdma_descq_cnt) {
-				tail = 0;
-				++dd->ipath_sdma_generation;
-			}
-		}
-
-		if ((ofs<<2) > dd->ipath_ibmaxlen) {
-			ipath_dbg("packet size %X > ibmax %X, fail\n",
-				ofs<<2, dd->ipath_ibmaxlen);
-			ret = -EMSGSIZE;
-			goto unlock;
-		}
-
-		/*
-		 * if the packet is >= 2KB mtu equivalent, we have to use
-		 * the large buffers, and have to mark each descriptor as
-		 * part of a large buffer packet.
-		 */
-		if (ofs >= IPATH_SMALLBUF_DWORDS) {
-			for (i = 0; i < pkt->naddr; i++) {
-				dd->ipath_sdma_descq[dtail].qw[0] |=
-					cpu_to_le64(1ULL << 14);
-				if (++dtail == dd->ipath_sdma_descq_cnt)
-					dtail = 0;
-			}
-		}
-
-		dd->ipath_sdma_descq_added += pkt->naddr;
-		pkt->added = dd->ipath_sdma_descq_added;
-		list_move_tail(&pkt->list, &pq->sent);
-		ret++;
-	}
-
-unlock_check_tail:
-	/* advance the tail on the chip if necessary */
-	if (dd->ipath_sdma_descq_tail != tail) {
-		wmb();
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_senddmatail, tail);
-		dd->ipath_sdma_descq_tail = tail;
-	}
-
-unlock:
-	spin_unlock_irqrestore(&dd->ipath_sdma_lock, flags);
-
-	return ret;
-}
-
-int ipath_user_sdma_writev(struct ipath_devdata *dd,
-			   struct ipath_user_sdma_queue *pq,
-			   const struct iovec *iov,
-			   unsigned long dim)
-{
-	int ret = 0;
-	struct list_head list;
-	int npkts = 0;
-
-	INIT_LIST_HEAD(&list);
-
-	mutex_lock(&pq->lock);
-
-	if (dd->ipath_sdma_descq_added != dd->ipath_sdma_descq_removed) {
-		ipath_user_sdma_hwqueue_clean(dd);
-		ipath_user_sdma_queue_clean(dd, pq);
-	}
-
-	while (dim) {
-		const int mxp = 8;
-
-		ret = ipath_user_sdma_queue_pkts(dd, pq, &list, iov, dim, mxp);
-		if (ret <= 0)
-			goto done_unlock;
-		else {
-			dim -= ret;
-			iov += ret;
-		}
-
-		/* force packets onto the sdma hw queue... */
-		if (!list_empty(&list)) {
-			/*
-			 * lazily clean hw queue.  the 4 is a guess of about
-			 * how many sdma descriptors a packet will take (it
-			 * doesn't have to be perfect).
-			 */
-			if (ipath_sdma_descq_freecnt(dd) < ret * 4) {
-				ipath_user_sdma_hwqueue_clean(dd);
-				ipath_user_sdma_queue_clean(dd, pq);
-			}
-
-			ret = ipath_user_sdma_push_pkts(dd, pq, &list);
-			if (ret < 0)
-				goto done_unlock;
-			else {
-				npkts += ret;
-				pq->counter += ret;
-
-				if (!list_empty(&list))
-					goto done_unlock;
-			}
-		}
-	}
-
-done_unlock:
-	if (!list_empty(&list))
-		ipath_user_sdma_free_pkt_list(&dd->pcidev->dev, pq, &list);
-	mutex_unlock(&pq->lock);
-
-	return (ret < 0) ? ret : npkts;
-}
-
-int ipath_user_sdma_make_progress(struct ipath_devdata *dd,
-				  struct ipath_user_sdma_queue *pq)
-{
-	int ret = 0;
-
-	mutex_lock(&pq->lock);
-	ipath_user_sdma_hwqueue_clean(dd);
-	ret = ipath_user_sdma_queue_clean(dd, pq);
-	mutex_unlock(&pq->lock);
-
-	return ret;
-}
-
-u32 ipath_user_sdma_complete_counter(const struct ipath_user_sdma_queue *pq)
-{
-	return pq->sent_counter;
-}
-
-u32 ipath_user_sdma_inflight_counter(struct ipath_user_sdma_queue *pq)
-{
-	return pq->counter;
-}
-
diff --git a/drivers/staging/rdma/ipath/ipath_user_sdma.h b/drivers/staging/rdma/ipath/ipath_user_sdma.h
deleted file mode 100644
index fc76316c4a58..000000000000
--- a/drivers/staging/rdma/ipath/ipath_user_sdma.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Copyright (c) 2007, 2008 QLogic Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-#include <linux/device.h>
-
-struct ipath_user_sdma_queue;
-
-struct ipath_user_sdma_queue *
-ipath_user_sdma_queue_create(struct device *dev, int unit, int port, int sport);
-void ipath_user_sdma_queue_destroy(struct ipath_user_sdma_queue *pq);
-
-int ipath_user_sdma_writev(struct ipath_devdata *dd,
-			   struct ipath_user_sdma_queue *pq,
-			   const struct iovec *iov,
-			   unsigned long dim);
-
-int ipath_user_sdma_make_progress(struct ipath_devdata *dd,
-				  struct ipath_user_sdma_queue *pq);
-
-void ipath_user_sdma_queue_drain(struct ipath_devdata *dd,
-				 struct ipath_user_sdma_queue *pq);
-
-u32 ipath_user_sdma_complete_counter(const struct ipath_user_sdma_queue *pq);
-u32 ipath_user_sdma_inflight_counter(struct ipath_user_sdma_queue *pq);
diff --git a/drivers/staging/rdma/ipath/ipath_verbs.c b/drivers/staging/rdma/ipath/ipath_verbs.c
deleted file mode 100644
index 1778dee13f99..000000000000
--- a/drivers/staging/rdma/ipath/ipath_verbs.c
+++ /dev/null
@@ -1,2377 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 <rdma/ib_mad.h>
-#include <rdma/ib_user_verbs.h>
-#include <linux/io.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/utsname.h>
-#include <linux/rculist.h>
-
-#include "ipath_kernel.h"
-#include "ipath_verbs.h"
-#include "ipath_common.h"
-
-static unsigned int ib_ipath_qp_table_size = 251;
-module_param_named(qp_table_size, ib_ipath_qp_table_size, uint, S_IRUGO);
-MODULE_PARM_DESC(qp_table_size, "QP table size");
-
-unsigned int ib_ipath_lkey_table_size = 12;
-module_param_named(lkey_table_size, ib_ipath_lkey_table_size, uint,
-		   S_IRUGO);
-MODULE_PARM_DESC(lkey_table_size,
-		 "LKEY table size in bits (2^n, 1 <= n <= 23)");
-
-static unsigned int ib_ipath_max_pds = 0xFFFF;
-module_param_named(max_pds, ib_ipath_max_pds, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_pds,
-		 "Maximum number of protection domains to support");
-
-static unsigned int ib_ipath_max_ahs = 0xFFFF;
-module_param_named(max_ahs, ib_ipath_max_ahs, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_ahs, "Maximum number of address handles to support");
-
-unsigned int ib_ipath_max_cqes = 0x2FFFF;
-module_param_named(max_cqes, ib_ipath_max_cqes, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_cqes,
-		 "Maximum number of completion queue entries to support");
-
-unsigned int ib_ipath_max_cqs = 0x1FFFF;
-module_param_named(max_cqs, ib_ipath_max_cqs, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_cqs, "Maximum number of completion queues to support");
-
-unsigned int ib_ipath_max_qp_wrs = 0x3FFF;
-module_param_named(max_qp_wrs, ib_ipath_max_qp_wrs, uint,
-		   S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_qp_wrs, "Maximum number of QP WRs to support");
-
-unsigned int ib_ipath_max_qps = 16384;
-module_param_named(max_qps, ib_ipath_max_qps, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_qps, "Maximum number of QPs to support");
-
-unsigned int ib_ipath_max_sges = 0x60;
-module_param_named(max_sges, ib_ipath_max_sges, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_sges, "Maximum number of SGEs to support");
-
-unsigned int ib_ipath_max_mcast_grps = 16384;
-module_param_named(max_mcast_grps, ib_ipath_max_mcast_grps, uint,
-		   S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_mcast_grps,
-		 "Maximum number of multicast groups to support");
-
-unsigned int ib_ipath_max_mcast_qp_attached = 16;
-module_param_named(max_mcast_qp_attached, ib_ipath_max_mcast_qp_attached,
-		   uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_mcast_qp_attached,
-		 "Maximum number of attached QPs to support");
-
-unsigned int ib_ipath_max_srqs = 1024;
-module_param_named(max_srqs, ib_ipath_max_srqs, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_srqs, "Maximum number of SRQs to support");
-
-unsigned int ib_ipath_max_srq_sges = 128;
-module_param_named(max_srq_sges, ib_ipath_max_srq_sges,
-		   uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_srq_sges, "Maximum number of SRQ SGEs to support");
-
-unsigned int ib_ipath_max_srq_wrs = 0x1FFFF;
-module_param_named(max_srq_wrs, ib_ipath_max_srq_wrs,
-		   uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(max_srq_wrs, "Maximum number of SRQ WRs support");
-
-static unsigned int ib_ipath_disable_sma;
-module_param_named(disable_sma, ib_ipath_disable_sma, uint, S_IWUSR | S_IRUGO);
-MODULE_PARM_DESC(disable_sma, "Disable the SMA");
-
-/*
- * Note that it is OK to post send work requests in the SQE and ERR
- * states; ipath_do_send() will process them and generate error
- * completions as per IB 1.2 C10-96.
- */
-const int ib_ipath_state_ops[IB_QPS_ERR + 1] = {
-	[IB_QPS_RESET] = 0,
-	[IB_QPS_INIT] = IPATH_POST_RECV_OK,
-	[IB_QPS_RTR] = IPATH_POST_RECV_OK | IPATH_PROCESS_RECV_OK,
-	[IB_QPS_RTS] = IPATH_POST_RECV_OK | IPATH_PROCESS_RECV_OK |
-	    IPATH_POST_SEND_OK | IPATH_PROCESS_SEND_OK |
-	    IPATH_PROCESS_NEXT_SEND_OK,
-	[IB_QPS_SQD] = IPATH_POST_RECV_OK | IPATH_PROCESS_RECV_OK |
-	    IPATH_POST_SEND_OK | IPATH_PROCESS_SEND_OK,
-	[IB_QPS_SQE] = IPATH_POST_RECV_OK | IPATH_PROCESS_RECV_OK |
-	    IPATH_POST_SEND_OK | IPATH_FLUSH_SEND,
-	[IB_QPS_ERR] = IPATH_POST_RECV_OK | IPATH_FLUSH_RECV |
-	    IPATH_POST_SEND_OK | IPATH_FLUSH_SEND,
-};
-
-struct ipath_ucontext {
-	struct ib_ucontext ibucontext;
-};
-
-static inline struct ipath_ucontext *to_iucontext(struct ib_ucontext
-						  *ibucontext)
-{
-	return container_of(ibucontext, struct ipath_ucontext, ibucontext);
-}
-
-/*
- * Translate ib_wr_opcode into ib_wc_opcode.
- */
-const enum ib_wc_opcode ib_ipath_wc_opcode[] = {
-	[IB_WR_RDMA_WRITE] = IB_WC_RDMA_WRITE,
-	[IB_WR_RDMA_WRITE_WITH_IMM] = IB_WC_RDMA_WRITE,
-	[IB_WR_SEND] = IB_WC_SEND,
-	[IB_WR_SEND_WITH_IMM] = IB_WC_SEND,
-	[IB_WR_RDMA_READ] = IB_WC_RDMA_READ,
-	[IB_WR_ATOMIC_CMP_AND_SWP] = IB_WC_COMP_SWAP,
-	[IB_WR_ATOMIC_FETCH_AND_ADD] = IB_WC_FETCH_ADD
-};
-
-/*
- * System image GUID.
- */
-static __be64 sys_image_guid;
-
-/**
- * ipath_copy_sge - copy data to SGE memory
- * @ss: the SGE state
- * @data: the data to copy
- * @length: the length of the data
- */
-void ipath_copy_sge(struct ipath_sge_state *ss, void *data, u32 length)
-{
-	struct ipath_sge *sge = &ss->sge;
-
-	while (length) {
-		u32 len = sge->length;
-
-		if (len > length)
-			len = length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		memcpy(sge->vaddr, data, len);
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--ss->num_sge)
-				*sge = *ss->sg_list++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-		data += len;
-		length -= len;
-	}
-}
-
-/**
- * ipath_skip_sge - skip over SGE memory - XXX almost dup of prev func
- * @ss: the SGE state
- * @length: the number of bytes to skip
- */
-void ipath_skip_sge(struct ipath_sge_state *ss, u32 length)
-{
-	struct ipath_sge *sge = &ss->sge;
-
-	while (length) {
-		u32 len = sge->length;
-
-		if (len > length)
-			len = length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--ss->num_sge)
-				*sge = *ss->sg_list++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-		length -= len;
-	}
-}
-
-/*
- * Count the number of DMA descriptors needed to send length bytes of data.
- * Don't modify the ipath_sge_state to get the count.
- * Return zero if any of the segments is not aligned.
- */
-static u32 ipath_count_sge(struct ipath_sge_state *ss, u32 length)
-{
-	struct ipath_sge *sg_list = ss->sg_list;
-	struct ipath_sge sge = ss->sge;
-	u8 num_sge = ss->num_sge;
-	u32 ndesc = 1;	/* count the header */
-
-	while (length) {
-		u32 len = sge.length;
-
-		if (len > length)
-			len = length;
-		if (len > sge.sge_length)
-			len = sge.sge_length;
-		BUG_ON(len == 0);
-		if (((long) sge.vaddr & (sizeof(u32) - 1)) ||
-		    (len != length && (len & (sizeof(u32) - 1)))) {
-			ndesc = 0;
-			break;
-		}
-		ndesc++;
-		sge.vaddr += len;
-		sge.length -= len;
-		sge.sge_length -= len;
-		if (sge.sge_length == 0) {
-			if (--num_sge)
-				sge = *sg_list++;
-		} else if (sge.length == 0 && sge.mr != NULL) {
-			if (++sge.n >= IPATH_SEGSZ) {
-				if (++sge.m >= sge.mr->mapsz)
-					break;
-				sge.n = 0;
-			}
-			sge.vaddr =
-				sge.mr->map[sge.m]->segs[sge.n].vaddr;
-			sge.length =
-				sge.mr->map[sge.m]->segs[sge.n].length;
-		}
-		length -= len;
-	}
-	return ndesc;
-}
-
-/*
- * Copy from the SGEs to the data buffer.
- */
-static void ipath_copy_from_sge(void *data, struct ipath_sge_state *ss,
-				u32 length)
-{
-	struct ipath_sge *sge = &ss->sge;
-
-	while (length) {
-		u32 len = sge->length;
-
-		if (len > length)
-			len = length;
-		if (len > sge->sge_length)
-			len = sge->sge_length;
-		BUG_ON(len == 0);
-		memcpy(data, sge->vaddr, len);
-		sge->vaddr += len;
-		sge->length -= len;
-		sge->sge_length -= len;
-		if (sge->sge_length == 0) {
-			if (--ss->num_sge)
-				*sge = *ss->sg_list++;
-		} else if (sge->length == 0 && sge->mr != NULL) {
-			if (++sge->n >= IPATH_SEGSZ) {
-				if (++sge->m >= sge->mr->mapsz)
-					break;
-				sge->n = 0;
-			}
-			sge->vaddr =
-				sge->mr->map[sge->m]->segs[sge->n].vaddr;
-			sge->length =
-				sge->mr->map[sge->m]->segs[sge->n].length;
-		}
-		data += len;
-		length -= len;
-	}
-}
-
-/**
- * ipath_post_one_send - post one RC, UC, or UD send work request
- * @qp: the QP to post on
- * @wr: the work request to send
- */
-static int ipath_post_one_send(struct ipath_qp *qp, struct ib_send_wr *wr)
-{
-	struct ipath_swqe *wqe;
-	u32 next;
-	int i;
-	int j;
-	int acc;
-	int ret;
-	unsigned long flags;
-	struct ipath_devdata *dd = to_idev(qp->ibqp.device)->dd;
-
-	spin_lock_irqsave(&qp->s_lock, flags);
-
-	if (qp->ibqp.qp_type != IB_QPT_SMI &&
-	    !(dd->ipath_flags & IPATH_LINKACTIVE)) {
-		ret = -ENETDOWN;
-		goto bail;
-	}
-
-	/* Check that state is OK to post send. */
-	if (unlikely(!(ib_ipath_state_ops[qp->state] & IPATH_POST_SEND_OK)))
-		goto bail_inval;
-
-	/* IB spec says that num_sge == 0 is OK. */
-	if (wr->num_sge > qp->s_max_sge)
-		goto bail_inval;
-
-	/*
-	 * Don't allow RDMA reads or atomic operations on UC or
-	 * undefined operations.
-	 * Make sure buffer is large enough to hold the result for atomics.
-	 */
-	if (qp->ibqp.qp_type == IB_QPT_UC) {
-		if ((unsigned) wr->opcode >= IB_WR_RDMA_READ)
-			goto bail_inval;
-	} else if (qp->ibqp.qp_type == IB_QPT_UD) {
-		/* Check UD opcode */
-		if (wr->opcode != IB_WR_SEND &&
-		    wr->opcode != IB_WR_SEND_WITH_IMM)
-			goto bail_inval;
-		/* Check UD destination address PD */
-		if (qp->ibqp.pd != ud_wr(wr)->ah->pd)
-			goto bail_inval;
-	} else if ((unsigned) wr->opcode > IB_WR_ATOMIC_FETCH_AND_ADD)
-		goto bail_inval;
-	else if (wr->opcode >= IB_WR_ATOMIC_CMP_AND_SWP &&
-		   (wr->num_sge == 0 ||
-		    wr->sg_list[0].length < sizeof(u64) ||
-		    wr->sg_list[0].addr & (sizeof(u64) - 1)))
-		goto bail_inval;
-	else if (wr->opcode >= IB_WR_RDMA_READ && !qp->s_max_rd_atomic)
-		goto bail_inval;
-
-	next = qp->s_head + 1;
-	if (next >= qp->s_size)
-		next = 0;
-	if (next == qp->s_last) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	wqe = get_swqe_ptr(qp, qp->s_head);
-
-	if (qp->ibqp.qp_type != IB_QPT_UC &&
-	    qp->ibqp.qp_type != IB_QPT_RC)
-		memcpy(&wqe->ud_wr, ud_wr(wr), sizeof(wqe->ud_wr));
-	else if (wr->opcode == IB_WR_RDMA_WRITE_WITH_IMM ||
-		 wr->opcode == IB_WR_RDMA_WRITE ||
-		 wr->opcode == IB_WR_RDMA_READ)
-		memcpy(&wqe->rdma_wr, rdma_wr(wr), sizeof(wqe->rdma_wr));
-	else if (wr->opcode == IB_WR_ATOMIC_CMP_AND_SWP ||
-		 wr->opcode == IB_WR_ATOMIC_FETCH_AND_ADD)
-		memcpy(&wqe->atomic_wr, atomic_wr(wr), sizeof(wqe->atomic_wr));
-	else
-		memcpy(&wqe->wr, wr, sizeof(wqe->wr));
-
-	wqe->length = 0;
-	if (wr->num_sge) {
-		acc = wr->opcode >= IB_WR_RDMA_READ ?
-			IB_ACCESS_LOCAL_WRITE : 0;
-		for (i = 0, j = 0; i < wr->num_sge; i++) {
-			u32 length = wr->sg_list[i].length;
-			int ok;
-
-			if (length == 0)
-				continue;
-			ok = ipath_lkey_ok(qp, &wqe->sg_list[j],
-					   &wr->sg_list[i], acc);
-			if (!ok)
-				goto bail_inval;
-			wqe->length += length;
-			j++;
-		}
-		wqe->wr.num_sge = j;
-	}
-	if (qp->ibqp.qp_type == IB_QPT_UC ||
-	    qp->ibqp.qp_type == IB_QPT_RC) {
-		if (wqe->length > 0x80000000U)
-			goto bail_inval;
-	} else if (wqe->length > to_idev(qp->ibqp.device)->dd->ipath_ibmtu)
-		goto bail_inval;
-	wqe->ssn = qp->s_ssn++;
-	qp->s_head = next;
-
-	ret = 0;
-	goto bail;
-
-bail_inval:
-	ret = -EINVAL;
-bail:
-	spin_unlock_irqrestore(&qp->s_lock, flags);
-	return ret;
-}
-
-/**
- * ipath_post_send - post a send on a QP
- * @ibqp: the QP to post the send on
- * @wr: the list of work requests to post
- * @bad_wr: the first bad WR is put here
- *
- * This may be called from interrupt context.
- */
-static int ipath_post_send(struct ib_qp *ibqp, struct ib_send_wr *wr,
-			   struct ib_send_wr **bad_wr)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-	int err = 0;
-
-	for (; wr; wr = wr->next) {
-		err = ipath_post_one_send(qp, wr);
-		if (err) {
-			*bad_wr = wr;
-			goto bail;
-		}
-	}
-
-	/* Try to do the send work in the caller's context. */
-	ipath_do_send((unsigned long) qp);
-
-bail:
-	return err;
-}
-
-/**
- * ipath_post_receive - post a receive on a QP
- * @ibqp: the QP to post the receive on
- * @wr: the WR to post
- * @bad_wr: the first bad WR is put here
- *
- * This may be called from interrupt context.
- */
-static int ipath_post_receive(struct ib_qp *ibqp, struct ib_recv_wr *wr,
-			      struct ib_recv_wr **bad_wr)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-	struct ipath_rwq *wq = qp->r_rq.wq;
-	unsigned long flags;
-	int ret;
-
-	/* Check that state is OK to post receive. */
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_POST_RECV_OK) || !wq) {
-		*bad_wr = wr;
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	for (; wr; wr = wr->next) {
-		struct ipath_rwqe *wqe;
-		u32 next;
-		int i;
-
-		if ((unsigned) wr->num_sge > qp->r_rq.max_sge) {
-			*bad_wr = wr;
-			ret = -EINVAL;
-			goto bail;
-		}
-
-		spin_lock_irqsave(&qp->r_rq.lock, flags);
-		next = wq->head + 1;
-		if (next >= qp->r_rq.size)
-			next = 0;
-		if (next == wq->tail) {
-			spin_unlock_irqrestore(&qp->r_rq.lock, flags);
-			*bad_wr = wr;
-			ret = -ENOMEM;
-			goto bail;
-		}
-
-		wqe = get_rwqe_ptr(&qp->r_rq, wq->head);
-		wqe->wr_id = wr->wr_id;
-		wqe->num_sge = wr->num_sge;
-		for (i = 0; i < wr->num_sge; i++)
-			wqe->sg_list[i] = wr->sg_list[i];
-		/* Make sure queue entry is written before the head index. */
-		smp_wmb();
-		wq->head = next;
-		spin_unlock_irqrestore(&qp->r_rq.lock, flags);
-	}
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_qp_rcv - processing an incoming packet on a QP
- * @dev: the device the packet came on
- * @hdr: the packet header
- * @has_grh: true if the packet has a GRH
- * @data: the packet data
- * @tlen: the packet length
- * @qp: the QP the packet came on
- *
- * This is called from ipath_ib_rcv() to process an incoming packet
- * for the given QP.
- * Called at interrupt level.
- */
-static void ipath_qp_rcv(struct ipath_ibdev *dev,
-			 struct ipath_ib_header *hdr, int has_grh,
-			 void *data, u32 tlen, struct ipath_qp *qp)
-{
-	/* Check for valid receive state. */
-	if (!(ib_ipath_state_ops[qp->state] & IPATH_PROCESS_RECV_OK)) {
-		dev->n_pkt_drops++;
-		return;
-	}
-
-	switch (qp->ibqp.qp_type) {
-	case IB_QPT_SMI:
-	case IB_QPT_GSI:
-		if (ib_ipath_disable_sma)
-			break;
-		/* FALLTHROUGH */
-	case IB_QPT_UD:
-		ipath_ud_rcv(dev, hdr, has_grh, data, tlen, qp);
-		break;
-
-	case IB_QPT_RC:
-		ipath_rc_rcv(dev, hdr, has_grh, data, tlen, qp);
-		break;
-
-	case IB_QPT_UC:
-		ipath_uc_rcv(dev, hdr, has_grh, data, tlen, qp);
-		break;
-
-	default:
-		break;
-	}
-}
-
-/**
- * ipath_ib_rcv - process an incoming packet
- * @arg: the device pointer
- * @rhdr: the header of the packet
- * @data: the packet data
- * @tlen: the packet length
- *
- * This is called from ipath_kreceive() to process an incoming packet at
- * interrupt level. Tlen is the length of the header + data + CRC in bytes.
- */
-void ipath_ib_rcv(struct ipath_ibdev *dev, void *rhdr, void *data,
-		  u32 tlen)
-{
-	struct ipath_ib_header *hdr = rhdr;
-	struct ipath_other_headers *ohdr;
-	struct ipath_qp *qp;
-	u32 qp_num;
-	int lnh;
-	u8 opcode;
-	u16 lid;
-
-	if (unlikely(dev == NULL))
-		goto bail;
-
-	if (unlikely(tlen < 24)) {	/* LRH+BTH+CRC */
-		dev->rcv_errors++;
-		goto bail;
-	}
-
-	/* Check for a valid destination LID (see ch. 7.11.1). */
-	lid = be16_to_cpu(hdr->lrh[1]);
-	if (lid < IPATH_MULTICAST_LID_BASE) {
-		lid &= ~((1 << dev->dd->ipath_lmc) - 1);
-		if (unlikely(lid != dev->dd->ipath_lid)) {
-			dev->rcv_errors++;
-			goto bail;
-		}
-	}
-
-	/* Check for GRH */
-	lnh = be16_to_cpu(hdr->lrh[0]) & 3;
-	if (lnh == IPATH_LRH_BTH)
-		ohdr = &hdr->u.oth;
-	else if (lnh == IPATH_LRH_GRH)
-		ohdr = &hdr->u.l.oth;
-	else {
-		dev->rcv_errors++;
-		goto bail;
-	}
-
-	opcode = (be32_to_cpu(ohdr->bth[0]) >> 24) & 0x7f;
-	dev->opstats[opcode].n_bytes += tlen;
-	dev->opstats[opcode].n_packets++;
-
-	/* Get the destination QP number. */
-	qp_num = be32_to_cpu(ohdr->bth[1]) & IPATH_QPN_MASK;
-	if (qp_num == IPATH_MULTICAST_QPN) {
-		struct ipath_mcast *mcast;
-		struct ipath_mcast_qp *p;
-
-		if (lnh != IPATH_LRH_GRH) {
-			dev->n_pkt_drops++;
-			goto bail;
-		}
-		mcast = ipath_mcast_find(&hdr->u.l.grh.dgid);
-		if (mcast == NULL) {
-			dev->n_pkt_drops++;
-			goto bail;
-		}
-		dev->n_multicast_rcv++;
-		list_for_each_entry_rcu(p, &mcast->qp_list, list)
-			ipath_qp_rcv(dev, hdr, 1, data, tlen, p->qp);
-		/*
-		 * Notify ipath_multicast_detach() if it is waiting for us
-		 * to finish.
-		 */
-		if (atomic_dec_return(&mcast->refcount) <= 1)
-			wake_up(&mcast->wait);
-	} else {
-		qp = ipath_lookup_qpn(&dev->qp_table, qp_num);
-		if (qp) {
-			dev->n_unicast_rcv++;
-			ipath_qp_rcv(dev, hdr, lnh == IPATH_LRH_GRH, data,
-				     tlen, qp);
-			/*
-			 * Notify ipath_destroy_qp() if it is waiting
-			 * for us to finish.
-			 */
-			if (atomic_dec_and_test(&qp->refcount))
-				wake_up(&qp->wait);
-		} else
-			dev->n_pkt_drops++;
-	}
-
-bail:;
-}
-
-/**
- * ipath_ib_timer - verbs timer
- * @arg: the device pointer
- *
- * This is called from ipath_do_rcv_timer() at interrupt level to check for
- * QPs which need retransmits and to collect performance numbers.
- */
-static void ipath_ib_timer(struct ipath_ibdev *dev)
-{
-	struct ipath_qp *resend = NULL;
-	struct ipath_qp *rnr = NULL;
-	struct list_head *last;
-	struct ipath_qp *qp;
-	unsigned long flags;
-
-	if (dev == NULL)
-		return;
-
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	/* Start filling the next pending queue. */
-	if (++dev->pending_index >= ARRAY_SIZE(dev->pending))
-		dev->pending_index = 0;
-	/* Save any requests still in the new queue, they have timed out. */
-	last = &dev->pending[dev->pending_index];
-	while (!list_empty(last)) {
-		qp = list_entry(last->next, struct ipath_qp, timerwait);
-		list_del_init(&qp->timerwait);
-		qp->timer_next = resend;
-		resend = qp;
-		atomic_inc(&qp->refcount);
-	}
-	last = &dev->rnrwait;
-	if (!list_empty(last)) {
-		qp = list_entry(last->next, struct ipath_qp, timerwait);
-		if (--qp->s_rnr_timeout == 0) {
-			do {
-				list_del_init(&qp->timerwait);
-				qp->timer_next = rnr;
-				rnr = qp;
-				atomic_inc(&qp->refcount);
-				if (list_empty(last))
-					break;
-				qp = list_entry(last->next, struct ipath_qp,
-						timerwait);
-			} while (qp->s_rnr_timeout == 0);
-		}
-	}
-	/*
-	 * We should only be in the started state if pma_sample_start != 0
-	 */
-	if (dev->pma_sample_status == IB_PMA_SAMPLE_STATUS_STARTED &&
-	    --dev->pma_sample_start == 0) {
-		dev->pma_sample_status = IB_PMA_SAMPLE_STATUS_RUNNING;
-		ipath_snapshot_counters(dev->dd, &dev->ipath_sword,
-					&dev->ipath_rword,
-					&dev->ipath_spkts,
-					&dev->ipath_rpkts,
-					&dev->ipath_xmit_wait);
-	}
-	if (dev->pma_sample_status == IB_PMA_SAMPLE_STATUS_RUNNING) {
-		if (dev->pma_sample_interval == 0) {
-			u64 ta, tb, tc, td, te;
-
-			dev->pma_sample_status = IB_PMA_SAMPLE_STATUS_DONE;
-			ipath_snapshot_counters(dev->dd, &ta, &tb,
-						&tc, &td, &te);
-
-			dev->ipath_sword = ta - dev->ipath_sword;
-			dev->ipath_rword = tb - dev->ipath_rword;
-			dev->ipath_spkts = tc - dev->ipath_spkts;
-			dev->ipath_rpkts = td - dev->ipath_rpkts;
-			dev->ipath_xmit_wait = te - dev->ipath_xmit_wait;
-		} else {
-			dev->pma_sample_interval--;
-		}
-	}
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-
-	/* XXX What if timer fires again while this is running? */
-	while (resend != NULL) {
-		qp = resend;
-		resend = qp->timer_next;
-
-		spin_lock_irqsave(&qp->s_lock, flags);
-		if (qp->s_last != qp->s_tail &&
-		    ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK) {
-			dev->n_timeouts++;
-			ipath_restart_rc(qp, qp->s_last_psn + 1);
-		}
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-
-		/* Notify ipath_destroy_qp() if it is waiting. */
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
-	}
-	while (rnr != NULL) {
-		qp = rnr;
-		rnr = qp->timer_next;
-
-		spin_lock_irqsave(&qp->s_lock, flags);
-		if (ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK)
-			ipath_schedule_send(qp);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-
-		/* Notify ipath_destroy_qp() if it is waiting. */
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
-	}
-}
-
-static void update_sge(struct ipath_sge_state *ss, u32 length)
-{
-	struct ipath_sge *sge = &ss->sge;
-
-	sge->vaddr += length;
-	sge->length -= length;
-	sge->sge_length -= length;
-	if (sge->sge_length == 0) {
-		if (--ss->num_sge)
-			*sge = *ss->sg_list++;
-	} else if (sge->length == 0 && sge->mr != NULL) {
-		if (++sge->n >= IPATH_SEGSZ) {
-			if (++sge->m >= sge->mr->mapsz)
-				return;
-			sge->n = 0;
-		}
-		sge->vaddr = sge->mr->map[sge->m]->segs[sge->n].vaddr;
-		sge->length = sge->mr->map[sge->m]->segs[sge->n].length;
-	}
-}
-
-#ifdef __LITTLE_ENDIAN
-static inline u32 get_upper_bits(u32 data, u32 shift)
-{
-	return data >> shift;
-}
-
-static inline u32 set_upper_bits(u32 data, u32 shift)
-{
-	return data << shift;
-}
-
-static inline u32 clear_upper_bytes(u32 data, u32 n, u32 off)
-{
-	data <<= ((sizeof(u32) - n) * BITS_PER_BYTE);
-	data >>= ((sizeof(u32) - n - off) * BITS_PER_BYTE);
-	return data;
-}
-#else
-static inline u32 get_upper_bits(u32 data, u32 shift)
-{
-	return data << shift;
-}
-
-static inline u32 set_upper_bits(u32 data, u32 shift)
-{
-	return data >> shift;
-}
-
-static inline u32 clear_upper_bytes(u32 data, u32 n, u32 off)
-{
-	data >>= ((sizeof(u32) - n) * BITS_PER_BYTE);
-	data <<= ((sizeof(u32) - n - off) * BITS_PER_BYTE);
-	return data;
-}
-#endif
-
-static void copy_io(u32 __iomem *piobuf, struct ipath_sge_state *ss,
-		    u32 length, unsigned flush_wc)
-{
-	u32 extra = 0;
-	u32 data = 0;
-	u32 last;
-
-	while (1) {
-		u32 len = ss->sge.length;
-		u32 off;
-
-		if (len > length)
-			len = length;
-		if (len > ss->sge.sge_length)
-			len = ss->sge.sge_length;
-		BUG_ON(len == 0);
-		/* If the source address is not aligned, try to align it. */
-		off = (unsigned long)ss->sge.vaddr & (sizeof(u32) - 1);
-		if (off) {
-			u32 *addr = (u32 *)((unsigned long)ss->sge.vaddr &
-					    ~(sizeof(u32) - 1));
-			u32 v = get_upper_bits(*addr, off * BITS_PER_BYTE);
-			u32 y;
-
-			y = sizeof(u32) - off;
-			if (len > y)
-				len = y;
-			if (len + extra >= sizeof(u32)) {
-				data |= set_upper_bits(v, extra *
-						       BITS_PER_BYTE);
-				len = sizeof(u32) - extra;
-				if (len == length) {
-					last = data;
-					break;
-				}
-				__raw_writel(data, piobuf);
-				piobuf++;
-				extra = 0;
-				data = 0;
-			} else {
-				/* Clear unused upper bytes */
-				data |= clear_upper_bytes(v, len, extra);
-				if (len == length) {
-					last = data;
-					break;
-				}
-				extra += len;
-			}
-		} else if (extra) {
-			/* Source address is aligned. */
-			u32 *addr = (u32 *) ss->sge.vaddr;
-			int shift = extra * BITS_PER_BYTE;
-			int ushift = 32 - shift;
-			u32 l = len;
-
-			while (l >= sizeof(u32)) {
-				u32 v = *addr;
-
-				data |= set_upper_bits(v, shift);
-				__raw_writel(data, piobuf);
-				data = get_upper_bits(v, ushift);
-				piobuf++;
-				addr++;
-				l -= sizeof(u32);
-			}
-			/*
-			 * We still have 'extra' number of bytes leftover.
-			 */
-			if (l) {
-				u32 v = *addr;
-
-				if (l + extra >= sizeof(u32)) {
-					data |= set_upper_bits(v, shift);
-					len -= l + extra - sizeof(u32);
-					if (len == length) {
-						last = data;
-						break;
-					}
-					__raw_writel(data, piobuf);
-					piobuf++;
-					extra = 0;
-					data = 0;
-				} else {
-					/* Clear unused upper bytes */
-					data |= clear_upper_bytes(v, l,
-								  extra);
-					if (len == length) {
-						last = data;
-						break;
-					}
-					extra += l;
-				}
-			} else if (len == length) {
-				last = data;
-				break;
-			}
-		} else if (len == length) {
-			u32 w;
-
-			/*
-			 * Need to round up for the last dword in the
-			 * packet.
-			 */
-			w = (len + 3) >> 2;
-			__iowrite32_copy(piobuf, ss->sge.vaddr, w - 1);
-			piobuf += w - 1;
-			last = ((u32 *) ss->sge.vaddr)[w - 1];
-			break;
-		} else {
-			u32 w = len >> 2;
-
-			__iowrite32_copy(piobuf, ss->sge.vaddr, w);
-			piobuf += w;
-
-			extra = len & (sizeof(u32) - 1);
-			if (extra) {
-				u32 v = ((u32 *) ss->sge.vaddr)[w];
-
-				/* Clear unused upper bytes */
-				data = clear_upper_bytes(v, extra, 0);
-			}
-		}
-		update_sge(ss, len);
-		length -= len;
-	}
-	/* Update address before sending packet. */
-	update_sge(ss, length);
-	if (flush_wc) {
-		/* must flush early everything before trigger word */
-		ipath_flush_wc();
-		__raw_writel(last, piobuf);
-		/* be sure trigger word is written */
-		ipath_flush_wc();
-	} else
-		__raw_writel(last, piobuf);
-}
-
-/*
- * Convert IB rate to delay multiplier.
- */
-unsigned ipath_ib_rate_to_mult(enum ib_rate rate)
-{
-	switch (rate) {
-	case IB_RATE_2_5_GBPS: return 8;
-	case IB_RATE_5_GBPS:   return 4;
-	case IB_RATE_10_GBPS:  return 2;
-	case IB_RATE_20_GBPS:  return 1;
-	default:	       return 0;
-	}
-}
-
-/*
- * Convert delay multiplier to IB rate
- */
-static enum ib_rate ipath_mult_to_ib_rate(unsigned mult)
-{
-	switch (mult) {
-	case 8:  return IB_RATE_2_5_GBPS;
-	case 4:  return IB_RATE_5_GBPS;
-	case 2:  return IB_RATE_10_GBPS;
-	case 1:  return IB_RATE_20_GBPS;
-	default: return IB_RATE_PORT_CURRENT;
-	}
-}
-
-static inline struct ipath_verbs_txreq *get_txreq(struct ipath_ibdev *dev)
-{
-	struct ipath_verbs_txreq *tx = NULL;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	if (!list_empty(&dev->txreq_free)) {
-		struct list_head *l = dev->txreq_free.next;
-
-		list_del(l);
-		tx = list_entry(l, struct ipath_verbs_txreq, txreq.list);
-	}
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-	return tx;
-}
-
-static inline void put_txreq(struct ipath_ibdev *dev,
-			     struct ipath_verbs_txreq *tx)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	list_add(&tx->txreq.list, &dev->txreq_free);
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-}
-
-static void sdma_complete(void *cookie, int status)
-{
-	struct ipath_verbs_txreq *tx = cookie;
-	struct ipath_qp *qp = tx->qp;
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	unsigned long flags;
-	enum ib_wc_status ibs = status == IPATH_SDMA_TXREQ_S_OK ?
-		IB_WC_SUCCESS : IB_WC_WR_FLUSH_ERR;
-
-	if (atomic_dec_and_test(&qp->s_dma_busy)) {
-		spin_lock_irqsave(&qp->s_lock, flags);
-		if (tx->wqe)
-			ipath_send_complete(qp, tx->wqe, ibs);
-		if ((ib_ipath_state_ops[qp->state] & IPATH_FLUSH_SEND &&
-		     qp->s_last != qp->s_head) ||
-		    (qp->s_flags & IPATH_S_WAIT_DMA))
-			ipath_schedule_send(qp);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-		wake_up(&qp->wait_dma);
-	} else if (tx->wqe) {
-		spin_lock_irqsave(&qp->s_lock, flags);
-		ipath_send_complete(qp, tx->wqe, ibs);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-	}
-
-	if (tx->txreq.flags & IPATH_SDMA_TXREQ_F_FREEBUF)
-		kfree(tx->txreq.map_addr);
-	put_txreq(dev, tx);
-
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-}
-
-static void decrement_dma_busy(struct ipath_qp *qp)
-{
-	unsigned long flags;
-
-	if (atomic_dec_and_test(&qp->s_dma_busy)) {
-		spin_lock_irqsave(&qp->s_lock, flags);
-		if ((ib_ipath_state_ops[qp->state] & IPATH_FLUSH_SEND &&
-		     qp->s_last != qp->s_head) ||
-		    (qp->s_flags & IPATH_S_WAIT_DMA))
-			ipath_schedule_send(qp);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-		wake_up(&qp->wait_dma);
-	}
-}
-
-/*
- * Compute the number of clock cycles of delay before sending the next packet.
- * The multipliers reflect the number of clocks for the fastest rate so
- * one tick at 4xDDR is 8 ticks at 1xSDR.
- * If the destination port will take longer to receive a packet than
- * the outgoing link can send it, we need to delay sending the next packet
- * by the difference in time it takes the receiver to receive and the sender
- * to send this packet.
- * Note that this delay is always correct for UC and RC but not always
- * optimal for UD. For UD, the destination HCA can be different for each
- * packet, in which case, we could send packets to a different destination
- * while "waiting" for the delay. The overhead for doing this without
- * HW support is more than just paying the cost of delaying some packets
- * unnecessarily.
- */
-static inline unsigned ipath_pkt_delay(u32 plen, u8 snd_mult, u8 rcv_mult)
-{
-	return (rcv_mult > snd_mult) ?
-		(plen * (rcv_mult - snd_mult) + 1) >> 1 : 0;
-}
-
-static int ipath_verbs_send_dma(struct ipath_qp *qp,
-				struct ipath_ib_header *hdr, u32 hdrwords,
-				struct ipath_sge_state *ss, u32 len,
-				u32 plen, u32 dwords)
-{
-	struct ipath_ibdev *dev = to_idev(qp->ibqp.device);
-	struct ipath_devdata *dd = dev->dd;
-	struct ipath_verbs_txreq *tx;
-	u32 *piobuf;
-	u32 control;
-	u32 ndesc;
-	int ret;
-
-	tx = qp->s_tx;
-	if (tx) {
-		qp->s_tx = NULL;
-		/* resend previously constructed packet */
-		atomic_inc(&qp->s_dma_busy);
-		ret = ipath_sdma_verbs_send(dd, tx->ss, tx->len, tx);
-		if (ret) {
-			qp->s_tx = tx;
-			decrement_dma_busy(qp);
-		}
-		goto bail;
-	}
-
-	tx = get_txreq(dev);
-	if (!tx) {
-		ret = -EBUSY;
-		goto bail;
-	}
-
-	/*
-	 * Get the saved delay count we computed for the previous packet
-	 * and save the delay count for this packet to be used next time
-	 * we get here.
-	 */
-	control = qp->s_pkt_delay;
-	qp->s_pkt_delay = ipath_pkt_delay(plen, dd->delay_mult, qp->s_dmult);
-
-	tx->qp = qp;
-	atomic_inc(&qp->refcount);
-	tx->wqe = qp->s_wqe;
-	tx->txreq.callback = sdma_complete;
-	tx->txreq.callback_cookie = tx;
-	tx->txreq.flags = IPATH_SDMA_TXREQ_F_HEADTOHOST |
-		IPATH_SDMA_TXREQ_F_INTREQ | IPATH_SDMA_TXREQ_F_FREEDESC;
-	if (plen + 1 >= IPATH_SMALLBUF_DWORDS)
-		tx->txreq.flags |= IPATH_SDMA_TXREQ_F_USELARGEBUF;
-
-	/* VL15 packets bypass credit check */
-	if ((be16_to_cpu(hdr->lrh[0]) >> 12) == 15) {
-		control |= 1ULL << 31;
-		tx->txreq.flags |= IPATH_SDMA_TXREQ_F_VL15;
-	}
-
-	if (len) {
-		/*
-		 * Don't try to DMA if it takes more descriptors than
-		 * the queue holds.
-		 */
-		ndesc = ipath_count_sge(ss, len);
-		if (ndesc >= dd->ipath_sdma_descq_cnt)
-			ndesc = 0;
-	} else
-		ndesc = 1;
-	if (ndesc) {
-		tx->hdr.pbc[0] = cpu_to_le32(plen);
-		tx->hdr.pbc[1] = cpu_to_le32(control);
-		memcpy(&tx->hdr.hdr, hdr, hdrwords << 2);
-		tx->txreq.sg_count = ndesc;
-		tx->map_len = (hdrwords + 2) << 2;
-		tx->txreq.map_addr = &tx->hdr;
-		atomic_inc(&qp->s_dma_busy);
-		ret = ipath_sdma_verbs_send(dd, ss, dwords, tx);
-		if (ret) {
-			/* save ss and length in dwords */
-			tx->ss = ss;
-			tx->len = dwords;
-			qp->s_tx = tx;
-			decrement_dma_busy(qp);
-		}
-		goto bail;
-	}
-
-	/* Allocate a buffer and copy the header and payload to it. */
-	tx->map_len = (plen + 1) << 2;
-	piobuf = kmalloc(tx->map_len, GFP_ATOMIC);
-	if (unlikely(piobuf == NULL)) {
-		ret = -EBUSY;
-		goto err_tx;
-	}
-	tx->txreq.map_addr = piobuf;
-	tx->txreq.flags |= IPATH_SDMA_TXREQ_F_FREEBUF;
-	tx->txreq.sg_count = 1;
-
-	*piobuf++ = (__force u32) cpu_to_le32(plen);
-	*piobuf++ = (__force u32) cpu_to_le32(control);
-	memcpy(piobuf, hdr, hdrwords << 2);
-	ipath_copy_from_sge(piobuf + hdrwords, ss, len);
-
-	atomic_inc(&qp->s_dma_busy);
-	ret = ipath_sdma_verbs_send(dd, NULL, 0, tx);
-	/*
-	 * If we couldn't queue the DMA request, save the info
-	 * and try again later rather than destroying the
-	 * buffer and undoing the side effects of the copy.
-	 */
-	if (ret) {
-		tx->ss = NULL;
-		tx->len = 0;
-		qp->s_tx = tx;
-		decrement_dma_busy(qp);
-	}
-	dev->n_unaligned++;
-	goto bail;
-
-err_tx:
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-	put_txreq(dev, tx);
-bail:
-	return ret;
-}
-
-static int ipath_verbs_send_pio(struct ipath_qp *qp,
-				struct ipath_ib_header *ibhdr, u32 hdrwords,
-				struct ipath_sge_state *ss, u32 len,
-				u32 plen, u32 dwords)
-{
-	struct ipath_devdata *dd = to_idev(qp->ibqp.device)->dd;
-	u32 *hdr = (u32 *) ibhdr;
-	u32 __iomem *piobuf;
-	unsigned flush_wc;
-	u32 control;
-	int ret;
-	unsigned long flags;
-
-	piobuf = ipath_getpiobuf(dd, plen, NULL);
-	if (unlikely(piobuf == NULL)) {
-		ret = -EBUSY;
-		goto bail;
-	}
-
-	/*
-	 * Get the saved delay count we computed for the previous packet
-	 * and save the delay count for this packet to be used next time
-	 * we get here.
-	 */
-	control = qp->s_pkt_delay;
-	qp->s_pkt_delay = ipath_pkt_delay(plen, dd->delay_mult, qp->s_dmult);
-
-	/* VL15 packets bypass credit check */
-	if ((be16_to_cpu(ibhdr->lrh[0]) >> 12) == 15)
-		control |= 1ULL << 31;
-
-	/*
-	 * Write the length to the control qword plus any needed flags.
-	 * We have to flush after the PBC for correctness on some cpus
-	 * or WC buffer can be written out of order.
-	 */
-	writeq(((u64) control << 32) | plen, piobuf);
-	piobuf += 2;
-
-	flush_wc = dd->ipath_flags & IPATH_PIO_FLUSH_WC;
-	if (len == 0) {
-		/*
-		 * If there is just the header portion, must flush before
-		 * writing last word of header for correctness, and after
-		 * the last header word (trigger word).
-		 */
-		if (flush_wc) {
-			ipath_flush_wc();
-			__iowrite32_copy(piobuf, hdr, hdrwords - 1);
-			ipath_flush_wc();
-			__raw_writel(hdr[hdrwords - 1], piobuf + hdrwords - 1);
-			ipath_flush_wc();
-		} else
-			__iowrite32_copy(piobuf, hdr, hdrwords);
-		goto done;
-	}
-
-	if (flush_wc)
-		ipath_flush_wc();
-	__iowrite32_copy(piobuf, hdr, hdrwords);
-	piobuf += hdrwords;
-
-	/* The common case is aligned and contained in one segment. */
-	if (likely(ss->num_sge == 1 && len <= ss->sge.length &&
-		   !((unsigned long)ss->sge.vaddr & (sizeof(u32) - 1)))) {
-		u32 *addr = (u32 *) ss->sge.vaddr;
-
-		/* Update address before sending packet. */
-		update_sge(ss, len);
-		if (flush_wc) {
-			__iowrite32_copy(piobuf, addr, dwords - 1);
-			/* must flush early everything before trigger word */
-			ipath_flush_wc();
-			__raw_writel(addr[dwords - 1], piobuf + dwords - 1);
-			/* be sure trigger word is written */
-			ipath_flush_wc();
-		} else
-			__iowrite32_copy(piobuf, addr, dwords);
-		goto done;
-	}
-	copy_io(piobuf, ss, len, flush_wc);
-done:
-	if (qp->s_wqe) {
-		spin_lock_irqsave(&qp->s_lock, flags);
-		ipath_send_complete(qp, qp->s_wqe, IB_WC_SUCCESS);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-	}
-	ret = 0;
-bail:
-	return ret;
-}
-
-/**
- * ipath_verbs_send - send a packet
- * @qp: the QP to send on
- * @hdr: the packet header
- * @hdrwords: the number of 32-bit words in the header
- * @ss: the SGE to send
- * @len: the length of the packet in bytes
- */
-int ipath_verbs_send(struct ipath_qp *qp, struct ipath_ib_header *hdr,
-		     u32 hdrwords, struct ipath_sge_state *ss, u32 len)
-{
-	struct ipath_devdata *dd = to_idev(qp->ibqp.device)->dd;
-	u32 plen;
-	int ret;
-	u32 dwords = (len + 3) >> 2;
-
-	/*
-	 * Calculate the send buffer trigger address.
-	 * The +1 counts for the pbc control dword following the pbc length.
-	 */
-	plen = hdrwords + dwords + 1;
-
-	/*
-	 * VL15 packets (IB_QPT_SMI) will always use PIO, so we
-	 * can defer SDMA restart until link goes ACTIVE without
-	 * worrying about just how we got there.
-	 */
-	if (qp->ibqp.qp_type == IB_QPT_SMI ||
-	    !(dd->ipath_flags & IPATH_HAS_SEND_DMA))
-		ret = ipath_verbs_send_pio(qp, hdr, hdrwords, ss, len,
-					   plen, dwords);
-	else
-		ret = ipath_verbs_send_dma(qp, hdr, hdrwords, ss, len,
-					   plen, dwords);
-
-	return ret;
-}
-
-int ipath_snapshot_counters(struct ipath_devdata *dd, u64 *swords,
-			    u64 *rwords, u64 *spkts, u64 *rpkts,
-			    u64 *xmit_wait)
-{
-	int ret;
-
-	if (!(dd->ipath_flags & IPATH_INITTED)) {
-		/* no hardware, freeze, etc. */
-		ret = -EINVAL;
-		goto bail;
-	}
-	*swords = ipath_snap_cntr(dd, dd->ipath_cregs->cr_wordsendcnt);
-	*rwords = ipath_snap_cntr(dd, dd->ipath_cregs->cr_wordrcvcnt);
-	*spkts = ipath_snap_cntr(dd, dd->ipath_cregs->cr_pktsendcnt);
-	*rpkts = ipath_snap_cntr(dd, dd->ipath_cregs->cr_pktrcvcnt);
-	*xmit_wait = ipath_snap_cntr(dd, dd->ipath_cregs->cr_sendstallcnt);
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_get_counters - get various chip counters
- * @dd: the infinipath device
- * @cntrs: counters are placed here
- *
- * Return the counters needed by recv_pma_get_portcounters().
- */
-int ipath_get_counters(struct ipath_devdata *dd,
-		       struct ipath_verbs_counters *cntrs)
-{
-	struct ipath_cregs const *crp = dd->ipath_cregs;
-	int ret;
-
-	if (!(dd->ipath_flags & IPATH_INITTED)) {
-		/* no hardware, freeze, etc. */
-		ret = -EINVAL;
-		goto bail;
-	}
-	cntrs->symbol_error_counter =
-		ipath_snap_cntr(dd, crp->cr_ibsymbolerrcnt);
-	cntrs->link_error_recovery_counter =
-		ipath_snap_cntr(dd, crp->cr_iblinkerrrecovcnt);
-	/*
-	 * The link downed counter counts when the other side downs the
-	 * connection.  We add in the number of times we downed the link
-	 * due to local link integrity errors to compensate.
-	 */
-	cntrs->link_downed_counter =
-		ipath_snap_cntr(dd, crp->cr_iblinkdowncnt);
-	cntrs->port_rcv_errors =
-		ipath_snap_cntr(dd, crp->cr_rxdroppktcnt) +
-		ipath_snap_cntr(dd, crp->cr_rcvovflcnt) +
-		ipath_snap_cntr(dd, crp->cr_portovflcnt) +
-		ipath_snap_cntr(dd, crp->cr_err_rlencnt) +
-		ipath_snap_cntr(dd, crp->cr_invalidrlencnt) +
-		ipath_snap_cntr(dd, crp->cr_errlinkcnt) +
-		ipath_snap_cntr(dd, crp->cr_erricrccnt) +
-		ipath_snap_cntr(dd, crp->cr_errvcrccnt) +
-		ipath_snap_cntr(dd, crp->cr_errlpcrccnt) +
-		ipath_snap_cntr(dd, crp->cr_badformatcnt) +
-		dd->ipath_rxfc_unsupvl_errs;
-	if (crp->cr_rxotherlocalphyerrcnt)
-		cntrs->port_rcv_errors +=
-			ipath_snap_cntr(dd, crp->cr_rxotherlocalphyerrcnt);
-	if (crp->cr_rxvlerrcnt)
-		cntrs->port_rcv_errors +=
-			ipath_snap_cntr(dd, crp->cr_rxvlerrcnt);
-	cntrs->port_rcv_remphys_errors =
-		ipath_snap_cntr(dd, crp->cr_rcvebpcnt);
-	cntrs->port_xmit_discards = ipath_snap_cntr(dd, crp->cr_unsupvlcnt);
-	cntrs->port_xmit_data = ipath_snap_cntr(dd, crp->cr_wordsendcnt);
-	cntrs->port_rcv_data = ipath_snap_cntr(dd, crp->cr_wordrcvcnt);
-	cntrs->port_xmit_packets = ipath_snap_cntr(dd, crp->cr_pktsendcnt);
-	cntrs->port_rcv_packets = ipath_snap_cntr(dd, crp->cr_pktrcvcnt);
-	cntrs->local_link_integrity_errors =
-		crp->cr_locallinkintegrityerrcnt ?
-		ipath_snap_cntr(dd, crp->cr_locallinkintegrityerrcnt) :
-		((dd->ipath_flags & IPATH_GPIO_ERRINTRS) ?
-		 dd->ipath_lli_errs : dd->ipath_lli_errors);
-	cntrs->excessive_buffer_overrun_errors =
-		crp->cr_excessbufferovflcnt ?
-		ipath_snap_cntr(dd, crp->cr_excessbufferovflcnt) :
-		dd->ipath_overrun_thresh_errs;
-	cntrs->vl15_dropped = crp->cr_vl15droppedpktcnt ?
-		ipath_snap_cntr(dd, crp->cr_vl15droppedpktcnt) : 0;
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_ib_piobufavail - callback when a PIO buffer is available
- * @arg: the device pointer
- *
- * This is called from ipath_intr() at interrupt level when a PIO buffer is
- * available after ipath_verbs_send() returned an error that no buffers were
- * available.  Return 1 if we consumed all the PIO buffers and we still have
- * QPs waiting for buffers (for now, just restart the send tasklet and
- * return zero).
- */
-int ipath_ib_piobufavail(struct ipath_ibdev *dev)
-{
-	struct list_head *list;
-	struct ipath_qp *qplist;
-	struct ipath_qp *qp;
-	unsigned long flags;
-
-	if (dev == NULL)
-		goto bail;
-
-	list = &dev->piowait;
-	qplist = NULL;
-
-	spin_lock_irqsave(&dev->pending_lock, flags);
-	while (!list_empty(list)) {
-		qp = list_entry(list->next, struct ipath_qp, piowait);
-		list_del_init(&qp->piowait);
-		qp->pio_next = qplist;
-		qplist = qp;
-		atomic_inc(&qp->refcount);
-	}
-	spin_unlock_irqrestore(&dev->pending_lock, flags);
-
-	while (qplist != NULL) {
-		qp = qplist;
-		qplist = qp->pio_next;
-
-		spin_lock_irqsave(&qp->s_lock, flags);
-		if (ib_ipath_state_ops[qp->state] & IPATH_PROCESS_SEND_OK)
-			ipath_schedule_send(qp);
-		spin_unlock_irqrestore(&qp->s_lock, flags);
-
-		/* Notify ipath_destroy_qp() if it is waiting. */
-		if (atomic_dec_and_test(&qp->refcount))
-			wake_up(&qp->wait);
-	}
-
-bail:
-	return 0;
-}
-
-static int ipath_query_device(struct ib_device *ibdev, struct ib_device_attr *props,
-			      struct ib_udata *uhw)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-
-	if (uhw->inlen || uhw->outlen)
-		return -EINVAL;
-
-	memset(props, 0, sizeof(*props));
-
-	props->device_cap_flags = IB_DEVICE_BAD_PKEY_CNTR |
-		IB_DEVICE_BAD_QKEY_CNTR | IB_DEVICE_SHUTDOWN_PORT |
-		IB_DEVICE_SYS_IMAGE_GUID | IB_DEVICE_RC_RNR_NAK_GEN |
-		IB_DEVICE_PORT_ACTIVE_EVENT | IB_DEVICE_SRQ_RESIZE;
-	props->page_size_cap = PAGE_SIZE;
-	props->vendor_id =
-		IPATH_SRC_OUI_1 << 16 | IPATH_SRC_OUI_2 << 8 | IPATH_SRC_OUI_3;
-	props->vendor_part_id = dev->dd->ipath_deviceid;
-	props->hw_ver = dev->dd->ipath_pcirev;
-
-	props->sys_image_guid = dev->sys_image_guid;
-
-	props->max_mr_size = ~0ull;
-	props->max_qp = ib_ipath_max_qps;
-	props->max_qp_wr = ib_ipath_max_qp_wrs;
-	props->max_sge = ib_ipath_max_sges;
-	props->max_sge_rd = ib_ipath_max_sges;
-	props->max_cq = ib_ipath_max_cqs;
-	props->max_ah = ib_ipath_max_ahs;
-	props->max_cqe = ib_ipath_max_cqes;
-	props->max_mr = dev->lk_table.max;
-	props->max_fmr = dev->lk_table.max;
-	props->max_map_per_fmr = 32767;
-	props->max_pd = ib_ipath_max_pds;
-	props->max_qp_rd_atom = IPATH_MAX_RDMA_ATOMIC;
-	props->max_qp_init_rd_atom = 255;
-	/* props->max_res_rd_atom */
-	props->max_srq = ib_ipath_max_srqs;
-	props->max_srq_wr = ib_ipath_max_srq_wrs;
-	props->max_srq_sge = ib_ipath_max_srq_sges;
-	/* props->local_ca_ack_delay */
-	props->atomic_cap = IB_ATOMIC_GLOB;
-	props->max_pkeys = ipath_get_npkeys(dev->dd);
-	props->max_mcast_grp = ib_ipath_max_mcast_grps;
-	props->max_mcast_qp_attach = ib_ipath_max_mcast_qp_attached;
-	props->max_total_mcast_qp_attach = props->max_mcast_qp_attach *
-		props->max_mcast_grp;
-
-	return 0;
-}
-
-const u8 ipath_cvt_physportstate[32] = {
-	[INFINIPATH_IBCS_LT_STATE_DISABLED] = IB_PHYSPORTSTATE_DISABLED,
-	[INFINIPATH_IBCS_LT_STATE_LINKUP] = IB_PHYSPORTSTATE_LINKUP,
-	[INFINIPATH_IBCS_LT_STATE_POLLACTIVE] = IB_PHYSPORTSTATE_POLL,
-	[INFINIPATH_IBCS_LT_STATE_POLLQUIET] = IB_PHYSPORTSTATE_POLL,
-	[INFINIPATH_IBCS_LT_STATE_SLEEPDELAY] = IB_PHYSPORTSTATE_SLEEP,
-	[INFINIPATH_IBCS_LT_STATE_SLEEPQUIET] = IB_PHYSPORTSTATE_SLEEP,
-	[INFINIPATH_IBCS_LT_STATE_CFGDEBOUNCE] =
-		IB_PHYSPORTSTATE_CFG_TRAIN,
-	[INFINIPATH_IBCS_LT_STATE_CFGRCVFCFG] =
-		IB_PHYSPORTSTATE_CFG_TRAIN,
-	[INFINIPATH_IBCS_LT_STATE_CFGWAITRMT] =
-		IB_PHYSPORTSTATE_CFG_TRAIN,
-	[INFINIPATH_IBCS_LT_STATE_CFGIDLE] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[INFINIPATH_IBCS_LT_STATE_RECOVERRETRAIN] =
-		IB_PHYSPORTSTATE_LINK_ERR_RECOVER,
-	[INFINIPATH_IBCS_LT_STATE_RECOVERWAITRMT] =
-		IB_PHYSPORTSTATE_LINK_ERR_RECOVER,
-	[INFINIPATH_IBCS_LT_STATE_RECOVERIDLE] =
-		IB_PHYSPORTSTATE_LINK_ERR_RECOVER,
-	[0x10] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x11] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x12] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x13] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x14] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x15] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x16] = IB_PHYSPORTSTATE_CFG_TRAIN,
-	[0x17] = IB_PHYSPORTSTATE_CFG_TRAIN
-};
-
-u32 ipath_get_cr_errpkey(struct ipath_devdata *dd)
-{
-	return ipath_read_creg32(dd, dd->ipath_cregs->cr_errpkey);
-}
-
-static int ipath_query_port(struct ib_device *ibdev,
-			    u8 port, struct ib_port_attr *props)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_devdata *dd = dev->dd;
-	enum ib_mtu mtu;
-	u16 lid = dd->ipath_lid;
-	u64 ibcstat;
-
-	memset(props, 0, sizeof(*props));
-	props->lid = lid ? lid : be16_to_cpu(IB_LID_PERMISSIVE);
-	props->lmc = dd->ipath_lmc;
-	props->sm_lid = dev->sm_lid;
-	props->sm_sl = dev->sm_sl;
-	ibcstat = dd->ipath_lastibcstat;
-	/* map LinkState to IB portinfo values.  */
-	props->state = ipath_ib_linkstate(dd, ibcstat) + 1;
-
-	/* See phys_state_show() */
-	props->phys_state = /* MEA: assumes shift == 0 */
-		ipath_cvt_physportstate[dd->ipath_lastibcstat &
-		dd->ibcs_lts_mask];
-	props->port_cap_flags = dev->port_cap_flags;
-	props->gid_tbl_len = 1;
-	props->max_msg_sz = 0x80000000;
-	props->pkey_tbl_len = ipath_get_npkeys(dd);
-	props->bad_pkey_cntr = ipath_get_cr_errpkey(dd) -
-		dev->z_pkey_violations;
-	props->qkey_viol_cntr = dev->qkey_violations;
-	props->active_width = dd->ipath_link_width_active;
-	/* See rate_show() */
-	props->active_speed = dd->ipath_link_speed_active;
-	props->max_vl_num = 1;		/* VLCap = VL0 */
-	props->init_type_reply = 0;
-
-	props->max_mtu = ipath_mtu4096 ? IB_MTU_4096 : IB_MTU_2048;
-	switch (dd->ipath_ibmtu) {
-	case 4096:
-		mtu = IB_MTU_4096;
-		break;
-	case 2048:
-		mtu = IB_MTU_2048;
-		break;
-	case 1024:
-		mtu = IB_MTU_1024;
-		break;
-	case 512:
-		mtu = IB_MTU_512;
-		break;
-	case 256:
-		mtu = IB_MTU_256;
-		break;
-	default:
-		mtu = IB_MTU_2048;
-	}
-	props->active_mtu = mtu;
-	props->subnet_timeout = dev->subnet_timeout;
-
-	return 0;
-}
-
-static int ipath_modify_device(struct ib_device *device,
-			       int device_modify_mask,
-			       struct ib_device_modify *device_modify)
-{
-	int ret;
-
-	if (device_modify_mask & ~(IB_DEVICE_MODIFY_SYS_IMAGE_GUID |
-				   IB_DEVICE_MODIFY_NODE_DESC)) {
-		ret = -EOPNOTSUPP;
-		goto bail;
-	}
-
-	if (device_modify_mask & IB_DEVICE_MODIFY_NODE_DESC)
-		memcpy(device->node_desc, device_modify->node_desc, 64);
-
-	if (device_modify_mask & IB_DEVICE_MODIFY_SYS_IMAGE_GUID)
-		to_idev(device)->sys_image_guid =
-			cpu_to_be64(device_modify->sys_image_guid);
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-static int ipath_modify_port(struct ib_device *ibdev,
-			     u8 port, int port_modify_mask,
-			     struct ib_port_modify *props)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-
-	dev->port_cap_flags |= props->set_port_cap_mask;
-	dev->port_cap_flags &= ~props->clr_port_cap_mask;
-	if (port_modify_mask & IB_PORT_SHUTDOWN)
-		ipath_set_linkstate(dev->dd, IPATH_IB_LINKDOWN);
-	if (port_modify_mask & IB_PORT_RESET_QKEY_CNTR)
-		dev->qkey_violations = 0;
-	return 0;
-}
-
-static int ipath_query_gid(struct ib_device *ibdev, u8 port,
-			   int index, union ib_gid *gid)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	int ret;
-
-	if (index >= 1) {
-		ret = -EINVAL;
-		goto bail;
-	}
-	gid->global.subnet_prefix = dev->gid_prefix;
-	gid->global.interface_id = dev->dd->ipath_guid;
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-static struct ib_pd *ipath_alloc_pd(struct ib_device *ibdev,
-				    struct ib_ucontext *context,
-				    struct ib_udata *udata)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	struct ipath_pd *pd;
-	struct ib_pd *ret;
-
-	/*
-	 * This is actually totally arbitrary.	Some correctness tests
-	 * assume there's a maximum number of PDs that can be allocated.
-	 * We don't actually have this limit, but we fail the test if
-	 * we allow allocations of more than we report for this value.
-	 */
-
-	pd = kmalloc(sizeof *pd, GFP_KERNEL);
-	if (!pd) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	spin_lock(&dev->n_pds_lock);
-	if (dev->n_pds_allocated == ib_ipath_max_pds) {
-		spin_unlock(&dev->n_pds_lock);
-		kfree(pd);
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	dev->n_pds_allocated++;
-	spin_unlock(&dev->n_pds_lock);
-
-	/* ib_alloc_pd() will initialize pd->ibpd. */
-	pd->user = udata != NULL;
-
-	ret = &pd->ibpd;
-
-bail:
-	return ret;
-}
-
-static int ipath_dealloc_pd(struct ib_pd *ibpd)
-{
-	struct ipath_pd *pd = to_ipd(ibpd);
-	struct ipath_ibdev *dev = to_idev(ibpd->device);
-
-	spin_lock(&dev->n_pds_lock);
-	dev->n_pds_allocated--;
-	spin_unlock(&dev->n_pds_lock);
-
-	kfree(pd);
-
-	return 0;
-}
-
-/**
- * ipath_create_ah - create an address handle
- * @pd: the protection domain
- * @ah_attr: the attributes of the AH
- *
- * This may be called from interrupt context.
- */
-static struct ib_ah *ipath_create_ah(struct ib_pd *pd,
-				     struct ib_ah_attr *ah_attr)
-{
-	struct ipath_ah *ah;
-	struct ib_ah *ret;
-	struct ipath_ibdev *dev = to_idev(pd->device);
-	unsigned long flags;
-
-	/* A multicast address requires a GRH (see ch. 8.4.1). */
-	if (ah_attr->dlid >= IPATH_MULTICAST_LID_BASE &&
-	    ah_attr->dlid != IPATH_PERMISSIVE_LID &&
-	    !(ah_attr->ah_flags & IB_AH_GRH)) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	if (ah_attr->dlid == 0) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	if (ah_attr->port_num < 1 ||
-	    ah_attr->port_num > pd->device->phys_port_cnt) {
-		ret = ERR_PTR(-EINVAL);
-		goto bail;
-	}
-
-	ah = kmalloc(sizeof *ah, GFP_ATOMIC);
-	if (!ah) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	spin_lock_irqsave(&dev->n_ahs_lock, flags);
-	if (dev->n_ahs_allocated == ib_ipath_max_ahs) {
-		spin_unlock_irqrestore(&dev->n_ahs_lock, flags);
-		kfree(ah);
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	dev->n_ahs_allocated++;
-	spin_unlock_irqrestore(&dev->n_ahs_lock, flags);
-
-	/* ib_create_ah() will initialize ah->ibah. */
-	ah->attr = *ah_attr;
-	ah->attr.static_rate = ipath_ib_rate_to_mult(ah_attr->static_rate);
-
-	ret = &ah->ibah;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_destroy_ah - destroy an address handle
- * @ibah: the AH to destroy
- *
- * This may be called from interrupt context.
- */
-static int ipath_destroy_ah(struct ib_ah *ibah)
-{
-	struct ipath_ibdev *dev = to_idev(ibah->device);
-	struct ipath_ah *ah = to_iah(ibah);
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->n_ahs_lock, flags);
-	dev->n_ahs_allocated--;
-	spin_unlock_irqrestore(&dev->n_ahs_lock, flags);
-
-	kfree(ah);
-
-	return 0;
-}
-
-static int ipath_query_ah(struct ib_ah *ibah, struct ib_ah_attr *ah_attr)
-{
-	struct ipath_ah *ah = to_iah(ibah);
-
-	*ah_attr = ah->attr;
-	ah_attr->static_rate = ipath_mult_to_ib_rate(ah->attr.static_rate);
-
-	return 0;
-}
-
-/**
- * ipath_get_npkeys - return the size of the PKEY table for port 0
- * @dd: the infinipath device
- */
-unsigned ipath_get_npkeys(struct ipath_devdata *dd)
-{
-	return ARRAY_SIZE(dd->ipath_pd[0]->port_pkeys);
-}
-
-/**
- * ipath_get_pkey - return the indexed PKEY from the port PKEY table
- * @dd: the infinipath device
- * @index: the PKEY index
- */
-unsigned ipath_get_pkey(struct ipath_devdata *dd, unsigned index)
-{
-	unsigned ret;
-
-	/* always a kernel port, no locking needed */
-	if (index >= ARRAY_SIZE(dd->ipath_pd[0]->port_pkeys))
-		ret = 0;
-	else
-		ret = dd->ipath_pd[0]->port_pkeys[index];
-
-	return ret;
-}
-
-static int ipath_query_pkey(struct ib_device *ibdev, u8 port, u16 index,
-			    u16 *pkey)
-{
-	struct ipath_ibdev *dev = to_idev(ibdev);
-	int ret;
-
-	if (index >= ipath_get_npkeys(dev->dd)) {
-		ret = -EINVAL;
-		goto bail;
-	}
-
-	*pkey = ipath_get_pkey(dev->dd, index);
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-/**
- * ipath_alloc_ucontext - allocate a ucontest
- * @ibdev: the infiniband device
- * @udata: not used by the InfiniPath driver
- */
-
-static struct ib_ucontext *ipath_alloc_ucontext(struct ib_device *ibdev,
-						struct ib_udata *udata)
-{
-	struct ipath_ucontext *context;
-	struct ib_ucontext *ret;
-
-	context = kmalloc(sizeof *context, GFP_KERNEL);
-	if (!context) {
-		ret = ERR_PTR(-ENOMEM);
-		goto bail;
-	}
-
-	ret = &context->ibucontext;
-
-bail:
-	return ret;
-}
-
-static int ipath_dealloc_ucontext(struct ib_ucontext *context)
-{
-	kfree(to_iucontext(context));
-	return 0;
-}
-
-static int ipath_verbs_register_sysfs(struct ib_device *dev);
-
-static void __verbs_timer(unsigned long arg)
-{
-	struct ipath_devdata *dd = (struct ipath_devdata *) arg;
-
-	/* Handle verbs layer timeouts. */
-	ipath_ib_timer(dd->verbs_dev);
-
-	mod_timer(&dd->verbs_timer, jiffies + 1);
-}
-
-static int enable_timer(struct ipath_devdata *dd)
-{
-	/*
-	 * Early chips had a design flaw where the chip and kernel idea
-	 * of the tail register don't always agree, and therefore we won't
-	 * get an interrupt on the next packet received.
-	 * If the board supports per packet receive interrupts, use it.
-	 * Otherwise, the timer function periodically checks for packets
-	 * to cover this case.
-	 * Either way, the timer is needed for verbs layer related
-	 * processing.
-	 */
-	if (dd->ipath_flags & IPATH_GPIO_INTR) {
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_debugportselect,
-				 0x2074076542310ULL);
-		/* Enable GPIO bit 2 interrupt */
-		dd->ipath_gpio_mask |= (u64) (1 << IPATH_GPIO_PORT0_BIT);
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_gpio_mask,
-				 dd->ipath_gpio_mask);
-	}
-
-	setup_timer(&dd->verbs_timer, __verbs_timer, (unsigned long)dd);
-
-	dd->verbs_timer.expires = jiffies + 1;
-	add_timer(&dd->verbs_timer);
-
-	return 0;
-}
-
-static int disable_timer(struct ipath_devdata *dd)
-{
-	/* Disable GPIO bit 2 interrupt */
-	if (dd->ipath_flags & IPATH_GPIO_INTR) {
-                /* Disable GPIO bit 2 interrupt */
-		dd->ipath_gpio_mask &= ~((u64) (1 << IPATH_GPIO_PORT0_BIT));
-		ipath_write_kreg(dd, dd->ipath_kregs->kr_gpio_mask,
-				 dd->ipath_gpio_mask);
-		/*
-		 * We might want to undo changes to debugportselect,
-		 * but how?
-		 */
-	}
-
-	del_timer_sync(&dd->verbs_timer);
-
-	return 0;
-}
-
-static int ipath_port_immutable(struct ib_device *ibdev, u8 port_num,
-			        struct ib_port_immutable *immutable)
-{
-	struct ib_port_attr attr;
-	int err;
-
-	err = ipath_query_port(ibdev, port_num, &attr);
-	if (err)
-		return err;
-
-	immutable->pkey_tbl_len = attr.pkey_tbl_len;
-	immutable->gid_tbl_len = attr.gid_tbl_len;
-	immutable->core_cap_flags = RDMA_CORE_PORT_IBA_IB;
-	immutable->max_mad_size = IB_MGMT_MAD_SIZE;
-
-	return 0;
-}
-
-/**
- * ipath_register_ib_device - register our device with the infiniband core
- * @dd: the device data structure
- * Return the allocated ipath_ibdev pointer or NULL on error.
- */
-int ipath_register_ib_device(struct ipath_devdata *dd)
-{
-	struct ipath_verbs_counters cntrs;
-	struct ipath_ibdev *idev;
-	struct ib_device *dev;
-	struct ipath_verbs_txreq *tx;
-	unsigned i;
-	int ret;
-
-	idev = (struct ipath_ibdev *)ib_alloc_device(sizeof *idev);
-	if (idev == NULL) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-
-	dev = &idev->ibdev;
-
-	if (dd->ipath_sdma_descq_cnt) {
-		tx = kmalloc_array(dd->ipath_sdma_descq_cnt, sizeof *tx,
-				   GFP_KERNEL);
-		if (tx == NULL) {
-			ret = -ENOMEM;
-			goto err_tx;
-		}
-	} else
-		tx = NULL;
-	idev->txreq_bufs = tx;
-
-	/* Only need to initialize non-zero fields. */
-	spin_lock_init(&idev->n_pds_lock);
-	spin_lock_init(&idev->n_ahs_lock);
-	spin_lock_init(&idev->n_cqs_lock);
-	spin_lock_init(&idev->n_qps_lock);
-	spin_lock_init(&idev->n_srqs_lock);
-	spin_lock_init(&idev->n_mcast_grps_lock);
-
-	spin_lock_init(&idev->qp_table.lock);
-	spin_lock_init(&idev->lk_table.lock);
-	idev->sm_lid = be16_to_cpu(IB_LID_PERMISSIVE);
-	/* Set the prefix to the default value (see ch. 4.1.1) */
-	idev->gid_prefix = cpu_to_be64(0xfe80000000000000ULL);
-
-	ret = ipath_init_qp_table(idev, ib_ipath_qp_table_size);
-	if (ret)
-		goto err_qp;
-
-	/*
-	 * The top ib_ipath_lkey_table_size bits are used to index the
-	 * table.  The lower 8 bits can be owned by the user (copied from
-	 * the LKEY).  The remaining bits act as a generation number or tag.
-	 */
-	idev->lk_table.max = 1 << ib_ipath_lkey_table_size;
-	idev->lk_table.table = kcalloc(idev->lk_table.max,
-				       sizeof(*idev->lk_table.table),
-				       GFP_KERNEL);
-	if (idev->lk_table.table == NULL) {
-		ret = -ENOMEM;
-		goto err_lk;
-	}
-	INIT_LIST_HEAD(&idev->pending_mmaps);
-	spin_lock_init(&idev->pending_lock);
-	idev->mmap_offset = PAGE_SIZE;
-	spin_lock_init(&idev->mmap_offset_lock);
-	INIT_LIST_HEAD(&idev->pending[0]);
-	INIT_LIST_HEAD(&idev->pending[1]);
-	INIT_LIST_HEAD(&idev->pending[2]);
-	INIT_LIST_HEAD(&idev->piowait);
-	INIT_LIST_HEAD(&idev->rnrwait);
-	INIT_LIST_HEAD(&idev->txreq_free);
-	idev->pending_index = 0;
-	idev->port_cap_flags =
-		IB_PORT_SYS_IMAGE_GUID_SUP | IB_PORT_CLIENT_REG_SUP;
-	if (dd->ipath_flags & IPATH_HAS_LINK_LATENCY)
-		idev->port_cap_flags |= IB_PORT_LINK_LATENCY_SUP;
-	idev->pma_counter_select[0] = IB_PMA_PORT_XMIT_DATA;
-	idev->pma_counter_select[1] = IB_PMA_PORT_RCV_DATA;
-	idev->pma_counter_select[2] = IB_PMA_PORT_XMIT_PKTS;
-	idev->pma_counter_select[3] = IB_PMA_PORT_RCV_PKTS;
-	idev->pma_counter_select[4] = IB_PMA_PORT_XMIT_WAIT;
-
-	/* Snapshot current HW counters to "clear" them. */
-	ipath_get_counters(dd, &cntrs);
-	idev->z_symbol_error_counter = cntrs.symbol_error_counter;
-	idev->z_link_error_recovery_counter =
-		cntrs.link_error_recovery_counter;
-	idev->z_link_downed_counter = cntrs.link_downed_counter;
-	idev->z_port_rcv_errors = cntrs.port_rcv_errors;
-	idev->z_port_rcv_remphys_errors =
-		cntrs.port_rcv_remphys_errors;
-	idev->z_port_xmit_discards = cntrs.port_xmit_discards;
-	idev->z_port_xmit_data = cntrs.port_xmit_data;
-	idev->z_port_rcv_data = cntrs.port_rcv_data;
-	idev->z_port_xmit_packets = cntrs.port_xmit_packets;
-	idev->z_port_rcv_packets = cntrs.port_rcv_packets;
-	idev->z_local_link_integrity_errors =
-		cntrs.local_link_integrity_errors;
-	idev->z_excessive_buffer_overrun_errors =
-		cntrs.excessive_buffer_overrun_errors;
-	idev->z_vl15_dropped = cntrs.vl15_dropped;
-
-	for (i = 0; i < dd->ipath_sdma_descq_cnt; i++, tx++)
-		list_add(&tx->txreq.list, &idev->txreq_free);
-
-	/*
-	 * The system image GUID is supposed to be the same for all
-	 * IB HCAs in a single system but since there can be other
-	 * device types in the system, we can't be sure this is unique.
-	 */
-	if (!sys_image_guid)
-		sys_image_guid = dd->ipath_guid;
-	idev->sys_image_guid = sys_image_guid;
-	idev->ib_unit = dd->ipath_unit;
-	idev->dd = dd;
-
-	strlcpy(dev->name, "ipath%d", IB_DEVICE_NAME_MAX);
-	dev->owner = THIS_MODULE;
-	dev->node_guid = dd->ipath_guid;
-	dev->uverbs_abi_ver = IPATH_UVERBS_ABI_VERSION;
-	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_CREATE_AH)		|
-		(1ull << IB_USER_VERBS_CMD_DESTROY_AH)		|
-		(1ull << IB_USER_VERBS_CMD_QUERY_AH)		|
-		(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_RESIZE_CQ)		|
-		(1ull << IB_USER_VERBS_CMD_DESTROY_CQ)		|
-		(1ull << IB_USER_VERBS_CMD_POLL_CQ)		|
-		(1ull << IB_USER_VERBS_CMD_REQ_NOTIFY_CQ)	|
-		(1ull << IB_USER_VERBS_CMD_CREATE_QP)		|
-		(1ull << IB_USER_VERBS_CMD_QUERY_QP)		|
-		(1ull << IB_USER_VERBS_CMD_MODIFY_QP)		|
-		(1ull << IB_USER_VERBS_CMD_DESTROY_QP)		|
-		(1ull << IB_USER_VERBS_CMD_POST_SEND)		|
-		(1ull << IB_USER_VERBS_CMD_POST_RECV)		|
-		(1ull << IB_USER_VERBS_CMD_ATTACH_MCAST)	|
-		(1ull << IB_USER_VERBS_CMD_DETACH_MCAST)	|
-		(1ull << IB_USER_VERBS_CMD_CREATE_SRQ)		|
-		(1ull << IB_USER_VERBS_CMD_MODIFY_SRQ)		|
-		(1ull << IB_USER_VERBS_CMD_QUERY_SRQ)		|
-		(1ull << IB_USER_VERBS_CMD_DESTROY_SRQ)		|
-		(1ull << IB_USER_VERBS_CMD_POST_SRQ_RECV);
-	dev->node_type = RDMA_NODE_IB_CA;
-	dev->phys_port_cnt = 1;
-	dev->num_comp_vectors = 1;
-	dev->dma_device = &dd->pcidev->dev;
-	dev->query_device = ipath_query_device;
-	dev->modify_device = ipath_modify_device;
-	dev->query_port = ipath_query_port;
-	dev->modify_port = ipath_modify_port;
-	dev->query_pkey = ipath_query_pkey;
-	dev->query_gid = ipath_query_gid;
-	dev->alloc_ucontext = ipath_alloc_ucontext;
-	dev->dealloc_ucontext = ipath_dealloc_ucontext;
-	dev->alloc_pd = ipath_alloc_pd;
-	dev->dealloc_pd = ipath_dealloc_pd;
-	dev->create_ah = ipath_create_ah;
-	dev->destroy_ah = ipath_destroy_ah;
-	dev->query_ah = ipath_query_ah;
-	dev->create_srq = ipath_create_srq;
-	dev->modify_srq = ipath_modify_srq;
-	dev->query_srq = ipath_query_srq;
-	dev->destroy_srq = ipath_destroy_srq;
-	dev->create_qp = ipath_create_qp;
-	dev->modify_qp = ipath_modify_qp;
-	dev->query_qp = ipath_query_qp;
-	dev->destroy_qp = ipath_destroy_qp;
-	dev->post_send = ipath_post_send;
-	dev->post_recv = ipath_post_receive;
-	dev->post_srq_recv = ipath_post_srq_receive;
-	dev->create_cq = ipath_create_cq;
-	dev->destroy_cq = ipath_destroy_cq;
-	dev->resize_cq = ipath_resize_cq;
-	dev->poll_cq = ipath_poll_cq;
-	dev->req_notify_cq = ipath_req_notify_cq;
-	dev->get_dma_mr = ipath_get_dma_mr;
-	dev->reg_phys_mr = ipath_reg_phys_mr;
-	dev->reg_user_mr = ipath_reg_user_mr;
-	dev->dereg_mr = ipath_dereg_mr;
-	dev->alloc_fmr = ipath_alloc_fmr;
-	dev->map_phys_fmr = ipath_map_phys_fmr;
-	dev->unmap_fmr = ipath_unmap_fmr;
-	dev->dealloc_fmr = ipath_dealloc_fmr;
-	dev->attach_mcast = ipath_multicast_attach;
-	dev->detach_mcast = ipath_multicast_detach;
-	dev->process_mad = ipath_process_mad;
-	dev->mmap = ipath_mmap;
-	dev->dma_ops = &ipath_dma_mapping_ops;
-	dev->get_port_immutable = ipath_port_immutable;
-
-	snprintf(dev->node_desc, sizeof(dev->node_desc),
-		 IPATH_IDSTR " %s", init_utsname()->nodename);
-
-	ret = ib_register_device(dev, NULL);
-	if (ret)
-		goto err_reg;
-
-	ret = ipath_verbs_register_sysfs(dev);
-	if (ret)
-		goto err_class;
-
-	enable_timer(dd);
-
-	goto bail;
-
-err_class:
-	ib_unregister_device(dev);
-err_reg:
-	kfree(idev->lk_table.table);
-err_lk:
-	kfree(idev->qp_table.table);
-err_qp:
-	kfree(idev->txreq_bufs);
-err_tx:
-	ib_dealloc_device(dev);
-	ipath_dev_err(dd, "cannot register verbs: %d!\n", -ret);
-	idev = NULL;
-
-bail:
-	dd->verbs_dev = idev;
-	return ret;
-}
-
-void ipath_unregister_ib_device(struct ipath_ibdev *dev)
-{
-	struct ib_device *ibdev = &dev->ibdev;
-	u32 qps_inuse;
-
-	ib_unregister_device(ibdev);
-
-	disable_timer(dev->dd);
-
-	if (!list_empty(&dev->pending[0]) ||
-	    !list_empty(&dev->pending[1]) ||
-	    !list_empty(&dev->pending[2]))
-		ipath_dev_err(dev->dd, "pending list not empty!\n");
-	if (!list_empty(&dev->piowait))
-		ipath_dev_err(dev->dd, "piowait list not empty!\n");
-	if (!list_empty(&dev->rnrwait))
-		ipath_dev_err(dev->dd, "rnrwait list not empty!\n");
-	if (!ipath_mcast_tree_empty())
-		ipath_dev_err(dev->dd, "multicast table memory leak!\n");
-	/*
-	 * Note that ipath_unregister_ib_device() can be called before all
-	 * the QPs are destroyed!
-	 */
-	qps_inuse = ipath_free_all_qps(&dev->qp_table);
-	if (qps_inuse)
-		ipath_dev_err(dev->dd, "QP memory leak! %u still in use\n",
-			qps_inuse);
-	kfree(dev->qp_table.table);
-	kfree(dev->lk_table.table);
-	kfree(dev->txreq_bufs);
-	ib_dealloc_device(ibdev);
-}
-
-static ssize_t show_rev(struct device *device, struct device_attribute *attr,
-			char *buf)
-{
-	struct ipath_ibdev *dev =
-		container_of(device, struct ipath_ibdev, ibdev.dev);
-
-	return sprintf(buf, "%x\n", dev->dd->ipath_pcirev);
-}
-
-static ssize_t show_hca(struct device *device, struct device_attribute *attr,
-			char *buf)
-{
-	struct ipath_ibdev *dev =
-		container_of(device, struct ipath_ibdev, ibdev.dev);
-	int ret;
-
-	ret = dev->dd->ipath_f_get_boardname(dev->dd, buf, 128);
-	if (ret < 0)
-		goto bail;
-	strcat(buf, "\n");
-	ret = strlen(buf);
-
-bail:
-	return ret;
-}
-
-static ssize_t show_stats(struct device *device, struct device_attribute *attr,
-			  char *buf)
-{
-	struct ipath_ibdev *dev =
-		container_of(device, struct ipath_ibdev, ibdev.dev);
-	int i;
-	int len;
-
-	len = sprintf(buf,
-		      "RC resends  %d\n"
-		      "RC no QACK  %d\n"
-		      "RC ACKs     %d\n"
-		      "RC SEQ NAKs %d\n"
-		      "RC RDMA seq %d\n"
-		      "RC RNR NAKs %d\n"
-		      "RC OTH NAKs %d\n"
-		      "RC timeouts %d\n"
-		      "RC RDMA dup %d\n"
-		      "piobuf wait %d\n"
-		      "unaligned   %d\n"
-		      "PKT drops   %d\n"
-		      "WQE errs    %d\n",
-		      dev->n_rc_resends, dev->n_rc_qacks, dev->n_rc_acks,
-		      dev->n_seq_naks, dev->n_rdma_seq, dev->n_rnr_naks,
-		      dev->n_other_naks, dev->n_timeouts,
-		      dev->n_rdma_dup_busy, dev->n_piowait, dev->n_unaligned,
-		      dev->n_pkt_drops, dev->n_wqe_errs);
-	for (i = 0; i < ARRAY_SIZE(dev->opstats); i++) {
-		const struct ipath_opcode_stats *si = &dev->opstats[i];
-
-		if (!si->n_packets && !si->n_bytes)
-			continue;
-		len += sprintf(buf + len, "%02x %llu/%llu\n", i,
-			       (unsigned long long) si->n_packets,
-			       (unsigned long long) si->n_bytes);
-	}
-	return len;
-}
-
-static DEVICE_ATTR(hw_rev, S_IRUGO, show_rev, NULL);
-static DEVICE_ATTR(hca_type, S_IRUGO, show_hca, NULL);
-static DEVICE_ATTR(board_id, S_IRUGO, show_hca, NULL);
-static DEVICE_ATTR(stats, S_IRUGO, show_stats, NULL);
-
-static struct device_attribute *ipath_class_attributes[] = {
-	&dev_attr_hw_rev,
-	&dev_attr_hca_type,
-	&dev_attr_board_id,
-	&dev_attr_stats
-};
-
-static int ipath_verbs_register_sysfs(struct ib_device *dev)
-{
-	int i;
-	int ret;
-
-	for (i = 0; i < ARRAY_SIZE(ipath_class_attributes); ++i) {
-		ret = device_create_file(&dev->dev,
-				       ipath_class_attributes[i]);
-		if (ret)
-			goto bail;
-	}
-	return 0;
-bail:
-	for (i = 0; i < ARRAY_SIZE(ipath_class_attributes); ++i)
-		device_remove_file(&dev->dev, ipath_class_attributes[i]);
-	return ret;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_verbs.h b/drivers/staging/rdma/ipath/ipath_verbs.h
deleted file mode 100644
index 0a90a56870ab..000000000000
--- a/drivers/staging/rdma/ipath/ipath_verbs.h
+++ /dev/null
@@ -1,945 +0,0 @@
-/*
- * Copyright (c) 2006, 2007, 2008 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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 IPATH_VERBS_H
-#define IPATH_VERBS_H
-
-#include <linux/types.h>
-#include <linux/spinlock.h>
-#include <linux/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/kref.h>
-#include <rdma/ib_pack.h>
-#include <rdma/ib_user_verbs.h>
-
-#include "ipath_kernel.h"
-
-#define IPATH_MAX_RDMA_ATOMIC	4
-
-#define QPN_MAX                 (1 << 24)
-#define QPNMAP_ENTRIES          (QPN_MAX / PAGE_SIZE / BITS_PER_BYTE)
-
-/*
- * Increment this value if any changes that break userspace ABI
- * compatibility are made.
- */
-#define IPATH_UVERBS_ABI_VERSION       2
-
-/*
- * Define an ib_cq_notify value that is not valid so we know when CQ
- * notifications are armed.
- */
-#define IB_CQ_NONE	(IB_CQ_NEXT_COMP + 1)
-
-/* 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
-
-/* Flags for checking QP state (see ib_ipath_state_ops[]) */
-#define IPATH_POST_SEND_OK		0x01
-#define IPATH_POST_RECV_OK		0x02
-#define IPATH_PROCESS_RECV_OK		0x04
-#define IPATH_PROCESS_SEND_OK		0x08
-#define IPATH_PROCESS_NEXT_SEND_OK	0x10
-#define IPATH_FLUSH_SEND		0x20
-#define IPATH_FLUSH_RECV		0x40
-#define IPATH_PROCESS_OR_FLUSH_SEND \
-	(IPATH_PROCESS_SEND_OK | IPATH_FLUSH_SEND)
-
-/* IB Performance Manager status values */
-#define IB_PMA_SAMPLE_STATUS_DONE	0x00
-#define IB_PMA_SAMPLE_STATUS_STARTED	0x01
-#define IB_PMA_SAMPLE_STATUS_RUNNING	0x02
-
-/* Mandatory IB performance counter select values. */
-#define IB_PMA_PORT_XMIT_DATA	cpu_to_be16(0x0001)
-#define IB_PMA_PORT_RCV_DATA	cpu_to_be16(0x0002)
-#define IB_PMA_PORT_XMIT_PKTS	cpu_to_be16(0x0003)
-#define IB_PMA_PORT_RCV_PKTS	cpu_to_be16(0x0004)
-#define IB_PMA_PORT_XMIT_WAIT	cpu_to_be16(0x0005)
-
-struct ib_reth {
-	__be64 vaddr;
-	__be32 rkey;
-	__be32 length;
-} __attribute__ ((packed));
-
-struct ib_atomic_eth {
-	__be32 vaddr[2];	/* unaligned so access as 2 32-bit words */
-	__be32 rkey;
-	__be64 swap_data;
-	__be64 compare_data;
-} __attribute__ ((packed));
-
-struct ipath_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;
-		struct ib_atomic_eth atomic_eth;
-	} u;
-} __attribute__ ((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 ipath_ib_header {
-	__be16 lrh[4];
-	union {
-		struct {
-			struct ib_grh grh;
-			struct ipath_other_headers oth;
-		} l;
-		struct ipath_other_headers oth;
-	} u;
-} __attribute__ ((packed));
-
-struct ipath_pio_header {
-	__le32 pbc[2];
-	struct ipath_ib_header hdr;
-} __attribute__ ((packed));
-
-/*
- * There is one struct ipath_mcast for each multicast GID.
- * All attached QPs are then stored as a list of
- * struct ipath_mcast_qp.
- */
-struct ipath_mcast_qp {
-	struct list_head list;
-	struct ipath_qp *qp;
-};
-
-struct ipath_mcast {
-	struct rb_node rb_node;
-	union ib_gid mgid;
-	struct list_head qp_list;
-	wait_queue_head_t wait;
-	atomic_t refcount;
-	int n_attached;
-};
-
-/* Protection domain */
-struct ipath_pd {
-	struct ib_pd ibpd;
-	int user;		/* non-zero if created from user space */
-};
-
-/* Address Handle */
-struct ipath_ah {
-	struct ib_ah ibah;
-	struct ib_ah_attr attr;
-};
-
-/*
- * This structure is used by ipath_mmap() to validate an offset
- * when an mmap() request is made.  The vm_area_struct then uses
- * this as its vm_private_data.
- */
-struct ipath_mmap_info {
-	struct list_head pending_mmaps;
-	struct ib_ucontext *context;
-	void *obj;
-	__u64 offset;
-	struct kref ref;
-	unsigned size;
-};
-
-/*
- * This structure is used to contain the head pointer, tail pointer,
- * and completion queue entries as a single memory allocation so
- * it can be mmap'ed into user space.
- */
-struct ipath_cq_wc {
-	u32 head;		/* index of next entry to fill */
-	u32 tail;		/* index of next ib_poll_cq() entry */
-	union {
-		/* these are actually size ibcq.cqe + 1 */
-		struct ib_uverbs_wc uqueue[0];
-		struct ib_wc kqueue[0];
-	};
-};
-
-/*
- * The completion queue structure.
- */
-struct ipath_cq {
-	struct ib_cq ibcq;
-	struct tasklet_struct comptask;
-	spinlock_t lock;
-	u8 notify;
-	u8 triggered;
-	struct ipath_cq_wc *queue;
-	struct ipath_mmap_info *ip;
-};
-
-/*
- * A segment is a linear region of low physical memory.
- * XXX Maybe we should use phys addr here and kmap()/kunmap().
- * Used by the verbs layer.
- */
-struct ipath_seg {
-	void *vaddr;
-	size_t length;
-};
-
-/* The number of ipath_segs that fit in a page. */
-#define IPATH_SEGSZ     (PAGE_SIZE / sizeof (struct ipath_seg))
-
-struct ipath_segarray {
-	struct ipath_seg segs[IPATH_SEGSZ];
-};
-
-struct ipath_mregion {
-	struct ib_pd *pd;	/* shares refcnt of ibmr.pd */
-	u64 user_base;		/* User's address for this region */
-	u64 iova;		/* IB start address of this region */
-	size_t length;
-	u32 lkey;
-	u32 offset;		/* offset (bytes) to start of region */
-	int access_flags;
-	u32 max_segs;		/* number of ipath_segs in all the arrays */
-	u32 mapsz;		/* size of the map array */
-	struct ipath_segarray *map[0];	/* the segments */
-};
-
-/*
- * These keep track of the copy progress within a memory region.
- * Used by the verbs layer.
- */
-struct ipath_sge {
-	struct ipath_mregion *mr;
-	void *vaddr;		/* kernel virtual address of segment */
-	u32 sge_length;		/* length of the SGE */
-	u32 length;		/* remaining length of the segment */
-	u16 m;			/* current index: mr->map[m] */
-	u16 n;			/* current index: mr->map[m]->segs[n] */
-};
-
-/* Memory region */
-struct ipath_mr {
-	struct ib_mr ibmr;
-	struct ib_umem *umem;
-	struct ipath_mregion mr;	/* must be last */
-};
-
-/*
- * Send work request queue entry.
- * The size of the sg_list is determined when the QP is created and stored
- * in qp->s_max_sge.
- */
-struct ipath_swqe {
-	union {
-		struct ib_send_wr wr;   /* don't use wr.sg_list */
-		struct ib_ud_wr ud_wr;
-		struct ib_rdma_wr rdma_wr;
-		struct ib_atomic_wr atomic_wr;
-	};
-
-	u32 psn;		/* first packet sequence number */
-	u32 lpsn;		/* last packet sequence number */
-	u32 ssn;		/* send sequence number */
-	u32 length;		/* total length of data in sg_list */
-	struct ipath_sge sg_list[0];
-};
-
-/*
- * Receive work request queue entry.
- * The size of the sg_list is determined when the QP (or SRQ) is created
- * and stored in qp->r_rq.max_sge (or srq->rq.max_sge).
- */
-struct ipath_rwqe {
-	u64 wr_id;
-	u8 num_sge;
-	struct ib_sge sg_list[0];
-};
-
-/*
- * This structure is used to contain the head pointer, tail pointer,
- * and receive work queue entries as a single memory allocation so
- * it can be mmap'ed into user space.
- * Note that the wq array elements are variable size so you can't
- * just index into the array to get the N'th element;
- * use get_rwqe_ptr() instead.
- */
-struct ipath_rwq {
-	u32 head;		/* new work requests posted to the head */
-	u32 tail;		/* receives pull requests from here. */
-	struct ipath_rwqe wq[0];
-};
-
-struct ipath_rq {
-	struct ipath_rwq *wq;
-	spinlock_t lock;
-	u32 size;		/* size of RWQE array */
-	u8 max_sge;
-};
-
-struct ipath_srq {
-	struct ib_srq ibsrq;
-	struct ipath_rq rq;
-	struct ipath_mmap_info *ip;
-	/* send signal when number of RWQEs < limit */
-	u32 limit;
-};
-
-struct ipath_sge_state {
-	struct ipath_sge *sg_list;      /* next SGE to be used if any */
-	struct ipath_sge sge;   /* progress state for the current SGE */
-	u8 num_sge;
-	u8 static_rate;
-};
-
-/*
- * This structure holds the information that the send tasklet needs
- * to send a RDMA read response or atomic operation.
- */
-struct ipath_ack_entry {
-	u8 opcode;
-	u8 sent;
-	u32 psn;
-	union {
-		struct ipath_sge_state rdma_sge;
-		u64 atomic_data;
-	};
-};
-
-/*
- * Variables prefixed with s_ are for the requester (sender).
- * Variables prefixed with r_ are for the responder (receiver).
- * Variables prefixed with ack_ are for responder replies.
- *
- * Common variables are protected by both r_rq.lock and s_lock in that order
- * which only happens in modify_qp() or changing the QP 'state'.
- */
-struct ipath_qp {
-	struct ib_qp ibqp;
-	struct ipath_qp *next;		/* link list for QPN hash table */
-	struct ipath_qp *timer_next;	/* link list for ipath_ib_timer() */
-	struct ipath_qp *pio_next;	/* link for ipath_ib_piobufavail() */
-	struct list_head piowait;	/* link for wait PIO buf */
-	struct list_head timerwait;	/* link for waiting for timeouts */
-	struct ib_ah_attr remote_ah_attr;
-	struct ipath_ib_header s_hdr;	/* next packet header to send */
-	atomic_t refcount;
-	wait_queue_head_t wait;
-	wait_queue_head_t wait_dma;
-	struct tasklet_struct s_task;
-	struct ipath_mmap_info *ip;
-	struct ipath_sge_state *s_cur_sge;
-	struct ipath_verbs_txreq *s_tx;
-	struct ipath_sge_state s_sge;	/* current send request data */
-	struct ipath_ack_entry s_ack_queue[IPATH_MAX_RDMA_ATOMIC + 1];
-	struct ipath_sge_state s_ack_rdma_sge;
-	struct ipath_sge_state s_rdma_read_sge;
-	struct ipath_sge_state r_sge;	/* current receive data */
-	spinlock_t s_lock;
-	atomic_t s_dma_busy;
-	u16 s_pkt_delay;
-	u16 s_hdrwords;		/* size of s_hdr in 32 bit words */
-	u32 s_cur_size;		/* size of send packet in bytes */
-	u32 s_len;		/* total length of s_sge */
-	u32 s_rdma_read_len;	/* total length of s_rdma_read_sge */
-	u32 s_next_psn;		/* PSN for next request */
-	u32 s_last_psn;		/* last response PSN processed */
-	u32 s_psn;		/* current packet sequence number */
-	u32 s_ack_rdma_psn;	/* PSN for sending RDMA read responses */
-	u32 s_ack_psn;		/* PSN for acking sends and RDMA writes */
-	u32 s_rnr_timeout;	/* number of milliseconds for RNR timeout */
-	u32 r_ack_psn;		/* PSN for next ACK or atomic ACK */
-	u64 r_wr_id;		/* ID for current receive WQE */
-	unsigned long r_aflags;
-	u32 r_len;		/* total length of r_sge */
-	u32 r_rcv_len;		/* receive data len processed */
-	u32 r_psn;		/* expected rcv packet sequence number */
-	u32 r_msn;		/* message sequence number */
-	u8 state;		/* QP state */
-	u8 s_state;		/* opcode of last packet sent */
-	u8 s_ack_state;		/* opcode of packet to ACK */
-	u8 s_nak_state;		/* non-zero if NAK is pending */
-	u8 r_state;		/* opcode of last packet received */
-	u8 r_nak_state;		/* non-zero if NAK is pending */
-	u8 r_min_rnr_timer;	/* retry timeout value for RNR NAKs */
-	u8 r_flags;
-	u8 r_max_rd_atomic;	/* max number of RDMA read/atomic to receive */
-	u8 r_head_ack_queue;	/* index into s_ack_queue[] */
-	u8 qp_access_flags;
-	u8 s_max_sge;		/* size of s_wq->sg_list */
-	u8 s_retry_cnt;		/* number of times to retry */
-	u8 s_rnr_retry_cnt;
-	u8 s_retry;		/* requester retry counter */
-	u8 s_rnr_retry;		/* requester RNR retry counter */
-	u8 s_pkey_index;	/* PKEY index to use */
-	u8 s_max_rd_atomic;	/* max number of RDMA read/atomic to send */
-	u8 s_num_rd_atomic;	/* number of RDMA read/atomic pending */
-	u8 s_tail_ack_queue;	/* index into s_ack_queue[] */
-	u8 s_flags;
-	u8 s_dmult;
-	u8 s_draining;
-	u8 timeout;		/* Timeout for this QP */
-	enum ib_mtu path_mtu;
-	u32 remote_qpn;
-	u32 qkey;		/* QKEY for this QP (for UD or RD) */
-	u32 s_size;		/* send work queue size */
-	u32 s_head;		/* new entries added here */
-	u32 s_tail;		/* next entry to process */
-	u32 s_cur;		/* current work queue entry */
-	u32 s_last;		/* last un-ACK'ed entry */
-	u32 s_ssn;		/* SSN of tail entry */
-	u32 s_lsn;		/* limit sequence number (credit) */
-	struct ipath_swqe *s_wq;	/* send work queue */
-	struct ipath_swqe *s_wqe;
-	struct ipath_sge *r_ud_sg_list;
-	struct ipath_rq r_rq;		/* receive work queue */
-	struct ipath_sge r_sg_list[0];	/* verified SGEs */
-};
-
-/*
- * Atomic bit definitions for r_aflags.
- */
-#define IPATH_R_WRID_VALID	0
-
-/*
- * Bit definitions for r_flags.
- */
-#define IPATH_R_REUSE_SGE	0x01
-#define IPATH_R_RDMAR_SEQ	0x02
-
-/*
- * Bit definitions for s_flags.
- *
- * IPATH_S_FENCE_PENDING - waiting for all prior RDMA read or atomic SWQEs
- *			   before processing the next SWQE
- * IPATH_S_RDMAR_PENDING - waiting for any RDMA read or atomic SWQEs
- *			   before processing the next SWQE
- * IPATH_S_WAITING - waiting for RNR timeout or send buffer available.
- * IPATH_S_WAIT_SSN_CREDIT - waiting for RC credits to process next SWQE
- * IPATH_S_WAIT_DMA - waiting for send DMA queue to drain before generating
- *		      next send completion entry not via send DMA.
- */
-#define IPATH_S_SIGNAL_REQ_WR	0x01
-#define IPATH_S_FENCE_PENDING	0x02
-#define IPATH_S_RDMAR_PENDING	0x04
-#define IPATH_S_ACK_PENDING	0x08
-#define IPATH_S_BUSY		0x10
-#define IPATH_S_WAITING		0x20
-#define IPATH_S_WAIT_SSN_CREDIT	0x40
-#define IPATH_S_WAIT_DMA	0x80
-
-#define IPATH_S_ANY_WAIT (IPATH_S_FENCE_PENDING | IPATH_S_RDMAR_PENDING | \
-	IPATH_S_WAITING | IPATH_S_WAIT_SSN_CREDIT | IPATH_S_WAIT_DMA)
-
-#define IPATH_PSN_CREDIT	512
-
-/*
- * Since struct ipath_swqe is not a fixed size, we can't simply index into
- * struct ipath_qp.s_wq.  This function does the array index computation.
- */
-static inline struct ipath_swqe *get_swqe_ptr(struct ipath_qp *qp,
-					      unsigned n)
-{
-	return (struct ipath_swqe *)((char *)qp->s_wq +
-				     (sizeof(struct ipath_swqe) +
-				      qp->s_max_sge *
-				      sizeof(struct ipath_sge)) * n);
-}
-
-/*
- * Since struct ipath_rwqe is not a fixed size, we can't simply index into
- * struct ipath_rwq.wq.  This function does the array index computation.
- */
-static inline struct ipath_rwqe *get_rwqe_ptr(struct ipath_rq *rq,
-					      unsigned n)
-{
-	return (struct ipath_rwqe *)
-		((char *) rq->wq->wq +
-		 (sizeof(struct ipath_rwqe) +
-		  rq->max_sge * sizeof(struct ib_sge)) * n);
-}
-
-/*
- * QPN-map pages start out as NULL, they get allocated upon
- * first use and are never deallocated. This way,
- * large bitmaps are not allocated unless large numbers of QPs are used.
- */
-struct qpn_map {
-	atomic_t n_free;
-	void *page;
-};
-
-struct ipath_qp_table {
-	spinlock_t lock;
-	u32 last;		/* last QP number allocated */
-	u32 max;		/* size of the hash table */
-	u32 nmaps;		/* size of the map table */
-	struct ipath_qp **table;
-	/* bit map of free numbers */
-	struct qpn_map map[QPNMAP_ENTRIES];
-};
-
-struct ipath_lkey_table {
-	spinlock_t lock;
-	u32 next;		/* next unused index (speeds search) */
-	u32 gen;		/* generation count */
-	u32 max;		/* size of the table */
-	struct ipath_mregion **table;
-};
-
-struct ipath_opcode_stats {
-	u64 n_packets;		/* number of packets */
-	u64 n_bytes;		/* total number of bytes */
-};
-
-struct ipath_ibdev {
-	struct ib_device ibdev;
-	struct ipath_devdata *dd;
-	struct list_head pending_mmaps;
-	spinlock_t mmap_offset_lock;
-	u32 mmap_offset;
-	int ib_unit;		/* This is the device number */
-	u16 sm_lid;		/* in host order */
-	u8 sm_sl;
-	u8 mkeyprot;
-	/* non-zero when timer is set */
-	unsigned long mkey_lease_timeout;
-
-	/* The following fields are really per port. */
-	struct ipath_qp_table qp_table;
-	struct ipath_lkey_table lk_table;
-	struct list_head pending[3];	/* FIFO of QPs waiting for ACKs */
-	struct list_head piowait;	/* list for wait PIO buf */
-	struct list_head txreq_free;
-	void *txreq_bufs;
-	/* list of QPs waiting for RNR timer */
-	struct list_head rnrwait;
-	spinlock_t pending_lock;
-	__be64 sys_image_guid;	/* in network order */
-	__be64 gid_prefix;	/* in network order */
-	__be64 mkey;
-
-	u32 n_pds_allocated;	/* number of PDs allocated for device */
-	spinlock_t n_pds_lock;
-	u32 n_ahs_allocated;	/* number of AHs allocated for device */
-	spinlock_t n_ahs_lock;
-	u32 n_cqs_allocated;	/* number of CQs allocated for device */
-	spinlock_t n_cqs_lock;
-	u32 n_qps_allocated;	/* number of QPs allocated for device */
-	spinlock_t n_qps_lock;
-	u32 n_srqs_allocated;	/* number of SRQs allocated for device */
-	spinlock_t n_srqs_lock;
-	u32 n_mcast_grps_allocated; /* number of mcast groups allocated */
-	spinlock_t n_mcast_grps_lock;
-
-	u64 ipath_sword;	/* total dwords sent (sample result) */
-	u64 ipath_rword;	/* total dwords received (sample result) */
-	u64 ipath_spkts;	/* total packets sent (sample result) */
-	u64 ipath_rpkts;	/* total packets received (sample result) */
-	/* # of ticks no data sent (sample result) */
-	u64 ipath_xmit_wait;
-	u64 rcv_errors;		/* # of packets with SW detected rcv errs */
-	u64 n_unicast_xmit;	/* total unicast packets sent */
-	u64 n_unicast_rcv;	/* total unicast packets received */
-	u64 n_multicast_xmit;	/* total multicast packets sent */
-	u64 n_multicast_rcv;	/* total multicast packets received */
-	u64 z_symbol_error_counter;		/* starting count for PMA */
-	u64 z_link_error_recovery_counter;	/* starting count for PMA */
-	u64 z_link_downed_counter;		/* starting count for PMA */
-	u64 z_port_rcv_errors;			/* starting count for PMA */
-	u64 z_port_rcv_remphys_errors;		/* starting count for PMA */
-	u64 z_port_xmit_discards;		/* starting count for PMA */
-	u64 z_port_xmit_data;			/* starting count for PMA */
-	u64 z_port_rcv_data;			/* starting count for PMA */
-	u64 z_port_xmit_packets;		/* starting count for PMA */
-	u64 z_port_rcv_packets;			/* starting count for PMA */
-	u32 z_pkey_violations;			/* starting count for PMA */
-	u32 z_local_link_integrity_errors;	/* starting count for PMA */
-	u32 z_excessive_buffer_overrun_errors;	/* starting count for PMA */
-	u32 z_vl15_dropped;			/* starting count for PMA */
-	u32 n_rc_resends;
-	u32 n_rc_acks;
-	u32 n_rc_qacks;
-	u32 n_seq_naks;
-	u32 n_rdma_seq;
-	u32 n_rnr_naks;
-	u32 n_other_naks;
-	u32 n_timeouts;
-	u32 n_pkt_drops;
-	u32 n_vl15_dropped;
-	u32 n_wqe_errs;
-	u32 n_rdma_dup_busy;
-	u32 n_piowait;
-	u32 n_unaligned;
-	u32 port_cap_flags;
-	u32 pma_sample_start;
-	u32 pma_sample_interval;
-	__be16 pma_counter_select[5];
-	u16 pma_tag;
-	u16 qkey_violations;
-	u16 mkey_violations;
-	u16 mkey_lease_period;
-	u16 pending_index;	/* which pending queue is active */
-	u8 pma_sample_status;
-	u8 subnet_timeout;
-	u8 vl_high_limit;
-	struct ipath_opcode_stats opstats[128];
-};
-
-struct ipath_verbs_counters {
-	u64 symbol_error_counter;
-	u64 link_error_recovery_counter;
-	u64 link_downed_counter;
-	u64 port_rcv_errors;
-	u64 port_rcv_remphys_errors;
-	u64 port_xmit_discards;
-	u64 port_xmit_data;
-	u64 port_rcv_data;
-	u64 port_xmit_packets;
-	u64 port_rcv_packets;
-	u32 local_link_integrity_errors;
-	u32 excessive_buffer_overrun_errors;
-	u32 vl15_dropped;
-};
-
-struct ipath_verbs_txreq {
-	struct ipath_qp         *qp;
-	struct ipath_swqe       *wqe;
-	u32                      map_len;
-	u32                      len;
-	struct ipath_sge_state  *ss;
-	struct ipath_pio_header  hdr;
-	struct ipath_sdma_txreq  txreq;
-};
-
-static inline struct ipath_mr *to_imr(struct ib_mr *ibmr)
-{
-	return container_of(ibmr, struct ipath_mr, ibmr);
-}
-
-static inline struct ipath_pd *to_ipd(struct ib_pd *ibpd)
-{
-	return container_of(ibpd, struct ipath_pd, ibpd);
-}
-
-static inline struct ipath_ah *to_iah(struct ib_ah *ibah)
-{
-	return container_of(ibah, struct ipath_ah, ibah);
-}
-
-static inline struct ipath_cq *to_icq(struct ib_cq *ibcq)
-{
-	return container_of(ibcq, struct ipath_cq, ibcq);
-}
-
-static inline struct ipath_srq *to_isrq(struct ib_srq *ibsrq)
-{
-	return container_of(ibsrq, struct ipath_srq, ibsrq);
-}
-
-static inline struct ipath_qp *to_iqp(struct ib_qp *ibqp)
-{
-	return container_of(ibqp, struct ipath_qp, ibqp);
-}
-
-static inline struct ipath_ibdev *to_idev(struct ib_device *ibdev)
-{
-	return container_of(ibdev, struct ipath_ibdev, ibdev);
-}
-
-/*
- * This must be called with s_lock held.
- */
-static inline void ipath_schedule_send(struct ipath_qp *qp)
-{
-	if (qp->s_flags & IPATH_S_ANY_WAIT)
-		qp->s_flags &= ~IPATH_S_ANY_WAIT;
-	if (!(qp->s_flags & IPATH_S_BUSY))
-		tasklet_hi_schedule(&qp->s_task);
-}
-
-int ipath_process_mad(struct ib_device *ibdev,
-		      int mad_flags,
-		      u8 port_num,
-		      const struct ib_wc *in_wc,
-		      const struct ib_grh *in_grh,
-		      const struct ib_mad_hdr *in, size_t in_mad_size,
-		      struct ib_mad_hdr *out, size_t *out_mad_size,
-		      u16 *out_mad_pkey_index);
-
-/*
- * Compare the lower 24 bits of the two values.
- * Returns an integer <, ==, or > than zero.
- */
-static inline int ipath_cmp24(u32 a, u32 b)
-{
-	return (((int) a) - ((int) b)) << 8;
-}
-
-struct ipath_mcast *ipath_mcast_find(union ib_gid *mgid);
-
-int ipath_snapshot_counters(struct ipath_devdata *dd, u64 *swords,
-			    u64 *rwords, u64 *spkts, u64 *rpkts,
-			    u64 *xmit_wait);
-
-int ipath_get_counters(struct ipath_devdata *dd,
-		       struct ipath_verbs_counters *cntrs);
-
-int ipath_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid);
-
-int ipath_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid);
-
-int ipath_mcast_tree_empty(void);
-
-__be32 ipath_compute_aeth(struct ipath_qp *qp);
-
-struct ipath_qp *ipath_lookup_qpn(struct ipath_qp_table *qpt, u32 qpn);
-
-struct ib_qp *ipath_create_qp(struct ib_pd *ibpd,
-			      struct ib_qp_init_attr *init_attr,
-			      struct ib_udata *udata);
-
-int ipath_destroy_qp(struct ib_qp *ibqp);
-
-int ipath_error_qp(struct ipath_qp *qp, enum ib_wc_status err);
-
-int ipath_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-		    int attr_mask, struct ib_udata *udata);
-
-int ipath_query_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
-		   int attr_mask, struct ib_qp_init_attr *init_attr);
-
-unsigned ipath_free_all_qps(struct ipath_qp_table *qpt);
-
-int ipath_init_qp_table(struct ipath_ibdev *idev, int size);
-
-void ipath_get_credit(struct ipath_qp *qp, u32 aeth);
-
-unsigned ipath_ib_rate_to_mult(enum ib_rate rate);
-
-int ipath_verbs_send(struct ipath_qp *qp, struct ipath_ib_header *hdr,
-		     u32 hdrwords, struct ipath_sge_state *ss, u32 len);
-
-void ipath_copy_sge(struct ipath_sge_state *ss, void *data, u32 length);
-
-void ipath_skip_sge(struct ipath_sge_state *ss, u32 length);
-
-void ipath_uc_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp);
-
-void ipath_rc_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp);
-
-void ipath_restart_rc(struct ipath_qp *qp, u32 psn);
-
-void ipath_rc_error(struct ipath_qp *qp, enum ib_wc_status err);
-
-int ipath_post_ud_send(struct ipath_qp *qp, struct ib_send_wr *wr);
-
-void ipath_ud_rcv(struct ipath_ibdev *dev, struct ipath_ib_header *hdr,
-		  int has_grh, void *data, u32 tlen, struct ipath_qp *qp);
-
-int ipath_alloc_lkey(struct ipath_lkey_table *rkt,
-		     struct ipath_mregion *mr);
-
-void ipath_free_lkey(struct ipath_lkey_table *rkt, u32 lkey);
-
-int ipath_lkey_ok(struct ipath_qp *qp, struct ipath_sge *isge,
-		  struct ib_sge *sge, int acc);
-
-int ipath_rkey_ok(struct ipath_qp *qp, struct ipath_sge_state *ss,
-		  u32 len, u64 vaddr, u32 rkey, int acc);
-
-int ipath_post_srq_receive(struct ib_srq *ibsrq, struct ib_recv_wr *wr,
-			   struct ib_recv_wr **bad_wr);
-
-struct ib_srq *ipath_create_srq(struct ib_pd *ibpd,
-				struct ib_srq_init_attr *srq_init_attr,
-				struct ib_udata *udata);
-
-int ipath_modify_srq(struct ib_srq *ibsrq, struct ib_srq_attr *attr,
-		     enum ib_srq_attr_mask attr_mask,
-		     struct ib_udata *udata);
-
-int ipath_query_srq(struct ib_srq *ibsrq, struct ib_srq_attr *attr);
-
-int ipath_destroy_srq(struct ib_srq *ibsrq);
-
-void ipath_cq_enter(struct ipath_cq *cq, struct ib_wc *entry, int sig);
-
-int ipath_poll_cq(struct ib_cq *ibcq, int num_entries, struct ib_wc *entry);
-
-struct ib_cq *ipath_create_cq(struct ib_device *ibdev,
-			      const struct ib_cq_init_attr *attr,
-			      struct ib_ucontext *context,
-			      struct ib_udata *udata);
-
-int ipath_destroy_cq(struct ib_cq *ibcq);
-
-int ipath_req_notify_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags notify_flags);
-
-int ipath_resize_cq(struct ib_cq *ibcq, int cqe, struct ib_udata *udata);
-
-struct ib_mr *ipath_get_dma_mr(struct ib_pd *pd, int acc);
-
-struct ib_mr *ipath_reg_phys_mr(struct ib_pd *pd,
-				struct ib_phys_buf *buffer_list,
-				int num_phys_buf, int acc, u64 *iova_start);
-
-struct ib_mr *ipath_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
-				u64 virt_addr, int mr_access_flags,
-				struct ib_udata *udata);
-
-int ipath_dereg_mr(struct ib_mr *ibmr);
-
-struct ib_fmr *ipath_alloc_fmr(struct ib_pd *pd, int mr_access_flags,
-			       struct ib_fmr_attr *fmr_attr);
-
-int ipath_map_phys_fmr(struct ib_fmr *ibfmr, u64 * page_list,
-		       int list_len, u64 iova);
-
-int ipath_unmap_fmr(struct list_head *fmr_list);
-
-int ipath_dealloc_fmr(struct ib_fmr *ibfmr);
-
-void ipath_release_mmap_info(struct kref *ref);
-
-struct ipath_mmap_info *ipath_create_mmap_info(struct ipath_ibdev *dev,
-					       u32 size,
-					       struct ib_ucontext *context,
-					       void *obj);
-
-void ipath_update_mmap_info(struct ipath_ibdev *dev,
-			    struct ipath_mmap_info *ip,
-			    u32 size, void *obj);
-
-int ipath_mmap(struct ib_ucontext *context, struct vm_area_struct *vma);
-
-void ipath_insert_rnr_queue(struct ipath_qp *qp);
-
-int ipath_init_sge(struct ipath_qp *qp, struct ipath_rwqe *wqe,
-		   u32 *lengthp, struct ipath_sge_state *ss);
-
-int ipath_get_rwqe(struct ipath_qp *qp, int wr_id_only);
-
-u32 ipath_make_grh(struct ipath_ibdev *dev, struct ib_grh *hdr,
-		   struct ib_global_route *grh, u32 hwords, u32 nwords);
-
-void ipath_make_ruc_header(struct ipath_ibdev *dev, struct ipath_qp *qp,
-			   struct ipath_other_headers *ohdr,
-			   u32 bth0, u32 bth2);
-
-void ipath_do_send(unsigned long data);
-
-void ipath_send_complete(struct ipath_qp *qp, struct ipath_swqe *wqe,
-			 enum ib_wc_status status);
-
-int ipath_make_rc_req(struct ipath_qp *qp);
-
-int ipath_make_uc_req(struct ipath_qp *qp);
-
-int ipath_make_ud_req(struct ipath_qp *qp);
-
-int ipath_register_ib_device(struct ipath_devdata *);
-
-void ipath_unregister_ib_device(struct ipath_ibdev *);
-
-void ipath_ib_rcv(struct ipath_ibdev *, void *, void *, u32);
-
-int ipath_ib_piobufavail(struct ipath_ibdev *);
-
-unsigned ipath_get_npkeys(struct ipath_devdata *);
-
-u32 ipath_get_cr_errpkey(struct ipath_devdata *);
-
-unsigned ipath_get_pkey(struct ipath_devdata *, unsigned);
-
-extern const enum ib_wc_opcode ib_ipath_wc_opcode[];
-
-/*
- * Below converts HCA-specific LinkTrainingState to IB PhysPortState
- * values.
- */
-extern const u8 ipath_cvt_physportstate[];
-#define IB_PHYSPORTSTATE_SLEEP 1
-#define IB_PHYSPORTSTATE_POLL 2
-#define IB_PHYSPORTSTATE_DISABLED 3
-#define IB_PHYSPORTSTATE_CFG_TRAIN 4
-#define IB_PHYSPORTSTATE_LINKUP 5
-#define IB_PHYSPORTSTATE_LINK_ERR_RECOVER 6
-
-extern const int ib_ipath_state_ops[];
-
-extern unsigned int ib_ipath_lkey_table_size;
-
-extern unsigned int ib_ipath_max_cqes;
-
-extern unsigned int ib_ipath_max_cqs;
-
-extern unsigned int ib_ipath_max_qp_wrs;
-
-extern unsigned int ib_ipath_max_qps;
-
-extern unsigned int ib_ipath_max_sges;
-
-extern unsigned int ib_ipath_max_mcast_grps;
-
-extern unsigned int ib_ipath_max_mcast_qp_attached;
-
-extern unsigned int ib_ipath_max_srqs;
-
-extern unsigned int ib_ipath_max_srq_sges;
-
-extern unsigned int ib_ipath_max_srq_wrs;
-
-extern const u32 ib_ipath_rnr_table[];
-
-extern struct ib_dma_mapping_ops ipath_dma_mapping_ops;
-
-#endif				/* IPATH_VERBS_H */
diff --git a/drivers/staging/rdma/ipath/ipath_verbs_mcast.c b/drivers/staging/rdma/ipath/ipath_verbs_mcast.c
deleted file mode 100644
index 72d476fa5b8f..000000000000
--- a/drivers/staging/rdma/ipath/ipath_verbs_mcast.c
+++ /dev/null
@@ -1,363 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2005, 2006 PathScale, 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/rculist.h>
-#include <linux/slab.h>
-
-#include "ipath_verbs.h"
-
-/*
- * Global table of GID to attached QPs.
- * The table is global to all ipath devices since a send from one QP/device
- * needs to be locally routed to any locally attached QPs on the same
- * or different device.
- */
-static struct rb_root mcast_tree;
-static DEFINE_SPINLOCK(mcast_lock);
-
-/**
- * ipath_mcast_qp_alloc - alloc a struct to link a QP to mcast GID struct
- * @qp: the QP to link
- */
-static struct ipath_mcast_qp *ipath_mcast_qp_alloc(struct ipath_qp *qp)
-{
-	struct ipath_mcast_qp *mqp;
-
-	mqp = kmalloc(sizeof *mqp, GFP_KERNEL);
-	if (!mqp)
-		goto bail;
-
-	mqp->qp = qp;
-	atomic_inc(&qp->refcount);
-
-bail:
-	return mqp;
-}
-
-static void ipath_mcast_qp_free(struct ipath_mcast_qp *mqp)
-{
-	struct ipath_qp *qp = mqp->qp;
-
-	/* Notify ipath_destroy_qp() if it is waiting. */
-	if (atomic_dec_and_test(&qp->refcount))
-		wake_up(&qp->wait);
-
-	kfree(mqp);
-}
-
-/**
- * ipath_mcast_alloc - allocate the multicast GID structure
- * @mgid: the multicast GID
- *
- * A list of QPs will be attached to this structure.
- */
-static struct ipath_mcast *ipath_mcast_alloc(union ib_gid *mgid)
-{
-	struct ipath_mcast *mcast;
-
-	mcast = kmalloc(sizeof *mcast, GFP_KERNEL);
-	if (!mcast)
-		goto bail;
-
-	mcast->mgid = *mgid;
-	INIT_LIST_HEAD(&mcast->qp_list);
-	init_waitqueue_head(&mcast->wait);
-	atomic_set(&mcast->refcount, 0);
-	mcast->n_attached = 0;
-
-bail:
-	return mcast;
-}
-
-static void ipath_mcast_free(struct ipath_mcast *mcast)
-{
-	struct ipath_mcast_qp *p, *tmp;
-
-	list_for_each_entry_safe(p, tmp, &mcast->qp_list, list)
-		ipath_mcast_qp_free(p);
-
-	kfree(mcast);
-}
-
-/**
- * ipath_mcast_find - search the global table for the given multicast GID
- * @mgid: the multicast GID to search for
- *
- * Returns NULL if not found.
- *
- * The caller is responsible for decrementing the reference count if found.
- */
-struct ipath_mcast *ipath_mcast_find(union ib_gid *mgid)
-{
-	struct rb_node *n;
-	unsigned long flags;
-	struct ipath_mcast *mcast;
-
-	spin_lock_irqsave(&mcast_lock, flags);
-	n = mcast_tree.rb_node;
-	while (n) {
-		int ret;
-
-		mcast = rb_entry(n, struct ipath_mcast, rb_node);
-
-		ret = memcmp(mgid->raw, mcast->mgid.raw,
-			     sizeof(union ib_gid));
-		if (ret < 0)
-			n = n->rb_left;
-		else if (ret > 0)
-			n = n->rb_right;
-		else {
-			atomic_inc(&mcast->refcount);
-			spin_unlock_irqrestore(&mcast_lock, flags);
-			goto bail;
-		}
-	}
-	spin_unlock_irqrestore(&mcast_lock, flags);
-
-	mcast = NULL;
-
-bail:
-	return mcast;
-}
-
-/**
- * ipath_mcast_add - insert mcast GID into table and attach QP struct
- * @mcast: the mcast GID table
- * @mqp: the QP to attach
- *
- * Return zero if both were added.  Return EEXIST if the GID was already in
- * the table but the QP was added.  Return ESRCH if the QP was already
- * attached and neither structure was added.
- */
-static int ipath_mcast_add(struct ipath_ibdev *dev,
-			   struct ipath_mcast *mcast,
-			   struct ipath_mcast_qp *mqp)
-{
-	struct rb_node **n = &mcast_tree.rb_node;
-	struct rb_node *pn = NULL;
-	int ret;
-
-	spin_lock_irq(&mcast_lock);
-
-	while (*n) {
-		struct ipath_mcast *tmcast;
-		struct ipath_mcast_qp *p;
-
-		pn = *n;
-		tmcast = rb_entry(pn, struct ipath_mcast, rb_node);
-
-		ret = memcmp(mcast->mgid.raw, tmcast->mgid.raw,
-			     sizeof(union ib_gid));
-		if (ret < 0) {
-			n = &pn->rb_left;
-			continue;
-		}
-		if (ret > 0) {
-			n = &pn->rb_right;
-			continue;
-		}
-
-		/* Search the QP list to see if this is already there. */
-		list_for_each_entry_rcu(p, &tmcast->qp_list, list) {
-			if (p->qp == mqp->qp) {
-				ret = ESRCH;
-				goto bail;
-			}
-		}
-		if (tmcast->n_attached == ib_ipath_max_mcast_qp_attached) {
-			ret = ENOMEM;
-			goto bail;
-		}
-
-		tmcast->n_attached++;
-
-		list_add_tail_rcu(&mqp->list, &tmcast->qp_list);
-		ret = EEXIST;
-		goto bail;
-	}
-
-	spin_lock(&dev->n_mcast_grps_lock);
-	if (dev->n_mcast_grps_allocated == ib_ipath_max_mcast_grps) {
-		spin_unlock(&dev->n_mcast_grps_lock);
-		ret = ENOMEM;
-		goto bail;
-	}
-
-	dev->n_mcast_grps_allocated++;
-	spin_unlock(&dev->n_mcast_grps_lock);
-
-	mcast->n_attached++;
-
-	list_add_tail_rcu(&mqp->list, &mcast->qp_list);
-
-	atomic_inc(&mcast->refcount);
-	rb_link_node(&mcast->rb_node, pn, n);
-	rb_insert_color(&mcast->rb_node, &mcast_tree);
-
-	ret = 0;
-
-bail:
-	spin_unlock_irq(&mcast_lock);
-
-	return ret;
-}
-
-int ipath_multicast_attach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-	struct ipath_ibdev *dev = to_idev(ibqp->device);
-	struct ipath_mcast *mcast;
-	struct ipath_mcast_qp *mqp;
-	int ret;
-
-	/*
-	 * Allocate data structures since its better to do this outside of
-	 * spin locks and it will most likely be needed.
-	 */
-	mcast = ipath_mcast_alloc(gid);
-	if (mcast == NULL) {
-		ret = -ENOMEM;
-		goto bail;
-	}
-	mqp = ipath_mcast_qp_alloc(qp);
-	if (mqp == NULL) {
-		ipath_mcast_free(mcast);
-		ret = -ENOMEM;
-		goto bail;
-	}
-	switch (ipath_mcast_add(dev, mcast, mqp)) {
-	case ESRCH:
-		/* Neither was used: can't attach the same QP twice. */
-		ipath_mcast_qp_free(mqp);
-		ipath_mcast_free(mcast);
-		ret = -EINVAL;
-		goto bail;
-	case EEXIST:		/* The mcast wasn't used */
-		ipath_mcast_free(mcast);
-		break;
-	case ENOMEM:
-		/* Exceeded the maximum number of mcast groups. */
-		ipath_mcast_qp_free(mqp);
-		ipath_mcast_free(mcast);
-		ret = -ENOMEM;
-		goto bail;
-	default:
-		break;
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-int ipath_multicast_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
-{
-	struct ipath_qp *qp = to_iqp(ibqp);
-	struct ipath_ibdev *dev = to_idev(ibqp->device);
-	struct ipath_mcast *mcast = NULL;
-	struct ipath_mcast_qp *p, *tmp;
-	struct rb_node *n;
-	int last = 0;
-	int ret;
-
-	spin_lock_irq(&mcast_lock);
-
-	/* Find the GID in the mcast table. */
-	n = mcast_tree.rb_node;
-	while (1) {
-		if (n == NULL) {
-			spin_unlock_irq(&mcast_lock);
-			ret = -EINVAL;
-			goto bail;
-		}
-
-		mcast = rb_entry(n, struct ipath_mcast, rb_node);
-		ret = memcmp(gid->raw, mcast->mgid.raw,
-			     sizeof(union ib_gid));
-		if (ret < 0)
-			n = n->rb_left;
-		else if (ret > 0)
-			n = n->rb_right;
-		else
-			break;
-	}
-
-	/* Search the QP list. */
-	list_for_each_entry_safe(p, tmp, &mcast->qp_list, list) {
-		if (p->qp != qp)
-			continue;
-		/*
-		 * We found it, so remove it, but don't poison the forward
-		 * link until we are sure there are no list walkers.
-		 */
-		list_del_rcu(&p->list);
-		mcast->n_attached--;
-
-		/* If this was the last attached QP, remove the GID too. */
-		if (list_empty(&mcast->qp_list)) {
-			rb_erase(&mcast->rb_node, &mcast_tree);
-			last = 1;
-		}
-		break;
-	}
-
-	spin_unlock_irq(&mcast_lock);
-
-	if (p) {
-		/*
-		 * Wait for any list walkers to finish before freeing the
-		 * list element.
-		 */
-		wait_event(mcast->wait, atomic_read(&mcast->refcount) <= 1);
-		ipath_mcast_qp_free(p);
-	}
-	if (last) {
-		atomic_dec(&mcast->refcount);
-		wait_event(mcast->wait, !atomic_read(&mcast->refcount));
-		ipath_mcast_free(mcast);
-		spin_lock_irq(&dev->n_mcast_grps_lock);
-		dev->n_mcast_grps_allocated--;
-		spin_unlock_irq(&dev->n_mcast_grps_lock);
-	}
-
-	ret = 0;
-
-bail:
-	return ret;
-}
-
-int ipath_mcast_tree_empty(void)
-{
-	return mcast_tree.rb_node == NULL;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_wc_ppc64.c b/drivers/staging/rdma/ipath/ipath_wc_ppc64.c
deleted file mode 100644
index 1a7e20a75149..000000000000
--- a/drivers/staging/rdma/ipath/ipath_wc_ppc64.c
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- *
- * This software is available to you under a choice of one of two
- * licenses.  You may choose to be licensed under the terms of the GNU
- * General Public License (GPL) Version 2, available from the file
- * COPYING in the main directory of this source tree, or the
- * OpenIB.org BSD license below:
- *
- *     Redistribution and use in source and binary forms, with or
- *     without modification, are permitted provided that the following
- *     conditions are met:
- *
- *      - Redistributions of source code must retain the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer.
- *
- *      - Redistributions in binary form must reproduce the above
- *        copyright notice, this list of conditions and the following
- *        disclaimer in the documentation and/or other materials
- *        provided with the distribution.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
- * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
- * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
- * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
- * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-/*
- * This file is conditionally built on PowerPC only.  Otherwise weak symbol
- * versions of the functions exported from here are used.
- */
-
-#include "ipath_kernel.h"
-
-/**
- * ipath_enable_wc - enable write combining for MMIO writes to the device
- * @dd: infinipath device
- *
- * Nothing to do on PowerPC, so just return without error.
- */
-int ipath_enable_wc(struct ipath_devdata *dd)
-{
-	return 0;
-}
diff --git a/drivers/staging/rdma/ipath/ipath_wc_x86_64.c b/drivers/staging/rdma/ipath/ipath_wc_x86_64.c
deleted file mode 100644
index 7b6e4c843e19..000000000000
--- a/drivers/staging/rdma/ipath/ipath_wc_x86_64.c
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- * Copyright (c) 2006, 2007 QLogic Corporation. All rights reserved.
- * Copyright (c) 2003, 2004, 2005, 2006 PathScale, 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.
- */
-
-/*
- * This file is conditionally built on x86_64 only.  Otherwise weak symbol
- * versions of the functions exported from here are used.
- */
-
-#include <linux/pci.h>
-#include <asm/processor.h>
-
-#include "ipath_kernel.h"
-
-/**
- * ipath_enable_wc - enable write combining for MMIO writes to the device
- * @dd: infinipath device
- *
- * This routine is x86_64-specific; it twiddles the CPU's MTRRs to enable
- * write combining.
- */
-int ipath_enable_wc(struct ipath_devdata *dd)
-{
-	int ret = 0;
-	u64 pioaddr, piolen;
-	unsigned bits;
-	const unsigned long addr = pci_resource_start(dd->pcidev, 0);
-	const size_t len = pci_resource_len(dd->pcidev, 0);
-
-	/*
-	 * Set the PIO buffers to be WCCOMB, so we get HT bursts to the
-	 * chip.  Linux (possibly the hardware) requires it to be on a power
-	 * of 2 address matching the length (which has to be a power of 2).
-	 * For rev1, that means the base address, for rev2, it will be just
-	 * the PIO buffers themselves.
-	 * For chips with two sets of buffers, the calculations are
-	 * somewhat more complicated; we need to sum, and the piobufbase
-	 * register has both offsets, 2K in low 32 bits, 4K in high 32 bits.
-	 * The buffers are still packed, so a single range covers both.
-	 */
-	if (dd->ipath_piobcnt2k && dd->ipath_piobcnt4k) { /* 2 sizes */
-		unsigned long pio2kbase, pio4kbase;
-		pio2kbase = dd->ipath_piobufbase & 0xffffffffUL;
-		pio4kbase = (dd->ipath_piobufbase >> 32) & 0xffffffffUL;
-		if (pio2kbase < pio4kbase) { /* all, for now */
-			pioaddr = addr + pio2kbase;
-			piolen = pio4kbase - pio2kbase +
-				dd->ipath_piobcnt4k * dd->ipath_4kalign;
-		} else {
-			pioaddr = addr + pio4kbase;
-			piolen = pio2kbase - pio4kbase +
-				dd->ipath_piobcnt2k * dd->ipath_palign;
-		}
-	} else {  /* single buffer size (2K, currently) */
-		pioaddr = addr + dd->ipath_piobufbase;
-		piolen = dd->ipath_piobcnt2k * dd->ipath_palign +
-			dd->ipath_piobcnt4k * dd->ipath_4kalign;
-	}
-
-	for (bits = 0; !(piolen & (1ULL << bits)); bits++)
-		/* do nothing */ ;
-
-	if (piolen != (1ULL << bits)) {
-		piolen >>= bits;
-		while (piolen >>= 1)
-			bits++;
-		piolen = 1ULL << (bits + 1);
-	}
-	if (pioaddr & (piolen - 1)) {
-		u64 atmp;
-		ipath_dbg("pioaddr %llx not on right boundary for size "
-			  "%llx, fixing\n",
-			  (unsigned long long) pioaddr,
-			  (unsigned long long) piolen);
-		atmp = pioaddr & ~(piolen - 1);
-		if (atmp < addr || (atmp + piolen) > (addr + len)) {
-			ipath_dev_err(dd, "No way to align address/size "
-				      "(%llx/%llx), no WC mtrr\n",
-				      (unsigned long long) atmp,
-				      (unsigned long long) piolen << 1);
-			ret = -ENODEV;
-		} else {
-			ipath_dbg("changing WC base from %llx to %llx, "
-				  "len from %llx to %llx\n",
-				  (unsigned long long) pioaddr,
-				  (unsigned long long) atmp,
-				  (unsigned long long) piolen,
-				  (unsigned long long) piolen << 1);
-			pioaddr = atmp;
-			piolen <<= 1;
-		}
-	}
-
-	if (!ret) {
-		dd->wc_cookie = arch_phys_wc_add(pioaddr, piolen);
-		if (dd->wc_cookie < 0) {
-			ipath_dev_err(dd, "Seting mtrr failed on PIO buffers\n");
-			ret = -ENODEV;
-		} else if (dd->wc_cookie == 0)
-			ipath_cdbg(VERBOSE, "Set mtrr for chip to WC not needed\n");
-		else
-			ipath_cdbg(VERBOSE, "Set mtrr for chip to WC\n");
-	}
-
-	return ret;
-}
-
-/**
- * ipath_disable_wc - disable write combining for MMIO writes to the device
- * @dd: infinipath device
- */
-void ipath_disable_wc(struct ipath_devdata *dd)
-{
-	arch_phys_wc_del(dd->wc_cookie);
-}
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index 3cdb40fea5ee..e5d29fe9d446 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -1240,11 +1240,6 @@ int rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
 	return 0;
 }
 
-static void update_bcn_fixed_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
 static void update_bcn_erpinfo_ie(struct adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
@@ -1279,31 +1274,6 @@ static void update_bcn_erpinfo_ie(struct adapter *padapter)
 	}
 }
 
-static void update_bcn_htcap_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
-static void update_bcn_htinfo_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
-static void update_bcn_rsn_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
-static void update_bcn_wpa_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
-static void update_bcn_wmm_ie(struct adapter *padapter)
-{
-	DBG_88E("%s\n", __func__);
-}
-
 static void update_bcn_wps_ie(struct adapter *padapter)
 {
 	u8 *pwps_ie = NULL, *pwps_ie_src;
@@ -1354,22 +1324,12 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 	kfree(pbackup_remainder_ie);
 }
 
-static void update_bcn_p2p_ie(struct adapter *padapter)
-{
-}
-
 static void update_bcn_vendor_spec_ie(struct adapter *padapter, u8 *oui)
 {
 	DBG_88E("%s\n", __func__);
 
-	if (!memcmp(RTW_WPA_OUI, oui, 4))
-		update_bcn_wpa_ie(padapter);
-	else if (!memcmp(WMM_OUI, oui, 4))
-		update_bcn_wmm_ie(padapter);
-	else if (!memcmp(WPS_OUI, oui, 4))
+	if (!memcmp(WPS_OUI, oui, 4))
 		update_bcn_wps_ie(padapter);
-	else if (!memcmp(P2P_OUI, oui, 4))
-		update_bcn_p2p_ie(padapter);
 	else
 		DBG_88E("unknown OUI type!\n");
 }
@@ -1391,24 +1351,12 @@ void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
 	spin_lock_bh(&pmlmepriv->bcn_update_lock);
 
 	switch (ie_id) {
-	case 0xFF:
-		update_bcn_fixed_ie(padapter);/* 8: TimeStamp, 2: Beacon Interval 2:Capability */
-		break;
 	case _TIM_IE_:
 		update_BCNTIM(padapter);
 		break;
 	case _ERPINFO_IE_:
 		update_bcn_erpinfo_ie(padapter);
 		break;
-	case _HT_CAPABILITY_IE_:
-		update_bcn_htcap_ie(padapter);
-		break;
-	case _RSN_IE_2_:
-		update_bcn_rsn_ie(padapter);
-		break;
-	case _HT_ADD_INFO_IE_:
-		update_bcn_htinfo_ie(padapter);
-		break;
 	case _VENDOR_SPECIFIC_IE_:
 		update_bcn_vendor_spec_ie(padapter, oui);
 		break;
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index 9b7026e7d55b..433b926ceae7 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -201,23 +201,20 @@ _next:
 
 		if (rtw_cmd_filter(pcmdpriv, pcmd) == _FAIL) {
 			pcmd->res = H2C_DROPPED;
-			goto post_process;
-		}
-
-		if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
-			cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
-
-			if (cmd_hdl) {
-				ret = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
-				pcmd->res = ret;
-			}
 		} else {
-			pcmd->res = H2C_PARAMETERS_ERROR;
-		}
+			if (pcmd->cmdcode < ARRAY_SIZE(wlancmds)) {
+			    cmd_hdl = wlancmds[pcmd->cmdcode].h2cfuns;
 
-		cmd_hdl = NULL;
+				if (cmd_hdl) {
+					ret = cmd_hdl(pcmd->padapter, pcmd->parmbuf);
+					pcmd->res = ret;
+				}
+			} else {
+				pcmd->res = H2C_PARAMETERS_ERROR;
+			}
 
-post_process:
+			cmd_hdl = NULL;
+		}
 
 		/* call callback function for post-processed */
 		if (pcmd->cmdcode < ARRAY_SIZE(rtw_cmd_callback)) {
@@ -242,15 +239,11 @@ post_process:
 	pcmdpriv->cmdthd_running = false;
 
 	/*  free all cmd_obj resources */
-	do {
-		pcmd = rtw_dequeue_cmd(&pcmdpriv->cmd_queue);
-		if (pcmd == NULL)
-			break;
-
+	while ((pcmd = rtw_dequeue_cmd(&pcmdpriv->cmd_queue))) {
 		/* DBG_88E("%s: leaving... drop cmdcode:%u\n", __func__, pcmd->cmdcode); */
 
 		rtw_free_cmd_obj(pcmd);
-	} while (1);
+	}
 
 	up(&pcmdpriv->terminate_cmdthread_sema);
 
@@ -570,31 +563,19 @@ u8 rtw_setopmode_cmd(struct adapter  *padapter, enum ndis_802_11_network_infra n
 	struct	setopmode_parm *psetop;
 
 	struct	cmd_priv   *pcmdpriv = &padapter->cmdpriv;
-	u8	res = _SUCCESS;
-
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
-	if (ph2c == NULL) {
-		res = false;
-		goto exit;
-	}
 	psetop = kzalloc(sizeof(struct setopmode_parm), GFP_KERNEL);
-
-	if (psetop == NULL) {
+	if (!ph2c || !psetop) {
 		kfree(ph2c);
-		res = false;
-		goto exit;
+		kfree(psetop);
+		return false;
 	}
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetop, _SetOpMode_CMD_);
 	psetop->mode = (u8)networktype;
 
-	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
-
-exit:
-
-
-	return res;
+	return rtw_enqueue_cmd(pcmdpriv, ph2c);
 }
 
 u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
@@ -607,28 +588,16 @@ u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_info *sta = (struct sta_info *)psta;
-	u8	res = _SUCCESS;
-
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
-	if (ph2c == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
 	psetstakey_para = kzalloc(sizeof(struct set_stakey_parm), GFP_KERNEL);
-	if (psetstakey_para == NULL) {
-		kfree(ph2c);
-		res = _FAIL;
-		goto exit;
-	}
-
 	psetstakey_rsp = kzalloc(sizeof(struct set_stakey_rsp), GFP_KERNEL);
-	if (psetstakey_rsp == NULL) {
+
+	if (!ph2c || !psetstakey_para || !psetstakey_rsp) {
 		kfree(ph2c);
 		kfree(psetstakey_para);
-		res = _FAIL;
-		goto exit;
+		kfree(psetstakey_rsp);
+		return _FAIL;
 	}
 
 	init_h2fwcmd_w_parm_no_rsp(ph2c, psetstakey_para, _SetStaKey_CMD_);
@@ -650,12 +619,7 @@ u8 rtw_setstakey_cmd(struct adapter *padapter, u8 *psta, u8 unicast_key)
 	/* jeff: set this because at least sw key is ready */
 	padapter->securitypriv.busetkipkey = true;
 
-	res = rtw_enqueue_cmd(pcmdpriv, ph2c);
-
-exit:
-
-
-	return res;
+	return rtw_enqueue_cmd(pcmdpriv, ph2c);
 }
 
 u8 rtw_clearstakey_cmd(struct adapter *padapter, u8 *psta, u8 entry, u8 enqueue)
@@ -1086,31 +1050,19 @@ u8 rtw_ps_cmd(struct adapter *padapter)
 	struct drvextra_cmd_parm	*pdrvextra_cmd_parm;
 	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 
-	u8	res = _SUCCESS;
-
 	ppscmd = kzalloc(sizeof(struct cmd_obj), GFP_ATOMIC);
-	if (ppscmd == NULL) {
-		res = _FAIL;
-		goto exit;
-	}
-
 	pdrvextra_cmd_parm = kzalloc(sizeof(struct drvextra_cmd_parm), GFP_ATOMIC);
-	if (pdrvextra_cmd_parm == NULL) {
+	if (!ppscmd || !pdrvextra_cmd_parm) {
 		kfree(ppscmd);
-		res = _FAIL;
-		goto exit;
+		kfree(pdrvextra_cmd_parm);
+		return _FAIL;
 	}
 
 	pdrvextra_cmd_parm->ec_id = POWER_SAVING_CTRL_WK_CID;
 	pdrvextra_cmd_parm->pbuf = NULL;
 	init_h2fwcmd_w_parm_no_rsp(ppscmd, pdrvextra_cmd_parm, GEN_CMD_CODE(_Set_Drv_Extra));
 
-	res = rtw_enqueue_cmd(pcmdpriv, ppscmd);
-
-exit:
-
-
-	return res;
+	return rtw_enqueue_cmd(pcmdpriv, ppscmd);
 }
 
 #ifdef CONFIG_88EU_AP_MODE
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index eb894233a785..2320fb11af24 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -224,7 +224,7 @@ static void efuse_read_phymap_from_txpktbuf(
 	)
 {
 	u16 dbg_addr = 0;
-	u32 start  = 0, passing_time = 0;
+	unsigned long start = 0;
 	u8 reg_0x143 = 0;
 	u32 lo32 = 0, hi32 = 0;
 	u16 len = 0, count = 0;
@@ -248,7 +248,7 @@ static void efuse_read_phymap_from_txpktbuf(
 		usb_write8(adapter, REG_TXPKTBUF_DBG, 0);
 		start = jiffies;
 		while (!(reg_0x143 = usb_read8(adapter, REG_TXPKTBUF_DBG)) &&
-		       (passing_time = rtw_get_passing_time_ms(start)) < 1000) {
+		       jiffies_to_msecs(jiffies - start) < 1000) {
 			DBG_88E("%s polling reg_0x143:0x%02x, reg_0x106:0x%02x\n", __func__, reg_0x143, usb_read8(adapter, 0x106));
 			usleep_range(1000, 2000);
 		}
diff --git a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
index 22f5b45f5f7f..cf60717a6c19 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ioctl_set.c
@@ -27,13 +27,6 @@
 
 extern void indicate_wx_scan_complete_event(struct adapter *padapter);
 
-#define IS_MAC_ADDRESS_BROADCAST(addr) \
-(\
-	((addr[0] == 0xff) && (addr[1] == 0xff) && \
-		(addr[2] == 0xff) && (addr[3] == 0xff) && \
-		(addr[4] == 0xff) && (addr[5] == 0xff))  ? true : false \
-)
-
 u8 rtw_do_join(struct adapter *padapter)
 {
 	struct list_head *plist, *phead;
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme.c b/drivers/staging/rtl8188eu/core/rtw_mlme.c
index c1b82f71b682..abab854e6889 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme.c
@@ -163,7 +163,8 @@ exit:
 
 static void _rtw_free_network(struct mlme_priv *pmlmepriv, struct wlan_network *pnetwork, u8 isfreeall)
 {
-	u32 curr_time, delta_time;
+	unsigned long curr_time;
+	u32 delta_time;
 	u32 lifetime = SCANQUEUE_LIFETIME;
 	struct __queue *free_queue = &(pmlmepriv->free_bss_pool);
 
@@ -272,7 +273,7 @@ int rtw_if_up(struct adapter *padapter)
 
 void rtw_generate_random_ibss(u8 *pibss)
 {
-	u32	curtime = jiffies;
+	unsigned long curtime = jiffies;
 
 	pibss[0] = 0x02;  /* in ad-hoc mode bit1 must set to 1 */
 	pibss[1] = 0x11;
@@ -365,20 +366,13 @@ struct	wlan_network	*rtw_get_oldest_wlan_network(struct __queue *scanned_queue)
 
 	phead = get_list_head(scanned_queue);
 
-	plist = phead->next;
-
-	while (1) {
-		if (phead == plist)
-			break;
-
+	for (plist = phead->next; plist != phead; plist = plist->next) {
 		pwlan = container_of(plist, struct wlan_network, list);
 
 		if (!pwlan->fixed) {
 			if (oldest == NULL || time_after(oldest->last_scanned, pwlan->last_scanned))
 				oldest = pwlan;
 		}
-
-		plist = plist->next;
 	}
 	return oldest;
 }
@@ -878,14 +872,14 @@ inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
 
 void rtw_scan_abort(struct adapter *adapter)
 {
-	u32 start;
+	unsigned long start;
 	struct mlme_priv	*pmlmepriv = &(adapter->mlmepriv);
 	struct mlme_ext_priv	*pmlmeext = &(adapter->mlmeextpriv);
 
 	start = jiffies;
 	pmlmeext->scan_abort = true;
 	while (check_fwstate(pmlmepriv, _FW_UNDER_SURVEY) &&
-	       rtw_get_passing_time_ms(start) <= 200) {
+	       jiffies_to_msecs(jiffies - start) <= 200) {
 		if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
 			break;
 		DBG_88E(FUNC_NDEV_FMT"fw_state=_FW_UNDER_SURVEY!\n", FUNC_NDEV_ARG(adapter->pnetdev));
@@ -1474,6 +1468,7 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 	, struct wlan_network **candidate, struct wlan_network *competitor)
 {
 	int updated = false;
+	unsigned long since_scan;
 	struct adapter *adapter = container_of(pmlmepriv, struct adapter, mlmepriv);
 
 
@@ -1494,7 +1489,8 @@ static int rtw_check_join_candidate(struct mlme_priv *pmlmepriv
 		goto exit;
 
 	if (pmlmepriv->to_roaming) {
-		if (rtw_get_passing_time_ms((u32)competitor->last_scanned) >= RTW_SCAN_RESULT_EXPIRE ||
+		since_scan = jiffies - competitor->last_scanned;
+		if (jiffies_to_msecs(since_scan) >= RTW_SCAN_RESULT_EXPIRE ||
 		    is_same_ess(&competitor->network, &pmlmepriv->cur_network.network) == false)
 			goto exit;
 	}
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index d900546b672f..3eca6874b6df 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -609,7 +609,7 @@ static void issue_probersp(struct adapter *padapter, unsigned char *da)
 	return;
 }
 
-static int _issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, int wait_ack)
+static int issue_probereq(struct adapter *padapter, struct ndis_802_11_ssid *pssid, u8 *da, bool wait_ack)
 {
 	int ret = _FAIL;
 	struct xmit_frame		*pmgntframe;
@@ -702,22 +702,16 @@ exit:
 	return ret;
 }
 
-static inline void issue_probereq(struct adapter *padapter,
-				  struct ndis_802_11_ssid *pssid, u8 *da)
-{
-	_issue_probereq(padapter, pssid, da, false);
-}
-
 static int issue_probereq_ex(struct adapter *padapter,
 			     struct ndis_802_11_ssid *pssid, u8 *da,
 			     int try_cnt, int wait_ms)
 {
 	int ret;
 	int i = 0;
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 
 	do {
-		ret = _issue_probereq(padapter, pssid, da, wait_ms > 0 ? true : false);
+		ret = issue_probereq(padapter, pssid, da, wait_ms > 0 ? true : false);
 
 		i++;
 
@@ -738,11 +732,13 @@ static int issue_probereq_ex(struct adapter *padapter,
 		if (da)
 			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 		else
 			DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 	}
 exit:
 	return ret;
@@ -1299,7 +1295,7 @@ int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int pow
 {
 	int ret;
 	int i = 0;
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
 	struct wlan_bssid_ex    *pnetwork = &(pmlmeinfo->network);
@@ -1329,11 +1325,13 @@ int issue_nulldata(struct adapter *padapter, unsigned char *da, unsigned int pow
 		if (da)
 			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 		else
 			DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 	}
 exit:
 	return ret;
@@ -1424,7 +1422,7 @@ int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int
 {
 	int ret;
 	int i = 0;
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 	struct mlme_ext_priv	*pmlmeext = &(padapter->mlmeextpriv);
 	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
 	struct wlan_bssid_ex    *pnetwork = &(pmlmeinfo->network);
@@ -1454,11 +1452,13 @@ int issue_qos_nulldata(struct adapter *padapter, unsigned char *da, u16 tid, int
 		if (da)
 			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 		else
 			DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 	}
 exit:
 	return ret;
@@ -1536,7 +1536,7 @@ static int issue_deauth_ex(struct adapter *padapter, u8 *da,
 {
 	int ret;
 	int i = 0;
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 
 	do {
 		ret = _issue_deauth(padapter, da, reason, wait_ms > 0 ? true : false);
@@ -1559,11 +1559,13 @@ static int issue_deauth_ex(struct adapter *padapter, u8 *da,
 		if (da)
 			DBG_88E(FUNC_ADPT_FMT" to %pM, ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), da, rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 		else
 			DBG_88E(FUNC_ADPT_FMT", ch:%u%s, %d/%d in %u ms\n",
 				FUNC_ADPT_ARG(padapter), rtw_get_oper_ch(padapter),
-				ret == _SUCCESS ? ", acked" : "", i, try_cnt, rtw_get_passing_time_ms(start));
+				ret == _SUCCESS ? ", acked" : "", i, try_cnt,
+				jiffies_to_msecs(jiffies - start));
 	}
 exit:
 	return ret;
@@ -1965,7 +1967,7 @@ unsigned int send_beacon(struct adapter *padapter)
 	int	issue = 0;
 	int poll = 0;
 
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 
 	rtw_hal_set_hwreg(padapter, HW_VAR_BCN_VALID, NULL);
 	do {
@@ -1981,13 +1983,16 @@ unsigned int send_beacon(struct adapter *padapter)
 	if (padapter->bSurpriseRemoved || padapter->bDriverStopped)
 		return _FAIL;
 	if (!bxmitok) {
-		DBG_88E("%s fail! %u ms\n", __func__, rtw_get_passing_time_ms(start));
+		DBG_88E("%s fail! %u ms\n", __func__,
+			jiffies_to_msecs(jiffies - start));
 		return _FAIL;
 	} else {
-		u32 passing_time = rtw_get_passing_time_ms(start);
+		u32 passing_time = jiffies_to_msecs(jiffies - start);
 
 		if (passing_time > 100 || issue > 3)
-			DBG_88E("%s success, issue:%d, poll:%d, %u ms\n", __func__, issue, poll, rtw_get_passing_time_ms(start));
+			DBG_88E("%s success, issue:%d, poll:%d, %u ms\n",
+				__func__, issue, poll,
+				jiffies_to_msecs(jiffies - start));
 		return _SUCCESS;
 	}
 }
@@ -2029,24 +2034,28 @@ static void site_survey(struct adapter *padapter)
 			for (i = 0; i < RTW_SSID_SCAN_AMOUNT; i++) {
 				if (pmlmeext->sitesurvey_res.ssid[i].SsidLength) {
 					/* todo: to issue two probe req??? */
-					issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+					issue_probereq(padapter,
+					&(pmlmeext->sitesurvey_res.ssid[i]),
+								NULL, false);
 					/* msleep(SURVEY_TO>>1); */
-					issue_probereq(padapter, &(pmlmeext->sitesurvey_res.ssid[i]), NULL);
+					issue_probereq(padapter,
+					&(pmlmeext->sitesurvey_res.ssid[i]),
+								NULL, false);
 				}
 			}
 
 			if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
 				/* todo: to issue two probe req??? */
-				issue_probereq(padapter, NULL, NULL);
+				issue_probereq(padapter, NULL, NULL, false);
 				/* msleep(SURVEY_TO>>1); */
-				issue_probereq(padapter, NULL, NULL);
+				issue_probereq(padapter, NULL, NULL, false);
 			}
 
 			if (pmlmeext->sitesurvey_res.scan_mode == SCAN_ACTIVE) {
 				/* todo: to issue two probe req??? */
-				issue_probereq(padapter, NULL, NULL);
+				issue_probereq(padapter, NULL, NULL, false);
 				/* msleep(SURVEY_TO>>1); */
-				issue_probereq(padapter, NULL, NULL);
+				issue_probereq(padapter, NULL, NULL, false);
 			}
 		}
 
@@ -4820,9 +4829,18 @@ void linked_status_chk(struct adapter *padapter)
 			} else {
 				if (rx_chk != _SUCCESS) {
 					if (pmlmeext->retry == 0) {
-						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
-						issue_probereq(padapter, &pmlmeinfo->network.Ssid, pmlmeinfo->network.MacAddress);
+						issue_probereq(padapter,
+						&pmlmeinfo->network.Ssid,
+						pmlmeinfo->network.MacAddress,
+									false);
+						issue_probereq(padapter,
+						&pmlmeinfo->network.Ssid,
+						pmlmeinfo->network.MacAddress,
+									false);
+						issue_probereq(padapter,
+						&pmlmeinfo->network.Ssid,
+						pmlmeinfo->network.MacAddress,
+									false);
 					}
 				}
 
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index 9765946466ab..5e1ef9fdcf47 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -348,7 +348,7 @@ void rtw_set_rpwm(struct adapter *padapter, u8 pslv)
 
 static u8 PS_RDY_CHECK(struct adapter *padapter)
 {
-	u32 curr_time, delta_time;
+	unsigned long curr_time, delta_time;
 	struct pwrctrl_priv	*pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
 
@@ -418,7 +418,7 @@ void rtw_set_ps_mode(struct adapter *padapter, u8 ps_mode, u8 smart_ps, u8 bcn_a
  */
 s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
 {
-	u32 start_time;
+	unsigned long start_time;
 	u8 bAwake = false;
 	s32 err = 0;
 
@@ -435,7 +435,7 @@ s32 LPS_RF_ON_check(struct adapter *padapter, u32 delay_ms)
 			break;
 		}
 
-		if (rtw_get_passing_time_ms(start_time) > delay_ms) {
+		if (jiffies_to_msecs(jiffies - start_time) > delay_ms) {
 			err = -1;
 			DBG_88E("%s: Wait for FW LPS leave more than %u ms!!!\n", __func__, delay_ms);
 			break;
@@ -561,24 +561,24 @@ int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *cal
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	unsigned long expires;
+	unsigned long start;
 	int ret = _SUCCESS;
 
 	expires = jiffies + msecs_to_jiffies(ips_deffer_ms);
 	if (time_before(pwrpriv->ips_deny_time, expires))
 		pwrpriv->ips_deny_time = jiffies + msecs_to_jiffies(ips_deffer_ms);
 
-{
-	u32 start = jiffies;
+	start = jiffies;
 	if (pwrpriv->ps_processing) {
 		DBG_88E("%s wait ps_processing...\n", __func__);
-		while (pwrpriv->ps_processing && rtw_get_passing_time_ms(start) <= 3000)
+		while (pwrpriv->ps_processing &&
+		       jiffies_to_msecs(jiffies - start) <= 3000)
 			usleep_range(1000, 3000);
 		if (pwrpriv->ps_processing)
 			DBG_88E("%s wait ps_processing timeout\n", __func__);
 		else
 			DBG_88E("%s wait ps_processing done\n", __func__);
 	}
-}
 
 	/* System suspend is not allowed to wakeup */
 	if ((!pwrpriv->bInternalAutoSuspend) && (pwrpriv->bInSuspend)) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_xmit.c b/drivers/staging/rtl8188eu/core/rtw_xmit.c
index cabb810369bd..e778132b73dc 100644
--- a/drivers/staging/rtl8188eu/core/rtw_xmit.c
+++ b/drivers/staging/rtl8188eu/core/rtw_xmit.c
@@ -641,7 +641,7 @@ static s32 xmitframe_addmic(struct adapter *padapter, struct xmit_frame *pxmitfr
 	if (pattrib->psta)
 		stainfo = pattrib->psta;
 	else
-		stainfo = rtw_get_stainfo(&padapter->stapriv , &pattrib->ra[0]);
+		stainfo = rtw_get_stainfo(&padapter->stapriv, &pattrib->ra[0]);
 
 
 	hw_hdr_offset = TXDESC_SIZE + (pxmitframe->pkt_offset * PACKET_OFFSET_SZ);
@@ -1702,12 +1702,12 @@ u32 rtw_get_ff_hwaddr(struct xmit_frame *pxmitframe)
 	return addr;
 }
 
-static void do_queue_select(struct adapter	*padapter, struct pkt_attrib *pattrib)
+static void do_queue_select(struct adapter *padapter, struct pkt_attrib *pattrib)
 {
 	u8 qsel;
 
 	qsel = pattrib->priority;
-	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority=%d , qsel = %d\n", pattrib->priority , qsel));
+	RT_TRACE(_module_rtl871x_xmit_c_, _drv_info_, ("### do_queue_select priority=%d , qsel = %d\n", pattrib->priority, qsel));
 
 	pattrib->qsel = qsel;
 }
@@ -2186,11 +2186,6 @@ void rtw_sctx_done_err(struct submit_ctx **sctx, int status)
 	}
 }
 
-void rtw_sctx_done(struct submit_ctx **sctx)
-{
-	rtw_sctx_done_err(sctx, RTW_SCTX_DONE_SUCCESS);
-}
-
 int rtw_ack_tx_wait(struct xmit_priv *pxmitpriv, u32 timeout_ms)
 {
 	struct submit_ctx *pack_tx_ops = &pxmitpriv->ack_tx_ops;
diff --git a/drivers/staging/rtl8188eu/hal/fw.c b/drivers/staging/rtl8188eu/hal/fw.c
index 23aa6d37acac..4d72537644b3 100644
--- a/drivers/staging/rtl8188eu/hal/fw.c
+++ b/drivers/staging/rtl8188eu/hal/fw.c
@@ -58,43 +58,31 @@ static void _rtl88e_fw_block_write(struct adapter *adapt,
 				   const u8 *buffer, u32 size)
 {
 	u32 blk_sz = sizeof(u32);
-	u8 *buf_ptr = (u8 *)buffer;
-	u32 *pu4BytePtr = (u32 *)buffer;
-	u32 i, offset, blk_cnt, remain;
+	const u8 *byte_buffer;
+	const u32 *dword_buffer = (u32 *)buffer;
+	u32 i, write_address, blk_cnt, remain;
 
 	blk_cnt = size / blk_sz;
 	remain = size % blk_sz;
 
-	for (i = 0; i < blk_cnt; i++) {
-		offset = i * blk_sz;
-		usb_write32(adapt, (FW_8192C_START_ADDRESS + offset),
-				*(pu4BytePtr + i));
-	}
+	write_address = FW_8192C_START_ADDRESS;
 
-	if (remain) {
-		offset = blk_cnt * blk_sz;
-		buf_ptr += offset;
-		for (i = 0; i < remain; i++) {
-			usb_write8(adapt, (FW_8192C_START_ADDRESS +
-						 offset + i), *(buf_ptr + i));
-		}
-	}
+	for (i = 0; i < blk_cnt; i++, write_address += blk_sz)
+		usb_write32(adapt, write_address, dword_buffer[i]);
+
+	byte_buffer = buffer + blk_cnt * blk_sz;
+	for (i = 0; i < remain; i++, write_address++)
+		usb_write8(adapt, write_address, byte_buffer[i]);
 }
 
 static void _rtl88e_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 {
-	u32 fwlen = *pfwlen;
-	u8 remain = (u8)(fwlen % 4);
-
-	remain = (remain == 0) ? 0 : (4 - remain);
+	u32 i;
 
-	while (remain > 0) {
-		pfwbuf[fwlen] = 0;
-		fwlen++;
-		remain--;
-	}
+	for (i = *pfwlen; i < roundup(*pfwlen, 4); i++)
+		pfwbuf[i] = 0;
 
-	*pfwlen = fwlen;
+	*pfwlen = i;
 }
 
 static void _rtl88e_fw_page_write(struct adapter *adapt,
diff --git a/drivers/staging/rtl8188eu/hal/hal_com.c b/drivers/staging/rtl8188eu/hal/hal_com.c
index 38e9fdc312d3..3871cda2eec2 100644
--- a/drivers/staging/rtl8188eu/hal/hal_com.c
+++ b/drivers/staging/rtl8188eu/hal/hal_com.c
@@ -32,19 +32,19 @@ void dump_chip_info(struct HAL_VERSION	chip_vers)
 	char buf[128];
 
 	cnt += sprintf((buf+cnt), "Chip Version Info: CHIP_8188E_");
-	cnt += sprintf((buf+cnt), "%s_", IS_NORMAL_CHIP(chip_vers) ?
+	cnt += sprintf((buf+cnt), "%s_", chip_vers.ChipType == NORMAL_CHIP ?
 		       "Normal_Chip" : "Test_Chip");
-	cnt += sprintf((buf+cnt), "%s_", IS_CHIP_VENDOR_TSMC(chip_vers) ?
+	cnt += sprintf((buf+cnt), "%s_", chip_vers.VendorType == CHIP_VENDOR_TSMC ?
 		       "TSMC" : "UMC");
-	if (IS_A_CUT(chip_vers))
+	if (chip_vers.CUTVersion == A_CUT_VERSION)
 		cnt += sprintf((buf+cnt), "A_CUT_");
-	else if (IS_B_CUT(chip_vers))
+	else if (chip_vers.CUTVersion == B_CUT_VERSION)
 		cnt += sprintf((buf+cnt), "B_CUT_");
-	else if (IS_C_CUT(chip_vers))
+	else if (chip_vers.CUTVersion == C_CUT_VERSION)
 		cnt += sprintf((buf+cnt), "C_CUT_");
-	else if (IS_D_CUT(chip_vers))
+	else if (chip_vers.CUTVersion == D_CUT_VERSION)
 		cnt += sprintf((buf+cnt), "D_CUT_");
-	else if (IS_E_CUT(chip_vers))
+	else if (chip_vers.CUTVersion == E_CUT_VERSION)
 		cnt += sprintf((buf+cnt), "E_CUT_");
 	else
 		cnt += sprintf((buf+cnt), "UNKNOWN_CUT(%d)_",
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index fca590949409..199a77acd7a9 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -67,7 +67,7 @@ static void Init_ODM_ComInfo_88E(struct adapter *Adapter)
 	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_FAB_VER, fab_ver);
 	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_CUT_VER, cut_ver);
 
-	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_MP_TEST_CHIP, IS_NORMAL_CHIP(hal_data->VersionID));
+	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_MP_TEST_CHIP, hal_data->VersionID.ChipType == NORMAL_CHIP ? true : false);
 
 	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_PATCH_ID, hal_data->CustomerID);
 	ODM_CmnInfoInit(dm_odm, ODM_CMNINFO_BWIFI_TEST, Adapter->registrypriv.wifi_spec);
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index e3e5d6f5d4f9..2592bc298f84 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -53,7 +53,7 @@ s32 iol_execute(struct adapter *padapter, u8 control)
 {
 	s32 status = _FAIL;
 	u8 reg_0x88 = 0;
-	u32 start = 0, passing_time = 0;
+	unsigned long start = 0;
 
 	control = control&0x0f;
 	reg_0x88 = usb_read8(padapter, REG_HMEBOX_E0);
@@ -61,8 +61,8 @@ s32 iol_execute(struct adapter *padapter, u8 control)
 
 	start = jiffies;
 	while ((reg_0x88 = usb_read8(padapter, REG_HMEBOX_E0)) & control &&
-	       (passing_time = rtw_get_passing_time_ms(start)) < 1000) {
-		;
+	       jiffies_to_msecs(jiffies - start) < 1000) {
+		udelay(5);
 	}
 
 	reg_0x88 = usb_read8(padapter, REG_HMEBOX_E0);
@@ -242,6 +242,7 @@ static s32 _LLTWrite(struct adapter *padapter, u32 address, u32 data)
 			status = _FAIL;
 			break;
 		}
+		udelay(5);
 	} while (count++);
 
 	return status;
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
index 7c5086ecff17..e04303ce80af 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
@@ -463,30 +463,26 @@ s32 rtl8188eu_xmitframe_complete(struct adapter *adapt, struct xmit_priv *pxmitp
 	}
 
 	/* 3 1. pick up first frame */
-	do {
-		rtw_free_xmitframe(pxmitpriv, pxmitframe);
-
-		pxmitframe = rtw_dequeue_xframe(pxmitpriv, pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
-		if (pxmitframe == NULL) {
-			/*  no more xmit frame, release xmit buffer */
-			rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
-			return false;
-		}
+	rtw_free_xmitframe(pxmitpriv, pxmitframe);
 
-		pxmitframe->pxmitbuf = pxmitbuf;
-		pxmitframe->buf_addr = pxmitbuf->pbuf;
-		pxmitbuf->priv_data = pxmitframe;
+	pxmitframe = rtw_dequeue_xframe(pxmitpriv, pxmitpriv->hwxmits, pxmitpriv->hwxmit_entry);
+	if (pxmitframe == NULL) {
+		/*  no more xmit frame, release xmit buffer */
+		rtw_free_xmitbuf(pxmitpriv, pxmitbuf);
+		return false;
+	}
 
-		pxmitframe->agg_num = 1; /*  alloc xmitframe should assign to 1. */
-		pxmitframe->pkt_offset = 1; /*  first frame of aggregation, reserve offset */
+	pxmitframe->pxmitbuf = pxmitbuf;
+	pxmitframe->buf_addr = pxmitbuf->pbuf;
+	pxmitbuf->priv_data = pxmitframe;
 
-		rtw_xmitframe_coalesce(adapt, pxmitframe->pkt, pxmitframe);
+	pxmitframe->agg_num = 1; /*  alloc xmitframe should assign to 1. */
+	pxmitframe->pkt_offset = 1; /*  first frame of aggregation, reserve offset */
 
-		/*  always return ndis_packet after rtw_xmitframe_coalesce */
-		rtw_os_xmit_complete(adapt, pxmitframe);
+	rtw_xmitframe_coalesce(adapt, pxmitframe->pkt, pxmitframe);
 
-		break;
-	} while (1);
+	/*  always return ndis_packet after rtw_xmitframe_coalesce */
+	rtw_os_xmit_complete(adapt, pxmitframe);
 
 	/* 3 2. aggregate same priority and same DA(AP or STA) frames */
 	pfirstframe = pxmitframe;
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index 7e72259f0e40..5789e1e23f0a 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -684,7 +684,7 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	struct hal_data_8188e		*haldata = GET_HAL_DATA(Adapter);
 	struct pwrctrl_priv		*pwrctrlpriv = &Adapter->pwrctrlpriv;
 	struct registry_priv	*pregistrypriv = &Adapter->registrypriv;
-	u32 init_start_time = jiffies;
+	unsigned long init_start_time = jiffies;
 
 	#define HAL_INIT_PROFILE_TAG(stage) do {} while (0)
 
@@ -903,7 +903,8 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_LCK);
 exit:
 HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_END);
 
-	DBG_88E("%s in %dms\n", __func__, rtw_get_passing_time_ms(init_start_time));
+	DBG_88E("%s in %dms\n", __func__,
+		jiffies_to_msecs(jiffies - init_start_time));
 
 
 	return status;
@@ -1149,14 +1150,15 @@ static void _ReadRFType(struct adapter *Adapter)
 
 static void _ReadAdapterInfo8188EU(struct adapter *Adapter)
 {
-	u32 start = jiffies;
+	unsigned long start = jiffies;
 
 	MSG_88E("====> %s\n", __func__);
 
 	_ReadRFType(Adapter);/* rf_chip -> _InitRFType() */
 	_ReadPROMContent(Adapter);
 
-	MSG_88E("<==== %s in %d ms\n", __func__, rtw_get_passing_time_ms(start));
+	MSG_88E("<==== %s in %d ms\n", __func__,
+		jiffies_to_msecs(jiffies - start));
 }
 
 #define GPIO_DEBUG_PORT_NUM 0
diff --git a/drivers/staging/rtl8188eu/include/HalVerDef.h b/drivers/staging/rtl8188eu/include/HalVerDef.h
index 56b4ff08e509..6f2b2a436b04 100644
--- a/drivers/staging/rtl8188eu/include/HalVerDef.h
+++ b/drivers/staging/rtl8188eu/include/HalVerDef.h
@@ -47,37 +47,4 @@ struct HAL_VERSION {
 	enum HAL_VENDOR		VendorType;
 };
 
-/*  Get element */
-#define GET_CVID_CHIP_TYPE(version)	(((version).ChipType))
-#define GET_CVID_MANUFACTUER(version)	(((version).VendorType))
-#define GET_CVID_CUT_VERSION(version)	(((version).CUTVersion))
-
-/* Common Macro. -- */
-/* HAL_VERSION VersionID */
-
-/* HAL_CHIP_TYPE_E */
-#define IS_TEST_CHIP(version)				\
-	((GET_CVID_CHIP_TYPE(version) == TEST_CHIP) ? true : false)
-#define IS_NORMAL_CHIP(version)				\
-	((GET_CVID_CHIP_TYPE(version) == NORMAL_CHIP) ? true : false)
-
-/* HAL_CUT_VERSION_E */
-#define IS_A_CUT(version)				\
-	((GET_CVID_CUT_VERSION(version) == A_CUT_VERSION) ? true : false)
-#define IS_B_CUT(version)				\
-	((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? true : false)
-#define IS_C_CUT(version)				\
-	((GET_CVID_CUT_VERSION(version) == C_CUT_VERSION) ? true : false)
-#define IS_D_CUT(version)				\
-	((GET_CVID_CUT_VERSION(version) == D_CUT_VERSION) ? true : false)
-#define IS_E_CUT(version)				\
-	((GET_CVID_CUT_VERSION(version) == E_CUT_VERSION) ? true : false)
-
-
-/* HAL_VENDOR_E */
-#define IS_CHIP_VENDOR_TSMC(version)			\
-	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_TSMC) ? true : false)
-#define IS_CHIP_VENDOR_UMC(version)			\
-	((GET_CVID_MANUFACTUER(version) == CHIP_VENDOR_UMC) ? true : false)
-
 #endif
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index e24fe8cc3d0b..22de53d6539a 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -87,8 +87,6 @@ u32  _rtw_down_sema(struct semaphore *sema);
 
 void _rtw_init_queue(struct __queue *pqueue);
 
-s32  rtw_get_passing_time_ms(u32 start);
-
 struct rtw_netdev_priv_indicator {
 	void *priv;
 	u32 sizeof_priv;
diff --git a/drivers/staging/rtl8188eu/include/rtw_xmit.h b/drivers/staging/rtl8188eu/include/rtw_xmit.h
index 62f5db169523..b7c20883d355 100644
--- a/drivers/staging/rtl8188eu/include/rtw_xmit.h
+++ b/drivers/staging/rtl8188eu/include/rtw_xmit.h
@@ -197,7 +197,6 @@ enum {
 void rtw_sctx_init(struct submit_ctx *sctx, int timeout_ms);
 int rtw_sctx_wait(struct submit_ctx *sctx);
 void rtw_sctx_done_err(struct submit_ctx **sctx, int status);
-void rtw_sctx_done(struct submit_ctx **sctx);
 
 struct xmit_buf {
 	struct list_head list;
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index d063d02db7f0..9201b94d017c 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -1100,7 +1100,7 @@ netdev_open_error:
 int rtw_ips_pwr_up(struct adapter *padapter)
 {
 	int result;
-	u32 start_time = jiffies;
+	unsigned long start_time = jiffies;
 
 	DBG_88E("===>  rtw_ips_pwr_up..............\n");
 	rtw_reset_drv_sw(padapter);
@@ -1109,13 +1109,14 @@ int rtw_ips_pwr_up(struct adapter *padapter)
 
 	rtw_led_control(padapter, LED_CTL_NO_LINK);
 
-	DBG_88E("<===  rtw_ips_pwr_up.............. in %dms\n", rtw_get_passing_time_ms(start_time));
+	DBG_88E("<===  rtw_ips_pwr_up.............. in %dms\n",
+		jiffies_to_msecs(jiffies - start_time));
 	return result;
 }
 
 void rtw_ips_pwr_down(struct adapter *padapter)
 {
-	u32 start_time = jiffies;
+	unsigned long start_time = jiffies;
 
 	DBG_88E("===> rtw_ips_pwr_down...................\n");
 
@@ -1124,7 +1125,8 @@ void rtw_ips_pwr_down(struct adapter *padapter)
 	rtw_led_control(padapter, LED_CTL_POWER_OFF);
 
 	rtw_ips_dev_unload(padapter);
-	DBG_88E("<=== rtw_ips_pwr_down..................... in %dms\n", rtw_get_passing_time_ms(start_time));
+	DBG_88E("<=== rtw_ips_pwr_down..................... in %dms\n",
+		jiffies_to_msecs(jiffies - start_time));
 }
 
 void rtw_ips_dev_unload(struct adapter *padapter)
diff --git a/drivers/staging/rtl8188eu/os_dep/osdep_service.c b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
index 466cd76fc1c4..d87b54711c0d 100644
--- a/drivers/staging/rtl8188eu/os_dep/osdep_service.c
+++ b/drivers/staging/rtl8188eu/os_dep/osdep_service.c
@@ -77,12 +77,6 @@ void	_rtw_init_queue(struct __queue *pqueue)
 	spin_lock_init(&(pqueue->lock));
 }
 
-/*  the input parameter start must be in jiffies */
-inline s32 rtw_get_passing_time_ms(u32 start)
-{
-	return jiffies_to_msecs(jiffies-start);
-}
-
 struct net_device *rtw_alloc_etherdev_with_old_priv(int sizeof_priv,
 						    void *old_priv)
 {
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index 82a7c27c517f..01d50f7c1667 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -227,7 +227,7 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 	struct net_device *pnetdev = padapter->pnetdev;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
-	u32 start_time = jiffies;
+	unsigned long start_time = jiffies;
 
 	pr_debug("==> %s (%s:%d)\n", __func__, current->comm, current->pid);
 
@@ -282,7 +282,7 @@ static int rtw_suspend(struct usb_interface *pusb_intf, pm_message_t message)
 
 exit:
 	pr_debug("<===  %s .............. in %dms\n", __func__,
-		 rtw_get_passing_time_ms(start_time));
+		 jiffies_to_msecs(jiffies - start_time));
 
 	return 0;
 }
@@ -292,7 +292,7 @@ static int rtw_resume_process(struct adapter *padapter)
 	struct net_device *pnetdev;
 	struct pwrctrl_priv *pwrpriv = NULL;
 	int ret = -1;
-	u32 start_time = jiffies;
+	unsigned long start_time = jiffies;
 
 	pr_debug("==> %s (%s:%d)\n", __func__, current->comm, current->pid);
 
@@ -323,7 +323,7 @@ exit:
 	if (pwrpriv)
 		pwrpriv->bInSuspend = false;
 	pr_debug("<===  %s return %d.............. in %dms\n", __func__,
-		ret, rtw_get_passing_time_ms(start_time));
+		ret, jiffies_to_msecs(jiffies - start_time));
 
 	return ret;
 }
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 0b407feb5407..5e3bbe5c3ca4 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -90,13 +90,12 @@ void rtl92e_set_bb_reg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask,
 
 u32 rtl92e_get_bb_reg(struct net_device *dev, u32 dwRegAddr, u32 dwBitMask)
 {
-	u32 Ret = 0, OriginalValue, BitShift;
+	u32 OriginalValue, BitShift;
 
 	OriginalValue = rtl92e_readl(dev, dwRegAddr);
 	BitShift = _rtl92e_calculate_bit_shift(dwBitMask);
-	Ret = (OriginalValue & dwBitMask) >> BitShift;
 
-	return Ret;
+	return (OriginalValue & dwBitMask) >> BitShift;
 }
 
 static u32 _rtl92e_phy_rf_read(struct net_device *dev,
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index e06864f64beb..f4a4eae72aa4 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -5114,21 +5114,6 @@ static void __exit rtl8192_usb_module_exit(void)
 	RT_TRACE(COMP_DOWN, "Exiting");
 }
 
-
-void rtl8192_try_wake_queue(struct net_device *dev, int pri)
-{
-	unsigned long flags;
-	short enough_desc;
-	struct r8192_priv *priv = (struct r8192_priv *)ieee80211_priv(dev);
-
-	spin_lock_irqsave(&priv->tx_lock, flags);
-	enough_desc = check_nic_enough_desc(dev, pri);
-	spin_unlock_irqrestore(&priv->tx_lock, flags);
-
-	if (enough_desc)
-		ieee80211_wake_queue(priv->ieee80211);
-}
-
 void EnableHWSecurityConfig8192(struct net_device *dev)
 {
 	u8 SECR_value = 0x0;
diff --git a/drivers/staging/rtl8192u/r819xU_phy.c b/drivers/staging/rtl8192u/r819xU_phy.c
index 70656441c145..f264d88364a1 100644
--- a/drivers/staging/rtl8192u/r819xU_phy.c
+++ b/drivers/staging/rtl8192u/r819xU_phy.c
@@ -38,21 +38,6 @@ static u32 RF_CHANNEL_TABLE_ZEBRA[] = {
 #define rtl819XAGCTAB_Array Rtl8192UsbAGCTAB_Array
 
 /******************************************************************************
- * function: This function reads BB parameters from header file we generate,
- *           and does register read/write
- * input:    u32	bitmask  //taget bit pos in the addr to be modified
- * output:   none
- * return:   u32	return the shift bit position of the mask
- ******************************************************************************/
-static u32 rtl8192_CalculateBitShift(u32 bitmask)
-{
-	u32 i;
-
-	i = ffs(bitmask) - 1;
-	return i;
-}
-
-/******************************************************************************
  * function:  This function checks different RF type to execute legal judgement.
  *            If RF Path is illegal, we will return false.
  * input:     net_device	 *dev
@@ -94,7 +79,7 @@ void rtl8192_setBBreg(struct net_device *dev, u32 reg_addr, u32 bitmask,
 
 	if (bitmask != bMaskDWord) {
 		read_nic_dword(dev, reg_addr, &reg);
-		bitshift = rtl8192_CalculateBitShift(bitmask);
+		bitshift = ffs(bitmask) - 1;
 		reg &= ~bitmask;
 		reg |= data << bitshift;
 		write_nic_dword(dev, reg_addr, reg);
@@ -117,7 +102,7 @@ u32 rtl8192_QueryBBReg(struct net_device *dev, u32 reg_addr, u32 bitmask)
 	u32 reg, bitshift;
 
 	read_nic_dword(dev, reg_addr, &reg);
-	bitshift = rtl8192_CalculateBitShift(bitmask);
+	bitshift = ffs(bitmask) - 1;
 
 	return (reg & bitmask) >> bitshift;
 }
@@ -306,7 +291,7 @@ void rtl8192_phy_SetRFReg(struct net_device *dev, RF90_RADIO_PATH_E eRFPath,
 		if (bitmask != bMask12Bits) {
 			/* RF data is 12 bits only */
 			reg = phy_FwRFSerialRead(dev, eRFPath, reg_addr);
-			bitshift =  rtl8192_CalculateBitShift(bitmask);
+			bitshift =  ffs(bitmask) - 1;
 			reg &= ~bitmask;
 			reg |= data << bitshift;
 
@@ -321,7 +306,7 @@ void rtl8192_phy_SetRFReg(struct net_device *dev, RF90_RADIO_PATH_E eRFPath,
 		if (bitmask != bMask12Bits) {
 			/* RF data is 12 bits only */
 			reg = rtl8192_phy_RFSerialRead(dev, eRFPath, reg_addr);
-			bitshift =  rtl8192_CalculateBitShift(bitmask);
+			bitshift =  ffs(bitmask) - 1;
 			reg &= ~bitmask;
 			reg |= data << bitshift;
 
@@ -356,7 +341,7 @@ u32 rtl8192_phy_QueryRFReg(struct net_device *dev, RF90_RADIO_PATH_E eRFPath,
 	} else {
 		reg = rtl8192_phy_RFSerialRead(dev, eRFPath, reg_addr);
 	}
-	bitshift =  rtl8192_CalculateBitShift(bitmask);
+	bitshift =  ffs(bitmask) - 1;
 	reg = (reg & bitmask) >> bitshift;
 	return reg;
 
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index a4a002b55128..04f727fc95ea 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -123,7 +123,7 @@ static void _free_network(struct mlme_priv *pmlmepriv,
 	spin_unlock_irqrestore(&free_queue->lock, irqL);
 }
 
-static void _free_network_nolock(struct mlme_priv *pmlmepriv,
+static void free_network_nolock(struct mlme_priv *pmlmepriv,
 			  struct wlan_network *pnetwork)
 {
 	struct  __queue *free_queue = &pmlmepriv->free_bss_pool;
@@ -234,12 +234,6 @@ static struct	wlan_network *alloc_network(struct mlme_priv *pmlmepriv)
 	return _r8712_alloc_network(pmlmepriv);
 }
 
-static void free_network_nolock(struct mlme_priv *pmlmepriv,
-			 struct wlan_network *pnetwork)
-{
-	_free_network_nolock(pmlmepriv, pnetwork);
-}
-
 void r8712_free_network_queue(struct _adapter *dev)
 {
 	_free_network_queue(dev);
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index 906b5782d165..f174b4d1a018 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -273,48 +273,6 @@ u8 EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 		return 0xFF;
 }
 
-/* Copy from WMAC fot EFUSE write 1 byte. */
-void EFUSE_Write1Byte(struct rtw_adapter *Adapter, u16 Address, u8 Value)
-{
-	u8	Bytetemp = {0x00};
-	u8	temp = {0x00};
-	u32	k = 0;
-	u16	contentLen = 0;
-
-	EFUSE_GetEfuseDefinition23a(Adapter, EFUSE_WIFI,
-				 TYPE_EFUSE_REAL_CONTENT_LEN,
-				 (void *)&contentLen);
-
-	if (Address < contentLen) { /* E-fuse 512Byte */
-		rtl8723au_write8(Adapter, EFUSE_CTRL, Value);
-
-		/* Write E-fuse Register address bit0~7 */
-		temp = Address & 0xFF;
-		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
-		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+2);
-
-		/* Write E-fuse Register address bit8~9 */
-		temp = ((Address >> 8) & 0x03) | (Bytetemp & 0xFC);
-		rtl8723au_write8(Adapter, EFUSE_CTRL+2, temp);
-
-		/* Write 0x30[31]= 1 */
-		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
-		temp = Bytetemp | 0x80;
-		rtl8723au_write8(Adapter, EFUSE_CTRL+3, temp);
-
-		/* Wait Write-ready (0x30[31]= 0) */
-		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
-		while (Bytetemp & 0x80) {
-			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
-			k++;
-			if (k == 100) {
-				k = 0;
-				break;
-			}
-		}
-	}
-}
-
 /* Read one byte from real Efuse. */
 int efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data)
 {
@@ -501,7 +459,8 @@ int rtw_BT_efuse_map_read23a(struct rtw_adapter *padapter,
 }
 
 /* Read All Efuse content */
-void Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType, u8 *Efuse)
+static void Efuse_ReadAllMap(struct rtw_adapter *pAdapter, u8 efuseType,
+			     u8 *Efuse)
 {
 	u16	mapLen = 0;
 
diff --git a/drivers/staging/sm750fb/modedb.h b/drivers/staging/sm750fb/modedb.h
deleted file mode 100644
index 83cb2e2ae51a..000000000000
--- a/drivers/staging/sm750fb/modedb.h
+++ /dev/null
@@ -1,233 +0,0 @@
-
-static const struct fb_videomode modedb2[] = {
-	{
-		/* 640x400 @ 70 Hz, 31.5 kHz hsync */
-		NULL, 70, 640, 400, 39721, 40, 24, 39, 9, 96, 2,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 640x480 @ 60 Hz, 31.5 kHz hsync */
-		NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 800x600 @ 56 Hz, 35.15 kHz hsync */
-		NULL, 56, 800, 600, 27777, 128, 24, 22, 1, 72, 2,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 87 Hz interlaced, 35.5 kHz hsync */
-		NULL, 87, 1024, 768, 22271, 56, 24, 33, 8, 160, 8,
-		0, FB_VMODE_INTERLACED
-	}, {
-		/* 640x400 @ 85 Hz, 37.86 kHz hsync */
-		NULL, 85, 640, 400, 31746, 96, 32, 41, 1, 64, 3,
-		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
-	}, {
-		/* 640x480 @ 72 Hz, 36.5 kHz hsync */
-		NULL, 72, 640, 480, 31746, 144, 40, 30, 8, 40, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 640x480 @ 75 Hz, 37.50 kHz hsync */
-		NULL, 75, 640, 480, 31746, 120, 16, 16, 1, 64, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 800x600 @ 60 Hz, 37.8 kHz hsync */
-		NULL, 60, 800, 600, 25000, 88, 40, 23, 1, 128, 4,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 640x480 @ 85 Hz, 43.27 kHz hsync */
-		NULL, 85, 640, 480, 27777, 80, 56, 25, 1, 56, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 89 Hz interlaced, 44 kHz hsync */
-		NULL, 69, 1152, 864, 15384, 96, 16, 110, 1, 216, 10,
-		0, FB_VMODE_INTERLACED
-	}, {
-		/* 800x600 @ 72 Hz, 48.0 kHz hsync */
-		NULL, 72, 800, 600, 20000, 64, 56, 23, 37, 120, 6,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 60 Hz, 48.4 kHz hsync */
-		NULL, 60, 1024, 768, 15384, 168, 8, 29, 3, 144, 6,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 640x480 @ 100 Hz, 53.01 kHz hsync */
-		NULL, 100, 640, 480, 21834, 96, 32, 36, 8, 96, 6,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 60 Hz, 53.5 kHz hsync */
-		NULL, 60, 1152, 864, 11123, 208, 64, 16, 4, 256, 8,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 800x600 @ 85 Hz, 55.84 kHz hsync */
-		NULL, 85, 800, 600, 16460, 160, 64, 36, 16, 64, 5,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 70 Hz, 56.5 kHz hsync */
-		NULL, 70, 1024, 768, 13333, 144, 24, 29, 3, 136, 6,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/*  1280x960-60 VESA */
-		NULL, 60, 1280, 960, 9259, 312, 96, 36, 1, 112, 3,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
-	}, {
-		/*  1280x1024-60 VESA */
-		NULL, 60, 1280, 1024, 9259, 248, 48, 38, 1, 112, 3,
-		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED, FB_MODE_IS_VESA
-	}, {
-		/* 1280x1024 @ 87 Hz interlaced, 51 kHz hsync */
-		NULL, 87, 1280, 1024, 12500, 56, 16, 128, 1, 216, 12,
-		0, FB_VMODE_INTERLACED
-	}, {
-		/* 800x600 @ 100 Hz, 64.02 kHz hsync */
-		NULL, 100, 800, 600, 14357, 160, 64, 30, 4, 64, 6,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 76 Hz, 62.5 kHz hsync */
-		NULL, 76, 1024, 768, 11764, 208, 8, 36, 16, 120, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 70 Hz, 62.4 kHz hsync */
-		NULL, 70, 1152, 864, 10869, 106, 56, 20, 1, 160, 10,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 61 Hz, 64.2 kHz hsync */
-		NULL, 61, 1280, 1024, 9090, 200, 48, 26, 1, 184, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1400x1050 @ 60Hz, 63.9 kHz hsync */
-		NULL, 68, 1400, 1050, 9259, 136, 40, 13, 1, 112, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1400x1050 @ 75,107 Hz, 82,392 kHz +hsync +vsync*/
-		NULL, 75, 1400, 1050, 9271, 120, 56, 13, 0, 112, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1400x1050 @ 60 Hz, ? kHz +hsync +vsync*/
-		NULL, 60, 1400, 1050, 9259, 128, 40, 12, 0, 112, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 85 Hz, 70.24 kHz hsync */
-		NULL, 85, 1024, 768, 10111, 192, 32, 34, 14, 160, 6,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 78 Hz, 70.8 kHz hsync */
-		NULL, 78, 1152, 864, 9090, 228, 88, 32, 0, 84, 12,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 70 Hz, 74.59 kHz hsync */
-		NULL, 70, 1280, 1024, 7905, 224, 32, 28, 8, 160, 8,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1600x1200 @ 60Hz, 75.00 kHz hsync */
-		NULL, 60, 1600, 1200, 6172, 304, 64, 46, 1, 192, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 84 Hz, 76.0 kHz hsync */
-		NULL, 84, 1152, 864, 7407, 184, 312, 32, 0, 128, 12,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 74 Hz, 78.85 kHz hsync */
-		NULL, 74, 1280, 1024, 7407, 256, 32, 34, 3, 144, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1024x768 @ 100Hz, 80.21 kHz hsync */
-		NULL, 100, 1024, 768, 8658, 192, 32, 21, 3, 192, 10,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 76 Hz, 81.13 kHz hsync */
-		NULL, 76, 1280, 1024, 7407, 248, 32, 34, 3, 104, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1600x1200 @ 70 Hz, 87.50 kHz hsync */
-		NULL, 70, 1600, 1200, 5291, 304, 64, 46, 1, 192, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1152x864 @ 100 Hz, 89.62 kHz hsync */
-		NULL, 100, 1152, 864, 7264, 224, 32, 17, 2, 128, 19,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 85 Hz, 91.15 kHz hsync */
-		NULL, 85, 1280, 1024, 6349, 224, 64, 44, 1, 160, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1600x1200 @ 75 Hz, 93.75 kHz hsync */
-		NULL, 75, 1600, 1200, 4938, 304, 64, 46, 1, 192, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1600x1200 @ 85 Hz, 105.77 kHz hsync */
-		NULL, 85, 1600, 1200, 4545, 272, 16, 37, 4, 192, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1280x1024 @ 100 Hz, 107.16 kHz hsync */
-		NULL, 100, 1280, 1024, 5502, 256, 32, 26, 7, 128, 15,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 1800x1440 @ 64Hz, 96.15 kHz hsync  */
-		NULL, 64, 1800, 1440, 4347, 304, 96, 46, 1, 192, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 1800x1440 @ 70Hz, 104.52 kHz hsync  */
-		NULL, 70, 1800, 1440, 4000, 304, 96, 46, 1, 192, 3,
-		FB_SYNC_HOR_HIGH_ACT|FB_SYNC_VERT_HIGH_ACT,
-		FB_VMODE_NONINTERLACED
-	}, {
-		/* 512x384 @ 78 Hz, 31.50 kHz hsync */
-		NULL, 78, 512, 384, 49603, 48, 16, 16, 1, 64, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 512x384 @ 85 Hz, 34.38 kHz hsync */
-		NULL, 85, 512, 384, 45454, 48, 16, 16, 1, 64, 3,
-		0, FB_VMODE_NONINTERLACED
-	}, {
-		/* 320x200 @ 70 Hz, 31.5 kHz hsync, 8:5 aspect ratio */
-		NULL, 70, 320, 200, 79440, 16, 16, 20, 4, 48, 1,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 320x240 @ 60 Hz, 31.5 kHz hsync, 4:3 aspect ratio */
-		NULL, 60, 320, 240, 79440, 16, 16, 16, 5, 48, 1,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 320x240 @ 72 Hz, 36.5 kHz hsync */
-		NULL, 72, 320, 240, 63492, 16, 16, 16, 4, 48, 2,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 400x300 @ 56 Hz, 35.2 kHz hsync, 4:3 aspect ratio */
-		NULL, 56, 400, 300, 55555, 64, 16, 10, 1, 32, 1,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 400x300 @ 60 Hz, 37.8 kHz hsync */
-		NULL, 60, 400, 300, 50000, 48, 16, 11, 1, 64, 2,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 400x300 @ 72 Hz, 48.0 kHz hsync */
-		NULL, 72, 400, 300, 40000, 32, 24, 11, 19, 64, 3,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 480x300 @ 56 Hz, 35.2 kHz hsync, 8:5 aspect ratio */
-		NULL, 56, 480, 300, 46176, 80, 16, 10, 1, 40, 1,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 480x300 @ 60 Hz, 37.8 kHz hsync */
-		NULL, 60, 480, 300, 41858, 56, 16, 11, 1, 80, 2,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 480x300 @ 63 Hz, 39.6 kHz hsync */
-		NULL, 63, 480, 300, 40000, 56, 16, 11, 1, 80, 2,
-		0, FB_VMODE_DOUBLE
-	}, {
-		/* 480x300 @ 72 Hz, 48.0 kHz hsync */
-		NULL, 72, 480, 300, 33386, 40, 24, 11, 19, 80, 3,
-		0, FB_VMODE_DOUBLE
-	},
-};
-static const int nmodedb2 = sizeof(modedb2);
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index 860e1c288ad5..c78421b5b0e7 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -21,8 +21,6 @@
 #include "sm750_accel.h"
 #include "sm750_cursor.h"
 
-#include "modedb.h"
-
 /*
  * #ifdef __BIG_ENDIAN
  * ssize_t lynxfb_ops_write(struct fb_info *info, const char __user *buf,
diff --git a/drivers/staging/speakup/Kconfig b/drivers/staging/speakup/Kconfig
index efd6f4560d3e..7e8037e230b8 100644
--- a/drivers/staging/speakup/Kconfig
+++ b/drivers/staging/speakup/Kconfig
@@ -1,7 +1,7 @@
 menu "Speakup console speech"
 
 config SPEAKUP
-	depends on VT
+	depends on VT && !MN10300
 	tristate "Speakup core"
 	---help---
 		This is the Speakup screen reader.  Think of it as a
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 63c59bc89b04..30cf973f326d 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -264,8 +264,9 @@ static struct notifier_block vt_notifier_block = {
 	.notifier_call = vt_notifier_call,
 };
 
-static unsigned char get_attributes(u16 *pos)
+static unsigned char get_attributes(struct vc_data *vc, u16 *pos)
 {
+	pos = screen_pos(vc, pos - (u16 *)vc->vc_origin, 1);
 	return (u_char) (scr_readw(pos) >> 8);
 }
 
@@ -275,7 +276,7 @@ static void speakup_date(struct vc_data *vc)
 	spk_y = spk_cy = vc->vc_y;
 	spk_pos = spk_cp = vc->vc_pos;
 	spk_old_attr = spk_attr;
-	spk_attr = get_attributes((u_short *) spk_pos);
+	spk_attr = get_attributes(vc, (u_short *)spk_pos);
 }
 
 static void bleep(u_short val)
@@ -469,8 +470,12 @@ static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
 	u16 ch = ' ';
 
 	if (vc && pos) {
-		u16 w = scr_readw(pos);
-		u16 c = w & 0xff;
+		u16 w;
+		u16 c;
+
+		pos = screen_pos(vc, pos - (u16 *)vc->vc_origin, 1);
+		w = scr_readw(pos);
+		c = w & 0xff;
 
 		if (w & vc->vc_hi_font_mask)
 			c |= 0x100;
@@ -746,7 +751,7 @@ static int get_line(struct vc_data *vc)
 	u_char tmp2;
 
 	spk_old_attr = spk_attr;
-	spk_attr = get_attributes((u_short *) spk_pos);
+	spk_attr = get_attributes(vc, (u_short *)spk_pos);
 	for (i = 0; i < vc->vc_cols; i++) {
 		buf[i] = (u_char) get_char(vc, (u_short *) tmp, &tmp2);
 		tmp += 2;
@@ -811,7 +816,7 @@ static int say_from_to(struct vc_data *vc, u_long from, u_long to,
 	u_short saved_punc_mask = spk_punc_mask;
 
 	spk_old_attr = spk_attr;
-	spk_attr = get_attributes((u_short *) from);
+	spk_attr = get_attributes(vc, (u_short *)from);
 	while (from < to) {
 		buf[i++] = (char)get_char(vc, (u_short *) from, &tmp);
 		from += 2;
@@ -886,7 +891,7 @@ static int get_sentence_buf(struct vc_data *vc, int read_punc)
 	sentmarks[bn][0] = &sentbuf[bn][0];
 	i = 0;
 	spk_old_attr = spk_attr;
-	spk_attr = get_attributes((u_short *) start);
+	spk_attr = get_attributes(vc, (u_short *)start);
 
 	while (start < end) {
 		sentbuf[bn][i] = (char)get_char(vc, (u_short *) start, &tmp);
@@ -1585,7 +1590,7 @@ static int count_highlight_color(struct vc_data *vc)
 		u16 *ptr;
 
 		for (ptr = start; ptr < end; ptr++) {
-			ch = get_attributes(ptr);
+			ch = get_attributes(vc, ptr);
 			bg = (ch & 0x70) >> 4;
 			speakup_console[vc_num]->ht.bgcount[bg]++;
 		}
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index aa5ab6c80ed4..41ef099b7aa6 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -142,7 +142,9 @@ static void __speakup_paste_selection(struct work_struct *work)
 	struct tty_ldisc *ld;
 	DECLARE_WAITQUEUE(wait, current);
 
-	ld = tty_ldisc_ref_wait(tty);
+	ld = tty_ldisc_ref(tty);
+	if (!ld)
+		goto tty_unref;
 	tty_buffer_lock_exclusive(&vc->port);
 
 	add_wait_queue(&vc->paste_wait, &wait);
@@ -162,6 +164,7 @@ static void __speakup_paste_selection(struct work_struct *work)
 
 	tty_buffer_unlock_exclusive(&vc->port);
 	tty_ldisc_deref(ld);
+tty_unref:
 	tty_kref_put(tty);
 }
 
diff --git a/drivers/staging/speakup/serialio.c b/drivers/staging/speakup/serialio.c
index 3b5835b28128..a5bbb338f275 100644
--- a/drivers/staging/speakup/serialio.c
+++ b/drivers/staging/speakup/serialio.c
@@ -6,6 +6,11 @@
 #include "spk_priv.h"
 #include "serialio.h"
 
+#include <linux/serial_core.h>
+/* WARNING:  Do not change this to <linux/serial.h> without testing that
+ * SERIAL_PORT_DFNS does get defined to the appropriate value. */
+#include <asm/serial.h>
+
 #ifndef SERIAL_PORT_DFNS
 #define SERIAL_PORT_DFNS
 #endif
@@ -23,9 +28,15 @@ const struct old_serial_port *spk_serial_init(int index)
 	int baud = 9600, quot = 0;
 	unsigned int cval = 0;
 	int cflag = CREAD | HUPCL | CLOCAL | B9600 | CS8;
-	const struct old_serial_port *ser = rs_table + index;
+	const struct old_serial_port *ser;
 	int err;
 
+	if (index >= ARRAY_SIZE(rs_table)) {
+		pr_info("no port info for ttyS%d\n", index);
+		return NULL;
+	}
+	ser = rs_table + index;
+
 	/*	Divisor, bytesize and parity */
 	quot = ser->baud_base / baud;
 	cval = cflag & (CSIZE | CSTOPB);
diff --git a/drivers/staging/unisys/include/channel.h b/drivers/staging/unisys/include/channel.h
index c6c24423a7f0..5af59a5fce61 100644
--- a/drivers/staging/unisys/include/channel.h
+++ b/drivers/staging/unisys/include/channel.h
@@ -60,15 +60,19 @@ enum channel_clientstate {
 	CHANNELCLI_DISABLED = 1,	/* client can see channel but is NOT
 					 * allowed to use it unless given TBD
 					 * explicit request (should actually be
-					 * < DETACHED) */
+					 * < DETACHED)
+					 */
 	CHANNELCLI_ATTACHING = 2,	/* legacy EFI client request
-					 * for EFI server to attach */
+					 * for EFI server to attach
+					 */
 	CHANNELCLI_ATTACHED = 3,	/* idle, but client may want
-					 * to use channel any time */
+					 * to use channel any time
+					 */
 	CHANNELCLI_BUSY = 4,	/* client either wants to use or is
-				 * using channel */
-	CHANNELCLI_OWNED = 5	/* "no worries" state - client can
-				 * access channel anytime */
+				 * using channel
+				 */
+	CHANNELCLI_OWNED = 5	/* "no worries" state - client can */
+				/* access channel anytime */
 };
 
 static inline const u8 *
@@ -116,11 +120,13 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 
 /* Values for ULTRA_CHANNEL_PROTOCOL.CliErrorBoot: */
 /* throttling invalid boot channel statetransition error due to client
- * disabled */
+ * disabled
+ */
 #define ULTRA_CLIERRORBOOT_THROTTLEMSG_DISABLED    0x01
 
 /* throttling invalid boot channel statetransition error due to client
- * not attached */
+ * not attached
+ */
 #define ULTRA_CLIERRORBOOT_THROTTLEMSG_NOTATTACHED 0x02
 
 /* throttling invalid boot channel statetransition error due to busy channel */
@@ -128,24 +134,28 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 
 /* Values for ULTRA_CHANNEL_PROTOCOL.CliErrorOS: */
 /* throttling invalid guest OS channel statetransition error due to
- * client disabled */
+ * client disabled
+ */
 #define ULTRA_CLIERROROS_THROTTLEMSG_DISABLED      0x01
 
 /* throttling invalid guest OS channel statetransition error due to
- * client not attached */
+ * client not attached
+ */
 #define ULTRA_CLIERROROS_THROTTLEMSG_NOTATTACHED   0x02
 
 /* throttling invalid guest OS channel statetransition error due to
- * busy channel */
+ * busy channel
+ */
 #define ULTRA_CLIERROROS_THROTTLEMSG_BUSY          0x04
 
 /* Values for ULTRA_CHANNEL_PROTOCOL.Features: This define exists so
-* that windows guest can look at the FeatureFlags in the io channel,
-* and configure the windows driver to use interrupts or not based on
-* this setting.  This flag is set in uislib after the
-* ULTRA_VHBA_init_channel is called.  All feature bits for all
-* channels should be defined here.  The io channel feature bits are
-* defined right here */
+ * that windows guest can look at the FeatureFlags in the io channel,
+ * and configure the windows driver to use interrupts or not based on
+ * this setting.  This flag is set in uislib after the
+ * ULTRA_VHBA_init_channel is called.  All feature bits for all
+ * channels should be defined here.  The io channel feature bits are
+ * defined right here
+ */
 #define ULTRA_IO_DRIVER_ENABLES_INTS (0x1ULL << 1)
 #define ULTRA_IO_CHANNEL_IS_POLLING (0x1ULL << 3)
 #define ULTRA_IO_IOVM_IS_OK_WITH_DRIVER_DISABLING_INTS (0x1ULL << 4)
@@ -156,7 +166,7 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 struct channel_header {
 	u64 signature;		/* Signature */
 	u32 legacy_state;	/* DEPRECATED - being replaced by */
-	/* /              SrvState, CliStateBoot, and CliStateOS below */
+			/* SrvState, CliStateBoot, and CliStateOS below */
 	u32 header_size;	/* sizeof(struct channel_header) */
 	u64 size;		/* Total size of this channel in bytes */
 	u64 features;		/* Flags to modify behavior */
@@ -169,25 +179,32 @@ struct channel_header {
 	uuid_le zone_uuid;	/* Guid of Channel's zone */
 	u32 cli_str_offset;	/* offset from channel header to
 				 * nul-terminated ClientString (0 if
-				 * ClientString not present) */
+				 * ClientString not present)
+				 */
 	u32 cli_state_boot;	/* CHANNEL_CLIENTSTATE of pre-boot
-				 * EFI client of this channel */
+				 * EFI client of this channel
+				 */
 	u32 cmd_state_cli;	/* CHANNEL_COMMANDSTATE (overloaded in
 				 * Windows drivers, see ServerStateUp,
-				 * ServerStateDown, etc) */
+				 * ServerStateDown, etc)
+				 */
 	u32 cli_state_os;	/* CHANNEL_CLIENTSTATE of Guest OS
-				 * client of this channel */
+				 * client of this channel
+				 */
 	u32 ch_characteristic;	/* CHANNEL_CHARACTERISTIC_<xxx> */
 	u32 cmd_state_srv;	/* CHANNEL_COMMANDSTATE (overloaded in
 				 * Windows drivers, see ServerStateUp,
-				 * ServerStateDown, etc) */
+				 * ServerStateDown, etc)
+				 */
 	u32 srv_state;		/* CHANNEL_SERVERSTATE */
 	u8 cli_error_boot;	/* bits to indicate err states for
 				 * boot clients, so err messages can
-				 * be throttled */
+				 * be throttled
+				 */
 	u8 cli_error_os;	/* bits to indicate err states for OS
 				 * clients, so err messages can be
-				 * throttled */
+				 * throttled
+				 */
 	u8 filler[1];		/* Pad out to 128 byte cacheline */
 	/* Please add all new single-byte values below here */
 	u8 recover_channel;
@@ -205,29 +222,33 @@ struct signal_queue_header {
 	u64 features;		/* Flags to modify behavior */
 	u64 num_sent;		/* Total # of signals placed in this queue */
 	u64 num_overflows;	/* Total # of inserts failed due to
-				 * full queue */
+				 * full queue
+				 */
 	u32 signal_size;	/* Total size of a signal for this queue */
 	u32 max_slots;		/* Max # of slots in queue, 1 slot is
-				 * always empty */
+				 * always empty
+				 */
 	u32 max_signals;	/* Max # of signals in queue
-				 * (MaxSignalSlots-1) */
+				 * (MaxSignalSlots-1)
+				 */
 	u32 head;		/* Queue head signal # */
 	/* 2nd cache line */
 	u64 num_received;	/* Total # of signals removed from this queue */
-	u32 tail;		/* Queue tail signal # (on separate
-				 * cache line) */
+	u32 tail;		/* Queue tail signal */
 	u32 reserved1;		/* Reserved field */
 	u64 reserved2;		/* Reserved field */
 	u64 client_queue;
 	u64 num_irq_received;	/* Total # of Interrupts received.  This
-					 * is incremented by the ISR in the
-					 * guest windows driver */
+				 * is incremented by the ISR in the
+				 * guest windows driver
+				 */
 	u64 num_empty;		/* Number of times that visor_signal_remove
-				 * is called and returned Empty
-				 * Status. */
+				 * is called and returned Empty Status.
+				 */
 	u32 errorflags;		/* Error bits set during SignalReinit
 				 * to denote trouble with client's
-				 * fields */
+				 * fields
+				 */
 	u8 filler[12];		/* Pad out to 64 byte cacheline */
 } __packed;
 
@@ -272,8 +293,7 @@ spar_check_channel_client(void __iomem *ch,
 			return 0;
 		}
 	}
-	if (expected_min_bytes > 0) {	/* caller wants us to verify
-					 * channel size */
+	if (expected_min_bytes > 0) {	/* verify channel size */
 		unsigned long long bytes =
 				readq(&((struct channel_header __iomem *)
 					(ch))->size);
@@ -284,8 +304,7 @@ spar_check_channel_client(void __iomem *ch,
 			return 0;
 		}
 	}
-	if (expected_version > 0) {	/* caller wants us to verify
-					 * channel version */
+	if (expected_version > 0) {	/* verify channel version */
 		unsigned long ver = readl(&((struct channel_header __iomem *)
 				    (ch))->version_id);
 		if (ver != expected_version) {
@@ -295,8 +314,7 @@ spar_check_channel_client(void __iomem *ch,
 			return 0;
 		}
 	}
-	if (expected_signature > 0) {	/* caller wants us to verify
-					 * channel signature */
+	if (expected_signature > 0) {	/* verify channel signature */
 		unsigned long long sig =
 				readq(&((struct channel_header __iomem *)
 					(ch))->signature);
@@ -319,8 +337,7 @@ static inline int spar_check_channel_server(uuid_le typeuuid, char *name,
 					    u64 expected_min_bytes,
 					    u64 actual_bytes)
 {
-	if (expected_min_bytes > 0)	/* caller wants us to verify
-					 * channel size */
+	if (expected_min_bytes > 0)	/* verify channel size */
 		if (actual_bytes < expected_min_bytes) {
 			pr_err("Channel mismatch on channel=%s(%pUL) field=size expected=0x%-8.8llx actual=0x%-8.8llx\n",
 			       name, &typeuuid, expected_min_bytes,
@@ -354,7 +371,8 @@ pathname_last_n_nodes(u8 *s, unsigned int n)
 		if (p == s)
 			break;	/* should never happen, unless someone
 				 * is changing the string while we are
-				 * looking at it!! */
+				 * looking at it!!
+				 */
 		if ((*p == '/') || (*p == '\\'))
 			n--;
 	}
@@ -395,7 +413,8 @@ spar_channel_client_acquire_os(void __iomem *ch, u8 *id)
 	if (readl(&hdr->cli_state_os) == CHANNELCLI_OWNED) {
 		if (readb(&hdr->cli_error_os) != 0) {
 			/* we are in an error msg throttling state;
-			 * come out of it */
+			 * come out of it
+			 */
 			pr_info("%s Channel OS client acquire now successful\n",
 				id);
 			writeb(0, &hdr->cli_error_os);
@@ -404,8 +423,9 @@ spar_channel_client_acquire_os(void __iomem *ch, u8 *id)
 	}
 
 	/* We have to do it the "hard way".  We transition to BUSY,
-	* and can use the channel iff our competitor has not also
-	* transitioned to BUSY. */
+	 * and can use the channel iff our competitor has not also
+	 * transitioned to BUSY.
+	 */
 	if (readl(&hdr->cli_state_os) != CHANNELCLI_ATTACHED) {
 		if ((readb(&hdr->cli_error_os)
 		     & ULTRA_CLIERROROS_THROTTLEMSG_NOTATTACHED) == 0) {
diff --git a/drivers/staging/unisys/include/iochannel.h b/drivers/staging/unisys/include/iochannel.h
index 14e656ff73ec..162ca187a66b 100644
--- a/drivers/staging/unisys/include/iochannel.h
+++ b/drivers/staging/unisys/include/iochannel.h
@@ -6,7 +6,8 @@
 /*
  * Everything needed for IOPart-GuestPart communication is define in
  * this file.  Note: Everything is OS-independent because this file is
- * used by Windows, Linux and possible EFI drivers.  */
+ * used by Windows, Linux and possible EFI drivers.
+ */
 
 /*
  * Communication flow between the IOPart and GuestPart uses the channel headers
@@ -66,21 +67,15 @@
  * IO Partition is defined below.
  */
 
-/*
- * Defines and enums.
- */
-
+/* Defines and enums. */
 #define MINNUM(a, b) (((a) < (b)) ? (a) : (b))
 #define MAXNUM(a, b) (((a) > (b)) ? (a) : (b))
 
-/* these define the two queues per data channel between iopart and
- * ioguestparts
- */
-#define IOCHAN_TO_IOPART 0 /* used by ioguestpart to 'insert' signals to
-			    * iopart */
-
-#define IOCHAN_FROM_IOPART 1 /* used by ioguestpart to 'remove' signals from
-			      * iopart - same queue as previous queue */
+/* define the two queues per data channel between iopart and ioguestparts */
+/* used by ioguestpart to 'insert' signals to iopart */
+#define IOCHAN_TO_IOPART 0
+/* used by ioguestpart to 'remove' signals from iopart, same previous queue */
+#define IOCHAN_FROM_IOPART 1
 
 /* size of cdb - i.e., scsi cmnd */
 #define MAX_CMND_SIZE 16
@@ -92,26 +87,29 @@
 /* various types of network packets that can be sent in cmdrsp */
 enum net_types {
 	NET_RCV_POST = 0,	/* submit buffer to hold receiving
-				 * incoming packet */
+				 * incoming packet
+				 */
 	/* virtnic -> uisnic */
 	NET_RCV,		/* incoming packet received */
 	/* uisnic -> virtpci */
-	NET_XMIT,		/* for outgoing net packets      */
+	NET_XMIT,		/* for outgoing net packets */
 	/* virtnic -> uisnic */
 	NET_XMIT_DONE,		/* outgoing packet xmitted */
 	/* uisnic -> virtpci */
 	NET_RCV_ENBDIS,		/* enable/disable packet reception */
 	/* virtnic -> uisnic */
-	NET_RCV_ENBDIS_ACK,	/* acknowledge enable/disable packet
-				 * reception */
+	NET_RCV_ENBDIS_ACK,	/* acknowledge enable/disable packet */
+				/* reception */
 	/* uisnic -> virtnic */
 	NET_RCV_PROMISC,	/* enable/disable promiscuous mode */
 	/* virtnic -> uisnic */
 	NET_CONNECT_STATUS,	/* indicate the loss or restoration of a network
-				 * connection */
+				 * connection
+				 */
 	/* uisnic -> virtnic */
 	NET_MACADDR,		/* indicates the client has requested to update
-				 * its MAC addr */
+				 * its MAC addr
+				 */
 	NET_MACADDR_ACK,	/* MAC address */
 
 };
@@ -170,51 +168,43 @@ struct vhba_wwnn {
 } __packed;
 
 /* WARNING: Values stired in this structure must contain maximum counts (not
- * maximum values). */
-struct vhba_config_max {	/* 20 bytes */
-	u32 max_channel;	/* maximum channel for devices attached to this
-				 * bus */
-	u32 max_id;		/* maximum SCSI ID for devices attached to this
-				 * bus */
-	u32 max_lun;		/* maximum SCSI LUN for devices attached to this
-				 * bus */
-	u32 cmd_per_lun;	/* maximum number of outstanding commands per
-				 * lun that are allowed at one time */
-	u32 max_io_size;	/* maximum io size for devices attached to this
-				 * bus */
+ * maximum values).
+ */
+struct vhba_config_max {/* 20 bytes */
+	u32 max_channel;/* maximum channel for devices attached to this bus */
+	u32 max_id;	/* maximum SCSI ID for devices attached to bus */
+	u32 max_lun;	/* maximum SCSI LUN for devices attached to bus */
+	u32 cmd_per_lun;/* maximum number of outstanding commands per LUN */
+	u32 max_io_size;/* maximum io size for devices attached to this bus */
 	/* max io size is often determined by the resource of the hba. e.g */
 	/* max scatter gather list length * page size / sector size */
 } __packed;
 
 struct uiscmdrsp_scsi {
-	u64 handle;		/* the handle to the cmd that was received -
-				 * send it back as is in the rsp packet.  */
+	u64 handle;		/* the handle to the cmd that was received */
+				/* send it back as is in the rsp packet.  */
 	u8 cmnd[MAX_CMND_SIZE];	/* the cdb for the command */
 	u32 bufflen;		/* length of data to be transferred out or in */
-	u16 guest_phys_entries;	/* Number of entries in scatter-gather (sg)
-				 * list */
+	u16 guest_phys_entries;	/* Number of entries in scatter-gather list */
 	struct guest_phys_info gpi_list[MAX_PHYS_INFO];	/* physical address
 							 * information for each
-							 * fragment */
+							 * fragment
+							 */
 	enum dma_data_direction  data_dir; /* direction of the data, if any */
-	struct uisscsi_dest vdest;	/* identifies the virtual hba, id,
-					 * channel, lun to which cmd was sent */
+	struct uisscsi_dest vdest;	/* identifies the virtual hba, id, */
+					/* channel, lun to which cmd was sent */
 
-	    /* the following fields are needed to queue the rsp back to cmd
-	     * originator */
-	int linuxstat;		/* the original Linux status - for use by linux
-				 * vdisk code */
+	/* Needed to queue the rsp back to cmd originator */
+	int linuxstat;		/* original Linux status used by linux vdisk */
 	u8 scsistat;		/* the scsi status */
-	u8 addlstat;		/* non-scsi status - covers cases like timeout
-				 * needed by windows guests */
+	u8 addlstat;		/* non-scsi status */
 #define ADDL_SEL_TIMEOUT	4
 
 	/* the following fields are need to determine the result of command */
 	 u8 sensebuf[MAX_SENSE_SIZE];	/* sense info in case cmd failed; */
 	/* it holds the sense_data struct; */
 	/* see that struct for details. */
-	void *vdisk; /* contains pointer to the vdisk so that we can clean up
-		      * when the IO completes. */
+	void *vdisk; /* pointer to the vdisk to clean up when IO completes. */
 	int no_disk_result;
 	/* used to return no disk inquiry result
 	 * when no_disk_result is set to 1,
@@ -258,15 +248,15 @@ struct uiscmdrsp_scsi {
  */
 #define NO_DISK_INQUIRY_RESULT_LEN 36
 
-#define MIN_INQUIRY_RESULT_LEN 5 /* we need at least 5 bytes minimum for inquiry
-				  * result */
+#define MIN_INQUIRY_RESULT_LEN 5 /* 5 bytes minimum for inquiry result */
 
 /* SCSI device version for no disk inquiry result */
 #define SCSI_SPC2_VER 4		/* indicates SCSI SPC2 (SPC3 is 5) */
 
 /* Windows and Linux want different things for a non-existent lun. So, we'll let
  * caller pass in the peripheral qualifier and type.
- * NOTE:[4] SCSI returns (n-4); so we return length-1-4 or length-5. */
+ * NOTE:[4] SCSI returns (n-4); so we return length-1-4 or length-5.
+ */
 
 #define SET_NO_DISK_INQUIRY_RESULT(buf, len, lun, lun0notpresent, notpresent) \
 	do {								\
@@ -305,9 +295,7 @@ struct uiscmdrsp_scsi {
 		}							\
 	} while (0)
 
-/*
- * Struct & Defines to support sense information.
- */
+/* Struct & Defines to support sense information. */
 
 /* The following struct is returned in sensebuf field in uiscmdrsp_scsi.  It is
  * initialized in exactly the manner that is recommended in Windows (hence the
@@ -342,13 +330,11 @@ struct sense_data {
 struct net_pkt_xmt {
 	int len;	/* full length of data in the packet */
 	int num_frags;	/* number of fragments in frags containing data */
-	struct phys_info frags[MAX_PHYS_INFO];	/* physical page information for
-						 * each fragment */
+	struct phys_info frags[MAX_PHYS_INFO];	/* physical page information */
 	char ethhdr[ETH_HEADER_SIZE];	/* the ethernet header  */
 	struct {
-		    /* these are needed for csum at uisnic end */
-		u8 valid;	/* 1 = rest of this struct is valid - else
-				 * ignore */
+		/* these are needed for csum at uisnic end */
+		u8 valid;	/* 1 = struct is valid - else ignore */
 		u8 hrawoffv;	/* 1 = hwrafoff is valid */
 		u8 nhrawoffv;	/* 1 = nhwrafoff is valid */
 		u16 protocol;	/* specifies packet protocol */
@@ -380,16 +366,18 @@ struct net_pkt_xmtdone {
  */
 #define RCVPOST_BUF_SIZE 4032
 #define MAX_NET_RCV_CHAIN \
-	((ETH_MAX_MTU+ETH_HEADER_SIZE + RCVPOST_BUF_SIZE-1) / RCVPOST_BUF_SIZE)
+	((ETH_MAX_MTU + ETH_HEADER_SIZE + RCVPOST_BUF_SIZE - 1) \
+	/ RCVPOST_BUF_SIZE)
 
 struct net_pkt_rcvpost {
 	    /* rcv buf size must be large enough to include ethernet data len +
 	     * ethernet header len - we are choosing 2K because it is guaranteed
-	     * to be describable */
-	    struct phys_info frag;	/* physical page information for the
-					 * single fragment 2K rcv buf */
-	    u64 unique_num;		/* This is used to make sure that
-					 * receive posts are returned to  */
+	     * to be describable
+	     */
+	    struct phys_info frag;	/* physical page information for the */
+					/* single fragment 2K rcv buf */
+	    u64 unique_num;
+	    /* unique_num ensure that receive posts are returned to */
 	    /* the Adapter which we sent them originally. */
 } __packed;
 
@@ -399,8 +387,7 @@ struct net_pkt_rcv {
 	u32 rcv_done_len;	/* length of received data */
 	u8 numrcvbufs;		/* number of receive buffers that contain the */
 	/* incoming data; guest end MUST chain these together. */
-	void *rcvbuf[MAX_NET_RCV_CHAIN];	/* the list of receive buffers
-						 * that must be chained; */
+	void *rcvbuf[MAX_NET_RCV_CHAIN];	/* list of chained rcvbufs */
 	/* each entry is a receive buffer provided by NET_RCV_POST. */
 	/* NOTE: first rcvbuf in the chain will also be provided in net.buf. */
 	u64 unique_num;
@@ -469,18 +456,17 @@ struct uiscmdrsp_scsitaskmgmt {
 #define TASK_MGMT_FAILED  0
 } __packed;
 
-/* The following is used by uissd to send disk add/remove notifications to
- * Guest */
+/* Used by uissd to send disk add/remove notifications to Guest */
 /* Note that the vHba pointer is not used by the Client/Guest side. */
 struct uiscmdrsp_disknotify {
 	u8 add;			/* 0-remove, 1-add */
-	void *v_hba;		/* Pointer to vhba_info for channel info to
-				 * route msg */
+	void *v_hba;		/* channel info to route msg */
 	u32 channel, id, lun;	/* SCSI Path of Disk to added or removed */
 } __packed;
 
 /* The following is used by virthba/vSCSI to send the Acquire/Release commands
- * to the IOVM. */
+ * to the IOVM.
+ */
 struct uiscmdrsp_vdiskmgmt {
 	enum vdisk_mgmt_types vdisktype;
 
@@ -533,8 +519,8 @@ struct uiscmdrsp {
 		struct uiscmdrsp_disknotify disknotify;
 		struct uiscmdrsp_vdiskmgmt vdiskmgmt;
 	};
-	void *private_data;	/* used to send the response when the cmd is
-				 * done (scsi & scsittaskmgmt). */
+	void *private_data;	/* send the response when the cmd is */
+				/* done (scsi & scsittaskmgmt). */
 	struct uiscmdrsp *next;	/* General Purpose Queue Link */
 	struct uiscmdrsp *activeQ_next;	/* Used to track active commands */
 	struct uiscmdrsp *activeQ_prev;	/* Used to track active commands */
@@ -564,15 +550,11 @@ struct spar_io_channel_protocol {
 	} __packed;
 
 #define MAX_CLIENTSTRING_LEN 1024
-	 u8 client_string[MAX_CLIENTSTRING_LEN];/* NULL terminated - so holds
-						 * max - 1 bytes */
+	/* client_string is NULL termimated so holds max -1 bytes */
+	 u8 client_string[MAX_CLIENTSTRING_LEN];
 } __packed;
 
-
-/*
- * INLINE functions for initializing and accessing I/O data channels
- */
-
+/* INLINE functions for initializing and accessing I/O data channels */
 #define SIZEOF_PROTOCOL (COVER(sizeof(struct spar_io_channel_protocol), 64))
 #define SIZEOF_CMDRSP (COVER(sizeof(struct uiscmdrsp), 64))
 
@@ -584,8 +566,7 @@ struct spar_io_channel_protocol {
  * pfn-off-size entires.
  */
 
-/* we deal with 4K page sizes when we it comes to passing page information
- * between */
+/* use 4K page sizes when we it comes to passing page information between */
 /* Guest and IOPartition. */
 #define PI_PAGE_SIZE  0x1000
 #define PI_PAGE_MASK  0x0FFF
@@ -594,18 +575,8 @@ struct spar_io_channel_protocol {
  * room)
  */
 static inline  u16
-add_physinfo_entries(u32 inp_pfn,	/* input - specifies the pfn to be used
-					 * to add entries */
-		     u16 inp_off,	/* input - specifies the off to be used
-					 * to add entries */
-		     u32 inp_len,	/* input - specifies the len to be used
-					 * to add entries */
-		     u16 index,		/* input - index in array at which new
-					 * entries are added */
-		     u16 max_pi_arr_entries,	/* input - specifies the maximum
-						 * entries pi_arr can hold */
-		     struct phys_info pi_arr[]) /* input & output - array to
-						  * which entries are added */
+add_physinfo_entries(u32 inp_pfn, u16 inp_off, u32 inp_len, u16 index,
+		     u16 max_pi_arr_entries, struct phys_info pi_arr[])
 {
 	u32 len;
 	u16 i, firstlen;
diff --git a/drivers/staging/unisys/include/vbushelper.h b/drivers/staging/unisys/include/vbushelper.h
index f272975b2920..f1b6aacb79d7 100644
--- a/drivers/staging/unisys/include/vbushelper.h
+++ b/drivers/staging/unisys/include/vbushelper.h
@@ -19,7 +19,8 @@
 #define __VBUSHELPER_H__
 
 /* TARGET_HOSTNAME specified as -DTARGET_HOSTNAME=\"thename\" on the
- * command line */
+ * command line
+ */
 
 #define TARGET_HOSTNAME "linuxguest"
 
diff --git a/drivers/staging/unisys/include/visorbus.h b/drivers/staging/unisys/include/visorbus.h
index 9235536fa75f..2a64a9ce0208 100644
--- a/drivers/staging/unisys/include/visorbus.h
+++ b/drivers/staging/unisys/include/visorbus.h
@@ -140,8 +140,8 @@ struct visor_device {
 	struct {
 		int major, minor;
 		void *attr;	/* private use by devmajorminor_attr.c you can
-				   * change this constant to whatever you
-				   * want; */
+				 * change this constant to whatever you want
+				 */
 	} devnodes[5];
 	/* the code will detect and behave appropriately) */
 	struct semaphore visordriver_callback_lock;
diff --git a/drivers/staging/unisys/visorbus/Kconfig b/drivers/staging/unisys/visorbus/Kconfig
index 9b299ac86015..511388075ffa 100644
--- a/drivers/staging/unisys/visorbus/Kconfig
+++ b/drivers/staging/unisys/visorbus/Kconfig
@@ -6,4 +6,9 @@ config UNISYS_VISORBUS
 	tristate "Unisys visorbus driver"
 	depends on UNISYSSPAR
 	---help---
-	If you say Y here, you will enable the Unisys visorbus driver.
+	The visorbus driver is a virtualized bus for the Unisys s-Par firmware.
+	Virtualized devices allow Linux guests on a system to share disks and
+	network cards that do not have SR-IOV support, and to be accessed using
+	the partition desktop application. The visorbus driver is required to
+	discover devices on an s-Par guest, and must be present for any other
+	s-Par guest driver to function correctly.
diff --git a/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h b/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h
index 3c97ebac4f32..23ad0ea6c9fc 100644
--- a/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h
+++ b/drivers/staging/unisys/visorbus/controlvmcompletionstatus.h
@@ -39,7 +39,8 @@
 #define CONTROLVM_RESP_ERROR_MAX_DEVICES        202	/* DEVICE_CREATE */
 /* Payload and Parameter Related------------------------------------[400-499] */
 #define CONTROLVM_RESP_ERROR_PAYLOAD_INVALID	400	/* SWITCH_ATTACHEXTPORT,
-							 * DEVICE_CONFIGURE */
+							 * DEVICE_CONFIGURE
+							 */
 #define CONTROLVM_RESP_ERROR_INITIATOR_PARAMETER_INVALID 401	/* Multiple */
 #define CONTROLVM_RESP_ERROR_TARGET_PARAMETER_INVALID 402 /* DEVICE_CONFIGURE */
 #define CONTROLVM_RESP_ERROR_CLIENT_PARAMETER_INVALID 403 /* DEVICE_CONFIGURE */
@@ -48,36 +49,43 @@
 							 * BUS_CONFIGURE,
 							 * DEVICE_CREATE,
 							 * DEVICE_CONFIG
-							 * DEVICE_DESTROY */
+							 * DEVICE_DESTROY
+							 */
 #define CONTROLVM_RESP_ERROR_DEVICE_INVALID	501 /* SWITCH_ATTACHINTPORT */
 						    /* DEVICE_CREATE,
 						     * DEVICE_CONFIGURE,
-						     * DEVICE_DESTROY */
+						     * DEVICE_DESTROY
+						     */
 #define CONTROLVM_RESP_ERROR_CHANNEL_INVALID	502 /* DEVICE_CREATE,
-						     * DEVICE_CONFIGURE */
+						     * DEVICE_CONFIGURE
+						     */
 /* Partition Driver Callback Interface----------------------[600-699] */
 #define CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE 604	/* BUS_CREATE,
 							 * BUS_DESTROY,
 							 * DEVICE_CREATE,
-							 * DEVICE_DESTROY */
+							 * DEVICE_DESTROY
+							 */
 /* Unable to invoke VIRTPCI callback */
 #define CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR 605
 							/* BUS_CREATE,
 							 * BUS_DESTROY,
 							 * DEVICE_CREATE,
-							 * DEVICE_DESTROY */
+							 * DEVICE_DESTROY
+							 */
 /* VIRTPCI Callback returned error */
 #define CONTROLVM_RESP_ERROR_GENERIC_DRIVER_CALLBACK_ERROR 606
 							/* SWITCH_ATTACHEXTPORT,
 							 * SWITCH_DETACHEXTPORT
-							 * DEVICE_CONFIGURE */
+							 * DEVICE_CONFIGURE
+							 */
 
 /* generic device callback returned error */
 /* Bus Related------------------------------------------------------[700-799] */
 #define CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED 700	/* BUS_DESTROY */
 /* Channel Related--------------------------------------------------[800-899] */
 #define CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN 800	/* GET_CHANNELINFO,
-							 * DEVICE_DESTROY */
+							 * DEVICE_DESTROY
+							 */
 #define CONTROLVM_RESP_ERROR_CHANNEL_SIZE_TOO_SMALL 801	/* DEVICE_CREATE */
 /* Chipset Shutdown Related---------------------------------------[1000-1099] */
 #define CONTROLVM_RESP_ERROR_CHIPSET_SHUTDOWN_FAILED            1000
diff --git a/drivers/staging/unisys/visorbus/periodic_work.c b/drivers/staging/unisys/visorbus/periodic_work.c
index 115b7aa9560a..00b152764f84 100644
--- a/drivers/staging/unisys/visorbus/periodic_work.c
+++ b/drivers/staging/unisys/visorbus/periodic_work.c
@@ -43,11 +43,12 @@ static void periodic_work_func(struct work_struct *work)
 	(*pw->workfunc)(pw->workfuncarg);
 }
 
-struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
-					struct workqueue_struct *workqueue,
-					void (*workfunc)(void *),
-					void *workfuncarg,
-					const char *devnam)
+struct periodic_work
+*visor_periodic_work_create(ulong jiffy_interval,
+			    struct workqueue_struct *workqueue,
+			    void (*workfunc)(void *),
+			    void *workfuncarg,
+			    const char *devnam)
 {
 	struct periodic_work *pw;
 
diff --git a/drivers/staging/unisys/visorbus/vbuschannel.h b/drivers/staging/unisys/visorbus/vbuschannel.h
index 80e64477e547..90fa12e62f26 100644
--- a/drivers/staging/unisys/visorbus/vbuschannel.h
+++ b/drivers/staging/unisys/visorbus/vbuschannel.h
@@ -36,10 +36,11 @@ static const uuid_le spar_vbus_channel_protocol_uuid =
 #define SPAR_VBUS_CHANNEL_PROTOCOL_SIGNATURE ULTRA_CHANNEL_PROTOCOL_SIGNATURE
 
 /* Must increment this whenever you insert or delete fields within this channel
-* struct.  Also increment whenever you change the meaning of fields within this
-* channel struct so as to break pre-existing software.  Note that you can
-* usually add fields to the END of the channel struct withOUT needing to
-* increment this. */
+ * struct.  Also increment whenever you change the meaning of fields within this
+ * channel struct so as to break pre-existing software.  Note that you can
+ * usually add fields to the END of the channel struct withOUT needing to
+ * increment this.
+ */
 #define SPAR_VBUS_CHANNEL_PROTOCOL_VERSIONID 1
 
 #define SPAR_VBUS_CHANNEL_OK_CLIENT(ch)       \
diff --git a/drivers/staging/unisys/visorbus/visorbus_main.c b/drivers/staging/unisys/visorbus/visorbus_main.c
index a272b48bab28..eac97d22278a 100644
--- a/drivers/staging/unisys/visorbus/visorbus_main.c
+++ b/drivers/staging/unisys/visorbus/visorbus_main.c
@@ -1078,7 +1078,8 @@ away:
 }
 
 /* Write the contents of <info> to the struct
- * spar_vbus_channel_protocol.chp_info. */
+ * spar_vbus_channel_protocol.chp_info.
+ */
 
 static int
 write_vbus_chp_info(struct visorchannel *chan,
@@ -1096,7 +1097,8 @@ write_vbus_chp_info(struct visorchannel *chan,
 }
 
 /* Write the contents of <info> to the struct
- * spar_vbus_channel_protocol.bus_info. */
+ * spar_vbus_channel_protocol.bus_info.
+ */
 
 static int
 write_vbus_bus_info(struct visorchannel *chan,
@@ -1370,7 +1372,8 @@ pause_state_change_complete(struct visor_device *dev, int status)
 
 	/* Notify the chipset driver that the pause is complete, which
 	* will presumably want to send some sort of response to the
-	* initiator. */
+	* initiator.
+	*/
 	(*chipset_responders.device_pause) (dev, status);
 }
 
@@ -1390,7 +1393,8 @@ resume_state_change_complete(struct visor_device *dev, int status)
 
 	/* Notify the chipset driver that the resume is complete,
 	 * which will presumably want to send some sort of response to
-	 * the initiator. */
+	 * the initiator.
+	 */
 	(*chipset_responders.device_resume) (dev, status);
 }
 
@@ -1437,7 +1441,8 @@ initiate_chipset_device_pause_resume(struct visor_device *dev, bool is_pause)
 		 * existing problem prevents us from ever getting a bus
 		 * resume...  This hack would fail to work should we
 		 * ever have a bus that contains NO devices, since we
-		 * would never even get here in that case. */
+		 * would never even get here in that case.
+		 */
 		fix_vbus_dev_info(dev);
 		if (!drv->resume)
 			goto away;
diff --git a/drivers/staging/unisys/visorbus/visorchannel.c b/drivers/staging/unisys/visorbus/visorchannel.c
index a4e117f101cf..891b8db7c5ec 100644
--- a/drivers/staging/unisys/visorbus/visorchannel.c
+++ b/drivers/staging/unisys/visorbus/visorchannel.c
@@ -41,8 +41,8 @@ struct visorchannel {
 	struct channel_header chan_hdr;
 	uuid_le guid;
 	ulong size;
-	bool needs_lock;	/* channel creator knows if more than one
-				 * thread will be inserting or removing */
+	bool needs_lock;	/* channel creator knows if more than one */
+				/* thread will be inserting or removing */
 	spinlock_t insert_lock; /* protect head writes in chan_hdr */
 	spinlock_t remove_lock;	/* protect tail writes in chan_hdr */
 
diff --git a/drivers/staging/unisys/visorbus/vmcallinterface.h b/drivers/staging/unisys/visorbus/vmcallinterface.h
index c8d8483bd4df..c043fa41ceda 100644
--- a/drivers/staging/unisys/visorbus/vmcallinterface.h
+++ b/drivers/staging/unisys/visorbus/vmcallinterface.h
@@ -43,20 +43,15 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 	     * - the 0x01 identifies it as the 1st instance of a VMCALL_VIRTPART
 	     *   type of VMCALL
 	     */
-
-	VMCALL_IO_CONTROLVM_ADDR = 0x0501,	/* used by all Guests, not just
-						 * IO */
-	VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET = 0x0708, /* Allow caller to
-							  * query virtual time
-							  * offset */
-	VMCALL_POST_CODE_LOGEVENT = 0x070B,	/* LOGEVENT Post Code (RDX) with
-						 * specified subsystem mask (RCX
-						 * - monitor_subsystems.h) and
-						 * severity (RDX) */
-	VMCALL_UPDATE_PHYSICAL_TIME = 0x0a02	/* Allow
-						 * ULTRA_SERVICE_CAPABILITY_TIME
-						 * capable guest to make
-						 * VMCALL */
+	/* used by all Guests, not just IO */
+	VMCALL_IO_CONTROLVM_ADDR = 0x0501,
+	/* Allow caller to query virtual time offset */
+	VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET = 0x0708,
+	/* LOGEVENT Post Code (RDX) with specified subsystem mask */
+	/* (RCX - monitor_subsystems.h) and severity (RDX) */
+	VMCALL_POST_CODE_LOGEVENT = 0x070B,
+	/* Allow ULTRA_SERVICE_CAPABILITY_TIME capable guest to make VMCALL */
+	VMCALL_UPDATE_PHYSICAL_TIME = 0x0a02
 };
 
 #define VMCALL_SUCCESS 0
@@ -74,7 +69,8 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 	unisys_extended_vmcall(method, param1, param2, param3)
 
     /* The following uses VMCALL_POST_CODE_LOGEVENT interface but is currently
-     * not used much */
+     * not used much
+     */
 #define ISSUE_IO_VMCALL_POSTCODE_SEVERITY(postcode, severity)		\
 	ISSUE_IO_EXTENDED_VMCALL(VMCALL_POST_CODE_LOGEVENT, severity,	\
 				 MDS_APPOS, postcode)
@@ -84,11 +80,11 @@ enum vmcall_monitor_interface_method_tuple { /* VMCALL identification tuples  */
 
 /* Parameters to VMCALL_IO_CONTROLVM_ADDR interface */
 struct vmcall_io_controlvm_addr_params {
-	    /* The Guest-relative physical address of the ControlVm channel.
-	    * This VMCall fills this in with the appropriate address. */
+	/* The Guest-relative physical address of the ControlVm channel. */
+	/* This VMCall fills this in with the appropriate address. */
 	u64 address;	/* contents provided by this VMCALL (OUT) */
-	    /* the size of the ControlVm channel in bytes This VMCall fills this
-	    * in with the appropriate address. */
+	/* the size of the ControlVm channel in bytes This VMCall fills this */
+	/* in with the appropriate address. */
 	u32 channel_bytes;	/* contents provided by this VMCALL (OUT) */
 	u8 unused[4];		/* Unused Bytes in the 64-Bit Aligned Struct */
 } __packed;
diff --git a/drivers/staging/unisys/visorhba/visorhba_main.c b/drivers/staging/unisys/visorhba/visorhba_main.c
index c119f20dfd44..d5178b44ba8c 100644
--- a/drivers/staging/unisys/visorhba/visorhba_main.c
+++ b/drivers/staging/unisys/visorhba/visorhba_main.c
@@ -453,7 +453,6 @@ visorhba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	struct uiscmdrsp *cmdrsp;
 	struct scsi_device *scsidev = scsicmd->device;
 	int insert_location;
-	unsigned char op;
 	unsigned char *cdb = scsicmd->cmnd;
 	struct Scsi_Host *scsihost = scsidev->host;
 	unsigned int i;
@@ -461,7 +460,6 @@ visorhba_queue_command_lck(struct scsi_cmnd *scsicmd,
 		(struct visorhba_devdata *)scsihost->hostdata;
 	struct scatterlist *sg = NULL;
 	struct scatterlist *sglist = NULL;
-	int err = 0;
 
 	if (devdata->serverdown || devdata->serverchangingstate)
 		return SCSI_MLQUEUE_DEVICE_BUSY;
@@ -496,10 +494,8 @@ visorhba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	if (cmdrsp->scsi.bufflen > devdata->max_buff_len)
 		devdata->max_buff_len = cmdrsp->scsi.bufflen;
 
-	if (scsi_sg_count(scsicmd) > MAX_PHYS_INFO) {
-		err = SCSI_MLQUEUE_DEVICE_BUSY;
+	if (scsi_sg_count(scsicmd) > MAX_PHYS_INFO)
 		goto err_del_scsipending_ent;
-	}
 
 	/* convert buffer to phys information  */
 	/* buffer is scatterlist - copy it out */
@@ -511,19 +507,17 @@ visorhba_queue_command_lck(struct scsi_cmnd *scsicmd,
 	}
 	cmdrsp->scsi.guest_phys_entries = scsi_sg_count(scsicmd);
 
-	op = cdb[0];
 	if (!visorchannel_signalinsert(devdata->dev->visorchannel,
 				       IOCHAN_TO_IOPART,
-				       cmdrsp)) {
+				       cmdrsp))
 		/* queue must be full and we aren't going to wait */
-		err = SCSI_MLQUEUE_DEVICE_BUSY;
 		goto err_del_scsipending_ent;
-	}
+
 	return 0;
 
 err_del_scsipending_ent:
 	del_scsipending_ent(devdata, insert_location);
-	return err;
+	return SCSI_MLQUEUE_DEVICE_BUSY;
 }
 
 /**
@@ -759,11 +753,9 @@ do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 	struct visorhba_devdata *devdata;
 	struct visordisk_info *vdisk;
 	struct scsi_device *scsidev;
-	struct sense_data *sd;
 
 	scsidev = scsicmd->device;
 	memcpy(scsicmd->sense_buffer, cmdrsp->scsi.sensebuf, MAX_SENSE_SIZE);
-	sd = (struct sense_data *)scsicmd->sense_buffer;
 
 	/* Do not log errors for disk-not-present inquiries */
 	if ((cmdrsp->scsi.cmnd[0] == INQUIRY) &&
diff --git a/drivers/staging/unisys/visorinput/Kconfig b/drivers/staging/unisys/visorinput/Kconfig
index d83deb4137e8..3476d419d32c 100644
--- a/drivers/staging/unisys/visorinput/Kconfig
+++ b/drivers/staging/unisys/visorinput/Kconfig
@@ -6,5 +6,10 @@ config UNISYS_VISORINPUT
 	tristate "Unisys visorinput driver"
 	depends on UNISYSSPAR && UNISYS_VISORBUS && FB
 	---help---
-	If you say Y here, you will enable the Unisys visorinput driver.
+	The Unisys s-Par visorinput driver provides a virtualized system
+	console (keyboard and mouse) that is accessible through the
+	s-Par firmware's user interface. s-Par provides video using the EFI
+	GOP protocol, so If this driver is not present, the Linux guest should
+	still boot with visible output in the partition desktop, but keyboard
+	and mouse interaction will not be available.
 
diff --git a/drivers/staging/unisys/visorinput/visorinput.c b/drivers/staging/unisys/visorinput/visorinput.c
index 5c16f6634368..38d4d5b884df 100644
--- a/drivers/staging/unisys/visorinput/visorinput.c
+++ b/drivers/staging/unisys/visorinput/visorinput.c
@@ -523,7 +523,7 @@ visorinput_channel_interrupt(struct visor_device *dev)
 	struct ultra_inputreport r;
 	int scancode, keycode;
 	struct input_dev *visorinput_dev;
-	int xmotion, ymotion, zmotion, button;
+	int xmotion, ymotion, button;
 	int i;
 
 	struct visorinput_devdata *devdata = dev_get_drvdata(&dev->device);
@@ -604,12 +604,10 @@ visorinput_channel_interrupt(struct visor_device *dev)
 			}
 			break;
 		case inputaction_wheel_rotate_away:
-			zmotion = r.activity.arg1;
 			input_report_rel(visorinput_dev, REL_WHEEL, 1);
 			input_sync(visorinput_dev);
 			break;
 		case inputaction_wheel_rotate_toward:
-			zmotion = r.activity.arg1;
 			input_report_rel(visorinput_dev, REL_WHEEL, -1);
 			input_sync(visorinput_dev);
 			break;
diff --git a/drivers/staging/unisys/visornic/visornic_main.c b/drivers/staging/unisys/visornic/visornic_main.c
index 296b11cea247..05194707278a 100644
--- a/drivers/staging/unisys/visornic/visornic_main.c
+++ b/drivers/staging/unisys/visornic/visornic_main.c
@@ -1742,7 +1742,6 @@ poll_for_irq(unsigned long v)
 	atomic_set(&devdata->interrupt_rcvd, 0);
 
 	mod_timer(&devdata->irq_poll_timer, msecs_to_jiffies(2));
-
 }
 
 /**
diff --git a/drivers/staging/vme/devices/vme_pio2_cntr.c b/drivers/staging/vme/devices/vme_pio2_cntr.c
index 6335471faa36..486c30c4956f 100644
--- a/drivers/staging/vme/devices/vme_pio2_cntr.c
+++ b/drivers/staging/vme/devices/vme_pio2_cntr.c
@@ -61,7 +61,7 @@ int pio2_cntr_reset(struct pio2_card *card)
 	/* Ensure all counter interrupts are cleared */
 	do {
 		retval = vme_master_read(card->window, &reg, 1,
-			PIO2_REGS_INT_STAT_CNTR);
+					 PIO2_REGS_INT_STAT_CNTR);
 		if (retval < 0)
 			return retval;
 	} while (reg != 0);
diff --git a/drivers/staging/vme/devices/vme_pio2_core.c b/drivers/staging/vme/devices/vme_pio2_core.c
index 35c6ce5047de..4f3cdbcedb3e 100644
--- a/drivers/staging/vme/devices/vme_pio2_core.c
+++ b/drivers/staging/vme/devices/vme_pio2_core.c
@@ -90,7 +90,7 @@ static void pio2_int(int level, int vector, void *ptr)
 	case 4:
 		/* Channels 0 to 7 */
 		retval = vme_master_read(card->window, &reg, 1,
-			PIO2_REGS_INT_STAT[vec - 1]);
+					 PIO2_REGS_INT_STAT[vec - 1]);
 		if (retval < 0) {
 			dev_err(&card->vdev->dev,
 				"Unable to read IRQ status register\n");
@@ -100,8 +100,8 @@ static void pio2_int(int level, int vector, void *ptr)
 			channel = ((vec - 1) * 8) + i;
 			if (reg & PIO2_CHANNEL_BIT[channel])
 				dev_info(&card->vdev->dev,
-					"Interrupt on I/O channel %d\n",
-					channel);
+					 "Interrupt on I/O channel %d\n",
+					 channel);
 		}
 		break;
 	case 5:
@@ -215,7 +215,7 @@ static int pio2_probe(struct vme_dev *vdev)
 	u8 reg;
 	int vec;
 
-	card = kzalloc(sizeof(struct pio2_card), GFP_KERNEL);
+	card = kzalloc(sizeof(*card), GFP_KERNEL);
 	if (!card) {
 		retval = -ENOMEM;
 		goto err_struct;
@@ -230,7 +230,7 @@ static int pio2_probe(struct vme_dev *vdev)
 	card->vdev = vdev;
 
 	for (i = 0; i < PIO2_VARIANT_LENGTH; i++) {
-		if (isdigit(card->variant[i]) == 0) {
+		if (!isdigit(card->variant[i])) {
 			dev_err(&card->vdev->dev, "Variant invalid\n");
 			retval = -EINVAL;
 			goto err_variant;
@@ -289,7 +289,7 @@ static int pio2_probe(struct vme_dev *vdev)
 	}
 
 	retval = vme_master_set(card->window, 1, card->base, 0x10000, VME_A24,
-		(VME_SCT | VME_USER | VME_DATA), VME_D16);
+				VME_SCT | VME_USER | VME_DATA, VME_D16);
 	if (retval) {
 		dev_err(&card->vdev->dev,
 			"Unable to configure VME master resource\n");
@@ -335,7 +335,7 @@ static int pio2_probe(struct vme_dev *vdev)
 
 	/* Set VME vector */
 	retval = vme_master_write(card->window, &card->irq_vector, 1,
-		PIO2_REGS_VME_VECTOR);
+				  PIO2_REGS_VME_VECTOR);
 	if (retval < 0)
 		return retval;
 
@@ -343,7 +343,7 @@ static int pio2_probe(struct vme_dev *vdev)
 	vec = card->irq_vector | PIO2_VME_VECTOR_SPUR;
 
 	retval = vme_irq_request(vdev, card->irq_level, vec,
-		&pio2_int, (void *)card);
+				 &pio2_int, card);
 	if (retval < 0) {
 		dev_err(&card->vdev->dev,
 			"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
@@ -356,7 +356,7 @@ static int pio2_probe(struct vme_dev *vdev)
 		vec = card->irq_vector | PIO2_VECTOR_BANK[i];
 
 		retval = vme_irq_request(vdev, card->irq_level, vec,
-			&pio2_int, (void *)card);
+					 &pio2_int, card);
 		if (retval < 0) {
 			dev_err(&card->vdev->dev,
 				"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
@@ -370,7 +370,7 @@ static int pio2_probe(struct vme_dev *vdev)
 		vec = card->irq_vector | PIO2_VECTOR_CNTR[i];
 
 		retval = vme_irq_request(vdev, card->irq_level, vec,
-			&pio2_int, (void *)card);
+			&pio2_int, card);
 		if (retval < 0) {
 			dev_err(&card->vdev->dev,
 				"Unable to attach VME interrupt vector0x%x, level 0x%x\n",
@@ -397,7 +397,7 @@ static int pio2_probe(struct vme_dev *vdev)
 	dev_set_drvdata(&card->vdev->dev, card);
 
 	dev_info(&card->vdev->dev,
-		"PIO2 (variant %s) configured at 0x%lx\n", card->variant,
+		 "PIO2 (variant %s) configured at 0x%lx\n", card->variant,
 		card->base);
 
 	return 0;
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
index 77901b345a71..df992c3cb5ce 100644
--- a/drivers/staging/vme/devices/vme_pio2_gpio.c
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -37,14 +37,13 @@ static int pio2_gpio_get(struct gpio_chip *chip, unsigned int offset)
 	struct pio2_card *card = gpio_to_pio2_card(chip);
 
 	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == OUTPUT) |
-		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
-
+	    (card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
 		dev_err(&card->vdev->dev, "Channel not available as input\n");
 		return 0;
 	}
 
 	retval = vme_master_read(card->window, &reg, 1,
-		PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
+				 PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
 	if (retval < 0) {
 		dev_err(&card->vdev->dev, "Unable to read from GPIO\n");
 		return 0;
@@ -67,16 +66,15 @@ static int pio2_gpio_get(struct gpio_chip *chip, unsigned int offset)
 	return 0;
 }
 
-static void pio2_gpio_set(struct gpio_chip *chip, unsigned int offset,
-	int value)
+static void pio2_gpio_set(struct gpio_chip *chip,
+			  unsigned int offset, int value)
 {
 	u8 reg;
 	int retval;
 	struct pio2_card *card = gpio_to_pio2_card(chip);
 
 	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == INPUT) |
-		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
-
+	    (card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
 		dev_err(&card->vdev->dev, "Channel not available as output\n");
 		return;
 	}
@@ -89,7 +87,7 @@ static void pio2_gpio_set(struct gpio_chip *chip, unsigned int offset,
 			~PIO2_CHANNEL_BIT[offset];
 
 	retval = vme_master_write(card->window, &reg, 1,
-		PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
+				  PIO2_REGS_DATA[PIO2_CHANNEL_BANK[offset]]);
 	if (retval < 0) {
 		dev_err(&card->vdev->dev, "Unable to write to GPIO\n");
 		return;
@@ -105,7 +103,7 @@ static int pio2_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 	struct pio2_card *card = gpio_to_pio2_card(chip);
 
 	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == OUTPUT) |
-		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+	    (card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
 		dev_err(&card->vdev->dev,
 			"Channel directionality not configurable at runtime\n");
 
@@ -124,7 +122,7 @@ static int pio2_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int value)
 	struct pio2_card *card = gpio_to_pio2_card(chip);
 
 	if ((card->bank[PIO2_CHANNEL_BANK[offset]].config == INPUT) |
-		(card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
+	    (card->bank[PIO2_CHANNEL_BANK[offset]].config == NOFIT)) {
 		dev_err(&card->vdev->dev,
 			"Channel directionality not configurable at runtime\n");
 
@@ -150,7 +148,7 @@ int pio2_gpio_reset(struct pio2_card *card)
 	/* Zero output registers */
 	for (i = 0; i < 4; i++) {
 		retval = vme_master_write(card->window, &data, 1,
-			PIO2_REGS_DATA[i]);
+					  PIO2_REGS_DATA[i]);
 		if (retval < 0)
 			return retval;
 		card->bank[i].value = 0;
@@ -159,12 +157,12 @@ int pio2_gpio_reset(struct pio2_card *card)
 	/* Set input interrupt masks */
 	for (i = 0; i < 4; i++) {
 		retval = vme_master_write(card->window, &data, 1,
-			PIO2_REGS_INT_MASK[i * 2]);
+					  PIO2_REGS_INT_MASK[i * 2]);
 		if (retval < 0)
 			return retval;
 
 		retval = vme_master_write(card->window, &data, 1,
-			PIO2_REGS_INT_MASK[(i * 2) + 1]);
+					  PIO2_REGS_INT_MASK[(i * 2) + 1]);
 		if (retval < 0)
 			return retval;
 
@@ -176,7 +174,7 @@ int pio2_gpio_reset(struct pio2_card *card)
 	for (i = 0; i < 4; i++) {
 		do {
 			retval = vme_master_read(card->window, &data, 1,
-				PIO2_REGS_INT_STAT[i]);
+						 PIO2_REGS_INT_STAT[i]);
 			if (retval < 0)
 				return retval;
 		} while (data != 0);
@@ -192,7 +190,7 @@ int pio2_gpio_init(struct pio2_card *card)
 
 	label = kasprintf(GFP_KERNEL,
 			  "%s@%s", driver_name, dev_name(&card->vdev->dev));
-	if (label == NULL)
+	if (!label)
 		return -ENOMEM;
 
 	card->gc.label = label;
@@ -207,7 +205,7 @@ int pio2_gpio_init(struct pio2_card *card)
 	card->gc.set = pio2_gpio_set;
 
 	/* This function adds a memory mapped GPIO chip */
-	retval = gpiochip_add(&(card->gc));
+	retval = gpiochip_add(&card->gc);
 	if (retval) {
 		dev_err(&card->vdev->dev, "Unable to register GPIO\n");
 		kfree(card->gc.label);
@@ -220,7 +218,7 @@ void pio2_gpio_exit(struct pio2_card *card)
 {
 	const char *label = card->gc.label;
 
-	gpiochip_remove(&(card->gc));
+	gpiochip_remove(&card->gc);
 	kfree(label);
 }
 
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 8e61a3b3e7e4..b95883bc68fe 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -308,8 +308,8 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 		switch (cmd) {
 		case VME_IRQ_GEN:
 			copied = copy_from_user(&irq_req, argp,
-						sizeof(struct vme_irq_id));
-			if (copied != 0) {
+						sizeof(irq_req));
+			if (copied) {
 				pr_warn("Partial copy from userspace\n");
 				return -EFAULT;
 			}
@@ -322,7 +322,7 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 	case MASTER_MINOR:
 		switch (cmd) {
 		case VME_GET_MASTER:
-			memset(&master, 0, sizeof(struct vme_master));
+			memset(&master, 0, sizeof(master));
 
 			/* XXX	We do not want to push aspace, cycle and width
 			 *	to userspace as they are
@@ -334,8 +334,8 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 						&master.cycle, &master.dwidth);
 
 			copied = copy_to_user(argp, &master,
-					      sizeof(struct vme_master));
-			if (copied != 0) {
+					      sizeof(master));
+			if (copied) {
 				pr_warn("Partial copy to userspace\n");
 				return -EFAULT;
 			}
@@ -350,7 +350,7 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 			}
 
 			copied = copy_from_user(&master, argp, sizeof(master));
-			if (copied != 0) {
+			if (copied) {
 				pr_warn("Partial copy from userspace\n");
 				return -EFAULT;
 			}
@@ -368,7 +368,7 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 	case SLAVE_MINOR:
 		switch (cmd) {
 		case VME_GET_SLAVE:
-			memset(&slave, 0, sizeof(struct vme_slave));
+			memset(&slave, 0, sizeof(slave));
 
 			/* XXX	We do not want to push aspace, cycle and width
 			 *	to userspace as they are
@@ -379,8 +379,8 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 					       &slave.aspace, &slave.cycle);
 
 			copied = copy_to_user(argp, &slave,
-					      sizeof(struct vme_slave));
-			if (copied != 0) {
+					      sizeof(slave));
+			if (copied) {
 				pr_warn("Partial copy to userspace\n");
 				return -EFAULT;
 			}
@@ -390,7 +390,7 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 		case VME_SET_SLAVE:
 
 			copied = copy_from_user(&slave, argp, sizeof(slave));
-			if (copied != 0) {
+			if (copied) {
 				pr_warn("Partial copy from userspace\n");
 				return -EFAULT;
 			}
@@ -757,7 +757,7 @@ static int __init vme_user_init(void)
 	 * we just change the code in vme_user_match().
 	 */
 	retval = vme_register_driver(&vme_user_driver, VME_MAX_SLOTS);
-	if (retval != 0)
+	if (retval)
 		goto err_reg;
 
 	return retval;
diff --git a/drivers/staging/vme/devices/vme_user.h b/drivers/staging/vme/devices/vme_user.h
index b8cc7bc78a73..a6cb75686fa4 100644
--- a/drivers/staging/vme/devices/vme_user.h
+++ b/drivers/staging/vme/devices/vme_user.h
@@ -20,7 +20,6 @@ struct vme_master {
 #endif
 } __packed;
 
-
 /*
  * IOCTL Commands and structures
  */
@@ -28,7 +27,6 @@ struct vme_master {
 /* Magic number for use in ioctls */
 #define VME_IOC_MAGIC 0xAE
 
-
 /* VMEbus Slave Window Configuration Structure */
 struct vme_slave {
 	__u32 enable;		/* State of Window */
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index e5be261f2e69..9417c935fc30 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: baseband.c
  *
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index 771ea4054174..807a5809b5d9 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: baseband.h
  *
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 927243ebcb56..a382fc6aa9d3 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: card.c
  * Purpose: Provide functions to setup NIC operation mode
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index 03fc1678896b..c2cde7e92c8f 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: card.h
  *
diff --git a/drivers/staging/vt6656/channel.c b/drivers/staging/vt6656/channel.c
index 8412d0532fb2..a0fe288c1322 100644
--- a/drivers/staging/vt6656/channel.c
+++ b/drivers/staging/vt6656/channel.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: channel.c
  *
diff --git a/drivers/staging/vt6656/channel.h b/drivers/staging/vt6656/channel.h
index 21c080803714..fcea6995fe26 100644
--- a/drivers/staging/vt6656/channel.h
+++ b/drivers/staging/vt6656/channel.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: channel.h
  *
diff --git a/drivers/staging/vt6656/desc.h b/drivers/staging/vt6656/desc.h
index f79af8513ff2..59e3071021bd 100644
--- a/drivers/staging/vt6656/desc.h
+++ b/drivers/staging/vt6656/desc.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: desc.h
  *
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index dec36f296f3d..76b5f4127f95 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: device.h
  *
diff --git a/drivers/staging/vt6656/dpc.c b/drivers/staging/vt6656/dpc.c
index e6367ed3b0bb..6019aac8bdd5 100644
--- a/drivers/staging/vt6656/dpc.c
+++ b/drivers/staging/vt6656/dpc.c
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: dpc.c
  *
diff --git a/drivers/staging/vt6656/dpc.h b/drivers/staging/vt6656/dpc.h
index 95e0e83a487e..5a92bd86cee2 100644
--- a/drivers/staging/vt6656/dpc.h
+++ b/drivers/staging/vt6656/dpc.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: dpc.h
  *
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index d440f284bf18..1b48f9c86f63 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: baseband.c
  *
diff --git a/drivers/staging/vt6656/firmware.h b/drivers/staging/vt6656/firmware.h
index d594dbe1c147..e2b54acb8fdb 100644
--- a/drivers/staging/vt6656/firmware.h
+++ b/drivers/staging/vt6656/firmware.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: firmware.h
  *
diff --git a/drivers/staging/vt6656/int.c b/drivers/staging/vt6656/int.c
index 14b8ebc6508d..8d05acbc0e23 100644
--- a/drivers/staging/vt6656/int.c
+++ b/drivers/staging/vt6656/int.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: int.c
  *
diff --git a/drivers/staging/vt6656/int.h b/drivers/staging/vt6656/int.h
index 154605c63947..97e55bacbb7c 100644
--- a/drivers/staging/vt6656/int.h
+++ b/drivers/staging/vt6656/int.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: int.h
  *
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index 181745d8e250..0246a8fc47fe 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: key.c
  *
@@ -163,7 +159,6 @@ int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 	}
 
-
 	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) {
 		vnt_set_keymode(hw, mac_addr, key, VNT_KEY_PAIRWISE,
 					key_dec_mode, true);
diff --git a/drivers/staging/vt6656/key.h b/drivers/staging/vt6656/key.h
index 3cb1291055ed..7861faf5138f 100644
--- a/drivers/staging/vt6656/key.h
+++ b/drivers/staging/vt6656/key.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: key.h
  *
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 5dfac05b9cf1..eeed16e9124e 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: mac.c
  *
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index d53fcef87b4a..4c6e610f1bc1 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: mac.h
  *
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 01e642db311e..ee8d1e1a24c2 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: main_usb.c
  *
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index 13afce27951c..c025dab0f62c 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: power.c
  *
diff --git a/drivers/staging/vt6656/power.h b/drivers/staging/vt6656/power.h
index 7696b714850c..9d1ebb695f9d 100644
--- a/drivers/staging/vt6656/power.h
+++ b/drivers/staging/vt6656/power.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: power.h
  *
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index c4286ccac320..816206c92f57 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: rf.c
  *
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index 3acdc65b1e56..c3d4f06d65f4 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: rf.h
  *
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index efb54f53b4f9..a0c69b697901 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: rxtx.c
  *
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
index 90b34ab2f6ce..4a79c404275b 100644
--- a/drivers/staging/vt6656/rxtx.h
+++ b/drivers/staging/vt6656/rxtx.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: rxtx.h
  *
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index c975c3b87093..351a99f3d684 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: usbpipe.c
  *
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
index e74aa0809928..8bafd9aee1fa 100644
--- a/drivers/staging/vt6656/usbpipe.h
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -12,10 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
- *
  *
  * File: usbpipe.h
  *
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 3cbf4791bac1..4846a898d39b 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: wcmd.c
  *
diff --git a/drivers/staging/vt6656/wcmd.h b/drivers/staging/vt6656/wcmd.h
index 2b0ee285eb0b..764c09ccd42d 100644
--- a/drivers/staging/vt6656/wcmd.h
+++ b/drivers/staging/vt6656/wcmd.h
@@ -12,9 +12,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  *
  * File: wcmd.h
  *
diff --git a/drivers/staging/wilc1000/Kconfig b/drivers/staging/wilc1000/Kconfig
index ee51b4278088..dce9cee9134a 100644
--- a/drivers/staging/wilc1000/Kconfig
+++ b/drivers/staging/wilc1000/Kconfig
@@ -1,41 +1,12 @@
-config WILC1000_DRIVER
-	bool "WILC1000 support (WiFi only)"
-	depends on CFG80211 && WEXT_CORE && INET
-	---help---
-	  This module only support IEEE 802.11n WiFi.
-
-if WILC1000_DRIVER
-
 config WILC1000
 	tristate
-
-choice
-        prompt "Memory Allocation"
-        default WILC1000_PREALLOCATE_AT_LOADING_DRIVER
-
-config WILC1000_PREALLOCATE_AT_LOADING_DRIVER
-	bool "Preallocate memory at loading driver"
+	select WIRELESS_EXT
 	---help---
-	  This choice supports static allocation of the memory
-	  for the receive buffer. The driver will allocate the RX buffer
-	  during initial time. The driver will also free the buffer
-	  by calling network device stop.
-
-config WILC1000_DYNAMICALLY_ALLOCATE_MEMROY
-        bool "Dynamically allocate memory in real time"
-        ---help---
-	  This choice supports dynamic allocation of the memory
-	  for the receive buffer. The driver will allocate the RX buffer
-	  when it is required.
-endchoice
-
-choice
-	prompt "Bus Type"
-	default WILC1000_SDIO
+	  This module only support IEEE 802.11n WiFi.
 
 config WILC1000_SDIO
-	bool "SDIO support"
-	depends on MMC
+	tristate "Atmel WILC1000 SDIO (WiFi only)"
+	depends on CFG80211 && INET && MMC
 	select WILC1000
 	---help---
 	  This module adds support for the SDIO interface of adapters using
@@ -48,9 +19,9 @@ config WILC1000_SDIO
 	  this if your platform is using the SDIO bus.
 
 config WILC1000_SPI
-	depends on SPI
+	tristate "Atmel WILC1000 SPI (WiFi only)"
+	depends on CFG80211 && INET && SPI
 	select WILC1000
-	bool "SPI support"
 	---help---
 	  This module adds support for the SPI interface of adapters using
 	  WILC1000 chipset. The Atmel WILC1000 has a Serial Peripheral
@@ -59,10 +30,9 @@ config WILC1000_SPI
 	  full-duplex slave synchronous serial interface that is available
 	  immediately following reset when pin 9 (SDIO_SPI_CFG) is tied to
 	  VDDIO. Select this if your platform is using the SPI bus.
-endchoice
 
 config WILC1000_HW_OOB_INTR
-	bool "Use out of band interrupt"
+	bool "WILC1000 out of band interrupt"
 	depends on WILC1000_SDIO
 	default n
 	---help---
@@ -71,5 +41,3 @@ config WILC1000_HW_OOB_INTR
 	  mechanism for SDIO host controllers that don't support SDIO interrupt.
 	  Select this option If the SDIO host controller in your platform
 	  doesn't support SDIO time devision interrupt.
-
-endif
diff --git a/drivers/staging/wilc1000/Makefile b/drivers/staging/wilc1000/Makefile
index 64c2f1b83dfb..20a5cb9d4f4c 100644
--- a/drivers/staging/wilc1000/Makefile
+++ b/drivers/staging/wilc1000/Makefile
@@ -1,28 +1,20 @@
 obj-$(CONFIG_WILC1000) += wilc1000.o
 
-ccflags-$(CONFIG_WILC1000_SDIO) += -DWILC_SDIO -DCOMPLEMENT_BOOT
-ccflags-$(CONFIG_WILC1000_HW_OOB_INTR) += -DWILC_SDIO_IRQ_GPIO
-ccflags-$(CONFIG_WILC1000_SPI) += -DWILC_SPI
-
 ccflags-y += -DSTA_FIRMWARE=\"atmel/wilc1000_fw.bin\" \
 		-DAP_FIRMWARE=\"atmel/wilc1000_ap_fw.bin\" \
 		-DP2P_CONCURRENCY_FIRMWARE=\"atmel/wilc1000_p2p_fw.bin\"
 
-ccflags-y += -I$(src)/ -D__CHECK_ENDIAN__ -DWILC_ASIC_A0 \
-		-Wno-unused-function -DWILC_DEBUGFS
+ccflags-y += -I$(src)/ -DWILC_ASIC_A0 -DWILC_DEBUGFS
 #ccflags-y += -DTCP_ACK_FILTER
 
-ccflags-$(CONFIG_WILC1000_PREALLOCATE_AT_LOADING_DRIVER) += -DMEMORY_STATIC \
-								-DWILC_PREALLOC_AT_INSMOD
-
-ccflags-$(CONFIG_WILC1000_DYNAMICALLY_ALLOCATE_MEMROY) += -DWILC_NORMAL_ALLOC
-
-
 wilc1000-objs := wilc_wfi_cfgoperations.o linux_wlan.o linux_mon.o \
 			wilc_msgqueue.o \
 			coreconfigurator.o host_interface.o \
-			wilc_sdio.o wilc_spi.o wilc_wlan_cfg.o wilc_debugfs.o \
+			wilc_wlan_cfg.o wilc_debugfs.o \
 			wilc_wlan.o
 
-wilc1000-$(CONFIG_WILC1000_SDIO) += linux_wlan_sdio.o
-wilc1000-$(CONFIG_WILC1000_SPI) += linux_wlan_spi.o
+obj-$(CONFIG_WILC1000_SDIO) += wilc1000-sdio.o
+wilc1000-sdio-objs += wilc_sdio.o
+
+obj-$(CONFIG_WILC1000_SPI) += wilc1000-spi.o
+wilc1000-spi-objs += wilc_spi.o
diff --git a/drivers/staging/wilc1000/coreconfigurator.c b/drivers/staging/wilc1000/coreconfigurator.c
index 9568bdb6319b..2d4d3f190c01 100644
--- a/drivers/staging/wilc1000/coreconfigurator.c
+++ b/drivers/staging/wilc1000/coreconfigurator.c
@@ -287,7 +287,7 @@ static inline u16 get_asoc_id(u8 *data)
 	return asoc_id;
 }
 
-u8 *get_tim_elm(u8 *pu8msa, u16 u16RxLen, u16 u16TagParamOffset)
+static u8 *get_tim_elm(u8 *pu8msa, u16 u16RxLen, u16 u16TagParamOffset)
 {
 	u16 u16index;
 
@@ -315,7 +315,7 @@ u8 *get_tim_elm(u8 *pu8msa, u16 u16RxLen, u16 u16TagParamOffset)
 
 /* This function gets the current channel information from
  * the 802.11n beacon/probe response frame */
-u8 get_current_channel_802_11n(u8 *pu8msa, u16 u16RxLen)
+static u8 get_current_channel_802_11n(u8 *pu8msa, u16 u16RxLen)
 {
 	u16 index;
 
@@ -344,7 +344,7 @@ u8 get_current_channel_802_11n(u8 *pu8msa, u16 u16RxLen)
  *  @date			1 Mar 2012
  *  @version		1.0
  */
-s32 parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo)
+s32 wilc_parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo)
 {
 	tstrNetworkInfo *pstrNetworkInfo = NULL;
 	u8 u8MsgType = 0;
@@ -436,7 +436,7 @@ s32 parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo)
 
 		/* Get DTIM Period */
 		pu8TimElm = get_tim_elm(pu8msa, u16RxLen + FCS_LEN, u8index);
-		if (pu8TimElm != NULL)
+		if (pu8TimElm)
 			pstrNetworkInfo->u8DtimPeriod = pu8TimElm[3];
 		pu8IEs = &pu8msa[MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN + CAP_INFO_LEN];
 		u16IEsLen = u16RxLen - (MAC_HDR_LEN + TIME_STAMP_LEN + BEACON_INTERVAL_LEN + CAP_INFO_LEN);
@@ -466,12 +466,12 @@ s32 parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo)
  *  @date		1 Mar 2012
  *  @version		1.0
  */
-s32 DeallocateNetworkInfo(tstrNetworkInfo *pstrNetworkInfo)
+s32 wilc_dealloc_network_info(tstrNetworkInfo *pstrNetworkInfo)
 {
 	s32 s32Error = 0;
 
-	if (pstrNetworkInfo != NULL) {
-		if (pstrNetworkInfo->pu8IEs != NULL) {
+	if (pstrNetworkInfo) {
+		if (pstrNetworkInfo->pu8IEs) {
 			kfree(pstrNetworkInfo->pu8IEs);
 			pstrNetworkInfo->pu8IEs = NULL;
 		} else {
@@ -499,7 +499,7 @@ s32 DeallocateNetworkInfo(tstrNetworkInfo *pstrNetworkInfo)
  *  @date			2 Apr 2012
  *  @version		1.0
  */
-s32 ParseAssocRespInfo(u8 *pu8Buffer, u32 u32BufferLen,
+s32 wilc_parse_assoc_resp_info(u8 *pu8Buffer, u32 u32BufferLen,
 			       tstrConnectRespInfo **ppstrConnectRespInfo)
 {
 	s32 s32Error = 0;
@@ -551,12 +551,12 @@ s32 ParseAssocRespInfo(u8 *pu8Buffer, u32 u32BufferLen,
  *  @date			2 Apr 2012
  *  @version		1.0
  */
-s32 DeallocateAssocRespInfo(tstrConnectRespInfo *pstrConnectRespInfo)
+s32 wilc_dealloc_assoc_resp_info(tstrConnectRespInfo *pstrConnectRespInfo)
 {
 	s32 s32Error = 0;
 
-	if (pstrConnectRespInfo != NULL) {
-		if (pstrConnectRespInfo->pu8RespIEs != NULL) {
+	if (pstrConnectRespInfo) {
+		if (pstrConnectRespInfo->pu8RespIEs) {
 			kfree(pstrConnectRespInfo->pu8RespIEs);
 			pstrConnectRespInfo->pu8RespIEs = NULL;
 		} else {
@@ -588,7 +588,8 @@ s32 DeallocateAssocRespInfo(tstrConnectRespInfo *pstrConnectRespInfo)
  *  @date		1 Mar 2012
  *  @version	1.0
  */
-s32 send_config_pkt(u8 mode, struct wid *wids, u32 count, u32 drv)
+s32 wilc_send_config_pkt(struct wilc *wilc, u8 mode, struct wid *wids,
+			 u32 count, u32 drv)
 {
 	s32 counter = 0, ret = 0;
 
@@ -596,11 +597,11 @@ s32 send_config_pkt(u8 mode, struct wid *wids, u32 count, u32 drv)
 		for (counter = 0; counter < count; counter++) {
 			PRINT_INFO(CORECONFIG_DBG, "Sending CFG packet [%d][%d]\n", !counter,
 				   (counter == count - 1));
-			if (!wilc_wlan_cfg_get(!counter,
+			if (!wilc_wlan_cfg_get(wilc, !counter,
 					       wids[counter].id,
 					       (counter == count - 1),
 					       drv)) {
-				ret = -1;
+				ret = -ETIMEDOUT;
 				printk("[Sendconfigpkt]Get Timed out\n");
 				break;
 			}
@@ -611,18 +612,17 @@ s32 send_config_pkt(u8 mode, struct wid *wids, u32 count, u32 drv)
 					wids[counter].id,
 					wids[counter].val,
 					wids[counter].size);
-
 		}
 	} else if (mode == SET_CFG) {
 		for (counter = 0; counter < count; counter++) {
 			PRINT_D(CORECONFIG_DBG, "Sending config SET PACKET WID:%x\n", wids[counter].id);
-			if (!wilc_wlan_cfg_set(!counter,
+			if (!wilc_wlan_cfg_set(wilc, !counter,
 					       wids[counter].id,
 					       wids[counter].val,
 					       wids[counter].size,
 					       (counter == count - 1),
 					       drv)) {
-				ret = -1;
+				ret = -ETIMEDOUT;
 				printk("[Sendconfigpkt]Set Timed out\n");
 				break;
 			}
diff --git a/drivers/staging/wilc1000/coreconfigurator.h b/drivers/staging/wilc1000/coreconfigurator.h
index 6294d929a800..fc43d04ca1da 100644
--- a/drivers/staging/wilc1000/coreconfigurator.h
+++ b/drivers/staging/wilc1000/coreconfigurator.h
@@ -72,7 +72,7 @@ typedef enum {
 
 struct wid {
 	u16 id;
-	enum WID_TYPE type;
+	enum wid_type type;
 	s32 size;
 	s8 *val;
 };
@@ -127,16 +127,18 @@ typedef struct {
 	size_t ie_len;
 } tstrDisconnectNotifInfo;
 
-s32 send_config_pkt(u8 mode, struct wid *wids, u32 count, u32 drv);
-s32 parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo);
-s32 DeallocateNetworkInfo(tstrNetworkInfo *pstrNetworkInfo);
+s32 wilc_send_config_pkt(struct wilc *wilc, u8 mode, struct wid *wids,
+			 u32 count, u32 drv);
+s32 wilc_parse_network_info(u8 *pu8MsgBuffer, tstrNetworkInfo **ppstrNetworkInfo);
+s32 wilc_dealloc_network_info(tstrNetworkInfo *pstrNetworkInfo);
 
-s32 ParseAssocRespInfo(u8 *pu8Buffer, u32 u32BufferLen,
+s32 wilc_parse_assoc_resp_info(u8 *pu8Buffer, u32 u32BufferLen,
 		       tstrConnectRespInfo **ppstrConnectRespInfo);
-s32 DeallocateAssocRespInfo(tstrConnectRespInfo *pstrConnectRespInfo);
-
-void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length);
-void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length);
-void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length);
-
+s32 wilc_dealloc_assoc_resp_info(tstrConnectRespInfo *pstrConnectRespInfo);
+void wilc_scan_complete_received(struct wilc *wilc, u8 *pu8Buffer,
+				 u32 u32Length);
+void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
+				u32 u32Length);
+void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
+				   u32 u32Length);
 #endif
diff --git a/drivers/staging/wilc1000/host_interface.c b/drivers/staging/wilc1000/host_interface.c
index dbbe72c7e255..8c7752034032 100644
--- a/drivers/staging/wilc1000/host_interface.c
+++ b/drivers/staging/wilc1000/host_interface.c
@@ -4,17 +4,12 @@
 #include <linux/delay.h>
 #include "host_interface.h"
 #include "coreconfigurator.h"
+#include "wilc_wlan.h"
 #include "wilc_wlan_if.h"
 #include "wilc_msgqueue.h"
 #include <linux/etherdevice.h>
 #include "wilc_wfi_netdevice.h"
 
-extern u8 connecting;
-
-extern struct timer_list hDuringIpTimer;
-
-extern u8 g_wilc_initialized;
-
 #define HOST_IF_MSG_SCAN                        0
 #define HOST_IF_MSG_CONNECT                     1
 #define HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO        2
@@ -48,7 +43,6 @@ extern u8 g_wilc_initialized;
 #define HOST_IF_MSG_FLUSH_CONNECT               30
 #define HOST_IF_MSG_GET_STATISTICS              31
 #define HOST_IF_MSG_SET_MULTICAST_FILTER        32
-#define HOST_IF_MSG_ADD_BA_SESSION              33
 #define HOST_IF_MSG_DEL_BA_SESSION              34
 #define HOST_IF_MSG_Q_IDLE                      35
 #define HOST_IF_MSG_DEL_ALL_STA                 36
@@ -199,7 +193,7 @@ union message_body {
 struct host_if_msg {
 	u16 id;
 	union message_body body;
-	struct host_if_drv *drv;
+	struct wilc_vif *vif;
 };
 
 struct join_bss_param {
@@ -231,10 +225,9 @@ struct join_bss_param {
 	u8 start_time[4];
 };
 
-static struct host_if_drv *wfidrv_list[NUM_CONCURRENT_IFC + 1];
 struct host_if_drv *terminated_handle;
-bool g_obtainingIP;
-u8 P2P_LISTEN_STATE;
+bool wilc_optaining_ip;
+static u8 P2P_LISTEN_STATE;
 static struct task_struct *hif_thread_handler;
 static WILC_MsgQueueHandle hif_msg_q;
 static struct semaphore hif_sema_thread;
@@ -243,7 +236,7 @@ static struct semaphore hif_sema_wait_response;
 static struct semaphore hif_sema_deinit;
 static struct timer_list periodic_rssi;
 
-u8 gau8MulticastMacAddrList[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
+u8 wilc_multicast_mac_addr_list[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
 
 static u8 rcv_assoc_resp[MAX_ASSOC_RESP_FRAME_SIZE];
 
@@ -259,86 +252,57 @@ static u8 del_beacon;
 static u32 clients_count;
 
 static u8 *join_req;
-u8 *info_element;
+static u8 *info_element;
 static u8 mode_11i;
-u8 auth_type;
-u32 join_req_size;
+static u8 auth_type;
+static u32 join_req_size;
 static u32 info_element_size;
-static struct host_if_drv *join_req_drv;
+static struct wilc_vif *join_req_vif;
 #define REAL_JOIN_REQ 0
 #define FLUSHED_JOIN_REQ 1
 #define FLUSHED_BYTE_POS 79
 
 static void *host_int_ParseJoinBssParam(tstrNetworkInfo *ptstrNetworkInfo);
 
-extern void chip_sleep_manually(u32 u32SleepTime);
-extern int linux_wlan_get_num_conn_ifcs(void);
-
-static int add_handler_in_list(struct host_if_drv *handler)
+/* The u8IfIdx starts from 0 to NUM_CONCURRENT_IFC -1, but 0 index used as
+ * special purpose in wilc device, so we add 1 to the index to starts from 1.
+ * As a result, the returned index will be 1 to NUM_CONCURRENT_IFC.
+ */
+int wilc_get_vif_idx(struct wilc_vif *vif)
 {
-	int i;
-
-	for (i = 1; i < ARRAY_SIZE(wfidrv_list); i++) {
-		if (!wfidrv_list[i]) {
-			wfidrv_list[i] = handler;
-			return 0;
-		}
-	}
-
-	return -ENOBUFS;
+	return vif->u8IfIdx + 1;
 }
 
-static int remove_handler_in_list(struct host_if_drv *handler)
+/* We need to minus 1 from idx which is from wilc device to get real index
+ * of wilc->vif[], because we add 1 when pass to wilc device in the function
+ * wilc_get_vif_idx.
+ * As a result, the index should be between 0 and NUM_CONCURRENT_IFC -1.
+ */
+static struct wilc_vif *wilc_get_vif_from_idx(struct wilc *wilc, int idx)
 {
-	int i;
+	int index = idx - 1;
 
-	for (i = 1; i < ARRAY_SIZE(wfidrv_list); i++) {
-		if (wfidrv_list[i] == handler) {
-			wfidrv_list[i] = NULL;
-			return 0;
-		}
-	}
-
-	return -EINVAL;
-}
-
-static int get_id_from_handler(struct host_if_drv *handler)
-{
-	int i;
-
-	if (!handler)
-		return 0;
-
-	for (i = 1; i < ARRAY_SIZE(wfidrv_list); i++) {
-		if (wfidrv_list[i] == handler)
-			return i;
-	}
-
-	return 0;
-}
-
-static struct host_if_drv *get_handler_from_id(int id)
-{
-	if (id <= 0 || id >= ARRAY_SIZE(wfidrv_list))
+	if (index < 0 || index >= NUM_CONCURRENT_IFC)
 		return NULL;
-	return wfidrv_list[id];
+
+	return wilc->vif[index];
 }
 
-static s32 Handle_SetChannel(struct host_if_drv *hif_drv,
-			     struct channel_attr *pstrHostIFSetChan)
+static s32 handle_set_channel(struct wilc_vif *vif,
+			      struct channel_attr *hif_set_ch)
 {
 	s32 result = 0;
 	struct wid wid;
 
 	wid.id = (u16)WID_CURRENT_CHANNEL;
 	wid.type = WID_CHAR;
-	wid.val = (char *)&pstrHostIFSetChan->set_ch;
+	wid.val = (char *)&hif_set_ch->set_ch;
 	wid.size = sizeof(char);
 
 	PRINT_D(HOSTINF_DBG, "Setting channel\n");
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to set channel\n");
@@ -348,21 +312,21 @@ static s32 Handle_SetChannel(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static s32 Handle_SetWfiDrvHandler(struct host_if_drv *hif_drv,
-				   struct drv_handler *pstrHostIfSetDrvHandler)
+static s32 handle_set_wfi_drv_handler(struct wilc_vif *vif,
+				      struct drv_handler *hif_drv_handler)
 {
 	s32 result = 0;
 	struct wid wid;
 
 	wid.id = (u16)WID_SET_DRV_HANDLER;
 	wid.type = WID_INT;
-	wid.val = (s8 *)&pstrHostIfSetDrvHandler->handler;
+	wid.val = (s8 *)&hif_drv_handler->handler;
 	wid.size = sizeof(u32);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 pstrHostIfSetDrvHandler->handler);
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				      hif_drv_handler->handler);
 
-	if (!hif_drv)
+	if (!hif_drv_handler->handler)
 		up(&hif_sema_driver);
 
 	if (result) {
@@ -373,21 +337,21 @@ static s32 Handle_SetWfiDrvHandler(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static s32 Handle_SetOperationMode(struct host_if_drv *hif_drv,
-				   struct op_mode *pstrHostIfSetOperationMode)
+static s32 handle_set_operation_mode(struct wilc_vif *vif,
+				     struct op_mode *hif_op_mode)
 {
 	s32 result = 0;
 	struct wid wid;
 
 	wid.id = (u16)WID_SET_OPERATION_MODE;
 	wid.type = WID_INT;
-	wid.val = (s8 *)&pstrHostIfSetOperationMode->mode;
+	wid.val = (s8 *)&hif_op_mode->mode;
 	wid.size = sizeof(u32);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
-	if ((pstrHostIfSetOperationMode->mode) == IDLE_MODE)
+	if ((hif_op_mode->mode) == IDLE_MODE)
 		up(&hif_sema_driver);
 
 	if (result) {
@@ -398,28 +362,34 @@ static s32 Handle_SetOperationMode(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 Handle_set_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
+static s32 host_int_get_ipaddress(struct wilc_vif *vif,
+				  struct host_if_drv *hif_drv,
+				  u8 *u16ipadd, u8 idx);
+
+static s32 handle_set_ip_address(struct wilc_vif *vif, u8 *ip_addr, u8 idx)
 {
 	s32 result = 0;
 	struct wid wid;
-	char firmwareIPAddress[4] = {0};
+	char firmware_ip_addr[4] = {0};
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
-	if (pu8IPAddr[0] < 192)
-		pu8IPAddr[0] = 0;
+	if (ip_addr[0] < 192)
+		ip_addr[0] = 0;
 
-	PRINT_INFO(HOSTINF_DBG, "Indx = %d, Handling set  IP = %pI4\n", idx, pu8IPAddr);
+	PRINT_INFO(HOSTINF_DBG, "Indx = %d, Handling set  IP = %pI4\n",
+		   idx, ip_addr);
 
-	memcpy(set_ip[idx], pu8IPAddr, IP_ALEN);
+	memcpy(set_ip[idx], ip_addr, IP_ALEN);
 
 	wid.id = (u16)WID_IP_ADDRESS;
 	wid.type = WID_STR;
-	wid.val = (u8 *)pu8IPAddr;
+	wid.val = (u8 *)ip_addr;
 	wid.size = IP_ALEN;
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
-	host_int_get_ipaddress(hif_drv, firmwareIPAddress, idx);
+	host_int_get_ipaddress(vif, hif_drv, firmware_ip_addr, idx);
 
 	if (result) {
 		PRINT_ER("Failed to set IP address\n");
@@ -431,7 +401,7 @@ s32 Handle_set_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
 	return result;
 }
 
-s32 Handle_get_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
+static s32 handle_get_ip_address(struct wilc_vif *vif, u8 idx)
 {
 	s32 result = 0;
 	struct wid wid;
@@ -441,8 +411,8 @@ s32 Handle_get_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
 	wid.val = kmalloc(IP_ALEN, GFP_KERNEL);
 	wid.size = IP_ALEN;
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	PRINT_INFO(HOSTINF_DBG, "%pI4\n", wid.val);
 
@@ -451,7 +421,7 @@ s32 Handle_get_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
 	kfree(wid.val);
 
 	if (memcmp(get_ip[idx], set_ip[idx], IP_ALEN) != 0)
-		host_int_setup_ipaddress(hif_drv, set_ip[idx], idx);
+		wilc_setup_ipaddress(vif, set_ip[idx], idx);
 
 	if (result != 0) {
 		PRINT_ER("Failed to get IP address\n");
@@ -465,8 +435,8 @@ s32 Handle_get_IPAddress(struct host_if_drv *hif_drv, u8 *pu8IPAddr, u8 idx)
 	return result;
 }
 
-static s32 Handle_SetMacAddress(struct host_if_drv *hif_drv,
-				struct set_mac_addr *pstrHostIfSetMacAddress)
+static s32 handle_set_mac_address(struct wilc_vif *vif,
+				  struct set_mac_addr *set_mac_addr)
 {
 	s32 result = 0;
 	struct wid wid;
@@ -476,7 +446,7 @@ static s32 Handle_SetMacAddress(struct host_if_drv *hif_drv,
 		PRINT_ER("No buffer to send mac address\n");
 		return -EFAULT;
 	}
-	memcpy(mac_buf, pstrHostIfSetMacAddress->mac_addr, ETH_ALEN);
+	memcpy(mac_buf, set_mac_addr->mac_addr, ETH_ALEN);
 
 	wid.id = (u16)WID_MAC_ADDR;
 	wid.type = WID_STR;
@@ -484,8 +454,8 @@ static s32 Handle_SetMacAddress(struct host_if_drv *hif_drv,
 	wid.size = ETH_ALEN;
 	PRINT_D(GENERIC_DBG, "mac addr = :%pM\n", wid.val);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result) {
 		PRINT_ER("Failed to set mac address\n");
 		result = -EFAULT;
@@ -495,19 +465,19 @@ static s32 Handle_SetMacAddress(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static s32 Handle_GetMacAddress(struct host_if_drv *hif_drv,
-				struct get_mac_addr *pstrHostIfGetMacAddress)
+static s32 handle_get_mac_address(struct wilc_vif *vif,
+				  struct get_mac_addr *get_mac_addr)
 {
 	s32 result = 0;
 	struct wid wid;
 
 	wid.id = (u16)WID_MAC_ADDR;
 	wid.type = WID_STR;
-	wid.val = pstrHostIfGetMacAddress->mac_addr;
+	wid.val = get_mac_addr->mac_addr;
 	wid.size = ETH_ALEN;
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to get mac address\n");
@@ -518,258 +488,270 @@ static s32 Handle_GetMacAddress(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static s32 Handle_CfgParam(struct host_if_drv *hif_drv,
-			   struct cfg_param_attr *strHostIFCfgParamAttr)
+static s32 handle_cfg_param(struct wilc_vif *vif,
+			    struct cfg_param_attr *cfg_param_attr)
 {
 	s32 result = 0;
-	struct wid strWIDList[32];
-	u8 u8WidCnt = 0;
+	struct wid wid_list[32];
+	struct host_if_drv *hif_drv = vif->hif_drv;
+	u8 wid_cnt = 0;
 
-	down(&hif_drv->gtOsCfgValuesSem);
+	down(&hif_drv->sem_cfg_values);
 
 	PRINT_D(HOSTINF_DBG, "Setting CFG params\n");
 
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & BSS_TYPE) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.bss_type < 6) {
-			strWIDList[u8WidCnt].id = WID_BSS_TYPE;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.bss_type;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.bss_type = (u8)strHostIFCfgParamAttr->cfg_attr_info.bss_type;
+	if (cfg_param_attr->cfg_attr_info.flag & BSS_TYPE) {
+		if (cfg_param_attr->cfg_attr_info.bss_type < 6) {
+			wid_list[wid_cnt].id = WID_BSS_TYPE;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.bss_type;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.bss_type = (u8)cfg_param_attr->cfg_attr_info.bss_type;
 		} else {
 			PRINT_ER("check value 6 over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & AUTH_TYPE) {
-		if ((strHostIFCfgParamAttr->cfg_attr_info.auth_type) == 1 || (strHostIFCfgParamAttr->cfg_attr_info.auth_type) == 2 || (strHostIFCfgParamAttr->cfg_attr_info.auth_type) == 5) {
-			strWIDList[u8WidCnt].id = WID_AUTH_TYPE;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.auth_type;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.auth_type = (u8)strHostIFCfgParamAttr->cfg_attr_info.auth_type;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & AUTH_TYPE) {
+		if (cfg_param_attr->cfg_attr_info.auth_type == 1 ||
+		    cfg_param_attr->cfg_attr_info.auth_type == 2 ||
+		    cfg_param_attr->cfg_attr_info.auth_type == 5) {
+			wid_list[wid_cnt].id = WID_AUTH_TYPE;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.auth_type;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.auth_type = (u8)cfg_param_attr->cfg_attr_info.auth_type;
 		} else {
 			PRINT_ER("Impossible value \n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & AUTHEN_TIMEOUT) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.auth_timeout > 0 && strHostIFCfgParamAttr->cfg_attr_info.auth_timeout < 65536) {
-			strWIDList[u8WidCnt].id = WID_AUTH_TIMEOUT;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.auth_timeout;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.auth_timeout = strHostIFCfgParamAttr->cfg_attr_info.auth_timeout;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & AUTHEN_TIMEOUT) {
+		if (cfg_param_attr->cfg_attr_info.auth_timeout > 0 &&
+		    cfg_param_attr->cfg_attr_info.auth_timeout < 65536) {
+			wid_list[wid_cnt].id = WID_AUTH_TIMEOUT;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.auth_timeout;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.auth_timeout = cfg_param_attr->cfg_attr_info.auth_timeout;
 		} else {
 			PRINT_ER("Range(1 ~ 65535) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & POWER_MANAGEMENT) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.power_mgmt_mode < 5) {
-			strWIDList[u8WidCnt].id = WID_POWER_MANAGEMENT;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.power_mgmt_mode;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.power_mgmt_mode = (u8)strHostIFCfgParamAttr->cfg_attr_info.power_mgmt_mode;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & POWER_MANAGEMENT) {
+		if (cfg_param_attr->cfg_attr_info.power_mgmt_mode < 5) {
+			wid_list[wid_cnt].id = WID_POWER_MANAGEMENT;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.power_mgmt_mode;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.power_mgmt_mode = (u8)cfg_param_attr->cfg_attr_info.power_mgmt_mode;
 		} else {
 			PRINT_ER("Invalide power mode\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & RETRY_SHORT) {
-		if ((strHostIFCfgParamAttr->cfg_attr_info.short_retry_limit > 0) && (strHostIFCfgParamAttr->cfg_attr_info.short_retry_limit < 256))	{
-			strWIDList[u8WidCnt].id = WID_SHORT_RETRY_LIMIT;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.short_retry_limit;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.short_retry_limit = strHostIFCfgParamAttr->cfg_attr_info.short_retry_limit;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & RETRY_SHORT) {
+		if (cfg_param_attr->cfg_attr_info.short_retry_limit > 0 &&
+		    cfg_param_attr->cfg_attr_info.short_retry_limit < 256) {
+			wid_list[wid_cnt].id = WID_SHORT_RETRY_LIMIT;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.short_retry_limit;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.short_retry_limit = cfg_param_attr->cfg_attr_info.short_retry_limit;
 		} else {
 			PRINT_ER("Range(1~256) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & RETRY_LONG) {
-		if ((strHostIFCfgParamAttr->cfg_attr_info.long_retry_limit > 0) && (strHostIFCfgParamAttr->cfg_attr_info.long_retry_limit < 256)) {
-			strWIDList[u8WidCnt].id = WID_LONG_RETRY_LIMIT;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.long_retry_limit;
-
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.long_retry_limit = strHostIFCfgParamAttr->cfg_attr_info.long_retry_limit;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & RETRY_LONG) {
+		if (cfg_param_attr->cfg_attr_info.long_retry_limit > 0 &&
+		    cfg_param_attr->cfg_attr_info.long_retry_limit < 256) {
+			wid_list[wid_cnt].id = WID_LONG_RETRY_LIMIT;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.long_retry_limit;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.long_retry_limit = cfg_param_attr->cfg_attr_info.long_retry_limit;
 		} else {
 			PRINT_ER("Range(1~256) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & FRAG_THRESHOLD) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.frag_threshold > 255 && strHostIFCfgParamAttr->cfg_attr_info.frag_threshold < 7937) {
-			strWIDList[u8WidCnt].id = WID_FRAG_THRESHOLD;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.frag_threshold;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.frag_threshold = strHostIFCfgParamAttr->cfg_attr_info.frag_threshold;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & FRAG_THRESHOLD) {
+		if (cfg_param_attr->cfg_attr_info.frag_threshold > 255 &&
+		    cfg_param_attr->cfg_attr_info.frag_threshold < 7937) {
+			wid_list[wid_cnt].id = WID_FRAG_THRESHOLD;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.frag_threshold;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.frag_threshold = cfg_param_attr->cfg_attr_info.frag_threshold;
 		} else {
 			PRINT_ER("Threshold Range fail\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & RTS_THRESHOLD) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.rts_threshold > 255 && strHostIFCfgParamAttr->cfg_attr_info.rts_threshold < 65536)	{
-			strWIDList[u8WidCnt].id = WID_RTS_THRESHOLD;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.rts_threshold;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.rts_threshold = strHostIFCfgParamAttr->cfg_attr_info.rts_threshold;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & RTS_THRESHOLD) {
+		if (cfg_param_attr->cfg_attr_info.rts_threshold > 255 &&
+		    cfg_param_attr->cfg_attr_info.rts_threshold < 65536) {
+			wid_list[wid_cnt].id = WID_RTS_THRESHOLD;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.rts_threshold;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.rts_threshold = cfg_param_attr->cfg_attr_info.rts_threshold;
 		} else {
 			PRINT_ER("Threshold Range fail\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & PREAMBLE) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.preamble_type < 3) {
-			strWIDList[u8WidCnt].id = WID_PREAMBLE;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.preamble_type;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.preamble_type = strHostIFCfgParamAttr->cfg_attr_info.preamble_type;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & PREAMBLE) {
+		if (cfg_param_attr->cfg_attr_info.preamble_type < 3) {
+			wid_list[wid_cnt].id = WID_PREAMBLE;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.preamble_type;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.preamble_type = cfg_param_attr->cfg_attr_info.preamble_type;
 		} else {
 			PRINT_ER("Preamle Range(0~2) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & SHORT_SLOT_ALLOWED) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.short_slot_allowed < 2) {
-			strWIDList[u8WidCnt].id = WID_SHORT_SLOT_ALLOWED;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.short_slot_allowed;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.short_slot_allowed = (u8)strHostIFCfgParamAttr->cfg_attr_info.short_slot_allowed;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & SHORT_SLOT_ALLOWED) {
+		if (cfg_param_attr->cfg_attr_info.short_slot_allowed < 2) {
+			wid_list[wid_cnt].id = WID_SHORT_SLOT_ALLOWED;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.short_slot_allowed;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.short_slot_allowed = (u8)cfg_param_attr->cfg_attr_info.short_slot_allowed;
 		} else {
 			PRINT_ER("Short slot(2) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & TXOP_PROT_DISABLE) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.txop_prot_disabled < 2) {
-			strWIDList[u8WidCnt].id = WID_11N_TXOP_PROT_DISABLE;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.txop_prot_disabled;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.txop_prot_disabled = (u8)strHostIFCfgParamAttr->cfg_attr_info.txop_prot_disabled;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & TXOP_PROT_DISABLE) {
+		if (cfg_param_attr->cfg_attr_info.txop_prot_disabled < 2) {
+			wid_list[wid_cnt].id = WID_11N_TXOP_PROT_DISABLE;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.txop_prot_disabled;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.txop_prot_disabled = (u8)cfg_param_attr->cfg_attr_info.txop_prot_disabled;
 		} else {
 			PRINT_ER("TXOP prot disable\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & BEACON_INTERVAL) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.beacon_interval > 0 && strHostIFCfgParamAttr->cfg_attr_info.beacon_interval < 65536) {
-			strWIDList[u8WidCnt].id = WID_BEACON_INTERVAL;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.beacon_interval;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.beacon_interval = strHostIFCfgParamAttr->cfg_attr_info.beacon_interval;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & BEACON_INTERVAL) {
+		if (cfg_param_attr->cfg_attr_info.beacon_interval > 0 &&
+		    cfg_param_attr->cfg_attr_info.beacon_interval < 65536) {
+			wid_list[wid_cnt].id = WID_BEACON_INTERVAL;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.beacon_interval;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.beacon_interval = cfg_param_attr->cfg_attr_info.beacon_interval;
 		} else {
 			PRINT_ER("Beacon interval(1~65535) fail\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & DTIM_PERIOD) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.dtim_period > 0 && strHostIFCfgParamAttr->cfg_attr_info.dtim_period < 256) {
-			strWIDList[u8WidCnt].id = WID_DTIM_PERIOD;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.dtim_period;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.dtim_period = strHostIFCfgParamAttr->cfg_attr_info.dtim_period;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & DTIM_PERIOD) {
+		if (cfg_param_attr->cfg_attr_info.dtim_period > 0 &&
+		    cfg_param_attr->cfg_attr_info.dtim_period < 256) {
+			wid_list[wid_cnt].id = WID_DTIM_PERIOD;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.dtim_period;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.dtim_period = cfg_param_attr->cfg_attr_info.dtim_period;
 		} else {
 			PRINT_ER("DTIM range(1~255) fail\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & SITE_SURVEY) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.site_survey_enabled < 3) {
-			strWIDList[u8WidCnt].id = WID_SITE_SURVEY;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.site_survey_enabled;
-			strWIDList[u8WidCnt].type = WID_CHAR;
-			strWIDList[u8WidCnt].size = sizeof(char);
-			hif_drv->strCfgValues.site_survey_enabled = (u8)strHostIFCfgParamAttr->cfg_attr_info.site_survey_enabled;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & SITE_SURVEY) {
+		if (cfg_param_attr->cfg_attr_info.site_survey_enabled < 3) {
+			wid_list[wid_cnt].id = WID_SITE_SURVEY;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.site_survey_enabled;
+			wid_list[wid_cnt].type = WID_CHAR;
+			wid_list[wid_cnt].size = sizeof(char);
+			hif_drv->cfg_values.site_survey_enabled = (u8)cfg_param_attr->cfg_attr_info.site_survey_enabled;
 		} else {
 			PRINT_ER("Site survey disable\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & SITE_SURVEY_SCAN_TIME) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.site_survey_scan_time > 0 && strHostIFCfgParamAttr->cfg_attr_info.site_survey_scan_time < 65536) {
-			strWIDList[u8WidCnt].id = WID_SITE_SURVEY_SCAN_TIME;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.site_survey_scan_time;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.site_survey_scan_time = strHostIFCfgParamAttr->cfg_attr_info.site_survey_scan_time;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & SITE_SURVEY_SCAN_TIME) {
+		if (cfg_param_attr->cfg_attr_info.site_survey_scan_time > 0 &&
+		    cfg_param_attr->cfg_attr_info.site_survey_scan_time < 65536) {
+			wid_list[wid_cnt].id = WID_SITE_SURVEY_SCAN_TIME;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.site_survey_scan_time;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.site_survey_scan_time = cfg_param_attr->cfg_attr_info.site_survey_scan_time;
 		} else {
 			PRINT_ER("Site survey scan time(1~65535) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & ACTIVE_SCANTIME) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.active_scan_time > 0 && strHostIFCfgParamAttr->cfg_attr_info.active_scan_time < 65536) {
-			strWIDList[u8WidCnt].id = WID_ACTIVE_SCAN_TIME;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.active_scan_time;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.active_scan_time = strHostIFCfgParamAttr->cfg_attr_info.active_scan_time;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & ACTIVE_SCANTIME) {
+		if (cfg_param_attr->cfg_attr_info.active_scan_time > 0 &&
+		    cfg_param_attr->cfg_attr_info.active_scan_time < 65536) {
+			wid_list[wid_cnt].id = WID_ACTIVE_SCAN_TIME;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.active_scan_time;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.active_scan_time = cfg_param_attr->cfg_attr_info.active_scan_time;
 		} else {
 			PRINT_ER("Active scan time(1~65535) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
-	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & PASSIVE_SCANTIME) {
-		if (strHostIFCfgParamAttr->cfg_attr_info.passive_scan_time > 0 && strHostIFCfgParamAttr->cfg_attr_info.passive_scan_time < 65536) {
-			strWIDList[u8WidCnt].id = WID_PASSIVE_SCAN_TIME;
-			strWIDList[u8WidCnt].val = (s8 *)&strHostIFCfgParamAttr->cfg_attr_info.passive_scan_time;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.passive_scan_time = strHostIFCfgParamAttr->cfg_attr_info.passive_scan_time;
+		wid_cnt++;
+	}
+	if (cfg_param_attr->cfg_attr_info.flag & PASSIVE_SCANTIME) {
+		if (cfg_param_attr->cfg_attr_info.passive_scan_time > 0 &&
+		    cfg_param_attr->cfg_attr_info.passive_scan_time < 65536) {
+			wid_list[wid_cnt].id = WID_PASSIVE_SCAN_TIME;
+			wid_list[wid_cnt].val = (s8 *)&cfg_param_attr->cfg_attr_info.passive_scan_time;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.passive_scan_time = cfg_param_attr->cfg_attr_info.passive_scan_time;
 		} else {
 			PRINT_ER("Passive scan time(1~65535) over\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
+		wid_cnt++;
 	}
-	if (strHostIFCfgParamAttr->cfg_attr_info.flag & CURRENT_TX_RATE) {
-		enum CURRENT_TXRATE curr_tx_rate = strHostIFCfgParamAttr->cfg_attr_info.curr_tx_rate;
+	if (cfg_param_attr->cfg_attr_info.flag & CURRENT_TX_RATE) {
+		enum CURRENT_TXRATE curr_tx_rate = cfg_param_attr->cfg_attr_info.curr_tx_rate;
 
 		if (curr_tx_rate == AUTORATE || curr_tx_rate == MBPS_1
 		    || curr_tx_rate == MBPS_2 || curr_tx_rate == MBPS_5_5
@@ -777,38 +759,40 @@ static s32 Handle_CfgParam(struct host_if_drv *hif_drv,
 		    || curr_tx_rate == MBPS_9 || curr_tx_rate == MBPS_12
 		    || curr_tx_rate == MBPS_18 || curr_tx_rate == MBPS_24
 		    || curr_tx_rate == MBPS_36 || curr_tx_rate == MBPS_48 || curr_tx_rate == MBPS_54) {
-			strWIDList[u8WidCnt].id = WID_CURRENT_TX_RATE;
-			strWIDList[u8WidCnt].val = (s8 *)&curr_tx_rate;
-			strWIDList[u8WidCnt].type = WID_SHORT;
-			strWIDList[u8WidCnt].size = sizeof(u16);
-			hif_drv->strCfgValues.curr_tx_rate = (u8)curr_tx_rate;
+			wid_list[wid_cnt].id = WID_CURRENT_TX_RATE;
+			wid_list[wid_cnt].val = (s8 *)&curr_tx_rate;
+			wid_list[wid_cnt].type = WID_SHORT;
+			wid_list[wid_cnt].size = sizeof(u16);
+			hif_drv->cfg_values.curr_tx_rate = (u8)curr_tx_rate;
 		} else {
 			PRINT_ER("out of TX rate\n");
 			result = -EINVAL;
 			goto ERRORHANDLER;
 		}
-		u8WidCnt++;
+		wid_cnt++;
 	}
 
-	result = send_config_pkt(SET_CFG, strWIDList, u8WidCnt,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, wid_list,
+				      wid_cnt, wilc_get_vif_idx(vif));
 
 	if (result)
 		PRINT_ER("Error in setting CFG params\n");
 
 ERRORHANDLER:
-	up(&hif_drv->gtOsCfgValuesSem);
+	up(&hif_drv->sem_cfg_values);
 	return result;
 }
 
-static s32 Handle_wait_msg_q_empty(void)
+static void Handle_wait_msg_q_empty(void)
 {
-	g_wilc_initialized = 0;
+	wilc_initialized = 0;
 	up(&hif_sema_wait_response);
-	return 0;
 }
 
-static s32 Handle_Scan(struct host_if_drv *hif_drv,
+static s32 Handle_ScanDone(struct wilc_vif *vif,
+			   enum scan_event enuEvent);
+
+static s32 Handle_Scan(struct wilc_vif *vif,
 		       struct scan_attr *pstrHostIFscanAttr)
 {
 	s32 result = 0;
@@ -818,21 +802,24 @@ static s32 Handle_Scan(struct host_if_drv *hif_drv,
 	u8 *pu8Buffer;
 	u8 valuesize = 0;
 	u8 *pu8HdnNtwrksWidVal = NULL;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	PRINT_D(HOSTINF_DBG, "Setting SCAN params\n");
-	PRINT_D(HOSTINF_DBG, "Scanning: In [%d] state\n", hif_drv->enuHostIFstate);
+	PRINT_D(HOSTINF_DBG, "Scanning: In [%d] state\n", hif_drv->hif_state);
 
-	hif_drv->usr_scan_req.pfUserScanResult = pstrHostIFscanAttr->result;
-	hif_drv->usr_scan_req.u32UserScanPvoid = pstrHostIFscanAttr->arg;
+	hif_drv->usr_scan_req.scan_result = pstrHostIFscanAttr->result;
+	hif_drv->usr_scan_req.arg = pstrHostIFscanAttr->arg;
 
-	if ((hif_drv->enuHostIFstate >= HOST_IF_SCANNING) && (hif_drv->enuHostIFstate < HOST_IF_CONNECTED)) {
-		PRINT_D(GENERIC_DBG, "Don't scan we are already in [%d] state\n", hif_drv->enuHostIFstate);
+	if ((hif_drv->hif_state >= HOST_IF_SCANNING) &&
+	    (hif_drv->hif_state < HOST_IF_CONNECTED)) {
+		PRINT_D(GENERIC_DBG, "Don't scan already in [%d] state\n",
+			hif_drv->hif_state);
 		PRINT_ER("Already scan\n");
 		result = -EBUSY;
 		goto ERRORHANDLER;
 	}
 
-	if (g_obtainingIP || connecting) {
+	if (wilc_optaining_ip || wilc_connecting) {
 		PRINT_D(GENERIC_DBG, "[handle_scan]: Don't do obss scan until IP adresss is obtained\n");
 		PRINT_ER("Don't do obss scan\n");
 		result = -EBUSY;
@@ -841,7 +828,7 @@ static s32 Handle_Scan(struct host_if_drv *hif_drv,
 
 	PRINT_D(HOSTINF_DBG, "Setting SCAN params\n");
 
-	hif_drv->usr_scan_req.u32RcvdChCount = 0;
+	hif_drv->usr_scan_req.rcvd_ch_cnt = 0;
 
 	strWIDList[u32WidsCount].id = (u16)WID_SSID_PROBE_REQ;
 	strWIDList[u32WidsCount].type = WID_STR;
@@ -904,13 +891,14 @@ static s32 Handle_Scan(struct host_if_drv *hif_drv,
 	strWIDList[u32WidsCount].val = (s8 *)&pstrHostIFscanAttr->src;
 	u32WidsCount++;
 
-	if (hif_drv->enuHostIFstate == HOST_IF_CONNECTED)
+	if (hif_drv->hif_state == HOST_IF_CONNECTED)
 		scan_while_connected = true;
-	else if (hif_drv->enuHostIFstate == HOST_IF_IDLE)
+	else if (hif_drv->hif_state == HOST_IF_IDLE)
 		scan_while_connected = false;
 
-	result = send_config_pkt(SET_CFG, strWIDList, u32WidsCount,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, strWIDList,
+				      u32WidsCount,
+				      wilc_get_vif_idx(vif));
 
 	if (result)
 		PRINT_ER("Failed to send scan paramters config packet\n");
@@ -919,8 +907,8 @@ static s32 Handle_Scan(struct host_if_drv *hif_drv,
 
 ERRORHANDLER:
 	if (result) {
-		del_timer(&hif_drv->hScanTimer);
-		Handle_ScanDone(hif_drv, SCAN_EVENT_ABORTED);
+		del_timer(&hif_drv->scan_timer);
+		Handle_ScanDone(vif, SCAN_EVENT_ABORTED);
 	}
 
 	kfree(pstrHostIFscanAttr->ch_freq_list);
@@ -936,12 +924,13 @@ ERRORHANDLER:
 	return result;
 }
 
-static s32 Handle_ScanDone(struct host_if_drv *hif_drv,
+static s32 Handle_ScanDone(struct wilc_vif *vif,
 			   enum scan_event enuEvent)
 {
 	s32 result = 0;
 	u8 u8abort_running_scan;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	PRINT_D(HOSTINF_DBG, "in Handle_ScanDone()\n");
 
@@ -953,8 +942,8 @@ static s32 Handle_ScanDone(struct host_if_drv *hif_drv,
 		wid.val = (s8 *)&u8abort_running_scan;
 		wid.size = sizeof(char);
 
-		result = send_config_pkt(SET_CFG, &wid, 1,
-					 get_id_from_handler(hif_drv));
+		result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+					 wilc_get_vif_idx(vif));
 
 		if (result) {
 			PRINT_ER("Failed to set abort running scan\n");
@@ -967,17 +956,17 @@ static s32 Handle_ScanDone(struct host_if_drv *hif_drv,
 		return result;
 	}
 
-	if (hif_drv->usr_scan_req.pfUserScanResult) {
-		hif_drv->usr_scan_req.pfUserScanResult(enuEvent, NULL,
-						       hif_drv->usr_scan_req.u32UserScanPvoid, NULL);
-		hif_drv->usr_scan_req.pfUserScanResult = NULL;
+	if (hif_drv->usr_scan_req.scan_result) {
+		hif_drv->usr_scan_req.scan_result(enuEvent, NULL,
+						  hif_drv->usr_scan_req.arg, NULL);
+		hif_drv->usr_scan_req.scan_result = NULL;
 	}
 
 	return result;
 }
 
-u8 u8ConnectedSSID[6] = {0};
-static s32 Handle_Connect(struct host_if_drv *hif_drv,
+u8 wilc_connected_ssid[6] = {0};
+static s32 Handle_Connect(struct wilc_vif *vif,
 			  struct connect_attr *pstrHostIFconnectAttr)
 {
 	s32 result = 0;
@@ -985,10 +974,11 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 	u32 u32WidsCount = 0, dummyval = 0;
 	u8 *pu8CurrByte = NULL;
 	struct join_bss_param *ptstrJoinBssParam;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	PRINT_D(GENERIC_DBG, "Handling connect request\n");
 
-	if (memcmp(pstrHostIFconnectAttr->bssid, u8ConnectedSSID, ETH_ALEN) == 0) {
+	if (memcmp(pstrHostIFconnectAttr->bssid, wilc_connected_ssid, ETH_ALEN) == 0) {
 		result = 0;
 		PRINT_ER("Trying to connect to an already connected AP, Discard connect request\n");
 		return result;
@@ -1008,7 +998,7 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 		memcpy(hif_drv->usr_conn_req.pu8bssid, pstrHostIFconnectAttr->bssid, 6);
 	}
 
-	hif_drv->usr_conn_req.ssidLen = pstrHostIFconnectAttr->ssid_len;
+	hif_drv->usr_conn_req.ssid_len = pstrHostIFconnectAttr->ssid_len;
 	if (pstrHostIFconnectAttr->ssid) {
 		hif_drv->usr_conn_req.pu8ssid = kmalloc(pstrHostIFconnectAttr->ssid_len + 1, GFP_KERNEL);
 		memcpy(hif_drv->usr_conn_req.pu8ssid,
@@ -1017,18 +1007,18 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 		hif_drv->usr_conn_req.pu8ssid[pstrHostIFconnectAttr->ssid_len] = '\0';
 	}
 
-	hif_drv->usr_conn_req.ConnReqIEsLen = pstrHostIFconnectAttr->ies_len;
+	hif_drv->usr_conn_req.ies_len = pstrHostIFconnectAttr->ies_len;
 	if (pstrHostIFconnectAttr->ies) {
-		hif_drv->usr_conn_req.pu8ConnReqIEs = kmalloc(pstrHostIFconnectAttr->ies_len, GFP_KERNEL);
-		memcpy(hif_drv->usr_conn_req.pu8ConnReqIEs,
+		hif_drv->usr_conn_req.ies = kmalloc(pstrHostIFconnectAttr->ies_len, GFP_KERNEL);
+		memcpy(hif_drv->usr_conn_req.ies,
 		       pstrHostIFconnectAttr->ies,
 		       pstrHostIFconnectAttr->ies_len);
 	}
 
 	hif_drv->usr_conn_req.u8security = pstrHostIFconnectAttr->security;
-	hif_drv->usr_conn_req.tenuAuth_type = pstrHostIFconnectAttr->auth_type;
-	hif_drv->usr_conn_req.pfUserConnectResult = pstrHostIFconnectAttr->result;
-	hif_drv->usr_conn_req.u32UserConnectPvoid = pstrHostIFconnectAttr->arg;
+	hif_drv->usr_conn_req.auth_type = pstrHostIFconnectAttr->auth_type;
+	hif_drv->usr_conn_req.conn_result = pstrHostIFconnectAttr->result;
+	hif_drv->usr_conn_req.arg = pstrHostIFconnectAttr->arg;
 
 	strWIDList[u32WidsCount].id = WID_SUCCESS_FRAME_COUNT;
 	strWIDList[u32WidsCount].type = WID_INT;
@@ -1051,14 +1041,14 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 	{
 		strWIDList[u32WidsCount].id = WID_INFO_ELEMENT_ASSOCIATE;
 		strWIDList[u32WidsCount].type = WID_BIN_DATA;
-		strWIDList[u32WidsCount].val = hif_drv->usr_conn_req.pu8ConnReqIEs;
-		strWIDList[u32WidsCount].size = hif_drv->usr_conn_req.ConnReqIEsLen;
+		strWIDList[u32WidsCount].val = hif_drv->usr_conn_req.ies;
+		strWIDList[u32WidsCount].size = hif_drv->usr_conn_req.ies_len;
 		u32WidsCount++;
 
 		if (memcmp("DIRECT-", pstrHostIFconnectAttr->ssid, 7)) {
-			info_element_size = hif_drv->usr_conn_req.ConnReqIEsLen;
+			info_element_size = hif_drv->usr_conn_req.ies_len;
 			info_element = kmalloc(info_element_size, GFP_KERNEL);
-			memcpy(info_element, hif_drv->usr_conn_req.pu8ConnReqIEs,
+			memcpy(info_element, hif_drv->usr_conn_req.ies,
 			       info_element_size);
 		}
 	}
@@ -1076,13 +1066,14 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 	strWIDList[u32WidsCount].id = (u16)WID_AUTH_TYPE;
 	strWIDList[u32WidsCount].type = WID_CHAR;
 	strWIDList[u32WidsCount].size = sizeof(char);
-	strWIDList[u32WidsCount].val = (s8 *)(&hif_drv->usr_conn_req.tenuAuth_type);
+	strWIDList[u32WidsCount].val = (s8 *)&hif_drv->usr_conn_req.auth_type;
 	u32WidsCount++;
 
 	if (memcmp("DIRECT-", pstrHostIFconnectAttr->ssid, 7))
-		auth_type = (u8)hif_drv->usr_conn_req.tenuAuth_type;
+		auth_type = (u8)hif_drv->usr_conn_req.auth_type;
 
-	PRINT_INFO(HOSTINF_DBG, "Authentication Type = %x\n", hif_drv->usr_conn_req.tenuAuth_type);
+	PRINT_INFO(HOSTINF_DBG, "Authentication Type = %x\n",
+		   hif_drv->usr_conn_req.auth_type);
 	PRINT_D(HOSTINF_DBG, "Connecting to network of SSID %s on channel %d\n",
 		hif_drv->usr_conn_req.pu8ssid, pstrHostIFconnectAttr->ch);
 
@@ -1141,7 +1132,7 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 	*(pu8CurrByte++)  = ptstrJoinBssParam->uapsd_cap;
 
 	*(pu8CurrByte++)  = ptstrJoinBssParam->ht_capable;
-	hif_drv->usr_conn_req.IsHTCapable = ptstrJoinBssParam->ht_capable;
+	hif_drv->usr_conn_req.ht_capable = ptstrJoinBssParam->ht_capable;
 
 	*(pu8CurrByte++)  =  ptstrJoinBssParam->rsn_found;
 	PRINT_D(HOSTINF_DBG, "* rsn found %d*\n", *(pu8CurrByte - 1));
@@ -1194,36 +1185,38 @@ static s32 Handle_Connect(struct host_if_drv *hif_drv,
 
 	if (memcmp("DIRECT-", pstrHostIFconnectAttr->ssid, 7)) {
 		memcpy(join_req, pu8CurrByte, join_req_size);
-		join_req_drv = hif_drv;
+		join_req_vif = vif;
 	}
 
 	PRINT_D(GENERIC_DBG, "send HOST_IF_WAITING_CONN_RESP\n");
 
 	if (pstrHostIFconnectAttr->bssid) {
-		memcpy(u8ConnectedSSID, pstrHostIFconnectAttr->bssid, ETH_ALEN);
-
-		PRINT_D(GENERIC_DBG, "save Bssid = %pM\n", pstrHostIFconnectAttr->bssid);
-		PRINT_D(GENERIC_DBG, "save bssid = %pM\n", u8ConnectedSSID);
+		memcpy(wilc_connected_ssid,
+		       pstrHostIFconnectAttr->bssid, ETH_ALEN);
+		PRINT_D(GENERIC_DBG, "save Bssid = %pM\n",
+			pstrHostIFconnectAttr->bssid);
+		PRINT_D(GENERIC_DBG, "save bssid = %pM\n", wilc_connected_ssid);
 	}
 
-	result = send_config_pkt(SET_CFG, strWIDList, u32WidsCount,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, strWIDList,
+				      u32WidsCount,
+				      wilc_get_vif_idx(vif));
 	if (result) {
 		PRINT_ER("failed to send config packet\n");
 		result = -EFAULT;
 		goto ERRORHANDLER;
 	} else {
 		PRINT_D(GENERIC_DBG, "set HOST_IF_WAITING_CONN_RESP\n");
-		hif_drv->enuHostIFstate = HOST_IF_WAITING_CONN_RESP;
+		hif_drv->hif_state = HOST_IF_WAITING_CONN_RESP;
 	}
 
 ERRORHANDLER:
 	if (result) {
 		tstrConnectInfo strConnectInfo;
 
-		del_timer(&hif_drv->hConnectTimer);
+		del_timer(&hif_drv->connect_timer);
 
-		PRINT_D(HOSTINF_DBG, "could not start connecting to the required network\n");
+		PRINT_D(HOSTINF_DBG, "could not start wilc_connecting to the required network\n");
 
 		memset(&strConnectInfo, 0, sizeof(tstrConnectInfo));
 
@@ -1244,7 +1237,7 @@ ERRORHANDLER:
 							       MAC_DISCONNECTED,
 							       NULL,
 							       pstrHostIFconnectAttr->arg);
-			hif_drv->enuHostIFstate = HOST_IF_IDLE;
+			hif_drv->hif_state = HOST_IF_IDLE;
 			kfree(strConnectInfo.pu8ReqIEs);
 			strConnectInfo.pu8ReqIEs = NULL;
 
@@ -1267,7 +1260,7 @@ ERRORHANDLER:
 	return result;
 }
 
-static s32 Handle_FlushConnect(struct host_if_drv *hif_drv)
+static s32 Handle_FlushConnect(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct wid strWIDList[5];
@@ -1303,8 +1296,9 @@ static s32 Handle_FlushConnect(struct host_if_drv *hif_drv)
 
 	u32WidsCount++;
 
-	result = send_config_pkt(SET_CFG, strWIDList, u32WidsCount,
-				 get_id_from_handler(join_req_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, strWIDList,
+				      u32WidsCount,
+				      wilc_get_vif_idx(join_req_vif));
 	if (result) {
 		PRINT_ER("failed to send config packet\n");
 		result = -EINVAL;
@@ -1313,43 +1307,44 @@ static s32 Handle_FlushConnect(struct host_if_drv *hif_drv)
 	return result;
 }
 
-static s32 Handle_ConnectTimeout(struct host_if_drv *hif_drv)
+static s32 Handle_ConnectTimeout(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	tstrConnectInfo strConnectInfo;
 	struct wid wid;
 	u16 u16DummyReasonCode = 0;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("Driver handler is NULL\n");
 		return result;
 	}
 
-	hif_drv->enuHostIFstate = HOST_IF_IDLE;
+	hif_drv->hif_state = HOST_IF_IDLE;
 
 	scan_while_connected = false;
 
 	memset(&strConnectInfo, 0, sizeof(tstrConnectInfo));
 
-	if (hif_drv->usr_conn_req.pfUserConnectResult) {
+	if (hif_drv->usr_conn_req.conn_result) {
 		if (hif_drv->usr_conn_req.pu8bssid) {
 			memcpy(strConnectInfo.au8bssid,
 			       hif_drv->usr_conn_req.pu8bssid, 6);
 		}
 
-		if (hif_drv->usr_conn_req.pu8ConnReqIEs) {
-			strConnectInfo.ReqIEsLen = hif_drv->usr_conn_req.ConnReqIEsLen;
-			strConnectInfo.pu8ReqIEs = kmalloc(hif_drv->usr_conn_req.ConnReqIEsLen, GFP_KERNEL);
+		if (hif_drv->usr_conn_req.ies) {
+			strConnectInfo.ReqIEsLen = hif_drv->usr_conn_req.ies_len;
+			strConnectInfo.pu8ReqIEs = kmalloc(hif_drv->usr_conn_req.ies_len, GFP_KERNEL);
 			memcpy(strConnectInfo.pu8ReqIEs,
-			       hif_drv->usr_conn_req.pu8ConnReqIEs,
-			       hif_drv->usr_conn_req.ConnReqIEsLen);
+			       hif_drv->usr_conn_req.ies,
+			       hif_drv->usr_conn_req.ies_len);
 		}
 
-		hif_drv->usr_conn_req.pfUserConnectResult(CONN_DISCONN_EVENT_CONN_RESP,
-							  &strConnectInfo,
-							  MAC_DISCONNECTED,
-							  NULL,
-							  hif_drv->usr_conn_req.u32UserConnectPvoid);
+		hif_drv->usr_conn_req.conn_result(CONN_DISCONN_EVENT_CONN_RESP,
+						  &strConnectInfo,
+						  MAC_DISCONNECTED,
+						  NULL,
+						  hif_drv->usr_conn_req.arg);
 
 		kfree(strConnectInfo.pu8ReqIEs);
 		strConnectInfo.pu8ReqIEs = NULL;
@@ -1364,25 +1359,28 @@ static s32 Handle_ConnectTimeout(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Sending disconnect request\n");
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send dissconect config packet\n");
 
-	hif_drv->usr_conn_req.ssidLen = 0;
+	hif_drv->usr_conn_req.ssid_len = 0;
 	kfree(hif_drv->usr_conn_req.pu8ssid);
+	hif_drv->usr_conn_req.pu8ssid = NULL;
 	kfree(hif_drv->usr_conn_req.pu8bssid);
-	hif_drv->usr_conn_req.ConnReqIEsLen = 0;
-	kfree(hif_drv->usr_conn_req.pu8ConnReqIEs);
+	hif_drv->usr_conn_req.pu8bssid = NULL;
+	hif_drv->usr_conn_req.ies_len = 0;
+	kfree(hif_drv->usr_conn_req.ies);
+	hif_drv->usr_conn_req.ies = NULL;
 
-	eth_zero_addr(u8ConnectedSSID);
+	eth_zero_addr(wilc_connected_ssid);
 
-	if (join_req && join_req_drv == hif_drv) {
+	if (join_req && join_req_vif == vif) {
 		kfree(join_req);
 		join_req = NULL;
 	}
 
-	if (info_element && join_req_drv == hif_drv) {
+	if (info_element && join_req_vif == vif) {
 		kfree(info_element);
 		info_element = NULL;
 	}
@@ -1390,7 +1388,7 @@ static s32 Handle_ConnectTimeout(struct host_if_drv *hif_drv)
 	return result;
 }
 
-static s32 Handle_RcvdNtwrkInfo(struct host_if_drv *hif_drv,
+static s32 Handle_RcvdNtwrkInfo(struct wilc_vif *vif,
 				struct rcvd_net_info *pstrRcvdNetworkInfo)
 {
 	u32 i;
@@ -1398,30 +1396,31 @@ static s32 Handle_RcvdNtwrkInfo(struct host_if_drv *hif_drv,
 	s32 result = 0;
 	tstrNetworkInfo *pstrNetworkInfo = NULL;
 	void *pJoinParams = NULL;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	bNewNtwrkFound = true;
 	PRINT_INFO(HOSTINF_DBG, "Handling received network info\n");
 
-	if (hif_drv->usr_scan_req.pfUserScanResult) {
+	if (hif_drv->usr_scan_req.scan_result) {
 		PRINT_D(HOSTINF_DBG, "State: Scanning, parsing network information received\n");
-		parse_network_info(pstrRcvdNetworkInfo->buffer, &pstrNetworkInfo);
+		wilc_parse_network_info(pstrRcvdNetworkInfo->buffer, &pstrNetworkInfo);
 		if ((!pstrNetworkInfo) ||
-		    (!hif_drv->usr_scan_req.pfUserScanResult)) {
+		    (!hif_drv->usr_scan_req.scan_result)) {
 			PRINT_ER("driver is null\n");
 			result = -EINVAL;
 			goto done;
 		}
 
-		for (i = 0; i < hif_drv->usr_scan_req.u32RcvdChCount; i++) {
-			if ((hif_drv->usr_scan_req.astrFoundNetworkInfo[i].au8bssid) &&
+		for (i = 0; i < hif_drv->usr_scan_req.rcvd_ch_cnt; i++) {
+			if ((hif_drv->usr_scan_req.net_info[i].au8bssid) &&
 			    (pstrNetworkInfo->au8bssid)) {
-				if (memcmp(hif_drv->usr_scan_req.astrFoundNetworkInfo[i].au8bssid,
+				if (memcmp(hif_drv->usr_scan_req.net_info[i].au8bssid,
 					   pstrNetworkInfo->au8bssid, 6) == 0) {
-					if (pstrNetworkInfo->s8rssi <= hif_drv->usr_scan_req.astrFoundNetworkInfo[i].s8rssi) {
+					if (pstrNetworkInfo->s8rssi <= hif_drv->usr_scan_req.net_info[i].s8rssi) {
 						PRINT_D(HOSTINF_DBG, "Network previously discovered\n");
 						goto done;
 					} else {
-						hif_drv->usr_scan_req.astrFoundNetworkInfo[i].s8rssi = pstrNetworkInfo->s8rssi;
+						hif_drv->usr_scan_req.net_info[i].s8rssi = pstrNetworkInfo->s8rssi;
 						bNewNtwrkFound = false;
 						break;
 					}
@@ -1432,30 +1431,30 @@ static s32 Handle_RcvdNtwrkInfo(struct host_if_drv *hif_drv,
 		if (bNewNtwrkFound) {
 			PRINT_D(HOSTINF_DBG, "New network found\n");
 
-			if (hif_drv->usr_scan_req.u32RcvdChCount < MAX_NUM_SCANNED_NETWORKS) {
-				hif_drv->usr_scan_req.astrFoundNetworkInfo[hif_drv->usr_scan_req.u32RcvdChCount].s8rssi = pstrNetworkInfo->s8rssi;
+			if (hif_drv->usr_scan_req.rcvd_ch_cnt < MAX_NUM_SCANNED_NETWORKS) {
+				hif_drv->usr_scan_req.net_info[hif_drv->usr_scan_req.rcvd_ch_cnt].s8rssi = pstrNetworkInfo->s8rssi;
 
-				if (hif_drv->usr_scan_req.astrFoundNetworkInfo[hif_drv->usr_scan_req.u32RcvdChCount].au8bssid &&
+				if (hif_drv->usr_scan_req.net_info[hif_drv->usr_scan_req.rcvd_ch_cnt].au8bssid &&
 				    pstrNetworkInfo->au8bssid) {
-					memcpy(hif_drv->usr_scan_req.astrFoundNetworkInfo[hif_drv->usr_scan_req.u32RcvdChCount].au8bssid,
+					memcpy(hif_drv->usr_scan_req.net_info[hif_drv->usr_scan_req.rcvd_ch_cnt].au8bssid,
 					       pstrNetworkInfo->au8bssid, 6);
 
-					hif_drv->usr_scan_req.u32RcvdChCount++;
+					hif_drv->usr_scan_req.rcvd_ch_cnt++;
 
 					pstrNetworkInfo->bNewNetwork = true;
 					pJoinParams = host_int_ParseJoinBssParam(pstrNetworkInfo);
 
-					hif_drv->usr_scan_req.pfUserScanResult(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
-									       hif_drv->usr_scan_req.u32UserScanPvoid,
-									       pJoinParams);
+					hif_drv->usr_scan_req.scan_result(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
+									  hif_drv->usr_scan_req.arg,
+									  pJoinParams);
 				}
 			} else {
 				PRINT_WRN(HOSTINF_DBG, "Discovered networks exceeded max. limit\n");
 			}
 		} else {
 			pstrNetworkInfo->bNewNetwork = false;
-			hif_drv->usr_scan_req.pfUserScanResult(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
-							       hif_drv->usr_scan_req.u32UserScanPvoid, NULL);
+			hif_drv->usr_scan_req.scan_result(SCAN_EVENT_NETWORK_FOUND, pstrNetworkInfo,
+							  hif_drv->usr_scan_req.arg, NULL);
 		}
 	}
 
@@ -1464,14 +1463,19 @@ done:
 	pstrRcvdNetworkInfo->buffer = NULL;
 
 	if (pstrNetworkInfo) {
-		DeallocateNetworkInfo(pstrNetworkInfo);
+		wilc_dealloc_network_info(pstrNetworkInfo);
 		pstrNetworkInfo = NULL;
 	}
 
 	return result;
 }
 
-static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
+static s32 host_int_get_assoc_res_info(struct wilc_vif *vif,
+				       u8 *pu8AssocRespInfo,
+				       u32 u32MaxAssocRespInfoLen,
+				       u32 *pu32RcvdAssocRespInfoLen);
+
+static s32 Handle_RcvdGnrlAsyncInfo(struct wilc_vif *vif,
 				    struct rcvd_async_info *pstrRcvdGnrlAsyncInfo)
 {
 	s32 result = 0;
@@ -1486,19 +1490,20 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 	tstrConnectInfo strConnectInfo;
 	tstrDisconnectNotifInfo strDisconnectNotifInfo;
 	s32 s32Err = 0;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("Driver handler is NULL\n");
 		return -ENODEV;
 	}
-	PRINT_D(GENERIC_DBG, "Current State = %d,Received state = %d\n", hif_drv->enuHostIFstate,
-		pstrRcvdGnrlAsyncInfo->buffer[7]);
+	PRINT_D(GENERIC_DBG, "Current State = %d,Received state = %d\n",
+		hif_drv->hif_state, pstrRcvdGnrlAsyncInfo->buffer[7]);
 
-	if ((hif_drv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) ||
-	    (hif_drv->enuHostIFstate == HOST_IF_CONNECTED) ||
-	    hif_drv->usr_scan_req.pfUserScanResult) {
+	if ((hif_drv->hif_state == HOST_IF_WAITING_CONN_RESP) ||
+	    (hif_drv->hif_state == HOST_IF_CONNECTED) ||
+	    hif_drv->usr_scan_req.scan_result) {
 		if (!pstrRcvdGnrlAsyncInfo->buffer ||
-		    !hif_drv->usr_conn_req.pfUserConnectResult) {
+		    !hif_drv->usr_conn_req.conn_result) {
 			PRINT_ER("driver is null\n");
 			return -EINVAL;
 		}
@@ -1518,8 +1523,8 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 		u8MacStatusReasonCode = pstrRcvdGnrlAsyncInfo->buffer[8];
 		u8MacStatusAdditionalInfo = pstrRcvdGnrlAsyncInfo->buffer[9];
 		PRINT_INFO(HOSTINF_DBG, "Recieved MAC status = %d with Reason = %d , Info = %d\n", u8MacStatus, u8MacStatusReasonCode, u8MacStatusAdditionalInfo);
-		if (hif_drv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
-			u32 u32RcvdAssocRespInfoLen;
+		if (hif_drv->hif_state == HOST_IF_WAITING_CONN_RESP) {
+			u32 u32RcvdAssocRespInfoLen = 0;
 			tstrConnectRespInfo *pstrConnectRespInfo = NULL;
 
 			PRINT_D(HOSTINF_DBG, "Recieved MAC status = %d with Reason = %d , Code = %d\n", u8MacStatus, u8MacStatusReasonCode, u8MacStatusAdditionalInfo);
@@ -1529,7 +1534,7 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 			if (u8MacStatus == MAC_CONNECTED) {
 				memset(rcv_assoc_resp, 0, MAX_ASSOC_RESP_FRAME_SIZE);
 
-				host_int_get_assoc_res_info(hif_drv,
+				host_int_get_assoc_res_info(vif,
 							    rcv_assoc_resp,
 							    MAX_ASSOC_RESP_FRAME_SIZE,
 							    &u32RcvdAssocRespInfoLen);
@@ -1538,10 +1543,10 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 
 				if (u32RcvdAssocRespInfoLen != 0) {
 					PRINT_D(HOSTINF_DBG, "Parsing association response\n");
-					s32Err = ParseAssocRespInfo(rcv_assoc_resp, u32RcvdAssocRespInfoLen,
+					s32Err = wilc_parse_assoc_resp_info(rcv_assoc_resp, u32RcvdAssocRespInfoLen,
 								    &pstrConnectRespInfo);
 					if (s32Err) {
-						PRINT_ER("ParseAssocRespInfo() returned error %d\n", s32Err);
+						PRINT_ER("wilc_parse_assoc_resp_info() returned error %d\n", s32Err);
 					} else {
 						strConnectInfo.u16ConnectStatus = pstrConnectRespInfo->u16ConnectStatus;
 
@@ -1556,7 +1561,7 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 						}
 
 						if (pstrConnectRespInfo) {
-							DeallocateAssocRespInfo(pstrConnectRespInfo);
+							wilc_dealloc_assoc_resp_info(pstrConnectRespInfo);
 							pstrConnectRespInfo = NULL;
 						}
 					}
@@ -1566,11 +1571,10 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 			if ((u8MacStatus == MAC_CONNECTED) &&
 			    (strConnectInfo.u16ConnectStatus != SUCCESSFUL_STATUSCODE))	{
 				PRINT_ER("Received MAC status is MAC_CONNECTED while the received status code in Asoc Resp is not SUCCESSFUL_STATUSCODE\n");
-				eth_zero_addr(u8ConnectedSSID);
-
+				eth_zero_addr(wilc_connected_ssid);
 			} else if (u8MacStatus == MAC_DISCONNECTED)    {
 				PRINT_ER("Received MAC status is MAC_DISCONNECTED\n");
-				eth_zero_addr(u8ConnectedSSID);
+				eth_zero_addr(wilc_connected_ssid);
 			}
 
 			if (hif_drv->usr_conn_req.pu8bssid) {
@@ -1579,40 +1583,40 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 
 				if ((u8MacStatus == MAC_CONNECTED) &&
 				    (strConnectInfo.u16ConnectStatus == SUCCESSFUL_STATUSCODE))	{
-					memcpy(hif_drv->au8AssociatedBSSID,
+					memcpy(hif_drv->assoc_bssid,
 					       hif_drv->usr_conn_req.pu8bssid, ETH_ALEN);
 				}
 			}
 
-			if (hif_drv->usr_conn_req.pu8ConnReqIEs) {
-				strConnectInfo.ReqIEsLen = hif_drv->usr_conn_req.ConnReqIEsLen;
-				strConnectInfo.pu8ReqIEs = kmalloc(hif_drv->usr_conn_req.ConnReqIEsLen, GFP_KERNEL);
+			if (hif_drv->usr_conn_req.ies) {
+				strConnectInfo.ReqIEsLen = hif_drv->usr_conn_req.ies_len;
+				strConnectInfo.pu8ReqIEs = kmalloc(hif_drv->usr_conn_req.ies_len, GFP_KERNEL);
 				memcpy(strConnectInfo.pu8ReqIEs,
-				       hif_drv->usr_conn_req.pu8ConnReqIEs,
-				       hif_drv->usr_conn_req.ConnReqIEsLen);
+				       hif_drv->usr_conn_req.ies,
+				       hif_drv->usr_conn_req.ies_len);
 			}
 
-			del_timer(&hif_drv->hConnectTimer);
-			hif_drv->usr_conn_req.pfUserConnectResult(CONN_DISCONN_EVENT_CONN_RESP,
-								  &strConnectInfo,
-								  u8MacStatus,
-								  NULL,
-								  hif_drv->usr_conn_req.u32UserConnectPvoid);
+			del_timer(&hif_drv->connect_timer);
+			hif_drv->usr_conn_req.conn_result(CONN_DISCONN_EVENT_CONN_RESP,
+							  &strConnectInfo,
+							  u8MacStatus,
+							  NULL,
+							  hif_drv->usr_conn_req.arg);
 
 			if ((u8MacStatus == MAC_CONNECTED) &&
 			    (strConnectInfo.u16ConnectStatus == SUCCESSFUL_STATUSCODE))	{
-				host_int_set_power_mgmt(hif_drv, 0, 0);
+				wilc_set_power_mgmt(vif, 0, 0);
 
 				PRINT_D(HOSTINF_DBG, "MAC status : CONNECTED and Connect Status : Successful\n");
-				hif_drv->enuHostIFstate = HOST_IF_CONNECTED;
+				hif_drv->hif_state = HOST_IF_CONNECTED;
 
 				PRINT_D(GENERIC_DBG, "Obtaining an IP, Disable Scan\n");
-				g_obtainingIP = true;
-				mod_timer(&hDuringIpTimer,
+				wilc_optaining_ip = true;
+				mod_timer(&wilc_during_ip_timer,
 					  jiffies + msecs_to_jiffies(10000));
 			} else {
 				PRINT_D(HOSTINF_DBG, "MAC status : %d and Connect Status : %d\n", u8MacStatus, strConnectInfo.u16ConnectStatus);
-				hif_drv->enuHostIFstate = HOST_IF_IDLE;
+				hif_drv->hif_state = HOST_IF_IDLE;
 				scan_while_connected = false;
 			}
 
@@ -1621,69 +1625,75 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 
 			kfree(strConnectInfo.pu8ReqIEs);
 			strConnectInfo.pu8ReqIEs = NULL;
-			hif_drv->usr_conn_req.ssidLen = 0;
+			hif_drv->usr_conn_req.ssid_len = 0;
 			kfree(hif_drv->usr_conn_req.pu8ssid);
+			hif_drv->usr_conn_req.pu8ssid = NULL;
 			kfree(hif_drv->usr_conn_req.pu8bssid);
-			hif_drv->usr_conn_req.ConnReqIEsLen = 0;
-			kfree(hif_drv->usr_conn_req.pu8ConnReqIEs);
+			hif_drv->usr_conn_req.pu8bssid = NULL;
+			hif_drv->usr_conn_req.ies_len = 0;
+			kfree(hif_drv->usr_conn_req.ies);
+			hif_drv->usr_conn_req.ies = NULL;
 		} else if ((u8MacStatus == MAC_DISCONNECTED) &&
-			   (hif_drv->enuHostIFstate == HOST_IF_CONNECTED)) {
+			   (hif_drv->hif_state == HOST_IF_CONNECTED)) {
 			PRINT_D(HOSTINF_DBG, "Received MAC_DISCONNECTED from the FW\n");
 
 			memset(&strDisconnectNotifInfo, 0, sizeof(tstrDisconnectNotifInfo));
 
-			if (hif_drv->usr_scan_req.pfUserScanResult) {
+			if (hif_drv->usr_scan_req.scan_result) {
 				PRINT_D(HOSTINF_DBG, "\n\n<< Abort the running OBSS Scan >>\n\n");
-				del_timer(&hif_drv->hScanTimer);
-				Handle_ScanDone((void *)hif_drv, SCAN_EVENT_ABORTED);
+				del_timer(&hif_drv->scan_timer);
+				Handle_ScanDone(vif, SCAN_EVENT_ABORTED);
 			}
 
 			strDisconnectNotifInfo.u16reason = 0;
 			strDisconnectNotifInfo.ie = NULL;
 			strDisconnectNotifInfo.ie_len = 0;
 
-			if (hif_drv->usr_conn_req.pfUserConnectResult) {
-				g_obtainingIP = false;
-				host_int_set_power_mgmt(hif_drv, 0, 0);
+			if (hif_drv->usr_conn_req.conn_result) {
+				wilc_optaining_ip = false;
+				wilc_set_power_mgmt(vif, 0, 0);
 
-				hif_drv->usr_conn_req.pfUserConnectResult(CONN_DISCONN_EVENT_DISCONN_NOTIF,
-									  NULL,
-									  0,
-									  &strDisconnectNotifInfo,
-									  hif_drv->usr_conn_req.u32UserConnectPvoid);
+				hif_drv->usr_conn_req.conn_result(CONN_DISCONN_EVENT_DISCONN_NOTIF,
+								  NULL,
+								  0,
+								  &strDisconnectNotifInfo,
+								  hif_drv->usr_conn_req.arg);
 			} else {
 				PRINT_ER("Connect result callback function is NULL\n");
 			}
 
-			eth_zero_addr(hif_drv->au8AssociatedBSSID);
+			eth_zero_addr(hif_drv->assoc_bssid);
 
-			hif_drv->usr_conn_req.ssidLen = 0;
+			hif_drv->usr_conn_req.ssid_len = 0;
 			kfree(hif_drv->usr_conn_req.pu8ssid);
+			hif_drv->usr_conn_req.pu8ssid = NULL;
 			kfree(hif_drv->usr_conn_req.pu8bssid);
-			hif_drv->usr_conn_req.ConnReqIEsLen = 0;
-			kfree(hif_drv->usr_conn_req.pu8ConnReqIEs);
+			hif_drv->usr_conn_req.pu8bssid = NULL;
+			hif_drv->usr_conn_req.ies_len = 0;
+			kfree(hif_drv->usr_conn_req.ies);
+			hif_drv->usr_conn_req.ies = NULL;
 
-			if (join_req && join_req_drv == hif_drv) {
+			if (join_req && join_req_vif == vif) {
 				kfree(join_req);
 				join_req = NULL;
 			}
 
-			if (info_element && join_req_drv == hif_drv) {
+			if (info_element && join_req_vif == vif) {
 				kfree(info_element);
 				info_element = NULL;
 			}
 
-			hif_drv->enuHostIFstate = HOST_IF_IDLE;
+			hif_drv->hif_state = HOST_IF_IDLE;
 			scan_while_connected = false;
 
 		} else if ((u8MacStatus == MAC_DISCONNECTED) &&
-			   (hif_drv->usr_scan_req.pfUserScanResult)) {
+			   (hif_drv->usr_scan_req.scan_result)) {
 			PRINT_D(HOSTINF_DBG, "Received MAC_DISCONNECTED from the FW while scanning\n");
 			PRINT_D(HOSTINF_DBG, "\n\n<< Abort the running Scan >>\n\n");
 
-			del_timer(&hif_drv->hScanTimer);
-			if (hif_drv->usr_scan_req.pfUserScanResult)
-				Handle_ScanDone(hif_drv, SCAN_EVENT_ABORTED);
+			del_timer(&hif_drv->scan_timer);
+			if (hif_drv->usr_scan_req.scan_result)
+				Handle_ScanDone(vif, SCAN_EVENT_ABORTED);
 		}
 	}
 
@@ -1693,7 +1703,7 @@ static s32 Handle_RcvdGnrlAsyncInfo(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static int Handle_Key(struct host_if_drv *hif_drv,
+static int Handle_Key(struct wilc_vif *vif,
 		      struct key_attr *pstrHostIFkeyAttr)
 {
 	s32 result = 0;
@@ -1703,6 +1713,7 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 	u8 *pu8keybuf;
 	s8 s8idxarray[1];
 	s8 ret = 0;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	switch (pstrHostIFkeyAttr->type) {
 	case WEP:
@@ -1742,12 +1753,11 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 			strWIDList[3].size = pstrHostIFkeyAttr->attr.wep.key_len;
 			strWIDList[3].val = (s8 *)pu8keybuf;
 
-			result = send_config_pkt(SET_CFG, strWIDList, 4,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						strWIDList, 4,
+						wilc_get_vif_idx(vif));
 			kfree(pu8keybuf);
-		}
-
-		if (pstrHostIFkeyAttr->action & ADDKEY) {
+		} else if (pstrHostIFkeyAttr->action & ADDKEY) {
 			PRINT_D(HOSTINF_DBG, "Handling WEP key\n");
 			pu8keybuf = kmalloc(pstrHostIFkeyAttr->attr.wep.key_len + 2, GFP_KERNEL);
 			if (!pu8keybuf) {
@@ -1765,8 +1775,9 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 			wid.val = (s8 *)pu8keybuf;
 			wid.size = pstrHostIFkeyAttr->attr.wep.key_len + 2;
 
-			result = send_config_pkt(SET_CFG, &wid, 1,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						&wid, 1,
+						wilc_get_vif_idx(vif));
 			kfree(pu8keybuf);
 		} else if (pstrHostIFkeyAttr->action & REMOVEKEY) {
 			PRINT_D(HOSTINF_DBG, "Removing key\n");
@@ -1777,8 +1788,9 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 			wid.val = s8idxarray;
 			wid.size = 1;
 
-			result = send_config_pkt(SET_CFG, &wid, 1,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						&wid, 1,
+						wilc_get_vif_idx(vif));
 		} else {
 			wid.id = (u16)WID_KEY_ID;
 			wid.type = WID_CHAR;
@@ -1787,13 +1799,14 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 
 			PRINT_D(HOSTINF_DBG, "Setting default key index\n");
 
-			result = send_config_pkt(SET_CFG, &wid, 1,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						&wid, 1,
+						wilc_get_vif_idx(vif));
 		}
-		up(&hif_drv->hSemTestKeyBlock);
+		up(&hif_drv->sem_test_key_block);
 		break;
 
-	case WPARxGtk:
+	case WPA_RX_GTK:
 		if (pstrHostIFkeyAttr->action & ADDKEY_AP) {
 			pu8keybuf = kzalloc(RX_MIC_KEY_MSG_LEN, GFP_KERNEL);
 			if (!pu8keybuf) {
@@ -1820,14 +1833,13 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 			strWIDList[1].val = (s8 *)pu8keybuf;
 			strWIDList[1].size = RX_MIC_KEY_MSG_LEN;
 
-			result = send_config_pkt(SET_CFG, strWIDList, 2,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						strWIDList, 2,
+						wilc_get_vif_idx(vif));
 
 			kfree(pu8keybuf);
-			up(&hif_drv->hSemTestKeyBlock);
-		}
-
-		if (pstrHostIFkeyAttr->action & ADDKEY) {
+			up(&hif_drv->sem_test_key_block);
+		} else if (pstrHostIFkeyAttr->action & ADDKEY) {
 			PRINT_D(HOSTINF_DBG, "Handling group key(Rx) function\n");
 
 			pu8keybuf = kzalloc(RX_MIC_KEY_MSG_LEN, GFP_KERNEL);
@@ -1837,10 +1849,10 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 				goto _WPARxGtk_end_case_;
 			}
 
-			if (hif_drv->enuHostIFstate == HOST_IF_CONNECTED)
-				memcpy(pu8keybuf, hif_drv->au8AssociatedBSSID, ETH_ALEN);
+			if (hif_drv->hif_state == HOST_IF_CONNECTED)
+				memcpy(pu8keybuf, hif_drv->assoc_bssid, ETH_ALEN);
 			else
-				PRINT_ER("Couldn't handle WPARxGtk while enuHostIFstate is not HOST_IF_CONNECTED\n");
+				PRINT_ER("Couldn't handle WPARxGtk while state is not HOST_IF_CONNECTED\n");
 
 			memcpy(pu8keybuf + 6, pstrHostIFkeyAttr->attr.wpa.seq, 8);
 			memcpy(pu8keybuf + 14, &pstrHostIFkeyAttr->attr.wpa.index, 1);
@@ -1853,11 +1865,12 @@ static int Handle_Key(struct host_if_drv *hif_drv,
 			wid.val = (s8 *)pu8keybuf;
 			wid.size = RX_MIC_KEY_MSG_LEN;
 
-			result = send_config_pkt(SET_CFG, &wid, 1,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						&wid, 1,
+						wilc_get_vif_idx(vif));
 
 			kfree(pu8keybuf);
-			up(&hif_drv->hSemTestKeyBlock);
+			up(&hif_drv->sem_test_key_block);
 		}
 _WPARxGtk_end_case_:
 		kfree(pstrHostIFkeyAttr->attr.wpa.key);
@@ -1867,7 +1880,7 @@ _WPARxGtk_end_case_:
 
 		break;
 
-	case WPAPtk:
+	case WPA_PTK:
 		if (pstrHostIFkeyAttr->action & ADDKEY_AP) {
 			pu8keybuf = kmalloc(PTK_KEY_MSG_LEN + 1, GFP_KERNEL);
 			if (!pu8keybuf) {
@@ -1892,12 +1905,12 @@ _WPARxGtk_end_case_:
 			strWIDList[1].val = (s8 *)pu8keybuf;
 			strWIDList[1].size = PTK_KEY_MSG_LEN + 1;
 
-			result = send_config_pkt(SET_CFG, strWIDList, 2,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						strWIDList, 2,
+						wilc_get_vif_idx(vif));
 			kfree(pu8keybuf);
-			up(&hif_drv->hSemTestKeyBlock);
-		}
-		if (pstrHostIFkeyAttr->action & ADDKEY) {
+			up(&hif_drv->sem_test_key_block);
+		} else if (pstrHostIFkeyAttr->action & ADDKEY) {
 			pu8keybuf = kmalloc(PTK_KEY_MSG_LEN, GFP_KERNEL);
 			if (!pu8keybuf) {
 				PRINT_ER("No buffer to send PTK Key\n");
@@ -1915,10 +1928,11 @@ _WPARxGtk_end_case_:
 			wid.val = (s8 *)pu8keybuf;
 			wid.size = PTK_KEY_MSG_LEN;
 
-			result = send_config_pkt(SET_CFG, &wid, 1,
-						 get_id_from_handler(hif_drv));
+			result = wilc_send_config_pkt(vif->wilc, SET_CFG,
+						&wid, 1,
+						wilc_get_vif_idx(vif));
 			kfree(pu8keybuf);
-			up(&hif_drv->hSemTestKeyBlock);
+			up(&hif_drv->sem_test_key_block);
 		}
 
 _WPAPtk_end_case_:
@@ -1950,8 +1964,8 @@ _WPAPtk_end_case_:
 		wid.val = (s8 *)pu8keybuf;
 		wid.size = (pstrHostIFkeyAttr->attr.pmkid.numpmkid * PMKSA_KEY_LEN) + 1;
 
-		result = send_config_pkt(SET_CFG, &wid, 1,
-					 get_id_from_handler(hif_drv));
+		result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+					 wilc_get_vif_idx(vif));
 
 		kfree(pu8keybuf);
 		break;
@@ -1963,9 +1977,10 @@ _WPAPtk_end_case_:
 	return result;
 }
 
-static void Handle_Disconnect(struct host_if_drv *hif_drv)
+static void Handle_Disconnect(struct wilc_vif *vif)
 {
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	s32 result = 0;
 	u16 u16DummyReasonCode = 0;
@@ -1977,13 +1992,13 @@ static void Handle_Disconnect(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Sending disconnect request\n");
 
-	g_obtainingIP = false;
-	host_int_set_power_mgmt(hif_drv, 0, 0);
+	wilc_optaining_ip = false;
+	wilc_set_power_mgmt(vif, 0, 0);
 
-	eth_zero_addr(u8ConnectedSSID);
+	eth_zero_addr(wilc_connected_ssid);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to send dissconect config packet\n");
@@ -1996,66 +2011,75 @@ static void Handle_Disconnect(struct host_if_drv *hif_drv)
 		strDisconnectNotifInfo.ie = NULL;
 		strDisconnectNotifInfo.ie_len = 0;
 
-		if (hif_drv->usr_scan_req.pfUserScanResult) {
-			del_timer(&hif_drv->hScanTimer);
-			hif_drv->usr_scan_req.pfUserScanResult(SCAN_EVENT_ABORTED, NULL,
-							       hif_drv->usr_scan_req.u32UserScanPvoid, NULL);
-
-			hif_drv->usr_scan_req.pfUserScanResult = NULL;
+		if (hif_drv->usr_scan_req.scan_result) {
+			del_timer(&hif_drv->scan_timer);
+			hif_drv->usr_scan_req.scan_result(SCAN_EVENT_ABORTED,
+							  NULL,
+							  hif_drv->usr_scan_req.arg,
+							  NULL);
+			hif_drv->usr_scan_req.scan_result = NULL;
 		}
 
-		if (hif_drv->usr_conn_req.pfUserConnectResult) {
-			if (hif_drv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
+		if (hif_drv->usr_conn_req.conn_result) {
+			if (hif_drv->hif_state == HOST_IF_WAITING_CONN_RESP) {
 				PRINT_D(HOSTINF_DBG, "Upper layer requested termination of connection\n");
-				del_timer(&hif_drv->hConnectTimer);
+				del_timer(&hif_drv->connect_timer);
 			}
 
-			hif_drv->usr_conn_req.pfUserConnectResult(CONN_DISCONN_EVENT_DISCONN_NOTIF, NULL,
-								  0, &strDisconnectNotifInfo, hif_drv->usr_conn_req.u32UserConnectPvoid);
+			hif_drv->usr_conn_req.conn_result(CONN_DISCONN_EVENT_DISCONN_NOTIF,
+							  NULL,
+							  0,
+							  &strDisconnectNotifInfo,
+							  hif_drv->usr_conn_req.arg);
 		} else {
-			PRINT_ER("usr_conn_req.pfUserConnectResult = NULL\n");
+			PRINT_ER("usr_conn_req.conn_result = NULL\n");
 		}
 
 		scan_while_connected = false;
 
-		hif_drv->enuHostIFstate = HOST_IF_IDLE;
+		hif_drv->hif_state = HOST_IF_IDLE;
 
-		eth_zero_addr(hif_drv->au8AssociatedBSSID);
+		eth_zero_addr(hif_drv->assoc_bssid);
 
-		hif_drv->usr_conn_req.ssidLen = 0;
+		hif_drv->usr_conn_req.ssid_len = 0;
 		kfree(hif_drv->usr_conn_req.pu8ssid);
+		hif_drv->usr_conn_req.pu8ssid = NULL;
 		kfree(hif_drv->usr_conn_req.pu8bssid);
-		hif_drv->usr_conn_req.ConnReqIEsLen = 0;
-		kfree(hif_drv->usr_conn_req.pu8ConnReqIEs);
+		hif_drv->usr_conn_req.pu8bssid = NULL;
+		hif_drv->usr_conn_req.ies_len = 0;
+		kfree(hif_drv->usr_conn_req.ies);
+		hif_drv->usr_conn_req.ies = NULL;
 
-		if (join_req && join_req_drv == hif_drv) {
+		if (join_req && join_req_vif == vif) {
 			kfree(join_req);
 			join_req = NULL;
 		}
 
-		if (info_element && join_req_drv == hif_drv) {
+		if (info_element && join_req_vif == vif) {
 			kfree(info_element);
 			info_element = NULL;
 		}
 	}
 
-	up(&hif_drv->hSemTestDisconnectBlock);
+	up(&hif_drv->sem_test_disconn_block);
 }
 
-void resolve_disconnect_aberration(struct host_if_drv *hif_drv)
+void wilc_resolve_disconnect_aberration(struct wilc_vif *vif)
 {
-	if (!hif_drv)
+	if (!vif->hif_drv)
 		return;
-	if ((hif_drv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) || (hif_drv->enuHostIFstate == HOST_IF_CONNECTING)) {
+	if ((vif->hif_drv->hif_state == HOST_IF_WAITING_CONN_RESP) ||
+	    (vif->hif_drv->hif_state == HOST_IF_CONNECTING)) {
 		PRINT_D(HOSTINF_DBG, "\n\n<< correcting Supplicant state machine >>\n\n");
-		host_int_disconnect(hif_drv, 1);
+		wilc_disconnect(vif, 1);
 	}
 }
 
-static s32 Handle_GetChnl(struct host_if_drv *hif_drv)
+static s32 Handle_GetChnl(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	wid.id = (u16)WID_CURRENT_CHANNEL;
 	wid.type = WID_CHAR;
@@ -2064,20 +2088,20 @@ static s32 Handle_GetChnl(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Getting channel value\n");
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to get channel number\n");
 		result = -EFAULT;
 	}
 
-	up(&hif_drv->hSemGetCHNL);
+	up(&hif_drv->sem_get_chnl);
 
 	return result;
 }
 
-static void Handle_GetRssi(struct host_if_drv *hif_drv)
+static void Handle_GetRssi(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct wid wid;
@@ -2089,20 +2113,21 @@ static void Handle_GetRssi(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Getting RSSI value\n");
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result) {
 		PRINT_ER("Failed to get RSSI value\n");
 		result = -EFAULT;
 	}
 
-	up(&hif_drv->hSemGetRSSI);
+	up(&vif->hif_drv->sem_get_rssi);
 }
 
-static void Handle_GetLinkspeed(struct host_if_drv *hif_drv)
+static void Handle_GetLinkspeed(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	link_speed = 0;
 
@@ -2113,17 +2138,18 @@ static void Handle_GetLinkspeed(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Getting LINKSPEED value\n");
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result) {
 		PRINT_ER("Failed to get LINKSPEED value\n");
 		result = -EFAULT;
 	}
 
-	up(&hif_drv->hSemGetLINKSPEED);
+	up(&hif_drv->sem_get_link_speed);
 }
 
-s32 Handle_GetStatistics(struct host_if_drv *hif_drv, struct rf_info *pstrStatistics)
+static s32 Handle_GetStatistics(struct wilc_vif *vif,
+				struct rf_info *pstrStatistics)
 {
 	struct wid strWIDList[5];
 	u32 u32WidsCount = 0, result = 0;
@@ -2131,35 +2157,36 @@ s32 Handle_GetStatistics(struct host_if_drv *hif_drv, struct rf_info *pstrStatis
 	strWIDList[u32WidsCount].id = WID_LINKSPEED;
 	strWIDList[u32WidsCount].type = WID_CHAR;
 	strWIDList[u32WidsCount].size = sizeof(char);
-	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->u8LinkSpeed;
+	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->link_speed;
 	u32WidsCount++;
 
 	strWIDList[u32WidsCount].id = WID_RSSI;
 	strWIDList[u32WidsCount].type = WID_CHAR;
 	strWIDList[u32WidsCount].size = sizeof(char);
-	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->s8RSSI;
+	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->rssi;
 	u32WidsCount++;
 
 	strWIDList[u32WidsCount].id = WID_SUCCESS_FRAME_COUNT;
 	strWIDList[u32WidsCount].type = WID_INT;
 	strWIDList[u32WidsCount].size = sizeof(u32);
-	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->u32TxCount;
+	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->tx_cnt;
 	u32WidsCount++;
 
 	strWIDList[u32WidsCount].id = WID_RECEIVED_FRAGMENT_COUNT;
 	strWIDList[u32WidsCount].type = WID_INT;
 	strWIDList[u32WidsCount].size = sizeof(u32);
-	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->u32RxCount;
+	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->rx_cnt;
 	u32WidsCount++;
 
 	strWIDList[u32WidsCount].id = WID_FAILED_COUNT;
 	strWIDList[u32WidsCount].type = WID_INT;
 	strWIDList[u32WidsCount].size = sizeof(u32);
-	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->u32TxFailureCount;
+	strWIDList[u32WidsCount].val = (s8 *)&pstrStatistics->tx_fail_cnt;
 	u32WidsCount++;
 
-	result = send_config_pkt(GET_CFG, strWIDList, u32WidsCount,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, strWIDList,
+				u32WidsCount,
+				wilc_get_vif_idx(vif));
 
 	if (result)
 		PRINT_ER("Failed to send scan paramters config packet\n");
@@ -2168,12 +2195,13 @@ s32 Handle_GetStatistics(struct host_if_drv *hif_drv, struct rf_info *pstrStatis
 	return 0;
 }
 
-static s32 Handle_Get_InActiveTime(struct host_if_drv *hif_drv,
+static s32 Handle_Get_InActiveTime(struct wilc_vif *vif,
 				   struct sta_inactive_t *strHostIfStaInactiveT)
 {
 	s32 result = 0;
 	u8 *stamac;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	wid.id = (u16)WID_SET_STA_MAC_INACTIVE_TIME;
 	wid.type = WID_STR;
@@ -2185,8 +2213,8 @@ static s32 Handle_Get_InActiveTime(struct host_if_drv *hif_drv,
 
 	PRINT_D(CFG80211_DBG, "SETING STA inactive time\n");
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to SET incative time\n");
@@ -2198,8 +2226,8 @@ static s32 Handle_Get_InActiveTime(struct host_if_drv *hif_drv,
 	wid.val = (s8 *)&inactive_time;
 	wid.size = sizeof(u32);
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 
 	if (result) {
 		PRINT_ER("Failed to get incative time\n");
@@ -2208,12 +2236,12 @@ static s32 Handle_Get_InActiveTime(struct host_if_drv *hif_drv,
 
 	PRINT_D(CFG80211_DBG, "Getting inactive time : %d\n", inactive_time);
 
-	up(&hif_drv->hSemInactiveTime);
+	up(&hif_drv->sem_inactive_time);
 
 	return result;
 }
 
-static void Handle_AddBeacon(struct host_if_drv *hif_drv,
+static void Handle_AddBeacon(struct wilc_vif *vif,
 			     struct beacon_attr *pstrSetBeaconParam)
 {
 	s32 result = 0;
@@ -2253,12 +2281,12 @@ static void Handle_AddBeacon(struct host_if_drv *hif_drv,
 	*pu8CurrByte++ = ((pstrSetBeaconParam->tail_len >> 16) & 0xFF);
 	*pu8CurrByte++ = ((pstrSetBeaconParam->tail_len >> 24) & 0xFF);
 
-	if (pstrSetBeaconParam->tail > 0)
+	if (pstrSetBeaconParam->tail)
 		memcpy(pu8CurrByte, pstrSetBeaconParam->tail, pstrSetBeaconParam->tail_len);
 	pu8CurrByte += pstrSetBeaconParam->tail_len;
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send add beacon config packet\n");
 
@@ -2268,7 +2296,7 @@ ERRORHANDLER:
 	kfree(pstrSetBeaconParam->tail);
 }
 
-static void Handle_DelBeacon(struct host_if_drv *hif_drv)
+static void Handle_DelBeacon(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct wid wid;
@@ -2286,8 +2314,8 @@ static void Handle_DelBeacon(struct host_if_drv *hif_drv)
 
 	PRINT_D(HOSTINF_DBG, "Deleting BEACON\n");
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send delete beacon config packet\n");
 }
@@ -2300,45 +2328,47 @@ static u32 WILC_HostIf_PackStaParam(u8 *pu8Buffer,
 	pu8CurrByte = pu8Buffer;
 
 	PRINT_D(HOSTINF_DBG, "Packing STA params\n");
-	memcpy(pu8CurrByte, pstrStationParam->au8BSSID, ETH_ALEN);
+	memcpy(pu8CurrByte, pstrStationParam->bssid, ETH_ALEN);
 	pu8CurrByte +=  ETH_ALEN;
 
-	*pu8CurrByte++ = pstrStationParam->u16AssocID & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u16AssocID >> 8) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->aid & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->aid >> 8) & 0xFF;
 
-	*pu8CurrByte++ = pstrStationParam->u8NumRates;
-	if (pstrStationParam->u8NumRates > 0)
-		memcpy(pu8CurrByte, pstrStationParam->pu8Rates, pstrStationParam->u8NumRates);
-	pu8CurrByte += pstrStationParam->u8NumRates;
+	*pu8CurrByte++ = pstrStationParam->rates_len;
+	if (pstrStationParam->rates_len > 0)
+		memcpy(pu8CurrByte, pstrStationParam->rates,
+		       pstrStationParam->rates_len);
+	pu8CurrByte += pstrStationParam->rates_len;
 
-	*pu8CurrByte++ = pstrStationParam->bIsHTSupported;
-	*pu8CurrByte++ = pstrStationParam->u16HTCapInfo & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u16HTCapInfo >> 8) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->ht_supported;
+	*pu8CurrByte++ = pstrStationParam->ht_capa_info & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->ht_capa_info >> 8) & 0xFF;
 
-	*pu8CurrByte++ = pstrStationParam->u8AmpduParams;
-	memcpy(pu8CurrByte, pstrStationParam->au8SuppMCsSet, WILC_SUPP_MCS_SET_SIZE);
+	*pu8CurrByte++ = pstrStationParam->ht_ampdu_params;
+	memcpy(pu8CurrByte, pstrStationParam->ht_supp_mcs_set,
+	       WILC_SUPP_MCS_SET_SIZE);
 	pu8CurrByte += WILC_SUPP_MCS_SET_SIZE;
 
-	*pu8CurrByte++ = pstrStationParam->u16HTExtParams & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u16HTExtParams >> 8) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->ht_ext_params & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->ht_ext_params >> 8) & 0xFF;
 
-	*pu8CurrByte++ = pstrStationParam->u32TxBeamformingCap & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 8) & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 16) & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u32TxBeamformingCap >> 24) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->ht_tx_bf_cap & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 8) & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 16) & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->ht_tx_bf_cap >> 24) & 0xFF;
 
-	*pu8CurrByte++ = pstrStationParam->u8ASELCap;
+	*pu8CurrByte++ = pstrStationParam->ht_ante_sel;
 
-	*pu8CurrByte++ = pstrStationParam->u16FlagsMask & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u16FlagsMask >> 8) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->flags_mask & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->flags_mask >> 8) & 0xFF;
 
-	*pu8CurrByte++ = pstrStationParam->u16FlagsSet & 0xFF;
-	*pu8CurrByte++ = (pstrStationParam->u16FlagsSet >> 8) & 0xFF;
+	*pu8CurrByte++ = pstrStationParam->flags_set & 0xFF;
+	*pu8CurrByte++ = (pstrStationParam->flags_set >> 8) & 0xFF;
 
 	return pu8CurrByte - pu8Buffer;
 }
 
-static void Handle_AddStation(struct host_if_drv *hif_drv,
+static void Handle_AddStation(struct wilc_vif *vif,
 			      struct add_sta_param *pstrStationParam)
 {
 	s32 result = 0;
@@ -2348,7 +2378,7 @@ static void Handle_AddStation(struct host_if_drv *hif_drv,
 	PRINT_D(HOSTINF_DBG, "Handling add station\n");
 	wid.id = (u16)WID_ADD_STA;
 	wid.type = WID_BIN;
-	wid.size = WILC_ADD_STA_LENGTH + pstrStationParam->u8NumRates;
+	wid.size = WILC_ADD_STA_LENGTH + pstrStationParam->rates_len;
 
 	wid.val = kmalloc(wid.size, GFP_KERNEL);
 	if (!wid.val)
@@ -2357,17 +2387,17 @@ static void Handle_AddStation(struct host_if_drv *hif_drv,
 	pu8CurrByte = wid.val;
 	pu8CurrByte += WILC_HostIf_PackStaParam(pu8CurrByte, pstrStationParam);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result != 0)
 		PRINT_ER("Failed to send add station config packet\n");
 
 ERRORHANDLER:
-	kfree(pstrStationParam->pu8Rates);
+	kfree(pstrStationParam->rates);
 	kfree(wid.val);
 }
 
-static void Handle_DelAllSta(struct host_if_drv *hif_drv,
+static void Handle_DelAllSta(struct wilc_vif *vif,
 			     struct del_all_sta *pstrDelAllStaParam)
 {
 	s32 result = 0;
@@ -2399,8 +2429,8 @@ static void Handle_DelAllSta(struct host_if_drv *hif_drv,
 		pu8CurrByte += ETH_ALEN;
 	}
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send add station config packet\n");
 
@@ -2410,7 +2440,7 @@ ERRORHANDLER:
 	up(&hif_sema_wait_response);
 }
 
-static void Handle_DelStation(struct host_if_drv *hif_drv,
+static void Handle_DelStation(struct wilc_vif *vif,
 			      struct del_sta *pstrDelStaParam)
 {
 	s32 result = 0;
@@ -2431,8 +2461,8 @@ static void Handle_DelStation(struct host_if_drv *hif_drv,
 
 	memcpy(pu8CurrByte, pstrDelStaParam->mac_addr, ETH_ALEN);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send add station config packet\n");
 
@@ -2440,7 +2470,7 @@ ERRORHANDLER:
 	kfree(wid.val);
 }
 
-static void Handle_EditStation(struct host_if_drv *hif_drv,
+static void Handle_EditStation(struct wilc_vif *vif,
 			       struct add_sta_param *pstrStationParam)
 {
 	s32 result = 0;
@@ -2449,7 +2479,7 @@ static void Handle_EditStation(struct host_if_drv *hif_drv,
 
 	wid.id = (u16)WID_EDIT_STA;
 	wid.type = WID_BIN;
-	wid.size = WILC_ADD_STA_LENGTH + pstrStationParam->u8NumRates;
+	wid.size = WILC_ADD_STA_LENGTH + pstrStationParam->rates_len;
 
 	PRINT_D(HOSTINF_DBG, "Handling edit station\n");
 	wid.val = kmalloc(wid.size, GFP_KERNEL);
@@ -2459,52 +2489,54 @@ static void Handle_EditStation(struct host_if_drv *hif_drv,
 	pu8CurrByte = wid.val;
 	pu8CurrByte += WILC_HostIf_PackStaParam(pu8CurrByte, pstrStationParam);
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send edit station config packet\n");
 
 ERRORHANDLER:
-	kfree(pstrStationParam->pu8Rates);
+	kfree(pstrStationParam->rates);
 	kfree(wid.val);
 }
 
-static int Handle_RemainOnChan(struct host_if_drv *hif_drv,
+static int Handle_RemainOnChan(struct wilc_vif *vif,
 			       struct remain_ch *pstrHostIfRemainOnChan)
 {
 	s32 result = 0;
 	u8 u8remain_on_chan_flag;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv->remain_on_ch_pending) {
-		hif_drv->remain_on_ch.pVoid = pstrHostIfRemainOnChan->pVoid;
-		hif_drv->remain_on_ch.pRemainOnChanExpired = pstrHostIfRemainOnChan->pRemainOnChanExpired;
-		hif_drv->remain_on_ch.pRemainOnChanReady = pstrHostIfRemainOnChan->pRemainOnChanReady;
-		hif_drv->remain_on_ch.u16Channel = pstrHostIfRemainOnChan->u16Channel;
-		hif_drv->remain_on_ch.u32ListenSessionID = pstrHostIfRemainOnChan->u32ListenSessionID;
+		hif_drv->remain_on_ch.arg = pstrHostIfRemainOnChan->arg;
+		hif_drv->remain_on_ch.expired = pstrHostIfRemainOnChan->expired;
+		hif_drv->remain_on_ch.ready = pstrHostIfRemainOnChan->ready;
+		hif_drv->remain_on_ch.ch = pstrHostIfRemainOnChan->ch;
+		hif_drv->remain_on_ch.id = pstrHostIfRemainOnChan->id;
 	} else {
-		pstrHostIfRemainOnChan->u16Channel = hif_drv->remain_on_ch.u16Channel;
+		pstrHostIfRemainOnChan->ch = hif_drv->remain_on_ch.ch;
 	}
 
-	if (hif_drv->usr_scan_req.pfUserScanResult) {
+	if (hif_drv->usr_scan_req.scan_result) {
 		PRINT_INFO(GENERIC_DBG, "Required to remain on chan while scanning return\n");
 		hif_drv->remain_on_ch_pending = 1;
 		result = -EBUSY;
 		goto ERRORHANDLER;
 	}
-	if (hif_drv->enuHostIFstate == HOST_IF_WAITING_CONN_RESP) {
+	if (hif_drv->hif_state == HOST_IF_WAITING_CONN_RESP) {
 		PRINT_INFO(GENERIC_DBG, "Required to remain on chan while connecting return\n");
 		result = -EBUSY;
 		goto ERRORHANDLER;
 	}
 
-	if (g_obtainingIP || connecting) {
+	if (wilc_optaining_ip || wilc_connecting) {
 		PRINT_D(GENERIC_DBG, "[handle_scan]: Don't do obss scan until IP adresss is obtained\n");
 		result = -EBUSY;
 		goto ERRORHANDLER;
 	}
 
-	PRINT_D(HOSTINF_DBG, "Setting channel :%d\n", pstrHostIfRemainOnChan->u16Channel);
+	PRINT_D(HOSTINF_DBG, "Setting channel :%d\n",
+		pstrHostIfRemainOnChan->ch);
 
 	u8remain_on_chan_flag = true;
 	wid.id = (u16)WID_REMAIN_ON_CHAN;
@@ -2517,23 +2549,23 @@ static int Handle_RemainOnChan(struct host_if_drv *hif_drv,
 	}
 
 	wid.val[0] = u8remain_on_chan_flag;
-	wid.val[1] = (s8)pstrHostIfRemainOnChan->u16Channel;
+	wid.val[1] = (s8)pstrHostIfRemainOnChan->ch;
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result != 0)
 		PRINT_ER("Failed to set remain on channel\n");
 
 ERRORHANDLER:
 	{
 		P2P_LISTEN_STATE = 1;
-		hif_drv->hRemainOnChannel.data = (unsigned long)hif_drv;
-		mod_timer(&hif_drv->hRemainOnChannel,
+		hif_drv->remain_on_ch_timer.data = (unsigned long)vif;
+		mod_timer(&hif_drv->remain_on_ch_timer,
 			  jiffies +
 			  msecs_to_jiffies(pstrHostIfRemainOnChan->u32duration));
 
-		if (hif_drv->remain_on_ch.pRemainOnChanReady)
-			hif_drv->remain_on_ch.pRemainOnChanReady(hif_drv->remain_on_ch.pVoid);
+		if (hif_drv->remain_on_ch.ready)
+			hif_drv->remain_on_ch.ready(hif_drv->remain_on_ch.arg);
 
 		if (hif_drv->remain_on_ch_pending)
 			hif_drv->remain_on_ch_pending = 0;
@@ -2542,14 +2574,16 @@ ERRORHANDLER:
 	return result;
 }
 
-static int Handle_RegisterFrame(struct host_if_drv *hif_drv,
+static int Handle_RegisterFrame(struct wilc_vif *vif,
 				struct reg_frame *pstrHostIfRegisterFrame)
 {
 	s32 result = 0;
 	struct wid wid;
 	u8 *pu8CurrByte;
 
-	PRINT_D(HOSTINF_DBG, "Handling frame register Flag : %d FrameType: %d\n", pstrHostIfRegisterFrame->bReg, pstrHostIfRegisterFrame->u16FrameType);
+	PRINT_D(HOSTINF_DBG, "Handling frame register : %d FrameType: %d\n",
+		pstrHostIfRegisterFrame->reg,
+		pstrHostIfRegisterFrame->frame_type);
 
 	wid.id = (u16)WID_REGISTER_FRAME;
 	wid.type = WID_STR;
@@ -2559,15 +2593,14 @@ static int Handle_RegisterFrame(struct host_if_drv *hif_drv,
 
 	pu8CurrByte = wid.val;
 
-	*pu8CurrByte++ = pstrHostIfRegisterFrame->bReg;
-	*pu8CurrByte++ = pstrHostIfRegisterFrame->u8Regid;
-	memcpy(pu8CurrByte, &pstrHostIfRegisterFrame->u16FrameType,
-	       sizeof(u16));
+	*pu8CurrByte++ = pstrHostIfRegisterFrame->reg;
+	*pu8CurrByte++ = pstrHostIfRegisterFrame->reg_id;
+	memcpy(pu8CurrByte, &pstrHostIfRegisterFrame->frame_type, sizeof(u16));
 
 	wid.size = sizeof(u16) + 2;
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result) {
 		PRINT_ER("Failed to frame register config packet\n");
 		result = -EINVAL;
@@ -2576,12 +2609,13 @@ static int Handle_RegisterFrame(struct host_if_drv *hif_drv,
 	return result;
 }
 
-static u32 Handle_ListenStateExpired(struct host_if_drv *hif_drv,
+static u32 Handle_ListenStateExpired(struct wilc_vif *vif,
 				     struct remain_ch *pstrHostIfRemainOnChan)
 {
 	u8 u8remain_on_chan_flag;
 	struct wid wid;
 	s32 result = 0;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	PRINT_D(HOSTINF_DBG, "CANCEL REMAIN ON CHAN\n");
 
@@ -2592,22 +2626,24 @@ static u32 Handle_ListenStateExpired(struct host_if_drv *hif_drv,
 		wid.size = 2;
 		wid.val = kmalloc(wid.size, GFP_KERNEL);
 
-		if (!wid.val)
+		if (!wid.val) {
 			PRINT_ER("Failed to allocate memory\n");
+			return -ENOMEM;
+		}
 
 		wid.val[0] = u8remain_on_chan_flag;
 		wid.val[1] = FALSE_FRMWR_CHANNEL;
 
-		result = send_config_pkt(SET_CFG, &wid, 1,
-					 get_id_from_handler(hif_drv));
+		result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+					 wilc_get_vif_idx(vif));
 		if (result != 0) {
 			PRINT_ER("Failed to set remain on channel\n");
 			goto _done_;
 		}
 
-		if (hif_drv->remain_on_ch.pRemainOnChanExpired) {
-			hif_drv->remain_on_ch.pRemainOnChanExpired(hif_drv->remain_on_ch.pVoid,
-								   pstrHostIfRemainOnChan->u32ListenSessionID);
+		if (hif_drv->remain_on_ch.expired) {
+			hif_drv->remain_on_ch.expired(hif_drv->remain_on_ch.arg,
+						      pstrHostIfRemainOnChan->id);
 		}
 		P2P_LISTEN_STATE = 0;
 	} else {
@@ -2623,21 +2659,21 @@ static void ListenTimerCB(unsigned long arg)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
-	struct host_if_drv *hif_drv = (struct host_if_drv *)arg;
+	struct wilc_vif *vif = (struct wilc_vif *)arg;
 
-	del_timer(&hif_drv->hRemainOnChannel);
+	del_timer(&vif->hif_drv->remain_on_ch_timer);
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_LISTEN_TIMER_FIRED;
-	msg.drv = hif_drv;
-	msg.body.remain_on_ch.u32ListenSessionID = hif_drv->remain_on_ch.u32ListenSessionID;
+	msg.vif = vif;
+	msg.body.remain_on_ch.id = vif->hif_drv->remain_on_ch.id;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("wilc_mq_send fail\n");
 }
 
-static void Handle_PowerManagement(struct host_if_drv *hif_drv,
+static void Handle_PowerManagement(struct wilc_vif *vif,
 				   struct power_mgmt_param *strPowerMgmtParam)
 {
 	s32 result = 0;
@@ -2656,13 +2692,13 @@ static void Handle_PowerManagement(struct host_if_drv *hif_drv,
 
 	PRINT_D(HOSTINF_DBG, "Handling Power Management\n");
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send power management config packet\n");
 }
 
-static void Handle_SetMulticastFilter(struct host_if_drv *hif_drv,
+static void Handle_SetMulticastFilter(struct wilc_vif *vif,
 				      struct set_multicast *strHostIfSetMulti)
 {
 	s32 result = 0;
@@ -2680,9 +2716,9 @@ static void Handle_SetMulticastFilter(struct host_if_drv *hif_drv,
 
 	pu8CurrByte = wid.val;
 	*pu8CurrByte++ = (strHostIfSetMulti->enabled & 0xFF);
-	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 8) & 0xFF);
-	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 16) & 0xFF);
-	*pu8CurrByte++ = ((strHostIfSetMulti->enabled >> 24) & 0xFF);
+	*pu8CurrByte++ = 0;
+	*pu8CurrByte++ = 0;
+	*pu8CurrByte++ = 0;
 
 	*pu8CurrByte++ = (strHostIfSetMulti->cnt & 0xFF);
 	*pu8CurrByte++ = ((strHostIfSetMulti->cnt >> 8) & 0xFF);
@@ -2690,10 +2726,11 @@ static void Handle_SetMulticastFilter(struct host_if_drv *hif_drv,
 	*pu8CurrByte++ = ((strHostIfSetMulti->cnt >> 24) & 0xFF);
 
 	if ((strHostIfSetMulti->cnt) > 0)
-		memcpy(pu8CurrByte, gau8MulticastMacAddrList, ((strHostIfSetMulti->cnt) * ETH_ALEN));
+		memcpy(pu8CurrByte, wilc_multicast_mac_addr_list,
+		       ((strHostIfSetMulti->cnt) * ETH_ALEN));
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_ER("Failed to send setup multicast config packet\n");
 
@@ -2701,71 +2738,7 @@ ERRORHANDLER:
 	kfree(wid.val);
 }
 
-static s32 Handle_AddBASession(struct host_if_drv *hif_drv,
-			       struct ba_session_info *strHostIfBASessionInfo)
-{
-	s32 result = 0;
-	struct wid wid;
-	int AddbaTimeout = 100;
-	char *ptr = NULL;
-
-	PRINT_D(HOSTINF_DBG, "Opening Block Ack session with\nBSSID = %.2x:%.2x:%.2x\nTID=%d\nBufferSize == %d\nSessionTimeOut = %d\n",
-		strHostIfBASessionInfo->au8Bssid[0],
-		strHostIfBASessionInfo->au8Bssid[1],
-		strHostIfBASessionInfo->au8Bssid[2],
-		strHostIfBASessionInfo->u16BufferSize,
-		strHostIfBASessionInfo->u16SessionTimeout,
-		strHostIfBASessionInfo->u8Ted);
-
-	wid.id = (u16)WID_11E_P_ACTION_REQ;
-	wid.type = WID_STR;
-	wid.val = kmalloc(BLOCK_ACK_REQ_SIZE, GFP_KERNEL);
-	wid.size = BLOCK_ACK_REQ_SIZE;
-	ptr = wid.val;
-	*ptr++ = 0x14;
-	*ptr++ = 0x3;
-	*ptr++ = 0x0;
-	memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
-	ptr += ETH_ALEN;
-	*ptr++ = strHostIfBASessionInfo->u8Ted;
-	*ptr++ = 1;
-	*ptr++ = (strHostIfBASessionInfo->u16BufferSize & 0xFF);
-	*ptr++ = ((strHostIfBASessionInfo->u16BufferSize >> 16) & 0xFF);
-	*ptr++ = (strHostIfBASessionInfo->u16SessionTimeout & 0xFF);
-	*ptr++ = ((strHostIfBASessionInfo->u16SessionTimeout >> 16) & 0xFF);
-	*ptr++ = (AddbaTimeout & 0xFF);
-	*ptr++ = ((AddbaTimeout >> 16) & 0xFF);
-	*ptr++ = 8;
-	*ptr++ = 0;
-
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
-	if (result)
-		PRINT_D(HOSTINF_DBG, "Couldn't open BA Session\n");
-
-	wid.id = (u16)WID_11E_P_ACTION_REQ;
-	wid.type = WID_STR;
-	wid.size = 15;
-	ptr = wid.val;
-	*ptr++ = 15;
-	*ptr++ = 7;
-	*ptr++ = 0x2;
-	memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
-	ptr += ETH_ALEN;
-	*ptr++ = strHostIfBASessionInfo->u8Ted;
-	*ptr++ = 8;
-	*ptr++ = (strHostIfBASessionInfo->u16BufferSize & 0xFF);
-	*ptr++ = ((strHostIfBASessionInfo->u16SessionTimeout >> 16) & 0xFF);
-	*ptr++ = 3;
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
-
-	kfree(wid.val);
-
-	return result;
-}
-
-static s32 Handle_DelAllRxBASessions(struct host_if_drv *hif_drv,
+static s32 Handle_DelAllRxBASessions(struct wilc_vif *vif,
 				     struct ba_session_info *strHostIfBASessionInfo)
 {
 	s32 result = 0;
@@ -2773,10 +2746,10 @@ static s32 Handle_DelAllRxBASessions(struct host_if_drv *hif_drv,
 	char *ptr = NULL;
 
 	PRINT_D(GENERIC_DBG, "Delete Block Ack session with\nBSSID = %.2x:%.2x:%.2x\nTID=%d\n",
-		strHostIfBASessionInfo->au8Bssid[0],
-		strHostIfBASessionInfo->au8Bssid[1],
-		strHostIfBASessionInfo->au8Bssid[2],
-		strHostIfBASessionInfo->u8Ted);
+		strHostIfBASessionInfo->bssid[0],
+		strHostIfBASessionInfo->bssid[1],
+		strHostIfBASessionInfo->bssid[2],
+		strHostIfBASessionInfo->tid);
 
 	wid.id = (u16)WID_DEL_ALL_RX_BA;
 	wid.type = WID_STR;
@@ -2786,14 +2759,14 @@ static s32 Handle_DelAllRxBASessions(struct host_if_drv *hif_drv,
 	*ptr++ = 0x14;
 	*ptr++ = 0x3;
 	*ptr++ = 0x2;
-	memcpy(ptr, strHostIfBASessionInfo->au8Bssid, ETH_ALEN);
+	memcpy(ptr, strHostIfBASessionInfo->bssid, ETH_ALEN);
 	ptr += ETH_ALEN;
-	*ptr++ = strHostIfBASessionInfo->u8Ted;
+	*ptr++ = strHostIfBASessionInfo->tid;
 	*ptr++ = 0;
 	*ptr++ = 32;
 
-	result = send_config_pkt(SET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, SET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result)
 		PRINT_D(HOSTINF_DBG, "Couldn't delete BA Session\n");
 
@@ -2808,19 +2781,20 @@ static int hostIFthread(void *pvArg)
 {
 	u32 u32Ret;
 	struct host_if_msg msg;
-	struct host_if_drv *hif_drv;
+	struct wilc *wilc = (struct wilc*)pvArg;
+	struct wilc_vif *vif;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	while (1) {
 		wilc_mq_recv(&hif_msg_q, &msg, sizeof(struct host_if_msg), &u32Ret);
-		hif_drv = (struct host_if_drv *)msg.drv;
+		vif = msg.vif;
 		if (msg.id == HOST_IF_MSG_EXIT) {
 			PRINT_D(GENERIC_DBG, "THREAD: Exiting HostIfThread\n");
 			break;
 		}
 
-		if ((!g_wilc_initialized)) {
+		if ((!wilc_initialized)) {
 			PRINT_D(GENERIC_DBG, "--WAIT--");
 			usleep_range(200 * 1000, 200 * 1000);
 			wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -2828,7 +2802,7 @@ static int hostIFthread(void *pvArg)
 		}
 
 		if (msg.id == HOST_IF_MSG_CONNECT &&
-		    hif_drv->usr_scan_req.pfUserScanResult) {
+		    vif->hif_drv->usr_scan_req.scan_result) {
 			PRINT_D(HOSTINF_DBG, "Requeue connect request till scan done received\n");
 			wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 			usleep_range(2 * 1000, 2 * 1000);
@@ -2841,167 +2815,169 @@ static int hostIFthread(void *pvArg)
 			break;
 
 		case HOST_IF_MSG_SCAN:
-			Handle_Scan(msg.drv, &msg.body.scan_info);
+			Handle_Scan(msg.vif, &msg.body.scan_info);
 			break;
 
 		case HOST_IF_MSG_CONNECT:
-			Handle_Connect(msg.drv, &msg.body.con_info);
+			Handle_Connect(msg.vif, &msg.body.con_info);
 			break;
 
 		case HOST_IF_MSG_FLUSH_CONNECT:
-			Handle_FlushConnect(msg.drv);
+			Handle_FlushConnect(msg.vif);
 			break;
 
 		case HOST_IF_MSG_RCVD_NTWRK_INFO:
-			Handle_RcvdNtwrkInfo(msg.drv, &msg.body.net_info);
+			Handle_RcvdNtwrkInfo(msg.vif, &msg.body.net_info);
 			break;
 
 		case HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO:
-			Handle_RcvdGnrlAsyncInfo(msg.drv, &msg.body.async_info);
+			Handle_RcvdGnrlAsyncInfo(vif,
+						 &msg.body.async_info);
 			break;
 
 		case HOST_IF_MSG_KEY:
-			Handle_Key(msg.drv, &msg.body.key_info);
+			Handle_Key(msg.vif, &msg.body.key_info);
 			break;
 
 		case HOST_IF_MSG_CFG_PARAMS:
-
-			Handle_CfgParam(msg.drv, &msg.body.cfg_info);
+			handle_cfg_param(msg.vif, &msg.body.cfg_info);
 			break;
 
 		case HOST_IF_MSG_SET_CHANNEL:
-			Handle_SetChannel(msg.drv, &msg.body.channel_info);
+			handle_set_channel(msg.vif, &msg.body.channel_info);
 			break;
 
 		case HOST_IF_MSG_DISCONNECT:
-			Handle_Disconnect(msg.drv);
+			Handle_Disconnect(msg.vif);
 			break;
 
 		case HOST_IF_MSG_RCVD_SCAN_COMPLETE:
-			del_timer(&hif_drv->hScanTimer);
+			del_timer(&vif->hif_drv->scan_timer);
 			PRINT_D(HOSTINF_DBG, "scan completed successfully\n");
 
-			if (!linux_wlan_get_num_conn_ifcs())
-				chip_sleep_manually(INFINITE_SLEEP_TIME);
+			if (!wilc_wlan_get_num_conn_ifcs(wilc))
+				wilc_chip_sleep_manually(wilc);
 
-			Handle_ScanDone(msg.drv, SCAN_EVENT_DONE);
+			Handle_ScanDone(msg.vif, SCAN_EVENT_DONE);
 
-			if (hif_drv->remain_on_ch_pending)
-				Handle_RemainOnChan(msg.drv, &msg.body.remain_on_ch);
+			if (vif->hif_drv->remain_on_ch_pending)
+				Handle_RemainOnChan(msg.vif,
+						    &msg.body.remain_on_ch);
 
 			break;
 
 		case HOST_IF_MSG_GET_RSSI:
-			Handle_GetRssi(msg.drv);
+			Handle_GetRssi(msg.vif);
 			break;
 
 		case HOST_IF_MSG_GET_LINKSPEED:
-			Handle_GetLinkspeed(msg.drv);
+			Handle_GetLinkspeed(msg.vif);
 			break;
 
 		case HOST_IF_MSG_GET_STATISTICS:
-			Handle_GetStatistics(msg.drv, (struct rf_info *)msg.body.data);
+			Handle_GetStatistics(msg.vif,
+					     (struct rf_info *)msg.body.data);
 			break;
 
 		case HOST_IF_MSG_GET_CHNL:
-			Handle_GetChnl(msg.drv);
+			Handle_GetChnl(msg.vif);
 			break;
 
 		case HOST_IF_MSG_ADD_BEACON:
-			Handle_AddBeacon(msg.drv, &msg.body.beacon_info);
+			Handle_AddBeacon(msg.vif, &msg.body.beacon_info);
 			break;
 
 		case HOST_IF_MSG_DEL_BEACON:
-			Handle_DelBeacon(msg.drv);
+			Handle_DelBeacon(msg.vif);
 			break;
 
 		case HOST_IF_MSG_ADD_STATION:
-			Handle_AddStation(msg.drv, &msg.body.add_sta_info);
+			Handle_AddStation(msg.vif, &msg.body.add_sta_info);
 			break;
 
 		case HOST_IF_MSG_DEL_STATION:
-			Handle_DelStation(msg.drv, &msg.body.del_sta_info);
+			Handle_DelStation(msg.vif, &msg.body.del_sta_info);
 			break;
 
 		case HOST_IF_MSG_EDIT_STATION:
-			Handle_EditStation(msg.drv, &msg.body.edit_sta_info);
+			Handle_EditStation(msg.vif, &msg.body.edit_sta_info);
 			break;
 
 		case HOST_IF_MSG_GET_INACTIVETIME:
-			Handle_Get_InActiveTime(msg.drv, &msg.body.mac_info);
+			Handle_Get_InActiveTime(msg.vif, &msg.body.mac_info);
 			break;
 
 		case HOST_IF_MSG_SCAN_TIMER_FIRED:
 			PRINT_D(HOSTINF_DBG, "Scan Timeout\n");
 
-			Handle_ScanDone(msg.drv, SCAN_EVENT_ABORTED);
+			Handle_ScanDone(msg.vif, SCAN_EVENT_ABORTED);
 			break;
 
 		case HOST_IF_MSG_CONNECT_TIMER_FIRED:
 			PRINT_D(HOSTINF_DBG, "Connect Timeout\n");
-			Handle_ConnectTimeout(msg.drv);
+			Handle_ConnectTimeout(msg.vif);
 			break;
 
 		case HOST_IF_MSG_POWER_MGMT:
-			Handle_PowerManagement(msg.drv, &msg.body.pwr_mgmt_info);
+			Handle_PowerManagement(msg.vif,
+					       &msg.body.pwr_mgmt_info);
 			break;
 
 		case HOST_IF_MSG_SET_WFIDRV_HANDLER:
-			Handle_SetWfiDrvHandler(msg.drv,
-						&msg.body.drv);
+			handle_set_wfi_drv_handler(msg.vif, &msg.body.drv);
 			break;
 
 		case HOST_IF_MSG_SET_OPERATION_MODE:
-			Handle_SetOperationMode(msg.drv, &msg.body.mode);
+			handle_set_operation_mode(msg.vif, &msg.body.mode);
 			break;
 
 		case HOST_IF_MSG_SET_IPADDRESS:
 			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_IPADDRESS\n");
-			Handle_set_IPAddress(msg.drv, msg.body.ip_info.ip_addr, msg.body.ip_info.idx);
+			handle_set_ip_address(vif,
+					      msg.body.ip_info.ip_addr,
+					      msg.body.ip_info.idx);
 			break;
 
 		case HOST_IF_MSG_GET_IPADDRESS:
 			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_IPADDRESS\n");
-			Handle_get_IPAddress(msg.drv, msg.body.ip_info.ip_addr, msg.body.ip_info.idx);
+			handle_get_ip_address(vif, msg.body.ip_info.idx);
 			break;
 
 		case HOST_IF_MSG_SET_MAC_ADDRESS:
-			Handle_SetMacAddress(msg.drv, &msg.body.set_mac_info);
+			handle_set_mac_address(msg.vif,
+					       &msg.body.set_mac_info);
 			break;
 
 		case HOST_IF_MSG_GET_MAC_ADDRESS:
-			Handle_GetMacAddress(msg.drv, &msg.body.get_mac_info);
+			handle_get_mac_address(msg.vif,
+					       &msg.body.get_mac_info);
 			break;
 
 		case HOST_IF_MSG_REMAIN_ON_CHAN:
 			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_REMAIN_ON_CHAN\n");
-			Handle_RemainOnChan(msg.drv, &msg.body.remain_on_ch);
+			Handle_RemainOnChan(msg.vif, &msg.body.remain_on_ch);
 			break;
 
 		case HOST_IF_MSG_REGISTER_FRAME:
 			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_REGISTER_FRAME\n");
-			Handle_RegisterFrame(msg.drv, &msg.body.reg_frame);
+			Handle_RegisterFrame(msg.vif, &msg.body.reg_frame);
 			break;
 
 		case HOST_IF_MSG_LISTEN_TIMER_FIRED:
-			Handle_ListenStateExpired(msg.drv, &msg.body.remain_on_ch);
+			Handle_ListenStateExpired(msg.vif, &msg.body.remain_on_ch);
 			break;
 
 		case HOST_IF_MSG_SET_MULTICAST_FILTER:
 			PRINT_D(HOSTINF_DBG, "HOST_IF_MSG_SET_MULTICAST_FILTER\n");
-			Handle_SetMulticastFilter(msg.drv, &msg.body.multicast_info);
-			break;
-
-		case HOST_IF_MSG_ADD_BA_SESSION:
-			Handle_AddBASession(msg.drv, &msg.body.session_info);
+			Handle_SetMulticastFilter(msg.vif, &msg.body.multicast_info);
 			break;
 
 		case HOST_IF_MSG_DEL_ALL_RX_BA_SESSIONS:
-			Handle_DelAllRxBASessions(msg.drv, &msg.body.session_info);
+			Handle_DelAllRxBASessions(msg.vif, &msg.body.session_info);
 			break;
 
 		case HOST_IF_MSG_DEL_ALL_STA:
-			Handle_DelAllSta(msg.drv, &msg.body.del_all_sta_info);
+			Handle_DelAllSta(msg.vif, &msg.body.del_all_sta_info);
 			break;
 
 		default:
@@ -3017,11 +2993,11 @@ static int hostIFthread(void *pvArg)
 
 static void TimerCB_Scan(unsigned long arg)
 {
-	void *pvArg = (void *)arg;
+	struct wilc_vif *vif = (struct wilc_vif *)arg;
 	struct host_if_msg msg;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
-	msg.drv = pvArg;
+	msg.vif = vif;
 	msg.id = HOST_IF_MSG_SCAN_TIMER_FIRED;
 
 	wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -3029,17 +3005,17 @@ static void TimerCB_Scan(unsigned long arg)
 
 static void TimerCB_Connect(unsigned long arg)
 {
-	void *pvArg = (void *)arg;
+	struct wilc_vif *vif = (struct wilc_vif *)arg;
 	struct host_if_msg msg;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
-	msg.drv = pvArg;
+	msg.vif = vif;
 	msg.id = HOST_IF_MSG_CONNECT_TIMER_FIRED;
 
 	wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 }
 
-s32 host_int_remove_key(struct host_if_drv *hif_drv, const u8 *pu8StaAddress)
+s32 wilc_remove_key(struct host_if_drv *hif_drv, const u8 *pu8StaAddress)
 {
 	struct wid wid;
 
@@ -3051,10 +3027,11 @@ s32 host_int_remove_key(struct host_if_drv *hif_drv, const u8 *pu8StaAddress)
 	return 0;
 }
 
-int host_int_remove_wep_key(struct host_if_drv *hif_drv, u8 index)
+int wilc_remove_wep_key(struct wilc_vif *vif, u8 index)
 {
 	int result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		result = -EFAULT;
@@ -3067,21 +3044,22 @@ int host_int_remove_wep_key(struct host_if_drv *hif_drv, u8 index)
 	msg.id = HOST_IF_MSG_KEY;
 	msg.body.key_info.type = WEP;
 	msg.body.key_info.action = REMOVEKEY;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.key_info.attr.wep.index = index;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Error in sending message queue : Request to remove WEP key\n");
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-int host_int_set_wep_default_key(struct host_if_drv *hif_drv, u8 index)
+int wilc_set_wep_default_keyid(struct wilc_vif *vif, u8 index)
 {
 	int result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		result = -EFAULT;
@@ -3094,24 +3072,23 @@ int host_int_set_wep_default_key(struct host_if_drv *hif_drv, u8 index)
 	msg.id = HOST_IF_MSG_KEY;
 	msg.body.key_info.type = WEP;
 	msg.body.key_info.action = DEFAULTKEY;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.key_info.attr.wep.index = index;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Error in sending message queue : Default key index\n");
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-int host_int_add_wep_key_bss_sta(struct host_if_drv *hif_drv,
-				 const u8 *key,
-				 u8 len,
-				 u8 index)
+int wilc_add_wep_key_bss_sta(struct wilc_vif *vif, const u8 *key, u8 len,
+			     u8 index)
 {
 	int result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -3123,7 +3100,7 @@ int host_int_add_wep_key_bss_sta(struct host_if_drv *hif_drv,
 	msg.id = HOST_IF_MSG_KEY;
 	msg.body.key_info.type = WEP;
 	msg.body.key_info.action = ADDKEY;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.key_info.attr.wep.key = kmemdup(key, len, GFP_KERNEL);
 	if (!msg.body.key_info.attr.wep.key)
 		return -ENOMEM;
@@ -3134,20 +3111,17 @@ int host_int_add_wep_key_bss_sta(struct host_if_drv *hif_drv,
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Error in sending message queue :WEP Key\n");
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-int host_int_add_wep_key_bss_ap(struct host_if_drv *hif_drv,
-				const u8 *key,
-				u8 len,
-				u8 index,
-				u8 mode,
-				enum AUTHTYPE auth_type)
+int wilc_add_wep_key_bss_ap(struct wilc_vif *vif, const u8 *key, u8 len,
+			    u8 index, u8 mode, enum AUTHTYPE auth_type)
 {
 	int result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 	int i;
 
 	if (!hif_drv) {
@@ -3164,7 +3138,7 @@ int host_int_add_wep_key_bss_ap(struct host_if_drv *hif_drv,
 	msg.id = HOST_IF_MSG_KEY;
 	msg.body.key_info.type = WEP;
 	msg.body.key_info.action = ADDKEY_AP;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.key_info.attr.wep.key = kmemdup(key, len, GFP_KERNEL);
 	if (!msg.body.key_info.attr.wep.key)
 		return -ENOMEM;
@@ -3178,85 +3152,86 @@ int host_int_add_wep_key_bss_ap(struct host_if_drv *hif_drv,
 
 	if (result)
 		PRINT_ER("Error in sending message queue :WEP Key\n");
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-s32 host_int_add_ptk(struct host_if_drv *hif_drv, const u8 *pu8Ptk,
-		     u8 u8PtkKeylen, const u8 *mac_addr,
-		     const u8 *pu8RxMic, const u8 *pu8TxMic,
-		     u8 mode, u8 u8Ciphermode, u8 u8Idx)
+int wilc_add_ptk(struct wilc_vif *vif, const u8 *ptk, u8 ptk_key_len,
+		 const u8 *mac_addr, const u8 *rx_mic, const u8 *tx_mic,
+		 u8 mode, u8 cipher_mode, u8 index)
 {
-	s32 result = 0;
+	int result = 0;
 	struct host_if_msg msg;
-	u8 u8KeyLen = u8PtkKeylen;
-	u32 i;
+	struct host_if_drv *hif_drv = vif->hif_drv;
+	u8 key_len = ptk_key_len;
+	int i;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
 		return -EFAULT;
 	}
 
-	if (pu8RxMic)
-		u8KeyLen += RX_MIC_KEY_LEN;
+	if (rx_mic)
+		key_len += RX_MIC_KEY_LEN;
 
-	if (pu8TxMic)
-		u8KeyLen += TX_MIC_KEY_LEN;
+	if (tx_mic)
+		key_len += TX_MIC_KEY_LEN;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_KEY;
-	msg.body.key_info.type = WPAPtk;
+	msg.body.key_info.type = WPA_PTK;
 	if (mode == AP_MODE) {
 		msg.body.key_info.action = ADDKEY_AP;
-		msg.body.key_info.attr.wpa.index = u8Idx;
+		msg.body.key_info.attr.wpa.index = index;
 	}
 	if (mode == STATION_MODE)
 		msg.body.key_info.action = ADDKEY;
 
-	msg.body.key_info.attr.wpa.key = kmalloc(u8PtkKeylen, GFP_KERNEL);
-	memcpy(msg.body.key_info.attr.wpa.key, pu8Ptk, u8PtkKeylen);
+	msg.body.key_info.attr.wpa.key = kmemdup(ptk, ptk_key_len, GFP_KERNEL);
+	if (!msg.body.key_info.attr.wpa.key)
+		return -ENOMEM;
 
-	if (pu8RxMic) {
-		memcpy(msg.body.key_info.attr.wpa.key + 16, pu8RxMic, RX_MIC_KEY_LEN);
+	if (rx_mic) {
+		memcpy(msg.body.key_info.attr.wpa.key + 16, rx_mic, RX_MIC_KEY_LEN);
 		if (INFO) {
 			for (i = 0; i < RX_MIC_KEY_LEN; i++)
-				PRINT_INFO(CFG80211_DBG, "PairwiseRx[%d] = %x\n", i, pu8RxMic[i]);
+				PRINT_INFO(CFG80211_DBG, "PairwiseRx[%d] = %x\n", i, rx_mic[i]);
 		}
 	}
-	if (pu8TxMic) {
-		memcpy(msg.body.key_info.attr.wpa.key + 24, pu8TxMic, TX_MIC_KEY_LEN);
+	if (tx_mic) {
+		memcpy(msg.body.key_info.attr.wpa.key + 24, tx_mic, TX_MIC_KEY_LEN);
 		if (INFO) {
 			for (i = 0; i < TX_MIC_KEY_LEN; i++)
-				PRINT_INFO(CFG80211_DBG, "PairwiseTx[%d] = %x\n", i, pu8TxMic[i]);
+				PRINT_INFO(CFG80211_DBG, "PairwiseTx[%d] = %x\n", i, tx_mic[i]);
 		}
 	}
 
-	msg.body.key_info.attr.wpa.key_len = u8KeyLen;
+	msg.body.key_info.attr.wpa.key_len = key_len;
 	msg.body.key_info.attr.wpa.mac_addr = mac_addr;
-	msg.body.key_info.attr.wpa.mode = u8Ciphermode;
-	msg.drv = hif_drv;
+	msg.body.key_info.attr.wpa.mode = cipher_mode;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 
 	if (result)
 		PRINT_ER("Error in sending message queue:  PTK Key\n");
 
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-s32 host_int_add_rx_gtk(struct host_if_drv *hif_drv, const u8 *pu8RxGtk,
-			u8 u8GtkKeylen,	u8 u8KeyIdx,
-			u32 u32KeyRSClen, const u8 *KeyRSC,
-			const u8 *pu8RxMic, const u8 *pu8TxMic,
-			u8 mode, u8 u8Ciphermode)
+int wilc_add_rx_gtk(struct wilc_vif *vif, const u8 *rx_gtk, u8 gtk_key_len,
+		    u8 index, u32 key_rsc_len, const u8 *key_rsc,
+		    const u8 *rx_mic, const u8 *tx_mic, u8 mode,
+		    u8 cipher_mode)
 {
-	s32 result = 0;
+	int result = 0;
 	struct host_if_msg msg;
-	u8 u8KeyLen = u8GtkKeylen;
+	struct host_if_drv *hif_drv = vif->hif_drv;
+	u8 key_len = gtk_key_len;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -3264,56 +3239,64 @@ s32 host_int_add_rx_gtk(struct host_if_drv *hif_drv, const u8 *pu8RxGtk,
 	}
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
-	if (pu8RxMic)
-		u8KeyLen += RX_MIC_KEY_LEN;
+	if (rx_mic)
+		key_len += RX_MIC_KEY_LEN;
 
-	if (pu8TxMic)
-		u8KeyLen += TX_MIC_KEY_LEN;
+	if (tx_mic)
+		key_len += TX_MIC_KEY_LEN;
 
-	if (KeyRSC) {
-		msg.body.key_info.attr.wpa.seq = kmalloc(u32KeyRSClen, GFP_KERNEL);
-		memcpy(msg.body.key_info.attr.wpa.seq, KeyRSC, u32KeyRSClen);
+	if (key_rsc) {
+		msg.body.key_info.attr.wpa.seq = kmemdup(key_rsc,
+							 key_rsc_len,
+							 GFP_KERNEL);
+		if (!msg.body.key_info.attr.wpa.seq)
+			return -ENOMEM;
 	}
 
 	msg.id = HOST_IF_MSG_KEY;
-	msg.body.key_info.type = WPARxGtk;
-	msg.drv = hif_drv;
+	msg.body.key_info.type = WPA_RX_GTK;
+	msg.vif = vif;
 
 	if (mode == AP_MODE) {
 		msg.body.key_info.action = ADDKEY_AP;
-		msg.body.key_info.attr.wpa.mode = u8Ciphermode;
+		msg.body.key_info.attr.wpa.mode = cipher_mode;
 	}
 	if (mode == STATION_MODE)
 		msg.body.key_info.action = ADDKEY;
 
-	msg.body.key_info.attr.wpa.key = kmalloc(u8KeyLen, GFP_KERNEL);
-	memcpy(msg.body.key_info.attr.wpa.key, pu8RxGtk, u8GtkKeylen);
+	msg.body.key_info.attr.wpa.key = kmemdup(rx_gtk,
+						 key_len,
+						 GFP_KERNEL);
+	if (!msg.body.key_info.attr.wpa.key)
+		return -ENOMEM;
 
-	if (pu8RxMic)
-		memcpy(msg.body.key_info.attr.wpa.key + 16, pu8RxMic,
+	if (rx_mic)
+		memcpy(msg.body.key_info.attr.wpa.key + 16, rx_mic,
 		       RX_MIC_KEY_LEN);
 
-	if (pu8TxMic)
-		memcpy(msg.body.key_info.attr.wpa.key + 24, pu8TxMic,
+	if (tx_mic)
+		memcpy(msg.body.key_info.attr.wpa.key + 24, tx_mic,
 		       TX_MIC_KEY_LEN);
 
-	msg.body.key_info.attr.wpa.index = u8KeyIdx;
-	msg.body.key_info.attr.wpa.key_len = u8KeyLen;
-	msg.body.key_info.attr.wpa.seq_len = u32KeyRSClen;
+	msg.body.key_info.attr.wpa.index = index;
+	msg.body.key_info.attr.wpa.key_len = key_len;
+	msg.body.key_info.attr.wpa.seq_len = key_rsc_len;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Error in sending message queue:  RX GTK\n");
 
-	down(&hif_drv->hSemTestKeyBlock);
+	down(&hif_drv->sem_test_key_block);
 
 	return result;
 }
 
-s32 host_int_set_pmkid_info(struct host_if_drv *hif_drv, struct host_if_pmkid_attr *pu8PmkidInfoArray)
+s32 wilc_set_pmkid_info(struct wilc_vif *vif,
+			struct host_if_pmkid_attr *pu8PmkidInfoArray)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 	u32 i;
 
 	if (!hif_drv) {
@@ -3326,7 +3309,7 @@ s32 host_int_set_pmkid_info(struct host_if_drv *hif_drv, struct host_if_pmkid_at
 	msg.id = HOST_IF_MSG_KEY;
 	msg.body.key_info.type = PMKSA;
 	msg.body.key_info.action = ADDKEY;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	for (i = 0; i < pu8PmkidInfoArray->numpmkid; i++) {
 		memcpy(msg.body.key_info.attr.pmkid.pmkidlist[i].bssid,
@@ -3342,37 +3325,7 @@ s32 host_int_set_pmkid_info(struct host_if_drv *hif_drv, struct host_if_pmkid_at
 	return result;
 }
 
-s32 host_int_get_pmkid_info(struct host_if_drv *hif_drv,
-			    u8 *pu8PmkidInfoArray,
-			    u32 u32PmkidInfoLen)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_PMKID_INFO;
-	wid.type = WID_STR;
-	wid.size = u32PmkidInfoLen;
-	wid.val = pu8PmkidInfoArray;
-
-	return 0;
-}
-
-s32 host_int_set_RSNAConfigPSKPassPhrase(struct host_if_drv *hif_drv,
-					 u8 *pu8PassPhrase,
-					 u8 u8Psklength)
-{
-	struct wid wid;
-
-	if ((u8Psklength > 7) && (u8Psklength < 65)) {
-		wid.id = (u16)WID_11I_PSK;
-		wid.type = WID_STR;
-		wid.val = pu8PassPhrase;
-		wid.size = u8Psklength;
-	}
-
-	return 0;
-}
-
-s32 host_int_get_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
+s32 wilc_get_mac_address(struct wilc_vif *vif, u8 *pu8MacAddress)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3381,7 +3334,7 @@ s32 host_int_get_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
 
 	msg.id = HOST_IF_MSG_GET_MAC_ADDRESS;
 	msg.body.get_mac_info.mac_addr = pu8MacAddress;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3393,7 +3346,7 @@ s32 host_int_get_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
 	return result;
 }
 
-s32 host_int_set_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
+s32 wilc_set_mac_address(struct wilc_vif *vif, u8 *pu8MacAddress)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3403,7 +3356,7 @@ s32 host_int_set_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_SET_MAC_ADDRESS;
 	memcpy(msg.body.set_mac_info.mac_addr, pu8MacAddress, ETH_ALEN);
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -3412,52 +3365,15 @@ s32 host_int_set_MacAddress(struct host_if_drv *hif_drv, u8 *pu8MacAddress)
 	return result;
 }
 
-s32 host_int_get_RSNAConfigPSKPassPhrase(struct host_if_drv *hif_drv,
-					 u8 *pu8PassPhrase, u8 u8Psklength)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_11I_PSK;
-	wid.type = WID_STR;
-	wid.size = u8Psklength;
-	wid.val = pu8PassPhrase;
-
-	return 0;
-}
-
-s32 host_int_set_start_scan_req(struct host_if_drv *hif_drv, u8 scanSource)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_START_SCAN_REQ;
-	wid.type = WID_CHAR;
-	wid.val = (s8 *)&scanSource;
-	wid.size = sizeof(char);
-
-	return 0;
-}
-
-s32 host_int_get_start_scan_req(struct host_if_drv *hif_drv, u8 *pu8ScanSource)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_START_SCAN_REQ;
-	wid.type = WID_CHAR;
-	wid.val = (s8 *)pu8ScanSource;
-	wid.size = sizeof(char);
-
-	return 0;
-}
-
-s32 host_int_set_join_req(struct host_if_drv *hif_drv, u8 *pu8bssid,
-			  const u8 *pu8ssid, size_t ssidLen,
-			  const u8 *pu8IEs, size_t IEsLen,
-			  wilc_connect_result pfConnectResult, void *pvUserArg,
-			  u8 u8security, enum AUTHTYPE tenuAuth_type,
-			  u8 u8channel, void *pJoinParams)
+s32 wilc_set_join_req(struct wilc_vif *vif, u8 *pu8bssid, const u8 *pu8ssid,
+		      size_t ssidLen, const u8 *pu8IEs, size_t IEsLen,
+		      wilc_connect_result pfConnectResult, void *pvUserArg,
+		      u8 u8security, enum AUTHTYPE tenuAuth_type,
+		      u8 u8channel, void *pJoinParams)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv || !pfConnectResult) {
 		PRINT_ER("Driver is null\n");
@@ -3479,7 +3395,7 @@ s32 host_int_set_join_req(struct host_if_drv *hif_drv, u8 *pu8bssid,
 	msg.body.con_info.result = pfConnectResult;
 	msg.body.con_info.arg = pvUserArg;
 	msg.body.con_info.params = pJoinParams;
-	msg.drv = hif_drv ;
+	msg.vif = vif;
 
 	if (pu8bssid) {
 		msg.body.con_info.bssid = kmalloc(6, GFP_KERNEL);
@@ -3497,10 +3413,11 @@ s32 host_int_set_join_req(struct host_if_drv *hif_drv, u8 *pu8bssid,
 		msg.body.con_info.ies = kmalloc(IEsLen, GFP_KERNEL);
 		memcpy(msg.body.con_info.ies, pu8IEs, IEsLen);
 	}
-	if (hif_drv->enuHostIFstate < HOST_IF_CONNECTING)
-		hif_drv->enuHostIFstate = HOST_IF_CONNECTING;
+	if (hif_drv->hif_state < HOST_IF_CONNECTING)
+		hif_drv->hif_state = HOST_IF_CONNECTING;
 	else
-		PRINT_D(GENERIC_DBG, "Don't set state to 'connecting' as state is %d\n", hif_drv->enuHostIFstate);
+		PRINT_D(GENERIC_DBG, "Don't set state to 'connecting' : %d\n",
+			hif_drv->hif_state);
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3508,17 +3425,18 @@ s32 host_int_set_join_req(struct host_if_drv *hif_drv, u8 *pu8bssid,
 		return -EFAULT;
 	}
 
-	hif_drv->hConnectTimer.data = (unsigned long)hif_drv;
-	mod_timer(&hif_drv->hConnectTimer,
+	hif_drv->connect_timer.data = (unsigned long)vif;
+	mod_timer(&hif_drv->connect_timer,
 		  jiffies + msecs_to_jiffies(HOST_IF_CONNECT_TIMEOUT));
 
 	return result;
 }
 
-s32 host_int_flush_join_req(struct host_if_drv *hif_drv)
+s32 wilc_flush_join_req(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!join_req)
 		return -EFAULT;
@@ -3529,7 +3447,7 @@ s32 host_int_flush_join_req(struct host_if_drv *hif_drv)
 	}
 
 	msg.id = HOST_IF_MSG_FLUSH_CONNECT;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3540,10 +3458,11 @@ s32 host_int_flush_join_req(struct host_if_drv *hif_drv)
 	return result;
 }
 
-s32 host_int_disconnect(struct host_if_drv *hif_drv, u16 u16ReasonCode)
+s32 wilc_disconnect(struct wilc_vif *vif, u16 u16ReasonCode)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("Driver is null\n");
@@ -3553,50 +3472,25 @@ s32 host_int_disconnect(struct host_if_drv *hif_drv, u16 u16ReasonCode)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_DISCONNECT;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Failed to send message queue: disconnect\n");
 
-	down(&hif_drv->hSemTestDisconnectBlock);
+	down(&hif_drv->sem_test_disconn_block);
 
 	return result;
 }
 
-s32 host_int_disconnect_station(struct host_if_drv *hif_drv, u8 assoc_id)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_DISCONNECT;
-	wid.type = WID_CHAR;
-	wid.val = (s8 *)&assoc_id;
-	wid.size = sizeof(char);
-
-	return 0;
-}
-
-s32 host_int_get_assoc_req_info(struct host_if_drv *hif_drv,
-				u8 *pu8AssocReqInfo,
-				u32 u32AssocReqInfoLen)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_ASSOC_REQ_INFO;
-	wid.type = WID_STR;
-	wid.val = pu8AssocReqInfo;
-	wid.size = u32AssocReqInfoLen;
-
-	return 0;
-}
-
-s32 host_int_get_assoc_res_info(struct host_if_drv *hif_drv,
-				u8 *pu8AssocRespInfo,
-				u32 u32MaxAssocRespInfoLen,
-				u32 *pu32RcvdAssocRespInfoLen)
+static s32 host_int_get_assoc_res_info(struct wilc_vif *vif,
+				       u8 *pu8AssocRespInfo,
+				       u32 u32MaxAssocRespInfoLen,
+				       u32 *pu32RcvdAssocRespInfoLen)
 {
 	s32 result = 0;
 	struct wid wid;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("Driver is null\n");
@@ -3608,8 +3502,8 @@ s32 host_int_get_assoc_res_info(struct host_if_drv *hif_drv,
 	wid.val = pu8AssocRespInfo;
 	wid.size = u32MaxAssocRespInfoLen;
 
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
+	result = wilc_send_config_pkt(vif->wilc, GET_CFG, &wid, 1,
+				 wilc_get_vif_idx(vif));
 	if (result) {
 		*pu32RcvdAssocRespInfoLen = 0;
 		PRINT_ER("Failed to send association response config packet\n");
@@ -3621,24 +3515,11 @@ s32 host_int_get_assoc_res_info(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_get_rx_power_level(struct host_if_drv *hif_drv,
-				u8 *pu8RxPowerLevel,
-				u32 u32RxPowerLevelLen)
-{
-	struct wid wid;
-
-	wid.id = (u16)WID_RX_POWER_LEVEL;
-	wid.type = WID_STR;
-	wid.val = pu8RxPowerLevel;
-	wid.size = u32RxPowerLevelLen;
-
-	return 0;
-}
-
-int host_int_set_mac_chnl_num(struct host_if_drv *hif_drv, u8 channel)
+int wilc_set_mac_chnl_num(struct wilc_vif *vif, u8 channel)
 {
 	int result;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -3648,7 +3529,7 @@ int host_int_set_mac_chnl_num(struct host_if_drv *hif_drv, u8 channel)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_SET_CHANNEL;
 	msg.body.channel_info.set_ch = channel;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3659,7 +3540,7 @@ int host_int_set_mac_chnl_num(struct host_if_drv *hif_drv, u8 channel)
 	return 0;
 }
 
-int host_int_wait_msg_queue_idle(void)
+int wilc_wait_msg_queue_idle(void)
 {
 	int result = 0;
 	struct host_if_msg msg;
@@ -3677,15 +3558,15 @@ int host_int_wait_msg_queue_idle(void)
 	return result;
 }
 
-int host_int_set_wfi_drv_handler(struct host_if_drv *hif_drv)
+int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index)
 {
 	int result = 0;
 	struct host_if_msg msg;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_SET_WFIDRV_HANDLER;
-	msg.body.drv.handler = get_id_from_handler(hif_drv);
-	msg.drv = hif_drv;
+	msg.body.drv.handler = index;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3696,7 +3577,7 @@ int host_int_set_wfi_drv_handler(struct host_if_drv *hif_drv)
 	return result;
 }
 
-int host_int_set_operation_mode(struct host_if_drv *hif_drv, u32 mode)
+int wilc_set_operation_mode(struct wilc_vif *vif, u32 mode)
 {
 	int result = 0;
 	struct host_if_msg msg;
@@ -3704,7 +3585,7 @@ int host_int_set_operation_mode(struct host_if_drv *hif_drv, u32 mode)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_SET_OPERATION_MODE;
 	msg.body.mode.mode = mode;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3715,36 +3596,12 @@ int host_int_set_operation_mode(struct host_if_drv *hif_drv, u32 mode)
 	return result;
 }
 
-s32 host_int_get_host_chnl_num(struct host_if_drv *hif_drv, u8 *pu8ChNo)
-{
-	s32 result = 0;
-	struct host_if_msg msg;
-
-	if (!hif_drv) {
-		PRINT_ER("driver is null\n");
-		return -EFAULT;
-	}
-
-	memset(&msg, 0, sizeof(struct host_if_msg));
-
-	msg.id = HOST_IF_MSG_GET_CHNL;
-	msg.drv = hif_drv;
-
-	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
-	if (result)
-		PRINT_ER("wilc mq send fail\n");
-	down(&hif_drv->hSemGetCHNL);
-
-	*pu8ChNo = ch_no;
-
-	return result;
-}
-
-s32 host_int_get_inactive_time(struct host_if_drv *hif_drv,
-			       const u8 *mac, u32 *pu32InactiveTime)
+s32 wilc_get_inactive_time(struct wilc_vif *vif, const u8 *mac,
+			   u32 *pu32InactiveTime)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -3755,55 +3612,28 @@ s32 host_int_get_inactive_time(struct host_if_drv *hif_drv,
 	memcpy(msg.body.mac_info.mac, mac, ETH_ALEN);
 
 	msg.id = HOST_IF_MSG_GET_INACTIVETIME;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
 		PRINT_ER("Failed to send get host channel param's message queue ");
 
-	down(&hif_drv->hSemInactiveTime);
+	down(&hif_drv->sem_inactive_time);
 
 	*pu32InactiveTime = inactive_time;
 
 	return result;
 }
 
-s32 host_int_test_get_int_wid(struct host_if_drv *hif_drv, u32 *pu32TestMemAddr)
-{
-	s32 result = 0;
-	struct wid wid;
-
-	if (!hif_drv) {
-		PRINT_ER("driver is null\n");
-		return -EFAULT;
-	}
-
-	wid.id = (u16)WID_MEMORY_ADDRESS;
-	wid.type = WID_INT;
-	wid.val = (s8 *)pu32TestMemAddr;
-	wid.size = sizeof(u32);
-
-	result = send_config_pkt(GET_CFG, &wid, 1,
-				 get_id_from_handler(hif_drv));
-
-	if (result) {
-		PRINT_ER("Failed to get wid value\n");
-		return -EINVAL;
-	} else {
-		PRINT_D(HOSTINF_DBG, "Successfully got wid value\n");
-	}
-
-	return result;
-}
-
-s32 host_int_get_rssi(struct host_if_drv *hif_drv, s8 *ps8Rssi)
+s32 wilc_get_rssi(struct wilc_vif *vif, s8 *ps8Rssi)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_GET_RSSI;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3811,7 +3641,7 @@ s32 host_int_get_rssi(struct host_if_drv *hif_drv, s8 *ps8Rssi)
 		return -EFAULT;
 	}
 
-	down(&hif_drv->hSemGetRSSI);
+	down(&hif_drv->sem_get_rssi);
 
 	if (!ps8Rssi) {
 		PRINT_ER("RSS pointer value is null");
@@ -3823,34 +3653,7 @@ s32 host_int_get_rssi(struct host_if_drv *hif_drv, s8 *ps8Rssi)
 	return result;
 }
 
-s32 host_int_get_link_speed(struct host_if_drv *hif_drv, s8 *ps8lnkspd)
-{
-	struct host_if_msg msg;
-	s32 result = 0;
-
-	memset(&msg, 0, sizeof(struct host_if_msg));
-	msg.id = HOST_IF_MSG_GET_LINKSPEED;
-	msg.drv = hif_drv;
-
-	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
-	if (result) {
-		PRINT_ER("Failed to send GET_LINKSPEED to message queue ");
-		return -EFAULT;
-	}
-
-	down(&hif_drv->hSemGetLINKSPEED);
-
-	if (!ps8lnkspd) {
-		PRINT_ER("LINKSPEED pointer value is null");
-		return -EFAULT;
-	}
-
-	*ps8lnkspd = link_speed;
-
-	return result;
-}
-
-s32 host_int_get_statistics(struct host_if_drv *hif_drv, struct rf_info *pstrStatistics)
+s32 wilc_get_statistics(struct wilc_vif *vif, struct rf_info *pstrStatistics)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -3858,7 +3661,7 @@ s32 host_int_get_statistics(struct host_if_drv *hif_drv, struct rf_info *pstrSta
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_GET_STATISTICS;
 	msg.body.data = (char *)pstrStatistics;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result) {
@@ -3870,14 +3673,14 @@ s32 host_int_get_statistics(struct host_if_drv *hif_drv, struct rf_info *pstrSta
 	return result;
 }
 
-s32 host_int_scan(struct host_if_drv *hif_drv, u8 u8ScanSource,
-		  u8 u8ScanType, u8 *pu8ChnlFreqList,
-		  u8 u8ChnlListLen, const u8 *pu8IEs,
-		  size_t IEsLen, wilc_scan_result ScanResult,
-		  void *pvUserArg, struct hidden_network *pstrHiddenNetwork)
+s32 wilc_scan(struct wilc_vif *vif, u8 u8ScanSource, u8 u8ScanType,
+	      u8 *pu8ChnlFreqList, u8 u8ChnlListLen, const u8 *pu8IEs,
+	      size_t IEsLen, wilc_scan_result ScanResult, void *pvUserArg,
+	      struct hidden_network *pstrHiddenNetwork)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv || !ScanResult) {
 		PRINT_ER("hif_drv or ScanResult = NULL\n");
@@ -3895,7 +3698,7 @@ s32 host_int_scan(struct host_if_drv *hif_drv, u8 u8ScanSource,
 	} else
 		PRINT_D(HOSTINF_DBG, "pstrHiddenNetwork IS EQUAL TO NULL\n");
 
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.scan_info.src = u8ScanSource;
 	msg.body.scan_info.type = u8ScanType;
 	msg.body.scan_info.result = ScanResult;
@@ -3916,18 +3719,19 @@ s32 host_int_scan(struct host_if_drv *hif_drv, u8 u8ScanSource,
 	}
 
 	PRINT_D(HOSTINF_DBG, ">> Starting the SCAN timer\n");
-	hif_drv->hScanTimer.data = (unsigned long)hif_drv;
-	mod_timer(&hif_drv->hScanTimer,
+	hif_drv->scan_timer.data = (unsigned long)vif;
+	mod_timer(&hif_drv->scan_timer,
 		  jiffies + msecs_to_jiffies(HOST_IF_SCAN_TIMEOUT));
 
 	return result;
 }
 
-s32 hif_set_cfg(struct host_if_drv *hif_drv,
-		struct cfg_param_val *pstrCfgParamVal)
+s32 wilc_hif_set_cfg(struct wilc_vif *vif,
+		     struct cfg_param_val *pstrCfgParamVal)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("hif_drv NULL\n");
@@ -3937,123 +3741,30 @@ s32 hif_set_cfg(struct host_if_drv *hif_drv,
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_CFG_PARAMS;
 	msg.body.cfg_info.cfg_attr_info = *pstrCfgParamVal;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 
 	return result;
 }
 
-s32 hif_get_cfg(struct host_if_drv *hif_drv, u16 u16WID, u16 *pu16WID_Value)
-{
-	s32 result = 0;
-
-	down(&hif_drv->gtOsCfgValuesSem);
-
-	if (!hif_drv) {
-		PRINT_ER("hif_drv NULL\n");
-		return -EFAULT;
-	}
-	PRINT_D(HOSTINF_DBG, "Getting configuration parameters\n");
-	switch (u16WID)	{
-	case WID_BSS_TYPE:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.bss_type;
-		break;
-
-	case WID_AUTH_TYPE:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.auth_type;
-		break;
-
-	case WID_AUTH_TIMEOUT:
-		*pu16WID_Value = hif_drv->strCfgValues.auth_timeout;
-		break;
-
-	case WID_POWER_MANAGEMENT:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.power_mgmt_mode;
-		break;
-
-	case WID_SHORT_RETRY_LIMIT:
-		*pu16WID_Value =       hif_drv->strCfgValues.short_retry_limit;
-		break;
-
-	case WID_LONG_RETRY_LIMIT:
-		*pu16WID_Value = hif_drv->strCfgValues.long_retry_limit;
-		break;
-
-	case WID_FRAG_THRESHOLD:
-		*pu16WID_Value = hif_drv->strCfgValues.frag_threshold;
-		break;
-
-	case WID_RTS_THRESHOLD:
-		*pu16WID_Value = hif_drv->strCfgValues.rts_threshold;
-		break;
-
-	case WID_PREAMBLE:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.preamble_type;
-		break;
-
-	case WID_SHORT_SLOT_ALLOWED:
-		*pu16WID_Value = (u16) hif_drv->strCfgValues.short_slot_allowed;
-		break;
-
-	case WID_11N_TXOP_PROT_DISABLE:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.txop_prot_disabled;
-		break;
-
-	case WID_BEACON_INTERVAL:
-		*pu16WID_Value = hif_drv->strCfgValues.beacon_interval;
-		break;
-
-	case WID_DTIM_PERIOD:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.dtim_period;
-		break;
-
-	case WID_SITE_SURVEY:
-		*pu16WID_Value = (u16)hif_drv->strCfgValues.site_survey_enabled;
-		break;
-
-	case WID_SITE_SURVEY_SCAN_TIME:
-		*pu16WID_Value = hif_drv->strCfgValues.site_survey_scan_time;
-		break;
-
-	case WID_ACTIVE_SCAN_TIME:
-		*pu16WID_Value = hif_drv->strCfgValues.active_scan_time;
-		break;
-
-	case WID_PASSIVE_SCAN_TIME:
-		*pu16WID_Value = hif_drv->strCfgValues.passive_scan_time;
-		break;
-
-	case WID_CURRENT_TX_RATE:
-		*pu16WID_Value = hif_drv->strCfgValues.curr_tx_rate;
-		break;
-
-	default:
-		break;
-	}
-
-	up(&hif_drv->gtOsCfgValuesSem);
-
-	return result;
-}
-
 static void GetPeriodicRSSI(unsigned long arg)
 {
-	struct host_if_drv *hif_drv = (struct host_if_drv *)arg;
+	struct wilc_vif *vif = (struct wilc_vif *)arg;
 
-	if (!hif_drv)	{
+	if (!vif->hif_drv) {
 		PRINT_ER("Driver handler is NULL\n");
 		return;
 	}
 
-	if (hif_drv->enuHostIFstate == HOST_IF_CONNECTED) {
+	if (vif->hif_drv->hif_state == HOST_IF_CONNECTED) {
 		s32 result = 0;
 		struct host_if_msg msg;
 
 		memset(&msg, 0, sizeof(struct host_if_msg));
 
 		msg.id = HOST_IF_MSG_GET_RSSI;
-		msg.drv = hif_drv;
+		msg.vif = vif;
 
 		result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 		if (result) {
@@ -4061,20 +3772,20 @@ static void GetPeriodicRSSI(unsigned long arg)
 			return;
 		}
 	}
-	periodic_rssi.data = (unsigned long)hif_drv;
+	periodic_rssi.data = (unsigned long)vif;
 	mod_timer(&periodic_rssi, jiffies + msecs_to_jiffies(5000));
 }
 
-s32 host_int_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
+s32 wilc_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 {
 	s32 result = 0;
 	struct host_if_drv *hif_drv;
-	int err;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
+	int i;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
 	PRINT_D(HOSTINF_DBG, "Initializing host interface for client %d\n", clients_count + 1);
 
@@ -4088,13 +3799,13 @@ s32 host_int_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 		goto _fail_;
 	}
 	*hif_drv_handler = hif_drv;
-	err = add_handler_in_list(hif_drv);
-	if (err) {
-		result = -EFAULT;
-		goto _fail_timer_2;
-	}
+	for (i = 0; i < wilc->vif_num; i++)
+		if (dev == wilc->vif[i]->ndev) {
+			wilc->vif[i]->hif_drv = hif_drv;
+			break;
+		}
 
-	g_obtainingIP = false;
+	wilc_optaining_ip = false;
 
 	PRINT_D(HOSTINF_DBG, "Global handle pointer value=%p\n", hif_drv);
 	if (clients_count == 0)	{
@@ -4103,12 +3814,12 @@ s32 host_int_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 		sema_init(&hif_sema_deinit, 1);
 	}
 
-	sema_init(&hif_drv->hSemTestKeyBlock, 0);
-	sema_init(&hif_drv->hSemTestDisconnectBlock, 0);
-	sema_init(&hif_drv->hSemGetRSSI, 0);
-	sema_init(&hif_drv->hSemGetLINKSPEED, 0);
-	sema_init(&hif_drv->hSemGetCHNL, 0);
-	sema_init(&hif_drv->hSemInactiveTime, 0);
+	sema_init(&hif_drv->sem_test_key_block, 0);
+	sema_init(&hif_drv->sem_test_disconn_block, 0);
+	sema_init(&hif_drv->sem_get_rssi, 0);
+	sema_init(&hif_drv->sem_get_link_speed, 0);
+	sema_init(&hif_drv->sem_get_chnl, 0);
+	sema_init(&hif_drv->sem_inactive_time, 0);
 
 	PRINT_D(HOSTINF_DBG, "INIT: CLIENT COUNT %d\n", clients_count);
 
@@ -4129,56 +3840,50 @@ s32 host_int_init(struct net_device *dev, struct host_if_drv **hif_drv_handler)
 			goto _fail_mq_;
 		}
 		setup_timer(&periodic_rssi, GetPeriodicRSSI,
-			    (unsigned long)hif_drv);
+			    (unsigned long)vif);
 		mod_timer(&periodic_rssi, jiffies + msecs_to_jiffies(5000));
 	}
 
-	setup_timer(&hif_drv->hScanTimer, TimerCB_Scan, 0);
-
-	setup_timer(&hif_drv->hConnectTimer, TimerCB_Connect, 0);
+	setup_timer(&hif_drv->scan_timer, TimerCB_Scan, 0);
+	setup_timer(&hif_drv->connect_timer, TimerCB_Connect, 0);
+	setup_timer(&hif_drv->remain_on_ch_timer, ListenTimerCB, 0);
 
-	setup_timer(&hif_drv->hRemainOnChannel, ListenTimerCB, 0);
+	sema_init(&hif_drv->sem_cfg_values, 1);
+	down(&hif_drv->sem_cfg_values);
 
-	sema_init(&hif_drv->gtOsCfgValuesSem, 1);
-	down(&hif_drv->gtOsCfgValuesSem);
+	hif_drv->hif_state = HOST_IF_IDLE;
+	hif_drv->cfg_values.site_survey_enabled = SITE_SURVEY_OFF;
+	hif_drv->cfg_values.scan_source = DEFAULT_SCAN;
+	hif_drv->cfg_values.active_scan_time = ACTIVE_SCAN_TIME;
+	hif_drv->cfg_values.passive_scan_time = PASSIVE_SCAN_TIME;
+	hif_drv->cfg_values.curr_tx_rate = AUTORATE;
 
-	hif_drv->enuHostIFstate = HOST_IF_IDLE;
-	hif_drv->strCfgValues.site_survey_enabled = SITE_SURVEY_OFF;
-	hif_drv->strCfgValues.scan_source = DEFAULT_SCAN;
-	hif_drv->strCfgValues.active_scan_time = ACTIVE_SCAN_TIME;
-	hif_drv->strCfgValues.passive_scan_time = PASSIVE_SCAN_TIME;
-	hif_drv->strCfgValues.curr_tx_rate = AUTORATE;
-
-	hif_drv->u64P2p_MgmtTimeout = 0;
+	hif_drv->p2p_timeout = 0;
 
 	PRINT_INFO(HOSTINF_DBG, "Initialization values, Site survey value: %d\n Scan source: %d\n Active scan time: %d\n Passive scan time: %d\nCurrent tx Rate = %d\n",
+		   hif_drv->cfg_values.site_survey_enabled,
+		   hif_drv->cfg_values.scan_source,
+		   hif_drv->cfg_values.active_scan_time,
+		   hif_drv->cfg_values.passive_scan_time,
+		   hif_drv->cfg_values.curr_tx_rate);
 
-		   hif_drv->strCfgValues.site_survey_enabled, hif_drv->strCfgValues.scan_source,
-		   hif_drv->strCfgValues.active_scan_time, hif_drv->strCfgValues.passive_scan_time,
-		   hif_drv->strCfgValues.curr_tx_rate);
-
-	up(&hif_drv->gtOsCfgValuesSem);
+	up(&hif_drv->sem_cfg_values);
 
 	clients_count++;
 
 	return result;
 
-_fail_timer_2:
-	up(&hif_drv->gtOsCfgValuesSem);
-	del_timer_sync(&hif_drv->hConnectTimer);
-	del_timer_sync(&hif_drv->hScanTimer);
-	kthread_stop(hif_thread_handler);
 _fail_mq_:
 	wilc_mq_destroy(&hif_msg_q);
 _fail_:
 	return result;
 }
 
-s32 host_int_deinit(struct host_if_drv *hif_drv)
+s32 wilc_deinit(struct wilc_vif *vif)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
-	int ret;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv)	{
 		PRINT_ER("hif_drv = NULL\n");
@@ -4190,28 +3895,27 @@ s32 host_int_deinit(struct host_if_drv *hif_drv)
 	terminated_handle = hif_drv;
 	PRINT_D(HOSTINF_DBG, "De-initializing host interface for client %d\n", clients_count);
 
-	if (del_timer_sync(&hif_drv->hScanTimer))
+	if (del_timer_sync(&hif_drv->scan_timer))
 		PRINT_D(HOSTINF_DBG, ">> Scan timer is active\n");
 
-	if (del_timer_sync(&hif_drv->hConnectTimer))
+	if (del_timer_sync(&hif_drv->connect_timer))
 		PRINT_D(HOSTINF_DBG, ">> Connect timer is active\n");
 
 	if (del_timer_sync(&periodic_rssi))
 		PRINT_D(HOSTINF_DBG, ">> Connect timer is active\n");
 
-	del_timer_sync(&hif_drv->hRemainOnChannel);
+	del_timer_sync(&hif_drv->remain_on_ch_timer);
 
-	host_int_set_wfi_drv_handler(NULL);
+	wilc_set_wfi_drv_handler(vif, 0);
 	down(&hif_sema_driver);
 
-	if (hif_drv->usr_scan_req.pfUserScanResult) {
-		hif_drv->usr_scan_req.pfUserScanResult(SCAN_EVENT_ABORTED, NULL,
-						       hif_drv->usr_scan_req.u32UserScanPvoid, NULL);
-
-		hif_drv->usr_scan_req.pfUserScanResult = NULL;
+	if (hif_drv->usr_scan_req.scan_result) {
+		hif_drv->usr_scan_req.scan_result(SCAN_EVENT_ABORTED, NULL,
+						  hif_drv->usr_scan_req.arg, NULL);
+		hif_drv->usr_scan_req.scan_result = NULL;
 	}
 
-	hif_drv->enuHostIFstate = HOST_IF_IDLE;
+	hif_drv->hif_state = HOST_IF_IDLE;
 
 	scan_while_connected = false;
 
@@ -4222,7 +3926,7 @@ s32 host_int_deinit(struct host_if_drv *hif_drv)
 			PRINT_D(HOSTINF_DBG, ">> Connect timer is active\n");
 
 		msg.id = HOST_IF_MSG_EXIT;
-		msg.drv = hif_drv;
+		msg.vif = vif;
 
 		result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 		if (result != 0)
@@ -4233,12 +3937,6 @@ s32 host_int_deinit(struct host_if_drv *hif_drv)
 		wilc_mq_destroy(&hif_msg_q);
 	}
 
-	down(&hif_drv->gtOsCfgValuesSem);
-
-	ret = remove_handler_in_list(hif_drv);
-	if (ret)
-		result = -ENOENT;
-
 	kfree(hif_drv);
 
 	clients_count--;
@@ -4247,15 +3945,20 @@ s32 host_int_deinit(struct host_if_drv *hif_drv)
 	return result;
 }
 
-void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length)
+void wilc_network_info_received(struct wilc *wilc, u8 *pu8Buffer,
+				u32 u32Length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	int id;
 	struct host_if_drv *hif_drv = NULL;
+	struct wilc_vif *vif;
 
 	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
-	hif_drv = get_handler_from_id(id);
+	vif = wilc_get_vif_from_idx(wilc, id);
+	if (!vif)
+		return;
+	hif_drv = vif->hif_drv;
 
 	if (!hif_drv || hif_drv == terminated_handle)	{
 		PRINT_ER("NetworkInfo received but driver not init[%p]\n", hif_drv);
@@ -4265,7 +3968,7 @@ void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_RCVD_NTWRK_INFO;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	msg.body.net_info.len = u32Length;
 	msg.body.net_info.buffer = kmalloc(u32Length, GFP_KERNEL);
@@ -4276,17 +3979,25 @@ void NetworkInfoReceived(u8 *pu8Buffer, u32 u32Length)
 		PRINT_ER("Error in sending network info message queue message parameters: Error(%d)\n", result);
 }
 
-void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length)
+void wilc_gnrl_async_info_received(struct wilc *wilc, u8 *pu8Buffer,
+				   u32 u32Length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	int id;
 	struct host_if_drv *hif_drv = NULL;
+	struct wilc_vif *vif;
 
 	down(&hif_sema_deinit);
 
 	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
-	hif_drv = get_handler_from_id(id);
+	vif = wilc_get_vif_from_idx(wilc, id);
+	if (!vif) {
+		up(&hif_sema_deinit);
+		return;
+	}
+
+	hif_drv = vif->hif_drv;
 	PRINT_D(HOSTINF_DBG, "General asynchronous info packet received\n");
 
 	if (!hif_drv || hif_drv == terminated_handle) {
@@ -4295,7 +4006,7 @@ void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length)
 		return;
 	}
 
-	if (!hif_drv->usr_conn_req.pfUserConnectResult) {
+	if (!hif_drv->usr_conn_req.conn_result) {
 		PRINT_ER("Received mac status is not needed when there is no current Connect Reques\n");
 		up(&hif_sema_deinit);
 		return;
@@ -4304,7 +4015,7 @@ void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length)
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_RCVD_GNRL_ASYNC_INFO;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	msg.body.async_info.len = u32Length;
 	msg.body.async_info.buffer = kmalloc(u32Length, GFP_KERNEL);
@@ -4317,26 +4028,31 @@ void GnrlAsyncInfoReceived(u8 *pu8Buffer, u32 u32Length)
 	up(&hif_sema_deinit);
 }
 
-void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length)
+void wilc_scan_complete_received(struct wilc *wilc, u8 *pu8Buffer,
+				 u32 u32Length)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	int id;
 	struct host_if_drv *hif_drv = NULL;
+	struct wilc_vif *vif;
 
 	id = ((pu8Buffer[u32Length - 4]) | (pu8Buffer[u32Length - 3] << 8) | (pu8Buffer[u32Length - 2] << 16) | (pu8Buffer[u32Length - 1] << 24));
-	hif_drv = get_handler_from_id(id);
+	vif = wilc_get_vif_from_idx(wilc, id);
+	if (!vif)
+		return;
+	hif_drv = vif->hif_drv;
 
 	PRINT_D(GENERIC_DBG, "Scan notification received %p\n", hif_drv);
 
 	if (!hif_drv || hif_drv == terminated_handle)
 		return;
 
-	if (hif_drv->usr_scan_req.pfUserScanResult) {
+	if (hif_drv->usr_scan_req.scan_result) {
 		memset(&msg, 0, sizeof(struct host_if_msg));
 
 		msg.id = HOST_IF_MSG_RCVD_SCAN_COMPLETE;
-		msg.drv = hif_drv;
+		msg.vif = vif;
 
 		result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 		if (result)
@@ -4346,14 +4062,15 @@ void host_int_ScanCompleteReceived(u8 *pu8Buffer, u32 u32Length)
 	return;
 }
 
-s32 host_int_remain_on_channel(struct host_if_drv *hif_drv, u32 u32SessionID,
-			       u32 u32duration, u16 chan,
-			       wilc_remain_on_chan_expired RemainOnChanExpired,
-			       wilc_remain_on_chan_ready RemainOnChanReady,
-			       void *pvUserArg)
+s32 wilc_remain_on_channel(struct wilc_vif *vif, u32 u32SessionID,
+			   u32 u32duration, u16 chan,
+			   wilc_remain_on_chan_expired RemainOnChanExpired,
+			   wilc_remain_on_chan_ready RemainOnChanReady,
+			   void *pvUserArg)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4363,13 +4080,13 @@ s32 host_int_remain_on_channel(struct host_if_drv *hif_drv, u32 u32SessionID,
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_REMAIN_ON_CHAN;
-	msg.body.remain_on_ch.u16Channel = chan;
-	msg.body.remain_on_ch.pRemainOnChanExpired = RemainOnChanExpired;
-	msg.body.remain_on_ch.pRemainOnChanReady = RemainOnChanReady;
-	msg.body.remain_on_ch.pVoid = pvUserArg;
+	msg.body.remain_on_ch.ch = chan;
+	msg.body.remain_on_ch.expired = RemainOnChanExpired;
+	msg.body.remain_on_ch.ready = RemainOnChanReady;
+	msg.body.remain_on_ch.arg = pvUserArg;
 	msg.body.remain_on_ch.u32duration = u32duration;
-	msg.body.remain_on_ch.u32ListenSessionID = u32SessionID;
-	msg.drv = hif_drv;
+	msg.body.remain_on_ch.id = u32SessionID;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -4378,22 +4095,23 @@ s32 host_int_remain_on_channel(struct host_if_drv *hif_drv, u32 u32SessionID,
 	return result;
 }
 
-s32 host_int_ListenStateExpired(struct host_if_drv *hif_drv, u32 u32SessionID)
+s32 wilc_listen_state_expired(struct wilc_vif *vif, u32 u32SessionID)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
 		return -EFAULT;
 	}
 
-	del_timer(&hif_drv->hRemainOnChannel);
+	del_timer(&hif_drv->remain_on_ch_timer);
 
 	memset(&msg, 0, sizeof(struct host_if_msg));
 	msg.id = HOST_IF_MSG_LISTEN_TIMER_FIRED;
-	msg.drv = hif_drv;
-	msg.body.remain_on_ch.u32ListenSessionID = u32SessionID;
+	msg.vif = vif;
+	msg.body.remain_on_ch.id = u32SessionID;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -4402,10 +4120,11 @@ s32 host_int_ListenStateExpired(struct host_if_drv *hif_drv, u32 u32SessionID)
 	return result;
 }
 
-s32 host_int_frame_register(struct host_if_drv *hif_drv, u16 u16FrameType, bool bReg)
+s32 wilc_frame_register(struct wilc_vif *vif, u16 u16FrameType, bool bReg)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4418,21 +4137,21 @@ s32 host_int_frame_register(struct host_if_drv *hif_drv, u16 u16FrameType, bool
 	switch (u16FrameType) {
 	case ACTION:
 		PRINT_D(HOSTINF_DBG, "ACTION\n");
-		msg.body.reg_frame.u8Regid = ACTION_FRM_IDX;
+		msg.body.reg_frame.reg_id = ACTION_FRM_IDX;
 		break;
 
 	case PROBE_REQ:
 		PRINT_D(HOSTINF_DBG, "PROBE REQ\n");
-		msg.body.reg_frame.u8Regid = PROBE_REQ_IDX;
+		msg.body.reg_frame.reg_id = PROBE_REQ_IDX;
 		break;
 
 	default:
 		PRINT_D(HOSTINF_DBG, "Not valid frame type\n");
 		break;
 	}
-	msg.body.reg_frame.u16FrameType = u16FrameType;
-	msg.body.reg_frame.bReg = bReg;
-	msg.drv = hif_drv;
+	msg.body.reg_frame.frame_type = u16FrameType;
+	msg.body.reg_frame.reg = bReg;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -4441,13 +4160,13 @@ s32 host_int_frame_register(struct host_if_drv *hif_drv, u16 u16FrameType, bool
 	return result;
 }
 
-s32 host_int_add_beacon(struct host_if_drv *hif_drv, u32 u32Interval,
-			u32 u32DTIMPeriod, u32 u32HeadLen, u8 *pu8Head,
-			u32 u32TailLen, u8 *pu8Tail)
+s32 wilc_add_beacon(struct wilc_vif *vif, u32 u32Interval, u32 u32DTIMPeriod,
+		    u32 u32HeadLen, u8 *pu8Head, u32 u32TailLen, u8 *pu8Tail)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct beacon_attr *pstrSetBeaconParam = &msg.body.beacon_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4459,7 +4178,7 @@ s32 host_int_add_beacon(struct host_if_drv *hif_drv, u32 u32Interval,
 	PRINT_D(HOSTINF_DBG, "Setting adding beacon message queue params\n");
 
 	msg.id = HOST_IF_MSG_ADD_BEACON;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	pstrSetBeaconParam->interval = u32Interval;
 	pstrSetBeaconParam->dtim_period = u32DTIMPeriod;
 	pstrSetBeaconParam->head_len = u32HeadLen;
@@ -4495,10 +4214,11 @@ ERRORHANDLER:
 	return result;
 }
 
-s32 host_int_del_beacon(struct host_if_drv *hif_drv)
+int wilc_del_beacon(struct wilc_vif *vif)
 {
-	s32 result = 0;
+	int result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4506,7 +4226,7 @@ s32 host_int_del_beacon(struct host_if_drv *hif_drv)
 	}
 
 	msg.id = HOST_IF_MSG_DEL_BEACON;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	PRINT_D(HOSTINF_DBG, "Setting deleting beacon message queue params\n");
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -4516,12 +4236,12 @@ s32 host_int_del_beacon(struct host_if_drv *hif_drv)
 	return result;
 }
 
-s32 host_int_add_station(struct host_if_drv *hif_drv,
-			 struct add_sta_param *pstrStaParams)
+int wilc_add_station(struct wilc_vif *vif, struct add_sta_param *sta_param)
 {
-	s32 result = 0;
+	int result = 0;
 	struct host_if_msg msg;
-	struct add_sta_param *pstrAddStationMsg = &msg.body.add_sta_info;
+	struct add_sta_param *add_sta_info = &msg.body.add_sta_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4533,17 +4253,15 @@ s32 host_int_add_station(struct host_if_drv *hif_drv,
 	PRINT_D(HOSTINF_DBG, "Setting adding station message queue params\n");
 
 	msg.id = HOST_IF_MSG_ADD_STATION;
-	msg.drv = hif_drv;
-
-	memcpy(pstrAddStationMsg, pstrStaParams, sizeof(struct add_sta_param));
-	if (pstrAddStationMsg->u8NumRates > 0) {
-		u8 *rates = kmalloc(pstrAddStationMsg->u8NumRates, GFP_KERNEL);
-
-		if (!rates)
+	msg.vif = vif;
+
+	memcpy(add_sta_info, sta_param, sizeof(struct add_sta_param));
+	if (add_sta_info->rates_len > 0) {
+		add_sta_info->rates = kmemdup(sta_param->rates,
+				      add_sta_info->rates_len,
+				      GFP_KERNEL);
+		if (!add_sta_info->rates)
 			return -ENOMEM;
-
-		memcpy(rates, pstrStaParams->pu8Rates, pstrAddStationMsg->u8NumRates);
-		pstrAddStationMsg->pu8Rates = rates;
 	}
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -4552,11 +4270,12 @@ s32 host_int_add_station(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_del_station(struct host_if_drv *hif_drv, const u8 *pu8MacAddr)
+int wilc_del_station(struct wilc_vif *vif, const u8 *mac_addr)
 {
-	s32 result = 0;
+	int result = 0;
 	struct host_if_msg msg;
-	struct del_sta *pstrDelStationMsg = &msg.body.del_sta_info;
+	struct del_sta *del_sta_info = &msg.body.del_sta_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4568,12 +4287,12 @@ s32 host_int_del_station(struct host_if_drv *hif_drv, const u8 *pu8MacAddr)
 	PRINT_D(HOSTINF_DBG, "Setting deleting station message queue params\n");
 
 	msg.id = HOST_IF_MSG_DEL_STATION;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
-	if (!pu8MacAddr)
-		eth_broadcast_addr(pstrDelStationMsg->mac_addr);
+	if (!mac_addr)
+		eth_broadcast_addr(del_sta_info->mac_addr);
 	else
-		memcpy(pstrDelStationMsg->mac_addr, pu8MacAddr, ETH_ALEN);
+		memcpy(del_sta_info->mac_addr, mac_addr, ETH_ALEN);
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -4581,12 +4300,12 @@ s32 host_int_del_station(struct host_if_drv *hif_drv, const u8 *pu8MacAddr)
 	return result;
 }
 
-s32 host_int_del_allstation(struct host_if_drv *hif_drv,
-			    u8 pu8MacAddr[][ETH_ALEN])
+s32 wilc_del_allstation(struct wilc_vif *vif, u8 pu8MacAddr[][ETH_ALEN])
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct del_all_sta *pstrDelAllStationMsg = &msg.body.del_all_sta_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 	u8 au8Zero_Buff[ETH_ALEN] = {0};
 	u32 i;
 	u8 u8AssocNumb = 0;
@@ -4601,7 +4320,7 @@ s32 host_int_del_allstation(struct host_if_drv *hif_drv,
 	PRINT_D(HOSTINF_DBG, "Setting deauthenticating station message queue params\n");
 
 	msg.id = HOST_IF_MSG_DEL_ALL_STA;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	for (i = 0; i < MAX_NUM_STA; i++) {
 		if (memcmp(pu8MacAddr[i], au8Zero_Buff, ETH_ALEN)) {
@@ -4632,12 +4351,13 @@ s32 host_int_del_allstation(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_edit_station(struct host_if_drv *hif_drv,
-			  struct add_sta_param *pstrStaParams)
+s32 wilc_edit_station(struct wilc_vif *vif,
+		      struct add_sta_param *pstrStaParams)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct add_sta_param *pstrAddStationMsg = &msg.body.add_sta_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4649,17 +4369,18 @@ s32 host_int_edit_station(struct host_if_drv *hif_drv,
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_EDIT_STATION;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	memcpy(pstrAddStationMsg, pstrStaParams, sizeof(struct add_sta_param));
-	if (pstrAddStationMsg->u8NumRates > 0) {
-		u8 *rates = kmalloc(pstrAddStationMsg->u8NumRates, GFP_KERNEL);
+	if (pstrAddStationMsg->rates_len > 0) {
+		u8 *rates = kmalloc(pstrAddStationMsg->rates_len, GFP_KERNEL);
 
 		if (!rates)
 			return -ENOMEM;
 
-		memcpy(rates, pstrStaParams->pu8Rates, pstrAddStationMsg->u8NumRates);
-		pstrAddStationMsg->pu8Rates = rates;
+		memcpy(rates, pstrStaParams->rates,
+		       pstrAddStationMsg->rates_len);
+		pstrAddStationMsg->rates = rates;
 	}
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -4669,13 +4390,12 @@ s32 host_int_edit_station(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_set_power_mgmt(struct host_if_drv *hif_drv,
-			    bool bIsEnabled,
-			    u32 u32Timeout)
+s32 wilc_set_power_mgmt(struct wilc_vif *vif, bool bIsEnabled, u32 u32Timeout)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct power_mgmt_param *pstrPowerMgmtParam = &msg.body.pwr_mgmt_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	PRINT_INFO(HOSTINF_DBG, "\n\n>> Setting PS to %d <<\n\n", bIsEnabled);
 
@@ -4689,7 +4409,7 @@ s32 host_int_set_power_mgmt(struct host_if_drv *hif_drv,
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_POWER_MGMT;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	pstrPowerMgmtParam->enabled = bIsEnabled;
 	pstrPowerMgmtParam->timeout = u32Timeout;
@@ -4700,13 +4420,13 @@ s32 host_int_set_power_mgmt(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_setup_multicast_filter(struct host_if_drv *hif_drv,
-				    bool bIsEnabled,
-				    u32 u32count)
+s32 wilc_setup_multicast_filter(struct wilc_vif *vif, bool bIsEnabled,
+				u32 u32count)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct set_multicast *pstrMulticastFilterParam = &msg.body.multicast_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4718,7 +4438,7 @@ s32 host_int_setup_multicast_filter(struct host_if_drv *hif_drv,
 	memset(&msg, 0, sizeof(struct host_if_msg));
 
 	msg.id = HOST_IF_MSG_SET_MULTICAST_FILTER;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 
 	pstrMulticastFilterParam->enabled = bIsEnabled;
 	pstrMulticastFilterParam->cnt = u32count;
@@ -4886,7 +4606,7 @@ static void *host_int_ParseJoinBssParam(tstrNetworkInfo *ptstrNetworkInfo)
 	return (void *)pNewJoinBssParam;
 }
 
-void host_int_freeJoinParams(void *pJoinParams)
+void wilc_free_join_params(void *pJoinParams)
 {
 	if ((struct bss_param *)pJoinParams)
 		kfree((struct bss_param *)pJoinParams);
@@ -4894,41 +4614,12 @@ void host_int_freeJoinParams(void *pJoinParams)
 		PRINT_ER("Unable to FREE null pointer\n");
 }
 
-s32 host_int_delBASession(struct host_if_drv *hif_drv, char *pBSSID, char TID)
-{
-	s32 result = 0;
-	struct host_if_msg msg;
-	struct ba_session_info *pBASessionInfo = &msg.body.session_info;
-
-	if (!hif_drv) {
-		PRINT_ER("driver is null\n");
-		return -EFAULT;
-	}
-
-	memset(&msg, 0, sizeof(struct host_if_msg));
-
-	msg.id = HOST_IF_MSG_DEL_BA_SESSION;
-
-	memcpy(pBASessionInfo->au8Bssid, pBSSID, ETH_ALEN);
-	pBASessionInfo->u8Ted = TID;
-	msg.drv = hif_drv;
-
-	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
-	if (result)
-		PRINT_ER("wilc_mq_send fail\n");
-
-	down(&hif_sema_wait_response);
-
-	return result;
-}
-
-s32 host_int_del_All_Rx_BASession(struct host_if_drv *hif_drv,
-				  char *pBSSID,
-				  char TID)
+s32 wilc_del_all_rx_ba_session(struct wilc_vif *vif, char *pBSSID, char TID)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
 	struct ba_session_info *pBASessionInfo = &msg.body.session_info;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	if (!hif_drv) {
 		PRINT_ER("driver is null\n");
@@ -4939,9 +4630,9 @@ s32 host_int_del_All_Rx_BASession(struct host_if_drv *hif_drv,
 
 	msg.id = HOST_IF_MSG_DEL_ALL_RX_BA_SESSIONS;
 
-	memcpy(pBASessionInfo->au8Bssid, pBSSID, ETH_ALEN);
-	pBASessionInfo->u8Ted = TID;
-	msg.drv = hif_drv;
+	memcpy(pBASessionInfo->bssid, pBSSID, ETH_ALEN);
+	pBASessionInfo->tid = TID;
+	msg.vif = vif;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
 	if (result)
@@ -4952,10 +4643,11 @@ s32 host_int_del_All_Rx_BASession(struct host_if_drv *hif_drv,
 	return result;
 }
 
-s32 host_int_setup_ipaddress(struct host_if_drv *hif_drv, u8 *u16ipadd, u8 idx)
+s32 wilc_setup_ipaddress(struct wilc_vif *vif, u8 *u16ipadd, u8 idx)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
+	struct host_if_drv *hif_drv = vif->hif_drv;
 
 	return 0;
 
@@ -4969,7 +4661,7 @@ s32 host_int_setup_ipaddress(struct host_if_drv *hif_drv, u8 *u16ipadd, u8 idx)
 	msg.id = HOST_IF_MSG_SET_IPADDRESS;
 
 	msg.body.ip_info.ip_addr = u16ipadd;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.ip_info.idx = idx;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
@@ -4979,7 +4671,9 @@ s32 host_int_setup_ipaddress(struct host_if_drv *hif_drv, u8 *u16ipadd, u8 idx)
 	return result;
 }
 
-s32 host_int_get_ipaddress(struct host_if_drv *hif_drv, u8 *u16ipadd, u8 idx)
+static s32 host_int_get_ipaddress(struct wilc_vif *vif,
+				  struct host_if_drv *hif_drv,
+				  u8 *u16ipadd, u8 idx)
 {
 	s32 result = 0;
 	struct host_if_msg msg;
@@ -4994,7 +4688,7 @@ s32 host_int_get_ipaddress(struct host_if_drv *hif_drv, u8 *u16ipadd, u8 idx)
 	msg.id = HOST_IF_MSG_GET_IPADDRESS;
 
 	msg.body.ip_info.ip_addr = u16ipadd;
-	msg.drv = hif_drv;
+	msg.vif = vif;
 	msg.body.ip_info.idx = idx;
 
 	result = wilc_mq_send(&hif_msg_q, &msg, sizeof(struct host_if_msg));
diff --git a/drivers/staging/wilc1000/host_interface.h b/drivers/staging/wilc1000/host_interface.h
index b854db5ac932..8faac27002e9 100644
--- a/drivers/staging/wilc1000/host_interface.h
+++ b/drivers/staging/wilc1000/host_interface.h
@@ -1,12 +1,3 @@
-/*!
- *  @file	host_interface.h
- *  @brief	File containg host interface APIs
- *  @author	zsalah
- *  @sa		host_interface.c
- *  @date	8 March 2012
- *  @version	1.0
- */
-
 #ifndef HOST_INT_H
 #define HOST_INT_H
 
@@ -19,8 +10,12 @@
 #define STATION_MODE	0x02
 #define GO_MODE		0x03
 #define CLIENT_MODE	0x04
+#define ACTION		0xD0
+#define PROBE_REQ	0x40
+#define PROBE_RESP	0x50
 
-
+#define ACTION_FRM_IDX				0
+#define PROBE_REQ_IDX				1
 #define MAX_NUM_STA				9
 #define ACTIVE_SCAN_TIME			10
 #define PASSIVE_SCAN_TIME			1200
@@ -58,11 +53,11 @@
 #define NUM_CONCURRENT_IFC			2
 
 struct rf_info {
-	u8 u8LinkSpeed;
-	s8 s8RSSI;
-	u32 u32TxCount;
-	u32 u32RxCount;
-	u32 u32TxFailureCount;
+	u8 link_speed;
+	s8 rssi;
+	u32 tx_cnt;
+	u32 rx_cnt;
+	u32 tx_fail_cnt;
 };
 
 enum host_if_state {
@@ -168,36 +163,23 @@ enum conn_event {
 
 enum KEY_TYPE {
 	WEP,
-	WPARxGtk,
-	WPAPtk,
+	WPA_RX_GTK,
+	WPA_PTK,
 	PMKSA,
 };
 
-
-/*Scan callBack function definition*/
 typedef void (*wilc_scan_result)(enum scan_event, tstrNetworkInfo *,
 				  void *, void *);
 
-/*Connect callBack function definition*/
 typedef void (*wilc_connect_result)(enum conn_event,
 				     tstrConnectInfo *,
 				     u8,
 				     tstrDisconnectNotifInfo *,
 				     void *);
 
-typedef void (*wilc_remain_on_chan_expired)(void *, u32);  /*Remain on channel expiration callback function*/
-typedef void (*wilc_remain_on_chan_ready)(void *); /*Remain on channel callback function*/
+typedef void (*wilc_remain_on_chan_expired)(void *, u32);
+typedef void (*wilc_remain_on_chan_ready)(void *);
 
-/*!
- *  @struct             rcvd_net_info
- *  @brief		Structure to hold Received Asynchronous Network info
- *  @details
- *  @todo
- *  @sa
- *  @author		Mostafa Abu Bakr
- *  @date		25 March 2012
- *  @version		1.0
- */
 struct rcvd_net_info {
 	u8 *buffer;
 	u32 len;
@@ -214,29 +196,23 @@ struct hidden_network {
 };
 
 struct user_scan_req {
-	/* Scan user call back function */
-	wilc_scan_result pfUserScanResult;
-
-	/* User specific parameter to be delivered through the Scan User Callback function */
-	void *u32UserScanPvoid;
-
-	u32 u32RcvdChCount;
-	struct found_net_info astrFoundNetworkInfo[MAX_NUM_SCANNED_NETWORKS];
+	wilc_scan_result scan_result;
+	void *arg;
+	u32 rcvd_ch_cnt;
+	struct found_net_info net_info[MAX_NUM_SCANNED_NETWORKS];
 };
 
 struct user_conn_req {
 	u8 *pu8bssid;
 	u8 *pu8ssid;
 	u8 u8security;
-	enum AUTHTYPE tenuAuth_type;
-	size_t ssidLen;
-	u8 *pu8ConnReqIEs;
-	size_t ConnReqIEsLen;
-	/* Connect user call back function */
-	wilc_connect_result pfUserConnectResult;
-	bool IsHTCapable;
-	/* User specific parameter to be delivered through the Connect User Callback function */
-	void *u32UserConnectPvoid;
+	enum AUTHTYPE auth_type;
+	size_t ssid_len;
+	u8 *ies;
+	size_t ies_len;
+	wilc_connect_result conn_result;
+	bool ht_capable;
+	void *arg;
 };
 
 struct drv_handler {
@@ -256,875 +232,155 @@ struct get_mac_addr {
 };
 
 struct ba_session_info {
-	u8 au8Bssid[ETH_ALEN];
-	u8 u8Ted;
-	u16 u16BufferSize;
-	u16 u16SessionTimeout;
+	u8 bssid[ETH_ALEN];
+	u8 tid;
+	u16 buf_size;
+	u16 time_out;
 };
 
 struct remain_ch {
-	u16 u16Channel;
+	u16 ch;
 	u32 u32duration;
-	wilc_remain_on_chan_expired pRemainOnChanExpired;
-	wilc_remain_on_chan_ready pRemainOnChanReady;
-	void *pVoid;
-	u32 u32ListenSessionID;
+	wilc_remain_on_chan_expired expired;
+	wilc_remain_on_chan_ready ready;
+	void *arg;
+	u32 id;
 };
 
 struct reg_frame {
-	bool bReg;
-	u16 u16FrameType;
-	u8 u8Regid;
+	bool reg;
+	u16 frame_type;
+	u8 reg_id;
 };
 
-
-#define ACTION			0xD0
-#define PROBE_REQ		0x40
-#define PROBE_RESP		0x50
-#define ACTION_FRM_IDX		0
-#define PROBE_REQ_IDX		1
-
-
 enum p2p_listen_state {
 	P2P_IDLE,
 	P2P_LISTEN,
 	P2P_GRP_FORMATION
 };
 
+struct wilc;
 struct host_if_drv {
 	struct user_scan_req usr_scan_req;
 	struct user_conn_req usr_conn_req;
 	struct remain_ch remain_on_ch;
 	u8 remain_on_ch_pending;
-	u64 u64P2p_MgmtTimeout;
-	u8 u8P2PConnect;
+	u64 p2p_timeout;
+	u8 p2p_connect;
+
+	enum host_if_state hif_state;
 
-	enum host_if_state enuHostIFstate;
+	u8 assoc_bssid[ETH_ALEN];
+	struct cfg_param_val cfg_values;
 
-	u8 au8AssociatedBSSID[ETH_ALEN];
-	struct cfg_param_val strCfgValues;
-/* semaphores */
-	struct semaphore gtOsCfgValuesSem;
-	struct semaphore hSemTestKeyBlock;
+	struct semaphore sem_cfg_values;
+	struct semaphore sem_test_key_block;
+	struct semaphore sem_test_disconn_block;
+	struct semaphore sem_get_rssi;
+	struct semaphore sem_get_link_speed;
+	struct semaphore sem_get_chnl;
+	struct semaphore sem_inactive_time;
 
-	struct semaphore hSemTestDisconnectBlock;
-	struct semaphore hSemGetRSSI;
-	struct semaphore hSemGetLINKSPEED;
-	struct semaphore hSemGetCHNL;
-	struct semaphore hSemInactiveTime;
-/* timer handlers */
-	struct timer_list hScanTimer;
-	struct timer_list hConnectTimer;
-	struct timer_list hRemainOnChannel;
+	struct timer_list scan_timer;
+	struct timer_list connect_timer;
+	struct timer_list remain_on_ch_timer;
 
 	bool IFC_UP;
 };
 
 struct add_sta_param {
-	u8 au8BSSID[ETH_ALEN];
-	u16 u16AssocID;
-	u8 u8NumRates;
-	const u8 *pu8Rates;
-	bool bIsHTSupported;
-	u16 u16HTCapInfo;
-	u8 u8AmpduParams;
-	u8 au8SuppMCsSet[16];
-	u16 u16HTExtParams;
-	u32 u32TxBeamformingCap;
-	u8 u8ASELCap;
-	u16 u16FlagsMask;               /*<! Determines which of u16FlagsSet were changed>*/
-	u16 u16FlagsSet;                /*<! Decoded according to tenuWILC_StaFlag */
+	u8 bssid[ETH_ALEN];
+	u16 aid;
+	u8 rates_len;
+	const u8 *rates;
+	bool ht_supported;
+	u16 ht_capa_info;
+	u8 ht_ampdu_params;
+	u8 ht_supp_mcs_set[16];
+	u16 ht_ext_params;
+	u32 ht_tx_bf_cap;
+	u8 ht_ante_sel;
+	u16 flags_mask;
+	u16 flags_set;
 };
 
-/*****************************************************************************/
-/*																			 */
-/*							Host Interface API								 */
-/*																			 */
-/*****************************************************************************/
-
-/**
- *  @brief              removes wpa/wpa2 keys
- *  @details    only in BSS STA mode if External Supplicant support is enabled.
- *                              removes all WPA/WPA2 station key entries from MAC hardware.
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  6 bytes of Station Adress in the station entry table
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_remove_key(struct host_if_drv *hWFIDrv, const u8 *pu8StaAddress);
-/**
- *  @brief              removes WEP key
- *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
- *                              remove a WEP key entry from MAC HW.
- *                              The BSS Station automatically finds the index of the entry using its
- *                              BSS ID and removes that entry from the MAC hardware.
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  6 bytes of Station Adress in the station entry table
- *  @return             Error code indicating success/failure
- *  @note               NO need for the STA add since it is not used for processing
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-int host_int_remove_wep_key(struct host_if_drv *wfi_drv, u8 index);
-/**
- *  @brief              sets WEP deafault key
- *  @details    Sets the index of the WEP encryption key in use,
- *                              in the key table
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  key index ( 0, 1, 2, 3)
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-int host_int_set_wep_default_key(struct host_if_drv *hif_drv, u8 index);
-
-/**
- *  @brief              sets WEP deafault key
- *  @details    valid only in BSS STA mode if External Supplicant support is enabled.
- *                              sets WEP key entry into MAC hardware when it receives the
- *                              corresponding request from NDIS.
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing WEP Key in the following format
- *|---------------------------------------|
- *|Key ID Value | Key Length |	Key		|
- *|-------------|------------|------------|
- |	1byte	  |		1byte  | Key Length	|
- ||---------------------------------------|
- |
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-int host_int_add_wep_key_bss_sta(struct host_if_drv *hif_drv,
-				 const u8 *key, u8 len, u8 index);
-/**
- *  @brief              host_int_add_wep_key_bss_ap
- *  @details    valid only in AP mode if External Supplicant support is enabled.
- *                              sets WEP key entry into MAC hardware when it receives the
- *                              corresponding request from NDIS.
- *  @param[in,out] handle to the wifi driver
- *
- *
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		mdaftedar
- *  @date		28 Feb 2013
- *  @version		1.0
- */
-int host_int_add_wep_key_bss_ap(struct host_if_drv *hif_drv,
-				const u8 *key, u8 len, u8 index, u8 mode,
-				enum AUTHTYPE auth_type);
-
-/**
- *  @brief              adds ptk Key
- *  @details
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing PTK Key in the following format
- *|-------------------------------------------------------------------------|
- *|Sta Adress | Key Length |	Temporal Key | Rx Michael Key |Tx Michael Key |
- *|-----------|------------|---------------|----------------|---------------|
- |	6 bytes |	1byte	 |   16 bytes	 |	  8 bytes	  |	   8 bytes	  |
- ||-------------------------------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_add_ptk(struct host_if_drv *hWFIDrv, const u8 *pu8Ptk, u8 u8PtkKeylen,
-			     const u8 *mac_addr, const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode, u8 u8Idx);
-
-/**
- *  @brief              host_int_get_inactive_time
- *  @details
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing inactive time
- *
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		mdaftedar
- *  @date		15 April 2013
- *  @version		1.0
- */
-s32 host_int_get_inactive_time(struct host_if_drv *hWFIDrv, const u8 *mac, u32 *pu32InactiveTime);
-
-/**
- *  @brief              adds Rx GTk Key
- *  @details
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing Rx GTK Key in the following format
- *|----------------------------------------------------------------------------|
- *|Sta Address | Key RSC | KeyID | Key Length | Temporal Key	| Rx Michael Key |
- *|------------|---------|-------|------------|---------------|----------------|
- |	6 bytes	 | 8 byte  |1 byte |  1 byte	|   16 bytes	|	  8 bytes	 |
- ||----------------------------------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_add_rx_gtk(struct host_if_drv *hWFIDrv, const u8 *pu8RxGtk, u8 u8GtkKeylen,
-				u8 u8KeyIdx, u32 u32KeyRSClen, const u8 *KeyRSC,
-				const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode, u8 u8Ciphermode);
-
-
-/**
- *  @brief              adds Tx GTk Key
- *  @details
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing Tx GTK Key in the following format
- *|----------------------------------------------------|
- | KeyID | Key Length | Temporal Key	| Tx Michael Key |
- ||-------|------------|--------------|----------------|
- ||1 byte |  1 byte	 |   16 bytes	|	  8 bytes	 |
- ||----------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_add_tx_gtk(struct host_if_drv *hWFIDrv, u8 u8KeyLen, u8 *pu8TxGtk, u8 u8KeyIdx);
-
-/**
- *  @brief              caches the pmkid
- *  @details    valid only in BSS STA mode if External Supplicant
- *                              support is enabled. This Function sets the PMKID in firmware
- *                              when host drivr receives the corresponding request from NDIS.
- *                              The firmware then includes theset PMKID in the appropriate
- *                              management frames
- *  @param[in,out] handle to the wifi driver
- *  @param[in]  message containing PMKID Info in the following format
- *|-----------------------------------------------------------------|
- *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
- *|-----------|------------|----------|-------|----------|----------|
- |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
- ||-----------------------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_set_pmkid_info(struct host_if_drv *hWFIDrv, struct host_if_pmkid_attr *pu8PmkidInfoArray);
-/**
- *  @brief              gets the cached the pmkid info
- *  @details    valid only in BSS STA mode if External Supplicant
- *                              support is enabled. This Function sets the PMKID in firmware
- *                              when host drivr receives the corresponding request from NDIS.
- *                              The firmware then includes theset PMKID in the appropriate
- *                              management frames
- *  @param[in,out] handle to the wifi driver,
- *
- *                                message containing PMKID Info in the following format
- *|-----------------------------------------------------------------|
- *|NumEntries |	BSSID[1] | PMKID[1] |  ...	| BSSID[K] | PMKID[K] |
- *|-----------|------------|----------|-------|----------|----------|
- |	   1	|		6	 |   16		|  ...	|	 6	   |	16	  |
- ||-----------------------------------------------------------------|
- *  @param[in]
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_get_pmkid_info(struct host_if_drv *hWFIDrv, u8 *pu8PmkidInfoArray,
-				    u32 u32PmkidInfoLen);
-
-/**
- *  @brief              sets the pass phrase
- *  @details    AP/STA mode. This function gives the pass phrase used to
- *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
- *                              The length of the field can vary from 8 to 64 bytes,
- *                              the lower layer should get the
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]   String containing PSK
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_set_RSNAConfigPSKPassPhrase(struct host_if_drv *hWFIDrv, u8 *pu8PassPhrase,
-						 u8 u8Psklength);
-/**
- *  @brief              gets the pass phrase
- *  @details    AP/STA mode. This function gets the pass phrase used to
- *                              generate the Pre-Shared Key when WPA/WPA2 is enabled
- *                              The length of the field can vary from 8 to 64 bytes,
- *                              the lower layer should get the
- *  @param[in,out] handle to the wifi driver,
- *                                String containing PSK
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_get_RSNAConfigPSKPassPhrase(struct host_if_drv *hWFIDrv,
-						 u8 *pu8PassPhrase, u8 u8Psklength);
-
-/**
- *  @brief              gets mac address
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		mdaftedar
- *  @date		19 April 2012
- *  @version		1.0
- */
-s32 host_int_get_MacAddress(struct host_if_drv *hWFIDrv, u8 *pu8MacAddress);
-
-/**
- *  @brief              sets mac address
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		mabubakr
- *  @date		16 July 2012
- *  @version		1.0
- */
-s32 host_int_set_MacAddress(struct host_if_drv *hWFIDrv, u8 *pu8MacAddress);
-
-/**
- *  @brief              wait until msg q is empty
- *  @details
- *  @param[in,out]
- *
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		asobhy
- *  @date		19 march 2014
- *  @version		1.0
- */
-int host_int_wait_msg_queue_idle(void);
-
-/**
- *  @brief              sets a start scan request
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	Scan Source one of the following values
- *                              DEFAULT_SCAN        0
- *                              USER_SCAN           BIT0
- *                              OBSS_PERIODIC_SCAN  BIT1
- *                              OBSS_ONETIME_SCAN   BIT2
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_set_start_scan_req(struct host_if_drv *hWFIDrv, u8 scanSource);
-/**
- *  @brief              gets scan source of the last scan
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *                              Scan Source one of the following values
- *                              DEFAULT_SCAN        0
- *                              USER_SCAN           BIT0
- *                              OBSS_PERIODIC_SCAN  BIT1
- *                              OBSS_ONETIME_SCAN   BIT2
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_get_start_scan_req(struct host_if_drv *hWFIDrv, u8 *pu8ScanSource);
-
-/**
- *  @brief              sets a join request
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	Index of the bss descriptor
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_set_join_req(struct host_if_drv *hWFIDrv, u8 *pu8bssid,
-				  const u8 *pu8ssid, size_t ssidLen,
-				  const u8 *pu8IEs, size_t IEsLen,
-				  wilc_connect_result pfConnectResult, void *pvUserArg,
-				  u8 u8security, enum AUTHTYPE tenuAuth_type,
-				  u8 u8channel,
-				  void *pJoinParams);
-
-/**
- *  @brief              Flush a join request parameters to FW, but actual connection
- *  @details    The function is called in situation where WILC is connected to AP and
- *                      required to switch to hybrid FW for P2P connection
- *  @param[in] handle to the wifi driver,
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		Amr Abdel-Moghny
- *  @date		19 DEC 2013
- *  @version		8.0
- */
-
-s32 host_int_flush_join_req(struct host_if_drv *hWFIDrv);
-
-
-/**
- *  @brief              disconnects from the currently associated network
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	Reason Code of the Disconnection
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_disconnect(struct host_if_drv *hWFIDrv, u16 u16ReasonCode);
-
-/**
- *  @brief              disconnects a sta
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	Association Id of the station to be disconnected
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_disconnect_station(struct host_if_drv *hWFIDrv, u8 assoc_id);
-/**
- *  @brief              gets a Association request info
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *                              Message containg assoc. req info in the following format
- * ------------------------------------------------------------------------
- |                        Management Frame Format                    |
- ||-------------------------------------------------------------------|
- ||Frame Control|Duration|DA|SA|BSSID|Sequence Control|Frame Body|FCS |
- ||-------------|--------|--|--|-----|----------------|----------|----|
- | 2           |2       |6 |6 |6    |		2       |0 - 2312  | 4  |
- ||-------------------------------------------------------------------|
- |                                                                   |
- |             Association Request Frame - Frame Body                |
- ||-------------------------------------------------------------------|
- | Capability Information | Listen Interval | SSID | Supported Rates |
- ||------------------------|-----------------|------|-----------------|
- |			2            |		 2         | 2-34 |		3-10        |
- | ---------------------------------------------------------------------
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_get_assoc_req_info(struct host_if_drv *hWFIDrv, u8 *pu8AssocReqInfo,
-					u32 u32AssocReqInfoLen);
-/**
- *  @brief              gets a Association Response info
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *                              Message containg assoc. resp info
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-
-s32 host_int_get_assoc_res_info(struct host_if_drv *hWFIDrv, u8 *pu8AssocRespInfo,
-					u32 u32MaxAssocRespInfoLen, u32 *pu32RcvdAssocRespInfoLen);
-/**
- *  @brief              gets a Association Response info
- *  @details    Valid only in STA mode. This function gives the RSSI
- *                              values observed in all the channels at the time of scanning.
- *                              The length of the field is 1 greater that the total number of
- *                              channels supported. Byte 0 contains the number of channels while
- *                              each of Byte N contains	the observed RSSI value for the channel index N.
- *  @param[in,out] handle to the wifi driver,
- *                              array of scanned channels' RSSI
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_get_rx_power_level(struct host_if_drv *hWFIDrv, u8 *pu8RxPowerLevel,
-					u32 u32RxPowerLevelLen);
-
-/**
- *  @brief              sets a channel
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	Index of the channel to be set
- *|-------------------------------------------------------------------|
- |          CHANNEL1      CHANNEL2 ....		             CHANNEL14	|
- |  Input:         1             2					            14	|
- ||-------------------------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-int host_int_set_mac_chnl_num(struct host_if_drv *wfi_drv, u8 channel);
-
-/**
- *  @brief              gets the current channel index
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *                              current channel index
- *|-----------------------------------------------------------------------|
- |          CHANNEL1      CHANNEL2 ....                     CHANNEL14	|
- |  Input:         1             2                                 14	|
- ||-----------------------------------------------------------------------|
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_get_host_chnl_num(struct host_if_drv *hWFIDrv, u8 *pu8ChNo);
-/**
- *  @brief              gets the sta rssi
- *  @details    gets the currently maintained RSSI value for the station.
- *                              The received signal strength value in dB.
- *                              The range of valid values is -128 to 0.
- *  @param[in,out] handle to the wifi driver,
- *                              rssi value in dB
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_get_rssi(struct host_if_drv *hWFIDrv, s8 *ps8Rssi);
-s32 host_int_get_link_speed(struct host_if_drv *hWFIDrv, s8 *ps8lnkspd);
-/**
- *  @brief              scans a set of channels
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]		Scan source
- *                              Scan Type	PASSIVE_SCAN = 0,
- *                                                      ACTIVE_SCAN  = 1
- *                              Channels Array
- *                              Channels Array length
- *                              Scan Callback function
- *                              User Argument to be delivered back through the Scan Cllback function
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_scan(struct host_if_drv *hWFIDrv, u8 u8ScanSource,
-			  u8 u8ScanType, u8 *pu8ChnlFreqList,
-			  u8 u8ChnlListLen, const u8 *pu8IEs,
-			  size_t IEsLen, wilc_scan_result ScanResult,
-			  void *pvUserArg,
-			  struct hidden_network *pstrHiddenNetwork);
-/**
- *  @brief              sets configuration wids values
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @param[in]	WID, WID value
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 hif_set_cfg(struct host_if_drv *hWFIDrv, struct cfg_param_val *pstrCfgParamVal);
-
-/**
- *  @brief              gets configuration wids values
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *                              WID value
- *  @param[in]	WID,
- *  @return             Error code indicating success/failure
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 hif_get_cfg(struct host_if_drv *hWFIDrv, u16 u16WID, u16 *pu16WID_Value);
-/*****************************************************************************/
-/*							Notification Functions							 */
-/*****************************************************************************/
-/**
- *  @brief              host interface initialization function
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_init(struct net_device *dev, struct host_if_drv **phWFIDrv);
-
-/**
- *  @brief              host interface initialization function
- *  @details
- *  @param[in,out] handle to the wifi driver,
- *  @note
- *  @author		zsalah
- *  @date		8 March 2012
- *  @version		1.0
- */
-s32 host_int_deinit(struct host_if_drv *hWFIDrv);
-
-
-/*!
- *  @fn		s32 host_int_add_beacon(WILC_WFIDrvHandle hWFIDrv,u8 u8Index)
- *  @brief		Sends a beacon to the firmware to be transmitted over the air
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	u32Interval	Beacon Interval. Period between two successive beacons on air
- *  @param[in]	u32DTIMPeriod DTIM Period. Indicates how many Beacon frames
- *              (including the current frame) appear before the next DTIM
- *  @param[in]	u32Headlen	Length of the head buffer in bytes
- *  @param[in]	pu8Head		Pointer to the beacon's head buffer. Beacon's head
- *		is the part from the beacon's start till the TIM element, NOT including the TIM
- *  @param[in]	u32Taillen	Length of the tail buffer in bytes
- *  @param[in]	pu8Tail		Pointer to the beacon's tail buffer. Beacon's tail
- *		starts just after the TIM inormation element
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		10 Julys 2012
- *  @version		1.0 Description
- *
- */
-s32 host_int_add_beacon(struct host_if_drv *hWFIDrv, u32 u32Interval,
-				u32 u32DTIMPeriod,
-				u32 u32HeadLen, u8 *pu8Head,
-				u32 u32TailLen, u8 *pu8tail);
-
-
-/*!
- *  @fn		s32 host_int_del_beacon(WILC_WFIDrvHandle hWFIDrv)
- *  @brief		Removes the beacon and stops trawilctting it over the air
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		10 Julys 2012
- *  @version		1.0 Description
- */
-s32 host_int_del_beacon(struct host_if_drv *hWFIDrv);
-
-/*!
- *  @fn		s32 host_int_add_station(WILC_WFIDrvHandle hWFIDrv,
- *					 struct add_sta_param *pstrStaParams)
- *  @brief		Notifies the firmware with a new associated stations
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	pstrStaParams	Station's parameters
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		12 July 2012
- *  @version		1.0 Description
- */
-s32 host_int_add_station(struct host_if_drv *hWFIDrv,
-			 struct add_sta_param *pstrStaParams);
-
-/*!
- *  @fn		s32 host_int_del_allstation(WILC_WFIDrvHandle hWFIDrv, const u8* pu8MacAddr)
- *  @brief		Deauthenticates clients when group is terminating
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	pu8MacAddr	Station's mac address
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Mai Daftedar
- *  @date		09 April 2014
- *  @version		1.0 Description
- */
-s32 host_int_del_allstation(struct host_if_drv *hWFIDrv, u8 pu8MacAddr[][ETH_ALEN]);
-
-/*!
- *  @fn		s32 host_int_del_station(WILC_WFIDrvHandle hWFIDrv, u8* pu8MacAddr)
- *  @brief		Notifies the firmware with a new deleted station
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	pu8MacAddr	Station's mac address
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		15 July 2012
- *  @version		1.0 Description
- */
-s32 host_int_del_station(struct host_if_drv *hWFIDrv, const u8 *pu8MacAddr);
-
-/*!
- *  @fn		s32 host_int_edit_station(WILC_WFIDrvHandle hWFIDrv,
- *					  struct add_sta_param *pstrStaParams)
- *  @brief		Notifies the firmware with new parameters of an already associated station
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	pstrStaParams	Station's parameters
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		15 July 2012
- *  @version		1.0 Description
- */
-s32 host_int_edit_station(struct host_if_drv *hWFIDrv,
-			  struct add_sta_param *pstrStaParams);
-
-/*!
- *  @fn		s32 host_int_set_power_mgmt(WILC_WFIDrvHandle hWFIDrv, bool bIsEnabled, u32 u32Timeout)
- *  @brief		Set the power management mode to enabled or disabled
- *  @details
- *  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	bIsEnabled	TRUE if enabled, FALSE otherwise
- *  @param[in]	u32Timeout	A timeout value of -1 allows the driver to adjust
- *							the dynamic ps timeout value
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		24 November 2012
- *  @version		1.0 Description
- */
-s32 host_int_set_power_mgmt(struct host_if_drv *hWFIDrv, bool bIsEnabled, u32 u32Timeout);
-/*  @param[in,out]	hWFIDrv		handle to the wifi driver
- *  @param[in]	bIsEnabled	TRUE if enabled, FALSE otherwise
- *  @param[in]	u8count		count of mac address entries in the filter table
- *
- *  @return	0 for Success, error otherwise
- *  @todo
- *  @sa
- *  @author		Adham Abozaeid
- *  @date		24 November 2012
- *  @version		1.0 Description
- */
-s32 host_int_setup_multicast_filter(struct host_if_drv *hWFIDrv, bool bIsEnabled, u32 u32count);
-/**
- *  @brief           host_int_setup_ipaddress
- *  @details       set IP address on firmware
- *  @param[in]
- *  @return         Error code.
- *  @author		Abdelrahman Sobhy
- *  @date
- *  @version	1.0
- */
-s32 host_int_setup_ipaddress(struct host_if_drv *hWFIDrv, u8 *pu8IPAddr, u8 idx);
-
-
-/**
- *  @brief           host_int_delBASession
- *  @details       Delete single Rx BA session
- *  @param[in]
- *  @return         Error code.
- *  @author		Abdelrahman Sobhy
- *  @date
- *  @version	1.0
- */
-s32 host_int_delBASession(struct host_if_drv *hWFIDrv, char *pBSSID, char TID);
-
-/**
- *  @brief           host_int_delBASession
- *  @details       Delete all Rx BA session
- *  @param[in]
- *  @return         Error code.
- *  @author		Abdelrahman Sobhy
- *  @date
- *  @version	1.0
- */
-s32 host_int_del_All_Rx_BASession(struct host_if_drv *hWFIDrv, char *pBSSID, char TID);
-
-
-/**
- *  @brief           host_int_get_ipaddress
- *  @details       get IP address on firmware
- *  @param[in]
- *  @return         Error code.
- *  @author		Abdelrahman Sobhy
- *  @date
- *  @version	1.0
- */
-s32 host_int_get_ipaddress(struct host_if_drv *hWFIDrv, u8 *pu8IPAddr, u8 idx);
-
-/**
- *  @brief           host_int_remain_on_channel
- *  @details
- *  @param[in]
- *  @return         Error code.
- *  @author
- *  @date
- *  @version	1.0
- */
-s32 host_int_remain_on_channel(struct host_if_drv *hWFIDrv, u32 u32SessionID, u32 u32duration, u16 chan, wilc_remain_on_chan_expired RemainOnChanExpired, wilc_remain_on_chan_ready RemainOnChanReady, void *pvUserArg);
-
-/**
- *  @brief              host_int_ListenStateExpired
- *  @details
- *  @param[in]          Handle to wifi driver
- *                              Duration to remain on channel
- *                              Channel to remain on
- *                              Pointer to fn to be called on receive frames in listen state
- *                              Pointer to remain-on-channel expired fn
- *                              Priv
- *  @return             Error code.
- *  @author
- *  @date
- *  @version		1.0
- */
-s32 host_int_ListenStateExpired(struct host_if_drv *hWFIDrv, u32 u32SessionID);
-
-/**
- *  @brief           host_int_frame_register
- *  @details
- *  @param[in]
- *  @return         Error code.
- *  @author
- *  @date
- *  @version	1.0
- */
-s32 host_int_frame_register(struct host_if_drv *hWFIDrv, u16 u16FrameType, bool bReg);
-/**
- *  @brief           host_int_set_wfi_drv_handler
- *  @details
- *  @param[in]
- *  @return         Error code.
- *  @author
- *  @date
- *  @version	1.0
- */
-int host_int_set_wfi_drv_handler(struct host_if_drv *address);
-int host_int_set_operation_mode(struct host_if_drv *wfi_drv, u32 mode);
-
-static s32 Handle_ScanDone(struct host_if_drv *drvHandler, enum scan_event enuEvent);
-
-void host_int_freeJoinParams(void *pJoinParams);
-
-s32 host_int_get_statistics(struct host_if_drv *hWFIDrv, struct rf_info *pstrStatistics);
+struct wilc_vif;
+s32 wilc_remove_key(struct host_if_drv *hWFIDrv, const u8 *pu8StaAddress);
+int wilc_remove_wep_key(struct wilc_vif *vif, u8 index);
+int wilc_set_wep_default_keyid(struct wilc_vif *vif, u8 index);
+int wilc_add_wep_key_bss_sta(struct wilc_vif *vif, const u8 *key, u8 len,
+			     u8 index);
+int wilc_add_wep_key_bss_ap(struct wilc_vif *vif, const u8 *key, u8 len,
+			    u8 index, u8 mode, enum AUTHTYPE auth_type);
+s32 wilc_add_ptk(struct wilc_vif *vif, const u8 *pu8Ptk, u8 u8PtkKeylen,
+		 const u8 *mac_addr, const u8 *pu8RxMic, const u8 *pu8TxMic,
+		 u8 mode, u8 u8Ciphermode, u8 u8Idx);
+s32 wilc_get_inactive_time(struct wilc_vif *vif, const u8 *mac,
+			   u32 *pu32InactiveTime);
+s32 wilc_add_rx_gtk(struct wilc_vif *vif, const u8 *pu8RxGtk, u8 u8GtkKeylen,
+		    u8 u8KeyIdx, u32 u32KeyRSClen, const u8 *KeyRSC,
+		    const u8 *pu8RxMic, const u8 *pu8TxMic, u8 mode,
+		    u8 u8Ciphermode);
+s32 wilc_add_tx_gtk(struct host_if_drv *hWFIDrv, u8 u8KeyLen,
+			u8 *pu8TxGtk, u8 u8KeyIdx);
+s32 wilc_set_pmkid_info(struct wilc_vif *vif,
+			struct host_if_pmkid_attr *pu8PmkidInfoArray);
+s32 wilc_get_mac_address(struct wilc_vif *vif, u8 *pu8MacAddress);
+s32 wilc_set_mac_address(struct wilc_vif *vif, u8 *pu8MacAddress);
+int wilc_wait_msg_queue_idle(void);
+s32 wilc_set_start_scan_req(struct host_if_drv *hWFIDrv, u8 scanSource);
+s32 wilc_set_join_req(struct wilc_vif *vif, u8 *pu8bssid, const u8 *pu8ssid,
+		      size_t ssidLen, const u8 *pu8IEs, size_t IEsLen,
+		      wilc_connect_result pfConnectResult, void *pvUserArg,
+		      u8 u8security, enum AUTHTYPE tenuAuth_type,
+		      u8 u8channel, void *pJoinParams);
+s32 wilc_flush_join_req(struct wilc_vif *vif);
+s32 wilc_disconnect(struct wilc_vif *vif, u16 u16ReasonCode);
+int wilc_set_mac_chnl_num(struct wilc_vif *vif, u8 channel);
+s32 wilc_get_rssi(struct wilc_vif *vif, s8 *ps8Rssi);
+s32 wilc_scan(struct wilc_vif *vif, u8 u8ScanSource, u8 u8ScanType,
+	      u8 *pu8ChnlFreqList, u8 u8ChnlListLen, const u8 *pu8IEs,
+	      size_t IEsLen, wilc_scan_result ScanResult, void *pvUserArg,
+	      struct hidden_network *pstrHiddenNetwork);
+s32 wilc_hif_set_cfg(struct wilc_vif *vif,
+		     struct cfg_param_val *pstrCfgParamVal);
+s32 wilc_init(struct net_device *dev, struct host_if_drv **phWFIDrv);
+s32 wilc_deinit(struct wilc_vif *vif);
+s32 wilc_add_beacon(struct wilc_vif *vif, u32 u32Interval, u32 u32DTIMPeriod,
+		    u32 u32HeadLen, u8 *pu8Head, u32 u32TailLen, u8 *pu8Tail);
+int wilc_del_beacon(struct wilc_vif *vif);
+int wilc_add_station(struct wilc_vif *vif, struct add_sta_param *sta_param);
+s32 wilc_del_allstation(struct wilc_vif *vif, u8 pu8MacAddr[][ETH_ALEN]);
+int wilc_del_station(struct wilc_vif *vif, const u8 *mac_addr);
+s32 wilc_edit_station(struct wilc_vif *vif,
+		      struct add_sta_param *pstrStaParams);
+s32 wilc_set_power_mgmt(struct wilc_vif *vif, bool bIsEnabled, u32 u32Timeout);
+s32 wilc_setup_multicast_filter(struct wilc_vif *vif, bool bIsEnabled,
+				u32 u32count);
+s32 wilc_setup_ipaddress(struct wilc_vif *vif, u8 *u16ipadd, u8 idx);
+s32 wilc_del_all_rx_ba_session(struct wilc_vif *vif, char *pBSSID, char TID);
+s32 wilc_remain_on_channel(struct wilc_vif *vif, u32 u32SessionID,
+			   u32 u32duration, u16 chan,
+			   wilc_remain_on_chan_expired RemainOnChanExpired,
+			   wilc_remain_on_chan_ready RemainOnChanReady,
+			   void *pvUserArg);
+s32 wilc_listen_state_expired(struct wilc_vif *vif, u32 u32SessionID);
+s32 wilc_frame_register(struct wilc_vif *vif, u16 u16FrameType, bool bReg);
+int wilc_set_wfi_drv_handler(struct wilc_vif *vif, int index);
+int wilc_set_operation_mode(struct wilc_vif *vif, u32 mode);
+
+void wilc_free_join_params(void *pJoinParams);
+
+s32 wilc_get_statistics(struct wilc_vif *vif, struct rf_info *pstrStatistics);
+void wilc_resolve_disconnect_aberration(struct wilc_vif *vif);
+int wilc_get_vif_idx(struct wilc_vif *vif);
+
+extern bool wilc_optaining_ip;
+extern u8 wilc_connected_ssid[6];
+extern u8 wilc_multicast_mac_addr_list[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
+
+extern int wilc_connecting;
+extern u8 wilc_initialized;
+extern struct timer_list wilc_during_ip_timer;
 
 #endif
diff --git a/drivers/staging/wilc1000/linux_mon.c b/drivers/staging/wilc1000/linux_mon.c
index 450af1b77f99..e550027645b7 100644
--- a/drivers/staging/wilc1000/linux_mon.c
+++ b/drivers/staging/wilc1000/linux_mon.c
@@ -26,12 +26,9 @@ struct wilc_wfi_radiotap_cb_hdr {
 
 static struct net_device *wilc_wfi_mon; /* global monitor netdev */
 
-extern int  mac_xmit(struct sk_buff *skb, struct net_device *dev);
-
-
-u8 srcAdd[6];
-u8 bssid[6];
-u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+static u8 srcAdd[6];
+static u8 bssid[6];
+static u8 broadcast[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 /**
  *  @brief      WILC_WFI_monitor_rx
  *  @details
@@ -195,7 +192,7 @@ static int mon_mgmt_tx(struct net_device *dev, const u8 *buf, size_t len)
 	mgmt_tx->size = len;
 
 	memcpy(mgmt_tx->buff, buf, len);
-	wilc_wlan_txq_add_mgmt_pkt(mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
+	wilc_wlan_txq_add_mgmt_pkt(dev, mgmt_tx, mgmt_tx->buff, mgmt_tx->size,
 				   mgmt_tx_complete);
 
 	netif_wake_queue(dev);
@@ -298,7 +295,7 @@ static netdev_tx_t WILC_WFI_mon_xmit(struct sk_buff *skb,
 		mon_mgmt_tx(mon_priv->real_ndev, skb->data, skb->len);
 		dev_kfree_skb(skb);
 	} else
-		ret = mac_xmit(skb, mon_priv->real_ndev);
+		ret = wilc_mac_xmit(skb, mon_priv->real_ndev);
 
 	return ret;
 }
diff --git a/drivers/staging/wilc1000/linux_wlan.c b/drivers/staging/wilc1000/linux_wlan.c
index 2a5b36fd8b48..54fe9d74b780 100644
--- a/drivers/staging/wilc1000/linux_wlan.c
+++ b/drivers/staging/wilc1000/linux_wlan.c
@@ -23,61 +23,8 @@
 #include <linux/kernel.h>
 #include <linux/skbuff.h>
 
-#include <linux/version.h>
 #include <linux/semaphore.h>
 
-#ifdef WILC_SDIO
-#include "linux_wlan_sdio.h"
-#else
-#include "linux_wlan_spi.h"
-#endif
-
-#if defined(CUSTOMER_PLATFORM)
-/*
- TODO : Write power control functions as customer platform.
- */
-#else
-
- #define _linux_wlan_device_power_on()		{}
- #define _linux_wlan_device_power_off()		{}
-
- #define _linux_wlan_device_detection()		{}
- #define _linux_wlan_device_removal()		{}
-#endif
-
-extern bool g_obtainingIP;
-extern void resolve_disconnect_aberration(void *drvHandler);
-extern u8 gau8MulticastMacAddrList[WILC_MULTICAST_TABLE_SIZE][ETH_ALEN];
-extern struct timer_list hDuringIpTimer;
-
-static int linux_wlan_device_power(int on_off)
-{
-	PRINT_D(INIT_DBG, "linux_wlan_device_power.. (%d)\n", on_off);
-
-	if (on_off) {
-		_linux_wlan_device_power_on();
-	} else {
-		_linux_wlan_device_power_off();
-	}
-
-	return 0;
-}
-
-static int linux_wlan_device_detection(int on_off)
-{
-	PRINT_D(INIT_DBG, "linux_wlan_device_detection.. (%d)\n", on_off);
-
-#ifdef WILC_SDIO
-	if (on_off) {
-		_linux_wlan_device_detection();
-	} else {
-		_linux_wlan_device_removal();
-	}
-#endif
-
-	return 0;
-}
-
 static int dev_state_ev_handler(struct notifier_block *this, unsigned long event, void *ptr);
 
 static struct notifier_block g_dev_notifier = {
@@ -86,40 +33,24 @@ static struct notifier_block g_dev_notifier = {
 
 #define IRQ_WAIT	1
 #define IRQ_NO_WAIT	0
-/*
- *      to sync between mac_close and module exit.
- *      don't initialize or de-initialize from init/deinitlocks
- *      to be initialized from module wilc_netdev_init and
- *      deinitialized from mdoule_exit
- */
 static struct semaphore close_exit_sync;
 
 static int wlan_deinit_locks(struct net_device *dev);
 static void wlan_deinitialize_threads(struct net_device *dev);
-extern void WILC_WFI_monitor_rx(u8 *buff, u32 size);
-extern void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size);
 
 static void linux_wlan_tx_complete(void *priv, int status);
 static int  mac_init_fn(struct net_device *ndev);
-int  mac_xmit(struct sk_buff *skb, struct net_device *dev);
-int  mac_open(struct net_device *ndev);
-int  mac_close(struct net_device *ndev);
 static struct net_device_stats *mac_stats(struct net_device *dev);
 static int  mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd);
 static void wilc_set_multicast_list(struct net_device *dev);
 
-/*
- * for now - in frmw_to_linux there should be private data to be passed to it
- * and this data should be pointer to net device
- */
-struct wilc *g_linux_wlan;
-bool bEnablePS = true;
+bool wilc_enable_ps = true;
 
 static const struct net_device_ops wilc_netdev_ops = {
 	.ndo_init = mac_init_fn,
-	.ndo_open = mac_open,
-	.ndo_stop = mac_close,
-	.ndo_start_xmit = mac_xmit,
+	.ndo_open = wilc_mac_open,
+	.ndo_stop = wilc_mac_close,
+	.ndo_start_xmit = wilc_mac_xmit,
 	.ndo_do_ioctl = mac_ioctl,
 	.ndo_get_stats = mac_stats,
 	.ndo_set_rx_mode  = wilc_set_multicast_list,
@@ -130,130 +61,129 @@ static int dev_state_ev_handler(struct notifier_block *this, unsigned long event
 {
 	struct in_ifaddr *dev_iface = (struct in_ifaddr *)ptr;
 	struct wilc_priv *priv;
-	struct host_if_drv *pstrWFIDrv;
+	struct host_if_drv *hif_drv;
 	struct net_device *dev;
-	u8 *pIP_Add_buff;
-	perInterface_wlan_t *nic;
+	u8 *ip_addr_buf;
+	struct wilc_vif *vif;
 	u8 null_ip[4] = {0};
 	char wlan_dev_name[5] = "wlan0";
 
-	if (dev_iface == NULL || dev_iface->ifa_dev == NULL || dev_iface->ifa_dev->dev == NULL)	{
+	if (!dev_iface || !dev_iface->ifa_dev || !dev_iface->ifa_dev->dev) {
 		PRINT_D(GENERIC_DBG, "dev_iface = NULL\n");
 		return NOTIFY_DONE;
 	}
 
-	if ((memcmp(dev_iface->ifa_label, "wlan0", 5)) && (memcmp(dev_iface->ifa_label, "p2p0", 4))) {
+	if (memcmp(dev_iface->ifa_label, "wlan0", 5) &&
+	    memcmp(dev_iface->ifa_label, "p2p0", 4)) {
 		PRINT_D(GENERIC_DBG, "Interface is neither WLAN0 nor P2P0\n");
 		return NOTIFY_DONE;
 	}
 
 	dev  = (struct net_device *)dev_iface->ifa_dev->dev;
-	if (dev->ieee80211_ptr == NULL || dev->ieee80211_ptr->wiphy == NULL) {
+	if (!dev->ieee80211_ptr || !dev->ieee80211_ptr->wiphy) {
 		PRINT_D(GENERIC_DBG, "No Wireless registerd\n");
 		return NOTIFY_DONE;
 	}
 	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
-	if (priv == NULL) {
+	if (!priv) {
 		PRINT_D(GENERIC_DBG, "No Wireless Priv\n");
 		return NOTIFY_DONE;
 	}
-	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
-	nic = netdev_priv(dev);
-	if (nic == NULL || pstrWFIDrv == NULL) {
+	hif_drv = (struct host_if_drv *)priv->hWILCWFIDrv;
+	vif = netdev_priv(dev);
+	if (!vif || !hif_drv) {
 		PRINT_D(GENERIC_DBG, "No Wireless Priv\n");
 		return NOTIFY_DONE;
 	}
 
-	PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler +++\n"); /* tony */
+	PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler +++\n");
 
 	switch (event) {
 	case NETDEV_UP:
-		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_UP %p\n", dev);       /* tony */
+		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_UP %p\n", dev);
 
 		PRINT_INFO(GENERIC_DBG, "\n ============== IP Address Obtained ===============\n\n");
 
-		/*If we are in station mode or client mode*/
-		if (nic->iftype == STATION_MODE || nic->iftype == CLIENT_MODE) {
-			pstrWFIDrv->IFC_UP = 1;
-			g_obtainingIP = false;
-			del_timer(&hDuringIpTimer);
+		if (vif->iftype == STATION_MODE || vif->iftype == CLIENT_MODE) {
+			hif_drv->IFC_UP = 1;
+			wilc_optaining_ip = false;
+			del_timer(&wilc_during_ip_timer);
 			PRINT_D(GENERIC_DBG, "IP obtained , enable scan\n");
 		}
 
-		if (bEnablePS)
-			host_int_set_power_mgmt(pstrWFIDrv, 1, 0);
+		if (wilc_enable_ps)
+			wilc_set_power_mgmt(vif, 1, 0);
 
 		PRINT_D(GENERIC_DBG, "[%s] Up IP\n", dev_iface->ifa_label);
 
-		pIP_Add_buff = (char *) (&(dev_iface->ifa_address));
-		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d\n", pIP_Add_buff[0], pIP_Add_buff[1], pIP_Add_buff[2], pIP_Add_buff[3]);
-		host_int_setup_ipaddress(pstrWFIDrv, pIP_Add_buff, nic->u8IfIdx);
+		ip_addr_buf = (char *)&dev_iface->ifa_address;
+		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d\n",
+			ip_addr_buf[0], ip_addr_buf[1],
+			ip_addr_buf[2], ip_addr_buf[3]);
+		wilc_setup_ipaddress(vif, ip_addr_buf, vif->u8IfIdx);
 
 		break;
 
 	case NETDEV_DOWN:
-		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_DOWN %p\n", dev);               /* tony */
+		PRINT_D(GENERIC_DBG, "dev_state_ev_handler event=NETDEV_DOWN %p\n", dev);
 
 		PRINT_INFO(GENERIC_DBG, "\n ============== IP Address Released ===============\n\n");
-		if (nic->iftype == STATION_MODE || nic->iftype == CLIENT_MODE) {
-			pstrWFIDrv->IFC_UP = 0;
-			g_obtainingIP = false;
+		if (vif->iftype == STATION_MODE || vif->iftype == CLIENT_MODE) {
+			hif_drv->IFC_UP = 0;
+			wilc_optaining_ip = false;
 		}
 
 		if (memcmp(dev_iface->ifa_label, wlan_dev_name, 5) == 0)
-			host_int_set_power_mgmt(pstrWFIDrv, 0, 0);
+			wilc_set_power_mgmt(vif, 0, 0);
 
-		resolve_disconnect_aberration(pstrWFIDrv);
+		wilc_resolve_disconnect_aberration(vif);
 
 		PRINT_D(GENERIC_DBG, "[%s] Down IP\n", dev_iface->ifa_label);
 
-		pIP_Add_buff = null_ip;
-		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d\n", pIP_Add_buff[0], pIP_Add_buff[1], pIP_Add_buff[2], pIP_Add_buff[3]);
+		ip_addr_buf = null_ip;
+		PRINT_D(GENERIC_DBG, "IP add=%d:%d:%d:%d\n",
+			ip_addr_buf[0], ip_addr_buf[1],
+			ip_addr_buf[2], ip_addr_buf[3]);
 
-		host_int_setup_ipaddress(pstrWFIDrv, pIP_Add_buff, nic->u8IfIdx);
+		wilc_setup_ipaddress(vif, ip_addr_buf, vif->u8IfIdx);
 
 		break;
 
 	default:
-		PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler event=default\n");        /* tony */
+		PRINT_INFO(GENERIC_DBG, "dev_state_ev_handler event=default\n");
 		PRINT_INFO(GENERIC_DBG, "[%s] unknown dev event: %lu\n", dev_iface->ifa_label, event);
 
 		break;
 	}
 
 	return NOTIFY_DONE;
-
 }
 
-#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
 static irqreturn_t isr_uh_routine(int irq, void *user_data)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 	struct net_device *dev = (struct net_device *)user_data;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 	PRINT_D(INT_DBG, "Interrupt received UH\n");
 
-	/*While mac is closing cacncel the handling of any interrupts received*/
 	if (wilc->close) {
 		PRINT_ER("Driver is CLOSING: Can't handle UH interrupt\n");
 		return IRQ_HANDLED;
 	}
 	return IRQ_WAKE_THREAD;
 }
-#endif
 
-irqreturn_t isr_bh_routine(int irq, void *userdata)
+static irqreturn_t isr_bh_routine(int irq, void *userdata)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(userdata);
-	wilc = nic->wilc;
+	vif = netdev_priv(userdata);
+	wilc = vif->wilc;
 
-	/*While mac is closing cacncel the handling of any interrupts received*/
 	if (wilc->close) {
 		PRINT_ER("Driver is CLOSING: Can't handle BH interrupt\n");
 		return IRQ_HANDLED;
@@ -265,154 +195,131 @@ irqreturn_t isr_bh_routine(int irq, void *userdata)
 	return IRQ_HANDLED;
 }
 
-#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
 static int init_irq(struct net_device *dev)
 {
 	int ret = 0;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
 
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
-
-	/*initialize GPIO and register IRQ num*/
-	/*GPIO request*/
-	if ((gpio_request(GPIO_NUM, "WILC_INTR") == 0) &&
-	    (gpio_direction_input(GPIO_NUM) == 0)) {
-#if defined(CUSTOMER_PLATFORM)
-/*
- TODO : save the registerd irq number to the private wilc context in kernel.
- *
- * ex) nic->dev_irq_num = gpio_to_irq(GPIO_NUM);
- */
-#else
-		wl->dev_irq_num = gpio_to_irq(GPIO_NUM);
-#endif
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
+
+	if ((gpio_request(wl->gpio, "WILC_INTR") == 0) &&
+	    (gpio_direction_input(wl->gpio) == 0)) {
+		wl->dev_irq_num = gpio_to_irq(wl->gpio);
 	} else {
 		ret = -1;
 		PRINT_ER("could not obtain gpio for WILC_INTR\n");
 	}
 
-	if ((ret != -1) && (request_threaded_irq(wl->dev_irq_num, isr_uh_routine, isr_bh_routine,
-						  IRQF_TRIGGER_LOW | IRQF_ONESHOT,               /*Without IRQF_ONESHOT the uh will remain kicked in and dont gave a chance to bh*/
-						  "WILC_IRQ", dev)) < 0) {
-
-		PRINT_ER("Failed to request IRQ for GPIO: %d\n", GPIO_NUM);
+	if (ret != -1 && request_threaded_irq(wl->dev_irq_num,
+					      isr_uh_routine,
+					      isr_bh_routine,
+					      IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+					      "WILC_IRQ", dev) < 0) {
+		PRINT_ER("Failed to request IRQ for GPIO: %d\n", wl->gpio);
+		gpio_free(wl->gpio);
 		ret = -1;
 	} else {
-
 		PRINT_D(INIT_DBG, "IRQ request succeeded IRQ-NUM= %d on GPIO: %d\n",
-			wl->dev_irq_num, GPIO_NUM);
+			wl->dev_irq_num, wl->gpio);
 	}
 
 	return ret;
 }
-#endif
 
 static void deinit_irq(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-#if (defined WILC_SPI) || (defined WILC_SDIO_IRQ_GPIO)
 	/* Deintialize IRQ */
-	if (&wilc->dev_irq_num != 0) {
+	if (wilc->dev_irq_num) {
 		free_irq(wilc->dev_irq_num, wilc);
-
-		gpio_free(GPIO_NUM);
+		gpio_free(wilc->gpio);
 	}
-#endif
 }
 
-/*
- *      OS functions
- */
-void linux_wlan_dbg(u8 *buff)
+void wilc_dbg(u8 *buff)
 {
 	PRINT_D(INIT_DBG, "%d\n", *buff);
 }
 
-int linux_wlan_lock_timeout(void *vp, u32 timeout)
+int wilc_lock_timeout(struct wilc *nic, void *vp, u32 timeout)
 {
+	/* FIXME: replace with mutex_lock or wait_for_completion */
 	int error = -1;
 
 	PRINT_D(LOCK_DBG, "Locking %p\n", vp);
-	if (vp != NULL)
-		error = down_timeout((struct semaphore *)vp, msecs_to_jiffies(timeout));
+	if (vp)
+		error = down_timeout((struct semaphore *)vp,
+				     msecs_to_jiffies(timeout));
 	else
 		PRINT_ER("Failed, mutex is NULL\n");
 	return error;
 }
 
-void linux_wlan_mac_indicate(struct wilc *wilc, int flag)
+void wilc_mac_indicate(struct wilc *wilc, int flag)
 {
-	/*I have to do it that way becuase there is no mean to encapsulate device pointer
-	 * as a parameter
-	 */
 	int status;
 
 	if (flag == WILC_MAC_INDICATE_STATUS) {
-		wilc_wlan_cfg_get_val(WID_STATUS, (unsigned char *)&status, 4);
+		wilc_wlan_cfg_get_val(WID_STATUS,
+				      (unsigned char *)&status, 4);
 		if (wilc->mac_status == WILC_MAC_STATUS_INIT) {
 			wilc->mac_status = status;
 			up(&wilc->sync_event);
 		} else {
 			wilc->mac_status = status;
 		}
-
-		if (wilc->mac_status == WILC_MAC_STATUS_CONNECT) {        /* Connect */
-		}
-
 	} else if (flag == WILC_MAC_INDICATE_SCAN) {
 		PRINT_D(GENERIC_DBG, "Scanning ...\n");
-
 	}
-
 }
 
-struct net_device *GetIfHandler(struct wilc *wilc, u8 *pMacHeader)
+static struct net_device *get_if_handler(struct wilc *wilc, u8 *mac_header)
 {
-	u8 *Bssid, *Bssid1;
+	u8 *bssid, *bssid1;
 	int i = 0;
 
-	Bssid  = pMacHeader + 10;
-	Bssid1 = pMacHeader + 4;
+	bssid = mac_header + 10;
+	bssid1 = mac_header + 4;
 
 	for (i = 0; i < wilc->vif_num; i++)
-		if (!memcmp(Bssid1, wilc->vif[i].bssid, ETH_ALEN) ||
-		    !memcmp(Bssid, wilc->vif[i].bssid, ETH_ALEN))
-			return wilc->vif[i].ndev;
+		if (!memcmp(bssid1, wilc->vif[i]->bssid, ETH_ALEN) ||
+		    !memcmp(bssid, wilc->vif[i]->bssid, ETH_ALEN))
+			return wilc->vif[i]->ndev;
 
 	PRINT_INFO(INIT_DBG, "Invalide handle\n");
 	for (i = 0; i < 25; i++)
-		PRINT_D(INIT_DBG, "%02x ", pMacHeader[i]);
-	Bssid  = pMacHeader + 18;
-	Bssid1 = pMacHeader + 12;
+		PRINT_D(INIT_DBG, "%02x ", mac_header[i]);
+	bssid = mac_header + 18;
+	bssid1 = mac_header + 12;
 	for (i = 0; i < wilc->vif_num; i++)
-		if (!memcmp(Bssid1, wilc->vif[i].bssid, ETH_ALEN) ||
-		    !memcmp(Bssid, wilc->vif[i].bssid, ETH_ALEN))
-			return wilc->vif[i].ndev;
+		if (!memcmp(bssid1, wilc->vif[i]->bssid, ETH_ALEN) ||
+		    !memcmp(bssid, wilc->vif[i]->bssid, ETH_ALEN))
+			return wilc->vif[i]->ndev;
 
 	PRINT_INFO(INIT_DBG, "\n");
 	return NULL;
 }
 
-int linux_wlan_set_bssid(struct net_device *wilc_netdev, u8 *pBSSID)
+int wilc_wlan_set_bssid(struct net_device *wilc_netdev, u8 *bssid)
 {
 	int i = 0;
 	int ret = -1;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(wilc_netdev);
-	wilc = nic->wilc;
+	vif = netdev_priv(wilc_netdev);
+	wilc = vif->wilc;
 
 	for (i = 0; i < wilc->vif_num; i++)
-		if (wilc->vif[i].ndev == wilc_netdev) {
-			memcpy(wilc->vif[i].bssid, pBSSID, 6);
+		if (wilc->vif[i]->ndev == wilc_netdev) {
+			memcpy(wilc->vif[i]->bssid, bssid, 6);
 			ret = 0;
 			break;
 		}
@@ -420,15 +327,14 @@ int linux_wlan_set_bssid(struct net_device *wilc_netdev, u8 *pBSSID)
 	return ret;
 }
 
-/*Function to get number of connected interfaces*/
-int linux_wlan_get_num_conn_ifcs(void)
+int wilc_wlan_get_num_conn_ifcs(struct wilc *wilc)
 {
 	u8 i = 0;
 	u8 null_bssid[6] = {0};
 	u8 ret_val = 0;
 
-	for (i = 0; i < g_linux_wlan->vif_num; i++)
-		if (memcmp(g_linux_wlan->vif[i].bssid, null_bssid, 6))
+	for (i = 0; i < wilc->vif_num; i++)
+		if (memcmp(wilc->vif[i]->bssid, null_bssid, 6))
 			ret_val++;
 
 	return ret_val;
@@ -439,7 +345,7 @@ int linux_wlan_get_num_conn_ifcs(void)
 static int linux_wlan_txq_task(void *vp)
 {
 	int ret, txq_count;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
 	struct net_device *dev = vp;
 #if defined USE_TX_BACKOFF_DELAY_IF_NO_BUFFERS
@@ -451,20 +357,16 @@ static int linux_wlan_txq_task(void *vp)
 	int backoff_weight = TX_BACKOFF_WEIGHT_MIN;
 #endif
 
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
 
-	/* inform wilc1000_wlan_init that TXQ task is started. */
 	up(&wl->txq_thread_started);
 	while (1) {
-
 		PRINT_D(TX_DBG, "txq_task Taking a nap :)\n");
 		down(&wl->txq_event);
-		/* wait_for_completion(&pd->txq_event); */
 		PRINT_D(TX_DBG, "txq_task Who waked me up :$\n");
 
 		if (wl->close) {
-			/*Unlock the mutex in the mac_close function to indicate the exiting of the TX thread */
 			up(&wl->txq_thread_started);
 
 			while (!kthread_should_stop())
@@ -479,23 +381,19 @@ static int linux_wlan_txq_task(void *vp)
 #else
 		do {
 			ret = wilc_wlan_handle_txq(dev, &txq_count);
-			if (txq_count < FLOW_CONTROL_LOWER_THRESHOLD /* && netif_queue_stopped(pd->wilc_netdev)*/) {
+			if (txq_count < FLOW_CONTROL_LOWER_THRESHOLD) {
 				PRINT_D(TX_DBG, "Waking up queue\n");
-				/* netif_wake_queue(pd->wilc_netdev); */
-				if (netif_queue_stopped(wl->vif[0].ndev))
-					netif_wake_queue(wl->vif[0].ndev);
-				if (netif_queue_stopped(wl->vif[1].ndev))
-					netif_wake_queue(wl->vif[1].ndev);
+
+				if (netif_queue_stopped(wl->vif[0]->ndev))
+					netif_wake_queue(wl->vif[0]->ndev);
+				if (netif_queue_stopped(wl->vif[1]->ndev))
+					netif_wake_queue(wl->vif[1]->ndev);
 			}
 
-			if (ret == WILC_TX_ERR_NO_BUF) { /* failed to allocate buffers in chip. */
+			if (ret == WILC_TX_ERR_NO_BUF) {
 				do {
-					/* Back off from sending packets for some time. */
-					/* schedule_timeout will allow RX task to run and free buffers.*/
-					/* set_current_state(TASK_UNINTERRUPTIBLE); */
-					/* timeout = schedule_timeout(timeout); */
 					msleep(TX_BACKOFF_WEIGHT_UNIT_MS << backoff_weight);
-				} while (/*timeout*/ 0);
+				} while (0);
 				backoff_weight += TX_BACKOFF_WEIGHT_INCR_STEP;
 				if (backoff_weight > TX_BACKOFF_WEIGHT_MAX)
 					backoff_weight = TX_BACKOFF_WEIGHT_MAX;
@@ -506,376 +404,326 @@ static int linux_wlan_txq_task(void *vp)
 						backoff_weight = TX_BACKOFF_WEIGHT_MIN;
 				}
 			}
-			/*TODO: drop packets after a certain time/number of retry count. */
-		} while (ret == WILC_TX_ERR_NO_BUF && !wl->close); /* retry sending packets if no more buffers in chip. */
+		} while (ret == WILC_TX_ERR_NO_BUF && !wl->close);
 #endif
 	}
 	return 0;
 }
 
-void linux_wlan_rx_complete(void)
+void wilc_rx_complete(struct wilc *nic)
 {
 	PRINT_D(RX_DBG, "RX completed\n");
 }
 
-int linux_wlan_get_firmware(perInterface_wlan_t *p_nic)
+int wilc_wlan_get_firmware(struct net_device *dev)
 {
-
-	perInterface_wlan_t *nic = p_nic;
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 	int ret = 0;
 	const struct firmware *wilc_firmware;
 	char *firmware;
 
-	if (nic->iftype == AP_MODE)
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
+
+	if (vif->iftype == AP_MODE) {
 		firmware = AP_FIRMWARE;
-	else if (nic->iftype == STATION_MODE)
+	} else if (vif->iftype == STATION_MODE) {
 		firmware = STA_FIRMWARE;
-
-	else {
+	} else {
 		PRINT_D(INIT_DBG, "Get P2P_CONCURRENCY_FIRMWARE\n");
 		firmware = P2P_CONCURRENCY_FIRMWARE;
 	}
 
-	if (nic == NULL) {
-		PRINT_ER("NIC is NULL\n");
+	if (!vif) {
+		PRINT_ER("vif is NULL\n");
 		goto _fail_;
 	}
 
-	if (&nic->wilc_netdev->dev == NULL) {
-		PRINT_ER("&nic->wilc_netdev->dev  is NULL\n");
+	if (!(&vif->ndev->dev)) {
+		PRINT_ER("&vif->ndev->dev  is NULL\n");
 		goto _fail_;
 	}
 
-	/*	the firmare should be located in /lib/firmware in
-	 *      root file system with the name specified above */
-
-#ifdef WILC_SDIO
-	if (request_firmware(&wilc_firmware, firmware, &g_linux_wlan->wilc_sdio_func->dev) != 0) {
+	if (request_firmware(&wilc_firmware, firmware, wilc->dev) != 0) {
 		PRINT_ER("%s - firmare not available\n", firmware);
 		ret = -1;
 		goto _fail_;
 	}
-#else
-	if (request_firmware(&wilc_firmware, firmware, &g_linux_wlan->wilc_spidev->dev) != 0) {
-		PRINT_ER("%s - firmare not available\n", firmware);
-		ret = -1;
-		goto _fail_;
-	}
-#endif
-	g_linux_wlan->firmware = wilc_firmware;
+	wilc->firmware = wilc_firmware;
 
 _fail_:
 
 	return ret;
-
 }
 
-static int linux_wlan_start_firmware(perInterface_wlan_t *nic)
+static int linux_wlan_start_firmware(struct net_device *dev)
 {
-
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 	int ret = 0;
-	/* start firmware */
+
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
+
 	PRINT_D(INIT_DBG, "Starting Firmware ...\n");
-	ret = wilc_wlan_start();
+	ret = wilc_wlan_start(wilc);
 	if (ret < 0) {
 		PRINT_ER("Failed to start Firmware\n");
-		goto _fail_;
+		return ret;
 	}
 
-	/* wait for mac ready */
 	PRINT_D(INIT_DBG, "Waiting for Firmware to get ready ...\n");
-	ret = linux_wlan_lock_timeout(&g_linux_wlan->sync_event, 5000);
+	ret = wilc_lock_timeout(wilc, &wilc->sync_event, 5000);
 	if (ret) {
 		PRINT_D(INIT_DBG, "Firmware start timed out");
-		goto _fail_;
+		return ret;
 	}
-	/*
-	 *      TODO: Driver shouoldn't wait forever for firmware to get started -
-	 *      in case of timeout this should be handled properly
-	 */
 	PRINT_D(INIT_DBG, "Firmware successfully started\n");
 
-_fail_:
-	return ret;
+	return 0;
 }
-static int linux_wlan_firmware_download(struct wilc *p_nic)
-{
 
+static int wilc1000_firmware_download(struct net_device *dev)
+{
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 	int ret = 0;
 
-	if (!g_linux_wlan->firmware) {
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
+
+	if (!wilc->firmware) {
 		PRINT_ER("Firmware buffer is NULL\n");
-		ret = -ENOBUFS;
-		goto _FAIL_;
+		return -ENOBUFS;
 	}
-	/**
-	 *      do the firmware download
-	 **/
 	PRINT_D(INIT_DBG, "Downloading Firmware ...\n");
-	ret = wilc_wlan_firmware_download(g_linux_wlan->firmware->data,
-					  g_linux_wlan->firmware->size);
+	ret = wilc_wlan_firmware_download(wilc, wilc->firmware->data,
+					  wilc->firmware->size);
 	if (ret < 0)
-		goto _FAIL_;
+		return ret;
 
-	/* Freeing FW buffer */
 	PRINT_D(INIT_DBG, "Freeing FW buffer ...\n");
 	PRINT_D(INIT_DBG, "Releasing firmware\n");
-	release_firmware(g_linux_wlan->firmware);
+	release_firmware(wilc->firmware);
+	wilc->firmware = NULL;
 
 	PRINT_D(INIT_DBG, "Download Succeeded\n");
 
-_FAIL_:
-	return ret;
+	return 0;
 }
 
-/* startup configuration - could be changed later using iconfig*/
-static int linux_wlan_init_test_config(struct net_device *dev, struct wilc *p_nic)
+static int linux_wlan_init_test_config(struct net_device *dev,
+				       struct wilc *wilc)
 {
-
 	unsigned char c_val[64];
 	unsigned char mac_add[] = {0x00, 0x80, 0xC2, 0x5E, 0xa2, 0xff};
 
 	struct wilc_priv *priv;
-	struct host_if_drv *pstrWFIDrv;
+	struct host_if_drv *hif_drv;
 
 	PRINT_D(TX_DBG, "Start configuring Firmware\n");
 	get_random_bytes(&mac_add[5], 1);
 	get_random_bytes(&mac_add[4], 1);
 	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
-	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
-	PRINT_D(INIT_DBG, "Host = %p\n", pstrWFIDrv);
+	hif_drv = (struct host_if_drv *)priv->hWILCWFIDrv;
+	PRINT_D(INIT_DBG, "Host = %p\n", hif_drv);
 
-	PRINT_D(INIT_DBG, "MAC address is : %02x-%02x-%02x-%02x-%02x-%02x\n", mac_add[0], mac_add[1], mac_add[2], mac_add[3], mac_add[4], mac_add[5]);
-	wilc_get_chipid(0);
+	PRINT_D(INIT_DBG, "MAC address is : %02x-%02x-%02x-%02x-%02x-%02x\n",
+		mac_add[0], mac_add[1], mac_add[2],
+		mac_add[3], mac_add[4], mac_add[5]);
+	wilc_get_chipid(wilc, 0);
 
 	*(int *)c_val = 1;
 
-	if (!wilc_wlan_cfg_set(1, WID_SET_DRV_HANDLER, c_val, 4, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 1, WID_SET_DRV_HANDLER, c_val, 4, 0, 0))
 		goto _fail_;
 
-	/*to tell fw that we are going to use PC test - WILC specific*/
 	c_val[0] = 0;
-	if (!wilc_wlan_cfg_set(0, WID_PC_TEST_MODE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_PC_TEST_MODE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = INFRASTRUCTURE;
-	if (!wilc_wlan_cfg_set(0, WID_BSS_TYPE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_BSS_TYPE, c_val, 1, 0, 0))
 		goto _fail_;
 
-	/* c_val[0] = RATE_AUTO; */
 	c_val[0] = RATE_AUTO;
-	if (!wilc_wlan_cfg_set(0, WID_CURRENT_TX_RATE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_CURRENT_TX_RATE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = G_MIXED_11B_2_MODE;
-	if (!wilc_wlan_cfg_set(0, WID_11G_OPERATING_MODE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11G_OPERATING_MODE, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
 	c_val[0] = 1;
-	if (!wilc_wlan_cfg_set(0, WID_CURRENT_CHANNEL, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_CURRENT_CHANNEL, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = G_SHORT_PREAMBLE;
-	if (!wilc_wlan_cfg_set(0, WID_PREAMBLE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_PREAMBLE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = AUTO_PROT;
-	if (!wilc_wlan_cfg_set(0, WID_11N_PROT_MECH, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_PROT_MECH, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = ACTIVE_SCAN;
-	if (!wilc_wlan_cfg_set(0, WID_SCAN_TYPE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_SCAN_TYPE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = SITE_SURVEY_OFF;
-	if (!wilc_wlan_cfg_set(0, WID_SITE_SURVEY, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_SITE_SURVEY, c_val, 1, 0, 0))
 		goto _fail_;
 
-	*((int *)c_val) = 0xffff; /* Never use RTS-CTS */
-	if (!wilc_wlan_cfg_set(0, WID_RTS_THRESHOLD, c_val, 2, 0, 0))
+	*((int *)c_val) = 0xffff;
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_RTS_THRESHOLD, c_val, 2, 0, 0))
 		goto _fail_;
 
 	*((int *)c_val) = 2346;
-	if (!wilc_wlan_cfg_set(0, WID_FRAG_THRESHOLD, c_val, 2, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_FRAG_THRESHOLD, c_val, 2, 0, 0))
 		goto _fail_;
 
-	/*  SSID                                                                 */
-	/*  --------------------------------------------------------------       */
-	/*  Configuration :   String with length less than 32 bytes              */
-	/*  Values to set :   Any string with length less than 32 bytes          */
-	/*                    ( In BSS Station Set SSID to "" (null string)      */
-	/*                      to enable Broadcast SSID suppport )              */
-	/*  --------------------------------------------------------------       */
 	c_val[0] = 0;
-	if (!wilc_wlan_cfg_set(0, WID_BCAST_SSID, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_BCAST_SSID, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 1;
-	if (!wilc_wlan_cfg_set(0, WID_QOS_ENABLE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_QOS_ENABLE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = NO_POWERSAVE;
-	if (!wilc_wlan_cfg_set(0, WID_POWER_MANAGEMENT, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_POWER_MANAGEMENT, c_val, 1, 0, 0))
 		goto _fail_;
 
-	c_val[0] = NO_ENCRYPT; /* NO_ENCRYPT, 0x79 */
-	if (!wilc_wlan_cfg_set(0, WID_11I_MODE, c_val, 1, 0, 0))
+	c_val[0] = NO_SECURITY; /* NO_ENCRYPT, 0x79 */
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11I_MODE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = OPEN_SYSTEM;
-	if (!wilc_wlan_cfg_set(0, WID_AUTH_TYPE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_AUTH_TYPE, c_val, 1, 0, 0))
 		goto _fail_;
 
-	/*  WEP/802 11I Configuration                                            */
-	/*  ------------------------------------------------------------------   */
-	/*  Configuration : WEP Key                                              */
-	/*  Values (0x)   : 5 byte for WEP40 and 13 bytes for WEP104             */
-	/*                  In case more than 5 bytes are passed on for WEP 40   */
-	/*                  only first 5 bytes will be used as the key           */
-	/*  ------------------------------------------------------------------   */
-
 	strcpy(c_val, "123456790abcdef1234567890");
-	if (!wilc_wlan_cfg_set(0, WID_WEP_KEY_VALUE, c_val, (strlen(c_val) + 1), 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_WEP_KEY_VALUE, c_val,
+			       (strlen(c_val) + 1), 0, 0))
 		goto _fail_;
 
-	/*  WEP/802 11I Configuration                                            */
-	/*  ------------------------------------------------------------------   */
-	/*  Configuration : AES/TKIP WPA/RSNA Pre-Shared Key                     */
-	/*  Values to set : Any string with length greater than equal to 8 bytes */
-	/*                  and less than 64 bytes                               */
-	/*  ------------------------------------------------------------------   */
 	strcpy(c_val, "12345678");
-	if (!wilc_wlan_cfg_set(0, WID_11I_PSK, c_val, (strlen(c_val)), 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11I_PSK, c_val, (strlen(c_val)), 0,
+			       0))
 		goto _fail_;
 
-	/*  IEEE802.1X Key Configuration                                         */
-	/*  ------------------------------------------------------------------   */
-	/*  Configuration : Radius Server Access Secret Key                      */
-	/*  Values to set : Any string with length greater than equal to 8 bytes */
-	/*                  and less than 65 bytes                               */
-	/*  ------------------------------------------------------------------   */
 	strcpy(c_val, "password");
-	if (!wilc_wlan_cfg_set(0, WID_1X_KEY, c_val, (strlen(c_val) + 1), 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_1X_KEY, c_val, (strlen(c_val) + 1),
+			       0, 0))
 		goto _fail_;
 
-	/*   IEEE802.1X Server Address Configuration                             */
-	/*  ------------------------------------------------------------------   */
-	/*  Configuration : Radius Server IP Address                             */
-	/*  Values to set : Any valid IP Address                                 */
-	/*  ------------------------------------------------------------------   */
 	c_val[0] = 192;
 	c_val[1] = 168;
 	c_val[2] = 1;
 	c_val[3] = 112;
-	if (!wilc_wlan_cfg_set(0, WID_1X_SERV_ADDR, c_val, 4, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_1X_SERV_ADDR, c_val, 4, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 3;
-	if (!wilc_wlan_cfg_set(0, WID_LISTEN_INTERVAL, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_LISTEN_INTERVAL, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 3;
-	if (!wilc_wlan_cfg_set(0, WID_DTIM_PERIOD, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_DTIM_PERIOD, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = NORMAL_ACK;
-	if (!wilc_wlan_cfg_set(0, WID_ACK_POLICY, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_ACK_POLICY, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 0;
-	if (!wilc_wlan_cfg_set(0, WID_USER_CONTROL_ON_TX_POWER, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_USER_CONTROL_ON_TX_POWER, c_val, 1,
+			       0, 0))
 		goto _fail_;
 
 	c_val[0] = 48;
-	if (!wilc_wlan_cfg_set(0, WID_TX_POWER_LEVEL_11A, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_TX_POWER_LEVEL_11A, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
 	c_val[0] = 28;
-	if (!wilc_wlan_cfg_set(0, WID_TX_POWER_LEVEL_11B, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_TX_POWER_LEVEL_11B, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
-	/*  Beacon Interval                                                      */
-	/*  -------------------------------------------------------------------- */
-	/*  Configuration : Sets the beacon interval value                       */
-	/*  Values to set : Any 16-bit value                                     */
-	/*  -------------------------------------------------------------------- */
-
 	*((int *)c_val) = 100;
-	if (!wilc_wlan_cfg_set(0, WID_BEACON_INTERVAL, c_val, 2, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_BEACON_INTERVAL, c_val, 2, 0, 0))
 		goto _fail_;
 
 	c_val[0] = REKEY_DISABLE;
-	if (!wilc_wlan_cfg_set(0, WID_REKEY_POLICY, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_REKEY_POLICY, c_val, 1, 0, 0))
 		goto _fail_;
 
-	/*  Rekey Time (s) (Used only when the Rekey policy is 2 or 4)           */
-	/*  -------------------------------------------------------------------- */
-	/*  Configuration : Sets the Rekey Time (s)                              */
-	/*  Values to set : 32-bit value                                         */
-	/*  -------------------------------------------------------------------- */
 	*((int *)c_val) = 84600;
-	if (!wilc_wlan_cfg_set(0, WID_REKEY_PERIOD, c_val, 4, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_REKEY_PERIOD, c_val, 4, 0, 0))
 		goto _fail_;
 
-	/*  Rekey Packet Count (in 1000s; used when Rekey Policy is 3)           */
-	/*  -------------------------------------------------------------------- */
-	/*  Configuration : Sets Rekey Group Packet count                        */
-	/*  Values to set : 32-bit Value                                         */
-	/*  -------------------------------------------------------------------- */
 	*((int *)c_val) = 500;
-	if (!wilc_wlan_cfg_set(0, WID_REKEY_PACKET_COUNT, c_val, 4, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_REKEY_PACKET_COUNT, c_val, 4, 0,
+			       0))
 		goto _fail_;
 
 	c_val[0] = 1;
-	if (!wilc_wlan_cfg_set(0, WID_SHORT_SLOT_ALLOWED, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_SHORT_SLOT_ALLOWED, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
 	c_val[0] = G_SELF_CTS_PROT;
-	if (!wilc_wlan_cfg_set(0, WID_11N_ERP_PROT_TYPE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_ERP_PROT_TYPE, c_val, 1, 0, 0))
 		goto _fail_;
 
-	c_val[0] = 1;  /* Enable N */
-	if (!wilc_wlan_cfg_set(0, WID_11N_ENABLE, c_val, 1, 0, 0))
+	c_val[0] = 1;
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_ENABLE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = HT_MIXED_MODE;
-	if (!wilc_wlan_cfg_set(0, WID_11N_OPERATING_MODE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_OPERATING_MODE, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
-	c_val[0] = 1;   /* TXOP Prot disable in N mode: No RTS-CTS on TX A-MPDUs to save air-time. */
-	if (!wilc_wlan_cfg_set(0, WID_11N_TXOP_PROT_DISABLE, c_val, 1, 0, 0))
+	c_val[0] = 1;
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_TXOP_PROT_DISABLE, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
 	memcpy(c_val, mac_add, 6);
 
-	if (!wilc_wlan_cfg_set(0, WID_MAC_ADDR, c_val, 6, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_MAC_ADDR, c_val, 6, 0, 0))
 		goto _fail_;
 
-	/**
-	 *      AP only
-	 **/
 	c_val[0] = DETECT_PROTECT_REPORT;
-	if (!wilc_wlan_cfg_set(0, WID_11N_OBSS_NONHT_DETECTION, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_OBSS_NONHT_DETECTION, c_val, 1,
+			       0, 0))
 		goto _fail_;
 
 	c_val[0] = RTS_CTS_NONHT_PROT;
-	if (!wilc_wlan_cfg_set(0, WID_11N_HT_PROT_TYPE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_HT_PROT_TYPE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 0;
-	if (!wilc_wlan_cfg_set(0, WID_11N_RIFS_PROT_ENABLE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_RIFS_PROT_ENABLE, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
 	c_val[0] = MIMO_MODE;
-	if (!wilc_wlan_cfg_set(0, WID_11N_SMPS_MODE, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_SMPS_MODE, c_val, 1, 0, 0))
 		goto _fail_;
 
 	c_val[0] = 7;
-	if (!wilc_wlan_cfg_set(0, WID_11N_CURRENT_TX_MCS, c_val, 1, 0, 0))
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_CURRENT_TX_MCS, c_val, 1, 0,
+			       0))
 		goto _fail_;
 
-	c_val[0] = 1; /* Enable N with immediate block ack. */
-	if (!wilc_wlan_cfg_set(0, WID_11N_IMMEDIATE_BA_ENABLED, c_val, 1, 1, 1))
+	c_val[0] = 1;
+	if (!wilc_wlan_cfg_set(wilc, 0, WID_11N_IMMEDIATE_BA_ENABLED, c_val, 1,
+			       1, 1))
 		goto _fail_;
 
 	return 0;
@@ -884,14 +732,13 @@ _fail_:
 	return -1;
 }
 
-/**************************/
 void wilc1000_wlan_deinit(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
 
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
 
 	if (!wl) {
 		netdev_err(dev, "wl is NULL\n");
@@ -901,20 +748,14 @@ void wilc1000_wlan_deinit(struct net_device *dev)
 	if (wl->initialized)	{
 		netdev_info(dev, "Deinitializing wilc1000...\n");
 
-#if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
-		/* johnny : remove */
-		PRINT_D(INIT_DBG, "skip wilc_bus_set_default_speed\n");
-#else
-		wilc_bus_set_default_speed();
-#endif
-
 		PRINT_D(INIT_DBG, "Disabling IRQ\n");
-#ifdef WILC_SDIO
-		mutex_lock(&wl->hif_cs);
-		disable_sdio_interrupt();
-		mutex_unlock(&wl->hif_cs);
-#endif
-		if (&wl->txq_event != NULL)
+		if (!wl->dev_irq_num &&
+		    wl->hif_func->disable_interrupt) {
+			mutex_lock(&wl->hif_cs);
+			wl->hif_func->disable_interrupt(wl);
+			mutex_unlock(&wl->hif_cs);
+		}
+		if (&wl->txq_event)
 			up(&wl->txq_event);
 
 		PRINT_D(INIT_DBG, "Deinitializing Threads\n");
@@ -923,25 +764,25 @@ void wilc1000_wlan_deinit(struct net_device *dev)
 		PRINT_D(INIT_DBG, "Deinitializing IRQ\n");
 		deinit_irq(dev);
 
-		wilc_wlan_stop();
+		wilc_wlan_stop(wl);
 
 		PRINT_D(INIT_DBG, "Deinitializing WILC Wlan\n");
 		wilc_wlan_cleanup(dev);
-#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
-  #if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
-		PRINT_D(INIT_DBG, "Disabling IRQ 2\n");
-
-		mutex_lock(&wl->hif_cs);
-		disable_sdio_interrupt();
-		mutex_unlock(&wl->hif_cs);
-  #endif
+#if defined(PLAT_ALLWINNER_A20) || defined(PLAT_ALLWINNER_A23) || defined(PLAT_ALLWINNER_A31)
+		if (!wl->dev_irq_num &&
+		    wl->hif_func->disable_interrupt) {
+
+			PRINT_D(INIT_DBG, "Disabling IRQ 2\n");
+
+			mutex_lock(&wl->hif_cs);
+			wl->hif_func->disable_interrupt(wl);
+			mutex_unlock(&wl->hif_cs);
+		}
 #endif
 
-		/*De-Initialize locks*/
 		PRINT_D(INIT_DBG, "Deinitializing Locks\n");
 		wlan_deinit_locks(dev);
 
-		/* announce that wilc1000 is not initialized */
 		wl->initialized = false;
 
 		PRINT_D(INIT_DBG, "wilc1000 deinitialization Done\n");
@@ -951,13 +792,13 @@ void wilc1000_wlan_deinit(struct net_device *dev)
 	}
 }
 
-int wlan_init_locks(struct net_device *dev)
+static int wlan_init_locks(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
 
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
 
 	PRINT_D(INIT_DBG, "Initializing Locks ...\n");
 
@@ -979,105 +820,68 @@ int wlan_init_locks(struct net_device *dev)
 
 static int wlan_deinit_locks(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
 	PRINT_D(INIT_DBG, "De-Initializing Locks\n");
 
-	if (&wilc->hif_cs != NULL)
+	if (&wilc->hif_cs)
 		mutex_destroy(&wilc->hif_cs);
 
-	if (&wilc->rxq_cs != NULL)
+	if (&wilc->rxq_cs)
 		mutex_destroy(&wilc->rxq_cs);
 
 	return 0;
 }
-void linux_to_wlan(wilc_wlan_inp_t *nwi, struct wilc *nic)
-{
-
-	PRINT_D(INIT_DBG, "Linux to Wlan services ...\n");
-
-	nwi->os_context.os_private = (void *)nic;
 
-#ifdef WILC_SDIO
-	nwi->io_func.io_type = HIF_SDIO;
-	nwi->io_func.io_init = linux_sdio_init;
-	nwi->io_func.io_deinit = linux_sdio_deinit;
-	nwi->io_func.u.sdio.sdio_cmd52 = linux_sdio_cmd52;
-	nwi->io_func.u.sdio.sdio_cmd53 = linux_sdio_cmd53;
-	nwi->io_func.u.sdio.sdio_set_max_speed = linux_sdio_set_max_speed;
-	nwi->io_func.u.sdio.sdio_set_default_speed = linux_sdio_set_default_speed;
-#else
-	nwi->io_func.io_type = HIF_SPI;
-	nwi->io_func.io_init = linux_spi_init;
-	nwi->io_func.io_deinit = linux_spi_deinit;
-	nwi->io_func.u.spi.spi_tx = linux_spi_write;
-	nwi->io_func.u.spi.spi_rx = linux_spi_read;
-	nwi->io_func.u.spi.spi_trx = linux_spi_write_read;
-	nwi->io_func.u.spi.spi_max_speed = linux_spi_set_max_speed;
-#endif
-}
-
-int wlan_initialize_threads(struct net_device *dev)
+static int wlan_initialize_threads(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
-	int ret = 0;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
 	PRINT_D(INIT_DBG, "Initializing Threads ...\n");
-
-	/* create tx task */
 	PRINT_D(INIT_DBG, "Creating kthread for transmission\n");
 	wilc->txq_thread = kthread_run(linux_wlan_txq_task, (void *)dev,
 				     "K_TXQ_TASK");
 	if (!wilc->txq_thread) {
 		PRINT_ER("couldn't create TXQ thread\n");
-		ret = -ENOBUFS;
-		goto _fail_2;
+		wilc->close = 0;
+		return -ENOBUFS;
 	}
-	/* wait for TXQ task to start. */
 	down(&wilc->txq_thread_started);
 
 	return 0;
-
-_fail_2:
-	/*De-Initialize 2nd thread*/
-	wilc->close = 0;
-	return ret;
 }
 
 static void wlan_deinitialize_threads(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
-
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
 
 	wl->close = 1;
 	PRINT_D(INIT_DBG, "Deinitializing Threads\n");
 
-	if (&wl->txq_event != NULL)
+	if (&wl->txq_event)
 		up(&wl->txq_event);
 
-	if (wl->txq_thread != NULL) {
+	if (wl->txq_thread) {
 		kthread_stop(wl->txq_thread);
 		wl->txq_thread = NULL;
 	}
 }
 
-int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
+int wilc1000_wlan_init(struct net_device *dev, struct wilc_vif *vif)
 {
-	wilc_wlan_inp_t nwi;
-	perInterface_wlan_t *nic = p_nic;
 	int ret = 0;
-	struct wilc *wl = nic->wilc;
+	struct wilc *wl = vif->wilc;
 
 	if (!wl->initialized) {
 		wl->mac_status = WILC_MAC_STATUS_INIT;
@@ -1085,22 +889,18 @@ int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
 
 		wlan_init_locks(dev);
 
-		linux_to_wlan(&nwi, wl);
-
-		ret = wilc_wlan_init(&nwi);
+		ret = wilc_wlan_init(dev);
 		if (ret < 0) {
 			PRINT_ER("Initializing WILC_Wlan FAILED\n");
 			ret = -EIO;
 			goto _fail_locks_;
 		}
 
-#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
-		if (init_irq(dev)) {
+		if (wl->gpio >= 0 && init_irq(dev)) {
 			PRINT_ER("couldn't initialize IRQ\n");
 			ret = -EIO;
 			goto _fail_locks_;
 		}
-#endif
 
 		ret = wlan_initialize_threads(dev);
 		if (ret < 0) {
@@ -1109,39 +909,35 @@ int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
 			goto _fail_wilc_wlan_;
 		}
 
-#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
-		if (enable_sdio_interrupt()) {
+		if (!wl->dev_irq_num &&
+		    wl->hif_func->enable_interrupt &&
+		    wl->hif_func->enable_interrupt(wl)) {
 			PRINT_ER("couldn't initialize IRQ\n");
 			ret = -EIO;
 			goto _fail_irq_init_;
 		}
-#endif
 
-		if (linux_wlan_get_firmware(nic)) {
+		if (wilc_wlan_get_firmware(dev)) {
 			PRINT_ER("Can't get firmware\n");
 			ret = -EIO;
 			goto _fail_irq_enable_;
 		}
 
-		/*Download firmware*/
-		ret = linux_wlan_firmware_download(wl);
+		ret = wilc1000_firmware_download(dev);
 		if (ret < 0) {
 			PRINT_ER("Failed to download firmware\n");
 			ret = -EIO;
 			goto _fail_irq_enable_;
 		}
 
-		/* Start firmware*/
-		ret = linux_wlan_start_firmware(nic);
+		ret = linux_wlan_start_firmware(dev);
 		if (ret < 0) {
 			PRINT_ER("Failed to start firmware\n");
 			ret = -EIO;
 			goto _fail_irq_enable_;
 		}
 
-		wilc_bus_set_max_speed();
-
-		if (wilc_wlan_cfg_get(1, WID_FIRMWARE_VERSION, 1, 0)) {
+		if (wilc_wlan_cfg_get(wl, 1, WID_FIRMWARE_VERSION, 1, 0)) {
 			int size;
 			char Firmware_ver[20];
 
@@ -1151,7 +947,6 @@ int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
 			Firmware_ver[size] = '\0';
 			PRINT_D(INIT_DBG, "***** Firmware Ver = %s  *******\n", Firmware_ver);
 		}
-		/* Initialize firmware with default configuration */
 		ret = linux_wlan_init_test_config(dev, wl);
 
 		if (ret < 0) {
@@ -1161,20 +956,19 @@ int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic)
 		}
 
 		wl->initialized = true;
-		return 0; /*success*/
+		return 0;
 
 _fail_fw_start_:
-		wilc_wlan_stop();
+		wilc_wlan_stop(wl);
 
 _fail_irq_enable_:
-#if (defined WILC_SDIO) && (!defined WILC_SDIO_IRQ_GPIO)
-		disable_sdio_interrupt();
+		if (!wl->dev_irq_num &&
+		    wl->hif_func->disable_interrupt)
+			wl->hif_func->disable_interrupt(wl);
 _fail_irq_init_:
-#endif
-#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
-		deinit_irq(dev);
+		if (wl->dev_irq_num)
+			deinit_irq(dev);
 
-#endif
 		wlan_deinitialize_threads(dev);
 _fail_wilc_wlan_:
 		wilc_wlan_cleanup(dev);
@@ -1187,44 +981,36 @@ _fail_locks_:
 	return ret;
 }
 
-/*
- *      - this function will be called automatically by OS when module inserted.
- */
-
-int mac_init_fn(struct net_device *ndev)
+static int mac_init_fn(struct net_device *ndev)
 {
-
-	/*Why we do this !!!*/
-	netif_start_queue(ndev); /* ma */
-	netif_stop_queue(ndev); /* ma */
+	netif_start_queue(ndev);
+	netif_stop_queue(ndev);
 
 	return 0;
 }
 
-/* This fn is called, when this device is setup using ifconfig */
-int mac_open(struct net_device *ndev)
+int wilc_mac_open(struct net_device *ndev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 
-	/*No need for setting mac address here anymore,*/
-	/*Just set it in init_test_config()*/
 	unsigned char mac_add[ETH_ALEN] = {0};
 	int ret = 0;
 	int i = 0;
 	struct wilc_priv *priv;
 	struct wilc *wl;
 
-	nic = netdev_priv(ndev);
-	wl = nic->wilc;
+	vif = netdev_priv(ndev);
+	wl = vif->wilc;
 
-#ifdef WILC_SPI
-	if (!wl|| !wl->wilc_spidev) {
+	if (!wl|| !wl->dev) {
 		netdev_err(ndev, "wilc1000: SPI device not ready\n");
 		return -ENODEV;
 	}
-#endif
-	nic = netdev_priv(ndev);
-	priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
+
+	vif = netdev_priv(ndev);
+	wilc = vif->wilc;
+	priv = wiphy_priv(vif->ndev->ieee80211_ptr->wiphy);
 	PRINT_D(INIT_DBG, "MAC OPEN[%p]\n", ndev);
 
 	ret = wilc_init_host_int(ndev);
@@ -1234,153 +1020,143 @@ int mac_open(struct net_device *ndev)
 		return ret;
 	}
 
-	/*initialize platform*/
 	PRINT_D(INIT_DBG, "*** re-init ***\n");
-	ret = wilc1000_wlan_init(ndev, nic);
+	ret = wilc1000_wlan_init(ndev, vif);
 	if (ret < 0) {
 		PRINT_ER("Failed to initialize wilc1000\n");
 		wilc_deinit_host_int(ndev);
 		return ret;
 	}
 
-	Set_machw_change_vir_if(ndev, false);
+	wilc_set_machw_change_vir_if(ndev, false);
 
-	host_int_get_MacAddress(priv->hWILCWFIDrv, mac_add);
+	wilc_get_mac_address(vif, mac_add);
 	PRINT_D(INIT_DBG, "Mac address: %pM\n", mac_add);
 
-	/* loop through the NUM of supported devices and set the MAC address */
 	for (i = 0; i < wl->vif_num; i++) {
-		if (ndev == wl->vif[i].ndev) {
-			memcpy(wl->vif[i].src_addr, mac_add, ETH_ALEN);
-			wl->vif[i].hif_drv = priv->hWILCWFIDrv;
+		if (ndev == wl->vif[i]->ndev) {
+			memcpy(wl->vif[i]->src_addr, mac_add, ETH_ALEN);
 			break;
 		}
 	}
 
-	/* TODO: get MAC address whenever the source is EPROM - hardcoded and copy it to ndev*/
-	memcpy(ndev->dev_addr, wl->vif[i].src_addr, ETH_ALEN);
+	memcpy(ndev->dev_addr, wl->vif[i]->src_addr, ETH_ALEN);
 
 	if (!is_valid_ether_addr(ndev->dev_addr)) {
 		PRINT_ER("Error: Wrong MAC address\n");
-		ret = -EINVAL;
-		goto _err_;
+		wilc_deinit_host_int(ndev);
+		wilc1000_wlan_deinit(ndev);
+		return -EINVAL;
 	}
 
-	wilc_mgmt_frame_register(nic->wilc_netdev->ieee80211_ptr->wiphy, nic->wilc_netdev->ieee80211_ptr,
-				 nic->g_struct_frame_reg[0].frame_type, nic->g_struct_frame_reg[0].reg);
-	wilc_mgmt_frame_register(nic->wilc_netdev->ieee80211_ptr->wiphy, nic->wilc_netdev->ieee80211_ptr,
-				 nic->g_struct_frame_reg[1].frame_type, nic->g_struct_frame_reg[1].reg);
+	wilc_mgmt_frame_register(vif->ndev->ieee80211_ptr->wiphy,
+				 vif->ndev->ieee80211_ptr,
+				 vif->g_struct_frame_reg[0].frame_type,
+				 vif->g_struct_frame_reg[0].reg);
+	wilc_mgmt_frame_register(vif->ndev->ieee80211_ptr->wiphy,
+				 vif->ndev->ieee80211_ptr,
+				 vif->g_struct_frame_reg[1].frame_type,
+				 vif->g_struct_frame_reg[1].reg);
 	netif_wake_queue(ndev);
 	wl->open_ifcs++;
-	nic->mac_opened = 1;
+	vif->mac_opened = 1;
 	return 0;
-
-_err_:
-	wilc_deinit_host_int(ndev);
-	wilc1000_wlan_deinit(ndev);
-	return ret;
 }
 
-struct net_device_stats *mac_stats(struct net_device *dev)
+static struct net_device_stats *mac_stats(struct net_device *dev)
 {
-	perInterface_wlan_t *nic = netdev_priv(dev);
+	struct wilc_vif *vif= netdev_priv(dev);
 
-	return &nic->netstats;
+	return &vif->netstats;
 }
 
-/* Setup the multicast filter */
 static void wilc_set_multicast_list(struct net_device *dev)
 {
-
 	struct netdev_hw_addr *ha;
 	struct wilc_priv *priv;
-	struct host_if_drv *pstrWFIDrv;
+	struct host_if_drv *hif_drv;
+	struct wilc_vif *vif;
 	int i = 0;
 
 	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
-	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
+	vif = netdev_priv(dev);
+	hif_drv = (struct host_if_drv *)priv->hWILCWFIDrv;
 
 	if (!dev)
 		return;
 
-	PRINT_D(INIT_DBG, "Setting Multicast List with count = %d.\n", dev->mc.count);
+	PRINT_D(INIT_DBG, "Setting Multicast List with count = %d.\n",
+		dev->mc.count);
 
 	if (dev->flags & IFF_PROMISC) {
-		/* Normally, we should configure the chip to retrive all packets
-		 * but we don't wanna support this right now */
-		/* TODO: add promiscuous mode support */
 		PRINT_D(INIT_DBG, "Set promiscuous mode ON, retrive all packets\n");
 		return;
 	}
 
-	/* If there's more addresses than we handle, get all multicast
-	 * packets and sort them out in software. */
-	if ((dev->flags & IFF_ALLMULTI) || (dev->mc.count) > WILC_MULTICAST_TABLE_SIZE) {
+	if ((dev->flags & IFF_ALLMULTI) ||
+	    (dev->mc.count) > WILC_MULTICAST_TABLE_SIZE) {
 		PRINT_D(INIT_DBG, "Disable multicast filter, retrive all multicast packets\n");
-		/* get all multicast packets */
-		host_int_setup_multicast_filter(pstrWFIDrv, false, 0);
+		wilc_setup_multicast_filter(vif, false, 0);
 		return;
 	}
 
-	/* No multicast?  Just get our own stuff */
 	if ((dev->mc.count) == 0) {
 		PRINT_D(INIT_DBG, "Enable multicast filter, retrive directed packets only.\n");
-		host_int_setup_multicast_filter(pstrWFIDrv, true, 0);
+		wilc_setup_multicast_filter(vif, true, 0);
 		return;
 	}
 
-	/* Store all of the multicast addresses in the hardware filter */
-	netdev_for_each_mc_addr(ha, dev)
-	{
-		memcpy(gau8MulticastMacAddrList[i], ha->addr, ETH_ALEN);
+	netdev_for_each_mc_addr(ha, dev) {
+		memcpy(wilc_multicast_mac_addr_list[i], ha->addr, ETH_ALEN);
 		PRINT_D(INIT_DBG, "Entry[%d]: %x:%x:%x:%x:%x:%x\n", i,
-			gau8MulticastMacAddrList[i][0], gau8MulticastMacAddrList[i][1], gau8MulticastMacAddrList[i][2], gau8MulticastMacAddrList[i][3], gau8MulticastMacAddrList[i][4], gau8MulticastMacAddrList[i][5]);
+			wilc_multicast_mac_addr_list[i][0],
+			wilc_multicast_mac_addr_list[i][1],
+			wilc_multicast_mac_addr_list[i][2],
+			wilc_multicast_mac_addr_list[i][3],
+			wilc_multicast_mac_addr_list[i][4],
+			wilc_multicast_mac_addr_list[i][5]);
 		i++;
 	}
 
-	host_int_setup_multicast_filter(pstrWFIDrv, true, (dev->mc.count));
+	wilc_setup_multicast_filter(vif, true, (dev->mc.count));
 
 	return;
-
 }
 
 static void linux_wlan_tx_complete(void *priv, int status)
 {
-
 	struct tx_complete_data *pv_data = (struct tx_complete_data *)priv;
 
 	if (status == 1)
 		PRINT_D(TX_DBG, "Packet sent successfully - Size = %d - Address = %p - SKB = %p\n", pv_data->size, pv_data->buff, pv_data->skb);
 	else
 		PRINT_D(TX_DBG, "Couldn't send packet - Size = %d - Address = %p - SKB = %p\n", pv_data->size, pv_data->buff, pv_data->skb);
-	/* Free the SK Buffer, its work is done */
 	dev_kfree_skb(pv_data->skb);
 	kfree(pv_data);
 }
 
-int mac_xmit(struct sk_buff *skb, struct net_device *ndev)
+int wilc_mac_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct tx_complete_data *tx_data = NULL;
-	int QueueCount;
-	char *pu8UdpBuffer;
+	int queue_count;
+	char *udp_buf;
 	struct iphdr *ih;
 	struct ethhdr *eth_h;
 	struct wilc *wilc;
 
-	nic = netdev_priv(ndev);
-	wilc = nic->wilc;
+	vif = netdev_priv(ndev);
+	wilc = vif->wilc;
 
 	PRINT_D(TX_DBG, "Sending packet just received from TCP/IP\n");
 
-	/* Stop the network interface queue */
 	if (skb->dev != ndev) {
 		PRINT_ER("Packet not destined to this device\n");
 		return 0;
 	}
 
-	tx_data = kmalloc(sizeof(struct tx_complete_data), GFP_ATOMIC);
-	if (tx_data == NULL) {
+	tx_data = kmalloc(sizeof(*tx_data), GFP_ATOMIC);
+	if (!tx_data) {
 		PRINT_ER("Failed to allocate memory for tx_data structure\n");
 		dev_kfree_skb(skb);
 		netif_wake_queue(ndev);
@@ -1395,59 +1171,55 @@ int mac_xmit(struct sk_buff *skb, struct net_device *ndev)
 	if (eth_h->h_proto == 0x8e88)
 		PRINT_D(INIT_DBG, "EAPOL transmitted\n");
 
-	/*get source and dest ip addresses*/
 	ih = (struct iphdr *)(skb->data + sizeof(struct ethhdr));
 
-	pu8UdpBuffer = (char *)ih + sizeof(struct iphdr);
-	if ((pu8UdpBuffer[1] == 68 && pu8UdpBuffer[3] == 67) || (pu8UdpBuffer[1] == 67 && pu8UdpBuffer[3] == 68))
-		PRINT_D(GENERIC_DBG, "DHCP Message transmitted, type:%x %x %x\n", pu8UdpBuffer[248], pu8UdpBuffer[249], pu8UdpBuffer[250]);
+	udp_buf = (char *)ih + sizeof(struct iphdr);
+	if ((udp_buf[1] == 68 && udp_buf[3] == 67) ||
+	    (udp_buf[1] == 67 && udp_buf[3] == 68))
+		PRINT_D(GENERIC_DBG, "DHCP Message transmitted, type:%x %x %x\n",
+			udp_buf[248], udp_buf[249], udp_buf[250]);
 
 	PRINT_D(TX_DBG, "Sending packet - Size = %d - Address = %p - SKB = %p\n", tx_data->size, tx_data->buff, tx_data->skb);
-
-	/* Send packet to MAC HW - for now the tx_complete function will be just status
-	 * indicator. still not sure if I need to suspend host transmission till the tx_complete
-	 * function called or not?
-	 * allocated buffer will be freed in tx_complete function.
-	 */
 	PRINT_D(TX_DBG, "Adding tx packet to TX Queue\n");
-	nic->netstats.tx_packets++;
-	nic->netstats.tx_bytes += tx_data->size;
-	tx_data->pBssid = wilc->vif[nic->u8IfIdx].bssid;
-	QueueCount = wilc_wlan_txq_add_net_pkt(ndev, (void *)tx_data,
-					       tx_data->buff, tx_data->size,
-					       linux_wlan_tx_complete);
-
-	if (QueueCount > FLOW_CONTROL_UPPER_THRESHOLD) {
-		netif_stop_queue(wilc->vif[0].ndev);
-		netif_stop_queue(wilc->vif[1].ndev);
+	vif->netstats.tx_packets++;
+	vif->netstats.tx_bytes += tx_data->size;
+	tx_data->pBssid = wilc->vif[vif->u8IfIdx]->bssid;
+	queue_count = wilc_wlan_txq_add_net_pkt(ndev, (void *)tx_data,
+						tx_data->buff, tx_data->size,
+						linux_wlan_tx_complete);
+
+	if (queue_count > FLOW_CONTROL_UPPER_THRESHOLD) {
+		netif_stop_queue(wilc->vif[0]->ndev);
+		netif_stop_queue(wilc->vif[1]->ndev);
 	}
 
 	return 0;
 }
 
-int mac_close(struct net_device *ndev)
+int wilc_mac_close(struct net_device *ndev)
 {
 	struct wilc_priv *priv;
-	perInterface_wlan_t *nic;
-	struct host_if_drv *pstrWFIDrv;
+	struct wilc_vif *vif;
+	struct host_if_drv *hif_drv;
 	struct wilc *wl;
 
-	nic = netdev_priv(ndev);
+	vif = netdev_priv(ndev);
 
-	if ((nic == NULL) || (nic->wilc_netdev == NULL) || (nic->wilc_netdev->ieee80211_ptr == NULL) || (nic->wilc_netdev->ieee80211_ptr->wiphy == NULL)) {
-		PRINT_ER("nic = NULL\n");
+	if (!vif || !vif->ndev || !vif->ndev->ieee80211_ptr ||
+	    !vif->ndev->ieee80211_ptr->wiphy) {
+		PRINT_ER("vif = NULL\n");
 		return 0;
 	}
 
-	priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
-	wl = nic->wilc;
+	priv = wiphy_priv(vif->ndev->ieee80211_ptr->wiphy);
+	wl = vif->wilc;
 
-	if (priv == NULL) {
+	if (!priv) {
 		PRINT_ER("priv = NULL\n");
 		return 0;
 	}
 
-	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
+	hif_drv = (struct host_if_drv *)priv->hWILCWFIDrv;
 
 	PRINT_D(GENERIC_DBG, "Mac close\n");
 
@@ -1456,8 +1228,8 @@ int mac_close(struct net_device *ndev)
 		return 0;
 	}
 
-	if (pstrWFIDrv == NULL)	{
-		PRINT_ER("pstrWFIDrv = NULL\n");
+	if (!hif_drv) {
+		PRINT_ER("hif_drv = NULL\n");
 		return 0;
 	}
 
@@ -1468,11 +1240,10 @@ int mac_close(struct net_device *ndev)
 		return 0;
 	}
 
-	if (nic->wilc_netdev != NULL) {
-		/* Stop the network interface queue */
-		netif_stop_queue(nic->wilc_netdev);
+	if (vif->ndev) {
+		netif_stop_queue(vif->ndev);
 
-		wilc_deinit_host_int(nic->wilc_netdev);
+		wilc_deinit_host_int(vif->ndev);
 	}
 
 	if (wl->open_ifcs == 0) {
@@ -1483,59 +1254,54 @@ int mac_close(struct net_device *ndev)
 	}
 
 	up(&close_exit_sync);
-	nic->mac_opened = 0;
+	vif->mac_opened = 0;
 
 	return 0;
 }
 
-int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
+static int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
 {
-
 	u8 *buff = NULL;
 	s8 rssi;
 	u32 size = 0, length = 0;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc_priv *priv;
-	s32 s32Error = 0;
+	s32 ret = 0;
 	struct wilc *wilc;
 
-	/* struct iwreq *wrq = (struct iwreq *) req;	// tony moved to case SIOCSIWPRIV */
-	nic = netdev_priv(ndev);
-	wilc = nic->wilc;
+	vif = netdev_priv(ndev);
+	wilc = vif->wilc;
 
 	if (!wilc->initialized)
 		return 0;
 
 	switch (cmd) {
-
-	/* ]] 2013-06-24 */
 	case SIOCSIWPRIV:
 	{
-		struct iwreq *wrq = (struct iwreq *) req;               /* added by tony */
+		struct iwreq *wrq = (struct iwreq *) req;
 
 		size = wrq->u.data.length;
 
 		if (size && wrq->u.data.pointer) {
-
-			buff = memdup_user(wrq->u.data.pointer, wrq->u.data.length);
+			buff = memdup_user(wrq->u.data.pointer,
+					   wrq->u.data.length);
 			if (IS_ERR(buff))
 				return PTR_ERR(buff);
 
 			if (strncasecmp(buff, "RSSI", length) == 0) {
-				priv = wiphy_priv(nic->wilc_netdev->ieee80211_ptr->wiphy);
-				s32Error = host_int_get_rssi(priv->hWILCWFIDrv, &(rssi));
-				if (s32Error)
+				priv = wiphy_priv(vif->ndev->ieee80211_ptr->wiphy);
+				ret = wilc_get_rssi(vif, &rssi);
+				if (ret)
 					PRINT_ER("Failed to send get rssi param's message queue ");
 				PRINT_INFO(GENERIC_DBG, "RSSI :%d\n", rssi);
 
-				/*Rounding up the rssi negative value*/
 				rssi += 5;
 
 				snprintf(buff, size, "rssi %d", rssi);
 
 				if (copy_to_user(wrq->u.data.pointer, buff, size)) {
 					PRINT_ER("%s: failed to copy data to user buffer\n", __func__);
-					s32Error = -EFAULT;
+					ret = -EFAULT;
 					goto done;
 				}
 			}
@@ -1546,7 +1312,7 @@ int mac_ioctl(struct net_device *ndev, struct ifreq *req, int cmd)
 	default:
 	{
 		PRINT_INFO(GENERIC_DBG, "Command - %d - has been received\n", cmd);
-		s32Error = -EOPNOTSUPP;
+		ret = -EOPNOTSUPP;
 		goto done;
 	}
 	}
@@ -1555,64 +1321,47 @@ done:
 
 	kfree(buff);
 
-	return s32Error;
+	return ret;
 }
 
-void frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset)
+void wilc_frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset)
 {
-
 	unsigned int frame_len = 0;
 	int stats;
 	unsigned char *buff_to_send = NULL;
 	struct sk_buff *skb;
 	struct net_device *wilc_netdev;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
-	wilc_netdev = GetIfHandler(wilc, buff);
-	if (wilc_netdev == NULL)
+	wilc_netdev = get_if_handler(wilc, buff);
+	if (!wilc_netdev)
 		return;
 
 	buff += pkt_offset;
-	nic = netdev_priv(wilc_netdev);
+	vif = netdev_priv(wilc_netdev);
 
 	if (size > 0) {
-
 		frame_len = size;
 		buff_to_send = buff;
 
-		/* Need to send the packet up to the host, allocate a skb buffer */
 		skb = dev_alloc_skb(frame_len);
-		if (skb == NULL) {
+		if (!skb) {
 			PRINT_ER("Low memory - packet droped\n");
 			return;
 		}
 
-		if (wilc == NULL || wilc_netdev == NULL)
+		if (!wilc || !wilc_netdev)
 			PRINT_ER("wilc_netdev in wilc is NULL");
 		skb->dev = wilc_netdev;
 
-		if (skb->dev == NULL)
+		if (!skb->dev)
 			PRINT_ER("skb->dev is NULL\n");
 
-		/*
-		 * for(i=0;i<40;i++)
-		 * {
-		 *      if(i<frame_len)
-		 *              WILC_PRINTF("buff_to_send[%d]=%2x\n",i,buff_to_send[i]);
-		 *
-		 * }*/
-
-		/* skb_put(skb, frame_len); */
 		memcpy(skb_put(skb, frame_len), buff_to_send, frame_len);
 
-		/* WILC_PRINTF("After MEM_CPY\n"); */
-
-		/* nic = netdev_priv(wilc_netdev); */
-
 		skb->protocol = eth_type_trans(skb, wilc_netdev);
-		/* Send the packet to the stack by giving it to the bridge */
-		nic->netstats.rx_packets++;
-		nic->netstats.rx_bytes += frame_len;
+		vif->netstats.rx_packets++;
+		vif->netstats.rx_bytes += frame_len;
 		skb->ip_summed = CHECKSUM_UNNECESSARY;
 		stats = netif_rx(skb);
 		PRINT_D(RX_DBG, "netif_rx ret value is: %d\n", stats);
@@ -1622,211 +1371,132 @@ void frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset)
 void WILC_WFI_mgmt_rx(struct wilc *wilc, u8 *buff, u32 size)
 {
 	int i = 0;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
-	/*Pass the frame on the monitor interface, if any.*/
-	/*Otherwise, pass it on p2p0 netdev, if registered on it*/
 	for (i = 0; i < wilc->vif_num; i++) {
-		nic = netdev_priv(wilc->vif[i].ndev);
-		if (nic->monitor_flag) {
+		vif = netdev_priv(wilc->vif[i]->ndev);
+		if (vif->monitor_flag) {
 			WILC_WFI_monitor_rx(buff, size);
 			return;
 		}
 	}
 
-	nic = netdev_priv(wilc->vif[1].ndev); /* p2p0 */
-	if ((buff[0] == nic->g_struct_frame_reg[0].frame_type && nic->g_struct_frame_reg[0].reg) ||
-	    (buff[0] == nic->g_struct_frame_reg[1].frame_type && nic->g_struct_frame_reg[1].reg))
-		WILC_WFI_p2p_rx(wilc->vif[1].ndev, buff, size);
+	vif = netdev_priv(wilc->vif[1]->ndev);
+	if ((buff[0] == vif->g_struct_frame_reg[0].frame_type && vif->g_struct_frame_reg[0].reg) ||
+	    (buff[0] == vif->g_struct_frame_reg[1].frame_type && vif->g_struct_frame_reg[1].reg))
+		WILC_WFI_p2p_rx(wilc->vif[1]->ndev, buff, size);
 }
 
-void wl_wlan_cleanup(void)
+void wilc_netdev_cleanup(struct wilc *wilc)
 {
 	int i = 0;
-	perInterface_wlan_t *nic[NUM_CONCURRENT_IFC];
+	struct wilc_vif *vif[NUM_CONCURRENT_IFC];
 
-	if (g_linux_wlan &&
-	   (g_linux_wlan->vif[0].ndev || g_linux_wlan->vif[1].ndev)) {
+	if (wilc && (wilc->vif[0]->ndev || wilc->vif[1]->ndev)) {
 		unregister_inetaddr_notifier(&g_dev_notifier);
 
 		for (i = 0; i < NUM_CONCURRENT_IFC; i++)
-			nic[i] = netdev_priv(g_linux_wlan->vif[i].ndev);
+			vif[i] = netdev_priv(wilc->vif[i]->ndev);
 	}
 
-	if (g_linux_wlan && g_linux_wlan->firmware)
-		release_firmware(g_linux_wlan->firmware);
+	if (wilc && wilc->firmware)
+		release_firmware(wilc->firmware);
 
-	if (g_linux_wlan &&
-	   (g_linux_wlan->vif[0].ndev || g_linux_wlan->vif[1].ndev)) {
-		linux_wlan_lock_timeout(&close_exit_sync, 12 * 1000);
+	if (wilc && (wilc->vif[0]->ndev || wilc->vif[1]->ndev)) {
+		wilc_lock_timeout(wilc, &close_exit_sync, 12 * 1000);
 
 		for (i = 0; i < NUM_CONCURRENT_IFC; i++)
-			if (g_linux_wlan->vif[i].ndev)
-				if (nic[i]->mac_opened)
-					mac_close(g_linux_wlan->vif[i].ndev);
+			if (wilc->vif[i]->ndev)
+				if (vif[i]->mac_opened)
+					wilc_mac_close(wilc->vif[i]->ndev);
 
 		for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
-			unregister_netdev(g_linux_wlan->vif[i].ndev);
-			wilc_free_wiphy(g_linux_wlan->vif[i].ndev);
-			free_netdev(g_linux_wlan->vif[i].ndev);
+			unregister_netdev(wilc->vif[i]->ndev);
+			wilc_free_wiphy(wilc->vif[i]->ndev);
+			free_netdev(wilc->vif[i]->ndev);
 		}
 	}
 
-	kfree(g_linux_wlan);
-
-#if defined(WILC_DEBUGFS)
-	wilc_debugfs_remove();
-#endif
-	linux_wlan_device_detection(0);
-	linux_wlan_device_power(0);
+	kfree(wilc);
 }
+EXPORT_SYMBOL_GPL(wilc_netdev_cleanup);
 
-int wilc_netdev_init(struct wilc **wilc)
+int wilc_netdev_init(struct wilc **wilc, struct device *dev, int io_type,
+		     int gpio, const struct wilc_hif_func *ops)
 {
 	int i;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct net_device *ndev;
+	struct wilc *wl;
 
 	sema_init(&close_exit_sync, 0);
 
-	/*create the common structure*/
-	g_linux_wlan = kzalloc(sizeof(*g_linux_wlan), GFP_KERNEL);
-	if (!g_linux_wlan)
+	wl = kzalloc(sizeof(*wl), GFP_KERNEL);
+	if (!wl)
 		return -ENOMEM;
 
-	*wilc = g_linux_wlan;
+	*wilc = wl;
+	wl->io_type = io_type;
+	wl->gpio = gpio;
+	wl->hif_func = ops;
 
 	register_inetaddr_notifier(&g_dev_notifier);
 
 	for (i = 0; i < NUM_CONCURRENT_IFC; i++) {
-		/*allocate first ethernet device with perinterface_wlan_t as its private data*/
-		ndev = alloc_etherdev(sizeof(perInterface_wlan_t));
+		ndev = alloc_etherdev(sizeof(struct wilc_vif));
 		if (!ndev) {
 			PRINT_ER("Failed to allocate ethernet dev\n");
 			return -1;
 		}
 
-		nic = netdev_priv(ndev);
-		memset(nic, 0, sizeof(perInterface_wlan_t));
+		vif = netdev_priv(ndev);
+		memset(vif, 0, sizeof(struct wilc_vif));
 
-		/*Name the Devices*/
-		if (i == 0) {
-		#if defined(NM73131)    /* tony, 2012-09-20 */
-			strcpy(ndev->name, "wilc_eth%d");
-		#elif defined(PLAT_CLM9722)                     /* rachel */
-			strcpy(ndev->name, "eth%d");
-		#else /* PANDA_BOARD, PLAT_ALLWINNER_A10, PLAT_ALLWINNER_A20, PLAT_ALLWINNER_A31, PLAT_AML8726_M3 or PLAT_WMS8304 */
+		if (i == 0)
 			strcpy(ndev->name, "wlan%d");
-		#endif
-		} else
+		else
 			strcpy(ndev->name, "p2p%d");
 
-		nic->u8IfIdx = g_linux_wlan->vif_num;
-		nic->wilc_netdev = ndev;
-		nic->wilc = *wilc;
-		g_linux_wlan->vif[g_linux_wlan->vif_num].ndev = ndev;
-		g_linux_wlan->vif_num++;
+		vif->u8IfIdx = wl->vif_num;
+		vif->wilc = *wilc;
+		wl->vif[i] = vif;
+		wl->vif[wl->vif_num]->ndev = ndev;
+		wl->vif_num++;
 		ndev->netdev_ops = &wilc_netdev_ops;
 
 		{
 			struct wireless_dev *wdev;
-			/*Register WiFi*/
-			wdev = wilc_create_wiphy(ndev);
+			wdev = wilc_create_wiphy(ndev, dev);
 
-			#ifdef WILC_SDIO
-			/* set netdev, tony */
-			SET_NETDEV_DEV(ndev, &local_sdio_func->dev);
-			#endif
+			if (dev)
+				SET_NETDEV_DEV(ndev, dev);
 
-			if (wdev == NULL) {
+			if (!wdev) {
 				PRINT_ER("Can't register WILC Wiphy\n");
 				return -1;
 			}
 
-			/*linking the wireless_dev structure with the netdevice*/
-			nic->wilc_netdev->ieee80211_ptr = wdev;
-			nic->wilc_netdev->ml_priv = nic;
-			wdev->netdev = nic->wilc_netdev;
-			nic->netstats.rx_packets = 0;
-			nic->netstats.tx_packets = 0;
-			nic->netstats.rx_bytes = 0;
-			nic->netstats.tx_bytes = 0;
-
+			vif->ndev->ieee80211_ptr = wdev;
+			vif->ndev->ml_priv = vif;
+			wdev->netdev = vif->ndev;
+			vif->netstats.rx_packets = 0;
+			vif->netstats.tx_packets = 0;
+			vif->netstats.rx_bytes = 0;
+			vif->netstats.tx_bytes = 0;
 		}
 
 		if (register_netdev(ndev)) {
-			PRINT_ER("Device couldn't be registered - %s\n", ndev->name);
-			return -1; /* ERROR */
+			PRINT_ER("Device couldn't be registered - %s\n",
+				 ndev->name);
+			return -1;
 		}
 
-		nic->iftype = STATION_MODE;
-		nic->mac_opened = 0;
-
-	}
-
-	#ifndef WILC_SDIO
-	if (!linux_spi_init(&g_linux_wlan->wilc_spidev)) {
-		PRINT_ER("Can't initialize SPI\n");
-		return -1; /* ERROR */
+		vif->iftype = STATION_MODE;
+		vif->mac_opened = 0;
 	}
-	g_linux_wlan->wilc_spidev = wilc_spi_dev;
-	#else
-	g_linux_wlan->wilc_sdio_func = local_sdio_func;
-	#endif
 
 	return 0;
 }
-
-/*The 1st function called after module inserted*/
-static int __init init_wilc_driver(void)
-{
-#ifdef WILC_SPI
-	struct wilc *wilc;
-#endif
-
-#if defined(WILC_DEBUGFS)
-	if (wilc_debugfs_init() < 0) {
-		PRINT_D(GENERIC_DBG, "fail to create debugfs for wilc driver\n");
-		return -1;
-	}
-#endif
-
-	printk("IN INIT FUNCTION\n");
-	printk("*** WILC1000 driver VERSION=[10.2] FW_VER=[10.2] ***\n");
-
-	linux_wlan_device_power(1);
-	msleep(100);
-	linux_wlan_device_detection(1);
-
-#ifdef WILC_SDIO
-	{
-		int ret;
-
-		ret = sdio_register_driver(&wilc_bus);
-		if (ret < 0)
-			PRINT_D(INIT_DBG, "init_wilc_driver: Failed register sdio driver\n");
-
-		return ret;
-	}
-#else
-	PRINT_D(INIT_DBG, "Initializing netdev\n");
-	if (wilc_netdev_init(&wilc))
-		PRINT_ER("Couldn't initialize netdev\n");
-	return 0;
-#endif
-}
-late_initcall(init_wilc_driver);
-
-static void __exit exit_wilc_driver(void)
-{
-#ifndef WILC_SDIO
-	PRINT_D(INIT_DBG, "SPI unregister...\n");
-	spi_unregister_driver(&wilc_bus);
-#else
-	PRINT_D(INIT_DBG, "SDIO unregister...\n");
-	sdio_unregister_driver(&wilc_bus);
-#endif
-}
-module_exit(exit_wilc_driver);
+EXPORT_SYMBOL_GPL(wilc_netdev_init);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/wilc1000/linux_wlan_common.h b/drivers/staging/wilc1000/linux_wlan_common.h
index 2b76e41ebd4d..5d40f05124c1 100644
--- a/drivers/staging/wilc1000/linux_wlan_common.h
+++ b/drivers/staging/wilc1000/linux_wlan_common.h
@@ -38,11 +38,8 @@ enum debug_region {
 #define FIRM_DBG                (1 << Firmware_debug)
 
 #if defined (WILC_DEBUGFS)
-int wilc_debugfs_init(void);
-void wilc_debugfs_remove(void);
-
-extern atomic_t REGION;
-extern atomic_t DEBUG_LEVEL;
+extern atomic_t WILC_REGION;
+extern atomic_t WILC_DEBUG_LEVEL;
 
 #define DEBUG           BIT(0)
 #define INFO            BIT(1)
@@ -51,8 +48,8 @@ extern atomic_t DEBUG_LEVEL;
 
 #define PRINT_D(region, ...)						\
 	do {								\
-		if ((atomic_read(&DEBUG_LEVEL) & DEBUG) &&		\
-		   ((atomic_read(&REGION)) & (region))) {		\
+		if ((atomic_read(&WILC_DEBUG_LEVEL) & DEBUG) &&	\
+		   ((atomic_read(&WILC_REGION)) & (region))) {	\
 			printk("DBG [%s: %d]", __func__, __LINE__);	\
 			printk(__VA_ARGS__);				\
 		}							\
@@ -60,8 +57,8 @@ extern atomic_t DEBUG_LEVEL;
 
 #define PRINT_INFO(region, ...)						\
 	do {								\
-		if ((atomic_read(&DEBUG_LEVEL) & INFO) &&		\
-		   ((atomic_read(&REGION)) & (region))) {		\
+		if ((atomic_read(&WILC_DEBUG_LEVEL) & INFO) &&	\
+		   ((atomic_read(&WILC_REGION)) & (region))) {	\
 			printk("INFO [%s]", __func__);			\
 			printk(__VA_ARGS__);				\
 		}							\
@@ -69,8 +66,8 @@ extern atomic_t DEBUG_LEVEL;
 
 #define PRINT_WRN(region, ...)						\
 	do {								\
-		if ((atomic_read(&DEBUG_LEVEL) & WRN) &&		\
-		   ((atomic_read(&REGION)) & (region))) {		\
+		if ((atomic_read(&WILC_DEBUG_LEVEL) & WRN) &&	\
+		   ((atomic_read(&WILC_REGION)) & (region))) {	\
 			printk("WRN [%s: %d]", __func__, __LINE__);	\
 			printk(__VA_ARGS__);				\
 		}							\
@@ -78,7 +75,7 @@ extern atomic_t DEBUG_LEVEL;
 
 #define PRINT_ER(...)							\
 	do {								\
-		if ((atomic_read(&DEBUG_LEVEL) & ERR)) {		\
+		if ((atomic_read(&WILC_DEBUG_LEVEL) & ERR)) {	\
 			printk("ERR [%s: %d]", __func__, __LINE__);	\
 			printk(__VA_ARGS__);				\
 		}							\
@@ -121,14 +118,13 @@ extern atomic_t DEBUG_LEVEL;
 		printk("ERR [%s: %d]", __func__, __LINE__);		\
 		printk(__VA_ARGS__);					\
 	} while (0)
+
 #endif
 
 #define FN_IN   /* PRINT_D(">>> \n") */
 #define FN_OUT  /* PRINT_D("<<<\n") */
 
-#ifdef MEMORY_STATIC
 #define LINUX_RX_SIZE	(96 * 1024)
-#endif
 #define LINUX_TX_SIZE	(64 * 1024)
 
 
diff --git a/drivers/staging/wilc1000/linux_wlan_sdio.c b/drivers/staging/wilc1000/linux_wlan_sdio.c
deleted file mode 100644
index 4aff953a88f1..000000000000
--- a/drivers/staging/wilc1000/linux_wlan_sdio.c
+++ /dev/null
@@ -1,251 +0,0 @@
-#include "wilc_wfi_netdevice.h"
-
-#include <linux/mmc/sdio_func.h>
-#include <linux/mmc/card.h>
-#include <linux/mmc/sdio_ids.h>
-#include <linux/mmc/sdio.h>
-#include <linux/mmc/host.h>
-
-
-
-#define SDIO_MODALIAS "wilc1000_sdio"
-
-#if defined(CUSTOMER_PLATFORM)
-/* TODO : User have to stable bus clock as user's environment. */
- #ifdef MAX_BUS_SPEED
- #define MAX_SPEED MAX_BUS_SPEED
- #else
- #define MAX_SPEED 50000000
- #endif
-#else
- #define MAX_SPEED (6 * 1000000) /* Max 50M */
-#endif
-
-struct wilc_sdio {
-	struct sdio_func *func;
-	struct wilc *wilc;
-};
-
-struct sdio_func *local_sdio_func;
-
-static unsigned int sdio_default_speed;
-
-#define SDIO_VENDOR_ID_WILC 0x0296
-#define SDIO_DEVICE_ID_WILC 0x5347
-
-static const struct sdio_device_id wilc_sdio_ids[] = {
-	{ SDIO_DEVICE(SDIO_VENDOR_ID_WILC, SDIO_DEVICE_ID_WILC) },
-	{ },
-};
-
-
-static void wilc_sdio_interrupt(struct sdio_func *func)
-{
-	struct wilc_sdio *wl_sdio;
-
-	wl_sdio = sdio_get_drvdata(func);
-
-#ifndef WILC_SDIO_IRQ_GPIO
-	sdio_release_host(func);
-	wilc_handle_isr(wl_sdio->wilc);
-	sdio_claim_host(func);
-#endif
-}
-
-
-int linux_sdio_cmd52(sdio_cmd52_t *cmd)
-{
-	struct sdio_func *func = g_linux_wlan->wilc_sdio_func;
-	int ret;
-	u8 data;
-
-	sdio_claim_host(func);
-
-	func->num = cmd->function;
-	if (cmd->read_write) {  /* write */
-		if (cmd->raw) {
-			sdio_writeb(func, cmd->data, cmd->address, &ret);
-			data = sdio_readb(func, cmd->address, &ret);
-			cmd->data = data;
-		} else {
-			sdio_writeb(func, cmd->data, cmd->address, &ret);
-		}
-	} else {        /* read */
-		data = sdio_readb(func, cmd->address, &ret);
-		cmd->data = data;
-	}
-
-	sdio_release_host(func);
-
-	if (ret < 0) {
-		PRINT_ER("wilc_sdio_cmd52..failed, err(%d)\n", ret);
-		return 0;
-	}
-	return 1;
-}
-
-
-int linux_sdio_cmd53(sdio_cmd53_t *cmd)
-{
-	struct sdio_func *func = g_linux_wlan->wilc_sdio_func;
-	int size, ret;
-
-	sdio_claim_host(func);
-
-	func->num = cmd->function;
-	func->cur_blksize = cmd->block_size;
-	if (cmd->block_mode)
-		size = cmd->count * cmd->block_size;
-	else
-		size = cmd->count;
-
-	if (cmd->read_write) {  /* write */
-		ret = sdio_memcpy_toio(func, cmd->address, (void *)cmd->buffer, size);
-	} else {        /* read */
-		ret = sdio_memcpy_fromio(func, (void *)cmd->buffer, cmd->address,  size);
-	}
-
-	sdio_release_host(func);
-
-
-	if (ret < 0) {
-		PRINT_ER("wilc_sdio_cmd53..failed, err(%d)\n", ret);
-		return 0;
-	}
-
-	return 1;
-}
-
-static int linux_sdio_probe(struct sdio_func *func, const struct sdio_device_id *id)
-{
-	struct wilc_sdio *wl_sdio;
-	struct wilc *wilc;
-
-	PRINT_D(INIT_DBG, "probe function\n");
-	wl_sdio = kzalloc(sizeof(struct wilc_sdio), GFP_KERNEL);
-	if (!wl_sdio)
-		return -ENOMEM;
-
-	PRINT_D(INIT_DBG, "Initializing netdev\n");
-	local_sdio_func = func;
-	if (wilc_netdev_init(&wilc)) {
-		PRINT_ER("Couldn't initialize netdev\n");
-		kfree(wl_sdio);
-		return -1;
-	}
-	wl_sdio->func = func;
-	wl_sdio->wilc = wilc;
-	sdio_set_drvdata(func, wl_sdio);
-
-	printk("Driver Initializing success\n");
-	return 0;
-}
-
-static void linux_sdio_remove(struct sdio_func *func)
-{
-	struct wilc_sdio *wl_sdio;
-
-	wl_sdio = sdio_get_drvdata(func);
-	wl_wlan_cleanup();
-	kfree(wl_sdio);
-}
-
-struct sdio_driver wilc_bus = {
-	.name		= SDIO_MODALIAS,
-	.id_table	= wilc_sdio_ids,
-	.probe		= linux_sdio_probe,
-	.remove		= linux_sdio_remove,
-};
-
-int enable_sdio_interrupt(void)
-{
-	int ret = 0;
-#ifndef WILC_SDIO_IRQ_GPIO
-
-	sdio_claim_host(local_sdio_func);
-	ret = sdio_claim_irq(local_sdio_func, wilc_sdio_interrupt);
-	sdio_release_host(local_sdio_func);
-
-	if (ret < 0) {
-		PRINT_ER("can't claim sdio_irq, err(%d)\n", ret);
-		ret = -EIO;
-	}
-#endif
-	return ret;
-}
-
-void disable_sdio_interrupt(void)
-{
-
-#ifndef WILC_SDIO_IRQ_GPIO
-	int ret;
-
-	PRINT_D(INIT_DBG, "disable_sdio_interrupt IN\n");
-
-	sdio_claim_host(local_sdio_func);
-	ret = sdio_release_irq(local_sdio_func);
-	if (ret < 0) {
-		PRINT_ER("can't release sdio_irq, err(%d)\n", ret);
-	}
-	sdio_release_host(local_sdio_func);
-
-	PRINT_D(INIT_DBG, "disable_sdio_interrupt OUT\n");
-#endif
-}
-
-static int linux_sdio_set_speed(int speed)
-{
-	struct mmc_ios ios;
-
-	sdio_claim_host(local_sdio_func);
-
-	memcpy((void *)&ios, (void *)&local_sdio_func->card->host->ios, sizeof(struct mmc_ios));
-	local_sdio_func->card->host->ios.clock = speed;
-	ios.clock = speed;
-	local_sdio_func->card->host->ops->set_ios(local_sdio_func->card->host, &ios);
-	sdio_release_host(local_sdio_func);
-	PRINT_INFO(INIT_DBG, "@@@@@@@@@@@@ change SDIO speed to %d @@@@@@@@@\n", speed);
-
-	return 1;
-}
-
-static int linux_sdio_get_speed(void)
-{
-	return local_sdio_func->card->host->ios.clock;
-}
-
-int linux_sdio_init(void *pv)
-{
-
-	/**
-	 *      TODO :
-	 **/
-
-
-	sdio_default_speed = linux_sdio_get_speed();
-	return 1;
-}
-
-void linux_sdio_deinit(void *pv)
-{
-
-	/**
-	 *      TODO :
-	 **/
-
-
-	sdio_unregister_driver(&wilc_bus);
-}
-
-int linux_sdio_set_max_speed(void)
-{
-	return linux_sdio_set_speed(MAX_SPEED);
-}
-
-int linux_sdio_set_default_speed(void)
-{
-	return linux_sdio_set_speed(sdio_default_speed);
-}
-
-
-
diff --git a/drivers/staging/wilc1000/linux_wlan_sdio.h b/drivers/staging/wilc1000/linux_wlan_sdio.h
deleted file mode 100644
index 4b515f5108e7..000000000000
--- a/drivers/staging/wilc1000/linux_wlan_sdio.h
+++ /dev/null
@@ -1,14 +0,0 @@
-extern struct sdio_func *local_sdio_func;
-extern struct sdio_driver wilc_bus;
-
-#include <linux/mmc/sdio_func.h>
-
-int linux_sdio_init(void *);
-void linux_sdio_deinit(void *);
-int linux_sdio_cmd52(sdio_cmd52_t *cmd);
-int linux_sdio_cmd53(sdio_cmd53_t *cmd);
-int enable_sdio_interrupt(void);
-void disable_sdio_interrupt(void);
-int linux_sdio_set_max_speed(void);
-int linux_sdio_set_default_speed(void);
-
diff --git a/drivers/staging/wilc1000/linux_wlan_spi.c b/drivers/staging/wilc1000/linux_wlan_spi.c
deleted file mode 100644
index 039d06192d6b..000000000000
--- a/drivers/staging/wilc1000/linux_wlan_spi.c
+++ /dev/null
@@ -1,409 +0,0 @@
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/fs.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/cdev.h>
-#include <linux/uaccess.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-
-#include "linux_wlan_common.h"
-#include "linux_wlan_spi.h"
-
-#define USE_SPI_DMA     0       /* johnny add */
-
-#ifdef WILC_ASIC_A0
- #if defined(PLAT_PANDA_ES_OMAP4460)
-  #define MIN_SPEED 12000000
-  #define MAX_SPEED 24000000
- #elif defined(PLAT_WMS8304)
-  #define MIN_SPEED 12000000
-  #define MAX_SPEED 24000000 /* 4000000 */
- #elif defined(CUSTOMER_PLATFORM)
-/*
-  TODO : define Clock speed under 48M.
- *
- * ex)
- * #define MIN_SPEED 24000000
- * #define MAX_SPEED 48000000
- */
- #else
-  #define MIN_SPEED 24000000
-  #define MAX_SPEED 48000000
- #endif
-#else /* WILC_ASIC_A0 */
-/* Limit clk to 6MHz on FPGA. */
- #define MIN_SPEED 6000000
- #define MAX_SPEED 6000000
-#endif /* WILC_ASIC_A0 */
-
-static u32 SPEED = MIN_SPEED;
-
-struct spi_device *wilc_spi_dev;
-void linux_spi_deinit(void *vp);
-
-static int __init wilc_bus_probe(struct spi_device *spi)
-{
-
-	PRINT_D(BUS_DBG, "spiModalias: %s\n", spi->modalias);
-	PRINT_D(BUS_DBG, "spiMax-Speed: %d\n", spi->max_speed_hz);
-	wilc_spi_dev = spi;
-
-	printk("Driver Initializing success\n");
-	return 0;
-}
-
-static int __exit wilc_bus_remove(struct spi_device *spi)
-{
-
-	return 0;
-}
-
-#ifdef CONFIG_OF
-static const struct of_device_id wilc1000_of_match[] = {
-	{ .compatible = "atmel,wilc_spi", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, wilc1000_of_match);
-#endif
-
-struct spi_driver wilc_bus __refdata = {
-	.driver = {
-		.name = MODALIAS,
-#ifdef CONFIG_OF
-		.of_match_table = wilc1000_of_match,
-#endif
-	},
-	.probe =  wilc_bus_probe,
-	.remove = __exit_p(wilc_bus_remove),
-};
-
-
-void linux_spi_deinit(void *vp)
-{
-
-	spi_unregister_driver(&wilc_bus);
-
-	SPEED = MIN_SPEED;
-	PRINT_ER("@@@@@@@@@@@@ restore SPI speed to %d @@@@@@@@@\n", SPEED);
-
-}
-
-
-
-int linux_spi_init(void *vp)
-{
-	int ret = 1;
-	static int called;
-
-
-	if (called == 0) {
-		called++;
-		ret = spi_register_driver(&wilc_bus);
-	}
-
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-	return ret;
-}
-
-#if defined(PLAT_WMS8304)
-#define TXRX_PHASE_SIZE (4096)
-#endif
-
-#if defined(TXRX_PHASE_SIZE)
-
-int linux_spi_write(u8 *b, u32 len)
-{
-	int ret;
-
-	if (len > 0 && b != NULL) {
-		int i = 0;
-		int blk = len / TXRX_PHASE_SIZE;
-		int remainder = len % TXRX_PHASE_SIZE;
-
-		char *r_buffer = kzalloc(TXRX_PHASE_SIZE, GFP_KERNEL);
-		if (!r_buffer)
-			return -ENOMEM;
-
-		if (blk) {
-			while (i < blk)	{
-				struct spi_message msg;
-				struct spi_transfer tr = {
-					.tx_buf = b + (i * TXRX_PHASE_SIZE),
-					.len = TXRX_PHASE_SIZE,
-					.speed_hz = SPEED,
-					.bits_per_word = 8,
-					.delay_usecs = 0,
-				};
-
-				tr.rx_buf = r_buffer;
-
-				memset(&msg, 0, sizeof(msg));
-				spi_message_init(&msg);
-				msg.spi = wilc_spi_dev;
-				msg.is_dma_mapped = USE_SPI_DMA;
-
-				spi_message_add_tail(&tr, &msg);
-				ret = spi_sync(wilc_spi_dev, &msg);
-				if (ret < 0) {
-					PRINT_ER("SPI transaction failed\n");
-				}
-				i++;
-
-			}
-		}
-		if (remainder) {
-			struct spi_message msg;
-			struct spi_transfer tr = {
-				.tx_buf = b + (blk * TXRX_PHASE_SIZE),
-				.len = remainder,
-				.speed_hz = SPEED,
-				.bits_per_word = 8,
-				.delay_usecs = 0,
-			};
-			tr.rx_buf = r_buffer;
-
-			memset(&msg, 0, sizeof(msg));
-			spi_message_init(&msg);
-			msg.spi = wilc_spi_dev;
-			msg.is_dma_mapped = USE_SPI_DMA;                                /* rachel */
-
-			spi_message_add_tail(&tr, &msg);
-			ret = spi_sync(wilc_spi_dev, &msg);
-			if (ret < 0) {
-				PRINT_ER("SPI transaction failed\n");
-			}
-		}
-		kfree(r_buffer);
-	} else {
-		PRINT_ER("can't write data with the following length: %d\n", len);
-		PRINT_ER("FAILED due to NULL buffer or ZERO length check the following length: %d\n", len);
-		ret = -1;
-	}
-
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-	return ret;
-
-}
-
-#else
-int linux_spi_write(u8 *b, u32 len)
-{
-
-	int ret;
-	struct spi_message msg;
-
-	if (len > 0 && b != NULL) {
-		struct spi_transfer tr = {
-			.tx_buf = b,
-			.len = len,
-			.speed_hz = SPEED,
-			.delay_usecs = 0,
-		};
-		char *r_buffer = kzalloc(len, GFP_KERNEL);
-		if (!r_buffer)
-			return -ENOMEM;
-
-		tr.rx_buf = r_buffer;
-		PRINT_D(BUS_DBG, "Request writing %d bytes\n", len);
-
-		memset(&msg, 0, sizeof(msg));
-		spi_message_init(&msg);
-/* [[johnny add */
-		msg.spi = wilc_spi_dev;
-		msg.is_dma_mapped = USE_SPI_DMA;
-/* ]] */
-		spi_message_add_tail(&tr, &msg);
-
-		ret = spi_sync(wilc_spi_dev, &msg);
-		if (ret < 0) {
-			PRINT_ER("SPI transaction failed\n");
-		}
-
-		kfree(r_buffer);
-	} else {
-		PRINT_ER("can't write data with the following length: %d\n", len);
-		PRINT_ER("FAILED due to NULL buffer or ZERO length check the following length: %d\n", len);
-		ret = -1;
-	}
-
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-
-	return ret;
-}
-
-#endif
-
-#if defined(TXRX_PHASE_SIZE)
-
-int linux_spi_read(u8 *rb, u32 rlen)
-{
-	int ret;
-
-	if (rlen > 0) {
-		int i = 0;
-
-		int blk = rlen / TXRX_PHASE_SIZE;
-		int remainder = rlen % TXRX_PHASE_SIZE;
-
-		char *t_buffer = kzalloc(TXRX_PHASE_SIZE, GFP_KERNEL);
-		if (!t_buffer)
-			return -ENOMEM;
-
-		if (blk) {
-			while (i < blk)	{
-				struct spi_message msg;
-				struct spi_transfer tr = {
-					.rx_buf = rb + (i * TXRX_PHASE_SIZE),
-					.len = TXRX_PHASE_SIZE,
-					.speed_hz = SPEED,
-					.bits_per_word = 8,
-					.delay_usecs = 0,
-				};
-				tr.tx_buf = t_buffer;
-
-				memset(&msg, 0, sizeof(msg));
-				spi_message_init(&msg);
-				msg.spi = wilc_spi_dev;
-				msg.is_dma_mapped = USE_SPI_DMA;
-
-				spi_message_add_tail(&tr, &msg);
-				ret = spi_sync(wilc_spi_dev, &msg);
-				if (ret < 0) {
-					PRINT_ER("SPI transaction failed\n");
-				}
-				i++;
-			}
-		}
-		if (remainder) {
-			struct spi_message msg;
-			struct spi_transfer tr = {
-				.rx_buf = rb + (blk * TXRX_PHASE_SIZE),
-				.len = remainder,
-				.speed_hz = SPEED,
-				.bits_per_word = 8,
-				.delay_usecs = 0,
-			};
-			tr.tx_buf = t_buffer;
-
-			memset(&msg, 0, sizeof(msg));
-			spi_message_init(&msg);
-			msg.spi = wilc_spi_dev;
-			msg.is_dma_mapped = USE_SPI_DMA;                                /* rachel */
-
-			spi_message_add_tail(&tr, &msg);
-			ret = spi_sync(wilc_spi_dev, &msg);
-			if (ret < 0) {
-				PRINT_ER("SPI transaction failed\n");
-			}
-		}
-
-		kfree(t_buffer);
-	} else {
-		PRINT_ER("can't read data with the following length: %u\n", rlen);
-		ret = -1;
-	}
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-	return ret;
-}
-
-#else
-int linux_spi_read(u8 *rb, u32 rlen)
-{
-
-	int ret;
-
-	if (rlen > 0) {
-		struct spi_message msg;
-		struct spi_transfer tr = {
-			.rx_buf = rb,
-			.len = rlen,
-			.speed_hz = SPEED,
-			.delay_usecs = 0,
-
-		};
-		char *t_buffer = kzalloc(rlen, GFP_KERNEL);
-		if (!t_buffer)
-			return -ENOMEM;
-
-		tr.tx_buf = t_buffer;
-
-		memset(&msg, 0, sizeof(msg));
-		spi_message_init(&msg);
-/* [[ johnny add */
-		msg.spi = wilc_spi_dev;
-		msg.is_dma_mapped = USE_SPI_DMA;
-/* ]] */
-		spi_message_add_tail(&tr, &msg);
-
-		ret = spi_sync(wilc_spi_dev, &msg);
-		if (ret < 0) {
-			PRINT_ER("SPI transaction failed\n");
-		}
-		kfree(t_buffer);
-	} else {
-		PRINT_ER("can't read data with the following length: %u\n", rlen);
-		ret = -1;
-	}
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-	return ret;
-}
-
-#endif
-
-int linux_spi_write_read(u8 *wb, u8 *rb, u32 rlen)
-{
-
-	int ret;
-
-	if (rlen > 0) {
-		struct spi_message msg;
-		struct spi_transfer tr = {
-			.rx_buf = rb,
-			.tx_buf = wb,
-			.len = rlen,
-			.speed_hz = SPEED,
-			.bits_per_word = 8,
-			.delay_usecs = 0,
-
-		};
-
-		memset(&msg, 0, sizeof(msg));
-		spi_message_init(&msg);
-		msg.spi = wilc_spi_dev;
-		msg.is_dma_mapped = USE_SPI_DMA;
-
-		spi_message_add_tail(&tr, &msg);
-		ret = spi_sync(wilc_spi_dev, &msg);
-		if (ret < 0) {
-			PRINT_ER("SPI transaction failed\n");
-		}
-	} else {
-		PRINT_ER("can't read data with the following length: %u\n", rlen);
-		ret = -1;
-	}
-	/* change return value to match WILC interface */
-	(ret < 0) ? (ret = 0) : (ret = 1);
-
-	return ret;
-}
-
-int linux_spi_set_max_speed(void)
-{
-	SPEED = MAX_SPEED;
-
-	PRINT_INFO(BUS_DBG, "@@@@@@@@@@@@ change SPI speed to %d @@@@@@@@@\n", SPEED);
-	return 1;
-}
diff --git a/drivers/staging/wilc1000/linux_wlan_spi.h b/drivers/staging/wilc1000/linux_wlan_spi.h
deleted file mode 100644
index 7356785296f9..000000000000
--- a/drivers/staging/wilc1000/linux_wlan_spi.h
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef LINUX_WLAN_SPI_H
-#define LINUX_WLAN_SPI_H
-
-#include <linux/spi/spi.h>
-extern struct spi_device *wilc_spi_dev;
-extern struct spi_driver wilc_bus;
-
-int linux_spi_init(void *vp);
-void linux_spi_deinit(void *vp);
-int linux_spi_write(u8 *b, u32 len);
-int linux_spi_read(u8 *rb, u32 rlen);
-int linux_spi_write_read(u8 *wb, u8 *rb, u32 rlen);
-int linux_spi_set_max_speed(void);
-#endif
diff --git a/drivers/staging/wilc1000/wilc_debugfs.c b/drivers/staging/wilc1000/wilc_debugfs.c
index ae111862e7a9..27c653a0cdf9 100644
--- a/drivers/staging/wilc1000/wilc_debugfs.c
+++ b/drivers/staging/wilc1000/wilc_debugfs.c
@@ -26,8 +26,10 @@ static struct dentry *wilc_dir;
 
 #define DBG_REGION_ALL	(GENERIC_DBG | HOSTAPD_DBG | HOSTINF_DBG | CORECONFIG_DBG | CFG80211_DBG | INT_DBG | TX_DBG | RX_DBG | LOCK_DBG | INIT_DBG | BUS_DBG | MEM_DBG)
 #define DBG_LEVEL_ALL	(DEBUG | INFO | WRN | ERR)
-atomic_t REGION = ATOMIC_INIT(INIT_DBG | GENERIC_DBG | CFG80211_DBG | FIRM_DBG | HOSTAPD_DBG);
-atomic_t DEBUG_LEVEL = ATOMIC_INIT(ERR);
+atomic_t WILC_REGION = ATOMIC_INIT(INIT_DBG | GENERIC_DBG | CFG80211_DBG | FIRM_DBG | HOSTAPD_DBG);
+EXPORT_SYMBOL_GPL(WILC_REGION);
+atomic_t WILC_DEBUG_LEVEL = ATOMIC_INIT(ERR);
+EXPORT_SYMBOL_GPL(WILC_DEBUG_LEVEL);
 
 /*
  * --------------------------------------------------------------------------------
@@ -43,7 +45,7 @@ static ssize_t wilc_debug_level_read(struct file *file, char __user *userbuf, si
 	if (*ppos > 0)
 		return 0;
 
-	res = scnprintf(buf, sizeof(buf), "Debug Level: %x\n", atomic_read(&DEBUG_LEVEL));
+	res = scnprintf(buf, sizeof(buf), "Debug Level: %x\n", atomic_read(&WILC_DEBUG_LEVEL));
 
 	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
 }
@@ -59,11 +61,11 @@ static ssize_t wilc_debug_level_write(struct file *filp, const char __user *buf,
 		return ret;
 
 	if (flag > DBG_LEVEL_ALL) {
-		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&DEBUG_LEVEL));
+		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&WILC_DEBUG_LEVEL));
 		return -EINVAL;
 	}
 
-	atomic_set(&DEBUG_LEVEL, (int)flag);
+	atomic_set(&WILC_DEBUG_LEVEL, (int)flag);
 
 	if (flag == 0)
 		printk("Debug-level disabled\n");
@@ -82,7 +84,7 @@ static ssize_t wilc_debug_region_read(struct file *file, char __user *userbuf, s
 	if (*ppos > 0)
 		return 0;
 
-	res = scnprintf(buf, sizeof(buf), "Debug region: %x\n", atomic_read(&REGION));
+	res = scnprintf(buf, sizeof(buf), "Debug region: %x\n", atomic_read(&WILC_REGION));
 
 	return simple_read_from_buffer(userbuf, count, ppos, buf, res);
 }
@@ -102,12 +104,12 @@ static ssize_t wilc_debug_region_write(struct file *filp, const char *buf, size_
 	flag = buffer[0] - '0';
 
 	if (flag > DBG_REGION_ALL) {
-		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&REGION));
+		printk("%s, value (0x%08x) is out of range, stay previous flag (0x%08x)\n", __func__, flag, atomic_read(&WILC_REGION));
 		return -EFAULT;
 	}
 
-	atomic_set(&REGION, (int)flag);
-	printk("new debug-region is %x\n", atomic_read(&REGION));
+	atomic_set(&WILC_REGION, (int)flag);
+	printk("new debug-region is %x\n", atomic_read(&WILC_REGION));
 
 	return count;
 }
@@ -136,7 +138,7 @@ static struct wilc_debugfs_info_t debugfs_info[] = {
 	{ "wilc_debug_region",	0666,	(INIT_DBG | GENERIC_DBG | CFG80211_DBG), FOPS(NULL, wilc_debug_region_read, wilc_debug_region_write, NULL), },
 };
 
-int wilc_debugfs_init(void)
+static int __init wilc_debugfs_init(void)
 {
 	int i;
 
@@ -171,11 +173,13 @@ int wilc_debugfs_init(void)
 	}
 	return 0;
 }
+module_init(wilc_debugfs_init);
 
-void wilc_debugfs_remove(void)
+static void __exit wilc_debugfs_remove(void)
 {
 	debugfs_remove_recursive(wilc_dir);
 }
+module_exit(wilc_debugfs_remove);
 
 #endif
 
diff --git a/drivers/staging/wilc1000/wilc_msgqueue.c b/drivers/staging/wilc1000/wilc_msgqueue.c
index 0eff121b8291..098390cdf319 100644
--- a/drivers/staging/wilc1000/wilc_msgqueue.c
+++ b/drivers/staging/wilc1000/wilc_msgqueue.c
@@ -115,7 +115,6 @@ int wilc_mq_recv(WILC_MsgQueueHandle *pHandle,
 			     u32 *pu32ReceivedLength)
 {
 	Message *pstrMessage;
-	int result = 0;
 	unsigned long flags;
 
 	if ((!pHandle) || (u32RecvBufferSize == 0)
@@ -135,12 +134,6 @@ int wilc_mq_recv(WILC_MsgQueueHandle *pHandle,
 
 	down(&pHandle->hSem);
 
-	/* other non-timeout scenarios */
-	if (result) {
-		PRINT_ER("Non-timeout\n");
-		return result;
-	}
-
 	if (pHandle->bExiting) {
 		PRINT_ER("pHandle fail\n");
 		return -EFAULT;
@@ -174,5 +167,5 @@ int wilc_mq_recv(WILC_MsgQueueHandle *pHandle,
 
 	spin_unlock_irqrestore(&pHandle->strCriticalSection, flags);
 
-	return result;
+	return 0;
 }
diff --git a/drivers/staging/wilc1000/wilc_msgqueue.h b/drivers/staging/wilc1000/wilc_msgqueue.h
index d231c334ed93..d7e0328bacee 100644
--- a/drivers/staging/wilc1000/wilc_msgqueue.h
+++ b/drivers/staging/wilc1000/wilc_msgqueue.h
@@ -35,7 +35,7 @@ typedef struct __MessageQueue_struct {
  *                              any other message queue having the same name in the system
  *  @param[in,out]	pHandle handle to the message queue object
  *  @param[in]	pstrAttrs Optional attributes, NULL for default
- *  @return		Error code indicating sucess/failure
+ *  @return		Error code indicating success/failure
  *  @author		syounan
  *  @date		30 Aug 2010
  *  @version		1.0
@@ -44,7 +44,7 @@ int wilc_mq_create(WILC_MsgQueueHandle *pHandle);
 
 /*!
  *  @brief		Sends a message
- *  @details		Sends a message, this API will block unil the message is
+ *  @details		Sends a message, this API will block until the message is
  *                              actually sent or until it is timedout (as long as the feature
  *                              CONFIG_WILC_MSG_QUEUE_TIMEOUT is enabled and pstrAttrs->u32Timeout
  *                              is not set to WILC_OS_INFINITY), zero timeout is a valid value
@@ -52,7 +52,7 @@ int wilc_mq_create(WILC_MsgQueueHandle *pHandle);
  *  @param[in]	pvSendBuffer pointer to the data to send
  *  @param[in]	u32SendBufferSize the size of the data to send
  *  @param[in]	pstrAttrs Optional attributes, NULL for default
- *  @return		Error code indicating sucess/failure
+ *  @return		Error code indicating success/failure
  *  @author		syounan
  *  @date		30 Aug 2010
  *  @version		1.0
@@ -62,7 +62,7 @@ int wilc_mq_send(WILC_MsgQueueHandle *pHandle,
 
 /*!
  *  @brief		Receives a message
- *  @details		Receives a message, this API will block unil a message is
+ *  @details		Receives a message, this API will block until a message is
  *                              received or until it is timedout (as long as the feature
  *                              CONFIG_WILC_MSG_QUEUE_TIMEOUT is enabled and pstrAttrs->u32Timeout
  *                              is not set to WILC_OS_INFINITY), zero timeout is a valid value
@@ -71,7 +71,7 @@ int wilc_mq_send(WILC_MsgQueueHandle *pHandle,
  *  @param[in]	u32RecvBufferSize the size of the receive buffer
  *  @param[out]	pu32ReceivedLength the length of received data
  *  @param[in]	pstrAttrs Optional attributes, NULL for default
- *  @return		Error code indicating sucess/failure
+ *  @return		Error code indicating success/failure
  *  @author		syounan
  *  @date		30 Aug 2010
  *  @version		1.0
@@ -84,7 +84,7 @@ int wilc_mq_recv(WILC_MsgQueueHandle *pHandle,
  *  @brief		Destroys an existing  Message queue
  *  @param[in]	pHandle handle to the message queue object
  *  @param[in]	pstrAttrs Optional attributes, NULL for default
- *  @return		Error code indicating sucess/failure
+ *  @return		Error code indicating success/failure
  *  @author		syounan
  *  @date		30 Aug 2010
  *  @version		1.0
diff --git a/drivers/staging/wilc1000/wilc_sdio.c b/drivers/staging/wilc1000/wilc_sdio.c
index 300c571e4e2d..e961b5004902 100644
--- a/drivers/staging/wilc1000/wilc_sdio.c
+++ b/drivers/staging/wilc1000/wilc_sdio.c
@@ -10,17 +10,29 @@
 #include <linux/string.h>
 #include "wilc_wlan_if.h"
 #include "wilc_wlan.h"
+#include "wilc_wfi_netdevice.h"
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio.h>
+#include <linux/mmc/host.h>
+#include <linux/of_gpio.h>
+
+#define SDIO_MODALIAS "wilc1000_sdio"
+
+#define SDIO_VENDOR_ID_WILC 0x0296
+#define SDIO_DEVICE_ID_WILC 0x5347
+
+static const struct sdio_device_id wilc_sdio_ids[] = {
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_WILC, SDIO_DEVICE_ID_WILC) },
+	{ },
+};
 
 #define WILC_SDIO_BLOCK_SIZE 512
 
 typedef struct {
-	void *os_context;
+	bool irq_gpio;
 	u32 block_size;
-	int (*sdio_cmd52)(sdio_cmd52_t *);
-	int (*sdio_cmd53)(sdio_cmd53_t *);
-	int (*sdio_set_max_speed)(void);
-	int (*sdio_set_default_speed)(void);
-	wilc_debug_func dPrint;
 	int nint;
 #define MAX_NUN_INT_THRPT_ENH2 (5) /* Max num interrupts allowed in registers 0xf7, 0xf8 */
 	int has_thrpt_enh3;
@@ -28,10 +40,155 @@ typedef struct {
 
 static wilc_sdio_t g_sdio;
 
-#ifdef WILC_SDIO_IRQ_GPIO
-static int sdio_write_reg(u32 addr, u32 data);
-static int sdio_read_reg(u32 addr, u32 *data);
-#endif
+static int sdio_write_reg(struct wilc *wilc, u32 addr, u32 data);
+static int sdio_read_reg(struct wilc *wilc, u32 addr, u32 *data);
+
+static void wilc_sdio_interrupt(struct sdio_func *func)
+{
+	sdio_release_host(func);
+	wilc_handle_isr(sdio_get_drvdata(func));
+	sdio_claim_host(func);
+}
+
+static int wilc_sdio_cmd52(struct wilc *wilc, sdio_cmd52_t *cmd)
+{
+	struct sdio_func *func = container_of(wilc->dev, struct sdio_func, dev);
+	int ret;
+	u8 data;
+
+	sdio_claim_host(func);
+
+	func->num = cmd->function;
+	if (cmd->read_write) {  /* write */
+		if (cmd->raw) {
+			sdio_writeb(func, cmd->data, cmd->address, &ret);
+			data = sdio_readb(func, cmd->address, &ret);
+			cmd->data = data;
+		} else {
+			sdio_writeb(func, cmd->data, cmd->address, &ret);
+		}
+	} else {        /* read */
+		data = sdio_readb(func, cmd->address, &ret);
+		cmd->data = data;
+	}
+
+	sdio_release_host(func);
+
+	if (ret)
+		dev_err(&func->dev, "wilc_sdio_cmd52..failed, err(%d)\n", ret);
+	return ret;
+}
+
+
+static int wilc_sdio_cmd53(struct wilc *wilc, sdio_cmd53_t *cmd)
+{
+	struct sdio_func *func = container_of(wilc->dev, struct sdio_func, dev);
+	int size, ret;
+
+	sdio_claim_host(func);
+
+	func->num = cmd->function;
+	func->cur_blksize = cmd->block_size;
+	if (cmd->block_mode)
+		size = cmd->count * cmd->block_size;
+	else
+		size = cmd->count;
+
+	if (cmd->read_write) {  /* write */
+		ret = sdio_memcpy_toio(func, cmd->address,
+				       (void *)cmd->buffer, size);
+	} else {        /* read */
+		ret = sdio_memcpy_fromio(func, (void *)cmd->buffer,
+					 cmd->address,  size);
+	}
+
+	sdio_release_host(func);
+
+	if (ret)
+		dev_err(&func->dev, "wilc_sdio_cmd53..failed, err(%d)\n", ret);
+
+	return ret;
+}
+
+static int linux_sdio_probe(struct sdio_func *func,
+			    const struct sdio_device_id *id)
+{
+	struct wilc *wilc;
+	int gpio, ret;
+
+	gpio = -1;
+	if (IS_ENABLED(CONFIG_WILC1000_HW_OOB_INTR)) {
+		gpio = of_get_gpio(func->dev.of_node, 0);
+		if (gpio < 0)
+			gpio = GPIO_NUM;
+	}
+
+	dev_dbg(&func->dev, "Initializing netdev\n");
+	ret = wilc_netdev_init(&wilc, &func->dev, HIF_SDIO, gpio,
+			     &wilc_hif_sdio);
+	if (ret) {
+		dev_err(&func->dev, "Couldn't initialize netdev\n");
+		return ret;
+	}
+	sdio_set_drvdata(func, wilc);
+	wilc->dev = &func->dev;
+
+	dev_info(&func->dev, "Driver Initializing success\n");
+	return 0;
+}
+
+static void linux_sdio_remove(struct sdio_func *func)
+{
+	wilc_netdev_cleanup(sdio_get_drvdata(func));
+}
+
+static struct sdio_driver wilc1000_sdio_driver = {
+	.name		= SDIO_MODALIAS,
+	.id_table	= wilc_sdio_ids,
+	.probe		= linux_sdio_probe,
+	.remove		= linux_sdio_remove,
+};
+module_driver(wilc1000_sdio_driver,
+	      sdio_register_driver,
+	      sdio_unregister_driver);
+MODULE_LICENSE("GPL");
+
+static int wilc_sdio_enable_interrupt(struct wilc *dev)
+{
+	struct sdio_func *func = container_of(dev->dev, struct sdio_func, dev);
+	int ret = 0;
+
+	sdio_claim_host(func);
+	ret = sdio_claim_irq(func, wilc_sdio_interrupt);
+	sdio_release_host(func);
+
+	if (ret < 0) {
+		dev_err(&func->dev, "can't claim sdio_irq, err(%d)\n", ret);
+		ret = -EIO;
+	}
+	return ret;
+}
+
+static void wilc_sdio_disable_interrupt(struct wilc *dev)
+{
+	struct sdio_func *func = container_of(dev->dev, struct sdio_func, dev);
+	int ret;
+
+	dev_dbg(&func->dev, "wilc_sdio_disable_interrupt IN\n");
+
+	sdio_claim_host(func);
+	ret = sdio_release_irq(func);
+	if (ret < 0)
+		dev_err(&func->dev, "can't release sdio_irq, err(%d)\n", ret);
+	sdio_release_host(func);
+
+	dev_info(&func->dev, "wilc_sdio_disable_interrupt OUT\n");
+}
+
+static int wilc_sdio_init(void)
+{
+	return 1;
+}
 
 /********************************************
  *
@@ -39,9 +196,11 @@ static int sdio_read_reg(u32 addr, u32 *data);
  *
  ********************************************/
 
-static int sdio_set_func0_csa_address(u32 adr)
+static int sdio_set_func0_csa_address(struct wilc *wilc, u32 adr)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	sdio_cmd52_t cmd;
+	int ret;
 
 	/**
 	 *      Review: BIG ENDIAN
@@ -51,22 +210,25 @@ static int sdio_set_func0_csa_address(u32 adr)
 	cmd.raw = 0;
 	cmd.address = 0x10c;
 	cmd.data = (u8)adr;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10c data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x10c data...\n");
 		goto _fail_;
 	}
 
 	cmd.address = 0x10d;
 	cmd.data = (u8)(adr >> 8);
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10d data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x10d data...\n");
 		goto _fail_;
 	}
 
 	cmd.address = 0x10e;
 	cmd.data = (u8)(adr >> 16);
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10e data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x10e data...\n");
 		goto _fail_;
 	}
 
@@ -75,24 +237,28 @@ _fail_:
 	return 0;
 }
 
-static int sdio_set_func0_block_size(u32 block_size)
+static int sdio_set_func0_block_size(struct wilc *wilc, u32 block_size)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	sdio_cmd52_t cmd;
+	int ret;
 
 	cmd.read_write = 1;
 	cmd.function = 0;
 	cmd.raw = 0;
 	cmd.address = 0x10;
 	cmd.data = (u8)block_size;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x10 data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x10 data...\n");
 		goto _fail_;
 	}
 
 	cmd.address = 0x11;
 	cmd.data = (u8)(block_size >> 8);
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x11 data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x11 data...\n");
 		goto _fail_;
 	}
 
@@ -107,23 +273,27 @@ _fail_:
  *
  ********************************************/
 
-static int sdio_set_func1_block_size(u32 block_size)
+static int sdio_set_func1_block_size(struct wilc *wilc, u32 block_size)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	sdio_cmd52_t cmd;
+	int ret;
 
 	cmd.read_write = 1;
 	cmd.function = 0;
 	cmd.raw = 0;
 	cmd.address = 0x110;
 	cmd.data = (u8)block_size;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x110 data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x110 data...\n");
 		goto _fail_;
 	}
 	cmd.address = 0x111;
 	cmd.data = (u8)(block_size >> 8);
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0x111 data...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Failed cmd52, set 0x111 data...\n");
 		goto _fail_;
 	}
 
@@ -132,100 +302,17 @@ _fail_:
 	return 0;
 }
 
-static int sdio_clear_int(void)
-{
-#ifndef WILC_SDIO_IRQ_GPIO
-	/* u32 sts; */
-	sdio_cmd52_t cmd;
-
-	cmd.read_write = 0;
-	cmd.function = 1;
-	cmd.raw = 0;
-	cmd.address = 0x4;
-	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
-
-	return cmd.data;
-#else
-	u32 reg;
-
-	if (!sdio_read_reg(WILC_HOST_RX_CTRL_0, &reg)) {
-		g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_HOST_RX_CTRL_0);
-		return 0;
-	}
-	reg &= ~0x1;
-	sdio_write_reg(WILC_HOST_RX_CTRL_0, reg);
-	return 1;
-#endif
-
-}
-
-u32 sdio_xfer_cnt(void)
-{
-	u32 cnt = 0;
-	sdio_cmd52_t cmd;
-
-	cmd.read_write = 0;
-	cmd.function = 1;
-	cmd.raw = 0;
-	cmd.address = 0x1C;
-	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
-	cnt = cmd.data;
-
-	cmd.read_write = 0;
-	cmd.function = 1;
-	cmd.raw = 0;
-	cmd.address = 0x1D;
-	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
-	cnt |= (cmd.data << 8);
-
-	cmd.read_write = 0;
-	cmd.function = 1;
-	cmd.raw = 0;
-	cmd.address = 0x1E;
-	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
-	cnt |= (cmd.data << 16);
-
-	return cnt;
-}
-
 /********************************************
  *
  *      Sdio interfaces
  *
  ********************************************/
-int sdio_check_bs(void)
+static int sdio_write_reg(struct wilc *wilc, u32 addr, u32 data)
 {
-	sdio_cmd52_t cmd;
-
-	/**
-	 *      poll until BS is 0
-	 **/
-	cmd.read_write = 0;
-	cmd.function = 0;
-	cmd.raw = 0;
-	cmd.address = 0xc;
-	cmd.data = 0;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, get BS register...\n");
-		goto _fail_;
-	}
-
-	return 1;
-
-_fail_:
-
-	return 0;
-}
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
+	int ret;
 
-static int sdio_write_reg(u32 addr, u32 data)
-{
-#ifdef BIG_ENDIAN
-	data = BYTE_SWAP(data);
-#endif
+	data = cpu_to_le32(data);
 
 	if ((addr >= 0xf0) && (addr <= 0xff)) {
 		sdio_cmd52_t cmd;
@@ -235,8 +322,10 @@ static int sdio_write_reg(u32 addr, u32 data)
 		cmd.raw = 0;
 		cmd.address = addr;
 		cmd.data = data;
-		if (!g_sdio.sdio_cmd52(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd 52, read reg (%08x) ...\n", addr);
+		ret = wilc_sdio_cmd52(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd 52, read reg (%08x) ...\n", addr);
 			goto _fail_;
 		}
 	} else {
@@ -245,7 +334,7 @@ static int sdio_write_reg(u32 addr, u32 data)
 		/**
 		 *      set the AHB address
 		 **/
-		if (!sdio_set_func0_csa_address(addr))
+		if (!sdio_set_func0_csa_address(wilc, addr))
 			goto _fail_;
 
 		cmd.read_write = 1;
@@ -256,9 +345,10 @@ static int sdio_write_reg(u32 addr, u32 data)
 		cmd.count = 4;
 		cmd.buffer = (u8 *)&data;
 		cmd.block_size = g_sdio.block_size; /* johnny : prevent it from setting unexpected value */
-
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53, write reg (%08x)...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53, write reg (%08x)...\n", addr);
 			goto _fail_;
 		}
 	}
@@ -270,11 +360,12 @@ _fail_:
 	return 0;
 }
 
-static int sdio_write(u32 addr, u8 *buf, u32 size)
+static int sdio_write(struct wilc *wilc, u32 addr, u8 *buf, u32 size)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	u32 block_size = g_sdio.block_size;
 	sdio_cmd53_t cmd;
-	int nblk, nleft;
+	int nblk, nleft, ret;
 
 	cmd.read_write = 1;
 	if (addr > 0) {
@@ -317,11 +408,13 @@ static int sdio_write(u32 addr, u8 *buf, u32 size)
 		cmd.buffer = buf;
 		cmd.block_size = block_size;
 		if (addr > 0) {
-			if (!sdio_set_func0_csa_address(addr))
+			if (!sdio_set_func0_csa_address(wilc, addr))
 				goto _fail_;
 		}
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block send...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53 [%x], block send...\n", addr);
 			goto _fail_;
 		}
 		if (addr > 0)
@@ -338,11 +431,13 @@ static int sdio_write(u32 addr, u8 *buf, u32 size)
 		cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
 
 		if (addr > 0) {
-			if (!sdio_set_func0_csa_address(addr))
+			if (!sdio_set_func0_csa_address(wilc, addr))
 				goto _fail_;
 		}
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], bytes send...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53 [%x], bytes send...\n", addr);
 			goto _fail_;
 		}
 	}
@@ -354,8 +449,11 @@ _fail_:
 	return 0;
 }
 
-static int sdio_read_reg(u32 addr, u32 *data)
+static int sdio_read_reg(struct wilc *wilc, u32 addr, u32 *data)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
+	int ret;
+
 	if ((addr >= 0xf0) && (addr <= 0xff)) {
 		sdio_cmd52_t cmd;
 
@@ -363,15 +461,17 @@ static int sdio_read_reg(u32 addr, u32 *data)
 		cmd.function = 0;
 		cmd.raw = 0;
 		cmd.address = addr;
-		if (!g_sdio.sdio_cmd52(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd 52, read reg (%08x) ...\n", addr);
+		ret = wilc_sdio_cmd52(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd 52, read reg (%08x) ...\n", addr);
 			goto _fail_;
 		}
 		*data = cmd.data;
 	} else {
 		sdio_cmd53_t cmd;
 
-		if (!sdio_set_func0_csa_address(addr))
+		if (!sdio_set_func0_csa_address(wilc, addr))
 			goto _fail_;
 
 		cmd.read_write = 0;
@@ -383,16 +483,15 @@ static int sdio_read_reg(u32 addr, u32 *data)
 		cmd.buffer = (u8 *)data;
 
 		cmd.block_size = g_sdio.block_size; /* johnny : prevent it from setting unexpected value */
-
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53, read reg (%08x)...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53, read reg (%08x)...\n", addr);
 			goto _fail_;
 		}
 	}
 
-#ifdef BIG_ENDIAN
-	*data = BYTE_SWAP(*data);
-#endif
+	*data = cpu_to_le32(*data);
 
 	return 1;
 
@@ -401,11 +500,12 @@ _fail_:
 	return 0;
 }
 
-static int sdio_read(u32 addr, u8 *buf, u32 size)
+static int sdio_read(struct wilc *wilc, u32 addr, u8 *buf, u32 size)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	u32 block_size = g_sdio.block_size;
 	sdio_cmd53_t cmd;
-	int nblk, nleft;
+	int nblk, nleft, ret;
 
 	cmd.read_write = 0;
 	if (addr > 0) {
@@ -448,11 +548,13 @@ static int sdio_read(u32 addr, u8 *buf, u32 size)
 		cmd.buffer = buf;
 		cmd.block_size = block_size;
 		if (addr > 0) {
-			if (!sdio_set_func0_csa_address(addr))
+			if (!sdio_set_func0_csa_address(wilc, addr))
 				goto _fail_;
 		}
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], block read...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53 [%x], block read...\n", addr);
 			goto _fail_;
 		}
 		if (addr > 0)
@@ -469,11 +571,13 @@ static int sdio_read(u32 addr, u8 *buf, u32 size)
 		cmd.block_size = block_size; /* johnny : prevent it from setting unexpected value */
 
 		if (addr > 0) {
-			if (!sdio_set_func0_csa_address(addr))
+			if (!sdio_set_func0_csa_address(wilc, addr))
 				goto _fail_;
 		}
-		if (!g_sdio.sdio_cmd53(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd53 [%x], bytes read...\n", addr);
+		ret = wilc_sdio_cmd53(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Failed cmd53 [%x], bytes read...\n", addr);
 			goto _fail_;
 		}
 	}
@@ -491,94 +595,29 @@ _fail_:
  *
  ********************************************/
 
-static int sdio_deinit(void *pv)
+static int sdio_deinit(struct wilc *wilc)
 {
 	return 1;
 }
 
-static int sdio_sync(void)
-{
-	u32 reg;
-
-	/**
-	 *      Disable power sequencer
-	 **/
-	if (!sdio_read_reg(WILC_MISC, &reg)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read misc reg...\n");
-		return 0;
-	}
-
-	reg &= ~BIT(8);
-	if (!sdio_write_reg(WILC_MISC, reg)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write misc reg...\n");
-		return 0;
-	}
-
-#ifdef WILC_SDIO_IRQ_GPIO
-	{
-		u32 reg;
-		int ret;
-
-		/**
-		 *      interrupt pin mux select
-		 **/
-		ret = sdio_read_reg(WILC_PIN_MUX_0, &reg);
-		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
-			return 0;
-		}
-		reg |= BIT(8);
-		ret = sdio_write_reg(WILC_PIN_MUX_0, reg);
-		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
-			return 0;
-		}
-
-		/**
-		 *      interrupt enable
-		 **/
-		ret = sdio_read_reg(WILC_INTR_ENABLE, &reg);
-		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
-			return 0;
-		}
-		reg |= BIT(16);
-		ret = sdio_write_reg(WILC_INTR_ENABLE, reg);
-		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
-			return 0;
-		}
-	}
-#endif
-
-	return 1;
-}
-
-static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
+static int sdio_init(struct wilc *wilc)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	sdio_cmd52_t cmd;
-	int loop;
+	int loop, ret;
 	u32 chipid;
 
 	memset(&g_sdio, 0, sizeof(wilc_sdio_t));
 
-	g_sdio.dPrint = func;
-	g_sdio.os_context = inp->os_context.os_private;
+	g_sdio.irq_gpio = (wilc->dev_irq_num);
 
-	if (inp->io_func.io_init) {
-		if (!inp->io_func.io_init(g_sdio.os_context)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed io init bus...\n");
-			return 0;
-		}
+	if (!wilc_sdio_init()) {
+		dev_err(&func->dev, "Failed io init bus...\n");
+		return 0;
 	} else {
 		return 0;
 	}
 
-	g_sdio.sdio_cmd52	= inp->io_func.u.sdio.sdio_cmd52;
-	g_sdio.sdio_cmd53	= inp->io_func.u.sdio.sdio_cmd53;
-	g_sdio.sdio_set_max_speed	= inp->io_func.u.sdio.sdio_set_max_speed;
-	g_sdio.sdio_set_default_speed	= inp->io_func.u.sdio.sdio_set_default_speed;
-
 	/**
 	 *      function 0 csa enable
 	 **/
@@ -587,16 +626,17 @@ static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	cmd.raw = 1;
 	cmd.address = 0x100;
 	cmd.data = 0x80;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, enable csa...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Fail cmd 52, enable csa...\n");
 		goto _fail_;
 	}
 
 	/**
 	 *      function 0 block size
 	 **/
-	if (!sdio_set_func0_block_size(WILC_SDIO_BLOCK_SIZE)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, set func 0 block size...\n");
+	if (!sdio_set_func0_block_size(wilc, WILC_SDIO_BLOCK_SIZE)) {
+		dev_err(&func->dev, "Fail cmd 52, set func 0 block size...\n");
 		goto _fail_;
 	}
 	g_sdio.block_size = WILC_SDIO_BLOCK_SIZE;
@@ -609,8 +649,10 @@ static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	cmd.raw = 1;
 	cmd.address = 0x2;
 	cmd.data = 0x2;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio] Fail cmd 52, set IOE register...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev,
+			"Fail cmd 52, set IOE register...\n");
 		goto _fail_;
 	}
 
@@ -624,8 +666,10 @@ static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	loop = 3;
 	do {
 		cmd.data = 0;
-		if (!g_sdio.sdio_cmd52(&cmd)) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, get IOR register...\n");
+		ret = wilc_sdio_cmd52(wilc, &cmd);
+		if (ret) {
+			dev_err(&func->dev,
+				"Fail cmd 52, get IOR register...\n");
 			goto _fail_;
 		}
 		if (cmd.data == 0x2)
@@ -633,15 +677,15 @@ static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	} while (loop--);
 
 	if (loop <= 0) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail func 1 is not ready...\n");
+		dev_err(&func->dev, "Fail func 1 is not ready...\n");
 		goto _fail_;
 	}
 
 	/**
 	 *      func 1 is ready, set func 1 block size
 	 **/
-	if (!sdio_set_func1_block_size(WILC_SDIO_BLOCK_SIZE)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail set func 1 block size...\n");
+	if (!sdio_set_func1_block_size(wilc, WILC_SDIO_BLOCK_SIZE)) {
+		dev_err(&func->dev, "Fail set func 1 block size...\n");
 		goto _fail_;
 	}
 
@@ -653,24 +697,25 @@ static int sdio_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	cmd.raw = 1;
 	cmd.address = 0x4;
 	cmd.data = 0x3;
-	if (!g_sdio.sdio_cmd52(&cmd)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd 52, set IEN register...\n");
+	ret = wilc_sdio_cmd52(wilc, &cmd);
+	if (ret) {
+		dev_err(&func->dev, "Fail cmd 52, set IEN register...\n");
 		goto _fail_;
 	}
 
 	/**
 	 *      make sure can read back chip id correctly
 	 **/
-	if (!sdio_read_reg(0x1000, &chipid)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Fail cmd read chip id...\n");
+	if (!sdio_read_reg(wilc, 0x1000, &chipid)) {
+		dev_err(&func->dev, "Fail cmd read chip id...\n");
 		goto _fail_;
 	}
-	g_sdio.dPrint(N_ERR, "[wilc sdio]: chipid (%08x)\n", chipid);
+	dev_err(&func->dev, "chipid (%08x)\n", chipid);
 	if ((chipid & 0xfff) > 0x2a0)
 		g_sdio.has_thrpt_enh3 = 1;
 	else
 		g_sdio.has_thrpt_enh3 = 0;
-	g_sdio.dPrint(N_ERR, "[wilc sdio]: has_thrpt_enh3 = %d...\n", g_sdio.has_thrpt_enh3);
+	dev_info(&func->dev, "has_thrpt_enh3 = %d...\n", g_sdio.has_thrpt_enh3);
 
 	return 1;
 
@@ -679,19 +724,8 @@ _fail_:
 	return 0;
 }
 
-static void sdio_set_max_speed(void)
+static int sdio_read_size(struct wilc *wilc, u32 *size)
 {
-	g_sdio.sdio_set_max_speed();
-}
-
-static void sdio_set_default_speed(void)
-{
-	g_sdio.sdio_set_default_speed();
-}
-
-static int sdio_read_size(u32 *size)
-{
-
 	u32 tmp;
 	sdio_cmd52_t cmd;
 
@@ -703,7 +737,7 @@ static int sdio_read_size(u32 *size)
 	cmd.raw = 0;
 	cmd.address = 0xf2;
 	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
+	wilc_sdio_cmd52(wilc, &cmd);
 	tmp = cmd.data;
 
 	/* cmd.read_write = 0; */
@@ -711,54 +745,53 @@ static int sdio_read_size(u32 *size)
 	/* cmd.raw = 0; */
 	cmd.address = 0xf3;
 	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
+	wilc_sdio_cmd52(wilc, &cmd);
 	tmp |= (cmd.data << 8);
 
 	*size = tmp;
 	return 1;
 }
 
-static int sdio_read_int(u32 *int_status)
+static int sdio_read_int(struct wilc *wilc, u32 *int_status)
 {
-
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	u32 tmp;
 	sdio_cmd52_t cmd;
 
-	sdio_read_size(&tmp);
+	sdio_read_size(wilc, &tmp);
 
 	/**
 	 *      Read IRQ flags
 	 **/
-#ifndef WILC_SDIO_IRQ_GPIO
-	cmd.function = 1;
-	cmd.address = 0x04;
-	cmd.data = 0;
-	g_sdio.sdio_cmd52(&cmd);
-
-	if (cmd.data & BIT(0))
-		tmp |= INT_0;
-	if (cmd.data & BIT(2))
-		tmp |= INT_1;
-	if (cmd.data & BIT(3))
-		tmp |= INT_2;
-	if (cmd.data & BIT(4))
-		tmp |= INT_3;
-	if (cmd.data & BIT(5))
-		tmp |= INT_4;
-	if (cmd.data & BIT(6))
-		tmp |= INT_5;
-	{
+	if (!g_sdio.irq_gpio) {
 		int i;
 
+		cmd.function = 1;
+		cmd.address = 0x04;
+		cmd.data = 0;
+		wilc_sdio_cmd52(wilc, &cmd);
+
+		if (cmd.data & BIT(0))
+			tmp |= INT_0;
+		if (cmd.data & BIT(2))
+			tmp |= INT_1;
+		if (cmd.data & BIT(3))
+			tmp |= INT_2;
+		if (cmd.data & BIT(4))
+			tmp |= INT_3;
+		if (cmd.data & BIT(5))
+			tmp |= INT_4;
+		if (cmd.data & BIT(6))
+			tmp |= INT_5;
 		for (i = g_sdio.nint; i < MAX_NUM_INT; i++) {
 			if ((tmp >> (IRG_FLAGS_OFFSET + i)) & 0x1) {
-				g_sdio.dPrint(N_ERR, "[wilc sdio]: Unexpected interrupt (1) : tmp=%x, data=%x\n", tmp, cmd.data);
+				dev_err(&func->dev,
+					"Unexpected interrupt (1) : tmp=%x, data=%x\n",
+					tmp, cmd.data);
 				break;
 			}
 		}
-	}
-#else
-	{
+	} else {
 		u32 irq_flags;
 
 		cmd.read_write = 0;
@@ -766,35 +799,32 @@ static int sdio_read_int(u32 *int_status)
 		cmd.raw = 0;
 		cmd.address = 0xf7;
 		cmd.data = 0;
-		g_sdio.sdio_cmd52(&cmd);
+		wilc_sdio_cmd52(wilc, &cmd);
 		irq_flags = cmd.data & 0x1f;
 		tmp |= ((irq_flags >> 0) << IRG_FLAGS_OFFSET);
 	}
 
-#endif
-
 	*int_status = tmp;
 
 	return 1;
 }
 
-static int sdio_clear_int_ext(u32 val)
+static int sdio_clear_int_ext(struct wilc *wilc, u32 val)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	int ret;
 
 	if (g_sdio.has_thrpt_enh3) {
 		u32 reg;
 
-#ifdef WILC_SDIO_IRQ_GPIO
-		{
+		if (g_sdio.irq_gpio) {
 			u32 flags;
 
 			flags = val & (BIT(MAX_NUN_INT_THRPT_ENH2) - 1);
 			reg = flags;
+		} else {
+			reg = 0;
 		}
-#else
-		reg = 0;
-#endif
 		/* select VMM table 0 */
 		if ((val & SEL_VMM_TBL0) == SEL_VMM_TBL0)
 			reg |= BIT(5);
@@ -813,16 +843,17 @@ static int sdio_clear_int_ext(u32 val)
 			cmd.address = 0xf8;
 			cmd.data = reg;
 
-			ret = g_sdio.sdio_cmd52(&cmd);
-			if (!ret) {
-				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf8 data (%d) ...\n", __LINE__);
+			ret = wilc_sdio_cmd52(wilc, &cmd);
+			if (ret) {
+				dev_err(&func->dev,
+					"Failed cmd52, set 0xf8 data (%d) ...\n",
+					__LINE__);
 				goto _fail_;
 			}
 
 		}
 	} else {
-#ifdef WILC_SDIO_IRQ_GPIO
-		{
+		if (g_sdio.irq_gpio) {
 			/* see below. has_thrpt_enh2 uses register 0xf8 to clear interrupts. */
 			/* Cannot clear multiple interrupts. Must clear each interrupt individually */
 			u32 flags;
@@ -842,9 +873,11 @@ static int sdio_clear_int_ext(u32 val)
 						cmd.address = 0xf8;
 						cmd.data = BIT(i);
 
-						ret = g_sdio.sdio_cmd52(&cmd);
-						if (!ret) {
-							g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf8 data (%d) ...\n", __LINE__);
+						ret = wilc_sdio_cmd52(wilc, &cmd);
+						if (ret) {
+							dev_err(&func->dev,
+								"Failed cmd52, set 0xf8 data (%d) ...\n",
+								__LINE__);
 							goto _fail_;
 						}
 
@@ -857,12 +890,13 @@ static int sdio_clear_int_ext(u32 val)
 					goto _fail_;
 				for (i = g_sdio.nint; i < MAX_NUM_INT; i++) {
 					if (flags & 1)
-						g_sdio.dPrint(N_ERR, "[wilc sdio]: Unexpected interrupt cleared %d...\n", i);
+						dev_err(&func->dev,
+							"Unexpected interrupt cleared %d...\n",
+							i);
 					flags >>= 1;
 				}
 			}
 		}
-#endif /* WILC_SDIO_IRQ_GPIO */
 
 		{
 			u32 vmm_ctl;
@@ -886,9 +920,11 @@ static int sdio_clear_int_ext(u32 val)
 				cmd.raw = 0;
 				cmd.address = 0xf6;
 				cmd.data = vmm_ctl;
-				ret = g_sdio.sdio_cmd52(&cmd);
-				if (!ret) {
-					g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed cmd52, set 0xf6 data (%d) ...\n", __LINE__);
+				ret = wilc_sdio_cmd52(wilc, &cmd);
+				if (ret) {
+					dev_err(&func->dev,
+						"Failed cmd52, set 0xf6 data (%d) ...\n",
+						__LINE__);
 					goto _fail_;
 				}
 			}
@@ -900,16 +936,18 @@ _fail_:
 	return 0;
 }
 
-static int sdio_sync_ext(int nint /*  how mant interrupts to enable. */)
+static int sdio_sync_ext(struct wilc *wilc, int nint)
 {
+	struct sdio_func *func = dev_to_sdio_func(wilc->dev);
 	u32 reg;
 
 	if (nint > MAX_NUM_INT) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Too many interupts (%d)...\n", nint);
+		dev_err(&func->dev, "Too many interupts (%d)...\n", nint);
 		return 0;
 	}
 	if (nint > MAX_NUN_INT_THRPT_ENH2) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Error: Cannot support more than 5 interrupts when has_thrpt_enh2=1.\n");
+		dev_err(&func->dev,
+			"Cannot support more than 5 interrupts when has_thrpt_enh2=1.\n");
 		return 0;
 	}
 
@@ -918,71 +956,77 @@ static int sdio_sync_ext(int nint /*  how mant interrupts to enable. */)
 	/**
 	 *      Disable power sequencer
 	 **/
-	if (!sdio_read_reg(WILC_MISC, &reg)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read misc reg...\n");
+	if (!sdio_read_reg(wilc, WILC_MISC, &reg)) {
+		dev_err(&func->dev, "Failed read misc reg...\n");
 		return 0;
 	}
 
 	reg &= ~BIT(8);
-	if (!sdio_write_reg(WILC_MISC, reg)) {
-		g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write misc reg...\n");
+	if (!sdio_write_reg(wilc, WILC_MISC, reg)) {
+		dev_err(&func->dev, "Failed write misc reg...\n");
 		return 0;
 	}
 
-#ifdef WILC_SDIO_IRQ_GPIO
-	{
+	if (g_sdio.irq_gpio) {
 		u32 reg;
 		int ret, i;
 
 		/**
 		 *      interrupt pin mux select
 		 **/
-		ret = sdio_read_reg(WILC_PIN_MUX_0, &reg);
+		ret = sdio_read_reg(wilc, WILC_PIN_MUX_0, &reg);
 		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+			dev_err(&func->dev, "Failed read reg (%08x)...\n",
+				WILC_PIN_MUX_0);
 			return 0;
 		}
 		reg |= BIT(8);
-		ret = sdio_write_reg(WILC_PIN_MUX_0, reg);
+		ret = sdio_write_reg(wilc, WILC_PIN_MUX_0, reg);
 		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+			dev_err(&func->dev, "Failed write reg (%08x)...\n",
+				WILC_PIN_MUX_0);
 			return 0;
 		}
 
 		/**
 		 *      interrupt enable
 		 **/
-		ret = sdio_read_reg(WILC_INTR_ENABLE, &reg);
+		ret = sdio_read_reg(wilc, WILC_INTR_ENABLE, &reg);
 		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+			dev_err(&func->dev, "Failed read reg (%08x)...\n",
+				WILC_INTR_ENABLE);
 			return 0;
 		}
 
 		for (i = 0; (i < 5) && (nint > 0); i++, nint--)
 			reg |= BIT((27 + i));
-		ret = sdio_write_reg(WILC_INTR_ENABLE, reg);
+		ret = sdio_write_reg(wilc, WILC_INTR_ENABLE, reg);
 		if (!ret) {
-			g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+			dev_err(&func->dev, "Failed write reg (%08x)...\n",
+				WILC_INTR_ENABLE);
 			return 0;
 		}
 		if (nint) {
-			ret = sdio_read_reg(WILC_INTR2_ENABLE, &reg);
+			ret = sdio_read_reg(wilc, WILC_INTR2_ENABLE, &reg);
 			if (!ret) {
-				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed read reg (%08x)...\n", WILC_INTR2_ENABLE);
+				dev_err(&func->dev,
+					"Failed read reg (%08x)...\n",
+					WILC_INTR2_ENABLE);
 				return 0;
 			}
 
 			for (i = 0; (i < 3) && (nint > 0); i++, nint--)
 				reg |= BIT(i);
 
-			ret = sdio_read_reg(WILC_INTR2_ENABLE, &reg);
+			ret = sdio_read_reg(wilc, WILC_INTR2_ENABLE, &reg);
 			if (!ret) {
-				g_sdio.dPrint(N_ERR, "[wilc sdio]: Failed write reg (%08x)...\n", WILC_INTR2_ENABLE);
+				dev_err(&func->dev,
+					"Failed write reg (%08x)...\n",
+					WILC_INTR2_ENABLE);
 				return 0;
 			}
 		}
 	}
-#endif /* WILC_SDIO_IRQ_GPIO */
 	return 1;
 }
 
@@ -992,23 +1036,20 @@ static int sdio_sync_ext(int nint /*  how mant interrupts to enable. */)
  *
  ********************************************/
 
-wilc_hif_func_t hif_sdio = {
-	sdio_init,
-	sdio_deinit,
-	sdio_read_reg,
-	sdio_write_reg,
-	sdio_read,
-	sdio_write,
-	sdio_sync,
-	sdio_clear_int,
-	sdio_read_int,
-	sdio_clear_int_ext,
-	sdio_read_size,
-	sdio_write,
-	sdio_read,
-	sdio_sync_ext,
-
-	sdio_set_max_speed,
-	sdio_set_default_speed,
+const struct wilc_hif_func wilc_hif_sdio = {
+	.hif_init = sdio_init,
+	.hif_deinit = sdio_deinit,
+	.hif_read_reg = sdio_read_reg,
+	.hif_write_reg = sdio_write_reg,
+	.hif_block_rx = sdio_read,
+	.hif_block_tx = sdio_write,
+	.hif_read_int = sdio_read_int,
+	.hif_clear_int_ext = sdio_clear_int_ext,
+	.hif_read_size = sdio_read_size,
+	.hif_block_tx_ext = sdio_write,
+	.hif_block_rx_ext = sdio_read,
+	.hif_sync_ext = sdio_sync_ext,
+	.enable_interrupt = wilc_sdio_enable_interrupt,
+	.disable_interrupt = wilc_sdio_disable_interrupt,
 };
 
diff --git a/drivers/staging/wilc1000/wilc_spi.c b/drivers/staging/wilc1000/wilc_spi.c
index 599508beabf8..86de50c9f7f5 100644
--- a/drivers/staging/wilc1000/wilc_spi.c
+++ b/drivers/staging/wilc1000/wilc_spi.c
@@ -6,18 +6,25 @@
 /*  */
 /*  */
 /* //////////////////////////////////////////////////////////////////////////// */
-
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/fs.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/device.h>
+#include <linux/spi/spi.h>
+#include <linux/of_gpio.h>
+
+#include "linux_wlan_common.h"
 #include <linux/string.h>
 #include "wilc_wlan_if.h"
 #include "wilc_wlan.h"
+#include "wilc_wfi_netdevice.h"
 
 typedef struct {
-	void *os_context;
-	int (*spi_tx)(u8 *, u32);
-	int (*spi_rx)(u8 *, u32);
-	int (*spi_trx)(u8 *, u8 *, u32);
-	int (*spi_max_speed)(void);
-	wilc_debug_func dPrint;
 	int crc_off;
 	int nint;
 	int has_thrpt_enh;
@@ -25,8 +32,8 @@ typedef struct {
 
 static wilc_spi_t g_spi;
 
-static int spi_read(u32, u8 *, u32);
-static int spi_write(u32, u8 *, u32);
+static int wilc_spi_read(struct wilc *wilc, u32, u8 *, u32);
+static int wilc_spi_write(struct wilc *wilc, u32, u8 *, u32);
 
 /********************************************
  *
@@ -111,165 +118,175 @@ static u8 crc7(u8 crc, const u8 *buffer, u32 len)
 #define DATA_PKT_SZ_8K				(8 * 1024)
 #define DATA_PKT_SZ					DATA_PKT_SZ_8K
 
-static int spi_cmd(u8 cmd, u32 adr, u32 data, u32 sz, u8 clockless)
+#define USE_SPI_DMA     0
+
+static const struct wilc1000_ops wilc1000_spi_ops;
+
+static int wilc_bus_probe(struct spi_device *spi)
 {
-	u8 bc[9];
-	int len = 5;
-	int result = N_OK;
+	int ret, gpio;
+	struct wilc *wilc;
 
-	bc[0] = cmd;
-	switch (cmd) {
-	case CMD_SINGLE_READ:                           /* single word (4 bytes) read */
-		bc[1] = (u8)(adr >> 16);
-		bc[2] = (u8)(adr >> 8);
-		bc[3] = (u8)adr;
-		len = 5;
-		break;
+	gpio = of_get_gpio(spi->dev.of_node, 0);
+	if (gpio < 0)
+		gpio = GPIO_NUM;
 
-	case CMD_INTERNAL_READ:                 /* internal register read */
-		bc[1] = (u8)(adr >> 8);
-		if (clockless)
-			bc[1] |= BIT(7);
-		bc[2] = (u8)adr;
-		bc[3] = 0x00;
-		len = 5;
-		break;
+	ret = wilc_netdev_init(&wilc, NULL, HIF_SPI, GPIO_NUM, &wilc_hif_spi);
+	if (ret)
+		return ret;
 
-	case CMD_TERMINATE:                                     /* termination */
-		bc[1] = 0x00;
-		bc[2] = 0x00;
-		bc[3] = 0x00;
-		len = 5;
-		break;
+	spi_set_drvdata(spi, wilc);
+	wilc->dev = &spi->dev;
 
-	case CMD_REPEAT:                                                /* repeat */
-		bc[1] = 0x00;
-		bc[2] = 0x00;
-		bc[3] = 0x00;
-		len = 5;
-		break;
+	return 0;
+}
 
-	case CMD_RESET:                                                 /* reset */
-		bc[1] = 0xff;
-		bc[2] = 0xff;
-		bc[3] = 0xff;
-		len = 5;
-		break;
+static int wilc_bus_remove(struct spi_device *spi)
+{
+	wilc_netdev_cleanup(spi_get_drvdata(spi));
+	return 0;
+}
 
-	case CMD_DMA_WRITE:                                     /* dma write */
-	case CMD_DMA_READ:                                      /* dma read */
-		bc[1] = (u8)(adr >> 16);
-		bc[2] = (u8)(adr >> 8);
-		bc[3] = (u8)adr;
-		bc[4] = (u8)(sz >> 8);
-		bc[5] = (u8)(sz);
-		len = 7;
-		break;
+static const struct of_device_id wilc1000_of_match[] = {
+	{ .compatible = "atmel,wilc_spi", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, wilc1000_of_match);
+
+struct spi_driver wilc1000_spi_driver = {
+	.driver = {
+		.name = MODALIAS,
+		.of_match_table = wilc1000_of_match,
+	},
+	.probe =  wilc_bus_probe,
+	.remove = wilc_bus_remove,
+};
+module_spi_driver(wilc1000_spi_driver);
+MODULE_LICENSE("GPL");
 
-	case CMD_DMA_EXT_WRITE:         /* dma extended write */
-	case CMD_DMA_EXT_READ:                  /* dma extended read */
-		bc[1] = (u8)(adr >> 16);
-		bc[2] = (u8)(adr >> 8);
-		bc[3] = (u8)adr;
-		bc[4] = (u8)(sz >> 16);
-		bc[5] = (u8)(sz >> 8);
-		bc[6] = (u8)(sz);
-		len = 8;
-		break;
+static int wilc_spi_tx(struct wilc *wilc, u8 *b, u32 len)
+{
+	struct spi_device *spi = to_spi_device(wilc->dev);
+	int ret;
+	struct spi_message msg;
 
-	case CMD_INTERNAL_WRITE:                /* internal register write */
-		bc[1] = (u8)(adr >> 8);
-		if (clockless)
-			bc[1] |= BIT(7);
-		bc[2] = (u8)(adr);
-		bc[3] = (u8)(data >> 24);
-		bc[4] = (u8)(data >> 16);
-		bc[5] = (u8)(data >> 8);
-		bc[6] = (u8)(data);
-		len = 8;
-		break;
+	if (len > 0 && b) {
+		struct spi_transfer tr = {
+			.tx_buf = b,
+			.len = len,
+			.delay_usecs = 0,
+		};
+		char *r_buffer = kzalloc(len, GFP_KERNEL);
 
-	case CMD_SINGLE_WRITE:                  /* single word write */
-		bc[1] = (u8)(adr >> 16);
-		bc[2] = (u8)(adr >> 8);
-		bc[3] = (u8)(adr);
-		bc[4] = (u8)(data >> 24);
-		bc[5] = (u8)(data >> 16);
-		bc[6] = (u8)(data >> 8);
-		bc[7] = (u8)(data);
-		len = 9;
-		break;
+		if (!r_buffer)
+			return -ENOMEM;
 
-	default:
-		result = N_FAIL;
-		break;
-	}
+		tr.rx_buf = r_buffer;
+		dev_dbg(&spi->dev, "Request writing %d bytes\n", len);
 
-	if (result) {
-		if (!g_spi.crc_off)
-			bc[len - 1] = (crc7(0x7f, (const u8 *)&bc[0], len - 1)) << 1;
-		else
-			len -= 1;
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+		msg.spi = spi;
+		msg.is_dma_mapped = USE_SPI_DMA;
+		spi_message_add_tail(&tr, &msg);
 
-		if (!g_spi.spi_tx(bc, len)) {
-			PRINT_ER("[wilc spi]: Failed cmd write, bus error...\n");
-			result = N_FAIL;
-		}
+		ret = spi_sync(spi, &msg);
+		if (ret < 0)
+			dev_err(&spi->dev, "SPI transaction failed\n");
+
+		kfree(r_buffer);
+	} else {
+		dev_err(&spi->dev,
+			"can't write data with the following length: %d\n",
+			len);
+		dev_err(&spi->dev,
+			"FAILED due to NULL buffer or ZERO length check the following length: %d\n",
+			len);
+		ret = -EINVAL;
 	}
 
-	return result;
+	return ret;
 }
 
-static int spi_cmd_rsp(u8 cmd)
+static int wilc_spi_rx(struct wilc *wilc, u8 *rb, u32 rlen)
 {
-	u8 rsp;
-	int result = N_OK;
+	struct spi_device *spi = to_spi_device(wilc->dev);
+	int ret;
 
-	/**
-	 *      Command/Control response
-	 **/
-	if ((cmd == CMD_RESET) ||
-	    (cmd == CMD_TERMINATE) ||
-	    (cmd == CMD_REPEAT)) {
-		if (!g_spi.spi_rx(&rsp, 1)) {
-			result = N_FAIL;
-			goto _fail_;
-		}
-	}
+	if (rlen > 0) {
+		struct spi_message msg;
+		struct spi_transfer tr = {
+			.rx_buf = rb,
+			.len = rlen,
+			.delay_usecs = 0,
 
-	if (!g_spi.spi_rx(&rsp, 1)) {
-		PRINT_ER("[wilc spi]: Failed cmd response read, bus error...\n");
-		result = N_FAIL;
-		goto _fail_;
-	}
+		};
+		char *t_buffer = kzalloc(rlen, GFP_KERNEL);
 
-	if (rsp != cmd) {
-		PRINT_ER("[wilc spi]: Failed cmd response, cmd (%02x), resp (%02x)\n", cmd, rsp);
-		result = N_FAIL;
-		goto _fail_;
-	}
+		if (!t_buffer)
+			return -ENOMEM;
 
-	/**
-	 *      State response
-	 **/
-	if (!g_spi.spi_rx(&rsp, 1)) {
-		PRINT_ER("[wilc spi]: Failed cmd state read, bus error...\n");
-		result = N_FAIL;
-		goto _fail_;
-	}
+		tr.tx_buf = t_buffer;
 
-	if (rsp != 0x00) {
-		PRINT_ER("[wilc spi]: Failed cmd state response state (%02x)\n", rsp);
-		result = N_FAIL;
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+		msg.spi = spi;
+		msg.is_dma_mapped = USE_SPI_DMA;
+		spi_message_add_tail(&tr, &msg);
+
+		ret = spi_sync(spi, &msg);
+		if (ret < 0)
+			dev_err(&spi->dev, "SPI transaction failed\n");
+		kfree(t_buffer);
+	} else {
+		dev_err(&spi->dev,
+			"can't read data with the following length: %u\n",
+			rlen);
+		ret = -EINVAL;
 	}
 
-_fail_:
+	return ret;
+}
 
-	return result;
+static int wilc_spi_tx_rx(struct wilc *wilc, u8 *wb, u8 *rb, u32 rlen)
+{
+	struct spi_device *spi = to_spi_device(wilc->dev);
+	int ret;
+
+	if (rlen > 0) {
+		struct spi_message msg;
+		struct spi_transfer tr = {
+			.rx_buf = rb,
+			.tx_buf = wb,
+			.len = rlen,
+			.bits_per_word = 8,
+			.delay_usecs = 0,
+
+		};
+
+		memset(&msg, 0, sizeof(msg));
+		spi_message_init(&msg);
+		msg.spi = spi;
+		msg.is_dma_mapped = USE_SPI_DMA;
+
+		spi_message_add_tail(&tr, &msg);
+		ret = spi_sync(spi, &msg);
+		if (ret < 0)
+			dev_err(&spi->dev, "SPI transaction failed\n");
+	} else {
+		dev_err(&spi->dev,
+			"can't read data with the following length: %u\n",
+			rlen);
+		ret = -EINVAL;
+	}
+
+	return ret;
 }
 
-static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
+static int spi_cmd_complete(struct wilc *wilc, u8 cmd, u32 adr, u8 *b, u32 sz,
+			    u8 clockless)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	u8 wb[32], rb[32];
 	u8 wix, rix;
 	u32 len2;
@@ -398,7 +415,7 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 #undef NUM_DUMMY_BYTES
 
 	if (len2 > ARRAY_SIZE(wb)) {
-		PRINT_ER("[wilc spi]: spi buffer size too small (%d) (%zu)\n",
+		dev_err(&spi->dev, "spi buffer size too small (%d) (%zu)\n",
 			 len2, ARRAY_SIZE(wb));
 		result = N_FAIL;
 		return result;
@@ -409,8 +426,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 	}
 	rix = len;
 
-	if (!g_spi.spi_trx(wb, rb, len2)) {
-		PRINT_ER("[wilc spi]: Failed cmd write, bus error...\n");
+	if (wilc_spi_tx_rx(wilc, wb, rb, len2)) {
+		dev_err(&spi->dev, "Failed cmd write, bus error...\n");
 		result = N_FAIL;
 		return result;
 	}
@@ -430,7 +447,7 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 	/* } while(&rptr[1] <= &rb[len2]); */
 
 	if (rsp != cmd) {
-		PRINT_ER("[wilc spi]: Failed cmd response, cmd (%02x)"
+		dev_err(&spi->dev, "Failed cmd response, cmd (%02x)"
 			 ", resp (%02x)\n", cmd, rsp);
 		result = N_FAIL;
 		return result;
@@ -441,8 +458,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 	 **/
 	rsp = rb[rix++];
 	if (rsp != 0x00) {
-		PRINT_ER("[wilc spi]: Failed cmd state response "
-			 "state (%02x)\n", rsp);
+		dev_err(&spi->dev, "Failed cmd state response state (%02x)\n",
+			rsp);
 		result = N_FAIL;
 		return result;
 	}
@@ -469,8 +486,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 		} while (retry--);
 
 		if (retry <= 0) {
-			PRINT_ER("[wilc spi]: Error, data read "
-				 "response (%02x)\n", rsp);
+			dev_err(&spi->dev,
+				"Error, data read response (%02x)\n", rsp);
 			result = N_RESET;
 			return result;
 		}
@@ -485,7 +502,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				b[2] = rb[rix++];
 				b[3] = rb[rix++];
 			} else {
-				PRINT_ER("[wilc spi]: buffer overrun when reading data.\n");
+				dev_err(&spi->dev,
+					"buffer overrun when reading data.\n");
 				result = N_FAIL;
 				return result;
 			}
@@ -498,7 +516,7 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 					crc[0] = rb[rix++];
 					crc[1] = rb[rix++];
 				} else {
-					PRINT_ER("[wilc spi]: buffer overrun when reading crc.\n");
+					dev_err(&spi->dev,"buffer overrun when reading crc.\n");
 					result = N_FAIL;
 					return result;
 				}
@@ -524,8 +542,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				/**
 				 * Read bytes
 				 **/
-				if (!g_spi.spi_rx(&b[ix], nbytes)) {
-					PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
+				if (wilc_spi_rx(wilc, &b[ix], nbytes)) {
+					dev_err(&spi->dev, "Failed data block read, bus error...\n");
 					result = N_FAIL;
 					goto _error_;
 				}
@@ -534,8 +552,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				 * Read Crc
 				 **/
 				if (!g_spi.crc_off) {
-					if (!g_spi.spi_rx(crc, 2)) {
-						PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
+					if (wilc_spi_rx(wilc, crc, 2)) {
+						dev_err(&spi->dev, "Failed data block crc read, bus error...\n");
 						result = N_FAIL;
 						goto _error_;
 					}
@@ -565,8 +583,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				 **/
 				retry = 10;
 				do {
-					if (!g_spi.spi_rx(&rsp, 1)) {
-						PRINT_ER("[wilc spi]: Failed data response read, bus error...\n");
+					if (wilc_spi_rx(wilc, &rsp, 1)) {
+						dev_err(&spi->dev, "Failed data response read, bus error...\n");
 						result = N_FAIL;
 						break;
 					}
@@ -581,8 +599,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				/**
 				 * Read bytes
 				 **/
-				if (!g_spi.spi_rx(&b[ix], nbytes)) {
-					PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
+				if (wilc_spi_rx(wilc, &b[ix], nbytes)) {
+					dev_err(&spi->dev, "Failed data block read, bus error...\n");
 					result = N_FAIL;
 					break;
 				}
@@ -591,8 +609,8 @@ static int spi_cmd_complete(u8 cmd, u32 adr, u8 *b, u32 sz, u8 clockless)
 				 * Read Crc
 				 **/
 				if (!g_spi.crc_off) {
-					if (!g_spi.spi_rx(crc, 2)) {
-						PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
+					if (wilc_spi_rx(wilc, crc, 2)) {
+						dev_err(&spi->dev, "Failed data block crc read, bus error...\n");
 						result = N_FAIL;
 						break;
 					}
@@ -607,76 +625,9 @@ _error_:
 	return result;
 }
 
-static int spi_data_read(u8 *b, u32 sz)
-{
-	int retry, ix, nbytes;
-	int result = N_OK;
-	u8 crc[2];
-	u8 rsp;
-
-	/**
-	 *      Data
-	 **/
-	ix = 0;
-	do {
-		if (sz <= DATA_PKT_SZ)
-			nbytes = sz;
-		else
-			nbytes = DATA_PKT_SZ;
-
-		/**
-		 *      Data Respnose header
-		 **/
-		retry = 10;
-		do {
-			if (!g_spi.spi_rx(&rsp, 1)) {
-				PRINT_ER("[wilc spi]: Failed data response read, bus error...\n");
-				result = N_FAIL;
-				break;
-			}
-			if (((rsp >> 4) & 0xf) == 0xf)
-				break;
-		} while (retry--);
-
-		if (result == N_FAIL)
-			break;
-
-		if (retry <= 0) {
-			PRINT_ER("[wilc spi]: Failed data response read...(%02x)\n", rsp);
-			result = N_FAIL;
-			break;
-		}
-
-		/**
-		 *      Read bytes
-		 **/
-		if (!g_spi.spi_rx(&b[ix], nbytes)) {
-			PRINT_ER("[wilc spi]: Failed data block read, bus error...\n");
-			result = N_FAIL;
-			break;
-		}
-
-		/**
-		 *      Read Crc
-		 **/
-		if (!g_spi.crc_off) {
-			if (!g_spi.spi_rx(crc, 2)) {
-				PRINT_ER("[wilc spi]: Failed data block crc read, bus error...\n");
-				result = N_FAIL;
-				break;
-			}
-		}
-
-		ix += nbytes;
-		sz -= nbytes;
-
-	} while (sz);
-
-	return result;
-}
-
-static int spi_data_write(u8 *b, u32 sz)
+static int spi_data_write(struct wilc *wilc, u8 *b, u32 sz)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int ix, nbytes;
 	int result = 1;
 	u8 cmd, order, crc[2] = {0};
@@ -709,8 +660,9 @@ static int spi_data_write(u8 *b, u32 sz)
 				order = 0x2;
 		}
 		cmd |= order;
-		if (!g_spi.spi_tx(&cmd, 1)) {
-			PRINT_ER("[wilc spi]: Failed data block cmd write, bus error...\n");
+		if (wilc_spi_tx(wilc, &cmd, 1)) {
+			dev_err(&spi->dev,
+				"Failed data block cmd write, bus error...\n");
 			result = N_FAIL;
 			break;
 		}
@@ -718,8 +670,9 @@ static int spi_data_write(u8 *b, u32 sz)
 		/**
 		 *      Write data
 		 **/
-		if (!g_spi.spi_tx(&b[ix], nbytes)) {
-			PRINT_ER("[wilc spi]: Failed data block write, bus error...\n");
+		if (wilc_spi_tx(wilc, &b[ix], nbytes)) {
+			dev_err(&spi->dev,
+				"Failed data block write, bus error...\n");
 			result = N_FAIL;
 			break;
 		}
@@ -728,8 +681,8 @@ static int spi_data_write(u8 *b, u32 sz)
 		 *      Write Crc
 		 **/
 		if (!g_spi.crc_off) {
-			if (!g_spi.spi_tx(crc, 2)) {
-				PRINT_ER("[wilc spi]: Failed data block crc write, bus error...\n");
+			if (wilc_spi_tx(wilc, crc, 2)) {
+				dev_err(&spi->dev,"Failed data block crc write, bus error...\n");
 				result = N_FAIL;
 				break;
 			}
@@ -752,34 +705,34 @@ static int spi_data_write(u8 *b, u32 sz)
  *
  ********************************************/
 
-static int spi_internal_write(u32 adr, u32 dat)
+static int spi_internal_write(struct wilc *wilc, u32 adr, u32 dat)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int result;
 
-#ifdef BIG_ENDIAN
-	dat = BYTE_SWAP(dat);
-#endif
-	result = spi_cmd_complete(CMD_INTERNAL_WRITE, adr, (u8 *)&dat, 4, 0);
+	dat = cpu_to_le32(dat);
+	result = spi_cmd_complete(wilc, CMD_INTERNAL_WRITE, adr, (u8 *)&dat, 4,
+				  0);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed internal write cmd...\n");
+		dev_err(&spi->dev, "Failed internal write cmd...\n");
 	}
 
 	return result;
 }
 
-static int spi_internal_read(u32 adr, u32 *data)
+static int spi_internal_read(struct wilc *wilc, u32 adr, u32 *data)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int result;
 
-	result = spi_cmd_complete(CMD_INTERNAL_READ, adr, (u8 *)data, 4, 0);
+	result = spi_cmd_complete(wilc, CMD_INTERNAL_READ, adr, (u8 *)data, 4,
+				  0);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed internal read cmd...\n");
+		dev_err(&spi->dev, "Failed internal read cmd...\n");
 		return 0;
 	}
 
-#ifdef BIG_ENDIAN
-	*data = BYTE_SWAP(*data);
-#endif
+	*data = cpu_to_le32(*data);
 
 	return 1;
 }
@@ -790,31 +743,31 @@ static int spi_internal_read(u32 adr, u32 *data)
  *
  ********************************************/
 
-static int spi_write_reg(u32 addr, u32 data)
+static int wilc_spi_write_reg(struct wilc *wilc, u32 addr, u32 data)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int result = N_OK;
 	u8 cmd = CMD_SINGLE_WRITE;
 	u8 clockless = 0;
 
-#ifdef BIG_ENDIAN
-	data = BYTE_SWAP(data);
-#endif
+	data = cpu_to_le32(data);
 	if (addr < 0x30) {
 		/* Clockless register*/
 		cmd = CMD_INTERNAL_WRITE;
 		clockless = 1;
 	}
 
-	result = spi_cmd_complete(cmd, addr, (u8 *)&data, 4, clockless);
+	result = spi_cmd_complete(wilc, cmd, addr, (u8 *)&data, 4, clockless);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed cmd, write reg (%08x)...\n", addr);
+		dev_err(&spi->dev, "Failed cmd, write reg (%08x)...\n", addr);
 	}
 
 	return result;
 }
 
-static int spi_write(u32 addr, u8 *buf, u32 size)
+static int wilc_spi_write(struct wilc *wilc, u32 addr, u8 *buf, u32 size)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int result;
 	u8 cmd = CMD_DMA_EXT_WRITE;
 
@@ -824,60 +777,61 @@ static int spi_write(u32 addr, u8 *buf, u32 size)
 	if (size <= 4)
 		return 0;
 
-	result = spi_cmd_complete(cmd, addr, NULL, size, 0);
+	result = spi_cmd_complete(wilc, cmd, addr, NULL, size, 0);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed cmd, write block (%08x)...\n", addr);
+		dev_err(&spi->dev,
+			"Failed cmd, write block (%08x)...\n", addr);
 		return 0;
 	}
 
 	/**
 	 *      Data
 	 **/
-	result = spi_data_write(buf, size);
+	result = spi_data_write(wilc, buf, size);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed block data write...\n");
+		dev_err(&spi->dev, "Failed block data write...\n");
 	}
 
 	return 1;
 }
 
-static int spi_read_reg(u32 addr, u32 *data)
+static int wilc_spi_read_reg(struct wilc *wilc, u32 addr, u32 *data)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int result = N_OK;
 	u8 cmd = CMD_SINGLE_READ;
 	u8 clockless = 0;
 
 	if (addr < 0x30) {
-		/* PRINT_ER("***** read addr %d\n\n", addr); */
+		/* dev_err(&spi->dev, "***** read addr %d\n\n", addr); */
 		/* Clockless register*/
 		cmd = CMD_INTERNAL_READ;
 		clockless = 1;
 	}
 
-	result = spi_cmd_complete(cmd, addr, (u8 *)data, 4, clockless);
+	result = spi_cmd_complete(wilc, cmd, addr, (u8 *)data, 4, clockless);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed cmd, read reg (%08x)...\n", addr);
+		dev_err(&spi->dev, "Failed cmd, read reg (%08x)...\n", addr);
 		return 0;
 	}
 
-#ifdef BIG_ENDIAN
-	*data = BYTE_SWAP(*data);
-#endif
+	*data = cpu_to_le32(*data);
 
 	return 1;
 }
 
-static int spi_read(u32 addr, u8 *buf, u32 size)
+static int wilc_spi_read(struct wilc *wilc, u32 addr, u8 *buf, u32 size)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	u8 cmd = CMD_DMA_EXT_READ;
 	int result;
 
 	if (size <= 4)
 		return 0;
 
-	result = spi_cmd_complete(cmd, addr, buf, size, 0);
+	result = spi_cmd_complete(wilc, cmd, addr, buf, size, 0);
 	if (result != N_OK) {
-		PRINT_ER("[wilc spi]: Failed cmd, read block (%08x)...\n", addr);
+		dev_err(&spi->dev, "Failed cmd, read block (%08x)...\n", addr);
 		return 0;
 	}
 
@@ -890,20 +844,7 @@ static int spi_read(u32 addr, u8 *buf, u32 size)
  *
  ********************************************/
 
-static int spi_clear_int(void)
-{
-	u32 reg;
-
-	if (!spi_read_reg(WILC_HOST_RX_CTRL_0, &reg)) {
-		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_HOST_RX_CTRL_0);
-		return 0;
-	}
-	reg &= ~0x1;
-	spi_write_reg(WILC_HOST_RX_CTRL_0, reg);
-	return 1;
-}
-
-static int spi_deinit(void *pv)
+static int _wilc_spi_deinit(struct wilc *wilc)
 {
 	/**
 	 *      TODO:
@@ -911,46 +852,9 @@ static int spi_deinit(void *pv)
 	return 1;
 }
 
-static int spi_sync(void)
-{
-	u32 reg;
-	int ret;
-
-	/**
-	 *      interrupt pin mux select
-	 **/
-	ret = spi_read_reg(WILC_PIN_MUX_0, &reg);
-	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
-		return 0;
-	}
-	reg |= BIT(8);
-	ret = spi_write_reg(WILC_PIN_MUX_0, reg);
-	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
-		return 0;
-	}
-
-	/**
-	 *      interrupt enable
-	 **/
-	ret = spi_read_reg(WILC_INTR_ENABLE, &reg);
-	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
-		return 0;
-	}
-	reg |= BIT(16);
-	ret = spi_write_reg(WILC_INTR_ENABLE, reg);
-	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
-		return 0;
-	}
-
-	return 1;
-}
-
-static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
+static int wilc_spi_init(struct wilc *wilc)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	u32 reg;
 	u32 chipid;
 
@@ -958,8 +862,8 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 
 	if (isinit) {
 
-		if (!spi_read_reg(0x1000, &chipid)) {
-			PRINT_ER("[wilc spi]: Fail cmd read chip id...\n");
+		if (!wilc_spi_read_reg(wilc, 0x1000, &chipid)) {
+			dev_err(&spi->dev, "Fail cmd read chip id...\n");
 			return 0;
 		}
 		return 1;
@@ -967,21 +871,6 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 
 	memset(&g_spi, 0, sizeof(wilc_spi_t));
 
-	g_spi.dPrint = func;
-	g_spi.os_context = inp->os_context.os_private;
-	if (inp->io_func.io_init) {
-		if (!inp->io_func.io_init(g_spi.os_context)) {
-			PRINT_ER("[wilc spi]: Failed io init bus...\n");
-			return 0;
-		}
-	} else {
-		return 0;
-	}
-	g_spi.spi_tx = inp->io_func.u.spi.spi_tx;
-	g_spi.spi_rx = inp->io_func.u.spi.spi_rx;
-	g_spi.spi_trx = inp->io_func.u.spi.spi_trx;
-	g_spi.spi_max_speed = inp->io_func.u.spi.spi_max_speed;
-
 	/**
 	 *      configure protocol
 	 **/
@@ -989,14 +878,15 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 
 	/* TODO: We can remove the CRC trials if there is a definite way to reset */
 	/* the SPI to it's initial value. */
-	if (!spi_internal_read(WILC_SPI_PROTOCOL_OFFSET, &reg)) {
+	if (!spi_internal_read(wilc, WILC_SPI_PROTOCOL_OFFSET, &reg)) {
 		/* Read failed. Try with CRC off. This might happen when module
 		 * is removed but chip isn't reset*/
 		g_spi.crc_off = 1;
-		PRINT_ER("[wilc spi]: Failed internal read protocol with CRC on, retyring with CRC off...\n");
-		if (!spi_internal_read(WILC_SPI_PROTOCOL_OFFSET, &reg)) {
+		dev_err(&spi->dev, "Failed internal read protocol with CRC on, retyring with CRC off...\n");
+		if (!spi_internal_read(wilc, WILC_SPI_PROTOCOL_OFFSET, &reg)) {
 			/* Reaad failed with both CRC on and off, something went bad */
-			PRINT_ER("[wilc spi]: Failed internal read protocol...\n");
+			dev_err(&spi->dev,
+				"Failed internal read protocol...\n");
 			return 0;
 		}
 	}
@@ -1004,8 +894,8 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 		reg &= ~0xc;    /* disable crc checking */
 		reg &= ~0x70;
 		reg |= (0x5 << 4);
-		if (!spi_internal_write(WILC_SPI_PROTOCOL_OFFSET, reg)) {
-			PRINT_ER("[wilc spi %d]: Failed internal write protocol reg...\n", __LINE__);
+		if (!spi_internal_write(wilc, WILC_SPI_PROTOCOL_OFFSET, reg)) {
+			dev_err(&spi->dev, "[wilc spi %d]: Failed internal write protocol reg...\n", __LINE__);
 			return 0;
 		}
 		g_spi.crc_off = 1;
@@ -1015,11 +905,11 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	/**
 	 *      make sure can read back chip id correctly
 	 **/
-	if (!spi_read_reg(0x1000, &chipid)) {
-		PRINT_ER("[wilc spi]: Fail cmd read chip id...\n");
+	if (!wilc_spi_read_reg(wilc, 0x1000, &chipid)) {
+		dev_err(&spi->dev, "Fail cmd read chip id...\n");
 		return 0;
 	}
-	/* PRINT_ER("[wilc spi]: chipid (%08x)\n", chipid); */
+	/* dev_err(&spi->dev, "chipid (%08x)\n", chipid); */
 
 	g_spi.has_thrpt_enh = 1;
 
@@ -1028,29 +918,24 @@ static int spi_init(wilc_wlan_inp_t *inp, wilc_debug_func func)
 	return 1;
 }
 
-static void spi_max_bus_speed(void)
-{
-	g_spi.spi_max_speed();
-}
-
-static void spi_default_bus_speed(void)
-{
-}
-
-static int spi_read_size(u32 *size)
+static int wilc_spi_read_size(struct wilc *wilc, u32 *size)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int ret;
 
 	if (g_spi.has_thrpt_enh) {
-		ret = spi_internal_read(0xe840 - WILC_SPI_REG_BASE, size);
+		ret = spi_internal_read(wilc, 0xe840 - WILC_SPI_REG_BASE,
+					size);
 		*size = *size  & IRQ_DMA_WD_CNT_MASK;
 	} else {
 		u32 tmp;
 		u32 byte_cnt;
 
-		ret = spi_read_reg(WILC_VMM_TO_HOST_SIZE, &byte_cnt);
+		ret = wilc_spi_read_reg(wilc, WILC_VMM_TO_HOST_SIZE,
+					&byte_cnt);
 		if (!ret) {
-			PRINT_ER("[wilc spi]: Failed read WILC_VMM_TO_HOST_SIZE ...\n");
+			dev_err(&spi->dev,
+				"Failed read WILC_VMM_TO_HOST_SIZE ...\n");
 			goto _fail_;
 		}
 		tmp = (byte_cnt >> 2) & IRQ_DMA_WD_CNT_MASK;
@@ -1065,19 +950,23 @@ _fail_:
 
 
 
-static int spi_read_int(u32 *int_status)
+static int wilc_spi_read_int(struct wilc *wilc, u32 *int_status)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int ret;
 
 	if (g_spi.has_thrpt_enh) {
-		ret = spi_internal_read(0xe840 - WILC_SPI_REG_BASE, int_status);
+		ret = spi_internal_read(wilc, 0xe840 - WILC_SPI_REG_BASE,
+					int_status);
 	} else {
 		u32 tmp;
 		u32 byte_cnt;
 
-		ret = spi_read_reg(WILC_VMM_TO_HOST_SIZE, &byte_cnt);
+		ret = wilc_spi_read_reg(wilc, WILC_VMM_TO_HOST_SIZE,
+					&byte_cnt);
 		if (!ret) {
-			PRINT_ER("[wilc spi]: Failed read WILC_VMM_TO_HOST_SIZE ...\n");
+			dev_err(&spi->dev,
+				"Failed read WILC_VMM_TO_HOST_SIZE ...\n");
 			goto _fail_;
 		}
 		tmp = (byte_cnt >> 2) & IRQ_DMA_WD_CNT_MASK;
@@ -1091,11 +980,12 @@ static int spi_read_int(u32 *int_status)
 
 				happended = 0;
 
-				spi_read_reg(0x1a90, &irq_flags);
+				wilc_spi_read_reg(wilc, 0x1a90, &irq_flags);
 				tmp |= ((irq_flags >> 27) << IRG_FLAGS_OFFSET);
 
 				if (g_spi.nint > 5) {
-					spi_read_reg(0x1a94, &irq_flags);
+					wilc_spi_read_reg(wilc, 0x1a94,
+							  &irq_flags);
 					tmp |= (((irq_flags >> 0) & 0x7) << (IRG_FLAGS_OFFSET + 5));
 				}
 
@@ -1105,7 +995,7 @@ static int spi_read_int(u32 *int_status)
 					unkmown_mask = ~((1ul << g_spi.nint) - 1);
 
 					if ((tmp >> IRG_FLAGS_OFFSET) & unkmown_mask) {
-						PRINT_ER("[wilc spi]: Unexpected interrupt (2): j=%d, tmp=%x, mask=%x\n", j, tmp, unkmown_mask);
+						dev_err(&spi->dev, "Unexpected interrupt (2): j=%d, tmp=%x, mask=%x\n", j, tmp, unkmown_mask);
 						happended = 1;
 					}
 				}
@@ -1121,12 +1011,14 @@ _fail_:
 	return ret;
 }
 
-static int spi_clear_int_ext(u32 val)
+static int wilc_spi_clear_int_ext(struct wilc *wilc, u32 val)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	int ret;
 
 	if (g_spi.has_thrpt_enh) {
-		ret = spi_internal_write(0xe844 - WILC_SPI_REG_BASE, val);
+		ret = spi_internal_write(wilc, 0xe844 - WILC_SPI_REG_BASE,
+					 val);
 	} else {
 		u32 flags;
 
@@ -1138,18 +1030,22 @@ static int spi_clear_int_ext(u32 val)
 			for (i = 0; i < g_spi.nint; i++) {
 				/* No matter what you write 1 or 0, it will clear interrupt. */
 				if (flags & 1)
-					ret = spi_write_reg(0x10c8 + i * 4, 1);
+					ret = wilc_spi_write_reg(wilc, 0x10c8 + i * 4, 1);
 				if (!ret)
 					break;
 				flags >>= 1;
 			}
 			if (!ret) {
-				PRINT_ER("[wilc spi]: Failed spi_write_reg, set reg %x ...\n", 0x10c8 + i * 4);
+				dev_err(&spi->dev,
+					"Failed wilc_spi_write_reg, set reg %x ...\n",
+					0x10c8 + i * 4);
 				goto _fail_;
 			}
 			for (i = g_spi.nint; i < MAX_NUM_INT; i++) {
 				if (flags & 1)
-					PRINT_ER("[wilc spi]: Unexpected interrupt cleared %d...\n", i);
+					dev_err(&spi->dev,
+						"Unexpected interrupt cleared %d...\n",
+						i);
 				flags >>= 1;
 			}
 		}
@@ -1165,9 +1061,11 @@ static int spi_clear_int_ext(u32 val)
 			if ((val & SEL_VMM_TBL1) == SEL_VMM_TBL1)
 				tbl_ctl |= BIT(1);
 
-			ret = spi_write_reg(WILC_VMM_TBL_CTL, tbl_ctl);
+			ret = wilc_spi_write_reg(wilc, WILC_VMM_TBL_CTL,
+						 tbl_ctl);
 			if (!ret) {
-				PRINT_ER("[wilc spi]: fail write reg vmm_tbl_ctl...\n");
+				dev_err(&spi->dev,
+					"fail write reg vmm_tbl_ctl...\n");
 				goto _fail_;
 			}
 
@@ -1175,9 +1073,10 @@ static int spi_clear_int_ext(u32 val)
 				/**
 				 *      enable vmm transfer.
 				 **/
-				ret = spi_write_reg(WILC_VMM_CORE_CTL, 1);
+				ret = wilc_spi_write_reg(wilc,
+							 WILC_VMM_CORE_CTL, 1);
 				if (!ret) {
-					PRINT_ER("[wilc spi]: fail write reg vmm_core_ctl...\n");
+					dev_err(&spi->dev,"fail write reg vmm_core_ctl...\n");
 					goto _fail_;
 				}
 			}
@@ -1187,13 +1086,14 @@ _fail_:
 	return ret;
 }
 
-static int spi_sync_ext(int nint /*  how mant interrupts to enable. */)
+static int wilc_spi_sync_ext(struct wilc *wilc, int nint)
 {
+	struct spi_device *spi = to_spi_device(wilc->dev);
 	u32 reg;
 	int ret, i;
 
 	if (nint > MAX_NUM_INT) {
-		PRINT_ER("[wilc spi]: Too many interupts (%d)...\n", nint);
+		dev_err(&spi->dev, "Too many interupts (%d)...\n", nint);
 		return 0;
 	}
 
@@ -1202,39 +1102,44 @@ static int spi_sync_ext(int nint /*  how mant interrupts to enable. */)
 	/**
 	 *      interrupt pin mux select
 	 **/
-	ret = spi_read_reg(WILC_PIN_MUX_0, &reg);
+	ret = wilc_spi_read_reg(wilc, WILC_PIN_MUX_0, &reg);
 	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_PIN_MUX_0);
+		dev_err(&spi->dev, "Failed read reg (%08x)...\n",
+			WILC_PIN_MUX_0);
 		return 0;
 	}
 	reg |= BIT(8);
-	ret = spi_write_reg(WILC_PIN_MUX_0, reg);
+	ret = wilc_spi_write_reg(wilc, WILC_PIN_MUX_0, reg);
 	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_PIN_MUX_0);
+		dev_err(&spi->dev, "Failed write reg (%08x)...\n",
+			WILC_PIN_MUX_0);
 		return 0;
 	}
 
 	/**
 	 *      interrupt enable
 	 **/
-	ret = spi_read_reg(WILC_INTR_ENABLE, &reg);
+	ret = wilc_spi_read_reg(wilc, WILC_INTR_ENABLE, &reg);
 	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR_ENABLE);
+		dev_err(&spi->dev, "Failed read reg (%08x)...\n",
+			WILC_INTR_ENABLE);
 		return 0;
 	}
 
 	for (i = 0; (i < 5) && (nint > 0); i++, nint--) {
 		reg |= (BIT((27 + i)));
 	}
-	ret = spi_write_reg(WILC_INTR_ENABLE, reg);
+	ret = wilc_spi_write_reg(wilc, WILC_INTR_ENABLE, reg);
 	if (!ret) {
-		PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR_ENABLE);
+		dev_err(&spi->dev, "Failed write reg (%08x)...\n",
+			WILC_INTR_ENABLE);
 		return 0;
 	}
 	if (nint) {
-		ret = spi_read_reg(WILC_INTR2_ENABLE, &reg);
+		ret = wilc_spi_read_reg(wilc, WILC_INTR2_ENABLE, &reg);
 		if (!ret) {
-			PRINT_ER("[wilc spi]: Failed read reg (%08x)...\n", WILC_INTR2_ENABLE);
+			dev_err(&spi->dev, "Failed read reg (%08x)...\n",
+				WILC_INTR2_ENABLE);
 			return 0;
 		}
 
@@ -1242,9 +1147,10 @@ static int spi_sync_ext(int nint /*  how mant interrupts to enable. */)
 			reg |= BIT(i);
 		}
 
-		ret = spi_read_reg(WILC_INTR2_ENABLE, &reg);
+		ret = wilc_spi_read_reg(wilc, WILC_INTR2_ENABLE, &reg);
 		if (!ret) {
-			PRINT_ER("[wilc spi]: Failed write reg (%08x)...\n", WILC_INTR2_ENABLE);
+			dev_err(&spi->dev, "Failed write reg (%08x)...\n",
+				WILC_INTR2_ENABLE);
 			return 0;
 		}
 	}
@@ -1256,21 +1162,17 @@ static int spi_sync_ext(int nint /*  how mant interrupts to enable. */)
  *      Global spi HIF function table
  *
  ********************************************/
-wilc_hif_func_t hif_spi = {
-	spi_init,
-	spi_deinit,
-	spi_read_reg,
-	spi_write_reg,
-	spi_read,
-	spi_write,
-	spi_sync,
-	spi_clear_int,
-	spi_read_int,
-	spi_clear_int_ext,
-	spi_read_size,
-	spi_write,
-	spi_read,
-	spi_sync_ext,
-	spi_max_bus_speed,
-	spi_default_bus_speed,
+const struct wilc_hif_func wilc_hif_spi = {
+	.hif_init = wilc_spi_init,
+	.hif_deinit = _wilc_spi_deinit,
+	.hif_read_reg = wilc_spi_read_reg,
+	.hif_write_reg = wilc_spi_write_reg,
+	.hif_block_rx = wilc_spi_read,
+	.hif_block_tx = wilc_spi_write,
+	.hif_read_int = wilc_spi_read_int,
+	.hif_clear_int_ext = wilc_spi_clear_int_ext,
+	.hif_read_size = wilc_spi_read_size,
+	.hif_block_tx_ext = wilc_spi_write,
+	.hif_block_rx_ext = wilc_spi_read,
+	.hif_sync_ext = wilc_spi_sync_ext,
 };
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
index 3e9501727812..53fb2d4bb0bd 100644
--- a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.c
@@ -1,40 +1,101 @@
-/*!
- *  @file	wilc_wfi_cfgopertaions.c
- *  @brief	CFG80211 Function Implementation functionality
- *  @author	aabouzaeid
- *                      mabubakr
- *                      mdaftedar
- *                      zsalah
- *  @sa		wilc_wfi_cfgopertaions.h top level OS wrapper file
- *  @date	31 Aug 2010
- *  @version	1.0
- */
-
 #include "wilc_wfi_cfgoperations.h"
-#ifdef WILC_SDIO
-#include "linux_wlan_sdio.h"
-#endif
+#include "host_interface.h"
 #include <linux/errno.h>
 
+#define NO_ENCRYPT		0
+#define ENCRYPT_ENABLED		BIT(0)
+#define WEP			BIT(1)
+#define WEP_EXTENDED		BIT(2)
+#define WPA			BIT(3)
+#define WPA2			BIT(4)
+#define AES			BIT(5)
+#define TKIP			BIT(6)
+
+#define FRAME_TYPE_ID			0
+#define ACTION_CAT_ID			24
+#define ACTION_SUBTYPE_ID		25
+#define P2P_PUB_ACTION_SUBTYPE		30
+
+#define ACTION_FRAME			0xd0
+#define GO_INTENT_ATTR_ID		0x04
+#define CHANLIST_ATTR_ID		0x0b
+#define OPERCHAN_ATTR_ID		0x11
+#define PUB_ACTION_ATTR_ID		0x04
+#define P2PELEM_ATTR_ID			0xdd
+
+#define GO_NEG_REQ			0x00
+#define GO_NEG_RSP			0x01
+#define GO_NEG_CONF			0x02
+#define P2P_INV_REQ			0x03
+#define P2P_INV_RSP			0x04
+#define PUBLIC_ACT_VENDORSPEC		0x09
+#define GAS_INTIAL_REQ			0x0a
+#define GAS_INTIAL_RSP			0x0b
+
+#define INVALID_CHANNEL			0
+
+#define nl80211_SCAN_RESULT_EXPIRE	(3 * HZ)
+#define SCAN_RESULT_EXPIRE		(40 * HZ)
+
+static const u32 cipher_suites[] = {
+	WLAN_CIPHER_SUITE_WEP40,
+	WLAN_CIPHER_SUITE_WEP104,
+	WLAN_CIPHER_SUITE_TKIP,
+	WLAN_CIPHER_SUITE_CCMP,
+	WLAN_CIPHER_SUITE_AES_CMAC,
+};
+
+static const struct ieee80211_txrx_stypes
+	wilc_wfi_cfg80211_mgmt_types[NUM_NL80211_IFTYPES] = {
+	[NL80211_IFTYPE_STATION] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
+	},
+	[NL80211_IFTYPE_AP] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+			BIT(IEEE80211_STYPE_AUTH >> 4) |
+			BIT(IEEE80211_STYPE_DEAUTH >> 4) |
+			BIT(IEEE80211_STYPE_ACTION >> 4)
+	},
+	[NL80211_IFTYPE_P2P_CLIENT] = {
+		.tx = 0xffff,
+		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
+			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
+			BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
+			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
+			BIT(IEEE80211_STYPE_AUTH >> 4) |
+			BIT(IEEE80211_STYPE_DEAUTH >> 4)
+	}
+};
+
+#define WILC_WFI_DWELL_PASSIVE 100
+#define WILC_WFI_DWELL_ACTIVE  40
+
+#define TCP_ACK_FILTER_LINK_SPEED_THRESH	54
+#define DEFAULT_LINK_SPEED			72
+
+
 #define IS_MANAGMEMENT				0x100
 #define IS_MANAGMEMENT_CALLBACK			0x080
 #define IS_MGMT_STATUS_SUCCES			0x040
 #define GET_PKT_OFFSET(a) (((a) >> 22) & 0x1ff)
 
-extern int linux_wlan_get_firmware(perInterface_wlan_t *p_nic);
+extern int wilc_mac_open(struct net_device *ndev);
+extern int wilc_mac_close(struct net_device *ndev);
 
-extern int mac_open(struct net_device *ndev);
-extern int mac_close(struct net_device *ndev);
-
-tstrNetworkInfo astrLastScannedNtwrksShadow[MAX_NUM_SCANNED_NETWORKS_SHADOW];
-u32 u32LastScannedNtwrksCountShadow;
-struct timer_list hDuringIpTimer;
-struct timer_list hAgingTimer;
+static tstrNetworkInfo last_scanned_shadow[MAX_NUM_SCANNED_NETWORKS_SHADOW];
+static u32 last_scanned_cnt;
+struct timer_list wilc_during_ip_timer;
+static struct timer_list hAgingTimer;
 static u8 op_ifcs;
-extern u8 u8ConnectedSSID[6];
 
-u8 g_wilc_initialized = 1;
-extern bool g_obtainingIP;
+u8 wilc_initialized = 1;
 
 #define CHAN2G(_channel, _freq, _flags) {	 \
 		.band             = IEEE80211_BAND_2GHZ, \
@@ -45,8 +106,7 @@ extern bool g_obtainingIP;
 		.max_power        = 30,			 \
 }
 
-/*Frequency range for channels*/
-static struct ieee80211_channel WILC_WFI_2ghz_channels[] = {
+static struct ieee80211_channel ieee80211_2ghz_channels[] = {
 	CHAN2G(1,  2412, 0),
 	CHAN2G(2,  2417, 0),
 	CHAN2G(3,  2422, 0),
@@ -69,9 +129,7 @@ static struct ieee80211_channel WILC_WFI_2ghz_channels[] = {
 		.flags    = (_flags),			\
 }
 
-
-/* Table 6 in section 3.2.1.1 */
-static struct ieee80211_rate WILC_WFI_rates[] = {
+static struct ieee80211_rate ieee80211_bitrates[] = {
 	RATETAB_ENT(10,  0,  0),
 	RATETAB_ENT(20,  1,  0),
 	RATETAB_ENT(55,  2,  0),
@@ -91,22 +149,19 @@ struct p2p_mgmt_data {
 	u8 *buff;
 };
 
-/*Global variable used to state the current  connected STA channel*/
-u8 u8WLANChannel = INVALID_CHANNEL;
-
-u8 curr_channel;
-
-u8 u8P2P_oui[] = {0x50, 0x6f, 0x9A, 0x09};
-u8 u8P2Plocalrandom = 0x01;
-u8 u8P2Precvrandom = 0x00;
-u8 u8P2P_vendorspec[] = {0xdd, 0x05, 0x00, 0x08, 0x40, 0x03};
-bool bWilc_ie;
+static u8 wlan_channel = INVALID_CHANNEL;
+static u8 curr_channel;
+static u8 p2p_oui[] = {0x50, 0x6f, 0x9A, 0x09};
+static u8 p2p_local_random = 0x01;
+static u8 p2p_recv_random = 0x00;
+static u8 p2p_vendor_spec[] = {0xdd, 0x05, 0x00, 0x08, 0x40, 0x03};
+static bool wilc_ie;
 
 static struct ieee80211_supported_band WILC_WFI_band_2ghz = {
-	.channels = WILC_WFI_2ghz_channels,
-	.n_channels = ARRAY_SIZE(WILC_WFI_2ghz_channels),
-	.bitrates = WILC_WFI_rates,
-	.n_bitrates = ARRAY_SIZE(WILC_WFI_rates),
+	.channels = ieee80211_2ghz_channels,
+	.n_channels = ARRAY_SIZE(ieee80211_2ghz_channels),
+	.bitrates = ieee80211_bitrates,
+	.n_bitrates = ARRAY_SIZE(ieee80211_bitrates),
 };
 
 
@@ -115,19 +170,19 @@ struct add_key_params {
 	bool pairwise;
 	u8 *mac_addr;
 };
-struct add_key_params g_add_gtk_key_params;
-struct wilc_wfi_key g_key_gtk_params;
-struct add_key_params g_add_ptk_key_params;
-struct wilc_wfi_key g_key_ptk_params;
-struct wilc_wfi_wep_key g_key_wep_params;
-bool g_ptk_keys_saved;
-bool g_gtk_keys_saved;
-bool g_wep_keys_saved;
+static struct add_key_params g_add_gtk_key_params;
+static struct wilc_wfi_key g_key_gtk_params;
+static struct add_key_params g_add_ptk_key_params;
+static struct wilc_wfi_key g_key_ptk_params;
+static struct wilc_wfi_wep_key g_key_wep_params;
+static bool g_ptk_keys_saved;
+static bool g_gtk_keys_saved;
+static bool g_wep_keys_saved;
 
 #define AGING_TIME	(9 * 1000)
-#define duringIP_TIME 15000
+#define during_ip_time	15000
 
-void clear_shadow_scan(void *pUserVoid)
+static void clear_shadow_scan(void)
 {
 	int i;
 
@@ -135,34 +190,33 @@ void clear_shadow_scan(void *pUserVoid)
 		del_timer_sync(&hAgingTimer);
 		PRINT_INFO(CORECONFIG_DBG, "destroy aging timer\n");
 
-		for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-			if (astrLastScannedNtwrksShadow[u32LastScannedNtwrksCountShadow].pu8IEs != NULL) {
-				kfree(astrLastScannedNtwrksShadow[i].pu8IEs);
-				astrLastScannedNtwrksShadow[u32LastScannedNtwrksCountShadow].pu8IEs = NULL;
+		for (i = 0; i < last_scanned_cnt; i++) {
+			if (last_scanned_shadow[last_scanned_cnt].pu8IEs) {
+				kfree(last_scanned_shadow[i].pu8IEs);
+				last_scanned_shadow[last_scanned_cnt].pu8IEs = NULL;
 			}
 
-			host_int_freeJoinParams(astrLastScannedNtwrksShadow[i].pJoinParams);
-			astrLastScannedNtwrksShadow[i].pJoinParams = NULL;
+			wilc_free_join_params(last_scanned_shadow[i].pJoinParams);
+			last_scanned_shadow[i].pJoinParams = NULL;
 		}
-		u32LastScannedNtwrksCountShadow = 0;
+		last_scanned_cnt = 0;
 	}
-
 }
 
-u32 get_rssi_avg(tstrNetworkInfo *pstrNetworkInfo)
+static u32 get_rssi_avg(tstrNetworkInfo *network_info)
 {
 	u8 i;
 	int rssi_v = 0;
-	u8 num_rssi = (pstrNetworkInfo->strRssi.u8Full) ? NUM_RSSI : (pstrNetworkInfo->strRssi.u8Index);
+	u8 num_rssi = (network_info->strRssi.u8Full) ? NUM_RSSI : (network_info->strRssi.u8Index);
 
 	for (i = 0; i < num_rssi; i++)
-		rssi_v += pstrNetworkInfo->strRssi.as8RSSI[i];
+		rssi_v += network_info->strRssi.as8RSSI[i];
 
 	rssi_v /= num_rssi;
 	return rssi_v;
 }
 
-void refresh_scan(void *pUserVoid, u8 all, bool bDirectScan)
+static void refresh_scan(void *user_void, u8 all, bool direct_scan)
 {
 	struct wilc_priv *priv;
 	struct wiphy *wiphy;
@@ -170,55 +224,49 @@ void refresh_scan(void *pUserVoid, u8 all, bool bDirectScan)
 	int i;
 	int rssi = 0;
 
-	priv = (struct wilc_priv *)pUserVoid;
+	priv = (struct wilc_priv *)user_void;
 	wiphy = priv->dev->ieee80211_ptr->wiphy;
 
-	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-		tstrNetworkInfo *pstrNetworkInfo;
+	for (i = 0; i < last_scanned_cnt; i++) {
+		tstrNetworkInfo *network_info;
 
-		pstrNetworkInfo = &(astrLastScannedNtwrksShadow[i]);
+		network_info = &last_scanned_shadow[i];
 
-
-		if ((!pstrNetworkInfo->u8Found) || all) {
-			s32 s32Freq;
+		if (!network_info->u8Found || all) {
+			s32 freq;
 			struct ieee80211_channel *channel;
 
-			if (pstrNetworkInfo != NULL) {
-
-				s32Freq = ieee80211_channel_to_frequency((s32)pstrNetworkInfo->u8channel, IEEE80211_BAND_2GHZ);
-				channel = ieee80211_get_channel(wiphy, s32Freq);
+			if (network_info) {
+				freq = ieee80211_channel_to_frequency((s32)network_info->u8channel, IEEE80211_BAND_2GHZ);
+				channel = ieee80211_get_channel(wiphy, freq);
 
-				rssi = get_rssi_avg(pstrNetworkInfo);
-				if (memcmp("DIRECT-", pstrNetworkInfo->au8ssid, 7) || bDirectScan)	{
-					bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN, pstrNetworkInfo->au8bssid, pstrNetworkInfo->u64Tsf, pstrNetworkInfo->u16CapInfo,
-								  pstrNetworkInfo->u16BeaconPeriod, (const u8 *)pstrNetworkInfo->pu8IEs,
-								  (size_t)pstrNetworkInfo->u16IEsLen, (((s32)rssi) * 100), GFP_KERNEL);
+				rssi = get_rssi_avg(network_info);
+				if (memcmp("DIRECT-", network_info->au8ssid, 7) ||
+				    direct_scan) {
+					bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN, network_info->au8bssid, network_info->u64Tsf, network_info->u16CapInfo,
+								  network_info->u16BeaconPeriod, (const u8 *)network_info->pu8IEs,
+								  (size_t)network_info->u16IEsLen, (((s32)rssi) * 100), GFP_KERNEL);
 					cfg80211_put_bss(wiphy, bss);
 				}
 			}
-
 		}
 	}
-
 }
 
-void reset_shadow_found(void *pUserVoid)
+static void reset_shadow_found(void)
 {
 	int i;
 
-	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-		astrLastScannedNtwrksShadow[i].u8Found = 0;
-
-	}
+	for (i = 0; i < last_scanned_cnt; i++)
+		last_scanned_shadow[i].u8Found = 0;
 }
 
-void update_scan_time(void *pUserVoid)
+static void update_scan_time(void)
 {
 	int i;
 
-	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-		astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan = jiffies;
-	}
+	for (i = 0; i < last_scanned_cnt; i++)
+		last_scanned_shadow[i].u32TimeRcvdInScan = jiffies;
 }
 
 static void remove_network_from_shadow(unsigned long arg)
@@ -227,24 +275,25 @@ static void remove_network_from_shadow(unsigned long arg)
 	int i, j;
 
 
-	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-		if (time_after(now, astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan + (unsigned long)(SCAN_RESULT_EXPIRE))) {
-			PRINT_D(CFG80211_DBG, "Network expired in ScanShadow: %s\n", astrLastScannedNtwrksShadow[i].au8ssid);
+	for (i = 0; i < last_scanned_cnt; i++) {
+		if (time_after(now, last_scanned_shadow[i].u32TimeRcvdInScan + (unsigned long)(SCAN_RESULT_EXPIRE))) {
+			PRINT_D(CFG80211_DBG, "Network expired in ScanShadow: %s\n", last_scanned_shadow[i].au8ssid);
 
-			kfree(astrLastScannedNtwrksShadow[i].pu8IEs);
-			astrLastScannedNtwrksShadow[i].pu8IEs = NULL;
+			kfree(last_scanned_shadow[i].pu8IEs);
+			last_scanned_shadow[i].pu8IEs = NULL;
 
-			host_int_freeJoinParams(astrLastScannedNtwrksShadow[i].pJoinParams);
+			wilc_free_join_params(last_scanned_shadow[i].pJoinParams);
 
-			for (j = i; (j < u32LastScannedNtwrksCountShadow - 1); j++) {
-				astrLastScannedNtwrksShadow[j] = astrLastScannedNtwrksShadow[j + 1];
-			}
-			u32LastScannedNtwrksCountShadow--;
+			for (j = i; (j < last_scanned_cnt - 1); j++)
+				last_scanned_shadow[j] = last_scanned_shadow[j + 1];
+
+			last_scanned_cnt--;
 		}
 	}
 
-	PRINT_D(CFG80211_DBG, "Number of cached networks: %d\n", u32LastScannedNtwrksCountShadow);
-	if (u32LastScannedNtwrksCountShadow != 0) {
+	PRINT_D(CFG80211_DBG, "Number of cached networks: %d\n",
+		last_scanned_cnt);
+	if (last_scanned_cnt != 0) {
 		hAgingTimer.data = arg;
 		mod_timer(&hAgingTimer, jiffies + msecs_to_jiffies(AGING_TIME));
 	} else {
@@ -255,24 +304,24 @@ static void remove_network_from_shadow(unsigned long arg)
 static void clear_duringIP(unsigned long arg)
 {
 	PRINT_D(GENERIC_DBG, "GO:IP Obtained , enable scan\n");
-	g_obtainingIP = false;
+	wilc_optaining_ip = false;
 }
 
-int is_network_in_shadow(tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid)
+static int is_network_in_shadow(tstrNetworkInfo *pstrNetworkInfo,
+				void *user_void)
 {
 	int state = -1;
 	int i;
 
-	if (u32LastScannedNtwrksCountShadow == 0) {
+	if (last_scanned_cnt == 0) {
 		PRINT_D(CFG80211_DBG, "Starting Aging timer\n");
-		hAgingTimer.data = (unsigned long)pUserVoid;
+		hAgingTimer.data = (unsigned long)user_void;
 		mod_timer(&hAgingTimer, jiffies + msecs_to_jiffies(AGING_TIME));
 		state = -1;
 	} else {
-		/* Linear search for now */
-		for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-			if (memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
-					pstrNetworkInfo->au8bssid, 6) == 0) {
+		for (i = 0; i < last_scanned_cnt; i++) {
+			if (memcmp(last_scanned_shadow[i].au8bssid,
+				   pstrNetworkInfo->au8bssid, 6) == 0) {
 				state = i;
 				break;
 			}
@@ -281,78 +330,60 @@ int is_network_in_shadow(tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid)
 	return state;
 }
 
-void add_network_to_shadow(tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid, void *pJoinParams)
+static void add_network_to_shadow(tstrNetworkInfo *pstrNetworkInfo,
+				  void *user_void, void *pJoinParams)
 {
-	int ap_found = is_network_in_shadow(pstrNetworkInfo, pUserVoid);
+	int ap_found = is_network_in_shadow(pstrNetworkInfo, user_void);
 	u32 ap_index = 0;
 	u8 rssi_index = 0;
 
-	if (u32LastScannedNtwrksCountShadow >= MAX_NUM_SCANNED_NETWORKS_SHADOW) {
+	if (last_scanned_cnt >= MAX_NUM_SCANNED_NETWORKS_SHADOW) {
 		PRINT_D(CFG80211_DBG, "Shadow network reached its maximum limit\n");
 		return;
 	}
 	if (ap_found == -1) {
-		ap_index = u32LastScannedNtwrksCountShadow;
-		u32LastScannedNtwrksCountShadow++;
-
+		ap_index = last_scanned_cnt;
+		last_scanned_cnt++;
 	} else {
 		ap_index = ap_found;
 	}
-	rssi_index = astrLastScannedNtwrksShadow[ap_index].strRssi.u8Index;
-	astrLastScannedNtwrksShadow[ap_index].strRssi.as8RSSI[rssi_index++] = pstrNetworkInfo->s8rssi;
+	rssi_index = last_scanned_shadow[ap_index].strRssi.u8Index;
+	last_scanned_shadow[ap_index].strRssi.as8RSSI[rssi_index++] = pstrNetworkInfo->s8rssi;
 	if (rssi_index == NUM_RSSI) {
 		rssi_index = 0;
-		astrLastScannedNtwrksShadow[ap_index].strRssi.u8Full = 1;
-	}
-	astrLastScannedNtwrksShadow[ap_index].strRssi.u8Index = rssi_index;
-
-	astrLastScannedNtwrksShadow[ap_index].s8rssi = pstrNetworkInfo->s8rssi;
-	astrLastScannedNtwrksShadow[ap_index].u16CapInfo = pstrNetworkInfo->u16CapInfo;
-
-	astrLastScannedNtwrksShadow[ap_index].u8SsidLen = pstrNetworkInfo->u8SsidLen;
-	memcpy(astrLastScannedNtwrksShadow[ap_index].au8ssid,
-		    pstrNetworkInfo->au8ssid, pstrNetworkInfo->u8SsidLen);
-
-	memcpy(astrLastScannedNtwrksShadow[ap_index].au8bssid,
-		    pstrNetworkInfo->au8bssid, ETH_ALEN);
-
-	astrLastScannedNtwrksShadow[ap_index].u16BeaconPeriod = pstrNetworkInfo->u16BeaconPeriod;
-	astrLastScannedNtwrksShadow[ap_index].u8DtimPeriod = pstrNetworkInfo->u8DtimPeriod;
-	astrLastScannedNtwrksShadow[ap_index].u8channel = pstrNetworkInfo->u8channel;
-
-	astrLastScannedNtwrksShadow[ap_index].u16IEsLen = pstrNetworkInfo->u16IEsLen;
-	astrLastScannedNtwrksShadow[ap_index].u64Tsf = pstrNetworkInfo->u64Tsf;
+		last_scanned_shadow[ap_index].strRssi.u8Full = 1;
+	}
+	last_scanned_shadow[ap_index].strRssi.u8Index = rssi_index;
+	last_scanned_shadow[ap_index].s8rssi = pstrNetworkInfo->s8rssi;
+	last_scanned_shadow[ap_index].u16CapInfo = pstrNetworkInfo->u16CapInfo;
+	last_scanned_shadow[ap_index].u8SsidLen = pstrNetworkInfo->u8SsidLen;
+	memcpy(last_scanned_shadow[ap_index].au8ssid,
+	       pstrNetworkInfo->au8ssid, pstrNetworkInfo->u8SsidLen);
+	memcpy(last_scanned_shadow[ap_index].au8bssid,
+	       pstrNetworkInfo->au8bssid, ETH_ALEN);
+	last_scanned_shadow[ap_index].u16BeaconPeriod = pstrNetworkInfo->u16BeaconPeriod;
+	last_scanned_shadow[ap_index].u8DtimPeriod = pstrNetworkInfo->u8DtimPeriod;
+	last_scanned_shadow[ap_index].u8channel = pstrNetworkInfo->u8channel;
+	last_scanned_shadow[ap_index].u16IEsLen = pstrNetworkInfo->u16IEsLen;
+	last_scanned_shadow[ap_index].u64Tsf = pstrNetworkInfo->u64Tsf;
 	if (ap_found != -1)
-		kfree(astrLastScannedNtwrksShadow[ap_index].pu8IEs);
-	astrLastScannedNtwrksShadow[ap_index].pu8IEs =
-		kmalloc(pstrNetworkInfo->u16IEsLen, GFP_KERNEL);        /* will be deallocated by the WILC_WFI_CfgScan() function */
-	memcpy(astrLastScannedNtwrksShadow[ap_index].pu8IEs,
-		    pstrNetworkInfo->pu8IEs, pstrNetworkInfo->u16IEsLen);
-
-	astrLastScannedNtwrksShadow[ap_index].u32TimeRcvdInScan = jiffies;
-	astrLastScannedNtwrksShadow[ap_index].u32TimeRcvdInScanCached = jiffies;
-	astrLastScannedNtwrksShadow[ap_index].u8Found = 1;
+		kfree(last_scanned_shadow[ap_index].pu8IEs);
+	last_scanned_shadow[ap_index].pu8IEs =
+		kmalloc(pstrNetworkInfo->u16IEsLen, GFP_KERNEL);
+	memcpy(last_scanned_shadow[ap_index].pu8IEs,
+	       pstrNetworkInfo->pu8IEs, pstrNetworkInfo->u16IEsLen);
+	last_scanned_shadow[ap_index].u32TimeRcvdInScan = jiffies;
+	last_scanned_shadow[ap_index].u32TimeRcvdInScanCached = jiffies;
+	last_scanned_shadow[ap_index].u8Found = 1;
 	if (ap_found != -1)
-		host_int_freeJoinParams(astrLastScannedNtwrksShadow[ap_index].pJoinParams);
-	astrLastScannedNtwrksShadow[ap_index].pJoinParams = pJoinParams;
-
+		wilc_free_join_params(last_scanned_shadow[ap_index].pJoinParams);
+	last_scanned_shadow[ap_index].pJoinParams = pJoinParams;
 }
 
-
-/**
- *  @brief      CfgScanResult
- *  @details  Callback function which returns the scan results found
- *
- *  @param[in] tenuScanEvent enuScanEvent: enum, indicating the scan event triggered, whether that is
- *                        SCAN_EVENT_NETWORK_FOUND or SCAN_EVENT_DONE
- *                        tstrNetworkInfo* pstrNetworkInfo: structure holding the scan results information
- *                        void* pUserVoid: Private structure associated with the wireless interface
- *  @return     NONE
- *  @author	mabubakr
- *  @date
- *  @version	1.0
- */
-static void CfgScanResult(enum scan_event enuScanEvent, tstrNetworkInfo *pstrNetworkInfo, void *pUserVoid, void *pJoinParams)
+static void CfgScanResult(enum scan_event scan_event,
+			  tstrNetworkInfo *network_info,
+			  void *user_void,
+			  void *join_params)
 {
 	struct wilc_priv *priv;
 	struct wiphy *wiphy;
@@ -360,56 +391,45 @@ static void CfgScanResult(enum scan_event enuScanEvent, tstrNetworkInfo *pstrNet
 	struct ieee80211_channel *channel;
 	struct cfg80211_bss *bss = NULL;
 
-	priv = (struct wilc_priv *)pUserVoid;
+	priv = (struct wilc_priv *)user_void;
 	if (priv->bCfgScanning) {
-		if (enuScanEvent == SCAN_EVENT_NETWORK_FOUND) {
+		if (scan_event == SCAN_EVENT_NETWORK_FOUND) {
 			wiphy = priv->dev->ieee80211_ptr->wiphy;
 
 			if (!wiphy)
 				return;
 
-			if (wiphy->signal_type == CFG80211_SIGNAL_TYPE_UNSPEC
-			    &&
-			    ((((s32)pstrNetworkInfo->s8rssi) * 100) < 0
-			     ||
-			     (((s32)pstrNetworkInfo->s8rssi) * 100) > 100)
-			    ) {
+			if (wiphy->signal_type == CFG80211_SIGNAL_TYPE_UNSPEC &&
+			    (((s32)network_info->s8rssi * 100) < 0 ||
+			    ((s32)network_info->s8rssi * 100) > 100)) {
 				PRINT_ER("wiphy signal type fial\n");
 				return;
 			}
 
-			if (pstrNetworkInfo != NULL) {
-				s32Freq = ieee80211_channel_to_frequency((s32)pstrNetworkInfo->u8channel, IEEE80211_BAND_2GHZ);
+			if (network_info) {
+				s32Freq = ieee80211_channel_to_frequency((s32)network_info->u8channel, IEEE80211_BAND_2GHZ);
 				channel = ieee80211_get_channel(wiphy, s32Freq);
 
 				if (!channel)
 					return;
 
 				PRINT_INFO(CFG80211_DBG, "Network Info:: CHANNEL Frequency: %d, RSSI: %d, CapabilityInfo: %d,"
-					   "BeaconPeriod: %d\n", channel->center_freq, (((s32)pstrNetworkInfo->s8rssi) * 100),
-					   pstrNetworkInfo->u16CapInfo, pstrNetworkInfo->u16BeaconPeriod);
-
-				if (pstrNetworkInfo->bNewNetwork) {
-					if (priv->u32RcvdChCount < MAX_NUM_SCANNED_NETWORKS) { /* TODO: mostafa: to be replaced by */
-						/*               max_scan_ssids */
-						PRINT_D(CFG80211_DBG, "Network %s found\n", pstrNetworkInfo->au8ssid);
-
+					   "BeaconPeriod: %d\n", channel->center_freq, (((s32)network_info->s8rssi) * 100),
+					   network_info->u16CapInfo, network_info->u16BeaconPeriod);
 
+				if (network_info->bNewNetwork) {
+					if (priv->u32RcvdChCount < MAX_NUM_SCANNED_NETWORKS) {
+						PRINT_D(CFG80211_DBG, "Network %s found\n", network_info->au8ssid);
 						priv->u32RcvdChCount++;
 
-
-
-						if (pJoinParams == NULL) {
+						if (!join_params)
 							PRINT_INFO(CORECONFIG_DBG, ">> Something really bad happened\n");
-						}
-						add_network_to_shadow(pstrNetworkInfo, priv, pJoinParams);
-
-						/*P2P peers are sent to WPA supplicant and added to shadow table*/
+						add_network_to_shadow(network_info, priv, join_params);
 
-						if (!(memcmp("DIRECT-", pstrNetworkInfo->au8ssid, 7))) {
-							bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN,  pstrNetworkInfo->au8bssid, pstrNetworkInfo->u64Tsf, pstrNetworkInfo->u16CapInfo,
-										  pstrNetworkInfo->u16BeaconPeriod, (const u8 *)pstrNetworkInfo->pu8IEs,
-										  (size_t)pstrNetworkInfo->u16IEsLen, (((s32)pstrNetworkInfo->s8rssi) * 100), GFP_KERNEL);
+						if (!(memcmp("DIRECT-", network_info->au8ssid, 7))) {
+							bss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN,  network_info->au8bssid, network_info->u64Tsf, network_info->u16CapInfo,
+										  network_info->u16BeaconPeriod, (const u8 *)network_info->pu8IEs,
+										  (size_t)network_info->u16IEsLen, (((s32)network_info->s8rssi) * 100), GFP_KERNEL);
 							cfg80211_put_bss(wiphy, bss);
 						}
 
@@ -419,19 +439,19 @@ static void CfgScanResult(enum scan_event enuScanEvent, tstrNetworkInfo *pstrNet
 					}
 				} else {
 					u32 i;
-					/* So this network is discovered before, we'll just update its RSSI */
+
 					for (i = 0; i < priv->u32RcvdChCount; i++) {
-						if (memcmp(astrLastScannedNtwrksShadow[i].au8bssid, pstrNetworkInfo->au8bssid, 6) == 0) {
-							PRINT_D(CFG80211_DBG, "Update RSSI of %s\n", astrLastScannedNtwrksShadow[i].au8ssid);
+						if (memcmp(last_scanned_shadow[i].au8bssid, network_info->au8bssid, 6) == 0) {
+							PRINT_D(CFG80211_DBG, "Update RSSI of %s\n", last_scanned_shadow[i].au8ssid);
 
-							astrLastScannedNtwrksShadow[i].s8rssi = pstrNetworkInfo->s8rssi;
-							astrLastScannedNtwrksShadow[i].u32TimeRcvdInScan = jiffies;
+							last_scanned_shadow[i].s8rssi = network_info->s8rssi;
+							last_scanned_shadow[i].u32TimeRcvdInScan = jiffies;
 							break;
 						}
 					}
 				}
 			}
-		} else if (enuScanEvent == SCAN_EVENT_DONE)    {
+		} else if (scan_event == SCAN_EVENT_DONE) {
 			PRINT_D(CFG80211_DBG, "Scan Done[%p]\n", priv->dev);
 			PRINT_D(CFG80211_DBG, "Refreshing Scan ...\n");
 			refresh_scan(priv, 1, false);
@@ -443,23 +463,19 @@ static void CfgScanResult(enum scan_event enuScanEvent, tstrNetworkInfo *pstrNet
 
 			down(&(priv->hSemScanReq));
 
-			if (priv->pstrScanReq != NULL) {
+			if (priv->pstrScanReq) {
 				cfg80211_scan_done(priv->pstrScanReq, false);
 				priv->u32RcvdChCount = 0;
 				priv->bCfgScanning = false;
 				priv->pstrScanReq = NULL;
 			}
 			up(&(priv->hSemScanReq));
-
-		}
-		/*Aborting any scan operation during mac close*/
-		else if (enuScanEvent == SCAN_EVENT_ABORTED) {
+		} else if (scan_event == SCAN_EVENT_ABORTED) {
 			down(&(priv->hSemScanReq));
 
 			PRINT_D(CFG80211_DBG, "Scan Aborted\n");
-			if (priv->pstrScanReq != NULL) {
-
-				update_scan_time(priv);
+			if (priv->pstrScanReq) {
+				update_scan_time();
 				refresh_scan(priv, 1, false);
 
 				cfg80211_scan_done(priv->pstrScanReq, false);
@@ -471,60 +487,7 @@ static void CfgScanResult(enum scan_event enuScanEvent, tstrNetworkInfo *pstrNet
 	}
 }
 
-
-/**
- *  @brief      WILC_WFI_Set_PMKSA
- *  @details  Check if pmksa is cached and set it.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-int WILC_WFI_Set_PMKSA(u8 *bssid, struct wilc_priv *priv)
-{
-	u32 i;
-	s32 s32Error = 0;
-
-
-	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
-
-		if (!memcmp(bssid, priv->pmkid_list.pmkidlist[i].bssid,
-				 ETH_ALEN)) {
-			PRINT_D(CFG80211_DBG, "PMKID successful comparison");
-
-			/*If bssid is found, set the values*/
-			s32Error = host_int_set_pmkid_info(priv->hWILCWFIDrv, &priv->pmkid_list);
-
-			if (s32Error != 0)
-				PRINT_ER("Error in pmkid\n");
-
-			break;
-		}
-	}
-
-	return s32Error;
-
-
-}
-int linux_wlan_set_bssid(struct net_device *wilc_netdev, u8 *pBSSID);
-
-
-/**
- *  @brief      CfgConnectResult
- *  @details
- *  @param[in] tenuConnDisconnEvent enuConnDisconnEvent: Type of connection response either
- *                        connection response or disconnection notification.
- *                        tstrConnectInfo* pstrConnectInfo: COnnection information.
- *                        u8 u8MacStatus: Mac Status from firmware
- *                        tstrDisconnectNotifInfo* pstrDisconnectNotifInfo: Disconnection Notification
- *                        void* pUserVoid: Private data associated with wireless interface
- *  @return     NONE
- *  @author	mabubakr
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-int connecting;
+int wilc_connecting;
 
 static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 			     tstrConnectInfo *pstrConnectInfo,
@@ -537,18 +500,17 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 	struct host_if_drv *pstrWFIDrv;
 	u8 NullBssid[ETH_ALEN] = {0};
 	struct wilc *wl;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
-	connecting = 0;
+	wilc_connecting = 0;
 
 	priv = (struct wilc_priv *)pUserVoid;
 	dev = priv->dev;
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif->wilc;
 	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
 
 	if (enuConnDisconnEvent == CONN_DISCONN_EVENT_CONN_RESP) {
-		/*Initialization*/
 		u16 u16ConnectStatus;
 
 		u16ConnectStatus = pstrConnectInfo->u16ConnectStatus;
@@ -557,15 +519,12 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 
 		if ((u8MacStatus == MAC_DISCONNECTED) &&
 		    (pstrConnectInfo->u16ConnectStatus == SUCCESSFUL_STATUSCODE)) {
-			/* The case here is that our station was waiting for association response frame and has just received it containing status code
-			 *  = SUCCESSFUL_STATUSCODE, while mac status is MAC_DISCONNECTED (which means something wrong happened) */
 			u16ConnectStatus = WLAN_STATUS_UNSPECIFIED_FAILURE;
-			linux_wlan_set_bssid(priv->dev, NullBssid);
-			eth_zero_addr(u8ConnectedSSID);
+			wilc_wlan_set_bssid(priv->dev, NullBssid);
+			eth_zero_addr(wilc_connected_ssid);
 
-			/*Invalidate u8WLANChannel value on wlan0 disconnect*/
-			if (!pstrWFIDrv->u8P2PConnect)
-				u8WLANChannel = INVALID_CHANNEL;
+			if (!pstrWFIDrv->p2p_connect)
+				wlan_channel = INVALID_CHANNEL;
 
 			PRINT_ER("Unspecified failure: Connection status %d : MAC status = %d\n", u16ConnectStatus, u8MacStatus);
 		}
@@ -579,13 +538,13 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 			memcpy(priv->au8AssociatedBss, pstrConnectInfo->au8bssid, ETH_ALEN);
 
 
-			for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-				if (memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
-						pstrConnectInfo->au8bssid, ETH_ALEN) == 0) {
+			for (i = 0; i < last_scanned_cnt; i++) {
+				if (memcmp(last_scanned_shadow[i].au8bssid,
+					   pstrConnectInfo->au8bssid, ETH_ALEN) == 0) {
 					unsigned long now = jiffies;
 
 					if (time_after(now,
-						       astrLastScannedNtwrksShadow[i].u32TimeRcvdInScanCached + (unsigned long)(nl80211_SCAN_RESULT_EXPIRE - (1 * HZ)))) {
+						       last_scanned_shadow[i].u32TimeRcvdInScanCached + (unsigned long)(nl80211_SCAN_RESULT_EXPIRE - (1 * HZ)))) {
 						bNeedScanRefresh = true;
 					}
 
@@ -593,12 +552,8 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 				}
 			}
 
-			if (bNeedScanRefresh) {
-				/*Also, refrsh DIRECT- results if */
+			if (bNeedScanRefresh)
 				refresh_scan(priv, 1, true);
-
-			}
-
 		}
 
 
@@ -609,68 +564,47 @@ static void CfgConnectResult(enum conn_event enuConnDisconnEvent,
 		cfg80211_connect_result(dev, pstrConnectInfo->au8bssid,
 					pstrConnectInfo->pu8ReqIEs, pstrConnectInfo->ReqIEsLen,
 					pstrConnectInfo->pu8RespIEs, pstrConnectInfo->u16RespIEsLen,
-					u16ConnectStatus, GFP_KERNEL);                         /* TODO: mostafa: u16ConnectStatus to */
-		/* be replaced by pstrConnectInfo->u16ConnectStatus */
+					u16ConnectStatus, GFP_KERNEL);
 	} else if (enuConnDisconnEvent == CONN_DISCONN_EVENT_DISCONN_NOTIF)    {
-		g_obtainingIP = false;
+		wilc_optaining_ip = false;
 		PRINT_ER("Received MAC_DISCONNECTED from firmware with reason %d on dev [%p]\n",
 			 pstrDisconnectNotifInfo->u16reason, priv->dev);
-		u8P2Plocalrandom = 0x01;
-		u8P2Precvrandom = 0x00;
-		bWilc_ie = false;
+		p2p_local_random = 0x01;
+		p2p_recv_random = 0x00;
+		wilc_ie = false;
 		eth_zero_addr(priv->au8AssociatedBss);
-		linux_wlan_set_bssid(priv->dev, NullBssid);
-		eth_zero_addr(u8ConnectedSSID);
-
-		/*Invalidate u8WLANChannel value on wlan0 disconnect*/
-		if (!pstrWFIDrv->u8P2PConnect)
-			u8WLANChannel = INVALID_CHANNEL;
-		/*Incase "P2P CLIENT Connected" send deauthentication reason by 3 to force the WPA_SUPPLICANT to directly change
-		 *      virtual interface to station*/
-		if ((pstrWFIDrv->IFC_UP) && (dev == wl->vif[1].ndev)) {
+		wilc_wlan_set_bssid(priv->dev, NullBssid);
+		eth_zero_addr(wilc_connected_ssid);
+
+		if (!pstrWFIDrv->p2p_connect)
+			wlan_channel = INVALID_CHANNEL;
+		if ((pstrWFIDrv->IFC_UP) && (dev == wl->vif[1]->ndev)) {
 			pstrDisconnectNotifInfo->u16reason = 3;
-		}
-		/*Incase "P2P CLIENT during connection(not connected)" send deauthentication reason by 1 to force the WPA_SUPPLICANT
-		 *      to scan again and retry the connection*/
-		else if ((!pstrWFIDrv->IFC_UP) && (dev == wl->vif[1].ndev)) {
+		} else if ((!pstrWFIDrv->IFC_UP) && (dev == wl->vif[1]->ndev)) {
 			pstrDisconnectNotifInfo->u16reason = 1;
 		}
 		cfg80211_disconnected(dev, pstrDisconnectNotifInfo->u16reason, pstrDisconnectNotifInfo->ie,
 				      pstrDisconnectNotifInfo->ie_len, false,
 				      GFP_KERNEL);
-
 	}
-
 }
 
-
-/**
- *  @brief      set_channel
- *  @details    Set channel for a given wireless interface. Some devices
- *                      may support multi-channel operation (by channel hopping) so cfg80211
- *                      doesn't verify much. Note, however, that the passed netdev may be
- *                      %NULL as well if the user requested changing the channel for the
- *                      device itself, or for a monitor interface.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int set_channel(struct wiphy *wiphy,
 		       struct cfg80211_chan_def *chandef)
 {
 	u32 channelnum = 0;
 	struct wilc_priv *priv;
 	int result = 0;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	channelnum = ieee80211_frequency_to_channel(chandef->chan->center_freq);
 	PRINT_D(CFG80211_DBG, "Setting channel %d with frequency %d\n", channelnum, chandef->chan->center_freq);
 
 	curr_channel = channelnum;
-	result = host_int_set_mac_chnl_num(priv->hWILCWFIDrv, channelnum);
+	result = wilc_set_mac_chnl_num(vif, channelnum);
 
 	if (result != 0)
 		PRINT_ER("Error in setting channel %d\n", channelnum);
@@ -678,19 +612,6 @@ static int set_channel(struct wiphy *wiphy,
 	return result;
 }
 
-/**
- *  @brief      scan
- *  @details    Request to do a scan. If returning zero, the scan request is given
- *                      the driver, and will be valid until passed to cfg80211_scan_done().
- *                      For scan results, call cfg80211_inform_bss(); you can call this outside
- *                      the scan/scan_done bracket too.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mabubakr
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-
 static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 {
 	struct wilc_priv *priv;
@@ -698,21 +619,20 @@ static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 	s32 s32Error = 0;
 	u8 au8ScanChanList[MAX_NUM_SCANNED_NETWORKS];
 	struct hidden_network strHiddenNetwork;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	priv->pstrScanReq = request;
 
 	priv->u32RcvdChCount = 0;
 
-	host_int_set_wfi_drv_handler(priv->hWILCWFIDrv);
-
-
-	reset_shadow_found(priv);
+	wilc_set_wfi_drv_handler(vif, wilc_get_vif_idx(vif));
+	reset_shadow_found();
 
 	priv->bCfgScanning = true;
-	if (request->n_channels <= MAX_NUM_SCANNED_NETWORKS) { /* TODO: mostafa: to be replaced by */
-		/*               max_scan_ssids */
+	if (request->n_channels <= MAX_NUM_SCANNED_NETWORKS) {
 		for (i = 0; i < request->n_channels; i++) {
 			au8ScanChanList[i] = (u8)ieee80211_frequency_to_channel(request->channels[i]->center_freq);
 			PRINT_INFO(CFG80211_DBG, "ScanChannel List[%d] = %d,", i, au8ScanChanList[i]);
@@ -724,15 +644,13 @@ static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 		PRINT_D(CFG80211_DBG, "Number of SSIDs %d\n", request->n_ssids);
 
 		if (request->n_ssids >= 1) {
-
-
 			strHiddenNetwork.pstrHiddenNetworkInfo = kmalloc(request->n_ssids * sizeof(struct hidden_network), GFP_KERNEL);
 			strHiddenNetwork.u8ssidnum = request->n_ssids;
 
 
 			for (i = 0; i < request->n_ssids; i++) {
-
-				if (request->ssids[i].ssid != NULL && request->ssids[i].ssid_len != 0) {
+				if (request->ssids[i].ssid &&
+				    request->ssids[i].ssid_len != 0) {
 					strHiddenNetwork.pstrHiddenNetworkInfo[i].pu8ssid = kmalloc(request->ssids[i].ssid_len, GFP_KERNEL);
 					memcpy(strHiddenNetwork.pstrHiddenNetworkInfo[i].pu8ssid, request->ssids[i].ssid, request->ssids[i].ssid_len);
 					strHiddenNetwork.pstrHiddenNetworkInfo[i].u8ssidlen = request->ssids[i].ssid_len;
@@ -742,18 +660,21 @@ static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 				}
 			}
 			PRINT_D(CFG80211_DBG, "Trigger Scan Request\n");
-			s32Error = host_int_scan(priv->hWILCWFIDrv, USER_SCAN, ACTIVE_SCAN,
-						 au8ScanChanList, request->n_channels,
-						 (const u8 *)request->ie, request->ie_len,
-						 CfgScanResult, (void *)priv, &strHiddenNetwork);
+			s32Error = wilc_scan(vif, USER_SCAN, ACTIVE_SCAN,
+					     au8ScanChanList,
+					     request->n_channels,
+					     (const u8 *)request->ie,
+					     request->ie_len, CfgScanResult,
+					     (void *)priv, &strHiddenNetwork);
 		} else {
 			PRINT_D(CFG80211_DBG, "Trigger Scan Request\n");
-			s32Error = host_int_scan(priv->hWILCWFIDrv, USER_SCAN, ACTIVE_SCAN,
-						 au8ScanChanList, request->n_channels,
-						 (const u8 *)request->ie, request->ie_len,
-						 CfgScanResult, (void *)priv, NULL);
+			s32Error = wilc_scan(vif, USER_SCAN, ACTIVE_SCAN,
+					     au8ScanChanList,
+					     request->n_channels,
+					     (const u8 *)request->ie,
+					     request->ie_len, CfgScanResult,
+					     (void *)priv, NULL);
 		}
-
 	} else {
 		PRINT_ER("Requested num of scanned channels is greater than the max, supported"
 			 " channels\n");
@@ -767,18 +688,6 @@ static int scan(struct wiphy *wiphy, struct cfg80211_scan_request *request)
 	return s32Error;
 }
 
-/**
- *  @brief      connect
- *  @details    Connect to the ESS with the specified parameters. When connected,
- *                      call cfg80211_connect_result() with status code %WLAN_STATUS_SUCCESS.
- *                      If the connection fails for some reason, call cfg80211_connect_result()
- *                      with the status from the AP.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mabubakr
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int connect(struct wiphy *wiphy, struct net_device *dev,
 		   struct cfg80211_connect_params *sme)
 {
@@ -793,39 +702,37 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 	struct wilc_priv *priv;
 	struct host_if_drv *pstrWFIDrv;
 	tstrNetworkInfo *pstrNetworkInfo = NULL;
+	struct wilc_vif *vif;
 
-
-	connecting = 1;
+	wilc_connecting = 1;
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 	pstrWFIDrv = (struct host_if_drv *)(priv->hWILCWFIDrv);
 
-	host_int_set_wfi_drv_handler(priv->hWILCWFIDrv);
+	wilc_set_wfi_drv_handler(vif, wilc_get_vif_idx(vif));
 
 	PRINT_D(CFG80211_DBG, "Connecting to SSID [%s] on netdev [%p] host if [%p]\n", sme->ssid, dev, priv->hWILCWFIDrv);
 	if (!(strncmp(sme->ssid, "DIRECT-", 7))) {
 		PRINT_D(CFG80211_DBG, "Connected to Direct network,OBSS disabled\n");
-		pstrWFIDrv->u8P2PConnect = 1;
-	} else
-		pstrWFIDrv->u8P2PConnect = 0;
+		pstrWFIDrv->p2p_connect = 1;
+	} else {
+		pstrWFIDrv->p2p_connect = 0;
+	}
 	PRINT_INFO(CFG80211_DBG, "Required SSID = %s\n , AuthType = %d\n", sme->ssid, sme->auth_type);
 
-	for (i = 0; i < u32LastScannedNtwrksCountShadow; i++) {
-		if ((sme->ssid_len == astrLastScannedNtwrksShadow[i].u8SsidLen) &&
-		    memcmp(astrLastScannedNtwrksShadow[i].au8ssid,
-				sme->ssid,
-				sme->ssid_len) == 0) {
+	for (i = 0; i < last_scanned_cnt; i++) {
+		if ((sme->ssid_len == last_scanned_shadow[i].u8SsidLen) &&
+		    memcmp(last_scanned_shadow[i].au8ssid,
+			   sme->ssid,
+			   sme->ssid_len) == 0) {
 			PRINT_INFO(CFG80211_DBG, "Network with required SSID is found %s\n", sme->ssid);
-			if (sme->bssid == NULL)	{
-				/* BSSID is not passed from the user, so decision of matching
-				 * is done by SSID only */
+			if (!sme->bssid) {
 				PRINT_INFO(CFG80211_DBG, "BSSID is not passed from the user\n");
 				break;
 			} else {
-				/* BSSID is also passed from the user, so decision of matching
-				 * should consider also this passed BSSID */
-				if (memcmp(astrLastScannedNtwrksShadow[i].au8bssid,
-						sme->bssid,
-						ETH_ALEN) == 0)	{
+				if (memcmp(last_scanned_shadow[i].au8bssid,
+					   sme->bssid,
+					   ETH_ALEN) == 0) {
 					PRINT_INFO(CFG80211_DBG, "BSSID is passed from the user and matched\n");
 					break;
 				}
@@ -833,10 +740,10 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 		}
 	}
 
-	if (i < u32LastScannedNtwrksCountShadow) {
+	if (i < last_scanned_cnt) {
 		PRINT_D(CFG80211_DBG, "Required bss is in scan results\n");
 
-		pstrNetworkInfo = &(astrLastScannedNtwrksShadow[i]);
+		pstrNetworkInfo = &last_scanned_shadow[i];
 
 		PRINT_INFO(CFG80211_DBG, "network BSSID to be associated: %x%x%x%x%x%x\n",
 			   pstrNetworkInfo->au8bssid[0], pstrNetworkInfo->au8bssid[1],
@@ -844,7 +751,7 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 			   pstrNetworkInfo->au8bssid[4], pstrNetworkInfo->au8bssid[5]);
 	} else {
 		s32Error = -ENOENT;
-		if (u32LastScannedNtwrksCountShadow == 0)
+		if (last_scanned_cnt == 0)
 			PRINT_D(CFG80211_DBG, "No Scan results yet\n");
 		else
 			PRINT_D(CFG80211_DBG, "Required bss not in scan results: Error(%d)\n", s32Error);
@@ -867,8 +774,6 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 	}
 
 	if (sme->crypto.cipher_group != NO_ENCRYPT) {
-		/* To determine the u8security value, first we check the group cipher suite then {in case of WPA or WPA2}
-		 *  we will add to it the pairwise cipher suite(s) */
 		pcwpa_version = "Default";
 		PRINT_D(CORECONFIG_DBG, ">> sme->crypto.wpa_versions: %x\n", sme->crypto.wpa_versions);
 		if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP40) {
@@ -891,8 +796,9 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 			g_key_wep_params.key_idx = sme->key_idx;
 			g_wep_keys_saved = true;
 
-			host_int_set_wep_default_key(priv->hWILCWFIDrv, sme->key_idx);
-			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, sme->key, sme->key_len, sme->key_idx);
+			wilc_set_wep_default_keyid(vif, sme->key_idx);
+			wilc_add_wep_key_bss_sta(vif, sme->key, sme->key_len,
+						 sme->key_idx);
 		} else if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_WEP104)   {
 			u8security = ENCRYPT_ENABLED | WEP | WEP_EXTENDED;
 			pcgroup_encrypt_val = "WEP104";
@@ -908,15 +814,15 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 			g_key_wep_params.key_idx = sme->key_idx;
 			g_wep_keys_saved = true;
 
-			host_int_set_wep_default_key(priv->hWILCWFIDrv, sme->key_idx);
-			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, sme->key, sme->key_len, sme->key_idx);
+			wilc_set_wep_default_keyid(vif, sme->key_idx);
+			wilc_add_wep_key_bss_sta(vif, sme->key, sme->key_len,
+						 sme->key_idx);
 		} else if (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)   {
 			if (sme->crypto.cipher_group == WLAN_CIPHER_SUITE_TKIP)	{
 				u8security = ENCRYPT_ENABLED | WPA2 | TKIP;
 				pcgroup_encrypt_val = "WPA2_TKIP";
 				pccipher_group = "TKIP";
-			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
-				     /* tenuSecurity_t = WPA2_AES; */
+			} else {
 				u8security = ENCRYPT_ENABLED | WPA2 | AES;
 				pcgroup_encrypt_val = "WPA2_AES";
 				pccipher_group = "AES";
@@ -927,12 +833,10 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 				u8security = ENCRYPT_ENABLED | WPA | TKIP;
 				pcgroup_encrypt_val = "WPA_TKIP";
 				pccipher_group = "TKIP";
-			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
-				     /* tenuSecurity_t = WPA_AES; */
+			} else {
 				u8security = ENCRYPT_ENABLED | WPA | AES;
 				pcgroup_encrypt_val = "WPA_AES";
 				pccipher_group = "AES";
-
 			}
 			pcwpa_version = "WPA_VERSION_1";
 
@@ -942,17 +846,14 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 
 			goto done;
 		}
-
 	}
 
-	/* After we set the u8security value from checking the group cipher suite, {in case of WPA or WPA2} we will
-	 *   add to it the pairwise cipher suite(s) */
 	if ((sme->crypto.wpa_versions & NL80211_WPA_VERSION_1)
 	    || (sme->crypto.wpa_versions & NL80211_WPA_VERSION_2)) {
 		for (i = 0; i < sme->crypto.n_ciphers_pairwise; i++) {
 			if (sme->crypto.ciphers_pairwise[i] == WLAN_CIPHER_SUITE_TKIP) {
 				u8security = u8security | TKIP;
-			} else {     /* TODO: mostafa: here we assume that any other encryption type is AES */
+			} else {
 				u8security = u8security | AES;
 			}
 		}
@@ -976,8 +877,6 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 		PRINT_D(CFG80211_DBG, "Automatic Authentation type = %d\n", sme->auth_type);
 	}
 
-
-	/* ai: key_mgmt: enterprise case */
 	if (sme->crypto.n_akm_suites) {
 		switch (sme->crypto.akm_suites[0]) {
 		case WLAN_AKM_SUITE_8021X:
@@ -997,19 +896,19 @@ static int connect(struct wiphy *wiphy, struct net_device *dev,
 
 	curr_channel = pstrNetworkInfo->u8channel;
 
-	if (!pstrWFIDrv->u8P2PConnect) {
-		u8WLANChannel = pstrNetworkInfo->u8channel;
-	}
+	if (!pstrWFIDrv->p2p_connect)
+		wlan_channel = pstrNetworkInfo->u8channel;
 
-	linux_wlan_set_bssid(dev, pstrNetworkInfo->au8bssid);
+	wilc_wlan_set_bssid(dev, pstrNetworkInfo->au8bssid);
 
-	s32Error = host_int_set_join_req(priv->hWILCWFIDrv, pstrNetworkInfo->au8bssid, sme->ssid,
-					 sme->ssid_len, sme->ie, sme->ie_len,
-					 CfgConnectResult, (void *)priv, u8security,
-					 tenuAuth_type, pstrNetworkInfo->u8channel,
-					 pstrNetworkInfo->pJoinParams);
+	s32Error = wilc_set_join_req(vif, pstrNetworkInfo->au8bssid, sme->ssid,
+				     sme->ssid_len, sme->ie, sme->ie_len,
+				     CfgConnectResult, (void *)priv,
+				     u8security, tenuAuth_type,
+				     pstrNetworkInfo->u8channel,
+				     pstrNetworkInfo->pJoinParams);
 	if (s32Error != 0) {
-		PRINT_ER("host_int_set_join_req(): Error(%d)\n", s32Error);
+		PRINT_ER("wilc_set_join_req(): Error(%d)\n", s32Error);
 		s32Error = -ENOENT;
 		goto done;
 	}
@@ -1019,40 +918,31 @@ done:
 	return s32Error;
 }
 
-
-/**
- *  @brief      disconnect
- *  @details    Disconnect from the BSS/ESS.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int disconnect(struct wiphy *wiphy, struct net_device *dev, u16 reason_code)
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
 	struct host_if_drv *pstrWFIDrv;
+	struct wilc_vif *vif;
 	u8 NullBssid[ETH_ALEN] = {0};
 
-	connecting = 0;
+	wilc_connecting = 0;
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
-	/*Invalidate u8WLANChannel value on wlan0 disconnect*/
 	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
-	if (!pstrWFIDrv->u8P2PConnect)
-		u8WLANChannel = INVALID_CHANNEL;
-	linux_wlan_set_bssid(priv->dev, NullBssid);
+	if (!pstrWFIDrv->p2p_connect)
+		wlan_channel = INVALID_CHANNEL;
+	wilc_wlan_set_bssid(priv->dev, NullBssid);
 
 	PRINT_D(CFG80211_DBG, "Disconnecting with reason code(%d)\n", reason_code);
 
-	u8P2Plocalrandom = 0x01;
-	u8P2Precvrandom = 0x00;
-	bWilc_ie = false;
-	pstrWFIDrv->u64P2p_MgmtTimeout = 0;
+	p2p_local_random = 0x01;
+	p2p_recv_random = 0x00;
+	wilc_ie = false;
+	pstrWFIDrv->p2p_timeout = 0;
 
-	s32Error = host_int_disconnect(priv->hWILCWFIDrv, reason_code);
+	s32Error = wilc_disconnect(vif, reason_code);
 	if (s32Error != 0) {
 		PRINT_ER("Error in disconnecting: Error(%d)\n", s32Error);
 		s32Error = -EINVAL;
@@ -1061,16 +951,6 @@ static int disconnect(struct wiphy *wiphy, struct net_device *dev, u16 reason_co
 	return s32Error;
 }
 
-/**
- *  @brief      add_key
- *  @details    Add a key with the given parameters. @mac_addr will be %NULL
- *                      when adding a group key.
- *  @param[in] key : key buffer; TKIP: 16-byte temporal key, 8-byte Tx Mic key, 8-byte Rx Mic Key
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 		   bool pairwise,
 		   const u8 *mac_addr, struct key_params *params)
@@ -1086,11 +966,11 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 	u8 u8pmode = NO_ENCRYPT;
 	enum AUTHTYPE tenuAuth_type = ANY;
 	struct wilc *wl;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(netdev);
-	wl = nic->wilc;
+	vif = netdev_priv(netdev);
+	wl = vif->wilc;
 
 	PRINT_D(CFG80211_DBG, "Adding key with cipher suite = %x\n", params->cipher);
 
@@ -1105,7 +985,6 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
 		if (priv->wdev->iftype == NL80211_IFTYPE_AP) {
-
 			priv->WILC_WFI_wep_default = key_index;
 			priv->WILC_WFI_wep_key_len[key_index] = params->key_len;
 			memcpy(priv->WILC_WFI_wep_key[key_index], params->key, params->key_len);
@@ -1123,7 +1002,9 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 			else
 				u8mode = ENCRYPT_ENABLED | WEP | WEP_EXTENDED;
 
-			host_int_add_wep_key_bss_ap(priv->hWILCWFIDrv, params->key, params->key_len, key_index, u8mode, tenuAuth_type);
+			wilc_add_wep_key_bss_ap(vif, params->key,
+						params->key_len, key_index,
+						u8mode, tenuAuth_type);
 			break;
 		}
 		if (memcmp(params->key, priv->WILC_WFI_wep_key[key_index], params->key_len)) {
@@ -1137,7 +1018,8 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				for (i = 0; i < params->key_len; i++)
 					PRINT_INFO(CFG80211_DBG, "WEP key value[%d] = %d\n", i, params->key[i]);
 			}
-			host_int_add_wep_key_bss_sta(priv->hWILCWFIDrv, params->key, params->key_len, key_index);
+			wilc_add_wep_key_bss_sta(vif, params->key,
+						 params->key_len, key_index);
 		}
 
 		break;
@@ -1145,14 +1027,12 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 	case WLAN_CIPHER_SUITE_TKIP:
 	case WLAN_CIPHER_SUITE_CCMP:
 		if (priv->wdev->iftype == NL80211_IFTYPE_AP || priv->wdev->iftype == NL80211_IFTYPE_P2P_GO) {
-
-			if (priv->wilc_gtk[key_index] == NULL) {
+			if (!priv->wilc_gtk[key_index]) {
 				priv->wilc_gtk[key_index] = kmalloc(sizeof(struct wilc_wfi_key), GFP_KERNEL);
 				priv->wilc_gtk[key_index]->key = NULL;
 				priv->wilc_gtk[key_index]->seq = NULL;
-
 			}
-			if (priv->wilc_ptk[key_index] == NULL) {
+			if (!priv->wilc_ptk[key_index]) {
 				priv->wilc_ptk[key_index] = kmalloc(sizeof(struct wilc_wfi_key), GFP_KERNEL);
 				priv->wilc_ptk[key_index]->key = NULL;
 				priv->wilc_ptk[key_index]->seq = NULL;
@@ -1169,18 +1049,14 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				priv->wilc_groupkey = u8gmode;
 
 				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
-
 					pu8TxMic = params->key + 24;
 					pu8RxMic = params->key + 16;
 					KeyLen = params->key_len - 16;
 				}
-				/* if there has been previous allocation for the same index through its key, free that memory and allocate again*/
 				kfree(priv->wilc_gtk[key_index]->key);
 
 				priv->wilc_gtk[key_index]->key = kmalloc(params->key_len, GFP_KERNEL);
 				memcpy(priv->wilc_gtk[key_index]->key, params->key, params->key_len);
-
-				/* if there has been previous allocation for the same index through its seq, free that memory and allocate again*/
 				kfree(priv->wilc_gtk[key_index]->seq);
 
 				if ((params->seq_len) > 0) {
@@ -1200,8 +1076,10 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				}
 
 
-				host_int_add_rx_gtk(priv->hWILCWFIDrv, params->key, KeyLen,
-						    key_index, params->seq_len, params->seq, pu8RxMic, pu8TxMic, AP_MODE, u8gmode);
+				wilc_add_rx_gtk(vif, params->key, KeyLen,
+						key_index, params->seq_len,
+						params->seq, pu8RxMic,
+						pu8TxMic, AP_MODE, u8gmode);
 
 			} else {
 				PRINT_INFO(CFG80211_DBG, "STA Address: %x%x%x%x%x\n", mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4]);
@@ -1213,7 +1091,6 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 
 
 				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
-
 					pu8TxMic = params->key + 24;
 					pu8RxMic = params->key + 16;
 					KeyLen = params->key_len - 16;
@@ -1245,8 +1122,9 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 				priv->wilc_ptk[key_index]->key_len = params->key_len;
 				priv->wilc_ptk[key_index]->seq_len = params->seq_len;
 
-				host_int_add_ptk(priv->hWILCWFIDrv, params->key, KeyLen, mac_addr,
-						 pu8RxMic, pu8TxMic, AP_MODE, u8pmode, key_index);
+				wilc_add_ptk(vif, params->key, KeyLen,
+					     mac_addr, pu8RxMic, pu8TxMic,
+					     AP_MODE, u8pmode, key_index);
 			}
 			break;
 		}
@@ -1255,14 +1133,12 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 			u8mode = 0;
 			if (!pairwise) {
 				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
-					/* swap the tx mic by rx mic */
 					pu8RxMic = params->key + 24;
 					pu8TxMic = params->key + 16;
 					KeyLen = params->key_len - 16;
 				}
 
-				/*save keys only on interface 0 (wifi interface)*/
-				if (!g_gtk_keys_saved && netdev == wl->vif[0].ndev) {
+				if (!g_gtk_keys_saved && netdev == wl->vif[0]->ndev) {
 					g_add_gtk_key_params.key_idx = key_index;
 					g_add_gtk_key_params.pairwise = pairwise;
 					if (!mac_addr) {
@@ -1287,18 +1163,19 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 					g_gtk_keys_saved = true;
 				}
 
-				host_int_add_rx_gtk(priv->hWILCWFIDrv, params->key, KeyLen,
-						    key_index, params->seq_len, params->seq, pu8RxMic, pu8TxMic, STATION_MODE, u8mode);
+				wilc_add_rx_gtk(vif, params->key, KeyLen,
+						key_index, params->seq_len,
+						params->seq, pu8RxMic,
+						pu8TxMic, STATION_MODE,
+						u8mode);
 			} else {
 				if (params->key_len > 16 && params->cipher == WLAN_CIPHER_SUITE_TKIP) {
-					/* swap the tx mic by rx mic */
 					pu8RxMic = params->key + 24;
 					pu8TxMic = params->key + 16;
 					KeyLen = params->key_len - 16;
 				}
 
-				/*save keys only on interface 0 (wifi interface)*/
-				if (!g_ptk_keys_saved && netdev == wl->vif[0].ndev) {
+				if (!g_ptk_keys_saved && netdev == wl->vif[0]->ndev) {
 					g_add_ptk_key_params.key_idx = key_index;
 					g_add_ptk_key_params.pairwise = pairwise;
 					if (!mac_addr) {
@@ -1323,8 +1200,9 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 					g_ptk_keys_saved = true;
 				}
 
-				host_int_add_ptk(priv->hWILCWFIDrv, params->key, KeyLen, mac_addr,
-						 pu8RxMic, pu8TxMic, STATION_MODE, u8mode, key_index);
+				wilc_add_ptk(vif, params->key, KeyLen,
+					     mac_addr, pu8RxMic, pu8TxMic,
+					     STATION_MODE, u8mode, key_index);
 				PRINT_D(CFG80211_DBG, "Adding pairwise key\n");
 				if (INFO) {
 					for (i = 0; i < params->key_len; i++)
@@ -1337,22 +1215,11 @@ static int add_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 	default:
 		PRINT_ER("Not supported cipher: Error(%d)\n", s32Error);
 		s32Error = -ENOTSUPP;
-
 	}
 
 	return s32Error;
 }
 
-/**
- *  @brief      del_key
- *  @details    Remove a key given the @mac_addr (%NULL for a group key)
- *                      and @key_index, return -ENOENT if the key doesn't exist.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 		   u8 key_index,
 		   bool pairwise,
@@ -1360,26 +1227,21 @@ static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 {
 	struct wilc_priv *priv;
 	struct wilc *wl;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(netdev);
-	wl = nic->wilc;
+	vif = netdev_priv(netdev);
+	wl = vif->wilc;
 
-	/*delete saved keys, if any*/
-	if (netdev == wl->vif[0].ndev) {
+	if (netdev == wl->vif[0]->ndev) {
 		g_ptk_keys_saved = false;
 		g_gtk_keys_saved = false;
 		g_wep_keys_saved = false;
 
-		/*Delete saved WEP keys params, if any*/
 		kfree(g_key_wep_params.key);
 		g_key_wep_params.key = NULL;
 
-		/*freeing memory allocated by "wilc_gtk" and "wilc_ptk" in "WILC_WIFI_ADD_KEY"*/
-
 		if ((priv->wilc_gtk[key_index]) != NULL) {
-
 			kfree(priv->wilc_gtk[key_index]->key);
 			priv->wilc_gtk[key_index]->key = NULL;
 			kfree(priv->wilc_gtk[key_index]->seq);
@@ -1387,11 +1249,9 @@ static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 
 			kfree(priv->wilc_gtk[key_index]);
 			priv->wilc_gtk[key_index] = NULL;
-
 		}
 
 		if ((priv->wilc_ptk[key_index]) != NULL) {
-
 			kfree(priv->wilc_ptk[key_index]->key);
 			priv->wilc_ptk[key_index]->key = NULL;
 			kfree(priv->wilc_ptk[key_index]->seq);
@@ -1400,7 +1260,6 @@ static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 			priv->wilc_ptk[key_index] = NULL;
 		}
 
-		/*Delete saved PTK and GTK keys params, if any*/
 		kfree(g_key_ptk_params.key);
 		g_key_ptk_params.key = NULL;
 		kfree(g_key_ptk_params.seq);
@@ -1411,8 +1270,7 @@ static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 		kfree(g_key_gtk_params.seq);
 		g_key_gtk_params.seq = NULL;
 
-		/*Reset WILC_CHANGING_VIR_IF register to allow adding futrue keys to CE H/W*/
-		Set_machw_change_vir_if(netdev, false);
+		wilc_set_machw_change_vir_if(netdev, false);
 	}
 
 	if (key_index >= 0 && key_index <= 3) {
@@ -1420,28 +1278,15 @@ static int del_key(struct wiphy *wiphy, struct net_device *netdev,
 		priv->WILC_WFI_wep_key_len[key_index] = 0;
 
 		PRINT_D(CFG80211_DBG, "Removing WEP key with index = %d\n", key_index);
-		host_int_remove_wep_key(priv->hWILCWFIDrv, key_index);
+		wilc_remove_wep_key(vif, key_index);
 	} else {
 		PRINT_D(CFG80211_DBG, "Removing all installed keys\n");
-		host_int_remove_key(priv->hWILCWFIDrv, mac_addr);
+		wilc_remove_key(priv->hWILCWFIDrv, mac_addr);
 	}
 
 	return 0;
 }
 
-/**
- *  @brief      get_key
- *  @details    Get information about the key with the given parameters.
- *                      @mac_addr will be %NULL when requesting information for a group
- *                      key. All pointers given to the @callback function need not be valid
- *                      after it returns. This function should return an error if it is
- *                      not possible to retrieve the key, -ENOENT if it doesn't exist.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int get_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 		   bool pairwise,
 		   const u8 *mac_addr, void *cookie, void (*callback)(void *cookie, struct key_params *))
@@ -1477,69 +1322,48 @@ static int get_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 
 	callback(cookie, &key_params);
 
-	return 0;        /* priv->wilc_gtk->key_len ?0 : -ENOENT; */
+	return 0;
 }
 
-/**
- *  @brief      set_default_key
- *  @details    Set the default management frame key on an interface
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int set_default_key(struct wiphy *wiphy, struct net_device *netdev, u8 key_index,
 			   bool unicast, bool multicast)
 {
 	struct wilc_priv *priv;
-
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	PRINT_D(CFG80211_DBG, "Setting default key with idx = %d\n", key_index);
 
 	if (key_index != priv->WILC_WFI_wep_default) {
-
-		host_int_set_wep_default_key(priv->hWILCWFIDrv, key_index);
+		wilc_set_wep_default_keyid(vif, key_index);
 	}
 
 	return 0;
 }
 
-/**
- *  @brief      get_station
- *  @details    Get station information for the station identified by @mac
- *  @param[in]   NONE
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-
 static int get_station(struct wiphy *wiphy, struct net_device *dev,
 		       const u8 *mac, struct station_info *sinfo)
 {
 	struct wilc_priv *priv;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	u32 i = 0;
 	u32 associatedsta = 0;
 	u32 inactive_time = 0;
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(dev);
+	vif = netdev_priv(dev);
 
-	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+	if (vif->iftype == AP_MODE || vif->iftype == GO_MODE) {
 		PRINT_D(HOSTAPD_DBG, "Getting station parameters\n");
 
 		PRINT_INFO(HOSTAPD_DBG, ": %x%x%x%x%x\n", mac[0], mac[1], mac[2], mac[3], mac[4]);
 
 		for (i = 0; i < NUM_STA_ASSOCIATED; i++) {
-
 			if (!(memcmp(mac, priv->assoc_stainfo.au8Sta_AssociatedBss[i], ETH_ALEN))) {
 				associatedsta = i;
 				break;
 			}
-
 		}
 
 		if (associatedsta == -1) {
@@ -1549,16 +1373,15 @@ static int get_station(struct wiphy *wiphy, struct net_device *dev,
 
 		sinfo->filled |= BIT(NL80211_STA_INFO_INACTIVE_TIME);
 
-		host_int_get_inactive_time(priv->hWILCWFIDrv, mac, &(inactive_time));
+		wilc_get_inactive_time(vif, mac, &inactive_time);
 		sinfo->inactive_time = 1000 * inactive_time;
 		PRINT_D(CFG80211_DBG, "Inactive time %d\n", sinfo->inactive_time);
-
 	}
 
-	if (nic->iftype == STATION_MODE) {
+	if (vif->iftype == STATION_MODE) {
 		struct rf_info strStatistics;
 
-		host_int_get_statistics(priv->hWILCWFIDrv, &strStatistics);
+		wilc_get_statistics(vif, &strStatistics);
 
 		sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL) |
 						BIT(NL80211_STA_INFO_RX_PACKETS) |
@@ -1566,16 +1389,17 @@ static int get_station(struct wiphy *wiphy, struct net_device *dev,
 						BIT(NL80211_STA_INFO_TX_FAILED) |
 						BIT(NL80211_STA_INFO_TX_BITRATE);
 
-		sinfo->signal		=  strStatistics.s8RSSI;
-		sinfo->rx_packets   =  strStatistics.u32RxCount;
-		sinfo->tx_packets   =  strStatistics.u32TxCount + strStatistics.u32TxFailureCount;
-		sinfo->tx_failed	=  strStatistics.u32TxFailureCount;
-		sinfo->txrate.legacy = strStatistics.u8LinkSpeed * 10;
+		sinfo->signal = strStatistics.rssi;
+		sinfo->rx_packets = strStatistics.rx_cnt;
+		sinfo->tx_packets = strStatistics.tx_cnt + strStatistics.tx_fail_cnt;
+		sinfo->tx_failed = strStatistics.tx_fail_cnt;
+		sinfo->txrate.legacy = strStatistics.link_speed * 10;
 
-		if ((strStatistics.u8LinkSpeed > TCP_ACK_FILTER_LINK_SPEED_THRESH) && (strStatistics.u8LinkSpeed != DEFAULT_LINK_SPEED))
-			Enable_TCP_ACK_Filter(true);
-		else if (strStatistics.u8LinkSpeed != DEFAULT_LINK_SPEED)
-			Enable_TCP_ACK_Filter(false);
+		if ((strStatistics.link_speed > TCP_ACK_FILTER_LINK_SPEED_THRESH) &&
+		    (strStatistics.link_speed != DEFAULT_LINK_SPEED))
+			wilc_enable_tcp_ack_filter(true);
+		else if (strStatistics.link_speed != DEFAULT_LINK_SPEED)
+			wilc_enable_tcp_ack_filter(false);
 
 		PRINT_D(CORECONFIG_DBG, "*** stats[%d][%d][%d][%d][%d]\n", sinfo->signal, sinfo->rx_packets, sinfo->tx_packets,
 			sinfo->tx_failed, sinfo->txrate.legacy);
@@ -1583,28 +1407,6 @@ static int get_station(struct wiphy *wiphy, struct net_device *dev,
 	return 0;
 }
 
-
-/**
- *  @brief      change_bss
- *  @details    Modify parameters for a given BSS.
- *  @param[in]
- *   -use_cts_prot: Whether to use CTS protection
- *	    (0 = no, 1 = yes, -1 = do not change)
- *  -use_short_preamble: Whether the use of short preambles is allowed
- *	    (0 = no, 1 = yes, -1 = do not change)
- *  -use_short_slot_time: Whether the use of short slot time is allowed
- *	    (0 = no, 1 = yes, -1 = do not change)
- *  -basic_rates: basic rates in IEEE 802.11 format
- *	    (or NULL for no change)
- *  -basic_rates_len: number of basic rates
- *  -ap_isolate: do not forward packets between connected stations
- *  -ht_opmode: HT Operation mode
- *         (u16 = opmode, -1 = do not change)
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int change_bss(struct wiphy *wiphy, struct net_device *dev,
 		      struct bss_parameters *params)
 {
@@ -1612,23 +1414,15 @@ static int change_bss(struct wiphy *wiphy, struct net_device *dev,
 	return 0;
 }
 
-/**
- *  @brief      set_wiphy_params
- *  @details    Notify that wiphy parameters have changed;
- *  @param[in]   Changed bitfield (see &enum wiphy_params_flags) describes which values
- *                      have changed.
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int set_wiphy_params(struct wiphy *wiphy, u32 changed)
 {
 	s32 s32Error = 0;
 	struct cfg_param_val pstrCfgParamVal;
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	pstrCfgParamVal.flag = 0;
 	PRINT_D(CFG80211_DBG, "Setting Wiphy params\n");
@@ -1640,17 +1434,14 @@ static int set_wiphy_params(struct wiphy *wiphy, u32 changed)
 		pstrCfgParamVal.short_retry_limit = priv->dev->ieee80211_ptr->wiphy->retry_short;
 	}
 	if (changed & WIPHY_PARAM_RETRY_LONG) {
-
 		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_RETRY_LONG %d\n", priv->dev->ieee80211_ptr->wiphy->retry_long);
 		pstrCfgParamVal.flag |= RETRY_LONG;
 		pstrCfgParamVal.long_retry_limit = priv->dev->ieee80211_ptr->wiphy->retry_long;
-
 	}
 	if (changed & WIPHY_PARAM_FRAG_THRESHOLD) {
 		PRINT_D(CFG80211_DBG, "Setting WIPHY_PARAM_FRAG_THRESHOLD %d\n", priv->dev->ieee80211_ptr->wiphy->frag_threshold);
 		pstrCfgParamVal.flag |= FRAG_THRESHOLD;
 		pstrCfgParamVal.frag_threshold = priv->dev->ieee80211_ptr->wiphy->frag_threshold;
-
 	}
 
 	if (changed & WIPHY_PARAM_RTS_THRESHOLD) {
@@ -1658,11 +1449,10 @@ static int set_wiphy_params(struct wiphy *wiphy, u32 changed)
 
 		pstrCfgParamVal.flag |= RTS_THRESHOLD;
 		pstrCfgParamVal.rts_threshold = priv->dev->ieee80211_ptr->wiphy->rts_threshold;
-
 	}
 
 	PRINT_D(CFG80211_DBG, "Setting CFG params in the host interface\n");
-	s32Error = hif_set_cfg(priv->hWILCWFIDrv, &pstrCfgParamVal);
+	s32Error = wilc_hif_set_cfg(vif, &pstrCfgParamVal);
 	if (s32Error)
 		PRINT_ER("Error in setting WIPHY PARAMS\n");
 
@@ -1670,33 +1460,22 @@ static int set_wiphy_params(struct wiphy *wiphy, u32 changed)
 	return s32Error;
 }
 
-/**
- *  @brief      set_pmksa
- *  @details    Cache a PMKID for a BSSID. This is mostly useful for fullmac
- *                      devices running firmwares capable of generating the (re) association
- *                      RSN IE. It allows for faster roaming between WPA2 BSSIDs.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 		     struct cfg80211_pmksa *pmksa)
 {
 	u32 i;
 	s32 s32Error = 0;
 	u8 flag = 0;
-
+	struct wilc_vif *vif;
 	struct wilc_priv *priv = wiphy_priv(wiphy);
 
+	vif = netdev_priv(priv->dev);
 	PRINT_D(CFG80211_DBG, "Setting PMKSA\n");
 
 
 	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
 		if (!memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
 				 ETH_ALEN)) {
-			/*If bssid already exists and pmkid value needs to reset*/
 			flag = PMKID_FOUND;
 			PRINT_D(CFG80211_DBG, "PMKID already exists\n");
 			break;
@@ -1717,24 +1496,14 @@ static int set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 
 	if (!s32Error) {
 		PRINT_D(CFG80211_DBG, "Setting pmkid in the host interface\n");
-		s32Error = host_int_set_pmkid_info(priv->hWILCWFIDrv, &priv->pmkid_list);
+		s32Error = wilc_set_pmkid_info(vif, &priv->pmkid_list);
 	}
 	return s32Error;
 }
 
-/**
- *  @brief      del_pmksa
- *  @details    Delete a cached PMKID.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 		     struct cfg80211_pmksa *pmksa)
 {
-
 	u32 i;
 	s32 s32Error = 0;
 
@@ -1745,7 +1514,6 @@ static int del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 	for (i = 0; i < priv->pmkid_list.numpmkid; i++)	{
 		if (!memcmp(pmksa->bssid, priv->pmkid_list.pmkidlist[i].bssid,
 				 ETH_ALEN)) {
-			/*If bssid is found, reset the values*/
 			PRINT_D(CFG80211_DBG, "Reseting PMKID values\n");
 			memset(&priv->pmkid_list.pmkidlist[i], 0, sizeof(struct host_if_pmkid));
 			break;
@@ -1769,43 +1537,18 @@ static int del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
 	return s32Error;
 }
 
-/**
- *  @brief      flush_pmksa
- *  @details    Flush all cached PMKIDs.
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int flush_pmksa(struct wiphy *wiphy, struct net_device *netdev)
 {
 	struct wilc_priv *priv = wiphy_priv(wiphy);
 
 	PRINT_D(CFG80211_DBG,  "Flushing  PMKID key values\n");
 
-	/*Get cashed Pmkids and set all with zeros*/
 	memset(&priv->pmkid_list, 0, sizeof(struct host_if_pmkid_attr));
 
 	return 0;
 }
 
-
-/**
- *  @brief      WILC_WFI_CfgParseRxAction
- *  @details Function parses the received  frames and modifies the following attributes:
- *                -GO Intent
- *		    -Channel list
- *		    -Operating Channel
- *
- *  @param[in] u8* Buffer, u32 length
- *  @return     NONE.
- *  @author	mdaftedar
- *  @date	12 DEC 2012
- *  @version
- */
-
-void WILC_WFI_CfgParseRxAction(u8 *buf, u32 len)
+static void WILC_WFI_CfgParseRxAction(u8 *buf, u32 len)
 {
 	u32 index = 0;
 	u32 i = 0, j = 0;
@@ -1822,42 +1565,30 @@ void WILC_WFI_CfgParseRxAction(u8 *buf, u32 len)
 			channel_list_attr_index = index;
 		else if (buf[index] ==  OPERCHAN_ATTR_ID)
 			op_channel_attr_index = index;
-		index += buf[index + 1] + 3; /* ID,Length byte */
+		index += buf[index + 1] + 3;
 	}
-	if (u8WLANChannel != INVALID_CHANNEL) {
-
-		/*Modify channel list attribute*/
+	if (wlan_channel != INVALID_CHANNEL) {
 		if (channel_list_attr_index) {
 			PRINT_D(GENERIC_DBG, "Modify channel list attribute\n");
 			for (i = channel_list_attr_index + 3; i < ((channel_list_attr_index + 3) + buf[channel_list_attr_index + 1]); i++) {
 				if (buf[i] == 0x51) {
 					for (j = i + 2; j < ((i + 2) + buf[i + 1]); j++) {
-						buf[j] = u8WLANChannel;
+						buf[j] = wlan_channel;
 					}
 					break;
 				}
 			}
 		}
-		/*Modify operating channel attribute*/
+
 		if (op_channel_attr_index) {
 			PRINT_D(GENERIC_DBG, "Modify operating channel attribute\n");
 			buf[op_channel_attr_index + 6] = 0x51;
-			buf[op_channel_attr_index + 7] = u8WLANChannel;
+			buf[op_channel_attr_index + 7] = wlan_channel;
 		}
 	}
 }
 
-/**
- *  @brief      WILC_WFI_CfgParseTxAction
- *  @details Function parses the transmitted  action frames and modifies the
- *               GO Intent attribute
- *  @param[in] u8* Buffer, u32 length, bool bOperChan, u8 iftype
- *  @return     NONE.
- *  @author	mdaftedar
- *  @date	12 DEC 2012
- *  @version
- */
-void WILC_WFI_CfgParseTxAction(u8 *buf, u32 len, bool bOperChan, u8 iftype)
+static void WILC_WFI_CfgParseTxAction(u8 *buf, u32 len, bool bOperChan, u8 iftype)
 {
 	u32 index = 0;
 	u32 i = 0, j = 0;
@@ -1876,44 +1607,31 @@ void WILC_WFI_CfgParseTxAction(u8 *buf, u32 len, bool bOperChan, u8 iftype)
 			channel_list_attr_index = index;
 		else if (buf[index] ==  OPERCHAN_ATTR_ID)
 			op_channel_attr_index = index;
-		index += buf[index + 1] + 3; /* ID,Length byte */
+		index += buf[index + 1] + 3;
 	}
-	if (u8WLANChannel != INVALID_CHANNEL && bOperChan) {
-
-		/*Modify channel list attribute*/
+	if (wlan_channel != INVALID_CHANNEL && bOperChan) {
 		if (channel_list_attr_index) {
 			PRINT_D(GENERIC_DBG, "Modify channel list attribute\n");
 			for (i = channel_list_attr_index + 3; i < ((channel_list_attr_index + 3) + buf[channel_list_attr_index + 1]); i++) {
 				if (buf[i] == 0x51) {
 					for (j = i + 2; j < ((i + 2) + buf[i + 1]); j++) {
-						buf[j] = u8WLANChannel;
+						buf[j] = wlan_channel;
 					}
 					break;
 				}
 			}
 		}
-		/*Modify operating channel attribute*/
+
 		if (op_channel_attr_index) {
 			PRINT_D(GENERIC_DBG, "Modify operating channel attribute\n");
 			buf[op_channel_attr_index + 6] = 0x51;
-			buf[op_channel_attr_index + 7] = u8WLANChannel;
+			buf[op_channel_attr_index + 7] = wlan_channel;
 		}
 	}
 }
 
-/*  @brief                       WILC_WFI_p2p_rx
- *  @details
- *  @param[in]
- *
- *  @return             None
- *  @author		Mai Daftedar
- *  @date			2 JUN 2013
- *  @version		1.0
- */
-
 void WILC_WFI_p2p_rx (struct net_device *dev, u8 *buff, u32 size)
 {
-
 	struct wilc_priv *priv;
 	u32 header, pkt_offset;
 	struct host_if_drv *pstrWFIDrv;
@@ -1923,11 +1641,8 @@ void WILC_WFI_p2p_rx (struct net_device *dev, u8 *buff, u32 size)
 	priv = wiphy_priv(dev->ieee80211_ptr->wiphy);
 	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
 
-	/* Get WILC header */
 	memcpy(&header, (buff - HOST_HDR_OFFSET), HOST_HDR_OFFSET);
 
-	/* The packet offset field conain info about what type of managment frame */
-	/* we are dealing with and ack status */
 	pkt_offset = GET_PKT_OFFSET(header);
 
 	if (pkt_offset & IS_MANAGMEMENT_CALLBACK) {
@@ -1948,21 +1663,18 @@ void WILC_WFI_p2p_rx (struct net_device *dev, u8 *buff, u32 size)
 			return;
 		}
 	} else {
-
 		PRINT_D(GENERIC_DBG, "Rx Frame Type:%x\n", buff[FRAME_TYPE_ID]);
 
-		/*Upper layer is informed that the frame is received on this freq*/
 		s32Freq = ieee80211_channel_to_frequency(curr_channel, IEEE80211_BAND_2GHZ);
 
 		if (ieee80211_is_action(buff[FRAME_TYPE_ID])) {
 			PRINT_D(GENERIC_DBG, "Rx Action Frame Type: %x %x\n", buff[ACTION_SUBTYPE_ID], buff[P2P_PUB_ACTION_SUBTYPE]);
 
-			if (priv->bCfgScanning && time_after_eq(jiffies, (unsigned long)pstrWFIDrv->u64P2p_MgmtTimeout)) {
+			if (priv->bCfgScanning && time_after_eq(jiffies, (unsigned long)pstrWFIDrv->p2p_timeout)) {
 				PRINT_D(GENERIC_DBG, "Receiving action frames from wrong channels\n");
 				return;
 			}
 			if (buff[ACTION_CAT_ID] == PUB_ACTION_ATTR_ID) {
-
 				switch (buff[ACTION_SUBTYPE_ID]) {
 				case GAS_INTIAL_REQ:
 					PRINT_D(GENERIC_DBG, "GAS INITIAL REQ %x\n", buff[ACTION_SUBTYPE_ID]);
@@ -1973,39 +1685,37 @@ void WILC_WFI_p2p_rx (struct net_device *dev, u8 *buff, u32 size)
 					break;
 
 				case PUBLIC_ACT_VENDORSPEC:
-					/*Now we have a public action vendor specific action frame, check if its a p2p public action frame
-					 * based on the standard its should have the p2p_oui attribute with the following values 50 6f 9A 09*/
-					if (!memcmp(u8P2P_oui, &buff[ACTION_SUBTYPE_ID + 1], 4)) {
+					if (!memcmp(p2p_oui, &buff[ACTION_SUBTYPE_ID + 1], 4)) {
 						if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP))	{
-							if (!bWilc_ie) {
+							if (!wilc_ie) {
 								for (i = P2P_PUB_ACTION_SUBTYPE; i < size; i++)	{
-									if (!memcmp(u8P2P_vendorspec, &buff[i], 6)) {
-										u8P2Precvrandom = buff[i + 6];
-										bWilc_ie = true;
-										PRINT_D(GENERIC_DBG, "WILC Vendor specific IE:%02x\n", u8P2Precvrandom);
+									if (!memcmp(p2p_vendor_spec, &buff[i], 6)) {
+										p2p_recv_random = buff[i + 6];
+										wilc_ie = true;
+										PRINT_D(GENERIC_DBG, "WILC Vendor specific IE:%02x\n", p2p_recv_random);
 										break;
 									}
 								}
 							}
 						}
-						if (u8P2Plocalrandom > u8P2Precvrandom)	{
+						if (p2p_local_random > p2p_recv_random)	{
 							if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
 							      || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
 								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < size; i++) {
-									if (buff[i] == P2PELEM_ATTR_ID && !(memcmp(u8P2P_oui, &buff[i + 2], 4))) {
+									if (buff[i] == P2PELEM_ATTR_ID && !(memcmp(p2p_oui, &buff[i + 2], 4))) {
 										WILC_WFI_CfgParseRxAction(&buff[i + 6], size - (i + 6));
 										break;
 									}
 								}
 							}
-						} else
-							PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+						} else {
+							PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", p2p_local_random, p2p_recv_random);
+						}
 					}
 
 
-					if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP) && (bWilc_ie))	{
+					if ((buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buff[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP) && (wilc_ie))	{
 						PRINT_D(GENERIC_DBG, "Sending P2P to host without extra elemnt\n");
-						/* extra attribute for sig_dbm: signal strength in mBm, or 0 if unknown */
 						cfg80211_rx_mgmt(priv->wdev, s32Freq, 0, buff, size - 7, 0);
 						return;
 					}
@@ -2022,16 +1732,6 @@ void WILC_WFI_p2p_rx (struct net_device *dev, u8 *buff, u32 size)
 	}
 }
 
-/**
- *  @brief                      WILC_WFI_mgmt_tx_complete
- *  @details            Returns result of writing mgmt frame to VMM (Tx buffers are freed here)
- *  @param[in]          priv
- *                              transmitting status
- *  @return             None
- *  @author		Amr Abdelmoghny
- *  @date			20 MAY 2013
- *  @version		1.0
- */
 static void WILC_WFI_mgmt_tx_complete(void *priv, int status)
 {
 	struct p2p_mgmt_data *pv_data = (struct p2p_mgmt_data *)priv;
@@ -2041,16 +1741,6 @@ static void WILC_WFI_mgmt_tx_complete(void *priv, int status)
 	kfree(pv_data);
 }
 
-/**
- * @brief               WILC_WFI_RemainOnChannelReady
- *  @details    Callback function, called from handle_remain_on_channel on being ready on channel
- *  @param
- *  @return     none
- *  @author	Amr abdelmoghny
- *  @date		9 JUNE 2013
- *  @version
- */
-
 static void WILC_WFI_RemainOnChannelReady(void *pUserVoid)
 {
 	struct wilc_priv *priv;
@@ -2068,16 +1758,6 @@ static void WILC_WFI_RemainOnChannelReady(void *pUserVoid)
 				  GFP_KERNEL);
 }
 
-/**
- * @brief               WILC_WFI_RemainOnChannelExpired
- *  @details    Callback function, called on expiration of remain-on-channel duration
- *  @param
- *  @return     none
- *  @author	Amr abdelmoghny
- *  @date		15 MAY 2013
- *  @version
- */
-
 static void WILC_WFI_RemainOnChannelExpired(void *pUserVoid, u32 u32SessionID)
 {
 	struct wilc_priv *priv;
@@ -2089,7 +1769,6 @@ static void WILC_WFI_RemainOnChannelExpired(void *pUserVoid, u32 u32SessionID)
 
 		priv->bInP2PlistenState = false;
 
-		/*Inform wpas of remain-on-channel expiration*/
 		cfg80211_remain_on_channel_expired(priv->wdev,
 						   priv->strRemainOnChanParams.u64ListenCookie,
 						   priv->strRemainOnChanParams.pstrListenChan,
@@ -2100,20 +1779,6 @@ static void WILC_WFI_RemainOnChannelExpired(void *pUserVoid, u32 u32SessionID)
 	}
 }
 
-
-/**
- *  @brief      remain_on_channel
- *  @details    Request the driver to remain awake on the specified
- *                      channel for the specified duration to complete an off-channel
- *                      operation (e.g., public action frame exchange). When the driver is
- *                      ready on the requested channel, it must indicate this with an event
- *                      notification by calling cfg80211_ready_on_channel().
- *  @param[in]
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int remain_on_channel(struct wiphy *wiphy,
 			     struct wireless_dev *wdev,
 			     struct ieee80211_channel *chan,
@@ -2121,8 +1786,10 @@ static int remain_on_channel(struct wiphy *wiphy,
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	PRINT_D(GENERIC_DBG, "Remaining on channel %d\n", chan->hw_value);
 
@@ -2134,61 +1801,37 @@ static int remain_on_channel(struct wiphy *wiphy,
 
 	curr_channel = chan->hw_value;
 
-	/*Setting params needed by WILC_WFI_RemainOnChannelExpired()*/
 	priv->strRemainOnChanParams.pstrListenChan = chan;
 	priv->strRemainOnChanParams.u64ListenCookie = *cookie;
 	priv->strRemainOnChanParams.u32ListenDuration = duration;
 	priv->strRemainOnChanParams.u32ListenSessionID++;
 
-	s32Error = host_int_remain_on_channel(priv->hWILCWFIDrv
-					      , priv->strRemainOnChanParams.u32ListenSessionID
-					      , duration
-					      , chan->hw_value
-					      , WILC_WFI_RemainOnChannelExpired
-					      , WILC_WFI_RemainOnChannelReady
-					      , (void *)priv);
+	s32Error = wilc_remain_on_channel(vif,
+				priv->strRemainOnChanParams.u32ListenSessionID,
+				duration, chan->hw_value,
+				WILC_WFI_RemainOnChannelExpired,
+				WILC_WFI_RemainOnChannelReady, (void *)priv);
 
 	return s32Error;
 }
 
-/**
- *  @brief      cancel_remain_on_channel
- *  @details    Cancel an on-going remain-on-channel operation.
- *                      This allows the operation to be terminated prior to timeout based on
- *                      the duration value.
- *  @param[in]   struct wiphy *wiphy,
- *  @param[in]  struct net_device *dev
- *  @param[in]  u64 cookie,
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int cancel_remain_on_channel(struct wiphy *wiphy,
 				    struct wireless_dev *wdev,
 				    u64 cookie)
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	PRINT_D(CFG80211_DBG, "Cancel remain on channel\n");
 
-	s32Error = host_int_ListenStateExpired(priv->hWILCWFIDrv, priv->strRemainOnChanParams.u32ListenSessionID);
+	s32Error = wilc_listen_state_expired(vif, priv->strRemainOnChanParams.u32ListenSessionID);
 	return s32Error;
 }
-/**
- *  @brief      WILC_WFI_mgmt_tx_frame
- *  @details
- *
- *  @param[in]
- *  @return     NONE.
- *  @author	mdaftedar
- *  @date	01 JUL 2012
- *  @version
- */
-extern bool bEnablePS;
+
 static int mgmt_tx(struct wiphy *wiphy,
 		   struct wireless_dev *wdev,
 		   struct cfg80211_mgmt_tx_params *params,
@@ -2203,10 +1846,10 @@ static int mgmt_tx(struct wiphy *wiphy,
 	struct wilc_priv *priv;
 	struct host_if_drv *pstrWFIDrv;
 	u32 i;
-	perInterface_wlan_t *nic;
-	u32 buf_len = len + sizeof(u8P2P_vendorspec) + sizeof(u8P2Plocalrandom);
+	struct wilc_vif *vif;
+	u32 buf_len = len + sizeof(p2p_vendor_spec) + sizeof(p2p_local_random);
 
-	nic = netdev_priv(wdev->netdev);
+	vif = netdev_priv(wdev->netdev);
 	priv = wiphy_priv(wiphy);
 	pstrWFIDrv = (struct host_if_drv *)priv->hWILCWFIDrv;
 
@@ -2215,15 +1858,13 @@ static int mgmt_tx(struct wiphy *wiphy,
 	mgmt = (const struct ieee80211_mgmt *) buf;
 
 	if (ieee80211_is_mgmt(mgmt->frame_control)) {
-
-		/*mgmt frame allocation*/
 		mgmt_tx = kmalloc(sizeof(struct p2p_mgmt_data), GFP_KERNEL);
-		if (mgmt_tx == NULL) {
+		if (!mgmt_tx) {
 			PRINT_ER("Failed to allocate memory for mgmt_tx structure\n");
 			return -EFAULT;
 		}
 		mgmt_tx->buff = kmalloc(buf_len, GFP_KERNEL);
-		if (mgmt_tx->buff == NULL) {
+		if (!mgmt_tx->buff) {
 			PRINT_ER("Failed to allocate memory for mgmt_tx buff\n");
 			kfree(mgmt_tx);
 			return -EFAULT;
@@ -2235,23 +1876,18 @@ static int mgmt_tx(struct wiphy *wiphy,
 		if (ieee80211_is_probe_resp(mgmt->frame_control)) {
 			PRINT_D(GENERIC_DBG, "TX: Probe Response\n");
 			PRINT_D(GENERIC_DBG, "Setting channel: %d\n", chan->hw_value);
-			host_int_set_mac_chnl_num(priv->hWILCWFIDrv, chan->hw_value);
-			/*Save the current channel after we tune to it*/
+			wilc_set_mac_chnl_num(vif, chan->hw_value);
 			curr_channel = chan->hw_value;
 		} else if (ieee80211_is_action(mgmt->frame_control))   {
 			PRINT_D(GENERIC_DBG, "ACTION FRAME:%x\n", (u16)mgmt->frame_control);
 
 
 			if (buf[ACTION_CAT_ID] == PUB_ACTION_ATTR_ID) {
-				/*Only set the channel, if not a negotiation confirmation frame
-				 * (If Negotiation confirmation frame, force it
-				 * to be transmitted on the same negotiation channel)*/
-
 				if (buf[ACTION_SUBTYPE_ID] != PUBLIC_ACT_VENDORSPEC ||
 				    buf[P2P_PUB_ACTION_SUBTYPE] != GO_NEG_CONF)	{
 					PRINT_D(GENERIC_DBG, "Setting channel: %d\n", chan->hw_value);
-					host_int_set_mac_chnl_num(priv->hWILCWFIDrv, chan->hw_value);
-					/*Save the current channel after we tune to it*/
+					wilc_set_mac_chnl_num(vif,
+							      chan->hw_value);
 					curr_channel = chan->hw_value;
 				}
 				switch (buf[ACTION_SUBTYPE_ID])	{
@@ -2269,48 +1905,37 @@ static int mgmt_tx(struct wiphy *wiphy,
 
 				case PUBLIC_ACT_VENDORSPEC:
 				{
-					/*Now we have a public action vendor specific action frame, check if its a p2p public action frame
-					 * based on the standard its should have the p2p_oui attribute with the following values 50 6f 9A 09*/
-					if (!memcmp(u8P2P_oui, &buf[ACTION_SUBTYPE_ID + 1], 4)) {
-						/*For the connection of two WILC's connection generate a rand number to determine who will be a GO*/
+					if (!memcmp(p2p_oui, &buf[ACTION_SUBTYPE_ID + 1], 4)) {
 						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP)) {
-							if (u8P2Plocalrandom == 1 && u8P2Precvrandom < u8P2Plocalrandom) {
-								get_random_bytes(&u8P2Plocalrandom, 1);
-								/*Increment the number to prevent if its 0*/
-								u8P2Plocalrandom++;
+							if (p2p_local_random == 1 && p2p_recv_random < p2p_local_random) {
+								get_random_bytes(&p2p_local_random, 1);
+								p2p_local_random++;
 							}
 						}
 
 						if ((buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == GO_NEG_RSP
 						      || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)) {
-							if (u8P2Plocalrandom > u8P2Precvrandom)	{
-								PRINT_D(GENERIC_DBG, "LOCAL WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+							if (p2p_local_random > p2p_recv_random)	{
+								PRINT_D(GENERIC_DBG, "LOCAL WILL BE GO LocaRand=%02x RecvRand %02x\n", p2p_local_random, p2p_recv_random);
 
-								/*Search for the p2p information information element , after the Public action subtype theres a byte for teh dialog token, skip that*/
 								for (i = P2P_PUB_ACTION_SUBTYPE + 2; i < len; i++) {
-									if (buf[i] == P2PELEM_ATTR_ID && !(memcmp(u8P2P_oui, &buf[i + 2], 4))) {
+									if (buf[i] == P2PELEM_ATTR_ID && !(memcmp(p2p_oui, &buf[i + 2], 4))) {
 										if (buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_REQ || buf[P2P_PUB_ACTION_SUBTYPE] == P2P_INV_RSP)
-											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), true, nic->iftype);
-
-										/*If using supplicant go intent, no need at all*/
-										/*to parse transmitted negotiation frames*/
+											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), true, vif->iftype);
 										else
-											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), false, nic->iftype);
+											WILC_WFI_CfgParseTxAction(&mgmt_tx->buff[i + 6], len - (i + 6), false, vif->iftype);
 										break;
 									}
 								}
 
 								if (buf[P2P_PUB_ACTION_SUBTYPE] != P2P_INV_REQ && buf[P2P_PUB_ACTION_SUBTYPE] != P2P_INV_RSP) {
-									/*
-									 * Adding WILC information element to allow two WILC devices to
-									 * identify each other and connect
-									 */
-									memcpy(&mgmt_tx->buff[len], u8P2P_vendorspec, sizeof(u8P2P_vendorspec));
-									mgmt_tx->buff[len + sizeof(u8P2P_vendorspec)] = u8P2Plocalrandom;
+									memcpy(&mgmt_tx->buff[len], p2p_vendor_spec, sizeof(p2p_vendor_spec));
+									mgmt_tx->buff[len + sizeof(p2p_vendor_spec)] = p2p_local_random;
 									mgmt_tx->size = buf_len;
 								}
-							} else
-								PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", u8P2Plocalrandom, u8P2Precvrandom);
+							} else {
+								PRINT_D(GENERIC_DBG, "PEER WILL BE GO LocaRand=%02x RecvRand %02x\n", p2p_local_random, p2p_recv_random);
+							}
 						}
 
 					} else {
@@ -2326,18 +1951,17 @@ static int mgmt_tx(struct wiphy *wiphy,
 					break;
 				}
 				}
-
 			}
 
 			PRINT_D(GENERIC_DBG, "TX: ACTION FRAME Type:%x : Chan:%d\n", buf[ACTION_SUBTYPE_ID], chan->hw_value);
-			pstrWFIDrv->u64P2p_MgmtTimeout = (jiffies + msecs_to_jiffies(wait));
-
-			PRINT_D(GENERIC_DBG, "Current Jiffies: %lu Timeout:%llu\n", jiffies, pstrWFIDrv->u64P2p_MgmtTimeout);
+			pstrWFIDrv->p2p_timeout = (jiffies + msecs_to_jiffies(wait));
 
+			PRINT_D(GENERIC_DBG, "Current Jiffies: %lu Timeout:%llu\n",
+				jiffies, pstrWFIDrv->p2p_timeout);
 		}
 
-		wilc_wlan_txq_add_mgmt_pkt(mgmt_tx, mgmt_tx->buff,
-					   mgmt_tx->size,
+		wilc_wlan_txq_add_mgmt_pkt(wdev->netdev, mgmt_tx,
+					   mgmt_tx->buff, mgmt_tx->size,
 					   WILC_WFI_mgmt_tx_complete);
 	} else {
 		PRINT_D(GENERIC_DBG, "This function transmits only management frames\n");
@@ -2357,7 +1981,7 @@ static int mgmt_tx_cancel_wait(struct wiphy *wiphy,
 
 
 	PRINT_D(GENERIC_DBG, "Tx Cancel wait :%lu\n", jiffies);
-	pstrWFIDrv->u64P2p_MgmtTimeout = jiffies;
+	pstrWFIDrv->p2p_timeout = jiffies;
 
 	if (!priv->bInP2PlistenState) {
 		cfg80211_remain_on_channel_expired(priv->wdev,
@@ -2369,28 +1993,16 @@ static int mgmt_tx_cancel_wait(struct wiphy *wiphy,
 	return 0;
 }
 
-/**
- *  @brief      wilc_mgmt_frame_register
- *  @details Notify driver that a management frame type was
- *              registered. Note that this callback may not sleep, and cannot run
- *                      concurrently with itself.
- *  @param[in]
- *  @return     NONE.
- *  @author	mdaftedar
- *  @date	01 JUL 2012
- *  @version
- */
 void wilc_mgmt_frame_register(struct wiphy *wiphy, struct wireless_dev *wdev,
 			      u16 frame_type, bool reg)
 {
-
 	struct wilc_priv *priv;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wl;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(priv->wdev->netdev);
-	wl = nic->wilc;
+	vif = netdev_priv(priv->wdev->netdev);
+	wl = vif->wilc;
 
 	if (!frame_type)
 		return;
@@ -2399,15 +2011,15 @@ void wilc_mgmt_frame_register(struct wiphy *wiphy, struct wireless_dev *wdev,
 	switch (frame_type) {
 	case PROBE_REQ:
 	{
-		nic->g_struct_frame_reg[0].frame_type = frame_type;
-		nic->g_struct_frame_reg[0].reg = reg;
+		vif->g_struct_frame_reg[0].frame_type = frame_type;
+		vif->g_struct_frame_reg[0].reg = reg;
 	}
 	break;
 
 	case ACTION:
 	{
-		nic->g_struct_frame_reg[1].frame_type = frame_type;
-		nic->g_struct_frame_reg[1].reg = reg;
+		vif->g_struct_frame_reg[1].frame_type = frame_type;
+		vif->g_struct_frame_reg[1].reg = reg;
 	}
 	break;
 
@@ -2415,54 +2027,27 @@ void wilc_mgmt_frame_register(struct wiphy *wiphy, struct wireless_dev *wdev,
 	{
 		break;
 	}
-
 	}
-	/*If mac is closed, then return*/
+
 	if (!wl->initialized) {
 		PRINT_D(GENERIC_DBG, "Return since mac is closed\n");
 		return;
 	}
-	host_int_frame_register(priv->hWILCWFIDrv, frame_type, reg);
-
-
+	wilc_frame_register(vif, frame_type, reg);
 }
 
-/**
- *  @brief      set_cqm_rssi_config
- *  @details    Configure connection quality monitor RSSI threshold.
- *  @param[in]   struct wiphy *wiphy:
- *  @param[in]	struct net_device *dev:
- *  @param[in]          s32 rssi_thold:
- *  @param[in]	u32 rssi_hyst:
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int set_cqm_rssi_config(struct wiphy *wiphy, struct net_device *dev,
 			       s32 rssi_thold, u32 rssi_hyst)
 {
 	PRINT_D(CFG80211_DBG, "Setting CQM RSSi Function\n");
 	return 0;
-
 }
-/**
- *  @brief      dump_station
- *  @details    Configure connection quality monitor RSSI threshold.
- *  @param[in]   struct wiphy *wiphy:
- *  @param[in]	struct net_device *dev
- *  @param[in]          int idx
- *  @param[in]	u8 *mac
- *  @param[in]	struct station_info *sinfo
- *  @return     int : Return 0 on Success
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
+
 static int dump_station(struct wiphy *wiphy, struct net_device *dev,
 			int idx, u8 *mac, struct station_info *sinfo)
 {
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 
 	PRINT_D(CFG80211_DBG, "Dumping station information\n");
 
@@ -2470,143 +2055,109 @@ static int dump_station(struct wiphy *wiphy, struct net_device *dev,
 		return -ENOENT;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	sinfo->filled |= BIT(NL80211_STA_INFO_SIGNAL);
 
-	host_int_get_rssi(priv->hWILCWFIDrv, &(sinfo->signal));
+	wilc_get_rssi(vif, &sinfo->signal);
 
 	return 0;
-
 }
 
-
-/**
- *  @brief      set_power_mgmt
- *  @details
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 JUL 2012
- *  @version	1.0
- */
 static int set_power_mgmt(struct wiphy *wiphy, struct net_device *dev,
 			  bool enabled, int timeout)
 {
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 
 	PRINT_D(CFG80211_DBG, " Power save Enabled= %d , TimeOut = %d\n", enabled, timeout);
 
-	if (wiphy == NULL)
+	if (!wiphy)
 		return -ENOENT;
 
 	priv = wiphy_priv(wiphy);
-	if (priv->hWILCWFIDrv == NULL) {
+	vif = netdev_priv(priv->dev);
+	if (!priv->hWILCWFIDrv) {
 		PRINT_ER("Driver is NULL\n");
 		return -EIO;
 	}
 
-	if (bEnablePS)
-		host_int_set_power_mgmt(priv->hWILCWFIDrv, enabled, timeout);
+	if (wilc_enable_ps)
+		wilc_set_power_mgmt(vif, enabled, timeout);
 
 
 	return 0;
-
 }
 
-/**
- *  @brief      change_virtual_intf
- *  @details    Change type/configuration of virtual interface,
- *                      keep the struct wireless_dev's iftype updated.
- *  @param[in]   NONE
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-int wilc1000_wlan_init(struct net_device *dev, perInterface_wlan_t *p_nic);
-
 static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 			       enum nl80211_iftype type, u32 *flags, struct vif_params *params)
 {
 	struct wilc_priv *priv;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	u8 interface_type;
 	u16 TID = 0;
 	u8 i;
 	struct wilc *wl;
 
-	nic = netdev_priv(dev);
+	vif = netdev_priv(dev);
 	priv = wiphy_priv(wiphy);
-	wl = nic->wilc;
+	wl = vif->wilc;
 
 	PRINT_D(HOSTAPD_DBG, "In Change virtual interface function\n");
 	PRINT_D(HOSTAPD_DBG, "Wireless interface name =%s\n", dev->name);
-	u8P2Plocalrandom = 0x01;
-	u8P2Precvrandom = 0x00;
-
-	bWilc_ie = false;
-
-	g_obtainingIP = false;
-	del_timer(&hDuringIpTimer);
+	p2p_local_random = 0x01;
+	p2p_recv_random = 0x00;
+	wilc_ie = false;
+	wilc_optaining_ip = false;
+	del_timer(&wilc_during_ip_timer);
 	PRINT_D(GENERIC_DBG, "Changing virtual interface, enable scan\n");
-	/*Set WILC_CHANGING_VIR_IF register to disallow adding futrue keys to CE H/W*/
+
 	if (g_ptk_keys_saved && g_gtk_keys_saved) {
-		Set_machw_change_vir_if(dev, true);
+		wilc_set_machw_change_vir_if(dev, true);
 	}
 
 	switch (type) {
 	case NL80211_IFTYPE_STATION:
-		connecting = 0;
+		wilc_connecting = 0;
 		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_STATION\n");
 
-		/* send delba over wlan interface */
-
-
 		dev->ieee80211_ptr->iftype = type;
 		priv->wdev->iftype = type;
-		nic->monitor_flag = 0;
-		nic->iftype = STATION_MODE;
+		vif->monitor_flag = 0;
+		vif->iftype = STATION_MODE;
 
-		/*Remove the enteries of the previously connected clients*/
 		memset(priv->assoc_stainfo.au8Sta_AssociatedBss, 0, MAX_NUM_STA * ETH_ALEN);
-		interface_type = nic->iftype;
-		nic->iftype = STATION_MODE;
+		interface_type = vif->iftype;
+		vif->iftype = STATION_MODE;
 
 		if (wl->initialized) {
-			host_int_del_All_Rx_BASession(priv->hWILCWFIDrv,
-						      wl->vif[0].bssid, TID);
-			/* ensure that the message Q is empty */
-			host_int_wait_msg_queue_idle();
+			wilc_del_all_rx_ba_session(vif, wl->vif[0]->bssid,
+						   TID);
+			wilc_wait_msg_queue_idle();
 
-			/*Eliminate host interface blocking state*/
 			up(&wl->cfg_event);
 
 			wilc1000_wlan_deinit(dev);
-			wilc1000_wlan_init(dev, nic);
-			g_wilc_initialized = 1;
-			nic->iftype = interface_type;
+			wilc1000_wlan_init(dev, vif);
+			wilc_initialized = 1;
+			vif->iftype = interface_type;
 
-			/*Setting interface 1 drv handler and mac address in newly downloaded FW*/
-			host_int_set_wfi_drv_handler(wl->vif[0].hif_drv);
-			host_int_set_MacAddress(wl->vif[0].hif_drv,
-						wl->vif[0].src_addr);
-			host_int_set_operation_mode(priv->hWILCWFIDrv, STATION_MODE);
+			wilc_set_wfi_drv_handler(vif,
+						 wilc_get_vif_idx(wl->vif[0]));
+			wilc_set_mac_address(wl->vif[0], wl->vif[0]->src_addr);
+			wilc_set_operation_mode(vif, STATION_MODE);
 
-			/*Add saved WEP keys, if any*/
 			if (g_wep_keys_saved) {
-				host_int_set_wep_default_key(wl->vif[0].hif_drv,
-							     g_key_wep_params.key_idx);
-				host_int_add_wep_key_bss_sta(wl->vif[0].hif_drv,
-							     g_key_wep_params.key,
-							     g_key_wep_params.key_len,
-							     g_key_wep_params.key_idx);
+				wilc_set_wep_default_keyid(wl->vif[0],
+						g_key_wep_params.key_idx);
+				wilc_add_wep_key_bss_sta(wl->vif[0],
+						g_key_wep_params.key,
+						g_key_wep_params.key_len,
+						g_key_wep_params.key_idx);
 			}
 
-			/*No matter the driver handler passed here, it will be overwriiten*/
-			/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
-			host_int_flush_join_req(priv->hWILCWFIDrv);
+			wilc_flush_join_req(vif);
 
-			/*Add saved PTK and GTK keys, if any*/
 			if (g_ptk_keys_saved && g_gtk_keys_saved) {
 				PRINT_D(CFG80211_DBG, "ptk %x %x %x\n", g_key_ptk_params.key[0],
 					g_key_ptk_params.key[1],
@@ -2614,15 +2165,15 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 				PRINT_D(CFG80211_DBG, "gtk %x %x %x\n", g_key_gtk_params.key[0],
 					g_key_gtk_params.key[1],
 					g_key_gtk_params.key[2]);
-				add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-					wl->vif[0].ndev,
+				add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+					wl->vif[0]->ndev,
 					g_add_ptk_key_params.key_idx,
 					g_add_ptk_key_params.pairwise,
 					g_add_ptk_key_params.mac_addr,
 					(struct key_params *)(&g_key_ptk_params));
 
-				add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-					wl->vif[0].ndev,
+				add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+					wl->vif[0]->ndev,
 					g_add_gtk_key_params.key_idx,
 					g_add_gtk_key_params.pairwise,
 					g_add_gtk_key_params.mac_addr,
@@ -2631,64 +2182,58 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 
 			if (wl->initialized)	{
 				for (i = 0; i < num_reg_frame; i++) {
-					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
-						nic->g_struct_frame_reg[i].reg);
-					host_int_frame_register(priv->hWILCWFIDrv,
-								nic->g_struct_frame_reg[i].frame_type,
-								nic->g_struct_frame_reg[i].reg);
+					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", vif->g_struct_frame_reg[i].frame_type,
+						vif->g_struct_frame_reg[i].reg);
+					wilc_frame_register(vif,
+								vif->g_struct_frame_reg[i].frame_type,
+								vif->g_struct_frame_reg[i].reg);
 				}
 			}
 
-			bEnablePS = true;
-			host_int_set_power_mgmt(priv->hWILCWFIDrv, 1, 0);
+			wilc_enable_ps = true;
+			wilc_set_power_mgmt(vif, 1, 0);
 		}
 		break;
 
 	case NL80211_IFTYPE_P2P_CLIENT:
-		bEnablePS = false;
-		host_int_set_power_mgmt(priv->hWILCWFIDrv, 0, 0);
-		connecting = 0;
+		wilc_enable_ps = false;
+		wilc_set_power_mgmt(vif, 0, 0);
+		wilc_connecting = 0;
 		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_P2P_CLIENT\n");
 
-		host_int_del_All_Rx_BASession(priv->hWILCWFIDrv,
-					      wl->vif[0].bssid, TID);
+		wilc_del_all_rx_ba_session(vif, wl->vif[0]->bssid, TID);
 
 		dev->ieee80211_ptr->iftype = type;
 		priv->wdev->iftype = type;
-		nic->monitor_flag = 0;
+		vif->monitor_flag = 0;
 
 		PRINT_D(HOSTAPD_DBG, "Downloading P2P_CONCURRENCY_FIRMWARE\n");
-		nic->iftype = CLIENT_MODE;
+		vif->iftype = CLIENT_MODE;
 
 
 		if (wl->initialized)	{
-			/* ensure that the message Q is empty */
-			host_int_wait_msg_queue_idle();
+			wilc_wait_msg_queue_idle();
 
 			wilc1000_wlan_deinit(dev);
-			wilc1000_wlan_init(dev, nic);
-			g_wilc_initialized = 1;
+			wilc1000_wlan_init(dev, vif);
+			wilc_initialized = 1;
 
-			host_int_set_wfi_drv_handler(wl->vif[0].hif_drv);
-			host_int_set_MacAddress(wl->vif[0].hif_drv,
-						wl->vif[0].src_addr);
-			host_int_set_operation_mode(priv->hWILCWFIDrv, STATION_MODE);
+			wilc_set_wfi_drv_handler(vif,
+						 wilc_get_vif_idx(wl->vif[0]));
+			wilc_set_mac_address(wl->vif[0], wl->vif[0]->src_addr);
+			wilc_set_operation_mode(vif, STATION_MODE);
 
-			/*Add saved WEP keys, if any*/
 			if (g_wep_keys_saved) {
-				host_int_set_wep_default_key(wl->vif[0].hif_drv,
-							     g_key_wep_params.key_idx);
-				host_int_add_wep_key_bss_sta(wl->vif[0].hif_drv,
-							     g_key_wep_params.key,
-							     g_key_wep_params.key_len,
-							     g_key_wep_params.key_idx);
+				wilc_set_wep_default_keyid(wl->vif[0],
+						g_key_wep_params.key_idx);
+				wilc_add_wep_key_bss_sta(wl->vif[0],
+						g_key_wep_params.key,
+						g_key_wep_params.key_len,
+						g_key_wep_params.key_idx);
 			}
 
-			/*No matter the driver handler passed here, it will be overwriiten*/
-			/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
-			host_int_flush_join_req(priv->hWILCWFIDrv);
+			wilc_flush_join_req(vif);
 
-			/*Add saved PTK and GTK keys, if any*/
 			if (g_ptk_keys_saved && g_gtk_keys_saved) {
 				PRINT_D(CFG80211_DBG, "ptk %x %x %x\n", g_key_ptk_params.key[0],
 					g_key_ptk_params.key[1],
@@ -2696,59 +2241,58 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 				PRINT_D(CFG80211_DBG, "gtk %x %x %x\n", g_key_gtk_params.key[0],
 					g_key_gtk_params.key[1],
 					g_key_gtk_params.key[2]);
-				add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-					wl->vif[0].ndev,
+				add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+					wl->vif[0]->ndev,
 					g_add_ptk_key_params.key_idx,
 					g_add_ptk_key_params.pairwise,
 					g_add_ptk_key_params.mac_addr,
 					(struct key_params *)(&g_key_ptk_params));
 
-				add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-					wl->vif[0].ndev,
+				add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+					wl->vif[0]->ndev,
 					g_add_gtk_key_params.key_idx,
 					g_add_gtk_key_params.pairwise,
 					g_add_gtk_key_params.mac_addr,
 					(struct key_params *)(&g_key_gtk_params));
 			}
 
-			/*Refresh scan, to refresh the scan results to the wpa_supplicant. Set MachHw to false to enable further key installments*/
 			refresh_scan(priv, 1, true);
-			Set_machw_change_vir_if(dev, false);
+			wilc_set_machw_change_vir_if(dev, false);
 
 			if (wl->initialized)	{
 				for (i = 0; i < num_reg_frame; i++) {
-					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
-						nic->g_struct_frame_reg[i].reg);
-					host_int_frame_register(priv->hWILCWFIDrv,
-								nic->g_struct_frame_reg[i].frame_type,
-								nic->g_struct_frame_reg[i].reg);
+					PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", vif->g_struct_frame_reg[i].frame_type,
+						vif->g_struct_frame_reg[i].reg);
+					wilc_frame_register(vif,
+								vif->g_struct_frame_reg[i].frame_type,
+								vif->g_struct_frame_reg[i].reg);
 				}
 			}
 		}
 		break;
 
 	case NL80211_IFTYPE_AP:
-		bEnablePS = false;
+		wilc_enable_ps = false;
 		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_AP %d\n", type);
 		dev->ieee80211_ptr->iftype = type;
 		priv->wdev->iftype = type;
-		nic->iftype = AP_MODE;
+		vif->iftype = AP_MODE;
 		PRINT_D(CORECONFIG_DBG, "priv->hWILCWFIDrv[%p]\n", priv->hWILCWFIDrv);
 
 		PRINT_D(HOSTAPD_DBG, "Downloading AP firmware\n");
-		linux_wlan_get_firmware(nic);
-		/*If wilc is running, then close-open to actually get new firmware running (serves P2P)*/
+		wilc_wlan_get_firmware(dev);
+
 		if (wl->initialized)	{
-			nic->iftype = AP_MODE;
-			mac_close(dev);
-			mac_open(dev);
+			vif->iftype = AP_MODE;
+			wilc_mac_close(dev);
+			wilc_mac_open(dev);
 
 			for (i = 0; i < num_reg_frame; i++) {
-				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
-					nic->g_struct_frame_reg[i].reg);
-				host_int_frame_register(priv->hWILCWFIDrv,
-							nic->g_struct_frame_reg[i].frame_type,
-							nic->g_struct_frame_reg[i].reg);
+				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", vif->g_struct_frame_reg[i].frame_type,
+					vif->g_struct_frame_reg[i].reg);
+				wilc_frame_register(vif,
+							vif->g_struct_frame_reg[i].frame_type,
+							vif->g_struct_frame_reg[i].reg);
 			}
 		}
 		break;
@@ -2756,16 +2300,12 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 	case NL80211_IFTYPE_P2P_GO:
 		PRINT_D(GENERIC_DBG, "start duringIP timer\n");
 
-		g_obtainingIP = true;
-		mod_timer(&hDuringIpTimer, jiffies + msecs_to_jiffies(duringIP_TIME));
-		host_int_set_power_mgmt(priv->hWILCWFIDrv, 0, 0);
-		/*Delete block ack has to be the latest config packet*/
-		/*sent before downloading new FW. This is because it blocks on*/
-		/*hWaitResponse semaphore, which allows previous config*/
-		/*packets to actually take action on old FW*/
-		host_int_del_All_Rx_BASession(priv->hWILCWFIDrv,
-					      wl->vif[0].bssid, TID);
-		bEnablePS = false;
+		wilc_optaining_ip = true;
+		mod_timer(&wilc_during_ip_timer,
+			  jiffies + msecs_to_jiffies(during_ip_time));
+		wilc_set_power_mgmt(vif, 0, 0);
+		wilc_del_all_rx_ba_session(vif, wl->vif[0]->bssid, TID);
+		wilc_enable_ps = false;
 		PRINT_D(HOSTAPD_DBG, "Interface type = NL80211_IFTYPE_GO\n");
 		dev->ieee80211_ptr->iftype = type;
 		priv->wdev->iftype = type;
@@ -2775,36 +2315,28 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 		PRINT_D(HOSTAPD_DBG, "Downloading P2P_CONCURRENCY_FIRMWARE\n");
 
 
-		nic->iftype = GO_MODE;
+		vif->iftype = GO_MODE;
 
-		/* ensure that the message Q is empty */
-		host_int_wait_msg_queue_idle();
+		wilc_wait_msg_queue_idle();
 		wilc1000_wlan_deinit(dev);
-		wilc1000_wlan_init(dev, nic);
-		g_wilc_initialized = 1;
-
+		wilc1000_wlan_init(dev, vif);
+		wilc_initialized = 1;
 
-		/*Setting interface 1 drv handler and mac address in newly downloaded FW*/
-		host_int_set_wfi_drv_handler(wl->vif[0].hif_drv);
-		host_int_set_MacAddress(wl->vif[0].hif_drv,
-					wl->vif[0].src_addr);
-		host_int_set_operation_mode(priv->hWILCWFIDrv, AP_MODE);
+		wilc_set_wfi_drv_handler(vif, wilc_get_vif_idx(wl->vif[0]));
+		wilc_set_mac_address(wl->vif[0], wl->vif[0]->src_addr);
+		wilc_set_operation_mode(vif, AP_MODE);
 
-		/*Add saved WEP keys, if any*/
 		if (g_wep_keys_saved) {
-			host_int_set_wep_default_key(wl->vif[0].hif_drv,
-						     g_key_wep_params.key_idx);
-			host_int_add_wep_key_bss_sta(wl->vif[0].hif_drv,
-						     g_key_wep_params.key,
-						     g_key_wep_params.key_len,
-						     g_key_wep_params.key_idx);
+			wilc_set_wep_default_keyid(wl->vif[0],
+						   g_key_wep_params.key_idx);
+			wilc_add_wep_key_bss_sta(wl->vif[0],
+						 g_key_wep_params.key,
+						 g_key_wep_params.key_len,
+						 g_key_wep_params.key_idx);
 		}
 
-		/*No matter the driver handler passed here, it will be overwriiten*/
-		/*in Handle_FlushConnect() with gu8FlushedJoinReqDrvHandler*/
-		host_int_flush_join_req(priv->hWILCWFIDrv);
+		wilc_flush_join_req(vif);
 
-		/*Add saved PTK and GTK keys, if any*/
 		if (g_ptk_keys_saved && g_gtk_keys_saved) {
 			PRINT_D(CFG80211_DBG, "ptk %x %x %x cipher %x\n", g_key_ptk_params.key[0],
 				g_key_ptk_params.key[1],
@@ -2814,15 +2346,15 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 				g_key_gtk_params.key[1],
 				g_key_gtk_params.key[2],
 				g_key_gtk_params.cipher);
-			add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-				wl->vif[0].ndev,
+			add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+				wl->vif[0]->ndev,
 				g_add_ptk_key_params.key_idx,
 				g_add_ptk_key_params.pairwise,
 				g_add_ptk_key_params.mac_addr,
 				(struct key_params *)(&g_key_ptk_params));
 
-			add_key(wl->vif[0].ndev->ieee80211_ptr->wiphy,
-				wl->vif[0].ndev,
+			add_key(wl->vif[0]->ndev->ieee80211_ptr->wiphy,
+				wl->vif[0]->ndev,
 				g_add_gtk_key_params.key_idx,
 				g_add_gtk_key_params.pairwise,
 				g_add_gtk_key_params.mac_addr,
@@ -2831,11 +2363,11 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 
 		if (wl->initialized)	{
 			for (i = 0; i < num_reg_frame; i++) {
-				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", nic->g_struct_frame_reg[i].frame_type,
-					nic->g_struct_frame_reg[i].reg);
-				host_int_frame_register(priv->hWILCWFIDrv,
-							nic->g_struct_frame_reg[i].frame_type,
-							nic->g_struct_frame_reg[i].reg);
+				PRINT_D(INIT_DBG, "Frame registering Type: %x - Reg: %d\n", vif->g_struct_frame_reg[i].frame_type,
+					vif->g_struct_frame_reg[i].reg);
+				wilc_frame_register(vif,
+							vif->g_struct_frame_reg[i].frame_type,
+							vif->g_struct_frame_reg[i].reg);
 			}
 		}
 		break;
@@ -2848,32 +2380,6 @@ static int change_virtual_intf(struct wiphy *wiphy, struct net_device *dev,
 	return 0;
 }
 
-/* (austin.2013-07-23)
- *
- *      To support revised cfg80211_ops
- *
- *              add_beacon --> start_ap
- *              set_beacon --> change_beacon
- *              del_beacon --> stop_ap
- *
- *              beacon_parameters  -->	cfg80211_ap_settings
- *                                                              cfg80211_beacon_data
- *
- *      applicable for linux kernel 3.4+
- */
-
-/**
- *  @brief      start_ap
- *  @details    Add a beacon with given parameters, @head, @interval
- *                      and @dtim_period will be valid, @tail is optional.
- *  @param[in]   wiphy
- *  @param[in]   dev	The net device structure
- *  @param[in]   settings	cfg80211_ap_settings parameters for the beacon to be added
- *  @return     int : Return 0 on Success.
- *  @author	austin
- *  @date	23 JUL 2013
- *  @version	1.0
- */
 static int start_ap(struct wiphy *wiphy, struct net_device *dev,
 		    struct cfg80211_ap_settings *settings)
 {
@@ -2881,11 +2387,11 @@ static int start_ap(struct wiphy *wiphy, struct net_device *dev,
 	struct wilc_priv *priv;
 	s32 s32Error = 0;
 	struct wilc *wl;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(dev);
-	wl = nic->wilc;
+	vif = netdev_priv(dev);
+	wl = vif ->wilc;
 	PRINT_D(HOSTAPD_DBG, "Starting ap\n");
 
 	PRINT_D(HOSTAPD_DBG, "Interval = %d\n DTIM period = %d\n Head length = %zu Tail length = %zu\n",
@@ -2896,73 +2402,53 @@ static int start_ap(struct wiphy *wiphy, struct net_device *dev,
 	if (s32Error != 0)
 		PRINT_ER("Error in setting channel\n");
 
-	linux_wlan_set_bssid(dev, wl->vif[0].src_addr);
+	wilc_wlan_set_bssid(dev, wl->vif[0]->src_addr);
 
-	s32Error = host_int_add_beacon(priv->hWILCWFIDrv,
-					settings->beacon_interval,
-					settings->dtim_period,
-					beacon->head_len, (u8 *)beacon->head,
-					beacon->tail_len, (u8 *)beacon->tail);
+	s32Error = wilc_add_beacon(vif, settings->beacon_interval,
+				   settings->dtim_period, beacon->head_len,
+				   (u8 *)beacon->head, beacon->tail_len,
+				   (u8 *)beacon->tail);
 
 	return s32Error;
 }
 
-/**
- *  @brief      change_beacon
- *  @details    Add a beacon with given parameters, @head, @interval
- *                      and @dtim_period will be valid, @tail is optional.
- *  @param[in]   wiphy
- *  @param[in]   dev	The net device structure
- *  @param[in]   beacon	cfg80211_beacon_data for the beacon to be changed
- *  @return     int : Return 0 on Success.
- *  @author	austin
- *  @date	23 JUL 2013
- *  @version	1.0
- */
 static int change_beacon(struct wiphy *wiphy, struct net_device *dev,
 			 struct cfg80211_beacon_data *beacon)
 {
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 	s32 s32Error = 0;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 	PRINT_D(HOSTAPD_DBG, "Setting beacon\n");
 
 
-	s32Error = host_int_add_beacon(priv->hWILCWFIDrv,
-					0,
-					0,
-					beacon->head_len, (u8 *)beacon->head,
-					beacon->tail_len, (u8 *)beacon->tail);
+	s32Error = wilc_add_beacon(vif, 0, 0, beacon->head_len,
+				   (u8 *)beacon->head, beacon->tail_len,
+				   (u8 *)beacon->tail);
 
 	return s32Error;
 }
 
-/**
- *  @brief      stop_ap
- *  @details    Remove beacon configuration and stop sending the beacon.
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	austin
- *  @date	23 JUL 2013
- *  @version	1.0
- */
 static int stop_ap(struct wiphy *wiphy, struct net_device *dev)
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
+	struct wilc_vif *vif;
 	u8 NullBssid[ETH_ALEN] = {0};
 
 	if (!wiphy)
 		return -EFAULT;
 
 	priv = wiphy_priv(wiphy);
+	vif = netdev_priv(priv->dev);
 
 	PRINT_D(HOSTAPD_DBG, "Deleting beacon\n");
 
-	linux_wlan_set_bssid(dev, NullBssid);
+	wilc_wlan_set_bssid(dev, NullBssid);
 
-	s32Error = host_int_del_beacon(priv->hWILCWFIDrv);
+	s32Error = wilc_del_beacon(vif);
 
 	if (s32Error)
 		PRINT_ER("Host delete beacon fail\n");
@@ -2970,68 +2456,70 @@ static int stop_ap(struct wiphy *wiphy, struct net_device *dev)
 	return s32Error;
 }
 
-/**
- *  @brief      add_station
- *  @details    Add a new station.
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int add_station(struct wiphy *wiphy, struct net_device *dev,
 		       const u8 *mac, struct station_parameters *params)
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
 	struct add_sta_param strStaParams = { {0} };
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 	if (!wiphy)
 		return -EFAULT;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(dev);
+	vif = netdev_priv(dev);
 
-	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
-		memcpy(strStaParams.au8BSSID, mac, ETH_ALEN);
+	if (vif->iftype == AP_MODE || vif->iftype == GO_MODE) {
+		memcpy(strStaParams.bssid, mac, ETH_ALEN);
 		memcpy(priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid], mac, ETH_ALEN);
-		strStaParams.u16AssocID = params->aid;
-		strStaParams.u8NumRates = params->supported_rates_len;
-		strStaParams.pu8Rates = params->supported_rates;
+		strStaParams.aid = params->aid;
+		strStaParams.rates_len = params->supported_rates_len;
+		strStaParams.rates = params->supported_rates;
 
 		PRINT_D(CFG80211_DBG, "Adding station parameters %d\n", params->aid);
 
 		PRINT_D(CFG80211_DBG, "BSSID = %x%x%x%x%x%x\n", priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][0], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][1], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][2], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][3], priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][4],
 			priv->assoc_stainfo.au8Sta_AssociatedBss[params->aid][5]);
-		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.u16AssocID);
-		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n", strStaParams.u8NumRates);
+		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.aid);
+		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n",
+			strStaParams.rates_len);
 
-		if (params->ht_capa == NULL) {
-			strStaParams.bIsHTSupported = false;
+		if (!params->ht_capa) {
+			strStaParams.ht_supported = false;
 		} else {
-			strStaParams.bIsHTSupported = true;
-			strStaParams.u16HTCapInfo = params->ht_capa->cap_info;
-			strStaParams.u8AmpduParams = params->ht_capa->ampdu_params_info;
-			memcpy(strStaParams.au8SuppMCsSet, &params->ht_capa->mcs, WILC_SUPP_MCS_SET_SIZE);
-			strStaParams.u16HTExtParams = params->ht_capa->extended_ht_cap_info;
-			strStaParams.u32TxBeamformingCap = params->ht_capa->tx_BF_cap_info;
-			strStaParams.u8ASELCap = params->ht_capa->antenna_selection_info;
+			strStaParams.ht_supported = true;
+			strStaParams.ht_capa_info = params->ht_capa->cap_info;
+			strStaParams.ht_ampdu_params = params->ht_capa->ampdu_params_info;
+			memcpy(strStaParams.ht_supp_mcs_set,
+			       &params->ht_capa->mcs,
+			       WILC_SUPP_MCS_SET_SIZE);
+			strStaParams.ht_ext_params = params->ht_capa->extended_ht_cap_info;
+			strStaParams.ht_tx_bf_cap = params->ht_capa->tx_BF_cap_info;
+			strStaParams.ht_ante_sel = params->ht_capa->antenna_selection_info;
 		}
 
-		strStaParams.u16FlagsMask = params->sta_flags_mask;
-		strStaParams.u16FlagsSet = params->sta_flags_set;
-
-		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n", strStaParams.bIsHTSupported);
-		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n", strStaParams.u16HTCapInfo);
-		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n", strStaParams.u8AmpduParams);
-		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n", strStaParams.u16HTExtParams);
-		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n", strStaParams.u32TxBeamformingCap);
-		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n", strStaParams.u8ASELCap);
-		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n", strStaParams.u16FlagsMask);
-		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n", strStaParams.u16FlagsSet);
-
-		s32Error = host_int_add_station(priv->hWILCWFIDrv, &strStaParams);
+		strStaParams.flags_mask = params->sta_flags_mask;
+		strStaParams.flags_set = params->sta_flags_set;
+
+		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n",
+			strStaParams.ht_supported);
+		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n",
+			strStaParams.ht_capa_info);
+		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n",
+			strStaParams.ht_ampdu_params);
+		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n",
+			strStaParams.ht_ext_params);
+		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n",
+			strStaParams.ht_tx_bf_cap);
+		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n",
+			strStaParams.ht_ante_sel);
+		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n",
+			strStaParams.flags_mask);
+		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n",
+			strStaParams.flags_set);
+
+		s32Error = wilc_add_station(vif, &strStaParams);
 		if (s32Error)
 			PRINT_ER("Host add station fail\n");
 	}
@@ -3039,41 +2527,33 @@ static int add_station(struct wiphy *wiphy, struct net_device *dev,
 	return s32Error;
 }
 
-/**
- *  @brief      del_station
- *  @details    Remove a station; @mac may be NULL to remove all stations.
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int del_station(struct wiphy *wiphy, struct net_device *dev,
 		       struct station_del_parameters *params)
 {
 	const u8 *mac = params->mac;
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 	if (!wiphy)
 		return -EFAULT;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(dev);
+	vif = netdev_priv(dev);
 
-	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
+	if (vif->iftype == AP_MODE || vif->iftype == GO_MODE) {
 		PRINT_D(HOSTAPD_DBG, "Deleting station\n");
 
 
-		if (mac == NULL) {
+		if (!mac) {
 			PRINT_D(HOSTAPD_DBG, "All associated stations\n");
-			s32Error = host_int_del_allstation(priv->hWILCWFIDrv, priv->assoc_stainfo.au8Sta_AssociatedBss);
+			s32Error = wilc_del_allstation(vif,
+				     priv->assoc_stainfo.au8Sta_AssociatedBss);
 		} else {
 			PRINT_D(HOSTAPD_DBG, "With mac address: %x%x%x%x%x%x\n", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 		}
 
-		s32Error = host_int_del_station(priv->hWILCWFIDrv, mac);
+		s32Error = wilc_del_station(vif, mac);
 
 		if (s32Error)
 			PRINT_ER("Host delete station fail\n");
@@ -3081,22 +2561,13 @@ static int del_station(struct wiphy *wiphy, struct net_device *dev,
 	return s32Error;
 }
 
-/**
- *  @brief      change_station
- *  @details    Modify a given station.
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 static int change_station(struct wiphy *wiphy, struct net_device *dev,
 			  const u8 *mac, struct station_parameters *params)
 {
 	s32 s32Error = 0;
 	struct wilc_priv *priv;
 	struct add_sta_param strStaParams = { {0} };
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 
 
 	PRINT_D(HOSTAPD_DBG, "Change station paramters\n");
@@ -3105,61 +2576,63 @@ static int change_station(struct wiphy *wiphy, struct net_device *dev,
 		return -EFAULT;
 
 	priv = wiphy_priv(wiphy);
-	nic = netdev_priv(dev);
-
-	if (nic->iftype == AP_MODE || nic->iftype == GO_MODE) {
-		memcpy(strStaParams.au8BSSID, mac, ETH_ALEN);
-		strStaParams.u16AssocID = params->aid;
-		strStaParams.u8NumRates = params->supported_rates_len;
-		strStaParams.pu8Rates = params->supported_rates;
-
-		PRINT_D(HOSTAPD_DBG, "BSSID = %x%x%x%x%x%x\n", strStaParams.au8BSSID[0], strStaParams.au8BSSID[1], strStaParams.au8BSSID[2], strStaParams.au8BSSID[3], strStaParams.au8BSSID[4],
-			strStaParams.au8BSSID[5]);
-		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.u16AssocID);
-		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n", strStaParams.u8NumRates);
-
-		if (params->ht_capa == NULL) {
-			strStaParams.bIsHTSupported = false;
+	vif = netdev_priv(dev);
+
+	if (vif->iftype == AP_MODE || vif->iftype == GO_MODE) {
+		memcpy(strStaParams.bssid, mac, ETH_ALEN);
+		strStaParams.aid = params->aid;
+		strStaParams.rates_len = params->supported_rates_len;
+		strStaParams.rates = params->supported_rates;
+
+		PRINT_D(HOSTAPD_DBG, "BSSID = %x%x%x%x%x%x\n",
+			strStaParams.bssid[0], strStaParams.bssid[1],
+			strStaParams.bssid[2], strStaParams.bssid[3],
+			strStaParams.bssid[4], strStaParams.bssid[5]);
+		PRINT_D(HOSTAPD_DBG, "ASSOC ID = %d\n", strStaParams.aid);
+		PRINT_D(HOSTAPD_DBG, "Number of supported rates = %d\n",
+			strStaParams.rates_len);
+
+		if (!params->ht_capa) {
+			strStaParams.ht_supported = false;
 		} else {
-			strStaParams.bIsHTSupported = true;
-			strStaParams.u16HTCapInfo = params->ht_capa->cap_info;
-			strStaParams.u8AmpduParams = params->ht_capa->ampdu_params_info;
-			memcpy(strStaParams.au8SuppMCsSet, &params->ht_capa->mcs, WILC_SUPP_MCS_SET_SIZE);
-			strStaParams.u16HTExtParams = params->ht_capa->extended_ht_cap_info;
-			strStaParams.u32TxBeamformingCap = params->ht_capa->tx_BF_cap_info;
-			strStaParams.u8ASELCap = params->ht_capa->antenna_selection_info;
-
+			strStaParams.ht_supported = true;
+			strStaParams.ht_capa_info = params->ht_capa->cap_info;
+			strStaParams.ht_ampdu_params = params->ht_capa->ampdu_params_info;
+			memcpy(strStaParams.ht_supp_mcs_set,
+			       &params->ht_capa->mcs,
+			       WILC_SUPP_MCS_SET_SIZE);
+			strStaParams.ht_ext_params = params->ht_capa->extended_ht_cap_info;
+			strStaParams.ht_tx_bf_cap = params->ht_capa->tx_BF_cap_info;
+			strStaParams.ht_ante_sel = params->ht_capa->antenna_selection_info;
 		}
 
-		strStaParams.u16FlagsMask = params->sta_flags_mask;
-		strStaParams.u16FlagsSet = params->sta_flags_set;
-
-		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n", strStaParams.bIsHTSupported);
-		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n", strStaParams.u16HTCapInfo);
-		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n", strStaParams.u8AmpduParams);
-		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n", strStaParams.u16HTExtParams);
-		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n", strStaParams.u32TxBeamformingCap);
-		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n", strStaParams.u8ASELCap);
-		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n", strStaParams.u16FlagsMask);
-		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n", strStaParams.u16FlagsSet);
-
-		s32Error = host_int_edit_station(priv->hWILCWFIDrv, &strStaParams);
+		strStaParams.flags_mask = params->sta_flags_mask;
+		strStaParams.flags_set = params->sta_flags_set;
+
+		PRINT_D(HOSTAPD_DBG, "IS HT supported = %d\n",
+			strStaParams.ht_supported);
+		PRINT_D(HOSTAPD_DBG, "Capability Info = %d\n",
+			strStaParams.ht_capa_info);
+		PRINT_D(HOSTAPD_DBG, "AMPDU Params = %d\n",
+			strStaParams.ht_ampdu_params);
+		PRINT_D(HOSTAPD_DBG, "HT Extended params = %d\n",
+			strStaParams.ht_ext_params);
+		PRINT_D(HOSTAPD_DBG, "Tx Beamforming Cap = %d\n",
+			strStaParams.ht_tx_bf_cap);
+		PRINT_D(HOSTAPD_DBG, "Antenna selection info = %d\n",
+			strStaParams.ht_ante_sel);
+		PRINT_D(HOSTAPD_DBG, "Flag Mask = %d\n",
+			strStaParams.flags_mask);
+		PRINT_D(HOSTAPD_DBG, "Flag Set = %d\n",
+			strStaParams.flags_set);
+
+		s32Error = wilc_edit_station(vif, &strStaParams);
 		if (s32Error)
 			PRINT_ER("Host edit station fail\n");
 	}
 	return s32Error;
 }
 
-
-/**
- *  @brief      add_virtual_intf
- *  @details
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 JUL 2012
- *  @version	1.0
- */
 static struct wireless_dev *add_virtual_intf(struct wiphy *wiphy,
 					     const char *name,
 					     unsigned char name_assign_type,
@@ -3167,7 +2640,7 @@ static struct wireless_dev *add_virtual_intf(struct wiphy *wiphy,
 					     u32 *flags,
 					     struct vif_params *params)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc_priv *priv;
 	struct net_device *new_ifc = NULL;
 
@@ -3177,32 +2650,23 @@ static struct wireless_dev *add_virtual_intf(struct wiphy *wiphy,
 
 	PRINT_D(HOSTAPD_DBG, "Adding monitor interface[%p]\n", priv->wdev->netdev);
 
-	nic = netdev_priv(priv->wdev->netdev);
+	vif = netdev_priv(priv->wdev->netdev);
 
 
 	if (type == NL80211_IFTYPE_MONITOR) {
 		PRINT_D(HOSTAPD_DBG, "Monitor interface mode: Initializing mon interface virtual device driver\n");
-		PRINT_D(HOSTAPD_DBG, "Adding monitor interface[%p]\n", nic->wilc_netdev);
-		new_ifc = WILC_WFI_init_mon_interface(name, nic->wilc_netdev);
-		if (new_ifc != NULL) {
+		PRINT_D(HOSTAPD_DBG, "Adding monitor interface[%p]\n", vif->ndev);
+		new_ifc = WILC_WFI_init_mon_interface(name, vif->ndev);
+		if (new_ifc) {
 			PRINT_D(HOSTAPD_DBG, "Setting monitor flag in private structure\n");
-			nic = netdev_priv(priv->wdev->netdev);
-			nic->monitor_flag = 1;
+			vif = netdev_priv(priv->wdev->netdev);
+			vif->monitor_flag = 1;
 		} else
 			PRINT_ER("Error in initializing monitor interface\n ");
 	}
 	return priv->wdev;
 }
 
-/**
- *  @brief      del_virtual_intf
- *  @details
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 JUL 2012
- *  @version	1.0
- */
 static int del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 {
 	PRINT_D(HOSTAPD_DBG, "Deleting virtual interface\n");
@@ -3210,7 +2674,6 @@ static int del_virtual_intf(struct wiphy *wiphy, struct wireless_dev *wdev)
 }
 
 static struct cfg80211_ops wilc_cfg80211_ops = {
-
 	.set_monitor_channel = set_channel,
 	.scan = scan,
 	.connect = connect,
@@ -3247,27 +2710,12 @@ static struct cfg80211_ops wilc_cfg80211_ops = {
 
 };
 
-
-
-
-
-/**
- *  @brief      WILC_WFI_update_stats
- *  @details    Modify parameters for a given BSS.
- *  @param[in]
- *  @return     int : Return 0 on Success.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 int WILC_WFI_update_stats(struct wiphy *wiphy, u32 pktlen, u8 changed)
 {
-
 	struct wilc_priv *priv;
 
 	priv = wiphy_priv(wiphy);
 	switch (changed) {
-
 	case WILC_WFI_RX_PKT:
 	{
 		priv->netstats.rx_packets++;
@@ -3291,46 +2739,31 @@ int WILC_WFI_update_stats(struct wiphy *wiphy, u32 pktlen, u8 changed)
 	return 0;
 }
 
-/**
- *  @brief      WILC_WFI_CfgAlloc
- *  @details    Allocation of the wireless device structure and assigning it
- *		to the cfg80211 operations structure.
- *  @param[in]   NONE
- *  @return     wireless_dev : Returns pointer to wireless_dev structure.
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-struct wireless_dev *WILC_WFI_CfgAlloc(void)
+static struct wireless_dev *WILC_WFI_CfgAlloc(void)
 {
-
 	struct wireless_dev *wdev;
 
 
 	PRINT_D(CFG80211_DBG, "Allocating wireless device\n");
-	/*Allocating the wireless device structure*/
+
 	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
 	if (!wdev) {
 		PRINT_ER("Cannot allocate wireless device\n");
 		goto _fail_;
 	}
 
-	/*Creating a new wiphy, linking wireless structure with the wiphy structure*/
 	wdev->wiphy = wiphy_new(&wilc_cfg80211_ops, sizeof(struct wilc_priv));
 	if (!wdev->wiphy) {
 		PRINT_ER("Cannot allocate wiphy\n");
 		goto _fail_mem_;
-
 	}
 
-	/* enable 802.11n HT */
 	WILC_WFI_band_2ghz.ht_cap.ht_supported = 1;
 	WILC_WFI_band_2ghz.ht_cap.cap |= (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT);
 	WILC_WFI_band_2ghz.ht_cap.mcs.rx_mask[0] = 0xff;
 	WILC_WFI_band_2ghz.ht_cap.ampdu_factor = IEEE80211_HT_MAX_AMPDU_8K;
 	WILC_WFI_band_2ghz.ht_cap.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE;
 
-	/*wiphy bands*/
 	wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &WILC_WFI_band_2ghz;
 
 	return wdev;
@@ -3339,18 +2772,9 @@ _fail_mem_:
 	kfree(wdev);
 _fail_:
 	return NULL;
-
 }
-/**
- *  @brief      wilc_create_wiphy
- *  @details    Registering of the wiphy structure and interface modes
- *  @param[in]   NONE
- *  @return     NONE
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
-struct wireless_dev *wilc_create_wiphy(struct net_device *net)
+
+struct wireless_dev *wilc_create_wiphy(struct net_device *net, struct device *dev)
 {
 	struct wilc_priv *priv;
 	struct wireless_dev *wdev;
@@ -3359,38 +2783,25 @@ struct wireless_dev *wilc_create_wiphy(struct net_device *net)
 	PRINT_D(CFG80211_DBG, "Registering wifi device\n");
 
 	wdev = WILC_WFI_CfgAlloc();
-	if (wdev == NULL) {
+	if (!wdev) {
 		PRINT_ER("CfgAlloc Failed\n");
 		return NULL;
 	}
 
-
-	/*Return hardware description structure (wiphy)'s priv*/
 	priv = wdev_priv(wdev);
 	sema_init(&(priv->SemHandleUpdateStats), 1);
-
-	/*Link the wiphy with wireless structure*/
 	priv->wdev = wdev;
-
-	/*Maximum number of probed ssid to be added by user for the scan request*/
 	wdev->wiphy->max_scan_ssids = MAX_NUM_PROBED_SSID;
-	/*Maximum number of pmkids to be cashed*/
 	wdev->wiphy->max_num_pmkids = WILC_MAX_NUM_PMKIDS;
 	PRINT_INFO(CFG80211_DBG, "Max number of PMKIDs = %d\n", wdev->wiphy->max_num_pmkids);
 
 	wdev->wiphy->max_scan_ie_len = 1000;
-
-	/*signal strength in mBm (100*dBm) */
 	wdev->wiphy->signal_type = CFG80211_SIGNAL_TYPE_MBM;
-
-	/*Set the availaible cipher suites*/
 	wdev->wiphy->cipher_suites = cipher_suites;
 	wdev->wiphy->n_cipher_suites = ARRAY_SIZE(cipher_suites);
-	/*Setting default managment types: for register action frame:  */
 	wdev->wiphy->mgmt_stypes = wilc_wfi_cfg80211_mgmt_types;
 
 	wdev->wiphy->max_remain_on_channel_duration = 500;
-	/*Setting the wiphy interfcae mode and type before registering the wiphy*/
 	wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) | BIT(NL80211_IFTYPE_AP) | BIT(NL80211_IFTYPE_MONITOR) | BIT(NL80211_IFTYPE_P2P_GO) |
 		BIT(NL80211_IFTYPE_P2P_CLIENT);
 	wdev->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
@@ -3402,36 +2813,21 @@ struct wireless_dev *wilc_create_wiphy(struct net_device *net)
 		   wdev->wiphy->max_scan_ssids, wdev->wiphy->max_scan_ie_len, wdev->wiphy->signal_type,
 		   wdev->wiphy->interface_modes, wdev->iftype);
 
-	#ifdef WILC_SDIO
-	set_wiphy_dev(wdev->wiphy, &local_sdio_func->dev);
-	#endif
+	set_wiphy_dev(wdev->wiphy, dev);
 
-	/*Register wiphy structure*/
 	s32Error = wiphy_register(wdev->wiphy);
 	if (s32Error) {
 		PRINT_ER("Cannot register wiphy device\n");
-		/*should define what action to be taken in such failure*/
 	} else {
 		PRINT_D(CFG80211_DBG, "Successful Registering\n");
 	}
 
 	priv->dev = net;
 	return wdev;
-
-
 }
-/**
- *  @brief      WILC_WFI_WiphyFree
- *  @details    Freeing allocation of the wireless device structure
- *  @param[in]   NONE
- *  @return     NONE
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
+
 int wilc_init_host_int(struct net_device *net)
 {
-
 	int s32Error = 0;
 
 	struct wilc_priv *priv;
@@ -3440,7 +2836,7 @@ int wilc_init_host_int(struct net_device *net)
 	priv = wdev_priv(net->ieee80211_ptr);
 	if (op_ifcs == 0) {
 		setup_timer(&hAgingTimer, remove_network_from_shadow, 0);
-		setup_timer(&hDuringIpTimer, clear_duringIP, 0);
+		setup_timer(&wilc_during_ip_timer, clear_duringIP, 0);
 	}
 	op_ifcs++;
 	if (s32Error < 0) {
@@ -3453,29 +2849,21 @@ int wilc_init_host_int(struct net_device *net)
 	priv->bInP2PlistenState = false;
 
 	sema_init(&(priv->hSemScanReq), 1);
-	s32Error = host_int_init(net, &priv->hWILCWFIDrv);
+	s32Error = wilc_init(net, &priv->hWILCWFIDrv);
 	if (s32Error)
 		PRINT_ER("Error while initializing hostinterface\n");
 
 	return s32Error;
 }
 
-/**
- *  @brief      WILC_WFI_WiphyFree
- *  @details    Freeing allocation of the wireless device structure
- *  @param[in]   NONE
- *  @return     NONE
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 int wilc_deinit_host_int(struct net_device *net)
 {
 	int s32Error = 0;
-
+	struct wilc_vif *vif;
 	struct wilc_priv *priv;
 
 	priv = wdev_priv(net->ieee80211_ptr);
+	vif = netdev_priv(priv->dev);
 
 	priv->gbAutoRateAdjusted = false;
 
@@ -3483,13 +2871,12 @@ int wilc_deinit_host_int(struct net_device *net)
 
 	op_ifcs--;
 
-	s32Error = host_int_deinit(priv->hWILCWFIDrv);
+	s32Error = wilc_deinit(vif);
 
-	/* Clear the Shadow scan */
-	clear_shadow_scan(priv);
+	clear_shadow_scan();
 	if (op_ifcs == 0) {
 		PRINT_D(CORECONFIG_DBG, "destroy during ip\n");
-		del_timer_sync(&hDuringIpTimer);
+		del_timer_sync(&wilc_during_ip_timer);
 	}
 
 	if (s32Error)
@@ -3498,16 +2885,6 @@ int wilc_deinit_host_int(struct net_device *net)
 	return s32Error;
 }
 
-
-/**
- *  @brief      WILC_WFI_WiphyFree
- *  @details    Freeing allocation of the wireless device structure
- *  @param[in]   NONE
- *  @return     NONE
- *  @author	mdaftedar
- *  @date	01 MAR 2012
- *  @version	1.0
- */
 void wilc_free_wiphy(struct net_device *net)
 {
 	PRINT_D(CFG80211_DBG, "Unregistering wiphy\n");
diff --git a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h
index 39cd8e1b5675..ab53d9d59081 100644
--- a/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h
+++ b/drivers/staging/wilc1000/wilc_wfi_cfgoperations.h
@@ -10,88 +10,7 @@
 #define NM_WFI_CFGOPERATIONS
 #include "wilc_wfi_netdevice.h"
 
-/* The following macros describe the bitfield map used by the firmware to determine its 11i mode */
-#define NO_ENCRYPT		0
-#define ENCRYPT_ENABLED		BIT(0)
-#define WEP			BIT(1)
-#define WEP_EXTENDED		BIT(2)
-#define WPA			BIT(3)
-#define WPA2			BIT(4)
-#define AES			BIT(5)
-#define TKIP			BIT(6)
-
-/*Public action frame index IDs*/
-#define FRAME_TYPE_ID			0
-#define ACTION_CAT_ID			24
-#define ACTION_SUBTYPE_ID		25
-#define P2P_PUB_ACTION_SUBTYPE		30
-
-/*Public action frame Attribute IDs*/
-#define ACTION_FRAME			0xd0
-#define GO_INTENT_ATTR_ID		0x04
-#define CHANLIST_ATTR_ID		0x0b
-#define OPERCHAN_ATTR_ID		0x11
-#define PUB_ACTION_ATTR_ID		0x04
-#define P2PELEM_ATTR_ID			0xdd
-
-/*Public action subtype values*/
-#define GO_NEG_REQ			0x00
-#define GO_NEG_RSP			0x01
-#define GO_NEG_CONF			0x02
-#define P2P_INV_REQ			0x03
-#define P2P_INV_RSP			0x04
-#define PUBLIC_ACT_VENDORSPEC		0x09
-#define GAS_INTIAL_REQ			0x0a
-#define GAS_INTIAL_RSP			0x0b
-
-#define INVALID_CHANNEL			0
-
-#define nl80211_SCAN_RESULT_EXPIRE	(3 * HZ)
-#define SCAN_RESULT_EXPIRE		(40 * HZ)
-
-static const u32 cipher_suites[] = {
-	WLAN_CIPHER_SUITE_WEP40,
-	WLAN_CIPHER_SUITE_WEP104,
-	WLAN_CIPHER_SUITE_TKIP,
-	WLAN_CIPHER_SUITE_CCMP,
-	WLAN_CIPHER_SUITE_AES_CMAC,
-};
-
-static const struct ieee80211_txrx_stypes
-	wilc_wfi_cfg80211_mgmt_types[NUM_NL80211_IFTYPES] = {
-	[NL80211_IFTYPE_STATION] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
-			BIT(IEEE80211_STYPE_PROBE_REQ >> 4)
-	},
-	[NL80211_IFTYPE_AP] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
-			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
-			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
-			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
-			BIT(IEEE80211_STYPE_AUTH >> 4) |
-			BIT(IEEE80211_STYPE_DEAUTH >> 4) |
-			BIT(IEEE80211_STYPE_ACTION >> 4)
-	},
-	[NL80211_IFTYPE_P2P_CLIENT] = {
-		.tx = 0xffff,
-		.rx = BIT(IEEE80211_STYPE_ACTION >> 4) |
-			BIT(IEEE80211_STYPE_PROBE_REQ >> 4) |
-			BIT(IEEE80211_STYPE_ASSOC_REQ >> 4) |
-			BIT(IEEE80211_STYPE_REASSOC_REQ >> 4) |
-			BIT(IEEE80211_STYPE_DISASSOC >> 4) |
-			BIT(IEEE80211_STYPE_AUTH >> 4) |
-			BIT(IEEE80211_STYPE_DEAUTH >> 4)
-	}
-};
-
-/* Time to stay on the channel */
-#define WILC_WFI_DWELL_PASSIVE 100
-#define WILC_WFI_DWELL_ACTIVE  40
-
-struct wireless_dev *WILC_WFI_CfgAlloc(void);
-struct wireless_dev *wilc_create_wiphy(struct net_device *net);
+struct wireless_dev *wilc_create_wiphy(struct net_device *net, struct device *dev);
 void wilc_free_wiphy(struct net_device *net);
 int WILC_WFI_update_stats(struct wiphy *wiphy, u32 pktlen, u8 changed);
 int wilc_deinit_host_int(struct net_device *net);
@@ -102,8 +21,4 @@ struct net_device *WILC_WFI_init_mon_interface(const char *name, struct net_devi
 void wilc_mgmt_frame_register(struct wiphy *wiphy, struct wireless_dev *wdev,
 			      u16 frame_type, bool reg);
 
-#define TCP_ACK_FILTER_LINK_SPEED_THRESH	54
-#define DEFAULT_LINK_SPEED			72
-void Enable_TCP_ACK_Filter(bool value);
-
 #endif
diff --git a/drivers/staging/wilc1000/wilc_wfi_netdevice.h b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
index 0bfe7626ad2d..98ac8ed04a06 100644
--- a/drivers/staging/wilc1000/wilc_wfi_netdevice.h
+++ b/drivers/staging/wilc1000/wilc_wfi_netdevice.h
@@ -149,6 +149,13 @@ typedef struct {
 } struct_frame_reg;
 
 struct wilc_vif {
+	u8 u8IfIdx;
+	u8 iftype;
+	int monitor_flag;
+	int mac_opened;
+	struct_frame_reg g_struct_frame_reg[num_reg_frame];
+	struct net_device_stats netstats;
+	struct wilc *wilc;
 	u8 src_addr[ETH_ALEN];
 	u8 bssid[ETH_ALEN];
 	struct host_if_drv *hif_drv;
@@ -156,14 +163,15 @@ struct wilc_vif {
 };
 
 struct wilc {
+	const struct wilc_hif_func *hif_func;
+	int io_type;
 	int mac_status;
+	int gpio;
 	bool initialized;
-	#if (!defined WILC_SDIO) || (defined WILC_SDIO_IRQ_GPIO)
-	unsigned short dev_irq_num;
-	#endif
+	int dev_irq_num;
 	int close;
 	u8 vif_num;
-	struct wilc_vif vif[NUM_CONCURRENT_IFC];
+	struct wilc_vif *vif[NUM_CONCURRENT_IFC];
 	u8 open_ifcs;
 
 	struct semaphore txq_add_to_head_cs;
@@ -180,42 +188,54 @@ struct wilc {
 
 	struct task_struct *txq_thread;
 
+	int quit;
+	int cfg_frame_in_use;
+	struct wilc_cfg_frame cfg_frame;
+	u32 cfg_frame_offset;
+	int cfg_seq_no;
+
+	u8 *rx_buffer;
+	u32 rx_buffer_offset;
+	u8 *tx_buffer;
+
+	unsigned long txq_spinlock_flags;
+
+	struct txq_entry_t *txq_head;
+	struct txq_entry_t *txq_tail;
+	int txq_entries;
+	int txq_exit;
+
+	struct rxq_entry_t *rxq_head;
+	struct rxq_entry_t *rxq_tail;
+	int rxq_entries;
+	int rxq_exit;
+
 	unsigned char eth_src_address[NUM_CONCURRENT_IFC][6];
 
 	const struct firmware *firmware;
 
-#ifdef WILC_SDIO
-	struct sdio_func *wilc_sdio_func;
-#else
-	struct spi_device *wilc_spidev;
-#endif
+	struct device *dev;
 };
 
-typedef struct {
-	u8 u8IfIdx;
-	u8 iftype;
-	int monitor_flag;
-	int mac_opened;
-	struct_frame_reg g_struct_frame_reg[num_reg_frame];
-	struct net_device *wilc_netdev;
-	struct net_device_stats netstats;
-	struct wilc *wilc;
-} perInterface_wlan_t;
-
 struct WILC_WFI_mon_priv {
 	struct net_device *real_ndev;
 };
 
-extern struct wilc *g_linux_wlan;
-extern struct net_device *WILC_WFI_devs[];
-void frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset);
-void linux_wlan_mac_indicate(struct wilc *wilc, int flag);
-void linux_wlan_rx_complete(void);
-void linux_wlan_dbg(u8 *buff);
-int linux_wlan_lock_timeout(void *vp, u32 timeout);
-void wl_wlan_cleanup(void);
-int wilc_netdev_init(struct wilc **wilc);
+int wilc1000_wlan_init(struct net_device *dev, struct wilc_vif *vif);
+
+void wilc_frmw_to_linux(struct wilc *wilc, u8 *buff, u32 size, u32 pkt_offset);
+void wilc_mac_indicate(struct wilc *wilc, int flag);
+void wilc_rx_complete(struct wilc *wilc);
+void wilc_dbg(u8 *buff);
+
+int wilc_lock_timeout(struct wilc *wilc, void *, u32 timeout);
+void wilc_netdev_cleanup(struct wilc *wilc);
+int wilc_netdev_init(struct wilc **wilc, struct device *, int io_type, int gpio,
+		     const struct wilc_hif_func *ops);
 void wilc1000_wlan_deinit(struct net_device *dev);
 void WILC_WFI_mgmt_rx(struct wilc *wilc, u8 *buff, u32 size);
-u16 Set_machw_change_vir_if(struct net_device *dev, bool bValue);
+u16 wilc_set_machw_change_vir_if(struct net_device *dev, bool value);
+int wilc_wlan_get_firmware(struct net_device *dev);
+int wilc_wlan_set_bssid(struct net_device *wilc_netdev, u8 *bssid);
+
 #endif
diff --git a/drivers/staging/wilc1000/wilc_wlan.c b/drivers/staging/wilc1000/wilc_wlan.c
index c02665747705..83af51bb83e8 100644
--- a/drivers/staging/wilc1000/wilc_wlan.c
+++ b/drivers/staging/wilc1000/wilc_wlan.c
@@ -1,95 +1,15 @@
-/* ////////////////////////////////////////////////////////////////////////// */
-/*  */
-/* Copyright (c) Atmel Corporation.  All rights reserved. */
-/*  */
-/* Module Name:  wilc_wlan.c */
-/*  */
-/*  */
-/* //////////////////////////////////////////////////////////////////////////// */
-
 #include "wilc_wlan_if.h"
+#include "wilc_wlan.h"
 #include "wilc_wfi_netdevice.h"
 #include "wilc_wlan_cfg.h"
 
-/********************************************
- *
- *      Global
- *
- ********************************************/
-extern wilc_hif_func_t hif_sdio;
-extern wilc_hif_func_t hif_spi;
-u32 wilc_get_chipid(u8 update);
-
-
-
-typedef struct {
-	int quit;
-
-	/**
-	 *      input interface functions
-	 **/
-	wilc_wlan_io_func_t io_func;
-
-	/**
-	 *      host interface functions
-	 **/
-	wilc_hif_func_t hif_func;
-
-	/**
-	 *      configuration interface functions
-	 **/
-	int cfg_frame_in_use;
-	wilc_cfg_frame_t cfg_frame;
-	u32 cfg_frame_offset;
-	int cfg_seq_no;
-
-	/**
-	 *      RX buffer
-	 **/
-	#ifdef MEMORY_STATIC
-	u8 *rx_buffer;
-	u32 rx_buffer_offset;
-	#endif
-	/**
-	 *      TX buffer
-	 **/
-	u8 *tx_buffer;
-	u32 tx_buffer_offset;
-
-	/**
-	 *      TX queue
-	 **/
-
-	unsigned long txq_spinlock_flags;
-
-	struct txq_entry_t *txq_head;
-	struct txq_entry_t *txq_tail;
-	int txq_entries;
-	int txq_exit;
-
-	/**
-	 *      RX queue
-	 **/
-	struct rxq_entry_t *rxq_head;
-	struct rxq_entry_t *rxq_tail;
-	int rxq_entries;
-	int rxq_exit;
-
-
-} wilc_wlan_dev_t;
-
-static wilc_wlan_dev_t g_wlan;
-
-static inline void chip_allow_sleep(void);
-static inline void chip_wakeup(void);
-/********************************************
- *
- *      Debug
- *
- ********************************************/
-
+#ifdef WILC_OPTIMIZE_SLEEP_INT
+static inline void chip_allow_sleep(struct wilc *wilc);
+#endif
+static inline void chip_wakeup(struct wilc *wilc);
 static u32 dbgflag = N_INIT | N_ERR | N_INTR | N_TXQ | N_RXQ;
 
+/* FIXME: replace with dev_debug() */
 static void wilc_debug(u32 flag, char *fmt, ...)
 {
 	char buf[256];
@@ -100,244 +20,214 @@ static void wilc_debug(u32 flag, char *fmt, ...)
 		vsprintf(buf, fmt, args);
 		va_end(args);
 
-		linux_wlan_dbg(buf);
+		wilc_dbg(buf);
 	}
 }
 
-static CHIP_PS_STATE_T genuChipPSstate = CHIP_WAKEDUP;
+static CHIP_PS_STATE_T chip_ps_state = CHIP_WAKEDUP;
 
-/*acquire_bus() and release_bus() are made static inline functions*/
-/*as a temporary workaround to fix a problem of receiving*/
-/*unknown interrupt from FW*/
-static inline void acquire_bus(BUS_ACQUIRE_T acquire)
+static inline void acquire_bus(struct wilc *wilc, BUS_ACQUIRE_T acquire)
 {
-
-	mutex_lock(&g_linux_wlan->hif_cs);
+	mutex_lock(&wilc->hif_cs);
 	#ifndef WILC_OPTIMIZE_SLEEP_INT
-	if (genuChipPSstate != CHIP_WAKEDUP)
+	if (chip_ps_state != CHIP_WAKEDUP)
 	#endif
 	{
 		if (acquire == ACQUIRE_AND_WAKEUP)
-			chip_wakeup();
+			chip_wakeup(wilc);
 	}
-
 }
-static inline void release_bus(BUS_RELEASE_T release)
+
+static inline void release_bus(struct wilc *wilc, BUS_RELEASE_T release)
 {
 	#ifdef WILC_OPTIMIZE_SLEEP_INT
 	if (release == RELEASE_ALLOW_SLEEP)
-		chip_allow_sleep();
+		chip_allow_sleep(wilc);
 	#endif
-	mutex_unlock(&g_linux_wlan->hif_cs);
+	mutex_unlock(&wilc->hif_cs);
 }
-/********************************************
- *
- *      Queue
- *
- ********************************************/
 
+#ifdef TCP_ACK_FILTER
 static void wilc_wlan_txq_remove(struct txq_entry_t *tqe)
 {
 
-	wilc_wlan_dev_t *p = &g_wlan;
-	if (tqe == p->txq_head)	{
-
-		p->txq_head = tqe->next;
-		if (p->txq_head)
-			p->txq_head->prev = NULL;
-
-
-	} else if (tqe == p->txq_tail)	    {
-		p->txq_tail = (tqe->prev);
-		if (p->txq_tail)
-			p->txq_tail->next = NULL;
+	if (tqe == wilc->txq_head) {
+		wilc->txq_head = tqe->next;
+		if (wilc->txq_head)
+			wilc->txq_head->prev = NULL;
+	} else if (tqe == wilc->txq_tail) {
+		wilc->txq_tail = (tqe->prev);
+		if (wilc->txq_tail)
+			wilc->txq_tail->next = NULL;
 	} else {
 		tqe->prev->next = tqe->next;
 		tqe->next->prev = tqe->prev;
 	}
-	p->txq_entries -= 1;
-
+	wilc->txq_entries -= 1;
 }
+#endif
 
-static struct txq_entry_t *wilc_wlan_txq_remove_from_head(void)
+static struct txq_entry_t *
+wilc_wlan_txq_remove_from_head(struct net_device *dev)
 {
 	struct txq_entry_t *tqe;
-	wilc_wlan_dev_t *p = &g_wlan;
 	unsigned long flags;
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 
-	spin_lock_irqsave(&g_linux_wlan->txq_spinlock, flags);
-	if (p->txq_head) {
-		tqe = p->txq_head;
-		p->txq_head = tqe->next;
-		if (p->txq_head)
-			p->txq_head->prev = NULL;
-
-		p->txq_entries -= 1;
-
-
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
+	spin_lock_irqsave(&wilc->txq_spinlock, flags);
+	if (wilc->txq_head) {
+		tqe = wilc->txq_head;
+		wilc->txq_head = tqe->next;
+		if (wilc->txq_head)
+			wilc->txq_head->prev = NULL;
 
+		wilc->txq_entries -= 1;
 	} else {
 		tqe = NULL;
 	}
-	spin_unlock_irqrestore(&g_linux_wlan->txq_spinlock, flags);
+	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 	return tqe;
 }
 
-static void wilc_wlan_txq_add_to_tail(struct txq_entry_t *tqe)
+static void wilc_wlan_txq_add_to_tail(struct net_device *dev,
+				      struct txq_entry_t *tqe)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	unsigned long flags;
-	spin_lock_irqsave(&g_linux_wlan->txq_spinlock, flags);
+	struct wilc_vif *vif;
+	struct wilc *wilc;
+
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	if (p->txq_head == NULL) {
+	spin_lock_irqsave(&wilc->txq_spinlock, flags);
+
+	if (!wilc->txq_head) {
 		tqe->next = NULL;
 		tqe->prev = NULL;
-		p->txq_head = tqe;
-		p->txq_tail = tqe;
+		wilc->txq_head = tqe;
+		wilc->txq_tail = tqe;
 	} else {
 		tqe->next = NULL;
-		tqe->prev = p->txq_tail;
-		p->txq_tail->next = tqe;
-		p->txq_tail = tqe;
+		tqe->prev = wilc->txq_tail;
+		wilc->txq_tail->next = tqe;
+		wilc->txq_tail = tqe;
 	}
-	p->txq_entries += 1;
-	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", p->txq_entries);
+	wilc->txq_entries += 1;
+	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", wilc->txq_entries);
 
-	spin_unlock_irqrestore(&g_linux_wlan->txq_spinlock, flags);
+	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 
-	/**
-	 *      wake up TX queue
-	 **/
 	PRINT_D(TX_DBG, "Wake the txq_handling\n");
 
-	up(&g_linux_wlan->txq_event);
+	up(&wilc->txq_event);
 }
 
-static int wilc_wlan_txq_add_to_head(struct txq_entry_t *tqe)
+static int wilc_wlan_txq_add_to_head(struct wilc *wilc, struct txq_entry_t *tqe)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	unsigned long flags;
-	if (linux_wlan_lock_timeout(&g_linux_wlan->txq_add_to_head_cs,
+	if (wilc_lock_timeout(wilc, &wilc->txq_add_to_head_cs,
 				    CFG_PKTS_TIMEOUT))
 		return -1;
 
-	spin_lock_irqsave(&g_linux_wlan->txq_spinlock, flags);
+	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	if (p->txq_head == NULL) {
+	if (!wilc->txq_head) {
 		tqe->next = NULL;
 		tqe->prev = NULL;
-		p->txq_head = tqe;
-		p->txq_tail = tqe;
+		wilc->txq_head = tqe;
+		wilc->txq_tail = tqe;
 	} else {
-		tqe->next = p->txq_head;
+		tqe->next = wilc->txq_head;
 		tqe->prev = NULL;
-		p->txq_head->prev = tqe;
-		p->txq_head = tqe;
+		wilc->txq_head->prev = tqe;
+		wilc->txq_head = tqe;
 	}
-	p->txq_entries += 1;
-	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", p->txq_entries);
+	wilc->txq_entries += 1;
+	PRINT_D(TX_DBG, "Number of entries in TxQ = %d\n", wilc->txq_entries);
 
-	spin_unlock_irqrestore(&g_linux_wlan->txq_spinlock, flags);
-	up(&g_linux_wlan->txq_add_to_head_cs);
-
-
-	/**
-	 *      wake up TX queue
-	 **/
-	up(&g_linux_wlan->txq_event);
+	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
+	up(&wilc->txq_add_to_head_cs);
+	up(&wilc->txq_event);
 	PRINT_D(TX_DBG, "Wake up the txq_handler\n");
 
 	return 0;
-
 }
 
-u32 Statisitcs_totalAcks = 0, Statisitcs_DroppedAcks = 0;
-
 #ifdef	TCP_ACK_FILTER
-struct Ack_session_info;
-struct Ack_session_info {
-	u32 Ack_seq_num;
-	u32 Bigger_Ack_num;
+struct ack_session_info;
+struct ack_session_info {
+	u32 seq_num;
+	u32 bigger_ack_num;
 	u16 src_port;
 	u16 dst_port;
 	u16 status;
 };
 
-typedef struct {
+struct pending_acks_info {
 	u32 ack_num;
-	u32 Session_index;
+	u32 session_index;
 	struct txq_entry_t  *txqe;
-} Pending_Acks_info_t /*Ack_info_t*/;
-
-
-
+};
 
-struct Ack_session_info *Free_head;
-struct Ack_session_info *Alloc_head;
 
 #define NOT_TCP_ACK			(-1)
 
 #define MAX_TCP_SESSION		25
 #define MAX_PENDING_ACKS		256
-struct Ack_session_info Acks_keep_track_info[2 * MAX_TCP_SESSION];
-Pending_Acks_info_t Pending_Acks_info[MAX_PENDING_ACKS];
-
-u32 PendingAcks_arrBase;
-u32 Opened_TCP_session;
-u32 Pending_Acks;
-
+static struct ack_session_info ack_session_info[2 * MAX_TCP_SESSION];
+static struct pending_acks_info pending_acks_info[MAX_PENDING_ACKS];
 
+static u32 pending_base;
+static u32 tcp_session;
+static u32 pending_acks;
 
-static inline int Init_TCP_tracking(void)
+static inline int init_tcp_tracking(void)
 {
-
 	return 0;
-
 }
-static inline int add_TCP_track_session(u32 src_prt, u32 dst_prt, u32 seq)
+
+static inline int add_tcp_session(u32 src_prt, u32 dst_prt, u32 seq)
 {
-	Acks_keep_track_info[Opened_TCP_session].Ack_seq_num = seq;
-	Acks_keep_track_info[Opened_TCP_session].Bigger_Ack_num = 0;
-	Acks_keep_track_info[Opened_TCP_session].src_port = src_prt;
-	Acks_keep_track_info[Opened_TCP_session].dst_port = dst_prt;
-	Opened_TCP_session++;
+	ack_session_info[tcp_session].seq_num = seq;
+	ack_session_info[tcp_session].bigger_ack_num = 0;
+	ack_session_info[tcp_session].src_port = src_prt;
+	ack_session_info[tcp_session].dst_port = dst_prt;
+	tcp_session++;
 
-	PRINT_D(TCP_ENH, "TCP Session %d to Ack %d\n", Opened_TCP_session, seq);
+	PRINT_D(TCP_ENH, "TCP Session %d to Ack %d\n", tcp_session, seq);
 	return 0;
 }
 
-static inline int Update_TCP_track_session(u32 index, u32 Ack)
+static inline int update_tcp_session(u32 index, u32 ack)
 {
-
-	if (Ack > Acks_keep_track_info[index].Bigger_Ack_num)
-		Acks_keep_track_info[index].Bigger_Ack_num = Ack;
+	if (ack > ack_session_info[index].bigger_ack_num)
+		ack_session_info[index].bigger_ack_num = ack;
 	return 0;
-
 }
-static inline int add_TCP_Pending_Ack(u32 Ack, u32 Session_index, struct txq_entry_t  *txqe)
-{
-	Statisitcs_totalAcks++;
-	if (Pending_Acks < MAX_PENDING_ACKS) {
-		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].ack_num = Ack;
-		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].txqe = txqe;
-		Pending_Acks_info[PendingAcks_arrBase + Pending_Acks].Session_index = Session_index;
-		txqe->tcp_PendingAck_index = PendingAcks_arrBase + Pending_Acks;
-		Pending_Acks++;
-
-	} else {
 
+static inline int add_tcp_pending_ack(u32 ack, u32 session_index,
+				      struct txq_entry_t *txqe)
+{
+	if (pending_acks < MAX_PENDING_ACKS) {
+		pending_acks_info[pending_base + pending_acks].ack_num = ack;
+		pending_acks_info[pending_base + pending_acks].txqe = txqe;
+		pending_acks_info[pending_base + pending_acks].session_index = session_index;
+		txqe->tcp_pending_ack_idx = pending_base + pending_acks;
+		pending_acks++;
 	}
 	return 0;
 }
-static inline int remove_TCP_related(void)
+static inline int remove_TCP_related(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	unsigned long flags;
 
-	spin_lock_irqsave(&g_linux_wlan->txq_spinlock, flags);
+	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	spin_unlock_irqrestore(&g_linux_wlan->txq_spinlock, flags);
+	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 	return 0;
 }
 
@@ -348,54 +238,55 @@ static inline int tcp_process(struct net_device *dev, struct txq_entry_t *tqe)
 	u8 *buffer = tqe->buffer;
 	unsigned short h_proto;
 	int i;
-	wilc_wlan_dev_t *p = &g_wlan;
 	unsigned long flags;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
 	eth_hdr_ptr = &buffer[0];
 	h_proto = ntohs(*((unsigned short *)&eth_hdr_ptr[12]));
-	if (h_proto == 0x0800) { /* IP */
+	if (h_proto == 0x0800) {
 		u8 *ip_hdr_ptr;
 		u8 protocol;
 
 		ip_hdr_ptr = &buffer[ETHERNET_HDR_LEN];
 		protocol = ip_hdr_ptr[9];
 
-
 		if (protocol == 0x06) {
 			u8 *tcp_hdr_ptr;
-			u32 IHL, Total_Length, Data_offset;
+			u32 IHL, total_length, data_offset;
 
 			tcp_hdr_ptr = &ip_hdr_ptr[IP_HDR_LEN];
 			IHL = (ip_hdr_ptr[0] & 0xf) << 2;
-			Total_Length = (((u32)ip_hdr_ptr[2]) << 8) + ((u32)ip_hdr_ptr[3]);
-			Data_offset = (((u32)tcp_hdr_ptr[12] & 0xf0) >> 2);
-			if (Total_Length == (IHL + Data_offset)) { /*we want to recognize the clear Acks(packet only carry Ack infos not with data) so data size must be equal zero*/
-				u32 seq_no, Ack_no;
-
-				seq_no	= (((u32)tcp_hdr_ptr[4]) << 24) + (((u32)tcp_hdr_ptr[5]) << 16) + (((u32)tcp_hdr_ptr[6]) << 8) + ((u32)tcp_hdr_ptr[7]);
-
-				Ack_no	= (((u32)tcp_hdr_ptr[8]) << 24) + (((u32)tcp_hdr_ptr[9]) << 16) + (((u32)tcp_hdr_ptr[10]) << 8) + ((u32)tcp_hdr_ptr[11]);
-
-
-				for (i = 0; i < Opened_TCP_session; i++) {
-					if (Acks_keep_track_info[i].Ack_seq_num == seq_no) {
-						Update_TCP_track_session(i, Ack_no);
+			total_length = ((u32)ip_hdr_ptr[2] << 8) +
+					(u32)ip_hdr_ptr[3];
+			data_offset = ((u32)tcp_hdr_ptr[12] & 0xf0) >> 2;
+			if (total_length == (IHL + data_offset)) {
+				u32 seq_no, ack_no;
+
+				seq_no = ((u32)tcp_hdr_ptr[4] << 24) +
+					 ((u32)tcp_hdr_ptr[5] << 16) +
+					 ((u32)tcp_hdr_ptr[6] << 8) +
+					 (u32)tcp_hdr_ptr[7];
+
+				ack_no = ((u32)tcp_hdr_ptr[8] << 24) +
+					 ((u32)tcp_hdr_ptr[9] << 16) +
+					 ((u32)tcp_hdr_ptr[10] << 8) +
+					 (u32)tcp_hdr_ptr[11];
+
+				for (i = 0; i < tcp_session; i++) {
+					if (ack_session_info[i].seq_num == seq_no) {
+						update_tcp_session(i, ack_no);
 						break;
 					}
 				}
-				if (i == Opened_TCP_session)
-					add_TCP_track_session(0, 0, seq_no);
-
-				add_TCP_Pending_Ack(Ack_no, i, tqe);
-
+				if (i == tcp_session)
+					add_tcp_session(0, 0, seq_no);
 
+				add_tcp_pending_ack(ack_no, i, tqe);
 			}
 
 		} else {
@@ -408,83 +299,81 @@ static inline int tcp_process(struct net_device *dev, struct txq_entry_t *tqe)
 	return ret;
 }
 
-
 static int wilc_wlan_txq_filter_dup_tcp_ack(struct net_device *dev)
 {
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 	u32 i = 0;
-	u32 Dropped = 0;
-	wilc_wlan_dev_t *p = &g_wlan;
+	u32 dropped = 0;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	spin_lock_irqsave(&wilc->txq_spinlock, p->txq_spinlock_flags);
-	for (i = PendingAcks_arrBase; i < (PendingAcks_arrBase + Pending_Acks); i++) {
-		if (Pending_Acks_info[i].ack_num < Acks_keep_track_info[Pending_Acks_info[i].Session_index].Bigger_Ack_num) {
+	spin_lock_irqsave(&wilc->txq_spinlock, wilc->txq_spinlock_flags);
+	for (i = pending_base; i < (pending_base + pending_acks); i++) {
+		if (pending_acks_info[i].ack_num < ack_session_info[pending_acks_info[i].session_index].bigger_ack_num) {
 			struct txq_entry_t *tqe;
 
-			PRINT_D(TCP_ENH, "DROP ACK: %u\n", Pending_Acks_info[i].ack_num);
-			tqe = Pending_Acks_info[i].txqe;
+			PRINT_D(TCP_ENH, "DROP ACK: %u\n",
+				pending_acks_info[i].ack_num);
+			tqe = pending_acks_info[i].txqe;
 			if (tqe) {
 				wilc_wlan_txq_remove(tqe);
-				Statisitcs_DroppedAcks++;
-				tqe->status = 1;                                /* mark the packet send */
+				tqe->status = 1;
 				if (tqe->tx_complete_func)
-					tqe->tx_complete_func(tqe->priv, tqe->status);
+					tqe->tx_complete_func(tqe->priv,
+							      tqe->status);
 				kfree(tqe);
-				Dropped++;
+				dropped++;
 			}
 		}
 	}
-	Pending_Acks = 0;
-	Opened_TCP_session = 0;
+	pending_acks = 0;
+	tcp_session = 0;
 
-	if (PendingAcks_arrBase == 0)
-		PendingAcks_arrBase = MAX_TCP_SESSION;
+	if (pending_base == 0)
+		pending_base = MAX_TCP_SESSION;
 	else
-		PendingAcks_arrBase = 0;
-
+		pending_base = 0;
 
-	spin_unlock_irqrestore(&wilc->txq_spinlock, p->txq_spinlock_flags);
+	spin_unlock_irqrestore(&wilc->txq_spinlock, wilc->txq_spinlock_flags);
 
-	while (Dropped > 0) {
-		/*consume the semaphore count of the removed packet*/
-		linux_wlan_lock_timeout(&wilc->txq_event, 1);
-		Dropped--;
+	while (dropped > 0) {
+		wilc_lock_timeout(wilc, &wilc->txq_event, 1);
+		dropped--;
 	}
 
 	return 1;
 }
 #endif
 
-bool EnableTCPAckFilter = false;
+static bool enabled = false;
 
-void Enable_TCP_ACK_Filter(bool value)
+void wilc_enable_tcp_ack_filter(bool value)
 {
-	EnableTCPAckFilter = value;
+	enabled = value;
 }
 
-bool is_TCP_ACK_Filter_Enabled(void)
+#ifdef TCP_ACK_FILTER
+static bool is_tcp_ack_filter_enabled(void)
 {
-	return EnableTCPAckFilter;
+	return enabled;
 }
+#endif
 
-static int wilc_wlan_txq_add_cfg_pkt(u8 *buffer, u32 buffer_size)
+static int wilc_wlan_txq_add_cfg_pkt(struct wilc *wilc, u8 *buffer, u32 buffer_size)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	struct txq_entry_t *tqe;
 
 	PRINT_D(TX_DBG, "Adding config packet ...\n");
-	if (p->quit) {
+	if (wilc->quit) {
 		PRINT_D(TX_DBG, "Return due to clear function\n");
-		up(&g_linux_wlan->cfg_event);
+		up(&wilc->cfg_event);
 		return 0;
 	}
 
-	tqe = kmalloc(sizeof(struct txq_entry_t), GFP_ATOMIC);
-	if (tqe == NULL) {
+	tqe = kmalloc(sizeof(*tqe), GFP_ATOMIC);
+	if (!tqe) {
 		PRINT_ER("Failed to allocate memory\n");
 		return 0;
 	}
@@ -495,14 +384,11 @@ static int wilc_wlan_txq_add_cfg_pkt(u8 *buffer, u32 buffer_size)
 	tqe->tx_complete_func = NULL;
 	tqe->priv = NULL;
 #ifdef TCP_ACK_FILTER
-	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
+	tqe->tcp_pending_ack_idx = NOT_TCP_ACK;
 #endif
-	/**
-	 *      Configuration packet always at the front
-	 **/
 	PRINT_D(TX_DBG, "Adding the config packet at the Queue tail\n");
 
-	if (wilc_wlan_txq_add_to_head(tqe))
+	if (wilc_wlan_txq_add_to_head(wilc, tqe))
 		return 0;
 	return 1;
 }
@@ -510,15 +396,18 @@ static int wilc_wlan_txq_add_cfg_pkt(u8 *buffer, u32 buffer_size)
 int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 			      u32 buffer_size, wilc_tx_complete_func_t func)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	struct txq_entry_t *tqe;
+	struct wilc_vif *vif = netdev_priv(dev);
+	struct wilc *wilc;
+
+	wilc = vif->wilc;
 
-	if (p->quit)
+	if (wilc->quit)
 		return 0;
 
-	tqe = kmalloc(sizeof(struct txq_entry_t), GFP_ATOMIC);
+	tqe = kmalloc(sizeof(*tqe), GFP_ATOMIC);
 
-	if (tqe == NULL)
+	if (!tqe)
 		return 0;
 	tqe->type = WILC_NET_PKT;
 	tqe->buffer = buffer;
@@ -528,27 +417,29 @@ int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 
 	PRINT_D(TX_DBG, "Adding mgmt packet at the Queue tail\n");
 #ifdef TCP_ACK_FILTER
-	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
-	if (is_TCP_ACK_Filter_Enabled())
+	tqe->tcp_pending_ack_idx = NOT_TCP_ACK;
+	if (is_tcp_ack_filter_enabled())
 		tcp_process(dev, tqe);
 #endif
-	wilc_wlan_txq_add_to_tail(tqe);
-	/*return number of itemes in the queue*/
-	return p->txq_entries;
+	wilc_wlan_txq_add_to_tail(dev, tqe);
+	return wilc->txq_entries;
 }
 
-int wilc_wlan_txq_add_mgmt_pkt(void *priv, u8 *buffer, u32 buffer_size, wilc_tx_complete_func_t func)
+int wilc_wlan_txq_add_mgmt_pkt(struct net_device *dev, void *priv, u8 *buffer,
+			       u32 buffer_size, wilc_tx_complete_func_t func)
 {
-
-	wilc_wlan_dev_t *p = &g_wlan;
 	struct txq_entry_t *tqe;
+	struct wilc_vif *vif = netdev_priv(dev);
+	struct wilc *wilc;
+
+	wilc = vif->wilc;
 
-	if (p->quit)
+	if (wilc->quit)
 		return 0;
 
-	tqe = kmalloc(sizeof(struct txq_entry_t), GFP_KERNEL);
+	tqe = kmalloc(sizeof(*tqe), GFP_KERNEL);
 
-	if (tqe == NULL)
+	if (!tqe)
 		return 0;
 	tqe->type = WILC_MGMT_PKT;
 	tqe->buffer = buffer;
@@ -556,25 +447,23 @@ int wilc_wlan_txq_add_mgmt_pkt(void *priv, u8 *buffer, u32 buffer_size, wilc_tx_
 	tqe->tx_complete_func = func;
 	tqe->priv = priv;
 #ifdef TCP_ACK_FILTER
-	tqe->tcp_PendingAck_index = NOT_TCP_ACK;
+	tqe->tcp_pending_ack_idx = NOT_TCP_ACK;
 #endif
 	PRINT_D(TX_DBG, "Adding Network packet at the Queue tail\n");
-	wilc_wlan_txq_add_to_tail(tqe);
+	wilc_wlan_txq_add_to_tail(dev, tqe);
 	return 1;
 }
 
-static struct txq_entry_t *wilc_wlan_txq_get_first(void)
+static struct txq_entry_t *wilc_wlan_txq_get_first(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	struct txq_entry_t *tqe;
 	unsigned long flags;
 
-	spin_lock_irqsave(&g_linux_wlan->txq_spinlock, flags);
-
-	tqe = p->txq_head;
+	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
-	spin_unlock_irqrestore(&g_linux_wlan->txq_spinlock, flags);
+	tqe = wilc->txq_head;
 
+	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 
 	return tqe;
 }
@@ -583,52 +472,50 @@ static struct txq_entry_t *wilc_wlan_txq_get_next(struct wilc *wilc,
 						  struct txq_entry_t *tqe)
 {
 	unsigned long flags;
+
 	spin_lock_irqsave(&wilc->txq_spinlock, flags);
 
 	tqe = tqe->next;
 	spin_unlock_irqrestore(&wilc->txq_spinlock, flags);
 
-
 	return tqe;
 }
 
 static int wilc_wlan_rxq_add(struct wilc *wilc, struct rxq_entry_t *rqe)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 
-	if (p->quit)
+	if (wilc->quit)
 		return 0;
 
 	mutex_lock(&wilc->rxq_cs);
-	if (p->rxq_head == NULL) {
+	if (!wilc->rxq_head) {
 		PRINT_D(RX_DBG, "Add to Queue head\n");
 		rqe->next = NULL;
-		p->rxq_head = rqe;
-		p->rxq_tail = rqe;
+		wilc->rxq_head = rqe;
+		wilc->rxq_tail = rqe;
 	} else {
 		PRINT_D(RX_DBG, "Add to Queue tail\n");
-		p->rxq_tail->next = rqe;
+		wilc->rxq_tail->next = rqe;
 		rqe->next = NULL;
-		p->rxq_tail = rqe;
+		wilc->rxq_tail = rqe;
 	}
-	p->rxq_entries += 1;
-	PRINT_D(RX_DBG, "Number of queue entries: %d\n", p->rxq_entries);
+	wilc->rxq_entries += 1;
+	PRINT_D(RX_DBG, "Number of queue entries: %d\n", wilc->rxq_entries);
 	mutex_unlock(&wilc->rxq_cs);
-	return p->rxq_entries;
+	return wilc->rxq_entries;
 }
 
 static struct rxq_entry_t *wilc_wlan_rxq_remove(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 
 	PRINT_D(RX_DBG, "Getting rxQ element\n");
-	if (p->rxq_head) {
+	if (wilc->rxq_head) {
 		struct rxq_entry_t *rqe;
 
 		mutex_lock(&wilc->rxq_cs);
-		rqe = p->rxq_head;
-		p->rxq_head = p->rxq_head->next;
-		p->rxq_entries -= 1;
+		rqe = wilc->rxq_head;
+		wilc->rxq_head = wilc->rxq_head->next;
+		wilc->rxq_entries -= 1;
 		PRINT_D(RX_DBG, "RXQ entries decreased\n");
 		mutex_unlock(&wilc->rxq_cs);
 		return rqe;
@@ -637,197 +524,151 @@ static struct rxq_entry_t *wilc_wlan_rxq_remove(struct wilc *wilc)
 	return NULL;
 }
 
-
-/********************************************
- *
- *      Power Save handle functions
- *
- ********************************************/
-
-
-
 #ifdef WILC_OPTIMIZE_SLEEP_INT
 
-static inline void chip_allow_sleep(void)
+static inline void chip_allow_sleep(struct wilc *wilc)
 {
 	u32 reg = 0;
 
-	/* Clear bit 1 */
-	g_wlan.hif_func.hif_read_reg(0xf0, &reg);
+	wilc->hif_func->hif_read_reg(wilc, 0xf0, &reg);
 
-	g_wlan.hif_func.hif_write_reg(0xf0, reg & ~BIT(0));
+	wilc->hif_func->hif_write_reg(wilc, 0xf0, reg & ~BIT(0));
 }
 
-static inline void chip_wakeup(void)
+static inline void chip_wakeup(struct wilc *wilc)
 {
 	u32 reg, clk_status_reg, trials = 0;
 	u32 sleep_time;
 
-	if ((g_wlan.io_func.io_type & 0x1) == HIF_SPI) {
+	if ((wilc->io_type & 0x1) == HIF_SPI) {
 		do {
-			g_wlan.hif_func.hif_read_reg(1, &reg);
-			/* Set bit 1 */
-			g_wlan.hif_func.hif_write_reg(1, reg | BIT(1));
-
-			/* Clear bit 1*/
-			g_wlan.hif_func.hif_write_reg(1, reg & ~BIT(1));
+			wilc->hif_func->hif_read_reg(wilc, 1, &reg);
+			wilc->hif_func->hif_write_reg(wilc, 1, reg | BIT(1));
+			wilc->hif_func->hif_write_reg(wilc, 1, reg & ~BIT(1));
 
 			do {
-				/* Wait for the chip to stabilize*/
 				usleep_range(2 * 1000, 2 * 1000);
-				/* Make sure chip is awake. This is an extra step that can be removed */
-				/* later to avoid the bus access overhead */
-				if ((wilc_get_chipid(true) == 0))
+				if ((wilc_get_chipid(wilc, true) == 0))
 					wilc_debug(N_ERR, "Couldn't read chip id. Wake up failed\n");
 
-			} while ((wilc_get_chipid(true) == 0) && ((++trials % 3) == 0));
+			} while ((wilc_get_chipid(wilc, true) == 0) && ((++trials % 3) == 0));
 
-		} while (wilc_get_chipid(true) == 0);
-	} else if ((g_wlan.io_func.io_type & 0x1) == HIF_SDIO)	 {
-		g_wlan.hif_func.hif_read_reg(0xf0, &reg);
+		} while (wilc_get_chipid(wilc, true) == 0);
+	} else if ((wilc->io_type & 0x1) == HIF_SDIO)	 {
+		wilc->hif_func->hif_read_reg(wilc, 0xf0, &reg);
 		do {
-			/* Set bit 1 */
-			g_wlan.hif_func.hif_write_reg(0xf0, reg | BIT(0));
+			wilc->hif_func->hif_write_reg(wilc, 0xf0,
+						      reg | BIT(0));
+			wilc->hif_func->hif_read_reg(wilc, 0xf1,
+						     &clk_status_reg);
 
-			/* Check the clock status */
-			g_wlan.hif_func.hif_read_reg(0xf1, &clk_status_reg);
-
-			/* in case of clocks off, wait 2ms, and check it again. */
-			/* if still off, wait for another 2ms, for a total wait of 6ms. */
-			/* If still off, redo the wake up sequence */
 			while (((clk_status_reg & 0x1) == 0) && (((++trials) % 3) == 0)) {
-				/* Wait for the chip to stabilize*/
 				usleep_range(2 * 1000, 2 * 1000);
 
-				/* Make sure chip is awake. This is an extra step that can be removed */
-				/* later to avoid the bus access overhead */
-				g_wlan.hif_func.hif_read_reg(0xf1, &clk_status_reg);
+				wilc->hif_func->hif_read_reg(wilc, 0xf1,
+							     &clk_status_reg);
 
 				if ((clk_status_reg & 0x1) == 0)
 					wilc_debug(N_ERR, "clocks still OFF. Wake up failed\n");
-
 			}
-			/* in case of failure, Reset the wakeup bit to introduce a new edge on the next loop */
 			if ((clk_status_reg & 0x1) == 0) {
-				/* Reset bit 0 */
-				g_wlan.hif_func.hif_write_reg(0xf0, reg &
-							      (~BIT(0)));
+				wilc->hif_func->hif_write_reg(wilc, 0xf0,
+							      reg & (~BIT(0)));
 			}
 		} while ((clk_status_reg & 0x1) == 0);
 	}
 
-
-	if (genuChipPSstate == CHIP_SLEEPING_MANUAL) {
-		g_wlan.hif_func.hif_read_reg(0x1C0C, &reg);
+	if (chip_ps_state == CHIP_SLEEPING_MANUAL) {
+		wilc->hif_func->hif_read_reg(wilc, 0x1C0C, &reg);
 		reg &= ~BIT(0);
-		g_wlan.hif_func.hif_write_reg(0x1C0C, reg);
+		wilc->hif_func->hif_write_reg(wilc, 0x1C0C, reg);
 
-		if (wilc_get_chipid(false) >= 0x1002b0) {
-			/* Enable PALDO back right after wakeup */
+		if (wilc_get_chipid(wilc, false) >= 0x1002b0) {
 			u32 val32;
 
-			g_wlan.hif_func.hif_read_reg(0x1e1c, &val32);
+			wilc->hif_func->hif_read_reg(wilc, 0x1e1c, &val32);
 			val32 |= BIT(6);
-			g_wlan.hif_func.hif_write_reg(0x1e1c, val32);
+			wilc->hif_func->hif_write_reg(wilc, 0x1e1c, val32);
 
-			g_wlan.hif_func.hif_read_reg(0x1e9c, &val32);
+			wilc->hif_func->hif_read_reg(wilc, 0x1e9c, &val32);
 			val32 |= BIT(6);
-			g_wlan.hif_func.hif_write_reg(0x1e9c, val32);
+			wilc->hif_func->hif_write_reg(wilc, 0x1e9c, val32);
 		}
 	}
-	genuChipPSstate = CHIP_WAKEDUP;
+	chip_ps_state = CHIP_WAKEDUP;
 }
 #else
-static inline void chip_wakeup(void)
+static inline void chip_wakeup(struct wilc *wilc)
 {
 	u32 reg, trials = 0;
 
 	do {
-		if ((g_wlan.io_func.io_type & 0x1) == HIF_SPI) {
-			g_wlan.hif_func.hif_read_reg(1, &reg);
-			/* Make sure bit 1 is 0 before we start. */
-			g_wlan.hif_func.hif_write_reg(1, reg & ~BIT(1));
-			/* Set bit 1 */
-			g_wlan.hif_func.hif_write_reg(1, reg | BIT(1));
-			/* Clear bit 1*/
-			g_wlan.hif_func.hif_write_reg(1, reg  & ~BIT(1));
-		} else if ((g_wlan.io_func.io_type & 0x1) == HIF_SDIO)	 {
-			/* Make sure bit 0 is 0 before we start. */
-			g_wlan.hif_func.hif_read_reg(0xf0, &reg);
-			g_wlan.hif_func.hif_write_reg(0xf0, reg & ~BIT(0));
-			/* Set bit 1 */
-			g_wlan.hif_func.hif_write_reg(0xf0, reg | BIT(0));
-			/* Clear bit 1 */
-			g_wlan.hif_func.hif_write_reg(0xf0, reg  & ~BIT(0));
+		if ((wilc->io_type & 0x1) == HIF_SPI) {
+			wilc->hif_func->hif_read_reg(wilc, 1, &reg);
+			wilc->hif_func->hif_write_reg(wilc, 1, reg & ~BIT(1));
+			wilc->hif_func->hif_write_reg(wilc, 1, reg | BIT(1));
+			wilc->hif_func->hif_write_reg(wilc, 1, reg  & ~BIT(1));
+		} else if ((wilc->io_type & 0x1) == HIF_SDIO)	 {
+			wilc->hif_func->hif_read_reg(wilc, 0xf0, &reg);
+			wilc->hif_func->hif_write_reg(wilc, 0xf0,
+						      reg & ~BIT(0));
+			wilc->hif_func->hif_write_reg(wilc, 0xf0,
+						      reg | BIT(0));
+			wilc->hif_func->hif_write_reg(wilc, 0xf0,
+						      reg  & ~BIT(0));
 		}
 
 		do {
-			/* Wait for the chip to stabilize*/
 			mdelay(3);
 
-			/* Make sure chip is awake. This is an extra step that can be removed */
-			/* later to avoid the bus access overhead */
-			if ((wilc_get_chipid(true) == 0))
+			if ((wilc_get_chipid(wilc, true) == 0))
 				wilc_debug(N_ERR, "Couldn't read chip id. Wake up failed\n");
 
-		} while ((wilc_get_chipid(true) == 0) && ((++trials % 3) == 0));
+		} while ((wilc_get_chipid(wilc, true) == 0) && ((++trials % 3) == 0));
 
-	} while (wilc_get_chipid(true) == 0);
+	} while (wilc_get_chipid(wilc, true) == 0);
 
-	if (genuChipPSstate == CHIP_SLEEPING_MANUAL) {
-		g_wlan.hif_func.hif_read_reg(0x1C0C, &reg);
+	if (chip_ps_state == CHIP_SLEEPING_MANUAL) {
+		wilc->hif_func->hif_read_reg(wilc, 0x1C0C, &reg);
 		reg &= ~BIT(0);
-		g_wlan.hif_func.hif_write_reg(0x1C0C, reg);
+		wilc->hif_func->hif_write_reg(wilc, 0x1C0C, reg);
 
-		if (wilc_get_chipid(false) >= 0x1002b0) {
-			/* Enable PALDO back right after wakeup */
+		if (wilc_get_chipid(wilc, false) >= 0x1002b0) {
 			u32 val32;
 
-			g_wlan.hif_func.hif_read_reg(0x1e1c, &val32);
+			wilc->hif_func->hif_read_reg(wilc, 0x1e1c, &val32);
 			val32 |= BIT(6);
-			g_wlan.hif_func.hif_write_reg(0x1e1c, val32);
+			wilc->hif_func->hif_write_reg(wilc, 0x1e1c, val32);
 
-			g_wlan.hif_func.hif_read_reg(0x1e9c, &val32);
+			wilc->hif_func->hif_read_reg(wilc, 0x1e9c, &val32);
 			val32 |= BIT(6);
-			g_wlan.hif_func.hif_write_reg(0x1e9c, val32);
+			wilc->hif_func->hif_write_reg(wilc, 0x1e9c, val32);
 		}
 	}
-	genuChipPSstate = CHIP_WAKEDUP;
+	chip_ps_state = CHIP_WAKEDUP;
 }
 #endif
-void chip_sleep_manually(u32 u32SleepTime)
+void wilc_chip_sleep_manually(struct wilc *wilc)
 {
-	if (genuChipPSstate != CHIP_WAKEDUP) {
-		/* chip is already sleeping. Do nothing */
+	if (chip_ps_state != CHIP_WAKEDUP)
 		return;
-	}
-	acquire_bus(ACQUIRE_ONLY);
+	acquire_bus(wilc, ACQUIRE_ONLY);
 
 #ifdef WILC_OPTIMIZE_SLEEP_INT
-	chip_allow_sleep();
+	chip_allow_sleep(wilc);
 #endif
+	wilc->hif_func->hif_write_reg(wilc, 0x10a8, 1);
 
-	/* Trigger the manual sleep interrupt */
-	g_wlan.hif_func.hif_write_reg(0x10a8, 1);
-
-	genuChipPSstate = CHIP_SLEEPING_MANUAL;
-	release_bus(RELEASE_ONLY);
-
+	chip_ps_state = CHIP_SLEEPING_MANUAL;
+	release_bus(wilc, RELEASE_ONLY);
 }
 
-
-/********************************************
- *
- *      Tx, Rx queue handle functions
- *
- ********************************************/
-int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
+int wilc_wlan_handle_txq(struct net_device *dev, u32 *txq_count)
 {
-	wilc_wlan_dev_t *p = (wilc_wlan_dev_t *)&g_wlan;
 	int i, entries = 0;
 	u32 sum;
 	u32 reg;
-	u8 *txb = p->tx_buffer;
+	u8 *txb;
 	u32 offset = 0;
 	int vmm_sz = 0;
 	struct txq_entry_t *tqe;
@@ -835,32 +676,29 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 	int counter;
 	int timeout;
 	u32 vmm_table[WILC_VMM_TBL_SIZE];
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	p->txq_exit = 0;
+	txb = wilc->tx_buffer;
+	wilc->txq_exit = 0;
 	do {
-		if (p->quit)
+		if (wilc->quit)
 			break;
 
-		linux_wlan_lock_timeout(&wilc->txq_add_to_head_cs,
+		wilc_lock_timeout(wilc, &wilc->txq_add_to_head_cs,
 					CFG_PKTS_TIMEOUT);
 #ifdef	TCP_ACK_FILTER
 		wilc_wlan_txq_filter_dup_tcp_ack(dev);
 #endif
-		/**
-		 *      build the vmm list
-		 **/
 		PRINT_D(TX_DBG, "Getting the head of the TxQ\n");
-		tqe = wilc_wlan_txq_get_first();
+		tqe = wilc_wlan_txq_get_first(wilc);
 		i = 0;
 		sum = 0;
 		do {
-			if ((tqe != NULL) && (i < (WILC_VMM_TBL_SIZE - 1)) /* reserve last entry to 0 */) {
-
+			if (tqe && (i < (WILC_VMM_TBL_SIZE - 1))) {
 				if (tqe->type == WILC_CFG_PKT)
 					vmm_sz = ETH_CONFIG_PKT_HDR_OFFSET;
 
@@ -872,23 +710,22 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 
 				vmm_sz += tqe->buffer_size;
 				PRINT_D(TX_DBG, "VMM Size before alignment = %d\n", vmm_sz);
-				if (vmm_sz & 0x3) {                                                                                                     /* has to be word aligned */
+				if (vmm_sz & 0x3)
 					vmm_sz = (vmm_sz + 4) & ~0x3;
-				}
+
 				if ((sum + vmm_sz) > LINUX_TX_SIZE)
 					break;
 
 				PRINT_D(TX_DBG, "VMM Size AFTER alignment = %d\n", vmm_sz);
-				vmm_table[i] = vmm_sz / 4;                                                                                /* table take the word size */
-				PRINT_D(TX_DBG, "VMMTable entry size = %d\n", vmm_table[i]);
+				vmm_table[i] = vmm_sz / 4;
+				PRINT_D(TX_DBG, "VMMTable entry size = %d\n",
+					vmm_table[i]);
 
 				if (tqe->type == WILC_CFG_PKT) {
 					vmm_table[i] |= BIT(10);
 					PRINT_D(TX_DBG, "VMMTable entry changed for CFG packet = %d\n", vmm_table[i]);
 				}
-#ifdef BIG_ENDIAN
-				vmm_table[i] = BYTE_SWAP(vmm_table[i]);
-#endif
+				vmm_table[i] = cpu_to_le32(vmm_table[i]);
 
 				i++;
 				sum += vmm_sz;
@@ -899,27 +736,24 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 			}
 		} while (1);
 
-		if (i == 0) {           /* nothing in the queue */
+		if (i == 0) {
 			PRINT_D(TX_DBG, "Nothing in TX-Q\n");
 			break;
 		} else {
 			PRINT_D(TX_DBG, "Mark the last entry in VMM table - number of previous entries = %d\n", i);
-			vmm_table[i] = 0x0;     /* mark the last element to 0 */
+			vmm_table[i] = 0x0;
 		}
-		acquire_bus(ACQUIRE_AND_WAKEUP);
+		acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 		counter = 0;
 		do {
-
-			ret = p->hif_func.hif_read_reg(WILC_HOST_TX_CTRL, &reg);
+			ret = wilc->hif_func->hif_read_reg(wilc, WILC_HOST_TX_CTRL,
+						       &reg);
 			if (!ret) {
 				wilc_debug(N_ERR, "[wilc txq]: fail can't read reg vmm_tbl_entry..\n");
 				break;
 			}
 
 			if ((reg & 0x1) == 0) {
-				/**
-				 *      write to vmm table
-				 **/
 				PRINT_D(TX_DBG, "Writing VMM table ... with Size = %d\n", ((i + 1) * 4));
 				break;
 			} else {
@@ -927,69 +761,52 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 				if (counter > 200) {
 					counter = 0;
 					PRINT_D(TX_DBG, "Looping in tx ctrl , forcce quit\n");
-					ret = p->hif_func.hif_write_reg(WILC_HOST_TX_CTRL, 0);
+					ret = wilc->hif_func->hif_write_reg(wilc, WILC_HOST_TX_CTRL, 0);
 					break;
 				}
-				/**
-				 *      wait for vmm table is ready
-				 **/
 				PRINT_WRN(GENERIC_DBG, "[wilc txq]: warn, vmm table not clear yet, wait...\n");
-				release_bus(RELEASE_ALLOW_SLEEP);
+				release_bus(wilc, RELEASE_ALLOW_SLEEP);
 				usleep_range(3000, 3000);
-				acquire_bus(ACQUIRE_AND_WAKEUP);
+				acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 			}
-		} while (!p->quit);
+		} while (!wilc->quit);
 
 		if (!ret)
 			goto _end_;
 
 		timeout = 200;
 		do {
-
-			/**
-			 * write to vmm table
-			 **/
-			ret = p->hif_func.hif_block_tx(WILC_VMM_TBL_RX_SHADOW_BASE, (u8 *)vmm_table, ((i + 1) * 4));
+			ret = wilc->hif_func->hif_block_tx(wilc, WILC_VMM_TBL_RX_SHADOW_BASE, (u8 *)vmm_table, ((i + 1) * 4));
 			if (!ret) {
 				wilc_debug(N_ERR, "ERR block TX of VMM table.\n");
 				break;
 			}
 
-
-			/**
-			 * interrupt firmware
-			 **/
-			ret = p->hif_func.hif_write_reg(WILC_HOST_VMM_CTL, 0x2);
+			ret = wilc->hif_func->hif_write_reg(wilc, WILC_HOST_VMM_CTL,
+							0x2);
 			if (!ret) {
 				wilc_debug(N_ERR, "[wilc txq]: fail can't write reg host_vmm_ctl..\n");
 				break;
 			}
 
-			/**
-			 *      wait for confirm...
-			 **/
-
 			do {
-				ret = p->hif_func.hif_read_reg(WILC_HOST_VMM_CTL, &reg);
+				ret = wilc->hif_func->hif_read_reg(wilc, WILC_HOST_VMM_CTL, &reg);
 				if (!ret) {
 					wilc_debug(N_ERR, "[wilc txq]: fail can't read reg host_vmm_ctl..\n");
 					break;
 				}
 				if ((reg >> 2) & 0x1) {
-					/**
-					 *      Get the entries
-					 **/
 					entries = ((reg >> 3) & 0x3f);
 					break;
 				} else {
-					release_bus(RELEASE_ALLOW_SLEEP);
+					release_bus(wilc, RELEASE_ALLOW_SLEEP);
 					usleep_range(3000, 3000);
-					acquire_bus(ACQUIRE_AND_WAKEUP);
+					acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 					PRINT_WRN(GENERIC_DBG, "Can't get VMM entery - reg = %2x\n", reg);
 				}
 			} while (--timeout);
 			if (timeout <= 0) {
-				ret = p->hif_func.hif_write_reg(WILC_HOST_VMM_CTL, 0x0);
+				ret = wilc->hif_func->hif_write_reg(wilc, WILC_HOST_VMM_CTL, 0x0);
 				break;
 			}
 
@@ -999,14 +816,13 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 			if (entries == 0) {
 				PRINT_WRN(GENERIC_DBG, "[wilc txq]: no more buffer in the chip (reg: %08x), retry later [[ %d, %x ]]\n", reg, i, vmm_table[i - 1]);
 
-				/* undo the transaction. */
-				ret = p->hif_func.hif_read_reg(WILC_HOST_TX_CTRL, &reg);
+				ret = wilc->hif_func->hif_read_reg(wilc, WILC_HOST_TX_CTRL, &reg);
 				if (!ret) {
 					wilc_debug(N_ERR, "[wilc txq]: fail can't read reg WILC_HOST_TX_CTRL..\n");
 					break;
 				}
 				reg &= ~BIT(0);
-				ret = p->hif_func.hif_write_reg(WILC_HOST_TX_CTRL, reg);
+				ret = wilc->hif_func->hif_write_reg(wilc, WILC_HOST_TX_CTRL, reg);
 				if (!ret) {
 					wilc_debug(N_ERR, "[wilc txq]: fail can't write reg WILC_HOST_TX_CTRL..\n");
 					break;
@@ -1025,58 +841,50 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 			goto _end_;
 		}
 
-		/* since copying data into txb takes some time, then
-		 * allow the bus lock to be released let the RX task go. */
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 
-		/**
-		 *      Copy data to the TX buffer
-		 **/
 		offset = 0;
 		i = 0;
 		do {
-			tqe = wilc_wlan_txq_remove_from_head();
-			if (tqe != NULL && (vmm_table[i] != 0)) {
+			tqe = wilc_wlan_txq_remove_from_head(dev);
+			if (tqe && (vmm_table[i] != 0)) {
 				u32 header, buffer_offset;
 
-#ifdef BIG_ENDIAN
-				vmm_table[i] = BYTE_SWAP(vmm_table[i]);
-#endif
-				vmm_sz = (vmm_table[i] & 0x3ff);        /* in word unit */
+				vmm_table[i] = cpu_to_le32(vmm_table[i]);
+				vmm_sz = (vmm_table[i] & 0x3ff);
 				vmm_sz *= 4;
-				header = (tqe->type << 31) | (tqe->buffer_size << 15) | vmm_sz;
+				header = (tqe->type << 31) |
+					 (tqe->buffer_size << 15) |
+					 vmm_sz;
 				if (tqe->type == WILC_MGMT_PKT)
 					header |= BIT(30);
 				else
 					header &= ~BIT(30);
 
-#ifdef BIG_ENDIAN
-				header = BYTE_SWAP(header);
-#endif
+				header = cpu_to_le32(header);
 				memcpy(&txb[offset], &header, 4);
 				if (tqe->type == WILC_CFG_PKT) {
 					buffer_offset = ETH_CONFIG_PKT_HDR_OFFSET;
-				}
-				else if (tqe->type == WILC_NET_PKT) {
-					char *pBSSID = ((struct tx_complete_data *)(tqe->priv))->pBssid;
+				} else if (tqe->type == WILC_NET_PKT) {
+					char *bssid = ((struct tx_complete_data *)(tqe->priv))->pBssid;
 
 					buffer_offset = ETH_ETHERNET_HDR_OFFSET;
-					/* copy the bssid at the sart of the buffer */
-					memcpy(&txb[offset + 4], pBSSID, 6);
-				}
-				else {
+					memcpy(&txb[offset + 4], bssid, 6);
+				} else {
 					buffer_offset = HOST_HDR_OFFSET;
 				}
 
-				memcpy(&txb[offset + buffer_offset], tqe->buffer, tqe->buffer_size);
+				memcpy(&txb[offset + buffer_offset],
+				       tqe->buffer, tqe->buffer_size);
 				offset += vmm_sz;
 				i++;
-				tqe->status = 1;                                /* mark the packet send */
+				tqe->status = 1;
 				if (tqe->tx_complete_func)
-					tqe->tx_complete_func(tqe->priv, tqe->status);
+					tqe->tx_complete_func(tqe->priv,
+							      tqe->status);
 				#ifdef TCP_ACK_FILTER
-				if (tqe->tcp_PendingAck_index != NOT_TCP_ACK)
-					Pending_Acks_info[tqe->tcp_PendingAck_index].txqe = NULL;
+				if (tqe->tcp_pending_ack_idx != NOT_TCP_ACK)
+					pending_acks_info[tqe->tcp_pending_ack_idx].txqe = NULL;
 				#endif
 				kfree(tqe);
 			} else {
@@ -1084,21 +892,15 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 			}
 		} while (--entries);
 
-		/**
-		 *      lock the bus
-		 **/
-		acquire_bus(ACQUIRE_AND_WAKEUP);
+		acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 
-		ret = p->hif_func.hif_clear_int_ext(ENABLE_TX_VMM);
+		ret = wilc->hif_func->hif_clear_int_ext(wilc, ENABLE_TX_VMM);
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc txq]: fail can't start tx VMM ...\n");
 			goto _end_;
 		}
 
-		/**
-		 *      transfer
-		 **/
-		ret = p->hif_func.hif_block_tx_ext(0, txb, offset);
+		ret = wilc->hif_func->hif_block_tx_ext(wilc, 0, txb, offset);
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc txq]: fail can't block tx ext...\n");
 			goto _end_;
@@ -1106,49 +908,43 @@ int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount)
 
 _end_:
 
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 		if (ret != 1)
 			break;
 	} while (0);
 	up(&wilc->txq_add_to_head_cs);
 
-	p->txq_exit = 1;
+	wilc->txq_exit = 1;
 	PRINT_D(TX_DBG, "THREAD: Exiting txq\n");
-	/* return tx[]q count */
-	*pu32TxqCount = p->txq_entries;
+	*txq_count = wilc->txq_entries;
 	return ret;
 }
 
 static void wilc_wlan_handle_rxq(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	int offset = 0, size, has_packet = 0;
 	u8 *buffer;
 	struct rxq_entry_t *rqe;
 
-	p->rxq_exit = 0;
-
-
-
+	wilc->rxq_exit = 0;
 
 	do {
-		if (p->quit) {
+		if (wilc->quit) {
 			PRINT_D(RX_DBG, "exit 1st do-while due to Clean_UP function\n");
 			up(&wilc->cfg_event);
 			break;
 		}
 		rqe = wilc_wlan_rxq_remove(wilc);
-		if (rqe == NULL) {
+		if (!rqe) {
 			PRINT_D(RX_DBG, "nothing in the queue - exit 1st do-while\n");
 			break;
 		}
 		buffer = rqe->buffer;
 		size = rqe->buffer_size;
-		PRINT_D(RX_DBG, "rxQ entery Size = %d - Address = %p\n", size, buffer);
+		PRINT_D(RX_DBG, "rxQ entery Size = %d - Address = %p\n",
+			size, buffer);
 		offset = 0;
 
-
-
 		do {
 			u32 header;
 			u32 pkt_len, pkt_offset, tp_len;
@@ -1156,12 +952,9 @@ static void wilc_wlan_handle_rxq(struct wilc *wilc)
 
 			PRINT_D(RX_DBG, "In the 2nd do-while\n");
 			memcpy(&header, &buffer[offset], 4);
-#ifdef BIG_ENDIAN
-			header = BYTE_SWAP(header);
-#endif
-			PRINT_D(RX_DBG, "Header = %04x - Offset = %d\n", header, offset);
-
-
+			header = cpu_to_le32(header);
+			PRINT_D(RX_DBG, "Header = %04x - Offset = %d\n",
+				header, offset);
 
 			is_cfg_packet = (header >> 31) & 0x1;
 			pkt_offset = (header >> 22) & 0x1ff;
@@ -1177,45 +970,34 @@ static void wilc_wlan_handle_rxq(struct wilc *wilc)
 			#define IS_MANAGMEMENT_CALLBACK			0x080
 			#define IS_MGMT_STATUS_SUCCES			0x040
 
-
 			if (pkt_offset & IS_MANAGMEMENT) {
-				/* reset mgmt indicator bit, to use pkt_offeset in furthur calculations */
-				pkt_offset &= ~(IS_MANAGMEMENT | IS_MANAGMEMENT_CALLBACK | IS_MGMT_STATUS_SUCCES);
+				pkt_offset &= ~(IS_MANAGMEMENT |
+						IS_MANAGMEMENT_CALLBACK |
+						IS_MGMT_STATUS_SUCCES);
 
 				WILC_WFI_mgmt_rx(wilc, &buffer[offset + HOST_HDR_OFFSET], pkt_len);
-			}
-			else
-			{
-
+			} else {
 				if (!is_cfg_packet) {
 					if (pkt_len > 0) {
-						frmw_to_linux(wilc,
+						wilc_frmw_to_linux(wilc,
 							      &buffer[offset],
 							      pkt_len,
 							      pkt_offset);
 						has_packet = 1;
 					}
 				} else {
-					wilc_cfg_rsp_t rsp;
-
-
+					struct wilc_cfg_rsp rsp;
 
-					wilc_wlan_cfg_indicate_rx(&buffer[pkt_offset + offset], pkt_len, &rsp);
+					wilc_wlan_cfg_indicate_rx(wilc, &buffer[pkt_offset + offset], pkt_len, &rsp);
 					if (rsp.type == WILC_CFG_RSP) {
-						/**
-						 *      wake up the waiting task...
-						 **/
-						PRINT_D(RX_DBG, "p->cfg_seq_no = %d - rsp.seq_no = %d\n", p->cfg_seq_no, rsp.seq_no);
-						if (p->cfg_seq_no == rsp.seq_no)
+						PRINT_D(RX_DBG, "wilc->cfg_seq_no = %d - rsp.seq_no = %d\n", wilc->cfg_seq_no, rsp.seq_no);
+						if (wilc->cfg_seq_no == rsp.seq_no)
 							up(&wilc->cfg_event);
 					} else if (rsp.type == WILC_CFG_RSP_STATUS) {
-						/**
-						 *      Call back to indicate status...
-						 **/
-						linux_wlan_mac_indicate(wilc, WILC_MAC_INDICATE_STATUS);
+						wilc_mac_indicate(wilc, WILC_MAC_INDICATE_STATUS);
 
 					} else if (rsp.type == WILC_CFG_RSP_SCAN) {
-						linux_wlan_mac_indicate(wilc, WILC_MAC_INDICATE_SCAN);
+						wilc_mac_indicate(wilc, WILC_MAC_INDICATE_SCAN);
 					}
 				}
 			}
@@ -1223,224 +1005,163 @@ static void wilc_wlan_handle_rxq(struct wilc *wilc)
 			if (offset >= size)
 				break;
 		} while (1);
-
-
-#ifndef MEMORY_STATIC
-		kfree(buffer);
-#endif
 		kfree(rqe);
 
 		if (has_packet)
-			linux_wlan_rx_complete();
+			wilc_rx_complete(wilc);
 
 	} while (1);
 
-	p->rxq_exit = 1;
+	wilc->rxq_exit = 1;
 	PRINT_D(RX_DBG, "THREAD: Exiting RX thread\n");
 }
 
-/********************************************
- *
- *      Fast DMA Isr
- *
- ********************************************/
-static void wilc_unknown_isr_ext(void)
+static void wilc_unknown_isr_ext(struct wilc *wilc)
 {
-	g_wlan.hif_func.hif_clear_int_ext(0);
+	wilc->hif_func->hif_clear_int_ext(wilc, 0);
 }
-static void wilc_pllupdate_isr_ext(u32 int_stats)
-{
 
+static void wilc_pllupdate_isr_ext(struct wilc *wilc, u32 int_stats)
+{
 	int trials = 10;
 
-	g_wlan.hif_func.hif_clear_int_ext(PLL_INT_CLR);
+	wilc->hif_func->hif_clear_int_ext(wilc, PLL_INT_CLR);
 
-	/* Waiting for PLL */
-	mdelay(WILC_PLL_TO);
+	if (wilc->io_type == HIF_SDIO)
+		mdelay(WILC_PLL_TO_SDIO);
+	else
+		mdelay(WILC_PLL_TO_SPI);
 
-	/* poll till read a valid data */
-	while (!(ISWILC1000(wilc_get_chipid(true)) && --trials)) {
+	while (!(ISWILC1000(wilc_get_chipid(wilc, true)) && --trials)) {
 		PRINT_D(TX_DBG, "PLL update retrying\n");
 		mdelay(1);
 	}
 }
 
-static void wilc_sleeptimer_isr_ext(u32 int_stats1)
+static void wilc_sleeptimer_isr_ext(struct wilc *wilc, u32 int_stats1)
 {
-	g_wlan.hif_func.hif_clear_int_ext(SLEEP_INT_CLR);
+	wilc->hif_func->hif_clear_int_ext(wilc, SLEEP_INT_CLR);
 #ifndef WILC_OPTIMIZE_SLEEP_INT
-	genuChipPSstate = CHIP_SLEEPING_AUTO;
+	chip_ps_state = CHIP_SLEEPING_AUTO;
 #endif
 }
 
 static void wilc_wlan_handle_isr_ext(struct wilc *wilc, u32 int_status)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
-#ifdef MEMORY_STATIC
-	u32 offset = p->rx_buffer_offset;
-#endif
+	u32 offset = wilc->rx_buffer_offset;
 	u8 *buffer = NULL;
 	u32 size;
 	u32 retries = 0;
 	int ret = 0;
 	struct rxq_entry_t *rqe;
 
-
-	/**
-	 *      Get the rx size
-	 **/
-
 	size = ((int_status & 0x7fff) << 2);
 
 	while (!size && retries < 10) {
 		u32 time = 0;
-		/*looping more secure*/
-		/*zero size make a crashe because the dma will not happen and that will block the firmware*/
+
 		wilc_debug(N_ERR, "RX Size equal zero ... Trying to read it again for %d time\n", time++);
-		p->hif_func.hif_read_size(&size);
+		wilc->hif_func->hif_read_size(wilc, &size);
 		size = ((size & 0x7fff) << 2);
 		retries++;
-
 	}
 
 	if (size > 0) {
-#ifdef MEMORY_STATIC
 		if (LINUX_RX_SIZE - offset < size)
 			offset = 0;
 
-		if (p->rx_buffer)
-			buffer = &p->rx_buffer[offset];
-		else {
+		if (wilc->rx_buffer) {
+			buffer = &wilc->rx_buffer[offset];
+		} else {
 			wilc_debug(N_ERR, "[wilc isr]: fail Rx Buffer is NULL...drop the packets (%d)\n", size);
 			goto _end_;
 		}
 
-#else
-		buffer = kmalloc(size, GFP_KERNEL);
-		if (buffer == NULL) {
-			wilc_debug(N_ERR, "[wilc isr]: fail alloc host memory...drop the packets (%d)\n", size);
-			usleep_range(100 * 1000, 100 * 1000);
-			goto _end_;
-		}
-#endif
-
-		/**
-		 *      clear the chip's interrupt	 after getting size some register getting corrupted after clear the interrupt
-		 **/
-		p->hif_func.hif_clear_int_ext(DATA_INT_CLR | ENABLE_RX_VMM);
-
-
-		/**
-		 * start transfer
-		 **/
-		ret = p->hif_func.hif_block_rx_ext(0, buffer, size);
+		wilc->hif_func->hif_clear_int_ext(wilc,
+					      DATA_INT_CLR | ENABLE_RX_VMM);
+		ret = wilc->hif_func->hif_block_rx_ext(wilc, 0, buffer, size);
 
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc isr]: fail block rx...\n");
 			goto _end_;
 		}
 _end_:
-
-
 		if (ret) {
-#ifdef MEMORY_STATIC
 			offset += size;
-			p->rx_buffer_offset = offset;
-#endif
-			/**
-			 *      add to rx queue
-			 **/
-			rqe = kmalloc(sizeof(struct rxq_entry_t), GFP_KERNEL);
-			if (rqe != NULL) {
+			wilc->rx_buffer_offset = offset;
+			rqe = kmalloc(sizeof(*rqe), GFP_KERNEL);
+			if (rqe) {
 				rqe->buffer = buffer;
 				rqe->buffer_size = size;
 				PRINT_D(RX_DBG, "rxq entery Size= %d - Address = %p\n", rqe->buffer_size, rqe->buffer);
 				wilc_wlan_rxq_add(wilc, rqe);
 			}
-		} else {
-#ifndef MEMORY_STATIC
-			kfree(buffer);
-#endif
 		}
 	}
 	wilc_wlan_handle_rxq(wilc);
 }
 
-void wilc_handle_isr(void *wilc)
+void wilc_handle_isr(struct wilc *wilc)
 {
 	u32 int_status;
 
-	acquire_bus(ACQUIRE_AND_WAKEUP);
-	g_wlan.hif_func.hif_read_int(&int_status);
+	acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
+	wilc->hif_func->hif_read_int(wilc, &int_status);
 
 	if (int_status & PLL_INT_EXT)
-		wilc_pllupdate_isr_ext(int_status);
+		wilc_pllupdate_isr_ext(wilc, int_status);
 
 	if (int_status & DATA_INT_EXT) {
 		wilc_wlan_handle_isr_ext(wilc, int_status);
 	#ifndef WILC_OPTIMIZE_SLEEP_INT
-		/* Chip is up and talking*/
-		genuChipPSstate = CHIP_WAKEDUP;
+		chip_ps_state = CHIP_WAKEDUP;
 	#endif
 	}
 	if (int_status & SLEEP_INT_EXT)
-		wilc_sleeptimer_isr_ext(int_status);
+		wilc_sleeptimer_isr_ext(wilc, int_status);
 
 	if (!(int_status & (ALL_INT_EXT))) {
-#ifdef WILC_SDIO
-		PRINT_D(TX_DBG, ">> UNKNOWN_INTERRUPT - 0x%08x\n", int_status);
-#endif
-		wilc_unknown_isr_ext();
+		wilc_unknown_isr_ext(wilc);
 	}
-	release_bus(RELEASE_ALLOW_SLEEP);
+	release_bus(wilc, RELEASE_ALLOW_SLEEP);
 }
+EXPORT_SYMBOL_GPL(wilc_handle_isr);
 
-/********************************************
- *
- *      Firmware download
- *
- ********************************************/
-int wilc_wlan_firmware_download(const u8 *buffer, u32 buffer_size)
+int wilc_wlan_firmware_download(struct wilc *wilc, const u8 *buffer, u32 buffer_size)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	u32 offset;
 	u32 addr, size, size2, blksz;
 	u8 *dma_buffer;
 	int ret = 0;
 
 	blksz = BIT(12);
-	/* Allocate a DMA coherent  buffer. */
 
 	dma_buffer = kmalloc(blksz, GFP_KERNEL);
-	if (dma_buffer == NULL) {
-		/*EIO	5*/
-		ret = -5;
+	if (!dma_buffer) {
+		ret = -EIO;
 		PRINT_ER("Can't allocate buffer for firmware download IO error\n ");
 		goto _fail_1;
 	}
 
 	PRINT_D(INIT_DBG, "Downloading firmware size = %d ...\n", buffer_size);
-	/**
-	 *      load the firmware
-	 **/
+
 	offset = 0;
 	do {
 		memcpy(&addr, &buffer[offset], 4);
 		memcpy(&size, &buffer[offset + 4], 4);
-#ifdef BIG_ENDIAN
-		addr = BYTE_SWAP(addr);
-		size = BYTE_SWAP(size);
-#endif
-		acquire_bus(ACQUIRE_ONLY);
+		addr = cpu_to_le32(addr);
+		size = cpu_to_le32(size);
+		acquire_bus(wilc, ACQUIRE_ONLY);
 		offset += 8;
 		while (((int)size) && (offset < buffer_size)) {
 			if (size <= blksz)
 				size2 = size;
 			else
 				size2 = blksz;
-			/* Copy firmware into a DMA coherent buffer */
+
 			memcpy(dma_buffer, &buffer[offset], size2);
-			ret = p->hif_func.hif_block_tx(addr, dma_buffer, size2);
+			ret = wilc->hif_func->hif_block_tx(wilc, addr, dma_buffer,
+						       size2);
 			if (!ret)
 				break;
 
@@ -1448,11 +1169,10 @@ int wilc_wlan_firmware_download(const u8 *buffer, u32 buffer_size)
 			offset += size2;
 			size -= size2;
 		}
-		release_bus(RELEASE_ONLY);
+		release_bus(wilc, RELEASE_ONLY);
 
 		if (!ret) {
-			/*EIO	5*/
-			ret = -5;
+			ret = -EIO;
 			PRINT_ER("Can't download firmware IO error\n ");
 			goto _fail_;
 		}
@@ -1468,40 +1188,29 @@ _fail_1:
 	return (ret < 0) ? ret : 0;
 }
 
-/********************************************
- *
- *      Common
- *
- ********************************************/
-int wilc_wlan_start(void)
+int wilc_wlan_start(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	u32 reg = 0;
 	int ret;
 	u32 chipid;
 
-	/**
-	 *      Set the host interface
-	 **/
-	if (p->io_func.io_type == HIF_SDIO) {
+	if (wilc->io_type == HIF_SDIO) {
 		reg = 0;
-		reg |= BIT(3); /* bug 4456 and 4557 */
-	} else if (p->io_func.io_type == HIF_SPI) {
+		reg |= BIT(3);
+	} else if (wilc->io_type == HIF_SPI) {
 		reg = 1;
 	}
-	acquire_bus(ACQUIRE_ONLY);
-	ret = p->hif_func.hif_write_reg(WILC_VMM_CORE_CFG, reg);
+	acquire_bus(wilc, ACQUIRE_ONLY);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_VMM_CORE_CFG, reg);
 	if (!ret) {
 		wilc_debug(N_ERR, "[wilc start]: fail write reg vmm_core_cfg...\n");
-		release_bus(RELEASE_ONLY);
-		/* EIO  5*/
-		ret = -5;
+		release_bus(wilc, RELEASE_ONLY);
+		ret = -EIO;
 		return ret;
 	}
 	reg = 0;
-#ifdef WILC_SDIO_IRQ_GPIO
-	reg |= WILC_HAVE_SDIO_IRQ_GPIO;
-#endif
+	if (wilc->io_type == HIF_SDIO && wilc->dev_irq_num)
+		reg |= WILC_HAVE_SDIO_IRQ_GPIO;
 
 #ifdef WILC_DISABLE_PMU
 #else
@@ -1519,123 +1228,103 @@ int wilc_wlan_start(void)
 #endif
 
 	reg |= WILC_HAVE_LEGACY_RF_SETTINGS;
-
-
-/*Set oscillator frequency*/
 #ifdef XTAL_24
 	reg |= WILC_HAVE_XTAL_24;
 #endif
-
-/*Enable/Disable GPIO configuration for FW logs*/
 #ifdef DISABLE_WILC_UART
 	reg |= WILC_HAVE_DISABLE_WILC_UART;
 #endif
 
-	ret = p->hif_func.hif_write_reg(WILC_GP_REG_1, reg);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_GP_REG_1, reg);
 	if (!ret) {
 		wilc_debug(N_ERR, "[wilc start]: fail write WILC_GP_REG_1 ...\n");
-		release_bus(RELEASE_ONLY);
-		/* EIO  5*/
-		ret = -5;
+		release_bus(wilc, RELEASE_ONLY);
+		ret = -EIO;
 		return ret;
 	}
 
-	/**
-	 *      Bus related
-	 **/
-	p->hif_func.hif_sync_ext(NUM_INT_EXT);
+	wilc->hif_func->hif_sync_ext(wilc, NUM_INT_EXT);
 
-	ret = p->hif_func.hif_read_reg(0x1000, &chipid);
+	ret = wilc->hif_func->hif_read_reg(wilc, 0x1000, &chipid);
 	if (!ret) {
 		wilc_debug(N_ERR, "[wilc start]: fail read reg 0x1000 ...\n");
-		release_bus(RELEASE_ONLY);
-		/* EIO  5*/
-		ret = -5;
+		release_bus(wilc, RELEASE_ONLY);
+		ret = -EIO;
 		return ret;
 	}
 
-	/**
-	 *      Go...
-	 **/
-
-
-	p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0, &reg);
 	if ((reg & BIT(10)) == BIT(10)) {
 		reg &= ~BIT(10);
-		p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
-		p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+		wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, reg);
+		wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0, &reg);
 	}
 
 	reg |= BIT(10);
-	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
-	p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
-	release_bus(RELEASE_ONLY);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, reg);
+	wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0, &reg);
+	release_bus(wilc, RELEASE_ONLY);
 
 	return (ret < 0) ? ret : 0;
 }
 
-void wilc_wlan_global_reset(void)
+void wilc_wlan_global_reset(struct wilc *wilc)
 {
-
-	wilc_wlan_dev_t *p = &g_wlan;
-
-	acquire_bus(ACQUIRE_AND_WAKEUP);
-	p->hif_func.hif_write_reg(WILC_GLB_RESET_0, 0x0);
-	release_bus(RELEASE_ONLY);
+	acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
+	wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, 0x0);
+	release_bus(wilc, RELEASE_ONLY);
 }
-int wilc_wlan_stop(void)
+int wilc_wlan_stop(struct wilc *wilc)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	u32 reg = 0;
 	int ret;
 	u8 timeout = 10;
-	/**
-	 *      TODO: stop the firmware, need a re-download
-	 **/
-	acquire_bus(ACQUIRE_AND_WAKEUP);
+	acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 
-	ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+	ret = wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0, &reg);
 	if (!ret) {
 		PRINT_ER("Error while reading reg\n");
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 		return ret;
 	}
 
 	reg &= ~BIT(10);
-
-
-	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, reg);
 	if (!ret) {
 		PRINT_ER("Error while writing reg\n");
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 		return ret;
 	}
 
-
-
 	do {
-		ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+		ret = wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0, &reg);
 		if (!ret) {
 			PRINT_ER("Error while reading reg\n");
-			release_bus(RELEASE_ALLOW_SLEEP);
+			release_bus(wilc, RELEASE_ALLOW_SLEEP);
 			return ret;
 		}
-		PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n", reg, timeout);
-		/*Workaround to ensure that the chip is actually reset*/
+		PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n",
+			reg, timeout);
+
 		if ((reg & BIT(10))) {
-			PRINT_D(GENERIC_DBG, "Bit 10 not reset : Retry %d\n", timeout);
+			PRINT_D(GENERIC_DBG, "Bit 10 not reset : Retry %d\n",
+				timeout);
 			reg &= ~BIT(10);
-			ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+			ret = wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0,
+							reg);
 			timeout--;
 		} else {
-			PRINT_D(GENERIC_DBG, "Bit 10 reset after : Retry %d\n", timeout);
-			ret = p->hif_func.hif_read_reg(WILC_GLB_RESET_0, &reg);
+			PRINT_D(GENERIC_DBG, "Bit 10 reset after : Retry %d\n",
+				timeout);
+			ret = wilc->hif_func->hif_read_reg(wilc, WILC_GLB_RESET_0,
+						       &reg);
 			if (!ret) {
 				PRINT_ER("Error while reading reg\n");
-				release_bus(RELEASE_ALLOW_SLEEP);
+				release_bus(wilc, RELEASE_ALLOW_SLEEP);
 				return ret;
 			}
-			PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n", reg, timeout);
+			PRINT_D(GENERIC_DBG, "Read RESET Reg %x : Retry%d\n",
+				reg, timeout);
 			break;
 		}
 
@@ -1643,33 +1332,32 @@ int wilc_wlan_stop(void)
 	reg = (BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(8) | BIT(9) | BIT(26) |
 	       BIT(29) | BIT(30) | BIT(31));
 
-	p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+	wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, reg);
 	reg = (u32)~BIT(10);
 
-	ret = p->hif_func.hif_write_reg(WILC_GLB_RESET_0, reg);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_GLB_RESET_0, reg);
 
-	release_bus(RELEASE_ALLOW_SLEEP);
+	release_bus(wilc, RELEASE_ALLOW_SLEEP);
 
 	return ret;
 }
 
 void wilc_wlan_cleanup(struct net_device *dev)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	struct txq_entry_t *tqe;
 	struct rxq_entry_t *rqe;
 	u32 reg = 0;
 	int ret;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	p->quit = 1;
+	wilc->quit = 1;
 	do {
-		tqe = wilc_wlan_txq_remove_from_head();
-		if (tqe == NULL)
+		tqe = wilc_wlan_txq_remove_from_head(dev);
+		if (!tqe)
 			break;
 		if (tqe->tx_complete_func)
 			tqe->tx_complete_func(tqe->priv, 0);
@@ -1678,157 +1366,133 @@ void wilc_wlan_cleanup(struct net_device *dev)
 
 	do {
 		rqe = wilc_wlan_rxq_remove(wilc);
-		if (rqe == NULL)
+		if (!rqe)
 			break;
-#ifndef MEMORY_STATIC
-		kfree(rqe->buffer);
-#endif
 		kfree(rqe);
 	} while (1);
 
-	/**
-	 *      clean up buffer
-	 **/
-
-	#ifdef MEMORY_STATIC
-	kfree(p->rx_buffer);
-	p->rx_buffer = NULL;
-	#endif
-	kfree(p->tx_buffer);
+	kfree(wilc->rx_buffer);
+	wilc->rx_buffer = NULL;
+	kfree(wilc->tx_buffer);
+	wilc->tx_buffer = NULL;
 
-	acquire_bus(ACQUIRE_AND_WAKEUP);
+	acquire_bus(wilc, ACQUIRE_AND_WAKEUP);
 
-
-	ret = p->hif_func.hif_read_reg(WILC_GP_REG_0, &reg);
+	ret = wilc->hif_func->hif_read_reg(wilc, WILC_GP_REG_0, &reg);
 	if (!ret) {
 		PRINT_ER("Error while reading reg\n");
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 	}
 	PRINT_ER("Writing ABORT reg\n");
-	ret = p->hif_func.hif_write_reg(WILC_GP_REG_0, (reg | ABORT_INT));
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_GP_REG_0,
+					(reg | ABORT_INT));
 	if (!ret) {
 		PRINT_ER("Error while writing reg\n");
-		release_bus(RELEASE_ALLOW_SLEEP);
+		release_bus(wilc, RELEASE_ALLOW_SLEEP);
 	}
-	release_bus(RELEASE_ALLOW_SLEEP);
-	/**
-	 *      io clean up
-	 **/
-	p->hif_func.hif_deinit(NULL);
-
+	release_bus(wilc, RELEASE_ALLOW_SLEEP);
+	wilc->hif_func->hif_deinit(NULL);
 }
 
-static int wilc_wlan_cfg_commit(int type, u32 drvHandler)
+static int wilc_wlan_cfg_commit(struct wilc *wilc, int type, u32 drv_handler)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
-	wilc_cfg_frame_t *cfg = &p->cfg_frame;
-	int total_len = p->cfg_frame_offset + 4 + DRIVER_HANDLER_SIZE;
-	int seq_no = p->cfg_seq_no % 256;
-	int driver_handler = (u32)drvHandler;
-
+	struct wilc_cfg_frame *cfg = &wilc->cfg_frame;
+	int total_len = wilc->cfg_frame_offset + 4 + DRIVER_HANDLER_SIZE;
+	int seq_no = wilc->cfg_seq_no % 256;
+	int driver_handler = (u32)drv_handler;
 
-	/**
-	 *      Set up header
-	 **/
-	if (type == WILC_CFG_SET) {             /* Set */
+	if (type == WILC_CFG_SET)
 		cfg->wid_header[0] = 'W';
-	} else {                                        /* Query */
+	else
 		cfg->wid_header[0] = 'Q';
-	}
-	cfg->wid_header[1] = seq_no;    /* sequence number */
+	cfg->wid_header[1] = seq_no;
 	cfg->wid_header[2] = (u8)total_len;
 	cfg->wid_header[3] = (u8)(total_len >> 8);
 	cfg->wid_header[4] = (u8)driver_handler;
 	cfg->wid_header[5] = (u8)(driver_handler >> 8);
 	cfg->wid_header[6] = (u8)(driver_handler >> 16);
 	cfg->wid_header[7] = (u8)(driver_handler >> 24);
-	p->cfg_seq_no = seq_no;
-
-	/**
-	 *      Add to TX queue
-	 **/
+	wilc->cfg_seq_no = seq_no;
 
-	if (!wilc_wlan_txq_add_cfg_pkt(&cfg->wid_header[0], total_len))
+	if (!wilc_wlan_txq_add_cfg_pkt(wilc, &cfg->wid_header[0], total_len))
 		return -1;
 
 	return 0;
 }
 
-int wilc_wlan_cfg_set(int start, u32 wid, u8 *buffer, u32 buffer_size,
-		      int commit, u32 drvHandler)
+int wilc_wlan_cfg_set(struct wilc *wilc, int start, u32 wid, u8 *buffer,
+		      u32 buffer_size, int commit, u32 drv_handler)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	u32 offset;
 	int ret_size;
 
-
-	if (p->cfg_frame_in_use)
+	if (wilc->cfg_frame_in_use)
 		return 0;
 
 	if (start)
-		p->cfg_frame_offset = 0;
+		wilc->cfg_frame_offset = 0;
 
-	offset = p->cfg_frame_offset;
-	ret_size = wilc_wlan_cfg_set_wid(p->cfg_frame.frame, offset, (u16)wid,
-					 buffer, buffer_size);
+	offset = wilc->cfg_frame_offset;
+	ret_size = wilc_wlan_cfg_set_wid(wilc->cfg_frame.frame, offset,
+					 (u16)wid, buffer, buffer_size);
 	offset += ret_size;
-	p->cfg_frame_offset = offset;
+	wilc->cfg_frame_offset = offset;
 
 	if (commit) {
-		PRINT_D(TX_DBG, "[WILC]PACKET Commit with sequence number %d\n", p->cfg_seq_no);
+		PRINT_D(TX_DBG, "[WILC]PACKET Commit with sequence number %d\n",
+			wilc->cfg_seq_no);
 		PRINT_D(RX_DBG, "Processing cfg_set()\n");
-		p->cfg_frame_in_use = 1;
+		wilc->cfg_frame_in_use = 1;
 
-		if (wilc_wlan_cfg_commit(WILC_CFG_SET, drvHandler))
+		if (wilc_wlan_cfg_commit(wilc, WILC_CFG_SET, drv_handler))
 			ret_size = 0;
 
-		if (linux_wlan_lock_timeout(&g_linux_wlan->cfg_event,
+		if (wilc_lock_timeout(wilc, &wilc->cfg_event,
 					    CFG_PKTS_TIMEOUT)) {
 			PRINT_D(TX_DBG, "Set Timed Out\n");
 			ret_size = 0;
 		}
-		p->cfg_frame_in_use = 0;
-		p->cfg_frame_offset = 0;
-		p->cfg_seq_no += 1;
-
+		wilc->cfg_frame_in_use = 0;
+		wilc->cfg_frame_offset = 0;
+		wilc->cfg_seq_no += 1;
 	}
 
 	return ret_size;
 }
-int wilc_wlan_cfg_get(int start, u32 wid, int commit, u32 drvHandler)
+
+int wilc_wlan_cfg_get(struct wilc *wilc, int start, u32 wid, int commit,
+		      u32 drv_handler)
 {
-	wilc_wlan_dev_t *p = &g_wlan;
 	u32 offset;
 	int ret_size;
 
-
-	if (p->cfg_frame_in_use)
+	if (wilc->cfg_frame_in_use)
 		return 0;
 
 	if (start)
-		p->cfg_frame_offset = 0;
+		wilc->cfg_frame_offset = 0;
 
-	offset = p->cfg_frame_offset;
-	ret_size = wilc_wlan_cfg_get_wid(p->cfg_frame.frame, offset, (u16)wid);
+	offset = wilc->cfg_frame_offset;
+	ret_size = wilc_wlan_cfg_get_wid(wilc->cfg_frame.frame, offset,
+					 (u16)wid);
 	offset += ret_size;
-	p->cfg_frame_offset = offset;
+	wilc->cfg_frame_offset = offset;
 
 	if (commit) {
-		p->cfg_frame_in_use = 1;
+		wilc->cfg_frame_in_use = 1;
 
-		if (wilc_wlan_cfg_commit(WILC_CFG_QUERY, drvHandler))
+		if (wilc_wlan_cfg_commit(wilc, WILC_CFG_QUERY, drv_handler))
 			ret_size = 0;
 
-
-		if (linux_wlan_lock_timeout(&g_linux_wlan->cfg_event,
+		if (wilc_lock_timeout(wilc, &wilc->cfg_event,
 					    CFG_PKTS_TIMEOUT)) {
 			PRINT_D(TX_DBG, "Get Timed Out\n");
 			ret_size = 0;
 		}
 		PRINT_D(GENERIC_DBG, "[WILC]Get Response received\n");
-		p->cfg_frame_in_use = 0;
-		p->cfg_frame_offset = 0;
-		p->cfg_seq_no += 1;
+		wilc->cfg_frame_in_use = 0;
+		wilc->cfg_frame_offset = 0;
+		wilc->cfg_seq_no += 1;
 	}
 
 	return ret_size;
@@ -1843,92 +1507,69 @@ int wilc_wlan_cfg_get_val(u32 wid, u8 *buffer, u32 buffer_size)
 	return ret;
 }
 
-void wilc_bus_set_max_speed(void)
-{
-
-	/* Increase bus speed to max possible.  */
-	g_wlan.hif_func.hif_set_max_bus_speed();
-}
-
-void wilc_bus_set_default_speed(void)
-{
-
-	/* Restore bus speed to default.  */
-	g_wlan.hif_func.hif_set_default_bus_speed();
-}
-u32 init_chip(void)
+static u32 init_chip(struct net_device *dev)
 {
 	u32 chipid;
 	u32 reg, ret = 0;
+	struct wilc_vif *vif;
+	struct wilc *wilc;
 
-	acquire_bus(ACQUIRE_ONLY);
-
-	chipid = wilc_get_chipid(true);
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
+	acquire_bus(wilc, ACQUIRE_ONLY);
 
+	chipid = wilc_get_chipid(wilc, true);
 
 	if ((chipid & 0xfff) != 0xa0) {
-		/**
-		 * Avoid booting from boot ROM. Make sure that Drive IRQN [SDIO platform]
-		 * or SD_DAT3 [SPI platform] to ?1?
-		 **/
-		/* Set cortus reset register to register control. */
-		ret = g_wlan.hif_func.hif_read_reg(0x1118, &reg);
+		ret = wilc->hif_func->hif_read_reg(wilc, 0x1118, &reg);
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc start]: fail read reg 0x1118 ...\n");
 			return ret;
 		}
 		reg |= BIT(0);
-		ret = g_wlan.hif_func.hif_write_reg(0x1118, reg);
+		ret = wilc->hif_func->hif_write_reg(wilc, 0x1118, reg);
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc start]: fail write reg 0x1118 ...\n");
 			return ret;
 		}
-		/**
-		 * Write branch intruction to IRAM (0x71 trap) at location 0xFFFF0000
-		 * (Cortus map) or C0000 (AHB map).
-		 **/
-		ret = g_wlan.hif_func.hif_write_reg(0xc0000, 0x71);
+		ret = wilc->hif_func->hif_write_reg(wilc, 0xc0000, 0x71);
 		if (!ret) {
 			wilc_debug(N_ERR, "[wilc start]: fail write reg 0xc0000 ...\n");
 			return ret;
 		}
 	}
 
-	release_bus(RELEASE_ONLY);
+	release_bus(wilc, RELEASE_ONLY);
 
 	return ret;
-
 }
 
-u32 wilc_get_chipid(u8 update)
+u32 wilc_get_chipid(struct wilc *wilc, u8 update)
 {
 	static u32 chipid;
-	/* SDIO can't read into global variables */
-	/* Use this variable as a temp, then copy to the global */
 	u32 tempchipid = 0;
 	u32 rfrevid;
 
 	if (chipid == 0 || update != 0) {
-		g_wlan.hif_func.hif_read_reg(0x1000, &tempchipid);
-		g_wlan.hif_func.hif_read_reg(0x13f4, &rfrevid);
+		wilc->hif_func->hif_read_reg(wilc, 0x1000, &tempchipid);
+		wilc->hif_func->hif_read_reg(wilc, 0x13f4, &rfrevid);
 		if (!ISWILC1000(tempchipid)) {
 			chipid = 0;
 			goto _fail_;
 		}
 		if (tempchipid == 0x1002a0) {
-			if (rfrevid == 0x1) { /* 1002A0 */
-			} else { /* if (rfrevid == 0x2) */   /* 1002A1 */
+			if (rfrevid == 0x1) {
+			} else {
 				tempchipid = 0x1002a1;
 			}
 		} else if (tempchipid == 0x1002b0) {
-			if (rfrevid == 3) { /* 1002B0 */
-			} else if (rfrevid == 4) { /* 1002B1 */
+			if (rfrevid == 3) {
+			} else if (rfrevid == 4) {
 				tempchipid = 0x1002b1;
-			} else { /* if(rfrevid == 5) */   /* 1002B2 */
+			} else {
 				tempchipid = 0x1002b2;
 			}
-		} else {
 		}
 
 		chipid = tempchipid;
@@ -1937,129 +1578,88 @@ _fail_:
 	return chipid;
 }
 
-int wilc_wlan_init(wilc_wlan_inp_t *inp)
+int wilc_wlan_init(struct net_device *dev)
 {
-
 	int ret = 0;
+	struct wilc_vif *vif = netdev_priv(dev);
+	struct wilc *wilc;
+
+	wilc = vif->wilc;
 
 	PRINT_D(INIT_DBG, "Initializing WILC_Wlan ...\n");
 
-	memset((void *)&g_wlan, 0, sizeof(wilc_wlan_dev_t));
-
-	/**
-	 *      store the input
-	 **/
-	memcpy((void *)&g_wlan.io_func, (void *)&inp->io_func, sizeof(wilc_wlan_io_func_t));
-	/***
-	 *      host interface init
-	 **/
-	if ((inp->io_func.io_type & 0x1) == HIF_SDIO) {
-		if (!hif_sdio.hif_init(inp, wilc_debug)) {
-			/* EIO	5 */
-			ret = -5;
-			goto _fail_;
-		}
-		memcpy((void *)&g_wlan.hif_func, &hif_sdio, sizeof(wilc_hif_func_t));
-	} else {
-		if ((inp->io_func.io_type & 0x1) == HIF_SPI) {
-			/**
-			 *      TODO:
-			 **/
-			if (!hif_spi.hif_init(inp, wilc_debug)) {
-				/* EIO	5 */
-				ret = -5;
-				goto _fail_;
-			}
-			memcpy((void *)&g_wlan.hif_func, &hif_spi, sizeof(wilc_hif_func_t));
-		} else {
-			/* EIO	5 */
-			ret = -5;
-			goto _fail_;
-		}
+	if (!wilc->hif_func->hif_init(wilc)) {
+		ret = -EIO;
+		goto _fail_;
 	}
 
-	/***
-	 *      mac interface init
-	 **/
 	if (!wilc_wlan_cfg_init(wilc_debug)) {
-		/* ENOBUFS	105 */
-		ret = -105;
+		ret = -ENOBUFS;
 		goto _fail_;
 	}
 
-	/**
-	 *      alloc tx, rx buffer
-	 **/
-	if (g_wlan.tx_buffer == NULL)
-		g_wlan.tx_buffer = kmalloc(LINUX_TX_SIZE, GFP_KERNEL);
-	PRINT_D(TX_DBG, "g_wlan.tx_buffer = %p\n", g_wlan.tx_buffer);
+	if (!wilc->tx_buffer)
+		wilc->tx_buffer = kmalloc(LINUX_TX_SIZE, GFP_KERNEL);
+	PRINT_D(TX_DBG, "wilc->tx_buffer = %p\n", wilc->tx_buffer);
 
-	if (g_wlan.tx_buffer == NULL) {
-		/* ENOBUFS	105 */
-		ret = -105;
+	if (!wilc->tx_buffer) {
+		ret = -ENOBUFS;
 		PRINT_ER("Can't allocate Tx Buffer");
 		goto _fail_;
 	}
 
-/* rx_buffer is not used unless we activate USE_MEM STATIC which is not applicable, allocating such memory is useless*/
-#if defined (MEMORY_STATIC)
-	if (g_wlan.rx_buffer == NULL)
-		g_wlan.rx_buffer = kmalloc(LINUX_RX_SIZE, GFP_KERNEL);
-	PRINT_D(TX_DBG, "g_wlan.rx_buffer =%p\n", g_wlan.rx_buffer);
-	if (g_wlan.rx_buffer == NULL) {
-		/* ENOBUFS	105 */
-		ret = -105;
+	if (!wilc->rx_buffer)
+		wilc->rx_buffer = kmalloc(LINUX_RX_SIZE, GFP_KERNEL);
+	PRINT_D(TX_DBG, "wilc->rx_buffer =%p\n", wilc->rx_buffer);
+	if (!wilc->rx_buffer) {
+		ret = -ENOBUFS;
 		PRINT_ER("Can't allocate Rx Buffer");
 		goto _fail_;
 	}
-#endif
 
-	if (!init_chip()) {
-		/* EIO	5 */
-		ret = -5;
+	if (!init_chip(dev)) {
+		ret = -EIO;
 		goto _fail_;
 	}
 #ifdef	TCP_ACK_FILTER
-	Init_TCP_tracking();
+	init_tcp_tracking();
 #endif
 
 	return 1;
 
 _fail_:
 
-  #ifdef MEMORY_STATIC
-	kfree(g_wlan.rx_buffer);
-	g_wlan.rx_buffer = NULL;
-  #endif
-	kfree(g_wlan.tx_buffer);
-	g_wlan.tx_buffer = NULL;
+	kfree(wilc->rx_buffer);
+	wilc->rx_buffer = NULL;
+	kfree(wilc->tx_buffer);
+	wilc->tx_buffer = NULL;
 
 	return ret;
-
 }
 
-u16 Set_machw_change_vir_if(struct net_device *dev, bool bValue)
+u16 wilc_set_machw_change_vir_if(struct net_device *dev, bool value)
 {
 	u16 ret;
 	u32 reg;
-	perInterface_wlan_t *nic;
+	struct wilc_vif *vif;
 	struct wilc *wilc;
 
-	nic = netdev_priv(dev);
-	wilc = nic->wilc;
+	vif = netdev_priv(dev);
+	wilc = vif->wilc;
 
-	/*Reset WILC_CHANGING_VIR_IF register to allow adding futrue keys to CE H/W*/
 	mutex_lock(&wilc->hif_cs);
-	ret = (&g_wlan)->hif_func.hif_read_reg(WILC_CHANGING_VIR_IF, &reg);
+	ret = wilc->hif_func->hif_read_reg(wilc, WILC_CHANGING_VIR_IF,
+					       &reg);
 	if (!ret)
 		PRINT_ER("Error while Reading reg WILC_CHANGING_VIR_IF\n");
 
-	if (bValue)
+	if (value)
 		reg |= BIT(31);
 	else
 		reg &= ~BIT(31);
 
-	ret = (&g_wlan)->hif_func.hif_write_reg(WILC_CHANGING_VIR_IF, reg);
+	ret = wilc->hif_func->hif_write_reg(wilc, WILC_CHANGING_VIR_IF,
+						reg);
 
 	if (!ret)
 		PRINT_ER("Error while writing reg WILC_CHANGING_VIR_IF\n");
diff --git a/drivers/staging/wilc1000/wilc_wlan.h b/drivers/staging/wilc1000/wilc_wlan.h
index 57e1d5174050..2edd7445f4a3 100644
--- a/drivers/staging/wilc1000/wilc_wlan.h
+++ b/drivers/staging/wilc1000/wilc_wlan.h
@@ -1,155 +1,131 @@
 #ifndef WILC_WLAN_H
 #define WILC_WLAN_H
 
+#include <linux/types.h>
 
-
-#define ISWILC1000(id)   (((id & 0xfffff000) == 0x100000) ? 1 : 0)
-
+#define ISWILC1000(id)			((id & 0xfffff000) == 0x100000 ? 1 : 0)
 
 /********************************************
  *
  *      Mac eth header length
  *
  ********************************************/
-#define DRIVER_HANDLER_SIZE 4
-#define MAX_MAC_HDR_LEN         26 /* QOS_MAC_HDR_LEN */
-#define SUB_MSDU_HEADER_LENGTH  14
-#define SNAP_HDR_LEN            8
-#define ETHERNET_HDR_LEN          14
-#define WORD_ALIGNMENT_PAD        0
-
-#define ETH_ETHERNET_HDR_OFFSET   (MAX_MAC_HDR_LEN + SUB_MSDU_HEADER_LENGTH + \
-				   SNAP_HDR_LEN - ETHERNET_HDR_LEN + WORD_ALIGNMENT_PAD)
-
-#define HOST_HDR_OFFSET		4
-#define ETHERNET_HDR_LEN          14
-#define IP_HDR_LEN                20
-#define IP_HDR_OFFSET             ETHERNET_HDR_LEN
-#define UDP_HDR_OFFSET            (IP_HDR_LEN + IP_HDR_OFFSET)
-#define UDP_HDR_LEN               8
-#define UDP_DATA_OFFSET           (UDP_HDR_OFFSET + UDP_HDR_LEN)
-#define ETH_CONFIG_PKT_HDR_LEN    UDP_DATA_OFFSET
-
-#define ETH_CONFIG_PKT_HDR_OFFSET (ETH_ETHERNET_HDR_OFFSET + \
-				   ETH_CONFIG_PKT_HDR_LEN)
-
-/********************************************
- *
- *      Endian Conversion
- *
- ********************************************/
-
-#define BYTE_SWAP(val) ((((val) & 0x000000FF) << 24) + \
-			(((val) & 0x0000FF00) << 8)  + \
-			(((val) & 0x00FF0000) >> 8)   +	\
-			(((val) & 0xFF000000) >> 24))
+#define DRIVER_HANDLER_SIZE		4
+#define MAX_MAC_HDR_LEN			26 /* QOS_MAC_HDR_LEN */
+#define SUB_MSDU_HEADER_LENGTH		14
+#define SNAP_HDR_LEN			8
+#define ETHERNET_HDR_LEN		14
+#define WORD_ALIGNMENT_PAD		0
+
+#define ETH_ETHERNET_HDR_OFFSET		(MAX_MAC_HDR_LEN + \
+					 SUB_MSDU_HEADER_LENGTH + \
+					 SNAP_HDR_LEN - \
+					 ETHERNET_HDR_LEN + \
+					 WORD_ALIGNMENT_PAD)
+
+#define HOST_HDR_OFFSET			4
+#define ETHERNET_HDR_LEN		14
+#define IP_HDR_LEN			20
+#define IP_HDR_OFFSET			ETHERNET_HDR_LEN
+#define UDP_HDR_OFFSET			(IP_HDR_LEN + IP_HDR_OFFSET)
+#define UDP_HDR_LEN			8
+#define UDP_DATA_OFFSET			(UDP_HDR_OFFSET + UDP_HDR_LEN)
+#define ETH_CONFIG_PKT_HDR_LEN		UDP_DATA_OFFSET
+
+#define ETH_CONFIG_PKT_HDR_OFFSET	(ETH_ETHERNET_HDR_OFFSET + \
+					 ETH_CONFIG_PKT_HDR_LEN)
 
 /********************************************
  *
  *      Register Defines
  *
  ********************************************/
-#define WILC_PERIPH_REG_BASE 0x1000
-#define WILC_CHANGING_VIR_IF                     (0x108c)
-#define WILC_CHIPID	(WILC_PERIPH_REG_BASE)
-#define WILC_GLB_RESET_0 (WILC_PERIPH_REG_BASE + 0x400)
-#define WILC_PIN_MUX_0 (WILC_PERIPH_REG_BASE + 0x408)
-#define WILC_HOST_TX_CTRL (WILC_PERIPH_REG_BASE + 0x6c)
-#define WILC_HOST_RX_CTRL_0 (WILC_PERIPH_REG_BASE + 0x70)
-#define WILC_HOST_RX_CTRL_1 (WILC_PERIPH_REG_BASE + 0x74)
-#define WILC_HOST_VMM_CTL	(WILC_PERIPH_REG_BASE + 0x78)
-#define WILC_HOST_RX_CTRL	(WILC_PERIPH_REG_BASE + 0x80)
-#define WILC_HOST_RX_EXTRA_SIZE	(WILC_PERIPH_REG_BASE + 0x84)
-#define WILC_HOST_TX_CTRL_1	(WILC_PERIPH_REG_BASE + 0x88)
-#define WILC_MISC	(WILC_PERIPH_REG_BASE + 0x428)
-#define WILC_INTR_REG_BASE (WILC_PERIPH_REG_BASE + 0xa00)
-#define WILC_INTR_ENABLE (WILC_INTR_REG_BASE)
-#define WILC_INTR2_ENABLE (WILC_INTR_REG_BASE + 4)
-
-#define WILC_INTR_POLARITY (WILC_INTR_REG_BASE + 0x10)
-#define WILC_INTR_TYPE (WILC_INTR_REG_BASE + 0x20)
-#define WILC_INTR_CLEAR (WILC_INTR_REG_BASE + 0x30)
-#define WILC_INTR_STATUS (WILC_INTR_REG_BASE + 0x40)
-
-#define WILC_VMM_TBL_SIZE 64
-#define WILC_VMM_TX_TBL_BASE (0x150400)
-#define WILC_VMM_RX_TBL_BASE (0x150500)
-
-#define WILC_VMM_BASE 0x150000
-#define WILC_VMM_CORE_CTL (WILC_VMM_BASE)
-#define WILC_VMM_TBL_CTL (WILC_VMM_BASE + 0x4)
-#define WILC_VMM_TBL_ENTRY (WILC_VMM_BASE + 0x8)
-#define WILC_VMM_TBL0_SIZE (WILC_VMM_BASE + 0xc)
-#define WILC_VMM_TO_HOST_SIZE (WILC_VMM_BASE + 0x10)
-#define WILC_VMM_CORE_CFG (WILC_VMM_BASE + 0x14)
-#define WILC_VMM_TBL_ACTIVE (WILC_VMM_BASE + 040)
-#define WILC_VMM_TBL_STATUS (WILC_VMM_BASE + 0x44)
-
-#define WILC_SPI_REG_BASE 0xe800
-#define WILC_SPI_CTL (WILC_SPI_REG_BASE)
-#define WILC_SPI_MASTER_DMA_ADDR (WILC_SPI_REG_BASE + 0x4)
-#define WILC_SPI_MASTER_DMA_COUNT (WILC_SPI_REG_BASE + 0x8)
-#define WILC_SPI_SLAVE_DMA_ADDR (WILC_SPI_REG_BASE + 0xc)
-#define WILC_SPI_SLAVE_DMA_COUNT (WILC_SPI_REG_BASE + 0x10)
-#define WILC_SPI_TX_MODE (WILC_SPI_REG_BASE + 0x20)
-#define WILC_SPI_PROTOCOL_CONFIG (WILC_SPI_REG_BASE + 0x24)
-#define WILC_SPI_INTR_CTL (WILC_SPI_REG_BASE + 0x2c)
-
-#define WILC_SPI_PROTOCOL_OFFSET (WILC_SPI_PROTOCOL_CONFIG - WILC_SPI_REG_BASE)
-
-#define WILC_AHB_DATA_MEM_BASE 0x30000
-#define WILC_AHB_SHARE_MEM_BASE 0xd0000
-
-#define WILC_VMM_TBL_RX_SHADOW_BASE WILC_AHB_SHARE_MEM_BASE
-#define WILC_VMM_TBL_RX_SHADOW_SIZE (256)
-
-#define WILC_GP_REG_0   0x149c
-#define WILC_GP_REG_1   0x14a0
-
-#define rHAVE_SDIO_IRQ_GPIO_BIT      (0)
-#define rHAVE_USE_PMU_BIT            (1)
-#define rHAVE_SLEEP_CLK_SRC_RTC_BIT  (2)
-#define rHAVE_SLEEP_CLK_SRC_XO_BIT   (3)
-#define rHAVE_EXT_PA_INV_TX_RX_BIT   (4)
-#define rHAVE_LEGACY_RF_SETTINGS_BIT (5)
-#define rHAVE_XTAL_24_BIT            (6)
-#define rHAVE_DISABLE_WILC_UART_BIT   (7)
-
-
-#define WILC_HAVE_SDIO_IRQ_GPIO       (1 << rHAVE_SDIO_IRQ_GPIO_BIT)
-#define WILC_HAVE_USE_PMU             (1 << rHAVE_USE_PMU_BIT)
-#define WILC_HAVE_SLEEP_CLK_SRC_RTC   (1 << rHAVE_SLEEP_CLK_SRC_RTC_BIT)
-#define WILC_HAVE_SLEEP_CLK_SRC_XO    (1 << rHAVE_SLEEP_CLK_SRC_XO_BIT)
-#define WILC_HAVE_EXT_PA_INV_TX_RX    (1 << rHAVE_EXT_PA_INV_TX_RX_BIT)
-#define WILC_HAVE_LEGACY_RF_SETTINGS  (1 << rHAVE_LEGACY_RF_SETTINGS_BIT)
-#define WILC_HAVE_XTAL_24             (1 << rHAVE_XTAL_24_BIT)
-#define WILC_HAVE_DISABLE_WILC_UART    (1 << rHAVE_DISABLE_WILC_UART_BIT)
-
+#define WILC_PERIPH_REG_BASE		0x1000
+#define WILC_CHANGING_VIR_IF		0x108c
+#define WILC_CHIPID			WILC_PERIPH_REG_BASE
+#define WILC_GLB_RESET_0		(WILC_PERIPH_REG_BASE + 0x400)
+#define WILC_PIN_MUX_0			(WILC_PERIPH_REG_BASE + 0x408)
+#define WILC_HOST_TX_CTRL		(WILC_PERIPH_REG_BASE + 0x6c)
+#define WILC_HOST_RX_CTRL_0		(WILC_PERIPH_REG_BASE + 0x70)
+#define WILC_HOST_RX_CTRL_1		(WILC_PERIPH_REG_BASE + 0x74)
+#define WILC_HOST_VMM_CTL		(WILC_PERIPH_REG_BASE + 0x78)
+#define WILC_HOST_RX_CTRL		(WILC_PERIPH_REG_BASE + 0x80)
+#define WILC_HOST_RX_EXTRA_SIZE		(WILC_PERIPH_REG_BASE + 0x84)
+#define WILC_HOST_TX_CTRL_1		(WILC_PERIPH_REG_BASE + 0x88)
+#define WILC_MISC			(WILC_PERIPH_REG_BASE + 0x428)
+#define WILC_INTR_REG_BASE		(WILC_PERIPH_REG_BASE + 0xa00)
+#define WILC_INTR_ENABLE		WILC_INTR_REG_BASE
+#define WILC_INTR2_ENABLE		(WILC_INTR_REG_BASE + 4)
+
+#define WILC_INTR_POLARITY		(WILC_INTR_REG_BASE + 0x10)
+#define WILC_INTR_TYPE			(WILC_INTR_REG_BASE + 0x20)
+#define WILC_INTR_CLEAR			(WILC_INTR_REG_BASE + 0x30)
+#define WILC_INTR_STATUS		(WILC_INTR_REG_BASE + 0x40)
+
+#define WILC_VMM_TBL_SIZE		64
+#define WILC_VMM_TX_TBL_BASE		0x150400
+#define WILC_VMM_RX_TBL_BASE		0x150500
+
+#define WILC_VMM_BASE			0x150000
+#define WILC_VMM_CORE_CTL		WILC_VMM_BASE
+#define WILC_VMM_TBL_CTL		(WILC_VMM_BASE + 0x4)
+#define WILC_VMM_TBL_ENTRY		(WILC_VMM_BASE + 0x8)
+#define WILC_VMM_TBL0_SIZE		(WILC_VMM_BASE + 0xc)
+#define WILC_VMM_TO_HOST_SIZE		(WILC_VMM_BASE + 0x10)
+#define WILC_VMM_CORE_CFG		(WILC_VMM_BASE + 0x14)
+#define WILC_VMM_TBL_ACTIVE		(WILC_VMM_BASE + 040)
+#define WILC_VMM_TBL_STATUS		(WILC_VMM_BASE + 0x44)
+
+#define WILC_SPI_REG_BASE		0xe800
+#define WILC_SPI_CTL			WILC_SPI_REG_BASE
+#define WILC_SPI_MASTER_DMA_ADDR	(WILC_SPI_REG_BASE + 0x4)
+#define WILC_SPI_MASTER_DMA_COUNT	(WILC_SPI_REG_BASE + 0x8)
+#define WILC_SPI_SLAVE_DMA_ADDR		(WILC_SPI_REG_BASE + 0xc)
+#define WILC_SPI_SLAVE_DMA_COUNT	(WILC_SPI_REG_BASE + 0x10)
+#define WILC_SPI_TX_MODE		(WILC_SPI_REG_BASE + 0x20)
+#define WILC_SPI_PROTOCOL_CONFIG	(WILC_SPI_REG_BASE + 0x24)
+#define WILC_SPI_INTR_CTL		(WILC_SPI_REG_BASE + 0x2c)
+
+#define WILC_SPI_PROTOCOL_OFFSET	(WILC_SPI_PROTOCOL_CONFIG - \
+					 WILC_SPI_REG_BASE)
+
+#define WILC_AHB_DATA_MEM_BASE		0x30000
+#define WILC_AHB_SHARE_MEM_BASE		0xd0000
+
+#define WILC_VMM_TBL_RX_SHADOW_BASE	WILC_AHB_SHARE_MEM_BASE
+#define WILC_VMM_TBL_RX_SHADOW_SIZE	256
+
+#define WILC_GP_REG_0			0x149c
+#define WILC_GP_REG_1			0x14a0
+
+#define WILC_HAVE_SDIO_IRQ_GPIO		BIT(0)
+#define WILC_HAVE_USE_PMU		BIT(1)
+#define WILC_HAVE_SLEEP_CLK_SRC_RTC	BIT(2)
+#define WILC_HAVE_SLEEP_CLK_SRC_XO	BIT(3)
+#define WILC_HAVE_EXT_PA_INV_TX_RX	BIT(4)
+#define WILC_HAVE_LEGACY_RF_SETTINGS	BIT(5)
+#define WILC_HAVE_XTAL_24		BIT(6)
+#define WILC_HAVE_DISABLE_WILC_UART	BIT(7)
 
 /********************************************
  *
  *      Wlan Defines
  *
  ********************************************/
-#define WILC_CFG_PKT	1
-#define WILC_NET_PKT 0
-#define WILC_MGMT_PKT 2
+#define WILC_CFG_PKT		1
+#define WILC_NET_PKT		0
+#define WILC_MGMT_PKT		2
 
-#define WILC_CFG_SET 1
-#define WILC_CFG_QUERY 0
-
-#define WILC_CFG_RSP	1
-#define WILC_CFG_RSP_STATUS 2
-#define WILC_CFG_RSP_SCAN 3
-
-#ifdef WILC_SDIO
-#define WILC_PLL_TO	4
-#else
-#define WILC_PLL_TO	2
-#endif
+#define WILC_CFG_SET		1
+#define WILC_CFG_QUERY		0
 
+#define WILC_CFG_RSP		1
+#define WILC_CFG_RSP_STATUS	2
+#define WILC_CFG_RSP_SCAN	3
 
-#define ABORT_INT   BIT(31)
+#define WILC_PLL_TO_SDIO	4
+#define WILC_PLL_TO_SPI		2
+#define ABORT_INT		BIT(31)
 
 /*******************************************/
 /*        E0 and later Interrupt flags.    */
@@ -165,15 +141,15 @@
 /* 20: INT4 flag                           */
 /* 21: INT5 flag                           */
 /*******************************************/
-#define                 IRG_FLAGS_OFFSET 16
-#define IRQ_DMA_WD_CNT_MASK ((1ul << IRG_FLAGS_OFFSET) - 1)
-#define INT_0           (1 << (IRG_FLAGS_OFFSET))
-#define INT_1           (1 << (IRG_FLAGS_OFFSET + 1))
-#define INT_2           (1 << (IRG_FLAGS_OFFSET + 2))
-#define INT_3           (1 << (IRG_FLAGS_OFFSET + 3))
-#define INT_4           (1 << (IRG_FLAGS_OFFSET + 4))
-#define INT_5           (1 << (IRG_FLAGS_OFFSET + 5))
-#define MAX_NUM_INT     (6)
+#define IRG_FLAGS_OFFSET	16
+#define IRQ_DMA_WD_CNT_MASK	((1ul << IRG_FLAGS_OFFSET) - 1)
+#define INT_0			BIT(IRG_FLAGS_OFFSET)
+#define INT_1			BIT(IRG_FLAGS_OFFSET + 1)
+#define INT_2			BIT(IRG_FLAGS_OFFSET + 2)
+#define INT_3			BIT(IRG_FLAGS_OFFSET + 3)
+#define INT_4			BIT(IRG_FLAGS_OFFSET + 4)
+#define INT_5			BIT(IRG_FLAGS_OFFSET + 5)
+#define MAX_NUM_INT		6
 
 /*******************************************/
 /*        E0 and later Interrupt flags.    */
@@ -188,30 +164,28 @@
 /* 7: Select VMM table 2                   */
 /* 8: Enable VMM                           */
 /*******************************************/
-#define CLR_INT0             BIT(0)
-#define CLR_INT1             BIT(1)
-#define CLR_INT2             BIT(2)
-#define CLR_INT3             BIT(3)
-#define CLR_INT4             BIT(4)
-#define CLR_INT5             BIT(5)
-#define SEL_VMM_TBL0         BIT(6)
-#define SEL_VMM_TBL1         BIT(7)
-#define EN_VMM               BIT(8)
-
-#define DATA_INT_EXT	INT_0
-#define PLL_INT_EXT         INT_1
-#define SLEEP_INT_EXT	INT_2
-#define ALL_INT_EXT     (DATA_INT_EXT | PLL_INT_EXT | SLEEP_INT_EXT)
-#define NUM_INT_EXT     (3)
-
-#define DATA_INT_CLR	CLR_INT0
-#define PLL_INT_CLR         CLR_INT1
-#define SLEEP_INT_CLR	CLR_INT2
-
-#define ENABLE_RX_VMM   (SEL_VMM_TBL1 | EN_VMM)
-#define ENABLE_TX_VMM   (SEL_VMM_TBL0 | EN_VMM)
-
-
+#define CLR_INT0		BIT(0)
+#define CLR_INT1		BIT(1)
+#define CLR_INT2		BIT(2)
+#define CLR_INT3		BIT(3)
+#define CLR_INT4		BIT(4)
+#define CLR_INT5		BIT(5)
+#define SEL_VMM_TBL0		BIT(6)
+#define SEL_VMM_TBL1		BIT(7)
+#define EN_VMM			BIT(8)
+
+#define DATA_INT_EXT		INT_0
+#define PLL_INT_EXT		INT_1
+#define SLEEP_INT_EXT		INT_2
+#define ALL_INT_EXT		(DATA_INT_EXT | PLL_INT_EXT | SLEEP_INT_EXT)
+#define NUM_INT_EXT		3
+
+#define DATA_INT_CLR		CLR_INT0
+#define PLL_INT_CLR		CLR_INT1
+#define SLEEP_INT_CLR		CLR_INT2
+
+#define ENABLE_RX_VMM		(SEL_VMM_TBL1 | EN_VMM)
+#define ENABLE_TX_VMM		(SEL_VMM_TBL0 | EN_VMM)
 /*time for expiring the semaphores of cfg packets*/
 #define CFG_PKTS_TIMEOUT	2000
 /********************************************
@@ -231,7 +205,7 @@ struct txq_entry_t {
 	struct txq_entry_t *next;
 	struct txq_entry_t *prev;
 	int type;
-	int tcp_PendingAck_index;
+	int tcp_pending_ack_idx;
 	u8 *buffer;
 	int buffer_size;
 	void *priv;
@@ -250,25 +224,26 @@ struct rxq_entry_t {
  *      Host IF Structure
  *
  ********************************************/
+struct wilc;
+struct wilc_hif_func {
+	int (*hif_init)(struct wilc *);
+	int (*hif_deinit)(struct wilc *);
+	int (*hif_read_reg)(struct wilc *, u32, u32 *);
+	int (*hif_write_reg)(struct wilc *, u32, u32);
+	int (*hif_block_rx)(struct wilc *, u32, u8 *, u32);
+	int (*hif_block_tx)(struct wilc *, u32, u8 *, u32);
+	int (*hif_read_int)(struct wilc *, u32 *);
+	int (*hif_clear_int_ext)(struct wilc *, u32);
+	int (*hif_read_size)(struct wilc *, u32 *);
+	int (*hif_block_tx_ext)(struct wilc *, u32, u8 *, u32);
+	int (*hif_block_rx_ext)(struct wilc *, u32, u8 *, u32);
+	int (*hif_sync_ext)(struct wilc *, int);
+	int (*enable_interrupt)(struct wilc *nic);
+	void (*disable_interrupt)(struct wilc *nic);
+};
 
-typedef struct {
-	int (*hif_init)(wilc_wlan_inp_t *, wilc_debug_func);
-	int (*hif_deinit)(void *);
-	int (*hif_read_reg)(u32, u32 *);
-	int (*hif_write_reg)(u32, u32);
-	int (*hif_block_rx)(u32, u8 *, u32);
-	int (*hif_block_tx)(u32, u8 *, u32);
-	int (*hif_sync)(void);
-	int (*hif_clear_int)(void);
-	int (*hif_read_int)(u32 *);
-	int (*hif_clear_int_ext)(u32);
-	int (*hif_read_size)(u32 *);
-	int (*hif_block_tx_ext)(u32, u8 *, u32);
-	int (*hif_block_rx_ext)(u32, u8 *, u32);
-	int (*hif_sync_ext)(int);
-	void (*hif_set_max_bus_speed)(void);
-	void (*hif_set_default_bus_speed)(void);
-} wilc_hif_func_t;
+extern const struct wilc_hif_func wilc_hif_spi;
+extern const struct wilc_hif_func wilc_hif_sdio;
 
 /********************************************
  *
@@ -276,37 +251,50 @@ typedef struct {
  *
  ********************************************/
 
-#define MAX_CFG_FRAME_SIZE 1468
+#define MAX_CFG_FRAME_SIZE	1468
 
-typedef struct {
+struct wilc_cfg_frame {
 	u8 ether_header[14];
 	u8 ip_header[20];
 	u8 udp_header[8];
 	u8 wid_header[8];
 	u8 frame[MAX_CFG_FRAME_SIZE];
-} wilc_cfg_frame_t;
-
-typedef struct {
-	int (*wlan_tx)(u8 *, u32, wilc_tx_complete_func_t);
-} wilc_wlan_cfg_func_t;
+};
 
-typedef struct {
+struct wilc_cfg_rsp {
 	int type;
 	u32 seq_no;
-} wilc_cfg_rsp_t;
+};
 
-int wilc_wlan_firmware_download(const u8 *buffer, u32 buffer_size);
-int wilc_wlan_start(void);
-int wilc_wlan_stop(void);
+struct wilc;
+
+int wilc_wlan_firmware_download(struct wilc *wilc, const u8 *buffer, u32 buffer_size);
+int wilc_wlan_start(struct wilc *);
+int wilc_wlan_stop(struct wilc *);
 int wilc_wlan_txq_add_net_pkt(struct net_device *dev, void *priv, u8 *buffer,
 			      u32 buffer_size, wilc_tx_complete_func_t func);
-int wilc_wlan_handle_txq(struct net_device *dev, u32 *pu32TxqCount);
-void wilc_handle_isr(void *wilc);
+int wilc_wlan_handle_txq(struct net_device *dev, u32 *txq_count);
+void wilc_handle_isr(struct wilc *wilc);
 void wilc_wlan_cleanup(struct net_device *dev);
-int wilc_wlan_cfg_set(int start, u32 wid, u8 *buffer, u32 buffer_size,
-		      int commit, u32 drvHandler);
-int wilc_wlan_cfg_get(int start, u32 wid, int commit, u32 drvHandler);
+int wilc_wlan_cfg_set(struct wilc *wilc, int start, u32 wid, u8 *buffer,
+		      u32 buffer_size, int commit, u32 drv_handler);
+int wilc_wlan_cfg_get(struct wilc *wilc, int start, u32 wid, int commit,
+		      u32 drv_handler);
 int wilc_wlan_cfg_get_val(u32 wid, u8 *buffer, u32 buffer_size);
-int wilc_wlan_txq_add_mgmt_pkt(void *priv, u8 *buffer, u32 buffer_size,
-			       wilc_tx_complete_func_t func);
+int wilc_wlan_txq_add_mgmt_pkt(struct net_device *dev, void *priv, u8 *buffer,
+			       u32 buffer_size, wilc_tx_complete_func_t func);
+void wilc_chip_sleep_manually(struct wilc *wilc);
+
+void wilc_enable_tcp_ack_filter(bool value);
+int wilc_wlan_get_num_conn_ifcs(struct wilc *);
+int wilc_mac_xmit(struct sk_buff *skb, struct net_device *dev);
+
+int wilc_mac_open(struct net_device *ndev);
+int wilc_mac_close(struct net_device *ndev);
+
+int wilc_wlan_set_bssid(struct net_device *wilc_netdev, u8 *pBSSID);
+void WILC_WFI_p2p_rx(struct net_device *dev, u8 *buff, u32 size);
+
+extern bool wilc_enable_ps;
+
 #endif
diff --git a/drivers/staging/wilc1000/wilc_wlan_cfg.c b/drivers/staging/wilc1000/wilc_wlan_cfg.c
index a34a81cdeb5e..b72c77bb35f1 100644
--- a/drivers/staging/wilc1000/wilc_wlan_cfg.c
+++ b/drivers/staging/wilc1000/wilc_wlan_cfg.c
@@ -275,9 +275,7 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 	while (size > 0) {
 		i = 0;
 		wid = info[0] | (info[1] << 8);
-#ifdef BIG_ENDIAN
-		wid = BYTE_SWAP(wid);
-#endif
+		wid = cpu_to_le32(wid);
 		PRINT_INFO(GENERIC_DBG, "Processing response for %d seq %d\n", wid, seq++);
 		switch ((wid >> 12) & 0x7) {
 		case WID_CHAR:
@@ -300,11 +298,7 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 					break;
 
 				if (g_cfg_hword[i].id == wid) {
-#ifdef BIG_ENDIAN
-					g_cfg_hword[i].val = (info[3] << 8) | (info[4]);
-#else
-					g_cfg_hword[i].val = info[3] | (info[4] << 8);
-#endif
+					g_cfg_hword[i].val = cpu_to_le16(info[3] | (info[4] << 8));
 					break;
 				}
 				i++;
@@ -318,11 +312,7 @@ static void wilc_wlan_parse_response_frame(u8 *info, int size)
 					break;
 
 				if (g_cfg_word[i].id == wid) {
-#ifdef BIG_ENDIAN
-					g_cfg_word[i].val = (info[3] << 24) | (info[4] << 16) | (info[5] << 8) | (info[6]);
-#else
-					g_cfg_word[i].val = info[3] | (info[4] << 8) | (info[5] << 16) | (info[6] << 24);
-#endif
+					g_cfg_word[i].val = cpu_to_le32(info[3] | (info[4] << 8) | (info[5] << 16) | (info[6] << 24));
 					break;
 				}
 				i++;
@@ -505,7 +495,8 @@ int wilc_wlan_cfg_get_wid_value(u16 wid, u8 *buffer, u32 buffer_size)
 	return ret;
 }
 
-int wilc_wlan_cfg_indicate_rx(u8 *frame, int size, wilc_cfg_rsp_t *rsp)
+int wilc_wlan_cfg_indicate_rx(struct wilc *wilc, u8 *frame, int size,
+			      struct wilc_cfg_rsp *rsp)
 {
 	int ret = 1;
 	u8 msg_type;
@@ -532,17 +523,17 @@ int wilc_wlan_cfg_indicate_rx(u8 *frame, int size, wilc_cfg_rsp_t *rsp)
 		rsp->seq_no = msg_id;
 		/*call host interface info parse as well*/
 		PRINT_INFO(RX_DBG, "Info message received\n");
-		GnrlAsyncInfoReceived(frame - 4, size + 4);
+		wilc_gnrl_async_info_received(wilc, frame - 4, size + 4);
 		break;
 
 	case 'N':
-		NetworkInfoReceived(frame - 4, size + 4);
+		wilc_network_info_received(wilc, frame - 4, size + 4);
 		rsp->type = 0;
 		break;
 
 	case 'S':
 		PRINT_INFO(RX_DBG, "Scan Notification Received\n");
-		host_int_ScanCompleteReceived(frame - 4, size + 4);
+		wilc_scan_complete_received(wilc, frame - 4, size + 4);
 		break;
 
 	default:
diff --git a/drivers/staging/wilc1000/wilc_wlan_cfg.h b/drivers/staging/wilc1000/wilc_wlan_cfg.h
index 30e60ec4d29f..5f74eb83562f 100644
--- a/drivers/staging/wilc1000/wilc_wlan_cfg.h
+++ b/drivers/staging/wilc1000/wilc_wlan_cfg.h
@@ -30,10 +30,12 @@ typedef struct {
 	u8 *str;
 } wilc_cfg_str_t;
 
+struct wilc;
 int wilc_wlan_cfg_set_wid(u8 *frame, u32 offset, u16 id, u8 *buf, int size);
 int wilc_wlan_cfg_get_wid(u8 *frame, u32 offset, u16 id);
 int wilc_wlan_cfg_get_wid_value(u16 wid, u8 *buffer, u32 buffer_size);
-int wilc_wlan_cfg_indicate_rx(u8 *frame, int size, wilc_cfg_rsp_t *rsp);
+int wilc_wlan_cfg_indicate_rx(struct wilc *wilc, u8 *frame, int size,
+			      struct wilc_cfg_rsp *rsp);
 int wilc_wlan_cfg_init(wilc_debug_func func);
 
 #endif
diff --git a/drivers/staging/wilc1000/wilc_wlan_if.h b/drivers/staging/wilc1000/wilc_wlan_if.h
index be972afe6e62..618903caff54 100644
--- a/drivers/staging/wilc1000/wilc_wlan_if.h
+++ b/drivers/staging/wilc1000/wilc_wlan_if.h
@@ -12,6 +12,7 @@
 
 #include <linux/semaphore.h>
 #include "linux_wlan_common.h"
+#include <linux/netdevice.h>
 
 /********************************************
  *
@@ -71,26 +72,6 @@ typedef struct {
 	u32 block_size;
 } sdio_cmd53_t;
 
-typedef struct {
-	int io_type;
-	int (*io_init)(void *);
-	void (*io_deinit)(void *);
-	union {
-		struct {
-			int (*sdio_cmd52)(sdio_cmd52_t *);
-			int (*sdio_cmd53)(sdio_cmd53_t *);
-			int (*sdio_set_max_speed)(void);
-			int (*sdio_set_default_speed)(void);
-		} sdio;
-		struct {
-			int (*spi_max_speed)(void);
-			int (*spi_tx)(u8 *, u32);
-			int (*spi_rx)(u8 *, u32);
-			int (*spi_trx)(u8 *, u8 *, u32);
-		} spi;
-	} u;
-} wilc_wlan_io_func_t;
-
 #define WILC_MAC_INDICATE_STATUS	0x1
 #define WILC_MAC_STATUS_INIT		-1
 #define WILC_MAC_STATUS_READY		0
@@ -98,15 +79,6 @@ typedef struct {
 
 #define WILC_MAC_INDICATE_SCAN		0x2
 
-typedef struct {
-	void *os_private;
-} wilc_wlan_os_context_t;
-
-typedef struct {
-	wilc_wlan_os_context_t os_context;
-	wilc_wlan_io_func_t io_func;
-} wilc_wlan_inp_t;
-
 struct tx_complete_data {
 	int size;
 	void *buff;
@@ -315,7 +287,7 @@ typedef enum {
 	SW_TRIGGER_ABORT,
 } TX_ABORT_OPTION_T;
 
-enum WID_TYPE {
+enum wid_type {
 	WID_CHAR		= 0,
 	WID_SHORT		= 1,
 	WID_INT			= 2,
@@ -937,10 +909,10 @@ typedef enum {
 	WID_MAX				= 0xFFFF
 } WID_T;
 
-int wilc_wlan_init(wilc_wlan_inp_t *inp);
-
+struct wilc;
+int wilc_wlan_init(struct net_device *dev);
 void wilc_bus_set_max_speed(void);
 void wilc_bus_set_default_speed(void);
-u32 wilc_get_chipid(u8 update);
+u32 wilc_get_chipid(struct wilc *wilc, u8 update);
 
 #endif
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 444ebed7313a..7551ac25d89d 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -177,9 +177,6 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 
 static void hfa384x_usbin_info(wlandevice_t *wlandev, hfa384x_usbin_t *usbin);
 
-static void
-hfa384x_usbout_tx(wlandevice_t *wlandev, hfa384x_usbout_t *usbout);
-
 static void hfa384x_usbin_ctlx(hfa384x_t *hw, hfa384x_usbin_t *usbin,
 			       int urb_status);
 
@@ -3504,7 +3501,7 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 		rxmeta->signal = usbin->rxfrm.desc.signal - hw->dbmadjust;
 		rxmeta->noise = usbin->rxfrm.desc.silence - hw->dbmadjust;
 
-		prism2sta_ev_rx(wlandev, skb);
+		p80211netdev_rx(wlandev, skb);
 
 		break;
 
@@ -3628,7 +3625,7 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 	}
 
 	/* pass it back up */
-	prism2sta_ev_rx(wlandev, skb);
+	p80211netdev_rx(wlandev, skb);
 }
 
 /*----------------------------------------------------------------
@@ -3674,7 +3671,6 @@ static void hfa384x_usbin_info(wlandevice_t *wlandev, hfa384x_usbin_t *usbin)
 static void hfa384x_usbout_callback(struct urb *urb)
 {
 	wlandevice_t *wlandev = urb->context;
-	hfa384x_usbout_t *usbout = urb->transfer_buffer;
 
 #ifdef DEBUG_USB
 	dbprint_urb(urb);
@@ -3683,7 +3679,7 @@ static void hfa384x_usbout_callback(struct urb *urb)
 	if (wlandev && wlandev->netdev) {
 		switch (urb->status) {
 		case 0:
-			hfa384x_usbout_tx(wlandev, usbout);
+			prism2sta_ev_alloc(wlandev);
 			break;
 
 		case -EPIPE:
@@ -4038,30 +4034,6 @@ static int hfa384x_usbctlx_submit(hfa384x_t *hw, hfa384x_usbctlx_t *ctlx)
 }
 
 /*----------------------------------------------------------------
-* hfa384x_usbout_tx
-*
-* At this point we have finished a send of a frame.  Mark the URB
-* as available and call ev_alloc to notify higher layers we're
-* ready for more.
-*
-* Arguments:
-*	wlandev		wlan device
-*	usbout		ptr to the usb transfer buffer
-*
-* Returns:
-*	nothing
-*
-* Side effects:
-*
-* Call context:
-*	interrupt
-----------------------------------------------------------------*/
-static void hfa384x_usbout_tx(wlandevice_t *wlandev, hfa384x_usbout_t *usbout)
-{
-	prism2sta_ev_alloc(wlandev);
-}
-
-/*----------------------------------------------------------------
 * hfa384x_isgood_pdrcore
 *
 * Quick check of PDR codes.
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
index c547e1cb4c0d..810ee68aa18e 100644
--- a/drivers/staging/wlan-ng/p80211netdev.h
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -141,7 +141,6 @@ typedef struct p80211_frmrx_t {
 struct iw_statistics *p80211wext_get_wireless_stats(netdevice_t *dev);
 /* wireless extensions' ioctls */
 extern struct iw_handler_def p80211wext_handler_def;
-int p80211wext_event_associated(struct wlandevice *wlandev, int assoc);
 
 /* WEP stuff */
 #define NUM_WEPKEYS 4
diff --git a/drivers/staging/wlan-ng/prism2mgmt.h b/drivers/staging/wlan-ng/prism2mgmt.h
index 16f123959104..7a9f424607b7 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.h
+++ b/drivers/staging/wlan-ng/prism2mgmt.h
@@ -68,7 +68,6 @@ u32 prism2sta_ifstate(wlandevice_t *wlandev, u32 ifstate);
 void prism2sta_ev_info(wlandevice_t *wlandev, hfa384x_InfFrame_t *inf);
 void prism2sta_ev_txexc(wlandevice_t *wlandev, u16 status);
 void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status);
-void prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb);
 void prism2sta_ev_alloc(wlandevice_t *wlandev);
 
 int prism2mgmt_mibset_mibget(wlandevice_t *wlandev, void *msgp);
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index b4a15ef3a405..cdda07d1c268 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -660,7 +660,6 @@ static int prism2mib_fragmentationthreshold(struct mibrec *mib,
 					    struct p80211msg_dot11req_mibset *msg,
 					    void *data)
 {
-	int result;
 	u32 *uint32 = (u32 *) data;
 
 	if (!isget)
@@ -672,9 +671,7 @@ static int prism2mib_fragmentationthreshold(struct mibrec *mib,
 			return 0;
 		}
 
-	result = prism2mib_uint32(mib, isget, wlandev, hw, msg, data);
-
-	return result;
+	return prism2mib_uint32(mib, isget, wlandev, hw, msg, data);
 }
 
 /*----------------------------------------------------------------
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index c57f48a1d8df..131223afd918 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -1837,27 +1837,6 @@ void prism2sta_ev_tx(wlandevice_t *wlandev, u16 status)
 }
 
 /*
- * prism2sta_ev_rx
- *
- * Handles the Rx event.
- *
- * Arguments:
- *	wlandev		wlan device structure
- *
- * Returns:
- *	nothing
- *
- * Side effects:
- *
- * Call context:
- *	interrupt
- */
-void prism2sta_ev_rx(wlandevice_t *wlandev, struct sk_buff *skb)
-{
-	p80211netdev_rx(wlandev, skb);
-}
-
-/*
  * prism2sta_ev_alloc
  *
  * Handles the Alloc event.
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 72204fbf2bb1..576a7a43470c 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -1333,7 +1333,7 @@ iscsit_check_dataout_hdr(struct iscsi_conn *conn, unsigned char *buf,
 			/*
 			 * Check if a delayed TASK_ABORTED status needs to
 			 * be sent now if the ISCSI_FLAG_CMD_FINAL has been
-			 * received with the unsolicitied data out.
+			 * received with the unsolicited data out.
 			 */
 			if (hdr->flags & ISCSI_FLAG_CMD_FINAL)
 				iscsit_stop_dataout_timer(cmd);
@@ -3435,7 +3435,7 @@ iscsit_build_sendtargets_response(struct iscsi_cmd *cmd,
 
 			if ((tpg->tpg_attrib.generate_node_acls == 0) &&
 			    (tpg->tpg_attrib.demo_mode_discovery == 0) &&
-			    (!core_tpg_get_initiator_node_acl(&tpg->tpg_se_tpg,
+			    (!target_tpg_has_node_acl(&tpg->tpg_se_tpg,
 				cmd->conn->sess->sess_ops->InitiatorName))) {
 				continue;
 			}
@@ -4459,9 +4459,6 @@ int iscsit_close_connection(
 
 		return 0;
 	}
-	spin_unlock_bh(&sess->conn_lock);
-
-	return 0;
 }
 
 int iscsit_close_session(struct iscsi_session *sess)
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index 255204cc43e6..2f821de63049 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -725,11 +725,8 @@ static ssize_t lio_target_nacl_cmdsn_depth_store(struct config_item *item,
 
 	if (iscsit_get_tpg(tpg) < 0)
 		return -EINVAL;
-	/*
-	 * iscsit_tpg_set_initiator_node_queue_depth() assumes force=1
-	 */
-	ret = iscsit_tpg_set_initiator_node_queue_depth(tpg,
-				config_item_name(acl_ci), cmdsn_depth, 1);
+
+	ret = core_tpg_set_initiator_node_queue_depth(se_nacl, cmdsn_depth);
 
 	pr_debug("LIO_Target_ConfigFS: %s/%s Set CmdSN Window: %u for"
 		"InitiatorName: %s\n", config_item_name(wwn_ci),
@@ -1593,28 +1590,30 @@ static int lio_tpg_check_prot_fabric_only(
 }
 
 /*
- * Called with spin_lock_bh(struct se_portal_group->session_lock) held..
- *
- * Also, this function calls iscsit_inc_session_usage_count() on the
+ * This function calls iscsit_inc_session_usage_count() on the
  * struct iscsi_session in question.
  */
 static int lio_tpg_shutdown_session(struct se_session *se_sess)
 {
 	struct iscsi_session *sess = se_sess->fabric_sess_ptr;
+	struct se_portal_group *se_tpg = &sess->tpg->tpg_se_tpg;
 
+	spin_lock_bh(&se_tpg->session_lock);
 	spin_lock(&sess->conn_lock);
 	if (atomic_read(&sess->session_fall_back_to_erl0) ||
 	    atomic_read(&sess->session_logout) ||
 	    (sess->time2retain_timer_flags & ISCSI_TF_EXPIRED)) {
 		spin_unlock(&sess->conn_lock);
+		spin_unlock_bh(&se_tpg->session_lock);
 		return 0;
 	}
 	atomic_set(&sess->session_reinstatement, 1);
 	spin_unlock(&sess->conn_lock);
 
 	iscsit_stop_time2retain_timer(sess);
-	iscsit_stop_session(sess, 1, 1);
+	spin_unlock_bh(&se_tpg->session_lock);
 
+	iscsit_stop_session(sess, 1, 1);
 	return 1;
 }
 
diff --git a/drivers/target/iscsi/iscsi_target_erl1.c b/drivers/target/iscsi/iscsi_target_erl1.c
index 2e561deb30a2..9214c9dafa2b 100644
--- a/drivers/target/iscsi/iscsi_target_erl1.c
+++ b/drivers/target/iscsi/iscsi_target_erl1.c
@@ -160,8 +160,7 @@ static int iscsit_handle_r2t_snack(
 			" protocol error.\n", cmd->init_task_tag, begrun,
 			(begrun + runlength), cmd->acked_data_sn);
 
-			return iscsit_reject_cmd(cmd,
-					ISCSI_REASON_PROTOCOL_ERROR, buf);
+		return iscsit_reject_cmd(cmd, ISCSI_REASON_PROTOCOL_ERROR, buf);
 	}
 
 	if (runlength) {
@@ -628,8 +627,8 @@ int iscsit_dataout_datapduinorder_no_fbit(
 			if (cmd->pdu_list[i].seq_no == pdu->seq_no) {
 				if (!first_pdu)
 					first_pdu = &cmd->pdu_list[i];
-				 xfer_len += cmd->pdu_list[i].length;
-				 pdu_count++;
+				xfer_len += cmd->pdu_list[i].length;
+				pdu_count++;
 			} else if (pdu_count)
 				break;
 		}
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index 2cbea2af7cd0..3a1f9a7e6bb6 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -1668,7 +1668,7 @@ void iscsi_set_session_parameters(
 				param->value);
 		} else if (!strcmp(param->name, INITIALR2T)) {
 			ops->InitialR2T = !strcmp(param->value, YES);
-			 pr_debug("InitialR2T:                   %s\n",
+			pr_debug("InitialR2T:                   %s\n",
 				param->value);
 		} else if (!strcmp(param->name, IMMEDIATEDATA)) {
 			ops->ImmediateData = !strcmp(param->value, YES);
diff --git a/drivers/target/iscsi/iscsi_target_tmr.c b/drivers/target/iscsi/iscsi_target_tmr.c
index 11320df939f7..3d637055c36f 100644
--- a/drivers/target/iscsi/iscsi_target_tmr.c
+++ b/drivers/target/iscsi/iscsi_target_tmr.c
@@ -82,7 +82,7 @@ int iscsit_tmr_task_warm_reset(
 		pr_err("TMR Opcode TARGET_WARM_RESET authorization"
 			" failed for Initiator Node: %s\n",
 			sess->se_sess->se_node_acl->initiatorname);
-		 return -1;
+		return -1;
 	}
 	/*
 	 * Do the real work in transport_generic_do_tmr().
diff --git a/drivers/target/iscsi/iscsi_target_tpg.c b/drivers/target/iscsi/iscsi_target_tpg.c
index 23c95cd14167..0814e5894a96 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.c
+++ b/drivers/target/iscsi/iscsi_target_tpg.c
@@ -590,16 +590,6 @@ int iscsit_tpg_del_network_portal(
 	return iscsit_tpg_release_np(tpg_np, tpg, np);
 }
 
-int iscsit_tpg_set_initiator_node_queue_depth(
-	struct iscsi_portal_group *tpg,
-	unsigned char *initiatorname,
-	u32 queue_depth,
-	int force)
-{
-	return core_tpg_set_initiator_node_queue_depth(&tpg->tpg_se_tpg,
-		initiatorname, queue_depth, force);
-}
-
 int iscsit_ta_authentication(struct iscsi_portal_group *tpg, u32 authentication)
 {
 	unsigned char buf1[256], buf2[256], *none = NULL;
diff --git a/drivers/target/iscsi/iscsi_target_tpg.h b/drivers/target/iscsi/iscsi_target_tpg.h
index 9db32bd24cd4..2da211920c18 100644
--- a/drivers/target/iscsi/iscsi_target_tpg.h
+++ b/drivers/target/iscsi/iscsi_target_tpg.h
@@ -26,8 +26,6 @@ extern struct iscsi_tpg_np *iscsit_tpg_add_network_portal(struct iscsi_portal_gr
 			int);
 extern int iscsit_tpg_del_network_portal(struct iscsi_portal_group *,
 			struct iscsi_tpg_np *);
-extern int iscsit_tpg_set_initiator_node_queue_depth(struct iscsi_portal_group *,
-			unsigned char *, u32, int);
 extern int iscsit_ta_authentication(struct iscsi_portal_group *, u32);
 extern int iscsit_ta_login_timeout(struct iscsi_portal_group *, u32);
 extern int iscsit_ta_netif_timeout(struct iscsi_portal_group *, u32);
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 4fb0eca86857..d41a5c300e31 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -1036,12 +1036,26 @@ static ssize_t tcm_loop_tpg_transport_status_store(struct config_item *item,
 	return -EINVAL;
 }
 
+static ssize_t tcm_loop_tpg_address_show(struct config_item *item,
+					 char *page)
+{
+	struct se_portal_group *se_tpg = to_tpg(item);
+	struct tcm_loop_tpg *tl_tpg = container_of(se_tpg,
+			struct tcm_loop_tpg, tl_se_tpg);
+	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
+
+	return snprintf(page, PAGE_SIZE, "%d:0:%d\n",
+			tl_hba->sh->host_no, tl_tpg->tl_tpgt);
+}
+
 CONFIGFS_ATTR(tcm_loop_tpg_, nexus);
 CONFIGFS_ATTR(tcm_loop_tpg_, transport_status);
+CONFIGFS_ATTR_RO(tcm_loop_tpg_, address);
 
 static struct configfs_attribute *tcm_loop_tpg_attrs[] = {
 	&tcm_loop_tpg_attr_nexus,
 	&tcm_loop_tpg_attr_transport_status,
+	&tcm_loop_tpg_attr_address,
 	NULL,
 };
 
diff --git a/drivers/target/sbp/sbp_target.c b/drivers/target/sbp/sbp_target.c
index 35f7d31b29d2..3072f1aca8ec 100644
--- a/drivers/target/sbp/sbp_target.c
+++ b/drivers/target/sbp/sbp_target.c
@@ -39,8 +39,6 @@
 
 #include "sbp_target.h"
 
-static const struct target_core_fabric_ops sbp_ops;
-
 /* FireWire address region for management and command block address handlers */
 static const struct fw_address_region sbp_register_region = {
 	.start	= CSR_REGISTER_BASE + 0x10000,
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index b9b9ffde4c7a..713c63d9681b 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -278,7 +278,7 @@ EXPORT_SYMBOL(target_depend_item);
 
 void target_undepend_item(struct config_item *item)
 {
-	return configfs_undepend_item(&target_core_fabrics, item);
+	return configfs_undepend_item(item);
 }
 EXPORT_SYMBOL(target_undepend_item);
 
@@ -499,6 +499,7 @@ DEF_CONFIGFS_ATTRIB_SHOW(max_unmap_lba_count);
 DEF_CONFIGFS_ATTRIB_SHOW(max_unmap_block_desc_count);
 DEF_CONFIGFS_ATTRIB_SHOW(unmap_granularity);
 DEF_CONFIGFS_ATTRIB_SHOW(unmap_granularity_alignment);
+DEF_CONFIGFS_ATTRIB_SHOW(unmap_zeroes_data);
 DEF_CONFIGFS_ATTRIB_SHOW(max_write_same_len);
 
 #define DEF_CONFIGFS_ATTRIB_STORE_U32(_name)				\
@@ -548,7 +549,8 @@ static ssize_t _name##_store(struct config_item *item, const char *page,\
 		size_t count)						\
 {									\
 	printk_once(KERN_WARNING					\
-		"ignoring deprecated ##_name## attribute\n");	\
+		"ignoring deprecated %s attribute\n",			\
+		__stringify(_name));					\
 	return count;							\
 }
 
@@ -866,6 +868,39 @@ static ssize_t emulate_rest_reord_store(struct config_item *item,
 	return count;
 }
 
+static ssize_t unmap_zeroes_data_store(struct config_item *item,
+		const char *page, size_t count)
+{
+	struct se_dev_attrib *da = to_attrib(item);
+	bool flag;
+	int ret;
+
+	ret = strtobool(page, &flag);
+	if (ret < 0)
+		return ret;
+
+	if (da->da_dev->export_count) {
+		pr_err("dev[%p]: Unable to change SE Device"
+		       " unmap_zeroes_data while export_count is %d\n",
+		       da->da_dev, da->da_dev->export_count);
+		return -EINVAL;
+	}
+	/*
+	 * We expect this value to be non-zero when generic Block Layer
+	 * Discard supported is detected iblock_configure_device().
+	 */
+	if (flag && !da->max_unmap_block_desc_count) {
+		pr_err("dev[%p]: Thin Provisioning LBPRZ will not be set"
+		       " because max_unmap_block_desc_count is zero\n",
+		       da->da_dev);
+		return -ENOSYS;
+	}
+	da->unmap_zeroes_data = flag;
+	pr_debug("dev[%p]: SE Device Thin Provisioning LBPRZ bit: %d\n",
+		 da->da_dev, flag);
+	return count;
+}
+
 /*
  * Note, this can only be called on unexported SE Device Object.
  */
@@ -998,6 +1033,7 @@ CONFIGFS_ATTR(, max_unmap_lba_count);
 CONFIGFS_ATTR(, max_unmap_block_desc_count);
 CONFIGFS_ATTR(, unmap_granularity);
 CONFIGFS_ATTR(, unmap_granularity_alignment);
+CONFIGFS_ATTR(, unmap_zeroes_data);
 CONFIGFS_ATTR(, max_write_same_len);
 
 /*
@@ -1034,6 +1070,7 @@ struct configfs_attribute *sbc_attrib_attrs[] = {
 	&attr_max_unmap_block_desc_count,
 	&attr_unmap_granularity,
 	&attr_unmap_granularity_alignment,
+	&attr_unmap_zeroes_data,
 	&attr_max_write_same_len,
 	NULL,
 };
@@ -1980,14 +2017,14 @@ static ssize_t target_dev_lba_map_store(struct config_item *item,
 	struct se_device *dev = to_device(item);
 	struct t10_alua_lba_map *lba_map = NULL;
 	struct list_head lba_list;
-	char *map_entries, *ptr;
+	char *map_entries, *orig, *ptr;
 	char state;
 	int pg_num = -1, pg;
 	int ret = 0, num = 0, pg_id, alua_state;
 	unsigned long start_lba = -1, end_lba = -1;
 	unsigned long segment_size = -1, segment_mult = -1;
 
-	map_entries = kstrdup(page, GFP_KERNEL);
+	orig = map_entries = kstrdup(page, GFP_KERNEL);
 	if (!map_entries)
 		return -ENOMEM;
 
@@ -2085,7 +2122,7 @@ out:
 	} else
 		core_alua_set_lba_map(dev, &lba_list,
 				      segment_size, segment_mult);
-	kfree(map_entries);
+	kfree(orig);
 	return count;
 }
 
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 88ea4e4f124b..da457e25717a 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -813,6 +813,8 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	dev->dev_attrib.unmap_granularity = DA_UNMAP_GRANULARITY_DEFAULT;
 	dev->dev_attrib.unmap_granularity_alignment =
 				DA_UNMAP_GRANULARITY_ALIGNMENT_DEFAULT;
+	dev->dev_attrib.unmap_zeroes_data =
+				DA_UNMAP_ZEROES_DATA_DEFAULT;
 	dev->dev_attrib.max_write_same_len = DA_MAX_WRITE_SAME_LEN;
 
 	xcopy_lun = &dev->xcopy_lun;
@@ -826,6 +828,50 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	return dev;
 }
 
+/*
+ * Check if the underlying struct block_device request_queue supports
+ * the QUEUE_FLAG_DISCARD bit for UNMAP/WRITE_SAME in SCSI + TRIM
+ * in ATA and we need to set TPE=1
+ */
+bool target_configure_unmap_from_queue(struct se_dev_attrib *attrib,
+				       struct request_queue *q, int block_size)
+{
+	if (!blk_queue_discard(q))
+		return false;
+
+	attrib->max_unmap_lba_count = (q->limits.max_discard_sectors << 9) /
+								block_size;
+	/*
+	 * Currently hardcoded to 1 in Linux/SCSI code..
+	 */
+	attrib->max_unmap_block_desc_count = 1;
+	attrib->unmap_granularity = q->limits.discard_granularity / block_size;
+	attrib->unmap_granularity_alignment = q->limits.discard_alignment /
+								block_size;
+	attrib->unmap_zeroes_data = q->limits.discard_zeroes_data;
+	return true;
+}
+EXPORT_SYMBOL(target_configure_unmap_from_queue);
+
+/*
+ * Convert from blocksize advertised to the initiator to the 512 byte
+ * units unconditionally used by the Linux block layer.
+ */
+sector_t target_to_linux_sector(struct se_device *dev, sector_t lb)
+{
+	switch (dev->dev_attrib.block_size) {
+	case 4096:
+		return lb << 3;
+	case 2048:
+		return lb << 2;
+	case 1024:
+		return lb << 1;
+	default:
+		return lb;
+	}
+}
+EXPORT_SYMBOL(target_to_linux_sector);
+
 int target_configure_device(struct se_device *dev)
 {
 	struct se_hba *hba = dev->se_hba;
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index e3195700211a..75f0f08b2a34 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -160,25 +160,11 @@ static int fd_configure_device(struct se_device *dev)
 			" block_device blocks: %llu logical_block_size: %d\n",
 			dev_size, div_u64(dev_size, fd_dev->fd_block_size),
 			fd_dev->fd_block_size);
-		/*
-		 * Check if the underlying struct block_device request_queue supports
-		 * the QUEUE_FLAG_DISCARD bit for UNMAP/WRITE_SAME in SCSI + TRIM
-		 * in ATA and we need to set TPE=1
-		 */
-		if (blk_queue_discard(q)) {
-			dev->dev_attrib.max_unmap_lba_count =
-				q->limits.max_discard_sectors;
-			/*
-			 * Currently hardcoded to 1 in Linux/SCSI code..
-			 */
-			dev->dev_attrib.max_unmap_block_desc_count = 1;
-			dev->dev_attrib.unmap_granularity =
-				q->limits.discard_granularity >> 9;
-			dev->dev_attrib.unmap_granularity_alignment =
-				q->limits.discard_alignment;
+
+		if (target_configure_unmap_from_queue(&dev->dev_attrib, q,
+						      fd_dev->fd_block_size))
 			pr_debug("IFILE: BLOCK Discard support available,"
-					" disabled by default\n");
-		}
+				 " disabled by default\n");
 		/*
 		 * Enable write same emulation for IBLOCK and use 0xFFFF as
 		 * the smaller WRITE_SAME(10) only has a two-byte block count.
@@ -490,9 +476,12 @@ fd_execute_unmap(struct se_cmd *cmd, sector_t lba, sector_t nolb)
 	if (S_ISBLK(inode->i_mode)) {
 		/* The backend is block device, use discard */
 		struct block_device *bdev = inode->i_bdev;
+		struct se_device *dev = cmd->se_dev;
 
-		ret = blkdev_issue_discard(bdev, lba,
-				nolb, GFP_KERNEL, 0);
+		ret = blkdev_issue_discard(bdev,
+					   target_to_linux_sector(dev, lba),
+					   target_to_linux_sector(dev,  nolb),
+					   GFP_KERNEL, 0);
 		if (ret < 0) {
 			pr_warn("FILEIO: blkdev_issue_discard() failed: %d\n",
 				ret);
diff --git a/drivers/target/target_core_iblock.c b/drivers/target/target_core_iblock.c
index f29c69120054..abe4eb997a84 100644
--- a/drivers/target/target_core_iblock.c
+++ b/drivers/target/target_core_iblock.c
@@ -121,27 +121,11 @@ static int iblock_configure_device(struct se_device *dev)
 	dev->dev_attrib.hw_max_sectors = queue_max_hw_sectors(q);
 	dev->dev_attrib.hw_queue_depth = q->nr_requests;
 
-	/*
-	 * Check if the underlying struct block_device request_queue supports
-	 * the QUEUE_FLAG_DISCARD bit for UNMAP/WRITE_SAME in SCSI + TRIM
-	 * in ATA and we need to set TPE=1
-	 */
-	if (blk_queue_discard(q)) {
-		dev->dev_attrib.max_unmap_lba_count =
-				q->limits.max_discard_sectors;
-
-		/*
-		 * Currently hardcoded to 1 in Linux/SCSI code..
-		 */
-		dev->dev_attrib.max_unmap_block_desc_count = 1;
-		dev->dev_attrib.unmap_granularity =
-				q->limits.discard_granularity >> 9;
-		dev->dev_attrib.unmap_granularity_alignment =
-				q->limits.discard_alignment;
-
+	if (target_configure_unmap_from_queue(&dev->dev_attrib, q,
+					      dev->dev_attrib.hw_block_size))
 		pr_debug("IBLOCK: BLOCK Discard support available,"
-				" disabled by default\n");
-	}
+			 " disabled by default\n");
+
 	/*
 	 * Enable write same emulation for IBLOCK and use 0xFFFF as
 	 * the smaller WRITE_SAME(10) only has a two-byte block count.
@@ -413,9 +397,13 @@ static sense_reason_t
 iblock_execute_unmap(struct se_cmd *cmd, sector_t lba, sector_t nolb)
 {
 	struct block_device *bdev = IBLOCK_DEV(cmd->se_dev)->ibd_bd;
+	struct se_device *dev = cmd->se_dev;
 	int ret;
 
-	ret = blkdev_issue_discard(bdev, lba, nolb, GFP_KERNEL, 0);
+	ret = blkdev_issue_discard(bdev,
+				   target_to_linux_sector(dev, lba),
+				   target_to_linux_sector(dev,  nolb),
+				   GFP_KERNEL, 0);
 	if (ret < 0) {
 		pr_err("blkdev_issue_discard() failed: %d\n", ret);
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
@@ -431,8 +419,10 @@ iblock_execute_write_same(struct se_cmd *cmd)
 	struct scatterlist *sg;
 	struct bio *bio;
 	struct bio_list list;
-	sector_t block_lba = cmd->t_task_lba;
-	sector_t sectors = sbc_get_write_same_sectors(cmd);
+	struct se_device *dev = cmd->se_dev;
+	sector_t block_lba = target_to_linux_sector(dev, cmd->t_task_lba);
+	sector_t sectors = target_to_linux_sector(dev,
+					sbc_get_write_same_sectors(cmd));
 
 	if (cmd->prot_op) {
 		pr_err("WRITE_SAME: Protection information with IBLOCK"
@@ -613,9 +603,9 @@ iblock_alloc_bip(struct se_cmd *cmd, struct bio *bio)
 	}
 
 	bip = bio_integrity_alloc(bio, GFP_NOIO, cmd->t_prot_nents);
-	if (!bip) {
+	if (IS_ERR(bip)) {
 		pr_err("Unable to allocate bio_integrity_payload\n");
-		return -ENOMEM;
+		return PTR_ERR(bip);
 	}
 
 	bip->bip_iter.bi_size = (cmd->data_length / dev->dev_attrib.block_size) *
@@ -646,12 +636,12 @@ iblock_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 		  enum dma_data_direction data_direction)
 {
 	struct se_device *dev = cmd->se_dev;
+	sector_t block_lba = target_to_linux_sector(dev, cmd->t_task_lba);
 	struct iblock_req *ibr;
 	struct bio *bio, *bio_start;
 	struct bio_list list;
 	struct scatterlist *sg;
 	u32 sg_num = sgl_nents;
-	sector_t block_lba;
 	unsigned bio_cnt;
 	int rw = 0;
 	int i;
@@ -677,24 +667,6 @@ iblock_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 		rw = READ;
 	}
 
-	/*
-	 * Convert the blocksize advertised to the initiator to the 512 byte
-	 * units unconditionally used by the Linux block layer.
-	 */
-	if (dev->dev_attrib.block_size == 4096)
-		block_lba = (cmd->t_task_lba << 3);
-	else if (dev->dev_attrib.block_size == 2048)
-		block_lba = (cmd->t_task_lba << 2);
-	else if (dev->dev_attrib.block_size == 1024)
-		block_lba = (cmd->t_task_lba << 1);
-	else if (dev->dev_attrib.block_size == 512)
-		block_lba = cmd->t_task_lba;
-	else {
-		pr_err("Unsupported SCSI -> BLOCK LBA conversion:"
-				" %u\n", dev->dev_attrib.block_size);
-		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
-	}
-
 	ibr = kzalloc(sizeof(struct iblock_req), GFP_KERNEL);
 	if (!ibr)
 		goto fail;
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index dae0750c2032..db4412fe6b8a 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -141,7 +141,6 @@ void	transport_dump_vpd_proto_id(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_assoc(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident_type(struct t10_vpd *, unsigned char *, int);
 int	transport_dump_vpd_ident(struct t10_vpd *, unsigned char *, int);
-bool	target_stop_cmd(struct se_cmd *cmd, unsigned long *flags);
 void	transport_clear_lun_ref(struct se_lun *);
 void	transport_send_task_abort(struct se_cmd *);
 sense_reason_t	target_cmd_size_check(struct se_cmd *cmd, unsigned int size);
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index e7933115087a..b1795735eafc 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -1457,8 +1457,7 @@ static void core_scsi3_nodeacl_undepend_item(struct se_node_acl *nacl)
 static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
 {
 	struct se_lun_acl *lun_acl;
-	struct se_node_acl *nacl;
-	struct se_portal_group *tpg;
+
 	/*
 	 * For nacl->dynamic_node_acl=1
 	 */
@@ -1467,17 +1466,13 @@ static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
 	if (!lun_acl)
 		return 0;
 
-	nacl = lun_acl->se_lun_nacl;
-	tpg = nacl->se_tpg;
-
 	return target_depend_item(&lun_acl->se_lun_group.cg_item);
 }
 
 static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 {
 	struct se_lun_acl *lun_acl;
-	struct se_node_acl *nacl;
-	struct se_portal_group *tpg;
+
 	/*
 	 * For nacl->dynamic_node_acl=1
 	 */
@@ -1487,8 +1482,6 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 		kref_put(&se_deve->pr_kref, target_pr_kref_release);
 		return;
 	}
-	nacl = lun_acl->se_lun_nacl;
-	tpg = nacl->se_tpg;
 
 	target_undepend_item(&lun_acl->se_lun_group.cg_item);
 	kref_put(&se_deve->pr_kref, target_pr_kref_release);
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index 98698d875742..a9057aa07176 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -141,9 +141,17 @@ sbc_emulate_readcapacity_16(struct se_cmd *cmd)
 	 * Set Thin Provisioning Enable bit following sbc3r22 in section
 	 * READ CAPACITY (16) byte 14 if emulate_tpu or emulate_tpws is enabled.
 	 */
-	if (dev->dev_attrib.emulate_tpu || dev->dev_attrib.emulate_tpws)
+	if (dev->dev_attrib.emulate_tpu || dev->dev_attrib.emulate_tpws) {
 		buf[14] |= 0x80;
 
+		/*
+		 * LBPRZ signifies that zeroes will be read back from an LBA after
+		 * an UNMAP or WRITE SAME w/ unmap bit (sbc3r36 5.16.2)
+		 */
+		if (dev->dev_attrib.unmap_zeroes_data)
+			buf[14] |= 0x40;
+	}
+
 	rbuf = transport_kmap_data_sg(cmd);
 	if (rbuf) {
 		memcpy(rbuf, buf, min_t(u32, sizeof(buf), cmd->data_length));
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 9413e1a949e5..0aa47babd16c 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -635,6 +635,18 @@ spc_emulate_evpd_b2(struct se_cmd *cmd, unsigned char *buf)
 	if (dev->dev_attrib.emulate_tpws != 0)
 		buf[5] |= 0x40 | 0x20;
 
+	/*
+	 * The unmap_zeroes_data set means that the underlying device supports
+	 * REQ_DISCARD and has the discard_zeroes_data bit set. This satisfies
+	 * the SBC requirements for LBPRZ, meaning that a subsequent read
+	 * will return zeroes after an UNMAP or WRITE SAME (16) to an LBA
+	 * See sbc4r36 6.6.4.
+	 */
+	if (((dev->dev_attrib.emulate_tpu != 0) ||
+	     (dev->dev_attrib.emulate_tpws != 0)) &&
+	     (dev->dev_attrib.unmap_zeroes_data != 0))
+		buf[5] |= 0x04;
+
 	return 0;
 }
 
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index 28fb3016370f..4f229e711e1c 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -68,23 +68,25 @@ void core_tmr_release_req(struct se_tmr_req *tmr)
 
 	if (dev) {
 		spin_lock_irqsave(&dev->se_tmr_lock, flags);
-		list_del(&tmr->tmr_list);
+		list_del_init(&tmr->tmr_list);
 		spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
 	}
 
 	kfree(tmr);
 }
 
-static void core_tmr_handle_tas_abort(
-	struct se_node_acl *tmr_nacl,
-	struct se_cmd *cmd,
-	int tas)
+static void core_tmr_handle_tas_abort(struct se_cmd *cmd, int tas)
 {
-	bool remove = true;
+	unsigned long flags;
+	bool remove = true, send_tas;
 	/*
 	 * TASK ABORTED status (TAS) bit support
 	 */
-	if ((tmr_nacl && (tmr_nacl != cmd->se_sess->se_node_acl)) && tas) {
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	send_tas = (cmd->transport_state & CMD_T_TAS);
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+
+	if (send_tas) {
 		remove = false;
 		transport_send_task_abort(cmd);
 	}
@@ -107,6 +109,46 @@ static int target_check_cdb_and_preempt(struct list_head *list,
 	return 1;
 }
 
+static bool __target_check_io_state(struct se_cmd *se_cmd,
+				    struct se_session *tmr_sess, int tas)
+{
+	struct se_session *sess = se_cmd->se_sess;
+
+	assert_spin_locked(&sess->sess_cmd_lock);
+	WARN_ON_ONCE(!irqs_disabled());
+	/*
+	 * If command already reached CMD_T_COMPLETE state within
+	 * target_complete_cmd() or CMD_T_FABRIC_STOP due to shutdown,
+	 * this se_cmd has been passed to fabric driver and will
+	 * not be aborted.
+	 *
+	 * Otherwise, obtain a local se_cmd->cmd_kref now for TMR
+	 * ABORT_TASK + LUN_RESET for CMD_T_ABORTED processing as
+	 * long as se_cmd->cmd_kref is still active unless zero.
+	 */
+	spin_lock(&se_cmd->t_state_lock);
+	if (se_cmd->transport_state & (CMD_T_COMPLETE | CMD_T_FABRIC_STOP)) {
+		pr_debug("Attempted to abort io tag: %llu already complete or"
+			" fabric stop, skipping\n", se_cmd->tag);
+		spin_unlock(&se_cmd->t_state_lock);
+		return false;
+	}
+	if (sess->sess_tearing_down || se_cmd->cmd_wait_set) {
+		pr_debug("Attempted to abort io tag: %llu already shutdown,"
+			" skipping\n", se_cmd->tag);
+		spin_unlock(&se_cmd->t_state_lock);
+		return false;
+	}
+	se_cmd->transport_state |= CMD_T_ABORTED;
+
+	if ((tmr_sess != se_cmd->se_sess) && tas)
+		se_cmd->transport_state |= CMD_T_TAS;
+
+	spin_unlock(&se_cmd->t_state_lock);
+
+	return kref_get_unless_zero(&se_cmd->cmd_kref);
+}
+
 void core_tmr_abort_task(
 	struct se_device *dev,
 	struct se_tmr_req *tmr,
@@ -130,34 +172,21 @@ void core_tmr_abort_task(
 		if (tmr->ref_task_tag != ref_tag)
 			continue;
 
-		if (!kref_get_unless_zero(&se_cmd->cmd_kref))
-			continue;
-
 		printk("ABORT_TASK: Found referenced %s task_tag: %llu\n",
 			se_cmd->se_tfo->get_fabric_name(), ref_tag);
 
-		spin_lock(&se_cmd->t_state_lock);
-		if (se_cmd->transport_state & CMD_T_COMPLETE) {
-			printk("ABORT_TASK: ref_tag: %llu already complete,"
-			       " skipping\n", ref_tag);
-			spin_unlock(&se_cmd->t_state_lock);
+		if (!__target_check_io_state(se_cmd, se_sess, 0)) {
 			spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
-
-			target_put_sess_cmd(se_cmd);
-
 			goto out;
 		}
-		se_cmd->transport_state |= CMD_T_ABORTED;
-		spin_unlock(&se_cmd->t_state_lock);
-
 		list_del_init(&se_cmd->se_cmd_list);
 		spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 
 		cancel_work_sync(&se_cmd->work);
 		transport_wait_for_tasks(se_cmd);
 
-		target_put_sess_cmd(se_cmd);
 		transport_cmd_finish_abort(se_cmd, true);
+		target_put_sess_cmd(se_cmd);
 
 		printk("ABORT_TASK: Sending TMR_FUNCTION_COMPLETE for"
 				" ref_tag: %llu\n", ref_tag);
@@ -178,9 +207,11 @@ static void core_tmr_drain_tmr_list(
 	struct list_head *preempt_and_abort_list)
 {
 	LIST_HEAD(drain_tmr_list);
+	struct se_session *sess;
 	struct se_tmr_req *tmr_p, *tmr_pp;
 	struct se_cmd *cmd;
 	unsigned long flags;
+	bool rc;
 	/*
 	 * Release all pending and outgoing TMRs aside from the received
 	 * LUN_RESET tmr..
@@ -201,22 +232,44 @@ static void core_tmr_drain_tmr_list(
 		/*
 		 * If this function was called with a valid pr_res_key
 		 * parameter (eg: for PROUT PREEMPT_AND_ABORT service action
-		 * skip non regisration key matching TMRs.
+		 * skip non registration key matching TMRs.
 		 */
 		if (target_check_cdb_and_preempt(preempt_and_abort_list, cmd))
 			continue;
 
+		sess = cmd->se_sess;
+		if (WARN_ON_ONCE(!sess))
+			continue;
+
+		spin_lock(&sess->sess_cmd_lock);
 		spin_lock(&cmd->t_state_lock);
-		if (!(cmd->transport_state & CMD_T_ACTIVE)) {
+		if (!(cmd->transport_state & CMD_T_ACTIVE) ||
+		     (cmd->transport_state & CMD_T_FABRIC_STOP)) {
 			spin_unlock(&cmd->t_state_lock);
+			spin_unlock(&sess->sess_cmd_lock);
 			continue;
 		}
 		if (cmd->t_state == TRANSPORT_ISTATE_PROCESSING) {
 			spin_unlock(&cmd->t_state_lock);
+			spin_unlock(&sess->sess_cmd_lock);
 			continue;
 		}
+		if (sess->sess_tearing_down || cmd->cmd_wait_set) {
+			spin_unlock(&cmd->t_state_lock);
+			spin_unlock(&sess->sess_cmd_lock);
+			continue;
+		}
+		cmd->transport_state |= CMD_T_ABORTED;
 		spin_unlock(&cmd->t_state_lock);
 
+		rc = kref_get_unless_zero(&cmd->cmd_kref);
+		if (!rc) {
+			printk("LUN_RESET TMR: non-zero kref_get_unless_zero\n");
+			spin_unlock(&sess->sess_cmd_lock);
+			continue;
+		}
+		spin_unlock(&sess->sess_cmd_lock);
+
 		list_move_tail(&tmr_p->tmr_list, &drain_tmr_list);
 	}
 	spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
@@ -230,20 +283,26 @@ static void core_tmr_drain_tmr_list(
 			(preempt_and_abort_list) ? "Preempt" : "", tmr_p,
 			tmr_p->function, tmr_p->response, cmd->t_state);
 
+		cancel_work_sync(&cmd->work);
+		transport_wait_for_tasks(cmd);
+
 		transport_cmd_finish_abort(cmd, 1);
+		target_put_sess_cmd(cmd);
 	}
 }
 
 static void core_tmr_drain_state_list(
 	struct se_device *dev,
 	struct se_cmd *prout_cmd,
-	struct se_node_acl *tmr_nacl,
+	struct se_session *tmr_sess,
 	int tas,
 	struct list_head *preempt_and_abort_list)
 {
 	LIST_HEAD(drain_task_list);
+	struct se_session *sess;
 	struct se_cmd *cmd, *next;
 	unsigned long flags;
+	int rc;
 
 	/*
 	 * Complete outstanding commands with TASK_ABORTED SAM status.
@@ -282,6 +341,16 @@ static void core_tmr_drain_state_list(
 		if (prout_cmd == cmd)
 			continue;
 
+		sess = cmd->se_sess;
+		if (WARN_ON_ONCE(!sess))
+			continue;
+
+		spin_lock(&sess->sess_cmd_lock);
+		rc = __target_check_io_state(cmd, tmr_sess, tas);
+		spin_unlock(&sess->sess_cmd_lock);
+		if (!rc)
+			continue;
+
 		list_move_tail(&cmd->state_list, &drain_task_list);
 		cmd->state_active = false;
 	}
@@ -289,7 +358,7 @@ static void core_tmr_drain_state_list(
 
 	while (!list_empty(&drain_task_list)) {
 		cmd = list_entry(drain_task_list.next, struct se_cmd, state_list);
-		list_del(&cmd->state_list);
+		list_del_init(&cmd->state_list);
 
 		pr_debug("LUN_RESET: %s cmd: %p"
 			" ITT/CmdSN: 0x%08llx/0x%08x, i_state: %d, t_state: %d"
@@ -313,16 +382,11 @@ static void core_tmr_drain_state_list(
 		 * loop above, but we do it down here given that
 		 * cancel_work_sync may block.
 		 */
-		if (cmd->t_state == TRANSPORT_COMPLETE)
-			cancel_work_sync(&cmd->work);
-
-		spin_lock_irqsave(&cmd->t_state_lock, flags);
-		target_stop_cmd(cmd, &flags);
-
-		cmd->transport_state |= CMD_T_ABORTED;
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+		cancel_work_sync(&cmd->work);
+		transport_wait_for_tasks(cmd);
 
-		core_tmr_handle_tas_abort(tmr_nacl, cmd, tas);
+		core_tmr_handle_tas_abort(cmd, tas);
+		target_put_sess_cmd(cmd);
 	}
 }
 
@@ -334,6 +398,7 @@ int core_tmr_lun_reset(
 {
 	struct se_node_acl *tmr_nacl = NULL;
 	struct se_portal_group *tmr_tpg = NULL;
+	struct se_session *tmr_sess = NULL;
 	int tas;
         /*
 	 * TASK_ABORTED status bit, this is configurable via ConfigFS
@@ -352,8 +417,9 @@ int core_tmr_lun_reset(
 	 * or struct se_device passthrough..
 	 */
 	if (tmr && tmr->task_cmd && tmr->task_cmd->se_sess) {
-		tmr_nacl = tmr->task_cmd->se_sess->se_node_acl;
-		tmr_tpg = tmr->task_cmd->se_sess->se_tpg;
+		tmr_sess = tmr->task_cmd->se_sess;
+		tmr_nacl = tmr_sess->se_node_acl;
+		tmr_tpg = tmr_sess->se_tpg;
 		if (tmr_nacl && tmr_tpg) {
 			pr_debug("LUN_RESET: TMR caller fabric: %s"
 				" initiator port %s\n",
@@ -366,7 +432,7 @@ int core_tmr_lun_reset(
 		dev->transport->name, tas);
 
 	core_tmr_drain_tmr_list(dev, tmr, preempt_and_abort_list);
-	core_tmr_drain_state_list(dev, prout_cmd, tmr_nacl, tas,
+	core_tmr_drain_state_list(dev, prout_cmd, tmr_sess, tas,
 				preempt_and_abort_list);
 
 	/*
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index 5fb9dd7f08bb..3608b1b5ecf7 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -75,9 +75,21 @@ struct se_node_acl *core_tpg_get_initiator_node_acl(
 	unsigned char *initiatorname)
 {
 	struct se_node_acl *acl;
-
+	/*
+	 * Obtain se_node_acl->acl_kref using fabric driver provided
+	 * initiatorname[] during node acl endpoint lookup driven by
+	 * new se_session login.
+	 *
+	 * The reference is held until se_session shutdown -> release
+	 * occurs via fabric driver invoked transport_deregister_session()
+	 * or transport_free_session() code.
+	 */
 	mutex_lock(&tpg->acl_node_mutex);
 	acl = __core_tpg_get_initiator_node_acl(tpg, initiatorname);
+	if (acl) {
+		if (!kref_get_unless_zero(&acl->acl_kref))
+			acl = NULL;
+	}
 	mutex_unlock(&tpg->acl_node_mutex);
 
 	return acl;
@@ -157,28 +169,25 @@ void core_tpg_add_node_to_devs(
 	mutex_unlock(&tpg->tpg_lun_mutex);
 }
 
-/*      core_set_queue_depth_for_node():
- *
- *
- */
-static int core_set_queue_depth_for_node(
-	struct se_portal_group *tpg,
-	struct se_node_acl *acl)
+static void
+target_set_nacl_queue_depth(struct se_portal_group *tpg,
+			    struct se_node_acl *acl, u32 queue_depth)
 {
+	acl->queue_depth = queue_depth;
+
 	if (!acl->queue_depth) {
-		pr_err("Queue depth for %s Initiator Node: %s is 0,"
+		pr_warn("Queue depth for %s Initiator Node: %s is 0,"
 			"defaulting to 1.\n", tpg->se_tpg_tfo->get_fabric_name(),
 			acl->initiatorname);
 		acl->queue_depth = 1;
 	}
-
-	return 0;
 }
 
 static struct se_node_acl *target_alloc_node_acl(struct se_portal_group *tpg,
 		const unsigned char *initiatorname)
 {
 	struct se_node_acl *acl;
+	u32 queue_depth;
 
 	acl = kzalloc(max(sizeof(*acl), tpg->se_tpg_tfo->node_acl_size),
 			GFP_KERNEL);
@@ -193,24 +202,20 @@ static struct se_node_acl *target_alloc_node_acl(struct se_portal_group *tpg,
 	spin_lock_init(&acl->nacl_sess_lock);
 	mutex_init(&acl->lun_entry_mutex);
 	atomic_set(&acl->acl_pr_ref_count, 0);
+
 	if (tpg->se_tpg_tfo->tpg_get_default_depth)
-		acl->queue_depth = tpg->se_tpg_tfo->tpg_get_default_depth(tpg);
+		queue_depth = tpg->se_tpg_tfo->tpg_get_default_depth(tpg);
 	else
-		acl->queue_depth = 1;
+		queue_depth = 1;
+	target_set_nacl_queue_depth(tpg, acl, queue_depth);
+
 	snprintf(acl->initiatorname, TRANSPORT_IQN_LEN, "%s", initiatorname);
 	acl->se_tpg = tpg;
 	acl->acl_index = scsi_get_new_index(SCSI_AUTH_INTR_INDEX);
 
 	tpg->se_tpg_tfo->set_default_node_attributes(acl);
 
-	if (core_set_queue_depth_for_node(tpg, acl) < 0)
-		goto out_free_acl;
-
 	return acl;
-
-out_free_acl:
-	kfree(acl);
-	return NULL;
 }
 
 static void target_add_node_acl(struct se_node_acl *acl)
@@ -219,7 +224,6 @@ static void target_add_node_acl(struct se_node_acl *acl)
 
 	mutex_lock(&tpg->acl_node_mutex);
 	list_add_tail(&acl->acl_list, &tpg->acl_node_list);
-	tpg->num_node_acls++;
 	mutex_unlock(&tpg->acl_node_mutex);
 
 	pr_debug("%s_TPG[%hu] - Added %s ACL with TCQ Depth: %d for %s"
@@ -232,6 +236,25 @@ static void target_add_node_acl(struct se_node_acl *acl)
 		acl->initiatorname);
 }
 
+bool target_tpg_has_node_acl(struct se_portal_group *tpg,
+			     const char *initiatorname)
+{
+	struct se_node_acl *acl;
+	bool found = false;
+
+	mutex_lock(&tpg->acl_node_mutex);
+	list_for_each_entry(acl, &tpg->acl_node_list, acl_list) {
+		if (!strcmp(acl->initiatorname, initiatorname)) {
+			found = true;
+			break;
+		}
+	}
+	mutex_unlock(&tpg->acl_node_mutex);
+
+	return found;
+}
+EXPORT_SYMBOL(target_tpg_has_node_acl);
+
 struct se_node_acl *core_tpg_check_initiator_node_acl(
 	struct se_portal_group *tpg,
 	unsigned char *initiatorname)
@@ -248,6 +271,15 @@ struct se_node_acl *core_tpg_check_initiator_node_acl(
 	acl = target_alloc_node_acl(tpg, initiatorname);
 	if (!acl)
 		return NULL;
+	/*
+	 * When allocating a dynamically generated node_acl, go ahead
+	 * and take the extra kref now before returning to the fabric
+	 * driver caller.
+	 *
+	 * Note this reference will be released at session shutdown
+	 * time within transport_free_session() code.
+	 */
+	kref_get(&acl->acl_kref);
 	acl->dynamic_node_acl = 1;
 
 	/*
@@ -318,7 +350,6 @@ void core_tpg_del_initiator_node_acl(struct se_node_acl *acl)
 		acl->dynamic_node_acl = 0;
 	}
 	list_del(&acl->acl_list);
-	tpg->num_node_acls--;
 	mutex_unlock(&tpg->acl_node_mutex);
 
 	spin_lock_irqsave(&acl->nacl_sess_lock, flags);
@@ -329,7 +360,8 @@ void core_tpg_del_initiator_node_acl(struct se_node_acl *acl)
 		if (sess->sess_tearing_down != 0)
 			continue;
 
-		target_get_session(sess);
+		if (!target_get_session(sess))
+			continue;
 		list_move(&sess->sess_acl_list, &sess_list);
 	}
 	spin_unlock_irqrestore(&acl->nacl_sess_lock, flags);
@@ -366,108 +398,52 @@ void core_tpg_del_initiator_node_acl(struct se_node_acl *acl)
  *
  */
 int core_tpg_set_initiator_node_queue_depth(
-	struct se_portal_group *tpg,
-	unsigned char *initiatorname,
-	u32 queue_depth,
-	int force)
+	struct se_node_acl *acl,
+	u32 queue_depth)
 {
-	struct se_session *sess, *init_sess = NULL;
-	struct se_node_acl *acl;
+	LIST_HEAD(sess_list);
+	struct se_portal_group *tpg = acl->se_tpg;
+	struct se_session *sess, *sess_tmp;
 	unsigned long flags;
-	int dynamic_acl = 0;
-
-	mutex_lock(&tpg->acl_node_mutex);
-	acl = __core_tpg_get_initiator_node_acl(tpg, initiatorname);
-	if (!acl) {
-		pr_err("Access Control List entry for %s Initiator"
-			" Node %s does not exists for TPG %hu, ignoring"
-			" request.\n", tpg->se_tpg_tfo->get_fabric_name(),
-			initiatorname, tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		mutex_unlock(&tpg->acl_node_mutex);
-		return -ENODEV;
-	}
-	if (acl->dynamic_node_acl) {
-		acl->dynamic_node_acl = 0;
-		dynamic_acl = 1;
-	}
-	mutex_unlock(&tpg->acl_node_mutex);
-
-	spin_lock_irqsave(&tpg->session_lock, flags);
-	list_for_each_entry(sess, &tpg->tpg_sess_list, sess_list) {
-		if (sess->se_node_acl != acl)
-			continue;
-
-		if (!force) {
-			pr_err("Unable to change queue depth for %s"
-				" Initiator Node: %s while session is"
-				" operational.  To forcefully change the queue"
-				" depth and force session reinstatement"
-				" use the \"force=1\" parameter.\n",
-				tpg->se_tpg_tfo->get_fabric_name(), initiatorname);
-			spin_unlock_irqrestore(&tpg->session_lock, flags);
-
-			mutex_lock(&tpg->acl_node_mutex);
-			if (dynamic_acl)
-				acl->dynamic_node_acl = 1;
-			mutex_unlock(&tpg->acl_node_mutex);
-			return -EEXIST;
-		}
-		/*
-		 * Determine if the session needs to be closed by our context.
-		 */
-		if (!tpg->se_tpg_tfo->shutdown_session(sess))
-			continue;
-
-		init_sess = sess;
-		break;
-	}
+	int rc;
 
 	/*
 	 * User has requested to change the queue depth for a Initiator Node.
 	 * Change the value in the Node's struct se_node_acl, and call
-	 * core_set_queue_depth_for_node() to add the requested queue depth.
-	 *
-	 * Finally call  tpg->se_tpg_tfo->close_session() to force session
-	 * reinstatement to occur if there is an active session for the
-	 * $FABRIC_MOD Initiator Node in question.
+	 * target_set_nacl_queue_depth() to set the new queue depth.
 	 */
-	acl->queue_depth = queue_depth;
+	target_set_nacl_queue_depth(tpg, acl, queue_depth);
+
+	spin_lock_irqsave(&acl->nacl_sess_lock, flags);
+	list_for_each_entry_safe(sess, sess_tmp, &acl->acl_sess_list,
+				 sess_acl_list) {
+		if (sess->sess_tearing_down != 0)
+			continue;
+		if (!target_get_session(sess))
+			continue;
+		spin_unlock_irqrestore(&acl->nacl_sess_lock, flags);
 
-	if (core_set_queue_depth_for_node(tpg, acl) < 0) {
-		spin_unlock_irqrestore(&tpg->session_lock, flags);
 		/*
-		 * Force session reinstatement if
-		 * core_set_queue_depth_for_node() failed, because we assume
-		 * the $FABRIC_MOD has already the set session reinstatement
-		 * bit from tpg->se_tpg_tfo->shutdown_session() called above.
+		 * Finally call tpg->se_tpg_tfo->close_session() to force session
+		 * reinstatement to occur if there is an active session for the
+		 * $FABRIC_MOD Initiator Node in question.
 		 */
-		if (init_sess)
-			tpg->se_tpg_tfo->close_session(init_sess);
-
-		mutex_lock(&tpg->acl_node_mutex);
-		if (dynamic_acl)
-			acl->dynamic_node_acl = 1;
-		mutex_unlock(&tpg->acl_node_mutex);
-		return -EINVAL;
+		rc = tpg->se_tpg_tfo->shutdown_session(sess);
+		target_put_session(sess);
+		if (!rc) {
+			spin_lock_irqsave(&acl->nacl_sess_lock, flags);
+			continue;
+		}
+		target_put_session(sess);
+		spin_lock_irqsave(&acl->nacl_sess_lock, flags);
 	}
-	spin_unlock_irqrestore(&tpg->session_lock, flags);
-	/*
-	 * If the $FABRIC_MOD session for the Initiator Node ACL exists,
-	 * forcefully shutdown the $FABRIC_MOD session/nexus.
-	 */
-	if (init_sess)
-		tpg->se_tpg_tfo->close_session(init_sess);
+	spin_unlock_irqrestore(&acl->nacl_sess_lock, flags);
 
 	pr_debug("Successfully changed queue depth to: %d for Initiator"
-		" Node: %s on %s Target Portal Group: %u\n", queue_depth,
-		initiatorname, tpg->se_tpg_tfo->get_fabric_name(),
+		" Node: %s on %s Target Portal Group: %u\n", acl->queue_depth,
+		acl->initiatorname, tpg->se_tpg_tfo->get_fabric_name(),
 		tpg->se_tpg_tfo->tpg_get_tag(tpg));
 
-	mutex_lock(&tpg->acl_node_mutex);
-	if (dynamic_acl)
-		acl->dynamic_node_acl = 1;
-	mutex_unlock(&tpg->acl_node_mutex);
-
 	return 0;
 }
 EXPORT_SYMBOL(core_tpg_set_initiator_node_queue_depth);
@@ -595,7 +571,6 @@ int core_tpg_deregister(struct se_portal_group *se_tpg)
 	 */
 	list_for_each_entry_safe(nacl, nacl_tmp, &node_list, acl_list) {
 		list_del(&nacl->acl_list);
-		se_tpg->num_node_acls--;
 
 		core_tpg_wait_for_nacl_pr_ref(nacl);
 		core_free_device_list_for_node(nacl, se_tpg);
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 4fdcee2006d1..867bc6d0a68a 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -341,7 +341,6 @@ void __transport_register_session(
 					&buf[0], PR_REG_ISID_LEN);
 			se_sess->sess_bin_isid = get_unaligned_be64(&buf[0]);
 		}
-		kref_get(&se_nacl->acl_kref);
 
 		spin_lock_irq(&se_nacl->nacl_sess_lock);
 		/*
@@ -384,9 +383,9 @@ static void target_release_session(struct kref *kref)
 	se_tpg->se_tpg_tfo->close_session(se_sess);
 }
 
-void target_get_session(struct se_session *se_sess)
+int target_get_session(struct se_session *se_sess)
 {
-	kref_get(&se_sess->sess_kref);
+	return kref_get_unless_zero(&se_sess->sess_kref);
 }
 EXPORT_SYMBOL(target_get_session);
 
@@ -432,6 +431,7 @@ void target_put_nacl(struct se_node_acl *nacl)
 {
 	kref_put(&nacl->acl_kref, target_complete_nacl);
 }
+EXPORT_SYMBOL(target_put_nacl);
 
 void transport_deregister_session_configfs(struct se_session *se_sess)
 {
@@ -464,6 +464,15 @@ EXPORT_SYMBOL(transport_deregister_session_configfs);
 
 void transport_free_session(struct se_session *se_sess)
 {
+	struct se_node_acl *se_nacl = se_sess->se_node_acl;
+	/*
+	 * Drop the se_node_acl->nacl_kref obtained from within
+	 * core_tpg_get_initiator_node_acl().
+	 */
+	if (se_nacl) {
+		se_sess->se_node_acl = NULL;
+		target_put_nacl(se_nacl);
+	}
 	if (se_sess->sess_cmd_map) {
 		percpu_ida_destroy(&se_sess->sess_tag_pool);
 		kvfree(se_sess->sess_cmd_map);
@@ -478,7 +487,7 @@ void transport_deregister_session(struct se_session *se_sess)
 	const struct target_core_fabric_ops *se_tfo;
 	struct se_node_acl *se_nacl;
 	unsigned long flags;
-	bool comp_nacl = true, drop_nacl = false;
+	bool drop_nacl = false;
 
 	if (!se_tpg) {
 		transport_free_session(se_sess);
@@ -502,7 +511,6 @@ void transport_deregister_session(struct se_session *se_sess)
 	if (se_nacl && se_nacl->dynamic_node_acl) {
 		if (!se_tfo->tpg_check_demo_mode_cache(se_tpg)) {
 			list_del(&se_nacl->acl_list);
-			se_tpg->num_node_acls--;
 			drop_nacl = true;
 		}
 	}
@@ -511,26 +519,21 @@ void transport_deregister_session(struct se_session *se_sess)
 	if (drop_nacl) {
 		core_tpg_wait_for_nacl_pr_ref(se_nacl);
 		core_free_device_list_for_node(se_nacl, se_tpg);
+		se_sess->se_node_acl = NULL;
 		kfree(se_nacl);
-		comp_nacl = false;
 	}
 	pr_debug("TARGET_CORE[%s]: Deregistered fabric_sess\n",
 		se_tpg->se_tpg_tfo->get_fabric_name());
 	/*
 	 * If last kref is dropping now for an explicit NodeACL, awake sleeping
 	 * ->acl_free_comp caller to wakeup configfs se_node_acl->acl_group
-	 * removal context.
+	 * removal context from within transport_free_session() code.
 	 */
-	if (se_nacl && comp_nacl)
-		target_put_nacl(se_nacl);
 
 	transport_free_session(se_sess);
 }
 EXPORT_SYMBOL(transport_deregister_session);
 
-/*
- * Called with cmd->t_state_lock held.
- */
 static void target_remove_from_state_list(struct se_cmd *cmd)
 {
 	struct se_device *dev = cmd->se_dev;
@@ -555,10 +558,6 @@ static int transport_cmd_check_stop(struct se_cmd *cmd, bool remove_from_lists,
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
-	if (write_pending)
-		cmd->t_state = TRANSPORT_WRITE_PENDING;
-
 	if (remove_from_lists) {
 		target_remove_from_state_list(cmd);
 
@@ -568,6 +567,10 @@ static int transport_cmd_check_stop(struct se_cmd *cmd, bool remove_from_lists,
 		cmd->se_lun = NULL;
 	}
 
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	if (write_pending)
+		cmd->t_state = TRANSPORT_WRITE_PENDING;
+
 	/*
 	 * Determine if frontend context caller is requesting the stopping of
 	 * this command for frontend exceptions.
@@ -621,6 +624,8 @@ static void transport_lun_remove_cmd(struct se_cmd *cmd)
 
 void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 {
+	bool ack_kref = (cmd->se_cmd_flags & SCF_ACK_KREF);
+
 	if (cmd->se_cmd_flags & SCF_SE_LUN_CMD)
 		transport_lun_remove_cmd(cmd);
 	/*
@@ -632,7 +637,7 @@ void transport_cmd_finish_abort(struct se_cmd *cmd, int remove)
 
 	if (transport_cmd_check_stop_to_fabric(cmd))
 		return;
-	if (remove)
+	if (remove && ack_kref)
 		transport_put_cmd(cmd);
 }
 
@@ -688,19 +693,10 @@ void target_complete_cmd(struct se_cmd *cmd, u8 scsi_status)
 	}
 
 	/*
-	 * See if we are waiting to complete for an exception condition.
-	 */
-	if (cmd->transport_state & CMD_T_REQUEST_STOP) {
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		complete(&cmd->task_stop_comp);
-		return;
-	}
-
-	/*
 	 * Check for case where an explicit ABORT_TASK has been received
 	 * and transport_wait_for_tasks() will be waiting for completion..
 	 */
-	if (cmd->transport_state & CMD_T_ABORTED &&
+	if (cmd->transport_state & CMD_T_ABORTED ||
 	    cmd->transport_state & CMD_T_STOP) {
 		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 		complete_all(&cmd->t_transport_stop_comp);
@@ -715,7 +711,10 @@ void target_complete_cmd(struct se_cmd *cmd, u8 scsi_status)
 	cmd->transport_state |= (CMD_T_COMPLETE | CMD_T_ACTIVE);
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	queue_work(target_completion_wq, &cmd->work);
+	if (cmd->se_cmd_flags & SCF_USE_CPUID)
+		queue_work_on(cmd->cpuid, target_completion_wq, &cmd->work);
+	else
+		queue_work(target_completion_wq, &cmd->work);
 }
 EXPORT_SYMBOL(target_complete_cmd);
 
@@ -1194,7 +1193,6 @@ void transport_init_se_cmd(
 	INIT_LIST_HEAD(&cmd->state_list);
 	init_completion(&cmd->t_transport_stop_comp);
 	init_completion(&cmd->cmd_wait_comp);
-	init_completion(&cmd->task_stop_comp);
 	spin_lock_init(&cmd->t_state_lock);
 	kref_init(&cmd->cmd_kref);
 	cmd->transport_state = CMD_T_DEV_ACTIVE;
@@ -1309,7 +1307,7 @@ EXPORT_SYMBOL(target_setup_cmd_from_cdb);
 
 /*
  * Used by fabric module frontends to queue tasks directly.
- * Many only be used from process context only
+ * May only be used from process context.
  */
 int transport_handle_cdb_direct(
 	struct se_cmd *cmd)
@@ -1428,6 +1426,12 @@ int target_submit_cmd_map_sgls(struct se_cmd *se_cmd, struct se_session *se_sess
 	 */
 	transport_init_se_cmd(se_cmd, se_tpg->se_tpg_tfo, se_sess,
 				data_length, data_dir, task_attr, sense);
+
+	if (flags & TARGET_SCF_USE_CPUID)
+		se_cmd->se_cmd_flags |= SCF_USE_CPUID;
+	else
+		se_cmd->cpuid = WORK_CPU_UNBOUND;
+
 	if (flags & TARGET_SCF_UNKNOWN_SIZE)
 		se_cmd->unknown_data_length = 1;
 	/*
@@ -1582,7 +1586,7 @@ static void target_complete_tmr_failure(struct work_struct *work)
 int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 		unsigned char *sense, u64 unpacked_lun,
 		void *fabric_tmr_ptr, unsigned char tm_type,
-		gfp_t gfp, unsigned int tag, int flags)
+		gfp_t gfp, u64 tag, int flags)
 {
 	struct se_portal_group *se_tpg;
 	int ret;
@@ -1626,33 +1630,6 @@ int target_submit_tmr(struct se_cmd *se_cmd, struct se_session *se_sess,
 EXPORT_SYMBOL(target_submit_tmr);
 
 /*
- * If the cmd is active, request it to be stopped and sleep until it
- * has completed.
- */
-bool target_stop_cmd(struct se_cmd *cmd, unsigned long *flags)
-	__releases(&cmd->t_state_lock)
-	__acquires(&cmd->t_state_lock)
-{
-	bool was_active = false;
-
-	if (cmd->transport_state & CMD_T_BUSY) {
-		cmd->transport_state |= CMD_T_REQUEST_STOP;
-		spin_unlock_irqrestore(&cmd->t_state_lock, *flags);
-
-		pr_debug("cmd %p waiting to complete\n", cmd);
-		wait_for_completion(&cmd->task_stop_comp);
-		pr_debug("cmd %p stopped successfully\n", cmd);
-
-		spin_lock_irqsave(&cmd->t_state_lock, *flags);
-		cmd->transport_state &= ~CMD_T_REQUEST_STOP;
-		cmd->transport_state &= ~CMD_T_BUSY;
-		was_active = true;
-	}
-
-	return was_active;
-}
-
-/*
  * Handle SAM-esque emulation for generic transport request failures.
  */
 void transport_generic_request_failure(struct se_cmd *cmd,
@@ -1850,19 +1827,21 @@ static bool target_handle_task_attr(struct se_cmd *cmd)
 	return true;
 }
 
+static int __transport_check_aborted_status(struct se_cmd *, int);
+
 void target_execute_cmd(struct se_cmd *cmd)
 {
 	/*
-	 * If the received CDB has aleady been aborted stop processing it here.
-	 */
-	if (transport_check_aborted_status(cmd, 1))
-		return;
-
-	/*
 	 * Determine if frontend context caller is requesting the stopping of
 	 * this command for frontend exceptions.
+	 *
+	 * If the received CDB has aleady been aborted stop processing it here.
 	 */
 	spin_lock_irq(&cmd->t_state_lock);
+	if (__transport_check_aborted_status(cmd, 1)) {
+		spin_unlock_irq(&cmd->t_state_lock);
+		return;
+	}
 	if (cmd->transport_state & CMD_T_STOP) {
 		pr_debug("%s:%d CMD_T_STOP for ITT: 0x%08llx\n",
 			__func__, __LINE__, cmd->tag);
@@ -2213,20 +2192,14 @@ static inline void transport_free_pages(struct se_cmd *cmd)
 }
 
 /**
- * transport_release_cmd - free a command
- * @cmd:       command to free
+ * transport_put_cmd - release a reference to a command
+ * @cmd:       command to release
  *
- * This routine unconditionally frees a command, and reference counting
- * or list removal must be done in the caller.
+ * This routine releases our reference to the command and frees it if possible.
  */
-static int transport_release_cmd(struct se_cmd *cmd)
+static int transport_put_cmd(struct se_cmd *cmd)
 {
 	BUG_ON(!cmd->se_tfo);
-
-	if (cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
-		core_tmr_release_req(cmd->se_tmr_req);
-	if (cmd->t_task_cdb != cmd->__t_task_cdb)
-		kfree(cmd->t_task_cdb);
 	/*
 	 * If this cmd has been setup with target_get_sess_cmd(), drop
 	 * the kref and call ->release_cmd() in kref callback.
@@ -2234,18 +2207,6 @@ static int transport_release_cmd(struct se_cmd *cmd)
 	return target_put_sess_cmd(cmd);
 }
 
-/**
- * transport_put_cmd - release a reference to a command
- * @cmd:       command to release
- *
- * This routine releases our reference to the command and frees it if possible.
- */
-static int transport_put_cmd(struct se_cmd *cmd)
-{
-	transport_free_pages(cmd);
-	return transport_release_cmd(cmd);
-}
-
 void *transport_kmap_data_sg(struct se_cmd *cmd)
 {
 	struct scatterlist *sg = cmd->t_data_sg;
@@ -2441,34 +2402,58 @@ static void transport_write_pending_qf(struct se_cmd *cmd)
 	}
 }
 
-int transport_generic_free_cmd(struct se_cmd *cmd, int wait_for_tasks)
+static bool
+__transport_wait_for_tasks(struct se_cmd *, bool, bool *, bool *,
+			   unsigned long *flags);
+
+static void target_wait_free_cmd(struct se_cmd *cmd, bool *aborted, bool *tas)
 {
 	unsigned long flags;
+
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	__transport_wait_for_tasks(cmd, true, aborted, tas, &flags);
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+}
+
+int transport_generic_free_cmd(struct se_cmd *cmd, int wait_for_tasks)
+{
 	int ret = 0;
+	bool aborted = false, tas = false;
 
 	if (!(cmd->se_cmd_flags & SCF_SE_LUN_CMD)) {
 		if (wait_for_tasks && (cmd->se_cmd_flags & SCF_SCSI_TMR_CDB))
-			 transport_wait_for_tasks(cmd);
+			target_wait_free_cmd(cmd, &aborted, &tas);
 
-		ret = transport_release_cmd(cmd);
+		if (!aborted || tas)
+			ret = transport_put_cmd(cmd);
 	} else {
 		if (wait_for_tasks)
-			transport_wait_for_tasks(cmd);
+			target_wait_free_cmd(cmd, &aborted, &tas);
 		/*
 		 * Handle WRITE failure case where transport_generic_new_cmd()
 		 * has already added se_cmd to state_list, but fabric has
 		 * failed command before I/O submission.
 		 */
-		if (cmd->state_active) {
-			spin_lock_irqsave(&cmd->t_state_lock, flags);
+		if (cmd->state_active)
 			target_remove_from_state_list(cmd);
-			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
-		}
 
 		if (cmd->se_lun)
 			transport_lun_remove_cmd(cmd);
 
-		ret = transport_put_cmd(cmd);
+		if (!aborted || tas)
+			ret = transport_put_cmd(cmd);
+	}
+	/*
+	 * If the task has been internally aborted due to TMR ABORT_TASK
+	 * or LUN_RESET, target_core_tmr.c is responsible for performing
+	 * the remaining calls to target_put_sess_cmd(), and not the
+	 * callers of this function.
+	 */
+	if (aborted) {
+		pr_debug("Detected CMD_T_ABORTED for ITT: %llu\n", cmd->tag);
+		wait_for_completion(&cmd->cmd_wait_comp);
+		cmd->se_tfo->release_cmd(cmd);
+		ret = 1;
 	}
 	return ret;
 }
@@ -2508,26 +2493,46 @@ out:
 }
 EXPORT_SYMBOL(target_get_sess_cmd);
 
+static void target_free_cmd_mem(struct se_cmd *cmd)
+{
+	transport_free_pages(cmd);
+
+	if (cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
+		core_tmr_release_req(cmd->se_tmr_req);
+	if (cmd->t_task_cdb != cmd->__t_task_cdb)
+		kfree(cmd->t_task_cdb);
+}
+
 static void target_release_cmd_kref(struct kref *kref)
 {
 	struct se_cmd *se_cmd = container_of(kref, struct se_cmd, cmd_kref);
 	struct se_session *se_sess = se_cmd->se_sess;
 	unsigned long flags;
+	bool fabric_stop;
 
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
 	if (list_empty(&se_cmd->se_cmd_list)) {
 		spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
+		target_free_cmd_mem(se_cmd);
 		se_cmd->se_tfo->release_cmd(se_cmd);
 		return;
 	}
-	if (se_sess->sess_tearing_down && se_cmd->cmd_wait_set) {
+
+	spin_lock(&se_cmd->t_state_lock);
+	fabric_stop = (se_cmd->transport_state & CMD_T_FABRIC_STOP);
+	spin_unlock(&se_cmd->t_state_lock);
+
+	if (se_cmd->cmd_wait_set || fabric_stop) {
+		list_del_init(&se_cmd->se_cmd_list);
 		spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
+		target_free_cmd_mem(se_cmd);
 		complete(&se_cmd->cmd_wait_comp);
 		return;
 	}
-	list_del(&se_cmd->se_cmd_list);
+	list_del_init(&se_cmd->se_cmd_list);
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 
+	target_free_cmd_mem(se_cmd);
 	se_cmd->se_tfo->release_cmd(se_cmd);
 }
 
@@ -2539,6 +2544,7 @@ int target_put_sess_cmd(struct se_cmd *se_cmd)
 	struct se_session *se_sess = se_cmd->se_sess;
 
 	if (!se_sess) {
+		target_free_cmd_mem(se_cmd);
 		se_cmd->se_tfo->release_cmd(se_cmd);
 		return 1;
 	}
@@ -2555,6 +2561,7 @@ void target_sess_cmd_list_set_waiting(struct se_session *se_sess)
 {
 	struct se_cmd *se_cmd;
 	unsigned long flags;
+	int rc;
 
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
 	if (se_sess->sess_tearing_down) {
@@ -2564,8 +2571,15 @@ void target_sess_cmd_list_set_waiting(struct se_session *se_sess)
 	se_sess->sess_tearing_down = 1;
 	list_splice_init(&se_sess->sess_cmd_list, &se_sess->sess_wait_list);
 
-	list_for_each_entry(se_cmd, &se_sess->sess_wait_list, se_cmd_list)
-		se_cmd->cmd_wait_set = 1;
+	list_for_each_entry(se_cmd, &se_sess->sess_wait_list, se_cmd_list) {
+		rc = kref_get_unless_zero(&se_cmd->cmd_kref);
+		if (rc) {
+			se_cmd->cmd_wait_set = 1;
+			spin_lock(&se_cmd->t_state_lock);
+			se_cmd->transport_state |= CMD_T_FABRIC_STOP;
+			spin_unlock(&se_cmd->t_state_lock);
+		}
+	}
 
 	spin_unlock_irqrestore(&se_sess->sess_cmd_lock, flags);
 }
@@ -2578,15 +2592,25 @@ void target_wait_for_sess_cmds(struct se_session *se_sess)
 {
 	struct se_cmd *se_cmd, *tmp_cmd;
 	unsigned long flags;
+	bool tas;
 
 	list_for_each_entry_safe(se_cmd, tmp_cmd,
 				&se_sess->sess_wait_list, se_cmd_list) {
-		list_del(&se_cmd->se_cmd_list);
+		list_del_init(&se_cmd->se_cmd_list);
 
 		pr_debug("Waiting for se_cmd: %p t_state: %d, fabric state:"
 			" %d\n", se_cmd, se_cmd->t_state,
 			se_cmd->se_tfo->get_cmd_state(se_cmd));
 
+		spin_lock_irqsave(&se_cmd->t_state_lock, flags);
+		tas = (se_cmd->transport_state & CMD_T_TAS);
+		spin_unlock_irqrestore(&se_cmd->t_state_lock, flags);
+
+		if (!target_put_sess_cmd(se_cmd)) {
+			if (tas)
+				target_put_sess_cmd(se_cmd);
+		}
+
 		wait_for_completion(&se_cmd->cmd_wait_comp);
 		pr_debug("After cmd_wait_comp: se_cmd: %p t_state: %d"
 			" fabric state: %d\n", se_cmd, se_cmd->t_state,
@@ -2608,53 +2632,75 @@ void transport_clear_lun_ref(struct se_lun *lun)
 	wait_for_completion(&lun->lun_ref_comp);
 }
 
-/**
- * transport_wait_for_tasks - wait for completion to occur
- * @cmd:	command to wait
- *
- * Called from frontend fabric context to wait for storage engine
- * to pause and/or release frontend generated struct se_cmd.
- */
-bool transport_wait_for_tasks(struct se_cmd *cmd)
+static bool
+__transport_wait_for_tasks(struct se_cmd *cmd, bool fabric_stop,
+			   bool *aborted, bool *tas, unsigned long *flags)
+	__releases(&cmd->t_state_lock)
+	__acquires(&cmd->t_state_lock)
 {
-	unsigned long flags;
 
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	assert_spin_locked(&cmd->t_state_lock);
+	WARN_ON_ONCE(!irqs_disabled());
+
+	if (fabric_stop)
+		cmd->transport_state |= CMD_T_FABRIC_STOP;
+
+	if (cmd->transport_state & CMD_T_ABORTED)
+		*aborted = true;
+
+	if (cmd->transport_state & CMD_T_TAS)
+		*tas = true;
+
 	if (!(cmd->se_cmd_flags & SCF_SE_LUN_CMD) &&
-	    !(cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)) {
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	    !(cmd->se_cmd_flags & SCF_SCSI_TMR_CDB))
 		return false;
-	}
 
 	if (!(cmd->se_cmd_flags & SCF_SUPPORTED_SAM_OPCODE) &&
-	    !(cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)) {
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	    !(cmd->se_cmd_flags & SCF_SCSI_TMR_CDB))
 		return false;
-	}
 
-	if (!(cmd->transport_state & CMD_T_ACTIVE)) {
-		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	if (!(cmd->transport_state & CMD_T_ACTIVE))
+		return false;
+
+	if (fabric_stop && *aborted)
 		return false;
-	}
 
 	cmd->transport_state |= CMD_T_STOP;
 
-	pr_debug("wait_for_tasks: Stopping %p ITT: 0x%08llx i_state: %d, t_state: %d, CMD_T_STOP\n",
-		cmd, cmd->tag, cmd->se_tfo->get_cmd_state(cmd), cmd->t_state);
+	pr_debug("wait_for_tasks: Stopping %p ITT: 0x%08llx i_state: %d,"
+		 " t_state: %d, CMD_T_STOP\n", cmd, cmd->tag,
+		 cmd->se_tfo->get_cmd_state(cmd), cmd->t_state);
 
-	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+	spin_unlock_irqrestore(&cmd->t_state_lock, *flags);
 
 	wait_for_completion(&cmd->t_transport_stop_comp);
 
-	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	spin_lock_irqsave(&cmd->t_state_lock, *flags);
 	cmd->transport_state &= ~(CMD_T_ACTIVE | CMD_T_STOP);
 
-	pr_debug("wait_for_tasks: Stopped wait_for_completion(&cmd->t_transport_stop_comp) for ITT: 0x%08llx\n",
-		cmd->tag);
+	pr_debug("wait_for_tasks: Stopped wait_for_completion(&cmd->"
+		 "t_transport_stop_comp) for ITT: 0x%08llx\n", cmd->tag);
+
+	return true;
+}
+
+/**
+ * transport_wait_for_tasks - wait for completion to occur
+ * @cmd:	command to wait
+ *
+ * Called from frontend fabric context to wait for storage engine
+ * to pause and/or release frontend generated struct se_cmd.
+ */
+bool transport_wait_for_tasks(struct se_cmd *cmd)
+{
+	unsigned long flags;
+	bool ret, aborted = false, tas = false;
 
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	ret = __transport_wait_for_tasks(cmd, false, &aborted, &tas, &flags);
 	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 
-	return true;
+	return ret;
 }
 EXPORT_SYMBOL(transport_wait_for_tasks);
 
@@ -2836,28 +2882,49 @@ transport_send_check_condition_and_sense(struct se_cmd *cmd,
 }
 EXPORT_SYMBOL(transport_send_check_condition_and_sense);
 
-int transport_check_aborted_status(struct se_cmd *cmd, int send_status)
+static int __transport_check_aborted_status(struct se_cmd *cmd, int send_status)
+	__releases(&cmd->t_state_lock)
+	__acquires(&cmd->t_state_lock)
 {
+	assert_spin_locked(&cmd->t_state_lock);
+	WARN_ON_ONCE(!irqs_disabled());
+
 	if (!(cmd->transport_state & CMD_T_ABORTED))
 		return 0;
-
 	/*
 	 * If cmd has been aborted but either no status is to be sent or it has
 	 * already been sent, just return
 	 */
-	if (!send_status || !(cmd->se_cmd_flags & SCF_SEND_DELAYED_TAS))
+	if (!send_status || !(cmd->se_cmd_flags & SCF_SEND_DELAYED_TAS)) {
+		if (send_status)
+			cmd->se_cmd_flags |= SCF_SEND_DELAYED_TAS;
 		return 1;
+	}
 
-	pr_debug("Sending delayed SAM_STAT_TASK_ABORTED status for CDB: 0x%02x ITT: 0x%08llx\n",
-		 cmd->t_task_cdb[0], cmd->tag);
+	pr_debug("Sending delayed SAM_STAT_TASK_ABORTED status for CDB:"
+		" 0x%02x ITT: 0x%08llx\n", cmd->t_task_cdb[0], cmd->tag);
 
 	cmd->se_cmd_flags &= ~SCF_SEND_DELAYED_TAS;
 	cmd->scsi_status = SAM_STAT_TASK_ABORTED;
 	trace_target_cmd_complete(cmd);
+
+	spin_unlock_irq(&cmd->t_state_lock);
 	cmd->se_tfo->queue_status(cmd);
+	spin_lock_irq(&cmd->t_state_lock);
 
 	return 1;
 }
+
+int transport_check_aborted_status(struct se_cmd *cmd, int send_status)
+{
+	int ret;
+
+	spin_lock_irq(&cmd->t_state_lock);
+	ret = __transport_check_aborted_status(cmd, send_status);
+	spin_unlock_irq(&cmd->t_state_lock);
+
+	return ret;
+}
 EXPORT_SYMBOL(transport_check_aborted_status);
 
 void transport_send_task_abort(struct se_cmd *cmd)
@@ -2879,11 +2946,17 @@ void transport_send_task_abort(struct se_cmd *cmd)
 	 */
 	if (cmd->data_direction == DMA_TO_DEVICE) {
 		if (cmd->se_tfo->write_pending_status(cmd) != 0) {
-			cmd->transport_state |= CMD_T_ABORTED;
+			spin_lock_irqsave(&cmd->t_state_lock, flags);
+			if (cmd->se_cmd_flags & SCF_SEND_DELAYED_TAS) {
+				spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+				goto send_abort;
+			}
 			cmd->se_cmd_flags |= SCF_SEND_DELAYED_TAS;
+			spin_unlock_irqrestore(&cmd->t_state_lock, flags);
 			return;
 		}
 	}
+send_abort:
 	cmd->scsi_status = SAM_STAT_TASK_ABORTED;
 
 	transport_lun_remove_cmd(cmd);
@@ -2900,8 +2973,17 @@ static void target_tmr_work(struct work_struct *work)
 	struct se_cmd *cmd = container_of(work, struct se_cmd, work);
 	struct se_device *dev = cmd->se_dev;
 	struct se_tmr_req *tmr = cmd->se_tmr_req;
+	unsigned long flags;
 	int ret;
 
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	if (cmd->transport_state & CMD_T_ABORTED) {
+		tmr->response = TMR_FUNCTION_REJECTED;
+		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+		goto check_stop;
+	}
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+
 	switch (tmr->function) {
 	case TMR_ABORT_TASK:
 		core_tmr_abort_task(dev, tmr, cmd->se_sess);
@@ -2934,9 +3016,17 @@ static void target_tmr_work(struct work_struct *work)
 		break;
 	}
 
+	spin_lock_irqsave(&cmd->t_state_lock, flags);
+	if (cmd->transport_state & CMD_T_ABORTED) {
+		spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+		goto check_stop;
+	}
 	cmd->t_state = TRANSPORT_ISTATE_PROCESSING;
+	spin_unlock_irqrestore(&cmd->t_state_lock, flags);
+
 	cmd->se_tfo->queue_tm_rsp(cmd);
 
+check_stop:
 	transport_cmd_check_stop_to_fabric(cmd);
 }
 
diff --git a/drivers/target/target_core_user.c b/drivers/target/target_core_user.c
index 5e6d6cb348fc..94f5154ac788 100644
--- a/drivers/target/target_core_user.c
+++ b/drivers/target/target_core_user.c
@@ -152,6 +152,7 @@ static struct genl_family tcmu_genl_family = {
 	.maxattr = TCMU_ATTR_MAX,
 	.mcgrps = tcmu_mcgrps,
 	.n_mcgrps = ARRAY_SIZE(tcmu_mcgrps),
+	.netnsok = true,
 };
 
 static struct tcmu_cmd *tcmu_alloc_cmd(struct se_cmd *se_cmd)
@@ -194,7 +195,7 @@ static struct tcmu_cmd *tcmu_alloc_cmd(struct se_cmd *se_cmd)
 
 static inline void tcmu_flush_dcache_range(void *vaddr, size_t size)
 {
-	unsigned long offset = (unsigned long) vaddr & ~PAGE_MASK;
+	unsigned long offset = offset_in_page(vaddr);
 
 	size = round_up(size+offset, PAGE_SIZE);
 	vaddr -= offset;
@@ -840,7 +841,7 @@ static int tcmu_netlink_event(enum tcmu_genl_cmd cmd, const char *name, int mino
 
 	genlmsg_end(skb, msg_header);
 
-	ret = genlmsg_multicast(&tcmu_genl_family, skb, 0,
+	ret = genlmsg_multicast_allns(&tcmu_genl_family, skb, 0,
 				TCMU_MCGRP_CONFIG, GFP_KERNEL);
 
 	/* We don't care if no one is listening */
@@ -902,7 +903,7 @@ static int tcmu_configure_device(struct se_device *dev)
 	info->version = __stringify(TCMU_MAILBOX_VERSION);
 
 	info->mem[0].name = "tcm-user command & data buffer";
-	info->mem[0].addr = (phys_addr_t) udev->mb_addr;
+	info->mem[0].addr = (phys_addr_t)(uintptr_t)udev->mb_addr;
 	info->mem[0].size = TCMU_RING_SIZE;
 	info->mem[0].memtype = UIO_MEM_VIRTUAL;
 
@@ -917,8 +918,10 @@ static int tcmu_configure_device(struct se_device *dev)
 	if (ret)
 		goto err_register;
 
+	/* User can set hw_block_size before enable the device */
+	if (dev->dev_attrib.hw_block_size == 0)
+		dev->dev_attrib.hw_block_size = 512;
 	/* Other attributes can be configured in userspace */
-	dev->dev_attrib.hw_block_size = 512;
 	dev->dev_attrib.hw_max_sectors = 128;
 	dev->dev_attrib.hw_queue_depth = 128;
 
diff --git a/drivers/target/tcm_fc/tcm_fc.h b/drivers/target/tcm_fc/tcm_fc.h
index 39909dadef3e..c30003bd4ff0 100644
--- a/drivers/target/tcm_fc/tcm_fc.h
+++ b/drivers/target/tcm_fc/tcm_fc.h
@@ -166,7 +166,6 @@ void ft_aborted_task(struct se_cmd *);
  */
 void ft_recv_req(struct ft_sess *, struct fc_frame *);
 struct ft_tpg *ft_lport_find_tpg(struct fc_lport *);
-struct ft_node_acl *ft_acl_get(struct ft_tpg *, struct fc_rport_priv *);
 
 void ft_recv_write_data(struct ft_cmd *, struct fc_frame *);
 void ft_dump_cmd(struct ft_cmd *, const char *caller);
diff --git a/drivers/target/tcm_fc/tfc_conf.c b/drivers/target/tcm_fc/tfc_conf.c
index 85aeaa0ad303..4d375e95841b 100644
--- a/drivers/target/tcm_fc/tfc_conf.c
+++ b/drivers/target/tcm_fc/tfc_conf.c
@@ -171,9 +171,31 @@ static ssize_t ft_nacl_node_name_store(struct config_item *item,
 CONFIGFS_ATTR(ft_nacl_, node_name);
 CONFIGFS_ATTR(ft_nacl_, port_name);
 
+static ssize_t ft_nacl_tag_show(struct config_item *item,
+		char *page)
+{
+	return snprintf(page, PAGE_SIZE, "%s", acl_to_nacl(item)->acl_tag);
+}
+
+static ssize_t ft_nacl_tag_store(struct config_item *item,
+		const char *page, size_t count)
+{
+	struct se_node_acl *se_nacl = acl_to_nacl(item);
+	int ret;
+
+	ret = core_tpg_set_initiator_node_tag(se_nacl->se_tpg, se_nacl, page);
+
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+CONFIGFS_ATTR(ft_nacl_, tag);
+
 static struct configfs_attribute *ft_nacl_base_attrs[] = {
 	&ft_nacl_attr_port_name,
 	&ft_nacl_attr_node_name,
+	&ft_nacl_attr_tag,
 	NULL,
 };
 
@@ -198,31 +220,6 @@ static int ft_init_nodeacl(struct se_node_acl *nacl, const char *name)
 	return 0;
 }
 
-struct ft_node_acl *ft_acl_get(struct ft_tpg *tpg, struct fc_rport_priv *rdata)
-{
-	struct ft_node_acl *found = NULL;
-	struct ft_node_acl *acl;
-	struct se_portal_group *se_tpg = &tpg->se_tpg;
-	struct se_node_acl *se_acl;
-
-	mutex_lock(&se_tpg->acl_node_mutex);
-	list_for_each_entry(se_acl, &se_tpg->acl_node_list, acl_list) {
-		acl = container_of(se_acl, struct ft_node_acl, se_node_acl);
-		pr_debug("acl %p port_name %llx\n",
-			acl, (unsigned long long)acl->node_auth.port_name);
-		if (acl->node_auth.port_name == rdata->ids.port_name ||
-		    acl->node_auth.node_name == rdata->ids.node_name) {
-			pr_debug("acl %p port_name %llx matched\n", acl,
-				    (unsigned long long)rdata->ids.port_name);
-			found = acl;
-			/* XXX need to hold onto ACL */
-			break;
-		}
-	}
-	mutex_unlock(&se_tpg->acl_node_mutex);
-	return found;
-}
-
 /*
  * local_port port_group (tpg) ops.
  */
diff --git a/drivers/target/tcm_fc/tfc_io.c b/drivers/target/tcm_fc/tfc_io.c
index 847c1aa6fbf4..6f7c65abfe2a 100644
--- a/drivers/target/tcm_fc/tfc_io.c
+++ b/drivers/target/tcm_fc/tfc_io.c
@@ -154,9 +154,9 @@ int ft_queue_data_in(struct se_cmd *se_cmd)
 			BUG_ON(!page);
 			from = kmap_atomic(page + (mem_off >> PAGE_SHIFT));
 			page_addr = from;
-			from += mem_off & ~PAGE_MASK;
+			from += offset_in_page(mem_off);
 			tlen = min(tlen, (size_t)(PAGE_SIZE -
-						(mem_off & ~PAGE_MASK)));
+						offset_in_page(mem_off)));
 			memcpy(to, from, tlen);
 			kunmap_atomic(page_addr);
 			to += tlen;
@@ -314,9 +314,9 @@ void ft_recv_write_data(struct ft_cmd *cmd, struct fc_frame *fp)
 
 		to = kmap_atomic(page + (mem_off >> PAGE_SHIFT));
 		page_addr = to;
-		to += mem_off & ~PAGE_MASK;
+		to += offset_in_page(mem_off);
 		tlen = min(tlen, (size_t)(PAGE_SIZE -
-					  (mem_off & ~PAGE_MASK)));
+					  offset_in_page(mem_off)));
 		memcpy(to, from, tlen);
 		kunmap_atomic(page_addr);
 
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 7b934eac995d..e19f4c58c6fa 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -191,10 +191,15 @@ out:
  * Caller holds ft_lport_lock.
  */
 static struct ft_sess *ft_sess_create(struct ft_tport *tport, u32 port_id,
-				      struct ft_node_acl *acl)
+				      struct fc_rport_priv *rdata)
 {
+	struct se_portal_group *se_tpg = &tport->tpg->se_tpg;
+	struct se_node_acl *se_acl;
 	struct ft_sess *sess;
 	struct hlist_head *head;
+	unsigned char initiatorname[TRANSPORT_IQN_LEN];
+
+	ft_format_wwn(&initiatorname[0], TRANSPORT_IQN_LEN, rdata->ids.port_name);
 
 	head = &tport->hash[ft_sess_hash(port_id)];
 	hlist_for_each_entry_rcu(sess, head, hash)
@@ -212,7 +217,14 @@ static struct ft_sess *ft_sess_create(struct ft_tport *tport, u32 port_id,
 		kfree(sess);
 		return NULL;
 	}
-	sess->se_sess->se_node_acl = &acl->se_node_acl;
+
+	se_acl = core_tpg_get_initiator_node_acl(se_tpg, &initiatorname[0]);
+	if (!se_acl) {
+		transport_free_session(sess->se_sess);
+		kfree(sess);
+		return NULL;
+	}
+	sess->se_sess->se_node_acl = se_acl;
 	sess->tport = tport;
 	sess->port_id = port_id;
 	kref_init(&sess->kref);	/* ref for table entry */
@@ -221,7 +233,7 @@ static struct ft_sess *ft_sess_create(struct ft_tport *tport, u32 port_id,
 
 	pr_debug("port_id %x sess %p\n", port_id, sess);
 
-	transport_register_session(&tport->tpg->se_tpg, &acl->se_node_acl,
+	transport_register_session(&tport->tpg->se_tpg, se_acl,
 				   sess->se_sess, sess);
 	return sess;
 }
@@ -260,6 +272,14 @@ static struct ft_sess *ft_sess_delete(struct ft_tport *tport, u32 port_id)
 	return NULL;
 }
 
+static void ft_close_sess(struct ft_sess *sess)
+{
+	transport_deregister_session_configfs(sess->se_sess);
+	target_sess_cmd_list_set_waiting(sess->se_sess);
+	target_wait_for_sess_cmds(sess->se_sess);
+	ft_sess_put(sess);
+}
+
 /*
  * Delete all sessions from tport.
  * Caller holds ft_lport_lock.
@@ -273,8 +293,7 @@ static void ft_sess_delete_all(struct ft_tport *tport)
 	     head < &tport->hash[FT_SESS_HASH_SIZE]; head++) {
 		hlist_for_each_entry_rcu(sess, head, hash) {
 			ft_sess_unhash(sess);
-			transport_deregister_session_configfs(sess->se_sess);
-			ft_sess_put(sess);	/* release from table */
+			ft_close_sess(sess);	/* release from table */
 		}
 	}
 }
@@ -313,8 +332,7 @@ void ft_sess_close(struct se_session *se_sess)
 	pr_debug("port_id %x\n", port_id);
 	ft_sess_unhash(sess);
 	mutex_unlock(&ft_lport_lock);
-	transport_deregister_session_configfs(se_sess);
-	ft_sess_put(sess);
+	ft_close_sess(sess);
 	/* XXX Send LOGO or PRLO */
 	synchronize_rcu();		/* let transport deregister happen */
 }
@@ -343,17 +361,12 @@ static int ft_prli_locked(struct fc_rport_priv *rdata, u32 spp_len,
 {
 	struct ft_tport *tport;
 	struct ft_sess *sess;
-	struct ft_node_acl *acl;
 	u32 fcp_parm;
 
 	tport = ft_tport_get(rdata->local_port);
 	if (!tport)
 		goto not_target;	/* not a target for this local port */
 
-	acl = ft_acl_get(tport->tpg, rdata);
-	if (!acl)
-		goto not_target;	/* no target for this remote */
-
 	if (!rspp)
 		goto fill;
 
@@ -375,7 +388,7 @@ static int ft_prli_locked(struct fc_rport_priv *rdata, u32 spp_len,
 		spp->spp_flags |= FC_SPP_EST_IMG_PAIR;
 		if (!(fcp_parm & FCP_SPPF_INIT_FCN))
 			return FC_SPP_RESP_CONF;
-		sess = ft_sess_create(tport, rdata->ids.port_id, acl);
+		sess = ft_sess_create(tport, rdata->ids.port_id, rdata);
 		if (!sess)
 			return FC_SPP_RESP_RES;
 		if (!sess->params)
@@ -460,8 +473,7 @@ static void ft_prlo(struct fc_rport_priv *rdata)
 		return;
 	}
 	mutex_unlock(&ft_lport_lock);
-	transport_deregister_session_configfs(sess->se_sess);
-	ft_sess_put(sess);		/* release from table */
+	ft_close_sess(sess);		/* release from table */
 	rdata->prli_count--;
 	/* XXX TBD - clearing actions.  unit attn, see 4.10 */
 }
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index 8cc4ac64a91c..7c92c09be213 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -195,7 +195,7 @@ config IMX_THERMAL
 	  passive trip is crossed.
 
 config SPEAR_THERMAL
-	bool "SPEAr thermal sensor driver"
+	tristate "SPEAr thermal sensor driver"
 	depends on PLAT_SPEAR || COMPILE_TEST
 	depends on OF
 	help
@@ -237,8 +237,8 @@ config DOVE_THERMAL
 	  framework.
 
 config DB8500_THERMAL
-	bool "DB8500 thermal management"
-	depends on ARCH_U8500
+	tristate "DB8500 thermal management"
+	depends on MFD_DB8500_PRCMU
 	default y
 	help
 	  Adds DB8500 thermal management implementation according to the thermal
diff --git a/drivers/thermal/cpu_cooling.c b/drivers/thermal/cpu_cooling.c
index e3fbc5a5d88f..6ceac4f2d4b2 100644
--- a/drivers/thermal/cpu_cooling.c
+++ b/drivers/thermal/cpu_cooling.c
@@ -377,26 +377,28 @@ static u32 cpu_power_to_freq(struct cpufreq_cooling_device *cpufreq_device,
  * get_load() - get load for a cpu since last updated
  * @cpufreq_device:	&struct cpufreq_cooling_device for this cpu
  * @cpu:	cpu number
+ * @cpu_idx:	index of the cpu in cpufreq_device->allowed_cpus
  *
  * Return: The average load of cpu @cpu in percentage since this
  * function was last called.
  */
-static u32 get_load(struct cpufreq_cooling_device *cpufreq_device, int cpu)
+static u32 get_load(struct cpufreq_cooling_device *cpufreq_device, int cpu,
+		    int cpu_idx)
 {
 	u32 load;
 	u64 now, now_idle, delta_time, delta_idle;
 
 	now_idle = get_cpu_idle_time(cpu, &now, 0);
-	delta_idle = now_idle - cpufreq_device->time_in_idle[cpu];
-	delta_time = now - cpufreq_device->time_in_idle_timestamp[cpu];
+	delta_idle = now_idle - cpufreq_device->time_in_idle[cpu_idx];
+	delta_time = now - cpufreq_device->time_in_idle_timestamp[cpu_idx];
 
 	if (delta_time <= delta_idle)
 		load = 0;
 	else
 		load = div64_u64(100 * (delta_time - delta_idle), delta_time);
 
-	cpufreq_device->time_in_idle[cpu] = now_idle;
-	cpufreq_device->time_in_idle_timestamp[cpu] = now;
+	cpufreq_device->time_in_idle[cpu_idx] = now_idle;
+	cpufreq_device->time_in_idle_timestamp[cpu_idx] = now;
 
 	return load;
 }
@@ -598,7 +600,7 @@ static int cpufreq_get_requested_power(struct thermal_cooling_device *cdev,
 		u32 load;
 
 		if (cpu_online(cpu))
-			load = get_load(cpufreq_device, cpu);
+			load = get_load(cpufreq_device, cpu, i);
 		else
 			load = 0;
 
diff --git a/drivers/thermal/int340x_thermal/processor_thermal_device.c b/drivers/thermal/int340x_thermal/processor_thermal_device.c
index ccc0ad02d066..36fa724a36c8 100644
--- a/drivers/thermal/int340x_thermal/processor_thermal_device.c
+++ b/drivers/thermal/int340x_thermal/processor_thermal_device.c
@@ -33,6 +33,12 @@
 /* Braswell thermal reporting device */
 #define PCI_DEVICE_ID_PROC_BSW_THERMAL	0x22DC
 
+/* Broxton thermal reporting device */
+#define PCI_DEVICE_ID_PROC_BXT0_THERMAL  0x0A8C
+#define PCI_DEVICE_ID_PROC_BXT1_THERMAL  0x1A8C
+#define PCI_DEVICE_ID_PROC_BXTX_THERMAL  0x4A8C
+#define PCI_DEVICE_ID_PROC_BXTP_THERMAL  0x5A8C
+
 struct power_config {
 	u32	index;
 	u32	min_uw;
@@ -404,6 +410,10 @@ static const struct pci_device_id proc_thermal_pci_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_HSB_THERMAL)},
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_SKL_THERMAL)},
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BSW_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BXT0_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BXT1_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BXTX_THERMAL)},
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_PROC_BXTP_THERMAL)},
 	{ 0, },
 };
 
diff --git a/drivers/thermal/intel_pch_thermal.c b/drivers/thermal/intel_pch_thermal.c
index 50c7da79be83..00d81af648b8 100644
--- a/drivers/thermal/intel_pch_thermal.c
+++ b/drivers/thermal/intel_pch_thermal.c
@@ -136,7 +136,7 @@ struct pch_dev_ops {
 
 
 /* dev ops for Wildcat Point */
-static struct pch_dev_ops pch_dev_ops_wpt = {
+static const struct pch_dev_ops pch_dev_ops_wpt = {
 	.hw_init = pch_wpt_init,
 	.get_temp = pch_wpt_get_temp,
 };
diff --git a/drivers/thermal/intel_quark_dts_thermal.c b/drivers/thermal/intel_quark_dts_thermal.c
index 5ed90e6c8a64..5d33b350da1c 100644
--- a/drivers/thermal/intel_quark_dts_thermal.c
+++ b/drivers/thermal/intel_quark_dts_thermal.c
@@ -125,8 +125,8 @@ static int soc_dts_enable(struct thermal_zone_device *tzd)
 	struct soc_sensor_entry *aux_entry = tzd->devdata;
 	int ret;
 
-	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_ENABLE, &out);
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			    QRK_DTS_REG_OFFSET_ENABLE, &out);
 	if (ret)
 		return ret;
 
@@ -137,8 +137,8 @@ static int soc_dts_enable(struct thermal_zone_device *tzd)
 
 	if (!aux_entry->locked) {
 		out |= QRK_DTS_ENABLE_BIT;
-		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
-					QRK_DTS_REG_OFFSET_ENABLE, out);
+		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, MBI_REG_WRITE,
+				     QRK_DTS_REG_OFFSET_ENABLE, out);
 		if (ret)
 			return ret;
 
@@ -158,8 +158,8 @@ static int soc_dts_disable(struct thermal_zone_device *tzd)
 	struct soc_sensor_entry *aux_entry = tzd->devdata;
 	int ret;
 
-	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_ENABLE, &out);
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			    QRK_DTS_REG_OFFSET_ENABLE, &out);
 	if (ret)
 		return ret;
 
@@ -170,8 +170,8 @@ static int soc_dts_disable(struct thermal_zone_device *tzd)
 
 	if (!aux_entry->locked) {
 		out &= ~QRK_DTS_ENABLE_BIT;
-		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
-					QRK_DTS_REG_OFFSET_ENABLE, out);
+		ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, MBI_REG_WRITE,
+				     QRK_DTS_REG_OFFSET_ENABLE, out);
 
 		if (ret)
 			return ret;
@@ -192,8 +192,8 @@ static int _get_trip_temp(int trip, int *temp)
 	u32 out;
 
 	mutex_lock(&dts_update_mutex);
-	status = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-				QRK_DTS_REG_OFFSET_PTPS, &out);
+	status = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			       QRK_DTS_REG_OFFSET_PTPS, &out);
 	mutex_unlock(&dts_update_mutex);
 
 	if (status)
@@ -236,8 +236,8 @@ static int update_trip_temp(struct soc_sensor_entry *aux_entry,
 		goto failed;
 	}
 
-	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-				QRK_DTS_REG_OFFSET_PTPS, &store_ptps);
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			    QRK_DTS_REG_OFFSET_PTPS, &store_ptps);
 	if (ret)
 		goto failed;
 
@@ -262,8 +262,8 @@ static int update_trip_temp(struct soc_sensor_entry *aux_entry,
 	out |= (temp_out & QRK_DTS_MASK_TP_THRES) <<
 		(trip * QRK_DTS_SHIFT_TP);
 
-	ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
-				QRK_DTS_REG_OFFSET_PTPS, out);
+	ret = iosf_mbi_write(QRK_MBI_UNIT_RMU, MBI_REG_WRITE,
+			     QRK_DTS_REG_OFFSET_PTPS, out);
 
 failed:
 	mutex_unlock(&dts_update_mutex);
@@ -294,8 +294,8 @@ static int sys_get_curr_temp(struct thermal_zone_device *tzd,
 	int ret;
 
 	mutex_lock(&dts_update_mutex);
-	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_TEMP, &out);
+	ret = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			    QRK_DTS_REG_OFFSET_TEMP, &out);
 	mutex_unlock(&dts_update_mutex);
 
 	if (ret)
@@ -350,13 +350,13 @@ static void free_soc_dts(struct soc_sensor_entry *aux_entry)
 	if (aux_entry) {
 		if (!aux_entry->locked) {
 			mutex_lock(&dts_update_mutex);
-			iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
-					QRK_DTS_REG_OFFSET_ENABLE,
-					aux_entry->store_dts_enable);
+			iosf_mbi_write(QRK_MBI_UNIT_RMU, MBI_REG_WRITE,
+				       QRK_DTS_REG_OFFSET_ENABLE,
+				       aux_entry->store_dts_enable);
 
-			iosf_mbi_write(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_WRITE,
-					QRK_DTS_REG_OFFSET_PTPS,
-					aux_entry->store_ptps);
+			iosf_mbi_write(QRK_MBI_UNIT_RMU, MBI_REG_WRITE,
+				       QRK_DTS_REG_OFFSET_PTPS,
+				       aux_entry->store_ptps);
 			mutex_unlock(&dts_update_mutex);
 		}
 		thermal_zone_device_unregister(aux_entry->tzone);
@@ -378,9 +378,8 @@ static struct soc_sensor_entry *alloc_soc_dts(void)
 	}
 
 	/* Check if DTS register is locked */
-	err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_LOCK,
-					&out);
+	err = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+			    QRK_DTS_REG_OFFSET_LOCK, &out);
 	if (err)
 		goto err_ret;
 
@@ -395,16 +394,16 @@ static struct soc_sensor_entry *alloc_soc_dts(void)
 	/* Store DTS default state if DTS registers are not locked */
 	if (!aux_entry->locked) {
 		/* Store DTS default enable for restore on exit */
-		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_ENABLE,
-					&aux_entry->store_dts_enable);
+		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+				    QRK_DTS_REG_OFFSET_ENABLE,
+				    &aux_entry->store_dts_enable);
 		if (err)
 			goto err_ret;
 
 		/* Store DTS default PTPS register for restore on exit */
-		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, QRK_MBI_RMU_READ,
-					QRK_DTS_REG_OFFSET_PTPS,
-					&aux_entry->store_ptps);
+		err = iosf_mbi_read(QRK_MBI_UNIT_RMU, MBI_REG_READ,
+				    QRK_DTS_REG_OFFSET_PTPS,
+				    &aux_entry->store_ptps);
 		if (err)
 			goto err_ret;
 	}
diff --git a/drivers/thermal/intel_soc_dts_iosf.c b/drivers/thermal/intel_soc_dts_iosf.c
index 5841d1d72996..f72e1db3216f 100644
--- a/drivers/thermal/intel_soc_dts_iosf.c
+++ b/drivers/thermal/intel_soc_dts_iosf.c
@@ -90,7 +90,7 @@ static int sys_get_trip_temp(struct thermal_zone_device *tzd, int trip,
 	dts = tzd->devdata;
 	sensors = dts->sensors;
 	mutex_lock(&sensors->dts_update_lock);
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_PTPS, &out);
 	mutex_unlock(&sensors->dts_update_lock);
 	if (status)
@@ -124,27 +124,27 @@ static int update_trip_temp(struct intel_soc_dts_sensor_entry *dts,
 
 	temp_out = (sensors->tj_max - temp) / 1000;
 
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_PTPS, &store_ptps);
 	if (status)
 		return status;
 
 	out = (store_ptps & ~(0xFF << (thres_index * 8)));
 	out |= (temp_out & 0xFF) << (thres_index * 8);
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 				SOC_DTS_OFFSET_PTPS, out);
 	if (status)
 		return status;
 
 	pr_debug("update_trip_temp PTPS = %x\n", out);
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_PTMC, &out);
 	if (status)
 		goto err_restore_ptps;
 
 	store_ptmc = out;
 
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_TE_AUX0 + thres_index,
 			       &te_out);
 	if (status)
@@ -167,12 +167,12 @@ static int update_trip_temp(struct intel_soc_dts_sensor_entry *dts,
 			out &= ~SOC_DTS_AUX0_ENABLE_BIT;
 		te_out &= ~int_enable_bit;
 	}
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 				SOC_DTS_OFFSET_PTMC, out);
 	if (status)
 		goto err_restore_te_out;
 
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 				SOC_DTS_TE_AUX0 + thres_index,
 				te_out);
 	if (status)
@@ -182,13 +182,13 @@ static int update_trip_temp(struct intel_soc_dts_sensor_entry *dts,
 
 	return 0;
 err_restore_te_out:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 		       SOC_DTS_OFFSET_PTMC, store_te_out);
 err_restore_ptmc:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 		       SOC_DTS_OFFSET_PTMC, store_ptmc);
 err_restore_ptps:
-	iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 		       SOC_DTS_OFFSET_PTPS, store_ptps);
 	/* Nothing we can do if restore fails */
 
@@ -235,7 +235,7 @@ static int sys_get_curr_temp(struct thermal_zone_device *tzd,
 
 	dts = tzd->devdata;
 	sensors = dts->sensors;
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_TEMP, &out);
 	if (status)
 		return status;
@@ -259,14 +259,14 @@ static int soc_dts_enable(int id)
 	u32 out;
 	int ret;
 
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			    SOC_DTS_OFFSET_ENABLE, &out);
 	if (ret)
 		return ret;
 
 	if (!(out & BIT(id))) {
 		out |= BIT(id);
-		ret = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		ret = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 				     SOC_DTS_OFFSET_ENABLE, out);
 		if (ret)
 			return ret;
@@ -278,7 +278,7 @@ static int soc_dts_enable(int id)
 static void remove_dts_thermal_zone(struct intel_soc_dts_sensor_entry *dts)
 {
 	if (dts) {
-		iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 			       SOC_DTS_OFFSET_ENABLE, dts->store_status);
 		thermal_zone_device_unregister(dts->tzone);
 	}
@@ -296,9 +296,8 @@ static int add_dts_thermal_zone(int id, struct intel_soc_dts_sensor_entry *dts,
 	int i;
 
 	/* Store status to restor on exit */
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
-			    SOC_DTS_OFFSET_ENABLE,
-			    &dts->store_status);
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
+			    SOC_DTS_OFFSET_ENABLE, &dts->store_status);
 	if (ret)
 		goto err_ret;
 
@@ -311,7 +310,7 @@ static int add_dts_thermal_zone(int id, struct intel_soc_dts_sensor_entry *dts,
 	}
 
 	/* Check if the writable trip we provide is not used by BIOS */
-	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	ret = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			    SOC_DTS_OFFSET_PTPS, &store_ptps);
 	if (ret)
 		trip_mask = 0;
@@ -374,19 +373,19 @@ void intel_soc_dts_iosf_interrupt_handler(struct intel_soc_dts_sensors *sensors)
 
 	spin_lock_irqsave(&sensors->intr_notify_lock, flags);
 
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_PTMC, &ptmc_out);
 	ptmc_out |= SOC_DTS_PTMC_APIC_DEASSERT_BIT;
-	status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+	status = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 				SOC_DTS_OFFSET_PTMC, ptmc_out);
 
-	status = iosf_mbi_read(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_READ,
+	status = iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ,
 			       SOC_DTS_OFFSET_PTTSS, &sticky_out);
 	pr_debug("status %d PTTSS %x\n", status, sticky_out);
 	if (sticky_out & SOC_DTS_TRIP_MASK) {
 		int i;
 		/* reset sticky bit */
-		status = iosf_mbi_write(BT_MBI_UNIT_PMC, BT_MBI_BUNIT_WRITE,
+		status = iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE,
 					SOC_DTS_OFFSET_PTTSS, sticky_out);
 		spin_unlock_irqrestore(&sensors->intr_notify_lock, flags);
 
diff --git a/drivers/thermal/of-thermal.c b/drivers/thermal/of-thermal.c
index be4eedcb839a..9043f8f91852 100644
--- a/drivers/thermal/of-thermal.c
+++ b/drivers/thermal/of-thermal.c
@@ -475,14 +475,10 @@ thermal_zone_of_sensor_register(struct device *dev, int sensor_id, void *data,
 
 	sensor_np = of_node_get(dev->of_node);
 
-	for_each_child_of_node(np, child) {
+	for_each_available_child_of_node(np, child) {
 		struct of_phandle_args sensor_specs;
 		int ret, id;
 
-		/* Check whether child is enabled or not */
-		if (!of_device_is_available(child))
-			continue;
-
 		/* For now, thermal framework supports only 1 sensor per zone */
 		ret = of_parse_phandle_with_args(child, "thermal-sensors",
 						 "#thermal-sensor-cells",
@@ -881,16 +877,12 @@ int __init of_parse_thermal_zones(void)
 		return 0; /* Run successfully on systems without thermal DT */
 	}
 
-	for_each_child_of_node(np, child) {
+	for_each_available_child_of_node(np, child) {
 		struct thermal_zone_device *zone;
 		struct thermal_zone_params *tzp;
 		int i, mask = 0;
 		u32 prop;
 
-		/* Check whether child is enabled or not */
-		if (!of_device_is_available(child))
-			continue;
-
 		tz = thermal_of_build_thermal_zone(child);
 		if (IS_ERR(tz)) {
 			pr_err("failed to build thermal zone %s: %ld\n",
@@ -968,13 +960,9 @@ void of_thermal_destroy_zones(void)
 		return;
 	}
 
-	for_each_child_of_node(np, child) {
+	for_each_available_child_of_node(np, child) {
 		struct thermal_zone_device *zone;
 
-		/* Check whether child is enabled or not */
-		if (!of_device_is_available(child))
-			continue;
-
 		zone = thermal_zone_get_zone_by_name(child->name);
 		if (IS_ERR(zone))
 			continue;
diff --git a/drivers/thermal/rcar_thermal.c b/drivers/thermal/rcar_thermal.c
index 13d01edc7a04..0e735acea33a 100644
--- a/drivers/thermal/rcar_thermal.c
+++ b/drivers/thermal/rcar_thermal.c
@@ -23,6 +23,7 @@
 #include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/reboot.h>
@@ -75,11 +76,13 @@ struct rcar_thermal_priv {
 #define rcar_has_irq_support(priv)	((priv)->common->base)
 #define rcar_id_to_shift(priv)		((priv)->id * 8)
 
-#ifdef DEBUG
-# define rcar_force_update_temp(priv)	1
-#else
-# define rcar_force_update_temp(priv)	0
-#endif
+#define USE_OF_THERMAL	1
+static const struct of_device_id rcar_thermal_dt_ids[] = {
+	{ .compatible = "renesas,rcar-thermal", },
+	{ .compatible = "renesas,rcar-gen2-thermal", .data = (void *)USE_OF_THERMAL },
+	{},
+};
+MODULE_DEVICE_TABLE(of, rcar_thermal_dt_ids);
 
 /*
  *		basic functions
@@ -200,20 +203,46 @@ err_out_unlock:
 	return ret;
 }
 
-static int rcar_thermal_get_temp(struct thermal_zone_device *zone, int *temp)
+static int rcar_thermal_get_current_temp(struct rcar_thermal_priv *priv,
+					 int *temp)
 {
-	struct rcar_thermal_priv *priv = rcar_zone_to_priv(zone);
+	int tmp;
+	int ret;
 
-	if (!rcar_has_irq_support(priv) || rcar_force_update_temp(priv))
-		rcar_thermal_update_temp(priv);
+	ret = rcar_thermal_update_temp(priv);
+	if (ret < 0)
+		return ret;
 
 	mutex_lock(&priv->lock);
-	*temp =  MCELSIUS((priv->ctemp * 5) - 65);
+	tmp =  MCELSIUS((priv->ctemp * 5) - 65);
 	mutex_unlock(&priv->lock);
 
+	if ((tmp < MCELSIUS(-45)) || (tmp > MCELSIUS(125))) {
+		struct device *dev = rcar_priv_to_dev(priv);
+
+		dev_err(dev, "it couldn't measure temperature correctly\n");
+		return -EIO;
+	}
+
+	*temp = tmp;
+
 	return 0;
 }
 
+static int rcar_thermal_of_get_temp(void *data, int *temp)
+{
+	struct rcar_thermal_priv *priv = data;
+
+	return rcar_thermal_get_current_temp(priv, temp);
+}
+
+static int rcar_thermal_get_temp(struct thermal_zone_device *zone, int *temp)
+{
+	struct rcar_thermal_priv *priv = rcar_zone_to_priv(zone);
+
+	return rcar_thermal_get_current_temp(priv, temp);
+}
+
 static int rcar_thermal_get_trip_type(struct thermal_zone_device *zone,
 				      int trip, enum thermal_trip_type *type)
 {
@@ -270,6 +299,10 @@ static int rcar_thermal_notify(struct thermal_zone_device *zone,
 	return 0;
 }
 
+static const struct thermal_zone_of_device_ops rcar_thermal_zone_of_ops = {
+	.get_temp	= rcar_thermal_of_get_temp,
+};
+
 static struct thermal_zone_device_ops rcar_thermal_zone_ops = {
 	.get_temp	= rcar_thermal_get_temp,
 	.get_trip_type	= rcar_thermal_get_trip_type,
@@ -288,6 +321,9 @@ static void _rcar_thermal_irq_ctrl(struct rcar_thermal_priv *priv, int enable)
 	unsigned long flags;
 	u32 mask = 0x3 << rcar_id_to_shift(priv); /* enable Rising/Falling */
 
+	if (!rcar_has_irq_support(priv))
+		return;
+
 	spin_lock_irqsave(&common->lock, flags);
 
 	rcar_thermal_common_bset(common, INTMSK, mask, enable ? 0 : mask);
@@ -299,14 +335,24 @@ static void rcar_thermal_work(struct work_struct *work)
 {
 	struct rcar_thermal_priv *priv;
 	int cctemp, nctemp;
+	int ret;
 
 	priv = container_of(work, struct rcar_thermal_priv, work.work);
 
-	rcar_thermal_get_temp(priv->zone, &cctemp);
-	rcar_thermal_update_temp(priv);
+	ret = rcar_thermal_get_current_temp(priv, &cctemp);
+	if (ret < 0)
+		return;
+
+	ret = rcar_thermal_update_temp(priv);
+	if (ret < 0)
+		return;
+
 	rcar_thermal_irq_enable(priv);
 
-	rcar_thermal_get_temp(priv->zone, &nctemp);
+	ret = rcar_thermal_get_current_temp(priv, &nctemp);
+	if (ret < 0)
+		return;
+
 	if (nctemp != cctemp)
 		thermal_zone_device_update(priv->zone);
 }
@@ -368,8 +414,7 @@ static int rcar_thermal_remove(struct platform_device *pdev)
 	struct rcar_thermal_priv *priv;
 
 	rcar_thermal_for_each_priv(priv, common) {
-		if (rcar_has_irq_support(priv))
-			rcar_thermal_irq_disable(priv);
+		rcar_thermal_irq_disable(priv);
 		thermal_zone_device_unregister(priv->zone);
 	}
 
@@ -385,6 +430,8 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 	struct rcar_thermal_priv *priv;
 	struct device *dev = &pdev->dev;
 	struct resource *res, *irq;
+	const struct of_device_id *of_id = of_match_device(rcar_thermal_dt_ids, dev);
+	unsigned long of_data = (unsigned long)of_id->data;
 	int mres = 0;
 	int i;
 	int ret = -ENODEV;
@@ -441,9 +488,17 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 		mutex_init(&priv->lock);
 		INIT_LIST_HEAD(&priv->list);
 		INIT_DELAYED_WORK(&priv->work, rcar_thermal_work);
-		rcar_thermal_update_temp(priv);
+		ret = rcar_thermal_update_temp(priv);
+		if (ret < 0)
+			goto error_unregister;
 
-		priv->zone = thermal_zone_device_register("rcar_thermal",
+		if (of_data == USE_OF_THERMAL)
+			priv->zone = thermal_zone_of_sensor_register(
+						dev, i, priv,
+						&rcar_thermal_zone_of_ops);
+		else
+			priv->zone = thermal_zone_device_register(
+						"rcar_thermal",
 						1, 0, priv,
 						&rcar_thermal_zone_ops, NULL, 0,
 						idle);
@@ -453,8 +508,7 @@ static int rcar_thermal_probe(struct platform_device *pdev)
 			goto error_unregister;
 		}
 
-		if (rcar_has_irq_support(priv))
-			rcar_thermal_irq_enable(priv);
+		rcar_thermal_irq_enable(priv);
 
 		list_move_tail(&priv->list, &common->head);
 
@@ -484,12 +538,6 @@ error_unregister:
 	return ret;
 }
 
-static const struct of_device_id rcar_thermal_dt_ids[] = {
-	{ .compatible = "renesas,rcar-thermal", },
-	{},
-};
-MODULE_DEVICE_TABLE(of, rcar_thermal_dt_ids);
-
 static struct platform_driver rcar_thermal_driver = {
 	.driver	= {
 		.name	= "rcar_thermal",
diff --git a/drivers/thermal/rockchip_thermal.c b/drivers/thermal/rockchip_thermal.c
index e845841ab036..b58e3fb9b311 100644
--- a/drivers/thermal/rockchip_thermal.c
+++ b/drivers/thermal/rockchip_thermal.c
@@ -38,7 +38,7 @@ enum tshut_mode {
 };
 
 /**
- * the system Temperature Sensors tshut(tshut) polarity
+ * The system Temperature Sensors tshut(tshut) polarity
  * the bit 8 is tshut polarity.
  * 0: low active, 1: high active
  */
@@ -57,10 +57,10 @@ enum sensor_id {
 };
 
 /**
-* The conversion table has the adc value and temperature.
-* ADC_DECREMENT is the adc value decremnet.(e.g. v2_code_table)
-* ADC_INCREMNET is the adc value incremnet.(e.g. v3_code_table)
-*/
+ * The conversion table has the adc value and temperature.
+ * ADC_DECREMENT: the adc value is of diminishing.(e.g. v2_code_table)
+ * ADC_INCREMENT: the adc value is incremental.(e.g. v3_code_table)
+ */
 enum adc_sort_mode {
 	ADC_DECREMENT = 0,
 	ADC_INCREMENT,
@@ -72,16 +72,17 @@ enum adc_sort_mode {
  */
 #define SOC_MAX_SENSORS	2
 
+/**
+ * struct chip_tsadc_table: hold information about chip-specific differences
+ * @id: conversion table
+ * @length: size of conversion table
+ * @data_mask: mask to apply on data inputs
+ * @mode: sort mode of this adc variant (incrementing or decrementing)
+ */
 struct chip_tsadc_table {
 	const struct tsadc_table *id;
-
-	/* the array table size*/
 	unsigned int length;
-
-	/* that analogic mask data */
 	u32 data_mask;
-
-	/* the sort mode is adc value that increment or decrement in table */
 	enum adc_sort_mode mode;
 };
 
@@ -153,6 +154,7 @@ struct rockchip_thermal_data {
 #define TSADCV2_SHUT_2GPIO_SRC_EN(chn)		BIT(4 + (chn))
 #define TSADCV2_SHUT_2CRU_SRC_EN(chn)		BIT(8 + (chn))
 
+#define TSADCV1_INT_PD_CLEAR_MASK		~BIT(16)
 #define TSADCV2_INT_PD_CLEAR_MASK		~BIT(8)
 
 #define TSADCV2_DATA_MASK			0xfff
@@ -168,6 +170,51 @@ struct tsadc_table {
 	int temp;
 };
 
+/**
+ * Note:
+ * Code to Temperature mapping of the Temperature sensor is a piece wise linear
+ * curve.Any temperature, code faling between to 2 give temperatures can be
+ * linearly interpolated.
+ * Code to Temperature mapping should be updated based on sillcon results.
+ */
+static const struct tsadc_table v1_code_table[] = {
+	{TSADCV3_DATA_MASK, -40000},
+	{436, -40000},
+	{431, -35000},
+	{426, -30000},
+	{421, -25000},
+	{416, -20000},
+	{411, -15000},
+	{406, -10000},
+	{401, -5000},
+	{395, 0},
+	{390, 5000},
+	{385, 10000},
+	{380, 15000},
+	{375, 20000},
+	{370, 25000},
+	{364, 30000},
+	{359, 35000},
+	{354, 40000},
+	{349, 45000},
+	{343, 50000},
+	{338, 55000},
+	{333, 60000},
+	{328, 65000},
+	{322, 70000},
+	{317, 75000},
+	{312, 80000},
+	{307, 85000},
+	{301, 90000},
+	{296, 95000},
+	{291, 100000},
+	{286, 105000},
+	{280, 110000},
+	{275, 115000},
+	{270, 120000},
+	{264, 125000},
+};
+
 static const struct tsadc_table v2_code_table[] = {
 	{TSADCV2_DATA_MASK, -40000},
 	{3800, -40000},
@@ -245,6 +292,44 @@ static const struct tsadc_table v3_code_table[] = {
 	{TSADCV3_DATA_MASK, 125000},
 };
 
+static const struct tsadc_table v4_code_table[] = {
+	{TSADCV3_DATA_MASK, -40000},
+	{431, -40000},
+	{426, -35000},
+	{421, -30000},
+	{415, -25000},
+	{410, -20000},
+	{405, -15000},
+	{399, -10000},
+	{394, -5000},
+	{389, 0},
+	{383, 5000},
+	{378, 10000},
+	{373, 15000},
+	{367, 20000},
+	{362, 25000},
+	{357, 30000},
+	{351, 35000},
+	{346, 40000},
+	{340, 45000},
+	{335, 50000},
+	{330, 55000},
+	{324, 60000},
+	{319, 65000},
+	{313, 70000},
+	{308, 75000},
+	{302, 80000},
+	{297, 85000},
+	{291, 90000},
+	{286, 95000},
+	{281, 100000},
+	{275, 105000},
+	{270, 110000},
+	{264, 115000},
+	{259, 120000},
+	{253, 125000},
+};
+
 static u32 rk_tsadcv2_temp_to_code(struct chip_tsadc_table table,
 				   int temp)
 {
@@ -368,6 +453,14 @@ static void rk_tsadcv2_initialize(void __iomem *regs,
 		       regs + TSADCV2_HIGHT_TSHUT_DEBOUNCE);
 }
 
+static void rk_tsadcv1_irq_ack(void __iomem *regs)
+{
+	u32 val;
+
+	val = readl_relaxed(regs + TSADCV2_INT_PD);
+	writel_relaxed(val & TSADCV1_INT_PD_CLEAR_MASK, regs + TSADCV2_INT_PD);
+}
+
 static void rk_tsadcv2_irq_ack(void __iomem *regs)
 {
 	u32 val;
@@ -429,6 +522,29 @@ static void rk_tsadcv2_tshut_mode(int chn, void __iomem *regs,
 	writel_relaxed(val, regs + TSADCV2_INT_EN);
 }
 
+static const struct rockchip_tsadc_chip rk3228_tsadc_data = {
+	.chn_id[SENSOR_CPU] = 0, /* cpu sensor is channel 0 */
+	.chn_num = 1, /* one channel for tsadc */
+
+	.tshut_mode = TSHUT_MODE_GPIO, /* default TSHUT via GPIO give PMIC */
+	.tshut_polarity = TSHUT_LOW_ACTIVE, /* default TSHUT LOW ACTIVE */
+	.tshut_temp = 95000,
+
+	.initialize = rk_tsadcv2_initialize,
+	.irq_ack = rk_tsadcv1_irq_ack,
+	.control = rk_tsadcv2_control,
+	.get_temp = rk_tsadcv2_get_temp,
+	.set_tshut_temp = rk_tsadcv2_tshut_temp,
+	.set_tshut_mode = rk_tsadcv2_tshut_mode,
+
+	.table = {
+		.id = v1_code_table,
+		.length = ARRAY_SIZE(v1_code_table),
+		.data_mask = TSADCV3_DATA_MASK,
+		.mode = ADC_DECREMENT,
+	},
+};
+
 static const struct rockchip_tsadc_chip rk3288_tsadc_data = {
 	.chn_id[SENSOR_CPU] = 1, /* cpu sensor is channel 1 */
 	.chn_id[SENSOR_GPU] = 2, /* gpu sensor is channel 2 */
@@ -477,8 +593,36 @@ static const struct rockchip_tsadc_chip rk3368_tsadc_data = {
 	},
 };
 
+static const struct rockchip_tsadc_chip rk3399_tsadc_data = {
+	.chn_id[SENSOR_CPU] = 0, /* cpu sensor is channel 0 */
+	.chn_id[SENSOR_GPU] = 1, /* gpu sensor is channel 1 */
+	.chn_num = 2, /* two channels for tsadc */
+
+	.tshut_mode = TSHUT_MODE_GPIO, /* default TSHUT via GPIO give PMIC */
+	.tshut_polarity = TSHUT_LOW_ACTIVE, /* default TSHUT LOW ACTIVE */
+	.tshut_temp = 95000,
+
+	.initialize = rk_tsadcv2_initialize,
+	.irq_ack = rk_tsadcv1_irq_ack,
+	.control = rk_tsadcv2_control,
+	.get_temp = rk_tsadcv2_get_temp,
+	.set_tshut_temp = rk_tsadcv2_tshut_temp,
+	.set_tshut_mode = rk_tsadcv2_tshut_mode,
+
+	.table = {
+		.id = v4_code_table,
+		.length = ARRAY_SIZE(v4_code_table),
+		.data_mask = TSADCV3_DATA_MASK,
+		.mode = ADC_DECREMENT,
+	},
+};
+
 static const struct of_device_id of_rockchip_thermal_match[] = {
 	{
+		.compatible = "rockchip,rk3228-tsadc",
+		.data = (void *)&rk3228_tsadc_data,
+	},
+	{
 		.compatible = "rockchip,rk3288-tsadc",
 		.data = (void *)&rk3288_tsadc_data,
 	},
@@ -486,6 +630,10 @@ static const struct of_device_id of_rockchip_thermal_match[] = {
 		.compatible = "rockchip,rk3368-tsadc",
 		.data = (void *)&rk3368_tsadc_data,
 	},
+	{
+		.compatible = "rockchip,rk3399-tsadc",
+		.data = (void *)&rk3399_tsadc_data,
+	},
 	{ /* end */ },
 };
 MODULE_DEVICE_TABLE(of, of_rockchip_thermal_match);
@@ -617,7 +765,7 @@ rockchip_thermal_register_sensor(struct platform_device *pdev,
 	return 0;
 }
 
-/*
+/**
  * Reset TSADC Controller, reset all tsadc registers.
  */
 static void rockchip_thermal_reset_controller(struct reset_control *reset)
diff --git a/drivers/thermal/spear_thermal.c b/drivers/thermal/spear_thermal.c
index 534dd9136662..81b35aace9de 100644
--- a/drivers/thermal/spear_thermal.c
+++ b/drivers/thermal/spear_thermal.c
@@ -54,8 +54,7 @@ static struct thermal_zone_device_ops ops = {
 	.get_temp = thermal_get_temp,
 };
 
-#ifdef CONFIG_PM
-static int spear_thermal_suspend(struct device *dev)
+static int __maybe_unused spear_thermal_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct thermal_zone_device *spear_thermal = platform_get_drvdata(pdev);
@@ -72,7 +71,7 @@ static int spear_thermal_suspend(struct device *dev)
 	return 0;
 }
 
-static int spear_thermal_resume(struct device *dev)
+static int __maybe_unused spear_thermal_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct thermal_zone_device *spear_thermal = platform_get_drvdata(pdev);
@@ -94,7 +93,6 @@ static int spear_thermal_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 static SIMPLE_DEV_PM_OPS(spear_thermal_pm_ops, spear_thermal_suspend,
 		spear_thermal_resume);
diff --git a/drivers/thermal/step_wise.c b/drivers/thermal/step_wise.c
index 2f9f7086ac3d..ea9366ad3e6b 100644
--- a/drivers/thermal/step_wise.c
+++ b/drivers/thermal/step_wise.c
@@ -63,6 +63,19 @@ static unsigned long get_target_state(struct thermal_instance *instance,
 	next_target = instance->target;
 	dev_dbg(&cdev->device, "cur_state=%ld\n", cur_state);
 
+	if (!instance->initialized) {
+		if (throttle) {
+			next_target = (cur_state + 1) >= instance->upper ?
+					instance->upper :
+					((cur_state + 1) < instance->lower ?
+					instance->lower : (cur_state + 1));
+		} else {
+			next_target = THERMAL_NO_TARGET;
+		}
+
+		return next_target;
+	}
+
 	switch (trend) {
 	case THERMAL_TREND_RAISING:
 		if (throttle) {
@@ -149,7 +162,7 @@ static void thermal_zone_trip_update(struct thermal_zone_device *tz, int trip)
 		dev_dbg(&instance->cdev->device, "old_target=%d, target=%d\n",
 					old_target, (int)instance->target);
 
-		if (old_target == instance->target)
+		if (instance->initialized && old_target == instance->target)
 			continue;
 
 		/* Activate a passive thermal instance */
@@ -161,7 +174,7 @@ static void thermal_zone_trip_update(struct thermal_zone_device *tz, int trip)
 			instance->target == THERMAL_NO_TARGET)
 			update_passive_instance(tz, trip_type, -1);
 
-
+		instance->initialized = true;
 		instance->cdev->updated = false; /* cdev needs update */
 	}
 
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index d9e525cc9c1c..a0a8fd1235e2 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -37,6 +37,7 @@
 #include <linux/of.h>
 #include <net/netlink.h>
 #include <net/genetlink.h>
+#include <linux/suspend.h>
 
 #define CREATE_TRACE_POINTS
 #include <trace/events/thermal.h>
@@ -59,6 +60,8 @@ static LIST_HEAD(thermal_governor_list);
 static DEFINE_MUTEX(thermal_list_lock);
 static DEFINE_MUTEX(thermal_governor_lock);
 
+static atomic_t in_suspend;
+
 static struct thermal_governor *def_governor;
 
 static struct thermal_governor *__find_governor(const char *name)
@@ -532,14 +535,31 @@ static void update_temperature(struct thermal_zone_device *tz)
 	mutex_unlock(&tz->lock);
 
 	trace_thermal_temperature(tz);
-	dev_dbg(&tz->device, "last_temperature=%d, current_temperature=%d\n",
-				tz->last_temperature, tz->temperature);
+	if (tz->last_temperature == THERMAL_TEMP_INVALID)
+		dev_dbg(&tz->device, "last_temperature N/A, current_temperature=%d\n",
+			tz->temperature);
+	else
+		dev_dbg(&tz->device, "last_temperature=%d, current_temperature=%d\n",
+			tz->last_temperature, tz->temperature);
+}
+
+static void thermal_zone_device_reset(struct thermal_zone_device *tz)
+{
+	struct thermal_instance *pos;
+
+	tz->temperature = THERMAL_TEMP_INVALID;
+	tz->passive = 0;
+	list_for_each_entry(pos, &tz->thermal_instances, tz_node)
+		pos->initialized = false;
 }
 
 void thermal_zone_device_update(struct thermal_zone_device *tz)
 {
 	int count;
 
+	if (atomic_read(&in_suspend))
+		return;
+
 	if (!tz->ops->get_temp)
 		return;
 
@@ -676,8 +696,12 @@ trip_point_temp_store(struct device *dev, struct device_attribute *attr,
 		return -EINVAL;
 
 	ret = tz->ops->set_trip_temp(tz, trip, temperature);
+	if (ret)
+		return ret;
 
-	return ret ? ret : count;
+	thermal_zone_device_update(tz);
+
+	return count;
 }
 
 static ssize_t
@@ -1321,6 +1345,7 @@ int thermal_zone_bind_cooling_device(struct thermal_zone_device *tz,
 	if (!result) {
 		list_add_tail(&dev->tz_node, &tz->thermal_instances);
 		list_add_tail(&dev->cdev_node, &cdev->thermal_instances);
+		atomic_set(&tz->need_update, 1);
 	}
 	mutex_unlock(&cdev->lock);
 	mutex_unlock(&tz->lock);
@@ -1430,6 +1455,7 @@ __thermal_cooling_device_register(struct device_node *np,
 				  const struct thermal_cooling_device_ops *ops)
 {
 	struct thermal_cooling_device *cdev;
+	struct thermal_zone_device *pos = NULL;
 	int result;
 
 	if (type && strlen(type) >= THERMAL_NAME_LENGTH)
@@ -1474,6 +1500,12 @@ __thermal_cooling_device_register(struct device_node *np,
 	/* Update binding information for 'this' new cdev */
 	bind_cdev(cdev);
 
+	mutex_lock(&thermal_list_lock);
+	list_for_each_entry(pos, &thermal_tz_list, node)
+		if (atomic_cmpxchg(&pos->need_update, 1, 0))
+			thermal_zone_device_update(pos);
+	mutex_unlock(&thermal_list_lock);
+
 	return cdev;
 }
 
@@ -1806,6 +1838,8 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 	tz->trips = trips;
 	tz->passive_delay = passive_delay;
 	tz->polling_delay = polling_delay;
+	/* A new thermal zone needs to be updated anyway. */
+	atomic_set(&tz->need_update, 1);
 
 	dev_set_name(&tz->device, "thermal_zone%d", tz->id);
 	result = device_register(&tz->device);
@@ -1900,7 +1934,10 @@ struct thermal_zone_device *thermal_zone_device_register(const char *type,
 
 	INIT_DELAYED_WORK(&(tz->poll_queue), thermal_zone_device_check);
 
-	thermal_zone_device_update(tz);
+	thermal_zone_device_reset(tz);
+	/* Update the new thermal zone and mark it as already updated. */
+	if (atomic_cmpxchg(&tz->need_update, 1, 0))
+		thermal_zone_device_update(tz);
 
 	return tz;
 
@@ -2140,6 +2177,36 @@ static void thermal_unregister_governors(void)
 	thermal_gov_power_allocator_unregister();
 }
 
+static int thermal_pm_notify(struct notifier_block *nb,
+				unsigned long mode, void *_unused)
+{
+	struct thermal_zone_device *tz;
+
+	switch (mode) {
+	case PM_HIBERNATION_PREPARE:
+	case PM_RESTORE_PREPARE:
+	case PM_SUSPEND_PREPARE:
+		atomic_set(&in_suspend, 1);
+		break;
+	case PM_POST_HIBERNATION:
+	case PM_POST_RESTORE:
+	case PM_POST_SUSPEND:
+		atomic_set(&in_suspend, 0);
+		list_for_each_entry(tz, &thermal_tz_list, node) {
+			thermal_zone_device_reset(tz);
+			thermal_zone_device_update(tz);
+		}
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static struct notifier_block thermal_pm_nb = {
+	.notifier_call = thermal_pm_notify,
+};
+
 static int __init thermal_init(void)
 {
 	int result;
@@ -2160,6 +2227,11 @@ static int __init thermal_init(void)
 	if (result)
 		goto exit_netlink;
 
+	result = register_pm_notifier(&thermal_pm_nb);
+	if (result)
+		pr_warn("Thermal: Can not register suspend notifier, return %d\n",
+			result);
+
 	return 0;
 
 exit_netlink:
@@ -2179,6 +2251,7 @@ error:
 
 static void __exit thermal_exit(void)
 {
+	unregister_pm_notifier(&thermal_pm_nb);
 	of_thermal_destroy_zones();
 	genetlink_exit();
 	class_unregister(&thermal_class);
diff --git a/drivers/thermal/thermal_core.h b/drivers/thermal/thermal_core.h
index d7ac1fccd659..749d41abfbab 100644
--- a/drivers/thermal/thermal_core.h
+++ b/drivers/thermal/thermal_core.h
@@ -41,6 +41,7 @@ struct thermal_instance {
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
 	int trip;
+	bool initialized;
 	unsigned long upper;	/* Highest cooling state for this trip point */
 	unsigned long lower;	/* Lowest cooling state for this trip point */
 	unsigned long target;	/* expected cooling state */
diff --git a/drivers/tty/amiserial.c b/drivers/tty/amiserial.c
index e53d9a512c6d..2caaf5a2516d 100644
--- a/drivers/tty/amiserial.c
+++ b/drivers/tty/amiserial.c
@@ -32,7 +32,6 @@
 #include <linux/delay.h>
 
 #undef SERIAL_PARANOIA_CHECK
-#define SERIAL_DO_RESTART
 
 /* Set of debugging defines */
 
diff --git a/drivers/tty/cyclades.c b/drivers/tty/cyclades.c
index d4a1331675ed..abbed201dc74 100644
--- a/drivers/tty/cyclades.c
+++ b/drivers/tty/cyclades.c
@@ -292,14 +292,14 @@ static void cyz_rx_restart(unsigned long);
 static struct timer_list cyz_rx_full_timer[NR_PORTS];
 #endif				/* CONFIG_CYZ_INTR */
 
-static inline void cyy_writeb(struct cyclades_port *port, u32 reg, u8 val)
+static void cyy_writeb(struct cyclades_port *port, u32 reg, u8 val)
 {
 	struct cyclades_card *card = port->card;
 
 	cy_writeb(port->u.cyy.base_addr + (reg << card->bus_index), val);
 }
 
-static inline u8 cyy_readb(struct cyclades_port *port, u32 reg)
+static u8 cyy_readb(struct cyclades_port *port, u32 reg)
 {
 	struct cyclades_card *card = port->card;
 
@@ -321,7 +321,7 @@ static inline bool cyz_fpga_loaded(struct cyclades_card *card)
 	return __cyz_fpga_loaded(card->ctl_addr.p9060);
 }
 
-static inline bool cyz_is_loaded(struct cyclades_card *card)
+static bool cyz_is_loaded(struct cyclades_card *card)
 {
 	struct FIRM_ID __iomem *fw_id = card->base_addr + ID_ADDRESS;
 
@@ -329,7 +329,7 @@ static inline bool cyz_is_loaded(struct cyclades_card *card)
 			readl(&fw_id->signature) == ZFIRM_ID;
 }
 
-static inline int serial_paranoia_check(struct cyclades_port *info,
+static int serial_paranoia_check(struct cyclades_port *info,
 		const char *name, const char *routine)
 {
 #ifdef SERIAL_PARANOIA_CHECK
diff --git a/drivers/tty/isicom.c b/drivers/tty/isicom.c
index 2054427992e0..99875949bfb7 100644
--- a/drivers/tty/isicom.c
+++ b/drivers/tty/isicom.c
@@ -220,7 +220,7 @@ static struct isi_port  isi_ports[PORT_COUNT];
  *	it wants to talk.
  */
 
-static inline int WaitTillCardIsFree(unsigned long base)
+static int WaitTillCardIsFree(unsigned long base)
 {
 	unsigned int count = 0;
 	unsigned int a = in_atomic(); /* do we run under spinlock? */
@@ -280,7 +280,7 @@ static void raise_dtr(struct isi_port *port)
 }
 
 /* card->lock HAS to be held */
-static inline void drop_dtr(struct isi_port *port)
+static void drop_dtr(struct isi_port *port)
 {
 	struct isi_board *card = port->card;
 	unsigned long base = card->base;
diff --git a/drivers/tty/moxa.c b/drivers/tty/moxa.c
index 14c54e041065..92982d7c0489 100644
--- a/drivers/tty/moxa.c
+++ b/drivers/tty/moxa.c
@@ -155,7 +155,6 @@ struct mon_str {
 #define LOWWAIT 	2
 #define EMPTYWAIT	3
 
-#define SERIAL_DO_RESTART
 
 #define WAKEUP_CHARS		256
 
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index e49c2bce551d..b280abaad91b 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -162,12 +162,23 @@ static inline int tty_put_user(struct tty_struct *tty, unsigned char x,
 	return put_user(x, ptr);
 }
 
-static inline int tty_copy_to_user(struct tty_struct *tty,
-					void __user *to,
-					const void *from,
-					unsigned long n)
+static int tty_copy_to_user(struct tty_struct *tty, void __user *to,
+			    size_t tail, size_t n)
 {
 	struct n_tty_data *ldata = tty->disc_data;
+	size_t size = N_TTY_BUF_SIZE - tail;
+	const void *from = read_buf_addr(ldata, tail);
+	int uncopied;
+
+	if (n > size) {
+		tty_audit_add_data(tty, from, size, ldata->icanon);
+		uncopied = copy_to_user(to, from, size);
+		if (uncopied)
+			return uncopied;
+		to += size;
+		n -= size;
+		from = ldata->read_buf;
+	}
 
 	tty_audit_add_data(tty, from, n, ldata->icanon);
 	return copy_to_user(to, from, n);
@@ -258,16 +269,13 @@ static void n_tty_check_throttle(struct tty_struct *tty)
 
 static void n_tty_check_unthrottle(struct tty_struct *tty)
 {
-	if (tty->driver->type == TTY_DRIVER_TYPE_PTY &&
-	    tty->link->ldisc->ops->write_wakeup == n_tty_write_wakeup) {
+	if (tty->driver->type == TTY_DRIVER_TYPE_PTY) {
 		if (chars_in_buffer(tty) > TTY_THRESHOLD_UNTHROTTLE)
 			return;
 		if (!tty->count)
 			return;
 		n_tty_kick_worker(tty);
-		n_tty_write_wakeup(tty->link);
-		if (waitqueue_active(&tty->link->write_wait))
-			wake_up_interruptible_poll(&tty->link->write_wait, POLLOUT);
+		tty_wakeup(tty->link);
 		return;
 	}
 
@@ -1201,9 +1209,7 @@ static void n_tty_receive_overrun(struct tty_struct *tty)
 	ldata->num_overrun++;
 	if (time_after(jiffies, ldata->overrun_time + HZ) ||
 			time_after(ldata->overrun_time, jiffies)) {
-		printk(KERN_WARNING "%s: %d input overrun(s)\n",
-			tty_name(tty),
-			ldata->num_overrun);
+		tty_warn(tty, "%d input overrun(s)\n", ldata->num_overrun);
 		ldata->overrun_time = jiffies;
 		ldata->num_overrun = 0;
 	}
@@ -1486,8 +1492,7 @@ n_tty_receive_char_flagged(struct tty_struct *tty, unsigned char c, char flag)
 		n_tty_receive_overrun(tty);
 		break;
 	default:
-		printk(KERN_ERR "%s: unknown flag %d\n",
-		       tty_name(tty), flag);
+		tty_err(tty, "unknown flag %d\n", flag);
 		break;
 	}
 }
@@ -2006,11 +2011,11 @@ static int copy_from_read_buf(struct tty_struct *tty,
 	n = min(head - ldata->read_tail, N_TTY_BUF_SIZE - tail);
 	n = min(*nr, n);
 	if (n) {
-		retval = copy_to_user(*b, read_buf_addr(ldata, tail), n);
+		const unsigned char *from = read_buf_addr(ldata, tail);
+		retval = copy_to_user(*b, from, n);
 		n -= retval;
-		is_eof = n == 1 && read_buf(ldata, tail) == EOF_CHAR(tty);
-		tty_audit_add_data(tty, read_buf_addr(ldata, tail), n,
-				ldata->icanon);
+		is_eof = n == 1 && *from == EOF_CHAR(tty);
+		tty_audit_add_data(tty, from, n, ldata->icanon);
 		smp_store_release(&ldata->read_tail, ldata->read_tail + n);
 		/* Turn single EOF into zero-length read */
 		if (L_EXTPROC(tty) && ldata->icanon && is_eof &&
@@ -2072,12 +2077,10 @@ static int canon_copy_from_read_buf(struct tty_struct *tty,
 	if (eol == N_TTY_BUF_SIZE && more) {
 		/* scan wrapped without finding set bit */
 		eol = find_next_bit(ldata->read_flags, more, 0);
-		if (eol != more)
-			found = 1;
-	} else if (eol != size)
-		found = 1;
+		found = eol != more;
+	} else
+		found = eol != size;
 
-	size = N_TTY_BUF_SIZE - tail;
 	n = eol - tail;
 	if (n > N_TTY_BUF_SIZE)
 		n += N_TTY_BUF_SIZE;
@@ -2088,17 +2091,10 @@ static int canon_copy_from_read_buf(struct tty_struct *tty,
 		n = c;
 	}
 
-	n_tty_trace("%s: eol:%zu found:%d n:%zu c:%zu size:%zu more:%zu\n",
-		    __func__, eol, found, n, c, size, more);
-
-	if (n > size) {
-		ret = tty_copy_to_user(tty, *b, read_buf_addr(ldata, tail), size);
-		if (ret)
-			return -EFAULT;
-		ret = tty_copy_to_user(tty, *b + size, ldata->read_buf, n - size);
-	} else
-		ret = tty_copy_to_user(tty, *b, read_buf_addr(ldata, tail), n);
+	n_tty_trace("%s: eol:%zu found:%d n:%zu c:%zu tail:%zu more:%zu\n",
+		    __func__, eol, found, n, c, tail, more);
 
+	ret = tty_copy_to_user(tty, *b, tail, n);
 	if (ret)
 		return -EFAULT;
 	*b += n;
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index a45660f62db5..2348fa613707 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -681,7 +681,14 @@ static void pty_unix98_remove(struct tty_driver *driver, struct tty_struct *tty)
 /* this is called once with whichever end is closed last */
 static void pty_unix98_shutdown(struct tty_struct *tty)
 {
-	devpts_kill_index(tty->driver_data, tty->index);
+	struct inode *ptmx_inode;
+
+	if (tty->driver->subtype == PTY_TYPE_MASTER)
+		ptmx_inode = tty->driver_data;
+	else
+		ptmx_inode = tty->link->driver_data;
+	devpts_kill_index(ptmx_inode, tty->index);
+	devpts_del_ref(ptmx_inode);
 }
 
 static const struct tty_operations ptm_unix98_ops = {
@@ -773,6 +780,18 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 	set_bit(TTY_PTY_LOCK, &tty->flags); /* LOCK THE SLAVE */
 	tty->driver_data = inode;
 
+	/*
+	 * In the case where all references to ptmx inode are dropped and we
+	 * still have /dev/tty opened pointing to the master/slave pair (ptmx
+	 * is closed/released before /dev/tty), we must make sure that the inode
+	 * is still valid when we call the final pty_unix98_shutdown, thus we
+	 * hold an additional reference to the ptmx inode. For the same /dev/tty
+	 * last close case, we also need to make sure the super_block isn't
+	 * destroyed (devpts instance unmounted), before /dev/tty is closed and
+	 * on its release devpts_kill_index is called.
+	 */
+	devpts_add_ref(inode);
+
 	tty_add_file(tty, filp);
 
 	slave_inode = devpts_pty_new(inode,
@@ -788,7 +807,7 @@ static int ptmx_open(struct inode *inode, struct file *filp)
 	if (retval)
 		goto err_release;
 
-	tty_debug_hangup(tty, "(tty count=%d)\n", tty->count);
+	tty_debug_hangup(tty, "opening (count=%d)\n", tty->count);
 
 	tty_unlock(tty);
 	return 0;
diff --git a/drivers/tty/serial/68328serial.c b/drivers/tty/serial/68328serial.c
index 0140ba4aacde..0982c1a44187 100644
--- a/drivers/tty/serial/68328serial.c
+++ b/drivers/tty/serial/68328serial.c
@@ -157,7 +157,7 @@ static void change_speed(struct m68k_serial *info, struct tty_struct *tty);
 #endif
 
 
-static int m68328_console_initted = 0;
+static int m68328_console_initted;
 static int m68328_console_baud    = CONSOLE_BAUD_RATE;
 static int m68328_console_cbaud   = DEFAULT_CBAUD;
 
@@ -274,8 +274,8 @@ static void receive_chars(struct m68k_serial *info, unsigned short rx)
 #endif	
 		ch = GET_FIELD(rx, URX_RXDATA);
 	
-		if(info->is_cons) {
-			if(URX_BREAK & rx) { /* whee, break received */
+		if (info->is_cons) {
+			if (URX_BREAK & rx) { /* whee, break received */
 				return;
 #ifdef CONFIG_MAGIC_SYSRQ
 			} else if (ch == 0x10) { /* ^P */
@@ -302,7 +302,7 @@ static void receive_chars(struct m68k_serial *info, unsigned short rx)
 
 		tty_insert_flip_char(&info->tport, ch, flag);
 #ifndef CONFIG_XCOPILOT_BUGS
-	} while((rx = uart->urx.w) & URX_DATA_READY);
+	} while ((rx = uart->urx.w) & URX_DATA_READY);
 #endif
 
 	tty_schedule_flip(&info->tport);
@@ -330,7 +330,7 @@ static void transmit_chars(struct m68k_serial *info, struct tty_struct *tty)
 	info->xmit_tail = info->xmit_tail & (SERIAL_XMIT_SIZE-1);
 	info->xmit_cnt--;
 
-	if(info->xmit_cnt <= 0) {
+	if (info->xmit_cnt <= 0) {
 		/* All done for now... TX ints off */
 		uart->ustcnt &= ~USTCNT_TX_INTR_MASK;
 		goto clear_and_return;
@@ -452,45 +452,45 @@ struct {
 }
 #ifndef CONFIG_M68VZ328
  hw_baud_table[18] = {
-	{0,0}, /* 0 */
-	{0,0}, /* 50 */
-	{0,0}, /* 75 */
-	{0,0}, /* 110 */
-	{0,0}, /* 134 */
-	{0,0}, /* 150 */
-	{0,0}, /* 200 */
-	{7,0x26}, /* 300 */
-	{6,0x26}, /* 600 */
-	{5,0x26}, /* 1200 */
-	{0,0}, /* 1800 */
-	{4,0x26}, /* 2400 */
-	{3,0x26}, /* 4800 */
-	{2,0x26}, /* 9600 */
-	{1,0x26}, /* 19200 */
-	{0,0x26}, /* 38400 */
-	{1,0x38}, /* 57600 */
-	{0,0x38}, /* 115200 */
+	{0, 0}, /* 0 */
+	{0, 0}, /* 50 */
+	{0, 0}, /* 75 */
+	{0, 0}, /* 110 */
+	{0, 0}, /* 134 */
+	{0, 0}, /* 150 */
+	{0, 0}, /* 200 */
+	{7, 0x26}, /* 300 */
+	{6, 0x26}, /* 600 */
+	{5, 0x26}, /* 1200 */
+	{0, 0}, /* 1800 */
+	{4, 0x26}, /* 2400 */
+	{3, 0x26}, /* 4800 */
+	{2, 0x26}, /* 9600 */
+	{1, 0x26}, /* 19200 */
+	{0, 0x26}, /* 38400 */
+	{1, 0x38}, /* 57600 */
+	{0, 0x38}, /* 115200 */
 };
 #else
  hw_baud_table[18] = {
-                 {0,0}, /* 0 */
-                 {0,0}, /* 50 */
-                 {0,0}, /* 75 */
-                 {0,0}, /* 110 */
-                 {0,0}, /* 134 */
-                 {0,0}, /* 150 */
-                 {0,0}, /* 200 */
-                 {0,0}, /* 300 */
-                 {7,0x26}, /* 600 */
-                 {6,0x26}, /* 1200 */
-                 {0,0}, /* 1800 */
-                 {5,0x26}, /* 2400 */
-                 {4,0x26}, /* 4800 */
-                 {3,0x26}, /* 9600 */
-                 {2,0x26}, /* 19200 */
-                 {1,0x26}, /* 38400 */
-                 {0,0x26}, /* 57600 */
-                 {1,0x38}, /* 115200 */
+                 {0, 0}, /* 0 */
+                 {0, 0}, /* 50 */
+                 {0, 0}, /* 75 */
+                 {0, 0}, /* 110 */
+                 {0, 0}, /* 134 */
+                 {0, 0}, /* 150 */
+                 {0, 0}, /* 200 */
+                 {0, 0}, /* 300 */
+                 {7, 0x26}, /* 600 */
+                 {6, 0x26}, /* 1200 */
+                 {0, 0}, /* 1800 */
+                 {5, 0x26}, /* 2400 */
+                 {4, 0x26}, /* 4800 */
+                 {3, 0x26}, /* 9600 */
+                 {2, 0x26}, /* 19200 */
+                 {1, 0x26}, /* 38400 */
+                 {0, 0x26}, /* 57600 */
+                 {1, 0x38}, /* 115200 */
 }; 
 #endif
 /* rate = 1036800 / ((65 - prescale) * (1<<divider)) */
@@ -538,7 +538,7 @@ static void change_speed(struct m68k_serial *info, struct tty_struct *tty)
 	
 #ifdef CONFIG_SERIAL_68328_RTS_CTS
 	if (cflag & CRTSCTS) {
-		uart->utx.w &= ~ UTX_NOCTS;
+		uart->utx.w &= ~UTX_NOCTS;
 	} else {
 		uart->utx.w |= UTX_NOCTS;
 	}
@@ -591,8 +591,8 @@ void console_print_68328(const char *p)
 {
 	char c;
 	
-	while((c=*(p++)) != 0) {
-		if(c == '\n')
+	while ((c = *(p++)) != 0) {
+		if (c == '\n')
 			rs_put_char('\r');
 		rs_put_char(c);
 	}
@@ -624,7 +624,7 @@ static void rs_flush_chars(struct tty_struct *tty)
 	if (serial_paranoia_check(info, tty->name, "rs_flush_chars"))
 		return;
 #ifndef USE_INTS
-	for(;;) {
+	for (;;) {
 #endif
 
 	/* Enable transmitter */
@@ -659,9 +659,9 @@ static void rs_flush_chars(struct tty_struct *tty)
 	local_irq_restore(flags);
 }
 
-extern void console_printn(const char * b, int count);
+extern void console_printn(const char *b, int count);
 
-static int rs_write(struct tty_struct * tty,
+static int rs_write(struct tty_struct *tty,
 		    const unsigned char *buf, int count)
 {
 	int	c, total = 0;
@@ -700,7 +700,7 @@ static int rs_write(struct tty_struct * tty,
 		/* Enable transmitter */
 		local_irq_disable();		
 #ifndef USE_INTS
-		while(info->xmit_cnt) {
+		while (info->xmit_cnt) {
 #endif
 
 		uart->ustcnt |= USTCNT_TXEN;
@@ -767,7 +767,7 @@ static void rs_flush_buffer(struct tty_struct *tty)
  * incoming characters should be throttled.
  * ------------------------------------------------------------
  */
-static void rs_throttle(struct tty_struct * tty)
+static void rs_throttle(struct tty_struct *tty)
 {
 	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 
@@ -780,7 +780,7 @@ static void rs_throttle(struct tty_struct * tty)
 	/* Turn off RTS line (do this atomic) */
 }
 
-static void rs_unthrottle(struct tty_struct * tty)
+static void rs_unthrottle(struct tty_struct *tty)
 {
 	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 
@@ -803,8 +803,8 @@ static void rs_unthrottle(struct tty_struct * tty)
  * ------------------------------------------------------------
  */
 
-static int get_serial_info(struct m68k_serial * info,
-			   struct serial_struct * retinfo)
+static int get_serial_info(struct m68k_serial *info,
+			   struct serial_struct *retinfo)
 {
 	struct serial_struct tmp;
   
@@ -827,7 +827,7 @@ static int get_serial_info(struct m68k_serial * info,
 }
 
 static int set_serial_info(struct m68k_serial *info, struct tty_struct *tty,
-			   struct serial_struct * new_info)
+			   struct serial_struct *new_info)
 {
 	struct tty_port *port = &info->tport;
 	struct serial_struct new_serial;
@@ -883,7 +883,7 @@ check_and_exit:
  * 	    transmit holding register is empty.  This functionality
  * 	    allows an RS485 driver to be written in user space. 
  */
-static int get_lsr_info(struct m68k_serial * info, unsigned int *value)
+static int get_lsr_info(struct m68k_serial *info, unsigned int *value)
 {
 #ifdef CONFIG_SERIAL_68328_RTS_CTS
 	m68328_uart *uart = &uart_addr[info->line];
@@ -904,7 +904,7 @@ static int get_lsr_info(struct m68k_serial * info, unsigned int *value)
 /*
  * This routine sends a break character out the serial port.
  */
-static void send_break(struct m68k_serial * info, unsigned int duration)
+static void send_break(struct m68k_serial *info, unsigned int duration)
 {
 	m68328_uart *uart = &uart_addr[info->line];
         unsigned long flags;
@@ -922,7 +922,7 @@ static void send_break(struct m68k_serial * info, unsigned int duration)
 static int rs_ioctl(struct tty_struct *tty,
 		    unsigned int cmd, unsigned long arg)
 {
-	struct m68k_serial * info = (struct m68k_serial *)tty->driver_data;
+	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 	int retval;
 
 	if (serial_paranoia_check(info, tty->name, "rs_ioctl"))
@@ -992,9 +992,9 @@ static void rs_set_termios(struct tty_struct *tty, struct ktermios *old_termios)
  * that IRQ if nothing is left in the chain.
  * ------------------------------------------------------------
  */
-static void rs_close(struct tty_struct *tty, struct file * filp)
+static void rs_close(struct tty_struct *tty, struct file *filp)
 {
-	struct m68k_serial * info = (struct m68k_serial *)tty->driver_data;
+	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 	struct tty_port *port = &info->tport;
 	m68328_uart *uart = &uart_addr[info->line];
 	unsigned long flags;
@@ -1079,7 +1079,7 @@ static void rs_close(struct tty_struct *tty, struct file * filp)
  */
 void rs_hangup(struct tty_struct *tty)
 {
-	struct m68k_serial * info = (struct m68k_serial *)tty->driver_data;
+	struct m68k_serial *info = (struct m68k_serial *)tty->driver_data;
 	
 	if (serial_paranoia_check(info, tty->name, "rs_hangup"))
 		return;
@@ -1098,7 +1098,7 @@ void rs_hangup(struct tty_struct *tty)
  * the IRQ chain.   It also performs the serial-specific
  * initialization for the tty structure.
  */
-int rs_open(struct tty_struct *tty, struct file * filp)
+int rs_open(struct tty_struct *tty, struct file *filp)
 {
 	struct m68k_serial	*info;
 	int retval;
@@ -1180,7 +1180,7 @@ rs68328_init(void)
 
 	local_irq_save(flags);
 
-	for(i=0;i<NR_PORTS;i++) {
+	for (i = 0; i < NR_PORTS; i++) {
 
 	    info = &m68k_soft[i];
 	    tty_port_init(&info->tport);
@@ -1198,7 +1198,7 @@ rs68328_init(void)
 	    printk(" is a builtin MC68328 UART\n");
 	    
 #ifdef CONFIG_M68VZ328
-		if (i > 0 )
+		if (i > 0)
 			PJSEL &= 0xCF;  /* PSW enable second port output */
 #endif
 
@@ -1263,7 +1263,7 @@ int m68328_console_setup(struct console *cp, char *arg)
 		return(-1);
 
 	if (arg)
-		n = simple_strtoul(arg,NULL,0);
+		n = simple_strtoul(arg, NULL, 0);
 
 	for (i = 0; i < ARRAY_SIZE(baud_table); i++)
 		if (baud_table[i] == n)
@@ -1279,7 +1279,7 @@ int m68328_console_setup(struct console *cp, char *arg)
 	}
 
 	m68328_set_baud(); /* make sure baud rate changes */
-	return(0);
+	return 0;
 }
 
 
@@ -1298,7 +1298,7 @@ void m68328_console_write (struct console *co, const char *str,
     while (count--) {
         if (*str == '\n')
            rs_put_char('\r');
-        rs_put_char( *str++ );
+        rs_put_char(*str++);
     }
 }
 
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 39126460c1f5..c9720a97a977 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -620,7 +620,7 @@ static int univ8250_console_setup(struct console *co, char *options)
  *	@options: ptr to option string from console command line
  *
  *	Only attempts to match console command lines of the form:
- *	    console=uart[8250],io|mmio|mmio32,<addr>[,<options>]
+ *	    console=uart[8250],io|mmio|mmio16|mmio32,<addr>[,<options>]
  *	    console=uart[8250],0x<addr>[,<options>]
  *	This form is used to register an initial earlycon boot console and
  *	replace it with the serial8250_console at 8250 driver init.
@@ -650,8 +650,9 @@ static int univ8250_console_match(struct console *co, char *name, int idx,
 
 		if (port->iotype != iotype)
 			continue;
-		if ((iotype == UPIO_MEM || iotype == UPIO_MEM32) &&
-		    (port->mapbase != addr))
+		if ((iotype == UPIO_MEM || iotype == UPIO_MEM16 ||
+		     iotype == UPIO_MEM32 || iotype == UPIO_MEM32BE)
+		    && (port->mapbase != addr))
 			continue;
 		if (iotype == UPIO_PORT && port->iobase != addr)
 			continue;
diff --git a/drivers/tty/serial/8250/8250_early.c b/drivers/tty/serial/8250/8250_early.c
index ceb85792a5cf..af62131af21e 100644
--- a/drivers/tty/serial/8250/8250_early.c
+++ b/drivers/tty/serial/8250/8250_early.c
@@ -42,6 +42,8 @@ static unsigned int __init serial8250_early_in(struct uart_port *port, int offse
 	switch (port->iotype) {
 	case UPIO_MEM:
 		return readb(port->membase + offset);
+	case UPIO_MEM16:
+		return readw(port->membase + (offset << 1));
 	case UPIO_MEM32:
 		return readl(port->membase + (offset << 2));
 	case UPIO_MEM32BE:
@@ -59,6 +61,9 @@ static void __init serial8250_early_out(struct uart_port *port, int offset, int
 	case UPIO_MEM:
 		writeb(value, port->membase + offset);
 		break;
+	case UPIO_MEM16:
+		writew(value, port->membase + (offset << 1));
+		break;
 	case UPIO_MEM32:
 		writel(value, port->membase + (offset << 2));
 		break;
@@ -73,43 +78,27 @@ static void __init serial8250_early_out(struct uart_port *port, int offset, int
 
 #define BOTH_EMPTY (UART_LSR_TEMT | UART_LSR_THRE)
 
-static void __init wait_for_xmitr(struct uart_port *port)
+static void __init serial_putc(struct uart_port *port, int c)
 {
 	unsigned int status;
 
+	serial8250_early_out(port, UART_TX, c);
+
 	for (;;) {
 		status = serial8250_early_in(port, UART_LSR);
 		if ((status & BOTH_EMPTY) == BOTH_EMPTY)
-			return;
+			break;
 		cpu_relax();
 	}
 }
 
-static void __init serial_putc(struct uart_port *port, int c)
-{
-	wait_for_xmitr(port);
-	serial8250_early_out(port, UART_TX, c);
-}
-
 static void __init early_serial8250_write(struct console *console,
 					const char *s, unsigned int count)
 {
 	struct earlycon_device *device = console->data;
 	struct uart_port *port = &device->port;
-	unsigned int ier;
-
-	/* Save the IER and disable interrupts preserving the UUE bit */
-	ier = serial8250_early_in(port, UART_IER);
-	if (ier)
-		serial8250_early_out(port, UART_IER, ier & UART_IER_UUE);
 
 	uart_console_write(port, s, count, serial_putc);
-
-	/* Wait for transmitter to become empty and restore the IER */
-	wait_for_xmitr(port);
-
-	if (ier)
-		serial8250_early_out(port, UART_IER, ier);
 }
 
 static void __init init_port(struct earlycon_device *device)
diff --git a/drivers/tty/serial/8250/8250_ingenic.c b/drivers/tty/serial/8250/8250_ingenic.c
index 49394b4c5cfd..d6e1ec9b4fde 100644
--- a/drivers/tty/serial/8250/8250_ingenic.c
+++ b/drivers/tty/serial/8250/8250_ingenic.c
@@ -48,6 +48,7 @@ static const struct of_device_id of_match[];
 #define UART_MCR_MDCE	BIT(7)
 #define UART_MCR_FCM	BIT(6)
 
+#ifdef CONFIG_SERIAL_EARLYCON
 static struct earlycon_device *early_device;
 
 static uint8_t __init early_in(struct uart_port *port, int offset)
@@ -140,6 +141,7 @@ OF_EARLYCON_DECLARE(jz4775_uart, "ingenic,jz4775-uart",
 EARLYCON_DECLARE(jz4780_uart, ingenic_early_console_setup);
 OF_EARLYCON_DECLARE(jz4780_uart, "ingenic,jz4780-uart",
 		    ingenic_early_console_setup);
+#endif /* CONFIG_SERIAL_EARLYCON */
 
 static void ingenic_uart_serial_out(struct uart_port *p, int offset, int value)
 {
diff --git a/drivers/tty/serial/8250/8250_mtk.c b/drivers/tty/serial/8250/8250_mtk.c
index 78883ca64ddd..0e590b233f03 100644
--- a/drivers/tty/serial/8250/8250_mtk.c
+++ b/drivers/tty/serial/8250/8250_mtk.c
@@ -16,7 +16,7 @@
  */
 #include <linux/clk.h>
 #include <linux/io.h>
-#include <linux/module.h>
+#include <linux/init.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
@@ -245,23 +245,6 @@ static int mtk8250_probe(struct platform_device *pdev)
 	return 0;
 }
 
-static int mtk8250_remove(struct platform_device *pdev)
-{
-	struct mtk8250_data *data = platform_get_drvdata(pdev);
-
-	pm_runtime_get_sync(&pdev->dev);
-
-	serial8250_unregister_port(data->line);
-
-	pm_runtime_disable(&pdev->dev);
-	pm_runtime_put_noidle(&pdev->dev);
-
-	if (!pm_runtime_status_suspended(&pdev->dev))
-		mtk8250_runtime_suspend(&pdev->dev);
-
-	return 0;
-}
-
 #ifdef CONFIG_PM_SLEEP
 static int mtk8250_suspend(struct device *dev)
 {
@@ -292,18 +275,18 @@ static const struct of_device_id mtk8250_of_match[] = {
 	{ .compatible = "mediatek,mt6577-uart" },
 	{ /* Sentinel */ }
 };
-MODULE_DEVICE_TABLE(of, mtk8250_of_match);
 
 static struct platform_driver mtk8250_platform_driver = {
 	.driver = {
-		.name		= "mt6577-uart",
-		.pm		= &mtk8250_pm_ops,
-		.of_match_table	= mtk8250_of_match,
+		.name			= "mt6577-uart",
+		.pm			= &mtk8250_pm_ops,
+		.of_match_table		= mtk8250_of_match,
+		.suppress_bind_attrs	= true,
+
 	},
 	.probe			= mtk8250_probe,
-	.remove			= mtk8250_remove,
 };
-module_platform_driver(mtk8250_platform_driver);
+builtin_platform_driver(mtk8250_platform_driver);
 
 #ifdef CONFIG_SERIAL_8250_CONSOLE
 static int __init early_mtk8250_setup(struct earlycon_device *device,
@@ -319,7 +302,3 @@ static int __init early_mtk8250_setup(struct earlycon_device *device,
 
 OF_EARLYCON_DECLARE(mtk8250, "mediatek,mt6577-uart", early_mtk8250_setup);
 #endif
-
-MODULE_AUTHOR("Matthias Brugger");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Mediatek 8250 serial port driver");
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/8250/8250_of.c
index de5029649795..33021c1f7d55 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/8250/8250_of.c
@@ -18,14 +18,9 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
-#include <linux/nwpserial.h>
 #include <linux/clk.h>
 
-#ifdef CONFIG_SERIAL_8250_MODULE
-#define CONFIG_SERIAL_8250 CONFIG_SERIAL_8250_MODULE
-#endif
-
-#include "8250/8250.h"
+#include "8250.h"
 
 struct of_serial_info {
 	struct clk *clk;
@@ -122,6 +117,9 @@ static int of_platform_serial_setup(struct platform_device *ofdev,
 		case 1:
 			port->iotype = UPIO_MEM;
 			break;
+		case 2:
+			port->iotype = UPIO_MEM16;
+			break;
 		case 4:
 			port->iotype = of_device_is_big_endian(np) ?
 				       UPIO_MEM32BE : UPIO_MEM32;
@@ -195,7 +193,6 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 		goto out;
 
 	switch (port_type) {
-#ifdef CONFIG_SERIAL_8250
 	case PORT_8250 ... PORT_MAX_8250:
 	{
 		struct uart_8250_port port8250;
@@ -212,12 +209,6 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 		ret = serial8250_register_8250_port(&port8250);
 		break;
 	}
-#endif
-#ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL
-	case PORT_NWPSERIAL:
-		ret = nwpserial_register_port(&port);
-		break;
-#endif
 	default:
 		/* need to add code for these */
 	case PORT_UNKNOWN:
@@ -245,16 +236,9 @@ static int of_platform_serial_remove(struct platform_device *ofdev)
 {
 	struct of_serial_info *info = platform_get_drvdata(ofdev);
 	switch (info->type) {
-#ifdef CONFIG_SERIAL_8250
 	case PORT_8250 ... PORT_MAX_8250:
 		serial8250_unregister_port(info->line);
 		break;
-#endif
-#ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL
-	case PORT_NWPSERIAL:
-		nwpserial_unregister_port(info->line);
-		break;
-#endif
 	default:
 		/* need to add code for these */
 		break;
@@ -267,7 +251,6 @@ static int of_platform_serial_remove(struct platform_device *ofdev)
 }
 
 #ifdef CONFIG_PM_SLEEP
-#ifdef CONFIG_SERIAL_8250
 static void of_serial_suspend_8250(struct of_serial_info *info)
 {
 	struct uart_8250_port *port8250 = serial8250_get_port(info->line);
@@ -288,15 +271,6 @@ static void of_serial_resume_8250(struct of_serial_info *info)
 
 	serial8250_resume_port(info->line);
 }
-#else
-static inline void of_serial_suspend_8250(struct of_serial_info *info)
-{
-}
-
-static inline void of_serial_resume_8250(struct of_serial_info *info)
-{
-}
-#endif
 
 static int of_serial_suspend(struct device *dev)
 {
@@ -353,10 +327,6 @@ static const struct of_device_id of_platform_serial_table[] = {
 		.data = (void *)PORT_XSCALE, },
 	{ .compatible = "mrvl,pxa-uart",
 		.data = (void *)PORT_XSCALE, },
-#ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL
-	{ .compatible = "ibm,qpace-nwp-serial",
-		.data = (void *)PORT_NWPSERIAL, },
-#endif
 	{ /* end of list */ },
 };
 MODULE_DEVICE_TABLE(of, of_platform_serial_table);
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index 4097f3f65b3b..7cd6f9a90542 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -1379,6 +1379,9 @@ ce4100_serial_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_INTEL_BSW_UART1	0x228a
 #define PCI_DEVICE_ID_INTEL_BSW_UART2	0x228c
 
+#define PCI_DEVICE_ID_INTEL_BDW_UART1	0x9ce3
+#define PCI_DEVICE_ID_INTEL_BDW_UART2	0x9ce4
+
 #define BYT_PRV_CLK			0x800
 #define BYT_PRV_CLK_EN			(1 << 0)
 #define BYT_PRV_CLK_M_VAL_SHIFT		1
@@ -1461,11 +1464,13 @@ byt_serial_setup(struct serial_private *priv,
 	switch (pdev->device) {
 	case PCI_DEVICE_ID_INTEL_BYT_UART1:
 	case PCI_DEVICE_ID_INTEL_BSW_UART1:
+	case PCI_DEVICE_ID_INTEL_BDW_UART1:
 		rx_param->src_id = 3;
 		tx_param->dst_id = 2;
 		break;
 	case PCI_DEVICE_ID_INTEL_BYT_UART2:
 	case PCI_DEVICE_ID_INTEL_BSW_UART2:
+	case PCI_DEVICE_ID_INTEL_BDW_UART2:
 		rx_param->src_id = 5;
 		tx_param->dst_id = 4;
 		break;
@@ -1936,6 +1941,7 @@ pci_wch_ch38x_setup(struct serial_private *priv,
 #define PCIE_VENDOR_ID_WCH		0x1c00
 #define PCIE_DEVICE_ID_WCH_CH382_2S1P	0x3250
 #define PCIE_DEVICE_ID_WCH_CH384_4S	0x3470
+#define PCIE_DEVICE_ID_WCH_CH382_2S	0x3253
 
 #define PCI_VENDOR_ID_PERICOM			0x12D8
 #define PCI_DEVICE_ID_PERICOM_PI7C9X7951	0x7951
@@ -2062,6 +2068,20 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= byt_serial_setup,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BDW_UART1,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= byt_serial_setup,
+	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BDW_UART2,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= byt_serial_setup,
+	},
 	/*
 	 * ITE
 	 */
@@ -2618,6 +2638,14 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= pci_wch_ch353_setup,
 	},
+	/* WCH CH382 2S card (16850 clone) */
+	{
+		.vendor         = PCIE_VENDOR_ID_WCH,
+		.device         = PCIE_DEVICE_ID_WCH_CH382_2S,
+		.subvendor      = PCI_ANY_ID,
+		.subdevice      = PCI_ANY_ID,
+		.setup          = pci_wch_ch38x_setup,
+	},
 	/* WCH CH382 2S1P card (16850 clone) */
 	{
 		.vendor         = PCIE_VENDOR_ID_WCH,
@@ -2936,6 +2964,7 @@ enum pci_board_num_t {
 	pbn_fintek_4,
 	pbn_fintek_8,
 	pbn_fintek_12,
+	pbn_wch382_2,
 	pbn_wch384_4,
 	pbn_pericom_PI7C9X7951,
 	pbn_pericom_PI7C9X7952,
@@ -3756,6 +3785,13 @@ static struct pciserial_board pci_boards[] = {
 		.base_baud	= 115200,
 		.first_offset	= 0x40,
 	},
+	[pbn_wch382_2] = {
+		.flags		= FL_BASE0,
+		.num_ports	= 2,
+		.base_baud	= 115200,
+		.uart_offset	= 8,
+		.first_offset	= 0xC0,
+	},
 	[pbn_wch384_4] = {
 		.flags		= FL_BASE0,
 		.num_ports	= 4,
@@ -5506,6 +5542,16 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
 		pbn_byt },
 
+	/* Intel Broadwell */
+	{	PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BDW_UART1,
+		PCI_ANY_ID,  PCI_ANY_ID,
+		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
+		pbn_byt },
+	{	PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BDW_UART2,
+		PCI_ANY_ID,  PCI_ANY_ID,
+		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
+		pbn_byt },
+
 	/*
 	 * Intel Quark x1000
 	 */
@@ -5545,6 +5591,10 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0, pbn_b0_bt_2_115200 },
 
+	{	PCIE_VENDOR_ID_WCH, PCIE_DEVICE_ID_WCH_CH382_2S,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0, pbn_wch382_2 },
+
 	{	PCIE_VENDOR_ID_WCH, PCIE_DEVICE_ID_WCH_CH384_4S,
 		PCI_ANY_ID, PCI_ANY_ID,
 		0, 0, pbn_wch384_4 },
diff --git a/drivers/tty/serial/8250/8250_port.c b/drivers/tty/serial/8250/8250_port.c
index 52d82d2ac726..8d262bce97e4 100644
--- a/drivers/tty/serial/8250/8250_port.c
+++ b/drivers/tty/serial/8250/8250_port.c
@@ -368,6 +368,18 @@ static void mem_serial_out(struct uart_port *p, int offset, int value)
 	writeb(value, p->membase + offset);
 }
 
+static void mem16_serial_out(struct uart_port *p, int offset, int value)
+{
+	offset = offset << p->regshift;
+	writew(value, p->membase + offset);
+}
+
+static unsigned int mem16_serial_in(struct uart_port *p, int offset)
+{
+	offset = offset << p->regshift;
+	return readw(p->membase + offset);
+}
+
 static void mem32_serial_out(struct uart_port *p, int offset, int value)
 {
 	offset = offset << p->regshift;
@@ -425,6 +437,11 @@ static void set_io_from_upio(struct uart_port *p)
 		p->serial_out = mem_serial_out;
 		break;
 
+	case UPIO_MEM16:
+		p->serial_in = mem16_serial_in;
+		p->serial_out = mem16_serial_out;
+		break;
+
 	case UPIO_MEM32:
 		p->serial_in = mem32_serial_in;
 		p->serial_out = mem32_serial_out;
@@ -459,6 +476,7 @@ serial_port_out_sync(struct uart_port *p, int offset, int value)
 {
 	switch (p->iotype) {
 	case UPIO_MEM:
+	case UPIO_MEM16:
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
 	case UPIO_AU:
@@ -2462,6 +2480,7 @@ static int serial8250_request_std_resource(struct uart_8250_port *up)
 	case UPIO_TSI:
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
+	case UPIO_MEM16:
 	case UPIO_MEM:
 		if (!port->mapbase)
 			break;
@@ -2499,6 +2518,7 @@ static void serial8250_release_std_resource(struct uart_8250_port *up)
 	case UPIO_TSI:
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
+	case UPIO_MEM16:
 	case UPIO_MEM:
 		if (!port->mapbase)
 			break;
diff --git a/drivers/tty/serial/8250/8250_uniphier.c b/drivers/tty/serial/8250/8250_uniphier.c
index 245edbb68d4b..bab6b3ae2540 100644
--- a/drivers/tty/serial/8250/8250_uniphier.c
+++ b/drivers/tty/serial/8250/8250_uniphier.c
@@ -13,6 +13,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/console.h>
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -34,6 +35,29 @@ struct uniphier8250_priv {
 	spinlock_t atomic_write_lock;
 };
 
+#ifdef CONFIG_SERIAL_8250_CONSOLE
+static int __init uniphier_early_console_setup(struct earlycon_device *device,
+					       const char *options)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	/* This hardware always expects MMIO32 register interface. */
+	device->port.iotype = UPIO_MEM32;
+	device->port.regshift = 2;
+
+	/*
+	 * Do not touch the divisor register in early_serial8250_setup();
+	 * we assume it has been initialized by a boot loader.
+	 */
+	device->baud = 0;
+
+	return early_serial8250_setup(device, options);
+}
+OF_EARLYCON_DECLARE(uniphier, "socionext,uniphier-uart",
+		    uniphier_early_console_setup);
+#endif
+
 /*
  * The register map is slightly different from that of 8250.
  * IO callbacks must be overridden for correct access to FCR, LCR, and MCR.
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 6412f1455beb..b03cb5175113 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -361,9 +361,8 @@ config SERIAL_8250_UNIPHIER
 
 config SERIAL_8250_INGENIC
 	bool "Support for Ingenic SoC serial ports"
-	depends on SERIAL_8250_CONSOLE && OF_FLATTREE
+	depends on OF_FLATTREE
 	select LIBFDT
-	select SERIAL_EARLYCON
 	help
 	  If you have a system using an Ingenic SoC and wish to make use of
 	  its UARTs, say Y to this option. If unsure, say N.
@@ -372,9 +371,18 @@ config SERIAL_8250_MID
 	tristate "Support for serial ports on Intel MID platforms"
 	depends on SERIAL_8250 && PCI
 	select HSU_DMA if SERIAL_8250_DMA
-	select HSU_DMA_PCI if X86_INTEL_MID
+	select HSU_DMA_PCI if (HSU_DMA && X86_INTEL_MID)
 	select RATIONAL
 	help
 	  Selecting this option will enable handling of the extra features
 	  present on the UART found on Intel Medfield SOC and various other
 	  Intel platforms.
+
+config SERIAL_OF_PLATFORM
+	tristate "Devicetree based probing for 8250 ports"
+	depends on SERIAL_8250 && OF
+	help
+	  This option is used for all 8250 compatible serial ports that
+	  are probed through devicetree, including Open Firmware based
+	  PowerPC systems and embedded systems on architectures using the
+	  flattened device tree format.
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index e177f8681ada..b9b9bca5b6c3 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -28,5 +28,6 @@ obj-$(CONFIG_SERIAL_8250_MT6577)	+= 8250_mtk.o
 obj-$(CONFIG_SERIAL_8250_UNIPHIER)	+= 8250_uniphier.o
 obj-$(CONFIG_SERIAL_8250_INGENIC)	+= 8250_ingenic.o
 obj-$(CONFIG_SERIAL_8250_MID)		+= 8250_mid.o
+obj-$(CONFIG_SERIAL_OF_PLATFORM)	+= 8250_of.o
 
 CFLAGS_8250_ingenic.o += -I$(srctree)/scripts/dtc/libfdt
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index f38beb28e7ae..39721ec4f415 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -115,6 +115,7 @@ config SERIAL_SB1250_DUART_CONSOLE
 
 config SERIAL_ATMEL
 	bool "AT91 / AT32 on-chip serial port support"
+	depends on HAS_DMA
 	depends on ARCH_AT91 || AVR32 || COMPILE_TEST
 	select SERIAL_CORE
 	select SERIAL_MCTRL_GPIO if GPIOLIB
@@ -571,9 +572,11 @@ config BFIN_UART3_CTSRTS
 
 config SERIAL_IMX
 	tristate "IMX serial port support"
+	depends on HAS_DMA
 	depends on ARCH_MXC || COMPILE_TEST
 	select SERIAL_CORE
 	select RATIONAL
+	select SERIAL_MCTRL_GPIO if GPIOLIB
 	help
 	  If you have a machine based on a Motorola IMX CPU you
 	  can enable its onboard serial port by enabling this option.
@@ -1044,7 +1047,7 @@ config SERIAL_SGI_IOC3
 	  say Y or M.  Otherwise, say N.
 
 config SERIAL_MSM
-	bool "MSM on-chip serial port support"
+	tristate "MSM on-chip serial port support"
 	depends on ARCH_QCOM
 	select SERIAL_CORE
 
@@ -1094,16 +1097,6 @@ config SERIAL_NETX_CONSOLE
 	  If you have enabled the serial port on the Hilscher NetX SoC
 	  you can make it the console by answering Y to this option.
 
-config SERIAL_OF_PLATFORM
-	tristate "Serial port on Open Firmware platform bus"
-	depends on OF
-	depends on SERIAL_8250 || SERIAL_OF_PLATFORM_NWPSERIAL
-	help
-	  If you have a PowerPC based system that has serial ports
-	  on a platform specific bus, you should enable this option.
-	  Currently, only 8250 compatible ports are supported, but
-	  others can easily be added.
-
 config SERIAL_OMAP
 	tristate "OMAP serial port support"
 	depends on ARCH_OMAP2PLUS
@@ -1131,23 +1124,6 @@ config SERIAL_OMAP_CONSOLE
 	  your boot loader about how to pass options to the kernel at
 	  boot time.)
 
-config SERIAL_OF_PLATFORM_NWPSERIAL
-	tristate "NWP serial port driver"
-	depends on PPC_DCR
-	select SERIAL_OF_PLATFORM
-	select SERIAL_CORE_CONSOLE
-	select SERIAL_CORE
-	help
-	  This driver supports the cell network processor nwp serial
-	  device.
-
-config SERIAL_OF_PLATFORM_NWPSERIAL_CONSOLE
-	bool "Console on NWP serial port"
-	depends on SERIAL_OF_PLATFORM_NWPSERIAL=y
-	select SERIAL_CORE_CONSOLE
-	help
-	  Support for Console on the NWP serial ports.
-
 config SERIAL_LANTIQ
 	bool "Lantiq serial driver"
 	depends on LANTIQ
@@ -1409,8 +1385,9 @@ config SERIAL_PCH_UART_CONSOLE
 	  warnings and which allows logins in single user mode).
 
 config SERIAL_MXS_AUART
-	depends on ARCH_MXS || COMPILE_TEST
 	tristate "MXS AUART support"
+	depends on HAS_DMA
+	depends on ARCH_MXS || COMPILE_TEST
 	select SERIAL_CORE
 	select SERIAL_MCTRL_GPIO if GPIOLIB
 	help
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 5ab41119b3dc..b391c9b31960 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -63,8 +63,6 @@ obj-$(CONFIG_SERIAL_ATMEL) += atmel_serial.o
 obj-$(CONFIG_SERIAL_UARTLITE) += uartlite.o
 obj-$(CONFIG_SERIAL_MSM) += msm_serial.o
 obj-$(CONFIG_SERIAL_NETX) += netx-serial.o
-obj-$(CONFIG_SERIAL_OF_PLATFORM) += of_serial.o
-obj-$(CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL) += nwpserial.o
 obj-$(CONFIG_SERIAL_KGDB_NMI) += kgdb_nmi.o
 obj-$(CONFIG_SERIAL_KS8695) += serial_ks8695.o
 obj-$(CONFIG_SERIAL_OMAP) += omap-serial.o
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 899a77187bde..c0da0ccbbcf5 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -60,6 +60,8 @@
 #include <linux/io.h>
 #include <linux/acpi.h>
 
+#include "amba-pl011.h"
+
 #define UART_NR			14
 
 #define SERIAL_AMBA_MAJOR	204
@@ -71,11 +73,27 @@
 #define UART_DR_ERROR		(UART011_DR_OE|UART011_DR_BE|UART011_DR_PE|UART011_DR_FE)
 #define UART_DUMMY_DR_RX	(1 << 16)
 
+static u16 pl011_std_offsets[REG_ARRAY_SIZE] = {
+	[REG_DR] = UART01x_DR,
+	[REG_FR] = UART01x_FR,
+	[REG_LCRH_RX] = UART011_LCRH,
+	[REG_LCRH_TX] = UART011_LCRH,
+	[REG_IBRD] = UART011_IBRD,
+	[REG_FBRD] = UART011_FBRD,
+	[REG_CR] = UART011_CR,
+	[REG_IFLS] = UART011_IFLS,
+	[REG_IMSC] = UART011_IMSC,
+	[REG_RIS] = UART011_RIS,
+	[REG_MIS] = UART011_MIS,
+	[REG_ICR] = UART011_ICR,
+	[REG_DMACR] = UART011_DMACR,
+};
+
 /* There is by now at least one vendor with differing details, so handle it */
 struct vendor_data {
+	const u16		*reg_offset;
 	unsigned int		ifls;
-	unsigned int		lcrh_tx;
-	unsigned int		lcrh_rx;
+	bool			access_32b;
 	bool			oversampling;
 	bool			dma_threshold;
 	bool			cts_event_workaround;
@@ -91,9 +109,8 @@ static unsigned int get_fifosize_arm(struct amba_device *dev)
 }
 
 static struct vendor_data vendor_arm = {
+	.reg_offset		= pl011_std_offsets,
 	.ifls			= UART011_IFLS_RX4_8|UART011_IFLS_TX4_8,
-	.lcrh_tx		= UART011_LCRH,
-	.lcrh_rx		= UART011_LCRH,
 	.oversampling		= false,
 	.dma_threshold		= false,
 	.cts_event_workaround	= false,
@@ -103,6 +120,7 @@ static struct vendor_data vendor_arm = {
 };
 
 static struct vendor_data vendor_sbsa = {
+	.reg_offset		= pl011_std_offsets,
 	.oversampling		= false,
 	.dma_threshold		= false,
 	.cts_event_workaround	= false,
@@ -110,15 +128,41 @@ static struct vendor_data vendor_sbsa = {
 	.fixed_options		= true,
 };
 
+static u16 pl011_st_offsets[REG_ARRAY_SIZE] = {
+	[REG_DR] = UART01x_DR,
+	[REG_ST_DMAWM] = ST_UART011_DMAWM,
+	[REG_ST_TIMEOUT] = ST_UART011_TIMEOUT,
+	[REG_FR] = UART01x_FR,
+	[REG_LCRH_RX] = ST_UART011_LCRH_RX,
+	[REG_LCRH_TX] = ST_UART011_LCRH_TX,
+	[REG_IBRD] = UART011_IBRD,
+	[REG_FBRD] = UART011_FBRD,
+	[REG_CR] = UART011_CR,
+	[REG_IFLS] = UART011_IFLS,
+	[REG_IMSC] = UART011_IMSC,
+	[REG_RIS] = UART011_RIS,
+	[REG_MIS] = UART011_MIS,
+	[REG_ICR] = UART011_ICR,
+	[REG_DMACR] = UART011_DMACR,
+	[REG_ST_XFCR] = ST_UART011_XFCR,
+	[REG_ST_XON1] = ST_UART011_XON1,
+	[REG_ST_XON2] = ST_UART011_XON2,
+	[REG_ST_XOFF1] = ST_UART011_XOFF1,
+	[REG_ST_XOFF2] = ST_UART011_XOFF2,
+	[REG_ST_ITCR] = ST_UART011_ITCR,
+	[REG_ST_ITIP] = ST_UART011_ITIP,
+	[REG_ST_ABCR] = ST_UART011_ABCR,
+	[REG_ST_ABIMSC] = ST_UART011_ABIMSC,
+};
+
 static unsigned int get_fifosize_st(struct amba_device *dev)
 {
 	return 64;
 }
 
 static struct vendor_data vendor_st = {
+	.reg_offset		= pl011_st_offsets,
 	.ifls			= UART011_IFLS_RX_HALF|UART011_IFLS_TX_HALF,
-	.lcrh_tx		= ST_UART011_LCRH_TX,
-	.lcrh_rx		= ST_UART011_LCRH_RX,
 	.oversampling		= true,
 	.dma_threshold		= true,
 	.cts_event_workaround	= true,
@@ -127,6 +171,29 @@ static struct vendor_data vendor_st = {
 	.get_fifosize		= get_fifosize_st,
 };
 
+static const u16 pl011_zte_offsets[REG_ARRAY_SIZE] = {
+	[REG_DR] = ZX_UART011_DR,
+	[REG_FR] = ZX_UART011_FR,
+	[REG_LCRH_RX] = ZX_UART011_LCRH,
+	[REG_LCRH_TX] = ZX_UART011_LCRH,
+	[REG_IBRD] = ZX_UART011_IBRD,
+	[REG_FBRD] = ZX_UART011_FBRD,
+	[REG_CR] = ZX_UART011_CR,
+	[REG_IFLS] = ZX_UART011_IFLS,
+	[REG_IMSC] = ZX_UART011_IMSC,
+	[REG_RIS] = ZX_UART011_RIS,
+	[REG_MIS] = ZX_UART011_MIS,
+	[REG_ICR] = ZX_UART011_ICR,
+	[REG_DMACR] = ZX_UART011_DMACR,
+};
+
+static struct vendor_data vendor_zte = {
+	.reg_offset		= pl011_zte_offsets,
+	.access_32b		= true,
+	.ifls			= UART011_IFLS_RX4_8|UART011_IFLS_TX4_8,
+	.get_fifosize		= get_fifosize_arm,
+};
+
 /* Deals with DMA transactions */
 
 struct pl011_sgbuf {
@@ -162,14 +229,13 @@ struct pl011_dmatx_data {
  */
 struct uart_amba_port {
 	struct uart_port	port;
+	const u16		*reg_offset;
 	struct clk		*clk;
 	const struct vendor_data *vendor;
 	unsigned int		dmacr;		/* dma control reg */
 	unsigned int		im;		/* interrupt mask */
 	unsigned int		old_status;
 	unsigned int		fifosize;	/* vendor-specific */
-	unsigned int		lcrh_tx;	/* vendor-specific */
-	unsigned int		lcrh_rx;	/* vendor-specific */
 	unsigned int		old_cr;		/* state during shutdown */
 	bool			autorts;
 	unsigned int		fixed_baud;	/* vendor-set fixed baud rate */
@@ -184,6 +250,32 @@ struct uart_amba_port {
 #endif
 };
 
+static unsigned int pl011_reg_to_offset(const struct uart_amba_port *uap,
+	unsigned int reg)
+{
+	return uap->reg_offset[reg];
+}
+
+static unsigned int pl011_read(const struct uart_amba_port *uap,
+	unsigned int reg)
+{
+	void __iomem *addr = uap->port.membase + pl011_reg_to_offset(uap, reg);
+
+	return (uap->port.iotype == UPIO_MEM32) ?
+		readl_relaxed(addr) : readw_relaxed(addr);
+}
+
+static void pl011_write(unsigned int val, const struct uart_amba_port *uap,
+	unsigned int reg)
+{
+	void __iomem *addr = uap->port.membase + pl011_reg_to_offset(uap, reg);
+
+	if (uap->port.iotype == UPIO_MEM32)
+		writel_relaxed(val, addr);
+	else
+		writew_relaxed(val, addr);
+}
+
 /*
  * Reads up to 256 characters from the FIFO or until it's empty and
  * inserts them into the TTY layer. Returns the number of characters
@@ -196,13 +288,12 @@ static int pl011_fifo_to_tty(struct uart_amba_port *uap)
 	int fifotaken = 0;
 
 	while (max_count--) {
-		status = readw(uap->port.membase + UART01x_FR);
+		status = pl011_read(uap, REG_FR);
 		if (status & UART01x_FR_RXFE)
 			break;
 
 		/* Take chars from the FIFO and update status */
-		ch = readw(uap->port.membase + UART01x_DR) |
-			UART_DUMMY_DR_RX;
+		ch = pl011_read(uap, REG_DR) | UART_DUMMY_DR_RX;
 		flag = TTY_NORMAL;
 		uap->port.icount.rx++;
 		fifotaken++;
@@ -284,7 +375,8 @@ static void pl011_dma_probe(struct uart_amba_port *uap)
 	struct amba_pl011_data *plat = dev_get_platdata(uap->port.dev);
 	struct device *dev = uap->port.dev;
 	struct dma_slave_config tx_conf = {
-		.dst_addr = uap->port.mapbase + UART01x_DR,
+		.dst_addr = uap->port.mapbase +
+				 pl011_reg_to_offset(uap, REG_DR),
 		.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE,
 		.direction = DMA_MEM_TO_DEV,
 		.dst_maxburst = uap->fifosize >> 1,
@@ -339,7 +431,8 @@ static void pl011_dma_probe(struct uart_amba_port *uap)
 
 	if (chan) {
 		struct dma_slave_config rx_conf = {
-			.src_addr = uap->port.mapbase + UART01x_DR,
+			.src_addr = uap->port.mapbase +
+				pl011_reg_to_offset(uap, REG_DR),
 			.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE,
 			.direction = DMA_DEV_TO_MEM,
 			.src_maxburst = uap->fifosize >> 2,
@@ -438,7 +531,7 @@ static void pl011_dma_tx_callback(void *data)
 
 	dmacr = uap->dmacr;
 	uap->dmacr = dmacr & ~UART011_TXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 
 	/*
 	 * If TX DMA was disabled, it means that we've stopped the DMA for
@@ -552,7 +645,7 @@ static int pl011_dma_tx_refill(struct uart_amba_port *uap)
 	dma_dev->device_issue_pending(chan);
 
 	uap->dmacr |= UART011_TXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 	uap->dmatx.queued = true;
 
 	/*
@@ -588,9 +681,9 @@ static bool pl011_dma_tx_irq(struct uart_amba_port *uap)
 	 */
 	if (uap->dmatx.queued) {
 		uap->dmacr |= UART011_TXDMAE;
-		writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+		pl011_write(uap->dmacr, uap, REG_DMACR);
 		uap->im &= ~UART011_TXIM;
-		writew(uap->im, uap->port.membase + UART011_IMSC);
+		pl011_write(uap->im, uap, REG_IMSC);
 		return true;
 	}
 
@@ -600,7 +693,7 @@ static bool pl011_dma_tx_irq(struct uart_amba_port *uap)
 	 */
 	if (pl011_dma_tx_refill(uap) > 0) {
 		uap->im &= ~UART011_TXIM;
-		writew(uap->im, uap->port.membase + UART011_IMSC);
+		pl011_write(uap->im, uap, REG_IMSC);
 		return true;
 	}
 	return false;
@@ -614,7 +707,7 @@ static inline void pl011_dma_tx_stop(struct uart_amba_port *uap)
 {
 	if (uap->dmatx.queued) {
 		uap->dmacr &= ~UART011_TXDMAE;
-		writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+		pl011_write(uap->dmacr, uap, REG_DMACR);
 	}
 }
 
@@ -640,14 +733,12 @@ static inline bool pl011_dma_tx_start(struct uart_amba_port *uap)
 		if (!uap->dmatx.queued) {
 			if (pl011_dma_tx_refill(uap) > 0) {
 				uap->im &= ~UART011_TXIM;
-				writew(uap->im, uap->port.membase +
-				       UART011_IMSC);
+				pl011_write(uap->im, uap, REG_IMSC);
 			} else
 				ret = false;
 		} else if (!(uap->dmacr & UART011_TXDMAE)) {
 			uap->dmacr |= UART011_TXDMAE;
-			writew(uap->dmacr,
-				       uap->port.membase + UART011_DMACR);
+			pl011_write(uap->dmacr, uap, REG_DMACR);
 		}
 		return ret;
 	}
@@ -658,9 +749,9 @@ static inline bool pl011_dma_tx_start(struct uart_amba_port *uap)
 	 */
 	dmacr = uap->dmacr;
 	uap->dmacr &= ~UART011_TXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 
-	if (readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF) {
+	if (pl011_read(uap, REG_FR) & UART01x_FR_TXFF) {
 		/*
 		 * No space in the FIFO, so enable the transmit interrupt
 		 * so we know when there is space.  Note that once we've
@@ -669,13 +760,13 @@ static inline bool pl011_dma_tx_start(struct uart_amba_port *uap)
 		return false;
 	}
 
-	writew(uap->port.x_char, uap->port.membase + UART01x_DR);
+	pl011_write(uap->port.x_char, uap, REG_DR);
 	uap->port.icount.tx++;
 	uap->port.x_char = 0;
 
 	/* Success - restore the DMA state */
 	uap->dmacr = dmacr;
-	writew(dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(dmacr, uap, REG_DMACR);
 
 	return true;
 }
@@ -703,7 +794,7 @@ __acquires(&uap->port.lock)
 			     DMA_TO_DEVICE);
 		uap->dmatx.queued = false;
 		uap->dmacr &= ~UART011_TXDMAE;
-		writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+		pl011_write(uap->dmacr, uap, REG_DMACR);
 	}
 }
 
@@ -743,11 +834,11 @@ static int pl011_dma_rx_trigger_dma(struct uart_amba_port *uap)
 	dma_async_issue_pending(rxchan);
 
 	uap->dmacr |= UART011_RXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 	uap->dmarx.running = true;
 
 	uap->im &= ~UART011_RXIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 
 	return 0;
 }
@@ -805,8 +896,8 @@ static void pl011_dma_rx_chars(struct uart_amba_port *uap,
 	 */
 	if (dma_count == pending && readfifo) {
 		/* Clear any error flags */
-		writew(UART011_OEIS | UART011_BEIS | UART011_PEIS | UART011_FEIS,
-		       uap->port.membase + UART011_ICR);
+		pl011_write(UART011_OEIS | UART011_BEIS | UART011_PEIS |
+			    UART011_FEIS, uap, REG_ICR);
 
 		/*
 		 * If we read all the DMA'd characters, and we had an
@@ -854,7 +945,7 @@ static void pl011_dma_rx_irq(struct uart_amba_port *uap)
 
 	/* Disable RX DMA - incoming data will wait in the FIFO */
 	uap->dmacr &= ~UART011_RXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 	uap->dmarx.running = false;
 
 	pending = sgbuf->sg.length - state.residue;
@@ -874,7 +965,7 @@ static void pl011_dma_rx_irq(struct uart_amba_port *uap)
 		dev_dbg(uap->port.dev, "could not retrigger RX DMA job "
 			"fall back to interrupt mode\n");
 		uap->im |= UART011_RXIM;
-		writew(uap->im, uap->port.membase + UART011_IMSC);
+		pl011_write(uap->im, uap, REG_IMSC);
 	}
 }
 
@@ -922,7 +1013,7 @@ static void pl011_dma_rx_callback(void *data)
 		dev_dbg(uap->port.dev, "could not retrigger RX DMA job "
 			"fall back to interrupt mode\n");
 		uap->im |= UART011_RXIM;
-		writew(uap->im, uap->port.membase + UART011_IMSC);
+		pl011_write(uap->im, uap, REG_IMSC);
 	}
 }
 
@@ -935,7 +1026,7 @@ static inline void pl011_dma_rx_stop(struct uart_amba_port *uap)
 {
 	/* FIXME.  Just disable the DMA enable */
 	uap->dmacr &= ~UART011_RXDMAE;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 }
 
 /*
@@ -979,7 +1070,7 @@ static void pl011_dma_rx_poll(unsigned long args)
 		spin_lock_irqsave(&uap->port.lock, flags);
 		pl011_dma_rx_stop(uap);
 		uap->im |= UART011_RXIM;
-		writew(uap->im, uap->port.membase + UART011_IMSC);
+		pl011_write(uap->im, uap, REG_IMSC);
 		spin_unlock_irqrestore(&uap->port.lock, flags);
 
 		uap->dmarx.running = false;
@@ -1041,7 +1132,7 @@ static void pl011_dma_startup(struct uart_amba_port *uap)
 skip_rx:
 	/* Turn on DMA error (RX/TX will be enabled on demand) */
 	uap->dmacr |= UART011_DMAONERR;
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 
 	/*
 	 * ST Micro variants has some specific dma burst threshold
@@ -1049,8 +1140,8 @@ skip_rx:
 	 * be issued above/below 16 bytes.
 	 */
 	if (uap->vendor->dma_threshold)
-		writew(ST_UART011_DMAWM_RX_16 | ST_UART011_DMAWM_TX_16,
-			       uap->port.membase + ST_UART011_DMAWM);
+		pl011_write(ST_UART011_DMAWM_RX_16 | ST_UART011_DMAWM_TX_16,
+			    uap, REG_ST_DMAWM);
 
 	if (uap->using_rx_dma) {
 		if (pl011_dma_rx_trigger_dma(uap))
@@ -1075,12 +1166,12 @@ static void pl011_dma_shutdown(struct uart_amba_port *uap)
 		return;
 
 	/* Disable RX and TX DMA */
-	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_BUSY)
+	while (pl011_read(uap, REG_FR) & UART01x_FR_BUSY)
 		barrier();
 
 	spin_lock_irq(&uap->port.lock);
 	uap->dmacr &= ~(UART011_DMAONERR | UART011_RXDMAE | UART011_TXDMAE);
-	writew(uap->dmacr, uap->port.membase + UART011_DMACR);
+	pl011_write(uap->dmacr, uap, REG_DMACR);
 	spin_unlock_irq(&uap->port.lock);
 
 	if (uap->using_tx_dma) {
@@ -1181,7 +1272,7 @@ static void pl011_stop_tx(struct uart_port *port)
 	    container_of(port, struct uart_amba_port, port);
 
 	uap->im &= ~UART011_TXIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 	pl011_dma_tx_stop(uap);
 }
 
@@ -1191,7 +1282,7 @@ static void pl011_tx_chars(struct uart_amba_port *uap, bool from_irq);
 static void pl011_start_tx_pio(struct uart_amba_port *uap)
 {
 	uap->im |= UART011_TXIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 	pl011_tx_chars(uap, false);
 }
 
@@ -1211,7 +1302,7 @@ static void pl011_stop_rx(struct uart_port *port)
 
 	uap->im &= ~(UART011_RXIM|UART011_RTIM|UART011_FEIM|
 		     UART011_PEIM|UART011_BEIM|UART011_OEIM);
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 
 	pl011_dma_rx_stop(uap);
 }
@@ -1222,7 +1313,7 @@ static void pl011_enable_ms(struct uart_port *port)
 	    container_of(port, struct uart_amba_port, port);
 
 	uap->im |= UART011_RIMIM|UART011_CTSMIM|UART011_DCDMIM|UART011_DSRMIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 }
 
 static void pl011_rx_chars(struct uart_amba_port *uap)
@@ -1242,7 +1333,7 @@ __acquires(&uap->port.lock)
 			dev_dbg(uap->port.dev, "could not trigger RX DMA job "
 				"fall back to interrupt mode again\n");
 			uap->im |= UART011_RXIM;
-			writew(uap->im, uap->port.membase + UART011_IMSC);
+			pl011_write(uap->im, uap, REG_IMSC);
 		} else {
 #ifdef CONFIG_DMA_ENGINE
 			/* Start Rx DMA poll */
@@ -1263,10 +1354,10 @@ static bool pl011_tx_char(struct uart_amba_port *uap, unsigned char c,
 			  bool from_irq)
 {
 	if (unlikely(!from_irq) &&
-	    readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
+	    pl011_read(uap, REG_FR) & UART01x_FR_TXFF)
 		return false; /* unable to transmit character */
 
-	writew(c, uap->port.membase + UART01x_DR);
+	pl011_write(c, uap, REG_DR);
 	uap->port.icount.tx++;
 
 	return true;
@@ -1313,7 +1404,7 @@ static void pl011_modem_status(struct uart_amba_port *uap)
 {
 	unsigned int status, delta;
 
-	status = readw(uap->port.membase + UART01x_FR) & UART01x_FR_MODEM_ANY;
+	status = pl011_read(uap, REG_FR) & UART01x_FR_MODEM_ANY;
 
 	delta = status ^ uap->old_status;
 	uap->old_status = status;
@@ -1341,15 +1432,15 @@ static void check_apply_cts_event_workaround(struct uart_amba_port *uap)
 		return;
 
 	/* workaround to make sure that all bits are unlocked.. */
-	writew(0x00, uap->port.membase + UART011_ICR);
+	pl011_write(0x00, uap, REG_ICR);
 
 	/*
 	 * WA: introduce 26ns(1 uart clk) delay before W1C;
 	 * single apb access will incur 2 pclk(133.12Mhz) delay,
 	 * so add 2 dummy reads
 	 */
-	dummy_read = readw(uap->port.membase + UART011_ICR);
-	dummy_read = readw(uap->port.membase + UART011_ICR);
+	dummy_read = pl011_read(uap, REG_ICR);
+	dummy_read = pl011_read(uap, REG_ICR);
 }
 
 static irqreturn_t pl011_int(int irq, void *dev_id)
@@ -1361,15 +1452,15 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 	int handled = 0;
 
 	spin_lock_irqsave(&uap->port.lock, flags);
-	imsc = readw(uap->port.membase + UART011_IMSC);
-	status = readw(uap->port.membase + UART011_RIS) & imsc;
+	imsc = pl011_read(uap, REG_IMSC);
+	status = pl011_read(uap, REG_RIS) & imsc;
 	if (status) {
 		do {
 			check_apply_cts_event_workaround(uap);
 
-			writew(status & ~(UART011_TXIS|UART011_RTIS|
-					  UART011_RXIS),
-			       uap->port.membase + UART011_ICR);
+			pl011_write(status & ~(UART011_TXIS|UART011_RTIS|
+					       UART011_RXIS),
+				    uap, REG_ICR);
 
 			if (status & (UART011_RTIS|UART011_RXIS)) {
 				if (pl011_dma_rx_running(uap))
@@ -1386,7 +1477,7 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 			if (pass_counter-- == 0)
 				break;
 
-			status = readw(uap->port.membase + UART011_RIS) & imsc;
+			status = pl011_read(uap, REG_RIS) & imsc;
 		} while (status != 0);
 		handled = 1;
 	}
@@ -1400,7 +1491,7 @@ static unsigned int pl011_tx_empty(struct uart_port *port)
 {
 	struct uart_amba_port *uap =
 	    container_of(port, struct uart_amba_port, port);
-	unsigned int status = readw(uap->port.membase + UART01x_FR);
+	unsigned int status = pl011_read(uap, REG_FR);
 	return status & (UART01x_FR_BUSY|UART01x_FR_TXFF) ? 0 : TIOCSER_TEMT;
 }
 
@@ -1409,7 +1500,7 @@ static unsigned int pl011_get_mctrl(struct uart_port *port)
 	struct uart_amba_port *uap =
 	    container_of(port, struct uart_amba_port, port);
 	unsigned int result = 0;
-	unsigned int status = readw(uap->port.membase + UART01x_FR);
+	unsigned int status = pl011_read(uap, REG_FR);
 
 #define TIOCMBIT(uartbit, tiocmbit)	\
 	if (status & uartbit)		\
@@ -1429,7 +1520,7 @@ static void pl011_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	    container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
-	cr = readw(uap->port.membase + UART011_CR);
+	cr = pl011_read(uap, REG_CR);
 
 #define	TIOCMBIT(tiocmbit, uartbit)		\
 	if (mctrl & tiocmbit)		\
@@ -1449,7 +1540,7 @@ static void pl011_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	}
 #undef TIOCMBIT
 
-	writew(cr, uap->port.membase + UART011_CR);
+	pl011_write(cr, uap, REG_CR);
 }
 
 static void pl011_break_ctl(struct uart_port *port, int break_state)
@@ -1460,12 +1551,12 @@ static void pl011_break_ctl(struct uart_port *port, int break_state)
 	unsigned int lcr_h;
 
 	spin_lock_irqsave(&uap->port.lock, flags);
-	lcr_h = readw(uap->port.membase + uap->lcrh_tx);
+	lcr_h = pl011_read(uap, REG_LCRH_TX);
 	if (break_state == -1)
 		lcr_h |= UART01x_LCRH_BRK;
 	else
 		lcr_h &= ~UART01x_LCRH_BRK;
-	writew(lcr_h, uap->port.membase + uap->lcrh_tx);
+	pl011_write(lcr_h, uap, REG_LCRH_TX);
 	spin_unlock_irqrestore(&uap->port.lock, flags);
 }
 
@@ -1475,9 +1566,8 @@ static void pl011_quiesce_irqs(struct uart_port *port)
 {
 	struct uart_amba_port *uap =
 	    container_of(port, struct uart_amba_port, port);
-	unsigned char __iomem *regs = uap->port.membase;
 
-	writew(readw(regs + UART011_MIS), regs + UART011_ICR);
+	pl011_write(pl011_read(uap, REG_MIS), uap, REG_ICR);
 	/*
 	 * There is no way to clear TXIM as this is "ready to transmit IRQ", so
 	 * we simply mask it. start_tx() will unmask it.
@@ -1491,7 +1581,8 @@ static void pl011_quiesce_irqs(struct uart_port *port)
 	 * (including tx queue), so we're also fine with start_tx()'s caller
 	 * side.
 	 */
-	writew(readw(regs + UART011_IMSC) & ~UART011_TXIM, regs + UART011_IMSC);
+	pl011_write(pl011_read(uap, REG_IMSC) & ~UART011_TXIM, uap,
+		    REG_IMSC);
 }
 
 static int pl011_get_poll_char(struct uart_port *port)
@@ -1506,11 +1597,11 @@ static int pl011_get_poll_char(struct uart_port *port)
 	 */
 	pl011_quiesce_irqs(port);
 
-	status = readw(uap->port.membase + UART01x_FR);
+	status = pl011_read(uap, REG_FR);
 	if (status & UART01x_FR_RXFE)
 		return NO_POLL_CHAR;
 
-	return readw(uap->port.membase + UART01x_DR);
+	return pl011_read(uap, REG_DR);
 }
 
 static void pl011_put_poll_char(struct uart_port *port,
@@ -1519,10 +1610,10 @@ static void pl011_put_poll_char(struct uart_port *port,
 	struct uart_amba_port *uap =
 	    container_of(port, struct uart_amba_port, port);
 
-	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
+	while (pl011_read(uap, REG_FR) & UART01x_FR_TXFF)
 		barrier();
 
-	writew(ch, uap->port.membase + UART01x_DR);
+	pl011_write(ch, uap, REG_DR);
 }
 
 #endif /* CONFIG_CONSOLE_POLL */
@@ -1546,15 +1637,16 @@ static int pl011_hwinit(struct uart_port *port)
 	uap->port.uartclk = clk_get_rate(uap->clk);
 
 	/* Clear pending error and receive interrupts */
-	writew(UART011_OEIS | UART011_BEIS | UART011_PEIS | UART011_FEIS |
-	       UART011_RTIS | UART011_RXIS, uap->port.membase + UART011_ICR);
+	pl011_write(UART011_OEIS | UART011_BEIS | UART011_PEIS |
+		    UART011_FEIS | UART011_RTIS | UART011_RXIS,
+		    uap, REG_ICR);
 
 	/*
 	 * Save interrupts enable mask, and enable RX interrupts in case if
 	 * the interrupt is used for NMI entry.
 	 */
-	uap->im = readw(uap->port.membase + UART011_IMSC);
-	writew(UART011_RTIM | UART011_RXIM, uap->port.membase + UART011_IMSC);
+	uap->im = pl011_read(uap, REG_IMSC);
+	pl011_write(UART011_RTIM | UART011_RXIM, uap, REG_IMSC);
 
 	if (dev_get_platdata(uap->port.dev)) {
 		struct amba_pl011_data *plat;
@@ -1566,24 +1658,30 @@ static int pl011_hwinit(struct uart_port *port)
 	return 0;
 }
 
+static bool pl011_split_lcrh(const struct uart_amba_port *uap)
+{
+	return pl011_reg_to_offset(uap, REG_LCRH_RX) !=
+	       pl011_reg_to_offset(uap, REG_LCRH_TX);
+}
+
 static void pl011_write_lcr_h(struct uart_amba_port *uap, unsigned int lcr_h)
 {
-	writew(lcr_h, uap->port.membase + uap->lcrh_rx);
-	if (uap->lcrh_rx != uap->lcrh_tx) {
+	pl011_write(lcr_h, uap, REG_LCRH_RX);
+	if (pl011_split_lcrh(uap)) {
 		int i;
 		/*
 		 * Wait 10 PCLKs before writing LCRH_TX register,
 		 * to get this delay write read only register 10 times
 		 */
 		for (i = 0; i < 10; ++i)
-			writew(0xff, uap->port.membase + UART011_MIS);
-		writew(lcr_h, uap->port.membase + uap->lcrh_tx);
+			pl011_write(0xff, uap, REG_MIS);
+		pl011_write(lcr_h, uap, REG_LCRH_TX);
 	}
 }
 
 static int pl011_allocate_irq(struct uart_amba_port *uap)
 {
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 
 	return request_irq(uap->port.irq, pl011_int, 0, "uart-pl011", uap);
 }
@@ -1598,12 +1696,11 @@ static void pl011_enable_interrupts(struct uart_amba_port *uap)
 	spin_lock_irq(&uap->port.lock);
 
 	/* Clear out any spuriously appearing RX interrupts */
-	writew(UART011_RTIS | UART011_RXIS,
-	       uap->port.membase + UART011_ICR);
+	pl011_write(UART011_RTIS | UART011_RXIS, uap, REG_ICR);
 	uap->im = UART011_RTIM;
 	if (!pl011_dma_rx_running(uap))
 		uap->im |= UART011_RXIM;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
+	pl011_write(uap->im, uap, REG_IMSC);
 	spin_unlock_irq(&uap->port.lock);
 }
 
@@ -1622,21 +1719,21 @@ static int pl011_startup(struct uart_port *port)
 	if (retval)
 		goto clk_dis;
 
-	writew(uap->vendor->ifls, uap->port.membase + UART011_IFLS);
+	pl011_write(uap->vendor->ifls, uap, REG_IFLS);
 
 	spin_lock_irq(&uap->port.lock);
 
 	/* restore RTS and DTR */
 	cr = uap->old_cr & (UART011_CR_RTS | UART011_CR_DTR);
 	cr |= UART01x_CR_UARTEN | UART011_CR_RXE | UART011_CR_TXE;
-	writew(cr, uap->port.membase + UART011_CR);
+	pl011_write(cr, uap, REG_CR);
 
 	spin_unlock_irq(&uap->port.lock);
 
 	/*
 	 * initialise the old status of the modem signals
 	 */
-	uap->old_status = readw(uap->port.membase + UART01x_FR) & UART01x_FR_MODEM_ANY;
+	uap->old_status = pl011_read(uap, REG_FR) & UART01x_FR_MODEM_ANY;
 
 	/* Startup DMA */
 	pl011_dma_startup(uap);
@@ -1677,9 +1774,9 @@ static void pl011_shutdown_channel(struct uart_amba_port *uap,
 {
       unsigned long val;
 
-      val = readw(uap->port.membase + lcrh);
+      val = pl011_read(uap, lcrh);
       val &= ~(UART01x_LCRH_BRK | UART01x_LCRH_FEN);
-      writew(val, uap->port.membase + lcrh);
+      pl011_write(val, uap, lcrh);
 }
 
 /*
@@ -1693,19 +1790,19 @@ static void pl011_disable_uart(struct uart_amba_port *uap)
 
 	uap->autorts = false;
 	spin_lock_irq(&uap->port.lock);
-	cr = readw(uap->port.membase + UART011_CR);
+	cr = pl011_read(uap, REG_CR);
 	uap->old_cr = cr;
 	cr &= UART011_CR_RTS | UART011_CR_DTR;
 	cr |= UART01x_CR_UARTEN | UART011_CR_TXE;
-	writew(cr, uap->port.membase + UART011_CR);
+	pl011_write(cr, uap, REG_CR);
 	spin_unlock_irq(&uap->port.lock);
 
 	/*
 	 * disable break condition and fifos
 	 */
-	pl011_shutdown_channel(uap, uap->lcrh_rx);
-	if (uap->lcrh_rx != uap->lcrh_tx)
-		pl011_shutdown_channel(uap, uap->lcrh_tx);
+	pl011_shutdown_channel(uap, REG_LCRH_RX);
+	if (pl011_split_lcrh(uap))
+		pl011_shutdown_channel(uap, REG_LCRH_TX);
 }
 
 static void pl011_disable_interrupts(struct uart_amba_port *uap)
@@ -1714,8 +1811,8 @@ static void pl011_disable_interrupts(struct uart_amba_port *uap)
 
 	/* mask all interrupts and clear all pending ones */
 	uap->im = 0;
-	writew(uap->im, uap->port.membase + UART011_IMSC);
-	writew(0xffff, uap->port.membase + UART011_ICR);
+	pl011_write(uap->im, uap, REG_IMSC);
+	pl011_write(0xffff, uap, REG_ICR);
 
 	spin_unlock_irq(&uap->port.lock);
 }
@@ -1867,8 +1964,8 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 		pl011_enable_ms(port);
 
 	/* first, disable everything */
-	old_cr = readw(port->membase + UART011_CR);
-	writew(0, port->membase + UART011_CR);
+	old_cr = pl011_read(uap, REG_CR);
+	pl011_write(0, uap, REG_CR);
 
 	if (termios->c_cflag & CRTSCTS) {
 		if (old_cr & UART011_CR_RTS)
@@ -1901,17 +1998,17 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 			quot -= 2;
 	}
 	/* Set baud rate */
-	writew(quot & 0x3f, port->membase + UART011_FBRD);
-	writew(quot >> 6, port->membase + UART011_IBRD);
+	pl011_write(quot & 0x3f, uap, REG_FBRD);
+	pl011_write(quot >> 6, uap, REG_IBRD);
 
 	/*
 	 * ----------v----------v----------v----------v-----
-	 * NOTE: lcrh_tx and lcrh_rx MUST BE WRITTEN AFTER
-	 * UART011_FBRD & UART011_IBRD.
+	 * NOTE: REG_LCRH_TX and REG_LCRH_RX MUST BE WRITTEN AFTER
+	 * REG_FBRD & REG_IBRD.
 	 * ----------^----------^----------^----------^-----
 	 */
 	pl011_write_lcr_h(uap, lcr_h);
-	writew(old_cr, port->membase + UART011_CR);
+	pl011_write(old_cr, uap, REG_CR);
 
 	spin_unlock_irqrestore(&port->lock, flags);
 }
@@ -2052,9 +2149,9 @@ static void pl011_console_putchar(struct uart_port *port, int ch)
 	struct uart_amba_port *uap =
 	    container_of(port, struct uart_amba_port, port);
 
-	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
+	while (pl011_read(uap, REG_FR) & UART01x_FR_TXFF)
 		barrier();
-	writew(ch, uap->port.membase + UART01x_DR);
+	pl011_write(ch, uap, REG_DR);
 }
 
 static void
@@ -2079,10 +2176,10 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 	 *	First save the CR then disable the interrupts
 	 */
 	if (!uap->vendor->always_enabled) {
-		old_cr = readw(uap->port.membase + UART011_CR);
+		old_cr = pl011_read(uap, REG_CR);
 		new_cr = old_cr & ~UART011_CR_CTSEN;
 		new_cr |= UART01x_CR_UARTEN | UART011_CR_TXE;
-		writew(new_cr, uap->port.membase + UART011_CR);
+		pl011_write(new_cr, uap, REG_CR);
 	}
 
 	uart_console_write(&uap->port, s, count, pl011_console_putchar);
@@ -2092,10 +2189,10 @@ pl011_console_write(struct console *co, const char *s, unsigned int count)
 	 *	and restore the TCR
 	 */
 	do {
-		status = readw(uap->port.membase + UART01x_FR);
+		status = pl011_read(uap, REG_FR);
 	} while (status & UART01x_FR_BUSY);
 	if (!uap->vendor->always_enabled)
-		writew(old_cr, uap->port.membase + UART011_CR);
+		pl011_write(old_cr, uap, REG_CR);
 
 	if (locked)
 		spin_unlock(&uap->port.lock);
@@ -2108,10 +2205,10 @@ static void __init
 pl011_console_get_options(struct uart_amba_port *uap, int *baud,
 			     int *parity, int *bits)
 {
-	if (readw(uap->port.membase + UART011_CR) & UART01x_CR_UARTEN) {
+	if (pl011_read(uap, REG_CR) & UART01x_CR_UARTEN) {
 		unsigned int lcr_h, ibrd, fbrd;
 
-		lcr_h = readw(uap->port.membase + uap->lcrh_tx);
+		lcr_h = pl011_read(uap, REG_LCRH_TX);
 
 		*parity = 'n';
 		if (lcr_h & UART01x_LCRH_PEN) {
@@ -2126,13 +2223,13 @@ pl011_console_get_options(struct uart_amba_port *uap, int *baud,
 		else
 			*bits = 8;
 
-		ibrd = readw(uap->port.membase + UART011_IBRD);
-		fbrd = readw(uap->port.membase + UART011_FBRD);
+		ibrd = pl011_read(uap, REG_IBRD);
+		fbrd = pl011_read(uap, REG_FBRD);
 
 		*baud = uap->port.uartclk * 4 / (64 * ibrd + fbrd);
 
 		if (uap->vendor->oversampling) {
-			if (readw(uap->port.membase + UART011_CR)
+			if (pl011_read(uap, REG_CR)
 				  & ST_UART011_CR_OVSFACT)
 				*baud *= 2;
 		}
@@ -2206,7 +2303,10 @@ static void pl011_putc(struct uart_port *port, int c)
 {
 	while (readl(port->membase + UART01x_FR) & UART01x_FR_TXFF)
 		;
-	writeb(c, port->membase + UART01x_DR);
+	if (port->iotype == UPIO_MEM32)
+		writel(c, port->membase + UART01x_DR);
+	else
+		writeb(c, port->membase + UART01x_DR);
 	while (readl(port->membase + UART01x_FR) & UART01x_FR_BUSY)
 		;
 }
@@ -2319,7 +2419,6 @@ static int pl011_setup_port(struct device *dev, struct uart_amba_port *uap,
 	uap->port.dev = dev;
 	uap->port.mapbase = mmiobase->start;
 	uap->port.membase = base;
-	uap->port.iotype = UPIO_MEM;
 	uap->port.fifosize = uap->fifosize;
 	uap->port.flags = UPF_BOOT_AUTOCONF;
 	uap->port.line = index;
@@ -2334,8 +2433,8 @@ static int pl011_register_port(struct uart_amba_port *uap)
 	int ret;
 
 	/* Ensure interrupts from this UART are masked and cleared */
-	writew(0, uap->port.membase + UART011_IMSC);
-	writew(0xffff, uap->port.membase + UART011_ICR);
+	pl011_write(0, uap, REG_IMSC);
+	pl011_write(0xffff, uap, REG_ICR);
 
 	if (!amba_reg.state) {
 		ret = uart_register_driver(&amba_reg);
@@ -2372,10 +2471,10 @@ static int pl011_probe(struct amba_device *dev, const struct amba_id *id)
 	if (IS_ERR(uap->clk))
 		return PTR_ERR(uap->clk);
 
+	uap->reg_offset = vendor->reg_offset;
 	uap->vendor = vendor;
-	uap->lcrh_rx = vendor->lcrh_rx;
-	uap->lcrh_tx = vendor->lcrh_tx;
 	uap->fifosize = vendor->get_fifosize(dev);
+	uap->port.iotype = vendor->access_32b ? UPIO_MEM32 : UPIO_MEM;
 	uap->port.irq = dev->irq[0];
 	uap->port.ops = &amba_pl011_pops;
 
@@ -2453,8 +2552,10 @@ static int sbsa_uart_probe(struct platform_device *pdev)
 	if (!uap)
 		return -ENOMEM;
 
+	uap->reg_offset	= vendor_sbsa.reg_offset;
 	uap->vendor	= &vendor_sbsa;
 	uap->fifosize	= 32;
+	uap->port.iotype = vendor_sbsa.access_32b ? UPIO_MEM32 : UPIO_MEM;
 	uap->port.irq	= platform_get_irq(pdev, 0);
 	uap->port.ops	= &sbsa_uart_pops;
 	uap->fixed_baud = baudrate;
diff --git a/drivers/tty/serial/amba-pl011.h b/drivers/tty/serial/amba-pl011.h
new file mode 100644
index 000000000000..411c60e1f9a4
--- /dev/null
+++ b/drivers/tty/serial/amba-pl011.h
@@ -0,0 +1,34 @@
+#ifndef AMBA_PL011_H
+#define AMBA_PL011_H
+
+enum {
+	REG_DR,
+	REG_ST_DMAWM,
+	REG_ST_TIMEOUT,
+	REG_FR,
+	REG_LCRH_RX,
+	REG_LCRH_TX,
+	REG_IBRD,
+	REG_FBRD,
+	REG_CR,
+	REG_IFLS,
+	REG_IMSC,
+	REG_RIS,
+	REG_MIS,
+	REG_ICR,
+	REG_DMACR,
+	REG_ST_XFCR,
+	REG_ST_XON1,
+	REG_ST_XON2,
+	REG_ST_XOFF1,
+	REG_ST_XOFF2,
+	REG_ST_ITCR,
+	REG_ST_ITIP,
+	REG_ST_ABCR,
+	REG_ST_ABIMSC,
+
+	/* The size of the array - must be last */
+	REG_ARRAY_SIZE,
+};
+
+#endif
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 94294558943c..1c0884d8ef32 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -22,7 +22,6 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  *
  */
-#include <linux/module.h>
 #include <linux/tty.h>
 #include <linux/ioport.h>
 #include <linux/slab.h>
@@ -155,7 +154,6 @@ struct atmel_uart_port {
 	struct circ_buf		rx_ring;
 
 	struct mctrl_gpios	*gpios;
-	int			gpio_irq[UART_GPIO_MAX];
 	unsigned int		tx_done_mask;
 	u32			fifo_size;
 	u32			rts_high;
@@ -190,8 +188,6 @@ static const struct of_device_id atmel_serial_dt_ids[] = {
 	{ .compatible = "atmel,at91sam9260-usart" },
 	{ /* sentinel */ }
 };
-
-MODULE_DEVICE_TABLE(of, atmel_serial_dt_ids);
 #endif
 
 static inline struct atmel_uart_port *
@@ -550,27 +546,21 @@ static void atmel_enable_ms(struct uart_port *port)
 
 	atmel_port->ms_irq_enabled = true;
 
-	if (atmel_port->gpio_irq[UART_GPIO_CTS] >= 0)
-		enable_irq(atmel_port->gpio_irq[UART_GPIO_CTS]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_CTS))
 		ier |= ATMEL_US_CTSIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_DSR] >= 0)
-		enable_irq(atmel_port->gpio_irq[UART_GPIO_DSR]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_DSR))
 		ier |= ATMEL_US_DSRIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_RI] >= 0)
-		enable_irq(atmel_port->gpio_irq[UART_GPIO_RI]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_RI))
 		ier |= ATMEL_US_RIIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_DCD] >= 0)
-		enable_irq(atmel_port->gpio_irq[UART_GPIO_DCD]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_DCD))
 		ier |= ATMEL_US_DCDIC;
 
 	atmel_uart_writel(port, ATMEL_US_IER, ier);
+
+	mctrl_gpio_enable_ms(atmel_port->gpios);
 }
 
 /*
@@ -589,24 +579,18 @@ static void atmel_disable_ms(struct uart_port *port)
 
 	atmel_port->ms_irq_enabled = false;
 
-	if (atmel_port->gpio_irq[UART_GPIO_CTS] >= 0)
-		disable_irq(atmel_port->gpio_irq[UART_GPIO_CTS]);
-	else
+	mctrl_gpio_disable_ms(atmel_port->gpios);
+
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_CTS))
 		idr |= ATMEL_US_CTSIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_DSR] >= 0)
-		disable_irq(atmel_port->gpio_irq[UART_GPIO_DSR]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_DSR))
 		idr |= ATMEL_US_DSRIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_RI] >= 0)
-		disable_irq(atmel_port->gpio_irq[UART_GPIO_RI]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_RI))
 		idr |= ATMEL_US_RIIC;
 
-	if (atmel_port->gpio_irq[UART_GPIO_DCD] >= 0)
-		disable_irq(atmel_port->gpio_irq[UART_GPIO_DCD]);
-	else
+	if (!mctrl_gpio_to_gpiod(atmel_port->gpios, UART_GPIO_DCD))
 		idr |= ATMEL_US_DCDIC;
 
 	atmel_uart_writel(port, ATMEL_US_IDR, idr);
@@ -1264,7 +1248,6 @@ static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 	struct uart_port *port = dev_id;
 	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
 	unsigned int status, pending, mask, pass_counter = 0;
-	bool gpio_handled = false;
 
 	spin_lock(&atmel_port->lock_suspended);
 
@@ -1272,24 +1255,6 @@ static irqreturn_t atmel_interrupt(int irq, void *dev_id)
 		status = atmel_get_lines_status(port);
 		mask = atmel_uart_readl(port, ATMEL_US_IMR);
 		pending = status & mask;
-		if (!gpio_handled) {
-			/*
-			 * Dealing with GPIO interrupt
-			 */
-			if (irq == atmel_port->gpio_irq[UART_GPIO_CTS])
-				pending |= ATMEL_US_CTSIC;
-
-			if (irq == atmel_port->gpio_irq[UART_GPIO_DSR])
-				pending |= ATMEL_US_DSRIC;
-
-			if (irq == atmel_port->gpio_irq[UART_GPIO_RI])
-				pending |= ATMEL_US_RIIC;
-
-			if (irq == atmel_port->gpio_irq[UART_GPIO_DCD])
-				pending |= ATMEL_US_DCDIC;
-
-			gpio_handled = true;
-		}
 		if (!pending)
 			break;
 
@@ -1778,45 +1743,6 @@ static void atmel_get_ip_name(struct uart_port *port)
 	}
 }
 
-static void atmel_free_gpio_irq(struct uart_port *port)
-{
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	enum mctrl_gpio_idx i;
-
-	for (i = 0; i < UART_GPIO_MAX; i++)
-		if (atmel_port->gpio_irq[i] >= 0)
-			free_irq(atmel_port->gpio_irq[i], port);
-}
-
-static int atmel_request_gpio_irq(struct uart_port *port)
-{
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	int *irq = atmel_port->gpio_irq;
-	enum mctrl_gpio_idx i;
-	int err = 0;
-
-	for (i = 0; (i < UART_GPIO_MAX) && !err; i++) {
-		if (irq[i] < 0)
-			continue;
-
-		irq_set_status_flags(irq[i], IRQ_NOAUTOEN);
-		err = request_irq(irq[i], atmel_interrupt, IRQ_TYPE_EDGE_BOTH,
-				  "atmel_serial", port);
-		if (err)
-			dev_err(port->dev, "atmel_startup - Can't get %d irq\n",
-				irq[i]);
-	}
-
-	/*
-	 * If something went wrong, rollback.
-	 */
-	while (err && (--i >= 0))
-		if (irq[i] >= 0)
-			free_irq(irq[i], port);
-
-	return err;
-}
-
 /*
  * Perform initialization and enable port for reception
  */
@@ -1846,13 +1772,6 @@ static int atmel_startup(struct uart_port *port)
 		return retval;
 	}
 
-	/*
-	 * Get the GPIO lines IRQ
-	 */
-	retval = atmel_request_gpio_irq(port);
-	if (retval)
-		goto free_irq;
-
 	tasklet_enable(&atmel_port->tasklet);
 
 	/*
@@ -1948,11 +1867,6 @@ static int atmel_startup(struct uart_port *port)
 	}
 
 	return 0;
-
-free_irq:
-	free_irq(port->irq, port);
-
-	return retval;
 }
 
 /*
@@ -2018,7 +1932,6 @@ static void atmel_shutdown(struct uart_port *port)
 	 * Free the interrupts
 	 */
 	free_irq(port->irq, port);
-	atmel_free_gpio_irq(port);
 
 	atmel_port->ms_irq_enabled = false;
 
@@ -2686,26 +2599,6 @@ static int atmel_serial_resume(struct platform_device *pdev)
 #define atmel_serial_resume NULL
 #endif
 
-static int atmel_init_gpios(struct atmel_uart_port *p, struct device *dev)
-{
-	enum mctrl_gpio_idx i;
-	struct gpio_desc *gpiod;
-
-	p->gpios = mctrl_gpio_init_noauto(dev, 0);
-	if (IS_ERR(p->gpios))
-		return PTR_ERR(p->gpios);
-
-	for (i = 0; i < UART_GPIO_MAX; i++) {
-		gpiod = mctrl_gpio_to_gpiod(p->gpios, i);
-		if (gpiod && (gpiod_get_direction(gpiod) == GPIOF_DIR_IN))
-			p->gpio_irq[i] = gpiod_to_irq(gpiod);
-		else
-			p->gpio_irq[i] = -EINVAL;
-	}
-
-	return 0;
-}
-
 static void atmel_serial_probe_fifos(struct atmel_uart_port *port,
 				     struct platform_device *pdev)
 {
@@ -2788,16 +2681,16 @@ static int atmel_serial_probe(struct platform_device *pdev)
 
 	spin_lock_init(&port->lock_suspended);
 
-	ret = atmel_init_gpios(port, &pdev->dev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Failed to initialize GPIOs.");
-		goto err_clear_bit;
-	}
-
 	ret = atmel_init_port(port, pdev);
 	if (ret)
 		goto err_clear_bit;
 
+	port->gpios = mctrl_gpio_init(&port->uart, 0);
+	if (IS_ERR(port->gpios)) {
+		ret = PTR_ERR(port->gpios);
+		goto err_clear_bit;
+	}
+
 	if (!atmel_use_pdc_rx(&port->uart)) {
 		ret = -ENOMEM;
 		data = kmalloc(sizeof(struct atmel_uart_char)
@@ -2866,37 +2759,14 @@ err:
 	return ret;
 }
 
-static int atmel_serial_remove(struct platform_device *pdev)
-{
-	struct uart_port *port = platform_get_drvdata(pdev);
-	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
-	int ret = 0;
-
-	tasklet_kill(&atmel_port->tasklet);
-
-	device_init_wakeup(&pdev->dev, 0);
-
-	ret = uart_remove_one_port(&atmel_uart, port);
-
-	kfree(atmel_port->rx_ring.buf);
-
-	/* "port" is allocated statically, so we shouldn't free it */
-
-	clear_bit(port->line, atmel_ports_in_use);
-
-	clk_put(atmel_port->clk);
-
-	return ret;
-}
-
 static struct platform_driver atmel_serial_driver = {
 	.probe		= atmel_serial_probe,
-	.remove		= atmel_serial_remove,
 	.suspend	= atmel_serial_suspend,
 	.resume		= atmel_serial_resume,
 	.driver		= {
-		.name	= "atmel_usart",
-		.of_match_table	= of_match_ptr(atmel_serial_dt_ids),
+		.name			= "atmel_usart",
+		.of_match_table		= of_match_ptr(atmel_serial_dt_ids),
+		.suppress_bind_attrs    = true,
 	},
 };
 
@@ -2914,17 +2784,4 @@ static int __init atmel_serial_init(void)
 
 	return ret;
 }
-
-static void __exit atmel_serial_exit(void)
-{
-	platform_driver_unregister(&atmel_serial_driver);
-	uart_unregister_driver(&atmel_uart);
-}
-
-module_init(atmel_serial_init);
-module_exit(atmel_serial_exit);
-
-MODULE_AUTHOR("Rick Bronson");
-MODULE_DESCRIPTION("Atmel AT91 / AT32 serial port driver");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:atmel_usart");
+device_initcall(atmel_serial_init);
diff --git a/drivers/tty/serial/bcm63xx_uart.c b/drivers/tty/serial/bcm63xx_uart.c
index a1c0a89d9c7f..c28e5c24da16 100644
--- a/drivers/tty/serial/bcm63xx_uart.c
+++ b/drivers/tty/serial/bcm63xx_uart.c
@@ -653,7 +653,7 @@ static struct uart_ops bcm_uart_ops = {
 
 
 #ifdef CONFIG_SERIAL_BCM63XX_CONSOLE
-static inline void wait_for_xmitr(struct uart_port *port)
+static void wait_for_xmitr(struct uart_port *port)
 {
 	unsigned int tmout;
 
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index ae3cf94b146b..293ecbb00684 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -213,7 +213,7 @@ static void bfin_serial_stop_rx(struct uart_port *port)
 static void bfin_serial_rx_chars(struct bfin_serial_port *uart)
 {
 	unsigned int status, ch, flg;
-	static struct timeval anomaly_start = { .tv_sec = 0 };
+	static u64 anomaly_start;
 
 	status = UART_GET_LSR(uart);
 	UART_CLEAR_LSR(uart);
@@ -246,27 +246,24 @@ static void bfin_serial_rx_chars(struct bfin_serial_port *uart)
 		 * character time +/- some percent.  So 1.5 sounds good.  All other
 		 * Blackfin families operate properly.  Woo.
 		 */
-		if (anomaly_start.tv_sec) {
-			struct timeval curr;
-			suseconds_t usecs;
+		if (anomaly_start > 0) {
+			u64 curr, nsecs, threshold_ns;
 
 			if ((~ch & (~ch + 1)) & 0xff)
 				goto known_good_char;
 
-			do_gettimeofday(&curr);
-			if (curr.tv_sec - anomaly_start.tv_sec > 1)
+			curr = ktime_get_ns();
+			nsecs = curr - anomaly_start;
+			if (nsecs >> 32)
 				goto known_good_char;
 
-			usecs = 0;
-			if (curr.tv_sec != anomaly_start.tv_sec)
-				usecs += USEC_PER_SEC;
-			usecs += curr.tv_usec - anomaly_start.tv_usec;
-
-			if (usecs > UART_GET_ANOMALY_THRESHOLD(uart))
+			threshold_ns = UART_GET_ANOMALY_THRESHOLD(uart)
+							* NSEC_PER_USEC;
+			if (nsecs > threshold_ns)
 				goto known_good_char;
 
 			if (ch)
-				anomaly_start.tv_sec = 0;
+				anomaly_start = 0;
 			else
 				anomaly_start = curr;
 
@@ -274,14 +271,14 @@ static void bfin_serial_rx_chars(struct bfin_serial_port *uart)
 
  known_good_char:
 			status &= ~BI;
-			anomaly_start.tv_sec = 0;
+			anomaly_start = 0;
 		}
 	}
 
 	if (status & BI) {
 		if (ANOMALY_05000363)
 			if (bfin_revid() < 5)
-				do_gettimeofday(&anomaly_start);
+				anomaly_start = ktime_get_ns();
 		uart->port.icount.brk++;
 		if (uart_handle_break(&uart->port))
 			goto ignore_char;
diff --git a/drivers/tty/serial/earlycon.c b/drivers/tty/serial/earlycon.c
index b5b2f2be6be7..3f2423690d01 100644
--- a/drivers/tty/serial/earlycon.c
+++ b/drivers/tty/serial/earlycon.c
@@ -71,10 +71,16 @@ static int __init parse_options(struct earlycon_device *device, char *options)
 		return -EINVAL;
 
 	switch (port->iotype) {
+	case UPIO_MEM:
+		port->mapbase = addr;
+		break;
+	case UPIO_MEM16:
+		port->regshift = 1;
+		port->mapbase = addr;
+		break;
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
-		port->regshift = 2;	/* fall-through */
-	case UPIO_MEM:
+		port->regshift = 2;
 		port->mapbase = addr;
 		break;
 	case UPIO_PORT:
@@ -91,10 +97,11 @@ static int __init parse_options(struct earlycon_device *device, char *options)
 		strlcpy(device->options, options, length);
 	}
 
-	if (port->iotype == UPIO_MEM || port->iotype == UPIO_MEM32 ||
-	    port->iotype == UPIO_MEM32BE)
+	if (port->iotype == UPIO_MEM || port->iotype == UPIO_MEM16 ||
+	    port->iotype == UPIO_MEM32 || port->iotype == UPIO_MEM32BE)
 		pr_info("Early serial console at MMIO%s 0x%llx (options '%s')\n",
 			(port->iotype == UPIO_MEM) ? "" :
+			(port->iotype == UPIO_MEM16) ? "16" :
 			(port->iotype == UPIO_MEM32) ? "32" : "32be",
 			(unsigned long long)port->mapbase,
 			device->options);
diff --git a/drivers/tty/serial/icom.c b/drivers/tty/serial/icom.c
index ffc7cb2585a6..c60a8d5e4020 100644
--- a/drivers/tty/serial/icom.c
+++ b/drivers/tty/serial/icom.c
@@ -22,7 +22,6 @@
   * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
   *
   */
-#define SERIAL_DO_RESTART
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/errno.h>
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 016e4be05cec..9362f54c816c 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -44,6 +44,8 @@
 #include <linux/platform_data/serial-imx.h>
 #include <linux/platform_data/dma-imx.h>
 
+#include "serial_mctrl_gpio.h"
+
 /* Register definitions */
 #define URXD0 0x0  /* Receiver Register */
 #define URTX0 0x40 /* Transmitter Register */
@@ -148,8 +150,11 @@
 #define USR2_TXFE	 (1<<14) /* Transmit buffer FIFO empty */
 #define USR2_DTRF	 (1<<13) /* DTR edge interrupt flag */
 #define USR2_IDLE	 (1<<12) /* Idle condition */
+#define USR2_RIDELT	 (1<<10) /* Ring Interrupt Delta */
+#define USR2_RIIN	 (1<<9)	 /* Ring Indicator Input */
 #define USR2_IRINT	 (1<<8)	 /* Serial infrared interrupt flag */
 #define USR2_WAKE	 (1<<7)	 /* Wake */
+#define USR2_DCDIN	 (1<<5)	 /* Data Carrier Detect Input */
 #define USR2_RTSF	 (1<<4)	 /* RTS edge interrupt flag */
 #define USR2_TXDC	 (1<<3)	 /* Transmitter complete */
 #define USR2_BRCD	 (1<<2)	 /* Break condition */
@@ -206,6 +211,8 @@ struct imx_port {
 	struct clk		*clk_per;
 	const struct imx_uart_data *devdata;
 
+	struct mctrl_gpios *gpios;
+
 	/* DMA fields */
 	unsigned int		dma_is_inited:1;
 	unsigned int		dma_is_enabled:1;
@@ -308,49 +315,24 @@ static void imx_port_ucrs_restore(struct uart_port *port,
 }
 #endif
 
-/*
- * Handle any change of modem status signal since we were last called.
- */
-static void imx_mctrl_check(struct imx_port *sport)
+static void imx_port_rts_active(struct imx_port *sport, unsigned long *ucr2)
 {
-	unsigned int status, changed;
-
-	status = sport->port.ops->get_mctrl(&sport->port);
-	changed = status ^ sport->old_status;
-
-	if (changed == 0)
-		return;
-
-	sport->old_status = status;
+	*ucr2 &= ~UCR2_CTSC;
+	*ucr2 |= UCR2_CTS;
 
-	if (changed & TIOCM_RI)
-		sport->port.icount.rng++;
-	if (changed & TIOCM_DSR)
-		sport->port.icount.dsr++;
-	if (changed & TIOCM_CAR)
-		uart_handle_dcd_change(&sport->port, status & TIOCM_CAR);
-	if (changed & TIOCM_CTS)
-		uart_handle_cts_change(&sport->port, status & TIOCM_CTS);
-
-	wake_up_interruptible(&sport->port.state->port.delta_msr_wait);
+	mctrl_gpio_set(sport->gpios, sport->port.mctrl | TIOCM_RTS);
 }
 
-/*
- * This is our per-port timeout handler, for checking the
- * modem status signals.
- */
-static void imx_timeout(unsigned long data)
+static void imx_port_rts_inactive(struct imx_port *sport, unsigned long *ucr2)
 {
-	struct imx_port *sport = (struct imx_port *)data;
-	unsigned long flags;
+	*ucr2 &= ~(UCR2_CTSC | UCR2_CTS);
 
-	if (sport->port.state) {
-		spin_lock_irqsave(&sport->port.lock, flags);
-		imx_mctrl_check(sport);
-		spin_unlock_irqrestore(&sport->port.lock, flags);
+	mctrl_gpio_set(sport->gpios, sport->port.mctrl & ~TIOCM_RTS);
+}
 
-		mod_timer(&sport->timer, jiffies + MCTRL_TIMEOUT);
-	}
+static void imx_port_rts_auto(struct imx_port *sport, unsigned long *ucr2)
+{
+	*ucr2 |= UCR2_CTSC;
 }
 
 /*
@@ -376,9 +358,9 @@ static void imx_stop_tx(struct uart_port *port)
 	    readl(port->membase + USR2) & USR2_TXDC) {
 		temp = readl(port->membase + UCR2);
 		if (port->rs485.flags & SER_RS485_RTS_AFTER_SEND)
-			temp &= ~UCR2_CTS;
+			imx_port_rts_inactive(sport, &temp);
 		else
-			temp |= UCR2_CTS;
+			imx_port_rts_active(sport, &temp);
 		writel(temp, port->membase + UCR2);
 
 		temp = readl(port->membase + UCR4);
@@ -420,6 +402,8 @@ static void imx_enable_ms(struct uart_port *port)
 	struct imx_port *sport = (struct imx_port *)port;
 
 	mod_timer(&sport->timer, jiffies);
+
+	mctrl_gpio_enable_ms(sport->gpios);
 }
 
 static void imx_dma_tx(struct imx_port *sport);
@@ -579,14 +563,14 @@ static void imx_start_tx(struct uart_port *port)
 	unsigned long temp;
 
 	if (port->rs485.flags & SER_RS485_ENABLED) {
-		/* enable transmitter and shifter empty irq */
 		temp = readl(port->membase + UCR2);
 		if (port->rs485.flags & SER_RS485_RTS_ON_SEND)
-			temp &= ~UCR2_CTS;
+			imx_port_rts_inactive(sport, &temp);
 		else
-			temp |= UCR2_CTS;
+			imx_port_rts_active(sport, &temp);
 		writel(temp, port->membase + UCR2);
 
+		/* enable transmitter and shifter empty irq */
 		temp = readl(port->membase + UCR4);
 		temp |= UCR4_TCEN;
 		writel(temp, port->membase + UCR4);
@@ -801,23 +785,35 @@ static unsigned int imx_tx_empty(struct uart_port *port)
 /*
  * We have a modem side uart, so the meanings of RTS and CTS are inverted.
  */
-static unsigned int imx_get_mctrl(struct uart_port *port)
+static unsigned int imx_get_hwmctrl(struct imx_port *sport)
 {
-	struct imx_port *sport = (struct imx_port *)port;
-	unsigned int tmp = TIOCM_DSR | TIOCM_CAR;
+	unsigned int tmp = TIOCM_DSR;
+	unsigned usr1 = readl(sport->port.membase + USR1);
 
-	if (readl(sport->port.membase + USR1) & USR1_RTSS)
+	if (usr1 & USR1_RTSS)
 		tmp |= TIOCM_CTS;
 
-	if (readl(sport->port.membase + UCR2) & UCR2_CTS)
-		tmp |= TIOCM_RTS;
+	/* in DCE mode DCDIN is always 0 */
+	if (!(usr1 & USR2_DCDIN))
+		tmp |= TIOCM_CAR;
 
-	if (readl(sport->port.membase + uts_reg(sport)) & UTS_LOOP)
-		tmp |= TIOCM_LOOP;
+	/* in DCE mode RIIN is always 0 */
+	if (readl(sport->port.membase + USR2) & USR2_RIIN)
+		tmp |= TIOCM_RI;
 
 	return tmp;
 }
 
+static unsigned int imx_get_mctrl(struct uart_port *port)
+{
+	struct imx_port *sport = (struct imx_port *)port;
+	unsigned int ret = imx_get_hwmctrl(sport);
+
+	mctrl_gpio_get(sport->gpios, &ret);
+
+	return ret;
+}
+
 static void imx_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	struct imx_port *sport = (struct imx_port *)port;
@@ -831,10 +827,17 @@ static void imx_set_mctrl(struct uart_port *port, unsigned int mctrl)
 		writel(temp, sport->port.membase + UCR2);
 	}
 
+	temp = readl(sport->port.membase + UCR3) & ~UCR3_DSR;
+	if (!(mctrl & TIOCM_DTR))
+		temp |= UCR3_DSR;
+	writel(temp, sport->port.membase + UCR3);
+
 	temp = readl(sport->port.membase + uts_reg(sport)) & ~UTS_LOOP;
 	if (mctrl & TIOCM_LOOP)
 		temp |= UTS_LOOP;
 	writel(temp, sport->port.membase + uts_reg(sport));
+
+	mctrl_gpio_set(sport->gpios, mctrl);
 }
 
 /*
@@ -857,6 +860,51 @@ static void imx_break_ctl(struct uart_port *port, int break_state)
 	spin_unlock_irqrestore(&sport->port.lock, flags);
 }
 
+/*
+ * Handle any change of modem status signal since we were last called.
+ */
+static void imx_mctrl_check(struct imx_port *sport)
+{
+	unsigned int status, changed;
+
+	status = imx_get_hwmctrl(sport);
+	changed = status ^ sport->old_status;
+
+	if (changed == 0)
+		return;
+
+	sport->old_status = status;
+
+	if (changed & TIOCM_RI)
+		sport->port.icount.rng++;
+	if (changed & TIOCM_DSR)
+		sport->port.icount.dsr++;
+	if (changed & TIOCM_CAR)
+		uart_handle_dcd_change(&sport->port, status & TIOCM_CAR);
+	if (changed & TIOCM_CTS)
+		uart_handle_cts_change(&sport->port, status & TIOCM_CTS);
+
+	wake_up_interruptible(&sport->port.state->port.delta_msr_wait);
+}
+
+/*
+ * This is our per-port timeout handler, for checking the
+ * modem status signals.
+ */
+static void imx_timeout(unsigned long data)
+{
+	struct imx_port *sport = (struct imx_port *)data;
+	unsigned long flags;
+
+	if (sport->port.state) {
+		spin_lock_irqsave(&sport->port.lock, flags);
+		imx_mctrl_check(sport);
+		spin_unlock_irqrestore(&sport->port.lock, flags);
+
+		mod_timer(&sport->timer, jiffies + MCTRL_TIMEOUT);
+	}
+}
+
 #define RX_BUF_SIZE	(PAGE_SIZE)
 static void imx_rx_dma_done(struct imx_port *sport)
 {
@@ -1207,6 +1255,8 @@ static void imx_shutdown(struct uart_port *port)
 		imx_uart_dma_exit(sport);
 	}
 
+	mctrl_gpio_disable_ms(sport->gpios);
+
 	spin_lock_irqsave(&sport->port.lock, flags);
 	temp = readl(sport->port.membase + UCR2);
 	temp &= ~(UCR2_TXEN);
@@ -1284,9 +1334,10 @@ imx_set_termios(struct uart_port *port, struct ktermios *termios,
 {
 	struct imx_port *sport = (struct imx_port *)port;
 	unsigned long flags;
-	unsigned int ucr2, old_ucr1, old_ucr2, baud, quot;
+	unsigned long ucr2, old_ucr1, old_ucr2;
+	unsigned int baud, quot;
 	unsigned int old_csize = old ? old->c_cflag & CSIZE : CS8;
-	unsigned int div, ufcr;
+	unsigned long div, ufcr;
 	unsigned long num, denom;
 	uint64_t tdiv64;
 
@@ -1315,19 +1366,25 @@ imx_set_termios(struct uart_port *port, struct ktermios *termios,
 				 * it under manual control and keep transmitter
 				 * disabled.
 				 */
-				if (!(port->rs485.flags &
-				      SER_RS485_RTS_AFTER_SEND))
-					ucr2 |= UCR2_CTS;
+				if (port->rs485.flags &
+				    SER_RS485_RTS_AFTER_SEND)
+					imx_port_rts_inactive(sport, &ucr2);
+				else
+					imx_port_rts_active(sport, &ucr2);
 			} else {
-				ucr2 |= UCR2_CTSC;
+				imx_port_rts_auto(sport, &ucr2);
 			}
 		} else {
 			termios->c_cflag &= ~CRTSCTS;
 		}
-	} else if (port->rs485.flags & SER_RS485_ENABLED)
+	} else if (port->rs485.flags & SER_RS485_ENABLED) {
 		/* disable transmitter */
-		if (!(port->rs485.flags & SER_RS485_RTS_AFTER_SEND))
-			ucr2 |= UCR2_CTS;
+		if (port->rs485.flags & SER_RS485_RTS_AFTER_SEND)
+			imx_port_rts_inactive(sport, &ucr2);
+		else
+			imx_port_rts_active(sport, &ucr2);
+	}
+
 
 	if (termios->c_cflag & CSTOPB)
 		ucr2 |= UCR2_STPB;
@@ -1568,11 +1625,10 @@ static int imx_rs485_config(struct uart_port *port,
 
 		/* disable transmitter */
 		temp = readl(sport->port.membase + UCR2);
-		temp &= ~UCR2_CTSC;
 		if (rs485conf->flags & SER_RS485_RTS_AFTER_SEND)
-			temp &= ~UCR2_CTS;
+			imx_port_rts_inactive(sport, &temp);
 		else
-			temp |= UCR2_CTS;
+			imx_port_rts_active(sport, &temp);
 		writel(temp, sport->port.membase + UCR2);
 	}
 
@@ -1857,11 +1913,10 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 		struct platform_device *pdev)
 {
 	struct device_node *np = pdev->dev.of_node;
-	const struct of_device_id *of_id =
-			of_match_device(imx_uart_dt_ids, &pdev->dev);
 	int ret;
 
-	if (!np)
+	sport->devdata = of_device_get_match_data(&pdev->dev);
+	if (!sport->devdata)
 		/* no device tree device */
 		return 1;
 
@@ -1878,8 +1933,6 @@ static int serial_imx_probe_dt(struct imx_port *sport,
 	if (of_get_property(np, "fsl,dte-mode", NULL))
 		sport->dte_mode = 1;
 
-	sport->devdata = of_id->data;
-
 	return 0;
 }
 #else
@@ -1948,6 +2001,10 @@ static int serial_imx_probe(struct platform_device *pdev)
 	sport->timer.function = imx_timeout;
 	sport->timer.data     = (unsigned long)sport;
 
+	sport->gpios = mctrl_gpio_init(&sport->port, 0);
+	if (IS_ERR(sport->gpios))
+		return PTR_ERR(sport->gpios);
+
 	sport->clk_ipg = devm_clk_get(&pdev->dev, "ipg");
 	if (IS_ERR(sport->clk_ipg)) {
 		ret = PTR_ERR(sport->clk_ipg);
diff --git a/drivers/tty/serial/jsm/jsm_driver.c b/drivers/tty/serial/jsm/jsm_driver.c
index efbd87a76656..a119f11bf2f4 100644
--- a/drivers/tty/serial/jsm/jsm_driver.c
+++ b/drivers/tty/serial/jsm/jsm_driver.c
@@ -70,7 +70,7 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out;
 	}
 
-	rc = pci_request_regions(pdev, "jsm");
+	rc = pci_request_regions(pdev, JSM_DRIVER_NAME);
 	if (rc) {
 		dev_err(&pdev->dev, "pci_request_region FAILED\n");
 		goto out_disable_device;
@@ -328,7 +328,7 @@ static struct pci_device_id jsm_pci_tbl[] = {
 MODULE_DEVICE_TABLE(pci, jsm_pci_tbl);
 
 static struct pci_driver jsm_driver = {
-	.name		= "jsm",
+	.name		= JSM_DRIVER_NAME,
 	.id_table	= jsm_pci_tbl,
 	.probe		= jsm_probe_one,
 	.remove		= jsm_remove_one,
diff --git a/drivers/tty/serial/jsm/jsm_neo.c b/drivers/tty/serial/jsm/jsm_neo.c
index 932b2accd06f..c6fdd6369534 100644
--- a/drivers/tty/serial/jsm/jsm_neo.c
+++ b/drivers/tty/serial/jsm/jsm_neo.c
@@ -714,7 +714,7 @@ static void neo_clear_break(struct jsm_channel *ch)
 /*
  * Parse the ISR register.
  */
-static inline void neo_parse_isr(struct jsm_board *brd, u32 port)
+static void neo_parse_isr(struct jsm_board *brd, u32 port)
 {
 	struct jsm_channel *ch;
 	u8 isr;
diff --git a/drivers/tty/serial/m32r_sio.c b/drivers/tty/serial/m32r_sio.c
index 8f7f83a14c93..0eeb64f2499c 100644
--- a/drivers/tty/serial/m32r_sio.c
+++ b/drivers/tty/serial/m32r_sio.c
@@ -990,7 +990,7 @@ static void __init m32r_sio_register_ports(struct uart_driver *drv)
 /*
  *	Wait for transmitter & holding register to empty
  */
-static inline void wait_for_xmitr(struct uart_sio_port *up)
+static void wait_for_xmitr(struct uart_sio_port *up)
 {
 	unsigned int status, tmout = 10000;
 
diff --git a/drivers/tty/serial/max310x.c b/drivers/tty/serial/max310x.c
index d45133056f51..3f98165b479c 100644
--- a/drivers/tty/serial/max310x.c
+++ b/drivers/tty/serial/max310x.c
@@ -1174,7 +1174,7 @@ static int max310x_probe(struct device *dev, struct max310x_devtype *devtype,
 #ifdef CONFIG_GPIOLIB
 	/* Setup GPIO cotroller */
 	s->gpio.owner		= THIS_MODULE;
-	s->gpio.dev		= dev;
+	s->gpio.parent		= dev;
 	s->gpio.label		= dev_name(dev);
 	s->gpio.direction_input	= max310x_gpio_direction_input;
 	s->gpio.get		= max310x_gpio_get;
diff --git a/drivers/tty/serial/men_z135_uart.c b/drivers/tty/serial/men_z135_uart.c
index 3141aa20843d..a44290e9b5a8 100644
--- a/drivers/tty/serial/men_z135_uart.c
+++ b/drivers/tty/serial/men_z135_uart.c
@@ -158,7 +158,7 @@ static inline void men_z135_reg_set(struct men_z135_port *uart,
  * @addr: Register address
  * @val: value to clear
  */
-static inline void men_z135_reg_clr(struct men_z135_port *uart,
+static void men_z135_reg_clr(struct men_z135_port *uart,
 				u32 addr, u32 val)
 {
 	struct uart_port *port = &uart->port;
diff --git a/drivers/tty/serial/meson_uart.c b/drivers/tty/serial/meson_uart.c
index 0fc83c962d10..b12a37bd37b6 100644
--- a/drivers/tty/serial/meson_uart.c
+++ b/drivers/tty/serial/meson_uart.c
@@ -57,6 +57,7 @@
 #define AML_UART_RX_EMPTY		BIT(20)
 #define AML_UART_TX_FULL		BIT(21)
 #define AML_UART_TX_EMPTY		BIT(22)
+#define AML_UART_XMIT_BUSY		BIT(25)
 #define AML_UART_ERR			(AML_UART_PARITY_ERR | \
 					 AML_UART_FRAME_ERR  | \
 					 AML_UART_TX_FIFO_WERR)
@@ -100,7 +101,8 @@ static unsigned int meson_uart_tx_empty(struct uart_port *port)
 	u32 val;
 
 	val = readl(port->membase + AML_UART_STATUS);
-	return (val & AML_UART_TX_EMPTY) ? TIOCSER_TEMT : 0;
+	val &= (AML_UART_TX_EMPTY | AML_UART_XMIT_BUSY);
+	return (val == AML_UART_TX_EMPTY) ? TIOCSER_TEMT : 0;
 }
 
 static void meson_uart_stop_tx(struct uart_port *port)
@@ -108,7 +110,7 @@ static void meson_uart_stop_tx(struct uart_port *port)
 	u32 val;
 
 	val = readl(port->membase + AML_UART_CONTROL);
-	val &= ~AML_UART_TX_EN;
+	val &= ~AML_UART_TX_INT_EN;
 	writel(val, port->membase + AML_UART_CONTROL);
 }
 
@@ -131,7 +133,7 @@ static void meson_uart_shutdown(struct uart_port *port)
 	spin_lock_irqsave(&port->lock, flags);
 
 	val = readl(port->membase + AML_UART_CONTROL);
-	val &= ~(AML_UART_RX_EN | AML_UART_TX_EN);
+	val &= ~AML_UART_RX_EN;
 	val &= ~(AML_UART_RX_INT_EN | AML_UART_TX_INT_EN);
 	writel(val, port->membase + AML_UART_CONTROL);
 
@@ -142,6 +144,7 @@ static void meson_uart_start_tx(struct uart_port *port)
 {
 	struct circ_buf *xmit = &port->state->xmit;
 	unsigned int ch;
+	u32 val;
 
 	if (uart_tx_stopped(port)) {
 		meson_uart_stop_tx(port);
@@ -165,6 +168,12 @@ static void meson_uart_start_tx(struct uart_port *port)
 		port->icount.tx++;
 	}
 
+	if (!uart_circ_empty(xmit)) {
+		val = readl(port->membase + AML_UART_CONTROL);
+		val |= AML_UART_TX_INT_EN;
+		writel(val, port->membase + AML_UART_CONTROL);
+	}
+
 	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
 		uart_write_wakeup(port);
 }
@@ -228,8 +237,10 @@ static irqreturn_t meson_uart_interrupt(int irq, void *dev_id)
 	if (!(readl(port->membase + AML_UART_STATUS) & AML_UART_RX_EMPTY))
 		meson_receive_chars(port);
 
-	if (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_FULL))
-		meson_uart_start_tx(port);
+	if (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_FULL)) {
+		if (readl(port->membase + AML_UART_CONTROL) & AML_UART_TX_INT_EN)
+			meson_uart_start_tx(port);
+	}
 
 	spin_unlock(&port->lock);
 
@@ -241,10 +252,9 @@ static const char *meson_uart_type(struct uart_port *port)
 	return (port->type == PORT_MESON) ? "meson_uart" : NULL;
 }
 
-static int meson_uart_startup(struct uart_port *port)
+static void meson_uart_reset(struct uart_port *port)
 {
 	u32 val;
-	int ret = 0;
 
 	val = readl(port->membase + AML_UART_CONTROL);
 	val |= (AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
@@ -252,6 +262,18 @@ static int meson_uart_startup(struct uart_port *port)
 
 	val &= ~(AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
 	writel(val, port->membase + AML_UART_CONTROL);
+}
+
+static int meson_uart_startup(struct uart_port *port)
+{
+	u32 val;
+	int ret = 0;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val |= AML_UART_CLR_ERR;
+	writel(val, port->membase + AML_UART_CONTROL);
+	val &= ~AML_UART_CLR_ERR;
+	writel(val, port->membase + AML_UART_CONTROL);
 
 	val |= (AML_UART_RX_EN | AML_UART_TX_EN);
 	writel(val, port->membase + AML_UART_CONTROL);
@@ -272,7 +294,7 @@ static void meson_uart_change_speed(struct uart_port *port, unsigned long baud)
 {
 	u32 val;
 
-	while (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_EMPTY))
+	while (!meson_uart_tx_empty(port))
 		cpu_relax();
 
 	val = readl(port->membase + AML_UART_REG5);
@@ -367,9 +389,26 @@ static int meson_uart_verify_port(struct uart_port *port,
 	return ret;
 }
 
+static int meson_uart_res_size(struct uart_port *port)
+{
+	struct platform_device *pdev = to_platform_device(port->dev);
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(port->dev, "cannot obtain I/O memory region");
+		return -ENODEV;
+	}
+
+	return resource_size(res);
+}
+
 static void meson_uart_release_port(struct uart_port *port)
 {
+	int size = meson_uart_res_size(port);
+
 	if (port->flags & UPF_IOREMAP) {
+		devm_release_mem_region(port->dev, port->mapbase, size);
 		devm_iounmap(port->dev, port->membase);
 		port->membase = NULL;
 	}
@@ -377,16 +416,10 @@ static void meson_uart_release_port(struct uart_port *port)
 
 static int meson_uart_request_port(struct uart_port *port)
 {
-	struct platform_device *pdev = to_platform_device(port->dev);
-	struct resource *res;
-	int size;
+	int size = meson_uart_res_size(port);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res) {
-		dev_err(&pdev->dev, "cannot obtain I/O memory region");
-		return -ENODEV;
-	}
-	size = resource_size(res);
+	if (size < 0)
+		return size;
 
 	if (!devm_request_mem_region(port->dev, port->mapbase, size,
 				     dev_name(port->dev))) {
@@ -448,6 +481,7 @@ static void meson_serial_console_write(struct console *co, const char *s,
 	struct uart_port *port;
 	unsigned long flags;
 	int locked;
+	u32 val, tmp;
 
 	port = meson_ports[co->index];
 	if (!port)
@@ -463,7 +497,13 @@ static void meson_serial_console_write(struct console *co, const char *s,
 		locked = 1;
 	}
 
+	val = readl(port->membase + AML_UART_CONTROL);
+	val |= AML_UART_TX_EN;
+	tmp = val & ~(AML_UART_TX_INT_EN | AML_UART_RX_INT_EN);
+	writel(tmp, port->membase + AML_UART_CONTROL);
+
 	uart_console_write(port, s, count, meson_console_putchar);
+	writel(val, port->membase + AML_UART_CONTROL);
 
 	if (locked)
 		spin_unlock(&port->lock);
@@ -570,6 +610,12 @@ static int meson_uart_probe(struct platform_device *pdev)
 	meson_ports[pdev->id] = port;
 	platform_set_drvdata(pdev, port);
 
+	/* reset port before registering (and possibly registering console) */
+	if (meson_uart_request_port(port) >= 0) {
+		meson_uart_reset(port);
+		meson_uart_release_port(port);
+	}
+
 	ret = uart_add_one_port(&meson_uart_driver, port);
 	if (ret)
 		meson_ports[pdev->id] = NULL;
diff --git a/drivers/tty/serial/nwpserial.c b/drivers/tty/serial/nwpserial.c
deleted file mode 100644
index 5da7622e88c3..000000000000
--- a/drivers/tty/serial/nwpserial.c
+++ /dev/null
@@ -1,477 +0,0 @@
-/*
- *  Serial Port driver for a NWP uart device
- *
- *    Copyright (C) 2008 IBM Corp., Benjamin Krill <ben@codiert.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/export.h>
-#include <linux/console.h>
-#include <linux/serial.h>
-#include <linux/serial_reg.h>
-#include <linux/serial_core.h>
-#include <linux/tty.h>
-#include <linux/tty_flip.h>
-#include <linux/irqreturn.h>
-#include <linux/mutex.h>
-#include <linux/of_platform.h>
-#include <linux/of_device.h>
-#include <linux/nwpserial.h>
-#include <linux/delay.h>
-#include <asm/prom.h>
-#include <asm/dcr.h>
-
-#define NWPSERIAL_NR               2
-
-#define NWPSERIAL_STATUS_RXVALID 0x1
-#define NWPSERIAL_STATUS_TXFULL  0x2
-
-struct nwpserial_port {
-	struct uart_port port;
-	dcr_host_t dcr_host;
-	unsigned int ier;
-	unsigned int mcr;
-};
-
-static DEFINE_MUTEX(nwpserial_mutex);
-static struct nwpserial_port nwpserial_ports[NWPSERIAL_NR];
-
-static void wait_for_bits(struct nwpserial_port *up, int bits)
-{
-	unsigned int status, tmout = 10000;
-
-	/* Wait up to 10ms for the character(s) to be sent. */
-	do {
-		status = dcr_read(up->dcr_host, UART_LSR);
-
-		if (--tmout == 0)
-			break;
-		udelay(1);
-	} while ((status & bits) != bits);
-}
-
-#ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL_CONSOLE
-static void nwpserial_console_putchar(struct uart_port *port, int c)
-{
-	struct nwpserial_port *up;
-	up = container_of(port, struct nwpserial_port, port);
-	/* check if tx buffer is full */
-	wait_for_bits(up, UART_LSR_THRE);
-	dcr_write(up->dcr_host, UART_TX, c);
-	up->port.icount.tx++;
-}
-
-static void
-nwpserial_console_write(struct console *co, const char *s, unsigned int count)
-{
-	struct nwpserial_port *up = &nwpserial_ports[co->index];
-	unsigned long flags;
-	int locked = 1;
-
-	if (oops_in_progress)
-		locked = spin_trylock_irqsave(&up->port.lock, flags);
-	else
-		spin_lock_irqsave(&up->port.lock, flags);
-
-	/* save and disable interrupt */
-	up->ier = dcr_read(up->dcr_host, UART_IER);
-	dcr_write(up->dcr_host, UART_IER, up->ier & ~UART_IER_RDI);
-
-	uart_console_write(&up->port, s, count, nwpserial_console_putchar);
-
-	/* wait for transmitter to become empty */
-	while ((dcr_read(up->dcr_host, UART_LSR) & UART_LSR_THRE) == 0)
-		cpu_relax();
-
-	/* restore interrupt state */
-	dcr_write(up->dcr_host, UART_IER, up->ier);
-
-	if (locked)
-		spin_unlock_irqrestore(&up->port.lock, flags);
-}
-
-static struct uart_driver nwpserial_reg;
-static struct console nwpserial_console = {
-	.name		= "ttySQ",
-	.write		= nwpserial_console_write,
-	.device		= uart_console_device,
-	.flags		= CON_PRINTBUFFER,
-	.index		= -1,
-	.data		= &nwpserial_reg,
-};
-#define NWPSERIAL_CONSOLE	(&nwpserial_console)
-#else
-#define NWPSERIAL_CONSOLE	NULL
-#endif /* CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL_CONSOLE */
-
-/**************************************************************************/
-
-static int nwpserial_request_port(struct uart_port *port)
-{
-	return 0;
-}
-
-static void nwpserial_release_port(struct uart_port *port)
-{
-	/* N/A */
-}
-
-static void nwpserial_config_port(struct uart_port *port, int flags)
-{
-	port->type = PORT_NWPSERIAL;
-}
-
-static irqreturn_t nwpserial_interrupt(int irq, void *dev_id)
-{
-	struct nwpserial_port *up = dev_id;
-	struct tty_port *port = &up->port.state->port;
-	irqreturn_t ret;
-	unsigned int iir;
-	unsigned char ch;
-
-	spin_lock(&up->port.lock);
-
-	/* check if the uart was the interrupt source. */
-	iir = dcr_read(up->dcr_host, UART_IIR);
-	if (!iir) {
-		ret = IRQ_NONE;
-		goto out;
-	}
-
-	do {
-		up->port.icount.rx++;
-		ch = dcr_read(up->dcr_host, UART_RX);
-		if (up->port.ignore_status_mask != NWPSERIAL_STATUS_RXVALID)
-			tty_insert_flip_char(port, ch, TTY_NORMAL);
-	} while (dcr_read(up->dcr_host, UART_LSR) & UART_LSR_DR);
-
-	spin_unlock(&up->port.lock);
-	tty_flip_buffer_push(port);
-	spin_lock(&up->port.lock);
-
-	ret = IRQ_HANDLED;
-
-	/* clear interrupt */
-	dcr_write(up->dcr_host, UART_IIR, 1);
-out:
-	spin_unlock(&up->port.lock);
-	return ret;
-}
-
-static int nwpserial_startup(struct uart_port *port)
-{
-	struct nwpserial_port *up;
-	int err;
-
-	up = container_of(port, struct nwpserial_port, port);
-
-	/* disable flow control by default */
-	up->mcr = dcr_read(up->dcr_host, UART_MCR) & ~UART_MCR_AFE;
-	dcr_write(up->dcr_host, UART_MCR, up->mcr);
-
-	/* register interrupt handler */
-	err = request_irq(up->port.irq, nwpserial_interrupt,
-			IRQF_SHARED, "nwpserial", up);
-	if (err)
-		return err;
-
-	/* enable interrupts */
-	up->ier = UART_IER_RDI;
-	dcr_write(up->dcr_host, UART_IER, up->ier);
-
-	/* enable receiving */
-	up->port.ignore_status_mask &= ~NWPSERIAL_STATUS_RXVALID;
-
-	return 0;
-}
-
-static void nwpserial_shutdown(struct uart_port *port)
-{
-	struct nwpserial_port *up;
-	up = container_of(port, struct nwpserial_port, port);
-
-	/* disable receiving */
-	up->port.ignore_status_mask |= NWPSERIAL_STATUS_RXVALID;
-
-	/* disable interrupts from this port */
-	up->ier = 0;
-	dcr_write(up->dcr_host, UART_IER, up->ier);
-
-	/* free irq */
-	free_irq(up->port.irq, up);
-}
-
-static int nwpserial_verify_port(struct uart_port *port,
-			struct serial_struct *ser)
-{
-	return -EINVAL;
-}
-
-static const char *nwpserial_type(struct uart_port *port)
-{
-	return port->type == PORT_NWPSERIAL ? "nwpserial" : NULL;
-}
-
-static void nwpserial_set_termios(struct uart_port *port,
-			struct ktermios *termios, struct ktermios *old)
-{
-	struct nwpserial_port *up;
-	up = container_of(port, struct nwpserial_port, port);
-
-	up->port.read_status_mask = NWPSERIAL_STATUS_RXVALID
-				| NWPSERIAL_STATUS_TXFULL;
-
-	up->port.ignore_status_mask = 0;
-	/* ignore all characters if CREAD is not set */
-	if ((termios->c_cflag & CREAD) == 0)
-		up->port.ignore_status_mask |= NWPSERIAL_STATUS_RXVALID;
-
-	/* Copy back the old hardware settings */
-	if (old)
-		tty_termios_copy_hw(termios, old);
-}
-
-static void nwpserial_break_ctl(struct uart_port *port, int ctl)
-{
-	/* N/A */
-}
-
-static void nwpserial_stop_rx(struct uart_port *port)
-{
-	struct nwpserial_port *up;
-	up = container_of(port, struct nwpserial_port, port);
-	/* don't forward any more data (like !CREAD) */
-	up->port.ignore_status_mask = NWPSERIAL_STATUS_RXVALID;
-}
-
-static void nwpserial_putchar(struct nwpserial_port *up, unsigned char c)
-{
-	/* check if tx buffer is full */
-	wait_for_bits(up, UART_LSR_THRE);
-	dcr_write(up->dcr_host, UART_TX, c);
-	up->port.icount.tx++;
-}
-
-static void nwpserial_start_tx(struct uart_port *port)
-{
-	struct nwpserial_port *up;
-	struct circ_buf *xmit;
-	up = container_of(port, struct nwpserial_port, port);
-	xmit  = &up->port.state->xmit;
-
-	if (port->x_char) {
-		nwpserial_putchar(up, up->port.x_char);
-		port->x_char = 0;
-	}
-
-	while (!(uart_circ_empty(xmit) || uart_tx_stopped(&up->port))) {
-		nwpserial_putchar(up, xmit->buf[xmit->tail]);
-		xmit->tail = (xmit->tail + 1) & (UART_XMIT_SIZE-1);
-	}
-}
-
-static unsigned int nwpserial_get_mctrl(struct uart_port *port)
-{
-	return 0;
-}
-
-static void nwpserial_set_mctrl(struct uart_port *port, unsigned int mctrl)
-{
-	/* N/A */
-}
-
-static void nwpserial_stop_tx(struct uart_port *port)
-{
-	/* N/A */
-}
-
-static unsigned int nwpserial_tx_empty(struct uart_port *port)
-{
-	struct nwpserial_port *up;
-	unsigned long flags;
-	int ret;
-	up = container_of(port, struct nwpserial_port, port);
-
-	spin_lock_irqsave(&up->port.lock, flags);
-	ret = dcr_read(up->dcr_host, UART_LSR);
-	spin_unlock_irqrestore(&up->port.lock, flags);
-
-	return ret & UART_LSR_TEMT ? TIOCSER_TEMT : 0;
-}
-
-static struct uart_ops nwpserial_pops = {
-	.tx_empty     = nwpserial_tx_empty,
-	.set_mctrl    = nwpserial_set_mctrl,
-	.get_mctrl    = nwpserial_get_mctrl,
-	.stop_tx      = nwpserial_stop_tx,
-	.start_tx     = nwpserial_start_tx,
-	.stop_rx      = nwpserial_stop_rx,
-	.break_ctl    = nwpserial_break_ctl,
-	.startup      = nwpserial_startup,
-	.shutdown     = nwpserial_shutdown,
-	.set_termios  = nwpserial_set_termios,
-	.type         = nwpserial_type,
-	.release_port = nwpserial_release_port,
-	.request_port = nwpserial_request_port,
-	.config_port  = nwpserial_config_port,
-	.verify_port  = nwpserial_verify_port,
-};
-
-static struct uart_driver nwpserial_reg = {
-	.owner       = THIS_MODULE,
-	.driver_name = "nwpserial",
-	.dev_name    = "ttySQ",
-	.major       = TTY_MAJOR,
-	.minor       = 68,
-	.nr          = NWPSERIAL_NR,
-	.cons        = NWPSERIAL_CONSOLE,
-};
-
-int nwpserial_register_port(struct uart_port *port)
-{
-	struct nwpserial_port *up = NULL;
-	int ret = -1;
-	int i;
-	static int first = 1;
-	int dcr_len;
-	int dcr_base;
-	struct device_node *dn;
-
-	mutex_lock(&nwpserial_mutex);
-
-	dn = port->dev->of_node;
-	if (dn == NULL)
-		goto out;
-
-	/* get dcr base. */
-	dcr_base = dcr_resource_start(dn, 0);
-
-	/* find matching entry */
-	for (i = 0; i < NWPSERIAL_NR; i++)
-		if (nwpserial_ports[i].port.iobase == dcr_base) {
-			up = &nwpserial_ports[i];
-			break;
-		}
-
-	/* we didn't find a mtching entry, search for a free port */
-	if (up == NULL)
-		for (i = 0; i < NWPSERIAL_NR; i++)
-			if (nwpserial_ports[i].port.type == PORT_UNKNOWN &&
-				nwpserial_ports[i].port.iobase == 0) {
-				up = &nwpserial_ports[i];
-				break;
-			}
-
-	if (up == NULL) {
-		ret = -EBUSY;
-		goto out;
-	}
-
-	if (first)
-		uart_register_driver(&nwpserial_reg);
-	first = 0;
-
-	up->port.membase      = port->membase;
-	up->port.irq          = port->irq;
-	up->port.uartclk      = port->uartclk;
-	up->port.fifosize     = port->fifosize;
-	up->port.regshift     = port->regshift;
-	up->port.iotype       = port->iotype;
-	up->port.flags        = port->flags;
-	up->port.mapbase      = port->mapbase;
-	up->port.private_data = port->private_data;
-
-	if (port->dev)
-		up->port.dev = port->dev;
-
-	if (up->port.iobase != dcr_base) {
-		up->port.ops          = &nwpserial_pops;
-		up->port.fifosize     = 16;
-
-		spin_lock_init(&up->port.lock);
-
-		up->port.iobase = dcr_base;
-		dcr_len = dcr_resource_len(dn, 0);
-
-		up->dcr_host = dcr_map(dn, dcr_base, dcr_len);
-		if (!DCR_MAP_OK(up->dcr_host)) {
-			printk(KERN_ERR "Cannot map DCR resources for NWPSERIAL");
-			goto out;
-		}
-	}
-
-	ret = uart_add_one_port(&nwpserial_reg, &up->port);
-	if (ret == 0)
-		ret = up->port.line;
-
-out:
-	mutex_unlock(&nwpserial_mutex);
-
-	return ret;
-}
-EXPORT_SYMBOL(nwpserial_register_port);
-
-void nwpserial_unregister_port(int line)
-{
-	struct nwpserial_port *up = &nwpserial_ports[line];
-	mutex_lock(&nwpserial_mutex);
-	uart_remove_one_port(&nwpserial_reg, &up->port);
-
-	up->port.type = PORT_UNKNOWN;
-
-	mutex_unlock(&nwpserial_mutex);
-}
-EXPORT_SYMBOL(nwpserial_unregister_port);
-
-#ifdef CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL_CONSOLE
-static int __init nwpserial_console_init(void)
-{
-	struct nwpserial_port *up = NULL;
-	struct device_node *dn;
-	const char *name;
-	int dcr_base;
-	int dcr_len;
-	int i;
-
-	/* search for a free port */
-	for (i = 0; i < NWPSERIAL_NR; i++)
-		if (nwpserial_ports[i].port.type == PORT_UNKNOWN) {
-			up = &nwpserial_ports[i];
-			break;
-		}
-
-	if (up == NULL)
-		return -1;
-
-	name = of_get_property(of_chosen, "linux,stdout-path", NULL);
-	if (name == NULL)
-		return -1;
-
-	dn = of_find_node_by_path(name);
-	if (!dn)
-		return -1;
-
-	spin_lock_init(&up->port.lock);
-	up->port.ops = &nwpserial_pops;
-	up->port.type = PORT_NWPSERIAL;
-	up->port.fifosize = 16;
-
-	dcr_base = dcr_resource_start(dn, 0);
-	dcr_len = dcr_resource_len(dn, 0);
-	up->port.iobase = dcr_base;
-
-	up->dcr_host = dcr_map(dn, dcr_base, dcr_len);
-	if (!DCR_MAP_OK(up->dcr_host)) {
-		printk("Cannot map DCR resources for SERIAL");
-		return -1;
-	}
-	register_console(&nwpserial_console);
-	return 0;
-}
-console_initcall(nwpserial_console_init);
-#endif /* CONFIG_SERIAL_OF_PLATFORM_NWPSERIAL_CONSOLE */
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index 9d4c84f7485f..fa49eb1e2fa2 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -1165,7 +1165,7 @@ serial_omap_type(struct uart_port *port)
 
 #define BOTH_EMPTY (UART_LSR_TEMT | UART_LSR_THRE)
 
-static inline void wait_for_xmitr(struct uart_omap_port *up)
+static void __maybe_unused wait_for_xmitr(struct uart_omap_port *up)
 {
 	unsigned int status, tmout = 10000;
 
@@ -1343,7 +1343,7 @@ static inline void serial_omap_add_console_port(struct uart_omap_port *up)
 
 /* Enable or disable the rs485 support */
 static int
-serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
+serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485)
 {
 	struct uart_omap_port *up = to_uart_omap_port(port);
 	unsigned int mode;
@@ -1356,8 +1356,12 @@ serial_omap_config_rs485(struct uart_port *port, struct serial_rs485 *rs485conf)
 	up->ier = 0;
 	serial_out(up, UART_IER, 0);
 
+	/* Clamp the delays to [0, 100ms] */
+	rs485->delay_rts_before_send = min(rs485->delay_rts_before_send, 100U);
+	rs485->delay_rts_after_send  = min(rs485->delay_rts_after_send, 100U);
+
 	/* store new config */
-	port->rs485 = *rs485conf;
+	port->rs485 = *rs485;
 
 	/*
 	 * Just as a precaution, only allow rs485
diff --git a/drivers/tty/serial/pxa.c b/drivers/tty/serial/pxa.c
index 9becba654892..41eab75ba2af 100644
--- a/drivers/tty/serial/pxa.c
+++ b/drivers/tty/serial/pxa.c
@@ -603,7 +603,7 @@ static struct uart_driver serial_pxa_reg;
 /*
  *	Wait for transmitter & holding register to empty
  */
-static inline void wait_for_xmitr(struct uart_pxa_port *up)
+static void wait_for_xmitr(struct uart_pxa_port *up)
 {
 	unsigned int status, tmout = 10000;
 
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
index edb5305b9d4d..13f8d5f70272 100644
--- a/drivers/tty/serial/sc16is7xx.c
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -389,6 +389,13 @@ static void sc16is7xx_fifo_write(struct uart_port *port, u8 to_send)
 	const u8 line = sc16is7xx_line(port);
 	u8 addr = (SC16IS7XX_THR_REG << SC16IS7XX_REG_SHIFT) | line;
 
+	/*
+	 * Don't send zero-length data, at least on SPI it confuses the chip
+	 * delivering wrong TXLVL data.
+	 */
+	if (unlikely(!to_send))
+		return;
+
 	regcache_cache_bypass(s->regmap, true);
 	regmap_raw_write(s->regmap, addr, s->buf, to_send);
 	regcache_cache_bypass(s->regmap, false);
@@ -630,6 +637,12 @@ static void sc16is7xx_handle_tx(struct uart_port *port)
 	if (likely(to_send)) {
 		/* Limit to size of TX FIFO */
 		txlen = sc16is7xx_port_read(port, SC16IS7XX_TXLVL_REG);
+		if (txlen > SC16IS7XX_FIFO_SIZE) {
+			dev_err_ratelimited(port->dev,
+				"chip reports %d free bytes in TX fifo, but it only has %d",
+				txlen, SC16IS7XX_FIFO_SIZE);
+			txlen = 0;
+		}
 		to_send = (to_send > txlen) ? txlen : to_send;
 
 		/* Add data to send */
@@ -1180,7 +1193,7 @@ static int sc16is7xx_probe(struct device *dev,
 	if (devtype->nr_gpio) {
 		/* Setup GPIO cotroller */
 		s->gpio.owner		 = THIS_MODULE;
-		s->gpio.dev		 = dev;
+		s->gpio.parent		 = dev;
 		s->gpio.label		 = dev_name(dev);
 		s->gpio.direction_input	 = sc16is7xx_gpio_direction_input;
 		s->gpio.get		 = sc16is7xx_gpio_get;
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index def5199ca004..b1f54ab1818c 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -110,7 +110,7 @@ static void uart_start(struct tty_struct *tty)
 	spin_unlock_irqrestore(&port->lock, flags);
 }
 
-static inline void
+static void
 uart_update_mctrl(struct uart_port *port, unsigned int set, unsigned int clear)
 {
 	unsigned long flags;
@@ -1818,8 +1818,8 @@ uart_get_console(struct uart_port *ports, int nr, struct console *co)
  *	@options: ptr for <options> field; NULL if not present (out)
  *
  *	Decodes earlycon kernel command line parameters of the form
- *	   earlycon=<name>,io|mmio|mmio32|mmio32be|mmio32native,<addr>,<options>
- *	   console=<name>,io|mmio|mmio32|mmio32be|mmio32native,<addr>,<options>
+ *	   earlycon=<name>,io|mmio|mmio16|mmio32|mmio32be|mmio32native,<addr>,<options>
+ *	   console=<name>,io|mmio|mmio16|mmio32|mmio32be|mmio32native,<addr>,<options>
  *
  *	The optional form
  *	   earlycon=<name>,0x<addr>,<options>
@@ -1834,6 +1834,9 @@ int uart_parse_earlycon(char *p, unsigned char *iotype, unsigned long *addr,
 	if (strncmp(p, "mmio,", 5) == 0) {
 		*iotype = UPIO_MEM;
 		p += 5;
+	} else if (strncmp(p, "mmio16,", 7) == 0) {
+		*iotype = UPIO_MEM16;
+		p += 7;
 	} else if (strncmp(p, "mmio32,", 7) == 0) {
 		*iotype = UPIO_MEM32;
 		p += 7;
@@ -2186,6 +2189,7 @@ uart_report_port(struct uart_driver *drv, struct uart_port *port)
 			 "I/O 0x%lx offset 0x%x", port->iobase, port->hub6);
 		break;
 	case UPIO_MEM:
+	case UPIO_MEM16:
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
 	case UPIO_AU:
@@ -2831,6 +2835,7 @@ int uart_match_port(struct uart_port *port1, struct uart_port *port2)
 		return (port1->iobase == port2->iobase) &&
 		       (port1->hub6   == port2->hub6);
 	case UPIO_MEM:
+	case UPIO_MEM16:
 	case UPIO_MEM32:
 	case UPIO_MEM32BE:
 	case UPIO_AU:
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
index 3eb57eb532f1..226ad23b136c 100644
--- a/drivers/tty/serial/serial_mctrl_gpio.c
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -193,6 +193,7 @@ struct mctrl_gpios *mctrl_gpio_init(struct uart_port *port, unsigned int idx)
 
 	return gpios;
 }
+EXPORT_SYMBOL_GPL(mctrl_gpio_init);
 
 void mctrl_gpio_free(struct device *dev, struct mctrl_gpios *gpios)
 {
@@ -247,3 +248,4 @@ void mctrl_gpio_disable_ms(struct mctrl_gpios *gpios)
 		disable_irq(gpios->irq[i]);
 	}
 }
+EXPORT_SYMBOL_GPL(mctrl_gpio_disable_ms);
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 51c7507b0444..4646a9f531ad 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -2,6 +2,7 @@
  * SuperH on-chip serial module support.  (SCI with no FIFO / with FIFO)
  *
  *  Copyright (C) 2002 - 2011  Paul Mundt
+ *  Copyright (C) 2015 Glider bvba
  *  Modified to support SH7720 SCIF. Markus Brunner, Mark Jonas (Jul 2007).
  *
  * based off of the old drivers/char/sh-sci.c by:
@@ -38,7 +39,6 @@
 #include <linux/major.h>
 #include <linux/module.h>
 #include <linux/mm.h>
-#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
@@ -76,6 +76,14 @@ enum {
 	((port)->irqs[SCIx_ERI_IRQ] &&	\
 	 ((port)->irqs[SCIx_RXI_IRQ] < 0))
 
+enum SCI_CLKS {
+	SCI_FCK,		/* Functional Clock */
+	SCI_SCK,		/* Optional External Clock */
+	SCI_BRG_INT,		/* Optional BRG Internal Clock Source */
+	SCI_SCIF_CLK,		/* Optional BRG External Clock Source */
+	SCI_NUM_CLKS
+};
+
 struct sci_port {
 	struct uart_port	port;
 
@@ -92,10 +100,9 @@ struct sci_port {
 	struct timer_list	break_timer;
 	int			break_flag;
 
-	/* Interface clock */
-	struct clk		*iclk;
-	/* Function clock */
-	struct clk		*fclk;
+	/* Clocks */
+	struct clk		*clks[SCI_NUM_CLKS];
+	unsigned long		clk_rates[SCI_NUM_CLKS];
 
 	int			irqs[SCIx_NR_IRQS];
 	char			*irqstr[SCIx_NR_IRQS];
@@ -116,8 +123,6 @@ struct sci_port {
 	struct timer_list		rx_timer;
 	unsigned int			rx_timeout;
 #endif
-
-	struct notifier_block		freq_transition;
 };
 
 #define SCI_NPORTS CONFIG_SERIAL_SH_SCI_NR_UARTS
@@ -163,6 +168,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -185,6 +192,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -206,6 +215,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= { 0x30, 16 },
 		[SCPDR]		= { 0x34, 16 },
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -227,6 +238,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= { 0x30, 16 },
 		[SCPDR]		= { 0x34, 16 },
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -249,6 +262,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -270,6 +285,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -291,6 +308,32 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
+	},
+
+	/*
+	 * Common SCIF definitions for ports with a Baud Rate Generator for
+	 * External Clock (BRG).
+	 */
+	[SCIx_SH4_SCIF_BRG_REGTYPE] = {
+		[SCSMR]		= { 0x00, 16 },
+		[SCBRR]		= { 0x04,  8 },
+		[SCSCR]		= { 0x08, 16 },
+		[SCxTDR]	= { 0x0c,  8 },
+		[SCxSR]		= { 0x10, 16 },
+		[SCxRDR]	= { 0x14,  8 },
+		[SCFCR]		= { 0x18, 16 },
+		[SCFDR]		= { 0x1c, 16 },
+		[SCTFDR]	= sci_reg_invalid,
+		[SCRFDR]	= sci_reg_invalid,
+		[SCSPTR]	= { 0x20, 16 },
+		[SCLSR]		= { 0x24, 16 },
+		[HSSRR]		= sci_reg_invalid,
+		[SCPCR]		= sci_reg_invalid,
+		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= { 0x30, 16 },
+		[SCCKS]		= { 0x34, 16 },
 	},
 
 	/*
@@ -312,6 +355,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= { 0x40, 16 },
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= { 0x30, 16 },
+		[SCCKS]		= { 0x34, 16 },
 	},
 
 	/*
@@ -334,6 +379,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -356,6 +403,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 
 	/*
@@ -378,6 +427,8 @@ static const struct plat_sci_reg sci_regmap[SCIx_NR_REGTYPES][SCIx_NR_REGS] = {
 		[HSSRR]		= sci_reg_invalid,
 		[SCPCR]		= sci_reg_invalid,
 		[SCPDR]		= sci_reg_invalid,
+		[SCDL]		= sci_reg_invalid,
+		[SCCKS]		= sci_reg_invalid,
 	},
 };
 
@@ -452,18 +503,24 @@ static int sci_probe_regmap(struct plat_sci_port *cfg)
 
 static void sci_port_enable(struct sci_port *sci_port)
 {
+	unsigned int i;
+
 	if (!sci_port->port.dev)
 		return;
 
 	pm_runtime_get_sync(sci_port->port.dev);
 
-	clk_prepare_enable(sci_port->iclk);
-	sci_port->port.uartclk = clk_get_rate(sci_port->iclk);
-	clk_prepare_enable(sci_port->fclk);
+	for (i = 0; i < SCI_NUM_CLKS; i++) {
+		clk_prepare_enable(sci_port->clks[i]);
+		sci_port->clk_rates[i] = clk_get_rate(sci_port->clks[i]);
+	}
+	sci_port->port.uartclk = sci_port->clk_rates[SCI_FCK];
 }
 
 static void sci_port_disable(struct sci_port *sci_port)
 {
+	unsigned int i;
+
 	if (!sci_port->port.dev)
 		return;
 
@@ -475,8 +532,8 @@ static void sci_port_disable(struct sci_port *sci_port)
 	del_timer_sync(&sci_port->break_timer);
 	sci_port->break_flag = 0;
 
-	clk_disable_unprepare(sci_port->fclk);
-	clk_disable_unprepare(sci_port->iclk);
+	for (i = SCI_NUM_CLKS; i-- > 0; )
+		clk_disable_unprepare(sci_port->clks[i]);
 
 	pm_runtime_put_sync(sci_port->port.dev);
 }
@@ -1606,29 +1663,6 @@ static irqreturn_t sci_mpxed_interrupt(int irq, void *ptr)
 	return ret;
 }
 
-/*
- * Here we define a transition notifier so that we can update all of our
- * ports' baud rate when the peripheral clock changes.
- */
-static int sci_notifier(struct notifier_block *self,
-			unsigned long phase, void *p)
-{
-	struct sci_port *sci_port;
-	unsigned long flags;
-
-	sci_port = container_of(self, struct sci_port, freq_transition);
-
-	if (phase == CPUFREQ_POSTCHANGE) {
-		struct uart_port *port = &sci_port->port;
-
-		spin_lock_irqsave(&port->lock, flags);
-		port->uartclk = clk_get_rate(sci_port->iclk);
-		spin_unlock_irqrestore(&port->lock, flags);
-	}
-
-	return NOTIFY_OK;
-}
-
 static const struct sci_irq_desc {
 	const char	*desc;
 	irq_handler_t	handler;
@@ -1864,90 +1898,149 @@ static void sci_shutdown(struct uart_port *port)
 	sci_free_irq(s);
 }
 
-static unsigned int sci_scbrr_calc(struct sci_port *s, unsigned int bps,
-				   unsigned long freq)
+static int sci_sck_calc(struct sci_port *s, unsigned int bps,
+			unsigned int *srr)
 {
-	if (s->sampling_rate)
-		return DIV_ROUND_CLOSEST(freq, s->sampling_rate * bps) - 1;
+	unsigned long freq = s->clk_rates[SCI_SCK];
+	unsigned int min_sr, max_sr, sr;
+	int err, min_err = INT_MAX;
+
+	if (s->sampling_rate) {
+		/* SCI(F) has a fixed sampling rate */
+		min_sr = max_sr = s->sampling_rate / 2;
+	} else {
+		/* HSCIF has a variable 1/(8..32) sampling rate */
+		min_sr = 8;
+		max_sr = 32;
+	}
+
+	for (sr = max_sr; sr >= min_sr; sr--) {
+		err = DIV_ROUND_CLOSEST(freq, sr) - bps;
+		if (abs(err) >= abs(min_err))
+			continue;
 
-	/* Warn, but use a safe default */
-	WARN_ON(1);
+		min_err = err;
+		*srr = sr - 1;
 
-	return ((freq + 16 * bps) / (32 * bps) - 1);
+		if (!err)
+			break;
+	}
+
+	dev_dbg(s->port.dev, "SCK: %u%+d bps using SR %u\n", bps, min_err,
+		*srr + 1);
+	return min_err;
 }
 
-/* calculate frame length from SMR */
-static int sci_baud_calc_frame_len(unsigned int smr_val)
+static int sci_brg_calc(struct sci_port *s, unsigned int bps,
+			unsigned long freq, unsigned int *dlr,
+			unsigned int *srr)
 {
-	int len = 10;
+	unsigned int min_sr, max_sr, sr, dl;
+	int err, min_err = INT_MAX;
 
-	if (smr_val & SCSMR_CHR)
-		len--;
-	if (smr_val & SCSMR_PE)
-		len++;
-	if (smr_val & SCSMR_STOP)
-		len++;
+	if (s->sampling_rate) {
+		/* SCIF has a fixed sampling rate */
+		min_sr = max_sr = s->sampling_rate / 2;
+	} else {
+		/* HSCIF has a variable 1/(8..32) sampling rate */
+		min_sr = 8;
+		max_sr = 32;
+	}
 
-	return len;
-}
+	for (sr = max_sr; sr >= min_sr; sr--) {
+		dl = DIV_ROUND_CLOSEST(freq, sr * bps);
+		dl = clamp(dl, 1U, 65535U);
+
+		err = DIV_ROUND_CLOSEST(freq, sr * dl) - bps;
+		if (abs(err) >= abs(min_err))
+			continue;
 
+		min_err = err;
+		*dlr = dl;
+		*srr = sr - 1;
 
-/* calculate sample rate, BRR, and clock select for HSCIF */
-static void sci_baud_calc_hscif(unsigned int bps, unsigned long freq,
-				int *brr, unsigned int *srr,
-				unsigned int *cks, int frame_len)
+		if (!err)
+			break;
+	}
+
+	dev_dbg(s->port.dev, "BRG: %u%+d bps using DL %u SR %u\n", bps,
+		min_err, *dlr, *srr + 1);
+	return min_err;
+}
+
+/* calculate sample rate, BRR, and clock select */
+static int sci_scbrr_calc(struct sci_port *s, unsigned int bps,
+			  unsigned int *brr, unsigned int *srr,
+			  unsigned int *cks)
 {
-	int sr, c, br, err, recv_margin;
-	int min_err = 1000; /* 100% */
-	int recv_max_margin = 0;
+	unsigned int min_sr, max_sr, shift, sr, br, prediv, scrate, c;
+	unsigned long freq = s->clk_rates[SCI_FCK];
+	int err, min_err = INT_MAX;
 
-	/* Find the combination of sample rate and clock select with the
-	   smallest deviation from the desired baud rate. */
-	for (sr = 8; sr <= 32; sr++) {
+	if (s->sampling_rate) {
+		min_sr = max_sr = s->sampling_rate;
+		shift = 0;
+	} else {
+		/* HSCIF has a variable sample rate */
+		min_sr = 8;
+		max_sr = 32;
+		shift = 1;
+	}
+
+	/*
+	 * Find the combination of sample rate and clock select with the
+	 * smallest deviation from the desired baud rate.
+	 * Prefer high sample rates to maximise the receive margin.
+	 *
+	 * M: Receive margin (%)
+	 * N: Ratio of bit rate to clock (N = sampling rate)
+	 * D: Clock duty (D = 0 to 1.0)
+	 * L: Frame length (L = 9 to 12)
+	 * F: Absolute value of clock frequency deviation
+	 *
+	 *  M = |(0.5 - 1 / 2 * N) - ((L - 0.5) * F) -
+	 *      (|D - 0.5| / N * (1 + F))|
+	 *  NOTE: Usually, treat D for 0.5, F is 0 by this calculation.
+	 */
+	for (sr = max_sr; sr >= min_sr; sr--) {
 		for (c = 0; c <= 3; c++) {
 			/* integerized formulas from HSCIF documentation */
-			br = DIV_ROUND_CLOSEST(freq, (sr *
-					      (1 << (2 * c + 1)) * bps)) - 1;
-			br = clamp(br, 0, 255);
-			err = DIV_ROUND_CLOSEST(freq, ((br + 1) * bps * sr *
-					       (1 << (2 * c + 1)) / 1000)) -
-					       1000;
-			/* Calc recv margin
-			 * M: Receive margin (%)
-			 * N: Ratio of bit rate to clock (N = sampling rate)
-			 * D: Clock duty (D = 0 to 1.0)
-			 * L: Frame length (L = 9 to 12)
-			 * F: Absolute value of clock frequency deviation
+			prediv = sr * (1 << (2 * c + shift));
+
+			/*
+			 * We need to calculate:
 			 *
-			 *  M = |(0.5 - 1 / 2 * N) - ((L - 0.5) * F) -
-			 *      (|D - 0.5| / N * (1 + F))|
-			 *  NOTE: Usually, treat D for 0.5, F is 0 by this
-			 *        calculation.
+			 *     br = freq / (prediv * bps) clamped to [1..256]
+			 *     err = freq / (br * prediv) - bps
+			 *
+			 * Watch out for overflow when calculating the desired
+			 * sampling clock rate!
 			 */
-			recv_margin = abs((500 -
-					DIV_ROUND_CLOSEST(1000, sr << 1)) / 10);
-			if (abs(min_err) > abs(err)) {
-				min_err = err;
-				recv_max_margin = recv_margin;
-			} else if ((min_err == err) &&
-				   (recv_margin > recv_max_margin))
-				recv_max_margin = recv_margin;
-			else
+			if (bps > UINT_MAX / prediv)
+				break;
+
+			scrate = prediv * bps;
+			br = DIV_ROUND_CLOSEST(freq, scrate);
+			br = clamp(br, 1U, 256U);
+
+			err = DIV_ROUND_CLOSEST(freq, br * prediv) - bps;
+			if (abs(err) >= abs(min_err))
 				continue;
 
-			*brr = br;
+			min_err = err;
+			*brr = br - 1;
 			*srr = sr - 1;
 			*cks = c;
+
+			if (!err)
+				goto found;
 		}
 	}
 
-	if (min_err == 1000) {
-		WARN_ON(1);
-		/* use defaults */
-		*brr = 255;
-		*srr = 15;
-		*cks = 0;
-	}
+found:
+	dev_dbg(s->port.dev, "BRR: %u%+d bps using N %u SR %u cks %u\n", bps,
+		min_err, *brr, *srr + 1, *cks);
+	return min_err;
 }
 
 static void sci_reset(struct uart_port *port)
@@ -1969,11 +2062,14 @@ static void sci_reset(struct uart_port *port)
 static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 			    struct ktermios *old)
 {
+	unsigned int baud, smr_val = 0, scr_val = 0, i;
+	unsigned int brr = 255, cks = 0, srr = 15, dl = 0, sccks = 0;
+	unsigned int brr1 = 255, cks1 = 0, srr1 = 15, dl1 = 0;
 	struct sci_port *s = to_sci_port(port);
 	const struct plat_sci_reg *reg;
-	unsigned int baud, smr_val = 0, max_baud, cks = 0;
-	int t = -1;
-	unsigned int srr = 15;
+	int min_err = INT_MAX, err;
+	unsigned long max_freq = 0;
+	int best_clk = -1;
 
 	if ((termios->c_cflag & CSIZE) == CS7)
 		smr_val |= SCSMR_CHR;
@@ -1992,41 +2088,123 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 	 * that the previous boot loader has enabled required clocks and
 	 * setup the baud rate generator hardware for us already.
 	 */
-	max_baud = port->uartclk ? port->uartclk / 16 : 115200;
-
-	baud = uart_get_baud_rate(port, termios, old, 0, max_baud);
-	if (likely(baud && port->uartclk)) {
-		if (s->cfg->type == PORT_HSCIF) {
-			int frame_len = sci_baud_calc_frame_len(smr_val);
-			sci_baud_calc_hscif(baud, port->uartclk, &t, &srr,
-					    &cks, frame_len);
-		} else {
-			t = sci_scbrr_calc(s, baud, port->uartclk);
-			for (cks = 0; t >= 256 && cks <= 3; cks++)
-				t >>= 2;
+	if (!port->uartclk) {
+		baud = uart_get_baud_rate(port, termios, old, 0, 115200);
+		goto done;
+	}
+
+	for (i = 0; i < SCI_NUM_CLKS; i++)
+		max_freq = max(max_freq, s->clk_rates[i]);
+
+	baud = uart_get_baud_rate(port, termios, old, 0,
+				  max_freq / max(s->sampling_rate, 8U));
+	if (!baud)
+		goto done;
+
+	/*
+	 * There can be multiple sources for the sampling clock.  Find the one
+	 * that gives us the smallest deviation from the desired baud rate.
+	 */
+
+	/* Optional Undivided External Clock */
+	if (s->clk_rates[SCI_SCK] && port->type != PORT_SCIFA &&
+	    port->type != PORT_SCIFB) {
+		err = sci_sck_calc(s, baud, &srr1);
+		if (abs(err) < abs(min_err)) {
+			best_clk = SCI_SCK;
+			scr_val = SCSCR_CKE1;
+			sccks = SCCKS_CKS;
+			min_err = err;
+			srr = srr1;
+			if (!err)
+				goto done;
 		}
 	}
 
+	/* Optional BRG Frequency Divided External Clock */
+	if (s->clk_rates[SCI_SCIF_CLK] && sci_getreg(port, SCDL)->size) {
+		err = sci_brg_calc(s, baud, s->clk_rates[SCI_SCIF_CLK], &dl1,
+				   &srr1);
+		if (abs(err) < abs(min_err)) {
+			best_clk = SCI_SCIF_CLK;
+			scr_val = SCSCR_CKE1;
+			sccks = 0;
+			min_err = err;
+			dl = dl1;
+			srr = srr1;
+			if (!err)
+				goto done;
+		}
+	}
+
+	/* Optional BRG Frequency Divided Internal Clock */
+	if (s->clk_rates[SCI_BRG_INT] && sci_getreg(port, SCDL)->size) {
+		err = sci_brg_calc(s, baud, s->clk_rates[SCI_BRG_INT], &dl1,
+				   &srr1);
+		if (abs(err) < abs(min_err)) {
+			best_clk = SCI_BRG_INT;
+			scr_val = SCSCR_CKE1;
+			sccks = SCCKS_XIN;
+			min_err = err;
+			dl = dl1;
+			srr = srr1;
+			if (!min_err)
+				goto done;
+		}
+	}
+
+	/* Divided Functional Clock using standard Bit Rate Register */
+	err = sci_scbrr_calc(s, baud, &brr1, &srr1, &cks1);
+	if (abs(err) < abs(min_err)) {
+		best_clk = SCI_FCK;
+		scr_val = 0;
+		min_err = err;
+		brr = brr1;
+		srr = srr1;
+		cks = cks1;
+	}
+
+done:
+	if (best_clk >= 0)
+		dev_dbg(port->dev, "Using clk %pC for %u%+d bps\n",
+			s->clks[best_clk], baud, min_err);
+
 	sci_port_enable(s);
 
-	sci_reset(port);
+	/*
+	 * Program the optional External Baud Rate Generator (BRG) first.
+	 * It controls the mux to select (H)SCK or frequency divided clock.
+	 */
+	if (best_clk >= 0 && sci_getreg(port, SCCKS)->size) {
+		serial_port_out(port, SCDL, dl);
+		serial_port_out(port, SCCKS, sccks);
+	}
 
-	smr_val |= serial_port_in(port, SCSMR) & SCSMR_CKS;
+	sci_reset(port);
 
 	uart_update_timeout(port, termios->c_cflag, baud);
 
-	dev_dbg(port->dev, "%s: SMR %x, cks %x, t %x, SCSCR %x\n",
-		__func__, smr_val, cks, t, s->cfg->scscr);
-
-	if (t >= 0) {
-		serial_port_out(port, SCSMR, (smr_val & ~SCSMR_CKS) | cks);
-		serial_port_out(port, SCBRR, t);
-		reg = sci_getreg(port, HSSRR);
-		if (reg->size)
+	if (best_clk >= 0) {
+		smr_val |= cks;
+		dev_dbg(port->dev,
+			 "SCR 0x%x SMR 0x%x BRR %u CKS 0x%x DL %u SRR %u\n",
+			 scr_val, smr_val, brr, sccks, dl, srr);
+		serial_port_out(port, SCSCR, scr_val);
+		serial_port_out(port, SCSMR, smr_val);
+		serial_port_out(port, SCBRR, brr);
+		if (sci_getreg(port, HSSRR)->size)
 			serial_port_out(port, HSSRR, srr | HSCIF_SRE);
-		udelay((1000000+(baud-1)) / baud); /* Wait one bit interval */
-	} else
+
+		/* Wait one bit interval */
+		udelay((1000000 + (baud - 1)) / baud);
+	} else {
+		/* Don't touch the bit rate configuration */
+		scr_val = s->cfg->scscr & (SCSCR_CKE1 | SCSCR_CKE0);
+		smr_val |= serial_port_in(port, SCSMR) & SCSMR_CKS;
+		dev_dbg(port->dev, "SCR 0x%x SMR 0x%x\n", scr_val, smr_val);
+		serial_port_out(port, SCSCR, scr_val);
 		serial_port_out(port, SCSMR, smr_val);
+	}
 
 	sci_init_pins(port, termios->c_cflag);
 
@@ -2051,7 +2229,9 @@ static void sci_set_termios(struct uart_port *port, struct ktermios *termios,
 		serial_port_out(port, SCFCR, ctrl);
 	}
 
-	serial_port_out(port, SCSCR, s->cfg->scscr);
+	scr_val |= s->cfg->scscr & ~(SCSCR_CKE1 | SCSCR_CKE0);
+	dev_dbg(port->dev, "SCSCR 0x%x\n", scr_val);
+	serial_port_out(port, SCSCR, scr_val);
 
 #ifdef CONFIG_SERIAL_SH_SCI_DMA
 	/*
@@ -2241,6 +2421,63 @@ static struct uart_ops sci_uart_ops = {
 #endif
 };
 
+static int sci_init_clocks(struct sci_port *sci_port, struct device *dev)
+{
+	const char *clk_names[] = {
+		[SCI_FCK] = "fck",
+		[SCI_SCK] = "sck",
+		[SCI_BRG_INT] = "brg_int",
+		[SCI_SCIF_CLK] = "scif_clk",
+	};
+	struct clk *clk;
+	unsigned int i;
+
+	if (sci_port->cfg->type == PORT_HSCIF)
+		clk_names[SCI_SCK] = "hsck";
+
+	for (i = 0; i < SCI_NUM_CLKS; i++) {
+		clk = devm_clk_get(dev, clk_names[i]);
+		if (PTR_ERR(clk) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+
+		if (IS_ERR(clk) && i == SCI_FCK) {
+			/*
+			 * "fck" used to be called "sci_ick", and we need to
+			 * maintain DT backward compatibility.
+			 */
+			clk = devm_clk_get(dev, "sci_ick");
+			if (PTR_ERR(clk) == -EPROBE_DEFER)
+				return -EPROBE_DEFER;
+
+			if (!IS_ERR(clk))
+				goto found;
+
+			/*
+			 * Not all SH platforms declare a clock lookup entry
+			 * for SCI devices, in which case we need to get the
+			 * global "peripheral_clk" clock.
+			 */
+			clk = devm_clk_get(dev, "peripheral_clk");
+			if (!IS_ERR(clk))
+				goto found;
+
+			dev_err(dev, "failed to get %s (%ld)\n", clk_names[i],
+				PTR_ERR(clk));
+			return PTR_ERR(clk);
+		}
+
+found:
+		if (IS_ERR(clk))
+			dev_dbg(dev, "failed to get %s (%ld)\n", clk_names[i],
+				PTR_ERR(clk));
+		else
+			dev_dbg(dev, "clk %s is %pC rate %pCr\n", clk_names[i],
+				clk, clk);
+		sci_port->clks[i] = IS_ERR(clk) ? NULL : clk;
+	}
+	return 0;
+}
+
 static int sci_init_single(struct platform_device *dev,
 			   struct sci_port *sci_port, unsigned int index,
 			   struct plat_sci_port *p, bool early)
@@ -2333,22 +2570,9 @@ static int sci_init_single(struct platform_device *dev,
 		sci_port->sampling_rate = p->sampling_rate;
 
 	if (!early) {
-		sci_port->iclk = clk_get(&dev->dev, "sci_ick");
-		if (IS_ERR(sci_port->iclk)) {
-			sci_port->iclk = clk_get(&dev->dev, "peripheral_clk");
-			if (IS_ERR(sci_port->iclk)) {
-				dev_err(&dev->dev, "can't get iclk\n");
-				return PTR_ERR(sci_port->iclk);
-			}
-		}
-
-		/*
-		 * The function clock is optional, ignore it if we can't
-		 * find it.
-		 */
-		sci_port->fclk = clk_get(&dev->dev, "sci_fck");
-		if (IS_ERR(sci_port->fclk))
-			sci_port->fclk = NULL;
+		ret = sci_init_clocks(sci_port, &dev->dev);
+		if (ret < 0)
+			return ret;
 
 		port->dev = &dev->dev;
 
@@ -2405,9 +2629,6 @@ static int sci_init_single(struct platform_device *dev,
 
 static void sci_cleanup_single(struct sci_port *port)
 {
-	clk_put(port->iclk);
-	clk_put(port->fclk);
-
 	pm_runtime_disable(port->port.dev);
 }
 
@@ -2426,7 +2647,7 @@ static void serial_console_write(struct console *co, const char *s,
 {
 	struct sci_port *sci_port = &sci_ports[co->index];
 	struct uart_port *port = &sci_port->port;
-	unsigned short bits, ctrl;
+	unsigned short bits, ctrl, ctrl_temp;
 	unsigned long flags;
 	int locked = 1;
 
@@ -2438,9 +2659,11 @@ static void serial_console_write(struct console *co, const char *s,
 	else
 		spin_lock(&port->lock);
 
-	/* first save the SCSCR then disable the interrupts */
+	/* first save SCSCR then disable interrupts, keep clock source */
 	ctrl = serial_port_in(port, SCSCR);
-	serial_port_out(port, SCSCR, sci_port->cfg->scscr);
+	ctrl_temp = (sci_port->cfg->scscr & ~(SCSCR_CKE1 | SCSCR_CKE0)) |
+		    (ctrl & (SCSCR_CKE1 | SCSCR_CKE0));
+	serial_port_out(port, SCSCR, ctrl_temp);
 
 	uart_console_write(port, s, count, serial_console_putchar);
 
@@ -2559,9 +2782,6 @@ static int sci_remove(struct platform_device *dev)
 {
 	struct sci_port *port = platform_get_drvdata(dev);
 
-	cpufreq_unregister_notifier(&port->freq_transition,
-				    CPUFREQ_TRANSITION_NOTIFIER);
-
 	uart_remove_one_port(&sci_uart_driver, &port->port);
 
 	sci_cleanup_single(port);
@@ -2569,42 +2789,44 @@ static int sci_remove(struct platform_device *dev)
 	return 0;
 }
 
-struct sci_port_info {
-	unsigned int type;
-	unsigned int regtype;
-};
+
+#define SCI_OF_DATA(type, regtype)	(void *)((type) << 16 | (regtype))
+#define SCI_OF_TYPE(data)		((unsigned long)(data) >> 16)
+#define SCI_OF_REGTYPE(data)		((unsigned long)(data) & 0xffff)
 
 static const struct of_device_id of_sci_match[] = {
+	/* SoC-specific types */
+	{
+		.compatible = "renesas,scif-r7s72100",
+		.data = SCI_OF_DATA(PORT_SCIF, SCIx_SH2_SCIF_FIFODATA_REGTYPE),
+	},
+	/* Family-specific types */
+	{
+		.compatible = "renesas,rcar-gen1-scif",
+		.data = SCI_OF_DATA(PORT_SCIF, SCIx_SH4_SCIF_BRG_REGTYPE),
+	}, {
+		.compatible = "renesas,rcar-gen2-scif",
+		.data = SCI_OF_DATA(PORT_SCIF, SCIx_SH4_SCIF_BRG_REGTYPE),
+	}, {
+		.compatible = "renesas,rcar-gen3-scif",
+		.data = SCI_OF_DATA(PORT_SCIF, SCIx_SH4_SCIF_BRG_REGTYPE),
+	},
+	/* Generic types */
 	{
 		.compatible = "renesas,scif",
-		.data = &(const struct sci_port_info) {
-			.type = PORT_SCIF,
-			.regtype = SCIx_SH4_SCIF_REGTYPE,
-		},
+		.data = SCI_OF_DATA(PORT_SCIF, SCIx_SH4_SCIF_REGTYPE),
 	}, {
 		.compatible = "renesas,scifa",
-		.data = &(const struct sci_port_info) {
-			.type = PORT_SCIFA,
-			.regtype = SCIx_SCIFA_REGTYPE,
-		},
+		.data = SCI_OF_DATA(PORT_SCIFA, SCIx_SCIFA_REGTYPE),
 	}, {
 		.compatible = "renesas,scifb",
-		.data = &(const struct sci_port_info) {
-			.type = PORT_SCIFB,
-			.regtype = SCIx_SCIFB_REGTYPE,
-		},
+		.data = SCI_OF_DATA(PORT_SCIFB, SCIx_SCIFB_REGTYPE),
 	}, {
 		.compatible = "renesas,hscif",
-		.data = &(const struct sci_port_info) {
-			.type = PORT_HSCIF,
-			.regtype = SCIx_HSCIF_REGTYPE,
-		},
+		.data = SCI_OF_DATA(PORT_HSCIF, SCIx_HSCIF_REGTYPE),
 	}, {
 		.compatible = "renesas,sci",
-		.data = &(const struct sci_port_info) {
-			.type = PORT_SCI,
-			.regtype = SCIx_SCI_REGTYPE,
-		},
+		.data = SCI_OF_DATA(PORT_SCI, SCIx_SCI_REGTYPE),
 	}, {
 		/* Terminator */
 	},
@@ -2616,24 +2838,21 @@ sci_parse_dt(struct platform_device *pdev, unsigned int *dev_id)
 {
 	struct device_node *np = pdev->dev.of_node;
 	const struct of_device_id *match;
-	const struct sci_port_info *info;
 	struct plat_sci_port *p;
 	int id;
 
 	if (!IS_ENABLED(CONFIG_OF) || !np)
 		return NULL;
 
-	match = of_match_node(of_sci_match, pdev->dev.of_node);
+	match = of_match_node(of_sci_match, np);
 	if (!match)
 		return NULL;
 
-	info = match->data;
-
 	p = devm_kzalloc(&pdev->dev, sizeof(struct plat_sci_port), GFP_KERNEL);
 	if (!p)
 		return NULL;
 
-	/* Get the line number for the aliases node. */
+	/* Get the line number from the aliases node. */
 	id = of_alias_get_id(np, "serial");
 	if (id < 0) {
 		dev_err(&pdev->dev, "failed to get alias id (%d)\n", id);
@@ -2643,8 +2862,8 @@ sci_parse_dt(struct platform_device *pdev, unsigned int *dev_id)
 	*dev_id = id;
 
 	p->flags = UPF_IOREMAP | UPF_BOOT_AUTOCONF;
-	p->type = info->type;
-	p->regtype = info->regtype;
+	p->type = SCI_OF_TYPE(match->data);
+	p->regtype = SCI_OF_REGTYPE(match->data);
 	p->scscr = SCSCR_RE | SCSCR_TE;
 
 	return p;
@@ -2714,16 +2933,6 @@ static int sci_probe(struct platform_device *dev)
 	if (ret)
 		return ret;
 
-	sp->freq_transition.notifier_call = sci_notifier;
-
-	ret = cpufreq_register_notifier(&sp->freq_transition,
-					CPUFREQ_TRANSITION_NOTIFIER);
-	if (unlikely(ret < 0)) {
-		uart_remove_one_port(&sci_uart_driver, &sp->port);
-		sci_cleanup_single(sp);
-		return ret;
-	}
-
 #ifdef CONFIG_SH_STANDARD_BIOS
 	sh_bios_gdb_detach();
 #endif
diff --git a/drivers/tty/serial/sh-sci.h b/drivers/tty/serial/sh-sci.h
index bf69bbdcc1f9..fb1760250421 100644
--- a/drivers/tty/serial/sh-sci.h
+++ b/drivers/tty/serial/sh-sci.h
@@ -27,6 +27,8 @@ enum {
 	HSSRR,				/* Sampling Rate Register */
 	SCPCR,				/* Serial Port Control Register */
 	SCPDR,				/* Serial Port Data Register */
+	SCDL,				/* BRG Frequency Division Register */
+	SCCKS,				/* BRG Clock Select Register */
 
 	SCIx_NR_REGS,
 };
@@ -109,6 +111,14 @@ enum {
 #define SCPDR_RTSD	BIT(4)	/* Serial Port RTS Output Pin Data */
 #define SCPDR_CTSD	BIT(3)	/* Serial Port CTS Input Pin Data */
 
+/*
+ * BRG Clock Select Register (Some SCIF and HSCIF)
+ * The Baud Rate Generator for external clock can provide a clock source for
+ * the sampling clock. It outputs either its frequency divided clock, or the
+ * (undivided) (H)SCK external clock.
+ */
+#define SCCKS_CKS	BIT(15)	/* Select (H)SCK (1) or divided SC_CLK (0) */
+#define SCCKS_XIN	BIT(14)	/* SC_CLK uses bus clock (1) or SCIF_CLK (0) */
 
 #define SCxSR_TEND(port)	(((port)->type == PORT_SCI) ? SCI_TEND   : SCIF_TEND)
 #define SCxSR_RDxF(port)	(((port)->type == PORT_SCI) ? SCI_RDRF   : SCIF_RDF)
diff --git a/drivers/tty/serial/sprd_serial.c b/drivers/tty/serial/sprd_serial.c
index 9dbae01d41ce..ef26c4a60be4 100644
--- a/drivers/tty/serial/sprd_serial.c
+++ b/drivers/tty/serial/sprd_serial.c
@@ -517,7 +517,7 @@ static struct uart_ops serial_sprd_ops = {
 };
 
 #ifdef CONFIG_SERIAL_SPRD_CONSOLE
-static inline void wait_for_xmitr(struct uart_port *port)
+static void wait_for_xmitr(struct uart_port *port)
 {
 	unsigned int status, tmout = 10000;
 
diff --git a/drivers/tty/serial/sunsu.c b/drivers/tty/serial/sunsu.c
index e124d2e88996..9ad98eaa35bf 100644
--- a/drivers/tty/serial/sunsu.c
+++ b/drivers/tty/serial/sunsu.c
@@ -1262,7 +1262,7 @@ static int sunsu_kbd_ms_init(struct uart_sunsu_port *up)
 /*
  *	Wait for transmitter & holding register to empty
  */
-static __inline__ void wait_for_xmitr(struct uart_sunsu_port *up)
+static void wait_for_xmitr(struct uart_sunsu_port *up)
 {
 	unsigned int status, tmout = 10000;
 
diff --git a/drivers/tty/serial/ucc_uart.c b/drivers/tty/serial/ucc_uart.c
index 73190f5d2832..1a7dc3c590b1 100644
--- a/drivers/tty/serial/ucc_uart.c
+++ b/drivers/tty/serial/ucc_uart.c
@@ -31,7 +31,7 @@
 #include <linux/dma-mapping.h>
 
 #include <linux/fs_uart_pd.h>
-#include <asm/ucc_slow.h>
+#include <soc/fsl/qe/ucc_slow.h>
 
 #include <linux/firmware.h>
 #include <asm/reg.h>
diff --git a/drivers/tty/serial/vt8500_serial.c b/drivers/tty/serial/vt8500_serial.c
index 4079ec56f5f9..b384060e3b1f 100644
--- a/drivers/tty/serial/vt8500_serial.c
+++ b/drivers/tty/serial/vt8500_serial.c
@@ -485,7 +485,7 @@ static struct uart_driver vt8500_uart_driver;
 
 #ifdef CONFIG_SERIAL_VT8500_CONSOLE
 
-static inline void wait_for_xmitr(struct uart_port *port)
+static void wait_for_xmitr(struct uart_port *port)
 {
 	unsigned int status, tmout = 10000;
 
diff --git a/drivers/tty/synclink_gt.c b/drivers/tty/synclink_gt.c
index 6fc39fbfc275..5505ea842179 100644
--- a/drivers/tty/synclink_gt.c
+++ b/drivers/tty/synclink_gt.c
@@ -89,7 +89,7 @@
  * module identification
  */
 static char *driver_name     = "SyncLink GT";
-static char *tty_driver_name = "synclink_gt";
+static char *slgt_driver_name = "synclink_gt";
 static char *tty_dev_prefix  = "ttySLG";
 MODULE_LICENSE("GPL");
 #define MGSL_MAGIC 0x5401
@@ -3799,7 +3799,7 @@ static int __init slgt_init(void)
 
 	/* Initialize the tty_driver structure */
 
-	serial_driver->driver_name = tty_driver_name;
+	serial_driver->driver_name = slgt_driver_name;
 	serial_driver->name = tty_dev_prefix;
 	serial_driver->major = ttymajor;
 	serial_driver->minor_start = 64;
diff --git a/drivers/tty/sysrq.c b/drivers/tty/sysrq.c
index 5381a728d23e..e5139402e7f8 100644
--- a/drivers/tty/sysrq.c
+++ b/drivers/tty/sysrq.c
@@ -133,6 +133,12 @@ static void sysrq_handle_crash(int key)
 {
 	char *killer = NULL;
 
+	/* we need to release the RCU read lock here,
+	 * otherwise we get an annoying
+	 * 'BUG: sleeping function called from invalid context'
+	 * complaint from the kernel before the panic.
+	 */
+	rcu_read_unlock();
 	panic_on_oops = 1;	/* force panic */
 	wmb();
 	*killer = 1;
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index bcc8e1e8bb72..a7eacef1bd22 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -256,19 +256,24 @@ const char *tty_name(const struct tty_struct *tty)
 
 EXPORT_SYMBOL(tty_name);
 
-int tty_paranoia_check(struct tty_struct *tty, struct inode *inode,
+const char *tty_driver_name(const struct tty_struct *tty)
+{
+	if (!tty || !tty->driver)
+		return "";
+	return tty->driver->name;
+}
+
+static int tty_paranoia_check(struct tty_struct *tty, struct inode *inode,
 			      const char *routine)
 {
 #ifdef TTY_PARANOIA_CHECK
 	if (!tty) {
-		printk(KERN_WARNING
-			"null TTY for (%d:%d) in %s\n",
+		pr_warn("(%d:%d): %s: NULL tty\n",
 			imajor(inode), iminor(inode), routine);
 		return 1;
 	}
 	if (tty->magic != TTY_MAGIC) {
-		printk(KERN_WARNING
-			"bad magic number for tty struct (%d:%d) in %s\n",
+		pr_warn("(%d:%d): %s: bad magic number\n",
 			imajor(inode), iminor(inode), routine);
 		return 1;
 	}
@@ -293,9 +298,8 @@ static int check_tty_count(struct tty_struct *tty, const char *routine)
 	    tty->link && tty->link->count)
 		count++;
 	if (tty->count != count) {
-		printk(KERN_WARNING "Warning: dev (%s) tty->count(%d) "
-				    "!= #fd's(%d) in %s\n",
-		       tty->name, tty->count, count, routine);
+		tty_warn(tty, "%s: tty->count(%d) != #fd's(%d)\n",
+			 routine, tty->count, count);
 		return count;
 	}
 #endif
@@ -420,10 +424,8 @@ int __tty_check_change(struct tty_struct *tty, int sig)
 	}
 	rcu_read_unlock();
 
-	if (!tty_pgrp) {
-		pr_warn("%s: tty_check_change: sig=%d, tty->pgrp == NULL!\n",
-			tty_name(tty), sig);
-	}
+	if (!tty_pgrp)
+		tty_warn(tty, "sig=%d, tty->pgrp == NULL!\n", sig);
 
 	return ret;
 }
@@ -781,7 +783,7 @@ static void do_tty_hangup(struct work_struct *work)
 
 void tty_hangup(struct tty_struct *tty)
 {
-	tty_debug_hangup(tty, "\n");
+	tty_debug_hangup(tty, "hangup\n");
 	schedule_work(&tty->hangup_work);
 }
 
@@ -798,7 +800,7 @@ EXPORT_SYMBOL(tty_hangup);
 
 void tty_vhangup(struct tty_struct *tty)
 {
-	tty_debug_hangup(tty, "\n");
+	tty_debug_hangup(tty, "vhangup\n");
 	__tty_hangup(tty, 0);
 }
 
@@ -835,7 +837,7 @@ void tty_vhangup_self(void)
 
 static void tty_vhangup_session(struct tty_struct *tty)
 {
-	tty_debug_hangup(tty, "\n");
+	tty_debug_hangup(tty, "session hangup\n");
 	__tty_hangup(tty, 1);
 }
 
@@ -1239,8 +1241,7 @@ static ssize_t tty_write(struct file *file, const char __user *buf,
 			return -EIO;
 	/* Short term debug to catch buggy drivers */
 	if (tty->ops->write_room == NULL)
-		printk(KERN_ERR "tty driver %s lacks a write_room method.\n",
-			tty->driver->name);
+		tty_err(tty, "missing write_room method\n");
 	ld = tty_ldisc_ref_wait(tty);
 	if (!ld->ops->write)
 		ret = -EIO;
@@ -1462,13 +1463,13 @@ static int tty_reopen(struct tty_struct *tty)
 {
 	struct tty_driver *driver = tty->driver;
 
-	if (!tty->count)
-		return -EIO;
-
 	if (driver->type == TTY_DRIVER_TYPE_PTY &&
 	    driver->subtype == PTY_TYPE_MASTER)
 		return -EIO;
 
+	if (!tty->count)
+		return -EAGAIN;
+
 	if (test_bit(TTY_EXCLUSIVE, &tty->flags) && !capable(CAP_SYS_ADMIN))
 		return -EBUSY;
 
@@ -1561,8 +1562,8 @@ err_module_put:
 	/* call the tty release_tty routine to clean out this slot */
 err_release_tty:
 	tty_unlock(tty);
-	printk_ratelimited(KERN_INFO "tty_init_dev: ldisc open failed, "
-				 "clearing slot %d\n", idx);
+	tty_info_ratelimited(tty, "ldisc open failed (%d), clearing slot %d\n",
+			     retval, idx);
 	release_tty(tty, idx);
 	return ERR_PTR(retval);
 }
@@ -1580,10 +1581,8 @@ void tty_free_termios(struct tty_struct *tty)
 	tp = tty->driver->termios[idx];
 	if (tp == NULL) {
 		tp = kmalloc(sizeof(struct ktermios), GFP_KERNEL);
-		if (tp == NULL) {
-			pr_warn("tty: no memory to save termios state.\n");
+		if (tp == NULL)
 			return;
-		}
 		tty->driver->termios[idx] = tp;
 	}
 	*tp = tty->termios;
@@ -1788,7 +1787,7 @@ int tty_release(struct inode *inode, struct file *filp)
 		return 0;
 	}
 
-	tty_debug_hangup(tty, "(tty count=%d)...\n", tty->count);
+	tty_debug_hangup(tty, "releasing (count=%d)\n", tty->count);
 
 	if (tty->ops->close)
 		tty->ops->close(tty, filp);
@@ -1837,8 +1836,7 @@ int tty_release(struct inode *inode, struct file *filp)
 
 		if (once) {
 			once = 0;
-			printk(KERN_WARNING "%s: %s: read/write wait queue active!\n",
-			       __func__, tty_name(tty));
+			tty_warn(tty, "read/write wait queue active!\n");
 		}
 		schedule_timeout_killable(timeout);
 		if (timeout < 120 * HZ)
@@ -1849,14 +1847,12 @@ int tty_release(struct inode *inode, struct file *filp)
 
 	if (o_tty) {
 		if (--o_tty->count < 0) {
-			printk(KERN_WARNING "%s: bad pty slave count (%d) for %s\n",
-				__func__, o_tty->count, tty_name(o_tty));
+			tty_warn(tty, "bad slave count (%d)\n", o_tty->count);
 			o_tty->count = 0;
 		}
 	}
 	if (--tty->count < 0) {
-		printk(KERN_WARNING "%s: bad tty->count (%d) for %s\n",
-				__func__, tty->count, tty_name(tty));
+		tty_warn(tty, "bad tty->count (%d)\n", tty->count);
 		tty->count = 0;
 	}
 
@@ -1907,7 +1903,7 @@ int tty_release(struct inode *inode, struct file *filp)
 	/* Wait for pending work before tty destruction commmences */
 	tty_flush_works(tty);
 
-	tty_debug_hangup(tty, "freeing structure...\n");
+	tty_debug_hangup(tty, "freeing structure\n");
 	/*
 	 * The release_tty function takes care of the details of clearing
 	 * the slots and preserving the termios structure. The tty_unlock_pair
@@ -2069,9 +2065,13 @@ retry_open:
 
 		if (tty) {
 			mutex_unlock(&tty_mutex);
-			tty_lock(tty);
-			/* safe to drop the kref from tty_driver_lookup_tty() */
-			tty_kref_put(tty);
+			retval = tty_lock_interruptible(tty);
+			tty_kref_put(tty);  /* drop kref from tty_driver_lookup_tty() */
+			if (retval) {
+				if (retval == -EINTR)
+					retval = -ERESTARTSYS;
+				goto err_unref;
+			}
 			retval = tty_reopen(tty);
 			if (retval < 0) {
 				tty_unlock(tty);
@@ -2087,7 +2087,11 @@ retry_open:
 
 	if (IS_ERR(tty)) {
 		retval = PTR_ERR(tty);
-		goto err_file;
+		if (retval != -EAGAIN || signal_pending(current))
+			goto err_file;
+		tty_free_file(filp);
+		schedule();
+		goto retry_open;
 	}
 
 	tty_add_file(tty, filp);
@@ -2097,7 +2101,7 @@ retry_open:
 	    tty->driver->subtype == PTY_TYPE_MASTER)
 		noctty = 1;
 
-	tty_debug_hangup(tty, "(tty count=%d)\n", tty->count);
+	tty_debug_hangup(tty, "opening (count=%d)\n", tty->count);
 
 	if (tty->ops->open)
 		retval = tty->ops->open(tty, filp);
@@ -2106,7 +2110,7 @@ retry_open:
 	filp->f_flags = saved_flags;
 
 	if (retval) {
-		tty_debug_hangup(tty, "error %d, releasing...\n", retval);
+		tty_debug_hangup(tty, "open error %d, releasing\n", retval);
 
 		tty_unlock(tty); /* need to call tty_release without BTM */
 		tty_release(inode, filp);
@@ -2156,6 +2160,7 @@ retry_open:
 	return 0;
 err_unlock:
 	mutex_unlock(&tty_mutex);
+err_unref:
 	/* after locks to avoid deadlock */
 	if (!IS_ERR_OR_NULL(driver))
 		tty_driver_kref_put(driver);
@@ -2653,6 +2658,28 @@ static int tiocsetd(struct tty_struct *tty, int __user *p)
 }
 
 /**
+ *	tiocgetd	-	get line discipline
+ *	@tty: tty device
+ *	@p: pointer to user data
+ *
+ *	Retrieves the line discipline id directly from the ldisc.
+ *
+ *	Locking: waits for ldisc reference (in case the line discipline
+ *		is changing or the tty is being hungup)
+ */
+
+static int tiocgetd(struct tty_struct *tty, int __user *p)
+{
+	struct tty_ldisc *ld;
+	int ret;
+
+	ld = tty_ldisc_ref_wait(tty);
+	ret = put_user(ld->ops->num, p);
+	tty_ldisc_deref(ld);
+	return ret;
+}
+
+/**
  *	send_break	-	performed time break
  *	@tty: device to break on
  *	@duration: timeout in mS
@@ -2870,7 +2897,7 @@ long tty_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		no_tty();
 		return 0;
 	case TIOCSCTTY:
-		return tiocsctty(tty, file, arg);
+		return tiocsctty(real_tty, file, arg);
 	case TIOCGPGRP:
 		return tiocgpgrp(tty, real_tty, p);
 	case TIOCSPGRP:
@@ -2878,7 +2905,7 @@ long tty_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case TIOCGSID:
 		return tiocgsid(tty, real_tty, p);
 	case TIOCGETD:
-		return put_user(tty->ldisc->ops->num, (int __user *)p);
+		return tiocgetd(tty, p);
 	case TIOCSETD:
 		return tiocsetd(tty, p);
 	case TIOCVHANGUP:
@@ -3028,28 +3055,24 @@ void __do_SAK(struct tty_struct *tty)
 	read_lock(&tasklist_lock);
 	/* Kill the entire session */
 	do_each_pid_task(session, PIDTYPE_SID, p) {
-		printk(KERN_NOTICE "SAK: killed process %d"
-			" (%s): task_session(p)==tty->session\n",
-			task_pid_nr(p), p->comm);
+		tty_notice(tty, "SAK: killed process %d (%s): by session\n",
+			   task_pid_nr(p), p->comm);
 		send_sig(SIGKILL, p, 1);
 	} while_each_pid_task(session, PIDTYPE_SID, p);
-	/* Now kill any processes that happen to have the
-	 * tty open.
-	 */
+
+	/* Now kill any processes that happen to have the tty open */
 	do_each_thread(g, p) {
 		if (p->signal->tty == tty) {
-			printk(KERN_NOTICE "SAK: killed process %d"
-			    " (%s): task_session(p)==tty->session\n",
-			    task_pid_nr(p), p->comm);
+			tty_notice(tty, "SAK: killed process %d (%s): by controlling tty\n",
+				   task_pid_nr(p), p->comm);
 			send_sig(SIGKILL, p, 1);
 			continue;
 		}
 		task_lock(p);
 		i = iterate_fd(p->files, 0, this_tty, tty);
 		if (i != 0) {
-			printk(KERN_NOTICE "SAK: killed process %d"
-			    " (%s): fd#%d opened to the tty\n",
-				    task_pid_nr(p), p->comm, i - 1);
+			tty_notice(tty, "SAK: killed process %d (%s): by fd#%d\n",
+				   task_pid_nr(p), p->comm, i - 1);
 			force_sig(SIGKILL, p);
 		}
 		task_unlock(p);
@@ -3219,7 +3242,7 @@ EXPORT_SYMBOL(tty_register_device);
 
 static void tty_device_create_release(struct device *dev)
 {
-	pr_debug("device: '%s': %s\n", dev_name(dev), __func__);
+	dev_dbg(dev, "releasing...\n");
 	kfree(dev);
 }
 
@@ -3255,8 +3278,8 @@ struct device *tty_register_device_attr(struct tty_driver *driver,
 	bool cdev = false;
 
 	if (index >= driver->num) {
-		printk(KERN_ERR "Attempt to register invalid tty line number "
-		       " (%d).\n", index);
+		pr_err("%s: Attempt to register invalid tty line number (%d)\n",
+		       driver->name, index);
 		return ERR_PTR(-EINVAL);
 	}
 
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index 1445dd39aa62..0ea351388724 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -216,7 +216,7 @@ int tty_unthrottle_safe(struct tty_struct *tty)
 
 void tty_wait_until_sent(struct tty_struct *tty, long timeout)
 {
-	tty_debug_wait_until_sent(tty, "\n");
+	tty_debug_wait_until_sent(tty, "wait until sent, timeout=%ld\n", timeout);
 
 	if (!timeout)
 		timeout = MAX_SCHEDULE_TIMEOUT;
@@ -239,19 +239,14 @@ EXPORT_SYMBOL(tty_wait_until_sent);
  *		Termios Helper Methods
  */
 
-static void unset_locked_termios(struct ktermios *termios,
-				 struct ktermios *old,
-				 struct ktermios *locked)
+static void unset_locked_termios(struct tty_struct *tty, struct ktermios *old)
 {
+	struct ktermios *termios = &tty->termios;
+	struct ktermios *locked  = &tty->termios_locked;
 	int	i;
 
 #define NOSET_MASK(x, y, z) (x = ((x) & ~(z)) | ((y) & (z)))
 
-	if (!locked) {
-		printk(KERN_WARNING "Warning?!? termios_locked is NULL.\n");
-		return;
-	}
-
 	NOSET_MASK(termios->c_iflag, old->c_iflag, locked->c_iflag);
 	NOSET_MASK(termios->c_oflag, old->c_oflag, locked->c_oflag);
 	NOSET_MASK(termios->c_cflag, old->c_cflag, locked->c_cflag);
@@ -463,10 +458,8 @@ void tty_termios_encode_baud_rate(struct ktermios *termios,
 	if (ifound == -1 && (ibaud != obaud || ibinput))
 		termios->c_cflag |= (BOTHER << IBSHIFT);
 #else
-	if (ifound == -1 || ofound == -1) {
-		printk_once(KERN_WARNING "tty: Unable to return correct "
-			  "speed data as your architecture needs updating.\n");
-	}
+	if (ifound == -1 || ofound == -1)
+		pr_warn_once("tty: Unable to return correct speed data as your architecture needs updating.\n");
 #endif
 }
 EXPORT_SYMBOL_GPL(tty_termios_encode_baud_rate);
@@ -556,7 +549,7 @@ int tty_set_termios(struct tty_struct *tty, struct ktermios *new_termios)
 	down_write(&tty->termios_rwsem);
 	old_termios = tty->termios;
 	tty->termios = *new_termios;
-	unset_locked_termios(&tty->termios, &old_termios, &tty->termios_locked);
+	unset_locked_termios(tty, &old_termios);
 
 	if (tty->ops->set_termios)
 		tty->ops->set_termios(tty, &old_termios);
diff --git a/drivers/tty/tty_ldisc.c b/drivers/tty/tty_ldisc.c
index 629e3c865072..a054d03c22e7 100644
--- a/drivers/tty/tty_ldisc.c
+++ b/drivers/tty/tty_ldisc.c
@@ -185,7 +185,7 @@ static struct tty_ldisc *tty_ldisc_get(struct tty_struct *tty, int disc)
  *
  *	Complement of tty_ldisc_get().
  */
-static inline void tty_ldisc_put(struct tty_ldisc *ld)
+static void tty_ldisc_put(struct tty_ldisc *ld)
 {
 	if (WARN_ON_ONCE(!ld))
 		return;
@@ -417,6 +417,10 @@ EXPORT_SYMBOL_GPL(tty_ldisc_flush);
  *	they are not on hot paths so a little discipline won't do
  *	any harm.
  *
+ *	The line discipline-related tty_struct fields are reset to
+ *	prevent the ldisc driver from re-using stale information for
+ *	the new ldisc instance.
+ *
  *	Locking: takes termios_rwsem
  */
 
@@ -425,6 +429,9 @@ static void tty_set_termios_ldisc(struct tty_struct *tty, int num)
 	down_write(&tty->termios_rwsem);
 	tty->termios.c_line = num;
 	up_write(&tty->termios_rwsem);
+
+	tty->disc_data = NULL;
+	tty->receive_room = 0;
 }
 
 /**
@@ -529,34 +536,21 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 
 	tty_lock(tty);
 	retval = tty_ldisc_lock(tty, 5 * HZ);
-	if (retval) {
-		tty_ldisc_put(new_ldisc);
-		tty_unlock(tty);
-		return retval;
-	}
+	if (retval)
+		goto err;
 
-	/*
-	 *	Check the no-op case
-	 */
+	/* Check the no-op case */
+	if (tty->ldisc->ops->num == ldisc)
+		goto out;
 
-	if (tty->ldisc->ops->num == ldisc) {
-		tty_ldisc_unlock(tty);
-		tty_ldisc_put(new_ldisc);
-		tty_unlock(tty);
-		return 0;
+	if (test_bit(TTY_HUPPED, &tty->flags)) {
+		/* We were raced by hangup */
+		retval = -EIO;
+		goto out;
 	}
 
 	old_ldisc = tty->ldisc;
 
-	if (test_bit(TTY_HUPPED, &tty->flags)) {
-		/* We were raced by the hangup method. It will have stomped
-		   the ldisc data and closed the ldisc down */
-		tty_ldisc_unlock(tty);
-		tty_ldisc_put(new_ldisc);
-		tty_unlock(tty);
-		return -EIO;
-	}
-
 	/* Shutdown the old discipline. */
 	tty_ldisc_close(tty, old_ldisc);
 
@@ -582,18 +576,15 @@ int tty_set_ldisc(struct tty_struct *tty, int ldisc)
 	   the old ldisc (if it was restored as part of error cleanup
 	   above). In either case, releasing a single reference from
 	   the old ldisc is correct. */
-
-	tty_ldisc_put(old_ldisc);
-
-	/*
-	 *	Allow ldisc referencing to occur again
-	 */
+	new_ldisc = old_ldisc;
+out:
 	tty_ldisc_unlock(tty);
 
 	/* Restart the work queue in case no characters kick it off. Safe if
 	   already running */
 	tty_buffer_restart_work(tty->port);
-
+err:
+	tty_ldisc_put(new_ldisc);	/* drop the extra reference */
 	tty_unlock(tty);
 	return retval;
 }
diff --git a/drivers/tty/tty_ldsem.c b/drivers/tty/tty_ldsem.c
index ad7eba5ca380..1bf8ed13f827 100644
--- a/drivers/tty/tty_ldsem.c
+++ b/drivers/tty/tty_ldsem.c
@@ -319,7 +319,7 @@ down_write_failed(struct ld_semaphore *sem, long count, long timeout)
 
 
 
-static inline int __ldsem_down_read_nested(struct ld_semaphore *sem,
+static int __ldsem_down_read_nested(struct ld_semaphore *sem,
 					   int subclass, long timeout)
 {
 	long count;
@@ -338,7 +338,7 @@ static inline int __ldsem_down_read_nested(struct ld_semaphore *sem,
 	return 1;
 }
 
-static inline int __ldsem_down_write_nested(struct ld_semaphore *sem,
+static int __ldsem_down_write_nested(struct ld_semaphore *sem,
 					    int subclass, long timeout)
 {
 	long count;
diff --git a/drivers/tty/tty_mutex.c b/drivers/tty/tty_mutex.c
index 0efcf713b756..dfa9ec03fa8e 100644
--- a/drivers/tty/tty_mutex.c
+++ b/drivers/tty/tty_mutex.c
@@ -12,23 +12,30 @@
 
 void __lockfunc tty_lock(struct tty_struct *tty)
 {
-	if (tty->magic != TTY_MAGIC) {
-		pr_err("L Bad %p\n", tty);
-		WARN_ON(1);
+	if (WARN(tty->magic != TTY_MAGIC, "L Bad %p\n", tty))
 		return;
-	}
 	tty_kref_get(tty);
 	mutex_lock(&tty->legacy_mutex);
 }
 EXPORT_SYMBOL(tty_lock);
 
+int tty_lock_interruptible(struct tty_struct *tty)
+{
+	int ret;
+
+	if (WARN(tty->magic != TTY_MAGIC, "L Bad %p\n", tty))
+		return -EIO;
+	tty_kref_get(tty);
+	ret = mutex_lock_interruptible(&tty->legacy_mutex);
+	if (ret)
+		tty_kref_put(tty);
+	return ret;
+}
+
 void __lockfunc tty_unlock(struct tty_struct *tty)
 {
-	if (tty->magic != TTY_MAGIC) {
-		pr_err("U Bad %p\n", tty);
-		WARN_ON(1);
+	if (WARN(tty->magic != TTY_MAGIC, "U Bad %p\n", tty))
 		return;
-	}
 	mutex_unlock(&tty->legacy_mutex);
 	tty_kref_put(tty);
 }
diff --git a/drivers/tty/tty_port.c b/drivers/tty/tty_port.c
index 482f33f20043..846ed481c24f 100644
--- a/drivers/tty/tty_port.c
+++ b/drivers/tty/tty_port.c
@@ -462,14 +462,13 @@ int tty_port_close_start(struct tty_port *port,
 
 	spin_lock_irqsave(&port->lock, flags);
 	if (tty->count == 1 && port->count != 1) {
-		printk(KERN_WARNING
-		    "tty_port_close_start: tty->count = 1 port count = %d.\n",
-								port->count);
+		tty_warn(tty, "%s: tty->count = 1 port count = %d\n", __func__,
+			 port->count);
 		port->count = 1;
 	}
 	if (--port->count < 0) {
-		printk(KERN_WARNING "tty_port_close_start: count = %d\n",
-								port->count);
+		tty_warn(tty, "%s: bad port count (%d)\n", __func__,
+			 port->count);
 		port->count = 0;
 	}
 
diff --git a/drivers/tty/vt/vt.c b/drivers/tty/vt/vt.c
index 4462d167900c..bd51bdd0a7bf 100644
--- a/drivers/tty/vt/vt.c
+++ b/drivers/tty/vt/vt.c
@@ -634,7 +634,7 @@ static void set_origin(struct vc_data *vc)
 	vc->vc_pos = vc->vc_origin + vc->vc_size_row * vc->vc_y + 2 * vc->vc_x;
 }
 
-static inline void save_screen(struct vc_data *vc)
+static void save_screen(struct vc_data *vc)
 {
 	WARN_CONSOLE_UNLOCKED();
 
@@ -4250,6 +4250,7 @@ unsigned short *screen_pos(struct vc_data *vc, int w_offset, int viewed)
 {
 	return screenpos(vc, 2 * w_offset, viewed);
 }
+EXPORT_SYMBOL_GPL(screen_pos);
 
 void getconsxy(struct vc_data *vc, unsigned char *p)
 {
diff --git a/drivers/usb/chipidea/Kconfig b/drivers/usb/chipidea/Kconfig
index 5619b8ca3bf3..3644a3500b70 100644
--- a/drivers/usb/chipidea/Kconfig
+++ b/drivers/usb/chipidea/Kconfig
@@ -37,9 +37,4 @@ config USB_CHIPIDEA_HOST
 	  Say Y here to enable host controller functionality of the
 	  ChipIdea driver.
 
-config USB_CHIPIDEA_DEBUG
-	bool "ChipIdea driver debug"
-	help
-	  Say Y here to enable debugging output of the ChipIdea driver.
-
 endif
diff --git a/drivers/usb/chipidea/Makefile b/drivers/usb/chipidea/Makefile
index 4decb12f2578..518e445476c3 100644
--- a/drivers/usb/chipidea/Makefile
+++ b/drivers/usb/chipidea/Makefile
@@ -1,11 +1,8 @@
-ccflags-$(CONFIG_USB_CHIPIDEA_DEBUG) := -DDEBUG
-
 obj-$(CONFIG_USB_CHIPIDEA)		+= ci_hdrc.o
 
-ci_hdrc-y				:= core.o otg.o
+ci_hdrc-y				:= core.o otg.o debug.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_UDC)	+= udc.o
 ci_hdrc-$(CONFIG_USB_CHIPIDEA_HOST)	+= host.o
-ci_hdrc-$(CONFIG_USB_CHIPIDEA_DEBUG)	+= debug.o
 ci_hdrc-$(CONFIG_USB_OTG_FSM)		+= otg_fsm.o
 
 # Glue/Bridge layers go here
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index 41d7cf6d63ba..cd414559040f 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -433,4 +433,7 @@ int hw_wait_reg(struct ci_hdrc *ci, enum ci_hw_regs reg, u32 mask,
 
 void ci_platform_configure(struct ci_hdrc *ci);
 
+int dbg_create_files(struct ci_hdrc *ci);
+
+void dbg_remove_files(struct ci_hdrc *ci);
 #endif	/* __DRIVERS_USB_CHIPIDEA_CI_H */
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.c b/drivers/usb/chipidea/ci_hdrc_imx.c
index 5a048b7b92e8..f14f4ab47ebb 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.c
+++ b/drivers/usb/chipidea/ci_hdrc_imx.c
@@ -345,6 +345,11 @@ static int ci_hdrc_imx_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static void ci_hdrc_imx_shutdown(struct platform_device *pdev)
+{
+	ci_hdrc_imx_remove(pdev);
+}
+
 #ifdef CONFIG_PM
 static int imx_controller_suspend(struct device *dev)
 {
@@ -462,6 +467,7 @@ static const struct dev_pm_ops ci_hdrc_imx_pm_ops = {
 static struct platform_driver ci_hdrc_imx_driver = {
 	.probe = ci_hdrc_imx_probe,
 	.remove = ci_hdrc_imx_remove,
+	.shutdown = ci_hdrc_imx_shutdown,
 	.driver = {
 		.name = "imx_usb",
 		.of_match_table = ci_hdrc_imx_dt_ids,
diff --git a/drivers/usb/chipidea/ci_hdrc_msm.c b/drivers/usb/chipidea/ci_hdrc_msm.c
index d79ecc08a1be..3889809fd0c4 100644
--- a/drivers/usb/chipidea/ci_hdrc_msm.c
+++ b/drivers/usb/chipidea/ci_hdrc_msm.c
@@ -25,7 +25,8 @@ static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 	case CI_HDRC_CONTROLLER_RESET_EVENT:
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_RESET_EVENT received\n");
 		writel(0, USB_AHBBURST);
-		writel(0, USB_AHBMODE);
+		/* use AHB transactor, allow posted data writes */
+		writel(0x8, USB_AHBMODE);
 		usb_phy_init(ci->usb_phy);
 		break;
 	case CI_HDRC_CONTROLLER_STOPPED_EVENT:
diff --git a/drivers/usb/chipidea/ci_hdrc_pci.c b/drivers/usb/chipidea/ci_hdrc_pci.c
index b59195edf636..b635ab67490d 100644
--- a/drivers/usb/chipidea/ci_hdrc_pci.c
+++ b/drivers/usb/chipidea/ci_hdrc_pci.c
@@ -85,8 +85,8 @@ static int ci_hdrc_pci_probe(struct pci_dev *pdev,
 
 	/* register a nop PHY */
 	ci->phy = usb_phy_generic_register();
-	if (!ci->phy)
-		return -ENOMEM;
+	if (IS_ERR(ci->phy))
+		return PTR_ERR(ci->phy);
 
 	memset(res, 0, sizeof(res));
 	res[0].start	= pci_resource_start(pdev, 0);
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 965d0e240dcb..7404064b9bbc 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -23,7 +23,6 @@
  * - BUS:    bus glue code, bus abstraction layer
  *
  * Compile Options
- * - CONFIG_USB_CHIPIDEA_DEBUG: enable debug facilities
  * - STALL_IN:  non-empty bulk-in pipes cannot be halted
  *              if defined mass storage compliance succeeds but with warnings
  *              => case 4: Hi >  Dn
@@ -71,7 +70,6 @@
 #include "udc.h"
 #include "bits.h"
 #include "host.h"
-#include "debug.h"
 #include "otg.h"
 #include "otg_fsm.h"
 
@@ -688,52 +686,39 @@ static int ci_get_platdata(struct device *dev,
 	if (usb_get_maximum_speed(dev) == USB_SPEED_FULL)
 		platdata->flags |= CI_HDRC_FORCE_FULLSPEED;
 
-	if (of_find_property(dev->of_node, "phy-clkgate-delay-us", NULL))
-		of_property_read_u32(dev->of_node, "phy-clkgate-delay-us",
+	of_property_read_u32(dev->of_node, "phy-clkgate-delay-us",
 				     &platdata->phy_clkgate_delay_us);
 
 	platdata->itc_setting = 1;
-	if (of_find_property(dev->of_node, "itc-setting", NULL)) {
-		ret = of_property_read_u32(dev->of_node, "itc-setting",
-			&platdata->itc_setting);
-		if (ret) {
-			dev_err(dev,
-				"failed to get itc-setting\n");
-			return ret;
-		}
-	}
 
-	if (of_find_property(dev->of_node, "ahb-burst-config", NULL)) {
-		ret = of_property_read_u32(dev->of_node, "ahb-burst-config",
-			&platdata->ahb_burst_config);
-		if (ret) {
-			dev_err(dev,
-				"failed to get ahb-burst-config\n");
-			return ret;
-		}
+	of_property_read_u32(dev->of_node, "itc-setting",
+					&platdata->itc_setting);
+
+	ret = of_property_read_u32(dev->of_node, "ahb-burst-config",
+				&platdata->ahb_burst_config);
+	if (!ret) {
 		platdata->flags |= CI_HDRC_OVERRIDE_AHB_BURST;
+	} else if (ret != -EINVAL) {
+		dev_err(dev, "failed to get ahb-burst-config\n");
+		return ret;
 	}
 
-	if (of_find_property(dev->of_node, "tx-burst-size-dword", NULL)) {
-		ret = of_property_read_u32(dev->of_node, "tx-burst-size-dword",
-			&platdata->tx_burst_size);
-		if (ret) {
-			dev_err(dev,
-				"failed to get tx-burst-size-dword\n");
-			return ret;
-		}
+	ret = of_property_read_u32(dev->of_node, "tx-burst-size-dword",
+				&platdata->tx_burst_size);
+	if (!ret) {
 		platdata->flags |= CI_HDRC_OVERRIDE_TX_BURST;
+	} else if (ret != -EINVAL) {
+		dev_err(dev, "failed to get tx-burst-size-dword\n");
+		return ret;
 	}
 
-	if (of_find_property(dev->of_node, "rx-burst-size-dword", NULL)) {
-		ret = of_property_read_u32(dev->of_node, "rx-burst-size-dword",
-			&platdata->rx_burst_size);
-		if (ret) {
-			dev_err(dev,
-				"failed to get rx-burst-size-dword\n");
-			return ret;
-		}
+	ret = of_property_read_u32(dev->of_node, "rx-burst-size-dword",
+				&platdata->rx_burst_size);
+	if (!ret) {
 		platdata->flags |= CI_HDRC_OVERRIDE_RX_BURST;
+	} else if (ret != -EINVAL) {
+		dev_err(dev, "failed to get rx-burst-size-dword\n");
+		return ret;
 	}
 
 	ext_id = ERR_PTR(-ENODEV);
diff --git a/drivers/usb/chipidea/debug.c b/drivers/usb/chipidea/debug.c
index 58c8485a0715..df47110bad2d 100644
--- a/drivers/usb/chipidea/debug.c
+++ b/drivers/usb/chipidea/debug.c
@@ -15,7 +15,6 @@
 #include "ci.h"
 #include "udc.h"
 #include "bits.h"
-#include "debug.h"
 #include "otg.h"
 
 /**
@@ -101,6 +100,9 @@ static ssize_t ci_port_test_write(struct file *file, const char __user *ubuf,
 	if (sscanf(buf, "%u", &mode) != 1)
 		return -EINVAL;
 
+	if (mode > 255)
+		return -EBADRQC;
+
 	pm_runtime_get_sync(ci->dev);
 	spin_lock_irqsave(&ci->lock, flags);
 	ret = hw_port_test_set(ci, mode);
diff --git a/drivers/usb/chipidea/debug.h b/drivers/usb/chipidea/debug.h
deleted file mode 100644
index e16478c4a943..000000000000
--- a/drivers/usb/chipidea/debug.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * debug.h - ChipIdea USB driver debug interfaces
- *
- * Copyright (C) 2008 Chipidea - MIPS Technologies, Inc. All rights reserved.
- *
- * Author: David Lopo
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef __DRIVERS_USB_CHIPIDEA_DEBUG_H
-#define __DRIVERS_USB_CHIPIDEA_DEBUG_H
-
-#ifdef CONFIG_USB_CHIPIDEA_DEBUG
-int dbg_create_files(struct ci_hdrc *ci);
-void dbg_remove_files(struct ci_hdrc *ci);
-#else
-static inline int dbg_create_files(struct ci_hdrc *ci)
-{
-	return 0;
-}
-
-static inline void dbg_remove_files(struct ci_hdrc *ci)
-{
-}
-#endif
-
-#endif /* __DRIVERS_USB_CHIPIDEA_DEBUG_H */
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index 3d24304405b3..053bac9d983c 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -190,6 +190,8 @@ static void host_stop(struct ci_hdrc *ci)
 			(ci->platdata->flags & CI_HDRC_TURN_VBUS_EARLY_ON))
 				regulator_disable(ci->platdata->reg_vbus);
 	}
+	ci->hcd = NULL;
+	ci->otg.host = NULL;
 }
 
 
diff --git a/drivers/usb/chipidea/otg.c b/drivers/usb/chipidea/otg.c
index 45f86da1d6d3..03b6743461d1 100644
--- a/drivers/usb/chipidea/otg.c
+++ b/drivers/usb/chipidea/otg.c
@@ -158,7 +158,7 @@ static void ci_otg_work(struct work_struct *work)
 int ci_hdrc_otg_init(struct ci_hdrc *ci)
 {
 	INIT_WORK(&ci->work, ci_otg_work);
-	ci->wq = create_singlethread_workqueue("ci_otg");
+	ci->wq = create_freezable_workqueue("ci_otg");
 	if (!ci->wq) {
 		dev_err(ci->dev, "can't create workqueue\n");
 		return -ENODEV;
diff --git a/drivers/usb/chipidea/otg_fsm.c b/drivers/usb/chipidea/otg_fsm.c
index 00ab59d45da1..ba90dc66703d 100644
--- a/drivers/usb/chipidea/otg_fsm.c
+++ b/drivers/usb/chipidea/otg_fsm.c
@@ -485,20 +485,30 @@ static void ci_otg_loc_conn(struct otg_fsm *fsm, int on)
 
 /*
  * Generate SOF by host.
- * This is controlled through suspend/resume the port.
  * In host mode, controller will automatically send SOF.
  * Suspend will block the data on the port.
+ *
+ * This is controlled through usbcore by usb autosuspend,
+ * so the usb device class driver need support autosuspend,
+ * otherwise the bus suspend will not happen.
  */
 static void ci_otg_loc_sof(struct otg_fsm *fsm, int on)
 {
-	struct ci_hdrc	*ci = container_of(fsm, struct ci_hdrc, fsm);
+	struct usb_device *udev;
 
-	if (on)
-		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_FPR,
-							PORTSC_FPR);
-	else
-		hw_write(ci, OP_PORTSC, PORTSC_W1C_BITS | PORTSC_SUSP,
-							PORTSC_SUSP);
+	if (!fsm->otg->host)
+		return;
+
+	udev = usb_hub_find_child(fsm->otg->host->root_hub, 1);
+	if (!udev)
+		return;
+
+	if (on) {
+		usb_disable_autosuspend(udev);
+	} else {
+		pm_runtime_set_autosuspend_delay(&udev->dev, 0);
+		usb_enable_autosuspend(udev);
+	}
 }
 
 /*
diff --git a/drivers/usb/chipidea/otg_fsm.h b/drivers/usb/chipidea/otg_fsm.h
index 2689375ae5da..262d6ef8df7c 100644
--- a/drivers/usb/chipidea/otg_fsm.h
+++ b/drivers/usb/chipidea/otg_fsm.h
@@ -62,7 +62,7 @@
 /* SSEND time before SRP */
 #define TB_SSEND_SRP         (1500)	/* minimum 1.5 sec, section:5.1.2 */
 
-#ifdef CONFIG_USB_OTG_FSM
+#if IS_ENABLED(CONFIG_USB_OTG_FSM)
 
 int ci_hdrc_otg_fsm_init(struct ci_hdrc *ci);
 int ci_otg_fsm_work(struct ci_hdrc *ci);
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index 391a1225b0ba..3eafa2c9a2ba 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -26,7 +26,6 @@
 #include "ci.h"
 #include "udc.h"
 #include "bits.h"
-#include "debug.h"
 #include "otg.h"
 #include "otg_fsm.h"
 
@@ -349,14 +348,13 @@ static int add_td_to_list(struct ci_hw_ep *hwep, struct ci_hw_req *hwreq,
 	if (node == NULL)
 		return -ENOMEM;
 
-	node->ptr = dma_pool_alloc(hwep->td_pool, GFP_ATOMIC,
+	node->ptr = dma_pool_zalloc(hwep->td_pool, GFP_ATOMIC,
 				   &node->dma);
 	if (node->ptr == NULL) {
 		kfree(node);
 		return -ENOMEM;
 	}
 
-	memset(node->ptr, 0, sizeof(struct ci_hw_td));
 	node->ptr->token = cpu_to_le32(length << __ffs(TD_TOTAL_BYTES));
 	node->ptr->token &= cpu_to_le32(TD_TOTAL_BYTES);
 	node->ptr->token |= cpu_to_le32(TD_STATUS_ACTIVE);
@@ -404,9 +402,9 @@ static inline u8 _usb_addr(struct ci_hw_ep *ep)
 }
 
 /**
- * _hardware_queue: configures a request at hardware level
- * @gadget: gadget
+ * _hardware_enqueue: configures a request at hardware level
  * @hwep:   endpoint
+ * @hwreq:  request
  *
  * This function returns an error code
  */
@@ -435,19 +433,28 @@ static int _hardware_enqueue(struct ci_hw_ep *hwep, struct ci_hw_req *hwreq)
 	if (hwreq->req.dma % PAGE_SIZE)
 		pages--;
 
-	if (rest == 0)
-		add_td_to_list(hwep, hwreq, 0);
+	if (rest == 0) {
+		ret = add_td_to_list(hwep, hwreq, 0);
+		if (ret < 0)
+			goto done;
+	}
 
 	while (rest > 0) {
 		unsigned count = min(hwreq->req.length - hwreq->req.actual,
 					(unsigned)(pages * CI_HDRC_PAGE_SIZE));
-		add_td_to_list(hwep, hwreq, count);
+		ret = add_td_to_list(hwep, hwreq, count);
+		if (ret < 0)
+			goto done;
+
 		rest -= count;
 	}
 
 	if (hwreq->req.zero && hwreq->req.length && hwep->dir == TX
-	    && (hwreq->req.length % hwep->ep.maxpacket == 0))
-		add_td_to_list(hwep, hwreq, 0);
+	    && (hwreq->req.length % hwep->ep.maxpacket == 0)) {
+		ret = add_td_to_list(hwep, hwreq, 0);
+		if (ret < 0)
+			goto done;
+	}
 
 	firstnode = list_first_entry(&hwreq->tds, struct td_node, td);
 
@@ -788,8 +795,12 @@ static void isr_get_status_complete(struct usb_ep *ep, struct usb_request *req)
 
 /**
  * _ep_queue: queues (submits) an I/O request to an endpoint
+ * @ep:        endpoint
+ * @req:       request
+ * @gfp_flags: GFP flags (not used)
  *
  * Caller must hold lock
+ * This function returns an error code
  */
 static int _ep_queue(struct usb_ep *ep, struct usb_request *req,
 		    gfp_t __maybe_unused gfp_flags)
diff --git a/drivers/usb/class/cdc-acm.c b/drivers/usb/class/cdc-acm.c
index 26ca4f910cb0..fa4e23930614 100644
--- a/drivers/usb/class/cdc-acm.c
+++ b/drivers/usb/class/cdc-acm.c
@@ -428,7 +428,8 @@ static void acm_read_bulk_callback(struct urb *urb)
 		set_bit(rb->index, &acm->read_urbs_free);
 		dev_dbg(&acm->data->dev, "%s - non-zero urb status: %d\n",
 							__func__, status);
-		return;
+		if ((status != -ENOENT) || (urb->actual_length == 0))
+			return;
 	}
 
 	usb_mark_last_busy(acm->dev);
@@ -1404,6 +1405,8 @@ made_compressed_probe:
 				usb_sndbulkpipe(usb_dev, epwrite->bEndpointAddress),
 				NULL, acm->writesize, acm_write_bulk, snd);
 		snd->urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
+		if (quirks & SEND_ZERO_PACKET)
+			snd->urb->transfer_flags |= URB_ZERO_PACKET;
 		snd->instance = acm;
 	}
 
@@ -1838,6 +1841,11 @@ static const struct usb_device_id acm_ids[] = {
 	},
 #endif
 
+	/*Samsung phone in firmware update mode */
+	{ USB_DEVICE(0x04e8, 0x685d),
+	.driver_info = IGNORE_DEVICE,
+	},
+
 	/* Exclude Infineon Flash Loader utility */
 	{ USB_DEVICE(0x058b, 0x0041),
 	.driver_info = IGNORE_DEVICE,
@@ -1861,6 +1869,10 @@ static const struct usb_device_id acm_ids[] = {
 	{ USB_INTERFACE_INFO(USB_CLASS_COMM, USB_CDC_SUBCLASS_ACM,
 		USB_CDC_ACM_PROTO_AT_CDMA) },
 
+	{ USB_DEVICE(0x1519, 0x0452), /* Intel 7260 modem */
+	.driver_info = SEND_ZERO_PACKET,
+	},
+
 	{ }
 };
 
diff --git a/drivers/usb/class/cdc-acm.h b/drivers/usb/class/cdc-acm.h
index dd9af38e7cda..ccfaba9ab4e4 100644
--- a/drivers/usb/class/cdc-acm.h
+++ b/drivers/usb/class/cdc-acm.h
@@ -134,3 +134,4 @@ struct acm {
 #define IGNORE_DEVICE			BIT(5)
 #define QUIRK_CONTROL_LINE_STATE	BIT(6)
 #define CLEAR_HALT_CONDITIONS		BIT(7)
+#define SEND_ZERO_PACKET		BIT(8)
diff --git a/drivers/usb/common/common.c b/drivers/usb/common/common.c
index 673d53038ed2..e6ec125e4485 100644
--- a/drivers/usb/common/common.c
+++ b/drivers/usb/common/common.c
@@ -17,6 +17,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/of.h>
 #include <linux/usb/otg.h>
+#include <linux/of_platform.h>
 
 const char *usb_otg_state_string(enum usb_otg_state state)
 {
@@ -106,25 +107,72 @@ static const char *const usb_dr_modes[] = {
 	[USB_DR_MODE_OTG]		= "otg",
 };
 
+static enum usb_dr_mode usb_get_dr_mode_from_string(const char *str)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(usb_dr_modes); i++)
+		if (!strcmp(usb_dr_modes[i], str))
+			return i;
+
+	return USB_DR_MODE_UNKNOWN;
+}
+
 enum usb_dr_mode usb_get_dr_mode(struct device *dev)
 {
 	const char *dr_mode;
-	int err, i;
+	int err;
 
 	err = device_property_read_string(dev, "dr_mode", &dr_mode);
 	if (err < 0)
 		return USB_DR_MODE_UNKNOWN;
 
-	for (i = 0; i < ARRAY_SIZE(usb_dr_modes); i++)
-		if (!strcmp(dr_mode, usb_dr_modes[i]))
-			return i;
-
-	return USB_DR_MODE_UNKNOWN;
+	return usb_get_dr_mode_from_string(dr_mode);
 }
 EXPORT_SYMBOL_GPL(usb_get_dr_mode);
 
 #ifdef CONFIG_OF
 /**
+ * of_usb_get_dr_mode_by_phy - Get dual role mode for the controller device
+ * which is associated with the given phy device_node
+ * @np:	Pointer to the given phy device_node
+ *
+ * In dts a usb controller associates with phy devices.  The function gets
+ * the string from property 'dr_mode' of the controller associated with the
+ * given phy device node, and returns the correspondig enum usb_dr_mode.
+ */
+enum usb_dr_mode of_usb_get_dr_mode_by_phy(struct device_node *phy_np)
+{
+	struct device_node *controller = NULL;
+	struct device_node *phy;
+	const char *dr_mode;
+	int index;
+	int err;
+
+	do {
+		controller = of_find_node_with_property(controller, "phys");
+		index = 0;
+		do {
+			phy = of_parse_phandle(controller, "phys", index);
+			of_node_put(phy);
+			if (phy == phy_np)
+				goto finish;
+			index++;
+		} while (phy);
+	} while (controller);
+
+finish:
+	err = of_property_read_string(controller, "dr_mode", &dr_mode);
+	of_node_put(controller);
+
+	if (err < 0)
+		return USB_DR_MODE_UNKNOWN;
+
+	return usb_get_dr_mode_from_string(dr_mode);
+}
+EXPORT_SYMBOL_GPL(of_usb_get_dr_mode_by_phy);
+
+/**
  * of_usb_host_tpl_support - to get if Targeted Peripheral List is supported
  * for given targeted hosts (non-PC hosts)
  * @np: Pointer to the given device_node
diff --git a/drivers/usb/core/devices.c b/drivers/usb/core/devices.c
index 2a3bbdf7eb94..cffa0a0d7de2 100644
--- a/drivers/usb/core/devices.c
+++ b/drivers/usb/core/devices.c
@@ -661,32 +661,8 @@ static unsigned int usb_device_poll(struct file *file,
 	return 0;
 }
 
-static loff_t usb_device_lseek(struct file *file, loff_t offset, int orig)
-{
-	loff_t ret;
-
-	mutex_lock(&file_inode(file)->i_mutex);
-
-	switch (orig) {
-	case 0:
-		file->f_pos = offset;
-		ret = file->f_pos;
-		break;
-	case 1:
-		file->f_pos += offset;
-		ret = file->f_pos;
-		break;
-	case 2:
-	default:
-		ret = -EINVAL;
-	}
-
-	mutex_unlock(&file_inode(file)->i_mutex);
-	return ret;
-}
-
 const struct file_operations usbfs_devices_fops = {
-	.llseek =	usb_device_lseek,
+	.llseek =	no_seek_end_llseek,
 	.read =		usb_device_read,
 	.poll =		usb_device_poll,
 };
diff --git a/drivers/usb/core/devio.c b/drivers/usb/core/devio.c
index 38ae877c46e3..59e7a3369084 100644
--- a/drivers/usb/core/devio.c
+++ b/drivers/usb/core/devio.c
@@ -100,6 +100,11 @@ static bool usbfs_snoop;
 module_param(usbfs_snoop, bool, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(usbfs_snoop, "true to log all usbfs traffic");
 
+static unsigned usbfs_snoop_max = 65536;
+module_param(usbfs_snoop_max, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(usbfs_snoop_max,
+		"maximum number of bytes to print while snooping");
+
 #define snoop(dev, format, arg...)				\
 	do {							\
 		if (usbfs_snoop)				\
@@ -157,30 +162,6 @@ static int connected(struct usb_dev_state *ps)
 			ps->dev->state != USB_STATE_NOTATTACHED);
 }
 
-static loff_t usbdev_lseek(struct file *file, loff_t offset, int orig)
-{
-	loff_t ret;
-
-	mutex_lock(&file_inode(file)->i_mutex);
-
-	switch (orig) {
-	case 0:
-		file->f_pos = offset;
-		ret = file->f_pos;
-		break;
-	case 1:
-		file->f_pos += offset;
-		ret = file->f_pos;
-		break;
-	case 2:
-	default:
-		ret = -EINVAL;
-	}
-
-	mutex_unlock(&file_inode(file)->i_mutex);
-	return ret;
-}
-
 static ssize_t usbdev_read(struct file *file, char __user *buf, size_t nbytes,
 			   loff_t *ppos)
 {
@@ -392,6 +373,7 @@ static void snoop_urb(struct usb_device *udev,
 					ep, t, d, length, timeout_or_status);
 	}
 
+	data_len = min(data_len, usbfs_snoop_max);
 	if (data && data_len > 0) {
 		print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_NONE, 32, 1,
 			data, data_len, 1);
@@ -402,7 +384,8 @@ static void snoop_urb_data(struct urb *urb, unsigned len)
 {
 	int i, size;
 
-	if (!usbfs_snoop)
+	len = min(len, usbfs_snoop_max);
+	if (!usbfs_snoop || len == 0)
 		return;
 
 	if (urb->num_sgs == 0) {
@@ -1709,8 +1692,12 @@ static struct async *reap_as(struct usb_dev_state *ps)
 static int proc_reapurb(struct usb_dev_state *ps, void __user *arg)
 {
 	struct async *as = reap_as(ps);
+
 	if (as) {
-		int retval = processcompl(as, (void __user * __user *)arg);
+		int retval;
+
+		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		retval = processcompl(as, (void __user * __user *)arg);
 		free_async(as);
 		return retval;
 	}
@@ -1726,6 +1713,7 @@ static int proc_reapurbnonblock(struct usb_dev_state *ps, void __user *arg)
 
 	as = async_getcompleted(ps);
 	if (as) {
+		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
 		retval = processcompl(as, (void __user * __user *)arg);
 		free_async(as);
 	} else {
@@ -1852,8 +1840,12 @@ static int processcompl_compat(struct async *as, void __user * __user *arg)
 static int proc_reapurb_compat(struct usb_dev_state *ps, void __user *arg)
 {
 	struct async *as = reap_as(ps);
+
 	if (as) {
-		int retval = processcompl_compat(as, (void __user * __user *)arg);
+		int retval;
+
+		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
+		retval = processcompl_compat(as, (void __user * __user *)arg);
 		free_async(as);
 		return retval;
 	}
@@ -1869,6 +1861,7 @@ static int proc_reapurbnonblock_compat(struct usb_dev_state *ps, void __user *ar
 
 	as = async_getcompleted(ps);
 	if (as) {
+		snoop(&ps->dev->dev, "reap %p\n", as->userurb);
 		retval = processcompl_compat(as, (void __user * __user *)arg);
 		free_async(as);
 	} else {
@@ -2273,7 +2266,7 @@ static long usbdev_do_ioctl(struct file *file, unsigned int cmd,
 #endif
 
 	case USBDEVFS_DISCARDURB:
-		snoop(&dev->dev, "%s: DISCARDURB\n", __func__);
+		snoop(&dev->dev, "%s: DISCARDURB %p\n", __func__, p);
 		ret = proc_unlinkurb(ps, p);
 		break;
 
@@ -2366,7 +2359,7 @@ static unsigned int usbdev_poll(struct file *file,
 
 const struct file_operations usbdev_file_operations = {
 	.owner =	  THIS_MODULE,
-	.llseek =	  usbdev_lseek,
+	.llseek =	  no_seek_end_llseek,
 	.read =		  usbdev_read,
 	.poll =		  usbdev_poll,
 	.unlocked_ioctl = usbdev_ioctl,
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 1c102d60cd9f..df0e3b92533a 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -3000,7 +3000,7 @@ EXPORT_SYMBOL_GPL(usb_hcd_platform_shutdown);
 
 #if defined(CONFIG_USB_MON) || defined(CONFIG_USB_MON_MODULE)
 
-struct usb_mon_operations *mon_ops;
+const struct usb_mon_operations *mon_ops;
 
 /*
  * The registration is unlocked.
@@ -3010,7 +3010,7 @@ struct usb_mon_operations *mon_ops;
  * symbols from usbcore, usbcore gets referenced and cannot be unloaded first.
  */
 
-int usb_mon_register (struct usb_mon_operations *ops)
+int usb_mon_register(const struct usb_mon_operations *ops)
 {
 
 	if (mon_ops)
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index ddbf32d599cb..51b436918f78 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -3324,7 +3324,7 @@ static int finish_port_resume(struct usb_device *udev)
 /*
  * There are some SS USB devices which take longer time for link training.
  * XHCI specs 4.19.4 says that when Link training is successful, port
- * sets CSC bit to 1. So if SW reads port status before successful link
+ * sets CCS bit to 1. So if SW reads port status before successful link
  * training, then it will not find device to be present.
  * USB Analyzer log with such buggy devices show that in some cases
  * device switch on the RX termination after long delay of host enabling
@@ -3335,14 +3335,17 @@ static int finish_port_resume(struct usb_device *udev)
  * routine implements a 2000 ms timeout for link training. If in a case
  * link trains before timeout, loop will exit earlier.
  *
+ * There are also some 2.0 hard drive based devices and 3.0 thumb
+ * drives that, when plugged into a 2.0 only port, take a long
+ * time to set CCS after VBUS enable.
+ *
  * FIXME: If a device was connected before suspend, but was removed
  * while system was asleep, then the loop in the following routine will
  * only exit at timeout.
  *
- * This routine should only be called when persist is enabled for a SS
- * device.
+ * This routine should only be called when persist is enabled.
  */
-static int wait_for_ss_port_enable(struct usb_device *udev,
+static int wait_for_connected(struct usb_device *udev,
 		struct usb_hub *hub, int *port1,
 		u16 *portchange, u16 *portstatus)
 {
@@ -3355,6 +3358,7 @@ static int wait_for_ss_port_enable(struct usb_device *udev,
 		delay_ms += 20;
 		status = hub_port_status(hub, *port1, portstatus, portchange);
 	}
+	dev_dbg(&udev->dev, "Waited %dms for CONNECT\n", delay_ms);
 	return status;
 }
 
@@ -3454,8 +3458,8 @@ int usb_port_resume(struct usb_device *udev, pm_message_t msg)
 		}
 	}
 
-	if (udev->persist_enabled && hub_is_superspeed(hub->hdev))
-		status = wait_for_ss_port_enable(udev, hub, &port1, &portchange,
+	if (udev->persist_enabled)
+		status = wait_for_connected(udev, hub, &port1, &portchange,
 				&portstatus);
 
 	status = check_port_resume_type(udev,
@@ -3895,17 +3899,30 @@ static void usb_enable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
 		return;
 	}
 
-	if (usb_set_lpm_timeout(udev, state, timeout))
+	if (usb_set_lpm_timeout(udev, state, timeout)) {
 		/* If we can't set the parent hub U1/U2 timeout,
 		 * device-initiated LPM won't be allowed either, so let the xHCI
 		 * host know that this link state won't be enabled.
 		 */
 		hcd->driver->disable_usb3_lpm_timeout(hcd, udev, state);
+	} else {
+		/* Only a configured device will accept the Set Feature
+		 * U1/U2_ENABLE
+		 */
+		if (udev->actconfig)
+			usb_set_device_initiated_lpm(udev, state, true);
 
-	/* Only a configured device will accept the Set Feature U1/U2_ENABLE */
-	else if (udev->actconfig)
-		usb_set_device_initiated_lpm(udev, state, true);
-
+		/* As soon as usb_set_lpm_timeout(timeout) returns 0, the
+		 * hub-initiated LPM is enabled. Thus, LPM is enabled no
+		 * matter the result of usb_set_device_initiated_lpm().
+		 * The only difference is whether device is able to initiate
+		 * LPM.
+		 */
+		if (state == USB3_LPM_U1)
+			udev->usb3_lpm_u1_enabled = 1;
+		else if (state == USB3_LPM_U2)
+			udev->usb3_lpm_u2_enabled = 1;
+	}
 }
 
 /*
@@ -3945,6 +3962,18 @@ static int usb_disable_link_state(struct usb_hcd *hcd, struct usb_device *udev,
 		dev_warn(&udev->dev, "Could not disable xHCI %s timeout, "
 				"bus schedule bandwidth may be impacted.\n",
 				usb3_lpm_names[state]);
+
+	/* As soon as usb_set_lpm_timeout(0) return 0, hub initiated LPM
+	 * is disabled. Hub will disallows link to enter U1/U2 as well,
+	 * even device is initiating LPM. Hence LPM is disabled if hub LPM
+	 * timeout set to 0, no matter device-initiated LPM is disabled or
+	 * not.
+	 */
+	if (state == USB3_LPM_U1)
+		udev->usb3_lpm_u1_enabled = 0;
+	else if (state == USB3_LPM_U2)
+		udev->usb3_lpm_u2_enabled = 0;
+
 	return 0;
 }
 
@@ -3979,8 +4008,6 @@ int usb_disable_lpm(struct usb_device *udev)
 	if (usb_disable_link_state(hcd, udev, USB3_LPM_U2))
 		goto enable_lpm;
 
-	udev->usb3_lpm_enabled = 0;
-
 	return 0;
 
 enable_lpm:
@@ -4017,6 +4044,8 @@ EXPORT_SYMBOL_GPL(usb_unlocked_disable_lpm);
 void usb_enable_lpm(struct usb_device *udev)
 {
 	struct usb_hcd *hcd;
+	struct usb_hub *hub;
+	struct usb_port *port_dev;
 
 	if (!udev || !udev->parent ||
 			udev->speed != USB_SPEED_SUPER ||
@@ -4036,10 +4065,17 @@ void usb_enable_lpm(struct usb_device *udev)
 	if (udev->lpm_disable_count > 0)
 		return;
 
-	usb_enable_link_state(hcd, udev, USB3_LPM_U1);
-	usb_enable_link_state(hcd, udev, USB3_LPM_U2);
+	hub = usb_hub_to_struct_hub(udev->parent);
+	if (!hub)
+		return;
+
+	port_dev = hub->ports[udev->portnum - 1];
+
+	if (port_dev->usb3_lpm_u1_permit)
+		usb_enable_link_state(hcd, udev, USB3_LPM_U1);
 
-	udev->usb3_lpm_enabled = 1;
+	if (port_dev->usb3_lpm_u2_permit)
+		usb_enable_link_state(hcd, udev, USB3_LPM_U2);
 }
 EXPORT_SYMBOL_GPL(usb_enable_lpm);
 
diff --git a/drivers/usb/core/hub.h b/drivers/usb/core/hub.h
index 688817fb3246..45d070dd1d03 100644
--- a/drivers/usb/core/hub.h
+++ b/drivers/usb/core/hub.h
@@ -92,6 +92,8 @@ struct usb_hub {
  * @status_lock: synchronize port_event() vs usb_port_{suspend|resume}
  * @portnum: port index num based one
  * @is_superspeed cache super-speed status
+ * @usb3_lpm_u1_permit: whether USB3 U1 LPM is permitted.
+ * @usb3_lpm_u2_permit: whether USB3 U2 LPM is permitted.
  */
 struct usb_port {
 	struct usb_device *child;
@@ -104,6 +106,8 @@ struct usb_port {
 	struct mutex status_lock;
 	u8 portnum;
 	unsigned int is_superspeed:1;
+	unsigned int usb3_lpm_u1_permit:1;
+	unsigned int usb3_lpm_u2_permit:1;
 };
 
 #define to_usb_port(_dev) \
@@ -155,4 +159,3 @@ static inline int hub_port_debounce_be_stable(struct usb_hub *hub,
 {
 	return hub_port_debounce(hub, port1, false);
 }
-
diff --git a/drivers/usb/core/port.c b/drivers/usb/core/port.c
index 5487fe308f01..14718a9ffcfb 100644
--- a/drivers/usb/core/port.c
+++ b/drivers/usb/core/port.c
@@ -50,6 +50,72 @@ static ssize_t connect_type_show(struct device *dev,
 }
 static DEVICE_ATTR_RO(connect_type);
 
+static ssize_t usb3_lpm_permit_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct usb_port *port_dev = to_usb_port(dev);
+	const char *p;
+
+	if (port_dev->usb3_lpm_u1_permit) {
+		if (port_dev->usb3_lpm_u2_permit)
+			p = "u1_u2";
+		else
+			p = "u1";
+	} else {
+		if (port_dev->usb3_lpm_u2_permit)
+			p = "u2";
+		else
+			p = "0";
+	}
+
+	return sprintf(buf, "%s\n", p);
+}
+
+static ssize_t usb3_lpm_permit_store(struct device *dev,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct usb_port *port_dev = to_usb_port(dev);
+	struct usb_device *udev = port_dev->child;
+	struct usb_hcd *hcd;
+
+	if (!strncmp(buf, "u1_u2", 5)) {
+		port_dev->usb3_lpm_u1_permit = 1;
+		port_dev->usb3_lpm_u2_permit = 1;
+
+	} else if (!strncmp(buf, "u1", 2)) {
+		port_dev->usb3_lpm_u1_permit = 1;
+		port_dev->usb3_lpm_u2_permit = 0;
+
+	} else if (!strncmp(buf, "u2", 2)) {
+		port_dev->usb3_lpm_u1_permit = 0;
+		port_dev->usb3_lpm_u2_permit = 1;
+
+	} else if (!strncmp(buf, "0", 1)) {
+		port_dev->usb3_lpm_u1_permit = 0;
+		port_dev->usb3_lpm_u2_permit = 0;
+	} else
+		return -EINVAL;
+
+	/* If device is connected to the port, disable or enable lpm
+	 * to make new u1 u2 setting take effect immediately.
+	 */
+	if (udev) {
+		hcd = bus_to_hcd(udev->bus);
+		if (!hcd)
+			return -EINVAL;
+		usb_lock_device(udev);
+		mutex_lock(hcd->bandwidth_mutex);
+		if (!usb_disable_lpm(udev))
+			usb_enable_lpm(udev);
+		mutex_unlock(hcd->bandwidth_mutex);
+		usb_unlock_device(udev);
+	}
+
+	return count;
+}
+static DEVICE_ATTR_RW(usb3_lpm_permit);
+
 static struct attribute *port_dev_attrs[] = {
 	&dev_attr_connect_type.attr,
 	NULL,
@@ -64,6 +130,21 @@ static const struct attribute_group *port_dev_group[] = {
 	NULL,
 };
 
+static struct attribute *port_dev_usb3_attrs[] = {
+	&dev_attr_usb3_lpm_permit.attr,
+	NULL,
+};
+
+static struct attribute_group port_dev_usb3_attr_grp = {
+	.attrs = port_dev_usb3_attrs,
+};
+
+static const struct attribute_group *port_dev_usb3_group[] = {
+	&port_dev_attr_grp,
+	&port_dev_usb3_attr_grp,
+	NULL,
+};
+
 static void usb_port_device_release(struct device *dev)
 {
 	struct usb_port *port_dev = to_usb_port(dev);
@@ -168,12 +249,18 @@ static int usb_port_runtime_suspend(struct device *dev)
 
 	return retval;
 }
+
+static int usb_port_prepare(struct device *dev)
+{
+	return 1;
+}
 #endif
 
 static const struct dev_pm_ops usb_port_pm_ops = {
 #ifdef CONFIG_PM
 	.runtime_suspend =	usb_port_runtime_suspend,
 	.runtime_resume =	usb_port_runtime_resume,
+	.prepare =		usb_port_prepare,
 #endif
 };
 
@@ -401,6 +488,7 @@ static void find_and_link_peer(struct usb_hub *hub, int port1)
 int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 {
 	struct usb_port *port_dev;
+	struct usb_device *hdev = hub->hdev;
 	int retval;
 
 	port_dev = kzalloc(sizeof(*port_dev), GFP_KERNEL);
@@ -417,7 +505,12 @@ int usb_hub_create_port_device(struct usb_hub *hub, int port1)
 	port_dev->portnum = port1;
 	set_bit(port1, hub->power_bits);
 	port_dev->dev.parent = hub->intfdev;
-	port_dev->dev.groups = port_dev_group;
+	if (hub_is_superspeed(hdev)) {
+		port_dev->usb3_lpm_u1_permit = 1;
+		port_dev->usb3_lpm_u2_permit = 1;
+		port_dev->dev.groups = port_dev_usb3_group;
+	} else
+		port_dev->dev.groups = port_dev_group;
 	port_dev->dev.type = &usb_port_device_type;
 	port_dev->dev.driver = &usb_port_driver;
 	if (hub_is_superspeed(hub->hdev))
diff --git a/drivers/usb/core/sysfs.c b/drivers/usb/core/sysfs.c
index d9ec2de6c4cf..65b6e6b84043 100644
--- a/drivers/usb/core/sysfs.c
+++ b/drivers/usb/core/sysfs.c
@@ -531,7 +531,7 @@ static ssize_t usb2_lpm_besl_store(struct device *dev,
 }
 static DEVICE_ATTR_RW(usb2_lpm_besl);
 
-static ssize_t usb3_hardware_lpm_show(struct device *dev,
+static ssize_t usb3_hardware_lpm_u1_show(struct device *dev,
 				      struct device_attribute *attr, char *buf)
 {
 	struct usb_device *udev = to_usb_device(dev);
@@ -539,7 +539,7 @@ static ssize_t usb3_hardware_lpm_show(struct device *dev,
 
 	usb_lock_device(udev);
 
-	if (udev->usb3_lpm_enabled)
+	if (udev->usb3_lpm_u1_enabled)
 		p = "enabled";
 	else
 		p = "disabled";
@@ -548,7 +548,26 @@ static ssize_t usb3_hardware_lpm_show(struct device *dev,
 
 	return sprintf(buf, "%s\n", p);
 }
-static DEVICE_ATTR_RO(usb3_hardware_lpm);
+static DEVICE_ATTR_RO(usb3_hardware_lpm_u1);
+
+static ssize_t usb3_hardware_lpm_u2_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct usb_device *udev = to_usb_device(dev);
+	const char *p;
+
+	usb_lock_device(udev);
+
+	if (udev->usb3_lpm_u2_enabled)
+		p = "enabled";
+	else
+		p = "disabled";
+
+	usb_unlock_device(udev);
+
+	return sprintf(buf, "%s\n", p);
+}
+static DEVICE_ATTR_RO(usb3_hardware_lpm_u2);
 
 static struct attribute *usb2_hardware_lpm_attr[] = {
 	&dev_attr_usb2_hardware_lpm.attr,
@@ -562,7 +581,8 @@ static struct attribute_group usb2_hardware_lpm_attr_group = {
 };
 
 static struct attribute *usb3_hardware_lpm_attr[] = {
-	&dev_attr_usb3_hardware_lpm.attr,
+	&dev_attr_usb3_hardware_lpm_u1.attr,
+	&dev_attr_usb3_hardware_lpm_u2.attr,
 	NULL,
 };
 static struct attribute_group usb3_hardware_lpm_attr_group = {
@@ -592,7 +612,8 @@ static int add_power_attributes(struct device *dev)
 		if (udev->usb2_hw_lpm_capable == 1)
 			rc = sysfs_merge_group(&dev->kobj,
 					&usb2_hardware_lpm_attr_group);
-		if (udev->lpm_capable == 1)
+		if (udev->speed == USB_SPEED_SUPER &&
+				udev->lpm_capable == 1)
 			rc = sysfs_merge_group(&dev->kobj,
 					&usb3_hardware_lpm_attr_group);
 	}
diff --git a/drivers/usb/core/usb.c b/drivers/usb/core/usb.c
index f8bbd0b6d9fe..ebb29caa3fe4 100644
--- a/drivers/usb/core/usb.c
+++ b/drivers/usb/core/usb.c
@@ -49,12 +49,7 @@ const char *usbcore_name = "usbcore";
 
 static bool nousb;	/* Disable USB when built into kernel image */
 
-/* To disable USB, kernel command line is 'nousb' not 'usbcore.nousb' */
-#ifdef MODULE
 module_param(nousb, bool, 0444);
-#else
-core_param(nousb, nousb, bool, 0444);
-#endif
 
 /*
  * for external read access to <nousb>
@@ -316,7 +311,13 @@ static int usb_dev_uevent(struct device *dev, struct kobj_uevent_env *env)
 
 static int usb_dev_prepare(struct device *dev)
 {
-	return 0;		/* Implement eventually? */
+	struct usb_device *udev = to_usb_device(dev);
+
+	/* Return 0 if the current wakeup setting is wrong, otherwise 1 */
+	if (udev->do_remote_wakeup != device_may_wakeup(dev))
+		return 0;
+
+	return 1;
 }
 
 static void usb_dev_complete(struct device *dev)
diff --git a/drivers/usb/dwc2/Kconfig b/drivers/usb/dwc2/Kconfig
index fd95ba6ec317..f0decc0d69b5 100644
--- a/drivers/usb/dwc2/Kconfig
+++ b/drivers/usb/dwc2/Kconfig
@@ -1,5 +1,6 @@
 config USB_DWC2
 	tristate "DesignWare USB2 DRD Core Support"
+	depends on HAS_DMA
 	depends on USB || USB_GADGET
 	help
 	  Say Y here if your system has a Dual Role Hi-Speed USB
diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index ef73e498e98f..46c4ba75dc2a 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -481,64 +481,168 @@ static void dwc2_init_fs_ls_pclk_sel(struct dwc2_hsotg *hsotg)
  * Do core a soft reset of the core.  Be careful with this because it
  * resets all the internal state machines of the core.
  */
-static int dwc2_core_reset(struct dwc2_hsotg *hsotg)
+int dwc2_core_reset(struct dwc2_hsotg *hsotg)
 {
 	u32 greset;
 	int count = 0;
-	u32 gusbcfg;
 
 	dev_vdbg(hsotg->dev, "%s()\n", __func__);
 
-	/* Wait for AHB master IDLE state */
+	/* Core Soft Reset */
+	greset = dwc2_readl(hsotg->regs + GRSTCTL);
+	greset |= GRSTCTL_CSFTRST;
+	dwc2_writel(greset, hsotg->regs + GRSTCTL);
 	do {
-		usleep_range(20000, 40000);
+		udelay(1);
 		greset = dwc2_readl(hsotg->regs + GRSTCTL);
 		if (++count > 50) {
 			dev_warn(hsotg->dev,
-				 "%s() HANG! AHB Idle GRSTCTL=%0x\n",
+				 "%s() HANG! Soft Reset GRSTCTL=%0x\n",
 				 __func__, greset);
 			return -EBUSY;
 		}
-	} while (!(greset & GRSTCTL_AHBIDLE));
+	} while (greset & GRSTCTL_CSFTRST);
 
-	/* Core Soft Reset */
+	/* Wait for AHB master IDLE state */
 	count = 0;
-	greset |= GRSTCTL_CSFTRST;
-	dwc2_writel(greset, hsotg->regs + GRSTCTL);
 	do {
-		usleep_range(20000, 40000);
+		udelay(1);
 		greset = dwc2_readl(hsotg->regs + GRSTCTL);
 		if (++count > 50) {
 			dev_warn(hsotg->dev,
-				 "%s() HANG! Soft Reset GRSTCTL=%0x\n",
+				 "%s() HANG! AHB Idle GRSTCTL=%0x\n",
 				 __func__, greset);
 			return -EBUSY;
 		}
-	} while (greset & GRSTCTL_CSFTRST);
+	} while (!(greset & GRSTCTL_AHBIDLE));
 
-	if (hsotg->dr_mode == USB_DR_MODE_HOST) {
-		gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-		gusbcfg &= ~GUSBCFG_FORCEDEVMODE;
-		gusbcfg |= GUSBCFG_FORCEHOSTMODE;
-		dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
-	} else if (hsotg->dr_mode == USB_DR_MODE_PERIPHERAL) {
-		gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-		gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
-		gusbcfg |= GUSBCFG_FORCEDEVMODE;
-		dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
-	} else if (hsotg->dr_mode == USB_DR_MODE_OTG) {
-		gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-		gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
-		gusbcfg &= ~GUSBCFG_FORCEDEVMODE;
-		dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
-	}
+	return 0;
+}
+
+/*
+ * Force the mode of the controller.
+ *
+ * Forcing the mode is needed for two cases:
+ *
+ * 1) If the dr_mode is set to either HOST or PERIPHERAL we force the
+ * controller to stay in a particular mode regardless of ID pin
+ * changes. We do this usually after a core reset.
+ *
+ * 2) During probe we want to read reset values of the hw
+ * configuration registers that are only available in either host or
+ * device mode. We may need to force the mode if the current mode does
+ * not allow us to access the register in the mode that we want.
+ *
+ * In either case it only makes sense to force the mode if the
+ * controller hardware is OTG capable.
+ *
+ * Checks are done in this function to determine whether doing a force
+ * would be valid or not.
+ *
+ * If a force is done, it requires a 25ms delay to take effect.
+ *
+ * Returns true if the mode was forced.
+ */
+static bool dwc2_force_mode(struct dwc2_hsotg *hsotg, bool host)
+{
+	u32 gusbcfg;
+	u32 set;
+	u32 clear;
+
+	dev_dbg(hsotg->dev, "Forcing mode to %s\n", host ? "host" : "device");
+
+	/*
+	 * Force mode has no effect if the hardware is not OTG.
+	 */
+	if (!dwc2_hw_is_otg(hsotg))
+		return false;
+
+	/*
+	 * If dr_mode is either peripheral or host only, there is no
+	 * need to ever force the mode to the opposite mode.
+	 */
+	if (WARN_ON(host && hsotg->dr_mode == USB_DR_MODE_PERIPHERAL))
+		return false;
+
+	if (WARN_ON(!host && hsotg->dr_mode == USB_DR_MODE_HOST))
+		return false;
+
+	gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
+
+	set = host ? GUSBCFG_FORCEHOSTMODE : GUSBCFG_FORCEDEVMODE;
+	clear = host ? GUSBCFG_FORCEDEVMODE : GUSBCFG_FORCEHOSTMODE;
+
+	gusbcfg &= ~clear;
+	gusbcfg |= set;
+	dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
+
+	msleep(25);
+	return true;
+}
+
+/*
+ * Clears the force mode bits.
+ */
+static void dwc2_clear_force_mode(struct dwc2_hsotg *hsotg)
+{
+	u32 gusbcfg;
+
+	gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
+	gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
+	gusbcfg &= ~GUSBCFG_FORCEDEVMODE;
+	dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
 
 	/*
 	 * NOTE: This long sleep is _very_ important, otherwise the core will
 	 * not stay in host mode after a connector ID change!
 	 */
-	usleep_range(150000, 200000);
+	msleep(25);
+}
+
+/*
+ * Sets or clears force mode based on the dr_mode parameter.
+ */
+void dwc2_force_dr_mode(struct dwc2_hsotg *hsotg)
+{
+	switch (hsotg->dr_mode) {
+	case USB_DR_MODE_HOST:
+		dwc2_force_mode(hsotg, true);
+		break;
+	case USB_DR_MODE_PERIPHERAL:
+		dwc2_force_mode(hsotg, false);
+		break;
+	case USB_DR_MODE_OTG:
+		dwc2_clear_force_mode(hsotg);
+		break;
+	default:
+		dev_warn(hsotg->dev, "%s() Invalid dr_mode=%d\n",
+			 __func__, hsotg->dr_mode);
+		break;
+	}
+
+	/*
+	 * NOTE: This is required for some rockchip soc based
+	 * platforms.
+	 */
+	msleep(50);
+}
 
+/*
+ * Do core a soft reset of the core.  Be careful with this because it
+ * resets all the internal state machines of the core.
+ *
+ * Additionally this will apply force mode as per the hsotg->dr_mode
+ * parameter.
+ */
+int dwc2_core_reset_and_force_dr_mode(struct dwc2_hsotg *hsotg)
+{
+	int retval;
+
+	retval = dwc2_core_reset(hsotg);
+	if (retval)
+		return retval;
+
+	dwc2_force_dr_mode(hsotg);
 	return 0;
 }
 
@@ -553,16 +657,20 @@ static int dwc2_fs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
 	 */
 	if (select_phy) {
 		dev_dbg(hsotg->dev, "FS PHY selected\n");
+
 		usbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-		usbcfg |= GUSBCFG_PHYSEL;
-		dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
+		if (!(usbcfg & GUSBCFG_PHYSEL)) {
+			usbcfg |= GUSBCFG_PHYSEL;
+			dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
 
-		/* Reset after a PHY select */
-		retval = dwc2_core_reset(hsotg);
-		if (retval) {
-			dev_err(hsotg->dev, "%s() Reset failed, aborting",
-					__func__);
-			return retval;
+			/* Reset after a PHY select */
+			retval = dwc2_core_reset_and_force_dr_mode(hsotg);
+
+			if (retval) {
+				dev_err(hsotg->dev,
+					"%s: Reset failed, aborting", __func__);
+				return retval;
+			}
 		}
 	}
 
@@ -597,13 +705,13 @@ static int dwc2_fs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
 
 static int dwc2_hs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
 {
-	u32 usbcfg;
+	u32 usbcfg, usbcfg_old;
 	int retval = 0;
 
 	if (!select_phy)
 		return 0;
 
-	usbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
+	usbcfg = usbcfg_old = dwc2_readl(hsotg->regs + GUSBCFG);
 
 	/*
 	 * HS PHY parameters. These parameters are preserved during soft reset
@@ -631,14 +739,16 @@ static int dwc2_hs_phy_init(struct dwc2_hsotg *hsotg, bool select_phy)
 		break;
 	}
 
-	dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
+	if (usbcfg != usbcfg_old) {
+		dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
 
-	/* Reset after setting the PHY parameters */
-	retval = dwc2_core_reset(hsotg);
-	if (retval) {
-		dev_err(hsotg->dev, "%s() Reset failed, aborting",
-				__func__);
-		return retval;
+		/* Reset after setting the PHY parameters */
+		retval = dwc2_core_reset_and_force_dr_mode(hsotg);
+		if (retval) {
+			dev_err(hsotg->dev,
+				"%s: Reset failed, aborting", __func__);
+			return retval;
+		}
 	}
 
 	return retval;
@@ -765,11 +875,10 @@ static void dwc2_gusbcfg_init(struct dwc2_hsotg *hsotg)
  * dwc2_core_init() - Initializes the DWC_otg controller registers and
  * prepares the core for device mode or host mode operation
  *
- * @hsotg:      Programming view of the DWC_otg controller
- * @select_phy: If true then also set the Phy type
- * @irq:        If >= 0, the irq to register
+ * @hsotg:         Programming view of the DWC_otg controller
+ * @initial_setup: If true then this is the first init for this instance.
  */
-int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq)
+int dwc2_core_init(struct dwc2_hsotg *hsotg, bool initial_setup)
 {
 	u32 usbcfg, otgctl;
 	int retval;
@@ -791,18 +900,26 @@ int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq)
 
 	dwc2_writel(usbcfg, hsotg->regs + GUSBCFG);
 
-	/* Reset the Controller */
-	retval = dwc2_core_reset(hsotg);
-	if (retval) {
-		dev_err(hsotg->dev, "%s(): Reset failed, aborting\n",
-				__func__);
-		return retval;
+	/*
+	 * Reset the Controller
+	 *
+	 * We only need to reset the controller if this is a re-init.
+	 * For the first init we know for sure that earlier code reset us (it
+	 * needed to in order to properly detect various parameters).
+	 */
+	if (!initial_setup) {
+		retval = dwc2_core_reset_and_force_dr_mode(hsotg);
+		if (retval) {
+			dev_err(hsotg->dev, "%s(): Reset failed, aborting\n",
+					__func__);
+			return retval;
+		}
 	}
 
 	/*
 	 * This needs to happen in FS mode before any other programming occurs
 	 */
-	retval = dwc2_phy_init(hsotg, select_phy);
+	retval = dwc2_phy_init(hsotg, initial_setup);
 	if (retval)
 		return retval;
 
@@ -1707,6 +1824,7 @@ void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
 	u32 hcchar;
 	u32 hctsiz = 0;
 	u16 num_packets;
+	u32 ec_mc;
 
 	if (dbg_hc(chan))
 		dev_vdbg(hsotg->dev, "%s()\n", __func__);
@@ -1743,6 +1861,13 @@ void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
 
 		hctsiz |= chan->xfer_len << TSIZ_XFERSIZE_SHIFT &
 			  TSIZ_XFERSIZE_MASK;
+
+		/* For split set ec_mc for immediate retries */
+		if (chan->ep_type == USB_ENDPOINT_XFER_INT ||
+		    chan->ep_type == USB_ENDPOINT_XFER_ISOC)
+			ec_mc = 3;
+		else
+			ec_mc = 1;
 	} else {
 		if (dbg_hc(chan))
 			dev_vdbg(hsotg->dev, "no split\n");
@@ -1805,6 +1930,9 @@ void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
 
 		hctsiz |= chan->xfer_len << TSIZ_XFERSIZE_SHIFT &
 			  TSIZ_XFERSIZE_MASK;
+
+		/* The ec_mc gets the multi_count for non-split */
+		ec_mc = chan->multi_count;
 	}
 
 	chan->start_pkt_count = num_packets;
@@ -1855,8 +1983,7 @@ void dwc2_hc_start_transfer(struct dwc2_hsotg *hsotg,
 
 	hcchar = dwc2_readl(hsotg->regs + HCCHAR(chan->hc_num));
 	hcchar &= ~HCCHAR_MULTICNT_MASK;
-	hcchar |= chan->multi_count << HCCHAR_MULTICNT_SHIFT &
-		  HCCHAR_MULTICNT_MASK;
+	hcchar |= (ec_mc << HCCHAR_MULTICNT_SHIFT) & HCCHAR_MULTICNT_MASK;
 	dwc2_hc_set_even_odd_frame(hsotg, chan, &hcchar);
 
 	if (hcchar & HCCHAR_CHDIS)
@@ -1905,7 +2032,6 @@ void dwc2_hc_start_transfer_ddma(struct dwc2_hsotg *hsotg,
 				 struct dwc2_host_chan *chan)
 {
 	u32 hcchar;
-	u32 hc_dma;
 	u32 hctsiz = 0;
 
 	if (chan->do_ping)
@@ -1934,14 +2060,14 @@ void dwc2_hc_start_transfer_ddma(struct dwc2_hsotg *hsotg,
 
 	dwc2_writel(hctsiz, hsotg->regs + HCTSIZ(chan->hc_num));
 
-	hc_dma = (u32)chan->desc_list_addr & HCDMA_DMA_ADDR_MASK;
+	dma_sync_single_for_device(hsotg->dev, chan->desc_list_addr,
+				   chan->desc_list_sz, DMA_TO_DEVICE);
+
+	dwc2_writel(chan->desc_list_addr, hsotg->regs + HCDMA(chan->hc_num));
 
-	/* Always start from first descriptor */
-	hc_dma &= ~HCDMA_CTD_MASK;
-	dwc2_writel(hc_dma, hsotg->regs + HCDMA(chan->hc_num));
 	if (dbg_hc(chan))
-		dev_vdbg(hsotg->dev, "Wrote %08x to HCDMA(%d)\n",
-			 hc_dma, chan->hc_num);
+		dev_vdbg(hsotg->dev, "Wrote %pad to HCDMA(%d)\n",
+			 &chan->desc_list_addr, chan->hc_num);
 
 	hcchar = dwc2_readl(hsotg->regs + HCCHAR(chan->hc_num));
 	hcchar &= ~HCCHAR_MULTICNT_MASK;
@@ -2485,6 +2611,29 @@ void dwc2_set_param_dma_desc_enable(struct dwc2_hsotg *hsotg, int val)
 	hsotg->core_params->dma_desc_enable = val;
 }
 
+void dwc2_set_param_dma_desc_fs_enable(struct dwc2_hsotg *hsotg, int val)
+{
+	int valid = 1;
+
+	if (val > 0 && (hsotg->core_params->dma_enable <= 0 ||
+			!hsotg->hw_params.dma_desc_enable))
+		valid = 0;
+	if (val < 0)
+		valid = 0;
+
+	if (!valid) {
+		if (val >= 0)
+			dev_err(hsotg->dev,
+				"%d invalid for dma_desc_fs_enable parameter. Check HW configuration.\n",
+				val);
+		val = (hsotg->core_params->dma_enable > 0 &&
+			hsotg->hw_params.dma_desc_enable);
+	}
+
+	hsotg->core_params->dma_desc_fs_enable = val;
+	dev_dbg(hsotg->dev, "Setting dma_desc_fs_enable to %d\n", val);
+}
+
 void dwc2_set_param_host_support_fs_ls_low_power(struct dwc2_hsotg *hsotg,
 						 int val)
 {
@@ -3016,6 +3165,7 @@ void dwc2_set_parameters(struct dwc2_hsotg *hsotg,
 	dwc2_set_param_otg_cap(hsotg, params->otg_cap);
 	dwc2_set_param_dma_enable(hsotg, params->dma_enable);
 	dwc2_set_param_dma_desc_enable(hsotg, params->dma_desc_enable);
+	dwc2_set_param_dma_desc_fs_enable(hsotg, params->dma_desc_fs_enable);
 	dwc2_set_param_host_support_fs_ls_low_power(hsotg,
 			params->host_support_fs_ls_low_power);
 	dwc2_set_param_enable_dynamic_fifo(hsotg,
@@ -3052,6 +3202,79 @@ void dwc2_set_parameters(struct dwc2_hsotg *hsotg,
 	dwc2_set_param_hibernation(hsotg, params->hibernation);
 }
 
+/*
+ * Forces either host or device mode if the controller is not
+ * currently in that mode.
+ *
+ * Returns true if the mode was forced.
+ */
+static bool dwc2_force_mode_if_needed(struct dwc2_hsotg *hsotg, bool host)
+{
+	if (host && dwc2_is_host_mode(hsotg))
+		return false;
+	else if (!host && dwc2_is_device_mode(hsotg))
+		return false;
+
+	return dwc2_force_mode(hsotg, host);
+}
+
+/*
+ * Gets host hardware parameters. Forces host mode if not currently in
+ * host mode. Should be called immediately after a core soft reset in
+ * order to get the reset values.
+ */
+static void dwc2_get_host_hwparams(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_hw_params *hw = &hsotg->hw_params;
+	u32 gnptxfsiz;
+	u32 hptxfsiz;
+	bool forced;
+
+	if (hsotg->dr_mode == USB_DR_MODE_PERIPHERAL)
+		return;
+
+	forced = dwc2_force_mode_if_needed(hsotg, true);
+
+	gnptxfsiz = dwc2_readl(hsotg->regs + GNPTXFSIZ);
+	hptxfsiz = dwc2_readl(hsotg->regs + HPTXFSIZ);
+	dev_dbg(hsotg->dev, "gnptxfsiz=%08x\n", gnptxfsiz);
+	dev_dbg(hsotg->dev, "hptxfsiz=%08x\n", hptxfsiz);
+
+	if (forced)
+		dwc2_clear_force_mode(hsotg);
+
+	hw->host_nperio_tx_fifo_size = (gnptxfsiz & FIFOSIZE_DEPTH_MASK) >>
+				       FIFOSIZE_DEPTH_SHIFT;
+	hw->host_perio_tx_fifo_size = (hptxfsiz & FIFOSIZE_DEPTH_MASK) >>
+				      FIFOSIZE_DEPTH_SHIFT;
+}
+
+/*
+ * Gets device hardware parameters. Forces device mode if not
+ * currently in device mode. Should be called immediately after a core
+ * soft reset in order to get the reset values.
+ */
+static void dwc2_get_dev_hwparams(struct dwc2_hsotg *hsotg)
+{
+	struct dwc2_hw_params *hw = &hsotg->hw_params;
+	bool forced;
+	u32 gnptxfsiz;
+
+	if (hsotg->dr_mode == USB_DR_MODE_HOST)
+		return;
+
+	forced = dwc2_force_mode_if_needed(hsotg, false);
+
+	gnptxfsiz = dwc2_readl(hsotg->regs + GNPTXFSIZ);
+	dev_dbg(hsotg->dev, "gnptxfsiz=%08x\n", gnptxfsiz);
+
+	if (forced)
+		dwc2_clear_force_mode(hsotg);
+
+	hw->dev_nperio_tx_fifo_size = (gnptxfsiz & FIFOSIZE_DEPTH_MASK) >>
+				       FIFOSIZE_DEPTH_SHIFT;
+}
+
 /**
  * During device initialization, read various hardware configuration
  * registers and interpret the contents.
@@ -3061,8 +3284,7 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	struct dwc2_hw_params *hw = &hsotg->hw_params;
 	unsigned width;
 	u32 hwcfg1, hwcfg2, hwcfg3, hwcfg4;
-	u32 hptxfsiz, grxfsiz, gnptxfsiz;
-	u32 gusbcfg;
+	u32 grxfsiz;
 
 	/*
 	 * Attempt to ensure this device is really a DWC_otg Controller.
@@ -3094,20 +3316,16 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	dev_dbg(hsotg->dev, "hwcfg4=%08x\n", hwcfg4);
 	dev_dbg(hsotg->dev, "grxfsiz=%08x\n", grxfsiz);
 
-	/* Force host mode to get HPTXFSIZ / GNPTXFSIZ exact power on value */
-	gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-	gusbcfg |= GUSBCFG_FORCEHOSTMODE;
-	dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
-	usleep_range(100000, 150000);
+	/*
+	 * Host specific hardware parameters. Reading these parameters
+	 * requires the controller to be in host mode. The mode will
+	 * be forced, if necessary, to read these values.
+	 */
+	dwc2_get_host_hwparams(hsotg);
+	dwc2_get_dev_hwparams(hsotg);
 
-	gnptxfsiz = dwc2_readl(hsotg->regs + GNPTXFSIZ);
-	hptxfsiz = dwc2_readl(hsotg->regs + HPTXFSIZ);
-	dev_dbg(hsotg->dev, "gnptxfsiz=%08x\n", gnptxfsiz);
-	dev_dbg(hsotg->dev, "hptxfsiz=%08x\n", hptxfsiz);
-	gusbcfg = dwc2_readl(hsotg->regs + GUSBCFG);
-	gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
-	dwc2_writel(gusbcfg, hsotg->regs + GUSBCFG);
-	usleep_range(100000, 150000);
+	/* hwcfg1 */
+	hw->dev_ep_dirs = hwcfg1;
 
 	/* hwcfg2 */
 	hw->op_mode = (hwcfg2 & GHWCFG2_OP_MODE_MASK) >>
@@ -3163,10 +3381,6 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	/* fifo sizes */
 	hw->host_rx_fifo_size = (grxfsiz & GRXFSIZ_DEPTH_MASK) >>
 				GRXFSIZ_DEPTH_SHIFT;
-	hw->host_nperio_tx_fifo_size = (gnptxfsiz & FIFOSIZE_DEPTH_MASK) >>
-				       FIFOSIZE_DEPTH_SHIFT;
-	hw->host_perio_tx_fifo_size = (hptxfsiz & FIFOSIZE_DEPTH_MASK) >>
-				      FIFOSIZE_DEPTH_SHIFT;
 
 	dev_dbg(hsotg->dev, "Detected values from hardware:\n");
 	dev_dbg(hsotg->dev, "  op_mode=%d\n",
@@ -3275,6 +3489,43 @@ void dwc2_disable_global_interrupts(struct dwc2_hsotg *hsotg)
 	dwc2_writel(ahbcfg, hsotg->regs + GAHBCFG);
 }
 
+/* Returns the controller's GHWCFG2.OTG_MODE. */
+unsigned dwc2_op_mode(struct dwc2_hsotg *hsotg)
+{
+	u32 ghwcfg2 = dwc2_readl(hsotg->regs + GHWCFG2);
+
+	return (ghwcfg2 & GHWCFG2_OP_MODE_MASK) >>
+		GHWCFG2_OP_MODE_SHIFT;
+}
+
+/* Returns true if the controller is capable of DRD. */
+bool dwc2_hw_is_otg(struct dwc2_hsotg *hsotg)
+{
+	unsigned op_mode = dwc2_op_mode(hsotg);
+
+	return (op_mode == GHWCFG2_OP_MODE_HNP_SRP_CAPABLE) ||
+		(op_mode == GHWCFG2_OP_MODE_SRP_ONLY_CAPABLE) ||
+		(op_mode == GHWCFG2_OP_MODE_NO_HNP_SRP_CAPABLE);
+}
+
+/* Returns true if the controller is host-only. */
+bool dwc2_hw_is_host(struct dwc2_hsotg *hsotg)
+{
+	unsigned op_mode = dwc2_op_mode(hsotg);
+
+	return (op_mode == GHWCFG2_OP_MODE_SRP_CAPABLE_HOST) ||
+		(op_mode == GHWCFG2_OP_MODE_NO_SRP_CAPABLE_HOST);
+}
+
+/* Returns true if the controller is device-only. */
+bool dwc2_hw_is_device(struct dwc2_hsotg *hsotg)
+{
+	unsigned op_mode = dwc2_op_mode(hsotg);
+
+	return (op_mode == GHWCFG2_OP_MODE_SRP_CAPABLE_DEVICE) ||
+		(op_mode == GHWCFG2_OP_MODE_NO_SRP_CAPABLE_DEVICE);
+}
+
 MODULE_DESCRIPTION("DESIGNWARE HS OTG Core");
 MODULE_AUTHOR("Synopsys, Inc.");
 MODULE_LICENSE("Dual BSD/GPL");
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index a66d3cb62b65..7fb6434f4639 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -246,6 +246,13 @@ enum dwc2_ep0_state {
  *                      value for this if none is specified.
  *                       0 - Address DMA
  *                       1 - Descriptor DMA (default, if available)
+ * @dma_desc_fs_enable: When DMA mode is enabled, specifies whether to use
+ *                      address DMA mode or descriptor DMA mode for accessing
+ *                      the data FIFOs in Full Speed mode only. The driver
+ *                      will automatically detect the value for this if none is
+ *                      specified.
+ *                       0 - Address DMA
+ *                       1 - Descriptor DMA in FS (default, if available)
  * @speed:              Specifies the maximum speed of operation in host and
  *                      device mode. The actual speed depends on the speed of
  *                      the attached device and the value of phy_type.
@@ -375,6 +382,7 @@ struct dwc2_core_params {
 	int otg_ver;
 	int dma_enable;
 	int dma_desc_enable;
+	int dma_desc_fs_enable;
 	int speed;
 	int enable_dynamic_fifo;
 	int en_multiple_tx_fifo;
@@ -451,15 +459,18 @@ struct dwc2_core_params {
  *                       1 - 16 bits
  *                       2 - 8 or 16 bits
  * @snpsid:             Value from SNPSID register
+ * @dev_ep_dirs:        Direction of device endpoints (GHWCFG1)
  */
 struct dwc2_hw_params {
 	unsigned op_mode:3;
 	unsigned arch:2;
 	unsigned dma_desc_enable:1;
+	unsigned dma_desc_fs_enable:1;
 	unsigned enable_dynamic_fifo:1;
 	unsigned en_multiple_tx_fifo:1;
 	unsigned host_rx_fifo_size:16;
 	unsigned host_nperio_tx_fifo_size:16;
+	unsigned dev_nperio_tx_fifo_size:16;
 	unsigned host_perio_tx_fifo_size:16;
 	unsigned nperio_tx_q_depth:3;
 	unsigned host_perio_tx_q_depth:3;
@@ -476,6 +487,7 @@ struct dwc2_hw_params {
 	unsigned power_optimized:1;
 	unsigned utmi_phy_data_width:2;
 	u32 snpsid;
+	u32 dev_ep_dirs;
 };
 
 /* Size of control and EP0 buffers */
@@ -676,6 +688,9 @@ struct dwc2_hregs_backup {
  * @otg_port:           OTG port number
  * @frame_list:         Frame list
  * @frame_list_dma:     Frame list DMA address
+ * @frame_list_sz:      Frame list size
+ * @desc_gen_cache:     Kmem cache for generic descriptors
+ * @desc_hsisoc_cache:  Kmem cache for hs isochronous descriptors
  *
  * These are for peripheral mode:
  *
@@ -770,6 +785,7 @@ struct dwc2_hsotg {
 	u16 frame_number;
 	u16 periodic_qh_count;
 	bool bus_suspended;
+	bool new_connection;
 
 #ifdef CONFIG_USB_DWC2_TRACK_MISSED_SOFS
 #define FRAME_NUM_ARRAY_SIZE 1000
@@ -794,6 +810,9 @@ struct dwc2_hsotg {
 	u8 otg_port;
 	u32 *frame_list;
 	dma_addr_t frame_list_dma;
+	u32 frame_list_sz;
+	struct kmem_cache *desc_gen_cache;
+	struct kmem_cache *desc_hsisoc_cache;
 
 #ifdef DEBUG
 	u32 frrem_samples;
@@ -864,10 +883,14 @@ enum dwc2_halt_status {
  * The following functions support initialization of the core driver component
  * and the DWC_otg controller
  */
+extern int dwc2_core_reset(struct dwc2_hsotg *hsotg);
+extern int dwc2_core_reset_and_force_dr_mode(struct dwc2_hsotg *hsotg);
 extern void dwc2_core_host_init(struct dwc2_hsotg *hsotg);
 extern int dwc2_enter_hibernation(struct dwc2_hsotg *hsotg);
 extern int dwc2_exit_hibernation(struct dwc2_hsotg *hsotg, bool restore);
 
+void dwc2_force_dr_mode(struct dwc2_hsotg *hsotg);
+
 /*
  * Host core Functions.
  * The following functions support managing the DWC_otg controller in host
@@ -901,7 +924,7 @@ extern void dwc2_read_packet(struct dwc2_hsotg *hsotg, u8 *dest, u16 bytes);
 extern void dwc2_flush_tx_fifo(struct dwc2_hsotg *hsotg, const int num);
 extern void dwc2_flush_rx_fifo(struct dwc2_hsotg *hsotg);
 
-extern int dwc2_core_init(struct dwc2_hsotg *hsotg, bool select_phy, int irq);
+extern int dwc2_core_init(struct dwc2_hsotg *hsotg, bool initial_setup);
 extern void dwc2_enable_global_interrupts(struct dwc2_hsotg *hcd);
 extern void dwc2_disable_global_interrupts(struct dwc2_hsotg *hcd);
 
@@ -942,6 +965,16 @@ extern void dwc2_set_param_dma_enable(struct dwc2_hsotg *hsotg, int val);
 extern void dwc2_set_param_dma_desc_enable(struct dwc2_hsotg *hsotg, int val);
 
 /*
+ * When DMA mode is enabled specifies whether to use
+ * address DMA or DMA Descritor mode with full speed devices
+ * for accessing the data FIFOs in host mode.
+ * 0 - address DMA
+ * 1 - FS DMA Descriptor(default, if available)
+ */
+extern void dwc2_set_param_dma_desc_fs_enable(struct dwc2_hsotg *hsotg,
+					      int val);
+
+/*
  * Specifies the maximum speed of operation in host and device mode.
  * The actual speed depends on the speed of the attached device and
  * the value of phy_type. The actual speed depends on the speed of the
@@ -1110,6 +1143,31 @@ extern int dwc2_lowlevel_hw_enable(struct dwc2_hsotg *hsotg);
 extern int dwc2_lowlevel_hw_disable(struct dwc2_hsotg *hsotg);
 
 /*
+ * The following functions check the controller's OTG operation mode
+ * capability (GHWCFG2.OTG_MODE).
+ *
+ * These functions can be used before the internal hsotg->hw_params
+ * are read in and cached so they always read directly from the
+ * GHWCFG2 register.
+ */
+unsigned dwc2_op_mode(struct dwc2_hsotg *hsotg);
+bool dwc2_hw_is_otg(struct dwc2_hsotg *hsotg);
+bool dwc2_hw_is_host(struct dwc2_hsotg *hsotg);
+bool dwc2_hw_is_device(struct dwc2_hsotg *hsotg);
+
+/*
+ * Returns the mode of operation, host or device
+ */
+static inline int dwc2_is_host_mode(struct dwc2_hsotg *hsotg)
+{
+	return (dwc2_readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) != 0;
+}
+static inline int dwc2_is_device_mode(struct dwc2_hsotg *hsotg)
+{
+	return (dwc2_readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) == 0;
+}
+
+/*
  * Dump core registers and SPRAM
  */
 extern void dwc2_dump_dev_registers(struct dwc2_hsotg *hsotg);
@@ -1154,12 +1212,14 @@ static inline int dwc2_hsotg_set_test_mode(struct dwc2_hsotg *hsotg,
 
 #if IS_ENABLED(CONFIG_USB_DWC2_HOST) || IS_ENABLED(CONFIG_USB_DWC2_DUAL_ROLE)
 extern int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg);
-extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_connect(struct dwc2_hsotg *hsotg);
+extern void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg, bool force);
 extern void dwc2_hcd_start(struct dwc2_hsotg *hsotg);
 #else
 static inline int dwc2_hcd_get_frame_number(struct dwc2_hsotg *hsotg)
 { return 0; }
-static inline void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg) {}
+static inline void dwc2_hcd_connect(struct dwc2_hsotg *hsotg) {}
+static inline void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg, bool force) {}
 static inline void dwc2_hcd_start(struct dwc2_hsotg *hsotg) {}
 static inline void dwc2_hcd_remove(struct dwc2_hsotg *hsotg) {}
 static inline int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
diff --git a/drivers/usb/dwc2/core_intr.c b/drivers/usb/dwc2/core_intr.c
index 27daa42788b1..d85c5c9f96c1 100644
--- a/drivers/usb/dwc2/core_intr.c
+++ b/drivers/usb/dwc2/core_intr.c
@@ -86,9 +86,6 @@ static void dwc2_handle_usb_port_intr(struct dwc2_hsotg *hsotg)
 		hprt0 &= ~HPRT0_ENA;
 		dwc2_writel(hprt0, hsotg->regs + HPRT0);
 	}
-
-	/* Clear interrupt */
-	dwc2_writel(GINTSTS_PRTINT, hsotg->regs + GINTSTS);
 }
 
 /**
@@ -98,11 +95,11 @@ static void dwc2_handle_usb_port_intr(struct dwc2_hsotg *hsotg)
  */
 static void dwc2_handle_mode_mismatch_intr(struct dwc2_hsotg *hsotg)
 {
-	dev_warn(hsotg->dev, "Mode Mismatch Interrupt: currently in %s mode\n",
-		 dwc2_is_host_mode(hsotg) ? "Host" : "Device");
-
 	/* Clear interrupt */
 	dwc2_writel(GINTSTS_MODEMIS, hsotg->regs + GINTSTS);
+
+	dev_warn(hsotg->dev, "Mode Mismatch Interrupt: currently in %s mode\n",
+		 dwc2_is_host_mode(hsotg) ? "Host" : "Device");
 }
 
 /**
@@ -239,7 +236,7 @@ static void dwc2_handle_otg_intr(struct dwc2_hsotg *hsotg)
 			dev_dbg(hsotg->dev, "a_suspend->a_peripheral (%d)\n",
 				hsotg->op_state);
 			spin_unlock(&hsotg->lock);
-			dwc2_hcd_disconnect(hsotg);
+			dwc2_hcd_disconnect(hsotg, false);
 			spin_lock(&hsotg->lock);
 			hsotg->op_state = OTG_STATE_A_PERIPHERAL;
 		} else {
@@ -276,9 +273,13 @@ static void dwc2_handle_otg_intr(struct dwc2_hsotg *hsotg)
  */
 static void dwc2_handle_conn_id_status_change_intr(struct dwc2_hsotg *hsotg)
 {
-	u32 gintmsk = dwc2_readl(hsotg->regs + GINTMSK);
+	u32 gintmsk;
+
+	/* Clear interrupt */
+	dwc2_writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
 
 	/* Need to disable SOF interrupt immediately */
+	gintmsk = dwc2_readl(hsotg->regs + GINTMSK);
 	gintmsk &= ~GINTSTS_SOF;
 	dwc2_writel(gintmsk, hsotg->regs + GINTMSK);
 
@@ -295,9 +296,6 @@ static void dwc2_handle_conn_id_status_change_intr(struct dwc2_hsotg *hsotg)
 		queue_work(hsotg->wq_otg, &hsotg->wf_otg);
 		spin_lock(&hsotg->lock);
 	}
-
-	/* Clear interrupt */
-	dwc2_writel(GINTSTS_CONIDSTSCHNG, hsotg->regs + GINTSTS);
 }
 
 /**
@@ -315,12 +313,12 @@ static void dwc2_handle_session_req_intr(struct dwc2_hsotg *hsotg)
 {
 	int ret;
 
-	dev_dbg(hsotg->dev, "Session request interrupt - lx_state=%d\n",
-							hsotg->lx_state);
-
 	/* Clear interrupt */
 	dwc2_writel(GINTSTS_SESSREQINT, hsotg->regs + GINTSTS);
 
+	dev_dbg(hsotg->dev, "Session request interrupt - lx_state=%d\n",
+							hsotg->lx_state);
+
 	if (dwc2_is_device_mode(hsotg)) {
 		if (hsotg->lx_state == DWC2_L2) {
 			ret = dwc2_exit_hibernation(hsotg, true);
@@ -347,6 +345,10 @@ static void dwc2_handle_session_req_intr(struct dwc2_hsotg *hsotg)
 static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
 {
 	int ret;
+
+	/* Clear interrupt */
+	dwc2_writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
+
 	dev_dbg(hsotg->dev, "++Resume or Remote Wakeup Detected Interrupt++\n");
 	dev_dbg(hsotg->dev, "%s lxstate = %d\n", __func__, hsotg->lx_state);
 
@@ -368,10 +370,9 @@ static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
 		/* Change to L0 state */
 		hsotg->lx_state = DWC2_L0;
 	} else {
-		if (hsotg->core_params->hibernation) {
-			dwc2_writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
+		if (hsotg->core_params->hibernation)
 			return;
-		}
+
 		if (hsotg->lx_state != DWC2_L1) {
 			u32 pcgcctl = dwc2_readl(hsotg->regs + PCGCTL);
 
@@ -385,9 +386,6 @@ static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
 			hsotg->lx_state = DWC2_L0;
 		}
 	}
-
-	/* Clear interrupt */
-	dwc2_writel(GINTSTS_WKUPINT, hsotg->regs + GINTSTS);
 }
 
 /*
@@ -396,14 +394,14 @@ static void dwc2_handle_wakeup_detected_intr(struct dwc2_hsotg *hsotg)
  */
 static void dwc2_handle_disconnect_intr(struct dwc2_hsotg *hsotg)
 {
+	dwc2_writel(GINTSTS_DISCONNINT, hsotg->regs + GINTSTS);
+
 	dev_dbg(hsotg->dev, "++Disconnect Detected Interrupt++ (%s) %s\n",
 		dwc2_is_host_mode(hsotg) ? "Host" : "Device",
 		dwc2_op_state_str(hsotg));
 
 	if (hsotg->op_state == OTG_STATE_A_HOST)
-		dwc2_hcd_disconnect(hsotg);
-
-	dwc2_writel(GINTSTS_DISCONNINT, hsotg->regs + GINTSTS);
+		dwc2_hcd_disconnect(hsotg, false);
 }
 
 /*
@@ -419,6 +417,9 @@ static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
 	u32 dsts;
 	int ret;
 
+	/* Clear interrupt */
+	dwc2_writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
+
 	dev_dbg(hsotg->dev, "USB SUSPEND\n");
 
 	if (dwc2_is_device_mode(hsotg)) {
@@ -437,7 +438,7 @@ static void dwc2_handle_usb_suspend_intr(struct dwc2_hsotg *hsotg)
 			if (!dwc2_is_device_connected(hsotg)) {
 				dev_dbg(hsotg->dev,
 						"ignore suspend request before enumeration\n");
-				goto clear_int;
+				return;
 			}
 
 			ret = dwc2_enter_hibernation(hsotg);
@@ -476,10 +477,6 @@ skip_power_saving:
 			hsotg->op_state = OTG_STATE_A_HOST;
 		}
 	}
-
-clear_int:
-	/* Clear interrupt */
-	dwc2_writel(GINTSTS_USBSUSP, hsotg->regs + GINTSTS);
 }
 
 #define GINTMSK_COMMON	(GINTSTS_WKUPINT | GINTSTS_SESSREQINT |		\
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 0abf73c91beb..422ab7da4eb5 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -2095,7 +2095,7 @@ static void dwc2_hsotg_irq_enumdone(struct dwc2_hsotg *hsotg)
 	 */
 
 	/* catch both EnumSpd_FS and EnumSpd_FS48 */
-	switch (dsts & DSTS_ENUMSPD_MASK) {
+	switch ((dsts & DSTS_ENUMSPD_MASK) >> DSTS_ENUMSPD_SHIFT) {
 	case DSTS_ENUMSPD_FS:
 	case DSTS_ENUMSPD_FS48:
 		hsotg->gadget.speed = USB_SPEED_FULL;
@@ -2244,54 +2244,6 @@ static void dwc2_hsotg_irq_fifoempty(struct dwc2_hsotg *hsotg, bool periodic)
 			GINTSTS_RXFLVL)
 
 /**
- * dwc2_hsotg_corereset - issue softreset to the core
- * @hsotg: The device state
- *
- * Issue a soft reset to the core, and await the core finishing it.
- */
-static int dwc2_hsotg_corereset(struct dwc2_hsotg *hsotg)
-{
-	int timeout;
-	u32 grstctl;
-
-	dev_dbg(hsotg->dev, "resetting core\n");
-
-	/* issue soft reset */
-	dwc2_writel(GRSTCTL_CSFTRST, hsotg->regs + GRSTCTL);
-
-	timeout = 10000;
-	do {
-		grstctl = dwc2_readl(hsotg->regs + GRSTCTL);
-	} while ((grstctl & GRSTCTL_CSFTRST) && timeout-- > 0);
-
-	if (grstctl & GRSTCTL_CSFTRST) {
-		dev_err(hsotg->dev, "Failed to get CSftRst asserted\n");
-		return -EINVAL;
-	}
-
-	timeout = 10000;
-
-	while (1) {
-		u32 grstctl = dwc2_readl(hsotg->regs + GRSTCTL);
-
-		if (timeout-- < 0) {
-			dev_info(hsotg->dev,
-				 "%s: reset failed, GRSTCTL=%08x\n",
-				 __func__, grstctl);
-			return -ETIMEDOUT;
-		}
-
-		if (!(grstctl & GRSTCTL_AHBIDLE))
-			continue;
-
-		break;		/* reset done */
-	}
-
-	dev_dbg(hsotg->dev, "reset successful\n");
-	return 0;
-}
-
-/**
  * dwc2_hsotg_core_init - issue softreset to the core
  * @hsotg: The device state
  *
@@ -2307,7 +2259,7 @@ void dwc2_hsotg_core_init_disconnected(struct dwc2_hsotg *hsotg,
 	kill_all_requests(hsotg, hsotg->eps_out[0], -ECONNRESET);
 
 	if (!is_usb_reset)
-		if (dwc2_hsotg_corereset(hsotg))
+		if (dwc2_core_reset(hsotg))
 			return;
 
 	/*
@@ -2585,7 +2537,7 @@ irq_retry:
 	if (gintsts & GINTSTS_GOUTNAKEFF) {
 		dev_info(hsotg->dev, "GOUTNakEff triggered\n");
 
-		dwc2_writel(DCTL_CGOUTNAK, hsotg->regs + DCTL);
+		__orr32(hsotg->regs + DCTL, DCTL_CGOUTNAK);
 
 		dwc2_hsotg_dump(hsotg);
 	}
@@ -2593,7 +2545,7 @@ irq_retry:
 	if (gintsts & GINTSTS_GINNAKEFF) {
 		dev_info(hsotg->dev, "GINNakEff triggered\n");
 
-		dwc2_writel(DCTL_CGNPINNAK, hsotg->regs + DCTL);
+		__orr32(hsotg->regs + DCTL, DCTL_CGNPINNAK);
 
 		dwc2_hsotg_dump(hsotg);
 	}
@@ -2911,15 +2863,15 @@ static void dwc2_hsotg_ep_stop_xfr(struct dwc2_hsotg *hsotg,
 				"%s: timeout DIEPINT.NAKEFF\n", __func__);
 	} else {
 		/* Clear any pending nak effect interrupt */
-		dwc2_writel(GINTSTS_GINNAKEFF, hsotg->regs + GINTSTS);
+		dwc2_writel(GINTSTS_GOUTNAKEFF, hsotg->regs + GINTSTS);
 
-		__orr32(hsotg->regs + DCTL, DCTL_SGNPINNAK);
+		__orr32(hsotg->regs + DCTL, DCTL_SGOUTNAK);
 
 		/* Wait for global nak to take effect */
 		if (dwc2_hsotg_wait_bit_set(hsotg, GINTSTS,
-						GINTSTS_GINNAKEFF, 100))
+						GINTSTS_GOUTNAKEFF, 100))
 			dev_warn(hsotg->dev,
-				"%s: timeout GINTSTS.GINNAKEFF\n", __func__);
+				"%s: timeout GINTSTS.GOUTNAKEFF\n", __func__);
 	}
 
 	/* Disable ep */
@@ -2944,7 +2896,7 @@ static void dwc2_hsotg_ep_stop_xfr(struct dwc2_hsotg *hsotg,
 		/* TODO: Flush shared tx fifo */
 	} else {
 		/* Remove global NAKs */
-		__bic32(hsotg->regs + DCTL, DCTL_SGNPINNAK);
+		__bic32(hsotg->regs + DCTL, DCTL_SGOUTNAK);
 	}
 }
 
@@ -3403,8 +3355,8 @@ static int dwc2_hsotg_hw_cfg(struct dwc2_hsotg *hsotg)
 
 	/* check hardware configuration */
 
-	cfg = dwc2_readl(hsotg->regs + GHWCFG2);
-	hsotg->num_of_eps = (cfg >> GHWCFG2_NUM_DEV_EP_SHIFT) & 0xF;
+	hsotg->num_of_eps = hsotg->hw_params.num_dev_ep;
+
 	/* Add ep0 */
 	hsotg->num_of_eps++;
 
@@ -3415,7 +3367,7 @@ static int dwc2_hsotg_hw_cfg(struct dwc2_hsotg *hsotg)
 	/* Same dwc2_hsotg_ep is used in both directions for ep0 */
 	hsotg->eps_out[0] = hsotg->eps_in[0];
 
-	cfg = dwc2_readl(hsotg->regs + GHWCFG1);
+	cfg = hsotg->hw_params.dev_ep_dirs;
 	for (i = 1, cfg >>= 2; i < hsotg->num_of_eps; i++, cfg >>= 2) {
 		ep_type = cfg & 3;
 		/* Direction in or both */
@@ -3434,11 +3386,8 @@ static int dwc2_hsotg_hw_cfg(struct dwc2_hsotg *hsotg)
 		}
 	}
 
-	cfg = dwc2_readl(hsotg->regs + GHWCFG3);
-	hsotg->fifo_mem = (cfg >> GHWCFG3_DFIFO_DEPTH_SHIFT);
-
-	cfg = dwc2_readl(hsotg->regs + GHWCFG4);
-	hsotg->dedicated_fifos = (cfg >> GHWCFG4_DED_FIFO_SHIFT) & 1;
+	hsotg->fifo_mem = hsotg->hw_params.total_fifo_size;
+	hsotg->dedicated_fifos = hsotg->hw_params.en_multiple_tx_fifo;
 
 	dev_info(hsotg->dev, "EPs: %d, %s fifos, %d entries in SPRAM\n",
 		 hsotg->num_of_eps,
@@ -3563,6 +3512,17 @@ int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 	memcpy(&hsotg->g_tx_fifo_sz[1], p_tx_fifo, sizeof(p_tx_fifo));
 	/* Device tree specific probe */
 	dwc2_hsotg_of_probe(hsotg);
+
+	/* Check against largest possible value. */
+	if (hsotg->g_np_g_tx_fifo_sz >
+	    hsotg->hw_params.dev_nperio_tx_fifo_size) {
+		dev_warn(dev, "Specified GNPTXFDEP=%d > %d\n",
+			 hsotg->g_np_g_tx_fifo_sz,
+			 hsotg->hw_params.dev_nperio_tx_fifo_size);
+		hsotg->g_np_g_tx_fifo_sz =
+			hsotg->hw_params.dev_nperio_tx_fifo_size;
+	}
+
 	/* Dump fifo information */
 	dev_dbg(dev, "NonPeriodic TXFIFO size: %d\n",
 						hsotg->g_np_g_tx_fifo_sz);
@@ -3579,31 +3539,12 @@ int dwc2_gadget_init(struct dwc2_hsotg *hsotg, int irq)
 	else if (hsotg->dr_mode == USB_DR_MODE_PERIPHERAL)
 		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
 
-	/*
-	 * Force Device mode before initialization.
-	 * This allows correctly configuring fifo for device mode.
-	 */
-	__bic32(hsotg->regs + GUSBCFG, GUSBCFG_FORCEHOSTMODE);
-	__orr32(hsotg->regs + GUSBCFG, GUSBCFG_FORCEDEVMODE);
-
-	/*
-	 * According to Synopsys databook, this sleep is needed for the force
-	 * device mode to take effect.
-	 */
-	msleep(25);
-
-	dwc2_hsotg_corereset(hsotg);
 	ret = dwc2_hsotg_hw_cfg(hsotg);
 	if (ret) {
 		dev_err(hsotg->dev, "Hardware configuration failed: %d\n", ret);
 		return ret;
 	}
 
-	dwc2_hsotg_init(hsotg);
-
-	/* Switch back to default configuration */
-	__bic32(hsotg->regs + GUSBCFG, GUSBCFG_FORCEDEVMODE);
-
 	hsotg->ctrl_buff = devm_kzalloc(hsotg->dev,
 			DWC2_CTRL_BUFF_SIZE, GFP_KERNEL);
 	if (!hsotg->ctrl_buff) {
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index 571c21727ff9..8847c72e55f6 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -268,15 +268,33 @@ static void dwc2_hcd_cleanup_channels(struct dwc2_hsotg *hsotg)
 }
 
 /**
+ * dwc2_hcd_connect() - Handles connect of the HCD
+ *
+ * @hsotg: Pointer to struct dwc2_hsotg
+ *
+ * Must be called with interrupt disabled and spinlock held
+ */
+void dwc2_hcd_connect(struct dwc2_hsotg *hsotg)
+{
+	if (hsotg->lx_state != DWC2_L0)
+		usb_hcd_resume_root_hub(hsotg->priv);
+
+	hsotg->flags.b.port_connect_status_change = 1;
+	hsotg->flags.b.port_connect_status = 1;
+}
+
+/**
  * dwc2_hcd_disconnect() - Handles disconnect of the HCD
  *
  * @hsotg: Pointer to struct dwc2_hsotg
+ * @force: If true, we won't try to reconnect even if we see device connected.
  *
  * Must be called with interrupt disabled and spinlock held
  */
-void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg)
+void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg, bool force)
 {
 	u32 intr;
+	u32 hprt0;
 
 	/* Set status flags for the hub driver */
 	hsotg->flags.b.port_connect_status_change = 1;
@@ -315,6 +333,24 @@ void dwc2_hcd_disconnect(struct dwc2_hsotg *hsotg)
 		dwc2_hcd_cleanup_channels(hsotg);
 
 	dwc2_host_disconnect(hsotg);
+
+	/*
+	 * Add an extra check here to see if we're actually connected but
+	 * we don't have a detection interrupt pending.  This can happen if:
+	 *   1. hardware sees connect
+	 *   2. hardware sees disconnect
+	 *   3. hardware sees connect
+	 *   4. dwc2_port_intr() - clears connect interrupt
+	 *   5. dwc2_handle_common_intr() - calls here
+	 *
+	 * Without the extra check here we will end calling disconnect
+	 * and won't get any future interrupts to handle the connect.
+	 */
+	if (!force) {
+		hprt0 = dwc2_readl(hsotg->regs + HPRT0);
+		if (!(hprt0 & HPRT0_CONNDET) && (hprt0 & HPRT0_CONNSTS))
+			dwc2_hcd_connect(hsotg);
+	}
 }
 
 /**
@@ -881,8 +917,10 @@ static int dwc2_assign_and_init_hc(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 		 */
 		chan->multi_count = dwc2_hb_mult(qh->maxp);
 
-	if (hsotg->core_params->dma_desc_enable > 0)
+	if (hsotg->core_params->dma_desc_enable > 0) {
 		chan->desc_list_addr = qh->desc_list_dma;
+		chan->desc_list_sz = qh->desc_list_sz;
+	}
 
 	dwc2_hc_init(hsotg, chan);
 	chan->qh = qh;
@@ -1382,7 +1420,7 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 			dev_err(hsotg->dev,
 				"Connection id status change timed out\n");
 		hsotg->op_state = OTG_STATE_B_PERIPHERAL;
-		dwc2_core_init(hsotg, false, -1);
+		dwc2_core_init(hsotg, false);
 		dwc2_enable_global_interrupts(hsotg);
 		spin_lock_irqsave(&hsotg->lock, flags);
 		dwc2_hsotg_core_init_disconnected(hsotg, false);
@@ -1405,7 +1443,7 @@ static void dwc2_conn_id_status_change(struct work_struct *work)
 		hsotg->op_state = OTG_STATE_A_HOST;
 
 		/* Initialize the Core for Host mode */
-		dwc2_core_init(hsotg, false, -1);
+		dwc2_core_init(hsotg, false);
 		dwc2_enable_global_interrupts(hsotg);
 		dwc2_hcd_start(hsotg);
 	}
@@ -1734,6 +1772,28 @@ static int dwc2_hcd_hub_control(struct dwc2_hsotg *hsotg, u16 typereq,
 			port_status |= USB_PORT_STAT_TEST;
 		/* USB_PORT_FEAT_INDICATOR unsupported always 0 */
 
+		if (hsotg->core_params->dma_desc_fs_enable) {
+			/*
+			 * Enable descriptor DMA only if a full speed
+			 * device is connected.
+			 */
+			if (hsotg->new_connection &&
+			    ((port_status &
+			      (USB_PORT_STAT_CONNECTION |
+			       USB_PORT_STAT_HIGH_SPEED |
+			       USB_PORT_STAT_LOW_SPEED)) ==
+			       USB_PORT_STAT_CONNECTION)) {
+				u32 hcfg;
+
+				dev_info(hsotg->dev, "Enabling descriptor DMA mode\n");
+				hsotg->core_params->dma_desc_enable = 1;
+				hcfg = dwc2_readl(hsotg->regs + HCFG);
+				hcfg |= HCFG_DESCDMA;
+				dwc2_writel(hcfg, hsotg->regs + HCFG);
+				hsotg->new_connection = false;
+			}
+		}
+
 		dev_vdbg(hsotg->dev, "port_status=%08x\n", port_status);
 		*(__le32 *)buf = cpu_to_le32(port_status);
 		break;
@@ -2298,13 +2358,19 @@ static void dwc2_hcd_reset_func(struct work_struct *work)
 {
 	struct dwc2_hsotg *hsotg = container_of(work, struct dwc2_hsotg,
 						reset_work.work);
+	unsigned long flags;
 	u32 hprt0;
 
 	dev_dbg(hsotg->dev, "USB RESET function called\n");
+
+	spin_lock_irqsave(&hsotg->lock, flags);
+
 	hprt0 = dwc2_read_hprt0(hsotg);
 	hprt0 &= ~HPRT0_RST;
 	dwc2_writel(hprt0, hsotg->regs + HPRT0);
 	hsotg->flags.b.port_reset_change = 1;
+
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 }
 
 /*
@@ -2366,7 +2432,7 @@ static void _dwc2_hcd_stop(struct usb_hcd *hcd)
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 	/* Ensure hcd is disconnected */
-	dwc2_hcd_disconnect(hsotg);
+	dwc2_hcd_disconnect(hsotg, true);
 	dwc2_hcd_stop(hsotg);
 	hsotg->lx_state = DWC2_L3;
 	hcd->state = HC_STATE_HALT;
@@ -3054,7 +3120,7 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 	dwc2_disable_global_interrupts(hsotg);
 
 	/* Initialize the DWC_otg core, and select the Phy type */
-	retval = dwc2_core_init(hsotg, true, irq);
+	retval = dwc2_core_init(hsotg, true);
 	if (retval)
 		goto error2;
 
@@ -3122,6 +3188,47 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 	if (!hsotg->status_buf)
 		goto error3;
 
+	/*
+	 * Create kmem caches to handle descriptor buffers in descriptor
+	 * DMA mode.
+	 * Alignment must be set to 512 bytes.
+	 */
+	if (hsotg->core_params->dma_desc_enable ||
+	    hsotg->core_params->dma_desc_fs_enable) {
+		hsotg->desc_gen_cache = kmem_cache_create("dwc2-gen-desc",
+				sizeof(struct dwc2_hcd_dma_desc) *
+				MAX_DMA_DESC_NUM_GENERIC, 512, SLAB_CACHE_DMA,
+				NULL);
+		if (!hsotg->desc_gen_cache) {
+			dev_err(hsotg->dev,
+				"unable to create dwc2 generic desc cache\n");
+
+			/*
+			 * Disable descriptor dma mode since it will not be
+			 * usable.
+			 */
+			hsotg->core_params->dma_desc_enable = 0;
+			hsotg->core_params->dma_desc_fs_enable = 0;
+		}
+
+		hsotg->desc_hsisoc_cache = kmem_cache_create("dwc2-hsisoc-desc",
+				sizeof(struct dwc2_hcd_dma_desc) *
+				MAX_DMA_DESC_NUM_HS_ISOC, 512, 0, NULL);
+		if (!hsotg->desc_hsisoc_cache) {
+			dev_err(hsotg->dev,
+				"unable to create dwc2 hs isoc desc cache\n");
+
+			kmem_cache_destroy(hsotg->desc_gen_cache);
+
+			/*
+			 * Disable descriptor dma mode since it will not be
+			 * usable.
+			 */
+			hsotg->core_params->dma_desc_enable = 0;
+			hsotg->core_params->dma_desc_fs_enable = 0;
+		}
+	}
+
 	hsotg->otg_port = 1;
 	hsotg->frame_list = NULL;
 	hsotg->frame_list_dma = 0;
@@ -3145,7 +3252,7 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 	 */
 	retval = usb_add_hcd(hcd, irq, IRQF_SHARED);
 	if (retval < 0)
-		goto error3;
+		goto error4;
 
 	device_wakeup_enable(hcd->self.controller);
 
@@ -3155,6 +3262,9 @@ int dwc2_hcd_init(struct dwc2_hsotg *hsotg, int irq)
 
 	return 0;
 
+error4:
+	kmem_cache_destroy(hsotg->desc_gen_cache);
+	kmem_cache_destroy(hsotg->desc_hsisoc_cache);
 error3:
 	dwc2_hcd_release(hsotg);
 error2:
@@ -3195,6 +3305,10 @@ void dwc2_hcd_remove(struct dwc2_hsotg *hsotg)
 
 	usb_remove_hcd(hcd);
 	hsotg->priv = NULL;
+
+	kmem_cache_destroy(hsotg->desc_gen_cache);
+	kmem_cache_destroy(hsotg->desc_hsisoc_cache);
+
 	dwc2_hcd_release(hsotg);
 	usb_put_hcd(hcd);
 
diff --git a/drivers/usb/dwc2/hcd.h b/drivers/usb/dwc2/hcd.h
index f105bada2fd1..8f0a29cefdf7 100644
--- a/drivers/usb/dwc2/hcd.h
+++ b/drivers/usb/dwc2/hcd.h
@@ -107,6 +107,7 @@ struct dwc2_qh;
  * @qh:                 QH for the transfer being processed by this channel
  * @hc_list_entry:      For linking to list of host channels
  * @desc_list_addr:     Current QH's descriptor list DMA address
+ * @desc_list_sz:       Current QH's descriptor list size
  *
  * This structure represents the state of a single host channel when acting in
  * host mode. It contains the data items needed to transfer packets to an
@@ -159,6 +160,7 @@ struct dwc2_host_chan {
 	struct dwc2_qh *qh;
 	struct list_head hc_list_entry;
 	dma_addr_t desc_list_addr;
+	u32 desc_list_sz;
 };
 
 struct dwc2_hcd_pipe_info {
@@ -251,6 +253,7 @@ enum dwc2_transaction_type {
  *                      schedule
  * @desc_list:          List of transfer descriptors
  * @desc_list_dma:      Physical address of desc_list
+ * @desc_list_sz:       Size of descriptors list
  * @n_bytes:            Xfer Bytes array. Each element corresponds to a transfer
  *                      descriptor and indicates original XferSize value for the
  *                      descriptor
@@ -284,6 +287,7 @@ struct dwc2_qh {
 	struct list_head qh_list_entry;
 	struct dwc2_hcd_dma_desc *desc_list;
 	dma_addr_t desc_list_dma;
+	u32 desc_list_sz;
 	u32 *n_bytes;
 	unsigned tt_buffer_dirty:1;
 };
@@ -340,6 +344,8 @@ struct dwc2_qtd {
 	u8 isoc_split_pos;
 	u16 isoc_frame_index;
 	u16 isoc_split_offset;
+	u16 isoc_td_last;
+	u16 isoc_td_first;
 	u32 ssplit_out_xfer_count;
 	u8 error_count;
 	u8 n_desc;
@@ -378,18 +384,6 @@ static inline void disable_hc_int(struct dwc2_hsotg *hsotg, int chnum, u32 intr)
 }
 
 /*
- * Returns the mode of operation, host or device
- */
-static inline int dwc2_is_host_mode(struct dwc2_hsotg *hsotg)
-{
-	return (dwc2_readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) != 0;
-}
-static inline int dwc2_is_device_mode(struct dwc2_hsotg *hsotg)
-{
-	return (dwc2_readl(hsotg->regs + GINTSTS) & GINTSTS_CURMODE_HOST) == 0;
-}
-
-/*
  * Reads HPRT0 in preparation to modify. It keeps the WC bits 0 so that if they
  * are read as 1, they won't clear when written back.
  */
@@ -535,6 +529,19 @@ static inline bool dbg_perio(void) { return false; }
 #define dwc2_max_packet(wmaxpacketsize) ((wmaxpacketsize) & 0x07ff)
 
 /*
+ * Returns true if frame1 index is greater than frame2 index. The comparison
+ * is done modulo FRLISTEN_64_SIZE. This accounts for the rollover of the
+ * frame number when the max index frame number is reached.
+ */
+static inline bool dwc2_frame_idx_num_gt(u16 fr_idx1, u16 fr_idx2)
+{
+	u16 diff = fr_idx1 - fr_idx2;
+	u16 sign = diff & (FRLISTEN_64_SIZE >> 1);
+
+	return diff && !sign;
+}
+
+/*
  * Returns true if frame1 is less than or equal to frame2. The comparison is
  * done modulo HFNUM_MAX_FRNUM. This accounts for the rollover of the
  * frame number when the max frame number is reached.
diff --git a/drivers/usb/dwc2/hcd_ddma.c b/drivers/usb/dwc2/hcd_ddma.c
index 78993aba9335..a41274aa52ad 100644
--- a/drivers/usb/dwc2/hcd_ddma.c
+++ b/drivers/usb/dwc2/hcd_ddma.c
@@ -87,22 +87,31 @@ static u16 dwc2_frame_incr_val(struct dwc2_qh *qh)
 static int dwc2_desc_list_alloc(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
 				gfp_t flags)
 {
-	qh->desc_list = dma_alloc_coherent(hsotg->dev,
-				sizeof(struct dwc2_hcd_dma_desc) *
-				dwc2_max_desc_num(qh), &qh->desc_list_dma,
-				flags);
+	struct kmem_cache *desc_cache;
 
+	if (qh->ep_type == USB_ENDPOINT_XFER_ISOC
+	    && qh->dev_speed == USB_SPEED_HIGH)
+		desc_cache = hsotg->desc_hsisoc_cache;
+	else
+		desc_cache = hsotg->desc_gen_cache;
+
+	qh->desc_list_sz = sizeof(struct dwc2_hcd_dma_desc) *
+						dwc2_max_desc_num(qh);
+
+	qh->desc_list = kmem_cache_zalloc(desc_cache, flags | GFP_DMA);
 	if (!qh->desc_list)
 		return -ENOMEM;
 
-	memset(qh->desc_list, 0,
-	       sizeof(struct dwc2_hcd_dma_desc) * dwc2_max_desc_num(qh));
+	qh->desc_list_dma = dma_map_single(hsotg->dev, qh->desc_list,
+					   qh->desc_list_sz,
+					   DMA_TO_DEVICE);
 
 	qh->n_bytes = kzalloc(sizeof(u32) * dwc2_max_desc_num(qh), flags);
 	if (!qh->n_bytes) {
-		dma_free_coherent(hsotg->dev, sizeof(struct dwc2_hcd_dma_desc)
-				  * dwc2_max_desc_num(qh), qh->desc_list,
-				  qh->desc_list_dma);
+		dma_unmap_single(hsotg->dev, qh->desc_list_dma,
+				 qh->desc_list_sz,
+				 DMA_FROM_DEVICE);
+		kfree(qh->desc_list);
 		qh->desc_list = NULL;
 		return -ENOMEM;
 	}
@@ -112,10 +121,18 @@ static int dwc2_desc_list_alloc(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
 
 static void dwc2_desc_list_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 {
+	struct kmem_cache *desc_cache;
+
+	if (qh->ep_type == USB_ENDPOINT_XFER_ISOC
+	    && qh->dev_speed == USB_SPEED_HIGH)
+		desc_cache = hsotg->desc_hsisoc_cache;
+	else
+		desc_cache = hsotg->desc_gen_cache;
+
 	if (qh->desc_list) {
-		dma_free_coherent(hsotg->dev, sizeof(struct dwc2_hcd_dma_desc)
-				  * dwc2_max_desc_num(qh), qh->desc_list,
-				  qh->desc_list_dma);
+		dma_unmap_single(hsotg->dev, qh->desc_list_dma,
+				 qh->desc_list_sz, DMA_FROM_DEVICE);
+		kmem_cache_free(desc_cache, qh->desc_list);
 		qh->desc_list = NULL;
 	}
 
@@ -128,21 +145,20 @@ static int dwc2_frame_list_alloc(struct dwc2_hsotg *hsotg, gfp_t mem_flags)
 	if (hsotg->frame_list)
 		return 0;
 
-	hsotg->frame_list = dma_alloc_coherent(hsotg->dev,
-					       4 * FRLISTEN_64_SIZE,
-					       &hsotg->frame_list_dma,
-					       mem_flags);
+	hsotg->frame_list_sz = 4 * FRLISTEN_64_SIZE;
+	hsotg->frame_list = kzalloc(hsotg->frame_list_sz, GFP_ATOMIC | GFP_DMA);
 	if (!hsotg->frame_list)
 		return -ENOMEM;
 
-	memset(hsotg->frame_list, 0, 4 * FRLISTEN_64_SIZE);
+	hsotg->frame_list_dma = dma_map_single(hsotg->dev, hsotg->frame_list,
+					       hsotg->frame_list_sz,
+					       DMA_TO_DEVICE);
+
 	return 0;
 }
 
 static void dwc2_frame_list_free(struct dwc2_hsotg *hsotg)
 {
-	u32 *frame_list;
-	dma_addr_t frame_list_dma;
 	unsigned long flags;
 
 	spin_lock_irqsave(&hsotg->lock, flags);
@@ -152,14 +168,14 @@ static void dwc2_frame_list_free(struct dwc2_hsotg *hsotg)
 		return;
 	}
 
-	frame_list = hsotg->frame_list;
-	frame_list_dma = hsotg->frame_list_dma;
+	dma_unmap_single(hsotg->dev, hsotg->frame_list_dma,
+			 hsotg->frame_list_sz, DMA_FROM_DEVICE);
+
+	kfree(hsotg->frame_list);
 	hsotg->frame_list = NULL;
 
 	spin_unlock_irqrestore(&hsotg->lock, flags);
 
-	dma_free_coherent(hsotg->dev, 4 * FRLISTEN_64_SIZE, frame_list,
-			  frame_list_dma);
 }
 
 static void dwc2_per_sched_enable(struct dwc2_hsotg *hsotg, u32 fr_list_en)
@@ -249,6 +265,15 @@ static void dwc2_update_frame_list(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
 		j = (j + inc) & (FRLISTEN_64_SIZE - 1);
 	} while (j != i);
 
+	/*
+	 * Sync frame list since controller will access it if periodic
+	 * channel is currently enabled.
+	 */
+	dma_sync_single_for_device(hsotg->dev,
+				   hsotg->frame_list_dma,
+				   hsotg->frame_list_sz,
+				   DMA_TO_DEVICE);
+
 	if (!enable)
 		return;
 
@@ -278,6 +303,7 @@ static void dwc2_release_channel_ddma(struct dwc2_hsotg *hsotg,
 			hsotg->non_periodic_channels--;
 	} else {
 		dwc2_update_frame_list(hsotg, qh, 0);
+		hsotg->available_host_channels++;
 	}
 
 	/*
@@ -360,6 +386,8 @@ err0:
  */
 void dwc2_hcd_qh_free_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 {
+	unsigned long flags;
+
 	dwc2_desc_list_free(hsotg, qh);
 
 	/*
@@ -369,8 +397,10 @@ void dwc2_hcd_qh_free_ddma(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 	 * when it comes here from endpoint disable routine
 	 * channel remains assigned.
 	 */
+	spin_lock_irqsave(&hsotg->lock, flags);
 	if (qh->channel)
 		dwc2_release_channel_ddma(hsotg, qh);
+	spin_unlock_irqrestore(&hsotg->lock, flags);
 
 	if ((qh->ep_type == USB_ENDPOINT_XFER_ISOC ||
 	     qh->ep_type == USB_ENDPOINT_XFER_INT) &&
@@ -524,14 +554,23 @@ static void dwc2_fill_host_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 	dma_desc->status = qh->n_bytes[idx] << HOST_DMA_ISOC_NBYTES_SHIFT &
 			   HOST_DMA_ISOC_NBYTES_MASK;
 
+	/* Set active bit */
+	dma_desc->status |= HOST_DMA_A;
+
+	qh->ntd++;
+	qtd->isoc_frame_index_last++;
+
 #ifdef ISOC_URB_GIVEBACK_ASAP
 	/* Set IOC for each descriptor corresponding to last frame of URB */
 	if (qtd->isoc_frame_index_last == qtd->urb->packet_count)
 		dma_desc->status |= HOST_DMA_IOC;
 #endif
 
-	qh->ntd++;
-	qtd->isoc_frame_index_last++;
+	dma_sync_single_for_device(hsotg->dev,
+			qh->desc_list_dma +
+			(idx * sizeof(struct dwc2_hcd_dma_desc)),
+			sizeof(struct dwc2_hcd_dma_desc),
+			DMA_TO_DEVICE);
 }
 
 static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
@@ -539,11 +578,32 @@ static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 {
 	struct dwc2_qtd *qtd;
 	u32 max_xfer_size;
-	u16 idx, inc, n_desc, ntd_max = 0;
+	u16 idx, inc, n_desc = 0, ntd_max = 0;
+	u16 cur_idx;
+	u16 next_idx;
 
 	idx = qh->td_last;
 	inc = qh->interval;
-	n_desc = 0;
+	hsotg->frame_number = dwc2_hcd_get_frame_number(hsotg);
+	cur_idx = dwc2_frame_list_idx(hsotg->frame_number);
+	next_idx = dwc2_desclist_idx_inc(qh->td_last, inc, qh->dev_speed);
+
+	/*
+	 * Ensure current frame number didn't overstep last scheduled
+	 * descriptor. If it happens, the only way to recover is to move
+	 * qh->td_last to current frame number + 1.
+	 * So that next isoc descriptor will be scheduled on frame number + 1
+	 * and not on a past frame.
+	 */
+	if (dwc2_frame_idx_num_gt(cur_idx, next_idx) || (cur_idx == next_idx)) {
+		if (inc < 32) {
+			dev_vdbg(hsotg->dev,
+				 "current frame number overstep last descriptor\n");
+			qh->td_last = dwc2_desclist_idx_inc(cur_idx, inc,
+							    qh->dev_speed);
+			idx = qh->td_last;
+		}
+	}
 
 	if (qh->interval) {
 		ntd_max = (dwc2_max_desc_num(qh) + qh->interval - 1) /
@@ -556,15 +616,20 @@ static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 			MAX_ISOC_XFER_SIZE_HS : MAX_ISOC_XFER_SIZE_FS;
 
 	list_for_each_entry(qtd, &qh->qtd_list, qtd_list_entry) {
+		if (qtd->in_process &&
+		    qtd->isoc_frame_index_last ==
+		    qtd->urb->packet_count)
+			continue;
+
+		qtd->isoc_td_first = idx;
 		while (qh->ntd < ntd_max && qtd->isoc_frame_index_last <
 						qtd->urb->packet_count) {
-			if (n_desc > 1)
-				qh->desc_list[n_desc - 1].status |= HOST_DMA_A;
 			dwc2_fill_host_isoc_dma_desc(hsotg, qtd, qh,
 						     max_xfer_size, idx);
 			idx = dwc2_desclist_idx_inc(idx, inc, qh->dev_speed);
 			n_desc++;
 		}
+		qtd->isoc_td_last = idx;
 		qtd->in_process = 1;
 	}
 
@@ -575,6 +640,11 @@ static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 	if (qh->ntd == ntd_max) {
 		idx = dwc2_desclist_idx_dec(qh->td_last, inc, qh->dev_speed);
 		qh->desc_list[idx].status |= HOST_DMA_IOC;
+		dma_sync_single_for_device(hsotg->dev,
+					   qh->desc_list_dma + (idx *
+					   sizeof(struct dwc2_hcd_dma_desc)),
+					   sizeof(struct dwc2_hcd_dma_desc),
+					   DMA_TO_DEVICE);
 	}
 #else
 	/*
@@ -604,13 +674,12 @@ static void dwc2_init_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 		idx = dwc2_desclist_idx_dec(qh->td_last, inc, qh->dev_speed);
 
 	qh->desc_list[idx].status |= HOST_DMA_IOC;
+	dma_sync_single_for_device(hsotg->dev,
+				   qh->desc_list_dma +
+				   (idx * sizeof(struct dwc2_hcd_dma_desc)),
+				   sizeof(struct dwc2_hcd_dma_desc),
+				   DMA_TO_DEVICE);
 #endif
-
-	if (n_desc) {
-		qh->desc_list[n_desc - 1].status |= HOST_DMA_A;
-		if (n_desc > 1)
-			qh->desc_list[0].status |= HOST_DMA_A;
-	}
 }
 
 static void dwc2_fill_host_dma_desc(struct dwc2_hsotg *hsotg,
@@ -647,6 +716,12 @@ static void dwc2_fill_host_dma_desc(struct dwc2_hsotg *hsotg,
 
 	dma_desc->buf = (u32)chan->xfer_dma;
 
+	dma_sync_single_for_device(hsotg->dev,
+				   qh->desc_list_dma +
+				   (n_desc * sizeof(struct dwc2_hcd_dma_desc)),
+				   sizeof(struct dwc2_hcd_dma_desc),
+				   DMA_TO_DEVICE);
+
 	/*
 	 * Last (or only) descriptor of IN transfer with actual size less
 	 * than MaxPacket
@@ -697,6 +772,12 @@ static void dwc2_init_non_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 					 "set A bit in desc %d (%p)\n",
 					 n_desc - 1,
 					 &qh->desc_list[n_desc - 1]);
+				dma_sync_single_for_device(hsotg->dev,
+					qh->desc_list_dma +
+					((n_desc - 1) *
+					sizeof(struct dwc2_hcd_dma_desc)),
+					sizeof(struct dwc2_hcd_dma_desc),
+					DMA_TO_DEVICE);
 			}
 			dwc2_fill_host_dma_desc(hsotg, chan, qtd, qh, n_desc);
 			dev_vdbg(hsotg->dev,
@@ -722,10 +803,19 @@ static void dwc2_init_non_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 				HOST_DMA_IOC | HOST_DMA_EOL | HOST_DMA_A;
 		dev_vdbg(hsotg->dev, "set IOC/EOL/A bits in desc %d (%p)\n",
 			 n_desc - 1, &qh->desc_list[n_desc - 1]);
+		dma_sync_single_for_device(hsotg->dev,
+					   qh->desc_list_dma + (n_desc - 1) *
+					   sizeof(struct dwc2_hcd_dma_desc),
+					   sizeof(struct dwc2_hcd_dma_desc),
+					   DMA_TO_DEVICE);
 		if (n_desc > 1) {
 			qh->desc_list[0].status |= HOST_DMA_A;
 			dev_vdbg(hsotg->dev, "set A bit in desc 0 (%p)\n",
 				 &qh->desc_list[0]);
+			dma_sync_single_for_device(hsotg->dev,
+					qh->desc_list_dma,
+					sizeof(struct dwc2_hcd_dma_desc),
+					DMA_TO_DEVICE);
 		}
 		chan->ntd = n_desc;
 	}
@@ -800,7 +890,7 @@ static int dwc2_cmpl_host_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 					struct dwc2_qtd *qtd,
 					struct dwc2_qh *qh, u16 idx)
 {
-	struct dwc2_hcd_dma_desc *dma_desc = &qh->desc_list[idx];
+	struct dwc2_hcd_dma_desc *dma_desc;
 	struct dwc2_hcd_iso_packet_desc *frame_desc;
 	u16 remain = 0;
 	int rc = 0;
@@ -808,6 +898,13 @@ static int dwc2_cmpl_host_isoc_dma_desc(struct dwc2_hsotg *hsotg,
 	if (!qtd->urb)
 		return -EINVAL;
 
+	dma_sync_single_for_cpu(hsotg->dev, qh->desc_list_dma + (idx *
+				sizeof(struct dwc2_hcd_dma_desc)),
+				sizeof(struct dwc2_hcd_dma_desc),
+				DMA_FROM_DEVICE);
+
+	dma_desc = &qh->desc_list[idx];
+
 	frame_desc = &qtd->urb->iso_descs[qtd->isoc_frame_index_last];
 	dma_desc->buf = (u32)(qtd->urb->dma + frame_desc->offset);
 	if (chan->ep_is_in)
@@ -911,17 +1008,51 @@ static void dwc2_complete_isoc_xfer_ddma(struct dwc2_hsotg *hsotg,
 	list_for_each_entry_safe(qtd, qtd_tmp, &qh->qtd_list, qtd_list_entry) {
 		if (!qtd->in_process)
 			break;
+
+		/*
+		 * Ensure idx corresponds to descriptor where first urb of this
+		 * qtd was added. In fact, during isoc desc init, dwc2 may skip
+		 * an index if current frame number is already over this index.
+		 */
+		if (idx != qtd->isoc_td_first) {
+			dev_vdbg(hsotg->dev,
+				 "try to complete %d instead of %d\n",
+				 idx, qtd->isoc_td_first);
+			idx = qtd->isoc_td_first;
+		}
+
 		do {
+			struct dwc2_qtd *qtd_next;
+			u16 cur_idx;
+
 			rc = dwc2_cmpl_host_isoc_dma_desc(hsotg, chan, qtd, qh,
 							  idx);
 			if (rc < 0)
 				return;
 			idx = dwc2_desclist_idx_inc(idx, qh->interval,
 						    chan->speed);
-			if (rc == DWC2_CMPL_STOP)
-				goto stop_scan;
+			if (!rc)
+				continue;
+
 			if (rc == DWC2_CMPL_DONE)
 				break;
+
+			/* rc == DWC2_CMPL_STOP */
+
+			if (qh->interval >= 32)
+				goto stop_scan;
+
+			qh->td_first = idx;
+			cur_idx = dwc2_frame_list_idx(hsotg->frame_number);
+			qtd_next = list_first_entry(&qh->qtd_list,
+						    struct dwc2_qtd,
+						    qtd_list_entry);
+			if (dwc2_frame_idx_num_gt(cur_idx,
+						  qtd_next->isoc_td_last))
+				break;
+
+			goto stop_scan;
+
 		} while (idx != qh->td_first);
 	}
 
@@ -1029,6 +1160,12 @@ static int dwc2_process_non_isoc_desc(struct dwc2_hsotg *hsotg,
 	if (!urb)
 		return -EINVAL;
 
+	dma_sync_single_for_cpu(hsotg->dev,
+				qh->desc_list_dma + (desc_num *
+				sizeof(struct dwc2_hcd_dma_desc)),
+				sizeof(struct dwc2_hcd_dma_desc),
+				DMA_FROM_DEVICE);
+
 	dma_desc = &qh->desc_list[desc_num];
 	n_bytes = qh->n_bytes[desc_num];
 	dev_vdbg(hsotg->dev,
@@ -1040,8 +1177,8 @@ static int dwc2_process_non_isoc_desc(struct dwc2_hsotg *hsotg,
 	if (failed || (*xfer_done && urb->status != -EINPROGRESS)) {
 		dwc2_host_complete(hsotg, qtd, urb->status);
 		dwc2_hcd_qtd_unlink_and_free(hsotg, qtd, qh);
-		dev_vdbg(hsotg->dev, "failed=%1x xfer_done=%1x status=%08x\n",
-			 failed, *xfer_done, urb->status);
+		dev_vdbg(hsotg->dev, "failed=%1x xfer_done=%1x\n",
+			 failed, *xfer_done);
 		return failed;
 	}
 
@@ -1096,21 +1233,23 @@ static void dwc2_complete_non_isoc_xfer_ddma(struct dwc2_hsotg *hsotg,
 
 	list_for_each_safe(qtd_item, qtd_tmp, &qh->qtd_list) {
 		int i;
+		int qtd_desc_count;
 
 		qtd = list_entry(qtd_item, struct dwc2_qtd, qtd_list_entry);
 		xfer_done = 0;
+		qtd_desc_count = qtd->n_desc;
 
-		for (i = 0; i < qtd->n_desc; i++) {
+		for (i = 0; i < qtd_desc_count; i++) {
 			if (dwc2_process_non_isoc_desc(hsotg, chan, chnum, qtd,
 						       desc_num, halt_status,
-						       &xfer_done)) {
-				qtd = NULL;
-				break;
-			}
+						       &xfer_done))
+				goto stop_scan;
+
 			desc_num++;
 		}
 	}
 
+stop_scan:
 	if (qh->ep_type != USB_ENDPOINT_XFER_CONTROL) {
 		/*
 		 * Resetting the data toggle for bulk and interrupt endpoints
@@ -1118,7 +1257,7 @@ static void dwc2_complete_non_isoc_xfer_ddma(struct dwc2_hsotg *hsotg,
 		 */
 		if (halt_status == DWC2_HC_XFER_STALL)
 			qh->data_toggle = DWC2_HC_PID_DATA0;
-		else if (qtd)
+		else
 			dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
 	}
 
@@ -1165,6 +1304,21 @@ void dwc2_hcd_complete_xfer_ddma(struct dwc2_hsotg *hsotg,
 		/* Release the channel if halted or session completed */
 		if (halt_status != DWC2_HC_XFER_COMPLETE ||
 		    list_empty(&qh->qtd_list)) {
+			struct dwc2_qtd *qtd, *qtd_tmp;
+
+			/*
+			 * Kill all remainings QTDs since channel has been
+			 * halted.
+			 */
+			list_for_each_entry_safe(qtd, qtd_tmp,
+						 &qh->qtd_list,
+						 qtd_list_entry) {
+				dwc2_host_complete(hsotg, qtd,
+						   -ECONNRESET);
+				dwc2_hcd_qtd_unlink_and_free(hsotg,
+							     qtd, qh);
+			}
+
 			/* Halt the channel if session completed */
 			if (halt_status == DWC2_HC_XFER_COMPLETE)
 				dwc2_hc_halt(hsotg, chan, halt_status);
@@ -1174,7 +1328,12 @@ void dwc2_hcd_complete_xfer_ddma(struct dwc2_hsotg *hsotg,
 			/* Keep in assigned schedule to continue transfer */
 			list_move(&qh->qh_list_entry,
 				  &hsotg->periodic_sched_assigned);
-			continue_isoc_xfer = 1;
+			/*
+			 * If channel has been halted during giveback of urb
+			 * then prevent any new scheduling.
+			 */
+			if (!chan->halt_status)
+				continue_isoc_xfer = 1;
 		}
 		/*
 		 * Todo: Consider the case when period exceeds FrameList size.
diff --git a/drivers/usb/dwc2/hcd_intr.c b/drivers/usb/dwc2/hcd_intr.c
index bda0b21b850f..cadba8b13c48 100644
--- a/drivers/usb/dwc2/hcd_intr.c
+++ b/drivers/usb/dwc2/hcd_intr.c
@@ -122,6 +122,9 @@ static void dwc2_sof_intr(struct dwc2_hsotg *hsotg)
 	struct dwc2_qh *qh;
 	enum dwc2_transaction_type tr_type;
 
+	/* Clear interrupt */
+	dwc2_writel(GINTSTS_SOF, hsotg->regs + GINTSTS);
+
 #ifdef DEBUG_SOF
 	dev_vdbg(hsotg->dev, "--Start of Frame Interrupt--\n");
 #endif
@@ -146,9 +149,6 @@ static void dwc2_sof_intr(struct dwc2_hsotg *hsotg)
 	tr_type = dwc2_hcd_select_transactions(hsotg);
 	if (tr_type != DWC2_TRANSACTION_NONE)
 		dwc2_hcd_queue_transactions(hsotg, tr_type);
-
-	/* Clear interrupt */
-	dwc2_writel(GINTSTS_SOF, hsotg->regs + GINTSTS);
 }
 
 /*
@@ -312,6 +312,7 @@ static void dwc2_hprt0_enable(struct dwc2_hsotg *hsotg, u32 hprt0,
 
 	if (do_reset) {
 		*hprt0_modify |= HPRT0_RST;
+		dwc2_writel(*hprt0_modify, hsotg->regs + HPRT0);
 		queue_delayed_work(hsotg->wq_otg, &hsotg->reset_work,
 				   msecs_to_jiffies(60));
 	} else {
@@ -347,15 +348,12 @@ static void dwc2_port_intr(struct dwc2_hsotg *hsotg)
 	 * Set flag and clear if detected
 	 */
 	if (hprt0 & HPRT0_CONNDET) {
+		dwc2_writel(hprt0_modify | HPRT0_CONNDET, hsotg->regs + HPRT0);
+
 		dev_vdbg(hsotg->dev,
 			 "--Port Interrupt HPRT0=0x%08x Port Connect Detected--\n",
 			 hprt0);
-		if (hsotg->lx_state != DWC2_L0)
-			usb_hcd_resume_root_hub(hsotg->priv);
-
-		hsotg->flags.b.port_connect_status_change = 1;
-		hsotg->flags.b.port_connect_status = 1;
-		hprt0_modify |= HPRT0_CONNDET;
+		dwc2_hcd_connect(hsotg);
 
 		/*
 		 * The Hub driver asserts a reset when it sees port connect
@@ -368,27 +366,36 @@ static void dwc2_port_intr(struct dwc2_hsotg *hsotg)
 	 * Clear if detected - Set internal flag if disabled
 	 */
 	if (hprt0 & HPRT0_ENACHG) {
+		dwc2_writel(hprt0_modify | HPRT0_ENACHG, hsotg->regs + HPRT0);
 		dev_vdbg(hsotg->dev,
 			 "  --Port Interrupt HPRT0=0x%08x Port Enable Changed (now %d)--\n",
 			 hprt0, !!(hprt0 & HPRT0_ENA));
-		hprt0_modify |= HPRT0_ENACHG;
-		if (hprt0 & HPRT0_ENA)
+		if (hprt0 & HPRT0_ENA) {
+			hsotg->new_connection = true;
 			dwc2_hprt0_enable(hsotg, hprt0, &hprt0_modify);
-		else
+		} else {
 			hsotg->flags.b.port_enable_change = 1;
+			if (hsotg->core_params->dma_desc_fs_enable) {
+				u32 hcfg;
+
+				hsotg->core_params->dma_desc_enable = 0;
+				hsotg->new_connection = false;
+				hcfg = dwc2_readl(hsotg->regs + HCFG);
+				hcfg &= ~HCFG_DESCDMA;
+				dwc2_writel(hcfg, hsotg->regs + HCFG);
+			}
+		}
 	}
 
 	/* Overcurrent Change Interrupt */
 	if (hprt0 & HPRT0_OVRCURRCHG) {
+		dwc2_writel(hprt0_modify | HPRT0_OVRCURRCHG,
+			    hsotg->regs + HPRT0);
 		dev_vdbg(hsotg->dev,
 			 "  --Port Interrupt HPRT0=0x%08x Port Overcurrent Changed--\n",
 			 hprt0);
 		hsotg->flags.b.port_over_current_change = 1;
-		hprt0_modify |= HPRT0_OVRCURRCHG;
 	}
-
-	/* Clear Port Interrupts */
-	dwc2_writel(hprt0_modify, hsotg->regs + HPRT0);
 }
 
 /*
@@ -518,11 +525,19 @@ void dwc2_hcd_save_data_toggle(struct dwc2_hsotg *hsotg,
 	u32 pid = (hctsiz & TSIZ_SC_MC_PID_MASK) >> TSIZ_SC_MC_PID_SHIFT;
 
 	if (chan->ep_type != USB_ENDPOINT_XFER_CONTROL) {
+		if (WARN(!chan || !chan->qh,
+			 "chan->qh must be specified for non-control eps\n"))
+			return;
+
 		if (pid == TSIZ_SC_MC_PID_DATA0)
 			chan->qh->data_toggle = DWC2_HC_PID_DATA0;
 		else
 			chan->qh->data_toggle = DWC2_HC_PID_DATA1;
 	} else {
+		if (WARN(!qtd,
+			 "qtd must be specified for control eps\n"))
+			return;
+
 		if (pid == TSIZ_SC_MC_PID_DATA0)
 			qtd->data_toggle = DWC2_HC_PID_DATA0;
 		else
diff --git a/drivers/usb/dwc2/hcd_queue.c b/drivers/usb/dwc2/hcd_queue.c
index 7d8d06cfe3c1..27d402f680a3 100644
--- a/drivers/usb/dwc2/hcd_queue.c
+++ b/drivers/usb/dwc2/hcd_queue.c
@@ -232,7 +232,7 @@ struct dwc2_qh *dwc2_hcd_qh_create(struct dwc2_hsotg *hsotg,
  */
 void dwc2_hcd_qh_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 {
-	if (hsotg->core_params->dma_desc_enable > 0) {
+	if (qh->desc_list) {
 		dwc2_hcd_qh_free_ddma(hsotg, qh);
 	} else {
 		/* kfree(NULL) is safe */
diff --git a/drivers/usb/dwc2/hw.h b/drivers/usb/dwc2/hw.h
index 553f24606c43..281b57b36ab4 100644
--- a/drivers/usb/dwc2/hw.h
+++ b/drivers/usb/dwc2/hw.h
@@ -769,10 +769,6 @@
 #define TSIZ_XFERSIZE_SHIFT		0
 
 #define HCDMA(_ch)			HSOTG_REG(0x0514 + 0x20 * (_ch))
-#define HCDMA_DMA_ADDR_MASK		(0x1fffff << 11)
-#define HCDMA_DMA_ADDR_SHIFT		11
-#define HCDMA_CTD_MASK			(0xff << 3)
-#define HCDMA_CTD_SHIFT			3
 
 #define HCDMAB(_ch)			HSOTG_REG(0x051c + 0x20 * (_ch))
 
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index 39c1cbf0e75d..690b9fd98b55 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -54,11 +54,44 @@
 
 static const char dwc2_driver_name[] = "dwc2";
 
+static const struct dwc2_core_params params_hi6220 = {
+	.otg_cap			= 2,	/* No HNP/SRP capable */
+	.otg_ver			= 0,	/* 1.3 */
+	.dma_enable			= 1,
+	.dma_desc_enable		= 0,
+	.dma_desc_fs_enable		= 0,
+	.speed				= 0,	/* High Speed */
+	.enable_dynamic_fifo		= 1,
+	.en_multiple_tx_fifo		= 1,
+	.host_rx_fifo_size		= 512,
+	.host_nperio_tx_fifo_size	= 512,
+	.host_perio_tx_fifo_size	= 512,
+	.max_transfer_size		= 65535,
+	.max_packet_count		= 511,
+	.host_channels			= 16,
+	.phy_type			= 1,	/* UTMI */
+	.phy_utmi_width			= 8,
+	.phy_ulpi_ddr			= 0,	/* Single */
+	.phy_ulpi_ext_vbus		= 0,
+	.i2c_enable			= 0,
+	.ulpi_fs_ls			= 0,
+	.host_support_fs_ls_low_power	= 0,
+	.host_ls_low_power_phy_clk	= 0,	/* 48 MHz */
+	.ts_dline			= 0,
+	.reload_ctl			= 0,
+	.ahbcfg				= GAHBCFG_HBSTLEN_INCR16 <<
+					  GAHBCFG_HBSTLEN_SHIFT,
+	.uframe_sched			= 0,
+	.external_id_pin_ctl		= -1,
+	.hibernation			= -1,
+};
+
 static const struct dwc2_core_params params_bcm2835 = {
 	.otg_cap			= 0,	/* HNP/SRP capable */
 	.otg_ver			= 0,	/* 1.3 */
 	.dma_enable			= 1,
 	.dma_desc_enable		= 0,
+	.dma_desc_fs_enable		= 0,
 	.speed				= 0,	/* High Speed */
 	.enable_dynamic_fifo		= 1,
 	.en_multiple_tx_fifo		= 1,
@@ -89,6 +122,7 @@ static const struct dwc2_core_params params_rk3066 = {
 	.otg_ver			= -1,
 	.dma_enable			= -1,
 	.dma_desc_enable		= 0,
+	.dma_desc_fs_enable		= 0,
 	.speed				= -1,
 	.enable_dynamic_fifo		= 1,
 	.en_multiple_tx_fifo		= -1,
@@ -115,6 +149,71 @@ static const struct dwc2_core_params params_rk3066 = {
 	.hibernation			= -1,
 };
 
+/*
+ * Check the dr_mode against the module configuration and hardware
+ * capabilities.
+ *
+ * The hardware, module, and dr_mode, can each be set to host, device,
+ * or otg. Check that all these values are compatible and adjust the
+ * value of dr_mode if possible.
+ *
+ *                      actual
+ *    HW  MOD dr_mode   dr_mode
+ *  ------------------------------
+ *   HST  HST  any    :  HST
+ *   HST  DEV  any    :  ---
+ *   HST  OTG  any    :  HST
+ *
+ *   DEV  HST  any    :  ---
+ *   DEV  DEV  any    :  DEV
+ *   DEV  OTG  any    :  DEV
+ *
+ *   OTG  HST  any    :  HST
+ *   OTG  DEV  any    :  DEV
+ *   OTG  OTG  any    :  dr_mode
+ */
+static int dwc2_get_dr_mode(struct dwc2_hsotg *hsotg)
+{
+	enum usb_dr_mode mode;
+
+	hsotg->dr_mode = usb_get_dr_mode(hsotg->dev);
+	if (hsotg->dr_mode == USB_DR_MODE_UNKNOWN)
+		hsotg->dr_mode = USB_DR_MODE_OTG;
+
+	mode = hsotg->dr_mode;
+
+	if (dwc2_hw_is_device(hsotg)) {
+		if (IS_ENABLED(CONFIG_USB_DWC2_HOST)) {
+			dev_err(hsotg->dev,
+				"Controller does not support host mode.\n");
+			return -EINVAL;
+		}
+		mode = USB_DR_MODE_PERIPHERAL;
+	} else if (dwc2_hw_is_host(hsotg)) {
+		if (IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL)) {
+			dev_err(hsotg->dev,
+				"Controller does not support device mode.\n");
+			return -EINVAL;
+		}
+		mode = USB_DR_MODE_HOST;
+	} else {
+		if (IS_ENABLED(CONFIG_USB_DWC2_HOST))
+			mode = USB_DR_MODE_HOST;
+		else if (IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL))
+			mode = USB_DR_MODE_PERIPHERAL;
+	}
+
+	if (mode != hsotg->dr_mode) {
+		dev_warn(hsotg->dev,
+			"Configuration mismatch. dr_mode forced to %s\n",
+			mode == USB_DR_MODE_HOST ? "host" : "device");
+
+		hsotg->dr_mode = mode;
+	}
+
+	return 0;
+}
+
 static int __dwc2_lowlevel_hw_enable(struct dwc2_hsotg *hsotg)
 {
 	struct platform_device *pdev = to_platform_device(hsotg->dev);
@@ -306,8 +405,28 @@ static int dwc2_driver_remove(struct platform_device *dev)
 	return 0;
 }
 
+/**
+ * dwc2_driver_shutdown() - Called on device shutdown
+ *
+ * @dev: Platform device
+ *
+ * In specific conditions (involving usb hubs) dwc2 devices can create a
+ * lot of interrupts, even to the point of overwhelming devices running
+ * at low frequencies. Some devices need to do special clock handling
+ * at shutdown-time which may bring the system clock below the threshold
+ * of being able to handle the dwc2 interrupts. Disabling dwc2-irqs
+ * prevents reboots/poweroffs from getting stuck in such cases.
+ */
+static void dwc2_driver_shutdown(struct platform_device *dev)
+{
+	struct dwc2_hsotg *hsotg = platform_get_drvdata(dev);
+
+	disable_irq(hsotg->irq);
+}
+
 static const struct of_device_id dwc2_of_match_table[] = {
 	{ .compatible = "brcm,bcm2835-usb", .data = &params_bcm2835 },
+	{ .compatible = "hisilicon,hi6220-usb", .data = &params_hi6220 },
 	{ .compatible = "rockchip,rk3066-usb", .data = &params_rk3066 },
 	{ .compatible = "snps,dwc2", .data = NULL },
 	{ .compatible = "samsung,s3c6400-hsotg", .data = NULL},
@@ -335,7 +454,6 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	struct dwc2_hsotg *hsotg;
 	struct resource *res;
 	int retval;
-	int irq;
 
 	match = of_match_device(dwc2_of_match_table, &dev->dev);
 	if (match && match->data) {
@@ -348,8 +466,10 @@ static int dwc2_driver_probe(struct platform_device *dev)
 		/*
 		 * Disable descriptor dma mode by default as the HW can support
 		 * it, but does not support it for SPLIT transactions.
+		 * Disable it for FS devices as well.
 		 */
 		defparams.dma_desc_enable = 0;
+		defparams.dma_desc_fs_enable = 0;
 	}
 
 	hsotg = devm_kzalloc(&dev->dev, sizeof(*hsotg), GFP_KERNEL);
@@ -375,19 +495,6 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	dev_dbg(&dev->dev, "mapped PA %08lx to VA %p\n",
 		(unsigned long)res->start, hsotg->regs);
 
-	hsotg->dr_mode = usb_get_dr_mode(&dev->dev);
-	if (IS_ENABLED(CONFIG_USB_DWC2_HOST) &&
-			hsotg->dr_mode != USB_DR_MODE_HOST) {
-		hsotg->dr_mode = USB_DR_MODE_HOST;
-		dev_warn(hsotg->dev,
-			"Configuration mismatch. Forcing host mode\n");
-	} else if (IS_ENABLED(CONFIG_USB_DWC2_PERIPHERAL) &&
-			hsotg->dr_mode != USB_DR_MODE_PERIPHERAL) {
-		hsotg->dr_mode = USB_DR_MODE_PERIPHERAL;
-		dev_warn(hsotg->dev,
-			"Configuration mismatch. Forcing peripheral mode\n");
-	}
-
 	retval = dwc2_lowlevel_hw_init(hsotg);
 	if (retval)
 		return retval;
@@ -401,15 +508,15 @@ static int dwc2_driver_probe(struct platform_device *dev)
 
 	dwc2_set_all_params(hsotg->core_params, -1);
 
-	irq = platform_get_irq(dev, 0);
-	if (irq < 0) {
+	hsotg->irq = platform_get_irq(dev, 0);
+	if (hsotg->irq < 0) {
 		dev_err(&dev->dev, "missing IRQ resource\n");
-		return irq;
+		return hsotg->irq;
 	}
 
 	dev_dbg(hsotg->dev, "registering common handler for irq%d\n",
-		irq);
-	retval = devm_request_irq(hsotg->dev, irq,
+		hsotg->irq);
+	retval = devm_request_irq(hsotg->dev, hsotg->irq,
 				  dwc2_handle_common_intr, IRQF_SHARED,
 				  dev_name(hsotg->dev), hsotg);
 	if (retval)
@@ -419,6 +526,16 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	if (retval)
 		return retval;
 
+	retval = dwc2_get_dr_mode(hsotg);
+	if (retval)
+		return retval;
+
+	/*
+	 * Reset before dwc2_get_hwparams() then it could get power-on real
+	 * reset value form registers.
+	 */
+	dwc2_core_reset_and_force_dr_mode(hsotg);
+
 	/* Detect config values from hardware */
 	retval = dwc2_get_hwparams(hsotg);
 	if (retval)
@@ -427,15 +544,17 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	/* Validate parameter values */
 	dwc2_set_parameters(hsotg, params);
 
+	dwc2_force_dr_mode(hsotg);
+
 	if (hsotg->dr_mode != USB_DR_MODE_HOST) {
-		retval = dwc2_gadget_init(hsotg, irq);
+		retval = dwc2_gadget_init(hsotg, hsotg->irq);
 		if (retval)
 			goto error;
 		hsotg->gadget_enabled = 1;
 	}
 
 	if (hsotg->dr_mode != USB_DR_MODE_PERIPHERAL) {
-		retval = dwc2_hcd_init(hsotg, irq);
+		retval = dwc2_hcd_init(hsotg, hsotg->irq);
 		if (retval) {
 			if (hsotg->gadget_enabled)
 				dwc2_hsotg_remove(hsotg);
@@ -502,6 +621,7 @@ static struct platform_driver dwc2_platform_driver = {
 	},
 	.probe = dwc2_driver_probe,
 	.remove = dwc2_driver_remove,
+	.shutdown = dwc2_driver_shutdown,
 };
 
 module_platform_driver(dwc2_platform_driver);
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index 5a42c4590402..a64ce1c94d6d 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -87,6 +87,15 @@ config USB_DWC3_KEYSTONE
 	  Support of USB2/3 functionality in TI Keystone2 platforms.
 	  Say 'Y' or 'M' here if you have one such device
 
+config USB_DWC3_OF_SIMPLE
+       tristate "Generic OF Simple Glue Layer"
+       depends on OF && COMMON_CLK
+       default USB_DWC3
+       help
+         Support USB2/3 functionality in simple SoC integrations.
+	 Currently supports Xilinx and Qualcomm DWC USB3 IP.
+	 Say 'Y' or 'M' if you have one such device.
+
 config USB_DWC3_ST
 	tristate "STMicroelectronics Platforms"
 	depends on ARCH_STI && OF
@@ -96,12 +105,4 @@ config USB_DWC3_ST
 	  inside (i.e. STiH407).
 	  Say 'Y' or 'M' if you have one such device.
 
-config USB_DWC3_QCOM
-	tristate "Qualcomm Platforms"
-	depends on ARCH_QCOM || COMPILE_TEST
-	default USB_DWC3
-	help
-	  Recent Qualcomm SoCs ship with one DesignWare Core USB3 IP inside,
-	  say 'Y' or 'M' if you have one such device.
-
 endif
diff --git a/drivers/usb/dwc3/Makefile b/drivers/usb/dwc3/Makefile
index acc951d46c27..22420e17d68b 100644
--- a/drivers/usb/dwc3/Makefile
+++ b/drivers/usb/dwc3/Makefile
@@ -37,5 +37,5 @@ obj-$(CONFIG_USB_DWC3_OMAP)		+= dwc3-omap.o
 obj-$(CONFIG_USB_DWC3_EXYNOS)		+= dwc3-exynos.o
 obj-$(CONFIG_USB_DWC3_PCI)		+= dwc3-pci.o
 obj-$(CONFIG_USB_DWC3_KEYSTONE)		+= dwc3-keystone.o
-obj-$(CONFIG_USB_DWC3_QCOM)		+= dwc3-qcom.o
+obj-$(CONFIG_USB_DWC3_OF_SIMPLE)	+= dwc3-of-simple.o
 obj-$(CONFIG_USB_DWC3_ST)		+= dwc3-st.o
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index 22b4797383cd..de5e01f41bc2 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -272,7 +272,8 @@ static int dwc3_event_buffers_setup(struct dwc3 *dwc)
 
 	for (n = 0; n < dwc->num_event_buffers; n++) {
 		evt = dwc->ev_buffs[n];
-		dev_dbg(dwc->dev, "Event buf %p dma %08llx length %d\n",
+		dwc3_trace(trace_dwc3_core,
+				"Event buf %p dma %08llx length %d\n",
 				evt->buf, (unsigned long long) evt->dma,
 				evt->length);
 
@@ -608,12 +609,13 @@ static int dwc3_core_init(struct dwc3 *dwc)
 		reg |= DWC3_GCTL_GBLHIBERNATIONEN;
 		break;
 	default:
-		dev_dbg(dwc->dev, "No power optimization available\n");
+		dwc3_trace(trace_dwc3_core, "No power optimization available\n");
 	}
 
 	/* check if current dwc3 is on simulation board */
 	if (dwc->hwparams.hwparams6 & DWC3_GHWPARAMS6_EN_FPGA) {
-		dev_dbg(dwc->dev, "it is on FPGA board\n");
+		dwc3_trace(trace_dwc3_core,
+				"running on FPGA platform\n");
 		dwc->is_fpga = true;
 	}
 
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 36f1cb74588c..e4f8b90d9627 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -37,6 +37,7 @@
 #define DWC3_MSG_MAX	500
 
 /* Global constants */
+#define DWC3_ZLP_BUF_SIZE	1024	/* size of a superspeed bulk */
 #define DWC3_EP0_BOUNCE_SIZE	512
 #define DWC3_ENDPOINTS_NUM	32
 #define DWC3_XHCI_RESOURCES_NUM	2
@@ -647,6 +648,7 @@ struct dwc3_scratchpad_array {
  * @ctrl_req: usb control request which is used for ep0
  * @ep0_trb: trb which is used for the ctrl_req
  * @ep0_bounce: bounce buffer for ep0
+ * @zlp_buf: used when request->zero is set
  * @setup_buf: used while precessing STD USB requests
  * @ctrl_req_addr: dma address of ctrl_req
  * @ep0_trb: dma address of ep0_trb
@@ -734,6 +736,7 @@ struct dwc3 {
 	struct usb_ctrlrequest	*ctrl_req;
 	struct dwc3_trb		*ep0_trb;
 	void			*ep0_bounce;
+	void			*zlp_buf;
 	void			*scratchbuf;
 	u8			*setup_buf;
 	dma_addr_t		ctrl_req_addr;
@@ -853,7 +856,6 @@ struct dwc3 {
 	unsigned		pullups_connected:1;
 	unsigned		resize_fifos:1;
 	unsigned		setup_packet_pending:1;
-	unsigned		start_config_issued:1;
 	unsigned		three_stage_setup:1;
 	unsigned		usb3_lpm_capable:1;
 
diff --git a/drivers/usb/dwc3/dwc3-of-simple.c b/drivers/usb/dwc3/dwc3-of-simple.c
new file mode 100644
index 000000000000..9c9f74155066
--- /dev/null
+++ b/drivers/usb/dwc3/dwc3-of-simple.c
@@ -0,0 +1,180 @@
+/**
+ * dwc3-of-simple.c - OF glue layer for simple integrations
+ *
+ * Copyright (c) 2015 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Author: Felipe Balbi <balbi@ti.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * This is a combination of the old dwc3-qcom.c by Ivan T. Ivanov
+ * <iivanov@mm-sol.com> and the original patch adding support for Xilinx' SoC
+ * by Subbaraya Sundeep Bhatta <subbaraya.sundeep.bhatta@xilinx.com>
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/pm_runtime.h>
+
+struct dwc3_of_simple {
+	struct device		*dev;
+	struct clk		**clks;
+	int			num_clocks;
+};
+
+static int dwc3_of_simple_probe(struct platform_device *pdev)
+{
+	struct dwc3_of_simple	*simple;
+	struct device		*dev = &pdev->dev;
+	struct device_node	*np = dev->of_node;
+
+	int			ret;
+	int			i;
+
+	simple = devm_kzalloc(dev, sizeof(*simple), GFP_KERNEL);
+	if (!simple)
+		return -ENOMEM;
+
+	ret = of_clk_get_parent_count(np);
+	if (ret < 0)
+		return ret;
+
+	simple->num_clocks = ret;
+
+	simple->clks = devm_kcalloc(dev, simple->num_clocks,
+			sizeof(struct clk *), GFP_KERNEL);
+	if (!simple->clks)
+		return -ENOMEM;
+
+	simple->dev = dev;
+
+	for (i = 0; i < simple->num_clocks; i++) {
+		struct clk	*clk;
+
+		clk = of_clk_get(np, i);
+		if (IS_ERR(clk)) {
+			while (--i >= 0)
+				clk_put(simple->clks[i]);
+			return PTR_ERR(clk);
+		}
+
+		ret = clk_prepare_enable(clk);
+		if (ret < 0) {
+			while (--i >= 0) {
+				clk_disable_unprepare(simple->clks[i]);
+				clk_put(simple->clks[i]);
+			}
+			clk_put(clk);
+
+			return ret;
+		}
+
+		simple->clks[i] = clk;
+	}
+
+	ret = of_platform_populate(np, NULL, NULL, dev);
+	if (ret) {
+		for (i = 0; i < simple->num_clocks; i++) {
+			clk_disable_unprepare(simple->clks[i]);
+			clk_put(simple->clks[i]);
+		}
+
+		return ret;
+	}
+
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
+	return 0;
+}
+
+static int dwc3_of_simple_remove(struct platform_device *pdev)
+{
+	struct dwc3_of_simple	*simple = platform_get_drvdata(pdev);
+	struct device		*dev = &pdev->dev;
+	int			i;
+
+	for (i = 0; i < simple->num_clocks; i++) {
+		clk_unprepare(simple->clks[i]);
+		clk_put(simple->clks[i]);
+	}
+
+	of_platform_depopulate(dev);
+
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int dwc3_of_simple_runtime_suspend(struct device *dev)
+{
+	struct dwc3_of_simple	*simple = dev_get_drvdata(dev);
+	int			i;
+
+	for (i = 0; i < simple->num_clocks; i++)
+		clk_disable(simple->clks[i]);
+
+	return 0;
+}
+
+static int dwc3_of_simple_runtime_resume(struct device *dev)
+{
+	struct dwc3_of_simple	*simple = dev_get_drvdata(dev);
+	int			ret;
+	int			i;
+
+	for (i = 0; i < simple->num_clocks; i++) {
+		ret = clk_enable(simple->clks[i]);
+		if (ret < 0) {
+			while (--i >= 0)
+				clk_disable(simple->clks[i]);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops dwc3_of_simple_dev_pm_ops = {
+	SET_RUNTIME_PM_OPS(dwc3_of_simple_runtime_suspend,
+			dwc3_of_simple_runtime_resume, NULL)
+};
+
+static const struct of_device_id of_dwc3_simple_match[] = {
+	{ .compatible = "qcom,dwc3" },
+	{ .compatible = "xlnx,zynqmp-dwc3" },
+	{ /* Sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, of_dwc3_simple_match);
+
+static struct platform_driver dwc3_of_simple_driver = {
+	.probe		= dwc3_of_simple_probe,
+	.remove		= dwc3_of_simple_remove,
+	.driver		= {
+		.name	= "dwc3-of-simple",
+		.of_match_table = of_dwc3_simple_match,
+	},
+};
+
+module_platform_driver(dwc3_of_simple_driver);
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("DesignWare USB3 OF Simple Glue Layer");
+MODULE_AUTHOR("Felipe Balbi <balbi@ti.com>");
diff --git a/drivers/usb/dwc3/dwc3-qcom.c b/drivers/usb/dwc3/dwc3-qcom.c
deleted file mode 100644
index 088026048f49..000000000000
--- a/drivers/usb/dwc3/dwc3-qcom.c
+++ /dev/null
@@ -1,130 +0,0 @@
-/* Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/clk.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/of_platform.h>
-#include <linux/platform_device.h>
-
-struct dwc3_qcom {
-	struct device		*dev;
-
-	struct clk		*core_clk;
-	struct clk		*iface_clk;
-	struct clk		*sleep_clk;
-};
-
-static int dwc3_qcom_probe(struct platform_device *pdev)
-{
-	struct device_node *node = pdev->dev.of_node;
-	struct dwc3_qcom *qdwc;
-	int ret;
-
-	qdwc = devm_kzalloc(&pdev->dev, sizeof(*qdwc), GFP_KERNEL);
-	if (!qdwc)
-		return -ENOMEM;
-
-	platform_set_drvdata(pdev, qdwc);
-
-	qdwc->dev = &pdev->dev;
-
-	qdwc->core_clk = devm_clk_get(qdwc->dev, "core");
-	if (IS_ERR(qdwc->core_clk)) {
-		dev_err(qdwc->dev, "failed to get core clock\n");
-		return PTR_ERR(qdwc->core_clk);
-	}
-
-	qdwc->iface_clk = devm_clk_get(qdwc->dev, "iface");
-	if (IS_ERR(qdwc->iface_clk)) {
-		dev_info(qdwc->dev, "failed to get optional iface clock\n");
-		qdwc->iface_clk = NULL;
-	}
-
-	qdwc->sleep_clk = devm_clk_get(qdwc->dev, "sleep");
-	if (IS_ERR(qdwc->sleep_clk)) {
-		dev_info(qdwc->dev, "failed to get optional sleep clock\n");
-		qdwc->sleep_clk = NULL;
-	}
-
-	ret = clk_prepare_enable(qdwc->core_clk);
-	if (ret) {
-		dev_err(qdwc->dev, "failed to enable core clock\n");
-		goto err_core;
-	}
-
-	ret = clk_prepare_enable(qdwc->iface_clk);
-	if (ret) {
-		dev_err(qdwc->dev, "failed to enable optional iface clock\n");
-		goto err_iface;
-	}
-
-	ret = clk_prepare_enable(qdwc->sleep_clk);
-	if (ret) {
-		dev_err(qdwc->dev, "failed to enable optional sleep clock\n");
-		goto err_sleep;
-	}
-
-	ret = of_platform_populate(node, NULL, NULL, qdwc->dev);
-	if (ret) {
-		dev_err(qdwc->dev, "failed to register core - %d\n", ret);
-		goto err_clks;
-	}
-
-	return 0;
-
-err_clks:
-	clk_disable_unprepare(qdwc->sleep_clk);
-err_sleep:
-	clk_disable_unprepare(qdwc->iface_clk);
-err_iface:
-	clk_disable_unprepare(qdwc->core_clk);
-err_core:
-	return ret;
-}
-
-static int dwc3_qcom_remove(struct platform_device *pdev)
-{
-	struct dwc3_qcom *qdwc = platform_get_drvdata(pdev);
-
-	of_platform_depopulate(&pdev->dev);
-
-	clk_disable_unprepare(qdwc->sleep_clk);
-	clk_disable_unprepare(qdwc->iface_clk);
-	clk_disable_unprepare(qdwc->core_clk);
-
-	return 0;
-}
-
-static const struct of_device_id of_dwc3_match[] = {
-	{ .compatible = "qcom,dwc3" },
-	{ /* Sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, of_dwc3_match);
-
-static struct platform_driver dwc3_qcom_driver = {
-	.probe		= dwc3_qcom_probe,
-	.remove		= dwc3_qcom_remove,
-	.driver		= {
-		.name	= "qcom-dwc3",
-		.of_match_table	= of_dwc3_match,
-	},
-};
-
-module_platform_driver(dwc3_qcom_driver);
-
-MODULE_ALIAS("platform:qcom-dwc3");
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("DesignWare USB3 QCOM Glue Layer");
-MODULE_AUTHOR("Ivan T. Ivanov <iivanov@mm-sol.com>");
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 5320e939e090..8d6b75c2f53b 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -555,7 +555,6 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 	int ret;
 	u32 reg;
 
-	dwc->start_config_issued = false;
 	cfg = le16_to_cpu(ctrl->wValue);
 
 	switch (state) {
@@ -737,10 +736,6 @@ static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ISOCH_DELAY");
 		ret = dwc3_ep0_set_isoch_delay(dwc, ctrl);
 		break;
-	case USB_REQ_SET_INTERFACE:
-		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_INTERFACE");
-		dwc->start_config_issued = false;
-		/* Fall through */
 	default:
 		dwc3_trace(trace_dwc3_ep0, "Forwarding to gadget driver");
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
@@ -817,6 +812,8 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 
 	status = DWC3_TRB_SIZE_TRBSTS(trb->size);
 	if (status == DWC3_TRBSTS_SETUP_PENDING) {
+		dwc->setup_packet_pending = true;
+
 		dwc3_trace(trace_dwc3_ep0, "Setup Pending received");
 
 		if (r)
@@ -916,8 +913,10 @@ static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 	}
 
 	status = DWC3_TRB_SIZE_TRBSTS(trb->size);
-	if (status == DWC3_TRBSTS_SETUP_PENDING)
+	if (status == DWC3_TRBSTS_SETUP_PENDING) {
+		dwc->setup_packet_pending = true;
 		dwc3_trace(trace_dwc3_ep0, "Setup Pending received");
+	}
 
 	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
@@ -971,7 +970,7 @@ static void __dwc3_ep0_do_control_data(struct dwc3 *dwc,
 		ret = usb_gadget_map_request(&dwc->gadget, &req->request,
 				dep->number);
 		if (ret) {
-			dev_dbg(dwc->dev, "failed to map request\n");
+			dwc3_trace(trace_dwc3_ep0, "failed to map request\n");
 			return;
 		}
 
@@ -999,7 +998,7 @@ static void __dwc3_ep0_do_control_data(struct dwc3 *dwc,
 		ret = usb_gadget_map_request(&dwc->gadget, &req->request,
 				dep->number);
 		if (ret) {
-			dev_dbg(dwc->dev, "failed to map request\n");
+			dwc3_trace(trace_dwc3_ep0, "failed to map request\n");
 			return;
 		}
 
@@ -1063,8 +1062,6 @@ static void dwc3_ep0_end_control_data(struct dwc3 *dwc, struct dwc3_ep *dep)
 static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 		const struct dwc3_event_depevt *event)
 {
-	dwc->setup_packet_pending = true;
-
 	switch (event->status) {
 	case DEPEVT_STATUS_CONTROL_DATA:
 		dwc3_trace(trace_dwc3_ep0, "Control Data");
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index a58376fd65fe..2363bad45af8 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -265,9 +265,6 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 		usb_gadget_unmap_request(&dwc->gadget, &req->request,
 				req->direction);
 
-	dev_dbg(dwc->dev, "request %p from %s completed %d/%d ===> %d\n",
-			req, dep->name, req->request.actual,
-			req->request.length, status);
 	trace_dwc3_gadget_giveback(req);
 
 	spin_unlock(&dwc->lock);
@@ -388,24 +385,66 @@ static void dwc3_free_trb_pool(struct dwc3_ep *dep)
 	dep->trb_pool_dma = 0;
 }
 
+static int dwc3_gadget_set_xfer_resource(struct dwc3 *dwc, struct dwc3_ep *dep);
+
+/**
+ * dwc3_gadget_start_config - Configure EP resources
+ * @dwc: pointer to our controller context structure
+ * @dep: endpoint that is being enabled
+ *
+ * The assignment of transfer resources cannot perfectly follow the
+ * data book due to the fact that the controller driver does not have
+ * all knowledge of the configuration in advance. It is given this
+ * information piecemeal by the composite gadget framework after every
+ * SET_CONFIGURATION and SET_INTERFACE. Trying to follow the databook
+ * programming model in this scenario can cause errors. For two
+ * reasons:
+ *
+ * 1) The databook says to do DEPSTARTCFG for every SET_CONFIGURATION
+ * and SET_INTERFACE (8.1.5). This is incorrect in the scenario of
+ * multiple interfaces.
+ *
+ * 2) The databook does not mention doing more DEPXFERCFG for new
+ * endpoint on alt setting (8.1.6).
+ *
+ * The following simplified method is used instead:
+ *
+ * All hardware endpoints can be assigned a transfer resource and this
+ * setting will stay persistent until either a core reset or
+ * hibernation. So whenever we do a DEPSTARTCFG(0) we can go ahead and
+ * do DEPXFERCFG for every hardware endpoint as well. We are
+ * guaranteed that there are as many transfer resources as endpoints.
+ *
+ * This function is called for each endpoint when it is being enabled
+ * but is triggered only when called for EP0-out, which always happens
+ * first, and which should only happen in one of the above conditions.
+ */
 static int dwc3_gadget_start_config(struct dwc3 *dwc, struct dwc3_ep *dep)
 {
 	struct dwc3_gadget_ep_cmd_params params;
 	u32			cmd;
+	int			i;
+	int			ret;
+
+	if (dep->number)
+		return 0;
 
 	memset(&params, 0x00, sizeof(params));
+	cmd = DWC3_DEPCMD_DEPSTARTCFG;
 
-	if (dep->number != 1) {
-		cmd = DWC3_DEPCMD_DEPSTARTCFG;
-		/* XferRscIdx == 0 for ep0 and 2 for the remaining */
-		if (dep->number > 1) {
-			if (dwc->start_config_issued)
-				return 0;
-			dwc->start_config_issued = true;
-			cmd |= DWC3_DEPCMD_PARAM(2);
-		}
+	ret = dwc3_send_gadget_ep_cmd(dwc, 0, cmd, &params);
+	if (ret)
+		return ret;
 
-		return dwc3_send_gadget_ep_cmd(dwc, 0, cmd, &params);
+	for (i = 0; i < DWC3_ENDPOINTS_NUM; i++) {
+		struct dwc3_ep *dep = dwc->eps[i];
+
+		if (!dep)
+			continue;
+
+		ret = dwc3_gadget_set_xfer_resource(dwc, dep);
+		if (ret)
+			return ret;
 	}
 
 	return 0;
@@ -519,10 +558,6 @@ static int __dwc3_gadget_ep_enable(struct dwc3_ep *dep,
 		struct dwc3_trb	*trb_st_hw;
 		struct dwc3_trb	*trb_link;
 
-		ret = dwc3_gadget_set_xfer_resource(dwc, dep);
-		if (ret)
-			return ret;
-
 		dep->endpoint.desc = desc;
 		dep->comp_desc = comp_desc;
 		dep->type = usb_endpoint_type(desc);
@@ -664,11 +699,10 @@ static int dwc3_gadget_ep_enable(struct usb_ep *ep,
 	dep = to_dwc3_ep(ep);
 	dwc = dep->dwc;
 
-	if (dep->flags & DWC3_EP_ENABLED) {
-		dev_WARN_ONCE(dwc->dev, true, "%s is already enabled\n",
-				dep->name);
+	if (dev_WARN_ONCE(dwc->dev, dep->flags & DWC3_EP_ENABLED,
+					"%s is already enabled\n",
+					dep->name))
 		return 0;
-	}
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	ret = __dwc3_gadget_ep_enable(dep, desc, ep->comp_desc, false, false);
@@ -692,11 +726,10 @@ static int dwc3_gadget_ep_disable(struct usb_ep *ep)
 	dep = to_dwc3_ep(ep);
 	dwc = dep->dwc;
 
-	if (!(dep->flags & DWC3_EP_ENABLED)) {
-		dev_WARN_ONCE(dwc->dev, true, "%s is already disabled\n",
-				dep->name);
+	if (dev_WARN_ONCE(dwc->dev, !(dep->flags & DWC3_EP_ENABLED),
+					"%s is already disabled\n",
+					dep->name))
 		return 0;
-	}
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	ret = __dwc3_gadget_ep_disable(dep);
@@ -985,8 +1018,6 @@ static int __dwc3_gadget_kick_transfer(struct dwc3_ep *dep, u16 cmd_param,
 	cmd |= DWC3_DEPCMD_PARAM(cmd_param);
 	ret = dwc3_send_gadget_ep_cmd(dwc, dep->number, cmd, &params);
 	if (ret < 0) {
-		dev_dbg(dwc->dev, "failed to send STARTTRANSFER command\n");
-
 		/*
 		 * FIXME we need to iterate over the list of requests
 		 * here and stop, unmap, free and del each of the linked
@@ -1044,6 +1075,20 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 	struct dwc3		*dwc = dep->dwc;
 	int			ret;
 
+	if (!dep->endpoint.desc) {
+		dwc3_trace(trace_dwc3_gadget,
+				"trying to queue request %p to disabled %s\n",
+				&req->request, dep->endpoint.name);
+		return -ESHUTDOWN;
+	}
+
+	if (WARN(req->dep != dep, "request %p belongs to '%s'\n",
+				&req->request, req->dep->name)) {
+		dwc3_trace(trace_dwc3_gadget, "request %p belongs to '%s'\n",
+				&req->request, req->dep->name);
+		return -EINVAL;
+	}
+
 	req->request.actual	= 0;
 	req->request.status	= -EINPROGRESS;
 	req->direction		= dep->direction;
@@ -1141,7 +1186,8 @@ static int __dwc3_gadget_ep_queue(struct dwc3_ep *dep, struct dwc3_request *req)
 
 out:
 	if (ret && ret != -EBUSY)
-		dev_dbg(dwc->dev, "%s: failed to kick transfers\n",
+		dwc3_trace(trace_dwc3_gadget,
+				"%s: failed to kick transfers\n",
 				dep->name);
 	if (ret == -EBUSY)
 		ret = 0;
@@ -1149,6 +1195,32 @@ out:
 	return ret;
 }
 
+static void __dwc3_gadget_ep_zlp_complete(struct usb_ep *ep,
+		struct usb_request *request)
+{
+	dwc3_gadget_ep_free_request(ep, request);
+}
+
+static int __dwc3_gadget_ep_queue_zlp(struct dwc3 *dwc, struct dwc3_ep *dep)
+{
+	struct dwc3_request		*req;
+	struct usb_request		*request;
+	struct usb_ep			*ep = &dep->endpoint;
+
+	dwc3_trace(trace_dwc3_gadget, "queueing ZLP\n");
+	request = dwc3_gadget_ep_alloc_request(ep, GFP_ATOMIC);
+	if (!request)
+		return -ENOMEM;
+
+	request->length = 0;
+	request->buf = dwc->zlp_buf;
+	request->complete = __dwc3_gadget_ep_zlp_complete;
+
+	req = to_dwc3_request(request);
+
+	return __dwc3_gadget_ep_queue(dep, req);
+}
+
 static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 	gfp_t gfp_flags)
 {
@@ -1161,22 +1233,18 @@ static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 	int				ret;
 
 	spin_lock_irqsave(&dwc->lock, flags);
-	if (!dep->endpoint.desc) {
-		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
-				request, ep->name);
-		ret = -ESHUTDOWN;
-		goto out;
-	}
-
-	if (WARN(req->dep != dep, "request %p belongs to '%s'\n",
-				request, req->dep->name)) {
-		ret = -EINVAL;
-		goto out;
-	}
-
 	ret = __dwc3_gadget_ep_queue(dep, req);
 
-out:
+	/*
+	 * Okay, here's the thing, if gadget driver has requested for a ZLP by
+	 * setting request->zero, instead of doing magic, we will just queue an
+	 * extra usb_request ourselves so that it gets handled the same way as
+	 * any other request.
+	 */
+	if (ret == 0 && request->zero && request->length &&
+	    (request->length % ep->maxpacket == 0))
+		ret = __dwc3_gadget_ep_queue_zlp(dwc, dep);
+
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
 	return ret;
@@ -1246,7 +1314,8 @@ int __dwc3_gadget_ep_set_halt(struct dwc3_ep *dep, int value, int protocol)
 		if (!protocol && ((dep->direction && dep->flags & DWC3_EP_BUSY) ||
 				(!list_empty(&dep->req_queued) ||
 				 !list_empty(&dep->request_list)))) {
-			dev_dbg(dwc->dev, "%s: pending request, cannot halt\n",
+			dwc3_trace(trace_dwc3_gadget,
+					"%s: pending request, cannot halt\n",
 					dep->name);
 			return -EAGAIN;
 		}
@@ -1373,7 +1442,7 @@ static int dwc3_gadget_wakeup(struct usb_gadget *g)
 
 	speed = reg & DWC3_DSTS_CONNECTSPD;
 	if (speed == DWC3_DSTS_SUPERSPEED) {
-		dev_dbg(dwc->dev, "no wakeup on SuperSpeed\n");
+		dwc3_trace(trace_dwc3_gadget, "no wakeup on SuperSpeed\n");
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1385,8 +1454,9 @@ static int dwc3_gadget_wakeup(struct usb_gadget *g)
 	case DWC3_LINK_STATE_U3:	/* in HS, means SUSPEND */
 		break;
 	default:
-		dev_dbg(dwc->dev, "can't wakeup from link state %d\n",
-				link_state);
+		dwc3_trace(trace_dwc3_gadget,
+				"can't wakeup from '%s'\n",
+				dwc3_gadget_link_string(link_state));
 		ret = -EINVAL;
 		goto out;
 	}
@@ -1604,8 +1674,6 @@ static int dwc3_gadget_start(struct usb_gadget *g,
 	}
 	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
 
-	dwc->start_config_issued = false;
-
 	/* Start with SuperSpeed Default */
 	dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
 
@@ -1825,7 +1893,8 @@ static int __dwc3_cleanup_done_trbs(struct dwc3 *dwc, struct dwc3_ep *dep,
 		if (count) {
 			trb_status = DWC3_TRB_SIZE_TRBSTS(trb->size);
 			if (trb_status == DWC3_TRBSTS_MISSED_ISOC) {
-				dev_dbg(dwc->dev, "incomplete IN transfer %s\n",
+				dwc3_trace(trace_dwc3_gadget,
+						"%s: incomplete IN transfer\n",
 						dep->name);
 				/*
 				 * If missed isoc occurred and there is
@@ -1887,10 +1956,9 @@ static int dwc3_cleanup_done_reqs(struct dwc3 *dwc, struct dwc3_ep *dep,
 
 	do {
 		req = next_request(&dep->req_queued);
-		if (!req) {
-			WARN_ON_ONCE(1);
+		if (WARN_ON_ONCE(!req))
 			return 1;
-		}
+
 		i = 0;
 		do {
 			slot = req->start_slot + i;
@@ -2004,7 +2072,8 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		dep->resource_index = 0;
 
 		if (usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
-			dev_dbg(dwc->dev, "%s is an Isochronous endpoint\n",
+			dwc3_trace(trace_dwc3_gadget,
+					"%s is an Isochronous endpoint\n",
 					dep->name);
 			return;
 		}
@@ -2031,7 +2100,8 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 			if (!ret || ret == -EBUSY)
 				return;
 
-			dev_dbg(dwc->dev, "%s: failed to kick transfers\n",
+			dwc3_trace(trace_dwc3_gadget,
+					"%s: failed to kick transfers\n",
 					dep->name);
 		}
 
@@ -2053,11 +2123,12 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		case DEPEVT_STREAMEVT_NOTFOUND:
 			/* FALLTHROUGH */
 		default:
-			dev_dbg(dwc->dev, "Couldn't find suitable stream\n");
+			dwc3_trace(trace_dwc3_gadget,
+					"unable to find suitable stream\n");
 		}
 		break;
 	case DWC3_DEPEVT_RXTXFIFOEVT:
-		dev_dbg(dwc->dev, "%s FIFO Overrun\n", dep->name);
+		dwc3_trace(trace_dwc3_gadget, "%s FIFO Overrun\n", dep->name);
 		break;
 	case DWC3_DEPEVT_EPCMDCMPLT:
 		dwc3_trace(trace_dwc3_gadget, "Endpoint Command Complete");
@@ -2202,7 +2273,6 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 
 	dwc3_disconnect_gadget(dwc);
-	dwc->start_config_issued = false;
 
 	dwc->gadget.speed = USB_SPEED_UNKNOWN;
 	dwc->setup_packet_pending = false;
@@ -2230,8 +2300,8 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 	 *
 	 * Our suggested workaround is to follow the Disconnect
 	 * Event steps here, instead, based on a setup_packet_pending
-	 * flag. Such flag gets set whenever we have a XferNotReady
-	 * event on EP0 and gets cleared on XferComplete for the
+	 * flag. Such flag gets set whenever we have a SETUP_PENDING
+	 * status for EP0 TRBs and gets cleared on XferComplete for the
 	 * same endpoint.
 	 *
 	 * Refers to:
@@ -2253,7 +2323,6 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 
 	dwc3_stop_active_transfers(dwc);
 	dwc3_clear_stall_all_ep(dwc);
-	dwc->start_config_issued = false;
 
 	/* Reset device address to zero */
 	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
@@ -2744,10 +2813,17 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 		goto err3;
 	}
 
+	dwc->zlp_buf = kzalloc(DWC3_ZLP_BUF_SIZE, GFP_KERNEL);
+	if (!dwc->zlp_buf) {
+		ret = -ENOMEM;
+		goto err4;
+	}
+
 	dwc->gadget.ops			= &dwc3_gadget_ops;
 	dwc->gadget.speed		= USB_SPEED_UNKNOWN;
 	dwc->gadget.sg_supported	= true;
 	dwc->gadget.name		= "dwc3-gadget";
+	dwc->gadget.is_otg		= dwc->dr_mode == USB_DR_MODE_OTG;
 
 	/*
 	 * FIXME We might be setting max_speed to <SUPER, however versions
@@ -2785,16 +2861,19 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 
 	ret = dwc3_gadget_init_endpoints(dwc);
 	if (ret)
-		goto err4;
+		goto err5;
 
 	ret = usb_add_gadget_udc(dwc->dev, &dwc->gadget);
 	if (ret) {
 		dev_err(dwc->dev, "failed to register udc\n");
-		goto err4;
+		goto err5;
 	}
 
 	return 0;
 
+err5:
+	kfree(dwc->zlp_buf);
+
 err4:
 	dwc3_gadget_free_endpoints(dwc);
 	dma_free_coherent(dwc->dev, DWC3_EP0_BOUNCE_SIZE,
@@ -2827,6 +2906,7 @@ void dwc3_gadget_exit(struct dwc3 *dwc)
 			dwc->ep0_bounce, dwc->ep0_bounce_addr);
 
 	kfree(dwc->setup_buf);
+	kfree(dwc->zlp_buf);
 
 	dma_free_coherent(dwc->dev, sizeof(*dwc->ep0_trb),
 			dwc->ep0_trb, dwc->ep0_trb_addr);
diff --git a/drivers/usb/dwc3/trace.h b/drivers/usb/dwc3/trace.h
index 9c10669ab91f..3ac7252f4427 100644
--- a/drivers/usb/dwc3/trace.h
+++ b/drivers/usb/dwc3/trace.h
@@ -117,6 +117,9 @@ DECLARE_EVENT_CLASS(dwc3_log_request,
 		__field(unsigned, actual)
 		__field(unsigned, length)
 		__field(int, status)
+		__field(int, zero)
+		__field(int, short_not_ok)
+		__field(int, no_interrupt)
 	),
 	TP_fast_assign(
 		snprintf(__get_str(name), DWC3_MSG_MAX, "%s", req->dep->name);
@@ -124,9 +127,15 @@ DECLARE_EVENT_CLASS(dwc3_log_request,
 		__entry->actual = req->request.actual;
 		__entry->length = req->request.length;
 		__entry->status = req->request.status;
+		__entry->zero = req->request.zero;
+		__entry->short_not_ok = req->request.short_not_ok;
+		__entry->no_interrupt = req->request.no_interrupt;
 	),
-	TP_printk("%s: req %p length %u/%u ==> %d",
+	TP_printk("%s: req %p length %u/%u %s%s%s ==> %d",
 		__get_str(name), __entry->req, __entry->actual, __entry->length,
+		__entry->zero ? "Z" : "z",
+		__entry->short_not_ok ? "S" : "s",
+		__entry->no_interrupt ? "i" : "I",
 		__entry->status
 	)
 );
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 33834aa09ed4..af5d922a8f5d 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -127,6 +127,12 @@ config USB_GADGET_STORAGE_NUM_BUFFERS
 	   a module parameter as well.
 	   If unsure, say 2.
 
+config U_SERIAL_CONSOLE
+	bool "Serial gadget console support"
+	depends on USB_G_SERIAL
+	help
+	   It supports the serial gadget can be used as a console.
+
 source "drivers/usb/gadget/udc/Kconfig"
 
 #
@@ -199,6 +205,9 @@ config USB_F_HID
 config USB_F_PRINTER
 	tristate
 
+config USB_F_TCM
+	tristate
+
 choice
 	tristate "USB Gadget Drivers"
 	default USB_ETH
@@ -451,6 +460,20 @@ config USB_CONFIGFS_F_PRINTER
 	  For more information, see Documentation/usb/gadget_printer.txt
 	  which includes sample code for accessing the device file.
 
+config USB_CONFIGFS_F_TCM
+	bool "USB Gadget Target Fabric"
+	depends on TARGET_CORE
+	depends on USB_CONFIGFS
+	select USB_LIBCOMPOSITE
+	select USB_F_TCM
+	help
+	  This fabric is a USB gadget component. Two USB protocols are
+	  supported that is BBB or BOT (Bulk Only Transport) and UAS
+	  (USB Attached SCSI). BOT is advertised on alternative
+	  interface 0 (primary) and UAS is on alternative interface 1.
+	  Both protocols can work on USB2.0 and USB3.0.
+	  UAS utilizes the USB 3.0 feature called streams support.
+
 source "drivers/usb/gadget/legacy/Kconfig"
 
 endchoice
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 163d305e1200..590c44989e5e 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -56,7 +56,6 @@ struct gadget_info {
 	struct list_head string_list;
 	struct list_head available_func;
 
-	const char *udc_name;
 	struct usb_composite_driver composite;
 	struct usb_composite_dev cdev;
 	bool use_os_desc;
@@ -233,21 +232,23 @@ static ssize_t gadget_dev_desc_bcdUSB_store(struct config_item *item,
 
 static ssize_t gadget_dev_desc_UDC_show(struct config_item *item, char *page)
 {
-	return sprintf(page, "%s\n", to_gadget_info(item)->udc_name ?: "");
+	char *udc_name = to_gadget_info(item)->composite.gadget_driver.udc_name;
+
+	return sprintf(page, "%s\n", udc_name ?: "");
 }
 
 static int unregister_gadget(struct gadget_info *gi)
 {
 	int ret;
 
-	if (!gi->udc_name)
+	if (!gi->composite.gadget_driver.udc_name)
 		return -ENODEV;
 
 	ret = usb_gadget_unregister_driver(&gi->composite.gadget_driver);
 	if (ret)
 		return ret;
-	kfree(gi->udc_name);
-	gi->udc_name = NULL;
+	kfree(gi->composite.gadget_driver.udc_name);
+	gi->composite.gadget_driver.udc_name = NULL;
 	return 0;
 }
 
@@ -271,14 +272,16 @@ static ssize_t gadget_dev_desc_UDC_store(struct config_item *item,
 		if (ret)
 			goto err;
 	} else {
-		if (gi->udc_name) {
+		if (gi->composite.gadget_driver.udc_name) {
 			ret = -EBUSY;
 			goto err;
 		}
-		ret = usb_udc_attach_driver(name, &gi->composite.gadget_driver);
-		if (ret)
+		gi->composite.gadget_driver.udc_name = name;
+		ret = usb_gadget_probe_driver(&gi->composite.gadget_driver);
+		if (ret) {
+			gi->composite.gadget_driver.udc_name = NULL;
 			goto err;
-		gi->udc_name = name;
+		}
 	}
 	mutex_unlock(&gi->lock);
 	return len;
@@ -427,9 +430,9 @@ static int config_usb_cfg_unlink(
 	 * remove the function.
 	 */
 	mutex_lock(&gi->lock);
-	if (gi->udc_name)
+	if (gi->composite.gadget_driver.udc_name)
 		unregister_gadget(gi);
-	WARN_ON(gi->udc_name);
+	WARN_ON(gi->composite.gadget_driver.udc_name);
 
 	list_for_each_entry(f, &cfg->func_list, list) {
 		if (f->fi == fi) {
@@ -873,10 +876,10 @@ static int os_desc_unlink(struct config_item *os_desc_ci,
 	struct usb_composite_dev *cdev = &gi->cdev;
 
 	mutex_lock(&gi->lock);
-	if (gi->udc_name)
+	if (gi->composite.gadget_driver.udc_name)
 		unregister_gadget(gi);
 	cdev->os_desc_config = NULL;
-	WARN_ON(gi->udc_name);
+	WARN_ON(gi->composite.gadget_driver.udc_name);
 	mutex_unlock(&gi->lock);
 	return 0;
 }
diff --git a/drivers/usb/gadget/function/Makefile b/drivers/usb/gadget/function/Makefile
index bd7def576955..cb8c225e8549 100644
--- a/drivers/usb/gadget/function/Makefile
+++ b/drivers/usb/gadget/function/Makefile
@@ -44,3 +44,5 @@ usb_f_hid-y			:= f_hid.o
 obj-$(CONFIG_USB_F_HID)		+= usb_f_hid.o
 usb_f_printer-y			:= f_printer.o
 obj-$(CONFIG_USB_F_PRINTER)	+= usb_f_printer.o
+usb_f_tcm-y			:= f_tcm.o
+obj-$(CONFIG_USB_F_TCM)		+= usb_f_tcm.o
diff --git a/drivers/usb/gadget/function/f_midi.c b/drivers/usb/gadget/function/f_midi.c
index 898a570319f1..fb1fe96d3215 100644
--- a/drivers/usb/gadget/function/f_midi.c
+++ b/drivers/usb/gadget/function/f_midi.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
+#include <linux/kfifo.h>
 
 #include <sound/core.h>
 #include <sound/initval.h>
@@ -75,6 +76,7 @@ struct f_midi {
 	struct usb_ep		*in_ep, *out_ep;
 	struct snd_card		*card;
 	struct snd_rawmidi	*rmidi;
+	u8			ms_id;
 
 	struct snd_rawmidi_substream *in_substream[MAX_PORTS];
 	struct snd_rawmidi_substream *out_substream[MAX_PORTS];
@@ -87,6 +89,9 @@ struct f_midi {
 	int index;
 	char *id;
 	unsigned int buflen, qlen;
+	/* This fifo is used as a buffer ring for pre-allocated IN usb_requests */
+	DECLARE_KFIFO_PTR(in_req_fifo, struct usb_request *);
+	unsigned int in_last_port;
 };
 
 static inline struct f_midi *func_to_midi(struct usb_function *f)
@@ -94,7 +99,7 @@ static inline struct f_midi *func_to_midi(struct usb_function *f)
 	return container_of(f, struct f_midi, func);
 }
 
-static void f_midi_transmit(struct f_midi *midi, struct usb_request *req);
+static void f_midi_transmit(struct f_midi *midi);
 
 DECLARE_UAC_AC_HEADER_DESCRIPTOR(1);
 DECLARE_USB_MIDI_OUT_JACK_DESCRIPTOR(1);
@@ -201,12 +206,6 @@ static inline struct usb_request *midi_alloc_ep_req(struct usb_ep *ep,
 	return alloc_ep_req(ep, length, length);
 }
 
-static void free_ep_req(struct usb_ep *ep, struct usb_request *req)
-{
-	kfree(req->buf);
-	usb_ep_free_request(ep, req);
-}
-
 static const uint8_t f_midi_cin_length[] = {
 	0, 0, 2, 3, 3, 1, 2, 3, 3, 3, 3, 3, 2, 2, 3, 1
 };
@@ -258,7 +257,8 @@ f_midi_complete(struct usb_ep *ep, struct usb_request *req)
 		} else if (ep == midi->in_ep) {
 			/* Our transmit completed. See if there's more to go.
 			 * f_midi_transmit eats req, don't queue it again. */
-			f_midi_transmit(midi, req);
+			req->length = 0;
+			f_midi_transmit(midi);
 			return;
 		}
 		break;
@@ -269,10 +269,12 @@ f_midi_complete(struct usb_ep *ep, struct usb_request *req)
 	case -ESHUTDOWN:	/* disconnect from host */
 		VDBG(cdev, "%s gone (%d), %d/%d\n", ep->name, status,
 				req->actual, req->length);
-		if (ep == midi->out_ep)
+		if (ep == midi->out_ep) {
 			f_midi_handle_out_data(ep, req);
-
-		free_ep_req(ep, req);
+			/* We don't need to free IN requests because it's handled
+			 * by the midi->in_req_fifo. */
+			free_ep_req(ep, req);
+		}
 		return;
 
 	case -EOVERFLOW:	/* buffer overrun on read means that
@@ -324,12 +326,11 @@ static int f_midi_start_ep(struct f_midi *midi,
 static int f_midi_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 {
 	struct f_midi *midi = func_to_midi(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
 	unsigned i;
 	int err;
 
-	/* For Control Device interface we do nothing */
-	if (intf == 0)
+	/* we only set alt for MIDIStreaming interface */
+	if (intf != midi->ms_id)
 		return 0;
 
 	err = f_midi_start_ep(midi, f, midi->in_ep);
@@ -340,23 +341,19 @@ static int f_midi_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 	if (err)
 		return err;
 
-	usb_ep_disable(midi->out_ep);
+	/* pre-allocate write usb requests to use on f_midi_transmit. */
+	while (kfifo_avail(&midi->in_req_fifo)) {
+		struct usb_request *req =
+			midi_alloc_ep_req(midi->in_ep, midi->buflen);
 
-	err = config_ep_by_speed(midi->gadget, f, midi->out_ep);
-	if (err) {
-		ERROR(cdev, "can't configure %s: %d\n",
-		      midi->out_ep->name, err);
-		return err;
-	}
+		if (req == NULL)
+			return -ENOMEM;
 
-	err = usb_ep_enable(midi->out_ep);
-	if (err) {
-		ERROR(cdev, "can't start %s: %d\n",
-		      midi->out_ep->name, err);
-		return err;
-	}
+		req->length = 0;
+		req->complete = f_midi_complete;
 
-	midi->out_ep->driver_data = midi;
+		kfifo_put(&midi->in_req_fifo, req);
+	}
 
 	/* allocate a bunch of read buffers and queue them all at once. */
 	for (i = 0; i < midi->qlen && err == 0; i++) {
@@ -368,9 +365,10 @@ static int f_midi_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 		req->complete = f_midi_complete;
 		err = usb_ep_queue(midi->out_ep, req, GFP_ATOMIC);
 		if (err) {
-			ERROR(midi, "%s queue req: %d\n",
+			ERROR(midi, "%s: couldn't enqueue request: %d\n",
 				    midi->out_ep->name, err);
 			free_ep_req(midi->out_ep, req);
+			return err;
 		}
 	}
 
@@ -381,6 +379,7 @@ static void f_midi_disable(struct usb_function *f)
 {
 	struct f_midi *midi = func_to_midi(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
+	struct usb_request *req = NULL;
 
 	DBG(cdev, "disable\n");
 
@@ -390,6 +389,10 @@ static void f_midi_disable(struct usb_function *f)
 	 */
 	usb_ep_disable(midi->in_ep);
 	usb_ep_disable(midi->out_ep);
+
+	/* release IN requests */
+	while (kfifo_get(&midi->in_req_fifo, &req))
+		free_ep_req(midi->in_ep, req);
 }
 
 static int f_midi_snd_free(struct snd_device *device)
@@ -511,57 +514,113 @@ static void f_midi_transmit_byte(struct usb_request *req,
 	}
 }
 
-static void f_midi_transmit(struct f_midi *midi, struct usb_request *req)
+static void f_midi_drop_out_substreams(struct f_midi *midi)
 {
-	struct usb_ep *ep = midi->in_ep;
-	int i;
-
-	if (!ep)
-		return;
-
-	if (!req)
-		req = midi_alloc_ep_req(ep, midi->buflen);
-
-	if (!req) {
-		ERROR(midi, "%s: alloc_ep_request failed\n", __func__);
-		return;
-	}
-	req->length = 0;
-	req->complete = f_midi_complete;
+	unsigned int i;
 
 	for (i = 0; i < MAX_PORTS; i++) {
 		struct gmidi_in_port *port = midi->in_port[i];
 		struct snd_rawmidi_substream *substream = midi->in_substream[i];
 
-		if (!port || !port->active || !substream)
+		if (!port)
+			break;
+
+		if (!port->active || !substream)
 			continue;
 
-		while (req->length + 3 < midi->buflen) {
-			uint8_t b;
-			if (snd_rawmidi_transmit(substream, &b, 1) != 1) {
-				port->active = 0;
+		snd_rawmidi_drop_output(substream);
+	}
+}
+
+static void f_midi_transmit(struct f_midi *midi)
+{
+	struct usb_ep *ep = midi->in_ep;
+	bool active;
+
+	/* We only care about USB requests if IN endpoint is enabled */
+	if (!ep || !ep->enabled)
+		goto drop_out;
+
+	do {
+		struct usb_request *req = NULL;
+		unsigned int len, i;
+
+		active = false;
+
+		/* We peek the request in order to reuse it if it fails
+		 * to enqueue on its endpoint */
+		len = kfifo_peek(&midi->in_req_fifo, &req);
+		if (len != 1) {
+			ERROR(midi, "%s: Couldn't get usb request\n", __func__);
+			goto drop_out;
+		}
+
+		/* If buffer overrun, then we ignore this transmission.
+		 * IMPORTANT: This will cause the user-space rawmidi device to block until a) usb
+		 * requests have been completed or b) snd_rawmidi_write() times out. */
+		if (req->length > 0)
+			return;
+
+		for (i = midi->in_last_port; i < MAX_PORTS; i++) {
+			struct gmidi_in_port *port = midi->in_port[i];
+			struct snd_rawmidi_substream *substream = midi->in_substream[i];
+
+			if (!port) {
+				/* Reset counter when we reach the last available port */
+				midi->in_last_port = 0;
+				break;
+			}
+
+			if (!port->active || !substream)
+				continue;
+
+			while (req->length + 3 < midi->buflen) {
+				uint8_t b;
+
+				if (snd_rawmidi_transmit(substream, &b, 1) != 1) {
+					port->active = 0;
+					break;
+				}
+				f_midi_transmit_byte(req, port, b);
+			}
+
+			active = !!port->active;
+			/* Check if last port is still active, which means that
+			 * there is still data on that substream but this current
+			 * request run out of space. */
+			if (active) {
+				midi->in_last_port = i;
+				/* There is no need to re-iterate though midi ports. */
 				break;
 			}
-			f_midi_transmit_byte(req, port, b);
 		}
-	}
 
-	if (req->length > 0 && ep->enabled) {
-		int err;
+		if (req->length > 0) {
+			int err;
 
-		err = usb_ep_queue(ep, req, GFP_ATOMIC);
-		if (err < 0)
-			ERROR(midi, "%s queue req: %d\n",
-			      midi->in_ep->name, err);
-	} else {
-		free_ep_req(ep, req);
-	}
+			err = usb_ep_queue(ep, req, GFP_ATOMIC);
+			if (err < 0) {
+				ERROR(midi, "%s failed to queue req: %d\n",
+				      midi->in_ep->name, err);
+				req->length = 0; /* Re-use request next time. */
+			} else {
+				/* Upon success, put request at the back of the queue. */
+				kfifo_skip(&midi->in_req_fifo);
+				kfifo_put(&midi->in_req_fifo, req);
+			}
+		}
+	} while (active);
+
+	return;
+
+drop_out:
+	f_midi_drop_out_substreams(midi);
 }
 
 static void f_midi_in_tasklet(unsigned long data)
 {
 	struct f_midi *midi = (struct f_midi *) data;
-	f_midi_transmit(midi, NULL);
+	f_midi_transmit(midi);
 }
 
 static int f_midi_in_open(struct snd_rawmidi_substream *substream)
@@ -687,6 +746,7 @@ static int f_midi_register_card(struct f_midi *midi)
 		goto fail;
 	}
 	midi->rmidi = rmidi;
+	midi->in_last_port = 0;
 	strcpy(rmidi->name, card->shortname);
 	rmidi->info_flags = SNDRV_RAWMIDI_INFO_OUTPUT |
 			    SNDRV_RAWMIDI_INFO_INPUT |
@@ -755,6 +815,7 @@ static int f_midi_bind(struct usb_configuration *c, struct usb_function *f)
 		goto fail;
 	ms_interface_desc.bInterfaceNumber = status;
 	ac_header_desc.baInterfaceNr[0] = status;
+	midi->ms_id = status;
 
 	status = -ENODEV;
 
@@ -1075,6 +1136,7 @@ static void f_midi_free(struct usb_function *f)
 	mutex_lock(&opts->lock);
 	for (i = opts->in_ports - 1; i >= 0; --i)
 		kfree(midi->in_port[i]);
+	kfifo_free(&midi->in_req_fifo);
 	kfree(midi);
 	--opts->refcnt;
 	mutex_unlock(&opts->lock);
@@ -1148,6 +1210,12 @@ static struct usb_function *f_midi_alloc(struct usb_function_instance *fi)
 	midi->index = opts->index;
 	midi->buflen = opts->buflen;
 	midi->qlen = opts->qlen;
+	midi->in_last_port = 0;
+
+	status = kfifo_alloc(&midi->in_req_fifo, midi->qlen, GFP_KERNEL);
+	if (status)
+		goto setup_fail;
+
 	++opts->refcnt;
 	mutex_unlock(&opts->lock);
 
diff --git a/drivers/usb/gadget/function/f_printer.c b/drivers/usb/gadget/function/f_printer.c
index 0fbfb2b2aa08..26ccad5d8680 100644
--- a/drivers/usb/gadget/function/f_printer.c
+++ b/drivers/usb/gadget/function/f_printer.c
@@ -673,7 +673,7 @@ printer_fsync(struct file *fd, loff_t start, loff_t end, int datasync)
 	unsigned long		flags;
 	int			tx_list_empty;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	spin_lock_irqsave(&dev->lock, flags);
 	tx_list_empty = (likely(list_empty(&dev->tx_reqs)));
 	spin_unlock_irqrestore(&dev->lock, flags);
@@ -683,7 +683,7 @@ printer_fsync(struct file *fd, loff_t start, loff_t end, int datasync)
 		wait_event_interruptible(dev->tx_flush_wait,
 				(likely(list_empty(&dev->tx_reqs_active))));
 	}
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 
 	return 0;
 }
diff --git a/drivers/usb/gadget/function/f_sourcesink.c b/drivers/usb/gadget/function/f_sourcesink.c
index 9f3ced62d916..242ba5caffe5 100644
--- a/drivers/usb/gadget/function/f_sourcesink.c
+++ b/drivers/usb/gadget/function/f_sourcesink.c
@@ -34,13 +34,6 @@
  * plus two that support control-OUT tests.  If the optional "autoresume"
  * mode is enabled, it provides good functional coverage for the "USBCV"
  * test harness from USB-IF.
- *
- * Note that because this doesn't queue more than one request at a time,
- * some other function must be used to test queueing logic.  The network
- * link (g_ether) is the best overall option for that, since its TX and RX
- * queues are relatively independent, will receive a range of packet sizes,
- * and can often be made to run out completely.  Those issues are important
- * when stress testing peripheral controller drivers.
  */
 struct f_sourcesink {
 	struct usb_function	function;
@@ -57,6 +50,8 @@ struct f_sourcesink {
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
 	unsigned buflen;
+	unsigned bulk_qlen;
+	unsigned iso_qlen;
 };
 
 static inline struct f_sourcesink *func_to_ss(struct usb_function *f)
@@ -303,12 +298,6 @@ static inline struct usb_request *ss_alloc_ep_req(struct usb_ep *ep, int len)
 	return alloc_ep_req(ep, len, ss->buflen);
 }
 
-void free_ep_req(struct usb_ep *ep, struct usb_request *req)
-{
-	kfree(req->buf);
-	usb_ep_free_request(ep, req);
-}
-
 static void disable_ep(struct usb_composite_dev *cdev, struct usb_ep *ep)
 {
 	int			value;
@@ -595,31 +584,33 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 {
 	struct usb_ep		*ep;
 	struct usb_request	*req;
-	int			i, size, status;
-
-	for (i = 0; i < 8; i++) {
-		if (is_iso) {
-			switch (speed) {
-			case USB_SPEED_SUPER:
-				size = ss->isoc_maxpacket *
-						(ss->isoc_mult + 1) *
-						(ss->isoc_maxburst + 1);
-				break;
-			case USB_SPEED_HIGH:
-				size = ss->isoc_maxpacket * (ss->isoc_mult + 1);
-				break;
-			default:
-				size = ss->isoc_maxpacket > 1023 ?
-						1023 : ss->isoc_maxpacket;
-				break;
-			}
-			ep = is_in ? ss->iso_in_ep : ss->iso_out_ep;
-			req = ss_alloc_ep_req(ep, size);
-		} else {
-			ep = is_in ? ss->in_ep : ss->out_ep;
-			req = ss_alloc_ep_req(ep, 0);
+	int			i, size, qlen, status = 0;
+
+	if (is_iso) {
+		switch (speed) {
+		case USB_SPEED_SUPER:
+			size = ss->isoc_maxpacket *
+					(ss->isoc_mult + 1) *
+					(ss->isoc_maxburst + 1);
+			break;
+		case USB_SPEED_HIGH:
+			size = ss->isoc_maxpacket * (ss->isoc_mult + 1);
+			break;
+		default:
+			size = ss->isoc_maxpacket > 1023 ?
+					1023 : ss->isoc_maxpacket;
+			break;
 		}
+		ep = is_in ? ss->iso_in_ep : ss->iso_out_ep;
+		qlen = ss->iso_qlen;
+	} else {
+		ep = is_in ? ss->in_ep : ss->out_ep;
+		qlen = ss->bulk_qlen;
+		size = 0;
+	}
 
+	for (i = 0; i < qlen; i++) {
+		req = ss_alloc_ep_req(ep, size);
 		if (!req)
 			return -ENOMEM;
 
@@ -638,10 +629,8 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 			      is_iso ? "ISO-" : "", is_in ? "IN" : "OUT",
 			      ep->name, status);
 			free_ep_req(ep, req);
+			return status;
 		}
-
-		if (!is_iso)
-			break;
 	}
 
 	return status;
@@ -869,6 +858,8 @@ static struct usb_function *source_sink_alloc_func(
 	ss->isoc_mult = ss_opts->isoc_mult;
 	ss->isoc_maxburst = ss_opts->isoc_maxburst;
 	ss->buflen = ss_opts->bulk_buflen;
+	ss->bulk_qlen = ss_opts->bulk_qlen;
+	ss->iso_qlen = ss_opts->iso_qlen;
 
 	ss->function.name = "source/sink";
 	ss->function.bind = sourcesink_bind;
@@ -1153,6 +1144,82 @@ end:
 
 CONFIGFS_ATTR(f_ss_opts_, bulk_buflen);
 
+static ssize_t f_ss_opts_bulk_qlen_show(struct config_item *item, char *page)
+{
+	struct f_ss_opts *opts = to_f_ss_opts(item);
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%u\n", opts->bulk_qlen);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_bulk_qlen_store(struct config_item *item,
+					   const char *page, size_t len)
+{
+	struct f_ss_opts *opts = to_f_ss_opts(item);
+	int ret;
+	u32 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou32(page, 0, &num);
+	if (ret)
+		goto end;
+
+	opts->bulk_qlen = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+CONFIGFS_ATTR(f_ss_opts_, bulk_qlen);
+
+static ssize_t f_ss_opts_iso_qlen_show(struct config_item *item, char *page)
+{
+	struct f_ss_opts *opts = to_f_ss_opts(item);
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%u\n", opts->iso_qlen);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_iso_qlen_store(struct config_item *item,
+					   const char *page, size_t len)
+{
+	struct f_ss_opts *opts = to_f_ss_opts(item);
+	int ret;
+	u32 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou32(page, 0, &num);
+	if (ret)
+		goto end;
+
+	opts->iso_qlen = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+CONFIGFS_ATTR(f_ss_opts_, iso_qlen);
+
 static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_attr_pattern,
 	&f_ss_opts_attr_isoc_interval,
@@ -1160,6 +1227,8 @@ static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_attr_isoc_mult,
 	&f_ss_opts_attr_isoc_maxburst,
 	&f_ss_opts_attr_bulk_buflen,
+	&f_ss_opts_attr_bulk_qlen,
+	&f_ss_opts_attr_iso_qlen,
 	NULL,
 };
 
@@ -1189,6 +1258,8 @@ static struct usb_function_instance *source_sink_alloc_inst(void)
 	ss_opts->isoc_interval = GZERO_ISOC_INTERVAL;
 	ss_opts->isoc_maxpacket = GZERO_ISOC_MAXPACKET;
 	ss_opts->bulk_buflen = GZERO_BULK_BUFLEN;
+	ss_opts->bulk_qlen = GZERO_SS_BULK_QLEN;
+	ss_opts->iso_qlen = GZERO_SS_ISO_QLEN;
 
 	config_group_init_type_name(&ss_opts->func_inst.group, "",
 				    &ss_func_type);
diff --git a/drivers/usb/gadget/function/f_tcm.c b/drivers/usb/gadget/function/f_tcm.c
new file mode 100644
index 000000000000..bad007b5a190
--- /dev/null
+++ b/drivers/usb/gadget/function/f_tcm.c
@@ -0,0 +1,2381 @@
+/* Target based USB-Gadget
+ *
+ * UAS protocol handling, target callbacks, configfs handling,
+ * BBB (USB Mass Storage Class Bulk-Only (BBB) and Transport protocol handling.
+ *
+ * Author: Sebastian Andrzej Siewior <bigeasy at linutronix dot de>
+ * License: GPLv2 as published by FSF.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/string.h>
+#include <linux/configfs.h>
+#include <linux/ctype.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/composite.h>
+#include <linux/usb/gadget.h>
+#include <linux/usb/storage.h>
+#include <scsi/scsi_tcq.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <asm/unaligned.h>
+
+#include "tcm.h"
+#include "u_tcm.h"
+#include "configfs.h"
+
+#define TPG_INSTANCES		1
+
+struct tpg_instance {
+	struct usb_function_instance	*func_inst;
+	struct usbg_tpg			*tpg;
+};
+
+static struct tpg_instance tpg_instances[TPG_INSTANCES];
+
+static DEFINE_MUTEX(tpg_instances_lock);
+
+static inline struct f_uas *to_f_uas(struct usb_function *f)
+{
+	return container_of(f, struct f_uas, function);
+}
+
+static void usbg_cmd_release(struct kref *);
+
+static inline void usbg_cleanup_cmd(struct usbg_cmd *cmd)
+{
+	kref_put(&cmd->ref, usbg_cmd_release);
+}
+
+/* Start bot.c code */
+
+static int bot_enqueue_cmd_cbw(struct f_uas *fu)
+{
+	int ret;
+
+	if (fu->flags & USBG_BOT_CMD_PEND)
+		return 0;
+
+	ret = usb_ep_queue(fu->ep_out, fu->cmd.req, GFP_ATOMIC);
+	if (!ret)
+		fu->flags |= USBG_BOT_CMD_PEND;
+	return ret;
+}
+
+static void bot_status_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct f_uas *fu = cmd->fu;
+
+	usbg_cleanup_cmd(cmd);
+	if (req->status < 0) {
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+		return;
+	}
+
+	/* CSW completed, wait for next CBW */
+	bot_enqueue_cmd_cbw(fu);
+}
+
+static void bot_enqueue_sense_code(struct f_uas *fu, struct usbg_cmd *cmd)
+{
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	int ret;
+	unsigned int csw_stat;
+
+	csw_stat = cmd->csw_code;
+	csw->Tag = cmd->bot_tag;
+	csw->Status = csw_stat;
+	fu->bot_status.req->context = cmd;
+	ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_ATOMIC);
+	if (ret)
+		pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
+}
+
+static void bot_err_compl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct f_uas *fu = cmd->fu;
+
+	if (req->status < 0)
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+
+	if (cmd->data_len) {
+		if (cmd->data_len > ep->maxpacket) {
+			req->length = ep->maxpacket;
+			cmd->data_len -= ep->maxpacket;
+		} else {
+			req->length = cmd->data_len;
+			cmd->data_len = 0;
+		}
+
+		usb_ep_queue(ep, req, GFP_ATOMIC);
+		return;
+	}
+	bot_enqueue_sense_code(fu, cmd);
+}
+
+static void bot_send_bad_status(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	struct usb_request *req;
+	struct usb_ep *ep;
+
+	csw->Residue = cpu_to_le32(cmd->data_len);
+
+	if (cmd->data_len) {
+		if (cmd->is_read) {
+			ep = fu->ep_in;
+			req = fu->bot_req_in;
+		} else {
+			ep = fu->ep_out;
+			req = fu->bot_req_out;
+		}
+
+		if (cmd->data_len > fu->ep_in->maxpacket) {
+			req->length = ep->maxpacket;
+			cmd->data_len -= ep->maxpacket;
+		} else {
+			req->length = cmd->data_len;
+			cmd->data_len = 0;
+		}
+		req->complete = bot_err_compl;
+		req->context = cmd;
+		req->buf = fu->cmd.buf;
+		usb_ep_queue(ep, req, GFP_KERNEL);
+	} else {
+		bot_enqueue_sense_code(fu, cmd);
+	}
+}
+
+static int bot_send_status(struct usbg_cmd *cmd, bool moved_data)
+{
+	struct f_uas *fu = cmd->fu;
+	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
+	int ret;
+
+	if (cmd->se_cmd.scsi_status == SAM_STAT_GOOD) {
+		if (!moved_data && cmd->data_len) {
+			/*
+			 * the host wants to move data, we don't. Fill / empty
+			 * the pipe and then send the csw with reside set.
+			 */
+			cmd->csw_code = US_BULK_STAT_OK;
+			bot_send_bad_status(cmd);
+			return 0;
+		}
+
+		csw->Tag = cmd->bot_tag;
+		csw->Residue = cpu_to_le32(0);
+		csw->Status = US_BULK_STAT_OK;
+		fu->bot_status.req->context = cmd;
+
+		ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_KERNEL);
+		if (ret)
+			pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
+	} else {
+		cmd->csw_code = US_BULK_STAT_FAIL;
+		bot_send_bad_status(cmd);
+	}
+	return 0;
+}
+
+/*
+ * Called after command (no data transfer) or after the write (to device)
+ * operation is completed
+ */
+static int bot_send_status_response(struct usbg_cmd *cmd)
+{
+	bool moved_data = false;
+
+	if (!cmd->is_read)
+		moved_data = true;
+	return bot_send_status(cmd, moved_data);
+}
+
+/* Read request completed, now we have to send the CSW */
+static void bot_read_compl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+
+	if (req->status < 0)
+		pr_err("ERR %s(%d)\n", __func__, __LINE__);
+
+	bot_send_status(cmd, true);
+}
+
+static int bot_send_read_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	int ret;
+
+	if (!cmd->data_len) {
+		cmd->csw_code = US_BULK_STAT_PHASE;
+		bot_send_bad_status(cmd);
+		return 0;
+	}
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		sg_copy_to_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+
+		fu->bot_req_in->buf = cmd->data_buf;
+	} else {
+		fu->bot_req_in->buf = NULL;
+		fu->bot_req_in->num_sgs = se_cmd->t_data_nents;
+		fu->bot_req_in->sg = se_cmd->t_data_sg;
+	}
+
+	fu->bot_req_in->complete = bot_read_compl;
+	fu->bot_req_in->length = se_cmd->data_length;
+	fu->bot_req_in->context = cmd;
+	ret = usb_ep_queue(fu->ep_in, fu->bot_req_in, GFP_ATOMIC);
+	if (ret)
+		pr_err("%s(%d)\n", __func__, __LINE__);
+	return 0;
+}
+
+static void usbg_data_write_cmpl(struct usb_ep *, struct usb_request *);
+static int usbg_prepare_w_request(struct usbg_cmd *, struct usb_request *);
+
+static int bot_send_write_request(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	int ret;
+
+	init_completion(&cmd->write_complete);
+	cmd->fu = fu;
+
+	if (!cmd->data_len) {
+		cmd->csw_code = US_BULK_STAT_PHASE;
+		return -EINVAL;
+	}
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_KERNEL);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		fu->bot_req_out->buf = cmd->data_buf;
+	} else {
+		fu->bot_req_out->buf = NULL;
+		fu->bot_req_out->num_sgs = se_cmd->t_data_nents;
+		fu->bot_req_out->sg = se_cmd->t_data_sg;
+	}
+
+	fu->bot_req_out->complete = usbg_data_write_cmpl;
+	fu->bot_req_out->length = se_cmd->data_length;
+	fu->bot_req_out->context = cmd;
+
+	ret = usbg_prepare_w_request(cmd, fu->bot_req_out);
+	if (ret)
+		goto cleanup;
+	ret = usb_ep_queue(fu->ep_out, fu->bot_req_out, GFP_KERNEL);
+	if (ret)
+		pr_err("%s(%d)\n", __func__, __LINE__);
+
+	wait_for_completion(&cmd->write_complete);
+	target_execute_cmd(se_cmd);
+cleanup:
+	return ret;
+}
+
+static int bot_submit_command(struct f_uas *, void *, unsigned int);
+
+static void bot_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_uas *fu = req->context;
+	int ret;
+
+	fu->flags &= ~USBG_BOT_CMD_PEND;
+
+	if (req->status < 0)
+		return;
+
+	ret = bot_submit_command(fu, req->buf, req->actual);
+	if (ret)
+		pr_err("%s(%d): %d\n", __func__, __LINE__, ret);
+}
+
+static int bot_prepare_reqs(struct f_uas *fu)
+{
+	int ret;
+
+	fu->bot_req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!fu->bot_req_in)
+		goto err;
+
+	fu->bot_req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!fu->bot_req_out)
+		goto err_out;
+
+	fu->cmd.req = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!fu->cmd.req)
+		goto err_cmd;
+
+	fu->bot_status.req = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!fu->bot_status.req)
+		goto err_sts;
+
+	fu->bot_status.req->buf = &fu->bot_status.csw;
+	fu->bot_status.req->length = US_BULK_CS_WRAP_LEN;
+	fu->bot_status.req->complete = bot_status_complete;
+	fu->bot_status.csw.Signature = cpu_to_le32(US_BULK_CS_SIGN);
+
+	fu->cmd.buf = kmalloc(fu->ep_out->maxpacket, GFP_KERNEL);
+	if (!fu->cmd.buf)
+		goto err_buf;
+
+	fu->cmd.req->complete = bot_cmd_complete;
+	fu->cmd.req->buf = fu->cmd.buf;
+	fu->cmd.req->length = fu->ep_out->maxpacket;
+	fu->cmd.req->context = fu;
+
+	ret = bot_enqueue_cmd_cbw(fu);
+	if (ret)
+		goto err_queue;
+	return 0;
+err_queue:
+	kfree(fu->cmd.buf);
+	fu->cmd.buf = NULL;
+err_buf:
+	usb_ep_free_request(fu->ep_in, fu->bot_status.req);
+err_sts:
+	usb_ep_free_request(fu->ep_out, fu->cmd.req);
+	fu->cmd.req = NULL;
+err_cmd:
+	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
+	fu->bot_req_out = NULL;
+err_out:
+	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
+	fu->bot_req_in = NULL;
+err:
+	pr_err("BOT: endpoint setup failed\n");
+	return -ENOMEM;
+}
+
+static void bot_cleanup_old_alt(struct f_uas *fu)
+{
+	if (!(fu->flags & USBG_ENABLED))
+		return;
+
+	usb_ep_disable(fu->ep_in);
+	usb_ep_disable(fu->ep_out);
+
+	if (!fu->bot_req_in)
+		return;
+
+	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
+	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
+	usb_ep_free_request(fu->ep_out, fu->cmd.req);
+	usb_ep_free_request(fu->ep_out, fu->bot_status.req);
+
+	kfree(fu->cmd.buf);
+
+	fu->bot_req_in = NULL;
+	fu->bot_req_out = NULL;
+	fu->cmd.req = NULL;
+	fu->bot_status.req = NULL;
+	fu->cmd.buf = NULL;
+}
+
+static void bot_set_alt(struct f_uas *fu)
+{
+	struct usb_function *f = &fu->function;
+	struct usb_gadget *gadget = f->config->cdev->gadget;
+	int ret;
+
+	fu->flags = USBG_IS_BOT;
+
+	config_ep_by_speed(gadget, f, fu->ep_in);
+	ret = usb_ep_enable(fu->ep_in);
+	if (ret)
+		goto err_b_in;
+
+	config_ep_by_speed(gadget, f, fu->ep_out);
+	ret = usb_ep_enable(fu->ep_out);
+	if (ret)
+		goto err_b_out;
+
+	ret = bot_prepare_reqs(fu);
+	if (ret)
+		goto err_wq;
+	fu->flags |= USBG_ENABLED;
+	pr_info("Using the BOT protocol\n");
+	return;
+err_wq:
+	usb_ep_disable(fu->ep_out);
+err_b_out:
+	usb_ep_disable(fu->ep_in);
+err_b_in:
+	fu->flags = USBG_IS_BOT;
+}
+
+static int usbg_bot_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_uas *fu = to_f_uas(f);
+	struct usb_composite_dev *cdev = f->config->cdev;
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+	u16 w_length = le16_to_cpu(ctrl->wLength);
+	int luns;
+	u8 *ret_lun;
+
+	switch (ctrl->bRequest) {
+	case US_BULK_GET_MAX_LUN:
+		if (ctrl->bRequestType != (USB_DIR_IN | USB_TYPE_CLASS |
+					USB_RECIP_INTERFACE))
+			return -ENOTSUPP;
+
+		if (w_length < 1)
+			return -EINVAL;
+		if (w_value != 0)
+			return -EINVAL;
+		luns = atomic_read(&fu->tpg->tpg_port_count);
+		if (!luns) {
+			pr_err("No LUNs configured?\n");
+			return -EINVAL;
+		}
+		/*
+		 * If 4 LUNs are present we return 3 i.e. LUN 0..3 can be
+		 * accessed. The upper limit is 0xf
+		 */
+		luns--;
+		if (luns > 0xf) {
+			pr_info_once("Limiting the number of luns to 16\n");
+			luns = 0xf;
+		}
+		ret_lun = cdev->req->buf;
+		*ret_lun = luns;
+		cdev->req->length = 1;
+		return usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
+
+	case US_BULK_RESET_REQUEST:
+		/* XXX maybe we should remove previous requests for IN + OUT */
+		bot_enqueue_cmd_cbw(fu);
+		return 0;
+	}
+	return -ENOTSUPP;
+}
+
+/* Start uas.c code */
+
+static void uasp_cleanup_one_stream(struct f_uas *fu, struct uas_stream *stream)
+{
+	/* We have either all three allocated or none */
+	if (!stream->req_in)
+		return;
+
+	usb_ep_free_request(fu->ep_in, stream->req_in);
+	usb_ep_free_request(fu->ep_out, stream->req_out);
+	usb_ep_free_request(fu->ep_status, stream->req_status);
+
+	stream->req_in = NULL;
+	stream->req_out = NULL;
+	stream->req_status = NULL;
+}
+
+static void uasp_free_cmdreq(struct f_uas *fu)
+{
+	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
+	kfree(fu->cmd.buf);
+	fu->cmd.req = NULL;
+	fu->cmd.buf = NULL;
+}
+
+static void uasp_cleanup_old_alt(struct f_uas *fu)
+{
+	int i;
+
+	if (!(fu->flags & USBG_ENABLED))
+		return;
+
+	usb_ep_disable(fu->ep_in);
+	usb_ep_disable(fu->ep_out);
+	usb_ep_disable(fu->ep_status);
+	usb_ep_disable(fu->ep_cmd);
+
+	for (i = 0; i < UASP_SS_EP_COMP_NUM_STREAMS; i++)
+		uasp_cleanup_one_stream(fu, &fu->stream[i]);
+	uasp_free_cmdreq(fu);
+}
+
+static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req);
+
+static int uasp_prepare_r_request(struct usbg_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct f_uas *fu = cmd->fu;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+	struct uas_stream *stream = cmd->stream;
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		sg_copy_to_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+
+		stream->req_in->buf = cmd->data_buf;
+	} else {
+		stream->req_in->buf = NULL;
+		stream->req_in->num_sgs = se_cmd->t_data_nents;
+		stream->req_in->sg = se_cmd->t_data_sg;
+	}
+
+	stream->req_in->complete = uasp_status_data_cmpl;
+	stream->req_in->length = se_cmd->data_length;
+	stream->req_in->context = cmd;
+
+	cmd->state = UASP_SEND_STATUS;
+	return 0;
+}
+
+static void uasp_prepare_status(struct usbg_cmd *cmd)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct sense_iu *iu = &cmd->sense_iu;
+	struct uas_stream *stream = cmd->stream;
+
+	cmd->state = UASP_QUEUE_COMMAND;
+	iu->iu_id = IU_ID_STATUS;
+	iu->tag = cpu_to_be16(cmd->tag);
+
+	/*
+	 * iu->status_qual = cpu_to_be16(STATUS QUALIFIER SAM-4. Where R U?);
+	 */
+	iu->len = cpu_to_be16(se_cmd->scsi_sense_length);
+	iu->status = se_cmd->scsi_status;
+	stream->req_status->context = cmd;
+	stream->req_status->length = se_cmd->scsi_sense_length + 16;
+	stream->req_status->buf = iu;
+	stream->req_status->complete = uasp_status_data_cmpl;
+}
+
+static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct uas_stream *stream = cmd->stream;
+	struct f_uas *fu = cmd->fu;
+	int ret;
+
+	if (req->status < 0)
+		goto cleanup;
+
+	switch (cmd->state) {
+	case UASP_SEND_DATA:
+		ret = uasp_prepare_r_request(cmd);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_RECEIVE_DATA:
+		ret = usbg_prepare_w_request(cmd, stream->req_out);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_SEND_STATUS:
+		uasp_prepare_status(cmd);
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+		break;
+
+	case UASP_QUEUE_COMMAND:
+		usbg_cleanup_cmd(cmd);
+		usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+		break;
+
+	default:
+		BUG();
+	}
+	return;
+
+cleanup:
+	usbg_cleanup_cmd(cmd);
+}
+
+static int uasp_send_status_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+	stream->req_status->complete = uasp_status_data_cmpl;
+	stream->req_status->context = cmd;
+	cmd->fu = fu;
+	uasp_prepare_status(cmd);
+	return usb_ep_queue(fu->ep_status, stream->req_status, GFP_ATOMIC);
+}
+
+static int uasp_send_read_response(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+	int ret;
+
+	cmd->fu = fu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+	if (fu->flags & USBG_USE_STREAMS) {
+
+		ret = uasp_prepare_r_request(cmd);
+		if (ret)
+			goto out;
+		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
+		if (ret) {
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+			kfree(cmd->data_buf);
+			cmd->data_buf = NULL;
+		}
+
+	} else {
+
+		iu->iu_id = IU_ID_READ_READY;
+		iu->tag = cpu_to_be16(cmd->tag);
+
+		stream->req_status->complete = uasp_status_data_cmpl;
+		stream->req_status->context = cmd;
+
+		cmd->state = UASP_SEND_DATA;
+		stream->req_status->buf = iu;
+		stream->req_status->length = sizeof(struct iu);
+
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
+	}
+out:
+	return ret;
+}
+
+static int uasp_send_write_request(struct usbg_cmd *cmd)
+{
+	struct f_uas *fu = cmd->fu;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct uas_stream *stream = cmd->stream;
+	struct sense_iu *iu = &cmd->sense_iu;
+	int ret;
+
+	init_completion(&cmd->write_complete);
+	cmd->fu = fu;
+
+	iu->tag = cpu_to_be16(cmd->tag);
+
+	if (fu->flags & USBG_USE_STREAMS) {
+
+		ret = usbg_prepare_w_request(cmd, stream->req_out);
+		if (ret)
+			goto cleanup;
+		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d)\n", __func__, __LINE__);
+
+	} else {
+
+		iu->iu_id = IU_ID_WRITE_READY;
+		iu->tag = cpu_to_be16(cmd->tag);
+
+		stream->req_status->complete = uasp_status_data_cmpl;
+		stream->req_status->context = cmd;
+
+		cmd->state = UASP_RECEIVE_DATA;
+		stream->req_status->buf = iu;
+		stream->req_status->length = sizeof(struct iu);
+
+		ret = usb_ep_queue(fu->ep_status, stream->req_status,
+				GFP_ATOMIC);
+		if (ret)
+			pr_err("%s(%d)\n", __func__, __LINE__);
+	}
+
+	wait_for_completion(&cmd->write_complete);
+	target_execute_cmd(se_cmd);
+cleanup:
+	return ret;
+}
+
+static int usbg_submit_command(struct f_uas *, void *, unsigned int);
+
+static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
+{
+	struct f_uas *fu = req->context;
+	int ret;
+
+	if (req->status < 0)
+		return;
+
+	ret = usbg_submit_command(fu, req->buf, req->actual);
+	/*
+	 * Once we tune for performance enqueue the command req here again so
+	 * we can receive a second command while we processing this one. Pay
+	 * attention to properly sync STAUS endpoint with DATA IN + OUT so you
+	 * don't break HS.
+	 */
+	if (!ret)
+		return;
+	usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+}
+
+static int uasp_alloc_stream_res(struct f_uas *fu, struct uas_stream *stream)
+{
+	stream->req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
+	if (!stream->req_in)
+		goto out;
+
+	stream->req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
+	if (!stream->req_out)
+		goto err_out;
+
+	stream->req_status = usb_ep_alloc_request(fu->ep_status, GFP_KERNEL);
+	if (!stream->req_status)
+		goto err_sts;
+
+	return 0;
+err_sts:
+	usb_ep_free_request(fu->ep_status, stream->req_status);
+	stream->req_status = NULL;
+err_out:
+	usb_ep_free_request(fu->ep_out, stream->req_out);
+	stream->req_out = NULL;
+out:
+	return -ENOMEM;
+}
+
+static int uasp_alloc_cmd(struct f_uas *fu)
+{
+	fu->cmd.req = usb_ep_alloc_request(fu->ep_cmd, GFP_KERNEL);
+	if (!fu->cmd.req)
+		goto err;
+
+	fu->cmd.buf = kmalloc(fu->ep_cmd->maxpacket, GFP_KERNEL);
+	if (!fu->cmd.buf)
+		goto err_buf;
+
+	fu->cmd.req->complete = uasp_cmd_complete;
+	fu->cmd.req->buf = fu->cmd.buf;
+	fu->cmd.req->length = fu->ep_cmd->maxpacket;
+	fu->cmd.req->context = fu;
+	return 0;
+
+err_buf:
+	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
+err:
+	return -ENOMEM;
+}
+
+static void uasp_setup_stream_res(struct f_uas *fu, int max_streams)
+{
+	int i;
+
+	for (i = 0; i < max_streams; i++) {
+		struct uas_stream *s = &fu->stream[i];
+
+		s->req_in->stream_id = i + 1;
+		s->req_out->stream_id = i + 1;
+		s->req_status->stream_id = i + 1;
+	}
+}
+
+static int uasp_prepare_reqs(struct f_uas *fu)
+{
+	int ret;
+	int i;
+	int max_streams;
+
+	if (fu->flags & USBG_USE_STREAMS)
+		max_streams = UASP_SS_EP_COMP_NUM_STREAMS;
+	else
+		max_streams = 1;
+
+	for (i = 0; i < max_streams; i++) {
+		ret = uasp_alloc_stream_res(fu, &fu->stream[i]);
+		if (ret)
+			goto err_cleanup;
+	}
+
+	ret = uasp_alloc_cmd(fu);
+	if (ret)
+		goto err_free_stream;
+	uasp_setup_stream_res(fu, max_streams);
+
+	ret = usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
+	if (ret)
+		goto err_free_stream;
+
+	return 0;
+
+err_free_stream:
+	uasp_free_cmdreq(fu);
+
+err_cleanup:
+	if (i) {
+		do {
+			uasp_cleanup_one_stream(fu, &fu->stream[i - 1]);
+			i--;
+		} while (i);
+	}
+	pr_err("UASP: endpoint setup failed\n");
+	return ret;
+}
+
+static void uasp_set_alt(struct f_uas *fu)
+{
+	struct usb_function *f = &fu->function;
+	struct usb_gadget *gadget = f->config->cdev->gadget;
+	int ret;
+
+	fu->flags = USBG_IS_UAS;
+
+	if (gadget->speed == USB_SPEED_SUPER)
+		fu->flags |= USBG_USE_STREAMS;
+
+	config_ep_by_speed(gadget, f, fu->ep_in);
+	ret = usb_ep_enable(fu->ep_in);
+	if (ret)
+		goto err_b_in;
+
+	config_ep_by_speed(gadget, f, fu->ep_out);
+	ret = usb_ep_enable(fu->ep_out);
+	if (ret)
+		goto err_b_out;
+
+	config_ep_by_speed(gadget, f, fu->ep_cmd);
+	ret = usb_ep_enable(fu->ep_cmd);
+	if (ret)
+		goto err_cmd;
+	config_ep_by_speed(gadget, f, fu->ep_status);
+	ret = usb_ep_enable(fu->ep_status);
+	if (ret)
+		goto err_status;
+
+	ret = uasp_prepare_reqs(fu);
+	if (ret)
+		goto err_wq;
+	fu->flags |= USBG_ENABLED;
+
+	pr_info("Using the UAS protocol\n");
+	return;
+err_wq:
+	usb_ep_disable(fu->ep_status);
+err_status:
+	usb_ep_disable(fu->ep_cmd);
+err_cmd:
+	usb_ep_disable(fu->ep_out);
+err_b_out:
+	usb_ep_disable(fu->ep_in);
+err_b_in:
+	fu->flags = 0;
+}
+
+static int get_cmd_dir(const unsigned char *cdb)
+{
+	int ret;
+
+	switch (cdb[0]) {
+	case READ_6:
+	case READ_10:
+	case READ_12:
+	case READ_16:
+	case INQUIRY:
+	case MODE_SENSE:
+	case MODE_SENSE_10:
+	case SERVICE_ACTION_IN_16:
+	case MAINTENANCE_IN:
+	case PERSISTENT_RESERVE_IN:
+	case SECURITY_PROTOCOL_IN:
+	case ACCESS_CONTROL_IN:
+	case REPORT_LUNS:
+	case READ_BLOCK_LIMITS:
+	case READ_POSITION:
+	case READ_CAPACITY:
+	case READ_TOC:
+	case READ_FORMAT_CAPACITIES:
+	case REQUEST_SENSE:
+		ret = DMA_FROM_DEVICE;
+		break;
+
+	case WRITE_6:
+	case WRITE_10:
+	case WRITE_12:
+	case WRITE_16:
+	case MODE_SELECT:
+	case MODE_SELECT_10:
+	case WRITE_VERIFY:
+	case WRITE_VERIFY_12:
+	case PERSISTENT_RESERVE_OUT:
+	case MAINTENANCE_OUT:
+	case SECURITY_PROTOCOL_OUT:
+	case ACCESS_CONTROL_OUT:
+		ret = DMA_TO_DEVICE;
+		break;
+	case ALLOW_MEDIUM_REMOVAL:
+	case TEST_UNIT_READY:
+	case SYNCHRONIZE_CACHE:
+	case START_STOP:
+	case ERASE:
+	case REZERO_UNIT:
+	case SEEK_10:
+	case SPACE:
+	case VERIFY:
+	case WRITE_FILEMARKS:
+		ret = DMA_NONE;
+		break;
+	default:
+#define CMD_DIR_MSG "target: Unknown data direction for SCSI Opcode 0x%02x\n"
+		pr_warn(CMD_DIR_MSG, cdb[0]);
+#undef CMD_DIR_MSG
+		ret = -EINVAL;
+	}
+	return ret;
+}
+
+static void usbg_data_write_cmpl(struct usb_ep *ep, struct usb_request *req)
+{
+	struct usbg_cmd *cmd = req->context;
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+
+	if (req->status < 0) {
+		pr_err("%s() state %d transfer failed\n", __func__, cmd->state);
+		goto cleanup;
+	}
+
+	if (req->num_sgs == 0) {
+		sg_copy_from_buffer(se_cmd->t_data_sg,
+				se_cmd->t_data_nents,
+				cmd->data_buf,
+				se_cmd->data_length);
+	}
+
+	complete(&cmd->write_complete);
+	return;
+
+cleanup:
+	usbg_cleanup_cmd(cmd);
+}
+
+static int usbg_prepare_w_request(struct usbg_cmd *cmd, struct usb_request *req)
+{
+	struct se_cmd *se_cmd = &cmd->se_cmd;
+	struct f_uas *fu = cmd->fu;
+	struct usb_gadget *gadget = fuas_to_gadget(fu);
+
+	if (!gadget->sg_supported) {
+		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
+		if (!cmd->data_buf)
+			return -ENOMEM;
+
+		req->buf = cmd->data_buf;
+	} else {
+		req->buf = NULL;
+		req->num_sgs = se_cmd->t_data_nents;
+		req->sg = se_cmd->t_data_sg;
+	}
+
+	req->complete = usbg_data_write_cmpl;
+	req->length = se_cmd->data_length;
+	req->context = cmd;
+	return 0;
+}
+
+static int usbg_send_status_response(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_status_response(cmd);
+	else
+		return uasp_send_status_response(cmd);
+}
+
+static int usbg_send_write_request(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_write_request(cmd);
+	else
+		return uasp_send_write_request(cmd);
+}
+
+static int usbg_send_read_response(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	struct f_uas *fu = cmd->fu;
+
+	if (fu->flags & USBG_IS_BOT)
+		return bot_send_read_response(cmd);
+	else
+		return uasp_send_read_response(cmd);
+}
+
+static void usbg_cmd_work(struct work_struct *work)
+{
+	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	struct usbg_tpg *tpg;
+	int dir;
+
+	se_cmd = &cmd->se_cmd;
+	tpg = cmd->fu->tpg;
+	tv_nexus = tpg->tpg_nexus;
+	dir = get_cmd_dir(cmd->cmd_buf);
+	if (dir < 0) {
+		transport_init_se_cmd(se_cmd,
+				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
+				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
+				cmd->prio_attr, cmd->sense_iu.sense);
+		goto out;
+	}
+
+	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
+			0, cmd->prio_attr, dir, TARGET_SCF_UNKNOWN_SIZE) < 0)
+		goto out;
+
+	return;
+
+out:
+	transport_send_check_condition_and_sense(se_cmd,
+			TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+	usbg_cleanup_cmd(cmd);
+}
+
+static int usbg_submit_command(struct f_uas *fu,
+		void *cmdbuf, unsigned int len)
+{
+	struct command_iu *cmd_iu = cmdbuf;
+	struct usbg_cmd *cmd;
+	struct usbg_tpg *tpg;
+	struct tcm_usbg_nexus *tv_nexus;
+	u32 cmd_len;
+
+	if (cmd_iu->iu_id != IU_ID_COMMAND) {
+		pr_err("Unsupported type %d\n", cmd_iu->iu_id);
+		return -EINVAL;
+	}
+
+	cmd = kzalloc(sizeof(*cmd), GFP_ATOMIC);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->fu = fu;
+
+	/* XXX until I figure out why I can't free in on complete */
+	kref_init(&cmd->ref);
+	kref_get(&cmd->ref);
+
+	tpg = fu->tpg;
+	cmd_len = (cmd_iu->len & ~0x3) + 16;
+	if (cmd_len > USBG_MAX_CMD)
+		goto err;
+
+	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
+
+	cmd->tag = be16_to_cpup(&cmd_iu->tag);
+	cmd->se_cmd.tag = cmd->tag;
+	if (fu->flags & USBG_USE_STREAMS) {
+		if (cmd->tag > UASP_SS_EP_COMP_NUM_STREAMS)
+			goto err;
+		if (!cmd->tag)
+			cmd->stream = &fu->stream[0];
+		else
+			cmd->stream = &fu->stream[cmd->tag - 1];
+	} else {
+		cmd->stream = &fu->stream[0];
+	}
+
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		pr_err("Missing nexus, ignoring command\n");
+		goto err;
+	}
+
+	switch (cmd_iu->prio_attr & 0x7) {
+	case UAS_HEAD_TAG:
+		cmd->prio_attr = TCM_HEAD_TAG;
+		break;
+	case UAS_ORDERED_TAG:
+		cmd->prio_attr = TCM_ORDERED_TAG;
+		break;
+	case UAS_ACA:
+		cmd->prio_attr = TCM_ACA_TAG;
+		break;
+	default:
+		pr_debug_once("Unsupported prio_attr: %02x.\n",
+				cmd_iu->prio_attr);
+	case UAS_SIMPLE_TAG:
+		cmd->prio_attr = TCM_SIMPLE_TAG;
+		break;
+	}
+
+	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
+
+	INIT_WORK(&cmd->work, usbg_cmd_work);
+	queue_work(tpg->workqueue, &cmd->work);
+
+	return 0;
+err:
+	kfree(cmd);
+	return -EINVAL;
+}
+
+static void bot_cmd_work(struct work_struct *work)
+{
+	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
+	struct se_cmd *se_cmd;
+	struct tcm_usbg_nexus *tv_nexus;
+	struct usbg_tpg *tpg;
+	int dir;
+
+	se_cmd = &cmd->se_cmd;
+	tpg = cmd->fu->tpg;
+	tv_nexus = tpg->tpg_nexus;
+	dir = get_cmd_dir(cmd->cmd_buf);
+	if (dir < 0) {
+		transport_init_se_cmd(se_cmd,
+				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
+				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
+				cmd->prio_attr, cmd->sense_iu.sense);
+		goto out;
+	}
+
+	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
+			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
+			cmd->data_len, cmd->prio_attr, dir, 0) < 0)
+		goto out;
+
+	return;
+
+out:
+	transport_send_check_condition_and_sense(se_cmd,
+				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
+	usbg_cleanup_cmd(cmd);
+}
+
+static int bot_submit_command(struct f_uas *fu,
+		void *cmdbuf, unsigned int len)
+{
+	struct bulk_cb_wrap *cbw = cmdbuf;
+	struct usbg_cmd *cmd;
+	struct usbg_tpg *tpg;
+	struct tcm_usbg_nexus *tv_nexus;
+	u32 cmd_len;
+
+	if (cbw->Signature != cpu_to_le32(US_BULK_CB_SIGN)) {
+		pr_err("Wrong signature on CBW\n");
+		return -EINVAL;
+	}
+	if (len != 31) {
+		pr_err("Wrong length for CBW\n");
+		return -EINVAL;
+	}
+
+	cmd_len = cbw->Length;
+	if (cmd_len < 1 || cmd_len > 16)
+		return -EINVAL;
+
+	cmd = kzalloc(sizeof(*cmd), GFP_ATOMIC);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->fu = fu;
+
+	/* XXX until I figure out why I can't free in on complete */
+	kref_init(&cmd->ref);
+	kref_get(&cmd->ref);
+
+	tpg = fu->tpg;
+
+	memcpy(cmd->cmd_buf, cbw->CDB, cmd_len);
+
+	cmd->bot_tag = cbw->Tag;
+
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		pr_err("Missing nexus, ignoring command\n");
+		goto err;
+	}
+
+	cmd->prio_attr = TCM_SIMPLE_TAG;
+	cmd->unpacked_lun = cbw->Lun;
+	cmd->is_read = cbw->Flags & US_BULK_FLAG_IN ? 1 : 0;
+	cmd->data_len = le32_to_cpu(cbw->DataTransferLength);
+	cmd->se_cmd.tag = le32_to_cpu(cmd->bot_tag);
+
+	INIT_WORK(&cmd->work, bot_cmd_work);
+	queue_work(tpg->workqueue, &cmd->work);
+
+	return 0;
+err:
+	kfree(cmd);
+	return -EINVAL;
+}
+
+/* Start fabric.c code */
+
+static int usbg_check_true(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int usbg_check_false(struct se_portal_group *se_tpg)
+{
+	return 0;
+}
+
+static char *usbg_get_fabric_name(void)
+{
+	return "usb_gadget";
+}
+
+static char *usbg_get_fabric_wwn(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	struct usbg_tport *tport = tpg->tport;
+
+	return &tport->tport_name[0];
+}
+
+static u16 usbg_get_tag(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	return tpg->tport_tpgt;
+}
+
+static u32 usbg_tpg_get_inst_index(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static void usbg_cmd_release(struct kref *ref)
+{
+	struct usbg_cmd *cmd = container_of(ref, struct usbg_cmd,
+			ref);
+
+	transport_generic_free_cmd(&cmd->se_cmd, 0);
+}
+
+static void usbg_release_cmd(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+	kfree(cmd->data_buf);
+	kfree(cmd);
+}
+
+static int usbg_shutdown_session(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static void usbg_close_session(struct se_session *se_sess)
+{
+}
+
+static u32 usbg_sess_get_index(struct se_session *se_sess)
+{
+	return 0;
+}
+
+/*
+ * XXX Error recovery: return != 0 if we expect writes. Dunno when that could be
+ */
+static int usbg_write_pending_status(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static void usbg_set_default_node_attrs(struct se_node_acl *nacl)
+{
+}
+
+static int usbg_get_cmd_state(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static void usbg_queue_tm_rsp(struct se_cmd *se_cmd)
+{
+}
+
+static void usbg_aborted_task(struct se_cmd *se_cmd)
+{
+}
+
+static const char *usbg_check_wwn(const char *name)
+{
+	const char *n;
+	unsigned int len;
+
+	n = strstr(name, "naa.");
+	if (!n)
+		return NULL;
+	n += 4;
+	len = strlen(n);
+	if (len == 0 || len > USBG_NAMELEN - 1)
+		return NULL;
+	return n;
+}
+
+static int usbg_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
+{
+	if (!usbg_check_wwn(name))
+		return -EINVAL;
+	return 0;
+}
+
+static struct se_portal_group *usbg_make_tpg(
+	struct se_wwn *wwn,
+	struct config_group *group,
+	const char *name)
+{
+	struct usbg_tport *tport = container_of(wwn, struct usbg_tport,
+			tport_wwn);
+	struct usbg_tpg *tpg;
+	unsigned long tpgt;
+	int ret;
+	struct f_tcm_opts *opts;
+	unsigned i;
+
+	if (strstr(name, "tpgt_") != name)
+		return ERR_PTR(-EINVAL);
+	if (kstrtoul(name + 5, 0, &tpgt) || tpgt > UINT_MAX)
+		return ERR_PTR(-EINVAL);
+	ret = -ENODEV;
+	mutex_lock(&tpg_instances_lock);
+	for (i = 0; i < TPG_INSTANCES; ++i)
+		if (tpg_instances[i].func_inst && !tpg_instances[i].tpg)
+			break;
+	if (i == TPG_INSTANCES)
+		goto unlock_inst;
+
+	opts = container_of(tpg_instances[i].func_inst, struct f_tcm_opts,
+		func_inst);
+	mutex_lock(&opts->dep_lock);
+	if (!opts->ready)
+		goto unlock_dep;
+
+	if (opts->has_dep) {
+		if (!try_module_get(opts->dependent))
+			goto unlock_dep;
+	} else {
+		ret = configfs_depend_item_unlocked(
+			group->cg_subsys,
+			&opts->func_inst.group.cg_item);
+		if (ret)
+			goto unlock_dep;
+	}
+
+	tpg = kzalloc(sizeof(struct usbg_tpg), GFP_KERNEL);
+	ret = -ENOMEM;
+	if (!tpg)
+		goto unref_dep;
+	mutex_init(&tpg->tpg_mutex);
+	atomic_set(&tpg->tpg_port_count, 0);
+	tpg->workqueue = alloc_workqueue("tcm_usb_gadget", 0, 1);
+	if (!tpg->workqueue)
+		goto free_tpg;
+
+	tpg->tport = tport;
+	tpg->tport_tpgt = tpgt;
+
+	/*
+	 * SPC doesn't assign a protocol identifier for USB-SCSI, so we
+	 * pretend to be SAS..
+	 */
+	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_SAS);
+	if (ret < 0)
+		goto free_workqueue;
+
+	tpg_instances[i].tpg = tpg;
+	tpg->fi = tpg_instances[i].func_inst;
+	mutex_unlock(&opts->dep_lock);
+	mutex_unlock(&tpg_instances_lock);
+	return &tpg->se_tpg;
+
+free_workqueue:
+	destroy_workqueue(tpg->workqueue);
+free_tpg:
+	kfree(tpg);
+unref_dep:
+	if (opts->has_dep)
+		module_put(opts->dependent);
+	else
+		configfs_undepend_item_unlocked(&opts->func_inst.group.cg_item);
+unlock_dep:
+	mutex_unlock(&opts->dep_lock);
+unlock_inst:
+	mutex_unlock(&tpg_instances_lock);
+
+	return ERR_PTR(ret);
+}
+
+static int tcm_usbg_drop_nexus(struct usbg_tpg *);
+
+static void usbg_drop_tpg(struct se_portal_group *se_tpg)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg,
+				struct usbg_tpg, se_tpg);
+	unsigned i;
+	struct f_tcm_opts *opts;
+
+	tcm_usbg_drop_nexus(tpg);
+	core_tpg_deregister(se_tpg);
+	destroy_workqueue(tpg->workqueue);
+
+	mutex_lock(&tpg_instances_lock);
+	for (i = 0; i < TPG_INSTANCES; ++i)
+		if (tpg_instances[i].tpg == tpg)
+			break;
+	if (i < TPG_INSTANCES)
+		tpg_instances[i].tpg = NULL;
+	opts = container_of(tpg_instances[i].func_inst,
+		struct f_tcm_opts, func_inst);
+	mutex_lock(&opts->dep_lock);
+	if (opts->has_dep)
+		module_put(opts->dependent);
+	else
+		configfs_undepend_item_unlocked(&opts->func_inst.group.cg_item);
+	mutex_unlock(&opts->dep_lock);
+	mutex_unlock(&tpg_instances_lock);
+
+	kfree(tpg);
+}
+
+static struct se_wwn *usbg_make_tport(
+	struct target_fabric_configfs *tf,
+	struct config_group *group,
+	const char *name)
+{
+	struct usbg_tport *tport;
+	const char *wnn_name;
+	u64 wwpn = 0;
+
+	wnn_name = usbg_check_wwn(name);
+	if (!wnn_name)
+		return ERR_PTR(-EINVAL);
+
+	tport = kzalloc(sizeof(struct usbg_tport), GFP_KERNEL);
+	if (!(tport))
+		return ERR_PTR(-ENOMEM);
+
+	tport->tport_wwpn = wwpn;
+	snprintf(tport->tport_name, sizeof(tport->tport_name), "%s", wnn_name);
+	return &tport->tport_wwn;
+}
+
+static void usbg_drop_tport(struct se_wwn *wwn)
+{
+	struct usbg_tport *tport = container_of(wwn,
+				struct usbg_tport, tport_wwn);
+	kfree(tport);
+}
+
+/*
+ * If somebody feels like dropping the version property, go ahead.
+ */
+static ssize_t usbg_wwn_version_show(struct config_item *item,  char *page)
+{
+	return sprintf(page, "usb-gadget fabric module\n");
+}
+
+CONFIGFS_ATTR_RO(usbg_wwn_, version);
+
+static struct configfs_attribute *usbg_wwn_attrs[] = {
+	&usbg_wwn_attr_version,
+	NULL,
+};
+
+static ssize_t tcm_usbg_tpg_enable_show(struct config_item *item, char *page)
+{
+	struct se_portal_group *se_tpg = to_tpg(item);
+	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	return snprintf(page, PAGE_SIZE, "%u\n", tpg->gadget_connect);
+}
+
+static int usbg_attach(struct usbg_tpg *);
+static void usbg_detach(struct usbg_tpg *);
+
+static ssize_t tcm_usbg_tpg_enable_store(struct config_item *item,
+		const char *page, size_t count)
+{
+	struct se_portal_group *se_tpg = to_tpg(item);
+	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	bool op;
+	ssize_t ret;
+
+	ret = strtobool(page, &op);
+	if (ret)
+		return ret;
+
+	if ((op && tpg->gadget_connect) || (!op && !tpg->gadget_connect))
+		return -EINVAL;
+
+	if (op)
+		ret = usbg_attach(tpg);
+	else
+		usbg_detach(tpg);
+	if (ret)
+		return ret;
+
+	tpg->gadget_connect = op;
+
+	return count;
+}
+
+static ssize_t tcm_usbg_tpg_nexus_show(struct config_item *item, char *page)
+{
+	struct se_portal_group *se_tpg = to_tpg(item);
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	struct tcm_usbg_nexus *tv_nexus;
+	ssize_t ret;
+
+	mutex_lock(&tpg->tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		ret = -ENODEV;
+		goto out;
+	}
+	ret = snprintf(page, PAGE_SIZE, "%s\n",
+			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+out:
+	mutex_unlock(&tpg->tpg_mutex);
+	return ret;
+}
+
+static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
+{
+	struct se_portal_group *se_tpg;
+	struct tcm_usbg_nexus *tv_nexus;
+	int ret;
+
+	mutex_lock(&tpg->tpg_mutex);
+	if (tpg->tpg_nexus) {
+		ret = -EEXIST;
+		pr_debug("tpg->tpg_nexus already exists\n");
+		goto err_unlock;
+	}
+	se_tpg = &tpg->se_tpg;
+
+	ret = -ENOMEM;
+	tv_nexus = kzalloc(sizeof(*tv_nexus), GFP_KERNEL);
+	if (!tv_nexus)
+		goto err_unlock;
+	tv_nexus->tvn_se_sess = transport_init_session(TARGET_PROT_NORMAL);
+	if (IS_ERR(tv_nexus->tvn_se_sess))
+		goto err_free;
+
+	/*
+	 * Since we are running in 'demo mode' this call with generate a
+	 * struct se_node_acl for the tcm_vhost struct se_portal_group with
+	 * the SCSI Initiator port name of the passed configfs group 'name'.
+	 */
+	tv_nexus->tvn_se_sess->se_node_acl = core_tpg_check_initiator_node_acl(
+			se_tpg, name);
+	if (!tv_nexus->tvn_se_sess->se_node_acl) {
+#define MAKE_NEXUS_MSG "core_tpg_check_initiator_node_acl() failed for %s\n"
+		pr_debug(MAKE_NEXUS_MSG, name);
+#undef MAKE_NEXUS_MSG
+		goto err_session;
+	}
+	/*
+	 * Now register the TCM vHost virtual I_T Nexus as active.
+	 */
+	transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+			tv_nexus->tvn_se_sess, tv_nexus);
+	tpg->tpg_nexus = tv_nexus;
+	mutex_unlock(&tpg->tpg_mutex);
+	return 0;
+
+err_session:
+	transport_free_session(tv_nexus->tvn_se_sess);
+err_free:
+	kfree(tv_nexus);
+err_unlock:
+	mutex_unlock(&tpg->tpg_mutex);
+	return ret;
+}
+
+static int tcm_usbg_drop_nexus(struct usbg_tpg *tpg)
+{
+	struct se_session *se_sess;
+	struct tcm_usbg_nexus *tv_nexus;
+	int ret = -ENODEV;
+
+	mutex_lock(&tpg->tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus)
+		goto out;
+
+	se_sess = tv_nexus->tvn_se_sess;
+	if (!se_sess)
+		goto out;
+
+	if (atomic_read(&tpg->tpg_port_count)) {
+		ret = -EPERM;
+#define MSG "Unable to remove Host I_T Nexus with active TPG port count: %d\n"
+		pr_err(MSG, atomic_read(&tpg->tpg_port_count));
+#undef MSG
+		goto out;
+	}
+
+	pr_debug("Removing I_T Nexus to Initiator Port: %s\n",
+			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+	/*
+	 * Release the SCSI I_T Nexus to the emulated vHost Target Port
+	 */
+	transport_deregister_session(tv_nexus->tvn_se_sess);
+	tpg->tpg_nexus = NULL;
+
+	kfree(tv_nexus);
+	ret = 0;
+out:
+	mutex_unlock(&tpg->tpg_mutex);
+	return ret;
+}
+
+static ssize_t tcm_usbg_tpg_nexus_store(struct config_item *item,
+		const char *page, size_t count)
+{
+	struct se_portal_group *se_tpg = to_tpg(item);
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+	unsigned char i_port[USBG_NAMELEN], *ptr;
+	int ret;
+
+	if (!strncmp(page, "NULL", 4)) {
+		ret = tcm_usbg_drop_nexus(tpg);
+		return (!ret) ? count : ret;
+	}
+	if (strlen(page) >= USBG_NAMELEN) {
+
+#define NEXUS_STORE_MSG "Emulated NAA Sas Address: %s, exceeds max: %d\n"
+		pr_err(NEXUS_STORE_MSG, page, USBG_NAMELEN);
+#undef NEXUS_STORE_MSG
+		return -EINVAL;
+	}
+	snprintf(i_port, USBG_NAMELEN, "%s", page);
+
+	ptr = strstr(i_port, "naa.");
+	if (!ptr) {
+		pr_err("Missing 'naa.' prefix\n");
+		return -EINVAL;
+	}
+
+	if (i_port[strlen(i_port) - 1] == '\n')
+		i_port[strlen(i_port) - 1] = '\0';
+
+	ret = tcm_usbg_make_nexus(tpg, &i_port[0]);
+	if (ret < 0)
+		return ret;
+	return count;
+}
+
+CONFIGFS_ATTR(tcm_usbg_tpg_, enable);
+CONFIGFS_ATTR(tcm_usbg_tpg_, nexus);
+
+static struct configfs_attribute *usbg_base_attrs[] = {
+	&tcm_usbg_tpg_attr_enable,
+	&tcm_usbg_tpg_attr_nexus,
+	NULL,
+};
+
+static int usbg_port_link(struct se_portal_group *se_tpg, struct se_lun *lun)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	atomic_inc(&tpg->tpg_port_count);
+	smp_mb__after_atomic();
+	return 0;
+}
+
+static void usbg_port_unlink(struct se_portal_group *se_tpg,
+		struct se_lun *se_lun)
+{
+	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
+
+	atomic_dec(&tpg->tpg_port_count);
+	smp_mb__after_atomic();
+}
+
+static int usbg_check_stop_free(struct se_cmd *se_cmd)
+{
+	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
+			se_cmd);
+
+	kref_put(&cmd->ref, usbg_cmd_release);
+	return 1;
+}
+
+static const struct target_core_fabric_ops usbg_ops = {
+	.module				= THIS_MODULE,
+	.name				= "usb_gadget",
+	.get_fabric_name		= usbg_get_fabric_name,
+	.tpg_get_wwn			= usbg_get_fabric_wwn,
+	.tpg_get_tag			= usbg_get_tag,
+	.tpg_check_demo_mode		= usbg_check_true,
+	.tpg_check_demo_mode_cache	= usbg_check_false,
+	.tpg_check_demo_mode_write_protect = usbg_check_false,
+	.tpg_check_prod_mode_write_protect = usbg_check_false,
+	.tpg_get_inst_index		= usbg_tpg_get_inst_index,
+	.release_cmd			= usbg_release_cmd,
+	.shutdown_session		= usbg_shutdown_session,
+	.close_session			= usbg_close_session,
+	.sess_get_index			= usbg_sess_get_index,
+	.sess_get_initiator_sid		= NULL,
+	.write_pending			= usbg_send_write_request,
+	.write_pending_status		= usbg_write_pending_status,
+	.set_default_node_attributes	= usbg_set_default_node_attrs,
+	.get_cmd_state			= usbg_get_cmd_state,
+	.queue_data_in			= usbg_send_read_response,
+	.queue_status			= usbg_send_status_response,
+	.queue_tm_rsp			= usbg_queue_tm_rsp,
+	.aborted_task			= usbg_aborted_task,
+	.check_stop_free		= usbg_check_stop_free,
+
+	.fabric_make_wwn		= usbg_make_tport,
+	.fabric_drop_wwn		= usbg_drop_tport,
+	.fabric_make_tpg		= usbg_make_tpg,
+	.fabric_drop_tpg		= usbg_drop_tpg,
+	.fabric_post_link		= usbg_port_link,
+	.fabric_pre_unlink		= usbg_port_unlink,
+	.fabric_init_nodeacl		= usbg_init_nodeacl,
+
+	.tfc_wwn_attrs			= usbg_wwn_attrs,
+	.tfc_tpg_base_attrs		= usbg_base_attrs,
+};
+
+/* Start gadget.c code */
+
+static struct usb_interface_descriptor bot_intf_desc = {
+	.bLength =              sizeof(bot_intf_desc),
+	.bDescriptorType =      USB_DT_INTERFACE,
+	.bNumEndpoints =        2,
+	.bAlternateSetting =	USB_G_ALT_INT_BBB,
+	.bInterfaceClass =      USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass =   USB_SC_SCSI,
+	.bInterfaceProtocol =   USB_PR_BULK,
+};
+
+static struct usb_interface_descriptor uasp_intf_desc = {
+	.bLength =		sizeof(uasp_intf_desc),
+	.bDescriptorType =	USB_DT_INTERFACE,
+	.bNumEndpoints =	4,
+	.bAlternateSetting =	USB_G_ALT_INT_UAS,
+	.bInterfaceClass =	USB_CLASS_MASS_STORAGE,
+	.bInterfaceSubClass =	USB_SC_SCSI,
+	.bInterfaceProtocol =	USB_PR_UAS,
+};
+
+static struct usb_endpoint_descriptor uasp_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_bi_pipe_desc = {
+	.bLength =		sizeof(uasp_bi_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		DATA_IN_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_bi_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_bi_ep_comp_desc = {
+	.bLength =		sizeof(uasp_bi_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bMaxBurst =		0,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+	.wBytesPerInterval =	0,
+};
+
+static struct usb_ss_ep_comp_descriptor bot_bi_ep_comp_desc = {
+	.bLength =		sizeof(bot_bi_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bMaxBurst =		0,
+};
+
+static struct usb_endpoint_descriptor uasp_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_bo_pipe_desc = {
+	.bLength =		sizeof(uasp_bo_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		DATA_OUT_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_bo_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(0x400),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_bo_ep_comp_desc = {
+	.bLength =		sizeof(uasp_bo_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+};
+
+static struct usb_ss_ep_comp_descriptor bot_bo_ep_comp_desc = {
+	.bLength =		sizeof(bot_bo_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_endpoint_descriptor uasp_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_status_pipe_desc = {
+	.bLength =		sizeof(uasp_status_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		STATUS_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_status_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_status_in_ep_comp_desc = {
+	.bLength =		sizeof(uasp_status_in_ep_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
+};
+
+static struct usb_endpoint_descriptor uasp_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(512),
+};
+
+static struct usb_endpoint_descriptor uasp_fs_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+};
+
+static struct usb_pipe_usage_descriptor uasp_cmd_pipe_desc = {
+	.bLength =		sizeof(uasp_cmd_pipe_desc),
+	.bDescriptorType =	USB_DT_PIPE_USAGE,
+	.bPipeID =		CMD_PIPE_ID,
+};
+
+static struct usb_endpoint_descriptor uasp_ss_cmd_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+};
+
+static struct usb_ss_ep_comp_descriptor uasp_cmd_comp_desc = {
+	.bLength =		sizeof(uasp_cmd_comp_desc),
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+};
+
+static struct usb_descriptor_header *uasp_fs_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_fs_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *uasp_hs_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bo_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+	NULL,
+};
+
+static struct usb_descriptor_header *uasp_ss_function_desc[] = {
+	(struct usb_descriptor_header *) &bot_intf_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
+	(struct usb_descriptor_header *) &bot_bi_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
+	(struct usb_descriptor_header *) &bot_bo_ep_comp_desc,
+
+	(struct usb_descriptor_header *) &uasp_intf_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
+	(struct usb_descriptor_header *) &uasp_bi_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
+	(struct usb_descriptor_header *) &uasp_bo_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_status_desc,
+	(struct usb_descriptor_header *) &uasp_status_in_ep_comp_desc,
+	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
+	(struct usb_descriptor_header *) &uasp_ss_cmd_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_comp_desc,
+	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
+	NULL,
+};
+
+static struct usb_string	tcm_us_strings[] = {
+	[USB_G_STR_INT_UAS].s		= "USB Attached SCSI",
+	[USB_G_STR_INT_BBB].s		= "Bulk Only Transport",
+	{ },
+};
+
+static struct usb_gadget_strings tcm_stringtab = {
+	.language = 0x0409,
+	.strings = tcm_us_strings,
+};
+
+static struct usb_gadget_strings *tcm_strings[] = {
+	&tcm_stringtab,
+	NULL,
+};
+
+static int tcm_bind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct f_uas		*fu = to_f_uas(f);
+	struct usb_string	*us;
+	struct usb_gadget	*gadget = c->cdev->gadget;
+	struct usb_ep		*ep;
+	struct f_tcm_opts	*opts;
+	int			iface;
+	int			ret;
+
+	opts = container_of(f->fi, struct f_tcm_opts, func_inst);
+
+	mutex_lock(&opts->dep_lock);
+	if (!opts->can_attach) {
+		mutex_unlock(&opts->dep_lock);
+		return -ENODEV;
+	}
+	mutex_unlock(&opts->dep_lock);
+	us = usb_gstrings_attach(c->cdev, tcm_strings,
+		ARRAY_SIZE(tcm_us_strings));
+	if (IS_ERR(us))
+		return PTR_ERR(us);
+	bot_intf_desc.iInterface = us[USB_G_STR_INT_BBB].id;
+	uasp_intf_desc.iInterface = us[USB_G_STR_INT_UAS].id;
+
+	iface = usb_interface_id(c, f);
+	if (iface < 0)
+		return iface;
+
+	bot_intf_desc.bInterfaceNumber = iface;
+	uasp_intf_desc.bInterfaceNumber = iface;
+	fu->iface = iface;
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bi_desc,
+			&uasp_bi_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+
+	fu->ep_in = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bo_desc,
+			&uasp_bo_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	fu->ep_out = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_status_desc,
+			&uasp_status_in_ep_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	fu->ep_status = ep;
+
+	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_cmd_desc,
+			&uasp_cmd_comp_desc);
+	if (!ep)
+		goto ep_fail;
+	fu->ep_cmd = ep;
+
+	/* Assume endpoint addresses are the same for both speeds */
+	uasp_bi_desc.bEndpointAddress =	uasp_ss_bi_desc.bEndpointAddress;
+	uasp_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
+	uasp_status_desc.bEndpointAddress =
+		uasp_ss_status_desc.bEndpointAddress;
+	uasp_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
+
+	uasp_fs_bi_desc.bEndpointAddress = uasp_ss_bi_desc.bEndpointAddress;
+	uasp_fs_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
+	uasp_fs_status_desc.bEndpointAddress =
+		uasp_ss_status_desc.bEndpointAddress;
+	uasp_fs_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
+
+	ret = usb_assign_descriptors(f, uasp_fs_function_desc,
+			uasp_hs_function_desc, uasp_ss_function_desc);
+	if (ret)
+		goto ep_fail;
+
+	return 0;
+ep_fail:
+	pr_err("Can't claim all required eps\n");
+
+	return -ENOTSUPP;
+}
+
+struct guas_setup_wq {
+	struct work_struct work;
+	struct f_uas *fu;
+	unsigned int alt;
+};
+
+static void tcm_delayed_set_alt(struct work_struct *wq)
+{
+	struct guas_setup_wq *work = container_of(wq, struct guas_setup_wq,
+			work);
+	struct f_uas *fu = work->fu;
+	int alt = work->alt;
+
+	kfree(work);
+
+	if (fu->flags & USBG_IS_BOT)
+		bot_cleanup_old_alt(fu);
+	if (fu->flags & USBG_IS_UAS)
+		uasp_cleanup_old_alt(fu);
+
+	if (alt == USB_G_ALT_INT_BBB)
+		bot_set_alt(fu);
+	else if (alt == USB_G_ALT_INT_UAS)
+		uasp_set_alt(fu);
+	usb_composite_setup_continue(fu->function.config->cdev);
+}
+
+static int tcm_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if ((alt == USB_G_ALT_INT_BBB) || (alt == USB_G_ALT_INT_UAS)) {
+		struct guas_setup_wq *work;
+
+		work = kmalloc(sizeof(*work), GFP_ATOMIC);
+		if (!work)
+			return -ENOMEM;
+		INIT_WORK(&work->work, tcm_delayed_set_alt);
+		work->fu = fu;
+		work->alt = alt;
+		schedule_work(&work->work);
+		return USB_GADGET_DELAYED_STATUS;
+	}
+	return -EOPNOTSUPP;
+}
+
+static void tcm_disable(struct usb_function *f)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if (fu->flags & USBG_IS_UAS)
+		uasp_cleanup_old_alt(fu);
+	else if (fu->flags & USBG_IS_BOT)
+		bot_cleanup_old_alt(fu);
+	fu->flags = 0;
+}
+
+static int tcm_setup(struct usb_function *f,
+		const struct usb_ctrlrequest *ctrl)
+{
+	struct f_uas *fu = to_f_uas(f);
+
+	if (!(fu->flags & USBG_IS_BOT))
+		return -EOPNOTSUPP;
+
+	return usbg_bot_setup(f, ctrl);
+}
+
+static inline struct f_tcm_opts *to_f_tcm_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_tcm_opts,
+		func_inst.group);
+}
+
+static void tcm_attr_release(struct config_item *item)
+{
+	struct f_tcm_opts *opts = to_f_tcm_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations tcm_item_ops = {
+	.release		= tcm_attr_release,
+};
+
+static struct config_item_type tcm_func_type = {
+	.ct_item_ops	= &tcm_item_ops,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void tcm_free_inst(struct usb_function_instance *f)
+{
+	struct f_tcm_opts *opts;
+	unsigned i;
+
+	opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	mutex_lock(&tpg_instances_lock);
+	for (i = 0; i < TPG_INSTANCES; ++i)
+		if (tpg_instances[i].func_inst == f)
+			break;
+	if (i < TPG_INSTANCES)
+		tpg_instances[i].func_inst = NULL;
+	mutex_unlock(&tpg_instances_lock);
+
+	kfree(opts);
+}
+
+static int tcm_register_callback(struct usb_function_instance *f)
+{
+	struct f_tcm_opts *opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	mutex_lock(&opts->dep_lock);
+	opts->can_attach = true;
+	mutex_unlock(&opts->dep_lock);
+
+	return 0;
+}
+
+static void tcm_unregister_callback(struct usb_function_instance *f)
+{
+	struct f_tcm_opts *opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	mutex_lock(&opts->dep_lock);
+	unregister_gadget_item(opts->
+		func_inst.group.cg_item.ci_parent->ci_parent);
+	opts->can_attach = false;
+	mutex_unlock(&opts->dep_lock);
+}
+
+static int usbg_attach(struct usbg_tpg *tpg)
+{
+	struct usb_function_instance *f = tpg->fi;
+	struct f_tcm_opts *opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	if (opts->tcm_register_callback)
+		return opts->tcm_register_callback(f);
+
+	return 0;
+}
+
+static void usbg_detach(struct usbg_tpg *tpg)
+{
+	struct usb_function_instance *f = tpg->fi;
+	struct f_tcm_opts *opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	if (opts->tcm_unregister_callback)
+		opts->tcm_unregister_callback(f);
+}
+
+static int tcm_set_name(struct usb_function_instance *f, const char *name)
+{
+	struct f_tcm_opts *opts = container_of(f, struct f_tcm_opts, func_inst);
+
+	pr_debug("tcm: Activating %s\n", name);
+
+	mutex_lock(&opts->dep_lock);
+	opts->ready = true;
+	mutex_unlock(&opts->dep_lock);
+
+	return 0;
+}
+
+static struct usb_function_instance *tcm_alloc_inst(void)
+{
+	struct f_tcm_opts *opts;
+	int i;
+
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&tpg_instances_lock);
+	for (i = 0; i < TPG_INSTANCES; ++i)
+		if (!tpg_instances[i].func_inst)
+			break;
+
+	if (i == TPG_INSTANCES) {
+		mutex_unlock(&tpg_instances_lock);
+		kfree(opts);
+		return ERR_PTR(-EBUSY);
+	}
+	tpg_instances[i].func_inst = &opts->func_inst;
+	mutex_unlock(&tpg_instances_lock);
+
+	mutex_init(&opts->dep_lock);
+	opts->func_inst.set_inst_name = tcm_set_name;
+	opts->func_inst.free_func_inst = tcm_free_inst;
+	opts->tcm_register_callback = tcm_register_callback;
+	opts->tcm_unregister_callback = tcm_unregister_callback;
+
+	config_group_init_type_name(&opts->func_inst.group, "",
+			&tcm_func_type);
+
+	return &opts->func_inst;
+}
+
+static void tcm_free(struct usb_function *f)
+{
+	struct f_uas *tcm = to_f_uas(f);
+
+	kfree(tcm);
+}
+
+static void tcm_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *tcm_alloc(struct usb_function_instance *fi)
+{
+	struct f_uas *fu;
+	unsigned i;
+
+	mutex_lock(&tpg_instances_lock);
+	for (i = 0; i < TPG_INSTANCES; ++i)
+		if (tpg_instances[i].func_inst == fi)
+			break;
+	if (i == TPG_INSTANCES) {
+		mutex_unlock(&tpg_instances_lock);
+		return ERR_PTR(-ENODEV);
+	}
+
+	fu = kzalloc(sizeof(*fu), GFP_KERNEL);
+	if (!fu) {
+		mutex_unlock(&tpg_instances_lock);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	fu->function.name = "Target Function";
+	fu->function.bind = tcm_bind;
+	fu->function.unbind = tcm_unbind;
+	fu->function.set_alt = tcm_set_alt;
+	fu->function.setup = tcm_setup;
+	fu->function.disable = tcm_disable;
+	fu->function.free_func = tcm_free;
+	fu->tpg = tpg_instances[i].tpg;
+	mutex_unlock(&tpg_instances_lock);
+
+	return &fu->function;
+}
+
+DECLARE_USB_FUNCTION(tcm, tcm_alloc_inst, tcm_alloc);
+
+static int tcm_init(void)
+{
+	int ret;
+
+	ret = usb_function_register(&tcmusb_func);
+	if (ret)
+		return ret;
+
+	ret = target_register_template(&usbg_ops);
+	if (ret)
+		usb_function_unregister(&tcmusb_func);
+
+	return ret;
+}
+module_init(tcm_init);
+
+static void tcm_exit(void)
+{
+	target_unregister_template(&usbg_ops);
+	usb_function_unregister(&tcmusb_func);
+}
+module_exit(tcm_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Sebastian Andrzej Siewior");
diff --git a/drivers/usb/gadget/function/g_zero.h b/drivers/usb/gadget/function/g_zero.h
index 15f180904f8a..492924d0d599 100644
--- a/drivers/usb/gadget/function/g_zero.h
+++ b/drivers/usb/gadget/function/g_zero.h
@@ -10,6 +10,8 @@
 #define GZERO_QLEN		32
 #define GZERO_ISOC_INTERVAL	4
 #define GZERO_ISOC_MAXPACKET	1024
+#define GZERO_SS_BULK_QLEN	1
+#define GZERO_SS_ISO_QLEN	8
 
 struct usb_zero_options {
 	unsigned pattern;
@@ -19,6 +21,8 @@ struct usb_zero_options {
 	unsigned isoc_maxburst;
 	unsigned bulk_buflen;
 	unsigned qlen;
+	unsigned ss_bulk_qlen;
+	unsigned ss_iso_qlen;
 };
 
 struct f_ss_opts {
@@ -29,6 +33,8 @@ struct f_ss_opts {
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
 	unsigned bulk_buflen;
+	unsigned bulk_qlen;
+	unsigned iso_qlen;
 
 	/*
 	 * Read/write access to configfs attributes is handled by configfs.
@@ -59,7 +65,6 @@ void lb_modexit(void);
 int lb_modinit(void);
 
 /* common utilities */
-void free_ep_req(struct usb_ep *ep, struct usb_request *req);
 void disable_endpoints(struct usb_composite_dev *cdev,
 		struct usb_ep *in, struct usb_ep *out,
 		struct usb_ep *iso_in, struct usb_ep *iso_out);
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.h b/drivers/usb/gadget/function/tcm.h
index 0b749e1aa2f1..b75c6f3e1980 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.h
+++ b/drivers/usb/gadget/function/tcm.h
@@ -16,8 +16,7 @@
 #define UASP_SS_EP_COMP_NUM_STREAMS (1 << UASP_SS_EP_COMP_LOG_STREAMS)
 
 enum {
-	USB_G_STR_CONFIG = USB_GADGET_FIRST_AVAIL_IDX,
-	USB_G_STR_INT_UAS,
+	USB_G_STR_INT_UAS = 0,
 	USB_G_STR_INT_BBB,
 };
 
@@ -40,6 +39,8 @@ struct usbg_tpg {
 	u32 gadget_connect;
 	struct tcm_usbg_nexus *tpg_nexus;
 	atomic_t tpg_port_count;
+
+	struct usb_function_instance *fi;
 };
 
 struct usbg_tport {
@@ -128,6 +129,4 @@ struct f_uas {
 	struct usb_request	*bot_req_out;
 };
 
-extern struct usbg_tpg *the_only_tpg_I_currently_have;
-
-#endif
+#endif /* __TARGET_USB_GADGET_H__ */
diff --git a/drivers/usb/gadget/function/u_ether.c b/drivers/usb/gadget/function/u_ether.c
index 6554322af2c1..637809e3bd0d 100644
--- a/drivers/usb/gadget/function/u_ether.c
+++ b/drivers/usb/gadget/function/u_ether.c
@@ -143,21 +143,11 @@ static inline int qlen(struct usb_gadget *gadget, unsigned qmult)
 
 static int ueth_change_mtu(struct net_device *net, int new_mtu)
 {
-	struct eth_dev	*dev = netdev_priv(net);
-	unsigned long	flags;
-	int		status = 0;
+	if (new_mtu <= ETH_HLEN || new_mtu > GETHER_MAX_ETH_FRAME_LEN)
+		return -ERANGE;
+	net->mtu = new_mtu;
 
-	/* don't change MTU on "live" link (peer won't know) */
-	spin_lock_irqsave(&dev->lock, flags);
-	if (dev->port_usb)
-		status = -EBUSY;
-	else if (new_mtu <= ETH_HLEN || new_mtu > GETHER_MAX_ETH_FRAME_LEN)
-		status = -ERANGE;
-	else
-		net->mtu = new_mtu;
-	spin_unlock_irqrestore(&dev->lock, flags);
-
-	return status;
+	return 0;
 }
 
 static void eth_get_drvinfo(struct net_device *net, struct ethtool_drvinfo *p)
diff --git a/drivers/usb/gadget/function/u_serial.c b/drivers/usb/gadget/function/u_serial.c
index f7771d86ad6c..6af145f2a99d 100644
--- a/drivers/usb/gadget/function/u_serial.c
+++ b/drivers/usb/gadget/function/u_serial.c
@@ -27,6 +27,8 @@
 #include <linux/slab.h>
 #include <linux/export.h>
 #include <linux/module.h>
+#include <linux/console.h>
+#include <linux/kthread.h>
 
 #include "u_serial.h"
 
@@ -79,6 +81,7 @@
  */
 #define QUEUE_SIZE		16
 #define WRITE_BUF_SIZE		8192		/* TX only */
+#define GS_CONSOLE_BUF_SIZE	8192
 
 /* circular buffer */
 struct gs_buf {
@@ -88,6 +91,17 @@ struct gs_buf {
 	char			*buf_put;
 };
 
+/* console info */
+struct gscons_info {
+	struct gs_port		*port;
+	struct task_struct	*console_thread;
+	struct gs_buf		con_buf;
+	/* protect the buf and busy flag */
+	spinlock_t		con_lock;
+	int			req_busy;
+	struct usb_request	*console_req;
+};
+
 /*
  * The port structure holds info for each port, one for each minor number
  * (and thus for each /dev/ node).
@@ -1023,6 +1037,246 @@ static const struct tty_operations gs_tty_ops = {
 
 static struct tty_driver *gs_tty_driver;
 
+#ifdef CONFIG_U_SERIAL_CONSOLE
+
+static struct gscons_info gscons_info;
+static struct console gserial_cons;
+
+static struct usb_request *gs_request_new(struct usb_ep *ep)
+{
+	struct usb_request *req = usb_ep_alloc_request(ep, GFP_ATOMIC);
+	if (!req)
+		return NULL;
+
+	req->buf = kmalloc(ep->maxpacket, GFP_ATOMIC);
+	if (!req->buf) {
+		usb_ep_free_request(ep, req);
+		return NULL;
+	}
+
+	return req;
+}
+
+static void gs_request_free(struct usb_request *req, struct usb_ep *ep)
+{
+	if (!req)
+		return;
+
+	kfree(req->buf);
+	usb_ep_free_request(ep, req);
+}
+
+static void gs_complete_out(struct usb_ep *ep, struct usb_request *req)
+{
+	struct gscons_info *info = &gscons_info;
+
+	switch (req->status) {
+	default:
+		pr_warn("%s: unexpected %s status %d\n",
+			__func__, ep->name, req->status);
+	case 0:
+		/* normal completion */
+		spin_lock(&info->con_lock);
+		info->req_busy = 0;
+		spin_unlock(&info->con_lock);
+
+		wake_up_process(info->console_thread);
+		break;
+	case -ESHUTDOWN:
+		/* disconnect */
+		pr_vdebug("%s: %s shutdown\n", __func__, ep->name);
+		break;
+	}
+}
+
+static int gs_console_connect(int port_num)
+{
+	struct gscons_info *info = &gscons_info;
+	struct gs_port *port;
+	struct usb_ep *ep;
+
+	if (port_num != gserial_cons.index) {
+		pr_err("%s: port num [%d] is not support console\n",
+		       __func__, port_num);
+		return -ENXIO;
+	}
+
+	port = ports[port_num].port;
+	ep = port->port_usb->in;
+	if (!info->console_req) {
+		info->console_req = gs_request_new(ep);
+		if (!info->console_req)
+			return -ENOMEM;
+		info->console_req->complete = gs_complete_out;
+	}
+
+	info->port = port;
+	spin_lock(&info->con_lock);
+	info->req_busy = 0;
+	spin_unlock(&info->con_lock);
+	pr_vdebug("port[%d] console connect!\n", port_num);
+	return 0;
+}
+
+static void gs_console_disconnect(struct usb_ep *ep)
+{
+	struct gscons_info *info = &gscons_info;
+	struct usb_request *req = info->console_req;
+
+	gs_request_free(req, ep);
+	info->console_req = NULL;
+}
+
+static int gs_console_thread(void *data)
+{
+	struct gscons_info *info = &gscons_info;
+	struct gs_port *port;
+	struct usb_request *req;
+	struct usb_ep *ep;
+	int xfer, ret, count, size;
+
+	do {
+		port = info->port;
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (!port || !port->port_usb
+		    || !port->port_usb->in || !info->console_req)
+			goto sched;
+
+		req = info->console_req;
+		ep = port->port_usb->in;
+
+		spin_lock_irq(&info->con_lock);
+		count = gs_buf_data_avail(&info->con_buf);
+		size = ep->maxpacket;
+
+		if (count > 0 && !info->req_busy) {
+			set_current_state(TASK_RUNNING);
+			if (count < size)
+				size = count;
+
+			xfer = gs_buf_get(&info->con_buf, req->buf, size);
+			req->length = xfer;
+
+			spin_unlock(&info->con_lock);
+			ret = usb_ep_queue(ep, req, GFP_ATOMIC);
+			spin_lock(&info->con_lock);
+			if (ret < 0)
+				info->req_busy = 0;
+			else
+				info->req_busy = 1;
+
+			spin_unlock_irq(&info->con_lock);
+		} else {
+			spin_unlock_irq(&info->con_lock);
+sched:
+			if (kthread_should_stop()) {
+				set_current_state(TASK_RUNNING);
+				break;
+			}
+			schedule();
+		}
+	} while (1);
+
+	return 0;
+}
+
+static int gs_console_setup(struct console *co, char *options)
+{
+	struct gscons_info *info = &gscons_info;
+	int status;
+
+	info->port = NULL;
+	info->console_req = NULL;
+	info->req_busy = 0;
+	spin_lock_init(&info->con_lock);
+
+	status = gs_buf_alloc(&info->con_buf, GS_CONSOLE_BUF_SIZE);
+	if (status) {
+		pr_err("%s: allocate console buffer failed\n", __func__);
+		return status;
+	}
+
+	info->console_thread = kthread_create(gs_console_thread,
+					      co, "gs_console");
+	if (IS_ERR(info->console_thread)) {
+		pr_err("%s: cannot create console thread\n", __func__);
+		gs_buf_free(&info->con_buf);
+		return PTR_ERR(info->console_thread);
+	}
+	wake_up_process(info->console_thread);
+
+	return 0;
+}
+
+static void gs_console_write(struct console *co,
+			     const char *buf, unsigned count)
+{
+	struct gscons_info *info = &gscons_info;
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->con_lock, flags);
+	gs_buf_put(&info->con_buf, buf, count);
+	spin_unlock_irqrestore(&info->con_lock, flags);
+
+	wake_up_process(info->console_thread);
+}
+
+static struct tty_driver *gs_console_device(struct console *co, int *index)
+{
+	struct tty_driver **p = (struct tty_driver **)co->data;
+
+	if (!*p)
+		return NULL;
+
+	*index = co->index;
+	return *p;
+}
+
+static struct console gserial_cons = {
+	.name =		"ttyGS",
+	.write =	gs_console_write,
+	.device =	gs_console_device,
+	.setup =	gs_console_setup,
+	.flags =	CON_PRINTBUFFER,
+	.index =	-1,
+	.data =		&gs_tty_driver,
+};
+
+static void gserial_console_init(void)
+{
+	register_console(&gserial_cons);
+}
+
+static void gserial_console_exit(void)
+{
+	struct gscons_info *info = &gscons_info;
+
+	unregister_console(&gserial_cons);
+	kthread_stop(info->console_thread);
+	gs_buf_free(&info->con_buf);
+}
+
+#else
+
+static int gs_console_connect(int port_num)
+{
+	return 0;
+}
+
+static void gs_console_disconnect(struct usb_ep *ep)
+{
+}
+
+static void gserial_console_init(void)
+{
+}
+
+static void gserial_console_exit(void)
+{
+}
+
+#endif
+
 static int
 gs_port_alloc(unsigned port_num, struct usb_cdc_line_coding *coding)
 {
@@ -1096,6 +1350,7 @@ void gserial_free_line(unsigned char port_num)
 
 	gserial_free_port(port);
 	tty_unregister_device(gs_tty_driver, port_num);
+	gserial_console_exit();
 }
 EXPORT_SYMBOL_GPL(gserial_free_line);
 
@@ -1138,6 +1393,7 @@ int gserial_alloc_line(unsigned char *line_num)
 		goto err;
 	}
 	*line_num = port_num;
+	gserial_console_init();
 err:
 	return ret;
 }
@@ -1219,6 +1475,7 @@ int gserial_connect(struct gserial *gser, u8 port_num)
 			gser->disconnect(gser);
 	}
 
+	status = gs_console_connect(port_num);
 	spin_unlock_irqrestore(&port->port_lock, flags);
 
 	return status;
@@ -1277,6 +1534,7 @@ void gserial_disconnect(struct gserial *gser)
 	port->read_allocated = port->read_started =
 		port->write_allocated = port->write_started = 0;
 
+	gs_console_disconnect(gser->in);
 	spin_unlock_irqrestore(&port->port_lock, flags);
 }
 EXPORT_SYMBOL_GPL(gserial_disconnect);
diff --git a/drivers/usb/gadget/function/u_tcm.h b/drivers/usb/gadget/function/u_tcm.h
new file mode 100644
index 000000000000..0bd751e0483f
--- /dev/null
+++ b/drivers/usb/gadget/function/u_tcm.h
@@ -0,0 +1,50 @@
+/*
+ * u_tcm.h
+ *
+ * Utility definitions for the tcm function
+ *
+ * Copyright (c) 2015 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@xxxxxxxxxxx>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_TCM_H
+#define U_TCM_H
+
+#include <linux/usb/composite.h>
+
+/**
+ * @dependent: optional dependent module. Meant for legacy gadget.
+ * If non-null its refcount will be increased when a tpg is created and
+ * decreased when tpg is dropped.
+ * @dep_lock: lock for dependent module operations.
+ * @ready: true if the dependent module information is set.
+ * @can_attach: true a function can be bound to gadget
+ * @has_dep: true if there is a dependent module
+ *
+ */
+struct f_tcm_opts {
+	struct usb_function_instance	func_inst;
+	struct module			*dependent;
+	struct mutex			dep_lock;
+	bool				ready;
+	bool				can_attach;
+	bool				has_dep;
+
+	/*
+	 * Callbacks to be removed when legacy tcm gadget disappears.
+	 *
+	 * If you use the new function registration interface
+	 * programmatically, you MUST set these callbacks to
+	 * something sensible (e.g. probe/remove the composite).
+	 */
+	int (*tcm_register_callback)(struct usb_function_instance *);
+	void (*tcm_unregister_callback)(struct usb_function_instance *);
+};
+
+#endif /* U_TCM_H */
diff --git a/drivers/usb/gadget/function/uvc_queue.c b/drivers/usb/gadget/function/uvc_queue.c
index 51d4a1703af2..912694f3d54e 100644
--- a/drivers/usb/gadget/function/uvc_queue.c
+++ b/drivers/usb/gadget/function/uvc_queue.c
@@ -41,7 +41,7 @@
  * videobuf2 queue operations
  */
 
-static int uvc_queue_setup(struct vb2_queue *vq, const void *parg,
+static int uvc_queue_setup(struct vb2_queue *vq,
 			   unsigned int *nbuffers, unsigned int *nplanes,
 			   unsigned int sizes[], void *alloc_ctxs[])
 {
@@ -329,7 +329,7 @@ struct uvc_buffer *uvcg_queue_next_buffer(struct uvc_video_queue *queue,
 
 	buf->buf.field = V4L2_FIELD_NONE;
 	buf->buf.sequence = queue->sequence++;
-	v4l2_get_timestamp(&buf->buf.timestamp);
+	buf->buf.vb2_buf.timestamp = ktime_get_ns();
 
 	vb2_set_plane_payload(&buf->buf.vb2_buf, 0, buf->bytesused);
 	vb2_buffer_done(&buf->buf.vb2_buf, VB2_BUF_STATE_DONE);
diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index 4d682ad7bf23..a23d1b90454c 100644
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -250,6 +250,7 @@ config USB_GADGET_TARGET
 	tristate "USB Gadget Target Fabric Module"
 	depends on TARGET_CORE
 	select USB_LIBCOMPOSITE
+	select USB_F_TCM
 	help
 	  This fabric is an USB gadget. Two USB protocols are supported that is
 	  BBB or BOT (Bulk Only Transport) and UAS (USB Attached SCSI). BOT is
diff --git a/drivers/usb/gadget/legacy/acm_ms.c b/drivers/usb/gadget/legacy/acm_ms.c
index 4b158e2d1e57..c16089efc322 100644
--- a/drivers/usb/gadget/legacy/acm_ms.c
+++ b/drivers/usb/gadget/legacy/acm_ms.c
@@ -40,7 +40,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	.bDeviceClass =		USB_CLASS_MISC /* 0xEF */,
 	.bDeviceSubClass =	2,
diff --git a/drivers/usb/gadget/legacy/audio.c b/drivers/usb/gadget/legacy/audio.c
index 685cf3b4b78f..5d7b3c6a422b 100644
--- a/drivers/usb/gadget/legacy/audio.c
+++ b/drivers/usb/gadget/legacy/audio.c
@@ -123,7 +123,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x200),
+	/* .bcdUSB = DYNAMIC */
 
 #ifdef CONFIG_GADGET_UAC1
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
diff --git a/drivers/usb/gadget/legacy/cdc2.c b/drivers/usb/gadget/legacy/cdc2.c
index ecd8c8d62f2e..51c08682de84 100644
--- a/drivers/usb/gadget/legacy/cdc2.c
+++ b/drivers/usb/gadget/legacy/cdc2.c
@@ -43,7 +43,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	.bDeviceClass =		USB_CLASS_COMM,
 	.bDeviceSubClass =	0,
diff --git a/drivers/usb/gadget/legacy/ether.c b/drivers/usb/gadget/legacy/ether.c
index 31e9160223e9..25a2c2e48592 100644
--- a/drivers/usb/gadget/legacy/ether.c
+++ b/drivers/usb/gadget/legacy/ether.c
@@ -151,7 +151,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16 (0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	.bDeviceClass =		USB_CLASS_COMM,
 	.bDeviceSubClass =	0,
diff --git a/drivers/usb/gadget/legacy/g_ffs.c b/drivers/usb/gadget/legacy/g_ffs.c
index 320a81b2baa6..f85639ef8a8f 100644
--- a/drivers/usb/gadget/legacy/g_ffs.c
+++ b/drivers/usb/gadget/legacy/g_ffs.c
@@ -69,7 +69,7 @@ static struct usb_device_descriptor gfs_dev_desc = {
 	.bLength		= sizeof gfs_dev_desc,
 	.bDescriptorType	= USB_DT_DEVICE,
 
-	.bcdUSB			= cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass		= USB_CLASS_PER_INTERFACE,
 
 	.idVendor		= cpu_to_le16(GFS_VENDOR_ID),
diff --git a/drivers/usb/gadget/legacy/gmidi.c b/drivers/usb/gadget/legacy/gmidi.c
index 8a18348ae86e..fc2ac150f5ff 100644
--- a/drivers/usb/gadget/legacy/gmidi.c
+++ b/drivers/usb/gadget/legacy/gmidi.c
@@ -21,19 +21,12 @@
 /* #define VERBOSE_DEBUG */
 
 #include <linux/kernel.h>
-#include <linux/slab.h>
 #include <linux/module.h>
-#include <linux/device.h>
 
-#include <sound/core.h>
 #include <sound/initval.h>
-#include <sound/rawmidi.h>
 
-#include <linux/usb/ch9.h>
 #include <linux/usb/composite.h>
 #include <linux/usb/gadget.h>
-#include <linux/usb/audio.h>
-#include <linux/usb/midi.h>
 
 #include "u_midi.h"
 
@@ -42,7 +35,6 @@
 MODULE_AUTHOR("Ben Williamson");
 MODULE_LICENSE("GPL v2");
 
-static const char shortname[] = "g_midi";
 static const char longname[] = "MIDI Gadget";
 
 USB_GADGET_COMPOSITE_OPTIONS();
@@ -61,7 +53,7 @@ MODULE_PARM_DESC(buflen, "MIDI buffer length");
 
 static unsigned int qlen = 32;
 module_param(qlen, uint, S_IRUGO);
-MODULE_PARM_DESC(qlen, "USB read request queue length");
+MODULE_PARM_DESC(qlen, "USB read and write request queue length");
 
 static unsigned int in_ports = 1;
 module_param(in_ports, uint, S_IRUGO);
@@ -86,7 +78,7 @@ MODULE_PARM_DESC(out_ports, "Number of MIDI output ports");
 static struct usb_device_descriptor device_desc = {
 	.bLength =		USB_DT_DEVICE_SIZE,
 	.bDescriptorType =	USB_DT_DEVICE,
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
 	.idVendor =		cpu_to_le16(DRIVER_VENDOR_NUM),
 	.idProduct =		cpu_to_le16(DRIVER_PRODUCT_NUM),
diff --git a/drivers/usb/gadget/legacy/hid.c b/drivers/usb/gadget/legacy/hid.c
index 7e5d2c48476e..a71a884f79fc 100644
--- a/drivers/usb/gadget/legacy/hid.c
+++ b/drivers/usb/gadget/legacy/hid.c
@@ -47,7 +47,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	/* .bDeviceClass =		USB_CLASS_COMM, */
 	/* .bDeviceSubClass =	0, */
diff --git a/drivers/usb/gadget/legacy/inode.c b/drivers/usb/gadget/legacy/inode.c
index f454c7af489c..87fb0fd6aaab 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -130,7 +130,8 @@ struct dev_data {
 					setup_can_stall : 1,
 					setup_out_ready : 1,
 					setup_out_error : 1,
-					setup_abort : 1;
+					setup_abort : 1,
+					gadget_registered : 1;
 	unsigned			setup_wLength;
 
 	/* the rest is basically write-once */
@@ -1137,10 +1138,9 @@ ep0_write (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 						dev->gadget->ep0, dev->req,
 						GFP_KERNEL);
 				}
+				spin_lock_irq(&dev->lock);
 				if (retval < 0) {
-					spin_lock_irq (&dev->lock);
 					clean_req (dev->gadget->ep0, dev->req);
-					spin_unlock_irq (&dev->lock);
 				} else
 					retval = len;
 
@@ -1180,7 +1180,8 @@ dev_release (struct inode *inode, struct file *fd)
 
 	/* closing ep0 === shutdown all */
 
-	usb_gadget_unregister_driver (&gadgetfs_driver);
+	if (dev->gadget_registered)
+		usb_gadget_unregister_driver (&gadgetfs_driver);
 
 	/* at this point "good" hardware has disconnected the
 	 * device from USB; the host won't see it any more.
@@ -1522,10 +1523,10 @@ static void destroy_ep_files (struct dev_data *dev)
 		spin_unlock_irq (&dev->lock);
 
 		/* break link to dcache */
-		mutex_lock (&parent->i_mutex);
+		inode_lock(parent);
 		d_delete (dentry);
 		dput (dentry);
-		mutex_unlock (&parent->i_mutex);
+		inode_unlock(parent);
 
 		spin_lock_irq (&dev->lock);
 	}
@@ -1848,6 +1849,7 @@ dev_config (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 		 * kick in after the ep0 descriptor is closed.
 		 */
 		value = len;
+		dev->gadget_registered = true;
 	}
 	return value;
 
diff --git a/drivers/usb/gadget/legacy/mass_storage.c b/drivers/usb/gadget/legacy/mass_storage.c
index bda3c519110f..e61af53c7d2b 100644
--- a/drivers/usb/gadget/legacy/mass_storage.c
+++ b/drivers/usb/gadget/legacy/mass_storage.c
@@ -55,7 +55,7 @@ static struct usb_device_descriptor msg_device_desc = {
 	.bLength =		sizeof msg_device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
 
 	/* Vendor and product id can be overridden by module parameters.  */
diff --git a/drivers/usb/gadget/legacy/multi.c b/drivers/usb/gadget/legacy/multi.c
index 4fe794ddcd49..229d704a620b 100644
--- a/drivers/usb/gadget/legacy/multi.c
+++ b/drivers/usb/gadget/legacy/multi.c
@@ -67,7 +67,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	.bDeviceClass =		USB_CLASS_MISC /* 0xEF */,
 	.bDeviceSubClass =	2,
diff --git a/drivers/usb/gadget/legacy/ncm.c b/drivers/usb/gadget/legacy/ncm.c
index 2bae4381332d..0aba68253e3d 100644
--- a/drivers/usb/gadget/legacy/ncm.c
+++ b/drivers/usb/gadget/legacy/ncm.c
@@ -49,7 +49,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16 (0x0200),
+	/* .bcdUSB = DYNAMIC */
 
 	.bDeviceClass =		USB_CLASS_COMM,
 	.bDeviceSubClass =	0,
diff --git a/drivers/usb/gadget/legacy/nokia.c b/drivers/usb/gadget/legacy/nokia.c
index 8b3f6fb1825d..09975046c694 100644
--- a/drivers/usb/gadget/legacy/nokia.c
+++ b/drivers/usb/gadget/legacy/nokia.c
@@ -89,7 +89,7 @@ static struct usb_gadget_strings *dev_strings[] = {
 static struct usb_device_descriptor device_desc = {
 	.bLength		= USB_DT_DEVICE_SIZE,
 	.bDescriptorType	= USB_DT_DEVICE,
-	.bcdUSB			= cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass		= USB_CLASS_COMM,
 	.idVendor		= cpu_to_le16(NOKIA_VENDOR_ID),
 	.idProduct		= cpu_to_le16(NOKIA_PRODUCT_ID),
diff --git a/drivers/usb/gadget/legacy/printer.c b/drivers/usb/gadget/legacy/printer.c
index a22d30a4def1..6f969a86175c 100644
--- a/drivers/usb/gadget/legacy/printer.c
+++ b/drivers/usb/gadget/legacy/printer.c
@@ -71,7 +71,7 @@ static struct usb_function *f_printer;
 static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
 	.bDeviceSubClass =	0,
 	.bDeviceProtocol =	0,
diff --git a/drivers/usb/gadget/legacy/serial.c b/drivers/usb/gadget/legacy/serial.c
index c5d42e0347a9..9d89adce756d 100644
--- a/drivers/usb/gadget/legacy/serial.c
+++ b/drivers/usb/gadget/legacy/serial.c
@@ -65,7 +65,7 @@ static struct usb_gadget_strings *dev_strings[] = {
 static struct usb_device_descriptor device_desc = {
 	.bLength =		USB_DT_DEVICE_SIZE,
 	.bDescriptorType =	USB_DT_DEVICE,
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	/* .bDeviceClass = f(use_acm) */
 	.bDeviceSubClass =	0,
 	.bDeviceProtocol =	0,
diff --git a/drivers/usb/gadget/legacy/tcm_usb_gadget.c b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
index 22e56158d585..0b0bb98319cd 100644
--- a/drivers/usb/gadget/legacy/tcm_usb_gadget.c
+++ b/drivers/usb/gadget/legacy/tcm_usb_gadget.c
@@ -21,1973 +21,30 @@
 #include <target/target_core_fabric.h>
 #include <asm/unaligned.h>
 
-#include "tcm_usb_gadget.h"
+#include "u_tcm.h"
 
 USB_GADGET_COMPOSITE_OPTIONS();
 
-static inline struct f_uas *to_f_uas(struct usb_function *f)
-{
-	return container_of(f, struct f_uas, function);
-}
-
-static void usbg_cmd_release(struct kref *);
-
-static inline void usbg_cleanup_cmd(struct usbg_cmd *cmd)
-{
-	kref_put(&cmd->ref, usbg_cmd_release);
-}
-
-/* Start bot.c code */
-
-static int bot_enqueue_cmd_cbw(struct f_uas *fu)
-{
-	int ret;
-
-	if (fu->flags & USBG_BOT_CMD_PEND)
-		return 0;
-
-	ret = usb_ep_queue(fu->ep_out, fu->cmd.req, GFP_ATOMIC);
-	if (!ret)
-		fu->flags |= USBG_BOT_CMD_PEND;
-	return ret;
-}
-
-static void bot_status_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct usbg_cmd *cmd = req->context;
-	struct f_uas *fu = cmd->fu;
-
-	usbg_cleanup_cmd(cmd);
-	if (req->status < 0) {
-		pr_err("ERR %s(%d)\n", __func__, __LINE__);
-		return;
-	}
-
-	/* CSW completed, wait for next CBW */
-	bot_enqueue_cmd_cbw(fu);
-}
-
-static void bot_enqueue_sense_code(struct f_uas *fu, struct usbg_cmd *cmd)
-{
-	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
-	int ret;
-	u8 *sense;
-	unsigned int csw_stat;
-
-	csw_stat = cmd->csw_code;
-
-	/*
-	 * We can't send SENSE as a response. So we take ASC & ASCQ from our
-	 * sense buffer and queue it and hope the host sends a REQUEST_SENSE
-	 * command where it learns why we failed.
-	 */
-	sense = cmd->sense_iu.sense;
-
-	csw->Tag = cmd->bot_tag;
-	csw->Status = csw_stat;
-	fu->bot_status.req->context = cmd;
-	ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_ATOMIC);
-	if (ret)
-		pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
-}
-
-static void bot_err_compl(struct usb_ep *ep, struct usb_request *req)
-{
-	struct usbg_cmd *cmd = req->context;
-	struct f_uas *fu = cmd->fu;
-
-	if (req->status < 0)
-		pr_err("ERR %s(%d)\n", __func__, __LINE__);
-
-	if (cmd->data_len) {
-		if (cmd->data_len > ep->maxpacket) {
-			req->length = ep->maxpacket;
-			cmd->data_len -= ep->maxpacket;
-		} else {
-			req->length = cmd->data_len;
-			cmd->data_len = 0;
-		}
-
-		usb_ep_queue(ep, req, GFP_ATOMIC);
-		return ;
-	}
-	bot_enqueue_sense_code(fu, cmd);
-}
-
-static void bot_send_bad_status(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
-	struct usb_request *req;
-	struct usb_ep *ep;
-
-	csw->Residue = cpu_to_le32(cmd->data_len);
-
-	if (cmd->data_len) {
-		if (cmd->is_read) {
-			ep = fu->ep_in;
-			req = fu->bot_req_in;
-		} else {
-			ep = fu->ep_out;
-			req = fu->bot_req_out;
-		}
-
-		if (cmd->data_len > fu->ep_in->maxpacket) {
-			req->length = ep->maxpacket;
-			cmd->data_len -= ep->maxpacket;
-		} else {
-			req->length = cmd->data_len;
-			cmd->data_len = 0;
-		}
-		req->complete = bot_err_compl;
-		req->context = cmd;
-		req->buf = fu->cmd.buf;
-		usb_ep_queue(ep, req, GFP_KERNEL);
-	} else {
-		bot_enqueue_sense_code(fu, cmd);
-	}
-}
-
-static int bot_send_status(struct usbg_cmd *cmd, bool moved_data)
-{
-	struct f_uas *fu = cmd->fu;
-	struct bulk_cs_wrap *csw = &fu->bot_status.csw;
-	int ret;
-
-	if (cmd->se_cmd.scsi_status == SAM_STAT_GOOD) {
-		if (!moved_data && cmd->data_len) {
-			/*
-			 * the host wants to move data, we don't. Fill / empty
-			 * the pipe and then send the csw with reside set.
-			 */
-			cmd->csw_code = US_BULK_STAT_OK;
-			bot_send_bad_status(cmd);
-			return 0;
-		}
-
-		csw->Tag = cmd->bot_tag;
-		csw->Residue = cpu_to_le32(0);
-		csw->Status = US_BULK_STAT_OK;
-		fu->bot_status.req->context = cmd;
-
-		ret = usb_ep_queue(fu->ep_in, fu->bot_status.req, GFP_KERNEL);
-		if (ret)
-			pr_err("%s(%d) ERR: %d\n", __func__, __LINE__, ret);
-	} else {
-		cmd->csw_code = US_BULK_STAT_FAIL;
-		bot_send_bad_status(cmd);
-	}
-	return 0;
-}
-
-/*
- * Called after command (no data transfer) or after the write (to device)
- * operation is completed
- */
-static int bot_send_status_response(struct usbg_cmd *cmd)
-{
-	bool moved_data = false;
-
-	if (!cmd->is_read)
-		moved_data = true;
-	return bot_send_status(cmd, moved_data);
-}
-
-/* Read request completed, now we have to send the CSW */
-static void bot_read_compl(struct usb_ep *ep, struct usb_request *req)
-{
-	struct usbg_cmd *cmd = req->context;
-
-	if (req->status < 0)
-		pr_err("ERR %s(%d)\n", __func__, __LINE__);
-
-	bot_send_status(cmd, true);
-}
-
-static int bot_send_read_response(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct usb_gadget *gadget = fuas_to_gadget(fu);
-	int ret;
-
-	if (!cmd->data_len) {
-		cmd->csw_code = US_BULK_STAT_PHASE;
-		bot_send_bad_status(cmd);
-		return 0;
-	}
-
-	if (!gadget->sg_supported) {
-		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
-		if (!cmd->data_buf)
-			return -ENOMEM;
-
-		sg_copy_to_buffer(se_cmd->t_data_sg,
-				se_cmd->t_data_nents,
-				cmd->data_buf,
-				se_cmd->data_length);
-
-		fu->bot_req_in->buf = cmd->data_buf;
-	} else {
-		fu->bot_req_in->buf = NULL;
-		fu->bot_req_in->num_sgs = se_cmd->t_data_nents;
-		fu->bot_req_in->sg = se_cmd->t_data_sg;
-	}
-
-	fu->bot_req_in->complete = bot_read_compl;
-	fu->bot_req_in->length = se_cmd->data_length;
-	fu->bot_req_in->context = cmd;
-	ret = usb_ep_queue(fu->ep_in, fu->bot_req_in, GFP_ATOMIC);
-	if (ret)
-		pr_err("%s(%d)\n", __func__, __LINE__);
-	return 0;
-}
-
-static void usbg_data_write_cmpl(struct usb_ep *, struct usb_request *);
-static int usbg_prepare_w_request(struct usbg_cmd *, struct usb_request *);
-
-static int bot_send_write_request(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct usb_gadget *gadget = fuas_to_gadget(fu);
-	int ret;
-
-	init_completion(&cmd->write_complete);
-	cmd->fu = fu;
-
-	if (!cmd->data_len) {
-		cmd->csw_code = US_BULK_STAT_PHASE;
-		return -EINVAL;
-	}
-
-	if (!gadget->sg_supported) {
-		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_KERNEL);
-		if (!cmd->data_buf)
-			return -ENOMEM;
-
-		fu->bot_req_out->buf = cmd->data_buf;
-	} else {
-		fu->bot_req_out->buf = NULL;
-		fu->bot_req_out->num_sgs = se_cmd->t_data_nents;
-		fu->bot_req_out->sg = se_cmd->t_data_sg;
-	}
-
-	fu->bot_req_out->complete = usbg_data_write_cmpl;
-	fu->bot_req_out->length = se_cmd->data_length;
-	fu->bot_req_out->context = cmd;
-
-	ret = usbg_prepare_w_request(cmd, fu->bot_req_out);
-	if (ret)
-		goto cleanup;
-	ret = usb_ep_queue(fu->ep_out, fu->bot_req_out, GFP_KERNEL);
-	if (ret)
-		pr_err("%s(%d)\n", __func__, __LINE__);
-
-	wait_for_completion(&cmd->write_complete);
-	target_execute_cmd(se_cmd);
-cleanup:
-	return ret;
-}
-
-static int bot_submit_command(struct f_uas *, void *, unsigned int);
-
-static void bot_cmd_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_uas *fu = req->context;
-	int ret;
-
-	fu->flags &= ~USBG_BOT_CMD_PEND;
-
-	if (req->status < 0)
-		return;
-
-	ret = bot_submit_command(fu, req->buf, req->actual);
-	if (ret)
-		pr_err("%s(%d): %d\n", __func__, __LINE__, ret);
-}
-
-static int bot_prepare_reqs(struct f_uas *fu)
-{
-	int ret;
-
-	fu->bot_req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
-	if (!fu->bot_req_in)
-		goto err;
-
-	fu->bot_req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
-	if (!fu->bot_req_out)
-		goto err_out;
-
-	fu->cmd.req = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
-	if (!fu->cmd.req)
-		goto err_cmd;
-
-	fu->bot_status.req = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
-	if (!fu->bot_status.req)
-		goto err_sts;
-
-	fu->bot_status.req->buf = &fu->bot_status.csw;
-	fu->bot_status.req->length = US_BULK_CS_WRAP_LEN;
-	fu->bot_status.req->complete = bot_status_complete;
-	fu->bot_status.csw.Signature = cpu_to_le32(US_BULK_CS_SIGN);
-
-	fu->cmd.buf = kmalloc(fu->ep_out->maxpacket, GFP_KERNEL);
-	if (!fu->cmd.buf)
-		goto err_buf;
-
-	fu->cmd.req->complete = bot_cmd_complete;
-	fu->cmd.req->buf = fu->cmd.buf;
-	fu->cmd.req->length = fu->ep_out->maxpacket;
-	fu->cmd.req->context = fu;
-
-	ret = bot_enqueue_cmd_cbw(fu);
-	if (ret)
-		goto err_queue;
-	return 0;
-err_queue:
-	kfree(fu->cmd.buf);
-	fu->cmd.buf = NULL;
-err_buf:
-	usb_ep_free_request(fu->ep_in, fu->bot_status.req);
-err_sts:
-	usb_ep_free_request(fu->ep_out, fu->cmd.req);
-	fu->cmd.req = NULL;
-err_cmd:
-	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
-	fu->bot_req_out = NULL;
-err_out:
-	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
-	fu->bot_req_in = NULL;
-err:
-	pr_err("BOT: endpoint setup failed\n");
-	return -ENOMEM;
-}
-
-static void bot_cleanup_old_alt(struct f_uas *fu)
-{
-	if (!(fu->flags & USBG_ENABLED))
-		return;
-
-	usb_ep_disable(fu->ep_in);
-	usb_ep_disable(fu->ep_out);
-
-	if (!fu->bot_req_in)
-		return;
-
-	usb_ep_free_request(fu->ep_in, fu->bot_req_in);
-	usb_ep_free_request(fu->ep_out, fu->bot_req_out);
-	usb_ep_free_request(fu->ep_out, fu->cmd.req);
-	usb_ep_free_request(fu->ep_out, fu->bot_status.req);
-
-	kfree(fu->cmd.buf);
-
-	fu->bot_req_in = NULL;
-	fu->bot_req_out = NULL;
-	fu->cmd.req = NULL;
-	fu->bot_status.req = NULL;
-	fu->cmd.buf = NULL;
-}
-
-static void bot_set_alt(struct f_uas *fu)
-{
-	struct usb_function *f = &fu->function;
-	struct usb_gadget *gadget = f->config->cdev->gadget;
-	int ret;
-
-	fu->flags = USBG_IS_BOT;
-
-	config_ep_by_speed(gadget, f, fu->ep_in);
-	ret = usb_ep_enable(fu->ep_in);
-	if (ret)
-		goto err_b_in;
-
-	config_ep_by_speed(gadget, f, fu->ep_out);
-	ret = usb_ep_enable(fu->ep_out);
-	if (ret)
-		goto err_b_out;
-
-	ret = bot_prepare_reqs(fu);
-	if (ret)
-		goto err_wq;
-	fu->flags |= USBG_ENABLED;
-	pr_info("Using the BOT protocol\n");
-	return;
-err_wq:
-	usb_ep_disable(fu->ep_out);
-err_b_out:
-	usb_ep_disable(fu->ep_in);
-err_b_in:
-	fu->flags = USBG_IS_BOT;
-}
-
-static int usbg_bot_setup(struct usb_function *f,
-		const struct usb_ctrlrequest *ctrl)
-{
-	struct f_uas *fu = to_f_uas(f);
-	struct usb_composite_dev *cdev = f->config->cdev;
-	u16 w_value = le16_to_cpu(ctrl->wValue);
-	u16 w_length = le16_to_cpu(ctrl->wLength);
-	int luns;
-	u8 *ret_lun;
-
-	switch (ctrl->bRequest) {
-	case US_BULK_GET_MAX_LUN:
-		if (ctrl->bRequestType != (USB_DIR_IN | USB_TYPE_CLASS |
-					USB_RECIP_INTERFACE))
-			return -ENOTSUPP;
-
-		if (w_length < 1)
-			return -EINVAL;
-		if (w_value != 0)
-			return -EINVAL;
-		luns = atomic_read(&fu->tpg->tpg_port_count);
-		if (!luns) {
-			pr_err("No LUNs configured?\n");
-			return -EINVAL;
-		}
-		/*
-		 * If 4 LUNs are present we return 3 i.e. LUN 0..3 can be
-		 * accessed. The upper limit is 0xf
-		 */
-		luns--;
-		if (luns > 0xf) {
-			pr_info_once("Limiting the number of luns to 16\n");
-			luns = 0xf;
-		}
-		ret_lun = cdev->req->buf;
-		*ret_lun = luns;
-		cdev->req->length = 1;
-		return usb_ep_queue(cdev->gadget->ep0, cdev->req, GFP_ATOMIC);
-		break;
-
-	case US_BULK_RESET_REQUEST:
-		/* XXX maybe we should remove previous requests for IN + OUT */
-		bot_enqueue_cmd_cbw(fu);
-		return 0;
-		break;
-	}
-	return -ENOTSUPP;
-}
-
-/* Start uas.c code */
-
-static void uasp_cleanup_one_stream(struct f_uas *fu, struct uas_stream *stream)
-{
-	/* We have either all three allocated or none */
-	if (!stream->req_in)
-		return;
-
-	usb_ep_free_request(fu->ep_in, stream->req_in);
-	usb_ep_free_request(fu->ep_out, stream->req_out);
-	usb_ep_free_request(fu->ep_status, stream->req_status);
-
-	stream->req_in = NULL;
-	stream->req_out = NULL;
-	stream->req_status = NULL;
-}
-
-static void uasp_free_cmdreq(struct f_uas *fu)
-{
-	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
-	kfree(fu->cmd.buf);
-	fu->cmd.req = NULL;
-	fu->cmd.buf = NULL;
-}
-
-static void uasp_cleanup_old_alt(struct f_uas *fu)
-{
-	int i;
-
-	if (!(fu->flags & USBG_ENABLED))
-		return;
-
-	usb_ep_disable(fu->ep_in);
-	usb_ep_disable(fu->ep_out);
-	usb_ep_disable(fu->ep_status);
-	usb_ep_disable(fu->ep_cmd);
-
-	for (i = 0; i < UASP_SS_EP_COMP_NUM_STREAMS; i++)
-		uasp_cleanup_one_stream(fu, &fu->stream[i]);
-	uasp_free_cmdreq(fu);
-}
-
-static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req);
-
-static int uasp_prepare_r_request(struct usbg_cmd *cmd)
-{
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct f_uas *fu = cmd->fu;
-	struct usb_gadget *gadget = fuas_to_gadget(fu);
-	struct uas_stream *stream = cmd->stream;
-
-	if (!gadget->sg_supported) {
-		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
-		if (!cmd->data_buf)
-			return -ENOMEM;
-
-		sg_copy_to_buffer(se_cmd->t_data_sg,
-				se_cmd->t_data_nents,
-				cmd->data_buf,
-				se_cmd->data_length);
-
-		stream->req_in->buf = cmd->data_buf;
-	} else {
-		stream->req_in->buf = NULL;
-		stream->req_in->num_sgs = se_cmd->t_data_nents;
-		stream->req_in->sg = se_cmd->t_data_sg;
-	}
-
-	stream->req_in->complete = uasp_status_data_cmpl;
-	stream->req_in->length = se_cmd->data_length;
-	stream->req_in->context = cmd;
-
-	cmd->state = UASP_SEND_STATUS;
-	return 0;
-}
-
-static void uasp_prepare_status(struct usbg_cmd *cmd)
-{
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct sense_iu *iu = &cmd->sense_iu;
-	struct uas_stream *stream = cmd->stream;
-
-	cmd->state = UASP_QUEUE_COMMAND;
-	iu->iu_id = IU_ID_STATUS;
-	iu->tag = cpu_to_be16(cmd->tag);
-
-	/*
-	 * iu->status_qual = cpu_to_be16(STATUS QUALIFIER SAM-4. Where R U?);
-	 */
-	iu->len = cpu_to_be16(se_cmd->scsi_sense_length);
-	iu->status = se_cmd->scsi_status;
-	stream->req_status->context = cmd;
-	stream->req_status->length = se_cmd->scsi_sense_length + 16;
-	stream->req_status->buf = iu;
-	stream->req_status->complete = uasp_status_data_cmpl;
-}
-
-static void uasp_status_data_cmpl(struct usb_ep *ep, struct usb_request *req)
-{
-	struct usbg_cmd *cmd = req->context;
-	struct uas_stream *stream = cmd->stream;
-	struct f_uas *fu = cmd->fu;
-	int ret;
-
-	if (req->status < 0)
-		goto cleanup;
-
-	switch (cmd->state) {
-	case UASP_SEND_DATA:
-		ret = uasp_prepare_r_request(cmd);
-		if (ret)
-			goto cleanup;
-		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
-		break;
-
-	case UASP_RECEIVE_DATA:
-		ret = usbg_prepare_w_request(cmd, stream->req_out);
-		if (ret)
-			goto cleanup;
-		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
-		break;
-
-	case UASP_SEND_STATUS:
-		uasp_prepare_status(cmd);
-		ret = usb_ep_queue(fu->ep_status, stream->req_status,
-				GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
-		break;
-
-	case UASP_QUEUE_COMMAND:
-		usbg_cleanup_cmd(cmd);
-		usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
-		break;
-
-	default:
-		BUG();
-	}
-	return;
-
-cleanup:
-	usbg_cleanup_cmd(cmd);
-}
-
-static int uasp_send_status_response(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct uas_stream *stream = cmd->stream;
-	struct sense_iu *iu = &cmd->sense_iu;
-
-	iu->tag = cpu_to_be16(cmd->tag);
-	stream->req_status->complete = uasp_status_data_cmpl;
-	stream->req_status->context = cmd;
-	cmd->fu = fu;
-	uasp_prepare_status(cmd);
-	return usb_ep_queue(fu->ep_status, stream->req_status, GFP_ATOMIC);
-}
-
-static int uasp_send_read_response(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct uas_stream *stream = cmd->stream;
-	struct sense_iu *iu = &cmd->sense_iu;
-	int ret;
-
-	cmd->fu = fu;
-
-	iu->tag = cpu_to_be16(cmd->tag);
-	if (fu->flags & USBG_USE_STREAMS) {
-
-		ret = uasp_prepare_r_request(cmd);
-		if (ret)
-			goto out;
-		ret = usb_ep_queue(fu->ep_in, stream->req_in, GFP_ATOMIC);
-		if (ret) {
-			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
-			kfree(cmd->data_buf);
-			cmd->data_buf = NULL;
-		}
-
-	} else {
-
-		iu->iu_id = IU_ID_READ_READY;
-		iu->tag = cpu_to_be16(cmd->tag);
-
-		stream->req_status->complete = uasp_status_data_cmpl;
-		stream->req_status->context = cmd;
-
-		cmd->state = UASP_SEND_DATA;
-		stream->req_status->buf = iu;
-		stream->req_status->length = sizeof(struct iu);
-
-		ret = usb_ep_queue(fu->ep_status, stream->req_status,
-				GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d) => %d\n", __func__, __LINE__, ret);
-	}
-out:
-	return ret;
-}
-
-static int uasp_send_write_request(struct usbg_cmd *cmd)
-{
-	struct f_uas *fu = cmd->fu;
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct uas_stream *stream = cmd->stream;
-	struct sense_iu *iu = &cmd->sense_iu;
-	int ret;
-
-	init_completion(&cmd->write_complete);
-	cmd->fu = fu;
-
-	iu->tag = cpu_to_be16(cmd->tag);
-
-	if (fu->flags & USBG_USE_STREAMS) {
-
-		ret = usbg_prepare_w_request(cmd, stream->req_out);
-		if (ret)
-			goto cleanup;
-		ret = usb_ep_queue(fu->ep_out, stream->req_out, GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d)\n", __func__, __LINE__);
-
-	} else {
-
-		iu->iu_id = IU_ID_WRITE_READY;
-		iu->tag = cpu_to_be16(cmd->tag);
-
-		stream->req_status->complete = uasp_status_data_cmpl;
-		stream->req_status->context = cmd;
-
-		cmd->state = UASP_RECEIVE_DATA;
-		stream->req_status->buf = iu;
-		stream->req_status->length = sizeof(struct iu);
-
-		ret = usb_ep_queue(fu->ep_status, stream->req_status,
-				GFP_ATOMIC);
-		if (ret)
-			pr_err("%s(%d)\n", __func__, __LINE__);
-	}
-
-	wait_for_completion(&cmd->write_complete);
-	target_execute_cmd(se_cmd);
-cleanup:
-	return ret;
-}
-
-static int usbg_submit_command(struct f_uas *, void *, unsigned int);
-
-static void uasp_cmd_complete(struct usb_ep *ep, struct usb_request *req)
-{
-	struct f_uas *fu = req->context;
-	int ret;
-
-	if (req->status < 0)
-		return;
-
-	ret = usbg_submit_command(fu, req->buf, req->actual);
-	/*
-	 * Once we tune for performance enqueue the command req here again so
-	 * we can receive a second command while we processing this one. Pay
-	 * attention to properly sync STAUS endpoint with DATA IN + OUT so you
-	 * don't break HS.
-	 */
-	if (!ret)
-		return;
-	usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
-}
-
-static int uasp_alloc_stream_res(struct f_uas *fu, struct uas_stream *stream)
-{
-	stream->req_in = usb_ep_alloc_request(fu->ep_in, GFP_KERNEL);
-	if (!stream->req_in)
-		goto out;
-
-	stream->req_out = usb_ep_alloc_request(fu->ep_out, GFP_KERNEL);
-	if (!stream->req_out)
-		goto err_out;
-
-	stream->req_status = usb_ep_alloc_request(fu->ep_status, GFP_KERNEL);
-	if (!stream->req_status)
-		goto err_sts;
-
-	return 0;
-err_sts:
-	usb_ep_free_request(fu->ep_status, stream->req_status);
-	stream->req_status = NULL;
-err_out:
-	usb_ep_free_request(fu->ep_out, stream->req_out);
-	stream->req_out = NULL;
-out:
-	return -ENOMEM;
-}
-
-static int uasp_alloc_cmd(struct f_uas *fu)
-{
-	fu->cmd.req = usb_ep_alloc_request(fu->ep_cmd, GFP_KERNEL);
-	if (!fu->cmd.req)
-		goto err;
-
-	fu->cmd.buf = kmalloc(fu->ep_cmd->maxpacket, GFP_KERNEL);
-	if (!fu->cmd.buf)
-		goto err_buf;
-
-	fu->cmd.req->complete = uasp_cmd_complete;
-	fu->cmd.req->buf = fu->cmd.buf;
-	fu->cmd.req->length = fu->ep_cmd->maxpacket;
-	fu->cmd.req->context = fu;
-	return 0;
-
-err_buf:
-	usb_ep_free_request(fu->ep_cmd, fu->cmd.req);
-err:
-	return -ENOMEM;
-}
-
-static void uasp_setup_stream_res(struct f_uas *fu, int max_streams)
-{
-	int i;
-
-	for (i = 0; i < max_streams; i++) {
-		struct uas_stream *s = &fu->stream[i];
-
-		s->req_in->stream_id = i + 1;
-		s->req_out->stream_id = i + 1;
-		s->req_status->stream_id = i + 1;
-	}
-}
-
-static int uasp_prepare_reqs(struct f_uas *fu)
-{
-	int ret;
-	int i;
-	int max_streams;
-
-	if (fu->flags & USBG_USE_STREAMS)
-		max_streams = UASP_SS_EP_COMP_NUM_STREAMS;
-	else
-		max_streams = 1;
-
-	for (i = 0; i < max_streams; i++) {
-		ret = uasp_alloc_stream_res(fu, &fu->stream[i]);
-		if (ret)
-			goto err_cleanup;
-	}
-
-	ret = uasp_alloc_cmd(fu);
-	if (ret)
-		goto err_free_stream;
-	uasp_setup_stream_res(fu, max_streams);
-
-	ret = usb_ep_queue(fu->ep_cmd, fu->cmd.req, GFP_ATOMIC);
-	if (ret)
-		goto err_free_stream;
-
-	return 0;
-
-err_free_stream:
-	uasp_free_cmdreq(fu);
-
-err_cleanup:
-	if (i) {
-		do {
-			uasp_cleanup_one_stream(fu, &fu->stream[i - 1]);
-			i--;
-		} while (i);
-	}
-	pr_err("UASP: endpoint setup failed\n");
-	return ret;
-}
-
-static void uasp_set_alt(struct f_uas *fu)
-{
-	struct usb_function *f = &fu->function;
-	struct usb_gadget *gadget = f->config->cdev->gadget;
-	int ret;
-
-	fu->flags = USBG_IS_UAS;
-
-	if (gadget->speed == USB_SPEED_SUPER)
-		fu->flags |= USBG_USE_STREAMS;
-
-	config_ep_by_speed(gadget, f, fu->ep_in);
-	ret = usb_ep_enable(fu->ep_in);
-	if (ret)
-		goto err_b_in;
-
-	config_ep_by_speed(gadget, f, fu->ep_out);
-	ret = usb_ep_enable(fu->ep_out);
-	if (ret)
-		goto err_b_out;
-
-	config_ep_by_speed(gadget, f, fu->ep_cmd);
-	ret = usb_ep_enable(fu->ep_cmd);
-	if (ret)
-		goto err_cmd;
-	config_ep_by_speed(gadget, f, fu->ep_status);
-	ret = usb_ep_enable(fu->ep_status);
-	if (ret)
-		goto err_status;
-
-	ret = uasp_prepare_reqs(fu);
-	if (ret)
-		goto err_wq;
-	fu->flags |= USBG_ENABLED;
-
-	pr_info("Using the UAS protocol\n");
-	return;
-err_wq:
-	usb_ep_disable(fu->ep_status);
-err_status:
-	usb_ep_disable(fu->ep_cmd);
-err_cmd:
-	usb_ep_disable(fu->ep_out);
-err_b_out:
-	usb_ep_disable(fu->ep_in);
-err_b_in:
-	fu->flags = 0;
-}
-
-static int get_cmd_dir(const unsigned char *cdb)
-{
-	int ret;
-
-	switch (cdb[0]) {
-	case READ_6:
-	case READ_10:
-	case READ_12:
-	case READ_16:
-	case INQUIRY:
-	case MODE_SENSE:
-	case MODE_SENSE_10:
-	case SERVICE_ACTION_IN_16:
-	case MAINTENANCE_IN:
-	case PERSISTENT_RESERVE_IN:
-	case SECURITY_PROTOCOL_IN:
-	case ACCESS_CONTROL_IN:
-	case REPORT_LUNS:
-	case READ_BLOCK_LIMITS:
-	case READ_POSITION:
-	case READ_CAPACITY:
-	case READ_TOC:
-	case READ_FORMAT_CAPACITIES:
-	case REQUEST_SENSE:
-		ret = DMA_FROM_DEVICE;
-		break;
-
-	case WRITE_6:
-	case WRITE_10:
-	case WRITE_12:
-	case WRITE_16:
-	case MODE_SELECT:
-	case MODE_SELECT_10:
-	case WRITE_VERIFY:
-	case WRITE_VERIFY_12:
-	case PERSISTENT_RESERVE_OUT:
-	case MAINTENANCE_OUT:
-	case SECURITY_PROTOCOL_OUT:
-	case ACCESS_CONTROL_OUT:
-		ret = DMA_TO_DEVICE;
-		break;
-	case ALLOW_MEDIUM_REMOVAL:
-	case TEST_UNIT_READY:
-	case SYNCHRONIZE_CACHE:
-	case START_STOP:
-	case ERASE:
-	case REZERO_UNIT:
-	case SEEK_10:
-	case SPACE:
-	case VERIFY:
-	case WRITE_FILEMARKS:
-		ret = DMA_NONE;
-		break;
-	default:
-		pr_warn("target: Unknown data direction for SCSI Opcode "
-				"0x%02x\n", cdb[0]);
-		ret = -EINVAL;
-	}
-	return ret;
-}
-
-static void usbg_data_write_cmpl(struct usb_ep *ep, struct usb_request *req)
-{
-	struct usbg_cmd *cmd = req->context;
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-
-	if (req->status < 0) {
-		pr_err("%s() state %d transfer failed\n", __func__, cmd->state);
-		goto cleanup;
-	}
-
-	if (req->num_sgs == 0) {
-		sg_copy_from_buffer(se_cmd->t_data_sg,
-				se_cmd->t_data_nents,
-				cmd->data_buf,
-				se_cmd->data_length);
-	}
-
-	complete(&cmd->write_complete);
-	return;
-
-cleanup:
-	usbg_cleanup_cmd(cmd);
-}
-
-static int usbg_prepare_w_request(struct usbg_cmd *cmd, struct usb_request *req)
-{
-	struct se_cmd *se_cmd = &cmd->se_cmd;
-	struct f_uas *fu = cmd->fu;
-	struct usb_gadget *gadget = fuas_to_gadget(fu);
-
-	if (!gadget->sg_supported) {
-		cmd->data_buf = kmalloc(se_cmd->data_length, GFP_ATOMIC);
-		if (!cmd->data_buf)
-			return -ENOMEM;
-
-		req->buf = cmd->data_buf;
-	} else {
-		req->buf = NULL;
-		req->num_sgs = se_cmd->t_data_nents;
-		req->sg = se_cmd->t_data_sg;
-	}
-
-	req->complete = usbg_data_write_cmpl;
-	req->length = se_cmd->data_length;
-	req->context = cmd;
-	return 0;
-}
-
-static int usbg_send_status_response(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	struct f_uas *fu = cmd->fu;
-
-	if (fu->flags & USBG_IS_BOT)
-		return bot_send_status_response(cmd);
-	else
-		return uasp_send_status_response(cmd);
-}
-
-static int usbg_send_write_request(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	struct f_uas *fu = cmd->fu;
-
-	if (fu->flags & USBG_IS_BOT)
-		return bot_send_write_request(cmd);
-	else
-		return uasp_send_write_request(cmd);
-}
-
-static int usbg_send_read_response(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	struct f_uas *fu = cmd->fu;
-
-	if (fu->flags & USBG_IS_BOT)
-		return bot_send_read_response(cmd);
-	else
-		return uasp_send_read_response(cmd);
-}
-
-static void usbg_cmd_work(struct work_struct *work)
-{
-	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
-	struct se_cmd *se_cmd;
-	struct tcm_usbg_nexus *tv_nexus;
-	struct usbg_tpg *tpg;
-	int dir;
-
-	se_cmd = &cmd->se_cmd;
-	tpg = cmd->fu->tpg;
-	tv_nexus = tpg->tpg_nexus;
-	dir = get_cmd_dir(cmd->cmd_buf);
-	if (dir < 0) {
-		transport_init_se_cmd(se_cmd,
-				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
-				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
-				cmd->prio_attr, cmd->sense_iu.sense);
-		goto out;
-	}
-
-	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
-			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
-			0, cmd->prio_attr, dir, TARGET_SCF_UNKNOWN_SIZE) < 0)
-		goto out;
-
-	return;
-
-out:
-	transport_send_check_condition_and_sense(se_cmd,
-			TCM_UNSUPPORTED_SCSI_OPCODE, 1);
-	usbg_cleanup_cmd(cmd);
-}
-
-static int usbg_submit_command(struct f_uas *fu,
-		void *cmdbuf, unsigned int len)
-{
-	struct command_iu *cmd_iu = cmdbuf;
-	struct usbg_cmd *cmd;
-	struct usbg_tpg *tpg;
-	struct se_cmd *se_cmd;
-	struct tcm_usbg_nexus *tv_nexus;
-	u32 cmd_len;
-	int ret;
-
-	if (cmd_iu->iu_id != IU_ID_COMMAND) {
-		pr_err("Unsupported type %d\n", cmd_iu->iu_id);
-		return -EINVAL;
-	}
-
-	cmd = kzalloc(sizeof *cmd, GFP_ATOMIC);
-	if (!cmd)
-		return -ENOMEM;
-
-	cmd->fu = fu;
-
-	/* XXX until I figure out why I can't free in on complete */
-	kref_init(&cmd->ref);
-	kref_get(&cmd->ref);
-
-	tpg = fu->tpg;
-	cmd_len = (cmd_iu->len & ~0x3) + 16;
-	if (cmd_len > USBG_MAX_CMD)
-		goto err;
-
-	memcpy(cmd->cmd_buf, cmd_iu->cdb, cmd_len);
-
-	cmd->tag = be16_to_cpup(&cmd_iu->tag);
-	cmd->se_cmd.tag = cmd->tag;
-	if (fu->flags & USBG_USE_STREAMS) {
-		if (cmd->tag > UASP_SS_EP_COMP_NUM_STREAMS)
-			goto err;
-		if (!cmd->tag)
-			cmd->stream = &fu->stream[0];
-		else
-			cmd->stream = &fu->stream[cmd->tag - 1];
-	} else {
-		cmd->stream = &fu->stream[0];
-	}
-
-	tv_nexus = tpg->tpg_nexus;
-	if (!tv_nexus) {
-		pr_err("Missing nexus, ignoring command\n");
-		goto err;
-	}
-
-	switch (cmd_iu->prio_attr & 0x7) {
-	case UAS_HEAD_TAG:
-		cmd->prio_attr = TCM_HEAD_TAG;
-		break;
-	case UAS_ORDERED_TAG:
-		cmd->prio_attr = TCM_ORDERED_TAG;
-		break;
-	case UAS_ACA:
-		cmd->prio_attr = TCM_ACA_TAG;
-		break;
-	default:
-		pr_debug_once("Unsupported prio_attr: %02x.\n",
-				cmd_iu->prio_attr);
-	case UAS_SIMPLE_TAG:
-		cmd->prio_attr = TCM_SIMPLE_TAG;
-		break;
-	}
-
-	se_cmd = &cmd->se_cmd;
-	cmd->unpacked_lun = scsilun_to_int(&cmd_iu->lun);
-
-	INIT_WORK(&cmd->work, usbg_cmd_work);
-	ret = queue_work(tpg->workqueue, &cmd->work);
-	if (ret < 0)
-		goto err;
-
-	return 0;
-err:
-	kfree(cmd);
-	return -EINVAL;
-}
-
-static void bot_cmd_work(struct work_struct *work)
-{
-	struct usbg_cmd *cmd = container_of(work, struct usbg_cmd, work);
-	struct se_cmd *se_cmd;
-	struct tcm_usbg_nexus *tv_nexus;
-	struct usbg_tpg *tpg;
-	int dir;
-
-	se_cmd = &cmd->se_cmd;
-	tpg = cmd->fu->tpg;
-	tv_nexus = tpg->tpg_nexus;
-	dir = get_cmd_dir(cmd->cmd_buf);
-	if (dir < 0) {
-		transport_init_se_cmd(se_cmd,
-				tv_nexus->tvn_se_sess->se_tpg->se_tpg_tfo,
-				tv_nexus->tvn_se_sess, cmd->data_len, DMA_NONE,
-				cmd->prio_attr, cmd->sense_iu.sense);
-		goto out;
-	}
-
-	if (target_submit_cmd(se_cmd, tv_nexus->tvn_se_sess,
-			cmd->cmd_buf, cmd->sense_iu.sense, cmd->unpacked_lun,
-			cmd->data_len, cmd->prio_attr, dir, 0) < 0)
-		goto out;
-
-	return;
-
-out:
-	transport_send_check_condition_and_sense(se_cmd,
-				TCM_UNSUPPORTED_SCSI_OPCODE, 1);
-	usbg_cleanup_cmd(cmd);
-}
-
-static int bot_submit_command(struct f_uas *fu,
-		void *cmdbuf, unsigned int len)
-{
-	struct bulk_cb_wrap *cbw = cmdbuf;
-	struct usbg_cmd *cmd;
-	struct usbg_tpg *tpg;
-	struct se_cmd *se_cmd;
-	struct tcm_usbg_nexus *tv_nexus;
-	u32 cmd_len;
-	int ret;
-
-	if (cbw->Signature != cpu_to_le32(US_BULK_CB_SIGN)) {
-		pr_err("Wrong signature on CBW\n");
-		return -EINVAL;
-	}
-	if (len != 31) {
-		pr_err("Wrong length for CBW\n");
-		return -EINVAL;
-	}
-
-	cmd_len = cbw->Length;
-	if (cmd_len < 1 || cmd_len > 16)
-		return -EINVAL;
-
-	cmd = kzalloc(sizeof *cmd, GFP_ATOMIC);
-	if (!cmd)
-		return -ENOMEM;
-
-	cmd->fu = fu;
-
-	/* XXX until I figure out why I can't free in on complete */
-	kref_init(&cmd->ref);
-	kref_get(&cmd->ref);
-
-	tpg = fu->tpg;
-
-	memcpy(cmd->cmd_buf, cbw->CDB, cmd_len);
-
-	cmd->bot_tag = cbw->Tag;
-
-	tv_nexus = tpg->tpg_nexus;
-	if (!tv_nexus) {
-		pr_err("Missing nexus, ignoring command\n");
-		goto err;
-	}
-
-	cmd->prio_attr = TCM_SIMPLE_TAG;
-	se_cmd = &cmd->se_cmd;
-	cmd->unpacked_lun = cbw->Lun;
-	cmd->is_read = cbw->Flags & US_BULK_FLAG_IN ? 1 : 0;
-	cmd->data_len = le32_to_cpu(cbw->DataTransferLength);
-	cmd->se_cmd.tag = le32_to_cpu(cmd->bot_tag);
-
-	INIT_WORK(&cmd->work, bot_cmd_work);
-	ret = queue_work(tpg->workqueue, &cmd->work);
-	if (ret < 0)
-		goto err;
-
-	return 0;
-err:
-	kfree(cmd);
-	return -EINVAL;
-}
-
-/* Start fabric.c code */
-
-static int usbg_check_true(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static int usbg_check_false(struct se_portal_group *se_tpg)
-{
-	return 0;
-}
-
-static char *usbg_get_fabric_name(void)
-{
-	return "usb_gadget";
-}
-
-static char *usbg_get_fabric_wwn(struct se_portal_group *se_tpg)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	struct usbg_tport *tport = tpg->tport;
-
-	return &tport->tport_name[0];
-}
-
-static u16 usbg_get_tag(struct se_portal_group *se_tpg)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-	return tpg->tport_tpgt;
-}
-
-static u32 usbg_tpg_get_inst_index(struct se_portal_group *se_tpg)
-{
-	return 1;
-}
-
-static void usbg_cmd_release(struct kref *ref)
-{
-	struct usbg_cmd *cmd = container_of(ref, struct usbg_cmd,
-			ref);
-
-	transport_generic_free_cmd(&cmd->se_cmd, 0);
-}
-
-static void usbg_release_cmd(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-	kfree(cmd->data_buf);
-	kfree(cmd);
-	return;
-}
-
-static int usbg_shutdown_session(struct se_session *se_sess)
-{
-	return 0;
-}
-
-static void usbg_close_session(struct se_session *se_sess)
-{
-	return;
-}
-
-static u32 usbg_sess_get_index(struct se_session *se_sess)
-{
-	return 0;
-}
-
-/*
- * XXX Error recovery: return != 0 if we expect writes. Dunno when that could be
- */
-static int usbg_write_pending_status(struct se_cmd *se_cmd)
-{
-	return 0;
-}
-
-static void usbg_set_default_node_attrs(struct se_node_acl *nacl)
-{
-	return;
-}
-
-static int usbg_get_cmd_state(struct se_cmd *se_cmd)
-{
-	return 0;
-}
-
-static void usbg_queue_tm_rsp(struct se_cmd *se_cmd)
-{
-}
-
-static void usbg_aborted_task(struct se_cmd *se_cmd)
-{
-	return;
-}
-
-static const char *usbg_check_wwn(const char *name)
-{
-	const char *n;
-	unsigned int len;
-
-	n = strstr(name, "naa.");
-	if (!n)
-		return NULL;
-	n += 4;
-	len = strlen(n);
-	if (len == 0 || len > USBG_NAMELEN - 1)
-		return NULL;
-	return n;
-}
-
-static int usbg_init_nodeacl(struct se_node_acl *se_nacl, const char *name)
-{
-	if (!usbg_check_wwn(name))
-		return -EINVAL;
-	return 0;
-}
-
-struct usbg_tpg *the_only_tpg_I_currently_have;
-
-static struct se_portal_group *usbg_make_tpg(
-	struct se_wwn *wwn,
-	struct config_group *group,
-	const char *name)
-{
-	struct usbg_tport *tport = container_of(wwn, struct usbg_tport,
-			tport_wwn);
-	struct usbg_tpg *tpg;
-	unsigned long tpgt;
-	int ret;
-
-	if (strstr(name, "tpgt_") != name)
-		return ERR_PTR(-EINVAL);
-	if (kstrtoul(name + 5, 0, &tpgt) || tpgt > UINT_MAX)
-		return ERR_PTR(-EINVAL);
-	if (the_only_tpg_I_currently_have) {
-		pr_err("Until the gadget framework can't handle multiple\n");
-		pr_err("gadgets, you can't do this here.\n");
-		return ERR_PTR(-EBUSY);
-	}
-
-	tpg = kzalloc(sizeof(struct usbg_tpg), GFP_KERNEL);
-	if (!tpg)
-		return ERR_PTR(-ENOMEM);
-	mutex_init(&tpg->tpg_mutex);
-	atomic_set(&tpg->tpg_port_count, 0);
-	tpg->workqueue = alloc_workqueue("tcm_usb_gadget", 0, 1);
-	if (!tpg->workqueue) {
-		kfree(tpg);
-		return NULL;
-	}
-
-	tpg->tport = tport;
-	tpg->tport_tpgt = tpgt;
-
-	/*
-	 * SPC doesn't assign a protocol identifier for USB-SCSI, so we
-	 * pretend to be SAS..
-	 */
-	ret = core_tpg_register(wwn, &tpg->se_tpg, SCSI_PROTOCOL_SAS);
-	if (ret < 0) {
-		destroy_workqueue(tpg->workqueue);
-		kfree(tpg);
-		return NULL;
-	}
-	the_only_tpg_I_currently_have = tpg;
-	return &tpg->se_tpg;
-}
-
-static void usbg_drop_tpg(struct se_portal_group *se_tpg)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg,
-				struct usbg_tpg, se_tpg);
-
-	core_tpg_deregister(se_tpg);
-	destroy_workqueue(tpg->workqueue);
-	kfree(tpg);
-	the_only_tpg_I_currently_have = NULL;
-}
-
-static struct se_wwn *usbg_make_tport(
-	struct target_fabric_configfs *tf,
-	struct config_group *group,
-	const char *name)
-{
-	struct usbg_tport *tport;
-	const char *wnn_name;
-	u64 wwpn = 0;
-
-	wnn_name = usbg_check_wwn(name);
-	if (!wnn_name)
-		return ERR_PTR(-EINVAL);
-
-	tport = kzalloc(sizeof(struct usbg_tport), GFP_KERNEL);
-	if (!(tport))
-		return ERR_PTR(-ENOMEM);
-	tport->tport_wwpn = wwpn;
-	snprintf(tport->tport_name, sizeof(tport->tport_name), "%s", wnn_name);
-	return &tport->tport_wwn;
-}
-
-static void usbg_drop_tport(struct se_wwn *wwn)
-{
-	struct usbg_tport *tport = container_of(wwn,
-				struct usbg_tport, tport_wwn);
-	kfree(tport);
-}
-
-/*
- * If somebody feels like dropping the version property, go ahead.
- */
-static ssize_t usbg_wwn_version_show(struct config_item *item, char *page)
-{
-	return sprintf(page, "usb-gadget fabric module\n");
-}
-
-CONFIGFS_ATTR_RO(usbg_wwn_, version);
-
-static struct configfs_attribute *usbg_wwn_attrs[] = {
-	&usbg_wwn_attr_version,
-	NULL,
-};
-
-static ssize_t tcm_usbg_tpg_enable_show(struct config_item *item, char *page)
-{
-	struct se_portal_group *se_tpg = to_tpg(item);
-	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-
-	return snprintf(page, PAGE_SIZE, "%u\n", tpg->gadget_connect);
-}
-
-static int usbg_attach(struct usbg_tpg *);
-static void usbg_detach(struct usbg_tpg *);
-
-static ssize_t tcm_usbg_tpg_enable_store(struct config_item *item,
-		const char *page, size_t count)
-{
-	struct se_portal_group *se_tpg = to_tpg(item);
-	struct usbg_tpg  *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-	unsigned long op;
-	ssize_t ret;
-
-	ret = kstrtoul(page, 0, &op);
-	if (ret < 0)
-		return -EINVAL;
-	if (op > 1)
-		return -EINVAL;
-
-	if (op && tpg->gadget_connect)
-		goto out;
-	if (!op && !tpg->gadget_connect)
-		goto out;
-
-	if (op) {
-		ret = usbg_attach(tpg);
-		if (ret)
-			goto out;
-	} else {
-		usbg_detach(tpg);
-	}
-	tpg->gadget_connect = op;
-out:
-	return count;
-}
-
-static ssize_t tcm_usbg_tpg_nexus_show(struct config_item *item, char *page)
-{
-	struct se_portal_group *se_tpg = to_tpg(item);
-	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-	struct tcm_usbg_nexus *tv_nexus;
-	ssize_t ret;
-
-	mutex_lock(&tpg->tpg_mutex);
-	tv_nexus = tpg->tpg_nexus;
-	if (!tv_nexus) {
-		ret = -ENODEV;
-		goto out;
-	}
-	ret = snprintf(page, PAGE_SIZE, "%s\n",
-			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
-out:
-	mutex_unlock(&tpg->tpg_mutex);
-	return ret;
-}
-
-static int tcm_usbg_make_nexus(struct usbg_tpg *tpg, char *name)
-{
-	struct se_portal_group *se_tpg;
-	struct tcm_usbg_nexus *tv_nexus;
-	int ret;
-
-	mutex_lock(&tpg->tpg_mutex);
-	if (tpg->tpg_nexus) {
-		ret = -EEXIST;
-		pr_debug("tpg->tpg_nexus already exists\n");
-		goto err_unlock;
-	}
-	se_tpg = &tpg->se_tpg;
-
-	ret = -ENOMEM;
-	tv_nexus = kzalloc(sizeof(*tv_nexus), GFP_KERNEL);
-	if (!tv_nexus)
-		goto err_unlock;
-	tv_nexus->tvn_se_sess = transport_init_session(TARGET_PROT_NORMAL);
-	if (IS_ERR(tv_nexus->tvn_se_sess))
-		goto err_free;
-
-	/*
-	 * Since we are running in 'demo mode' this call with generate a
-	 * struct se_node_acl for the tcm_vhost struct se_portal_group with
-	 * the SCSI Initiator port name of the passed configfs group 'name'.
-	 */
-	tv_nexus->tvn_se_sess->se_node_acl = core_tpg_check_initiator_node_acl(
-			se_tpg, name);
-	if (!tv_nexus->tvn_se_sess->se_node_acl) {
-		pr_debug("core_tpg_check_initiator_node_acl() failed"
-				" for %s\n", name);
-		goto err_session;
-	}
-	/*
-	 * Now register the TCM vHost virtual I_T Nexus as active.
-	 */
-	transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
-			tv_nexus->tvn_se_sess, tv_nexus);
-	tpg->tpg_nexus = tv_nexus;
-	mutex_unlock(&tpg->tpg_mutex);
-	return 0;
-
-err_session:
-	transport_free_session(tv_nexus->tvn_se_sess);
-err_free:
-	kfree(tv_nexus);
-err_unlock:
-	mutex_unlock(&tpg->tpg_mutex);
-	return ret;
-}
-
-static int tcm_usbg_drop_nexus(struct usbg_tpg *tpg)
-{
-	struct se_session *se_sess;
-	struct tcm_usbg_nexus *tv_nexus;
-	int ret = -ENODEV;
-
-	mutex_lock(&tpg->tpg_mutex);
-	tv_nexus = tpg->tpg_nexus;
-	if (!tv_nexus)
-		goto out;
-
-	se_sess = tv_nexus->tvn_se_sess;
-	if (!se_sess)
-		goto out;
-
-	if (atomic_read(&tpg->tpg_port_count)) {
-		ret = -EPERM;
-		pr_err("Unable to remove Host I_T Nexus with"
-				" active TPG port count: %d\n",
-				atomic_read(&tpg->tpg_port_count));
-		goto out;
-	}
-
-	pr_debug("Removing I_T Nexus to Initiator Port: %s\n",
-			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
-	/*
-	 * Release the SCSI I_T Nexus to the emulated vHost Target Port
-	 */
-	transport_deregister_session(tv_nexus->tvn_se_sess);
-	tpg->tpg_nexus = NULL;
-
-	kfree(tv_nexus);
-	ret = 0;
-out:
-	mutex_unlock(&tpg->tpg_mutex);
-	return ret;
-}
-
-static ssize_t tcm_usbg_tpg_nexus_store(struct config_item *item,
-		const char *page, size_t count)
-{
-	struct se_portal_group *se_tpg = to_tpg(item);
-	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-	unsigned char i_port[USBG_NAMELEN], *ptr;
-	int ret;
-
-	if (!strncmp(page, "NULL", 4)) {
-		ret = tcm_usbg_drop_nexus(tpg);
-		return (!ret) ? count : ret;
-	}
-	if (strlen(page) >= USBG_NAMELEN) {
-		pr_err("Emulated NAA Sas Address: %s, exceeds"
-				" max: %d\n", page, USBG_NAMELEN);
-		return -EINVAL;
-	}
-	snprintf(i_port, USBG_NAMELEN, "%s", page);
-
-	ptr = strstr(i_port, "naa.");
-	if (!ptr) {
-		pr_err("Missing 'naa.' prefix\n");
-		return -EINVAL;
-	}
-
-	if (i_port[strlen(i_port) - 1] == '\n')
-		i_port[strlen(i_port) - 1] = '\0';
-
-	ret = tcm_usbg_make_nexus(tpg, &i_port[4]);
-	if (ret < 0)
-		return ret;
-	return count;
-}
-
-CONFIGFS_ATTR(tcm_usbg_tpg_, enable);
-CONFIGFS_ATTR(tcm_usbg_tpg_, nexus);
-
-static struct configfs_attribute *usbg_base_attrs[] = {
-	&tcm_usbg_tpg_attr_enable,
-	&tcm_usbg_tpg_attr_nexus,
-	NULL,
-};
-
-static int usbg_port_link(struct se_portal_group *se_tpg, struct se_lun *lun)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-
-	atomic_inc(&tpg->tpg_port_count);
-	smp_mb__after_atomic();
-	return 0;
-}
-
-static void usbg_port_unlink(struct se_portal_group *se_tpg,
-		struct se_lun *se_lun)
-{
-	struct usbg_tpg *tpg = container_of(se_tpg, struct usbg_tpg, se_tpg);
-
-	atomic_dec(&tpg->tpg_port_count);
-	smp_mb__after_atomic();
-}
-
-static int usbg_check_stop_free(struct se_cmd *se_cmd)
-{
-	struct usbg_cmd *cmd = container_of(se_cmd, struct usbg_cmd,
-			se_cmd);
-
-	kref_put(&cmd->ref, usbg_cmd_release);
-	return 1;
-}
-
-static const struct target_core_fabric_ops usbg_ops = {
-	.module				= THIS_MODULE,
-	.name				= "usb_gadget",
-	.get_fabric_name		= usbg_get_fabric_name,
-	.tpg_get_wwn			= usbg_get_fabric_wwn,
-	.tpg_get_tag			= usbg_get_tag,
-	.tpg_check_demo_mode		= usbg_check_true,
-	.tpg_check_demo_mode_cache	= usbg_check_false,
-	.tpg_check_demo_mode_write_protect = usbg_check_false,
-	.tpg_check_prod_mode_write_protect = usbg_check_false,
-	.tpg_get_inst_index		= usbg_tpg_get_inst_index,
-	.release_cmd			= usbg_release_cmd,
-	.shutdown_session		= usbg_shutdown_session,
-	.close_session			= usbg_close_session,
-	.sess_get_index			= usbg_sess_get_index,
-	.sess_get_initiator_sid		= NULL,
-	.write_pending			= usbg_send_write_request,
-	.write_pending_status		= usbg_write_pending_status,
-	.set_default_node_attributes	= usbg_set_default_node_attrs,
-	.get_cmd_state			= usbg_get_cmd_state,
-	.queue_data_in			= usbg_send_read_response,
-	.queue_status			= usbg_send_status_response,
-	.queue_tm_rsp			= usbg_queue_tm_rsp,
-	.aborted_task			= usbg_aborted_task,
-	.check_stop_free		= usbg_check_stop_free,
-
-	.fabric_make_wwn		= usbg_make_tport,
-	.fabric_drop_wwn		= usbg_drop_tport,
-	.fabric_make_tpg		= usbg_make_tpg,
-	.fabric_drop_tpg		= usbg_drop_tpg,
-	.fabric_post_link		= usbg_port_link,
-	.fabric_pre_unlink		= usbg_port_unlink,
-	.fabric_init_nodeacl		= usbg_init_nodeacl,
-
-	.tfc_wwn_attrs			= usbg_wwn_attrs,
-	.tfc_tpg_base_attrs		= usbg_base_attrs,
-};
-
-/* Start gadget.c code */
-
-static struct usb_interface_descriptor bot_intf_desc = {
-	.bLength =              sizeof(bot_intf_desc),
-	.bDescriptorType =      USB_DT_INTERFACE,
-	.bNumEndpoints =        2,
-	.bAlternateSetting =	USB_G_ALT_INT_BBB,
-	.bInterfaceClass =      USB_CLASS_MASS_STORAGE,
-	.bInterfaceSubClass =   USB_SC_SCSI,
-	.bInterfaceProtocol =   USB_PR_BULK,
-};
-
-static struct usb_interface_descriptor uasp_intf_desc = {
-	.bLength =		sizeof(uasp_intf_desc),
-	.bDescriptorType =	USB_DT_INTERFACE,
-	.bNumEndpoints =	4,
-	.bAlternateSetting =	USB_G_ALT_INT_UAS,
-	.bInterfaceClass =	USB_CLASS_MASS_STORAGE,
-	.bInterfaceSubClass =	USB_SC_SCSI,
-	.bInterfaceProtocol =	USB_PR_UAS,
-};
-
-static struct usb_endpoint_descriptor uasp_bi_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor uasp_fs_bi_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_pipe_usage_descriptor uasp_bi_pipe_desc = {
-	.bLength =		sizeof(uasp_bi_pipe_desc),
-	.bDescriptorType =	USB_DT_PIPE_USAGE,
-	.bPipeID =		DATA_IN_PIPE_ID,
-};
-
-static struct usb_endpoint_descriptor uasp_ss_bi_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor uasp_bi_ep_comp_desc = {
-	.bLength =		sizeof(uasp_bi_ep_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-	.bMaxBurst =		0,
-	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
-	.wBytesPerInterval =	0,
-};
-
-static struct usb_ss_ep_comp_descriptor bot_bi_ep_comp_desc = {
-	.bLength =		sizeof(bot_bi_ep_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-	.bMaxBurst =		0,
-};
-
-static struct usb_endpoint_descriptor uasp_bo_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor uasp_fs_bo_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_pipe_usage_descriptor uasp_bo_pipe_desc = {
-	.bLength =		sizeof(uasp_bo_pipe_desc),
-	.bDescriptorType =	USB_DT_PIPE_USAGE,
-	.bPipeID =		DATA_OUT_PIPE_ID,
-};
-
-static struct usb_endpoint_descriptor uasp_ss_bo_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(0x400),
-};
-
-static struct usb_ss_ep_comp_descriptor uasp_bo_ep_comp_desc = {
-	.bLength =		sizeof(uasp_bo_ep_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
-};
-
-static struct usb_ss_ep_comp_descriptor bot_bo_ep_comp_desc = {
-	.bLength =		sizeof(bot_bo_ep_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-};
-
-static struct usb_endpoint_descriptor uasp_status_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor uasp_fs_status_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_pipe_usage_descriptor uasp_status_pipe_desc = {
-	.bLength =		sizeof(uasp_status_pipe_desc),
-	.bDescriptorType =	USB_DT_PIPE_USAGE,
-	.bPipeID =		STATUS_PIPE_ID,
-};
-
-static struct usb_endpoint_descriptor uasp_ss_status_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_IN,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor uasp_status_in_ep_comp_desc = {
-	.bLength =		sizeof(uasp_status_in_ep_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-	.bmAttributes =		UASP_SS_EP_COMP_LOG_STREAMS,
-};
-
-static struct usb_endpoint_descriptor uasp_cmd_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(512),
-};
-
-static struct usb_endpoint_descriptor uasp_fs_cmd_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-};
-
-static struct usb_pipe_usage_descriptor uasp_cmd_pipe_desc = {
-	.bLength =		sizeof(uasp_cmd_pipe_desc),
-	.bDescriptorType =	USB_DT_PIPE_USAGE,
-	.bPipeID =		CMD_PIPE_ID,
-};
-
-static struct usb_endpoint_descriptor uasp_ss_cmd_desc = {
-	.bLength =		USB_DT_ENDPOINT_SIZE,
-	.bDescriptorType =	USB_DT_ENDPOINT,
-	.bEndpointAddress =	USB_DIR_OUT,
-	.bmAttributes =		USB_ENDPOINT_XFER_BULK,
-	.wMaxPacketSize =	cpu_to_le16(1024),
-};
-
-static struct usb_ss_ep_comp_descriptor uasp_cmd_comp_desc = {
-	.bLength =		sizeof(uasp_cmd_comp_desc),
-	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
-};
-
-static struct usb_descriptor_header *uasp_fs_function_desc[] = {
-	(struct usb_descriptor_header *) &bot_intf_desc,
-	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
-	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
-
-	(struct usb_descriptor_header *) &uasp_intf_desc,
-	(struct usb_descriptor_header *) &uasp_fs_bi_desc,
-	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_fs_bo_desc,
-	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_fs_status_desc,
-	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_fs_cmd_desc,
-	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
-	NULL,
-};
-
-static struct usb_descriptor_header *uasp_hs_function_desc[] = {
-	(struct usb_descriptor_header *) &bot_intf_desc,
-	(struct usb_descriptor_header *) &uasp_bi_desc,
-	(struct usb_descriptor_header *) &uasp_bo_desc,
-
-	(struct usb_descriptor_header *) &uasp_intf_desc,
-	(struct usb_descriptor_header *) &uasp_bi_desc,
-	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_bo_desc,
-	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_status_desc,
-	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_cmd_desc,
-	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
-	NULL,
-};
-
-static struct usb_descriptor_header *uasp_ss_function_desc[] = {
-	(struct usb_descriptor_header *) &bot_intf_desc,
-	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
-	(struct usb_descriptor_header *) &bot_bi_ep_comp_desc,
-	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
-	(struct usb_descriptor_header *) &bot_bo_ep_comp_desc,
-
-	(struct usb_descriptor_header *) &uasp_intf_desc,
-	(struct usb_descriptor_header *) &uasp_ss_bi_desc,
-	(struct usb_descriptor_header *) &uasp_bi_ep_comp_desc,
-	(struct usb_descriptor_header *) &uasp_bi_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_ss_bo_desc,
-	(struct usb_descriptor_header *) &uasp_bo_ep_comp_desc,
-	(struct usb_descriptor_header *) &uasp_bo_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_ss_status_desc,
-	(struct usb_descriptor_header *) &uasp_status_in_ep_comp_desc,
-	(struct usb_descriptor_header *) &uasp_status_pipe_desc,
-	(struct usb_descriptor_header *) &uasp_ss_cmd_desc,
-	(struct usb_descriptor_header *) &uasp_cmd_comp_desc,
-	(struct usb_descriptor_header *) &uasp_cmd_pipe_desc,
-	NULL,
-};
-
 #define UAS_VENDOR_ID	0x0525	/* NetChip */
 #define UAS_PRODUCT_ID	0xa4a5	/* Linux-USB File-backed Storage Gadget */
 
 static struct usb_device_descriptor usbg_device_desc = {
 	.bLength =		sizeof(usbg_device_desc),
 	.bDescriptorType =	USB_DT_DEVICE,
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass =		USB_CLASS_PER_INTERFACE,
 	.idVendor =		cpu_to_le16(UAS_VENDOR_ID),
 	.idProduct =		cpu_to_le16(UAS_PRODUCT_ID),
 	.bNumConfigurations =   1,
 };
 
+#define USB_G_STR_CONFIG USB_GADGET_FIRST_AVAIL_IDX
+
 static struct usb_string	usbg_us_strings[] = {
 	[USB_GADGET_MANUFACTURER_IDX].s	= "Target Manufactor",
 	[USB_GADGET_PRODUCT_IDX].s	= "Target Product",
 	[USB_GADGET_SERIAL_IDX].s	= "000000000001",
 	[USB_G_STR_CONFIG].s		= "default config",
-	[USB_G_STR_INT_UAS].s		= "USB Attached SCSI",
-	[USB_G_STR_INT_BBB].s		= "Bulk Only Transport",
 	{ },
 };
 
@@ -2001,184 +58,42 @@ static struct usb_gadget_strings *usbg_strings[] = {
 	NULL,
 };
 
-static int guas_unbind(struct usb_composite_dev *cdev)
-{
-	return 0;
-}
-
-static struct usb_configuration usbg_config_driver = {
-	.label                  = "Linux Target",
-	.bConfigurationValue    = 1,
-	.bmAttributes           = USB_CONFIG_ATT_SELFPOWER,
-};
+static struct usb_function_instance *fi_tcm;
+static struct usb_function *f_tcm;
 
-static int usbg_bind(struct usb_configuration *c, struct usb_function *f)
+static int guas_unbind(struct usb_composite_dev *cdev)
 {
-	struct f_uas		*fu = to_f_uas(f);
-	struct usb_gadget	*gadget = c->cdev->gadget;
-	struct usb_ep		*ep;
-	int			iface;
-	int			ret;
-
-	iface = usb_interface_id(c, f);
-	if (iface < 0)
-		return iface;
-
-	bot_intf_desc.bInterfaceNumber = iface;
-	uasp_intf_desc.bInterfaceNumber = iface;
-	fu->iface = iface;
-	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bi_desc,
-			&uasp_bi_ep_comp_desc);
-	if (!ep)
-		goto ep_fail;
-	fu->ep_in = ep;
-
-	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_bo_desc,
-			&uasp_bo_ep_comp_desc);
-	if (!ep)
-		goto ep_fail;
-	fu->ep_out = ep;
-
-	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_status_desc,
-			&uasp_status_in_ep_comp_desc);
-	if (!ep)
-		goto ep_fail;
-	fu->ep_status = ep;
-
-	ep = usb_ep_autoconfig_ss(gadget, &uasp_ss_cmd_desc,
-			&uasp_cmd_comp_desc);
-	if (!ep)
-		goto ep_fail;
-	fu->ep_cmd = ep;
-
-	/* Assume endpoint addresses are the same for both speeds */
-	uasp_bi_desc.bEndpointAddress =	uasp_ss_bi_desc.bEndpointAddress;
-	uasp_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
-	uasp_status_desc.bEndpointAddress =
-		uasp_ss_status_desc.bEndpointAddress;
-	uasp_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
-
-	uasp_fs_bi_desc.bEndpointAddress = uasp_ss_bi_desc.bEndpointAddress;
-	uasp_fs_bo_desc.bEndpointAddress = uasp_ss_bo_desc.bEndpointAddress;
-	uasp_fs_status_desc.bEndpointAddress =
-		uasp_ss_status_desc.bEndpointAddress;
-	uasp_fs_cmd_desc.bEndpointAddress = uasp_ss_cmd_desc.bEndpointAddress;
-
-	ret = usb_assign_descriptors(f, uasp_fs_function_desc,
-			uasp_hs_function_desc, uasp_ss_function_desc);
-	if (ret)
-		goto ep_fail;
+	if (!IS_ERR_OR_NULL(f_tcm))
+		usb_put_function(f_tcm);
 
 	return 0;
-ep_fail:
-	pr_err("Can't claim all required eps\n");
-	return -ENOTSUPP;
 }
 
-static void usbg_unbind(struct usb_configuration *c, struct usb_function *f)
+static int tcm_do_config(struct usb_configuration *c)
 {
-	struct f_uas *fu = to_f_uas(f);
+	int status;
 
-	usb_free_all_descriptors(f);
-	kfree(fu);
-}
-
-struct guas_setup_wq {
-	struct work_struct work;
-	struct f_uas *fu;
-	unsigned int alt;
-};
-
-static void usbg_delayed_set_alt(struct work_struct *wq)
-{
-	struct guas_setup_wq *work = container_of(wq, struct guas_setup_wq,
-			work);
-	struct f_uas *fu = work->fu;
-	int alt = work->alt;
-
-	kfree(work);
-
-	if (fu->flags & USBG_IS_BOT)
-		bot_cleanup_old_alt(fu);
-	if (fu->flags & USBG_IS_UAS)
-		uasp_cleanup_old_alt(fu);
-
-	if (alt == USB_G_ALT_INT_BBB)
-		bot_set_alt(fu);
-	else if (alt == USB_G_ALT_INT_UAS)
-		uasp_set_alt(fu);
-	usb_composite_setup_continue(fu->function.config->cdev);
-}
-
-static int usbg_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
-{
-	struct f_uas *fu = to_f_uas(f);
-
-	if ((alt == USB_G_ALT_INT_BBB) || (alt == USB_G_ALT_INT_UAS)) {
-		struct guas_setup_wq *work;
+	f_tcm = usb_get_function(fi_tcm);
+	if (IS_ERR(f_tcm))
+		return PTR_ERR(f_tcm);
 
-		work = kmalloc(sizeof(*work), GFP_ATOMIC);
-		if (!work)
-			return -ENOMEM;
-		INIT_WORK(&work->work, usbg_delayed_set_alt);
-		work->fu = fu;
-		work->alt = alt;
-		schedule_work(&work->work);
-		return USB_GADGET_DELAYED_STATUS;
+	status = usb_add_function(c, f_tcm);
+	if (status < 0) {
+		usb_put_function(f_tcm);
+		return status;
 	}
-	return -EOPNOTSUPP;
-}
-
-static void usbg_disable(struct usb_function *f)
-{
-	struct f_uas *fu = to_f_uas(f);
-
-	if (fu->flags & USBG_IS_UAS)
-		uasp_cleanup_old_alt(fu);
-	else if (fu->flags & USBG_IS_BOT)
-		bot_cleanup_old_alt(fu);
-	fu->flags = 0;
-}
-
-static int usbg_setup(struct usb_function *f,
-		const struct usb_ctrlrequest *ctrl)
-{
-	struct f_uas *fu = to_f_uas(f);
-
-	if (!(fu->flags & USBG_IS_BOT))
-		return -EOPNOTSUPP;
 
-	return usbg_bot_setup(f, ctrl);
+	return 0;
 }
 
-static int usbg_cfg_bind(struct usb_configuration *c)
-{
-	struct f_uas *fu;
-	int ret;
-
-	fu = kzalloc(sizeof(*fu), GFP_KERNEL);
-	if (!fu)
-		return -ENOMEM;
-	fu->function.name = "Target Function";
-	fu->function.bind = usbg_bind;
-	fu->function.unbind = usbg_unbind;
-	fu->function.set_alt = usbg_set_alt;
-	fu->function.setup = usbg_setup;
-	fu->function.disable = usbg_disable;
-	fu->tpg = the_only_tpg_I_currently_have;
-
-	bot_intf_desc.iInterface = usbg_us_strings[USB_G_STR_INT_BBB].id;
-	uasp_intf_desc.iInterface = usbg_us_strings[USB_G_STR_INT_UAS].id;
-
-	ret = usb_add_function(c, &fu->function);
-	if (ret)
-		goto err;
+static struct usb_configuration usbg_config_driver = {
+	.label                  = "Linux Target",
+	.bConfigurationValue    = 1,
+	.bmAttributes           = USB_CONFIG_ATT_SELFPOWER,
+};
 
-	return 0;
-err:
-	kfree(fu);
-	return ret;
-}
+static int usbg_attach(struct usb_function_instance *f);
+static void usbg_detach(struct usb_function_instance *f);
 
 static int usb_target_bind(struct usb_composite_dev *cdev)
 {
@@ -2196,8 +111,7 @@ static int usb_target_bind(struct usb_composite_dev *cdev)
 	usbg_config_driver.iConfiguration =
 		usbg_us_strings[USB_G_STR_CONFIG].id;
 
-	ret = usb_add_config(cdev, &usbg_config_driver,
-			usbg_cfg_bind);
+	ret = usb_add_config(cdev, &usbg_config_driver, tcm_do_config);
 	if (ret)
 		return ret;
 	usb_composite_overwrite_options(cdev, &coverwrite);
@@ -2213,25 +127,44 @@ static struct usb_composite_driver usbg_driver = {
 	.unbind         = guas_unbind,
 };
 
-static int usbg_attach(struct usbg_tpg *tpg)
+static int usbg_attach(struct usb_function_instance *f)
 {
 	return usb_composite_probe(&usbg_driver);
 }
 
-static void usbg_detach(struct usbg_tpg *tpg)
+static void usbg_detach(struct usb_function_instance *f)
 {
 	usb_composite_unregister(&usbg_driver);
 }
 
 static int __init usb_target_gadget_init(void)
 {
-	return target_register_template(&usbg_ops);
+	struct f_tcm_opts *tcm_opts;
+
+	fi_tcm = usb_get_function_instance("tcm");
+	if (IS_ERR(fi_tcm))
+		return PTR_ERR(fi_tcm);
+
+	tcm_opts = container_of(fi_tcm, struct f_tcm_opts, func_inst);
+	mutex_lock(&tcm_opts->dep_lock);
+	tcm_opts->tcm_register_callback = usbg_attach;
+	tcm_opts->tcm_unregister_callback = usbg_detach;
+	tcm_opts->dependent = THIS_MODULE;
+	tcm_opts->can_attach = true;
+	tcm_opts->has_dep = true;
+	mutex_unlock(&tcm_opts->dep_lock);
+
+	fi_tcm->set_inst_name(fi_tcm, "tcm-legacy");
+
+	return 0;
 }
 module_init(usb_target_gadget_init);
 
 static void __exit usb_target_gadget_exit(void)
 {
-	target_unregister_template(&usbg_ops);
+	if (!IS_ERR_OR_NULL(fi_tcm))
+		usb_put_function_instance(fi_tcm);
+
 }
 module_exit(usb_target_gadget_exit);
 
diff --git a/drivers/usb/gadget/legacy/webcam.c b/drivers/usb/gadget/legacy/webcam.c
index 72c976bf3530..f9661cd627c8 100644
--- a/drivers/usb/gadget/legacy/webcam.c
+++ b/drivers/usb/gadget/legacy/webcam.c
@@ -77,7 +77,7 @@ static struct usb_function *f_uvc;
 static struct usb_device_descriptor webcam_device_descriptor = {
 	.bLength		= USB_DT_DEVICE_SIZE,
 	.bDescriptorType	= USB_DT_DEVICE,
-	.bcdUSB			= cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass		= USB_CLASS_MISC,
 	.bDeviceSubClass	= 0x02,
 	.bDeviceProtocol	= 0x01,
diff --git a/drivers/usb/gadget/legacy/zero.c b/drivers/usb/gadget/legacy/zero.c
index 37a410056fed..d02e2ce73ea5 100644
--- a/drivers/usb/gadget/legacy/zero.c
+++ b/drivers/usb/gadget/legacy/zero.c
@@ -68,6 +68,8 @@ static struct usb_zero_options gzero_options = {
 	.isoc_maxpacket = GZERO_ISOC_MAXPACKET,
 	.bulk_buflen = GZERO_BULK_BUFLEN,
 	.qlen = GZERO_QLEN,
+	.ss_bulk_qlen = GZERO_SS_BULK_QLEN,
+	.ss_iso_qlen = GZERO_SS_ISO_QLEN,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -113,7 +115,7 @@ static struct usb_device_descriptor device_desc = {
 	.bLength =		sizeof device_desc,
 	.bDescriptorType =	USB_DT_DEVICE,
 
-	.bcdUSB =		cpu_to_le16(0x0200),
+	/* .bcdUSB = DYNAMIC */
 	.bDeviceClass =		USB_CLASS_VENDOR_SPEC,
 
 	.idVendor =		cpu_to_le16(DRIVER_VENDOR_NUM),
@@ -255,6 +257,14 @@ static struct usb_function_instance *func_inst_lb;
 module_param_named(qlen, gzero_options.qlen, uint, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(qlen, "depth of loopback queue");
 
+module_param_named(ss_bulk_qlen, gzero_options.ss_bulk_qlen, uint,
+		S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(bulk_qlen, "depth of sourcesink queue for bulk transfer");
+
+module_param_named(ss_iso_qlen, gzero_options.ss_iso_qlen, uint,
+		S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(iso_qlen, "depth of sourcesink queue for iso transfer");
+
 static int zero_bind(struct usb_composite_dev *cdev)
 {
 	struct f_ss_opts	*ss_opts;
@@ -285,6 +295,8 @@ static int zero_bind(struct usb_composite_dev *cdev)
 	ss_opts->isoc_mult = gzero_options.isoc_mult;
 	ss_opts->isoc_maxburst = gzero_options.isoc_maxburst;
 	ss_opts->bulk_buflen = gzero_options.bulk_buflen;
+	ss_opts->bulk_qlen = gzero_options.ss_bulk_qlen;
+	ss_opts->iso_qlen = gzero_options.ss_iso_qlen;
 
 	func_ss = usb_get_function(func_inst_ss);
 	if (IS_ERR(func_ss)) {
diff --git a/drivers/usb/gadget/u_f.c b/drivers/usb/gadget/u_f.c
index c6276f0268ae..4bc7eea8bfc8 100644
--- a/drivers/usb/gadget/u_f.c
+++ b/drivers/usb/gadget/u_f.c
@@ -11,7 +11,6 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/usb/gadget.h>
 #include "u_f.h"
 
 struct usb_request *alloc_ep_req(struct usb_ep *ep, int len, int default_len)
diff --git a/drivers/usb/gadget/u_f.h b/drivers/usb/gadget/u_f.h
index 1d5f0eb68552..4247cc098a89 100644
--- a/drivers/usb/gadget/u_f.h
+++ b/drivers/usb/gadget/u_f.h
@@ -16,6 +16,8 @@
 #ifndef __U_F_H__
 #define __U_F_H__
 
+#include <linux/usb/gadget.h>
+
 /* Variable Length Array Macros **********************************************/
 #define vla_group(groupname) size_t groupname##__next = 0
 #define vla_group_size(groupname) groupname##__next
@@ -45,8 +47,12 @@
 struct usb_ep;
 struct usb_request;
 
+/* Requests allocated via alloc_ep_req() must be freed by free_ep_req(). */
 struct usb_request *alloc_ep_req(struct usb_ep *ep, int len, int default_len);
+static inline void free_ep_req(struct usb_ep *ep, struct usb_request *req)
+{
+	kfree(req->buf);
+	usb_ep_free_request(ep, req);
+}
 
 #endif /* __U_F_H__ */
-
-
diff --git a/drivers/usb/gadget/udc/Kconfig b/drivers/usb/gadget/udc/Kconfig
index cdbff54e07ac..753c29bd11ad 100644
--- a/drivers/usb/gadget/udc/Kconfig
+++ b/drivers/usb/gadget/udc/Kconfig
@@ -174,6 +174,17 @@ config USB_RENESAS_USBHS_UDC
 	   dynamically linked module called "renesas_usbhs" and force all
 	   gadget drivers to also be dynamically linked.
 
+config USB_RENESAS_USB3
+	tristate 'Renesas USB3.0 Peripheral controller'
+	depends on ARCH_SHMOBILE || COMPILE_TEST
+	help
+	   Renesas USB3.0 Peripheral controller is a USB peripheral controller
+	   that supports super, high, and full speed USB 3.0 data transfers.
+
+	   Say "y" to link the driver statically, or "m" to build a
+	   dynamically linked module called "renesas_usb3" and force all
+	   gadget drivers to also be dynamically linked.
+
 config USB_PXA27X
 	tristate "PXA 27x"
 	help
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index fba2049bf985..dfee53446319 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -19,6 +19,7 @@ fsl_usb2_udc-y			:= fsl_udc_core.o
 fsl_usb2_udc-$(CONFIG_ARCH_MXC)	+= fsl_mxc_udc.o
 obj-$(CONFIG_USB_M66592)	+= m66592-udc.o
 obj-$(CONFIG_USB_R8A66597)	+= r8a66597-udc.o
+obj-$(CONFIG_USB_RENESAS_USB3)	+= renesas_usb3.o
 obj-$(CONFIG_USB_FSL_QE)	+= fsl_qe_udc.o
 obj-$(CONFIG_USB_S3C_HSUDC)	+= s3c-hsudc.o
 obj-$(CONFIG_USB_LPC32XX)	+= lpc32xx_udc.o
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index f92f5aff0dd5..8755b2c2aada 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -91,7 +91,7 @@ static ssize_t queue_dbg_read(struct file *file, char __user *buf,
 	if (!access_ok(VERIFY_WRITE, buf, nbytes))
 		return -EFAULT;
 
-	mutex_lock(&file_inode(file)->i_mutex);
+	inode_lock(file_inode(file));
 	list_for_each_entry_safe(req, tmp_req, queue, queue) {
 		len = snprintf(tmpbuf, sizeof(tmpbuf),
 				"%8p %08x %c%c%c %5d %c%c%c\n",
@@ -118,7 +118,7 @@ static ssize_t queue_dbg_read(struct file *file, char __user *buf,
 		nbytes -= len;
 		buf += len;
 	}
-	mutex_unlock(&file_inode(file)->i_mutex);
+	inode_unlock(file_inode(file));
 
 	return actual;
 }
@@ -143,7 +143,7 @@ static int regs_dbg_open(struct inode *inode, struct file *file)
 	u32 *data;
 	int ret = -ENOMEM;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	udc = inode->i_private;
 	data = kmalloc(inode->i_size, GFP_KERNEL);
 	if (!data)
@@ -158,7 +158,7 @@ static int regs_dbg_open(struct inode *inode, struct file *file)
 	ret = 0;
 
 out:
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 
 	return ret;
 }
@@ -169,11 +169,11 @@ static ssize_t regs_dbg_read(struct file *file, char __user *buf,
 	struct inode *inode = file_inode(file);
 	int ret;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	ret = simple_read_from_buffer(buf, nbytes, ppos,
 			file->private_data,
 			file_inode(file)->i_size);
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 
 	return ret;
 }
diff --git a/drivers/usb/gadget/udc/bcm63xx_udc.c b/drivers/usb/gadget/udc/bcm63xx_udc.c
index 8cbb00325824..f5fccb3e4152 100644
--- a/drivers/usb/gadget/udc/bcm63xx_udc.c
+++ b/drivers/usb/gadget/udc/bcm63xx_udc.c
@@ -1083,7 +1083,7 @@ static int bcm63xx_ep_disable(struct usb_ep *ep)
 	struct bcm63xx_ep *bep = our_ep(ep);
 	struct bcm63xx_udc *udc = bep->udc;
 	struct iudma_ch *iudma = bep->iudma;
-	struct list_head *pos, *n;
+	struct bcm63xx_req *breq, *n;
 	unsigned long flags;
 
 	if (!ep || !ep->desc)
@@ -1099,10 +1099,7 @@ static int bcm63xx_ep_disable(struct usb_ep *ep)
 	iudma_reset_channel(udc, iudma);
 
 	if (!list_empty(&bep->queue)) {
-		list_for_each_safe(pos, n, &bep->queue) {
-			struct bcm63xx_req *breq =
-				list_entry(pos, struct bcm63xx_req, queue);
-
+		list_for_each_entry_safe(breq, n, &bep->queue, queue) {
 			usb_gadget_unmap_request(&udc->gadget, &breq->req,
 						 iudma->is_tx);
 			list_del(&breq->queue);
diff --git a/drivers/usb/gadget/udc/fsl_qe_udc.c b/drivers/usb/gadget/udc/fsl_qe_udc.c
index 5fb6f8b4f0b4..93d28cb00b76 100644
--- a/drivers/usb/gadget/udc/fsl_qe_udc.c
+++ b/drivers/usb/gadget/udc/fsl_qe_udc.c
@@ -38,7 +38,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/otg.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/qe.h>
 #include <asm/cpm.h>
 #include <asm/dma.h>
 #include <asm/reg.h>
@@ -2340,7 +2340,7 @@ static struct qe_udc *qe_udc_config(struct platform_device *ofdev)
 {
 	struct qe_udc *udc;
 	struct device_node *np = ofdev->dev.of_node;
-	unsigned int tmp_addr = 0;
+	unsigned long tmp_addr = 0;
 	struct usb_device_para __iomem *usbpram;
 	unsigned int i;
 	u64 size;
diff --git a/drivers/usb/gadget/udc/gr_udc.c b/drivers/usb/gadget/udc/gr_udc.c
index b9429bc42511..39b7136d31d9 100644
--- a/drivers/usb/gadget/udc/gr_udc.c
+++ b/drivers/usb/gadget/udc/gr_udc.c
@@ -253,13 +253,12 @@ static struct gr_dma_desc *gr_alloc_dma_desc(struct gr_ep *ep, gfp_t gfp_flags)
 	dma_addr_t paddr;
 	struct gr_dma_desc *dma_desc;
 
-	dma_desc = dma_pool_alloc(ep->dev->desc_pool, gfp_flags, &paddr);
+	dma_desc = dma_pool_zalloc(ep->dev->desc_pool, gfp_flags, &paddr);
 	if (!dma_desc) {
 		dev_err(ep->dev->dev, "Could not allocate from DMA pool\n");
 		return NULL;
 	}
 
-	memset(dma_desc, 0, sizeof(*dma_desc));
 	dma_desc->paddr = paddr;
 
 	return dma_desc;
diff --git a/drivers/usb/gadget/udc/lpc32xx_udc.c b/drivers/usb/gadget/udc/lpc32xx_udc.c
index 00b5006baf15..79fe6b77ee44 100644
--- a/drivers/usb/gadget/udc/lpc32xx_udc.c
+++ b/drivers/usb/gadget/udc/lpc32xx_udc.c
@@ -28,42 +28,29 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
-#include <linux/ioport.h>
-#include <linux/slab.h>
-#include <linux/errno.h>
-#include <linux/init.h>
-#include <linux/list.h>
+#include <linux/dma-mapping.h>
+#include <linux/dmapool.h>
+#include <linux/i2c.h>
 #include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
 #include <linux/proc_fs.h>
-#include <linux/clk.h>
+#include <linux/slab.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
-#include <linux/i2c.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <linux/dma-mapping.h>
-#include <linux/dmapool.h>
-#include <linux/workqueue.h>
-#include <linux/of.h>
 #include <linux/usb/isp1301.h>
 
-#include <asm/byteorder.h>
-#include <mach/hardware.h>
-#include <linux/io.h>
-#include <asm/irq.h>
-
-#include <mach/platform.h>
-#include <mach/irqs.h>
-#include <mach/board.h>
 #ifdef CONFIG_USB_GADGET_DEBUG_FILES
 #include <linux/debugfs.h>
 #include <linux/seq_file.h>
 #endif
 
+#include <mach/hardware.h>
+#include <mach/platform.h>
+
 /*
  * USB device configuration structure
  */
diff --git a/drivers/usb/gadget/udc/net2280.h b/drivers/usb/gadget/udc/net2280.h
index 4dff60d34f73..0d32052bf16f 100644
--- a/drivers/usb/gadget/udc/net2280.h
+++ b/drivers/usb/gadget/udc/net2280.h
@@ -369,9 +369,20 @@ static inline void set_max_speed(struct net2280_ep *ep, u32 max)
 	static const u32 ep_enhanced[9] = { 0x10, 0x60, 0x30, 0x80,
 					  0x50, 0x20, 0x70, 0x40, 0x90 };
 
-	if (ep->dev->enhanced_mode)
+	if (ep->dev->enhanced_mode) {
 		reg = ep_enhanced[ep->num];
-	else{
+		switch (ep->dev->gadget.speed) {
+		case USB_SPEED_SUPER:
+			reg += 2;
+			break;
+		case USB_SPEED_FULL:
+			reg += 1;
+			break;
+		case USB_SPEED_HIGH:
+		default:
+			break;
+		}
+	} else {
 		reg = (ep->num + 1) * 0x10;
 		if (ep->dev->gadget.speed != USB_SPEED_HIGH)
 			reg += 1;
diff --git a/drivers/usb/gadget/udc/renesas_usb3.c b/drivers/usb/gadget/udc/renesas_usb3.c
new file mode 100644
index 000000000000..93a3bec81df7
--- /dev/null
+++ b/drivers/usb/gadget/udc/renesas_usb3.c
@@ -0,0 +1,1975 @@
+/*
+ * Renesas USB3.0 Peripheral driver (USB gadget)
+ *
+ * Copyright (C) 2015  Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+/* register definitions */
+#define USB3_AXI_INT_STA	0x008
+#define USB3_AXI_INT_ENA	0x00c
+#define USB3_DMA_INT_STA	0x010
+#define USB3_DMA_INT_ENA	0x014
+#define USB3_USB_COM_CON	0x200
+#define USB3_USB20_CON		0x204
+#define USB3_USB30_CON		0x208
+#define USB3_USB_STA		0x210
+#define USB3_DRD_CON		0x218
+#define USB3_USB_INT_STA_1	0x220
+#define USB3_USB_INT_STA_2	0x224
+#define USB3_USB_INT_ENA_1	0x228
+#define USB3_USB_INT_ENA_2	0x22c
+#define USB3_STUP_DAT_0		0x230
+#define USB3_STUP_DAT_1		0x234
+#define USB3_P0_MOD		0x280
+#define USB3_P0_CON		0x288
+#define USB3_P0_STA		0x28c
+#define USB3_P0_INT_STA		0x290
+#define USB3_P0_INT_ENA		0x294
+#define USB3_P0_LNG		0x2a0
+#define USB3_P0_READ		0x2a4
+#define USB3_P0_WRITE		0x2a8
+#define USB3_PIPE_COM		0x2b0
+#define USB3_PN_MOD		0x2c0
+#define USB3_PN_RAMMAP		0x2c4
+#define USB3_PN_CON		0x2c8
+#define USB3_PN_STA		0x2cc
+#define USB3_PN_INT_STA		0x2d0
+#define USB3_PN_INT_ENA		0x2d4
+#define USB3_PN_LNG		0x2e0
+#define USB3_PN_READ		0x2e4
+#define USB3_PN_WRITE		0x2e8
+#define USB3_SSIFCMD		0x340
+
+/* AXI_INT_ENA and AXI_INT_STA */
+#define AXI_INT_DMAINT		BIT(31)
+#define AXI_INT_EPCINT		BIT(30)
+
+/* LCLKSEL */
+#define LCLKSEL_LSEL		BIT(18)
+
+/* USB_COM_CON */
+#define USB_COM_CON_CONF		BIT(24)
+#define USB_COM_CON_SPD_MODE		BIT(17)
+#define USB_COM_CON_EP0_EN		BIT(16)
+#define USB_COM_CON_DEV_ADDR_SHIFT	8
+#define USB_COM_CON_DEV_ADDR_MASK	GENMASK(14, USB_COM_CON_DEV_ADDR_SHIFT)
+#define USB_COM_CON_DEV_ADDR(n)		(((n) << USB_COM_CON_DEV_ADDR_SHIFT) & \
+					 USB_COM_CON_DEV_ADDR_MASK)
+#define USB_COM_CON_RX_DETECTION	BIT(1)
+#define USB_COM_CON_PIPE_CLR		BIT(0)
+
+/* USB20_CON */
+#define USB20_CON_B2_PUE		BIT(31)
+#define USB20_CON_B2_SUSPEND		BIT(24)
+#define USB20_CON_B2_CONNECT		BIT(17)
+#define USB20_CON_B2_TSTMOD_SHIFT	8
+#define USB20_CON_B2_TSTMOD_MASK	GENMASK(10, USB20_CON_B2_TSTMOD_SHIFT)
+#define USB20_CON_B2_TSTMOD(n)		(((n) << USB20_CON_B2_TSTMOD_SHIFT) & \
+					 USB20_CON_B2_TSTMOD_MASK)
+#define USB20_CON_B2_TSTMOD_EN		BIT(0)
+
+/* USB30_CON */
+#define USB30_CON_POW_SEL_SHIFT		24
+#define USB30_CON_POW_SEL_MASK		GENMASK(26, USB30_CON_POW_SEL_SHIFT)
+#define USB30_CON_POW_SEL_IN_U3		BIT(26)
+#define USB30_CON_POW_SEL_IN_DISCON	0
+#define USB30_CON_POW_SEL_P2_TO_P0	BIT(25)
+#define USB30_CON_POW_SEL_P0_TO_P3	BIT(24)
+#define USB30_CON_POW_SEL_P0_TO_P2	0
+#define USB30_CON_B3_PLLWAKE		BIT(23)
+#define USB30_CON_B3_CONNECT		BIT(17)
+#define USB30_CON_B3_HOTRST_CMP		BIT(1)
+
+/* USB_STA */
+#define USB_STA_SPEED_MASK	(BIT(2) | BIT(1))
+#define USB_STA_SPEED_HS	BIT(2)
+#define USB_STA_SPEED_FS	BIT(1)
+#define USB_STA_SPEED_SS	0
+#define USB_STA_VBUS_STA	BIT(0)
+
+/* DRD_CON */
+#define DRD_CON_PERI_CON	BIT(24)
+
+/* USB_INT_ENA_1 and USB_INT_STA_1 */
+#define USB_INT_1_B3_PLLWKUP	BIT(31)
+#define USB_INT_1_B3_LUPSUCS	BIT(30)
+#define USB_INT_1_B3_DISABLE	BIT(27)
+#define USB_INT_1_B3_WRMRST	BIT(21)
+#define USB_INT_1_B3_HOTRST	BIT(20)
+#define USB_INT_1_B2_USBRST	BIT(12)
+#define USB_INT_1_B2_L1SPND	BIT(11)
+#define USB_INT_1_B2_SPND	BIT(9)
+#define USB_INT_1_B2_RSUM	BIT(8)
+#define USB_INT_1_SPEED		BIT(1)
+#define USB_INT_1_VBUS_CNG	BIT(0)
+
+/* USB_INT_ENA_2 and USB_INT_STA_2 */
+#define USB_INT_2_PIPE(n)	BIT(n)
+
+/* P0_MOD */
+#define P0_MOD_DIR		BIT(6)
+
+/* P0_CON and PN_CON */
+#define PX_CON_BYTE_EN_MASK		(BIT(10) | BIT(9))
+#define PX_CON_BYTE_EN_SHIFT		9
+#define PX_CON_BYTE_EN_BYTES(n)		(((n) << PX_CON_BYTE_EN_SHIFT) & \
+					 PX_CON_BYTE_EN_MASK)
+#define PX_CON_SEND			BIT(8)
+
+/* P0_CON */
+#define P0_CON_ST_RES_MASK		(BIT(27) | BIT(26))
+#define P0_CON_ST_RES_FORCE_STALL	BIT(27)
+#define P0_CON_ST_RES_NORMAL		BIT(26)
+#define P0_CON_ST_RES_FORCE_NRDY	0
+#define P0_CON_OT_RES_MASK		(BIT(25) | BIT(24))
+#define P0_CON_OT_RES_FORCE_STALL	BIT(25)
+#define P0_CON_OT_RES_NORMAL		BIT(24)
+#define P0_CON_OT_RES_FORCE_NRDY	0
+#define P0_CON_IN_RES_MASK		(BIT(17) | BIT(16))
+#define P0_CON_IN_RES_FORCE_STALL	BIT(17)
+#define P0_CON_IN_RES_NORMAL		BIT(16)
+#define P0_CON_IN_RES_FORCE_NRDY	0
+#define P0_CON_RES_WEN			BIT(7)
+#define P0_CON_BCLR			BIT(1)
+
+/* P0_STA and PN_STA */
+#define PX_STA_BUFSTS		BIT(0)
+
+/* P0_INT_ENA and P0_INT_STA */
+#define P0_INT_STSED		BIT(18)
+#define P0_INT_STSST		BIT(17)
+#define P0_INT_SETUP		BIT(16)
+#define P0_INT_RCVNL		BIT(8)
+#define P0_INT_ERDY		BIT(7)
+#define P0_INT_FLOW		BIT(6)
+#define P0_INT_STALL		BIT(2)
+#define P0_INT_NRDY		BIT(1)
+#define P0_INT_BFRDY		BIT(0)
+#define P0_INT_ALL_BITS		(P0_INT_STSED | P0_INT_SETUP | P0_INT_BFRDY)
+
+/* PN_MOD */
+#define PN_MOD_DIR		BIT(6)
+#define PN_MOD_TYPE_SHIFT	4
+#define PN_MOD_TYPE_MASK	GENMASK(5, PN_MOD_TYPE_SHIFT)
+#define PN_MOD_TYPE(n)		(((n) << PN_MOD_TYPE_SHIFT) & \
+				 PN_MOD_TYPE_MASK)
+#define PN_MOD_EPNUM_MASK	GENMASK(3, 0)
+#define PN_MOD_EPNUM(n)		((n) & PN_MOD_EPNUM_MASK)
+
+/* PN_RAMMAP */
+#define PN_RAMMAP_RAMAREA_SHIFT	29
+#define PN_RAMMAP_RAMAREA_MASK	GENMASK(31, PN_RAMMAP_RAMAREA_SHIFT)
+#define PN_RAMMAP_RAMAREA_16KB	BIT(31)
+#define PN_RAMMAP_RAMAREA_8KB	(BIT(30) | BIT(29))
+#define PN_RAMMAP_RAMAREA_4KB	BIT(30)
+#define PN_RAMMAP_RAMAREA_2KB	BIT(29)
+#define PN_RAMMAP_RAMAREA_1KB	0
+#define PN_RAMMAP_MPKT_SHIFT	16
+#define PN_RAMMAP_MPKT_MASK	GENMASK(26, PN_RAMMAP_MPKT_SHIFT)
+#define PN_RAMMAP_MPKT(n)	(((n) << PN_RAMMAP_MPKT_SHIFT) & \
+				 PN_RAMMAP_MPKT_MASK)
+#define PN_RAMMAP_RAMIF_SHIFT	14
+#define PN_RAMMAP_RAMIF_MASK	GENMASK(15, PN_RAMMAP_RAMIF_SHIFT)
+#define PN_RAMMAP_RAMIF(n)	(((n) << PN_RAMMAP_RAMIF_SHIFT) & \
+				 PN_RAMMAP_RAMIF_MASK)
+#define PN_RAMMAP_BASEAD_MASK	GENMASK(13, 0)
+#define PN_RAMMAP_BASEAD(offs)	(((offs) >> 3) & PN_RAMMAP_BASEAD_MASK)
+#define PN_RAMMAP_DATA(area, ramif, basead)	((PN_RAMMAP_##area) | \
+						 (PN_RAMMAP_RAMIF(ramif)) | \
+						 (PN_RAMMAP_BASEAD(basead)))
+
+/* PN_CON */
+#define PN_CON_EN		BIT(31)
+#define PN_CON_DATAIF_EN	BIT(30)
+#define PN_CON_RES_MASK		(BIT(17) | BIT(16))
+#define PN_CON_RES_FORCE_STALL	BIT(17)
+#define PN_CON_RES_NORMAL	BIT(16)
+#define PN_CON_RES_FORCE_NRDY	0
+#define PN_CON_LAST		BIT(11)
+#define PN_CON_RES_WEN		BIT(7)
+#define PN_CON_CLR		BIT(0)
+
+/* PN_INT_STA and PN_INT_ENA */
+#define PN_INT_LSTTR	BIT(4)
+#define PN_INT_BFRDY	BIT(0)
+
+/* USB3_SSIFCMD */
+#define SSIFCMD_URES_U2		BIT(9)
+#define SSIFCMD_URES_U1		BIT(8)
+#define SSIFCMD_UDIR_U2		BIT(7)
+#define SSIFCMD_UDIR_U1		BIT(6)
+#define SSIFCMD_UREQ_U2		BIT(5)
+#define SSIFCMD_UREQ_U1		BIT(4)
+
+#define USB3_EP0_SS_MAX_PACKET_SIZE	512
+#define USB3_EP0_HSFS_MAX_PACKET_SIZE	64
+#define USB3_EP0_BUF_SIZE		8
+#define USB3_MAX_NUM_PIPES		30
+#define USB3_WAIT_US			3
+
+struct renesas_usb3;
+struct renesas_usb3_request {
+	struct usb_request	req;
+	struct list_head	queue;
+};
+
+#define USB3_EP_NAME_SIZE	8
+struct renesas_usb3_ep {
+	struct usb_ep ep;
+	struct renesas_usb3 *usb3;
+	int num;
+	char ep_name[USB3_EP_NAME_SIZE];
+	struct list_head queue;
+	u32 rammap_val;
+	bool dir_in;
+	bool halt;
+	bool wedge;
+	bool started;
+};
+
+struct renesas_usb3_priv {
+	int ramsize_per_ramif;		/* unit = bytes */
+	int num_ramif;
+	int ramsize_per_pipe;		/* unit = bytes */
+	bool workaround_for_vbus;	/* if true, don't check vbus signal */
+};
+
+struct renesas_usb3 {
+	void __iomem *reg;
+
+	struct usb_gadget gadget;
+	struct usb_gadget_driver *driver;
+
+	struct renesas_usb3_ep *usb3_ep;
+	int num_usb3_eps;
+
+	spinlock_t lock;
+	int disabled_count;
+
+	struct usb_request *ep0_req;
+	u16 test_mode;
+	u8 ep0_buf[USB3_EP0_BUF_SIZE];
+	bool softconnect;
+	bool workaround_for_vbus;
+};
+
+#define gadget_to_renesas_usb3(_gadget)	\
+		container_of(_gadget, struct renesas_usb3, gadget)
+#define renesas_usb3_to_gadget(renesas_usb3) (&renesas_usb3->gadget)
+#define usb3_to_dev(_usb3)	(_usb3->gadget.dev.parent)
+
+#define usb_ep_to_usb3_ep(_ep) container_of(_ep, struct renesas_usb3_ep, ep)
+#define usb3_ep_to_usb3(_usb3_ep) (_usb3_ep->usb3)
+#define usb_req_to_usb3_req(_req) container_of(_req, \
+					    struct renesas_usb3_request, req)
+
+#define usb3_get_ep(usb3, n) ((usb3)->usb3_ep + (n))
+#define usb3_for_each_ep(usb3_ep, usb3, i)			\
+		for ((i) = 0, usb3_ep = usb3_get_ep(usb3, (i));	\
+		     (i) < (usb3)->num_usb3_eps;		\
+		     (i)++, usb3_ep = usb3_get_ep(usb3, (i)))
+
+static const char udc_name[] = "renesas_usb3";
+
+static void usb3_write(struct renesas_usb3 *usb3, u32 data, u32 offs)
+{
+	iowrite32(data, usb3->reg + offs);
+}
+
+static u32 usb3_read(struct renesas_usb3 *usb3, u32 offs)
+{
+	return ioread32(usb3->reg + offs);
+}
+
+static void usb3_set_bit(struct renesas_usb3 *usb3, u32 bits, u32 offs)
+{
+	u32 val = usb3_read(usb3, offs);
+
+	val |= bits;
+	usb3_write(usb3, val, offs);
+}
+
+static void usb3_clear_bit(struct renesas_usb3 *usb3, u32 bits, u32 offs)
+{
+	u32 val = usb3_read(usb3, offs);
+
+	val &= ~bits;
+	usb3_write(usb3, val, offs);
+}
+
+static int usb3_wait(struct renesas_usb3 *usb3, u32 reg, u32 mask,
+		     u32 expected)
+{
+	int i;
+
+	for (i = 0; i < USB3_WAIT_US; i++) {
+		if ((usb3_read(usb3, reg) & mask) == expected)
+			return 0;
+		udelay(1);
+	}
+
+	dev_dbg(usb3_to_dev(usb3), "%s: timed out (%8x, %08x, %08x)\n",
+		__func__, reg, mask, expected);
+
+	return -EBUSY;
+}
+
+static void usb3_enable_irq_1(struct renesas_usb3 *usb3, u32 bits)
+{
+	usb3_set_bit(usb3, bits, USB3_USB_INT_ENA_1);
+}
+
+static void usb3_disable_irq_1(struct renesas_usb3 *usb3, u32 bits)
+{
+	usb3_clear_bit(usb3, bits, USB3_USB_INT_ENA_1);
+}
+
+static void usb3_enable_pipe_irq(struct renesas_usb3 *usb3, int num)
+{
+	usb3_set_bit(usb3, USB_INT_2_PIPE(num), USB3_USB_INT_ENA_2);
+}
+
+static void usb3_disable_pipe_irq(struct renesas_usb3 *usb3, int num)
+{
+	usb3_clear_bit(usb3, USB_INT_2_PIPE(num), USB3_USB_INT_ENA_2);
+}
+
+static void usb3_init_axi_bridge(struct renesas_usb3 *usb3)
+{
+	/* Set AXI_INT */
+	usb3_write(usb3, ~0, USB3_DMA_INT_STA);
+	usb3_write(usb3, 0, USB3_DMA_INT_ENA);
+	usb3_set_bit(usb3, AXI_INT_DMAINT | AXI_INT_EPCINT, USB3_AXI_INT_ENA);
+}
+
+static void usb3_init_epc_registers(struct renesas_usb3 *usb3)
+{
+	/* FIXME: How to change host / peripheral mode as well? */
+	usb3_set_bit(usb3, DRD_CON_PERI_CON, USB3_DRD_CON);
+
+	usb3_write(usb3, ~0, USB3_USB_INT_STA_1);
+	usb3_enable_irq_1(usb3, USB_INT_1_VBUS_CNG);
+}
+
+static bool usb3_wakeup_usb2_phy(struct renesas_usb3 *usb3)
+{
+	if (!(usb3_read(usb3, USB3_USB20_CON) & USB20_CON_B2_SUSPEND))
+		return true;	/* already waked it up */
+
+	usb3_clear_bit(usb3, USB20_CON_B2_SUSPEND, USB3_USB20_CON);
+	usb3_enable_irq_1(usb3, USB_INT_1_B2_RSUM);
+
+	return false;
+}
+
+static void usb3_usb2_pullup(struct renesas_usb3 *usb3, int pullup)
+{
+	u32 bits = USB20_CON_B2_PUE | USB20_CON_B2_CONNECT;
+
+	if (usb3->softconnect && pullup)
+		usb3_set_bit(usb3, bits, USB3_USB20_CON);
+	else
+		usb3_clear_bit(usb3, bits, USB3_USB20_CON);
+}
+
+static void usb3_set_test_mode(struct renesas_usb3 *usb3)
+{
+	u32 val = usb3_read(usb3, USB3_USB20_CON);
+
+	val &= ~USB20_CON_B2_TSTMOD_MASK;
+	val |= USB20_CON_B2_TSTMOD(usb3->test_mode);
+	usb3_write(usb3, val | USB20_CON_B2_TSTMOD_EN, USB3_USB20_CON);
+	if (!usb3->test_mode)
+		usb3_clear_bit(usb3, USB20_CON_B2_TSTMOD_EN, USB3_USB20_CON);
+}
+
+static void usb3_start_usb2_connection(struct renesas_usb3 *usb3)
+{
+	usb3->disabled_count++;
+	usb3_set_bit(usb3, USB_COM_CON_EP0_EN, USB3_USB_COM_CON);
+	usb3_set_bit(usb3, USB_COM_CON_SPD_MODE, USB3_USB_COM_CON);
+	usb3_usb2_pullup(usb3, 1);
+}
+
+static int usb3_is_usb3_phy_in_u3(struct renesas_usb3 *usb3)
+{
+	return usb3_read(usb3, USB3_USB30_CON) & USB30_CON_POW_SEL_IN_U3;
+}
+
+static bool usb3_wakeup_usb3_phy(struct renesas_usb3 *usb3)
+{
+	if (!usb3_is_usb3_phy_in_u3(usb3))
+		return true;	/* already waked it up */
+
+	usb3_set_bit(usb3, USB30_CON_B3_PLLWAKE, USB3_USB30_CON);
+	usb3_enable_irq_1(usb3, USB_INT_1_B3_PLLWKUP);
+
+	return false;
+}
+
+static u16 usb3_feature_get_un_enabled(struct renesas_usb3 *usb3)
+{
+	u32 mask_u2 = SSIFCMD_UDIR_U2 | SSIFCMD_UREQ_U2;
+	u32 mask_u1 = SSIFCMD_UDIR_U1 | SSIFCMD_UREQ_U1;
+	u32 val = usb3_read(usb3, USB3_SSIFCMD);
+	u16 ret = 0;
+
+	/* Enables {U2,U1} if the bits of UDIR and UREQ are set to 0 */
+	if (!(val & mask_u2))
+		ret |= 1 << USB_DEV_STAT_U2_ENABLED;
+	if (!(val & mask_u1))
+		ret |= 1 << USB_DEV_STAT_U1_ENABLED;
+
+	return ret;
+}
+
+static void usb3_feature_u2_enable(struct renesas_usb3 *usb3, bool enable)
+{
+	u32 bits = SSIFCMD_UDIR_U2 | SSIFCMD_UREQ_U2;
+
+	/* Enables U2 if the bits of UDIR and UREQ are set to 0 */
+	if (enable)
+		usb3_clear_bit(usb3, bits, USB3_SSIFCMD);
+	else
+		usb3_set_bit(usb3, bits, USB3_SSIFCMD);
+}
+
+static void usb3_feature_u1_enable(struct renesas_usb3 *usb3, bool enable)
+{
+	u32 bits = SSIFCMD_UDIR_U1 | SSIFCMD_UREQ_U1;
+
+	/* Enables U1 if the bits of UDIR and UREQ are set to 0 */
+	if (enable)
+		usb3_clear_bit(usb3, bits, USB3_SSIFCMD);
+	else
+		usb3_set_bit(usb3, bits, USB3_SSIFCMD);
+}
+
+static void usb3_start_operation_for_usb3(struct renesas_usb3 *usb3)
+{
+	usb3_set_bit(usb3, USB_COM_CON_EP0_EN, USB3_USB_COM_CON);
+	usb3_clear_bit(usb3, USB_COM_CON_SPD_MODE, USB3_USB_COM_CON);
+	usb3_set_bit(usb3, USB30_CON_B3_CONNECT, USB3_USB30_CON);
+}
+
+static void usb3_start_usb3_connection(struct renesas_usb3 *usb3)
+{
+	usb3_start_operation_for_usb3(usb3);
+	usb3_set_bit(usb3, USB_COM_CON_RX_DETECTION, USB3_USB_COM_CON);
+
+	usb3_enable_irq_1(usb3, USB_INT_1_B3_LUPSUCS | USB_INT_1_B3_DISABLE |
+			  USB_INT_1_SPEED);
+}
+
+static void usb3_stop_usb3_connection(struct renesas_usb3 *usb3)
+{
+	usb3_clear_bit(usb3, USB30_CON_B3_CONNECT, USB3_USB30_CON);
+}
+
+static void usb3_transition_to_default_state(struct renesas_usb3 *usb3,
+					     bool is_usb3)
+{
+	usb3_set_bit(usb3, USB_INT_2_PIPE(0), USB3_USB_INT_ENA_2);
+	usb3_write(usb3, P0_INT_ALL_BITS, USB3_P0_INT_STA);
+	usb3_set_bit(usb3, P0_INT_ALL_BITS, USB3_P0_INT_ENA);
+
+	if (is_usb3)
+		usb3_enable_irq_1(usb3, USB_INT_1_B3_WRMRST |
+				  USB_INT_1_B3_HOTRST);
+	else
+		usb3_enable_irq_1(usb3, USB_INT_1_B2_SPND |
+				  USB_INT_1_B2_L1SPND | USB_INT_1_B2_USBRST);
+}
+
+static void usb3_connect(struct renesas_usb3 *usb3)
+{
+	if (usb3_wakeup_usb3_phy(usb3))
+		usb3_start_usb3_connection(usb3);
+}
+
+static void usb3_reset_epc(struct renesas_usb3 *usb3)
+{
+	usb3_clear_bit(usb3, USB_COM_CON_CONF, USB3_USB_COM_CON);
+	usb3_clear_bit(usb3, USB_COM_CON_EP0_EN, USB3_USB_COM_CON);
+	usb3_set_bit(usb3, USB_COM_CON_PIPE_CLR, USB3_USB_COM_CON);
+	usb3->test_mode = 0;
+	usb3_set_test_mode(usb3);
+}
+
+static void usb3_disconnect(struct renesas_usb3 *usb3)
+{
+	usb3->disabled_count = 0;
+	usb3_usb2_pullup(usb3, 0);
+	usb3_clear_bit(usb3, USB30_CON_B3_CONNECT, USB3_USB30_CON);
+	usb3_reset_epc(usb3);
+
+	if (usb3->driver)
+		usb3->driver->disconnect(&usb3->gadget);
+}
+
+static void usb3_check_vbus(struct renesas_usb3 *usb3)
+{
+	if (usb3->workaround_for_vbus) {
+		usb3_connect(usb3);
+	} else {
+		if (usb3_read(usb3, USB3_USB_STA) & USB_STA_VBUS_STA)
+			usb3_connect(usb3);
+		else
+			usb3_disconnect(usb3);
+	}
+}
+
+static void renesas_usb3_init_controller(struct renesas_usb3 *usb3)
+{
+	usb3_init_axi_bridge(usb3);
+	usb3_init_epc_registers(usb3);
+
+	usb3_check_vbus(usb3);
+}
+
+static void renesas_usb3_stop_controller(struct renesas_usb3 *usb3)
+{
+	usb3_disconnect(usb3);
+	usb3_write(usb3, 0, USB3_P0_INT_ENA);
+	usb3_write(usb3, 0, USB3_PN_INT_ENA);
+	usb3_write(usb3, 0, USB3_USB_INT_ENA_1);
+	usb3_write(usb3, 0, USB3_USB_INT_ENA_2);
+	usb3_write(usb3, 0, USB3_AXI_INT_ENA);
+}
+
+static void usb3_irq_epc_int_1_pll_wakeup(struct renesas_usb3 *usb3)
+{
+	usb3_disable_irq_1(usb3, USB_INT_1_B3_PLLWKUP);
+	usb3_clear_bit(usb3, USB30_CON_B3_PLLWAKE, USB3_USB30_CON);
+	usb3_start_usb3_connection(usb3);
+}
+
+static void usb3_irq_epc_int_1_linkup_success(struct renesas_usb3 *usb3)
+{
+	usb3_transition_to_default_state(usb3, true);
+}
+
+static void usb3_irq_epc_int_1_resume(struct renesas_usb3 *usb3)
+{
+	usb3_disable_irq_1(usb3, USB_INT_1_B2_RSUM);
+	usb3_start_usb2_connection(usb3);
+	usb3_transition_to_default_state(usb3, false);
+}
+
+static void usb3_irq_epc_int_1_disable(struct renesas_usb3 *usb3)
+{
+	usb3_stop_usb3_connection(usb3);
+	if (usb3_wakeup_usb2_phy(usb3))
+		usb3_irq_epc_int_1_resume(usb3);
+}
+
+static void usb3_irq_epc_int_1_bus_reset(struct renesas_usb3 *usb3)
+{
+	usb3_reset_epc(usb3);
+	if (usb3->disabled_count < 3)
+		usb3_start_usb3_connection(usb3);
+	else
+		usb3_start_usb2_connection(usb3);
+}
+
+static void usb3_irq_epc_int_1_vbus_change(struct renesas_usb3 *usb3)
+{
+	usb3_check_vbus(usb3);
+}
+
+static void usb3_irq_epc_int_1_hot_reset(struct renesas_usb3 *usb3)
+{
+	usb3_reset_epc(usb3);
+	usb3_set_bit(usb3, USB_COM_CON_EP0_EN, USB3_USB_COM_CON);
+
+	/* This bit shall be set within 12ms from the start of HotReset */
+	usb3_set_bit(usb3, USB30_CON_B3_HOTRST_CMP, USB3_USB30_CON);
+}
+
+static void usb3_irq_epc_int_1_warm_reset(struct renesas_usb3 *usb3)
+{
+	usb3_reset_epc(usb3);
+	usb3_set_bit(usb3, USB_COM_CON_EP0_EN, USB3_USB_COM_CON);
+
+	usb3_start_operation_for_usb3(usb3);
+	usb3_enable_irq_1(usb3, USB_INT_1_SPEED);
+}
+
+static void usb3_irq_epc_int_1_speed(struct renesas_usb3 *usb3)
+{
+	u32 speed = usb3_read(usb3, USB3_USB_STA) & USB_STA_SPEED_MASK;
+
+	switch (speed) {
+	case USB_STA_SPEED_SS:
+		usb3->gadget.speed = USB_SPEED_SUPER;
+		break;
+	case USB_STA_SPEED_HS:
+		usb3->gadget.speed = USB_SPEED_HIGH;
+		break;
+	case USB_STA_SPEED_FS:
+		usb3->gadget.speed = USB_SPEED_FULL;
+		break;
+	default:
+		usb3->gadget.speed = USB_SPEED_UNKNOWN;
+		break;
+	}
+}
+
+static void usb3_irq_epc_int_1(struct renesas_usb3 *usb3, u32 int_sta_1)
+{
+	if (int_sta_1 & USB_INT_1_B3_PLLWKUP)
+		usb3_irq_epc_int_1_pll_wakeup(usb3);
+
+	if (int_sta_1 & USB_INT_1_B3_LUPSUCS)
+		usb3_irq_epc_int_1_linkup_success(usb3);
+
+	if (int_sta_1 & USB_INT_1_B3_HOTRST)
+		usb3_irq_epc_int_1_hot_reset(usb3);
+
+	if (int_sta_1 & USB_INT_1_B3_WRMRST)
+		usb3_irq_epc_int_1_warm_reset(usb3);
+
+	if (int_sta_1 & USB_INT_1_B3_DISABLE)
+		usb3_irq_epc_int_1_disable(usb3);
+
+	if (int_sta_1 & USB_INT_1_B2_USBRST)
+		usb3_irq_epc_int_1_bus_reset(usb3);
+
+	if (int_sta_1 & USB_INT_1_B2_RSUM)
+		usb3_irq_epc_int_1_resume(usb3);
+
+	if (int_sta_1 & USB_INT_1_SPEED)
+		usb3_irq_epc_int_1_speed(usb3);
+
+	if (int_sta_1 & USB_INT_1_VBUS_CNG)
+		usb3_irq_epc_int_1_vbus_change(usb3);
+}
+
+static struct renesas_usb3_request *__usb3_get_request(struct renesas_usb3_ep
+						       *usb3_ep)
+{
+	return list_first_entry_or_null(&usb3_ep->queue,
+					struct renesas_usb3_request, queue);
+}
+
+static struct renesas_usb3_request *usb3_get_request(struct renesas_usb3_ep
+						     *usb3_ep)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	struct renesas_usb3_request *usb3_req;
+	unsigned long flags;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	usb3_req = __usb3_get_request(usb3_ep);
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	return usb3_req;
+}
+
+static void usb3_request_done(struct renesas_usb3_ep *usb3_ep,
+			      struct renesas_usb3_request *usb3_req, int status)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	dev_dbg(usb3_to_dev(usb3), "giveback: ep%2d, %u, %u, %d\n",
+		usb3_ep->num, usb3_req->req.length, usb3_req->req.actual,
+		status);
+	usb3_req->req.status = status;
+	spin_lock_irqsave(&usb3->lock, flags);
+	usb3_ep->started = false;
+	list_del_init(&usb3_req->queue);
+	spin_unlock_irqrestore(&usb3->lock, flags);
+	usb_gadget_giveback_request(&usb3_ep->ep, &usb3_req->req);
+}
+
+static void usb3_irq_epc_pipe0_status_end(struct renesas_usb3 *usb3)
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, 0);
+	struct renesas_usb3_request *usb3_req = usb3_get_request(usb3_ep);
+
+	if (usb3_req)
+		usb3_request_done(usb3_ep, usb3_req, 0);
+	if (usb3->test_mode)
+		usb3_set_test_mode(usb3);
+}
+
+static void usb3_get_setup_data(struct renesas_usb3 *usb3,
+				struct usb_ctrlrequest *ctrl)
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, 0);
+	u32 *data = (u32 *)ctrl;
+
+	*data++ = usb3_read(usb3, USB3_STUP_DAT_0);
+	*data = usb3_read(usb3, USB3_STUP_DAT_1);
+
+	/* update this driver's flag */
+	usb3_ep->dir_in = !!(ctrl->bRequestType & USB_DIR_IN);
+}
+
+static void usb3_set_p0_con_update_res(struct renesas_usb3 *usb3, u32 res)
+{
+	u32 val = usb3_read(usb3, USB3_P0_CON);
+
+	val &= ~(P0_CON_ST_RES_MASK | P0_CON_OT_RES_MASK | P0_CON_IN_RES_MASK);
+	val |= res | P0_CON_RES_WEN;
+	usb3_write(usb3, val, USB3_P0_CON);
+}
+
+static void usb3_set_p0_con_for_ctrl_read_data(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_FORCE_NRDY |
+				   P0_CON_OT_RES_FORCE_STALL |
+				   P0_CON_IN_RES_NORMAL);
+}
+
+static void usb3_set_p0_con_for_ctrl_read_status(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_NORMAL |
+				   P0_CON_OT_RES_FORCE_STALL |
+				   P0_CON_IN_RES_NORMAL);
+}
+
+static void usb3_set_p0_con_for_ctrl_write_data(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_FORCE_NRDY |
+				   P0_CON_OT_RES_NORMAL |
+				   P0_CON_IN_RES_FORCE_STALL);
+}
+
+static void usb3_set_p0_con_for_ctrl_write_status(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_NORMAL |
+				   P0_CON_OT_RES_NORMAL |
+				   P0_CON_IN_RES_FORCE_STALL);
+}
+
+static void usb3_set_p0_con_for_no_data(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_NORMAL |
+				   P0_CON_OT_RES_FORCE_STALL |
+				   P0_CON_IN_RES_FORCE_STALL);
+}
+
+static void usb3_set_p0_con_stall(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_FORCE_STALL |
+				   P0_CON_OT_RES_FORCE_STALL |
+				   P0_CON_IN_RES_FORCE_STALL);
+}
+
+static void usb3_set_p0_con_stop(struct renesas_usb3 *usb3)
+{
+	usb3_set_p0_con_update_res(usb3, P0_CON_ST_RES_FORCE_NRDY |
+				   P0_CON_OT_RES_FORCE_NRDY |
+				   P0_CON_IN_RES_FORCE_NRDY);
+}
+
+static int usb3_pn_change(struct renesas_usb3 *usb3, int num)
+{
+	if (num == 0 || num > usb3->num_usb3_eps)
+		return -ENXIO;
+
+	usb3_write(usb3, num, USB3_PIPE_COM);
+
+	return 0;
+}
+
+static void usb3_set_pn_con_update_res(struct renesas_usb3 *usb3, u32 res)
+{
+	u32 val = usb3_read(usb3, USB3_PN_CON);
+
+	val &= ~PN_CON_RES_MASK;
+	val |= res & PN_CON_RES_MASK;
+	val |= PN_CON_RES_WEN;
+	usb3_write(usb3, val, USB3_PN_CON);
+}
+
+static void usb3_pn_start(struct renesas_usb3 *usb3)
+{
+	usb3_set_pn_con_update_res(usb3, PN_CON_RES_NORMAL);
+}
+
+static void usb3_pn_stop(struct renesas_usb3 *usb3)
+{
+	usb3_set_pn_con_update_res(usb3, PN_CON_RES_FORCE_NRDY);
+}
+
+static void usb3_pn_stall(struct renesas_usb3 *usb3)
+{
+	usb3_set_pn_con_update_res(usb3, PN_CON_RES_FORCE_STALL);
+}
+
+static int usb3_pn_con_clear(struct renesas_usb3 *usb3)
+{
+	usb3_set_bit(usb3, PN_CON_CLR, USB3_PN_CON);
+
+	return usb3_wait(usb3, USB3_PN_CON, PN_CON_CLR, 0);
+}
+
+static bool usb3_is_transfer_complete(struct renesas_usb3_ep *usb3_ep,
+				      struct renesas_usb3_request *usb3_req)
+{
+	struct usb_request *req = &usb3_req->req;
+
+	if ((!req->zero && req->actual == req->length) ||
+	    (req->actual % usb3_ep->ep.maxpacket) || (req->length == 0))
+		return true;
+	else
+		return false;
+}
+
+static int usb3_wait_pipe_status(struct renesas_usb3_ep *usb3_ep, u32 mask)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	u32 sta_reg = usb3_ep->num ? USB3_PN_STA : USB3_P0_STA;
+
+	return usb3_wait(usb3, sta_reg, mask, mask);
+}
+
+static void usb3_set_px_con_send(struct renesas_usb3_ep *usb3_ep, int bytes,
+				 bool last)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	u32 con_reg = usb3_ep->num ? USB3_PN_CON : USB3_P0_CON;
+	u32 val = usb3_read(usb3, con_reg);
+
+	val |= PX_CON_SEND | PX_CON_BYTE_EN_BYTES(bytes);
+	val |= (usb3_ep->num && last) ? PN_CON_LAST : 0;
+	usb3_write(usb3, val, con_reg);
+}
+
+static int usb3_write_pipe(struct renesas_usb3_ep *usb3_ep,
+			   struct renesas_usb3_request *usb3_req,
+			   u32 fifo_reg)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	int i;
+	int len = min_t(unsigned, usb3_req->req.length - usb3_req->req.actual,
+			usb3_ep->ep.maxpacket);
+	u8 *buf = usb3_req->req.buf + usb3_req->req.actual;
+	u32 tmp = 0;
+	bool is_last;
+
+	if (usb3_wait_pipe_status(usb3_ep, PX_STA_BUFSTS) < 0)
+		return -EBUSY;
+
+	/* Update gadget driver parameter */
+	usb3_req->req.actual += len;
+
+	/* Write data to the register */
+	if (len >= 4) {
+		iowrite32_rep(usb3->reg + fifo_reg, buf, len / 4);
+		buf += (len / 4) * 4;
+		len %= 4;	/* update len to use usb3_set_pX_con_send() */
+	}
+
+	if (len) {
+		for (i = 0; i < len; i++)
+			tmp |= buf[i] << (8 * i);
+		usb3_write(usb3, tmp, fifo_reg);
+	}
+
+	is_last = usb3_is_transfer_complete(usb3_ep, usb3_req);
+	/* Send the data */
+	usb3_set_px_con_send(usb3_ep, len, is_last);
+
+	return is_last ? 0 : -EAGAIN;
+}
+
+static u32 usb3_get_received_length(struct renesas_usb3_ep *usb3_ep)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	u32 lng_reg = usb3_ep->num ? USB3_PN_LNG : USB3_P0_LNG;
+
+	return usb3_read(usb3, lng_reg);
+}
+
+static int usb3_read_pipe(struct renesas_usb3_ep *usb3_ep,
+			  struct renesas_usb3_request *usb3_req, u32 fifo_reg)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	int i;
+	int len = min_t(unsigned, usb3_req->req.length - usb3_req->req.actual,
+			usb3_get_received_length(usb3_ep));
+	u8 *buf = usb3_req->req.buf + usb3_req->req.actual;
+	u32 tmp = 0;
+
+	if (!len)
+		return 0;
+
+	/* Update gadget driver parameter */
+	usb3_req->req.actual += len;
+
+	/* Read data from the register */
+	if (len >= 4) {
+		ioread32_rep(usb3->reg + fifo_reg, buf, len / 4);
+		buf += (len / 4) * 4;
+		len %= 4;
+	}
+
+	if (len) {
+		tmp = usb3_read(usb3, fifo_reg);
+		for (i = 0; i < len; i++)
+			buf[i] = (tmp >> (8 * i)) & 0xff;
+	}
+
+	return usb3_is_transfer_complete(usb3_ep, usb3_req) ? 0 : -EAGAIN;
+}
+
+static void usb3_set_status_stage(struct renesas_usb3_ep *usb3_ep,
+				  struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+
+	if (usb3_ep->dir_in) {
+		usb3_set_p0_con_for_ctrl_read_status(usb3);
+	} else {
+		if (!usb3_req->req.length)
+			usb3_set_p0_con_for_no_data(usb3);
+		else
+			usb3_set_p0_con_for_ctrl_write_status(usb3);
+	}
+}
+
+static void usb3_p0_xfer(struct renesas_usb3_ep *usb3_ep,
+			 struct renesas_usb3_request *usb3_req)
+{
+	int ret = -EAGAIN;
+
+	if (usb3_ep->dir_in)
+		ret = usb3_write_pipe(usb3_ep, usb3_req, USB3_P0_WRITE);
+	else
+		ret = usb3_read_pipe(usb3_ep, usb3_req, USB3_P0_READ);
+
+	if (!ret)
+		usb3_set_status_stage(usb3_ep, usb3_req);
+}
+
+static void usb3_start_pipe0(struct renesas_usb3_ep *usb3_ep,
+			     struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+
+	if (usb3_ep->started)
+		return;
+
+	usb3_ep->started = true;
+
+	if (usb3_ep->dir_in) {
+		usb3_set_bit(usb3, P0_MOD_DIR, USB3_P0_MOD);
+		usb3_set_p0_con_for_ctrl_read_data(usb3);
+	} else {
+		usb3_clear_bit(usb3, P0_MOD_DIR, USB3_P0_MOD);
+		usb3_set_p0_con_for_ctrl_write_data(usb3);
+	}
+
+	usb3_p0_xfer(usb3_ep, usb3_req);
+}
+
+static void usb3_start_pipen(struct renesas_usb3_ep *usb3_ep,
+			     struct renesas_usb3_request *usb3_req)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	struct renesas_usb3_request *usb3_req_first = usb3_get_request(usb3_ep);
+	unsigned long flags;
+	int ret = -EAGAIN;
+	u32 enable_bits = 0;
+
+	if (usb3_ep->halt || usb3_ep->started)
+		return;
+	if (usb3_req != usb3_req_first)
+		return;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	if (usb3_pn_change(usb3, usb3_ep->num) < 0)
+		goto out;
+
+	usb3_ep->started = true;
+	usb3_pn_start(usb3);
+
+	if (usb3_ep->dir_in) {
+		ret = usb3_write_pipe(usb3_ep, usb3_req, USB3_PN_WRITE);
+		enable_bits |= PN_INT_LSTTR;
+	}
+
+	if (ret < 0)
+		enable_bits |= PN_INT_BFRDY;
+
+	if (enable_bits) {
+		usb3_set_bit(usb3, enable_bits, USB3_PN_INT_ENA);
+		usb3_enable_pipe_irq(usb3, usb3_ep->num);
+	}
+out:
+	spin_unlock_irqrestore(&usb3->lock, flags);
+}
+
+static int renesas_usb3_ep_queue(struct usb_ep *_ep, struct usb_request *_req,
+				 gfp_t gfp_flags)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+	struct renesas_usb3_request *usb3_req = usb_req_to_usb3_req(_req);
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	dev_dbg(usb3_to_dev(usb3), "ep_queue: ep%2d, %u\n", usb3_ep->num,
+		_req->length);
+
+	_req->status = -EINPROGRESS;
+	_req->actual = 0;
+	spin_lock_irqsave(&usb3->lock, flags);
+	list_add_tail(&usb3_req->queue, &usb3_ep->queue);
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	if (!usb3_ep->num)
+		usb3_start_pipe0(usb3_ep, usb3_req);
+	else
+		usb3_start_pipen(usb3_ep, usb3_req);
+
+	return 0;
+}
+
+static void usb3_set_device_address(struct renesas_usb3 *usb3, u16 addr)
+{
+	/* DEV_ADDR bit field is cleared by WarmReset, HotReset and BusReset */
+	usb3_set_bit(usb3, USB_COM_CON_DEV_ADDR(addr), USB3_USB_COM_CON);
+}
+
+static bool usb3_std_req_set_address(struct renesas_usb3 *usb3,
+				     struct usb_ctrlrequest *ctrl)
+{
+	if (ctrl->wValue >= 128)
+		return true;	/* stall */
+
+	usb3_set_device_address(usb3, ctrl->wValue);
+	usb3_set_p0_con_for_no_data(usb3);
+
+	return false;
+}
+
+static void usb3_pipe0_internal_xfer(struct renesas_usb3 *usb3,
+				     void *tx_data, size_t len,
+				     void (*complete)(struct usb_ep *ep,
+						      struct usb_request *req))
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, 0);
+
+	if (tx_data)
+		memcpy(usb3->ep0_buf, tx_data,
+		       min_t(size_t, len, USB3_EP0_BUF_SIZE));
+
+	usb3->ep0_req->buf = &usb3->ep0_buf;
+	usb3->ep0_req->length = len;
+	usb3->ep0_req->complete = complete;
+	renesas_usb3_ep_queue(&usb3_ep->ep, usb3->ep0_req, GFP_ATOMIC);
+}
+
+static void usb3_pipe0_get_status_completion(struct usb_ep *ep,
+					     struct usb_request *req)
+{
+}
+
+static bool usb3_std_req_get_status(struct renesas_usb3 *usb3,
+				    struct usb_ctrlrequest *ctrl)
+{
+	bool stall = false;
+	struct renesas_usb3_ep *usb3_ep;
+	int num;
+	u16 status = 0;
+
+	switch (ctrl->bRequestType & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		if (usb3->gadget.is_selfpowered)
+			status |= 1 << USB_DEVICE_SELF_POWERED;
+		if (usb3->gadget.speed == USB_SPEED_SUPER)
+			status |= usb3_feature_get_un_enabled(usb3);
+		break;
+	case USB_RECIP_INTERFACE:
+		break;
+	case USB_RECIP_ENDPOINT:
+		num = le16_to_cpu(ctrl->wIndex) & USB_ENDPOINT_NUMBER_MASK;
+		usb3_ep = usb3_get_ep(usb3, num);
+		if (usb3_ep->halt)
+			status |= 1 << USB_ENDPOINT_HALT;
+		break;
+	default:
+		stall = true;
+		break;
+	}
+
+	if (!stall) {
+		status = cpu_to_le16(status);
+		dev_dbg(usb3_to_dev(usb3), "get_status: req = %p\n",
+			usb_req_to_usb3_req(usb3->ep0_req));
+		usb3_pipe0_internal_xfer(usb3, &status, sizeof(status),
+					 usb3_pipe0_get_status_completion);
+	}
+
+	return stall;
+}
+
+static bool usb3_std_req_feature_device(struct renesas_usb3 *usb3,
+					struct usb_ctrlrequest *ctrl, bool set)
+{
+	bool stall = true;
+	u16 w_value = le16_to_cpu(ctrl->wValue);
+
+	switch (w_value) {
+	case USB_DEVICE_TEST_MODE:
+		if (!set)
+			break;
+		usb3->test_mode = le16_to_cpu(ctrl->wIndex) >> 8;
+		stall = false;
+		break;
+	case USB_DEVICE_U1_ENABLE:
+	case USB_DEVICE_U2_ENABLE:
+		if (usb3->gadget.speed != USB_SPEED_SUPER)
+			break;
+		if (w_value == USB_DEVICE_U1_ENABLE)
+			usb3_feature_u1_enable(usb3, set);
+		if (w_value == USB_DEVICE_U2_ENABLE)
+			usb3_feature_u2_enable(usb3, set);
+		stall = false;
+		break;
+	default:
+		break;
+	}
+
+	return stall;
+}
+
+static int usb3_set_halt_p0(struct renesas_usb3_ep *usb3_ep, bool halt)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+
+	if (unlikely(usb3_ep->num))
+		return -EINVAL;
+
+	usb3_ep->halt = halt;
+	if (halt)
+		usb3_set_p0_con_stall(usb3);
+	else
+		usb3_set_p0_con_stop(usb3);
+
+	return 0;
+}
+
+static int usb3_set_halt_pn(struct renesas_usb3_ep *usb3_ep, bool halt,
+			    bool is_clear_feature)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	if (!usb3_pn_change(usb3, usb3_ep->num)) {
+		usb3_ep->halt = halt;
+		if (halt) {
+			usb3_pn_stall(usb3);
+		} else if (!is_clear_feature || !usb3_ep->wedge) {
+			usb3_pn_con_clear(usb3);
+			usb3_set_bit(usb3, PN_CON_EN, USB3_PN_CON);
+			usb3_pn_stop(usb3);
+		}
+	}
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	return 0;
+}
+
+static int usb3_set_halt(struct renesas_usb3_ep *usb3_ep, bool halt,
+			 bool is_clear_feature)
+{
+	int ret = 0;
+
+	if (halt && usb3_ep->started)
+		return -EAGAIN;
+
+	if (usb3_ep->num)
+		ret = usb3_set_halt_pn(usb3_ep, halt, is_clear_feature);
+	else
+		ret = usb3_set_halt_p0(usb3_ep, halt);
+
+	return ret;
+}
+
+static bool usb3_std_req_feature_endpoint(struct renesas_usb3 *usb3,
+					  struct usb_ctrlrequest *ctrl,
+					  bool set)
+{
+	int num = le16_to_cpu(ctrl->wIndex) & USB_ENDPOINT_NUMBER_MASK;
+	struct renesas_usb3_ep *usb3_ep;
+	struct renesas_usb3_request *usb3_req;
+
+	if (le16_to_cpu(ctrl->wValue) != USB_ENDPOINT_HALT)
+		return true;	/* stall */
+
+	usb3_ep = usb3_get_ep(usb3, num);
+	usb3_set_halt(usb3_ep, set, true);
+
+	/* Restarts a queue if clear feature */
+	if (!set) {
+		usb3_ep->started = false;
+		usb3_req = usb3_get_request(usb3_ep);
+		if (usb3_req)
+			usb3_start_pipen(usb3_ep, usb3_req);
+	}
+
+	return false;
+}
+
+static bool usb3_std_req_feature(struct renesas_usb3 *usb3,
+				 struct usb_ctrlrequest *ctrl, bool set)
+{
+	bool stall = false;
+
+	switch (ctrl->bRequestType & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		stall = usb3_std_req_feature_device(usb3, ctrl, set);
+		break;
+	case USB_RECIP_INTERFACE:
+		break;
+	case USB_RECIP_ENDPOINT:
+		stall = usb3_std_req_feature_endpoint(usb3, ctrl, set);
+		break;
+	default:
+		stall = true;
+		break;
+	}
+
+	if (!stall)
+		usb3_set_p0_con_for_no_data(usb3);
+
+	return stall;
+}
+
+static void usb3_pipe0_set_sel_completion(struct usb_ep *ep,
+					  struct usb_request *req)
+{
+	/* TODO */
+}
+
+static bool usb3_std_req_set_sel(struct renesas_usb3 *usb3,
+				 struct usb_ctrlrequest *ctrl)
+{
+	u16 w_length = le16_to_cpu(ctrl->wLength);
+
+	if (w_length != 6)
+		return true;	/* stall */
+
+	dev_dbg(usb3_to_dev(usb3), "set_sel: req = %p\n",
+		usb_req_to_usb3_req(usb3->ep0_req));
+	usb3_pipe0_internal_xfer(usb3, NULL, 6, usb3_pipe0_set_sel_completion);
+
+	return false;
+}
+
+static bool usb3_std_req_set_configuration(struct renesas_usb3 *usb3,
+					   struct usb_ctrlrequest *ctrl)
+{
+	if (ctrl->wValue > 0)
+		usb3_set_bit(usb3, USB_COM_CON_CONF, USB3_USB_COM_CON);
+	else
+		usb3_clear_bit(usb3, USB_COM_CON_CONF, USB3_USB_COM_CON);
+
+	return false;
+}
+
+/**
+ * usb3_handle_standard_request - handle some standard requests
+ * @usb3: the renesas_usb3 pointer
+ * @ctrl: a pointer of setup data
+ *
+ * Returns true if this function handled a standard request
+ */
+static bool usb3_handle_standard_request(struct renesas_usb3 *usb3,
+					 struct usb_ctrlrequest *ctrl)
+{
+	bool ret = false;
+	bool stall = false;
+
+	if ((ctrl->bRequestType & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+		switch (ctrl->bRequest) {
+		case USB_REQ_SET_ADDRESS:
+			stall = usb3_std_req_set_address(usb3, ctrl);
+			ret = true;
+			break;
+		case USB_REQ_GET_STATUS:
+			stall = usb3_std_req_get_status(usb3, ctrl);
+			ret = true;
+			break;
+		case USB_REQ_CLEAR_FEATURE:
+			stall = usb3_std_req_feature(usb3, ctrl, false);
+			ret = true;
+			break;
+		case USB_REQ_SET_FEATURE:
+			stall = usb3_std_req_feature(usb3, ctrl, true);
+			ret = true;
+			break;
+		case USB_REQ_SET_SEL:
+			stall = usb3_std_req_set_sel(usb3, ctrl);
+			ret = true;
+			break;
+		case USB_REQ_SET_ISOCH_DELAY:
+			/* This hardware doesn't support Isochronous xfer */
+			stall = true;
+			ret = true;
+			break;
+		case USB_REQ_SET_CONFIGURATION:
+			usb3_std_req_set_configuration(usb3, ctrl);
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (stall)
+		usb3_set_p0_con_stall(usb3);
+
+	return ret;
+}
+
+static int usb3_p0_con_clear_buffer(struct renesas_usb3 *usb3)
+{
+	usb3_set_bit(usb3, P0_CON_BCLR, USB3_P0_CON);
+
+	return usb3_wait(usb3, USB3_P0_CON, P0_CON_BCLR, 0);
+}
+
+static void usb3_irq_epc_pipe0_setup(struct renesas_usb3 *usb3)
+{
+	struct usb_ctrlrequest ctrl;
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, 0);
+
+	/* Call giveback function if previous transfer is not completed */
+	if (usb3_ep->started)
+		usb3_request_done(usb3_ep, usb3_get_request(usb3_ep),
+				  -ECONNRESET);
+
+	usb3_p0_con_clear_buffer(usb3);
+	usb3_get_setup_data(usb3, &ctrl);
+	if (!usb3_handle_standard_request(usb3, &ctrl))
+		if (usb3->driver->setup(&usb3->gadget, &ctrl) < 0)
+			usb3_set_p0_con_stall(usb3);
+}
+
+static void usb3_irq_epc_pipe0_bfrdy(struct renesas_usb3 *usb3)
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, 0);
+	struct renesas_usb3_request *usb3_req = usb3_get_request(usb3_ep);
+
+	if (!usb3_req)
+		return;
+
+	usb3_p0_xfer(usb3_ep, usb3_req);
+}
+
+static void usb3_irq_epc_pipe0(struct renesas_usb3 *usb3)
+{
+	u32 p0_int_sta = usb3_read(usb3, USB3_P0_INT_STA);
+
+	p0_int_sta &= usb3_read(usb3, USB3_P0_INT_ENA);
+	usb3_write(usb3, p0_int_sta, USB3_P0_INT_STA);
+	if (p0_int_sta & P0_INT_STSED)
+		usb3_irq_epc_pipe0_status_end(usb3);
+	if (p0_int_sta & P0_INT_SETUP)
+		usb3_irq_epc_pipe0_setup(usb3);
+	if (p0_int_sta & P0_INT_BFRDY)
+		usb3_irq_epc_pipe0_bfrdy(usb3);
+}
+
+static void usb3_request_done_pipen(struct renesas_usb3 *usb3,
+				    struct renesas_usb3_ep *usb3_ep,
+				    struct renesas_usb3_request *usb3_req,
+				    int status)
+{
+	usb3_pn_stop(usb3);
+	usb3_disable_pipe_irq(usb3, usb3_ep->num);
+	usb3_request_done(usb3_ep, usb3_req, status);
+
+	/* get next usb3_req */
+	usb3_req = usb3_get_request(usb3_ep);
+	if (usb3_req)
+		usb3_start_pipen(usb3_ep, usb3_req);
+}
+
+static void usb3_irq_epc_pipen_lsttr(struct renesas_usb3 *usb3, int num)
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, num);
+	struct renesas_usb3_request *usb3_req = usb3_get_request(usb3_ep);
+
+	if (!usb3_req)
+		return;
+
+	if (usb3_ep->dir_in) {
+		dev_dbg(usb3_to_dev(usb3), "%s: len = %u, actual = %u\n",
+			__func__, usb3_req->req.length, usb3_req->req.actual);
+		usb3_request_done_pipen(usb3, usb3_ep, usb3_req, 0);
+	}
+}
+
+static void usb3_irq_epc_pipen_bfrdy(struct renesas_usb3 *usb3, int num)
+{
+	struct renesas_usb3_ep *usb3_ep = usb3_get_ep(usb3, num);
+	struct renesas_usb3_request *usb3_req = usb3_get_request(usb3_ep);
+
+	if (!usb3_req)
+		return;
+
+	if (usb3_ep->dir_in) {
+		/* Do not stop the IN pipe here to detect LSTTR interrupt */
+		if (!usb3_write_pipe(usb3_ep, usb3_req, USB3_PN_WRITE))
+			usb3_clear_bit(usb3, PN_INT_BFRDY, USB3_PN_INT_ENA);
+	} else {
+		if (!usb3_read_pipe(usb3_ep, usb3_req, USB3_PN_READ))
+			usb3_request_done_pipen(usb3, usb3_ep, usb3_req, 0);
+	}
+}
+
+static void usb3_irq_epc_pipen(struct renesas_usb3 *usb3, int num)
+{
+	u32 pn_int_sta;
+
+	if (usb3_pn_change(usb3, num) < 0)
+		return;
+
+	pn_int_sta = usb3_read(usb3, USB3_PN_INT_STA);
+	pn_int_sta &= usb3_read(usb3, USB3_PN_INT_ENA);
+	usb3_write(usb3, pn_int_sta, USB3_PN_INT_STA);
+	if (pn_int_sta & PN_INT_LSTTR)
+		usb3_irq_epc_pipen_lsttr(usb3, num);
+	if (pn_int_sta & PN_INT_BFRDY)
+		usb3_irq_epc_pipen_bfrdy(usb3, num);
+}
+
+static void usb3_irq_epc_int_2(struct renesas_usb3 *usb3, u32 int_sta_2)
+{
+	int i;
+
+	for (i = 0; i < usb3->num_usb3_eps; i++) {
+		if (int_sta_2 & USB_INT_2_PIPE(i)) {
+			if (!i)
+				usb3_irq_epc_pipe0(usb3);
+			else
+				usb3_irq_epc_pipen(usb3, i);
+		}
+	}
+}
+
+static void usb3_irq_epc(struct renesas_usb3 *usb3)
+{
+	u32 int_sta_1 = usb3_read(usb3, USB3_USB_INT_STA_1);
+	u32 int_sta_2 = usb3_read(usb3, USB3_USB_INT_STA_2);
+
+	int_sta_1 &= usb3_read(usb3, USB3_USB_INT_ENA_1);
+	if (int_sta_1) {
+		usb3_write(usb3, int_sta_1, USB3_USB_INT_STA_1);
+		usb3_irq_epc_int_1(usb3, int_sta_1);
+	}
+
+	int_sta_2 &= usb3_read(usb3, USB3_USB_INT_ENA_2);
+	if (int_sta_2)
+		usb3_irq_epc_int_2(usb3, int_sta_2);
+}
+
+static irqreturn_t renesas_usb3_irq(int irq, void *_usb3)
+{
+	struct renesas_usb3 *usb3 = _usb3;
+	irqreturn_t ret = IRQ_NONE;
+	u32 axi_int_sta = usb3_read(usb3, USB3_AXI_INT_STA);
+
+	if (axi_int_sta & AXI_INT_EPCINT) {
+		usb3_irq_epc(usb3);
+		ret = IRQ_HANDLED;
+	}
+
+	return ret;
+}
+
+static void usb3_write_pn_mod(struct renesas_usb3_ep *usb3_ep,
+			      const struct usb_endpoint_descriptor *desc)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	u32 val = 0;
+
+	val |= usb3_ep->dir_in ? PN_MOD_DIR : 0;
+	val |= PN_MOD_TYPE(usb_endpoint_type(desc));
+	val |= PN_MOD_EPNUM(usb_endpoint_num(desc));
+	usb3_write(usb3, val, USB3_PN_MOD);
+}
+
+static u32 usb3_calc_ramarea(int ram_size)
+{
+	WARN_ON(ram_size > SZ_16K);
+
+	if (ram_size <= SZ_1K)
+		return PN_RAMMAP_RAMAREA_1KB;
+	else if (ram_size <= SZ_2K)
+		return PN_RAMMAP_RAMAREA_2KB;
+	else if (ram_size <= SZ_4K)
+		return PN_RAMMAP_RAMAREA_4KB;
+	else if (ram_size <= SZ_8K)
+		return PN_RAMMAP_RAMAREA_8KB;
+	else
+		return PN_RAMMAP_RAMAREA_16KB;
+}
+
+static u32 usb3_calc_rammap_val(struct renesas_usb3_ep *usb3_ep,
+				const struct usb_endpoint_descriptor *desc)
+{
+	return usb3_ep->rammap_val | PN_RAMMAP_MPKT(usb_endpoint_maxp(desc));
+}
+
+static int usb3_enable_pipe_n(struct renesas_usb3_ep *usb3_ep,
+			      const struct usb_endpoint_descriptor *desc)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	usb3_ep->dir_in = usb_endpoint_dir_in(desc);
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	if (!usb3_pn_change(usb3, usb3_ep->num)) {
+		usb3_write_pn_mod(usb3_ep, desc);
+		usb3_write(usb3, usb3_calc_rammap_val(usb3_ep, desc),
+			   USB3_PN_RAMMAP);
+		usb3_pn_con_clear(usb3);
+		usb3_set_bit(usb3, PN_CON_EN, USB3_PN_CON);
+	}
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	return 0;
+}
+
+static int usb3_disable_pipe_n(struct renesas_usb3_ep *usb3_ep)
+{
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	usb3_ep->halt = false;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	if (!usb3_pn_change(usb3, usb3_ep->num)) {
+		usb3_write(usb3, 0, USB3_PN_RAMMAP);
+		usb3_clear_bit(usb3, PN_CON_EN, USB3_PN_CON);
+	}
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	return 0;
+}
+
+/*------- usb_ep_ops -----------------------------------------------------*/
+static int renesas_usb3_ep_enable(struct usb_ep *_ep,
+				  const struct usb_endpoint_descriptor *desc)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+
+	return usb3_enable_pipe_n(usb3_ep, desc);
+}
+
+static int renesas_usb3_ep_disable(struct usb_ep *_ep)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+	struct renesas_usb3_request *usb3_req;
+
+	do {
+		usb3_req = usb3_get_request(usb3_ep);
+		if (!usb3_req)
+			break;
+		usb3_request_done(usb3_ep, usb3_req, -ESHUTDOWN);
+	} while (1);
+
+	return usb3_disable_pipe_n(usb3_ep);
+}
+
+static struct usb_request *__renesas_usb3_ep_alloc_request(gfp_t gfp_flags)
+{
+	struct renesas_usb3_request *usb3_req;
+
+	usb3_req = kzalloc(sizeof(struct renesas_usb3_request), gfp_flags);
+	if (!usb3_req)
+		return NULL;
+
+	INIT_LIST_HEAD(&usb3_req->queue);
+
+	return &usb3_req->req;
+}
+
+static void __renesas_usb3_ep_free_request(struct usb_request *_req)
+{
+	struct renesas_usb3_request *usb3_req = usb_req_to_usb3_req(_req);
+
+	kfree(usb3_req);
+}
+
+static struct usb_request *renesas_usb3_ep_alloc_request(struct usb_ep *_ep,
+							 gfp_t gfp_flags)
+{
+	return __renesas_usb3_ep_alloc_request(gfp_flags);
+}
+
+static void renesas_usb3_ep_free_request(struct usb_ep *_ep,
+					 struct usb_request *_req)
+{
+	__renesas_usb3_ep_free_request(_req);
+}
+
+static int renesas_usb3_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+	struct renesas_usb3_request *usb3_req = usb_req_to_usb3_req(_req);
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+
+	dev_dbg(usb3_to_dev(usb3), "ep_dequeue: ep%2d, %u\n", usb3_ep->num,
+		_req->length);
+
+	usb3_request_done_pipen(usb3, usb3_ep, usb3_req, -ECONNRESET);
+
+	return 0;
+}
+
+static int renesas_usb3_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	return usb3_set_halt(usb_ep_to_usb3_ep(_ep), !!value, false);
+}
+
+static int renesas_usb3_ep_set_wedge(struct usb_ep *_ep)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+
+	usb3_ep->wedge = true;
+	return usb3_set_halt(usb3_ep, true, false);
+}
+
+static void renesas_usb3_ep_fifo_flush(struct usb_ep *_ep)
+{
+	struct renesas_usb3_ep *usb3_ep = usb_ep_to_usb3_ep(_ep);
+	struct renesas_usb3 *usb3 = usb3_ep_to_usb3(usb3_ep);
+	unsigned long flags;
+
+	if (usb3_ep->num) {
+		spin_lock_irqsave(&usb3->lock, flags);
+		if (!usb3_pn_change(usb3, usb3_ep->num)) {
+			usb3_pn_con_clear(usb3);
+			usb3_set_bit(usb3, PN_CON_EN, USB3_PN_CON);
+		}
+		spin_unlock_irqrestore(&usb3->lock, flags);
+	} else {
+		usb3_p0_con_clear_buffer(usb3);
+	}
+}
+
+static struct usb_ep_ops renesas_usb3_ep_ops = {
+	.enable		= renesas_usb3_ep_enable,
+	.disable	= renesas_usb3_ep_disable,
+
+	.alloc_request	= renesas_usb3_ep_alloc_request,
+	.free_request	= renesas_usb3_ep_free_request,
+
+	.queue		= renesas_usb3_ep_queue,
+	.dequeue	= renesas_usb3_ep_dequeue,
+
+	.set_halt	= renesas_usb3_ep_set_halt,
+	.set_wedge	= renesas_usb3_ep_set_wedge,
+	.fifo_flush	= renesas_usb3_ep_fifo_flush,
+};
+
+/*------- usb_gadget_ops -------------------------------------------------*/
+static int renesas_usb3_start(struct usb_gadget *gadget,
+			      struct usb_gadget_driver *driver)
+{
+	struct renesas_usb3 *usb3;
+
+	if (!driver || driver->max_speed < USB_SPEED_FULL ||
+	    !driver->setup)
+		return -EINVAL;
+
+	usb3 = gadget_to_renesas_usb3(gadget);
+
+	/* hook up the driver */
+	usb3->driver = driver;
+
+	renesas_usb3_init_controller(usb3);
+
+	return 0;
+}
+
+static int renesas_usb3_stop(struct usb_gadget *gadget)
+{
+	struct renesas_usb3 *usb3 = gadget_to_renesas_usb3(gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&usb3->lock, flags);
+	usb3->softconnect = false;
+	usb3->gadget.speed = USB_SPEED_UNKNOWN;
+	usb3->driver = NULL;
+	renesas_usb3_stop_controller(usb3);
+	spin_unlock_irqrestore(&usb3->lock, flags);
+
+	return 0;
+}
+
+static int renesas_usb3_get_frame(struct usb_gadget *_gadget)
+{
+	return -EOPNOTSUPP;
+}
+
+static int renesas_usb3_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct renesas_usb3 *usb3 = gadget_to_renesas_usb3(gadget);
+
+	usb3->softconnect = !!is_on;
+
+	return 0;
+}
+
+static int renesas_usb3_set_selfpowered(struct usb_gadget *gadget, int is_self)
+{
+	gadget->is_selfpowered = !!is_self;
+
+	return 0;
+}
+
+static const struct usb_gadget_ops renesas_usb3_gadget_ops = {
+	.get_frame		= renesas_usb3_get_frame,
+	.udc_start		= renesas_usb3_start,
+	.udc_stop		= renesas_usb3_stop,
+	.pullup			= renesas_usb3_pullup,
+	.set_selfpowered	= renesas_usb3_set_selfpowered,
+};
+
+/*------- platform_driver ------------------------------------------------*/
+static int renesas_usb3_remove(struct platform_device *pdev)
+{
+	struct renesas_usb3 *usb3 = platform_get_drvdata(pdev);
+
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	usb_del_gadget_udc(&usb3->gadget);
+
+	__renesas_usb3_ep_free_request(usb3->ep0_req);
+
+	return 0;
+}
+
+static int renesas_usb3_init_ep(struct renesas_usb3 *usb3, struct device *dev,
+				const struct renesas_usb3_priv *priv)
+{
+	struct renesas_usb3_ep *usb3_ep;
+	int i;
+
+	/* calculate num_usb3_eps from renesas_usb3_priv */
+	usb3->num_usb3_eps = priv->ramsize_per_ramif * priv->num_ramif * 2 /
+			     priv->ramsize_per_pipe + 1;
+
+	if (usb3->num_usb3_eps > USB3_MAX_NUM_PIPES)
+		usb3->num_usb3_eps = USB3_MAX_NUM_PIPES;
+
+	usb3->usb3_ep = devm_kzalloc(dev, sizeof(*usb3_ep) * usb3->num_usb3_eps,
+				     GFP_KERNEL);
+	if (!usb3->usb3_ep)
+		return -ENOMEM;
+
+	dev_dbg(dev, "%s: num_usb3_eps = %d\n", __func__, usb3->num_usb3_eps);
+	/*
+	 * This driver prepares pipes as the followings:
+	 *  - odd pipes = IN pipe
+	 *  - even pipes = OUT pipe (except pipe 0)
+	 */
+	usb3_for_each_ep(usb3_ep, usb3, i) {
+		snprintf(usb3_ep->ep_name, sizeof(usb3_ep->ep_name), "ep%d", i);
+		usb3_ep->usb3 = usb3;
+		usb3_ep->num = i;
+		usb3_ep->ep.name = usb3_ep->ep_name;
+		usb3_ep->ep.ops = &renesas_usb3_ep_ops;
+		INIT_LIST_HEAD(&usb3_ep->queue);
+		INIT_LIST_HEAD(&usb3_ep->ep.ep_list);
+		if (!i) {
+			/* for control pipe */
+			usb3->gadget.ep0 = &usb3_ep->ep;
+			usb_ep_set_maxpacket_limit(&usb3_ep->ep,
+						USB3_EP0_HSFS_MAX_PACKET_SIZE);
+			usb3_ep->ep.caps.type_control = true;
+			usb3_ep->ep.caps.dir_in = true;
+			usb3_ep->ep.caps.dir_out = true;
+			continue;
+		}
+
+		/* for bulk or interrupt pipe */
+		usb_ep_set_maxpacket_limit(&usb3_ep->ep, ~0);
+		list_add_tail(&usb3_ep->ep.ep_list, &usb3->gadget.ep_list);
+		usb3_ep->ep.caps.type_bulk = true;
+		usb3_ep->ep.caps.type_int = true;
+		if (i & 1)
+			usb3_ep->ep.caps.dir_in = true;
+		else
+			usb3_ep->ep.caps.dir_out = true;
+	}
+
+	return 0;
+}
+
+static void renesas_usb3_init_ram(struct renesas_usb3 *usb3, struct device *dev,
+				  const struct renesas_usb3_priv *priv)
+{
+	struct renesas_usb3_ep *usb3_ep;
+	int i;
+	u32 ramif[2], basead[2];	/* index 0 = for IN pipes */
+	u32 *cur_ramif, *cur_basead;
+	u32 val;
+
+	memset(ramif, 0, sizeof(ramif));
+	memset(basead, 0, sizeof(basead));
+
+	/*
+	 * This driver prepares pipes as the followings:
+	 *  - all pipes = the same size as "ramsize_per_pipe"
+	 * Please refer to the "Method of Specifying RAM Mapping"
+	 */
+	usb3_for_each_ep(usb3_ep, usb3, i) {
+		if (!i)
+			continue;	/* out of scope if ep num = 0 */
+		if (usb3_ep->ep.caps.dir_in) {
+			cur_ramif = &ramif[0];
+			cur_basead = &basead[0];
+		} else {
+			cur_ramif = &ramif[1];
+			cur_basead = &basead[1];
+		}
+
+		if (*cur_basead > priv->ramsize_per_ramif)
+			continue;	/* out of memory for IN or OUT pipe */
+
+		/* calculate rammap_val */
+		val = PN_RAMMAP_RAMIF(*cur_ramif);
+		val |= usb3_calc_ramarea(priv->ramsize_per_pipe);
+		val |= PN_RAMMAP_BASEAD(*cur_basead);
+		usb3_ep->rammap_val = val;
+
+		dev_dbg(dev, "ep%2d: val = %08x, ramif = %d, base = %x\n",
+			i, val, *cur_ramif, *cur_basead);
+
+		/* update current ramif */
+		if (*cur_ramif + 1 == priv->num_ramif) {
+			*cur_ramif = 0;
+			*cur_basead += priv->ramsize_per_pipe;
+		} else {
+			(*cur_ramif)++;
+		}
+	}
+}
+
+static const struct renesas_usb3_priv renesas_usb3_priv_r8a7795 = {
+	.ramsize_per_ramif = SZ_16K,
+	.num_ramif = 2,
+	.ramsize_per_pipe = SZ_4K,
+	.workaround_for_vbus = true,
+};
+
+static const struct of_device_id usb3_of_match[] = {
+	{
+		.compatible = "renesas,r8a7795-usb3-peri",
+		.data = &renesas_usb3_priv_r8a7795,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, usb3_of_match);
+
+static int renesas_usb3_probe(struct platform_device *pdev)
+{
+	struct renesas_usb3 *usb3;
+	struct resource *res;
+	const struct of_device_id *match;
+	int irq, ret;
+	const struct renesas_usb3_priv *priv;
+
+	match = of_match_node(usb3_of_match, pdev->dev.of_node);
+	if (!match)
+		return -ENODEV;
+	priv = match->data;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return -ENODEV;
+
+	usb3 = devm_kzalloc(&pdev->dev, sizeof(*usb3), GFP_KERNEL);
+	if (!usb3)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	usb3->reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(usb3->reg))
+		return PTR_ERR(usb3->reg);
+
+	platform_set_drvdata(pdev, usb3);
+	spin_lock_init(&usb3->lock);
+
+	usb3->gadget.ops = &renesas_usb3_gadget_ops;
+	usb3->gadget.name = udc_name;
+	usb3->gadget.max_speed = USB_SPEED_SUPER;
+	INIT_LIST_HEAD(&usb3->gadget.ep_list);
+	ret = renesas_usb3_init_ep(usb3, &pdev->dev, priv);
+	if (ret < 0)
+		return ret;
+	renesas_usb3_init_ram(usb3, &pdev->dev, priv);
+
+	ret = devm_request_irq(&pdev->dev, irq, renesas_usb3_irq, 0,
+			       dev_name(&pdev->dev), usb3);
+	if (ret < 0)
+		return ret;
+
+	/* for ep0 handling */
+	usb3->ep0_req = __renesas_usb3_ep_alloc_request(GFP_KERNEL);
+	if (!usb3->ep0_req)
+		return -ENOMEM;
+
+	ret = usb_add_gadget_udc(&pdev->dev, &usb3->gadget);
+	if (ret < 0)
+		goto err_add_udc;
+
+	usb3->workaround_for_vbus = priv->workaround_for_vbus;
+
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
+	dev_info(&pdev->dev, "probed\n");
+
+	return 0;
+
+err_add_udc:
+	__renesas_usb3_ep_free_request(usb3->ep0_req);
+
+	return ret;
+}
+
+static struct platform_driver renesas_usb3_driver = {
+	.probe		= renesas_usb3_probe,
+	.remove		= renesas_usb3_remove,
+	.driver		= {
+		.name =	(char *)udc_name,
+		.of_match_table = of_match_ptr(usb3_of_match),
+	},
+};
+module_platform_driver(renesas_usb3_driver);
+
+MODULE_DESCRIPTION("Renesas USB3.0 Peripheral driver");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Yoshihiro Shimoda <yoshihiro.shimoda.uh@renesas.com>");
+MODULE_ALIAS("platform:renesas_usb3");
diff --git a/drivers/usb/gadget/udc/s3c-hsudc.c b/drivers/usb/gadget/udc/s3c-hsudc.c
index e9def42ce50d..82a9e2a3bedc 100644
--- a/drivers/usb/gadget/udc/s3c-hsudc.c
+++ b/drivers/usb/gadget/udc/s3c-hsudc.c
@@ -569,7 +569,7 @@ static int s3c_hsudc_handle_reqfeat(struct s3c_hsudc *hsudc,
 		hsep = &hsudc->ep[ep_num];
 		switch (le16_to_cpu(ctrl->wValue)) {
 		case USB_ENDPOINT_HALT:
-			if (set || (!set && !hsep->wedge))
+			if (set || !hsep->wedge)
 				s3c_hsudc_set_halt(&hsep->ep, set);
 			return 0;
 		}
diff --git a/drivers/usb/gadget/udc/udc-core.c b/drivers/usb/gadget/udc/udc-core.c
index f660afba715d..b86a6f03592e 100644
--- a/drivers/usb/gadget/udc/udc-core.c
+++ b/drivers/usb/gadget/udc/udc-core.c
@@ -51,8 +51,12 @@ struct usb_udc {
 
 static struct class *udc_class;
 static LIST_HEAD(udc_list);
+static LIST_HEAD(gadget_driver_pending_list);
 static DEFINE_MUTEX(udc_lock);
 
+static int udc_bind_to_driver(struct usb_udc *udc,
+		struct usb_gadget_driver *driver);
+
 /* ------------------------------------------------------------------------- */
 
 #ifdef	CONFIG_HAS_DMA
@@ -356,6 +360,7 @@ int usb_add_gadget_udc_release(struct device *parent, struct usb_gadget *gadget,
 		void (*release)(struct device *dev))
 {
 	struct usb_udc		*udc;
+	struct usb_gadget_driver *driver;
 	int			ret = -ENOMEM;
 
 	udc = kzalloc(sizeof(*udc), GFP_KERNEL);
@@ -403,6 +408,19 @@ int usb_add_gadget_udc_release(struct device *parent, struct usb_gadget *gadget,
 	usb_gadget_set_state(gadget, USB_STATE_NOTATTACHED);
 	udc->vbus = true;
 
+	/* pick up one of pending gadget drivers */
+	list_for_each_entry(driver, &gadget_driver_pending_list, pending) {
+		if (!driver->udc_name || strcmp(driver->udc_name,
+						dev_name(&udc->dev)) == 0) {
+			ret = udc_bind_to_driver(udc, driver);
+			if (ret != -EPROBE_DEFER)
+				list_del(&driver->pending);
+			if (ret)
+				goto err4;
+			break;
+		}
+	}
+
 	mutex_unlock(&udc_lock);
 
 	return 0;
@@ -473,10 +491,14 @@ void usb_del_gadget_udc(struct usb_gadget *gadget)
 
 	mutex_lock(&udc_lock);
 	list_del(&udc->list);
-	mutex_unlock(&udc_lock);
 
-	if (udc->driver)
+	if (udc->driver) {
+		struct usb_gadget_driver *driver = udc->driver;
+
 		usb_gadget_remove_driver(udc);
+		list_add(&driver->pending, &gadget_driver_pending_list);
+	}
+	mutex_unlock(&udc_lock);
 
 	kobject_uevent(&udc->dev.kobj, KOBJ_REMOVE);
 	flush_work(&gadget->work);
@@ -520,50 +542,36 @@ err1:
 	return ret;
 }
 
-int usb_udc_attach_driver(const char *name, struct usb_gadget_driver *driver)
-{
-	struct usb_udc *udc = NULL;
-	int ret = -ENODEV;
-
-	mutex_lock(&udc_lock);
-	list_for_each_entry(udc, &udc_list, list) {
-		ret = strcmp(name, dev_name(&udc->dev));
-		if (!ret)
-			break;
-	}
-	if (ret) {
-		ret = -ENODEV;
-		goto out;
-	}
-	if (udc->driver) {
-		ret = -EBUSY;
-		goto out;
-	}
-	ret = udc_bind_to_driver(udc, driver);
-out:
-	mutex_unlock(&udc_lock);
-	return ret;
-}
-EXPORT_SYMBOL_GPL(usb_udc_attach_driver);
-
 int usb_gadget_probe_driver(struct usb_gadget_driver *driver)
 {
 	struct usb_udc		*udc = NULL;
-	int			ret;
+	int			ret = -ENODEV;
 
 	if (!driver || !driver->bind || !driver->setup)
 		return -EINVAL;
 
 	mutex_lock(&udc_lock);
-	list_for_each_entry(udc, &udc_list, list) {
-		/* For now we take the first one */
-		if (!udc->driver)
+	if (driver->udc_name) {
+		list_for_each_entry(udc, &udc_list, list) {
+			ret = strcmp(driver->udc_name, dev_name(&udc->dev));
+			if (!ret)
+				break;
+		}
+		if (!ret && !udc->driver)
 			goto found;
+	} else {
+		list_for_each_entry(udc, &udc_list, list) {
+			/* For now we take the first one */
+			if (!udc->driver)
+				goto found;
+		}
 	}
 
-	pr_debug("couldn't find an available UDC\n");
+	list_add_tail(&driver->pending, &gadget_driver_pending_list);
+	pr_info("udc-core: couldn't find an available UDC - added [%s] to list of pending drivers\n",
+		driver->function);
 	mutex_unlock(&udc_lock);
-	return -ENODEV;
+	return 0;
 found:
 	ret = udc_bind_to_driver(udc, driver);
 	mutex_unlock(&udc_lock);
@@ -589,6 +597,10 @@ int usb_gadget_unregister_driver(struct usb_gadget_driver *driver)
 			break;
 		}
 
+	if (ret) {
+		list_del(&driver->pending);
+		ret = 0;
+	}
 	mutex_unlock(&udc_lock);
 	return ret;
 }
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 3bb08870148f..1f117c360ebb 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -41,6 +41,15 @@ config USB_XHCI_PLATFORM
 
 	  If unsure, say N.
 
+config USB_XHCI_MTK
+	tristate "xHCI support for Mediatek MT65xx"
+	select MFD_SYSCON
+	depends on ARCH_MEDIATEK || COMPILE_TEST
+	---help---
+	  Say 'Y' to enable the support for the xHCI host controller
+	  found in Mediatek MT65xx SoCs.
+	  If unsure, say N.
+
 config USB_XHCI_MVEBU
 	tristate "xHCI support for Marvell Armada 375/38x"
 	select USB_XHCI_PLATFORM
@@ -220,6 +229,8 @@ config USB_EHCI_TEGRA
        depends on ARCH_TEGRA
        select USB_EHCI_ROOT_HUB_TT
        select USB_PHY
+	select USB_ULPI
+	select USB_ULPI_VIEWPORT
        help
          This driver enables support for the internal USB Host Controllers
          found in NVIDIA Tegra SoCs. The controllers are EHCI compliant.
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index e7558abc994d..65a06b4382bf 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -13,6 +13,9 @@ fhci-$(CONFIG_FHCI_DEBUG) += fhci-dbg.o
 xhci-hcd-y := xhci.o xhci-mem.o
 xhci-hcd-y += xhci-ring.o xhci-hub.o xhci-dbg.o
 xhci-hcd-y += xhci-trace.o
+ifneq ($(CONFIG_USB_XHCI_MTK), )
+	xhci-hcd-y += xhci-mtk-sch.o
+endif
 
 xhci-plat-hcd-y := xhci-plat.o
 ifneq ($(CONFIG_USB_XHCI_MVEBU), )
@@ -64,6 +67,7 @@ obj-$(CONFIG_USB_FHCI_HCD)	+= fhci.o
 obj-$(CONFIG_USB_XHCI_HCD)	+= xhci-hcd.o
 obj-$(CONFIG_USB_XHCI_PCI)	+= xhci-pci.o
 obj-$(CONFIG_USB_XHCI_PLATFORM) += xhci-plat-hcd.o
+obj-$(CONFIG_USB_XHCI_MTK)	+= xhci-mtk.o
 obj-$(CONFIG_USB_SL811_HCD)	+= sl811-hcd.o
 obj-$(CONFIG_USB_SL811_CS)	+= sl811_cs.o
 obj-$(CONFIG_USB_U132_HCD)	+= u132-hcd.o
diff --git a/drivers/usb/host/bcma-hcd.c b/drivers/usb/host/bcma-hcd.c
index 5398e3d42822..291aaa2baed8 100644
--- a/drivers/usb/host/bcma-hcd.c
+++ b/drivers/usb/host/bcma-hcd.c
@@ -21,6 +21,7 @@
  */
 #include <linux/bcma/bcma.h>
 #include <linux/delay.h>
+#include <linux/gpio/consumer.h>
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -36,6 +37,7 @@ MODULE_LICENSE("GPL");
 struct bcma_hcd_device {
 	struct platform_device *ehci_dev;
 	struct platform_device *ohci_dev;
+	struct gpio_desc *gpio_desc;
 };
 
 /* Wait for bitmask in a register to get set or cleared.
@@ -228,19 +230,12 @@ static void bcma_hcd_init_chip_arm(struct bcma_device *dev)
 
 static void bcma_hci_platform_power_gpio(struct bcma_device *dev, bool val)
 {
-	int gpio;
+	struct bcma_hcd_device *usb_dev = bcma_get_drvdata(dev);
 
-	gpio = of_get_named_gpio(dev->dev.of_node, "vcc-gpio", 0);
-	if (!gpio_is_valid(gpio))
+	if (IS_ERR_OR_NULL(usb_dev->gpio_desc))
 		return;
 
-	if (val) {
-		gpio_request(gpio, "bcma-hcd-gpio");
-		gpio_set_value(gpio, 1);
-	} else {
-		gpio_set_value(gpio, 0);
-		gpio_free(gpio);
-	}
+	gpiod_set_value(usb_dev->gpio_desc, val);
 }
 
 static const struct usb_ehci_pdata ehci_pdata = {
@@ -314,7 +309,11 @@ static int bcma_hcd_probe(struct bcma_device *dev)
 	if (!usb_dev)
 		return -ENOMEM;
 
-	bcma_hci_platform_power_gpio(dev, true);
+	if (dev->dev.of_node)
+		usb_dev->gpio_desc = devm_get_gpiod_from_child(&dev->dev, "vcc",
+							       &dev->dev.of_node->fwnode);
+	if (!IS_ERR_OR_NULL(usb_dev->gpio_desc))
+		gpiod_direction_output(usb_dev->gpio_desc, 1);
 
 	switch (dev->id.id) {
 	case BCMA_CORE_NS_USB20:
diff --git a/drivers/usb/host/ehci-dbg.c b/drivers/usb/host/ehci-dbg.c
index b26b96e25a13..b7d623f1523c 100644
--- a/drivers/usb/host/ehci-dbg.c
+++ b/drivers/usb/host/ehci-dbg.c
@@ -436,7 +436,8 @@ static void qh_lines (
 	scratch = hc32_to_cpup(ehci, &hw->hw_info1);
 	hw_curr = (mark == '*') ? hc32_to_cpup(ehci, &hw->hw_current) : 0;
 	temp = scnprintf (next, size,
-			"qh/%p dev%d %cs ep%d %08x %08x (%08x%c %s nak%d)",
+			"qh/%p dev%d %cs ep%d %08x %08x (%08x%c %s nak%d)"
+			" [cur %08x next %08x buf[0] %08x]",
 			qh, scratch & 0x007f,
 			speed_char (scratch),
 			(scratch >> 8) & 0x000f,
@@ -444,7 +445,10 @@ static void qh_lines (
 			hc32_to_cpup(ehci, &hw->hw_token), mark,
 			(cpu_to_hc32(ehci, QTD_TOGGLE) & hw->hw_token)
 				? "data1" : "data0",
-			(hc32_to_cpup(ehci, &hw->hw_alt_next) >> 1) & 0x0f);
+			(hc32_to_cpup(ehci, &hw->hw_alt_next) >> 1) & 0x0f,
+			hc32_to_cpup(ehci, &hw->hw_current),
+			hc32_to_cpup(ehci, &hw->hw_qtd_next),
+			hc32_to_cpup(ehci, &hw->hw_buf[0]));
 	size -= temp;
 	next += temp;
 
@@ -464,7 +468,8 @@ static void qh_lines (
 				mark = '/';
 		}
 		temp = snprintf (next, size,
-				"\n\t%p%c%s len=%d %08x urb %p",
+				"\n\t%p%c%s len=%d %08x urb %p"
+				" [td %08x buf[0] %08x]",
 				td, mark, ({ char *tmp;
 				 switch ((scratch>>8)&0x03) {
 				 case 0: tmp = "out"; break;
@@ -474,7 +479,9 @@ static void qh_lines (
 				 } tmp;}),
 				(scratch >> 16) & 0x7fff,
 				scratch,
-				td->urb);
+				td->urb,
+				(u32) td->qtd_dma,
+				hc32_to_cpup(ehci, &td->hw_buf[0]));
 		if (size < temp)
 			temp = size;
 		size -= temp;
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 48c92bf78bd0..14178bbf0694 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -98,7 +98,7 @@ module_param (park, uint, S_IRUGO);
 MODULE_PARM_DESC (park, "park setting; 1-3 back-to-back async packets");
 
 /* for flakey hardware, ignore overcurrent indicators */
-static bool ignore_oc = 0;
+static bool ignore_oc;
 module_param (ignore_oc, bool, S_IRUGO);
 MODULE_PARM_DESC (ignore_oc, "ignore bogus hardware overcurrent indications");
 
diff --git a/drivers/usb/host/ehci-mem.c b/drivers/usb/host/ehci-mem.c
index b6205fac3567..4de43011df23 100644
--- a/drivers/usb/host/ehci-mem.c
+++ b/drivers/usb/host/ehci-mem.c
@@ -128,21 +128,13 @@ static void ehci_mem_cleanup (struct ehci_hcd *ehci)
 	ehci->dummy = NULL;
 
 	/* DMA consistent memory and pools */
-	if (ehci->qtd_pool)
-		dma_pool_destroy (ehci->qtd_pool);
+	dma_pool_destroy(ehci->qtd_pool);
 	ehci->qtd_pool = NULL;
-
-	if (ehci->qh_pool) {
-		dma_pool_destroy (ehci->qh_pool);
-		ehci->qh_pool = NULL;
-	}
-
-	if (ehci->itd_pool)
-		dma_pool_destroy (ehci->itd_pool);
+	dma_pool_destroy(ehci->qh_pool);
+	ehci->qh_pool = NULL;
+	dma_pool_destroy(ehci->itd_pool);
 	ehci->itd_pool = NULL;
-
-	if (ehci->sitd_pool)
-		dma_pool_destroy (ehci->sitd_pool);
+	dma_pool_destroy(ehci->sitd_pool);
 	ehci->sitd_pool = NULL;
 
 	if (ehci->periodic)
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index c4f84c81de01..c23e2858c815 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -57,8 +57,8 @@ static int ehci_msm_reset(struct usb_hcd *hcd)
 
 	/* bursts of unspecified length. */
 	writel(0, USB_AHBBURST);
-	/* Use the AHB transactor */
-	writel(0, USB_AHBMODE);
+	/* Use the AHB transactor, allow posted data writes */
+	writel(0x8, USB_AHBMODE);
 	/* Disable streaming mode and select host mode */
 	writel(0x13, USB_USBMODE);
 
diff --git a/drivers/usb/host/ehci-q.c b/drivers/usb/host/ehci-q.c
index 54f5332f814d..aad0777240d3 100644
--- a/drivers/usb/host/ehci-q.c
+++ b/drivers/usb/host/ehci-q.c
@@ -132,10 +132,14 @@ qh_refresh (struct ehci_hcd *ehci, struct ehci_qh *qh)
 	 * qtd is updated in qh_completions(). Update the QH
 	 * overlay here.
 	 */
-	if (qh->hw->hw_token & ACTIVE_BIT(ehci))
+	if (qh->hw->hw_token & ACTIVE_BIT(ehci)) {
 		qh->hw->hw_qtd_next = qtd->hw_next;
-	else
+		if (qh->should_be_inactive)
+			ehci_warn(ehci, "qh %p should be inactive!\n", qh);
+	} else {
 		qh_update(ehci, qh, qtd);
+	}
+	qh->should_be_inactive = 0;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -438,6 +442,7 @@ qh_completions (struct ehci_hcd *ehci, struct ehci_qh *qh)
 					(hw->hw_token & ACTIVE_BIT(ehci))) {
 				token = hc32_to_cpu(ehci, hw->hw_token);
 				hw->hw_token &= ~ACTIVE_BIT(ehci);
+				qh->should_be_inactive = 1;
 
 				/* An unlink may leave an incomplete
 				 * async transaction in the TT buffer.
diff --git a/drivers/usb/host/ehci.h b/drivers/usb/host/ehci.h
index 46f62e41bcde..ec61aedb0067 100644
--- a/drivers/usb/host/ehci.h
+++ b/drivers/usb/host/ehci.h
@@ -439,6 +439,7 @@ struct ehci_qh {
 	unsigned		dequeue_during_giveback:1;
 	unsigned		exception:1;	/* got a fault, or an unlink
 						   was requested */
+	unsigned		should_be_inactive:1;
 };
 
 /*-------------------------------------------------------------------------*/
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index c6cebb96fd21..0960f41f945a 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -31,7 +31,7 @@
 #include <linux/of_platform.h>
 #include <linux/of_gpio.h>
 #include <linux/slab.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/qe.h>
 #include <asm/fsl_gtm.h>
 #include "fhci.h"
 
diff --git a/drivers/usb/host/fhci-hub.c b/drivers/usb/host/fhci-hub.c
index 3bacdd7befe9..60d55eb3de0d 100644
--- a/drivers/usb/host/fhci-hub.c
+++ b/drivers/usb/host/fhci-hub.c
@@ -24,7 +24,7 @@
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
 #include <linux/gpio.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/qe.h>
 #include "fhci.h"
 
 /* virtual root hub specific descriptor */
diff --git a/drivers/usb/host/fhci-sched.c b/drivers/usb/host/fhci-sched.c
index 95ca5986e672..a9609a336efe 100644
--- a/drivers/usb/host/fhci-sched.c
+++ b/drivers/usb/host/fhci-sched.c
@@ -25,7 +25,7 @@
 #include <linux/io.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
-#include <asm/qe.h>
+#include <soc/fsl/qe/qe.h>
 #include <asm/fsl_gtm.h>
 #include "fhci.h"
 
diff --git a/drivers/usb/host/fhci-tds.c b/drivers/usb/host/fhci-tds.c
index 1498061f0aea..f82ad5df1b0d 100644
--- a/drivers/usb/host/fhci-tds.c
+++ b/drivers/usb/host/fhci-tds.c
@@ -85,7 +85,7 @@ static struct usb_td __iomem *next_bd(struct usb_td __iomem *base,
 
 void fhci_push_dummy_bd(struct endpoint *ep)
 {
-	if (ep->already_pushed_dummy_bd == false) {
+	if (!ep->already_pushed_dummy_bd) {
 		u16 td_status = in_be16(&ep->empty_td->status);
 
 		out_be32(&ep->empty_td->buf_ptr, DUMMY_BD_BUFFER);
diff --git a/drivers/usb/host/fhci.h b/drivers/usb/host/fhci.h
index 154e6a007727..3fc82c1c3c73 100644
--- a/drivers/usb/host/fhci.h
+++ b/drivers/usb/host/fhci.h
@@ -27,8 +27,8 @@
 #include <linux/io.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
-#include <asm/qe.h>
-#include <asm/immap_qe.h>
+#include <soc/fsl/qe/qe.h>
+#include <soc/fsl/qe/immap_qe.h>
 
 #define USB_CLOCK	48000000
 
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 760cb57e954e..04dcedfdebf8 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -99,13 +99,13 @@ static void io_watchdog_func(unsigned long _ohci);
 
 
 /* Some boards misreport power switching/overcurrent */
-static bool distrust_firmware = 1;
+static bool distrust_firmware = true;
 module_param (distrust_firmware, bool, 0);
 MODULE_PARM_DESC (distrust_firmware,
 	"true to distrust firmware power/overcurrent setup");
 
 /* Some boards leave IR set wrongly, since they fail BIOS/SMM handshakes */
-static bool no_handshake = 0;
+static bool no_handshake;
 module_param (no_handshake, bool, 0);
 MODULE_PARM_DESC (no_handshake, "true (not default) disables BIOS handshake");
 
diff --git a/drivers/usb/host/ohci-pxa27x.c b/drivers/usb/host/ohci-pxa27x.c
index ba1bec7db026..e8c006e7a960 100644
--- a/drivers/usb/host/ohci-pxa27x.c
+++ b/drivers/usb/host/ohci-pxa27x.c
@@ -365,19 +365,19 @@ static int ohci_pxa_of_init(struct platform_device *pdev)
 	if (!pdata)
 		return -ENOMEM;
 
-	if (of_get_property(np, "marvell,enable-port1", NULL))
+	if (of_property_read_bool(np, "marvell,enable-port1"))
 		pdata->flags |= ENABLE_PORT1;
-	if (of_get_property(np, "marvell,enable-port2", NULL))
+	if (of_property_read_bool(np, "marvell,enable-port2"))
 		pdata->flags |= ENABLE_PORT2;
-	if (of_get_property(np, "marvell,enable-port3", NULL))
+	if (of_property_read_bool(np, "marvell,enable-port3"))
 		pdata->flags |= ENABLE_PORT3;
-	if (of_get_property(np, "marvell,port-sense-low", NULL))
+	if (of_property_read_bool(np, "marvell,port-sense-low"))
 		pdata->flags |= POWER_SENSE_LOW;
-	if (of_get_property(np, "marvell,power-control-low", NULL))
+	if (of_property_read_bool(np, "marvell,power-control-low"))
 		pdata->flags |= POWER_CONTROL_LOW;
-	if (of_get_property(np, "marvell,no-oc-protection", NULL))
+	if (of_property_read_bool(np, "marvell,no-oc-protection"))
 		pdata->flags |= NO_OC_PROTECTION;
-	if (of_get_property(np, "marvell,oc-mode-perport", NULL))
+	if (of_property_read_bool(np, "marvell,oc-mode-perport"))
 		pdata->flags |= OC_MODE_PERPORT;
 	if (!of_property_read_u32(np, "marvell,power-on-delay", &tmp))
 		pdata->power_on_delay = tmp;
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index 1f139d82cee0..bc74aca8a54c 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -394,8 +394,7 @@ static void ehci_quiesce(struct oxu_hcd *oxu)
 	u32	temp;
 
 #ifdef DEBUG
-	if (!HC_IS_RUNNING(oxu_to_hcd(oxu)->state))
-		BUG();
+	BUG_ON(!HC_IS_RUNNING(oxu_to_hcd(oxu)->state));
 #endif
 
 	/* wait for any schedule enables/disables to take effect */
@@ -1709,9 +1708,8 @@ static void start_unlink_async(struct oxu_hcd *oxu, struct ehci_qh *qh)
 
 #ifdef DEBUG
 	assert_spin_locked(&oxu->lock);
-	if (oxu->reclaim || (qh->qh_state != QH_STATE_LINKED
-				&& qh->qh_state != QH_STATE_UNLINK_WAIT))
-		BUG();
+	BUG_ON(oxu->reclaim || (qh->qh_state != QH_STATE_LINKED
+				&& qh->qh_state != QH_STATE_UNLINK_WAIT));
 #endif
 
 	/* stop async schedule right now? */
diff --git a/drivers/usb/host/pci-quirks.c b/drivers/usb/host/pci-quirks.c
index f9400564cb72..26cb8c861e6e 100644
--- a/drivers/usb/host/pci-quirks.c
+++ b/drivers/usb/host/pci-quirks.c
@@ -984,24 +984,17 @@ static void quirk_usb_handoff_xhci(struct pci_dev *pdev)
 	 * Find the Legacy Support Capability register -
 	 * this is optional for xHCI host controllers.
 	 */
-	ext_cap_offset = xhci_find_next_cap_offset(base, XHCI_HCC_PARAMS_OFFSET);
-	do {
-		if ((ext_cap_offset + sizeof(val)) > len) {
-			/* We're reading garbage from the controller */
-			dev_warn(&pdev->dev,
-				 "xHCI controller failing to respond");
-			return;
-		}
+	ext_cap_offset = xhci_find_next_ext_cap(base, 0, XHCI_EXT_CAPS_LEGACY);
 
-		if (!ext_cap_offset)
-			/* We've reached the end of the extended capabilities */
-			goto hc_init;
+	if (!ext_cap_offset)
+		goto hc_init;
 
-		val = readl(base + ext_cap_offset);
-		if (XHCI_EXT_CAPS_ID(val) == XHCI_EXT_CAPS_LEGACY)
-			break;
-		ext_cap_offset = xhci_find_next_cap_offset(base, ext_cap_offset);
-	} while (1);
+	if ((ext_cap_offset + sizeof(val)) > len) {
+		/* We're reading garbage from the controller */
+		dev_warn(&pdev->dev, "xHCI controller failing to respond");
+		return;
+	}
+	val = readl(base + ext_cap_offset);
 
 	/* If the BIOS owns the HC, signal that the OS wants it, and wait */
 	if (val & XHCI_HC_BIOS_OWNED) {
diff --git a/drivers/usb/host/u132-hcd.c b/drivers/usb/host/u132-hcd.c
index 692ccc69345e..05c85c7baf84 100644
--- a/drivers/usb/host/u132-hcd.c
+++ b/drivers/usb/host/u132-hcd.c
@@ -73,7 +73,7 @@ MODULE_LICENSE("GPL");
 #define INT_MODULE_PARM(n, v) static int n = v;module_param(n, int, 0444)
 INT_MODULE_PARM(testing, 0);
 /* Some boards misreport power switching/overcurrent*/
-static bool distrust_firmware = 1;
+static bool distrust_firmware = true;
 module_param(distrust_firmware, bool, 0);
 MODULE_PARM_DESC(distrust_firmware, "true to distrust firmware power/overcurren"
 	"t setup");
diff --git a/drivers/usb/host/uhci-debug.c b/drivers/usb/host/uhci-debug.c
index 1b28a000d5c6..9c6635d43db0 100644
--- a/drivers/usb/host/uhci-debug.c
+++ b/drivers/usb/host/uhci-debug.c
@@ -584,27 +584,8 @@ static int uhci_debug_open(struct inode *inode, struct file *file)
 
 static loff_t uhci_debug_lseek(struct file *file, loff_t off, int whence)
 {
-	struct uhci_debug *up;
-	loff_t new = -1;
-
-	up = file->private_data;
-
-	/*
-	 * XXX: atomic 64bit seek access, but that needs to be fixed in the VFS
-	 */
-	switch (whence) {
-	case 0:
-		new = off;
-		break;
-	case 1:
-		new = file->f_pos + off;
-		break;
-	}
-
-	if (new < 0 || new > up->size)
-		return -EINVAL;
-
-	return (file->f_pos = new);
+	struct uhci_debug *up = file->private_data;
+	return no_seek_end_llseek_size(file, off, whence, up->size);
 }
 
 static ssize_t uhci_debug_read(struct file *file, char __user *buf,
diff --git a/drivers/usb/host/uhci-q.c b/drivers/usb/host/uhci-q.c
index da6f56d996ce..c17ea1589b83 100644
--- a/drivers/usb/host/uhci-q.c
+++ b/drivers/usb/host/uhci-q.c
@@ -248,11 +248,10 @@ static struct uhci_qh *uhci_alloc_qh(struct uhci_hcd *uhci,
 	dma_addr_t dma_handle;
 	struct uhci_qh *qh;
 
-	qh = dma_pool_alloc(uhci->qh_pool, GFP_ATOMIC, &dma_handle);
+	qh = dma_pool_zalloc(uhci->qh_pool, GFP_ATOMIC, &dma_handle);
 	if (!qh)
 		return NULL;
 
-	memset(qh, 0, sizeof(*qh));
 	qh->dma_handle = dma_handle;
 
 	qh->element = UHCI_PTR_TERM(uhci);
diff --git a/drivers/usb/host/whci/qset.c b/drivers/usb/host/whci/qset.c
index 9f1c0538b211..1a8e960d073b 100644
--- a/drivers/usb/host/whci/qset.c
+++ b/drivers/usb/host/whci/qset.c
@@ -30,10 +30,9 @@ struct whc_qset *qset_alloc(struct whc *whc, gfp_t mem_flags)
 	struct whc_qset *qset;
 	dma_addr_t dma;
 
-	qset = dma_pool_alloc(whc->qset_pool, mem_flags, &dma);
+	qset = dma_pool_zalloc(whc->qset_pool, mem_flags, &dma);
 	if (qset == NULL)
 		return NULL;
-	memset(qset, 0, sizeof(struct whc_qset));
 
 	qset->qset_dma = dma;
 	qset->whc = whc;
@@ -400,7 +399,7 @@ static void urb_dequeue_work(struct work_struct *work)
 	struct whc *whc = qset->whc;
 	unsigned long flags;
 
-	if (wurb->is_async == true)
+	if (wurb->is_async)
 		asl_update(whc, WUSBCMD_ASYNC_UPDATED
 			   | WUSBCMD_ASYNC_SYNCED_DB
 			   | WUSBCMD_ASYNC_QSET_RM);
diff --git a/drivers/usb/host/xhci-ext-caps.h b/drivers/usb/host/xhci-ext-caps.h
index 9fe3225e6c61..e0244fb3903d 100644
--- a/drivers/usb/host/xhci-ext-caps.h
+++ b/drivers/usb/host/xhci-ext-caps.h
@@ -91,66 +91,43 @@
 #include <linux/io.h>
 
 /**
- * Return the next extended capability pointer register.
- *
- * @base	PCI register base address.
- *
- * @ext_offset	Offset of the 32-bit register that contains the extended
- * capabilites pointer.  If searching for the first extended capability, pass
- * in XHCI_HCC_PARAMS_OFFSET.  If searching for the next extended capability,
- * pass in the offset of the current extended capability register.
- *
- * Returns 0 if there is no next extended capability register or returns the register offset
- * from the PCI registers base address.
- */
-static inline int xhci_find_next_cap_offset(void __iomem *base, int ext_offset)
-{
-	u32 next;
-
-	next = readl(base + ext_offset);
-
-	if (ext_offset == XHCI_HCC_PARAMS_OFFSET) {
-		/* Find the first extended capability */
-		next = XHCI_HCC_EXT_CAPS(next);
-		ext_offset = 0;
-	} else {
-		/* Find the next extended capability */
-		next = XHCI_EXT_CAPS_NEXT(next);
-	}
-
-	if (!next)
-		return 0;
-	/*
-	 * Address calculation from offset of extended capabilities
-	 * (or HCCPARAMS) register - see section 5.3.6 and section 7.
-	 */
-	return ext_offset + (next << 2);
-}
-
-/**
  * Find the offset of the extended capabilities with capability ID id.
  *
- * @base PCI MMIO registers base address.
- * @ext_offset Offset from base of the first extended capability to look at,
- * 		or the address of HCCPARAMS.
- * @id Extended capability ID to search for.
+ * @base	PCI MMIO registers base address.
+ * @start	address at which to start looking, (0 or HCC_PARAMS to start at
+ *		beginning of list)
+ * @id		Extended capability ID to search for.
  *
- * This uses an arbitrary limit of XHCI_MAX_EXT_CAPS extended capabilities
- * to make sure that the list doesn't contain a loop.
+ * Returns the offset of the next matching extended capability structure.
+ * Some capabilities can occur several times, e.g., the XHCI_EXT_CAPS_PROTOCOL,
+ * and this provides a way to find them all.
  */
-static inline int xhci_find_ext_cap_by_id(void __iomem *base, int ext_offset, int id)
+
+static inline int xhci_find_next_ext_cap(void __iomem *base, u32 start, int id)
 {
 	u32 val;
-	int limit = XHCI_MAX_EXT_CAPS;
-
-	while (ext_offset && limit > 0) {
-		val = readl(base + ext_offset);
-		if (XHCI_EXT_CAPS_ID(val) == id)
-			break;
-		ext_offset = xhci_find_next_cap_offset(base, ext_offset);
-		limit--;
-	}
-	if (limit > 0)
-		return ext_offset;
+	u32 next;
+	u32 offset;
+
+	offset = start;
+	if (!start || start == XHCI_HCC_PARAMS_OFFSET) {
+		val = readl(base + XHCI_HCC_PARAMS_OFFSET);
+		if (val == ~0)
+			return 0;
+		offset = XHCI_HCC_EXT_CAPS(val) << 2;
+		if (!offset)
+			return 0;
+	};
+	do {
+		val = readl(base + offset);
+		if (val == ~0)
+			return 0;
+		if (XHCI_EXT_CAPS_ID(val) == id && offset != start)
+			return offset;
+
+		next = XHCI_EXT_CAPS_NEXT(val);
+		offset += next << 2;
+	} while (next);
+
 	return 0;
 }
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index f980c239eded..b30b4ce294d3 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -855,7 +855,7 @@ static u32 xhci_get_port_status(struct usb_hcd *hcd,
 		xhci_hub_report_usb2_link_state(&status, raw_port_status);
 	}
 	if (bus_state->port_c_suspend & (1 << wIndex))
-		status |= 1 << USB_PORT_FEAT_C_SUSPEND;
+		status |= USB_PORT_STAT_C_SUSPEND << 16;
 
 	return status;
 }
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index c48cbe731356..5cd080e0a685 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -47,13 +47,12 @@ static struct xhci_segment *xhci_segment_alloc(struct xhci_hcd *xhci,
 	if (!seg)
 		return NULL;
 
-	seg->trbs = dma_pool_alloc(xhci->segment_pool, flags, &dma);
+	seg->trbs = dma_pool_zalloc(xhci->segment_pool, flags, &dma);
 	if (!seg->trbs) {
 		kfree(seg);
 		return NULL;
 	}
 
-	memset(seg->trbs, 0, TRB_SEGMENT_SIZE);
 	/* If the cycle state is 0, set the cycle bit to 1 for all the TRBs */
 	if (cycle_state == 0) {
 		for (i = 0; i < TRBS_PER_SEGMENT; i++)
@@ -517,12 +516,11 @@ static struct xhci_container_ctx *xhci_alloc_container_ctx(struct xhci_hcd *xhci
 	if (type == XHCI_CTX_TYPE_INPUT)
 		ctx->size += CTX_SIZE(xhci->hcc_params);
 
-	ctx->bytes = dma_pool_alloc(xhci->device_pool, flags, &ctx->dma);
+	ctx->bytes = dma_pool_zalloc(xhci->device_pool, flags, &ctx->dma);
 	if (!ctx->bytes) {
 		kfree(ctx);
 		return NULL;
 	}
-	memset(ctx->bytes, 0, ctx->size);
 	return ctx;
 }
 
@@ -1245,7 +1243,7 @@ static unsigned int xhci_microframes_to_exponent(struct usb_device *udev,
 	interval = fls(desc_interval) - 1;
 	interval = clamp_val(interval, min_exponent, max_exponent);
 	if ((1 << interval) != desc_interval)
-		dev_warn(&udev->dev,
+		dev_dbg(&udev->dev,
 			 "ep %#x - rounding interval to %d microframes, ep desc says %d microframes\n",
 			 ep->desc.bEndpointAddress,
 			 1 << interval,
@@ -2064,17 +2062,19 @@ static void xhci_set_hc_event_deq(struct xhci_hcd *xhci)
 }
 
 static void xhci_add_in_port(struct xhci_hcd *xhci, unsigned int num_ports,
-		__le32 __iomem *addr, u8 major_revision, int max_caps)
+		__le32 __iomem *addr, int max_caps)
 {
 	u32 temp, port_offset, port_count;
 	int i;
+	u8 major_revision;
 	struct xhci_hub *rhub;
 
 	temp = readl(addr);
+	major_revision = XHCI_EXT_PORT_MAJOR(temp);
 
-	if (XHCI_EXT_PORT_MAJOR(temp) == 0x03) {
+	if (major_revision == 0x03) {
 		rhub = &xhci->usb3_rhub;
-	} else if (XHCI_EXT_PORT_MAJOR(temp) <= 0x02) {
+	} else if (major_revision <= 0x02) {
 		rhub = &xhci->usb2_rhub;
 	} else {
 		xhci_warn(xhci, "Ignoring unknown port speed, "
@@ -2190,19 +2190,12 @@ static void xhci_add_in_port(struct xhci_hcd *xhci, unsigned int num_ports,
  */
 static int xhci_setup_port_arrays(struct xhci_hcd *xhci, gfp_t flags)
 {
-	__le32 __iomem *addr, *tmp_addr;
-	u32 offset, tmp_offset;
+	void __iomem *base;
+	u32 offset;
 	unsigned int num_ports;
 	int i, j, port_index;
 	int cap_count = 0;
-
-	addr = &xhci->cap_regs->hcc_params;
-	offset = XHCI_HCC_EXT_CAPS(readl(addr));
-	if (offset == 0) {
-		xhci_err(xhci, "No Extended Capability registers, "
-				"unable to set up roothub.\n");
-		return -ENODEV;
-	}
+	u32 cap_start;
 
 	num_ports = HCS_MAX_PORTS(xhci->hcs_params1);
 	xhci->port_array = kzalloc(sizeof(*xhci->port_array)*num_ports, flags);
@@ -2220,48 +2213,34 @@ static int xhci_setup_port_arrays(struct xhci_hcd *xhci, gfp_t flags)
 		for (j = 0; j < XHCI_MAX_INTERVAL; j++)
 			INIT_LIST_HEAD(&bw_table->interval_bw[j].endpoints);
 	}
+	base = &xhci->cap_regs->hc_capbase;
 
-	/*
-	 * For whatever reason, the first capability offset is from the
-	 * capability register base, not from the HCCPARAMS register.
-	 * See section 5.3.6 for offset calculation.
-	 */
-	addr = &xhci->cap_regs->hc_capbase + offset;
-
-	tmp_addr = addr;
-	tmp_offset = offset;
+	cap_start = xhci_find_next_ext_cap(base, 0, XHCI_EXT_CAPS_PROTOCOL);
+	if (!cap_start) {
+		xhci_err(xhci, "No Extended Capability registers, unable to set up roothub\n");
+		return -ENODEV;
+	}
 
+	offset = cap_start;
 	/* count extended protocol capability entries for later caching */
-	do {
-		u32 cap_id;
-		cap_id = readl(tmp_addr);
-		if (XHCI_EXT_CAPS_ID(cap_id) == XHCI_EXT_CAPS_PROTOCOL)
-			cap_count++;
-		tmp_offset = XHCI_EXT_CAPS_NEXT(cap_id);
-		tmp_addr += tmp_offset;
-	} while (tmp_offset);
+	while (offset) {
+		cap_count++;
+		offset = xhci_find_next_ext_cap(base, offset,
+						      XHCI_EXT_CAPS_PROTOCOL);
+	}
 
 	xhci->ext_caps = kzalloc(sizeof(*xhci->ext_caps) * cap_count, flags);
 	if (!xhci->ext_caps)
 		return -ENOMEM;
 
-	while (1) {
-		u32 cap_id;
-
-		cap_id = readl(addr);
-		if (XHCI_EXT_CAPS_ID(cap_id) == XHCI_EXT_CAPS_PROTOCOL)
-			xhci_add_in_port(xhci, num_ports, addr,
-					(u8) XHCI_EXT_PORT_MAJOR(cap_id),
-					cap_count);
-		offset = XHCI_EXT_CAPS_NEXT(cap_id);
-		if (!offset || (xhci->num_usb2_ports + xhci->num_usb3_ports)
-				== num_ports)
+	offset = cap_start;
+
+	while (offset) {
+		xhci_add_in_port(xhci, num_ports, base + offset, cap_count);
+		if (xhci->num_usb2_ports + xhci->num_usb3_ports == num_ports)
 			break;
-		/*
-		 * Once you're into the Extended Capabilities, the offset is
-		 * always relative to the register holding the offset.
-		 */
-		addr += offset;
+		offset = xhci_find_next_ext_cap(base, offset,
+						XHCI_EXT_CAPS_PROTOCOL);
 	}
 
 	if (xhci->num_usb2_ports == 0 && xhci->num_usb3_ports == 0) {
diff --git a/drivers/usb/host/xhci-mtk-sch.c b/drivers/usb/host/xhci-mtk-sch.c
new file mode 100644
index 000000000000..73f763c4f5f5
--- /dev/null
+++ b/drivers/usb/host/xhci-mtk-sch.c
@@ -0,0 +1,425 @@
+/*
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author:
+ *  Zhigang.Wei <zhigang.wei@mediatek.com>
+ *  Chunfeng.Yun <chunfeng.yun@mediatek.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+
+#include "xhci.h"
+#include "xhci-mtk.h"
+
+#define SS_BW_BOUNDARY	51000
+/* table 5-5. High-speed Isoc Transaction Limits in usb_20 spec */
+#define HS_BW_BOUNDARY	6144
+/* usb2 spec section11.18.1: at most 188 FS bytes per microframe */
+#define FS_PAYLOAD_MAX 188
+
+/* mtk scheduler bitmasks */
+#define EP_BPKTS(p)	((p) & 0x3f)
+#define EP_BCSCOUNT(p)	(((p) & 0x7) << 8)
+#define EP_BBM(p)	((p) << 11)
+#define EP_BOFFSET(p)	((p) & 0x3fff)
+#define EP_BREPEAT(p)	(((p) & 0x7fff) << 16)
+
+static int is_fs_or_ls(enum usb_device_speed speed)
+{
+	return speed == USB_SPEED_FULL || speed == USB_SPEED_LOW;
+}
+
+/*
+* get the index of bandwidth domains array which @ep belongs to.
+*
+* the bandwidth domain array is saved to @sch_array of struct xhci_hcd_mtk,
+* each HS root port is treated as a single bandwidth domain,
+* but each SS root port is treated as two bandwidth domains, one for IN eps,
+* one for OUT eps.
+* @real_port value is defined as follow according to xHCI spec:
+* 1 for SSport0, ..., N+1 for SSportN, N+2 for HSport0, N+3 for HSport1, etc
+* so the bandwidth domain array is organized as follow for simplification:
+* SSport0-OUT, SSport0-IN, ..., SSportX-OUT, SSportX-IN, HSport0, ..., HSportY
+*/
+static int get_bw_index(struct xhci_hcd *xhci, struct usb_device *udev,
+	struct usb_host_endpoint *ep)
+{
+	struct xhci_virt_device *virt_dev;
+	int bw_index;
+
+	virt_dev = xhci->devs[udev->slot_id];
+
+	if (udev->speed == USB_SPEED_SUPER) {
+		if (usb_endpoint_dir_out(&ep->desc))
+			bw_index = (virt_dev->real_port - 1) * 2;
+		else
+			bw_index = (virt_dev->real_port - 1) * 2 + 1;
+	} else {
+		/* add one more for each SS port */
+		bw_index = virt_dev->real_port + xhci->num_usb3_ports - 1;
+	}
+
+	return bw_index;
+}
+
+static void setup_sch_info(struct usb_device *udev,
+		struct xhci_ep_ctx *ep_ctx, struct mu3h_sch_ep_info *sch_ep)
+{
+	u32 ep_type;
+	u32 ep_interval;
+	u32 max_packet_size;
+	u32 max_burst;
+	u32 mult;
+	u32 esit_pkts;
+
+	ep_type = CTX_TO_EP_TYPE(le32_to_cpu(ep_ctx->ep_info2));
+	ep_interval = CTX_TO_EP_INTERVAL(le32_to_cpu(ep_ctx->ep_info));
+	max_packet_size = MAX_PACKET_DECODED(le32_to_cpu(ep_ctx->ep_info2));
+	max_burst = CTX_TO_MAX_BURST(le32_to_cpu(ep_ctx->ep_info2));
+	mult = CTX_TO_EP_MULT(le32_to_cpu(ep_ctx->ep_info));
+
+	sch_ep->esit = 1 << ep_interval;
+	sch_ep->offset = 0;
+	sch_ep->burst_mode = 0;
+
+	if (udev->speed == USB_SPEED_HIGH) {
+		sch_ep->cs_count = 0;
+
+		/*
+		 * usb_20 spec section5.9
+		 * a single microframe is enough for HS synchromous endpoints
+		 * in a interval
+		 */
+		sch_ep->num_budget_microframes = 1;
+		sch_ep->repeat = 0;
+
+		/*
+		 * xHCI spec section6.2.3.4
+		 * @max_burst is the number of additional transactions
+		 * opportunities per microframe
+		 */
+		sch_ep->pkts = max_burst + 1;
+		sch_ep->bw_cost_per_microframe = max_packet_size * sch_ep->pkts;
+	} else if (udev->speed == USB_SPEED_SUPER) {
+		/* usb3_r1 spec section4.4.7 & 4.4.8 */
+		sch_ep->cs_count = 0;
+		esit_pkts = (mult + 1) * (max_burst + 1);
+		if (ep_type == INT_IN_EP || ep_type == INT_OUT_EP) {
+			sch_ep->pkts = esit_pkts;
+			sch_ep->num_budget_microframes = 1;
+			sch_ep->repeat = 0;
+		}
+
+		if (ep_type == ISOC_IN_EP || ep_type == ISOC_OUT_EP) {
+			if (esit_pkts <= sch_ep->esit)
+				sch_ep->pkts = 1;
+			else
+				sch_ep->pkts = roundup_pow_of_two(esit_pkts)
+					/ sch_ep->esit;
+
+			sch_ep->num_budget_microframes =
+				DIV_ROUND_UP(esit_pkts, sch_ep->pkts);
+
+			if (sch_ep->num_budget_microframes > 1)
+				sch_ep->repeat = 1;
+			else
+				sch_ep->repeat = 0;
+		}
+		sch_ep->bw_cost_per_microframe = max_packet_size * sch_ep->pkts;
+	} else if (is_fs_or_ls(udev->speed)) {
+
+		/*
+		 * usb_20 spec section11.18.4
+		 * assume worst cases
+		 */
+		sch_ep->repeat = 0;
+		sch_ep->pkts = 1; /* at most one packet for each microframe */
+		if (ep_type == INT_IN_EP || ep_type == INT_OUT_EP) {
+			sch_ep->cs_count = 3; /* at most need 3 CS*/
+			/* one for SS and one for budgeted transaction */
+			sch_ep->num_budget_microframes = sch_ep->cs_count + 2;
+			sch_ep->bw_cost_per_microframe = max_packet_size;
+		}
+		if (ep_type == ISOC_OUT_EP) {
+
+			/*
+			 * the best case FS budget assumes that 188 FS bytes
+			 * occur in each microframe
+			 */
+			sch_ep->num_budget_microframes = DIV_ROUND_UP(
+				max_packet_size, FS_PAYLOAD_MAX);
+			sch_ep->bw_cost_per_microframe = FS_PAYLOAD_MAX;
+			sch_ep->cs_count = sch_ep->num_budget_microframes;
+		}
+		if (ep_type == ISOC_IN_EP) {
+			/* at most need additional two CS. */
+			sch_ep->cs_count = DIV_ROUND_UP(
+				max_packet_size, FS_PAYLOAD_MAX) + 2;
+			sch_ep->num_budget_microframes = sch_ep->cs_count + 2;
+			sch_ep->bw_cost_per_microframe = FS_PAYLOAD_MAX;
+		}
+	}
+}
+
+/* Get maximum bandwidth when we schedule at offset slot. */
+static u32 get_max_bw(struct mu3h_sch_bw_info *sch_bw,
+	struct mu3h_sch_ep_info *sch_ep, u32 offset)
+{
+	u32 num_esit;
+	u32 max_bw = 0;
+	int i;
+	int j;
+
+	num_esit = XHCI_MTK_MAX_ESIT / sch_ep->esit;
+	for (i = 0; i < num_esit; i++) {
+		u32 base = offset + i * sch_ep->esit;
+
+		for (j = 0; j < sch_ep->num_budget_microframes; j++) {
+			if (sch_bw->bus_bw[base + j] > max_bw)
+				max_bw = sch_bw->bus_bw[base + j];
+		}
+	}
+	return max_bw;
+}
+
+static void update_bus_bw(struct mu3h_sch_bw_info *sch_bw,
+	struct mu3h_sch_ep_info *sch_ep, int bw_cost)
+{
+	u32 num_esit;
+	u32 base;
+	int i;
+	int j;
+
+	num_esit = XHCI_MTK_MAX_ESIT / sch_ep->esit;
+	for (i = 0; i < num_esit; i++) {
+		base = sch_ep->offset + i * sch_ep->esit;
+		for (j = 0; j < sch_ep->num_budget_microframes; j++)
+			sch_bw->bus_bw[base + j] += bw_cost;
+	}
+}
+
+static int check_sch_bw(struct usb_device *udev,
+	struct mu3h_sch_bw_info *sch_bw, struct mu3h_sch_ep_info *sch_ep)
+{
+	u32 offset;
+	u32 esit;
+	u32 num_budget_microframes;
+	u32 min_bw;
+	u32 min_index;
+	u32 worst_bw;
+	u32 bw_boundary;
+
+	if (sch_ep->esit > XHCI_MTK_MAX_ESIT)
+		sch_ep->esit = XHCI_MTK_MAX_ESIT;
+
+	esit = sch_ep->esit;
+	num_budget_microframes = sch_ep->num_budget_microframes;
+
+	/*
+	 * Search through all possible schedule microframes.
+	 * and find a microframe where its worst bandwidth is minimum.
+	 */
+	min_bw = ~0;
+	min_index = 0;
+	for (offset = 0; offset < esit; offset++) {
+		if ((offset + num_budget_microframes) > sch_ep->esit)
+			break;
+
+		/*
+		 * usb_20 spec section11.18:
+		 * must never schedule Start-Split in Y6
+		 */
+		if (is_fs_or_ls(udev->speed) && (offset % 8 == 6))
+			continue;
+
+		worst_bw = get_max_bw(sch_bw, sch_ep, offset);
+		if (min_bw > worst_bw) {
+			min_bw = worst_bw;
+			min_index = offset;
+		}
+		if (min_bw == 0)
+			break;
+	}
+	sch_ep->offset = min_index;
+
+	bw_boundary = (udev->speed == USB_SPEED_SUPER)
+				? SS_BW_BOUNDARY : HS_BW_BOUNDARY;
+
+	/* check bandwidth */
+	if (min_bw + sch_ep->bw_cost_per_microframe > bw_boundary)
+		return -ERANGE;
+
+	/* update bus bandwidth info */
+	update_bus_bw(sch_bw, sch_ep, sch_ep->bw_cost_per_microframe);
+
+	return 0;
+}
+
+static bool need_bw_sch(struct usb_host_endpoint *ep,
+	enum usb_device_speed speed, int has_tt)
+{
+	/* only for periodic endpoints */
+	if (usb_endpoint_xfer_control(&ep->desc)
+		|| usb_endpoint_xfer_bulk(&ep->desc))
+		return false;
+
+	/*
+	 * for LS & FS periodic endpoints which its device is not behind
+	 * a TT are also ignored, root-hub will schedule them directly,
+	 * but need set @bpkts field of endpoint context to 1.
+	 */
+	if (is_fs_or_ls(speed) && !has_tt)
+		return false;
+
+	return true;
+}
+
+int xhci_mtk_sch_init(struct xhci_hcd_mtk *mtk)
+{
+	struct mu3h_sch_bw_info *sch_array;
+	int num_usb_bus;
+	int i;
+
+	/* ss IN and OUT are separated */
+	num_usb_bus = mtk->num_u3_ports * 2 + mtk->num_u2_ports;
+
+	sch_array = kcalloc(num_usb_bus, sizeof(*sch_array), GFP_KERNEL);
+	if (sch_array == NULL)
+		return -ENOMEM;
+
+	for (i = 0; i < num_usb_bus; i++)
+		INIT_LIST_HEAD(&sch_array[i].bw_ep_list);
+
+	mtk->sch_array = sch_array;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xhci_mtk_sch_init);
+
+void xhci_mtk_sch_exit(struct xhci_hcd_mtk *mtk)
+{
+	kfree(mtk->sch_array);
+}
+EXPORT_SYMBOL_GPL(xhci_mtk_sch_exit);
+
+int xhci_mtk_add_ep_quirk(struct usb_hcd *hcd, struct usb_device *udev,
+		struct usb_host_endpoint *ep)
+{
+	struct xhci_hcd_mtk *mtk = hcd_to_mtk(hcd);
+	struct xhci_hcd *xhci;
+	struct xhci_ep_ctx *ep_ctx;
+	struct xhci_slot_ctx *slot_ctx;
+	struct xhci_virt_device *virt_dev;
+	struct mu3h_sch_bw_info *sch_bw;
+	struct mu3h_sch_ep_info *sch_ep;
+	struct mu3h_sch_bw_info *sch_array;
+	unsigned int ep_index;
+	int bw_index;
+	int ret = 0;
+
+	xhci = hcd_to_xhci(hcd);
+	virt_dev = xhci->devs[udev->slot_id];
+	ep_index = xhci_get_endpoint_index(&ep->desc);
+	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
+	ep_ctx = xhci_get_ep_ctx(xhci, virt_dev->in_ctx, ep_index);
+	sch_array = mtk->sch_array;
+
+	xhci_dbg(xhci, "%s() type:%d, speed:%d, mpkt:%d, dir:%d, ep:%p\n",
+		__func__, usb_endpoint_type(&ep->desc), udev->speed,
+		GET_MAX_PACKET(usb_endpoint_maxp(&ep->desc)),
+		usb_endpoint_dir_in(&ep->desc), ep);
+
+	if (!need_bw_sch(ep, udev->speed, slot_ctx->tt_info & TT_SLOT)) {
+		/*
+		 * set @bpkts to 1 if it is LS or FS periodic endpoint, and its
+		 * device does not connected through an external HS hub
+		 */
+		if (usb_endpoint_xfer_int(&ep->desc)
+			|| usb_endpoint_xfer_isoc(&ep->desc))
+			ep_ctx->reserved[0] |= cpu_to_le32(EP_BPKTS(1));
+
+		return 0;
+	}
+
+	bw_index = get_bw_index(xhci, udev, ep);
+	sch_bw = &sch_array[bw_index];
+
+	sch_ep = kzalloc(sizeof(struct mu3h_sch_ep_info), GFP_NOIO);
+	if (!sch_ep)
+		return -ENOMEM;
+
+	setup_sch_info(udev, ep_ctx, sch_ep);
+
+	ret = check_sch_bw(udev, sch_bw, sch_ep);
+	if (ret) {
+		xhci_err(xhci, "Not enough bandwidth!\n");
+		kfree(sch_ep);
+		return -ENOSPC;
+	}
+
+	list_add_tail(&sch_ep->endpoint, &sch_bw->bw_ep_list);
+	sch_ep->ep = ep;
+
+	ep_ctx->reserved[0] |= cpu_to_le32(EP_BPKTS(sch_ep->pkts)
+		| EP_BCSCOUNT(sch_ep->cs_count) | EP_BBM(sch_ep->burst_mode));
+	ep_ctx->reserved[1] |= cpu_to_le32(EP_BOFFSET(sch_ep->offset)
+		| EP_BREPEAT(sch_ep->repeat));
+
+	xhci_dbg(xhci, " PKTS:%x, CSCOUNT:%x, BM:%x, OFFSET:%x, REPEAT:%x\n",
+			sch_ep->pkts, sch_ep->cs_count, sch_ep->burst_mode,
+			sch_ep->offset, sch_ep->repeat);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(xhci_mtk_add_ep_quirk);
+
+void xhci_mtk_drop_ep_quirk(struct usb_hcd *hcd, struct usb_device *udev,
+		struct usb_host_endpoint *ep)
+{
+	struct xhci_hcd_mtk *mtk = hcd_to_mtk(hcd);
+	struct xhci_hcd *xhci;
+	struct xhci_slot_ctx *slot_ctx;
+	struct xhci_virt_device *virt_dev;
+	struct mu3h_sch_bw_info *sch_array;
+	struct mu3h_sch_bw_info *sch_bw;
+	struct mu3h_sch_ep_info *sch_ep;
+	int bw_index;
+
+	xhci = hcd_to_xhci(hcd);
+	virt_dev = xhci->devs[udev->slot_id];
+	slot_ctx = xhci_get_slot_ctx(xhci, virt_dev->in_ctx);
+	sch_array = mtk->sch_array;
+
+	xhci_dbg(xhci, "%s() type:%d, speed:%d, mpks:%d, dir:%d, ep:%p\n",
+		__func__, usb_endpoint_type(&ep->desc), udev->speed,
+		GET_MAX_PACKET(usb_endpoint_maxp(&ep->desc)),
+		usb_endpoint_dir_in(&ep->desc), ep);
+
+	if (!need_bw_sch(ep, udev->speed, slot_ctx->tt_info & TT_SLOT))
+		return;
+
+	bw_index = get_bw_index(xhci, udev, ep);
+	sch_bw = &sch_array[bw_index];
+
+	list_for_each_entry(sch_ep, &sch_bw->bw_ep_list, endpoint) {
+		if (sch_ep->ep == ep) {
+			update_bus_bw(sch_bw, sch_ep,
+				-sch_ep->bw_cost_per_microframe);
+			list_del(&sch_ep->endpoint);
+			kfree(sch_ep);
+			break;
+		}
+	}
+}
+EXPORT_SYMBOL_GPL(xhci_mtk_drop_ep_quirk);
diff --git a/drivers/usb/host/xhci-mtk.c b/drivers/usb/host/xhci-mtk.c
new file mode 100644
index 000000000000..9532f5aef71b
--- /dev/null
+++ b/drivers/usb/host/xhci-mtk.c
@@ -0,0 +1,786 @@
+/*
+ * MediaTek xHCI Host Controller Driver
+ *
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author:
+ *  Chunfeng Yun <chunfeng.yun@mediatek.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/iopoll.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include "xhci.h"
+#include "xhci-mtk.h"
+
+/* ip_pw_ctrl0 register */
+#define CTRL0_IP_SW_RST	BIT(0)
+
+/* ip_pw_ctrl1 register */
+#define CTRL1_IP_HOST_PDN	BIT(0)
+
+/* ip_pw_ctrl2 register */
+#define CTRL2_IP_DEV_PDN	BIT(0)
+
+/* ip_pw_sts1 register */
+#define STS1_IP_SLEEP_STS	BIT(30)
+#define STS1_XHCI_RST		BIT(11)
+#define STS1_SYS125_RST	BIT(10)
+#define STS1_REF_RST		BIT(8)
+#define STS1_SYSPLL_STABLE	BIT(0)
+
+/* ip_xhci_cap register */
+#define CAP_U3_PORT_NUM(p)	((p) & 0xff)
+#define CAP_U2_PORT_NUM(p)	(((p) >> 8) & 0xff)
+
+/* u3_ctrl_p register */
+#define CTRL_U3_PORT_HOST_SEL	BIT(2)
+#define CTRL_U3_PORT_PDN	BIT(1)
+#define CTRL_U3_PORT_DIS	BIT(0)
+
+/* u2_ctrl_p register */
+#define CTRL_U2_PORT_HOST_SEL	BIT(2)
+#define CTRL_U2_PORT_PDN	BIT(1)
+#define CTRL_U2_PORT_DIS	BIT(0)
+
+/* u2_phy_pll register */
+#define CTRL_U2_FORCE_PLL_STB	BIT(28)
+
+#define PERI_WK_CTRL0		0x400
+#define UWK_CTR0_0P_LS_PE	BIT(8)  /* posedge */
+#define UWK_CTR0_0P_LS_NE	BIT(7)  /* negedge for 0p linestate*/
+#define UWK_CTL1_1P_LS_C(x)	(((x) & 0xf) << 1)
+#define UWK_CTL1_1P_LS_E	BIT(0)
+
+#define PERI_WK_CTRL1		0x404
+#define UWK_CTL1_IS_C(x)	(((x) & 0xf) << 26)
+#define UWK_CTL1_IS_E		BIT(25)
+#define UWK_CTL1_0P_LS_C(x)	(((x) & 0xf) << 21)
+#define UWK_CTL1_0P_LS_E	BIT(20)
+#define UWK_CTL1_IDDIG_C(x)	(((x) & 0xf) << 11)  /* cycle debounce */
+#define UWK_CTL1_IDDIG_E	BIT(10) /* enable debounce */
+#define UWK_CTL1_IDDIG_P	BIT(9)  /* polarity */
+#define UWK_CTL1_0P_LS_P	BIT(7)
+#define UWK_CTL1_IS_P		BIT(6)  /* polarity for ip sleep */
+
+enum ssusb_wakeup_src {
+	SSUSB_WK_IP_SLEEP = 1,
+	SSUSB_WK_LINE_STATE = 2,
+};
+
+static int xhci_mtk_host_enable(struct xhci_hcd_mtk *mtk)
+{
+	struct mu3c_ippc_regs __iomem *ippc = mtk->ippc_regs;
+	u32 value, check_val;
+	int ret;
+	int i;
+
+	/* power on host ip */
+	value = readl(&ippc->ip_pw_ctr1);
+	value &= ~CTRL1_IP_HOST_PDN;
+	writel(value, &ippc->ip_pw_ctr1);
+
+	/* power on and enable all u3 ports */
+	for (i = 0; i < mtk->num_u3_ports; i++) {
+		value = readl(&ippc->u3_ctrl_p[i]);
+		value &= ~(CTRL_U3_PORT_PDN | CTRL_U3_PORT_DIS);
+		value |= CTRL_U3_PORT_HOST_SEL;
+		writel(value, &ippc->u3_ctrl_p[i]);
+	}
+
+	/* power on and enable all u2 ports */
+	for (i = 0; i < mtk->num_u2_ports; i++) {
+		value = readl(&ippc->u2_ctrl_p[i]);
+		value &= ~(CTRL_U2_PORT_PDN | CTRL_U2_PORT_DIS);
+		value |= CTRL_U2_PORT_HOST_SEL;
+		writel(value, &ippc->u2_ctrl_p[i]);
+	}
+
+	/*
+	 * wait for clocks to be stable, and clock domains reset to
+	 * be inactive after power on and enable ports
+	 */
+	check_val = STS1_SYSPLL_STABLE | STS1_REF_RST |
+			STS1_SYS125_RST | STS1_XHCI_RST;
+
+	ret = readl_poll_timeout(&ippc->ip_pw_sts1, value,
+			  (check_val == (value & check_val)), 100, 20000);
+	if (ret) {
+		dev_err(mtk->dev, "clocks are not stable (0x%x)\n", value);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int xhci_mtk_host_disable(struct xhci_hcd_mtk *mtk)
+{
+	struct mu3c_ippc_regs __iomem *ippc = mtk->ippc_regs;
+	u32 value;
+	int ret;
+	int i;
+
+	/* power down all u3 ports */
+	for (i = 0; i < mtk->num_u3_ports; i++) {
+		value = readl(&ippc->u3_ctrl_p[i]);
+		value |= CTRL_U3_PORT_PDN;
+		writel(value, &ippc->u3_ctrl_p[i]);
+	}
+
+	/* power down all u2 ports */
+	for (i = 0; i < mtk->num_u2_ports; i++) {
+		value = readl(&ippc->u2_ctrl_p[i]);
+		value |= CTRL_U2_PORT_PDN;
+		writel(value, &ippc->u2_ctrl_p[i]);
+	}
+
+	/* power down host ip */
+	value = readl(&ippc->ip_pw_ctr1);
+	value |= CTRL1_IP_HOST_PDN;
+	writel(value, &ippc->ip_pw_ctr1);
+
+	/* wait for host ip to sleep */
+	ret = readl_poll_timeout(&ippc->ip_pw_sts1, value,
+			  (value & STS1_IP_SLEEP_STS), 100, 100000);
+	if (ret) {
+		dev_err(mtk->dev, "ip sleep failed!!!\n");
+		return ret;
+	}
+	return 0;
+}
+
+static int xhci_mtk_ssusb_config(struct xhci_hcd_mtk *mtk)
+{
+	struct mu3c_ippc_regs __iomem *ippc = mtk->ippc_regs;
+	u32 value;
+
+	/* reset whole ip */
+	value = readl(&ippc->ip_pw_ctr0);
+	value |= CTRL0_IP_SW_RST;
+	writel(value, &ippc->ip_pw_ctr0);
+	udelay(1);
+	value = readl(&ippc->ip_pw_ctr0);
+	value &= ~CTRL0_IP_SW_RST;
+	writel(value, &ippc->ip_pw_ctr0);
+
+	/*
+	 * device ip is default power-on in fact
+	 * power down device ip, otherwise ip-sleep will fail
+	 */
+	value = readl(&ippc->ip_pw_ctr2);
+	value |= CTRL2_IP_DEV_PDN;
+	writel(value, &ippc->ip_pw_ctr2);
+
+	value = readl(&ippc->ip_xhci_cap);
+	mtk->num_u3_ports = CAP_U3_PORT_NUM(value);
+	mtk->num_u2_ports = CAP_U2_PORT_NUM(value);
+	dev_dbg(mtk->dev, "%s u2p:%d, u3p:%d\n", __func__,
+			mtk->num_u2_ports, mtk->num_u3_ports);
+
+	return xhci_mtk_host_enable(mtk);
+}
+
+static int xhci_mtk_clks_enable(struct xhci_hcd_mtk *mtk)
+{
+	int ret;
+
+	ret = clk_prepare_enable(mtk->sys_clk);
+	if (ret) {
+		dev_err(mtk->dev, "failed to enable sys_clk\n");
+		goto sys_clk_err;
+	}
+
+	if (mtk->wakeup_src) {
+		ret = clk_prepare_enable(mtk->wk_deb_p0);
+		if (ret) {
+			dev_err(mtk->dev, "failed to enable wk_deb_p0\n");
+			goto usb_p0_err;
+		}
+
+		ret = clk_prepare_enable(mtk->wk_deb_p1);
+		if (ret) {
+			dev_err(mtk->dev, "failed to enable wk_deb_p1\n");
+			goto usb_p1_err;
+		}
+	}
+	return 0;
+
+usb_p1_err:
+	clk_disable_unprepare(mtk->wk_deb_p0);
+usb_p0_err:
+	clk_disable_unprepare(mtk->sys_clk);
+sys_clk_err:
+	return -EINVAL;
+}
+
+static void xhci_mtk_clks_disable(struct xhci_hcd_mtk *mtk)
+{
+	if (mtk->wakeup_src) {
+		clk_disable_unprepare(mtk->wk_deb_p1);
+		clk_disable_unprepare(mtk->wk_deb_p0);
+	}
+	clk_disable_unprepare(mtk->sys_clk);
+}
+
+/* only clocks can be turn off for ip-sleep wakeup mode */
+static void usb_wakeup_ip_sleep_en(struct xhci_hcd_mtk *mtk)
+{
+	u32 tmp;
+	struct regmap *pericfg = mtk->pericfg;
+
+	regmap_read(pericfg, PERI_WK_CTRL1, &tmp);
+	tmp &= ~UWK_CTL1_IS_P;
+	tmp &= ~(UWK_CTL1_IS_C(0xf));
+	tmp |= UWK_CTL1_IS_C(0x8);
+	regmap_write(pericfg, PERI_WK_CTRL1, tmp);
+	regmap_write(pericfg, PERI_WK_CTRL1, tmp | UWK_CTL1_IS_E);
+
+	regmap_read(pericfg, PERI_WK_CTRL1, &tmp);
+	dev_dbg(mtk->dev, "%s(): WK_CTRL1[P6,E25,C26:29]=%#x\n",
+		__func__, tmp);
+}
+
+static void usb_wakeup_ip_sleep_dis(struct xhci_hcd_mtk *mtk)
+{
+	u32 tmp;
+
+	regmap_read(mtk->pericfg, PERI_WK_CTRL1, &tmp);
+	tmp &= ~UWK_CTL1_IS_E;
+	regmap_write(mtk->pericfg, PERI_WK_CTRL1, tmp);
+}
+
+/*
+* for line-state wakeup mode, phy's power should not power-down
+* and only support cable plug in/out
+*/
+static void usb_wakeup_line_state_en(struct xhci_hcd_mtk *mtk)
+{
+	u32 tmp;
+	struct regmap *pericfg = mtk->pericfg;
+
+	/* line-state of u2-port0 */
+	regmap_read(pericfg, PERI_WK_CTRL1, &tmp);
+	tmp &= ~UWK_CTL1_0P_LS_P;
+	tmp &= ~(UWK_CTL1_0P_LS_C(0xf));
+	tmp |= UWK_CTL1_0P_LS_C(0x8);
+	regmap_write(pericfg, PERI_WK_CTRL1, tmp);
+	regmap_read(pericfg, PERI_WK_CTRL1, &tmp);
+	regmap_write(pericfg, PERI_WK_CTRL1, tmp | UWK_CTL1_0P_LS_E);
+
+	/* line-state of u2-port1 */
+	regmap_read(pericfg, PERI_WK_CTRL0, &tmp);
+	tmp &= ~(UWK_CTL1_1P_LS_C(0xf));
+	tmp |= UWK_CTL1_1P_LS_C(0x8);
+	regmap_write(pericfg, PERI_WK_CTRL0, tmp);
+	regmap_write(pericfg, PERI_WK_CTRL0, tmp | UWK_CTL1_1P_LS_E);
+}
+
+static void usb_wakeup_line_state_dis(struct xhci_hcd_mtk *mtk)
+{
+	u32 tmp;
+	struct regmap *pericfg = mtk->pericfg;
+
+	/* line-state of u2-port0 */
+	regmap_read(pericfg, PERI_WK_CTRL1, &tmp);
+	tmp &= ~UWK_CTL1_0P_LS_E;
+	regmap_write(pericfg, PERI_WK_CTRL1, tmp);
+
+	/* line-state of u2-port1 */
+	regmap_read(pericfg, PERI_WK_CTRL0, &tmp);
+	tmp &= ~UWK_CTL1_1P_LS_E;
+	regmap_write(pericfg, PERI_WK_CTRL0, tmp);
+}
+
+static void usb_wakeup_enable(struct xhci_hcd_mtk *mtk)
+{
+	if (mtk->wakeup_src == SSUSB_WK_IP_SLEEP)
+		usb_wakeup_ip_sleep_en(mtk);
+	else if (mtk->wakeup_src == SSUSB_WK_LINE_STATE)
+		usb_wakeup_line_state_en(mtk);
+}
+
+static void usb_wakeup_disable(struct xhci_hcd_mtk *mtk)
+{
+	if (mtk->wakeup_src == SSUSB_WK_IP_SLEEP)
+		usb_wakeup_ip_sleep_dis(mtk);
+	else if (mtk->wakeup_src == SSUSB_WK_LINE_STATE)
+		usb_wakeup_line_state_dis(mtk);
+}
+
+static int usb_wakeup_of_property_parse(struct xhci_hcd_mtk *mtk,
+				struct device_node *dn)
+{
+	struct device *dev = mtk->dev;
+
+	/*
+	* wakeup function is optional, so it is not an error if this property
+	* does not exist, and in such case, no need to get relative
+	* properties anymore.
+	*/
+	of_property_read_u32(dn, "mediatek,wakeup-src", &mtk->wakeup_src);
+	if (!mtk->wakeup_src)
+		return 0;
+
+	mtk->wk_deb_p0 = devm_clk_get(dev, "wakeup_deb_p0");
+	if (IS_ERR(mtk->wk_deb_p0)) {
+		dev_err(dev, "fail to get wakeup_deb_p0\n");
+		return PTR_ERR(mtk->wk_deb_p0);
+	}
+
+	mtk->wk_deb_p1 = devm_clk_get(dev, "wakeup_deb_p1");
+	if (IS_ERR(mtk->wk_deb_p1)) {
+		dev_err(dev, "fail to get wakeup_deb_p1\n");
+		return PTR_ERR(mtk->wk_deb_p1);
+	}
+
+	mtk->pericfg = syscon_regmap_lookup_by_phandle(dn,
+						"mediatek,syscon-wakeup");
+	if (IS_ERR(mtk->pericfg)) {
+		dev_err(dev, "fail to get pericfg regs\n");
+		return PTR_ERR(mtk->pericfg);
+	}
+
+	return 0;
+}
+
+static int xhci_mtk_setup(struct usb_hcd *hcd);
+static const struct xhci_driver_overrides xhci_mtk_overrides __initconst = {
+	.extra_priv_size = sizeof(struct xhci_hcd),
+	.reset = xhci_mtk_setup,
+};
+
+static struct hc_driver __read_mostly xhci_mtk_hc_driver;
+
+static int xhci_mtk_phy_init(struct xhci_hcd_mtk *mtk)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < mtk->num_phys; i++) {
+		ret = phy_init(mtk->phys[i]);
+		if (ret)
+			goto exit_phy;
+	}
+	return 0;
+
+exit_phy:
+	for (; i > 0; i--)
+		phy_exit(mtk->phys[i - 1]);
+
+	return ret;
+}
+
+static int xhci_mtk_phy_exit(struct xhci_hcd_mtk *mtk)
+{
+	int i;
+
+	for (i = 0; i < mtk->num_phys; i++)
+		phy_exit(mtk->phys[i]);
+
+	return 0;
+}
+
+static int xhci_mtk_phy_power_on(struct xhci_hcd_mtk *mtk)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < mtk->num_phys; i++) {
+		ret = phy_power_on(mtk->phys[i]);
+		if (ret)
+			goto power_off_phy;
+	}
+	return 0;
+
+power_off_phy:
+	for (; i > 0; i--)
+		phy_power_off(mtk->phys[i - 1]);
+
+	return ret;
+}
+
+static void xhci_mtk_phy_power_off(struct xhci_hcd_mtk *mtk)
+{
+	unsigned int i;
+
+	for (i = 0; i < mtk->num_phys; i++)
+		phy_power_off(mtk->phys[i]);
+}
+
+static int xhci_mtk_ldos_enable(struct xhci_hcd_mtk *mtk)
+{
+	int ret;
+
+	ret = regulator_enable(mtk->vbus);
+	if (ret) {
+		dev_err(mtk->dev, "failed to enable vbus\n");
+		return ret;
+	}
+
+	ret = regulator_enable(mtk->vusb33);
+	if (ret) {
+		dev_err(mtk->dev, "failed to enable vusb33\n");
+		regulator_disable(mtk->vbus);
+		return ret;
+	}
+	return 0;
+}
+
+static void xhci_mtk_ldos_disable(struct xhci_hcd_mtk *mtk)
+{
+	regulator_disable(mtk->vbus);
+	regulator_disable(mtk->vusb33);
+}
+
+static void xhci_mtk_quirks(struct device *dev, struct xhci_hcd *xhci)
+{
+	struct usb_hcd *hcd = xhci_to_hcd(xhci);
+	struct xhci_hcd_mtk *mtk = hcd_to_mtk(hcd);
+
+	/*
+	 * As of now platform drivers don't provide MSI support so we ensure
+	 * here that the generic code does not try to make a pci_dev from our
+	 * dev struct in order to setup MSI
+	 */
+	xhci->quirks |= XHCI_PLAT;
+	xhci->quirks |= XHCI_MTK_HOST;
+	/*
+	 * MTK host controller gives a spurious successful event after a
+	 * short transfer. Ignore it.
+	 */
+	xhci->quirks |= XHCI_SPURIOUS_SUCCESS;
+	if (mtk->lpm_support)
+		xhci->quirks |= XHCI_LPM_SUPPORT;
+}
+
+/* called during probe() after chip reset completes */
+static int xhci_mtk_setup(struct usb_hcd *hcd)
+{
+	struct xhci_hcd_mtk *mtk = hcd_to_mtk(hcd);
+	int ret;
+
+	if (usb_hcd_is_primary_hcd(hcd)) {
+		ret = xhci_mtk_ssusb_config(mtk);
+		if (ret)
+			return ret;
+		ret = xhci_mtk_sch_init(mtk);
+		if (ret)
+			return ret;
+	}
+
+	return xhci_gen_setup(hcd, xhci_mtk_quirks);
+}
+
+static int xhci_mtk_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node;
+	struct xhci_hcd_mtk *mtk;
+	const struct hc_driver *driver;
+	struct xhci_hcd *xhci;
+	struct resource *res;
+	struct usb_hcd *hcd;
+	struct phy *phy;
+	int phy_num;
+	int ret = -ENODEV;
+	int irq;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	driver = &xhci_mtk_hc_driver;
+	mtk = devm_kzalloc(dev, sizeof(*mtk), GFP_KERNEL);
+	if (!mtk)
+		return -ENOMEM;
+
+	mtk->dev = dev;
+	mtk->vbus = devm_regulator_get(dev, "vbus");
+	if (IS_ERR(mtk->vbus)) {
+		dev_err(dev, "fail to get vbus\n");
+		return PTR_ERR(mtk->vbus);
+	}
+
+	mtk->vusb33 = devm_regulator_get(dev, "vusb33");
+	if (IS_ERR(mtk->vusb33)) {
+		dev_err(dev, "fail to get vusb33\n");
+		return PTR_ERR(mtk->vusb33);
+	}
+
+	mtk->sys_clk = devm_clk_get(dev, "sys_ck");
+	if (IS_ERR(mtk->sys_clk)) {
+		dev_err(dev, "fail to get sys_ck\n");
+		return PTR_ERR(mtk->sys_clk);
+	}
+
+	mtk->lpm_support = of_property_read_bool(node, "usb3-lpm-capable");
+
+	ret = usb_wakeup_of_property_parse(mtk, node);
+	if (ret)
+		return ret;
+
+	mtk->num_phys = of_count_phandle_with_args(node,
+			"phys", "#phy-cells");
+	if (mtk->num_phys > 0) {
+		mtk->phys = devm_kcalloc(dev, mtk->num_phys,
+					sizeof(*mtk->phys), GFP_KERNEL);
+		if (!mtk->phys)
+			return -ENOMEM;
+	} else {
+		mtk->num_phys = 0;
+	}
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+	device_enable_async_suspend(dev);
+
+	ret = xhci_mtk_ldos_enable(mtk);
+	if (ret)
+		goto disable_pm;
+
+	ret = xhci_mtk_clks_enable(mtk);
+	if (ret)
+		goto disable_ldos;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		goto disable_clk;
+
+	/* Initialize dma_mask and coherent_dma_mask to 32-bits */
+	ret = dma_set_coherent_mask(dev, DMA_BIT_MASK(32));
+	if (ret)
+		goto disable_clk;
+
+	if (!dev->dma_mask)
+		dev->dma_mask = &dev->coherent_dma_mask;
+	else
+		dma_set_mask(dev, DMA_BIT_MASK(32));
+
+	hcd = usb_create_hcd(driver, dev, dev_name(dev));
+	if (!hcd) {
+		ret = -ENOMEM;
+		goto disable_clk;
+	}
+
+	/*
+	 * USB 2.0 roothub is stored in the platform_device.
+	 * Swap it with mtk HCD.
+	 */
+	mtk->hcd = platform_get_drvdata(pdev);
+	platform_set_drvdata(pdev, mtk);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	hcd->regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(hcd->regs)) {
+		ret = PTR_ERR(hcd->regs);
+		goto put_usb2_hcd;
+	}
+	hcd->rsrc_start = res->start;
+	hcd->rsrc_len = resource_size(res);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	mtk->ippc_regs = devm_ioremap_resource(dev, res);
+	if (IS_ERR(mtk->ippc_regs)) {
+		ret = PTR_ERR(mtk->ippc_regs);
+		goto put_usb2_hcd;
+	}
+
+	for (phy_num = 0; phy_num < mtk->num_phys; phy_num++) {
+		phy = devm_of_phy_get_by_index(dev, node, phy_num);
+		if (IS_ERR(phy)) {
+			ret = PTR_ERR(phy);
+			goto put_usb2_hcd;
+		}
+		mtk->phys[phy_num] = phy;
+	}
+
+	ret = xhci_mtk_phy_init(mtk);
+	if (ret)
+		goto put_usb2_hcd;
+
+	ret = xhci_mtk_phy_power_on(mtk);
+	if (ret)
+		goto exit_phys;
+
+	device_init_wakeup(dev, true);
+
+	xhci = hcd_to_xhci(hcd);
+	xhci->main_hcd = hcd;
+	xhci->shared_hcd = usb_create_shared_hcd(driver, dev,
+			dev_name(dev), hcd);
+	if (!xhci->shared_hcd) {
+		ret = -ENOMEM;
+		goto power_off_phys;
+	}
+
+	if (HCC_MAX_PSA(xhci->hcc_params) >= 4)
+		xhci->shared_hcd->can_do_streams = 1;
+
+	ret = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (ret)
+		goto put_usb3_hcd;
+
+	ret = usb_add_hcd(xhci->shared_hcd, irq, IRQF_SHARED);
+	if (ret)
+		goto dealloc_usb2_hcd;
+
+	return 0;
+
+dealloc_usb2_hcd:
+	usb_remove_hcd(hcd);
+
+put_usb3_hcd:
+	xhci_mtk_sch_exit(mtk);
+	usb_put_hcd(xhci->shared_hcd);
+
+power_off_phys:
+	xhci_mtk_phy_power_off(mtk);
+	device_init_wakeup(dev, false);
+
+exit_phys:
+	xhci_mtk_phy_exit(mtk);
+
+put_usb2_hcd:
+	usb_put_hcd(hcd);
+
+disable_clk:
+	xhci_mtk_clks_disable(mtk);
+
+disable_ldos:
+	xhci_mtk_ldos_disable(mtk);
+
+disable_pm:
+	pm_runtime_put_sync(dev);
+	pm_runtime_disable(dev);
+	return ret;
+}
+
+static int xhci_mtk_remove(struct platform_device *dev)
+{
+	struct xhci_hcd_mtk *mtk = platform_get_drvdata(dev);
+	struct usb_hcd	*hcd = mtk->hcd;
+	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+
+	usb_remove_hcd(xhci->shared_hcd);
+	xhci_mtk_phy_power_off(mtk);
+	xhci_mtk_phy_exit(mtk);
+	device_init_wakeup(&dev->dev, false);
+
+	usb_remove_hcd(hcd);
+	usb_put_hcd(xhci->shared_hcd);
+	usb_put_hcd(hcd);
+	xhci_mtk_sch_exit(mtk);
+	xhci_mtk_clks_disable(mtk);
+	xhci_mtk_ldos_disable(mtk);
+	pm_runtime_put_sync(&dev->dev);
+	pm_runtime_disable(&dev->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * if ip sleep fails, and all clocks are disabled, access register will hang
+ * AHB bus, so stop polling roothubs to avoid regs access on bus suspend.
+ * and no need to check whether ip sleep failed or not; this will cause SPM
+ * to wake up system immediately after system suspend complete if ip sleep
+ * fails, it is what we wanted.
+ */
+static int xhci_mtk_suspend(struct device *dev)
+{
+	struct xhci_hcd_mtk *mtk = dev_get_drvdata(dev);
+	struct usb_hcd *hcd = mtk->hcd;
+	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
+
+	xhci_dbg(xhci, "%s: stop port polling\n", __func__);
+	clear_bit(HCD_FLAG_POLL_RH, &hcd->flags);
+	del_timer_sync(&hcd->rh_timer);
+	clear_bit(HCD_FLAG_POLL_RH, &xhci->shared_hcd->flags);
+	del_timer_sync(&xhci->shared_hcd->rh_timer);
+
+	xhci_mtk_host_disable(mtk);
+	xhci_mtk_phy_power_off(mtk);
+	xhci_mtk_clks_disable(mtk);
+	usb_wakeup_enable(mtk);
+	return 0;
+}
+
+static int xhci_mtk_resume(struct device *dev)
+{
+	struct xhci_hcd_mtk *mtk = dev_get_drvdata(dev);
+	struct usb_hcd *hcd = mtk->hcd;
+	struct xhci_hcd *xhci = hcd_to_xhci(hcd);
+
+	usb_wakeup_disable(mtk);
+	xhci_mtk_clks_enable(mtk);
+	xhci_mtk_phy_power_on(mtk);
+	xhci_mtk_host_enable(mtk);
+
+	xhci_dbg(xhci, "%s: restart port polling\n", __func__);
+	set_bit(HCD_FLAG_POLL_RH, &hcd->flags);
+	usb_hcd_poll_rh_status(hcd);
+	set_bit(HCD_FLAG_POLL_RH, &xhci->shared_hcd->flags);
+	usb_hcd_poll_rh_status(xhci->shared_hcd);
+	return 0;
+}
+
+static const struct dev_pm_ops xhci_mtk_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(xhci_mtk_suspend, xhci_mtk_resume)
+};
+#define DEV_PM_OPS	(&xhci_mtk_pm_ops)
+#else
+#define DEV_PM_OPS	NULL
+#endif /* CONFIG_PM */
+
+#ifdef CONFIG_OF
+static const struct of_device_id mtk_xhci_of_match[] = {
+	{ .compatible = "mediatek,mt8173-xhci"},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, mtk_xhci_of_match);
+#endif
+
+static struct platform_driver mtk_xhci_driver = {
+	.probe	= xhci_mtk_probe,
+	.remove	= xhci_mtk_remove,
+	.driver	= {
+		.name = "xhci-mtk",
+		.pm = DEV_PM_OPS,
+		.of_match_table = of_match_ptr(mtk_xhci_of_match),
+	},
+};
+MODULE_ALIAS("platform:xhci-mtk");
+
+static int __init xhci_mtk_init(void)
+{
+	xhci_init_driver(&xhci_mtk_hc_driver, &xhci_mtk_overrides);
+	return platform_driver_register(&mtk_xhci_driver);
+}
+module_init(xhci_mtk_init);
+
+static void __exit xhci_mtk_exit(void)
+{
+	platform_driver_unregister(&mtk_xhci_driver);
+}
+module_exit(xhci_mtk_exit);
+
+MODULE_AUTHOR("Chunfeng Yun <chunfeng.yun@mediatek.com>");
+MODULE_DESCRIPTION("MediaTek xHCI Host Controller Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/host/xhci-mtk.h b/drivers/usb/host/xhci-mtk.h
new file mode 100644
index 000000000000..7da677c79ea8
--- /dev/null
+++ b/drivers/usb/host/xhci-mtk.h
@@ -0,0 +1,162 @@
+/*
+ * Copyright (c) 2015 MediaTek Inc.
+ * Author:
+ *  Zhigang.Wei <zhigang.wei@mediatek.com>
+ *  Chunfeng.Yun <chunfeng.yun@mediatek.com>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef _XHCI_MTK_H_
+#define _XHCI_MTK_H_
+
+#include "xhci.h"
+
+/**
+ * To simplify scheduler algorithm, set a upper limit for ESIT,
+ * if a synchromous ep's ESIT is larger than @XHCI_MTK_MAX_ESIT,
+ * round down to the limit value, that means allocating more
+ * bandwidth to it.
+ */
+#define XHCI_MTK_MAX_ESIT	64
+
+/**
+ * struct mu3h_sch_bw_info: schedule information for bandwidth domain
+ *
+ * @bus_bw: array to keep track of bandwidth already used at each uframes
+ * @bw_ep_list: eps in the bandwidth domain
+ *
+ * treat a HS root port as a bandwidth domain, but treat a SS root port as
+ * two bandwidth domains, one for IN eps and another for OUT eps.
+ */
+struct mu3h_sch_bw_info {
+	u32 bus_bw[XHCI_MTK_MAX_ESIT];
+	struct list_head bw_ep_list;
+};
+
+/**
+ * struct mu3h_sch_ep_info: schedule information for endpoint
+ *
+ * @esit: unit is 125us, equal to 2 << Interval field in ep-context
+ * @num_budget_microframes: number of continuous uframes
+ *		(@repeat==1) scheduled within the interval
+ * @bw_cost_per_microframe: bandwidth cost per microframe
+ * @endpoint: linked into bandwidth domain which it belongs to
+ * @ep: address of usb_host_endpoint struct
+ * @offset: which uframe of the interval that transfer should be
+ *		scheduled first time within the interval
+ * @repeat: the time gap between two uframes that transfers are
+ *		scheduled within a interval. in the simple algorithm, only
+ *		assign 0 or 1 to it; 0 means using only one uframe in a
+ *		interval, and 1 means using @num_budget_microframes
+ *		continuous uframes
+ * @pkts: number of packets to be transferred in the scheduled uframes
+ * @cs_count: number of CS that host will trigger
+ * @burst_mode: burst mode for scheduling. 0: normal burst mode,
+ *		distribute the bMaxBurst+1 packets for a single burst
+ *		according to @pkts and @repeat, repeate the burst multiple
+ *		times; 1: distribute the (bMaxBurst+1)*(Mult+1) packets
+ *		according to @pkts and @repeat. normal mode is used by
+ *		default
+ */
+struct mu3h_sch_ep_info {
+	u32 esit;
+	u32 num_budget_microframes;
+	u32 bw_cost_per_microframe;
+	struct list_head endpoint;
+	void *ep;
+	/*
+	 * mtk xHCI scheduling information put into reserved DWs
+	 * in ep context
+	 */
+	u32 offset;
+	u32 repeat;
+	u32 pkts;
+	u32 cs_count;
+	u32 burst_mode;
+};
+
+#define MU3C_U3_PORT_MAX 4
+#define MU3C_U2_PORT_MAX 5
+
+/**
+ * struct mu3c_ippc_regs: MTK ssusb ip port control registers
+ * @ip_pw_ctr0~3: ip power and clock control registers
+ * @ip_pw_sts1~2: ip power and clock status registers
+ * @ip_xhci_cap: ip xHCI capability register
+ * @u3_ctrl_p[x]: ip usb3 port x control register, only low 4bytes are used
+ * @u2_ctrl_p[x]: ip usb2 port x control register, only low 4bytes are used
+ * @u2_phy_pll: usb2 phy pll control register
+ */
+struct mu3c_ippc_regs {
+	__le32 ip_pw_ctr0;
+	__le32 ip_pw_ctr1;
+	__le32 ip_pw_ctr2;
+	__le32 ip_pw_ctr3;
+	__le32 ip_pw_sts1;
+	__le32 ip_pw_sts2;
+	__le32 reserved0[3];
+	__le32 ip_xhci_cap;
+	__le32 reserved1[2];
+	__le64 u3_ctrl_p[MU3C_U3_PORT_MAX];
+	__le64 u2_ctrl_p[MU3C_U2_PORT_MAX];
+	__le32 reserved2;
+	__le32 u2_phy_pll;
+	__le32 reserved3[33]; /* 0x80 ~ 0xff */
+};
+
+struct xhci_hcd_mtk {
+	struct device *dev;
+	struct usb_hcd *hcd;
+	struct mu3h_sch_bw_info *sch_array;
+	struct mu3c_ippc_regs __iomem *ippc_regs;
+	int num_u2_ports;
+	int num_u3_ports;
+	struct regulator *vusb33;
+	struct regulator *vbus;
+	struct clk *sys_clk;	/* sys and mac clock */
+	struct clk *wk_deb_p0;	/* port0's wakeup debounce clock */
+	struct clk *wk_deb_p1;
+	struct regmap *pericfg;
+	struct phy **phys;
+	int num_phys;
+	int wakeup_src;
+	bool lpm_support;
+};
+
+static inline struct xhci_hcd_mtk *hcd_to_mtk(struct usb_hcd *hcd)
+{
+	return dev_get_drvdata(hcd->self.controller);
+}
+
+#if IS_ENABLED(CONFIG_USB_XHCI_MTK)
+int xhci_mtk_sch_init(struct xhci_hcd_mtk *mtk);
+void xhci_mtk_sch_exit(struct xhci_hcd_mtk *mtk);
+int xhci_mtk_add_ep_quirk(struct usb_hcd *hcd, struct usb_device *udev,
+		struct usb_host_endpoint *ep);
+void xhci_mtk_drop_ep_quirk(struct usb_hcd *hcd, struct usb_device *udev,
+		struct usb_host_endpoint *ep);
+
+#else
+static inline int xhci_mtk_add_ep_quirk(struct usb_hcd *hcd,
+	struct usb_device *udev, struct usb_host_endpoint *ep)
+{
+	return 0;
+}
+
+static inline void xhci_mtk_drop_ep_quirk(struct usb_hcd *hcd,
+	struct usb_device *udev, struct usb_host_endpoint *ep)
+{
+}
+
+#endif
+
+#endif		/* _XHCI_MTK_H_ */
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index c62109091d12..f0640b7a1c42 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -28,7 +28,9 @@
 #include "xhci.h"
 #include "xhci-trace.h"
 
-#define PORT2_SSIC_CONFIG_REG2	0x883c
+#define SSIC_PORT_NUM		2
+#define SSIC_PORT_CFG2		0x880c
+#define SSIC_PORT_CFG2_OFFSET	0x30
 #define PROG_DONE		(1 << 30)
 #define SSIC_PORT_UNUSED	(1 << 31)
 
@@ -45,6 +47,7 @@
 #define PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI		0x22b5
 #define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_XHCI		0xa12f
 #define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_XHCI	0x9d2f
+#define PCI_DEVICE_ID_INTEL_BROXTON_M_XHCI		0x0aa8
 
 static const char hcd_name[] = "xhci_hcd";
 
@@ -53,7 +56,6 @@ static struct hc_driver __read_mostly xhci_pci_hc_driver;
 static int xhci_pci_setup(struct usb_hcd *hcd);
 
 static const struct xhci_driver_overrides xhci_pci_overrides __initconst = {
-	.extra_priv_size = sizeof(struct xhci_hcd),
 	.reset = xhci_pci_setup,
 };
 
@@ -152,9 +154,14 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
 		(pdev->device == PCI_DEVICE_ID_INTEL_SUNRISEPOINT_LP_XHCI ||
 		 pdev->device == PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_XHCI ||
-		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI)) {
+		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI ||
+		 pdev->device == PCI_DEVICE_ID_INTEL_BROXTON_M_XHCI)) {
 		xhci->quirks |= XHCI_PME_STUCK_QUIRK;
 	}
+	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
+		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI) {
+		xhci->quirks |= XHCI_SSIC_PORT_UNUSED;
+	}
 	if (pdev->vendor == PCI_VENDOR_ID_ETRON &&
 			pdev->device == PCI_DEVICE_ID_EJ168) {
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
@@ -313,22 +320,20 @@ static void xhci_pci_remove(struct pci_dev *dev)
  * SSIC PORT need to be marked as "unused" before putting xHCI
  * into D3. After D3 exit, the SSIC port need to be marked as "used".
  * Without this change, xHCI might not enter D3 state.
- * Make sure PME works on some Intel xHCI controllers by writing 1 to clear
- * the Internal PME flag bit in vendor specific PMCTRL register at offset 0x80a4
  */
-static void xhci_pme_quirk(struct usb_hcd *hcd, bool suspend)
+static void xhci_ssic_port_unused_quirk(struct usb_hcd *hcd, bool suspend)
 {
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
-	struct pci_dev		*pdev = to_pci_dev(hcd->self.controller);
 	u32 val;
 	void __iomem *reg;
+	int i;
 
-	if (pdev->vendor == PCI_VENDOR_ID_INTEL &&
-		 pdev->device == PCI_DEVICE_ID_INTEL_CHERRYVIEW_XHCI) {
-
-		reg = (void __iomem *) xhci->cap_regs + PORT2_SSIC_CONFIG_REG2;
+	for (i = 0; i < SSIC_PORT_NUM; i++) {
+		reg = (void __iomem *) xhci->cap_regs +
+				SSIC_PORT_CFG2 +
+				i * SSIC_PORT_CFG2_OFFSET;
 
-		/* Notify SSIC that SSIC profile programming is not done */
+		/* Notify SSIC that SSIC profile programming is not done. */
 		val = readl(reg) & ~PROG_DONE;
 		writel(val, reg);
 
@@ -345,6 +350,17 @@ static void xhci_pme_quirk(struct usb_hcd *hcd, bool suspend)
 		writel(val, reg);
 		readl(reg);
 	}
+}
+
+/*
+ * Make sure PME works on some Intel xHCI controllers by writing 1 to clear
+ * the Internal PME flag bit in vendor specific PMCTRL register at offset 0x80a4
+ */
+static void xhci_pme_quirk(struct usb_hcd *hcd)
+{
+	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
+	void __iomem *reg;
+	u32 val;
 
 	reg = (void __iomem *) xhci->cap_regs + 0x80a4;
 	val = readl(reg);
@@ -356,6 +372,7 @@ static int xhci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 {
 	struct xhci_hcd	*xhci = hcd_to_xhci(hcd);
 	struct pci_dev		*pdev = to_pci_dev(hcd->self.controller);
+	int			ret;
 
 	/*
 	 * Systems with the TI redriver that loses port status change events
@@ -365,9 +382,16 @@ static int xhci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 		pdev->no_d3cold = true;
 
 	if (xhci->quirks & XHCI_PME_STUCK_QUIRK)
-		xhci_pme_quirk(hcd, true);
+		xhci_pme_quirk(hcd);
+
+	if (xhci->quirks & XHCI_SSIC_PORT_UNUSED)
+		xhci_ssic_port_unused_quirk(hcd, true);
 
-	return xhci_suspend(xhci, do_wakeup);
+	ret = xhci_suspend(xhci, do_wakeup);
+	if (ret && (xhci->quirks & XHCI_SSIC_PORT_UNUSED))
+		xhci_ssic_port_unused_quirk(hcd, false);
+
+	return ret;
 }
 
 static int xhci_pci_resume(struct usb_hcd *hcd, bool hibernated)
@@ -397,8 +421,11 @@ static int xhci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL)
 		usb_enable_intel_xhci_ports(pdev);
 
+	if (xhci->quirks & XHCI_SSIC_PORT_UNUSED)
+		xhci_ssic_port_unused_quirk(hcd, false);
+
 	if (xhci->quirks & XHCI_PME_STUCK_QUIRK)
-		xhci_pme_quirk(hcd, false);
+		xhci_pme_quirk(hcd);
 
 	retval = xhci_resume(xhci, hibernated);
 	return retval;
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 05647e6753cd..d39d6bf1d090 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -22,6 +22,7 @@
 #include <linux/acpi.h>
 
 #include "xhci.h"
+#include "xhci-plat.h"
 #include "xhci-mvebu.h"
 #include "xhci-rcar.h"
 
@@ -31,7 +32,7 @@ static int xhci_plat_setup(struct usb_hcd *hcd);
 static int xhci_plat_start(struct usb_hcd *hcd);
 
 static const struct xhci_driver_overrides xhci_plat_overrides __initconst = {
-	.extra_priv_size = sizeof(struct xhci_hcd),
+	.extra_priv_size = sizeof(struct xhci_plat_priv),
 	.reset = xhci_plat_setup,
 	.start = xhci_plat_start,
 };
@@ -49,11 +50,10 @@ static void xhci_plat_quirks(struct device *dev, struct xhci_hcd *xhci)
 /* called during probe() after chip reset completes */
 static int xhci_plat_setup(struct usb_hcd *hcd)
 {
-	struct device_node *of_node = hcd->self.controller->of_node;
 	int ret;
 
-	if (of_device_is_compatible(of_node, "renesas,xhci-r8a7790") ||
-	    of_device_is_compatible(of_node, "renesas,xhci-r8a7791")) {
+	if (xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_RENESAS_RCAR_GEN2) ||
+	    xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_RENESAS_RCAR_GEN3)) {
 		ret = xhci_rcar_init_quirk(hcd);
 		if (ret)
 			return ret;
@@ -64,19 +64,62 @@ static int xhci_plat_setup(struct usb_hcd *hcd)
 
 static int xhci_plat_start(struct usb_hcd *hcd)
 {
-	struct device_node *of_node = hcd->self.controller->of_node;
-
-	if (of_device_is_compatible(of_node, "renesas,xhci-r8a7790") ||
-	    of_device_is_compatible(of_node, "renesas,xhci-r8a7791"))
+	if (xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_RENESAS_RCAR_GEN2) ||
+	    xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_RENESAS_RCAR_GEN3))
 		xhci_rcar_start(hcd);
 
 	return xhci_run(hcd);
 }
 
+#ifdef CONFIG_OF
+static const struct xhci_plat_priv xhci_plat_marvell_armada = {
+	.type = XHCI_PLAT_TYPE_MARVELL_ARMADA,
+};
+
+static const struct xhci_plat_priv xhci_plat_renesas_rcar_gen2 = {
+	.type = XHCI_PLAT_TYPE_RENESAS_RCAR_GEN2,
+	.firmware_name = XHCI_RCAR_FIRMWARE_NAME_V1,
+};
+
+static const struct xhci_plat_priv xhci_plat_renesas_rcar_gen3 = {
+	.type = XHCI_PLAT_TYPE_RENESAS_RCAR_GEN3,
+	.firmware_name = XHCI_RCAR_FIRMWARE_NAME_V2,
+};
+
+static const struct of_device_id usb_xhci_of_match[] = {
+	{
+		.compatible = "generic-xhci",
+	}, {
+		.compatible = "xhci-platform",
+	}, {
+		.compatible = "marvell,armada-375-xhci",
+		.data = &xhci_plat_marvell_armada,
+	}, {
+		.compatible = "marvell,armada-380-xhci",
+		.data = &xhci_plat_marvell_armada,
+	}, {
+		.compatible = "renesas,xhci-r8a7790",
+		.data = &xhci_plat_renesas_rcar_gen2,
+	}, {
+		.compatible = "renesas,xhci-r8a7791",
+		.data = &xhci_plat_renesas_rcar_gen2,
+	}, {
+		.compatible = "renesas,xhci-r8a7793",
+		.data = &xhci_plat_renesas_rcar_gen2,
+	}, {
+		.compatible = "renesas,xhci-r8a7795",
+		.data = &xhci_plat_renesas_rcar_gen3,
+	}, {
+	},
+};
+MODULE_DEVICE_TABLE(of, usb_xhci_of_match);
+#endif
+
 static int xhci_plat_probe(struct platform_device *pdev)
 {
 	struct device_node	*node = pdev->dev.of_node;
 	struct usb_xhci_pdata	*pdata = dev_get_platdata(&pdev->dev);
+	const struct of_device_id *match;
 	const struct hc_driver	*driver;
 	struct xhci_hcd		*xhci;
 	struct resource         *res;
@@ -134,10 +177,18 @@ static int xhci_plat_probe(struct platform_device *pdev)
 			goto put_hcd;
 	}
 
-	if (of_device_is_compatible(pdev->dev.of_node,
-				    "marvell,armada-375-xhci") ||
-	    of_device_is_compatible(pdev->dev.of_node,
-				    "marvell,armada-380-xhci")) {
+	xhci = hcd_to_xhci(hcd);
+	match = of_match_node(usb_xhci_of_match, node);
+	if (match) {
+		const struct xhci_plat_priv *priv_match = match->data;
+		struct xhci_plat_priv *priv = hcd_to_xhci_priv(hcd);
+
+		/* Just copy data for now */
+		if (priv_match)
+			*priv = *priv_match;
+	}
+
+	if (xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_MARVELL_ARMADA)) {
 		ret = xhci_mvebu_mbus_init_quirk(pdev);
 		if (ret)
 			goto disable_clk;
@@ -145,7 +196,6 @@ static int xhci_plat_probe(struct platform_device *pdev)
 
 	device_wakeup_enable(hcd->self.controller);
 
-	xhci = hcd_to_xhci(hcd);
 	xhci->clk = clk;
 	xhci->main_hcd = hcd;
 	xhci->shared_hcd = usb_create_shared_hcd(driver, &pdev->dev,
@@ -256,19 +306,6 @@ static const struct dev_pm_ops xhci_plat_pm_ops = {
 #define DEV_PM_OPS	NULL
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_OF
-static const struct of_device_id usb_xhci_of_match[] = {
-	{ .compatible = "generic-xhci" },
-	{ .compatible = "xhci-platform" },
-	{ .compatible = "marvell,armada-375-xhci"},
-	{ .compatible = "marvell,armada-380-xhci"},
-	{ .compatible = "renesas,xhci-r8a7790"},
-	{ .compatible = "renesas,xhci-r8a7791"},
-	{ },
-};
-MODULE_DEVICE_TABLE(of, usb_xhci_of_match);
-#endif
-
 static const struct acpi_device_id usb_xhci_acpi_match[] = {
 	/* XHCI-compliant USB Controller */
 	{ "PNP0D10", },
diff --git a/drivers/usb/host/xhci-plat.h b/drivers/usb/host/xhci-plat.h
new file mode 100644
index 000000000000..5a2e2e3936c4
--- /dev/null
+++ b/drivers/usb/host/xhci-plat.h
@@ -0,0 +1,39 @@
+/*
+ * xhci-plat.h - xHCI host controller driver platform Bus Glue.
+ *
+ * Copyright (C) 2015 Renesas Electronics Corporation
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#ifndef _XHCI_PLAT_H
+#define _XHCI_PLAT_H
+
+#include "xhci.h"	/* for hcd_to_xhci() */
+
+enum xhci_plat_type {
+	XHCI_PLAT_TYPE_MARVELL_ARMADA,
+	XHCI_PLAT_TYPE_RENESAS_RCAR_GEN2,
+	XHCI_PLAT_TYPE_RENESAS_RCAR_GEN3,
+};
+
+struct xhci_plat_priv {
+	enum xhci_plat_type type;
+	const char *firmware_name;
+};
+
+#define hcd_to_xhci_priv(h) ((struct xhci_plat_priv *)hcd_to_xhci(h)->priv)
+
+static inline bool xhci_plat_type_is(struct usb_hcd *hcd,
+				     enum xhci_plat_type type)
+{
+	struct xhci_plat_priv *priv = hcd_to_xhci_priv(hcd);
+
+	if (priv && priv->type == type)
+		return true;
+	else
+		return false;
+}
+#endif	/* _XHCI_PLAT_H */
diff --git a/drivers/usb/host/xhci-rcar.c b/drivers/usb/host/xhci-rcar.c
index ff0d1b44ea58..623100e9385e 100644
--- a/drivers/usb/host/xhci-rcar.c
+++ b/drivers/usb/host/xhci-rcar.c
@@ -14,10 +14,17 @@
 #include <linux/usb/phy.h>
 
 #include "xhci.h"
+#include "xhci-plat.h"
 #include "xhci-rcar.h"
 
-#define FIRMWARE_NAME		"r8a779x_usb3_v1.dlmem"
-MODULE_FIRMWARE(FIRMWARE_NAME);
+/*
+* - The V2 firmware is possible to use on R-Car Gen2. However, the V2 causes
+*   performance degradation. So, this driver continues to use the V1 if R-Car
+*   Gen2.
+* - The V1 firmware is impossible to use on R-Car Gen3.
+*/
+MODULE_FIRMWARE(XHCI_RCAR_FIRMWARE_NAME_V1);
+MODULE_FIRMWARE(XHCI_RCAR_FIRMWARE_NAME_V2);
 
 /*** Register Offset ***/
 #define RCAR_USB3_INT_ENA	0x224	/* Interrupt Enable */
@@ -56,6 +63,19 @@ MODULE_FIRMWARE(FIRMWARE_NAME);
 #define RCAR_USB3_RX_POL_VAL	BIT(21)
 #define RCAR_USB3_TX_POL_VAL	BIT(4)
 
+static void xhci_rcar_start_gen2(struct usb_hcd *hcd)
+{
+	/* LCLK Select */
+	writel(RCAR_USB3_LCLK_ENA_VAL, hcd->regs + RCAR_USB3_LCLK);
+	/* USB3.0 Configuration */
+	writel(RCAR_USB3_CONF1_VAL, hcd->regs + RCAR_USB3_CONF1);
+	writel(RCAR_USB3_CONF2_VAL, hcd->regs + RCAR_USB3_CONF2);
+	writel(RCAR_USB3_CONF3_VAL, hcd->regs + RCAR_USB3_CONF3);
+	/* USB3.0 Polarity */
+	writel(RCAR_USB3_RX_POL_VAL, hcd->regs + RCAR_USB3_RX_POL);
+	writel(RCAR_USB3_TX_POL_VAL, hcd->regs + RCAR_USB3_TX_POL);
+}
+
 void xhci_rcar_start(struct usb_hcd *hcd)
 {
 	u32 temp;
@@ -65,27 +85,23 @@ void xhci_rcar_start(struct usb_hcd *hcd)
 		temp = readl(hcd->regs + RCAR_USB3_INT_ENA);
 		temp |= RCAR_USB3_INT_ENA_VAL;
 		writel(temp, hcd->regs + RCAR_USB3_INT_ENA);
-		/* LCLK Select */
-		writel(RCAR_USB3_LCLK_ENA_VAL, hcd->regs + RCAR_USB3_LCLK);
-		/* USB3.0 Configuration */
-		writel(RCAR_USB3_CONF1_VAL, hcd->regs + RCAR_USB3_CONF1);
-		writel(RCAR_USB3_CONF2_VAL, hcd->regs + RCAR_USB3_CONF2);
-		writel(RCAR_USB3_CONF3_VAL, hcd->regs + RCAR_USB3_CONF3);
-		/* USB3.0 Polarity */
-		writel(RCAR_USB3_RX_POL_VAL, hcd->regs + RCAR_USB3_RX_POL);
-		writel(RCAR_USB3_TX_POL_VAL, hcd->regs + RCAR_USB3_TX_POL);
+		if (xhci_plat_type_is(hcd, XHCI_PLAT_TYPE_RENESAS_RCAR_GEN2))
+			xhci_rcar_start_gen2(hcd);
 	}
 }
 
-static int xhci_rcar_download_firmware(struct device *dev, void __iomem *regs)
+static int xhci_rcar_download_firmware(struct usb_hcd *hcd)
 {
+	struct device *dev = hcd->self.controller;
+	void __iomem *regs = hcd->regs;
+	struct xhci_plat_priv *priv = hcd_to_xhci_priv(hcd);
 	const struct firmware *fw;
 	int retval, index, j, time;
 	int timeout = 10000;
 	u32 data, val, temp;
 
 	/* request R-Car USB3.0 firmware */
-	retval = request_firmware(&fw, FIRMWARE_NAME, dev);
+	retval = request_firmware(&fw, priv->firmware_name, dev);
 	if (retval)
 		return retval;
 
@@ -144,5 +160,5 @@ int xhci_rcar_init_quirk(struct usb_hcd *hcd)
 	if (!hcd->regs)
 		return 0;
 
-	return xhci_rcar_download_firmware(hcd->self.controller, hcd->regs);
+	return xhci_rcar_download_firmware(hcd);
 }
diff --git a/drivers/usb/host/xhci-rcar.h b/drivers/usb/host/xhci-rcar.h
index 58501256715d..2941a25cfe98 100644
--- a/drivers/usb/host/xhci-rcar.h
+++ b/drivers/usb/host/xhci-rcar.h
@@ -11,6 +11,9 @@
 #ifndef _XHCI_RCAR_H
 #define _XHCI_RCAR_H
 
+#define XHCI_RCAR_FIRMWARE_NAME_V1	"r8a779x_usb3_v1.dlmem"
+#define XHCI_RCAR_FIRMWARE_NAME_V2	"r8a779x_usb3_v2.dlmem"
+
 #if IS_ENABLED(CONFIG_USB_XHCI_RCAR)
 void xhci_rcar_start(struct usb_hcd *hcd);
 int xhci_rcar_init_quirk(struct usb_hcd *hcd);
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index eeaa6c6bd540..3915657e6078 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -68,6 +68,7 @@
 #include <linux/slab.h>
 #include "xhci.h"
 #include "xhci-trace.h"
+#include "xhci-mtk.h"
 
 /*
  * Returns zero if the TRB isn't in this segment, otherwise it returns the DMA
@@ -2192,10 +2193,6 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 		}
 	/* Fast path - was this the last TRB in the TD for this URB? */
 	} else if (event_trb == td->last_trb) {
-		if (td->urb_length_set && trb_comp_code == COMP_SHORT_TX)
-			return finish_td(xhci, td, event_trb, event, ep,
-					 status, false);
-
 		if (EVENT_TRB_LEN(le32_to_cpu(event->transfer_len)) != 0) {
 			td->urb->actual_length =
 				td->urb->transfer_buffer_length -
@@ -2247,12 +2244,6 @@ static int process_bulk_intr_td(struct xhci_hcd *xhci, struct xhci_td *td,
 			td->urb->actual_length +=
 				TRB_LEN(le32_to_cpu(cur_trb->generic.field[2])) -
 				EVENT_TRB_LEN(le32_to_cpu(event->transfer_len));
-
-		if (trb_comp_code == COMP_SHORT_TX) {
-			xhci_dbg(xhci, "mid bulk/intr SP, wait for last TRB event\n");
-			td->urb_length_set = true;
-			return 0;
-		}
 	}
 
 	return finish_td(xhci, td, event_trb, event, ep, status, false);
@@ -3075,17 +3066,22 @@ static u32 xhci_td_remainder(struct xhci_hcd *xhci, int transferred,
 {
 	u32 maxp, total_packet_count;
 
-	if (xhci->hci_version < 0x100)
+	/* MTK xHCI is mostly 0.97 but contains some features from 1.0 */
+	if (xhci->hci_version < 0x100 && !(xhci->quirks & XHCI_MTK_HOST))
 		return ((td_total_len - transferred) >> 10);
 
-	maxp = GET_MAX_PACKET(usb_endpoint_maxp(&urb->ep->desc));
-	total_packet_count = DIV_ROUND_UP(td_total_len, maxp);
-
 	/* One TRB with a zero-length data packet. */
 	if (num_trbs_left == 0 || (transferred == 0 && trb_buff_len == 0) ||
 	    trb_buff_len == td_total_len)
 		return 0;
 
+	/* for MTK xHCI, TD size doesn't include this TRB */
+	if (xhci->quirks & XHCI_MTK_HOST)
+		trb_buff_len = 0;
+
+	maxp = GET_MAX_PACKET(usb_endpoint_maxp(&urb->ep->desc));
+	total_packet_count = DIV_ROUND_UP(td_total_len, maxp);
+
 	/* Queueing functions don't count the current TRB into transferred */
 	return (total_packet_count - ((transferred + trb_buff_len) / maxp));
 }
@@ -3473,7 +3469,7 @@ int xhci_queue_ctrl_tx(struct xhci_hcd *xhci, gfp_t mem_flags,
 		field |= 0x1;
 
 	/* xHCI 1.0/1.1 6.4.1.2.1: Transfer Type field */
-	if (xhci->hci_version >= 0x100) {
+	if ((xhci->hci_version >= 0x100) || (xhci->quirks & XHCI_MTK_HOST)) {
 		if (urb->transfer_buffer_length > 0) {
 			if (setup->bRequestType & USB_DIR_IN)
 				field |= TRB_TX_TYPE(TRB_DATA_IN);
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index 3f912705dcef..0c8087d3c313 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -31,6 +31,7 @@
 
 #include "xhci.h"
 #include "xhci-trace.h"
+#include "xhci-mtk.h"
 
 #define DRIVER_AUTHOR "Sarah Sharp"
 #define DRIVER_DESC "'eXtensible' Host Controller (xHC) Driver"
@@ -634,7 +635,11 @@ int xhci_run(struct usb_hcd *hcd)
 			"// Set the interrupt modulation register");
 	temp = readl(&xhci->ir_set->irq_control);
 	temp &= ~ER_IRQ_INTERVAL_MASK;
-	temp |= (u32) 160;
+	/*
+	 * the increment interval is 8 times as much as that defined
+	 * in xHCI spec on MTK's controller
+	 */
+	temp |= (u32) ((xhci->quirks & XHCI_MTK_HOST) ? 20 : 160);
 	writel(temp, &xhci->ir_set->irq_control);
 
 	/* Set the HCD state before we enable the irqs */
@@ -1549,7 +1554,9 @@ int xhci_urb_dequeue(struct usb_hcd *hcd, struct urb *urb, int status)
 		xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
 				"HW died, freeing TD.");
 		urb_priv = urb->hcpriv;
-		for (i = urb_priv->td_cnt; i < urb_priv->length; i++) {
+		for (i = urb_priv->td_cnt;
+		     i < urb_priv->length && xhci->devs[urb->dev->slot_id];
+		     i++) {
 			td = urb_priv->td[i];
 			if (!list_empty(&td->td_list))
 				list_del_init(&td->td_list);
@@ -1698,6 +1705,9 @@ int xhci_drop_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
 
 	xhci_endpoint_zero(xhci, xhci->devs[udev->slot_id], ep);
 
+	if (xhci->quirks & XHCI_MTK_HOST)
+		xhci_mtk_drop_ep_quirk(hcd, udev, ep);
+
 	xhci_dbg(xhci, "drop ep 0x%x, slot id %d, new drop flags = %#x, new add flags = %#x\n",
 			(unsigned int) ep->desc.bEndpointAddress,
 			udev->slot_id,
@@ -1793,6 +1803,15 @@ int xhci_add_endpoint(struct usb_hcd *hcd, struct usb_device *udev,
 		return -ENOMEM;
 	}
 
+	if (xhci->quirks & XHCI_MTK_HOST) {
+		ret = xhci_mtk_add_ep_quirk(hcd, udev, ep);
+		if (ret < 0) {
+			xhci_free_or_cache_endpoint_ring(xhci,
+				virt_dev, ep_index);
+			return ret;
+		}
+	}
+
 	ctrl_ctx->add_flags |= cpu_to_le32(added_ctxs);
 	new_add_flags = le32_to_cpu(ctrl_ctx->add_flags);
 
@@ -4960,7 +4979,7 @@ EXPORT_SYMBOL_GPL(xhci_gen_setup);
 static const struct hc_driver xhci_hc_driver = {
 	.description =		"xhci-hcd",
 	.product_desc =		"xHCI Host Controller",
-	.hcd_priv_size =	sizeof(struct xhci_hcd *),
+	.hcd_priv_size =	sizeof(struct xhci_hcd),
 
 	/*
 	 * generic hardware linkage
@@ -5059,6 +5078,10 @@ static int __init xhci_hcd_init(void)
 	BUILD_BUG_ON(sizeof(struct xhci_intr_reg) != 8*32/8);
 	/* xhci_run_regs has eight fields and embeds 128 xhci_intr_regs */
 	BUILD_BUG_ON(sizeof(struct xhci_run_regs) != (8+8*128)*32/8);
+
+	if (usb_disabled())
+		return -ENODEV;
+
 	return 0;
 }
 
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index 0b9451250e33..cc651383ce5a 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1630,6 +1630,8 @@ struct xhci_hcd {
 /* For controllers with a broken beyond repair streams implementation */
 #define XHCI_BROKEN_STREAMS	(1 << 19)
 #define XHCI_PME_STUCK_QUIRK	(1 << 20)
+#define XHCI_MTK_HOST		(1 << 21)
+#define XHCI_SSIC_PORT_UNUSED	(1 << 22)
 	unsigned int		num_active_eps;
 	unsigned int		limit_active_eps;
 	/* There are two roothubs to keep track of bus suspend info for */
@@ -1656,6 +1658,9 @@ struct xhci_hcd {
 	u32			port_status_u0;
 /* Compliance Mode Timer Triggered every 2 seconds */
 #define COMP_MODE_RCVRY_MSECS 2000
+
+	/* platform-specific data -- must come last */
+	unsigned long		priv[0] __aligned(sizeof(s64));
 };
 
 /* Platform specific overrides to generic XHCI hc_driver ops */
diff --git a/drivers/usb/misc/sisusbvga/sisusb.c b/drivers/usb/misc/sisusbvga/sisusb.c
index 306d6852ebc7..8efbabacc84e 100644
--- a/drivers/usb/misc/sisusbvga/sisusb.c
+++ b/drivers/usb/misc/sisusbvga/sisusb.c
@@ -2825,21 +2825,7 @@ sisusb_lseek(struct file *file, loff_t offset, int orig)
 		return -ENODEV;
 	}
 
-	switch (orig) {
-		case 0:
-			file->f_pos = offset;
-			ret = file->f_pos;
-			/* never negative, no force_successful_syscall needed */
-			break;
-		case 1:
-			file->f_pos += offset;
-			ret = file->f_pos;
-			/* never negative, no force_successful_syscall needed */
-			break;
-		default:
-			/* seeking relative to "end of file" is not supported */
-			ret = -EINVAL;
-	}
+	ret = no_seek_end_llseek(file, offset, orig);
 
 	mutex_unlock(&sisusb->lock);
 	return ret;
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index 637f3f7cfce8..92fdb6e9faff 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -22,18 +22,42 @@ static void complicated_callback(struct urb *urb);
 /*-------------------------------------------------------------------------*/
 
 /* FIXME make these public somewhere; usbdevfs.h? */
-struct usbtest_param {
+
+/* Parameter for usbtest driver. */
+struct usbtest_param_32 {
 	/* inputs */
-	unsigned		test_num;	/* 0..(TEST_CASES-1) */
-	unsigned		iterations;
-	unsigned		length;
-	unsigned		vary;
-	unsigned		sglen;
+	__u32		test_num;	/* 0..(TEST_CASES-1) */
+	__u32		iterations;
+	__u32		length;
+	__u32		vary;
+	__u32		sglen;
 
 	/* outputs */
-	struct timeval		duration;
+	__s32		duration_sec;
+	__s32		duration_usec;
 };
-#define USBTEST_REQUEST	_IOWR('U', 100, struct usbtest_param)
+
+/*
+ * Compat parameter to the usbtest driver.
+ * This supports older user space binaries compiled with 64 bit compiler.
+ */
+struct usbtest_param_64 {
+	/* inputs */
+	__u32		test_num;	/* 0..(TEST_CASES-1) */
+	__u32		iterations;
+	__u32		length;
+	__u32		vary;
+	__u32		sglen;
+
+	/* outputs */
+	__s64		duration_sec;
+	__s64		duration_usec;
+};
+
+/* IOCTL interface to the driver. */
+#define USBTEST_REQUEST_32    _IOWR('U', 100, struct usbtest_param_32)
+/* COMPAT IOCTL interface to the driver. */
+#define USBTEST_REQUEST_64    _IOWR('U', 100, struct usbtest_param_64)
 
 /*-------------------------------------------------------------------------*/
 
@@ -1030,7 +1054,7 @@ struct ctrl_ctx {
 	unsigned		pending;
 	int			status;
 	struct urb		**urb;
-	struct usbtest_param	*param;
+	struct usbtest_param_32	*param;
 	int			last;
 };
 
@@ -1155,7 +1179,7 @@ error:
 }
 
 static int
-test_ctrl_queue(struct usbtest_dev *dev, struct usbtest_param *param)
+test_ctrl_queue(struct usbtest_dev *dev, struct usbtest_param_32 *param)
 {
 	struct usb_device	*udev = testdev_to_usbdev(dev);
 	struct urb		**urb;
@@ -1849,7 +1873,7 @@ static void complicated_callback(struct urb *urb)
 			goto done;
 		default:
 			dev_err(&ctx->dev->intf->dev,
-					"iso resubmit err %d\n",
+					"resubmit err %d\n",
 					status);
 			/* FALLTHROUGH */
 		case -ENODEV:			/* disconnected */
@@ -1863,7 +1887,7 @@ static void complicated_callback(struct urb *urb)
 	if (ctx->pending == 0) {
 		if (ctx->errors)
 			dev_err(&ctx->dev->intf->dev,
-				"iso test, %lu errors out of %lu\n",
+				"during the test, %lu errors out of %lu\n",
 				ctx->errors, ctx->packet_count);
 		complete(&ctx->done);
 	}
@@ -1930,7 +1954,7 @@ static struct urb *iso_alloc_urb(
 }
 
 static int
-test_queue(struct usbtest_dev *dev, struct usbtest_param *param,
+test_queue(struct usbtest_dev *dev, struct usbtest_param_32 *param,
 		int pipe, struct usb_endpoint_descriptor *desc, unsigned offset)
 {
 	struct transfer_context	context;
@@ -2049,81 +2073,20 @@ static int test_unaligned_bulk(
 	return retval;
 }
 
-/*-------------------------------------------------------------------------*/
-
-/* We only have this one interface to user space, through usbfs.
- * User mode code can scan usbfs to find N different devices (maybe on
- * different busses) to use when testing, and allocate one thread per
- * test.  So discovery is simplified, and we have no device naming issues.
- *
- * Don't use these only as stress/load tests.  Use them along with with
- * other USB bus activity:  plugging, unplugging, mousing, mp3 playback,
- * video capture, and so on.  Run different tests at different times, in
- * different sequences.  Nothing here should interact with other devices,
- * except indirectly by consuming USB bandwidth and CPU resources for test
- * threads and request completion.  But the only way to know that for sure
- * is to test when HC queues are in use by many devices.
- *
- * WARNING:  Because usbfs grabs udev->dev.sem before calling this ioctl(),
- * it locks out usbcore in certain code paths.  Notably, if you disconnect
- * the device-under-test, hub_wq will wait block forever waiting for the
- * ioctl to complete ... so that usb_disconnect() can abort the pending
- * urbs and then call usbtest_disconnect().  To abort a test, you're best
- * off just killing the userspace task and waiting for it to exit.
- */
-
+/* Run tests. */
 static int
-usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
+usbtest_do_ioctl(struct usb_interface *intf, struct usbtest_param_32 *param)
 {
 	struct usbtest_dev	*dev = usb_get_intfdata(intf);
 	struct usb_device	*udev = testdev_to_usbdev(dev);
-	struct usbtest_param	*param = buf;
-	int			retval = -EOPNOTSUPP;
 	struct urb		*urb;
 	struct scatterlist	*sg;
 	struct usb_sg_request	req;
-	struct timeval		start;
 	unsigned		i;
-
-	/* FIXME USBDEVFS_CONNECTINFO doesn't say how fast the device is. */
-
-	pattern = mod_pattern;
-
-	if (code != USBTEST_REQUEST)
-		return -EOPNOTSUPP;
+	int	retval = -EOPNOTSUPP;
 
 	if (param->iterations <= 0)
 		return -EINVAL;
-
-	if (param->sglen > MAX_SGLEN)
-		return -EINVAL;
-
-	if (mutex_lock_interruptible(&dev->lock))
-		return -ERESTARTSYS;
-
-	/* FIXME: What if a system sleep starts while a test is running? */
-
-	/* some devices, like ez-usb default devices, need a non-default
-	 * altsetting to have any active endpoints.  some tests change
-	 * altsettings; force a default so most tests don't need to check.
-	 */
-	if (dev->info->alt >= 0) {
-		int	res;
-
-		if (intf->altsetting->desc.bInterfaceNumber) {
-			mutex_unlock(&dev->lock);
-			return -ENODEV;
-		}
-		res = set_altsetting(dev, dev->info->alt);
-		if (res) {
-			dev_err(&intf->dev,
-					"set altsetting to %d failed, %d\n",
-					dev->info->alt, res);
-			mutex_unlock(&dev->lock);
-			return res;
-		}
-	}
-
 	/*
 	 * Just a bunch of test cases that every HCD is expected to handle.
 	 *
@@ -2133,7 +2096,6 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 	 * FIXME add more tests!  cancel requests, verify the data, control
 	 * queueing, concurrent read+write threads, and so on.
 	 */
-	do_gettimeofday(&start);
 	switch (param->test_num) {
 
 	case 0:
@@ -2548,13 +2510,116 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 				dev->in_pipe, NULL, 0);
 		break;
 	}
-	do_gettimeofday(&param->duration);
-	param->duration.tv_sec -= start.tv_sec;
-	param->duration.tv_usec -= start.tv_usec;
-	if (param->duration.tv_usec < 0) {
-		param->duration.tv_usec += 1000 * 1000;
-		param->duration.tv_sec -= 1;
+	return retval;
+}
+
+/*-------------------------------------------------------------------------*/
+
+/* We only have this one interface to user space, through usbfs.
+ * User mode code can scan usbfs to find N different devices (maybe on
+ * different busses) to use when testing, and allocate one thread per
+ * test.  So discovery is simplified, and we have no device naming issues.
+ *
+ * Don't use these only as stress/load tests.  Use them along with with
+ * other USB bus activity:  plugging, unplugging, mousing, mp3 playback,
+ * video capture, and so on.  Run different tests at different times, in
+ * different sequences.  Nothing here should interact with other devices,
+ * except indirectly by consuming USB bandwidth and CPU resources for test
+ * threads and request completion.  But the only way to know that for sure
+ * is to test when HC queues are in use by many devices.
+ *
+ * WARNING:  Because usbfs grabs udev->dev.sem before calling this ioctl(),
+ * it locks out usbcore in certain code paths.  Notably, if you disconnect
+ * the device-under-test, hub_wq will wait block forever waiting for the
+ * ioctl to complete ... so that usb_disconnect() can abort the pending
+ * urbs and then call usbtest_disconnect().  To abort a test, you're best
+ * off just killing the userspace task and waiting for it to exit.
+ */
+
+static int
+usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
+{
+
+	struct usbtest_dev	*dev = usb_get_intfdata(intf);
+	struct usbtest_param_64 *param_64 = buf;
+	struct usbtest_param_32 temp;
+	struct usbtest_param_32 *param_32 = buf;
+	struct timespec64 start;
+	struct timespec64 end;
+	struct timespec64 duration;
+	int retval = -EOPNOTSUPP;
+
+	/* FIXME USBDEVFS_CONNECTINFO doesn't say how fast the device is. */
+
+	pattern = mod_pattern;
+
+	if (mutex_lock_interruptible(&dev->lock))
+		return -ERESTARTSYS;
+
+	/* FIXME: What if a system sleep starts while a test is running? */
+
+	/* some devices, like ez-usb default devices, need a non-default
+	 * altsetting to have any active endpoints.  some tests change
+	 * altsettings; force a default so most tests don't need to check.
+	 */
+	if (dev->info->alt >= 0) {
+		if (intf->altsetting->desc.bInterfaceNumber) {
+			retval = -ENODEV;
+			goto free_mutex;
+		}
+		retval = set_altsetting(dev, dev->info->alt);
+		if (retval) {
+			dev_err(&intf->dev,
+					"set altsetting to %d failed, %d\n",
+					dev->info->alt, retval);
+			goto free_mutex;
+		}
+	}
+
+	switch (code) {
+	case USBTEST_REQUEST_64:
+		temp.test_num = param_64->test_num;
+		temp.iterations = param_64->iterations;
+		temp.length = param_64->length;
+		temp.sglen = param_64->sglen;
+		temp.vary = param_64->vary;
+		param_32 = &temp;
+		break;
+
+	case USBTEST_REQUEST_32:
+		break;
+
+	default:
+		retval = -EOPNOTSUPP;
+		goto free_mutex;
+	}
+
+	ktime_get_ts64(&start);
+
+	retval = usbtest_do_ioctl(intf, param_32);
+	if (retval)
+		goto free_mutex;
+
+	ktime_get_ts64(&end);
+
+	duration = timespec64_sub(end, start);
+
+	temp.duration_sec = duration.tv_sec;
+	temp.duration_usec = duration.tv_nsec/NSEC_PER_USEC;
+
+	switch (code) {
+	case USBTEST_REQUEST_32:
+		param_32->duration_sec = temp.duration_sec;
+		param_32->duration_usec = temp.duration_usec;
+		break;
+
+	case USBTEST_REQUEST_64:
+		param_64->duration_sec = temp.duration_sec;
+		param_64->duration_usec = temp.duration_usec;
+		break;
 	}
+
+free_mutex:
 	mutex_unlock(&dev->lock);
 	return retval;
 }
diff --git a/drivers/usb/mon/mon_bin.c b/drivers/usb/mon/mon_bin.c
index 3598f1a62673..1a874a1f3890 100644
--- a/drivers/usb/mon/mon_bin.c
+++ b/drivers/usb/mon/mon_bin.c
@@ -18,6 +18,7 @@
 #include <linux/mm.h>
 #include <linux/scatterlist.h>
 #include <linux/slab.h>
+#include <linux/time64.h>
 
 #include <asm/uaccess.h>
 
@@ -92,8 +93,8 @@ struct mon_bin_hdr {
 	unsigned short busnum;	/* Bus number */
 	char flag_setup;
 	char flag_data;
-	s64 ts_sec;		/* gettimeofday */
-	s32 ts_usec;		/* gettimeofday */
+	s64 ts_sec;		/* getnstimeofday64 */
+	s32 ts_usec;		/* getnstimeofday64 */
 	int status;
 	unsigned int len_urb;	/* Length of data (submitted or actual) */
 	unsigned int len_cap;	/* Delivered length */
@@ -483,7 +484,7 @@ static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
     char ev_type, int status)
 {
 	const struct usb_endpoint_descriptor *epd = &urb->ep->desc;
-	struct timeval ts;
+	struct timespec64 ts;
 	unsigned long flags;
 	unsigned int urb_length;
 	unsigned int offset;
@@ -494,7 +495,7 @@ static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
 	struct mon_bin_hdr *ep;
 	char data_tag = 0;
 
-	do_gettimeofday(&ts);
+	getnstimeofday64(&ts);
 
 	spin_lock_irqsave(&rp->b_lock, flags);
 
@@ -568,7 +569,7 @@ static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
 	ep->busnum = urb->dev->bus->busnum;
 	ep->id = (unsigned long) urb;
 	ep->ts_sec = ts.tv_sec;
-	ep->ts_usec = ts.tv_usec;
+	ep->ts_usec = ts.tv_nsec / NSEC_PER_USEC;
 	ep->status = status;
 	ep->len_urb = urb_length;
 	ep->len_cap = length + lendesc;
@@ -629,12 +630,12 @@ static void mon_bin_complete(void *data, struct urb *urb, int status)
 static void mon_bin_error(void *data, struct urb *urb, int error)
 {
 	struct mon_reader_bin *rp = data;
-	struct timeval ts;
+	struct timespec64 ts;
 	unsigned long flags;
 	unsigned int offset;
 	struct mon_bin_hdr *ep;
 
-	do_gettimeofday(&ts);
+	getnstimeofday64(&ts);
 
 	spin_lock_irqsave(&rp->b_lock, flags);
 
@@ -656,7 +657,7 @@ static void mon_bin_error(void *data, struct urb *urb, int error)
 	ep->busnum = urb->dev->bus->busnum;
 	ep->id = (unsigned long) urb;
 	ep->ts_sec = ts.tv_sec;
-	ep->ts_usec = ts.tv_usec;
+	ep->ts_usec = ts.tv_nsec / NSEC_PER_USEC;
 	ep->status = error;
 
 	ep->flag_setup = '-';
diff --git a/drivers/usb/mon/mon_main.c b/drivers/usb/mon/mon_main.c
index f7c292f4891e..fec3f1128fdc 100644
--- a/drivers/usb/mon/mon_main.c
+++ b/drivers/usb/mon/mon_main.c
@@ -241,7 +241,7 @@ static struct notifier_block mon_nb = {
 /*
  * Ops
  */
-static struct usb_mon_operations mon_ops_0 = {
+static const struct usb_mon_operations mon_ops_0 = {
 	.urb_submit =	mon_submit,
 	.urb_submit_error = mon_submit_error,
 	.urb_complete =	mon_complete,
diff --git a/drivers/usb/mon/mon_text.c b/drivers/usb/mon/mon_text.c
index ad408251d955..e59334b09c41 100644
--- a/drivers/usb/mon/mon_text.c
+++ b/drivers/usb/mon/mon_text.c
@@ -9,6 +9,7 @@
 #include <linux/usb.h>
 #include <linux/slab.h>
 #include <linux/time.h>
+#include <linux/ktime.h>
 #include <linux/export.h>
 #include <linux/mutex.h>
 #include <linux/debugfs.h>
@@ -176,12 +177,12 @@ static inline char mon_text_get_data(struct mon_event_text *ep, struct urb *urb,
 
 static inline unsigned int mon_get_timestamp(void)
 {
-	struct timeval tval;
+	struct timespec64 now;
 	unsigned int stamp;
 
-	do_gettimeofday(&tval);
-	stamp = tval.tv_sec & 0xFFF;	/* 2^32 = 4294967296. Limit to 4096s. */
-	stamp = stamp * 1000000 + tval.tv_usec;
+	ktime_get_ts64(&now);
+	stamp = now.tv_sec & 0xFFF;  /* 2^32 = 4294967296. Limit to 4096s. */
+	stamp = stamp * USEC_PER_SEC + now.tv_nsec / NSEC_PER_USEC;
 	return stamp;
 }
 
@@ -386,7 +387,8 @@ static ssize_t mon_text_read_t(struct file *file, char __user *buf,
 	struct mon_event_text *ep;
 	struct mon_text_ptr ptr;
 
-	if (IS_ERR(ep = mon_text_read_wait(rp, file)))
+	ep = mon_text_read_wait(rp, file);
+	if (IS_ERR(ep))
 		return PTR_ERR(ep);
 	mutex_lock(&rp->printf_lock);
 	ptr.cnt = 0;
@@ -413,7 +415,8 @@ static ssize_t mon_text_read_u(struct file *file, char __user *buf,
 	struct mon_event_text *ep;
 	struct mon_text_ptr ptr;
 
-	if (IS_ERR(ep = mon_text_read_wait(rp, file)))
+	ep = mon_text_read_wait(rp, file);
+	if (IS_ERR(ep))
 		return PTR_ERR(ep);
 	mutex_lock(&rp->printf_lock);
 	ptr.cnt = 0;
diff --git a/drivers/usb/musb/musb_core.c b/drivers/usb/musb/musb_core.c
index ee9ff7028b92..c3791a01ab31 100644
--- a/drivers/usb/musb/musb_core.c
+++ b/drivers/usb/musb/musb_core.c
@@ -1360,8 +1360,7 @@ static int ep_config_from_table(struct musb *musb)
 		break;
 	}
 
-	printk(KERN_DEBUG "%s: setup fifo_mode %d\n",
-			musb_driver_name, fifo_mode);
+	pr_debug("%s: setup fifo_mode %d\n", musb_driver_name, fifo_mode);
 
 
 done:
@@ -1390,7 +1389,7 @@ done:
 		musb->nr_endpoints = max(epn, musb->nr_endpoints);
 	}
 
-	printk(KERN_DEBUG "%s: %d/%d max ep, %d/%d memory\n",
+	pr_debug("%s: %d/%d max ep, %d/%d memory\n",
 			musb_driver_name,
 			n + 1, musb->config->num_eps * 2 - 1,
 			offset, (1 << (musb->config->ram_bits + 2)));
@@ -1491,8 +1490,7 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 	if (reg & MUSB_CONFIGDATA_SOFTCONE)
 		strcat(aInfo, ", SoftConn");
 
-	printk(KERN_DEBUG "%s: ConfigData=0x%02x (%s)\n",
-			musb_driver_name, reg, aInfo);
+	pr_debug("%s: ConfigData=0x%02x (%s)\n", musb_driver_name, reg, aInfo);
 
 	aDate[0] = 0;
 	if (MUSB_CONTROLLER_MHDRC == musb_type) {
@@ -1502,9 +1500,8 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 		musb->is_multipoint = 0;
 		type = "";
 #ifndef	CONFIG_USB_OTG_BLACKLIST_HUB
-		printk(KERN_ERR
-			"%s: kernel must blacklist external hubs\n",
-			musb_driver_name);
+		pr_err("%s: kernel must blacklist external hubs\n",
+		       musb_driver_name);
 #endif
 	}
 
@@ -1513,8 +1510,8 @@ static int musb_core_init(u16 musb_type, struct musb *musb)
 	snprintf(aRevision, 32, "%d.%d%s", MUSB_HWVERS_MAJOR(musb->hwvers),
 		MUSB_HWVERS_MINOR(musb->hwvers),
 		(musb->hwvers & MUSB_HWVERS_RC) ? "RC" : "");
-	printk(KERN_DEBUG "%s: %sHDRC RTL version %s %s\n",
-			musb_driver_name, type, aRevision, aDate);
+	pr_debug("%s: %sHDRC RTL version %s %s\n",
+		 musb_driver_name, type, aRevision, aDate);
 
 	/* configure ep0 */
 	musb_configure_ep0(musb);
@@ -1705,6 +1702,23 @@ EXPORT_SYMBOL_GPL(musb_dma_completion);
 #define use_dma			0
 #endif
 
+static void (*musb_phy_callback)(enum musb_vbus_id_status status);
+
+/*
+ * musb_mailbox - optional phy notifier function
+ * @status phy state change
+ *
+ * Optionally gets called from the USB PHY. Note that the USB PHY must be
+ * disabled at the point the phy_callback is registered or unregistered.
+ */
+void musb_mailbox(enum musb_vbus_id_status status)
+{
+	if (musb_phy_callback)
+		musb_phy_callback(status);
+
+};
+EXPORT_SYMBOL_GPL(musb_mailbox);
+
 /*-------------------------------------------------------------------------*/
 
 static ssize_t
@@ -2117,8 +2131,15 @@ musb_init_controller(struct device *dev, int nIrq, void __iomem *ctrl)
 		musb->xceiv->io_ops = &musb_ulpi_access;
 	}
 
+	if (musb->ops->phy_callback)
+		musb_phy_callback = musb->ops->phy_callback;
+
 	pm_runtime_get_sync(musb->controller);
 
+	status = usb_phy_init(musb->xceiv);
+	if (status < 0)
+		goto err_usb_phy_init;
+
 	if (use_dma && dev->dma_mask) {
 		musb->dma_controller =
 			musb_dma_controller_create(musb, musb->mregs);
@@ -2239,7 +2260,11 @@ fail3:
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
 	if (musb->dma_controller)
 		musb_dma_controller_destroy(musb->dma_controller);
+
 fail2_5:
+	usb_phy_shutdown(musb->xceiv);
+
+err_usb_phy_init:
 	pm_runtime_put_sync(musb->controller);
 
 fail2:
@@ -2295,10 +2320,13 @@ static int musb_remove(struct platform_device *pdev)
 	 */
 	musb_exit_debugfs(musb);
 	musb_shutdown(pdev);
+	musb_phy_callback = NULL;
 
 	if (musb->dma_controller)
 		musb_dma_controller_destroy(musb->dma_controller);
 
+	usb_phy_shutdown(musb->xceiv);
+
 	cancel_work_sync(&musb->irq_work);
 	cancel_delayed_work_sync(&musb->finish_resume_work);
 	cancel_delayed_work_sync(&musb->deassert_reset_work);
diff --git a/drivers/usb/musb/musb_core.h b/drivers/usb/musb/musb_core.h
index 2337d7a7d62d..fd215fb45fd4 100644
--- a/drivers/usb/musb/musb_core.h
+++ b/drivers/usb/musb/musb_core.h
@@ -168,6 +168,7 @@ struct musb_io;
  * @adjust_channel_params: pre check for standard dma channel_program func
  * @pre_root_reset_end: called before the root usb port reset flag gets cleared
  * @post_root_reset_end: called after the root usb port reset flag gets cleared
+ * @phy_callback: optional callback function for the phy to call
  */
 struct musb_platform_ops {
 
@@ -214,6 +215,7 @@ struct musb_platform_ops {
 				dma_addr_t *dma_addr, u32 *len);
 	void	(*pre_root_reset_end)(struct musb *musb);
 	void	(*post_root_reset_end)(struct musb *musb);
+	void	(*phy_callback)(enum musb_vbus_id_status status);
 };
 
 /*
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index 67ad630c86c9..87bd578799a8 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -353,9 +353,8 @@ static void txstate(struct musb *musb, struct musb_request *req)
 					 *	1	>0	Yes(FS bulk)
 					 */
 					if (!musb_ep->hb_mult ||
-						(musb_ep->hb_mult &&
-						 can_bulk_split(musb,
-						    musb_ep->type)))
+					    can_bulk_split(musb,
+							   musb_ep->type))
 						csr |= MUSB_TXCSR_AUTOSET;
 				}
 				csr &= ~MUSB_TXCSR_P_UNDERRUN;
diff --git a/drivers/usb/musb/musb_host.c b/drivers/usb/musb/musb_host.c
index 795a45b1b25b..58487a473521 100644
--- a/drivers/usb/musb/musb_host.c
+++ b/drivers/usb/musb/musb_host.c
@@ -662,7 +662,7 @@ static int musb_tx_dma_set_mode_mentor(struct dma_controller *dma,
 		csr &= ~(MUSB_TXCSR_AUTOSET | MUSB_TXCSR_DMAMODE);
 		csr |= MUSB_TXCSR_DMAENAB; /* against programmer's guide */
 	}
-	channel->desired_mode = mode;
+	channel->desired_mode = *mode;
 	musb_writew(epio, MUSB_TXCSR, csr);
 
 	return 0;
@@ -2003,10 +2003,8 @@ void musb_host_rx(struct musb *musb, u8 epnum)
 				qh->offset,
 				urb->transfer_buffer_length);
 
-			done = musb_rx_dma_in_inventra_cppi41(c, hw_ep, qh,
-							      urb, xfer_len,
-							      iso_err);
-			if (done)
+			if (musb_rx_dma_in_inventra_cppi41(c, hw_ep, qh, urb,
+							   xfer_len, iso_err))
 				goto finish;
 			else
 				dev_err(musb->controller, "error: rx_dma failed\n");
diff --git a/drivers/usb/musb/omap2430.c b/drivers/usb/musb/omap2430.c
index 1bd9232ff76f..c84e0322c108 100644
--- a/drivers/usb/musb/omap2430.c
+++ b/drivers/usb/musb/omap2430.c
@@ -36,7 +36,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/err.h>
 #include <linux/delay.h>
-#include <linux/usb/musb-omap.h>
+#include <linux/usb/musb.h>
 #include <linux/phy/omap_control_phy.h>
 #include <linux/of_platform.h>
 
@@ -46,7 +46,7 @@
 struct omap2430_glue {
 	struct device		*dev;
 	struct platform_device	*musb;
-	enum omap_musb_vbus_id_status status;
+	enum musb_vbus_id_status status;
 	struct work_struct	omap_musb_mailbox_work;
 	struct device		*control_otghs;
 };
@@ -234,7 +234,7 @@ static inline void omap2430_low_level_init(struct musb *musb)
 	musb_writel(musb->mregs, OTG_FORCESTDBY, l);
 }
 
-void omap_musb_mailbox(enum omap_musb_vbus_id_status status)
+static void omap2430_musb_mailbox(enum musb_vbus_id_status status)
 {
 	struct omap2430_glue	*glue = _glue;
 
@@ -251,7 +251,6 @@ void omap_musb_mailbox(enum omap_musb_vbus_id_status status)
 
 	schedule_work(&glue->omap_musb_mailbox_work);
 }
-EXPORT_SYMBOL_GPL(omap_musb_mailbox);
 
 static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 {
@@ -262,7 +261,7 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 	struct usb_otg *otg = musb->xceiv->otg;
 
 	switch (glue->status) {
-	case OMAP_MUSB_ID_GROUND:
+	case MUSB_ID_GROUND:
 		dev_dbg(dev, "ID GND\n");
 
 		otg->default_a = true;
@@ -276,7 +275,7 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 		}
 		break;
 
-	case OMAP_MUSB_VBUS_VALID:
+	case MUSB_VBUS_VALID:
 		dev_dbg(dev, "VBUS Connect\n");
 
 		otg->default_a = false;
@@ -287,8 +286,8 @@ static void omap_musb_set_mailbox(struct omap2430_glue *glue)
 		omap_control_usb_set_mode(glue->control_otghs, USB_MODE_DEVICE);
 		break;
 
-	case OMAP_MUSB_ID_FLOAT:
-	case OMAP_MUSB_VBUS_OFF:
+	case MUSB_ID_FLOAT:
+	case MUSB_VBUS_OFF:
 		dev_dbg(dev, "VBUS Disconnect\n");
 
 		musb->xceiv->last_event = USB_EVENT_NONE;
@@ -430,7 +429,7 @@ static int omap2430_musb_init(struct musb *musb)
 
 	setup_timer(&musb_idle_timer, musb_do_idle, (unsigned long) musb);
 
-	if (glue->status != OMAP_MUSB_UNKNOWN)
+	if (glue->status != MUSB_UNKNOWN)
 		omap_musb_set_mailbox(glue);
 
 	phy_init(musb->phy);
@@ -455,7 +454,7 @@ static void omap2430_musb_enable(struct musb *musb)
 
 	switch (glue->status) {
 
-	case OMAP_MUSB_ID_GROUND:
+	case MUSB_ID_GROUND:
 		omap_control_usb_set_mode(glue->control_otghs, USB_MODE_HOST);
 		if (data->interface_type != MUSB_INTERFACE_UTMI)
 			break;
@@ -474,7 +473,7 @@ static void omap2430_musb_enable(struct musb *musb)
 		}
 		break;
 
-	case OMAP_MUSB_VBUS_VALID:
+	case MUSB_VBUS_VALID:
 		omap_control_usb_set_mode(glue->control_otghs, USB_MODE_DEVICE);
 		break;
 
@@ -488,7 +487,7 @@ static void omap2430_musb_disable(struct musb *musb)
 	struct device *dev = musb->controller;
 	struct omap2430_glue *glue = dev_get_drvdata(dev->parent);
 
-	if (glue->status != OMAP_MUSB_UNKNOWN)
+	if (glue->status != MUSB_UNKNOWN)
 		omap_control_usb_set_mode(glue->control_otghs,
 			USB_MODE_DISCONNECT);
 }
@@ -520,6 +519,8 @@ static const struct musb_platform_ops omap2430_ops = {
 
 	.enable		= omap2430_musb_enable,
 	.disable	= omap2430_musb_disable,
+
+	.phy_callback	= omap2430_musb_mailbox,
 };
 
 static u64 omap2430_dmamask = DMA_BIT_MASK(32);
@@ -551,7 +552,7 @@ static int omap2430_probe(struct platform_device *pdev)
 
 	glue->dev			= &pdev->dev;
 	glue->musb			= musb;
-	glue->status			= OMAP_MUSB_UNKNOWN;
+	glue->status			= MUSB_UNKNOWN;
 	glue->control_otghs = ERR_PTR(-ENODEV);
 
 	if (np) {
@@ -663,8 +664,11 @@ static int omap2430_remove(struct platform_device *pdev)
 {
 	struct omap2430_glue		*glue = platform_get_drvdata(pdev);
 
+	pm_runtime_get_sync(glue->dev);
 	cancel_work_sync(&glue->omap_musb_mailbox_work);
 	platform_device_unregister(glue->musb);
+	pm_runtime_put_sync(glue->dev);
+	pm_runtime_disable(glue->dev);
 
 	return 0;
 }
diff --git a/drivers/usb/musb/ux500.c b/drivers/usb/musb/ux500.c
index b2685e75a683..3eaa4ba6867d 100644
--- a/drivers/usb/musb/ux500.c
+++ b/drivers/usb/musb/ux500.c
@@ -348,7 +348,9 @@ static int ux500_suspend(struct device *dev)
 	struct ux500_glue	*glue = dev_get_drvdata(dev);
 	struct musb		*musb = glue_to_musb(glue);
 
-	usb_phy_set_suspend(musb->xceiv, 1);
+	if (musb)
+		usb_phy_set_suspend(musb->xceiv, 1);
+
 	clk_disable_unprepare(glue->clk);
 
 	return 0;
@@ -366,7 +368,8 @@ static int ux500_resume(struct device *dev)
 		return ret;
 	}
 
-	usb_phy_set_suspend(musb->xceiv, 0);
+	if (musb)
+		usb_phy_set_suspend(musb->xceiv, 0);
 
 	return 0;
 }
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index 22e8ecb6bfbd..c6904742e2aa 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -66,6 +66,7 @@ config AM335X_PHY_USB
 	select USB_PHY
 	select AM335X_CONTROL_USB
 	select NOP_USB_XCEIV
+	select USB_COMMON
 	help
 	  This driver provides PHY support for that phy which part for the
 	  AM335x SoC.
@@ -186,19 +187,6 @@ config USB_MXS_PHY
 
 	  MXS Phy is used by some of the i.MX SoCs, for example imx23/28/6x.
 
-config USB_RCAR_PHY
-	tristate "Renesas R-Car USB PHY support"
-	depends on USB || USB_GADGET
-	depends on ARCH_R8A7778 || ARCH_R8A7779 || COMPILE_TEST
-	select USB_PHY
-	help
-	  Say Y here to add support for the Renesas R-Car USB common PHY driver.
-	  This chip is typically used as USB PHY for USB host, gadget.
-	  This driver supports R8A7778 and R8A7779.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called phy-rcar-usb.
-
 config USB_ULPI
 	bool "Generic ULPI Transceiver Driver"
 	depends on ARM || ARM64
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index 19c0dccbb116..b433e5d89be4 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -23,7 +23,6 @@ obj-$(CONFIG_USB_MSM_OTG)		+= phy-msm-usb.o
 obj-$(CONFIG_USB_QCOM_8X16_PHY)	+= phy-qcom-8x16-usb.o
 obj-$(CONFIG_USB_MV_OTG)		+= phy-mv-usb.o
 obj-$(CONFIG_USB_MXS_PHY)		+= phy-mxs-usb.o
-obj-$(CONFIG_USB_RCAR_PHY)		+= phy-rcar-usb.o
 obj-$(CONFIG_USB_ULPI)			+= phy-ulpi.o
 obj-$(CONFIG_USB_ULPI_VIEWPORT)		+= phy-ulpi-viewport.o
 obj-$(CONFIG_KEYSTONE_USB_PHY)		+= phy-keystone.o
diff --git a/drivers/usb/phy/phy-am335x-control.c b/drivers/usb/phy/phy-am335x-control.c
index 7b3035ff9434..42a1afe36a90 100644
--- a/drivers/usb/phy/phy-am335x-control.c
+++ b/drivers/usb/phy/phy-am335x-control.c
@@ -4,7 +4,8 @@
 #include <linux/of.h>
 #include <linux/io.h>
 #include <linux/delay.h>
-#include "am35x-phy-control.h"
+#include <linux/usb/otg.h>
+#include "phy-am335x-control.h"
 
 struct am335x_control_usb {
 	struct device *dev;
@@ -58,7 +59,8 @@ static void am335x_phy_wkup(struct  phy_control *phy_ctrl, u32 id, bool on)
 	spin_unlock(&usb_ctrl->lock);
 }
 
-static void am335x_phy_power(struct phy_control *phy_ctrl, u32 id, bool on)
+static void am335x_phy_power(struct phy_control *phy_ctrl, u32 id,
+				enum usb_dr_mode dr_mode, bool on)
 {
 	struct am335x_control_usb *usb_ctrl;
 	u32 val;
@@ -80,8 +82,14 @@ static void am335x_phy_power(struct phy_control *phy_ctrl, u32 id, bool on)
 
 	val = readl(usb_ctrl->phy_reg + reg);
 	if (on) {
-		val &= ~(USBPHY_CM_PWRDN | USBPHY_OTG_PWRDN);
-		val |= USBPHY_OTGVDET_EN | USBPHY_OTGSESSEND_EN;
+		if (dr_mode == USB_DR_MODE_HOST) {
+			val &= ~(USBPHY_CM_PWRDN | USBPHY_OTG_PWRDN |
+					USBPHY_OTGVDET_EN);
+			val |= USBPHY_OTGSESSEND_EN;
+		} else {
+			val &= ~(USBPHY_CM_PWRDN | USBPHY_OTG_PWRDN);
+			val |= USBPHY_OTGVDET_EN | USBPHY_OTGSESSEND_EN;
+		}
 	} else {
 		val |= USBPHY_CM_PWRDN | USBPHY_OTG_PWRDN;
 	}
diff --git a/drivers/usb/phy/am35x-phy-control.h b/drivers/usb/phy/phy-am335x-control.h
index b96594d1962c..e86b3165d69d 100644
--- a/drivers/usb/phy/am35x-phy-control.h
+++ b/drivers/usb/phy/phy-am335x-control.h
@@ -2,13 +2,15 @@
 #define _AM335x_PHY_CONTROL_H_
 
 struct phy_control {
-	void (*phy_power)(struct phy_control *phy_ctrl, u32 id, bool on);
+	void (*phy_power)(struct phy_control *phy_ctrl, u32 id,
+			enum usb_dr_mode dr_mode, bool on);
 	void (*phy_wkup)(struct phy_control *phy_ctrl, u32 id, bool on);
 };
 
-static inline void phy_ctrl_power(struct phy_control *phy_ctrl, u32 id, bool on)
+static inline void phy_ctrl_power(struct phy_control *phy_ctrl, u32 id,
+				enum usb_dr_mode dr_mode, bool on)
 {
-	phy_ctrl->phy_power(phy_ctrl, id, on);
+	phy_ctrl->phy_power(phy_ctrl, id, dr_mode, on);
 }
 
 static inline void phy_ctrl_wkup(struct phy_control *phy_ctrl, u32 id, bool on)
diff --git a/drivers/usb/phy/phy-am335x.c b/drivers/usb/phy/phy-am335x.c
index 90b67a4ca221..39b424f7f629 100644
--- a/drivers/usb/phy/phy-am335x.c
+++ b/drivers/usb/phy/phy-am335x.c
@@ -8,21 +8,23 @@
 #include <linux/regulator/consumer.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
+#include <linux/usb/of.h>
 
-#include "am35x-phy-control.h"
+#include "phy-am335x-control.h"
 #include "phy-generic.h"
 
 struct am335x_phy {
 	struct usb_phy_generic usb_phy_gen;
 	struct phy_control *phy_ctrl;
 	int id;
+	enum usb_dr_mode dr_mode;
 };
 
 static int am335x_init(struct usb_phy *phy)
 {
 	struct am335x_phy *am_phy = dev_get_drvdata(phy->dev);
 
-	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, true);
+	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, am_phy->dr_mode, true);
 	return 0;
 }
 
@@ -30,7 +32,7 @@ static void am335x_shutdown(struct usb_phy *phy)
 {
 	struct am335x_phy *am_phy = dev_get_drvdata(phy->dev);
 
-	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, false);
+	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, am_phy->dr_mode, false);
 }
 
 static int am335x_phy_probe(struct platform_device *pdev)
@@ -46,12 +48,15 @@ static int am335x_phy_probe(struct platform_device *pdev)
 	am_phy->phy_ctrl = am335x_get_phy_control(dev);
 	if (!am_phy->phy_ctrl)
 		return -EPROBE_DEFER;
+
 	am_phy->id = of_alias_get_id(pdev->dev.of_node, "phy");
 	if (am_phy->id < 0) {
 		dev_err(&pdev->dev, "Missing PHY id: %d\n", am_phy->id);
 		return am_phy->id;
 	}
 
+	am_phy->dr_mode = of_usb_get_dr_mode_by_phy(pdev->dev.of_node);
+
 	ret = usb_phy_gen_create_phy(dev, &am_phy->usb_phy_gen, NULL);
 	if (ret)
 		return ret;
@@ -75,7 +80,7 @@ static int am335x_phy_probe(struct platform_device *pdev)
 	 */
 
 	device_set_wakeup_enable(dev, false);
-	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, false);
+	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, am_phy->dr_mode, false);
 
 	return 0;
 }
@@ -105,7 +110,7 @@ static int am335x_phy_suspend(struct device *dev)
 	if (device_may_wakeup(dev))
 		phy_ctrl_wkup(am_phy->phy_ctrl, am_phy->id, true);
 
-	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, false);
+	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, am_phy->dr_mode, false);
 
 	return 0;
 }
@@ -115,7 +120,7 @@ static int am335x_phy_resume(struct device *dev)
 	struct platform_device	*pdev = to_platform_device(dev);
 	struct am335x_phy	*am_phy = platform_get_drvdata(pdev);
 
-	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, true);
+	phy_ctrl_power(am_phy->phy_ctrl, am_phy->id, am_phy->dr_mode, true);
 
 	if (device_may_wakeup(dev))
 		phy_ctrl_wkup(am_phy->phy_ctrl, am_phy->id, false);
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index 0d19a6d61a71..72b387d592c2 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -757,14 +757,8 @@ static int msm_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 	otg->host = host;
 	dev_dbg(otg->usb_phy->dev, "host driver registered w/ tranceiver\n");
 
-	/*
-	 * Kick the state machine work, if peripheral is not supported
-	 * or peripheral is already registered with us.
-	 */
-	if (motg->pdata->mode == USB_DR_MODE_HOST || otg->gadget) {
-		pm_runtime_get_sync(otg->usb_phy->dev);
-		schedule_work(&motg->sm_work);
-	}
+	pm_runtime_get_sync(otg->usb_phy->dev);
+	schedule_work(&motg->sm_work);
 
 	return 0;
 }
@@ -827,14 +821,8 @@ static int msm_otg_set_peripheral(struct usb_otg *otg,
 	dev_dbg(otg->usb_phy->dev,
 		"peripheral driver registered w/ tranceiver\n");
 
-	/*
-	 * Kick the state machine work, if host is not supported
-	 * or host is already registered with us.
-	 */
-	if (motg->pdata->mode == USB_DR_MODE_PERIPHERAL || otg->host) {
-		pm_runtime_get_sync(otg->usb_phy->dev);
-		schedule_work(&motg->sm_work);
-	}
+	pm_runtime_get_sync(otg->usb_phy->dev);
+	schedule_work(&motg->sm_work);
 
 	return 0;
 }
@@ -1599,6 +1587,8 @@ static int msm_otg_read_dt(struct platform_device *pdev, struct msm_otg *motg)
 						&motg->id.nb);
 		if (ret < 0) {
 			dev_err(&pdev->dev, "register ID notifier failed\n");
+			extcon_unregister_notifier(motg->vbus.extcon,
+						   EXTCON_USB, &motg->vbus.nb);
 			return ret;
 		}
 
@@ -1660,15 +1650,6 @@ static int msm_otg_probe(struct platform_device *pdev)
 	if (!motg)
 		return -ENOMEM;
 
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		if (!np)
-			return -ENXIO;
-		ret = msm_otg_read_dt(pdev, motg);
-		if (ret)
-			return ret;
-	}
-
 	motg->phy.otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
 				     GFP_KERNEL);
 	if (!motg->phy.otg)
@@ -1710,6 +1691,15 @@ static int msm_otg_probe(struct platform_device *pdev)
 	if (!motg->regs)
 		return -ENOMEM;
 
+	pdata = dev_get_platdata(&pdev->dev);
+	if (!pdata) {
+		if (!np)
+			return -ENXIO;
+		ret = msm_otg_read_dt(pdev, motg);
+		if (ret)
+			return ret;
+	}
+
 	/*
 	 * NOTE: The PHYs can be multiplexed between the chipidea controller
 	 * and the dwc3 controller, using a single bit. It is important that
@@ -1717,8 +1707,10 @@ static int msm_otg_probe(struct platform_device *pdev)
 	 */
 	if (motg->phy_number) {
 		phy_select = devm_ioremap_nocache(&pdev->dev, USB2_PHY_SEL, 4);
-		if (!phy_select)
-			return -ENOMEM;
+		if (!phy_select) {
+			ret = -ENOMEM;
+			goto unregister_extcon;
+		}
 		/* Enable second PHY with the OTG port */
 		writel(0x1, phy_select);
 	}
@@ -1728,7 +1720,8 @@ static int msm_otg_probe(struct platform_device *pdev)
 	motg->irq = platform_get_irq(pdev, 0);
 	if (motg->irq < 0) {
 		dev_err(&pdev->dev, "platform_get_irq failed\n");
-		return motg->irq;
+		ret = motg->irq;
+		goto unregister_extcon;
 	}
 
 	regs[0].supply = "vddcx";
@@ -1737,7 +1730,7 @@ static int msm_otg_probe(struct platform_device *pdev)
 
 	ret = devm_regulator_bulk_get(motg->phy.dev, ARRAY_SIZE(regs), regs);
 	if (ret)
-		return ret;
+		goto unregister_extcon;
 
 	motg->vddcx = regs[0].consumer;
 	motg->v3p3  = regs[1].consumer;
@@ -1834,6 +1827,12 @@ disable_clks:
 	clk_disable_unprepare(motg->clk);
 	if (!IS_ERR(motg->core_clk))
 		clk_disable_unprepare(motg->core_clk);
+unregister_extcon:
+	extcon_unregister_notifier(motg->id.extcon,
+				   EXTCON_USB_HOST, &motg->id.nb);
+	extcon_unregister_notifier(motg->vbus.extcon,
+				   EXTCON_USB, &motg->vbus.nb);
+
 	return ret;
 }
 
diff --git a/drivers/usb/phy/phy-mxs-usb.c b/drivers/usb/phy/phy-mxs-usb.c
index c2936dc48ca7..00bfea01be65 100644
--- a/drivers/usb/phy/phy-mxs-usb.c
+++ b/drivers/usb/phy/phy-mxs-usb.c
@@ -220,7 +220,7 @@ static int mxs_phy_hw_init(struct mxs_phy *mxs_phy)
 /* Return true if the vbus is there */
 static bool mxs_phy_get_vbus_status(struct mxs_phy *mxs_phy)
 {
-	unsigned int vbus_value;
+	unsigned int vbus_value = 0;
 
 	if (!mxs_phy->regmap_anatop)
 		return false;
diff --git a/drivers/usb/phy/phy-rcar-usb.c b/drivers/usb/phy/phy-rcar-usb.c
deleted file mode 100644
index 1e09b8377885..000000000000
--- a/drivers/usb/phy/phy-rcar-usb.c
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * Renesas R-Car USB phy driver
- *
- * Copyright (C) 2012-2013 Renesas Solutions Corp.
- * Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>
- * Copyright (C) 2013 Cogent Embedded, Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/usb/otg.h>
-#include <linux/platform_device.h>
-#include <linux/spinlock.h>
-#include <linux/module.h>
-#include <linux/platform_data/usb-rcar-phy.h>
-
-/* REGS block */
-#define USBPCTRL0	0x00
-#define USBPCTRL1	0x04
-#define USBST		0x08
-#define USBEH0		0x0C
-#define USBOH0		0x1C
-#define USBCTL0		0x58
-
-/* High-speed signal quality characteristic control registers (R8A7778 only) */
-#define HSQCTL1		0x24
-#define HSQCTL2		0x28
-
-/* USBPCTRL0 */
-#define OVC2		(1 << 10) /* (R8A7779 only)			*/
-				/* Switches the OVC input pin for port 2: */
-				/* 1: USB_OVC2, 0: OVC2			*/
-#define OVC1_VBUS1	(1 << 9) /* Switches the OVC input pin for port 1: */
-				/* 1: USB_OVC1, 0: OVC1/VBUS1		*/
-				/* Function mode: set to 0		*/
-#define OVC0		(1 << 8) /* Switches the OVC input pin for port 0: */
-				/* 1: USB_OVC0 pin, 0: OVC0		*/
-#define OVC2_ACT 	(1 << 6) /* (R8A7779 only)			*/
-				/* Host mode: OVC2 polarity:		*/
-				/* 1: active-high, 0: active-low	*/
-#define PENC		(1 << 4) /* Function mode: output level of PENC1 pin: */
-				/* 1: high, 0: low			*/
-#define OVC0_ACT 	(1 << 3) /* Host mode: OVC0 polarity:		*/
-				/* 1: active-high, 0: active-low	*/
-#define OVC1_ACT	(1 << 1) /* Host mode: OVC1 polarity:		*/
-				/* 1: active-high, 0: active-low	*/
-				/* Function mode: be sure to set to 1	*/
-#define PORT1		(1 << 0) /* Selects port 1 mode:		*/
-				/* 1: function, 0: host			*/
-/* USBPCTRL1 */
-#define PHY_RST		(1 << 2)
-#define PLL_ENB		(1 << 1)
-#define PHY_ENB		(1 << 0)
-
-/* USBST */
-#define ST_ACT		(1 << 31)
-#define ST_PLL		(1 << 30)
-
-struct rcar_usb_phy_priv {
-	struct usb_phy phy;
-	spinlock_t lock;
-
-	void __iomem *reg0;
-	void __iomem *reg1;
-	int counter;
-};
-
-#define usb_phy_to_priv(p) container_of(p, struct rcar_usb_phy_priv, phy)
-
-
-/*
- * USB initial/install operation.
- *
- * This function setup USB phy.
- * The used value and setting order came from
- * [USB :: Initial setting] on datasheet.
- */
-static int rcar_usb_phy_init(struct usb_phy *phy)
-{
-	struct rcar_usb_phy_priv *priv = usb_phy_to_priv(phy);
-	struct device *dev = phy->dev;
-	struct rcar_phy_platform_data *pdata = dev_get_platdata(dev);
-	void __iomem *reg0 = priv->reg0;
-	void __iomem *reg1 = priv->reg1;
-	static const u8 ovcn_act[] = { OVC0_ACT, OVC1_ACT, OVC2_ACT };
-	int i;
-	u32 val;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	if (priv->counter++ == 0) {
-
-		/*
-		 * USB phy start-up
-		 */
-
-		/* (1) USB-PHY standby release */
-		iowrite32(PHY_ENB, (reg0 + USBPCTRL1));
-
-		/* (2) start USB-PHY internal PLL */
-		iowrite32(PHY_ENB | PLL_ENB, (reg0 + USBPCTRL1));
-
-		/* (3) set USB-PHY in accord with the conditions of usage */
-		if (reg1) {
-			u32 hsqctl1 = pdata->ferrite_bead ? 0x41 : 0;
-			u32 hsqctl2 = pdata->ferrite_bead ? 0x0d : 7;
-
-			iowrite32(hsqctl1, reg1 + HSQCTL1);
-			iowrite32(hsqctl2, reg1 + HSQCTL2);
-		}
-
-		/* (4) USB module status check */
-		for (i = 0; i < 1024; i++) {
-			udelay(10);
-			val = ioread32(reg0 + USBST);
-			if (val == (ST_ACT | ST_PLL))
-				break;
-		}
-
-		if (val != (ST_ACT | ST_PLL)) {
-			dev_err(dev, "USB phy not ready\n");
-			goto phy_init_end;
-		}
-
-		/* (5) USB-PHY reset clear */
-		iowrite32(PHY_ENB | PLL_ENB | PHY_RST, (reg0 + USBPCTRL1));
-
-		/* Board specific port settings */
-		val = 0;
-		if (pdata->port1_func)
-			val |= PORT1;
-		if (pdata->penc1)
-			val |= PENC;
-		for (i = 0; i < 3; i++) {
-			/* OVCn bits follow each other in the right order */
-			if (pdata->ovc_pin[i].select_3_3v)
-				val |= OVC0 << i;
-			/* OVCn_ACT bits are spaced by irregular intervals */
-			if (pdata->ovc_pin[i].active_high)
-				val |= ovcn_act[i];
-		}
-		iowrite32(val, (reg0 + USBPCTRL0));
-
-		/*
-		 * Bus alignment settings
-		 */
-
-		/* (1) EHCI bus alignment (little endian) */
-		iowrite32(0x00000000, (reg0 + USBEH0));
-
-		/* (1) OHCI bus alignment (little endian) */
-		iowrite32(0x00000000, (reg0 + USBOH0));
-	}
-
-phy_init_end:
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
-static void rcar_usb_phy_shutdown(struct usb_phy *phy)
-{
-	struct rcar_usb_phy_priv *priv = usb_phy_to_priv(phy);
-	void __iomem *reg0 = priv->reg0;
-	unsigned long flags;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	if (priv->counter-- == 1)	/* last user */
-		iowrite32(0x00000000, (reg0 + USBPCTRL1));
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-}
-
-static int rcar_usb_phy_probe(struct platform_device *pdev)
-{
-	struct rcar_usb_phy_priv *priv;
-	struct resource *res0, *res1;
-	struct device *dev = &pdev->dev;
-	void __iomem *reg0, *reg1 = NULL;
-	int ret;
-
-	if (!dev_get_platdata(&pdev->dev)) {
-		dev_err(dev, "No platform data\n");
-		return -EINVAL;
-	}
-
-	res0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	reg0 = devm_ioremap_resource(dev, res0);
-	if (IS_ERR(reg0))
-		return PTR_ERR(reg0);
-
-	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	reg1 = devm_ioremap_resource(dev, res1);
-	if (IS_ERR(reg1))
-		return PTR_ERR(reg1);
-
-	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv)
-		return -ENOMEM;
-
-	priv->reg0		= reg0;
-	priv->reg1		= reg1;
-	priv->counter		= 0;
-	priv->phy.dev		= dev;
-	priv->phy.label		= dev_name(dev);
-	priv->phy.init		= rcar_usb_phy_init;
-	priv->phy.shutdown	= rcar_usb_phy_shutdown;
-	spin_lock_init(&priv->lock);
-
-	ret = usb_add_phy(&priv->phy, USB_PHY_TYPE_USB2);
-	if (ret < 0) {
-		dev_err(dev, "usb phy addition error\n");
-		return ret;
-	}
-
-	platform_set_drvdata(pdev, priv);
-
-	return ret;
-}
-
-static int rcar_usb_phy_remove(struct platform_device *pdev)
-{
-	struct rcar_usb_phy_priv *priv = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&priv->phy);
-
-	return 0;
-}
-
-static struct platform_driver rcar_usb_phy_driver = {
-	.driver		= {
-		.name	= "rcar_usb_phy",
-	},
-	.probe		= rcar_usb_phy_probe,
-	.remove		= rcar_usb_phy_remove,
-};
-
-module_platform_driver(rcar_usb_phy_driver);
-
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("Renesas R-Car USB phy");
-MODULE_AUTHOR("Kuninori Morimoto <kuninori.morimoto.gx@renesas.com>");
diff --git a/drivers/usb/phy/phy-twl6030-usb.c b/drivers/usb/phy/phy-twl6030-usb.c
index 12741856a75c..014dbbd72132 100644
--- a/drivers/usb/phy/phy-twl6030-usb.c
+++ b/drivers/usb/phy/phy-twl6030-usb.c
@@ -25,7 +25,7 @@
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/io.h>
-#include <linux/usb/musb-omap.h>
+#include <linux/usb/musb.h>
 #include <linux/usb/phy_companion.h>
 #include <linux/phy/omap_usb.h>
 #include <linux/i2c/twl.h>
@@ -102,7 +102,7 @@ struct twl6030_usb {
 
 	int			irq1;
 	int			irq2;
-	enum omap_musb_vbus_id_status linkstat;
+	enum musb_vbus_id_status linkstat;
 	u8			asleep;
 	bool			vbus_enable;
 	const char		*regulator;
@@ -189,13 +189,13 @@ static ssize_t twl6030_usb_vbus_show(struct device *dev,
 	spin_lock_irqsave(&twl->lock, flags);
 
 	switch (twl->linkstat) {
-	case OMAP_MUSB_VBUS_VALID:
+	case MUSB_VBUS_VALID:
 	       ret = snprintf(buf, PAGE_SIZE, "vbus\n");
 	       break;
-	case OMAP_MUSB_ID_GROUND:
+	case MUSB_ID_GROUND:
 	       ret = snprintf(buf, PAGE_SIZE, "id\n");
 	       break;
-	case OMAP_MUSB_VBUS_OFF:
+	case MUSB_VBUS_OFF:
 	       ret = snprintf(buf, PAGE_SIZE, "none\n");
 	       break;
 	default:
@@ -210,7 +210,7 @@ static DEVICE_ATTR(vbus, 0444, twl6030_usb_vbus_show, NULL);
 static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 {
 	struct twl6030_usb *twl = _twl;
-	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
+	enum musb_vbus_id_status status = MUSB_UNKNOWN;
 	u8 vbus_state, hw_state;
 	int ret;
 
@@ -225,14 +225,14 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 				dev_err(twl->dev, "Failed to enable usb3v3\n");
 
 			twl->asleep = 1;
-			status = OMAP_MUSB_VBUS_VALID;
+			status = MUSB_VBUS_VALID;
 			twl->linkstat = status;
-			omap_musb_mailbox(status);
+			musb_mailbox(status);
 		} else {
-			if (twl->linkstat != OMAP_MUSB_UNKNOWN) {
-				status = OMAP_MUSB_VBUS_OFF;
+			if (twl->linkstat != MUSB_UNKNOWN) {
+				status = MUSB_VBUS_OFF;
 				twl->linkstat = status;
-				omap_musb_mailbox(status);
+				musb_mailbox(status);
 				if (twl->asleep) {
 					regulator_disable(twl->usb3v3);
 					twl->asleep = 0;
@@ -248,7 +248,7 @@ static irqreturn_t twl6030_usb_irq(int irq, void *_twl)
 static irqreturn_t twl6030_usbotg_irq(int irq, void *_twl)
 {
 	struct twl6030_usb *twl = _twl;
-	enum omap_musb_vbus_id_status status = OMAP_MUSB_UNKNOWN;
+	enum musb_vbus_id_status status = MUSB_UNKNOWN;
 	u8 hw_state;
 	int ret;
 
@@ -262,9 +262,9 @@ static irqreturn_t twl6030_usbotg_irq(int irq, void *_twl)
 		twl->asleep = 1;
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x1, USB_ID_INT_EN_HI_CLR);
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x10, USB_ID_INT_EN_HI_SET);
-		status = OMAP_MUSB_ID_GROUND;
+		status = MUSB_ID_GROUND;
 		twl->linkstat = status;
-		omap_musb_mailbox(status);
+		musb_mailbox(status);
 	} else  {
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x10, USB_ID_INT_EN_HI_CLR);
 		twl6030_writeb(twl, TWL_MODULE_USB, 0x1, USB_ID_INT_EN_HI_SET);
@@ -334,7 +334,7 @@ static int twl6030_usb_probe(struct platform_device *pdev)
 	twl->dev		= &pdev->dev;
 	twl->irq1		= platform_get_irq(pdev, 0);
 	twl->irq2		= platform_get_irq(pdev, 1);
-	twl->linkstat		= OMAP_MUSB_UNKNOWN;
+	twl->linkstat		= MUSB_UNKNOWN;
 
 	twl->comparator.set_vbus	= twl6030_set_vbus;
 	twl->comparator.start_srp	= twl6030_start_srp;
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index d82fa36c3465..5af9ca5d54ab 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -302,37 +302,37 @@ static void usbhsc_set_buswait(struct usbhs_priv *priv)
  */
 
 /* commonly used on old SH-Mobile SoCs */
-static u32 usbhsc_default_pipe_type[] = {
-		USB_ENDPOINT_XFER_CONTROL,
-		USB_ENDPOINT_XFER_ISOC,
-		USB_ENDPOINT_XFER_ISOC,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_INT,
+static struct renesas_usbhs_driver_pipe_config usbhsc_default_pipe[] = {
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_CONTROL, 64, 0x00, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_ISOC, 1024, 0x08, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_ISOC, 1024, 0x18, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x28, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x38, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x48, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x04, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x05, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x06, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x07, false),
 };
 
 /* commonly used on newer SH-Mobile and R-Car SoCs */
-static u32 usbhsc_new_pipe_type[] = {
-		USB_ENDPOINT_XFER_CONTROL,
-		USB_ENDPOINT_XFER_ISOC,
-		USB_ENDPOINT_XFER_ISOC,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_INT,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
-		USB_ENDPOINT_XFER_BULK,
+static struct renesas_usbhs_driver_pipe_config usbhsc_new_pipe[] = {
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_CONTROL, 64, 0x00, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_ISOC, 1024, 0x08, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_ISOC, 1024, 0x28, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x48, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x58, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x68, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x04, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x05, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_INT, 64, 0x06, false),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x78, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x88, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0x98, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0xa8, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0xb8, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0xc8, true),
+	RENESAS_USBHS_PIPE(USB_ENDPOINT_XFER_BULK, 512, 0xd8, true),
 };
 
 /*
@@ -481,6 +481,15 @@ static const struct of_device_id usbhs_of_match[] = {
 		.compatible = "renesas,usbhs-r8a7795",
 		.data = (void *)USBHS_TYPE_RCAR_GEN2,
 	},
+	{
+		.compatible = "renesas,rcar-gen2-usbhs",
+		.data = (void *)USBHS_TYPE_RCAR_GEN2,
+	},
+	{
+		/* Gen3 is compatible with Gen2 */
+		.compatible = "renesas,rcar-gen3-usbhs",
+		.data = (void *)USBHS_TYPE_RCAR_GEN2,
+	},
 	{ },
 };
 MODULE_DEVICE_TABLE(of, usbhs_of_match);
@@ -564,10 +573,9 @@ static int usbhs_probe(struct platform_device *pdev)
 	switch (priv->dparam.type) {
 	case USBHS_TYPE_RCAR_GEN2:
 		priv->pfunc = usbhs_rcar2_ops;
-		if (!priv->dparam.pipe_type) {
-			priv->dparam.pipe_type = usbhsc_new_pipe_type;
-			priv->dparam.pipe_size =
-				ARRAY_SIZE(usbhsc_new_pipe_type);
+		if (!priv->dparam.pipe_configs) {
+			priv->dparam.pipe_configs = usbhsc_new_pipe;
+			priv->dparam.pipe_size = ARRAY_SIZE(usbhsc_new_pipe);
 		}
 		break;
 	default:
@@ -586,9 +594,9 @@ static int usbhs_probe(struct platform_device *pdev)
 	dfunc->notify_hotplug	= usbhsc_drvcllbck_notify_hotplug;
 
 	/* set default param if platform doesn't have */
-	if (!priv->dparam.pipe_type) {
-		priv->dparam.pipe_type = usbhsc_default_pipe_type;
-		priv->dparam.pipe_size = ARRAY_SIZE(usbhsc_default_pipe_type);
+	if (!priv->dparam.pipe_configs) {
+		priv->dparam.pipe_configs = usbhsc_default_pipe;
+		priv->dparam.pipe_size = ARRAY_SIZE(usbhsc_default_pipe);
 	}
 	if (!priv->dparam.pio_dma_border)
 		priv->dparam.pio_dma_border = 64; /* 64byte */
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 8f7a78e70975..657f9672ceba 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -1042,6 +1042,8 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 	struct usbhsg_gpriv *gpriv;
 	struct usbhsg_uep *uep;
 	struct device *dev = usbhs_priv_to_dev(priv);
+	struct renesas_usbhs_driver_pipe_config *pipe_configs =
+					usbhs_get_dparam(priv, pipe_configs);
 	int pipe_size = usbhs_get_dparam(priv, pipe_size);
 	int i;
 	int ret;
@@ -1111,13 +1113,16 @@ int usbhs_mod_gadget_probe(struct usbhs_priv *priv)
 			gpriv->gadget.ep0 = &uep->ep;
 			usb_ep_set_maxpacket_limit(&uep->ep, 64);
 			uep->ep.caps.type_control = true;
-		}
-		/* init normal pipe */
-		else {
-			usb_ep_set_maxpacket_limit(&uep->ep, 512);
-			uep->ep.caps.type_iso = true;
-			uep->ep.caps.type_bulk = true;
-			uep->ep.caps.type_int = true;
+		} else {
+			/* init normal pipe */
+			if (pipe_configs[i].type == USB_ENDPOINT_XFER_ISOC)
+				uep->ep.caps.type_iso = true;
+			if (pipe_configs[i].type == USB_ENDPOINT_XFER_BULK)
+				uep->ep.caps.type_bulk = true;
+			if (pipe_configs[i].type == USB_ENDPOINT_XFER_INT)
+				uep->ep.caps.type_int = true;
+			usb_ep_set_maxpacket_limit(&uep->ep,
+						   pipe_configs[i].bufsize);
 			list_add_tail(&uep->ep.ep_list, &gpriv->gadget.ep_list);
 		}
 		uep->ep.caps.dir_in = true;
diff --git a/drivers/usb/renesas_usbhs/mod_host.c b/drivers/usb/renesas_usbhs/mod_host.c
index bd050359926c..1a8e4c45c4c5 100644
--- a/drivers/usb/renesas_usbhs/mod_host.c
+++ b/drivers/usb/renesas_usbhs/mod_host.c
@@ -1414,7 +1414,8 @@ static void usbhsh_pipe_init_for_host(struct usbhs_priv *priv)
 {
 	struct usbhsh_hpriv *hpriv = usbhsh_priv_to_hpriv(priv);
 	struct usbhs_pipe *pipe;
-	u32 *pipe_type = usbhs_get_dparam(priv, pipe_type);
+	struct renesas_usbhs_driver_pipe_config *pipe_configs =
+					usbhs_get_dparam(priv, pipe_configs);
 	int pipe_size = usbhs_get_dparam(priv, pipe_size);
 	int old_type, dir_in, i;
 
@@ -1442,15 +1443,15 @@ static void usbhsh_pipe_init_for_host(struct usbhs_priv *priv)
 		 * USB_ENDPOINT_XFER_BULK -> dir in
 		 * ...
 		 */
-		dir_in = (pipe_type[i] == old_type);
-		old_type = pipe_type[i];
+		dir_in = (pipe_configs[i].type == old_type);
+		old_type = pipe_configs[i].type;
 
-		if (USB_ENDPOINT_XFER_CONTROL == pipe_type[i]) {
+		if (USB_ENDPOINT_XFER_CONTROL == pipe_configs[i].type) {
 			pipe = usbhs_dcp_malloc(priv);
 			usbhsh_hpriv_to_dcp(hpriv) = pipe;
 		} else {
 			pipe = usbhs_pipe_malloc(priv,
-						 pipe_type[i],
+						 pipe_configs[i].type,
 						 dir_in);
 		}
 
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index 4f9c3356127a..0e95d2925dc5 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -44,6 +44,15 @@ char *usbhs_pipe_name(struct usbhs_pipe *pipe)
 	return usbhsp_pipe_name[usbhs_pipe_type(pipe)];
 }
 
+static struct renesas_usbhs_driver_pipe_config
+*usbhsp_get_pipe_config(struct usbhs_priv *priv, int pipe_num)
+{
+	struct renesas_usbhs_driver_pipe_config *pipe_configs =
+					usbhs_get_dparam(priv, pipe_configs);
+
+	return &pipe_configs[pipe_num];
+}
+
 /*
  *		DCPCTR/PIPEnCTR functions
  */
@@ -384,18 +393,6 @@ void usbhs_pipe_set_trans_count_if_bulk(struct usbhs_pipe *pipe, int len)
 /*
  *		pipe setup
  */
-static int usbhsp_possible_double_buffer(struct usbhs_pipe *pipe)
-{
-	/*
-	 * only ISO / BULK pipe can use double buffer
-	 */
-	if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_BULK) ||
-	    usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_ISOC))
-		return 1;
-
-	return 0;
-}
-
 static u16 usbhsp_setup_pipecfg(struct usbhs_pipe *pipe,
 				int is_host,
 				int dir_in)
@@ -412,7 +409,6 @@ static u16 usbhsp_setup_pipecfg(struct usbhs_pipe *pipe,
 		[USB_ENDPOINT_XFER_INT]  = TYPE_INT,
 		[USB_ENDPOINT_XFER_ISOC] = TYPE_ISO,
 	};
-	int is_double = usbhsp_possible_double_buffer(pipe);
 
 	if (usbhs_pipe_is_dcp(pipe))
 		return -EINVAL;
@@ -434,10 +430,7 @@ static u16 usbhsp_setup_pipecfg(struct usbhs_pipe *pipe,
 	    usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_BULK))
 		bfre = 0; /* FIXME */
 
-	/* DBLB */
-	if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_ISOC) ||
-	    usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_BULK))
-		dblb = (is_double) ? DBLB : 0;
+	/* DBLB: see usbhs_pipe_config_update() */
 
 	/* CNTMD */
 	if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_BULK))
@@ -473,13 +466,13 @@ static u16 usbhsp_setup_pipecfg(struct usbhs_pipe *pipe,
 static u16 usbhsp_setup_pipebuff(struct usbhs_pipe *pipe)
 {
 	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
-	struct usbhs_pipe_info *info = usbhs_priv_to_pipeinfo(priv);
 	struct device *dev = usbhs_priv_to_dev(priv);
 	int pipe_num = usbhs_pipe_number(pipe);
-	int is_double = usbhsp_possible_double_buffer(pipe);
 	u16 buff_size;
 	u16 bufnmb;
 	u16 bufnmb_cnt;
+	struct renesas_usbhs_driver_pipe_config *pipe_config =
+					usbhsp_get_pipe_config(priv, pipe_num);
 
 	/*
 	 * PIPEBUF
@@ -489,56 +482,13 @@ static u16 usbhsp_setup_pipebuff(struct usbhs_pipe *pipe)
 	 *  - "Features"  - "Pipe configuration"
 	 *  - "Operation" - "FIFO Buffer Memory"
 	 *  - "Operation" - "Pipe Control"
-	 *
-	 * ex) if pipe6 - pipe9 are USB_ENDPOINT_XFER_INT (SH7724)
-	 *
-	 * BUFNMB:	PIPE
-	 * 0:		pipe0 (DCP 256byte)
-	 * 1:		-
-	 * 2:		-
-	 * 3:		-
-	 * 4:		pipe6 (INT 64byte)
-	 * 5:		pipe7 (INT 64byte)
-	 * 6:		pipe8 (INT 64byte)
-	 * 7:		pipe9 (INT 64byte)
-	 * 8 - xx:	free (for BULK, ISOC)
 	 */
-
-	/*
-	 * FIXME
-	 *
-	 * it doesn't have good buffer allocator
-	 *
-	 * DCP : 256 byte
-	 * BULK: 512 byte
-	 * INT :  64 byte
-	 * ISOC: 512 byte
-	 */
-	if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_CONTROL))
-		buff_size = 256;
-	else if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_INT))
-		buff_size = 64;
-	else
-		buff_size = 512;
+	buff_size = pipe_config->bufsize;
+	bufnmb = pipe_config->bufnum;
 
 	/* change buff_size to register value */
 	bufnmb_cnt = (buff_size / 64) - 1;
 
-	/* BUFNMB has been reserved for INT pipe
-	 * see above */
-	if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_INT)) {
-		bufnmb = pipe_num - 2;
-	} else {
-		bufnmb = info->bufnmb_last;
-		info->bufnmb_last += bufnmb_cnt + 1;
-
-		/*
-		 * double buffer
-		 */
-		if (is_double)
-			info->bufnmb_last += bufnmb_cnt + 1;
-	}
-
 	dev_dbg(dev, "pipe : %d : buff_size 0x%x: bufnmb 0x%x\n",
 		pipe_num, buff_size, bufnmb);
 
@@ -549,8 +499,13 @@ static u16 usbhsp_setup_pipebuff(struct usbhs_pipe *pipe)
 void usbhs_pipe_config_update(struct usbhs_pipe *pipe, u16 devsel,
 			      u16 epnum, u16 maxp)
 {
+	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	int pipe_num = usbhs_pipe_number(pipe);
+	struct renesas_usbhs_driver_pipe_config *pipe_config =
+					usbhsp_get_pipe_config(priv, pipe_num);
+	u16 dblb = pipe_config->double_buf ? DBLB : 0;
+
 	if (devsel > 0xA) {
-		struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
 		struct device *dev = usbhs_priv_to_dev(priv);
 
 		dev_err(dev, "devsel error %d\n", devsel);
@@ -568,7 +523,7 @@ void usbhs_pipe_config_update(struct usbhs_pipe *pipe, u16 devsel,
 			     maxp);
 
 	if (!usbhs_pipe_is_dcp(pipe))
-		usbhsp_pipe_cfg_set(pipe,  0x000F, epnum);
+		usbhsp_pipe_cfg_set(pipe,  0x000F | DBLB, epnum | dblb);
 }
 
 /*
@@ -708,23 +663,7 @@ void usbhs_pipe_init(struct usbhs_priv *priv,
 	struct usbhs_pipe *pipe;
 	int i;
 
-	/*
-	 * FIXME
-	 *
-	 * driver needs good allocator.
-	 *
-	 * find first free buffer area (BULK, ISOC)
-	 * (DCP, INT area is fixed)
-	 *
-	 * buffer number 0 - 3 have been reserved for DCP
-	 * see
-	 *	usbhsp_to_bufnmb
-	 */
-	info->bufnmb_last = 4;
 	usbhs_for_each_pipe_with_dcp(pipe, priv, i) {
-		if (usbhs_pipe_type_is(pipe, USB_ENDPOINT_XFER_INT))
-			info->bufnmb_last++;
-
 		usbhsp_flags_init(pipe);
 		pipe->fifo = NULL;
 		pipe->mod_private = NULL;
@@ -851,12 +790,13 @@ int usbhs_pipe_probe(struct usbhs_priv *priv)
 	struct usbhs_pipe_info *info = usbhs_priv_to_pipeinfo(priv);
 	struct usbhs_pipe *pipe;
 	struct device *dev = usbhs_priv_to_dev(priv);
-	u32 *pipe_type = usbhs_get_dparam(priv, pipe_type);
+	struct renesas_usbhs_driver_pipe_config *pipe_configs =
+					usbhs_get_dparam(priv, pipe_configs);
 	int pipe_size = usbhs_get_dparam(priv, pipe_size);
 	int i;
 
 	/* This driver expects 1st pipe is DCP */
-	if (pipe_type[0] != USB_ENDPOINT_XFER_CONTROL) {
+	if (pipe_configs[0].type != USB_ENDPOINT_XFER_CONTROL) {
 		dev_err(dev, "1st PIPE is not DCP\n");
 		return -EINVAL;
 	}
@@ -876,10 +816,10 @@ int usbhs_pipe_probe(struct usbhs_priv *priv)
 		pipe->priv = priv;
 
 		usbhs_pipe_type(pipe) =
-			pipe_type[i] & USB_ENDPOINT_XFERTYPE_MASK;
+			pipe_configs[i].type & USB_ENDPOINT_XFERTYPE_MASK;
 
 		dev_dbg(dev, "pipe %x\t: %s\n",
-			i, usbhsp_pipe_name[pipe_type[i]]);
+			i, usbhsp_pipe_name[pipe_configs[i].type]);
 	}
 
 	return 0;
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index b0bc7b603016..3212ab51e844 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -46,7 +46,6 @@ struct usbhs_pipe {
 struct usbhs_pipe_info {
 	struct usbhs_pipe *pipe;
 	int size;	/* array size of "pipe" */
-	int bufnmb_last;	/* FIXME : driver needs good allocator */
 
 	int (*dma_map_ctrl)(struct usbhs_pkt *pkt, int map);
 };
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index 7d4f51a32e66..73a366de5102 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -38,13 +38,14 @@ static void cp210x_change_speed(struct tty_struct *, struct usb_serial_port *,
 							struct ktermios *);
 static void cp210x_set_termios(struct tty_struct *, struct usb_serial_port *,
 							struct ktermios*);
+static bool cp210x_tx_empty(struct usb_serial_port *port);
 static int cp210x_tiocmget(struct tty_struct *);
 static int cp210x_tiocmset(struct tty_struct *, unsigned int, unsigned int);
 static int cp210x_tiocmset_port(struct usb_serial_port *port,
 		unsigned int, unsigned int);
 static void cp210x_break_ctl(struct tty_struct *, int);
-static int cp210x_startup(struct usb_serial *);
-static void cp210x_release(struct usb_serial *);
+static int cp210x_port_probe(struct usb_serial_port *);
+static int cp210x_port_remove(struct usb_serial_port *);
 static void cp210x_dtr_rts(struct usb_serial_port *p, int on);
 
 static const struct usb_device_id id_table[] = {
@@ -98,6 +99,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x81AC) }, /* MSD Dash Hawk */
 	{ USB_DEVICE(0x10C4, 0x81AD) }, /* INSYS USB Modem */
 	{ USB_DEVICE(0x10C4, 0x81C8) }, /* Lipowsky Industrie Elektronik GmbH, Baby-JTAG */
+	{ USB_DEVICE(0x10C4, 0x81D7) }, /* IAI Corp. RCB-CV-USB USB to RS485 Adaptor */
 	{ USB_DEVICE(0x10C4, 0x81E2) }, /* Lipowsky Industrie Elektronik GmbH, Baby-LIN */
 	{ USB_DEVICE(0x10C4, 0x81E7) }, /* Aerocomm Radio */
 	{ USB_DEVICE(0x10C4, 0x81E8) }, /* Zephyr Bioharness */
@@ -160,6 +162,10 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x17F4, 0xAAAA) }, /* Wavesense Jazz blood glucose meter */
 	{ USB_DEVICE(0x1843, 0x0200) }, /* Vaisala USB Instrument Cable */
 	{ USB_DEVICE(0x18EF, 0xE00F) }, /* ELV USB-I2C-Interface */
+	{ USB_DEVICE(0x18EF, 0xE025) }, /* ELV Marble Sound Board 1 */
+	{ USB_DEVICE(0x1901, 0x0190) }, /* GE B850 CP2105 Recorder interface */
+	{ USB_DEVICE(0x1901, 0x0193) }, /* GE B650 CP2104 PMC interface */
+	{ USB_DEVICE(0x19CF, 0x3000) }, /* Parrot NMEA GPS Flight Recorder */
 	{ USB_DEVICE(0x1ADB, 0x0001) }, /* Schweitzer Engineering C662 Cable */
 	{ USB_DEVICE(0x1B1C, 0x1C00) }, /* Corsair USB Dongle */
 	{ USB_DEVICE(0x1BA4, 0x0002) },	/* Silicon Labs 358x factory default */
@@ -196,8 +202,9 @@ static const struct usb_device_id id_table[] = {
 
 MODULE_DEVICE_TABLE(usb, id_table);
 
-struct cp210x_serial_private {
+struct cp210x_port_private {
 	__u8			bInterfaceNumber;
+	bool			has_swapped_line_ctl;
 };
 
 static struct usb_serial_driver cp210x_device = {
@@ -213,10 +220,11 @@ static struct usb_serial_driver cp210x_device = {
 	.close			= cp210x_close,
 	.break_ctl		= cp210x_break_ctl,
 	.set_termios		= cp210x_set_termios,
+	.tx_empty		= cp210x_tx_empty,
 	.tiocmget		= cp210x_tiocmget,
 	.tiocmset		= cp210x_tiocmset,
-	.attach			= cp210x_startup,
-	.release		= cp210x_release,
+	.port_probe		= cp210x_port_probe,
+	.port_remove		= cp210x_port_remove,
 	.dtr_rts		= cp210x_dtr_rts
 };
 
@@ -299,6 +307,25 @@ static struct usb_serial_driver * const serial_drivers[] = {
 #define CONTROL_WRITE_DTR	0x0100
 #define CONTROL_WRITE_RTS	0x0200
 
+/* CP210X_GET_COMM_STATUS returns these 0x13 bytes */
+struct cp210x_comm_status {
+	__le32   ulErrors;
+	__le32   ulHoldReasons;
+	__le32   ulAmountInInQueue;
+	__le32   ulAmountInOutQueue;
+	u8       bEofReceived;
+	u8       bWaitForImmediate;
+	u8       bReserved;
+} __packed;
+
+/*
+ * CP210X_PURGE - 16 bits passed in wValue of USB request.
+ * SiLabs app note AN571 gives a strange description of the 4 bits:
+ * bit 0 or bit 2 clears the transmit queue and 1 or 3 receive.
+ * writing 1 to all, however, purges cp2108 well enough to avoid the hang.
+ */
+#define PURGE_ALL		0x000f
+
 /*
  * cp210x_get_config
  * Reads from the CP210x configuration registers
@@ -310,7 +337,7 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
 		unsigned int *data, int size)
 {
 	struct usb_serial *serial = port->serial;
-	struct cp210x_serial_private *spriv = usb_get_serial_data(serial);
+	struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
 	__le32 *buf;
 	int result, i, length;
 
@@ -324,7 +351,7 @@ static int cp210x_get_config(struct usb_serial_port *port, u8 request,
 	/* Issue the request, attempting to read 'size' bytes */
 	result = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
 				request, REQTYPE_INTERFACE_TO_HOST, 0x0000,
-				spriv->bInterfaceNumber, buf, size,
+				port_priv->bInterfaceNumber, buf, size,
 				USB_CTRL_GET_TIMEOUT);
 
 	/* Convert data into an array of integers */
@@ -355,7 +382,7 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
 		unsigned int *data, int size)
 {
 	struct usb_serial *serial = port->serial;
-	struct cp210x_serial_private *spriv = usb_get_serial_data(serial);
+	struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
 	__le32 *buf;
 	int result, i, length;
 
@@ -374,13 +401,13 @@ static int cp210x_set_config(struct usb_serial_port *port, u8 request,
 		result = usb_control_msg(serial->dev,
 				usb_sndctrlpipe(serial->dev, 0),
 				request, REQTYPE_HOST_TO_INTERFACE, 0x0000,
-				spriv->bInterfaceNumber, buf, size,
+				port_priv->bInterfaceNumber, buf, size,
 				USB_CTRL_SET_TIMEOUT);
 	} else {
 		result = usb_control_msg(serial->dev,
 				usb_sndctrlpipe(serial->dev, 0),
 				request, REQTYPE_HOST_TO_INTERFACE, data[0],
-				spriv->bInterfaceNumber, NULL, 0,
+				port_priv->bInterfaceNumber, NULL, 0,
 				USB_CTRL_SET_TIMEOUT);
 	}
 
@@ -410,6 +437,60 @@ static inline int cp210x_set_config_single(struct usb_serial_port *port,
 }
 
 /*
+ * Detect CP2108 GET_LINE_CTL bug and activate workaround.
+ * Write a known good value 0x800, read it back.
+ * If it comes back swapped the bug is detected.
+ * Preserve the original register value.
+ */
+static int cp210x_detect_swapped_line_ctl(struct usb_serial_port *port)
+{
+	struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
+	unsigned int line_ctl_save;
+	unsigned int line_ctl_test;
+	int err;
+
+	err = cp210x_get_config(port, CP210X_GET_LINE_CTL, &line_ctl_save, 2);
+	if (err)
+		return err;
+
+	line_ctl_test = 0x800;
+	err = cp210x_set_config(port, CP210X_SET_LINE_CTL, &line_ctl_test, 2);
+	if (err)
+		return err;
+
+	err = cp210x_get_config(port, CP210X_GET_LINE_CTL, &line_ctl_test, 2);
+	if (err)
+		return err;
+
+	if (line_ctl_test == 8) {
+		port_priv->has_swapped_line_ctl = true;
+		line_ctl_save = swab16((u16)line_ctl_save);
+	}
+
+	return cp210x_set_config(port, CP210X_SET_LINE_CTL, &line_ctl_save, 2);
+}
+
+/*
+ * Must always be called instead of cp210x_get_config(CP210X_GET_LINE_CTL)
+ * to workaround cp2108 bug and get correct value.
+ */
+static int cp210x_get_line_ctl(struct usb_serial_port *port, unsigned int *ctl)
+{
+	struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
+	int err;
+
+	err = cp210x_get_config(port, CP210X_GET_LINE_CTL, ctl, 2);
+	if (err)
+		return err;
+
+	/* Workaround swapped bytes in 16-bit value from CP210X_GET_LINE_CTL */
+	if (port_priv->has_swapped_line_ctl)
+		*ctl = swab16((u16)(*ctl));
+
+	return 0;
+}
+
+/*
  * cp210x_quantise_baudrate
  * Quantises the baud rate as per AN205 Table 1
  */
@@ -474,11 +555,63 @@ static int cp210x_open(struct tty_struct *tty, struct usb_serial_port *port)
 
 static void cp210x_close(struct usb_serial_port *port)
 {
+	unsigned int purge_ctl;
+
 	usb_serial_generic_close(port);
+
+	/* Clear both queues; cp2108 needs this to avoid an occasional hang */
+	purge_ctl = PURGE_ALL;
+	cp210x_set_config(port, CP210X_PURGE, &purge_ctl, 2);
+
 	cp210x_set_config_single(port, CP210X_IFC_ENABLE, UART_DISABLE);
 }
 
 /*
+ * Read how many bytes are waiting in the TX queue.
+ */
+static int cp210x_get_tx_queue_byte_count(struct usb_serial_port *port,
+		u32 *count)
+{
+	struct usb_serial *serial = port->serial;
+	struct cp210x_port_private *port_priv = usb_get_serial_port_data(port);
+	struct cp210x_comm_status *sts;
+	int result;
+
+	sts = kmalloc(sizeof(*sts), GFP_KERNEL);
+	if (!sts)
+		return -ENOMEM;
+
+	result = usb_control_msg(serial->dev, usb_rcvctrlpipe(serial->dev, 0),
+			CP210X_GET_COMM_STATUS, REQTYPE_INTERFACE_TO_HOST,
+			0, port_priv->bInterfaceNumber, sts, sizeof(*sts),
+			USB_CTRL_GET_TIMEOUT);
+	if (result == sizeof(*sts)) {
+		*count = le32_to_cpu(sts->ulAmountInOutQueue);
+		result = 0;
+	} else {
+		dev_err(&port->dev, "failed to get comm status: %d\n", result);
+		if (result >= 0)
+			result = -EPROTO;
+	}
+
+	kfree(sts);
+
+	return result;
+}
+
+static bool cp210x_tx_empty(struct usb_serial_port *port)
+{
+	int err;
+	u32 count;
+
+	err = cp210x_get_tx_queue_byte_count(port, &count);
+	if (err)
+		return true;
+
+	return !count;
+}
+
+/*
  * cp210x_get_termios
  * Reads the baud rate, data bits, parity, stop bits and flow control mode
  * from the device, corrects any unsupported values, and configures the
@@ -519,7 +652,7 @@ static void cp210x_get_termios_port(struct usb_serial_port *port,
 
 	cflag = *cflagp;
 
-	cp210x_get_config(port, CP210X_GET_LINE_CTL, &bits, 2);
+	cp210x_get_line_ctl(port, &bits);
 	cflag &= ~CSIZE;
 	switch (bits & BITS_DATA_MASK) {
 	case BITS_DATA_5:
@@ -687,7 +820,7 @@ static void cp210x_set_termios(struct tty_struct *tty,
 
 	/* If the number of data bits is to be updated */
 	if ((cflag & CSIZE) != (old_cflag & CSIZE)) {
-		cp210x_get_config(port, CP210X_GET_LINE_CTL, &bits, 2);
+		cp210x_get_line_ctl(port, &bits);
 		bits &= ~BITS_DATA_MASK;
 		switch (cflag & CSIZE) {
 		case CS5:
@@ -721,7 +854,7 @@ static void cp210x_set_termios(struct tty_struct *tty,
 
 	if ((cflag     & (PARENB|PARODD|CMSPAR)) !=
 	    (old_cflag & (PARENB|PARODD|CMSPAR))) {
-		cp210x_get_config(port, CP210X_GET_LINE_CTL, &bits, 2);
+		cp210x_get_line_ctl(port, &bits);
 		bits &= ~BITS_PARITY_MASK;
 		if (cflag & PARENB) {
 			if (cflag & CMSPAR) {
@@ -747,7 +880,7 @@ static void cp210x_set_termios(struct tty_struct *tty,
 	}
 
 	if ((cflag & CSTOPB) != (old_cflag & CSTOPB)) {
-		cp210x_get_config(port, CP210X_GET_LINE_CTL, &bits, 2);
+		cp210x_get_line_ctl(port, &bits);
 		bits &= ~BITS_STOP_MASK;
 		if (cflag & CSTOPB) {
 			bits |= BITS_STOP_2;
@@ -862,29 +995,39 @@ static void cp210x_break_ctl(struct tty_struct *tty, int break_state)
 	cp210x_set_config(port, CP210X_SET_BREAK, &state, 2);
 }
 
-static int cp210x_startup(struct usb_serial *serial)
+static int cp210x_port_probe(struct usb_serial_port *port)
 {
+	struct usb_serial *serial = port->serial;
 	struct usb_host_interface *cur_altsetting;
-	struct cp210x_serial_private *spriv;
+	struct cp210x_port_private *port_priv;
+	int ret;
 
-	spriv = kzalloc(sizeof(*spriv), GFP_KERNEL);
-	if (!spriv)
+	port_priv = kzalloc(sizeof(*port_priv), GFP_KERNEL);
+	if (!port_priv)
 		return -ENOMEM;
 
 	cur_altsetting = serial->interface->cur_altsetting;
-	spriv->bInterfaceNumber = cur_altsetting->desc.bInterfaceNumber;
+	port_priv->bInterfaceNumber = cur_altsetting->desc.bInterfaceNumber;
+
+	usb_set_serial_port_data(port, port_priv);
 
-	usb_set_serial_data(serial, spriv);
+	ret = cp210x_detect_swapped_line_ctl(port);
+	if (ret) {
+		kfree(port_priv);
+		return ret;
+	}
 
 	return 0;
 }
 
-static void cp210x_release(struct usb_serial *serial)
+static int cp210x_port_remove(struct usb_serial_port *port)
 {
-	struct cp210x_serial_private *spriv;
+	struct cp210x_port_private *port_priv;
+
+	port_priv = usb_get_serial_port_data(port);
+	kfree(port_priv);
 
-	spriv = usb_get_serial_data(serial);
-	kfree(spriv);
+	return 0;
 }
 
 module_usb_serial_driver(serial_drivers, id_table);
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index a5a0376bbd48..8c660ae401d8 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -824,6 +824,7 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_TURTELIZER_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_jtag_quirk },
 	{ USB_DEVICE(RATOC_VENDOR_ID, RATOC_PRODUCT_ID_USB60F) },
+	{ USB_DEVICE(RATOC_VENDOR_ID, RATOC_PRODUCT_ID_SCU18) },
 	{ USB_DEVICE(FTDI_VID, FTDI_REU_TINY_PID) },
 
 	/* Papouch devices based on FTDI chip */
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 67c6d4469730..a84df2513994 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -615,6 +615,7 @@
  */
 #define RATOC_VENDOR_ID		0x0584
 #define RATOC_PRODUCT_ID_USB60F	0xb020
+#define RATOC_PRODUCT_ID_SCU18	0xb03a
 
 /*
  * Infineon Technologies
diff --git a/drivers/usb/serial/io_edgeport.c b/drivers/usb/serial/io_edgeport.c
index c0866971db2b..f49327d20ee8 100644
--- a/drivers/usb/serial/io_edgeport.c
+++ b/drivers/usb/serial/io_edgeport.c
@@ -1046,9 +1046,8 @@ static void edge_close(struct usb_serial_port *port)
 
 	edge_port->closePending = true;
 
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPChase))) {
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPChase) {
 		/* flush and chase */
 		edge_port->chaseResponsePending = true;
 
@@ -1061,9 +1060,8 @@ static void edge_close(struct usb_serial_port *port)
 			edge_port->chaseResponsePending = false;
 	}
 
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPClose))) {
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPClose) {
 	       /* close the port */
 		dev_dbg(&port->dev, "%s - Sending IOSP_CMD_CLOSE_PORT\n", __func__);
 		send_iosp_ext_cmd(edge_port, IOSP_CMD_CLOSE_PORT, 0);
@@ -1612,9 +1610,8 @@ static void edge_break(struct tty_struct *tty, int break_state)
 	struct edgeport_serial *edge_serial = usb_get_serial_data(port->serial);
 	int status;
 
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPChase))) {
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPChase) {
 		/* flush and chase */
 		edge_port->chaseResponsePending = true;
 
@@ -1628,9 +1625,8 @@ static void edge_break(struct tty_struct *tty, int break_state)
 		}
 	}
 
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPSetClrBreak))) {
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPSetClrBreak) {
 		if (break_state == -1) {
 			dev_dbg(&port->dev, "%s - Sending IOSP_CMD_SET_BREAK\n", __func__);
 			status = send_iosp_ext_cmd(edge_port,
@@ -2465,9 +2461,8 @@ static void change_port_settings(struct tty_struct *tty,
 		unsigned char stop_char  = STOP_CHAR(tty);
 		unsigned char start_char = START_CHAR(tty);
 
-		if ((!edge_serial->is_epic) ||
-		    ((edge_serial->is_epic) &&
-		     (edge_serial->epic_descriptor.Supports.IOSPSetXChar))) {
+		if (!edge_serial->is_epic ||
+		    edge_serial->epic_descriptor.Supports.IOSPSetXChar) {
 			send_iosp_ext_cmd(edge_port,
 					IOSP_CMD_SET_XON_CHAR, start_char);
 			send_iosp_ext_cmd(edge_port,
@@ -2494,13 +2489,11 @@ static void change_port_settings(struct tty_struct *tty,
 	}
 
 	/* Set flow control to the configured value */
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPSetRxFlow)))
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPSetRxFlow)
 		send_iosp_ext_cmd(edge_port, IOSP_CMD_SET_RX_FLOW, rxFlow);
-	if ((!edge_serial->is_epic) ||
-	    ((edge_serial->is_epic) &&
-	     (edge_serial->epic_descriptor.Supports.IOSPSetTxFlow)))
+	if (!edge_serial->is_epic ||
+	    edge_serial->epic_descriptor.Supports.IOSPSetTxFlow)
 		send_iosp_ext_cmd(edge_port, IOSP_CMD_SET_TX_FLOW, txFlow);
 
 
diff --git a/drivers/usb/serial/mos7840.c b/drivers/usb/serial/mos7840.c
index 8ac9b55f05af..2c69bfcdacc6 100644
--- a/drivers/usb/serial/mos7840.c
+++ b/drivers/usb/serial/mos7840.c
@@ -635,7 +635,7 @@ static void mos7840_interrupt_callback(struct urb *urb)
 	 * Byte 4 IIR Port 4 (port.number is 3)
 	 * Byte 5 FIFO status for both */
 
-	if (length && length > 5) {
+	if (length > 5) {
 		dev_dbg(&urb->dev->dev, "%s", "Wrong data !!!\n");
 		return;
 	}
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index f2280606b73c..348e19834b83 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -268,6 +268,9 @@ static void option_instat_callback(struct urb *urb);
 #define TELIT_PRODUCT_CC864_SINGLE		0x1006
 #define TELIT_PRODUCT_DE910_DUAL		0x1010
 #define TELIT_PRODUCT_UE910_V2			0x1012
+#define TELIT_PRODUCT_LE922_USBCFG0		0x1042
+#define TELIT_PRODUCT_LE922_USBCFG3		0x1043
+#define TELIT_PRODUCT_LE922_USBCFG5		0x1045
 #define TELIT_PRODUCT_LE920			0x1200
 #define TELIT_PRODUCT_LE910			0x1201
 
@@ -313,6 +316,7 @@ static void option_instat_callback(struct urb *urb);
 #define TOSHIBA_PRODUCT_G450			0x0d45
 
 #define ALINK_VENDOR_ID				0x1e0e
+#define SIMCOM_PRODUCT_SIM7100E			0x9001 /* Yes, ALINK_VENDOR_ID */
 #define ALINK_PRODUCT_PH300			0x9100
 #define ALINK_PRODUCT_3GU			0x9200
 
@@ -605,6 +609,10 @@ static const struct option_blacklist_info zte_1255_blacklist = {
 	.reserved = BIT(3) | BIT(4),
 };
 
+static const struct option_blacklist_info simcom_sim7100e_blacklist = {
+	.reserved = BIT(5) | BIT(6),
+};
+
 static const struct option_blacklist_info telit_le910_blacklist = {
 	.sendsetup = BIT(0),
 	.reserved = BIT(1) | BIT(2),
@@ -615,6 +623,16 @@ static const struct option_blacklist_info telit_le920_blacklist = {
 	.reserved = BIT(1) | BIT(5),
 };
 
+static const struct option_blacklist_info telit_le922_blacklist_usbcfg0 = {
+	.sendsetup = BIT(2),
+	.reserved = BIT(0) | BIT(1) | BIT(3),
+};
+
+static const struct option_blacklist_info telit_le922_blacklist_usbcfg3 = {
+	.sendsetup = BIT(0),
+	.reserved = BIT(1) | BIT(2) | BIT(3),
+};
+
 static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_COLT) },
 	{ USB_DEVICE(OPTION_VENDOR_ID, OPTION_PRODUCT_RICOLA) },
@@ -1110,9 +1128,13 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC650) },
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC680) },
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6000)}, /* ZTE AC8700 */
+	{ USB_DEVICE_AND_INTERFACE_INFO(QUALCOMM_VENDOR_ID, 0x6001, 0xff, 0xff, 0xff), /* 4G LTE usb-modem U901 */
+	  .driver_info = (kernel_ulong_t)&net_intf3_blacklist },
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6613)}, /* Onda H600/ZTE MF330 */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x0023)}, /* ONYX 3G device */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9000)}, /* SIMCom SIM5218 */
+	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9003), /* Quectel UC20 */
+	  .driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6001) },
 	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_CMU_300) },
 	{ USB_DEVICE(CMOTECH_VENDOR_ID, CMOTECH_PRODUCT_6003),
@@ -1160,6 +1182,12 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_CC864_SINGLE) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_DE910_DUAL) },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_UE910_V2) },
+	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG0),
+		.driver_info = (kernel_ulong_t)&telit_le922_blacklist_usbcfg0 },
+	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG3),
+		.driver_info = (kernel_ulong_t)&telit_le922_blacklist_usbcfg3 },
+	{ USB_DEVICE_INTERFACE_CLASS(TELIT_VENDOR_ID, TELIT_PRODUCT_LE922_USBCFG5, 0xff),
+		.driver_info = (kernel_ulong_t)&telit_le922_blacklist_usbcfg0 },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE910),
 		.driver_info = (kernel_ulong_t)&telit_le910_blacklist },
 	{ USB_DEVICE(TELIT_VENDOR_ID, TELIT_PRODUCT_LE920),
@@ -1629,6 +1657,8 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(ALINK_VENDOR_ID, 0x9000) },
 	{ USB_DEVICE(ALINK_VENDOR_ID, ALINK_PRODUCT_PH300) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ALINK_VENDOR_ID, ALINK_PRODUCT_3GU, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE(ALINK_VENDOR_ID, SIMCOM_PRODUCT_SIM7100E),
+	  .driver_info = (kernel_ulong_t)&simcom_sim7100e_blacklist },
 	{ USB_DEVICE(ALCATEL_VENDOR_ID, ALCATEL_PRODUCT_X060S_X200),
 	  .driver_info = (kernel_ulong_t)&alcatel_x200_blacklist
 	},
@@ -1679,7 +1709,7 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_EU3_P) },
 	{ USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PH8),
 		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
-	{ USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_AHXX) },
+	{ USB_DEVICE_INTERFACE_CLASS(CINTERION_VENDOR_ID, CINTERION_PRODUCT_AHXX, 0xff) },
 	{ USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_PLXX),
 		.driver_info = (kernel_ulong_t)&net_intf4_blacklist },
 	{ USB_DEVICE(CINTERION_VENDOR_ID, CINTERION_PRODUCT_HC28_MDM) }, 
diff --git a/drivers/usb/serial/qcserial.c b/drivers/usb/serial/qcserial.c
index 9919d2a9faf2..1bc6089b9008 100644
--- a/drivers/usb/serial/qcserial.c
+++ b/drivers/usb/serial/qcserial.c
@@ -157,14 +157,17 @@ static const struct usb_device_id id_table[] = {
 	{DEVICE_SWI(0x1199, 0x9056)},	/* Sierra Wireless Modem */
 	{DEVICE_SWI(0x1199, 0x9060)},	/* Sierra Wireless Modem */
 	{DEVICE_SWI(0x1199, 0x9061)},	/* Sierra Wireless Modem */
-	{DEVICE_SWI(0x1199, 0x9070)},	/* Sierra Wireless MC74xx/EM74xx */
-	{DEVICE_SWI(0x1199, 0x9071)},	/* Sierra Wireless MC74xx/EM74xx */
+	{DEVICE_SWI(0x1199, 0x9070)},	/* Sierra Wireless MC74xx */
+	{DEVICE_SWI(0x1199, 0x9071)},	/* Sierra Wireless MC74xx */
+	{DEVICE_SWI(0x1199, 0x9078)},	/* Sierra Wireless EM74xx */
+	{DEVICE_SWI(0x1199, 0x9079)},	/* Sierra Wireless EM74xx */
 	{DEVICE_SWI(0x413c, 0x81a2)},	/* Dell Wireless 5806 Gobi(TM) 4G LTE Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a3)},	/* Dell Wireless 5570 HSPA+ (42Mbps) Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a4)},	/* Dell Wireless 5570e HSPA+ (42Mbps) Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a8)},	/* Dell Wireless 5808 Gobi(TM) 4G LTE Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81a9)},	/* Dell Wireless 5808e Gobi(TM) 4G LTE Mobile Broadband Card */
 	{DEVICE_SWI(0x413c, 0x81b1)},	/* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card */
+	{DEVICE_SWI(0x413c, 0x81b3)},	/* Dell Wireless 5809e Gobi(TM) 4G LTE Mobile Broadband Card (rev3) */
 
 	/* Huawei devices */
 	{DEVICE_HWI(0x03f0, 0x581d)},	/* HP lt4112 LTE/HSPA+ Gobi 4G Modem (Huawei me906e) */
diff --git a/drivers/usb/serial/visor.c b/drivers/usb/serial/visor.c
index 60afb39eb73c..337a0be89fcf 100644
--- a/drivers/usb/serial/visor.c
+++ b/drivers/usb/serial/visor.c
@@ -544,6 +544,11 @@ static int treo_attach(struct usb_serial *serial)
 		(serial->num_interrupt_in == 0))
 		return 0;
 
+	if (serial->num_bulk_in < 2 || serial->num_interrupt_in < 2) {
+		dev_err(&serial->interface->dev, "missing endpoints\n");
+		return -ENODEV;
+	}
+
 	/*
 	* It appears that Treos and Kyoceras want to use the
 	* 1st bulk in endpoint to communicate with the 2nd bulk out endpoint,
@@ -597,8 +602,10 @@ static int clie_5_attach(struct usb_serial *serial)
 	 */
 
 	/* some sanity check */
-	if (serial->num_ports < 2)
-		return -1;
+	if (serial->num_bulk_out < 2) {
+		dev_err(&serial->interface->dev, "missing bulk out endpoints\n");
+		return -ENODEV;
+	}
 
 	/* port 0 now uses the modified endpoint Address */
 	port = serial->port[0];
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index 5c66d3f7a6d0..9ff9404f99d7 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -49,18 +49,18 @@ struct uas_dev_info {
 };
 
 enum {
-	SUBMIT_STATUS_URB	= (1 << 1),
-	ALLOC_DATA_IN_URB	= (1 << 2),
-	SUBMIT_DATA_IN_URB	= (1 << 3),
-	ALLOC_DATA_OUT_URB	= (1 << 4),
-	SUBMIT_DATA_OUT_URB	= (1 << 5),
-	ALLOC_CMD_URB		= (1 << 6),
-	SUBMIT_CMD_URB		= (1 << 7),
-	COMMAND_INFLIGHT        = (1 << 8),
-	DATA_IN_URB_INFLIGHT    = (1 << 9),
-	DATA_OUT_URB_INFLIGHT   = (1 << 10),
-	COMMAND_ABORTED         = (1 << 11),
-	IS_IN_WORK_LIST         = (1 << 12),
+	SUBMIT_STATUS_URB	= BIT(1),
+	ALLOC_DATA_IN_URB	= BIT(2),
+	SUBMIT_DATA_IN_URB	= BIT(3),
+	ALLOC_DATA_OUT_URB	= BIT(4),
+	SUBMIT_DATA_OUT_URB	= BIT(5),
+	ALLOC_CMD_URB		= BIT(6),
+	SUBMIT_CMD_URB		= BIT(7),
+	COMMAND_INFLIGHT        = BIT(8),
+	DATA_IN_URB_INFLIGHT    = BIT(9),
+	DATA_OUT_URB_INFLIGHT   = BIT(10),
+	COMMAND_ABORTED         = BIT(11),
+	IS_IN_WORK_LIST         = BIT(12),
 };
 
 /* Overrides scsi_pointer */
@@ -74,7 +74,7 @@ struct uas_cmd_info {
 
 /* I hate forward declarations, but I actually have a loop */
 static int uas_submit_urbs(struct scsi_cmnd *cmnd,
-				struct uas_dev_info *devinfo, gfp_t gfp);
+				struct uas_dev_info *devinfo);
 static void uas_do_work(struct work_struct *work);
 static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller);
 static void uas_free_streams(struct uas_dev_info *devinfo);
@@ -105,7 +105,7 @@ static void uas_do_work(struct work_struct *work)
 		if (!(cmdinfo->state & IS_IN_WORK_LIST))
 			continue;
 
-		err = uas_submit_urbs(cmnd, cmnd->device->hostdata, GFP_ATOMIC);
+		err = uas_submit_urbs(cmnd, cmnd->device->hostdata);
 		if (!err)
 			cmdinfo->state &= ~IS_IN_WORK_LIST;
 		else
@@ -240,7 +240,7 @@ static void uas_xfer_data(struct urb *urb, struct scsi_cmnd *cmnd,
 	int err;
 
 	cmdinfo->state |= direction | SUBMIT_STATUS_URB;
-	err = uas_submit_urbs(cmnd, cmnd->device->hostdata, GFP_ATOMIC);
+	err = uas_submit_urbs(cmnd, cmnd->device->hostdata);
 	if (err) {
 		uas_add_work(cmdinfo);
 	}
@@ -512,7 +512,7 @@ static struct urb *uas_submit_sense_urb(struct scsi_cmnd *cmnd, gfp_t gfp)
 }
 
 static int uas_submit_urbs(struct scsi_cmnd *cmnd,
-			   struct uas_dev_info *devinfo, gfp_t gfp)
+			   struct uas_dev_info *devinfo)
 {
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct urb *urb;
@@ -520,14 +520,14 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	lockdep_assert_held(&devinfo->lock);
 	if (cmdinfo->state & SUBMIT_STATUS_URB) {
-		urb = uas_submit_sense_urb(cmnd, gfp);
+		urb = uas_submit_sense_urb(cmnd, GFP_ATOMIC);
 		if (!urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~SUBMIT_STATUS_URB;
 	}
 
 	if (cmdinfo->state & ALLOC_DATA_IN_URB) {
-		cmdinfo->data_in_urb = uas_alloc_data_urb(devinfo, gfp,
+		cmdinfo->data_in_urb = uas_alloc_data_urb(devinfo, GFP_ATOMIC,
 							cmnd, DMA_FROM_DEVICE);
 		if (!cmdinfo->data_in_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
@@ -536,7 +536,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	if (cmdinfo->state & SUBMIT_DATA_IN_URB) {
 		usb_anchor_urb(cmdinfo->data_in_urb, &devinfo->data_urbs);
-		err = usb_submit_urb(cmdinfo->data_in_urb, gfp);
+		err = usb_submit_urb(cmdinfo->data_in_urb, GFP_ATOMIC);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->data_in_urb);
 			uas_log_cmd_state(cmnd, "data in submit err", err);
@@ -547,7 +547,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 	}
 
 	if (cmdinfo->state & ALLOC_DATA_OUT_URB) {
-		cmdinfo->data_out_urb = uas_alloc_data_urb(devinfo, gfp,
+		cmdinfo->data_out_urb = uas_alloc_data_urb(devinfo, GFP_ATOMIC,
 							cmnd, DMA_TO_DEVICE);
 		if (!cmdinfo->data_out_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
@@ -556,7 +556,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	if (cmdinfo->state & SUBMIT_DATA_OUT_URB) {
 		usb_anchor_urb(cmdinfo->data_out_urb, &devinfo->data_urbs);
-		err = usb_submit_urb(cmdinfo->data_out_urb, gfp);
+		err = usb_submit_urb(cmdinfo->data_out_urb, GFP_ATOMIC);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->data_out_urb);
 			uas_log_cmd_state(cmnd, "data out submit err", err);
@@ -567,7 +567,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 	}
 
 	if (cmdinfo->state & ALLOC_CMD_URB) {
-		cmdinfo->cmd_urb = uas_alloc_cmd_urb(devinfo, gfp, cmnd);
+		cmdinfo->cmd_urb = uas_alloc_cmd_urb(devinfo, GFP_ATOMIC, cmnd);
 		if (!cmdinfo->cmd_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~ALLOC_CMD_URB;
@@ -575,7 +575,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	if (cmdinfo->state & SUBMIT_CMD_URB) {
 		usb_anchor_urb(cmdinfo->cmd_urb, &devinfo->cmd_urbs);
-		err = usb_submit_urb(cmdinfo->cmd_urb, gfp);
+		err = usb_submit_urb(cmdinfo->cmd_urb, GFP_ATOMIC);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->cmd_urb);
 			uas_log_cmd_state(cmnd, "cmd submit err", err);
@@ -653,7 +653,7 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	if (!devinfo->use_streams)
 		cmdinfo->state &= ~(SUBMIT_DATA_IN_URB | SUBMIT_DATA_OUT_URB);
 
-	err = uas_submit_urbs(cmnd, devinfo, GFP_ATOMIC);
+	err = uas_submit_urbs(cmnd, devinfo);
 	if (err) {
 		/* If we did nothing, give up now */
 		if (cmdinfo->state & SUBMIT_STATUS_URB) {
diff --git a/drivers/uwb/uwbd.c b/drivers/uwb/uwbd.c
index bdcb13cc1d54..01c20a260a8b 100644
--- a/drivers/uwb/uwbd.c
+++ b/drivers/uwb/uwbd.c
@@ -279,7 +279,6 @@ static int uwbd(void *param)
 			HZ);
 		if (should_stop)
 			break;
-		try_to_freeze();
 
 		spin_lock_irqsave(&rc->uwbd.event_list_lock, flags);
 		if (!list_empty(&rc->uwbd.event_list)) {
diff --git a/drivers/vfio/Kconfig b/drivers/vfio/Kconfig
index 850d86ca685b..da6e2ce77495 100644
--- a/drivers/vfio/Kconfig
+++ b/drivers/vfio/Kconfig
@@ -31,6 +31,21 @@ menuconfig VFIO
 
 	  If you don't know what to do here, say N.
 
+menuconfig VFIO_NOIOMMU
+	bool "VFIO No-IOMMU support"
+	depends on VFIO
+	help
+	  VFIO is built on the ability to isolate devices using the IOMMU.
+	  Only with an IOMMU can userspace access to DMA capable devices be
+	  considered secure.  VFIO No-IOMMU mode enables IOMMU groups for
+	  devices without IOMMU backing for the purpose of re-using the VFIO
+	  infrastructure in a non-secure mode.  Use of this mode will result
+	  in an unsupportable kernel and will therefore taint the kernel.
+	  Device assignment to virtual machines is also not possible with
+	  this mode since there is no IOMMU to provide DMA translation.
+
+	  If you don't know what to do here, say N.
+
 source "drivers/vfio/pci/Kconfig"
 source "drivers/vfio/platform/Kconfig"
 source "virt/lib/Kconfig"
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index 56bf6dbb93db..8c80a48e3233 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -446,7 +446,8 @@ static long vfio_pci_ioctl(void *device_data,
 		info.num_regions = VFIO_PCI_NUM_REGIONS;
 		info.num_irqs = VFIO_PCI_NUM_IRQS;
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_GET_REGION_INFO) {
 		struct pci_dev *pdev = vdev->pdev;
@@ -520,7 +521,8 @@ static long vfio_pci_ioctl(void *device_data,
 			return -EINVAL;
 		}
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_GET_IRQ_INFO) {
 		struct vfio_irq_info info;
@@ -555,7 +557,8 @@ static long vfio_pci_ioctl(void *device_data,
 		else
 			info.flags |= VFIO_IRQ_INFO_NORESIZE;
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_SET_IRQS) {
 		struct vfio_irq_set hdr;
@@ -940,13 +943,13 @@ static int vfio_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (pdev->hdr_type != PCI_HEADER_TYPE_NORMAL)
 		return -EINVAL;
 
-	group = iommu_group_get(&pdev->dev);
+	group = vfio_iommu_group_get(&pdev->dev);
 	if (!group)
 		return -EINVAL;
 
 	vdev = kzalloc(sizeof(*vdev), GFP_KERNEL);
 	if (!vdev) {
-		iommu_group_put(group);
+		vfio_iommu_group_put(group, &pdev->dev);
 		return -ENOMEM;
 	}
 
@@ -957,7 +960,7 @@ static int vfio_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	ret = vfio_add_group_dev(&pdev->dev, &vfio_pci_ops, vdev);
 	if (ret) {
-		iommu_group_put(group);
+		vfio_iommu_group_put(group, &pdev->dev);
 		kfree(vdev);
 		return ret;
 	}
@@ -993,7 +996,7 @@ static void vfio_pci_remove(struct pci_dev *pdev)
 	if (!vdev)
 		return;
 
-	iommu_group_put(pdev->dev.iommu_group);
+	vfio_iommu_group_put(pdev->dev.iommu_group, &pdev->dev);
 	kfree(vdev);
 
 	if (vfio_pci_is_vga(pdev)) {
diff --git a/drivers/vfio/platform/reset/vfio_platform_amdxgbe.c b/drivers/vfio/platform/reset/vfio_platform_amdxgbe.c
index da5356f48d0b..d4030d0c38e9 100644
--- a/drivers/vfio/platform/reset/vfio_platform_amdxgbe.c
+++ b/drivers/vfio/platform/reset/vfio_platform_amdxgbe.c
@@ -110,7 +110,7 @@ int vfio_platform_amdxgbe_reset(struct vfio_platform_device *vdev)
 	usleep_range(10, 15);
 
 	count = 2000;
-	while (count-- && (ioread32(xgmac_regs->ioaddr + DMA_MR) & 1))
+	while (--count && (ioread32(xgmac_regs->ioaddr + DMA_MR) & 1))
 		usleep_range(500, 600);
 
 	if (!count)
diff --git a/drivers/vfio/platform/vfio_platform_common.c b/drivers/vfio/platform/vfio_platform_common.c
index 418cdd9ba3f4..e65b142d3422 100644
--- a/drivers/vfio/platform/vfio_platform_common.c
+++ b/drivers/vfio/platform/vfio_platform_common.c
@@ -219,7 +219,8 @@ static long vfio_platform_ioctl(void *device_data,
 		info.num_regions = vdev->num_regions;
 		info.num_irqs = vdev->num_irqs;
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_GET_REGION_INFO) {
 		struct vfio_region_info info;
@@ -240,7 +241,8 @@ static long vfio_platform_ioctl(void *device_data,
 		info.size = vdev->regions[info.index].size;
 		info.flags = vdev->regions[info.index].flags;
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_GET_IRQ_INFO) {
 		struct vfio_irq_info info;
@@ -259,7 +261,8 @@ static long vfio_platform_ioctl(void *device_data,
 		info.flags = vdev->irqs[info.index].flags;
 		info.count = vdev->irqs[info.index].count;
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_DEVICE_SET_IRQS) {
 		struct vfio_irq_set hdr;
diff --git a/drivers/vfio/vfio.c b/drivers/vfio/vfio.c
index 6070b793cbcb..ecca316386f5 100644
--- a/drivers/vfio/vfio.c
+++ b/drivers/vfio/vfio.c
@@ -62,6 +62,7 @@ struct vfio_container {
 	struct rw_semaphore		group_lock;
 	struct vfio_iommu_driver	*iommu_driver;
 	void				*iommu_data;
+	bool				noiommu;
 };
 
 struct vfio_unbound_dev {
@@ -84,6 +85,7 @@ struct vfio_group {
 	struct list_head		unbound_list;
 	struct mutex			unbound_lock;
 	atomic_t			opened;
+	bool				noiommu;
 };
 
 struct vfio_device {
@@ -95,6 +97,123 @@ struct vfio_device {
 	void				*device_data;
 };
 
+#ifdef CONFIG_VFIO_NOIOMMU
+static bool noiommu __read_mostly;
+module_param_named(enable_unsafe_noiommu_mode,
+		   noiommu, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(enable_unsafe_noiommu_mode, "Enable UNSAFE, no-IOMMU mode.  This mode provides no device isolation, no DMA translation, no host kernel protection, cannot be used for device assignment to virtual machines, requires RAWIO permissions, and will taint the kernel.  If you do not know what this is for, step away. (default: false)");
+#endif
+
+/*
+ * vfio_iommu_group_{get,put} are only intended for VFIO bus driver probe
+ * and remove functions, any use cases other than acquiring the first
+ * reference for the purpose of calling vfio_add_group_dev() or removing
+ * that symmetric reference after vfio_del_group_dev() should use the raw
+ * iommu_group_{get,put} functions.  In particular, vfio_iommu_group_put()
+ * removes the device from the dummy group and cannot be nested.
+ */
+struct iommu_group *vfio_iommu_group_get(struct device *dev)
+{
+	struct iommu_group *group;
+	int __maybe_unused ret;
+
+	group = iommu_group_get(dev);
+
+#ifdef CONFIG_VFIO_NOIOMMU
+	/*
+	 * With noiommu enabled, an IOMMU group will be created for a device
+	 * that doesn't already have one and doesn't have an iommu_ops on their
+	 * bus.  We set iommudata simply to be able to identify these groups
+	 * as special use and for reclamation later.
+	 */
+	if (group || !noiommu || iommu_present(dev->bus))
+		return group;
+
+	group = iommu_group_alloc();
+	if (IS_ERR(group))
+		return NULL;
+
+	iommu_group_set_name(group, "vfio-noiommu");
+	iommu_group_set_iommudata(group, &noiommu, NULL);
+	ret = iommu_group_add_device(group, dev);
+	iommu_group_put(group);
+	if (ret)
+		return NULL;
+
+	/*
+	 * Where to taint?  At this point we've added an IOMMU group for a
+	 * device that is not backed by iommu_ops, therefore any iommu_
+	 * callback using iommu_ops can legitimately Oops.  So, while we may
+	 * be about to give a DMA capable device to a user without IOMMU
+	 * protection, which is clearly taint-worthy, let's go ahead and do
+	 * it here.
+	 */
+	add_taint(TAINT_USER, LOCKDEP_STILL_OK);
+	dev_warn(dev, "Adding kernel taint for vfio-noiommu group on device\n");
+#endif
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(vfio_iommu_group_get);
+
+void vfio_iommu_group_put(struct iommu_group *group, struct device *dev)
+{
+#ifdef CONFIG_VFIO_NOIOMMU
+	if (iommu_group_get_iommudata(group) == &noiommu)
+		iommu_group_remove_device(dev);
+#endif
+
+	iommu_group_put(group);
+}
+EXPORT_SYMBOL_GPL(vfio_iommu_group_put);
+
+#ifdef CONFIG_VFIO_NOIOMMU
+static void *vfio_noiommu_open(unsigned long arg)
+{
+	if (arg != VFIO_NOIOMMU_IOMMU)
+		return ERR_PTR(-EINVAL);
+	if (!capable(CAP_SYS_RAWIO))
+		return ERR_PTR(-EPERM);
+
+	return NULL;
+}
+
+static void vfio_noiommu_release(void *iommu_data)
+{
+}
+
+static long vfio_noiommu_ioctl(void *iommu_data,
+			       unsigned int cmd, unsigned long arg)
+{
+	if (cmd == VFIO_CHECK_EXTENSION)
+		return noiommu && (arg == VFIO_NOIOMMU_IOMMU) ? 1 : 0;
+
+	return -ENOTTY;
+}
+
+static int vfio_noiommu_attach_group(void *iommu_data,
+				     struct iommu_group *iommu_group)
+{
+	return iommu_group_get_iommudata(iommu_group) == &noiommu ? 0 : -EINVAL;
+}
+
+static void vfio_noiommu_detach_group(void *iommu_data,
+				      struct iommu_group *iommu_group)
+{
+}
+
+static const struct vfio_iommu_driver_ops vfio_noiommu_ops = {
+	.name = "vfio-noiommu",
+	.owner = THIS_MODULE,
+	.open = vfio_noiommu_open,
+	.release = vfio_noiommu_release,
+	.ioctl = vfio_noiommu_ioctl,
+	.attach_group = vfio_noiommu_attach_group,
+	.detach_group = vfio_noiommu_detach_group,
+};
+#endif
+
+
 /**
  * IOMMU driver registration
  */
@@ -217,6 +336,9 @@ static struct vfio_group *vfio_create_group(struct iommu_group *iommu_group)
 	atomic_set(&group->container_users, 0);
 	atomic_set(&group->opened, 0);
 	group->iommu_group = iommu_group;
+#ifdef CONFIG_VFIO_NOIOMMU
+	group->noiommu = (iommu_group_get_iommudata(iommu_group) == &noiommu);
+#endif
 
 	group->nb.notifier_call = vfio_iommu_group_notifier;
 
@@ -252,7 +374,8 @@ static struct vfio_group *vfio_create_group(struct iommu_group *iommu_group)
 
 	dev = device_create(vfio.class, NULL,
 			    MKDEV(MAJOR(vfio.group_devt), minor),
-			    group, "%d", iommu_group_id(iommu_group));
+			    group, "%s%d", group->noiommu ? "noiommu-" : "",
+			    iommu_group_id(iommu_group));
 	if (IS_ERR(dev)) {
 		vfio_free_group_minor(minor);
 		vfio_group_unlock_and_free(group);
@@ -854,6 +977,14 @@ static long vfio_ioctl_check_extension(struct vfio_container *container,
 			mutex_lock(&vfio.iommu_drivers_lock);
 			list_for_each_entry(driver, &vfio.iommu_drivers_list,
 					    vfio_next) {
+
+#ifdef CONFIG_VFIO_NOIOMMU
+				if (!list_empty(&container->group_list) &&
+				    (container->noiommu !=
+				     (driver->ops == &vfio_noiommu_ops)))
+					continue;
+#endif
+
 				if (!try_module_get(driver->ops->owner))
 					continue;
 
@@ -925,6 +1056,15 @@ static long vfio_ioctl_set_iommu(struct vfio_container *container,
 	list_for_each_entry(driver, &vfio.iommu_drivers_list, vfio_next) {
 		void *data;
 
+#ifdef CONFIG_VFIO_NOIOMMU
+		/*
+		 * Only noiommu containers can use vfio-noiommu and noiommu
+		 * containers can only use vfio-noiommu.
+		 */
+		if (container->noiommu != (driver->ops == &vfio_noiommu_ops))
+			continue;
+#endif
+
 		if (!try_module_get(driver->ops->owner))
 			continue;
 
@@ -1187,6 +1327,9 @@ static int vfio_group_set_container(struct vfio_group *group, int container_fd)
 	if (atomic_read(&group->container_users))
 		return -EINVAL;
 
+	if (group->noiommu && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
 	f = fdget(container_fd);
 	if (!f.file)
 		return -EBADF;
@@ -1202,6 +1345,13 @@ static int vfio_group_set_container(struct vfio_group *group, int container_fd)
 
 	down_write(&container->group_lock);
 
+	/* Real groups and fake groups cannot mix */
+	if (!list_empty(&container->group_list) &&
+	    container->noiommu != group->noiommu) {
+		ret = -EPERM;
+		goto unlock_out;
+	}
+
 	driver = container->iommu_driver;
 	if (driver) {
 		ret = driver->ops->attach_group(container->iommu_data,
@@ -1211,6 +1361,7 @@ static int vfio_group_set_container(struct vfio_group *group, int container_fd)
 	}
 
 	group->container = container;
+	container->noiommu = group->noiommu;
 	list_add(&group->container_next, &container->group_list);
 
 	/* Get a reference on the container and mark a user within the group */
@@ -1241,6 +1392,9 @@ static int vfio_group_get_device_fd(struct vfio_group *group, char *buf)
 	    !group->container->iommu_driver || !vfio_group_viable(group))
 		return -EINVAL;
 
+	if (group->noiommu && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
 	device = vfio_device_get_from_name(group, buf);
 	if (!device)
 		return -ENODEV;
@@ -1283,6 +1437,10 @@ static int vfio_group_get_device_fd(struct vfio_group *group, char *buf)
 
 	fd_install(ret, filep);
 
+	if (group->noiommu)
+		dev_warn(device->dev, "vfio-noiommu device opened by user "
+			 "(%s:%d)\n", current->comm, task_pid_nr(current));
+
 	return ret;
 }
 
@@ -1371,6 +1529,11 @@ static int vfio_group_fops_open(struct inode *inode, struct file *filep)
 	if (!group)
 		return -ENODEV;
 
+	if (group->noiommu && !capable(CAP_SYS_RAWIO)) {
+		vfio_group_put(group);
+		return -EPERM;
+	}
+
 	/* Do we need multiple instances of the group open?  Seems not. */
 	opened = atomic_cmpxchg(&group->opened, 0, 1);
 	if (opened) {
@@ -1533,6 +1696,11 @@ struct vfio_group *vfio_group_get_external_user(struct file *filep)
 	if (!atomic_inc_not_zero(&group->container_users))
 		return ERR_PTR(-EINVAL);
 
+	if (group->noiommu) {
+		atomic_dec(&group->container_users);
+		return ERR_PTR(-EPERM);
+	}
+
 	if (!group->container->iommu_driver ||
 			!vfio_group_viable(group)) {
 		atomic_dec(&group->container_users);
@@ -1625,6 +1793,9 @@ static int __init vfio_init(void)
 	request_module_nowait("vfio_iommu_type1");
 	request_module_nowait("vfio_iommu_spapr_tce");
 
+#ifdef CONFIG_VFIO_NOIOMMU
+	vfio_register_iommu_driver(&vfio_noiommu_ops);
+#endif
 	return 0;
 
 err_cdev_add:
@@ -1641,6 +1812,9 @@ static void __exit vfio_cleanup(void)
 {
 	WARN_ON(!list_empty(&vfio.group_list));
 
+#ifdef CONFIG_VFIO_NOIOMMU
+	vfio_unregister_iommu_driver(&vfio_noiommu_ops);
+#endif
 	idr_destroy(&vfio.group_idr);
 	cdev_del(&vfio.group_cdev);
 	unregister_chrdev_region(vfio.group_devt, MINORMASK);
diff --git a/drivers/vfio/vfio_iommu_type1.c b/drivers/vfio/vfio_iommu_type1.c
index 59d47cb638d5..75b24e93cedb 100644
--- a/drivers/vfio/vfio_iommu_type1.c
+++ b/drivers/vfio/vfio_iommu_type1.c
@@ -995,11 +995,12 @@ static long vfio_iommu_type1_ioctl(void *iommu_data,
 		if (info.argsz < minsz)
 			return -EINVAL;
 
-		info.flags = 0;
+		info.flags = VFIO_IOMMU_INFO_PGSIZES;
 
 		info.iova_pgsizes = vfio_pgsize_bitmap(iommu);
 
-		return copy_to_user((void __user *)arg, &info, minsz);
+		return copy_to_user((void __user *)arg, &info, minsz) ?
+			-EFAULT : 0;
 
 	} else if (cmd == VFIO_IOMMU_MAP_DMA) {
 		struct vfio_iommu_type1_dma_map map;
@@ -1032,7 +1033,8 @@ static long vfio_iommu_type1_ioctl(void *iommu_data,
 		if (ret)
 			return ret;
 
-		return copy_to_user((void __user *)arg, &unmap, minsz);
+		return copy_to_user((void __user *)arg, &unmap, minsz) ?
+			-EFAULT : 0;
 	}
 
 	return -ENOTTY;
diff --git a/drivers/vhost/vhost.c b/drivers/vhost/vhost.c
index ad2146a9ab2d..236553e81027 100644
--- a/drivers/vhost/vhost.c
+++ b/drivers/vhost/vhost.c
@@ -1156,6 +1156,8 @@ int vhost_init_used(struct vhost_virtqueue *vq)
 {
 	__virtio16 last_used_idx;
 	int r;
+	bool is_le = vq->is_le;
+
 	if (!vq->private_data) {
 		vq->is_le = virtio_legacy_is_little_endian();
 		return 0;
@@ -1165,15 +1167,20 @@ int vhost_init_used(struct vhost_virtqueue *vq)
 
 	r = vhost_update_used_flags(vq);
 	if (r)
-		return r;
+		goto err;
 	vq->signalled_used_valid = false;
-	if (!access_ok(VERIFY_READ, &vq->used->idx, sizeof vq->used->idx))
-		return -EFAULT;
+	if (!access_ok(VERIFY_READ, &vq->used->idx, sizeof vq->used->idx)) {
+		r = -EFAULT;
+		goto err;
+	}
 	r = __get_user(last_used_idx, &vq->used->idx);
 	if (r)
-		return r;
+		goto err;
 	vq->last_used_idx = vhost16_to_cpu(vq, last_used_idx);
 	return 0;
+err:
+	vq->is_le = is_le;
+	return r;
 }
 EXPORT_SYMBOL_GPL(vhost_init_used);
 
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index 98ffe71e8af2..510e559c060e 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -566,11 +566,13 @@ static ssize_t adp8860_bl_ambient_light_level_show(struct device *dev,
 
 	mutex_lock(&data->lock);
 	error = adp8860_read(data->client, ADP8860_PH1LEVL, &reg_val);
-	ret_val = reg_val;
-	error |= adp8860_read(data->client, ADP8860_PH1LEVH, &reg_val);
+	if (!error) {
+		ret_val = reg_val;
+		error = adp8860_read(data->client, ADP8860_PH1LEVH, &reg_val);
+	}
 	mutex_unlock(&data->lock);
 
-	if (error < 0)
+	if (error)
 		return error;
 
 	/* Return 13-bit conversion value for the first light sensor */
@@ -621,10 +623,12 @@ static ssize_t adp8860_bl_ambient_light_zone_store(struct device *dev,
 
 		/* Set user supplied ambient light zone */
 		mutex_lock(&data->lock);
-		adp8860_read(data->client, ADP8860_CFGR, &reg_val);
-		reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
-		reg_val |= (val - 1) << CFGR_BLV_SHIFT;
-		adp8860_write(data->client, ADP8860_CFGR, reg_val);
+		ret = adp8860_read(data->client, ADP8860_CFGR, &reg_val);
+		if (!ret) {
+			reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
+			reg_val |= (val - 1) << CFGR_BLV_SHIFT;
+			adp8860_write(data->client, ADP8860_CFGR, reg_val);
+		}
 		mutex_unlock(&data->lock);
 	}
 
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
index 9d738352d7d4..21acac90fd77 100644
--- a/drivers/video/backlight/adp8870_bl.c
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -807,10 +807,12 @@ static ssize_t adp8870_bl_ambient_light_zone_store(struct device *dev,
 
 		/* Set user supplied ambient light zone */
 		mutex_lock(&data->lock);
-		adp8870_read(data->client, ADP8870_CFGR, &reg_val);
-		reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
-		reg_val |= (val - 1) << CFGR_BLV_SHIFT;
-		adp8870_write(data->client, ADP8870_CFGR, reg_val);
+		ret = adp8870_read(data->client, ADP8870_CFGR, &reg_val);
+		if (!ret) {
+			reg_val &= ~(CFGR_BLV_MASK << CFGR_BLV_SHIFT);
+			reg_val |= (val - 1) << CFGR_BLV_SHIFT;
+			adp8870_write(data->client, ADP8870_CFGR, reg_val);
+		}
 		mutex_unlock(&data->lock);
 	}
 
diff --git a/drivers/video/backlight/gpio_backlight.c b/drivers/video/backlight/gpio_backlight.c
index 5fbbc2ebdf93..18134416b154 100644
--- a/drivers/video/backlight/gpio_backlight.c
+++ b/drivers/video/backlight/gpio_backlight.c
@@ -89,6 +89,7 @@ static int gpio_backlight_probe(struct platform_device *pdev)
 	struct backlight_device *bl;
 	struct gpio_backlight *gbl;
 	struct device_node *np = pdev->dev.of_node;
+	unsigned long flags = GPIOF_DIR_OUT;
 	int ret;
 
 	if (!pdata && !np) {
@@ -114,9 +115,12 @@ static int gpio_backlight_probe(struct platform_device *pdev)
 		gbl->def_value = pdata->def_value;
 	}
 
-	ret = devm_gpio_request_one(gbl->dev, gbl->gpio, GPIOF_DIR_OUT |
-				    (gbl->active ? GPIOF_INIT_LOW
-						 : GPIOF_INIT_HIGH),
+	if (gbl->active)
+		flags |= gbl->def_value ? GPIOF_INIT_HIGH : GPIOF_INIT_LOW;
+	else
+		flags |= gbl->def_value ? GPIOF_INIT_LOW : GPIOF_INIT_HIGH;
+
+	ret = devm_gpio_request_one(gbl->dev, gbl->gpio, flags,
 				    pdata ? pdata->name : "backlight");
 	if (ret < 0) {
 		dev_err(&pdev->dev, "unable to request GPIO\n");
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index ae3c6b6fd5db..64f9e1b8655f 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -198,7 +198,9 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	struct platform_pwm_backlight_data defdata;
 	struct backlight_properties props;
 	struct backlight_device *bl;
+	struct device_node *node = pdev->dev.of_node;
 	struct pwm_bl_data *pb;
+	int initial_blank = FB_BLANK_UNBLANK;
 	int ret;
 
 	if (!data) {
@@ -242,7 +244,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	pb->enabled = false;
 
 	pb->enable_gpio = devm_gpiod_get_optional(&pdev->dev, "enable",
-						  GPIOD_OUT_HIGH);
+						  GPIOD_ASIS);
 	if (IS_ERR(pb->enable_gpio)) {
 		ret = PTR_ERR(pb->enable_gpio);
 		goto err_alloc;
@@ -264,15 +266,32 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 		pb->enable_gpio = gpio_to_desc(data->enable_gpio);
 	}
 
+	if (pb->enable_gpio) {
+		/*
+		 * If the driver is probed from the device tree and there is a
+		 * phandle link pointing to the backlight node, it is safe to
+		 * assume that another driver will enable the backlight at the
+		 * appropriate time. Therefore, if it is disabled, keep it so.
+		 */
+		if (node && node->phandle &&
+		    gpiod_get_direction(pb->enable_gpio) == GPIOF_DIR_OUT &&
+		    gpiod_get_value(pb->enable_gpio) == 0)
+			initial_blank = FB_BLANK_POWERDOWN;
+		else
+			gpiod_direction_output(pb->enable_gpio, 1);
+	}
+
 	pb->power_supply = devm_regulator_get(&pdev->dev, "power");
 	if (IS_ERR(pb->power_supply)) {
 		ret = PTR_ERR(pb->power_supply);
 		goto err_alloc;
 	}
 
+	if (node && node->phandle && !regulator_is_enabled(pb->power_supply))
+		initial_blank = FB_BLANK_POWERDOWN;
+
 	pb->pwm = devm_pwm_get(&pdev->dev, NULL);
-	if (IS_ERR(pb->pwm) && PTR_ERR(pb->pwm) != -EPROBE_DEFER
-	    && !pdev->dev.of_node) {
+	if (IS_ERR(pb->pwm) && PTR_ERR(pb->pwm) != -EPROBE_DEFER && !node) {
 		dev_err(&pdev->dev, "unable to request PWM, trying legacy API\n");
 		pb->legacy = true;
 		pb->pwm = pwm_request(data->pwm_id, "pwm-backlight");
@@ -309,6 +328,8 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	if (IS_ERR(bl)) {
 		dev_err(&pdev->dev, "failed to register backlight\n");
 		ret = PTR_ERR(bl);
+		if (pb->legacy)
+			pwm_free(pb->pwm);
 		goto err_alloc;
 	}
 
@@ -320,6 +341,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	}
 
 	bl->props.brightness = data->dft_brightness;
+	bl->props.power = initial_blank;
 	backlight_update_status(bl);
 
 	platform_set_drvdata(pdev, bl);
diff --git a/drivers/video/backlight/tps65217_bl.c b/drivers/video/backlight/tps65217_bl.c
index 61d72bffd402..fd524ad860a5 100644
--- a/drivers/video/backlight/tps65217_bl.c
+++ b/drivers/video/backlight/tps65217_bl.c
@@ -320,10 +320,19 @@ static int tps65217_bl_probe(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id tps65217_bl_of_match[] = {
+	{ .compatible = "ti,tps65217-bl", },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, tps65217_bl_of_match);
+#endif
+
 static struct platform_driver tps65217_bl_driver = {
 	.probe		= tps65217_bl_probe,
 	.driver		= {
 		.name	= "tps65217-bl",
+		.of_match_table = of_match_ptr(tps65217_bl_of_match),
 	},
 };
 
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 92f394927f24..6e92917ba77a 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -709,6 +709,7 @@ static int con2fb_acquire_newinfo(struct vc_data *vc, struct fb_info *info,
 	}
 
 	if (!err) {
+		ops->cur_blink_jiffies = HZ / 5;
 		info->fbcon_par = ops;
 
 		if (vc)
@@ -956,6 +957,7 @@ static const char *fbcon_startup(void)
 	ops->currcon = -1;
 	ops->graphics = 1;
 	ops->cur_rotate = -1;
+	ops->cur_blink_jiffies = HZ / 5;
 	info->fbcon_par = ops;
 	p->con_rotate = initial_rotation;
 	set_blitting_type(vc, info);
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index e6d16d65e4e6..8ea45a5cd806 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -5,6 +5,7 @@
 menuconfig FB
 	tristate "Support for frame buffer devices"
 	select FB_CMDLINE
+	select FB_NOTIFY
 	---help---
 	  The frame buffer device provides an abstraction for the graphics
 	  hardware. It represents the frame buffer of some video hardware and
@@ -56,6 +57,9 @@ config FIRMWARE_EDID
 config FB_CMDLINE
 	bool
 
+config FB_NOTIFY
+	bool
+
 config FB_DDC
        tristate
        depends on FB
@@ -1506,6 +1510,7 @@ config FB_SIS
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
 	select FB_BOOT_VESA_SUPPORT if FB_SIS = y
+	select FB_SIS_300 if !FB_SIS_315
 	help
 	  This is the frame buffer device driver for the SiS 300, 315, 330
 	  and 340 series as well as XGI V3XT, V5, V8, Z7 graphics chipsets.
@@ -1880,6 +1885,8 @@ config FB_PXA
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
+	select VIDEOMODE_HELPERS if OF
+	select FB_MODE_HELPERS if OF
 	---help---
 	  Frame buffer driver for the built-in LCD controller in the Intel
 	  PXA2x0 processor.
@@ -1990,16 +1997,6 @@ config FB_SH_MOBILE_LCDC
 	---help---
 	  Frame buffer driver for the on-chip SH-Mobile LCD controller.
 
-config FB_SH_MOBILE_HDMI
-	tristate "SuperH Mobile HDMI controller support"
-	depends on FB_SH_MOBILE_LCDC
-	select FB_MODE_HELPERS
-	select SOUND
-	select SND
-	select SND_SOC
-	---help---
-	  Driver for the on-chip SH-Mobile HDMI controller.
-
 config FB_TMIO
 	tristate "Toshiba Mobile IO FrameBuffer support"
 	depends on FB && (MFD_TMIO || COMPILE_TEST)
diff --git a/drivers/video/fbdev/Makefile b/drivers/video/fbdev/Makefile
index 50ed1b4fc2bf..65fb15075c8f 100644
--- a/drivers/video/fbdev/Makefile
+++ b/drivers/video/fbdev/Makefile
@@ -118,7 +118,6 @@ obj-$(CONFIG_FB_UDL)		  += udlfb.o
 obj-$(CONFIG_FB_SMSCUFX)	  += smscufx.o
 obj-$(CONFIG_FB_XILINX)           += xilinxfb.o
 obj-$(CONFIG_SH_MIPI_DSI)	  += sh_mipi_dsi.o
-obj-$(CONFIG_FB_SH_MOBILE_HDMI)	  += sh_mobile_hdmi.o
 obj-$(CONFIG_FB_SH_MOBILE_MERAM)  += sh_mobile_meram.o
 obj-$(CONFIG_FB_SH_MOBILE_LCDC)	  += sh_mobile_lcdcfb.o
 obj-$(CONFIG_FB_OMAP)             += omap/
diff --git a/drivers/video/fbdev/auo_k190x.c b/drivers/video/fbdev/auo_k190x.c
index 8d2499d1cafb..9580374667ba 100644
--- a/drivers/video/fbdev/auo_k190x.c
+++ b/drivers/video/fbdev/auo_k190x.c
@@ -773,9 +773,7 @@ static void auok190x_recover(struct auok190xfb_par *par)
 /*
  * Power-management
  */
-
-#ifdef CONFIG_PM
-static int auok190x_runtime_suspend(struct device *dev)
+static int __maybe_unused auok190x_runtime_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct fb_info *info = platform_get_drvdata(pdev);
@@ -822,7 +820,7 @@ finish:
 	return 0;
 }
 
-static int auok190x_runtime_resume(struct device *dev)
+static int __maybe_unused auok190x_runtime_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct fb_info *info = platform_get_drvdata(pdev);
@@ -856,7 +854,7 @@ static int auok190x_runtime_resume(struct device *dev)
 	return 0;
 }
 
-static int auok190x_suspend(struct device *dev)
+static int __maybe_unused auok190x_suspend(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct fb_info *info = platform_get_drvdata(pdev);
@@ -896,7 +894,7 @@ static int auok190x_suspend(struct device *dev)
 	return 0;
 }
 
-static int auok190x_resume(struct device *dev)
+static int __maybe_unused auok190x_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct fb_info *info = platform_get_drvdata(pdev);
@@ -933,7 +931,6 @@ static int auok190x_resume(struct device *dev)
 
 	return 0;
 }
-#endif
 
 const struct dev_pm_ops auok190x_pm = {
 	SET_RUNTIME_PM_OPS(auok190x_runtime_suspend, auok190x_runtime_resume,
diff --git a/drivers/video/fbdev/core/Makefile b/drivers/video/fbdev/core/Makefile
index 23d86a8b7d7b..9e3ddf225393 100644
--- a/drivers/video/fbdev/core/Makefile
+++ b/drivers/video/fbdev/core/Makefile
@@ -1,5 +1,5 @@
-obj-y                             += fb_notify.o
 obj-$(CONFIG_FB_CMDLINE)          += fb_cmdline.o
+obj-$(CONFIG_FB_NOTIFY)           += fb_notify.o
 obj-$(CONFIG_FB)                  += fb.o
 fb-y                              := fbmem.o fbmon.o fbcmap.o fbsysfs.o \
                                      modedb.o fbcvt.o
diff --git a/drivers/video/fbdev/core/fb_defio.c b/drivers/video/fbdev/core/fb_defio.c
index 3fc63c208d08..57721c73177f 100644
--- a/drivers/video/fbdev/core/fb_defio.c
+++ b/drivers/video/fbdev/core/fb_defio.c
@@ -78,13 +78,13 @@ int fb_deferred_io_fsync(struct file *file, loff_t start, loff_t end, int datasy
 	if (!info->fbdefio)
 		return 0;
 
-	mutex_lock(&inode->i_mutex);
+	inode_lock(inode);
 	/* Kill off the delayed work */
 	cancel_delayed_work_sync(&info->deferred_work);
 
 	/* Run it immediately */
 	schedule_delayed_work(&info->deferred_work, 0);
-	mutex_unlock(&inode->i_mutex);
+	inode_unlock(inode);
 
 	return 0;
 }
diff --git a/drivers/video/fbdev/core/fbmem.c b/drivers/video/fbdev/core/fbmem.c
index 0705d8883ede..4e73b6f6b1c0 100644
--- a/drivers/video/fbdev/core/fbmem.c
+++ b/drivers/video/fbdev/core/fbmem.c
@@ -1608,6 +1608,11 @@ static int do_remove_conflicting_framebuffers(struct apertures_struct *a,
 	return 0;
 }
 
+static bool lockless_register_fb;
+module_param_named_unsafe(lockless_register_fb, lockless_register_fb, bool, 0400);
+MODULE_PARM_DESC(lockless_register_fb,
+	"Lockless framebuffer registration for debugging [default=off]");
+
 static int do_register_framebuffer(struct fb_info *fb_info)
 {
 	int i, ret;
@@ -1675,15 +1680,18 @@ static int do_register_framebuffer(struct fb_info *fb_info)
 	registered_fb[i] = fb_info;
 
 	event.info = fb_info;
-	console_lock();
+	if (!lockless_register_fb)
+		console_lock();
 	if (!lock_fb_info(fb_info)) {
-		console_unlock();
+		if (!lockless_register_fb)
+			console_unlock();
 		return -ENODEV;
 	}
 
 	fb_notifier_call_chain(FB_EVENT_FB_REGISTERED, &event);
 	unlock_fb_info(fb_info);
-	console_unlock();
+	if (!lockless_register_fb)
+		console_unlock();
 	return 0;
 }
 
diff --git a/drivers/video/fbdev/da8xx-fb.c b/drivers/video/fbdev/da8xx-fb.c
index 0081725c6b5b..6b2a06d09f2b 100644
--- a/drivers/video/fbdev/da8xx-fb.c
+++ b/drivers/video/fbdev/da8xx-fb.c
@@ -152,7 +152,7 @@ static void lcdc_write(unsigned int val, unsigned int addr)
 
 struct da8xx_fb_par {
 	struct device		*dev;
-	resource_size_t p_palette_base;
+	dma_addr_t		p_palette_base;
 	unsigned char *v_palette_base;
 	dma_addr_t		vram_phys;
 	unsigned long		vram_size;
@@ -1428,7 +1428,7 @@ static int fb_probe(struct platform_device *device)
 
 	par->vram_virt = dma_alloc_coherent(NULL,
 					    par->vram_size,
-					    (resource_size_t *) &par->vram_phys,
+					    &par->vram_phys,
 					    GFP_KERNEL | GFP_DMA);
 	if (!par->vram_virt) {
 		dev_err(&device->dev,
@@ -1448,7 +1448,7 @@ static int fb_probe(struct platform_device *device)
 
 	/* allocate palette buffer */
 	par->v_palette_base = dma_zalloc_coherent(NULL, PALETTE_SIZE,
-						  (resource_size_t *)&par->p_palette_base,
+						  &par->p_palette_base,
 						  GFP_KERNEL | GFP_DMA);
 	if (!par->v_palette_base) {
 		dev_err(&device->dev,
diff --git a/drivers/video/fbdev/exynos/s6e8ax0.c b/drivers/video/fbdev/exynos/s6e8ax0.c
index 95873f26e39c..de2f3e793786 100644
--- a/drivers/video/fbdev/exynos/s6e8ax0.c
+++ b/drivers/video/fbdev/exynos/s6e8ax0.c
@@ -829,8 +829,7 @@ static int s6e8ax0_probe(struct mipi_dsim_lcd_device *dsim_dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM
-static int s6e8ax0_suspend(struct mipi_dsim_lcd_device *dsim_dev)
+static int __maybe_unused s6e8ax0_suspend(struct mipi_dsim_lcd_device *dsim_dev)
 {
 	struct s6e8ax0 *lcd = dev_get_drvdata(&dsim_dev->dev);
 
@@ -843,7 +842,7 @@ static int s6e8ax0_suspend(struct mipi_dsim_lcd_device *dsim_dev)
 	return 0;
 }
 
-static int s6e8ax0_resume(struct mipi_dsim_lcd_device *dsim_dev)
+static int __maybe_unused s6e8ax0_resume(struct mipi_dsim_lcd_device *dsim_dev)
 {
 	struct s6e8ax0 *lcd = dev_get_drvdata(&dsim_dev->dev);
 
@@ -855,10 +854,6 @@ static int s6e8ax0_resume(struct mipi_dsim_lcd_device *dsim_dev)
 
 	return 0;
 }
-#else
-#define s6e8ax0_suspend		NULL
-#define s6e8ax0_resume		NULL
-#endif
 
 static struct mipi_dsim_lcd_driver s6e8ax0_dsim_ddi_driver = {
 	.name = "s6e8ax0",
@@ -867,8 +862,8 @@ static struct mipi_dsim_lcd_driver s6e8ax0_dsim_ddi_driver = {
 	.power_on = s6e8ax0_power_on,
 	.set_sequence = s6e8ax0_set_sequence,
 	.probe = s6e8ax0_probe,
-	.suspend = s6e8ax0_suspend,
-	.resume = s6e8ax0_resume,
+	.suspend = IS_ENABLED(CONFIG_PM) ? s6e8ax0_suspend : NULL,
+	.resume = IS_ENABLED(CONFIG_PM) ? s6e8ax0_resume : NULL,
 };
 
 static int s6e8ax0_init(void)
diff --git a/drivers/video/fbdev/geode/display_gx1.c b/drivers/video/fbdev/geode/display_gx1.c
index 926d53eeb549..b383eb9882bf 100644
--- a/drivers/video/fbdev/geode/display_gx1.c
+++ b/drivers/video/fbdev/geode/display_gx1.c
@@ -208,7 +208,7 @@ static void gx1_set_hw_palette_reg(struct fb_info *info, unsigned regno,
 	writel(val, par->dc_regs + DC_PAL_DATA);
 }
 
-struct geode_dc_ops gx1_dc_ops = {
+const struct geode_dc_ops gx1_dc_ops = {
 	.set_mode	 = gx1_set_mode,
 	.set_palette_reg = gx1_set_hw_palette_reg,
 };
diff --git a/drivers/video/fbdev/geode/display_gx1.h b/drivers/video/fbdev/geode/display_gx1.h
index 671c05558c79..e1cc41b343ca 100644
--- a/drivers/video/fbdev/geode/display_gx1.h
+++ b/drivers/video/fbdev/geode/display_gx1.h
@@ -18,7 +18,7 @@
 unsigned gx1_gx_base(void);
 int gx1_frame_buffer_size(void);
 
-extern struct geode_dc_ops gx1_dc_ops;
+extern const struct geode_dc_ops gx1_dc_ops;
 
 /* GX1 configuration I/O registers */
 
diff --git a/drivers/video/fbdev/geode/geodefb.h b/drivers/video/fbdev/geode/geodefb.h
index ae04820e0c57..e2e07934868f 100644
--- a/drivers/video/fbdev/geode/geodefb.h
+++ b/drivers/video/fbdev/geode/geodefb.h
@@ -31,8 +31,8 @@ struct geodefb_par {
 	int panel_y;
 	void __iomem *dc_regs;
 	void __iomem *vid_regs;
-	struct geode_dc_ops  *dc_ops;
-	struct geode_vid_ops *vid_ops;
+	const struct geode_dc_ops  *dc_ops;
+	const struct geode_vid_ops *vid_ops;
 };
 
 #endif /* !__GEODEFB_H__ */
diff --git a/drivers/video/fbdev/geode/video_cs5530.c b/drivers/video/fbdev/geode/video_cs5530.c
index 649c3943d431..880613200ecf 100644
--- a/drivers/video/fbdev/geode/video_cs5530.c
+++ b/drivers/video/fbdev/geode/video_cs5530.c
@@ -186,7 +186,7 @@ static int cs5530_blank_display(struct fb_info *info, int blank_mode)
 	return 0;
 }
 
-struct geode_vid_ops cs5530_vid_ops = {
+const struct geode_vid_ops cs5530_vid_ops = {
 	.set_dclk          = cs5530_set_dclk_frequency,
 	.configure_display = cs5530_configure_display,
 	.blank_display     = cs5530_blank_display,
diff --git a/drivers/video/fbdev/geode/video_cs5530.h b/drivers/video/fbdev/geode/video_cs5530.h
index 56cecca7f1ce..c843348bfa20 100644
--- a/drivers/video/fbdev/geode/video_cs5530.h
+++ b/drivers/video/fbdev/geode/video_cs5530.h
@@ -15,7 +15,7 @@
 #ifndef __VIDEO_CS5530_H__
 #define __VIDEO_CS5530_H__
 
-extern struct geode_vid_ops cs5530_vid_ops;
+extern const struct geode_vid_ops cs5530_vid_ops;
 
 /* CS5530 Video device registers */
 
diff --git a/drivers/video/fbdev/i740fb.c b/drivers/video/fbdev/i740fb.c
index 452e1163ad02..cf5ccd0f2252 100644
--- a/drivers/video/fbdev/i740fb.c
+++ b/drivers/video/fbdev/i740fb.c
@@ -346,11 +346,10 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
 	const u32 err_target = freq / (1000 * I740_RFREQ / I740_FFIX);
 	u32 err_best = 512 * I740_FFIX;
 	u32 f_err, f_vco;
-	int m_best = 0, n_best = 0, p_best = 0, d_best = 0;
+	int m_best = 0, n_best = 0, p_best = 0;
 	int m, n;
 
 	p_best = min(15, ilog2(I740_MAX_VCO_FREQ / (freq / I740_RFREQ_FIX)));
-	d_best = 0;
 	f_vco = (freq * (1 << p_best)) / I740_RFREQ_FIX;
 	freq = freq / I740_RFREQ_FIX;
 
@@ -363,7 +362,7 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
 			m = 3;
 
 		{
-			u32 f_out = (((m * I740_REF_FREQ * (4 << 2 * d_best))
+			u32 f_out = (((m * I740_REF_FREQ * 4)
 				 / n) + ((1 << p_best) / 2)) / (1 << p_best);
 
 			f_err = (freq - f_out);
@@ -386,8 +385,7 @@ static void i740_calc_vclk(u32 freq, struct i740fb_par *par)
 	par->video_clk2_n = (n_best - 2) & 0xFF;
 	par->video_clk2_mn_msbs = ((((n_best - 2) >> 4) & VCO_N_MSBS)
 				 | (((m_best - 2) >> 8) & VCO_M_MSBS));
-	par->video_clk2_div_sel =
-		((p_best << 4) | (d_best ? 4 : 0) | REF_DIV_1);
+	par->video_clk2_div_sel = ((p_best << 4) | REF_DIV_1);
 }
 
 static int i740fb_decode_var(const struct fb_var_screeninfo *var,
diff --git a/drivers/video/fbdev/imxfb.c b/drivers/video/fbdev/imxfb.c
index cee88603efc9..bb2f1e866020 100644
--- a/drivers/video/fbdev/imxfb.c
+++ b/drivers/video/fbdev/imxfb.c
@@ -902,6 +902,21 @@ static int imxfb_probe(struct platform_device *pdev)
 		goto failed_getclock;
 	}
 
+	/*
+	 * The LCDC controller does not have an enable bit. The
+	 * controller starts directly when the clocks are enabled.
+	 * If the clocks are enabled when the controller is not yet
+	 * programmed with proper register values (enabled at the
+	 * bootloader, for example) then it just goes into some undefined
+	 * state.
+	 * To avoid this issue, let's enable and disable LCDC IPG clock
+	 * so that we force some kind of 'reset' to the LCDC block.
+	 */
+	ret = clk_prepare_enable(fbi->clk_ipg);
+	if (ret)
+		goto failed_getclock;
+	clk_disable_unprepare(fbi->clk_ipg);
+
 	fbi->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
 	if (IS_ERR(fbi->clk_ahb)) {
 		ret = PTR_ERR(fbi->clk_ahb);
diff --git a/drivers/video/fbdev/mmp/hw/mmp_ctrl.c b/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
index de54a4748065..b6f83d5df9fd 100644
--- a/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
+++ b/drivers/video/fbdev/mmp/hw/mmp_ctrl.c
@@ -503,8 +503,7 @@ static int mmphw_probe(struct platform_device *pdev)
 	ctrl->reg_base = devm_ioremap_nocache(ctrl->dev,
 			res->start, resource_size(res));
 	if (ctrl->reg_base == NULL) {
-		dev_err(ctrl->dev, "%s: res %x - %x map failed\n", __func__,
-			res->start, res->end);
+		dev_err(ctrl->dev, "%s: res %pR map failed\n", __func__, res);
 		ret = -ENOMEM;
 		goto failed;
 	}
diff --git a/drivers/video/fbdev/ocfb.c b/drivers/video/fbdev/ocfb.c
index c9293aea8ec3..a970edc2a6f8 100644
--- a/drivers/video/fbdev/ocfb.c
+++ b/drivers/video/fbdev/ocfb.c
@@ -123,11 +123,11 @@ static int ocfb_setupfb(struct ocfb_dev *fbdev)
 
 	/* Horizontal timings */
 	ocfb_writereg(fbdev, OCFB_HTIM, (var->hsync_len - 1) << 24 |
-		      (var->right_margin - 1) << 16 | (var->xres - 1));
+		      (var->left_margin - 1) << 16 | (var->xres - 1));
 
 	/* Vertical timings */
 	ocfb_writereg(fbdev, OCFB_VTIM, (var->vsync_len - 1) << 24 |
-		      (var->lower_margin - 1) << 16 | (var->yres - 1));
+		      (var->upper_margin - 1) << 16 | (var->yres - 1));
 
 	/* Total length of frame */
 	hlen = var->left_margin + var->right_margin + var->hsync_len +
diff --git a/drivers/video/fbdev/omap2/Kconfig b/drivers/video/fbdev/omap2/Kconfig
index c22955d2de9a..0921c4de8407 100644
--- a/drivers/video/fbdev/omap2/Kconfig
+++ b/drivers/video/fbdev/omap2/Kconfig
@@ -1,10 +1,5 @@
-config OMAP2_VRFB
-	bool
-
 if ARCH_OMAP2PLUS
 
-source "drivers/video/fbdev/omap2/dss/Kconfig"
 source "drivers/video/fbdev/omap2/omapfb/Kconfig"
-source "drivers/video/fbdev/omap2/displays-new/Kconfig"
 
 endif
diff --git a/drivers/video/fbdev/omap2/Makefile b/drivers/video/fbdev/omap2/Makefile
index f8745ec369cc..71ab5ac91106 100644
--- a/drivers/video/fbdev/omap2/Makefile
+++ b/drivers/video/fbdev/omap2/Makefile
@@ -1,5 +1 @@
-obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
-
-obj-y += dss/
-obj-y += displays-new/
-obj-$(CONFIG_FB_OMAP2) += omapfb/
+obj-y += omapfb/
diff --git a/drivers/video/fbdev/omap2/omapfb/Kconfig b/drivers/video/fbdev/omap2/omapfb/Kconfig
index 4cb12ce68855..e6226aeed17e 100644
--- a/drivers/video/fbdev/omap2/omapfb/Kconfig
+++ b/drivers/video/fbdev/omap2/omapfb/Kconfig
@@ -1,7 +1,12 @@
+config OMAP2_VRFB
+	bool
+
 menuconfig FB_OMAP2
         tristate "OMAP2+ frame buffer support"
-        depends on FB && OMAP2_DSS && !DRM_OMAP
+        depends on FB
+        depends on DRM_OMAP = n
 
+        select FB_OMAP2_DSS
 	select OMAP2_VRFB if ARCH_OMAP2 || ARCH_OMAP3
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
@@ -9,6 +14,8 @@ menuconfig FB_OMAP2
         help
 	  Frame buffer driver for OMAP2+ based boards.
 
+if FB_OMAP2
+
 config FB_OMAP2_DEBUG_SUPPORT
         bool "Debug support for OMAP2+ FB"
 	default y
@@ -25,3 +32,8 @@ config FB_OMAP2_NUM_FBS
 	help
 	  Select the number of framebuffers created. OMAP2/3 has 3 overlays
 	  so normally this would be 3.
+
+source "drivers/video/fbdev/omap2/omapfb/dss/Kconfig"
+source "drivers/video/fbdev/omap2/omapfb/displays/Kconfig"
+
+endif
diff --git a/drivers/video/fbdev/omap2/omapfb/Makefile b/drivers/video/fbdev/omap2/omapfb/Makefile
index 51c2e00d9bf8..ad68ecf141af 100644
--- a/drivers/video/fbdev/omap2/omapfb/Makefile
+++ b/drivers/video/fbdev/omap2/omapfb/Makefile
@@ -1,2 +1,5 @@
+obj-$(CONFIG_OMAP2_VRFB) += vrfb.o
+obj-y += dss/
+obj-y += displays/
 obj-$(CONFIG_FB_OMAP2) += omapfb.o
 omapfb-y := omapfb-main.o omapfb-sysfs.o omapfb-ioctl.o
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/Kconfig b/drivers/video/fbdev/omap2/omapfb/displays/Kconfig
new file mode 100644
index 000000000000..08f12039dd02
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/Kconfig
@@ -0,0 +1,86 @@
+menu "OMAPFB Panel and Encoder Drivers"
+        depends on FB_OMAP2_DSS
+
+config FB_OMAP2_ENCODER_OPA362
+	tristate "OPA362 external analog amplifier"
+	help
+	  Driver for OPA362 external analog TV amplifier controlled
+	  through a GPIO.
+
+config FB_OMAP2_ENCODER_TFP410
+        tristate "TFP410 DPI to DVI Encoder"
+	help
+	  Driver for TFP410 DPI to DVI encoder.
+
+config FB_OMAP2_ENCODER_TPD12S015
+        tristate "TPD12S015 HDMI ESD protection and level shifter"
+	help
+	  Driver for TPD12S015, which offers HDMI ESD protection and level
+	  shifting.
+
+config FB_OMAP2_CONNECTOR_DVI
+        tristate "DVI Connector"
+	depends on I2C
+	help
+	  Driver for a generic DVI connector.
+
+config FB_OMAP2_CONNECTOR_HDMI
+        tristate "HDMI Connector"
+	help
+	  Driver for a generic HDMI connector.
+
+config FB_OMAP2_CONNECTOR_ANALOG_TV
+        tristate "Analog TV Connector"
+	help
+	  Driver for a generic analog TV connector.
+
+config FB_OMAP2_PANEL_DPI
+	tristate "Generic DPI panel"
+	help
+	  Driver for generic DPI panels.
+
+config FB_OMAP2_PANEL_DSI_CM
+	tristate "Generic DSI Command Mode Panel"
+	depends on BACKLIGHT_CLASS_DEVICE
+	help
+	  Driver for generic DSI command mode panels.
+
+config FB_OMAP2_PANEL_SONY_ACX565AKM
+	tristate "ACX565AKM Panel"
+	depends on SPI && BACKLIGHT_CLASS_DEVICE
+	help
+	  This is the LCD panel used on Nokia N900
+
+config FB_OMAP2_PANEL_LGPHILIPS_LB035Q02
+	tristate "LG.Philips LB035Q02 LCD Panel"
+	depends on SPI
+	help
+	  LCD Panel used on the Gumstix Overo Palo35
+
+config FB_OMAP2_PANEL_SHARP_LS037V7DW01
+        tristate "Sharp LS037V7DW01 LCD Panel"
+        depends on BACKLIGHT_CLASS_DEVICE
+        help
+          LCD Panel used in TI's SDP3430 and EVM boards
+
+config FB_OMAP2_PANEL_TPO_TD028TTEC1
+        tristate "TPO TD028TTEC1 LCD Panel"
+        depends on SPI
+        help
+          LCD panel used in Openmoko.
+
+config FB_OMAP2_PANEL_TPO_TD043MTEA1
+        tristate "TPO TD043MTEA1 LCD Panel"
+        depends on SPI
+        help
+          LCD Panel used in OMAP3 Pandora
+
+config FB_OMAP2_PANEL_NEC_NL8048HL11
+	tristate "NEC NL8048HL11 Panel"
+	depends on SPI
+	depends on BACKLIGHT_CLASS_DEVICE
+	help
+		This NEC NL8048HL11 panel is TFT LCD used in the
+		Zoom2/3/3630 sdp boards.
+
+endmenu
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/Makefile b/drivers/video/fbdev/omap2/omapfb/displays/Makefile
new file mode 100644
index 000000000000..4f7459272256
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/Makefile
@@ -0,0 +1,14 @@
+obj-$(CONFIG_FB_OMAP2_ENCODER_OPA362) += encoder-opa362.o
+obj-$(CONFIG_FB_OMAP2_ENCODER_TFP410) += encoder-tfp410.o
+obj-$(CONFIG_FB_OMAP2_ENCODER_TPD12S015) += encoder-tpd12s015.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_DVI) += connector-dvi.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_HDMI) += connector-hdmi.o
+obj-$(CONFIG_FB_OMAP2_CONNECTOR_ANALOG_TV) += connector-analog-tv.o
+obj-$(CONFIG_FB_OMAP2_PANEL_DPI) += panel-dpi.o
+obj-$(CONFIG_FB_OMAP2_PANEL_DSI_CM) += panel-dsi-cm.o
+obj-$(CONFIG_FB_OMAP2_PANEL_SONY_ACX565AKM) += panel-sony-acx565akm.o
+obj-$(CONFIG_FB_OMAP2_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
+obj-$(CONFIG_FB_OMAP2_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
+obj-$(CONFIG_FB_OMAP2_PANEL_TPO_TD028TTEC1) += panel-tpo-td028ttec1.o
+obj-$(CONFIG_FB_OMAP2_PANEL_TPO_TD043MTEA1) += panel-tpo-td043mtea1.o
+obj-$(CONFIG_FB_OMAP2_PANEL_NEC_NL8048HL11) += panel-nec-nl8048hl11.o
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c
new file mode 100644
index 000000000000..8511c648a15c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-analog-tv.c
@@ -0,0 +1,320 @@
+/*
+ * Analog TV Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct device *dev;
+
+	struct omap_video_timings timings;
+
+	enum omap_dss_venc_type connector_type;
+	bool invert_polarity;
+};
+
+static const struct omap_video_timings tvc_pal_timings = {
+	.x_res		= 720,
+	.y_res		= 574,
+	.pixelclock	= 13500000,
+	.hsw		= 64,
+	.hfp		= 12,
+	.hbp		= 68,
+	.vsw		= 5,
+	.vfp		= 5,
+	.vbp		= 41,
+
+	.interlace	= true,
+};
+
+static const struct of_device_id tvc_of_match[];
+
+struct tvc_of_data {
+	enum omap_dss_venc_type connector_type;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tvc_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(ddata->dev, "connect\n");
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.atv->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void tvc_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(ddata->dev, "disconnect\n");
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.atv->disconnect(in, dssdev);
+}
+
+static int tvc_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(ddata->dev, "enable\n");
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	in->ops.atv->set_timings(in, &ddata->timings);
+
+	if (!ddata->dev->of_node) {
+		in->ops.atv->set_type(in, ddata->connector_type);
+
+		in->ops.atv->invert_vid_out_polarity(in,
+			ddata->invert_polarity);
+	}
+
+	r = in->ops.atv->enable(in);
+	if (r)
+		return r;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return r;
+}
+
+static void tvc_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(ddata->dev, "disable\n");
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	in->ops.atv->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tvc_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.atv->set_timings(in, timings);
+}
+
+static void tvc_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->timings;
+}
+
+static int tvc_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.atv->check_timings(in, timings);
+}
+
+static u32 tvc_get_wss(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.atv->get_wss(in);
+}
+
+static int tvc_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.atv->set_wss(in, wss);
+}
+
+static struct omap_dss_driver tvc_driver = {
+	.connect		= tvc_connect,
+	.disconnect		= tvc_disconnect,
+
+	.enable			= tvc_enable,
+	.disable		= tvc_disable,
+
+	.set_timings		= tvc_set_timings,
+	.get_timings		= tvc_get_timings,
+	.check_timings		= tvc_check_timings,
+
+	.get_resolution		= omapdss_default_get_resolution,
+
+	.get_wss		= tvc_get_wss,
+	.set_wss		= tvc_set_wss,
+};
+
+static int tvc_probe_pdata(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct connector_atv_platform_data *pdata;
+	struct omap_dss_device *in, *dssdev;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "Failed to find video source\n");
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	ddata->connector_type = pdata->connector_type;
+	ddata->invert_polarity = pdata->invert_polarity;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int tvc_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int tvc_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+	ddata->dev = &pdev->dev;
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = tvc_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = tvc_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->timings = tvc_pal_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->driver = &tvc_driver;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_VENC;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = tvc_pal_timings;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+err_reg:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit tvc_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(&ddata->dssdev);
+
+	tvc_disable(dssdev);
+	tvc_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id tvc_of_match[] = {
+	{ .compatible = "omapdss,svideo-connector", },
+	{ .compatible = "omapdss,composite-video-connector", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, tvc_of_match);
+
+static struct platform_driver tvc_connector_driver = {
+	.probe	= tvc_probe,
+	.remove	= __exit_p(tvc_remove),
+	.driver	= {
+		.name	= "connector-analog-tv",
+		.of_match_table = tvc_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(tvc_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Analog TV Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c
new file mode 100644
index 000000000000..d811e6dcaef7
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-dvi.c
@@ -0,0 +1,398 @@
+/*
+ * Generic DVI Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <drm/drm_edid.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static const struct omap_video_timings dvic_default_timings = {
+	.x_res		= 640,
+	.y_res		= 480,
+
+	.pixelclock	= 23500000,
+
+	.hfp		= 48,
+	.hsw		= 32,
+	.hbp		= 80,
+
+	.vfp		= 3,
+	.vsw		= 4,
+	.vbp		= 7,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct omap_video_timings timings;
+
+	struct i2c_adapter *i2c_adapter;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int dvic_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dvi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void dvic_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dvi->disconnect(in, dssdev);
+}
+
+static int dvic_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	in->ops.dvi->set_timings(in, &ddata->timings);
+
+	r = in->ops.dvi->enable(in);
+	if (r)
+		return r;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void dvic_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	in->ops.dvi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void dvic_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dvi->set_timings(in, timings);
+}
+
+static void dvic_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->timings;
+}
+
+static int dvic_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dvi->check_timings(in, timings);
+}
+
+static int dvic_ddc_read(struct i2c_adapter *adapter,
+		unsigned char *buf, u16 count, u8 offset)
+{
+	int r, retries;
+
+	for (retries = 3; retries > 0; retries--) {
+		struct i2c_msg msgs[] = {
+			{
+				.addr   = DDC_ADDR,
+				.flags  = 0,
+				.len    = 1,
+				.buf    = &offset,
+			}, {
+				.addr   = DDC_ADDR,
+				.flags  = I2C_M_RD,
+				.len    = count,
+				.buf    = buf,
+			}
+		};
+
+		r = i2c_transfer(adapter, msgs, 2);
+		if (r == 2)
+			return 0;
+
+		if (r != -EAGAIN)
+			break;
+	}
+
+	return r < 0 ? r : -EIO;
+}
+
+static int dvic_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	int r, l, bytes_read;
+
+	if (!ddata->i2c_adapter)
+		return -ENODEV;
+
+	l = min(EDID_LENGTH, len);
+	r = dvic_ddc_read(ddata->i2c_adapter, edid, l, 0);
+	if (r)
+		return r;
+
+	bytes_read = l;
+
+	/* if there are extensions, read second block */
+	if (len > EDID_LENGTH && edid[0x7e] > 0) {
+		l = min(EDID_LENGTH, len - EDID_LENGTH);
+
+		r = dvic_ddc_read(ddata->i2c_adapter, edid + EDID_LENGTH,
+				l, EDID_LENGTH);
+		if (r)
+			return r;
+
+		bytes_read += l;
+	}
+
+	return bytes_read;
+}
+
+static bool dvic_detect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	unsigned char out;
+	int r;
+
+	if (!ddata->i2c_adapter)
+		return true;
+
+	r = dvic_ddc_read(ddata->i2c_adapter, &out, 1, 0);
+
+	return r == 0;
+}
+
+static struct omap_dss_driver dvic_driver = {
+	.connect	= dvic_connect,
+	.disconnect	= dvic_disconnect,
+
+	.enable		= dvic_enable,
+	.disable	= dvic_disable,
+
+	.set_timings	= dvic_set_timings,
+	.get_timings	= dvic_get_timings,
+	.check_timings	= dvic_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+
+	.read_edid	= dvic_read_edid,
+	.detect		= dvic_detect,
+};
+
+static int dvic_probe_pdata(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct connector_dvi_platform_data *pdata;
+	struct omap_dss_device *in, *dssdev;
+	int i2c_bus_num;
+
+	pdata = dev_get_platdata(&pdev->dev);
+	i2c_bus_num = pdata->i2c_bus_num;
+
+	if (i2c_bus_num != -1) {
+		struct i2c_adapter *adapter;
+
+		adapter = i2c_get_adapter(i2c_bus_num);
+		if (!adapter) {
+			dev_err(&pdev->dev,
+					"Failed to get I2C adapter, bus %d\n",
+					i2c_bus_num);
+			return -EPROBE_DEFER;
+		}
+
+		ddata->i2c_adapter = adapter;
+	}
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		i2c_put_adapter(ddata->i2c_adapter);
+
+		dev_err(&pdev->dev, "Failed to find video source\n");
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int dvic_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	struct device_node *adapter_node;
+	struct i2c_adapter *adapter;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	adapter_node = of_parse_phandle(node, "ddc-i2c-bus", 0);
+	if (adapter_node) {
+		adapter = of_get_i2c_adapter_by_node(adapter_node);
+		if (adapter == NULL) {
+			dev_err(&pdev->dev, "failed to parse ddc-i2c-bus\n");
+			omap_dss_put_device(ddata->in);
+			return -EPROBE_DEFER;
+		}
+
+		ddata->i2c_adapter = adapter;
+	}
+
+	return 0;
+}
+
+static int dvic_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = dvic_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = dvic_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->timings = dvic_default_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->driver = &dvic_driver;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_DVI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = dvic_default_timings;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	omap_dss_put_device(ddata->in);
+
+	i2c_put_adapter(ddata->i2c_adapter);
+
+	return r;
+}
+
+static int __exit dvic_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(&ddata->dssdev);
+
+	dvic_disable(dssdev);
+	dvic_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	i2c_put_adapter(ddata->i2c_adapter);
+
+	return 0;
+}
+
+static const struct of_device_id dvic_of_match[] = {
+	{ .compatible = "omapdss,dvi-connector", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, dvic_of_match);
+
+static struct platform_driver dvi_connector_driver = {
+	.probe	= dvic_probe,
+	.remove	= __exit_p(dvic_remove),
+	.driver	= {
+		.name	= "connector-dvi",
+		.of_match_table = dvic_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(dvi_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic DVI Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c b/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c
new file mode 100644
index 000000000000..6ee4129bc0c0
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/connector-hdmi.c
@@ -0,0 +1,348 @@
+/*
+ * HDMI Connector driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <drm/drm_edid.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static const struct omap_video_timings hdmic_default_timings = {
+	.x_res		= 640,
+	.y_res		= 480,
+	.pixelclock	= 25175000,
+	.hsw		= 96,
+	.hfp		= 16,
+	.hbp		= 48,
+	.vsw		= 2,
+	.vfp		= 11,
+	.vbp		= 31,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+
+	.interlace	= false,
+};
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct device *dev;
+
+	struct omap_video_timings timings;
+
+	int hpd_gpio;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int hdmic_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(ddata->dev, "connect\n");
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.hdmi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void hdmic_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(ddata->dev, "disconnect\n");
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.hdmi->disconnect(in, dssdev);
+}
+
+static int hdmic_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(ddata->dev, "enable\n");
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	in->ops.hdmi->set_timings(in, &ddata->timings);
+
+	r = in->ops.hdmi->enable(in);
+	if (r)
+		return r;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return r;
+}
+
+static void hdmic_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(ddata->dev, "disable\n");
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	in->ops.hdmi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void hdmic_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.hdmi->set_timings(in, timings);
+}
+
+static void hdmic_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->timings;
+}
+
+static int hdmic_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->check_timings(in, timings);
+}
+
+static int hdmic_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->read_edid(in, edid, len);
+}
+
+static bool hdmic_detect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (gpio_is_valid(ddata->hpd_gpio))
+		return gpio_get_value_cansleep(ddata->hpd_gpio);
+	else
+		return in->ops.hdmi->detect(in);
+}
+
+static int hdmic_set_hdmi_mode(struct omap_dss_device *dssdev, bool hdmi_mode)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->set_hdmi_mode(in, hdmi_mode);
+}
+
+static int hdmic_set_infoframe(struct omap_dss_device *dssdev,
+		const struct hdmi_avi_infoframe *avi)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->set_infoframe(in, avi);
+}
+
+static struct omap_dss_driver hdmic_driver = {
+	.connect		= hdmic_connect,
+	.disconnect		= hdmic_disconnect,
+
+	.enable			= hdmic_enable,
+	.disable		= hdmic_disable,
+
+	.set_timings		= hdmic_set_timings,
+	.get_timings		= hdmic_get_timings,
+	.check_timings		= hdmic_check_timings,
+
+	.get_resolution		= omapdss_default_get_resolution,
+
+	.read_edid		= hdmic_read_edid,
+	.detect			= hdmic_detect,
+	.set_hdmi_mode		= hdmic_set_hdmi_mode,
+	.set_hdmi_infoframe	= hdmic_set_infoframe,
+};
+
+static int hdmic_probe_pdata(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct connector_hdmi_platform_data *pdata;
+	struct omap_dss_device *in, *dssdev;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	ddata->hpd_gpio = -ENODEV;
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "Failed to find video source\n");
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int hdmic_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int gpio;
+
+	/* HPD GPIO */
+	gpio = of_get_named_gpio(node, "hpd-gpios", 0);
+	if (gpio_is_valid(gpio))
+		ddata->hpd_gpio = gpio;
+	else
+		ddata->hpd_gpio = -ENODEV;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int hdmic_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+	ddata->dev = &pdev->dev;
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = hdmic_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = hdmic_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->hpd_gpio)) {
+		r = devm_gpio_request_one(&pdev->dev, ddata->hpd_gpio,
+				GPIOF_DIR_IN, "hdmi_hpd");
+		if (r)
+			goto err_reg;
+	}
+
+	ddata->timings = hdmic_default_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->driver = &hdmic_driver;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_HDMI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = hdmic_default_timings;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+err_reg:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit hdmic_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(&ddata->dssdev);
+
+	hdmic_disable(dssdev);
+	hdmic_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id hdmic_of_match[] = {
+	{ .compatible = "omapdss,hdmi-connector", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, hdmic_of_match);
+
+static struct platform_driver hdmi_connector_driver = {
+	.probe	= hdmic_probe,
+	.remove	= __exit_p(hdmic_remove),
+	.driver	= {
+		.name	= "connector-hdmi",
+		.of_match_table = hdmic_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(hdmi_connector_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("HDMI Connector driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c
new file mode 100644
index 000000000000..8c246c213e06
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-opa362.c
@@ -0,0 +1,278 @@
+/*
+ * OPA362 analog video amplifier with output/power control
+ *
+ * Copyright (C) 2014 Golden Delicious Computers
+ * Author: H. Nikolaus Schaller <hns@goldelico.com>
+ *
+ * based on encoder-tfp410
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct gpio_desc *enable_gpio;
+
+	struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int opa362_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(dssdev->dev, "connect\n");
+
+	if (omapdss_device_is_connected(dssdev))
+		return -EBUSY;
+
+	r = in->ops.atv->connect(in, dssdev);
+	if (r)
+		return r;
+
+	dst->src = dssdev;
+	dssdev->dst = dst;
+
+	return 0;
+}
+
+static void opa362_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(dssdev->dev, "disconnect\n");
+
+	WARN_ON(!omapdss_device_is_connected(dssdev));
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	WARN_ON(dst != dssdev->dst);
+	if (dst != dssdev->dst)
+		return;
+
+	dst->src = NULL;
+	dssdev->dst = NULL;
+
+	in->ops.atv->disconnect(in, &ddata->dssdev);
+}
+
+static int opa362_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(dssdev->dev, "enable\n");
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	in->ops.atv->set_timings(in, &ddata->timings);
+
+	r = in->ops.atv->enable(in);
+	if (r)
+		return r;
+
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void opa362_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(dssdev->dev, "disable\n");
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+	in->ops.atv->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void opa362_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(dssdev->dev, "set_timings\n");
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.atv->set_timings(in, timings);
+}
+
+static void opa362_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	dev_dbg(dssdev->dev, "get_timings\n");
+
+	*timings = ddata->timings;
+}
+
+static int opa362_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(dssdev->dev, "check_timings\n");
+
+	return in->ops.atv->check_timings(in, timings);
+}
+
+static void opa362_set_type(struct omap_dss_device *dssdev,
+		enum omap_dss_venc_type type)
+{
+	/* we can only drive a COMPOSITE output */
+	WARN_ON(type != OMAP_DSS_VENC_TYPE_COMPOSITE);
+
+}
+
+static const struct omapdss_atv_ops opa362_atv_ops = {
+	.connect	= opa362_connect,
+	.disconnect	= opa362_disconnect,
+
+	.enable		= opa362_enable,
+	.disable	= opa362_disable,
+
+	.check_timings	= opa362_check_timings,
+	.set_timings	= opa362_set_timings,
+	.get_timings	= opa362_get_timings,
+
+	.set_type	= opa362_set_type,
+};
+
+static int opa362_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev, *in;
+	struct gpio_desc *gpio;
+	int r;
+
+	dev_dbg(&pdev->dev, "probe\n");
+
+	if (node == NULL) {
+		dev_err(&pdev->dev, "Unable to find device tree\n");
+		return -EINVAL;
+	}
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		return PTR_ERR(gpio);
+
+	ddata->enable_gpio = gpio;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	dssdev = &ddata->dssdev;
+	dssdev->ops.atv = &opa362_atv_ops;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_VENC;
+	dssdev->output_type = OMAP_DISPLAY_TYPE_VENC;
+	dssdev->owner = THIS_MODULE;
+
+	r = omapdss_register_output(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register output\n");
+		goto err_reg;
+	}
+
+	return 0;
+err_reg:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit opa362_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_output(&ddata->dssdev);
+
+	WARN_ON(omapdss_device_is_enabled(dssdev));
+	if (omapdss_device_is_enabled(dssdev))
+		opa362_disable(dssdev);
+
+	WARN_ON(omapdss_device_is_connected(dssdev));
+	if (omapdss_device_is_connected(dssdev))
+		opa362_disconnect(dssdev, dssdev->dst);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id opa362_of_match[] = {
+	{ .compatible = "omapdss,ti,opa362", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, opa362_of_match);
+
+static struct platform_driver opa362_driver = {
+	.probe	= opa362_probe,
+	.remove	= __exit_p(opa362_remove),
+	.driver	= {
+		.name	= "amplifier-opa362",
+		.of_match_table = opa362_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(opa362_driver);
+
+MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
+MODULE_DESCRIPTION("OPA362 analog video amplifier with output/power control");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c
new file mode 100644
index 000000000000..d9048b3df495
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tfp410.c
@@ -0,0 +1,320 @@
+/*
+ * TFP410 DPI-to-DVI encoder driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	int pd_gpio;
+	int data_lines;
+
+	struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tfp410_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return -EBUSY;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	dst->src = dssdev;
+	dssdev->dst = dst;
+
+	return 0;
+}
+
+static void tfp410_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	WARN_ON(!omapdss_device_is_connected(dssdev));
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	WARN_ON(dst != dssdev->dst);
+	if (dst != dssdev->dst)
+		return;
+
+	dst->src = NULL;
+	dssdev->dst = NULL;
+
+	in->ops.dpi->disconnect(in, &ddata->dssdev);
+}
+
+static int tfp410_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	in->ops.dpi->set_timings(in, &ddata->timings);
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	if (gpio_is_valid(ddata->pd_gpio))
+		gpio_set_value_cansleep(ddata->pd_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void tfp410_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (gpio_is_valid(ddata->pd_gpio))
+		gpio_set_value_cansleep(ddata->pd_gpio, 0);
+
+	in->ops.dpi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tfp410_fix_timings(struct omap_video_timings *timings)
+{
+	timings->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	timings->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	timings->de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+}
+
+static void tfp410_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	tfp410_fix_timings(timings);
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void tfp410_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->timings;
+}
+
+static int tfp410_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	tfp410_fix_timings(timings);
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static const struct omapdss_dvi_ops tfp410_dvi_ops = {
+	.connect	= tfp410_connect,
+	.disconnect	= tfp410_disconnect,
+
+	.enable		= tfp410_enable,
+	.disable	= tfp410_disable,
+
+	.check_timings	= tfp410_check_timings,
+	.set_timings	= tfp410_set_timings,
+	.get_timings	= tfp410_get_timings,
+};
+
+static int tfp410_probe_pdata(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct encoder_tfp410_platform_data *pdata;
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	ddata->pd_gpio = pdata->power_down_gpio;
+
+	ddata->data_lines = pdata->data_lines;
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "Failed to find video source\n");
+		return -ENODEV;
+	}
+
+	ddata->in = in;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int tfp410_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "powerdown-gpios", 0);
+
+	if (gpio_is_valid(gpio) || gpio == -ENOENT) {
+		ddata->pd_gpio = gpio;
+	} else {
+		dev_err(&pdev->dev, "failed to parse PD gpio\n");
+		return gpio;
+	}
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int tfp410_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = tfp410_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = tfp410_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->pd_gpio)) {
+		r = devm_gpio_request_one(&pdev->dev, ddata->pd_gpio,
+				GPIOF_OUT_INIT_LOW, "tfp410 PD");
+		if (r) {
+			dev_err(&pdev->dev, "Failed to request PD GPIO %d\n",
+					ddata->pd_gpio);
+			goto err_gpio;
+		}
+	}
+
+	dssdev = &ddata->dssdev;
+	dssdev->ops.dvi = &tfp410_dvi_ops;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->output_type = OMAP_DISPLAY_TYPE_DVI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->phy.dpi.data_lines = ddata->data_lines;
+	dssdev->port_num = 1;
+
+	r = omapdss_register_output(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register output\n");
+		goto err_reg;
+	}
+
+	return 0;
+err_reg:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit tfp410_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_output(&ddata->dssdev);
+
+	WARN_ON(omapdss_device_is_enabled(dssdev));
+	if (omapdss_device_is_enabled(dssdev))
+		tfp410_disable(dssdev);
+
+	WARN_ON(omapdss_device_is_connected(dssdev));
+	if (omapdss_device_is_connected(dssdev))
+		tfp410_disconnect(dssdev, dssdev->dst);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id tfp410_of_match[] = {
+	{ .compatible = "omapdss,ti,tfp410", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, tfp410_of_match);
+
+static struct platform_driver tfp410_driver = {
+	.probe	= tfp410_probe,
+	.remove	= __exit_p(tfp410_remove),
+	.driver	= {
+		.name	= "tfp410",
+		.of_match_table = tfp410_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(tfp410_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("TFP410 DPI to DVI encoder driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c
new file mode 100644
index 000000000000..677e2545fcbe
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/encoder-tpd12s015.c
@@ -0,0 +1,328 @@
+/*
+ * TPD12S015 HDMI ESD protection & level shifter chip driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct gpio_desc *ct_cp_hpd_gpio;
+	struct gpio_desc *ls_oe_gpio;
+	struct gpio_desc *hpd_gpio;
+
+	struct omap_video_timings timings;
+};
+
+#define to_panel_data(x) container_of(x, struct panel_drv_data, dssdev)
+
+static int tpd_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	r = in->ops.hdmi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	dst->src = dssdev;
+	dssdev->dst = dst;
+
+	if (ddata->ct_cp_hpd_gpio) {
+		gpiod_set_value_cansleep(ddata->ct_cp_hpd_gpio, 1);
+		/* DC-DC converter needs at max 300us to get to 90% of 5V */
+		udelay(300);
+	}
+
+	return 0;
+}
+
+static void tpd_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	gpiod_set_value_cansleep(ddata->ct_cp_hpd_gpio, 0);
+
+	dst->src = NULL;
+	dssdev->dst = NULL;
+
+	in->ops.hdmi->disconnect(in, &ddata->dssdev);
+}
+
+static int tpd_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+		return 0;
+
+	in->ops.hdmi->set_timings(in, &ddata->timings);
+
+	r = in->ops.hdmi->enable(in);
+	if (r)
+		return r;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return r;
+}
+
+static void tpd_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+		return;
+
+	in->ops.hdmi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tpd_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->timings = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.hdmi->set_timings(in, timings);
+}
+
+static void tpd_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->timings;
+}
+
+static int tpd_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	r = in->ops.hdmi->check_timings(in, timings);
+
+	return r;
+}
+
+static int tpd_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!gpiod_get_value_cansleep(ddata->hpd_gpio))
+		return -ENODEV;
+
+	gpiod_set_value_cansleep(ddata->ls_oe_gpio, 1);
+
+	r = in->ops.hdmi->read_edid(in, edid, len);
+
+	gpiod_set_value_cansleep(ddata->ls_oe_gpio, 0);
+
+	return r;
+}
+
+static bool tpd_detect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	return gpiod_get_value_cansleep(ddata->hpd_gpio);
+}
+
+static int tpd_set_infoframe(struct omap_dss_device *dssdev,
+		const struct hdmi_avi_infoframe *avi)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->set_infoframe(in, avi);
+}
+
+static int tpd_set_hdmi_mode(struct omap_dss_device *dssdev,
+		bool hdmi_mode)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.hdmi->set_hdmi_mode(in, hdmi_mode);
+}
+
+static const struct omapdss_hdmi_ops tpd_hdmi_ops = {
+	.connect		= tpd_connect,
+	.disconnect		= tpd_disconnect,
+
+	.enable			= tpd_enable,
+	.disable		= tpd_disable,
+
+	.check_timings		= tpd_check_timings,
+	.set_timings		= tpd_set_timings,
+	.get_timings		= tpd_get_timings,
+
+	.read_edid		= tpd_read_edid,
+	.detect			= tpd_detect,
+	.set_infoframe		= tpd_set_infoframe,
+	.set_hdmi_mode		= tpd_set_hdmi_mode,
+};
+
+static int tpd_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int tpd_probe(struct platform_device *pdev)
+{
+	struct omap_dss_device *in, *dssdev;
+	struct panel_drv_data *ddata;
+	int r;
+	struct gpio_desc *gpio;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	if (pdev->dev.of_node) {
+		r = tpd_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+
+	gpio = devm_gpiod_get_index_optional(&pdev->dev, NULL, 0,
+		GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		goto err_gpio;
+
+	ddata->ct_cp_hpd_gpio = gpio;
+
+	gpio = devm_gpiod_get_index_optional(&pdev->dev, NULL, 1,
+		GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		goto err_gpio;
+
+	ddata->ls_oe_gpio = gpio;
+
+	gpio = devm_gpiod_get_index(&pdev->dev, NULL, 2,
+		GPIOD_IN);
+	if (IS_ERR(gpio))
+		goto err_gpio;
+
+	ddata->hpd_gpio = gpio;
+
+	dssdev = &ddata->dssdev;
+	dssdev->ops.hdmi = &tpd_hdmi_ops;
+	dssdev->dev = &pdev->dev;
+	dssdev->type = OMAP_DISPLAY_TYPE_HDMI;
+	dssdev->output_type = OMAP_DISPLAY_TYPE_HDMI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->port_num = 1;
+
+	in = ddata->in;
+
+	r = omapdss_register_output(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register output\n");
+		goto err_reg;
+	}
+
+	return 0;
+err_reg:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit tpd_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_output(&ddata->dssdev);
+
+	WARN_ON(omapdss_device_is_enabled(dssdev));
+	if (omapdss_device_is_enabled(dssdev))
+		tpd_disable(dssdev);
+
+	WARN_ON(omapdss_device_is_connected(dssdev));
+	if (omapdss_device_is_connected(dssdev))
+		tpd_disconnect(dssdev, dssdev->dst);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id tpd_of_match[] = {
+	{ .compatible = "omapdss,ti,tpd12s015", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, tpd_of_match);
+
+static struct platform_driver tpd_driver = {
+	.probe	= tpd_probe,
+	.remove	= __exit_p(tpd_remove),
+	.driver	= {
+		.name	= "tpd12s015",
+		.of_match_table = tpd_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(tpd_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("TPD12S015 driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c
new file mode 100644
index 000000000000..e780fd4f8b46
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-dpi.c
@@ -0,0 +1,328 @@
+/*
+ * Generic MIPI DPI Panel Driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+#include <video/of_display_timing.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	int data_lines;
+
+	struct omap_video_timings videomode;
+
+	/* used for non-DT boot, to be removed */
+	int backlight_gpio;
+
+	struct gpio_desc *enable_gpio;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int panel_dpi_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void panel_dpi_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int panel_dpi_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+	if (gpio_is_valid(ddata->backlight_gpio))
+		gpio_set_value_cansleep(ddata->backlight_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void panel_dpi_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (gpio_is_valid(ddata->backlight_gpio))
+		gpio_set_value_cansleep(ddata->backlight_gpio, 0);
+
+	gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+	in->ops.dpi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void panel_dpi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void panel_dpi_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int panel_dpi_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver panel_dpi_ops = {
+	.connect	= panel_dpi_connect,
+	.disconnect	= panel_dpi_disconnect,
+
+	.enable		= panel_dpi_enable,
+	.disable	= panel_dpi_disable,
+
+	.set_timings	= panel_dpi_set_timings,
+	.get_timings	= panel_dpi_get_timings,
+	.check_timings	= panel_dpi_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+static int panel_dpi_probe_pdata(struct platform_device *pdev)
+{
+	const struct panel_dpi_platform_data *pdata;
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev, *in;
+	struct videomode vm;
+	int r;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	videomode_from_timing(pdata->display_timing, &vm);
+	videomode_to_omap_video_timings(&vm, &ddata->videomode);
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	r = devm_gpio_request_one(&pdev->dev, pdata->enable_gpio,
+					GPIOF_OUT_INIT_LOW, "panel enable");
+	if (r)
+		goto err_gpio;
+
+	ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
+	ddata->backlight_gpio = pdata->backlight_gpio;
+
+	return 0;
+
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int panel_dpi_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int r;
+	struct display_timing timing;
+	struct videomode vm;
+	struct gpio_desc *gpio;
+
+	gpio = devm_gpiod_get_optional(&pdev->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio))
+		return PTR_ERR(gpio);
+
+	ddata->enable_gpio = gpio;
+
+	ddata->backlight_gpio = -ENOENT;
+
+	r = of_get_display_timing(node, "panel-timing", &timing);
+	if (r) {
+		dev_err(&pdev->dev, "failed to get video timing\n");
+		return r;
+	}
+
+	videomode_from_timing(&timing, &vm);
+	videomode_to_omap_video_timings(&vm, &ddata->videomode);
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int panel_dpi_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = panel_dpi_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = panel_dpi_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->backlight_gpio)) {
+		r = devm_gpio_request_one(&pdev->dev, ddata->backlight_gpio,
+				GPIOF_OUT_INIT_LOW, "panel backlight");
+		if (r)
+			goto err_gpio;
+	}
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &pdev->dev;
+	dssdev->driver = &panel_dpi_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+	dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit panel_dpi_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(dssdev);
+
+	panel_dpi_disable(dssdev);
+	panel_dpi_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id panel_dpi_of_match[] = {
+	{ .compatible = "omapdss,panel-dpi", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, panel_dpi_of_match);
+
+static struct platform_driver panel_dpi_driver = {
+	.probe = panel_dpi_probe,
+	.remove = __exit_p(panel_dpi_remove),
+	.driver = {
+		.name = "panel-dpi",
+		.of_match_table = panel_dpi_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(panel_dpi_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic MIPI DPI Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c
new file mode 100644
index 000000000000..3414c2609320
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-dsi-cm.c
@@ -0,0 +1,1388 @@
+/*
+ * Generic DSI Command Mode panel driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@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.
+ */
+
+/* #define DEBUG */
+
+#include <linux/backlight.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+#include <video/mipi_display.h>
+
+/* DSI Virtual channel. Hardcoded for now. */
+#define TCH 0
+
+#define DCS_READ_NUM_ERRORS	0x05
+#define DCS_BRIGHTNESS		0x51
+#define DCS_CTRL_DISPLAY	0x53
+#define DCS_GET_ID1		0xda
+#define DCS_GET_ID2		0xdb
+#define DCS_GET_ID3		0xdc
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct omap_video_timings timings;
+
+	struct platform_device *pdev;
+
+	struct mutex lock;
+
+	struct backlight_device *bldev;
+
+	unsigned long	hw_guard_end;	/* next value of jiffies when we can
+					 * issue the next sleep in/out command
+					 */
+	unsigned long	hw_guard_wait;	/* max guard time in jiffies */
+
+	/* panel HW configuration from DT or platform data */
+	int reset_gpio;
+	int ext_te_gpio;
+
+	bool use_dsi_backlight;
+
+	struct omap_dsi_pin_config pin_config;
+
+	/* runtime variables */
+	bool enabled;
+
+	bool te_enabled;
+
+	atomic_t do_update;
+	int channel;
+
+	struct delayed_work te_timeout_work;
+
+	bool intro_printed;
+
+	struct workqueue_struct *workqueue;
+
+	bool ulps_enabled;
+	unsigned ulps_timeout;
+	struct delayed_work ulps_work;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static irqreturn_t dsicm_te_isr(int irq, void *data);
+static void dsicm_te_timeout_work_callback(struct work_struct *work);
+static int _dsicm_enable_te(struct panel_drv_data *ddata, bool enable);
+
+static int dsicm_panel_reset(struct panel_drv_data *ddata);
+
+static void dsicm_ulps_work(struct work_struct *work);
+
+static void hw_guard_start(struct panel_drv_data *ddata, int guard_msec)
+{
+	ddata->hw_guard_wait = msecs_to_jiffies(guard_msec);
+	ddata->hw_guard_end = jiffies + ddata->hw_guard_wait;
+}
+
+static void hw_guard_wait(struct panel_drv_data *ddata)
+{
+	unsigned long wait = ddata->hw_guard_end - jiffies;
+
+	if ((long)wait > 0 && wait <= ddata->hw_guard_wait) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(wait);
+	}
+}
+
+static int dsicm_dcs_read_1(struct panel_drv_data *ddata, u8 dcs_cmd, u8 *data)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+	u8 buf[1];
+
+	r = in->ops.dsi->dcs_read(in, ddata->channel, dcs_cmd, buf, 1);
+
+	if (r < 0)
+		return r;
+
+	*data = buf[0];
+
+	return 0;
+}
+
+static int dsicm_dcs_write_0(struct panel_drv_data *ddata, u8 dcs_cmd)
+{
+	struct omap_dss_device *in = ddata->in;
+	return in->ops.dsi->dcs_write(in, ddata->channel, &dcs_cmd, 1);
+}
+
+static int dsicm_dcs_write_1(struct panel_drv_data *ddata, u8 dcs_cmd, u8 param)
+{
+	struct omap_dss_device *in = ddata->in;
+	u8 buf[2] = { dcs_cmd, param };
+
+	return in->ops.dsi->dcs_write(in, ddata->channel, buf, 2);
+}
+
+static int dsicm_sleep_in(struct panel_drv_data *ddata)
+
+{
+	struct omap_dss_device *in = ddata->in;
+	u8 cmd;
+	int r;
+
+	hw_guard_wait(ddata);
+
+	cmd = MIPI_DCS_ENTER_SLEEP_MODE;
+	r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, &cmd, 1);
+	if (r)
+		return r;
+
+	hw_guard_start(ddata, 120);
+
+	usleep_range(5000, 10000);
+
+	return 0;
+}
+
+static int dsicm_sleep_out(struct panel_drv_data *ddata)
+{
+	int r;
+
+	hw_guard_wait(ddata);
+
+	r = dsicm_dcs_write_0(ddata, MIPI_DCS_EXIT_SLEEP_MODE);
+	if (r)
+		return r;
+
+	hw_guard_start(ddata, 120);
+
+	usleep_range(5000, 10000);
+
+	return 0;
+}
+
+static int dsicm_get_id(struct panel_drv_data *ddata, u8 *id1, u8 *id2, u8 *id3)
+{
+	int r;
+
+	r = dsicm_dcs_read_1(ddata, DCS_GET_ID1, id1);
+	if (r)
+		return r;
+	r = dsicm_dcs_read_1(ddata, DCS_GET_ID2, id2);
+	if (r)
+		return r;
+	r = dsicm_dcs_read_1(ddata, DCS_GET_ID3, id3);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static int dsicm_set_update_window(struct panel_drv_data *ddata,
+		u16 x, u16 y, u16 w, u16 h)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+	u16 x1 = x;
+	u16 x2 = x + w - 1;
+	u16 y1 = y;
+	u16 y2 = y + h - 1;
+
+	u8 buf[5];
+	buf[0] = MIPI_DCS_SET_COLUMN_ADDRESS;
+	buf[1] = (x1 >> 8) & 0xff;
+	buf[2] = (x1 >> 0) & 0xff;
+	buf[3] = (x2 >> 8) & 0xff;
+	buf[4] = (x2 >> 0) & 0xff;
+
+	r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, buf, sizeof(buf));
+	if (r)
+		return r;
+
+	buf[0] = MIPI_DCS_SET_PAGE_ADDRESS;
+	buf[1] = (y1 >> 8) & 0xff;
+	buf[2] = (y1 >> 0) & 0xff;
+	buf[3] = (y2 >> 8) & 0xff;
+	buf[4] = (y2 >> 0) & 0xff;
+
+	r = in->ops.dsi->dcs_write_nosync(in, ddata->channel, buf, sizeof(buf));
+	if (r)
+		return r;
+
+	in->ops.dsi->bta_sync(in, ddata->channel);
+
+	return r;
+}
+
+static void dsicm_queue_ulps_work(struct panel_drv_data *ddata)
+{
+	if (ddata->ulps_timeout > 0)
+		queue_delayed_work(ddata->workqueue, &ddata->ulps_work,
+				msecs_to_jiffies(ddata->ulps_timeout));
+}
+
+static void dsicm_cancel_ulps_work(struct panel_drv_data *ddata)
+{
+	cancel_delayed_work(&ddata->ulps_work);
+}
+
+static int dsicm_enter_ulps(struct panel_drv_data *ddata)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (ddata->ulps_enabled)
+		return 0;
+
+	dsicm_cancel_ulps_work(ddata);
+
+	r = _dsicm_enable_te(ddata, false);
+	if (r)
+		goto err;
+
+	if (gpio_is_valid(ddata->ext_te_gpio))
+		disable_irq(gpio_to_irq(ddata->ext_te_gpio));
+
+	in->ops.dsi->disable(in, false, true);
+
+	ddata->ulps_enabled = true;
+
+	return 0;
+
+err:
+	dev_err(&ddata->pdev->dev, "enter ULPS failed");
+	dsicm_panel_reset(ddata);
+
+	ddata->ulps_enabled = false;
+
+	dsicm_queue_ulps_work(ddata);
+
+	return r;
+}
+
+static int dsicm_exit_ulps(struct panel_drv_data *ddata)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!ddata->ulps_enabled)
+		return 0;
+
+	r = in->ops.dsi->enable(in);
+	if (r) {
+		dev_err(&ddata->pdev->dev, "failed to enable DSI\n");
+		goto err1;
+	}
+
+	in->ops.dsi->enable_hs(in, ddata->channel, true);
+
+	r = _dsicm_enable_te(ddata, true);
+	if (r) {
+		dev_err(&ddata->pdev->dev, "failed to re-enable TE");
+		goto err2;
+	}
+
+	if (gpio_is_valid(ddata->ext_te_gpio))
+		enable_irq(gpio_to_irq(ddata->ext_te_gpio));
+
+	dsicm_queue_ulps_work(ddata);
+
+	ddata->ulps_enabled = false;
+
+	return 0;
+
+err2:
+	dev_err(&ddata->pdev->dev, "failed to exit ULPS");
+
+	r = dsicm_panel_reset(ddata);
+	if (!r) {
+		if (gpio_is_valid(ddata->ext_te_gpio))
+			enable_irq(gpio_to_irq(ddata->ext_te_gpio));
+		ddata->ulps_enabled = false;
+	}
+err1:
+	dsicm_queue_ulps_work(ddata);
+
+	return r;
+}
+
+static int dsicm_wake_up(struct panel_drv_data *ddata)
+{
+	if (ddata->ulps_enabled)
+		return dsicm_exit_ulps(ddata);
+
+	dsicm_cancel_ulps_work(ddata);
+	dsicm_queue_ulps_work(ddata);
+	return 0;
+}
+
+static int dsicm_bl_update_status(struct backlight_device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+	int level;
+
+	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+			dev->props.power == FB_BLANK_UNBLANK)
+		level = dev->props.brightness;
+	else
+		level = 0;
+
+	dev_dbg(&ddata->pdev->dev, "update brightness to %d\n", level);
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->enabled) {
+		in->ops.dsi->bus_lock(in);
+
+		r = dsicm_wake_up(ddata);
+		if (!r)
+			r = dsicm_dcs_write_1(ddata, DCS_BRIGHTNESS, level);
+
+		in->ops.dsi->bus_unlock(in);
+	} else {
+		r = 0;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	return r;
+}
+
+static int dsicm_bl_get_intensity(struct backlight_device *dev)
+{
+	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+			dev->props.power == FB_BLANK_UNBLANK)
+		return dev->props.brightness;
+
+	return 0;
+}
+
+static const struct backlight_ops dsicm_bl_ops = {
+	.get_brightness = dsicm_bl_get_intensity,
+	.update_status  = dsicm_bl_update_status,
+};
+
+static void dsicm_get_resolution(struct omap_dss_device *dssdev,
+		u16 *xres, u16 *yres)
+{
+	*xres = dssdev->panel.timings.x_res;
+	*yres = dssdev->panel.timings.y_res;
+}
+
+static ssize_t dsicm_num_errors_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *in = ddata->in;
+	u8 errors = 0;
+	int r;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->enabled) {
+		in->ops.dsi->bus_lock(in);
+
+		r = dsicm_wake_up(ddata);
+		if (!r)
+			r = dsicm_dcs_read_1(ddata, DCS_READ_NUM_ERRORS,
+					&errors);
+
+		in->ops.dsi->bus_unlock(in);
+	} else {
+		r = -ENODEV;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	if (r)
+		return r;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", errors);
+}
+
+static ssize_t dsicm_hw_revision_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *in = ddata->in;
+	u8 id1, id2, id3;
+	int r;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->enabled) {
+		in->ops.dsi->bus_lock(in);
+
+		r = dsicm_wake_up(ddata);
+		if (!r)
+			r = dsicm_get_id(ddata, &id1, &id2, &id3);
+
+		in->ops.dsi->bus_unlock(in);
+	} else {
+		r = -ENODEV;
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	if (r)
+		return r;
+
+	return snprintf(buf, PAGE_SIZE, "%02x.%02x.%02x\n", id1, id2, id3);
+}
+
+static ssize_t dsicm_store_ulps(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *in = ddata->in;
+	unsigned long t;
+	int r;
+
+	r = kstrtoul(buf, 0, &t);
+	if (r)
+		return r;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->enabled) {
+		in->ops.dsi->bus_lock(in);
+
+		if (t)
+			r = dsicm_enter_ulps(ddata);
+		else
+			r = dsicm_wake_up(ddata);
+
+		in->ops.dsi->bus_unlock(in);
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	if (r)
+		return r;
+
+	return count;
+}
+
+static ssize_t dsicm_show_ulps(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	unsigned t;
+
+	mutex_lock(&ddata->lock);
+	t = ddata->ulps_enabled;
+	mutex_unlock(&ddata->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static ssize_t dsicm_store_ulps_timeout(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *in = ddata->in;
+	unsigned long t;
+	int r;
+
+	r = kstrtoul(buf, 0, &t);
+	if (r)
+		return r;
+
+	mutex_lock(&ddata->lock);
+	ddata->ulps_timeout = t;
+
+	if (ddata->enabled) {
+		/* dsicm_wake_up will restart the timer */
+		in->ops.dsi->bus_lock(in);
+		r = dsicm_wake_up(ddata);
+		in->ops.dsi->bus_unlock(in);
+	}
+
+	mutex_unlock(&ddata->lock);
+
+	if (r)
+		return r;
+
+	return count;
+}
+
+static ssize_t dsicm_show_ulps_timeout(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	unsigned t;
+
+	mutex_lock(&ddata->lock);
+	t = ddata->ulps_timeout;
+	mutex_unlock(&ddata->lock);
+
+	return snprintf(buf, PAGE_SIZE, "%u\n", t);
+}
+
+static DEVICE_ATTR(num_dsi_errors, S_IRUGO, dsicm_num_errors_show, NULL);
+static DEVICE_ATTR(hw_revision, S_IRUGO, dsicm_hw_revision_show, NULL);
+static DEVICE_ATTR(ulps, S_IRUGO | S_IWUSR,
+		dsicm_show_ulps, dsicm_store_ulps);
+static DEVICE_ATTR(ulps_timeout, S_IRUGO | S_IWUSR,
+		dsicm_show_ulps_timeout, dsicm_store_ulps_timeout);
+
+static struct attribute *dsicm_attrs[] = {
+	&dev_attr_num_dsi_errors.attr,
+	&dev_attr_hw_revision.attr,
+	&dev_attr_ulps.attr,
+	&dev_attr_ulps_timeout.attr,
+	NULL,
+};
+
+static struct attribute_group dsicm_attr_group = {
+	.attrs = dsicm_attrs,
+};
+
+static void dsicm_hw_reset(struct panel_drv_data *ddata)
+{
+	if (!gpio_is_valid(ddata->reset_gpio))
+		return;
+
+	gpio_set_value(ddata->reset_gpio, 1);
+	udelay(10);
+	/* reset the panel */
+	gpio_set_value(ddata->reset_gpio, 0);
+	/* assert reset */
+	udelay(10);
+	gpio_set_value(ddata->reset_gpio, 1);
+	/* wait after releasing reset */
+	usleep_range(5000, 10000);
+}
+
+static int dsicm_power_on(struct panel_drv_data *ddata)
+{
+	struct omap_dss_device *in = ddata->in;
+	u8 id1, id2, id3;
+	int r;
+	struct omap_dss_dsi_config dsi_config = {
+		.mode = OMAP_DSS_DSI_CMD_MODE,
+		.pixel_format = OMAP_DSS_DSI_FMT_RGB888,
+		.timings = &ddata->timings,
+		.hs_clk_min = 150000000,
+		.hs_clk_max = 300000000,
+		.lp_clk_min = 7000000,
+		.lp_clk_max = 10000000,
+	};
+
+	if (ddata->pin_config.num_pins > 0) {
+		r = in->ops.dsi->configure_pins(in, &ddata->pin_config);
+		if (r) {
+			dev_err(&ddata->pdev->dev,
+				"failed to configure DSI pins\n");
+			goto err0;
+		}
+	}
+
+	r = in->ops.dsi->set_config(in, &dsi_config);
+	if (r) {
+		dev_err(&ddata->pdev->dev, "failed to configure DSI\n");
+		goto err0;
+	}
+
+	r = in->ops.dsi->enable(in);
+	if (r) {
+		dev_err(&ddata->pdev->dev, "failed to enable DSI\n");
+		goto err0;
+	}
+
+	dsicm_hw_reset(ddata);
+
+	in->ops.dsi->enable_hs(in, ddata->channel, false);
+
+	r = dsicm_sleep_out(ddata);
+	if (r)
+		goto err;
+
+	r = dsicm_get_id(ddata, &id1, &id2, &id3);
+	if (r)
+		goto err;
+
+	r = dsicm_dcs_write_1(ddata, DCS_BRIGHTNESS, 0xff);
+	if (r)
+		goto err;
+
+	r = dsicm_dcs_write_1(ddata, DCS_CTRL_DISPLAY,
+			(1<<2) | (1<<5));	/* BL | BCTRL */
+	if (r)
+		goto err;
+
+	r = dsicm_dcs_write_1(ddata, MIPI_DCS_SET_PIXEL_FORMAT,
+		MIPI_DCS_PIXEL_FMT_24BIT);
+	if (r)
+		goto err;
+
+	r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_DISPLAY_ON);
+	if (r)
+		goto err;
+
+	r = _dsicm_enable_te(ddata, ddata->te_enabled);
+	if (r)
+		goto err;
+
+	r = in->ops.dsi->enable_video_output(in, ddata->channel);
+	if (r)
+		goto err;
+
+	ddata->enabled = 1;
+
+	if (!ddata->intro_printed) {
+		dev_info(&ddata->pdev->dev, "panel revision %02x.%02x.%02x\n",
+			id1, id2, id3);
+		ddata->intro_printed = true;
+	}
+
+	in->ops.dsi->enable_hs(in, ddata->channel, true);
+
+	return 0;
+err:
+	dev_err(&ddata->pdev->dev, "error while enabling panel, issuing HW reset\n");
+
+	dsicm_hw_reset(ddata);
+
+	in->ops.dsi->disable(in, true, false);
+err0:
+	return r;
+}
+
+static void dsicm_power_off(struct panel_drv_data *ddata)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	in->ops.dsi->disable_video_output(in, ddata->channel);
+
+	r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_DISPLAY_OFF);
+	if (!r)
+		r = dsicm_sleep_in(ddata);
+
+	if (r) {
+		dev_err(&ddata->pdev->dev,
+				"error disabling panel, issuing HW reset\n");
+		dsicm_hw_reset(ddata);
+	}
+
+	in->ops.dsi->disable(in, true, false);
+
+	ddata->enabled = 0;
+}
+
+static int dsicm_panel_reset(struct panel_drv_data *ddata)
+{
+	dev_err(&ddata->pdev->dev, "performing LCD reset\n");
+
+	dsicm_power_off(ddata);
+	dsicm_hw_reset(ddata);
+	return dsicm_power_on(ddata);
+}
+
+static int dsicm_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	struct device *dev = &ddata->pdev->dev;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dsi->connect(in, dssdev);
+	if (r) {
+		dev_err(dev, "Failed to connect to video source\n");
+		return r;
+	}
+
+	r = in->ops.dsi->request_vc(ddata->in, &ddata->channel);
+	if (r) {
+		dev_err(dev, "failed to get virtual channel\n");
+		goto err_req_vc;
+	}
+
+	r = in->ops.dsi->set_vc_id(ddata->in, ddata->channel, TCH);
+	if (r) {
+		dev_err(dev, "failed to set VC_ID\n");
+		goto err_vc_id;
+	}
+
+	return 0;
+
+err_vc_id:
+	in->ops.dsi->release_vc(ddata->in, ddata->channel);
+err_req_vc:
+	in->ops.dsi->disconnect(in, dssdev);
+	return r;
+}
+
+static void dsicm_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dsi->release_vc(in, ddata->channel);
+	in->ops.dsi->disconnect(in, dssdev);
+}
+
+static int dsicm_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(&ddata->pdev->dev, "enable\n");
+
+	mutex_lock(&ddata->lock);
+
+	if (!omapdss_device_is_connected(dssdev)) {
+		r = -ENODEV;
+		goto err;
+	}
+
+	if (omapdss_device_is_enabled(dssdev)) {
+		r = 0;
+		goto err;
+	}
+
+	in->ops.dsi->bus_lock(in);
+
+	r = dsicm_power_on(ddata);
+
+	in->ops.dsi->bus_unlock(in);
+
+	if (r)
+		goto err;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	mutex_unlock(&ddata->lock);
+
+	return 0;
+err:
+	dev_dbg(&ddata->pdev->dev, "enable failed\n");
+	mutex_unlock(&ddata->lock);
+	return r;
+}
+
+static void dsicm_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(&ddata->pdev->dev, "disable\n");
+
+	mutex_lock(&ddata->lock);
+
+	dsicm_cancel_ulps_work(ddata);
+
+	in->ops.dsi->bus_lock(in);
+
+	if (omapdss_device_is_enabled(dssdev)) {
+		r = dsicm_wake_up(ddata);
+		if (!r)
+			dsicm_power_off(ddata);
+	}
+
+	in->ops.dsi->bus_unlock(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+	mutex_unlock(&ddata->lock);
+}
+
+static void dsicm_framedone_cb(int err, void *data)
+{
+	struct panel_drv_data *ddata = data;
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->pdev->dev, "framedone, err %d\n", err);
+	in->ops.dsi->bus_unlock(ddata->in);
+}
+
+static irqreturn_t dsicm_te_isr(int irq, void *data)
+{
+	struct panel_drv_data *ddata = data;
+	struct omap_dss_device *in = ddata->in;
+	int old;
+	int r;
+
+	old = atomic_cmpxchg(&ddata->do_update, 1, 0);
+
+	if (old) {
+		cancel_delayed_work(&ddata->te_timeout_work);
+
+		r = in->ops.dsi->update(in, ddata->channel, dsicm_framedone_cb,
+				ddata);
+		if (r)
+			goto err;
+	}
+
+	return IRQ_HANDLED;
+err:
+	dev_err(&ddata->pdev->dev, "start update failed\n");
+	in->ops.dsi->bus_unlock(in);
+	return IRQ_HANDLED;
+}
+
+static void dsicm_te_timeout_work_callback(struct work_struct *work)
+{
+	struct panel_drv_data *ddata = container_of(work, struct panel_drv_data,
+					te_timeout_work.work);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_err(&ddata->pdev->dev, "TE not received for 250ms!\n");
+
+	atomic_set(&ddata->do_update, 0);
+	in->ops.dsi->bus_unlock(in);
+}
+
+static int dsicm_update(struct omap_dss_device *dssdev,
+				    u16 x, u16 y, u16 w, u16 h)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(&ddata->pdev->dev, "update %d, %d, %d x %d\n", x, y, w, h);
+
+	mutex_lock(&ddata->lock);
+	in->ops.dsi->bus_lock(in);
+
+	r = dsicm_wake_up(ddata);
+	if (r)
+		goto err;
+
+	if (!ddata->enabled) {
+		r = 0;
+		goto err;
+	}
+
+	/* XXX no need to send this every frame, but dsi break if not done */
+	r = dsicm_set_update_window(ddata, 0, 0,
+			dssdev->panel.timings.x_res,
+			dssdev->panel.timings.y_res);
+	if (r)
+		goto err;
+
+	if (ddata->te_enabled && gpio_is_valid(ddata->ext_te_gpio)) {
+		schedule_delayed_work(&ddata->te_timeout_work,
+				msecs_to_jiffies(250));
+		atomic_set(&ddata->do_update, 1);
+	} else {
+		r = in->ops.dsi->update(in, ddata->channel, dsicm_framedone_cb,
+				ddata);
+		if (r)
+			goto err;
+	}
+
+	/* note: no bus_unlock here. unlock is in framedone_cb */
+	mutex_unlock(&ddata->lock);
+	return 0;
+err:
+	in->ops.dsi->bus_unlock(in);
+	mutex_unlock(&ddata->lock);
+	return r;
+}
+
+static int dsicm_sync(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->pdev->dev, "sync\n");
+
+	mutex_lock(&ddata->lock);
+	in->ops.dsi->bus_lock(in);
+	in->ops.dsi->bus_unlock(in);
+	mutex_unlock(&ddata->lock);
+
+	dev_dbg(&ddata->pdev->dev, "sync done\n");
+
+	return 0;
+}
+
+static int _dsicm_enable_te(struct panel_drv_data *ddata, bool enable)
+{
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (enable)
+		r = dsicm_dcs_write_1(ddata, MIPI_DCS_SET_TEAR_ON, 0);
+	else
+		r = dsicm_dcs_write_0(ddata, MIPI_DCS_SET_TEAR_OFF);
+
+	if (!gpio_is_valid(ddata->ext_te_gpio))
+		in->ops.dsi->enable_te(in, enable);
+
+	/* possible panel bug */
+	msleep(100);
+
+	return r;
+}
+
+static int dsicm_enable_te(struct omap_dss_device *dssdev, bool enable)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	mutex_lock(&ddata->lock);
+
+	if (ddata->te_enabled == enable)
+		goto end;
+
+	in->ops.dsi->bus_lock(in);
+
+	if (ddata->enabled) {
+		r = dsicm_wake_up(ddata);
+		if (r)
+			goto err;
+
+		r = _dsicm_enable_te(ddata, enable);
+		if (r)
+			goto err;
+	}
+
+	ddata->te_enabled = enable;
+
+	in->ops.dsi->bus_unlock(in);
+end:
+	mutex_unlock(&ddata->lock);
+
+	return 0;
+err:
+	in->ops.dsi->bus_unlock(in);
+	mutex_unlock(&ddata->lock);
+
+	return r;
+}
+
+static int dsicm_get_te(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	int r;
+
+	mutex_lock(&ddata->lock);
+	r = ddata->te_enabled;
+	mutex_unlock(&ddata->lock);
+
+	return r;
+}
+
+static int dsicm_memory_read(struct omap_dss_device *dssdev,
+		void *buf, size_t size,
+		u16 x, u16 y, u16 w, u16 h)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+	int first = 1;
+	int plen;
+	unsigned buf_used = 0;
+
+	if (size < w * h * 3)
+		return -ENOMEM;
+
+	mutex_lock(&ddata->lock);
+
+	if (!ddata->enabled) {
+		r = -ENODEV;
+		goto err1;
+	}
+
+	size = min(w * h * 3,
+			dssdev->panel.timings.x_res *
+			dssdev->panel.timings.y_res * 3);
+
+	in->ops.dsi->bus_lock(in);
+
+	r = dsicm_wake_up(ddata);
+	if (r)
+		goto err2;
+
+	/* plen 1 or 2 goes into short packet. until checksum error is fixed,
+	 * use short packets. plen 32 works, but bigger packets seem to cause
+	 * an error. */
+	if (size % 2)
+		plen = 1;
+	else
+		plen = 2;
+
+	dsicm_set_update_window(ddata, x, y, w, h);
+
+	r = in->ops.dsi->set_max_rx_packet_size(in, ddata->channel, plen);
+	if (r)
+		goto err2;
+
+	while (buf_used < size) {
+		u8 dcs_cmd = first ? 0x2e : 0x3e;
+		first = 0;
+
+		r = in->ops.dsi->dcs_read(in, ddata->channel, dcs_cmd,
+				buf + buf_used, size - buf_used);
+
+		if (r < 0) {
+			dev_err(dssdev->dev, "read error\n");
+			goto err3;
+		}
+
+		buf_used += r;
+
+		if (r < plen) {
+			dev_err(&ddata->pdev->dev, "short read\n");
+			break;
+		}
+
+		if (signal_pending(current)) {
+			dev_err(&ddata->pdev->dev, "signal pending, "
+					"aborting memory read\n");
+			r = -ERESTARTSYS;
+			goto err3;
+		}
+	}
+
+	r = buf_used;
+
+err3:
+	in->ops.dsi->set_max_rx_packet_size(in, ddata->channel, 1);
+err2:
+	in->ops.dsi->bus_unlock(in);
+err1:
+	mutex_unlock(&ddata->lock);
+	return r;
+}
+
+static void dsicm_ulps_work(struct work_struct *work)
+{
+	struct panel_drv_data *ddata = container_of(work, struct panel_drv_data,
+			ulps_work.work);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	mutex_lock(&ddata->lock);
+
+	if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE || !ddata->enabled) {
+		mutex_unlock(&ddata->lock);
+		return;
+	}
+
+	in->ops.dsi->bus_lock(in);
+
+	dsicm_enter_ulps(ddata);
+
+	in->ops.dsi->bus_unlock(in);
+	mutex_unlock(&ddata->lock);
+}
+
+static struct omap_dss_driver dsicm_ops = {
+	.connect	= dsicm_connect,
+	.disconnect	= dsicm_disconnect,
+
+	.enable		= dsicm_enable,
+	.disable	= dsicm_disable,
+
+	.update		= dsicm_update,
+	.sync		= dsicm_sync,
+
+	.get_resolution	= dsicm_get_resolution,
+	.get_recommended_bpp = omapdss_default_get_recommended_bpp,
+
+	.enable_te	= dsicm_enable_te,
+	.get_te		= dsicm_get_te,
+
+	.memory_read	= dsicm_memory_read,
+};
+
+static int dsicm_probe_pdata(struct platform_device *pdev)
+{
+	const struct panel_dsicm_platform_data *pdata;
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return -EPROBE_DEFER;
+	}
+	ddata->in = in;
+
+	ddata->reset_gpio = pdata->reset_gpio;
+
+	if (pdata->use_ext_te)
+		ddata->ext_te_gpio = pdata->ext_te_gpio;
+	else
+		ddata->ext_te_gpio = -1;
+
+	ddata->ulps_timeout = pdata->ulps_timeout;
+
+	ddata->use_dsi_backlight = pdata->use_dsi_backlight;
+
+	ddata->pin_config = pdata->pin_config;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int dsicm_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "reset-gpios", 0);
+	if (!gpio_is_valid(gpio)) {
+		dev_err(&pdev->dev, "failed to parse reset gpio\n");
+		return gpio;
+	}
+	ddata->reset_gpio = gpio;
+
+	gpio = of_get_named_gpio(node, "te-gpios", 0);
+	if (gpio_is_valid(gpio) || gpio == -ENOENT) {
+		ddata->ext_te_gpio = gpio;
+	} else {
+		dev_err(&pdev->dev, "failed to parse TE gpio\n");
+		return gpio;
+	}
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	/* TODO: ulps, backlight */
+
+	return 0;
+}
+
+static int dsicm_probe(struct platform_device *pdev)
+{
+	struct backlight_properties props;
+	struct panel_drv_data *ddata;
+	struct backlight_device *bldev = NULL;
+	struct device *dev = &pdev->dev;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	dev_dbg(dev, "probe\n");
+
+	ddata = devm_kzalloc(dev, sizeof(*ddata), GFP_KERNEL);
+	if (!ddata)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+	ddata->pdev = pdev;
+
+	if (dev_get_platdata(dev)) {
+		r = dsicm_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = dsicm_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->timings.x_res = 864;
+	ddata->timings.y_res = 480;
+	ddata->timings.pixelclock = 864 * 480 * 60;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = dev;
+	dssdev->driver = &dsicm_ops;
+	dssdev->panel.timings = ddata->timings;
+	dssdev->type = OMAP_DISPLAY_TYPE_DSI;
+	dssdev->owner = THIS_MODULE;
+
+	dssdev->panel.dsi_pix_fmt = OMAP_DSS_DSI_FMT_RGB888;
+	dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
+		OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	mutex_init(&ddata->lock);
+
+	atomic_set(&ddata->do_update, 0);
+
+	if (gpio_is_valid(ddata->reset_gpio)) {
+		r = devm_gpio_request_one(dev, ddata->reset_gpio,
+				GPIOF_OUT_INIT_LOW, "taal rst");
+		if (r) {
+			dev_err(dev, "failed to request reset gpio\n");
+			return r;
+		}
+	}
+
+	if (gpio_is_valid(ddata->ext_te_gpio)) {
+		r = devm_gpio_request_one(dev, ddata->ext_te_gpio,
+				GPIOF_IN, "taal irq");
+		if (r) {
+			dev_err(dev, "GPIO request failed\n");
+			return r;
+		}
+
+		r = devm_request_irq(dev, gpio_to_irq(ddata->ext_te_gpio),
+				dsicm_te_isr,
+				IRQF_TRIGGER_RISING,
+				"taal vsync", ddata);
+
+		if (r) {
+			dev_err(dev, "IRQ request failed\n");
+			return r;
+		}
+
+		INIT_DEFERRABLE_WORK(&ddata->te_timeout_work,
+					dsicm_te_timeout_work_callback);
+
+		dev_dbg(dev, "Using GPIO TE\n");
+	}
+
+	ddata->workqueue = create_singlethread_workqueue("dsicm_wq");
+	if (ddata->workqueue == NULL) {
+		dev_err(dev, "can't create workqueue\n");
+		return -ENOMEM;
+	}
+	INIT_DELAYED_WORK(&ddata->ulps_work, dsicm_ulps_work);
+
+	dsicm_hw_reset(ddata);
+
+	if (ddata->use_dsi_backlight) {
+		memset(&props, 0, sizeof(struct backlight_properties));
+		props.max_brightness = 255;
+
+		props.type = BACKLIGHT_RAW;
+		bldev = backlight_device_register(dev_name(dev),
+				dev, ddata, &dsicm_bl_ops, &props);
+		if (IS_ERR(bldev)) {
+			r = PTR_ERR(bldev);
+			goto err_bl;
+		}
+
+		ddata->bldev = bldev;
+
+		bldev->props.fb_blank = FB_BLANK_UNBLANK;
+		bldev->props.power = FB_BLANK_UNBLANK;
+		bldev->props.brightness = 255;
+
+		dsicm_bl_update_status(bldev);
+	}
+
+	r = sysfs_create_group(&dev->kobj, &dsicm_attr_group);
+	if (r) {
+		dev_err(dev, "failed to create sysfs files\n");
+		goto err_sysfs_create;
+	}
+
+	return 0;
+
+err_sysfs_create:
+	if (bldev != NULL)
+		backlight_device_unregister(bldev);
+err_bl:
+	destroy_workqueue(ddata->workqueue);
+err_reg:
+	return r;
+}
+
+static int __exit dsicm_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct backlight_device *bldev;
+
+	dev_dbg(&pdev->dev, "remove\n");
+
+	omapdss_unregister_display(dssdev);
+
+	dsicm_disable(dssdev);
+	dsicm_disconnect(dssdev);
+
+	sysfs_remove_group(&pdev->dev.kobj, &dsicm_attr_group);
+
+	bldev = ddata->bldev;
+	if (bldev != NULL) {
+		bldev->props.power = FB_BLANK_POWERDOWN;
+		dsicm_bl_update_status(bldev);
+		backlight_device_unregister(bldev);
+	}
+
+	omap_dss_put_device(ddata->in);
+
+	dsicm_cancel_ulps_work(ddata);
+	destroy_workqueue(ddata->workqueue);
+
+	/* reset, to be sure that the panel is in a valid state */
+	dsicm_hw_reset(ddata);
+
+	return 0;
+}
+
+static const struct of_device_id dsicm_of_match[] = {
+	{ .compatible = "omapdss,panel-dsi-cm", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, dsicm_of_match);
+
+static struct platform_driver dsicm_driver = {
+	.probe = dsicm_probe,
+	.remove = __exit_p(dsicm_remove),
+	.driver = {
+		.name = "panel-dsi-cm",
+		.of_match_table = dsicm_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(dsicm_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Generic DSI Command Mode Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..18eb60e9c9ec
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,404 @@
+/*
+ * LG.Philips LB035Q02 LCD Panel driver
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ * Based on a driver by: Steve Sakoman <steve@sakoman.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/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/mutex.h>
+#include <linux/gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+static struct omap_video_timings lb035q02_timings = {
+	.x_res = 320,
+	.y_res = 240,
+
+	.pixelclock	= 6500000,
+
+	.hsw		= 2,
+	.hfp		= 20,
+	.hbp		= 68,
+
+	.vsw		= 2,
+	.vfp		= 4,
+	.vbp		= 18,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	struct spi_device *spi;
+
+	int data_lines;
+
+	struct omap_video_timings videomode;
+
+	/* used for non-DT boot, to be removed */
+	int backlight_gpio;
+
+	struct gpio_desc *enable_gpio;
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int lb035q02_write_reg(struct spi_device *spi, u8 reg, u16 val)
+{
+	struct spi_message msg;
+	struct spi_transfer index_xfer = {
+		.len		= 3,
+		.cs_change	= 1,
+	};
+	struct spi_transfer value_xfer = {
+		.len		= 3,
+	};
+	u8	buffer[16];
+
+	spi_message_init(&msg);
+
+	/* register index */
+	buffer[0] = 0x70;
+	buffer[1] = 0x00;
+	buffer[2] = reg & 0x7f;
+	index_xfer.tx_buf = buffer;
+	spi_message_add_tail(&index_xfer, &msg);
+
+	/* register value */
+	buffer[4] = 0x72;
+	buffer[5] = val >> 8;
+	buffer[6] = val;
+	value_xfer.tx_buf = buffer + 4;
+	spi_message_add_tail(&value_xfer, &msg);
+
+	return spi_sync(spi, &msg);
+}
+
+static void init_lb035q02_panel(struct spi_device *spi)
+{
+	/* Init sequence from page 28 of the lb035q02 spec */
+	lb035q02_write_reg(spi, 0x01, 0x6300);
+	lb035q02_write_reg(spi, 0x02, 0x0200);
+	lb035q02_write_reg(spi, 0x03, 0x0177);
+	lb035q02_write_reg(spi, 0x04, 0x04c7);
+	lb035q02_write_reg(spi, 0x05, 0xffc0);
+	lb035q02_write_reg(spi, 0x06, 0xe806);
+	lb035q02_write_reg(spi, 0x0a, 0x4008);
+	lb035q02_write_reg(spi, 0x0b, 0x0000);
+	lb035q02_write_reg(spi, 0x0d, 0x0030);
+	lb035q02_write_reg(spi, 0x0e, 0x2800);
+	lb035q02_write_reg(spi, 0x0f, 0x0000);
+	lb035q02_write_reg(spi, 0x16, 0x9f80);
+	lb035q02_write_reg(spi, 0x17, 0x0a0f);
+	lb035q02_write_reg(spi, 0x1e, 0x00c1);
+	lb035q02_write_reg(spi, 0x30, 0x0300);
+	lb035q02_write_reg(spi, 0x31, 0x0007);
+	lb035q02_write_reg(spi, 0x32, 0x0000);
+	lb035q02_write_reg(spi, 0x33, 0x0000);
+	lb035q02_write_reg(spi, 0x34, 0x0707);
+	lb035q02_write_reg(spi, 0x35, 0x0004);
+	lb035q02_write_reg(spi, 0x36, 0x0302);
+	lb035q02_write_reg(spi, 0x37, 0x0202);
+	lb035q02_write_reg(spi, 0x3a, 0x0a0d);
+	lb035q02_write_reg(spi, 0x3b, 0x0806);
+}
+
+static int lb035q02_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	init_lb035q02_panel(ddata->spi);
+
+	return 0;
+}
+
+static void lb035q02_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int lb035q02_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 1);
+
+	if (gpio_is_valid(ddata->backlight_gpio))
+		gpio_set_value_cansleep(ddata->backlight_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void lb035q02_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (ddata->enable_gpio)
+		gpiod_set_value_cansleep(ddata->enable_gpio, 0);
+
+	if (gpio_is_valid(ddata->backlight_gpio))
+		gpio_set_value_cansleep(ddata->backlight_gpio, 0);
+
+	in->ops.dpi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void lb035q02_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void lb035q02_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int lb035q02_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver lb035q02_ops = {
+	.connect	= lb035q02_connect,
+	.disconnect	= lb035q02_disconnect,
+
+	.enable		= lb035q02_enable,
+	.disable	= lb035q02_disable,
+
+	.set_timings	= lb035q02_set_timings,
+	.get_timings	= lb035q02_get_timings,
+	.check_timings	= lb035q02_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+static int lb035q02_probe_pdata(struct spi_device *spi)
+{
+	const struct panel_lb035q02_platform_data *pdata;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev, *in;
+	int r;
+
+	pdata = dev_get_platdata(&spi->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&spi->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	r = devm_gpio_request_one(&spi->dev, pdata->enable_gpio,
+					GPIOF_OUT_INIT_LOW, "panel enable");
+	if (r)
+		goto err_gpio;
+
+	ddata->enable_gpio = gpio_to_desc(pdata->enable_gpio);
+
+	ddata->backlight_gpio = pdata->backlight_gpio;
+
+	return 0;
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int lb035q02_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	struct gpio_desc *gpio;
+
+	gpio = devm_gpiod_get(&spi->dev, "enable", GPIOD_OUT_LOW);
+	if (IS_ERR(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return PTR_ERR(gpio);
+	}
+
+	ddata->enable_gpio = gpio;
+
+	ddata->backlight_gpio = -ENOENT;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, ddata);
+
+	ddata->spi = spi;
+
+	if (dev_get_platdata(&spi->dev)) {
+		r = lb035q02_probe_pdata(spi);
+		if (r)
+			return r;
+	} else if (spi->dev.of_node) {
+		r = lb035q02_probe_of(spi);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->backlight_gpio)) {
+		r = devm_gpio_request_one(&spi->dev, ddata->backlight_gpio,
+				GPIOF_OUT_INIT_LOW, "panel backlight");
+		if (r)
+			goto err_gpio;
+	}
+
+	ddata->videomode = lb035q02_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &spi->dev;
+	dssdev->driver = &lb035q02_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+	dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&spi->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(dssdev);
+
+	lb035q02_disable(dssdev);
+	lb035q02_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id lb035q02_of_match[] = {
+	{ .compatible = "omapdss,lgphilips,lb035q02", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, lb035q02_of_match);
+
+static struct spi_driver lb035q02_spi_driver = {
+	.probe		= lb035q02_panel_spi_probe,
+	.remove		= lb035q02_panel_spi_remove,
+	.driver		= {
+		.name	= "panel_lgphilips_lb035q02",
+		.of_match_table = lb035q02_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_spi_driver(lb035q02_spi_driver);
+
+MODULE_ALIAS("spi:lgphilips,lb035q02");
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("LG.Philips LB035Q02 LCD Panel driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c
new file mode 100644
index 000000000000..8a928c9a2fc9
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-nec-nl8048hl11.c
@@ -0,0 +1,437 @@
+/*
+ * NEC NL8048HL11 Panel driver
+ *
+ * Copyright (C) 2010 Texas Instruments Inc.
+ * Author: Erik Gilling <konkers@android.com>
+ * Converted to new DSS device model: Tomi Valkeinen <tomi.valkeinen@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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device	dssdev;
+	struct omap_dss_device *in;
+
+	struct omap_video_timings videomode;
+
+	int data_lines;
+
+	int res_gpio;
+	int qvga_gpio;
+
+	struct spi_device *spi;
+};
+
+#define LCD_XRES		800
+#define LCD_YRES		480
+/*
+ * NEC PIX Clock Ratings
+ * MIN:21.8MHz TYP:23.8MHz MAX:25.7MHz
+ */
+#define LCD_PIXEL_CLOCK		23800000
+
+static const struct {
+	unsigned char addr;
+	unsigned char dat;
+} nec_8048_init_seq[] = {
+	{ 3, 0x01 }, { 0, 0x00 }, { 1, 0x01 }, { 4, 0x00 }, { 5, 0x14 },
+	{ 6, 0x24 }, { 16, 0xD7 }, { 17, 0x00 }, { 18, 0x00 }, { 19, 0x55 },
+	{ 20, 0x01 }, { 21, 0x70 }, { 22, 0x1E }, { 23, 0x25 },	{ 24, 0x25 },
+	{ 25, 0x02 }, { 26, 0x02 }, { 27, 0xA0 }, { 32, 0x2F }, { 33, 0x0F },
+	{ 34, 0x0F }, { 35, 0x0F }, { 36, 0x0F }, { 37, 0x0F },	{ 38, 0x0F },
+	{ 39, 0x00 }, { 40, 0x02 }, { 41, 0x02 }, { 42, 0x02 },	{ 43, 0x0F },
+	{ 44, 0x0F }, { 45, 0x0F }, { 46, 0x0F }, { 47, 0x0F },	{ 48, 0x0F },
+	{ 49, 0x0F }, { 50, 0x00 }, { 51, 0x02 }, { 52, 0x02 }, { 53, 0x02 },
+	{ 80, 0x0C }, { 83, 0x42 }, { 84, 0x42 }, { 85, 0x41 },	{ 86, 0x14 },
+	{ 89, 0x88 }, { 90, 0x01 }, { 91, 0x00 }, { 92, 0x02 },	{ 93, 0x0C },
+	{ 94, 0x1C }, { 95, 0x27 }, { 98, 0x49 }, { 99, 0x27 }, { 102, 0x76 },
+	{ 103, 0x27 }, { 112, 0x01 }, { 113, 0x0E }, { 114, 0x02 },
+	{ 115, 0x0C }, { 118, 0x0C }, { 121, 0x30 }, { 130, 0x00 },
+	{ 131, 0x00 }, { 132, 0xFC }, { 134, 0x00 }, { 136, 0x00 },
+	{ 138, 0x00 }, { 139, 0x00 }, { 140, 0x00 }, { 141, 0xFC },
+	{ 143, 0x00 }, { 145, 0x00 }, { 147, 0x00 }, { 148, 0x00 },
+	{ 149, 0x00 }, { 150, 0xFC }, { 152, 0x00 }, { 154, 0x00 },
+	{ 156, 0x00 }, { 157, 0x00 }, { 2, 0x00 },
+};
+
+static const struct omap_video_timings nec_8048_panel_timings = {
+	.x_res		= LCD_XRES,
+	.y_res		= LCD_YRES,
+	.pixelclock	= LCD_PIXEL_CLOCK,
+	.hfp		= 6,
+	.hsw		= 1,
+	.hbp		= 4,
+	.vfp		= 3,
+	.vsw		= 1,
+	.vbp		= 4,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int nec_8048_spi_send(struct spi_device *spi, unsigned char reg_addr,
+			unsigned char reg_data)
+{
+	int ret = 0;
+	unsigned int cmd = 0, data = 0;
+
+	cmd = 0x0000 | reg_addr; /* register address write */
+	data = 0x0100 | reg_data; /* register data write */
+	data = (cmd << 16) | data;
+
+	ret = spi_write(spi, (unsigned char *)&data, 4);
+	if (ret)
+		pr_err("error in spi_write %x\n", data);
+
+	return ret;
+}
+
+static int init_nec_8048_wvga_lcd(struct spi_device *spi)
+{
+	unsigned int i;
+	/* Initialization Sequence */
+	/* nec_8048_spi_send(spi, REG, VAL) */
+	for (i = 0; i < (ARRAY_SIZE(nec_8048_init_seq) - 1); i++)
+		nec_8048_spi_send(spi, nec_8048_init_seq[i].addr,
+				nec_8048_init_seq[i].dat);
+	udelay(20);
+	nec_8048_spi_send(spi, nec_8048_init_seq[i].addr,
+				nec_8048_init_seq[i].dat);
+	return 0;
+}
+
+static int nec_8048_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void nec_8048_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int nec_8048_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	if (gpio_is_valid(ddata->res_gpio))
+		gpio_set_value_cansleep(ddata->res_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void nec_8048_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (gpio_is_valid(ddata->res_gpio))
+		gpio_set_value_cansleep(ddata->res_gpio, 0);
+
+	in->ops.dpi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void nec_8048_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void nec_8048_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int nec_8048_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver nec_8048_ops = {
+	.connect	= nec_8048_connect,
+	.disconnect	= nec_8048_disconnect,
+
+	.enable		= nec_8048_enable,
+	.disable	= nec_8048_disable,
+
+	.set_timings	= nec_8048_set_timings,
+	.get_timings	= nec_8048_get_timings,
+	.check_timings	= nec_8048_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+
+static int nec_8048_probe_pdata(struct spi_device *spi)
+{
+	const struct panel_nec_nl8048hl11_platform_data *pdata;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&spi->dev);
+
+	ddata->qvga_gpio = pdata->qvga_gpio;
+	ddata->res_gpio = pdata->res_gpio;
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&spi->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int nec_8048_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "reset-gpios", 0);
+	if (!gpio_is_valid(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return gpio;
+	}
+	ddata->res_gpio = gpio;
+
+	/* XXX the panel spec doesn't mention any QVGA pin?? */
+	ddata->qvga_gpio = -ENOENT;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int nec_8048_probe(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	spi->mode = SPI_MODE_0;
+	spi->bits_per_word = 32;
+
+	r = spi_setup(spi);
+	if (r < 0) {
+		dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+		return r;
+	}
+
+	init_nec_8048_wvga_lcd(spi);
+
+	ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, ddata);
+
+	ddata->spi = spi;
+
+	if (dev_get_platdata(&spi->dev)) {
+		r = nec_8048_probe_pdata(spi);
+		if (r)
+			return r;
+	} else if (spi->dev.of_node) {
+		r = nec_8048_probe_of(spi);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->qvga_gpio)) {
+		r = devm_gpio_request_one(&spi->dev, ddata->qvga_gpio,
+				GPIOF_OUT_INIT_HIGH, "lcd QVGA");
+		if (r)
+			goto err_gpio;
+	}
+
+	if (gpio_is_valid(ddata->res_gpio)) {
+		r = devm_gpio_request_one(&spi->dev, ddata->res_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd RES");
+		if (r)
+			goto err_gpio;
+	}
+
+	ddata->videomode = nec_8048_panel_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &spi->dev;
+	dssdev->driver = &nec_8048_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&spi->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int nec_8048_remove(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+	omapdss_unregister_display(dssdev);
+
+	nec_8048_disable(dssdev);
+	nec_8048_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int nec_8048_suspend(struct device *dev)
+{
+	struct spi_device *spi = to_spi_device(dev);
+
+	nec_8048_spi_send(spi, 2, 0x01);
+	mdelay(40);
+
+	return 0;
+}
+
+static int nec_8048_resume(struct device *dev)
+{
+	struct spi_device *spi = to_spi_device(dev);
+
+	/* reinitialize the panel */
+	spi_setup(spi);
+	nec_8048_spi_send(spi, 2, 0x00);
+	init_nec_8048_wvga_lcd(spi);
+
+	return 0;
+}
+static SIMPLE_DEV_PM_OPS(nec_8048_pm_ops, nec_8048_suspend,
+		nec_8048_resume);
+#define NEC_8048_PM_OPS (&nec_8048_pm_ops)
+#else
+#define NEC_8048_PM_OPS NULL
+#endif
+
+static const struct of_device_id nec_8048_of_match[] = {
+	{ .compatible = "omapdss,nec,nl8048hl11", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, nec_8048_of_match);
+
+static struct spi_driver nec_8048_driver = {
+	.driver = {
+		.name	= "panel-nec-nl8048hl11",
+		.pm	= NEC_8048_PM_OPS,
+		.of_match_table = nec_8048_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe	= nec_8048_probe,
+	.remove	= nec_8048_remove,
+};
+
+module_spi_driver(nec_8048_driver);
+
+MODULE_ALIAS("spi:nec,nl8048hl11");
+MODULE_AUTHOR("Erik Gilling <konkers@android.com>");
+MODULE_DESCRIPTION("NEC-NL8048HL11 Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c
new file mode 100644
index 000000000000..abfd1f6e3327
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-sharp-ls037v7dw01.c
@@ -0,0 +1,415 @@
+/*
+ * LCD panel driver for Sharp LS037V7DW01
+ *
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/regulator/consumer.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+	struct regulator *vcc;
+
+	int data_lines;
+
+	struct omap_video_timings videomode;
+
+	struct gpio_desc *resb_gpio;	/* low = reset active min 20 us */
+	struct gpio_desc *ini_gpio;	/* high = power on */
+	struct gpio_desc *mo_gpio;	/* low = 480x640, high = 240x320 */
+	struct gpio_desc *lr_gpio;	/* high = conventional horizontal scanning */
+	struct gpio_desc *ud_gpio;	/* high = conventional vertical scanning */
+};
+
+static const struct omap_video_timings sharp_ls_timings = {
+	.x_res = 480,
+	.y_res = 640,
+
+	.pixelclock	= 19200000,
+
+	.hsw		= 2,
+	.hfp		= 1,
+	.hbp		= 28,
+
+	.vsw		= 1,
+	.vfp		= 1,
+	.vbp		= 1,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int sharp_ls_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void sharp_ls_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int sharp_ls_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	if (ddata->vcc) {
+		r = regulator_enable(ddata->vcc);
+		if (r != 0)
+			return r;
+	}
+
+	r = in->ops.dpi->enable(in);
+	if (r) {
+		regulator_disable(ddata->vcc);
+		return r;
+	}
+
+	/* wait couple of vsyncs until enabling the LCD */
+	msleep(50);
+
+	if (ddata->resb_gpio)
+		gpiod_set_value_cansleep(ddata->resb_gpio, 1);
+
+	if (ddata->ini_gpio)
+		gpiod_set_value_cansleep(ddata->ini_gpio, 1);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void sharp_ls_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	if (ddata->ini_gpio)
+		gpiod_set_value_cansleep(ddata->ini_gpio, 0);
+
+	if (ddata->resb_gpio)
+		gpiod_set_value_cansleep(ddata->resb_gpio, 0);
+
+	/* wait at least 5 vsyncs after disabling the LCD */
+
+	msleep(100);
+
+	in->ops.dpi->disable(in);
+
+	if (ddata->vcc)
+		regulator_disable(ddata->vcc);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void sharp_ls_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void sharp_ls_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int sharp_ls_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver sharp_ls_ops = {
+	.connect	= sharp_ls_connect,
+	.disconnect	= sharp_ls_disconnect,
+
+	.enable		= sharp_ls_enable,
+	.disable	= sharp_ls_disable,
+
+	.set_timings	= sharp_ls_set_timings,
+	.get_timings	= sharp_ls_get_timings,
+	.check_timings	= sharp_ls_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+static int sharp_ls_get_gpio(struct device *dev, int gpio, unsigned long flags,
+		  char *desc, struct gpio_desc **gpiod)
+{
+	struct gpio_desc *gd;
+	int r;
+
+	*gpiod = NULL;
+
+	r = devm_gpio_request_one(dev, gpio, flags, desc);
+	if (r)
+		return r == -ENOENT ? 0 : r;
+
+	gd = gpio_to_desc(gpio);
+	if (IS_ERR(gd))
+		return PTR_ERR(gd) == -ENOENT ? 0 : PTR_ERR(gd);
+
+	*gpiod = gd;
+	return 0;
+}
+
+static int sharp_ls_probe_pdata(struct platform_device *pdev)
+{
+	const struct panel_sharp_ls037v7dw01_platform_data *pdata;
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev, *in;
+	int r;
+
+	pdata = dev_get_platdata(&pdev->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&pdev->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->mo_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd MO", &ddata->mo_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->lr_gpio, GPIOF_OUT_INIT_HIGH,
+		"lcd LR", &ddata->lr_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->ud_gpio, GPIOF_OUT_INIT_HIGH,
+		"lcd UD", &ddata->ud_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->resb_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd RESB", &ddata->resb_gpio);
+	if (r)
+		return r;
+	r = sharp_ls_get_gpio(&pdev->dev, pdata->ini_gpio, GPIOF_OUT_INIT_LOW,
+		"lcd INI", &ddata->ini_gpio);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static  int sharp_ls_get_gpio_of(struct device *dev, int index, int val,
+	const char *desc, struct gpio_desc **gpiod)
+{
+	struct gpio_desc *gd;
+
+	*gpiod = NULL;
+
+	gd = devm_gpiod_get_index(dev, desc, index, GPIOD_OUT_LOW);
+	if (IS_ERR(gd))
+		return PTR_ERR(gd);
+
+	*gpiod = gd;
+	return 0;
+}
+
+static int sharp_ls_probe_of(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct device_node *node = pdev->dev.of_node;
+	struct omap_dss_device *in;
+	int r;
+
+	ddata->vcc = devm_regulator_get(&pdev->dev, "envdd");
+	if (IS_ERR(ddata->vcc)) {
+		dev_err(&pdev->dev, "failed to get regulator\n");
+		return PTR_ERR(ddata->vcc);
+	}
+
+	/* lcd INI */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "enable", &ddata->ini_gpio);
+	if (r)
+		return r;
+
+	/* lcd RESB */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "reset", &ddata->resb_gpio);
+	if (r)
+		return r;
+
+	/* lcd MO */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 0, 0, "mode", &ddata->mo_gpio);
+	if (r)
+		return r;
+
+	/* lcd LR */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 1, 1, "mode", &ddata->lr_gpio);
+	if (r)
+		return r;
+
+	/* lcd UD */
+	r = sharp_ls_get_gpio_of(&pdev->dev, 2, 1, "mode", &ddata->ud_gpio);
+	if (r)
+		return r;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&pdev->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int sharp_ls_probe(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	ddata = devm_kzalloc(&pdev->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, ddata);
+
+	if (dev_get_platdata(&pdev->dev)) {
+		r = sharp_ls_probe_pdata(pdev);
+		if (r)
+			return r;
+	} else if (pdev->dev.of_node) {
+		r = sharp_ls_probe_of(pdev);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->videomode = sharp_ls_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &pdev->dev;
+	dssdev->driver = &sharp_ls_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+	dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&pdev->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int __exit sharp_ls_remove(struct platform_device *pdev)
+{
+	struct panel_drv_data *ddata = platform_get_drvdata(pdev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	omapdss_unregister_display(dssdev);
+
+	sharp_ls_disable(dssdev);
+	sharp_ls_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id sharp_ls_of_match[] = {
+	{ .compatible = "omapdss,sharp,ls037v7dw01", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, sharp_ls_of_match);
+
+static struct platform_driver sharp_ls_driver = {
+	.probe = sharp_ls_probe,
+	.remove = __exit_p(sharp_ls_remove),
+	.driver = {
+		.name = "panel-sharp-ls037v7dw01",
+		.of_match_table = sharp_ls_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_platform_driver(sharp_ls_driver);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ti.com>");
+MODULE_DESCRIPTION("Sharp LS037V7DW01 Panel Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c
new file mode 100644
index 000000000000..31efcca801bd
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-sony-acx565akm.c
@@ -0,0 +1,917 @@
+/*
+ * Sony ACX565AKM LCD Panel driver
+ *
+ * Copyright (C) 2010 Nokia Corporation
+ *
+ * Original Driver Author: Imre Deak <imre.deak@nokia.com>
+ * Based on panel-generic.c by Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ * Adapted to new DSS2 framework: Roger Quadros <roger.quadros@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/jiffies.h>
+#include <linux/sched.h>
+#include <linux/backlight.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/of.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+#define MIPID_CMD_READ_DISP_ID		0x04
+#define MIPID_CMD_READ_RED		0x06
+#define MIPID_CMD_READ_GREEN		0x07
+#define MIPID_CMD_READ_BLUE		0x08
+#define MIPID_CMD_READ_DISP_STATUS	0x09
+#define MIPID_CMD_RDDSDR		0x0F
+#define MIPID_CMD_SLEEP_IN		0x10
+#define MIPID_CMD_SLEEP_OUT		0x11
+#define MIPID_CMD_DISP_OFF		0x28
+#define MIPID_CMD_DISP_ON		0x29
+#define MIPID_CMD_WRITE_DISP_BRIGHTNESS	0x51
+#define MIPID_CMD_READ_DISP_BRIGHTNESS	0x52
+#define MIPID_CMD_WRITE_CTRL_DISP	0x53
+
+#define CTRL_DISP_BRIGHTNESS_CTRL_ON	(1 << 5)
+#define CTRL_DISP_AMBIENT_LIGHT_CTRL_ON	(1 << 4)
+#define CTRL_DISP_BACKLIGHT_ON		(1 << 2)
+#define CTRL_DISP_AUTO_BRIGHTNESS_ON	(1 << 1)
+
+#define MIPID_CMD_READ_CTRL_DISP	0x54
+#define MIPID_CMD_WRITE_CABC		0x55
+#define MIPID_CMD_READ_CABC		0x56
+
+#define MIPID_VER_LPH8923		3
+#define MIPID_VER_LS041Y3		4
+#define MIPID_VER_L4F00311		8
+#define MIPID_VER_ACX565AKM		9
+
+struct panel_drv_data {
+	struct omap_dss_device	dssdev;
+	struct omap_dss_device *in;
+
+	int reset_gpio;
+	int datapairs;
+
+	struct omap_video_timings videomode;
+
+	char		*name;
+	int		enabled;
+	int		model;
+	int		revision;
+	u8		display_id[3];
+	unsigned	has_bc:1;
+	unsigned	has_cabc:1;
+	unsigned	cabc_mode;
+	unsigned long	hw_guard_end;		/* next value of jiffies
+						   when we can issue the
+						   next sleep in/out command */
+	unsigned long	hw_guard_wait;		/* max guard time in jiffies */
+
+	struct spi_device	*spi;
+	struct mutex		mutex;
+
+	struct backlight_device *bl_dev;
+};
+
+static const struct omap_video_timings acx565akm_panel_timings = {
+	.x_res		= 800,
+	.y_res		= 480,
+	.pixelclock	= 24000000,
+	.hfp		= 28,
+	.hsw		= 4,
+	.hbp		= 24,
+	.vfp		= 3,
+	.vsw		= 3,
+	.vbp		= 4,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static void acx565akm_transfer(struct panel_drv_data *ddata, int cmd,
+			      const u8 *wbuf, int wlen, u8 *rbuf, int rlen)
+{
+	struct spi_message	m;
+	struct spi_transfer	*x, xfer[5];
+	int			r;
+
+	BUG_ON(ddata->spi == NULL);
+
+	spi_message_init(&m);
+
+	memset(xfer, 0, sizeof(xfer));
+	x = &xfer[0];
+
+	cmd &=  0xff;
+	x->tx_buf = &cmd;
+	x->bits_per_word = 9;
+	x->len = 2;
+
+	if (rlen > 1 && wlen == 0) {
+		/*
+		 * Between the command and the response data there is a
+		 * dummy clock cycle. Add an extra bit after the command
+		 * word to account for this.
+		 */
+		x->bits_per_word = 10;
+		cmd <<= 1;
+	}
+	spi_message_add_tail(x, &m);
+
+	if (wlen) {
+		x++;
+		x->tx_buf = wbuf;
+		x->len = wlen;
+		x->bits_per_word = 9;
+		spi_message_add_tail(x, &m);
+	}
+
+	if (rlen) {
+		x++;
+		x->rx_buf	= rbuf;
+		x->len		= rlen;
+		spi_message_add_tail(x, &m);
+	}
+
+	r = spi_sync(ddata->spi, &m);
+	if (r < 0)
+		dev_dbg(&ddata->spi->dev, "spi_sync %d\n", r);
+}
+
+static inline void acx565akm_cmd(struct panel_drv_data *ddata, int cmd)
+{
+	acx565akm_transfer(ddata, cmd, NULL, 0, NULL, 0);
+}
+
+static inline void acx565akm_write(struct panel_drv_data *ddata,
+			       int reg, const u8 *buf, int len)
+{
+	acx565akm_transfer(ddata, reg, buf, len, NULL, 0);
+}
+
+static inline void acx565akm_read(struct panel_drv_data *ddata,
+			      int reg, u8 *buf, int len)
+{
+	acx565akm_transfer(ddata, reg, NULL, 0, buf, len);
+}
+
+static void hw_guard_start(struct panel_drv_data *ddata, int guard_msec)
+{
+	ddata->hw_guard_wait = msecs_to_jiffies(guard_msec);
+	ddata->hw_guard_end = jiffies + ddata->hw_guard_wait;
+}
+
+static void hw_guard_wait(struct panel_drv_data *ddata)
+{
+	unsigned long wait = ddata->hw_guard_end - jiffies;
+
+	if ((long)wait > 0 && wait <= ddata->hw_guard_wait) {
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_timeout(wait);
+	}
+}
+
+static void set_sleep_mode(struct panel_drv_data *ddata, int on)
+{
+	int cmd;
+
+	if (on)
+		cmd = MIPID_CMD_SLEEP_IN;
+	else
+		cmd = MIPID_CMD_SLEEP_OUT;
+	/*
+	 * We have to keep 120msec between sleep in/out commands.
+	 * (8.2.15, 8.2.16).
+	 */
+	hw_guard_wait(ddata);
+	acx565akm_cmd(ddata, cmd);
+	hw_guard_start(ddata, 120);
+}
+
+static void set_display_state(struct panel_drv_data *ddata, int enabled)
+{
+	int cmd = enabled ? MIPID_CMD_DISP_ON : MIPID_CMD_DISP_OFF;
+
+	acx565akm_cmd(ddata, cmd);
+}
+
+static int panel_enabled(struct panel_drv_data *ddata)
+{
+	u32 disp_status;
+	int enabled;
+
+	acx565akm_read(ddata, MIPID_CMD_READ_DISP_STATUS,
+			(u8 *)&disp_status, 4);
+	disp_status = __be32_to_cpu(disp_status);
+	enabled = (disp_status & (1 << 17)) && (disp_status & (1 << 10));
+	dev_dbg(&ddata->spi->dev,
+		"LCD panel %senabled by bootloader (status 0x%04x)\n",
+		enabled ? "" : "not ", disp_status);
+	return enabled;
+}
+
+static int panel_detect(struct panel_drv_data *ddata)
+{
+	acx565akm_read(ddata, MIPID_CMD_READ_DISP_ID, ddata->display_id, 3);
+	dev_dbg(&ddata->spi->dev, "MIPI display ID: %02x%02x%02x\n",
+		ddata->display_id[0],
+		ddata->display_id[1],
+		ddata->display_id[2]);
+
+	switch (ddata->display_id[0]) {
+	case 0x10:
+		ddata->model = MIPID_VER_ACX565AKM;
+		ddata->name = "acx565akm";
+		ddata->has_bc = 1;
+		ddata->has_cabc = 1;
+		break;
+	case 0x29:
+		ddata->model = MIPID_VER_L4F00311;
+		ddata->name = "l4f00311";
+		break;
+	case 0x45:
+		ddata->model = MIPID_VER_LPH8923;
+		ddata->name = "lph8923";
+		break;
+	case 0x83:
+		ddata->model = MIPID_VER_LS041Y3;
+		ddata->name = "ls041y3";
+		break;
+	default:
+		ddata->name = "unknown";
+		dev_err(&ddata->spi->dev, "invalid display ID\n");
+		return -ENODEV;
+	}
+
+	ddata->revision = ddata->display_id[1];
+
+	dev_info(&ddata->spi->dev, "omapfb: %s rev %02x LCD detected\n",
+			ddata->name, ddata->revision);
+
+	return 0;
+}
+
+/*----------------------Backlight Control-------------------------*/
+
+static void enable_backlight_ctrl(struct panel_drv_data *ddata, int enable)
+{
+	u16 ctrl;
+
+	acx565akm_read(ddata, MIPID_CMD_READ_CTRL_DISP, (u8 *)&ctrl, 1);
+	if (enable) {
+		ctrl |= CTRL_DISP_BRIGHTNESS_CTRL_ON |
+			CTRL_DISP_BACKLIGHT_ON;
+	} else {
+		ctrl &= ~(CTRL_DISP_BRIGHTNESS_CTRL_ON |
+			  CTRL_DISP_BACKLIGHT_ON);
+	}
+
+	ctrl |= 1 << 8;
+	acx565akm_write(ddata, MIPID_CMD_WRITE_CTRL_DISP, (u8 *)&ctrl, 2);
+}
+
+static void set_cabc_mode(struct panel_drv_data *ddata, unsigned mode)
+{
+	u16 cabc_ctrl;
+
+	ddata->cabc_mode = mode;
+	if (!ddata->enabled)
+		return;
+	cabc_ctrl = 0;
+	acx565akm_read(ddata, MIPID_CMD_READ_CABC, (u8 *)&cabc_ctrl, 1);
+	cabc_ctrl &= ~3;
+	cabc_ctrl |= (1 << 8) | (mode & 3);
+	acx565akm_write(ddata, MIPID_CMD_WRITE_CABC, (u8 *)&cabc_ctrl, 2);
+}
+
+static unsigned get_cabc_mode(struct panel_drv_data *ddata)
+{
+	return ddata->cabc_mode;
+}
+
+static unsigned get_hw_cabc_mode(struct panel_drv_data *ddata)
+{
+	u8 cabc_ctrl;
+
+	acx565akm_read(ddata, MIPID_CMD_READ_CABC, &cabc_ctrl, 1);
+	return cabc_ctrl & 3;
+}
+
+static void acx565akm_set_brightness(struct panel_drv_data *ddata, int level)
+{
+	int bv;
+
+	bv = level | (1 << 8);
+	acx565akm_write(ddata, MIPID_CMD_WRITE_DISP_BRIGHTNESS, (u8 *)&bv, 2);
+
+	if (level)
+		enable_backlight_ctrl(ddata, 1);
+	else
+		enable_backlight_ctrl(ddata, 0);
+}
+
+static int acx565akm_get_actual_brightness(struct panel_drv_data *ddata)
+{
+	u8 bv;
+
+	acx565akm_read(ddata, MIPID_CMD_READ_DISP_BRIGHTNESS, &bv, 1);
+
+	return bv;
+}
+
+
+static int acx565akm_bl_update_status(struct backlight_device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+	int level;
+
+	dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+			dev->props.power == FB_BLANK_UNBLANK)
+		level = dev->props.brightness;
+	else
+		level = 0;
+
+	if (ddata->has_bc)
+		acx565akm_set_brightness(ddata, level);
+	else
+		return -ENODEV;
+
+	return 0;
+}
+
+static int acx565akm_bl_get_intensity(struct backlight_device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+
+	dev_dbg(&dev->dev, "%s\n", __func__);
+
+	if (!ddata->has_bc)
+		return -ENODEV;
+
+	if (dev->props.fb_blank == FB_BLANK_UNBLANK &&
+			dev->props.power == FB_BLANK_UNBLANK) {
+		if (ddata->has_bc)
+			return acx565akm_get_actual_brightness(ddata);
+		else
+			return dev->props.brightness;
+	}
+
+	return 0;
+}
+
+static int acx565akm_bl_update_status_locked(struct backlight_device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+	int r;
+
+	mutex_lock(&ddata->mutex);
+	r = acx565akm_bl_update_status(dev);
+	mutex_unlock(&ddata->mutex);
+
+	return r;
+}
+
+static int acx565akm_bl_get_intensity_locked(struct backlight_device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&dev->dev);
+	int r;
+
+	mutex_lock(&ddata->mutex);
+	r = acx565akm_bl_get_intensity(dev);
+	mutex_unlock(&ddata->mutex);
+
+	return r;
+}
+
+static const struct backlight_ops acx565akm_bl_ops = {
+	.get_brightness = acx565akm_bl_get_intensity_locked,
+	.update_status  = acx565akm_bl_update_status_locked,
+};
+
+/*--------------------Auto Brightness control via Sysfs---------------------*/
+
+static const char * const cabc_modes[] = {
+	"off",		/* always used when CABC is not supported */
+	"ui",
+	"still-image",
+	"moving-image",
+};
+
+static ssize_t show_cabc_mode(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	const char *mode_str;
+	int mode;
+	int len;
+
+	if (!ddata->has_cabc)
+		mode = 0;
+	else
+		mode = get_cabc_mode(ddata);
+	mode_str = "unknown";
+	if (mode >= 0 && mode < ARRAY_SIZE(cabc_modes))
+		mode_str = cabc_modes[mode];
+	len = snprintf(buf, PAGE_SIZE, "%s\n", mode_str);
+
+	return len < PAGE_SIZE - 1 ? len : PAGE_SIZE - 1;
+}
+
+static ssize_t store_cabc_mode(struct device *dev,
+		struct device_attribute *attr,
+		const char *buf, size_t count)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(cabc_modes); i++) {
+		const char *mode_str = cabc_modes[i];
+		int cmp_len = strlen(mode_str);
+
+		if (count > 0 && buf[count - 1] == '\n')
+			count--;
+		if (count != cmp_len)
+			continue;
+
+		if (strncmp(buf, mode_str, cmp_len) == 0)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(cabc_modes))
+		return -EINVAL;
+
+	if (!ddata->has_cabc && i != 0)
+		return -EINVAL;
+
+	mutex_lock(&ddata->mutex);
+	set_cabc_mode(ddata, i);
+	mutex_unlock(&ddata->mutex);
+
+	return count;
+}
+
+static ssize_t show_cabc_available_modes(struct device *dev,
+		struct device_attribute *attr,
+		char *buf)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	int len;
+	int i;
+
+	if (!ddata->has_cabc)
+		return snprintf(buf, PAGE_SIZE, "%s\n", cabc_modes[0]);
+
+	for (i = 0, len = 0;
+	     len < PAGE_SIZE && i < ARRAY_SIZE(cabc_modes); i++)
+		len += snprintf(&buf[len], PAGE_SIZE - len, "%s%s%s",
+			i ? " " : "", cabc_modes[i],
+			i == ARRAY_SIZE(cabc_modes) - 1 ? "\n" : "");
+
+	return len < PAGE_SIZE ? len : PAGE_SIZE - 1;
+}
+
+static DEVICE_ATTR(cabc_mode, S_IRUGO | S_IWUSR,
+		show_cabc_mode, store_cabc_mode);
+static DEVICE_ATTR(cabc_available_modes, S_IRUGO,
+		show_cabc_available_modes, NULL);
+
+static struct attribute *bldev_attrs[] = {
+	&dev_attr_cabc_mode.attr,
+	&dev_attr_cabc_available_modes.attr,
+	NULL,
+};
+
+static struct attribute_group bldev_attr_group = {
+	.attrs = bldev_attrs,
+};
+
+static int acx565akm_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.sdi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void acx565akm_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.sdi->disconnect(in, dssdev);
+}
+
+static int acx565akm_panel_power_on(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+	in->ops.sdi->set_timings(in, &ddata->videomode);
+
+	if (ddata->datapairs > 0)
+		in->ops.sdi->set_datapairs(in, ddata->datapairs);
+
+	r = in->ops.sdi->enable(in);
+	if (r) {
+		pr_err("%s sdi enable failed\n", __func__);
+		return r;
+	}
+
+	/*FIXME tweak me */
+	msleep(50);
+
+	if (gpio_is_valid(ddata->reset_gpio))
+		gpio_set_value(ddata->reset_gpio, 1);
+
+	if (ddata->enabled) {
+		dev_dbg(&ddata->spi->dev, "panel already enabled\n");
+		return 0;
+	}
+
+	/*
+	 * We have to meet all the following delay requirements:
+	 * 1. tRW: reset pulse width 10usec (7.12.1)
+	 * 2. tRT: reset cancel time 5msec (7.12.1)
+	 * 3. Providing PCLK,HS,VS signals for 2 frames = ~50msec worst
+	 *    case (7.6.2)
+	 * 4. 120msec before the sleep out command (7.12.1)
+	 */
+	msleep(120);
+
+	set_sleep_mode(ddata, 0);
+	ddata->enabled = 1;
+
+	/* 5msec between sleep out and the next command. (8.2.16) */
+	usleep_range(5000, 10000);
+	set_display_state(ddata, 1);
+	set_cabc_mode(ddata, ddata->cabc_mode);
+
+	return acx565akm_bl_update_status(ddata->bl_dev);
+}
+
+static void acx565akm_panel_power_off(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(dssdev->dev, "%s\n", __func__);
+
+	if (!ddata->enabled)
+		return;
+
+	set_display_state(ddata, 0);
+	set_sleep_mode(ddata, 1);
+	ddata->enabled = 0;
+	/*
+	 * We have to provide PCLK,HS,VS signals for 2 frames (worst case
+	 * ~50msec) after sending the sleep in command and asserting the
+	 * reset signal. We probably could assert the reset w/o the delay
+	 * but we still delay to avoid possible artifacts. (7.6.1)
+	 */
+	msleep(50);
+
+	if (gpio_is_valid(ddata->reset_gpio))
+		gpio_set_value(ddata->reset_gpio, 0);
+
+	/* FIXME need to tweak this delay */
+	msleep(100);
+
+	in->ops.sdi->disable(in);
+}
+
+static int acx565akm_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	int r;
+
+	dev_dbg(dssdev->dev, "%s\n", __func__);
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	mutex_lock(&ddata->mutex);
+	r = acx565akm_panel_power_on(dssdev);
+	mutex_unlock(&ddata->mutex);
+	if (r)
+		return r;
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void acx565akm_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	dev_dbg(dssdev->dev, "%s\n", __func__);
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	mutex_lock(&ddata->mutex);
+	acx565akm_panel_power_off(dssdev);
+	mutex_unlock(&ddata->mutex);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void acx565akm_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.sdi->set_timings(in, timings);
+}
+
+static void acx565akm_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int acx565akm_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.sdi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver acx565akm_ops = {
+	.connect	= acx565akm_connect,
+	.disconnect	= acx565akm_disconnect,
+
+	.enable		= acx565akm_enable,
+	.disable	= acx565akm_disable,
+
+	.set_timings	= acx565akm_set_timings,
+	.get_timings	= acx565akm_get_timings,
+	.check_timings	= acx565akm_check_timings,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+static int acx565akm_probe_pdata(struct spi_device *spi)
+{
+	const struct panel_acx565akm_platform_data *pdata;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&spi->dev);
+
+	ddata->reset_gpio = pdata->reset_gpio;
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&spi->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+	ddata->in = in;
+
+	ddata->datapairs = pdata->datapairs;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int acx565akm_probe_of(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct device_node *np = spi->dev.of_node;
+
+	ddata->reset_gpio = of_get_named_gpio(np, "reset-gpios", 0);
+
+	ddata->in = omapdss_of_find_source_for_first_ep(np);
+	if (IS_ERR(ddata->in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(ddata->in);
+	}
+
+	return 0;
+}
+
+static int acx565akm_probe(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	struct backlight_device *bldev;
+	int max_brightness, brightness;
+	struct backlight_properties props;
+	int r;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	spi->mode = SPI_MODE_3;
+
+	ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, ddata);
+
+	ddata->spi = spi;
+
+	mutex_init(&ddata->mutex);
+
+	if (dev_get_platdata(&spi->dev)) {
+		r = acx565akm_probe_pdata(spi);
+		if (r)
+			return r;
+	} else if (spi->dev.of_node) {
+		r = acx565akm_probe_of(spi);
+		if (r)
+			return r;
+	} else {
+		dev_err(&spi->dev, "platform data missing!\n");
+		return -ENODEV;
+	}
+
+	if (gpio_is_valid(ddata->reset_gpio)) {
+		r = devm_gpio_request_one(&spi->dev, ddata->reset_gpio,
+				GPIOF_OUT_INIT_LOW, "lcd reset");
+		if (r)
+			goto err_gpio;
+	}
+
+	if (gpio_is_valid(ddata->reset_gpio))
+		gpio_set_value(ddata->reset_gpio, 1);
+
+	/*
+	 * After reset we have to wait 5 msec before the first
+	 * command can be sent.
+	 */
+	usleep_range(5000, 10000);
+
+	ddata->enabled = panel_enabled(ddata);
+
+	r = panel_detect(ddata);
+
+	if (!ddata->enabled && gpio_is_valid(ddata->reset_gpio))
+		gpio_set_value(ddata->reset_gpio, 0);
+
+	if (r) {
+		dev_err(&spi->dev, "%s panel detect error\n", __func__);
+		goto err_detect;
+	}
+
+	memset(&props, 0, sizeof(props));
+	props.fb_blank = FB_BLANK_UNBLANK;
+	props.power = FB_BLANK_UNBLANK;
+	props.type = BACKLIGHT_RAW;
+
+	bldev = backlight_device_register("acx565akm", &ddata->spi->dev,
+			ddata, &acx565akm_bl_ops, &props);
+	if (IS_ERR(bldev)) {
+		r = PTR_ERR(bldev);
+		goto err_reg_bl;
+	}
+	ddata->bl_dev = bldev;
+	if (ddata->has_cabc) {
+		r = sysfs_create_group(&bldev->dev.kobj, &bldev_attr_group);
+		if (r) {
+			dev_err(&bldev->dev,
+				"%s failed to create sysfs files\n", __func__);
+			goto err_sysfs;
+		}
+		ddata->cabc_mode = get_hw_cabc_mode(ddata);
+	}
+
+	max_brightness = 255;
+
+	if (ddata->has_bc)
+		brightness = acx565akm_get_actual_brightness(ddata);
+	else
+		brightness = 0;
+
+	bldev->props.max_brightness = max_brightness;
+	bldev->props.brightness = brightness;
+
+	acx565akm_bl_update_status(bldev);
+
+
+	ddata->videomode = acx565akm_panel_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &spi->dev;
+	dssdev->driver = &acx565akm_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_SDI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&spi->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	sysfs_remove_group(&bldev->dev.kobj, &bldev_attr_group);
+err_sysfs:
+	backlight_device_unregister(bldev);
+err_reg_bl:
+err_detect:
+err_gpio:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int acx565akm_remove(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+	sysfs_remove_group(&ddata->bl_dev->dev.kobj, &bldev_attr_group);
+	backlight_device_unregister(ddata->bl_dev);
+
+	omapdss_unregister_display(dssdev);
+
+	acx565akm_disable(dssdev);
+	acx565akm_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id acx565akm_of_match[] = {
+	{ .compatible = "omapdss,sony,acx565akm", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, acx565akm_of_match);
+
+static struct spi_driver acx565akm_driver = {
+	.driver = {
+		.name	= "acx565akm",
+		.of_match_table = acx565akm_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe	= acx565akm_probe,
+	.remove	= acx565akm_remove,
+};
+
+module_spi_driver(acx565akm_driver);
+
+MODULE_AUTHOR("Nokia Corporation");
+MODULE_DESCRIPTION("acx565akm LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c
new file mode 100644
index 000000000000..4d657f3ab679
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td028ttec1.c
@@ -0,0 +1,511 @@
+/*
+ * Toppoly TD028TTEC1 panel support
+ *
+ * Copyright (C) 2008 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Neo 1973 code (jbt6k74.c):
+ * Copyright (C) 2006-2007 by OpenMoko, Inc.
+ * Author: Harald Welte <laforge@openmoko.org>
+ *
+ * Ported and adapted from Neo 1973 U-Boot by:
+ * H. Nikolaus Schaller <hns@goldelico.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/gpio.h>
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+struct panel_drv_data {
+	struct omap_dss_device dssdev;
+	struct omap_dss_device *in;
+
+	int data_lines;
+
+	struct omap_video_timings videomode;
+
+	struct spi_device *spi_dev;
+};
+
+static struct omap_video_timings td028ttec1_panel_timings = {
+	.x_res		= 480,
+	.y_res		= 640,
+	.pixelclock	= 22153000,
+	.hfp		= 24,
+	.hsw		= 8,
+	.hbp		= 8,
+	.vfp		= 4,
+	.vsw		= 2,
+	.vbp		= 2,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define JBT_COMMAND	0x000
+#define JBT_DATA	0x100
+
+static int jbt_ret_write_0(struct panel_drv_data *ddata, u8 reg)
+{
+	int rc;
+	u16 tx_buf = JBT_COMMAND | reg;
+
+	rc = spi_write(ddata->spi_dev, (u8 *)&tx_buf,
+			1*sizeof(u16));
+	if (rc != 0)
+		dev_err(&ddata->spi_dev->dev,
+			"jbt_ret_write_0 spi_write ret %d\n", rc);
+
+	return rc;
+}
+
+static int jbt_reg_write_1(struct panel_drv_data *ddata, u8 reg, u8 data)
+{
+	int rc;
+	u16 tx_buf[2];
+
+	tx_buf[0] = JBT_COMMAND | reg;
+	tx_buf[1] = JBT_DATA | data;
+	rc = spi_write(ddata->spi_dev, (u8 *)tx_buf,
+			2*sizeof(u16));
+	if (rc != 0)
+		dev_err(&ddata->spi_dev->dev,
+			"jbt_reg_write_1 spi_write ret %d\n", rc);
+
+	return rc;
+}
+
+static int jbt_reg_write_2(struct panel_drv_data *ddata, u8 reg, u16 data)
+{
+	int rc;
+	u16 tx_buf[3];
+
+	tx_buf[0] = JBT_COMMAND | reg;
+	tx_buf[1] = JBT_DATA | (data >> 8);
+	tx_buf[2] = JBT_DATA | (data & 0xff);
+
+	rc = spi_write(ddata->spi_dev, (u8 *)tx_buf,
+			3*sizeof(u16));
+
+	if (rc != 0)
+		dev_err(&ddata->spi_dev->dev,
+			"jbt_reg_write_2 spi_write ret %d\n", rc);
+
+	return rc;
+}
+
+enum jbt_register {
+	JBT_REG_SLEEP_IN		= 0x10,
+	JBT_REG_SLEEP_OUT		= 0x11,
+
+	JBT_REG_DISPLAY_OFF		= 0x28,
+	JBT_REG_DISPLAY_ON		= 0x29,
+
+	JBT_REG_RGB_FORMAT		= 0x3a,
+	JBT_REG_QUAD_RATE		= 0x3b,
+
+	JBT_REG_POWER_ON_OFF		= 0xb0,
+	JBT_REG_BOOSTER_OP		= 0xb1,
+	JBT_REG_BOOSTER_MODE		= 0xb2,
+	JBT_REG_BOOSTER_FREQ		= 0xb3,
+	JBT_REG_OPAMP_SYSCLK		= 0xb4,
+	JBT_REG_VSC_VOLTAGE		= 0xb5,
+	JBT_REG_VCOM_VOLTAGE		= 0xb6,
+	JBT_REG_EXT_DISPL		= 0xb7,
+	JBT_REG_OUTPUT_CONTROL		= 0xb8,
+	JBT_REG_DCCLK_DCEV		= 0xb9,
+	JBT_REG_DISPLAY_MODE1		= 0xba,
+	JBT_REG_DISPLAY_MODE2		= 0xbb,
+	JBT_REG_DISPLAY_MODE		= 0xbc,
+	JBT_REG_ASW_SLEW		= 0xbd,
+	JBT_REG_DUMMY_DISPLAY		= 0xbe,
+	JBT_REG_DRIVE_SYSTEM		= 0xbf,
+
+	JBT_REG_SLEEP_OUT_FR_A		= 0xc0,
+	JBT_REG_SLEEP_OUT_FR_B		= 0xc1,
+	JBT_REG_SLEEP_OUT_FR_C		= 0xc2,
+	JBT_REG_SLEEP_IN_LCCNT_D	= 0xc3,
+	JBT_REG_SLEEP_IN_LCCNT_E	= 0xc4,
+	JBT_REG_SLEEP_IN_LCCNT_F	= 0xc5,
+	JBT_REG_SLEEP_IN_LCCNT_G	= 0xc6,
+
+	JBT_REG_GAMMA1_FINE_1		= 0xc7,
+	JBT_REG_GAMMA1_FINE_2		= 0xc8,
+	JBT_REG_GAMMA1_INCLINATION	= 0xc9,
+	JBT_REG_GAMMA1_BLUE_OFFSET	= 0xca,
+
+	JBT_REG_BLANK_CONTROL		= 0xcf,
+	JBT_REG_BLANK_TH_TV		= 0xd0,
+	JBT_REG_CKV_ON_OFF		= 0xd1,
+	JBT_REG_CKV_1_2			= 0xd2,
+	JBT_REG_OEV_TIMING		= 0xd3,
+	JBT_REG_ASW_TIMING_1		= 0xd4,
+	JBT_REG_ASW_TIMING_2		= 0xd5,
+
+	JBT_REG_HCLOCK_VGA		= 0xec,
+	JBT_REG_HCLOCK_QVGA		= 0xed,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int td028ttec1_panel_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void td028ttec1_panel_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int td028ttec1_panel_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	dev_dbg(dssdev->dev, "td028ttec1_panel_enable() - state %d\n",
+		dssdev->state);
+
+	/* three times command zero */
+	r |= jbt_ret_write_0(ddata, 0x00);
+	usleep_range(1000, 2000);
+	r |= jbt_ret_write_0(ddata, 0x00);
+	usleep_range(1000, 2000);
+	r |= jbt_ret_write_0(ddata, 0x00);
+	usleep_range(1000, 2000);
+
+	if (r) {
+		dev_warn(dssdev->dev, "transfer error\n");
+		goto transfer_err;
+	}
+
+	/* deep standby out */
+	r |= jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x17);
+
+	/* RGB I/F on, RAM write off, QVGA through, SIGCON enable */
+	r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE, 0x80);
+
+	/* Quad mode off */
+	r |= jbt_reg_write_1(ddata, JBT_REG_QUAD_RATE, 0x00);
+
+	/* AVDD on, XVDD on */
+	r |= jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x16);
+
+	/* Output control */
+	r |= jbt_reg_write_2(ddata, JBT_REG_OUTPUT_CONTROL, 0xfff9);
+
+	/* Sleep mode off */
+	r |= jbt_ret_write_0(ddata, JBT_REG_SLEEP_OUT);
+
+	/* at this point we have like 50% grey */
+
+	/* initialize register set */
+	r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE1, 0x01);
+	r |= jbt_reg_write_1(ddata, JBT_REG_DISPLAY_MODE2, 0x00);
+	r |= jbt_reg_write_1(ddata, JBT_REG_RGB_FORMAT, 0x60);
+	r |= jbt_reg_write_1(ddata, JBT_REG_DRIVE_SYSTEM, 0x10);
+	r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_OP, 0x56);
+	r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_MODE, 0x33);
+	r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_FREQ, 0x11);
+	r |= jbt_reg_write_1(ddata, JBT_REG_BOOSTER_FREQ, 0x11);
+	r |= jbt_reg_write_1(ddata, JBT_REG_OPAMP_SYSCLK, 0x02);
+	r |= jbt_reg_write_1(ddata, JBT_REG_VSC_VOLTAGE, 0x2b);
+	r |= jbt_reg_write_1(ddata, JBT_REG_VCOM_VOLTAGE, 0x40);
+	r |= jbt_reg_write_1(ddata, JBT_REG_EXT_DISPL, 0x03);
+	r |= jbt_reg_write_1(ddata, JBT_REG_DCCLK_DCEV, 0x04);
+	/*
+	 * default of 0x02 in JBT_REG_ASW_SLEW responsible for 72Hz requirement
+	 * to avoid red / blue flicker
+	 */
+	r |= jbt_reg_write_1(ddata, JBT_REG_ASW_SLEW, 0x04);
+	r |= jbt_reg_write_1(ddata, JBT_REG_DUMMY_DISPLAY, 0x00);
+
+	r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_A, 0x11);
+	r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_B, 0x11);
+	r |= jbt_reg_write_1(ddata, JBT_REG_SLEEP_OUT_FR_C, 0x11);
+	r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_D, 0x2040);
+	r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_E, 0x60c0);
+	r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_F, 0x1020);
+	r |= jbt_reg_write_2(ddata, JBT_REG_SLEEP_IN_LCCNT_G, 0x60c0);
+
+	r |= jbt_reg_write_2(ddata, JBT_REG_GAMMA1_FINE_1, 0x5533);
+	r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_FINE_2, 0x00);
+	r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_INCLINATION, 0x00);
+	r |= jbt_reg_write_1(ddata, JBT_REG_GAMMA1_BLUE_OFFSET, 0x00);
+
+	r |= jbt_reg_write_2(ddata, JBT_REG_HCLOCK_VGA, 0x1f0);
+	r |= jbt_reg_write_1(ddata, JBT_REG_BLANK_CONTROL, 0x02);
+	r |= jbt_reg_write_2(ddata, JBT_REG_BLANK_TH_TV, 0x0804);
+
+	r |= jbt_reg_write_1(ddata, JBT_REG_CKV_ON_OFF, 0x01);
+	r |= jbt_reg_write_2(ddata, JBT_REG_CKV_1_2, 0x0000);
+
+	r |= jbt_reg_write_2(ddata, JBT_REG_OEV_TIMING, 0x0d0e);
+	r |= jbt_reg_write_2(ddata, JBT_REG_ASW_TIMING_1, 0x11a4);
+	r |= jbt_reg_write_1(ddata, JBT_REG_ASW_TIMING_2, 0x0e);
+
+	r |= jbt_ret_write_0(ddata, JBT_REG_DISPLAY_ON);
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+transfer_err:
+
+	return r ? -EIO : 0;
+}
+
+static void td028ttec1_panel_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	dev_dbg(dssdev->dev, "td028ttec1_panel_disable()\n");
+
+	jbt_ret_write_0(ddata, JBT_REG_DISPLAY_OFF);
+	jbt_reg_write_2(ddata, JBT_REG_OUTPUT_CONTROL, 0x8002);
+	jbt_ret_write_0(ddata, JBT_REG_SLEEP_IN);
+	jbt_reg_write_1(ddata, JBT_REG_POWER_ON_OFF, 0x00);
+
+	in->ops.dpi->disable(in);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void td028ttec1_panel_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void td028ttec1_panel_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int td028ttec1_panel_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver td028ttec1_ops = {
+	.connect	= td028ttec1_panel_connect,
+	.disconnect	= td028ttec1_panel_disconnect,
+
+	.enable		= td028ttec1_panel_enable,
+	.disable	= td028ttec1_panel_disable,
+
+	.set_timings	= td028ttec1_panel_set_timings,
+	.get_timings	= td028ttec1_panel_get_timings,
+	.check_timings	= td028ttec1_panel_check_timings,
+};
+
+static int td028ttec1_panel_probe_pdata(struct spi_device *spi)
+{
+	const struct panel_tpo_td028ttec1_platform_data *pdata;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&spi->dev);
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&spi->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int td028ttec1_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int td028ttec1_panel_probe(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	spi->bits_per_word = 9;
+	spi->mode = SPI_MODE_3;
+
+	r = spi_setup(spi);
+	if (r < 0) {
+		dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+		return r;
+	}
+
+	ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, ddata);
+
+	ddata->spi_dev = spi;
+
+	if (dev_get_platdata(&spi->dev)) {
+		r = td028ttec1_panel_probe_pdata(spi);
+		if (r)
+			return r;
+	} else if (spi->dev.of_node) {
+		r = td028ttec1_probe_of(spi);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->videomode = td028ttec1_panel_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &spi->dev;
+	dssdev->driver = &td028ttec1_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+	dssdev->phy.dpi.data_lines = ddata->data_lines;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&spi->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int td028ttec1_panel_remove(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->spi_dev->dev, "%s\n", __func__);
+
+	omapdss_unregister_display(dssdev);
+
+	td028ttec1_panel_disable(dssdev);
+	td028ttec1_panel_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	return 0;
+}
+
+static const struct of_device_id td028ttec1_of_match[] = {
+	{ .compatible = "omapdss,toppoly,td028ttec1", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, td028ttec1_of_match);
+
+static struct spi_driver td028ttec1_spi_driver = {
+	.probe		= td028ttec1_panel_probe,
+	.remove		= td028ttec1_panel_remove,
+
+	.driver         = {
+		.name   = "panel-tpo-td028ttec1",
+		.of_match_table = td028ttec1_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+module_spi_driver(td028ttec1_spi_driver);
+
+MODULE_ALIAS("spi:toppoly,td028ttec1");
+MODULE_AUTHOR("H. Nikolaus Schaller <hns@goldelico.com>");
+MODULE_DESCRIPTION("Toppoly TD028TTEC1 panel driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c
new file mode 100644
index 000000000000..68e3b68a2920
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/displays/panel-tpo-td043mtea1.c
@@ -0,0 +1,686 @@
+/*
+ * TPO TD043MTEA1 Panel driver
+ *
+ * Author: Gražvydas Ignotas <notasas@gmail.com>
+ * Converted to new DSS device model: Tomi Valkeinen <tomi.valkeinen@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; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/regulator/consumer.h>
+#include <linux/gpio.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/of_gpio.h>
+
+#include <video/omapdss.h>
+#include <video/omap-panel-data.h>
+
+#define TPO_R02_MODE(x)		((x) & 7)
+#define TPO_R02_MODE_800x480	7
+#define TPO_R02_NCLK_RISING	BIT(3)
+#define TPO_R02_HSYNC_HIGH	BIT(4)
+#define TPO_R02_VSYNC_HIGH	BIT(5)
+
+#define TPO_R03_NSTANDBY	BIT(0)
+#define TPO_R03_EN_CP_CLK	BIT(1)
+#define TPO_R03_EN_VGL_PUMP	BIT(2)
+#define TPO_R03_EN_PWM		BIT(3)
+#define TPO_R03_DRIVING_CAP_100	BIT(4)
+#define TPO_R03_EN_PRE_CHARGE	BIT(6)
+#define TPO_R03_SOFTWARE_CTL	BIT(7)
+
+#define TPO_R04_NFLIP_H		BIT(0)
+#define TPO_R04_NFLIP_V		BIT(1)
+#define TPO_R04_CP_CLK_FREQ_1H	BIT(2)
+#define TPO_R04_VGL_FREQ_1H	BIT(4)
+
+#define TPO_R03_VAL_NORMAL (TPO_R03_NSTANDBY | TPO_R03_EN_CP_CLK | \
+			TPO_R03_EN_VGL_PUMP |  TPO_R03_EN_PWM | \
+			TPO_R03_DRIVING_CAP_100 | TPO_R03_EN_PRE_CHARGE | \
+			TPO_R03_SOFTWARE_CTL)
+
+#define TPO_R03_VAL_STANDBY (TPO_R03_DRIVING_CAP_100 | \
+			TPO_R03_EN_PRE_CHARGE | TPO_R03_SOFTWARE_CTL)
+
+static const u16 tpo_td043_def_gamma[12] = {
+	105, 315, 381, 431, 490, 537, 579, 686, 780, 837, 880, 1023
+};
+
+struct panel_drv_data {
+	struct omap_dss_device	dssdev;
+	struct omap_dss_device *in;
+
+	struct omap_video_timings videomode;
+
+	int data_lines;
+
+	struct spi_device *spi;
+	struct regulator *vcc_reg;
+	int nreset_gpio;
+	u16 gamma[12];
+	u32 mode;
+	u32 hmirror:1;
+	u32 vmirror:1;
+	u32 powered_on:1;
+	u32 spi_suspended:1;
+	u32 power_on_resume:1;
+};
+
+static const struct omap_video_timings tpo_td043_timings = {
+	.x_res		= 800,
+	.y_res		= 480,
+
+	.pixelclock	= 36000000,
+
+	.hsw		= 1,
+	.hfp		= 68,
+	.hbp		= 214,
+
+	.vsw		= 1,
+	.vfp		= 39,
+	.vbp		= 34,
+
+	.vsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.hsync_level	= OMAPDSS_SIG_ACTIVE_LOW,
+	.data_pclk_edge	= OMAPDSS_DRIVE_SIG_FALLING_EDGE,
+	.de_level	= OMAPDSS_SIG_ACTIVE_HIGH,
+	.sync_pclk_edge	= OMAPDSS_DRIVE_SIG_RISING_EDGE,
+};
+
+#define to_panel_data(p) container_of(p, struct panel_drv_data, dssdev)
+
+static int tpo_td043_write(struct spi_device *spi, u8 addr, u8 data)
+{
+	struct spi_message	m;
+	struct spi_transfer	xfer;
+	u16			w;
+	int			r;
+
+	spi_message_init(&m);
+
+	memset(&xfer, 0, sizeof(xfer));
+
+	w = ((u16)addr << 10) | (1 << 8) | data;
+	xfer.tx_buf = &w;
+	xfer.bits_per_word = 16;
+	xfer.len = 2;
+	spi_message_add_tail(&xfer, &m);
+
+	r = spi_sync(spi, &m);
+	if (r < 0)
+		dev_warn(&spi->dev, "failed to write to LCD reg (%d)\n", r);
+	return r;
+}
+
+static void tpo_td043_write_gamma(struct spi_device *spi, u16 gamma[12])
+{
+	u8 i, val;
+
+	/* gamma bits [9:8] */
+	for (val = i = 0; i < 4; i++)
+		val |= (gamma[i] & 0x300) >> ((i + 1) * 2);
+	tpo_td043_write(spi, 0x11, val);
+
+	for (val = i = 0; i < 4; i++)
+		val |= (gamma[i+4] & 0x300) >> ((i + 1) * 2);
+	tpo_td043_write(spi, 0x12, val);
+
+	for (val = i = 0; i < 4; i++)
+		val |= (gamma[i+8] & 0x300) >> ((i + 1) * 2);
+	tpo_td043_write(spi, 0x13, val);
+
+	/* gamma bits [7:0] */
+	for (val = i = 0; i < 12; i++)
+		tpo_td043_write(spi, 0x14 + i, gamma[i] & 0xff);
+}
+
+static int tpo_td043_write_mirror(struct spi_device *spi, bool h, bool v)
+{
+	u8 reg4 = TPO_R04_NFLIP_H | TPO_R04_NFLIP_V |
+		TPO_R04_CP_CLK_FREQ_1H | TPO_R04_VGL_FREQ_1H;
+	if (h)
+		reg4 &= ~TPO_R04_NFLIP_H;
+	if (v)
+		reg4 &= ~TPO_R04_NFLIP_V;
+
+	return tpo_td043_write(spi, 4, reg4);
+}
+
+static int tpo_td043_set_hmirror(struct omap_dss_device *dssdev, bool enable)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dssdev->dev);
+
+	ddata->hmirror = enable;
+	return tpo_td043_write_mirror(ddata->spi, ddata->hmirror,
+			ddata->vmirror);
+}
+
+static bool tpo_td043_get_hmirror(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dssdev->dev);
+
+	return ddata->hmirror;
+}
+
+static ssize_t tpo_td043_vmirror_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", ddata->vmirror);
+}
+
+static ssize_t tpo_td043_vmirror_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	int val;
+	int ret;
+
+	ret = kstrtoint(buf, 0, &val);
+	if (ret < 0)
+		return ret;
+
+	val = !!val;
+
+	ret = tpo_td043_write_mirror(ddata->spi, ddata->hmirror, val);
+	if (ret < 0)
+		return ret;
+
+	ddata->vmirror = val;
+
+	return count;
+}
+
+static ssize_t tpo_td043_mode_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", ddata->mode);
+}
+
+static ssize_t tpo_td043_mode_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	long val;
+	int ret;
+
+	ret = kstrtol(buf, 0, &val);
+	if (ret != 0 || val & ~7)
+		return -EINVAL;
+
+	ddata->mode = val;
+
+	val |= TPO_R02_NCLK_RISING;
+	tpo_td043_write(ddata->spi, 2, val);
+
+	return count;
+}
+
+static ssize_t tpo_td043_gamma_show(struct device *dev,
+	struct device_attribute *attr, char *buf)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	ssize_t len = 0;
+	int ret;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(ddata->gamma); i++) {
+		ret = snprintf(buf + len, PAGE_SIZE - len, "%u ",
+				ddata->gamma[i]);
+		if (ret < 0)
+			return ret;
+		len += ret;
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t tpo_td043_gamma_store(struct device *dev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	unsigned int g[12];
+	int ret;
+	int i;
+
+	ret = sscanf(buf, "%u %u %u %u %u %u %u %u %u %u %u %u",
+			&g[0], &g[1], &g[2], &g[3], &g[4], &g[5],
+			&g[6], &g[7], &g[8], &g[9], &g[10], &g[11]);
+
+	if (ret != 12)
+		return -EINVAL;
+
+	for (i = 0; i < 12; i++)
+		ddata->gamma[i] = g[i];
+
+	tpo_td043_write_gamma(ddata->spi, ddata->gamma);
+
+	return count;
+}
+
+static DEVICE_ATTR(vmirror, S_IRUGO | S_IWUSR,
+		tpo_td043_vmirror_show, tpo_td043_vmirror_store);
+static DEVICE_ATTR(mode, S_IRUGO | S_IWUSR,
+		tpo_td043_mode_show, tpo_td043_mode_store);
+static DEVICE_ATTR(gamma, S_IRUGO | S_IWUSR,
+		tpo_td043_gamma_show, tpo_td043_gamma_store);
+
+static struct attribute *tpo_td043_attrs[] = {
+	&dev_attr_vmirror.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_gamma.attr,
+	NULL,
+};
+
+static struct attribute_group tpo_td043_attr_group = {
+	.attrs = tpo_td043_attrs,
+};
+
+static int tpo_td043_power_on(struct panel_drv_data *ddata)
+{
+	int r;
+
+	if (ddata->powered_on)
+		return 0;
+
+	r = regulator_enable(ddata->vcc_reg);
+	if (r != 0)
+		return r;
+
+	/* wait for panel to stabilize */
+	msleep(160);
+
+	if (gpio_is_valid(ddata->nreset_gpio))
+		gpio_set_value(ddata->nreset_gpio, 1);
+
+	tpo_td043_write(ddata->spi, 2,
+			TPO_R02_MODE(ddata->mode) | TPO_R02_NCLK_RISING);
+	tpo_td043_write(ddata->spi, 3, TPO_R03_VAL_NORMAL);
+	tpo_td043_write(ddata->spi, 0x20, 0xf0);
+	tpo_td043_write(ddata->spi, 0x21, 0xf0);
+	tpo_td043_write_mirror(ddata->spi, ddata->hmirror,
+			ddata->vmirror);
+	tpo_td043_write_gamma(ddata->spi, ddata->gamma);
+
+	ddata->powered_on = 1;
+	return 0;
+}
+
+static void tpo_td043_power_off(struct panel_drv_data *ddata)
+{
+	if (!ddata->powered_on)
+		return;
+
+	tpo_td043_write(ddata->spi, 3,
+			TPO_R03_VAL_STANDBY | TPO_R03_EN_PWM);
+
+	if (gpio_is_valid(ddata->nreset_gpio))
+		gpio_set_value(ddata->nreset_gpio, 0);
+
+	/* wait for at least 2 vsyncs before cutting off power */
+	msleep(50);
+
+	tpo_td043_write(ddata->spi, 3, TPO_R03_VAL_STANDBY);
+
+	regulator_disable(ddata->vcc_reg);
+
+	ddata->powered_on = 0;
+}
+
+static int tpo_td043_connect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (omapdss_device_is_connected(dssdev))
+		return 0;
+
+	r = in->ops.dpi->connect(in, dssdev);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void tpo_td043_disconnect(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return;
+
+	in->ops.dpi->disconnect(in, dssdev);
+}
+
+static int tpo_td043_enable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+	int r;
+
+	if (!omapdss_device_is_connected(dssdev))
+		return -ENODEV;
+
+	if (omapdss_device_is_enabled(dssdev))
+		return 0;
+
+	if (ddata->data_lines)
+		in->ops.dpi->set_data_lines(in, ddata->data_lines);
+	in->ops.dpi->set_timings(in, &ddata->videomode);
+
+	r = in->ops.dpi->enable(in);
+	if (r)
+		return r;
+
+	/*
+	 * If we are resuming from system suspend, SPI clocks might not be
+	 * enabled yet, so we'll program the LCD from SPI PM resume callback.
+	 */
+	if (!ddata->spi_suspended) {
+		r = tpo_td043_power_on(ddata);
+		if (r) {
+			in->ops.dpi->disable(in);
+			return r;
+		}
+	}
+
+	dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+	return 0;
+}
+
+static void tpo_td043_disable(struct omap_dss_device *dssdev)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	if (!omapdss_device_is_enabled(dssdev))
+		return;
+
+	in->ops.dpi->disable(in);
+
+	if (!ddata->spi_suspended)
+		tpo_td043_power_off(ddata);
+
+	dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+}
+
+static void tpo_td043_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	ddata->videomode = *timings;
+	dssdev->panel.timings = *timings;
+
+	in->ops.dpi->set_timings(in, timings);
+}
+
+static void tpo_td043_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+
+	*timings = ddata->videomode;
+}
+
+static int tpo_td043_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct panel_drv_data *ddata = to_panel_data(dssdev);
+	struct omap_dss_device *in = ddata->in;
+
+	return in->ops.dpi->check_timings(in, timings);
+}
+
+static struct omap_dss_driver tpo_td043_ops = {
+	.connect	= tpo_td043_connect,
+	.disconnect	= tpo_td043_disconnect,
+
+	.enable		= tpo_td043_enable,
+	.disable	= tpo_td043_disable,
+
+	.set_timings	= tpo_td043_set_timings,
+	.get_timings	= tpo_td043_get_timings,
+	.check_timings	= tpo_td043_check_timings,
+
+	.set_mirror	= tpo_td043_set_hmirror,
+	.get_mirror	= tpo_td043_get_hmirror,
+
+	.get_resolution	= omapdss_default_get_resolution,
+};
+
+
+static int tpo_td043_probe_pdata(struct spi_device *spi)
+{
+	const struct panel_tpo_td043mtea1_platform_data *pdata;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev, *in;
+
+	pdata = dev_get_platdata(&spi->dev);
+
+	ddata->nreset_gpio = pdata->nreset_gpio;
+
+	in = omap_dss_find_output(pdata->source);
+	if (in == NULL) {
+		dev_err(&spi->dev, "failed to find video source '%s'\n",
+				pdata->source);
+		return -EPROBE_DEFER;
+	}
+	ddata->in = in;
+
+	ddata->data_lines = pdata->data_lines;
+
+	dssdev = &ddata->dssdev;
+	dssdev->name = pdata->name;
+
+	return 0;
+}
+
+static int tpo_td043_probe_of(struct spi_device *spi)
+{
+	struct device_node *node = spi->dev.of_node;
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *in;
+	int gpio;
+
+	gpio = of_get_named_gpio(node, "reset-gpios", 0);
+	if (!gpio_is_valid(gpio)) {
+		dev_err(&spi->dev, "failed to parse enable gpio\n");
+		return gpio;
+	}
+	ddata->nreset_gpio = gpio;
+
+	in = omapdss_of_find_source_for_first_ep(node);
+	if (IS_ERR(in)) {
+		dev_err(&spi->dev, "failed to find video source\n");
+		return PTR_ERR(in);
+	}
+
+	ddata->in = in;
+
+	return 0;
+}
+
+static int tpo_td043_probe(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata;
+	struct omap_dss_device *dssdev;
+	int r;
+
+	dev_dbg(&spi->dev, "%s\n", __func__);
+
+	spi->bits_per_word = 16;
+	spi->mode = SPI_MODE_0;
+
+	r = spi_setup(spi);
+	if (r < 0) {
+		dev_err(&spi->dev, "spi_setup failed: %d\n", r);
+		return r;
+	}
+
+	ddata = devm_kzalloc(&spi->dev, sizeof(*ddata), GFP_KERNEL);
+	if (ddata == NULL)
+		return -ENOMEM;
+
+	dev_set_drvdata(&spi->dev, ddata);
+
+	ddata->spi = spi;
+
+	if (dev_get_platdata(&spi->dev)) {
+		r = tpo_td043_probe_pdata(spi);
+		if (r)
+			return r;
+	} else if (spi->dev.of_node) {
+		r = tpo_td043_probe_of(spi);
+		if (r)
+			return r;
+	} else {
+		return -ENODEV;
+	}
+
+	ddata->mode = TPO_R02_MODE_800x480;
+	memcpy(ddata->gamma, tpo_td043_def_gamma, sizeof(ddata->gamma));
+
+	ddata->vcc_reg = devm_regulator_get(&spi->dev, "vcc");
+	if (IS_ERR(ddata->vcc_reg)) {
+		dev_err(&spi->dev, "failed to get LCD VCC regulator\n");
+		r = PTR_ERR(ddata->vcc_reg);
+		goto err_regulator;
+	}
+
+	if (gpio_is_valid(ddata->nreset_gpio)) {
+		r = devm_gpio_request_one(&spi->dev,
+				ddata->nreset_gpio, GPIOF_OUT_INIT_LOW,
+				"lcd reset");
+		if (r < 0) {
+			dev_err(&spi->dev, "couldn't request reset GPIO\n");
+			goto err_gpio_req;
+		}
+	}
+
+	r = sysfs_create_group(&spi->dev.kobj, &tpo_td043_attr_group);
+	if (r) {
+		dev_err(&spi->dev, "failed to create sysfs files\n");
+		goto err_sysfs;
+	}
+
+	ddata->videomode = tpo_td043_timings;
+
+	dssdev = &ddata->dssdev;
+	dssdev->dev = &spi->dev;
+	dssdev->driver = &tpo_td043_ops;
+	dssdev->type = OMAP_DISPLAY_TYPE_DPI;
+	dssdev->owner = THIS_MODULE;
+	dssdev->panel.timings = ddata->videomode;
+
+	r = omapdss_register_display(dssdev);
+	if (r) {
+		dev_err(&spi->dev, "Failed to register panel\n");
+		goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	sysfs_remove_group(&spi->dev.kobj, &tpo_td043_attr_group);
+err_sysfs:
+err_gpio_req:
+err_regulator:
+	omap_dss_put_device(ddata->in);
+	return r;
+}
+
+static int tpo_td043_remove(struct spi_device *spi)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(&spi->dev);
+	struct omap_dss_device *dssdev = &ddata->dssdev;
+	struct omap_dss_device *in = ddata->in;
+
+	dev_dbg(&ddata->spi->dev, "%s\n", __func__);
+
+	omapdss_unregister_display(dssdev);
+
+	tpo_td043_disable(dssdev);
+	tpo_td043_disconnect(dssdev);
+
+	omap_dss_put_device(in);
+
+	sysfs_remove_group(&spi->dev.kobj, &tpo_td043_attr_group);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tpo_td043_spi_suspend(struct device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+
+	dev_dbg(dev, "tpo_td043_spi_suspend, tpo %p\n", ddata);
+
+	ddata->power_on_resume = ddata->powered_on;
+	tpo_td043_power_off(ddata);
+	ddata->spi_suspended = 1;
+
+	return 0;
+}
+
+static int tpo_td043_spi_resume(struct device *dev)
+{
+	struct panel_drv_data *ddata = dev_get_drvdata(dev);
+	int ret;
+
+	dev_dbg(dev, "tpo_td043_spi_resume\n");
+
+	if (ddata->power_on_resume) {
+		ret = tpo_td043_power_on(ddata);
+		if (ret)
+			return ret;
+	}
+	ddata->spi_suspended = 0;
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(tpo_td043_spi_pm,
+	tpo_td043_spi_suspend, tpo_td043_spi_resume);
+
+static const struct of_device_id tpo_td043_of_match[] = {
+	{ .compatible = "omapdss,tpo,td043mtea1", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, tpo_td043_of_match);
+
+static struct spi_driver tpo_td043_spi_driver = {
+	.driver = {
+		.name	= "panel-tpo-td043mtea1",
+		.pm	= &tpo_td043_spi_pm,
+		.of_match_table = tpo_td043_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe	= tpo_td043_probe,
+	.remove	= tpo_td043_remove,
+};
+
+module_spi_driver(tpo_td043_spi_driver);
+
+MODULE_ALIAS("spi:tpo,td043mtea1");
+MODULE_AUTHOR("Gražvydas Ignotas <notasas@gmail.com>");
+MODULE_DESCRIPTION("TPO TD043MTEA1 LCD Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/Kconfig b/drivers/video/fbdev/omap2/omapfb/dss/Kconfig
new file mode 100644
index 000000000000..27d220212870
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/Kconfig
@@ -0,0 +1,129 @@
+config FB_OMAP2_DSS_INIT
+	bool
+
+config FB_OMAP2_DSS
+        tristate
+	select VIDEOMODE_HELPERS
+	select FB_OMAP2_DSS_INIT
+	select HDMI
+
+config FB_OMAP2_DSS_DEBUG
+	bool "Debug support"
+	default n
+	help
+	  This enables printing of debug messages. Alternatively, debug messages
+	  can also be enabled by setting CONFIG_DYNAMIC_DEBUG and then setting
+	  appropriate flags in <debugfs>/dynamic_debug/control.
+
+config FB_OMAP2_DSS_DEBUGFS
+	bool "Debugfs filesystem support"
+	depends on DEBUG_FS
+	default n
+	help
+	  This enables debugfs for OMAPDSS at <debugfs>/omapdss. This enables
+	  querying about clock configuration and register configuration of dss,
+	  dispc, dsi, hdmi and rfbi.
+
+config FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	bool "Collect DSS IRQ statistics"
+	depends on FB_OMAP2_DSS_DEBUGFS
+	default n
+	help
+	  Collect DSS IRQ statistics, printable via debugfs.
+
+	  The statistics can be found from
+	  <debugfs>/omapdss/dispc_irq for DISPC interrupts, and
+	  <debugfs>/omapdss/dsi_irq for DSI interrupts.
+
+config FB_OMAP2_DSS_DPI
+	bool "DPI support"
+	default y
+	help
+	  DPI Interface. This is the Parallel Display Interface.
+
+config FB_OMAP2_DSS_RFBI
+	bool "RFBI support"
+	depends on BROKEN
+        default n
+	help
+	  MIPI DBI support (RFBI, Remote Framebuffer Interface, in Texas
+	  Instrument's terminology).
+
+	  DBI is a bus between the host processor and a peripheral,
+	  such as a display or a framebuffer chip.
+
+	  See http://www.mipi.org/ for DBI specifications.
+
+config FB_OMAP2_DSS_VENC
+	bool "VENC support"
+        default y
+	help
+	  OMAP Video Encoder support for S-Video and composite TV-out.
+
+config FB_OMAP2_DSS_HDMI_COMMON
+	bool
+
+config FB_OMAP4_DSS_HDMI
+	bool "HDMI support for OMAP4"
+        default y
+	select FB_OMAP2_DSS_HDMI_COMMON
+	help
+	  HDMI support for OMAP4 based SoCs.
+
+config FB_OMAP5_DSS_HDMI
+	bool "HDMI support for OMAP5"
+	default n
+	select FB_OMAP2_DSS_HDMI_COMMON
+	help
+	  HDMI Interface for OMAP5 and similar cores. This adds the High
+	  Definition Multimedia Interface. See http://www.hdmi.org/ for HDMI
+	  specification.
+
+config FB_OMAP2_DSS_SDI
+	bool "SDI support"
+        default n
+	help
+	  SDI (Serial Display Interface) support.
+
+	  SDI is a high speed one-way display serial bus between the host
+	  processor and a display.
+
+config FB_OMAP2_DSS_DSI
+	bool "DSI support"
+        default n
+	help
+	  MIPI DSI (Display Serial Interface) support.
+
+	  DSI is a high speed half-duplex serial interface between the host
+	  processor and a peripheral, such as a display or a framebuffer chip.
+
+	  See http://www.mipi.org/ for DSI specifications.
+
+config FB_OMAP2_DSS_MIN_FCK_PER_PCK
+	int "Minimum FCK/PCK ratio (for scaling)"
+	range 0 32
+	default 0
+	help
+	  This can be used to adjust the minimum FCK/PCK ratio.
+
+	  With this you can make sure that DISPC FCK is at least
+	  n x PCK. Video plane scaling requires higher FCK than
+	  normally.
+
+	  If this is set to 0, there's no extra constraint on the
+	  DISPC FCK. However, the FCK will at minimum be
+	  2xPCK (if active matrix) or 3xPCK (if passive matrix).
+
+	  Max FCK is 173MHz, so this doesn't work if your PCK
+	  is very high.
+
+config FB_OMAP2_DSS_SLEEP_AFTER_VENC_RESET
+	bool "Sleep 20ms after VENC reset"
+	default y
+	help
+	  There is a 20ms sleep after VENC reset which seemed to fix the
+	  reset. The reason for the bug is unclear, and it's also unclear
+	  on what platforms this happens.
+
+	  This option enables the sleep, and is enabled by default. You can
+	  disable the sleep if it doesn't cause problems on your platform.
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/Makefile b/drivers/video/fbdev/omap2/omapfb/dss/Makefile
new file mode 100644
index 000000000000..02308e24f3ef
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/Makefile
@@ -0,0 +1,18 @@
+obj-$(CONFIG_FB_OMAP2_DSS_INIT) += omapdss-boot-init.o
+obj-$(CONFIG_FB_OMAP2_DSS) += omapdss.o
+# Core DSS files
+omapdss-y := core.o dss.o dss_features.o dispc.o dispc_coefs.o display.o \
+	output.o dss-of.o pll.o video-pll.o
+# DSS compat layer files
+omapdss-y += manager.o manager-sysfs.o overlay.o overlay-sysfs.o apply.o \
+	dispc-compat.o display-sysfs.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_DPI) += dpi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_RFBI) += rfbi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_VENC) += venc.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_SDI) += sdi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_DSI) += dsi.o
+omapdss-$(CONFIG_FB_OMAP2_DSS_HDMI_COMMON) += hdmi_common.o hdmi_wp.o hdmi_pll.o \
+	hdmi_phy.o
+omapdss-$(CONFIG_FB_OMAP4_DSS_HDMI) += hdmi4.o hdmi4_core.o
+omapdss-$(CONFIG_FB_OMAP5_DSS_HDMI) += hdmi5.o hdmi5_core.o
+ccflags-$(CONFIG_FB_OMAP2_DSS_DEBUG) += -DDEBUG
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/apply.c b/drivers/video/fbdev/omap2/omapfb/dss/apply.c
new file mode 100644
index 000000000000..663ccc3bf4e5
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/apply.c
@@ -0,0 +1,1702 @@
+/*
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 DSS_SUBSYS_NAME "APPLY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc-compat.h"
+
+/*
+ * We have 4 levels of cache for the dispc settings. First two are in SW and
+ * the latter two in HW.
+ *
+ *       set_info()
+ *          v
+ * +--------------------+
+ * |     user_info      |
+ * +--------------------+
+ *          v
+ *        apply()
+ *          v
+ * +--------------------+
+ * |       info         |
+ * +--------------------+
+ *          v
+ *      write_regs()
+ *          v
+ * +--------------------+
+ * |  shadow registers  |
+ * +--------------------+
+ *          v
+ * VFP or lcd/digit_enable
+ *          v
+ * +--------------------+
+ * |      registers     |
+ * +--------------------+
+ */
+
+struct ovl_priv_data {
+
+	bool user_info_dirty;
+	struct omap_overlay_info user_info;
+
+	bool info_dirty;
+	struct omap_overlay_info info;
+
+	bool shadow_info_dirty;
+
+	bool extra_info_dirty;
+	bool shadow_extra_info_dirty;
+
+	bool enabled;
+	u32 fifo_low, fifo_high;
+
+	/*
+	 * True if overlay is to be enabled. Used to check and calculate configs
+	 * for the overlay before it is enabled in the HW.
+	 */
+	bool enabling;
+};
+
+struct mgr_priv_data {
+
+	bool user_info_dirty;
+	struct omap_overlay_manager_info user_info;
+
+	bool info_dirty;
+	struct omap_overlay_manager_info info;
+
+	bool shadow_info_dirty;
+
+	/* If true, GO bit is up and shadow registers cannot be written.
+	 * Never true for manual update displays */
+	bool busy;
+
+	/* If true, dispc output is enabled */
+	bool updating;
+
+	/* If true, a display is enabled using this manager */
+	bool enabled;
+
+	bool extra_info_dirty;
+	bool shadow_extra_info_dirty;
+
+	struct omap_video_timings timings;
+	struct dss_lcd_mgr_config lcd_config;
+
+	void (*framedone_handler)(void *);
+	void *framedone_handler_data;
+};
+
+static struct {
+	struct ovl_priv_data ovl_priv_data_array[MAX_DSS_OVERLAYS];
+	struct mgr_priv_data mgr_priv_data_array[MAX_DSS_MANAGERS];
+
+	bool irq_enabled;
+} dss_data;
+
+/* protects dss_data */
+static spinlock_t data_lock;
+/* lock for blocking functions */
+static DEFINE_MUTEX(apply_lock);
+static DECLARE_COMPLETION(extra_updated_completion);
+
+static void dss_register_vsync_isr(void);
+
+static struct ovl_priv_data *get_ovl_priv(struct omap_overlay *ovl)
+{
+	return &dss_data.ovl_priv_data_array[ovl->id];
+}
+
+static struct mgr_priv_data *get_mgr_priv(struct omap_overlay_manager *mgr)
+{
+	return &dss_data.mgr_priv_data_array[mgr->id];
+}
+
+static void apply_init_priv(void)
+{
+	const int num_ovls = dss_feat_get_num_ovls();
+	struct mgr_priv_data *mp;
+	int i;
+
+	spin_lock_init(&data_lock);
+
+	for (i = 0; i < num_ovls; ++i) {
+		struct ovl_priv_data *op;
+
+		op = &dss_data.ovl_priv_data_array[i];
+
+		op->info.color_mode = OMAP_DSS_COLOR_RGB16;
+		op->info.rotation_type = OMAP_DSS_ROT_DMA;
+
+		op->info.global_alpha = 255;
+
+		switch (i) {
+		case 0:
+			op->info.zorder = 0;
+			break;
+		case 1:
+			op->info.zorder =
+				dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 3 : 0;
+			break;
+		case 2:
+			op->info.zorder =
+				dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 2 : 0;
+			break;
+		case 3:
+			op->info.zorder =
+				dss_has_feature(FEAT_ALPHA_FREE_ZORDER) ? 1 : 0;
+			break;
+		}
+
+		op->user_info = op->info;
+	}
+
+	/*
+	 * Initialize some of the lcd_config fields for TV manager, this lets
+	 * us prevent checking if the manager is LCD or TV at some places
+	 */
+	mp = &dss_data.mgr_priv_data_array[OMAP_DSS_CHANNEL_DIGIT];
+
+	mp->lcd_config.video_port_width = 24;
+	mp->lcd_config.clock_info.lck_div = 1;
+	mp->lcd_config.clock_info.pck_div = 1;
+}
+
+/*
+ * A LCD manager's stallmode decides whether it is in manual or auto update. TV
+ * manager is always auto update, stallmode field for TV manager is false by
+ * default
+ */
+static bool ovl_manual_update(struct omap_overlay *ovl)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(ovl->manager);
+
+	return mp->lcd_config.stallmode;
+}
+
+static bool mgr_manual_update(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	return mp->lcd_config.stallmode;
+}
+
+static int dss_check_settings_low(struct omap_overlay_manager *mgr,
+		bool applying)
+{
+	struct omap_overlay_info *oi;
+	struct omap_overlay_manager_info *mi;
+	struct omap_overlay *ovl;
+	struct omap_overlay_info *ois[MAX_DSS_OVERLAYS];
+	struct ovl_priv_data *op;
+	struct mgr_priv_data *mp;
+
+	mp = get_mgr_priv(mgr);
+
+	if (!mp->enabled)
+		return 0;
+
+	if (applying && mp->user_info_dirty)
+		mi = &mp->user_info;
+	else
+		mi = &mp->info;
+
+	/* collect the infos to be tested into the array */
+	list_for_each_entry(ovl, &mgr->overlays, list) {
+		op = get_ovl_priv(ovl);
+
+		if (!op->enabled && !op->enabling)
+			oi = NULL;
+		else if (applying && op->user_info_dirty)
+			oi = &op->user_info;
+		else
+			oi = &op->info;
+
+		ois[ovl->id] = oi;
+	}
+
+	return dss_mgr_check(mgr, mi, &mp->timings, &mp->lcd_config, ois);
+}
+
+/*
+ * check manager and overlay settings using overlay_info from data->info
+ */
+static int dss_check_settings(struct omap_overlay_manager *mgr)
+{
+	return dss_check_settings_low(mgr, false);
+}
+
+/*
+ * check manager and overlay settings using overlay_info from ovl->info if
+ * dirty and from data->info otherwise
+ */
+static int dss_check_settings_apply(struct omap_overlay_manager *mgr)
+{
+	return dss_check_settings_low(mgr, true);
+}
+
+static bool need_isr(void)
+{
+	const int num_mgrs = dss_feat_get_num_mgrs();
+	int i;
+
+	for (i = 0; i < num_mgrs; ++i) {
+		struct omap_overlay_manager *mgr;
+		struct mgr_priv_data *mp;
+		struct omap_overlay *ovl;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mp->enabled)
+			continue;
+
+		if (mgr_manual_update(mgr)) {
+			/* to catch FRAMEDONE */
+			if (mp->updating)
+				return true;
+		} else {
+			/* to catch GO bit going down */
+			if (mp->busy)
+				return true;
+
+			/* to write new values to registers */
+			if (mp->info_dirty)
+				return true;
+
+			/* to set GO bit */
+			if (mp->shadow_info_dirty)
+				return true;
+
+			/*
+			 * NOTE: we don't check extra_info flags for disabled
+			 * managers, once the manager is enabled, the extra_info
+			 * related manager changes will be taken in by HW.
+			 */
+
+			/* to write new values to registers */
+			if (mp->extra_info_dirty)
+				return true;
+
+			/* to set GO bit */
+			if (mp->shadow_extra_info_dirty)
+				return true;
+
+			list_for_each_entry(ovl, &mgr->overlays, list) {
+				struct ovl_priv_data *op;
+
+				op = get_ovl_priv(ovl);
+
+				/*
+				 * NOTE: we check extra_info flags even for
+				 * disabled overlays, as extra_infos need to be
+				 * always written.
+				 */
+
+				/* to write new values to registers */
+				if (op->extra_info_dirty)
+					return true;
+
+				/* to set GO bit */
+				if (op->shadow_extra_info_dirty)
+					return true;
+
+				if (!op->enabled)
+					continue;
+
+				/* to write new values to registers */
+				if (op->info_dirty)
+					return true;
+
+				/* to set GO bit */
+				if (op->shadow_info_dirty)
+					return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+static bool need_go(struct omap_overlay_manager *mgr)
+{
+	struct omap_overlay *ovl;
+	struct mgr_priv_data *mp;
+	struct ovl_priv_data *op;
+
+	mp = get_mgr_priv(mgr);
+
+	if (mp->shadow_info_dirty || mp->shadow_extra_info_dirty)
+		return true;
+
+	list_for_each_entry(ovl, &mgr->overlays, list) {
+		op = get_ovl_priv(ovl);
+		if (op->shadow_info_dirty || op->shadow_extra_info_dirty)
+			return true;
+	}
+
+	return false;
+}
+
+/* returns true if an extra_info field is currently being updated */
+static bool extra_info_update_ongoing(void)
+{
+	const int num_mgrs = dss_feat_get_num_mgrs();
+	int i;
+
+	for (i = 0; i < num_mgrs; ++i) {
+		struct omap_overlay_manager *mgr;
+		struct omap_overlay *ovl;
+		struct mgr_priv_data *mp;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mp->enabled)
+			continue;
+
+		if (!mp->updating)
+			continue;
+
+		if (mp->extra_info_dirty || mp->shadow_extra_info_dirty)
+			return true;
+
+		list_for_each_entry(ovl, &mgr->overlays, list) {
+			struct ovl_priv_data *op = get_ovl_priv(ovl);
+
+			if (op->extra_info_dirty || op->shadow_extra_info_dirty)
+				return true;
+		}
+	}
+
+	return false;
+}
+
+/* wait until no extra_info updates are pending */
+static void wait_pending_extra_info_updates(void)
+{
+	bool updating;
+	unsigned long flags;
+	unsigned long t;
+	int r;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	updating = extra_info_update_ongoing();
+
+	if (!updating) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		return;
+	}
+
+	init_completion(&extra_updated_completion);
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	t = msecs_to_jiffies(500);
+	r = wait_for_completion_timeout(&extra_updated_completion, t);
+	if (r == 0)
+		DSSWARN("timeout in wait_pending_extra_info_updates\n");
+}
+
+static struct omap_dss_device *dss_mgr_get_device(struct omap_overlay_manager *mgr)
+{
+	struct omap_dss_device *dssdev;
+
+	dssdev = mgr->output;
+	if (dssdev == NULL)
+		return NULL;
+
+	while (dssdev->dst)
+		dssdev = dssdev->dst;
+
+	if (dssdev->driver)
+		return dssdev;
+	else
+		return NULL;
+}
+
+static struct omap_dss_device *dss_ovl_get_device(struct omap_overlay *ovl)
+{
+	return ovl->manager ? dss_mgr_get_device(ovl->manager) : NULL;
+}
+
+static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
+{
+	unsigned long timeout = msecs_to_jiffies(500);
+	u32 irq;
+	int r;
+
+	if (mgr->output == NULL)
+		return -ENODEV;
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	switch (mgr->output->id) {
+	case OMAP_DSS_OUTPUT_VENC:
+		irq = DISPC_IRQ_EVSYNC_ODD;
+		break;
+	case OMAP_DSS_OUTPUT_HDMI:
+		irq = DISPC_IRQ_EVSYNC_EVEN;
+		break;
+	default:
+		irq = dispc_mgr_get_vsync_irq(mgr->id);
+		break;
+	}
+
+	r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+
+	dispc_runtime_put();
+
+	return r;
+}
+
+static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
+{
+	unsigned long timeout = msecs_to_jiffies(500);
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	u32 irq;
+	unsigned long flags;
+	int r;
+	int i;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mgr_manual_update(mgr)) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		return 0;
+	}
+
+	if (!mp->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		return 0;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	irq = dispc_mgr_get_vsync_irq(mgr->id);
+
+	i = 0;
+	while (1) {
+		bool shadow_dirty, dirty;
+
+		spin_lock_irqsave(&data_lock, flags);
+		dirty = mp->info_dirty;
+		shadow_dirty = mp->shadow_info_dirty;
+		spin_unlock_irqrestore(&data_lock, flags);
+
+		if (!dirty && !shadow_dirty) {
+			r = 0;
+			break;
+		}
+
+		/* 4 iterations is the worst case:
+		 * 1 - initial iteration, dirty = true (between VFP and VSYNC)
+		 * 2 - first VSYNC, dirty = true
+		 * 3 - dirty = false, shadow_dirty = true
+		 * 4 - shadow_dirty = false */
+		if (i++ == 3) {
+			DSSERR("mgr(%d)->wait_for_go() not finishing\n",
+					mgr->id);
+			r = 0;
+			break;
+		}
+
+		r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+		if (r == -ERESTARTSYS)
+			break;
+
+		if (r) {
+			DSSERR("mgr(%d)->wait_for_go() timeout\n", mgr->id);
+			break;
+		}
+	}
+
+	dispc_runtime_put();
+
+	return r;
+}
+
+static int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
+{
+	unsigned long timeout = msecs_to_jiffies(500);
+	struct ovl_priv_data *op;
+	struct mgr_priv_data *mp;
+	u32 irq;
+	unsigned long flags;
+	int r;
+	int i;
+
+	if (!ovl->manager)
+		return 0;
+
+	mp = get_mgr_priv(ovl->manager);
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (ovl_manual_update(ovl)) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		return 0;
+	}
+
+	if (!mp->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		return 0;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	irq = dispc_mgr_get_vsync_irq(ovl->manager->id);
+
+	op = get_ovl_priv(ovl);
+	i = 0;
+	while (1) {
+		bool shadow_dirty, dirty;
+
+		spin_lock_irqsave(&data_lock, flags);
+		dirty = op->info_dirty;
+		shadow_dirty = op->shadow_info_dirty;
+		spin_unlock_irqrestore(&data_lock, flags);
+
+		if (!dirty && !shadow_dirty) {
+			r = 0;
+			break;
+		}
+
+		/* 4 iterations is the worst case:
+		 * 1 - initial iteration, dirty = true (between VFP and VSYNC)
+		 * 2 - first VSYNC, dirty = true
+		 * 3 - dirty = false, shadow_dirty = true
+		 * 4 - shadow_dirty = false */
+		if (i++ == 3) {
+			DSSERR("ovl(%d)->wait_for_go() not finishing\n",
+					ovl->id);
+			r = 0;
+			break;
+		}
+
+		r = omap_dispc_wait_for_irq_interruptible_timeout(irq, timeout);
+		if (r == -ERESTARTSYS)
+			break;
+
+		if (r) {
+			DSSERR("ovl(%d)->wait_for_go() timeout\n", ovl->id);
+			break;
+		}
+	}
+
+	dispc_runtime_put();
+
+	return r;
+}
+
+static void dss_ovl_write_regs(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	struct omap_overlay_info *oi;
+	bool replication;
+	struct mgr_priv_data *mp;
+	int r;
+
+	DSSDBG("writing ovl %d regs\n", ovl->id);
+
+	if (!op->enabled || !op->info_dirty)
+		return;
+
+	oi = &op->info;
+
+	mp = get_mgr_priv(ovl->manager);
+
+	replication = dss_ovl_use_replication(mp->lcd_config, oi->color_mode);
+
+	r = dispc_ovl_setup(ovl->id, oi, replication, &mp->timings, false);
+	if (r) {
+		/*
+		 * We can't do much here, as this function can be called from
+		 * vsync interrupt.
+		 */
+		DSSERR("dispc_ovl_setup failed for ovl %d\n", ovl->id);
+
+		/* This will leave fifo configurations in a nonoptimal state */
+		op->enabled = false;
+		dispc_ovl_enable(ovl->id, false);
+		return;
+	}
+
+	op->info_dirty = false;
+	if (mp->updating)
+		op->shadow_info_dirty = true;
+}
+
+static void dss_ovl_write_regs_extra(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	struct mgr_priv_data *mp;
+
+	DSSDBG("writing ovl %d regs extra\n", ovl->id);
+
+	if (!op->extra_info_dirty)
+		return;
+
+	/* note: write also when op->enabled == false, so that the ovl gets
+	 * disabled */
+
+	dispc_ovl_enable(ovl->id, op->enabled);
+	dispc_ovl_set_fifo_threshold(ovl->id, op->fifo_low, op->fifo_high);
+
+	mp = get_mgr_priv(ovl->manager);
+
+	op->extra_info_dirty = false;
+	if (mp->updating)
+		op->shadow_extra_info_dirty = true;
+}
+
+static void dss_mgr_write_regs(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	struct omap_overlay *ovl;
+
+	DSSDBG("writing mgr %d regs\n", mgr->id);
+
+	if (!mp->enabled)
+		return;
+
+	WARN_ON(mp->busy);
+
+	/* Commit overlay settings */
+	list_for_each_entry(ovl, &mgr->overlays, list) {
+		dss_ovl_write_regs(ovl);
+		dss_ovl_write_regs_extra(ovl);
+	}
+
+	if (mp->info_dirty) {
+		dispc_mgr_setup(mgr->id, &mp->info);
+
+		mp->info_dirty = false;
+		if (mp->updating)
+			mp->shadow_info_dirty = true;
+	}
+}
+
+static void dss_mgr_write_regs_extra(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	DSSDBG("writing mgr %d regs extra\n", mgr->id);
+
+	if (!mp->extra_info_dirty)
+		return;
+
+	dispc_mgr_set_timings(mgr->id, &mp->timings);
+
+	/* lcd_config parameters */
+	if (dss_mgr_is_lcd(mgr->id))
+		dispc_mgr_set_lcd_config(mgr->id, &mp->lcd_config);
+
+	mp->extra_info_dirty = false;
+	if (mp->updating)
+		mp->shadow_extra_info_dirty = true;
+}
+
+static void dss_write_regs(void)
+{
+	const int num_mgrs = omap_dss_get_num_overlay_managers();
+	int i;
+
+	for (i = 0; i < num_mgrs; ++i) {
+		struct omap_overlay_manager *mgr;
+		struct mgr_priv_data *mp;
+		int r;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mp->enabled || mgr_manual_update(mgr) || mp->busy)
+			continue;
+
+		r = dss_check_settings(mgr);
+		if (r) {
+			DSSERR("cannot write registers for manager %s: "
+					"illegal configuration\n", mgr->name);
+			continue;
+		}
+
+		dss_mgr_write_regs(mgr);
+		dss_mgr_write_regs_extra(mgr);
+	}
+}
+
+static void dss_set_go_bits(void)
+{
+	const int num_mgrs = omap_dss_get_num_overlay_managers();
+	int i;
+
+	for (i = 0; i < num_mgrs; ++i) {
+		struct omap_overlay_manager *mgr;
+		struct mgr_priv_data *mp;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mp->enabled || mgr_manual_update(mgr) || mp->busy)
+			continue;
+
+		if (!need_go(mgr))
+			continue;
+
+		mp->busy = true;
+
+		if (!dss_data.irq_enabled && need_isr())
+			dss_register_vsync_isr();
+
+		dispc_mgr_go(mgr->id);
+	}
+
+}
+
+static void mgr_clear_shadow_dirty(struct omap_overlay_manager *mgr)
+{
+	struct omap_overlay *ovl;
+	struct mgr_priv_data *mp;
+	struct ovl_priv_data *op;
+
+	mp = get_mgr_priv(mgr);
+	mp->shadow_info_dirty = false;
+	mp->shadow_extra_info_dirty = false;
+
+	list_for_each_entry(ovl, &mgr->overlays, list) {
+		op = get_ovl_priv(ovl);
+		op->shadow_info_dirty = false;
+		op->shadow_extra_info_dirty = false;
+	}
+}
+
+static int dss_mgr_connect_compat(struct omap_overlay_manager *mgr,
+		struct omap_dss_device *dst)
+{
+	return mgr->set_output(mgr, dst);
+}
+
+static void dss_mgr_disconnect_compat(struct omap_overlay_manager *mgr,
+		struct omap_dss_device *dst)
+{
+	mgr->unset_output(mgr);
+}
+
+static void dss_mgr_start_update_compat(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	WARN_ON(mp->updating);
+
+	r = dss_check_settings(mgr);
+	if (r) {
+		DSSERR("cannot start manual update: illegal configuration\n");
+		spin_unlock_irqrestore(&data_lock, flags);
+		return;
+	}
+
+	dss_mgr_write_regs(mgr);
+	dss_mgr_write_regs_extra(mgr);
+
+	mp->updating = true;
+
+	if (!dss_data.irq_enabled && need_isr())
+		dss_register_vsync_isr();
+
+	dispc_mgr_enable_sync(mgr->id);
+
+	spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static void dss_apply_irq_handler(void *data, u32 mask);
+
+static void dss_register_vsync_isr(void)
+{
+	const int num_mgrs = dss_feat_get_num_mgrs();
+	u32 mask;
+	int r, i;
+
+	mask = 0;
+	for (i = 0; i < num_mgrs; ++i)
+		mask |= dispc_mgr_get_vsync_irq(i);
+
+	for (i = 0; i < num_mgrs; ++i)
+		mask |= dispc_mgr_get_framedone_irq(i);
+
+	r = omap_dispc_register_isr(dss_apply_irq_handler, NULL, mask);
+	WARN_ON(r);
+
+	dss_data.irq_enabled = true;
+}
+
+static void dss_unregister_vsync_isr(void)
+{
+	const int num_mgrs = dss_feat_get_num_mgrs();
+	u32 mask;
+	int r, i;
+
+	mask = 0;
+	for (i = 0; i < num_mgrs; ++i)
+		mask |= dispc_mgr_get_vsync_irq(i);
+
+	for (i = 0; i < num_mgrs; ++i)
+		mask |= dispc_mgr_get_framedone_irq(i);
+
+	r = omap_dispc_unregister_isr(dss_apply_irq_handler, NULL, mask);
+	WARN_ON(r);
+
+	dss_data.irq_enabled = false;
+}
+
+static void dss_apply_irq_handler(void *data, u32 mask)
+{
+	const int num_mgrs = dss_feat_get_num_mgrs();
+	int i;
+	bool extra_updating;
+
+	spin_lock(&data_lock);
+
+	/* clear busy, updating flags, shadow_dirty flags */
+	for (i = 0; i < num_mgrs; i++) {
+		struct omap_overlay_manager *mgr;
+		struct mgr_priv_data *mp;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mp->enabled)
+			continue;
+
+		mp->updating = dispc_mgr_is_enabled(i);
+
+		if (!mgr_manual_update(mgr)) {
+			bool was_busy = mp->busy;
+			mp->busy = dispc_mgr_go_busy(i);
+
+			if (was_busy && !mp->busy)
+				mgr_clear_shadow_dirty(mgr);
+		}
+	}
+
+	dss_write_regs();
+	dss_set_go_bits();
+
+	extra_updating = extra_info_update_ongoing();
+	if (!extra_updating)
+		complete_all(&extra_updated_completion);
+
+	/* call framedone handlers for manual update displays */
+	for (i = 0; i < num_mgrs; i++) {
+		struct omap_overlay_manager *mgr;
+		struct mgr_priv_data *mp;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		mp = get_mgr_priv(mgr);
+
+		if (!mgr_manual_update(mgr) || !mp->framedone_handler)
+			continue;
+
+		if (mask & dispc_mgr_get_framedone_irq(i))
+			mp->framedone_handler(mp->framedone_handler_data);
+	}
+
+	if (!need_isr())
+		dss_unregister_vsync_isr();
+
+	spin_unlock(&data_lock);
+}
+
+static void omap_dss_mgr_apply_ovl(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op;
+
+	op = get_ovl_priv(ovl);
+
+	if (!op->user_info_dirty)
+		return;
+
+	op->user_info_dirty = false;
+	op->info_dirty = true;
+	op->info = op->user_info;
+}
+
+static void omap_dss_mgr_apply_mgr(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp;
+
+	mp = get_mgr_priv(mgr);
+
+	if (!mp->user_info_dirty)
+		return;
+
+	mp->user_info_dirty = false;
+	mp->info_dirty = true;
+	mp->info = mp->user_info;
+}
+
+static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
+{
+	unsigned long flags;
+	struct omap_overlay *ovl;
+	int r;
+
+	DSSDBG("omap_dss_mgr_apply(%s)\n", mgr->name);
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	r = dss_check_settings_apply(mgr);
+	if (r) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		DSSERR("failed to apply settings: illegal configuration.\n");
+		return r;
+	}
+
+	/* Configure overlays */
+	list_for_each_entry(ovl, &mgr->overlays, list)
+		omap_dss_mgr_apply_ovl(ovl);
+
+	/* Configure manager */
+	omap_dss_mgr_apply_mgr(mgr);
+
+	dss_write_regs();
+	dss_set_go_bits();
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	return 0;
+}
+
+static void dss_apply_ovl_enable(struct omap_overlay *ovl, bool enable)
+{
+	struct ovl_priv_data *op;
+
+	op = get_ovl_priv(ovl);
+
+	if (op->enabled == enable)
+		return;
+
+	op->enabled = enable;
+	op->extra_info_dirty = true;
+}
+
+static void dss_apply_ovl_fifo_thresholds(struct omap_overlay *ovl,
+		u32 fifo_low, u32 fifo_high)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+
+	if (op->fifo_low == fifo_low && op->fifo_high == fifo_high)
+		return;
+
+	op->fifo_low = fifo_low;
+	op->fifo_high = fifo_high;
+	op->extra_info_dirty = true;
+}
+
+static void dss_ovl_setup_fifo(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	u32 fifo_low, fifo_high;
+	bool use_fifo_merge = false;
+
+	if (!op->enabled && !op->enabling)
+		return;
+
+	dispc_ovl_compute_fifo_thresholds(ovl->id, &fifo_low, &fifo_high,
+			use_fifo_merge, ovl_manual_update(ovl));
+
+	dss_apply_ovl_fifo_thresholds(ovl, fifo_low, fifo_high);
+}
+
+static void dss_mgr_setup_fifos(struct omap_overlay_manager *mgr)
+{
+	struct omap_overlay *ovl;
+	struct mgr_priv_data *mp;
+
+	mp = get_mgr_priv(mgr);
+
+	if (!mp->enabled)
+		return;
+
+	list_for_each_entry(ovl, &mgr->overlays, list)
+		dss_ovl_setup_fifo(ovl);
+}
+
+static void dss_setup_fifos(void)
+{
+	const int num_mgrs = omap_dss_get_num_overlay_managers();
+	struct omap_overlay_manager *mgr;
+	int i;
+
+	for (i = 0; i < num_mgrs; ++i) {
+		mgr = omap_dss_get_overlay_manager(i);
+		dss_mgr_setup_fifos(mgr);
+	}
+}
+
+static int dss_mgr_enable_compat(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+	int r;
+
+	mutex_lock(&apply_lock);
+
+	if (mp->enabled)
+		goto out;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	mp->enabled = true;
+
+	r = dss_check_settings(mgr);
+	if (r) {
+		DSSERR("failed to enable manager %d: check_settings failed\n",
+				mgr->id);
+		goto err;
+	}
+
+	dss_setup_fifos();
+
+	dss_write_regs();
+	dss_set_go_bits();
+
+	if (!mgr_manual_update(mgr))
+		mp->updating = true;
+
+	if (!dss_data.irq_enabled && need_isr())
+		dss_register_vsync_isr();
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	if (!mgr_manual_update(mgr))
+		dispc_mgr_enable_sync(mgr->id);
+
+out:
+	mutex_unlock(&apply_lock);
+
+	return 0;
+
+err:
+	mp->enabled = false;
+	spin_unlock_irqrestore(&data_lock, flags);
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static void dss_mgr_disable_compat(struct omap_overlay_manager *mgr)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+
+	mutex_lock(&apply_lock);
+
+	if (!mp->enabled)
+		goto out;
+
+	wait_pending_extra_info_updates();
+
+	if (!mgr_manual_update(mgr))
+		dispc_mgr_disable_sync(mgr->id);
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	mp->updating = false;
+	mp->enabled = false;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+out:
+	mutex_unlock(&apply_lock);
+}
+
+static int dss_mgr_set_info(struct omap_overlay_manager *mgr,
+		struct omap_overlay_manager_info *info)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+	int r;
+
+	r = dss_mgr_simple_check(mgr, info);
+	if (r)
+		return r;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	mp->user_info = *info;
+	mp->user_info_dirty = true;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	return 0;
+}
+
+static void dss_mgr_get_info(struct omap_overlay_manager *mgr,
+		struct omap_overlay_manager_info *info)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	*info = mp->user_info;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_mgr_set_output(struct omap_overlay_manager *mgr,
+		struct omap_dss_device *output)
+{
+	int r;
+
+	mutex_lock(&apply_lock);
+
+	if (mgr->output) {
+		DSSERR("manager %s is already connected to an output\n",
+			mgr->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	if ((mgr->supported_outputs & output->id) == 0) {
+		DSSERR("output does not support manager %s\n",
+			mgr->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	output->manager = mgr;
+	mgr->output = output;
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+err:
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static int dss_mgr_unset_output(struct omap_overlay_manager *mgr)
+{
+	int r;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+	unsigned long flags;
+
+	mutex_lock(&apply_lock);
+
+	if (!mgr->output) {
+		DSSERR("failed to unset output, output not set\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mp->enabled) {
+		DSSERR("output can't be unset when manager is enabled\n");
+		r = -EINVAL;
+		goto err1;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	mgr->output->manager = NULL;
+	mgr->output = NULL;
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+err1:
+	spin_unlock_irqrestore(&data_lock, flags);
+err:
+	mutex_unlock(&apply_lock);
+
+	return r;
+}
+
+static void dss_apply_mgr_timings(struct omap_overlay_manager *mgr,
+		const struct omap_video_timings *timings)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	mp->timings = *timings;
+	mp->extra_info_dirty = true;
+}
+
+static void dss_mgr_set_timings_compat(struct omap_overlay_manager *mgr,
+		const struct omap_video_timings *timings)
+{
+	unsigned long flags;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mp->updating) {
+		DSSERR("cannot set timings for %s: manager needs to be disabled\n",
+			mgr->name);
+		goto out;
+	}
+
+	dss_apply_mgr_timings(mgr, timings);
+out:
+	spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static void dss_apply_mgr_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	mp->lcd_config = *config;
+	mp->extra_info_dirty = true;
+}
+
+static void dss_mgr_set_lcd_config_compat(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	unsigned long flags;
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (mp->enabled) {
+		DSSERR("cannot apply lcd config for %s: manager needs to be disabled\n",
+			mgr->name);
+		goto out;
+	}
+
+	dss_apply_mgr_lcd_config(mgr, config);
+out:
+	spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_ovl_set_info(struct omap_overlay *ovl,
+		struct omap_overlay_info *info)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	int r;
+
+	r = dss_ovl_simple_check(ovl, info);
+	if (r)
+		return r;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	op->user_info = *info;
+	op->user_info_dirty = true;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	return 0;
+}
+
+static void dss_ovl_get_info(struct omap_overlay *ovl,
+		struct omap_overlay_info *info)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	*info = op->user_info;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+}
+
+static int dss_ovl_set_manager(struct omap_overlay *ovl,
+		struct omap_overlay_manager *mgr)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	int r;
+
+	if (!mgr)
+		return -EINVAL;
+
+	mutex_lock(&apply_lock);
+
+	if (ovl->manager) {
+		DSSERR("overlay '%s' already has a manager '%s'\n",
+				ovl->name, ovl->manager->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (op->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		DSSERR("overlay has to be disabled to change the manager\n");
+		r = -EINVAL;
+		goto err1;
+	}
+
+	dispc_ovl_set_channel_out(ovl->id, mgr->id);
+
+	ovl->manager = mgr;
+	list_add_tail(&ovl->list, &mgr->overlays);
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	dispc_runtime_put();
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+
+err1:
+	dispc_runtime_put();
+err:
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static int dss_ovl_unset_manager(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	int r;
+
+	mutex_lock(&apply_lock);
+
+	if (!ovl->manager) {
+		DSSERR("failed to detach overlay: manager not set\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (op->enabled) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		DSSERR("overlay has to be disabled to unset the manager\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	/* wait for pending extra_info updates to ensure the ovl is disabled */
+	wait_pending_extra_info_updates();
+
+	/*
+	 * For a manual update display, there is no guarantee that the overlay
+	 * is really disabled in HW, we may need an extra update from this
+	 * manager before the configurations can go in. Return an error if the
+	 * overlay needed an update from the manager.
+	 *
+	 * TODO: Instead of returning an error, try to do a dummy manager update
+	 * here to disable the overlay in hardware. Use the *GATED fields in
+	 * the DISPC_CONFIG registers to do a dummy update.
+	 */
+	spin_lock_irqsave(&data_lock, flags);
+
+	if (ovl_manual_update(ovl) && op->extra_info_dirty) {
+		spin_unlock_irqrestore(&data_lock, flags);
+		DSSERR("need an update to change the manager\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	ovl->manager = NULL;
+	list_del(&ovl->list);
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+err:
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static bool dss_ovl_is_enabled(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	bool e;
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	e = op->enabled;
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	return e;
+}
+
+static int dss_ovl_enable(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	int r;
+
+	mutex_lock(&apply_lock);
+
+	if (op->enabled) {
+		r = 0;
+		goto err1;
+	}
+
+	if (ovl->manager == NULL || ovl->manager->output == NULL) {
+		r = -EINVAL;
+		goto err1;
+	}
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	op->enabling = true;
+
+	r = dss_check_settings(ovl->manager);
+	if (r) {
+		DSSERR("failed to enable overlay %d: check_settings failed\n",
+				ovl->id);
+		goto err2;
+	}
+
+	dss_setup_fifos();
+
+	op->enabling = false;
+	dss_apply_ovl_enable(ovl, true);
+
+	dss_write_regs();
+	dss_set_go_bits();
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+err2:
+	op->enabling = false;
+	spin_unlock_irqrestore(&data_lock, flags);
+err1:
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static int dss_ovl_disable(struct omap_overlay *ovl)
+{
+	struct ovl_priv_data *op = get_ovl_priv(ovl);
+	unsigned long flags;
+	int r;
+
+	mutex_lock(&apply_lock);
+
+	if (!op->enabled) {
+		r = 0;
+		goto err;
+	}
+
+	if (ovl->manager == NULL || ovl->manager->output == NULL) {
+		r = -EINVAL;
+		goto err;
+	}
+
+	spin_lock_irqsave(&data_lock, flags);
+
+	dss_apply_ovl_enable(ovl, false);
+	dss_write_regs();
+	dss_set_go_bits();
+
+	spin_unlock_irqrestore(&data_lock, flags);
+
+	mutex_unlock(&apply_lock);
+
+	return 0;
+
+err:
+	mutex_unlock(&apply_lock);
+	return r;
+}
+
+static int dss_mgr_register_framedone_handler_compat(struct omap_overlay_manager *mgr,
+		void (*handler)(void *), void *data)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	if (mp->framedone_handler)
+		return -EBUSY;
+
+	mp->framedone_handler = handler;
+	mp->framedone_handler_data = data;
+
+	return 0;
+}
+
+static void dss_mgr_unregister_framedone_handler_compat(struct omap_overlay_manager *mgr,
+		void (*handler)(void *), void *data)
+{
+	struct mgr_priv_data *mp = get_mgr_priv(mgr);
+
+	WARN_ON(mp->framedone_handler != handler ||
+			mp->framedone_handler_data != data);
+
+	mp->framedone_handler = NULL;
+	mp->framedone_handler_data = NULL;
+}
+
+static const struct dss_mgr_ops apply_mgr_ops = {
+	.connect = dss_mgr_connect_compat,
+	.disconnect = dss_mgr_disconnect_compat,
+	.start_update = dss_mgr_start_update_compat,
+	.enable = dss_mgr_enable_compat,
+	.disable = dss_mgr_disable_compat,
+	.set_timings = dss_mgr_set_timings_compat,
+	.set_lcd_config = dss_mgr_set_lcd_config_compat,
+	.register_framedone_handler = dss_mgr_register_framedone_handler_compat,
+	.unregister_framedone_handler = dss_mgr_unregister_framedone_handler_compat,
+};
+
+static int compat_refcnt;
+static DEFINE_MUTEX(compat_init_lock);
+
+int omapdss_compat_init(void)
+{
+	struct platform_device *pdev = dss_get_core_pdev();
+	int i, r;
+
+	mutex_lock(&compat_init_lock);
+
+	if (compat_refcnt++ > 0)
+		goto out;
+
+	apply_init_priv();
+
+	dss_init_overlay_managers_sysfs(pdev);
+	dss_init_overlays(pdev);
+
+	for (i = 0; i < omap_dss_get_num_overlay_managers(); i++) {
+		struct omap_overlay_manager *mgr;
+
+		mgr = omap_dss_get_overlay_manager(i);
+
+		mgr->set_output = &dss_mgr_set_output;
+		mgr->unset_output = &dss_mgr_unset_output;
+		mgr->apply = &omap_dss_mgr_apply;
+		mgr->set_manager_info = &dss_mgr_set_info;
+		mgr->get_manager_info = &dss_mgr_get_info;
+		mgr->wait_for_go = &dss_mgr_wait_for_go;
+		mgr->wait_for_vsync = &dss_mgr_wait_for_vsync;
+		mgr->get_device = &dss_mgr_get_device;
+	}
+
+	for (i = 0; i < omap_dss_get_num_overlays(); i++) {
+		struct omap_overlay *ovl = omap_dss_get_overlay(i);
+
+		ovl->is_enabled = &dss_ovl_is_enabled;
+		ovl->enable = &dss_ovl_enable;
+		ovl->disable = &dss_ovl_disable;
+		ovl->set_manager = &dss_ovl_set_manager;
+		ovl->unset_manager = &dss_ovl_unset_manager;
+		ovl->set_overlay_info = &dss_ovl_set_info;
+		ovl->get_overlay_info = &dss_ovl_get_info;
+		ovl->wait_for_go = &dss_mgr_wait_for_go_ovl;
+		ovl->get_device = &dss_ovl_get_device;
+	}
+
+	r = dss_install_mgr_ops(&apply_mgr_ops);
+	if (r)
+		goto err_mgr_ops;
+
+	r = display_init_sysfs(pdev);
+	if (r)
+		goto err_disp_sysfs;
+
+	dispc_runtime_get();
+
+	r = dss_dispc_initialize_irq();
+	if (r)
+		goto err_init_irq;
+
+	dispc_runtime_put();
+
+out:
+	mutex_unlock(&compat_init_lock);
+
+	return 0;
+
+err_init_irq:
+	dispc_runtime_put();
+	display_uninit_sysfs(pdev);
+
+err_disp_sysfs:
+	dss_uninstall_mgr_ops();
+
+err_mgr_ops:
+	dss_uninit_overlay_managers_sysfs(pdev);
+	dss_uninit_overlays(pdev);
+
+	compat_refcnt--;
+
+	mutex_unlock(&compat_init_lock);
+
+	return r;
+}
+EXPORT_SYMBOL(omapdss_compat_init);
+
+void omapdss_compat_uninit(void)
+{
+	struct platform_device *pdev = dss_get_core_pdev();
+
+	mutex_lock(&compat_init_lock);
+
+	if (--compat_refcnt > 0)
+		goto out;
+
+	dss_dispc_uninitialize_irq();
+
+	display_uninit_sysfs(pdev);
+
+	dss_uninstall_mgr_ops();
+
+	dss_uninit_overlay_managers_sysfs(pdev);
+	dss_uninit_overlays(pdev);
+out:
+	mutex_unlock(&compat_init_lock);
+}
+EXPORT_SYMBOL(omapdss_compat_uninit);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/core.c b/drivers/video/fbdev/omap2/omapfb/dss/core.c
new file mode 100644
index 000000000000..5a87179b7312
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/core.c
@@ -0,0 +1,343 @@
+/*
+ * linux/drivers/video/omap2/dss/core.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "CORE"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/seq_file.h>
+#include <linux/debugfs.h>
+#include <linux/io.h>
+#include <linux/device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/suspend.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static struct {
+	struct platform_device *pdev;
+
+	const char *default_display_name;
+} core;
+
+static char *def_disp_name;
+module_param_named(def_disp, def_disp_name, charp, 0);
+MODULE_PARM_DESC(def_disp, "default display name");
+
+const char *omapdss_get_default_display_name(void)
+{
+	return core.default_display_name;
+}
+EXPORT_SYMBOL(omapdss_get_default_display_name);
+
+enum omapdss_version omapdss_get_version(void)
+{
+	struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
+	return pdata->version;
+}
+EXPORT_SYMBOL(omapdss_get_version);
+
+struct platform_device *dss_get_core_pdev(void)
+{
+	return core.pdev;
+}
+
+int dss_dsi_enable_pads(int dsi_id, unsigned lane_mask)
+{
+	struct omap_dss_board_info *board_data = core.pdev->dev.platform_data;
+
+	if (!board_data->dsi_enable_pads)
+		return -ENOENT;
+
+	return board_data->dsi_enable_pads(dsi_id, lane_mask);
+}
+
+void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask)
+{
+	struct omap_dss_board_info *board_data = core.pdev->dev.platform_data;
+
+	if (!board_data->dsi_disable_pads)
+		return;
+
+	return board_data->dsi_disable_pads(dsi_id, lane_mask);
+}
+
+int dss_set_min_bus_tput(struct device *dev, unsigned long tput)
+{
+	struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
+
+	if (pdata->set_min_bus_tput)
+		return pdata->set_min_bus_tput(dev, tput);
+	else
+		return 0;
+}
+
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+static int dss_debug_show(struct seq_file *s, void *unused)
+{
+	void (*func)(struct seq_file *) = s->private;
+	func(s);
+	return 0;
+}
+
+static int dss_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dss_debug_show, inode->i_private);
+}
+
+static const struct file_operations dss_debug_fops = {
+	.open           = dss_debug_open,
+	.read           = seq_read,
+	.llseek         = seq_lseek,
+	.release        = single_release,
+};
+
+static struct dentry *dss_debugfs_dir;
+
+static int dss_initialize_debugfs(void)
+{
+	dss_debugfs_dir = debugfs_create_dir("omapdss", NULL);
+	if (IS_ERR(dss_debugfs_dir)) {
+		int err = PTR_ERR(dss_debugfs_dir);
+		dss_debugfs_dir = NULL;
+		return err;
+	}
+
+	debugfs_create_file("clk", S_IRUGO, dss_debugfs_dir,
+			&dss_debug_dump_clocks, &dss_debug_fops);
+
+	return 0;
+}
+
+static void dss_uninitialize_debugfs(void)
+{
+	if (dss_debugfs_dir)
+		debugfs_remove_recursive(dss_debugfs_dir);
+}
+
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *))
+{
+	struct dentry *d;
+
+	d = debugfs_create_file(name, S_IRUGO, dss_debugfs_dir,
+			write, &dss_debug_fops);
+
+	return PTR_ERR_OR_ZERO(d);
+}
+#else /* CONFIG_FB_OMAP2_DSS_DEBUGFS */
+static inline int dss_initialize_debugfs(void)
+{
+	return 0;
+}
+static inline void dss_uninitialize_debugfs(void)
+{
+}
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *))
+{
+	return 0;
+}
+#endif /* CONFIG_FB_OMAP2_DSS_DEBUGFS */
+
+/* PLATFORM DEVICE */
+static int omap_dss_pm_notif(struct notifier_block *b, unsigned long v, void *d)
+{
+	DSSDBG("pm notif %lu\n", v);
+
+	switch (v) {
+	case PM_SUSPEND_PREPARE:
+	case PM_HIBERNATION_PREPARE:
+	case PM_RESTORE_PREPARE:
+		DSSDBG("suspending displays\n");
+		return dss_suspend_all_devices();
+
+	case PM_POST_SUSPEND:
+	case PM_POST_HIBERNATION:
+	case PM_POST_RESTORE:
+		DSSDBG("resuming displays\n");
+		return dss_resume_all_devices();
+
+	default:
+		return 0;
+	}
+}
+
+static struct notifier_block omap_dss_pm_notif_block = {
+	.notifier_call = omap_dss_pm_notif,
+};
+
+static int __init omap_dss_probe(struct platform_device *pdev)
+{
+	struct omap_dss_board_info *pdata = pdev->dev.platform_data;
+	int r;
+
+	core.pdev = pdev;
+
+	dss_features_init(omapdss_get_version());
+
+	r = dss_initialize_debugfs();
+	if (r)
+		goto err_debugfs;
+
+	if (def_disp_name)
+		core.default_display_name = def_disp_name;
+	else if (pdata->default_display_name)
+		core.default_display_name = pdata->default_display_name;
+	else if (pdata->default_device)
+		core.default_display_name = pdata->default_device->name;
+
+	register_pm_notifier(&omap_dss_pm_notif_block);
+
+	return 0;
+
+err_debugfs:
+
+	return r;
+}
+
+static int omap_dss_remove(struct platform_device *pdev)
+{
+	unregister_pm_notifier(&omap_dss_pm_notif_block);
+
+	dss_uninitialize_debugfs();
+
+	return 0;
+}
+
+static void omap_dss_shutdown(struct platform_device *pdev)
+{
+	DSSDBG("shutdown\n");
+	dss_disable_all_devices();
+}
+
+static struct platform_driver omap_dss_driver = {
+	.remove         = omap_dss_remove,
+	.shutdown	= omap_dss_shutdown,
+	.driver         = {
+		.name   = "omapdss",
+	},
+};
+
+/* INIT */
+static int (*dss_output_drv_reg_funcs[])(void) __initdata = {
+	dss_init_platform_driver,
+	dispc_init_platform_driver,
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+	dsi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+	dpi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+	sdi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_RFBI
+	rfbi_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+	venc_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP4_DSS_HDMI
+	hdmi4_init_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP5_DSS_HDMI
+	hdmi5_init_platform_driver,
+#endif
+};
+
+static void (*dss_output_drv_unreg_funcs[])(void) = {
+#ifdef CONFIG_FB_OMAP5_DSS_HDMI
+	hdmi5_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP4_DSS_HDMI
+	hdmi4_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+	venc_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_RFBI
+	rfbi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+	sdi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+	dpi_uninit_platform_driver,
+#endif
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+	dsi_uninit_platform_driver,
+#endif
+	dispc_uninit_platform_driver,
+	dss_uninit_platform_driver,
+};
+
+static int __init omap_dss_init(void)
+{
+	int r;
+	int i;
+
+	r = platform_driver_probe(&omap_dss_driver, omap_dss_probe);
+	if (r)
+		return r;
+
+	for (i = 0; i < ARRAY_SIZE(dss_output_drv_reg_funcs); ++i) {
+		r = dss_output_drv_reg_funcs[i]();
+		if (r)
+			goto err_reg;
+	}
+
+	return 0;
+
+err_reg:
+	for (i = ARRAY_SIZE(dss_output_drv_reg_funcs) - i;
+			i < ARRAY_SIZE(dss_output_drv_reg_funcs);
+			++i)
+		dss_output_drv_unreg_funcs[i]();
+
+	platform_driver_unregister(&omap_dss_driver);
+
+	return r;
+}
+
+static void __exit omap_dss_exit(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dss_output_drv_unreg_funcs); ++i)
+		dss_output_drv_unreg_funcs[i]();
+
+	platform_driver_unregister(&omap_dss_driver);
+}
+
+module_init(omap_dss_init);
+module_exit(omap_dss_exit);
+
+MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@nokia.com>");
+MODULE_DESCRIPTION("OMAP2/3 Display Subsystem");
+MODULE_LICENSE("GPL v2");
+
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c
new file mode 100644
index 000000000000..6607db37a5e4
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.c
@@ -0,0 +1,667 @@
+/*
+ * Copyright (C) 2012 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 DSS_SUBSYS_NAME "APPLY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/jiffies.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/seq_file.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc-compat.h"
+
+#define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
+					 DISPC_IRQ_OCP_ERR | \
+					 DISPC_IRQ_VID1_FIFO_UNDERFLOW | \
+					 DISPC_IRQ_VID2_FIFO_UNDERFLOW | \
+					 DISPC_IRQ_SYNC_LOST | \
+					 DISPC_IRQ_SYNC_LOST_DIGIT)
+
+#define DISPC_MAX_NR_ISRS		8
+
+struct omap_dispc_isr_data {
+	omap_dispc_isr_t	isr;
+	void			*arg;
+	u32			mask;
+};
+
+struct dispc_irq_stats {
+	unsigned long last_reset;
+	unsigned irq_count;
+	unsigned irqs[32];
+};
+
+static struct {
+	spinlock_t irq_lock;
+	u32 irq_error_mask;
+	struct omap_dispc_isr_data registered_isr[DISPC_MAX_NR_ISRS];
+	u32 error_irqs;
+	struct work_struct error_work;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	spinlock_t irq_stats_lock;
+	struct dispc_irq_stats irq_stats;
+#endif
+} dispc_compat;
+
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dispc_dump_irqs(struct seq_file *s)
+{
+	unsigned long flags;
+	struct dispc_irq_stats stats;
+
+	spin_lock_irqsave(&dispc_compat.irq_stats_lock, flags);
+
+	stats = dispc_compat.irq_stats;
+	memset(&dispc_compat.irq_stats, 0, sizeof(dispc_compat.irq_stats));
+	dispc_compat.irq_stats.last_reset = jiffies;
+
+	spin_unlock_irqrestore(&dispc_compat.irq_stats_lock, flags);
+
+	seq_printf(s, "period %u ms\n",
+			jiffies_to_msecs(jiffies - stats.last_reset));
+
+	seq_printf(s, "irqs %d\n", stats.irq_count);
+#define PIS(x) \
+	seq_printf(s, "%-20s %10d\n", #x, stats.irqs[ffs(DISPC_IRQ_##x)-1]);
+
+	PIS(FRAMEDONE);
+	PIS(VSYNC);
+	PIS(EVSYNC_EVEN);
+	PIS(EVSYNC_ODD);
+	PIS(ACBIAS_COUNT_STAT);
+	PIS(PROG_LINE_NUM);
+	PIS(GFX_FIFO_UNDERFLOW);
+	PIS(GFX_END_WIN);
+	PIS(PAL_GAMMA_MASK);
+	PIS(OCP_ERR);
+	PIS(VID1_FIFO_UNDERFLOW);
+	PIS(VID1_END_WIN);
+	PIS(VID2_FIFO_UNDERFLOW);
+	PIS(VID2_END_WIN);
+	if (dss_feat_get_num_ovls() > 3) {
+		PIS(VID3_FIFO_UNDERFLOW);
+		PIS(VID3_END_WIN);
+	}
+	PIS(SYNC_LOST);
+	PIS(SYNC_LOST_DIGIT);
+	PIS(WAKEUP);
+	if (dss_has_feature(FEAT_MGR_LCD2)) {
+		PIS(FRAMEDONE2);
+		PIS(VSYNC2);
+		PIS(ACBIAS_COUNT_STAT2);
+		PIS(SYNC_LOST2);
+	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		PIS(FRAMEDONE3);
+		PIS(VSYNC3);
+		PIS(ACBIAS_COUNT_STAT3);
+		PIS(SYNC_LOST3);
+	}
+#undef PIS
+}
+#endif
+
+/* dispc.irq_lock has to be locked by the caller */
+static void _omap_dispc_set_irqs(void)
+{
+	u32 mask;
+	int i;
+	struct omap_dispc_isr_data *isr_data;
+
+	mask = dispc_compat.irq_error_mask;
+
+	for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+		isr_data = &dispc_compat.registered_isr[i];
+
+		if (isr_data->isr == NULL)
+			continue;
+
+		mask |= isr_data->mask;
+	}
+
+	dispc_write_irqenable(mask);
+}
+
+int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
+{
+	int i;
+	int ret;
+	unsigned long flags;
+	struct omap_dispc_isr_data *isr_data;
+
+	if (isr == NULL)
+		return -EINVAL;
+
+	spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+
+	/* check for duplicate entry */
+	for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+		isr_data = &dispc_compat.registered_isr[i];
+		if (isr_data->isr == isr && isr_data->arg == arg &&
+				isr_data->mask == mask) {
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	isr_data = NULL;
+	ret = -EBUSY;
+
+	for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+		isr_data = &dispc_compat.registered_isr[i];
+
+		if (isr_data->isr != NULL)
+			continue;
+
+		isr_data->isr = isr;
+		isr_data->arg = arg;
+		isr_data->mask = mask;
+		ret = 0;
+
+		break;
+	}
+
+	if (ret)
+		goto err;
+
+	_omap_dispc_set_irqs();
+
+	spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+	return 0;
+err:
+	spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(omap_dispc_register_isr);
+
+int omap_dispc_unregister_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
+{
+	int i;
+	unsigned long flags;
+	int ret = -EINVAL;
+	struct omap_dispc_isr_data *isr_data;
+
+	spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+
+	for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+		isr_data = &dispc_compat.registered_isr[i];
+		if (isr_data->isr != isr || isr_data->arg != arg ||
+				isr_data->mask != mask)
+			continue;
+
+		/* found the correct isr */
+
+		isr_data->isr = NULL;
+		isr_data->arg = NULL;
+		isr_data->mask = 0;
+
+		ret = 0;
+		break;
+	}
+
+	if (ret == 0)
+		_omap_dispc_set_irqs();
+
+	spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+	return ret;
+}
+EXPORT_SYMBOL(omap_dispc_unregister_isr);
+
+static void print_irq_status(u32 status)
+{
+	if ((status & dispc_compat.irq_error_mask) == 0)
+		return;
+
+#define PIS(x) (status & DISPC_IRQ_##x) ? (#x " ") : ""
+
+	pr_debug("DISPC IRQ: 0x%x: %s%s%s%s%s%s%s%s%s\n",
+		status,
+		PIS(OCP_ERR),
+		PIS(GFX_FIFO_UNDERFLOW),
+		PIS(VID1_FIFO_UNDERFLOW),
+		PIS(VID2_FIFO_UNDERFLOW),
+		dss_feat_get_num_ovls() > 3 ? PIS(VID3_FIFO_UNDERFLOW) : "",
+		PIS(SYNC_LOST),
+		PIS(SYNC_LOST_DIGIT),
+		dss_has_feature(FEAT_MGR_LCD2) ? PIS(SYNC_LOST2) : "",
+		dss_has_feature(FEAT_MGR_LCD3) ? PIS(SYNC_LOST3) : "");
+#undef PIS
+}
+
+/* Called from dss.c. Note that we don't touch clocks here,
+ * but we presume they are on because we got an IRQ. However,
+ * an irq handler may turn the clocks off, so we may not have
+ * clock later in the function. */
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
+{
+	int i;
+	u32 irqstatus, irqenable;
+	u32 handledirqs = 0;
+	u32 unhandled_errors;
+	struct omap_dispc_isr_data *isr_data;
+	struct omap_dispc_isr_data registered_isr[DISPC_MAX_NR_ISRS];
+
+	spin_lock(&dispc_compat.irq_lock);
+
+	irqstatus = dispc_read_irqstatus();
+	irqenable = dispc_read_irqenable();
+
+	/* IRQ is not for us */
+	if (!(irqstatus & irqenable)) {
+		spin_unlock(&dispc_compat.irq_lock);
+		return IRQ_NONE;
+	}
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	spin_lock(&dispc_compat.irq_stats_lock);
+	dispc_compat.irq_stats.irq_count++;
+	dss_collect_irq_stats(irqstatus, dispc_compat.irq_stats.irqs);
+	spin_unlock(&dispc_compat.irq_stats_lock);
+#endif
+
+	print_irq_status(irqstatus);
+
+	/* Ack the interrupt. Do it here before clocks are possibly turned
+	 * off */
+	dispc_clear_irqstatus(irqstatus);
+	/* flush posted write */
+	dispc_read_irqstatus();
+
+	/* make a copy and unlock, so that isrs can unregister
+	 * themselves */
+	memcpy(registered_isr, dispc_compat.registered_isr,
+			sizeof(registered_isr));
+
+	spin_unlock(&dispc_compat.irq_lock);
+
+	for (i = 0; i < DISPC_MAX_NR_ISRS; i++) {
+		isr_data = &registered_isr[i];
+
+		if (!isr_data->isr)
+			continue;
+
+		if (isr_data->mask & irqstatus) {
+			isr_data->isr(isr_data->arg, irqstatus);
+			handledirqs |= isr_data->mask;
+		}
+	}
+
+	spin_lock(&dispc_compat.irq_lock);
+
+	unhandled_errors = irqstatus & ~handledirqs & dispc_compat.irq_error_mask;
+
+	if (unhandled_errors) {
+		dispc_compat.error_irqs |= unhandled_errors;
+
+		dispc_compat.irq_error_mask &= ~unhandled_errors;
+		_omap_dispc_set_irqs();
+
+		schedule_work(&dispc_compat.error_work);
+	}
+
+	spin_unlock(&dispc_compat.irq_lock);
+
+	return IRQ_HANDLED;
+}
+
+static void dispc_error_worker(struct work_struct *work)
+{
+	int i;
+	u32 errors;
+	unsigned long flags;
+	static const unsigned fifo_underflow_bits[] = {
+		DISPC_IRQ_GFX_FIFO_UNDERFLOW,
+		DISPC_IRQ_VID1_FIFO_UNDERFLOW,
+		DISPC_IRQ_VID2_FIFO_UNDERFLOW,
+		DISPC_IRQ_VID3_FIFO_UNDERFLOW,
+	};
+
+	spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+	errors = dispc_compat.error_irqs;
+	dispc_compat.error_irqs = 0;
+	spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+	dispc_runtime_get();
+
+	for (i = 0; i < omap_dss_get_num_overlays(); ++i) {
+		struct omap_overlay *ovl;
+		unsigned bit;
+
+		ovl = omap_dss_get_overlay(i);
+		bit = fifo_underflow_bits[i];
+
+		if (bit & errors) {
+			DSSERR("FIFO UNDERFLOW on %s, disabling the overlay\n",
+					ovl->name);
+			ovl->disable(ovl);
+			msleep(50);
+		}
+	}
+
+	for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+		struct omap_overlay_manager *mgr;
+		unsigned bit;
+
+		mgr = omap_dss_get_overlay_manager(i);
+		bit = dispc_mgr_get_sync_lost_irq(i);
+
+		if (bit & errors) {
+			int j;
+
+			DSSERR("SYNC_LOST on channel %s, restarting the output "
+					"with video overlays disabled\n",
+					mgr->name);
+
+			dss_mgr_disable(mgr);
+
+			for (j = 0; j < omap_dss_get_num_overlays(); ++j) {
+				struct omap_overlay *ovl;
+				ovl = omap_dss_get_overlay(j);
+
+				if (ovl->id != OMAP_DSS_GFX &&
+						ovl->manager == mgr)
+					ovl->disable(ovl);
+			}
+
+			dss_mgr_enable(mgr);
+		}
+	}
+
+	if (errors & DISPC_IRQ_OCP_ERR) {
+		DSSERR("OCP_ERR\n");
+		for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+			struct omap_overlay_manager *mgr;
+
+			mgr = omap_dss_get_overlay_manager(i);
+			dss_mgr_disable(mgr);
+		}
+	}
+
+	spin_lock_irqsave(&dispc_compat.irq_lock, flags);
+	dispc_compat.irq_error_mask |= errors;
+	_omap_dispc_set_irqs();
+	spin_unlock_irqrestore(&dispc_compat.irq_lock, flags);
+
+	dispc_runtime_put();
+}
+
+int dss_dispc_initialize_irq(void)
+{
+	int r;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	spin_lock_init(&dispc_compat.irq_stats_lock);
+	dispc_compat.irq_stats.last_reset = jiffies;
+	dss_debugfs_create_file("dispc_irq", dispc_dump_irqs);
+#endif
+
+	spin_lock_init(&dispc_compat.irq_lock);
+
+	memset(dispc_compat.registered_isr, 0,
+			sizeof(dispc_compat.registered_isr));
+
+	dispc_compat.irq_error_mask = DISPC_IRQ_MASK_ERROR;
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		dispc_compat.irq_error_mask |= DISPC_IRQ_SYNC_LOST2;
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		dispc_compat.irq_error_mask |= DISPC_IRQ_SYNC_LOST3;
+	if (dss_feat_get_num_ovls() > 3)
+		dispc_compat.irq_error_mask |= DISPC_IRQ_VID3_FIFO_UNDERFLOW;
+
+	/*
+	 * there's SYNC_LOST_DIGIT waiting after enabling the DSS,
+	 * so clear it
+	 */
+	dispc_clear_irqstatus(dispc_read_irqstatus());
+
+	INIT_WORK(&dispc_compat.error_work, dispc_error_worker);
+
+	_omap_dispc_set_irqs();
+
+	r = dispc_request_irq(omap_dispc_irq_handler, &dispc_compat);
+	if (r) {
+		DSSERR("dispc_request_irq failed\n");
+		return r;
+	}
+
+	return 0;
+}
+
+void dss_dispc_uninitialize_irq(void)
+{
+	dispc_free_irq(&dispc_compat);
+}
+
+static void dispc_mgr_disable_isr(void *data, u32 mask)
+{
+	struct completion *compl = data;
+	complete(compl);
+}
+
+static void dispc_mgr_enable_lcd_out(enum omap_channel channel)
+{
+	dispc_mgr_enable(channel, true);
+}
+
+static void dispc_mgr_disable_lcd_out(enum omap_channel channel)
+{
+	DECLARE_COMPLETION_ONSTACK(framedone_compl);
+	int r;
+	u32 irq;
+
+	if (!dispc_mgr_is_enabled(channel))
+		return;
+
+	/*
+	 * When we disable LCD output, we need to wait for FRAMEDONE to know
+	 * that DISPC has finished with the LCD output.
+	 */
+
+	irq = dispc_mgr_get_framedone_irq(channel);
+
+	r = omap_dispc_register_isr(dispc_mgr_disable_isr, &framedone_compl,
+			irq);
+	if (r)
+		DSSERR("failed to register FRAMEDONE isr\n");
+
+	dispc_mgr_enable(channel, false);
+
+	/* if we couldn't register for framedone, just sleep and exit */
+	if (r) {
+		msleep(100);
+		return;
+	}
+
+	if (!wait_for_completion_timeout(&framedone_compl,
+				msecs_to_jiffies(100)))
+		DSSERR("timeout waiting for FRAME DONE\n");
+
+	r = omap_dispc_unregister_isr(dispc_mgr_disable_isr, &framedone_compl,
+			irq);
+	if (r)
+		DSSERR("failed to unregister FRAMEDONE isr\n");
+}
+
+static void dispc_digit_out_enable_isr(void *data, u32 mask)
+{
+	struct completion *compl = data;
+
+	/* ignore any sync lost interrupts */
+	if (mask & (DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD))
+		complete(compl);
+}
+
+static void dispc_mgr_enable_digit_out(void)
+{
+	DECLARE_COMPLETION_ONSTACK(vsync_compl);
+	int r;
+	u32 irq_mask;
+
+	if (dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
+		return;
+
+	/*
+	 * Digit output produces some sync lost interrupts during the first
+	 * frame when enabling. Those need to be ignored, so we register for the
+	 * sync lost irq to prevent the error handler from triggering.
+	 */
+
+	irq_mask = dispc_mgr_get_vsync_irq(OMAP_DSS_CHANNEL_DIGIT) |
+		dispc_mgr_get_sync_lost_irq(OMAP_DSS_CHANNEL_DIGIT);
+
+	r = omap_dispc_register_isr(dispc_digit_out_enable_isr, &vsync_compl,
+			irq_mask);
+	if (r) {
+		DSSERR("failed to register %x isr\n", irq_mask);
+		return;
+	}
+
+	dispc_mgr_enable(OMAP_DSS_CHANNEL_DIGIT, true);
+
+	/* wait for the first evsync */
+	if (!wait_for_completion_timeout(&vsync_compl, msecs_to_jiffies(100)))
+		DSSERR("timeout waiting for digit out to start\n");
+
+	r = omap_dispc_unregister_isr(dispc_digit_out_enable_isr, &vsync_compl,
+			irq_mask);
+	if (r)
+		DSSERR("failed to unregister %x isr\n", irq_mask);
+}
+
+static void dispc_mgr_disable_digit_out(void)
+{
+	DECLARE_COMPLETION_ONSTACK(framedone_compl);
+	int r, i;
+	u32 irq_mask;
+	int num_irqs;
+
+	if (!dispc_mgr_is_enabled(OMAP_DSS_CHANNEL_DIGIT))
+		return;
+
+	/*
+	 * When we disable the digit output, we need to wait for FRAMEDONE to
+	 * know that DISPC has finished with the output.
+	 */
+
+	irq_mask = dispc_mgr_get_framedone_irq(OMAP_DSS_CHANNEL_DIGIT);
+	num_irqs = 1;
+
+	if (!irq_mask) {
+		/*
+		 * omap 2/3 don't have framedone irq for TV, so we need to use
+		 * vsyncs for this.
+		 */
+
+		irq_mask = dispc_mgr_get_vsync_irq(OMAP_DSS_CHANNEL_DIGIT);
+		/*
+		 * We need to wait for both even and odd vsyncs. Note that this
+		 * is not totally reliable, as we could get a vsync interrupt
+		 * before we disable the output, which leads to timeout in the
+		 * wait_for_completion.
+		 */
+		num_irqs = 2;
+	}
+
+	r = omap_dispc_register_isr(dispc_mgr_disable_isr, &framedone_compl,
+			irq_mask);
+	if (r)
+		DSSERR("failed to register %x isr\n", irq_mask);
+
+	dispc_mgr_enable(OMAP_DSS_CHANNEL_DIGIT, false);
+
+	/* if we couldn't register the irq, just sleep and exit */
+	if (r) {
+		msleep(100);
+		return;
+	}
+
+	for (i = 0; i < num_irqs; ++i) {
+		if (!wait_for_completion_timeout(&framedone_compl,
+					msecs_to_jiffies(100)))
+			DSSERR("timeout waiting for digit out to stop\n");
+	}
+
+	r = omap_dispc_unregister_isr(dispc_mgr_disable_isr, &framedone_compl,
+			irq_mask);
+	if (r)
+		DSSERR("failed to unregister %x isr\n", irq_mask);
+}
+
+void dispc_mgr_enable_sync(enum omap_channel channel)
+{
+	if (dss_mgr_is_lcd(channel))
+		dispc_mgr_enable_lcd_out(channel);
+	else if (channel == OMAP_DSS_CHANNEL_DIGIT)
+		dispc_mgr_enable_digit_out();
+	else
+		WARN_ON(1);
+}
+
+void dispc_mgr_disable_sync(enum omap_channel channel)
+{
+	if (dss_mgr_is_lcd(channel))
+		dispc_mgr_disable_lcd_out(channel);
+	else if (channel == OMAP_DSS_CHANNEL_DIGIT)
+		dispc_mgr_disable_digit_out();
+	else
+		WARN_ON(1);
+}
+
+static inline void dispc_irq_wait_handler(void *data, u32 mask)
+{
+	complete((struct completion *)data);
+}
+
+int omap_dispc_wait_for_irq_interruptible_timeout(u32 irqmask,
+		unsigned long timeout)
+{
+
+	int r;
+	DECLARE_COMPLETION_ONSTACK(completion);
+
+	r = omap_dispc_register_isr(dispc_irq_wait_handler, &completion,
+			irqmask);
+
+	if (r)
+		return r;
+
+	timeout = wait_for_completion_interruptible_timeout(&completion,
+			timeout);
+
+	omap_dispc_unregister_isr(dispc_irq_wait_handler, &completion, irqmask);
+
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	if (timeout == -ERESTARTSYS)
+		return -ERESTARTSYS;
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h
new file mode 100644
index 000000000000..14a69b3d4fb0
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc-compat.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2012 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 __OMAP2_DSS_DISPC_COMPAT_H
+#define __OMAP2_DSS_DISPC_COMPAT_H
+
+void dispc_mgr_enable_sync(enum omap_channel channel);
+void dispc_mgr_disable_sync(enum omap_channel channel);
+
+int omap_dispc_wait_for_irq_interruptible_timeout(u32 irqmask,
+		unsigned long timeout);
+
+int dss_dispc_initialize_irq(void);
+void dss_dispc_uninitialize_irq(void);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc.c
new file mode 100644
index 000000000000..5491e304f4fe
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc.c
@@ -0,0 +1,4234 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DISPC"
+
+#include <linux/kernel.h>
+#include <linux/dma-mapping.h>
+#include <linux/vmalloc.h>
+#include <linux/export.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/seq_file.h>
+#include <linux/delay.h>
+#include <linux/workqueue.h>
+#include <linux/hardirq.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/sizes.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+#include "dispc.h"
+
+/* DISPC */
+#define DISPC_SZ_REGS			SZ_4K
+
+enum omap_burst_size {
+	BURST_SIZE_X2 = 0,
+	BURST_SIZE_X4 = 1,
+	BURST_SIZE_X8 = 2,
+};
+
+#define REG_GET(idx, start, end) \
+	FLD_GET(dispc_read_reg(idx), start, end)
+
+#define REG_FLD_MOD(idx, val, start, end)				\
+	dispc_write_reg(idx, FLD_MOD(dispc_read_reg(idx), val, start, end))
+
+struct dispc_features {
+	u8 sw_start;
+	u8 fp_start;
+	u8 bp_start;
+	u16 sw_max;
+	u16 vp_max;
+	u16 hp_max;
+	u8 mgr_width_start;
+	u8 mgr_height_start;
+	u16 mgr_width_max;
+	u16 mgr_height_max;
+	unsigned long max_lcd_pclk;
+	unsigned long max_tv_pclk;
+	int (*calc_scaling) (unsigned long pclk, unsigned long lclk,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem);
+	unsigned long (*calc_core_clk) (unsigned long pclk,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		bool mem_to_mem);
+	u8 num_fifos;
+
+	/* swap GFX & WB fifos */
+	bool gfx_fifo_workaround:1;
+
+	/* no DISPC_IRQ_FRAMEDONETV on this SoC */
+	bool no_framedone_tv:1;
+
+	/* revert to the OMAP4 mechanism of DISPC Smart Standby operation */
+	bool mstandby_workaround:1;
+
+	bool set_max_preload:1;
+
+	/* PIXEL_INC is not added to the last pixel of a line */
+	bool last_pixel_inc_missing:1;
+
+	/* POL_FREQ has ALIGN bit */
+	bool supports_sync_align:1;
+
+	bool has_writeback:1;
+};
+
+#define DISPC_MAX_NR_FIFOS 5
+
+static struct {
+	struct platform_device *pdev;
+	void __iomem    *base;
+
+	int irq;
+	irq_handler_t user_handler;
+	void *user_data;
+
+	unsigned long core_clk_rate;
+	unsigned long tv_pclk_rate;
+
+	u32 fifo_size[DISPC_MAX_NR_FIFOS];
+	/* maps which plane is using a fifo. fifo-id -> plane-id */
+	int fifo_assignment[DISPC_MAX_NR_FIFOS];
+
+	bool		ctx_valid;
+	u32		ctx[DISPC_SZ_REGS / sizeof(u32)];
+
+	const struct dispc_features *feat;
+
+	bool is_enabled;
+
+	struct regmap *syscon_pol;
+	u32 syscon_pol_offset;
+
+	/* DISPC_CONTROL & DISPC_CONFIG lock*/
+	spinlock_t control_lock;
+} dispc;
+
+enum omap_color_component {
+	/* used for all color formats for OMAP3 and earlier
+	 * and for RGB and Y color component on OMAP4
+	 */
+	DISPC_COLOR_COMPONENT_RGB_Y		= 1 << 0,
+	/* used for UV component for
+	 * OMAP_DSS_COLOR_YUV2, OMAP_DSS_COLOR_UYVY, OMAP_DSS_COLOR_NV12
+	 * color formats on OMAP4
+	 */
+	DISPC_COLOR_COMPONENT_UV		= 1 << 1,
+};
+
+enum mgr_reg_fields {
+	DISPC_MGR_FLD_ENABLE,
+	DISPC_MGR_FLD_STNTFT,
+	DISPC_MGR_FLD_GO,
+	DISPC_MGR_FLD_TFTDATALINES,
+	DISPC_MGR_FLD_STALLMODE,
+	DISPC_MGR_FLD_TCKENABLE,
+	DISPC_MGR_FLD_TCKSELECTION,
+	DISPC_MGR_FLD_CPR,
+	DISPC_MGR_FLD_FIFOHANDCHECK,
+	/* used to maintain a count of the above fields */
+	DISPC_MGR_FLD_NUM,
+};
+
+struct dispc_reg_field {
+	u16 reg;
+	u8 high;
+	u8 low;
+};
+
+static const struct {
+	const char *name;
+	u32 vsync_irq;
+	u32 framedone_irq;
+	u32 sync_lost_irq;
+	struct dispc_reg_field reg_desc[DISPC_MGR_FLD_NUM];
+} mgr_desc[] = {
+	[OMAP_DSS_CHANNEL_LCD] = {
+		.name		= "LCD",
+		.vsync_irq	= DISPC_IRQ_VSYNC,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_DIGIT] = {
+		.name		= "DIGIT",
+		.vsync_irq	= DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONETV,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST_DIGIT,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL,  1,  1 },
+			[DISPC_MGR_FLD_STNTFT]		= { },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL,  6,  6 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { },
+			[DISPC_MGR_FLD_STALLMODE]	= { },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG,  12, 12 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG,  13, 13 },
+			[DISPC_MGR_FLD_CPR]		= { },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_LCD2] = {
+		.name		= "LCD2",
+		.vsync_irq	= DISPC_IRQ_VSYNC2,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE2,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST2,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL2,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL2,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL2,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL2,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL2, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG2,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG2,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG2,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG2,  16, 16 },
+		},
+	},
+	[OMAP_DSS_CHANNEL_LCD3] = {
+		.name		= "LCD3",
+		.vsync_irq	= DISPC_IRQ_VSYNC3,
+		.framedone_irq	= DISPC_IRQ_FRAMEDONE3,
+		.sync_lost_irq	= DISPC_IRQ_SYNC_LOST3,
+		.reg_desc	= {
+			[DISPC_MGR_FLD_ENABLE]		= { DISPC_CONTROL3,  0,  0 },
+			[DISPC_MGR_FLD_STNTFT]		= { DISPC_CONTROL3,  3,  3 },
+			[DISPC_MGR_FLD_GO]		= { DISPC_CONTROL3,  5,  5 },
+			[DISPC_MGR_FLD_TFTDATALINES]	= { DISPC_CONTROL3,  9,  8 },
+			[DISPC_MGR_FLD_STALLMODE]	= { DISPC_CONTROL3, 11, 11 },
+			[DISPC_MGR_FLD_TCKENABLE]	= { DISPC_CONFIG3,  10, 10 },
+			[DISPC_MGR_FLD_TCKSELECTION]	= { DISPC_CONFIG3,  11, 11 },
+			[DISPC_MGR_FLD_CPR]		= { DISPC_CONFIG3,  15, 15 },
+			[DISPC_MGR_FLD_FIFOHANDCHECK]	= { DISPC_CONFIG3,  16, 16 },
+		},
+	},
+};
+
+struct color_conv_coef {
+	int ry, rcr, rcb, gy, gcr, gcb, by, bcr, bcb;
+	int full_range;
+};
+
+static unsigned long dispc_fclk_rate(void);
+static unsigned long dispc_core_clk_rate(void);
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel);
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel);
+
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane);
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane);
+
+static inline void dispc_write_reg(const u16 idx, u32 val)
+{
+	__raw_writel(val, dispc.base + idx);
+}
+
+static inline u32 dispc_read_reg(const u16 idx)
+{
+	return __raw_readl(dispc.base + idx);
+}
+
+static u32 mgr_fld_read(enum omap_channel channel, enum mgr_reg_fields regfld)
+{
+	const struct dispc_reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+	return REG_GET(rfld.reg, rfld.high, rfld.low);
+}
+
+static void mgr_fld_write(enum omap_channel channel,
+					enum mgr_reg_fields regfld, int val) {
+	const struct dispc_reg_field rfld = mgr_desc[channel].reg_desc[regfld];
+	const bool need_lock = rfld.reg == DISPC_CONTROL || rfld.reg == DISPC_CONFIG;
+	unsigned long flags;
+
+	if (need_lock)
+		spin_lock_irqsave(&dispc.control_lock, flags);
+
+	REG_FLD_MOD(rfld.reg, val, rfld.high, rfld.low);
+
+	if (need_lock)
+		spin_unlock_irqrestore(&dispc.control_lock, flags);
+}
+
+#define SR(reg) \
+	dispc.ctx[DISPC_##reg / sizeof(u32)] = dispc_read_reg(DISPC_##reg)
+#define RR(reg) \
+	dispc_write_reg(DISPC_##reg, dispc.ctx[DISPC_##reg / sizeof(u32)])
+
+static void dispc_save_context(void)
+{
+	int i, j;
+
+	DSSDBG("dispc_save_context\n");
+
+	SR(IRQENABLE);
+	SR(CONTROL);
+	SR(CONFIG);
+	SR(LINE_NUMBER);
+	if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+			dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+		SR(GLOBAL_ALPHA);
+	if (dss_has_feature(FEAT_MGR_LCD2)) {
+		SR(CONTROL2);
+		SR(CONFIG2);
+	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		SR(CONTROL3);
+		SR(CONFIG3);
+	}
+
+	for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+		SR(DEFAULT_COLOR(i));
+		SR(TRANS_COLOR(i));
+		SR(SIZE_MGR(i));
+		if (i == OMAP_DSS_CHANNEL_DIGIT)
+			continue;
+		SR(TIMING_H(i));
+		SR(TIMING_V(i));
+		SR(POL_FREQ(i));
+		SR(DIVISORo(i));
+
+		SR(DATA_CYCLE1(i));
+		SR(DATA_CYCLE2(i));
+		SR(DATA_CYCLE3(i));
+
+		if (dss_has_feature(FEAT_CPR)) {
+			SR(CPR_COEF_R(i));
+			SR(CPR_COEF_G(i));
+			SR(CPR_COEF_B(i));
+		}
+	}
+
+	for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+		SR(OVL_BA0(i));
+		SR(OVL_BA1(i));
+		SR(OVL_POSITION(i));
+		SR(OVL_SIZE(i));
+		SR(OVL_ATTRIBUTES(i));
+		SR(OVL_FIFO_THRESHOLD(i));
+		SR(OVL_ROW_INC(i));
+		SR(OVL_PIXEL_INC(i));
+		if (dss_has_feature(FEAT_PRELOAD))
+			SR(OVL_PRELOAD(i));
+		if (i == OMAP_DSS_GFX) {
+			SR(OVL_WINDOW_SKIP(i));
+			SR(OVL_TABLE_BA(i));
+			continue;
+		}
+		SR(OVL_FIR(i));
+		SR(OVL_PICTURE_SIZE(i));
+		SR(OVL_ACCU0(i));
+		SR(OVL_ACCU1(i));
+
+		for (j = 0; j < 8; j++)
+			SR(OVL_FIR_COEF_H(i, j));
+
+		for (j = 0; j < 8; j++)
+			SR(OVL_FIR_COEF_HV(i, j));
+
+		for (j = 0; j < 5; j++)
+			SR(OVL_CONV_COEF(i, j));
+
+		if (dss_has_feature(FEAT_FIR_COEF_V)) {
+			for (j = 0; j < 8; j++)
+				SR(OVL_FIR_COEF_V(i, j));
+		}
+
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			SR(OVL_BA0_UV(i));
+			SR(OVL_BA1_UV(i));
+			SR(OVL_FIR2(i));
+			SR(OVL_ACCU2_0(i));
+			SR(OVL_ACCU2_1(i));
+
+			for (j = 0; j < 8; j++)
+				SR(OVL_FIR_COEF_H2(i, j));
+
+			for (j = 0; j < 8; j++)
+				SR(OVL_FIR_COEF_HV2(i, j));
+
+			for (j = 0; j < 8; j++)
+				SR(OVL_FIR_COEF_V2(i, j));
+		}
+		if (dss_has_feature(FEAT_ATTR2))
+			SR(OVL_ATTRIBUTES2(i));
+	}
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV))
+		SR(DIVISOR);
+
+	dispc.ctx_valid = true;
+
+	DSSDBG("context saved\n");
+}
+
+static void dispc_restore_context(void)
+{
+	int i, j;
+
+	DSSDBG("dispc_restore_context\n");
+
+	if (!dispc.ctx_valid)
+		return;
+
+	/*RR(IRQENABLE);*/
+	/*RR(CONTROL);*/
+	RR(CONFIG);
+	RR(LINE_NUMBER);
+	if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+			dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+		RR(GLOBAL_ALPHA);
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		RR(CONFIG2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		RR(CONFIG3);
+
+	for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+		RR(DEFAULT_COLOR(i));
+		RR(TRANS_COLOR(i));
+		RR(SIZE_MGR(i));
+		if (i == OMAP_DSS_CHANNEL_DIGIT)
+			continue;
+		RR(TIMING_H(i));
+		RR(TIMING_V(i));
+		RR(POL_FREQ(i));
+		RR(DIVISORo(i));
+
+		RR(DATA_CYCLE1(i));
+		RR(DATA_CYCLE2(i));
+		RR(DATA_CYCLE3(i));
+
+		if (dss_has_feature(FEAT_CPR)) {
+			RR(CPR_COEF_R(i));
+			RR(CPR_COEF_G(i));
+			RR(CPR_COEF_B(i));
+		}
+	}
+
+	for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+		RR(OVL_BA0(i));
+		RR(OVL_BA1(i));
+		RR(OVL_POSITION(i));
+		RR(OVL_SIZE(i));
+		RR(OVL_ATTRIBUTES(i));
+		RR(OVL_FIFO_THRESHOLD(i));
+		RR(OVL_ROW_INC(i));
+		RR(OVL_PIXEL_INC(i));
+		if (dss_has_feature(FEAT_PRELOAD))
+			RR(OVL_PRELOAD(i));
+		if (i == OMAP_DSS_GFX) {
+			RR(OVL_WINDOW_SKIP(i));
+			RR(OVL_TABLE_BA(i));
+			continue;
+		}
+		RR(OVL_FIR(i));
+		RR(OVL_PICTURE_SIZE(i));
+		RR(OVL_ACCU0(i));
+		RR(OVL_ACCU1(i));
+
+		for (j = 0; j < 8; j++)
+			RR(OVL_FIR_COEF_H(i, j));
+
+		for (j = 0; j < 8; j++)
+			RR(OVL_FIR_COEF_HV(i, j));
+
+		for (j = 0; j < 5; j++)
+			RR(OVL_CONV_COEF(i, j));
+
+		if (dss_has_feature(FEAT_FIR_COEF_V)) {
+			for (j = 0; j < 8; j++)
+				RR(OVL_FIR_COEF_V(i, j));
+		}
+
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			RR(OVL_BA0_UV(i));
+			RR(OVL_BA1_UV(i));
+			RR(OVL_FIR2(i));
+			RR(OVL_ACCU2_0(i));
+			RR(OVL_ACCU2_1(i));
+
+			for (j = 0; j < 8; j++)
+				RR(OVL_FIR_COEF_H2(i, j));
+
+			for (j = 0; j < 8; j++)
+				RR(OVL_FIR_COEF_HV2(i, j));
+
+			for (j = 0; j < 8; j++)
+				RR(OVL_FIR_COEF_V2(i, j));
+		}
+		if (dss_has_feature(FEAT_ATTR2))
+			RR(OVL_ATTRIBUTES2(i));
+	}
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV))
+		RR(DIVISOR);
+
+	/* enable last, because LCD & DIGIT enable are here */
+	RR(CONTROL);
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		RR(CONTROL2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		RR(CONTROL3);
+	/* clear spurious SYNC_LOST_DIGIT interrupts */
+	dispc_clear_irqstatus(DISPC_IRQ_SYNC_LOST_DIGIT);
+
+	/*
+	 * enable last so IRQs won't trigger before
+	 * the context is fully restored
+	 */
+	RR(IRQENABLE);
+
+	DSSDBG("context restored\n");
+}
+
+#undef SR
+#undef RR
+
+int dispc_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("dispc_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dispc.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+EXPORT_SYMBOL(dispc_runtime_get);
+
+void dispc_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("dispc_runtime_put\n");
+
+	r = pm_runtime_put_sync(&dispc.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+EXPORT_SYMBOL(dispc_runtime_put);
+
+u32 dispc_mgr_get_vsync_irq(enum omap_channel channel)
+{
+	return mgr_desc[channel].vsync_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_vsync_irq);
+
+u32 dispc_mgr_get_framedone_irq(enum omap_channel channel)
+{
+	if (channel == OMAP_DSS_CHANNEL_DIGIT && dispc.feat->no_framedone_tv)
+		return 0;
+
+	return mgr_desc[channel].framedone_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_framedone_irq);
+
+u32 dispc_mgr_get_sync_lost_irq(enum omap_channel channel)
+{
+	return mgr_desc[channel].sync_lost_irq;
+}
+EXPORT_SYMBOL(dispc_mgr_get_sync_lost_irq);
+
+u32 dispc_wb_get_framedone_irq(void)
+{
+	return DISPC_IRQ_FRAMEDONEWB;
+}
+
+bool dispc_mgr_go_busy(enum omap_channel channel)
+{
+	return mgr_fld_read(channel, DISPC_MGR_FLD_GO) == 1;
+}
+EXPORT_SYMBOL(dispc_mgr_go_busy);
+
+void dispc_mgr_go(enum omap_channel channel)
+{
+	WARN_ON(!dispc_mgr_is_enabled(channel));
+	WARN_ON(dispc_mgr_go_busy(channel));
+
+	DSSDBG("GO %s\n", mgr_desc[channel].name);
+
+	mgr_fld_write(channel, DISPC_MGR_FLD_GO, 1);
+}
+EXPORT_SYMBOL(dispc_mgr_go);
+
+bool dispc_wb_go_busy(void)
+{
+	return REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+}
+
+void dispc_wb_go(void)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+	bool enable, go;
+
+	enable = REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0) == 1;
+
+	if (!enable)
+		return;
+
+	go = REG_GET(DISPC_CONTROL2, 6, 6) == 1;
+	if (go) {
+		DSSERR("GO bit not down for WB\n");
+		return;
+	}
+
+	REG_FLD_MOD(DISPC_CONTROL2, 1, 6, 6);
+}
+
+static void dispc_ovl_write_firh_reg(enum omap_plane plane, int reg, u32 value)
+{
+	dispc_write_reg(DISPC_OVL_FIR_COEF_H(plane, reg), value);
+}
+
+static void dispc_ovl_write_firhv_reg(enum omap_plane plane, int reg, u32 value)
+{
+	dispc_write_reg(DISPC_OVL_FIR_COEF_HV(plane, reg), value);
+}
+
+static void dispc_ovl_write_firv_reg(enum omap_plane plane, int reg, u32 value)
+{
+	dispc_write_reg(DISPC_OVL_FIR_COEF_V(plane, reg), value);
+}
+
+static void dispc_ovl_write_firh2_reg(enum omap_plane plane, int reg, u32 value)
+{
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	dispc_write_reg(DISPC_OVL_FIR_COEF_H2(plane, reg), value);
+}
+
+static void dispc_ovl_write_firhv2_reg(enum omap_plane plane, int reg,
+		u32 value)
+{
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	dispc_write_reg(DISPC_OVL_FIR_COEF_HV2(plane, reg), value);
+}
+
+static void dispc_ovl_write_firv2_reg(enum omap_plane plane, int reg, u32 value)
+{
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	dispc_write_reg(DISPC_OVL_FIR_COEF_V2(plane, reg), value);
+}
+
+static void dispc_ovl_set_scale_coef(enum omap_plane plane, int fir_hinc,
+				int fir_vinc, int five_taps,
+				enum omap_color_component color_comp)
+{
+	const struct dispc_coef *h_coef, *v_coef;
+	int i;
+
+	h_coef = dispc_ovl_get_scale_coef(fir_hinc, true);
+	v_coef = dispc_ovl_get_scale_coef(fir_vinc, five_taps);
+
+	for (i = 0; i < 8; i++) {
+		u32 h, hv;
+
+		h = FLD_VAL(h_coef[i].hc0_vc00, 7, 0)
+			| FLD_VAL(h_coef[i].hc1_vc0, 15, 8)
+			| FLD_VAL(h_coef[i].hc2_vc1, 23, 16)
+			| FLD_VAL(h_coef[i].hc3_vc2, 31, 24);
+		hv = FLD_VAL(h_coef[i].hc4_vc22, 7, 0)
+			| FLD_VAL(v_coef[i].hc1_vc0, 15, 8)
+			| FLD_VAL(v_coef[i].hc2_vc1, 23, 16)
+			| FLD_VAL(v_coef[i].hc3_vc2, 31, 24);
+
+		if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+			dispc_ovl_write_firh_reg(plane, i, h);
+			dispc_ovl_write_firhv_reg(plane, i, hv);
+		} else {
+			dispc_ovl_write_firh2_reg(plane, i, h);
+			dispc_ovl_write_firhv2_reg(plane, i, hv);
+		}
+
+	}
+
+	if (five_taps) {
+		for (i = 0; i < 8; i++) {
+			u32 v;
+			v = FLD_VAL(v_coef[i].hc0_vc00, 7, 0)
+				| FLD_VAL(v_coef[i].hc4_vc22, 15, 8);
+			if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y)
+				dispc_ovl_write_firv_reg(plane, i, v);
+			else
+				dispc_ovl_write_firv2_reg(plane, i, v);
+		}
+	}
+}
+
+
+static void dispc_ovl_write_color_conv_coef(enum omap_plane plane,
+		const struct color_conv_coef *ct)
+{
+#define CVAL(x, y) (FLD_VAL(x, 26, 16) | FLD_VAL(y, 10, 0))
+
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 0), CVAL(ct->rcr, ct->ry));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 1), CVAL(ct->gy,  ct->rcb));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 2), CVAL(ct->gcb, ct->gcr));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 3), CVAL(ct->bcr, ct->by));
+	dispc_write_reg(DISPC_OVL_CONV_COEF(plane, 4), CVAL(0, ct->bcb));
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), ct->full_range, 11, 11);
+
+#undef CVAL
+}
+
+static void dispc_setup_color_conv_coef(void)
+{
+	int i;
+	int num_ovl = dss_feat_get_num_ovls();
+	const struct color_conv_coef ctbl_bt601_5_ovl = {
+		/* YUV -> RGB */
+		298, 409, 0, 298, -208, -100, 298, 0, 517, 0,
+	};
+	const struct color_conv_coef ctbl_bt601_5_wb = {
+		/* RGB -> YUV */
+		66, 129, 25, 112, -94, -18, -38, -74, 112, 0,
+	};
+
+	for (i = 1; i < num_ovl; i++)
+		dispc_ovl_write_color_conv_coef(i, &ctbl_bt601_5_ovl);
+
+	if (dispc.feat->has_writeback)
+		dispc_ovl_write_color_conv_coef(OMAP_DSS_WB, &ctbl_bt601_5_wb);
+}
+
+static void dispc_ovl_set_ba0(enum omap_plane plane, u32 paddr)
+{
+	dispc_write_reg(DISPC_OVL_BA0(plane), paddr);
+}
+
+static void dispc_ovl_set_ba1(enum omap_plane plane, u32 paddr)
+{
+	dispc_write_reg(DISPC_OVL_BA1(plane), paddr);
+}
+
+static void dispc_ovl_set_ba0_uv(enum omap_plane plane, u32 paddr)
+{
+	dispc_write_reg(DISPC_OVL_BA0_UV(plane), paddr);
+}
+
+static void dispc_ovl_set_ba1_uv(enum omap_plane plane, u32 paddr)
+{
+	dispc_write_reg(DISPC_OVL_BA1_UV(plane), paddr);
+}
+
+static void dispc_ovl_set_pos(enum omap_plane plane,
+		enum omap_overlay_caps caps, int x, int y)
+{
+	u32 val;
+
+	if ((caps & OMAP_DSS_OVL_CAP_POS) == 0)
+		return;
+
+	val = FLD_VAL(y, 26, 16) | FLD_VAL(x, 10, 0);
+
+	dispc_write_reg(DISPC_OVL_POSITION(plane), val);
+}
+
+static void dispc_ovl_set_input_size(enum omap_plane plane, int width,
+		int height)
+{
+	u32 val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
+
+	if (plane == OMAP_DSS_GFX || plane == OMAP_DSS_WB)
+		dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+	else
+		dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+}
+
+static void dispc_ovl_set_output_size(enum omap_plane plane, int width,
+		int height)
+{
+	u32 val;
+
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	val = FLD_VAL(height - 1, 26, 16) | FLD_VAL(width - 1, 10, 0);
+
+	if (plane == OMAP_DSS_WB)
+		dispc_write_reg(DISPC_OVL_PICTURE_SIZE(plane), val);
+	else
+		dispc_write_reg(DISPC_OVL_SIZE(plane), val);
+}
+
+static void dispc_ovl_set_zorder(enum omap_plane plane,
+		enum omap_overlay_caps caps, u8 zorder)
+{
+	if ((caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+		return;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), zorder, 27, 26);
+}
+
+static void dispc_ovl_enable_zorder_planes(void)
+{
+	int i;
+
+	if (!dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+		return;
+
+	for (i = 0; i < dss_feat_get_num_ovls(); i++)
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(i), 1, 25, 25);
+}
+
+static void dispc_ovl_set_pre_mult_alpha(enum omap_plane plane,
+		enum omap_overlay_caps caps, bool enable)
+{
+	if ((caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+		return;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 28, 28);
+}
+
+static void dispc_ovl_setup_global_alpha(enum omap_plane plane,
+		enum omap_overlay_caps caps, u8 global_alpha)
+{
+	static const unsigned shifts[] = { 0, 8, 16, 24, };
+	int shift;
+
+	if ((caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+		return;
+
+	shift = shifts[plane];
+	REG_FLD_MOD(DISPC_GLOBAL_ALPHA, global_alpha, shift + 7, shift);
+}
+
+static void dispc_ovl_set_pix_inc(enum omap_plane plane, s32 inc)
+{
+	dispc_write_reg(DISPC_OVL_PIXEL_INC(plane), inc);
+}
+
+static void dispc_ovl_set_row_inc(enum omap_plane plane, s32 inc)
+{
+	dispc_write_reg(DISPC_OVL_ROW_INC(plane), inc);
+}
+
+static void dispc_ovl_set_color_mode(enum omap_plane plane,
+		enum omap_color_mode color_mode)
+{
+	u32 m = 0;
+	if (plane != OMAP_DSS_GFX) {
+		switch (color_mode) {
+		case OMAP_DSS_COLOR_NV12:
+			m = 0x0; break;
+		case OMAP_DSS_COLOR_RGBX16:
+			m = 0x1; break;
+		case OMAP_DSS_COLOR_RGBA16:
+			m = 0x2; break;
+		case OMAP_DSS_COLOR_RGB12U:
+			m = 0x4; break;
+		case OMAP_DSS_COLOR_ARGB16:
+			m = 0x5; break;
+		case OMAP_DSS_COLOR_RGB16:
+			m = 0x6; break;
+		case OMAP_DSS_COLOR_ARGB16_1555:
+			m = 0x7; break;
+		case OMAP_DSS_COLOR_RGB24U:
+			m = 0x8; break;
+		case OMAP_DSS_COLOR_RGB24P:
+			m = 0x9; break;
+		case OMAP_DSS_COLOR_YUV2:
+			m = 0xa; break;
+		case OMAP_DSS_COLOR_UYVY:
+			m = 0xb; break;
+		case OMAP_DSS_COLOR_ARGB32:
+			m = 0xc; break;
+		case OMAP_DSS_COLOR_RGBA32:
+			m = 0xd; break;
+		case OMAP_DSS_COLOR_RGBX32:
+			m = 0xe; break;
+		case OMAP_DSS_COLOR_XRGB16_1555:
+			m = 0xf; break;
+		default:
+			BUG(); return;
+		}
+	} else {
+		switch (color_mode) {
+		case OMAP_DSS_COLOR_CLUT1:
+			m = 0x0; break;
+		case OMAP_DSS_COLOR_CLUT2:
+			m = 0x1; break;
+		case OMAP_DSS_COLOR_CLUT4:
+			m = 0x2; break;
+		case OMAP_DSS_COLOR_CLUT8:
+			m = 0x3; break;
+		case OMAP_DSS_COLOR_RGB12U:
+			m = 0x4; break;
+		case OMAP_DSS_COLOR_ARGB16:
+			m = 0x5; break;
+		case OMAP_DSS_COLOR_RGB16:
+			m = 0x6; break;
+		case OMAP_DSS_COLOR_ARGB16_1555:
+			m = 0x7; break;
+		case OMAP_DSS_COLOR_RGB24U:
+			m = 0x8; break;
+		case OMAP_DSS_COLOR_RGB24P:
+			m = 0x9; break;
+		case OMAP_DSS_COLOR_RGBX16:
+			m = 0xa; break;
+		case OMAP_DSS_COLOR_RGBA16:
+			m = 0xb; break;
+		case OMAP_DSS_COLOR_ARGB32:
+			m = 0xc; break;
+		case OMAP_DSS_COLOR_RGBA32:
+			m = 0xd; break;
+		case OMAP_DSS_COLOR_RGBX32:
+			m = 0xe; break;
+		case OMAP_DSS_COLOR_XRGB16_1555:
+			m = 0xf; break;
+		default:
+			BUG(); return;
+		}
+	}
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), m, 4, 1);
+}
+
+static void dispc_ovl_configure_burst_type(enum omap_plane plane,
+		enum omap_dss_rotation_type rotation_type)
+{
+	if (dss_has_feature(FEAT_BURST_2D) == 0)
+		return;
+
+	if (rotation_type == OMAP_DSS_ROT_TILER)
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), 1, 29, 29);
+	else
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), 0, 29, 29);
+}
+
+void dispc_ovl_set_channel_out(enum omap_plane plane, enum omap_channel channel)
+{
+	int shift;
+	u32 val;
+	int chan = 0, chan2 = 0;
+
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		shift = 8;
+		break;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+	case OMAP_DSS_VIDEO3:
+		shift = 16;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+	if (dss_has_feature(FEAT_MGR_LCD2)) {
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+			chan = 0;
+			chan2 = 0;
+			break;
+		case OMAP_DSS_CHANNEL_DIGIT:
+			chan = 1;
+			chan2 = 0;
+			break;
+		case OMAP_DSS_CHANNEL_LCD2:
+			chan = 0;
+			chan2 = 1;
+			break;
+		case OMAP_DSS_CHANNEL_LCD3:
+			if (dss_has_feature(FEAT_MGR_LCD3)) {
+				chan = 0;
+				chan2 = 2;
+			} else {
+				BUG();
+				return;
+			}
+			break;
+		case OMAP_DSS_CHANNEL_WB:
+			chan = 0;
+			chan2 = 3;
+			break;
+		default:
+			BUG();
+			return;
+		}
+
+		val = FLD_MOD(val, chan, shift, shift);
+		val = FLD_MOD(val, chan2, 31, 30);
+	} else {
+		val = FLD_MOD(val, channel, shift, shift);
+	}
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
+}
+EXPORT_SYMBOL(dispc_ovl_set_channel_out);
+
+static enum omap_channel dispc_ovl_get_channel_out(enum omap_plane plane)
+{
+	int shift;
+	u32 val;
+
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		shift = 8;
+		break;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+	case OMAP_DSS_VIDEO3:
+		shift = 16;
+		break;
+	default:
+		BUG();
+		return 0;
+	}
+
+	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+
+	if (FLD_GET(val, shift, shift) == 1)
+		return OMAP_DSS_CHANNEL_DIGIT;
+
+	if (!dss_has_feature(FEAT_MGR_LCD2))
+		return OMAP_DSS_CHANNEL_LCD;
+
+	switch (FLD_GET(val, 31, 30)) {
+	case 0:
+	default:
+		return OMAP_DSS_CHANNEL_LCD;
+	case 1:
+		return OMAP_DSS_CHANNEL_LCD2;
+	case 2:
+		return OMAP_DSS_CHANNEL_LCD3;
+	case 3:
+		return OMAP_DSS_CHANNEL_WB;
+	}
+}
+
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel)
+{
+	enum omap_plane plane = OMAP_DSS_WB;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), channel, 18, 16);
+}
+
+static void dispc_ovl_set_burst_size(enum omap_plane plane,
+		enum omap_burst_size burst_size)
+{
+	static const unsigned shifts[] = { 6, 14, 14, 14, 14, };
+	int shift;
+
+	shift = shifts[plane];
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), burst_size, shift + 1, shift);
+}
+
+static void dispc_configure_burst_sizes(void)
+{
+	int i;
+	const int burst_size = BURST_SIZE_X8;
+
+	/* Configure burst size always to maximum size */
+	for (i = 0; i < dss_feat_get_num_ovls(); ++i)
+		dispc_ovl_set_burst_size(i, burst_size);
+	if (dispc.feat->has_writeback)
+		dispc_ovl_set_burst_size(OMAP_DSS_WB, burst_size);
+}
+
+static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
+{
+	unsigned unit = dss_feat_get_burst_size_unit();
+	/* burst multiplier is always x8 (see dispc_configure_burst_sizes()) */
+	return unit * 8;
+}
+
+void dispc_enable_gamma_table(bool enable)
+{
+	/*
+	 * This is partially implemented to support only disabling of
+	 * the gamma table.
+	 */
+	if (enable) {
+		DSSWARN("Gamma table enabling for TV not yet supported");
+		return;
+	}
+
+	REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
+}
+
+static void dispc_mgr_enable_cpr(enum omap_channel channel, bool enable)
+{
+	if (channel == OMAP_DSS_CHANNEL_DIGIT)
+		return;
+
+	mgr_fld_write(channel, DISPC_MGR_FLD_CPR, enable);
+}
+
+static void dispc_mgr_set_cpr_coef(enum omap_channel channel,
+		const struct omap_dss_cpr_coefs *coefs)
+{
+	u32 coef_r, coef_g, coef_b;
+
+	if (!dss_mgr_is_lcd(channel))
+		return;
+
+	coef_r = FLD_VAL(coefs->rr, 31, 22) | FLD_VAL(coefs->rg, 20, 11) |
+		FLD_VAL(coefs->rb, 9, 0);
+	coef_g = FLD_VAL(coefs->gr, 31, 22) | FLD_VAL(coefs->gg, 20, 11) |
+		FLD_VAL(coefs->gb, 9, 0);
+	coef_b = FLD_VAL(coefs->br, 31, 22) | FLD_VAL(coefs->bg, 20, 11) |
+		FLD_VAL(coefs->bb, 9, 0);
+
+	dispc_write_reg(DISPC_CPR_COEF_R(channel), coef_r);
+	dispc_write_reg(DISPC_CPR_COEF_G(channel), coef_g);
+	dispc_write_reg(DISPC_CPR_COEF_B(channel), coef_b);
+}
+
+static void dispc_ovl_set_vid_color_conv(enum omap_plane plane, bool enable)
+{
+	u32 val;
+
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	val = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+	val = FLD_MOD(val, enable, 9, 9);
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), val);
+}
+
+static void dispc_ovl_enable_replication(enum omap_plane plane,
+		enum omap_overlay_caps caps, bool enable)
+{
+	static const unsigned shifts[] = { 5, 10, 10, 10 };
+	int shift;
+
+	if ((caps & OMAP_DSS_OVL_CAP_REPLICATION) == 0)
+		return;
+
+	shift = shifts[plane];
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, shift, shift);
+}
+
+static void dispc_mgr_set_size(enum omap_channel channel, u16 width,
+		u16 height)
+{
+	u32 val;
+
+	val = FLD_VAL(height - 1, dispc.feat->mgr_height_start, 16) |
+		FLD_VAL(width - 1, dispc.feat->mgr_width_start, 0);
+
+	dispc_write_reg(DISPC_SIZE_MGR(channel), val);
+}
+
+static void dispc_init_fifos(void)
+{
+	u32 size;
+	int fifo;
+	u8 start, end;
+	u32 unit;
+	int i;
+
+	unit = dss_feat_get_buffer_size_unit();
+
+	dss_feat_get_reg_field(FEAT_REG_FIFOSIZE, &start, &end);
+
+	for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+		size = REG_GET(DISPC_OVL_FIFO_SIZE_STATUS(fifo), start, end);
+		size *= unit;
+		dispc.fifo_size[fifo] = size;
+
+		/*
+		 * By default fifos are mapped directly to overlays, fifo 0 to
+		 * ovl 0, fifo 1 to ovl 1, etc.
+		 */
+		dispc.fifo_assignment[fifo] = fifo;
+	}
+
+	/*
+	 * The GFX fifo on OMAP4 is smaller than the other fifos. The small fifo
+	 * causes problems with certain use cases, like using the tiler in 2D
+	 * mode. The below hack swaps the fifos of GFX and WB planes, thus
+	 * giving GFX plane a larger fifo. WB but should work fine with a
+	 * smaller fifo.
+	 */
+	if (dispc.feat->gfx_fifo_workaround) {
+		u32 v;
+
+		v = dispc_read_reg(DISPC_GLOBAL_BUFFER);
+
+		v = FLD_MOD(v, 4, 2, 0); /* GFX BUF top to WB */
+		v = FLD_MOD(v, 4, 5, 3); /* GFX BUF bottom to WB */
+		v = FLD_MOD(v, 0, 26, 24); /* WB BUF top to GFX */
+		v = FLD_MOD(v, 0, 29, 27); /* WB BUF bottom to GFX */
+
+		dispc_write_reg(DISPC_GLOBAL_BUFFER, v);
+
+		dispc.fifo_assignment[OMAP_DSS_GFX] = OMAP_DSS_WB;
+		dispc.fifo_assignment[OMAP_DSS_WB] = OMAP_DSS_GFX;
+	}
+
+	/*
+	 * Setup default fifo thresholds.
+	 */
+	for (i = 0; i < dss_feat_get_num_ovls(); ++i) {
+		u32 low, high;
+		const bool use_fifomerge = false;
+		const bool manual_update = false;
+
+		dispc_ovl_compute_fifo_thresholds(i, &low, &high,
+			use_fifomerge, manual_update);
+
+		dispc_ovl_set_fifo_threshold(i, low, high);
+	}
+
+	if (dispc.feat->has_writeback) {
+		u32 low, high;
+		const bool use_fifomerge = false;
+		const bool manual_update = false;
+
+		dispc_ovl_compute_fifo_thresholds(OMAP_DSS_WB, &low, &high,
+			use_fifomerge, manual_update);
+
+		dispc_ovl_set_fifo_threshold(OMAP_DSS_WB, low, high);
+	}
+}
+
+static u32 dispc_ovl_get_fifo_size(enum omap_plane plane)
+{
+	int fifo;
+	u32 size = 0;
+
+	for (fifo = 0; fifo < dispc.feat->num_fifos; ++fifo) {
+		if (dispc.fifo_assignment[fifo] == plane)
+			size += dispc.fifo_size[fifo];
+	}
+
+	return size;
+}
+
+void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high)
+{
+	u8 hi_start, hi_end, lo_start, lo_end;
+	u32 unit;
+
+	unit = dss_feat_get_buffer_size_unit();
+
+	WARN_ON(low % unit != 0);
+	WARN_ON(high % unit != 0);
+
+	low /= unit;
+	high /= unit;
+
+	dss_feat_get_reg_field(FEAT_REG_FIFOHIGHTHRESHOLD, &hi_start, &hi_end);
+	dss_feat_get_reg_field(FEAT_REG_FIFOLOWTHRESHOLD, &lo_start, &lo_end);
+
+	DSSDBG("fifo(%d) threshold (bytes), old %u/%u, new %u/%u\n",
+			plane,
+			REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+				lo_start, lo_end) * unit,
+			REG_GET(DISPC_OVL_FIFO_THRESHOLD(plane),
+				hi_start, hi_end) * unit,
+			low * unit, high * unit);
+
+	dispc_write_reg(DISPC_OVL_FIFO_THRESHOLD(plane),
+			FLD_VAL(high, hi_start, hi_end) |
+			FLD_VAL(low, lo_start, lo_end));
+
+	/*
+	 * configure the preload to the pipeline's high threhold, if HT it's too
+	 * large for the preload field, set the threshold to the maximum value
+	 * that can be held by the preload register
+	 */
+	if (dss_has_feature(FEAT_PRELOAD) && dispc.feat->set_max_preload &&
+			plane != OMAP_DSS_WB)
+		dispc_write_reg(DISPC_OVL_PRELOAD(plane), min(high, 0xfffu));
+}
+
+void dispc_enable_fifomerge(bool enable)
+{
+	if (!dss_has_feature(FEAT_FIFO_MERGE)) {
+		WARN_ON(enable);
+		return;
+	}
+
+	DSSDBG("FIFO merge %s\n", enable ? "enabled" : "disabled");
+	REG_FLD_MOD(DISPC_CONFIG, enable ? 1 : 0, 14, 14);
+}
+
+void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
+		u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
+		bool manual_update)
+{
+	/*
+	 * All sizes are in bytes. Both the buffer and burst are made of
+	 * buffer_units, and the fifo thresholds must be buffer_unit aligned.
+	 */
+
+	unsigned buf_unit = dss_feat_get_buffer_size_unit();
+	unsigned ovl_fifo_size, total_fifo_size, burst_size;
+	int i;
+
+	burst_size = dispc_ovl_get_burst_size(plane);
+	ovl_fifo_size = dispc_ovl_get_fifo_size(plane);
+
+	if (use_fifomerge) {
+		total_fifo_size = 0;
+		for (i = 0; i < dss_feat_get_num_ovls(); ++i)
+			total_fifo_size += dispc_ovl_get_fifo_size(i);
+	} else {
+		total_fifo_size = ovl_fifo_size;
+	}
+
+	/*
+	 * We use the same low threshold for both fifomerge and non-fifomerge
+	 * cases, but for fifomerge we calculate the high threshold using the
+	 * combined fifo size
+	 */
+
+	if (manual_update && dss_has_feature(FEAT_OMAP3_DSI_FIFO_BUG)) {
+		*fifo_low = ovl_fifo_size - burst_size * 2;
+		*fifo_high = total_fifo_size - burst_size;
+	} else if (plane == OMAP_DSS_WB) {
+		/*
+		 * Most optimal configuration for writeback is to push out data
+		 * to the interconnect the moment writeback pushes enough pixels
+		 * in the FIFO to form a burst
+		 */
+		*fifo_low = 0;
+		*fifo_high = burst_size;
+	} else {
+		*fifo_low = ovl_fifo_size - burst_size;
+		*fifo_high = total_fifo_size - buf_unit;
+	}
+}
+
+static void dispc_ovl_set_mflag(enum omap_plane plane, bool enable)
+{
+	int bit;
+
+	if (plane == OMAP_DSS_GFX)
+		bit = 14;
+	else
+		bit = 23;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable, bit, bit);
+}
+
+static void dispc_ovl_set_mflag_threshold(enum omap_plane plane,
+	int low, int high)
+{
+	dispc_write_reg(DISPC_OVL_MFLAG_THRESHOLD(plane),
+		FLD_VAL(high, 31, 16) |	FLD_VAL(low, 15, 0));
+}
+
+static void dispc_init_mflag(void)
+{
+	int i;
+
+	/*
+	 * HACK: NV12 color format and MFLAG seem to have problems working
+	 * together: using two displays, and having an NV12 overlay on one of
+	 * the displays will cause underflows/synclosts when MFLAG_CTRL=2.
+	 * Changing MFLAG thresholds and PRELOAD to certain values seem to
+	 * remove the errors, but there doesn't seem to be a clear logic on
+	 * which values work and which not.
+	 *
+	 * As a work-around, set force MFLAG to always on.
+	 */
+	dispc_write_reg(DISPC_GLOBAL_MFLAG_ATTRIBUTE,
+		(1 << 0) |	/* MFLAG_CTRL = force always on */
+		(0 << 2));	/* MFLAG_START = disable */
+
+	for (i = 0; i < dss_feat_get_num_ovls(); ++i) {
+		u32 size = dispc_ovl_get_fifo_size(i);
+		u32 unit = dss_feat_get_buffer_size_unit();
+		u32 low, high;
+
+		dispc_ovl_set_mflag(i, true);
+
+		/*
+		 * Simulation team suggests below thesholds:
+		 * HT = fifosize * 5 / 8;
+		 * LT = fifosize * 4 / 8;
+		 */
+
+		low = size * 4 / 8 / unit;
+		high = size * 5 / 8 / unit;
+
+		dispc_ovl_set_mflag_threshold(i, low, high);
+	}
+
+	if (dispc.feat->has_writeback) {
+		u32 size = dispc_ovl_get_fifo_size(OMAP_DSS_WB);
+		u32 unit = dss_feat_get_buffer_size_unit();
+		u32 low, high;
+
+		dispc_ovl_set_mflag(OMAP_DSS_WB, true);
+
+		/*
+		 * Simulation team suggests below thesholds:
+		 * HT = fifosize * 5 / 8;
+		 * LT = fifosize * 4 / 8;
+		 */
+
+		low = size * 4 / 8 / unit;
+		high = size * 5 / 8 / unit;
+
+		dispc_ovl_set_mflag_threshold(OMAP_DSS_WB, low, high);
+	}
+}
+
+static void dispc_ovl_set_fir(enum omap_plane plane,
+				int hinc, int vinc,
+				enum omap_color_component color_comp)
+{
+	u32 val;
+
+	if (color_comp == DISPC_COLOR_COMPONENT_RGB_Y) {
+		u8 hinc_start, hinc_end, vinc_start, vinc_end;
+
+		dss_feat_get_reg_field(FEAT_REG_FIRHINC,
+					&hinc_start, &hinc_end);
+		dss_feat_get_reg_field(FEAT_REG_FIRVINC,
+					&vinc_start, &vinc_end);
+		val = FLD_VAL(vinc, vinc_start, vinc_end) |
+				FLD_VAL(hinc, hinc_start, hinc_end);
+
+		dispc_write_reg(DISPC_OVL_FIR(plane), val);
+	} else {
+		val = FLD_VAL(vinc, 28, 16) | FLD_VAL(hinc, 12, 0);
+		dispc_write_reg(DISPC_OVL_FIR2(plane), val);
+	}
+}
+
+static void dispc_ovl_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
+{
+	u32 val;
+	u8 hor_start, hor_end, vert_start, vert_end;
+
+	dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+	dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+	val = FLD_VAL(vaccu, vert_start, vert_end) |
+			FLD_VAL(haccu, hor_start, hor_end);
+
+	dispc_write_reg(DISPC_OVL_ACCU0(plane), val);
+}
+
+static void dispc_ovl_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
+{
+	u32 val;
+	u8 hor_start, hor_end, vert_start, vert_end;
+
+	dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+	dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+	val = FLD_VAL(vaccu, vert_start, vert_end) |
+			FLD_VAL(haccu, hor_start, hor_end);
+
+	dispc_write_reg(DISPC_OVL_ACCU1(plane), val);
+}
+
+static void dispc_ovl_set_vid_accu2_0(enum omap_plane plane, int haccu,
+		int vaccu)
+{
+	u32 val;
+
+	val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+	dispc_write_reg(DISPC_OVL_ACCU2_0(plane), val);
+}
+
+static void dispc_ovl_set_vid_accu2_1(enum omap_plane plane, int haccu,
+		int vaccu)
+{
+	u32 val;
+
+	val = FLD_VAL(vaccu, 26, 16) | FLD_VAL(haccu, 10, 0);
+	dispc_write_reg(DISPC_OVL_ACCU2_1(plane), val);
+}
+
+static void dispc_ovl_set_scale_param(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool five_taps, u8 rotation,
+		enum omap_color_component color_comp)
+{
+	int fir_hinc, fir_vinc;
+
+	fir_hinc = 1024 * orig_width / out_width;
+	fir_vinc = 1024 * orig_height / out_height;
+
+	dispc_ovl_set_scale_coef(plane, fir_hinc, fir_vinc, five_taps,
+				color_comp);
+	dispc_ovl_set_fir(plane, fir_hinc, fir_vinc, color_comp);
+}
+
+static void dispc_ovl_set_accu_uv(enum omap_plane plane,
+		u16 orig_width,	u16 orig_height, u16 out_width, u16 out_height,
+		bool ilace, enum omap_color_mode color_mode, u8 rotation)
+{
+	int h_accu2_0, h_accu2_1;
+	int v_accu2_0, v_accu2_1;
+	int chroma_hinc, chroma_vinc;
+	int idx;
+
+	struct accu {
+		s8 h0_m, h0_n;
+		s8 h1_m, h1_n;
+		s8 v0_m, v0_n;
+		s8 v1_m, v1_n;
+	};
+
+	const struct accu *accu_table;
+	const struct accu *accu_val;
+
+	static const struct accu accu_nv12[4] = {
+		{  0, 1,  0, 1 , -1, 2, 0, 1 },
+		{  1, 2, -3, 4 ,  0, 1, 0, 1 },
+		{ -1, 1,  0, 1 , -1, 2, 0, 1 },
+		{ -1, 2, -1, 2 , -1, 1, 0, 1 },
+	};
+
+	static const struct accu accu_nv12_ilace[4] = {
+		{  0, 1,  0, 1 , -3, 4, -1, 4 },
+		{ -1, 4, -3, 4 ,  0, 1,  0, 1 },
+		{ -1, 1,  0, 1 , -1, 4, -3, 4 },
+		{ -3, 4, -3, 4 , -1, 1,  0, 1 },
+	};
+
+	static const struct accu accu_yuv[4] = {
+		{  0, 1, 0, 1,  0, 1, 0, 1 },
+		{  0, 1, 0, 1,  0, 1, 0, 1 },
+		{ -1, 1, 0, 1,  0, 1, 0, 1 },
+		{  0, 1, 0, 1, -1, 1, 0, 1 },
+	};
+
+	switch (rotation) {
+	case OMAP_DSS_ROT_0:
+		idx = 0;
+		break;
+	case OMAP_DSS_ROT_90:
+		idx = 1;
+		break;
+	case OMAP_DSS_ROT_180:
+		idx = 2;
+		break;
+	case OMAP_DSS_ROT_270:
+		idx = 3;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_NV12:
+		if (ilace)
+			accu_table = accu_nv12_ilace;
+		else
+			accu_table = accu_nv12;
+		break;
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+		accu_table = accu_yuv;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	accu_val = &accu_table[idx];
+
+	chroma_hinc = 1024 * orig_width / out_width;
+	chroma_vinc = 1024 * orig_height / out_height;
+
+	h_accu2_0 = (accu_val->h0_m * chroma_hinc / accu_val->h0_n) % 1024;
+	h_accu2_1 = (accu_val->h1_m * chroma_hinc / accu_val->h1_n) % 1024;
+	v_accu2_0 = (accu_val->v0_m * chroma_vinc / accu_val->v0_n) % 1024;
+	v_accu2_1 = (accu_val->v1_m * chroma_vinc / accu_val->v1_n) % 1024;
+
+	dispc_ovl_set_vid_accu2_0(plane, h_accu2_0, v_accu2_0);
+	dispc_ovl_set_vid_accu2_1(plane, h_accu2_1, v_accu2_1);
+}
+
+static void dispc_ovl_set_scaling_common(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	int accu0 = 0;
+	int accu1 = 0;
+	u32 l;
+
+	dispc_ovl_set_scale_param(plane, orig_width, orig_height,
+				out_width, out_height, five_taps,
+				rotation, DISPC_COLOR_COMPONENT_RGB_Y);
+	l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+
+	/* RESIZEENABLE and VERTICALTAPS */
+	l &= ~((0x3 << 5) | (0x1 << 21));
+	l |= (orig_width != out_width) ? (1 << 5) : 0;
+	l |= (orig_height != out_height) ? (1 << 6) : 0;
+	l |= five_taps ? (1 << 21) : 0;
+
+	/* VRESIZECONF and HRESIZECONF */
+	if (dss_has_feature(FEAT_RESIZECONF)) {
+		l &= ~(0x3 << 7);
+		l |= (orig_width <= out_width) ? 0 : (1 << 7);
+		l |= (orig_height <= out_height) ? 0 : (1 << 8);
+	}
+
+	/* LINEBUFFERSPLIT */
+	if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+		l &= ~(0x1 << 22);
+		l |= five_taps ? (1 << 22) : 0;
+	}
+
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+	/*
+	 * field 0 = even field = bottom field
+	 * field 1 = odd field = top field
+	 */
+	if (ilace && !fieldmode) {
+		accu1 = 0;
+		accu0 = ((1024 * orig_height / out_height) / 2) & 0x3ff;
+		if (accu0 >= 1024/2) {
+			accu1 = 1024/2;
+			accu0 -= accu1;
+		}
+	}
+
+	dispc_ovl_set_vid_accu0(plane, 0, accu0);
+	dispc_ovl_set_vid_accu1(plane, 0, accu1);
+}
+
+static void dispc_ovl_set_scaling_uv(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	int scale_x = out_width != orig_width;
+	int scale_y = out_height != orig_height;
+	bool chroma_upscale = plane != OMAP_DSS_WB ? true : false;
+
+	if (!dss_has_feature(FEAT_HANDLE_UV_SEPARATE))
+		return;
+	if ((color_mode != OMAP_DSS_COLOR_YUV2 &&
+			color_mode != OMAP_DSS_COLOR_UYVY &&
+			color_mode != OMAP_DSS_COLOR_NV12)) {
+		/* reset chroma resampling for RGB formats  */
+		if (plane != OMAP_DSS_WB)
+			REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 8, 8);
+		return;
+	}
+
+	dispc_ovl_set_accu_uv(plane, orig_width, orig_height, out_width,
+			out_height, ilace, color_mode, rotation);
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_NV12:
+		if (chroma_upscale) {
+			/* UV is subsampled by 2 horizontally and vertically */
+			orig_height >>= 1;
+			orig_width >>= 1;
+		} else {
+			/* UV is downsampled by 2 horizontally and vertically */
+			orig_height <<= 1;
+			orig_width <<= 1;
+		}
+
+		break;
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+		/* For YUV422 with 90/270 rotation, we don't upsample chroma */
+		if (rotation == OMAP_DSS_ROT_0 ||
+				rotation == OMAP_DSS_ROT_180) {
+			if (chroma_upscale)
+				/* UV is subsampled by 2 horizontally */
+				orig_width >>= 1;
+			else
+				/* UV is downsampled by 2 horizontally */
+				orig_width <<= 1;
+		}
+
+		/* must use FIR for YUV422 if rotated */
+		if (rotation != OMAP_DSS_ROT_0)
+			scale_x = scale_y = true;
+
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	if (out_width != orig_width)
+		scale_x = true;
+	if (out_height != orig_height)
+		scale_y = true;
+
+	dispc_ovl_set_scale_param(plane, orig_width, orig_height,
+			out_width, out_height, five_taps,
+				rotation, DISPC_COLOR_COMPONENT_UV);
+
+	if (plane != OMAP_DSS_WB)
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane),
+			(scale_x || scale_y) ? 1 : 0, 8, 8);
+
+	/* set H scaling */
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_x ? 1 : 0, 5, 5);
+	/* set V scaling */
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), scale_y ? 1 : 0, 6, 6);
+}
+
+static void dispc_ovl_set_scaling(enum omap_plane plane,
+		u16 orig_width, u16 orig_height,
+		u16 out_width, u16 out_height,
+		bool ilace, bool five_taps,
+		bool fieldmode, enum omap_color_mode color_mode,
+		u8 rotation)
+{
+	BUG_ON(plane == OMAP_DSS_GFX);
+
+	dispc_ovl_set_scaling_common(plane,
+			orig_width, orig_height,
+			out_width, out_height,
+			ilace, five_taps,
+			fieldmode, color_mode,
+			rotation);
+
+	dispc_ovl_set_scaling_uv(plane,
+		orig_width, orig_height,
+		out_width, out_height,
+		ilace, five_taps,
+		fieldmode, color_mode,
+		rotation);
+}
+
+static void dispc_ovl_set_rotation_attrs(enum omap_plane plane, u8 rotation,
+		enum omap_dss_rotation_type rotation_type,
+		bool mirroring, enum omap_color_mode color_mode)
+{
+	bool row_repeat = false;
+	int vidrot = 0;
+
+	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY) {
+
+		if (mirroring) {
+			switch (rotation) {
+			case OMAP_DSS_ROT_0:
+				vidrot = 2;
+				break;
+			case OMAP_DSS_ROT_90:
+				vidrot = 1;
+				break;
+			case OMAP_DSS_ROT_180:
+				vidrot = 0;
+				break;
+			case OMAP_DSS_ROT_270:
+				vidrot = 3;
+				break;
+			}
+		} else {
+			switch (rotation) {
+			case OMAP_DSS_ROT_0:
+				vidrot = 0;
+				break;
+			case OMAP_DSS_ROT_90:
+				vidrot = 1;
+				break;
+			case OMAP_DSS_ROT_180:
+				vidrot = 2;
+				break;
+			case OMAP_DSS_ROT_270:
+				vidrot = 3;
+				break;
+			}
+		}
+
+		if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
+			row_repeat = true;
+		else
+			row_repeat = false;
+	}
+
+	/*
+	 * OMAP4/5 Errata i631:
+	 * NV12 in 1D mode must use ROTATION=1. Otherwise DSS will fetch extra
+	 * rows beyond the framebuffer, which may cause OCP error.
+	 */
+	if (color_mode == OMAP_DSS_COLOR_NV12 &&
+			rotation_type != OMAP_DSS_ROT_TILER)
+		vidrot = 1;
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), vidrot, 13, 12);
+	if (dss_has_feature(FEAT_ROWREPEATENABLE))
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane),
+			row_repeat ? 1 : 0, 18, 18);
+
+	if (color_mode == OMAP_DSS_COLOR_NV12) {
+		bool doublestride = (rotation_type == OMAP_DSS_ROT_TILER) &&
+					(rotation == OMAP_DSS_ROT_0 ||
+					rotation == OMAP_DSS_ROT_180);
+		/* DOUBLESTRIDE */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), doublestride, 22, 22);
+	}
+
+}
+
+static int color_mode_to_bpp(enum omap_color_mode color_mode)
+{
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_CLUT1:
+		return 1;
+	case OMAP_DSS_COLOR_CLUT2:
+		return 2;
+	case OMAP_DSS_COLOR_CLUT4:
+		return 4;
+	case OMAP_DSS_COLOR_CLUT8:
+	case OMAP_DSS_COLOR_NV12:
+		return 8;
+	case OMAP_DSS_COLOR_RGB12U:
+	case OMAP_DSS_COLOR_RGB16:
+	case OMAP_DSS_COLOR_ARGB16:
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_RGBA16:
+	case OMAP_DSS_COLOR_RGBX16:
+	case OMAP_DSS_COLOR_ARGB16_1555:
+	case OMAP_DSS_COLOR_XRGB16_1555:
+		return 16;
+	case OMAP_DSS_COLOR_RGB24P:
+		return 24;
+	case OMAP_DSS_COLOR_RGB24U:
+	case OMAP_DSS_COLOR_ARGB32:
+	case OMAP_DSS_COLOR_RGBA32:
+	case OMAP_DSS_COLOR_RGBX32:
+		return 32;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static s32 pixinc(int pixels, u8 ps)
+{
+	if (pixels == 1)
+		return 1;
+	else if (pixels > 1)
+		return 1 + (pixels - 1) * ps;
+	else if (pixels < 0)
+		return 1 - (-pixels + 1) * ps;
+	else
+		BUG();
+		return 0;
+}
+
+static void calc_vrfb_rotation_offset(u8 rotation, bool mirror,
+		u16 screen_width,
+		u16 width, u16 height,
+		enum omap_color_mode color_mode, bool fieldmode,
+		unsigned int field_offset,
+		unsigned *offset0, unsigned *offset1,
+		s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+	u8 ps;
+
+	/* FIXME CLUT formats */
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_CLUT1:
+	case OMAP_DSS_COLOR_CLUT2:
+	case OMAP_DSS_COLOR_CLUT4:
+	case OMAP_DSS_COLOR_CLUT8:
+		BUG();
+		return;
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+		ps = 4;
+		break;
+	default:
+		ps = color_mode_to_bpp(color_mode) / 8;
+		break;
+	}
+
+	DSSDBG("calc_rot(%d): scrw %d, %dx%d\n", rotation, screen_width,
+			width, height);
+
+	/*
+	 * field 0 = even field = bottom field
+	 * field 1 = odd field = top field
+	 */
+	switch (rotation + mirror * 4) {
+	case OMAP_DSS_ROT_0:
+	case OMAP_DSS_ROT_180:
+		/*
+		 * If the pixel format is YUV or UYVY divide the width
+		 * of the image by 2 for 0 and 180 degree rotation.
+		 */
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			width = width >> 1;
+	case OMAP_DSS_ROT_90:
+	case OMAP_DSS_ROT_270:
+		*offset1 = 0;
+		if (field_offset)
+			*offset0 = field_offset * screen_width * ps;
+		else
+			*offset0 = 0;
+
+		*row_inc = pixinc(1 +
+			(y_predecim * screen_width - x_predecim * width) +
+			(fieldmode ? screen_width : 0), ps);
+		*pix_inc = pixinc(x_predecim, ps);
+		break;
+
+	case OMAP_DSS_ROT_0 + 4:
+	case OMAP_DSS_ROT_180 + 4:
+		/* If the pixel format is YUV or UYVY divide the width
+		 * of the image by 2  for 0 degree and 180 degree
+		 */
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			width = width >> 1;
+	case OMAP_DSS_ROT_90 + 4:
+	case OMAP_DSS_ROT_270 + 4:
+		*offset1 = 0;
+		if (field_offset)
+			*offset0 = field_offset * screen_width * ps;
+		else
+			*offset0 = 0;
+		*row_inc = pixinc(1 -
+			(y_predecim * screen_width + x_predecim * width) -
+			(fieldmode ? screen_width : 0), ps);
+		*pix_inc = pixinc(x_predecim, ps);
+		break;
+
+	default:
+		BUG();
+		return;
+	}
+}
+
+static void calc_dma_rotation_offset(u8 rotation, bool mirror,
+		u16 screen_width,
+		u16 width, u16 height,
+		enum omap_color_mode color_mode, bool fieldmode,
+		unsigned int field_offset,
+		unsigned *offset0, unsigned *offset1,
+		s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+	u8 ps;
+	u16 fbw, fbh;
+
+	/* FIXME CLUT formats */
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_CLUT1:
+	case OMAP_DSS_COLOR_CLUT2:
+	case OMAP_DSS_COLOR_CLUT4:
+	case OMAP_DSS_COLOR_CLUT8:
+		BUG();
+		return;
+	default:
+		ps = color_mode_to_bpp(color_mode) / 8;
+		break;
+	}
+
+	DSSDBG("calc_rot(%d): scrw %d, %dx%d\n", rotation, screen_width,
+			width, height);
+
+	/* width & height are overlay sizes, convert to fb sizes */
+
+	if (rotation == OMAP_DSS_ROT_0 || rotation == OMAP_DSS_ROT_180) {
+		fbw = width;
+		fbh = height;
+	} else {
+		fbw = height;
+		fbh = width;
+	}
+
+	/*
+	 * field 0 = even field = bottom field
+	 * field 1 = odd field = top field
+	 */
+	switch (rotation + mirror * 4) {
+	case OMAP_DSS_ROT_0:
+		*offset1 = 0;
+		if (field_offset)
+			*offset0 = *offset1 + field_offset * screen_width * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(1 +
+			(y_predecim * screen_width - fbw * x_predecim) +
+			(fieldmode ? screen_width : 0),	ps);
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			*pix_inc = pixinc(x_predecim, 2 * ps);
+		else
+			*pix_inc = pixinc(x_predecim, ps);
+		break;
+	case OMAP_DSS_ROT_90:
+		*offset1 = screen_width * (fbh - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 + field_offset * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(screen_width * (fbh * x_predecim - 1) +
+				y_predecim + (fieldmode ? 1 : 0), ps);
+		*pix_inc = pixinc(-x_predecim * screen_width, ps);
+		break;
+	case OMAP_DSS_ROT_180:
+		*offset1 = (screen_width * (fbh - 1) + fbw - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 - field_offset * screen_width * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(-1 -
+			(y_predecim * screen_width - fbw * x_predecim) -
+			(fieldmode ? screen_width : 0),	ps);
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			*pix_inc = pixinc(-x_predecim, 2 * ps);
+		else
+			*pix_inc = pixinc(-x_predecim, ps);
+		break;
+	case OMAP_DSS_ROT_270:
+		*offset1 = (fbw - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 - field_offset * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(-screen_width * (fbh * x_predecim - 1) -
+				y_predecim - (fieldmode ? 1 : 0), ps);
+		*pix_inc = pixinc(x_predecim * screen_width, ps);
+		break;
+
+	/* mirroring */
+	case OMAP_DSS_ROT_0 + 4:
+		*offset1 = (fbw - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 + field_offset * screen_width * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(y_predecim * screen_width * 2 - 1 +
+				(fieldmode ? screen_width : 0),
+				ps);
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			*pix_inc = pixinc(-x_predecim, 2 * ps);
+		else
+			*pix_inc = pixinc(-x_predecim, ps);
+		break;
+
+	case OMAP_DSS_ROT_90 + 4:
+		*offset1 = 0;
+		if (field_offset)
+			*offset0 = *offset1 + field_offset * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(-screen_width * (fbh * x_predecim - 1) +
+				y_predecim + (fieldmode ? 1 : 0),
+				ps);
+		*pix_inc = pixinc(x_predecim * screen_width, ps);
+		break;
+
+	case OMAP_DSS_ROT_180 + 4:
+		*offset1 = screen_width * (fbh - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 - field_offset * screen_width * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(1 - y_predecim * screen_width * 2 -
+				(fieldmode ? screen_width : 0),
+				ps);
+		if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY)
+			*pix_inc = pixinc(x_predecim, 2 * ps);
+		else
+			*pix_inc = pixinc(x_predecim, ps);
+		break;
+
+	case OMAP_DSS_ROT_270 + 4:
+		*offset1 = (screen_width * (fbh - 1) + fbw - 1) * ps;
+		if (field_offset)
+			*offset0 = *offset1 - field_offset * ps;
+		else
+			*offset0 = *offset1;
+		*row_inc = pixinc(screen_width * (fbh * x_predecim - 1) -
+				y_predecim - (fieldmode ? 1 : 0),
+				ps);
+		*pix_inc = pixinc(-x_predecim * screen_width, ps);
+		break;
+
+	default:
+		BUG();
+		return;
+	}
+}
+
+static void calc_tiler_rotation_offset(u16 screen_width, u16 width,
+		enum omap_color_mode color_mode, bool fieldmode,
+		unsigned int field_offset, unsigned *offset0, unsigned *offset1,
+		s32 *row_inc, s32 *pix_inc, int x_predecim, int y_predecim)
+{
+	u8 ps;
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_CLUT1:
+	case OMAP_DSS_COLOR_CLUT2:
+	case OMAP_DSS_COLOR_CLUT4:
+	case OMAP_DSS_COLOR_CLUT8:
+		BUG();
+		return;
+	default:
+		ps = color_mode_to_bpp(color_mode) / 8;
+		break;
+	}
+
+	DSSDBG("scrw %d, width %d\n", screen_width, width);
+
+	/*
+	 * field 0 = even field = bottom field
+	 * field 1 = odd field = top field
+	 */
+	*offset1 = 0;
+	if (field_offset)
+		*offset0 = *offset1 + field_offset * screen_width * ps;
+	else
+		*offset0 = *offset1;
+	*row_inc = pixinc(1 + (y_predecim * screen_width - width * x_predecim) +
+			(fieldmode ? screen_width : 0), ps);
+	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+		color_mode == OMAP_DSS_COLOR_UYVY)
+		*pix_inc = pixinc(x_predecim, 2 * ps);
+	else
+		*pix_inc = pixinc(x_predecim, ps);
+}
+
+/*
+ * This function is used to avoid synclosts in OMAP3, because of some
+ * undocumented horizontal position and timing related limitations.
+ */
+static int check_horiz_timing_omap3(unsigned long pclk, unsigned long lclk,
+		const struct omap_video_timings *t, u16 pos_x,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		bool five_taps)
+{
+	const int ds = DIV_ROUND_UP(height, out_height);
+	unsigned long nonactive;
+	static const u8 limits[3] = { 8, 10, 20 };
+	u64 val, blank;
+	int i;
+
+	nonactive = t->x_res + t->hfp + t->hsw + t->hbp - out_width;
+
+	i = 0;
+	if (out_height < height)
+		i++;
+	if (out_width < width)
+		i++;
+	blank = div_u64((u64)(t->hbp + t->hsw + t->hfp) * lclk, pclk);
+	DSSDBG("blanking period + ppl = %llu (limit = %u)\n", blank, limits[i]);
+	if (blank <= limits[i])
+		return -EINVAL;
+
+	/* FIXME add checks for 3-tap filter once the limitations are known */
+	if (!five_taps)
+		return 0;
+
+	/*
+	 * Pixel data should be prepared before visible display point starts.
+	 * So, atleast DS-2 lines must have already been fetched by DISPC
+	 * during nonactive - pos_x period.
+	 */
+	val = div_u64((u64)(nonactive - pos_x) * lclk, pclk);
+	DSSDBG("(nonactive - pos_x) * pcd = %llu max(0, DS - 2) * width = %d\n",
+		val, max(0, ds - 2) * width);
+	if (val < max(0, ds - 2) * width)
+		return -EINVAL;
+
+	/*
+	 * All lines need to be refilled during the nonactive period of which
+	 * only one line can be loaded during the active period. So, atleast
+	 * DS - 1 lines should be loaded during nonactive period.
+	 */
+	val =  div_u64((u64)nonactive * lclk, pclk);
+	DSSDBG("nonactive * pcd  = %llu, max(0, DS - 1) * width = %d\n",
+		val, max(0, ds - 1) * width);
+	if (val < max(0, ds - 1) * width)
+		return -EINVAL;
+
+	return 0;
+}
+
+static unsigned long calc_core_clk_five_taps(unsigned long pclk,
+		const struct omap_video_timings *mgr_timings, u16 width,
+		u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode)
+{
+	u32 core_clk = 0;
+	u64 tmp;
+
+	if (height <= out_height && width <= out_width)
+		return (unsigned long) pclk;
+
+	if (height > out_height) {
+		unsigned int ppl = mgr_timings->x_res;
+
+		tmp = (u64)pclk * height * out_width;
+		do_div(tmp, 2 * out_height * ppl);
+		core_clk = tmp;
+
+		if (height > 2 * out_height) {
+			if (ppl == out_width)
+				return 0;
+
+			tmp = (u64)pclk * (height - 2 * out_height) * out_width;
+			do_div(tmp, 2 * out_height * (ppl - out_width));
+			core_clk = max_t(u32, core_clk, tmp);
+		}
+	}
+
+	if (width > out_width) {
+		tmp = (u64)pclk * width;
+		do_div(tmp, out_width);
+		core_clk = max_t(u32, core_clk, tmp);
+
+		if (color_mode == OMAP_DSS_COLOR_RGB24U)
+			core_clk <<= 1;
+	}
+
+	return core_clk;
+}
+
+static unsigned long calc_core_clk_24xx(unsigned long pclk, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+	if (height > out_height && width > out_width)
+		return pclk * 4;
+	else
+		return pclk * 2;
+}
+
+static unsigned long calc_core_clk_34xx(unsigned long pclk, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+	unsigned int hf, vf;
+
+	/*
+	 * FIXME how to determine the 'A' factor
+	 * for the no downscaling case ?
+	 */
+
+	if (width > 3 * out_width)
+		hf = 4;
+	else if (width > 2 * out_width)
+		hf = 3;
+	else if (width > out_width)
+		hf = 2;
+	else
+		hf = 1;
+	if (height > out_height)
+		vf = 2;
+	else
+		vf = 1;
+
+	return pclk * vf * hf;
+}
+
+static unsigned long calc_core_clk_44xx(unsigned long pclk, u16 width,
+		u16 height, u16 out_width, u16 out_height, bool mem_to_mem)
+{
+	/*
+	 * If the overlay/writeback is in mem to mem mode, there are no
+	 * downscaling limitations with respect to pixel clock, return 1 as
+	 * required core clock to represent that we have sufficient enough
+	 * core clock to do maximum downscaling
+	 */
+	if (mem_to_mem)
+		return 1;
+
+	if (width > out_width)
+		return DIV_ROUND_UP(pclk, out_width) * width;
+	else
+		return pclk;
+}
+
+static int dispc_ovl_calc_scaling_24xx(unsigned long pclk, unsigned long lclk,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+	int error;
+	u16 in_width, in_height;
+	int min_factor = min(*decim_x, *decim_y);
+	const int maxsinglelinewidth =
+			dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+	*five_taps = false;
+
+	do {
+		in_height = height / *decim_y;
+		in_width = width / *decim_x;
+		*core_clk = dispc.feat->calc_core_clk(pclk, in_width,
+				in_height, out_width, out_height, mem_to_mem);
+		error = (in_width > maxsinglelinewidth || !*core_clk ||
+			*core_clk > dispc_core_clk_rate());
+		if (error) {
+			if (*decim_x == *decim_y) {
+				*decim_x = min_factor;
+				++*decim_y;
+			} else {
+				swap(*decim_x, *decim_y);
+				if (*decim_x < *decim_y)
+					++*decim_x;
+			}
+		}
+	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+	if (error) {
+		DSSERR("failed to find scaling settings\n");
+		return -EINVAL;
+	}
+
+	if (in_width > maxsinglelinewidth) {
+		DSSERR("Cannot scale max input width exceeded");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int dispc_ovl_calc_scaling_34xx(unsigned long pclk, unsigned long lclk,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+	int error;
+	u16 in_width, in_height;
+	const int maxsinglelinewidth =
+			dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+
+	do {
+		in_height = height / *decim_y;
+		in_width = width / *decim_x;
+		*five_taps = in_height > out_height;
+
+		if (in_width > maxsinglelinewidth)
+			if (in_height > out_height &&
+						in_height < out_height * 2)
+				*five_taps = false;
+again:
+		if (*five_taps)
+			*core_clk = calc_core_clk_five_taps(pclk, mgr_timings,
+						in_width, in_height, out_width,
+						out_height, color_mode);
+		else
+			*core_clk = dispc.feat->calc_core_clk(pclk, in_width,
+					in_height, out_width, out_height,
+					mem_to_mem);
+
+		error = check_horiz_timing_omap3(pclk, lclk, mgr_timings,
+				pos_x, in_width, in_height, out_width,
+				out_height, *five_taps);
+		if (error && *five_taps) {
+			*five_taps = false;
+			goto again;
+		}
+
+		error = (error || in_width > maxsinglelinewidth * 2 ||
+			(in_width > maxsinglelinewidth && *five_taps) ||
+			!*core_clk || *core_clk > dispc_core_clk_rate());
+
+		if (!error) {
+			/* verify that we're inside the limits of scaler */
+			if (in_width / 4 > out_width)
+					error = 1;
+
+			if (*five_taps) {
+				if (in_height / 4 > out_height)
+					error = 1;
+			} else {
+				if (in_height / 2 > out_height)
+					error = 1;
+			}
+		}
+
+		if (error)
+			++*decim_y;
+	} while (*decim_x <= *x_predecim && *decim_y <= *y_predecim && error);
+
+	if (error) {
+		DSSERR("failed to find scaling settings\n");
+		return -EINVAL;
+	}
+
+	if (check_horiz_timing_omap3(pclk, lclk, mgr_timings, pos_x, in_width,
+				in_height, out_width, out_height, *five_taps)) {
+			DSSERR("horizontal timing too tight\n");
+			return -EINVAL;
+	}
+
+	if (in_width > (maxsinglelinewidth * 2)) {
+		DSSERR("Cannot setup scaling");
+		DSSERR("width exceeds maximum width possible");
+		return -EINVAL;
+	}
+
+	if (in_width > maxsinglelinewidth && *five_taps) {
+		DSSERR("cannot setup scaling with five taps");
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int dispc_ovl_calc_scaling_44xx(unsigned long pclk, unsigned long lclk,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, int *decim_x, int *decim_y,
+		u16 pos_x, unsigned long *core_clk, bool mem_to_mem)
+{
+	u16 in_width, in_width_max;
+	int decim_x_min = *decim_x;
+	u16 in_height = height / *decim_y;
+	const int maxsinglelinewidth =
+				dss_feat_get_param_max(FEAT_PARAM_LINEWIDTH);
+	const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+
+	if (mem_to_mem) {
+		in_width_max = out_width * maxdownscale;
+	} else {
+		in_width_max = dispc_core_clk_rate() /
+					DIV_ROUND_UP(pclk, out_width);
+	}
+
+	*decim_x = DIV_ROUND_UP(width, in_width_max);
+
+	*decim_x = *decim_x > decim_x_min ? *decim_x : decim_x_min;
+	if (*decim_x > *x_predecim)
+		return -EINVAL;
+
+	do {
+		in_width = width / *decim_x;
+	} while (*decim_x <= *x_predecim &&
+			in_width > maxsinglelinewidth && ++*decim_x);
+
+	if (in_width > maxsinglelinewidth) {
+		DSSERR("Cannot scale width exceeds max line width");
+		return -EINVAL;
+	}
+
+	*core_clk = dispc.feat->calc_core_clk(pclk, in_width, in_height,
+				out_width, out_height, mem_to_mem);
+	return 0;
+}
+
+#define DIV_FRAC(dividend, divisor) \
+	((dividend) * 100 / (divisor) - ((dividend) / (divisor) * 100))
+
+static int dispc_ovl_calc_scaling(unsigned long pclk, unsigned long lclk,
+		enum omap_overlay_caps caps,
+		const struct omap_video_timings *mgr_timings,
+		u16 width, u16 height, u16 out_width, u16 out_height,
+		enum omap_color_mode color_mode, bool *five_taps,
+		int *x_predecim, int *y_predecim, u16 pos_x,
+		enum omap_dss_rotation_type rotation_type, bool mem_to_mem)
+{
+	const int maxdownscale = dss_feat_get_param_max(FEAT_PARAM_DOWNSCALE);
+	const int max_decim_limit = 16;
+	unsigned long core_clk = 0;
+	int decim_x, decim_y, ret;
+
+	if (width == out_width && height == out_height)
+		return 0;
+
+	if (!mem_to_mem && (pclk == 0 || mgr_timings->pixelclock == 0)) {
+		DSSERR("cannot calculate scaling settings: pclk is zero\n");
+		return -EINVAL;
+	}
+
+	if ((caps & OMAP_DSS_OVL_CAP_SCALE) == 0)
+		return -EINVAL;
+
+	if (mem_to_mem) {
+		*x_predecim = *y_predecim = 1;
+	} else {
+		*x_predecim = max_decim_limit;
+		*y_predecim = (rotation_type == OMAP_DSS_ROT_TILER &&
+				dss_has_feature(FEAT_BURST_2D)) ?
+				2 : max_decim_limit;
+	}
+
+	if (color_mode == OMAP_DSS_COLOR_CLUT1 ||
+	    color_mode == OMAP_DSS_COLOR_CLUT2 ||
+	    color_mode == OMAP_DSS_COLOR_CLUT4 ||
+	    color_mode == OMAP_DSS_COLOR_CLUT8) {
+		*x_predecim = 1;
+		*y_predecim = 1;
+		*five_taps = false;
+		return 0;
+	}
+
+	decim_x = DIV_ROUND_UP(DIV_ROUND_UP(width, out_width), maxdownscale);
+	decim_y = DIV_ROUND_UP(DIV_ROUND_UP(height, out_height), maxdownscale);
+
+	if (decim_x > *x_predecim || out_width > width * 8)
+		return -EINVAL;
+
+	if (decim_y > *y_predecim || out_height > height * 8)
+		return -EINVAL;
+
+	ret = dispc.feat->calc_scaling(pclk, lclk, mgr_timings, width, height,
+		out_width, out_height, color_mode, five_taps,
+		x_predecim, y_predecim, &decim_x, &decim_y, pos_x, &core_clk,
+		mem_to_mem);
+	if (ret)
+		return ret;
+
+	DSSDBG("%dx%d -> %dx%d (%d.%02d x %d.%02d), decim %dx%d %dx%d (%d.%02d x %d.%02d), taps %d, req clk %lu, cur clk %lu\n",
+		width, height,
+		out_width, out_height,
+		out_width / width, DIV_FRAC(out_width, width),
+		out_height / height, DIV_FRAC(out_height, height),
+
+		decim_x, decim_y,
+		width / decim_x, height / decim_y,
+		out_width / (width / decim_x), DIV_FRAC(out_width, width / decim_x),
+		out_height / (height / decim_y), DIV_FRAC(out_height, height / decim_y),
+
+		*five_taps ? 5 : 3,
+		core_clk, dispc_core_clk_rate());
+
+	if (!core_clk || core_clk > dispc_core_clk_rate()) {
+		DSSERR("failed to set up scaling, "
+			"required core clk rate = %lu Hz, "
+			"current core clk rate = %lu Hz\n",
+			core_clk, dispc_core_clk_rate());
+		return -EINVAL;
+	}
+
+	*x_predecim = decim_x;
+	*y_predecim = decim_y;
+	return 0;
+}
+
+int dispc_ovl_check(enum omap_plane plane, enum omap_channel channel,
+		const struct omap_overlay_info *oi,
+		const struct omap_video_timings *timings,
+		int *x_predecim, int *y_predecim)
+{
+	enum omap_overlay_caps caps = dss_feat_get_overlay_caps(plane);
+	bool five_taps = true;
+	bool fieldmode = false;
+	u16 in_height = oi->height;
+	u16 in_width = oi->width;
+	bool ilace = timings->interlace;
+	u16 out_width, out_height;
+	int pos_x = oi->pos_x;
+	unsigned long pclk = dispc_mgr_pclk_rate(channel);
+	unsigned long lclk = dispc_mgr_lclk_rate(channel);
+
+	out_width = oi->out_width == 0 ? oi->width : oi->out_width;
+	out_height = oi->out_height == 0 ? oi->height : oi->out_height;
+
+	if (ilace && oi->height == out_height)
+		fieldmode = true;
+
+	if (ilace) {
+		if (fieldmode)
+			in_height /= 2;
+		out_height /= 2;
+
+		DSSDBG("adjusting for ilace: height %d, out_height %d\n",
+				in_height, out_height);
+	}
+
+	if (!dss_feat_color_mode_supported(plane, oi->color_mode))
+		return -EINVAL;
+
+	return dispc_ovl_calc_scaling(pclk, lclk, caps, timings, in_width,
+			in_height, out_width, out_height, oi->color_mode,
+			&five_taps, x_predecim, y_predecim, pos_x,
+			oi->rotation_type, false);
+}
+EXPORT_SYMBOL(dispc_ovl_check);
+
+static int dispc_ovl_setup_common(enum omap_plane plane,
+		enum omap_overlay_caps caps, u32 paddr, u32 p_uv_addr,
+		u16 screen_width, int pos_x, int pos_y, u16 width, u16 height,
+		u16 out_width, u16 out_height, enum omap_color_mode color_mode,
+		u8 rotation, bool mirror, u8 zorder, u8 pre_mult_alpha,
+		u8 global_alpha, enum omap_dss_rotation_type rotation_type,
+		bool replication, const struct omap_video_timings *mgr_timings,
+		bool mem_to_mem)
+{
+	bool five_taps = true;
+	bool fieldmode = false;
+	int r, cconv = 0;
+	unsigned offset0, offset1;
+	s32 row_inc;
+	s32 pix_inc;
+	u16 frame_width, frame_height;
+	unsigned int field_offset = 0;
+	u16 in_height = height;
+	u16 in_width = width;
+	int x_predecim = 1, y_predecim = 1;
+	bool ilace = mgr_timings->interlace;
+	unsigned long pclk = dispc_plane_pclk_rate(plane);
+	unsigned long lclk = dispc_plane_lclk_rate(plane);
+
+	if (paddr == 0 && rotation_type != OMAP_DSS_ROT_TILER)
+		return -EINVAL;
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_NV12:
+		if (in_width & 1) {
+			DSSERR("input width %d is not even for YUV format\n",
+				in_width);
+			return -EINVAL;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	out_width = out_width == 0 ? width : out_width;
+	out_height = out_height == 0 ? height : out_height;
+
+	if (ilace && height == out_height)
+		fieldmode = true;
+
+	if (ilace) {
+		if (fieldmode)
+			in_height /= 2;
+		pos_y /= 2;
+		out_height /= 2;
+
+		DSSDBG("adjusting for ilace: height %d, pos_y %d, "
+			"out_height %d\n", in_height, pos_y,
+			out_height);
+	}
+
+	if (!dss_feat_color_mode_supported(plane, color_mode))
+		return -EINVAL;
+
+	r = dispc_ovl_calc_scaling(pclk, lclk, caps, mgr_timings, in_width,
+			in_height, out_width, out_height, color_mode,
+			&five_taps, &x_predecim, &y_predecim, pos_x,
+			rotation_type, mem_to_mem);
+	if (r)
+		return r;
+
+	in_width = in_width / x_predecim;
+	in_height = in_height / y_predecim;
+
+	if (x_predecim > 1 || y_predecim > 1)
+		DSSDBG("predecimation %d x %x, new input size %d x %d\n",
+			x_predecim, y_predecim, in_width, in_height);
+
+	switch (color_mode) {
+	case OMAP_DSS_COLOR_YUV2:
+	case OMAP_DSS_COLOR_UYVY:
+	case OMAP_DSS_COLOR_NV12:
+		if (in_width & 1) {
+			DSSDBG("predecimated input width is not even for YUV format\n");
+			DSSDBG("adjusting input width %d -> %d\n",
+				in_width, in_width & ~1);
+
+			in_width &= ~1;
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	if (color_mode == OMAP_DSS_COLOR_YUV2 ||
+			color_mode == OMAP_DSS_COLOR_UYVY ||
+			color_mode == OMAP_DSS_COLOR_NV12)
+		cconv = 1;
+
+	if (ilace && !fieldmode) {
+		/*
+		 * when downscaling the bottom field may have to start several
+		 * source lines below the top field. Unfortunately ACCUI
+		 * registers will only hold the fractional part of the offset
+		 * so the integer part must be added to the base address of the
+		 * bottom field.
+		 */
+		if (!in_height || in_height == out_height)
+			field_offset = 0;
+		else
+			field_offset = in_height / out_height / 2;
+	}
+
+	/* Fields are independent but interleaved in memory. */
+	if (fieldmode)
+		field_offset = 1;
+
+	offset0 = 0;
+	offset1 = 0;
+	row_inc = 0;
+	pix_inc = 0;
+
+	if (plane == OMAP_DSS_WB) {
+		frame_width = out_width;
+		frame_height = out_height;
+	} else {
+		frame_width = in_width;
+		frame_height = height;
+	}
+
+	if (rotation_type == OMAP_DSS_ROT_TILER)
+		calc_tiler_rotation_offset(screen_width, frame_width,
+				color_mode, fieldmode, field_offset,
+				&offset0, &offset1, &row_inc, &pix_inc,
+				x_predecim, y_predecim);
+	else if (rotation_type == OMAP_DSS_ROT_DMA)
+		calc_dma_rotation_offset(rotation, mirror, screen_width,
+				frame_width, frame_height,
+				color_mode, fieldmode, field_offset,
+				&offset0, &offset1, &row_inc, &pix_inc,
+				x_predecim, y_predecim);
+	else
+		calc_vrfb_rotation_offset(rotation, mirror,
+				screen_width, frame_width, frame_height,
+				color_mode, fieldmode, field_offset,
+				&offset0, &offset1, &row_inc, &pix_inc,
+				x_predecim, y_predecim);
+
+	DSSDBG("offset0 %u, offset1 %u, row_inc %d, pix_inc %d\n",
+			offset0, offset1, row_inc, pix_inc);
+
+	dispc_ovl_set_color_mode(plane, color_mode);
+
+	dispc_ovl_configure_burst_type(plane, rotation_type);
+
+	dispc_ovl_set_ba0(plane, paddr + offset0);
+	dispc_ovl_set_ba1(plane, paddr + offset1);
+
+	if (OMAP_DSS_COLOR_NV12 == color_mode) {
+		dispc_ovl_set_ba0_uv(plane, p_uv_addr + offset0);
+		dispc_ovl_set_ba1_uv(plane, p_uv_addr + offset1);
+	}
+
+	if (dispc.feat->last_pixel_inc_missing)
+		row_inc += pix_inc - 1;
+
+	dispc_ovl_set_row_inc(plane, row_inc);
+	dispc_ovl_set_pix_inc(plane, pix_inc);
+
+	DSSDBG("%d,%d %dx%d -> %dx%d\n", pos_x, pos_y, in_width,
+			in_height, out_width, out_height);
+
+	dispc_ovl_set_pos(plane, caps, pos_x, pos_y);
+
+	dispc_ovl_set_input_size(plane, in_width, in_height);
+
+	if (caps & OMAP_DSS_OVL_CAP_SCALE) {
+		dispc_ovl_set_scaling(plane, in_width, in_height, out_width,
+				   out_height, ilace, five_taps, fieldmode,
+				   color_mode, rotation);
+		dispc_ovl_set_output_size(plane, out_width, out_height);
+		dispc_ovl_set_vid_color_conv(plane, cconv);
+	}
+
+	dispc_ovl_set_rotation_attrs(plane, rotation, rotation_type, mirror,
+			color_mode);
+
+	dispc_ovl_set_zorder(plane, caps, zorder);
+	dispc_ovl_set_pre_mult_alpha(plane, caps, pre_mult_alpha);
+	dispc_ovl_setup_global_alpha(plane, caps, global_alpha);
+
+	dispc_ovl_enable_replication(plane, caps, replication);
+
+	return 0;
+}
+
+int dispc_ovl_setup(enum omap_plane plane, const struct omap_overlay_info *oi,
+		bool replication, const struct omap_video_timings *mgr_timings,
+		bool mem_to_mem)
+{
+	int r;
+	enum omap_overlay_caps caps = dss_feat_get_overlay_caps(plane);
+	enum omap_channel channel;
+
+	channel = dispc_ovl_get_channel_out(plane);
+
+	DSSDBG("dispc_ovl_setup %d, pa %pad, pa_uv %pad, sw %d, %d,%d, %dx%d ->"
+		" %dx%d, cmode %x, rot %d, mir %d, chan %d repl %d\n",
+		plane, &oi->paddr, &oi->p_uv_addr, oi->screen_width, oi->pos_x,
+		oi->pos_y, oi->width, oi->height, oi->out_width, oi->out_height,
+		oi->color_mode, oi->rotation, oi->mirror, channel, replication);
+
+	r = dispc_ovl_setup_common(plane, caps, oi->paddr, oi->p_uv_addr,
+		oi->screen_width, oi->pos_x, oi->pos_y, oi->width, oi->height,
+		oi->out_width, oi->out_height, oi->color_mode, oi->rotation,
+		oi->mirror, oi->zorder, oi->pre_mult_alpha, oi->global_alpha,
+		oi->rotation_type, replication, mgr_timings, mem_to_mem);
+
+	return r;
+}
+EXPORT_SYMBOL(dispc_ovl_setup);
+
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+		bool mem_to_mem, const struct omap_video_timings *mgr_timings)
+{
+	int r;
+	u32 l;
+	enum omap_plane plane = OMAP_DSS_WB;
+	const int pos_x = 0, pos_y = 0;
+	const u8 zorder = 0, global_alpha = 0;
+	const bool replication = false;
+	bool truncation;
+	int in_width = mgr_timings->x_res;
+	int in_height = mgr_timings->y_res;
+	enum omap_overlay_caps caps =
+		OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA;
+
+	DSSDBG("dispc_wb_setup, pa %x, pa_uv %x, %d,%d -> %dx%d, cmode %x, "
+		"rot %d, mir %d\n", wi->paddr, wi->p_uv_addr, in_width,
+		in_height, wi->width, wi->height, wi->color_mode, wi->rotation,
+		wi->mirror);
+
+	r = dispc_ovl_setup_common(plane, caps, wi->paddr, wi->p_uv_addr,
+		wi->buf_width, pos_x, pos_y, in_width, in_height, wi->width,
+		wi->height, wi->color_mode, wi->rotation, wi->mirror, zorder,
+		wi->pre_mult_alpha, global_alpha, wi->rotation_type,
+		replication, mgr_timings, mem_to_mem);
+
+	switch (wi->color_mode) {
+	case OMAP_DSS_COLOR_RGB16:
+	case OMAP_DSS_COLOR_RGB24P:
+	case OMAP_DSS_COLOR_ARGB16:
+	case OMAP_DSS_COLOR_RGBA16:
+	case OMAP_DSS_COLOR_RGB12U:
+	case OMAP_DSS_COLOR_ARGB16_1555:
+	case OMAP_DSS_COLOR_XRGB16_1555:
+	case OMAP_DSS_COLOR_RGBX16:
+		truncation = true;
+		break;
+	default:
+		truncation = false;
+		break;
+	}
+
+	/* setup extra DISPC_WB_ATTRIBUTES */
+	l = dispc_read_reg(DISPC_OVL_ATTRIBUTES(plane));
+	l = FLD_MOD(l, truncation, 10, 10);	/* TRUNCATIONENABLE */
+	l = FLD_MOD(l, mem_to_mem, 19, 19);	/* WRITEBACKMODE */
+	if (mem_to_mem)
+		l = FLD_MOD(l, 1, 26, 24);	/* CAPTUREMODE */
+	else
+		l = FLD_MOD(l, 0, 26, 24);	/* CAPTUREMODE */
+	dispc_write_reg(DISPC_OVL_ATTRIBUTES(plane), l);
+
+	if (mem_to_mem) {
+		/* WBDELAYCOUNT */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), 0, 7, 0);
+	} else {
+		int wbdelay;
+
+		wbdelay = min(mgr_timings->vfp + mgr_timings->vsw +
+			mgr_timings->vbp, 255);
+
+		/* WBDELAYCOUNT */
+		REG_FLD_MOD(DISPC_OVL_ATTRIBUTES2(plane), wbdelay, 7, 0);
+	}
+
+	return r;
+}
+
+int dispc_ovl_enable(enum omap_plane plane, bool enable)
+{
+	DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
+
+	REG_FLD_MOD(DISPC_OVL_ATTRIBUTES(plane), enable ? 1 : 0, 0, 0);
+
+	return 0;
+}
+EXPORT_SYMBOL(dispc_ovl_enable);
+
+bool dispc_ovl_enabled(enum omap_plane plane)
+{
+	return REG_GET(DISPC_OVL_ATTRIBUTES(plane), 0, 0);
+}
+EXPORT_SYMBOL(dispc_ovl_enabled);
+
+void dispc_mgr_enable(enum omap_channel channel, bool enable)
+{
+	mgr_fld_write(channel, DISPC_MGR_FLD_ENABLE, enable);
+	/* flush posted write */
+	mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
+}
+EXPORT_SYMBOL(dispc_mgr_enable);
+
+bool dispc_mgr_is_enabled(enum omap_channel channel)
+{
+	return !!mgr_fld_read(channel, DISPC_MGR_FLD_ENABLE);
+}
+EXPORT_SYMBOL(dispc_mgr_is_enabled);
+
+void dispc_wb_enable(bool enable)
+{
+	dispc_ovl_enable(OMAP_DSS_WB, enable);
+}
+
+bool dispc_wb_is_enabled(void)
+{
+	return dispc_ovl_enabled(OMAP_DSS_WB);
+}
+
+static void dispc_lcd_enable_signal_polarity(bool act_high)
+{
+	if (!dss_has_feature(FEAT_LCDENABLEPOL))
+		return;
+
+	REG_FLD_MOD(DISPC_CONTROL, act_high ? 1 : 0, 29, 29);
+}
+
+void dispc_lcd_enable_signal(bool enable)
+{
+	if (!dss_has_feature(FEAT_LCDENABLESIGNAL))
+		return;
+
+	REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 28, 28);
+}
+
+void dispc_pck_free_enable(bool enable)
+{
+	if (!dss_has_feature(FEAT_PCKFREEENABLE))
+		return;
+
+	REG_FLD_MOD(DISPC_CONTROL, enable ? 1 : 0, 27, 27);
+}
+
+static void dispc_mgr_enable_fifohandcheck(enum omap_channel channel, bool enable)
+{
+	mgr_fld_write(channel, DISPC_MGR_FLD_FIFOHANDCHECK, enable);
+}
+
+
+static void dispc_mgr_set_lcd_type_tft(enum omap_channel channel)
+{
+	mgr_fld_write(channel, DISPC_MGR_FLD_STNTFT, 1);
+}
+
+static void dispc_set_loadmode(enum omap_dss_load_mode mode)
+{
+	REG_FLD_MOD(DISPC_CONFIG, mode, 2, 1);
+}
+
+
+static void dispc_mgr_set_default_color(enum omap_channel channel, u32 color)
+{
+	dispc_write_reg(DISPC_DEFAULT_COLOR(channel), color);
+}
+
+static void dispc_mgr_set_trans_key(enum omap_channel ch,
+		enum omap_dss_trans_key_type type,
+		u32 trans_key)
+{
+	mgr_fld_write(ch, DISPC_MGR_FLD_TCKSELECTION, type);
+
+	dispc_write_reg(DISPC_TRANS_COLOR(ch), trans_key);
+}
+
+static void dispc_mgr_enable_trans_key(enum omap_channel ch, bool enable)
+{
+	mgr_fld_write(ch, DISPC_MGR_FLD_TCKENABLE, enable);
+}
+
+static void dispc_mgr_enable_alpha_fixed_zorder(enum omap_channel ch,
+		bool enable)
+{
+	if (!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+		return;
+
+	if (ch == OMAP_DSS_CHANNEL_LCD)
+		REG_FLD_MOD(DISPC_CONFIG, enable, 18, 18);
+	else if (ch == OMAP_DSS_CHANNEL_DIGIT)
+		REG_FLD_MOD(DISPC_CONFIG, enable, 19, 19);
+}
+
+void dispc_mgr_setup(enum omap_channel channel,
+		const struct omap_overlay_manager_info *info)
+{
+	dispc_mgr_set_default_color(channel, info->default_color);
+	dispc_mgr_set_trans_key(channel, info->trans_key_type, info->trans_key);
+	dispc_mgr_enable_trans_key(channel, info->trans_enabled);
+	dispc_mgr_enable_alpha_fixed_zorder(channel,
+			info->partial_alpha_enabled);
+	if (dss_has_feature(FEAT_CPR)) {
+		dispc_mgr_enable_cpr(channel, info->cpr_enable);
+		dispc_mgr_set_cpr_coef(channel, &info->cpr_coefs);
+	}
+}
+EXPORT_SYMBOL(dispc_mgr_setup);
+
+static void dispc_mgr_set_tft_data_lines(enum omap_channel channel, u8 data_lines)
+{
+	int code;
+
+	switch (data_lines) {
+	case 12:
+		code = 0;
+		break;
+	case 16:
+		code = 1;
+		break;
+	case 18:
+		code = 2;
+		break;
+	case 24:
+		code = 3;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	mgr_fld_write(channel, DISPC_MGR_FLD_TFTDATALINES, code);
+}
+
+static void dispc_mgr_set_io_pad_mode(enum dss_io_pad_mode mode)
+{
+	u32 l;
+	int gpout0, gpout1;
+
+	switch (mode) {
+	case DSS_IO_PAD_MODE_RESET:
+		gpout0 = 0;
+		gpout1 = 0;
+		break;
+	case DSS_IO_PAD_MODE_RFBI:
+		gpout0 = 1;
+		gpout1 = 0;
+		break;
+	case DSS_IO_PAD_MODE_BYPASS:
+		gpout0 = 1;
+		gpout1 = 1;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	l = dispc_read_reg(DISPC_CONTROL);
+	l = FLD_MOD(l, gpout0, 15, 15);
+	l = FLD_MOD(l, gpout1, 16, 16);
+	dispc_write_reg(DISPC_CONTROL, l);
+}
+
+static void dispc_mgr_enable_stallmode(enum omap_channel channel, bool enable)
+{
+	mgr_fld_write(channel, DISPC_MGR_FLD_STALLMODE, enable);
+}
+
+void dispc_mgr_set_lcd_config(enum omap_channel channel,
+		const struct dss_lcd_mgr_config *config)
+{
+	dispc_mgr_set_io_pad_mode(config->io_pad_mode);
+
+	dispc_mgr_enable_stallmode(channel, config->stallmode);
+	dispc_mgr_enable_fifohandcheck(channel, config->fifohandcheck);
+
+	dispc_mgr_set_clock_div(channel, &config->clock_info);
+
+	dispc_mgr_set_tft_data_lines(channel, config->video_port_width);
+
+	dispc_lcd_enable_signal_polarity(config->lcden_sig_polarity);
+
+	dispc_mgr_set_lcd_type_tft(channel);
+}
+EXPORT_SYMBOL(dispc_mgr_set_lcd_config);
+
+static bool _dispc_mgr_size_ok(u16 width, u16 height)
+{
+	return width <= dispc.feat->mgr_width_max &&
+		height <= dispc.feat->mgr_height_max;
+}
+
+static bool _dispc_lcd_timings_ok(int hsw, int hfp, int hbp,
+		int vsw, int vfp, int vbp)
+{
+	if (hsw < 1 || hsw > dispc.feat->sw_max ||
+			hfp < 1 || hfp > dispc.feat->hp_max ||
+			hbp < 1 || hbp > dispc.feat->hp_max ||
+			vsw < 1 || vsw > dispc.feat->sw_max ||
+			vfp < 0 || vfp > dispc.feat->vp_max ||
+			vbp < 0 || vbp > dispc.feat->vp_max)
+		return false;
+	return true;
+}
+
+static bool _dispc_mgr_pclk_ok(enum omap_channel channel,
+		unsigned long pclk)
+{
+	if (dss_mgr_is_lcd(channel))
+		return pclk <= dispc.feat->max_lcd_pclk ? true : false;
+	else
+		return pclk <= dispc.feat->max_tv_pclk ? true : false;
+}
+
+bool dispc_mgr_timings_ok(enum omap_channel channel,
+		const struct omap_video_timings *timings)
+{
+	if (!_dispc_mgr_size_ok(timings->x_res, timings->y_res))
+		return false;
+
+	if (!_dispc_mgr_pclk_ok(channel, timings->pixelclock))
+		return false;
+
+	if (dss_mgr_is_lcd(channel)) {
+		/* TODO: OMAP4+ supports interlace for LCD outputs */
+		if (timings->interlace)
+			return false;
+
+		if (!_dispc_lcd_timings_ok(timings->hsw, timings->hfp,
+				timings->hbp, timings->vsw, timings->vfp,
+				timings->vbp))
+			return false;
+	}
+
+	return true;
+}
+
+static void _dispc_mgr_set_lcd_timings(enum omap_channel channel, int hsw,
+		int hfp, int hbp, int vsw, int vfp, int vbp,
+		enum omap_dss_signal_level vsync_level,
+		enum omap_dss_signal_level hsync_level,
+		enum omap_dss_signal_edge data_pclk_edge,
+		enum omap_dss_signal_level de_level,
+		enum omap_dss_signal_edge sync_pclk_edge)
+
+{
+	u32 timing_h, timing_v, l;
+	bool onoff, rf, ipc, vs, hs, de;
+
+	timing_h = FLD_VAL(hsw-1, dispc.feat->sw_start, 0) |
+			FLD_VAL(hfp-1, dispc.feat->fp_start, 8) |
+			FLD_VAL(hbp-1, dispc.feat->bp_start, 20);
+	timing_v = FLD_VAL(vsw-1, dispc.feat->sw_start, 0) |
+			FLD_VAL(vfp, dispc.feat->fp_start, 8) |
+			FLD_VAL(vbp, dispc.feat->bp_start, 20);
+
+	dispc_write_reg(DISPC_TIMING_H(channel), timing_h);
+	dispc_write_reg(DISPC_TIMING_V(channel), timing_v);
+
+	switch (vsync_level) {
+	case OMAPDSS_SIG_ACTIVE_LOW:
+		vs = true;
+		break;
+	case OMAPDSS_SIG_ACTIVE_HIGH:
+		vs = false;
+		break;
+	default:
+		BUG();
+	}
+
+	switch (hsync_level) {
+	case OMAPDSS_SIG_ACTIVE_LOW:
+		hs = true;
+		break;
+	case OMAPDSS_SIG_ACTIVE_HIGH:
+		hs = false;
+		break;
+	default:
+		BUG();
+	}
+
+	switch (de_level) {
+	case OMAPDSS_SIG_ACTIVE_LOW:
+		de = true;
+		break;
+	case OMAPDSS_SIG_ACTIVE_HIGH:
+		de = false;
+		break;
+	default:
+		BUG();
+	}
+
+	switch (data_pclk_edge) {
+	case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+		ipc = false;
+		break;
+	case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+		ipc = true;
+		break;
+	default:
+		BUG();
+	}
+
+	/* always use the 'rf' setting */
+	onoff = true;
+
+	switch (sync_pclk_edge) {
+	case OMAPDSS_DRIVE_SIG_FALLING_EDGE:
+		rf = false;
+		break;
+	case OMAPDSS_DRIVE_SIG_RISING_EDGE:
+		rf = true;
+		break;
+	default:
+		BUG();
+	}
+
+	l = FLD_VAL(onoff, 17, 17) |
+		FLD_VAL(rf, 16, 16) |
+		FLD_VAL(de, 15, 15) |
+		FLD_VAL(ipc, 14, 14) |
+		FLD_VAL(hs, 13, 13) |
+		FLD_VAL(vs, 12, 12);
+
+	/* always set ALIGN bit when available */
+	if (dispc.feat->supports_sync_align)
+		l |= (1 << 18);
+
+	dispc_write_reg(DISPC_POL_FREQ(channel), l);
+
+	if (dispc.syscon_pol) {
+		const int shifts[] = {
+			[OMAP_DSS_CHANNEL_LCD] = 0,
+			[OMAP_DSS_CHANNEL_LCD2] = 1,
+			[OMAP_DSS_CHANNEL_LCD3] = 2,
+		};
+
+		u32 mask, val;
+
+		mask = (1 << 0) | (1 << 3) | (1 << 6);
+		val = (rf << 0) | (ipc << 3) | (onoff << 6);
+
+		mask <<= 16 + shifts[channel];
+		val <<= 16 + shifts[channel];
+
+		regmap_update_bits(dispc.syscon_pol, dispc.syscon_pol_offset,
+			mask, val);
+	}
+}
+
+/* change name to mode? */
+void dispc_mgr_set_timings(enum omap_channel channel,
+		const struct omap_video_timings *timings)
+{
+	unsigned xtot, ytot;
+	unsigned long ht, vt;
+	struct omap_video_timings t = *timings;
+
+	DSSDBG("channel %d xres %u yres %u\n", channel, t.x_res, t.y_res);
+
+	if (!dispc_mgr_timings_ok(channel, &t)) {
+		BUG();
+		return;
+	}
+
+	if (dss_mgr_is_lcd(channel)) {
+		_dispc_mgr_set_lcd_timings(channel, t.hsw, t.hfp, t.hbp, t.vsw,
+				t.vfp, t.vbp, t.vsync_level, t.hsync_level,
+				t.data_pclk_edge, t.de_level, t.sync_pclk_edge);
+
+		xtot = t.x_res + t.hfp + t.hsw + t.hbp;
+		ytot = t.y_res + t.vfp + t.vsw + t.vbp;
+
+		ht = timings->pixelclock / xtot;
+		vt = timings->pixelclock / xtot / ytot;
+
+		DSSDBG("pck %u\n", timings->pixelclock);
+		DSSDBG("hsw %d hfp %d hbp %d vsw %d vfp %d vbp %d\n",
+			t.hsw, t.hfp, t.hbp, t.vsw, t.vfp, t.vbp);
+		DSSDBG("vsync_level %d hsync_level %d data_pclk_edge %d de_level %d sync_pclk_edge %d\n",
+			t.vsync_level, t.hsync_level, t.data_pclk_edge,
+			t.de_level, t.sync_pclk_edge);
+
+		DSSDBG("hsync %luHz, vsync %luHz\n", ht, vt);
+	} else {
+		if (t.interlace)
+			t.y_res /= 2;
+	}
+
+	dispc_mgr_set_size(channel, t.x_res, t.y_res);
+}
+EXPORT_SYMBOL(dispc_mgr_set_timings);
+
+static void dispc_mgr_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
+		u16 pck_div)
+{
+	BUG_ON(lck_div < 1);
+	BUG_ON(pck_div < 1);
+
+	dispc_write_reg(DISPC_DIVISORo(channel),
+			FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
+
+	if (!dss_has_feature(FEAT_CORE_CLK_DIV) &&
+			channel == OMAP_DSS_CHANNEL_LCD)
+		dispc.core_clk_rate = dispc_fclk_rate() / lck_div;
+}
+
+static void dispc_mgr_get_lcd_divisor(enum omap_channel channel, int *lck_div,
+		int *pck_div)
+{
+	u32 l;
+	l = dispc_read_reg(DISPC_DIVISORo(channel));
+	*lck_div = FLD_GET(l, 23, 16);
+	*pck_div = FLD_GET(l, 7, 0);
+}
+
+static unsigned long dispc_fclk_rate(void)
+{
+	struct dss_pll *pll;
+	unsigned long r = 0;
+
+	switch (dss_get_dispc_clk_source()) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		r = dss_get_dispc_clk_rate();
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		pll = dss_pll_find("dsi0");
+		if (!pll)
+			pll = dss_pll_find("video0");
+
+		r = pll->cinfo.clkout[0];
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		pll = dss_pll_find("dsi1");
+		if (!pll)
+			pll = dss_pll_find("video1");
+
+		r = pll->cinfo.clkout[0];
+		break;
+	default:
+		BUG();
+		return 0;
+	}
+
+	return r;
+}
+
+static unsigned long dispc_mgr_lclk_rate(enum omap_channel channel)
+{
+	struct dss_pll *pll;
+	int lcd;
+	unsigned long r;
+	u32 l;
+
+	if (dss_mgr_is_lcd(channel)) {
+		l = dispc_read_reg(DISPC_DIVISORo(channel));
+
+		lcd = FLD_GET(l, 23, 16);
+
+		switch (dss_get_lcd_clk_source(channel)) {
+		case OMAP_DSS_CLK_SRC_FCK:
+			r = dss_get_dispc_clk_rate();
+			break;
+		case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+			pll = dss_pll_find("dsi0");
+			if (!pll)
+				pll = dss_pll_find("video0");
+
+			r = pll->cinfo.clkout[0];
+			break;
+		case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+			pll = dss_pll_find("dsi1");
+			if (!pll)
+				pll = dss_pll_find("video1");
+
+			r = pll->cinfo.clkout[0];
+			break;
+		default:
+			BUG();
+			return 0;
+		}
+
+		return r / lcd;
+	} else {
+		return dispc_fclk_rate();
+	}
+}
+
+static unsigned long dispc_mgr_pclk_rate(enum omap_channel channel)
+{
+	unsigned long r;
+
+	if (dss_mgr_is_lcd(channel)) {
+		int pcd;
+		u32 l;
+
+		l = dispc_read_reg(DISPC_DIVISORo(channel));
+
+		pcd = FLD_GET(l, 7, 0);
+
+		r = dispc_mgr_lclk_rate(channel);
+
+		return r / pcd;
+	} else {
+		return dispc.tv_pclk_rate;
+	}
+}
+
+void dispc_set_tv_pclk(unsigned long pclk)
+{
+	dispc.tv_pclk_rate = pclk;
+}
+
+static unsigned long dispc_core_clk_rate(void)
+{
+	return dispc.core_clk_rate;
+}
+
+static unsigned long dispc_plane_pclk_rate(enum omap_plane plane)
+{
+	enum omap_channel channel;
+
+	if (plane == OMAP_DSS_WB)
+		return 0;
+
+	channel = dispc_ovl_get_channel_out(plane);
+
+	return dispc_mgr_pclk_rate(channel);
+}
+
+static unsigned long dispc_plane_lclk_rate(enum omap_plane plane)
+{
+	enum omap_channel channel;
+
+	if (plane == OMAP_DSS_WB)
+		return 0;
+
+	channel	= dispc_ovl_get_channel_out(plane);
+
+	return dispc_mgr_lclk_rate(channel);
+}
+
+static void dispc_dump_clocks_channel(struct seq_file *s, enum omap_channel channel)
+{
+	int lcd, pcd;
+	enum omap_dss_clk_source lcd_clk_src;
+
+	seq_printf(s, "- %s -\n", mgr_desc[channel].name);
+
+	lcd_clk_src = dss_get_lcd_clk_source(channel);
+
+	seq_printf(s, "%s clk source = %s (%s)\n", mgr_desc[channel].name,
+		dss_get_generic_clk_source_name(lcd_clk_src),
+		dss_feat_get_clk_source_name(lcd_clk_src));
+
+	dispc_mgr_get_lcd_divisor(channel, &lcd, &pcd);
+
+	seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+		dispc_mgr_lclk_rate(channel), lcd);
+	seq_printf(s, "pck\t\t%-16lupck div\t%u\n",
+		dispc_mgr_pclk_rate(channel), pcd);
+}
+
+void dispc_dump_clocks(struct seq_file *s)
+{
+	int lcd;
+	u32 l;
+	enum omap_dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
+
+	if (dispc_runtime_get())
+		return;
+
+	seq_printf(s, "- DISPC -\n");
+
+	seq_printf(s, "dispc fclk source = %s (%s)\n",
+			dss_get_generic_clk_source_name(dispc_clk_src),
+			dss_feat_get_clk_source_name(dispc_clk_src));
+
+	seq_printf(s, "fck\t\t%-16lu\n", dispc_fclk_rate());
+
+	if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+		seq_printf(s, "- DISPC-CORE-CLK -\n");
+		l = dispc_read_reg(DISPC_DIVISOR);
+		lcd = FLD_GET(l, 23, 16);
+
+		seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+				(dispc_fclk_rate()/lcd), lcd);
+	}
+
+	dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD);
+
+	if (dss_has_feature(FEAT_MGR_LCD2))
+		dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD2);
+	if (dss_has_feature(FEAT_MGR_LCD3))
+		dispc_dump_clocks_channel(s, OMAP_DSS_CHANNEL_LCD3);
+
+	dispc_runtime_put();
+}
+
+static void dispc_dump_regs(struct seq_file *s)
+{
+	int i, j;
+	const char *mgr_names[] = {
+		[OMAP_DSS_CHANNEL_LCD]		= "LCD",
+		[OMAP_DSS_CHANNEL_DIGIT]	= "TV",
+		[OMAP_DSS_CHANNEL_LCD2]		= "LCD2",
+		[OMAP_DSS_CHANNEL_LCD3]		= "LCD3",
+	};
+	const char *ovl_names[] = {
+		[OMAP_DSS_GFX]		= "GFX",
+		[OMAP_DSS_VIDEO1]	= "VID1",
+		[OMAP_DSS_VIDEO2]	= "VID2",
+		[OMAP_DSS_VIDEO3]	= "VID3",
+		[OMAP_DSS_WB]		= "WB",
+	};
+	const char **p_names;
+
+#define DUMPREG(r) seq_printf(s, "%-50s %08x\n", #r, dispc_read_reg(r))
+
+	if (dispc_runtime_get())
+		return;
+
+	/* DISPC common registers */
+	DUMPREG(DISPC_REVISION);
+	DUMPREG(DISPC_SYSCONFIG);
+	DUMPREG(DISPC_SYSSTATUS);
+	DUMPREG(DISPC_IRQSTATUS);
+	DUMPREG(DISPC_IRQENABLE);
+	DUMPREG(DISPC_CONTROL);
+	DUMPREG(DISPC_CONFIG);
+	DUMPREG(DISPC_CAPABLE);
+	DUMPREG(DISPC_LINE_STATUS);
+	DUMPREG(DISPC_LINE_NUMBER);
+	if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER) ||
+			dss_has_feature(FEAT_ALPHA_FREE_ZORDER))
+		DUMPREG(DISPC_GLOBAL_ALPHA);
+	if (dss_has_feature(FEAT_MGR_LCD2)) {
+		DUMPREG(DISPC_CONTROL2);
+		DUMPREG(DISPC_CONFIG2);
+	}
+	if (dss_has_feature(FEAT_MGR_LCD3)) {
+		DUMPREG(DISPC_CONTROL3);
+		DUMPREG(DISPC_CONFIG3);
+	}
+	if (dss_has_feature(FEAT_MFLAG))
+		DUMPREG(DISPC_GLOBAL_MFLAG_ATTRIBUTE);
+
+#undef DUMPREG
+
+#define DISPC_REG(i, name) name(i)
+#define DUMPREG(i, r) seq_printf(s, "%s(%s)%*s %08x\n", #r, p_names[i], \
+	(int)(48 - strlen(#r) - strlen(p_names[i])), " ", \
+	dispc_read_reg(DISPC_REG(i, r)))
+
+	p_names = mgr_names;
+
+	/* DISPC channel specific registers */
+	for (i = 0; i < dss_feat_get_num_mgrs(); i++) {
+		DUMPREG(i, DISPC_DEFAULT_COLOR);
+		DUMPREG(i, DISPC_TRANS_COLOR);
+		DUMPREG(i, DISPC_SIZE_MGR);
+
+		if (i == OMAP_DSS_CHANNEL_DIGIT)
+			continue;
+
+		DUMPREG(i, DISPC_TIMING_H);
+		DUMPREG(i, DISPC_TIMING_V);
+		DUMPREG(i, DISPC_POL_FREQ);
+		DUMPREG(i, DISPC_DIVISORo);
+
+		DUMPREG(i, DISPC_DATA_CYCLE1);
+		DUMPREG(i, DISPC_DATA_CYCLE2);
+		DUMPREG(i, DISPC_DATA_CYCLE3);
+
+		if (dss_has_feature(FEAT_CPR)) {
+			DUMPREG(i, DISPC_CPR_COEF_R);
+			DUMPREG(i, DISPC_CPR_COEF_G);
+			DUMPREG(i, DISPC_CPR_COEF_B);
+		}
+	}
+
+	p_names = ovl_names;
+
+	for (i = 0; i < dss_feat_get_num_ovls(); i++) {
+		DUMPREG(i, DISPC_OVL_BA0);
+		DUMPREG(i, DISPC_OVL_BA1);
+		DUMPREG(i, DISPC_OVL_POSITION);
+		DUMPREG(i, DISPC_OVL_SIZE);
+		DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+		DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+		DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+		DUMPREG(i, DISPC_OVL_ROW_INC);
+		DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+		if (dss_has_feature(FEAT_PRELOAD))
+			DUMPREG(i, DISPC_OVL_PRELOAD);
+		if (dss_has_feature(FEAT_MFLAG))
+			DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+		if (i == OMAP_DSS_GFX) {
+			DUMPREG(i, DISPC_OVL_WINDOW_SKIP);
+			DUMPREG(i, DISPC_OVL_TABLE_BA);
+			continue;
+		}
+
+		DUMPREG(i, DISPC_OVL_FIR);
+		DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+		DUMPREG(i, DISPC_OVL_ACCU0);
+		DUMPREG(i, DISPC_OVL_ACCU1);
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			DUMPREG(i, DISPC_OVL_BA0_UV);
+			DUMPREG(i, DISPC_OVL_BA1_UV);
+			DUMPREG(i, DISPC_OVL_FIR2);
+			DUMPREG(i, DISPC_OVL_ACCU2_0);
+			DUMPREG(i, DISPC_OVL_ACCU2_1);
+		}
+		if (dss_has_feature(FEAT_ATTR2))
+			DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+	}
+
+	if (dispc.feat->has_writeback) {
+		i = OMAP_DSS_WB;
+		DUMPREG(i, DISPC_OVL_BA0);
+		DUMPREG(i, DISPC_OVL_BA1);
+		DUMPREG(i, DISPC_OVL_SIZE);
+		DUMPREG(i, DISPC_OVL_ATTRIBUTES);
+		DUMPREG(i, DISPC_OVL_FIFO_THRESHOLD);
+		DUMPREG(i, DISPC_OVL_FIFO_SIZE_STATUS);
+		DUMPREG(i, DISPC_OVL_ROW_INC);
+		DUMPREG(i, DISPC_OVL_PIXEL_INC);
+
+		if (dss_has_feature(FEAT_MFLAG))
+			DUMPREG(i, DISPC_OVL_MFLAG_THRESHOLD);
+
+		DUMPREG(i, DISPC_OVL_FIR);
+		DUMPREG(i, DISPC_OVL_PICTURE_SIZE);
+		DUMPREG(i, DISPC_OVL_ACCU0);
+		DUMPREG(i, DISPC_OVL_ACCU1);
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			DUMPREG(i, DISPC_OVL_BA0_UV);
+			DUMPREG(i, DISPC_OVL_BA1_UV);
+			DUMPREG(i, DISPC_OVL_FIR2);
+			DUMPREG(i, DISPC_OVL_ACCU2_0);
+			DUMPREG(i, DISPC_OVL_ACCU2_1);
+		}
+		if (dss_has_feature(FEAT_ATTR2))
+			DUMPREG(i, DISPC_OVL_ATTRIBUTES2);
+	}
+
+#undef DISPC_REG
+#undef DUMPREG
+
+#define DISPC_REG(plane, name, i) name(plane, i)
+#define DUMPREG(plane, name, i) \
+	seq_printf(s, "%s_%d(%s)%*s %08x\n", #name, i, p_names[plane], \
+	(int)(46 - strlen(#name) - strlen(p_names[plane])), " ", \
+	dispc_read_reg(DISPC_REG(plane, name, i)))
+
+	/* Video pipeline coefficient registers */
+
+	/* start from OMAP_DSS_VIDEO1 */
+	for (i = 1; i < dss_feat_get_num_ovls(); i++) {
+		for (j = 0; j < 8; j++)
+			DUMPREG(i, DISPC_OVL_FIR_COEF_H, j);
+
+		for (j = 0; j < 8; j++)
+			DUMPREG(i, DISPC_OVL_FIR_COEF_HV, j);
+
+		for (j = 0; j < 5; j++)
+			DUMPREG(i, DISPC_OVL_CONV_COEF, j);
+
+		if (dss_has_feature(FEAT_FIR_COEF_V)) {
+			for (j = 0; j < 8; j++)
+				DUMPREG(i, DISPC_OVL_FIR_COEF_V, j);
+		}
+
+		if (dss_has_feature(FEAT_HANDLE_UV_SEPARATE)) {
+			for (j = 0; j < 8; j++)
+				DUMPREG(i, DISPC_OVL_FIR_COEF_H2, j);
+
+			for (j = 0; j < 8; j++)
+				DUMPREG(i, DISPC_OVL_FIR_COEF_HV2, j);
+
+			for (j = 0; j < 8; j++)
+				DUMPREG(i, DISPC_OVL_FIR_COEF_V2, j);
+		}
+	}
+
+	dispc_runtime_put();
+
+#undef DISPC_REG
+#undef DUMPREG
+}
+
+/* calculate clock rates using dividers in cinfo */
+int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
+		struct dispc_clock_info *cinfo)
+{
+	if (cinfo->lck_div > 255 || cinfo->lck_div == 0)
+		return -EINVAL;
+	if (cinfo->pck_div < 1 || cinfo->pck_div > 255)
+		return -EINVAL;
+
+	cinfo->lck = dispc_fclk_rate / cinfo->lck_div;
+	cinfo->pck = cinfo->lck / cinfo->pck_div;
+
+	return 0;
+}
+
+bool dispc_div_calc(unsigned long dispc,
+		unsigned long pck_min, unsigned long pck_max,
+		dispc_div_calc_func func, void *data)
+{
+	int lckd, lckd_start, lckd_stop;
+	int pckd, pckd_start, pckd_stop;
+	unsigned long pck, lck;
+	unsigned long lck_max;
+	unsigned long pckd_hw_min, pckd_hw_max;
+	unsigned min_fck_per_pck;
+	unsigned long fck;
+
+#ifdef CONFIG_FB_OMAP2_DSS_MIN_FCK_PER_PCK
+	min_fck_per_pck = CONFIG_FB_OMAP2_DSS_MIN_FCK_PER_PCK;
+#else
+	min_fck_per_pck = 0;
+#endif
+
+	pckd_hw_min = dss_feat_get_param_min(FEAT_PARAM_DSS_PCD);
+	pckd_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_PCD);
+
+	lck_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	pck_min = pck_min ? pck_min : 1;
+	pck_max = pck_max ? pck_max : ULONG_MAX;
+
+	lckd_start = max(DIV_ROUND_UP(dispc, lck_max), 1ul);
+	lckd_stop = min(dispc / pck_min, 255ul);
+
+	for (lckd = lckd_start; lckd <= lckd_stop; ++lckd) {
+		lck = dispc / lckd;
+
+		pckd_start = max(DIV_ROUND_UP(lck, pck_max), pckd_hw_min);
+		pckd_stop = min(lck / pck_min, pckd_hw_max);
+
+		for (pckd = pckd_start; pckd <= pckd_stop; ++pckd) {
+			pck = lck / pckd;
+
+			/*
+			 * For OMAP2/3 the DISPC fclk is the same as LCD's logic
+			 * clock, which means we're configuring DISPC fclk here
+			 * also. Thus we need to use the calculated lck. For
+			 * OMAP4+ the DISPC fclk is a separate clock.
+			 */
+			if (dss_has_feature(FEAT_CORE_CLK_DIV))
+				fck = dispc_core_clk_rate();
+			else
+				fck = lck;
+
+			if (fck < pck * min_fck_per_pck)
+				continue;
+
+			if (func(lckd, pckd, lck, pck, data))
+				return true;
+		}
+	}
+
+	return false;
+}
+
+void dispc_mgr_set_clock_div(enum omap_channel channel,
+		const struct dispc_clock_info *cinfo)
+{
+	DSSDBG("lck = %lu (%u)\n", cinfo->lck, cinfo->lck_div);
+	DSSDBG("pck = %lu (%u)\n", cinfo->pck, cinfo->pck_div);
+
+	dispc_mgr_set_lcd_divisor(channel, cinfo->lck_div, cinfo->pck_div);
+}
+
+int dispc_mgr_get_clock_div(enum omap_channel channel,
+		struct dispc_clock_info *cinfo)
+{
+	unsigned long fck;
+
+	fck = dispc_fclk_rate();
+
+	cinfo->lck_div = REG_GET(DISPC_DIVISORo(channel), 23, 16);
+	cinfo->pck_div = REG_GET(DISPC_DIVISORo(channel), 7, 0);
+
+	cinfo->lck = fck / cinfo->lck_div;
+	cinfo->pck = cinfo->lck / cinfo->pck_div;
+
+	return 0;
+}
+
+u32 dispc_read_irqstatus(void)
+{
+	return dispc_read_reg(DISPC_IRQSTATUS);
+}
+EXPORT_SYMBOL(dispc_read_irqstatus);
+
+void dispc_clear_irqstatus(u32 mask)
+{
+	dispc_write_reg(DISPC_IRQSTATUS, mask);
+}
+EXPORT_SYMBOL(dispc_clear_irqstatus);
+
+u32 dispc_read_irqenable(void)
+{
+	return dispc_read_reg(DISPC_IRQENABLE);
+}
+EXPORT_SYMBOL(dispc_read_irqenable);
+
+void dispc_write_irqenable(u32 mask)
+{
+	u32 old_mask = dispc_read_reg(DISPC_IRQENABLE);
+
+	/* clear the irqstatus for newly enabled irqs */
+	dispc_clear_irqstatus((mask ^ old_mask) & mask);
+
+	dispc_write_reg(DISPC_IRQENABLE, mask);
+}
+EXPORT_SYMBOL(dispc_write_irqenable);
+
+void dispc_enable_sidle(void)
+{
+	REG_FLD_MOD(DISPC_SYSCONFIG, 2, 4, 3);	/* SIDLEMODE: smart idle */
+}
+
+void dispc_disable_sidle(void)
+{
+	REG_FLD_MOD(DISPC_SYSCONFIG, 1, 4, 3);	/* SIDLEMODE: no idle */
+}
+
+static void _omap_dispc_initial_config(void)
+{
+	u32 l;
+
+	/* Exclusively enable DISPC_CORE_CLK and set divider to 1 */
+	if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+		l = dispc_read_reg(DISPC_DIVISOR);
+		/* Use DISPC_DIVISOR.LCD, instead of DISPC_DIVISOR1.LCD */
+		l = FLD_MOD(l, 1, 0, 0);
+		l = FLD_MOD(l, 1, 23, 16);
+		dispc_write_reg(DISPC_DIVISOR, l);
+
+		dispc.core_clk_rate = dispc_fclk_rate();
+	}
+
+	/* FUNCGATED */
+	if (dss_has_feature(FEAT_FUNCGATED))
+		REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
+
+	dispc_setup_color_conv_coef();
+
+	dispc_set_loadmode(OMAP_DSS_LOAD_FRAME_ONLY);
+
+	dispc_init_fifos();
+
+	dispc_configure_burst_sizes();
+
+	dispc_ovl_enable_zorder_planes();
+
+	if (dispc.feat->mstandby_workaround)
+		REG_FLD_MOD(DISPC_MSTANDBY_CTRL, 1, 0, 0);
+
+	if (dss_has_feature(FEAT_MFLAG))
+		dispc_init_mflag();
+}
+
+static const struct dispc_features omap24xx_dispc_feats = {
+	.sw_start		=	5,
+	.fp_start		=	15,
+	.bp_start		=	27,
+	.sw_max			=	64,
+	.vp_max			=	255,
+	.hp_max			=	256,
+	.mgr_width_start	=	10,
+	.mgr_height_start	=	26,
+	.mgr_width_max		=	2048,
+	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	66500000,
+	.calc_scaling		=	dispc_ovl_calc_scaling_24xx,
+	.calc_core_clk		=	calc_core_clk_24xx,
+	.num_fifos		=	3,
+	.no_framedone_tv	=	true,
+	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
+};
+
+static const struct dispc_features omap34xx_rev1_0_dispc_feats = {
+	.sw_start		=	5,
+	.fp_start		=	15,
+	.bp_start		=	27,
+	.sw_max			=	64,
+	.vp_max			=	255,
+	.hp_max			=	256,
+	.mgr_width_start	=	10,
+	.mgr_height_start	=	26,
+	.mgr_width_max		=	2048,
+	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	173000000,
+	.max_tv_pclk		=	59000000,
+	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
+	.calc_core_clk		=	calc_core_clk_34xx,
+	.num_fifos		=	3,
+	.no_framedone_tv	=	true,
+	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
+};
+
+static const struct dispc_features omap34xx_rev3_0_dispc_feats = {
+	.sw_start		=	7,
+	.fp_start		=	19,
+	.bp_start		=	31,
+	.sw_max			=	256,
+	.vp_max			=	4095,
+	.hp_max			=	4096,
+	.mgr_width_start	=	10,
+	.mgr_height_start	=	26,
+	.mgr_width_max		=	2048,
+	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	173000000,
+	.max_tv_pclk		=	59000000,
+	.calc_scaling		=	dispc_ovl_calc_scaling_34xx,
+	.calc_core_clk		=	calc_core_clk_34xx,
+	.num_fifos		=	3,
+	.no_framedone_tv	=	true,
+	.set_max_preload	=	false,
+	.last_pixel_inc_missing	=	true,
+};
+
+static const struct dispc_features omap44xx_dispc_feats = {
+	.sw_start		=	7,
+	.fp_start		=	19,
+	.bp_start		=	31,
+	.sw_max			=	256,
+	.vp_max			=	4095,
+	.hp_max			=	4096,
+	.mgr_width_start	=	10,
+	.mgr_height_start	=	26,
+	.mgr_width_max		=	2048,
+	.mgr_height_max		=	2048,
+	.max_lcd_pclk		=	170000000,
+	.max_tv_pclk		=	185625000,
+	.calc_scaling		=	dispc_ovl_calc_scaling_44xx,
+	.calc_core_clk		=	calc_core_clk_44xx,
+	.num_fifos		=	5,
+	.gfx_fifo_workaround	=	true,
+	.set_max_preload	=	true,
+	.supports_sync_align	=	true,
+	.has_writeback		=	true,
+};
+
+static const struct dispc_features omap54xx_dispc_feats = {
+	.sw_start		=	7,
+	.fp_start		=	19,
+	.bp_start		=	31,
+	.sw_max			=	256,
+	.vp_max			=	4095,
+	.hp_max			=	4096,
+	.mgr_width_start	=	11,
+	.mgr_height_start	=	27,
+	.mgr_width_max		=	4096,
+	.mgr_height_max		=	4096,
+	.max_lcd_pclk		=	170000000,
+	.max_tv_pclk		=	186000000,
+	.calc_scaling		=	dispc_ovl_calc_scaling_44xx,
+	.calc_core_clk		=	calc_core_clk_44xx,
+	.num_fifos		=	5,
+	.gfx_fifo_workaround	=	true,
+	.mstandby_workaround	=	true,
+	.set_max_preload	=	true,
+	.supports_sync_align	=	true,
+	.has_writeback		=	true,
+};
+
+static int dispc_init_features(struct platform_device *pdev)
+{
+	const struct dispc_features *src;
+	struct dispc_features *dst;
+
+	dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(&pdev->dev, "Failed to allocate DISPC Features\n");
+		return -ENOMEM;
+	}
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+		src = &omap24xx_dispc_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP34xx_ES1:
+		src = &omap34xx_rev1_0_dispc_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
+		src = &omap34xx_rev3_0_dispc_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		src = &omap44xx_dispc_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+	case OMAPDSS_VER_DRA7xx:
+		src = &omap54xx_dispc_feats;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	dispc.feat = dst;
+
+	return 0;
+}
+
+static irqreturn_t dispc_irq_handler(int irq, void *arg)
+{
+	if (!dispc.is_enabled)
+		return IRQ_NONE;
+
+	return dispc.user_handler(irq, dispc.user_data);
+}
+
+int dispc_request_irq(irq_handler_t handler, void *dev_id)
+{
+	int r;
+
+	if (dispc.user_handler != NULL)
+		return -EBUSY;
+
+	dispc.user_handler = handler;
+	dispc.user_data = dev_id;
+
+	/* ensure the dispc_irq_handler sees the values above */
+	smp_wmb();
+
+	r = devm_request_irq(&dispc.pdev->dev, dispc.irq, dispc_irq_handler,
+			     IRQF_SHARED, "OMAP DISPC", &dispc);
+	if (r) {
+		dispc.user_handler = NULL;
+		dispc.user_data = NULL;
+	}
+
+	return r;
+}
+EXPORT_SYMBOL(dispc_request_irq);
+
+void dispc_free_irq(void *dev_id)
+{
+	devm_free_irq(&dispc.pdev->dev, dispc.irq, &dispc);
+
+	dispc.user_handler = NULL;
+	dispc.user_data = NULL;
+}
+EXPORT_SYMBOL(dispc_free_irq);
+
+/* DISPC HW IP initialisation */
+static int dispc_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	u32 rev;
+	int r = 0;
+	struct resource *dispc_mem;
+	struct device_node *np = pdev->dev.of_node;
+
+	dispc.pdev = pdev;
+
+	spin_lock_init(&dispc.control_lock);
+
+	r = dispc_init_features(dispc.pdev);
+	if (r)
+		return r;
+
+	dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+	if (!dispc_mem) {
+		DSSERR("can't get IORESOURCE_MEM DISPC\n");
+		return -EINVAL;
+	}
+
+	dispc.base = devm_ioremap(&pdev->dev, dispc_mem->start,
+				  resource_size(dispc_mem));
+	if (!dispc.base) {
+		DSSERR("can't ioremap DISPC\n");
+		return -ENOMEM;
+	}
+
+	dispc.irq = platform_get_irq(dispc.pdev, 0);
+	if (dispc.irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		return -ENODEV;
+	}
+
+	if (np && of_property_read_bool(np, "syscon-pol")) {
+		dispc.syscon_pol = syscon_regmap_lookup_by_phandle(np, "syscon-pol");
+		if (IS_ERR(dispc.syscon_pol)) {
+			dev_err(&pdev->dev, "failed to get syscon-pol regmap\n");
+			return PTR_ERR(dispc.syscon_pol);
+		}
+
+		if (of_property_read_u32_index(np, "syscon-pol", 1,
+				&dispc.syscon_pol_offset)) {
+			dev_err(&pdev->dev, "failed to get syscon-pol offset\n");
+			return -EINVAL;
+		}
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	_omap_dispc_initial_config();
+
+	rev = dispc_read_reg(DISPC_REVISION);
+	dev_dbg(&pdev->dev, "OMAP DISPC rev %d.%d\n",
+	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+	dispc_runtime_put();
+
+	dss_init_overlay_managers();
+
+	dss_debugfs_create_file("dispc", dispc_dump_regs);
+
+	return 0;
+
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	return r;
+}
+
+static void dispc_unbind(struct device *dev, struct device *master,
+			       void *data)
+{
+	pm_runtime_disable(dev);
+
+	dss_uninit_overlay_managers();
+}
+
+static const struct component_ops dispc_component_ops = {
+	.bind	= dispc_bind,
+	.unbind	= dispc_unbind,
+};
+
+static int dispc_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dispc_component_ops);
+}
+
+static int dispc_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dispc_component_ops);
+	return 0;
+}
+
+static int dispc_runtime_suspend(struct device *dev)
+{
+	dispc.is_enabled = false;
+	/* ensure the dispc_irq_handler sees the is_enabled value */
+	smp_wmb();
+	/* wait for current handler to finish before turning the DISPC off */
+	synchronize_irq(dispc.irq);
+
+	dispc_save_context();
+
+	return 0;
+}
+
+static int dispc_runtime_resume(struct device *dev)
+{
+	/*
+	 * The reset value for load mode is 0 (OMAP_DSS_LOAD_CLUT_AND_FRAME)
+	 * but we always initialize it to 2 (OMAP_DSS_LOAD_FRAME_ONLY) in
+	 * _omap_dispc_initial_config(). We can thus use it to detect if
+	 * we have lost register context.
+	 */
+	if (REG_GET(DISPC_CONFIG, 2, 1) != OMAP_DSS_LOAD_FRAME_ONLY) {
+		_omap_dispc_initial_config();
+
+		dispc_restore_context();
+	}
+
+	dispc.is_enabled = true;
+	/* ensure the dispc_irq_handler sees the is_enabled value */
+	smp_wmb();
+
+	return 0;
+}
+
+static const struct dev_pm_ops dispc_pm_ops = {
+	.runtime_suspend = dispc_runtime_suspend,
+	.runtime_resume = dispc_runtime_resume,
+};
+
+static const struct of_device_id dispc_of_match[] = {
+	{ .compatible = "ti,omap2-dispc", },
+	{ .compatible = "ti,omap3-dispc", },
+	{ .compatible = "ti,omap4-dispc", },
+	{ .compatible = "ti,omap5-dispc", },
+	{ .compatible = "ti,dra7-dispc", },
+	{},
+};
+
+static struct platform_driver omap_dispchw_driver = {
+	.probe		= dispc_probe,
+	.remove         = dispc_remove,
+	.driver         = {
+		.name   = "omapdss_dispc",
+		.pm	= &dispc_pm_ops,
+		.of_match_table = dispc_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init dispc_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_dispchw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc.h b/drivers/video/fbdev/omap2/omapfb/dss/dispc.h
new file mode 100644
index 000000000000..483744223dd1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc.h
@@ -0,0 +1,918 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc.h
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 __OMAP2_DISPC_REG_H
+#define __OMAP2_DISPC_REG_H
+
+/* DISPC common registers */
+#define DISPC_REVISION			0x0000
+#define DISPC_SYSCONFIG			0x0010
+#define DISPC_SYSSTATUS			0x0014
+#define DISPC_IRQSTATUS			0x0018
+#define DISPC_IRQENABLE			0x001C
+#define DISPC_CONTROL			0x0040
+#define DISPC_CONFIG			0x0044
+#define DISPC_CAPABLE			0x0048
+#define DISPC_LINE_STATUS		0x005C
+#define DISPC_LINE_NUMBER		0x0060
+#define DISPC_GLOBAL_ALPHA		0x0074
+#define DISPC_CONTROL2			0x0238
+#define DISPC_CONFIG2			0x0620
+#define DISPC_DIVISOR			0x0804
+#define DISPC_GLOBAL_BUFFER		0x0800
+#define DISPC_CONTROL3                  0x0848
+#define DISPC_CONFIG3                   0x084C
+#define DISPC_MSTANDBY_CTRL		0x0858
+#define DISPC_GLOBAL_MFLAG_ATTRIBUTE	0x085C
+
+/* DISPC overlay registers */
+#define DISPC_OVL_BA0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA0_OFFSET(n))
+#define DISPC_OVL_BA1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA1_OFFSET(n))
+#define DISPC_OVL_BA0_UV(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA0_UV_OFFSET(n))
+#define DISPC_OVL_BA1_UV(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_BA1_UV_OFFSET(n))
+#define DISPC_OVL_POSITION(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_POS_OFFSET(n))
+#define DISPC_OVL_SIZE(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_SIZE_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ATTR_OFFSET(n))
+#define DISPC_OVL_ATTRIBUTES2(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_ATTR2_OFFSET(n))
+#define DISPC_OVL_FIFO_THRESHOLD(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIFO_THRESH_OFFSET(n))
+#define DISPC_OVL_FIFO_SIZE_STATUS(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIFO_SIZE_STATUS_OFFSET(n))
+#define DISPC_OVL_ROW_INC(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ROW_INC_OFFSET(n))
+#define DISPC_OVL_PIXEL_INC(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_PIX_INC_OFFSET(n))
+#define DISPC_OVL_WINDOW_SKIP(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_WINDOW_SKIP_OFFSET(n))
+#define DISPC_OVL_TABLE_BA(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_TABLE_BA_OFFSET(n))
+#define DISPC_OVL_FIR(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_OFFSET(n))
+#define DISPC_OVL_FIR2(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_FIR2_OFFSET(n))
+#define DISPC_OVL_PICTURE_SIZE(n)	(DISPC_OVL_BASE(n) + \
+					DISPC_PIC_SIZE_OFFSET(n))
+#define DISPC_OVL_ACCU0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU0_OFFSET(n))
+#define DISPC_OVL_ACCU1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU1_OFFSET(n))
+#define DISPC_OVL_ACCU2_0(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU2_0_OFFSET(n))
+#define DISPC_OVL_ACCU2_1(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_ACCU2_1_OFFSET(n))
+#define DISPC_OVL_FIR_COEF_H(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_H_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_HV_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_H2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_H2_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_HV2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_HV2_OFFSET(n, i))
+#define DISPC_OVL_CONV_COEF(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_CONV_COEF_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_V_OFFSET(n, i))
+#define DISPC_OVL_FIR_COEF_V2(n, i)	(DISPC_OVL_BASE(n) + \
+					DISPC_FIR_COEF_V2_OFFSET(n, i))
+#define DISPC_OVL_PRELOAD(n)		(DISPC_OVL_BASE(n) + \
+					DISPC_PRELOAD_OFFSET(n))
+#define DISPC_OVL_MFLAG_THRESHOLD(n)	DISPC_MFLAG_THRESHOLD_OFFSET(n)
+
+/* DISPC up/downsampling FIR filter coefficient structure */
+struct dispc_coef {
+	s8 hc4_vc22;
+	s8 hc3_vc2;
+	u8 hc2_vc1;
+	s8 hc1_vc0;
+	s8 hc0_vc00;
+};
+
+const struct dispc_coef *dispc_ovl_get_scale_coef(int inc, int five_taps);
+
+/* DISPC manager/channel specific registers */
+static inline u16 DISPC_DEFAULT_COLOR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x004C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0050;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03AC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0814;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_TRANS_COLOR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0054;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0058;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B0;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0818;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_TIMING_H(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0064;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0400;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0840;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_TIMING_V(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0068;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0404;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0844;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_POL_FREQ(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x006C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x0408;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x083C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_DIVISORo(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0070;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x040C;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0838;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* Named as DISPC_SIZE_LCD, DISPC_SIZE_DIGIT and DISPC_SIZE_LCD2 in TRM */
+static inline u16 DISPC_SIZE_MGR(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x007C;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		return 0x0078;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03CC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0834;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE1(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01D4;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C0;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0828;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE2(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01D8;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C4;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x082C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_DATA_CYCLE3(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x01DC;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03C8;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0830;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_R(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0220;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03BC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0824;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_G(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0224;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B8;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x0820;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_CPR_COEF_B(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return 0x0228;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		BUG();
+		return 0;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return 0x03B4;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return 0x081C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* DISPC overlay register base addresses */
+static inline u16 DISPC_OVL_BASE(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0080;
+	case OMAP_DSS_VIDEO1:
+		return 0x00BC;
+	case OMAP_DSS_VIDEO2:
+		return 0x014C;
+	case OMAP_DSS_VIDEO3:
+		return 0x0300;
+	case OMAP_DSS_WB:
+		return 0x0500;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* DISPC overlay register offsets */
+static inline u16 DISPC_BA0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0000;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0008;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_BA1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0004;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x000C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_BA0_UV_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0544;
+	case OMAP_DSS_VIDEO2:
+		return 0x04BC;
+	case OMAP_DSS_VIDEO3:
+		return 0x0310;
+	case OMAP_DSS_WB:
+		return 0x0118;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_BA1_UV_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0548;
+	case OMAP_DSS_VIDEO2:
+		return 0x04C0;
+	case OMAP_DSS_VIDEO3:
+		return 0x0314;
+	case OMAP_DSS_WB:
+		return 0x011C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_POS_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0008;
+	case OMAP_DSS_VIDEO3:
+		return 0x009C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_SIZE_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x000C;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x00A8;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ATTR_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0020;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0010;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0070;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ATTR2_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0568;
+	case OMAP_DSS_VIDEO2:
+		return 0x04DC;
+	case OMAP_DSS_VIDEO3:
+		return 0x032C;
+	case OMAP_DSS_WB:
+		return 0x0310;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_FIFO_THRESH_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0024;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0014;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x008C;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_FIFO_SIZE_STATUS_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0028;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0018;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0088;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ROW_INC_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x002C;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x001C;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x00A4;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_PIX_INC_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0030;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0020;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0098;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_WINDOW_SKIP_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0034;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+	case OMAP_DSS_VIDEO3:
+		BUG();
+		return 0;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_TABLE_BA_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0038;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+	case OMAP_DSS_VIDEO3:
+		BUG();
+		return 0;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_FIR_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0024;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0090;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_FIR2_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0580;
+	case OMAP_DSS_VIDEO2:
+		return 0x055C;
+	case OMAP_DSS_VIDEO3:
+		return 0x0424;
+	case OMAP_DSS_WB:
+		return 0x290;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_PIC_SIZE_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0028;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0094;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+
+static inline u16 DISPC_ACCU0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x002C;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0000;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ACCU2_0_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0584;
+	case OMAP_DSS_VIDEO2:
+		return 0x0560;
+	case OMAP_DSS_VIDEO3:
+		return 0x0428;
+	case OMAP_DSS_WB:
+		return 0x0294;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ACCU1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0030;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0004;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_ACCU2_1_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0588;
+	case OMAP_DSS_VIDEO2:
+		return 0x0564;
+	case OMAP_DSS_VIDEO3:
+		return 0x042C;
+	case OMAP_DSS_WB:
+		return 0x0298;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0034 + i * 0x8;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0010 + i * 0x8;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_H2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x058C + i * 0x8;
+	case OMAP_DSS_VIDEO2:
+		return 0x0568 + i * 0x8;
+	case OMAP_DSS_VIDEO3:
+		return 0x0430 + i * 0x8;
+	case OMAP_DSS_WB:
+		return 0x02A0 + i * 0x8;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+		return 0x0038 + i * 0x8;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0014 + i * 0x8;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_HV2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0590 + i * 8;
+	case OMAP_DSS_VIDEO2:
+		return 0x056C + i * 0x8;
+	case OMAP_DSS_VIDEO3:
+		return 0x0434 + i * 0x8;
+	case OMAP_DSS_WB:
+		return 0x02A4 + i * 0x8;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4,} */
+static inline u16 DISPC_CONV_COEF_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+	case OMAP_DSS_VIDEO2:
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0074 + i * 0x4;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x0124 + i * 0x4;
+	case OMAP_DSS_VIDEO2:
+		return 0x00B4 + i * 0x4;
+	case OMAP_DSS_VIDEO3:
+	case OMAP_DSS_WB:
+		return 0x0050 + i * 0x4;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+/* coef index i = {0, 1, 2, 3, 4, 5, 6, 7} */
+static inline u16 DISPC_FIR_COEF_V2_OFFSET(enum omap_plane plane, u16 i)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		BUG();
+		return 0;
+	case OMAP_DSS_VIDEO1:
+		return 0x05CC + i * 0x4;
+	case OMAP_DSS_VIDEO2:
+		return 0x05A8 + i * 0x4;
+	case OMAP_DSS_VIDEO3:
+		return 0x0470 + i * 0x4;
+	case OMAP_DSS_WB:
+		return 0x02E0 + i * 0x4;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_PRELOAD_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x01AC;
+	case OMAP_DSS_VIDEO1:
+		return 0x0174;
+	case OMAP_DSS_VIDEO2:
+		return 0x00E8;
+	case OMAP_DSS_VIDEO3:
+		return 0x00A0;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static inline u16 DISPC_MFLAG_THRESHOLD_OFFSET(enum omap_plane plane)
+{
+	switch (plane) {
+	case OMAP_DSS_GFX:
+		return 0x0860;
+	case OMAP_DSS_VIDEO1:
+		return 0x0864;
+	case OMAP_DSS_VIDEO2:
+		return 0x0868;
+	case OMAP_DSS_VIDEO3:
+		return 0x086c;
+	case OMAP_DSS_WB:
+		return 0x0870;
+	default:
+		BUG();
+		return 0;
+	}
+}
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c b/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c
new file mode 100644
index 000000000000..038c15b04215
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dispc_coefs.c
@@ -0,0 +1,325 @@
+/*
+ * linux/drivers/video/omap2/dss/dispc_coefs.c
+ *
+ * Copyright (C) 2011 Texas Instruments
+ * Author: Chandrabhanu Mahapatra <cmahapatra@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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/kernel.h>
+#include <video/omapdss.h>
+
+#include "dispc.h"
+
+static const struct dispc_coef coef3_M8[8] = {
+	{ 0,  0, 128,  0, 0 },
+	{ 0, -4, 123,  9, 0 },
+	{ 0, -4, 108, 24, 0 },
+	{ 0, -2,  87, 43, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 43,  87, -2, 0 },
+	{ 0, 24, 108, -4, 0 },
+	{ 0,  9, 123, -4, 0 },
+};
+
+static const struct dispc_coef coef3_M9[8] = {
+	{ 0,  6, 116,  6, 0 },
+	{ 0,  0, 112, 16, 0 },
+	{ 0, -2, 100, 30, 0 },
+	{ 0, -2,  83, 47, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 47,  83, -2, 0 },
+	{ 0, 30, 100, -2, 0 },
+	{ 0, 16, 112,  0, 0 },
+};
+
+static const struct dispc_coef coef3_M10[8] = {
+	{ 0, 10, 108, 10, 0 },
+	{ 0,  3, 104, 21, 0 },
+	{ 0,  0,  94, 34, 0 },
+	{ 0, -1,  80, 49, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 49,  80, -1, 0 },
+	{ 0, 34,  94,  0, 0 },
+	{ 0, 21, 104,  3, 0 },
+};
+
+static const struct dispc_coef coef3_M11[8] = {
+	{ 0, 14, 100, 14, 0 },
+	{ 0,  6,  98, 24, 0 },
+	{ 0,  2,  90, 36, 0 },
+	{ 0,  0,  78, 50, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 50,  78,  0, 0 },
+	{ 0, 36,  90,  2, 0 },
+	{ 0, 24,  98,  6, 0 },
+};
+
+static const struct dispc_coef coef3_M12[8] = {
+	{ 0, 16,  96, 16, 0 },
+	{ 0,  9,  93, 26, 0 },
+	{ 0,  4,  86, 38, 0 },
+	{ 0,  1,  76, 51, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 51,  76,  1, 0 },
+	{ 0, 38,  86,  4, 0 },
+	{ 0, 26,  93,  9, 0 },
+};
+
+static const struct dispc_coef coef3_M13[8] = {
+	{ 0, 18,  92, 18, 0 },
+	{ 0, 10,  90, 28, 0 },
+	{ 0,  5,  83, 40, 0 },
+	{ 0,  1,  75, 52, 0 },
+	{ 0, 64,  64,  0, 0 },
+	{ 0, 52,  75,  1, 0 },
+	{ 0, 40,  83,  5, 0 },
+	{ 0, 28,  90, 10, 0 },
+};
+
+static const struct dispc_coef coef3_M14[8] = {
+	{ 0, 20, 88, 20, 0 },
+	{ 0, 12, 86, 30, 0 },
+	{ 0,  6, 81, 41, 0 },
+	{ 0,  2, 74, 52, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 52, 74,  2, 0 },
+	{ 0, 41, 81,  6, 0 },
+	{ 0, 30, 86, 12, 0 },
+};
+
+static const struct dispc_coef coef3_M16[8] = {
+	{ 0, 22, 84, 22, 0 },
+	{ 0, 14, 82, 32, 0 },
+	{ 0,  8, 78, 42, 0 },
+	{ 0,  3, 72, 53, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 53, 72,  3, 0 },
+	{ 0, 42, 78,  8, 0 },
+	{ 0, 32, 82, 14, 0 },
+};
+
+static const struct dispc_coef coef3_M19[8] = {
+	{ 0, 24, 80, 24, 0 },
+	{ 0, 16, 79, 33, 0 },
+	{ 0,  9, 76, 43, 0 },
+	{ 0,  4, 70, 54, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 54, 70,  4, 0 },
+	{ 0, 43, 76,  9, 0 },
+	{ 0, 33, 79, 16, 0 },
+};
+
+static const struct dispc_coef coef3_M22[8] = {
+	{ 0, 25, 78, 25, 0 },
+	{ 0, 17, 77, 34, 0 },
+	{ 0, 10, 74, 44, 0 },
+	{ 0,  5, 69, 54, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 54, 69,  5, 0 },
+	{ 0, 44, 74, 10, 0 },
+	{ 0, 34, 77, 17, 0 },
+};
+
+static const struct dispc_coef coef3_M26[8] = {
+	{ 0, 26, 76, 26, 0 },
+	{ 0, 19, 74, 35, 0 },
+	{ 0, 11, 72, 45, 0 },
+	{ 0,  5, 69, 54, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 54, 69,  5, 0 },
+	{ 0, 45, 72, 11, 0 },
+	{ 0, 35, 74, 19, 0 },
+};
+
+static const struct dispc_coef coef3_M32[8] = {
+	{ 0, 27, 74, 27, 0 },
+	{ 0, 19, 73, 36, 0 },
+	{ 0, 12, 71, 45, 0 },
+	{ 0,  6, 68, 54, 0 },
+	{ 0, 64, 64,  0, 0 },
+	{ 0, 54, 68,  6, 0 },
+	{ 0, 45, 71, 12, 0 },
+	{ 0, 36, 73, 19, 0 },
+};
+
+static const struct dispc_coef coef5_M8[8] = {
+	{   0,   0, 128,   0,   0 },
+	{  -2,  14, 125, -10,   1 },
+	{  -6,  33, 114, -15,   2 },
+	{ -10,  55,  98, -16,   1 },
+	{   0, -14,  78,  78, -14 },
+	{   1, -16,  98,  55, -10 },
+	{   2, -15, 114,  33,  -6 },
+	{   1, -10, 125,  14,  -2 },
+};
+
+static const struct dispc_coef coef5_M9[8] = {
+	{  -3,  10, 114,  10,  -3 },
+	{  -6,  24, 111,   0,  -1 },
+	{  -8,  40, 103,  -7,   0 },
+	{ -11,  58,  91, -11,   1 },
+	{   0, -12,  76,  76, -12 },
+	{   1, -11,  91,  58, -11 },
+	{   0,  -7, 103,  40,  -8 },
+	{  -1,   0, 111,  24,  -6 },
+};
+
+static const struct dispc_coef coef5_M10[8] = {
+	{  -4,  18, 100,  18,  -4 },
+	{  -6,  30,  99,   8,  -3 },
+	{  -8,  44,  93,   0,  -1 },
+	{  -9,  58,  84,  -5,   0 },
+	{   0,  -8,  72,  72,  -8 },
+	{   0,  -5,  84,  58,  -9 },
+	{  -1,   0,  93,  44,  -8 },
+	{  -3,   8,  99,  30,  -6 },
+};
+
+static const struct dispc_coef coef5_M11[8] = {
+	{  -5,  23,  92,  23,  -5 },
+	{  -6,  34,  90,  13,  -3 },
+	{  -6,  45,  85,   6,  -2 },
+	{  -6,  57,  78,   0,  -1 },
+	{   0,  -4,  68,  68,  -4 },
+	{  -1,   0,  78,  57,  -6 },
+	{  -2,   6,  85,  45,  -6 },
+	{  -3,  13,  90,  34,  -6 },
+};
+
+static const struct dispc_coef coef5_M12[8] = {
+	{  -4,  26,  84,  26,  -4 },
+	{  -5,  36,  82,  18,  -3 },
+	{  -4,  46,  78,  10,  -2 },
+	{  -3,  55,  72,   5,  -1 },
+	{   0,   0,  64,  64,   0 },
+	{  -1,   5,  72,  55,  -3 },
+	{  -2,  10,  78,  46,  -4 },
+	{  -3,  18,  82,  36,  -5 },
+};
+
+static const struct dispc_coef coef5_M13[8] = {
+	{  -3,  28,  78,  28,  -3 },
+	{  -3,  37,  76,  21,  -3 },
+	{  -2,  45,  73,  14,  -2 },
+	{   0,  53,  68,   8,  -1 },
+	{   0,   3,  61,  61,   3 },
+	{  -1,   8,  68,  53,   0 },
+	{  -2,  14,  73,  45,  -2 },
+	{  -3,  21,  76,  37,  -3 },
+};
+
+static const struct dispc_coef coef5_M14[8] = {
+	{  -2,  30,  72,  30,  -2 },
+	{  -1,  37,  71,  23,  -2 },
+	{   0,  45,  69,  16,  -2 },
+	{   3,  52,  64,  10,  -1 },
+	{   0,   6,  58,  58,   6 },
+	{  -1,  10,  64,  52,   3 },
+	{  -2,  16,  69,  45,   0 },
+	{  -2,  23,  71,  37,  -1 },
+};
+
+static const struct dispc_coef coef5_M16[8] = {
+	{   0,  31,  66,  31,   0 },
+	{   1,  38,  65,  25,  -1 },
+	{   3,  44,  62,  20,  -1 },
+	{   6,  49,  59,  14,   0 },
+	{   0,  10,  54,  54,  10 },
+	{   0,  14,  59,  49,   6 },
+	{  -1,  20,  62,  44,   3 },
+	{  -1,  25,  65,  38,   1 },
+};
+
+static const struct dispc_coef coef5_M19[8] = {
+	{   3,  32,  58,  32,   3 },
+	{   4,  38,  58,  27,   1 },
+	{   7,  42,  55,  23,   1 },
+	{  10,  46,  54,  18,   0 },
+	{   0,  14,  50,  50,  14 },
+	{   0,  18,  54,  46,  10 },
+	{   1,  23,  55,  42,   7 },
+	{   1,  27,  58,  38,   4 },
+};
+
+static const struct dispc_coef coef5_M22[8] = {
+	{   4,  33,  54,  33,   4 },
+	{   6,  37,  54,  28,   3 },
+	{   9,  41,  53,  24,   1 },
+	{  12,  45,  51,  20,   0 },
+	{   0,  16,  48,  48,  16 },
+	{   0,  20,  51,  45,  12 },
+	{   1,  24,  53,  41,   9 },
+	{   3,  28,  54,  37,   6 },
+};
+
+static const struct dispc_coef coef5_M26[8] = {
+	{   6,  33,  50,  33,   6 },
+	{   8,  36,  51,  29,   4 },
+	{  11,  40,  50,  25,   2 },
+	{  14,  43,  48,  22,   1 },
+	{   0,  18,  46,  46,  18 },
+	{   1,  22,  48,  43,  14 },
+	{   2,  25,  50,  40,  11 },
+	{   4,  29,  51,  36,   8 },
+};
+
+static const struct dispc_coef coef5_M32[8] = {
+	{   7,  33,  48,  33,   7 },
+	{  10,  36,  48,  29,   5 },
+	{  13,  39,  47,  26,   3 },
+	{  16,  42,  46,  23,   1 },
+	{   0,  19,  45,  45,  19 },
+	{   1,  23,  46,  42,  16 },
+	{   3,  26,  47,  39,  13 },
+	{   5,  29,  48,  36,  10 },
+};
+
+const struct dispc_coef *dispc_ovl_get_scale_coef(int inc, int five_taps)
+{
+	int i;
+	static const struct {
+		int Mmin;
+		int Mmax;
+		const struct dispc_coef *coef_3;
+		const struct dispc_coef *coef_5;
+	} coefs[] = {
+		{ 27, 32, coef3_M32, coef5_M32 },
+		{ 23, 26, coef3_M26, coef5_M26 },
+		{ 20, 22, coef3_M22, coef5_M22 },
+		{ 17, 19, coef3_M19, coef5_M19 },
+		{ 15, 16, coef3_M16, coef5_M16 },
+		{ 14, 14, coef3_M14, coef5_M14 },
+		{ 13, 13, coef3_M13, coef5_M13 },
+		{ 12, 12, coef3_M12, coef5_M12 },
+		{ 11, 11, coef3_M11, coef5_M11 },
+		{ 10, 10, coef3_M10, coef5_M10 },
+		{  9,  9,  coef3_M9,  coef5_M9 },
+		{  4,  8,  coef3_M8,  coef5_M8 },
+		/*
+		 * When upscaling more than two times, blockiness and outlines
+		 * around the image are observed when M8 tables are used. M11,
+		 * M16 and M19 tables are used to prevent this.
+		 */
+		{  3,  3, coef3_M11, coef5_M11 },
+		{  2,  2, coef3_M16, coef5_M16 },
+		{  0,  1, coef3_M19, coef5_M19 },
+	};
+
+	inc /= 128;
+	for (i = 0; i < ARRAY_SIZE(coefs); ++i)
+		if (inc >= coefs[i].Mmin && inc <= coefs[i].Mmax)
+			return five_taps ? coefs[i].coef_5 : coefs[i].coef_3;
+	return NULL;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c
new file mode 100644
index 000000000000..75b5286029ee
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/display-sysfs.c
@@ -0,0 +1,356 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DISPLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/sysfs.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+static ssize_t display_name_show(struct omap_dss_device *dssdev, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			dssdev->name ?
+			dssdev->name : "");
+}
+
+static ssize_t display_enabled_show(struct omap_dss_device *dssdev, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			omapdss_device_is_enabled(dssdev));
+}
+
+static ssize_t display_enabled_store(struct omap_dss_device *dssdev,
+		const char *buf, size_t size)
+{
+	int r;
+	bool enable;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	if (enable == omapdss_device_is_enabled(dssdev))
+		return size;
+
+	if (omapdss_device_is_connected(dssdev) == false)
+		return -ENODEV;
+
+	if (enable) {
+		r = dssdev->driver->enable(dssdev);
+		if (r)
+			return r;
+	} else {
+		dssdev->driver->disable(dssdev);
+	}
+
+	return size;
+}
+
+static ssize_t display_tear_show(struct omap_dss_device *dssdev, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			dssdev->driver->get_te ?
+			dssdev->driver->get_te(dssdev) : 0);
+}
+
+static ssize_t display_tear_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
+{
+	int r;
+	bool te;
+
+	if (!dssdev->driver->enable_te || !dssdev->driver->get_te)
+		return -ENOENT;
+
+	r = strtobool(buf, &te);
+	if (r)
+		return r;
+
+	r = dssdev->driver->enable_te(dssdev, te);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t display_timings_show(struct omap_dss_device *dssdev, char *buf)
+{
+	struct omap_video_timings t;
+
+	if (!dssdev->driver->get_timings)
+		return -ENOENT;
+
+	dssdev->driver->get_timings(dssdev, &t);
+
+	return snprintf(buf, PAGE_SIZE, "%u,%u/%u/%u/%u,%u/%u/%u/%u\n",
+			t.pixelclock,
+			t.x_res, t.hfp, t.hbp, t.hsw,
+			t.y_res, t.vfp, t.vbp, t.vsw);
+}
+
+static ssize_t display_timings_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
+{
+	struct omap_video_timings t = dssdev->panel.timings;
+	int r, found;
+
+	if (!dssdev->driver->set_timings || !dssdev->driver->check_timings)
+		return -ENOENT;
+
+	found = 0;
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+	if (strncmp("pal", buf, 3) == 0) {
+		t = omap_dss_pal_timings;
+		found = 1;
+	} else if (strncmp("ntsc", buf, 4) == 0) {
+		t = omap_dss_ntsc_timings;
+		found = 1;
+	}
+#endif
+	if (!found && sscanf(buf, "%u,%hu/%hu/%hu/%hu,%hu/%hu/%hu/%hu",
+				&t.pixelclock,
+				&t.x_res, &t.hfp, &t.hbp, &t.hsw,
+				&t.y_res, &t.vfp, &t.vbp, &t.vsw) != 9)
+		return -EINVAL;
+
+	r = dssdev->driver->check_timings(dssdev, &t);
+	if (r)
+		return r;
+
+	dssdev->driver->disable(dssdev);
+	dssdev->driver->set_timings(dssdev, &t);
+	r = dssdev->driver->enable(dssdev);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t display_rotate_show(struct omap_dss_device *dssdev, char *buf)
+{
+	int rotate;
+	if (!dssdev->driver->get_rotate)
+		return -ENOENT;
+	rotate = dssdev->driver->get_rotate(dssdev);
+	return snprintf(buf, PAGE_SIZE, "%u\n", rotate);
+}
+
+static ssize_t display_rotate_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
+{
+	int rot, r;
+
+	if (!dssdev->driver->set_rotate || !dssdev->driver->get_rotate)
+		return -ENOENT;
+
+	r = kstrtoint(buf, 0, &rot);
+	if (r)
+		return r;
+
+	r = dssdev->driver->set_rotate(dssdev, rot);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t display_mirror_show(struct omap_dss_device *dssdev, char *buf)
+{
+	int mirror;
+	if (!dssdev->driver->get_mirror)
+		return -ENOENT;
+	mirror = dssdev->driver->get_mirror(dssdev);
+	return snprintf(buf, PAGE_SIZE, "%u\n", mirror);
+}
+
+static ssize_t display_mirror_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
+{
+	int r;
+	bool mirror;
+
+	if (!dssdev->driver->set_mirror || !dssdev->driver->get_mirror)
+		return -ENOENT;
+
+	r = strtobool(buf, &mirror);
+	if (r)
+		return r;
+
+	r = dssdev->driver->set_mirror(dssdev, mirror);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t display_wss_show(struct omap_dss_device *dssdev, char *buf)
+{
+	unsigned int wss;
+
+	if (!dssdev->driver->get_wss)
+		return -ENOENT;
+
+	wss = dssdev->driver->get_wss(dssdev);
+
+	return snprintf(buf, PAGE_SIZE, "0x%05x\n", wss);
+}
+
+static ssize_t display_wss_store(struct omap_dss_device *dssdev,
+	const char *buf, size_t size)
+{
+	u32 wss;
+	int r;
+
+	if (!dssdev->driver->get_wss || !dssdev->driver->set_wss)
+		return -ENOENT;
+
+	r = kstrtou32(buf, 0, &wss);
+	if (r)
+		return r;
+
+	if (wss > 0xfffff)
+		return -EINVAL;
+
+	r = dssdev->driver->set_wss(dssdev, wss);
+	if (r)
+		return r;
+
+	return size;
+}
+
+struct display_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_dss_device *, char *);
+	ssize_t	(*store)(struct omap_dss_device *, const char *, size_t);
+};
+
+#define DISPLAY_ATTR(_name, _mode, _show, _store) \
+	struct display_attribute display_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static DISPLAY_ATTR(name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(display_name, S_IRUGO, display_name_show, NULL);
+static DISPLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
+		display_enabled_show, display_enabled_store);
+static DISPLAY_ATTR(tear_elim, S_IRUGO|S_IWUSR,
+		display_tear_show, display_tear_store);
+static DISPLAY_ATTR(timings, S_IRUGO|S_IWUSR,
+		display_timings_show, display_timings_store);
+static DISPLAY_ATTR(rotate, S_IRUGO|S_IWUSR,
+		display_rotate_show, display_rotate_store);
+static DISPLAY_ATTR(mirror, S_IRUGO|S_IWUSR,
+		display_mirror_show, display_mirror_store);
+static DISPLAY_ATTR(wss, S_IRUGO|S_IWUSR,
+		display_wss_show, display_wss_store);
+
+static struct attribute *display_sysfs_attrs[] = {
+	&display_attr_name.attr,
+	&display_attr_display_name.attr,
+	&display_attr_enabled.attr,
+	&display_attr_tear_elim.attr,
+	&display_attr_timings.attr,
+	&display_attr_rotate.attr,
+	&display_attr_mirror.attr,
+	&display_attr_wss.attr,
+	NULL
+};
+
+static ssize_t display_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_dss_device *dssdev;
+	struct display_attribute *display_attr;
+
+	dssdev = container_of(kobj, struct omap_dss_device, kobj);
+	display_attr = container_of(attr, struct display_attribute, attr);
+
+	if (!display_attr->show)
+		return -ENOENT;
+
+	return display_attr->show(dssdev, buf);
+}
+
+static ssize_t display_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_dss_device *dssdev;
+	struct display_attribute *display_attr;
+
+	dssdev = container_of(kobj, struct omap_dss_device, kobj);
+	display_attr = container_of(attr, struct display_attribute, attr);
+
+	if (!display_attr->store)
+		return -ENOENT;
+
+	return display_attr->store(dssdev, buf, size);
+}
+
+static const struct sysfs_ops display_sysfs_ops = {
+	.show = display_attr_show,
+	.store = display_attr_store,
+};
+
+static struct kobj_type display_ktype = {
+	.sysfs_ops = &display_sysfs_ops,
+	.default_attrs = display_sysfs_attrs,
+};
+
+int display_init_sysfs(struct platform_device *pdev)
+{
+	struct omap_dss_device *dssdev = NULL;
+	int r;
+
+	for_each_dss_dev(dssdev) {
+		r = kobject_init_and_add(&dssdev->kobj, &display_ktype,
+			&pdev->dev.kobj, "%s", dssdev->alias);
+		if (r) {
+			DSSERR("failed to create sysfs files\n");
+			omap_dss_put_device(dssdev);
+			goto err;
+		}
+	}
+
+	return 0;
+
+err:
+	display_uninit_sysfs(pdev);
+
+	return r;
+}
+
+void display_uninit_sysfs(struct platform_device *pdev)
+{
+	struct omap_dss_device *dssdev = NULL;
+
+	for_each_dss_dev(dssdev) {
+		if (kobject_name(&dssdev->kobj) == NULL)
+			continue;
+
+		kobject_del(&dssdev->kobj);
+		kobject_put(&dssdev->kobj);
+
+		memset(&dssdev->kobj, 0, sizeof(dssdev->kobj));
+	}
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/display.c b/drivers/video/fbdev/omap2/omapfb/dss/display.c
new file mode 100644
index 000000000000..ef5b9027985d
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/display.c
@@ -0,0 +1,338 @@
+/*
+ * linux/drivers/video/omap2/dss/display.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DISPLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/jiffies.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+#include "dss_features.h"
+
+void omapdss_default_get_resolution(struct omap_dss_device *dssdev,
+			u16 *xres, u16 *yres)
+{
+	*xres = dssdev->panel.timings.x_res;
+	*yres = dssdev->panel.timings.y_res;
+}
+EXPORT_SYMBOL(omapdss_default_get_resolution);
+
+int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
+{
+	switch (dssdev->type) {
+	case OMAP_DISPLAY_TYPE_DPI:
+		if (dssdev->phy.dpi.data_lines == 24)
+			return 24;
+		else
+			return 16;
+
+	case OMAP_DISPLAY_TYPE_DBI:
+		if (dssdev->ctrl.pixel_size == 24)
+			return 24;
+		else
+			return 16;
+	case OMAP_DISPLAY_TYPE_DSI:
+		if (dsi_get_pixel_size(dssdev->panel.dsi_pix_fmt) > 16)
+			return 24;
+		else
+			return 16;
+	case OMAP_DISPLAY_TYPE_VENC:
+	case OMAP_DISPLAY_TYPE_SDI:
+	case OMAP_DISPLAY_TYPE_HDMI:
+	case OMAP_DISPLAY_TYPE_DVI:
+		return 24;
+	default:
+		BUG();
+		return 0;
+	}
+}
+EXPORT_SYMBOL(omapdss_default_get_recommended_bpp);
+
+void omapdss_default_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	*timings = dssdev->panel.timings;
+}
+EXPORT_SYMBOL(omapdss_default_get_timings);
+
+int dss_suspend_all_devices(void)
+{
+	struct omap_dss_device *dssdev = NULL;
+
+	for_each_dss_dev(dssdev) {
+		if (!dssdev->driver)
+			continue;
+
+		if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+			dssdev->driver->disable(dssdev);
+			dssdev->activate_after_resume = true;
+		} else {
+			dssdev->activate_after_resume = false;
+		}
+	}
+
+	return 0;
+}
+
+int dss_resume_all_devices(void)
+{
+	struct omap_dss_device *dssdev = NULL;
+
+	for_each_dss_dev(dssdev) {
+		if (!dssdev->driver)
+			continue;
+
+		if (dssdev->activate_after_resume) {
+			dssdev->driver->enable(dssdev);
+			dssdev->activate_after_resume = false;
+		}
+	}
+
+	return 0;
+}
+
+void dss_disable_all_devices(void)
+{
+	struct omap_dss_device *dssdev = NULL;
+
+	for_each_dss_dev(dssdev) {
+		if (!dssdev->driver)
+			continue;
+
+		if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+			dssdev->driver->disable(dssdev);
+	}
+}
+
+static LIST_HEAD(panel_list);
+static DEFINE_MUTEX(panel_list_mutex);
+static int disp_num_counter;
+
+int omapdss_register_display(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_driver *drv = dssdev->driver;
+	int id;
+
+	/*
+	 * Note: this presumes all the displays are either using DT or non-DT,
+	 * which normally should be the case. This also presumes that all
+	 * displays either have an DT alias, or none has.
+	 */
+
+	if (dssdev->dev->of_node) {
+		id = of_alias_get_id(dssdev->dev->of_node, "display");
+
+		if (id < 0)
+			id = disp_num_counter++;
+	} else {
+		id = disp_num_counter++;
+	}
+
+	snprintf(dssdev->alias, sizeof(dssdev->alias), "display%d", id);
+
+	/* Use 'label' property for name, if it exists */
+	if (dssdev->dev->of_node)
+		of_property_read_string(dssdev->dev->of_node, "label",
+			&dssdev->name);
+
+	if (dssdev->name == NULL)
+		dssdev->name = dssdev->alias;
+
+	if (drv && drv->get_resolution == NULL)
+		drv->get_resolution = omapdss_default_get_resolution;
+	if (drv && drv->get_recommended_bpp == NULL)
+		drv->get_recommended_bpp = omapdss_default_get_recommended_bpp;
+	if (drv && drv->get_timings == NULL)
+		drv->get_timings = omapdss_default_get_timings;
+
+	mutex_lock(&panel_list_mutex);
+	list_add_tail(&dssdev->panel_list, &panel_list);
+	mutex_unlock(&panel_list_mutex);
+	return 0;
+}
+EXPORT_SYMBOL(omapdss_register_display);
+
+void omapdss_unregister_display(struct omap_dss_device *dssdev)
+{
+	mutex_lock(&panel_list_mutex);
+	list_del(&dssdev->panel_list);
+	mutex_unlock(&panel_list_mutex);
+}
+EXPORT_SYMBOL(omapdss_unregister_display);
+
+struct omap_dss_device *omap_dss_get_device(struct omap_dss_device *dssdev)
+{
+	if (!try_module_get(dssdev->owner))
+		return NULL;
+
+	if (get_device(dssdev->dev) == NULL) {
+		module_put(dssdev->owner);
+		return NULL;
+	}
+
+	return dssdev;
+}
+EXPORT_SYMBOL(omap_dss_get_device);
+
+void omap_dss_put_device(struct omap_dss_device *dssdev)
+{
+	put_device(dssdev->dev);
+	module_put(dssdev->owner);
+}
+EXPORT_SYMBOL(omap_dss_put_device);
+
+/*
+ * ref count of the found device is incremented.
+ * ref count of from-device is decremented.
+ */
+struct omap_dss_device *omap_dss_get_next_device(struct omap_dss_device *from)
+{
+	struct list_head *l;
+	struct omap_dss_device *dssdev;
+
+	mutex_lock(&panel_list_mutex);
+
+	if (list_empty(&panel_list)) {
+		dssdev = NULL;
+		goto out;
+	}
+
+	if (from == NULL) {
+		dssdev = list_first_entry(&panel_list, struct omap_dss_device,
+				panel_list);
+		omap_dss_get_device(dssdev);
+		goto out;
+	}
+
+	omap_dss_put_device(from);
+
+	list_for_each(l, &panel_list) {
+		dssdev = list_entry(l, struct omap_dss_device, panel_list);
+		if (dssdev == from) {
+			if (list_is_last(l, &panel_list)) {
+				dssdev = NULL;
+				goto out;
+			}
+
+			dssdev = list_entry(l->next, struct omap_dss_device,
+					panel_list);
+			omap_dss_get_device(dssdev);
+			goto out;
+		}
+	}
+
+	WARN(1, "'from' dssdev not found\n");
+
+	dssdev = NULL;
+out:
+	mutex_unlock(&panel_list_mutex);
+	return dssdev;
+}
+EXPORT_SYMBOL(omap_dss_get_next_device);
+
+struct omap_dss_device *omap_dss_find_device(void *data,
+		int (*match)(struct omap_dss_device *dssdev, void *data))
+{
+	struct omap_dss_device *dssdev = NULL;
+
+	while ((dssdev = omap_dss_get_next_device(dssdev)) != NULL) {
+		if (match(dssdev, data))
+			return dssdev;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_device);
+
+void videomode_to_omap_video_timings(const struct videomode *vm,
+		struct omap_video_timings *ovt)
+{
+	memset(ovt, 0, sizeof(*ovt));
+
+	ovt->pixelclock = vm->pixelclock;
+	ovt->x_res = vm->hactive;
+	ovt->hbp = vm->hback_porch;
+	ovt->hfp = vm->hfront_porch;
+	ovt->hsw = vm->hsync_len;
+	ovt->y_res = vm->vactive;
+	ovt->vbp = vm->vback_porch;
+	ovt->vfp = vm->vfront_porch;
+	ovt->vsw = vm->vsync_len;
+
+	ovt->vsync_level = vm->flags & DISPLAY_FLAGS_VSYNC_HIGH ?
+		OMAPDSS_SIG_ACTIVE_HIGH :
+		OMAPDSS_SIG_ACTIVE_LOW;
+	ovt->hsync_level = vm->flags & DISPLAY_FLAGS_HSYNC_HIGH ?
+		OMAPDSS_SIG_ACTIVE_HIGH :
+		OMAPDSS_SIG_ACTIVE_LOW;
+	ovt->de_level = vm->flags & DISPLAY_FLAGS_DE_HIGH ?
+		OMAPDSS_SIG_ACTIVE_HIGH :
+		OMAPDSS_SIG_ACTIVE_LOW;
+	ovt->data_pclk_edge = vm->flags & DISPLAY_FLAGS_PIXDATA_POSEDGE ?
+		OMAPDSS_DRIVE_SIG_RISING_EDGE :
+		OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+	ovt->sync_pclk_edge = ovt->data_pclk_edge;
+}
+EXPORT_SYMBOL(videomode_to_omap_video_timings);
+
+void omap_video_timings_to_videomode(const struct omap_video_timings *ovt,
+		struct videomode *vm)
+{
+	memset(vm, 0, sizeof(*vm));
+
+	vm->pixelclock = ovt->pixelclock;
+
+	vm->hactive = ovt->x_res;
+	vm->hback_porch = ovt->hbp;
+	vm->hfront_porch = ovt->hfp;
+	vm->hsync_len = ovt->hsw;
+	vm->vactive = ovt->y_res;
+	vm->vback_porch = ovt->vbp;
+	vm->vfront_porch = ovt->vfp;
+	vm->vsync_len = ovt->vsw;
+
+	if (ovt->hsync_level == OMAPDSS_SIG_ACTIVE_HIGH)
+		vm->flags |= DISPLAY_FLAGS_HSYNC_HIGH;
+	else
+		vm->flags |= DISPLAY_FLAGS_HSYNC_LOW;
+
+	if (ovt->vsync_level == OMAPDSS_SIG_ACTIVE_HIGH)
+		vm->flags |= DISPLAY_FLAGS_VSYNC_HIGH;
+	else
+		vm->flags |= DISPLAY_FLAGS_VSYNC_LOW;
+
+	if (ovt->de_level == OMAPDSS_SIG_ACTIVE_HIGH)
+		vm->flags |= DISPLAY_FLAGS_DE_HIGH;
+	else
+		vm->flags |= DISPLAY_FLAGS_DE_LOW;
+
+	if (ovt->data_pclk_edge == OMAPDSS_DRIVE_SIG_RISING_EDGE)
+		vm->flags |= DISPLAY_FLAGS_PIXDATA_POSEDGE;
+	else
+		vm->flags |= DISPLAY_FLAGS_PIXDATA_NEGEDGE;
+}
+EXPORT_SYMBOL(omap_video_timings_to_videomode);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dpi.c b/drivers/video/fbdev/omap2/omapfb/dss/dpi.c
new file mode 100644
index 000000000000..7953e6a52346
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dpi.c
@@ -0,0 +1,899 @@
+/*
+ * linux/drivers/video/omap2/dss/dpi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DPI"
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define HSDIV_DISPC	0
+
+struct dpi_data {
+	struct platform_device *pdev;
+
+	struct regulator *vdds_dsi_reg;
+	struct dss_pll *pll;
+
+	struct mutex lock;
+
+	struct omap_video_timings timings;
+	struct dss_lcd_mgr_config mgr_config;
+	int data_lines;
+
+	struct omap_dss_device output;
+
+	bool port_initialized;
+};
+
+static struct dpi_data *dpi_get_data_from_dssdev(struct omap_dss_device *dssdev)
+{
+	return container_of(dssdev, struct dpi_data, output);
+}
+
+/* only used in non-DT mode */
+static struct dpi_data *dpi_get_data_from_pdev(struct platform_device *pdev)
+{
+	return dev_get_drvdata(&pdev->dev);
+}
+
+static struct dss_pll *dpi_get_pll(enum omap_channel channel)
+{
+	/*
+	 * XXX we can't currently use DSI PLL for DPI with OMAP3, as the DSI PLL
+	 * would also be used for DISPC fclk. Meaning, when the DPI output is
+	 * disabled, DISPC clock will be disabled, and TV out will stop.
+	 */
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
+		return NULL;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+			return dss_pll_find("dsi0");
+		case OMAP_DSS_CHANNEL_LCD2:
+			return dss_pll_find("dsi1");
+		default:
+			return NULL;
+		}
+
+	case OMAPDSS_VER_OMAP5:
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+			return dss_pll_find("dsi0");
+		case OMAP_DSS_CHANNEL_LCD3:
+			return dss_pll_find("dsi1");
+		default:
+			return NULL;
+		}
+
+	case OMAPDSS_VER_DRA7xx:
+		switch (channel) {
+		case OMAP_DSS_CHANNEL_LCD:
+		case OMAP_DSS_CHANNEL_LCD2:
+			return dss_pll_find("video0");
+		case OMAP_DSS_CHANNEL_LCD3:
+			return dss_pll_find("video1");
+		default:
+			return NULL;
+		}
+
+	default:
+		return NULL;
+	}
+}
+
+static enum omap_dss_clk_source dpi_get_alt_clk_src(enum omap_channel channel)
+{
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		return OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC;
+	case OMAP_DSS_CHANNEL_LCD2:
+		return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+	case OMAP_DSS_CHANNEL_LCD3:
+		return OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC;
+	default:
+		/* this shouldn't happen */
+		WARN_ON(1);
+		return OMAP_DSS_CLK_SRC_FCK;
+	}
+}
+
+struct dpi_clk_calc_ctx {
+	struct dss_pll *pll;
+
+	/* inputs */
+
+	unsigned long pck_min, pck_max;
+
+	/* outputs */
+
+	struct dss_pll_clock_info dsi_cinfo;
+	unsigned long fck;
+	struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	/*
+	 * Odd dividers give us uneven duty cycle, causing problem when level
+	 * shifted. So skip all odd dividers when the pixel clock is on the
+	 * higher side.
+	 */
+	if (ctx->pck_min >= 100000000) {
+		if (lckd > 1 && lckd % 2 != 0)
+			return false;
+
+		if (pckd > 1 && pckd % 2 != 0)
+			return false;
+	}
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	return true;
+}
+
+
+static bool dpi_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+		void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	/*
+	 * Odd dividers give us uneven duty cycle, causing problem when level
+	 * shifted. So skip all odd dividers when the pixel clock is on the
+	 * higher side.
+	 */
+	if (m_dispc > 1 && m_dispc % 2 != 0 && ctx->pck_min >= 100000000)
+		return false;
+
+	ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+	ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+	return dispc_div_calc(dispc, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+
+static bool dpi_calc_pll_cb(int n, int m, unsigned long fint,
+		unsigned long clkdco,
+		void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.n = n;
+	ctx->dsi_cinfo.m = m;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkdco = clkdco;
+
+	return dss_pll_hsdiv_calc(ctx->pll, clkdco,
+		ctx->pck_min, dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+		dpi_calc_hsdiv_cb, ctx);
+}
+
+static bool dpi_calc_dss_cb(unsigned long fck, void *data)
+{
+	struct dpi_clk_calc_ctx *ctx = data;
+
+	ctx->fck = fck;
+
+	return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+static bool dpi_dsi_clk_calc(struct dpi_data *dpi, unsigned long pck,
+		struct dpi_clk_calc_ctx *ctx)
+{
+	unsigned long clkin;
+	unsigned long pll_min, pll_max;
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->pll = dpi->pll;
+	ctx->pck_min = pck - 1000;
+	ctx->pck_max = pck + 1000;
+
+	pll_min = 0;
+	pll_max = 0;
+
+	clkin = clk_get_rate(ctx->pll->clkin);
+
+	return dss_pll_calc(ctx->pll, clkin,
+			pll_min, pll_max,
+			dpi_calc_pll_cb, ctx);
+}
+
+static bool dpi_dss_clk_calc(unsigned long pck, struct dpi_clk_calc_ctx *ctx)
+{
+	int i;
+
+	/*
+	 * DSS fck gives us very few possibilities, so finding a good pixel
+	 * clock may not be possible. We try multiple times to find the clock,
+	 * each time widening the pixel clock range we look for, up to
+	 * +/- ~15MHz.
+	 */
+
+	for (i = 0; i < 25; ++i) {
+		bool ok;
+
+		memset(ctx, 0, sizeof(*ctx));
+		if (pck > 1000 * i * i * i)
+			ctx->pck_min = max(pck - 1000 * i * i * i, 0lu);
+		else
+			ctx->pck_min = 0;
+		ctx->pck_max = pck + 1000 * i * i * i;
+
+		ok = dss_div_calc(pck, ctx->pck_min, dpi_calc_dss_cb, ctx);
+		if (ok)
+			return ok;
+	}
+
+	return false;
+}
+
+
+
+static int dpi_set_dsi_clk(struct dpi_data *dpi, enum omap_channel channel,
+		unsigned long pck_req, unsigned long *fck, int *lck_div,
+		int *pck_div)
+{
+	struct dpi_clk_calc_ctx ctx;
+	int r;
+	bool ok;
+
+	ok = dpi_dsi_clk_calc(dpi, pck_req, &ctx);
+	if (!ok)
+		return -EINVAL;
+
+	r = dss_pll_set_config(dpi->pll, &ctx.dsi_cinfo);
+	if (r)
+		return r;
+
+	dss_select_lcd_clk_source(channel,
+			dpi_get_alt_clk_src(channel));
+
+	dpi->mgr_config.clock_info = ctx.dispc_cinfo;
+
+	*fck = ctx.dsi_cinfo.clkout[HSDIV_DISPC];
+	*lck_div = ctx.dispc_cinfo.lck_div;
+	*pck_div = ctx.dispc_cinfo.pck_div;
+
+	return 0;
+}
+
+static int dpi_set_dispc_clk(struct dpi_data *dpi, unsigned long pck_req,
+		unsigned long *fck, int *lck_div, int *pck_div)
+{
+	struct dpi_clk_calc_ctx ctx;
+	int r;
+	bool ok;
+
+	ok = dpi_dss_clk_calc(pck_req, &ctx);
+	if (!ok)
+		return -EINVAL;
+
+	r = dss_set_fck_rate(ctx.fck);
+	if (r)
+		return r;
+
+	dpi->mgr_config.clock_info = ctx.dispc_cinfo;
+
+	*fck = ctx.fck;
+	*lck_div = ctx.dispc_cinfo.lck_div;
+	*pck_div = ctx.dispc_cinfo.pck_div;
+
+	return 0;
+}
+
+static int dpi_set_mode(struct dpi_data *dpi)
+{
+	struct omap_dss_device *out = &dpi->output;
+	struct omap_overlay_manager *mgr = out->manager;
+	struct omap_video_timings *t = &dpi->timings;
+	int lck_div = 0, pck_div = 0;
+	unsigned long fck = 0;
+	unsigned long pck;
+	int r = 0;
+
+	if (dpi->pll)
+		r = dpi_set_dsi_clk(dpi, mgr->id, t->pixelclock, &fck,
+				&lck_div, &pck_div);
+	else
+		r = dpi_set_dispc_clk(dpi, t->pixelclock, &fck,
+				&lck_div, &pck_div);
+	if (r)
+		return r;
+
+	pck = fck / lck_div / pck_div;
+
+	if (pck != t->pixelclock) {
+		DSSWARN("Could not find exact pixel clock. Requested %d Hz, got %lu Hz\n",
+			t->pixelclock, pck);
+
+		t->pixelclock = pck;
+	}
+
+	dss_mgr_set_timings(mgr, t);
+
+	return 0;
+}
+
+static void dpi_config_lcd_manager(struct dpi_data *dpi)
+{
+	struct omap_dss_device *out = &dpi->output;
+	struct omap_overlay_manager *mgr = out->manager;
+
+	dpi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+	dpi->mgr_config.stallmode = false;
+	dpi->mgr_config.fifohandcheck = false;
+
+	dpi->mgr_config.video_port_width = dpi->data_lines;
+
+	dpi->mgr_config.lcden_sig_polarity = 0;
+
+	dss_mgr_set_lcd_config(mgr, &dpi->mgr_config);
+}
+
+static int dpi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+	struct omap_dss_device *out = &dpi->output;
+	int r;
+
+	mutex_lock(&dpi->lock);
+
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI) && !dpi->vdds_dsi_reg) {
+		DSSERR("no VDSS_DSI regulator\n");
+		r = -ENODEV;
+		goto err_no_reg;
+	}
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err_no_out_mgr;
+	}
+
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI)) {
+		r = regulator_enable(dpi->vdds_dsi_reg);
+		if (r)
+			goto err_reg_enable;
+	}
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_get_dispc;
+
+	r = dss_dpi_select_source(out->port_num, out->manager->id);
+	if (r)
+		goto err_src_sel;
+
+	if (dpi->pll) {
+		r = dss_pll_enable(dpi->pll);
+		if (r)
+			goto err_dsi_pll_init;
+	}
+
+	r = dpi_set_mode(dpi);
+	if (r)
+		goto err_set_mode;
+
+	dpi_config_lcd_manager(dpi);
+
+	mdelay(2);
+
+	r = dss_mgr_enable(out->manager);
+	if (r)
+		goto err_mgr_enable;
+
+	mutex_unlock(&dpi->lock);
+
+	return 0;
+
+err_mgr_enable:
+err_set_mode:
+	if (dpi->pll)
+		dss_pll_disable(dpi->pll);
+err_dsi_pll_init:
+err_src_sel:
+	dispc_runtime_put();
+err_get_dispc:
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+		regulator_disable(dpi->vdds_dsi_reg);
+err_reg_enable:
+err_no_out_mgr:
+err_no_reg:
+	mutex_unlock(&dpi->lock);
+	return r;
+}
+
+static void dpi_display_disable(struct omap_dss_device *dssdev)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+	struct omap_overlay_manager *mgr = dpi->output.manager;
+
+	mutex_lock(&dpi->lock);
+
+	dss_mgr_disable(mgr);
+
+	if (dpi->pll) {
+		dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+		dss_pll_disable(dpi->pll);
+	}
+
+	dispc_runtime_put();
+
+	if (dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+		regulator_disable(dpi->vdds_dsi_reg);
+
+	mutex_unlock(&dpi->lock);
+}
+
+static void dpi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+	DSSDBG("dpi_set_timings\n");
+
+	mutex_lock(&dpi->lock);
+
+	dpi->timings = *timings;
+
+	mutex_unlock(&dpi->lock);
+}
+
+static void dpi_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+	mutex_lock(&dpi->lock);
+
+	*timings = dpi->timings;
+
+	mutex_unlock(&dpi->lock);
+}
+
+static int dpi_check_timings(struct omap_dss_device *dssdev,
+			struct omap_video_timings *timings)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+	struct omap_overlay_manager *mgr = dpi->output.manager;
+	int lck_div, pck_div;
+	unsigned long fck;
+	unsigned long pck;
+	struct dpi_clk_calc_ctx ctx;
+	bool ok;
+
+	if (mgr && !dispc_mgr_timings_ok(mgr->id, timings))
+		return -EINVAL;
+
+	if (timings->pixelclock == 0)
+		return -EINVAL;
+
+	if (dpi->pll) {
+		ok = dpi_dsi_clk_calc(dpi, timings->pixelclock, &ctx);
+		if (!ok)
+			return -EINVAL;
+
+		fck = ctx.dsi_cinfo.clkout[HSDIV_DISPC];
+	} else {
+		ok = dpi_dss_clk_calc(timings->pixelclock, &ctx);
+		if (!ok)
+			return -EINVAL;
+
+		fck = ctx.fck;
+	}
+
+	lck_div = ctx.dispc_cinfo.lck_div;
+	pck_div = ctx.dispc_cinfo.pck_div;
+
+	pck = fck / lck_div / pck_div;
+
+	timings->pixelclock = pck;
+
+	return 0;
+}
+
+static void dpi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+
+	mutex_lock(&dpi->lock);
+
+	dpi->data_lines = data_lines;
+
+	mutex_unlock(&dpi->lock);
+}
+
+static int dpi_verify_dsi_pll(struct dss_pll *pll)
+{
+	int r;
+
+	/* do initial setup with the PLL to see if it is operational */
+
+	r = dss_pll_enable(pll);
+	if (r)
+		return r;
+
+	dss_pll_disable(pll);
+
+	return 0;
+}
+
+static int dpi_init_regulator(struct dpi_data *dpi)
+{
+	struct regulator *vdds_dsi;
+
+	if (!dss_has_feature(FEAT_DPI_USES_VDDS_DSI))
+		return 0;
+
+	if (dpi->vdds_dsi_reg)
+		return 0;
+
+	vdds_dsi = devm_regulator_get(&dpi->pdev->dev, "vdds_dsi");
+	if (IS_ERR(vdds_dsi)) {
+		if (PTR_ERR(vdds_dsi) != -EPROBE_DEFER)
+			DSSERR("can't get VDDS_DSI regulator\n");
+		return PTR_ERR(vdds_dsi);
+	}
+
+	dpi->vdds_dsi_reg = vdds_dsi;
+
+	return 0;
+}
+
+static void dpi_init_pll(struct dpi_data *dpi)
+{
+	struct dss_pll *pll;
+
+	if (dpi->pll)
+		return;
+
+	pll = dpi_get_pll(dpi->output.dispc_channel);
+	if (!pll)
+		return;
+
+	/* On DRA7 we need to set a mux to use the PLL */
+	if (omapdss_get_version() == OMAPDSS_VER_DRA7xx)
+		dss_ctrl_pll_set_control_mux(pll->id, dpi->output.dispc_channel);
+
+	if (dpi_verify_dsi_pll(pll)) {
+		DSSWARN("DSI PLL not operational\n");
+		return;
+	}
+
+	dpi->pll = pll;
+}
+
+/*
+ * Return a hardcoded channel for the DPI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dpi_get_channel(int port_num)
+{
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+	case OMAPDSS_VER_AM43xx:
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_DRA7xx:
+		switch (port_num) {
+		case 2:
+			return OMAP_DSS_CHANNEL_LCD3;
+		case 1:
+			return OMAP_DSS_CHANNEL_LCD2;
+		case 0:
+		default:
+			return OMAP_DSS_CHANNEL_LCD;
+		}
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		return OMAP_DSS_CHANNEL_LCD2;
+
+	case OMAPDSS_VER_OMAP5:
+		return OMAP_DSS_CHANNEL_LCD3;
+
+	default:
+		DSSWARN("unsupported DSS version\n");
+		return OMAP_DSS_CHANNEL_LCD;
+	}
+}
+
+static int dpi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct dpi_data *dpi = dpi_get_data_from_dssdev(dssdev);
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = dpi_init_regulator(dpi);
+	if (r)
+		return r;
+
+	dpi_init_pll(dpi);
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void dpi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_dpi_ops dpi_ops = {
+	.connect = dpi_connect,
+	.disconnect = dpi_disconnect,
+
+	.enable = dpi_display_enable,
+	.disable = dpi_display_disable,
+
+	.check_timings = dpi_check_timings,
+	.set_timings = dpi_set_timings,
+	.get_timings = dpi_get_timings,
+
+	.set_data_lines = dpi_set_data_lines,
+};
+
+static void dpi_init_output(struct platform_device *pdev)
+{
+	struct dpi_data *dpi = dpi_get_data_from_pdev(pdev);
+	struct omap_dss_device *out = &dpi->output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_DPI;
+	out->output_type = OMAP_DISPLAY_TYPE_DPI;
+	out->name = "dpi.0";
+	out->dispc_channel = dpi_get_channel(0);
+	out->ops.dpi = &dpi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void dpi_uninit_output(struct platform_device *pdev)
+{
+	struct dpi_data *dpi = dpi_get_data_from_pdev(pdev);
+	struct omap_dss_device *out = &dpi->output;
+
+	omapdss_unregister_output(out);
+}
+
+static void dpi_init_output_port(struct platform_device *pdev,
+	struct device_node *port)
+{
+	struct dpi_data *dpi = port->data;
+	struct omap_dss_device *out = &dpi->output;
+	int r;
+	u32 port_num;
+
+	r = of_property_read_u32(port, "reg", &port_num);
+	if (r)
+		port_num = 0;
+
+	switch (port_num) {
+	case 2:
+		out->name = "dpi.2";
+		break;
+	case 1:
+		out->name = "dpi.1";
+		break;
+	case 0:
+	default:
+		out->name = "dpi.0";
+		break;
+	}
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_DPI;
+	out->output_type = OMAP_DISPLAY_TYPE_DPI;
+	out->dispc_channel = dpi_get_channel(port_num);
+	out->port_num = port_num;
+	out->ops.dpi = &dpi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void dpi_uninit_output_port(struct device_node *port)
+{
+	struct dpi_data *dpi = port->data;
+	struct omap_dss_device *out = &dpi->output;
+
+	omapdss_unregister_output(out);
+}
+
+static int dpi_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dpi_data *dpi;
+
+	dpi = devm_kzalloc(&pdev->dev, sizeof(*dpi), GFP_KERNEL);
+	if (!dpi)
+		return -ENOMEM;
+
+	dpi->pdev = pdev;
+
+	dev_set_drvdata(&pdev->dev, dpi);
+
+	mutex_init(&dpi->lock);
+
+	dpi_init_output(pdev);
+
+	return 0;
+}
+
+static void dpi_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	dpi_uninit_output(pdev);
+}
+
+static const struct component_ops dpi_component_ops = {
+	.bind	= dpi_bind,
+	.unbind	= dpi_unbind,
+};
+
+static int dpi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dpi_component_ops);
+}
+
+static int dpi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dpi_component_ops);
+	return 0;
+}
+
+static struct platform_driver omap_dpi_driver = {
+	.probe		= dpi_probe,
+	.remove		= dpi_remove,
+	.driver         = {
+		.name   = "omapdss_dpi",
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init dpi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dpi_driver);
+}
+
+void dpi_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_dpi_driver);
+}
+
+int dpi_init_port(struct platform_device *pdev, struct device_node *port)
+{
+	struct dpi_data *dpi;
+	struct device_node *ep;
+	u32 datalines;
+	int r;
+
+	dpi = devm_kzalloc(&pdev->dev, sizeof(*dpi), GFP_KERNEL);
+	if (!dpi)
+		return -ENOMEM;
+
+	ep = omapdss_of_get_next_endpoint(port, NULL);
+	if (!ep)
+		return 0;
+
+	r = of_property_read_u32(ep, "data-lines", &datalines);
+	if (r) {
+		DSSERR("failed to parse datalines\n");
+		goto err_datalines;
+	}
+
+	dpi->data_lines = datalines;
+
+	of_node_put(ep);
+
+	dpi->pdev = pdev;
+	port->data = dpi;
+
+	mutex_init(&dpi->lock);
+
+	dpi_init_output_port(pdev, port);
+
+	dpi->port_initialized = true;
+
+	return 0;
+
+err_datalines:
+	of_node_put(ep);
+
+	return r;
+}
+
+void dpi_uninit_port(struct device_node *port)
+{
+	struct dpi_data *dpi = port->data;
+
+	if (!dpi->port_initialized)
+		return;
+
+	dpi_uninit_output_port(port);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dsi.c b/drivers/video/fbdev/omap2/omapfb/dss/dsi.c
new file mode 100644
index 000000000000..0eec073b3919
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dsi.c
@@ -0,0 +1,5607 @@
+/*
+ * linux/drivers/video/omap2/dss/dsi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DSI"
+
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/semaphore.h>
+#include <linux/seq_file.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/wait.h>
+#include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/debugfs.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include <video/mipi_display.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define DSI_CATCH_MISSING_TE
+
+struct dsi_reg { u16 module; u16 idx; };
+
+#define DSI_REG(mod, idx)		((const struct dsi_reg) { mod, idx })
+
+/* DSI Protocol Engine */
+
+#define DSI_PROTO			0
+#define DSI_PROTO_SZ			0x200
+
+#define DSI_REVISION			DSI_REG(DSI_PROTO, 0x0000)
+#define DSI_SYSCONFIG			DSI_REG(DSI_PROTO, 0x0010)
+#define DSI_SYSSTATUS			DSI_REG(DSI_PROTO, 0x0014)
+#define DSI_IRQSTATUS			DSI_REG(DSI_PROTO, 0x0018)
+#define DSI_IRQENABLE			DSI_REG(DSI_PROTO, 0x001C)
+#define DSI_CTRL			DSI_REG(DSI_PROTO, 0x0040)
+#define DSI_GNQ				DSI_REG(DSI_PROTO, 0x0044)
+#define DSI_COMPLEXIO_CFG1		DSI_REG(DSI_PROTO, 0x0048)
+#define DSI_COMPLEXIO_IRQ_STATUS	DSI_REG(DSI_PROTO, 0x004C)
+#define DSI_COMPLEXIO_IRQ_ENABLE	DSI_REG(DSI_PROTO, 0x0050)
+#define DSI_CLK_CTRL			DSI_REG(DSI_PROTO, 0x0054)
+#define DSI_TIMING1			DSI_REG(DSI_PROTO, 0x0058)
+#define DSI_TIMING2			DSI_REG(DSI_PROTO, 0x005C)
+#define DSI_VM_TIMING1			DSI_REG(DSI_PROTO, 0x0060)
+#define DSI_VM_TIMING2			DSI_REG(DSI_PROTO, 0x0064)
+#define DSI_VM_TIMING3			DSI_REG(DSI_PROTO, 0x0068)
+#define DSI_CLK_TIMING			DSI_REG(DSI_PROTO, 0x006C)
+#define DSI_TX_FIFO_VC_SIZE		DSI_REG(DSI_PROTO, 0x0070)
+#define DSI_RX_FIFO_VC_SIZE		DSI_REG(DSI_PROTO, 0x0074)
+#define DSI_COMPLEXIO_CFG2		DSI_REG(DSI_PROTO, 0x0078)
+#define DSI_RX_FIFO_VC_FULLNESS		DSI_REG(DSI_PROTO, 0x007C)
+#define DSI_VM_TIMING4			DSI_REG(DSI_PROTO, 0x0080)
+#define DSI_TX_FIFO_VC_EMPTINESS	DSI_REG(DSI_PROTO, 0x0084)
+#define DSI_VM_TIMING5			DSI_REG(DSI_PROTO, 0x0088)
+#define DSI_VM_TIMING6			DSI_REG(DSI_PROTO, 0x008C)
+#define DSI_VM_TIMING7			DSI_REG(DSI_PROTO, 0x0090)
+#define DSI_STOPCLK_TIMING		DSI_REG(DSI_PROTO, 0x0094)
+#define DSI_VC_CTRL(n)			DSI_REG(DSI_PROTO, 0x0100 + (n * 0x20))
+#define DSI_VC_TE(n)			DSI_REG(DSI_PROTO, 0x0104 + (n * 0x20))
+#define DSI_VC_LONG_PACKET_HEADER(n)	DSI_REG(DSI_PROTO, 0x0108 + (n * 0x20))
+#define DSI_VC_LONG_PACKET_PAYLOAD(n)	DSI_REG(DSI_PROTO, 0x010C + (n * 0x20))
+#define DSI_VC_SHORT_PACKET_HEADER(n)	DSI_REG(DSI_PROTO, 0x0110 + (n * 0x20))
+#define DSI_VC_IRQSTATUS(n)		DSI_REG(DSI_PROTO, 0x0118 + (n * 0x20))
+#define DSI_VC_IRQENABLE(n)		DSI_REG(DSI_PROTO, 0x011C + (n * 0x20))
+
+/* DSIPHY_SCP */
+
+#define DSI_PHY				1
+#define DSI_PHY_OFFSET			0x200
+#define DSI_PHY_SZ			0x40
+
+#define DSI_DSIPHY_CFG0			DSI_REG(DSI_PHY, 0x0000)
+#define DSI_DSIPHY_CFG1			DSI_REG(DSI_PHY, 0x0004)
+#define DSI_DSIPHY_CFG2			DSI_REG(DSI_PHY, 0x0008)
+#define DSI_DSIPHY_CFG5			DSI_REG(DSI_PHY, 0x0014)
+#define DSI_DSIPHY_CFG10		DSI_REG(DSI_PHY, 0x0028)
+
+/* DSI_PLL_CTRL_SCP */
+
+#define DSI_PLL				2
+#define DSI_PLL_OFFSET			0x300
+#define DSI_PLL_SZ			0x20
+
+#define DSI_PLL_CONTROL			DSI_REG(DSI_PLL, 0x0000)
+#define DSI_PLL_STATUS			DSI_REG(DSI_PLL, 0x0004)
+#define DSI_PLL_GO			DSI_REG(DSI_PLL, 0x0008)
+#define DSI_PLL_CONFIGURATION1		DSI_REG(DSI_PLL, 0x000C)
+#define DSI_PLL_CONFIGURATION2		DSI_REG(DSI_PLL, 0x0010)
+
+#define REG_GET(dsidev, idx, start, end) \
+	FLD_GET(dsi_read_reg(dsidev, idx), start, end)
+
+#define REG_FLD_MOD(dsidev, idx, val, start, end) \
+	dsi_write_reg(dsidev, idx, FLD_MOD(dsi_read_reg(dsidev, idx), val, start, end))
+
+/* Global interrupts */
+#define DSI_IRQ_VC0		(1 << 0)
+#define DSI_IRQ_VC1		(1 << 1)
+#define DSI_IRQ_VC2		(1 << 2)
+#define DSI_IRQ_VC3		(1 << 3)
+#define DSI_IRQ_WAKEUP		(1 << 4)
+#define DSI_IRQ_RESYNC		(1 << 5)
+#define DSI_IRQ_PLL_LOCK	(1 << 7)
+#define DSI_IRQ_PLL_UNLOCK	(1 << 8)
+#define DSI_IRQ_PLL_RECALL	(1 << 9)
+#define DSI_IRQ_COMPLEXIO_ERR	(1 << 10)
+#define DSI_IRQ_HS_TX_TIMEOUT	(1 << 14)
+#define DSI_IRQ_LP_RX_TIMEOUT	(1 << 15)
+#define DSI_IRQ_TE_TRIGGER	(1 << 16)
+#define DSI_IRQ_ACK_TRIGGER	(1 << 17)
+#define DSI_IRQ_SYNC_LOST	(1 << 18)
+#define DSI_IRQ_LDO_POWER_GOOD	(1 << 19)
+#define DSI_IRQ_TA_TIMEOUT	(1 << 20)
+#define DSI_IRQ_ERROR_MASK \
+	(DSI_IRQ_HS_TX_TIMEOUT | DSI_IRQ_LP_RX_TIMEOUT | DSI_IRQ_SYNC_LOST | \
+	DSI_IRQ_TA_TIMEOUT)
+#define DSI_IRQ_CHANNEL_MASK	0xf
+
+/* Virtual channel interrupts */
+#define DSI_VC_IRQ_CS		(1 << 0)
+#define DSI_VC_IRQ_ECC_CORR	(1 << 1)
+#define DSI_VC_IRQ_PACKET_SENT	(1 << 2)
+#define DSI_VC_IRQ_FIFO_TX_OVF	(1 << 3)
+#define DSI_VC_IRQ_FIFO_RX_OVF	(1 << 4)
+#define DSI_VC_IRQ_BTA		(1 << 5)
+#define DSI_VC_IRQ_ECC_NO_CORR	(1 << 6)
+#define DSI_VC_IRQ_FIFO_TX_UDF	(1 << 7)
+#define DSI_VC_IRQ_PP_BUSY_CHANGE (1 << 8)
+#define DSI_VC_IRQ_ERROR_MASK \
+	(DSI_VC_IRQ_CS | DSI_VC_IRQ_ECC_CORR | DSI_VC_IRQ_FIFO_TX_OVF | \
+	DSI_VC_IRQ_FIFO_RX_OVF | DSI_VC_IRQ_ECC_NO_CORR | \
+	DSI_VC_IRQ_FIFO_TX_UDF)
+
+/* ComplexIO interrupts */
+#define DSI_CIO_IRQ_ERRSYNCESC1		(1 << 0)
+#define DSI_CIO_IRQ_ERRSYNCESC2		(1 << 1)
+#define DSI_CIO_IRQ_ERRSYNCESC3		(1 << 2)
+#define DSI_CIO_IRQ_ERRSYNCESC4		(1 << 3)
+#define DSI_CIO_IRQ_ERRSYNCESC5		(1 << 4)
+#define DSI_CIO_IRQ_ERRESC1		(1 << 5)
+#define DSI_CIO_IRQ_ERRESC2		(1 << 6)
+#define DSI_CIO_IRQ_ERRESC3		(1 << 7)
+#define DSI_CIO_IRQ_ERRESC4		(1 << 8)
+#define DSI_CIO_IRQ_ERRESC5		(1 << 9)
+#define DSI_CIO_IRQ_ERRCONTROL1		(1 << 10)
+#define DSI_CIO_IRQ_ERRCONTROL2		(1 << 11)
+#define DSI_CIO_IRQ_ERRCONTROL3		(1 << 12)
+#define DSI_CIO_IRQ_ERRCONTROL4		(1 << 13)
+#define DSI_CIO_IRQ_ERRCONTROL5		(1 << 14)
+#define DSI_CIO_IRQ_STATEULPS1		(1 << 15)
+#define DSI_CIO_IRQ_STATEULPS2		(1 << 16)
+#define DSI_CIO_IRQ_STATEULPS3		(1 << 17)
+#define DSI_CIO_IRQ_STATEULPS4		(1 << 18)
+#define DSI_CIO_IRQ_STATEULPS5		(1 << 19)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_1	(1 << 20)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_1	(1 << 21)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_2	(1 << 22)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_2	(1 << 23)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_3	(1 << 24)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_3	(1 << 25)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_4	(1 << 26)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_4	(1 << 27)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP0_5	(1 << 28)
+#define DSI_CIO_IRQ_ERRCONTENTIONLP1_5	(1 << 29)
+#define DSI_CIO_IRQ_ULPSACTIVENOT_ALL0	(1 << 30)
+#define DSI_CIO_IRQ_ULPSACTIVENOT_ALL1	(1 << 31)
+#define DSI_CIO_IRQ_ERROR_MASK \
+	(DSI_CIO_IRQ_ERRSYNCESC1 | DSI_CIO_IRQ_ERRSYNCESC2 | \
+	 DSI_CIO_IRQ_ERRSYNCESC3 | DSI_CIO_IRQ_ERRSYNCESC4 | \
+	 DSI_CIO_IRQ_ERRSYNCESC5 | \
+	 DSI_CIO_IRQ_ERRESC1 | DSI_CIO_IRQ_ERRESC2 | \
+	 DSI_CIO_IRQ_ERRESC3 | DSI_CIO_IRQ_ERRESC4 | \
+	 DSI_CIO_IRQ_ERRESC5 | \
+	 DSI_CIO_IRQ_ERRCONTROL1 | DSI_CIO_IRQ_ERRCONTROL2 | \
+	 DSI_CIO_IRQ_ERRCONTROL3 | DSI_CIO_IRQ_ERRCONTROL4 | \
+	 DSI_CIO_IRQ_ERRCONTROL5 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_1 | DSI_CIO_IRQ_ERRCONTENTIONLP1_1 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_2 | DSI_CIO_IRQ_ERRCONTENTIONLP1_2 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_3 | DSI_CIO_IRQ_ERRCONTENTIONLP1_3 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_4 | DSI_CIO_IRQ_ERRCONTENTIONLP1_4 | \
+	 DSI_CIO_IRQ_ERRCONTENTIONLP0_5 | DSI_CIO_IRQ_ERRCONTENTIONLP1_5)
+
+typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
+
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+	struct omap_overlay_manager *mgr);
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+	struct omap_overlay_manager *mgr);
+
+static int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel);
+
+/* DSI PLL HSDIV indices */
+#define HSDIV_DISPC	0
+#define HSDIV_DSI	1
+
+#define DSI_MAX_NR_ISRS                2
+#define DSI_MAX_NR_LANES	5
+
+enum dsi_lane_function {
+	DSI_LANE_UNUSED	= 0,
+	DSI_LANE_CLK,
+	DSI_LANE_DATA1,
+	DSI_LANE_DATA2,
+	DSI_LANE_DATA3,
+	DSI_LANE_DATA4,
+};
+
+struct dsi_lane_config {
+	enum dsi_lane_function function;
+	u8 polarity;
+};
+
+struct dsi_isr_data {
+	omap_dsi_isr_t	isr;
+	void		*arg;
+	u32		mask;
+};
+
+enum fifo_size {
+	DSI_FIFO_SIZE_0		= 0,
+	DSI_FIFO_SIZE_32	= 1,
+	DSI_FIFO_SIZE_64	= 2,
+	DSI_FIFO_SIZE_96	= 3,
+	DSI_FIFO_SIZE_128	= 4,
+};
+
+enum dsi_vc_source {
+	DSI_VC_SOURCE_L4 = 0,
+	DSI_VC_SOURCE_VP,
+};
+
+struct dsi_irq_stats {
+	unsigned long last_reset;
+	unsigned irq_count;
+	unsigned dsi_irqs[32];
+	unsigned vc_irqs[4][32];
+	unsigned cio_irqs[32];
+};
+
+struct dsi_isr_tables {
+	struct dsi_isr_data isr_table[DSI_MAX_NR_ISRS];
+	struct dsi_isr_data isr_table_vc[4][DSI_MAX_NR_ISRS];
+	struct dsi_isr_data isr_table_cio[DSI_MAX_NR_ISRS];
+};
+
+struct dsi_clk_calc_ctx {
+	struct platform_device *dsidev;
+	struct dss_pll *pll;
+
+	/* inputs */
+
+	const struct omap_dss_dsi_config *config;
+
+	unsigned long req_pck_min, req_pck_nom, req_pck_max;
+
+	/* outputs */
+
+	struct dss_pll_clock_info dsi_cinfo;
+	struct dispc_clock_info dispc_cinfo;
+
+	struct omap_video_timings dispc_vm;
+	struct omap_dss_dsi_videomode_timings dsi_vm;
+};
+
+struct dsi_lp_clock_info {
+	unsigned long lp_clk;
+	u16 lp_clk_div;
+};
+
+struct dsi_data {
+	struct platform_device *pdev;
+	void __iomem *proto_base;
+	void __iomem *phy_base;
+	void __iomem *pll_base;
+
+	int module_id;
+
+	int irq;
+
+	bool is_enabled;
+
+	struct clk *dss_clk;
+
+	struct dispc_clock_info user_dispc_cinfo;
+	struct dss_pll_clock_info user_dsi_cinfo;
+
+	struct dsi_lp_clock_info user_lp_cinfo;
+	struct dsi_lp_clock_info current_lp_cinfo;
+
+	struct dss_pll pll;
+
+	bool vdds_dsi_enabled;
+	struct regulator *vdds_dsi_reg;
+
+	struct {
+		enum dsi_vc_source source;
+		struct omap_dss_device *dssdev;
+		enum fifo_size tx_fifo_size;
+		enum fifo_size rx_fifo_size;
+		int vc_id;
+	} vc[4];
+
+	struct mutex lock;
+	struct semaphore bus_lock;
+
+	spinlock_t irq_lock;
+	struct dsi_isr_tables isr_tables;
+	/* space for a copy used by the interrupt handler */
+	struct dsi_isr_tables isr_tables_copy;
+
+	int update_channel;
+#ifdef DSI_PERF_MEASURE
+	unsigned update_bytes;
+#endif
+
+	bool te_enabled;
+	bool ulps_enabled;
+
+	void (*framedone_callback)(int, void *);
+	void *framedone_data;
+
+	struct delayed_work framedone_timeout_work;
+
+#ifdef DSI_CATCH_MISSING_TE
+	struct timer_list te_timer;
+#endif
+
+	unsigned long cache_req_pck;
+	unsigned long cache_clk_freq;
+	struct dss_pll_clock_info cache_cinfo;
+
+	u32		errors;
+	spinlock_t	errors_lock;
+#ifdef DSI_PERF_MEASURE
+	ktime_t perf_setup_time;
+	ktime_t perf_start_time;
+#endif
+	int debug_read;
+	int debug_write;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	spinlock_t irq_stats_lock;
+	struct dsi_irq_stats irq_stats;
+#endif
+
+	unsigned num_lanes_supported;
+	unsigned line_buffer_size;
+
+	struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
+	unsigned num_lanes_used;
+
+	unsigned scp_clk_refcount;
+
+	struct dss_lcd_mgr_config mgr_config;
+	struct omap_video_timings timings;
+	enum omap_dss_dsi_pixel_format pix_fmt;
+	enum omap_dss_dsi_mode mode;
+	struct omap_dss_dsi_videomode_timings vm_timings;
+
+	struct omap_dss_device output;
+};
+
+struct dsi_packet_sent_handler_data {
+	struct platform_device *dsidev;
+	struct completion *completion;
+};
+
+struct dsi_module_id_data {
+	u32 address;
+	int id;
+};
+
+static const struct of_device_id dsi_of_match[];
+
+#ifdef DSI_PERF_MEASURE
+static bool dsi_perf;
+module_param(dsi_perf, bool, 0644);
+#endif
+
+static inline struct dsi_data *dsi_get_dsidrv_data(struct platform_device *dsidev)
+{
+	return dev_get_drvdata(&dsidev->dev);
+}
+
+static inline struct platform_device *dsi_get_dsidev_from_dssdev(struct omap_dss_device *dssdev)
+{
+	return to_platform_device(dssdev->dev);
+}
+
+static struct platform_device *dsi_get_dsidev_from_id(int module)
+{
+	struct omap_dss_device *out;
+	enum omap_dss_output_id	id;
+
+	switch (module) {
+	case 0:
+		id = OMAP_DSS_OUTPUT_DSI1;
+		break;
+	case 1:
+		id = OMAP_DSS_OUTPUT_DSI2;
+		break;
+	default:
+		return NULL;
+	}
+
+	out = omap_dss_get_output(id);
+
+	return out ? to_platform_device(out->dev) : NULL;
+}
+
+static inline void dsi_write_reg(struct platform_device *dsidev,
+		const struct dsi_reg idx, u32 val)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	void __iomem *base;
+
+	switch(idx.module) {
+		case DSI_PROTO: base = dsi->proto_base; break;
+		case DSI_PHY: base = dsi->phy_base; break;
+		case DSI_PLL: base = dsi->pll_base; break;
+		default: return;
+	}
+
+	__raw_writel(val, base + idx.idx);
+}
+
+static inline u32 dsi_read_reg(struct platform_device *dsidev,
+		const struct dsi_reg idx)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	void __iomem *base;
+
+	switch(idx.module) {
+		case DSI_PROTO: base = dsi->proto_base; break;
+		case DSI_PHY: base = dsi->phy_base; break;
+		case DSI_PLL: base = dsi->pll_base; break;
+		default: return 0;
+	}
+
+	return __raw_readl(base + idx.idx);
+}
+
+static void dsi_bus_lock(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	down(&dsi->bus_lock);
+}
+
+static void dsi_bus_unlock(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	up(&dsi->bus_lock);
+}
+
+static bool dsi_bus_is_locked(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->bus_lock.count == 0;
+}
+
+static void dsi_completion_handler(void *data, u32 mask)
+{
+	complete((struct completion *)data);
+}
+
+static inline int wait_for_bit_change(struct platform_device *dsidev,
+		const struct dsi_reg idx, int bitnum, int value)
+{
+	unsigned long timeout;
+	ktime_t wait;
+	int t;
+
+	/* first busyloop to see if the bit changes right away */
+	t = 100;
+	while (t-- > 0) {
+		if (REG_GET(dsidev, idx, bitnum, bitnum) == value)
+			return value;
+	}
+
+	/* then loop for 500ms, sleeping for 1ms in between */
+	timeout = jiffies + msecs_to_jiffies(500);
+	while (time_before(jiffies, timeout)) {
+		if (REG_GET(dsidev, idx, bitnum, bitnum) == value)
+			return value;
+
+		wait = ns_to_ktime(1000 * 1000);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+	}
+
+	return !value;
+}
+
+u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
+{
+	switch (fmt) {
+	case OMAP_DSS_DSI_FMT_RGB888:
+	case OMAP_DSS_DSI_FMT_RGB666:
+		return 24;
+	case OMAP_DSS_DSI_FMT_RGB666_PACKED:
+		return 18;
+	case OMAP_DSS_DSI_FMT_RGB565:
+		return 16;
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+#ifdef DSI_PERF_MEASURE
+static void dsi_perf_mark_setup(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	dsi->perf_setup_time = ktime_get();
+}
+
+static void dsi_perf_mark_start(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	dsi->perf_start_time = ktime_get();
+}
+
+static void dsi_perf_show(struct platform_device *dsidev, const char *name)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	ktime_t t, setup_time, trans_time;
+	u32 total_bytes;
+	u32 setup_us, trans_us, total_us;
+
+	if (!dsi_perf)
+		return;
+
+	t = ktime_get();
+
+	setup_time = ktime_sub(dsi->perf_start_time, dsi->perf_setup_time);
+	setup_us = (u32)ktime_to_us(setup_time);
+	if (setup_us == 0)
+		setup_us = 1;
+
+	trans_time = ktime_sub(t, dsi->perf_start_time);
+	trans_us = (u32)ktime_to_us(trans_time);
+	if (trans_us == 0)
+		trans_us = 1;
+
+	total_us = setup_us + trans_us;
+
+	total_bytes = dsi->update_bytes;
+
+	printk(KERN_INFO "DSI(%s): %u us + %u us = %u us (%uHz), "
+			"%u bytes, %u kbytes/sec\n",
+			name,
+			setup_us,
+			trans_us,
+			total_us,
+			1000*1000 / total_us,
+			total_bytes,
+			total_bytes * 1000 / total_us);
+}
+#else
+static inline void dsi_perf_mark_setup(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_mark_start(struct platform_device *dsidev)
+{
+}
+
+static inline void dsi_perf_show(struct platform_device *dsidev,
+		const char *name)
+{
+}
+#endif
+
+static int verbose_irq;
+
+static void print_irq_status(u32 status)
+{
+	if (status == 0)
+		return;
+
+	if (!verbose_irq && (status & ~DSI_IRQ_CHANNEL_MASK) == 0)
+		return;
+
+#define PIS(x) (status & DSI_IRQ_##x) ? (#x " ") : ""
+
+	pr_debug("DSI IRQ: 0x%x: %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+		status,
+		verbose_irq ? PIS(VC0) : "",
+		verbose_irq ? PIS(VC1) : "",
+		verbose_irq ? PIS(VC2) : "",
+		verbose_irq ? PIS(VC3) : "",
+		PIS(WAKEUP),
+		PIS(RESYNC),
+		PIS(PLL_LOCK),
+		PIS(PLL_UNLOCK),
+		PIS(PLL_RECALL),
+		PIS(COMPLEXIO_ERR),
+		PIS(HS_TX_TIMEOUT),
+		PIS(LP_RX_TIMEOUT),
+		PIS(TE_TRIGGER),
+		PIS(ACK_TRIGGER),
+		PIS(SYNC_LOST),
+		PIS(LDO_POWER_GOOD),
+		PIS(TA_TIMEOUT));
+#undef PIS
+}
+
+static void print_irq_status_vc(int channel, u32 status)
+{
+	if (status == 0)
+		return;
+
+	if (!verbose_irq && (status & ~DSI_VC_IRQ_PACKET_SENT) == 0)
+		return;
+
+#define PIS(x) (status & DSI_VC_IRQ_##x) ? (#x " ") : ""
+
+	pr_debug("DSI VC(%d) IRQ 0x%x: %s%s%s%s%s%s%s%s%s\n",
+		channel,
+		status,
+		PIS(CS),
+		PIS(ECC_CORR),
+		PIS(ECC_NO_CORR),
+		verbose_irq ? PIS(PACKET_SENT) : "",
+		PIS(BTA),
+		PIS(FIFO_TX_OVF),
+		PIS(FIFO_RX_OVF),
+		PIS(FIFO_TX_UDF),
+		PIS(PP_BUSY_CHANGE));
+#undef PIS
+}
+
+static void print_irq_status_cio(u32 status)
+{
+	if (status == 0)
+		return;
+
+#define PIS(x) (status & DSI_CIO_IRQ_##x) ? (#x " ") : ""
+
+	pr_debug("DSI CIO IRQ 0x%x: %s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+		status,
+		PIS(ERRSYNCESC1),
+		PIS(ERRSYNCESC2),
+		PIS(ERRSYNCESC3),
+		PIS(ERRESC1),
+		PIS(ERRESC2),
+		PIS(ERRESC3),
+		PIS(ERRCONTROL1),
+		PIS(ERRCONTROL2),
+		PIS(ERRCONTROL3),
+		PIS(STATEULPS1),
+		PIS(STATEULPS2),
+		PIS(STATEULPS3),
+		PIS(ERRCONTENTIONLP0_1),
+		PIS(ERRCONTENTIONLP1_1),
+		PIS(ERRCONTENTIONLP0_2),
+		PIS(ERRCONTENTIONLP1_2),
+		PIS(ERRCONTENTIONLP0_3),
+		PIS(ERRCONTENTIONLP1_3),
+		PIS(ULPSACTIVENOT_ALL0),
+		PIS(ULPSACTIVENOT_ALL1));
+#undef PIS
+}
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_collect_irq_stats(struct platform_device *dsidev, u32 irqstatus,
+		u32 *vcstatus, u32 ciostatus)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+
+	spin_lock(&dsi->irq_stats_lock);
+
+	dsi->irq_stats.irq_count++;
+	dss_collect_irq_stats(irqstatus, dsi->irq_stats.dsi_irqs);
+
+	for (i = 0; i < 4; ++i)
+		dss_collect_irq_stats(vcstatus[i], dsi->irq_stats.vc_irqs[i]);
+
+	dss_collect_irq_stats(ciostatus, dsi->irq_stats.cio_irqs);
+
+	spin_unlock(&dsi->irq_stats_lock);
+}
+#else
+#define dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus)
+#endif
+
+static int debug_irq;
+
+static void dsi_handle_irq_errors(struct platform_device *dsidev, u32 irqstatus,
+		u32 *vcstatus, u32 ciostatus)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+
+	if (irqstatus & DSI_IRQ_ERROR_MASK) {
+		DSSERR("DSI error, irqstatus %x\n", irqstatus);
+		print_irq_status(irqstatus);
+		spin_lock(&dsi->errors_lock);
+		dsi->errors |= irqstatus & DSI_IRQ_ERROR_MASK;
+		spin_unlock(&dsi->errors_lock);
+	} else if (debug_irq) {
+		print_irq_status(irqstatus);
+	}
+
+	for (i = 0; i < 4; ++i) {
+		if (vcstatus[i] & DSI_VC_IRQ_ERROR_MASK) {
+			DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
+				       i, vcstatus[i]);
+			print_irq_status_vc(i, vcstatus[i]);
+		} else if (debug_irq) {
+			print_irq_status_vc(i, vcstatus[i]);
+		}
+	}
+
+	if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
+		DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
+		print_irq_status_cio(ciostatus);
+	} else if (debug_irq) {
+		print_irq_status_cio(ciostatus);
+	}
+}
+
+static void dsi_call_isrs(struct dsi_isr_data *isr_array,
+		unsigned isr_array_size, u32 irqstatus)
+{
+	struct dsi_isr_data *isr_data;
+	int i;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+		if (isr_data->isr && isr_data->mask & irqstatus)
+			isr_data->isr(isr_data->arg, irqstatus);
+	}
+}
+
+static void dsi_handle_isrs(struct dsi_isr_tables *isr_tables,
+		u32 irqstatus, u32 *vcstatus, u32 ciostatus)
+{
+	int i;
+
+	dsi_call_isrs(isr_tables->isr_table,
+			ARRAY_SIZE(isr_tables->isr_table),
+			irqstatus);
+
+	for (i = 0; i < 4; ++i) {
+		if (vcstatus[i] == 0)
+			continue;
+		dsi_call_isrs(isr_tables->isr_table_vc[i],
+				ARRAY_SIZE(isr_tables->isr_table_vc[i]),
+				vcstatus[i]);
+	}
+
+	if (ciostatus != 0)
+		dsi_call_isrs(isr_tables->isr_table_cio,
+				ARRAY_SIZE(isr_tables->isr_table_cio),
+				ciostatus);
+}
+
+static irqreturn_t omap_dsi_irq_handler(int irq, void *arg)
+{
+	struct platform_device *dsidev;
+	struct dsi_data *dsi;
+	u32 irqstatus, vcstatus[4], ciostatus;
+	int i;
+
+	dsidev = (struct platform_device *) arg;
+	dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (!dsi->is_enabled)
+		return IRQ_NONE;
+
+	spin_lock(&dsi->irq_lock);
+
+	irqstatus = dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+	/* IRQ is not for us */
+	if (!irqstatus) {
+		spin_unlock(&dsi->irq_lock);
+		return IRQ_NONE;
+	}
+
+	dsi_write_reg(dsidev, DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
+	/* flush posted write */
+	dsi_read_reg(dsidev, DSI_IRQSTATUS);
+
+	for (i = 0; i < 4; ++i) {
+		if ((irqstatus & (1 << i)) == 0) {
+			vcstatus[i] = 0;
+			continue;
+		}
+
+		vcstatus[i] = dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
+
+		dsi_write_reg(dsidev, DSI_VC_IRQSTATUS(i), vcstatus[i]);
+		/* flush posted write */
+		dsi_read_reg(dsidev, DSI_VC_IRQSTATUS(i));
+	}
+
+	if (irqstatus & DSI_IRQ_COMPLEXIO_ERR) {
+		ciostatus = dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
+
+		dsi_write_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
+		/* flush posted write */
+		dsi_read_reg(dsidev, DSI_COMPLEXIO_IRQ_STATUS);
+	} else {
+		ciostatus = 0;
+	}
+
+#ifdef DSI_CATCH_MISSING_TE
+	if (irqstatus & DSI_IRQ_TE_TRIGGER)
+		del_timer(&dsi->te_timer);
+#endif
+
+	/* make a copy and unlock, so that isrs can unregister
+	 * themselves */
+	memcpy(&dsi->isr_tables_copy, &dsi->isr_tables,
+		sizeof(dsi->isr_tables));
+
+	spin_unlock(&dsi->irq_lock);
+
+	dsi_handle_isrs(&dsi->isr_tables_copy, irqstatus, vcstatus, ciostatus);
+
+	dsi_handle_irq_errors(dsidev, irqstatus, vcstatus, ciostatus);
+
+	dsi_collect_irq_stats(dsidev, irqstatus, vcstatus, ciostatus);
+
+	return IRQ_HANDLED;
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_configure_irqs(struct platform_device *dsidev,
+		struct dsi_isr_data *isr_array,
+		unsigned isr_array_size, u32 default_mask,
+		const struct dsi_reg enable_reg,
+		const struct dsi_reg status_reg)
+{
+	struct dsi_isr_data *isr_data;
+	u32 mask;
+	u32 old_mask;
+	int i;
+
+	mask = default_mask;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+
+		if (isr_data->isr == NULL)
+			continue;
+
+		mask |= isr_data->mask;
+	}
+
+	old_mask = dsi_read_reg(dsidev, enable_reg);
+	/* clear the irqstatus for newly enabled irqs */
+	dsi_write_reg(dsidev, status_reg, (mask ^ old_mask) & mask);
+	dsi_write_reg(dsidev, enable_reg, mask);
+
+	/* flush posted writes */
+	dsi_read_reg(dsidev, enable_reg);
+	dsi_read_reg(dsidev, status_reg);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 mask = DSI_IRQ_ERROR_MASK;
+#ifdef DSI_CATCH_MISSING_TE
+	mask |= DSI_IRQ_TE_TRIGGER;
+#endif
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table), mask,
+			DSI_IRQENABLE, DSI_IRQSTATUS);
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_vc(struct platform_device *dsidev, int vc)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_vc[vc],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[vc]),
+			DSI_VC_IRQ_ERROR_MASK,
+			DSI_VC_IRQENABLE(vc), DSI_VC_IRQSTATUS(vc));
+}
+
+/* dsi->irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_cio(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	_omap_dsi_configure_irqs(dsidev, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio),
+			DSI_CIO_IRQ_ERROR_MASK,
+			DSI_COMPLEXIO_IRQ_ENABLE, DSI_COMPLEXIO_IRQ_STATUS);
+}
+
+static void _dsi_initialize_irq(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int vc;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	memset(&dsi->isr_tables, 0, sizeof(dsi->isr_tables));
+
+	_omap_dsi_set_irqs(dsidev);
+	for (vc = 0; vc < 4; ++vc)
+		_omap_dsi_set_irqs_vc(dsidev, vc);
+	_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+}
+
+static int _dsi_register_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+		struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+	struct dsi_isr_data *isr_data;
+	int free_idx;
+	int i;
+
+	BUG_ON(isr == NULL);
+
+	/* check for duplicate entry and find a free slot */
+	free_idx = -1;
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+
+		if (isr_data->isr == isr && isr_data->arg == arg &&
+				isr_data->mask == mask) {
+			return -EINVAL;
+		}
+
+		if (isr_data->isr == NULL && free_idx == -1)
+			free_idx = i;
+	}
+
+	if (free_idx == -1)
+		return -EBUSY;
+
+	isr_data = &isr_array[free_idx];
+	isr_data->isr = isr;
+	isr_data->arg = arg;
+	isr_data->mask = mask;
+
+	return 0;
+}
+
+static int _dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+		struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+	struct dsi_isr_data *isr_data;
+	int i;
+
+	for (i = 0; i < isr_array_size; i++) {
+		isr_data = &isr_array[i];
+		if (isr_data->isr != isr || isr_data->arg != arg ||
+				isr_data->mask != mask)
+			continue;
+
+		isr_data->isr = NULL;
+		isr_data->arg = NULL;
+		isr_data->mask = 0;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int dsi_register_isr(struct platform_device *dsidev, omap_dsi_isr_t isr,
+		void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+	if (r == 0)
+		_omap_dsi_set_irqs(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_unregister_isr(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table,
+			ARRAY_SIZE(dsi->isr_tables.isr_table));
+
+	if (r == 0)
+		_omap_dsi_set_irqs(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_register_isr_vc(struct platform_device *dsidev, int channel,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask,
+			dsi->isr_tables.isr_table_vc[channel],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_vc(dsidev, channel);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_unregister_isr_vc(struct platform_device *dsidev, int channel,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask,
+			dsi->isr_tables.isr_table_vc[channel],
+			ARRAY_SIZE(dsi->isr_tables.isr_table_vc[channel]));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_vc(dsidev, channel);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_register_isr_cio(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_register_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static int dsi_unregister_isr_cio(struct platform_device *dsidev,
+		omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	int r;
+
+	spin_lock_irqsave(&dsi->irq_lock, flags);
+
+	r = _dsi_unregister_isr(isr, arg, mask, dsi->isr_tables.isr_table_cio,
+			ARRAY_SIZE(dsi->isr_tables.isr_table_cio));
+
+	if (r == 0)
+		_omap_dsi_set_irqs_cio(dsidev);
+
+	spin_unlock_irqrestore(&dsi->irq_lock, flags);
+
+	return r;
+}
+
+static u32 dsi_get_errors(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	u32 e;
+	spin_lock_irqsave(&dsi->errors_lock, flags);
+	e = dsi->errors;
+	dsi->errors = 0;
+	spin_unlock_irqrestore(&dsi->errors_lock, flags);
+	return e;
+}
+
+static int dsi_runtime_get(struct platform_device *dsidev)
+{
+	int r;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	DSSDBG("dsi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dsi->pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void dsi_runtime_put(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+
+	DSSDBG("dsi_runtime_put\n");
+
+	r = pm_runtime_put_sync(&dsi->pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static int dsi_regulator_init(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct regulator *vdds_dsi;
+	int r;
+
+	if (dsi->vdds_dsi_reg != NULL)
+		return 0;
+
+	vdds_dsi = devm_regulator_get(&dsi->pdev->dev, "vdd");
+
+	if (IS_ERR(vdds_dsi)) {
+		if (PTR_ERR(vdds_dsi) != -EPROBE_DEFER)
+			DSSERR("can't get DSI VDD regulator\n");
+		return PTR_ERR(vdds_dsi);
+	}
+
+	if (regulator_can_change_voltage(vdds_dsi)) {
+		r = regulator_set_voltage(vdds_dsi, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(vdds_dsi);
+			DSSERR("can't set the DSI regulator voltage\n");
+			return r;
+		}
+	}
+
+	dsi->vdds_dsi_reg = vdds_dsi;
+
+	return 0;
+}
+
+static void _dsi_print_reset_status(struct platform_device *dsidev)
+{
+	u32 l;
+	int b0, b1, b2;
+
+	/* A dummy read using the SCP interface to any DSIPHY register is
+	 * required after DSIPHY reset to complete the reset of the DSI complex
+	 * I/O. */
+	l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+	if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC)) {
+		b0 = 28;
+		b1 = 27;
+		b2 = 26;
+	} else {
+		b0 = 24;
+		b1 = 25;
+		b2 = 26;
+	}
+
+#define DSI_FLD_GET(fld, start, end)\
+	FLD_GET(dsi_read_reg(dsidev, DSI_##fld), start, end)
+
+	pr_debug("DSI resets: PLL (%d) CIO (%d) PHY (%x%x%x, %d, %d, %d)\n",
+		DSI_FLD_GET(PLL_STATUS, 0, 0),
+		DSI_FLD_GET(COMPLEXIO_CFG1, 29, 29),
+		DSI_FLD_GET(DSIPHY_CFG5, b0, b0),
+		DSI_FLD_GET(DSIPHY_CFG5, b1, b1),
+		DSI_FLD_GET(DSIPHY_CFG5, b2, b2),
+		DSI_FLD_GET(DSIPHY_CFG5, 29, 29),
+		DSI_FLD_GET(DSIPHY_CFG5, 30, 30),
+		DSI_FLD_GET(DSIPHY_CFG5, 31, 31));
+
+#undef DSI_FLD_GET
+}
+
+static inline int dsi_if_enable(struct platform_device *dsidev, bool enable)
+{
+	DSSDBG("dsi_if_enable(%d)\n", enable);
+
+	enable = enable ? 1 : 0;
+	REG_FLD_MOD(dsidev, DSI_CTRL, enable, 0, 0); /* IF_EN */
+
+	if (wait_for_bit_change(dsidev, DSI_CTRL, 0, enable) != enable) {
+			DSSERR("Failed to set dsi_if_enable to %d\n", enable);
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static unsigned long dsi_get_pll_hsdiv_dispc_rate(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->pll.cinfo.clkout[HSDIV_DISPC];
+}
+
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->pll.cinfo.clkout[HSDIV_DSI];
+}
+
+static unsigned long dsi_get_txbyteclkhs(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	return dsi->pll.cinfo.clkdco / 16;
+}
+
+static unsigned long dsi_fclk_rate(struct platform_device *dsidev)
+{
+	unsigned long r;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dss_get_dsi_clk_source(dsi->module_id) == OMAP_DSS_CLK_SRC_FCK) {
+		/* DSI FCLK source is DSS_CLK_FCK */
+		r = clk_get_rate(dsi->dss_clk);
+	} else {
+		/* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+		r = dsi_get_pll_hsdiv_dsi_rate(dsidev);
+	}
+
+	return r;
+}
+
+static int dsi_lp_clock_calc(unsigned long dsi_fclk,
+		unsigned long lp_clk_min, unsigned long lp_clk_max,
+		struct dsi_lp_clock_info *lp_cinfo)
+{
+	unsigned lp_clk_div;
+	unsigned long lp_clk;
+
+	lp_clk_div = DIV_ROUND_UP(dsi_fclk, lp_clk_max * 2);
+	lp_clk = dsi_fclk / 2 / lp_clk_div;
+
+	if (lp_clk < lp_clk_min || lp_clk > lp_clk_max)
+		return -EINVAL;
+
+	lp_cinfo->lp_clk_div = lp_clk_div;
+	lp_cinfo->lp_clk = lp_clk;
+
+	return 0;
+}
+
+static int dsi_set_lp_clk_divisor(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long dsi_fclk;
+	unsigned lp_clk_div;
+	unsigned long lp_clk;
+	unsigned lpdiv_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_LPDIV);
+
+
+	lp_clk_div = dsi->user_lp_cinfo.lp_clk_div;
+
+	if (lp_clk_div == 0 || lp_clk_div > lpdiv_max)
+		return -EINVAL;
+
+	dsi_fclk = dsi_fclk_rate(dsidev);
+
+	lp_clk = dsi_fclk / 2 / lp_clk_div;
+
+	DSSDBG("LP_CLK_DIV %u, LP_CLK %lu\n", lp_clk_div, lp_clk);
+	dsi->current_lp_cinfo.lp_clk = lp_clk;
+	dsi->current_lp_cinfo.lp_clk_div = lp_clk_div;
+
+	/* LP_CLK_DIVISOR */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, lp_clk_div, 12, 0);
+
+	/* LP_RX_SYNCHRO_ENABLE */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, dsi_fclk > 30000000 ? 1 : 0, 21, 21);
+
+	return 0;
+}
+
+static void dsi_enable_scp_clk(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->scp_clk_refcount++ == 0)
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dsi_disable_scp_clk(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	WARN_ON(dsi->scp_clk_refcount == 0);
+	if (--dsi->scp_clk_refcount == 0)
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 14, 14); /* CIO_CLK_ICG */
+}
+
+enum dsi_pll_power_state {
+	DSI_PLL_POWER_OFF	= 0x0,
+	DSI_PLL_POWER_ON_HSCLK	= 0x1,
+	DSI_PLL_POWER_ON_ALL	= 0x2,
+	DSI_PLL_POWER_ON_DIV	= 0x3,
+};
+
+static int dsi_pll_power(struct platform_device *dsidev,
+		enum dsi_pll_power_state state)
+{
+	int t = 0;
+
+	/* DSI-PLL power command 0x3 is not working */
+	if (dss_has_feature(FEAT_DSI_PLL_PWR_BUG) &&
+			state == DSI_PLL_POWER_ON_DIV)
+		state = DSI_PLL_POWER_ON_ALL;
+
+	/* PLL_PWR_CMD */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, state, 31, 30);
+
+	/* PLL_PWR_STATUS */
+	while (FLD_GET(dsi_read_reg(dsidev, DSI_CLK_CTRL), 29, 28) != state) {
+		if (++t > 1000) {
+			DSSERR("Failed to set DSI PLL power mode to %d\n",
+					state);
+			return -ENODEV;
+		}
+		udelay(1);
+	}
+
+	return 0;
+}
+
+
+static void dsi_pll_calc_dsi_fck(struct dss_pll_clock_info *cinfo)
+{
+	unsigned long max_dsi_fck;
+
+	max_dsi_fck = dss_feat_get_param_max(FEAT_PARAM_DSI_FCK);
+
+	cinfo->mX[HSDIV_DSI] = DIV_ROUND_UP(cinfo->clkdco, max_dsi_fck);
+	cinfo->clkout[HSDIV_DSI] = cinfo->clkdco / cinfo->mX[HSDIV_DSI];
+}
+
+static int dsi_pll_enable(struct dss_pll *pll)
+{
+	struct dsi_data *dsi = container_of(pll, struct dsi_data, pll);
+	struct platform_device *dsidev = dsi->pdev;
+	int r = 0;
+
+	DSSDBG("PLL init\n");
+
+	r = dsi_regulator_init(dsidev);
+	if (r)
+		return r;
+
+	r = dsi_runtime_get(dsidev);
+	if (r)
+		return r;
+
+	/*
+	 * Note: SCP CLK is not required on OMAP3, but it is required on OMAP4.
+	 */
+	dsi_enable_scp_clk(dsidev);
+
+	if (!dsi->vdds_dsi_enabled) {
+		r = regulator_enable(dsi->vdds_dsi_reg);
+		if (r)
+			goto err0;
+		dsi->vdds_dsi_enabled = true;
+	}
+
+	/* XXX PLL does not come out of reset without this... */
+	dispc_pck_free_enable(1);
+
+	if (wait_for_bit_change(dsidev, DSI_PLL_STATUS, 0, 1) != 1) {
+		DSSERR("PLL not coming out of reset.\n");
+		r = -ENODEV;
+		dispc_pck_free_enable(0);
+		goto err1;
+	}
+
+	/* XXX ... but if left on, we get problems when planes do not
+	 * fill the whole display. No idea about this */
+	dispc_pck_free_enable(0);
+
+	r = dsi_pll_power(dsidev, DSI_PLL_POWER_ON_ALL);
+
+	if (r)
+		goto err1;
+
+	DSSDBG("PLL init done\n");
+
+	return 0;
+err1:
+	if (dsi->vdds_dsi_enabled) {
+		regulator_disable(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_enabled = false;
+	}
+err0:
+	dsi_disable_scp_clk(dsidev);
+	dsi_runtime_put(dsidev);
+	return r;
+}
+
+static void dsi_pll_uninit(struct platform_device *dsidev, bool disconnect_lanes)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi_pll_power(dsidev, DSI_PLL_POWER_OFF);
+	if (disconnect_lanes) {
+		WARN_ON(!dsi->vdds_dsi_enabled);
+		regulator_disable(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_enabled = false;
+	}
+
+	dsi_disable_scp_clk(dsidev);
+	dsi_runtime_put(dsidev);
+
+	DSSDBG("PLL uninit done\n");
+}
+
+static void dsi_pll_disable(struct dss_pll *pll)
+{
+	struct dsi_data *dsi = container_of(pll, struct dsi_data, pll);
+	struct platform_device *dsidev = dsi->pdev;
+
+	dsi_pll_uninit(dsidev, true);
+}
+
+static void dsi_dump_dsidev_clocks(struct platform_device *dsidev,
+		struct seq_file *s)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dss_pll_clock_info *cinfo = &dsi->pll.cinfo;
+	enum omap_dss_clk_source dispc_clk_src, dsi_clk_src;
+	int dsi_module = dsi->module_id;
+	struct dss_pll *pll = &dsi->pll;
+
+	dispc_clk_src = dss_get_dispc_clk_source();
+	dsi_clk_src = dss_get_dsi_clk_source(dsi_module);
+
+	if (dsi_runtime_get(dsidev))
+		return;
+
+	seq_printf(s,	"- DSI%d PLL -\n", dsi_module + 1);
+
+	seq_printf(s,	"dsi pll clkin\t%lu\n", clk_get_rate(pll->clkin));
+
+	seq_printf(s,	"Fint\t\t%-16lun %u\n", cinfo->fint, cinfo->n);
+
+	seq_printf(s,	"CLKIN4DDR\t%-16lum %u\n",
+			cinfo->clkdco, cinfo->m);
+
+	seq_printf(s,	"DSI_PLL_HSDIV_DISPC (%s)\t%-16lum_dispc %u\t(%s)\n",
+			dss_feat_get_clk_source_name(dsi_module == 0 ?
+				OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+				OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC),
+			cinfo->clkout[HSDIV_DISPC],
+			cinfo->mX[HSDIV_DISPC],
+			dispc_clk_src == OMAP_DSS_CLK_SRC_FCK ?
+			"off" : "on");
+
+	seq_printf(s,	"DSI_PLL_HSDIV_DSI (%s)\t%-16lum_dsi %u\t(%s)\n",
+			dss_feat_get_clk_source_name(dsi_module == 0 ?
+				OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+				OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI),
+			cinfo->clkout[HSDIV_DSI],
+			cinfo->mX[HSDIV_DSI],
+			dsi_clk_src == OMAP_DSS_CLK_SRC_FCK ?
+			"off" : "on");
+
+	seq_printf(s,	"- DSI%d -\n", dsi_module + 1);
+
+	seq_printf(s,	"dsi fclk source = %s (%s)\n",
+			dss_get_generic_clk_source_name(dsi_clk_src),
+			dss_feat_get_clk_source_name(dsi_clk_src));
+
+	seq_printf(s,	"DSI_FCLK\t%lu\n", dsi_fclk_rate(dsidev));
+
+	seq_printf(s,	"DDR_CLK\t\t%lu\n",
+			cinfo->clkdco / 4);
+
+	seq_printf(s,	"TxByteClkHS\t%lu\n", dsi_get_txbyteclkhs(dsidev));
+
+	seq_printf(s,	"LP_CLK\t\t%lu\n", dsi->current_lp_cinfo.lp_clk);
+
+	dsi_runtime_put(dsidev);
+}
+
+void dsi_dump_clocks(struct seq_file *s)
+{
+	struct platform_device *dsidev;
+	int i;
+
+	for  (i = 0; i < MAX_NUM_DSI; i++) {
+		dsidev = dsi_get_dsidev_from_id(i);
+		if (dsidev)
+			dsi_dump_dsidev_clocks(dsidev, s);
+	}
+}
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_dump_dsidev_irqs(struct platform_device *dsidev,
+		struct seq_file *s)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned long flags;
+	struct dsi_irq_stats stats;
+
+	spin_lock_irqsave(&dsi->irq_stats_lock, flags);
+
+	stats = dsi->irq_stats;
+	memset(&dsi->irq_stats, 0, sizeof(dsi->irq_stats));
+	dsi->irq_stats.last_reset = jiffies;
+
+	spin_unlock_irqrestore(&dsi->irq_stats_lock, flags);
+
+	seq_printf(s, "period %u ms\n",
+			jiffies_to_msecs(jiffies - stats.last_reset));
+
+	seq_printf(s, "irqs %d\n", stats.irq_count);
+#define PIS(x) \
+	seq_printf(s, "%-20s %10d\n", #x, stats.dsi_irqs[ffs(DSI_IRQ_##x)-1]);
+
+	seq_printf(s, "-- DSI%d interrupts --\n", dsi->module_id + 1);
+	PIS(VC0);
+	PIS(VC1);
+	PIS(VC2);
+	PIS(VC3);
+	PIS(WAKEUP);
+	PIS(RESYNC);
+	PIS(PLL_LOCK);
+	PIS(PLL_UNLOCK);
+	PIS(PLL_RECALL);
+	PIS(COMPLEXIO_ERR);
+	PIS(HS_TX_TIMEOUT);
+	PIS(LP_RX_TIMEOUT);
+	PIS(TE_TRIGGER);
+	PIS(ACK_TRIGGER);
+	PIS(SYNC_LOST);
+	PIS(LDO_POWER_GOOD);
+	PIS(TA_TIMEOUT);
+#undef PIS
+
+#define PIS(x) \
+	seq_printf(s, "%-20s %10d %10d %10d %10d\n", #x, \
+			stats.vc_irqs[0][ffs(DSI_VC_IRQ_##x)-1], \
+			stats.vc_irqs[1][ffs(DSI_VC_IRQ_##x)-1], \
+			stats.vc_irqs[2][ffs(DSI_VC_IRQ_##x)-1], \
+			stats.vc_irqs[3][ffs(DSI_VC_IRQ_##x)-1]);
+
+	seq_printf(s, "-- VC interrupts --\n");
+	PIS(CS);
+	PIS(ECC_CORR);
+	PIS(PACKET_SENT);
+	PIS(FIFO_TX_OVF);
+	PIS(FIFO_RX_OVF);
+	PIS(BTA);
+	PIS(ECC_NO_CORR);
+	PIS(FIFO_TX_UDF);
+	PIS(PP_BUSY_CHANGE);
+#undef PIS
+
+#define PIS(x) \
+	seq_printf(s, "%-20s %10d\n", #x, \
+			stats.cio_irqs[ffs(DSI_CIO_IRQ_##x)-1]);
+
+	seq_printf(s, "-- CIO interrupts --\n");
+	PIS(ERRSYNCESC1);
+	PIS(ERRSYNCESC2);
+	PIS(ERRSYNCESC3);
+	PIS(ERRESC1);
+	PIS(ERRESC2);
+	PIS(ERRESC3);
+	PIS(ERRCONTROL1);
+	PIS(ERRCONTROL2);
+	PIS(ERRCONTROL3);
+	PIS(STATEULPS1);
+	PIS(STATEULPS2);
+	PIS(STATEULPS3);
+	PIS(ERRCONTENTIONLP0_1);
+	PIS(ERRCONTENTIONLP1_1);
+	PIS(ERRCONTENTIONLP0_2);
+	PIS(ERRCONTENTIONLP1_2);
+	PIS(ERRCONTENTIONLP0_3);
+	PIS(ERRCONTENTIONLP1_3);
+	PIS(ULPSACTIVENOT_ALL0);
+	PIS(ULPSACTIVENOT_ALL1);
+#undef PIS
+}
+
+static void dsi1_dump_irqs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+	dsi_dump_dsidev_irqs(dsidev, s);
+}
+
+static void dsi2_dump_irqs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+	dsi_dump_dsidev_irqs(dsidev, s);
+}
+#endif
+
+static void dsi_dump_dsidev_regs(struct platform_device *dsidev,
+		struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(dsidev, r))
+
+	if (dsi_runtime_get(dsidev))
+		return;
+	dsi_enable_scp_clk(dsidev);
+
+	DUMPREG(DSI_REVISION);
+	DUMPREG(DSI_SYSCONFIG);
+	DUMPREG(DSI_SYSSTATUS);
+	DUMPREG(DSI_IRQSTATUS);
+	DUMPREG(DSI_IRQENABLE);
+	DUMPREG(DSI_CTRL);
+	DUMPREG(DSI_COMPLEXIO_CFG1);
+	DUMPREG(DSI_COMPLEXIO_IRQ_STATUS);
+	DUMPREG(DSI_COMPLEXIO_IRQ_ENABLE);
+	DUMPREG(DSI_CLK_CTRL);
+	DUMPREG(DSI_TIMING1);
+	DUMPREG(DSI_TIMING2);
+	DUMPREG(DSI_VM_TIMING1);
+	DUMPREG(DSI_VM_TIMING2);
+	DUMPREG(DSI_VM_TIMING3);
+	DUMPREG(DSI_CLK_TIMING);
+	DUMPREG(DSI_TX_FIFO_VC_SIZE);
+	DUMPREG(DSI_RX_FIFO_VC_SIZE);
+	DUMPREG(DSI_COMPLEXIO_CFG2);
+	DUMPREG(DSI_RX_FIFO_VC_FULLNESS);
+	DUMPREG(DSI_VM_TIMING4);
+	DUMPREG(DSI_TX_FIFO_VC_EMPTINESS);
+	DUMPREG(DSI_VM_TIMING5);
+	DUMPREG(DSI_VM_TIMING6);
+	DUMPREG(DSI_VM_TIMING7);
+	DUMPREG(DSI_STOPCLK_TIMING);
+
+	DUMPREG(DSI_VC_CTRL(0));
+	DUMPREG(DSI_VC_TE(0));
+	DUMPREG(DSI_VC_LONG_PACKET_HEADER(0));
+	DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(0));
+	DUMPREG(DSI_VC_SHORT_PACKET_HEADER(0));
+	DUMPREG(DSI_VC_IRQSTATUS(0));
+	DUMPREG(DSI_VC_IRQENABLE(0));
+
+	DUMPREG(DSI_VC_CTRL(1));
+	DUMPREG(DSI_VC_TE(1));
+	DUMPREG(DSI_VC_LONG_PACKET_HEADER(1));
+	DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(1));
+	DUMPREG(DSI_VC_SHORT_PACKET_HEADER(1));
+	DUMPREG(DSI_VC_IRQSTATUS(1));
+	DUMPREG(DSI_VC_IRQENABLE(1));
+
+	DUMPREG(DSI_VC_CTRL(2));
+	DUMPREG(DSI_VC_TE(2));
+	DUMPREG(DSI_VC_LONG_PACKET_HEADER(2));
+	DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(2));
+	DUMPREG(DSI_VC_SHORT_PACKET_HEADER(2));
+	DUMPREG(DSI_VC_IRQSTATUS(2));
+	DUMPREG(DSI_VC_IRQENABLE(2));
+
+	DUMPREG(DSI_VC_CTRL(3));
+	DUMPREG(DSI_VC_TE(3));
+	DUMPREG(DSI_VC_LONG_PACKET_HEADER(3));
+	DUMPREG(DSI_VC_LONG_PACKET_PAYLOAD(3));
+	DUMPREG(DSI_VC_SHORT_PACKET_HEADER(3));
+	DUMPREG(DSI_VC_IRQSTATUS(3));
+	DUMPREG(DSI_VC_IRQENABLE(3));
+
+	DUMPREG(DSI_DSIPHY_CFG0);
+	DUMPREG(DSI_DSIPHY_CFG1);
+	DUMPREG(DSI_DSIPHY_CFG2);
+	DUMPREG(DSI_DSIPHY_CFG5);
+
+	DUMPREG(DSI_PLL_CONTROL);
+	DUMPREG(DSI_PLL_STATUS);
+	DUMPREG(DSI_PLL_GO);
+	DUMPREG(DSI_PLL_CONFIGURATION1);
+	DUMPREG(DSI_PLL_CONFIGURATION2);
+
+	dsi_disable_scp_clk(dsidev);
+	dsi_runtime_put(dsidev);
+#undef DUMPREG
+}
+
+static void dsi1_dump_regs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(0);
+
+	dsi_dump_dsidev_regs(dsidev, s);
+}
+
+static void dsi2_dump_regs(struct seq_file *s)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_id(1);
+
+	dsi_dump_dsidev_regs(dsidev, s);
+}
+
+enum dsi_cio_power_state {
+	DSI_COMPLEXIO_POWER_OFF		= 0x0,
+	DSI_COMPLEXIO_POWER_ON		= 0x1,
+	DSI_COMPLEXIO_POWER_ULPS	= 0x2,
+};
+
+static int dsi_cio_power(struct platform_device *dsidev,
+		enum dsi_cio_power_state state)
+{
+	int t = 0;
+
+	/* PWR_CMD */
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG1, state, 28, 27);
+
+	/* PWR_STATUS */
+	while (FLD_GET(dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1),
+			26, 25) != state) {
+		if (++t > 1000) {
+			DSSERR("failed to set complexio power state to "
+					"%d\n", state);
+			return -ENODEV;
+		}
+		udelay(1);
+	}
+
+	return 0;
+}
+
+static unsigned dsi_get_line_buf_size(struct platform_device *dsidev)
+{
+	int val;
+
+	/* line buffer on OMAP3 is 1024 x 24bits */
+	/* XXX: for some reason using full buffer size causes
+	 * considerable TX slowdown with update sizes that fill the
+	 * whole buffer */
+	if (!dss_has_feature(FEAT_DSI_GNQ))
+		return 1023 * 3;
+
+	val = REG_GET(dsidev, DSI_GNQ, 14, 12); /* VP1_LINE_BUFFER_SIZE */
+
+	switch (val) {
+	case 1:
+		return 512 * 3;		/* 512x24 bits */
+	case 2:
+		return 682 * 3;		/* 682x24 bits */
+	case 3:
+		return 853 * 3;		/* 853x24 bits */
+	case 4:
+		return 1024 * 3;	/* 1024x24 bits */
+	case 5:
+		return 1194 * 3;	/* 1194x24 bits */
+	case 6:
+		return 1365 * 3;	/* 1365x24 bits */
+	case 7:
+		return 1920 * 3;	/* 1920x24 bits */
+	default:
+		BUG();
+		return 0;
+	}
+}
+
+static int dsi_set_lane_config(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	static const u8 offsets[] = { 0, 4, 8, 12, 16 };
+	static const enum dsi_lane_function functions[] = {
+		DSI_LANE_CLK,
+		DSI_LANE_DATA1,
+		DSI_LANE_DATA2,
+		DSI_LANE_DATA3,
+		DSI_LANE_DATA4,
+	};
+	u32 r;
+	int i;
+
+	r = dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG1);
+
+	for (i = 0; i < dsi->num_lanes_used; ++i) {
+		unsigned offset = offsets[i];
+		unsigned polarity, lane_number;
+		unsigned t;
+
+		for (t = 0; t < dsi->num_lanes_supported; ++t)
+			if (dsi->lanes[t].function == functions[i])
+				break;
+
+		if (t == dsi->num_lanes_supported)
+			return -EINVAL;
+
+		lane_number = t;
+		polarity = dsi->lanes[t].polarity;
+
+		r = FLD_MOD(r, lane_number + 1, offset + 2, offset);
+		r = FLD_MOD(r, polarity, offset + 3, offset + 3);
+	}
+
+	/* clear the unused lanes */
+	for (; i < dsi->num_lanes_supported; ++i) {
+		unsigned offset = offsets[i];
+
+		r = FLD_MOD(r, 0, offset + 2, offset);
+		r = FLD_MOD(r, 0, offset + 3, offset + 3);
+	}
+
+	dsi_write_reg(dsidev, DSI_COMPLEXIO_CFG1, r);
+
+	return 0;
+}
+
+static inline unsigned ns2ddr(struct platform_device *dsidev, unsigned ns)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	/* convert time in ns to ddr ticks, rounding up */
+	unsigned long ddr_clk = dsi->pll.cinfo.clkdco / 4;
+	return (ns * (ddr_clk / 1000 / 1000) + 999) / 1000;
+}
+
+static inline unsigned ddr2ns(struct platform_device *dsidev, unsigned ddr)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	unsigned long ddr_clk = dsi->pll.cinfo.clkdco / 4;
+	return ddr * 1000 * 1000 / (ddr_clk / 1000);
+}
+
+static void dsi_cio_timings(struct platform_device *dsidev)
+{
+	u32 r;
+	u32 ths_prepare, ths_prepare_ths_zero, ths_trail, ths_exit;
+	u32 tlpx_half, tclk_trail, tclk_zero;
+	u32 tclk_prepare;
+
+	/* calculate timings */
+
+	/* 1 * DDR_CLK = 2 * UI */
+
+	/* min 40ns + 4*UI	max 85ns + 6*UI */
+	ths_prepare = ns2ddr(dsidev, 70) + 2;
+
+	/* min 145ns + 10*UI */
+	ths_prepare_ths_zero = ns2ddr(dsidev, 175) + 2;
+
+	/* min max(8*UI, 60ns+4*UI) */
+	ths_trail = ns2ddr(dsidev, 60) + 5;
+
+	/* min 100ns */
+	ths_exit = ns2ddr(dsidev, 145);
+
+	/* tlpx min 50n */
+	tlpx_half = ns2ddr(dsidev, 25);
+
+	/* min 60ns */
+	tclk_trail = ns2ddr(dsidev, 60) + 2;
+
+	/* min 38ns, max 95ns */
+	tclk_prepare = ns2ddr(dsidev, 65);
+
+	/* min tclk-prepare + tclk-zero = 300ns */
+	tclk_zero = ns2ddr(dsidev, 260);
+
+	DSSDBG("ths_prepare %u (%uns), ths_prepare_ths_zero %u (%uns)\n",
+		ths_prepare, ddr2ns(dsidev, ths_prepare),
+		ths_prepare_ths_zero, ddr2ns(dsidev, ths_prepare_ths_zero));
+	DSSDBG("ths_trail %u (%uns), ths_exit %u (%uns)\n",
+			ths_trail, ddr2ns(dsidev, ths_trail),
+			ths_exit, ddr2ns(dsidev, ths_exit));
+
+	DSSDBG("tlpx_half %u (%uns), tclk_trail %u (%uns), "
+			"tclk_zero %u (%uns)\n",
+			tlpx_half, ddr2ns(dsidev, tlpx_half),
+			tclk_trail, ddr2ns(dsidev, tclk_trail),
+			tclk_zero, ddr2ns(dsidev, tclk_zero));
+	DSSDBG("tclk_prepare %u (%uns)\n",
+			tclk_prepare, ddr2ns(dsidev, tclk_prepare));
+
+	/* program timings */
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
+	r = FLD_MOD(r, ths_prepare, 31, 24);
+	r = FLD_MOD(r, ths_prepare_ths_zero, 23, 16);
+	r = FLD_MOD(r, ths_trail, 15, 8);
+	r = FLD_MOD(r, ths_exit, 7, 0);
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG0, r);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
+	r = FLD_MOD(r, tlpx_half, 20, 16);
+	r = FLD_MOD(r, tclk_trail, 15, 8);
+	r = FLD_MOD(r, tclk_zero, 7, 0);
+
+	if (dss_has_feature(FEAT_DSI_PHY_DCC)) {
+		r = FLD_MOD(r, 0, 21, 21);	/* DCCEN = disable */
+		r = FLD_MOD(r, 1, 22, 22);	/* CLKINP_DIVBY2EN = enable */
+		r = FLD_MOD(r, 1, 23, 23);	/* CLKINP_SEL = enable */
+	}
+
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG1, r);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
+	r = FLD_MOD(r, tclk_prepare, 7, 0);
+	dsi_write_reg(dsidev, DSI_DSIPHY_CFG2, r);
+}
+
+/* lane masks have lane 0 at lsb. mask_p for positive lines, n for negative */
+static void dsi_cio_enable_lane_override(struct platform_device *dsidev,
+		unsigned mask_p, unsigned mask_n)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+	u32 l;
+	u8 lptxscp_start = dsi->num_lanes_supported == 3 ? 22 : 26;
+
+	l = 0;
+
+	for (i = 0; i < dsi->num_lanes_supported; ++i) {
+		unsigned p = dsi->lanes[i].polarity;
+
+		if (mask_p & (1 << i))
+			l |= 1 << (i * 2 + (p ? 0 : 1));
+
+		if (mask_n & (1 << i))
+			l |= 1 << (i * 2 + (p ? 1 : 0));
+	}
+
+	/*
+	 * Bits in REGLPTXSCPDAT4TO0DXDY:
+	 * 17: DY0 18: DX0
+	 * 19: DY1 20: DX1
+	 * 21: DY2 22: DX2
+	 * 23: DY3 24: DX3
+	 * 25: DY4 26: DX4
+	 */
+
+	/* Set the lane override configuration */
+
+	/* REGLPTXSCPDAT4TO0DXDY */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, l, lptxscp_start, 17);
+
+	/* Enable lane override */
+
+	/* ENLPTXSCPDAT */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 1, 27, 27);
+}
+
+static void dsi_cio_disable_lane_override(struct platform_device *dsidev)
+{
+	/* Disable lane override */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 27, 27); /* ENLPTXSCPDAT */
+	/* Reset the lane override configuration */
+	/* REGLPTXSCPDAT4TO0DXDY */
+	REG_FLD_MOD(dsidev, DSI_DSIPHY_CFG10, 0, 22, 17);
+}
+
+static int dsi_cio_wait_tx_clk_esc_reset(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int t, i;
+	bool in_use[DSI_MAX_NR_LANES];
+	static const u8 offsets_old[] = { 28, 27, 26 };
+	static const u8 offsets_new[] = { 24, 25, 26, 27, 28 };
+	const u8 *offsets;
+
+	if (dss_has_feature(FEAT_DSI_REVERSE_TXCLKESC))
+		offsets = offsets_old;
+	else
+		offsets = offsets_new;
+
+	for (i = 0; i < dsi->num_lanes_supported; ++i)
+		in_use[i] = dsi->lanes[i].function != DSI_LANE_UNUSED;
+
+	t = 100000;
+	while (true) {
+		u32 l;
+		int ok;
+
+		l = dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+		ok = 0;
+		for (i = 0; i < dsi->num_lanes_supported; ++i) {
+			if (!in_use[i] || (l & (1 << offsets[i])))
+				ok++;
+		}
+
+		if (ok == dsi->num_lanes_supported)
+			break;
+
+		if (--t == 0) {
+			for (i = 0; i < dsi->num_lanes_supported; ++i) {
+				if (!in_use[i] || (l & (1 << offsets[i])))
+					continue;
+
+				DSSERR("CIO TXCLKESC%d domain not coming " \
+						"out of reset\n", i);
+			}
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+/* return bitmask of enabled lanes, lane0 being the lsb */
+static unsigned dsi_get_lane_mask(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned mask = 0;
+	int i;
+
+	for (i = 0; i < dsi->num_lanes_supported; ++i) {
+		if (dsi->lanes[i].function != DSI_LANE_UNUSED)
+			mask |= 1 << i;
+	}
+
+	return mask;
+}
+
+static int dsi_cio_init(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+	u32 l;
+
+	DSSDBG("DSI CIO init starts");
+
+	r = dss_dsi_enable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+	if (r)
+		return r;
+
+	dsi_enable_scp_clk(dsidev);
+
+	/* A dummy read using the SCP interface to any DSIPHY register is
+	 * required after DSIPHY reset to complete the reset of the DSI complex
+	 * I/O. */
+	dsi_read_reg(dsidev, DSI_DSIPHY_CFG5);
+
+	if (wait_for_bit_change(dsidev, DSI_DSIPHY_CFG5, 30, 1) != 1) {
+		DSSERR("CIO SCP Clock domain not coming out of reset.\n");
+		r = -EIO;
+		goto err_scp_clk_dom;
+	}
+
+	r = dsi_set_lane_config(dsidev);
+	if (r)
+		goto err_scp_clk_dom;
+
+	/* set TX STOP MODE timer to maximum for this operation */
+	l = dsi_read_reg(dsidev, DSI_TIMING1);
+	l = FLD_MOD(l, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
+	l = FLD_MOD(l, 1, 14, 14);	/* STOP_STATE_X16_IO */
+	l = FLD_MOD(l, 1, 13, 13);	/* STOP_STATE_X4_IO */
+	l = FLD_MOD(l, 0x1fff, 12, 0);	/* STOP_STATE_COUNTER_IO */
+	dsi_write_reg(dsidev, DSI_TIMING1, l);
+
+	if (dsi->ulps_enabled) {
+		unsigned mask_p;
+		int i;
+
+		DSSDBG("manual ulps exit\n");
+
+		/* ULPS is exited by Mark-1 state for 1ms, followed by
+		 * stop state. DSS HW cannot do this via the normal
+		 * ULPS exit sequence, as after reset the DSS HW thinks
+		 * that we are not in ULPS mode, and refuses to send the
+		 * sequence. So we need to send the ULPS exit sequence
+		 * manually by setting positive lines high and negative lines
+		 * low for 1ms.
+		 */
+
+		mask_p = 0;
+
+		for (i = 0; i < dsi->num_lanes_supported; ++i) {
+			if (dsi->lanes[i].function == DSI_LANE_UNUSED)
+				continue;
+			mask_p |= 1 << i;
+		}
+
+		dsi_cio_enable_lane_override(dsidev, mask_p, 0);
+	}
+
+	r = dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ON);
+	if (r)
+		goto err_cio_pwr;
+
+	if (wait_for_bit_change(dsidev, DSI_COMPLEXIO_CFG1, 29, 1) != 1) {
+		DSSERR("CIO PWR clock domain not coming out of reset.\n");
+		r = -ENODEV;
+		goto err_cio_pwr_dom;
+	}
+
+	dsi_if_enable(dsidev, true);
+	dsi_if_enable(dsidev, false);
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 1, 20, 20); /* LP_CLK_ENABLE */
+
+	r = dsi_cio_wait_tx_clk_esc_reset(dsidev);
+	if (r)
+		goto err_tx_clk_esc_rst;
+
+	if (dsi->ulps_enabled) {
+		/* Keep Mark-1 state for 1ms (as per DSI spec) */
+		ktime_t wait = ns_to_ktime(1000 * 1000);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+
+		/* Disable the override. The lanes should be set to Mark-11
+		 * state by the HW */
+		dsi_cio_disable_lane_override(dsidev);
+	}
+
+	/* FORCE_TX_STOP_MODE_IO */
+	REG_FLD_MOD(dsidev, DSI_TIMING1, 0, 15, 15);
+
+	dsi_cio_timings(dsidev);
+
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		/* DDR_CLK_ALWAYS_ON */
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL,
+			dsi->vm_timings.ddr_clk_always_on, 13, 13);
+	}
+
+	dsi->ulps_enabled = false;
+
+	DSSDBG("CIO init done\n");
+
+	return 0;
+
+err_tx_clk_esc_rst:
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 20, 20); /* LP_CLK_ENABLE */
+err_cio_pwr_dom:
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+err_cio_pwr:
+	if (dsi->ulps_enabled)
+		dsi_cio_disable_lane_override(dsidev);
+err_scp_clk_dom:
+	dsi_disable_scp_clk(dsidev);
+	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+	return r;
+}
+
+static void dsi_cio_uninit(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	/* DDR_CLK_ALWAYS_ON */
+	REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 13, 13);
+
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_OFF);
+	dsi_disable_scp_clk(dsidev);
+	dss_dsi_disable_pads(dsi->module_id, dsi_get_lane_mask(dsidev));
+}
+
+static void dsi_config_tx_fifo(struct platform_device *dsidev,
+		enum fifo_size size1, enum fifo_size size2,
+		enum fifo_size size3, enum fifo_size size4)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 r = 0;
+	int add = 0;
+	int i;
+
+	dsi->vc[0].tx_fifo_size = size1;
+	dsi->vc[1].tx_fifo_size = size2;
+	dsi->vc[2].tx_fifo_size = size3;
+	dsi->vc[3].tx_fifo_size = size4;
+
+	for (i = 0; i < 4; i++) {
+		u8 v;
+		int size = dsi->vc[i].tx_fifo_size;
+
+		if (add + size > 4) {
+			DSSERR("Illegal FIFO configuration\n");
+			BUG();
+			return;
+		}
+
+		v = FLD_VAL(add, 2, 0) | FLD_VAL(size, 7, 4);
+		r |= v << (8 * i);
+		/*DSSDBG("TX FIFO vc %d: size %d, add %d\n", i, size, add); */
+		add += size;
+	}
+
+	dsi_write_reg(dsidev, DSI_TX_FIFO_VC_SIZE, r);
+}
+
+static void dsi_config_rx_fifo(struct platform_device *dsidev,
+		enum fifo_size size1, enum fifo_size size2,
+		enum fifo_size size3, enum fifo_size size4)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 r = 0;
+	int add = 0;
+	int i;
+
+	dsi->vc[0].rx_fifo_size = size1;
+	dsi->vc[1].rx_fifo_size = size2;
+	dsi->vc[2].rx_fifo_size = size3;
+	dsi->vc[3].rx_fifo_size = size4;
+
+	for (i = 0; i < 4; i++) {
+		u8 v;
+		int size = dsi->vc[i].rx_fifo_size;
+
+		if (add + size > 4) {
+			DSSERR("Illegal FIFO configuration\n");
+			BUG();
+			return;
+		}
+
+		v = FLD_VAL(add, 2, 0) | FLD_VAL(size, 7, 4);
+		r |= v << (8 * i);
+		/*DSSDBG("RX FIFO vc %d: size %d, add %d\n", i, size, add); */
+		add += size;
+	}
+
+	dsi_write_reg(dsidev, DSI_RX_FIFO_VC_SIZE, r);
+}
+
+static int dsi_force_tx_stop_mode_io(struct platform_device *dsidev)
+{
+	u32 r;
+
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
+	r = FLD_MOD(r, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
+
+	if (wait_for_bit_change(dsidev, DSI_TIMING1, 15, 0) != 0) {
+		DSSERR("TX_STOP bit not going down\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static bool dsi_vc_is_enabled(struct platform_device *dsidev, int channel)
+{
+	return REG_GET(dsidev, DSI_VC_CTRL(channel), 0, 0);
+}
+
+static void dsi_packet_sent_handler_vp(void *data, u32 mask)
+{
+	struct dsi_packet_sent_handler_data *vp_data =
+		(struct dsi_packet_sent_handler_data *) data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(vp_data->dsidev);
+	const int channel = dsi->update_channel;
+	u8 bit = dsi->te_enabled ? 30 : 31;
+
+	if (REG_GET(vp_data->dsidev, DSI_VC_TE(channel), bit, bit) == 0)
+		complete(vp_data->completion);
+}
+
+static int dsi_sync_vc_vp(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	DECLARE_COMPLETION_ONSTACK(completion);
+	struct dsi_packet_sent_handler_data vp_data = {
+		.dsidev = dsidev,
+		.completion = &completion
+	};
+	int r = 0;
+	u8 bit;
+
+	bit = dsi->te_enabled ? 30 : 31;
+
+	r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+	if (r)
+		goto err0;
+
+	/* Wait for completion only if TE_EN/TE_START is still set */
+	if (REG_GET(dsidev, DSI_VC_TE(channel), bit, bit)) {
+		if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(10)) == 0) {
+			DSSERR("Failed to complete previous frame transfer\n");
+			r = -EIO;
+			goto err1;
+		}
+	}
+
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+
+	return 0;
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_vp,
+		&vp_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+	return r;
+}
+
+static void dsi_packet_sent_handler_l4(void *data, u32 mask)
+{
+	struct dsi_packet_sent_handler_data *l4_data =
+		(struct dsi_packet_sent_handler_data *) data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(l4_data->dsidev);
+	const int channel = dsi->update_channel;
+
+	if (REG_GET(l4_data->dsidev, DSI_VC_CTRL(channel), 5, 5) == 0)
+		complete(l4_data->completion);
+}
+
+static int dsi_sync_vc_l4(struct platform_device *dsidev, int channel)
+{
+	DECLARE_COMPLETION_ONSTACK(completion);
+	struct dsi_packet_sent_handler_data l4_data = {
+		.dsidev = dsidev,
+		.completion = &completion
+	};
+	int r = 0;
+
+	r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+	if (r)
+		goto err0;
+
+	/* Wait for completion only if TX_FIFO_NOT_EMPTY is still set */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 5, 5)) {
+		if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(10)) == 0) {
+			DSSERR("Failed to complete previous l4 transfer\n");
+			r = -EIO;
+			goto err1;
+		}
+	}
+
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+
+	return 0;
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
+		&l4_data, DSI_VC_IRQ_PACKET_SENT);
+err0:
+	return r;
+}
+
+static int dsi_sync_vc(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	WARN_ON(in_interrupt());
+
+	if (!dsi_vc_is_enabled(dsidev, channel))
+		return 0;
+
+	switch (dsi->vc[channel].source) {
+	case DSI_VC_SOURCE_VP:
+		return dsi_sync_vc_vp(dsidev, channel);
+	case DSI_VC_SOURCE_L4:
+		return dsi_sync_vc_l4(dsidev, channel);
+	default:
+		BUG();
+		return -EINVAL;
+	}
+}
+
+static int dsi_vc_enable(struct platform_device *dsidev, int channel,
+		bool enable)
+{
+	DSSDBG("dsi_vc_enable channel %d, enable %d\n",
+			channel, enable);
+
+	enable = enable ? 1 : 0;
+
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 0, 0);
+
+	if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel),
+		0, enable) != enable) {
+			DSSERR("Failed to set dsi_vc_enable to %d\n", enable);
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static void dsi_vc_initial_config(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 r;
+
+	DSSDBG("Initial config of virtual channel %d", channel);
+
+	r = dsi_read_reg(dsidev, DSI_VC_CTRL(channel));
+
+	if (FLD_GET(r, 15, 15)) /* VC_BUSY */
+		DSSERR("VC(%d) busy when trying to configure it!\n",
+				channel);
+
+	r = FLD_MOD(r, 0, 1, 1); /* SOURCE, 0 = L4 */
+	r = FLD_MOD(r, 0, 2, 2); /* BTA_SHORT_EN  */
+	r = FLD_MOD(r, 0, 3, 3); /* BTA_LONG_EN */
+	r = FLD_MOD(r, 0, 4, 4); /* MODE, 0 = command */
+	r = FLD_MOD(r, 1, 7, 7); /* CS_TX_EN */
+	r = FLD_MOD(r, 1, 8, 8); /* ECC_TX_EN */
+	r = FLD_MOD(r, 0, 9, 9); /* MODE_SPEED, high speed on/off */
+	if (dss_has_feature(FEAT_DSI_VC_OCP_WIDTH))
+		r = FLD_MOD(r, 3, 11, 10);	/* OCP_WIDTH = 32 bit */
+
+	r = FLD_MOD(r, 4, 29, 27); /* DMA_RX_REQ_NB = no dma */
+	r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
+
+	dsi_write_reg(dsidev, DSI_VC_CTRL(channel), r);
+
+	dsi->vc[channel].source = DSI_VC_SOURCE_L4;
+}
+
+static int dsi_vc_config_source(struct platform_device *dsidev, int channel,
+		enum dsi_vc_source source)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->vc[channel].source == source)
+		return 0;
+
+	DSSDBG("Source config of virtual channel %d", channel);
+
+	dsi_sync_vc(dsidev, channel);
+
+	dsi_vc_enable(dsidev, channel, 0);
+
+	/* VC_BUSY */
+	if (wait_for_bit_change(dsidev, DSI_VC_CTRL(channel), 15, 0) != 0) {
+		DSSERR("vc(%d) busy when trying to config for VP\n", channel);
+		return -EIO;
+	}
+
+	/* SOURCE, 0 = L4, 1 = video port */
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), source, 1, 1);
+
+	/* DCS_CMD_ENABLE */
+	if (dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC)) {
+		bool enable = source == DSI_VC_SOURCE_VP;
+		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 30, 30);
+	}
+
+	dsi_vc_enable(dsidev, channel, 1);
+
+	dsi->vc[channel].source = source;
+
+	return 0;
+}
+
+static void dsi_vc_enable_hs(struct omap_dss_device *dssdev, int channel,
+		bool enable)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	DSSDBG("dsi_vc_enable_hs(%d, %d)\n", channel, enable);
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	dsi_vc_enable(dsidev, channel, 0);
+	dsi_if_enable(dsidev, 0);
+
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), enable, 9, 9);
+
+	dsi_vc_enable(dsidev, channel, 1);
+	dsi_if_enable(dsidev, 1);
+
+	dsi_force_tx_stop_mode_io(dsidev);
+
+	/* start the DDR clock by sending a NULL packet */
+	if (dsi->vm_timings.ddr_clk_always_on && enable)
+		dsi_vc_send_null(dssdev, channel);
+}
+
+static void dsi_vc_flush_long_data(struct platform_device *dsidev, int channel)
+{
+	while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+		u32 val;
+		val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+		DSSDBG("\t\tb1 %#02x b2 %#02x b3 %#02x b4 %#02x\n",
+				(val >> 0) & 0xff,
+				(val >> 8) & 0xff,
+				(val >> 16) & 0xff,
+				(val >> 24) & 0xff);
+	}
+}
+
+static void dsi_show_rx_ack_with_err(u16 err)
+{
+	DSSERR("\tACK with ERROR (%#x):\n", err);
+	if (err & (1 << 0))
+		DSSERR("\t\tSoT Error\n");
+	if (err & (1 << 1))
+		DSSERR("\t\tSoT Sync Error\n");
+	if (err & (1 << 2))
+		DSSERR("\t\tEoT Sync Error\n");
+	if (err & (1 << 3))
+		DSSERR("\t\tEscape Mode Entry Command Error\n");
+	if (err & (1 << 4))
+		DSSERR("\t\tLP Transmit Sync Error\n");
+	if (err & (1 << 5))
+		DSSERR("\t\tHS Receive Timeout Error\n");
+	if (err & (1 << 6))
+		DSSERR("\t\tFalse Control Error\n");
+	if (err & (1 << 7))
+		DSSERR("\t\t(reserved7)\n");
+	if (err & (1 << 8))
+		DSSERR("\t\tECC Error, single-bit (corrected)\n");
+	if (err & (1 << 9))
+		DSSERR("\t\tECC Error, multi-bit (not corrected)\n");
+	if (err & (1 << 10))
+		DSSERR("\t\tChecksum Error\n");
+	if (err & (1 << 11))
+		DSSERR("\t\tData type not recognized\n");
+	if (err & (1 << 12))
+		DSSERR("\t\tInvalid VC ID\n");
+	if (err & (1 << 13))
+		DSSERR("\t\tInvalid Transmission Length\n");
+	if (err & (1 << 14))
+		DSSERR("\t\t(reserved14)\n");
+	if (err & (1 << 15))
+		DSSERR("\t\tDSI Protocol Violation\n");
+}
+
+static u16 dsi_vc_flush_receive_data(struct platform_device *dsidev,
+		int channel)
+{
+	/* RX_FIFO_NOT_EMPTY */
+	while (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+		u32 val;
+		u8 dt;
+		val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+		DSSERR("\trawval %#08x\n", val);
+		dt = FLD_GET(val, 5, 0);
+		if (dt == MIPI_DSI_RX_ACKNOWLEDGE_AND_ERROR_REPORT) {
+			u16 err = FLD_GET(val, 23, 8);
+			dsi_show_rx_ack_with_err(err);
+		} else if (dt == MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_1BYTE) {
+			DSSERR("\tDCS short response, 1 byte: %#x\n",
+					FLD_GET(val, 23, 8));
+		} else if (dt == MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_2BYTE) {
+			DSSERR("\tDCS short response, 2 byte: %#x\n",
+					FLD_GET(val, 23, 8));
+		} else if (dt == MIPI_DSI_RX_DCS_LONG_READ_RESPONSE) {
+			DSSERR("\tDCS long response, len %d\n",
+					FLD_GET(val, 23, 8));
+			dsi_vc_flush_long_data(dsidev, channel);
+		} else {
+			DSSERR("\tunknown datatype 0x%02x\n", dt);
+		}
+	}
+	return 0;
+}
+
+static int dsi_vc_send_bta(struct platform_device *dsidev, int channel)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->debug_write || dsi->debug_read)
+		DSSDBG("dsi_vc_send_bta %d\n", channel);
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	/* RX_FIFO_NOT_EMPTY */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+		DSSERR("rx fifo not empty when sending BTA, dumping data:\n");
+		dsi_vc_flush_receive_data(dsidev, channel);
+	}
+
+	REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 6, 6); /* BTA_EN */
+
+	/* flush posted write */
+	dsi_read_reg(dsidev, DSI_VC_CTRL(channel));
+
+	return 0;
+}
+
+static int dsi_vc_send_bta_sync(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	DECLARE_COMPLETION_ONSTACK(completion);
+	int r = 0;
+	u32 err;
+
+	r = dsi_register_isr_vc(dsidev, channel, dsi_completion_handler,
+			&completion, DSI_VC_IRQ_BTA);
+	if (r)
+		goto err0;
+
+	r = dsi_register_isr(dsidev, dsi_completion_handler, &completion,
+			DSI_IRQ_ERROR_MASK);
+	if (r)
+		goto err1;
+
+	r = dsi_vc_send_bta(dsidev, channel);
+	if (r)
+		goto err2;
+
+	if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(500)) == 0) {
+		DSSERR("Failed to receive BTA\n");
+		r = -EIO;
+		goto err2;
+	}
+
+	err = dsi_get_errors(dsidev);
+	if (err) {
+		DSSERR("Error while sending BTA: %x\n", err);
+		r = -EIO;
+		goto err2;
+	}
+err2:
+	dsi_unregister_isr(dsidev, dsi_completion_handler, &completion,
+			DSI_IRQ_ERROR_MASK);
+err1:
+	dsi_unregister_isr_vc(dsidev, channel, dsi_completion_handler,
+			&completion, DSI_VC_IRQ_BTA);
+err0:
+	return r;
+}
+
+static inline void dsi_vc_write_long_header(struct platform_device *dsidev,
+		int channel, u8 data_type, u16 len, u8 ecc)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 val;
+	u8 data_id;
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	data_id = data_type | dsi->vc[channel].vc_id << 6;
+
+	val = FLD_VAL(data_id, 7, 0) | FLD_VAL(len, 23, 8) |
+		FLD_VAL(ecc, 31, 24);
+
+	dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_HEADER(channel), val);
+}
+
+static inline void dsi_vc_write_long_payload(struct platform_device *dsidev,
+		int channel, u8 b1, u8 b2, u8 b3, u8 b4)
+{
+	u32 val;
+
+	val = b4 << 24 | b3 << 16 | b2 << 8  | b1 << 0;
+
+/*	DSSDBG("\twriting %02x, %02x, %02x, %02x (%#010x)\n",
+			b1, b2, b3, b4, val); */
+
+	dsi_write_reg(dsidev, DSI_VC_LONG_PACKET_PAYLOAD(channel), val);
+}
+
+static int dsi_vc_send_long(struct platform_device *dsidev, int channel,
+		u8 data_type, u8 *data, u16 len, u8 ecc)
+{
+	/*u32 val; */
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+	u8 *p;
+	int r = 0;
+	u8 b1, b2, b3, b4;
+
+	if (dsi->debug_write)
+		DSSDBG("dsi_vc_send_long, %d bytes\n", len);
+
+	/* len + header */
+	if (dsi->vc[channel].tx_fifo_size * 32 * 4 < len + 4) {
+		DSSERR("unable to send long packet: packet too long.\n");
+		return -EINVAL;
+	}
+
+	dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_L4);
+
+	dsi_vc_write_long_header(dsidev, channel, data_type, len, ecc);
+
+	p = data;
+	for (i = 0; i < len >> 2; i++) {
+		if (dsi->debug_write)
+			DSSDBG("\tsending full packet %d\n", i);
+
+		b1 = *p++;
+		b2 = *p++;
+		b3 = *p++;
+		b4 = *p++;
+
+		dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, b4);
+	}
+
+	i = len % 4;
+	if (i) {
+		b1 = 0; b2 = 0; b3 = 0;
+
+		if (dsi->debug_write)
+			DSSDBG("\tsending remainder bytes %d\n", i);
+
+		switch (i) {
+		case 3:
+			b1 = *p++;
+			b2 = *p++;
+			b3 = *p++;
+			break;
+		case 2:
+			b1 = *p++;
+			b2 = *p++;
+			break;
+		case 1:
+			b1 = *p++;
+			break;
+		}
+
+		dsi_vc_write_long_payload(dsidev, channel, b1, b2, b3, 0);
+	}
+
+	return r;
+}
+
+static int dsi_vc_send_short(struct platform_device *dsidev, int channel,
+		u8 data_type, u16 data, u8 ecc)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 r;
+	u8 data_id;
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	if (dsi->debug_write)
+		DSSDBG("dsi_vc_send_short(ch%d, dt %#x, b1 %#x, b2 %#x)\n",
+				channel,
+				data_type, data & 0xff, (data >> 8) & 0xff);
+
+	dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_L4);
+
+	if (FLD_GET(dsi_read_reg(dsidev, DSI_VC_CTRL(channel)), 16, 16)) {
+		DSSERR("ERROR FIFO FULL, aborting transfer\n");
+		return -EINVAL;
+	}
+
+	data_id = data_type | dsi->vc[channel].vc_id << 6;
+
+	r = (data_id << 0) | (data << 8) | (ecc << 24);
+
+	dsi_write_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel), r);
+
+	return 0;
+}
+
+static int dsi_vc_send_null(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_send_long(dsidev, channel, MIPI_DSI_NULL_PACKET, NULL,
+		0, 0);
+}
+
+static int dsi_vc_write_nosync_common(struct platform_device *dsidev,
+		int channel, u8 *data, int len, enum dss_dsi_content_type type)
+{
+	int r;
+
+	if (len == 0) {
+		BUG_ON(type == DSS_DSI_CONTENT_DCS);
+		r = dsi_vc_send_short(dsidev, channel,
+				MIPI_DSI_GENERIC_SHORT_WRITE_0_PARAM, 0, 0);
+	} else if (len == 1) {
+		r = dsi_vc_send_short(dsidev, channel,
+				type == DSS_DSI_CONTENT_GENERIC ?
+				MIPI_DSI_GENERIC_SHORT_WRITE_1_PARAM :
+				MIPI_DSI_DCS_SHORT_WRITE, data[0], 0);
+	} else if (len == 2) {
+		r = dsi_vc_send_short(dsidev, channel,
+				type == DSS_DSI_CONTENT_GENERIC ?
+				MIPI_DSI_GENERIC_SHORT_WRITE_2_PARAM :
+				MIPI_DSI_DCS_SHORT_WRITE_PARAM,
+				data[0] | (data[1] << 8), 0);
+	} else {
+		r = dsi_vc_send_long(dsidev, channel,
+				type == DSS_DSI_CONTENT_GENERIC ?
+				MIPI_DSI_GENERIC_LONG_WRITE :
+				MIPI_DSI_DCS_LONG_WRITE, data, len, 0);
+	}
+
+	return r;
+}
+
+static int dsi_vc_dcs_write_nosync(struct omap_dss_device *dssdev, int channel,
+		u8 *data, int len)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_write_nosync_common(dsidev, channel, data, len,
+			DSS_DSI_CONTENT_DCS);
+}
+
+static int dsi_vc_generic_write_nosync(struct omap_dss_device *dssdev, int channel,
+		u8 *data, int len)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_write_nosync_common(dsidev, channel, data, len,
+			DSS_DSI_CONTENT_GENERIC);
+}
+
+static int dsi_vc_write_common(struct omap_dss_device *dssdev, int channel,
+		u8 *data, int len, enum dss_dsi_content_type type)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	int r;
+
+	r = dsi_vc_write_nosync_common(dsidev, channel, data, len, type);
+	if (r)
+		goto err;
+
+	r = dsi_vc_send_bta_sync(dssdev, channel);
+	if (r)
+		goto err;
+
+	/* RX_FIFO_NOT_EMPTY */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20)) {
+		DSSERR("rx fifo not empty after write, dumping data:\n");
+		dsi_vc_flush_receive_data(dsidev, channel);
+		r = -EIO;
+		goto err;
+	}
+
+	return 0;
+err:
+	DSSERR("dsi_vc_write_common(ch %d, cmd 0x%02x, len %d) failed\n",
+			channel, data[0], len);
+	return r;
+}
+
+static int dsi_vc_dcs_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+		int len)
+{
+	return dsi_vc_write_common(dssdev, channel, data, len,
+			DSS_DSI_CONTENT_DCS);
+}
+
+static int dsi_vc_generic_write(struct omap_dss_device *dssdev, int channel, u8 *data,
+		int len)
+{
+	return dsi_vc_write_common(dssdev, channel, data, len,
+			DSS_DSI_CONTENT_GENERIC);
+}
+
+static int dsi_vc_dcs_send_read_request(struct platform_device *dsidev,
+		int channel, u8 dcs_cmd)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+
+	if (dsi->debug_read)
+		DSSDBG("dsi_vc_dcs_send_read_request(ch%d, dcs_cmd %x)\n",
+			channel, dcs_cmd);
+
+	r = dsi_vc_send_short(dsidev, channel, MIPI_DSI_DCS_READ, dcs_cmd, 0);
+	if (r) {
+		DSSERR("dsi_vc_dcs_send_read_request(ch %d, cmd 0x%02x)"
+			" failed\n", channel, dcs_cmd);
+		return r;
+	}
+
+	return 0;
+}
+
+static int dsi_vc_generic_send_read_request(struct platform_device *dsidev,
+		int channel, u8 *reqdata, int reqlen)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u16 data;
+	u8 data_type;
+	int r;
+
+	if (dsi->debug_read)
+		DSSDBG("dsi_vc_generic_send_read_request(ch %d, reqlen %d)\n",
+			channel, reqlen);
+
+	if (reqlen == 0) {
+		data_type = MIPI_DSI_GENERIC_READ_REQUEST_0_PARAM;
+		data = 0;
+	} else if (reqlen == 1) {
+		data_type = MIPI_DSI_GENERIC_READ_REQUEST_1_PARAM;
+		data = reqdata[0];
+	} else if (reqlen == 2) {
+		data_type = MIPI_DSI_GENERIC_READ_REQUEST_2_PARAM;
+		data = reqdata[0] | (reqdata[1] << 8);
+	} else {
+		BUG();
+		return -EINVAL;
+	}
+
+	r = dsi_vc_send_short(dsidev, channel, data_type, data, 0);
+	if (r) {
+		DSSERR("dsi_vc_generic_send_read_request(ch %d, reqlen %d)"
+			" failed\n", channel, reqlen);
+		return r;
+	}
+
+	return 0;
+}
+
+static int dsi_vc_read_rx_fifo(struct platform_device *dsidev, int channel,
+		u8 *buf, int buflen, enum dss_dsi_content_type type)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 val;
+	u8 dt;
+	int r;
+
+	/* RX_FIFO_NOT_EMPTY */
+	if (REG_GET(dsidev, DSI_VC_CTRL(channel), 20, 20) == 0) {
+		DSSERR("RX fifo empty when trying to read.\n");
+		r = -EIO;
+		goto err;
+	}
+
+	val = dsi_read_reg(dsidev, DSI_VC_SHORT_PACKET_HEADER(channel));
+	if (dsi->debug_read)
+		DSSDBG("\theader: %08x\n", val);
+	dt = FLD_GET(val, 5, 0);
+	if (dt == MIPI_DSI_RX_ACKNOWLEDGE_AND_ERROR_REPORT) {
+		u16 err = FLD_GET(val, 23, 8);
+		dsi_show_rx_ack_with_err(err);
+		r = -EIO;
+		goto err;
+
+	} else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+			MIPI_DSI_RX_GENERIC_SHORT_READ_RESPONSE_1BYTE :
+			MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_1BYTE)) {
+		u8 data = FLD_GET(val, 15, 8);
+		if (dsi->debug_read)
+			DSSDBG("\t%s short response, 1 byte: %02x\n",
+				type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+				"DCS", data);
+
+		if (buflen < 1) {
+			r = -EIO;
+			goto err;
+		}
+
+		buf[0] = data;
+
+		return 1;
+	} else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+			MIPI_DSI_RX_GENERIC_SHORT_READ_RESPONSE_2BYTE :
+			MIPI_DSI_RX_DCS_SHORT_READ_RESPONSE_2BYTE)) {
+		u16 data = FLD_GET(val, 23, 8);
+		if (dsi->debug_read)
+			DSSDBG("\t%s short response, 2 byte: %04x\n",
+				type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+				"DCS", data);
+
+		if (buflen < 2) {
+			r = -EIO;
+			goto err;
+		}
+
+		buf[0] = data & 0xff;
+		buf[1] = (data >> 8) & 0xff;
+
+		return 2;
+	} else if (dt == (type == DSS_DSI_CONTENT_GENERIC ?
+			MIPI_DSI_RX_GENERIC_LONG_READ_RESPONSE :
+			MIPI_DSI_RX_DCS_LONG_READ_RESPONSE)) {
+		int w;
+		int len = FLD_GET(val, 23, 8);
+		if (dsi->debug_read)
+			DSSDBG("\t%s long response, len %d\n",
+				type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" :
+				"DCS", len);
+
+		if (len > buflen) {
+			r = -EIO;
+			goto err;
+		}
+
+		/* two byte checksum ends the packet, not included in len */
+		for (w = 0; w < len + 2;) {
+			int b;
+			val = dsi_read_reg(dsidev,
+				DSI_VC_SHORT_PACKET_HEADER(channel));
+			if (dsi->debug_read)
+				DSSDBG("\t\t%02x %02x %02x %02x\n",
+						(val >> 0) & 0xff,
+						(val >> 8) & 0xff,
+						(val >> 16) & 0xff,
+						(val >> 24) & 0xff);
+
+			for (b = 0; b < 4; ++b) {
+				if (w < len)
+					buf[w] = (val >> (b * 8)) & 0xff;
+				/* we discard the 2 byte checksum */
+				++w;
+			}
+		}
+
+		return len;
+	} else {
+		DSSERR("\tunknown datatype 0x%02x\n", dt);
+		r = -EIO;
+		goto err;
+	}
+
+err:
+	DSSERR("dsi_vc_read_rx_fifo(ch %d type %s) failed\n", channel,
+		type == DSS_DSI_CONTENT_GENERIC ? "GENERIC" : "DCS");
+
+	return r;
+}
+
+static int dsi_vc_dcs_read(struct omap_dss_device *dssdev, int channel, u8 dcs_cmd,
+		u8 *buf, int buflen)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	int r;
+
+	r = dsi_vc_dcs_send_read_request(dsidev, channel, dcs_cmd);
+	if (r)
+		goto err;
+
+	r = dsi_vc_send_bta_sync(dssdev, channel);
+	if (r)
+		goto err;
+
+	r = dsi_vc_read_rx_fifo(dsidev, channel, buf, buflen,
+		DSS_DSI_CONTENT_DCS);
+	if (r < 0)
+		goto err;
+
+	if (r != buflen) {
+		r = -EIO;
+		goto err;
+	}
+
+	return 0;
+err:
+	DSSERR("dsi_vc_dcs_read(ch %d, cmd 0x%02x) failed\n", channel, dcs_cmd);
+	return r;
+}
+
+static int dsi_vc_generic_read(struct omap_dss_device *dssdev, int channel,
+		u8 *reqdata, int reqlen, u8 *buf, int buflen)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	int r;
+
+	r = dsi_vc_generic_send_read_request(dsidev, channel, reqdata, reqlen);
+	if (r)
+		return r;
+
+	r = dsi_vc_send_bta_sync(dssdev, channel);
+	if (r)
+		return r;
+
+	r = dsi_vc_read_rx_fifo(dsidev, channel, buf, buflen,
+		DSS_DSI_CONTENT_GENERIC);
+	if (r < 0)
+		return r;
+
+	if (r != buflen) {
+		r = -EIO;
+		return r;
+	}
+
+	return 0;
+}
+
+static int dsi_vc_set_max_rx_packet_size(struct omap_dss_device *dssdev, int channel,
+		u16 len)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+
+	return dsi_vc_send_short(dsidev, channel,
+			MIPI_DSI_SET_MAXIMUM_RETURN_PACKET_SIZE, len, 0);
+}
+
+static int dsi_enter_ulps(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	DECLARE_COMPLETION_ONSTACK(completion);
+	int r, i;
+	unsigned mask;
+
+	DSSDBG("Entering ULPS");
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	WARN_ON(dsi->ulps_enabled);
+
+	if (dsi->ulps_enabled)
+		return 0;
+
+	/* DDR_CLK_ALWAYS_ON */
+	if (REG_GET(dsidev, DSI_CLK_CTRL, 13, 13)) {
+		dsi_if_enable(dsidev, 0);
+		REG_FLD_MOD(dsidev, DSI_CLK_CTRL, 0, 13, 13);
+		dsi_if_enable(dsidev, 1);
+	}
+
+	dsi_sync_vc(dsidev, 0);
+	dsi_sync_vc(dsidev, 1);
+	dsi_sync_vc(dsidev, 2);
+	dsi_sync_vc(dsidev, 3);
+
+	dsi_force_tx_stop_mode_io(dsidev);
+
+	dsi_vc_enable(dsidev, 0, false);
+	dsi_vc_enable(dsidev, 1, false);
+	dsi_vc_enable(dsidev, 2, false);
+	dsi_vc_enable(dsidev, 3, false);
+
+	if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 16, 16)) {	/* HS_BUSY */
+		DSSERR("HS busy when enabling ULPS\n");
+		return -EIO;
+	}
+
+	if (REG_GET(dsidev, DSI_COMPLEXIO_CFG2, 17, 17)) {	/* LP_BUSY */
+		DSSERR("LP busy when enabling ULPS\n");
+		return -EIO;
+	}
+
+	r = dsi_register_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+	if (r)
+		return r;
+
+	mask = 0;
+
+	for (i = 0; i < dsi->num_lanes_supported; ++i) {
+		if (dsi->lanes[i].function == DSI_LANE_UNUSED)
+			continue;
+		mask |= 1 << i;
+	}
+	/* Assert TxRequestEsc for data lanes and TxUlpsClk for clk lane */
+	/* LANEx_ULPS_SIG2 */
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, mask, 9, 5);
+
+	/* flush posted write and wait for SCP interface to finish the write */
+	dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);
+
+	if (wait_for_completion_timeout(&completion,
+				msecs_to_jiffies(1000)) == 0) {
+		DSSERR("ULPS enable timeout\n");
+		r = -EIO;
+		goto err;
+	}
+
+	dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+
+	/* Reset LANEx_ULPS_SIG2 */
+	REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, 0, 9, 5);
+
+	/* flush posted write and wait for SCP interface to finish the write */
+	dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);
+
+	dsi_cio_power(dsidev, DSI_COMPLEXIO_POWER_ULPS);
+
+	dsi_if_enable(dsidev, false);
+
+	dsi->ulps_enabled = true;
+
+	return 0;
+
+err:
+	dsi_unregister_isr_cio(dsidev, dsi_completion_handler, &completion,
+			DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
+	return r;
+}
+
+static void dsi_set_lp_rx_timeout(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
+{
+	unsigned long fck;
+	unsigned long total_ticks;
+	u32 r;
+
+	BUG_ON(ticks > 0x1fff);
+
+	/* ticks in DSI_FCK */
+	fck = dsi_fclk_rate(dsidev);
+
+	r = dsi_read_reg(dsidev, DSI_TIMING2);
+	r = FLD_MOD(r, 1, 15, 15);	/* LP_RX_TO */
+	r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);	/* LP_RX_TO_X16 */
+	r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);	/* LP_RX_TO_X4 */
+	r = FLD_MOD(r, ticks, 12, 0);	/* LP_RX_COUNTER */
+	dsi_write_reg(dsidev, DSI_TIMING2, r);
+
+	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+	DSSDBG("LP_RX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+			total_ticks,
+			ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+			(total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_ta_timeout(struct platform_device *dsidev, unsigned ticks,
+		bool x8, bool x16)
+{
+	unsigned long fck;
+	unsigned long total_ticks;
+	u32 r;
+
+	BUG_ON(ticks > 0x1fff);
+
+	/* ticks in DSI_FCK */
+	fck = dsi_fclk_rate(dsidev);
+
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
+	r = FLD_MOD(r, 1, 31, 31);	/* TA_TO */
+	r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);	/* TA_TO_X16 */
+	r = FLD_MOD(r, x8 ? 1 : 0, 29, 29);	/* TA_TO_X8 */
+	r = FLD_MOD(r, ticks, 28, 16);	/* TA_TO_COUNTER */
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
+
+	total_ticks = ticks * (x16 ? 16 : 1) * (x8 ? 8 : 1);
+
+	DSSDBG("TA_TO %lu ticks (%#x%s%s) = %lu ns\n",
+			total_ticks,
+			ticks, x8 ? " x8" : "", x16 ? " x16" : "",
+			(total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_stop_state_counter(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
+{
+	unsigned long fck;
+	unsigned long total_ticks;
+	u32 r;
+
+	BUG_ON(ticks > 0x1fff);
+
+	/* ticks in DSI_FCK */
+	fck = dsi_fclk_rate(dsidev);
+
+	r = dsi_read_reg(dsidev, DSI_TIMING1);
+	r = FLD_MOD(r, 1, 15, 15);	/* FORCE_TX_STOP_MODE_IO */
+	r = FLD_MOD(r, x16 ? 1 : 0, 14, 14);	/* STOP_STATE_X16_IO */
+	r = FLD_MOD(r, x4 ? 1 : 0, 13, 13);	/* STOP_STATE_X4_IO */
+	r = FLD_MOD(r, ticks, 12, 0);	/* STOP_STATE_COUNTER_IO */
+	dsi_write_reg(dsidev, DSI_TIMING1, r);
+
+	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+	DSSDBG("STOP_STATE_COUNTER %lu ticks (%#x%s%s) = %lu ns\n",
+			total_ticks,
+			ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+			(total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_set_hs_tx_timeout(struct platform_device *dsidev,
+		unsigned ticks, bool x4, bool x16)
+{
+	unsigned long fck;
+	unsigned long total_ticks;
+	u32 r;
+
+	BUG_ON(ticks > 0x1fff);
+
+	/* ticks in TxByteClkHS */
+	fck = dsi_get_txbyteclkhs(dsidev);
+
+	r = dsi_read_reg(dsidev, DSI_TIMING2);
+	r = FLD_MOD(r, 1, 31, 31);	/* HS_TX_TO */
+	r = FLD_MOD(r, x16 ? 1 : 0, 30, 30);	/* HS_TX_TO_X16 */
+	r = FLD_MOD(r, x4 ? 1 : 0, 29, 29);	/* HS_TX_TO_X8 (4 really) */
+	r = FLD_MOD(r, ticks, 28, 16);	/* HS_TX_TO_COUNTER */
+	dsi_write_reg(dsidev, DSI_TIMING2, r);
+
+	total_ticks = ticks * (x16 ? 16 : 1) * (x4 ? 4 : 1);
+
+	DSSDBG("HS_TX_TO %lu ticks (%#x%s%s) = %lu ns\n",
+			total_ticks,
+			ticks, x4 ? " x4" : "", x16 ? " x16" : "",
+			(total_ticks * 1000) / (fck / 1000 / 1000));
+}
+
+static void dsi_config_vp_num_line_buffers(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int num_line_buffers;
+
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+		struct omap_video_timings *timings = &dsi->timings;
+		/*
+		 * Don't use line buffers if width is greater than the video
+		 * port's line buffer size
+		 */
+		if (dsi->line_buffer_size <= timings->x_res * bpp / 8)
+			num_line_buffers = 0;
+		else
+			num_line_buffers = 2;
+	} else {
+		/* Use maximum number of line buffers in command mode */
+		num_line_buffers = 2;
+	}
+
+	/* LINE_BUFFER */
+	REG_FLD_MOD(dsidev, DSI_CTRL, num_line_buffers, 13, 12);
+}
+
+static void dsi_config_vp_sync_events(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	bool sync_end;
+	u32 r;
+
+	if (dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE)
+		sync_end = true;
+	else
+		sync_end = false;
+
+	r = dsi_read_reg(dsidev, DSI_CTRL);
+	r = FLD_MOD(r, 1, 9, 9);		/* VP_DE_POL */
+	r = FLD_MOD(r, 1, 10, 10);		/* VP_HSYNC_POL */
+	r = FLD_MOD(r, 1, 11, 11);		/* VP_VSYNC_POL */
+	r = FLD_MOD(r, 1, 15, 15);		/* VP_VSYNC_START */
+	r = FLD_MOD(r, sync_end, 16, 16);	/* VP_VSYNC_END */
+	r = FLD_MOD(r, 1, 17, 17);		/* VP_HSYNC_START */
+	r = FLD_MOD(r, sync_end, 18, 18);	/* VP_HSYNC_END */
+	dsi_write_reg(dsidev, DSI_CTRL, r);
+}
+
+static void dsi_config_blanking_modes(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int blanking_mode = dsi->vm_timings.blanking_mode;
+	int hfp_blanking_mode = dsi->vm_timings.hfp_blanking_mode;
+	int hbp_blanking_mode = dsi->vm_timings.hbp_blanking_mode;
+	int hsa_blanking_mode = dsi->vm_timings.hsa_blanking_mode;
+	u32 r;
+
+	/*
+	 * 0 = TX FIFO packets sent or LPS in corresponding blanking periods
+	 * 1 = Long blanking packets are sent in corresponding blanking periods
+	 */
+	r = dsi_read_reg(dsidev, DSI_CTRL);
+	r = FLD_MOD(r, blanking_mode, 20, 20);		/* BLANKING_MODE */
+	r = FLD_MOD(r, hfp_blanking_mode, 21, 21);	/* HFP_BLANKING */
+	r = FLD_MOD(r, hbp_blanking_mode, 22, 22);	/* HBP_BLANKING */
+	r = FLD_MOD(r, hsa_blanking_mode, 23, 23);	/* HSA_BLANKING */
+	dsi_write_reg(dsidev, DSI_CTRL, r);
+}
+
+/*
+ * According to section 'HS Command Mode Interleaving' in OMAP TRM, Scenario 3
+ * results in maximum transition time for data and clock lanes to enter and
+ * exit HS mode. Hence, this is the scenario where the least amount of command
+ * mode data can be interleaved. We program the minimum amount of TXBYTECLKHS
+ * clock cycles that can be used to interleave command mode data in HS so that
+ * all scenarios are satisfied.
+ */
+static int dsi_compute_interleave_hs(int blank, bool ddr_alwon, int enter_hs,
+		int exit_hs, int exiths_clk, int ddr_pre, int ddr_post)
+{
+	int transition;
+
+	/*
+	 * If DDR_CLK_ALWAYS_ON is set, we need to consider HS mode transition
+	 * time of data lanes only, if it isn't set, we need to consider HS
+	 * transition time of both data and clock lanes. HS transition time
+	 * of Scenario 3 is considered.
+	 */
+	if (ddr_alwon) {
+		transition = enter_hs + exit_hs + max(enter_hs, 2) + 1;
+	} else {
+		int trans1, trans2;
+		trans1 = ddr_pre + enter_hs + exit_hs + max(enter_hs, 2) + 1;
+		trans2 = ddr_pre + enter_hs + exiths_clk + ddr_post + ddr_pre +
+				enter_hs + 1;
+		transition = max(trans1, trans2);
+	}
+
+	return blank > transition ? blank - transition : 0;
+}
+
+/*
+ * According to section 'LP Command Mode Interleaving' in OMAP TRM, Scenario 1
+ * results in maximum transition time for data lanes to enter and exit LP mode.
+ * Hence, this is the scenario where the least amount of command mode data can
+ * be interleaved. We program the minimum amount of bytes that can be
+ * interleaved in LP so that all scenarios are satisfied.
+ */
+static int dsi_compute_interleave_lp(int blank, int enter_hs, int exit_hs,
+		int lp_clk_div, int tdsi_fclk)
+{
+	int trans_lp;	/* time required for a LP transition, in TXBYTECLKHS */
+	int tlp_avail;	/* time left for interleaving commands, in CLKIN4DDR */
+	int ttxclkesc;	/* period of LP transmit escape clock, in CLKIN4DDR */
+	int thsbyte_clk = 16;	/* Period of TXBYTECLKHS clock, in CLKIN4DDR */
+	int lp_inter;	/* cmd mode data that can be interleaved, in bytes */
+
+	/* maximum LP transition time according to Scenario 1 */
+	trans_lp = exit_hs + max(enter_hs, 2) + 1;
+
+	/* CLKIN4DDR = 16 * TXBYTECLKHS */
+	tlp_avail = thsbyte_clk * (blank - trans_lp);
+
+	ttxclkesc = tdsi_fclk * lp_clk_div;
+
+	lp_inter = ((tlp_avail - 8 * thsbyte_clk - 5 * tdsi_fclk) / ttxclkesc -
+			26) / 16;
+
+	return max(lp_inter, 0);
+}
+
+static void dsi_config_cmd_mode_interleaving(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int blanking_mode;
+	int hfp_blanking_mode, hbp_blanking_mode, hsa_blanking_mode;
+	int hsa, hfp, hbp, width_bytes, bllp, lp_clk_div;
+	int ddr_clk_pre, ddr_clk_post, enter_hs_mode_lat, exit_hs_mode_lat;
+	int tclk_trail, ths_exit, exiths_clk;
+	bool ddr_alwon;
+	struct omap_video_timings *timings = &dsi->timings;
+	int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+	int ndl = dsi->num_lanes_used - 1;
+	int dsi_fclk_hsdiv = dsi->user_dsi_cinfo.mX[HSDIV_DSI] + 1;
+	int hsa_interleave_hs = 0, hsa_interleave_lp = 0;
+	int hfp_interleave_hs = 0, hfp_interleave_lp = 0;
+	int hbp_interleave_hs = 0, hbp_interleave_lp = 0;
+	int bl_interleave_hs = 0, bl_interleave_lp = 0;
+	u32 r;
+
+	r = dsi_read_reg(dsidev, DSI_CTRL);
+	blanking_mode = FLD_GET(r, 20, 20);
+	hfp_blanking_mode = FLD_GET(r, 21, 21);
+	hbp_blanking_mode = FLD_GET(r, 22, 22);
+	hsa_blanking_mode = FLD_GET(r, 23, 23);
+
+	r = dsi_read_reg(dsidev, DSI_VM_TIMING1);
+	hbp = FLD_GET(r, 11, 0);
+	hfp = FLD_GET(r, 23, 12);
+	hsa = FLD_GET(r, 31, 24);
+
+	r = dsi_read_reg(dsidev, DSI_CLK_TIMING);
+	ddr_clk_post = FLD_GET(r, 7, 0);
+	ddr_clk_pre = FLD_GET(r, 15, 8);
+
+	r = dsi_read_reg(dsidev, DSI_VM_TIMING7);
+	exit_hs_mode_lat = FLD_GET(r, 15, 0);
+	enter_hs_mode_lat = FLD_GET(r, 31, 16);
+
+	r = dsi_read_reg(dsidev, DSI_CLK_CTRL);
+	lp_clk_div = FLD_GET(r, 12, 0);
+	ddr_alwon = FLD_GET(r, 13, 13);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
+	ths_exit = FLD_GET(r, 7, 0);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
+	tclk_trail = FLD_GET(r, 15, 8);
+
+	exiths_clk = ths_exit + tclk_trail;
+
+	width_bytes = DIV_ROUND_UP(timings->x_res * bpp, 8);
+	bllp = hbp + hfp + hsa + DIV_ROUND_UP(width_bytes + 6, ndl);
+
+	if (!hsa_blanking_mode) {
+		hsa_interleave_hs = dsi_compute_interleave_hs(hsa, ddr_alwon,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					exiths_clk, ddr_clk_pre, ddr_clk_post);
+		hsa_interleave_lp = dsi_compute_interleave_lp(hsa,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					lp_clk_div, dsi_fclk_hsdiv);
+	}
+
+	if (!hfp_blanking_mode) {
+		hfp_interleave_hs = dsi_compute_interleave_hs(hfp, ddr_alwon,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					exiths_clk, ddr_clk_pre, ddr_clk_post);
+		hfp_interleave_lp = dsi_compute_interleave_lp(hfp,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					lp_clk_div, dsi_fclk_hsdiv);
+	}
+
+	if (!hbp_blanking_mode) {
+		hbp_interleave_hs = dsi_compute_interleave_hs(hbp, ddr_alwon,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					exiths_clk, ddr_clk_pre, ddr_clk_post);
+
+		hbp_interleave_lp = dsi_compute_interleave_lp(hbp,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					lp_clk_div, dsi_fclk_hsdiv);
+	}
+
+	if (!blanking_mode) {
+		bl_interleave_hs = dsi_compute_interleave_hs(bllp, ddr_alwon,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					exiths_clk, ddr_clk_pre, ddr_clk_post);
+
+		bl_interleave_lp = dsi_compute_interleave_lp(bllp,
+					enter_hs_mode_lat, exit_hs_mode_lat,
+					lp_clk_div, dsi_fclk_hsdiv);
+	}
+
+	DSSDBG("DSI HS interleaving(TXBYTECLKHS) HSA %d, HFP %d, HBP %d, BLLP %d\n",
+		hsa_interleave_hs, hfp_interleave_hs, hbp_interleave_hs,
+		bl_interleave_hs);
+
+	DSSDBG("DSI LP interleaving(bytes) HSA %d, HFP %d, HBP %d, BLLP %d\n",
+		hsa_interleave_lp, hfp_interleave_lp, hbp_interleave_lp,
+		bl_interleave_lp);
+
+	r = dsi_read_reg(dsidev, DSI_VM_TIMING4);
+	r = FLD_MOD(r, hsa_interleave_hs, 23, 16);
+	r = FLD_MOD(r, hfp_interleave_hs, 15, 8);
+	r = FLD_MOD(r, hbp_interleave_hs, 7, 0);
+	dsi_write_reg(dsidev, DSI_VM_TIMING4, r);
+
+	r = dsi_read_reg(dsidev, DSI_VM_TIMING5);
+	r = FLD_MOD(r, hsa_interleave_lp, 23, 16);
+	r = FLD_MOD(r, hfp_interleave_lp, 15, 8);
+	r = FLD_MOD(r, hbp_interleave_lp, 7, 0);
+	dsi_write_reg(dsidev, DSI_VM_TIMING5, r);
+
+	r = dsi_read_reg(dsidev, DSI_VM_TIMING6);
+	r = FLD_MOD(r, bl_interleave_hs, 31, 15);
+	r = FLD_MOD(r, bl_interleave_lp, 16, 0);
+	dsi_write_reg(dsidev, DSI_VM_TIMING6, r);
+}
+
+static int dsi_proto_config(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u32 r;
+	int buswidth = 0;
+
+	dsi_config_tx_fifo(dsidev, DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32);
+
+	dsi_config_rx_fifo(dsidev, DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32,
+			DSI_FIFO_SIZE_32);
+
+	/* XXX what values for the timeouts? */
+	dsi_set_stop_state_counter(dsidev, 0x1000, false, false);
+	dsi_set_ta_timeout(dsidev, 0x1fff, true, true);
+	dsi_set_lp_rx_timeout(dsidev, 0x1fff, true, true);
+	dsi_set_hs_tx_timeout(dsidev, 0x1fff, true, true);
+
+	switch (dsi_get_pixel_size(dsi->pix_fmt)) {
+	case 16:
+		buswidth = 0;
+		break;
+	case 18:
+		buswidth = 1;
+		break;
+	case 24:
+		buswidth = 2;
+		break;
+	default:
+		BUG();
+		return -EINVAL;
+	}
+
+	r = dsi_read_reg(dsidev, DSI_CTRL);
+	r = FLD_MOD(r, 1, 1, 1);	/* CS_RX_EN */
+	r = FLD_MOD(r, 1, 2, 2);	/* ECC_RX_EN */
+	r = FLD_MOD(r, 1, 3, 3);	/* TX_FIFO_ARBITRATION */
+	r = FLD_MOD(r, 1, 4, 4);	/* VP_CLK_RATIO, always 1, see errata*/
+	r = FLD_MOD(r, buswidth, 7, 6); /* VP_DATA_BUS_WIDTH */
+	r = FLD_MOD(r, 0, 8, 8);	/* VP_CLK_POL */
+	r = FLD_MOD(r, 1, 14, 14);	/* TRIGGER_RESET_MODE */
+	r = FLD_MOD(r, 1, 19, 19);	/* EOT_ENABLE */
+	if (!dss_has_feature(FEAT_DSI_DCS_CMD_CONFIG_VC)) {
+		r = FLD_MOD(r, 1, 24, 24);	/* DCS_CMD_ENABLE */
+		/* DCS_CMD_CODE, 1=start, 0=continue */
+		r = FLD_MOD(r, 0, 25, 25);
+	}
+
+	dsi_write_reg(dsidev, DSI_CTRL, r);
+
+	dsi_config_vp_num_line_buffers(dsidev);
+
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		dsi_config_vp_sync_events(dsidev);
+		dsi_config_blanking_modes(dsidev);
+		dsi_config_cmd_mode_interleaving(dsidev);
+	}
+
+	dsi_vc_initial_config(dsidev, 0);
+	dsi_vc_initial_config(dsidev, 1);
+	dsi_vc_initial_config(dsidev, 2);
+	dsi_vc_initial_config(dsidev, 3);
+
+	return 0;
+}
+
+static void dsi_proto_timings(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	unsigned tlpx, tclk_zero, tclk_prepare, tclk_trail;
+	unsigned tclk_pre, tclk_post;
+	unsigned ths_prepare, ths_prepare_ths_zero, ths_zero;
+	unsigned ths_trail, ths_exit;
+	unsigned ddr_clk_pre, ddr_clk_post;
+	unsigned enter_hs_mode_lat, exit_hs_mode_lat;
+	unsigned ths_eot;
+	int ndl = dsi->num_lanes_used - 1;
+	u32 r;
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG0);
+	ths_prepare = FLD_GET(r, 31, 24);
+	ths_prepare_ths_zero = FLD_GET(r, 23, 16);
+	ths_zero = ths_prepare_ths_zero - ths_prepare;
+	ths_trail = FLD_GET(r, 15, 8);
+	ths_exit = FLD_GET(r, 7, 0);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG1);
+	tlpx = FLD_GET(r, 20, 16) * 2;
+	tclk_trail = FLD_GET(r, 15, 8);
+	tclk_zero = FLD_GET(r, 7, 0);
+
+	r = dsi_read_reg(dsidev, DSI_DSIPHY_CFG2);
+	tclk_prepare = FLD_GET(r, 7, 0);
+
+	/* min 8*UI */
+	tclk_pre = 20;
+	/* min 60ns + 52*UI */
+	tclk_post = ns2ddr(dsidev, 60) + 26;
+
+	ths_eot = DIV_ROUND_UP(4, ndl);
+
+	ddr_clk_pre = DIV_ROUND_UP(tclk_pre + tlpx + tclk_zero + tclk_prepare,
+			4);
+	ddr_clk_post = DIV_ROUND_UP(tclk_post + ths_trail, 4) + ths_eot;
+
+	BUG_ON(ddr_clk_pre == 0 || ddr_clk_pre > 255);
+	BUG_ON(ddr_clk_post == 0 || ddr_clk_post > 255);
+
+	r = dsi_read_reg(dsidev, DSI_CLK_TIMING);
+	r = FLD_MOD(r, ddr_clk_pre, 15, 8);
+	r = FLD_MOD(r, ddr_clk_post, 7, 0);
+	dsi_write_reg(dsidev, DSI_CLK_TIMING, r);
+
+	DSSDBG("ddr_clk_pre %u, ddr_clk_post %u\n",
+			ddr_clk_pre,
+			ddr_clk_post);
+
+	enter_hs_mode_lat = 1 + DIV_ROUND_UP(tlpx, 4) +
+		DIV_ROUND_UP(ths_prepare, 4) +
+		DIV_ROUND_UP(ths_zero + 3, 4);
+
+	exit_hs_mode_lat = DIV_ROUND_UP(ths_trail + ths_exit, 4) + 1 + ths_eot;
+
+	r = FLD_VAL(enter_hs_mode_lat, 31, 16) |
+		FLD_VAL(exit_hs_mode_lat, 15, 0);
+	dsi_write_reg(dsidev, DSI_VM_TIMING7, r);
+
+	DSSDBG("enter_hs_mode_lat %u, exit_hs_mode_lat %u\n",
+			enter_hs_mode_lat, exit_hs_mode_lat);
+
+	 if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		/* TODO: Implement a video mode check_timings function */
+		int hsa = dsi->vm_timings.hsa;
+		int hfp = dsi->vm_timings.hfp;
+		int hbp = dsi->vm_timings.hbp;
+		int vsa = dsi->vm_timings.vsa;
+		int vfp = dsi->vm_timings.vfp;
+		int vbp = dsi->vm_timings.vbp;
+		int window_sync = dsi->vm_timings.window_sync;
+		bool hsync_end;
+		struct omap_video_timings *timings = &dsi->timings;
+		int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+		int tl, t_he, width_bytes;
+
+		hsync_end = dsi->vm_timings.trans_mode == OMAP_DSS_DSI_PULSE_MODE;
+		t_he = hsync_end ?
+			((hsa == 0 && ndl == 3) ? 1 : DIV_ROUND_UP(4, ndl)) : 0;
+
+		width_bytes = DIV_ROUND_UP(timings->x_res * bpp, 8);
+
+		/* TL = t_HS + HSA + t_HE + HFP + ceil((WC + 6) / NDL) + HBP */
+		tl = DIV_ROUND_UP(4, ndl) + (hsync_end ? hsa : 0) + t_he + hfp +
+			DIV_ROUND_UP(width_bytes + 6, ndl) + hbp;
+
+		DSSDBG("HBP: %d, HFP: %d, HSA: %d, TL: %d TXBYTECLKHS\n", hbp,
+			hfp, hsync_end ? hsa : 0, tl);
+		DSSDBG("VBP: %d, VFP: %d, VSA: %d, VACT: %d lines\n", vbp, vfp,
+			vsa, timings->y_res);
+
+		r = dsi_read_reg(dsidev, DSI_VM_TIMING1);
+		r = FLD_MOD(r, hbp, 11, 0);	/* HBP */
+		r = FLD_MOD(r, hfp, 23, 12);	/* HFP */
+		r = FLD_MOD(r, hsync_end ? hsa : 0, 31, 24);	/* HSA */
+		dsi_write_reg(dsidev, DSI_VM_TIMING1, r);
+
+		r = dsi_read_reg(dsidev, DSI_VM_TIMING2);
+		r = FLD_MOD(r, vbp, 7, 0);	/* VBP */
+		r = FLD_MOD(r, vfp, 15, 8);	/* VFP */
+		r = FLD_MOD(r, vsa, 23, 16);	/* VSA */
+		r = FLD_MOD(r, window_sync, 27, 24);	/* WINDOW_SYNC */
+		dsi_write_reg(dsidev, DSI_VM_TIMING2, r);
+
+		r = dsi_read_reg(dsidev, DSI_VM_TIMING3);
+		r = FLD_MOD(r, timings->y_res, 14, 0);	/* VACT */
+		r = FLD_MOD(r, tl, 31, 16);		/* TL */
+		dsi_write_reg(dsidev, DSI_VM_TIMING3, r);
+	}
+}
+
+static int dsi_configure_pins(struct omap_dss_device *dssdev,
+		const struct omap_dsi_pin_config *pin_cfg)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int num_pins;
+	const int *pins;
+	struct dsi_lane_config lanes[DSI_MAX_NR_LANES];
+	int num_lanes;
+	int i;
+
+	static const enum dsi_lane_function functions[] = {
+		DSI_LANE_CLK,
+		DSI_LANE_DATA1,
+		DSI_LANE_DATA2,
+		DSI_LANE_DATA3,
+		DSI_LANE_DATA4,
+	};
+
+	num_pins = pin_cfg->num_pins;
+	pins = pin_cfg->pins;
+
+	if (num_pins < 4 || num_pins > dsi->num_lanes_supported * 2
+			|| num_pins % 2 != 0)
+		return -EINVAL;
+
+	for (i = 0; i < DSI_MAX_NR_LANES; ++i)
+		lanes[i].function = DSI_LANE_UNUSED;
+
+	num_lanes = 0;
+
+	for (i = 0; i < num_pins; i += 2) {
+		u8 lane, pol;
+		int dx, dy;
+
+		dx = pins[i];
+		dy = pins[i + 1];
+
+		if (dx < 0 || dx >= dsi->num_lanes_supported * 2)
+			return -EINVAL;
+
+		if (dy < 0 || dy >= dsi->num_lanes_supported * 2)
+			return -EINVAL;
+
+		if (dx & 1) {
+			if (dy != dx - 1)
+				return -EINVAL;
+			pol = 1;
+		} else {
+			if (dy != dx + 1)
+				return -EINVAL;
+			pol = 0;
+		}
+
+		lane = dx / 2;
+
+		lanes[lane].function = functions[i / 2];
+		lanes[lane].polarity = pol;
+		num_lanes++;
+	}
+
+	memcpy(dsi->lanes, lanes, sizeof(dsi->lanes));
+	dsi->num_lanes_used = num_lanes;
+
+	return 0;
+}
+
+static int dsi_enable_video_output(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dsi->output.manager;
+	int bpp = dsi_get_pixel_size(dsi->pix_fmt);
+	struct omap_dss_device *out = &dsi->output;
+	u8 data_type;
+	u16 word_count;
+	int r;
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		return -ENODEV;
+	}
+
+	r = dsi_display_init_dispc(dsidev, mgr);
+	if (r)
+		goto err_init_dispc;
+
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		switch (dsi->pix_fmt) {
+		case OMAP_DSS_DSI_FMT_RGB888:
+			data_type = MIPI_DSI_PACKED_PIXEL_STREAM_24;
+			break;
+		case OMAP_DSS_DSI_FMT_RGB666:
+			data_type = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
+			break;
+		case OMAP_DSS_DSI_FMT_RGB666_PACKED:
+			data_type = MIPI_DSI_PACKED_PIXEL_STREAM_18;
+			break;
+		case OMAP_DSS_DSI_FMT_RGB565:
+			data_type = MIPI_DSI_PACKED_PIXEL_STREAM_16;
+			break;
+		default:
+			r = -EINVAL;
+			goto err_pix_fmt;
+		}
+
+		dsi_if_enable(dsidev, false);
+		dsi_vc_enable(dsidev, channel, false);
+
+		/* MODE, 1 = video mode */
+		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 1, 4, 4);
+
+		word_count = DIV_ROUND_UP(dsi->timings.x_res * bpp, 8);
+
+		dsi_vc_write_long_header(dsidev, channel, data_type,
+				word_count, 0);
+
+		dsi_vc_enable(dsidev, channel, true);
+		dsi_if_enable(dsidev, true);
+	}
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		goto err_mgr_enable;
+
+	return 0;
+
+err_mgr_enable:
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		dsi_if_enable(dsidev, false);
+		dsi_vc_enable(dsidev, channel, false);
+	}
+err_pix_fmt:
+	dsi_display_uninit_dispc(dsidev, mgr);
+err_init_dispc:
+	return r;
+}
+
+static void dsi_disable_video_output(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dsi->output.manager;
+
+	if (dsi->mode == OMAP_DSS_DSI_VIDEO_MODE) {
+		dsi_if_enable(dsidev, false);
+		dsi_vc_enable(dsidev, channel, false);
+
+		/* MODE, 0 = command mode */
+		REG_FLD_MOD(dsidev, DSI_VC_CTRL(channel), 0, 4, 4);
+
+		dsi_vc_enable(dsidev, channel, true);
+		dsi_if_enable(dsidev, true);
+	}
+
+	dss_mgr_disable(mgr);
+
+	dsi_display_uninit_dispc(dsidev, mgr);
+}
+
+static void dsi_update_screen_dispc(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_overlay_manager *mgr = dsi->output.manager;
+	unsigned bytespp;
+	unsigned bytespl;
+	unsigned bytespf;
+	unsigned total_len;
+	unsigned packet_payload;
+	unsigned packet_len;
+	u32 l;
+	int r;
+	const unsigned channel = dsi->update_channel;
+	const unsigned line_buf_size = dsi->line_buffer_size;
+	u16 w = dsi->timings.x_res;
+	u16 h = dsi->timings.y_res;
+
+	DSSDBG("dsi_update_screen_dispc(%dx%d)\n", w, h);
+
+	dsi_vc_config_source(dsidev, channel, DSI_VC_SOURCE_VP);
+
+	bytespp	= dsi_get_pixel_size(dsi->pix_fmt) / 8;
+	bytespl = w * bytespp;
+	bytespf = bytespl * h;
+
+	/* NOTE: packet_payload has to be equal to N * bytespl, where N is
+	 * number of lines in a packet.  See errata about VP_CLK_RATIO */
+
+	if (bytespf < line_buf_size)
+		packet_payload = bytespf;
+	else
+		packet_payload = (line_buf_size) / bytespl * bytespl;
+
+	packet_len = packet_payload + 1;	/* 1 byte for DCS cmd */
+	total_len = (bytespf / packet_payload) * packet_len;
+
+	if (bytespf % packet_payload)
+		total_len += (bytespf % packet_payload) + 1;
+
+	l = FLD_VAL(total_len, 23, 0); /* TE_SIZE */
+	dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
+
+	dsi_vc_write_long_header(dsidev, channel, MIPI_DSI_DCS_LONG_WRITE,
+		packet_len, 0);
+
+	if (dsi->te_enabled)
+		l = FLD_MOD(l, 1, 30, 30); /* TE_EN */
+	else
+		l = FLD_MOD(l, 1, 31, 31); /* TE_START */
+	dsi_write_reg(dsidev, DSI_VC_TE(channel), l);
+
+	/* We put SIDLEMODE to no-idle for the duration of the transfer,
+	 * because DSS interrupts are not capable of waking up the CPU and the
+	 * framedone interrupt could be delayed for quite a long time. I think
+	 * the same goes for any DSS interrupts, but for some reason I have not
+	 * seen the problem anywhere else than here.
+	 */
+	dispc_disable_sidle();
+
+	dsi_perf_mark_start(dsidev);
+
+	r = schedule_delayed_work(&dsi->framedone_timeout_work,
+		msecs_to_jiffies(250));
+	BUG_ON(r == 0);
+
+	dss_mgr_set_timings(mgr, &dsi->timings);
+
+	dss_mgr_start_update(mgr);
+
+	if (dsi->te_enabled) {
+		/* disable LP_RX_TO, so that we can receive TE.  Time to wait
+		 * for TE is longer than the timer allows */
+		REG_FLD_MOD(dsidev, DSI_TIMING2, 0, 15, 15); /* LP_RX_TO */
+
+		dsi_vc_send_bta(dsidev, channel);
+
+#ifdef DSI_CATCH_MISSING_TE
+		mod_timer(&dsi->te_timer, jiffies + msecs_to_jiffies(250));
+#endif
+	}
+}
+
+#ifdef DSI_CATCH_MISSING_TE
+static void dsi_te_timeout(unsigned long arg)
+{
+	DSSERR("TE not received for 250ms!\n");
+}
+#endif
+
+static void dsi_handle_framedone(struct platform_device *dsidev, int error)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	/* SIDLEMODE back to smart-idle */
+	dispc_enable_sidle();
+
+	if (dsi->te_enabled) {
+		/* enable LP_RX_TO again after the TE */
+		REG_FLD_MOD(dsidev, DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
+	}
+
+	dsi->framedone_callback(error, dsi->framedone_data);
+
+	if (!error)
+		dsi_perf_show(dsidev, "DISPC");
+}
+
+static void dsi_framedone_timeout_work_callback(struct work_struct *work)
+{
+	struct dsi_data *dsi = container_of(work, struct dsi_data,
+			framedone_timeout_work.work);
+	/* XXX While extremely unlikely, we could get FRAMEDONE interrupt after
+	 * 250ms which would conflict with this timeout work. What should be
+	 * done is first cancel the transfer on the HW, and then cancel the
+	 * possibly scheduled framedone work. However, cancelling the transfer
+	 * on the HW is buggy, and would probably require resetting the whole
+	 * DSI */
+
+	DSSERR("Framedone not received for 250ms!\n");
+
+	dsi_handle_framedone(dsi->pdev, -ETIMEDOUT);
+}
+
+static void dsi_framedone_irq_callback(void *data)
+{
+	struct platform_device *dsidev = (struct platform_device *) data;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	/* Note: We get FRAMEDONE when DISPC has finished sending pixels and
+	 * turns itself off. However, DSI still has the pixels in its buffers,
+	 * and is sending the data.
+	 */
+
+	cancel_delayed_work(&dsi->framedone_timeout_work);
+
+	dsi_handle_framedone(dsidev, 0);
+}
+
+static int dsi_update(struct omap_dss_device *dssdev, int channel,
+		void (*callback)(int, void *), void *data)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	u16 dw, dh;
+
+	dsi_perf_mark_setup(dsidev);
+
+	dsi->update_channel = channel;
+
+	dsi->framedone_callback = callback;
+	dsi->framedone_data = data;
+
+	dw = dsi->timings.x_res;
+	dh = dsi->timings.y_res;
+
+#ifdef DSI_PERF_MEASURE
+	dsi->update_bytes = dw * dh *
+		dsi_get_pixel_size(dsi->pix_fmt) / 8;
+#endif
+	dsi_update_screen_dispc(dsidev);
+
+	return 0;
+}
+
+/* Display funcs */
+
+static int dsi_configure_dispc_clocks(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dispc_clock_info dispc_cinfo;
+	int r;
+	unsigned long fck;
+
+	fck = dsi_get_pll_hsdiv_dispc_rate(dsidev);
+
+	dispc_cinfo.lck_div = dsi->user_dispc_cinfo.lck_div;
+	dispc_cinfo.pck_div = dsi->user_dispc_cinfo.pck_div;
+
+	r = dispc_calc_clock_rates(fck, &dispc_cinfo);
+	if (r) {
+		DSSERR("Failed to calc dispc clocks\n");
+		return r;
+	}
+
+	dsi->mgr_config.clock_info = dispc_cinfo;
+
+	return 0;
+}
+
+static int dsi_display_init_dispc(struct platform_device *dsidev,
+		struct omap_overlay_manager *mgr)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+
+	dss_select_lcd_clk_source(mgr->id, dsi->module_id == 0 ?
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC :
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC);
+
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE) {
+		r = dss_mgr_register_framedone_handler(mgr,
+				dsi_framedone_irq_callback, dsidev);
+		if (r) {
+			DSSERR("can't register FRAMEDONE handler\n");
+			goto err;
+		}
+
+		dsi->mgr_config.stallmode = true;
+		dsi->mgr_config.fifohandcheck = true;
+	} else {
+		dsi->mgr_config.stallmode = false;
+		dsi->mgr_config.fifohandcheck = false;
+	}
+
+	/*
+	 * override interlace, logic level and edge related parameters in
+	 * omap_video_timings with default values
+	 */
+	dsi->timings.interlace = false;
+	dsi->timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	dsi->timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	dsi->timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+	dss_mgr_set_timings(mgr, &dsi->timings);
+
+	r = dsi_configure_dispc_clocks(dsidev);
+	if (r)
+		goto err1;
+
+	dsi->mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+	dsi->mgr_config.video_port_width =
+			dsi_get_pixel_size(dsi->pix_fmt);
+	dsi->mgr_config.lcden_sig_polarity = 0;
+
+	dss_mgr_set_lcd_config(mgr, &dsi->mgr_config);
+
+	return 0;
+err1:
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
+		dss_mgr_unregister_framedone_handler(mgr,
+				dsi_framedone_irq_callback, dsidev);
+err:
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+	return r;
+}
+
+static void dsi_display_uninit_dispc(struct platform_device *dsidev,
+		struct omap_overlay_manager *mgr)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (dsi->mode == OMAP_DSS_DSI_CMD_MODE)
+		dss_mgr_unregister_framedone_handler(mgr,
+				dsi_framedone_irq_callback, dsidev);
+
+	dss_select_lcd_clk_source(mgr->id, OMAP_DSS_CLK_SRC_FCK);
+}
+
+static int dsi_configure_dsi_clocks(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dss_pll_clock_info cinfo;
+	int r;
+
+	cinfo = dsi->user_dsi_cinfo;
+
+	r = dss_pll_set_config(&dsi->pll, &cinfo);
+	if (r) {
+		DSSERR("Failed to set dsi clocks\n");
+		return r;
+	}
+
+	return 0;
+}
+
+static int dsi_display_init_dsi(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r;
+
+	r = dss_pll_enable(&dsi->pll);
+	if (r)
+		goto err0;
+
+	r = dsi_configure_dsi_clocks(dsidev);
+	if (r)
+		goto err1;
+
+	dss_select_dsi_clk_source(dsi->module_id, dsi->module_id == 0 ?
+			OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI :
+			OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI);
+
+	DSSDBG("PLL OK\n");
+
+	r = dsi_cio_init(dsidev);
+	if (r)
+		goto err2;
+
+	_dsi_print_reset_status(dsidev);
+
+	dsi_proto_timings(dsidev);
+	dsi_set_lp_clk_divisor(dsidev);
+
+	if (1)
+		_dsi_print_reset_status(dsidev);
+
+	r = dsi_proto_config(dsidev);
+	if (r)
+		goto err3;
+
+	/* enable interface */
+	dsi_vc_enable(dsidev, 0, 1);
+	dsi_vc_enable(dsidev, 1, 1);
+	dsi_vc_enable(dsidev, 2, 1);
+	dsi_vc_enable(dsidev, 3, 1);
+	dsi_if_enable(dsidev, 1);
+	dsi_force_tx_stop_mode_io(dsidev);
+
+	return 0;
+err3:
+	dsi_cio_uninit(dsidev);
+err2:
+	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
+err1:
+	dss_pll_disable(&dsi->pll);
+err0:
+	return r;
+}
+
+static void dsi_display_uninit_dsi(struct platform_device *dsidev,
+		bool disconnect_lanes, bool enter_ulps)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (enter_ulps && !dsi->ulps_enabled)
+		dsi_enter_ulps(dsidev);
+
+	/* disable interface */
+	dsi_if_enable(dsidev, 0);
+	dsi_vc_enable(dsidev, 0, 0);
+	dsi_vc_enable(dsidev, 1, 0);
+	dsi_vc_enable(dsidev, 2, 0);
+	dsi_vc_enable(dsidev, 3, 0);
+
+	dss_select_dsi_clk_source(dsi->module_id, OMAP_DSS_CLK_SRC_FCK);
+	dsi_cio_uninit(dsidev);
+	dsi_pll_uninit(dsidev, disconnect_lanes);
+}
+
+static int dsi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int r = 0;
+
+	DSSDBG("dsi_display_enable\n");
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	mutex_lock(&dsi->lock);
+
+	r = dsi_runtime_get(dsidev);
+	if (r)
+		goto err_get_dsi;
+
+	_dsi_initialize_irq(dsidev);
+
+	r = dsi_display_init_dsi(dsidev);
+	if (r)
+		goto err_init_dsi;
+
+	mutex_unlock(&dsi->lock);
+
+	return 0;
+
+err_init_dsi:
+	dsi_runtime_put(dsidev);
+err_get_dsi:
+	mutex_unlock(&dsi->lock);
+	DSSDBG("dsi_display_enable FAILED\n");
+	return r;
+}
+
+static void dsi_display_disable(struct omap_dss_device *dssdev,
+		bool disconnect_lanes, bool enter_ulps)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	DSSDBG("dsi_display_disable\n");
+
+	WARN_ON(!dsi_bus_is_locked(dsidev));
+
+	mutex_lock(&dsi->lock);
+
+	dsi_sync_vc(dsidev, 0);
+	dsi_sync_vc(dsidev, 1);
+	dsi_sync_vc(dsidev, 2);
+	dsi_sync_vc(dsidev, 3);
+
+	dsi_display_uninit_dsi(dsidev, disconnect_lanes, enter_ulps);
+
+	dsi_runtime_put(dsidev);
+
+	mutex_unlock(&dsi->lock);
+}
+
+static int dsi_enable_te(struct omap_dss_device *dssdev, bool enable)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	dsi->te_enabled = enable;
+	return 0;
+}
+
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+static void print_dsi_vm(const char *str,
+		const struct omap_dss_dsi_videomode_timings *t)
+{
+	unsigned long byteclk = t->hsclk / 4;
+	int bl, wc, pps, tot;
+
+	wc = DIV_ROUND_UP(t->hact * t->bitspp, 8);
+	pps = DIV_ROUND_UP(wc + 6, t->ndl); /* pixel packet size */
+	bl = t->hss + t->hsa + t->hse + t->hbp + t->hfp;
+	tot = bl + pps;
+
+#define TO_DSI_T(x) ((u32)div64_u64((u64)x * 1000000000llu, byteclk))
+
+	pr_debug("%s bck %lu, %u/%u/%u/%u/%u/%u = %u+%u = %u, "
+			"%u/%u/%u/%u/%u/%u = %u + %u = %u\n",
+			str,
+			byteclk,
+			t->hss, t->hsa, t->hse, t->hbp, pps, t->hfp,
+			bl, pps, tot,
+			TO_DSI_T(t->hss),
+			TO_DSI_T(t->hsa),
+			TO_DSI_T(t->hse),
+			TO_DSI_T(t->hbp),
+			TO_DSI_T(pps),
+			TO_DSI_T(t->hfp),
+
+			TO_DSI_T(bl),
+			TO_DSI_T(pps),
+
+			TO_DSI_T(tot));
+#undef TO_DSI_T
+}
+
+static void print_dispc_vm(const char *str, const struct omap_video_timings *t)
+{
+	unsigned long pck = t->pixelclock;
+	int hact, bl, tot;
+
+	hact = t->x_res;
+	bl = t->hsw + t->hbp + t->hfp;
+	tot = hact + bl;
+
+#define TO_DISPC_T(x) ((u32)div64_u64((u64)x * 1000000000llu, pck))
+
+	pr_debug("%s pck %lu, %u/%u/%u/%u = %u+%u = %u, "
+			"%u/%u/%u/%u = %u + %u = %u\n",
+			str,
+			pck,
+			t->hsw, t->hbp, hact, t->hfp,
+			bl, hact, tot,
+			TO_DISPC_T(t->hsw),
+			TO_DISPC_T(t->hbp),
+			TO_DISPC_T(hact),
+			TO_DISPC_T(t->hfp),
+			TO_DISPC_T(bl),
+			TO_DISPC_T(hact),
+			TO_DISPC_T(tot));
+#undef TO_DISPC_T
+}
+
+/* note: this is not quite accurate */
+static void print_dsi_dispc_vm(const char *str,
+		const struct omap_dss_dsi_videomode_timings *t)
+{
+	struct omap_video_timings vm = { 0 };
+	unsigned long byteclk = t->hsclk / 4;
+	unsigned long pck;
+	u64 dsi_tput;
+	int dsi_hact, dsi_htot;
+
+	dsi_tput = (u64)byteclk * t->ndl * 8;
+	pck = (u32)div64_u64(dsi_tput, t->bitspp);
+	dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(t->hact * t->bitspp, 8) + 6, t->ndl);
+	dsi_htot = t->hss + t->hsa + t->hse + t->hbp + dsi_hact + t->hfp;
+
+	vm.pixelclock = pck;
+	vm.hsw = div64_u64((u64)(t->hsa + t->hse) * pck, byteclk);
+	vm.hbp = div64_u64((u64)t->hbp * pck, byteclk);
+	vm.hfp = div64_u64((u64)t->hfp * pck, byteclk);
+	vm.x_res = t->hact;
+
+	print_dispc_vm(str, &vm);
+}
+#endif /* PRINT_VERBOSE_VM_TIMINGS */
+
+static bool dsi_cm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+	struct omap_video_timings *t = &ctx->dispc_vm;
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	*t = *ctx->config->timings;
+	t->pixelclock = pck;
+	t->x_res = ctx->config->timings->x_res;
+	t->y_res = ctx->config->timings->y_res;
+	t->hsw = t->hfp = t->hbp = t->vsw = 1;
+	t->vfp = t->vbp = 0;
+
+	return true;
+}
+
+static bool dsi_cm_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+		void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+	ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+	return dispc_div_calc(dispc, ctx->req_pck_min, ctx->req_pck_max,
+			dsi_cm_calc_dispc_cb, ctx);
+}
+
+static bool dsi_cm_calc_pll_cb(int n, int m, unsigned long fint,
+		unsigned long clkdco, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.n = n;
+	ctx->dsi_cinfo.m = m;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkdco = clkdco;
+
+	return dss_pll_hsdiv_calc(ctx->pll, clkdco, ctx->req_pck_min,
+			dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+			dsi_cm_calc_hsdiv_cb, ctx);
+}
+
+static bool dsi_cm_calc(struct dsi_data *dsi,
+		const struct omap_dss_dsi_config *cfg,
+		struct dsi_clk_calc_ctx *ctx)
+{
+	unsigned long clkin;
+	int bitspp, ndl;
+	unsigned long pll_min, pll_max;
+	unsigned long pck, txbyteclk;
+
+	clkin = clk_get_rate(dsi->pll.clkin);
+	bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	ndl = dsi->num_lanes_used - 1;
+
+	/*
+	 * Here we should calculate minimum txbyteclk to be able to send the
+	 * frame in time, and also to handle TE. That's not very simple, though,
+	 * especially as we go to LP between each pixel packet due to HW
+	 * "feature". So let's just estimate very roughly and multiply by 1.5.
+	 */
+	pck = cfg->timings->pixelclock;
+	pck = pck * 3 / 2;
+	txbyteclk = pck * bitspp / 8 / ndl;
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->dsidev = dsi->pdev;
+	ctx->pll = &dsi->pll;
+	ctx->config = cfg;
+	ctx->req_pck_min = pck;
+	ctx->req_pck_nom = pck;
+	ctx->req_pck_max = pck * 3 / 2;
+
+	pll_min = max(cfg->hs_clk_min * 4, txbyteclk * 4 * 4);
+	pll_max = cfg->hs_clk_max * 4;
+
+	return dss_pll_calc(ctx->pll, clkin,
+			pll_min, pll_max,
+			dsi_cm_calc_pll_cb, ctx);
+}
+
+static bool dsi_vm_calc_blanking(struct dsi_clk_calc_ctx *ctx)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(ctx->dsidev);
+	const struct omap_dss_dsi_config *cfg = ctx->config;
+	int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	int ndl = dsi->num_lanes_used - 1;
+	unsigned long hsclk = ctx->dsi_cinfo.clkdco / 4;
+	unsigned long byteclk = hsclk / 4;
+
+	unsigned long dispc_pck, req_pck_min, req_pck_nom, req_pck_max;
+	int xres;
+	int panel_htot, panel_hbl; /* pixels */
+	int dispc_htot, dispc_hbl; /* pixels */
+	int dsi_htot, dsi_hact, dsi_hbl, hss, hse; /* byteclks */
+	int hfp, hsa, hbp;
+	const struct omap_video_timings *req_vm;
+	struct omap_video_timings *dispc_vm;
+	struct omap_dss_dsi_videomode_timings *dsi_vm;
+	u64 dsi_tput, dispc_tput;
+
+	dsi_tput = (u64)byteclk * ndl * 8;
+
+	req_vm = cfg->timings;
+	req_pck_min = ctx->req_pck_min;
+	req_pck_max = ctx->req_pck_max;
+	req_pck_nom = ctx->req_pck_nom;
+
+	dispc_pck = ctx->dispc_cinfo.pck;
+	dispc_tput = (u64)dispc_pck * bitspp;
+
+	xres = req_vm->x_res;
+
+	panel_hbl = req_vm->hfp + req_vm->hbp + req_vm->hsw;
+	panel_htot = xres + panel_hbl;
+
+	dsi_hact = DIV_ROUND_UP(DIV_ROUND_UP(xres * bitspp, 8) + 6, ndl);
+
+	/*
+	 * When there are no line buffers, DISPC and DSI must have the
+	 * same tput. Otherwise DISPC tput needs to be higher than DSI's.
+	 */
+	if (dsi->line_buffer_size < xres * bitspp / 8) {
+		if (dispc_tput != dsi_tput)
+			return false;
+	} else {
+		if (dispc_tput < dsi_tput)
+			return false;
+	}
+
+	/* DSI tput must be over the min requirement */
+	if (dsi_tput < (u64)bitspp * req_pck_min)
+		return false;
+
+	/* When non-burst mode, DSI tput must be below max requirement. */
+	if (cfg->trans_mode != OMAP_DSS_DSI_BURST_MODE) {
+		if (dsi_tput > (u64)bitspp * req_pck_max)
+			return false;
+	}
+
+	hss = DIV_ROUND_UP(4, ndl);
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+		if (ndl == 3 && req_vm->hsw == 0)
+			hse = 1;
+		else
+			hse = DIV_ROUND_UP(4, ndl);
+	} else {
+		hse = 0;
+	}
+
+	/* DSI htot to match the panel's nominal pck */
+	dsi_htot = div64_u64((u64)panel_htot * byteclk, req_pck_nom);
+
+	/* fail if there would be no time for blanking */
+	if (dsi_htot < hss + hse + dsi_hact)
+		return false;
+
+	/* total DSI blanking needed to achieve panel's TL */
+	dsi_hbl = dsi_htot - dsi_hact;
+
+	/* DISPC htot to match the DSI TL */
+	dispc_htot = div64_u64((u64)dsi_htot * dispc_pck, byteclk);
+
+	/* verify that the DSI and DISPC TLs are the same */
+	if ((u64)dsi_htot * dispc_pck != (u64)dispc_htot * byteclk)
+		return false;
+
+	dispc_hbl = dispc_htot - xres;
+
+	/* setup DSI videomode */
+
+	dsi_vm = &ctx->dsi_vm;
+	memset(dsi_vm, 0, sizeof(*dsi_vm));
+
+	dsi_vm->hsclk = hsclk;
+
+	dsi_vm->ndl = ndl;
+	dsi_vm->bitspp = bitspp;
+
+	if (cfg->trans_mode != OMAP_DSS_DSI_PULSE_MODE) {
+		hsa = 0;
+	} else if (ndl == 3 && req_vm->hsw == 0) {
+		hsa = 0;
+	} else {
+		hsa = div64_u64((u64)req_vm->hsw * byteclk, req_pck_nom);
+		hsa = max(hsa - hse, 1);
+	}
+
+	hbp = div64_u64((u64)req_vm->hbp * byteclk, req_pck_nom);
+	hbp = max(hbp, 1);
+
+	hfp = dsi_hbl - (hss + hsa + hse + hbp);
+	if (hfp < 1) {
+		int t;
+		/* we need to take cycles from hbp */
+
+		t = 1 - hfp;
+		hbp = max(hbp - t, 1);
+		hfp = dsi_hbl - (hss + hsa + hse + hbp);
+
+		if (hfp < 1 && hsa > 0) {
+			/* we need to take cycles from hsa */
+			t = 1 - hfp;
+			hsa = max(hsa - t, 1);
+			hfp = dsi_hbl - (hss + hsa + hse + hbp);
+		}
+	}
+
+	if (hfp < 1)
+		return false;
+
+	dsi_vm->hss = hss;
+	dsi_vm->hsa = hsa;
+	dsi_vm->hse = hse;
+	dsi_vm->hbp = hbp;
+	dsi_vm->hact = xres;
+	dsi_vm->hfp = hfp;
+
+	dsi_vm->vsa = req_vm->vsw;
+	dsi_vm->vbp = req_vm->vbp;
+	dsi_vm->vact = req_vm->y_res;
+	dsi_vm->vfp = req_vm->vfp;
+
+	dsi_vm->trans_mode = cfg->trans_mode;
+
+	dsi_vm->blanking_mode = 0;
+	dsi_vm->hsa_blanking_mode = 1;
+	dsi_vm->hfp_blanking_mode = 1;
+	dsi_vm->hbp_blanking_mode = 1;
+
+	dsi_vm->ddr_clk_always_on = cfg->ddr_clk_always_on;
+	dsi_vm->window_sync = 4;
+
+	/* setup DISPC videomode */
+
+	dispc_vm = &ctx->dispc_vm;
+	*dispc_vm = *req_vm;
+	dispc_vm->pixelclock = dispc_pck;
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_PULSE_MODE) {
+		hsa = div64_u64((u64)req_vm->hsw * dispc_pck,
+				req_pck_nom);
+		hsa = max(hsa, 1);
+	} else {
+		hsa = 1;
+	}
+
+	hbp = div64_u64((u64)req_vm->hbp * dispc_pck, req_pck_nom);
+	hbp = max(hbp, 1);
+
+	hfp = dispc_hbl - hsa - hbp;
+	if (hfp < 1) {
+		int t;
+		/* we need to take cycles from hbp */
+
+		t = 1 - hfp;
+		hbp = max(hbp - t, 1);
+		hfp = dispc_hbl - hsa - hbp;
+
+		if (hfp < 1) {
+			/* we need to take cycles from hsa */
+			t = 1 - hfp;
+			hsa = max(hsa - t, 1);
+			hfp = dispc_hbl - hsa - hbp;
+		}
+	}
+
+	if (hfp < 1)
+		return false;
+
+	dispc_vm->hfp = hfp;
+	dispc_vm->hsw = hsa;
+	dispc_vm->hbp = hbp;
+
+	return true;
+}
+
+
+static bool dsi_vm_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	if (dsi_vm_calc_blanking(ctx) == false)
+		return false;
+
+#ifdef PRINT_VERBOSE_VM_TIMINGS
+	print_dispc_vm("dispc", &ctx->dispc_vm);
+	print_dsi_vm("dsi  ", &ctx->dsi_vm);
+	print_dispc_vm("req  ", ctx->config->timings);
+	print_dsi_dispc_vm("act  ", &ctx->dsi_vm);
+#endif
+
+	return true;
+}
+
+static bool dsi_vm_calc_hsdiv_cb(int m_dispc, unsigned long dispc,
+		void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+	unsigned long pck_max;
+
+	ctx->dsi_cinfo.mX[HSDIV_DISPC] = m_dispc;
+	ctx->dsi_cinfo.clkout[HSDIV_DISPC] = dispc;
+
+	/*
+	 * In burst mode we can let the dispc pck be arbitrarily high, but it
+	 * limits our scaling abilities. So for now, don't aim too high.
+	 */
+
+	if (ctx->config->trans_mode == OMAP_DSS_DSI_BURST_MODE)
+		pck_max = ctx->req_pck_max + 10000000;
+	else
+		pck_max = ctx->req_pck_max;
+
+	return dispc_div_calc(dispc, ctx->req_pck_min, pck_max,
+			dsi_vm_calc_dispc_cb, ctx);
+}
+
+static bool dsi_vm_calc_pll_cb(int n, int m, unsigned long fint,
+		unsigned long clkdco, void *data)
+{
+	struct dsi_clk_calc_ctx *ctx = data;
+
+	ctx->dsi_cinfo.n = n;
+	ctx->dsi_cinfo.m = m;
+	ctx->dsi_cinfo.fint = fint;
+	ctx->dsi_cinfo.clkdco = clkdco;
+
+	return dss_pll_hsdiv_calc(ctx->pll, clkdco, ctx->req_pck_min,
+			dss_feat_get_param_max(FEAT_PARAM_DSS_FCK),
+			dsi_vm_calc_hsdiv_cb, ctx);
+}
+
+static bool dsi_vm_calc(struct dsi_data *dsi,
+		const struct omap_dss_dsi_config *cfg,
+		struct dsi_clk_calc_ctx *ctx)
+{
+	const struct omap_video_timings *t = cfg->timings;
+	unsigned long clkin;
+	unsigned long pll_min;
+	unsigned long pll_max;
+	int ndl = dsi->num_lanes_used - 1;
+	int bitspp = dsi_get_pixel_size(cfg->pixel_format);
+	unsigned long byteclk_min;
+
+	clkin = clk_get_rate(dsi->pll.clkin);
+
+	memset(ctx, 0, sizeof(*ctx));
+	ctx->dsidev = dsi->pdev;
+	ctx->pll = &dsi->pll;
+	ctx->config = cfg;
+
+	/* these limits should come from the panel driver */
+	ctx->req_pck_min = t->pixelclock - 1000;
+	ctx->req_pck_nom = t->pixelclock;
+	ctx->req_pck_max = t->pixelclock + 1000;
+
+	byteclk_min = div64_u64((u64)ctx->req_pck_min * bitspp, ndl * 8);
+	pll_min = max(cfg->hs_clk_min * 4, byteclk_min * 4 * 4);
+
+	if (cfg->trans_mode == OMAP_DSS_DSI_BURST_MODE) {
+		pll_max = cfg->hs_clk_max * 4;
+	} else {
+		unsigned long byteclk_max;
+		byteclk_max = div64_u64((u64)ctx->req_pck_max * bitspp,
+				ndl * 8);
+
+		pll_max = byteclk_max * 4 * 4;
+	}
+
+	return dss_pll_calc(ctx->pll, clkin,
+			pll_min, pll_max,
+			dsi_vm_calc_pll_cb, ctx);
+}
+
+static int dsi_set_config(struct omap_dss_device *dssdev,
+		const struct omap_dss_dsi_config *config)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dsi_clk_calc_ctx ctx;
+	bool ok;
+	int r;
+
+	mutex_lock(&dsi->lock);
+
+	dsi->pix_fmt = config->pixel_format;
+	dsi->mode = config->mode;
+
+	if (config->mode == OMAP_DSS_DSI_VIDEO_MODE)
+		ok = dsi_vm_calc(dsi, config, &ctx);
+	else
+		ok = dsi_cm_calc(dsi, config, &ctx);
+
+	if (!ok) {
+		DSSERR("failed to find suitable DSI clock settings\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	dsi_pll_calc_dsi_fck(&ctx.dsi_cinfo);
+
+	r = dsi_lp_clock_calc(ctx.dsi_cinfo.clkout[HSDIV_DSI],
+		config->lp_clk_min, config->lp_clk_max, &dsi->user_lp_cinfo);
+	if (r) {
+		DSSERR("failed to find suitable DSI LP clock settings\n");
+		goto err;
+	}
+
+	dsi->user_dsi_cinfo = ctx.dsi_cinfo;
+	dsi->user_dispc_cinfo = ctx.dispc_cinfo;
+
+	dsi->timings = ctx.dispc_vm;
+	dsi->vm_timings = ctx.dsi_vm;
+
+	mutex_unlock(&dsi->lock);
+
+	return 0;
+err:
+	mutex_unlock(&dsi->lock);
+
+	return r;
+}
+
+/*
+ * Return a hardcoded channel for the DSI output. This should work for
+ * current use cases, but this can be later expanded to either resolve
+ * the channel in some more dynamic manner, or get the channel as a user
+ * parameter.
+ */
+static enum omap_channel dsi_get_channel(int module_id)
+{
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+	case OMAPDSS_VER_AM43xx:
+		DSSWARN("DSI not supported\n");
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+		return OMAP_DSS_CHANNEL_LCD;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		switch (module_id) {
+		case 0:
+			return OMAP_DSS_CHANNEL_LCD;
+		case 1:
+			return OMAP_DSS_CHANNEL_LCD2;
+		default:
+			DSSWARN("unsupported module id\n");
+			return OMAP_DSS_CHANNEL_LCD;
+		}
+
+	case OMAPDSS_VER_OMAP5:
+		switch (module_id) {
+		case 0:
+			return OMAP_DSS_CHANNEL_LCD;
+		case 1:
+			return OMAP_DSS_CHANNEL_LCD3;
+		default:
+			DSSWARN("unsupported module id\n");
+			return OMAP_DSS_CHANNEL_LCD;
+		}
+
+	default:
+		DSSWARN("unsupported DSS version\n");
+		return OMAP_DSS_CHANNEL_LCD;
+	}
+}
+
+static int dsi_request_vc(struct omap_dss_device *dssdev, int *channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+		if (!dsi->vc[i].dssdev) {
+			dsi->vc[i].dssdev = dssdev;
+			*channel = i;
+			return 0;
+		}
+	}
+
+	DSSERR("cannot get VC for display %s", dssdev->name);
+	return -ENOSPC;
+}
+
+static int dsi_set_vc_id(struct omap_dss_device *dssdev, int channel, int vc_id)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if (vc_id < 0 || vc_id > 3) {
+		DSSERR("VC ID out of range\n");
+		return -EINVAL;
+	}
+
+	if (channel < 0 || channel > 3) {
+		DSSERR("Virtual Channel out of range\n");
+		return -EINVAL;
+	}
+
+	if (dsi->vc[channel].dssdev != dssdev) {
+		DSSERR("Virtual Channel not allocated to display %s\n",
+			dssdev->name);
+		return -EINVAL;
+	}
+
+	dsi->vc[channel].vc_id = vc_id;
+
+	return 0;
+}
+
+static void dsi_release_vc(struct omap_dss_device *dssdev, int channel)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	if ((channel >= 0 && channel <= 3) &&
+		dsi->vc[channel].dssdev == dssdev) {
+		dsi->vc[channel].dssdev = NULL;
+		dsi->vc[channel].vc_id = 0;
+	}
+}
+
+
+static int dsi_get_clocks(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct clk *clk;
+
+	clk = devm_clk_get(&dsidev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get fck\n");
+		return PTR_ERR(clk);
+	}
+
+	dsi->dss_clk = clk;
+
+	return 0;
+}
+
+static int dsi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct platform_device *dsidev = dsi_get_dsidev_from_dssdev(dssdev);
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = dsi_regulator_init(dsidev);
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dssdev->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void dsi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_dsi_ops dsi_ops = {
+	.connect = dsi_connect,
+	.disconnect = dsi_disconnect,
+
+	.bus_lock = dsi_bus_lock,
+	.bus_unlock = dsi_bus_unlock,
+
+	.enable = dsi_display_enable,
+	.disable = dsi_display_disable,
+
+	.enable_hs = dsi_vc_enable_hs,
+
+	.configure_pins = dsi_configure_pins,
+	.set_config = dsi_set_config,
+
+	.enable_video_output = dsi_enable_video_output,
+	.disable_video_output = dsi_disable_video_output,
+
+	.update = dsi_update,
+
+	.enable_te = dsi_enable_te,
+
+	.request_vc = dsi_request_vc,
+	.set_vc_id = dsi_set_vc_id,
+	.release_vc = dsi_release_vc,
+
+	.dcs_write = dsi_vc_dcs_write,
+	.dcs_write_nosync = dsi_vc_dcs_write_nosync,
+	.dcs_read = dsi_vc_dcs_read,
+
+	.gen_write = dsi_vc_generic_write,
+	.gen_write_nosync = dsi_vc_generic_write_nosync,
+	.gen_read = dsi_vc_generic_read,
+
+	.bta_sync = dsi_vc_send_bta_sync,
+
+	.set_max_rx_packet_size = dsi_vc_set_max_rx_packet_size,
+};
+
+static void dsi_init_output(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_dss_device *out = &dsi->output;
+
+	out->dev = &dsidev->dev;
+	out->id = dsi->module_id == 0 ?
+			OMAP_DSS_OUTPUT_DSI1 : OMAP_DSS_OUTPUT_DSI2;
+
+	out->output_type = OMAP_DISPLAY_TYPE_DSI;
+	out->name = dsi->module_id == 0 ? "dsi.0" : "dsi.1";
+	out->dispc_channel = dsi_get_channel(dsi->module_id);
+	out->ops.dsi = &dsi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void dsi_uninit_output(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct omap_dss_device *out = &dsi->output;
+
+	omapdss_unregister_output(out);
+}
+
+static int dsi_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+	struct property *prop;
+	u32 lane_arr[10];
+	int len, num_pins;
+	int r, i;
+	struct device_node *ep;
+	struct omap_dsi_pin_config pin_cfg;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	prop = of_find_property(ep, "lanes", &len);
+	if (prop == NULL) {
+		dev_err(&pdev->dev, "failed to find lane data\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	num_pins = len / sizeof(u32);
+
+	if (num_pins < 4 || num_pins % 2 != 0 ||
+		num_pins > dsi->num_lanes_supported * 2) {
+		dev_err(&pdev->dev, "bad number of lanes\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	r = of_property_read_u32_array(ep, "lanes", lane_arr, num_pins);
+	if (r) {
+		dev_err(&pdev->dev, "failed to read lane data\n");
+		goto err;
+	}
+
+	pin_cfg.num_pins = num_pins;
+	for (i = 0; i < num_pins; ++i)
+		pin_cfg.pins[i] = (int)lane_arr[i];
+
+	r = dsi_configure_pins(&dsi->output, &pin_cfg);
+	if (r) {
+		dev_err(&pdev->dev, "failed to configure pins");
+		goto err;
+	}
+
+	of_node_put(ep);
+
+	return 0;
+
+err:
+	of_node_put(ep);
+	return r;
+}
+
+static const struct dss_pll_ops dsi_pll_ops = {
+	.enable = dsi_pll_enable,
+	.disable = dsi_pll_disable,
+	.set_config = dss_pll_write_config_type_a,
+};
+
+static const struct dss_pll_hw dss_omap3_dsi_pll_hw = {
+	.n_max = (1 << 7) - 1,
+	.m_max = (1 << 11) - 1,
+	.mX_max = (1 << 4) - 1,
+	.fint_min = 750000,
+	.fint_max = 2100000,
+	.clkdco_low = 1000000000,
+	.clkdco_max = 1800000000,
+
+	.n_msb = 7,
+	.n_lsb = 1,
+	.m_msb = 18,
+	.m_lsb = 8,
+
+	.mX_msb[0] = 22,
+	.mX_lsb[0] = 19,
+	.mX_msb[1] = 26,
+	.mX_lsb[1] = 23,
+
+	.has_stopmode = true,
+	.has_freqsel = true,
+	.has_selfreqdco = false,
+	.has_refsel = false,
+};
+
+static const struct dss_pll_hw dss_omap4_dsi_pll_hw = {
+	.n_max = (1 << 8) - 1,
+	.m_max = (1 << 12) - 1,
+	.mX_max = (1 << 5) - 1,
+	.fint_min = 500000,
+	.fint_max = 2500000,
+	.clkdco_low = 1000000000,
+	.clkdco_max = 1800000000,
+
+	.n_msb = 8,
+	.n_lsb = 1,
+	.m_msb = 20,
+	.m_lsb = 9,
+
+	.mX_msb[0] = 25,
+	.mX_lsb[0] = 21,
+	.mX_msb[1] = 30,
+	.mX_lsb[1] = 26,
+
+	.has_stopmode = true,
+	.has_freqsel = false,
+	.has_selfreqdco = false,
+	.has_refsel = false,
+};
+
+static const struct dss_pll_hw dss_omap5_dsi_pll_hw = {
+	.n_max = (1 << 8) - 1,
+	.m_max = (1 << 12) - 1,
+	.mX_max = (1 << 5) - 1,
+	.fint_min = 150000,
+	.fint_max = 52000000,
+	.clkdco_low = 1000000000,
+	.clkdco_max = 1800000000,
+
+	.n_msb = 8,
+	.n_lsb = 1,
+	.m_msb = 20,
+	.m_lsb = 9,
+
+	.mX_msb[0] = 25,
+	.mX_lsb[0] = 21,
+	.mX_msb[1] = 30,
+	.mX_lsb[1] = 26,
+
+	.has_stopmode = true,
+	.has_freqsel = false,
+	.has_selfreqdco = true,
+	.has_refsel = true,
+};
+
+static int dsi_init_pll_data(struct platform_device *dsidev)
+{
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+	struct dss_pll *pll = &dsi->pll;
+	struct clk *clk;
+	int r;
+
+	clk = devm_clk_get(&dsidev->dev, "sys_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get sys_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	pll->name = dsi->module_id == 0 ? "dsi0" : "dsi1";
+	pll->id = dsi->module_id == 0 ? DSS_PLL_DSI1 : DSS_PLL_DSI2;
+	pll->clkin = clk;
+	pll->base = dsi->pll_base;
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_OMAP3630:
+	case OMAPDSS_VER_AM35xx:
+		pll->hw = &dss_omap3_dsi_pll_hw;
+		break;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		pll->hw = &dss_omap4_dsi_pll_hw;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+		pll->hw = &dss_omap5_dsi_pll_hw;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	pll->ops = &dsi_pll_ops;
+
+	r = dss_pll_register(pll);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+/* DSI1 HW IP initialisation */
+static int dsi_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *dsidev = to_platform_device(dev);
+	u32 rev;
+	int r, i;
+	struct dsi_data *dsi;
+	struct resource *dsi_mem;
+	struct resource *res;
+	struct resource temp_res;
+
+	dsi = devm_kzalloc(&dsidev->dev, sizeof(*dsi), GFP_KERNEL);
+	if (!dsi)
+		return -ENOMEM;
+
+	dsi->pdev = dsidev;
+	dev_set_drvdata(&dsidev->dev, dsi);
+
+	spin_lock_init(&dsi->irq_lock);
+	spin_lock_init(&dsi->errors_lock);
+	dsi->errors = 0;
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	spin_lock_init(&dsi->irq_stats_lock);
+	dsi->irq_stats.last_reset = jiffies;
+#endif
+
+	mutex_init(&dsi->lock);
+	sema_init(&dsi->bus_lock, 1);
+
+	INIT_DEFERRABLE_WORK(&dsi->framedone_timeout_work,
+			     dsi_framedone_timeout_work_callback);
+
+#ifdef DSI_CATCH_MISSING_TE
+	init_timer(&dsi->te_timer);
+	dsi->te_timer.function = dsi_te_timeout;
+	dsi->te_timer.data = 0;
+#endif
+
+	res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "proto");
+	if (!res) {
+		res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+		if (!res) {
+			DSSERR("can't get IORESOURCE_MEM DSI\n");
+			return -EINVAL;
+		}
+
+		temp_res.start = res->start;
+		temp_res.end = temp_res.start + DSI_PROTO_SZ - 1;
+		res = &temp_res;
+	}
+
+	dsi_mem = res;
+
+	dsi->proto_base = devm_ioremap(&dsidev->dev, res->start,
+		resource_size(res));
+	if (!dsi->proto_base) {
+		DSSERR("can't ioremap DSI protocol engine\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "phy");
+	if (!res) {
+		res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+		if (!res) {
+			DSSERR("can't get IORESOURCE_MEM DSI\n");
+			return -EINVAL;
+		}
+
+		temp_res.start = res->start + DSI_PHY_OFFSET;
+		temp_res.end = temp_res.start + DSI_PHY_SZ - 1;
+		res = &temp_res;
+	}
+
+	dsi->phy_base = devm_ioremap(&dsidev->dev, res->start,
+		resource_size(res));
+	if (!dsi->proto_base) {
+		DSSERR("can't ioremap DSI PHY\n");
+		return -ENOMEM;
+	}
+
+	res = platform_get_resource_byname(dsidev, IORESOURCE_MEM, "pll");
+	if (!res) {
+		res = platform_get_resource(dsidev, IORESOURCE_MEM, 0);
+		if (!res) {
+			DSSERR("can't get IORESOURCE_MEM DSI\n");
+			return -EINVAL;
+		}
+
+		temp_res.start = res->start + DSI_PLL_OFFSET;
+		temp_res.end = temp_res.start + DSI_PLL_SZ - 1;
+		res = &temp_res;
+	}
+
+	dsi->pll_base = devm_ioremap(&dsidev->dev, res->start,
+		resource_size(res));
+	if (!dsi->proto_base) {
+		DSSERR("can't ioremap DSI PLL\n");
+		return -ENOMEM;
+	}
+
+	dsi->irq = platform_get_irq(dsi->pdev, 0);
+	if (dsi->irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		return -ENODEV;
+	}
+
+	r = devm_request_irq(&dsidev->dev, dsi->irq, omap_dsi_irq_handler,
+			     IRQF_SHARED, dev_name(&dsidev->dev), dsi->pdev);
+	if (r < 0) {
+		DSSERR("request_irq failed\n");
+		return r;
+	}
+
+	if (dsidev->dev.of_node) {
+		const struct of_device_id *match;
+		const struct dsi_module_id_data *d;
+
+		match = of_match_node(dsi_of_match, dsidev->dev.of_node);
+		if (!match) {
+			DSSERR("unsupported DSI module\n");
+			return -ENODEV;
+		}
+
+		d = match->data;
+
+		while (d->address != 0 && d->address != dsi_mem->start)
+			d++;
+
+		if (d->address == 0) {
+			DSSERR("unsupported DSI module\n");
+			return -ENODEV;
+		}
+
+		dsi->module_id = d->id;
+	} else {
+		dsi->module_id = dsidev->id;
+	}
+
+	/* DSI VCs initialization */
+	for (i = 0; i < ARRAY_SIZE(dsi->vc); i++) {
+		dsi->vc[i].source = DSI_VC_SOURCE_L4;
+		dsi->vc[i].dssdev = NULL;
+		dsi->vc[i].vc_id = 0;
+	}
+
+	r = dsi_get_clocks(dsidev);
+	if (r)
+		return r;
+
+	dsi_init_pll_data(dsidev);
+
+	pm_runtime_enable(&dsidev->dev);
+
+	r = dsi_runtime_get(dsidev);
+	if (r)
+		goto err_runtime_get;
+
+	rev = dsi_read_reg(dsidev, DSI_REVISION);
+	dev_dbg(&dsidev->dev, "OMAP DSI rev %d.%d\n",
+	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+	/* DSI on OMAP3 doesn't have register DSI_GNQ, set number
+	 * of data to 3 by default */
+	if (dss_has_feature(FEAT_DSI_GNQ))
+		/* NB_DATA_LANES */
+		dsi->num_lanes_supported = 1 + REG_GET(dsidev, DSI_GNQ, 11, 9);
+	else
+		dsi->num_lanes_supported = 3;
+
+	dsi->line_buffer_size = dsi_get_line_buf_size(dsidev);
+
+	dsi_init_output(dsidev);
+
+	if (dsidev->dev.of_node) {
+		r = dsi_probe_of(dsidev);
+		if (r) {
+			DSSERR("Invalid DSI DT data\n");
+			goto err_probe_of;
+		}
+
+		r = of_platform_populate(dsidev->dev.of_node, NULL, NULL,
+			&dsidev->dev);
+		if (r)
+			DSSERR("Failed to populate DSI child devices: %d\n", r);
+	}
+
+	dsi_runtime_put(dsidev);
+
+	if (dsi->module_id == 0)
+		dss_debugfs_create_file("dsi1_regs", dsi1_dump_regs);
+	else if (dsi->module_id == 1)
+		dss_debugfs_create_file("dsi2_regs", dsi2_dump_regs);
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+	if (dsi->module_id == 0)
+		dss_debugfs_create_file("dsi1_irqs", dsi1_dump_irqs);
+	else if (dsi->module_id == 1)
+		dss_debugfs_create_file("dsi2_irqs", dsi2_dump_irqs);
+#endif
+
+	return 0;
+
+err_probe_of:
+	dsi_uninit_output(dsidev);
+	dsi_runtime_put(dsidev);
+
+err_runtime_get:
+	pm_runtime_disable(&dsidev->dev);
+	return r;
+}
+
+static void dsi_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *dsidev = to_platform_device(dev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
+
+	of_platform_depopulate(&dsidev->dev);
+
+	WARN_ON(dsi->scp_clk_refcount > 0);
+
+	dss_pll_unregister(&dsi->pll);
+
+	dsi_uninit_output(dsidev);
+
+	pm_runtime_disable(&dsidev->dev);
+
+	if (dsi->vdds_dsi_reg != NULL && dsi->vdds_dsi_enabled) {
+		regulator_disable(dsi->vdds_dsi_reg);
+		dsi->vdds_dsi_enabled = false;
+	}
+}
+
+static const struct component_ops dsi_component_ops = {
+	.bind	= dsi_bind,
+	.unbind	= dsi_unbind,
+};
+
+static int dsi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &dsi_component_ops);
+}
+
+static int dsi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &dsi_component_ops);
+	return 0;
+}
+
+static int dsi_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+
+	dsi->is_enabled = false;
+	/* ensure the irq handler sees the is_enabled value */
+	smp_wmb();
+	/* wait for current handler to finish before turning the DSI off */
+	synchronize_irq(dsi->irq);
+
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int dsi_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dsi_data *dsi = dsi_get_dsidrv_data(pdev);
+	int r;
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	dsi->is_enabled = true;
+	/* ensure the irq handler sees the is_enabled value */
+	smp_wmb();
+
+	return 0;
+}
+
+static const struct dev_pm_ops dsi_pm_ops = {
+	.runtime_suspend = dsi_runtime_suspend,
+	.runtime_resume = dsi_runtime_resume,
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap3[] = {
+	{ .address = 0x4804fc00, .id = 0, },
+	{ },
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap4[] = {
+	{ .address = 0x58004000, .id = 0, },
+	{ .address = 0x58005000, .id = 1, },
+	{ },
+};
+
+static const struct dsi_module_id_data dsi_of_data_omap5[] = {
+	{ .address = 0x58004000, .id = 0, },
+	{ .address = 0x58009000, .id = 1, },
+	{ },
+};
+
+static const struct of_device_id dsi_of_match[] = {
+	{ .compatible = "ti,omap3-dsi", .data = dsi_of_data_omap3, },
+	{ .compatible = "ti,omap4-dsi", .data = dsi_of_data_omap4, },
+	{ .compatible = "ti,omap5-dsi", .data = dsi_of_data_omap5, },
+	{},
+};
+
+static struct platform_driver omap_dsihw_driver = {
+	.probe		= dsi_probe,
+	.remove		= dsi_remove,
+	.driver         = {
+		.name   = "omapdss_dsi",
+		.pm	= &dsi_pm_ops,
+		.of_match_table = dsi_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init dsi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsihw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_dsihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c b/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c
new file mode 100644
index 000000000000..bf407b6ba15c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss-of.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2013 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/seq_file.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+struct device_node *
+omapdss_of_get_next_port(const struct device_node *parent,
+			 struct device_node *prev)
+{
+	struct device_node *port = NULL;
+
+	if (!parent)
+		return NULL;
+
+	if (!prev) {
+		struct device_node *ports;
+		/*
+		 * It's the first call, we have to find a port subnode
+		 * within this node or within an optional 'ports' node.
+		 */
+		ports = of_get_child_by_name(parent, "ports");
+		if (ports)
+			parent = ports;
+
+		port = of_get_child_by_name(parent, "port");
+
+		/* release the 'ports' node */
+		of_node_put(ports);
+	} else {
+		struct device_node *ports;
+
+		ports = of_get_parent(prev);
+		if (!ports)
+			return NULL;
+
+		do {
+			port = of_get_next_child(ports, prev);
+			if (!port) {
+				of_node_put(ports);
+				return NULL;
+			}
+			prev = port;
+		} while (of_node_cmp(port->name, "port") != 0);
+
+		of_node_put(ports);
+	}
+
+	return port;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_next_port);
+
+struct device_node *
+omapdss_of_get_next_endpoint(const struct device_node *parent,
+			     struct device_node *prev)
+{
+	struct device_node *ep = NULL;
+
+	if (!parent)
+		return NULL;
+
+	do {
+		ep = of_get_next_child(parent, prev);
+		if (!ep)
+			return NULL;
+		prev = ep;
+	} while (of_node_cmp(ep->name, "endpoint") != 0);
+
+	return ep;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_next_endpoint);
+
+struct device_node *dss_of_port_get_parent_device(struct device_node *port)
+{
+	struct device_node *np;
+	int i;
+
+	if (!port)
+		return NULL;
+
+	np = of_get_parent(port);
+
+	for (i = 0; i < 2 && np; ++i) {
+		struct property *prop;
+
+		prop = of_find_property(np, "compatible", NULL);
+
+		if (prop)
+			return np;
+
+		np = of_get_next_parent(np);
+	}
+
+	return NULL;
+}
+
+u32 dss_of_port_get_port_number(struct device_node *port)
+{
+	int r;
+	u32 reg;
+
+	r = of_property_read_u32(port, "reg", &reg);
+	if (r)
+		reg = 0;
+
+	return reg;
+}
+
+static struct device_node *omapdss_of_get_remote_port(const struct device_node *node)
+{
+	struct device_node *np;
+
+	np = of_parse_phandle(node, "remote-endpoint", 0);
+	if (!np)
+		return NULL;
+
+	np = of_get_next_parent(np);
+
+	return np;
+}
+
+struct device_node *
+omapdss_of_get_first_endpoint(const struct device_node *parent)
+{
+	struct device_node *port, *ep;
+
+	port = omapdss_of_get_next_port(parent, NULL);
+
+	if (!port)
+		return NULL;
+
+	ep = omapdss_of_get_next_endpoint(port, NULL);
+
+	of_node_put(port);
+
+	return ep;
+}
+EXPORT_SYMBOL_GPL(omapdss_of_get_first_endpoint);
+
+struct omap_dss_device *
+omapdss_of_find_source_for_first_ep(struct device_node *node)
+{
+	struct device_node *ep;
+	struct device_node *src_port;
+	struct omap_dss_device *src;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return ERR_PTR(-EINVAL);
+
+	src_port = omapdss_of_get_remote_port(ep);
+	if (!src_port) {
+		of_node_put(ep);
+		return ERR_PTR(-EINVAL);
+	}
+
+	of_node_put(ep);
+
+	src = omap_dss_find_output_by_port_node(src_port);
+
+	of_node_put(src_port);
+
+	return src ? src : ERR_PTR(-EPROBE_DEFER);
+}
+EXPORT_SYMBOL_GPL(omapdss_of_find_source_for_first_ep);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss.c b/drivers/video/fbdev/omap2/omapfb/dss/dss.c
new file mode 100644
index 000000000000..0078c4d1fc31
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss.c
@@ -0,0 +1,1329 @@
+/*
+ * linux/drivers/video/omap2/dss/dss.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "DSS"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/export.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/seq_file.h>
+#include <linux/clk.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/gfp.h>
+#include <linux/sizes.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <linux/regulator/consumer.h>
+#include <linux/suspend.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+#define DSS_SZ_REGS			SZ_512
+
+struct dss_reg {
+	u16 idx;
+};
+
+#define DSS_REG(idx)			((const struct dss_reg) { idx })
+
+#define DSS_REVISION			DSS_REG(0x0000)
+#define DSS_SYSCONFIG			DSS_REG(0x0010)
+#define DSS_SYSSTATUS			DSS_REG(0x0014)
+#define DSS_CONTROL			DSS_REG(0x0040)
+#define DSS_SDI_CONTROL			DSS_REG(0x0044)
+#define DSS_PLL_CONTROL			DSS_REG(0x0048)
+#define DSS_SDI_STATUS			DSS_REG(0x005C)
+
+#define REG_GET(idx, start, end) \
+	FLD_GET(dss_read_reg(idx), start, end)
+
+#define REG_FLD_MOD(idx, val, start, end) \
+	dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
+
+struct dss_features {
+	u8 fck_div_max;
+	u8 dss_fck_multiplier;
+	const char *parent_clk_name;
+	const enum omap_display_type *ports;
+	int num_ports;
+	int (*dpi_select_source)(int port, enum omap_channel channel);
+};
+
+static struct {
+	struct platform_device *pdev;
+	void __iomem    *base;
+	struct regmap	*syscon_pll_ctrl;
+	u32		syscon_pll_ctrl_offset;
+
+	struct clk	*parent_clk;
+	struct clk	*dss_clk;
+	unsigned long	dss_clk_rate;
+
+	unsigned long	cache_req_pck;
+	unsigned long	cache_prate;
+	struct dispc_clock_info cache_dispc_cinfo;
+
+	enum omap_dss_clk_source dsi_clk_source[MAX_NUM_DSI];
+	enum omap_dss_clk_source dispc_clk_source;
+	enum omap_dss_clk_source lcd_clk_source[MAX_DSS_LCD_MANAGERS];
+
+	bool		ctx_valid;
+	u32		ctx[DSS_SZ_REGS / sizeof(u32)];
+
+	const struct dss_features *feat;
+
+	struct dss_pll	*video1_pll;
+	struct dss_pll	*video2_pll;
+} dss;
+
+static const char * const dss_generic_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DSI_PLL_HSDIV_DISPC",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DSI_PLL_HSDIV_DSI",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCK",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC]	= "DSI_PLL2_HSDIV_DISPC",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "DSI_PLL2_HSDIV_DSI",
+};
+
+static bool dss_initialized;
+
+bool omapdss_is_initialized(void)
+{
+	return dss_initialized;
+}
+EXPORT_SYMBOL(omapdss_is_initialized);
+
+static inline void dss_write_reg(const struct dss_reg idx, u32 val)
+{
+	__raw_writel(val, dss.base + idx.idx);
+}
+
+static inline u32 dss_read_reg(const struct dss_reg idx)
+{
+	return __raw_readl(dss.base + idx.idx);
+}
+
+#define SR(reg) \
+	dss.ctx[(DSS_##reg).idx / sizeof(u32)] = dss_read_reg(DSS_##reg)
+#define RR(reg) \
+	dss_write_reg(DSS_##reg, dss.ctx[(DSS_##reg).idx / sizeof(u32)])
+
+static void dss_save_context(void)
+{
+	DSSDBG("dss_save_context\n");
+
+	SR(CONTROL);
+
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		SR(SDI_CONTROL);
+		SR(PLL_CONTROL);
+	}
+
+	dss.ctx_valid = true;
+
+	DSSDBG("context saved\n");
+}
+
+static void dss_restore_context(void)
+{
+	DSSDBG("dss_restore_context\n");
+
+	if (!dss.ctx_valid)
+		return;
+
+	RR(CONTROL);
+
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		RR(SDI_CONTROL);
+		RR(PLL_CONTROL);
+	}
+
+	DSSDBG("context restored\n");
+}
+
+#undef SR
+#undef RR
+
+void dss_ctrl_pll_enable(enum dss_pll_id pll_id, bool enable)
+{
+	unsigned shift;
+	unsigned val;
+
+	if (!dss.syscon_pll_ctrl)
+		return;
+
+	val = !enable;
+
+	switch (pll_id) {
+	case DSS_PLL_VIDEO1:
+		shift = 0;
+		break;
+	case DSS_PLL_VIDEO2:
+		shift = 1;
+		break;
+	case DSS_PLL_HDMI:
+		shift = 2;
+		break;
+	default:
+		DSSERR("illegal DSS PLL ID %d\n", pll_id);
+		return;
+	}
+
+	regmap_update_bits(dss.syscon_pll_ctrl, dss.syscon_pll_ctrl_offset,
+		1 << shift, val << shift);
+}
+
+void dss_ctrl_pll_set_control_mux(enum dss_pll_id pll_id,
+	enum omap_channel channel)
+{
+	unsigned shift, val;
+
+	if (!dss.syscon_pll_ctrl)
+		return;
+
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		shift = 3;
+
+		switch (pll_id) {
+		case DSS_PLL_VIDEO1:
+			val = 0; break;
+		case DSS_PLL_HDMI:
+			val = 1; break;
+		default:
+			DSSERR("error in PLL mux config for LCD\n");
+			return;
+		}
+
+		break;
+	case OMAP_DSS_CHANNEL_LCD2:
+		shift = 5;
+
+		switch (pll_id) {
+		case DSS_PLL_VIDEO1:
+			val = 0; break;
+		case DSS_PLL_VIDEO2:
+			val = 1; break;
+		case DSS_PLL_HDMI:
+			val = 2; break;
+		default:
+			DSSERR("error in PLL mux config for LCD2\n");
+			return;
+		}
+
+		break;
+	case OMAP_DSS_CHANNEL_LCD3:
+		shift = 7;
+
+		switch (pll_id) {
+		case DSS_PLL_VIDEO1:
+			val = 1; break;
+		case DSS_PLL_VIDEO2:
+			val = 0; break;
+		case DSS_PLL_HDMI:
+			val = 2; break;
+		default:
+			DSSERR("error in PLL mux config for LCD3\n");
+			return;
+		}
+
+		break;
+	default:
+		DSSERR("error in PLL mux config\n");
+		return;
+	}
+
+	regmap_update_bits(dss.syscon_pll_ctrl, dss.syscon_pll_ctrl_offset,
+		0x3 << shift, val << shift);
+}
+
+void dss_sdi_init(int datapairs)
+{
+	u32 l;
+
+	BUG_ON(datapairs > 3 || datapairs < 1);
+
+	l = dss_read_reg(DSS_SDI_CONTROL);
+	l = FLD_MOD(l, 0xf, 19, 15);		/* SDI_PDIV */
+	l = FLD_MOD(l, datapairs-1, 3, 2);	/* SDI_PRSEL */
+	l = FLD_MOD(l, 2, 1, 0);		/* SDI_BWSEL */
+	dss_write_reg(DSS_SDI_CONTROL, l);
+
+	l = dss_read_reg(DSS_PLL_CONTROL);
+	l = FLD_MOD(l, 0x7, 25, 22);	/* SDI_PLL_FREQSEL */
+	l = FLD_MOD(l, 0xb, 16, 11);	/* SDI_PLL_REGN */
+	l = FLD_MOD(l, 0xb4, 10, 1);	/* SDI_PLL_REGM */
+	dss_write_reg(DSS_PLL_CONTROL, l);
+}
+
+int dss_sdi_enable(void)
+{
+	unsigned long timeout;
+
+	dispc_pck_free_enable(1);
+
+	/* Reset SDI PLL */
+	REG_FLD_MOD(DSS_PLL_CONTROL, 1, 18, 18); /* SDI_PLL_SYSRESET */
+	udelay(1);	/* wait 2x PCLK */
+
+	/* Lock SDI PLL */
+	REG_FLD_MOD(DSS_PLL_CONTROL, 1, 28, 28); /* SDI_PLL_GOBIT */
+
+	/* Waiting for PLL lock request to complete */
+	timeout = jiffies + msecs_to_jiffies(500);
+	while (dss_read_reg(DSS_SDI_STATUS) & (1 << 6)) {
+		if (time_after_eq(jiffies, timeout)) {
+			DSSERR("PLL lock request timed out\n");
+			goto err1;
+		}
+	}
+
+	/* Clearing PLL_GO bit */
+	REG_FLD_MOD(DSS_PLL_CONTROL, 0, 28, 28);
+
+	/* Waiting for PLL to lock */
+	timeout = jiffies + msecs_to_jiffies(500);
+	while (!(dss_read_reg(DSS_SDI_STATUS) & (1 << 5))) {
+		if (time_after_eq(jiffies, timeout)) {
+			DSSERR("PLL lock timed out\n");
+			goto err1;
+		}
+	}
+
+	dispc_lcd_enable_signal(1);
+
+	/* Waiting for SDI reset to complete */
+	timeout = jiffies + msecs_to_jiffies(500);
+	while (!(dss_read_reg(DSS_SDI_STATUS) & (1 << 2))) {
+		if (time_after_eq(jiffies, timeout)) {
+			DSSERR("SDI reset timed out\n");
+			goto err2;
+		}
+	}
+
+	return 0;
+
+ err2:
+	dispc_lcd_enable_signal(0);
+ err1:
+	/* Reset SDI PLL */
+	REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
+
+	dispc_pck_free_enable(0);
+
+	return -ETIMEDOUT;
+}
+
+void dss_sdi_disable(void)
+{
+	dispc_lcd_enable_signal(0);
+
+	dispc_pck_free_enable(0);
+
+	/* Reset SDI PLL */
+	REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
+}
+
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src)
+{
+	return dss_generic_clk_source_names[clk_src];
+}
+
+void dss_dump_clocks(struct seq_file *s)
+{
+	const char *fclk_name, *fclk_real_name;
+	unsigned long fclk_rate;
+
+	if (dss_runtime_get())
+		return;
+
+	seq_printf(s, "- DSS -\n");
+
+	fclk_name = dss_get_generic_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+	fclk_real_name = dss_feat_get_clk_source_name(OMAP_DSS_CLK_SRC_FCK);
+	fclk_rate = clk_get_rate(dss.dss_clk);
+
+	seq_printf(s, "%s (%s) = %lu\n",
+			fclk_name, fclk_real_name,
+			fclk_rate);
+
+	dss_runtime_put();
+}
+
+static void dss_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
+
+	if (dss_runtime_get())
+		return;
+
+	DUMPREG(DSS_REVISION);
+	DUMPREG(DSS_SYSCONFIG);
+	DUMPREG(DSS_SYSSTATUS);
+	DUMPREG(DSS_CONTROL);
+
+	if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+			OMAP_DISPLAY_TYPE_SDI) {
+		DUMPREG(DSS_SDI_CONTROL);
+		DUMPREG(DSS_PLL_CONTROL);
+		DUMPREG(DSS_SDI_STATUS);
+	}
+
+	dss_runtime_put();
+#undef DUMPREG
+}
+
+static void dss_select_dispc_clk_source(enum omap_dss_clk_source clk_src)
+{
+	int b;
+	u8 start, end;
+
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		b = 1;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		b = 2;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	dss_feat_get_reg_field(FEAT_REG_DISPC_CLK_SWITCH, &start, &end);
+
+	REG_FLD_MOD(DSS_CONTROL, b, start, end);	/* DISPC_CLK_SWITCH */
+
+	dss.dispc_clk_source = clk_src;
+}
+
+void dss_select_dsi_clk_source(int dsi_module,
+		enum omap_dss_clk_source clk_src)
+{
+	int b, pos;
+
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI:
+		BUG_ON(dsi_module != 0);
+		b = 1;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI:
+		BUG_ON(dsi_module != 1);
+		b = 1;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	pos = dsi_module == 0 ? 1 : 10;
+	REG_FLD_MOD(DSS_CONTROL, b, pos, pos);	/* DSIx_CLK_SWITCH */
+
+	dss.dsi_clk_source[dsi_module] = clk_src;
+}
+
+void dss_select_lcd_clk_source(enum omap_channel channel,
+		enum omap_dss_clk_source clk_src)
+{
+	int b, ix, pos;
+
+	if (!dss_has_feature(FEAT_LCD_CLK_SRC)) {
+		dss_select_dispc_clk_source(clk_src);
+		return;
+	}
+
+	switch (clk_src) {
+	case OMAP_DSS_CLK_SRC_FCK:
+		b = 0;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD);
+		b = 1;
+		break;
+	case OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC:
+		BUG_ON(channel != OMAP_DSS_CHANNEL_LCD2 &&
+		       channel != OMAP_DSS_CHANNEL_LCD3);
+		b = 1;
+		break;
+	default:
+		BUG();
+		return;
+	}
+
+	pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+	     (channel == OMAP_DSS_CHANNEL_LCD2 ? 12 : 19);
+	REG_FLD_MOD(DSS_CONTROL, b, pos, pos);	/* LCDx_CLK_SWITCH */
+
+	ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+	    (channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
+	dss.lcd_clk_source[ix] = clk_src;
+}
+
+enum omap_dss_clk_source dss_get_dispc_clk_source(void)
+{
+	return dss.dispc_clk_source;
+}
+
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module)
+{
+	return dss.dsi_clk_source[dsi_module];
+}
+
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
+{
+	if (dss_has_feature(FEAT_LCD_CLK_SRC)) {
+		int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 :
+			(channel == OMAP_DSS_CHANNEL_LCD2 ? 1 : 2);
+		return dss.lcd_clk_source[ix];
+	} else {
+		/* LCD_CLK source is the same as DISPC_FCLK source for
+		 * OMAP2 and OMAP3 */
+		return dss.dispc_clk_source;
+	}
+}
+
+bool dss_div_calc(unsigned long pck, unsigned long fck_min,
+		dss_div_calc_func func, void *data)
+{
+	int fckd, fckd_start, fckd_stop;
+	unsigned long fck;
+	unsigned long fck_hw_max;
+	unsigned long fckd_hw_max;
+	unsigned long prate;
+	unsigned m;
+
+	fck_hw_max = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	if (dss.parent_clk == NULL) {
+		unsigned pckd;
+
+		pckd = fck_hw_max / pck;
+
+		fck = pck * pckd;
+
+		fck = clk_round_rate(dss.dss_clk, fck);
+
+		return func(fck, data);
+	}
+
+	fckd_hw_max = dss.feat->fck_div_max;
+
+	m = dss.feat->dss_fck_multiplier;
+	prate = clk_get_rate(dss.parent_clk);
+
+	fck_min = fck_min ? fck_min : 1;
+
+	fckd_start = min(prate * m / fck_min, fckd_hw_max);
+	fckd_stop = max(DIV_ROUND_UP(prate * m, fck_hw_max), 1ul);
+
+	for (fckd = fckd_start; fckd >= fckd_stop; --fckd) {
+		fck = DIV_ROUND_UP(prate, fckd) * m;
+
+		if (func(fck, data))
+			return true;
+	}
+
+	return false;
+}
+
+int dss_set_fck_rate(unsigned long rate)
+{
+	int r;
+
+	DSSDBG("set fck to %lu\n", rate);
+
+	r = clk_set_rate(dss.dss_clk, rate);
+	if (r)
+		return r;
+
+	dss.dss_clk_rate = clk_get_rate(dss.dss_clk);
+
+	WARN_ONCE(dss.dss_clk_rate != rate,
+			"clk rate mismatch: %lu != %lu", dss.dss_clk_rate,
+			rate);
+
+	return 0;
+}
+
+unsigned long dss_get_dispc_clk_rate(void)
+{
+	return dss.dss_clk_rate;
+}
+
+static int dss_setup_default_clock(void)
+{
+	unsigned long max_dss_fck, prate;
+	unsigned long fck;
+	unsigned fck_div;
+	int r;
+
+	max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+	if (dss.parent_clk == NULL) {
+		fck = clk_round_rate(dss.dss_clk, max_dss_fck);
+	} else {
+		prate = clk_get_rate(dss.parent_clk);
+
+		fck_div = DIV_ROUND_UP(prate * dss.feat->dss_fck_multiplier,
+				max_dss_fck);
+		fck = DIV_ROUND_UP(prate, fck_div) * dss.feat->dss_fck_multiplier;
+	}
+
+	r = dss_set_fck_rate(fck);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+void dss_set_venc_output(enum omap_dss_venc_type type)
+{
+	int l = 0;
+
+	if (type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+		l = 0;
+	else if (type == OMAP_DSS_VENC_TYPE_SVIDEO)
+		l = 1;
+	else
+		BUG();
+
+	/* venc out selection. 0 = comp, 1 = svideo */
+	REG_FLD_MOD(DSS_CONTROL, l, 6, 6);
+}
+
+void dss_set_dac_pwrdn_bgz(bool enable)
+{
+	REG_FLD_MOD(DSS_CONTROL, enable, 5, 5);	/* DAC Power-Down Control */
+}
+
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select src)
+{
+	enum omap_display_type dp;
+	dp = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+
+	/* Complain about invalid selections */
+	WARN_ON((src == DSS_VENC_TV_CLK) && !(dp & OMAP_DISPLAY_TYPE_VENC));
+	WARN_ON((src == DSS_HDMI_M_PCLK) && !(dp & OMAP_DISPLAY_TYPE_HDMI));
+
+	/* Select only if we have options */
+	if ((dp & OMAP_DISPLAY_TYPE_VENC) && (dp & OMAP_DISPLAY_TYPE_HDMI))
+		REG_FLD_MOD(DSS_CONTROL, src, 15, 15);	/* VENC_HDMI_SWITCH */
+}
+
+enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void)
+{
+	enum omap_display_type displays;
+
+	displays = dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_DIGIT);
+	if ((displays & OMAP_DISPLAY_TYPE_HDMI) == 0)
+		return DSS_VENC_TV_CLK;
+
+	if ((displays & OMAP_DISPLAY_TYPE_VENC) == 0)
+		return DSS_HDMI_M_PCLK;
+
+	return REG_GET(DSS_CONTROL, 15, 15);
+}
+
+static int dss_dpi_select_source_omap2_omap3(int port, enum omap_channel channel)
+{
+	if (channel != OMAP_DSS_CHANNEL_LCD)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int dss_dpi_select_source_omap4(int port, enum omap_channel channel)
+{
+	int val;
+
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD2:
+		val = 0;
+		break;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		val = 1;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	REG_FLD_MOD(DSS_CONTROL, val, 17, 17);
+
+	return 0;
+}
+
+static int dss_dpi_select_source_omap5(int port, enum omap_channel channel)
+{
+	int val;
+
+	switch (channel) {
+	case OMAP_DSS_CHANNEL_LCD:
+		val = 1;
+		break;
+	case OMAP_DSS_CHANNEL_LCD2:
+		val = 2;
+		break;
+	case OMAP_DSS_CHANNEL_LCD3:
+		val = 3;
+		break;
+	case OMAP_DSS_CHANNEL_DIGIT:
+		val = 0;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	REG_FLD_MOD(DSS_CONTROL, val, 17, 16);
+
+	return 0;
+}
+
+static int dss_dpi_select_source_dra7xx(int port, enum omap_channel channel)
+{
+	switch (port) {
+	case 0:
+		return dss_dpi_select_source_omap5(port, channel);
+	case 1:
+		if (channel != OMAP_DSS_CHANNEL_LCD2)
+			return -EINVAL;
+		break;
+	case 2:
+		if (channel != OMAP_DSS_CHANNEL_LCD3)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int dss_dpi_select_source(int port, enum omap_channel channel)
+{
+	return dss.feat->dpi_select_source(port, channel);
+}
+
+static int dss_get_clocks(void)
+{
+	struct clk *clk;
+
+	clk = devm_clk_get(&dss.pdev->dev, "fck");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get clock fck\n");
+		return PTR_ERR(clk);
+	}
+
+	dss.dss_clk = clk;
+
+	if (dss.feat->parent_clk_name) {
+		clk = clk_get(NULL, dss.feat->parent_clk_name);
+		if (IS_ERR(clk)) {
+			DSSERR("Failed to get %s\n", dss.feat->parent_clk_name);
+			return PTR_ERR(clk);
+		}
+	} else {
+		clk = NULL;
+	}
+
+	dss.parent_clk = clk;
+
+	return 0;
+}
+
+static void dss_put_clocks(void)
+{
+	if (dss.parent_clk)
+		clk_put(dss.parent_clk);
+}
+
+int dss_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("dss_runtime_get\n");
+
+	r = pm_runtime_get_sync(&dss.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+void dss_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("dss_runtime_put\n");
+
+	r = pm_runtime_put_sync(&dss.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS && r != -EBUSY);
+}
+
+/* DEBUGFS */
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+void dss_debug_dump_clocks(struct seq_file *s)
+{
+	dss_dump_clocks(s);
+	dispc_dump_clocks(s);
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+	dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+
+static const enum omap_display_type omap2plus_ports[] = {
+	OMAP_DISPLAY_TYPE_DPI,
+};
+
+static const enum omap_display_type omap34xx_ports[] = {
+	OMAP_DISPLAY_TYPE_DPI,
+	OMAP_DISPLAY_TYPE_SDI,
+};
+
+static const enum omap_display_type dra7xx_ports[] = {
+	OMAP_DISPLAY_TYPE_DPI,
+	OMAP_DISPLAY_TYPE_DPI,
+	OMAP_DISPLAY_TYPE_DPI,
+};
+
+static const struct dss_features omap24xx_dss_feats = {
+	/*
+	 * fck div max is really 16, but the divider range has gaps. The range
+	 * from 1 to 6 has no gaps, so let's use that as a max.
+	 */
+	.fck_div_max		=	6,
+	.dss_fck_multiplier	=	2,
+	.parent_clk_name	=	"core_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+	.ports			=	omap2plus_ports,
+	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap34xx_dss_feats = {
+	.fck_div_max		=	16,
+	.dss_fck_multiplier	=	2,
+	.parent_clk_name	=	"dpll4_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+	.ports			=	omap34xx_ports,
+	.num_ports		=	ARRAY_SIZE(omap34xx_ports),
+};
+
+static const struct dss_features omap3630_dss_feats = {
+	.fck_div_max		=	32,
+	.dss_fck_multiplier	=	1,
+	.parent_clk_name	=	"dpll4_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+	.ports			=	omap2plus_ports,
+	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap44xx_dss_feats = {
+	.fck_div_max		=	32,
+	.dss_fck_multiplier	=	1,
+	.parent_clk_name	=	"dpll_per_x2_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap4,
+	.ports			=	omap2plus_ports,
+	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features omap54xx_dss_feats = {
+	.fck_div_max		=	64,
+	.dss_fck_multiplier	=	1,
+	.parent_clk_name	=	"dpll_per_x2_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_omap5,
+	.ports			=	omap2plus_ports,
+	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features am43xx_dss_feats = {
+	.fck_div_max		=	0,
+	.dss_fck_multiplier	=	0,
+	.parent_clk_name	=	NULL,
+	.dpi_select_source	=	&dss_dpi_select_source_omap2_omap3,
+	.ports			=	omap2plus_ports,
+	.num_ports		=	ARRAY_SIZE(omap2plus_ports),
+};
+
+static const struct dss_features dra7xx_dss_feats = {
+	.fck_div_max		=	64,
+	.dss_fck_multiplier	=	1,
+	.parent_clk_name	=	"dpll_per_x2_ck",
+	.dpi_select_source	=	&dss_dpi_select_source_dra7xx,
+	.ports			=	dra7xx_ports,
+	.num_ports		=	ARRAY_SIZE(dra7xx_ports),
+};
+
+static int dss_init_features(struct platform_device *pdev)
+{
+	const struct dss_features *src;
+	struct dss_features *dst;
+
+	dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(&pdev->dev, "Failed to allocate local DSS Features\n");
+		return -ENOMEM;
+	}
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP24xx:
+		src = &omap24xx_dss_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+	case OMAPDSS_VER_AM35xx:
+		src = &omap34xx_dss_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP3630:
+		src = &omap3630_dss_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		src = &omap44xx_dss_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+		src = &omap54xx_dss_feats;
+		break;
+
+	case OMAPDSS_VER_AM43xx:
+		src = &am43xx_dss_feats;
+		break;
+
+	case OMAPDSS_VER_DRA7xx:
+		src = &dra7xx_dss_feats;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	dss.feat = dst;
+
+	return 0;
+}
+
+static int dss_init_ports(struct platform_device *pdev)
+{
+	struct device_node *parent = pdev->dev.of_node;
+	struct device_node *port;
+	int r;
+
+	if (parent == NULL)
+		return 0;
+
+	port = omapdss_of_get_next_port(parent, NULL);
+	if (!port)
+		return 0;
+
+	if (dss.feat->num_ports == 0)
+		return 0;
+
+	do {
+		enum omap_display_type port_type;
+		u32 reg;
+
+		r = of_property_read_u32(port, "reg", &reg);
+		if (r)
+			reg = 0;
+
+		if (reg >= dss.feat->num_ports)
+			continue;
+
+		port_type = dss.feat->ports[reg];
+
+		switch (port_type) {
+		case OMAP_DISPLAY_TYPE_DPI:
+			dpi_init_port(pdev, port);
+			break;
+		case OMAP_DISPLAY_TYPE_SDI:
+			sdi_init_port(pdev, port);
+			break;
+		default:
+			break;
+		}
+	} while ((port = omapdss_of_get_next_port(parent, port)) != NULL);
+
+	return 0;
+}
+
+static void dss_uninit_ports(struct platform_device *pdev)
+{
+	struct device_node *parent = pdev->dev.of_node;
+	struct device_node *port;
+
+	if (parent == NULL)
+		return;
+
+	port = omapdss_of_get_next_port(parent, NULL);
+	if (!port)
+		return;
+
+	if (dss.feat->num_ports == 0)
+		return;
+
+	do {
+		enum omap_display_type port_type;
+		u32 reg;
+		int r;
+
+		r = of_property_read_u32(port, "reg", &reg);
+		if (r)
+			reg = 0;
+
+		if (reg >= dss.feat->num_ports)
+			continue;
+
+		port_type = dss.feat->ports[reg];
+
+		switch (port_type) {
+		case OMAP_DISPLAY_TYPE_DPI:
+			dpi_uninit_port(port);
+			break;
+		case OMAP_DISPLAY_TYPE_SDI:
+			sdi_uninit_port(port);
+			break;
+		default:
+			break;
+		}
+	} while ((port = omapdss_of_get_next_port(parent, port)) != NULL);
+}
+
+static int dss_video_pll_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct regulator *pll_regulator;
+	int r;
+
+	if (!np)
+		return 0;
+
+	if (of_property_read_bool(np, "syscon-pll-ctrl")) {
+		dss.syscon_pll_ctrl = syscon_regmap_lookup_by_phandle(np,
+			"syscon-pll-ctrl");
+		if (IS_ERR(dss.syscon_pll_ctrl)) {
+			dev_err(&pdev->dev,
+				"failed to get syscon-pll-ctrl regmap\n");
+			return PTR_ERR(dss.syscon_pll_ctrl);
+		}
+
+		if (of_property_read_u32_index(np, "syscon-pll-ctrl", 1,
+				&dss.syscon_pll_ctrl_offset)) {
+			dev_err(&pdev->dev,
+				"failed to get syscon-pll-ctrl offset\n");
+			return -EINVAL;
+		}
+	}
+
+	pll_regulator = devm_regulator_get(&pdev->dev, "vdda_video");
+	if (IS_ERR(pll_regulator)) {
+		r = PTR_ERR(pll_regulator);
+
+		switch (r) {
+		case -ENOENT:
+			pll_regulator = NULL;
+			break;
+
+		case -EPROBE_DEFER:
+			return -EPROBE_DEFER;
+
+		default:
+			DSSERR("can't get DPLL VDDA regulator\n");
+			return r;
+		}
+	}
+
+	if (of_property_match_string(np, "reg-names", "pll1") >= 0) {
+		dss.video1_pll = dss_video_pll_init(pdev, 0, pll_regulator);
+		if (IS_ERR(dss.video1_pll))
+			return PTR_ERR(dss.video1_pll);
+	}
+
+	if (of_property_match_string(np, "reg-names", "pll2") >= 0) {
+		dss.video2_pll = dss_video_pll_init(pdev, 1, pll_regulator);
+		if (IS_ERR(dss.video2_pll)) {
+			dss_video_pll_uninit(dss.video1_pll);
+			return PTR_ERR(dss.video2_pll);
+		}
+	}
+
+	return 0;
+}
+
+/* DSS HW IP initialisation */
+static int dss_bind(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct resource *dss_mem;
+	u32 rev;
+	int r;
+
+	dss.pdev = pdev;
+
+	r = dss_init_features(dss.pdev);
+	if (r)
+		return r;
+
+	dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+	if (!dss_mem) {
+		DSSERR("can't get IORESOURCE_MEM DSS\n");
+		return -EINVAL;
+	}
+
+	dss.base = devm_ioremap(&pdev->dev, dss_mem->start,
+				resource_size(dss_mem));
+	if (!dss.base) {
+		DSSERR("can't ioremap DSS\n");
+		return -ENOMEM;
+	}
+
+	r = dss_get_clocks();
+	if (r)
+		return r;
+
+	r = dss_setup_default_clock();
+	if (r)
+		goto err_setup_clocks;
+
+	r = dss_video_pll_probe(pdev);
+	if (r)
+		goto err_pll_init;
+
+	r = dss_init_ports(pdev);
+	if (r)
+		goto err_init_ports;
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = dss_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	dss.dss_clk_rate = clk_get_rate(dss.dss_clk);
+
+	/* Select DPLL */
+	REG_FLD_MOD(DSS_CONTROL, 0, 0, 0);
+
+	dss_select_dispc_clk_source(OMAP_DSS_CLK_SRC_FCK);
+
+#ifdef CONFIG_FB_OMAP2_DSS_VENC
+	REG_FLD_MOD(DSS_CONTROL, 1, 4, 4);	/* venc dac demen */
+	REG_FLD_MOD(DSS_CONTROL, 1, 3, 3);	/* venc clock 4x enable */
+	REG_FLD_MOD(DSS_CONTROL, 0, 2, 2);	/* venc clock mode = normal */
+#endif
+	dss.dsi_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+	dss.dsi_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
+	dss.dispc_clk_source = OMAP_DSS_CLK_SRC_FCK;
+	dss.lcd_clk_source[0] = OMAP_DSS_CLK_SRC_FCK;
+	dss.lcd_clk_source[1] = OMAP_DSS_CLK_SRC_FCK;
+
+	rev = dss_read_reg(DSS_REVISION);
+	printk(KERN_INFO "OMAP DSS rev %d.%d\n",
+			FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+	dss_runtime_put();
+
+	r = component_bind_all(&pdev->dev, NULL);
+	if (r)
+		goto err_component;
+
+	dss_debugfs_create_file("dss", dss_dump_regs);
+
+	pm_set_vt_switch(0);
+
+	dss_initialized = true;
+
+	return 0;
+
+err_component:
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	dss_uninit_ports(pdev);
+err_init_ports:
+	if (dss.video1_pll)
+		dss_video_pll_uninit(dss.video1_pll);
+
+	if (dss.video2_pll)
+		dss_video_pll_uninit(dss.video2_pll);
+err_pll_init:
+err_setup_clocks:
+	dss_put_clocks();
+	return r;
+}
+
+static void dss_unbind(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	dss_initialized = false;
+
+	component_unbind_all(&pdev->dev, NULL);
+
+	if (dss.video1_pll)
+		dss_video_pll_uninit(dss.video1_pll);
+
+	if (dss.video2_pll)
+		dss_video_pll_uninit(dss.video2_pll);
+
+	dss_uninit_ports(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+
+	dss_put_clocks();
+}
+
+static const struct component_master_ops dss_component_ops = {
+	.bind = dss_bind,
+	.unbind = dss_unbind,
+};
+
+static int dss_component_compare(struct device *dev, void *data)
+{
+	struct device *child = data;
+	return dev == child;
+}
+
+static int dss_add_child_component(struct device *dev, void *data)
+{
+	struct component_match **match = data;
+
+	/*
+	 * HACK
+	 * We don't have a working driver for rfbi, so skip it here always.
+	 * Otherwise dss will never get probed successfully, as it will wait
+	 * for rfbi to get probed.
+	 */
+	if (strstr(dev_name(dev), "rfbi"))
+		return 0;
+
+	component_match_add(dev->parent, match, dss_component_compare, dev);
+
+	return 0;
+}
+
+static int dss_probe(struct platform_device *pdev)
+{
+	struct component_match *match = NULL;
+	int r;
+
+	/* add all the child devices as components */
+	device_for_each_child(&pdev->dev, &match, dss_add_child_component);
+
+	r = component_master_add_with_match(&pdev->dev, &dss_component_ops, match);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static int dss_remove(struct platform_device *pdev)
+{
+	component_master_del(&pdev->dev, &dss_component_ops);
+	return 0;
+}
+
+static int dss_runtime_suspend(struct device *dev)
+{
+	dss_save_context();
+	dss_set_min_bus_tput(dev, 0);
+
+	pinctrl_pm_select_sleep_state(dev);
+
+	return 0;
+}
+
+static int dss_runtime_resume(struct device *dev)
+{
+	int r;
+
+	pinctrl_pm_select_default_state(dev);
+
+	/*
+	 * Set an arbitrarily high tput request to ensure OPP100.
+	 * What we should really do is to make a request to stay in OPP100,
+	 * without any tput requirements, but that is not currently possible
+	 * via the PM layer.
+	 */
+
+	r = dss_set_min_bus_tput(dev, 1000000000);
+	if (r)
+		return r;
+
+	dss_restore_context();
+	return 0;
+}
+
+static const struct dev_pm_ops dss_pm_ops = {
+	.runtime_suspend = dss_runtime_suspend,
+	.runtime_resume = dss_runtime_resume,
+};
+
+static const struct of_device_id dss_of_match[] = {
+	{ .compatible = "ti,omap2-dss", },
+	{ .compatible = "ti,omap3-dss", },
+	{ .compatible = "ti,omap4-dss", },
+	{ .compatible = "ti,omap5-dss", },
+	{ .compatible = "ti,dra7-dss", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, dss_of_match);
+
+static struct platform_driver omap_dsshw_driver = {
+	.probe		= dss_probe,
+	.remove		= dss_remove,
+	.driver         = {
+		.name   = "omapdss_dss",
+		.pm	= &dss_pm_ops,
+		.of_match_table = dss_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init dss_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_dsshw_driver);
+}
+
+void dss_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss.h b/drivers/video/fbdev/omap2/omapfb/dss/dss.h
new file mode 100644
index 000000000000..b9066afee301
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss.h
@@ -0,0 +1,468 @@
+/*
+ * linux/drivers/video/omap2/dss/dss.h
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __OMAP2_DSS_H
+#define __OMAP2_DSS_H
+
+#include <linux/interrupt.h>
+
+#ifdef pr_fmt
+#undef pr_fmt
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define pr_fmt(fmt) DSS_SUBSYS_NAME ": " fmt
+#else
+#define pr_fmt(fmt) fmt
+#endif
+
+#define DSSDBG(format, ...) \
+	pr_debug(format, ## __VA_ARGS__)
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSERR(format, ...) \
+	printk(KERN_ERR "omapdss " DSS_SUBSYS_NAME " error: " format, \
+	## __VA_ARGS__)
+#else
+#define DSSERR(format, ...) \
+	printk(KERN_ERR "omapdss error: " format, ## __VA_ARGS__)
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSINFO(format, ...) \
+	printk(KERN_INFO "omapdss " DSS_SUBSYS_NAME ": " format, \
+	## __VA_ARGS__)
+#else
+#define DSSINFO(format, ...) \
+	printk(KERN_INFO "omapdss: " format, ## __VA_ARGS__)
+#endif
+
+#ifdef DSS_SUBSYS_NAME
+#define DSSWARN(format, ...) \
+	printk(KERN_WARNING "omapdss " DSS_SUBSYS_NAME ": " format, \
+	## __VA_ARGS__)
+#else
+#define DSSWARN(format, ...) \
+	printk(KERN_WARNING "omapdss: " format, ## __VA_ARGS__)
+#endif
+
+/* OMAP TRM gives bitfields as start:end, where start is the higher bit
+   number. For example 7:0 */
+#define FLD_MASK(start, end)	(((1 << ((start) - (end) + 1)) - 1) << (end))
+#define FLD_VAL(val, start, end) (((val) << (end)) & FLD_MASK(start, end))
+#define FLD_GET(val, start, end) (((val) & FLD_MASK(start, end)) >> (end))
+#define FLD_MOD(orig, val, start, end) \
+	(((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
+
+enum dss_io_pad_mode {
+	DSS_IO_PAD_MODE_RESET,
+	DSS_IO_PAD_MODE_RFBI,
+	DSS_IO_PAD_MODE_BYPASS,
+};
+
+enum dss_hdmi_venc_clk_source_select {
+	DSS_VENC_TV_CLK = 0,
+	DSS_HDMI_M_PCLK = 1,
+};
+
+enum dss_dsi_content_type {
+	DSS_DSI_CONTENT_DCS,
+	DSS_DSI_CONTENT_GENERIC,
+};
+
+enum dss_writeback_channel {
+	DSS_WB_LCD1_MGR =	0,
+	DSS_WB_LCD2_MGR =	1,
+	DSS_WB_TV_MGR =		2,
+	DSS_WB_OVL0 =		3,
+	DSS_WB_OVL1 =		4,
+	DSS_WB_OVL2 =		5,
+	DSS_WB_OVL3 =		6,
+	DSS_WB_LCD3_MGR =	7,
+};
+
+enum dss_pll_id {
+	DSS_PLL_DSI1,
+	DSS_PLL_DSI2,
+	DSS_PLL_HDMI,
+	DSS_PLL_VIDEO1,
+	DSS_PLL_VIDEO2,
+};
+
+struct dss_pll;
+
+#define DSS_PLL_MAX_HSDIVS 4
+
+/*
+ * Type-A PLLs: clkout[]/mX[] refer to hsdiv outputs m4, m5, m6, m7.
+ * Type-B PLLs: clkout[0] refers to m2.
+ */
+struct dss_pll_clock_info {
+	/* rates that we get with dividers below */
+	unsigned long fint;
+	unsigned long clkdco;
+	unsigned long clkout[DSS_PLL_MAX_HSDIVS];
+
+	/* dividers */
+	u16 n;
+	u16 m;
+	u32 mf;
+	u16 mX[DSS_PLL_MAX_HSDIVS];
+	u16 sd;
+};
+
+struct dss_pll_ops {
+	int (*enable)(struct dss_pll *pll);
+	void (*disable)(struct dss_pll *pll);
+	int (*set_config)(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo);
+};
+
+struct dss_pll_hw {
+	unsigned n_max;
+	unsigned m_min;
+	unsigned m_max;
+	unsigned mX_max;
+
+	unsigned long fint_min, fint_max;
+	unsigned long clkdco_min, clkdco_low, clkdco_max;
+
+	u8 n_msb, n_lsb;
+	u8 m_msb, m_lsb;
+	u8 mX_msb[DSS_PLL_MAX_HSDIVS], mX_lsb[DSS_PLL_MAX_HSDIVS];
+
+	bool has_stopmode;
+	bool has_freqsel;
+	bool has_selfreqdco;
+	bool has_refsel;
+};
+
+struct dss_pll {
+	const char *name;
+	enum dss_pll_id id;
+
+	struct clk *clkin;
+	struct regulator *regulator;
+
+	void __iomem *base;
+
+	const struct dss_pll_hw *hw;
+
+	const struct dss_pll_ops *ops;
+
+	struct dss_pll_clock_info cinfo;
+};
+
+struct dispc_clock_info {
+	/* rates that we get with dividers below */
+	unsigned long lck;
+	unsigned long pck;
+
+	/* dividers */
+	u16 lck_div;
+	u16 pck_div;
+};
+
+struct dss_lcd_mgr_config {
+	enum dss_io_pad_mode io_pad_mode;
+
+	bool stallmode;
+	bool fifohandcheck;
+
+	struct dispc_clock_info clock_info;
+
+	int video_port_width;
+
+	int lcden_sig_polarity;
+};
+
+struct seq_file;
+struct platform_device;
+
+/* core */
+struct platform_device *dss_get_core_pdev(void);
+int dss_dsi_enable_pads(int dsi_id, unsigned lane_mask);
+void dss_dsi_disable_pads(int dsi_id, unsigned lane_mask);
+int dss_set_min_bus_tput(struct device *dev, unsigned long tput);
+int dss_debugfs_create_file(const char *name, void (*write)(struct seq_file *));
+
+/* display */
+int dss_suspend_all_devices(void);
+int dss_resume_all_devices(void);
+void dss_disable_all_devices(void);
+
+int display_init_sysfs(struct platform_device *pdev);
+void display_uninit_sysfs(struct platform_device *pdev);
+
+/* manager */
+int dss_init_overlay_managers(void);
+void dss_uninit_overlay_managers(void);
+int dss_init_overlay_managers_sysfs(struct platform_device *pdev);
+void dss_uninit_overlay_managers_sysfs(struct platform_device *pdev);
+int dss_mgr_simple_check(struct omap_overlay_manager *mgr,
+		const struct omap_overlay_manager_info *info);
+int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
+		const struct omap_video_timings *timings);
+int dss_mgr_check(struct omap_overlay_manager *mgr,
+		struct omap_overlay_manager_info *info,
+		const struct omap_video_timings *mgr_timings,
+		const struct dss_lcd_mgr_config *config,
+		struct omap_overlay_info **overlay_infos);
+
+static inline bool dss_mgr_is_lcd(enum omap_channel id)
+{
+	if (id == OMAP_DSS_CHANNEL_LCD || id == OMAP_DSS_CHANNEL_LCD2 ||
+			id == OMAP_DSS_CHANNEL_LCD3)
+		return true;
+	else
+		return false;
+}
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+		struct platform_device *pdev);
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr);
+
+/* overlay */
+void dss_init_overlays(struct platform_device *pdev);
+void dss_uninit_overlays(struct platform_device *pdev);
+void dss_overlay_setup_dispc_manager(struct omap_overlay_manager *mgr);
+int dss_ovl_simple_check(struct omap_overlay *ovl,
+		const struct omap_overlay_info *info);
+int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
+		const struct omap_video_timings *mgr_timings);
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+		enum omap_color_mode mode);
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+		struct platform_device *pdev);
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl);
+
+/* DSS */
+int dss_init_platform_driver(void) __init;
+void dss_uninit_platform_driver(void);
+
+int dss_runtime_get(void);
+void dss_runtime_put(void);
+
+unsigned long dss_get_dispc_clk_rate(void);
+int dss_dpi_select_source(int port, enum omap_channel channel);
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
+enum dss_hdmi_venc_clk_source_select dss_get_hdmi_venc_clk_source(void);
+const char *dss_get_generic_clk_source_name(enum omap_dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
+
+/* DSS VIDEO PLL */
+struct dss_pll *dss_video_pll_init(struct platform_device *pdev, int id,
+	struct regulator *regulator);
+void dss_video_pll_uninit(struct dss_pll *pll);
+
+/* dss-of */
+struct device_node *dss_of_port_get_parent_device(struct device_node *port);
+u32 dss_of_port_get_port_number(struct device_node *port);
+
+#if defined(CONFIG_FB_OMAP2_DSS_DEBUGFS)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
+
+void dss_ctrl_pll_enable(enum dss_pll_id pll_id, bool enable);
+void dss_ctrl_pll_set_control_mux(enum dss_pll_id pll_id,
+	enum omap_channel channel);
+
+void dss_sdi_init(int datapairs);
+int dss_sdi_enable(void);
+void dss_sdi_disable(void);
+
+void dss_select_dsi_clk_source(int dsi_module,
+		enum omap_dss_clk_source clk_src);
+void dss_select_lcd_clk_source(enum omap_channel channel,
+		enum omap_dss_clk_source clk_src);
+enum omap_dss_clk_source dss_get_dispc_clk_source(void);
+enum omap_dss_clk_source dss_get_dsi_clk_source(int dsi_module);
+enum omap_dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel);
+
+void dss_set_venc_output(enum omap_dss_venc_type type);
+void dss_set_dac_pwrdn_bgz(bool enable);
+
+int dss_set_fck_rate(unsigned long rate);
+
+typedef bool (*dss_div_calc_func)(unsigned long fck, void *data);
+bool dss_div_calc(unsigned long pck, unsigned long fck_min,
+		dss_div_calc_func func, void *data);
+
+/* SDI */
+int sdi_init_platform_driver(void) __init;
+void sdi_uninit_platform_driver(void);
+
+#ifdef CONFIG_FB_OMAP2_DSS_SDI
+int sdi_init_port(struct platform_device *pdev, struct device_node *port);
+void sdi_uninit_port(struct device_node *port);
+#else
+static inline int sdi_init_port(struct platform_device *pdev,
+		struct device_node *port)
+{
+	return 0;
+}
+static inline void sdi_uninit_port(struct device_node *port)
+{
+}
+#endif
+
+/* DSI */
+
+#ifdef CONFIG_FB_OMAP2_DSS_DSI
+
+struct dentry;
+struct file_operations;
+
+int dsi_init_platform_driver(void) __init;
+void dsi_uninit_platform_driver(void);
+
+void dsi_dump_clocks(struct seq_file *s);
+
+void dsi_irq_handler(void);
+u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt);
+
+#else
+static inline u8 dsi_get_pixel_size(enum omap_dss_dsi_pixel_format fmt)
+{
+	WARN(1, "%s: DSI not compiled in, returning pixel_size as 0\n",
+	     __func__);
+	return 0;
+}
+#endif
+
+/* DPI */
+int dpi_init_platform_driver(void) __init;
+void dpi_uninit_platform_driver(void);
+
+#ifdef CONFIG_FB_OMAP2_DSS_DPI
+int dpi_init_port(struct platform_device *pdev, struct device_node *port);
+void dpi_uninit_port(struct device_node *port);
+#else
+static inline int dpi_init_port(struct platform_device *pdev,
+		struct device_node *port)
+{
+	return 0;
+}
+static inline void dpi_uninit_port(struct device_node *port)
+{
+}
+#endif
+
+/* DISPC */
+int dispc_init_platform_driver(void) __init;
+void dispc_uninit_platform_driver(void);
+void dispc_dump_clocks(struct seq_file *s);
+
+void dispc_enable_sidle(void);
+void dispc_disable_sidle(void);
+
+void dispc_lcd_enable_signal(bool enable);
+void dispc_pck_free_enable(bool enable);
+void dispc_enable_fifomerge(bool enable);
+void dispc_enable_gamma_table(bool enable);
+
+typedef bool (*dispc_div_calc_func)(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data);
+bool dispc_div_calc(unsigned long dispc,
+		unsigned long pck_min, unsigned long pck_max,
+		dispc_div_calc_func func, void *data);
+
+bool dispc_mgr_timings_ok(enum omap_channel channel,
+		const struct omap_video_timings *timings);
+int dispc_calc_clock_rates(unsigned long dispc_fclk_rate,
+		struct dispc_clock_info *cinfo);
+
+
+void dispc_ovl_set_fifo_threshold(enum omap_plane plane, u32 low, u32 high);
+void dispc_ovl_compute_fifo_thresholds(enum omap_plane plane,
+		u32 *fifo_low, u32 *fifo_high, bool use_fifomerge,
+		bool manual_update);
+
+void dispc_mgr_set_clock_div(enum omap_channel channel,
+		const struct dispc_clock_info *cinfo);
+int dispc_mgr_get_clock_div(enum omap_channel channel,
+		struct dispc_clock_info *cinfo);
+void dispc_set_tv_pclk(unsigned long pclk);
+
+u32 dispc_wb_get_framedone_irq(void);
+bool dispc_wb_go_busy(void);
+void dispc_wb_go(void);
+void dispc_wb_enable(bool enable);
+bool dispc_wb_is_enabled(void);
+void dispc_wb_set_channel_in(enum dss_writeback_channel channel);
+int dispc_wb_setup(const struct omap_dss_writeback_info *wi,
+		bool mem_to_mem, const struct omap_video_timings *timings);
+
+/* VENC */
+int venc_init_platform_driver(void) __init;
+void venc_uninit_platform_driver(void);
+
+/* HDMI */
+int hdmi4_init_platform_driver(void) __init;
+void hdmi4_uninit_platform_driver(void);
+
+int hdmi5_init_platform_driver(void) __init;
+void hdmi5_uninit_platform_driver(void);
+
+/* RFBI */
+int rfbi_init_platform_driver(void) __init;
+void rfbi_uninit_platform_driver(void);
+
+
+#ifdef CONFIG_FB_OMAP2_DSS_COLLECT_IRQ_STATS
+static inline void dss_collect_irq_stats(u32 irqstatus, unsigned *irq_arr)
+{
+	int b;
+	for (b = 0; b < 32; ++b) {
+		if (irqstatus & (1 << b))
+			irq_arr[b]++;
+	}
+}
+#endif
+
+/* PLL */
+typedef bool (*dss_pll_calc_func)(int n, int m, unsigned long fint,
+		unsigned long clkdco, void *data);
+typedef bool (*dss_hsdiv_calc_func)(int m_dispc, unsigned long dispc,
+		void *data);
+
+int dss_pll_register(struct dss_pll *pll);
+void dss_pll_unregister(struct dss_pll *pll);
+struct dss_pll *dss_pll_find(const char *name);
+int dss_pll_enable(struct dss_pll *pll);
+void dss_pll_disable(struct dss_pll *pll);
+int dss_pll_set_config(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo);
+
+bool dss_pll_hsdiv_calc(const struct dss_pll *pll, unsigned long clkdco,
+		unsigned long out_min, unsigned long out_max,
+		dss_hsdiv_calc_func func, void *data);
+bool dss_pll_calc(const struct dss_pll *pll, unsigned long clkin,
+		unsigned long pll_min, unsigned long pll_max,
+		dss_pll_calc_func func, void *data);
+int dss_pll_write_config_type_a(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo);
+int dss_pll_write_config_type_b(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo);
+int dss_pll_wait_reset_done(struct dss_pll *pll);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c
new file mode 100644
index 000000000000..c886a2927f73
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.c
@@ -0,0 +1,951 @@
+/*
+ * linux/drivers/video/omap2/dss/dss_features.c
+ *
+ * Copyright (C) 2010 Texas Instruments
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+/* Defines a generic omap register field */
+struct dss_reg_field {
+	u8 start, end;
+};
+
+struct dss_param_range {
+	int min, max;
+};
+
+struct omap_dss_features {
+	const struct dss_reg_field *reg_fields;
+	const int num_reg_fields;
+
+	const enum dss_feat_id *features;
+	const int num_features;
+
+	const int num_mgrs;
+	const int num_ovls;
+	const enum omap_display_type *supported_displays;
+	const enum omap_dss_output_id *supported_outputs;
+	const enum omap_color_mode *supported_color_modes;
+	const enum omap_overlay_caps *overlay_caps;
+	const char * const *clksrc_names;
+	const struct dss_param_range *dss_params;
+
+	const enum omap_dss_rotation_type supported_rotation_types;
+
+	const u32 buffer_size_unit;
+	const u32 burst_size_unit;
+};
+
+/* This struct is assigned to one of the below during initialization */
+static const struct omap_dss_features *omap_current_dss_features;
+
+static const struct dss_reg_field omap2_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 11, 0 },
+	[FEAT_REG_FIRVINC]			= { 27, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 8, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 24, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 8, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+};
+
+static const struct dss_reg_field omap3_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 11, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 27, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 10, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+};
+
+static const struct dss_reg_field am43xx_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]	= { 11, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 27, 16 },
+	[FEAT_REG_FIFOSIZE]		= { 10, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 9, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 25, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 0, 0 },
+};
+
+static const struct dss_reg_field omap4_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 15, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 31, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 15, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 10, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 26, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 9, 8 },
+};
+
+static const struct dss_reg_field omap5_dss_reg_fields[] = {
+	[FEAT_REG_FIRHINC]			= { 12, 0 },
+	[FEAT_REG_FIRVINC]			= { 28, 16 },
+	[FEAT_REG_FIFOLOWTHRESHOLD]		= { 15, 0 },
+	[FEAT_REG_FIFOHIGHTHRESHOLD]		= { 31, 16 },
+	[FEAT_REG_FIFOSIZE]			= { 15, 0 },
+	[FEAT_REG_HORIZONTALACCU]		= { 10, 0 },
+	[FEAT_REG_VERTICALACCU]			= { 26, 16 },
+	[FEAT_REG_DISPC_CLK_SWITCH]		= { 9, 7 },
+};
+
+static const enum omap_display_type omap2_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3430_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3630_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type am43xx_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+};
+
+static const enum omap_display_type omap4_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DBI | OMAP_DISPLAY_TYPE_DSI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_VENC | OMAP_DISPLAY_TYPE_HDMI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+};
+
+static const enum omap_display_type omap5_dss_supported_displays[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DISPLAY_TYPE_HDMI | OMAP_DISPLAY_TYPE_DPI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
+	OMAP_DISPLAY_TYPE_DSI,
+};
+
+static const enum omap_dss_output_id omap2_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3430_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_SDI | OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id omap3630_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC,
+};
+
+static const enum omap_dss_output_id am43xx_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI,
+};
+
+static const enum omap_dss_output_id omap4_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DBI | OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_VENC | OMAP_DSS_OUTPUT_HDMI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI2,
+};
+
+static const enum omap_dss_output_id omap5_dss_supported_outputs[] = {
+	/* OMAP_DSS_CHANNEL_LCD */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1 | OMAP_DSS_OUTPUT_DSI2,
+
+	/* OMAP_DSS_CHANNEL_DIGIT */
+	OMAP_DSS_OUTPUT_HDMI,
+
+	/* OMAP_DSS_CHANNEL_LCD2 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI1,
+
+	/* OMAP_DSS_CHANNEL_LCD3 */
+	OMAP_DSS_OUTPUT_DPI | OMAP_DSS_OUTPUT_DBI |
+	OMAP_DSS_OUTPUT_DSI2,
+};
+
+static const enum omap_color_mode omap2_dss_supported_color_modes[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
+	OMAP_DSS_COLOR_CLUT4 | OMAP_DSS_COLOR_CLUT8 |
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_RGB16 |
+	OMAP_DSS_COLOR_RGB24U | OMAP_DSS_COLOR_RGB24P,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+	OMAP_DSS_COLOR_UYVY,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+	OMAP_DSS_COLOR_UYVY,
+};
+
+static const enum omap_color_mode omap3_dss_supported_color_modes[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
+	OMAP_DSS_COLOR_CLUT4 | OMAP_DSS_COLOR_CLUT8 |
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_ARGB32 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_COLOR_RGB24U | OMAP_DSS_COLOR_RGB24P |
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_RGB16 |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_UYVY,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_YUV2 |
+	OMAP_DSS_COLOR_UYVY | OMAP_DSS_COLOR_ARGB32 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
+};
+
+static const enum omap_color_mode omap4_dss_supported_color_modes[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_COLOR_CLUT1 | OMAP_DSS_COLOR_CLUT2 |
+	OMAP_DSS_COLOR_CLUT4 | OMAP_DSS_COLOR_CLUT8 |
+	OMAP_DSS_COLOR_RGB12U | OMAP_DSS_COLOR_ARGB16 |
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_ARGB32 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32 |
+	OMAP_DSS_COLOR_ARGB16_1555 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_XRGB16_1555,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+
+       /* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+
+	/* OMAP_DSS_VIDEO3 */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+
+	/* OMAP_DSS_WB */
+	OMAP_DSS_COLOR_RGB16 | OMAP_DSS_COLOR_RGB12U |
+	OMAP_DSS_COLOR_YUV2 | OMAP_DSS_COLOR_ARGB16_1555 |
+	OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_NV12 |
+	OMAP_DSS_COLOR_RGBA16 | OMAP_DSS_COLOR_RGB24U |
+	OMAP_DSS_COLOR_RGB24P | OMAP_DSS_COLOR_UYVY |
+	OMAP_DSS_COLOR_ARGB16 | OMAP_DSS_COLOR_XRGB16_1555 |
+	OMAP_DSS_COLOR_ARGB32 | OMAP_DSS_COLOR_RGBX16 |
+	OMAP_DSS_COLOR_RGBX32,
+};
+
+static const enum omap_overlay_caps omap2_dss_overlay_caps[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap3430_dss_overlay_caps[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap3630_dss_overlay_caps[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const enum omap_overlay_caps omap4_dss_overlay_caps[] = {
+	/* OMAP_DSS_GFX */
+	OMAP_DSS_OVL_CAP_GLOBAL_ALPHA | OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA |
+		OMAP_DSS_OVL_CAP_ZORDER | OMAP_DSS_OVL_CAP_POS |
+		OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO1 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO2 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+
+	/* OMAP_DSS_VIDEO3 */
+	OMAP_DSS_OVL_CAP_SCALE | OMAP_DSS_OVL_CAP_GLOBAL_ALPHA |
+		OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA | OMAP_DSS_OVL_CAP_ZORDER |
+		OMAP_DSS_OVL_CAP_POS | OMAP_DSS_OVL_CAP_REPLICATION,
+};
+
+static const char * const omap2_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "N/A",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "N/A",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCLK1",
+};
+
+static const char * const omap3_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DSI1_PLL_FCLK",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DSI2_PLL_FCLK",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS1_ALWON_FCLK",
+};
+
+static const char * const omap4_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "PLL1_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "PLL1_CLK2",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_FCLK",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC]	= "PLL2_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "PLL2_CLK2",
+};
+
+static const char * const omap5_dss_clk_source_names[] = {
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]	= "DPLL_DSI1_A_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]	= "DPLL_DSI1_A_CLK2",
+	[OMAP_DSS_CLK_SRC_FCK]			= "DSS_CLK",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DISPC]	= "DPLL_DSI1_C_CLK1",
+	[OMAP_DSS_CLK_SRC_DSI2_PLL_HSDIV_DSI]	= "DPLL_DSI1_C_CLK2",
+};
+
+static const struct dss_param_range omap2_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 133000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 2, 255 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 2 },
+	/*
+	 * Assuming the line width buffer to be 768 pixels as OMAP2 DISPC
+	 * scaler cannot scale a image with width more than 768.
+	 */
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 768 },
+};
+
+static const struct dss_param_range omap3_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 173000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 1, (1 << 13) - 1},
+	[FEAT_PARAM_DSI_FCK]			= { 0, 173000000 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 1024 },
+};
+
+static const struct dss_param_range am43xx_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 200000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 1024 },
+};
+
+static const struct dss_param_range omap4_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 186000000 },
+	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
+	[FEAT_PARAM_DSI_FCK]			= { 0, 170000000 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 2048 },
+};
+
+static const struct dss_param_range omap5_dss_param_range[] = {
+	[FEAT_PARAM_DSS_FCK]			= { 0, 209250000 },
+	[FEAT_PARAM_DSS_PCD]			= { 1, 255 },
+	[FEAT_PARAM_DSIPLL_LPDIV]		= { 0, (1 << 13) - 1 },
+	[FEAT_PARAM_DSI_FCK]			= { 0, 209250000 },
+	[FEAT_PARAM_DOWNSCALE]			= { 1, 4 },
+	[FEAT_PARAM_LINEWIDTH]			= { 1, 2048 },
+};
+
+static const enum dss_feat_id omap2_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+};
+
+static const enum dss_feat_id omap3430_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_DSI_REVERSE_TXCLKESC,
+	FEAT_VENC_REQUIRES_TV_DAC_CLK,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_OMAP3_DSI_FIFO_BUG,
+	FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id am35xx_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_DSI_REVERSE_TXCLKESC,
+	FEAT_VENC_REQUIRES_TV_DAC_CLK,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_OMAP3_DSI_FIFO_BUG,
+};
+
+static const enum dss_feat_id am43xx_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+};
+
+static const enum dss_feat_id omap3630_dss_feat_list[] = {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	FEAT_DSI_PLL_PWR_BUG,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_OMAP3_DSI_FIFO_BUG,
+	FEAT_DPI_USES_VDDS_DSI,
+};
+
+static const enum dss_feat_id omap4430_es1_0_dss_feat_list[] = {
+	FEAT_MGR_LCD2,
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_GNQ,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap4430_es2_0_1_2_dss_feat_list[] = {
+	FEAT_MGR_LCD2,
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_GNQ,
+	FEAT_HDMI_CTS_SWMODE,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap4_dss_feat_list[] = {
+	FEAT_MGR_LCD2,
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_GNQ,
+	FEAT_HDMI_CTS_SWMODE,
+	FEAT_HDMI_AUDIO_USE_MCLK,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_BURST_2D,
+};
+
+static const enum dss_feat_id omap5_dss_feat_list[] = {
+	FEAT_MGR_LCD2,
+	FEAT_MGR_LCD3,
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_GNQ,
+	FEAT_HDMI_CTS_SWMODE,
+	FEAT_HDMI_AUDIO_USE_MCLK,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	FEAT_BURST_2D,
+	FEAT_DSI_PHY_DCC,
+	FEAT_MFLAG,
+};
+
+/* OMAP2 DSS Features */
+static const struct omap_dss_features omap2_dss_features = {
+	.reg_fields = omap2_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap2_dss_reg_fields),
+
+	.features = omap2_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap2_dss_feat_list),
+
+	.num_mgrs = 2,
+	.num_ovls = 3,
+	.supported_displays = omap2_dss_supported_displays,
+	.supported_outputs = omap2_dss_supported_outputs,
+	.supported_color_modes = omap2_dss_supported_color_modes,
+	.overlay_caps = omap2_dss_overlay_caps,
+	.clksrc_names = omap2_dss_clk_source_names,
+	.dss_params = omap2_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+/* OMAP3 DSS Features */
+static const struct omap_dss_features omap3430_dss_features = {
+	.reg_fields = omap3_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+	.features = omap3430_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap3430_dss_feat_list),
+
+	.num_mgrs = 2,
+	.num_ovls = 3,
+	.supported_displays = omap3430_dss_supported_displays,
+	.supported_outputs = omap3430_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3430_dss_overlay_caps,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+/*
+ * AM35xx DSS Features. This is basically OMAP3 DSS Features without the
+ * vdds_dsi regulator.
+ */
+static const struct omap_dss_features am35xx_dss_features = {
+	.reg_fields = omap3_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+	.features = am35xx_dss_feat_list,
+	.num_features = ARRAY_SIZE(am35xx_dss_feat_list),
+
+	.num_mgrs = 2,
+	.num_ovls = 3,
+	.supported_displays = omap3430_dss_supported_displays,
+	.supported_outputs = omap3430_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3430_dss_overlay_caps,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+static const struct omap_dss_features am43xx_dss_features = {
+	.reg_fields = am43xx_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(am43xx_dss_reg_fields),
+
+	.features = am43xx_dss_feat_list,
+	.num_features = ARRAY_SIZE(am43xx_dss_feat_list),
+
+	.num_mgrs = 1,
+	.num_ovls = 3,
+	.supported_displays = am43xx_dss_supported_displays,
+	.supported_outputs = am43xx_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3430_dss_overlay_caps,
+	.clksrc_names = omap2_dss_clk_source_names,
+	.dss_params = am43xx_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+static const struct omap_dss_features omap3630_dss_features = {
+	.reg_fields = omap3_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+
+	.features = omap3630_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap3630_dss_feat_list),
+
+	.num_mgrs = 2,
+	.num_ovls = 3,
+	.supported_displays = omap3630_dss_supported_displays,
+	.supported_outputs = omap3630_dss_supported_outputs,
+	.supported_color_modes = omap3_dss_supported_color_modes,
+	.overlay_caps = omap3630_dss_overlay_caps,
+	.clksrc_names = omap3_dss_clk_source_names,
+	.dss_params = omap3_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_VRFB,
+	.buffer_size_unit = 1,
+	.burst_size_unit = 8,
+};
+
+/* OMAP4 DSS Features */
+/* For OMAP4430 ES 1.0 revision */
+static const struct omap_dss_features omap4430_es1_0_dss_features  = {
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+	.features = omap4430_es1_0_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap4430_es1_0_dss_feat_list),
+
+	.num_mgrs = 3,
+	.num_ovls = 4,
+	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.overlay_caps = omap4_dss_overlay_caps,
+	.clksrc_names = omap4_dss_clk_source_names,
+	.dss_params = omap4_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
+/* For OMAP4430 ES 2.0, 2.1 and 2.2 revisions */
+static const struct omap_dss_features omap4430_es2_0_1_2_dss_features = {
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+	.features = omap4430_es2_0_1_2_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap4430_es2_0_1_2_dss_feat_list),
+
+	.num_mgrs = 3,
+	.num_ovls = 4,
+	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.overlay_caps = omap4_dss_overlay_caps,
+	.clksrc_names = omap4_dss_clk_source_names,
+	.dss_params = omap4_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
+/* For all the other OMAP4 versions */
+static const struct omap_dss_features omap4_dss_features = {
+	.reg_fields = omap4_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
+
+	.features = omap4_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap4_dss_feat_list),
+
+	.num_mgrs = 3,
+	.num_ovls = 4,
+	.supported_displays = omap4_dss_supported_displays,
+	.supported_outputs = omap4_dss_supported_outputs,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.overlay_caps = omap4_dss_overlay_caps,
+	.clksrc_names = omap4_dss_clk_source_names,
+	.dss_params = omap4_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
+/* OMAP5 DSS Features */
+static const struct omap_dss_features omap5_dss_features = {
+	.reg_fields = omap5_dss_reg_fields,
+	.num_reg_fields = ARRAY_SIZE(omap5_dss_reg_fields),
+
+	.features = omap5_dss_feat_list,
+	.num_features = ARRAY_SIZE(omap5_dss_feat_list),
+
+	.num_mgrs = 4,
+	.num_ovls = 4,
+	.supported_displays = omap5_dss_supported_displays,
+	.supported_outputs = omap5_dss_supported_outputs,
+	.supported_color_modes = omap4_dss_supported_color_modes,
+	.overlay_caps = omap4_dss_overlay_caps,
+	.clksrc_names = omap5_dss_clk_source_names,
+	.dss_params = omap5_dss_param_range,
+	.supported_rotation_types = OMAP_DSS_ROT_DMA | OMAP_DSS_ROT_TILER,
+	.buffer_size_unit = 16,
+	.burst_size_unit = 16,
+};
+
+/* Functions returning values related to a DSS feature */
+int dss_feat_get_num_mgrs(void)
+{
+	return omap_current_dss_features->num_mgrs;
+}
+EXPORT_SYMBOL(dss_feat_get_num_mgrs);
+
+int dss_feat_get_num_ovls(void)
+{
+	return omap_current_dss_features->num_ovls;
+}
+EXPORT_SYMBOL(dss_feat_get_num_ovls);
+
+unsigned long dss_feat_get_param_min(enum dss_range_param param)
+{
+	return omap_current_dss_features->dss_params[param].min;
+}
+
+unsigned long dss_feat_get_param_max(enum dss_range_param param)
+{
+	return omap_current_dss_features->dss_params[param].max;
+}
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
+{
+	return omap_current_dss_features->supported_displays[channel];
+}
+
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel)
+{
+	return omap_current_dss_features->supported_outputs[channel];
+}
+
+enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane)
+{
+	return omap_current_dss_features->supported_color_modes[plane];
+}
+EXPORT_SYMBOL(dss_feat_get_supported_color_modes);
+
+enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane)
+{
+	return omap_current_dss_features->overlay_caps[plane];
+}
+
+bool dss_feat_color_mode_supported(enum omap_plane plane,
+		enum omap_color_mode color_mode)
+{
+	return omap_current_dss_features->supported_color_modes[plane] &
+			color_mode;
+}
+
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id)
+{
+	return omap_current_dss_features->clksrc_names[id];
+}
+
+u32 dss_feat_get_buffer_size_unit(void)
+{
+	return omap_current_dss_features->buffer_size_unit;
+}
+
+u32 dss_feat_get_burst_size_unit(void)
+{
+	return omap_current_dss_features->burst_size_unit;
+}
+
+/* DSS has_feature check */
+bool dss_has_feature(enum dss_feat_id id)
+{
+	int i;
+	const enum dss_feat_id *features = omap_current_dss_features->features;
+	const int num_features = omap_current_dss_features->num_features;
+
+	for (i = 0; i < num_features; i++) {
+		if (features[i] == id)
+			return true;
+	}
+
+	return false;
+}
+
+void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end)
+{
+	if (id >= omap_current_dss_features->num_reg_fields)
+		BUG();
+
+	*start = omap_current_dss_features->reg_fields[id].start;
+	*end = omap_current_dss_features->reg_fields[id].end;
+}
+
+bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type)
+{
+	return omap_current_dss_features->supported_rotation_types & rot_type;
+}
+
+void dss_features_init(enum omapdss_version version)
+{
+	switch (version) {
+	case OMAPDSS_VER_OMAP24xx:
+		omap_current_dss_features = &omap2_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP34xx_ES1:
+	case OMAPDSS_VER_OMAP34xx_ES3:
+		omap_current_dss_features = &omap3430_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP3630:
+		omap_current_dss_features = &omap3630_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP4430_ES1:
+		omap_current_dss_features = &omap4430_es1_0_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP4430_ES2:
+		omap_current_dss_features = &omap4430_es2_0_1_2_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP4:
+		omap_current_dss_features = &omap4_dss_features;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+	case OMAPDSS_VER_DRA7xx:
+		omap_current_dss_features = &omap5_dss_features;
+		break;
+
+	case OMAPDSS_VER_AM35xx:
+		omap_current_dss_features = &am35xx_dss_features;
+		break;
+
+	case OMAPDSS_VER_AM43xx:
+		omap_current_dss_features = &am43xx_dss_features;
+		break;
+
+	default:
+		DSSWARN("Unsupported OMAP version");
+		break;
+	}
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h
new file mode 100644
index 000000000000..3d67d39f192f
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/dss_features.h
@@ -0,0 +1,108 @@
+/*
+ * linux/drivers/video/omap2/dss/dss_features.h
+ *
+ * Copyright (C) 2010 Texas Instruments
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 __OMAP2_DSS_FEATURES_H
+#define __OMAP2_DSS_FEATURES_H
+
+#define MAX_DSS_MANAGERS	4
+#define MAX_DSS_OVERLAYS	4
+#define MAX_DSS_LCD_MANAGERS	3
+#define MAX_NUM_DSI		2
+
+/* DSS has feature id */
+enum dss_feat_id {
+	FEAT_LCDENABLEPOL,
+	FEAT_LCDENABLESIGNAL,
+	FEAT_PCKFREEENABLE,
+	FEAT_FUNCGATED,
+	FEAT_MGR_LCD2,
+	FEAT_MGR_LCD3,
+	FEAT_LINEBUFFERSPLIT,
+	FEAT_ROWREPEATENABLE,
+	FEAT_RESIZECONF,
+	/* Independent core clk divider */
+	FEAT_CORE_CLK_DIV,
+	FEAT_LCD_CLK_SRC,
+	/* DSI-PLL power command 0x3 is not working */
+	FEAT_DSI_PLL_PWR_BUG,
+	FEAT_DSI_DCS_CMD_CONFIG_VC,
+	FEAT_DSI_VC_OCP_WIDTH,
+	FEAT_DSI_REVERSE_TXCLKESC,
+	FEAT_DSI_GNQ,
+	FEAT_DPI_USES_VDDS_DSI,
+	FEAT_HDMI_CTS_SWMODE,
+	FEAT_HDMI_AUDIO_USE_MCLK,
+	FEAT_HANDLE_UV_SEPARATE,
+	FEAT_ATTR2,
+	FEAT_VENC_REQUIRES_TV_DAC_CLK,
+	FEAT_CPR,
+	FEAT_PRELOAD,
+	FEAT_FIR_COEF_V,
+	FEAT_ALPHA_FIXED_ZORDER,
+	FEAT_ALPHA_FREE_ZORDER,
+	FEAT_FIFO_MERGE,
+	/* An unknown HW bug causing the normal FIFO thresholds not to work */
+	FEAT_OMAP3_DSI_FIFO_BUG,
+	FEAT_BURST_2D,
+	FEAT_DSI_PHY_DCC,
+	FEAT_MFLAG,
+};
+
+/* DSS register field id */
+enum dss_feat_reg_field {
+	FEAT_REG_FIRHINC,
+	FEAT_REG_FIRVINC,
+	FEAT_REG_FIFOHIGHTHRESHOLD,
+	FEAT_REG_FIFOLOWTHRESHOLD,
+	FEAT_REG_FIFOSIZE,
+	FEAT_REG_HORIZONTALACCU,
+	FEAT_REG_VERTICALACCU,
+	FEAT_REG_DISPC_CLK_SWITCH,
+};
+
+enum dss_range_param {
+	FEAT_PARAM_DSS_FCK,
+	FEAT_PARAM_DSS_PCD,
+	FEAT_PARAM_DSIPLL_LPDIV,
+	FEAT_PARAM_DSI_FCK,
+	FEAT_PARAM_DOWNSCALE,
+	FEAT_PARAM_LINEWIDTH,
+};
+
+/* DSS Feature Functions */
+unsigned long dss_feat_get_param_min(enum dss_range_param param);
+unsigned long dss_feat_get_param_max(enum dss_range_param param);
+enum omap_overlay_caps dss_feat_get_overlay_caps(enum omap_plane plane);
+bool dss_feat_color_mode_supported(enum omap_plane plane,
+		enum omap_color_mode color_mode);
+const char *dss_feat_get_clk_source_name(enum omap_dss_clk_source id);
+
+u32 dss_feat_get_buffer_size_unit(void);	/* in bytes */
+u32 dss_feat_get_burst_size_unit(void);		/* in bytes */
+
+bool dss_feat_rotation_type_supported(enum omap_dss_rotation_type rot_type);
+
+bool dss_has_feature(enum dss_feat_id id);
+void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end);
+void dss_features_init(enum omapdss_version version);
+
+enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
+enum omap_dss_output_id dss_feat_get_supported_outputs(enum omap_channel channel);
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h
new file mode 100644
index 000000000000..53616b02b613
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi.h
@@ -0,0 +1,370 @@
+/*
+ * HDMI driver definition for TI OMAP4 Processor.
+ *
+ * Copyright (C) 2010-2011 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _HDMI_H
+#define _HDMI_H
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/hdmi.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+/* HDMI Wrapper */
+
+#define HDMI_WP_REVISION			0x0
+#define HDMI_WP_SYSCONFIG			0x10
+#define HDMI_WP_IRQSTATUS_RAW			0x24
+#define HDMI_WP_IRQSTATUS			0x28
+#define HDMI_WP_IRQENABLE_SET			0x2C
+#define HDMI_WP_IRQENABLE_CLR			0x30
+#define HDMI_WP_IRQWAKEEN			0x34
+#define HDMI_WP_PWR_CTRL			0x40
+#define HDMI_WP_DEBOUNCE			0x44
+#define HDMI_WP_VIDEO_CFG			0x50
+#define HDMI_WP_VIDEO_SIZE			0x60
+#define HDMI_WP_VIDEO_TIMING_H			0x68
+#define HDMI_WP_VIDEO_TIMING_V			0x6C
+#define HDMI_WP_CLK				0x70
+#define HDMI_WP_AUDIO_CFG			0x80
+#define HDMI_WP_AUDIO_CFG2			0x84
+#define HDMI_WP_AUDIO_CTRL			0x88
+#define HDMI_WP_AUDIO_DATA			0x8C
+
+/* HDMI WP IRQ flags */
+#define HDMI_IRQ_CORE				(1 << 0)
+#define HDMI_IRQ_OCP_TIMEOUT			(1 << 4)
+#define HDMI_IRQ_AUDIO_FIFO_UNDERFLOW		(1 << 8)
+#define HDMI_IRQ_AUDIO_FIFO_OVERFLOW		(1 << 9)
+#define HDMI_IRQ_AUDIO_FIFO_SAMPLE_REQ		(1 << 10)
+#define HDMI_IRQ_VIDEO_VSYNC			(1 << 16)
+#define HDMI_IRQ_VIDEO_FRAME_DONE		(1 << 17)
+#define HDMI_IRQ_PHY_LINE5V_ASSERT		(1 << 24)
+#define HDMI_IRQ_LINK_CONNECT			(1 << 25)
+#define HDMI_IRQ_LINK_DISCONNECT		(1 << 26)
+#define HDMI_IRQ_PLL_LOCK			(1 << 29)
+#define HDMI_IRQ_PLL_UNLOCK			(1 << 30)
+#define HDMI_IRQ_PLL_RECAL			(1 << 31)
+
+/* HDMI PLL */
+
+#define PLLCTRL_PLL_CONTROL			0x0
+#define PLLCTRL_PLL_STATUS			0x4
+#define PLLCTRL_PLL_GO				0x8
+#define PLLCTRL_CFG1				0xC
+#define PLLCTRL_CFG2				0x10
+#define PLLCTRL_CFG3				0x14
+#define PLLCTRL_SSC_CFG1			0x18
+#define PLLCTRL_SSC_CFG2			0x1C
+#define PLLCTRL_CFG4				0x20
+
+/* HDMI PHY */
+
+#define HDMI_TXPHY_TX_CTRL			0x0
+#define HDMI_TXPHY_DIGITAL_CTRL			0x4
+#define HDMI_TXPHY_POWER_CTRL			0x8
+#define HDMI_TXPHY_PAD_CFG_CTRL			0xC
+#define HDMI_TXPHY_BIST_CONTROL			0x1C
+
+enum hdmi_pll_pwr {
+	HDMI_PLLPWRCMD_ALLOFF = 0,
+	HDMI_PLLPWRCMD_PLLONLY = 1,
+	HDMI_PLLPWRCMD_BOTHON_ALLCLKS = 2,
+	HDMI_PLLPWRCMD_BOTHON_NOPHYCLK = 3
+};
+
+enum hdmi_phy_pwr {
+	HDMI_PHYPWRCMD_OFF = 0,
+	HDMI_PHYPWRCMD_LDOON = 1,
+	HDMI_PHYPWRCMD_TXON = 2
+};
+
+enum hdmi_core_hdmi_dvi {
+	HDMI_DVI = 0,
+	HDMI_HDMI = 1
+};
+
+enum hdmi_packing_mode {
+	HDMI_PACK_10b_RGB_YUV444 = 0,
+	HDMI_PACK_24b_RGB_YUV444_YUV422 = 1,
+	HDMI_PACK_20b_YUV422 = 2,
+	HDMI_PACK_ALREADYPACKED = 7
+};
+
+enum hdmi_stereo_channels {
+	HDMI_AUDIO_STEREO_NOCHANNELS = 0,
+	HDMI_AUDIO_STEREO_ONECHANNEL = 1,
+	HDMI_AUDIO_STEREO_TWOCHANNELS = 2,
+	HDMI_AUDIO_STEREO_THREECHANNELS = 3,
+	HDMI_AUDIO_STEREO_FOURCHANNELS = 4
+};
+
+enum hdmi_audio_type {
+	HDMI_AUDIO_TYPE_LPCM = 0,
+	HDMI_AUDIO_TYPE_IEC = 1
+};
+
+enum hdmi_audio_justify {
+	HDMI_AUDIO_JUSTIFY_LEFT = 0,
+	HDMI_AUDIO_JUSTIFY_RIGHT = 1
+};
+
+enum hdmi_audio_sample_order {
+	HDMI_AUDIO_SAMPLE_RIGHT_FIRST = 0,
+	HDMI_AUDIO_SAMPLE_LEFT_FIRST = 1
+};
+
+enum hdmi_audio_samples_perword {
+	HDMI_AUDIO_ONEWORD_ONESAMPLE = 0,
+	HDMI_AUDIO_ONEWORD_TWOSAMPLES = 1
+};
+
+enum hdmi_audio_sample_size_omap {
+	HDMI_AUDIO_SAMPLE_16BITS = 0,
+	HDMI_AUDIO_SAMPLE_24BITS = 1
+};
+
+enum hdmi_audio_transf_mode {
+	HDMI_AUDIO_TRANSF_DMA = 0,
+	HDMI_AUDIO_TRANSF_IRQ = 1
+};
+
+enum hdmi_audio_blk_strt_end_sig {
+	HDMI_AUDIO_BLOCK_SIG_STARTEND_ON = 0,
+	HDMI_AUDIO_BLOCK_SIG_STARTEND_OFF = 1
+};
+
+enum hdmi_core_audio_layout {
+	HDMI_AUDIO_LAYOUT_2CH = 0,
+	HDMI_AUDIO_LAYOUT_8CH = 1,
+	HDMI_AUDIO_LAYOUT_6CH = 2
+};
+
+enum hdmi_core_cts_mode {
+	HDMI_AUDIO_CTS_MODE_HW = 0,
+	HDMI_AUDIO_CTS_MODE_SW = 1
+};
+
+enum hdmi_audio_mclk_mode {
+	HDMI_AUDIO_MCLK_128FS = 0,
+	HDMI_AUDIO_MCLK_256FS = 1,
+	HDMI_AUDIO_MCLK_384FS = 2,
+	HDMI_AUDIO_MCLK_512FS = 3,
+	HDMI_AUDIO_MCLK_768FS = 4,
+	HDMI_AUDIO_MCLK_1024FS = 5,
+	HDMI_AUDIO_MCLK_1152FS = 6,
+	HDMI_AUDIO_MCLK_192FS = 7
+};
+
+struct hdmi_video_format {
+	enum hdmi_packing_mode	packing_mode;
+	u32			y_res;	/* Line per panel */
+	u32			x_res;	/* pixel per line */
+};
+
+struct hdmi_config {
+	struct omap_video_timings timings;
+	struct hdmi_avi_infoframe infoframe;
+	enum hdmi_core_hdmi_dvi hdmi_dvi_mode;
+};
+
+struct hdmi_audio_format {
+	enum hdmi_stereo_channels		stereo_channels;
+	u8					active_chnnls_msk;
+	enum hdmi_audio_type			type;
+	enum hdmi_audio_justify			justification;
+	enum hdmi_audio_sample_order		sample_order;
+	enum hdmi_audio_samples_perword		samples_per_word;
+	enum hdmi_audio_sample_size_omap	sample_size;
+	enum hdmi_audio_blk_strt_end_sig	en_sig_blk_strt_end;
+};
+
+struct hdmi_audio_dma {
+	u8				transfer_size;
+	u8				block_size;
+	enum hdmi_audio_transf_mode	mode;
+	u16				fifo_threshold;
+};
+
+struct hdmi_core_audio_i2s_config {
+	u8 in_length_bits;
+	u8 justification;
+	u8 sck_edge_mode;
+	u8 vbit;
+	u8 direction;
+	u8 shift;
+	u8 active_sds;
+};
+
+struct hdmi_core_audio_config {
+	struct hdmi_core_audio_i2s_config	i2s_cfg;
+	struct snd_aes_iec958			*iec60958_cfg;
+	bool					fs_override;
+	u32					n;
+	u32					cts;
+	u32					aud_par_busclk;
+	enum hdmi_core_audio_layout		layout;
+	enum hdmi_core_cts_mode			cts_mode;
+	bool					use_mclk;
+	enum hdmi_audio_mclk_mode		mclk_mode;
+	bool					en_acr_pkt;
+	bool					en_dsd_audio;
+	bool					en_parallel_aud_input;
+	bool					en_spdif;
+};
+
+struct hdmi_wp_data {
+	void __iomem *base;
+	phys_addr_t phys_base;
+};
+
+struct hdmi_pll_data {
+	struct dss_pll pll;
+
+	void __iomem *base;
+
+	struct hdmi_wp_data *wp;
+};
+
+struct hdmi_phy_data {
+	void __iomem *base;
+
+	u8 lane_function[4];
+	u8 lane_polarity[4];
+};
+
+struct hdmi_core_data {
+	void __iomem *base;
+};
+
+static inline void hdmi_write_reg(void __iomem *base_addr, const u32 idx,
+		u32 val)
+{
+	__raw_writel(val, base_addr + idx);
+}
+
+static inline u32 hdmi_read_reg(void __iomem *base_addr, const u32 idx)
+{
+	return __raw_readl(base_addr + idx);
+}
+
+#define REG_FLD_MOD(base, idx, val, start, end) \
+	hdmi_write_reg(base, idx, FLD_MOD(hdmi_read_reg(base, idx),\
+							val, start, end))
+#define REG_GET(base, idx, start, end) \
+	FLD_GET(hdmi_read_reg(base, idx), start, end)
+
+static inline int hdmi_wait_for_bit_change(void __iomem *base_addr,
+		const u32 idx, int b2, int b1, u32 val)
+{
+	u32 t = 0, v;
+	while (val != (v = REG_GET(base_addr, idx, b2, b1))) {
+		if (t++ > 10000)
+			return v;
+		udelay(1);
+	}
+	return v;
+}
+
+/* HDMI wrapper funcs */
+int hdmi_wp_video_start(struct hdmi_wp_data *wp);
+void hdmi_wp_video_stop(struct hdmi_wp_data *wp);
+void hdmi_wp_dump(struct hdmi_wp_data *wp, struct seq_file *s);
+u32 hdmi_wp_get_irqstatus(struct hdmi_wp_data *wp);
+void hdmi_wp_set_irqstatus(struct hdmi_wp_data *wp, u32 irqstatus);
+void hdmi_wp_set_irqenable(struct hdmi_wp_data *wp, u32 mask);
+void hdmi_wp_clear_irqenable(struct hdmi_wp_data *wp, u32 mask);
+int hdmi_wp_set_phy_pwr(struct hdmi_wp_data *wp, enum hdmi_phy_pwr val);
+int hdmi_wp_set_pll_pwr(struct hdmi_wp_data *wp, enum hdmi_pll_pwr val);
+void hdmi_wp_video_config_format(struct hdmi_wp_data *wp,
+		struct hdmi_video_format *video_fmt);
+void hdmi_wp_video_config_interface(struct hdmi_wp_data *wp,
+		struct omap_video_timings *timings);
+void hdmi_wp_video_config_timing(struct hdmi_wp_data *wp,
+		struct omap_video_timings *timings);
+void hdmi_wp_init_vid_fmt_timings(struct hdmi_video_format *video_fmt,
+		struct omap_video_timings *timings, struct hdmi_config *param);
+int hdmi_wp_init(struct platform_device *pdev, struct hdmi_wp_data *wp);
+phys_addr_t hdmi_wp_get_audio_dma_addr(struct hdmi_wp_data *wp);
+
+/* HDMI PLL funcs */
+void hdmi_pll_dump(struct hdmi_pll_data *pll, struct seq_file *s);
+void hdmi_pll_compute(struct hdmi_pll_data *pll,
+	unsigned long target_tmds, struct dss_pll_clock_info *pi);
+int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll,
+	struct hdmi_wp_data *wp);
+void hdmi_pll_uninit(struct hdmi_pll_data *hpll);
+
+/* HDMI PHY funcs */
+int hdmi_phy_configure(struct hdmi_phy_data *phy, unsigned long hfbitclk,
+	unsigned long lfbitclk);
+void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s);
+int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy);
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes);
+
+/* HDMI common funcs */
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+	struct hdmi_phy_data *phy);
+
+/* Audio funcs */
+int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts);
+int hdmi_wp_audio_enable(struct hdmi_wp_data *wp, bool enable);
+int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable);
+void hdmi_wp_audio_config_format(struct hdmi_wp_data *wp,
+		struct hdmi_audio_format *aud_fmt);
+void hdmi_wp_audio_config_dma(struct hdmi_wp_data *wp,
+		struct hdmi_audio_dma *aud_dma);
+static inline bool hdmi_mode_has_audio(struct hdmi_config *cfg)
+{
+	return cfg->hdmi_dvi_mode == HDMI_HDMI ? true : false;
+}
+
+/* HDMI DRV data */
+struct omap_hdmi {
+	struct mutex lock;
+	struct platform_device *pdev;
+
+	struct hdmi_wp_data	wp;
+	struct hdmi_pll_data	pll;
+	struct hdmi_phy_data	phy;
+	struct hdmi_core_data	core;
+
+	struct hdmi_config cfg;
+
+	struct regulator *vdda_reg;
+
+	bool core_enabled;
+
+	struct omap_dss_device output;
+
+	struct platform_device *audio_pdev;
+	void (*audio_abort_cb)(struct device *dev);
+	int wp_idlemode;
+
+	bool audio_configured;
+	struct omap_dss_audio audio_config;
+
+	/* This lock should be taken when booleans bellow are touched. */
+	spinlock_t audio_playing_lock;
+	bool audio_playing;
+	bool display_enabled;
+};
+
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c
new file mode 100644
index 000000000000..7103c659a534
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4.c
@@ -0,0 +1,839 @@
+/*
+ * HDMI interface DSS driver for TI's OMAP4 family of SoCs.
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *	Mythri pk <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/component.h>
+#include <video/omapdss.h>
+#include <sound/omap-hdmi-audio.h>
+
+#include "hdmi4_core.h"
+#include "dss.h"
+#include "dss_features.h"
+#include "hdmi.h"
+
+static struct omap_hdmi hdmi;
+
+static int hdmi_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0);
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_put\n");
+
+	r = pm_runtime_put_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+	struct hdmi_wp_data *wp = data;
+	u32 irqstatus;
+
+	irqstatus = hdmi_wp_get_irqstatus(wp);
+	hdmi_wp_set_irqstatus(wp, irqstatus);
+
+	if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+			irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		/*
+		 * If we get both connect and disconnect interrupts at the same
+		 * time, turn off the PHY, clear interrupts, and restart, which
+		 * raises connect interrupt if a cable is connected, or nothing
+		 * if cable is not connected.
+		 */
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+		hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+				HDMI_IRQ_LINK_DISCONNECT);
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	} else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+	} else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int hdmi_init_regulator(void)
+{
+	int r;
+	struct regulator *reg;
+
+	if (hdmi.vdda_reg != NULL)
+		return 0;
+
+	reg = devm_regulator_get(&hdmi.pdev->dev, "vdda");
+
+	if (IS_ERR(reg)) {
+		if (PTR_ERR(reg) != -EPROBE_DEFER)
+			DSSERR("can't get VDDA regulator\n");
+		return PTR_ERR(reg);
+	}
+
+	if (regulator_can_change_voltage(reg)) {
+		r = regulator_set_voltage(reg, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(reg);
+			DSSWARN("can't set the regulator voltage\n");
+			return r;
+		}
+	}
+
+	hdmi.vdda_reg = reg;
+
+	return 0;
+}
+
+static int hdmi_power_on_core(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	r = regulator_enable(hdmi.vdda_reg);
+	if (r)
+		return r;
+
+	r = hdmi_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	/* Make selection of HDMI in DSS */
+	dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+	hdmi.core_enabled = true;
+
+	return 0;
+
+err_runtime_get:
+	regulator_disable(hdmi.vdda_reg);
+
+	return r;
+}
+
+static void hdmi_power_off_core(struct omap_dss_device *dssdev)
+{
+	hdmi.core_enabled = false;
+
+	hdmi_runtime_put();
+	regulator_disable(hdmi.vdda_reg);
+}
+
+static int hdmi_power_on_full(struct omap_dss_device *dssdev)
+{
+	int r;
+	struct omap_video_timings *p;
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+	struct hdmi_wp_data *wp = &hdmi.wp;
+	struct dss_pll_clock_info hdmi_cinfo = { 0 };
+
+	r = hdmi_power_on_core(dssdev);
+	if (r)
+		return r;
+
+	/* disable and clear irqs */
+	hdmi_wp_clear_irqenable(wp, 0xffffffff);
+	hdmi_wp_set_irqstatus(wp, 0xffffffff);
+
+	p = &hdmi.cfg.timings;
+
+	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
+
+	hdmi_pll_compute(&hdmi.pll, p->pixelclock, &hdmi_cinfo);
+
+	r = dss_pll_enable(&hdmi.pll.pll);
+	if (r) {
+		DSSERR("Failed to enable PLL\n");
+		goto err_pll_enable;
+	}
+
+	r = dss_pll_set_config(&hdmi.pll.pll, &hdmi_cinfo);
+	if (r) {
+		DSSERR("Failed to configure PLL\n");
+		goto err_pll_cfg;
+	}
+
+	r = hdmi_phy_configure(&hdmi.phy, hdmi_cinfo.clkdco,
+		hdmi_cinfo.clkout[0]);
+	if (r) {
+		DSSDBG("Failed to configure PHY\n");
+		goto err_phy_cfg;
+	}
+
+	r = hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	if (r)
+		goto err_phy_pwr;
+
+	hdmi4_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
+
+	/* bypass TV gamma table */
+	dispc_enable_gamma_table(0);
+
+	/* tv size */
+	dss_mgr_set_timings(mgr, p);
+
+	r = hdmi_wp_video_start(&hdmi.wp);
+	if (r)
+		goto err_vid_enable;
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		goto err_mgr_enable;
+
+	hdmi_wp_set_irqenable(wp,
+		HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
+	return 0;
+
+err_mgr_enable:
+	hdmi_wp_video_stop(&hdmi.wp);
+err_vid_enable:
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
+err_phy_cfg:
+err_pll_cfg:
+	dss_pll_disable(&hdmi.pll.pll);
+err_pll_enable:
+	hdmi_power_off_core(dssdev);
+	return -EIO;
+}
+
+static void hdmi_power_off_full(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
+	dss_mgr_disable(mgr);
+
+	hdmi_wp_video_stop(&hdmi.wp);
+
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
+	dss_pll_disable(&hdmi.pll.pll);
+
+	hdmi_power_off_core(dssdev);
+}
+
+static int hdmi_display_check_timing(struct omap_dss_device *dssdev,
+					struct omap_video_timings *timings)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	if (!dispc_mgr_timings_ok(out->dispc_channel, timings))
+		return -EINVAL;
+
+	return 0;
+}
+
+static void hdmi_display_set_timing(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	mutex_lock(&hdmi.lock);
+
+	hdmi.cfg.timings = *timings;
+
+	dispc_set_tv_pclk(timings->pixelclock);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static void hdmi_display_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	*timings = hdmi.cfg.timings;
+}
+
+static void hdmi_dump_regs(struct seq_file *s)
+{
+	mutex_lock(&hdmi.lock);
+
+	if (hdmi_runtime_get()) {
+		mutex_unlock(&hdmi.lock);
+		return;
+	}
+
+	hdmi_wp_dump(&hdmi.wp, s);
+	hdmi_pll_dump(&hdmi.pll, s);
+	hdmi_phy_dump(&hdmi.phy, s);
+	hdmi4_core_dump(&hdmi.core, s);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+}
+
+static int read_edid(u8 *buf, int len)
+{
+	int r;
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_runtime_get();
+	BUG_ON(r);
+
+	r = hdmi4_read_edid(&hdmi.core,  buf, len);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+
+	return r;
+}
+
+static void hdmi_start_audio_stream(struct omap_hdmi *hd)
+{
+	hdmi_wp_audio_enable(&hd->wp, true);
+	hdmi4_audio_start(&hd->core, &hd->wp);
+}
+
+static void hdmi_stop_audio_stream(struct omap_hdmi *hd)
+{
+	hdmi4_audio_stop(&hd->core, &hd->wp);
+	hdmi_wp_audio_enable(&hd->wp, false);
+}
+
+static int hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+	unsigned long flags;
+	int r = 0;
+
+	DSSDBG("ENTER hdmi_display_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err0;
+	}
+
+	r = hdmi_power_on_full(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	if (hdmi.audio_configured) {
+		r = hdmi4_audio_config(&hdmi.core, &hdmi.wp, &hdmi.audio_config,
+				       hdmi.cfg.timings.pixelclock);
+		if (r) {
+			DSSERR("Error restoring audio configuration: %d", r);
+			hdmi.audio_abort_cb(&hdmi.pdev->dev);
+			hdmi.audio_configured = false;
+		}
+	}
+
+	spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+	if (hdmi.audio_configured && hdmi.audio_playing)
+		hdmi_start_audio_stream(&hdmi);
+	hdmi.display_enabled = true;
+	spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+	unsigned long flags;
+
+	DSSDBG("Enter hdmi_display_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+	hdmi_stop_audio_stream(&hdmi);
+	hdmi.display_enabled = false;
+	spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+	hdmi_power_off_full(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_core_enable(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	DSSDBG("ENTER omapdss_hdmi_core_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_power_on_core(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_core_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("Enter omapdss_hdmi_core_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	hdmi_power_off_core(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = hdmi_init_regulator();
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void hdmi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static int hdmi_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	bool need_enable;
+	int r;
+
+	need_enable = hdmi.core_enabled == false;
+
+	if (need_enable) {
+		r = hdmi_core_enable(dssdev);
+		if (r)
+			return r;
+	}
+
+	r = read_edid(edid, len);
+
+	if (need_enable)
+		hdmi_core_disable(dssdev);
+
+	return r;
+}
+
+static int hdmi_set_infoframe(struct omap_dss_device *dssdev,
+		const struct hdmi_avi_infoframe *avi)
+{
+	hdmi.cfg.infoframe = *avi;
+	return 0;
+}
+
+static int hdmi_set_hdmi_mode(struct omap_dss_device *dssdev,
+		bool hdmi_mode)
+{
+	hdmi.cfg.hdmi_dvi_mode = hdmi_mode ? HDMI_HDMI : HDMI_DVI;
+	return 0;
+}
+
+static const struct omapdss_hdmi_ops hdmi_ops = {
+	.connect		= hdmi_connect,
+	.disconnect		= hdmi_disconnect,
+
+	.enable			= hdmi_display_enable,
+	.disable		= hdmi_display_disable,
+
+	.check_timings		= hdmi_display_check_timing,
+	.set_timings		= hdmi_display_set_timing,
+	.get_timings		= hdmi_display_get_timings,
+
+	.read_edid		= hdmi_read_edid,
+	.set_infoframe		= hdmi_set_infoframe,
+	.set_hdmi_mode		= hdmi_set_hdmi_mode,
+};
+
+static void hdmi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_HDMI;
+	out->output_type = OMAP_DISPLAY_TYPE_HDMI;
+	out->name = "hdmi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+	out->ops.hdmi = &hdmi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void hdmi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	omapdss_unregister_output(out);
+}
+
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep;
+	int r;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+	if (r)
+		goto err;
+
+	of_node_put(ep);
+	return 0;
+
+err:
+	of_node_put(ep);
+	return r;
+}
+
+/* Audio callbacks */
+static int hdmi_audio_startup(struct device *dev,
+			      void (*abort_cb)(struct device *dev))
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&hd->lock);
+
+	if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	hd->audio_abort_cb = abort_cb;
+
+out:
+	mutex_unlock(&hd->lock);
+
+	return ret;
+}
+
+static int hdmi_audio_shutdown(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+
+	mutex_lock(&hd->lock);
+	hd->audio_abort_cb = NULL;
+	hd->audio_configured = false;
+	hd->audio_playing = false;
+	mutex_unlock(&hd->lock);
+
+	return 0;
+}
+
+static int hdmi_audio_start(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+	spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+	if (hd->display_enabled)
+		hdmi_start_audio_stream(hd);
+	hd->audio_playing = true;
+
+	spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+	return 0;
+}
+
+static void hdmi_audio_stop(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+	spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+	if (hd->display_enabled)
+		hdmi_stop_audio_stream(hd);
+	hd->audio_playing = false;
+
+	spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+}
+
+static int hdmi_audio_config(struct device *dev,
+			     struct omap_dss_audio *dss_audio)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	int ret;
+
+	mutex_lock(&hd->lock);
+
+	if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	ret = hdmi4_audio_config(&hd->core, &hd->wp, dss_audio,
+				 hd->cfg.timings.pixelclock);
+	if (!ret) {
+		hd->audio_configured = true;
+		hd->audio_config = *dss_audio;
+	}
+out:
+	mutex_unlock(&hd->lock);
+
+	return ret;
+}
+
+static const struct omap_hdmi_audio_ops hdmi_audio_ops = {
+	.audio_startup = hdmi_audio_startup,
+	.audio_shutdown = hdmi_audio_shutdown,
+	.audio_start = hdmi_audio_start,
+	.audio_stop = hdmi_audio_stop,
+	.audio_config = hdmi_audio_config,
+};
+
+static int hdmi_audio_register(struct device *dev)
+{
+	struct omap_hdmi_audio_pdata pdata = {
+		.dev = dev,
+		.dss_version = omapdss_get_version(),
+		.audio_dma_addr = hdmi_wp_get_audio_dma_addr(&hdmi.wp),
+		.ops = &hdmi_audio_ops,
+	};
+
+	hdmi.audio_pdev = platform_device_register_data(
+		dev, "omap-hdmi-audio", PLATFORM_DEVID_AUTO,
+		&pdata, sizeof(pdata));
+
+	if (IS_ERR(hdmi.audio_pdev))
+		return PTR_ERR(hdmi.audio_pdev);
+
+	return 0;
+}
+
+/* HDMI HW IP initialisation */
+static int hdmi4_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	int r;
+	int irq;
+
+	hdmi.pdev = pdev;
+	dev_set_drvdata(&pdev->dev, &hdmi);
+
+	mutex_init(&hdmi.lock);
+	spin_lock_init(&hdmi.audio_playing_lock);
+
+	if (pdev->dev.of_node) {
+		r = hdmi_probe_of(pdev);
+		if (r)
+			return r;
+	}
+
+	r = hdmi_wp_init(pdev, &hdmi.wp);
+	if (r)
+		return r;
+
+	r = hdmi_pll_init(pdev, &hdmi.pll, &hdmi.wp);
+	if (r)
+		return r;
+
+	r = hdmi_phy_init(pdev, &hdmi.phy);
+	if (r)
+		goto err;
+
+	r = hdmi4_core_init(pdev, &hdmi.core);
+	if (r)
+		goto err;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto err;
+	}
+
+	r = devm_request_threaded_irq(&pdev->dev, irq,
+			NULL, hdmi_irq_handler,
+			IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+	if (r) {
+		DSSERR("HDMI IRQ request failed\n");
+		goto err;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	hdmi_init_output(pdev);
+
+	r = hdmi_audio_register(&pdev->dev);
+	if (r) {
+		DSSERR("Registering HDMI audio failed\n");
+		hdmi_uninit_output(pdev);
+		pm_runtime_disable(&pdev->dev);
+		return r;
+	}
+
+	dss_debugfs_create_file("hdmi", hdmi_dump_regs);
+
+	return 0;
+err:
+	hdmi_pll_uninit(&hdmi.pll);
+	return r;
+}
+
+static void hdmi4_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if (hdmi.audio_pdev)
+		platform_device_unregister(hdmi.audio_pdev);
+
+	hdmi_uninit_output(pdev);
+
+	hdmi_pll_uninit(&hdmi.pll);
+
+	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi4_component_ops = {
+	.bind	= hdmi4_bind,
+	.unbind	= hdmi4_unbind,
+};
+
+static int hdmi4_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &hdmi4_component_ops);
+}
+
+static int hdmi4_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &hdmi4_component_ops);
+	return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume = hdmi_runtime_resume,
+};
+
+static const struct of_device_id hdmi_of_match[] = {
+	{ .compatible = "ti,omap4-hdmi", },
+	{},
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+	.probe		= hdmi4_probe,
+	.remove		= hdmi4_remove,
+	.driver         = {
+		.name   = "omapdss_hdmi",
+		.pm	= &hdmi_pm_ops,
+		.of_match_table = hdmi_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init hdmi4_init_platform_driver(void)
+{
+	return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi4_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c
new file mode 100644
index 000000000000..fa72e735dad2
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.c
@@ -0,0 +1,904 @@
+/*
+ * ti_hdmi_4xxx_ip.c
+ *
+ * HDMI TI81xx, TI38xx, TI OMAP4 etc IP driver Library
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *	Mythri pk <mythripk@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 DSS_SUBSYS_NAME "HDMICORE"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <sound/asound.h>
+#include <sound/asoundef.h>
+
+#include "hdmi4_core.h"
+#include "dss_features.h"
+
+#define HDMI_CORE_AV		0x500
+
+static inline void __iomem *hdmi_av_base(struct hdmi_core_data *core)
+{
+	return core->base + HDMI_CORE_AV;
+}
+
+static int hdmi_core_ddc_init(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	/* Turn on CLK for DDC */
+	REG_FLD_MOD(base, HDMI_CORE_AV_DPD, 0x7, 2, 0);
+
+	/* IN_PROG */
+	if (REG_GET(base, HDMI_CORE_DDC_STATUS, 4, 4) == 1) {
+		/* Abort transaction */
+		REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0xf, 3, 0);
+		/* IN_PROG */
+		if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+					4, 4, 0) != 0) {
+			DSSERR("Timeout aborting DDC transaction\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* Clk SCL Devices */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0xA, 3, 0);
+
+	/* HDMI_CORE_DDC_STATUS_IN_PROG */
+	if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+				4, 4, 0) != 0) {
+		DSSERR("Timeout starting SCL clock\n");
+		return -ETIMEDOUT;
+	}
+
+	/* Clear FIFO */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x9, 3, 0);
+
+	/* HDMI_CORE_DDC_STATUS_IN_PROG */
+	if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+				4, 4, 0) != 0) {
+		DSSERR("Timeout clearing DDC fifo\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int hdmi_core_ddc_edid(struct hdmi_core_data *core,
+		u8 *pedid, int ext)
+{
+	void __iomem *base = core->base;
+	u32 i;
+	char checksum;
+	u32 offset = 0;
+
+	/* HDMI_CORE_DDC_STATUS_IN_PROG */
+	if (hdmi_wait_for_bit_change(base, HDMI_CORE_DDC_STATUS,
+				4, 4, 0) != 0) {
+		DSSERR("Timeout waiting DDC to be ready\n");
+		return -ETIMEDOUT;
+	}
+
+	if (ext % 2 != 0)
+		offset = 0x80;
+
+	/* Load Segment Address Register */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_SEGM, ext / 2, 7, 0);
+
+	/* Load Slave Address Register */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_ADDR, 0xA0 >> 1, 7, 1);
+
+	/* Load Offset Address Register */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_OFFSET, offset, 7, 0);
+
+	/* Load Byte Count */
+	REG_FLD_MOD(base, HDMI_CORE_DDC_COUNT1, 0x80, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_DDC_COUNT2, 0x0, 1, 0);
+
+	/* Set DDC_CMD */
+	if (ext)
+		REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x4, 3, 0);
+	else
+		REG_FLD_MOD(base, HDMI_CORE_DDC_CMD, 0x2, 3, 0);
+
+	/* HDMI_CORE_DDC_STATUS_BUS_LOW */
+	if (REG_GET(base, HDMI_CORE_DDC_STATUS, 6, 6) == 1) {
+		DSSERR("I2C Bus Low?\n");
+		return -EIO;
+	}
+	/* HDMI_CORE_DDC_STATUS_NO_ACK */
+	if (REG_GET(base, HDMI_CORE_DDC_STATUS, 5, 5) == 1) {
+		DSSERR("I2C No Ack\n");
+		return -EIO;
+	}
+
+	for (i = 0; i < 0x80; ++i) {
+		int t;
+
+		/* IN_PROG */
+		if (REG_GET(base, HDMI_CORE_DDC_STATUS, 4, 4) == 0) {
+			DSSERR("operation stopped when reading edid\n");
+			return -EIO;
+		}
+
+		t = 0;
+		/* FIFO_EMPTY */
+		while (REG_GET(base, HDMI_CORE_DDC_STATUS, 2, 2) == 1) {
+			if (t++ > 10000) {
+				DSSERR("timeout reading edid\n");
+				return -ETIMEDOUT;
+			}
+			udelay(1);
+		}
+
+		pedid[i] = REG_GET(base, HDMI_CORE_DDC_DATA, 7, 0);
+	}
+
+	checksum = 0;
+	for (i = 0; i < 0x80; ++i)
+		checksum += pedid[i];
+
+	if (checksum != 0) {
+		DSSERR("E-EDID checksum failed!!\n");
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int hdmi4_read_edid(struct hdmi_core_data *core, u8 *edid, int len)
+{
+	int r, l;
+
+	if (len < 128)
+		return -EINVAL;
+
+	r = hdmi_core_ddc_init(core);
+	if (r)
+		return r;
+
+	r = hdmi_core_ddc_edid(core, edid, 0);
+	if (r)
+		return r;
+
+	l = 128;
+
+	if (len >= 128 * 2 && edid[0x7e] > 0) {
+		r = hdmi_core_ddc_edid(core, edid + 0x80, 1);
+		if (r)
+			return r;
+		l += 128;
+	}
+
+	return l;
+}
+
+static void hdmi_core_init(struct hdmi_core_video_config *video_cfg)
+{
+	DSSDBG("Enter hdmi_core_init\n");
+
+	/* video core */
+	video_cfg->ip_bus_width = HDMI_INPUT_8BIT;
+	video_cfg->op_dither_truc = HDMI_OUTPUTTRUNCATION_8BIT;
+	video_cfg->deep_color_pkt = HDMI_DEEPCOLORPACKECTDISABLE;
+	video_cfg->pkt_mode = HDMI_PACKETMODERESERVEDVALUE;
+	video_cfg->hdmi_dvi = HDMI_DVI;
+	video_cfg->tclk_sel_clkmult = HDMI_FPLL10IDCK;
+}
+
+static void hdmi_core_powerdown_disable(struct hdmi_core_data *core)
+{
+	DSSDBG("Enter hdmi_core_powerdown_disable\n");
+	REG_FLD_MOD(core->base, HDMI_CORE_SYS_SYS_CTRL1, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_release(struct hdmi_core_data *core)
+{
+	DSSDBG("Enter hdmi_core_swreset_release\n");
+	REG_FLD_MOD(core->base, HDMI_CORE_SYS_SRST, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_assert(struct hdmi_core_data *core)
+{
+	DSSDBG("Enter hdmi_core_swreset_assert\n");
+	REG_FLD_MOD(core->base, HDMI_CORE_SYS_SRST, 0x1, 0, 0);
+}
+
+/* HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_data *core,
+				struct hdmi_core_video_config *cfg)
+{
+	u32 r = 0;
+	void __iomem *core_sys_base = core->base;
+	void __iomem *core_av_base = hdmi_av_base(core);
+
+	/* sys_ctrl1 default configuration not tunable */
+	r = hdmi_read_reg(core_sys_base, HDMI_CORE_SYS_SYS_CTRL1);
+	r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_VEN_FOLLOWVSYNC, 5, 5);
+	r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_HEN_FOLLOWHSYNC, 4, 4);
+	r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_BSEL_24BITBUS, 2, 2);
+	r = FLD_MOD(r, HDMI_CORE_SYS_SYS_CTRL1_EDGE_RISINGEDGE, 1, 1);
+	hdmi_write_reg(core_sys_base, HDMI_CORE_SYS_SYS_CTRL1, r);
+
+	REG_FLD_MOD(core_sys_base,
+			HDMI_CORE_SYS_VID_ACEN, cfg->ip_bus_width, 7, 6);
+
+	/* Vid_Mode */
+	r = hdmi_read_reg(core_sys_base, HDMI_CORE_SYS_VID_MODE);
+
+	/* dither truncation configuration */
+	if (cfg->op_dither_truc > HDMI_OUTPUTTRUNCATION_12BIT) {
+		r = FLD_MOD(r, cfg->op_dither_truc - 3, 7, 6);
+		r = FLD_MOD(r, 1, 5, 5);
+	} else {
+		r = FLD_MOD(r, cfg->op_dither_truc, 7, 6);
+		r = FLD_MOD(r, 0, 5, 5);
+	}
+	hdmi_write_reg(core_sys_base, HDMI_CORE_SYS_VID_MODE, r);
+
+	/* HDMI_Ctrl */
+	r = hdmi_read_reg(core_av_base, HDMI_CORE_AV_HDMI_CTRL);
+	r = FLD_MOD(r, cfg->deep_color_pkt, 6, 6);
+	r = FLD_MOD(r, cfg->pkt_mode, 5, 3);
+	r = FLD_MOD(r, cfg->hdmi_dvi, 0, 0);
+	hdmi_write_reg(core_av_base, HDMI_CORE_AV_HDMI_CTRL, r);
+
+	/* TMDS_CTRL */
+	REG_FLD_MOD(core_sys_base,
+			HDMI_CORE_SYS_TMDS_CTRL, cfg->tclk_sel_clkmult, 6, 5);
+}
+
+static void hdmi_core_write_avi_infoframe(struct hdmi_core_data *core,
+	struct hdmi_avi_infoframe *frame)
+{
+	void __iomem *av_base = hdmi_av_base(core);
+	u8 data[HDMI_INFOFRAME_SIZE(AVI)];
+	int i;
+
+	hdmi_avi_infoframe_pack(frame, data, sizeof(data));
+
+	print_hex_dump_debug("AVI: ", DUMP_PREFIX_NONE, 16, 1, data,
+		HDMI_INFOFRAME_SIZE(AVI), false);
+
+	for (i = 0; i < sizeof(data); ++i) {
+		hdmi_write_reg(av_base, HDMI_CORE_AV_AVI_BASE + i * 4,
+			data[i]);
+	}
+}
+
+static void hdmi_core_av_packet_config(struct hdmi_core_data *core,
+		struct hdmi_core_packet_enable_repeat repeat_cfg)
+{
+	/* enable/repeat the infoframe */
+	hdmi_write_reg(hdmi_av_base(core), HDMI_CORE_AV_PB_CTRL1,
+		(repeat_cfg.audio_pkt << 5) |
+		(repeat_cfg.audio_pkt_repeat << 4) |
+		(repeat_cfg.avi_infoframe << 1) |
+		(repeat_cfg.avi_infoframe_repeat));
+
+	/* enable/repeat the packet */
+	hdmi_write_reg(hdmi_av_base(core), HDMI_CORE_AV_PB_CTRL2,
+		(repeat_cfg.gen_cntrl_pkt << 3) |
+		(repeat_cfg.gen_cntrl_pkt_repeat << 2) |
+		(repeat_cfg.generic_pkt << 1) |
+		(repeat_cfg.generic_pkt_repeat));
+}
+
+void hdmi4_configure(struct hdmi_core_data *core,
+	struct hdmi_wp_data *wp, struct hdmi_config *cfg)
+{
+	/* HDMI */
+	struct omap_video_timings video_timing;
+	struct hdmi_video_format video_format;
+	/* HDMI core */
+	struct hdmi_core_video_config v_core_cfg;
+	struct hdmi_core_packet_enable_repeat repeat_cfg = { 0 };
+
+	hdmi_core_init(&v_core_cfg);
+
+	hdmi_wp_init_vid_fmt_timings(&video_format, &video_timing, cfg);
+
+	hdmi_wp_video_config_timing(wp, &video_timing);
+
+	/* video config */
+	video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+	hdmi_wp_video_config_format(wp, &video_format);
+
+	hdmi_wp_video_config_interface(wp, &video_timing);
+
+	/*
+	 * configure core video part
+	 * set software reset in the core
+	 */
+	hdmi_core_swreset_assert(core);
+
+	/* power down off */
+	hdmi_core_powerdown_disable(core);
+
+	v_core_cfg.pkt_mode = HDMI_PACKETMODE24BITPERPIXEL;
+	v_core_cfg.hdmi_dvi = cfg->hdmi_dvi_mode;
+
+	hdmi_core_video_config(core, &v_core_cfg);
+
+	/* release software reset in the core */
+	hdmi_core_swreset_release(core);
+
+	if (cfg->hdmi_dvi_mode == HDMI_HDMI) {
+		hdmi_core_write_avi_infoframe(core, &cfg->infoframe);
+
+		/* enable/repeat the infoframe */
+		repeat_cfg.avi_infoframe = HDMI_PACKETENABLE;
+		repeat_cfg.avi_infoframe_repeat = HDMI_PACKETREPEATON;
+		/* wakeup */
+		repeat_cfg.audio_pkt = HDMI_PACKETENABLE;
+		repeat_cfg.audio_pkt_repeat = HDMI_PACKETREPEATON;
+	}
+
+	hdmi_core_av_packet_config(core, repeat_cfg);
+}
+
+void hdmi4_core_dump(struct hdmi_core_data *core, struct seq_file *s)
+{
+	int i;
+
+#define CORE_REG(i, name) name(i)
+#define DUMPCORE(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(core->base, r))
+#define DUMPCOREAV(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(hdmi_av_base(core), r))
+#define DUMPCOREAV2(i, r) seq_printf(s, "%s[%d]%*s %08x\n", #r, i, \
+		(i < 10) ? 32 - (int)strlen(#r) : 31 - (int)strlen(#r), " ", \
+		hdmi_read_reg(hdmi_av_base(core), CORE_REG(i, r)))
+
+	DUMPCORE(HDMI_CORE_SYS_VND_IDL);
+	DUMPCORE(HDMI_CORE_SYS_DEV_IDL);
+	DUMPCORE(HDMI_CORE_SYS_DEV_IDH);
+	DUMPCORE(HDMI_CORE_SYS_DEV_REV);
+	DUMPCORE(HDMI_CORE_SYS_SRST);
+	DUMPCORE(HDMI_CORE_SYS_SYS_CTRL1);
+	DUMPCORE(HDMI_CORE_SYS_SYS_STAT);
+	DUMPCORE(HDMI_CORE_SYS_SYS_CTRL3);
+	DUMPCORE(HDMI_CORE_SYS_DE_DLY);
+	DUMPCORE(HDMI_CORE_SYS_DE_CTRL);
+	DUMPCORE(HDMI_CORE_SYS_DE_TOP);
+	DUMPCORE(HDMI_CORE_SYS_DE_CNTL);
+	DUMPCORE(HDMI_CORE_SYS_DE_CNTH);
+	DUMPCORE(HDMI_CORE_SYS_DE_LINL);
+	DUMPCORE(HDMI_CORE_SYS_DE_LINH_1);
+	DUMPCORE(HDMI_CORE_SYS_HRES_L);
+	DUMPCORE(HDMI_CORE_SYS_HRES_H);
+	DUMPCORE(HDMI_CORE_SYS_VRES_L);
+	DUMPCORE(HDMI_CORE_SYS_VRES_H);
+	DUMPCORE(HDMI_CORE_SYS_IADJUST);
+	DUMPCORE(HDMI_CORE_SYS_POLDETECT);
+	DUMPCORE(HDMI_CORE_SYS_HWIDTH1);
+	DUMPCORE(HDMI_CORE_SYS_HWIDTH2);
+	DUMPCORE(HDMI_CORE_SYS_VWIDTH);
+	DUMPCORE(HDMI_CORE_SYS_VID_CTRL);
+	DUMPCORE(HDMI_CORE_SYS_VID_ACEN);
+	DUMPCORE(HDMI_CORE_SYS_VID_MODE);
+	DUMPCORE(HDMI_CORE_SYS_VID_BLANK1);
+	DUMPCORE(HDMI_CORE_SYS_VID_BLANK3);
+	DUMPCORE(HDMI_CORE_SYS_VID_BLANK1);
+	DUMPCORE(HDMI_CORE_SYS_DC_HEADER);
+	DUMPCORE(HDMI_CORE_SYS_VID_DITHER);
+	DUMPCORE(HDMI_CORE_SYS_RGB2XVYCC_CT);
+	DUMPCORE(HDMI_CORE_SYS_R2Y_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_R2Y_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_G2Y_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_G2Y_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_B2Y_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_B2Y_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_R2CB_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_R2CB_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_G2CB_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_G2CB_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_B2CB_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_B2CB_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_R2CR_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_R2CR_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_G2CR_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_G2CR_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_B2CR_COEFF_LOW);
+	DUMPCORE(HDMI_CORE_SYS_B2CR_COEFF_UP);
+	DUMPCORE(HDMI_CORE_SYS_RGB_OFFSET_LOW);
+	DUMPCORE(HDMI_CORE_SYS_RGB_OFFSET_UP);
+	DUMPCORE(HDMI_CORE_SYS_Y_OFFSET_LOW);
+	DUMPCORE(HDMI_CORE_SYS_Y_OFFSET_UP);
+	DUMPCORE(HDMI_CORE_SYS_CBCR_OFFSET_LOW);
+	DUMPCORE(HDMI_CORE_SYS_CBCR_OFFSET_UP);
+	DUMPCORE(HDMI_CORE_SYS_INTR_STATE);
+	DUMPCORE(HDMI_CORE_SYS_INTR1);
+	DUMPCORE(HDMI_CORE_SYS_INTR2);
+	DUMPCORE(HDMI_CORE_SYS_INTR3);
+	DUMPCORE(HDMI_CORE_SYS_INTR4);
+	DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK1);
+	DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK2);
+	DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK3);
+	DUMPCORE(HDMI_CORE_SYS_INTR_UNMASK4);
+	DUMPCORE(HDMI_CORE_SYS_INTR_CTRL);
+	DUMPCORE(HDMI_CORE_SYS_TMDS_CTRL);
+
+	DUMPCORE(HDMI_CORE_DDC_ADDR);
+	DUMPCORE(HDMI_CORE_DDC_SEGM);
+	DUMPCORE(HDMI_CORE_DDC_OFFSET);
+	DUMPCORE(HDMI_CORE_DDC_COUNT1);
+	DUMPCORE(HDMI_CORE_DDC_COUNT2);
+	DUMPCORE(HDMI_CORE_DDC_STATUS);
+	DUMPCORE(HDMI_CORE_DDC_CMD);
+	DUMPCORE(HDMI_CORE_DDC_DATA);
+
+	DUMPCOREAV(HDMI_CORE_AV_ACR_CTRL);
+	DUMPCOREAV(HDMI_CORE_AV_FREQ_SVAL);
+	DUMPCOREAV(HDMI_CORE_AV_N_SVAL1);
+	DUMPCOREAV(HDMI_CORE_AV_N_SVAL2);
+	DUMPCOREAV(HDMI_CORE_AV_N_SVAL3);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL1);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL2);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_SVAL3);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL1);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL2);
+	DUMPCOREAV(HDMI_CORE_AV_CTS_HVAL3);
+	DUMPCOREAV(HDMI_CORE_AV_AUD_MODE);
+	DUMPCOREAV(HDMI_CORE_AV_SPDIF_CTRL);
+	DUMPCOREAV(HDMI_CORE_AV_HW_SPDIF_FS);
+	DUMPCOREAV(HDMI_CORE_AV_SWAP_I2S);
+	DUMPCOREAV(HDMI_CORE_AV_SPDIF_ERTH);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_IN_MAP);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_IN_CTRL);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_CHST0);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_CHST1);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_CHST2);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_CHST4);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_CHST5);
+	DUMPCOREAV(HDMI_CORE_AV_ASRC);
+	DUMPCOREAV(HDMI_CORE_AV_I2S_IN_LEN);
+	DUMPCOREAV(HDMI_CORE_AV_HDMI_CTRL);
+	DUMPCOREAV(HDMI_CORE_AV_AUDO_TXSTAT);
+	DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_1);
+	DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_2);
+	DUMPCOREAV(HDMI_CORE_AV_AUD_PAR_BUSCLK_3);
+	DUMPCOREAV(HDMI_CORE_AV_TEST_TXCTRL);
+	DUMPCOREAV(HDMI_CORE_AV_DPD);
+	DUMPCOREAV(HDMI_CORE_AV_PB_CTRL1);
+	DUMPCOREAV(HDMI_CORE_AV_PB_CTRL2);
+	DUMPCOREAV(HDMI_CORE_AV_AVI_TYPE);
+	DUMPCOREAV(HDMI_CORE_AV_AVI_VERS);
+	DUMPCOREAV(HDMI_CORE_AV_AVI_LEN);
+	DUMPCOREAV(HDMI_CORE_AV_AVI_CHSUM);
+
+	for (i = 0; i < HDMI_CORE_AV_AVI_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_AVI_DBYTE);
+
+	DUMPCOREAV(HDMI_CORE_AV_SPD_TYPE);
+	DUMPCOREAV(HDMI_CORE_AV_SPD_VERS);
+	DUMPCOREAV(HDMI_CORE_AV_SPD_LEN);
+	DUMPCOREAV(HDMI_CORE_AV_SPD_CHSUM);
+
+	for (i = 0; i < HDMI_CORE_AV_SPD_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_SPD_DBYTE);
+
+	DUMPCOREAV(HDMI_CORE_AV_AUDIO_TYPE);
+	DUMPCOREAV(HDMI_CORE_AV_AUDIO_VERS);
+	DUMPCOREAV(HDMI_CORE_AV_AUDIO_LEN);
+	DUMPCOREAV(HDMI_CORE_AV_AUDIO_CHSUM);
+
+	for (i = 0; i < HDMI_CORE_AV_AUD_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_AUD_DBYTE);
+
+	DUMPCOREAV(HDMI_CORE_AV_MPEG_TYPE);
+	DUMPCOREAV(HDMI_CORE_AV_MPEG_VERS);
+	DUMPCOREAV(HDMI_CORE_AV_MPEG_LEN);
+	DUMPCOREAV(HDMI_CORE_AV_MPEG_CHSUM);
+
+	for (i = 0; i < HDMI_CORE_AV_MPEG_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_MPEG_DBYTE);
+
+	for (i = 0; i < HDMI_CORE_AV_GEN_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_GEN_DBYTE);
+
+	DUMPCOREAV(HDMI_CORE_AV_CP_BYTE1);
+
+	for (i = 0; i < HDMI_CORE_AV_GEN2_DBYTE_NELEMS; i++)
+		DUMPCOREAV2(i, HDMI_CORE_AV_GEN2_DBYTE);
+
+	DUMPCOREAV(HDMI_CORE_AV_CEC_ADDR_ID);
+}
+
+static void hdmi_core_audio_config(struct hdmi_core_data *core,
+					struct hdmi_core_audio_config *cfg)
+{
+	u32 r;
+	void __iomem *av_base = hdmi_av_base(core);
+
+	/*
+	 * Parameters for generation of Audio Clock Recovery packets
+	 */
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL1, cfg->n, 7, 0);
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL2, cfg->n >> 8, 7, 0);
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_N_SVAL3, cfg->n >> 16, 7, 0);
+
+	if (cfg->cts_mode == HDMI_AUDIO_CTS_MODE_SW) {
+		REG_FLD_MOD(av_base, HDMI_CORE_AV_CTS_SVAL1, cfg->cts, 7, 0);
+		REG_FLD_MOD(av_base,
+				HDMI_CORE_AV_CTS_SVAL2, cfg->cts >> 8, 7, 0);
+		REG_FLD_MOD(av_base,
+				HDMI_CORE_AV_CTS_SVAL3, cfg->cts >> 16, 7, 0);
+	} else {
+		REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_1,
+				cfg->aud_par_busclk, 7, 0);
+		REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_2,
+				(cfg->aud_par_busclk >> 8), 7, 0);
+		REG_FLD_MOD(av_base, HDMI_CORE_AV_AUD_PAR_BUSCLK_3,
+				(cfg->aud_par_busclk >> 16), 7, 0);
+	}
+
+	/* Set ACR clock divisor */
+	REG_FLD_MOD(av_base,
+			HDMI_CORE_AV_FREQ_SVAL, cfg->mclk_mode, 2, 0);
+
+	r = hdmi_read_reg(av_base, HDMI_CORE_AV_ACR_CTRL);
+	/*
+	 * Use TMDS clock for ACR packets. For devices that use
+	 * the MCLK, this is the first part of the MCLK initialization.
+	 */
+	r = FLD_MOD(r, 0, 2, 2);
+
+	r = FLD_MOD(r, cfg->en_acr_pkt, 1, 1);
+	r = FLD_MOD(r, cfg->cts_mode, 0, 0);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_ACR_CTRL, r);
+
+	/* For devices using MCLK, this completes its initialization. */
+	if (cfg->use_mclk)
+		REG_FLD_MOD(av_base, HDMI_CORE_AV_ACR_CTRL, 1, 2, 2);
+
+	/* Override of SPDIF sample frequency with value in I2S_CHST4 */
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_SPDIF_CTRL,
+						cfg->fs_override, 1, 1);
+
+	/*
+	 * Set IEC-60958-3 channel status word. It is passed to the IP
+	 * just as it is received. The user of the driver is responsible
+	 * for its contents.
+	 */
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST0,
+		       cfg->iec60958_cfg->status[0]);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST1,
+		       cfg->iec60958_cfg->status[1]);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST2,
+		       cfg->iec60958_cfg->status[2]);
+	/* yes, this is correct: status[3] goes to CHST4 register */
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST4,
+		       cfg->iec60958_cfg->status[3]);
+	/* yes, this is correct: status[4] goes to CHST5 register */
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_CHST5,
+		       cfg->iec60958_cfg->status[4]);
+
+	/* set I2S parameters */
+	r = hdmi_read_reg(av_base, HDMI_CORE_AV_I2S_IN_CTRL);
+	r = FLD_MOD(r, cfg->i2s_cfg.sck_edge_mode, 6, 6);
+	r = FLD_MOD(r, cfg->i2s_cfg.vbit, 4, 4);
+	r = FLD_MOD(r, cfg->i2s_cfg.justification, 2, 2);
+	r = FLD_MOD(r, cfg->i2s_cfg.direction, 1, 1);
+	r = FLD_MOD(r, cfg->i2s_cfg.shift, 0, 0);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_IN_CTRL, r);
+
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_I2S_IN_LEN,
+			cfg->i2s_cfg.in_length_bits, 3, 0);
+
+	/* Audio channels and mode parameters */
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_HDMI_CTRL, cfg->layout, 2, 1);
+	r = hdmi_read_reg(av_base, HDMI_CORE_AV_AUD_MODE);
+	r = FLD_MOD(r, cfg->i2s_cfg.active_sds, 7, 4);
+	r = FLD_MOD(r, cfg->en_dsd_audio, 3, 3);
+	r = FLD_MOD(r, cfg->en_parallel_aud_input, 2, 2);
+	r = FLD_MOD(r, cfg->en_spdif, 1, 1);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_MODE, r);
+
+	/* Audio channel mappings */
+	/* TODO: Make channel mapping dynamic. For now, map channels
+	 * in the ALSA order: FL/FR/RL/RR/C/LFE/SL/SR. Remapping is needed as
+	 * HDMI speaker order is different. See CEA-861 Section 6.6.2.
+	 */
+	hdmi_write_reg(av_base, HDMI_CORE_AV_I2S_IN_MAP, 0x78);
+	REG_FLD_MOD(av_base, HDMI_CORE_AV_SWAP_I2S, 1, 5, 5);
+}
+
+static void hdmi_core_audio_infoframe_cfg(struct hdmi_core_data *core,
+		struct snd_cea_861_aud_if *info_aud)
+{
+	u8 sum = 0, checksum = 0;
+	void __iomem *av_base = hdmi_av_base(core);
+
+	/*
+	 * Set audio info frame type, version and length as
+	 * described in HDMI 1.4a Section 8.2.2 specification.
+	 * Checksum calculation is defined in Section 5.3.5.
+	 */
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUDIO_TYPE, 0x84);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUDIO_VERS, 0x01);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUDIO_LEN, 0x0a);
+	sum += 0x84 + 0x001 + 0x00a;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(0),
+		       info_aud->db1_ct_cc);
+	sum += info_aud->db1_ct_cc;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(1),
+		       info_aud->db2_sf_ss);
+	sum += info_aud->db2_sf_ss;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(2), info_aud->db3);
+	sum += info_aud->db3;
+
+	/*
+	 * The OMAP HDMI IP requires to use the 8-channel channel code when
+	 * transmitting more than two channels.
+	 */
+	if (info_aud->db4_ca != 0x00)
+		info_aud->db4_ca = 0x13;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(3), info_aud->db4_ca);
+	sum += info_aud->db4_ca;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(4),
+		       info_aud->db5_dminh_lsv);
+	sum += info_aud->db5_dminh_lsv;
+
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(5), 0x00);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(6), 0x00);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(7), 0x00);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(8), 0x00);
+	hdmi_write_reg(av_base, HDMI_CORE_AV_AUD_DBYTE(9), 0x00);
+
+	checksum = 0x100 - sum;
+	hdmi_write_reg(av_base,
+					HDMI_CORE_AV_AUDIO_CHSUM, checksum);
+
+	/*
+	 * TODO: Add MPEG and SPD enable and repeat cfg when EDID parsing
+	 * is available.
+	 */
+}
+
+int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+		struct omap_dss_audio *audio, u32 pclk)
+{
+	struct hdmi_audio_format audio_format;
+	struct hdmi_audio_dma audio_dma;
+	struct hdmi_core_audio_config acore;
+	int err, n, cts, channel_count;
+	unsigned int fs_nr;
+	bool word_length_16b = false;
+
+	if (!audio || !audio->iec || !audio->cea || !core)
+		return -EINVAL;
+
+	acore.iec60958_cfg = audio->iec;
+	/*
+	 * In the IEC-60958 status word, check if the audio sample word length
+	 * is 16-bit as several optimizations can be performed in such case.
+	 */
+	if (!(audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24))
+		if (audio->iec->status[4] & IEC958_AES4_CON_WORDLEN_20_16)
+			word_length_16b = true;
+
+	/* I2S configuration. See Phillips' specification */
+	if (word_length_16b)
+		acore.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+	else
+		acore.i2s_cfg.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+	/*
+	 * The I2S input word length is twice the lenght given in the IEC-60958
+	 * status word. If the word size is greater than
+	 * 20 bits, increment by one.
+	 */
+	acore.i2s_cfg.in_length_bits = audio->iec->status[4]
+		& IEC958_AES4_CON_WORDLEN;
+	if (audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24)
+		acore.i2s_cfg.in_length_bits++;
+	acore.i2s_cfg.sck_edge_mode = HDMI_AUDIO_I2S_SCK_EDGE_RISING;
+	acore.i2s_cfg.vbit = HDMI_AUDIO_I2S_VBIT_FOR_PCM;
+	acore.i2s_cfg.direction = HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST;
+	acore.i2s_cfg.shift = HDMI_AUDIO_I2S_FIRST_BIT_SHIFT;
+
+	/* convert sample frequency to a number */
+	switch (audio->iec->status[3] & IEC958_AES3_CON_FS) {
+	case IEC958_AES3_CON_FS_32000:
+		fs_nr = 32000;
+		break;
+	case IEC958_AES3_CON_FS_44100:
+		fs_nr = 44100;
+		break;
+	case IEC958_AES3_CON_FS_48000:
+		fs_nr = 48000;
+		break;
+	case IEC958_AES3_CON_FS_88200:
+		fs_nr = 88200;
+		break;
+	case IEC958_AES3_CON_FS_96000:
+		fs_nr = 96000;
+		break;
+	case IEC958_AES3_CON_FS_176400:
+		fs_nr = 176400;
+		break;
+	case IEC958_AES3_CON_FS_192000:
+		fs_nr = 192000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = hdmi_compute_acr(pclk, fs_nr, &n, &cts);
+
+	/* Audio clock regeneration settings */
+	acore.n = n;
+	acore.cts = cts;
+	if (dss_has_feature(FEAT_HDMI_CTS_SWMODE)) {
+		acore.aud_par_busclk = 0;
+		acore.cts_mode = HDMI_AUDIO_CTS_MODE_SW;
+		acore.use_mclk = dss_has_feature(FEAT_HDMI_AUDIO_USE_MCLK);
+	} else {
+		acore.aud_par_busclk = (((128 * 31) - 1) << 8);
+		acore.cts_mode = HDMI_AUDIO_CTS_MODE_HW;
+		acore.use_mclk = true;
+	}
+
+	if (acore.use_mclk)
+		acore.mclk_mode = HDMI_AUDIO_MCLK_128FS;
+
+	/* Audio channels settings */
+	channel_count = (audio->cea->db1_ct_cc &
+			 CEA861_AUDIO_INFOFRAME_DB1CC) + 1;
+
+	switch (channel_count) {
+	case 2:
+		audio_format.active_chnnls_msk = 0x03;
+		break;
+	case 3:
+		audio_format.active_chnnls_msk = 0x07;
+		break;
+	case 4:
+		audio_format.active_chnnls_msk = 0x0f;
+		break;
+	case 5:
+		audio_format.active_chnnls_msk = 0x1f;
+		break;
+	case 6:
+		audio_format.active_chnnls_msk = 0x3f;
+		break;
+	case 7:
+		audio_format.active_chnnls_msk = 0x7f;
+		break;
+	case 8:
+		audio_format.active_chnnls_msk = 0xff;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/*
+	 * the HDMI IP needs to enable four stereo channels when transmitting
+	 * more than 2 audio channels.  Similarly, the channel count in the
+	 * Audio InfoFrame has to match the sample_present bits (some channels
+	 * are padded with zeroes)
+	 */
+	if (channel_count == 2) {
+		audio_format.stereo_channels = HDMI_AUDIO_STEREO_ONECHANNEL;
+		acore.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN;
+		acore.layout = HDMI_AUDIO_LAYOUT_2CH;
+	} else {
+		audio_format.stereo_channels = HDMI_AUDIO_STEREO_FOURCHANNELS;
+		acore.i2s_cfg.active_sds = HDMI_AUDIO_I2S_SD0_EN |
+				HDMI_AUDIO_I2S_SD1_EN | HDMI_AUDIO_I2S_SD2_EN |
+				HDMI_AUDIO_I2S_SD3_EN;
+		acore.layout = HDMI_AUDIO_LAYOUT_8CH;
+		audio->cea->db1_ct_cc = 7;
+	}
+
+	acore.en_spdif = false;
+	/* use sample frequency from channel status word */
+	acore.fs_override = true;
+	/* enable ACR packets */
+	acore.en_acr_pkt = true;
+	/* disable direct streaming digital audio */
+	acore.en_dsd_audio = false;
+	/* use parallel audio interface */
+	acore.en_parallel_aud_input = true;
+
+	/* DMA settings */
+	if (word_length_16b)
+		audio_dma.transfer_size = 0x10;
+	else
+		audio_dma.transfer_size = 0x20;
+	audio_dma.block_size = 0xC0;
+	audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+	audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+	/* audio FIFO format settings */
+	if (word_length_16b) {
+		audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+		audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+		audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+	} else {
+		audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_ONESAMPLE;
+		audio_format.sample_size = HDMI_AUDIO_SAMPLE_24BITS;
+		audio_format.justification = HDMI_AUDIO_JUSTIFY_RIGHT;
+	}
+	audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+	audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+	/* disable start/stop signals of IEC 60958 blocks */
+	audio_format.en_sig_blk_strt_end = HDMI_AUDIO_BLOCK_SIG_STARTEND_ON;
+
+	/* configure DMA and audio FIFO format*/
+	hdmi_wp_audio_config_dma(wp, &audio_dma);
+	hdmi_wp_audio_config_format(wp, &audio_format);
+
+	/* configure the core*/
+	hdmi_core_audio_config(core, &acore);
+
+	/* configure CEA 861 audio infoframe*/
+	hdmi_core_audio_infoframe_cfg(core, audio->cea);
+
+	return 0;
+}
+
+int hdmi4_audio_start(struct hdmi_core_data *core, struct hdmi_wp_data *wp)
+{
+	REG_FLD_MOD(hdmi_av_base(core),
+		    HDMI_CORE_AV_AUD_MODE, true, 0, 0);
+
+	hdmi_wp_audio_core_req_enable(wp, true);
+
+	return 0;
+}
+
+void hdmi4_audio_stop(struct hdmi_core_data *core, struct hdmi_wp_data *wp)
+{
+	REG_FLD_MOD(hdmi_av_base(core),
+		    HDMI_CORE_AV_AUD_MODE, false, 0, 0);
+
+	hdmi_wp_audio_core_req_enable(wp, false);
+}
+
+int hdmi4_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
+{
+	struct resource *res;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
+	if (!res) {
+		DSSERR("can't get CORE mem resource\n");
+		return -EINVAL;
+	}
+
+	core->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(core->base)) {
+		DSSERR("can't ioremap CORE\n");
+		return PTR_ERR(core->base);
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h
new file mode 100644
index 000000000000..a069f96ec6f6
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi4_core.h
@@ -0,0 +1,273 @@
+/*
+ * HDMI header definition for OMAP4 HDMI core IP
+ *
+ * Copyright (C) 2010-2011 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _HDMI4_CORE_H_
+#define _HDMI4_CORE_H_
+
+#include "hdmi.h"
+
+/* OMAP4 HDMI IP Core System */
+
+#define HDMI_CORE_SYS_VND_IDL			0x0
+#define HDMI_CORE_SYS_DEV_IDL			0x8
+#define HDMI_CORE_SYS_DEV_IDH			0xC
+#define HDMI_CORE_SYS_DEV_REV			0x10
+#define HDMI_CORE_SYS_SRST			0x14
+#define HDMI_CORE_SYS_SYS_CTRL1			0x20
+#define HDMI_CORE_SYS_SYS_STAT			0x24
+#define HDMI_CORE_SYS_SYS_CTRL3			0x28
+#define HDMI_CORE_SYS_DCTL			0x34
+#define HDMI_CORE_SYS_DE_DLY			0xC8
+#define HDMI_CORE_SYS_DE_CTRL			0xCC
+#define HDMI_CORE_SYS_DE_TOP			0xD0
+#define HDMI_CORE_SYS_DE_CNTL			0xD8
+#define HDMI_CORE_SYS_DE_CNTH			0xDC
+#define HDMI_CORE_SYS_DE_LINL			0xE0
+#define HDMI_CORE_SYS_DE_LINH_1			0xE4
+#define HDMI_CORE_SYS_HRES_L			0xE8
+#define HDMI_CORE_SYS_HRES_H			0xEC
+#define HDMI_CORE_SYS_VRES_L			0xF0
+#define HDMI_CORE_SYS_VRES_H			0xF4
+#define HDMI_CORE_SYS_IADJUST			0xF8
+#define HDMI_CORE_SYS_POLDETECT			0xFC
+#define HDMI_CORE_SYS_HWIDTH1			0x110
+#define HDMI_CORE_SYS_HWIDTH2			0x114
+#define HDMI_CORE_SYS_VWIDTH			0x11C
+#define HDMI_CORE_SYS_VID_CTRL			0x120
+#define HDMI_CORE_SYS_VID_ACEN			0x124
+#define HDMI_CORE_SYS_VID_MODE			0x128
+#define HDMI_CORE_SYS_VID_BLANK1		0x12C
+#define HDMI_CORE_SYS_VID_BLANK2		0x130
+#define HDMI_CORE_SYS_VID_BLANK3		0x134
+#define HDMI_CORE_SYS_DC_HEADER			0x138
+#define HDMI_CORE_SYS_VID_DITHER		0x13C
+#define HDMI_CORE_SYS_RGB2XVYCC_CT		0x140
+#define HDMI_CORE_SYS_R2Y_COEFF_LOW		0x144
+#define HDMI_CORE_SYS_R2Y_COEFF_UP		0x148
+#define HDMI_CORE_SYS_G2Y_COEFF_LOW		0x14C
+#define HDMI_CORE_SYS_G2Y_COEFF_UP		0x150
+#define HDMI_CORE_SYS_B2Y_COEFF_LOW		0x154
+#define HDMI_CORE_SYS_B2Y_COEFF_UP		0x158
+#define HDMI_CORE_SYS_R2CB_COEFF_LOW		0x15C
+#define HDMI_CORE_SYS_R2CB_COEFF_UP		0x160
+#define HDMI_CORE_SYS_G2CB_COEFF_LOW		0x164
+#define HDMI_CORE_SYS_G2CB_COEFF_UP		0x168
+#define HDMI_CORE_SYS_B2CB_COEFF_LOW		0x16C
+#define HDMI_CORE_SYS_B2CB_COEFF_UP		0x170
+#define HDMI_CORE_SYS_R2CR_COEFF_LOW		0x174
+#define HDMI_CORE_SYS_R2CR_COEFF_UP		0x178
+#define HDMI_CORE_SYS_G2CR_COEFF_LOW		0x17C
+#define HDMI_CORE_SYS_G2CR_COEFF_UP		0x180
+#define HDMI_CORE_SYS_B2CR_COEFF_LOW		0x184
+#define HDMI_CORE_SYS_B2CR_COEFF_UP		0x188
+#define HDMI_CORE_SYS_RGB_OFFSET_LOW		0x18C
+#define HDMI_CORE_SYS_RGB_OFFSET_UP		0x190
+#define HDMI_CORE_SYS_Y_OFFSET_LOW		0x194
+#define HDMI_CORE_SYS_Y_OFFSET_UP		0x198
+#define HDMI_CORE_SYS_CBCR_OFFSET_LOW		0x19C
+#define HDMI_CORE_SYS_CBCR_OFFSET_UP		0x1A0
+#define HDMI_CORE_SYS_INTR_STATE		0x1C0
+#define HDMI_CORE_SYS_INTR1			0x1C4
+#define HDMI_CORE_SYS_INTR2			0x1C8
+#define HDMI_CORE_SYS_INTR3			0x1CC
+#define HDMI_CORE_SYS_INTR4			0x1D0
+#define HDMI_CORE_SYS_INTR_UNMASK1		0x1D4
+#define HDMI_CORE_SYS_INTR_UNMASK2		0x1D8
+#define HDMI_CORE_SYS_INTR_UNMASK3		0x1DC
+#define HDMI_CORE_SYS_INTR_UNMASK4		0x1E0
+#define HDMI_CORE_SYS_INTR_CTRL			0x1E4
+#define HDMI_CORE_SYS_TMDS_CTRL			0x208
+
+/* value definitions for HDMI_CORE_SYS_SYS_CTRL1 fields */
+#define HDMI_CORE_SYS_SYS_CTRL1_VEN_FOLLOWVSYNC	0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_HEN_FOLLOWHSYNC	0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_BSEL_24BITBUS	0x1
+#define HDMI_CORE_SYS_SYS_CTRL1_EDGE_RISINGEDGE	0x1
+
+/* HDMI DDC E-DID */
+#define HDMI_CORE_DDC_ADDR			0x3B4
+#define HDMI_CORE_DDC_SEGM			0x3B8
+#define HDMI_CORE_DDC_OFFSET			0x3BC
+#define HDMI_CORE_DDC_COUNT1			0x3C0
+#define HDMI_CORE_DDC_COUNT2			0x3C4
+#define HDMI_CORE_DDC_STATUS			0x3C8
+#define HDMI_CORE_DDC_CMD			0x3CC
+#define HDMI_CORE_DDC_DATA			0x3D0
+
+/* HDMI IP Core Audio Video */
+
+#define HDMI_CORE_AV_ACR_CTRL			0x4
+#define HDMI_CORE_AV_FREQ_SVAL			0x8
+#define HDMI_CORE_AV_N_SVAL1			0xC
+#define HDMI_CORE_AV_N_SVAL2			0x10
+#define HDMI_CORE_AV_N_SVAL3			0x14
+#define HDMI_CORE_AV_CTS_SVAL1			0x18
+#define HDMI_CORE_AV_CTS_SVAL2			0x1C
+#define HDMI_CORE_AV_CTS_SVAL3			0x20
+#define HDMI_CORE_AV_CTS_HVAL1			0x24
+#define HDMI_CORE_AV_CTS_HVAL2			0x28
+#define HDMI_CORE_AV_CTS_HVAL3			0x2C
+#define HDMI_CORE_AV_AUD_MODE			0x50
+#define HDMI_CORE_AV_SPDIF_CTRL			0x54
+#define HDMI_CORE_AV_HW_SPDIF_FS		0x60
+#define HDMI_CORE_AV_SWAP_I2S			0x64
+#define HDMI_CORE_AV_SPDIF_ERTH			0x6C
+#define HDMI_CORE_AV_I2S_IN_MAP			0x70
+#define HDMI_CORE_AV_I2S_IN_CTRL		0x74
+#define HDMI_CORE_AV_I2S_CHST0			0x78
+#define HDMI_CORE_AV_I2S_CHST1			0x7C
+#define HDMI_CORE_AV_I2S_CHST2			0x80
+#define HDMI_CORE_AV_I2S_CHST4			0x84
+#define HDMI_CORE_AV_I2S_CHST5			0x88
+#define HDMI_CORE_AV_ASRC			0x8C
+#define HDMI_CORE_AV_I2S_IN_LEN			0x90
+#define HDMI_CORE_AV_HDMI_CTRL			0xBC
+#define HDMI_CORE_AV_AUDO_TXSTAT		0xC0
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_1		0xCC
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_2		0xD0
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_3		0xD4
+#define HDMI_CORE_AV_TEST_TXCTRL		0xF0
+#define HDMI_CORE_AV_DPD			0xF4
+#define HDMI_CORE_AV_PB_CTRL1			0xF8
+#define HDMI_CORE_AV_PB_CTRL2			0xFC
+#define HDMI_CORE_AV_AVI_BASE			0x100
+#define HDMI_CORE_AV_AVI_TYPE			0x100
+#define HDMI_CORE_AV_AVI_VERS			0x104
+#define HDMI_CORE_AV_AVI_LEN			0x108
+#define HDMI_CORE_AV_AVI_CHSUM			0x10C
+#define HDMI_CORE_AV_AVI_DBYTE(n)		(n * 4 + 0x110)
+#define HDMI_CORE_AV_SPD_TYPE			0x180
+#define HDMI_CORE_AV_SPD_VERS			0x184
+#define HDMI_CORE_AV_SPD_LEN			0x188
+#define HDMI_CORE_AV_SPD_CHSUM			0x18C
+#define HDMI_CORE_AV_SPD_DBYTE(n)		(n * 4 + 0x190)
+#define HDMI_CORE_AV_AUDIO_TYPE			0x200
+#define HDMI_CORE_AV_AUDIO_VERS			0x204
+#define HDMI_CORE_AV_AUDIO_LEN			0x208
+#define HDMI_CORE_AV_AUDIO_CHSUM		0x20C
+#define HDMI_CORE_AV_AUD_DBYTE(n)		(n * 4 + 0x210)
+#define HDMI_CORE_AV_MPEG_TYPE			0x280
+#define HDMI_CORE_AV_MPEG_VERS			0x284
+#define HDMI_CORE_AV_MPEG_LEN			0x288
+#define HDMI_CORE_AV_MPEG_CHSUM			0x28C
+#define HDMI_CORE_AV_MPEG_DBYTE(n)		(n * 4 + 0x290)
+#define HDMI_CORE_AV_GEN_DBYTE(n)		(n * 4 + 0x300)
+#define HDMI_CORE_AV_CP_BYTE1			0x37C
+#define HDMI_CORE_AV_GEN2_DBYTE(n)		(n * 4 + 0x380)
+#define HDMI_CORE_AV_CEC_ADDR_ID		0x3FC
+
+#define HDMI_CORE_AV_SPD_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_GEN2_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_MPEG_DBYTE_ELSIZE		0x4
+#define HDMI_CORE_AV_GEN_DBYTE_ELSIZE		0x4
+
+#define HDMI_CORE_AV_AVI_DBYTE_NELEMS		15
+#define HDMI_CORE_AV_SPD_DBYTE_NELEMS		27
+#define HDMI_CORE_AV_AUD_DBYTE_NELEMS		10
+#define HDMI_CORE_AV_MPEG_DBYTE_NELEMS		27
+#define HDMI_CORE_AV_GEN_DBYTE_NELEMS		31
+#define HDMI_CORE_AV_GEN2_DBYTE_NELEMS		31
+
+enum hdmi_core_inputbus_width {
+	HDMI_INPUT_8BIT = 0,
+	HDMI_INPUT_10BIT = 1,
+	HDMI_INPUT_12BIT = 2
+};
+
+enum hdmi_core_dither_trunc {
+	HDMI_OUTPUTTRUNCATION_8BIT = 0,
+	HDMI_OUTPUTTRUNCATION_10BIT = 1,
+	HDMI_OUTPUTTRUNCATION_12BIT = 2,
+	HDMI_OUTPUTDITHER_8BIT = 3,
+	HDMI_OUTPUTDITHER_10BIT = 4,
+	HDMI_OUTPUTDITHER_12BIT = 5
+};
+
+enum hdmi_core_deepcolor_ed {
+	HDMI_DEEPCOLORPACKECTDISABLE = 0,
+	HDMI_DEEPCOLORPACKECTENABLE = 1
+};
+
+enum hdmi_core_packet_mode {
+	HDMI_PACKETMODERESERVEDVALUE = 0,
+	HDMI_PACKETMODE24BITPERPIXEL = 4,
+	HDMI_PACKETMODE30BITPERPIXEL = 5,
+	HDMI_PACKETMODE36BITPERPIXEL = 6,
+	HDMI_PACKETMODE48BITPERPIXEL = 7
+};
+
+enum hdmi_core_tclkselclkmult {
+	HDMI_FPLL05IDCK = 0,
+	HDMI_FPLL10IDCK = 1,
+	HDMI_FPLL20IDCK = 2,
+	HDMI_FPLL40IDCK = 3
+};
+
+enum hdmi_core_packet_ctrl {
+	HDMI_PACKETENABLE = 1,
+	HDMI_PACKETDISABLE = 0,
+	HDMI_PACKETREPEATON = 1,
+	HDMI_PACKETREPEATOFF = 0
+};
+
+enum hdmi_audio_i2s_config {
+	HDMI_AUDIO_I2S_MSB_SHIFTED_FIRST = 0,
+	HDMI_AUDIO_I2S_LSB_SHIFTED_FIRST = 1,
+	HDMI_AUDIO_I2S_SCK_EDGE_FALLING = 0,
+	HDMI_AUDIO_I2S_SCK_EDGE_RISING = 1,
+	HDMI_AUDIO_I2S_VBIT_FOR_PCM = 0,
+	HDMI_AUDIO_I2S_VBIT_FOR_COMPRESSED = 1,
+	HDMI_AUDIO_I2S_FIRST_BIT_SHIFT = 0,
+	HDMI_AUDIO_I2S_FIRST_BIT_NO_SHIFT = 1,
+	HDMI_AUDIO_I2S_SD0_EN = 1,
+	HDMI_AUDIO_I2S_SD1_EN = 1 << 1,
+	HDMI_AUDIO_I2S_SD2_EN = 1 << 2,
+	HDMI_AUDIO_I2S_SD3_EN = 1 << 3,
+};
+
+struct hdmi_core_video_config {
+	enum hdmi_core_inputbus_width	ip_bus_width;
+	enum hdmi_core_dither_trunc	op_dither_truc;
+	enum hdmi_core_deepcolor_ed	deep_color_pkt;
+	enum hdmi_core_packet_mode	pkt_mode;
+	enum hdmi_core_hdmi_dvi		hdmi_dvi;
+	enum hdmi_core_tclkselclkmult	tclk_sel_clkmult;
+};
+
+struct hdmi_core_packet_enable_repeat {
+	u32	audio_pkt;
+	u32	audio_pkt_repeat;
+	u32	avi_infoframe;
+	u32	avi_infoframe_repeat;
+	u32	gen_cntrl_pkt;
+	u32	gen_cntrl_pkt_repeat;
+	u32	generic_pkt;
+	u32	generic_pkt_repeat;
+};
+
+int hdmi4_read_edid(struct hdmi_core_data *core, u8 *edid, int len);
+void hdmi4_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+		struct hdmi_config *cfg);
+void hdmi4_core_dump(struct hdmi_core_data *core, struct seq_file *s);
+int hdmi4_core_init(struct platform_device *pdev, struct hdmi_core_data *core);
+
+int hdmi4_audio_start(struct hdmi_core_data *core, struct hdmi_wp_data *wp);
+void hdmi4_audio_stop(struct hdmi_core_data *core, struct hdmi_wp_data *wp);
+int hdmi4_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+		struct omap_dss_audio *audio, u32 pclk);
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c
new file mode 100644
index 000000000000..a955a2c4c061
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5.c
@@ -0,0 +1,876 @@
+/*
+ * HDMI driver for OMAP5
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *	Yong Zhi
+ *	Mythri pk
+ *	Archit Taneja <archit@ti.com>
+ *	Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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 DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/clk.h>
+#include <linux/gpio.h>
+#include <linux/regulator/consumer.h>
+#include <linux/component.h>
+#include <video/omapdss.h>
+#include <sound/omap-hdmi-audio.h>
+
+#include "hdmi5_core.h"
+#include "dss.h"
+#include "dss_features.h"
+
+static struct omap_hdmi hdmi;
+
+static int hdmi_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0);
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static void hdmi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("hdmi_runtime_put\n");
+
+	r = pm_runtime_put_sync(&hdmi.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static irqreturn_t hdmi_irq_handler(int irq, void *data)
+{
+	struct hdmi_wp_data *wp = data;
+	u32 irqstatus;
+
+	irqstatus = hdmi_wp_get_irqstatus(wp);
+	hdmi_wp_set_irqstatus(wp, irqstatus);
+
+	if ((irqstatus & HDMI_IRQ_LINK_CONNECT) &&
+			irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		u32 v;
+		/*
+		 * If we get both connect and disconnect interrupts at the same
+		 * time, turn off the PHY, clear interrupts, and restart, which
+		 * raises connect interrupt if a cable is connected, or nothing
+		 * if cable is not connected.
+		 */
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_OFF);
+
+		/*
+		 * We always get bogus CONNECT & DISCONNECT interrupts when
+		 * setting the PHY to LDOON. To ignore those, we force the RXDET
+		 * line to 0 until the PHY power state has been changed.
+		 */
+		v = hdmi_read_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL);
+		v = FLD_MOD(v, 1, 15, 15); /* FORCE_RXDET_HIGH */
+		v = FLD_MOD(v, 0, 14, 7); /* RXDET_LINE */
+		hdmi_write_reg(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, v);
+
+		hdmi_wp_set_irqstatus(wp, HDMI_IRQ_LINK_CONNECT |
+				HDMI_IRQ_LINK_DISCONNECT);
+
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+
+		REG_FLD_MOD(hdmi.phy.base, HDMI_TXPHY_PAD_CFG_CTRL, 0, 15, 15);
+
+	} else if (irqstatus & HDMI_IRQ_LINK_CONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_TXON);
+	} else if (irqstatus & HDMI_IRQ_LINK_DISCONNECT) {
+		hdmi_wp_set_phy_pwr(wp, HDMI_PHYPWRCMD_LDOON);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int hdmi_init_regulator(void)
+{
+	int r;
+	struct regulator *reg;
+
+	if (hdmi.vdda_reg != NULL)
+		return 0;
+
+	reg = devm_regulator_get(&hdmi.pdev->dev, "vdda");
+	if (IS_ERR(reg)) {
+		DSSERR("can't get VDDA regulator\n");
+		return PTR_ERR(reg);
+	}
+
+	if (regulator_can_change_voltage(reg)) {
+		r = regulator_set_voltage(reg, 1800000, 1800000);
+		if (r) {
+			devm_regulator_put(reg);
+			DSSWARN("can't set the regulator voltage\n");
+			return r;
+		}
+	}
+
+	hdmi.vdda_reg = reg;
+
+	return 0;
+}
+
+static int hdmi_power_on_core(struct omap_dss_device *dssdev)
+{
+	int r;
+
+	r = regulator_enable(hdmi.vdda_reg);
+	if (r)
+		return r;
+
+	r = hdmi_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	/* Make selection of HDMI in DSS */
+	dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+	hdmi.core_enabled = true;
+
+	return 0;
+
+err_runtime_get:
+	regulator_disable(hdmi.vdda_reg);
+
+	return r;
+}
+
+static void hdmi_power_off_core(struct omap_dss_device *dssdev)
+{
+	hdmi.core_enabled = false;
+
+	hdmi_runtime_put();
+	regulator_disable(hdmi.vdda_reg);
+}
+
+static int hdmi_power_on_full(struct omap_dss_device *dssdev)
+{
+	int r;
+	struct omap_video_timings *p;
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+	struct dss_pll_clock_info hdmi_cinfo = { 0 };
+
+	r = hdmi_power_on_core(dssdev);
+	if (r)
+		return r;
+
+	p = &hdmi.cfg.timings;
+
+	DSSDBG("hdmi_power_on x_res= %d y_res = %d\n", p->x_res, p->y_res);
+
+	hdmi_pll_compute(&hdmi.pll, p->pixelclock, &hdmi_cinfo);
+
+	/* disable and clear irqs */
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+	hdmi_wp_set_irqstatus(&hdmi.wp,
+			hdmi_wp_get_irqstatus(&hdmi.wp));
+
+	r = dss_pll_enable(&hdmi.pll.pll);
+	if (r) {
+		DSSERR("Failed to enable PLL\n");
+		goto err_pll_enable;
+	}
+
+	r = dss_pll_set_config(&hdmi.pll.pll, &hdmi_cinfo);
+	if (r) {
+		DSSERR("Failed to configure PLL\n");
+		goto err_pll_cfg;
+	}
+
+	r = hdmi_phy_configure(&hdmi.phy, hdmi_cinfo.clkdco,
+		hdmi_cinfo.clkout[0]);
+	if (r) {
+		DSSDBG("Failed to start PHY\n");
+		goto err_phy_cfg;
+	}
+
+	r = hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_LDOON);
+	if (r)
+		goto err_phy_pwr;
+
+	hdmi5_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
+
+	/* bypass TV gamma table */
+	dispc_enable_gamma_table(0);
+
+	/* tv size */
+	dss_mgr_set_timings(mgr, p);
+
+	r = hdmi_wp_video_start(&hdmi.wp);
+	if (r)
+		goto err_vid_enable;
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		goto err_mgr_enable;
+
+	hdmi_wp_set_irqenable(&hdmi.wp,
+			HDMI_IRQ_LINK_CONNECT | HDMI_IRQ_LINK_DISCONNECT);
+
+	return 0;
+
+err_mgr_enable:
+	hdmi_wp_video_stop(&hdmi.wp);
+err_vid_enable:
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+err_phy_pwr:
+err_phy_cfg:
+err_pll_cfg:
+	dss_pll_disable(&hdmi.pll.pll);
+err_pll_enable:
+	hdmi_power_off_core(dssdev);
+	return -EIO;
+}
+
+static void hdmi_power_off_full(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = hdmi.output.manager;
+
+	hdmi_wp_clear_irqenable(&hdmi.wp, 0xffffffff);
+
+	dss_mgr_disable(mgr);
+
+	hdmi_wp_video_stop(&hdmi.wp);
+
+	hdmi_wp_set_phy_pwr(&hdmi.wp, HDMI_PHYPWRCMD_OFF);
+
+	dss_pll_disable(&hdmi.pll.pll);
+
+	hdmi_power_off_core(dssdev);
+}
+
+static int hdmi_display_check_timing(struct omap_dss_device *dssdev,
+					struct omap_video_timings *timings)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	/* TODO: proper interlace support */
+	if (timings->interlace)
+		return -EINVAL;
+
+	if (!dispc_mgr_timings_ok(out->dispc_channel, timings))
+		return -EINVAL;
+
+	return 0;
+}
+
+static void hdmi_display_set_timing(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	mutex_lock(&hdmi.lock);
+
+	hdmi.cfg.timings = *timings;
+
+	dispc_set_tv_pclk(timings->pixelclock);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static void hdmi_display_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	*timings = hdmi.cfg.timings;
+}
+
+static void hdmi_dump_regs(struct seq_file *s)
+{
+	mutex_lock(&hdmi.lock);
+
+	if (hdmi_runtime_get()) {
+		mutex_unlock(&hdmi.lock);
+		return;
+	}
+
+	hdmi_wp_dump(&hdmi.wp, s);
+	hdmi_pll_dump(&hdmi.pll, s);
+	hdmi_phy_dump(&hdmi.phy, s);
+	hdmi5_core_dump(&hdmi.core, s);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+}
+
+static int read_edid(u8 *buf, int len)
+{
+	int r;
+	int idlemode;
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_runtime_get();
+	BUG_ON(r);
+
+	idlemode = REG_GET(hdmi.wp.base, HDMI_WP_SYSCONFIG, 3, 2);
+	/* No-idle mode */
+	REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, 1, 3, 2);
+
+	r = hdmi5_read_edid(&hdmi.core,  buf, len);
+
+	REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, idlemode, 3, 2);
+
+	hdmi_runtime_put();
+	mutex_unlock(&hdmi.lock);
+
+	return r;
+}
+
+static void hdmi_start_audio_stream(struct omap_hdmi *hd)
+{
+	REG_FLD_MOD(hdmi.wp.base, HDMI_WP_SYSCONFIG, 1, 3, 2);
+	hdmi_wp_audio_enable(&hd->wp, true);
+	hdmi_wp_audio_core_req_enable(&hd->wp, true);
+}
+
+static void hdmi_stop_audio_stream(struct omap_hdmi *hd)
+{
+	hdmi_wp_audio_core_req_enable(&hd->wp, false);
+	hdmi_wp_audio_enable(&hd->wp, false);
+	REG_FLD_MOD(hd->wp.base, HDMI_WP_SYSCONFIG, hd->wp_idlemode, 3, 2);
+}
+
+static int hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+	unsigned long flags;
+	int r = 0;
+
+	DSSDBG("ENTER hdmi_display_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err0;
+	}
+
+	r = hdmi_power_on_full(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	if (hdmi.audio_configured) {
+		r = hdmi5_audio_config(&hdmi.core, &hdmi.wp, &hdmi.audio_config,
+				       hdmi.cfg.timings.pixelclock);
+		if (r) {
+			DSSERR("Error restoring audio configuration: %d", r);
+			hdmi.audio_abort_cb(&hdmi.pdev->dev);
+			hdmi.audio_configured = false;
+		}
+	}
+
+	spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+	if (hdmi.audio_configured && hdmi.audio_playing)
+		hdmi_start_audio_stream(&hdmi);
+	hdmi.display_enabled = true;
+	spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+	unsigned long flags;
+
+	DSSDBG("Enter hdmi_display_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	spin_lock_irqsave(&hdmi.audio_playing_lock, flags);
+	hdmi_stop_audio_stream(&hdmi);
+	hdmi.display_enabled = false;
+	spin_unlock_irqrestore(&hdmi.audio_playing_lock, flags);
+
+	hdmi_power_off_full(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_core_enable(struct omap_dss_device *dssdev)
+{
+	int r = 0;
+
+	DSSDBG("ENTER omapdss_hdmi_core_enable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	r = hdmi_power_on_core(dssdev);
+	if (r) {
+		DSSERR("failed to power on device\n");
+		goto err0;
+	}
+
+	mutex_unlock(&hdmi.lock);
+	return 0;
+
+err0:
+	mutex_unlock(&hdmi.lock);
+	return r;
+}
+
+static void hdmi_core_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("Enter omapdss_hdmi_core_disable\n");
+
+	mutex_lock(&hdmi.lock);
+
+	hdmi_power_off_core(dssdev);
+
+	mutex_unlock(&hdmi.lock);
+}
+
+static int hdmi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = hdmi_init_regulator();
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void hdmi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static int hdmi_read_edid(struct omap_dss_device *dssdev,
+		u8 *edid, int len)
+{
+	bool need_enable;
+	int r;
+
+	need_enable = hdmi.core_enabled == false;
+
+	if (need_enable) {
+		r = hdmi_core_enable(dssdev);
+		if (r)
+			return r;
+	}
+
+	r = read_edid(edid, len);
+
+	if (need_enable)
+		hdmi_core_disable(dssdev);
+
+	return r;
+}
+
+static int hdmi_set_infoframe(struct omap_dss_device *dssdev,
+		const struct hdmi_avi_infoframe *avi)
+{
+	hdmi.cfg.infoframe = *avi;
+	return 0;
+}
+
+static int hdmi_set_hdmi_mode(struct omap_dss_device *dssdev,
+		bool hdmi_mode)
+{
+	hdmi.cfg.hdmi_dvi_mode = hdmi_mode ? HDMI_HDMI : HDMI_DVI;
+	return 0;
+}
+
+static const struct omapdss_hdmi_ops hdmi_ops = {
+	.connect		= hdmi_connect,
+	.disconnect		= hdmi_disconnect,
+
+	.enable			= hdmi_display_enable,
+	.disable		= hdmi_display_disable,
+
+	.check_timings		= hdmi_display_check_timing,
+	.set_timings		= hdmi_display_set_timing,
+	.get_timings		= hdmi_display_get_timings,
+
+	.read_edid		= hdmi_read_edid,
+	.set_infoframe		= hdmi_set_infoframe,
+	.set_hdmi_mode		= hdmi_set_hdmi_mode,
+};
+
+static void hdmi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_HDMI;
+	out->output_type = OMAP_DISPLAY_TYPE_HDMI;
+	out->name = "hdmi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+	out->ops.hdmi = &hdmi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void hdmi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &hdmi.output;
+
+	omapdss_unregister_output(out);
+}
+
+static int hdmi_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep;
+	int r;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	r = hdmi_parse_lanes_of(pdev, ep, &hdmi.phy);
+	if (r)
+		goto err;
+
+	of_node_put(ep);
+	return 0;
+
+err:
+	of_node_put(ep);
+	return r;
+}
+
+/* Audio callbacks */
+static int hdmi_audio_startup(struct device *dev,
+			      void (*abort_cb)(struct device *dev))
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&hd->lock);
+
+	if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	hd->audio_abort_cb = abort_cb;
+
+out:
+	mutex_unlock(&hd->lock);
+
+	return ret;
+}
+
+static int hdmi_audio_shutdown(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+
+	mutex_lock(&hd->lock);
+	hd->audio_abort_cb = NULL;
+	hd->audio_configured = false;
+	hd->audio_playing = false;
+	mutex_unlock(&hd->lock);
+
+	return 0;
+}
+
+static int hdmi_audio_start(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+	spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+	if (hd->display_enabled)
+		hdmi_start_audio_stream(hd);
+	hd->audio_playing = true;
+
+	spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+	return 0;
+}
+
+static void hdmi_audio_stop(struct device *dev)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	unsigned long flags;
+
+	WARN_ON(!hdmi_mode_has_audio(&hd->cfg));
+
+	spin_lock_irqsave(&hd->audio_playing_lock, flags);
+
+	if (hd->display_enabled)
+		hdmi_stop_audio_stream(hd);
+	hd->audio_playing = false;
+
+	spin_unlock_irqrestore(&hd->audio_playing_lock, flags);
+}
+
+static int hdmi_audio_config(struct device *dev,
+			     struct omap_dss_audio *dss_audio)
+{
+	struct omap_hdmi *hd = dev_get_drvdata(dev);
+	int ret;
+
+	mutex_lock(&hd->lock);
+
+	if (!hdmi_mode_has_audio(&hd->cfg) || !hd->display_enabled) {
+		ret = -EPERM;
+		goto out;
+	}
+
+	ret = hdmi5_audio_config(&hd->core, &hd->wp, dss_audio,
+				 hd->cfg.timings.pixelclock);
+
+	if (!ret) {
+		hd->audio_configured = true;
+		hd->audio_config = *dss_audio;
+	}
+out:
+	mutex_unlock(&hd->lock);
+
+	return ret;
+}
+
+static const struct omap_hdmi_audio_ops hdmi_audio_ops = {
+	.audio_startup = hdmi_audio_startup,
+	.audio_shutdown = hdmi_audio_shutdown,
+	.audio_start = hdmi_audio_start,
+	.audio_stop = hdmi_audio_stop,
+	.audio_config = hdmi_audio_config,
+};
+
+static int hdmi_audio_register(struct device *dev)
+{
+	struct omap_hdmi_audio_pdata pdata = {
+		.dev = dev,
+		.dss_version = omapdss_get_version(),
+		.audio_dma_addr = hdmi_wp_get_audio_dma_addr(&hdmi.wp),
+		.ops = &hdmi_audio_ops,
+	};
+
+	hdmi.audio_pdev = platform_device_register_data(
+		dev, "omap-hdmi-audio", PLATFORM_DEVID_AUTO,
+		&pdata, sizeof(pdata));
+
+	if (IS_ERR(hdmi.audio_pdev))
+		return PTR_ERR(hdmi.audio_pdev);
+
+	hdmi_runtime_get();
+	hdmi.wp_idlemode =
+		REG_GET(hdmi.wp.base, HDMI_WP_SYSCONFIG, 3, 2);
+	hdmi_runtime_put();
+
+	return 0;
+}
+
+/* HDMI HW IP initialisation */
+static int hdmi5_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	int r;
+	int irq;
+
+	hdmi.pdev = pdev;
+	dev_set_drvdata(&pdev->dev, &hdmi);
+
+	mutex_init(&hdmi.lock);
+	spin_lock_init(&hdmi.audio_playing_lock);
+
+	if (pdev->dev.of_node) {
+		r = hdmi_probe_of(pdev);
+		if (r)
+			return r;
+	}
+
+	r = hdmi_wp_init(pdev, &hdmi.wp);
+	if (r)
+		return r;
+
+	r = hdmi_pll_init(pdev, &hdmi.pll, &hdmi.wp);
+	if (r)
+		return r;
+
+	r = hdmi_phy_init(pdev, &hdmi.phy);
+	if (r)
+		goto err;
+
+	r = hdmi5_core_init(pdev, &hdmi.core);
+	if (r)
+		goto err;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		DSSERR("platform_get_irq failed\n");
+		r = -ENODEV;
+		goto err;
+	}
+
+	r = devm_request_threaded_irq(&pdev->dev, irq,
+			NULL, hdmi_irq_handler,
+			IRQF_ONESHOT, "OMAP HDMI", &hdmi.wp);
+	if (r) {
+		DSSERR("HDMI IRQ request failed\n");
+		goto err;
+	}
+
+	pm_runtime_enable(&pdev->dev);
+
+	hdmi_init_output(pdev);
+
+	r = hdmi_audio_register(&pdev->dev);
+	if (r) {
+		DSSERR("Registering HDMI audio failed %d\n", r);
+		hdmi_uninit_output(pdev);
+		pm_runtime_disable(&pdev->dev);
+		return r;
+	}
+
+	dss_debugfs_create_file("hdmi", hdmi_dump_regs);
+
+	return 0;
+err:
+	hdmi_pll_uninit(&hdmi.pll);
+	return r;
+}
+
+static void hdmi5_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	if (hdmi.audio_pdev)
+		platform_device_unregister(hdmi.audio_pdev);
+
+	hdmi_uninit_output(pdev);
+
+	hdmi_pll_uninit(&hdmi.pll);
+
+	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops hdmi5_component_ops = {
+	.bind	= hdmi5_bind,
+	.unbind	= hdmi5_unbind,
+};
+
+static int hdmi5_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &hdmi5_component_ops);
+}
+
+static int hdmi5_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &hdmi5_component_ops);
+	return 0;
+}
+
+static int hdmi_runtime_suspend(struct device *dev)
+{
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int hdmi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static const struct dev_pm_ops hdmi_pm_ops = {
+	.runtime_suspend = hdmi_runtime_suspend,
+	.runtime_resume = hdmi_runtime_resume,
+};
+
+static const struct of_device_id hdmi_of_match[] = {
+	{ .compatible = "ti,omap5-hdmi", },
+	{ .compatible = "ti,dra7-hdmi", },
+	{},
+};
+
+static struct platform_driver omapdss_hdmihw_driver = {
+	.probe		= hdmi5_probe,
+	.remove		= hdmi5_remove,
+	.driver         = {
+		.name   = "omapdss_hdmi5",
+		.pm	= &hdmi_pm_ops,
+		.of_match_table = hdmi_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init hdmi5_init_platform_driver(void)
+{
+	return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi5_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c
new file mode 100644
index 000000000000..8ea531d2652c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.c
@@ -0,0 +1,916 @@
+/*
+ * OMAP5 HDMI CORE IP driver library
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * Authors:
+ *	Yong Zhi
+ *	Mythri pk
+ *	Archit Taneja <archit@ti.com>
+ *	Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <drm/drm_edid.h>
+#include <sound/asound.h>
+#include <sound/asoundef.h>
+
+#include "hdmi5_core.h"
+
+/* only 24 bit color depth used for now */
+static const struct csc_table csc_table_deepcolor[] = {
+	/* HDMI_DEEP_COLOR_24BIT */
+	[0] = { 7036, 0, 0, 32, 0, 7036, 0, 32, 0, 0, 7036, 32, },
+	/* HDMI_DEEP_COLOR_30BIT */
+	[1] = { 7015, 0, 0, 128, 0, 7015, 0, 128, 0, 0, 7015, 128, },
+	/* HDMI_DEEP_COLOR_36BIT */
+	[2] = { 7010, 0, 0, 512, 0, 7010, 0, 512, 0, 0, 7010, 512, },
+	/* FULL RANGE */
+	[3] = { 8192, 0, 0, 0, 0, 8192, 0, 0, 0, 0, 8192, 0, },
+};
+
+static void hdmi_core_ddc_init(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+	const unsigned long long iclk = 266000000;	/* DSS L3 ICLK */
+	const unsigned ss_scl_high = 4000;		/* ns */
+	const unsigned ss_scl_low = 4700;		/* ns */
+	const unsigned fs_scl_high = 600;		/* ns */
+	const unsigned fs_scl_low = 1300;		/* ns */
+	const unsigned sda_hold = 1000;			/* ns */
+	const unsigned sfr_div = 10;
+	unsigned long long sfr;
+	unsigned v;
+
+	sfr = iclk / sfr_div;	/* SFR_DIV */
+	sfr /= 1000;		/* SFR clock in kHz */
+
+	/* Reset */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SOFTRSTZ, 0, 0, 0);
+	if (hdmi_wait_for_bit_change(base, HDMI_CORE_I2CM_SOFTRSTZ,
+				0, 0, 1) != 1)
+		DSSERR("HDMI I2CM reset failed\n");
+
+	/* Standard (0) or Fast (1) Mode */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_DIV, 0, 3, 3);
+
+	/* Standard Mode SCL High counter */
+	v = DIV_ROUND_UP_ULL(ss_scl_high * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Standard Mode SCL Low counter */
+	v = DIV_ROUND_UP_ULL(ss_scl_low * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Fast Mode SCL High Counter */
+	v = DIV_ROUND_UP_ULL(fs_scl_high * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* Fast Mode SCL Low Counter */
+	v = DIV_ROUND_UP_ULL(fs_scl_low * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR,
+			(v >> 8) & 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR,
+			v & 0xff, 7, 0);
+
+	/* SDA Hold Time */
+	v = DIV_ROUND_UP_ULL(sda_hold * sfr, 1000000);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SDA_HOLD_ADDR, v & 0xff, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SLAVE, 0x50, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGADDR, 0x30, 6, 0);
+
+	/* NACK_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 7, 7);
+
+	/* NACK_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 6, 6);
+
+	/* ARBITRATION_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 3, 3);
+
+	/* ARBITRATION_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x0, 2, 2);
+
+	/* DONE_POL to high */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 3, 3);
+
+	/* DONE_MASK to unmasked */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x0, 2, 2);
+}
+
+static void hdmi_core_ddc_uninit(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	/* Mask I2C interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+}
+
+static int hdmi_core_ddc_edid(struct hdmi_core_data *core, u8 *pedid, u8 ext)
+{
+	void __iomem *base = core->base;
+	u8 cur_addr;
+	char checksum = 0;
+	const int retries = 1000;
+	u8 seg_ptr = ext / 2;
+	u8 edidbase = ((ext % 2) * 0x80);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_SEGPTR, seg_ptr, 7, 0);
+
+	/*
+	 * TODO: We use polling here, although we probably should use proper
+	 * interrupts.
+	 */
+	for (cur_addr = 0; cur_addr < 128; ++cur_addr) {
+		int i;
+
+		/* clear ERROR and DONE */
+		REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+		REG_FLD_MOD(base, HDMI_CORE_I2CM_ADDRESS,
+				edidbase + cur_addr, 7, 0);
+
+		if (seg_ptr)
+			REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 1, 1);
+		else
+			REG_FLD_MOD(base, HDMI_CORE_I2CM_OPERATION, 1, 0, 0);
+
+		for (i = 0; i < retries; ++i) {
+			u32 stat;
+
+			stat = REG_GET(base, HDMI_CORE_IH_I2CM_STAT0, 1, 0);
+
+			/* I2CM_ERROR */
+			if (stat & 1) {
+				DSSERR("HDMI I2C Master Error\n");
+				return -EIO;
+			}
+
+			/* I2CM_DONE */
+			if (stat & (1 << 1))
+				break;
+
+			usleep_range(250, 1000);
+		}
+
+		if (i == retries) {
+			DSSERR("HDMI I2C timeout reading EDID\n");
+			return -EIO;
+		}
+
+		pedid[cur_addr] = REG_GET(base, HDMI_CORE_I2CM_DATAI, 7, 0);
+		checksum += pedid[cur_addr];
+	}
+
+	return 0;
+
+}
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len)
+{
+	int r, n, i;
+	int max_ext_blocks = (len / 128) - 1;
+
+	if (len < 128)
+		return -EINVAL;
+
+	hdmi_core_ddc_init(core);
+
+	r = hdmi_core_ddc_edid(core, edid, 0);
+	if (r)
+		goto out;
+
+	n = edid[0x7e];
+
+	if (n > max_ext_blocks)
+		n = max_ext_blocks;
+
+	for (i = 1; i <= n; i++) {
+		r = hdmi_core_ddc_edid(core, edid + i * EDID_LENGTH, i);
+		if (r)
+			goto out;
+	}
+
+out:
+	hdmi_core_ddc_uninit(core);
+
+	return r ? r : len;
+}
+
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s)
+{
+
+#define DUMPCORE(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(core->base, r))
+
+	DUMPCORE(HDMI_CORE_FC_INVIDCONF);
+	DUMPCORE(HDMI_CORE_FC_INHACTIV0);
+	DUMPCORE(HDMI_CORE_FC_INHACTIV1);
+	DUMPCORE(HDMI_CORE_FC_INHBLANK0);
+	DUMPCORE(HDMI_CORE_FC_INHBLANK1);
+	DUMPCORE(HDMI_CORE_FC_INVACTIV0);
+	DUMPCORE(HDMI_CORE_FC_INVACTIV1);
+	DUMPCORE(HDMI_CORE_FC_INVBLANK);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY0);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINDELAY1);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH0);
+	DUMPCORE(HDMI_CORE_FC_HSYNCINWIDTH1);
+	DUMPCORE(HDMI_CORE_FC_VSYNCINDELAY);
+	DUMPCORE(HDMI_CORE_FC_VSYNCINWIDTH);
+	DUMPCORE(HDMI_CORE_FC_CTRLDUR);
+	DUMPCORE(HDMI_CORE_FC_EXCTRLDUR);
+	DUMPCORE(HDMI_CORE_FC_EXCTRLSPAC);
+	DUMPCORE(HDMI_CORE_FC_CH0PREAM);
+	DUMPCORE(HDMI_CORE_FC_CH1PREAM);
+	DUMPCORE(HDMI_CORE_FC_CH2PREAM);
+	DUMPCORE(HDMI_CORE_FC_AVICONF0);
+	DUMPCORE(HDMI_CORE_FC_AVICONF1);
+	DUMPCORE(HDMI_CORE_FC_AVICONF2);
+	DUMPCORE(HDMI_CORE_FC_AVIVID);
+	DUMPCORE(HDMI_CORE_FC_PRCONF);
+
+	DUMPCORE(HDMI_CORE_MC_CLKDIS);
+	DUMPCORE(HDMI_CORE_MC_SWRSTZREQ);
+	DUMPCORE(HDMI_CORE_MC_FLOWCTRL);
+	DUMPCORE(HDMI_CORE_MC_PHYRSTZ);
+	DUMPCORE(HDMI_CORE_MC_LOCKONCLOCK);
+
+	DUMPCORE(HDMI_CORE_I2CM_SLAVE);
+	DUMPCORE(HDMI_CORE_I2CM_ADDRESS);
+	DUMPCORE(HDMI_CORE_I2CM_DATAO);
+	DUMPCORE(HDMI_CORE_I2CM_DATAI);
+	DUMPCORE(HDMI_CORE_I2CM_OPERATION);
+	DUMPCORE(HDMI_CORE_I2CM_INT);
+	DUMPCORE(HDMI_CORE_I2CM_CTLINT);
+	DUMPCORE(HDMI_CORE_I2CM_DIV);
+	DUMPCORE(HDMI_CORE_I2CM_SEGADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SOFTRSTZ);
+	DUMPCORE(HDMI_CORE_I2CM_SEGPTR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR);
+	DUMPCORE(HDMI_CORE_I2CM_SDA_HOLD_ADDR);
+}
+
+static void hdmi_core_init(struct hdmi_core_vid_config *video_cfg,
+			struct hdmi_config *cfg)
+{
+	DSSDBG("hdmi_core_init\n");
+
+	/* video core */
+	video_cfg->data_enable_pol = 1; /* It is always 1*/
+	video_cfg->v_fc_config.timings.hsync_level = cfg->timings.hsync_level;
+	video_cfg->v_fc_config.timings.x_res = cfg->timings.x_res;
+	video_cfg->v_fc_config.timings.hsw = cfg->timings.hsw - 1;
+	video_cfg->v_fc_config.timings.hbp = cfg->timings.hbp;
+	video_cfg->v_fc_config.timings.hfp = cfg->timings.hfp;
+	video_cfg->hblank = cfg->timings.hfp +
+				cfg->timings.hbp + cfg->timings.hsw - 1;
+	video_cfg->v_fc_config.timings.vsync_level = cfg->timings.vsync_level;
+	video_cfg->v_fc_config.timings.y_res = cfg->timings.y_res;
+	video_cfg->v_fc_config.timings.vsw = cfg->timings.vsw;
+	video_cfg->v_fc_config.timings.vfp = cfg->timings.vfp;
+	video_cfg->v_fc_config.timings.vbp = cfg->timings.vbp;
+	video_cfg->vblank_osc = 0; /* Always 0 - need to confirm */
+	video_cfg->vblank = cfg->timings.vsw +
+				cfg->timings.vfp + cfg->timings.vbp;
+	video_cfg->v_fc_config.hdmi_dvi_mode = cfg->hdmi_dvi_mode;
+	video_cfg->v_fc_config.timings.interlace = cfg->timings.interlace;
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_data *core,
+			struct hdmi_core_vid_config *cfg)
+{
+	void __iomem *base = core->base;
+	unsigned char r = 0;
+	bool vsync_pol, hsync_pol;
+
+	vsync_pol =
+		cfg->v_fc_config.timings.vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+	hsync_pol =
+		cfg->v_fc_config.timings.hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
+	/* Set hsync, vsync and data-enable polarity  */
+	r = hdmi_read_reg(base, HDMI_CORE_FC_INVIDCONF);
+	r = FLD_MOD(r, vsync_pol, 6, 6);
+	r = FLD_MOD(r, hsync_pol, 5, 5);
+	r = FLD_MOD(r, cfg->data_enable_pol, 4, 4);
+	r = FLD_MOD(r, cfg->vblank_osc, 1, 1);
+	r = FLD_MOD(r, cfg->v_fc_config.timings.interlace, 0, 0);
+	hdmi_write_reg(base, HDMI_CORE_FC_INVIDCONF, r);
+
+	/* set x resolution */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV1,
+			cfg->v_fc_config.timings.x_res >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHACTIV0,
+			cfg->v_fc_config.timings.x_res & 0xFF, 7, 0);
+
+	/* set y resolution */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV1,
+			cfg->v_fc_config.timings.y_res >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVACTIV0,
+			cfg->v_fc_config.timings.y_res & 0xFF, 7, 0);
+
+	/* set horizontal blanking pixels */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK1, cfg->hblank >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_INHBLANK0, cfg->hblank & 0xFF, 7, 0);
+
+	/* set vertial blanking pixels */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVBLANK, cfg->vblank, 7, 0);
+
+	/* set horizontal sync offset */
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY1,
+			cfg->v_fc_config.timings.hfp >> 8, 4, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINDELAY0,
+			cfg->v_fc_config.timings.hfp & 0xFF, 7, 0);
+
+	/* set vertical sync offset */
+	REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINDELAY,
+			cfg->v_fc_config.timings.vfp, 7, 0);
+
+	/* set horizontal sync pulse width */
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH1,
+			(cfg->v_fc_config.timings.hsw >> 8), 1, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_HSYNCINWIDTH0,
+			cfg->v_fc_config.timings.hsw & 0xFF, 7, 0);
+
+	/*  set vertical sync pulse width */
+	REG_FLD_MOD(base, HDMI_CORE_FC_VSYNCINWIDTH,
+			cfg->v_fc_config.timings.vsw, 5, 0);
+
+	/* select DVI mode */
+	REG_FLD_MOD(base, HDMI_CORE_FC_INVIDCONF,
+			cfg->v_fc_config.hdmi_dvi_mode, 3, 3);
+}
+
+static void hdmi_core_config_video_packetizer(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+	int clr_depth = 0;	/* 24 bit color depth */
+
+	/* COLOR_DEPTH */
+	REG_FLD_MOD(base, HDMI_CORE_VP_PR_CD, clr_depth, 7, 4);
+	/* BYPASS_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 1, 6, 6);
+	/* PP_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 1 : 0, 5, 5);
+	/* YCC422_EN */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, 0, 3, 3);
+	/* PP_STUFFING */
+	REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, clr_depth ? 1 : 0, 1, 1);
+	/* YCC422_STUFFING */
+	REG_FLD_MOD(base, HDMI_CORE_VP_STUFF, 1, 2, 2);
+	/* OUTPUT_SELECTOR */
+	REG_FLD_MOD(base, HDMI_CORE_VP_CONF, clr_depth ? 0 : 2, 1, 0);
+}
+
+static void hdmi_core_config_csc(struct hdmi_core_data *core)
+{
+	int clr_depth = 0;	/* 24 bit color depth */
+
+	/* CSC_COLORDEPTH */
+	REG_FLD_MOD(core->base, HDMI_CORE_CSC_SCALE, clr_depth, 7, 4);
+}
+
+static void hdmi_core_config_video_sampler(struct hdmi_core_data *core)
+{
+	int video_mapping = 1;	/* for 24 bit color depth */
+
+	/* VIDEO_MAPPING */
+	REG_FLD_MOD(core->base, HDMI_CORE_TX_INVID0, video_mapping, 4, 0);
+}
+
+static void hdmi_core_write_avi_infoframe(struct hdmi_core_data *core,
+	struct hdmi_avi_infoframe *frame)
+{
+	void __iomem *base = core->base;
+	u8 data[HDMI_INFOFRAME_SIZE(AVI)];
+	u8 *ptr;
+	unsigned y, a, b, s;
+	unsigned c, m, r;
+	unsigned itc, ec, q, sc;
+	unsigned vic;
+	unsigned yq, cn, pr;
+
+	hdmi_avi_infoframe_pack(frame, data, sizeof(data));
+
+	print_hex_dump_debug("AVI: ", DUMP_PREFIX_NONE, 16, 1, data,
+		HDMI_INFOFRAME_SIZE(AVI), false);
+
+	ptr = data + HDMI_INFOFRAME_HEADER_SIZE;
+
+	y = (ptr[0] >> 5) & 0x3;
+	a = (ptr[0] >> 4) & 0x1;
+	b = (ptr[0] >> 2) & 0x3;
+	s = (ptr[0] >> 0) & 0x3;
+
+	c = (ptr[1] >> 6) & 0x3;
+	m = (ptr[1] >> 4) & 0x3;
+	r = (ptr[1] >> 0) & 0x3;
+
+	itc = (ptr[2] >> 7) & 0x1;
+	ec = (ptr[2] >> 4) & 0x7;
+	q = (ptr[2] >> 2) & 0x3;
+	sc = (ptr[2] >> 0) & 0x3;
+
+	vic = ptr[3];
+
+	yq = (ptr[4] >> 6) & 0x3;
+	cn = (ptr[4] >> 4) & 0x3;
+	pr = (ptr[4] >> 0) & 0xf;
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AVICONF0,
+		(a << 6) | (s << 4) | (b << 2) | (y << 0));
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AVICONF1,
+		(c << 6) | (m << 4) | (r << 0));
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AVICONF2,
+		(itc << 7) | (ec << 4) | (q << 2) | (sc << 0));
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AVIVID, vic);
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AVICONF3,
+		(yq << 2) | (cn << 0));
+
+	REG_FLD_MOD(base, HDMI_CORE_FC_PRCONF, pr, 3, 0);
+}
+
+static void hdmi_core_csc_config(struct hdmi_core_data *core,
+		struct csc_table csc_coeff)
+{
+	void __iomem *base = core->base;
+
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_MSB, csc_coeff.a1 >> 8 , 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A1_LSB, csc_coeff.a1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_MSB, csc_coeff.a2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A2_LSB, csc_coeff.a2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_MSB, csc_coeff.a3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A3_LSB, csc_coeff.a3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_MSB, csc_coeff.a4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_A4_LSB, csc_coeff.a4, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_MSB, csc_coeff.b1 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B1_LSB, csc_coeff.b1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_MSB, csc_coeff.b2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B2_LSB, csc_coeff.b2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_MSB, csc_coeff.b3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B3_LSB, csc_coeff.b3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_MSB, csc_coeff.b4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_B4_LSB, csc_coeff.b4, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_MSB, csc_coeff.c1 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C1_LSB, csc_coeff.c1, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_MSB, csc_coeff.c2 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C2_LSB, csc_coeff.c2, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_MSB, csc_coeff.c3 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C3_LSB, csc_coeff.c3, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_MSB, csc_coeff.c4 >> 8, 6, 0);
+	REG_FLD_MOD(base, HDMI_CORE_CSC_COEF_C4_LSB, csc_coeff.c4, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_MC_FLOWCTRL, 0x1, 0, 0);
+}
+
+static void hdmi_core_configure_range(struct hdmi_core_data *core)
+{
+	struct csc_table csc_coeff = { 0 };
+
+	/* support limited range with 24 bit color depth for now */
+	csc_coeff = csc_table_deepcolor[0];
+
+	hdmi_core_csc_config(core, csc_coeff);
+}
+
+static void hdmi_core_enable_video_path(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	DSSDBG("hdmi_core_enable_video_path\n");
+
+	REG_FLD_MOD(base, HDMI_CORE_FC_CTRLDUR, 0x0C, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLDUR, 0x20, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_EXCTRLSPAC, 0x01, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH0PREAM, 0x0B, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH1PREAM, 0x16, 5, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_CH2PREAM, 0x21, 5, 0);
+	REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 0, 0);
+	REG_FLD_MOD(base, HDMI_CORE_MC_CLKDIS, 0x00, 1, 1);
+}
+
+static void hdmi_core_mask_interrupts(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	/* Master IRQ mask */
+	REG_FLD_MOD(base, HDMI_CORE_IH_MUTE, 0x3, 1, 0);
+
+	/* Mask all the interrupts in HDMI core */
+
+	REG_FLD_MOD(base, HDMI_CORE_VP_MASK, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK0, 0xe7, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK1, 0xfb, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_MASK2, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 0x3, 3, 2);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_CEC_MASK, 0x7f, 6, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 6, 6);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_CTLINT, 0x1, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_I2CM_INT, 0x1, 2, 2);
+
+	REG_FLD_MOD(base, HDMI_CORE_PHY_MASK0, 0xf3, 7, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+
+	/* Clear all the current interrupt bits */
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xe7, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xfb, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0x7, 2, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0x7f, 6, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0x3, 1, 0);
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+}
+
+static void hdmi_core_enable_interrupts(struct hdmi_core_data *core)
+{
+	/* Unmute interrupts */
+	REG_FLD_MOD(core->base, HDMI_CORE_IH_MUTE, 0x0, 1, 0);
+}
+
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core)
+{
+	void __iomem *base = core->base;
+
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT1, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_FC_STAT2, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_AS_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_PHY_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CM_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_CEC_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_VP_STAT0, 0xff, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_IH_I2CMPHY_STAT0, 0xff, 7, 0);
+
+	return 0;
+}
+
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+		struct hdmi_config *cfg)
+{
+	struct omap_video_timings video_timing;
+	struct hdmi_video_format video_format;
+	struct hdmi_core_vid_config v_core_cfg;
+
+	hdmi_core_mask_interrupts(core);
+
+	hdmi_core_init(&v_core_cfg, cfg);
+
+	hdmi_wp_init_vid_fmt_timings(&video_format, &video_timing, cfg);
+
+	hdmi_wp_video_config_timing(wp, &video_timing);
+
+	/* video config */
+	video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+	hdmi_wp_video_config_format(wp, &video_format);
+
+	hdmi_wp_video_config_interface(wp, &video_timing);
+
+	/* support limited range with 24 bit color depth for now */
+	hdmi_core_configure_range(core);
+	cfg->infoframe.quantization_range = HDMI_QUANTIZATION_RANGE_LIMITED;
+
+	/*
+	 * configure core video part, set software reset in the core
+	 */
+	v_core_cfg.packet_mode = HDMI_PACKETMODE24BITPERPIXEL;
+
+	hdmi_core_video_config(core, &v_core_cfg);
+
+	hdmi_core_config_video_packetizer(core);
+	hdmi_core_config_csc(core);
+	hdmi_core_config_video_sampler(core);
+
+	if (cfg->hdmi_dvi_mode == HDMI_HDMI)
+		hdmi_core_write_avi_infoframe(core, &cfg->infoframe);
+
+	hdmi_core_enable_video_path(core);
+
+	hdmi_core_enable_interrupts(core);
+}
+
+static void hdmi5_core_audio_config(struct hdmi_core_data *core,
+			struct hdmi_core_audio_config *cfg)
+{
+	void __iomem *base = core->base;
+	u8 val;
+
+	/* Mute audio before configuring */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0xf, 7, 4);
+
+	/* Set the N parameter */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N1, cfg->n, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N2, cfg->n >> 8, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_N3, cfg->n >> 16, 3, 0);
+
+	/*
+	 * CTS manual mode. Automatic mode is not supported when using audio
+	 * parallel interface.
+	 */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, 1, 4, 4);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS1, cfg->cts, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS2, cfg->cts >> 8, 7, 0);
+	REG_FLD_MOD(base, HDMI_CORE_AUD_CTS3, cfg->cts >> 16, 3, 0);
+
+	/* Layout of Audio Sample Packets: 2-channel or multichannels */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+		REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 0, 0);
+	else
+		REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 1, 0, 0);
+
+	/* Configure IEC-609580 Validity bits */
+	/* Channel 0 is valid */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 0, 0);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, 0, 4, 4);
+
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH)
+		val = 1;
+	else
+		val = 0;
+
+	/* Channels 1, 2 setting */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 1, 1);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 5, 5);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 2, 2);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 6, 6);
+	/* Channel 3 setting */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH)
+		val = 1;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 3, 3);
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSV, val, 7, 7);
+
+	/* Configure IEC-60958 User bits */
+	/* TODO: should be set by user. */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSU, 0, 7, 0);
+
+	/* Configure IEC-60958 Channel Status word */
+	/* CGMSA */
+	val = cfg->iec60958_cfg->status[5] & IEC958_AES5_CON_CGMSA;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 5, 4);
+
+	/* Copyright */
+	val = (cfg->iec60958_cfg->status[0] &
+			IEC958_AES0_CON_NOT_COPYRIGHT) >> 2;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(0), val, 0, 0);
+
+	/* Category */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(1),
+		cfg->iec60958_cfg->status[1]);
+
+	/* PCM audio mode */
+	val = (cfg->iec60958_cfg->status[0] & IEC958_AES0_CON_MODE) >> 6;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 6, 4);
+
+	/* Source number */
+	val = cfg->iec60958_cfg->status[2] & IEC958_AES2_CON_SOURCE;
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(2), val, 3, 0);
+
+	/* Channel number right 0  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 2, 3, 0);
+	/* Channel number right 1*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(3), 4, 7, 4);
+	/* Channel number right 2  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 6, 3, 0);
+	/* Channel number right 3*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(4), 8, 7, 4);
+	/* Channel number left 0  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 1, 3, 0);
+	/* Channel number left 1*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(5), 3, 7, 4);
+	/* Channel number left 2  */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 5, 3, 0);
+	/* Channel number left 3*/
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCHNLS(6), 7, 7, 4);
+
+	/* Clock accuracy and sample rate */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(7),
+		cfg->iec60958_cfg->status[3]);
+
+	/* Original sample rate and word length */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDSCHNLS(8),
+		cfg->iec60958_cfg->status[4]);
+
+	/* Enable FIFO empty and full interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_INT, 3, 3, 2);
+
+	/* Configure GPA */
+	/* select HBR/SPDIF interfaces */
+	if (cfg->layout == HDMI_AUDIO_LAYOUT_2CH) {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable two channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 3, 7, 0);
+	} else if (cfg->layout == HDMI_AUDIO_LAYOUT_6CH) {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable six channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0x3F, 7, 0);
+	} else {
+		/* select HBR/SPDIF interfaces */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_CONF0, 0, 5, 5);
+		/* enable eight channels in GPA */
+		REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF1, 0xFF, 7, 0);
+	}
+
+	/* disable HBR */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 0, 0, 0);
+	/* enable PCUV */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_CONF2, 1, 1, 1);
+	/* enable GPA FIFO full and empty mask */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_MASK, 3, 1, 0);
+	/* set polarity of GPA FIFO empty interrupts */
+	REG_FLD_MOD(base, HDMI_CORE_AUD_GP_POL, 1, 0, 0);
+
+	/* unmute audio */
+	REG_FLD_MOD(base, HDMI_CORE_FC_AUDSCONF, 0, 7, 4);
+}
+
+static void hdmi5_core_audio_infoframe_cfg(struct hdmi_core_data *core,
+	 struct snd_cea_861_aud_if *info_aud)
+{
+	void __iomem *base = core->base;
+
+	/* channel count and coding type fields in AUDICONF0 are swapped */
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF0,
+		(info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC) << 4 |
+		(info_aud->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CT) >> 4);
+
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF1, info_aud->db2_sf_ss);
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF2, info_aud->db4_ca);
+	hdmi_write_reg(base, HDMI_CORE_FC_AUDICONF3,
+	  (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_DM_INH) >> 3 |
+	  (info_aud->db5_dminh_lsv & CEA861_AUDIO_INFOFRAME_DB5_LSV));
+}
+
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct omap_dss_audio *audio, u32 pclk)
+{
+	struct hdmi_audio_format audio_format;
+	struct hdmi_audio_dma audio_dma;
+	struct hdmi_core_audio_config core_cfg;
+	int err, n, cts, channel_count;
+	unsigned int fs_nr;
+	bool word_length_16b = false;
+
+	if (!audio || !audio->iec || !audio->cea || !core)
+		return -EINVAL;
+
+	core_cfg.iec60958_cfg = audio->iec;
+
+	if (!(audio->iec->status[4] & IEC958_AES4_CON_MAX_WORDLEN_24) &&
+		(audio->iec->status[4] & IEC958_AES4_CON_WORDLEN_20_16))
+			word_length_16b = true;
+
+	/* only 16-bit word length supported atm */
+	if (!word_length_16b)
+		return -EINVAL;
+
+	switch (audio->iec->status[3] & IEC958_AES3_CON_FS) {
+	case IEC958_AES3_CON_FS_32000:
+		fs_nr = 32000;
+		break;
+	case IEC958_AES3_CON_FS_44100:
+		fs_nr = 44100;
+		break;
+	case IEC958_AES3_CON_FS_48000:
+		fs_nr = 48000;
+		break;
+	case IEC958_AES3_CON_FS_88200:
+		fs_nr = 88200;
+		break;
+	case IEC958_AES3_CON_FS_96000:
+		fs_nr = 96000;
+		break;
+	case IEC958_AES3_CON_FS_176400:
+		fs_nr = 176400;
+		break;
+	case IEC958_AES3_CON_FS_192000:
+		fs_nr = 192000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	err = hdmi_compute_acr(pclk, fs_nr, &n, &cts);
+	core_cfg.n = n;
+	core_cfg.cts = cts;
+
+	/* Audio channels settings */
+	channel_count = (audio->cea->db1_ct_cc & CEA861_AUDIO_INFOFRAME_DB1CC)
+				+ 1;
+
+	if (channel_count == 2)
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_2CH;
+	else if (channel_count == 6)
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_6CH;
+	else
+		core_cfg.layout = HDMI_AUDIO_LAYOUT_8CH;
+
+	/* DMA settings */
+	if (word_length_16b)
+		audio_dma.transfer_size = 0x10;
+	else
+		audio_dma.transfer_size = 0x20;
+	audio_dma.block_size = 0xC0;
+	audio_dma.mode = HDMI_AUDIO_TRANSF_DMA;
+	audio_dma.fifo_threshold = 0x20; /* in number of samples */
+
+	/* audio FIFO format settings for 16-bit samples*/
+	audio_format.samples_per_word = HDMI_AUDIO_ONEWORD_TWOSAMPLES;
+	audio_format.sample_size = HDMI_AUDIO_SAMPLE_16BITS;
+	audio_format.justification = HDMI_AUDIO_JUSTIFY_LEFT;
+	audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+
+	/* only LPCM atm */
+	audio_format.type = HDMI_AUDIO_TYPE_LPCM;
+
+	/* only allowed option */
+	audio_format.sample_order = HDMI_AUDIO_SAMPLE_LEFT_FIRST;
+
+	/* disable start/stop signals of IEC 60958 blocks */
+	audio_format.en_sig_blk_strt_end = HDMI_AUDIO_BLOCK_SIG_STARTEND_ON;
+
+	/* configure DMA and audio FIFO format*/
+	hdmi_wp_audio_config_dma(wp, &audio_dma);
+	hdmi_wp_audio_config_format(wp, &audio_format);
+
+	/* configure the core */
+	hdmi5_core_audio_config(core, &core_cfg);
+
+	/* configure CEA 861 audio infoframe */
+	hdmi5_core_audio_infoframe_cfg(core, audio->cea);
+
+	return 0;
+}
+
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core)
+{
+	struct resource *res;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "core");
+	if (!res) {
+		DSSERR("can't get CORE IORESOURCE_MEM HDMI\n");
+		return -EINVAL;
+	}
+
+	core->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(core->base)) {
+		DSSERR("can't ioremap HDMI core\n");
+		return PTR_ERR(core->base);
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h
new file mode 100644
index 000000000000..f2f1022c5516
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi5_core.h
@@ -0,0 +1,304 @@
+/*
+ * HDMI driver definition for TI OMAP5 processors.
+ *
+ * Copyright (C) 2011-2012 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 version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _HDMI5_CORE_H_
+#define _HDMI5_CORE_H_
+
+#include "hdmi.h"
+
+/* HDMI IP Core System */
+
+/* HDMI Identification */
+#define HDMI_CORE_DESIGN_ID			0x00000
+#define HDMI_CORE_REVISION_ID			0x00004
+#define HDMI_CORE_PRODUCT_ID0			0x00008
+#define HDMI_CORE_PRODUCT_ID1			0x0000C
+#define HDMI_CORE_CONFIG0_ID			0x00010
+#define HDMI_CORE_CONFIG1_ID			0x00014
+#define HDMI_CORE_CONFIG2_ID			0x00018
+#define HDMI_CORE_CONFIG3_ID			0x0001C
+
+/* HDMI Interrupt */
+#define HDMI_CORE_IH_FC_STAT0			0x00400
+#define HDMI_CORE_IH_FC_STAT1			0x00404
+#define HDMI_CORE_IH_FC_STAT2			0x00408
+#define HDMI_CORE_IH_AS_STAT0			0x0040C
+#define HDMI_CORE_IH_PHY_STAT0			0x00410
+#define HDMI_CORE_IH_I2CM_STAT0			0x00414
+#define HDMI_CORE_IH_CEC_STAT0			0x00418
+#define HDMI_CORE_IH_VP_STAT0			0x0041C
+#define HDMI_CORE_IH_I2CMPHY_STAT0		0x00420
+#define HDMI_CORE_IH_MUTE			0x007FC
+
+/* HDMI Video Sampler */
+#define HDMI_CORE_TX_INVID0			0x00800
+#define HDMI_CORE_TX_INSTUFFING			0x00804
+#define HDMI_CORE_TX_RGYDATA0			0x00808
+#define HDMI_CORE_TX_RGYDATA1			0x0080C
+#define HDMI_CORE_TX_RCRDATA0			0x00810
+#define HDMI_CORE_TX_RCRDATA1			0x00814
+#define HDMI_CORE_TX_BCBDATA0			0x00818
+#define HDMI_CORE_TX_BCBDATA1			0x0081C
+
+/* HDMI Video Packetizer */
+#define HDMI_CORE_VP_STATUS			0x02000
+#define HDMI_CORE_VP_PR_CD			0x02004
+#define HDMI_CORE_VP_STUFF			0x02008
+#define HDMI_CORE_VP_REMAP			0x0200C
+#define HDMI_CORE_VP_CONF			0x02010
+#define HDMI_CORE_VP_STAT			0x02014
+#define HDMI_CORE_VP_INT			0x02018
+#define HDMI_CORE_VP_MASK			0x0201C
+#define HDMI_CORE_VP_POL			0x02020
+
+/* Frame Composer */
+#define HDMI_CORE_FC_INVIDCONF			0x04000
+#define HDMI_CORE_FC_INHACTIV0			0x04004
+#define HDMI_CORE_FC_INHACTIV1			0x04008
+#define HDMI_CORE_FC_INHBLANK0			0x0400C
+#define HDMI_CORE_FC_INHBLANK1			0x04010
+#define HDMI_CORE_FC_INVACTIV0			0x04014
+#define HDMI_CORE_FC_INVACTIV1			0x04018
+#define HDMI_CORE_FC_INVBLANK			0x0401C
+#define HDMI_CORE_FC_HSYNCINDELAY0		0x04020
+#define HDMI_CORE_FC_HSYNCINDELAY1		0x04024
+#define HDMI_CORE_FC_HSYNCINWIDTH0		0x04028
+#define HDMI_CORE_FC_HSYNCINWIDTH1		0x0402C
+#define HDMI_CORE_FC_VSYNCINDELAY		0x04030
+#define HDMI_CORE_FC_VSYNCINWIDTH		0x04034
+#define HDMI_CORE_FC_INFREQ0			0x04038
+#define HDMI_CORE_FC_INFREQ1			0x0403C
+#define HDMI_CORE_FC_INFREQ2			0x04040
+#define HDMI_CORE_FC_CTRLDUR			0x04044
+#define HDMI_CORE_FC_EXCTRLDUR			0x04048
+#define HDMI_CORE_FC_EXCTRLSPAC			0x0404C
+#define HDMI_CORE_FC_CH0PREAM			0x04050
+#define HDMI_CORE_FC_CH1PREAM			0x04054
+#define HDMI_CORE_FC_CH2PREAM			0x04058
+#define HDMI_CORE_FC_AVICONF3			0x0405C
+#define HDMI_CORE_FC_GCP			0x04060
+#define HDMI_CORE_FC_AVICONF0			0x04064
+#define HDMI_CORE_FC_AVICONF1			0x04068
+#define HDMI_CORE_FC_AVICONF2			0x0406C
+#define HDMI_CORE_FC_AVIVID			0x04070
+#define HDMI_CORE_FC_AVIETB0			0x04074
+#define HDMI_CORE_FC_AVIETB1			0x04078
+#define HDMI_CORE_FC_AVISBB0			0x0407C
+#define HDMI_CORE_FC_AVISBB1			0x04080
+#define HDMI_CORE_FC_AVIELB0			0x04084
+#define HDMI_CORE_FC_AVIELB1			0x04088
+#define HDMI_CORE_FC_AVISRB0			0x0408C
+#define HDMI_CORE_FC_AVISRB1			0x04090
+#define HDMI_CORE_FC_AUDICONF0			0x04094
+#define HDMI_CORE_FC_AUDICONF1			0x04098
+#define HDMI_CORE_FC_AUDICONF2			0x0409C
+#define HDMI_CORE_FC_AUDICONF3			0x040A0
+#define HDMI_CORE_FC_VSDIEEEID0			0x040A4
+#define HDMI_CORE_FC_VSDSIZE			0x040A8
+#define HDMI_CORE_FC_VSDIEEEID1			0x040C0
+#define HDMI_CORE_FC_VSDIEEEID2			0x040C4
+#define HDMI_CORE_FC_VSDPAYLOAD(n)		(n * 4 + 0x040C8)
+#define HDMI_CORE_FC_SPDVENDORNAME(n)		(n * 4 + 0x04128)
+#define HDMI_CORE_FC_SPDPRODUCTNAME(n)		(n * 4 + 0x04148)
+#define HDMI_CORE_FC_SPDDEVICEINF		0x04188
+#define HDMI_CORE_FC_AUDSCONF			0x0418C
+#define HDMI_CORE_FC_AUDSSTAT			0x04190
+#define HDMI_CORE_FC_AUDSV			0x04194
+#define HDMI_CORE_FC_AUDSU			0x04198
+#define HDMI_CORE_FC_AUDSCHNLS(n)		(n * 4 + 0x0419C)
+#define HDMI_CORE_FC_CTRLQHIGH			0x041CC
+#define HDMI_CORE_FC_CTRLQLOW			0x041D0
+#define HDMI_CORE_FC_ACP0			0x041D4
+#define HDMI_CORE_FC_ACP(n)			((16-n) * 4 + 0x04208)
+#define HDMI_CORE_FC_ISCR1_0			0x04248
+#define HDMI_CORE_FC_ISCR1(n)			((16-n) * 4 + 0x0424C)
+#define HDMI_CORE_FC_ISCR2(n)			((15-n) * 4 + 0x0428C)
+#define HDMI_CORE_FC_DATAUTO0			0x042CC
+#define HDMI_CORE_FC_DATAUTO1			0x042D0
+#define HDMI_CORE_FC_DATAUTO2			0x042D4
+#define HDMI_CORE_FC_DATMAN			0x042D8
+#define HDMI_CORE_FC_DATAUTO3			0x042DC
+#define HDMI_CORE_FC_RDRB(n)			(n * 4 + 0x042E0)
+#define HDMI_CORE_FC_STAT0			0x04340
+#define HDMI_CORE_FC_INT0			0x04344
+#define HDMI_CORE_FC_MASK0			0x04348
+#define HDMI_CORE_FC_POL0			0x0434C
+#define HDMI_CORE_FC_STAT1			0x04350
+#define HDMI_CORE_FC_INT1			0x04354
+#define HDMI_CORE_FC_MASK1			0x04358
+#define HDMI_CORE_FC_POL1			0x0435C
+#define HDMI_CORE_FC_STAT2			0x04360
+#define HDMI_CORE_FC_INT2			0x04364
+#define HDMI_CORE_FC_MASK2			0x04368
+#define HDMI_CORE_FC_POL2			0x0436C
+#define HDMI_CORE_FC_PRCONF			0x04380
+#define HDMI_CORE_FC_GMD_STAT			0x04400
+#define HDMI_CORE_FC_GMD_EN			0x04404
+#define HDMI_CORE_FC_GMD_UP			0x04408
+#define HDMI_CORE_FC_GMD_CONF			0x0440C
+#define HDMI_CORE_FC_GMD_HB			0x04410
+#define HDMI_CORE_FC_GMD_PB(n)			(n * 4 + 0x04414)
+#define HDMI_CORE_FC_DBGFORCE			0x04800
+#define HDMI_CORE_FC_DBGAUD0CH0			0x04804
+#define HDMI_CORE_FC_DBGAUD1CH0			0x04808
+#define HDMI_CORE_FC_DBGAUD2CH0			0x0480C
+#define HDMI_CORE_FC_DBGAUD0CH1			0x04810
+#define HDMI_CORE_FC_DBGAUD1CH1			0x04814
+#define HDMI_CORE_FC_DBGAUD2CH1			0x04818
+#define HDMI_CORE_FC_DBGAUD0CH2			0x0481C
+#define HDMI_CORE_FC_DBGAUD1CH2			0x04820
+#define HDMI_CORE_FC_DBGAUD2CH2			0x04824
+#define HDMI_CORE_FC_DBGAUD0CH3			0x04828
+#define HDMI_CORE_FC_DBGAUD1CH3			0x0482C
+#define HDMI_CORE_FC_DBGAUD2CH3			0x04830
+#define HDMI_CORE_FC_DBGAUD0CH4			0x04834
+#define HDMI_CORE_FC_DBGAUD1CH4			0x04838
+#define HDMI_CORE_FC_DBGAUD2CH4			0x0483C
+#define HDMI_CORE_FC_DBGAUD0CH5			0x04840
+#define HDMI_CORE_FC_DBGAUD1CH5			0x04844
+#define HDMI_CORE_FC_DBGAUD2CH5			0x04848
+#define HDMI_CORE_FC_DBGAUD0CH6			0x0484C
+#define HDMI_CORE_FC_DBGAUD1CH6			0x04850
+#define HDMI_CORE_FC_DBGAUD2CH6			0x04854
+#define HDMI_CORE_FC_DBGAUD0CH7			0x04858
+#define HDMI_CORE_FC_DBGAUD1CH7			0x0485C
+#define HDMI_CORE_FC_DBGAUD2CH7			0x04860
+#define HDMI_CORE_FC_DBGTMDS0			0x04864
+#define HDMI_CORE_FC_DBGTMDS1			0x04868
+#define HDMI_CORE_FC_DBGTMDS2			0x0486C
+#define HDMI_CORE_PHY_MASK0			0x0C018
+#define HDMI_CORE_PHY_I2CM_INT_ADDR		0x0C09C
+#define HDMI_CORE_PHY_I2CM_CTLINT_ADDR		0x0C0A0
+
+/* HDMI Audio */
+#define HDMI_CORE_AUD_CONF0			0x0C400
+#define HDMI_CORE_AUD_CONF1			0x0C404
+#define HDMI_CORE_AUD_INT			0x0C408
+#define HDMI_CORE_AUD_N1			0x0C800
+#define HDMI_CORE_AUD_N2			0x0C804
+#define HDMI_CORE_AUD_N3			0x0C808
+#define HDMI_CORE_AUD_CTS1			0x0C80C
+#define HDMI_CORE_AUD_CTS2			0x0C810
+#define HDMI_CORE_AUD_CTS3			0x0C814
+#define HDMI_CORE_AUD_INCLKFS			0x0C818
+#define HDMI_CORE_AUD_CC08			0x0CC08
+#define HDMI_CORE_AUD_GP_CONF0			0x0D400
+#define HDMI_CORE_AUD_GP_CONF1			0x0D404
+#define HDMI_CORE_AUD_GP_CONF2			0x0D408
+#define HDMI_CORE_AUD_D010			0x0D010
+#define HDMI_CORE_AUD_GP_STAT			0x0D40C
+#define HDMI_CORE_AUD_GP_INT			0x0D410
+#define HDMI_CORE_AUD_GP_POL			0x0D414
+#define HDMI_CORE_AUD_GP_MASK			0x0D418
+
+/* HDMI Main Controller */
+#define HDMI_CORE_MC_CLKDIS			0x10004
+#define HDMI_CORE_MC_SWRSTZREQ			0x10008
+#define HDMI_CORE_MC_FLOWCTRL			0x10010
+#define HDMI_CORE_MC_PHYRSTZ			0x10014
+#define HDMI_CORE_MC_LOCKONCLOCK		0x10018
+
+/* HDMI COLOR SPACE CONVERTER */
+#define HDMI_CORE_CSC_CFG			0x10400
+#define HDMI_CORE_CSC_SCALE			0x10404
+#define HDMI_CORE_CSC_COEF_A1_MSB		0x10408
+#define HDMI_CORE_CSC_COEF_A1_LSB		0x1040C
+#define HDMI_CORE_CSC_COEF_A2_MSB		0x10410
+#define HDMI_CORE_CSC_COEF_A2_LSB		0x10414
+#define HDMI_CORE_CSC_COEF_A3_MSB		0x10418
+#define HDMI_CORE_CSC_COEF_A3_LSB		0x1041C
+#define HDMI_CORE_CSC_COEF_A4_MSB		0x10420
+#define HDMI_CORE_CSC_COEF_A4_LSB		0x10424
+#define HDMI_CORE_CSC_COEF_B1_MSB		0x10428
+#define HDMI_CORE_CSC_COEF_B1_LSB		0x1042C
+#define HDMI_CORE_CSC_COEF_B2_MSB		0x10430
+#define HDMI_CORE_CSC_COEF_B2_LSB		0x10434
+#define HDMI_CORE_CSC_COEF_B3_MSB		0x10438
+#define HDMI_CORE_CSC_COEF_B3_LSB		0x1043C
+#define HDMI_CORE_CSC_COEF_B4_MSB		0x10440
+#define HDMI_CORE_CSC_COEF_B4_LSB		0x10444
+#define HDMI_CORE_CSC_COEF_C1_MSB		0x10448
+#define HDMI_CORE_CSC_COEF_C1_LSB		0x1044C
+#define HDMI_CORE_CSC_COEF_C2_MSB		0x10450
+#define HDMI_CORE_CSC_COEF_C2_LSB		0x10454
+#define HDMI_CORE_CSC_COEF_C3_MSB		0x10458
+#define HDMI_CORE_CSC_COEF_C3_LSB		0x1045C
+#define HDMI_CORE_CSC_COEF_C4_MSB		0x10460
+#define HDMI_CORE_CSC_COEF_C4_LSB		0x10464
+
+/* HDMI HDCP */
+#define HDMI_CORE_HDCP_MASK			0x14020
+
+/* HDMI CEC */
+#define HDMI_CORE_CEC_MASK			0x17408
+
+/* HDMI I2C Master */
+#define HDMI_CORE_I2CM_SLAVE			0x157C8
+#define HDMI_CORE_I2CM_ADDRESS			0x157CC
+#define HDMI_CORE_I2CM_DATAO			0x157D0
+#define HDMI_CORE_I2CM_DATAI			0X157D4
+#define HDMI_CORE_I2CM_OPERATION		0x157D8
+#define HDMI_CORE_I2CM_INT			0x157DC
+#define HDMI_CORE_I2CM_CTLINT			0x157E0
+#define HDMI_CORE_I2CM_DIV			0x157E4
+#define HDMI_CORE_I2CM_SEGADDR			0x157E8
+#define HDMI_CORE_I2CM_SOFTRSTZ			0x157EC
+#define HDMI_CORE_I2CM_SEGPTR			0x157F0
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_1_ADDR	0x157F4
+#define HDMI_CORE_I2CM_SS_SCL_HCNT_0_ADDR	0x157F8
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_1_ADDR	0x157FC
+#define HDMI_CORE_I2CM_SS_SCL_LCNT_0_ADDR	0x15800
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_1_ADDR	0x15804
+#define HDMI_CORE_I2CM_FS_SCL_HCNT_0_ADDR	0x15808
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_1_ADDR	0x1580C
+#define HDMI_CORE_I2CM_FS_SCL_LCNT_0_ADDR	0x15810
+#define HDMI_CORE_I2CM_SDA_HOLD_ADDR		0x15814
+
+enum hdmi_core_packet_mode {
+	HDMI_PACKETMODERESERVEDVALUE = 0,
+	HDMI_PACKETMODE24BITPERPIXEL = 4,
+	HDMI_PACKETMODE30BITPERPIXEL = 5,
+	HDMI_PACKETMODE36BITPERPIXEL = 6,
+	HDMI_PACKETMODE48BITPERPIXEL = 7,
+};
+
+struct hdmi_core_vid_config {
+	struct hdmi_config v_fc_config;
+	enum hdmi_core_packet_mode packet_mode;
+	int data_enable_pol;
+	int vblank_osc;
+	int hblank;
+	int vblank;
+};
+
+struct csc_table {
+	u16 a1, a2, a3, a4;
+	u16 b1, b2, b3, b4;
+	u16 c1, c2, c3, c4;
+};
+
+int hdmi5_read_edid(struct hdmi_core_data *core, u8 *edid, int len);
+void hdmi5_core_dump(struct hdmi_core_data *core, struct seq_file *s);
+int hdmi5_core_handle_irqs(struct hdmi_core_data *core);
+void hdmi5_configure(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct hdmi_config *cfg);
+int hdmi5_core_init(struct platform_device *pdev, struct hdmi_core_data *core);
+
+int hdmi5_audio_config(struct hdmi_core_data *core, struct hdmi_wp_data *wp,
+			struct omap_dss_audio *audio, u32 pclk);
+#endif
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c
new file mode 100644
index 000000000000..1b8fcc6c4ba1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_common.c
@@ -0,0 +1,148 @@
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <video/omapdss.h>
+
+#include "hdmi.h"
+
+int hdmi_parse_lanes_of(struct platform_device *pdev, struct device_node *ep,
+	struct hdmi_phy_data *phy)
+{
+	struct property *prop;
+	int r, len;
+
+	prop = of_find_property(ep, "lanes", &len);
+	if (prop) {
+		u32 lanes[8];
+
+		if (len / sizeof(u32) != ARRAY_SIZE(lanes)) {
+			dev_err(&pdev->dev, "bad number of lanes\n");
+			return -EINVAL;
+		}
+
+		r = of_property_read_u32_array(ep, "lanes", lanes,
+			ARRAY_SIZE(lanes));
+		if (r) {
+			dev_err(&pdev->dev, "failed to read lane data\n");
+			return r;
+		}
+
+		r = hdmi_phy_parse_lanes(phy, lanes);
+		if (r) {
+			dev_err(&pdev->dev, "failed to parse lane data\n");
+			return r;
+		}
+	} else {
+		static const u32 default_lanes[] = { 0, 1, 2, 3, 4, 5, 6, 7 };
+
+		r = hdmi_phy_parse_lanes(phy, default_lanes);
+		if (WARN_ON(r)) {
+			dev_err(&pdev->dev, "failed to parse lane data\n");
+			return r;
+		}
+	}
+
+	return 0;
+}
+
+int hdmi_compute_acr(u32 pclk, u32 sample_freq, u32 *n, u32 *cts)
+{
+	u32 deep_color;
+	bool deep_color_correct = false;
+
+	if (n == NULL || cts == NULL)
+		return -EINVAL;
+
+	/* TODO: When implemented, query deep color mode here. */
+	deep_color = 100;
+
+	/*
+	 * When using deep color, the default N value (as in the HDMI
+	 * specification) yields to an non-integer CTS. Hence, we
+	 * modify it while keeping the restrictions described in
+	 * section 7.2.1 of the HDMI 1.4a specification.
+	 */
+	switch (sample_freq) {
+	case 32000:
+	case 48000:
+	case 96000:
+	case 192000:
+		if (deep_color == 125)
+			if (pclk == 27027000 || pclk == 74250000)
+				deep_color_correct = true;
+		if (deep_color == 150)
+			if (pclk == 27027000)
+				deep_color_correct = true;
+		break;
+	case 44100:
+	case 88200:
+	case 176400:
+		if (deep_color == 125)
+			if (pclk == 27027000)
+				deep_color_correct = true;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	if (deep_color_correct) {
+		switch (sample_freq) {
+		case 32000:
+			*n = 8192;
+			break;
+		case 44100:
+			*n = 12544;
+			break;
+		case 48000:
+			*n = 8192;
+			break;
+		case 88200:
+			*n = 25088;
+			break;
+		case 96000:
+			*n = 16384;
+			break;
+		case 176400:
+			*n = 50176;
+			break;
+		case 192000:
+			*n = 32768;
+			break;
+		default:
+			return -EINVAL;
+		}
+	} else {
+		switch (sample_freq) {
+		case 32000:
+			*n = 4096;
+			break;
+		case 44100:
+			*n = 6272;
+			break;
+		case 48000:
+			*n = 6144;
+			break;
+		case 88200:
+			*n = 12544;
+			break;
+		case 96000:
+			*n = 12288;
+			break;
+		case 176400:
+			*n = 25088;
+			break;
+		case 192000:
+			*n = 24576;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+	/* Calculate CTS. See HDMI 1.3a or 1.4a specifications */
+	*cts = (pclk/1000) * (*n / 128) * deep_color / (sample_freq / 10);
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c
new file mode 100644
index 000000000000..1f5d19c119ce
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_phy.c
@@ -0,0 +1,247 @@
+/*
+ * HDMI PHY
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * This program is free software; you can 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/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+struct hdmi_phy_features {
+	bool bist_ctrl;
+	bool ldo_voltage;
+	unsigned long max_phy;
+};
+
+static const struct hdmi_phy_features *phy_feat;
+
+void hdmi_phy_dump(struct hdmi_phy_data *phy, struct seq_file *s)
+{
+#define DUMPPHY(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(phy->base, r))
+
+	DUMPPHY(HDMI_TXPHY_TX_CTRL);
+	DUMPPHY(HDMI_TXPHY_DIGITAL_CTRL);
+	DUMPPHY(HDMI_TXPHY_POWER_CTRL);
+	DUMPPHY(HDMI_TXPHY_PAD_CFG_CTRL);
+	if (phy_feat->bist_ctrl)
+		DUMPPHY(HDMI_TXPHY_BIST_CONTROL);
+}
+
+int hdmi_phy_parse_lanes(struct hdmi_phy_data *phy, const u32 *lanes)
+{
+	int i;
+
+	for (i = 0; i < 8; i += 2) {
+		u8 lane, pol;
+		int dx, dy;
+
+		dx = lanes[i];
+		dy = lanes[i + 1];
+
+		if (dx < 0 || dx >= 8)
+			return -EINVAL;
+
+		if (dy < 0 || dy >= 8)
+			return -EINVAL;
+
+		if (dx & 1) {
+			if (dy != dx - 1)
+				return -EINVAL;
+			pol = 1;
+		} else {
+			if (dy != dx + 1)
+				return -EINVAL;
+			pol = 0;
+		}
+
+		lane = dx / 2;
+
+		phy->lane_function[lane] = i / 2;
+		phy->lane_polarity[lane] = pol;
+	}
+
+	return 0;
+}
+
+static void hdmi_phy_configure_lanes(struct hdmi_phy_data *phy)
+{
+	static const u16 pad_cfg_list[] = {
+		0x0123,
+		0x0132,
+		0x0312,
+		0x0321,
+		0x0231,
+		0x0213,
+		0x1023,
+		0x1032,
+		0x3012,
+		0x3021,
+		0x2031,
+		0x2013,
+		0x1203,
+		0x1302,
+		0x3102,
+		0x3201,
+		0x2301,
+		0x2103,
+		0x1230,
+		0x1320,
+		0x3120,
+		0x3210,
+		0x2310,
+		0x2130,
+	};
+
+	u16 lane_cfg = 0;
+	int i;
+	unsigned lane_cfg_val;
+	u16 pol_val = 0;
+
+	for (i = 0; i < 4; ++i)
+		lane_cfg |= phy->lane_function[i] << ((3 - i) * 4);
+
+	pol_val |= phy->lane_polarity[0] << 0;
+	pol_val |= phy->lane_polarity[1] << 3;
+	pol_val |= phy->lane_polarity[2] << 2;
+	pol_val |= phy->lane_polarity[3] << 1;
+
+	for (i = 0; i < ARRAY_SIZE(pad_cfg_list); ++i)
+		if (pad_cfg_list[i] == lane_cfg)
+			break;
+
+	if (WARN_ON(i == ARRAY_SIZE(pad_cfg_list)))
+		i = 0;
+
+	lane_cfg_val = i;
+
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, lane_cfg_val, 26, 22);
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_PAD_CFG_CTRL, pol_val, 30, 27);
+}
+
+int hdmi_phy_configure(struct hdmi_phy_data *phy, unsigned long hfbitclk,
+	unsigned long lfbitclk)
+{
+	u8 freqout;
+
+	/*
+	 * Read address 0 in order to get the SCP reset done completed
+	 * Dummy access performed to make sure reset is done
+	 */
+	hdmi_read_reg(phy->base, HDMI_TXPHY_TX_CTRL);
+
+	/*
+	 * In OMAP5+, the HFBITCLK must be divided by 2 before issuing the
+	 * HDMI_PHYPWRCMD_LDOON command.
+	*/
+	if (phy_feat->bist_ctrl)
+		REG_FLD_MOD(phy->base, HDMI_TXPHY_BIST_CONTROL, 1, 11, 11);
+
+	/*
+	 * If the hfbitclk != lfbitclk, it means the lfbitclk was configured
+	 * to be used for TMDS.
+	 */
+	if (hfbitclk != lfbitclk)
+		freqout = 0;
+	else if (hfbitclk / 10 < phy_feat->max_phy)
+		freqout = 1;
+	else
+		freqout = 2;
+
+	/*
+	 * Write to phy address 0 to configure the clock
+	 * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
+	 */
+	REG_FLD_MOD(phy->base, HDMI_TXPHY_TX_CTRL, freqout, 31, 30);
+
+	/* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
+	hdmi_write_reg(phy->base, HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
+
+	/* Setup max LDO voltage */
+	if (phy_feat->ldo_voltage)
+		REG_FLD_MOD(phy->base, HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+
+	hdmi_phy_configure_lanes(phy);
+
+	return 0;
+}
+
+static const struct hdmi_phy_features omap44xx_phy_feats = {
+	.bist_ctrl	=	false,
+	.ldo_voltage	=	true,
+	.max_phy	=	185675000,
+};
+
+static const struct hdmi_phy_features omap54xx_phy_feats = {
+	.bist_ctrl	=	true,
+	.ldo_voltage	=	false,
+	.max_phy	=	186000000,
+};
+
+static int hdmi_phy_init_features(struct platform_device *pdev)
+{
+	struct hdmi_phy_features *dst;
+	const struct hdmi_phy_features *src;
+
+	dst = devm_kzalloc(&pdev->dev, sizeof(*dst), GFP_KERNEL);
+	if (!dst) {
+		dev_err(&pdev->dev, "Failed to allocate HDMI PHY Features\n");
+		return -ENOMEM;
+	}
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		src = &omap44xx_phy_feats;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+	case OMAPDSS_VER_DRA7xx:
+		src = &omap54xx_phy_feats;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	memcpy(dst, src, sizeof(*dst));
+	phy_feat = dst;
+
+	return 0;
+}
+
+int hdmi_phy_init(struct platform_device *pdev, struct hdmi_phy_data *phy)
+{
+	int r;
+	struct resource *res;
+
+	r = hdmi_phy_init_features(pdev);
+	if (r)
+		return r;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "phy");
+	if (!res) {
+		DSSERR("can't get PHY mem resource\n");
+		return -EINVAL;
+	}
+
+	phy->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(phy->base)) {
+		DSSERR("can't ioremap TX PHY\n");
+		return PTR_ERR(phy->base);
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c
new file mode 100644
index 000000000000..06e23a7c432c
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_pll.c
@@ -0,0 +1,255 @@
+/*
+ * HDMI PLL
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * This program is free software; you can 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 DSS_SUBSYS_NAME "HDMIPLL"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+void hdmi_pll_dump(struct hdmi_pll_data *pll, struct seq_file *s)
+{
+#define DUMPPLL(r) seq_printf(s, "%-35s %08x\n", #r,\
+		hdmi_read_reg(pll->base, r))
+
+	DUMPPLL(PLLCTRL_PLL_CONTROL);
+	DUMPPLL(PLLCTRL_PLL_STATUS);
+	DUMPPLL(PLLCTRL_PLL_GO);
+	DUMPPLL(PLLCTRL_CFG1);
+	DUMPPLL(PLLCTRL_CFG2);
+	DUMPPLL(PLLCTRL_CFG3);
+	DUMPPLL(PLLCTRL_SSC_CFG1);
+	DUMPPLL(PLLCTRL_SSC_CFG2);
+	DUMPPLL(PLLCTRL_CFG4);
+}
+
+void hdmi_pll_compute(struct hdmi_pll_data *pll,
+	unsigned long target_tmds, struct dss_pll_clock_info *pi)
+{
+	unsigned long fint, clkdco, clkout;
+	unsigned long target_bitclk, target_clkdco;
+	unsigned long min_dco;
+	unsigned n, m, mf, m2, sd;
+	unsigned long clkin;
+	const struct dss_pll_hw *hw = pll->pll.hw;
+
+	clkin = clk_get_rate(pll->pll.clkin);
+
+	DSSDBG("clkin %lu, target tmds %lu\n", clkin, target_tmds);
+
+	target_bitclk = target_tmds * 10;
+
+	/* Fint */
+	n = DIV_ROUND_UP(clkin, hw->fint_max);
+	fint = clkin / n;
+
+	/* adjust m2 so that the clkdco will be high enough */
+	min_dco = roundup(hw->clkdco_min, fint);
+	m2 = DIV_ROUND_UP(min_dco, target_bitclk);
+	if (m2 == 0)
+		m2 = 1;
+
+	target_clkdco = target_bitclk * m2;
+	m = target_clkdco / fint;
+
+	clkdco = fint * m;
+
+	/* adjust clkdco with fractional mf */
+	if (WARN_ON(target_clkdco - clkdco > fint))
+		mf = 0;
+	else
+		mf = (u32)div_u64(262144ull * (target_clkdco - clkdco), fint);
+
+	if (mf > 0)
+		clkdco += (u32)div_u64((u64)mf * fint, 262144);
+
+	clkout = clkdco / m2;
+
+	/* sigma-delta */
+	sd = DIV_ROUND_UP(fint * m, 250000000);
+
+	DSSDBG("N = %u, M = %u, M.f = %u, M2 = %u, SD = %u\n",
+		n, m, mf, m2, sd);
+	DSSDBG("Fint %lu, clkdco %lu, clkout %lu\n", fint, clkdco, clkout);
+
+	pi->n = n;
+	pi->m = m;
+	pi->mf = mf;
+	pi->mX[0] = m2;
+	pi->sd = sd;
+
+	pi->fint = fint;
+	pi->clkdco = clkdco;
+	pi->clkout[0] = clkout;
+}
+
+static int hdmi_pll_enable(struct dss_pll *dsspll)
+{
+	struct hdmi_pll_data *pll = container_of(dsspll, struct hdmi_pll_data, pll);
+	struct hdmi_wp_data *wp = pll->wp;
+	u16 r = 0;
+
+	dss_ctrl_pll_enable(DSS_PLL_HDMI, true);
+
+	r = hdmi_wp_set_pll_pwr(wp, HDMI_PLLPWRCMD_BOTHON_ALLCLKS);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+static void hdmi_pll_disable(struct dss_pll *dsspll)
+{
+	struct hdmi_pll_data *pll = container_of(dsspll, struct hdmi_pll_data, pll);
+	struct hdmi_wp_data *wp = pll->wp;
+
+	hdmi_wp_set_pll_pwr(wp, HDMI_PLLPWRCMD_ALLOFF);
+
+	dss_ctrl_pll_enable(DSS_PLL_HDMI, false);
+}
+
+static const struct dss_pll_ops dsi_pll_ops = {
+	.enable = hdmi_pll_enable,
+	.disable = hdmi_pll_disable,
+	.set_config = dss_pll_write_config_type_b,
+};
+
+static const struct dss_pll_hw dss_omap4_hdmi_pll_hw = {
+	.n_max = 255,
+	.m_min = 20,
+	.m_max = 4095,
+	.mX_max = 127,
+	.fint_min = 500000,
+	.fint_max = 2500000,
+
+	.clkdco_min = 500000000,
+	.clkdco_low = 1000000000,
+	.clkdco_max = 2000000000,
+
+	.n_msb = 8,
+	.n_lsb = 1,
+	.m_msb = 20,
+	.m_lsb = 9,
+
+	.mX_msb[0] = 24,
+	.mX_lsb[0] = 18,
+
+	.has_selfreqdco = true,
+};
+
+static const struct dss_pll_hw dss_omap5_hdmi_pll_hw = {
+	.n_max = 255,
+	.m_min = 20,
+	.m_max = 2045,
+	.mX_max = 127,
+	.fint_min = 620000,
+	.fint_max = 2500000,
+
+	.clkdco_min = 750000000,
+	.clkdco_low = 1500000000,
+	.clkdco_max = 2500000000UL,
+
+	.n_msb = 8,
+	.n_lsb = 1,
+	.m_msb = 20,
+	.m_lsb = 9,
+
+	.mX_msb[0] = 24,
+	.mX_lsb[0] = 18,
+
+	.has_selfreqdco = true,
+	.has_refsel = true,
+};
+
+static int dsi_init_pll_data(struct platform_device *pdev, struct hdmi_pll_data *hpll)
+{
+	struct dss_pll *pll = &hpll->pll;
+	struct clk *clk;
+	int r;
+
+	clk = devm_clk_get(&pdev->dev, "sys_clk");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get sys_clk\n");
+		return PTR_ERR(clk);
+	}
+
+	pll->name = "hdmi";
+	pll->id = DSS_PLL_HDMI;
+	pll->base = hpll->base;
+	pll->clkin = clk;
+
+	switch (omapdss_get_version()) {
+	case OMAPDSS_VER_OMAP4430_ES1:
+	case OMAPDSS_VER_OMAP4430_ES2:
+	case OMAPDSS_VER_OMAP4:
+		pll->hw = &dss_omap4_hdmi_pll_hw;
+		break;
+
+	case OMAPDSS_VER_OMAP5:
+	case OMAPDSS_VER_DRA7xx:
+		pll->hw = &dss_omap5_hdmi_pll_hw;
+		break;
+
+	default:
+		return -ENODEV;
+	}
+
+	pll->ops = &dsi_pll_ops;
+
+	r = dss_pll_register(pll);
+	if (r)
+		return r;
+
+	return 0;
+}
+
+int hdmi_pll_init(struct platform_device *pdev, struct hdmi_pll_data *pll,
+	struct hdmi_wp_data *wp)
+{
+	int r;
+	struct resource *res;
+
+	pll->wp = wp;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pll");
+	if (!res) {
+		DSSERR("can't get PLL mem resource\n");
+		return -EINVAL;
+	}
+
+	pll->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pll->base)) {
+		DSSERR("can't ioremap PLLCTRL\n");
+		return PTR_ERR(pll->base);
+	}
+
+	r = dsi_init_pll_data(pdev, pll);
+	if (r) {
+		DSSERR("failed to init HDMI PLL\n");
+		return r;
+	}
+
+	return 0;
+}
+
+void hdmi_pll_uninit(struct hdmi_pll_data *hpll)
+{
+	struct dss_pll *pll = &hpll->pll;
+
+	dss_pll_unregister(pll);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c
new file mode 100644
index 000000000000..7c544bc56fb5
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/hdmi_wp.c
@@ -0,0 +1,282 @@
+/*
+ * HDMI wrapper
+ *
+ * Copyright (C) 2013 Texas Instruments Incorporated
+ *
+ * This program is free software; you can 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 DSS_SUBSYS_NAME "HDMIWP"
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+void hdmi_wp_dump(struct hdmi_wp_data *wp, struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, hdmi_read_reg(wp->base, r))
+
+	DUMPREG(HDMI_WP_REVISION);
+	DUMPREG(HDMI_WP_SYSCONFIG);
+	DUMPREG(HDMI_WP_IRQSTATUS_RAW);
+	DUMPREG(HDMI_WP_IRQSTATUS);
+	DUMPREG(HDMI_WP_IRQENABLE_SET);
+	DUMPREG(HDMI_WP_IRQENABLE_CLR);
+	DUMPREG(HDMI_WP_IRQWAKEEN);
+	DUMPREG(HDMI_WP_PWR_CTRL);
+	DUMPREG(HDMI_WP_DEBOUNCE);
+	DUMPREG(HDMI_WP_VIDEO_CFG);
+	DUMPREG(HDMI_WP_VIDEO_SIZE);
+	DUMPREG(HDMI_WP_VIDEO_TIMING_H);
+	DUMPREG(HDMI_WP_VIDEO_TIMING_V);
+	DUMPREG(HDMI_WP_CLK);
+	DUMPREG(HDMI_WP_AUDIO_CFG);
+	DUMPREG(HDMI_WP_AUDIO_CFG2);
+	DUMPREG(HDMI_WP_AUDIO_CTRL);
+	DUMPREG(HDMI_WP_AUDIO_DATA);
+}
+
+u32 hdmi_wp_get_irqstatus(struct hdmi_wp_data *wp)
+{
+	return hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS);
+}
+
+void hdmi_wp_set_irqstatus(struct hdmi_wp_data *wp, u32 irqstatus)
+{
+	hdmi_write_reg(wp->base, HDMI_WP_IRQSTATUS, irqstatus);
+	/* flush posted write */
+	hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS);
+}
+
+void hdmi_wp_set_irqenable(struct hdmi_wp_data *wp, u32 mask)
+{
+	hdmi_write_reg(wp->base, HDMI_WP_IRQENABLE_SET, mask);
+}
+
+void hdmi_wp_clear_irqenable(struct hdmi_wp_data *wp, u32 mask)
+{
+	hdmi_write_reg(wp->base, HDMI_WP_IRQENABLE_CLR, mask);
+}
+
+/* PHY_PWR_CMD */
+int hdmi_wp_set_phy_pwr(struct hdmi_wp_data *wp, enum hdmi_phy_pwr val)
+{
+	/* Return if already the state */
+	if (REG_GET(wp->base, HDMI_WP_PWR_CTRL, 5, 4) == val)
+		return 0;
+
+	/* Command for power control of HDMI PHY */
+	REG_FLD_MOD(wp->base, HDMI_WP_PWR_CTRL, val, 7, 6);
+
+	/* Status of the power control of HDMI PHY */
+	if (hdmi_wait_for_bit_change(wp->base, HDMI_WP_PWR_CTRL, 5, 4, val)
+			!= val) {
+		DSSERR("Failed to set PHY power mode to %d\n", val);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+/* PLL_PWR_CMD */
+int hdmi_wp_set_pll_pwr(struct hdmi_wp_data *wp, enum hdmi_pll_pwr val)
+{
+	/* Command for power control of HDMI PLL */
+	REG_FLD_MOD(wp->base, HDMI_WP_PWR_CTRL, val, 3, 2);
+
+	/* wait till PHY_PWR_STATUS is set */
+	if (hdmi_wait_for_bit_change(wp->base, HDMI_WP_PWR_CTRL, 1, 0, val)
+			!= val) {
+		DSSERR("Failed to set PLL_PWR_STATUS\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+int hdmi_wp_video_start(struct hdmi_wp_data *wp)
+{
+	REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, true, 31, 31);
+
+	return 0;
+}
+
+void hdmi_wp_video_stop(struct hdmi_wp_data *wp)
+{
+	int i;
+
+	hdmi_write_reg(wp->base, HDMI_WP_IRQSTATUS, HDMI_IRQ_VIDEO_FRAME_DONE);
+
+	REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, false, 31, 31);
+
+	for (i = 0; i < 50; ++i) {
+		u32 v;
+
+		msleep(20);
+
+		v = hdmi_read_reg(wp->base, HDMI_WP_IRQSTATUS_RAW);
+		if (v & HDMI_IRQ_VIDEO_FRAME_DONE)
+			return;
+	}
+
+	DSSERR("no HDMI FRAMEDONE when disabling output\n");
+}
+
+void hdmi_wp_video_config_format(struct hdmi_wp_data *wp,
+		struct hdmi_video_format *video_fmt)
+{
+	u32 l = 0;
+
+	REG_FLD_MOD(wp->base, HDMI_WP_VIDEO_CFG, video_fmt->packing_mode,
+		10, 8);
+
+	l |= FLD_VAL(video_fmt->y_res, 31, 16);
+	l |= FLD_VAL(video_fmt->x_res, 15, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_VIDEO_SIZE, l);
+}
+
+void hdmi_wp_video_config_interface(struct hdmi_wp_data *wp,
+		struct omap_video_timings *timings)
+{
+	u32 r;
+	bool vsync_pol, hsync_pol;
+	DSSDBG("Enter hdmi_wp_video_config_interface\n");
+
+	vsync_pol = timings->vsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+	hsync_pol = timings->hsync_level == OMAPDSS_SIG_ACTIVE_HIGH;
+
+	r = hdmi_read_reg(wp->base, HDMI_WP_VIDEO_CFG);
+	r = FLD_MOD(r, vsync_pol, 7, 7);
+	r = FLD_MOD(r, hsync_pol, 6, 6);
+	r = FLD_MOD(r, timings->interlace, 3, 3);
+	r = FLD_MOD(r, 1, 1, 0); /* HDMI_TIMING_MASTER_24BIT */
+	hdmi_write_reg(wp->base, HDMI_WP_VIDEO_CFG, r);
+}
+
+void hdmi_wp_video_config_timing(struct hdmi_wp_data *wp,
+		struct omap_video_timings *timings)
+{
+	u32 timing_h = 0;
+	u32 timing_v = 0;
+
+	DSSDBG("Enter hdmi_wp_video_config_timing\n");
+
+	timing_h |= FLD_VAL(timings->hbp, 31, 20);
+	timing_h |= FLD_VAL(timings->hfp, 19, 8);
+	timing_h |= FLD_VAL(timings->hsw, 7, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_VIDEO_TIMING_H, timing_h);
+
+	timing_v |= FLD_VAL(timings->vbp, 31, 20);
+	timing_v |= FLD_VAL(timings->vfp, 19, 8);
+	timing_v |= FLD_VAL(timings->vsw, 7, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_VIDEO_TIMING_V, timing_v);
+}
+
+void hdmi_wp_init_vid_fmt_timings(struct hdmi_video_format *video_fmt,
+		struct omap_video_timings *timings, struct hdmi_config *param)
+{
+	DSSDBG("Enter hdmi_wp_video_init_format\n");
+
+	video_fmt->packing_mode = HDMI_PACK_10b_RGB_YUV444;
+	video_fmt->y_res = param->timings.y_res;
+	video_fmt->x_res = param->timings.x_res;
+	if (param->timings.interlace)
+		video_fmt->y_res /= 2;
+
+	timings->hbp = param->timings.hbp;
+	timings->hfp = param->timings.hfp;
+	timings->hsw = param->timings.hsw;
+	timings->vbp = param->timings.vbp;
+	timings->vfp = param->timings.vfp;
+	timings->vsw = param->timings.vsw;
+	timings->vsync_level = param->timings.vsync_level;
+	timings->hsync_level = param->timings.hsync_level;
+	timings->interlace = param->timings.interlace;
+}
+
+void hdmi_wp_audio_config_format(struct hdmi_wp_data *wp,
+		struct hdmi_audio_format *aud_fmt)
+{
+	u32 r;
+
+	DSSDBG("Enter hdmi_wp_audio_config_format\n");
+
+	r = hdmi_read_reg(wp->base, HDMI_WP_AUDIO_CFG);
+	if (omapdss_get_version() == OMAPDSS_VER_OMAP4430_ES1 ||
+	    omapdss_get_version() == OMAPDSS_VER_OMAP4430_ES2 ||
+	    omapdss_get_version() == OMAPDSS_VER_OMAP4) {
+		r = FLD_MOD(r, aud_fmt->stereo_channels, 26, 24);
+		r = FLD_MOD(r, aud_fmt->active_chnnls_msk, 23, 16);
+	}
+	r = FLD_MOD(r, aud_fmt->en_sig_blk_strt_end, 5, 5);
+	r = FLD_MOD(r, aud_fmt->type, 4, 4);
+	r = FLD_MOD(r, aud_fmt->justification, 3, 3);
+	r = FLD_MOD(r, aud_fmt->sample_order, 2, 2);
+	r = FLD_MOD(r, aud_fmt->samples_per_word, 1, 1);
+	r = FLD_MOD(r, aud_fmt->sample_size, 0, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_AUDIO_CFG, r);
+}
+
+void hdmi_wp_audio_config_dma(struct hdmi_wp_data *wp,
+		struct hdmi_audio_dma *aud_dma)
+{
+	u32 r;
+
+	DSSDBG("Enter hdmi_wp_audio_config_dma\n");
+
+	r = hdmi_read_reg(wp->base, HDMI_WP_AUDIO_CFG2);
+	r = FLD_MOD(r, aud_dma->transfer_size, 15, 8);
+	r = FLD_MOD(r, aud_dma->block_size, 7, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_AUDIO_CFG2, r);
+
+	r = hdmi_read_reg(wp->base, HDMI_WP_AUDIO_CTRL);
+	r = FLD_MOD(r, aud_dma->mode, 9, 9);
+	r = FLD_MOD(r, aud_dma->fifo_threshold, 8, 0);
+	hdmi_write_reg(wp->base, HDMI_WP_AUDIO_CTRL, r);
+}
+
+int hdmi_wp_audio_enable(struct hdmi_wp_data *wp, bool enable)
+{
+	REG_FLD_MOD(wp->base, HDMI_WP_AUDIO_CTRL, enable, 31, 31);
+
+	return 0;
+}
+
+int hdmi_wp_audio_core_req_enable(struct hdmi_wp_data *wp, bool enable)
+{
+	REG_FLD_MOD(wp->base, HDMI_WP_AUDIO_CTRL, enable, 30, 30);
+
+	return 0;
+}
+
+int hdmi_wp_init(struct platform_device *pdev, struct hdmi_wp_data *wp)
+{
+	struct resource *res;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "wp");
+	if (!res) {
+		DSSERR("can't get WP mem resource\n");
+		return -EINVAL;
+	}
+	wp->phys_base = res->start;
+
+	wp->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wp->base)) {
+		DSSERR("can't ioremap HDMI WP\n");
+		return PTR_ERR(wp->base);
+	}
+
+	return 0;
+}
+
+phys_addr_t hdmi_wp_get_audio_dma_addr(struct hdmi_wp_data *wp)
+{
+	return wp->phys_base + HDMI_WP_AUDIO_DATA;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c
new file mode 100644
index 000000000000..a7414fb12830
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/manager-sysfs.c
@@ -0,0 +1,531 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t manager_name_show(struct omap_overlay_manager *mgr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", mgr->name);
+}
+
+static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
+{
+	struct omap_dss_device *dssdev = mgr->get_device(mgr);
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", dssdev ?
+			dssdev->name : "<none>");
+}
+
+static int manager_display_match(struct omap_dss_device *dssdev, void *data)
+{
+	const char *str = data;
+
+	return sysfs_streq(dssdev->name, str);
+}
+
+static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	int r = 0;
+	size_t len = size;
+	struct omap_dss_device *dssdev = NULL;
+	struct omap_dss_device *old_dssdev;
+
+	if (buf[size-1] == '\n')
+		--len;
+
+	if (len > 0)
+		dssdev = omap_dss_find_device((void *)buf,
+			manager_display_match);
+
+	if (len > 0 && dssdev == NULL)
+		return -EINVAL;
+
+	if (dssdev) {
+		DSSDBG("display %s found\n", dssdev->name);
+
+		if (omapdss_device_is_connected(dssdev)) {
+			DSSERR("new display is already connected\n");
+			r = -EINVAL;
+			goto put_device;
+		}
+
+		if (omapdss_device_is_enabled(dssdev)) {
+			DSSERR("new display is not disabled\n");
+			r = -EINVAL;
+			goto put_device;
+		}
+	}
+
+	old_dssdev = mgr->get_device(mgr);
+	if (old_dssdev) {
+		if (omapdss_device_is_enabled(old_dssdev)) {
+			DSSERR("old display is not disabled\n");
+			r = -EINVAL;
+			goto put_device;
+		}
+
+		old_dssdev->driver->disconnect(old_dssdev);
+	}
+
+	if (dssdev) {
+		r = dssdev->driver->connect(dssdev);
+		if (r) {
+			DSSERR("failed to connect new device\n");
+			goto put_device;
+		}
+
+		old_dssdev = mgr->get_device(mgr);
+		if (old_dssdev != dssdev) {
+			DSSERR("failed to connect device to this manager\n");
+			dssdev->driver->disconnect(dssdev);
+			goto put_device;
+		}
+
+		r = mgr->apply(mgr);
+		if (r) {
+			DSSERR("failed to apply dispc config\n");
+			goto put_device;
+		}
+	}
+
+put_device:
+	if (dssdev)
+		omap_dss_put_device(dssdev);
+
+	return r ? r : size;
+}
+
+static ssize_t manager_default_color_show(struct omap_overlay_manager *mgr,
+					  char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%#x\n", info.default_color);
+}
+
+static ssize_t manager_default_color_store(struct omap_overlay_manager *mgr,
+					   const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	u32 color;
+	int r;
+
+	r = kstrtouint(buf, 0, &color);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.default_color = color;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static const char *trans_key_type_str[] = {
+	"gfx-destination",
+	"video-source",
+};
+
+static ssize_t manager_trans_key_type_show(struct omap_overlay_manager *mgr,
+					   char *buf)
+{
+	enum omap_dss_trans_key_type key_type;
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	key_type = info.trans_key_type;
+	BUG_ON(key_type >= ARRAY_SIZE(trans_key_type_str));
+
+	return snprintf(buf, PAGE_SIZE, "%s\n", trans_key_type_str[key_type]);
+}
+
+static ssize_t manager_trans_key_type_store(struct omap_overlay_manager *mgr,
+					    const char *buf, size_t size)
+{
+	enum omap_dss_trans_key_type key_type;
+	struct omap_overlay_manager_info info;
+	int r;
+
+	for (key_type = OMAP_DSS_COLOR_KEY_GFX_DST;
+			key_type < ARRAY_SIZE(trans_key_type_str); key_type++) {
+		if (sysfs_streq(buf, trans_key_type_str[key_type]))
+			break;
+	}
+
+	if (key_type == ARRAY_SIZE(trans_key_type_str))
+		return -EINVAL;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_key_type = key_type;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_trans_key_value_show(struct omap_overlay_manager *mgr,
+					    char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%#x\n", info.trans_key);
+}
+
+static ssize_t manager_trans_key_value_store(struct omap_overlay_manager *mgr,
+					     const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	u32 key_value;
+	int r;
+
+	r = kstrtouint(buf, 0, &key_value);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_key = key_value;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_trans_key_enabled_show(struct omap_overlay_manager *mgr,
+					      char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.trans_enabled);
+}
+
+static ssize_t manager_trans_key_enabled_store(struct omap_overlay_manager *mgr,
+					       const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	bool enable;
+	int r;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.trans_enabled = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_alpha_blending_enabled_show(
+		struct omap_overlay_manager *mgr, char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	if(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+		return -ENODEV;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+		info.partial_alpha_enabled);
+}
+
+static ssize_t manager_alpha_blending_enabled_store(
+		struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	bool enable;
+	int r;
+
+	if(!dss_has_feature(FEAT_ALPHA_FIXED_ZORDER))
+		return -ENODEV;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.partial_alpha_enabled = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_cpr_enable_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.cpr_enable);
+}
+
+static ssize_t manager_cpr_enable_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	int r;
+	bool enable;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	mgr->get_manager_info(mgr, &info);
+
+	if (info.cpr_enable == enable)
+		return size;
+
+	info.cpr_enable = enable;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t manager_cpr_coef_show(struct omap_overlay_manager *mgr,
+		char *buf)
+{
+	struct omap_overlay_manager_info info;
+
+	mgr->get_manager_info(mgr, &info);
+
+	return snprintf(buf, PAGE_SIZE,
+			"%d %d %d %d %d %d %d %d %d\n",
+			info.cpr_coefs.rr,
+			info.cpr_coefs.rg,
+			info.cpr_coefs.rb,
+			info.cpr_coefs.gr,
+			info.cpr_coefs.gg,
+			info.cpr_coefs.gb,
+			info.cpr_coefs.br,
+			info.cpr_coefs.bg,
+			info.cpr_coefs.bb);
+}
+
+static ssize_t manager_cpr_coef_store(struct omap_overlay_manager *mgr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager_info info;
+	struct omap_dss_cpr_coefs coefs;
+	int r, i;
+	s16 *arr;
+
+	if (!dss_has_feature(FEAT_CPR))
+		return -ENODEV;
+
+	if (sscanf(buf, "%hd %hd %hd %hd %hd %hd %hd %hd %hd",
+				&coefs.rr, &coefs.rg, &coefs.rb,
+				&coefs.gr, &coefs.gg, &coefs.gb,
+				&coefs.br, &coefs.bg, &coefs.bb) != 9)
+		return -EINVAL;
+
+	arr = (s16[]){ coefs.rr, coefs.rg, coefs.rb,
+		coefs.gr, coefs.gg, coefs.gb,
+		coefs.br, coefs.bg, coefs.bb };
+
+	for (i = 0; i < 9; ++i) {
+		if (arr[i] < -512 || arr[i] > 511)
+			return -EINVAL;
+	}
+
+	mgr->get_manager_info(mgr, &info);
+
+	info.cpr_coefs = coefs;
+
+	r = mgr->set_manager_info(mgr, &info);
+	if (r)
+		return r;
+
+	r = mgr->apply(mgr);
+	if (r)
+		return r;
+
+	return size;
+}
+
+struct manager_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_overlay_manager *, char *);
+	ssize_t	(*store)(struct omap_overlay_manager *, const char *, size_t);
+};
+
+#define MANAGER_ATTR(_name, _mode, _show, _store) \
+	struct manager_attribute manager_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static MANAGER_ATTR(name, S_IRUGO, manager_name_show, NULL);
+static MANAGER_ATTR(display, S_IRUGO|S_IWUSR,
+		manager_display_show, manager_display_store);
+static MANAGER_ATTR(default_color, S_IRUGO|S_IWUSR,
+		manager_default_color_show, manager_default_color_store);
+static MANAGER_ATTR(trans_key_type, S_IRUGO|S_IWUSR,
+		manager_trans_key_type_show, manager_trans_key_type_store);
+static MANAGER_ATTR(trans_key_value, S_IRUGO|S_IWUSR,
+		manager_trans_key_value_show, manager_trans_key_value_store);
+static MANAGER_ATTR(trans_key_enabled, S_IRUGO|S_IWUSR,
+		manager_trans_key_enabled_show,
+		manager_trans_key_enabled_store);
+static MANAGER_ATTR(alpha_blending_enabled, S_IRUGO|S_IWUSR,
+		manager_alpha_blending_enabled_show,
+		manager_alpha_blending_enabled_store);
+static MANAGER_ATTR(cpr_enable, S_IRUGO|S_IWUSR,
+		manager_cpr_enable_show,
+		manager_cpr_enable_store);
+static MANAGER_ATTR(cpr_coef, S_IRUGO|S_IWUSR,
+		manager_cpr_coef_show,
+		manager_cpr_coef_store);
+
+
+static struct attribute *manager_sysfs_attrs[] = {
+	&manager_attr_name.attr,
+	&manager_attr_display.attr,
+	&manager_attr_default_color.attr,
+	&manager_attr_trans_key_type.attr,
+	&manager_attr_trans_key_value.attr,
+	&manager_attr_trans_key_enabled.attr,
+	&manager_attr_alpha_blending_enabled.attr,
+	&manager_attr_cpr_enable.attr,
+	&manager_attr_cpr_coef.attr,
+	NULL
+};
+
+static ssize_t manager_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_overlay_manager *manager;
+	struct manager_attribute *manager_attr;
+
+	manager = container_of(kobj, struct omap_overlay_manager, kobj);
+	manager_attr = container_of(attr, struct manager_attribute, attr);
+
+	if (!manager_attr->show)
+		return -ENOENT;
+
+	return manager_attr->show(manager, buf);
+}
+
+static ssize_t manager_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay_manager *manager;
+	struct manager_attribute *manager_attr;
+
+	manager = container_of(kobj, struct omap_overlay_manager, kobj);
+	manager_attr = container_of(attr, struct manager_attribute, attr);
+
+	if (!manager_attr->store)
+		return -ENOENT;
+
+	return manager_attr->store(manager, buf, size);
+}
+
+static const struct sysfs_ops manager_sysfs_ops = {
+	.show = manager_attr_show,
+	.store = manager_attr_store,
+};
+
+static struct kobj_type manager_ktype = {
+	.sysfs_ops = &manager_sysfs_ops,
+	.default_attrs = manager_sysfs_attrs,
+};
+
+int dss_manager_kobj_init(struct omap_overlay_manager *mgr,
+		struct platform_device *pdev)
+{
+	return kobject_init_and_add(&mgr->kobj, &manager_ktype,
+			&pdev->dev.kobj, "manager%d", mgr->id);
+}
+
+void dss_manager_kobj_uninit(struct omap_overlay_manager *mgr)
+{
+	kobject_del(&mgr->kobj);
+	kobject_put(&mgr->kobj);
+
+	memset(&mgr->kobj, 0, sizeof(mgr->kobj));
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/manager.c b/drivers/video/fbdev/omap2/omapfb/dss/manager.c
new file mode 100644
index 000000000000..08a67f4f6a20
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/manager.c
@@ -0,0 +1,263 @@
+/*
+ * linux/drivers/video/omap2/dss/manager.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "MANAGER"
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/jiffies.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static int num_managers;
+static struct omap_overlay_manager *managers;
+
+int dss_init_overlay_managers(void)
+{
+	int i;
+
+	num_managers = dss_feat_get_num_mgrs();
+
+	managers = kzalloc(sizeof(struct omap_overlay_manager) * num_managers,
+			GFP_KERNEL);
+
+	BUG_ON(managers == NULL);
+
+	for (i = 0; i < num_managers; ++i) {
+		struct omap_overlay_manager *mgr = &managers[i];
+
+		switch (i) {
+		case 0:
+			mgr->name = "lcd";
+			mgr->id = OMAP_DSS_CHANNEL_LCD;
+			break;
+		case 1:
+			mgr->name = "tv";
+			mgr->id = OMAP_DSS_CHANNEL_DIGIT;
+			break;
+		case 2:
+			mgr->name = "lcd2";
+			mgr->id = OMAP_DSS_CHANNEL_LCD2;
+			break;
+		case 3:
+			mgr->name = "lcd3";
+			mgr->id = OMAP_DSS_CHANNEL_LCD3;
+			break;
+		}
+
+		mgr->caps = 0;
+		mgr->supported_displays =
+			dss_feat_get_supported_displays(mgr->id);
+		mgr->supported_outputs =
+			dss_feat_get_supported_outputs(mgr->id);
+
+		INIT_LIST_HEAD(&mgr->overlays);
+	}
+
+	return 0;
+}
+
+int dss_init_overlay_managers_sysfs(struct platform_device *pdev)
+{
+	int i, r;
+
+	for (i = 0; i < num_managers; ++i) {
+		struct omap_overlay_manager *mgr = &managers[i];
+
+		r = dss_manager_kobj_init(mgr, pdev);
+		if (r)
+			DSSERR("failed to create sysfs file\n");
+	}
+
+	return 0;
+}
+
+void dss_uninit_overlay_managers(void)
+{
+	kfree(managers);
+	managers = NULL;
+	num_managers = 0;
+}
+
+void dss_uninit_overlay_managers_sysfs(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < num_managers; ++i) {
+		struct omap_overlay_manager *mgr = &managers[i];
+
+		dss_manager_kobj_uninit(mgr);
+	}
+}
+
+int omap_dss_get_num_overlay_managers(void)
+{
+	return num_managers;
+}
+EXPORT_SYMBOL(omap_dss_get_num_overlay_managers);
+
+struct omap_overlay_manager *omap_dss_get_overlay_manager(int num)
+{
+	if (num >= num_managers)
+		return NULL;
+
+	return &managers[num];
+}
+EXPORT_SYMBOL(omap_dss_get_overlay_manager);
+
+int dss_mgr_simple_check(struct omap_overlay_manager *mgr,
+		const struct omap_overlay_manager_info *info)
+{
+	if (dss_has_feature(FEAT_ALPHA_FIXED_ZORDER)) {
+		/*
+		 * OMAP3 supports only graphics source transparency color key
+		 * and alpha blending simultaneously. See TRM 15.4.2.4.2.2
+		 * Alpha Mode.
+		 */
+		if (info->partial_alpha_enabled && info->trans_enabled
+			&& info->trans_key_type != OMAP_DSS_COLOR_KEY_GFX_DST) {
+			DSSERR("check_manager: illegal transparency key\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int dss_mgr_check_zorder(struct omap_overlay_manager *mgr,
+		struct omap_overlay_info **overlay_infos)
+{
+	struct omap_overlay *ovl1, *ovl2;
+	struct omap_overlay_info *info1, *info2;
+
+	list_for_each_entry(ovl1, &mgr->overlays, list) {
+		info1 = overlay_infos[ovl1->id];
+
+		if (info1 == NULL)
+			continue;
+
+		list_for_each_entry(ovl2, &mgr->overlays, list) {
+			if (ovl1 == ovl2)
+				continue;
+
+			info2 = overlay_infos[ovl2->id];
+
+			if (info2 == NULL)
+				continue;
+
+			if (info1->zorder == info2->zorder) {
+				DSSERR("overlays %d and %d have the same "
+						"zorder %d\n",
+					ovl1->id, ovl2->id, info1->zorder);
+				return -EINVAL;
+			}
+		}
+	}
+
+	return 0;
+}
+
+int dss_mgr_check_timings(struct omap_overlay_manager *mgr,
+		const struct omap_video_timings *timings)
+{
+	if (!dispc_mgr_timings_ok(mgr->id, timings)) {
+		DSSERR("check_manager: invalid timings\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int dss_mgr_check_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	struct dispc_clock_info cinfo = config->clock_info;
+	int dl = config->video_port_width;
+	bool stallmode = config->stallmode;
+	bool fifohandcheck = config->fifohandcheck;
+
+	if (cinfo.lck_div < 1 || cinfo.lck_div > 255)
+		return -EINVAL;
+
+	if (cinfo.pck_div < 1 || cinfo.pck_div > 255)
+		return -EINVAL;
+
+	if (dl != 12 && dl != 16 && dl != 18 && dl != 24)
+		return -EINVAL;
+
+	/* fifohandcheck should be used only with stallmode */
+	if (!stallmode && fifohandcheck)
+		return -EINVAL;
+
+	/*
+	 * io pad mode can be only checked by using dssdev connected to the
+	 * manager. Ignore checking these for now, add checks when manager
+	 * is capable of holding information related to the connected interface
+	 */
+
+	return 0;
+}
+
+int dss_mgr_check(struct omap_overlay_manager *mgr,
+		struct omap_overlay_manager_info *info,
+		const struct omap_video_timings *mgr_timings,
+		const struct dss_lcd_mgr_config *lcd_config,
+		struct omap_overlay_info **overlay_infos)
+{
+	struct omap_overlay *ovl;
+	int r;
+
+	if (dss_has_feature(FEAT_ALPHA_FREE_ZORDER)) {
+		r = dss_mgr_check_zorder(mgr, overlay_infos);
+		if (r)
+			return r;
+	}
+
+	r = dss_mgr_check_timings(mgr, mgr_timings);
+	if (r)
+		return r;
+
+	r = dss_mgr_check_lcd_config(mgr, lcd_config);
+	if (r)
+		return r;
+
+	list_for_each_entry(ovl, &mgr->overlays, list) {
+		struct omap_overlay_info *oi;
+		int r;
+
+		oi = overlay_infos[ovl->id];
+
+		if (oi == NULL)
+			continue;
+
+		r = dss_ovl_check(ovl, oi, mgr_timings);
+		if (r)
+			return r;
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c b/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c
new file mode 100644
index 000000000000..136d30484d02
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/omapdss-boot-init.c
@@ -0,0 +1,225 @@
+/*
+ * Copyright (C) 2014 Texas Instruments
+ * Author: Tomi Valkeinen <tomi.valkeinen@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ * As omapdss panel drivers are omapdss specific, but we want to define the
+ * DT-data in generic manner, we convert the compatible strings of the panel and
+ * encoder nodes from "panel-foo" to "omapdss,panel-foo". This way we can have
+ * both correct DT data and omapdss specific drivers.
+ *
+ * When we get generic panel drivers to the kernel, this file will be removed.
+ */
+
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_graph.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+
+static struct list_head dss_conv_list __initdata;
+
+static const char prefix[] __initconst = "omapdss,";
+
+struct dss_conv_node {
+	struct list_head list;
+	struct device_node *node;
+	bool root;
+};
+
+static int __init omapdss_count_strings(const struct property *prop)
+{
+	const char *p = prop->value;
+	int l = 0, total = 0;
+	int i;
+
+	for (i = 0; total < prop->length; total += l, p += l, i++)
+		l = strlen(p) + 1;
+
+	return i;
+}
+
+static void __init omapdss_update_prop(struct device_node *node, char *compat,
+	int len)
+{
+	struct property *prop;
+
+	prop = kzalloc(sizeof(*prop), GFP_KERNEL);
+	if (!prop)
+		return;
+
+	prop->name = "compatible";
+	prop->value = compat;
+	prop->length = len;
+
+	of_update_property(node, prop);
+}
+
+static void __init omapdss_prefix_strcpy(char *dst, int dst_len,
+	const char *src, int src_len)
+{
+	size_t total = 0;
+
+	while (total < src_len) {
+		size_t l = strlen(src) + 1;
+
+		strcpy(dst, prefix);
+		dst += strlen(prefix);
+
+		strcpy(dst, src);
+		dst += l;
+
+		src += l;
+		total += l;
+	}
+}
+
+/* prepend compatible property strings with "omapdss," */
+static void __init omapdss_omapify_node(struct device_node *node)
+{
+	struct property *prop;
+	char *new_compat;
+	int num_strs;
+	int new_len;
+
+	prop = of_find_property(node, "compatible", NULL);
+
+	if (!prop || !prop->value)
+		return;
+
+	if (strnlen(prop->value, prop->length) >= prop->length)
+		return;
+
+	/* is it already prefixed? */
+	if (strncmp(prefix, prop->value, strlen(prefix)) == 0)
+		return;
+
+	num_strs = omapdss_count_strings(prop);
+
+	new_len = prop->length + strlen(prefix) * num_strs;
+	new_compat = kmalloc(new_len, GFP_KERNEL);
+
+	omapdss_prefix_strcpy(new_compat, new_len, prop->value, prop->length);
+
+	omapdss_update_prop(node, new_compat, new_len);
+}
+
+static void __init omapdss_add_to_list(struct device_node *node, bool root)
+{
+	struct dss_conv_node *n = kmalloc(sizeof(struct dss_conv_node),
+		GFP_KERNEL);
+	if (n) {
+		n->node = node;
+		n->root = root;
+		list_add(&n->list, &dss_conv_list);
+	}
+}
+
+static bool __init omapdss_list_contains(const struct device_node *node)
+{
+	struct dss_conv_node *n;
+
+	list_for_each_entry(n, &dss_conv_list, list) {
+		if (n->node == node)
+			return true;
+	}
+
+	return false;
+}
+
+static void __init omapdss_walk_device(struct device_node *node, bool root)
+{
+	struct device_node *n;
+
+	omapdss_add_to_list(node, root);
+
+	/*
+	 * of_graph_get_remote_port_parent() prints an error if there is no
+	 * port/ports node. To avoid that, check first that there's the node.
+	 */
+	n = of_get_child_by_name(node, "ports");
+	if (!n)
+		n = of_get_child_by_name(node, "port");
+	if (!n)
+		return;
+
+	of_node_put(n);
+
+	n = NULL;
+	while ((n = of_graph_get_next_endpoint(node, n)) != NULL) {
+		struct device_node *pn;
+
+		pn = of_graph_get_remote_port_parent(n);
+
+		if (!pn)
+			continue;
+
+		if (!of_device_is_available(pn) || omapdss_list_contains(pn)) {
+			of_node_put(pn);
+			continue;
+		}
+
+		omapdss_walk_device(pn, false);
+	}
+}
+
+static const struct of_device_id omapdss_of_match[] __initconst = {
+	{ .compatible = "ti,omap2-dss", },
+	{ .compatible = "ti,omap3-dss", },
+	{ .compatible = "ti,omap4-dss", },
+	{ .compatible = "ti,omap5-dss", },
+	{ .compatible = "ti,dra7-dss", },
+	{},
+};
+
+static int __init omapdss_boot_init(void)
+{
+	struct device_node *dss, *child;
+
+	INIT_LIST_HEAD(&dss_conv_list);
+
+	dss = of_find_matching_node(NULL, omapdss_of_match);
+
+	if (dss == NULL || !of_device_is_available(dss))
+		return 0;
+
+	omapdss_walk_device(dss, true);
+
+	for_each_available_child_of_node(dss, child) {
+		if (!of_find_property(child, "compatible", NULL))
+			continue;
+
+		omapdss_walk_device(child, true);
+	}
+
+	while (!list_empty(&dss_conv_list)) {
+		struct dss_conv_node *n;
+
+		n = list_first_entry(&dss_conv_list, struct dss_conv_node,
+			list);
+
+		if (!n->root)
+			omapdss_omapify_node(n->node);
+
+		list_del(&n->list);
+		of_node_put(n->node);
+		kfree(n);
+	}
+
+	return 0;
+}
+
+subsys_initcall(omapdss_boot_init);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/output.c b/drivers/video/fbdev/omap2/omapfb/dss/output.c
new file mode 100644
index 000000000000..16072159bd24
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/output.c
@@ -0,0 +1,267 @@
+/*
+ * Copyright (C) 2012 Texas Instruments Ltd
+ * Author: Archit Taneja <archit@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * 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/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/of.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+static LIST_HEAD(output_list);
+static DEFINE_MUTEX(output_lock);
+
+int omapdss_output_set_device(struct omap_dss_device *out,
+		struct omap_dss_device *dssdev)
+{
+	int r;
+
+	mutex_lock(&output_lock);
+
+	if (out->dst) {
+		DSSERR("output already has device %s connected to it\n",
+			out->dst->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	if (out->output_type != dssdev->type) {
+		DSSERR("output type and display type don't match\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	out->dst = dssdev;
+	dssdev->src = out;
+
+	mutex_unlock(&output_lock);
+
+	return 0;
+err:
+	mutex_unlock(&output_lock);
+
+	return r;
+}
+EXPORT_SYMBOL(omapdss_output_set_device);
+
+int omapdss_output_unset_device(struct omap_dss_device *out)
+{
+	int r;
+
+	mutex_lock(&output_lock);
+
+	if (!out->dst) {
+		DSSERR("output doesn't have a device connected to it\n");
+		r = -EINVAL;
+		goto err;
+	}
+
+	if (out->dst->state != OMAP_DSS_DISPLAY_DISABLED) {
+		DSSERR("device %s is not disabled, cannot unset device\n",
+				out->dst->name);
+		r = -EINVAL;
+		goto err;
+	}
+
+	out->dst->src = NULL;
+	out->dst = NULL;
+
+	mutex_unlock(&output_lock);
+
+	return 0;
+err:
+	mutex_unlock(&output_lock);
+
+	return r;
+}
+EXPORT_SYMBOL(omapdss_output_unset_device);
+
+int omapdss_register_output(struct omap_dss_device *out)
+{
+	list_add_tail(&out->list, &output_list);
+	return 0;
+}
+EXPORT_SYMBOL(omapdss_register_output);
+
+void omapdss_unregister_output(struct omap_dss_device *out)
+{
+	list_del(&out->list);
+}
+EXPORT_SYMBOL(omapdss_unregister_output);
+
+struct omap_dss_device *omap_dss_get_output(enum omap_dss_output_id id)
+{
+	struct omap_dss_device *out;
+
+	list_for_each_entry(out, &output_list, list) {
+		if (out->id == id)
+			return out;
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL(omap_dss_get_output);
+
+struct omap_dss_device *omap_dss_find_output(const char *name)
+{
+	struct omap_dss_device *out;
+
+	list_for_each_entry(out, &output_list, list) {
+		if (strcmp(out->name, name) == 0)
+			return omap_dss_get_device(out);
+	}
+
+	return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_output);
+
+struct omap_dss_device *omap_dss_find_output_by_port_node(struct device_node *port)
+{
+	struct device_node *src_node;
+	struct omap_dss_device *out;
+	u32 reg;
+
+	src_node = dss_of_port_get_parent_device(port);
+	if (!src_node)
+		return NULL;
+
+	reg = dss_of_port_get_port_number(port);
+
+	list_for_each_entry(out, &output_list, list) {
+		if (out->dev->of_node == src_node && out->port_num == reg) {
+			of_node_put(src_node);
+			return omap_dss_get_device(out);
+		}
+	}
+
+	of_node_put(src_node);
+
+	return NULL;
+}
+EXPORT_SYMBOL(omap_dss_find_output_by_port_node);
+
+struct omap_dss_device *omapdss_find_output_from_display(struct omap_dss_device *dssdev)
+{
+	while (dssdev->src)
+		dssdev = dssdev->src;
+
+	if (dssdev->id != 0)
+		return omap_dss_get_device(dssdev);
+
+	return NULL;
+}
+EXPORT_SYMBOL(omapdss_find_output_from_display);
+
+struct omap_overlay_manager *omapdss_find_mgr_from_display(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out;
+	struct omap_overlay_manager *mgr;
+
+	out = omapdss_find_output_from_display(dssdev);
+
+	if (out == NULL)
+		return NULL;
+
+	mgr = out->manager;
+
+	omap_dss_put_device(out);
+
+	return mgr;
+}
+EXPORT_SYMBOL(omapdss_find_mgr_from_display);
+
+static const struct dss_mgr_ops *dss_mgr_ops;
+
+int dss_install_mgr_ops(const struct dss_mgr_ops *mgr_ops)
+{
+	if (dss_mgr_ops)
+		return -EBUSY;
+
+	dss_mgr_ops = mgr_ops;
+
+	return 0;
+}
+EXPORT_SYMBOL(dss_install_mgr_ops);
+
+void dss_uninstall_mgr_ops(void)
+{
+	dss_mgr_ops = NULL;
+}
+EXPORT_SYMBOL(dss_uninstall_mgr_ops);
+
+int dss_mgr_connect(struct omap_overlay_manager *mgr,
+		struct omap_dss_device *dst)
+{
+	return dss_mgr_ops->connect(mgr, dst);
+}
+EXPORT_SYMBOL(dss_mgr_connect);
+
+void dss_mgr_disconnect(struct omap_overlay_manager *mgr,
+		struct omap_dss_device *dst)
+{
+	dss_mgr_ops->disconnect(mgr, dst);
+}
+EXPORT_SYMBOL(dss_mgr_disconnect);
+
+void dss_mgr_set_timings(struct omap_overlay_manager *mgr,
+		const struct omap_video_timings *timings)
+{
+	dss_mgr_ops->set_timings(mgr, timings);
+}
+EXPORT_SYMBOL(dss_mgr_set_timings);
+
+void dss_mgr_set_lcd_config(struct omap_overlay_manager *mgr,
+		const struct dss_lcd_mgr_config *config)
+{
+	dss_mgr_ops->set_lcd_config(mgr, config);
+}
+EXPORT_SYMBOL(dss_mgr_set_lcd_config);
+
+int dss_mgr_enable(struct omap_overlay_manager *mgr)
+{
+	return dss_mgr_ops->enable(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_enable);
+
+void dss_mgr_disable(struct omap_overlay_manager *mgr)
+{
+	dss_mgr_ops->disable(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_disable);
+
+void dss_mgr_start_update(struct omap_overlay_manager *mgr)
+{
+	dss_mgr_ops->start_update(mgr);
+}
+EXPORT_SYMBOL(dss_mgr_start_update);
+
+int dss_mgr_register_framedone_handler(struct omap_overlay_manager *mgr,
+		void (*handler)(void *), void *data)
+{
+	return dss_mgr_ops->register_framedone_handler(mgr, handler, data);
+}
+EXPORT_SYMBOL(dss_mgr_register_framedone_handler);
+
+void dss_mgr_unregister_framedone_handler(struct omap_overlay_manager *mgr,
+		void (*handler)(void *), void *data)
+{
+	dss_mgr_ops->unregister_framedone_handler(mgr, handler, data);
+}
+EXPORT_SYMBOL(dss_mgr_unregister_framedone_handler);
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c b/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c
new file mode 100644
index 000000000000..4cc5ddebfb34
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/overlay-sysfs.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "OVERLAY"
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/kobject.h>
+#include <linux/platform_device.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static ssize_t overlay_name_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", ovl->name);
+}
+
+static ssize_t overlay_manager_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n",
+			ovl->manager ? ovl->manager->name : "<none>");
+}
+
+static ssize_t overlay_manager_store(struct omap_overlay *ovl, const char *buf,
+		size_t size)
+{
+	int i, r;
+	struct omap_overlay_manager *mgr = NULL;
+	struct omap_overlay_manager *old_mgr;
+	int len = size;
+
+	if (buf[size-1] == '\n')
+		--len;
+
+	if (len > 0) {
+		for (i = 0; i < omap_dss_get_num_overlay_managers(); ++i) {
+			mgr = omap_dss_get_overlay_manager(i);
+
+			if (sysfs_streq(buf, mgr->name))
+				break;
+
+			mgr = NULL;
+		}
+	}
+
+	if (len > 0 && mgr == NULL)
+		return -EINVAL;
+
+	if (mgr)
+		DSSDBG("manager %s found\n", mgr->name);
+
+	if (mgr == ovl->manager)
+		return size;
+
+	old_mgr = ovl->manager;
+
+	r = dispc_runtime_get();
+	if (r)
+		return r;
+
+	/* detach old manager */
+	if (old_mgr) {
+		r = ovl->unset_manager(ovl);
+		if (r) {
+			DSSERR("detach failed\n");
+			goto err;
+		}
+
+		r = old_mgr->apply(old_mgr);
+		if (r)
+			goto err;
+	}
+
+	if (mgr) {
+		r = ovl->set_manager(ovl, mgr);
+		if (r) {
+			DSSERR("Failed to attach overlay\n");
+			goto err;
+		}
+
+		r = mgr->apply(mgr);
+		if (r)
+			goto err;
+	}
+
+	dispc_runtime_put();
+
+	return size;
+
+err:
+	dispc_runtime_put();
+	return r;
+}
+
+static ssize_t overlay_input_size_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.width, info.height);
+}
+
+static ssize_t overlay_screen_width_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.screen_width);
+}
+
+static ssize_t overlay_position_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.pos_x, info.pos_y);
+}
+
+static ssize_t overlay_position_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	char *last;
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.pos_x = simple_strtoul(buf, &last, 10);
+	++last;
+	if (last - buf >= size)
+		return -EINVAL;
+
+	info.pos_y = simple_strtoul(last, &last, 10);
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_output_size_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d,%d\n",
+			info.out_width, info.out_height);
+}
+
+static ssize_t overlay_output_size_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	char *last;
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.out_width = simple_strtoul(buf, &last, 10);
+	++last;
+	if (last - buf >= size)
+		return -EINVAL;
+
+	info.out_height = simple_strtoul(last, &last, 10);
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_enabled_show(struct omap_overlay *ovl, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%d\n", ovl->is_enabled(ovl));
+}
+
+static ssize_t overlay_enabled_store(struct omap_overlay *ovl, const char *buf,
+		size_t size)
+{
+	int r;
+	bool enable;
+
+	r = strtobool(buf, &enable);
+	if (r)
+		return r;
+
+	if (enable)
+		r = ovl->enable(ovl);
+	else
+		r = ovl->disable(ovl);
+
+	if (r)
+		return r;
+
+	return size;
+}
+
+static ssize_t overlay_global_alpha_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			info.global_alpha);
+}
+
+static ssize_t overlay_global_alpha_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 alpha;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_GLOBAL_ALPHA) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.global_alpha = alpha;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_pre_mult_alpha_show(struct omap_overlay *ovl,
+		char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			info.pre_mult_alpha);
+}
+
+static ssize_t overlay_pre_mult_alpha_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 alpha;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_PRE_MULT_ALPHA) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &alpha);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.pre_mult_alpha = alpha;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+static ssize_t overlay_zorder_show(struct omap_overlay *ovl, char *buf)
+{
+	struct omap_overlay_info info;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", info.zorder);
+}
+
+static ssize_t overlay_zorder_store(struct omap_overlay *ovl,
+		const char *buf, size_t size)
+{
+	int r;
+	u8 zorder;
+	struct omap_overlay_info info;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_ZORDER) == 0)
+		return -ENODEV;
+
+	r = kstrtou8(buf, 0, &zorder);
+	if (r)
+		return r;
+
+	ovl->get_overlay_info(ovl, &info);
+
+	info.zorder = zorder;
+
+	r = ovl->set_overlay_info(ovl, &info);
+	if (r)
+		return r;
+
+	if (ovl->manager) {
+		r = ovl->manager->apply(ovl->manager);
+		if (r)
+			return r;
+	}
+
+	return size;
+}
+
+struct overlay_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct omap_overlay *, char *);
+	ssize_t	(*store)(struct omap_overlay *, const char *, size_t);
+};
+
+#define OVERLAY_ATTR(_name, _mode, _show, _store) \
+	struct overlay_attribute overlay_attr_##_name = \
+	__ATTR(_name, _mode, _show, _store)
+
+static OVERLAY_ATTR(name, S_IRUGO, overlay_name_show, NULL);
+static OVERLAY_ATTR(manager, S_IRUGO|S_IWUSR,
+		overlay_manager_show, overlay_manager_store);
+static OVERLAY_ATTR(input_size, S_IRUGO, overlay_input_size_show, NULL);
+static OVERLAY_ATTR(screen_width, S_IRUGO, overlay_screen_width_show, NULL);
+static OVERLAY_ATTR(position, S_IRUGO|S_IWUSR,
+		overlay_position_show, overlay_position_store);
+static OVERLAY_ATTR(output_size, S_IRUGO|S_IWUSR,
+		overlay_output_size_show, overlay_output_size_store);
+static OVERLAY_ATTR(enabled, S_IRUGO|S_IWUSR,
+		overlay_enabled_show, overlay_enabled_store);
+static OVERLAY_ATTR(global_alpha, S_IRUGO|S_IWUSR,
+		overlay_global_alpha_show, overlay_global_alpha_store);
+static OVERLAY_ATTR(pre_mult_alpha, S_IRUGO|S_IWUSR,
+		overlay_pre_mult_alpha_show,
+		overlay_pre_mult_alpha_store);
+static OVERLAY_ATTR(zorder, S_IRUGO|S_IWUSR,
+		overlay_zorder_show, overlay_zorder_store);
+
+static struct attribute *overlay_sysfs_attrs[] = {
+	&overlay_attr_name.attr,
+	&overlay_attr_manager.attr,
+	&overlay_attr_input_size.attr,
+	&overlay_attr_screen_width.attr,
+	&overlay_attr_position.attr,
+	&overlay_attr_output_size.attr,
+	&overlay_attr_enabled.attr,
+	&overlay_attr_global_alpha.attr,
+	&overlay_attr_pre_mult_alpha.attr,
+	&overlay_attr_zorder.attr,
+	NULL
+};
+
+static ssize_t overlay_attr_show(struct kobject *kobj, struct attribute *attr,
+		char *buf)
+{
+	struct omap_overlay *overlay;
+	struct overlay_attribute *overlay_attr;
+
+	overlay = container_of(kobj, struct omap_overlay, kobj);
+	overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+	if (!overlay_attr->show)
+		return -ENOENT;
+
+	return overlay_attr->show(overlay, buf);
+}
+
+static ssize_t overlay_attr_store(struct kobject *kobj, struct attribute *attr,
+		const char *buf, size_t size)
+{
+	struct omap_overlay *overlay;
+	struct overlay_attribute *overlay_attr;
+
+	overlay = container_of(kobj, struct omap_overlay, kobj);
+	overlay_attr = container_of(attr, struct overlay_attribute, attr);
+
+	if (!overlay_attr->store)
+		return -ENOENT;
+
+	return overlay_attr->store(overlay, buf, size);
+}
+
+static const struct sysfs_ops overlay_sysfs_ops = {
+	.show = overlay_attr_show,
+	.store = overlay_attr_store,
+};
+
+static struct kobj_type overlay_ktype = {
+	.sysfs_ops = &overlay_sysfs_ops,
+	.default_attrs = overlay_sysfs_attrs,
+};
+
+int dss_overlay_kobj_init(struct omap_overlay *ovl,
+		struct platform_device *pdev)
+{
+	return kobject_init_and_add(&ovl->kobj, &overlay_ktype,
+			&pdev->dev.kobj, "overlay%d", ovl->id);
+}
+
+void dss_overlay_kobj_uninit(struct omap_overlay *ovl)
+{
+	kobject_del(&ovl->kobj);
+	kobject_put(&ovl->kobj);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/overlay.c b/drivers/video/fbdev/omap2/omapfb/dss/overlay.c
new file mode 100644
index 000000000000..2f7cee985cdd
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/overlay.c
@@ -0,0 +1,202 @@
+/*
+ * linux/drivers/video/omap2/dss/overlay.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "OVERLAY"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/sysfs.h>
+#include <linux/platform_device.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+static int num_overlays;
+static struct omap_overlay *overlays;
+
+int omap_dss_get_num_overlays(void)
+{
+	return num_overlays;
+}
+EXPORT_SYMBOL(omap_dss_get_num_overlays);
+
+struct omap_overlay *omap_dss_get_overlay(int num)
+{
+	if (num >= num_overlays)
+		return NULL;
+
+	return &overlays[num];
+}
+EXPORT_SYMBOL(omap_dss_get_overlay);
+
+void dss_init_overlays(struct platform_device *pdev)
+{
+	int i, r;
+
+	num_overlays = dss_feat_get_num_ovls();
+
+	overlays = kzalloc(sizeof(struct omap_overlay) * num_overlays,
+			GFP_KERNEL);
+
+	BUG_ON(overlays == NULL);
+
+	for (i = 0; i < num_overlays; ++i) {
+		struct omap_overlay *ovl = &overlays[i];
+
+		switch (i) {
+		case 0:
+			ovl->name = "gfx";
+			ovl->id = OMAP_DSS_GFX;
+			break;
+		case 1:
+			ovl->name = "vid1";
+			ovl->id = OMAP_DSS_VIDEO1;
+			break;
+		case 2:
+			ovl->name = "vid2";
+			ovl->id = OMAP_DSS_VIDEO2;
+			break;
+		case 3:
+			ovl->name = "vid3";
+			ovl->id = OMAP_DSS_VIDEO3;
+			break;
+		}
+
+		ovl->caps = dss_feat_get_overlay_caps(ovl->id);
+		ovl->supported_modes =
+			dss_feat_get_supported_color_modes(ovl->id);
+
+		r = dss_overlay_kobj_init(ovl, pdev);
+		if (r)
+			DSSERR("failed to create sysfs file\n");
+	}
+}
+
+void dss_uninit_overlays(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < num_overlays; ++i) {
+		struct omap_overlay *ovl = &overlays[i];
+		dss_overlay_kobj_uninit(ovl);
+	}
+
+	kfree(overlays);
+	overlays = NULL;
+	num_overlays = 0;
+}
+
+int dss_ovl_simple_check(struct omap_overlay *ovl,
+		const struct omap_overlay_info *info)
+{
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0) {
+		if (info->out_width != 0 && info->width != info->out_width) {
+			DSSERR("check_overlay: overlay %d doesn't support "
+					"scaling\n", ovl->id);
+			return -EINVAL;
+		}
+
+		if (info->out_height != 0 && info->height != info->out_height) {
+			DSSERR("check_overlay: overlay %d doesn't support "
+					"scaling\n", ovl->id);
+			return -EINVAL;
+		}
+	}
+
+	if ((ovl->supported_modes & info->color_mode) == 0) {
+		DSSERR("check_overlay: overlay %d doesn't support mode %d\n",
+				ovl->id, info->color_mode);
+		return -EINVAL;
+	}
+
+	if (info->zorder >= omap_dss_get_num_overlays()) {
+		DSSERR("check_overlay: zorder %d too high\n", info->zorder);
+		return -EINVAL;
+	}
+
+	if (dss_feat_rotation_type_supported(info->rotation_type) == 0) {
+		DSSERR("check_overlay: rotation type %d not supported\n",
+				info->rotation_type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int dss_ovl_check(struct omap_overlay *ovl, struct omap_overlay_info *info,
+		const struct omap_video_timings *mgr_timings)
+{
+	u16 outw, outh;
+	u16 dw, dh;
+
+	dw = mgr_timings->x_res;
+	dh = mgr_timings->y_res;
+
+	if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0) {
+		outw = info->width;
+		outh = info->height;
+	} else {
+		if (info->out_width == 0)
+			outw = info->width;
+		else
+			outw = info->out_width;
+
+		if (info->out_height == 0)
+			outh = info->height;
+		else
+			outh = info->out_height;
+	}
+
+	if (dw < info->pos_x + outw) {
+		DSSERR("overlay %d horizontally not inside the display area "
+				"(%d + %d >= %d)\n",
+				ovl->id, info->pos_x, outw, dw);
+		return -EINVAL;
+	}
+
+	if (dh < info->pos_y + outh) {
+		DSSERR("overlay %d vertically not inside the display area "
+				"(%d + %d >= %d)\n",
+				ovl->id, info->pos_y, outh, dh);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Checks if replication logic should be used. Only use when overlay is in
+ * RGB12U or RGB16 mode, and video port width interface is 18bpp or 24bpp
+ */
+bool dss_ovl_use_replication(struct dss_lcd_mgr_config config,
+		enum omap_color_mode mode)
+{
+	if (mode != OMAP_DSS_COLOR_RGB12U && mode != OMAP_DSS_COLOR_RGB16)
+		return false;
+
+	return config.video_port_width > 16;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/pll.c b/drivers/video/fbdev/omap2/omapfb/dss/pll.c
new file mode 100644
index 000000000000..f974ddcd3b6e
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/pll.c
@@ -0,0 +1,389 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "PLL"
+
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/regulator/consumer.h>
+#include <linux/sched.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+
+#define PLL_CONTROL			0x0000
+#define PLL_STATUS			0x0004
+#define PLL_GO				0x0008
+#define PLL_CONFIGURATION1		0x000C
+#define PLL_CONFIGURATION2		0x0010
+#define PLL_CONFIGURATION3		0x0014
+#define PLL_SSC_CONFIGURATION1		0x0018
+#define PLL_SSC_CONFIGURATION2		0x001C
+#define PLL_CONFIGURATION4		0x0020
+
+static struct dss_pll *dss_plls[4];
+
+int dss_pll_register(struct dss_pll *pll)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+		if (!dss_plls[i]) {
+			dss_plls[i] = pll;
+			return 0;
+		}
+	}
+
+	return -EBUSY;
+}
+
+void dss_pll_unregister(struct dss_pll *pll)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+		if (dss_plls[i] == pll) {
+			dss_plls[i] = NULL;
+			return;
+		}
+	}
+}
+
+struct dss_pll *dss_pll_find(const char *name)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dss_plls); ++i) {
+		if (dss_plls[i] && strcmp(dss_plls[i]->name, name) == 0)
+			return dss_plls[i];
+	}
+
+	return NULL;
+}
+
+int dss_pll_enable(struct dss_pll *pll)
+{
+	int r;
+
+	r = clk_prepare_enable(pll->clkin);
+	if (r)
+		return r;
+
+	if (pll->regulator) {
+		r = regulator_enable(pll->regulator);
+		if (r)
+			goto err_reg;
+	}
+
+	r = pll->ops->enable(pll);
+	if (r)
+		goto err_enable;
+
+	return 0;
+
+err_enable:
+	if (pll->regulator)
+		regulator_disable(pll->regulator);
+err_reg:
+	clk_disable_unprepare(pll->clkin);
+	return r;
+}
+
+void dss_pll_disable(struct dss_pll *pll)
+{
+	pll->ops->disable(pll);
+
+	if (pll->regulator)
+		regulator_disable(pll->regulator);
+
+	clk_disable_unprepare(pll->clkin);
+
+	memset(&pll->cinfo, 0, sizeof(pll->cinfo));
+}
+
+int dss_pll_set_config(struct dss_pll *pll, const struct dss_pll_clock_info *cinfo)
+{
+	int r;
+
+	r = pll->ops->set_config(pll, cinfo);
+	if (r)
+		return r;
+
+	pll->cinfo = *cinfo;
+
+	return 0;
+}
+
+bool dss_pll_hsdiv_calc(const struct dss_pll *pll, unsigned long clkdco,
+		unsigned long out_min, unsigned long out_max,
+		dss_hsdiv_calc_func func, void *data)
+{
+	const struct dss_pll_hw *hw = pll->hw;
+	int m, m_start, m_stop;
+	unsigned long out;
+
+	out_min = out_min ? out_min : 1;
+	out_max = out_max ? out_max : ULONG_MAX;
+
+	m_start = max(DIV_ROUND_UP(clkdco, out_max), 1ul);
+
+	m_stop = min((unsigned)(clkdco / out_min), hw->mX_max);
+
+	for (m = m_start; m <= m_stop; ++m) {
+		out = clkdco / m;
+
+		if (func(m, out, data))
+			return true;
+	}
+
+	return false;
+}
+
+bool dss_pll_calc(const struct dss_pll *pll, unsigned long clkin,
+		unsigned long pll_min, unsigned long pll_max,
+		dss_pll_calc_func func, void *data)
+{
+	const struct dss_pll_hw *hw = pll->hw;
+	int n, n_start, n_stop;
+	int m, m_start, m_stop;
+	unsigned long fint, clkdco;
+	unsigned long pll_hw_max;
+	unsigned long fint_hw_min, fint_hw_max;
+
+	pll_hw_max = hw->clkdco_max;
+
+	fint_hw_min = hw->fint_min;
+	fint_hw_max = hw->fint_max;
+
+	n_start = max(DIV_ROUND_UP(clkin, fint_hw_max), 1ul);
+	n_stop = min((unsigned)(clkin / fint_hw_min), hw->n_max);
+
+	pll_max = pll_max ? pll_max : ULONG_MAX;
+
+	for (n = n_start; n <= n_stop; ++n) {
+		fint = clkin / n;
+
+		m_start = max(DIV_ROUND_UP(DIV_ROUND_UP(pll_min, fint), 2),
+				1ul);
+		m_stop = min3((unsigned)(pll_max / fint / 2),
+				(unsigned)(pll_hw_max / fint / 2),
+				hw->m_max);
+
+		for (m = m_start; m <= m_stop; ++m) {
+			clkdco = 2 * m * fint;
+
+			if (func(n, m, fint, clkdco, data))
+				return true;
+		}
+	}
+
+	return false;
+}
+
+static int wait_for_bit_change(void __iomem *reg, int bitnum, int value)
+{
+	unsigned long timeout;
+	ktime_t wait;
+	int t;
+
+	/* first busyloop to see if the bit changes right away */
+	t = 100;
+	while (t-- > 0) {
+		if (FLD_GET(readl_relaxed(reg), bitnum, bitnum) == value)
+			return value;
+	}
+
+	/* then loop for 500ms, sleeping for 1ms in between */
+	timeout = jiffies + msecs_to_jiffies(500);
+	while (time_before(jiffies, timeout)) {
+		if (FLD_GET(readl_relaxed(reg), bitnum, bitnum) == value)
+			return value;
+
+		wait = ns_to_ktime(1000 * 1000);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		schedule_hrtimeout(&wait, HRTIMER_MODE_REL);
+	}
+
+	return !value;
+}
+
+int dss_pll_wait_reset_done(struct dss_pll *pll)
+{
+	void __iomem *base = pll->base;
+
+	if (wait_for_bit_change(base + PLL_STATUS, 0, 1) != 1)
+		return -ETIMEDOUT;
+	else
+		return 0;
+}
+
+static int dss_wait_hsdiv_ack(struct dss_pll *pll, u32 hsdiv_ack_mask)
+{
+	int t = 100;
+
+	while (t-- > 0) {
+		u32 v = readl_relaxed(pll->base + PLL_STATUS);
+		v &= hsdiv_ack_mask;
+		if (v == hsdiv_ack_mask)
+			return 0;
+	}
+
+	return -ETIMEDOUT;
+}
+
+int dss_pll_write_config_type_a(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo)
+{
+	const struct dss_pll_hw *hw = pll->hw;
+	void __iomem *base = pll->base;
+	int r = 0;
+	u32 l;
+
+	l = 0;
+	if (hw->has_stopmode)
+		l = FLD_MOD(l, 1, 0, 0);		/* PLL_STOPMODE */
+	l = FLD_MOD(l, cinfo->n - 1, hw->n_msb, hw->n_lsb);	/* PLL_REGN */
+	l = FLD_MOD(l, cinfo->m, hw->m_msb, hw->m_lsb);		/* PLL_REGM */
+	/* M4 */
+	l = FLD_MOD(l, cinfo->mX[0] ? cinfo->mX[0] - 1 : 0,
+			hw->mX_msb[0], hw->mX_lsb[0]);
+	/* M5 */
+	l = FLD_MOD(l, cinfo->mX[1] ? cinfo->mX[1] - 1 : 0,
+			hw->mX_msb[1], hw->mX_lsb[1]);
+	writel_relaxed(l, base + PLL_CONFIGURATION1);
+
+	l = 0;
+	/* M6 */
+	l = FLD_MOD(l, cinfo->mX[2] ? cinfo->mX[2] - 1 : 0,
+			hw->mX_msb[2], hw->mX_lsb[2]);
+	/* M7 */
+	l = FLD_MOD(l, cinfo->mX[3] ? cinfo->mX[3] - 1 : 0,
+			hw->mX_msb[3], hw->mX_lsb[3]);
+	writel_relaxed(l, base + PLL_CONFIGURATION3);
+
+	l = readl_relaxed(base + PLL_CONFIGURATION2);
+	if (hw->has_freqsel) {
+		u32 f = cinfo->fint < 1000000 ? 0x3 :
+			cinfo->fint < 1250000 ? 0x4 :
+			cinfo->fint < 1500000 ? 0x5 :
+			cinfo->fint < 1750000 ? 0x6 :
+			0x7;
+
+		l = FLD_MOD(l, f, 4, 1);	/* PLL_FREQSEL */
+	} else if (hw->has_selfreqdco) {
+		u32 f = cinfo->clkdco < hw->clkdco_low ? 0x2 : 0x4;
+
+		l = FLD_MOD(l, f, 3, 1);	/* PLL_SELFREQDCO */
+	}
+	l = FLD_MOD(l, 1, 13, 13);		/* PLL_REFEN */
+	l = FLD_MOD(l, 0, 14, 14);		/* PHY_CLKINEN */
+	l = FLD_MOD(l, 0, 16, 16);		/* M4_CLOCK_EN */
+	l = FLD_MOD(l, 0, 18, 18);		/* M5_CLOCK_EN */
+	l = FLD_MOD(l, 1, 20, 20);		/* HSDIVBYPASS */
+	if (hw->has_refsel)
+		l = FLD_MOD(l, 3, 22, 21);	/* REFSEL = sysclk */
+	l = FLD_MOD(l, 0, 23, 23);		/* M6_CLOCK_EN */
+	l = FLD_MOD(l, 0, 25, 25);		/* M7_CLOCK_EN */
+	writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+	writel_relaxed(1, base + PLL_GO);	/* PLL_GO */
+
+	if (wait_for_bit_change(base + PLL_GO, 0, 0) != 0) {
+		DSSERR("DSS DPLL GO bit not going down.\n");
+		r = -EIO;
+		goto err;
+	}
+
+	if (wait_for_bit_change(base + PLL_STATUS, 1, 1) != 1) {
+		DSSERR("cannot lock DSS DPLL\n");
+		r = -EIO;
+		goto err;
+	}
+
+	l = readl_relaxed(base + PLL_CONFIGURATION2);
+	l = FLD_MOD(l, 1, 14, 14);			/* PHY_CLKINEN */
+	l = FLD_MOD(l, cinfo->mX[0] ? 1 : 0, 16, 16);	/* M4_CLOCK_EN */
+	l = FLD_MOD(l, cinfo->mX[1] ? 1 : 0, 18, 18);	/* M5_CLOCK_EN */
+	l = FLD_MOD(l, 0, 20, 20);			/* HSDIVBYPASS */
+	l = FLD_MOD(l, cinfo->mX[2] ? 1 : 0, 23, 23);	/* M6_CLOCK_EN */
+	l = FLD_MOD(l, cinfo->mX[3] ? 1 : 0, 25, 25);	/* M7_CLOCK_EN */
+	writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+	r = dss_wait_hsdiv_ack(pll,
+		(cinfo->mX[0] ? BIT(7) : 0) |
+		(cinfo->mX[1] ? BIT(8) : 0) |
+		(cinfo->mX[2] ? BIT(10) : 0) |
+		(cinfo->mX[3] ? BIT(11) : 0));
+	if (r) {
+		DSSERR("failed to enable HSDIV clocks\n");
+		goto err;
+	}
+
+err:
+	return r;
+}
+
+int dss_pll_write_config_type_b(struct dss_pll *pll,
+		const struct dss_pll_clock_info *cinfo)
+{
+	const struct dss_pll_hw *hw = pll->hw;
+	void __iomem *base = pll->base;
+	u32 l;
+
+	l = 0;
+	l = FLD_MOD(l, cinfo->m, 20, 9);	/* PLL_REGM */
+	l = FLD_MOD(l, cinfo->n - 1, 8, 1);	/* PLL_REGN */
+	writel_relaxed(l, base + PLL_CONFIGURATION1);
+
+	l = readl_relaxed(base + PLL_CONFIGURATION2);
+	l = FLD_MOD(l, 0x0, 12, 12);	/* PLL_HIGHFREQ divide by 2 */
+	l = FLD_MOD(l, 0x1, 13, 13);	/* PLL_REFEN */
+	l = FLD_MOD(l, 0x0, 14, 14);	/* PHY_CLKINEN */
+	if (hw->has_refsel)
+		l = FLD_MOD(l, 0x3, 22, 21);	/* REFSEL = SYSCLK */
+
+	/* PLL_SELFREQDCO */
+	if (cinfo->clkdco > hw->clkdco_low)
+		l = FLD_MOD(l, 0x4, 3, 1);
+	else
+		l = FLD_MOD(l, 0x2, 3, 1);
+	writel_relaxed(l, base + PLL_CONFIGURATION2);
+
+	l = readl_relaxed(base + PLL_CONFIGURATION3);
+	l = FLD_MOD(l, cinfo->sd, 17, 10);	/* PLL_REGSD */
+	writel_relaxed(l, base + PLL_CONFIGURATION3);
+
+	l = readl_relaxed(base + PLL_CONFIGURATION4);
+	l = FLD_MOD(l, cinfo->mX[0], 24, 18);	/* PLL_REGM2 */
+	l = FLD_MOD(l, cinfo->mf, 17, 0);	/* PLL_REGM_F */
+	writel_relaxed(l, base + PLL_CONFIGURATION4);
+
+	writel_relaxed(1, base + PLL_GO);	/* PLL_GO */
+
+	if (wait_for_bit_change(base + PLL_GO, 0, 0) != 0) {
+		DSSERR("DSS DPLL GO bit not going down.\n");
+		return -EIO;
+	}
+
+	if (wait_for_bit_change(base + PLL_STATUS, 1, 1) != 1) {
+		DSSERR("cannot lock DSS DPLL\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c b/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c
new file mode 100644
index 000000000000..aea6a1d0fb20
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/rfbi.c
@@ -0,0 +1,1078 @@
+/*
+ * linux/drivers/video/omap2/dss/rfbi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * Some code and ideas taken from drivers/video/omap/ driver
+ * by Imre Deak.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "RFBI"
+
+#include <linux/kernel.h>
+#include <linux/dma-mapping.h>
+#include <linux/export.h>
+#include <linux/vmalloc.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/kfifo.h>
+#include <linux/ktime.h>
+#include <linux/hrtimer.h>
+#include <linux/seq_file.h>
+#include <linux/semaphore.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+struct rfbi_reg { u16 idx; };
+
+#define RFBI_REG(idx)		((const struct rfbi_reg) { idx })
+
+#define RFBI_REVISION		RFBI_REG(0x0000)
+#define RFBI_SYSCONFIG		RFBI_REG(0x0010)
+#define RFBI_SYSSTATUS		RFBI_REG(0x0014)
+#define RFBI_CONTROL		RFBI_REG(0x0040)
+#define RFBI_PIXEL_CNT		RFBI_REG(0x0044)
+#define RFBI_LINE_NUMBER	RFBI_REG(0x0048)
+#define RFBI_CMD		RFBI_REG(0x004c)
+#define RFBI_PARAM		RFBI_REG(0x0050)
+#define RFBI_DATA		RFBI_REG(0x0054)
+#define RFBI_READ		RFBI_REG(0x0058)
+#define RFBI_STATUS		RFBI_REG(0x005c)
+
+#define RFBI_CONFIG(n)		RFBI_REG(0x0060 + (n)*0x18)
+#define RFBI_ONOFF_TIME(n)	RFBI_REG(0x0064 + (n)*0x18)
+#define RFBI_CYCLE_TIME(n)	RFBI_REG(0x0068 + (n)*0x18)
+#define RFBI_DATA_CYCLE1(n)	RFBI_REG(0x006c + (n)*0x18)
+#define RFBI_DATA_CYCLE2(n)	RFBI_REG(0x0070 + (n)*0x18)
+#define RFBI_DATA_CYCLE3(n)	RFBI_REG(0x0074 + (n)*0x18)
+
+#define RFBI_VSYNC_WIDTH	RFBI_REG(0x0090)
+#define RFBI_HSYNC_WIDTH	RFBI_REG(0x0094)
+
+#define REG_FLD_MOD(idx, val, start, end) \
+	rfbi_write_reg(idx, FLD_MOD(rfbi_read_reg(idx), val, start, end))
+
+enum omap_rfbi_cycleformat {
+	OMAP_DSS_RFBI_CYCLEFORMAT_1_1 = 0,
+	OMAP_DSS_RFBI_CYCLEFORMAT_2_1 = 1,
+	OMAP_DSS_RFBI_CYCLEFORMAT_3_1 = 2,
+	OMAP_DSS_RFBI_CYCLEFORMAT_3_2 = 3,
+};
+
+enum omap_rfbi_datatype {
+	OMAP_DSS_RFBI_DATATYPE_12 = 0,
+	OMAP_DSS_RFBI_DATATYPE_16 = 1,
+	OMAP_DSS_RFBI_DATATYPE_18 = 2,
+	OMAP_DSS_RFBI_DATATYPE_24 = 3,
+};
+
+enum omap_rfbi_parallelmode {
+	OMAP_DSS_RFBI_PARALLELMODE_8 = 0,
+	OMAP_DSS_RFBI_PARALLELMODE_9 = 1,
+	OMAP_DSS_RFBI_PARALLELMODE_12 = 2,
+	OMAP_DSS_RFBI_PARALLELMODE_16 = 3,
+};
+
+static int rfbi_convert_timings(struct rfbi_timings *t);
+static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div);
+
+static struct {
+	struct platform_device *pdev;
+	void __iomem	*base;
+
+	unsigned long	l4_khz;
+
+	enum omap_rfbi_datatype datatype;
+	enum omap_rfbi_parallelmode parallelmode;
+
+	enum omap_rfbi_te_mode te_mode;
+	int te_enabled;
+
+	void (*framedone_callback)(void *data);
+	void *framedone_callback_data;
+
+	struct omap_dss_device *dssdev[2];
+
+	struct semaphore bus_lock;
+
+	struct omap_video_timings timings;
+	int pixel_size;
+	int data_lines;
+	struct rfbi_timings intf_timings;
+
+	struct omap_dss_device output;
+} rfbi;
+
+static inline void rfbi_write_reg(const struct rfbi_reg idx, u32 val)
+{
+	__raw_writel(val, rfbi.base + idx.idx);
+}
+
+static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
+{
+	return __raw_readl(rfbi.base + idx.idx);
+}
+
+static int rfbi_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("rfbi_runtime_get\n");
+
+	r = pm_runtime_get_sync(&rfbi.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void rfbi_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("rfbi_runtime_put\n");
+
+	r = pm_runtime_put_sync(&rfbi.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static void rfbi_bus_lock(void)
+{
+	down(&rfbi.bus_lock);
+}
+
+static void rfbi_bus_unlock(void)
+{
+	up(&rfbi.bus_lock);
+}
+
+static void rfbi_write_command(const void *buf, u32 len)
+{
+	switch (rfbi.parallelmode) {
+	case OMAP_DSS_RFBI_PARALLELMODE_8:
+	{
+		const u8 *b = buf;
+		for (; len; len--)
+			rfbi_write_reg(RFBI_CMD, *b++);
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_16:
+	{
+		const u16 *w = buf;
+		BUG_ON(len & 1);
+		for (; len; len -= 2)
+			rfbi_write_reg(RFBI_CMD, *w++);
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_9:
+	case OMAP_DSS_RFBI_PARALLELMODE_12:
+	default:
+		BUG();
+	}
+}
+
+static void rfbi_read_data(void *buf, u32 len)
+{
+	switch (rfbi.parallelmode) {
+	case OMAP_DSS_RFBI_PARALLELMODE_8:
+	{
+		u8 *b = buf;
+		for (; len; len--) {
+			rfbi_write_reg(RFBI_READ, 0);
+			*b++ = rfbi_read_reg(RFBI_READ);
+		}
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_16:
+	{
+		u16 *w = buf;
+		BUG_ON(len & ~1);
+		for (; len; len -= 2) {
+			rfbi_write_reg(RFBI_READ, 0);
+			*w++ = rfbi_read_reg(RFBI_READ);
+		}
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_9:
+	case OMAP_DSS_RFBI_PARALLELMODE_12:
+	default:
+		BUG();
+	}
+}
+
+static void rfbi_write_data(const void *buf, u32 len)
+{
+	switch (rfbi.parallelmode) {
+	case OMAP_DSS_RFBI_PARALLELMODE_8:
+	{
+		const u8 *b = buf;
+		for (; len; len--)
+			rfbi_write_reg(RFBI_PARAM, *b++);
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_16:
+	{
+		const u16 *w = buf;
+		BUG_ON(len & 1);
+		for (; len; len -= 2)
+			rfbi_write_reg(RFBI_PARAM, *w++);
+		break;
+	}
+
+	case OMAP_DSS_RFBI_PARALLELMODE_9:
+	case OMAP_DSS_RFBI_PARALLELMODE_12:
+	default:
+		BUG();
+
+	}
+}
+
+static void rfbi_write_pixels(const void __iomem *buf, int scr_width,
+		u16 x, u16 y,
+		u16 w, u16 h)
+{
+	int start_offset = scr_width * y + x;
+	int horiz_offset = scr_width - w;
+	int i;
+
+	if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
+	   rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
+		const u16 __iomem *pd = buf;
+		pd += start_offset;
+
+		for (; h; --h) {
+			for (i = 0; i < w; ++i) {
+				const u8 __iomem *b = (const u8 __iomem *)pd;
+				rfbi_write_reg(RFBI_PARAM, __raw_readb(b+1));
+				rfbi_write_reg(RFBI_PARAM, __raw_readb(b+0));
+				++pd;
+			}
+			pd += horiz_offset;
+		}
+	} else if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_24 &&
+	   rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_8) {
+		const u32 __iomem *pd = buf;
+		pd += start_offset;
+
+		for (; h; --h) {
+			for (i = 0; i < w; ++i) {
+				const u8 __iomem *b = (const u8 __iomem *)pd;
+				rfbi_write_reg(RFBI_PARAM, __raw_readb(b+2));
+				rfbi_write_reg(RFBI_PARAM, __raw_readb(b+1));
+				rfbi_write_reg(RFBI_PARAM, __raw_readb(b+0));
+				++pd;
+			}
+			pd += horiz_offset;
+		}
+	} else if (rfbi.datatype == OMAP_DSS_RFBI_DATATYPE_16 &&
+	   rfbi.parallelmode == OMAP_DSS_RFBI_PARALLELMODE_16) {
+		const u16 __iomem *pd = buf;
+		pd += start_offset;
+
+		for (; h; --h) {
+			for (i = 0; i < w; ++i) {
+				rfbi_write_reg(RFBI_PARAM, __raw_readw(pd));
+				++pd;
+			}
+			pd += horiz_offset;
+		}
+	} else {
+		BUG();
+	}
+}
+
+static int rfbi_transfer_area(struct omap_dss_device *dssdev,
+		void (*callback)(void *data), void *data)
+{
+	u32 l;
+	int r;
+	struct omap_overlay_manager *mgr = rfbi.output.manager;
+	u16 width = rfbi.timings.x_res;
+	u16 height = rfbi.timings.y_res;
+
+	/*BUG_ON(callback == 0);*/
+	BUG_ON(rfbi.framedone_callback != NULL);
+
+	DSSDBG("rfbi_transfer_area %dx%d\n", width, height);
+
+	dss_mgr_set_timings(mgr, &rfbi.timings);
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		return r;
+
+	rfbi.framedone_callback = callback;
+	rfbi.framedone_callback_data = data;
+
+	rfbi_write_reg(RFBI_PIXEL_CNT, width * height);
+
+	l = rfbi_read_reg(RFBI_CONTROL);
+	l = FLD_MOD(l, 1, 0, 0); /* enable */
+	if (!rfbi.te_enabled)
+		l = FLD_MOD(l, 1, 4, 4); /* ITE */
+
+	rfbi_write_reg(RFBI_CONTROL, l);
+
+	return 0;
+}
+
+static void framedone_callback(void *data)
+{
+	void (*callback)(void *data);
+
+	DSSDBG("FRAMEDONE\n");
+
+	REG_FLD_MOD(RFBI_CONTROL, 0, 0, 0);
+
+	callback = rfbi.framedone_callback;
+	rfbi.framedone_callback = NULL;
+
+	if (callback != NULL)
+		callback(rfbi.framedone_callback_data);
+}
+
+#if 1 /* VERBOSE */
+static void rfbi_print_timings(void)
+{
+	u32 l;
+	u32 time;
+
+	l = rfbi_read_reg(RFBI_CONFIG(0));
+	time = 1000000000 / rfbi.l4_khz;
+	if (l & (1 << 4))
+		time *= 2;
+
+	DSSDBG("Tick time %u ps\n", time);
+	l = rfbi_read_reg(RFBI_ONOFF_TIME(0));
+	DSSDBG("CSONTIME %d, CSOFFTIME %d, WEONTIME %d, WEOFFTIME %d, "
+		"REONTIME %d, REOFFTIME %d\n",
+		l & 0x0f, (l >> 4) & 0x3f, (l >> 10) & 0x0f, (l >> 14) & 0x3f,
+		(l >> 20) & 0x0f, (l >> 24) & 0x3f);
+
+	l = rfbi_read_reg(RFBI_CYCLE_TIME(0));
+	DSSDBG("WECYCLETIME %d, RECYCLETIME %d, CSPULSEWIDTH %d, "
+		"ACCESSTIME %d\n",
+		(l & 0x3f), (l >> 6) & 0x3f, (l >> 12) & 0x3f,
+		(l >> 22) & 0x3f);
+}
+#else
+static void rfbi_print_timings(void) {}
+#endif
+
+
+
+
+static u32 extif_clk_period;
+
+static inline unsigned long round_to_extif_ticks(unsigned long ps, int div)
+{
+	int bus_tick = extif_clk_period * div;
+	return (ps + bus_tick - 1) / bus_tick * bus_tick;
+}
+
+static int calc_reg_timing(struct rfbi_timings *t, int div)
+{
+	t->clk_div = div;
+
+	t->cs_on_time = round_to_extif_ticks(t->cs_on_time, div);
+
+	t->we_on_time = round_to_extif_ticks(t->we_on_time, div);
+	t->we_off_time = round_to_extif_ticks(t->we_off_time, div);
+	t->we_cycle_time = round_to_extif_ticks(t->we_cycle_time, div);
+
+	t->re_on_time = round_to_extif_ticks(t->re_on_time, div);
+	t->re_off_time = round_to_extif_ticks(t->re_off_time, div);
+	t->re_cycle_time = round_to_extif_ticks(t->re_cycle_time, div);
+
+	t->access_time = round_to_extif_ticks(t->access_time, div);
+	t->cs_off_time = round_to_extif_ticks(t->cs_off_time, div);
+	t->cs_pulse_width = round_to_extif_ticks(t->cs_pulse_width, div);
+
+	DSSDBG("[reg]cson %d csoff %d reon %d reoff %d\n",
+	       t->cs_on_time, t->cs_off_time, t->re_on_time, t->re_off_time);
+	DSSDBG("[reg]weon %d weoff %d recyc %d wecyc %d\n",
+	       t->we_on_time, t->we_off_time, t->re_cycle_time,
+	       t->we_cycle_time);
+	DSSDBG("[reg]rdaccess %d cspulse %d\n",
+	       t->access_time, t->cs_pulse_width);
+
+	return rfbi_convert_timings(t);
+}
+
+static int calc_extif_timings(struct rfbi_timings *t)
+{
+	u32 max_clk_div;
+	int div;
+
+	rfbi_get_clk_info(&extif_clk_period, &max_clk_div);
+	for (div = 1; div <= max_clk_div; div++) {
+		if (calc_reg_timing(t, div) == 0)
+			break;
+	}
+
+	if (div <= max_clk_div)
+		return 0;
+
+	DSSERR("can't setup timings\n");
+	return -1;
+}
+
+
+static void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t)
+{
+	int r;
+
+	if (!t->converted) {
+		r = calc_extif_timings(t);
+		if (r < 0)
+			DSSERR("Failed to calc timings\n");
+	}
+
+	BUG_ON(!t->converted);
+
+	rfbi_write_reg(RFBI_ONOFF_TIME(rfbi_module), t->tim[0]);
+	rfbi_write_reg(RFBI_CYCLE_TIME(rfbi_module), t->tim[1]);
+
+	/* TIMEGRANULARITY */
+	REG_FLD_MOD(RFBI_CONFIG(rfbi_module),
+		    (t->tim[2] ? 1 : 0), 4, 4);
+
+	rfbi_print_timings();
+}
+
+static int ps_to_rfbi_ticks(int time, int div)
+{
+	unsigned long tick_ps;
+	int ret;
+
+	/* Calculate in picosecs to yield more exact results */
+	tick_ps = 1000000000 / (rfbi.l4_khz) * div;
+
+	ret = (time + tick_ps - 1) / tick_ps;
+
+	return ret;
+}
+
+static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div)
+{
+	*clk_period = 1000000000 / rfbi.l4_khz;
+	*max_clk_div = 2;
+}
+
+static int rfbi_convert_timings(struct rfbi_timings *t)
+{
+	u32 l;
+	int reon, reoff, weon, weoff, cson, csoff, cs_pulse;
+	int actim, recyc, wecyc;
+	int div = t->clk_div;
+
+	if (div <= 0 || div > 2)
+		return -1;
+
+	/* Make sure that after conversion it still holds that:
+	 * weoff > weon, reoff > reon, recyc >= reoff, wecyc >= weoff,
+	 * csoff > cson, csoff >= max(weoff, reoff), actim > reon
+	 */
+	weon = ps_to_rfbi_ticks(t->we_on_time, div);
+	weoff = ps_to_rfbi_ticks(t->we_off_time, div);
+	if (weoff <= weon)
+		weoff = weon + 1;
+	if (weon > 0x0f)
+		return -1;
+	if (weoff > 0x3f)
+		return -1;
+
+	reon = ps_to_rfbi_ticks(t->re_on_time, div);
+	reoff = ps_to_rfbi_ticks(t->re_off_time, div);
+	if (reoff <= reon)
+		reoff = reon + 1;
+	if (reon > 0x0f)
+		return -1;
+	if (reoff > 0x3f)
+		return -1;
+
+	cson = ps_to_rfbi_ticks(t->cs_on_time, div);
+	csoff = ps_to_rfbi_ticks(t->cs_off_time, div);
+	if (csoff <= cson)
+		csoff = cson + 1;
+	if (csoff < max(weoff, reoff))
+		csoff = max(weoff, reoff);
+	if (cson > 0x0f)
+		return -1;
+	if (csoff > 0x3f)
+		return -1;
+
+	l =  cson;
+	l |= csoff << 4;
+	l |= weon  << 10;
+	l |= weoff << 14;
+	l |= reon  << 20;
+	l |= reoff << 24;
+
+	t->tim[0] = l;
+
+	actim = ps_to_rfbi_ticks(t->access_time, div);
+	if (actim <= reon)
+		actim = reon + 1;
+	if (actim > 0x3f)
+		return -1;
+
+	wecyc = ps_to_rfbi_ticks(t->we_cycle_time, div);
+	if (wecyc < weoff)
+		wecyc = weoff;
+	if (wecyc > 0x3f)
+		return -1;
+
+	recyc = ps_to_rfbi_ticks(t->re_cycle_time, div);
+	if (recyc < reoff)
+		recyc = reoff;
+	if (recyc > 0x3f)
+		return -1;
+
+	cs_pulse = ps_to_rfbi_ticks(t->cs_pulse_width, div);
+	if (cs_pulse > 0x3f)
+		return -1;
+
+	l =  wecyc;
+	l |= recyc    << 6;
+	l |= cs_pulse << 12;
+	l |= actim    << 22;
+
+	t->tim[1] = l;
+
+	t->tim[2] = div - 1;
+
+	t->converted = 1;
+
+	return 0;
+}
+
+/* xxx FIX module selection missing */
+static int rfbi_setup_te(enum omap_rfbi_te_mode mode,
+			     unsigned hs_pulse_time, unsigned vs_pulse_time,
+			     int hs_pol_inv, int vs_pol_inv, int extif_div)
+{
+	int hs, vs;
+	int min;
+	u32 l;
+
+	hs = ps_to_rfbi_ticks(hs_pulse_time, 1);
+	vs = ps_to_rfbi_ticks(vs_pulse_time, 1);
+	if (hs < 2)
+		return -EDOM;
+	if (mode == OMAP_DSS_RFBI_TE_MODE_2)
+		min = 2;
+	else /* OMAP_DSS_RFBI_TE_MODE_1 */
+		min = 4;
+	if (vs < min)
+		return -EDOM;
+	if (vs == hs)
+		return -EINVAL;
+	rfbi.te_mode = mode;
+	DSSDBG("setup_te: mode %d hs %d vs %d hs_inv %d vs_inv %d\n",
+		mode, hs, vs, hs_pol_inv, vs_pol_inv);
+
+	rfbi_write_reg(RFBI_HSYNC_WIDTH, hs);
+	rfbi_write_reg(RFBI_VSYNC_WIDTH, vs);
+
+	l = rfbi_read_reg(RFBI_CONFIG(0));
+	if (hs_pol_inv)
+		l &= ~(1 << 21);
+	else
+		l |= 1 << 21;
+	if (vs_pol_inv)
+		l &= ~(1 << 20);
+	else
+		l |= 1 << 20;
+
+	return 0;
+}
+
+/* xxx FIX module selection missing */
+static int rfbi_enable_te(bool enable, unsigned line)
+{
+	u32 l;
+
+	DSSDBG("te %d line %d mode %d\n", enable, line, rfbi.te_mode);
+	if (line > (1 << 11) - 1)
+		return -EINVAL;
+
+	l = rfbi_read_reg(RFBI_CONFIG(0));
+	l &= ~(0x3 << 2);
+	if (enable) {
+		rfbi.te_enabled = 1;
+		l |= rfbi.te_mode << 2;
+	} else
+		rfbi.te_enabled = 0;
+	rfbi_write_reg(RFBI_CONFIG(0), l);
+	rfbi_write_reg(RFBI_LINE_NUMBER, line);
+
+	return 0;
+}
+
+static int rfbi_configure_bus(int rfbi_module, int bpp, int lines)
+{
+	u32 l;
+	int cycle1 = 0, cycle2 = 0, cycle3 = 0;
+	enum omap_rfbi_cycleformat cycleformat;
+	enum omap_rfbi_datatype datatype;
+	enum omap_rfbi_parallelmode parallelmode;
+
+	switch (bpp) {
+	case 12:
+		datatype = OMAP_DSS_RFBI_DATATYPE_12;
+		break;
+	case 16:
+		datatype = OMAP_DSS_RFBI_DATATYPE_16;
+		break;
+	case 18:
+		datatype = OMAP_DSS_RFBI_DATATYPE_18;
+		break;
+	case 24:
+		datatype = OMAP_DSS_RFBI_DATATYPE_24;
+		break;
+	default:
+		BUG();
+		return 1;
+	}
+	rfbi.datatype = datatype;
+
+	switch (lines) {
+	case 8:
+		parallelmode = OMAP_DSS_RFBI_PARALLELMODE_8;
+		break;
+	case 9:
+		parallelmode = OMAP_DSS_RFBI_PARALLELMODE_9;
+		break;
+	case 12:
+		parallelmode = OMAP_DSS_RFBI_PARALLELMODE_12;
+		break;
+	case 16:
+		parallelmode = OMAP_DSS_RFBI_PARALLELMODE_16;
+		break;
+	default:
+		BUG();
+		return 1;
+	}
+	rfbi.parallelmode = parallelmode;
+
+	if ((bpp % lines) == 0) {
+		switch (bpp / lines) {
+		case 1:
+			cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_1_1;
+			break;
+		case 2:
+			cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_2_1;
+			break;
+		case 3:
+			cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_3_1;
+			break;
+		default:
+			BUG();
+			return 1;
+		}
+	} else if ((2 * bpp % lines) == 0) {
+		if ((2 * bpp / lines) == 3)
+			cycleformat = OMAP_DSS_RFBI_CYCLEFORMAT_3_2;
+		else {
+			BUG();
+			return 1;
+		}
+	} else {
+		BUG();
+		return 1;
+	}
+
+	switch (cycleformat) {
+	case OMAP_DSS_RFBI_CYCLEFORMAT_1_1:
+		cycle1 = lines;
+		break;
+
+	case OMAP_DSS_RFBI_CYCLEFORMAT_2_1:
+		cycle1 = lines;
+		cycle2 = lines;
+		break;
+
+	case OMAP_DSS_RFBI_CYCLEFORMAT_3_1:
+		cycle1 = lines;
+		cycle2 = lines;
+		cycle3 = lines;
+		break;
+
+	case OMAP_DSS_RFBI_CYCLEFORMAT_3_2:
+		cycle1 = lines;
+		cycle2 = (lines / 2) | ((lines / 2) << 16);
+		cycle3 = (lines << 16);
+		break;
+	}
+
+	REG_FLD_MOD(RFBI_CONTROL, 0, 3, 2); /* clear CS */
+
+	l = 0;
+	l |= FLD_VAL(parallelmode, 1, 0);
+	l |= FLD_VAL(0, 3, 2);		/* TRIGGERMODE: ITE */
+	l |= FLD_VAL(0, 4, 4);		/* TIMEGRANULARITY */
+	l |= FLD_VAL(datatype, 6, 5);
+	/* l |= FLD_VAL(2, 8, 7); */	/* L4FORMAT, 2pix/L4 */
+	l |= FLD_VAL(0, 8, 7);	/* L4FORMAT, 1pix/L4 */
+	l |= FLD_VAL(cycleformat, 10, 9);
+	l |= FLD_VAL(0, 12, 11);	/* UNUSEDBITS */
+	l |= FLD_VAL(0, 16, 16);	/* A0POLARITY */
+	l |= FLD_VAL(0, 17, 17);	/* REPOLARITY */
+	l |= FLD_VAL(0, 18, 18);	/* WEPOLARITY */
+	l |= FLD_VAL(0, 19, 19);	/* CSPOLARITY */
+	l |= FLD_VAL(1, 20, 20);	/* TE_VSYNC_POLARITY */
+	l |= FLD_VAL(1, 21, 21);	/* HSYNCPOLARITY */
+	rfbi_write_reg(RFBI_CONFIG(rfbi_module), l);
+
+	rfbi_write_reg(RFBI_DATA_CYCLE1(rfbi_module), cycle1);
+	rfbi_write_reg(RFBI_DATA_CYCLE2(rfbi_module), cycle2);
+	rfbi_write_reg(RFBI_DATA_CYCLE3(rfbi_module), cycle3);
+
+
+	l = rfbi_read_reg(RFBI_CONTROL);
+	l = FLD_MOD(l, rfbi_module+1, 3, 2); /* Select CSx */
+	l = FLD_MOD(l, 0, 1, 1); /* clear bypass */
+	rfbi_write_reg(RFBI_CONTROL, l);
+
+
+	DSSDBG("RFBI config: bpp %d, lines %d, cycles: 0x%x 0x%x 0x%x\n",
+	       bpp, lines, cycle1, cycle2, cycle3);
+
+	return 0;
+}
+
+static int rfbi_configure(struct omap_dss_device *dssdev)
+{
+	return rfbi_configure_bus(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+			rfbi.data_lines);
+}
+
+static int rfbi_update(struct omap_dss_device *dssdev, void (*callback)(void *),
+		void *data)
+{
+	return rfbi_transfer_area(dssdev, callback, data);
+}
+
+static void rfbi_set_size(struct omap_dss_device *dssdev, u16 w, u16 h)
+{
+	rfbi.timings.x_res = w;
+	rfbi.timings.y_res = h;
+}
+
+static void rfbi_set_pixel_size(struct omap_dss_device *dssdev, int pixel_size)
+{
+	rfbi.pixel_size = pixel_size;
+}
+
+static void rfbi_set_data_lines(struct omap_dss_device *dssdev, int data_lines)
+{
+	rfbi.data_lines = data_lines;
+}
+
+static void rfbi_set_interface_timings(struct omap_dss_device *dssdev,
+		struct rfbi_timings *timings)
+{
+	rfbi.intf_timings = *timings;
+}
+
+static void rfbi_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
+
+	if (rfbi_runtime_get())
+		return;
+
+	DUMPREG(RFBI_REVISION);
+	DUMPREG(RFBI_SYSCONFIG);
+	DUMPREG(RFBI_SYSSTATUS);
+	DUMPREG(RFBI_CONTROL);
+	DUMPREG(RFBI_PIXEL_CNT);
+	DUMPREG(RFBI_LINE_NUMBER);
+	DUMPREG(RFBI_CMD);
+	DUMPREG(RFBI_PARAM);
+	DUMPREG(RFBI_DATA);
+	DUMPREG(RFBI_READ);
+	DUMPREG(RFBI_STATUS);
+
+	DUMPREG(RFBI_CONFIG(0));
+	DUMPREG(RFBI_ONOFF_TIME(0));
+	DUMPREG(RFBI_CYCLE_TIME(0));
+	DUMPREG(RFBI_DATA_CYCLE1(0));
+	DUMPREG(RFBI_DATA_CYCLE2(0));
+	DUMPREG(RFBI_DATA_CYCLE3(0));
+
+	DUMPREG(RFBI_CONFIG(1));
+	DUMPREG(RFBI_ONOFF_TIME(1));
+	DUMPREG(RFBI_CYCLE_TIME(1));
+	DUMPREG(RFBI_DATA_CYCLE1(1));
+	DUMPREG(RFBI_DATA_CYCLE2(1));
+	DUMPREG(RFBI_DATA_CYCLE3(1));
+
+	DUMPREG(RFBI_VSYNC_WIDTH);
+	DUMPREG(RFBI_HSYNC_WIDTH);
+
+	rfbi_runtime_put();
+#undef DUMPREG
+}
+
+static void rfbi_config_lcd_manager(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = rfbi.output.manager;
+	struct dss_lcd_mgr_config mgr_config;
+
+	mgr_config.io_pad_mode = DSS_IO_PAD_MODE_RFBI;
+
+	mgr_config.stallmode = true;
+	/* Do we need fifohandcheck for RFBI? */
+	mgr_config.fifohandcheck = false;
+
+	mgr_config.video_port_width = rfbi.pixel_size;
+	mgr_config.lcden_sig_polarity = 0;
+
+	dss_mgr_set_lcd_config(mgr, &mgr_config);
+
+	/*
+	 * Set rfbi.timings with default values, the x_res and y_res fields
+	 * are expected to be already configured by the panel driver via
+	 * omapdss_rfbi_set_size()
+	 */
+	rfbi.timings.hsw = 1;
+	rfbi.timings.hfp = 1;
+	rfbi.timings.hbp = 1;
+	rfbi.timings.vsw = 1;
+	rfbi.timings.vfp = 0;
+	rfbi.timings.vbp = 0;
+
+	rfbi.timings.interlace = false;
+	rfbi.timings.hsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.vsync_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	rfbi.timings.de_level = OMAPDSS_SIG_ACTIVE_HIGH;
+	rfbi.timings.sync_pclk_edge = OMAPDSS_DRIVE_SIG_FALLING_EDGE;
+
+	dss_mgr_set_timings(mgr, &rfbi.timings);
+}
+
+static int rfbi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &rfbi.output;
+	int r;
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		return -ENODEV;
+	}
+
+	r = rfbi_runtime_get();
+	if (r)
+		return r;
+
+	r = dss_mgr_register_framedone_handler(out->manager,
+			framedone_callback, NULL);
+	if (r) {
+		DSSERR("can't get FRAMEDONE irq\n");
+		goto err1;
+	}
+
+	rfbi_config_lcd_manager(dssdev);
+
+	rfbi_configure_bus(dssdev->phy.rfbi.channel, rfbi.pixel_size,
+			rfbi.data_lines);
+
+	rfbi_set_timings(dssdev->phy.rfbi.channel, &rfbi.intf_timings);
+
+	return 0;
+err1:
+	rfbi_runtime_put();
+	return r;
+}
+
+static void rfbi_display_disable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &rfbi.output;
+
+	dss_mgr_unregister_framedone_handler(out->manager,
+			framedone_callback, NULL);
+
+	rfbi_runtime_put();
+}
+
+static int rfbi_init_display(struct omap_dss_device *dssdev)
+{
+	rfbi.dssdev[dssdev->phy.rfbi.channel] = dssdev;
+	return 0;
+}
+
+static void rfbi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &rfbi.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_DBI;
+	out->output_type = OMAP_DISPLAY_TYPE_DBI;
+	out->name = "rfbi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void rfbi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &rfbi.output;
+
+	omapdss_unregister_output(out);
+}
+
+/* RFBI HW IP initialisation */
+static int rfbi_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	u32 rev;
+	struct resource *rfbi_mem;
+	struct clk *clk;
+	int r;
+
+	rfbi.pdev = pdev;
+
+	sema_init(&rfbi.bus_lock, 1);
+
+	rfbi_mem = platform_get_resource(rfbi.pdev, IORESOURCE_MEM, 0);
+	if (!rfbi_mem) {
+		DSSERR("can't get IORESOURCE_MEM RFBI\n");
+		return -EINVAL;
+	}
+
+	rfbi.base = devm_ioremap(&pdev->dev, rfbi_mem->start,
+				 resource_size(rfbi_mem));
+	if (!rfbi.base) {
+		DSSERR("can't ioremap RFBI\n");
+		return -ENOMEM;
+	}
+
+	clk = clk_get(&pdev->dev, "ick");
+	if (IS_ERR(clk)) {
+		DSSERR("can't get ick\n");
+		return PTR_ERR(clk);
+	}
+
+	rfbi.l4_khz = clk_get_rate(clk) / 1000;
+
+	clk_put(clk);
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = rfbi_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	msleep(10);
+
+	rev = rfbi_read_reg(RFBI_REVISION);
+	dev_dbg(&pdev->dev, "OMAP RFBI rev %d.%d\n",
+	       FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+	rfbi_runtime_put();
+
+	dss_debugfs_create_file("rfbi", rfbi_dump_regs);
+
+	rfbi_init_output(pdev);
+
+	return 0;
+
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	return r;
+}
+
+static void rfbi_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	rfbi_uninit_output(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static const struct component_ops rfbi_component_ops = {
+	.bind	= rfbi_bind,
+	.unbind	= rfbi_unbind,
+};
+
+static int rfbi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &rfbi_component_ops);
+}
+
+static int rfbi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &rfbi_component_ops);
+	return 0;
+}
+
+static int rfbi_runtime_suspend(struct device *dev)
+{
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int rfbi_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		return r;
+
+	return 0;
+}
+
+static const struct dev_pm_ops rfbi_pm_ops = {
+	.runtime_suspend = rfbi_runtime_suspend,
+	.runtime_resume = rfbi_runtime_resume,
+};
+
+static struct platform_driver omap_rfbihw_driver = {
+	.probe		= rfbi_probe,
+	.remove         = rfbi_remove,
+	.driver         = {
+		.name   = "omapdss_rfbi",
+		.pm	= &rfbi_pm_ops,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init rfbi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/sdi.c b/drivers/video/fbdev/omap2/omapfb/dss/sdi.c
new file mode 100644
index 000000000000..d747cc6b59e1
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/sdi.c
@@ -0,0 +1,454 @@
+/*
+ * linux/drivers/video/omap2/dss/sdi.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "SDI"
+
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/regulator/consumer.h>
+#include <linux/export.h>
+#include <linux/platform_device.h>
+#include <linux/string.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+#include "dss.h"
+
+static struct {
+	struct platform_device *pdev;
+
+	bool update_enabled;
+	struct regulator *vdds_sdi_reg;
+
+	struct dss_lcd_mgr_config mgr_config;
+	struct omap_video_timings timings;
+	int datapairs;
+
+	struct omap_dss_device output;
+
+	bool port_initialized;
+} sdi;
+
+struct sdi_clk_calc_ctx {
+	unsigned long pck_min, pck_max;
+
+	unsigned long fck;
+	struct dispc_clock_info dispc_cinfo;
+};
+
+static bool dpi_calc_dispc_cb(int lckd, int pckd, unsigned long lck,
+		unsigned long pck, void *data)
+{
+	struct sdi_clk_calc_ctx *ctx = data;
+
+	ctx->dispc_cinfo.lck_div = lckd;
+	ctx->dispc_cinfo.pck_div = pckd;
+	ctx->dispc_cinfo.lck = lck;
+	ctx->dispc_cinfo.pck = pck;
+
+	return true;
+}
+
+static bool dpi_calc_dss_cb(unsigned long fck, void *data)
+{
+	struct sdi_clk_calc_ctx *ctx = data;
+
+	ctx->fck = fck;
+
+	return dispc_div_calc(fck, ctx->pck_min, ctx->pck_max,
+			dpi_calc_dispc_cb, ctx);
+}
+
+static int sdi_calc_clock_div(unsigned long pclk,
+		unsigned long *fck,
+		struct dispc_clock_info *dispc_cinfo)
+{
+	int i;
+	struct sdi_clk_calc_ctx ctx;
+
+	/*
+	 * DSS fclk gives us very few possibilities, so finding a good pixel
+	 * clock may not be possible. We try multiple times to find the clock,
+	 * each time widening the pixel clock range we look for, up to
+	 * +/- 1MHz.
+	 */
+
+	for (i = 0; i < 10; ++i) {
+		bool ok;
+
+		memset(&ctx, 0, sizeof(ctx));
+		if (pclk > 1000 * i * i * i)
+			ctx.pck_min = max(pclk - 1000 * i * i * i, 0lu);
+		else
+			ctx.pck_min = 0;
+		ctx.pck_max = pclk + 1000 * i * i * i;
+
+		ok = dss_div_calc(pclk, ctx.pck_min, dpi_calc_dss_cb, &ctx);
+		if (ok) {
+			*fck = ctx.fck;
+			*dispc_cinfo = ctx.dispc_cinfo;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static void sdi_config_lcd_manager(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = sdi.output.manager;
+
+	sdi.mgr_config.io_pad_mode = DSS_IO_PAD_MODE_BYPASS;
+
+	sdi.mgr_config.stallmode = false;
+	sdi.mgr_config.fifohandcheck = false;
+
+	sdi.mgr_config.video_port_width = 24;
+	sdi.mgr_config.lcden_sig_polarity = 1;
+
+	dss_mgr_set_lcd_config(mgr, &sdi.mgr_config);
+}
+
+static int sdi_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &sdi.output;
+	struct omap_video_timings *t = &sdi.timings;
+	unsigned long fck;
+	struct dispc_clock_info dispc_cinfo;
+	unsigned long pck;
+	int r;
+
+	if (out->manager == NULL) {
+		DSSERR("failed to enable display: no output/manager\n");
+		return -ENODEV;
+	}
+
+	r = regulator_enable(sdi.vdds_sdi_reg);
+	if (r)
+		goto err_reg_enable;
+
+	r = dispc_runtime_get();
+	if (r)
+		goto err_get_dispc;
+
+	/* 15.5.9.1.2 */
+	t->data_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+	t->sync_pclk_edge = OMAPDSS_DRIVE_SIG_RISING_EDGE;
+
+	r = sdi_calc_clock_div(t->pixelclock, &fck, &dispc_cinfo);
+	if (r)
+		goto err_calc_clock_div;
+
+	sdi.mgr_config.clock_info = dispc_cinfo;
+
+	pck = fck / dispc_cinfo.lck_div / dispc_cinfo.pck_div;
+
+	if (pck != t->pixelclock) {
+		DSSWARN("Could not find exact pixel clock. Requested %d Hz, got %lu Hz\n",
+			t->pixelclock, pck);
+
+		t->pixelclock = pck;
+	}
+
+
+	dss_mgr_set_timings(out->manager, t);
+
+	r = dss_set_fck_rate(fck);
+	if (r)
+		goto err_set_dss_clock_div;
+
+	sdi_config_lcd_manager(dssdev);
+
+	/*
+	 * LCLK and PCLK divisors are located in shadow registers, and we
+	 * normally write them to DISPC registers when enabling the output.
+	 * However, SDI uses pck-free as source clock for its PLL, and pck-free
+	 * is affected by the divisors. And as we need the PLL before enabling
+	 * the output, we need to write the divisors early.
+	 *
+	 * It seems just writing to the DISPC register is enough, and we don't
+	 * need to care about the shadow register mechanism for pck-free. The
+	 * exact reason for this is unknown.
+	 */
+	dispc_mgr_set_clock_div(out->manager->id, &sdi.mgr_config.clock_info);
+
+	dss_sdi_init(sdi.datapairs);
+	r = dss_sdi_enable();
+	if (r)
+		goto err_sdi_enable;
+	mdelay(2);
+
+	r = dss_mgr_enable(out->manager);
+	if (r)
+		goto err_mgr_enable;
+
+	return 0;
+
+err_mgr_enable:
+	dss_sdi_disable();
+err_sdi_enable:
+err_set_dss_clock_div:
+err_calc_clock_div:
+	dispc_runtime_put();
+err_get_dispc:
+	regulator_disable(sdi.vdds_sdi_reg);
+err_reg_enable:
+	return r;
+}
+
+static void sdi_display_disable(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = sdi.output.manager;
+
+	dss_mgr_disable(mgr);
+
+	dss_sdi_disable();
+
+	dispc_runtime_put();
+
+	regulator_disable(sdi.vdds_sdi_reg);
+}
+
+static void sdi_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	sdi.timings = *timings;
+}
+
+static void sdi_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	*timings = sdi.timings;
+}
+
+static int sdi_check_timings(struct omap_dss_device *dssdev,
+			struct omap_video_timings *timings)
+{
+	struct omap_overlay_manager *mgr = sdi.output.manager;
+
+	if (mgr && !dispc_mgr_timings_ok(mgr->id, timings))
+		return -EINVAL;
+
+	if (timings->pixelclock == 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static void sdi_set_datapairs(struct omap_dss_device *dssdev, int datapairs)
+{
+	sdi.datapairs = datapairs;
+}
+
+static int sdi_init_regulator(void)
+{
+	struct regulator *vdds_sdi;
+
+	if (sdi.vdds_sdi_reg)
+		return 0;
+
+	vdds_sdi = devm_regulator_get(&sdi.pdev->dev, "vdds_sdi");
+	if (IS_ERR(vdds_sdi)) {
+		if (PTR_ERR(vdds_sdi) != -EPROBE_DEFER)
+			DSSERR("can't get VDDS_SDI regulator\n");
+		return PTR_ERR(vdds_sdi);
+	}
+
+	sdi.vdds_sdi_reg = vdds_sdi;
+
+	return 0;
+}
+
+static int sdi_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = sdi_init_regulator();
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void sdi_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_sdi_ops sdi_ops = {
+	.connect = sdi_connect,
+	.disconnect = sdi_disconnect,
+
+	.enable = sdi_display_enable,
+	.disable = sdi_display_disable,
+
+	.check_timings = sdi_check_timings,
+	.set_timings = sdi_set_timings,
+	.get_timings = sdi_get_timings,
+
+	.set_datapairs = sdi_set_datapairs,
+};
+
+static void sdi_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &sdi.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_SDI;
+	out->output_type = OMAP_DISPLAY_TYPE_SDI;
+	out->name = "sdi.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_LCD;
+	/* We have SDI only on OMAP3, where it's on port 1 */
+	out->port_num = 1;
+	out->ops.sdi = &sdi_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void sdi_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &sdi.output;
+
+	omapdss_unregister_output(out);
+}
+
+static int sdi_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	sdi.pdev = pdev;
+
+	sdi_init_output(pdev);
+
+	return 0;
+}
+
+static void sdi_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	sdi_uninit_output(pdev);
+}
+
+static const struct component_ops sdi_component_ops = {
+	.bind	= sdi_bind,
+	.unbind	= sdi_unbind,
+};
+
+static int sdi_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &sdi_component_ops);
+}
+
+static int sdi_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &sdi_component_ops);
+	return 0;
+}
+
+static struct platform_driver omap_sdi_driver = {
+	.probe		= sdi_probe,
+	.remove         = sdi_remove,
+	.driver         = {
+		.name   = "omapdss_sdi",
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init sdi_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_sdi_driver);
+}
+
+void sdi_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_sdi_driver);
+}
+
+int sdi_init_port(struct platform_device *pdev, struct device_node *port)
+{
+	struct device_node *ep;
+	u32 datapairs;
+	int r;
+
+	ep = omapdss_of_get_next_endpoint(port, NULL);
+	if (!ep)
+		return 0;
+
+	r = of_property_read_u32(ep, "datapairs", &datapairs);
+	if (r) {
+		DSSERR("failed to parse datapairs\n");
+		goto err_datapairs;
+	}
+
+	sdi.datapairs = datapairs;
+
+	of_node_put(ep);
+
+	sdi.pdev = pdev;
+
+	sdi_init_output(pdev);
+
+	sdi.port_initialized = true;
+
+	return 0;
+
+err_datapairs:
+	of_node_put(ep);
+
+	return r;
+}
+
+void sdi_uninit_port(struct device_node *port)
+{
+	if (!sdi.port_initialized)
+		return;
+
+	sdi_uninit_output(sdi.pdev);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/venc.c b/drivers/video/fbdev/omap2/omapfb/dss/venc.c
new file mode 100644
index 000000000000..26e0ee30adf8
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/venc.c
@@ -0,0 +1,997 @@
+/*
+ * linux/drivers/video/omap2/dss/venc.c
+ *
+ * Copyright (C) 2009 Nokia Corporation
+ * Author: Tomi Valkeinen <tomi.valkeinen@nokia.com>
+ *
+ * VENC settings from TI's DSS 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.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "VENC"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/completion.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <linux/seq_file.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/pm_runtime.h>
+#include <linux/of.h>
+#include <linux/component.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+/* Venc registers */
+#define VENC_REV_ID				0x00
+#define VENC_STATUS				0x04
+#define VENC_F_CONTROL				0x08
+#define VENC_VIDOUT_CTRL			0x10
+#define VENC_SYNC_CTRL				0x14
+#define VENC_LLEN				0x1C
+#define VENC_FLENS				0x20
+#define VENC_HFLTR_CTRL				0x24
+#define VENC_CC_CARR_WSS_CARR			0x28
+#define VENC_C_PHASE				0x2C
+#define VENC_GAIN_U				0x30
+#define VENC_GAIN_V				0x34
+#define VENC_GAIN_Y				0x38
+#define VENC_BLACK_LEVEL			0x3C
+#define VENC_BLANK_LEVEL			0x40
+#define VENC_X_COLOR				0x44
+#define VENC_M_CONTROL				0x48
+#define VENC_BSTAMP_WSS_DATA			0x4C
+#define VENC_S_CARR				0x50
+#define VENC_LINE21				0x54
+#define VENC_LN_SEL				0x58
+#define VENC_L21__WC_CTL			0x5C
+#define VENC_HTRIGGER_VTRIGGER			0x60
+#define VENC_SAVID__EAVID			0x64
+#define VENC_FLEN__FAL				0x68
+#define VENC_LAL__PHASE_RESET			0x6C
+#define VENC_HS_INT_START_STOP_X		0x70
+#define VENC_HS_EXT_START_STOP_X		0x74
+#define VENC_VS_INT_START_X			0x78
+#define VENC_VS_INT_STOP_X__VS_INT_START_Y	0x7C
+#define VENC_VS_INT_STOP_Y__VS_EXT_START_X	0x80
+#define VENC_VS_EXT_STOP_X__VS_EXT_START_Y	0x84
+#define VENC_VS_EXT_STOP_Y			0x88
+#define VENC_AVID_START_STOP_X			0x90
+#define VENC_AVID_START_STOP_Y			0x94
+#define VENC_FID_INT_START_X__FID_INT_START_Y	0xA0
+#define VENC_FID_INT_OFFSET_Y__FID_EXT_START_X	0xA4
+#define VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y	0xA8
+#define VENC_TVDETGP_INT_START_STOP_X		0xB0
+#define VENC_TVDETGP_INT_START_STOP_Y		0xB4
+#define VENC_GEN_CTRL				0xB8
+#define VENC_OUTPUT_CONTROL			0xC4
+#define VENC_OUTPUT_TEST			0xC8
+#define VENC_DAC_B__DAC_C			0xC8
+
+struct venc_config {
+	u32 f_control;
+	u32 vidout_ctrl;
+	u32 sync_ctrl;
+	u32 llen;
+	u32 flens;
+	u32 hfltr_ctrl;
+	u32 cc_carr_wss_carr;
+	u32 c_phase;
+	u32 gain_u;
+	u32 gain_v;
+	u32 gain_y;
+	u32 black_level;
+	u32 blank_level;
+	u32 x_color;
+	u32 m_control;
+	u32 bstamp_wss_data;
+	u32 s_carr;
+	u32 line21;
+	u32 ln_sel;
+	u32 l21__wc_ctl;
+	u32 htrigger_vtrigger;
+	u32 savid__eavid;
+	u32 flen__fal;
+	u32 lal__phase_reset;
+	u32 hs_int_start_stop_x;
+	u32 hs_ext_start_stop_x;
+	u32 vs_int_start_x;
+	u32 vs_int_stop_x__vs_int_start_y;
+	u32 vs_int_stop_y__vs_ext_start_x;
+	u32 vs_ext_stop_x__vs_ext_start_y;
+	u32 vs_ext_stop_y;
+	u32 avid_start_stop_x;
+	u32 avid_start_stop_y;
+	u32 fid_int_start_x__fid_int_start_y;
+	u32 fid_int_offset_y__fid_ext_start_x;
+	u32 fid_ext_start_y__fid_ext_offset_y;
+	u32 tvdetgp_int_start_stop_x;
+	u32 tvdetgp_int_start_stop_y;
+	u32 gen_ctrl;
+};
+
+/* from TRM */
+static const struct venc_config venc_config_pal_trm = {
+	.f_control				= 0,
+	.vidout_ctrl				= 1,
+	.sync_ctrl				= 0x40,
+	.llen					= 0x35F, /* 863 */
+	.flens					= 0x270, /* 624 */
+	.hfltr_ctrl				= 0,
+	.cc_carr_wss_carr			= 0x2F7225ED,
+	.c_phase				= 0,
+	.gain_u					= 0x111,
+	.gain_v					= 0x181,
+	.gain_y					= 0x140,
+	.black_level				= 0x3B,
+	.blank_level				= 0x3B,
+	.x_color				= 0x7,
+	.m_control				= 0x2,
+	.bstamp_wss_data			= 0x3F,
+	.s_carr					= 0x2A098ACB,
+	.line21					= 0,
+	.ln_sel					= 0x01290015,
+	.l21__wc_ctl				= 0x0000F603,
+	.htrigger_vtrigger			= 0,
+
+	.savid__eavid				= 0x06A70108,
+	.flen__fal				= 0x00180270,
+	.lal__phase_reset			= 0x00040135,
+	.hs_int_start_stop_x			= 0x00880358,
+	.hs_ext_start_stop_x			= 0x000F035F,
+	.vs_int_start_x				= 0x01A70000,
+	.vs_int_stop_x__vs_int_start_y		= 0x000001A7,
+	.vs_int_stop_y__vs_ext_start_x		= 0x01AF0000,
+	.vs_ext_stop_x__vs_ext_start_y		= 0x000101AF,
+	.vs_ext_stop_y				= 0x00000025,
+	.avid_start_stop_x			= 0x03530083,
+	.avid_start_stop_y			= 0x026C002E,
+	.fid_int_start_x__fid_int_start_y	= 0x0001008A,
+	.fid_int_offset_y__fid_ext_start_x	= 0x002E0138,
+	.fid_ext_start_y__fid_ext_offset_y	= 0x01380001,
+
+	.tvdetgp_int_start_stop_x		= 0x00140001,
+	.tvdetgp_int_start_stop_y		= 0x00010001,
+	.gen_ctrl				= 0x00FF0000,
+};
+
+/* from TRM */
+static const struct venc_config venc_config_ntsc_trm = {
+	.f_control				= 0,
+	.vidout_ctrl				= 1,
+	.sync_ctrl				= 0x8040,
+	.llen					= 0x359,
+	.flens					= 0x20C,
+	.hfltr_ctrl				= 0,
+	.cc_carr_wss_carr			= 0x043F2631,
+	.c_phase				= 0,
+	.gain_u					= 0x102,
+	.gain_v					= 0x16C,
+	.gain_y					= 0x12F,
+	.black_level				= 0x43,
+	.blank_level				= 0x38,
+	.x_color				= 0x7,
+	.m_control				= 0x1,
+	.bstamp_wss_data			= 0x38,
+	.s_carr					= 0x21F07C1F,
+	.line21					= 0,
+	.ln_sel					= 0x01310011,
+	.l21__wc_ctl				= 0x0000F003,
+	.htrigger_vtrigger			= 0,
+
+	.savid__eavid				= 0x069300F4,
+	.flen__fal				= 0x0016020C,
+	.lal__phase_reset			= 0x00060107,
+	.hs_int_start_stop_x			= 0x008E0350,
+	.hs_ext_start_stop_x			= 0x000F0359,
+	.vs_int_start_x				= 0x01A00000,
+	.vs_int_stop_x__vs_int_start_y		= 0x020701A0,
+	.vs_int_stop_y__vs_ext_start_x		= 0x01AC0024,
+	.vs_ext_stop_x__vs_ext_start_y		= 0x020D01AC,
+	.vs_ext_stop_y				= 0x00000006,
+	.avid_start_stop_x			= 0x03480078,
+	.avid_start_stop_y			= 0x02060024,
+	.fid_int_start_x__fid_int_start_y	= 0x0001008A,
+	.fid_int_offset_y__fid_ext_start_x	= 0x01AC0106,
+	.fid_ext_start_y__fid_ext_offset_y	= 0x01060006,
+
+	.tvdetgp_int_start_stop_x		= 0x00140001,
+	.tvdetgp_int_start_stop_y		= 0x00010001,
+	.gen_ctrl				= 0x00F90000,
+};
+
+static const struct venc_config venc_config_pal_bdghi = {
+	.f_control				= 0,
+	.vidout_ctrl				= 0,
+	.sync_ctrl				= 0,
+	.hfltr_ctrl				= 0,
+	.x_color				= 0,
+	.line21					= 0,
+	.ln_sel					= 21,
+	.htrigger_vtrigger			= 0,
+	.tvdetgp_int_start_stop_x		= 0x00140001,
+	.tvdetgp_int_start_stop_y		= 0x00010001,
+	.gen_ctrl				= 0x00FB0000,
+
+	.llen					= 864-1,
+	.flens					= 625-1,
+	.cc_carr_wss_carr			= 0x2F7625ED,
+	.c_phase				= 0xDF,
+	.gain_u					= 0x111,
+	.gain_v					= 0x181,
+	.gain_y					= 0x140,
+	.black_level				= 0x3e,
+	.blank_level				= 0x3e,
+	.m_control				= 0<<2 | 1<<1,
+	.bstamp_wss_data			= 0x42,
+	.s_carr					= 0x2a098acb,
+	.l21__wc_ctl				= 0<<13 | 0x16<<8 | 0<<0,
+	.savid__eavid				= 0x06A70108,
+	.flen__fal				= 23<<16 | 624<<0,
+	.lal__phase_reset			= 2<<17 | 310<<0,
+	.hs_int_start_stop_x			= 0x00920358,
+	.hs_ext_start_stop_x			= 0x000F035F,
+	.vs_int_start_x				= 0x1a7<<16,
+	.vs_int_stop_x__vs_int_start_y		= 0x000601A7,
+	.vs_int_stop_y__vs_ext_start_x		= 0x01AF0036,
+	.vs_ext_stop_x__vs_ext_start_y		= 0x27101af,
+	.vs_ext_stop_y				= 0x05,
+	.avid_start_stop_x			= 0x03530082,
+	.avid_start_stop_y			= 0x0270002E,
+	.fid_int_start_x__fid_int_start_y	= 0x0005008A,
+	.fid_int_offset_y__fid_ext_start_x	= 0x002E0138,
+	.fid_ext_start_y__fid_ext_offset_y	= 0x01380005,
+};
+
+const struct omap_video_timings omap_dss_pal_timings = {
+	.x_res		= 720,
+	.y_res		= 574,
+	.pixelclock	= 13500000,
+	.hsw		= 64,
+	.hfp		= 12,
+	.hbp		= 68,
+	.vsw		= 5,
+	.vfp		= 5,
+	.vbp		= 41,
+
+	.interlace	= true,
+};
+EXPORT_SYMBOL(omap_dss_pal_timings);
+
+const struct omap_video_timings omap_dss_ntsc_timings = {
+	.x_res		= 720,
+	.y_res		= 482,
+	.pixelclock	= 13500000,
+	.hsw		= 64,
+	.hfp		= 16,
+	.hbp		= 58,
+	.vsw		= 6,
+	.vfp		= 6,
+	.vbp		= 31,
+
+	.interlace	= true,
+};
+EXPORT_SYMBOL(omap_dss_ntsc_timings);
+
+static struct {
+	struct platform_device *pdev;
+	void __iomem *base;
+	struct mutex venc_lock;
+	u32 wss_data;
+	struct regulator *vdda_dac_reg;
+
+	struct clk	*tv_dac_clk;
+
+	struct omap_video_timings timings;
+	enum omap_dss_venc_type type;
+	bool invert_polarity;
+
+	struct omap_dss_device output;
+} venc;
+
+static inline void venc_write_reg(int idx, u32 val)
+{
+	__raw_writel(val, venc.base + idx);
+}
+
+static inline u32 venc_read_reg(int idx)
+{
+	u32 l = __raw_readl(venc.base + idx);
+	return l;
+}
+
+static void venc_write_config(const struct venc_config *config)
+{
+	DSSDBG("write venc conf\n");
+
+	venc_write_reg(VENC_LLEN, config->llen);
+	venc_write_reg(VENC_FLENS, config->flens);
+	venc_write_reg(VENC_CC_CARR_WSS_CARR, config->cc_carr_wss_carr);
+	venc_write_reg(VENC_C_PHASE, config->c_phase);
+	venc_write_reg(VENC_GAIN_U, config->gain_u);
+	venc_write_reg(VENC_GAIN_V, config->gain_v);
+	venc_write_reg(VENC_GAIN_Y, config->gain_y);
+	venc_write_reg(VENC_BLACK_LEVEL, config->black_level);
+	venc_write_reg(VENC_BLANK_LEVEL, config->blank_level);
+	venc_write_reg(VENC_M_CONTROL, config->m_control);
+	venc_write_reg(VENC_BSTAMP_WSS_DATA, config->bstamp_wss_data |
+			venc.wss_data);
+	venc_write_reg(VENC_S_CARR, config->s_carr);
+	venc_write_reg(VENC_L21__WC_CTL, config->l21__wc_ctl);
+	venc_write_reg(VENC_SAVID__EAVID, config->savid__eavid);
+	venc_write_reg(VENC_FLEN__FAL, config->flen__fal);
+	venc_write_reg(VENC_LAL__PHASE_RESET, config->lal__phase_reset);
+	venc_write_reg(VENC_HS_INT_START_STOP_X, config->hs_int_start_stop_x);
+	venc_write_reg(VENC_HS_EXT_START_STOP_X, config->hs_ext_start_stop_x);
+	venc_write_reg(VENC_VS_INT_START_X, config->vs_int_start_x);
+	venc_write_reg(VENC_VS_INT_STOP_X__VS_INT_START_Y,
+		       config->vs_int_stop_x__vs_int_start_y);
+	venc_write_reg(VENC_VS_INT_STOP_Y__VS_EXT_START_X,
+		       config->vs_int_stop_y__vs_ext_start_x);
+	venc_write_reg(VENC_VS_EXT_STOP_X__VS_EXT_START_Y,
+		       config->vs_ext_stop_x__vs_ext_start_y);
+	venc_write_reg(VENC_VS_EXT_STOP_Y, config->vs_ext_stop_y);
+	venc_write_reg(VENC_AVID_START_STOP_X, config->avid_start_stop_x);
+	venc_write_reg(VENC_AVID_START_STOP_Y, config->avid_start_stop_y);
+	venc_write_reg(VENC_FID_INT_START_X__FID_INT_START_Y,
+		       config->fid_int_start_x__fid_int_start_y);
+	venc_write_reg(VENC_FID_INT_OFFSET_Y__FID_EXT_START_X,
+		       config->fid_int_offset_y__fid_ext_start_x);
+	venc_write_reg(VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y,
+		       config->fid_ext_start_y__fid_ext_offset_y);
+
+	venc_write_reg(VENC_DAC_B__DAC_C,  venc_read_reg(VENC_DAC_B__DAC_C));
+	venc_write_reg(VENC_VIDOUT_CTRL, config->vidout_ctrl);
+	venc_write_reg(VENC_HFLTR_CTRL, config->hfltr_ctrl);
+	venc_write_reg(VENC_X_COLOR, config->x_color);
+	venc_write_reg(VENC_LINE21, config->line21);
+	venc_write_reg(VENC_LN_SEL, config->ln_sel);
+	venc_write_reg(VENC_HTRIGGER_VTRIGGER, config->htrigger_vtrigger);
+	venc_write_reg(VENC_TVDETGP_INT_START_STOP_X,
+		       config->tvdetgp_int_start_stop_x);
+	venc_write_reg(VENC_TVDETGP_INT_START_STOP_Y,
+		       config->tvdetgp_int_start_stop_y);
+	venc_write_reg(VENC_GEN_CTRL, config->gen_ctrl);
+	venc_write_reg(VENC_F_CONTROL, config->f_control);
+	venc_write_reg(VENC_SYNC_CTRL, config->sync_ctrl);
+}
+
+static void venc_reset(void)
+{
+	int t = 1000;
+
+	venc_write_reg(VENC_F_CONTROL, 1<<8);
+	while (venc_read_reg(VENC_F_CONTROL) & (1<<8)) {
+		if (--t == 0) {
+			DSSERR("Failed to reset venc\n");
+			return;
+		}
+	}
+
+#ifdef CONFIG_FB_OMAP2_DSS_SLEEP_AFTER_VENC_RESET
+	/* the magical sleep that makes things work */
+	/* XXX more info? What bug this circumvents? */
+	msleep(20);
+#endif
+}
+
+static int venc_runtime_get(void)
+{
+	int r;
+
+	DSSDBG("venc_runtime_get\n");
+
+	r = pm_runtime_get_sync(&venc.pdev->dev);
+	WARN_ON(r < 0);
+	return r < 0 ? r : 0;
+}
+
+static void venc_runtime_put(void)
+{
+	int r;
+
+	DSSDBG("venc_runtime_put\n");
+
+	r = pm_runtime_put_sync(&venc.pdev->dev);
+	WARN_ON(r < 0 && r != -ENOSYS);
+}
+
+static const struct venc_config *venc_timings_to_config(
+		struct omap_video_timings *timings)
+{
+	if (memcmp(&omap_dss_pal_timings, timings, sizeof(*timings)) == 0)
+		return &venc_config_pal_trm;
+
+	if (memcmp(&omap_dss_ntsc_timings, timings, sizeof(*timings)) == 0)
+		return &venc_config_ntsc_trm;
+
+	BUG();
+	return NULL;
+}
+
+static int venc_power_on(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = venc.output.manager;
+	u32 l;
+	int r;
+
+	r = venc_runtime_get();
+	if (r)
+		goto err0;
+
+	venc_reset();
+	venc_write_config(venc_timings_to_config(&venc.timings));
+
+	dss_set_venc_output(venc.type);
+	dss_set_dac_pwrdn_bgz(1);
+
+	l = 0;
+
+	if (venc.type == OMAP_DSS_VENC_TYPE_COMPOSITE)
+		l |= 1 << 1;
+	else /* S-Video */
+		l |= (1 << 0) | (1 << 2);
+
+	if (venc.invert_polarity == false)
+		l |= 1 << 3;
+
+	venc_write_reg(VENC_OUTPUT_CONTROL, l);
+
+	dss_mgr_set_timings(mgr, &venc.timings);
+
+	r = regulator_enable(venc.vdda_dac_reg);
+	if (r)
+		goto err1;
+
+	r = dss_mgr_enable(mgr);
+	if (r)
+		goto err2;
+
+	return 0;
+
+err2:
+	regulator_disable(venc.vdda_dac_reg);
+err1:
+	venc_write_reg(VENC_OUTPUT_CONTROL, 0);
+	dss_set_dac_pwrdn_bgz(0);
+
+	venc_runtime_put();
+err0:
+	return r;
+}
+
+static void venc_power_off(struct omap_dss_device *dssdev)
+{
+	struct omap_overlay_manager *mgr = venc.output.manager;
+
+	venc_write_reg(VENC_OUTPUT_CONTROL, 0);
+	dss_set_dac_pwrdn_bgz(0);
+
+	dss_mgr_disable(mgr);
+
+	regulator_disable(venc.vdda_dac_reg);
+
+	venc_runtime_put();
+}
+
+static int venc_display_enable(struct omap_dss_device *dssdev)
+{
+	struct omap_dss_device *out = &venc.output;
+	int r;
+
+	DSSDBG("venc_display_enable\n");
+
+	mutex_lock(&venc.venc_lock);
+
+	if (out->manager == NULL) {
+		DSSERR("Failed to enable display: no output/manager\n");
+		r = -ENODEV;
+		goto err0;
+	}
+
+	r = venc_power_on(dssdev);
+	if (r)
+		goto err0;
+
+	venc.wss_data = 0;
+
+	mutex_unlock(&venc.venc_lock);
+
+	return 0;
+err0:
+	mutex_unlock(&venc.venc_lock);
+	return r;
+}
+
+static void venc_display_disable(struct omap_dss_device *dssdev)
+{
+	DSSDBG("venc_display_disable\n");
+
+	mutex_lock(&venc.venc_lock);
+
+	venc_power_off(dssdev);
+
+	mutex_unlock(&venc.venc_lock);
+}
+
+static void venc_set_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	DSSDBG("venc_set_timings\n");
+
+	mutex_lock(&venc.venc_lock);
+
+	/* Reset WSS data when the TV standard changes. */
+	if (memcmp(&venc.timings, timings, sizeof(*timings)))
+		venc.wss_data = 0;
+
+	venc.timings = *timings;
+
+	dispc_set_tv_pclk(13500000);
+
+	mutex_unlock(&venc.venc_lock);
+}
+
+static int venc_check_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	DSSDBG("venc_check_timings\n");
+
+	if (memcmp(&omap_dss_pal_timings, timings, sizeof(*timings)) == 0)
+		return 0;
+
+	if (memcmp(&omap_dss_ntsc_timings, timings, sizeof(*timings)) == 0)
+		return 0;
+
+	return -EINVAL;
+}
+
+static void venc_get_timings(struct omap_dss_device *dssdev,
+		struct omap_video_timings *timings)
+{
+	mutex_lock(&venc.venc_lock);
+
+	*timings = venc.timings;
+
+	mutex_unlock(&venc.venc_lock);
+}
+
+static u32 venc_get_wss(struct omap_dss_device *dssdev)
+{
+	/* Invert due to VENC_L21_WC_CTL:INV=1 */
+	return (venc.wss_data >> 8) ^ 0xfffff;
+}
+
+static int venc_set_wss(struct omap_dss_device *dssdev, u32 wss)
+{
+	const struct venc_config *config;
+	int r;
+
+	DSSDBG("venc_set_wss\n");
+
+	mutex_lock(&venc.venc_lock);
+
+	config = venc_timings_to_config(&venc.timings);
+
+	/* Invert due to VENC_L21_WC_CTL:INV=1 */
+	venc.wss_data = (wss ^ 0xfffff) << 8;
+
+	r = venc_runtime_get();
+	if (r)
+		goto err;
+
+	venc_write_reg(VENC_BSTAMP_WSS_DATA, config->bstamp_wss_data |
+			venc.wss_data);
+
+	venc_runtime_put();
+
+err:
+	mutex_unlock(&venc.venc_lock);
+
+	return r;
+}
+
+static void venc_set_type(struct omap_dss_device *dssdev,
+		enum omap_dss_venc_type type)
+{
+	mutex_lock(&venc.venc_lock);
+
+	venc.type = type;
+
+	mutex_unlock(&venc.venc_lock);
+}
+
+static void venc_invert_vid_out_polarity(struct omap_dss_device *dssdev,
+		bool invert_polarity)
+{
+	mutex_lock(&venc.venc_lock);
+
+	venc.invert_polarity = invert_polarity;
+
+	mutex_unlock(&venc.venc_lock);
+}
+
+static int venc_init_regulator(void)
+{
+	struct regulator *vdda_dac;
+
+	if (venc.vdda_dac_reg != NULL)
+		return 0;
+
+	if (venc.pdev->dev.of_node)
+		vdda_dac = devm_regulator_get(&venc.pdev->dev, "vdda");
+	else
+		vdda_dac = devm_regulator_get(&venc.pdev->dev, "vdda_dac");
+
+	if (IS_ERR(vdda_dac)) {
+		if (PTR_ERR(vdda_dac) != -EPROBE_DEFER)
+			DSSERR("can't get VDDA_DAC regulator\n");
+		return PTR_ERR(vdda_dac);
+	}
+
+	venc.vdda_dac_reg = vdda_dac;
+
+	return 0;
+}
+
+static void venc_dump_regs(struct seq_file *s)
+{
+#define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, venc_read_reg(r))
+
+	if (venc_runtime_get())
+		return;
+
+	DUMPREG(VENC_F_CONTROL);
+	DUMPREG(VENC_VIDOUT_CTRL);
+	DUMPREG(VENC_SYNC_CTRL);
+	DUMPREG(VENC_LLEN);
+	DUMPREG(VENC_FLENS);
+	DUMPREG(VENC_HFLTR_CTRL);
+	DUMPREG(VENC_CC_CARR_WSS_CARR);
+	DUMPREG(VENC_C_PHASE);
+	DUMPREG(VENC_GAIN_U);
+	DUMPREG(VENC_GAIN_V);
+	DUMPREG(VENC_GAIN_Y);
+	DUMPREG(VENC_BLACK_LEVEL);
+	DUMPREG(VENC_BLANK_LEVEL);
+	DUMPREG(VENC_X_COLOR);
+	DUMPREG(VENC_M_CONTROL);
+	DUMPREG(VENC_BSTAMP_WSS_DATA);
+	DUMPREG(VENC_S_CARR);
+	DUMPREG(VENC_LINE21);
+	DUMPREG(VENC_LN_SEL);
+	DUMPREG(VENC_L21__WC_CTL);
+	DUMPREG(VENC_HTRIGGER_VTRIGGER);
+	DUMPREG(VENC_SAVID__EAVID);
+	DUMPREG(VENC_FLEN__FAL);
+	DUMPREG(VENC_LAL__PHASE_RESET);
+	DUMPREG(VENC_HS_INT_START_STOP_X);
+	DUMPREG(VENC_HS_EXT_START_STOP_X);
+	DUMPREG(VENC_VS_INT_START_X);
+	DUMPREG(VENC_VS_INT_STOP_X__VS_INT_START_Y);
+	DUMPREG(VENC_VS_INT_STOP_Y__VS_EXT_START_X);
+	DUMPREG(VENC_VS_EXT_STOP_X__VS_EXT_START_Y);
+	DUMPREG(VENC_VS_EXT_STOP_Y);
+	DUMPREG(VENC_AVID_START_STOP_X);
+	DUMPREG(VENC_AVID_START_STOP_Y);
+	DUMPREG(VENC_FID_INT_START_X__FID_INT_START_Y);
+	DUMPREG(VENC_FID_INT_OFFSET_Y__FID_EXT_START_X);
+	DUMPREG(VENC_FID_EXT_START_Y__FID_EXT_OFFSET_Y);
+	DUMPREG(VENC_TVDETGP_INT_START_STOP_X);
+	DUMPREG(VENC_TVDETGP_INT_START_STOP_Y);
+	DUMPREG(VENC_GEN_CTRL);
+	DUMPREG(VENC_OUTPUT_CONTROL);
+	DUMPREG(VENC_OUTPUT_TEST);
+
+	venc_runtime_put();
+
+#undef DUMPREG
+}
+
+static int venc_get_clocks(struct platform_device *pdev)
+{
+	struct clk *clk;
+
+	if (dss_has_feature(FEAT_VENC_REQUIRES_TV_DAC_CLK)) {
+		clk = devm_clk_get(&pdev->dev, "tv_dac_clk");
+		if (IS_ERR(clk)) {
+			DSSERR("can't get tv_dac_clk\n");
+			return PTR_ERR(clk);
+		}
+	} else {
+		clk = NULL;
+	}
+
+	venc.tv_dac_clk = clk;
+
+	return 0;
+}
+
+static int venc_connect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	struct omap_overlay_manager *mgr;
+	int r;
+
+	r = venc_init_regulator();
+	if (r)
+		return r;
+
+	mgr = omap_dss_get_overlay_manager(dssdev->dispc_channel);
+	if (!mgr)
+		return -ENODEV;
+
+	r = dss_mgr_connect(mgr, dssdev);
+	if (r)
+		return r;
+
+	r = omapdss_output_set_device(dssdev, dst);
+	if (r) {
+		DSSERR("failed to connect output to new device: %s\n",
+				dst->name);
+		dss_mgr_disconnect(mgr, dssdev);
+		return r;
+	}
+
+	return 0;
+}
+
+static void venc_disconnect(struct omap_dss_device *dssdev,
+		struct omap_dss_device *dst)
+{
+	WARN_ON(dst != dssdev->dst);
+
+	if (dst != dssdev->dst)
+		return;
+
+	omapdss_output_unset_device(dssdev);
+
+	if (dssdev->manager)
+		dss_mgr_disconnect(dssdev->manager, dssdev);
+}
+
+static const struct omapdss_atv_ops venc_ops = {
+	.connect = venc_connect,
+	.disconnect = venc_disconnect,
+
+	.enable = venc_display_enable,
+	.disable = venc_display_disable,
+
+	.check_timings = venc_check_timings,
+	.set_timings = venc_set_timings,
+	.get_timings = venc_get_timings,
+
+	.set_type = venc_set_type,
+	.invert_vid_out_polarity = venc_invert_vid_out_polarity,
+
+	.set_wss = venc_set_wss,
+	.get_wss = venc_get_wss,
+};
+
+static void venc_init_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &venc.output;
+
+	out->dev = &pdev->dev;
+	out->id = OMAP_DSS_OUTPUT_VENC;
+	out->output_type = OMAP_DISPLAY_TYPE_VENC;
+	out->name = "venc.0";
+	out->dispc_channel = OMAP_DSS_CHANNEL_DIGIT;
+	out->ops.atv = &venc_ops;
+	out->owner = THIS_MODULE;
+
+	omapdss_register_output(out);
+}
+
+static void venc_uninit_output(struct platform_device *pdev)
+{
+	struct omap_dss_device *out = &venc.output;
+
+	omapdss_unregister_output(out);
+}
+
+static int venc_probe_of(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *ep;
+	u32 channels;
+	int r;
+
+	ep = omapdss_of_get_first_endpoint(node);
+	if (!ep)
+		return 0;
+
+	venc.invert_polarity = of_property_read_bool(ep, "ti,invert-polarity");
+
+	r = of_property_read_u32(ep, "ti,channels", &channels);
+	if (r) {
+		dev_err(&pdev->dev,
+			"failed to read property 'ti,channels': %d\n", r);
+		goto err;
+	}
+
+	switch (channels) {
+	case 1:
+		venc.type = OMAP_DSS_VENC_TYPE_COMPOSITE;
+		break;
+	case 2:
+		venc.type = OMAP_DSS_VENC_TYPE_SVIDEO;
+		break;
+	default:
+		dev_err(&pdev->dev, "bad channel propert '%d'\n", channels);
+		r = -EINVAL;
+		goto err;
+	}
+
+	of_node_put(ep);
+
+	return 0;
+err:
+	of_node_put(ep);
+
+	return 0;
+}
+
+/* VENC HW IP initialisation */
+static int venc_bind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	u8 rev_id;
+	struct resource *venc_mem;
+	int r;
+
+	venc.pdev = pdev;
+
+	mutex_init(&venc.venc_lock);
+
+	venc.wss_data = 0;
+
+	venc_mem = platform_get_resource(venc.pdev, IORESOURCE_MEM, 0);
+	if (!venc_mem) {
+		DSSERR("can't get IORESOURCE_MEM VENC\n");
+		return -EINVAL;
+	}
+
+	venc.base = devm_ioremap(&pdev->dev, venc_mem->start,
+				 resource_size(venc_mem));
+	if (!venc.base) {
+		DSSERR("can't ioremap VENC\n");
+		return -ENOMEM;
+	}
+
+	r = venc_get_clocks(pdev);
+	if (r)
+		return r;
+
+	pm_runtime_enable(&pdev->dev);
+
+	r = venc_runtime_get();
+	if (r)
+		goto err_runtime_get;
+
+	rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+	dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+	venc_runtime_put();
+
+	if (pdev->dev.of_node) {
+		r = venc_probe_of(pdev);
+		if (r) {
+			DSSERR("Invalid DT data\n");
+			goto err_probe_of;
+		}
+	}
+
+	dss_debugfs_create_file("venc", venc_dump_regs);
+
+	venc_init_output(pdev);
+
+	return 0;
+
+err_probe_of:
+err_runtime_get:
+	pm_runtime_disable(&pdev->dev);
+	return r;
+}
+
+static void venc_unbind(struct device *dev, struct device *master, void *data)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	venc_uninit_output(pdev);
+
+	pm_runtime_disable(&pdev->dev);
+}
+
+static const struct component_ops venc_component_ops = {
+	.bind	= venc_bind,
+	.unbind	= venc_unbind,
+};
+
+static int venc_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &venc_component_ops);
+}
+
+static int venc_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &venc_component_ops);
+	return 0;
+}
+
+static int venc_runtime_suspend(struct device *dev)
+{
+	if (venc.tv_dac_clk)
+		clk_disable_unprepare(venc.tv_dac_clk);
+
+	dispc_runtime_put();
+
+	return 0;
+}
+
+static int venc_runtime_resume(struct device *dev)
+{
+	int r;
+
+	r = dispc_runtime_get();
+	if (r < 0)
+		return r;
+
+	if (venc.tv_dac_clk)
+		clk_prepare_enable(venc.tv_dac_clk);
+
+	return 0;
+}
+
+static const struct dev_pm_ops venc_pm_ops = {
+	.runtime_suspend = venc_runtime_suspend,
+	.runtime_resume = venc_runtime_resume,
+};
+
+static const struct of_device_id venc_of_match[] = {
+	{ .compatible = "ti,omap2-venc", },
+	{ .compatible = "ti,omap3-venc", },
+	{ .compatible = "ti,omap4-venc", },
+	{},
+};
+
+static struct platform_driver omap_venchw_driver = {
+	.probe		= venc_probe,
+	.remove		= venc_remove,
+	.driver         = {
+		.name   = "omapdss_venc",
+		.pm	= &venc_pm_ops,
+		.of_match_table = venc_of_match,
+		.suppress_bind_attrs = true,
+	},
+};
+
+int __init venc_init_platform_driver(void)
+{
+	return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+	platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c b/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c
new file mode 100644
index 000000000000..b1ec59e42940
--- /dev/null
+++ b/drivers/video/fbdev/omap2/omapfb/dss/video-pll.c
@@ -0,0 +1,211 @@
+/*
+* Copyright (C) 2014 Texas Instruments 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.
+*
+* You should have received a copy of the GNU General Public License along with
+* this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+
+#include <video/omapdss.h>
+
+#include "dss.h"
+#include "dss_features.h"
+
+struct dss_video_pll {
+	struct dss_pll pll;
+
+	struct device *dev;
+
+	void __iomem *clkctrl_base;
+};
+
+#define REG_MOD(reg, val, start, end) \
+	writel_relaxed(FLD_MOD(readl_relaxed(reg), val, start, end), reg)
+
+static void dss_dpll_enable_scp_clk(struct dss_video_pll *vpll)
+{
+	REG_MOD(vpll->clkctrl_base, 1, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dss_dpll_disable_scp_clk(struct dss_video_pll *vpll)
+{
+	REG_MOD(vpll->clkctrl_base, 0, 14, 14); /* CIO_CLK_ICG */
+}
+
+static void dss_dpll_power_enable(struct dss_video_pll *vpll)
+{
+	REG_MOD(vpll->clkctrl_base, 2, 31, 30); /* PLL_POWER_ON_ALL */
+
+	/*
+	 * DRA7x PLL CTRL's PLL_PWR_STATUS seems to always return 0,
+	 * so we have to use fixed delay here.
+	 */
+	msleep(1);
+}
+
+static void dss_dpll_power_disable(struct dss_video_pll *vpll)
+{
+	REG_MOD(vpll->clkctrl_base, 0, 31, 30);	/* PLL_POWER_OFF */
+}
+
+static int dss_video_pll_enable(struct dss_pll *pll)
+{
+	struct dss_video_pll *vpll = container_of(pll, struct dss_video_pll, pll);
+	int r;
+
+	r = dss_runtime_get();
+	if (r)
+		return r;
+
+	dss_ctrl_pll_enable(pll->id, true);
+
+	dss_dpll_enable_scp_clk(vpll);
+
+	r = dss_pll_wait_reset_done(pll);
+	if (r)
+		goto err_reset;
+
+	dss_dpll_power_enable(vpll);
+
+	return 0;
+
+err_reset:
+	dss_dpll_disable_scp_clk(vpll);
+	dss_ctrl_pll_enable(pll->id, false);
+	dss_runtime_put();
+
+	return r;
+}
+
+static void dss_video_pll_disable(struct dss_pll *pll)
+{
+	struct dss_video_pll *vpll = container_of(pll, struct dss_video_pll, pll);
+
+	dss_dpll_power_disable(vpll);
+
+	dss_dpll_disable_scp_clk(vpll);
+
+	dss_ctrl_pll_enable(pll->id, false);
+
+	dss_runtime_put();
+}
+
+static const struct dss_pll_ops dss_pll_ops = {
+	.enable = dss_video_pll_enable,
+	.disable = dss_video_pll_disable,
+	.set_config = dss_pll_write_config_type_a,
+};
+
+static const struct dss_pll_hw dss_dra7_video_pll_hw = {
+	.n_max = (1 << 8) - 1,
+	.m_max = (1 << 12) - 1,
+	.mX_max = (1 << 5) - 1,
+	.fint_min = 500000,
+	.fint_max = 2500000,
+	.clkdco_max = 1800000000,
+
+	.n_msb = 8,
+	.n_lsb = 1,
+	.m_msb = 20,
+	.m_lsb = 9,
+
+	.mX_msb[0] = 25,
+	.mX_lsb[0] = 21,
+	.mX_msb[1] = 30,
+	.mX_lsb[1] = 26,
+
+	.has_refsel = true,
+};
+
+struct dss_pll *dss_video_pll_init(struct platform_device *pdev, int id,
+	struct regulator *regulator)
+{
+	const char * const reg_name[] = { "pll1", "pll2" };
+	const char * const clkctrl_name[] = { "pll1_clkctrl", "pll2_clkctrl" };
+	const char * const clkin_name[] = { "video1_clk", "video2_clk" };
+
+	struct resource *res;
+	struct dss_video_pll *vpll;
+	void __iomem *pll_base, *clkctrl_base;
+	struct clk *clk;
+	struct dss_pll *pll;
+	int r;
+
+	/* PLL CONTROL */
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, reg_name[id]);
+	if (!res) {
+		dev_err(&pdev->dev,
+			"missing platform resource data for pll%d\n", id);
+		return ERR_PTR(-ENODEV);
+	}
+
+	pll_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pll_base)) {
+		dev_err(&pdev->dev, "failed to ioremap pll%d reg_name\n", id);
+		return ERR_CAST(pll_base);
+	}
+
+	/* CLOCK CONTROL */
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+		clkctrl_name[id]);
+	if (!res) {
+		dev_err(&pdev->dev,
+			"missing platform resource data for pll%d\n", id);
+		return ERR_PTR(-ENODEV);
+	}
+
+	clkctrl_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(clkctrl_base)) {
+		dev_err(&pdev->dev, "failed to ioremap pll%d clkctrl\n", id);
+		return ERR_CAST(clkctrl_base);
+	}
+
+	/* CLKIN */
+
+	clk = devm_clk_get(&pdev->dev, clkin_name[id]);
+	if (IS_ERR(clk)) {
+		DSSERR("can't get video pll clkin\n");
+		return ERR_CAST(clk);
+	}
+
+	vpll = devm_kzalloc(&pdev->dev, sizeof(*vpll), GFP_KERNEL);
+	if (!vpll)
+		return ERR_PTR(-ENOMEM);
+
+	vpll->dev = &pdev->dev;
+	vpll->clkctrl_base = clkctrl_base;
+
+	pll = &vpll->pll;
+
+	pll->name = id == 0 ? "video0" : "video1";
+	pll->id = id == 0 ? DSS_PLL_VIDEO1 : DSS_PLL_VIDEO2;
+	pll->clkin = clk;
+	pll->regulator = regulator;
+	pll->base = pll_base;
+	pll->hw = &dss_dra7_video_pll_hw;
+	pll->ops = &dss_pll_ops;
+
+	r = dss_pll_register(pll);
+	if (r)
+		return ERR_PTR(r);
+
+	return pll;
+}
+
+void dss_video_pll_uninit(struct dss_pll *pll)
+{
+	dss_pll_unregister(pll);
+}
diff --git a/drivers/video/fbdev/omap2/vrfb.c b/drivers/video/fbdev/omap2/omapfb/vrfb.c
index f346b02eee1d..f346b02eee1d 100644
--- a/drivers/video/fbdev/omap2/vrfb.c
+++ b/drivers/video/fbdev/omap2/omapfb/vrfb.c
diff --git a/drivers/video/fbdev/pxafb.c b/drivers/video/fbdev/pxafb.c
index 94813af97f09..33b2bb315a2a 100644
--- a/drivers/video/fbdev/pxafb.c
+++ b/drivers/video/fbdev/pxafb.c
@@ -55,6 +55,9 @@
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 #include <linux/console.h>
+#include <linux/of_graph.h>
+#include <video/of_display_timing.h>
+#include <video/videomode.h>
 
 #include <mach/hardware.h>
 #include <asm/io.h>
@@ -457,7 +460,7 @@ static int pxafb_adjust_timing(struct pxafb_info *fbi,
 static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
-	struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+	struct pxafb_mach_info *inf = fbi->inf;
 	int err;
 
 	if (inf->fixed_modes) {
@@ -1230,7 +1233,7 @@ static unsigned int __smart_timing(unsigned time_ns, unsigned long lcd_clk)
 static void setup_smart_timing(struct pxafb_info *fbi,
 				struct fb_var_screeninfo *var)
 {
-	struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+	struct pxafb_mach_info *inf = fbi->inf;
 	struct pxafb_mode_info *mode = &inf->modes[0];
 	unsigned long lclk = clk_get_rate(fbi->clk);
 	unsigned t1, t2, t3, t4;
@@ -1258,14 +1261,13 @@ static void setup_smart_timing(struct pxafb_info *fbi,
 static int pxafb_smart_thread(void *arg)
 {
 	struct pxafb_info *fbi = arg;
-	struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
+	struct pxafb_mach_info *inf = fbi->inf;
 
 	if (!inf->smart_update) {
 		pr_err("%s: not properly initialized, thread terminated\n",
 				__func__);
 		return -EINVAL;
 	}
-	inf = dev_get_platdata(fbi->dev);
 
 	pr_debug("%s(): task starting\n", __func__);
 
@@ -1788,11 +1790,11 @@ decode_mode:
 		fbi->video_mem_size = video_mem_size;
 }
 
-static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
+static struct pxafb_info *pxafb_init_fbinfo(struct device *dev,
+					    struct pxafb_mach_info *inf)
 {
 	struct pxafb_info *fbi;
 	void *addr;
-	struct pxafb_mach_info *inf = dev_get_platdata(dev);
 
 	/* Alloc the pxafb_info and pseudo_palette in one step */
 	fbi = kmalloc(sizeof(struct pxafb_info) + sizeof(u32) * 16, GFP_KERNEL);
@@ -1801,6 +1803,7 @@ static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
 
 	memset(fbi, 0, sizeof(struct pxafb_info));
 	fbi->dev = dev;
+	fbi->inf = inf;
 
 	fbi->clk = clk_get(dev, NULL);
 	if (IS_ERR(fbi->clk)) {
@@ -1852,10 +1855,9 @@ static struct pxafb_info *pxafb_init_fbinfo(struct device *dev)
 }
 
 #ifdef CONFIG_FB_PXA_PARAMETERS
-static int parse_opt_mode(struct device *dev, const char *this_opt)
+static int parse_opt_mode(struct device *dev, const char *this_opt,
+			  struct pxafb_mach_info *inf)
 {
-	struct pxafb_mach_info *inf = dev_get_platdata(dev);
-
 	const char *name = this_opt+5;
 	unsigned int namelen = strlen(name);
 	int res_specified = 0, bpp_specified = 0;
@@ -1911,9 +1913,9 @@ done:
 	return 0;
 }
 
-static int parse_opt(struct device *dev, char *this_opt)
+static int parse_opt(struct device *dev, char *this_opt,
+		     struct pxafb_mach_info *inf)
 {
-	struct pxafb_mach_info *inf = dev_get_platdata(dev);
 	struct pxafb_mode_info *mode = &inf->modes[0];
 	char s[64];
 
@@ -1922,7 +1924,7 @@ static int parse_opt(struct device *dev, char *this_opt)
 	if (!strncmp(this_opt, "vmem:", 5)) {
 		video_mem_size = memparse(this_opt + 5, NULL);
 	} else if (!strncmp(this_opt, "mode:", 5)) {
-		return parse_opt_mode(dev, this_opt);
+		return parse_opt_mode(dev, this_opt, inf);
 	} else if (!strncmp(this_opt, "pixclock:", 9)) {
 		mode->pixclock = simple_strtoul(this_opt+9, NULL, 0);
 		sprintf(s, "pixclock: %ld\n", mode->pixclock);
@@ -2011,7 +2013,8 @@ static int parse_opt(struct device *dev, char *this_opt)
 	return 0;
 }
 
-static int pxafb_parse_options(struct device *dev, char *options)
+static int pxafb_parse_options(struct device *dev, char *options,
+			       struct pxafb_mach_info *inf)
 {
 	char *this_opt;
 	int ret;
@@ -2023,7 +2026,7 @@ static int pxafb_parse_options(struct device *dev, char *options)
 
 	/* could be made table driven or similar?... */
 	while ((this_opt = strsep(&options, ",")) != NULL) {
-		ret = parse_opt(dev, this_opt);
+		ret = parse_opt(dev, this_opt, inf);
 		if (ret)
 			return ret;
 	}
@@ -2092,22 +2095,180 @@ static void pxafb_check_options(struct device *dev, struct pxafb_mach_info *inf)
 #define pxafb_check_options(...)	do {} while (0)
 #endif
 
+#if defined(CONFIG_OF)
+static const char * const lcd_types[] = {
+	"unknown", "mono-stn", "mono-dstn", "color-stn", "color-dstn",
+	"color-tft", "smart-panel", NULL
+};
+
+static int of_get_pxafb_display(struct device *dev, struct device_node *disp,
+				struct pxafb_mach_info *info, u32 bus_width)
+{
+	struct display_timings *timings;
+	struct videomode vm;
+	int i, ret = -EINVAL;
+	const char *s;
+
+	ret = of_property_read_string(disp, "lcd-type", &s);
+	if (ret)
+		s = "color-tft";
+
+	for (i = 0; lcd_types[i]; i++)
+		if (!strcmp(s, lcd_types[i]))
+			break;
+	if (!i || !lcd_types[i]) {
+		dev_err(dev, "lcd-type %s is unknown\n", s);
+		return -EINVAL;
+	}
+	info->lcd_conn |= LCD_CONN_TYPE(i);
+	info->lcd_conn |= LCD_CONN_WIDTH(bus_width);
+
+	timings = of_get_display_timings(disp);
+	if (!timings)
+		goto out;
+
+	ret = -ENOMEM;
+	info->modes = kmalloc_array(timings->num_timings,
+				    sizeof(info->modes[0]), GFP_KERNEL);
+	if (!info->modes)
+		goto out;
+	info->num_modes = timings->num_timings;
+
+	for (i = 0; i < timings->num_timings; i++) {
+		ret = videomode_from_timings(timings, &vm, i);
+		if (ret) {
+			dev_err(dev, "videomode_from_timings %d failed: %d\n",
+				i, ret);
+			goto out;
+		}
+		if (vm.flags & DISPLAY_FLAGS_PIXDATA_POSEDGE)
+			info->lcd_conn |= LCD_PCLK_EDGE_RISE;
+		if (vm.flags & DISPLAY_FLAGS_PIXDATA_NEGEDGE)
+			info->lcd_conn |= LCD_PCLK_EDGE_FALL;
+		if (vm.flags & DISPLAY_FLAGS_DE_HIGH)
+			info->lcd_conn |= LCD_BIAS_ACTIVE_HIGH;
+		if (vm.flags & DISPLAY_FLAGS_DE_LOW)
+			info->lcd_conn |= LCD_BIAS_ACTIVE_LOW;
+		if (vm.flags & DISPLAY_FLAGS_HSYNC_HIGH)
+			info->modes[i].sync |= FB_SYNC_HOR_HIGH_ACT;
+		if (vm.flags & DISPLAY_FLAGS_VSYNC_HIGH)
+			info->modes[i].sync |= FB_SYNC_VERT_HIGH_ACT;
+
+		info->modes[i].pixclock = 1000000000UL / (vm.pixelclock / 1000);
+		info->modes[i].xres = vm.hactive;
+		info->modes[i].yres = vm.vactive;
+		info->modes[i].hsync_len = vm.hsync_len;
+		info->modes[i].left_margin = vm.hback_porch;
+		info->modes[i].right_margin = vm.hfront_porch;
+		info->modes[i].vsync_len = vm.vsync_len;
+		info->modes[i].upper_margin = vm.vback_porch;
+		info->modes[i].lower_margin = vm.vfront_porch;
+	}
+	ret = 0;
+
+out:
+	display_timings_release(timings);
+	return ret;
+}
+
+static int of_get_pxafb_mode_info(struct device *dev,
+				  struct pxafb_mach_info *info)
+{
+	struct device_node *display, *np;
+	u32 bus_width;
+	int ret, i;
+
+	np = of_graph_get_next_endpoint(dev->of_node, NULL);
+	if (!np) {
+		dev_err(dev, "could not find endpoint\n");
+		return -EINVAL;
+	}
+	ret = of_property_read_u32(np, "bus-width", &bus_width);
+	if (ret) {
+		dev_err(dev, "no bus-width specified: %d\n", ret);
+		return ret;
+	}
+
+	display = of_graph_get_remote_port_parent(np);
+	of_node_put(np);
+	if (!display) {
+		dev_err(dev, "no display defined\n");
+		return -EINVAL;
+	}
+
+	ret = of_get_pxafb_display(dev, display, info, bus_width);
+	of_node_put(display);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < info->num_modes; i++)
+		info->modes[i].bpp = bus_width;
+
+	return 0;
+}
+
+static struct pxafb_mach_info *of_pxafb_of_mach_info(struct device *dev)
+{
+	int ret;
+	struct pxafb_mach_info *info;
+
+	if (!dev->of_node)
+		return NULL;
+	info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return ERR_PTR(-ENOMEM);
+	ret = of_get_pxafb_mode_info(dev, info);
+	if (ret) {
+		kfree(info->modes);
+		return ERR_PTR(ret);
+	}
+
+	/*
+	 * On purpose, neither lccrX registers nor video memory size can be
+	 * specified through device-tree, they are considered more a debug hack
+	 * available through command line.
+	 */
+	return info;
+}
+#else
+static struct pxafb_mach_info *of_pxafb_of_mach_info(struct device *dev)
+{
+	return NULL;
+}
+#endif
+
 static int pxafb_probe(struct platform_device *dev)
 {
 	struct pxafb_info *fbi;
-	struct pxafb_mach_info *inf;
+	struct pxafb_mach_info *inf, *pdata;
 	struct resource *r;
-	int irq, ret;
+	int i, irq, ret;
 
 	dev_dbg(&dev->dev, "pxafb_probe\n");
 
-	inf = dev_get_platdata(&dev->dev);
 	ret = -ENOMEM;
-	fbi = NULL;
+	pdata = dev_get_platdata(&dev->dev);
+	inf = devm_kmalloc(&dev->dev, sizeof(*inf), GFP_KERNEL);
 	if (!inf)
 		goto failed;
 
-	ret = pxafb_parse_options(&dev->dev, g_options);
+	if (pdata) {
+		*inf = *pdata;
+		inf->modes =
+			devm_kmalloc_array(&dev->dev, pdata->num_modes,
+					   sizeof(inf->modes[0]), GFP_KERNEL);
+		if (!inf->modes)
+			goto failed;
+		for (i = 0; i < inf->num_modes; i++)
+			inf->modes[i] = pdata->modes[i];
+	}
+
+	if (!pdata)
+		inf = of_pxafb_of_mach_info(&dev->dev);
+	if (IS_ERR_OR_NULL(inf))
+		goto failed;
+
+	ret = pxafb_parse_options(&dev->dev, g_options, inf);
 	if (ret < 0)
 		goto failed;
 
@@ -2125,7 +2286,7 @@ static int pxafb_probe(struct platform_device *dev)
 		goto failed;
 	}
 
-	fbi = pxafb_init_fbinfo(&dev->dev);
+	fbi = pxafb_init_fbinfo(&dev->dev, inf);
 	if (!fbi) {
 		/* only reason for pxafb_init_fbinfo to fail is kmalloc */
 		dev_err(&dev->dev, "Failed to initialize framebuffer device\n");
@@ -2299,11 +2460,20 @@ static int pxafb_remove(struct platform_device *dev)
 	return 0;
 }
 
+static const struct of_device_id pxafb_of_dev_id[] = {
+	{ .compatible = "marvell,pxa270-lcdc", },
+	{ .compatible = "marvell,pxa300-lcdc", },
+	{ .compatible = "marvell,pxa2xx-lcdc", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, pxafb_of_dev_id);
+
 static struct platform_driver pxafb_driver = {
 	.probe		= pxafb_probe,
 	.remove 	= pxafb_remove,
 	.driver		= {
 		.name	= "pxa2xx-fb",
+		.of_match_table = pxafb_of_dev_id,
 #ifdef CONFIG_PM
 		.pm	= &pxafb_pm_ops,
 #endif
diff --git a/drivers/video/fbdev/pxafb.h b/drivers/video/fbdev/pxafb.h
index 26ba9fa3f737..5dc414e26fc8 100644
--- a/drivers/video/fbdev/pxafb.h
+++ b/drivers/video/fbdev/pxafb.h
@@ -167,6 +167,8 @@ struct pxafb_info {
 
 	void (*lcd_power)(int, struct fb_var_screeninfo *);
 	void (*backlight_power)(int);
+
+	struct pxafb_mach_info	*inf;
 };
 
 #define TO_INF(ptr,member) container_of(ptr,struct pxafb_info,member)
diff --git a/drivers/video/fbdev/riva/fbdev.c b/drivers/video/fbdev/riva/fbdev.c
index f1ad2747064b..2ef26ad99341 100644
--- a/drivers/video/fbdev/riva/fbdev.c
+++ b/drivers/video/fbdev/riva/fbdev.c
@@ -1765,6 +1765,7 @@ static int riva_get_EDID_i2c(struct fb_info *info)
 	int i;
 
 	NVTRACE_ENTER();
+	par->riva.LockUnlock(&par->riva, 0);
 	riva_create_i2c_busses(par);
 	for (i = 0; i < 3; i++) {
 		if (!par->chan[i].par)
diff --git a/drivers/video/fbdev/sh_mobile_hdmi.c b/drivers/video/fbdev/sh_mobile_hdmi.c
deleted file mode 100644
index 7c72a3f02056..000000000000
--- a/drivers/video/fbdev/sh_mobile_hdmi.c
+++ /dev/null
@@ -1,1489 +0,0 @@
-/*
- * SH-Mobile High-Definition Multimedia Interface (HDMI) driver
- * for SLISHDMI13T and SLIPHDMIT IP cores
- *
- * 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/clk.h>
-#include <linux/console.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/init.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-#include <linux/types.h>
-#include <linux/workqueue.h>
-#include <sound/soc.h>
-#include <sound/soc-dapm.h>
-#include <sound/initval.h>
-
-#include <video/sh_mobile_hdmi.h>
-#include <video/sh_mobile_lcdc.h>
-
-#include "sh_mobile_lcdcfb.h"
-
-/* HDMI Core Control Register (HTOP0) */
-#define HDMI_SYSTEM_CTRL			0x00 /* System control */
-#define HDMI_L_R_DATA_SWAP_CTRL_RPKT		0x01 /* L/R data swap control,
-							bits 19..16 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8	0x02 /* bits 15..8 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0	0x03 /* bits 7..0 of 20-bit N for Audio Clock Regeneration packet */
-#define HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS		0x04 /* SPDIF audio sampling frequency,
-							bits 19..16 of Internal CTS */
-#define HDMI_INTERNAL_CTS_15_8			0x05 /* bits 15..8 of Internal CTS */
-#define HDMI_INTERNAL_CTS_7_0			0x06 /* bits 7..0 of Internal CTS */
-#define HDMI_EXTERNAL_CTS_19_16			0x07 /* External CTS */
-#define HDMI_EXTERNAL_CTS_15_8			0x08 /* External CTS */
-#define HDMI_EXTERNAL_CTS_7_0			0x09 /* External CTS */
-#define HDMI_AUDIO_SETTING_1			0x0A /* Audio setting.1 */
-#define HDMI_AUDIO_SETTING_2			0x0B /* Audio setting.2 */
-#define HDMI_I2S_AUDIO_SET			0x0C /* I2S audio setting */
-#define HDMI_DSD_AUDIO_SET			0x0D /* DSD audio setting */
-#define HDMI_DEBUG_MONITOR_1			0x0E /* Debug monitor.1 */
-#define HDMI_DEBUG_MONITOR_2			0x0F /* Debug monitor.2 */
-#define HDMI_I2S_INPUT_PIN_SWAP			0x10 /* I2S input pin swap */
-#define HDMI_AUDIO_STATUS_BITS_SETTING_1	0x11 /* Audio status bits setting.1 */
-#define HDMI_AUDIO_STATUS_BITS_SETTING_2	0x12 /* Audio status bits setting.2 */
-#define HDMI_CATEGORY_CODE			0x13 /* Category code */
-#define HDMI_SOURCE_NUM_AUDIO_WORD_LEN		0x14 /* Source number/Audio word length */
-#define HDMI_AUDIO_VIDEO_SETTING_1		0x15 /* Audio/Video setting.1 */
-#define HDMI_VIDEO_SETTING_1			0x16 /* Video setting.1 */
-#define HDMI_DEEP_COLOR_MODES			0x17 /* Deep Color Modes */
-
-/* 12 16- and 10-bit Color space conversion parameters: 0x18..0x2f */
-#define HDMI_COLOR_SPACE_CONVERSION_PARAMETERS	0x18
-
-#define HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS	0x30 /* External video parameter settings */
-#define HDMI_EXTERNAL_H_TOTAL_7_0		0x31 /* External horizontal total (LSB) */
-#define HDMI_EXTERNAL_H_TOTAL_11_8		0x32 /* External horizontal total (MSB) */
-#define HDMI_EXTERNAL_H_BLANK_7_0		0x33 /* External horizontal blank (LSB) */
-#define HDMI_EXTERNAL_H_BLANK_9_8		0x34 /* External horizontal blank (MSB) */
-#define HDMI_EXTERNAL_H_DELAY_7_0		0x35 /* External horizontal delay (LSB) */
-#define HDMI_EXTERNAL_H_DELAY_9_8		0x36 /* External horizontal delay (MSB) */
-#define HDMI_EXTERNAL_H_DURATION_7_0		0x37 /* External horizontal duration (LSB) */
-#define HDMI_EXTERNAL_H_DURATION_9_8		0x38 /* External horizontal duration (MSB) */
-#define HDMI_EXTERNAL_V_TOTAL_7_0		0x39 /* External vertical total (LSB) */
-#define HDMI_EXTERNAL_V_TOTAL_9_8		0x3A /* External vertical total (MSB) */
-#define HDMI_AUDIO_VIDEO_SETTING_2		0x3B /* Audio/Video setting.2 */
-#define HDMI_EXTERNAL_V_BLANK			0x3D /* External vertical blank */
-#define HDMI_EXTERNAL_V_DELAY			0x3E /* External vertical delay */
-#define HDMI_EXTERNAL_V_DURATION		0x3F /* External vertical duration */
-#define HDMI_CTRL_PKT_MANUAL_SEND_CONTROL	0x40 /* Control packet manual send control */
-#define HDMI_CTRL_PKT_AUTO_SEND			0x41 /* Control packet auto send with VSYNC control */
-#define HDMI_AUTO_CHECKSUM_OPTION		0x42 /* Auto checksum option */
-#define HDMI_VIDEO_SETTING_2			0x45 /* Video setting.2 */
-#define HDMI_OUTPUT_OPTION			0x46 /* Output option */
-#define HDMI_SLIPHDMIT_PARAM_OPTION		0x51 /* SLIPHDMIT parameter option */
-#define HDMI_HSYNC_PMENT_AT_EMB_7_0		0x52 /* HSYNC placement at embedded sync (LSB) */
-#define HDMI_HSYNC_PMENT_AT_EMB_15_8		0x53 /* HSYNC placement at embedded sync (MSB) */
-#define HDMI_VSYNC_PMENT_AT_EMB_7_0		0x54 /* VSYNC placement at embedded sync (LSB) */
-#define HDMI_VSYNC_PMENT_AT_EMB_14_8		0x55 /* VSYNC placement at embedded sync (MSB) */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_1		0x56 /* SLIPHDMIT parameter settings.1 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_2		0x57 /* SLIPHDMIT parameter settings.2 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_3		0x58 /* SLIPHDMIT parameter settings.3 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_5		0x59 /* SLIPHDMIT parameter settings.5 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_6		0x5A /* SLIPHDMIT parameter settings.6 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_7		0x5B /* SLIPHDMIT parameter settings.7 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_8		0x5C /* SLIPHDMIT parameter settings.8 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_9		0x5D /* SLIPHDMIT parameter settings.9 */
-#define HDMI_SLIPHDMIT_PARAM_SETTINGS_10	0x5E /* SLIPHDMIT parameter settings.10 */
-#define HDMI_CTRL_PKT_BUF_INDEX			0x5F /* Control packet buffer index */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB0		0x60 /* Control packet data buffer access window - HB0 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB1		0x61 /* Control packet data buffer access window - HB1 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_HB2		0x62 /* Control packet data buffer access window - HB2 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB0		0x63 /* Control packet data buffer access window - PB0 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB1		0x64 /* Control packet data buffer access window - PB1 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB2		0x65 /* Control packet data buffer access window - PB2 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB3		0x66 /* Control packet data buffer access window - PB3 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB4		0x67 /* Control packet data buffer access window - PB4 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB5		0x68 /* Control packet data buffer access window - PB5 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB6		0x69 /* Control packet data buffer access window - PB6 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB7		0x6A /* Control packet data buffer access window - PB7 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB8		0x6B /* Control packet data buffer access window - PB8 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB9		0x6C /* Control packet data buffer access window - PB9 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB10		0x6D /* Control packet data buffer access window - PB10 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB11		0x6E /* Control packet data buffer access window - PB11 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB12		0x6F /* Control packet data buffer access window - PB12 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB13		0x70 /* Control packet data buffer access window - PB13 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB14		0x71 /* Control packet data buffer access window - PB14 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB15		0x72 /* Control packet data buffer access window - PB15 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB16		0x73 /* Control packet data buffer access window - PB16 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB17		0x74 /* Control packet data buffer access window - PB17 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB18		0x75 /* Control packet data buffer access window - PB18 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB19		0x76 /* Control packet data buffer access window - PB19 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB20		0x77 /* Control packet data buffer access window - PB20 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB21		0x78 /* Control packet data buffer access window - PB21 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB22		0x79 /* Control packet data buffer access window - PB22 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB23		0x7A /* Control packet data buffer access window - PB23 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB24		0x7B /* Control packet data buffer access window - PB24 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB25		0x7C /* Control packet data buffer access window - PB25 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB26		0x7D /* Control packet data buffer access window - PB26 */
-#define HDMI_CTRL_PKT_BUF_ACCESS_PB27		0x7E /* Control packet data buffer access window - PB27 */
-#define HDMI_EDID_KSV_FIFO_ACCESS_WINDOW	0x80 /* EDID/KSV FIFO access window */
-#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_7_0	0x81 /* DDC bus access frequency control (LSB) */
-#define HDMI_DDC_BUS_ACCESS_FREQ_CTRL_15_8	0x82 /* DDC bus access frequency control (MSB) */
-#define HDMI_INTERRUPT_MASK_1			0x92 /* Interrupt mask.1 */
-#define HDMI_INTERRUPT_MASK_2			0x93 /* Interrupt mask.2 */
-#define HDMI_INTERRUPT_STATUS_1			0x94 /* Interrupt status.1 */
-#define HDMI_INTERRUPT_STATUS_2			0x95 /* Interrupt status.2 */
-#define HDMI_INTERRUPT_MASK_3			0x96 /* Interrupt mask.3 */
-#define HDMI_INTERRUPT_MASK_4			0x97 /* Interrupt mask.4 */
-#define HDMI_INTERRUPT_STATUS_3			0x98 /* Interrupt status.3 */
-#define HDMI_INTERRUPT_STATUS_4			0x99 /* Interrupt status.4 */
-#define HDMI_SOFTWARE_HDCP_CONTROL_1		0x9A /* Software HDCP control.1 */
-#define HDMI_FRAME_COUNTER			0x9C /* Frame counter */
-#define HDMI_FRAME_COUNTER_FOR_RI_CHECK		0x9D /* Frame counter for Ri check */
-#define HDMI_HDCP_CONTROL			0xAF /* HDCP control */
-#define HDMI_RI_FRAME_COUNT_REGISTER		0xB2 /* Ri frame count register */
-#define HDMI_DDC_BUS_CONTROL			0xB7 /* DDC bus control */
-#define HDMI_HDCP_STATUS			0xB8 /* HDCP status */
-#define HDMI_SHA0				0xB9 /* sha0 */
-#define HDMI_SHA1				0xBA /* sha1 */
-#define HDMI_SHA2				0xBB /* sha2 */
-#define HDMI_SHA3				0xBC /* sha3 */
-#define HDMI_SHA4				0xBD /* sha4 */
-#define HDMI_BCAPS_READ				0xBE /* BCAPS read / debug */
-#define HDMI_AKSV_BKSV_7_0_MONITOR		0xBF /* AKSV/BKSV[7:0] monitor */
-#define HDMI_AKSV_BKSV_15_8_MONITOR		0xC0 /* AKSV/BKSV[15:8] monitor */
-#define HDMI_AKSV_BKSV_23_16_MONITOR		0xC1 /* AKSV/BKSV[23:16] monitor */
-#define HDMI_AKSV_BKSV_31_24_MONITOR		0xC2 /* AKSV/BKSV[31:24] monitor */
-#define HDMI_AKSV_BKSV_39_32_MONITOR		0xC3 /* AKSV/BKSV[39:32] monitor */
-#define HDMI_EDID_SEGMENT_POINTER		0xC4 /* EDID segment pointer */
-#define HDMI_EDID_WORD_ADDRESS			0xC5 /* EDID word address */
-#define HDMI_EDID_DATA_FIFO_ADDRESS		0xC6 /* EDID data FIFO address */
-#define HDMI_NUM_OF_HDMI_DEVICES		0xC7 /* Number of HDMI devices */
-#define HDMI_HDCP_ERROR_CODE			0xC8 /* HDCP error code */
-#define HDMI_100MS_TIMER_SET			0xC9 /* 100ms timer setting */
-#define HDMI_5SEC_TIMER_SET			0xCA /* 5sec timer setting */
-#define HDMI_RI_READ_COUNT			0xCB /* Ri read count */
-#define HDMI_AN_SEED				0xCC /* An seed */
-#define HDMI_MAX_NUM_OF_RCIVRS_ALLOWED		0xCD /* Maximum number of receivers allowed */
-#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_1	0xCE /* HDCP memory access control.1 */
-#define HDMI_HDCP_MEMORY_ACCESS_CONTROL_2	0xCF /* HDCP memory access control.2 */
-#define HDMI_HDCP_CONTROL_2			0xD0 /* HDCP Control 2 */
-#define HDMI_HDCP_KEY_MEMORY_CONTROL		0xD2 /* HDCP Key Memory Control */
-#define HDMI_COLOR_SPACE_CONV_CONFIG_1		0xD3 /* Color space conversion configuration.1 */
-#define HDMI_VIDEO_SETTING_3			0xD4 /* Video setting.3 */
-#define HDMI_RI_7_0				0xD5 /* Ri[7:0] */
-#define HDMI_RI_15_8				0xD6 /* Ri[15:8] */
-#define HDMI_PJ					0xD7 /* Pj */
-#define HDMI_SHA_RD				0xD8 /* sha_rd */
-#define HDMI_RI_7_0_SAVED			0xD9 /* Ri[7:0] saved */
-#define HDMI_RI_15_8_SAVED			0xDA /* Ri[15:8] saved */
-#define HDMI_PJ_SAVED				0xDB /* Pj saved */
-#define HDMI_NUM_OF_DEVICES			0xDC /* Number of devices */
-#define HDMI_HOT_PLUG_MSENS_STATUS		0xDF /* Hot plug/MSENS status */
-#define HDMI_BCAPS_WRITE			0xE0 /* bcaps */
-#define HDMI_BSTAT_7_0				0xE1 /* bstat[7:0] */
-#define HDMI_BSTAT_15_8				0xE2 /* bstat[15:8] */
-#define HDMI_BKSV_7_0				0xE3 /* bksv[7:0] */
-#define HDMI_BKSV_15_8				0xE4 /* bksv[15:8] */
-#define HDMI_BKSV_23_16				0xE5 /* bksv[23:16] */
-#define HDMI_BKSV_31_24				0xE6 /* bksv[31:24] */
-#define HDMI_BKSV_39_32				0xE7 /* bksv[39:32] */
-#define HDMI_AN_7_0				0xE8 /* An[7:0] */
-#define HDMI_AN_15_8				0xE9 /* An [15:8] */
-#define HDMI_AN_23_16				0xEA /* An [23:16] */
-#define HDMI_AN_31_24				0xEB /* An [31:24] */
-#define HDMI_AN_39_32				0xEC /* An [39:32] */
-#define HDMI_AN_47_40				0xED /* An [47:40] */
-#define HDMI_AN_55_48				0xEE /* An [55:48] */
-#define HDMI_AN_63_56				0xEF /* An [63:56] */
-#define HDMI_PRODUCT_ID				0xF0 /* Product ID */
-#define HDMI_REVISION_ID			0xF1 /* Revision ID */
-#define HDMI_TEST_MODE				0xFE /* Test mode */
-
-/* HDMI Control Register (HTOP1) */
-#define HDMI_HTOP1_TEST_MODE			0x0000 /* Test mode */
-#define HDMI_HTOP1_VIDEO_INPUT			0x0008 /* VideoInput */
-#define HDMI_HTOP1_CORE_RSTN			0x000C /* CoreResetn */
-#define HDMI_HTOP1_PLLBW			0x0018 /* PLLBW */
-#define HDMI_HTOP1_CLK_TO_PHY			0x001C /* Clk to Phy */
-#define HDMI_HTOP1_VIDEO_INPUT2			0x0020 /* VideoInput2 */
-#define HDMI_HTOP1_TISEMP0_1			0x0024 /* tisemp0-1 */
-#define HDMI_HTOP1_TISEMP2_C			0x0028 /* tisemp2-c */
-#define HDMI_HTOP1_TISIDRV			0x002C /* tisidrv */
-#define HDMI_HTOP1_TISEN			0x0034 /* tisen */
-#define HDMI_HTOP1_TISDREN			0x0038 /* tisdren  */
-#define HDMI_HTOP1_CISRANGE			0x003C /* cisrange  */
-#define HDMI_HTOP1_ENABLE_SELECTOR		0x0040 /* Enable Selector */
-#define HDMI_HTOP1_MACRO_RESET			0x0044 /* Macro reset */
-#define HDMI_HTOP1_PLL_CALIBRATION		0x0048 /* PLL calibration */
-#define HDMI_HTOP1_RE_CALIBRATION		0x004C /* Re-calibration */
-#define HDMI_HTOP1_CURRENT			0x0050 /* Current */
-#define HDMI_HTOP1_PLL_LOCK_DETECT		0x0054 /* PLL lock detect */
-#define HDMI_HTOP1_PHY_TEST_MODE		0x0058 /* PHY Test Mode */
-#define HDMI_HTOP1_CLK_SET			0x0080 /* Clock Set */
-#define HDMI_HTOP1_DDC_FAIL_SAFE		0x0084 /* DDC fail safe */
-#define HDMI_HTOP1_PRBS				0x0088 /* PRBS */
-#define HDMI_HTOP1_EDID_AINC_CONTROL		0x008C /* EDID ainc Control */
-#define HDMI_HTOP1_HTOP_DCL_MODE		0x00FC /* Deep Coloer Mode */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF0		0x0100 /* Deep Color:FRC COEF0 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF1		0x0104 /* Deep Color:FRC COEF1 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF2		0x0108 /* Deep Color:FRC COEF2 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF3		0x010C /* Deep Color:FRC COEF3 */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF0_C		0x0110 /* Deep Color:FRC COEF0C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF1_C		0x0114 /* Deep Color:FRC COEF1C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF2_C		0x0118 /* Deep Color:FRC COEF2C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_COEF3_C		0x011C /* Deep Color:FRC COEF3C */
-#define HDMI_HTOP1_HTOP_DCL_FRC_MODE		0x0120 /* Deep Color:FRC Mode */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START1		0x0124 /* Deep Color:Rect Start1 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE1		0x0128 /* Deep Color:Rect Size1 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START2		0x012C /* Deep Color:Rect Start2 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE2		0x0130 /* Deep Color:Rect Size2 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START3		0x0134 /* Deep Color:Rect Start3 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE3		0x0138 /* Deep Color:Rect Size3 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_START4		0x013C /* Deep Color:Rect Start4 */
-#define HDMI_HTOP1_HTOP_DCL_RECT_SIZE4		0x0140 /* Deep Color:Rect Size4 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_1	0x0144 /* Deep Color:Fil Para Y1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_2	0x0148 /* Deep Color:Fil Para Y1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_1	0x014C /* Deep Color:Fil Para CB1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_2	0x0150 /* Deep Color:Fil Para CB1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_1	0x0154 /* Deep Color:Fil Para CR1_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_2	0x0158 /* Deep Color:Fil Para CR1_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_1	0x015C /* Deep Color:Fil Para Y2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_2	0x0160 /* Deep Color:Fil Para Y2_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_1	0x0164 /* Deep Color:Fil Para CB2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_2	0x0168 /* Deep Color:Fil Para CB2_2 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_1	0x016C /* Deep Color:Fil Para CR2_1 */
-#define HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_2	0x0170 /* Deep Color:Fil Para CR2_2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_Y1		0x0174 /* Deep Color:Cor Para Y1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CB1	0x0178 /* Deep Color:Cor Para CB1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CR1	0x017C /* Deep Color:Cor Para CR1 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_Y2		0x0180 /* Deep Color:Cor Para Y2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CB2	0x0184 /* Deep Color:Cor Para CB2 */
-#define HDMI_HTOP1_HTOP_DCL_COR_PARA_CR2	0x0188 /* Deep Color:Cor Para CR2 */
-#define HDMI_HTOP1_EDID_DATA_READ		0x0200 /* EDID Data Read 128Byte:0x03FC */
-
-enum hotplug_state {
-	HDMI_HOTPLUG_DISCONNECTED,
-	HDMI_HOTPLUG_CONNECTED,
-	HDMI_HOTPLUG_EDID_DONE,
-};
-
-struct sh_hdmi {
-	struct sh_mobile_lcdc_entity entity;
-
-	void __iomem *base;
-	void __iomem *htop1;
-	enum hotplug_state hp_state;	/* hot-plug status */
-	u8 preprogrammed_vic;		/* use a pre-programmed VIC or
-					   the external mode */
-	u8 edid_block_addr;
-	u8 edid_segment_nr;
-	u8 edid_blocks;
-	int irq;
-	struct clk *hdmi_clk;
-	struct device *dev;
-	struct delayed_work edid_work;
-	struct fb_videomode mode;
-	struct fb_monspecs monspec;
-
-	/* register access functions */
-	void (*write)(struct sh_hdmi *hdmi, u8 data, u8 reg);
-	u8 (*read)(struct sh_hdmi *hdmi, u8 reg);
-};
-
-#define entity_to_sh_hdmi(e)	container_of(e, struct sh_hdmi, entity)
-
-static void __hdmi_write8(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-	iowrite8(data, hdmi->base + reg);
-}
-
-static u8 __hdmi_read8(struct sh_hdmi *hdmi, u8 reg)
-{
-	return ioread8(hdmi->base + reg);
-}
-
-static void __hdmi_write32(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-	iowrite32((u32)data, hdmi->base + (reg * 4));
-	udelay(100);
-}
-
-static u8 __hdmi_read32(struct sh_hdmi *hdmi, u8 reg)
-{
-	return (u8)ioread32(hdmi->base + (reg * 4));
-}
-
-static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
-{
-	hdmi->write(hdmi, data, reg);
-}
-
-static u8 hdmi_read(struct sh_hdmi *hdmi, u8 reg)
-{
-	return hdmi->read(hdmi, reg);
-}
-
-static void hdmi_bit_set(struct sh_hdmi *hdmi, u8 mask, u8 data, u8 reg)
-{
-	u8 val = hdmi_read(hdmi, reg);
-
-	val &= ~mask;
-	val |= (data & mask);
-
-	hdmi_write(hdmi, val, reg);
-}
-
-static void hdmi_htop1_write(struct sh_hdmi *hdmi, u32 data, u32 reg)
-{
-	iowrite32(data, hdmi->htop1 + reg);
-	udelay(100);
-}
-
-static u32 hdmi_htop1_read(struct sh_hdmi *hdmi, u32 reg)
-{
-	return ioread32(hdmi->htop1 + reg);
-}
-
-/*
- *	HDMI sound
- */
-static unsigned int sh_hdmi_snd_read(struct snd_soc_codec *codec,
-				     unsigned int reg)
-{
-	struct sh_hdmi *hdmi = snd_soc_codec_get_drvdata(codec);
-
-	return hdmi_read(hdmi, reg);
-}
-
-static int sh_hdmi_snd_write(struct snd_soc_codec *codec,
-			     unsigned int reg,
-			     unsigned int value)
-{
-	struct sh_hdmi *hdmi = snd_soc_codec_get_drvdata(codec);
-
-	hdmi_write(hdmi, value, reg);
-	return 0;
-}
-
-static struct snd_soc_dai_driver sh_hdmi_dai = {
-	.name = "sh_mobile_hdmi-hifi",
-	.playback = {
-		.stream_name = "Playback",
-		.channels_min = 2,
-		.channels_max = 8,
-		.rates = SNDRV_PCM_RATE_32000 | SNDRV_PCM_RATE_44100  |
-			 SNDRV_PCM_RATE_48000 | SNDRV_PCM_RATE_88200  |
-			 SNDRV_PCM_RATE_96000 | SNDRV_PCM_RATE_176400 |
-			 SNDRV_PCM_RATE_192000,
-		.formats = SNDRV_PCM_FMTBIT_S16_LE | SNDRV_PCM_FMTBIT_S24_LE,
-	},
-};
-
-static int sh_hdmi_snd_probe(struct snd_soc_codec *codec)
-{
-	dev_info(codec->dev, "SH Mobile HDMI Audio Codec");
-
-	return 0;
-}
-
-static struct snd_soc_codec_driver soc_codec_dev_sh_hdmi = {
-	.probe		= sh_hdmi_snd_probe,
-	.read		= sh_hdmi_snd_read,
-	.write		= sh_hdmi_snd_write,
-};
-
-/*
- *	HDMI video
- */
-
-/* External video parameter settings */
-static void sh_hdmi_external_video_param(struct sh_hdmi *hdmi)
-{
-	struct fb_videomode *mode = &hdmi->mode;
-	u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
-	u8 sync = 0;
-
-	htotal = mode->xres + mode->right_margin + mode->left_margin
-	       + mode->hsync_len;
-	hdelay = mode->hsync_len + mode->left_margin;
-	hblank = mode->right_margin + hdelay;
-
-	/*
-	 * Vertical timing looks a bit different in Figure 18,
-	 * but let's try the same first by setting offset = 0
-	 */
-	vtotal = mode->yres + mode->upper_margin + mode->lower_margin
-	       + mode->vsync_len;
-	vdelay = mode->vsync_len + mode->upper_margin;
-	vblank = mode->lower_margin + vdelay;
-	voffset = min(mode->upper_margin / 2, 6U);
-
-	/*
-	 * [3]: VSYNC polarity: Positive
-	 * [2]: HSYNC polarity: Positive
-	 * [1]: Interlace/Progressive: Progressive
-	 * [0]: External video settings enable: used.
-	 */
-	if (mode->sync & FB_SYNC_HOR_HIGH_ACT)
-		sync |= 4;
-	if (mode->sync & FB_SYNC_VERT_HIGH_ACT)
-		sync |= 8;
-
-	dev_dbg(hdmi->dev, "H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
-		htotal, hblank, hdelay, mode->hsync_len,
-		vtotal, vblank, vdelay, mode->vsync_len, sync);
-
-	hdmi_write(hdmi, sync | (voffset << 4), HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
-
-	hdmi_write(hdmi, htotal, HDMI_EXTERNAL_H_TOTAL_7_0);
-	hdmi_write(hdmi, htotal >> 8, HDMI_EXTERNAL_H_TOTAL_11_8);
-
-	hdmi_write(hdmi, hblank, HDMI_EXTERNAL_H_BLANK_7_0);
-	hdmi_write(hdmi, hblank >> 8, HDMI_EXTERNAL_H_BLANK_9_8);
-
-	hdmi_write(hdmi, hdelay, HDMI_EXTERNAL_H_DELAY_7_0);
-	hdmi_write(hdmi, hdelay >> 8, HDMI_EXTERNAL_H_DELAY_9_8);
-
-	hdmi_write(hdmi, mode->hsync_len, HDMI_EXTERNAL_H_DURATION_7_0);
-	hdmi_write(hdmi, mode->hsync_len >> 8, HDMI_EXTERNAL_H_DURATION_9_8);
-
-	hdmi_write(hdmi, vtotal, HDMI_EXTERNAL_V_TOTAL_7_0);
-	hdmi_write(hdmi, vtotal >> 8, HDMI_EXTERNAL_V_TOTAL_9_8);
-
-	hdmi_write(hdmi, vblank, HDMI_EXTERNAL_V_BLANK);
-
-	hdmi_write(hdmi, vdelay, HDMI_EXTERNAL_V_DELAY);
-
-	hdmi_write(hdmi, mode->vsync_len, HDMI_EXTERNAL_V_DURATION);
-
-	/* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for external mode */
-	if (!hdmi->preprogrammed_vic)
-		hdmi_write(hdmi, sync | 1 | (voffset << 4),
-			   HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
-}
-
-/**
- * sh_hdmi_video_config()
- */
-static void sh_hdmi_video_config(struct sh_hdmi *hdmi)
-{
-	/*
-	 * [7:4]: Audio sampling frequency: 48kHz
-	 * [3:1]: Input video format: RGB and YCbCr 4:4:4 (Y on Green)
-	 * [0]: Internal/External DE select: internal
-	 */
-	hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
-
-	/*
-	 * [7:6]: Video output format: RGB 4:4:4
-	 * [5:4]: Input video data width: 8 bit
-	 * [3:1]: EAV/SAV location: channel 1
-	 * [0]: Video input color space: RGB
-	 */
-	hdmi_write(hdmi, 0x34, HDMI_VIDEO_SETTING_1);
-
-	/*
-	 * [7:6]: Together with bit [6] of HDMI_AUDIO_VIDEO_SETTING_2, which is
-	 * left at 0 by default, this configures 24bpp and sets the Color Depth
-	 * (CD) field in the General Control Packet
-	 */
-	hdmi_write(hdmi, 0x20, HDMI_DEEP_COLOR_MODES);
-}
-
-/**
- * sh_hdmi_audio_config()
- */
-static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
-{
-	u8 data;
-	struct sh_mobile_hdmi_info *pdata = dev_get_platdata(hdmi->dev);
-
-	/*
-	 * [7:4] L/R data swap control
-	 * [3:0] appropriate N[19:16]
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_L_R_DATA_SWAP_CTRL_RPKT);
-	/* appropriate N[15:8] */
-	hdmi_write(hdmi, 0x18, HDMI_20_BIT_N_FOR_AUDIO_RPKT_15_8);
-	/* appropriate N[7:0] */
-	hdmi_write(hdmi, 0x00, HDMI_20_BIT_N_FOR_AUDIO_RPKT_7_0);
-
-	/* [7:4] 48 kHz	SPDIF not used */
-	hdmi_write(hdmi, 0x20, HDMI_SPDIF_AUDIO_SAMP_FREQ_CTS);
-
-	/*
-	 * [6:5] set required down sampling rate if required
-	 * [4:3] set required audio source
-	 */
-	switch (pdata->flags & HDMI_SND_SRC_MASK) {
-	default:
-		/* fall through */
-	case HDMI_SND_SRC_I2S:
-		data = 0x0 << 3;
-		break;
-	case HDMI_SND_SRC_SPDIF:
-		data = 0x1 << 3;
-		break;
-	case HDMI_SND_SRC_DSD:
-		data = 0x2 << 3;
-		break;
-	case HDMI_SND_SRC_HBR:
-		data = 0x3 << 3;
-		break;
-	}
-	hdmi_write(hdmi, data, HDMI_AUDIO_SETTING_1);
-
-	/* [3:0] set sending channel number for channel status */
-	hdmi_write(hdmi, 0x40, HDMI_AUDIO_SETTING_2);
-
-	/*
-	 * [5:2] set valid I2S source input pin
-	 * [1:0] set input I2S source mode
-	 */
-	hdmi_write(hdmi, 0x04, HDMI_I2S_AUDIO_SET);
-
-	/* [7:4] set valid DSD source input pin */
-	hdmi_write(hdmi, 0x00, HDMI_DSD_AUDIO_SET);
-
-	/* [7:0] set appropriate I2S input pin swap settings if required */
-	hdmi_write(hdmi, 0x00, HDMI_I2S_INPUT_PIN_SWAP);
-
-	/*
-	 * [7] set validity bit for channel status
-	 * [3:0] set original sample frequency for channel status
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_1);
-
-	/*
-	 * [7] set value for channel status
-	 * [6] set value for channel status
-	 * [5] set copyright bit for channel status
-	 * [4:2] set additional information for channel status
-	 * [1:0] set clock accuracy for channel status
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_AUDIO_STATUS_BITS_SETTING_2);
-
-	/* [7:0] set category code for channel status */
-	hdmi_write(hdmi, 0x00, HDMI_CATEGORY_CODE);
-
-	/*
-	 * [7:4] set source number for channel status
-	 * [3:0] set word length for channel status
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_SOURCE_NUM_AUDIO_WORD_LEN);
-
-	/* [7:4] set sample frequency for channel status */
-	hdmi_write(hdmi, 0x20, HDMI_AUDIO_VIDEO_SETTING_1);
-}
-
-/**
- * sh_hdmi_phy_config() - configure the HDMI PHY for the used video mode
- */
-static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
-{
-	if (hdmi->mode.pixclock < 10000) {
-		/* for 1080p8bit 148MHz */
-		hdmi_write(hdmi, 0x1d, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
-		hdmi_write(hdmi, 0x4c, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
-		hdmi_write(hdmi, 0x1e, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
-		hdmi_write(hdmi, 0x48, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
-		hdmi_write(hdmi, 0x0e, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
-		hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
-		hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
-	} else if (hdmi->mode.pixclock < 30000) {
-		/* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
-		/*
-		 * [1:0]	Speed_A
-		 * [3:2]	Speed_B
-		 * [4]		PLLA_Bypass
-		 * [6]		DRV_TEST_EN
-		 * [7]		DRV_TEST_IN
-		 */
-		hdmi_write(hdmi, 0x0f, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
-		/* PLLB_CONFIG[17], PLLA_CONFIG[17] - not in PHY datasheet */
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
-		/*
-		 * [2:0]	BGR_I_OFFSET
-		 * [6:4]	BGR_V_OFFSET
-		 */
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
-		/* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
-		hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
-		/*
-		 * PLLA_CONFIG[15:8]: regulator voltage[0], CP current,
-		 * LPF capacitance, LPF resistance[1]
-		 */
-		hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
-		/* PLLB_CONFIG[7:0]: LPF resistance[0], VCO offset, VCO gain */
-		hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
-		/*
-		 * PLLB_CONFIG[15:8]: regulator voltage[0], CP current,
-		 * LPF capacitance, LPF resistance[1]
-		 */
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
-		/* DRV_CONFIG, PE_CONFIG */
-		hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
-		/*
-		 * [2:0]	AMON_SEL (4 == LPF voltage)
-		 * [4]		PLLA_CONFIG[16]
-		 * [5]		PLLB_CONFIG[16]
-		 */
-		hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
-	} else {
-		/* for 480p8bit 27MHz */
-		hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
-		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
-		hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
-		hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
-		hdmi_write(hdmi, 0x48, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
-		hdmi_write(hdmi, 0x0F, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
-		hdmi_write(hdmi, 0x20, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
-		hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
-	}
-}
-
-/**
- * sh_hdmi_avi_infoframe_setup() - Auxiliary Video Information InfoFrame CONTROL PACKET
- */
-static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
-{
-	u8 vic;
-
-	/* AVI InfoFrame */
-	hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* Packet Type = 0x82 */
-	hdmi_write(hdmi, 0x82, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-
-	/* Version = 0x02 */
-	hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-
-	/* Length = 13 (0x0D) */
-	hdmi_write(hdmi, 0x0D, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* N. A. Checksum */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
-
-	/*
-	 * Y = RGB
-	 * A0 = No Data
-	 * B = Bar Data not valid
-	 * S = No Data
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
-
-	/*
-	 * [7:6] C = Colorimetry: no data
-	 * [5:4] M = 2: 16:9, 1: 4:3 Picture Aspect Ratio
-	 * [3:0] R = 8: Active Frame Aspect Ratio: same as picture aspect ratio
-	 */
-	hdmi_write(hdmi, 0x28, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
-
-	/*
-	 * ITC = No Data
-	 * EC = xvYCC601
-	 * Q = Default (depends on video format)
-	 * SC = No Known non_uniform Scaling
-	 */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
-
-	/*
-	 * VIC should be ignored if external config is used, so, we could just use 0,
-	 * but play safe and use a valid value in any case just in case
-	 */
-	if (hdmi->preprogrammed_vic)
-		vic = hdmi->preprogrammed_vic;
-	else
-		vic = 4;
-	hdmi_write(hdmi, vic, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
-
-	/* PR = No Repetition */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
-
-	/* Line Number of End of Top Bar (lower 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
-
-	/* Line Number of End of Top Bar (upper 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
-
-	/* Line Number of Start of Bottom Bar (lower 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
-
-	/* Line Number of Start of Bottom Bar (upper 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
-
-	/* Pixel Number of End of Left Bar (lower 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
-
-	/* Pixel Number of End of Left Bar (upper 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB11);
-
-	/* Pixel Number of Start of Right Bar (lower 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB12);
-
-	/* Pixel Number of Start of Right Bar (upper 8 bits) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB13);
-}
-
-/**
- * sh_hdmi_audio_infoframe_setup() - Audio InfoFrame of CONTROL PACKET
- */
-static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
-{
-	/* Audio InfoFrame */
-	hdmi_write(hdmi, 0x08, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* Packet Type = 0x84 */
-	hdmi_write(hdmi, 0x84, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-
-	/* Version Number = 0x01 */
-	hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-
-	/* 0 Length = 10 (0x0A) */
-	hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* n. a. Checksum */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0);
-
-	/* Audio Channel Count = Refer to Stream Header */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
-
-	/* Refer to Stream Header */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
-
-	/* Format depends on coding type (i.e. CT0...CT3) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB3);
-
-	/* Speaker Channel Allocation = Front Right + Front Left */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
-
-	/* Level Shift Value = 0 dB, Down - mix is permitted or no information */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
-
-	/* Reserved (0) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB6);
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB7);
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB8);
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB9);
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
-}
-
-/**
- * sh_hdmi_configure() - Initialise HDMI for output
- */
-static void sh_hdmi_configure(struct sh_hdmi *hdmi)
-{
-	/* Configure video format */
-	sh_hdmi_video_config(hdmi);
-
-	/* Configure audio format */
-	sh_hdmi_audio_config(hdmi);
-
-	/* Configure PHY */
-	sh_hdmi_phy_config(hdmi);
-
-	/* Auxiliary Video Information (AVI) InfoFrame */
-	sh_hdmi_avi_infoframe_setup(hdmi);
-
-	/* Audio InfoFrame */
-	sh_hdmi_audio_infoframe_setup(hdmi);
-
-	/*
-	 * Control packet auto send with VSYNC control: auto send
-	 * General control, Gamut metadata, ISRC, and ACP packets
-	 */
-	hdmi_write(hdmi, 0x8E, HDMI_CTRL_PKT_AUTO_SEND);
-
-	/* FIXME */
-	msleep(10);
-
-	/* PS mode b->d, reset PLLA and PLLB */
-	hdmi_bit_set(hdmi, 0xFC, 0x4C, HDMI_SYSTEM_CTRL);
-
-	udelay(10);
-
-	hdmi_bit_set(hdmi, 0xFC, 0x40, HDMI_SYSTEM_CTRL);
-}
-
-static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
-		const struct fb_videomode *mode,
-		unsigned long *hdmi_rate, unsigned long *parent_rate)
-{
-	unsigned long target = PICOS2KHZ(mode->pixclock) * 1000, rate_error;
-	struct sh_mobile_hdmi_info *pdata = dev_get_platdata(hdmi->dev);
-
-	*hdmi_rate = clk_round_rate(hdmi->hdmi_clk, target);
-	if ((long)*hdmi_rate < 0)
-		*hdmi_rate = clk_get_rate(hdmi->hdmi_clk);
-
-	rate_error = (long)*hdmi_rate > 0 ? abs(*hdmi_rate - target) : ULONG_MAX;
-	if (rate_error && pdata->clk_optimize_parent)
-		rate_error = pdata->clk_optimize_parent(target, hdmi_rate, parent_rate);
-	else if (clk_get_parent(hdmi->hdmi_clk))
-		*parent_rate = clk_get_rate(clk_get_parent(hdmi->hdmi_clk));
-
-	dev_dbg(hdmi->dev, "%u-%u-%u-%u x %u-%u-%u-%u\n",
-		mode->left_margin, mode->xres,
-		mode->right_margin, mode->hsync_len,
-		mode->upper_margin, mode->yres,
-		mode->lower_margin, mode->vsync_len);
-
-	dev_dbg(hdmi->dev, "\t@%lu(+/-%lu)Hz, e=%lu / 1000, r=%uHz, p=%luHz\n", target,
-		rate_error, rate_error ? 10000 / (10 * target / rate_error) : 0,
-		mode->refresh, *parent_rate);
-
-	return rate_error;
-}
-
-static int sh_hdmi_read_edid(struct sh_hdmi *hdmi, unsigned long *hdmi_rate,
-			     unsigned long *parent_rate)
-{
-	struct sh_mobile_lcdc_chan *ch = hdmi->entity.lcdc;
-	const struct fb_videomode *mode, *found = NULL;
-	unsigned int f_width = 0, f_height = 0, f_refresh = 0;
-	unsigned long found_rate_error = ULONG_MAX; /* silly compiler... */
-	bool scanning = false, preferred_bad = false;
-	bool use_edid_mode = false;
-	u8 edid[128];
-	char *forced;
-	int i;
-
-	/* Read EDID */
-	dev_dbg(hdmi->dev, "Read back EDID code:");
-	for (i = 0; i < 128; i++) {
-		edid[i] = (hdmi->htop1) ?
-			(u8)hdmi_htop1_read(hdmi, HDMI_HTOP1_EDID_DATA_READ + (i * 4)) :
-			hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
-#ifdef DEBUG
-		if ((i % 16) == 0) {
-			printk(KERN_CONT "\n");
-			printk(KERN_DEBUG "%02X | %02X", i, edid[i]);
-		} else {
-			printk(KERN_CONT " %02X", edid[i]);
-		}
-#endif
-	}
-#ifdef DEBUG
-	printk(KERN_CONT "\n");
-#endif
-
-	if (!hdmi->edid_blocks) {
-		fb_edid_to_monspecs(edid, &hdmi->monspec);
-		hdmi->edid_blocks = edid[126] + 1;
-
-		dev_dbg(hdmi->dev, "%d main modes, %d extension blocks\n",
-			hdmi->monspec.modedb_len, hdmi->edid_blocks - 1);
-	} else {
-		dev_dbg(hdmi->dev, "Extension %u detected, DTD start %u\n",
-			edid[0], edid[2]);
-		fb_edid_add_monspecs(edid, &hdmi->monspec);
-	}
-
-	if (hdmi->edid_blocks > hdmi->edid_segment_nr * 2 +
-	    (hdmi->edid_block_addr >> 7) + 1) {
-		/* More blocks to read */
-		if (hdmi->edid_block_addr) {
-			hdmi->edid_block_addr = 0;
-			hdmi->edid_segment_nr++;
-		} else {
-			hdmi->edid_block_addr = 0x80;
-		}
-		/* Set EDID word address  */
-		hdmi_write(hdmi, hdmi->edid_block_addr, HDMI_EDID_WORD_ADDRESS);
-		/* Enable EDID interrupt */
-		hdmi_write(hdmi, 0xC6, HDMI_INTERRUPT_MASK_1);
-		/* Set EDID segment pointer - starts reading EDID */
-		hdmi_write(hdmi, hdmi->edid_segment_nr, HDMI_EDID_SEGMENT_POINTER);
-		return -EAGAIN;
-	}
-
-	/* All E-EDID blocks ready */
-	dev_dbg(hdmi->dev, "%d main and extended modes\n", hdmi->monspec.modedb_len);
-
-	fb_get_options("sh_mobile_lcdc", &forced);
-	if (forced && *forced) {
-		/* Only primitive parsing so far */
-		i = sscanf(forced, "%ux%u@%u",
-			   &f_width, &f_height, &f_refresh);
-		if (i < 2) {
-			f_width = 0;
-			f_height = 0;
-		} else {
-			/* The user wants us to use the EDID data */
-			scanning = true;
-		}
-		dev_dbg(hdmi->dev, "Forced mode %ux%u@%uHz\n",
-			f_width, f_height, f_refresh);
-	}
-
-	/* Walk monitor modes to find the best or the exact match */
-	for (i = 0, mode = hdmi->monspec.modedb;
-	     i < hdmi->monspec.modedb_len && scanning;
-	     i++, mode++) {
-		unsigned long rate_error;
-
-		if (!f_width && !f_height) {
-			/*
-			 * A parameter string "video=sh_mobile_lcdc:0x0" means
-			 * use the preferred EDID mode. If it is rejected by
-			 * .fb_check_var(), keep looking, until an acceptable
-			 * one is found.
-			 */
-			if ((mode->flag & FB_MODE_IS_FIRST) || preferred_bad)
-				scanning = false;
-			else
-				continue;
-		} else if (f_width != mode->xres || f_height != mode->yres) {
-			/* No interest in unmatching modes */
-			continue;
-		}
-
-		rate_error = sh_hdmi_rate_error(hdmi, mode, hdmi_rate, parent_rate);
-
-		if (scanning) {
-			if (f_refresh == mode->refresh || (!f_refresh && !rate_error))
-				/*
-				 * Exact match if either the refresh rate
-				 * matches or it hasn't been specified and we've
-				 * found a mode, for which we can configure the
-				 * clock precisely
-				 */
-				scanning = false;
-			else if (found && found_rate_error <= rate_error)
-				/*
-				 * We otherwise search for the closest matching
-				 * clock rate - either if no refresh rate has
-				 * been specified or we cannot find an exactly
-				 * matching one
-				 */
-				continue;
-		}
-
-		/* Check if supported: sufficient fb memory, supported clock-rate */
-		if (ch && ch->notify &&
-		    ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_MODE, mode,
-			       NULL)) {
-			scanning = true;
-			preferred_bad = true;
-			continue;
-		}
-
-		found = mode;
-		found_rate_error = rate_error;
-		use_edid_mode = true;
-	}
-
-	/*
-	 * TODO 1: if no default mode is present, postpone running the config
-	 * until after the LCDC channel is initialized.
-	 * TODO 2: consider registering the HDMI platform device from the LCDC
-	 * driver.
-	 */
-	if (!found && hdmi->entity.def_mode.xres != 0) {
-		found = &hdmi->entity.def_mode;
-		found_rate_error = sh_hdmi_rate_error(hdmi, found, hdmi_rate,
-						      parent_rate);
-	}
-
-	/* No cookie today */
-	if (!found)
-		return -ENXIO;
-
-	if (found->xres == 640 && found->yres == 480 && found->refresh == 60)
-		hdmi->preprogrammed_vic = 1;
-	else if (found->xres == 720 && found->yres == 480 && found->refresh == 60)
-		hdmi->preprogrammed_vic = 2;
-	else if (found->xres == 720 && found->yres == 576 && found->refresh == 50)
-		hdmi->preprogrammed_vic = 17;
-	else if (found->xres == 1280 && found->yres == 720 && found->refresh == 60)
-		hdmi->preprogrammed_vic = 4;
-	else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 24)
-		hdmi->preprogrammed_vic = 32;
-	else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 50)
-		hdmi->preprogrammed_vic = 31;
-	else if (found->xres == 1920 && found->yres == 1080 && found->refresh == 60)
-		hdmi->preprogrammed_vic = 16;
-	else
-		hdmi->preprogrammed_vic = 0;
-
-	dev_dbg(hdmi->dev, "Using %s %s mode %ux%u@%uHz (%luHz), "
-		"clock error %luHz\n", use_edid_mode ? "EDID" : "default",
-		hdmi->preprogrammed_vic ? "VIC" : "external", found->xres,
-		found->yres, found->refresh, PICOS2KHZ(found->pixclock) * 1000,
-		found_rate_error);
-
-	hdmi->mode = *found;
-	sh_hdmi_external_video_param(hdmi);
-
-	return 0;
-}
-
-static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
-{
-	struct sh_hdmi *hdmi = dev_id;
-	u8 status1, status2, mask1, mask2;
-
-	/* mode_b and PLLA and PLLB reset */
-	hdmi_bit_set(hdmi, 0xFC, 0x2C, HDMI_SYSTEM_CTRL);
-
-	/* How long shall reset be held? */
-	udelay(10);
-
-	/* mode_b and PLLA and PLLB reset release */
-	hdmi_bit_set(hdmi, 0xFC, 0x20, HDMI_SYSTEM_CTRL);
-
-	status1 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_1);
-	status2 = hdmi_read(hdmi, HDMI_INTERRUPT_STATUS_2);
-
-	mask1 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_1);
-	mask2 = hdmi_read(hdmi, HDMI_INTERRUPT_MASK_2);
-
-	/* Correct would be to ack only set bits, but the datasheet requires 0xff */
-	hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_1);
-	hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_2);
-
-	if (printk_ratelimit())
-		dev_dbg(hdmi->dev, "IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
-			irq, status1, mask1, status2, mask2);
-
-	if (!((status1 & mask1) | (status2 & mask2))) {
-		return IRQ_NONE;
-	} else if (status1 & 0xc0) {
-		u8 msens;
-
-		/* Datasheet specifies 10ms... */
-		udelay(500);
-
-		msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
-		dev_dbg(hdmi->dev, "MSENS 0x%x\n", msens);
-		/* Check, if hot plug & MSENS pin status are both high */
-		if ((msens & 0xC0) == 0xC0) {
-			/* Display plug in */
-			hdmi->edid_segment_nr = 0;
-			hdmi->edid_block_addr = 0;
-			hdmi->edid_blocks = 0;
-			hdmi->hp_state = HDMI_HOTPLUG_CONNECTED;
-
-			/* Set EDID word address  */
-			hdmi_write(hdmi, 0x00, HDMI_EDID_WORD_ADDRESS);
-			/* Enable EDID interrupt */
-			hdmi_write(hdmi, 0xC6, HDMI_INTERRUPT_MASK_1);
-			/* Set EDID segment pointer - starts reading EDID */
-			hdmi_write(hdmi, 0x00, HDMI_EDID_SEGMENT_POINTER);
-		} else if (!(status1 & 0x80)) {
-			/* Display unplug, beware multiple interrupts */
-			if (hdmi->hp_state != HDMI_HOTPLUG_DISCONNECTED) {
-				hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
-				schedule_delayed_work(&hdmi->edid_work, 0);
-			}
-			/* display_off will switch back to mode_a */
-		}
-	} else if (status1 & 2) {
-		/* EDID error interrupt: retry */
-		/* Set EDID word address  */
-		hdmi_write(hdmi, hdmi->edid_block_addr, HDMI_EDID_WORD_ADDRESS);
-		/* Set EDID segment pointer */
-		hdmi_write(hdmi, hdmi->edid_segment_nr, HDMI_EDID_SEGMENT_POINTER);
-	} else if (status1 & 4) {
-		/* Disable EDID interrupt */
-		hdmi_write(hdmi, 0xC0, HDMI_INTERRUPT_MASK_1);
-		schedule_delayed_work(&hdmi->edid_work, msecs_to_jiffies(10));
-	}
-
-	return IRQ_HANDLED;
-}
-
-static int sh_hdmi_display_on(struct sh_mobile_lcdc_entity *entity)
-{
-	struct sh_hdmi *hdmi = entity_to_sh_hdmi(entity);
-
-	dev_dbg(hdmi->dev, "%s(%p): state %x\n", __func__, hdmi,
-		hdmi->hp_state);
-
-	/*
-	 * hp_state can be set to
-	 * HDMI_HOTPLUG_DISCONNECTED:	on monitor unplug
-	 * HDMI_HOTPLUG_CONNECTED:	on monitor plug-in
-	 * HDMI_HOTPLUG_EDID_DONE:	on EDID read completion
-	 */
-	if (hdmi->hp_state == HDMI_HOTPLUG_EDID_DONE) {
-		/* PS mode d->e. All functions are active */
-		hdmi_bit_set(hdmi, 0xFC, 0x80, HDMI_SYSTEM_CTRL);
-		dev_dbg(hdmi->dev, "HDMI running\n");
-	}
-
-	return hdmi->hp_state == HDMI_HOTPLUG_DISCONNECTED
-		? SH_MOBILE_LCDC_DISPLAY_DISCONNECTED
-		: SH_MOBILE_LCDC_DISPLAY_CONNECTED;
-}
-
-static void sh_hdmi_display_off(struct sh_mobile_lcdc_entity *entity)
-{
-	struct sh_hdmi *hdmi = entity_to_sh_hdmi(entity);
-
-	dev_dbg(hdmi->dev, "%s(%p)\n", __func__, hdmi);
-	/* PS mode e->a */
-	hdmi_bit_set(hdmi, 0xFC, 0x10, HDMI_SYSTEM_CTRL);
-}
-
-static const struct sh_mobile_lcdc_entity_ops sh_hdmi_ops = {
-	.display_on = sh_hdmi_display_on,
-	.display_off = sh_hdmi_display_off,
-};
-
-/**
- * sh_hdmi_clk_configure() - set HDMI clock frequency and enable the clock
- * @hdmi:		driver context
- * @hdmi_rate:		HDMI clock frequency in Hz
- * @parent_rate:	if != 0 - set parent clock rate for optimal precision
- * return:		configured positive rate if successful
- *			0 if couldn't set the rate, but managed to enable the
- *			clock, negative error, if couldn't enable the clock
- */
-static long sh_hdmi_clk_configure(struct sh_hdmi *hdmi, unsigned long hdmi_rate,
-				  unsigned long parent_rate)
-{
-	int ret;
-
-	if (parent_rate && clk_get_parent(hdmi->hdmi_clk)) {
-		ret = clk_set_rate(clk_get_parent(hdmi->hdmi_clk), parent_rate);
-		if (ret < 0) {
-			dev_warn(hdmi->dev, "Cannot set parent rate %ld: %d\n", parent_rate, ret);
-			hdmi_rate = clk_round_rate(hdmi->hdmi_clk, hdmi_rate);
-		} else {
-			dev_dbg(hdmi->dev, "HDMI set parent frequency %lu\n", parent_rate);
-		}
-	}
-
-	ret = clk_set_rate(hdmi->hdmi_clk, hdmi_rate);
-	if (ret < 0) {
-		dev_warn(hdmi->dev, "Cannot set rate %ld: %d\n", hdmi_rate, ret);
-		hdmi_rate = 0;
-	} else {
-		dev_dbg(hdmi->dev, "HDMI set frequency %lu\n", hdmi_rate);
-	}
-
-	return hdmi_rate;
-}
-
-/* Hotplug interrupt occurred, read EDID */
-static void sh_hdmi_edid_work_fn(struct work_struct *work)
-{
-	struct sh_hdmi *hdmi = container_of(work, struct sh_hdmi, edid_work.work);
-	struct sh_mobile_lcdc_chan *ch = hdmi->entity.lcdc;
-	int ret;
-
-	dev_dbg(hdmi->dev, "%s(%p): begin, hotplug status %d\n", __func__, hdmi,
-		hdmi->hp_state);
-
-	if (hdmi->hp_state == HDMI_HOTPLUG_CONNECTED) {
-		unsigned long parent_rate = 0, hdmi_rate;
-
-		ret = sh_hdmi_read_edid(hdmi, &hdmi_rate, &parent_rate);
-		if (ret < 0)
-			goto out;
-
-		hdmi->hp_state = HDMI_HOTPLUG_EDID_DONE;
-
-		/* Reconfigure the clock */
-		ret = sh_hdmi_clk_configure(hdmi, hdmi_rate, parent_rate);
-		if (ret < 0)
-			goto out;
-
-		msleep(10);
-		sh_hdmi_configure(hdmi);
-		/* Switched to another (d) power-save mode */
-		msleep(10);
-
-		if (ch && ch->notify)
-			ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_CONNECT,
-				   &hdmi->mode, &hdmi->monspec);
-	} else {
-		hdmi->monspec.modedb_len = 0;
-		fb_destroy_modedb(hdmi->monspec.modedb);
-		hdmi->monspec.modedb = NULL;
-
-		if (ch && ch->notify)
-			ch->notify(ch, SH_MOBILE_LCDC_EVENT_DISPLAY_DISCONNECT,
-				   NULL, NULL);
-
-		ret = 0;
-	}
-
-out:
-	if (ret < 0 && ret != -EAGAIN)
-		hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
-
-	dev_dbg(hdmi->dev, "%s(%p): end\n", __func__, hdmi);
-}
-
-static void sh_hdmi_htop1_init(struct sh_hdmi *hdmi)
-{
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_MODE);
-	hdmi_htop1_write(hdmi, 0x0000000b, 0x0010);
-	hdmi_htop1_write(hdmi, 0x00006710, HDMI_HTOP1_HTOP_DCL_FRC_MODE);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y1_2);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB1_2);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR1_2);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_Y2_2);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CB2_2);
-	hdmi_htop1_write(hdmi, 0x01020406, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_1);
-	hdmi_htop1_write(hdmi, 0x07080806, HDMI_HTOP1_HTOP_DCL_FIL_PARA_CR2_2);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_Y1);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CB1);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CR1);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_Y2);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CB2);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_HTOP_DCL_COR_PARA_CR2);
-	hdmi_htop1_write(hdmi, 0x00000008, HDMI_HTOP1_CURRENT);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_TISEMP0_1);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_TISEMP2_C);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_PHY_TEST_MODE);
-	hdmi_htop1_write(hdmi, 0x00000081, HDMI_HTOP1_TISIDRV);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_PLLBW);
-	hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISEN);
-	hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISDREN);
-	hdmi_htop1_write(hdmi, 0x00000003, HDMI_HTOP1_ENABLE_SELECTOR);
-	hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_MACRO_RESET);
-	hdmi_htop1_write(hdmi, 0x00000016, HDMI_HTOP1_CISRANGE);
-	msleep(100);
-	hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_ENABLE_SELECTOR);
-	msleep(100);
-	hdmi_htop1_write(hdmi, 0x00000003, HDMI_HTOP1_ENABLE_SELECTOR);
-	hdmi_htop1_write(hdmi, 0x00000001, HDMI_HTOP1_MACRO_RESET);
-	hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISEN);
-	hdmi_htop1_write(hdmi, 0x0000000f, HDMI_HTOP1_TISDREN);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_VIDEO_INPUT);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_CLK_TO_PHY);
-	hdmi_htop1_write(hdmi, 0x00000000, HDMI_HTOP1_VIDEO_INPUT2);
-	hdmi_htop1_write(hdmi, 0x0000000a, HDMI_HTOP1_CLK_SET);
-}
-
-static int __init sh_hdmi_probe(struct platform_device *pdev)
-{
-	struct sh_mobile_hdmi_info *pdata = dev_get_platdata(&pdev->dev);
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	struct resource *htop1_res;
-	int irq = platform_get_irq(pdev, 0), ret;
-	struct sh_hdmi *hdmi;
-	long rate;
-
-	if (!res || !pdata || irq < 0)
-		return -ENODEV;
-
-	htop1_res = NULL;
-	if (pdata->flags & HDMI_HAS_HTOP1) {
-		htop1_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-		if (!htop1_res) {
-			dev_err(&pdev->dev, "htop1 needs register base\n");
-			return -EINVAL;
-		}
-	}
-
-	hdmi = devm_kzalloc(&pdev->dev, sizeof(*hdmi), GFP_KERNEL);
-	if (!hdmi) {
-		dev_err(&pdev->dev, "Cannot allocate device data\n");
-		return -ENOMEM;
-	}
-
-	hdmi->dev = &pdev->dev;
-	hdmi->entity.owner = THIS_MODULE;
-	hdmi->entity.ops = &sh_hdmi_ops;
-	hdmi->irq = irq;
-
-	hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
-	if (IS_ERR(hdmi->hdmi_clk)) {
-		ret = PTR_ERR(hdmi->hdmi_clk);
-		dev_err(&pdev->dev, "Unable to get clock: %d\n", ret);
-		return ret;
-	}
-
-	/* select register access functions */
-	if (pdata->flags & HDMI_32BIT_REG) {
-		hdmi->write	= __hdmi_write32;
-		hdmi->read	= __hdmi_read32;
-	} else {
-		hdmi->write	= __hdmi_write8;
-		hdmi->read	= __hdmi_read8;
-	}
-
-	/* An arbitrary relaxed pixclock just to get things started: from standard 480p */
-	rate = clk_round_rate(hdmi->hdmi_clk, PICOS2KHZ(37037));
-	if (rate > 0)
-		rate = sh_hdmi_clk_configure(hdmi, rate, 0);
-
-	if (rate < 0) {
-		ret = rate;
-		goto erate;
-	}
-
-	ret = clk_prepare_enable(hdmi->hdmi_clk);
-	if (ret < 0) {
-		dev_err(hdmi->dev, "Cannot enable clock: %d\n", ret);
-		goto erate;
-	}
-
-	dev_dbg(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);
-
-	if (!request_mem_region(res->start, resource_size(res), dev_name(&pdev->dev))) {
-		dev_err(&pdev->dev, "HDMI register region already claimed\n");
-		ret = -EBUSY;
-		goto ereqreg;
-	}
-
-	hdmi->base = ioremap(res->start, resource_size(res));
-	if (!hdmi->base) {
-		dev_err(&pdev->dev, "HDMI register region already claimed\n");
-		ret = -ENOMEM;
-		goto emap;
-	}
-
-	platform_set_drvdata(pdev, &hdmi->entity);
-
-	INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
-
-	pm_runtime_enable(&pdev->dev);
-	pm_runtime_get_sync(&pdev->dev);
-
-	/* init interrupt polarity */
-	if (pdata->flags & HDMI_OUTPUT_PUSH_PULL)
-		hdmi_bit_set(hdmi, 0x02, 0x02, HDMI_SYSTEM_CTRL);
-
-	if (pdata->flags & HDMI_OUTPUT_POLARITY_HI)
-		hdmi_bit_set(hdmi, 0x01, 0x01, HDMI_SYSTEM_CTRL);
-
-	/* enable htop1 register if needed */
-	if (htop1_res) {
-		hdmi->htop1 = ioremap(htop1_res->start, resource_size(htop1_res));
-		if (!hdmi->htop1) {
-			dev_err(&pdev->dev, "control register region already claimed\n");
-			ret = -ENOMEM;
-			goto emap_htop1;
-		}
-		sh_hdmi_htop1_init(hdmi);
-	}
-
-	/* Product and revision IDs are 0 in sh-mobile version */
-	dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
-		 hdmi_read(hdmi, HDMI_PRODUCT_ID), hdmi_read(hdmi, HDMI_REVISION_ID));
-
-	ret = request_irq(irq, sh_hdmi_hotplug, 0,
-			  dev_name(&pdev->dev), hdmi);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Unable to request irq: %d\n", ret);
-		goto ereqirq;
-	}
-
-	ret = snd_soc_register_codec(&pdev->dev,
-			&soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "codec registration failed\n");
-		goto ecodec;
-	}
-
-	return 0;
-
-ecodec:
-	free_irq(irq, hdmi);
-ereqirq:
-	if (hdmi->htop1)
-		iounmap(hdmi->htop1);
-emap_htop1:
-	pm_runtime_put(&pdev->dev);
-	pm_runtime_disable(&pdev->dev);
-	iounmap(hdmi->base);
-emap:
-	release_mem_region(res->start, resource_size(res));
-ereqreg:
-	clk_disable_unprepare(hdmi->hdmi_clk);
-erate:
-	clk_put(hdmi->hdmi_clk);
-
-	return ret;
-}
-
-static int __exit sh_hdmi_remove(struct platform_device *pdev)
-{
-	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	snd_soc_unregister_codec(&pdev->dev);
-
-	/* No new work will be scheduled, wait for running ISR */
-	free_irq(hdmi->irq, hdmi);
-	/* Wait for already scheduled work */
-	cancel_delayed_work_sync(&hdmi->edid_work);
-	pm_runtime_put(&pdev->dev);
-	pm_runtime_disable(&pdev->dev);
-	clk_disable_unprepare(hdmi->hdmi_clk);
-	clk_put(hdmi->hdmi_clk);
-	if (hdmi->htop1)
-		iounmap(hdmi->htop1);
-	iounmap(hdmi->base);
-	release_mem_region(res->start, resource_size(res));
-
-	return 0;
-}
-
-static int sh_hdmi_suspend(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-
-	disable_irq(hdmi->irq);
-	/* Wait for already scheduled work */
-	cancel_delayed_work_sync(&hdmi->edid_work);
-	return 0;
-}
-
-static int sh_hdmi_resume(struct device *dev)
-{
-	struct platform_device *pdev = to_platform_device(dev);
-	struct sh_mobile_hdmi_info *pdata = dev_get_platdata(dev);
-	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
-
-	/* Re-init interrupt polarity */
-	if (pdata->flags & HDMI_OUTPUT_PUSH_PULL)
-		hdmi_bit_set(hdmi, 0x02, 0x02, HDMI_SYSTEM_CTRL);
-
-	if (pdata->flags & HDMI_OUTPUT_POLARITY_HI)
-		hdmi_bit_set(hdmi, 0x01, 0x01, HDMI_SYSTEM_CTRL);
-
-	/* Re-init htop1 */
-	if (hdmi->htop1)
-		sh_hdmi_htop1_init(hdmi);
-
-	/* Now it's safe to enable interrupts again */
-	enable_irq(hdmi->irq);
-	return 0;
-}
-
-static const struct dev_pm_ops sh_hdmi_pm_ops = {
-	.suspend	= sh_hdmi_suspend,
-	.resume		= sh_hdmi_resume,
-};
-
-static struct platform_driver sh_hdmi_driver = {
-	.remove		= __exit_p(sh_hdmi_remove),
-	.driver = {
-		.name	= "sh-mobile-hdmi",
-		.pm	= &sh_hdmi_pm_ops,
-	},
-};
-
-module_platform_driver_probe(sh_hdmi_driver, sh_hdmi_probe);
-
-MODULE_AUTHOR("Guennadi Liakhovetski <g.liakhovetski@gmx.de>");
-MODULE_DESCRIPTION("SuperH / ARM-shmobile HDMI driver");
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/fbdev/simplefb.c b/drivers/video/fbdev/simplefb.c
index 52c5c7e63b52..48ccf6db62a2 100644
--- a/drivers/video/fbdev/simplefb.c
+++ b/drivers/video/fbdev/simplefb.c
@@ -28,7 +28,10 @@
 #include <linux/platform_device.h>
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include <linux/of.h>
 #include <linux/of_platform.h>
+#include <linux/parser.h>
+#include <linux/regulator/consumer.h>
 
 static struct fb_fix_screeninfo simplefb_fix = {
 	.id		= "simple",
@@ -174,6 +177,10 @@ struct simplefb_par {
 	int clk_count;
 	struct clk **clks;
 #endif
+#if defined CONFIG_OF && defined CONFIG_REGULATOR
+	u32 regulator_count;
+	struct regulator **regulators;
+#endif
 };
 
 #if defined CONFIG_OF && defined CONFIG_COMMON_CLK
@@ -269,6 +276,110 @@ static int simplefb_clocks_init(struct simplefb_par *par,
 static void simplefb_clocks_destroy(struct simplefb_par *par) { }
 #endif
 
+#if defined CONFIG_OF && defined CONFIG_REGULATOR
+
+#define SUPPLY_SUFFIX "-supply"
+
+/*
+ * Regulator handling code.
+ *
+ * Here we handle the num-supplies and vin*-supply properties of our
+ * "simple-framebuffer" dt node. This is necessary so that we can make sure
+ * that any regulators needed by the display hardware that the bootloader
+ * set up for us (and for which it provided a simplefb dt node), stay up,
+ * for the life of the simplefb driver.
+ *
+ * When the driver unloads, we cleanly disable, and then release the
+ * regulators.
+ *
+ * We only complain about errors here, no action is taken as the most likely
+ * error can only happen due to a mismatch between the bootloader which set
+ * up simplefb, and the regulator definitions in the device tree. Chances are
+ * that there are no adverse effects, and if there are, a clean teardown of
+ * the fb probe will not help us much either. So just complain and carry on,
+ * and hope that the user actually gets a working fb at the end of things.
+ */
+static int simplefb_regulators_init(struct simplefb_par *par,
+	struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct property *prop;
+	struct regulator *regulator;
+	const char *p;
+	int count = 0, i = 0, ret;
+
+	if (dev_get_platdata(&pdev->dev) || !np)
+		return 0;
+
+	/* Count the number of regulator supplies */
+	for_each_property_of_node(np, prop) {
+		p = strstr(prop->name, SUPPLY_SUFFIX);
+		if (p && p != prop->name)
+			count++;
+	}
+
+	if (!count)
+		return 0;
+
+	par->regulators = devm_kcalloc(&pdev->dev, count,
+				       sizeof(struct regulator *), GFP_KERNEL);
+	if (!par->regulators)
+		return -ENOMEM;
+
+	/* Get all the regulators */
+	for_each_property_of_node(np, prop) {
+		char name[32]; /* 32 is max size of property name */
+
+		p = strstr(prop->name, SUPPLY_SUFFIX);
+		if (!p || p == prop->name)
+			continue;
+
+		strlcpy(name, prop->name,
+			strlen(prop->name) - strlen(SUPPLY_SUFFIX) + 1);
+		regulator = devm_regulator_get_optional(&pdev->dev, name);
+		if (IS_ERR(regulator)) {
+			if (PTR_ERR(regulator) == -EPROBE_DEFER)
+				return -EPROBE_DEFER;
+			dev_err(&pdev->dev, "regulator %s not found: %ld\n",
+				name, PTR_ERR(regulator));
+			continue;
+		}
+		par->regulators[i++] = regulator;
+	}
+	par->regulator_count = i;
+
+	/* Enable all the regulators */
+	for (i = 0; i < par->regulator_count; i++) {
+		ret = regulator_enable(par->regulators[i]);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"failed to enable regulator %d: %d\n",
+				i, ret);
+			devm_regulator_put(par->regulators[i]);
+			par->regulators[i] = NULL;
+		}
+	}
+
+	return 0;
+}
+
+static void simplefb_regulators_destroy(struct simplefb_par *par)
+{
+	int i;
+
+	if (!par->regulators)
+		return;
+
+	for (i = 0; i < par->regulator_count; i++)
+		if (par->regulators[i])
+			regulator_disable(par->regulators[i]);
+}
+#else
+static int simplefb_regulators_init(struct simplefb_par *par,
+	struct platform_device *pdev) { return 0; }
+static void simplefb_regulators_destroy(struct simplefb_par *par) { }
+#endif
+
 static int simplefb_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -340,6 +451,10 @@ static int simplefb_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto error_unmap;
 
+	ret = simplefb_regulators_init(par, pdev);
+	if (ret < 0)
+		goto error_clocks;
+
 	dev_info(&pdev->dev, "framebuffer at 0x%lx, 0x%x bytes, mapped to 0x%p\n",
 			     info->fix.smem_start, info->fix.smem_len,
 			     info->screen_base);
@@ -351,13 +466,15 @@ static int simplefb_probe(struct platform_device *pdev)
 	ret = register_framebuffer(info);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Unable to register simplefb: %d\n", ret);
-		goto error_clocks;
+		goto error_regulators;
 	}
 
 	dev_info(&pdev->dev, "fb%d: simplefb registered!\n", info->node);
 
 	return 0;
 
+error_regulators:
+	simplefb_regulators_destroy(par);
 error_clocks:
 	simplefb_clocks_destroy(par);
 error_unmap:
@@ -373,6 +490,7 @@ static int simplefb_remove(struct platform_device *pdev)
 	struct simplefb_par *par = info->par;
 
 	unregister_framebuffer(info);
+	simplefb_regulators_destroy(par);
 	simplefb_clocks_destroy(par);
 	framebuffer_release(info);
 
diff --git a/drivers/video/fbdev/sm712fb.c b/drivers/video/fbdev/sm712fb.c
index 629bfa2d2f51..86ae1d4556fc 100644
--- a/drivers/video/fbdev/sm712fb.c
+++ b/drivers/video/fbdev/sm712fb.c
@@ -28,9 +28,7 @@
 #include <linux/console.h>
 #include <linux/screen_info.h>
 
-#ifdef CONFIG_PM
 #include <linux/pm.h>
-#endif
 
 #include "sm712.h"
 
@@ -1545,8 +1543,7 @@ static void smtcfb_pci_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 }
 
-#ifdef CONFIG_PM
-static int smtcfb_pci_suspend(struct device *device)
+static int __maybe_unused smtcfb_pci_suspend(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct smtcfb_info *sfb;
@@ -1569,7 +1566,7 @@ static int smtcfb_pci_suspend(struct device *device)
 	return 0;
 }
 
-static int smtcfb_pci_resume(struct device *device)
+static int __maybe_unused smtcfb_pci_resume(struct device *device)
 {
 	struct pci_dev *pdev = to_pci_dev(device);
 	struct smtcfb_info *sfb;
@@ -1610,20 +1607,13 @@ static int smtcfb_pci_resume(struct device *device)
 }
 
 static SIMPLE_DEV_PM_OPS(sm7xx_pm_ops, smtcfb_pci_suspend, smtcfb_pci_resume);
-#define SM7XX_PM_OPS (&sm7xx_pm_ops)
-
-#else  /* !CONFIG_PM */
-
-#define SM7XX_PM_OPS NULL
-
-#endif /* !CONFIG_PM */
 
 static struct pci_driver smtcfb_driver = {
 	.name = "smtcfb",
 	.id_table = smtcfb_pci_table,
 	.probe = smtcfb_pci_probe,
 	.remove = smtcfb_pci_remove,
-	.driver.pm  = SM7XX_PM_OPS,
+	.driver.pm  = &sm7xx_pm_ops,
 };
 
 static int __init sm712fb_init(void)
diff --git a/drivers/video/fbdev/via/via-gpio.c b/drivers/video/fbdev/via/via-gpio.c
index 6f433b8cee12..1e89c3434071 100644
--- a/drivers/video/fbdev/via/via-gpio.c
+++ b/drivers/video/fbdev/via/via-gpio.c
@@ -6,7 +6,7 @@
  */
 
 #include <linux/spinlock.h>
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/platform_device.h>
 #include <linux/via-core.h>
 #include <linux/via-gpio.h>
@@ -83,9 +83,7 @@ struct viafb_gpio_cfg {
 static void via_gpio_set(struct gpio_chip *chip, unsigned int nr,
 			 int value)
 {
-	struct viafb_gpio_cfg *cfg = container_of(chip,
-						  struct viafb_gpio_cfg,
-						  gpio_chip);
+	struct viafb_gpio_cfg *cfg = gpiochip_get_data(chip);
 	u8 reg;
 	struct viafb_gpio *gpio;
 	unsigned long flags;
@@ -115,9 +113,7 @@ static int via_gpio_dir_out(struct gpio_chip *chip, unsigned int nr,
  */
 static int via_gpio_dir_input(struct gpio_chip *chip, unsigned int nr)
 {
-	struct viafb_gpio_cfg *cfg = container_of(chip,
-						  struct viafb_gpio_cfg,
-						  gpio_chip);
+	struct viafb_gpio_cfg *cfg = gpiochip_get_data(chip);
 	struct viafb_gpio *gpio;
 	unsigned long flags;
 
@@ -131,9 +127,7 @@ static int via_gpio_dir_input(struct gpio_chip *chip, unsigned int nr)
 
 static int via_gpio_get(struct gpio_chip *chip, unsigned int nr)
 {
-	struct viafb_gpio_cfg *cfg = container_of(chip,
-						  struct viafb_gpio_cfg,
-						  gpio_chip);
+	struct viafb_gpio_cfg *cfg = gpiochip_get_data(chip);
 	u8 reg;
 	struct viafb_gpio *gpio;
 	unsigned long flags;
@@ -142,7 +136,7 @@ static int via_gpio_get(struct gpio_chip *chip, unsigned int nr)
 	gpio = cfg->active_gpios[nr];
 	reg = via_read_reg(VIASR, gpio->vg_port_index);
 	spin_unlock_irqrestore(&cfg->vdev->reg_lock, flags);
-	return reg & (0x04 << gpio->vg_mask_shift);
+	return !!(reg & (0x04 << gpio->vg_mask_shift));
 }
 
 
@@ -255,7 +249,8 @@ static int viafb_gpio_probe(struct platform_device *platdev)
 	 * Get registered.
 	 */
 	viafb_gpio_config.gpio_chip.base = -1;  /* Dynamic */
-	ret = gpiochip_add(&viafb_gpio_config.gpio_chip);
+	ret = gpiochip_add_data(&viafb_gpio_config.gpio_chip,
+				&viafb_gpio_config);
 	if (ret) {
 		printk(KERN_ERR "viafb: failed to add gpios (%d)\n", ret);
 		viafb_gpio_config.gpio_chip.ngpio = 0;
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index 7efc32945810..0c3691f46575 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -209,8 +209,8 @@ static unsigned leak_balloon(struct virtio_balloon *vb, size_t num)
 	 */
 	if (vb->num_pfns != 0)
 		tell_host(vb, vb->deflate_vq);
-	mutex_unlock(&vb->balloon_lock);
 	release_pages_balloon(vb);
+	mutex_unlock(&vb->balloon_lock);
 	return num_freed_pages;
 }
 
@@ -388,7 +388,7 @@ static int init_vqs(struct virtio_balloon *vb)
 {
 	struct virtqueue *vqs[3];
 	vq_callback_t *callbacks[] = { balloon_ack, balloon_ack, stats_request };
-	const char *names[] = { "inflate", "deflate", "stats" };
+	static const char * const names[] = { "inflate", "deflate", "stats" };
 	int err, nvqs;
 
 	/*
diff --git a/drivers/virtio/virtio_input.c b/drivers/virtio/virtio_input.c
index c96944b59856..350a2a5a49db 100644
--- a/drivers/virtio/virtio_input.c
+++ b/drivers/virtio/virtio_input.c
@@ -170,7 +170,7 @@ static int virtinput_init_vqs(struct virtio_input *vi)
 	struct virtqueue *vqs[2];
 	vq_callback_t *cbs[] = { virtinput_recv_events,
 				 virtinput_recv_status };
-	static const char *names[] = { "events", "status" };
+	static const char * const names[] = { "events", "status" };
 	int err;
 
 	err = vi->vdev->config->find_vqs(vi->vdev, 2, vqs, cbs, names);
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index f499d9da7237..745c6ee1bb3e 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -482,7 +482,7 @@ error_available:
 static int vm_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 		       struct virtqueue *vqs[],
 		       vq_callback_t *callbacks[],
-		       const char *names[])
+		       const char * const names[])
 {
 	struct virtio_mmio_device *vm_dev = to_virtio_mmio_device(vdev);
 	unsigned int irq = platform_get_irq(vm_dev->pdev, 0);
diff --git a/drivers/virtio/virtio_pci_common.c b/drivers/virtio/virtio_pci_common.c
index 78f804af6c20..f6bed86c17f9 100644
--- a/drivers/virtio/virtio_pci_common.c
+++ b/drivers/virtio/virtio_pci_common.c
@@ -296,7 +296,7 @@ void vp_del_vqs(struct virtio_device *vdev)
 static int vp_try_to_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 			      struct virtqueue *vqs[],
 			      vq_callback_t *callbacks[],
-			      const char *names[],
+			      const char * const names[],
 			      bool use_msix,
 			      bool per_vq_vectors)
 {
@@ -376,7 +376,7 @@ error_find:
 int vp_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 		struct virtqueue *vqs[],
 		vq_callback_t *callbacks[],
-		const char *names[])
+		const char * const names[])
 {
 	int err;
 
@@ -545,6 +545,7 @@ err_enable_device:
 static void virtio_pci_remove(struct pci_dev *pci_dev)
 {
 	struct virtio_pci_device *vp_dev = pci_get_drvdata(pci_dev);
+	struct device *dev = get_device(&vp_dev->vdev.dev);
 
 	unregister_virtio_device(&vp_dev->vdev);
 
@@ -554,6 +555,7 @@ static void virtio_pci_remove(struct pci_dev *pci_dev)
 		virtio_pci_modern_remove(vp_dev);
 
 	pci_disable_device(pci_dev);
+	put_device(dev);
 }
 
 static struct pci_driver virtio_pci_driver = {
diff --git a/drivers/virtio/virtio_pci_common.h b/drivers/virtio/virtio_pci_common.h
index b976d968e793..2cc252270b2d 100644
--- a/drivers/virtio/virtio_pci_common.h
+++ b/drivers/virtio/virtio_pci_common.h
@@ -139,7 +139,7 @@ void vp_del_vqs(struct virtio_device *vdev);
 int vp_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 		       struct virtqueue *vqs[],
 		       vq_callback_t *callbacks[],
-		       const char *names[]);
+		       const char * const names[]);
 const char *vp_bus_name(struct virtio_device *vdev);
 
 /* Setup the affinity for a virtqueue:
diff --git a/drivers/virtio/virtio_pci_modern.c b/drivers/virtio/virtio_pci_modern.c
index 8e5cf194cc0b..7760fc1a2218 100644
--- a/drivers/virtio/virtio_pci_modern.c
+++ b/drivers/virtio/virtio_pci_modern.c
@@ -418,7 +418,7 @@ err_new_queue:
 static int vp_modern_find_vqs(struct virtio_device *vdev, unsigned nvqs,
 			      struct virtqueue *vqs[],
 			      vq_callback_t *callbacks[],
-			      const char *names[])
+			      const char * const names[])
 {
 	struct virtio_pci_device *vp_dev = to_vp_device(vdev);
 	struct virtqueue *vq;
@@ -679,7 +679,7 @@ int virtio_pci_modern_probe(struct virtio_pci_device *vp_dev)
 
 	pci_read_config_dword(pci_dev,
 			      notify + offsetof(struct virtio_pci_notify_cap,
-						cap.length),
+						cap.offset),
 			      &notify_offset);
 
 	/* We don't know how many VQs we'll map, ahead of the time.
diff --git a/drivers/virtio/virtio_ring.c b/drivers/virtio/virtio_ring.c
index ee663c458b20..e12e385f7ac3 100644
--- a/drivers/virtio/virtio_ring.c
+++ b/drivers/virtio/virtio_ring.c
@@ -517,10 +517,10 @@ void *virtqueue_get_buf(struct virtqueue *_vq, unsigned int *len)
 	/* If we expect an interrupt for the next entry, tell host
 	 * by writing event index and flush out the write before
 	 * the read in the next get_buf call. */
-	if (!(vq->avail_flags_shadow & VRING_AVAIL_F_NO_INTERRUPT)) {
-		vring_used_event(&vq->vring) = cpu_to_virtio16(_vq->vdev, vq->last_used_idx);
-		virtio_mb(vq->weak_barriers);
-	}
+	if (!(vq->avail_flags_shadow & VRING_AVAIL_F_NO_INTERRUPT))
+		virtio_store_mb(vq->weak_barriers,
+				&vring_used_event(&vq->vring),
+				cpu_to_virtio16(_vq->vdev, vq->last_used_idx));
 
 #ifdef DEBUG
 	vq->last_add_time_valid = false;
@@ -653,8 +653,11 @@ bool virtqueue_enable_cb_delayed(struct virtqueue *_vq)
 	}
 	/* TODO: tune this threshold */
 	bufs = (u16)(vq->avail_idx_shadow - vq->last_used_idx) * 3 / 4;
-	vring_used_event(&vq->vring) = cpu_to_virtio16(_vq->vdev, vq->last_used_idx + bufs);
-	virtio_mb(vq->weak_barriers);
+
+	virtio_store_mb(vq->weak_barriers,
+			&vring_used_event(&vq->vring),
+			cpu_to_virtio16(_vq->vdev, vq->last_used_idx + bufs));
+
 	if (unlikely((u16)(virtio16_to_cpu(_vq->vdev, vq->vring.used->idx) - vq->last_used_idx) > bufs)) {
 		END_USE(vq);
 		return false;
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index 1c427beffadd..80825a7e8e48 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -46,6 +46,13 @@ config WATCHDOG_NOWAYOUT
 	  get killed. If you say Y here, the watchdog cannot be stopped once
 	  it has been started.
 
+config WATCHDOG_SYSFS
+	bool "Read different watchdog information through sysfs"
+	default n
+	help
+	  Say Y here if you want to enable watchdog device status read through
+	  sysfs attributes.
+
 #
 # General Watchdog drivers
 #
@@ -135,6 +142,17 @@ config MENF21BMC_WATCHDOG
 	  This driver can also be built as a module. If so the module
 	  will be called menf21bmc_wdt.
 
+config TANGOX_WATCHDOG
+	tristate "Sigma Designs SMP86xx/SMP87xx watchdog"
+	select WATCHDOG_CORE
+	depends on ARCH_TANGO || COMPILE_TEST
+	depends on HAS_IOMEM
+	help
+	  Support for the watchdog in Sigma Designs SMP86xx (tango3)
+	  and SMP87xx (tango4) family chips.
+
+	  This driver can be built as a module. The module name is tangox_wdt.
+
 config WM831X_WATCHDOG
 	tristate "WM831x watchdog"
 	depends on MFD_WM831X
@@ -161,6 +179,17 @@ config XILINX_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called of_xilinx_wdt.
 
+config ZIIRAVE_WATCHDOG
+	tristate "Zodiac RAVE Watchdog Timer"
+	depends on I2C
+	select WATCHDOG_CORE
+	help
+	  Watchdog driver for the Zodiac Aerospace RAVE Switch Watchdog
+	  Processor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ziirave_wdt.
+
 # ALPHA Architecture
 
 # ARM Architecture
@@ -173,6 +202,16 @@ config ARM_SP805_WATCHDOG
 	  ARM Primecell SP805 Watchdog timer. This will reboot your system when
 	  the timeout is reached.
 
+config ASM9260_WATCHDOG
+	tristate "Alphascale ASM9260 watchdog"
+	depends on MACH_ASM9260
+	depends on OF
+	select WATCHDOG_CORE
+	select RESET_CONTROLLER
+	help
+	  Watchdog timer embedded into Alphascale asm9260 chips. This will reboot your
+	  system when the timeout is reached.
+
 config AT91RM9200_WATCHDOG
 	tristate "AT91RM9200 watchdog"
 	depends on SOC_AT91RM9200 && MFD_SYSCON
@@ -426,6 +465,16 @@ config NUC900_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called nuc900_wdt.
 
+config TS4800_WATCHDOG
+	tristate "TS-4800 Watchdog"
+	depends on HAS_IOMEM && OF
+	select WATCHDOG_CORE
+	select MFD_SYSCON
+	help
+	  Technologic Systems TS-4800 has watchdog timer implemented in
+	  an external FPGA. Say Y here if you want to support for the
+	  watchdog timer on TS-4800 board.
+
 config TS72XX_WATCHDOG
 	tristate "TS-72XX SBC Watchdog"
 	depends on MACH_TS72XX
@@ -570,6 +619,7 @@ config DIGICOLOR_WATCHDOG
 config LPC18XX_WATCHDOG
 	tristate "LPC18xx/43xx Watchdog"
 	depends on ARCH_LPC18XX || COMPILE_TEST
+	depends on HAS_IOMEM
 	select WATCHDOG_CORE
 	help
 	  Say Y here if to include support for the watchdog timer
@@ -578,6 +628,16 @@ config LPC18XX_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called lpc18xx_wdt.
 
+config ATLAS7_WATCHDOG
+	tristate "CSRatlas7 watchdog"
+	depends on ARCH_ATLAS7
+	help
+	  Say Y here to include Watchdog timer support for the watchdog
+	  existing on the CSRatlas7 series platforms.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called atlas7_wdt.
+
 # AVR32 Architecture
 
 config AT32AP700X_WDT
@@ -1316,6 +1376,7 @@ config BCM_KONA_WDT_DEBUG
 config BCM7038_WDT
 	tristate "BCM7038 Watchdog"
 	select WATCHDOG_CORE
+	depends on HAS_IOMEM
 	help
 	 Watchdog driver for the built-in hardware in Broadcom 7038 SoCs.
 
@@ -1325,6 +1386,7 @@ config IMGPDC_WDT
 	tristate "Imagination Technologies PDC Watchdog Timer"
 	depends on HAS_IOMEM
 	depends on METAG || MIPS || COMPILE_TEST
+	select WATCHDOG_CORE
 	help
 	  Driver for Imagination Technologies PowerDown Controller
 	  Watchdog Timer.
@@ -1345,6 +1407,13 @@ config RALINK_WDT
 	help
 	  Hardware driver for the Ralink SoC Watchdog Timer.
 
+config MT7621_WDT
+	tristate "Mediatek SoC watchdog"
+	select WATCHDOG_CORE
+	depends on SOC_MT7620 || SOC_MT7621
+	help
+	  Hardware driver for the Mediatek/Ralink MT7621/8 SoC Watchdog Timer.
+
 # PARISC Architecture
 
 # POWERPC Architecture
@@ -1500,6 +1569,17 @@ config WATCHDOG_RIO
 	  machines.  The watchdog timeout period is normally one minute but
 	  can be changed with a boot-time parameter.
 
+config WATCHDOG_SUN4V
+	tristate "Sun4v Watchdog support"
+	select WATCHDOG_CORE
+	depends on SPARC64
+	help
+	  Say Y here to support the hypervisor watchdog capability embedded
+	  in the SPARC sun4v architecture.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called sun4v_wdt.
+
 # XTENSA Architecture
 
 # Xen Architecture
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 53d4827ddfe1..f6a6a387c6c7 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_USBPCWATCHDOG) += pcwd_usb.o
 
 # ARM Architecture
 obj-$(CONFIG_ARM_SP805_WATCHDOG) += sp805_wdt.o
+obj-$(CONFIG_ASM9260_WATCHDOG) += asm9260_wdt.o
 obj-$(CONFIG_AT91RM9200_WATCHDOG) += at91rm9200_wdt.o
 obj-$(CONFIG_AT91SAM9X_WATCHDOG) += at91sam9_wdt.o
 obj-$(CONFIG_CADENCE_WATCHDOG) += cadence_wdt.o
@@ -53,6 +54,7 @@ obj-$(CONFIG_RN5T618_WATCHDOG) += rn5t618_wdt.o
 obj-$(CONFIG_COH901327_WATCHDOG) += coh901327_wdt.o
 obj-$(CONFIG_STMP3XXX_RTC_WATCHDOG) += stmp3xxx_rtc_wdt.o
 obj-$(CONFIG_NUC900_WATCHDOG) += nuc900_wdt.o
+obj-$(CONFIG_TS4800_WATCHDOG) += ts4800_wdt.o
 obj-$(CONFIG_TS72XX_WATCHDOG) += ts72xx_wdt.o
 obj-$(CONFIG_IMX2_WDT) += imx2_wdt.o
 obj-$(CONFIG_UX500_WATCHDOG) += ux500_wdt.o
@@ -69,6 +71,7 @@ obj-$(CONFIG_MEDIATEK_WATCHDOG) += mtk_wdt.o
 obj-$(CONFIG_DIGICOLOR_WATCHDOG) += digicolor_wdt.o
 obj-$(CONFIG_LPC18XX_WATCHDOG) += lpc18xx_wdt.o
 obj-$(CONFIG_BCM7038_WDT) += bcm7038_wdt.o
+obj-$(CONFIG_ATLAS7_WATCHDOG) += atlas7_wdt.o
 
 # AVR32 Architecture
 obj-$(CONFIG_AT32AP700X_WDT) += at32ap700x_wdt.o
@@ -149,6 +152,7 @@ octeon-wdt-y := octeon-wdt-main.o octeon-wdt-nmi.o
 obj-$(CONFIG_LANTIQ_WDT) += lantiq_wdt.o
 obj-$(CONFIG_RALINK_WDT) += rt2880_wdt.o
 obj-$(CONFIG_IMGPDC_WDT) += imgpdc_wdt.o
+obj-$(CONFIG_MT7621_WDT) += mt7621_wdt.o
 
 # PARISC Architecture
 
@@ -175,6 +179,7 @@ obj-$(CONFIG_SH_WDT) += shwdt.o
 
 obj-$(CONFIG_WATCHDOG_RIO)		+= riowd.o
 obj-$(CONFIG_WATCHDOG_CP1XXX)		+= cpwd.o
+obj-$(CONFIG_WATCHDOG_SUN4V)		+= sun4v_wdt.o
 
 # XTENSA Architecture
 
@@ -187,8 +192,10 @@ obj-$(CONFIG_DA9055_WATCHDOG) += da9055_wdt.o
 obj-$(CONFIG_DA9062_WATCHDOG) += da9062_wdt.o
 obj-$(CONFIG_DA9063_WATCHDOG) += da9063_wdt.o
 obj-$(CONFIG_GPIO_WATCHDOG)	+= gpio_wdt.o
+obj-$(CONFIG_TANGOX_WATCHDOG) += tangox_wdt.o
 obj-$(CONFIG_WM831X_WATCHDOG) += wm831x_wdt.o
 obj-$(CONFIG_WM8350_WATCHDOG) += wm8350_wdt.o
 obj-$(CONFIG_MAX63XX_WATCHDOG) += max63xx_wdt.o
+obj-$(CONFIG_ZIIRAVE_WATCHDOG) += ziirave_wdt.o
 obj-$(CONFIG_SOFT_WATCHDOG) += softdog.o
 obj-$(CONFIG_MENF21BMC_WATCHDOG) += menf21bmc_wdt.o
diff --git a/drivers/watchdog/asm9260_wdt.c b/drivers/watchdog/asm9260_wdt.c
new file mode 100644
index 000000000000..c9686b2fdafd
--- /dev/null
+++ b/drivers/watchdog/asm9260_wdt.c
@@ -0,0 +1,403 @@
+/*
+ * Watchdog driver for Alphascale ASM9260.
+ *
+ * Copyright (c) 2014 Oleksij Rempel <linux@rempel-privat.de>
+ *
+ * Licensed under GPLv2 or later.
+ */
+
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/reset.h>
+#include <linux/watchdog.h>
+
+#define CLOCK_FREQ	1000000
+
+/* Watchdog Mode register */
+#define HW_WDMOD			0x00
+/* Wake interrupt. Set by HW, can't be cleared. */
+#define BM_MOD_WDINT			BIT(3)
+/* This bit set if timeout reached. Cleared by SW. */
+#define BM_MOD_WDTOF			BIT(2)
+/* HW Reset on timeout */
+#define BM_MOD_WDRESET			BIT(1)
+/* WD enable */
+#define BM_MOD_WDEN			BIT(0)
+
+/*
+ * Watchdog Timer Constant register
+ * Minimal value is 0xff, the meaning of this value
+ * depends on used clock: T = WDCLK * (0xff + 1) * 4
+ */
+#define HW_WDTC				0x04
+#define BM_WDTC_MAX(freq)		(0x7fffffff / (freq))
+
+/* Watchdog Feed register */
+#define HW_WDFEED			0x08
+
+/* Watchdog Timer Value register */
+#define HW_WDTV				0x0c
+
+#define ASM9260_WDT_DEFAULT_TIMEOUT	30
+
+enum asm9260_wdt_mode {
+	HW_RESET,
+	SW_RESET,
+	DEBUG,
+};
+
+struct asm9260_wdt_priv {
+	struct device		*dev;
+	struct watchdog_device	wdd;
+	struct clk		*clk;
+	struct clk		*clk_ahb;
+	struct reset_control	*rst;
+	struct notifier_block	restart_handler;
+
+	void __iomem		*iobase;
+	int			irq;
+	unsigned long		wdt_freq;
+	enum asm9260_wdt_mode	mode;
+};
+
+static int asm9260_wdt_feed(struct watchdog_device *wdd)
+{
+	struct asm9260_wdt_priv *priv = watchdog_get_drvdata(wdd);
+
+	iowrite32(0xaa, priv->iobase + HW_WDFEED);
+	iowrite32(0x55, priv->iobase + HW_WDFEED);
+
+	return 0;
+}
+
+static unsigned int asm9260_wdt_gettimeleft(struct watchdog_device *wdd)
+{
+	struct asm9260_wdt_priv *priv = watchdog_get_drvdata(wdd);
+	u32 counter;
+
+	counter = ioread32(priv->iobase + HW_WDTV);
+
+	return DIV_ROUND_CLOSEST(counter, priv->wdt_freq);
+}
+
+static int asm9260_wdt_updatetimeout(struct watchdog_device *wdd)
+{
+	struct asm9260_wdt_priv *priv = watchdog_get_drvdata(wdd);
+	u32 counter;
+
+	counter = wdd->timeout * priv->wdt_freq;
+
+	iowrite32(counter, priv->iobase + HW_WDTC);
+
+	return 0;
+}
+
+static int asm9260_wdt_enable(struct watchdog_device *wdd)
+{
+	struct asm9260_wdt_priv *priv = watchdog_get_drvdata(wdd);
+	u32 mode = 0;
+
+	if (priv->mode == HW_RESET)
+		mode = BM_MOD_WDRESET;
+
+	iowrite32(BM_MOD_WDEN | mode, priv->iobase + HW_WDMOD);
+
+	asm9260_wdt_updatetimeout(wdd);
+
+	asm9260_wdt_feed(wdd);
+
+	return 0;
+}
+
+static int asm9260_wdt_disable(struct watchdog_device *wdd)
+{
+	struct asm9260_wdt_priv *priv = watchdog_get_drvdata(wdd);
+
+	/* The only way to disable WD is to reset it. */
+	reset_control_assert(priv->rst);
+	reset_control_deassert(priv->rst);
+
+	return 0;
+}
+
+static int asm9260_wdt_settimeout(struct watchdog_device *wdd, unsigned int to)
+{
+	wdd->timeout = to;
+	asm9260_wdt_updatetimeout(wdd);
+
+	return 0;
+}
+
+static void asm9260_wdt_sys_reset(struct asm9260_wdt_priv *priv)
+{
+	/* init WD if it was not started */
+
+	iowrite32(BM_MOD_WDEN | BM_MOD_WDRESET, priv->iobase + HW_WDMOD);
+
+	iowrite32(0xff, priv->iobase + HW_WDTC);
+	/* first pass correct sequence */
+	asm9260_wdt_feed(&priv->wdd);
+	/*
+	 * Then write wrong pattern to the feed to trigger reset
+	 * ASAP.
+	 */
+	iowrite32(0xff, priv->iobase + HW_WDFEED);
+
+	mdelay(1000);
+}
+
+static irqreturn_t asm9260_wdt_irq(int irq, void *devid)
+{
+	struct asm9260_wdt_priv *priv = devid;
+	u32 stat;
+
+	stat = ioread32(priv->iobase + HW_WDMOD);
+	if (!(stat & BM_MOD_WDINT))
+		return IRQ_NONE;
+
+	if (priv->mode == DEBUG) {
+		dev_info(priv->dev, "Watchdog Timeout. Do nothing.\n");
+	} else {
+		dev_info(priv->dev, "Watchdog Timeout. Doing SW Reset.\n");
+		asm9260_wdt_sys_reset(priv);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static int asm9260_restart_handler(struct notifier_block *this,
+				   unsigned long mode, void *cmd)
+{
+	struct asm9260_wdt_priv *priv =
+		container_of(this, struct asm9260_wdt_priv, restart_handler);
+
+	asm9260_wdt_sys_reset(priv);
+
+	return NOTIFY_DONE;
+}
+
+static const struct watchdog_info asm9260_wdt_ident = {
+	.options          =     WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING
+				| WDIOF_MAGICCLOSE,
+	.identity         =	"Alphascale asm9260 Watchdog",
+};
+
+static struct watchdog_ops asm9260_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= asm9260_wdt_enable,
+	.stop		= asm9260_wdt_disable,
+	.get_timeleft	= asm9260_wdt_gettimeleft,
+	.ping		= asm9260_wdt_feed,
+	.set_timeout	= asm9260_wdt_settimeout,
+};
+
+static int asm9260_wdt_get_dt_clks(struct asm9260_wdt_priv *priv)
+{
+	int err;
+	unsigned long clk;
+
+	priv->clk = devm_clk_get(priv->dev, "mod");
+	if (IS_ERR(priv->clk)) {
+		dev_err(priv->dev, "Failed to get \"mod\" clk\n");
+		return PTR_ERR(priv->clk);
+	}
+
+	/* configure AHB clock */
+	priv->clk_ahb = devm_clk_get(priv->dev, "ahb");
+	if (IS_ERR(priv->clk_ahb)) {
+		dev_err(priv->dev, "Failed to get \"ahb\" clk\n");
+		return PTR_ERR(priv->clk_ahb);
+	}
+
+	err = clk_prepare_enable(priv->clk_ahb);
+	if (err) {
+		dev_err(priv->dev, "Failed to enable ahb_clk!\n");
+		return err;
+	}
+
+	err = clk_set_rate(priv->clk, CLOCK_FREQ);
+	if (err) {
+		clk_disable_unprepare(priv->clk_ahb);
+		dev_err(priv->dev, "Failed to set rate!\n");
+		return err;
+	}
+
+	err = clk_prepare_enable(priv->clk);
+	if (err) {
+		clk_disable_unprepare(priv->clk_ahb);
+		dev_err(priv->dev, "Failed to enable clk!\n");
+		return err;
+	}
+
+	/* wdt has internal divider */
+	clk = clk_get_rate(priv->clk);
+	if (!clk) {
+		clk_disable_unprepare(priv->clk);
+		clk_disable_unprepare(priv->clk_ahb);
+		dev_err(priv->dev, "Failed, clk is 0!\n");
+		return -EINVAL;
+	}
+
+	priv->wdt_freq = clk / 2;
+
+	return 0;
+}
+
+static void asm9260_wdt_get_dt_mode(struct asm9260_wdt_priv *priv)
+{
+	const char *tmp;
+	int ret;
+
+	/* default mode */
+	priv->mode = HW_RESET;
+
+	ret = of_property_read_string(priv->dev->of_node,
+				      "alphascale,mode", &tmp);
+	if (ret < 0)
+		return;
+
+	if (!strcmp(tmp, "hw"))
+		priv->mode = HW_RESET;
+	else if (!strcmp(tmp, "sw"))
+		priv->mode = SW_RESET;
+	else if (!strcmp(tmp, "debug"))
+		priv->mode = DEBUG;
+	else
+		dev_warn(priv->dev, "unknown reset-type: %s. Using default \"hw\" mode.",
+			 tmp);
+}
+
+static int asm9260_wdt_probe(struct platform_device *pdev)
+{
+	struct asm9260_wdt_priv *priv;
+	struct watchdog_device *wdd;
+	struct resource *res;
+	int ret;
+	const char * const mode_name[] = { "hw", "sw", "debug", };
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct asm9260_wdt_priv),
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->iobase = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(priv->iobase))
+		return PTR_ERR(priv->iobase);
+
+	ret = asm9260_wdt_get_dt_clks(priv);
+	if (ret)
+		return ret;
+
+	priv->rst = devm_reset_control_get(&pdev->dev, "wdt_rst");
+	if (IS_ERR(priv->rst))
+		return PTR_ERR(priv->rst);
+
+	wdd = &priv->wdd;
+	wdd->info = &asm9260_wdt_ident;
+	wdd->ops = &asm9260_wdt_ops;
+	wdd->min_timeout = 1;
+	wdd->max_timeout = BM_WDTC_MAX(priv->wdt_freq);
+	wdd->parent = &pdev->dev;
+
+	watchdog_set_drvdata(wdd, priv);
+
+	/*
+	 * If 'timeout-sec' unspecified in devicetree, assume a 30 second
+	 * default, unless the max timeout is less than 30 seconds, then use
+	 * the max instead.
+	 */
+	wdd->timeout = ASM9260_WDT_DEFAULT_TIMEOUT;
+	watchdog_init_timeout(wdd, 0, &pdev->dev);
+
+	asm9260_wdt_get_dt_mode(priv);
+
+	if (priv->mode != HW_RESET)
+		priv->irq = platform_get_irq(pdev, 0);
+
+	if (priv->irq > 0) {
+		/*
+		 * Not all supported platforms specify an interrupt for the
+		 * watchdog, so let's make it optional.
+		 */
+		ret = devm_request_irq(&pdev->dev, priv->irq,
+				       asm9260_wdt_irq, 0, pdev->name, priv);
+		if (ret < 0)
+			dev_warn(&pdev->dev, "failed to request IRQ\n");
+	}
+
+	ret = watchdog_register_device(wdd);
+	if (ret)
+		goto clk_off;
+
+	platform_set_drvdata(pdev, priv);
+
+	priv->restart_handler.notifier_call = asm9260_restart_handler;
+	priv->restart_handler.priority = 128;
+	ret = register_restart_handler(&priv->restart_handler);
+	if (ret)
+		dev_warn(&pdev->dev, "cannot register restart handler\n");
+
+	dev_info(&pdev->dev, "Watchdog enabled (timeout: %d sec, mode: %s)\n",
+		 wdd->timeout, mode_name[priv->mode]);
+	return 0;
+
+clk_off:
+	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(priv->clk_ahb);
+	return ret;
+}
+
+static void asm9260_wdt_shutdown(struct platform_device *pdev)
+{
+	struct asm9260_wdt_priv *priv = platform_get_drvdata(pdev);
+
+	asm9260_wdt_disable(&priv->wdd);
+}
+
+static int asm9260_wdt_remove(struct platform_device *pdev)
+{
+	struct asm9260_wdt_priv *priv = platform_get_drvdata(pdev);
+
+	asm9260_wdt_disable(&priv->wdd);
+
+	unregister_restart_handler(&priv->restart_handler);
+
+	watchdog_unregister_device(&priv->wdd);
+
+	clk_disable_unprepare(priv->clk);
+	clk_disable_unprepare(priv->clk_ahb);
+
+	return 0;
+}
+
+static const struct of_device_id asm9260_wdt_of_match[] = {
+	{ .compatible = "alphascale,asm9260-wdt"},
+	{},
+};
+MODULE_DEVICE_TABLE(of, asm9260_wdt_of_match);
+
+static struct platform_driver asm9260_wdt_driver = {
+	.driver = {
+		.name = "asm9260-wdt",
+		.owner = THIS_MODULE,
+		.of_match_table	= asm9260_wdt_of_match,
+	},
+	.probe = asm9260_wdt_probe,
+	.remove = asm9260_wdt_remove,
+	.shutdown = asm9260_wdt_shutdown,
+};
+module_platform_driver(asm9260_wdt_driver);
+
+MODULE_DESCRIPTION("asm9260 WatchDog Timer Driver");
+MODULE_AUTHOR("Oleksij Rempel <linux@rempel-privat.de>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/atlas7_wdt.c b/drivers/watchdog/atlas7_wdt.c
new file mode 100644
index 000000000000..df6d9242a319
--- /dev/null
+++ b/drivers/watchdog/atlas7_wdt.c
@@ -0,0 +1,242 @@
+/*
+ * Watchdog driver for CSR Atlas7
+ *
+ * Copyright (c) 2015 Cambridge Silicon Radio Limited, a CSR plc group company.
+ *
+ * Licensed under GPLv2.
+ */
+
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/watchdog.h>
+
+#define ATLAS7_TIMER_WDT_INDEX		5
+#define ATLAS7_WDT_DEFAULT_TIMEOUT	20
+
+#define ATLAS7_WDT_CNT_CTRL	(0 + 4 * ATLAS7_TIMER_WDT_INDEX)
+#define ATLAS7_WDT_CNT_MATCH	(0x18 + 4 * ATLAS7_TIMER_WDT_INDEX)
+#define ATLAS7_WDT_CNT		(0x48 +  4 * ATLAS7_TIMER_WDT_INDEX)
+#define ATLAS7_WDT_CNT_EN	(BIT(0) | BIT(1))
+#define ATLAS7_WDT_EN		0x64
+
+static unsigned int timeout = ATLAS7_WDT_DEFAULT_TIMEOUT;
+static bool nowayout = WATCHDOG_NOWAYOUT;
+
+module_param(timeout, uint, 0);
+module_param(nowayout, bool, 0);
+
+MODULE_PARM_DESC(timeout, "Default watchdog timeout (in seconds)");
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+			__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+struct atlas7_wdog {
+	struct device *dev;
+	void __iomem *base;
+	unsigned long tick_rate;
+	struct clk *clk;
+};
+
+static unsigned int atlas7_wdt_gettimeleft(struct watchdog_device *wdd)
+{
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+	u32 counter, match, delta;
+
+	counter = readl(wdt->base + ATLAS7_WDT_CNT);
+	match = readl(wdt->base + ATLAS7_WDT_CNT_MATCH);
+	delta = match - counter;
+
+	return  delta / wdt->tick_rate;
+}
+
+static int atlas7_wdt_ping(struct watchdog_device *wdd)
+{
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+	u32 counter, match, delta;
+
+	counter = readl(wdt->base + ATLAS7_WDT_CNT);
+	delta = wdd->timeout * wdt->tick_rate;
+	match = counter + delta;
+
+	writel(match, wdt->base + ATLAS7_WDT_CNT_MATCH);
+
+	return 0;
+}
+
+static int atlas7_wdt_enable(struct watchdog_device *wdd)
+{
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+
+	atlas7_wdt_ping(wdd);
+
+	writel(readl(wdt->base + ATLAS7_WDT_CNT_CTRL) | ATLAS7_WDT_CNT_EN,
+	      wdt->base + ATLAS7_WDT_CNT_CTRL);
+	writel(1, wdt->base + ATLAS7_WDT_EN);
+
+	return 0;
+}
+
+static int atlas7_wdt_disable(struct watchdog_device *wdd)
+{
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+
+	writel(0, wdt->base + ATLAS7_WDT_EN);
+	writel(readl(wdt->base + ATLAS7_WDT_CNT_CTRL) & ~ATLAS7_WDT_CNT_EN,
+	      wdt->base + ATLAS7_WDT_CNT_CTRL);
+
+	return 0;
+}
+
+static int atlas7_wdt_settimeout(struct watchdog_device *wdd, unsigned int to)
+{
+	wdd->timeout = to;
+
+	return 0;
+}
+
+#define OPTIONS (WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE)
+
+static const struct watchdog_info atlas7_wdt_ident = {
+	.options = OPTIONS,
+	.firmware_version = 0,
+	.identity = "atlas7 Watchdog",
+};
+
+static struct watchdog_ops atlas7_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = atlas7_wdt_enable,
+	.stop = atlas7_wdt_disable,
+	.get_timeleft = atlas7_wdt_gettimeleft,
+	.ping = atlas7_wdt_ping,
+	.set_timeout = atlas7_wdt_settimeout,
+};
+
+static struct watchdog_device atlas7_wdd = {
+	.info = &atlas7_wdt_ident,
+	.ops = &atlas7_wdt_ops,
+	.timeout = ATLAS7_WDT_DEFAULT_TIMEOUT,
+};
+
+static const struct of_device_id atlas7_wdt_ids[] = {
+	{ .compatible = "sirf,atlas7-tick"},
+	{}
+};
+
+static int atlas7_wdt_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct atlas7_wdog *wdt;
+	struct resource *res;
+	struct clk *clk;
+	int ret;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	wdt->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wdt->base))
+		return PTR_ERR(wdt->base);
+
+	clk = of_clk_get(np, 0);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+	ret = clk_prepare_enable(clk);
+	if (ret) {
+		dev_err(&pdev->dev, "clk enable failed\n");
+		goto err;
+	}
+
+	/* disable watchdog hardware */
+	writel(0, wdt->base + ATLAS7_WDT_CNT_CTRL);
+
+	wdt->tick_rate = clk_get_rate(clk);
+	wdt->clk = clk;
+	atlas7_wdd.min_timeout = 1;
+	atlas7_wdd.max_timeout = UINT_MAX / wdt->tick_rate;
+
+	watchdog_init_timeout(&atlas7_wdd, 0, &pdev->dev);
+	watchdog_set_nowayout(&atlas7_wdd, nowayout);
+
+	watchdog_set_drvdata(&atlas7_wdd, wdt);
+	platform_set_drvdata(pdev, &atlas7_wdd);
+
+	ret = watchdog_register_device(&atlas7_wdd);
+	if (ret)
+		goto err1;
+
+	return 0;
+
+err1:
+	clk_disable_unprepare(clk);
+err:
+	clk_put(clk);
+	return ret;
+}
+
+static void atlas7_wdt_shutdown(struct platform_device *pdev)
+{
+	struct watchdog_device *wdd = platform_get_drvdata(pdev);
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+
+	atlas7_wdt_disable(wdd);
+	clk_disable_unprepare(wdt->clk);
+}
+
+static int atlas7_wdt_remove(struct platform_device *pdev)
+{
+	struct watchdog_device *wdd = platform_get_drvdata(pdev);
+	struct atlas7_wdog *wdt = watchdog_get_drvdata(wdd);
+
+	atlas7_wdt_shutdown(pdev);
+	clk_put(wdt->clk);
+	return 0;
+}
+
+static int __maybe_unused atlas7_wdt_suspend(struct device *dev)
+{
+	/*
+	 * NOTE:timer controller registers settings are saved
+	 * and restored back by the timer-atlas7.c
+	 */
+	return 0;
+}
+
+static int __maybe_unused atlas7_wdt_resume(struct device *dev)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+
+	/*
+	 * NOTE: Since timer controller registers settings are saved
+	 * and restored back by the timer-atlas7.c, so we need not
+	 * update WD settings except refreshing timeout.
+	 */
+	atlas7_wdt_ping(wdd);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(atlas7_wdt_pm_ops,
+		atlas7_wdt_suspend, atlas7_wdt_resume);
+
+MODULE_DEVICE_TABLE(of, atlas7_wdt_ids);
+
+static struct platform_driver atlas7_wdt_driver = {
+	.driver = {
+		.name = "atlas7-wdt",
+		.pm = &atlas7_wdt_pm_ops,
+		.of_match_table	= atlas7_wdt_ids,
+	},
+	.probe = atlas7_wdt_probe,
+	.remove = atlas7_wdt_remove,
+	.shutdown = atlas7_wdt_shutdown,
+};
+module_platform_driver(atlas7_wdt_driver);
+
+MODULE_DESCRIPTION("CSRatlas7 watchdog driver");
+MODULE_AUTHOR("Guo Zeng <Guo.Zeng@csr.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:atlas7-wdt");
diff --git a/drivers/watchdog/bcm2835_wdt.c b/drivers/watchdog/bcm2835_wdt.c
index 8a5ce5b5a0b6..2e6164c4abc0 100644
--- a/drivers/watchdog/bcm2835_wdt.c
+++ b/drivers/watchdog/bcm2835_wdt.c
@@ -79,7 +79,6 @@ static int bcm2835_wdt_stop(struct watchdog_device *wdog)
 	struct bcm2835_wdt *wdt = watchdog_get_drvdata(wdog);
 
 	writel_relaxed(PM_PASSWORD | PM_RSTC_RESET, wdt->base + PM_RSTC);
-	dev_info(wdog->dev, "Watchdog timer stopped");
 	return 0;
 }
 
diff --git a/drivers/watchdog/bcm47xx_wdt.c b/drivers/watchdog/bcm47xx_wdt.c
index 4064a43f1360..df1c2a4b0165 100644
--- a/drivers/watchdog/bcm47xx_wdt.c
+++ b/drivers/watchdog/bcm47xx_wdt.c
@@ -20,7 +20,6 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/watchdog.h>
 #include <linux/timer.h>
@@ -88,12 +87,22 @@ static int bcm47xx_wdt_hard_set_timeout(struct watchdog_device *wdd,
 	return 0;
 }
 
+static int bcm47xx_wdt_restart(struct watchdog_device *wdd)
+{
+	struct bcm47xx_wdt *wdt = bcm47xx_wdt_get(wdd);
+
+	wdt->timer_set(wdt, 1);
+
+	return 0;
+}
+
 static struct watchdog_ops bcm47xx_wdt_hard_ops = {
 	.owner		= THIS_MODULE,
 	.start		= bcm47xx_wdt_hard_start,
 	.stop		= bcm47xx_wdt_hard_stop,
 	.ping		= bcm47xx_wdt_hard_keepalive,
 	.set_timeout	= bcm47xx_wdt_hard_set_timeout,
+	.restart        = bcm47xx_wdt_restart,
 };
 
 static void bcm47xx_wdt_soft_timer_tick(unsigned long data)
@@ -158,34 +167,13 @@ static const struct watchdog_info bcm47xx_wdt_info = {
 				WDIOF_MAGICCLOSE,
 };
 
-static int bcm47xx_wdt_notify_sys(struct notifier_block *this,
-				  unsigned long code, void *unused)
-{
-	struct bcm47xx_wdt *wdt;
-
-	wdt = container_of(this, struct bcm47xx_wdt, notifier);
-	if (code == SYS_DOWN || code == SYS_HALT)
-		wdt->wdd.ops->stop(&wdt->wdd);
-	return NOTIFY_DONE;
-}
-
-static int bcm47xx_wdt_restart(struct notifier_block *this, unsigned long mode,
-			       void *cmd)
-{
-	struct bcm47xx_wdt *wdt;
-
-	wdt = container_of(this, struct bcm47xx_wdt, restart_handler);
-	wdt->timer_set(wdt, 1);
-
-	return NOTIFY_DONE;
-}
-
 static struct watchdog_ops bcm47xx_wdt_soft_ops = {
 	.owner		= THIS_MODULE,
 	.start		= bcm47xx_wdt_soft_start,
 	.stop		= bcm47xx_wdt_soft_stop,
 	.ping		= bcm47xx_wdt_soft_keepalive,
 	.set_timeout	= bcm47xx_wdt_soft_set_timeout,
+	.restart        = bcm47xx_wdt_restart,
 };
 
 static int bcm47xx_wdt_probe(struct platform_device *pdev)
@@ -214,32 +202,18 @@ static int bcm47xx_wdt_probe(struct platform_device *pdev)
 	if (ret)
 		goto err_timer;
 	watchdog_set_nowayout(&wdt->wdd, nowayout);
-
-	wdt->notifier.notifier_call = &bcm47xx_wdt_notify_sys;
-
-	ret = register_reboot_notifier(&wdt->notifier);
-	if (ret)
-		goto err_timer;
-
-	wdt->restart_handler.notifier_call = &bcm47xx_wdt_restart;
-	wdt->restart_handler.priority = 64;
-	ret = register_restart_handler(&wdt->restart_handler);
-	if (ret)
-		goto err_notifier;
+	watchdog_set_restart_priority(&wdt->wdd, 64);
+	watchdog_stop_on_reboot(&wdt->wdd);
 
 	ret = watchdog_register_device(&wdt->wdd);
 	if (ret)
-		goto err_handler;
+		goto err_timer;
 
 	dev_info(&pdev->dev, "BCM47xx Watchdog Timer enabled (%d seconds%s%s)\n",
 		timeout, nowayout ? ", nowayout" : "",
 		soft ? ", Software Timer" : "");
 	return 0;
 
-err_handler:
-	unregister_restart_handler(&wdt->restart_handler);
-err_notifier:
-	unregister_reboot_notifier(&wdt->notifier);
 err_timer:
 	if (soft)
 		del_timer_sync(&wdt->soft_timer);
@@ -255,7 +229,6 @@ static int bcm47xx_wdt_remove(struct platform_device *pdev)
 		return -ENXIO;
 
 	watchdog_unregister_device(&wdt->wdd);
-	unregister_reboot_notifier(&wdt->notifier);
 
 	return 0;
 }
diff --git a/drivers/watchdog/cadence_wdt.c b/drivers/watchdog/cadence_wdt.c
index bcfd2a22208f..4dda9024e229 100644
--- a/drivers/watchdog/cadence_wdt.c
+++ b/drivers/watchdog/cadence_wdt.c
@@ -18,7 +18,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/watchdog.h>
 
 #define CDNS_WDT_DEFAULT_TIMEOUT	10
@@ -72,7 +71,6 @@ MODULE_PARM_DESC(nowayout,
  * @ctrl_clksel: counter clock prescaler selection
  * @io_lock: spinlock for IO register access
  * @cdns_wdt_device: watchdog device structure
- * @cdns_wdt_notifier: notifier structure
  *
  * Structure containing parameters specific to cadence watchdog.
  */
@@ -84,7 +82,6 @@ struct cdns_wdt {
 	u32			ctrl_clksel;
 	spinlock_t		io_lock;
 	struct watchdog_device	cdns_wdt_device;
-	struct notifier_block	cdns_wdt_notifier;
 };
 
 /* Write access to Registers */
@@ -280,29 +277,6 @@ static struct watchdog_ops cdns_wdt_ops = {
 	.set_timeout = cdns_wdt_settimeout,
 };
 
-/**
- * cdns_wdt_notify_sys - Notifier for reboot or shutdown.
- *
- * @this: handle to notifier block
- * @code: turn off indicator
- * @unused: unused
- * Return: NOTIFY_DONE
- *
- * This notifier is invoked whenever the system reboot or shutdown occur
- * because we need to disable the WDT before system goes down as WDT might
- * reset on the next boot.
- */
-static int cdns_wdt_notify_sys(struct notifier_block *this, unsigned long code,
-			       void *unused)
-{
-	struct cdns_wdt *wdt = container_of(this, struct cdns_wdt,
-					    cdns_wdt_notifier);
-	if (code == SYS_DOWN || code == SYS_HALT)
-		cdns_wdt_stop(&wdt->cdns_wdt_device);
-
-	return NOTIFY_DONE;
-}
-
 /************************Platform Operations*****************************/
 /**
  * cdns_wdt_probe - Probe call for the device.
@@ -360,6 +334,7 @@ static int cdns_wdt_probe(struct platform_device *pdev)
 	}
 
 	watchdog_set_nowayout(cdns_wdt_device, nowayout);
+	watchdog_stop_on_reboot(cdns_wdt_device);
 	watchdog_set_drvdata(cdns_wdt_device, wdt);
 
 	wdt->clk = devm_clk_get(&pdev->dev, NULL);
@@ -386,14 +361,6 @@ static int cdns_wdt_probe(struct platform_device *pdev)
 
 	spin_lock_init(&wdt->io_lock);
 
-	wdt->cdns_wdt_notifier.notifier_call = &cdns_wdt_notify_sys;
-	ret = register_reboot_notifier(&wdt->cdns_wdt_notifier);
-	if (ret != 0) {
-		dev_err(&pdev->dev, "cannot register reboot notifier err=%d)\n",
-			ret);
-		goto err_clk_disable;
-	}
-
 	ret = watchdog_register_device(cdns_wdt_device);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to register wdt device\n");
@@ -427,7 +394,6 @@ static int cdns_wdt_remove(struct platform_device *pdev)
 
 	cdns_wdt_stop(&wdt->cdns_wdt_device);
 	watchdog_unregister_device(&wdt->cdns_wdt_device);
-	unregister_reboot_notifier(&wdt->cdns_wdt_notifier);
 	clk_disable_unprepare(wdt->clk);
 
 	return 0;
@@ -455,8 +421,7 @@ static void cdns_wdt_shutdown(struct platform_device *pdev)
  */
 static int __maybe_unused cdns_wdt_suspend(struct device *dev)
 {
-	struct platform_device *pdev = container_of(dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
 
 	cdns_wdt_stop(&wdt->cdns_wdt_device);
@@ -474,8 +439,7 @@ static int __maybe_unused cdns_wdt_suspend(struct device *dev)
 static int __maybe_unused cdns_wdt_resume(struct device *dev)
 {
 	int ret;
-	struct platform_device *pdev = container_of(dev,
-			struct platform_device, dev);
+	struct platform_device *pdev = to_platform_device(dev);
 	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
 
 	ret = clk_prepare_enable(wdt->clk);
diff --git a/drivers/watchdog/da9052_wdt.c b/drivers/watchdog/da9052_wdt.c
index 67e67977bd29..2fc19a32a320 100644
--- a/drivers/watchdog/da9052_wdt.c
+++ b/drivers/watchdog/da9052_wdt.c
@@ -31,7 +31,6 @@
 struct da9052_wdt_data {
 	struct watchdog_device wdt;
 	struct da9052 *da9052;
-	struct kref kref;
 	unsigned long jpast;
 };
 
@@ -51,10 +50,6 @@ static const struct {
 };
 
 
-static void da9052_wdt_release_resources(struct kref *r)
-{
-}
-
 static int da9052_wdt_set_timeout(struct watchdog_device *wdt_dev,
 				  unsigned int timeout)
 {
@@ -104,20 +99,6 @@ static int da9052_wdt_set_timeout(struct watchdog_device *wdt_dev,
 	return 0;
 }
 
-static void da9052_wdt_ref(struct watchdog_device *wdt_dev)
-{
-	struct da9052_wdt_data *driver_data = watchdog_get_drvdata(wdt_dev);
-
-	kref_get(&driver_data->kref);
-}
-
-static void da9052_wdt_unref(struct watchdog_device *wdt_dev)
-{
-	struct da9052_wdt_data *driver_data = watchdog_get_drvdata(wdt_dev);
-
-	kref_put(&driver_data->kref, da9052_wdt_release_resources);
-}
-
 static int da9052_wdt_start(struct watchdog_device *wdt_dev)
 {
 	return da9052_wdt_set_timeout(wdt_dev, wdt_dev->timeout);
@@ -170,8 +151,6 @@ static const struct watchdog_ops da9052_wdt_ops = {
 	.stop = da9052_wdt_stop,
 	.ping = da9052_wdt_ping,
 	.set_timeout = da9052_wdt_set_timeout,
-	.ref = da9052_wdt_ref,
-	.unref = da9052_wdt_unref,
 };
 
 
@@ -198,8 +177,6 @@ static int da9052_wdt_probe(struct platform_device *pdev)
 	da9052_wdt->parent = &pdev->dev;
 	watchdog_set_drvdata(da9052_wdt, driver_data);
 
-	kref_init(&driver_data->kref);
-
 	ret = da9052_reg_update(da9052, DA9052_CONTROL_D_REG,
 				DA9052_CONTROLD_TWDSCALE, 0);
 	if (ret < 0) {
@@ -225,7 +202,6 @@ static int da9052_wdt_remove(struct platform_device *pdev)
 	struct da9052_wdt_data *driver_data = platform_get_drvdata(pdev);
 
 	watchdog_unregister_device(&driver_data->wdt);
-	kref_put(&driver_data->kref, da9052_wdt_release_resources);
 
 	return 0;
 }
diff --git a/drivers/watchdog/da9055_wdt.c b/drivers/watchdog/da9055_wdt.c
index 04d1430d93d2..8377c43f3f20 100644
--- a/drivers/watchdog/da9055_wdt.c
+++ b/drivers/watchdog/da9055_wdt.c
@@ -35,7 +35,6 @@ MODULE_PARM_DESC(nowayout,
 struct da9055_wdt_data {
 	struct watchdog_device wdt;
 	struct da9055 *da9055;
-	struct kref kref;
 };
 
 static const struct {
@@ -99,24 +98,6 @@ static int da9055_wdt_ping(struct watchdog_device *wdt_dev)
 				 DA9055_WATCHDOG_MASK, 1);
 }
 
-static void da9055_wdt_release_resources(struct kref *r)
-{
-}
-
-static void da9055_wdt_ref(struct watchdog_device *wdt_dev)
-{
-	struct da9055_wdt_data *driver_data = watchdog_get_drvdata(wdt_dev);
-
-	kref_get(&driver_data->kref);
-}
-
-static void da9055_wdt_unref(struct watchdog_device *wdt_dev)
-{
-	struct da9055_wdt_data *driver_data = watchdog_get_drvdata(wdt_dev);
-
-	kref_put(&driver_data->kref, da9055_wdt_release_resources);
-}
-
 static int da9055_wdt_start(struct watchdog_device *wdt_dev)
 {
 	return da9055_wdt_set_timeout(wdt_dev, wdt_dev->timeout);
@@ -138,8 +119,6 @@ static const struct watchdog_ops da9055_wdt_ops = {
 	.stop = da9055_wdt_stop,
 	.ping = da9055_wdt_ping,
 	.set_timeout = da9055_wdt_set_timeout,
-	.ref = da9055_wdt_ref,
-	.unref = da9055_wdt_unref,
 };
 
 static int da9055_wdt_probe(struct platform_device *pdev)
@@ -165,8 +144,6 @@ static int da9055_wdt_probe(struct platform_device *pdev)
 	watchdog_set_nowayout(da9055_wdt, nowayout);
 	watchdog_set_drvdata(da9055_wdt, driver_data);
 
-	kref_init(&driver_data->kref);
-
 	ret = da9055_wdt_stop(da9055_wdt);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "Failed to stop watchdog, %d\n", ret);
@@ -189,7 +166,6 @@ static int da9055_wdt_remove(struct platform_device *pdev)
 	struct da9055_wdt_data *driver_data = platform_get_drvdata(pdev);
 
 	watchdog_unregister_device(&driver_data->wdt);
-	kref_put(&driver_data->kref, da9055_wdt_release_resources);
 
 	return 0;
 }
diff --git a/drivers/watchdog/da9063_wdt.c b/drivers/watchdog/da9063_wdt.c
index 6bf130bd863d..11e887572649 100644
--- a/drivers/watchdog/da9063_wdt.c
+++ b/drivers/watchdog/da9063_wdt.c
@@ -20,7 +20,6 @@
 #include <linux/delay.h>
 #include <linux/mfd/da9063/registers.h>
 #include <linux/mfd/da9063/core.h>
-#include <linux/reboot.h>
 #include <linux/regmap.h>
 
 /*
@@ -39,7 +38,6 @@ static const unsigned int wdt_timeout[] = { 0, 2, 4, 8, 16, 32, 65, 131 };
 struct da9063_watchdog {
 	struct da9063 *da9063;
 	struct watchdog_device wdtdev;
-	struct notifier_block restart_handler;
 };
 
 static unsigned int da9063_wdt_timeout_to_sel(unsigned int secs)
@@ -121,12 +119,9 @@ static int da9063_wdt_set_timeout(struct watchdog_device *wdd,
 	return ret;
 }
 
-static int da9063_wdt_restart_handler(struct notifier_block *this,
-				      unsigned long mode, void *cmd)
+static int da9063_wdt_restart(struct watchdog_device *wdd)
 {
-	struct da9063_watchdog *wdt = container_of(this,
-						   struct da9063_watchdog,
-						   restart_handler);
+	struct da9063_watchdog *wdt = watchdog_get_drvdata(wdd);
 	int ret;
 
 	ret = regmap_write(wdt->da9063->regmap, DA9063_REG_CONTROL_F,
@@ -135,7 +130,7 @@ static int da9063_wdt_restart_handler(struct notifier_block *this,
 		dev_alert(wdt->da9063->dev, "Failed to shutdown (err = %d)\n",
 			  ret);
 
-	return NOTIFY_DONE;
+	return ret;
 }
 
 static const struct watchdog_info da9063_watchdog_info = {
@@ -149,6 +144,7 @@ static const struct watchdog_ops da9063_watchdog_ops = {
 	.stop = da9063_wdt_stop,
 	.ping = da9063_wdt_ping,
 	.set_timeout = da9063_wdt_set_timeout,
+	.restart = da9063_wdt_restart,
 };
 
 static int da9063_wdt_probe(struct platform_device *pdev)
@@ -179,6 +175,8 @@ static int da9063_wdt_probe(struct platform_device *pdev)
 
 	wdt->wdtdev.status = WATCHDOG_NOWAYOUT_INIT_STATUS;
 
+	watchdog_set_restart_priority(&wdt->wdtdev, 128);
+
 	watchdog_set_drvdata(&wdt->wdtdev, wdt);
 	dev_set_drvdata(&pdev->dev, wdt);
 
@@ -186,13 +184,6 @@ static int da9063_wdt_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-	wdt->restart_handler.notifier_call = da9063_wdt_restart_handler;
-	wdt->restart_handler.priority = 128;
-	ret = register_restart_handler(&wdt->restart_handler);
-	if (ret)
-		dev_err(wdt->da9063->dev,
-			"Failed to register restart handler (err = %d)\n", ret);
-
 	return 0;
 }
 
@@ -200,8 +191,6 @@ static int da9063_wdt_remove(struct platform_device *pdev)
 {
 	struct da9063_watchdog *wdt = dev_get_drvdata(&pdev->dev);
 
-	unregister_restart_handler(&wdt->restart_handler);
-
 	watchdog_unregister_device(&wdt->wdtdev);
 
 	return 0;
diff --git a/drivers/watchdog/diag288_wdt.c b/drivers/watchdog/diag288_wdt.c
index 3db9d0e0673d..861d3d3133f8 100644
--- a/drivers/watchdog/diag288_wdt.c
+++ b/drivers/watchdog/diag288_wdt.c
@@ -106,6 +106,10 @@ static int __diag288_lpar(unsigned int func, unsigned int timeout,
 	return __diag288(func, timeout, action, 0);
 }
 
+static unsigned long wdt_status;
+
+#define DIAG_WDOG_BUSY	0
+
 static int wdt_start(struct watchdog_device *dev)
 {
 	char *ebc_cmd;
@@ -113,12 +117,17 @@ static int wdt_start(struct watchdog_device *dev)
 	int ret;
 	unsigned int func;
 
+	if (test_and_set_bit(DIAG_WDOG_BUSY, &wdt_status))
+		return -EBUSY;
+
 	ret = -ENODEV;
 
 	if (MACHINE_IS_VM) {
 		ebc_cmd = kmalloc(MAX_CMDLEN, GFP_KERNEL);
-		if (!ebc_cmd)
+		if (!ebc_cmd) {
+			clear_bit(DIAG_WDOG_BUSY, &wdt_status);
 			return -ENOMEM;
+		}
 		len = strlcpy(ebc_cmd, wdt_cmd, MAX_CMDLEN);
 		ASCEBC(ebc_cmd, MAX_CMDLEN);
 		EBC_TOUPPER(ebc_cmd, MAX_CMDLEN);
@@ -135,6 +144,7 @@ static int wdt_start(struct watchdog_device *dev)
 
 	if (ret) {
 		pr_err("The watchdog cannot be activated\n");
+		clear_bit(DIAG_WDOG_BUSY, &wdt_status);
 		return ret;
 	}
 	return 0;
@@ -146,6 +156,9 @@ static int wdt_stop(struct watchdog_device *dev)
 
 	diag_stat_inc(DIAG_STAT_X288);
 	ret = __diag288(WDT_FUNC_CANCEL, 0, 0, 0);
+
+	clear_bit(DIAG_WDOG_BUSY, &wdt_status);
+
 	return ret;
 }
 
@@ -220,17 +233,10 @@ static struct watchdog_device wdt_dev = {
  * It makes no sense to go into suspend while the watchdog is running.
  * Depending on the memory size, the watchdog might trigger, while we
  * are still saving the memory.
- * We reuse the open flag to ensure that suspend and watchdog open are
- * exclusive operations
  */
 static int wdt_suspend(void)
 {
-	if (test_and_set_bit(WDOG_DEV_OPEN, &wdt_dev.status)) {
-		pr_err("Linux cannot be suspended while the watchdog is in use\n");
-		return notifier_from_errno(-EBUSY);
-	}
-	if (test_bit(WDOG_ACTIVE, &wdt_dev.status)) {
-		clear_bit(WDOG_DEV_OPEN, &wdt_dev.status);
+	if (test_and_set_bit(DIAG_WDOG_BUSY, &wdt_status)) {
 		pr_err("Linux cannot be suspended while the watchdog is in use\n");
 		return notifier_from_errno(-EBUSY);
 	}
@@ -239,7 +245,7 @@ static int wdt_suspend(void)
 
 static int wdt_resume(void)
 {
-	clear_bit(WDOG_DEV_OPEN, &wdt_dev.status);
+	clear_bit(DIAG_WDOG_BUSY, &wdt_status);
 	return NOTIFY_DONE;
 }
 
diff --git a/drivers/watchdog/digicolor_wdt.c b/drivers/watchdog/digicolor_wdt.c
index 50abe1bf62a5..1ccb0b239348 100644
--- a/drivers/watchdog/digicolor_wdt.c
+++ b/drivers/watchdog/digicolor_wdt.c
@@ -15,7 +15,6 @@
 #include <linux/delay.h>
 #include <linux/clk.h>
 #include <linux/watchdog.h>
-#include <linux/reboot.h>
 #include <linux/platform_device.h>
 #include <linux/of_address.h>
 
@@ -28,7 +27,6 @@
 struct dc_wdt {
 	void __iomem		*base;
 	struct clk		*clk;
-	struct notifier_block	restart_handler;
 	spinlock_t		lock;
 };
 
@@ -50,16 +48,15 @@ static void dc_wdt_set(struct dc_wdt *wdt, u32 ticks)
 	spin_unlock_irqrestore(&wdt->lock, flags);
 }
 
-static int dc_restart_handler(struct notifier_block *this, unsigned long mode,
-			      void *cmd)
+static int dc_wdt_restart(struct watchdog_device *wdog)
 {
-	struct dc_wdt *wdt = container_of(this, struct dc_wdt, restart_handler);
+	struct dc_wdt *wdt = watchdog_get_drvdata(wdog);
 
 	dc_wdt_set(wdt, 1);
 	/* wait for reset to assert... */
 	mdelay(500);
 
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static int dc_wdt_start(struct watchdog_device *wdog)
@@ -104,6 +101,7 @@ static struct watchdog_ops dc_wdt_ops = {
 	.stop		= dc_wdt_stop,
 	.set_timeout	= dc_wdt_set_timeout,
 	.get_timeleft	= dc_wdt_get_timeleft,
+	.restart        = dc_wdt_restart,
 };
 
 static struct watchdog_info dc_wdt_info = {
@@ -148,6 +146,7 @@ static int dc_wdt_probe(struct platform_device *pdev)
 	spin_lock_init(&wdt->lock);
 
 	watchdog_set_drvdata(&dc_wdt_wdd, wdt);
+	watchdog_set_restart_priority(&dc_wdt_wdd, 128);
 	watchdog_init_timeout(&dc_wdt_wdd, timeout, dev);
 	ret = watchdog_register_device(&dc_wdt_wdd);
 	if (ret) {
@@ -155,12 +154,6 @@ static int dc_wdt_probe(struct platform_device *pdev)
 		goto err_iounmap;
 	}
 
-	wdt->restart_handler.notifier_call = dc_restart_handler;
-	wdt->restart_handler.priority = 128;
-	ret = register_restart_handler(&wdt->restart_handler);
-	if (ret)
-		dev_warn(&pdev->dev, "cannot register restart handler\n");
-
 	return 0;
 
 err_iounmap:
@@ -172,7 +165,6 @@ static int dc_wdt_remove(struct platform_device *pdev)
 {
 	struct dc_wdt *wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&wdt->restart_handler);
 	watchdog_unregister_device(&dc_wdt_wdd);
 	iounmap(wdt->base);
 
diff --git a/drivers/watchdog/dw_wdt.c b/drivers/watchdog/dw_wdt.c
index 6ea0634345e9..8fefa4ad46d4 100644
--- a/drivers/watchdog/dw_wdt.c
+++ b/drivers/watchdog/dw_wdt.c
@@ -81,7 +81,7 @@ static inline int dw_wdt_top_in_seconds(unsigned top)
 	 * There are 16 possible timeout values in 0..15 where the number of
 	 * cycles is 2 ^ (16 + i) and the watchdog counts down.
 	 */
-	return (1 << (16 + top)) / clk_get_rate(dw_wdt.clk);
+	return (1U << (16 + top)) / clk_get_rate(dw_wdt.clk);
 }
 
 static int dw_wdt_get_top(void)
diff --git a/drivers/watchdog/gpio_wdt.c b/drivers/watchdog/gpio_wdt.c
index 90d59d3f38a3..ba066e4a707b 100644
--- a/drivers/watchdog/gpio_wdt.c
+++ b/drivers/watchdog/gpio_wdt.c
@@ -12,10 +12,8 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/module.h>
-#include <linux/notifier.h>
 #include <linux/of_gpio.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/watchdog.h>
 
 #define SOFT_TIMEOUT_MIN	1
@@ -36,7 +34,6 @@ struct gpio_wdt_priv {
 	unsigned int		hw_algo;
 	unsigned int		hw_margin;
 	unsigned long		last_jiffies;
-	struct notifier_block	notifier;
 	struct timer_list	timer;
 	struct watchdog_device	wdd;
 };
@@ -57,7 +54,8 @@ static void gpio_wdt_hwping(unsigned long data)
 
 	if (priv->armed && time_after(jiffies, priv->last_jiffies +
 				      msecs_to_jiffies(wdd->timeout * 1000))) {
-		dev_crit(wdd->dev, "Timer expired. System will reboot soon!\n");
+		dev_crit(wdd->parent,
+			 "Timer expired. System will reboot soon!\n");
 		return;
 	}
 
@@ -126,26 +124,6 @@ static int gpio_wdt_set_timeout(struct watchdog_device *wdd, unsigned int t)
 	return gpio_wdt_ping(wdd);
 }
 
-static int gpio_wdt_notify_sys(struct notifier_block *nb, unsigned long code,
-			       void *unused)
-{
-	struct gpio_wdt_priv *priv = container_of(nb, struct gpio_wdt_priv,
-						  notifier);
-
-	mod_timer(&priv->timer, 0);
-
-	switch (code) {
-	case SYS_HALT:
-	case SYS_POWER_OFF:
-		gpio_wdt_disable(priv);
-		break;
-	default:
-		break;
-	}
-
-	return NOTIFY_DONE;
-}
-
 static const struct watchdog_info gpio_wdt_ident = {
 	.options	= WDIOF_MAGICCLOSE | WDIOF_KEEPALIVEPING |
 			  WDIOF_SETTIMEOUT,
@@ -224,23 +202,16 @@ static int gpio_wdt_probe(struct platform_device *pdev)
 
 	setup_timer(&priv->timer, gpio_wdt_hwping, (unsigned long)&priv->wdd);
 
+	watchdog_stop_on_reboot(&priv->wdd);
+
 	ret = watchdog_register_device(&priv->wdd);
 	if (ret)
 		return ret;
 
-	priv->notifier.notifier_call = gpio_wdt_notify_sys;
-	ret = register_reboot_notifier(&priv->notifier);
-	if (ret)
-		goto error_unregister;
-
 	if (priv->always_running)
 		gpio_wdt_start_impl(priv);
 
 	return 0;
-
-error_unregister:
-	watchdog_unregister_device(&priv->wdd);
-	return ret;
 }
 
 static int gpio_wdt_remove(struct platform_device *pdev)
@@ -248,7 +219,6 @@ static int gpio_wdt_remove(struct platform_device *pdev)
 	struct gpio_wdt_priv *priv = platform_get_drvdata(pdev);
 
 	del_timer_sync(&priv->timer);
-	unregister_reboot_notifier(&priv->notifier);
 	watchdog_unregister_device(&priv->wdd);
 
 	return 0;
diff --git a/drivers/watchdog/hpwdt.c b/drivers/watchdog/hpwdt.c
index 286369d4f0f5..92443c319e59 100644
--- a/drivers/watchdog/hpwdt.c
+++ b/drivers/watchdog/hpwdt.c
@@ -1,11 +1,11 @@
 /*
- *	HP WatchDog Driver
+ *	HPE WatchDog Driver
  *	based on
  *
  *	SoftDog	0.05:	A Software Watchdog Device
  *
- *	(c) Copyright 2007 Hewlett-Packard Development Company, L.P.
- *	Thomas Mingarelli <thomas.mingarelli@hp.com>
+ *	(c) Copyright 2015 Hewlett Packard Enterprise Development LP
+ *	Thomas Mingarelli <thomas.mingarelli@hpe.com>
  *
  *	This program is free software; you can redistribute it and/or
  *	modify it under the terms of the GNU General Public License
@@ -580,7 +580,7 @@ static const struct watchdog_info ident = {
 	.options = WDIOF_SETTIMEOUT |
 		   WDIOF_KEEPALIVEPING |
 		   WDIOF_MAGICCLOSE,
-	.identity = "HP iLO2+ HW Watchdog Timer",
+	.identity = "HPE iLO2+ HW Watchdog Timer",
 };
 
 static long hpwdt_ioctl(struct file *file, unsigned int cmd,
@@ -758,7 +758,7 @@ static int hpwdt_init_nmi_decoding(struct pci_dev *dev)
 		goto error2;
 
 	dev_info(&dev->dev,
-			"HP Watchdog Timer Driver: NMI decoding initialized"
+			"HPE Watchdog Timer Driver: NMI decoding initialized"
 			", allow kernel dump: %s (default = 1/ON)\n",
 			(allow_kdump == 0) ? "OFF" : "ON");
 	return 0;
@@ -863,7 +863,7 @@ static int hpwdt_init_one(struct pci_dev *dev,
 		goto error_misc_register;
 	}
 
-	dev_info(&dev->dev, "HP Watchdog Timer Driver: %s"
+	dev_info(&dev->dev, "HPE Watchdog Timer Driver: %s"
 			", timer margin: %d seconds (nowayout=%d).\n",
 			HPWDT_VERSION, soft_margin, nowayout);
 	return 0;
diff --git a/drivers/watchdog/imgpdc_wdt.c b/drivers/watchdog/imgpdc_wdt.c
index 15ab07230960..3679f2e1922f 100644
--- a/drivers/watchdog/imgpdc_wdt.c
+++ b/drivers/watchdog/imgpdc_wdt.c
@@ -45,7 +45,6 @@
 #include <linux/log2.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/slab.h>
 #include <linux/watchdog.h>
 
@@ -87,7 +86,6 @@ struct pdc_wdt_dev {
 	struct clk *wdt_clk;
 	struct clk *sys_clk;
 	void __iomem *base;
-	struct notifier_block restart_handler;
 };
 
 static int pdc_wdt_keepalive(struct watchdog_device *wdt_dev)
@@ -152,6 +150,16 @@ static int pdc_wdt_start(struct watchdog_device *wdt_dev)
 	return 0;
 }
 
+static int pdc_wdt_restart(struct watchdog_device *wdt_dev)
+{
+	struct pdc_wdt_dev *wdt = watchdog_get_drvdata(wdt_dev);
+
+	/* Assert SOFT_RESET */
+	writel(0x1, wdt->base + PDC_WDT_SOFT_RESET);
+
+	return 0;
+}
+
 static struct watchdog_info pdc_wdt_info = {
 	.identity	= "IMG PDC Watchdog",
 	.options	= WDIOF_SETTIMEOUT |
@@ -165,20 +173,9 @@ static const struct watchdog_ops pdc_wdt_ops = {
 	.stop		= pdc_wdt_stop,
 	.ping		= pdc_wdt_keepalive,
 	.set_timeout	= pdc_wdt_set_timeout,
+	.restart        = pdc_wdt_restart,
 };
 
-static int pdc_wdt_restart(struct notifier_block *this, unsigned long mode,
-			   void *cmd)
-{
-	struct pdc_wdt_dev *wdt = container_of(this, struct pdc_wdt_dev,
-					       restart_handler);
-
-	/* Assert SOFT_RESET */
-	writel(0x1, wdt->base + PDC_WDT_SOFT_RESET);
-
-	return NOTIFY_OK;
-}
-
 static int pdc_wdt_probe(struct platform_device *pdev)
 {
 	u64 div;
@@ -282,6 +279,7 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 	}
 
 	watchdog_set_nowayout(&pdc_wdt->wdt_dev, nowayout);
+	watchdog_set_restart_priority(&pdc_wdt->wdt_dev, 128);
 
 	platform_set_drvdata(pdev, pdc_wdt);
 
@@ -289,13 +287,6 @@ static int pdc_wdt_probe(struct platform_device *pdev)
 	if (ret)
 		goto disable_wdt_clk;
 
-	pdc_wdt->restart_handler.notifier_call = pdc_wdt_restart;
-	pdc_wdt->restart_handler.priority = 128;
-	ret = register_restart_handler(&pdc_wdt->restart_handler);
-	if (ret)
-		dev_warn(&pdev->dev, "failed to register restart handler: %d\n",
-			 ret);
-
 	return 0;
 
 disable_wdt_clk:
@@ -316,7 +307,6 @@ static int pdc_wdt_remove(struct platform_device *pdev)
 {
 	struct pdc_wdt_dev *pdc_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&pdc_wdt->restart_handler);
 	pdc_wdt_stop(&pdc_wdt->wdt_dev);
 	watchdog_unregister_device(&pdc_wdt->wdt_dev);
 	clk_disable_unprepare(pdc_wdt->wdt_clk);
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index 29ef719a6a3c..e47966aa2db0 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -29,10 +29,8 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <linux/notifier.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/regmap.h>
 #include <linux/timer.h>
 #include <linux/watchdog.h>
@@ -64,7 +62,6 @@ struct imx2_wdt_device {
 	struct regmap *regmap;
 	struct timer_list timer;	/* Pings the watchdog when closed */
 	struct watchdog_device wdog;
-	struct notifier_block restart_handler;
 };
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
@@ -83,13 +80,11 @@ static const struct watchdog_info imx2_wdt_info = {
 	.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE,
 };
 
-static int imx2_restart_handler(struct notifier_block *this, unsigned long mode,
-				void *cmd)
+static int imx2_wdt_restart(struct watchdog_device *wdog)
 {
+	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 	unsigned int wcr_enable = IMX2_WDT_WCR_WDE;
-	struct imx2_wdt_device *wdev = container_of(this,
-						    struct imx2_wdt_device,
-						    restart_handler);
+
 	/* Assert SRS signal */
 	regmap_write(wdev->regmap, IMX2_WDT_WCR, wcr_enable);
 	/*
@@ -105,7 +100,7 @@ static int imx2_restart_handler(struct notifier_block *this, unsigned long mode,
 	/* wait for reset to assert... */
 	mdelay(500);
 
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static inline void imx2_wdt_setup(struct watchdog_device *wdog)
@@ -213,6 +208,7 @@ static const struct watchdog_ops imx2_wdt_ops = {
 	.stop = imx2_wdt_stop,
 	.ping = imx2_wdt_ping,
 	.set_timeout = imx2_wdt_set_timeout,
+	.restart = imx2_wdt_restart,
 };
 
 static const struct regmap_config imx2_wdt_regmap_config = {
@@ -275,6 +271,7 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, wdog);
 	watchdog_set_drvdata(wdog, wdev);
 	watchdog_set_nowayout(wdog, nowayout);
+	watchdog_set_restart_priority(wdog, 128);
 	watchdog_init_timeout(wdog, timeout, &pdev->dev);
 
 	setup_timer(&wdev->timer, imx2_wdt_timer_ping, (unsigned long)wdog);
@@ -294,12 +291,6 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 		goto disable_clk;
 	}
 
-	wdev->restart_handler.notifier_call = imx2_restart_handler;
-	wdev->restart_handler.priority = 128;
-	ret = register_restart_handler(&wdev->restart_handler);
-	if (ret)
-		dev_err(&pdev->dev, "cannot register restart handler\n");
-
 	dev_info(&pdev->dev, "timeout %d sec (nowayout=%d)\n",
 		 wdog->timeout, nowayout);
 
@@ -315,8 +306,6 @@ static int __exit imx2_wdt_remove(struct platform_device *pdev)
 	struct watchdog_device *wdog = platform_get_drvdata(pdev);
 	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
-	unregister_restart_handler(&wdev->restart_handler);
-
 	watchdog_unregister_device(wdog);
 
 	if (imx2_wdt_is_running(wdev)) {
diff --git a/drivers/watchdog/lpc18xx_wdt.c b/drivers/watchdog/lpc18xx_wdt.c
index ab7b8b185d99..6914c83aa6d9 100644
--- a/drivers/watchdog/lpc18xx_wdt.c
+++ b/drivers/watchdog/lpc18xx_wdt.c
@@ -18,7 +18,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/watchdog.h>
 
 /* Registers */
@@ -59,7 +58,6 @@ struct lpc18xx_wdt_dev {
 	unsigned long		clk_rate;
 	void __iomem		*base;
 	struct timer_list	timer;
-	struct notifier_block	restart_handler;
 	spinlock_t		lock;
 };
 
@@ -155,27 +153,9 @@ static int lpc18xx_wdt_start(struct watchdog_device *wdt_dev)
 	return 0;
 }
 
-static struct watchdog_info lpc18xx_wdt_info = {
-	.identity	= "NXP LPC18xx Watchdog",
-	.options	= WDIOF_SETTIMEOUT |
-			  WDIOF_KEEPALIVEPING |
-			  WDIOF_MAGICCLOSE,
-};
-
-static const struct watchdog_ops lpc18xx_wdt_ops = {
-	.owner		= THIS_MODULE,
-	.start		= lpc18xx_wdt_start,
-	.stop		= lpc18xx_wdt_stop,
-	.ping		= lpc18xx_wdt_feed,
-	.set_timeout	= lpc18xx_wdt_set_timeout,
-	.get_timeleft	= lpc18xx_wdt_get_timeleft,
-};
-
-static int lpc18xx_wdt_restart(struct notifier_block *this, unsigned long mode,
-			       void *cmd)
+static int lpc18xx_wdt_restart(struct watchdog_device *wdt_dev)
 {
-	struct lpc18xx_wdt_dev *lpc18xx_wdt = container_of(this,
-				struct lpc18xx_wdt_dev, restart_handler);
+	struct lpc18xx_wdt_dev *lpc18xx_wdt = watchdog_get_drvdata(wdt_dev);
 	unsigned long flags;
 	int val;
 
@@ -197,9 +177,26 @@ static int lpc18xx_wdt_restart(struct notifier_block *this, unsigned long mode,
 
 	spin_unlock_irqrestore(&lpc18xx_wdt->lock, flags);
 
-	return NOTIFY_OK;
+	return 0;
 }
 
+static struct watchdog_info lpc18xx_wdt_info = {
+	.identity	= "NXP LPC18xx Watchdog",
+	.options	= WDIOF_SETTIMEOUT |
+			  WDIOF_KEEPALIVEPING |
+			  WDIOF_MAGICCLOSE,
+};
+
+static const struct watchdog_ops lpc18xx_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= lpc18xx_wdt_start,
+	.stop		= lpc18xx_wdt_stop,
+	.ping		= lpc18xx_wdt_feed,
+	.set_timeout	= lpc18xx_wdt_set_timeout,
+	.get_timeleft	= lpc18xx_wdt_get_timeleft,
+	.restart        = lpc18xx_wdt_restart,
+};
+
 static int lpc18xx_wdt_probe(struct platform_device *pdev)
 {
 	struct lpc18xx_wdt_dev *lpc18xx_wdt;
@@ -273,6 +270,7 @@ static int lpc18xx_wdt_probe(struct platform_device *pdev)
 		    (unsigned long)&lpc18xx_wdt->wdt_dev);
 
 	watchdog_set_nowayout(&lpc18xx_wdt->wdt_dev, nowayout);
+	watchdog_set_restart_priority(&lpc18xx_wdt->wdt_dev, 128);
 
 	platform_set_drvdata(pdev, lpc18xx_wdt);
 
@@ -280,12 +278,6 @@ static int lpc18xx_wdt_probe(struct platform_device *pdev)
 	if (ret)
 		goto disable_wdt_clk;
 
-	lpc18xx_wdt->restart_handler.notifier_call = lpc18xx_wdt_restart;
-	lpc18xx_wdt->restart_handler.priority = 128;
-	ret = register_restart_handler(&lpc18xx_wdt->restart_handler);
-	if (ret)
-		dev_warn(dev, "failed to register restart handler: %d\n", ret);
-
 	return 0;
 
 disable_wdt_clk:
@@ -306,8 +298,6 @@ static int lpc18xx_wdt_remove(struct platform_device *pdev)
 {
 	struct lpc18xx_wdt_dev *lpc18xx_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&lpc18xx_wdt->restart_handler);
-
 	dev_warn(&pdev->dev, "I quit now, hardware will probably reboot!\n");
 	del_timer(&lpc18xx_wdt->timer);
 
diff --git a/drivers/watchdog/max63xx_wdt.c b/drivers/watchdog/max63xx_wdt.c
index f36ca4be0720..ac5840d9689a 100644
--- a/drivers/watchdog/max63xx_wdt.c
+++ b/drivers/watchdog/max63xx_wdt.c
@@ -292,4 +292,4 @@ MODULE_PARM_DESC(nodelay,
 		 "Force selection of a timeout setting without initial delay "
 		 "(max6373/74 only, default=0)");
 
-MODULE_LICENSE("GPL");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/watchdog/mena21_wdt.c b/drivers/watchdog/mena21_wdt.c
index 098fa9c34d6d..af6a7c489f08 100644
--- a/drivers/watchdog/mena21_wdt.c
+++ b/drivers/watchdog/mena21_wdt.c
@@ -100,12 +100,12 @@ static int a21_wdt_set_timeout(struct watchdog_device *wdt,
 	struct a21_wdt_drv *drv = watchdog_get_drvdata(wdt);
 
 	if (timeout != 1 && timeout != 30) {
-		dev_err(wdt->dev, "Only 1 and 30 allowed as timeout\n");
+		dev_err(wdt->parent, "Only 1 and 30 allowed as timeout\n");
 		return -EINVAL;
 	}
 
 	if (timeout == 30 && wdt->timeout == 1) {
-		dev_err(wdt->dev,
+		dev_err(wdt->parent,
 			"Transition from fast to slow mode not allowed\n");
 		return -EINVAL;
 	}
diff --git a/drivers/watchdog/meson_wdt.c b/drivers/watchdog/meson_wdt.c
index 1f4155ee3404..aea5d2f44ad7 100644
--- a/drivers/watchdog/meson_wdt.c
+++ b/drivers/watchdog/meson_wdt.c
@@ -17,51 +17,64 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <linux/notifier.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/watchdog.h>
 
 #define DRV_NAME		"meson_wdt"
 
 #define MESON_WDT_TC		0x00
-#define MESON_WDT_TC_EN		BIT(22)
-#define MESON_WDT_TC_TM_MASK	0x3fffff
 #define MESON_WDT_DC_RESET	(3 << 24)
 
 #define MESON_WDT_RESET		0x04
 
 #define MESON_WDT_TIMEOUT	30
 #define MESON_WDT_MIN_TIMEOUT	1
-#define MESON_WDT_MAX_TIMEOUT	(MESON_WDT_TC_TM_MASK / 100000)
 
-#define MESON_SEC_TO_TC(s)	((s) * 100000)
+#define MESON_SEC_TO_TC(s, c)	((s) * (c))
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
 static unsigned int timeout = MESON_WDT_TIMEOUT;
 
+struct meson_wdt_data {
+	unsigned int enable;
+	unsigned int terminal_count_mask;
+	unsigned int count_unit;
+};
+
+static struct meson_wdt_data meson6_wdt_data = {
+	.enable			= BIT(22),
+	.terminal_count_mask	= 0x3fffff,
+	.count_unit		= 100000, /* 10 us */
+};
+
+static struct meson_wdt_data meson8b_wdt_data = {
+	.enable			= BIT(19),
+	.terminal_count_mask	= 0xffff,
+	.count_unit		= 7812, /* 128 us */
+};
+
 struct meson_wdt_dev {
 	struct watchdog_device wdt_dev;
 	void __iomem *wdt_base;
-	struct notifier_block restart_handler;
+	const struct meson_wdt_data *data;
 };
 
-static int meson_restart_handle(struct notifier_block *this, unsigned long mode,
-				void *cmd)
+static int meson_wdt_restart(struct watchdog_device *wdt_dev)
 {
-	u32 tc_reboot = MESON_WDT_DC_RESET | MESON_WDT_TC_EN;
-	struct meson_wdt_dev *meson_wdt = container_of(this,
-						       struct meson_wdt_dev,
-						       restart_handler);
+	struct meson_wdt_dev *meson_wdt = watchdog_get_drvdata(wdt_dev);
+	u32 tc_reboot = MESON_WDT_DC_RESET;
+
+	tc_reboot |= meson_wdt->data->enable;
 
 	while (1) {
 		writel(tc_reboot, meson_wdt->wdt_base + MESON_WDT_TC);
 		mdelay(5);
 	}
 
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static int meson_wdt_ping(struct watchdog_device *wdt_dev)
@@ -80,8 +93,8 @@ static void meson_wdt_change_timeout(struct watchdog_device *wdt_dev,
 	u32 reg;
 
 	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
-	reg &= ~MESON_WDT_TC_TM_MASK;
-	reg |= MESON_SEC_TO_TC(timeout);
+	reg &= ~meson_wdt->data->terminal_count_mask;
+	reg |= MESON_SEC_TO_TC(timeout, meson_wdt->data->count_unit);
 	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
 }
 
@@ -102,7 +115,7 @@ static int meson_wdt_stop(struct watchdog_device *wdt_dev)
 	u32 reg;
 
 	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
-	reg &= ~MESON_WDT_TC_EN;
+	reg &= ~meson_wdt->data->enable;
 	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
 
 	return 0;
@@ -117,7 +130,7 @@ static int meson_wdt_start(struct watchdog_device *wdt_dev)
 	meson_wdt_ping(wdt_dev);
 
 	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
-	reg |= MESON_WDT_TC_EN;
+	reg |= meson_wdt->data->enable;
 	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
 
 	return 0;
@@ -136,12 +149,21 @@ static const struct watchdog_ops meson_wdt_ops = {
 	.stop		= meson_wdt_stop,
 	.ping		= meson_wdt_ping,
 	.set_timeout	= meson_wdt_set_timeout,
+	.restart        = meson_wdt_restart,
+};
+
+static const struct of_device_id meson_wdt_dt_ids[] = {
+	{ .compatible = "amlogic,meson6-wdt", .data = &meson6_wdt_data },
+	{ .compatible = "amlogic,meson8b-wdt", .data = &meson8b_wdt_data },
+	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(of, meson_wdt_dt_ids);
 
 static int meson_wdt_probe(struct platform_device *pdev)
 {
 	struct resource *res;
 	struct meson_wdt_dev *meson_wdt;
+	const struct of_device_id *of_id;
 	int err;
 
 	meson_wdt = devm_kzalloc(&pdev->dev, sizeof(*meson_wdt), GFP_KERNEL);
@@ -153,17 +175,28 @@ static int meson_wdt_probe(struct platform_device *pdev)
 	if (IS_ERR(meson_wdt->wdt_base))
 		return PTR_ERR(meson_wdt->wdt_base);
 
+	of_id = of_match_device(meson_wdt_dt_ids, &pdev->dev);
+	if (!of_id) {
+		dev_err(&pdev->dev, "Unable to initialize WDT data\n");
+		return -ENODEV;
+	}
+	meson_wdt->data = of_id->data;
+
 	meson_wdt->wdt_dev.parent = &pdev->dev;
 	meson_wdt->wdt_dev.info = &meson_wdt_info;
 	meson_wdt->wdt_dev.ops = &meson_wdt_ops;
-	meson_wdt->wdt_dev.timeout = MESON_WDT_TIMEOUT;
-	meson_wdt->wdt_dev.max_timeout = MESON_WDT_MAX_TIMEOUT;
+	meson_wdt->wdt_dev.max_timeout =
+		meson_wdt->data->terminal_count_mask / meson_wdt->data->count_unit;
 	meson_wdt->wdt_dev.min_timeout = MESON_WDT_MIN_TIMEOUT;
+	meson_wdt->wdt_dev.timeout = min_t(unsigned int,
+					   MESON_WDT_TIMEOUT,
+					   meson_wdt->wdt_dev.max_timeout);
 
 	watchdog_set_drvdata(&meson_wdt->wdt_dev, meson_wdt);
 
 	watchdog_init_timeout(&meson_wdt->wdt_dev, timeout, &pdev->dev);
 	watchdog_set_nowayout(&meson_wdt->wdt_dev, nowayout);
+	watchdog_set_restart_priority(&meson_wdt->wdt_dev, 128);
 
 	meson_wdt_stop(&meson_wdt->wdt_dev);
 
@@ -173,13 +206,6 @@ static int meson_wdt_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, meson_wdt);
 
-	meson_wdt->restart_handler.notifier_call = meson_restart_handle;
-	meson_wdt->restart_handler.priority = 128;
-	err = register_restart_handler(&meson_wdt->restart_handler);
-	if (err)
-		dev_err(&pdev->dev,
-			"cannot register restart handler (err=%d)\n", err);
-
 	dev_info(&pdev->dev, "Watchdog enabled (timeout=%d sec, nowayout=%d)",
 		 meson_wdt->wdt_dev.timeout, nowayout);
 
@@ -190,8 +216,6 @@ static int meson_wdt_remove(struct platform_device *pdev)
 {
 	struct meson_wdt_dev *meson_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&meson_wdt->restart_handler);
-
 	watchdog_unregister_device(&meson_wdt->wdt_dev);
 
 	return 0;
@@ -204,12 +228,6 @@ static void meson_wdt_shutdown(struct platform_device *pdev)
 	meson_wdt_stop(&meson_wdt->wdt_dev);
 }
 
-static const struct of_device_id meson_wdt_dt_ids[] = {
-	{ .compatible = "amlogic,meson6-wdt" },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, meson_wdt_dt_ids);
-
 static struct platform_driver meson_wdt_driver = {
 	.probe		= meson_wdt_probe,
 	.remove		= meson_wdt_remove,
diff --git a/drivers/watchdog/moxart_wdt.c b/drivers/watchdog/moxart_wdt.c
index 60b0605bd7e6..885c81bc4210 100644
--- a/drivers/watchdog/moxart_wdt.c
+++ b/drivers/watchdog/moxart_wdt.c
@@ -15,9 +15,7 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/kernel.h>
-#include <linux/notifier.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/watchdog.h>
 #include <linux/moduleparam.h>
 
@@ -29,22 +27,19 @@ struct moxart_wdt_dev {
 	struct watchdog_device dev;
 	void __iomem *base;
 	unsigned int clock_frequency;
-	struct notifier_block restart_handler;
 };
 
 static int heartbeat;
 
-static int moxart_restart_handle(struct notifier_block *this,
-				 unsigned long mode, void *cmd)
+static int moxart_wdt_restart(struct watchdog_device *wdt_dev)
 {
-	struct moxart_wdt_dev *moxart_wdt = container_of(this,
-							 struct moxart_wdt_dev,
-							 restart_handler);
+	struct moxart_wdt_dev *moxart_wdt = watchdog_get_drvdata(wdt_dev);
+
 	writel(1, moxart_wdt->base + REG_COUNT);
 	writel(0x5ab9, moxart_wdt->base + REG_MODE);
 	writel(0x03, moxart_wdt->base + REG_ENABLE);
 
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static int moxart_wdt_stop(struct watchdog_device *wdt_dev)
@@ -87,6 +82,7 @@ static const struct watchdog_ops moxart_wdt_ops = {
 	.start          = moxart_wdt_start,
 	.stop           = moxart_wdt_stop,
 	.set_timeout    = moxart_wdt_set_timeout,
+	.restart        = moxart_wdt_restart,
 };
 
 static int moxart_wdt_probe(struct platform_device *pdev)
@@ -134,6 +130,7 @@ static int moxart_wdt_probe(struct platform_device *pdev)
 
 	watchdog_init_timeout(&moxart_wdt->dev, heartbeat, dev);
 	watchdog_set_nowayout(&moxart_wdt->dev, nowayout);
+	watchdog_set_restart_priority(&moxart_wdt->dev, 128);
 
 	watchdog_set_drvdata(&moxart_wdt->dev, moxart_wdt);
 
@@ -141,13 +138,6 @@ static int moxart_wdt_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	moxart_wdt->restart_handler.notifier_call = moxart_restart_handle;
-	moxart_wdt->restart_handler.priority = 128;
-	err = register_restart_handler(&moxart_wdt->restart_handler);
-	if (err)
-		dev_err(dev, "cannot register restart notifier (err=%d)\n",
-			err);
-
 	dev_dbg(dev, "Watchdog enabled (heartbeat=%d sec, nowayout=%d)\n",
 		moxart_wdt->dev.timeout, nowayout);
 
@@ -158,7 +148,6 @@ static int moxart_wdt_remove(struct platform_device *pdev)
 {
 	struct moxart_wdt_dev *moxart_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&moxart_wdt->restart_handler);
 	moxart_wdt_stop(&moxart_wdt->dev);
 
 	return 0;
diff --git a/drivers/watchdog/mt7621_wdt.c b/drivers/watchdog/mt7621_wdt.c
new file mode 100644
index 000000000000..4a2290f900a8
--- /dev/null
+++ b/drivers/watchdog/mt7621_wdt.c
@@ -0,0 +1,186 @@
+/*
+ * Ralink MT7621/MT7628 built-in hardware watchdog timer
+ *
+ * Copyright (C) 2014 John Crispin <blogic@openwrt.org>
+ *
+ * This driver was based on: drivers/watchdog/rt2880_wdt.c
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/reset.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/watchdog.h>
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+
+#include <asm/mach-ralink/ralink_regs.h>
+
+#define SYSC_RSTSTAT			0x38
+#define WDT_RST_CAUSE			BIT(1)
+
+#define RALINK_WDT_TIMEOUT		30
+
+#define TIMER_REG_TMRSTAT		0x00
+#define TIMER_REG_TMR1LOAD		0x24
+#define TIMER_REG_TMR1CTL		0x20
+
+#define TMR1CTL_ENABLE			BIT(7)
+#define TMR1CTL_RESTART			BIT(9)
+#define TMR1CTL_PRESCALE_SHIFT		16
+
+static void __iomem *mt7621_wdt_base;
+static struct reset_control *mt7621_wdt_reset;
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout,
+		 "Watchdog cannot be stopped once started (default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static inline void rt_wdt_w32(unsigned reg, u32 val)
+{
+	iowrite32(val, mt7621_wdt_base + reg);
+}
+
+static inline u32 rt_wdt_r32(unsigned reg)
+{
+	return ioread32(mt7621_wdt_base + reg);
+}
+
+static int mt7621_wdt_ping(struct watchdog_device *w)
+{
+	rt_wdt_w32(TIMER_REG_TMRSTAT, TMR1CTL_RESTART);
+
+	return 0;
+}
+
+static int mt7621_wdt_set_timeout(struct watchdog_device *w, unsigned int t)
+{
+	w->timeout = t;
+	rt_wdt_w32(TIMER_REG_TMR1LOAD, t * 1000);
+	mt7621_wdt_ping(w);
+
+	return 0;
+}
+
+static int mt7621_wdt_start(struct watchdog_device *w)
+{
+	u32 t;
+
+	/* set the prescaler to 1ms == 1000us */
+	rt_wdt_w32(TIMER_REG_TMR1CTL, 1000 << TMR1CTL_PRESCALE_SHIFT);
+
+	mt7621_wdt_set_timeout(w, w->timeout);
+
+	t = rt_wdt_r32(TIMER_REG_TMR1CTL);
+	t |= TMR1CTL_ENABLE;
+	rt_wdt_w32(TIMER_REG_TMR1CTL, t);
+
+	return 0;
+}
+
+static int mt7621_wdt_stop(struct watchdog_device *w)
+{
+	u32 t;
+
+	mt7621_wdt_ping(w);
+
+	t = rt_wdt_r32(TIMER_REG_TMR1CTL);
+	t &= ~TMR1CTL_ENABLE;
+	rt_wdt_w32(TIMER_REG_TMR1CTL, t);
+
+	return 0;
+}
+
+static int mt7621_wdt_bootcause(void)
+{
+	if (rt_sysc_r32(SYSC_RSTSTAT) & WDT_RST_CAUSE)
+		return WDIOF_CARDRESET;
+
+	return 0;
+}
+
+static struct watchdog_info mt7621_wdt_info = {
+	.identity = "Mediatek Watchdog",
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
+};
+
+static struct watchdog_ops mt7621_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = mt7621_wdt_start,
+	.stop = mt7621_wdt_stop,
+	.ping = mt7621_wdt_ping,
+	.set_timeout = mt7621_wdt_set_timeout,
+};
+
+static struct watchdog_device mt7621_wdt_dev = {
+	.info = &mt7621_wdt_info,
+	.ops = &mt7621_wdt_ops,
+	.min_timeout = 1,
+	.max_timeout = 0xfffful / 1000,
+};
+
+static int mt7621_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	mt7621_wdt_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(mt7621_wdt_base))
+		return PTR_ERR(mt7621_wdt_base);
+
+	mt7621_wdt_reset = devm_reset_control_get(&pdev->dev, NULL);
+	if (!IS_ERR(mt7621_wdt_reset))
+		reset_control_deassert(mt7621_wdt_reset);
+
+	mt7621_wdt_dev.dev = &pdev->dev;
+	mt7621_wdt_dev.bootstatus = mt7621_wdt_bootcause();
+
+	watchdog_init_timeout(&mt7621_wdt_dev, mt7621_wdt_dev.max_timeout,
+			      &pdev->dev);
+	watchdog_set_nowayout(&mt7621_wdt_dev, nowayout);
+
+	ret = watchdog_register_device(&mt7621_wdt_dev);
+
+	return 0;
+}
+
+static int mt7621_wdt_remove(struct platform_device *pdev)
+{
+	watchdog_unregister_device(&mt7621_wdt_dev);
+
+	return 0;
+}
+
+static void mt7621_wdt_shutdown(struct platform_device *pdev)
+{
+	mt7621_wdt_stop(&mt7621_wdt_dev);
+}
+
+static const struct of_device_id mt7621_wdt_match[] = {
+	{ .compatible = "mediatek,mt7621-wdt" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, mt7621_wdt_match);
+
+static struct platform_driver mt7621_wdt_driver = {
+	.probe		= mt7621_wdt_probe,
+	.remove		= mt7621_wdt_remove,
+	.shutdown	= mt7621_wdt_shutdown,
+	.driver		= {
+		.name		= KBUILD_MODNAME,
+		.of_match_table	= mt7621_wdt_match,
+	},
+};
+
+module_platform_driver(mt7621_wdt_driver);
+
+MODULE_DESCRIPTION("MediaTek MT762x hardware watchdog driver");
+MODULE_AUTHOR("John Crispin <blogic@openwrt.org");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/watchdog/mtk_wdt.c b/drivers/watchdog/mtk_wdt.c
index b751f43d76ed..b78776c05554 100644
--- a/drivers/watchdog/mtk_wdt.c
+++ b/drivers/watchdog/mtk_wdt.c
@@ -28,8 +28,6 @@
 #include <linux/platform_device.h>
 #include <linux/types.h>
 #include <linux/watchdog.h>
-#include <linux/notifier.h>
-#include <linux/reboot.h>
 #include <linux/delay.h>
 
 #define WDT_MAX_TIMEOUT		31
@@ -64,16 +62,13 @@ static unsigned int timeout = WDT_MAX_TIMEOUT;
 struct mtk_wdt_dev {
 	struct watchdog_device wdt_dev;
 	void __iomem *wdt_base;
-	struct notifier_block restart_handler;
 };
 
-static int mtk_reset_handler(struct notifier_block *this, unsigned long mode,
-				void *cmd)
+static int mtk_wdt_restart(struct watchdog_device *wdt_dev)
 {
-	struct mtk_wdt_dev *mtk_wdt;
+	struct mtk_wdt_dev *mtk_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base;
 
-	mtk_wdt = container_of(this, struct mtk_wdt_dev, restart_handler);
 	wdt_base = mtk_wdt->wdt_base;
 
 	while (1) {
@@ -81,7 +76,7 @@ static int mtk_reset_handler(struct notifier_block *this, unsigned long mode,
 		mdelay(5);
 	}
 
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static int mtk_wdt_ping(struct watchdog_device *wdt_dev)
@@ -161,6 +156,7 @@ static const struct watchdog_ops mtk_wdt_ops = {
 	.stop		= mtk_wdt_stop,
 	.ping		= mtk_wdt_ping,
 	.set_timeout	= mtk_wdt_set_timeout,
+	.restart	= mtk_wdt_restart,
 };
 
 static int mtk_wdt_probe(struct platform_device *pdev)
@@ -189,6 +185,7 @@ static int mtk_wdt_probe(struct platform_device *pdev)
 
 	watchdog_init_timeout(&mtk_wdt->wdt_dev, timeout, &pdev->dev);
 	watchdog_set_nowayout(&mtk_wdt->wdt_dev, nowayout);
+	watchdog_set_restart_priority(&mtk_wdt->wdt_dev, 128);
 
 	watchdog_set_drvdata(&mtk_wdt->wdt_dev, mtk_wdt);
 
@@ -198,13 +195,6 @@ static int mtk_wdt_probe(struct platform_device *pdev)
 	if (unlikely(err))
 		return err;
 
-	mtk_wdt->restart_handler.notifier_call = mtk_reset_handler;
-	mtk_wdt->restart_handler.priority = 128;
-	err = register_restart_handler(&mtk_wdt->restart_handler);
-	if (err)
-		dev_warn(&pdev->dev,
-			"cannot register restart handler (err=%d)\n", err);
-
 	dev_info(&pdev->dev, "Watchdog enabled (timeout=%d sec, nowayout=%d)\n",
 			mtk_wdt->wdt_dev.timeout, nowayout);
 
@@ -223,8 +213,6 @@ static int mtk_wdt_remove(struct platform_device *pdev)
 {
 	struct mtk_wdt_dev *mtk_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&mtk_wdt->restart_handler);
-
 	watchdog_unregister_device(&mtk_wdt->wdt_dev);
 
 	return 0;
diff --git a/drivers/watchdog/omap_wdt.c b/drivers/watchdog/omap_wdt.c
index 6f17c935a6cf..1b02bfa81b29 100644
--- a/drivers/watchdog/omap_wdt.c
+++ b/drivers/watchdog/omap_wdt.c
@@ -271,7 +271,8 @@ static int omap_wdt_probe(struct platform_device *pdev)
 			wdev->wdog.bootstatus = WDIOF_CARDRESET;
 	}
 
-	omap_wdt_disable(wdev);
+	if (!early_enable)
+		omap_wdt_disable(wdev);
 
 	ret = watchdog_register_device(&wdev->wdog);
 	if (ret) {
@@ -283,11 +284,11 @@ static int omap_wdt_probe(struct platform_device *pdev)
 		readl_relaxed(wdev->base + OMAP_WATCHDOG_REV) & 0xFF,
 		wdev->wdog.timeout);
 
-	pm_runtime_put_sync(wdev->dev);
-
 	if (early_enable)
 		omap_wdt_start(&wdev->wdog);
 
+	pm_runtime_put(wdev->dev);
+
 	return 0;
 }
 
diff --git a/drivers/watchdog/pcwd_usb.c b/drivers/watchdog/pcwd_usb.c
index 1a11aedc4fe8..68952d9ccf83 100644
--- a/drivers/watchdog/pcwd_usb.c
+++ b/drivers/watchdog/pcwd_usb.c
@@ -608,7 +608,7 @@ static int usb_pcwd_probe(struct usb_interface *interface,
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
 	struct usb_pcwd_private *usb_pcwd = NULL;
-	int pipe, maxp;
+	int pipe;
 	int retval = -ENOMEM;
 	int got_fw_rev;
 	unsigned char fw_rev_major, fw_rev_minor;
@@ -641,7 +641,6 @@ static int usb_pcwd_probe(struct usb_interface *interface,
 
 	/* get a handle to the interrupt data pipe */
 	pipe = usb_rcvintpipe(udev, endpoint->bEndpointAddress);
-	maxp = usb_maxpacket(udev, pipe, usb_pipeout(pipe));
 
 	/* allocate memory for our device and initialize it */
 	usb_pcwd = kzalloc(sizeof(struct usb_pcwd_private), GFP_KERNEL);
diff --git a/drivers/watchdog/qcom-wdt.c b/drivers/watchdog/qcom-wdt.c
index 773dcfaee7b2..424f9a952fee 100644
--- a/drivers/watchdog/qcom-wdt.c
+++ b/drivers/watchdog/qcom-wdt.c
@@ -17,7 +17,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/watchdog.h>
 
 #define WDT_RST		0x38
@@ -28,7 +27,6 @@ struct qcom_wdt {
 	struct watchdog_device	wdd;
 	struct clk		*clk;
 	unsigned long		rate;
-	struct notifier_block	restart_nb;
 	void __iomem		*base;
 };
 
@@ -72,25 +70,9 @@ static int qcom_wdt_set_timeout(struct watchdog_device *wdd,
 	return qcom_wdt_start(wdd);
 }
 
-static const struct watchdog_ops qcom_wdt_ops = {
-	.start		= qcom_wdt_start,
-	.stop		= qcom_wdt_stop,
-	.ping		= qcom_wdt_ping,
-	.set_timeout	= qcom_wdt_set_timeout,
-	.owner		= THIS_MODULE,
-};
-
-static const struct watchdog_info qcom_wdt_info = {
-	.options	= WDIOF_KEEPALIVEPING
-			| WDIOF_MAGICCLOSE
-			| WDIOF_SETTIMEOUT,
-	.identity	= KBUILD_MODNAME,
-};
-
-static int qcom_wdt_restart(struct notifier_block *nb, unsigned long action,
-			    void *data)
+static int qcom_wdt_restart(struct watchdog_device *wdd)
 {
-	struct qcom_wdt *wdt = container_of(nb, struct qcom_wdt, restart_nb);
+	struct qcom_wdt *wdt = to_qcom_wdt(wdd);
 	u32 timeout;
 
 	/*
@@ -110,9 +92,25 @@ static int qcom_wdt_restart(struct notifier_block *nb, unsigned long action,
 	wmb();
 
 	msleep(150);
-	return NOTIFY_DONE;
+	return 0;
 }
 
+static const struct watchdog_ops qcom_wdt_ops = {
+	.start		= qcom_wdt_start,
+	.stop		= qcom_wdt_stop,
+	.ping		= qcom_wdt_ping,
+	.set_timeout	= qcom_wdt_set_timeout,
+	.restart        = qcom_wdt_restart,
+	.owner		= THIS_MODULE,
+};
+
+static const struct watchdog_info qcom_wdt_info = {
+	.options	= WDIOF_KEEPALIVEPING
+			| WDIOF_MAGICCLOSE
+			| WDIOF_SETTIMEOUT,
+	.identity	= KBUILD_MODNAME,
+};
+
 static int qcom_wdt_probe(struct platform_device *pdev)
 {
 	struct qcom_wdt *wdt;
@@ -166,7 +164,6 @@ static int qcom_wdt_probe(struct platform_device *pdev)
 		goto err_clk_unprepare;
 	}
 
-	wdt->wdd.dev = &pdev->dev;
 	wdt->wdd.info = &qcom_wdt_info;
 	wdt->wdd.ops = &qcom_wdt_ops;
 	wdt->wdd.min_timeout = 1;
@@ -187,14 +184,6 @@ static int qcom_wdt_probe(struct platform_device *pdev)
 		goto err_clk_unprepare;
 	}
 
-	/*
-	 * WDT restart notifier has priority 0 (use as a last resort)
-	 */
-	wdt->restart_nb.notifier_call = qcom_wdt_restart;
-	ret = register_restart_handler(&wdt->restart_nb);
-	if (ret)
-		dev_err(&pdev->dev, "failed to setup restart handler\n");
-
 	platform_set_drvdata(pdev, wdt);
 	return 0;
 
@@ -207,7 +196,6 @@ static int qcom_wdt_remove(struct platform_device *pdev)
 {
 	struct qcom_wdt *wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&wdt->restart_nb);
 	watchdog_unregister_device(&wdt->wdd);
 	clk_disable_unprepare(wdt->clk);
 	return 0;
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index d781000c7825..0093450441fe 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -41,7 +41,6 @@
 #include <linux/of.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
-#include <linux/reboot.h>
 #include <linux/delay.h>
 
 #define S3C2410_WTCON		0x00
@@ -130,7 +129,6 @@ struct s3c2410_wdt {
 	unsigned long		wtdat_save;
 	struct watchdog_device	wdt_device;
 	struct notifier_block	freq_transition;
-	struct notifier_block	restart_handler;
 	struct s3c2410_wdt_variant *drv_data;
 	struct regmap *pmureg;
 };
@@ -351,6 +349,29 @@ static int s3c2410wdt_set_heartbeat(struct watchdog_device *wdd, unsigned timeou
 	return 0;
 }
 
+static int s3c2410wdt_restart(struct watchdog_device *wdd)
+{
+	struct s3c2410_wdt *wdt = watchdog_get_drvdata(wdd);
+	void __iomem *wdt_base = wdt->reg_base;
+
+	/* disable watchdog, to be safe  */
+	writel(0, wdt_base + S3C2410_WTCON);
+
+	/* put initial values into count and data */
+	writel(0x80, wdt_base + S3C2410_WTCNT);
+	writel(0x80, wdt_base + S3C2410_WTDAT);
+
+	/* set the watchdog to go and reset... */
+	writel(S3C2410_WTCON_ENABLE | S3C2410_WTCON_DIV16 |
+		S3C2410_WTCON_RSTEN | S3C2410_WTCON_PRESCALE(0x20),
+		wdt_base + S3C2410_WTCON);
+
+	/* wait for reset to assert... */
+	mdelay(500);
+
+	return 0;
+}
+
 #define OPTIONS (WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE)
 
 static const struct watchdog_info s3c2410_wdt_ident = {
@@ -365,6 +386,7 @@ static struct watchdog_ops s3c2410wdt_ops = {
 	.stop = s3c2410wdt_stop,
 	.ping = s3c2410wdt_keepalive,
 	.set_timeout = s3c2410wdt_set_heartbeat,
+	.restart = s3c2410wdt_restart,
 };
 
 static struct watchdog_device s3c2410_wdd = {
@@ -452,31 +474,6 @@ static inline void s3c2410wdt_cpufreq_deregister(struct s3c2410_wdt *wdt)
 }
 #endif
 
-static int s3c2410wdt_restart(struct notifier_block *this,
-			      unsigned long mode, void *cmd)
-{
-	struct s3c2410_wdt *wdt = container_of(this, struct s3c2410_wdt,
-					       restart_handler);
-	void __iomem *wdt_base = wdt->reg_base;
-
-	/* disable watchdog, to be safe  */
-	writel(0, wdt_base + S3C2410_WTCON);
-
-	/* put initial values into count and data */
-	writel(0x80, wdt_base + S3C2410_WTCNT);
-	writel(0x80, wdt_base + S3C2410_WTDAT);
-
-	/* set the watchdog to go and reset... */
-	writel(S3C2410_WTCON_ENABLE | S3C2410_WTCON_DIV16 |
-		S3C2410_WTCON_RSTEN | S3C2410_WTCON_PRESCALE(0x20),
-		wdt_base + S3C2410_WTCON);
-
-	/* wait for reset to assert... */
-	mdelay(500);
-
-	return NOTIFY_DONE;
-}
-
 static inline unsigned int s3c2410wdt_get_bootstatus(struct s3c2410_wdt *wdt)
 {
 	unsigned int rst_stat;
@@ -605,6 +602,7 @@ static int s3c2410wdt_probe(struct platform_device *pdev)
 	}
 
 	watchdog_set_nowayout(&wdt->wdt_device, nowayout);
+	watchdog_set_restart_priority(&wdt->wdt_device, 128);
 
 	wdt->wdt_device.bootstatus = s3c2410wdt_get_bootstatus(wdt);
 	wdt->wdt_device.parent = &pdev->dev;
@@ -632,12 +630,6 @@ static int s3c2410wdt_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, wdt);
 
-	wdt->restart_handler.notifier_call = s3c2410wdt_restart;
-	wdt->restart_handler.priority = 128;
-	ret = register_restart_handler(&wdt->restart_handler);
-	if (ret)
-		pr_err("cannot register restart handler, %d\n", ret);
-
 	/* print out a statement of readiness */
 
 	wtcon = readl(wdt->reg_base + S3C2410_WTCON);
@@ -667,8 +659,6 @@ static int s3c2410wdt_remove(struct platform_device *dev)
 	int ret;
 	struct s3c2410_wdt *wdt = platform_get_drvdata(dev);
 
-	unregister_restart_handler(&wdt->restart_handler);
-
 	ret = s3c2410wdt_mask_and_disable_reset(wdt, true);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/watchdog/softdog.c b/drivers/watchdog/softdog.c
index 0dc5e323d59d..99a06f9e3930 100644
--- a/drivers/watchdog/softdog.c
+++ b/drivers/watchdog/softdog.c
@@ -43,7 +43,6 @@
 #include <linux/types.h>
 #include <linux/timer.h>
 #include <linux/watchdog.h>
-#include <linux/notifier.h>
 #include <linux/reboot.h>
 #include <linux/init.h>
 #include <linux/jiffies.h>
@@ -87,6 +86,7 @@ static struct timer_list watchdog_ticktock =
 
 static void watchdog_fire(unsigned long data)
 {
+	module_put(THIS_MODULE);
 	if (soft_noboot)
 		pr_crit("Triggered - Reboot ignored\n");
 	else if (soft_panic) {
@@ -105,13 +105,16 @@ static void watchdog_fire(unsigned long data)
 
 static int softdog_ping(struct watchdog_device *w)
 {
-	mod_timer(&watchdog_ticktock, jiffies+(w->timeout*HZ));
+	if (!mod_timer(&watchdog_ticktock, jiffies+(w->timeout*HZ)))
+		__module_get(THIS_MODULE);
 	return 0;
 }
 
 static int softdog_stop(struct watchdog_device *w)
 {
-	del_timer(&watchdog_ticktock);
+	if (del_timer(&watchdog_ticktock))
+		module_put(THIS_MODULE);
+
 	return 0;
 }
 
@@ -122,26 +125,9 @@ static int softdog_set_timeout(struct watchdog_device *w, unsigned int t)
 }
 
 /*
- *	Notifier for system down
- */
-
-static int softdog_notify_sys(struct notifier_block *this, unsigned long code,
-	void *unused)
-{
-	if (code == SYS_DOWN || code == SYS_HALT)
-		/* Turn the WDT off */
-		softdog_stop(NULL);
-	return NOTIFY_DONE;
-}
-
-/*
  *	Kernel Interfaces
  */
 
-static struct notifier_block softdog_notifier = {
-	.notifier_call	= softdog_notify_sys,
-};
-
 static struct watchdog_info softdog_info = {
 	.identity = "Software Watchdog",
 	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
@@ -175,18 +161,11 @@ static int __init watchdog_init(void)
 	softdog_dev.timeout = soft_margin;
 
 	watchdog_set_nowayout(&softdog_dev, nowayout);
-
-	ret = register_reboot_notifier(&softdog_notifier);
-	if (ret) {
-		pr_err("cannot register reboot notifier (err=%d)\n", ret);
-		return ret;
-	}
+	watchdog_stop_on_reboot(&softdog_dev);
 
 	ret = watchdog_register_device(&softdog_dev);
-	if (ret) {
-		unregister_reboot_notifier(&softdog_notifier);
+	if (ret)
 		return ret;
-	}
 
 	pr_info("Software Watchdog Timer: 0.08 initialized. soft_noboot=%d soft_margin=%d sec soft_panic=%d (nowayout=%d)\n",
 		soft_noboot, soft_margin, soft_panic, nowayout);
@@ -197,7 +176,6 @@ static int __init watchdog_init(void)
 static void __exit watchdog_exit(void)
 {
 	watchdog_unregister_device(&softdog_dev);
-	unregister_reboot_notifier(&softdog_notifier);
 }
 
 module_init(watchdog_init);
diff --git a/drivers/watchdog/sp5100_tco.c b/drivers/watchdog/sp5100_tco.c
index eb8044ef0ea0..6467b91f2245 100644
--- a/drivers/watchdog/sp5100_tco.c
+++ b/drivers/watchdog/sp5100_tco.c
@@ -306,6 +306,10 @@ static struct miscdevice sp5100_tco_miscdev = {
 static const struct pci_device_id sp5100_tco_pci_tbl[] = {
 	{ PCI_VENDOR_ID_ATI, PCI_DEVICE_ID_ATI_SBX00_SMBUS, PCI_ANY_ID,
 	  PCI_ANY_ID, },
+	{ PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_HUDSON2_SMBUS, PCI_ANY_ID,
+	  PCI_ANY_ID, },
+	{ PCI_VENDOR_ID_AMD, PCI_DEVICE_ID_AMD_KERNCZ_SMBUS, PCI_ANY_ID,
+	  PCI_ANY_ID, },
 	{ 0, },			/* End of list */
 };
 MODULE_DEVICE_TABLE(pci, sp5100_tco_pci_tbl);
@@ -331,21 +335,24 @@ static unsigned char sp5100_tco_setupdevice(void)
 	if (!sp5100_tco_pci)
 		return 0;
 
-	pr_info("PCI Revision ID: 0x%x\n", sp5100_tco_pci->revision);
+	pr_info("PCI Vendor ID: 0x%x, Device ID: 0x%x, Revision ID: 0x%x\n",
+		sp5100_tco_pci->vendor, sp5100_tco_pci->device,
+		sp5100_tco_pci->revision);
 
 	/*
 	 * Determine type of southbridge chipset.
 	 */
-	if (sp5100_tco_pci->revision >= 0x40) {
-		dev_name = SB800_DEVNAME;
-		index_reg = SB800_IO_PM_INDEX_REG;
-		data_reg = SB800_IO_PM_DATA_REG;
-		base_addr = SB800_PM_WATCHDOG_BASE;
-	} else {
+	if (sp5100_tco_pci->device == PCI_DEVICE_ID_ATI_SBX00_SMBUS &&
+	    sp5100_tco_pci->revision < 0x40) {
 		dev_name = SP5100_DEVNAME;
 		index_reg = SP5100_IO_PM_INDEX_REG;
 		data_reg = SP5100_IO_PM_DATA_REG;
 		base_addr = SP5100_PM_WATCHDOG_BASE;
+	} else {
+		dev_name = SB800_DEVNAME;
+		index_reg = SB800_IO_PM_INDEX_REG;
+		data_reg = SB800_IO_PM_DATA_REG;
+		base_addr = SB800_PM_WATCHDOG_BASE;
 	}
 
 	/* Request the IO ports used by this driver */
@@ -381,7 +388,12 @@ static unsigned char sp5100_tco_setupdevice(void)
 	 * Secondly, Find the watchdog timer MMIO address
 	 * from SBResource_MMIO register.
 	 */
-	if (sp5100_tco_pci->revision >= 0x40) {
+	if (sp5100_tco_pci->device == PCI_DEVICE_ID_ATI_SBX00_SMBUS &&
+	    sp5100_tco_pci->revision < 0x40) {
+		/* Read SBResource_MMIO from PCI config(PCI_Reg: 9Ch) */
+		pci_read_config_dword(sp5100_tco_pci,
+				      SP5100_SB_RESOURCE_MMIO_BASE, &val);
+	} else {
 		/* Read SBResource_MMIO from AcpiMmioEn(PM_Reg: 24h) */
 		outb(SB800_PM_ACPI_MMIO_EN+3, SB800_IO_PM_INDEX_REG);
 		val = inb(SB800_IO_PM_DATA_REG);
@@ -391,10 +403,6 @@ static unsigned char sp5100_tco_setupdevice(void)
 		val = val << 8 | inb(SB800_IO_PM_DATA_REG);
 		outb(SB800_PM_ACPI_MMIO_EN+0, SB800_IO_PM_INDEX_REG);
 		val = val << 8 | inb(SB800_IO_PM_DATA_REG);
-	} else {
-		/* Read SBResource_MMIO from PCI config(PCI_Reg: 9Ch) */
-		pci_read_config_dword(sp5100_tco_pci,
-				      SP5100_SB_RESOURCE_MMIO_BASE, &val);
 	}
 
 	/* The SBResource_MMIO is enabled and mapped memory space? */
diff --git a/drivers/watchdog/sp805_wdt.c b/drivers/watchdog/sp805_wdt.c
index 01d816251302..e7a715e82021 100644
--- a/drivers/watchdog/sp805_wdt.c
+++ b/drivers/watchdog/sp805_wdt.c
@@ -139,12 +139,11 @@ static int wdt_config(struct watchdog_device *wdd, bool ping)
 
 	writel_relaxed(UNLOCK, wdt->base + WDTLOCK);
 	writel_relaxed(wdt->load_val, wdt->base + WDTLOAD);
+	writel_relaxed(INT_MASK, wdt->base + WDTINTCLR);
 
-	if (!ping) {
-		writel_relaxed(INT_MASK, wdt->base + WDTINTCLR);
+	if (!ping)
 		writel_relaxed(INT_ENABLE | RESET_ENABLE, wdt->base +
 				WDTCONTROL);
-	}
 
 	writel_relaxed(LOCK, wdt->base + WDTLOCK);
 
diff --git a/drivers/watchdog/stmp3xxx_rtc_wdt.c b/drivers/watchdog/stmp3xxx_rtc_wdt.c
index 3ee6128a540e..d8b11eb269ad 100644
--- a/drivers/watchdog/stmp3xxx_rtc_wdt.c
+++ b/drivers/watchdog/stmp3xxx_rtc_wdt.c
@@ -14,6 +14,8 @@
 #include <linux/watchdog.h>
 #include <linux/platform_device.h>
 #include <linux/stmp3xxx_rtc_wdt.h>
+#include <linux/notifier.h>
+#include <linux/reboot.h>
 
 #define WDOG_TICK_RATE 1000 /* 1 kHz clock */
 #define STMP3XXX_DEFAULT_TIMEOUT 19
@@ -69,6 +71,25 @@ static struct watchdog_device stmp3xxx_wdd = {
 	.status = WATCHDOG_NOWAYOUT_INIT_STATUS,
 };
 
+static int wdt_notify_sys(struct notifier_block *nb, unsigned long code,
+			  void *unused)
+{
+	switch (code) {
+	case SYS_DOWN:	/* keep enabled, system might crash while going down */
+		break;
+	case SYS_HALT:	/* allow the system to actually halt */
+	case SYS_POWER_OFF:
+		wdt_stop(&stmp3xxx_wdd);
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block wdt_notifier = {
+	.notifier_call = wdt_notify_sys,
+};
+
 static int stmp3xxx_wdt_probe(struct platform_device *pdev)
 {
 	int ret;
@@ -84,6 +105,9 @@ static int stmp3xxx_wdt_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	if (register_reboot_notifier(&wdt_notifier))
+		dev_warn(&pdev->dev, "cannot register reboot notifier\n");
+
 	dev_info(&pdev->dev, "initialized watchdog with heartbeat %ds\n",
 			stmp3xxx_wdd.timeout);
 	return 0;
@@ -91,6 +115,7 @@ static int stmp3xxx_wdt_probe(struct platform_device *pdev)
 
 static int stmp3xxx_wdt_remove(struct platform_device *pdev)
 {
+	unregister_reboot_notifier(&wdt_notifier);
 	watchdog_unregister_device(&stmp3xxx_wdd);
 	return 0;
 }
diff --git a/drivers/watchdog/sun4v_wdt.c b/drivers/watchdog/sun4v_wdt.c
new file mode 100644
index 000000000000..1467fe50a76f
--- /dev/null
+++ b/drivers/watchdog/sun4v_wdt.c
@@ -0,0 +1,191 @@
+/*
+ *	sun4v watchdog timer
+ *	(c) Copyright 2016 Oracle Corporation
+ *
+ *	Implement a simple watchdog driver using the built-in sun4v hypervisor
+ *	watchdog support. If time expires, the hypervisor stops or bounces
+ *	the guest domain.
+ *
+ *	This program is free software; you can redistribute it and/or
+ *	modify it under the terms of the 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) KBUILD_MODNAME ": " fmt
+
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/watchdog.h>
+#include <asm/hypervisor.h>
+#include <asm/mdesc.h>
+
+#define WDT_TIMEOUT			60
+#define WDT_MAX_TIMEOUT			31536000
+#define WDT_MIN_TIMEOUT			1
+#define WDT_DEFAULT_RESOLUTION_MS	1000	/* 1 second */
+
+static unsigned int timeout;
+module_param(timeout, uint, 0);
+MODULE_PARM_DESC(timeout, "Watchdog timeout in seconds (default="
+	__MODULE_STRING(WDT_TIMEOUT) ")");
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, S_IRUGO);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+	__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static int sun4v_wdt_stop(struct watchdog_device *wdd)
+{
+	sun4v_mach_set_watchdog(0, NULL);
+
+	return 0;
+}
+
+static int sun4v_wdt_ping(struct watchdog_device *wdd)
+{
+	int hverr;
+
+	/*
+	 * HV watchdog timer will round up the timeout
+	 * passed in to the nearest multiple of the
+	 * watchdog resolution in milliseconds.
+	 */
+	hverr = sun4v_mach_set_watchdog(wdd->timeout * 1000, NULL);
+	if (hverr == HV_EINVAL)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int sun4v_wdt_set_timeout(struct watchdog_device *wdd,
+				 unsigned int timeout)
+{
+	wdd->timeout = timeout;
+
+	return 0;
+}
+
+static const struct watchdog_info sun4v_wdt_ident = {
+	.options =	WDIOF_SETTIMEOUT |
+			WDIOF_MAGICCLOSE |
+			WDIOF_KEEPALIVEPING,
+	.identity =	"sun4v hypervisor watchdog",
+	.firmware_version = 0,
+};
+
+static struct watchdog_ops sun4v_wdt_ops = {
+	.owner =	THIS_MODULE,
+	.start =	sun4v_wdt_ping,
+	.stop =		sun4v_wdt_stop,
+	.ping =		sun4v_wdt_ping,
+	.set_timeout =	sun4v_wdt_set_timeout,
+};
+
+static struct watchdog_device wdd = {
+	.info = &sun4v_wdt_ident,
+	.ops = &sun4v_wdt_ops,
+	.min_timeout = WDT_MIN_TIMEOUT,
+	.max_timeout = WDT_MAX_TIMEOUT,
+	.timeout = WDT_TIMEOUT,
+};
+
+static int __init sun4v_wdt_init(void)
+{
+	struct mdesc_handle *handle;
+	u64 node;
+	const u64 *value;
+	int err = 0;
+	unsigned long major = 1, minor = 1;
+
+	/*
+	 * There are 2 properties that can be set from the control
+	 * domain for the watchdog.
+	 * watchdog-resolution
+	 * watchdog-max-timeout
+	 *
+	 * We can expect a handle to be returned otherwise something
+	 * serious is wrong. Correct to return -ENODEV here.
+	 */
+
+	handle = mdesc_grab();
+	if (!handle)
+		return -ENODEV;
+
+	node = mdesc_node_by_name(handle, MDESC_NODE_NULL, "platform");
+	err = -ENODEV;
+	if (node == MDESC_NODE_NULL)
+		goto out_release;
+
+	/*
+	 * This is a safe way to validate if we are on the right
+	 * platform.
+	 */
+	if (sun4v_hvapi_register(HV_GRP_CORE, major, &minor))
+		goto out_hv_unreg;
+
+	/* Allow value of watchdog-resolution up to 1s (default) */
+	value = mdesc_get_property(handle, node, "watchdog-resolution", NULL);
+	err = -EINVAL;
+	if (value) {
+		if (*value == 0 ||
+		    *value > WDT_DEFAULT_RESOLUTION_MS)
+			goto out_hv_unreg;
+	}
+
+	value = mdesc_get_property(handle, node, "watchdog-max-timeout", NULL);
+	if (value) {
+		/*
+		 * If the property value (in ms) is smaller than
+		 * min_timeout, return -EINVAL.
+		 */
+		if (*value < wdd.min_timeout * 1000)
+			goto out_hv_unreg;
+
+		/*
+		 * If the property value is smaller than
+		 * default max_timeout  then set watchdog max_timeout to
+		 * the value of the property in seconds.
+		 */
+		if (*value < wdd.max_timeout * 1000)
+			wdd.max_timeout = *value  / 1000;
+	}
+
+	watchdog_init_timeout(&wdd, timeout, NULL);
+
+	watchdog_set_nowayout(&wdd, nowayout);
+
+	err = watchdog_register_device(&wdd);
+	if (err)
+		goto out_hv_unreg;
+
+	pr_info("initialized (timeout=%ds, nowayout=%d)\n",
+		 wdd.timeout, nowayout);
+
+	mdesc_release(handle);
+
+	return 0;
+
+out_hv_unreg:
+	sun4v_hvapi_unregister(HV_GRP_CORE);
+
+out_release:
+	mdesc_release(handle);
+	return err;
+}
+
+static void __exit sun4v_wdt_exit(void)
+{
+	sun4v_hvapi_unregister(HV_GRP_CORE);
+	watchdog_unregister_device(&wdd);
+}
+
+module_init(sun4v_wdt_init);
+module_exit(sun4v_wdt_exit);
+
+MODULE_AUTHOR("Wim Coekaerts <wim.coekaerts@oracle.com>");
+MODULE_DESCRIPTION("sun4v watchdog driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/sunxi_wdt.c b/drivers/watchdog/sunxi_wdt.c
index 47bd8a14d01f..e027deb54740 100644
--- a/drivers/watchdog/sunxi_wdt.c
+++ b/drivers/watchdog/sunxi_wdt.c
@@ -21,11 +21,9 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
-#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
-#include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/watchdog.h>
 
@@ -60,7 +58,6 @@ struct sunxi_wdt_dev {
 	struct watchdog_device wdt_dev;
 	void __iomem *wdt_base;
 	const struct sunxi_wdt_reg *wdt_regs;
-	struct notifier_block restart_handler;
 };
 
 /*
@@ -86,12 +83,9 @@ static const int wdt_timeout_map[] = {
 };
 
 
-static int sunxi_restart_handle(struct notifier_block *this, unsigned long mode,
-				void *cmd)
+static int sunxi_wdt_restart(struct watchdog_device *wdt_dev)
 {
-	struct sunxi_wdt_dev *sunxi_wdt = container_of(this,
-						       struct sunxi_wdt_dev,
-						       restart_handler);
+	struct sunxi_wdt_dev *sunxi_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = sunxi_wdt->wdt_base;
 	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
 	u32 val;
@@ -120,7 +114,7 @@ static int sunxi_restart_handle(struct notifier_block *this, unsigned long mode,
 		val |= WDT_MODE_EN;
 		writel(val, wdt_base + regs->wdt_mode);
 	}
-	return NOTIFY_DONE;
+	return 0;
 }
 
 static int sunxi_wdt_ping(struct watchdog_device *wdt_dev)
@@ -208,6 +202,7 @@ static const struct watchdog_ops sunxi_wdt_ops = {
 	.stop		= sunxi_wdt_stop,
 	.ping		= sunxi_wdt_ping,
 	.set_timeout	= sunxi_wdt_set_timeout,
+	.restart	= sunxi_wdt_restart,
 };
 
 static const struct sunxi_wdt_reg sun4i_wdt_reg = {
@@ -268,6 +263,7 @@ static int sunxi_wdt_probe(struct platform_device *pdev)
 
 	watchdog_init_timeout(&sunxi_wdt->wdt_dev, timeout, &pdev->dev);
 	watchdog_set_nowayout(&sunxi_wdt->wdt_dev, nowayout);
+	watchdog_set_restart_priority(&sunxi_wdt->wdt_dev, 128);
 
 	watchdog_set_drvdata(&sunxi_wdt->wdt_dev, sunxi_wdt);
 
@@ -277,13 +273,6 @@ static int sunxi_wdt_probe(struct platform_device *pdev)
 	if (unlikely(err))
 		return err;
 
-	sunxi_wdt->restart_handler.notifier_call = sunxi_restart_handle;
-	sunxi_wdt->restart_handler.priority = 128;
-	err = register_restart_handler(&sunxi_wdt->restart_handler);
-	if (err)
-		dev_err(&pdev->dev,
-			"cannot register restart handler (err=%d)\n", err);
-
 	dev_info(&pdev->dev, "Watchdog enabled (timeout=%d sec, nowayout=%d)",
 			sunxi_wdt->wdt_dev.timeout, nowayout);
 
@@ -294,8 +283,6 @@ static int sunxi_wdt_remove(struct platform_device *pdev)
 {
 	struct sunxi_wdt_dev *sunxi_wdt = platform_get_drvdata(pdev);
 
-	unregister_restart_handler(&sunxi_wdt->restart_handler);
-
 	watchdog_unregister_device(&sunxi_wdt->wdt_dev);
 	watchdog_set_drvdata(&sunxi_wdt->wdt_dev, NULL);
 
diff --git a/drivers/watchdog/tangox_wdt.c b/drivers/watchdog/tangox_wdt.c
new file mode 100644
index 000000000000..709c1ed6fd79
--- /dev/null
+++ b/drivers/watchdog/tangox_wdt.c
@@ -0,0 +1,225 @@
+/*
+ *  Copyright (C) 2015 Mans Rullgard <mans@mansr.com>
+ *  SMP86xx/SMP87xx Watchdog driver
+ *
+ *  This program is free software; you can redistribute it and/or modify it
+ *  under  the terms of the 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/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/notifier.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/watchdog.h>
+
+#define DEFAULT_TIMEOUT 30
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout,
+		 "Watchdog cannot be stopped once started (default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static unsigned int timeout;
+module_param(timeout, int, 0);
+MODULE_PARM_DESC(timeout, "Watchdog timeout");
+
+/*
+ * Counter counts down from programmed value.  Reset asserts when
+ * the counter reaches 1.
+ */
+#define WD_COUNTER		0
+
+#define WD_CONFIG		4
+#define WD_CONFIG_XTAL_IN	BIT(0)
+#define WD_CONFIG_DISABLE	BIT(31)
+
+struct tangox_wdt_device {
+	struct watchdog_device wdt;
+	void __iomem *base;
+	unsigned long clk_rate;
+	struct clk *clk;
+	struct notifier_block restart;
+};
+
+static int tangox_wdt_set_timeout(struct watchdog_device *wdt,
+				  unsigned int new_timeout)
+{
+	wdt->timeout = new_timeout;
+
+	return 0;
+}
+
+static int tangox_wdt_start(struct watchdog_device *wdt)
+{
+	struct tangox_wdt_device *dev = watchdog_get_drvdata(wdt);
+	u32 ticks;
+
+	ticks = 1 + wdt->timeout * dev->clk_rate;
+	writel(ticks, dev->base + WD_COUNTER);
+
+	return 0;
+}
+
+static int tangox_wdt_stop(struct watchdog_device *wdt)
+{
+	struct tangox_wdt_device *dev = watchdog_get_drvdata(wdt);
+
+	writel(0, dev->base + WD_COUNTER);
+
+	return 0;
+}
+
+static unsigned int tangox_wdt_get_timeleft(struct watchdog_device *wdt)
+{
+	struct tangox_wdt_device *dev = watchdog_get_drvdata(wdt);
+	u32 count;
+
+	count = readl(dev->base + WD_COUNTER);
+
+	if (!count)
+		return 0;
+
+	return (count - 1) / dev->clk_rate;
+}
+
+static const struct watchdog_info tangox_wdt_info = {
+	.options  = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING | WDIOF_MAGICCLOSE,
+	.identity = "tangox watchdog",
+};
+
+static const struct watchdog_ops tangox_wdt_ops = {
+	.start		= tangox_wdt_start,
+	.stop		= tangox_wdt_stop,
+	.set_timeout	= tangox_wdt_set_timeout,
+	.get_timeleft	= tangox_wdt_get_timeleft,
+};
+
+static int tangox_wdt_restart(struct notifier_block *nb, unsigned long action,
+			      void *data)
+{
+	struct tangox_wdt_device *dev =
+		container_of(nb, struct tangox_wdt_device, restart);
+
+	writel(1, dev->base + WD_COUNTER);
+
+	return NOTIFY_DONE;
+}
+
+static int tangox_wdt_probe(struct platform_device *pdev)
+{
+	struct tangox_wdt_device *dev;
+	struct resource *res;
+	u32 config;
+	int err;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->base))
+		return PTR_ERR(dev->base);
+
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(dev->clk))
+		return PTR_ERR(dev->clk);
+
+	err = clk_prepare_enable(dev->clk);
+	if (err)
+		return err;
+
+	dev->clk_rate = clk_get_rate(dev->clk);
+
+	dev->wdt.parent = &pdev->dev;
+	dev->wdt.info = &tangox_wdt_info;
+	dev->wdt.ops = &tangox_wdt_ops;
+	dev->wdt.timeout = DEFAULT_TIMEOUT;
+	dev->wdt.min_timeout = 1;
+	dev->wdt.max_timeout = (U32_MAX - 1) / dev->clk_rate;
+
+	watchdog_init_timeout(&dev->wdt, timeout, &pdev->dev);
+	watchdog_set_nowayout(&dev->wdt, nowayout);
+	watchdog_set_drvdata(&dev->wdt, dev);
+
+	/*
+	 * Deactivate counter if disable bit is set to avoid
+	 * accidental reset.
+	 */
+	config = readl(dev->base + WD_CONFIG);
+	if (config & WD_CONFIG_DISABLE)
+		writel(0, dev->base + WD_COUNTER);
+
+	writel(WD_CONFIG_XTAL_IN, dev->base + WD_CONFIG);
+
+	/*
+	 * Mark as active and restart with configured timeout if
+	 * already running.
+	 */
+	if (readl(dev->base + WD_COUNTER)) {
+		set_bit(WDOG_ACTIVE, &dev->wdt.status);
+		tangox_wdt_start(&dev->wdt);
+	}
+
+	err = watchdog_register_device(&dev->wdt);
+	if (err) {
+		clk_disable_unprepare(dev->clk);
+		return err;
+	}
+
+	platform_set_drvdata(pdev, dev);
+
+	dev->restart.notifier_call = tangox_wdt_restart;
+	dev->restart.priority = 128;
+	err = register_restart_handler(&dev->restart);
+	if (err)
+		dev_warn(&pdev->dev, "failed to register restart handler\n");
+
+	dev_info(&pdev->dev, "SMP86xx/SMP87xx watchdog registered\n");
+
+	return 0;
+}
+
+static int tangox_wdt_remove(struct platform_device *pdev)
+{
+	struct tangox_wdt_device *dev = platform_get_drvdata(pdev);
+
+	tangox_wdt_stop(&dev->wdt);
+	clk_disable_unprepare(dev->clk);
+
+	unregister_restart_handler(&dev->restart);
+	watchdog_unregister_device(&dev->wdt);
+
+	return 0;
+}
+
+static const struct of_device_id tangox_wdt_dt_ids[] = {
+	{ .compatible = "sigma,smp8642-wdt" },
+	{ .compatible = "sigma,smp8759-wdt" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, tangox_wdt_dt_ids);
+
+static struct platform_driver tangox_wdt_driver = {
+	.probe	= tangox_wdt_probe,
+	.remove	= tangox_wdt_remove,
+	.driver	= {
+		.name		= "tangox-wdt",
+		.of_match_table	= tangox_wdt_dt_ids,
+	},
+};
+
+module_platform_driver(tangox_wdt_driver);
+
+MODULE_AUTHOR("Mans Rullgard <mans@mansr.com>");
+MODULE_DESCRIPTION("SMP86xx/SMP87xx Watchdog driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/ts4800_wdt.c b/drivers/watchdog/ts4800_wdt.c
new file mode 100644
index 000000000000..2b8de8602b67
--- /dev/null
+++ b/drivers/watchdog/ts4800_wdt.c
@@ -0,0 +1,215 @@
+/*
+ * Watchdog driver for TS-4800 based boards
+ *
+ * Copyright (c) 2015 - Savoir-faire Linux
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/watchdog.h>
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout,
+	"Watchdog cannot be stopped once started (default="
+	__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+/* possible feed values */
+#define TS4800_WDT_FEED_2S       0x1
+#define TS4800_WDT_FEED_10S      0x2
+#define TS4800_WDT_DISABLE       0x3
+
+struct ts4800_wdt {
+	struct watchdog_device  wdd;
+	struct regmap           *regmap;
+	u32                     feed_offset;
+	u32                     feed_val;
+};
+
+/*
+ * TS-4800 supports the following timeout values:
+ *
+ *   value desc
+ *   ---------------------
+ *     0    feed for 338ms
+ *     1    feed for 2.706s
+ *     2    feed for 10.824s
+ *     3    disable watchdog
+ *
+ * Keep the regmap/timeout map ordered by timeout
+ */
+static const struct {
+	const int timeout;
+	const int regval;
+} ts4800_wdt_map[] = {
+	{ 2,  TS4800_WDT_FEED_2S },
+	{ 10, TS4800_WDT_FEED_10S },
+};
+
+#define MAX_TIMEOUT_INDEX       (ARRAY_SIZE(ts4800_wdt_map) - 1)
+
+static void ts4800_write_feed(struct ts4800_wdt *wdt, u32 val)
+{
+	regmap_write(wdt->regmap, wdt->feed_offset, val);
+}
+
+static int ts4800_wdt_start(struct watchdog_device *wdd)
+{
+	struct ts4800_wdt *wdt = watchdog_get_drvdata(wdd);
+
+	ts4800_write_feed(wdt, wdt->feed_val);
+	return 0;
+}
+
+static int ts4800_wdt_stop(struct watchdog_device *wdd)
+{
+	struct ts4800_wdt *wdt = watchdog_get_drvdata(wdd);
+
+	ts4800_write_feed(wdt, TS4800_WDT_DISABLE);
+	return 0;
+}
+
+static int ts4800_wdt_set_timeout(struct watchdog_device *wdd,
+				  unsigned int timeout)
+{
+	struct ts4800_wdt *wdt = watchdog_get_drvdata(wdd);
+	int i;
+
+	for (i = 0; i < MAX_TIMEOUT_INDEX; i++) {
+		if (ts4800_wdt_map[i].timeout >= timeout)
+			break;
+	}
+
+	wdd->timeout = ts4800_wdt_map[i].timeout;
+	wdt->feed_val = ts4800_wdt_map[i].regval;
+
+	return 0;
+}
+
+static const struct watchdog_ops ts4800_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = ts4800_wdt_start,
+	.stop = ts4800_wdt_stop,
+	.set_timeout = ts4800_wdt_set_timeout,
+};
+
+static const struct watchdog_info ts4800_wdt_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE | WDIOF_KEEPALIVEPING,
+	.identity = "TS-4800 Watchdog",
+};
+
+static int ts4800_wdt_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct device_node *syscon_np;
+	struct watchdog_device *wdd;
+	struct ts4800_wdt *wdt;
+	u32 reg;
+	int ret;
+
+	syscon_np = of_parse_phandle(np, "syscon", 0);
+	if (!syscon_np) {
+		dev_err(&pdev->dev, "no syscon property\n");
+		return -ENODEV;
+	}
+
+	ret = of_property_read_u32_index(np, "syscon", 1, &reg);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "no offset in syscon\n");
+		return ret;
+	}
+
+	/* allocate memory for watchdog struct */
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	/* set regmap and offset to know where to write */
+	wdt->feed_offset = reg;
+	wdt->regmap = syscon_node_to_regmap(syscon_np);
+	if (IS_ERR(wdt->regmap)) {
+		dev_err(&pdev->dev, "cannot get parent's regmap\n");
+		return PTR_ERR(wdt->regmap);
+	}
+
+	/* Initialize struct watchdog_device */
+	wdd = &wdt->wdd;
+	wdd->parent = &pdev->dev;
+	wdd->info = &ts4800_wdt_info;
+	wdd->ops = &ts4800_wdt_ops;
+	wdd->min_timeout = ts4800_wdt_map[0].timeout;
+	wdd->max_timeout = ts4800_wdt_map[MAX_TIMEOUT_INDEX].timeout;
+
+	watchdog_set_drvdata(wdd, wdt);
+	watchdog_set_nowayout(wdd, nowayout);
+	watchdog_init_timeout(wdd, 0, &pdev->dev);
+
+	/*
+	 * As this watchdog supports only a few values, ts4800_wdt_set_timeout
+	 * must be called to initialize timeout and feed_val with valid values.
+	 * Default to maximum timeout if none, or an invalid one, is provided in
+	 * device tree.
+	 */
+	if (!wdd->timeout)
+		wdd->timeout = wdd->max_timeout;
+	ts4800_wdt_set_timeout(wdd, wdd->timeout);
+
+	/*
+	 * The feed register is write-only, so it is not possible to determine
+	 * watchdog's state. Disable it to be in a known state.
+	 */
+	ts4800_wdt_stop(wdd);
+
+	ret = watchdog_register_device(wdd);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"failed to register watchdog device\n");
+		return ret;
+	}
+
+	platform_set_drvdata(pdev, wdt);
+
+	dev_info(&pdev->dev,
+		 "initialized (timeout = %d sec, nowayout = %d)\n",
+		 wdd->timeout, nowayout);
+
+	return 0;
+}
+
+static int ts4800_wdt_remove(struct platform_device *pdev)
+{
+	struct ts4800_wdt *wdt = platform_get_drvdata(pdev);
+
+	watchdog_unregister_device(&wdt->wdd);
+
+	return 0;
+}
+
+static const struct of_device_id ts4800_wdt_of_match[] = {
+	{ .compatible = "technologic,ts4800-wdt", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ts4800_wdt_of_match);
+
+static struct platform_driver ts4800_wdt_driver = {
+	.probe		= ts4800_wdt_probe,
+	.remove		= ts4800_wdt_remove,
+	.driver		= {
+		.name	= "ts4800_wdt",
+		.of_match_table = ts4800_wdt_of_match,
+	},
+};
+
+module_platform_driver(ts4800_wdt_driver);
+
+MODULE_AUTHOR("Damien Riegel <damien.riegel@savoirfairelinux.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:ts4800_wdt");
diff --git a/drivers/watchdog/w83627hf_wdt.c b/drivers/watchdog/w83627hf_wdt.c
index 5824e25eebbb..cab14bc9106c 100644
--- a/drivers/watchdog/w83627hf_wdt.c
+++ b/drivers/watchdog/w83627hf_wdt.c
@@ -36,8 +36,6 @@
 #include <linux/types.h>
 #include <linux/watchdog.h>
 #include <linux/ioport.h>
-#include <linux/notifier.h>
-#include <linux/reboot.h>
 #include <linux/init.h>
 #include <linux/io.h>
 
@@ -288,18 +286,6 @@ static unsigned int wdt_get_time(struct watchdog_device *wdog)
 }
 
 /*
- *	Notifier for system down
- */
-static int wdt_notify_sys(struct notifier_block *this, unsigned long code,
-	void *unused)
-{
-	if (code == SYS_DOWN || code == SYS_HALT)
-		wdt_set_time(0);	/* Turn the WDT off */
-
-	return NOTIFY_DONE;
-}
-
-/*
  *	Kernel Interfaces
  */
 
@@ -329,10 +315,6 @@ static struct watchdog_device wdt_dev = {
  *	turn the timebomb registers off.
  */
 
-static struct notifier_block wdt_notifier = {
-	.notifier_call = wdt_notify_sys,
-};
-
 static int wdt_find(int addr)
 {
 	u8 val;
@@ -456,6 +438,7 @@ static int __init wdt_init(void)
 
 	watchdog_init_timeout(&wdt_dev, timeout, NULL);
 	watchdog_set_nowayout(&wdt_dev, nowayout);
+	watchdog_stop_on_reboot(&wdt_dev);
 
 	ret = w83627hf_init(&wdt_dev, chip);
 	if (ret) {
@@ -463,30 +446,19 @@ static int __init wdt_init(void)
 		return ret;
 	}
 
-	ret = register_reboot_notifier(&wdt_notifier);
-	if (ret != 0) {
-		pr_err("cannot register reboot notifier (err=%d)\n", ret);
-		return ret;
-	}
-
 	ret = watchdog_register_device(&wdt_dev);
 	if (ret)
-		goto unreg_reboot;
+		return ret;
 
 	pr_info("initialized. timeout=%d sec (nowayout=%d)\n",
 		wdt_dev.timeout, nowayout);
 
 	return ret;
-
-unreg_reboot:
-	unregister_reboot_notifier(&wdt_notifier);
-	return ret;
 }
 
 static void __exit wdt_exit(void)
 {
 	watchdog_unregister_device(&wdt_dev);
-	unregister_reboot_notifier(&wdt_notifier);
 }
 
 module_init(wdt_init);
diff --git a/drivers/watchdog/watchdog_core.c b/drivers/watchdog/watchdog_core.c
index 873f13972cf4..e600fd93b7de 100644
--- a/drivers/watchdog/watchdog_core.c
+++ b/drivers/watchdog/watchdog_core.c
@@ -32,6 +32,7 @@
 #include <linux/types.h>	/* For standard types */
 #include <linux/errno.h>	/* For the -ENODEV/... values */
 #include <linux/kernel.h>	/* For printk/panic/... */
+#include <linux/reboot.h>	/* For restart handler */
 #include <linux/watchdog.h>	/* For watchdog specific items */
 #include <linux/init.h>		/* For __init/__exit/... */
 #include <linux/idr.h>		/* For ida_* macros */
@@ -41,7 +42,6 @@
 #include "watchdog_core.h"	/* For watchdog_dev_register/... */
 
 static DEFINE_IDA(watchdog_ida);
-static struct class *watchdog_class;
 
 /*
  * Deferred Registration infrastructure.
@@ -137,9 +137,63 @@ int watchdog_init_timeout(struct watchdog_device *wdd,
 }
 EXPORT_SYMBOL_GPL(watchdog_init_timeout);
 
+static int watchdog_reboot_notifier(struct notifier_block *nb,
+				    unsigned long code, void *data)
+{
+	struct watchdog_device *wdd = container_of(nb, struct watchdog_device,
+						   reboot_nb);
+
+	if (code == SYS_DOWN || code == SYS_HALT) {
+		if (watchdog_active(wdd)) {
+			int ret;
+
+			ret = wdd->ops->stop(wdd);
+			if (ret)
+				return NOTIFY_BAD;
+		}
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int watchdog_restart_notifier(struct notifier_block *nb,
+				     unsigned long action, void *data)
+{
+	struct watchdog_device *wdd = container_of(nb, struct watchdog_device,
+						   restart_nb);
+
+	int ret;
+
+	ret = wdd->ops->restart(wdd);
+	if (ret)
+		return NOTIFY_BAD;
+
+	return NOTIFY_DONE;
+}
+
+/**
+ * watchdog_set_restart_priority - Change priority of restart handler
+ * @wdd: watchdog device
+ * @priority: priority of the restart handler, should follow these guidelines:
+ *   0:   use watchdog's restart function as last resort, has limited restart
+ *        capabilies
+ *   128: default restart handler, use if no other handler is expected to be
+ *        available and/or if restart is sufficient to restart the entire system
+ *   255: preempt all other handlers
+ *
+ * If a wdd->ops->restart function is provided when watchdog_register_device is
+ * called, it will be registered as a restart handler with the priority given
+ * here.
+ */
+void watchdog_set_restart_priority(struct watchdog_device *wdd, int priority)
+{
+	wdd->restart_nb.priority = priority;
+}
+EXPORT_SYMBOL_GPL(watchdog_set_restart_priority);
+
 static int __watchdog_register_device(struct watchdog_device *wdd)
 {
-	int ret, id = -1, devno;
+	int ret, id = -1;
 
 	if (wdd == NULL || wdd->info == NULL || wdd->ops == NULL)
 		return -EINVAL;
@@ -156,8 +210,6 @@ static int __watchdog_register_device(struct watchdog_device *wdd)
 	 * corrupted in a later stage then we expect a kernel panic!
 	 */
 
-	mutex_init(&wdd->lock);
-
 	/* Use alias for watchdog id if possible */
 	if (wdd->parent) {
 		ret = of_alias_get_id(wdd->parent->of_node, "watchdog");
@@ -192,14 +244,26 @@ static int __watchdog_register_device(struct watchdog_device *wdd)
 		}
 	}
 
-	devno = wdd->cdev.dev;
-	wdd->dev = device_create(watchdog_class, wdd->parent, devno,
-					NULL, "watchdog%d", wdd->id);
-	if (IS_ERR(wdd->dev)) {
-		watchdog_dev_unregister(wdd);
-		ida_simple_remove(&watchdog_ida, id);
-		ret = PTR_ERR(wdd->dev);
-		return ret;
+	if (test_bit(WDOG_STOP_ON_REBOOT, &wdd->status)) {
+		wdd->reboot_nb.notifier_call = watchdog_reboot_notifier;
+
+		ret = register_reboot_notifier(&wdd->reboot_nb);
+		if (ret) {
+			pr_err("watchdog%d: Cannot register reboot notifier (%d)\n",
+			       wdd->id, ret);
+			watchdog_dev_unregister(wdd);
+			ida_simple_remove(&watchdog_ida, wdd->id);
+			return ret;
+		}
+	}
+
+	if (wdd->ops->restart) {
+		wdd->restart_nb.notifier_call = watchdog_restart_notifier;
+
+		ret = register_restart_handler(&wdd->restart_nb);
+		if (ret)
+			pr_warn("watchog%d: Cannot register restart handler (%d)\n",
+				wdd->id, ret);
 	}
 
 	return 0;
@@ -232,19 +296,17 @@ EXPORT_SYMBOL_GPL(watchdog_register_device);
 
 static void __watchdog_unregister_device(struct watchdog_device *wdd)
 {
-	int ret;
-	int devno;
-
 	if (wdd == NULL)
 		return;
 
-	devno = wdd->cdev.dev;
-	ret = watchdog_dev_unregister(wdd);
-	if (ret)
-		pr_err("error unregistering /dev/watchdog (err=%d)\n", ret);
-	device_destroy(watchdog_class, devno);
+	if (wdd->ops->restart)
+		unregister_restart_handler(&wdd->restart_nb);
+
+	if (test_bit(WDOG_STOP_ON_REBOOT, &wdd->status))
+		unregister_reboot_notifier(&wdd->reboot_nb);
+
+	watchdog_dev_unregister(wdd);
 	ida_simple_remove(&watchdog_ida, wdd->id);
-	wdd->dev = NULL;
 }
 
 /**
@@ -287,17 +349,9 @@ static int __init watchdog_init(void)
 {
 	int err;
 
-	watchdog_class = class_create(THIS_MODULE, "watchdog");
-	if (IS_ERR(watchdog_class)) {
-		pr_err("couldn't create class\n");
-		return PTR_ERR(watchdog_class);
-	}
-
 	err = watchdog_dev_init();
-	if (err < 0) {
-		class_destroy(watchdog_class);
+	if (err < 0)
 		return err;
-	}
 
 	watchdog_deferred_registration();
 	return 0;
@@ -306,7 +360,6 @@ static int __init watchdog_init(void)
 static void __exit watchdog_exit(void)
 {
 	watchdog_dev_exit();
-	class_destroy(watchdog_class);
 	ida_destroy(&watchdog_ida);
 }
 
diff --git a/drivers/watchdog/watchdog_core.h b/drivers/watchdog/watchdog_core.h
index 6c951418fca7..86ff962d1e15 100644
--- a/drivers/watchdog/watchdog_core.h
+++ b/drivers/watchdog/watchdog_core.h
@@ -32,6 +32,6 @@
  *	Functions/procedures to be called by the core
  */
 extern int watchdog_dev_register(struct watchdog_device *);
-extern int watchdog_dev_unregister(struct watchdog_device *);
+extern void watchdog_dev_unregister(struct watchdog_device *);
 extern int __init watchdog_dev_init(void);
 extern void __exit watchdog_dev_exit(void);
diff --git a/drivers/watchdog/watchdog_dev.c b/drivers/watchdog/watchdog_dev.c
index 56a649e66eb2..ba2ecce4aae6 100644
--- a/drivers/watchdog/watchdog_dev.c
+++ b/drivers/watchdog/watchdog_dev.c
@@ -32,27 +32,51 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/module.h>	/* For module stuff/... */
-#include <linux/types.h>	/* For standard types (like size_t) */
+#include <linux/cdev.h>		/* For character device */
 #include <linux/errno.h>	/* For the -ENODEV/... values */
-#include <linux/kernel.h>	/* For printk/panic/... */
 #include <linux/fs.h>		/* For file operations */
-#include <linux/watchdog.h>	/* For watchdog specific items */
-#include <linux/miscdevice.h>	/* For handling misc devices */
 #include <linux/init.h>		/* For __init/__exit/... */
+#include <linux/kernel.h>	/* For printk/panic/... */
+#include <linux/kref.h>		/* For data references */
+#include <linux/miscdevice.h>	/* For handling misc devices */
+#include <linux/module.h>	/* For module stuff/... */
+#include <linux/mutex.h>	/* For mutexes */
+#include <linux/slab.h>		/* For memory functions */
+#include <linux/types.h>	/* For standard types (like size_t) */
+#include <linux/watchdog.h>	/* For watchdog specific items */
 #include <linux/uaccess.h>	/* For copy_to_user/put_user/... */
 
 #include "watchdog_core.h"
 
+/*
+ * struct watchdog_core_data - watchdog core internal data
+ * @kref:	Reference count.
+ * @cdev:	The watchdog's Character device.
+ * @wdd:	Pointer to watchdog device.
+ * @lock:	Lock for watchdog core.
+ * @status:	Watchdog core internal status bits.
+ */
+struct watchdog_core_data {
+	struct kref kref;
+	struct cdev cdev;
+	struct watchdog_device *wdd;
+	struct mutex lock;
+	unsigned long status;		/* Internal status bits */
+#define _WDOG_DEV_OPEN		0	/* Opened ? */
+#define _WDOG_ALLOW_RELEASE	1	/* Did we receive the magic char ? */
+};
+
 /* the dev_t structure to store the dynamically allocated watchdog devices */
 static dev_t watchdog_devt;
-/* the watchdog device behind /dev/watchdog */
-static struct watchdog_device *old_wdd;
+/* Reference to watchdog device behind /dev/watchdog */
+static struct watchdog_core_data *old_wd_data;
 
 /*
  *	watchdog_ping: ping the watchdog.
  *	@wdd: the watchdog device to ping
  *
+ *	The caller must hold wd_data->lock.
+ *
  *	If the watchdog has no own ping operation then it needs to be
  *	restarted via the start operation. This wrapper function does
  *	exactly that.
@@ -61,25 +85,16 @@ static struct watchdog_device *old_wdd;
 
 static int watchdog_ping(struct watchdog_device *wdd)
 {
-	int err = 0;
-
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_ping;
-	}
+	int err;
 
 	if (!watchdog_active(wdd))
-		goto out_ping;
+		return 0;
 
 	if (wdd->ops->ping)
 		err = wdd->ops->ping(wdd);	/* ping the watchdog */
 	else
 		err = wdd->ops->start(wdd);	/* restart watchdog */
 
-out_ping:
-	mutex_unlock(&wdd->lock);
 	return err;
 }
 
@@ -87,6 +102,8 @@ out_ping:
  *	watchdog_start: wrapper to start the watchdog.
  *	@wdd: the watchdog device to start
  *
+ *	The caller must hold wd_data->lock.
+ *
  *	Start the watchdog if it is not active and mark it active.
  *	This function returns zero on success or a negative errno code for
  *	failure.
@@ -94,24 +111,15 @@ out_ping:
 
 static int watchdog_start(struct watchdog_device *wdd)
 {
-	int err = 0;
-
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_start;
-	}
+	int err;
 
 	if (watchdog_active(wdd))
-		goto out_start;
+		return 0;
 
 	err = wdd->ops->start(wdd);
 	if (err == 0)
 		set_bit(WDOG_ACTIVE, &wdd->status);
 
-out_start:
-	mutex_unlock(&wdd->lock);
 	return err;
 }
 
@@ -119,6 +127,8 @@ out_start:
  *	watchdog_stop: wrapper to stop the watchdog.
  *	@wdd: the watchdog device to stop
  *
+ *	The caller must hold wd_data->lock.
+ *
  *	Stop the watchdog if it is still active and unmark it active.
  *	This function returns zero on success or a negative errno code for
  *	failure.
@@ -127,93 +137,59 @@ out_start:
 
 static int watchdog_stop(struct watchdog_device *wdd)
 {
-	int err = 0;
-
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_stop;
-	}
+	int err;
 
 	if (!watchdog_active(wdd))
-		goto out_stop;
+		return 0;
 
 	if (test_bit(WDOG_NO_WAY_OUT, &wdd->status)) {
-		dev_info(wdd->dev, "nowayout prevents watchdog being stopped!\n");
-		err = -EBUSY;
-		goto out_stop;
+		pr_info("watchdog%d: nowayout prevents watchdog being stopped!\n",
+			wdd->id);
+		return -EBUSY;
 	}
 
 	err = wdd->ops->stop(wdd);
 	if (err == 0)
 		clear_bit(WDOG_ACTIVE, &wdd->status);
 
-out_stop:
-	mutex_unlock(&wdd->lock);
 	return err;
 }
 
 /*
  *	watchdog_get_status: wrapper to get the watchdog status
  *	@wdd: the watchdog device to get the status from
- *	@status: the status of the watchdog device
+ *
+ *	The caller must hold wd_data->lock.
  *
  *	Get the watchdog's status flags.
  */
 
-static int watchdog_get_status(struct watchdog_device *wdd,
-							unsigned int *status)
+static unsigned int watchdog_get_status(struct watchdog_device *wdd)
 {
-	int err = 0;
-
-	*status = 0;
 	if (!wdd->ops->status)
-		return -EOPNOTSUPP;
-
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_status;
-	}
-
-	*status = wdd->ops->status(wdd);
+		return 0;
 
-out_status:
-	mutex_unlock(&wdd->lock);
-	return err;
+	return wdd->ops->status(wdd);
 }
 
 /*
  *	watchdog_set_timeout: set the watchdog timer timeout
  *	@wdd: the watchdog device to set the timeout for
  *	@timeout: timeout to set in seconds
+ *
+ *	The caller must hold wd_data->lock.
  */
 
 static int watchdog_set_timeout(struct watchdog_device *wdd,
 							unsigned int timeout)
 {
-	int err;
-
 	if (!wdd->ops->set_timeout || !(wdd->info->options & WDIOF_SETTIMEOUT))
 		return -EOPNOTSUPP;
 
 	if (watchdog_timeout_invalid(wdd, timeout))
 		return -EINVAL;
 
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_timeout;
-	}
-
-	err = wdd->ops->set_timeout(wdd, timeout);
-
-out_timeout:
-	mutex_unlock(&wdd->lock);
-	return err;
+	return wdd->ops->set_timeout(wdd, timeout);
 }
 
 /*
@@ -221,59 +197,156 @@ out_timeout:
  *	@wdd: the watchdog device to get the remaining time from
  *	@timeleft: the time that's left
  *
+ *	The caller must hold wd_data->lock.
+ *
  *	Get the time before a watchdog will reboot (if not pinged).
  */
 
 static int watchdog_get_timeleft(struct watchdog_device *wdd,
 							unsigned int *timeleft)
 {
-	int err = 0;
-
 	*timeleft = 0;
+
 	if (!wdd->ops->get_timeleft)
 		return -EOPNOTSUPP;
 
-	mutex_lock(&wdd->lock);
+	*timeleft = wdd->ops->get_timeleft(wdd);
 
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_timeleft;
-	}
+	return 0;
+}
 
-	*timeleft = wdd->ops->get_timeleft(wdd);
+#ifdef CONFIG_WATCHDOG_SYSFS
+static ssize_t nowayout_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
 
-out_timeleft:
-	mutex_unlock(&wdd->lock);
-	return err;
+	return sprintf(buf, "%d\n", !!test_bit(WDOG_NO_WAY_OUT, &wdd->status));
+}
+static DEVICE_ATTR_RO(nowayout);
+
+static ssize_t status_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+	struct watchdog_core_data *wd_data = wdd->wd_data;
+	unsigned int status;
+
+	mutex_lock(&wd_data->lock);
+	status = watchdog_get_status(wdd);
+	mutex_unlock(&wd_data->lock);
+
+	return sprintf(buf, "%u\n", status);
+}
+static DEVICE_ATTR_RO(status);
+
+static ssize_t bootstatus_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%u\n", wdd->bootstatus);
+}
+static DEVICE_ATTR_RO(bootstatus);
+
+static ssize_t timeleft_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+	struct watchdog_core_data *wd_data = wdd->wd_data;
+	ssize_t status;
+	unsigned int val;
+
+	mutex_lock(&wd_data->lock);
+	status = watchdog_get_timeleft(wdd, &val);
+	mutex_unlock(&wd_data->lock);
+	if (!status)
+		status = sprintf(buf, "%u\n", val);
+
+	return status;
 }
+static DEVICE_ATTR_RO(timeleft);
+
+static ssize_t timeout_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%u\n", wdd->timeout);
+}
+static DEVICE_ATTR_RO(timeout);
+
+static ssize_t identity_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%s\n", wdd->info->identity);
+}
+static DEVICE_ATTR_RO(identity);
+
+static ssize_t state_show(struct device *dev, struct device_attribute *attr,
+				char *buf)
+{
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+
+	if (watchdog_active(wdd))
+		return sprintf(buf, "active\n");
+
+	return sprintf(buf, "inactive\n");
+}
+static DEVICE_ATTR_RO(state);
+
+static umode_t wdt_is_visible(struct kobject *kobj, struct attribute *attr,
+				int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct watchdog_device *wdd = dev_get_drvdata(dev);
+	umode_t mode = attr->mode;
+
+	if (attr == &dev_attr_status.attr && !wdd->ops->status)
+		mode = 0;
+	else if (attr == &dev_attr_timeleft.attr && !wdd->ops->get_timeleft)
+		mode = 0;
+
+	return mode;
+}
+static struct attribute *wdt_attrs[] = {
+	&dev_attr_state.attr,
+	&dev_attr_identity.attr,
+	&dev_attr_timeout.attr,
+	&dev_attr_timeleft.attr,
+	&dev_attr_bootstatus.attr,
+	&dev_attr_status.attr,
+	&dev_attr_nowayout.attr,
+	NULL,
+};
+
+static const struct attribute_group wdt_group = {
+	.attrs = wdt_attrs,
+	.is_visible = wdt_is_visible,
+};
+__ATTRIBUTE_GROUPS(wdt);
+#else
+#define wdt_groups	NULL
+#endif
 
 /*
  *	watchdog_ioctl_op: call the watchdog drivers ioctl op if defined
  *	@wdd: the watchdog device to do the ioctl on
  *	@cmd: watchdog command
  *	@arg: argument pointer
+ *
+ *	The caller must hold wd_data->lock.
  */
 
 static int watchdog_ioctl_op(struct watchdog_device *wdd, unsigned int cmd,
 							unsigned long arg)
 {
-	int err;
-
 	if (!wdd->ops->ioctl)
 		return -ENOIOCTLCMD;
 
-	mutex_lock(&wdd->lock);
-
-	if (test_bit(WDOG_UNREGISTERED, &wdd->status)) {
-		err = -ENODEV;
-		goto out_ioctl;
-	}
-
-	err = wdd->ops->ioctl(wdd, cmd, arg);
-
-out_ioctl:
-	mutex_unlock(&wdd->lock);
-	return err;
+	return wdd->ops->ioctl(wdd, cmd, arg);
 }
 
 /*
@@ -291,10 +364,11 @@ out_ioctl:
 static ssize_t watchdog_write(struct file *file, const char __user *data,
 						size_t len, loff_t *ppos)
 {
-	struct watchdog_device *wdd = file->private_data;
+	struct watchdog_core_data *wd_data = file->private_data;
+	struct watchdog_device *wdd;
+	int err;
 	size_t i;
 	char c;
-	int err;
 
 	if (len == 0)
 		return 0;
@@ -303,18 +377,25 @@ static ssize_t watchdog_write(struct file *file, const char __user *data,
 	 * Note: just in case someone wrote the magic character
 	 * five months ago...
 	 */
-	clear_bit(WDOG_ALLOW_RELEASE, &wdd->status);
+	clear_bit(_WDOG_ALLOW_RELEASE, &wd_data->status);
 
 	/* scan to see whether or not we got the magic character */
 	for (i = 0; i != len; i++) {
 		if (get_user(c, data + i))
 			return -EFAULT;
 		if (c == 'V')
-			set_bit(WDOG_ALLOW_RELEASE, &wdd->status);
+			set_bit(_WDOG_ALLOW_RELEASE, &wd_data->status);
 	}
 
 	/* someone wrote to us, so we send the watchdog a keepalive ping */
-	err = watchdog_ping(wdd);
+
+	err = -ENODEV;
+	mutex_lock(&wd_data->lock);
+	wdd = wd_data->wdd;
+	if (wdd)
+		err = watchdog_ping(wdd);
+	mutex_unlock(&wd_data->lock);
+
 	if (err < 0)
 		return err;
 
@@ -334,71 +415,94 @@ static ssize_t watchdog_write(struct file *file, const char __user *data,
 static long watchdog_ioctl(struct file *file, unsigned int cmd,
 							unsigned long arg)
 {
-	struct watchdog_device *wdd = file->private_data;
+	struct watchdog_core_data *wd_data = file->private_data;
 	void __user *argp = (void __user *)arg;
+	struct watchdog_device *wdd;
 	int __user *p = argp;
 	unsigned int val;
 	int err;
 
+	mutex_lock(&wd_data->lock);
+
+	wdd = wd_data->wdd;
+	if (!wdd) {
+		err = -ENODEV;
+		goto out_ioctl;
+	}
+
 	err = watchdog_ioctl_op(wdd, cmd, arg);
 	if (err != -ENOIOCTLCMD)
-		return err;
+		goto out_ioctl;
 
 	switch (cmd) {
 	case WDIOC_GETSUPPORT:
-		return copy_to_user(argp, wdd->info,
+		err = copy_to_user(argp, wdd->info,
 			sizeof(struct watchdog_info)) ? -EFAULT : 0;
+		break;
 	case WDIOC_GETSTATUS:
-		err = watchdog_get_status(wdd, &val);
-		if (err == -ENODEV)
-			return err;
-		return put_user(val, p);
+		val = watchdog_get_status(wdd);
+		err = put_user(val, p);
+		break;
 	case WDIOC_GETBOOTSTATUS:
-		return put_user(wdd->bootstatus, p);
+		err = put_user(wdd->bootstatus, p);
+		break;
 	case WDIOC_SETOPTIONS:
-		if (get_user(val, p))
-			return -EFAULT;
+		if (get_user(val, p)) {
+			err = -EFAULT;
+			break;
+		}
 		if (val & WDIOS_DISABLECARD) {
 			err = watchdog_stop(wdd);
 			if (err < 0)
-				return err;
+				break;
 		}
-		if (val & WDIOS_ENABLECARD) {
+		if (val & WDIOS_ENABLECARD)
 			err = watchdog_start(wdd);
-			if (err < 0)
-				return err;
-		}
-		return 0;
+		break;
 	case WDIOC_KEEPALIVE:
-		if (!(wdd->info->options & WDIOF_KEEPALIVEPING))
-			return -EOPNOTSUPP;
-		return watchdog_ping(wdd);
+		if (!(wdd->info->options & WDIOF_KEEPALIVEPING)) {
+			err = -EOPNOTSUPP;
+			break;
+		}
+		err = watchdog_ping(wdd);
+		break;
 	case WDIOC_SETTIMEOUT:
-		if (get_user(val, p))
-			return -EFAULT;
+		if (get_user(val, p)) {
+			err = -EFAULT;
+			break;
+		}
 		err = watchdog_set_timeout(wdd, val);
 		if (err < 0)
-			return err;
+			break;
 		/* If the watchdog is active then we send a keepalive ping
 		 * to make sure that the watchdog keep's running (and if
 		 * possible that it takes the new timeout) */
 		err = watchdog_ping(wdd);
 		if (err < 0)
-			return err;
+			break;
 		/* Fall */
 	case WDIOC_GETTIMEOUT:
 		/* timeout == 0 means that we don't know the timeout */
-		if (wdd->timeout == 0)
-			return -EOPNOTSUPP;
-		return put_user(wdd->timeout, p);
+		if (wdd->timeout == 0) {
+			err = -EOPNOTSUPP;
+			break;
+		}
+		err = put_user(wdd->timeout, p);
+		break;
 	case WDIOC_GETTIMELEFT:
 		err = watchdog_get_timeleft(wdd, &val);
-		if (err)
-			return err;
-		return put_user(val, p);
+		if (err < 0)
+			break;
+		err = put_user(val, p);
+		break;
 	default:
-		return -ENOTTY;
+		err = -ENOTTY;
+		break;
 	}
+
+out_ioctl:
+	mutex_unlock(&wd_data->lock);
+	return err;
 }
 
 /*
@@ -413,45 +517,59 @@ static long watchdog_ioctl(struct file *file, unsigned int cmd,
 
 static int watchdog_open(struct inode *inode, struct file *file)
 {
-	int err = -EBUSY;
+	struct watchdog_core_data *wd_data;
 	struct watchdog_device *wdd;
+	int err;
 
 	/* Get the corresponding watchdog device */
 	if (imajor(inode) == MISC_MAJOR)
-		wdd = old_wdd;
+		wd_data = old_wd_data;
 	else
-		wdd = container_of(inode->i_cdev, struct watchdog_device, cdev);
+		wd_data = container_of(inode->i_cdev, struct watchdog_core_data,
+				       cdev);
 
 	/* the watchdog is single open! */
-	if (test_and_set_bit(WDOG_DEV_OPEN, &wdd->status))
+	if (test_and_set_bit(_WDOG_DEV_OPEN, &wd_data->status))
 		return -EBUSY;
 
+	wdd = wd_data->wdd;
+
 	/*
 	 * If the /dev/watchdog device is open, we don't want the module
 	 * to be unloaded.
 	 */
-	if (!try_module_get(wdd->ops->owner))
-		goto out;
+	if (!try_module_get(wdd->ops->owner)) {
+		err = -EBUSY;
+		goto out_clear;
+	}
 
 	err = watchdog_start(wdd);
 	if (err < 0)
 		goto out_mod;
 
-	file->private_data = wdd;
+	file->private_data = wd_data;
 
-	if (wdd->ops->ref)
-		wdd->ops->ref(wdd);
+	kref_get(&wd_data->kref);
 
 	/* dev/watchdog is a virtual (and thus non-seekable) filesystem */
 	return nonseekable_open(inode, file);
 
 out_mod:
-	module_put(wdd->ops->owner);
-out:
-	clear_bit(WDOG_DEV_OPEN, &wdd->status);
+	module_put(wd_data->wdd->ops->owner);
+out_clear:
+	clear_bit(_WDOG_DEV_OPEN, &wd_data->status);
 	return err;
 }
 
+static void watchdog_core_data_release(struct kref *kref)
+{
+	struct watchdog_core_data *wd_data;
+
+	wd_data = container_of(kref, struct watchdog_core_data, kref);
+
+	kfree(wd_data);
+}
+
 /*
  *	watchdog_release: release the watchdog device.
  *	@inode: inode of device
@@ -464,9 +582,16 @@ out:
 
 static int watchdog_release(struct inode *inode, struct file *file)
 {
-	struct watchdog_device *wdd = file->private_data;
+	struct watchdog_core_data *wd_data = file->private_data;
+	struct watchdog_device *wdd;
 	int err = -EBUSY;
 
+	mutex_lock(&wd_data->lock);
+
+	wdd = wd_data->wdd;
+	if (!wdd)
+		goto done;
+
 	/*
 	 * We only stop the watchdog if we received the magic character
 	 * or if WDIOF_MAGICCLOSE is not set. If nowayout was set then
@@ -474,29 +599,24 @@ static int watchdog_release(struct inode *inode, struct file *file)
 	 */
 	if (!test_bit(WDOG_ACTIVE, &wdd->status))
 		err = 0;
-	else if (test_and_clear_bit(WDOG_ALLOW_RELEASE, &wdd->status) ||
+	else if (test_and_clear_bit(_WDOG_ALLOW_RELEASE, &wd_data->status) ||
 		 !(wdd->info->options & WDIOF_MAGICCLOSE))
 		err = watchdog_stop(wdd);
 
 	/* If the watchdog was not stopped, send a keepalive ping */
 	if (err < 0) {
-		mutex_lock(&wdd->lock);
-		if (!test_bit(WDOG_UNREGISTERED, &wdd->status))
-			dev_crit(wdd->dev, "watchdog did not stop!\n");
-		mutex_unlock(&wdd->lock);
+		pr_crit("watchdog%d: watchdog did not stop!\n", wdd->id);
 		watchdog_ping(wdd);
 	}
 
-	/* Allow the owner module to be unloaded again */
-	module_put(wdd->ops->owner);
-
 	/* make sure that /dev/watchdog can be re-opened */
-	clear_bit(WDOG_DEV_OPEN, &wdd->status);
-
-	/* Note wdd may be gone after this, do not use after this! */
-	if (wdd->ops->unref)
-		wdd->ops->unref(wdd);
+	clear_bit(_WDOG_DEV_OPEN, &wd_data->status);
 
+done:
+	mutex_unlock(&wd_data->lock);
+	/* Allow the owner module to be unloaded again */
+	module_put(wd_data->cdev.owner);
+	kref_put(&wd_data->kref, watchdog_core_data_release);
 	return 0;
 }
 
@@ -515,20 +635,31 @@ static struct miscdevice watchdog_miscdev = {
 };
 
 /*
- *	watchdog_dev_register: register a watchdog device
+ *	watchdog_cdev_register: register watchdog character device
  *	@wdd: watchdog device
+ *	@devno: character device number
  *
- *	Register a watchdog device including handling the legacy
+ *	Register a watchdog character device including handling the legacy
  *	/dev/watchdog node. /dev/watchdog is actually a miscdevice and
  *	thus we set it up like that.
  */
 
-int watchdog_dev_register(struct watchdog_device *wdd)
+static int watchdog_cdev_register(struct watchdog_device *wdd, dev_t devno)
 {
-	int err, devno;
+	struct watchdog_core_data *wd_data;
+	int err;
+
+	wd_data = kzalloc(sizeof(struct watchdog_core_data), GFP_KERNEL);
+	if (!wd_data)
+		return -ENOMEM;
+	kref_init(&wd_data->kref);
+	mutex_init(&wd_data->lock);
+
+	wd_data->wdd = wdd;
+	wdd->wd_data = wd_data;
 
 	if (wdd->id == 0) {
-		old_wdd = wdd;
+		old_wd_data = wd_data;
 		watchdog_miscdev.parent = wdd->parent;
 		err = misc_register(&watchdog_miscdev);
 		if (err != 0) {
@@ -537,48 +668,106 @@ int watchdog_dev_register(struct watchdog_device *wdd)
 			if (err == -EBUSY)
 				pr_err("%s: a legacy watchdog module is probably present.\n",
 					wdd->info->identity);
-			old_wdd = NULL;
+			old_wd_data = NULL;
+			kfree(wd_data);
 			return err;
 		}
 	}
 
 	/* Fill in the data structures */
-	devno = MKDEV(MAJOR(watchdog_devt), wdd->id);
-	cdev_init(&wdd->cdev, &watchdog_fops);
-	wdd->cdev.owner = wdd->ops->owner;
+	cdev_init(&wd_data->cdev, &watchdog_fops);
+	wd_data->cdev.owner = wdd->ops->owner;
 
 	/* Add the device */
-	err  = cdev_add(&wdd->cdev, devno, 1);
+	err = cdev_add(&wd_data->cdev, devno, 1);
 	if (err) {
 		pr_err("watchdog%d unable to add device %d:%d\n",
 			wdd->id,  MAJOR(watchdog_devt), wdd->id);
 		if (wdd->id == 0) {
 			misc_deregister(&watchdog_miscdev);
-			old_wdd = NULL;
+			old_wd_data = NULL;
+			kref_put(&wd_data->kref, watchdog_core_data_release);
 		}
 	}
 	return err;
 }
 
 /*
- *	watchdog_dev_unregister: unregister a watchdog device
+ *	watchdog_cdev_unregister: unregister watchdog character device
  *	@watchdog: watchdog device
  *
- *	Unregister the watchdog and if needed the legacy /dev/watchdog device.
+ *	Unregister watchdog character device and if needed the legacy
+ *	/dev/watchdog device.
  */
 
-int watchdog_dev_unregister(struct watchdog_device *wdd)
+static void watchdog_cdev_unregister(struct watchdog_device *wdd)
 {
-	mutex_lock(&wdd->lock);
-	set_bit(WDOG_UNREGISTERED, &wdd->status);
-	mutex_unlock(&wdd->lock);
+	struct watchdog_core_data *wd_data = wdd->wd_data;
 
-	cdev_del(&wdd->cdev);
+	cdev_del(&wd_data->cdev);
 	if (wdd->id == 0) {
 		misc_deregister(&watchdog_miscdev);
-		old_wdd = NULL;
+		old_wd_data = NULL;
 	}
-	return 0;
+
+	mutex_lock(&wd_data->lock);
+	wd_data->wdd = NULL;
+	wdd->wd_data = NULL;
+	mutex_unlock(&wd_data->lock);
+
+	kref_put(&wd_data->kref, watchdog_core_data_release);
+}
+
+static struct class watchdog_class = {
+	.name =		"watchdog",
+	.owner =	THIS_MODULE,
+	.dev_groups =	wdt_groups,
+};
+
+/*
+ *	watchdog_dev_register: register a watchdog device
+ *	@wdd: watchdog device
+ *
+ *	Register a watchdog device including handling the legacy
+ *	/dev/watchdog node. /dev/watchdog is actually a miscdevice and
+ *	thus we set it up like that.
+ */
+
+int watchdog_dev_register(struct watchdog_device *wdd)
+{
+	struct device *dev;
+	dev_t devno;
+	int ret;
+
+	devno = MKDEV(MAJOR(watchdog_devt), wdd->id);
+
+	ret = watchdog_cdev_register(wdd, devno);
+	if (ret)
+		return ret;
+
+	dev = device_create_with_groups(&watchdog_class, wdd->parent,
+					devno, wdd, wdd->groups,
+					"watchdog%d", wdd->id);
+	if (IS_ERR(dev)) {
+		watchdog_cdev_unregister(wdd);
+		return PTR_ERR(dev);
+	}
+
+	return ret;
+}
+
+/*
+ *	watchdog_dev_unregister: unregister a watchdog device
+ *	@watchdog: watchdog device
+ *
+ *	Unregister watchdog device and if needed the legacy
+ *	/dev/watchdog device.
+ */
+
+void watchdog_dev_unregister(struct watchdog_device *wdd)
+{
+	device_destroy(&watchdog_class, wdd->wd_data->cdev.dev);
+	watchdog_cdev_unregister(wdd);
 }
 
 /*
@@ -589,10 +778,22 @@ int watchdog_dev_unregister(struct watchdog_device *wdd)
 
 int __init watchdog_dev_init(void)
 {
-	int err = alloc_chrdev_region(&watchdog_devt, 0, MAX_DOGS, "watchdog");
-	if (err < 0)
+	int err;
+
+	err = class_register(&watchdog_class);
+	if (err < 0) {
+		pr_err("couldn't register class\n");
+		return err;
+	}
+
+	err = alloc_chrdev_region(&watchdog_devt, 0, MAX_DOGS, "watchdog");
+	if (err < 0) {
 		pr_err("watchdog: unable to allocate char dev region\n");
-	return err;
+		class_unregister(&watchdog_class);
+		return err;
+	}
+
+	return 0;
 }
 
 /*
@@ -604,4 +805,5 @@ int __init watchdog_dev_init(void)
 void __exit watchdog_dev_exit(void)
 {
 	unregister_chrdev_region(watchdog_devt, MAX_DOGS);
+	class_unregister(&watchdog_class);
 }
diff --git a/drivers/watchdog/ziirave_wdt.c b/drivers/watchdog/ziirave_wdt.c
new file mode 100644
index 000000000000..0c7cb7302cf0
--- /dev/null
+++ b/drivers/watchdog/ziirave_wdt.c
@@ -0,0 +1,367 @@
+/*
+ * Copyright (C) 2015 Zodiac Inflight Innovations
+ *
+ * Author: Martyn Welch <martyn.welch@collabora.co.uk>
+ *
+ * Based on twl4030_wdt.c by Timo Kokkonen <timo.t.kokkonen at nokia.com>:
+ *
+ * Copyright (C) Nokia Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/sysfs.h>
+#include <linux/types.h>
+#include <linux/version.h>
+#include <linux/watchdog.h>
+
+#define ZIIRAVE_TIMEOUT_MIN	3
+#define ZIIRAVE_TIMEOUT_MAX	255
+
+#define ZIIRAVE_PING_VALUE	0x0
+
+#define ZIIRAVE_STATE_INITIAL	0x0
+#define ZIIRAVE_STATE_OFF	0x1
+#define ZIIRAVE_STATE_ON	0x2
+
+static char *ziirave_reasons[] = {"power cycle", "triggered", NULL, NULL,
+				  "host request", NULL, "illegal configuration",
+				  "illegal instruction", "illegal trap",
+				  "unknown"};
+
+#define ZIIRAVE_WDT_FIRM_VER_MAJOR	0x1
+#define ZIIRAVE_WDT_BOOT_VER_MAJOR	0x3
+#define ZIIRAVE_WDT_RESET_REASON	0x5
+#define ZIIRAVE_WDT_STATE		0x6
+#define ZIIRAVE_WDT_TIMEOUT		0x7
+#define ZIIRAVE_WDT_TIME_LEFT		0x8
+#define ZIIRAVE_WDT_PING		0x9
+#define ZIIRAVE_WDT_RESET_DURATION	0xa
+
+struct ziirave_wdt_rev {
+	unsigned char major;
+	unsigned char minor;
+};
+
+struct ziirave_wdt_data {
+	struct watchdog_device wdd;
+	struct ziirave_wdt_rev bootloader_rev;
+	struct ziirave_wdt_rev firmware_rev;
+	int reset_reason;
+};
+
+static int wdt_timeout;
+module_param(wdt_timeout, int, 0);
+MODULE_PARM_DESC(wdt_timeout, "Watchdog timeout in seconds");
+
+static int reset_duration;
+module_param(reset_duration, int, 0);
+MODULE_PARM_DESC(reset_duration,
+		 "Watchdog reset pulse duration in milliseconds");
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+static int ziirave_wdt_revision(struct i2c_client *client,
+				struct ziirave_wdt_rev *rev, u8 command)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, command);
+	if (ret < 0)
+		return ret;
+
+	rev->major = ret;
+
+	ret = i2c_smbus_read_byte_data(client, command + 1);
+	if (ret < 0)
+		return ret;
+
+	rev->minor = ret;
+
+	return 0;
+}
+
+static int ziirave_wdt_set_state(struct watchdog_device *wdd, int state)
+{
+	struct i2c_client *client = to_i2c_client(wdd->parent);
+
+	return i2c_smbus_write_byte_data(client, ZIIRAVE_WDT_STATE, state);
+}
+
+static int ziirave_wdt_start(struct watchdog_device *wdd)
+{
+	return ziirave_wdt_set_state(wdd, ZIIRAVE_STATE_ON);
+}
+
+static int ziirave_wdt_stop(struct watchdog_device *wdd)
+{
+	return ziirave_wdt_set_state(wdd, ZIIRAVE_STATE_OFF);
+}
+
+static int ziirave_wdt_ping(struct watchdog_device *wdd)
+{
+	struct i2c_client *client = to_i2c_client(wdd->parent);
+
+	return i2c_smbus_write_byte_data(client, ZIIRAVE_WDT_PING,
+					 ZIIRAVE_PING_VALUE);
+}
+
+static int ziirave_wdt_set_timeout(struct watchdog_device *wdd,
+				   unsigned int timeout)
+{
+	struct i2c_client *client = to_i2c_client(wdd->parent);
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(client, ZIIRAVE_WDT_TIMEOUT, timeout);
+	if (!ret)
+		wdd->timeout = timeout;
+
+	return ret;
+}
+
+static unsigned int ziirave_wdt_get_timeleft(struct watchdog_device *wdd)
+{
+	struct i2c_client *client = to_i2c_client(wdd->parent);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(client, ZIIRAVE_WDT_TIME_LEFT);
+	if (ret < 0)
+		ret = 0;
+
+	return ret;
+}
+
+static const struct watchdog_info ziirave_wdt_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE | WDIOF_KEEPALIVEPING,
+	.identity = "Zodiac RAVE Watchdog",
+};
+
+static const struct watchdog_ops ziirave_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= ziirave_wdt_start,
+	.stop		= ziirave_wdt_stop,
+	.ping		= ziirave_wdt_ping,
+	.set_timeout	= ziirave_wdt_set_timeout,
+	.get_timeleft	= ziirave_wdt_get_timeleft,
+};
+
+static ssize_t ziirave_wdt_sysfs_show_firm(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev->parent);
+	struct ziirave_wdt_data *w_priv = i2c_get_clientdata(client);
+
+	return sprintf(buf, "02.%02u.%02u", w_priv->firmware_rev.major,
+		       w_priv->firmware_rev.minor);
+}
+
+static DEVICE_ATTR(firmware_version, S_IRUGO, ziirave_wdt_sysfs_show_firm,
+		   NULL);
+
+static ssize_t ziirave_wdt_sysfs_show_boot(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev->parent);
+	struct ziirave_wdt_data *w_priv = i2c_get_clientdata(client);
+
+	return sprintf(buf, "01.%02u.%02u", w_priv->bootloader_rev.major,
+		       w_priv->bootloader_rev.minor);
+}
+
+static DEVICE_ATTR(bootloader_version, S_IRUGO, ziirave_wdt_sysfs_show_boot,
+		   NULL);
+
+static ssize_t ziirave_wdt_sysfs_show_reason(struct device *dev,
+					     struct device_attribute *attr,
+					     char *buf)
+{
+	struct i2c_client *client = to_i2c_client(dev->parent);
+	struct ziirave_wdt_data *w_priv = i2c_get_clientdata(client);
+
+	return sprintf(buf, "%s", ziirave_reasons[w_priv->reset_reason]);
+}
+
+static DEVICE_ATTR(reset_reason, S_IRUGO, ziirave_wdt_sysfs_show_reason,
+		   NULL);
+
+static struct attribute *ziirave_wdt_attrs[] = {
+	&dev_attr_firmware_version.attr,
+	&dev_attr_bootloader_version.attr,
+	&dev_attr_reset_reason.attr,
+	NULL
+};
+ATTRIBUTE_GROUPS(ziirave_wdt);
+
+static int ziirave_wdt_init_duration(struct i2c_client *client)
+{
+	int ret;
+
+	if (!reset_duration) {
+		/* See if the reset pulse duration is provided in an of_node */
+		if (!client->dev.of_node)
+			ret = -ENODEV;
+		else
+			ret = of_property_read_u32(client->dev.of_node,
+						   "reset-duration-ms",
+						   &reset_duration);
+		if (ret) {
+			dev_info(&client->dev,
+				 "Unable to set reset pulse duration, using default\n");
+			return 0;
+		}
+	}
+
+	if (reset_duration < 1 || reset_duration > 255)
+		return -EINVAL;
+
+	dev_info(&client->dev, "Setting reset duration to %dms",
+		 reset_duration);
+
+	return i2c_smbus_write_byte_data(client, ZIIRAVE_WDT_RESET_DURATION,
+					 reset_duration);
+}
+
+static int ziirave_wdt_probe(struct i2c_client *client,
+			     const struct i2c_device_id *id)
+{
+	int ret;
+	struct ziirave_wdt_data *w_priv;
+	int val;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
+		return -ENODEV;
+
+	w_priv = devm_kzalloc(&client->dev, sizeof(*w_priv), GFP_KERNEL);
+	if (!w_priv)
+		return -ENOMEM;
+
+	w_priv->wdd.info = &ziirave_wdt_info;
+	w_priv->wdd.ops = &ziirave_wdt_ops;
+	w_priv->wdd.min_timeout = ZIIRAVE_TIMEOUT_MIN;
+	w_priv->wdd.max_timeout = ZIIRAVE_TIMEOUT_MAX;
+	w_priv->wdd.parent = &client->dev;
+	w_priv->wdd.groups = ziirave_wdt_groups;
+
+	ret = watchdog_init_timeout(&w_priv->wdd, wdt_timeout, &client->dev);
+	if (ret) {
+		dev_info(&client->dev,
+			 "Unable to select timeout value, using default\n");
+	}
+
+	/*
+	 * The default value set in the watchdog should be perfectly valid, so
+	 * pass that in if we haven't provided one via the module parameter or
+	 * of property.
+	 */
+	if (w_priv->wdd.timeout == 0) {
+		val = i2c_smbus_read_byte_data(client, ZIIRAVE_WDT_TIMEOUT);
+		if (val < 0)
+			return val;
+
+		if (val < ZIIRAVE_TIMEOUT_MIN)
+			return -ENODEV;
+
+		w_priv->wdd.timeout = val;
+	} else {
+		ret = ziirave_wdt_set_timeout(&w_priv->wdd,
+					      w_priv->wdd.timeout);
+		if (ret)
+			return ret;
+
+		dev_info(&client->dev, "Timeout set to %ds.",
+			 w_priv->wdd.timeout);
+	}
+
+	watchdog_set_nowayout(&w_priv->wdd, nowayout);
+
+	i2c_set_clientdata(client, w_priv);
+
+	/* If in unconfigured state, set to stopped */
+	val = i2c_smbus_read_byte_data(client, ZIIRAVE_WDT_STATE);
+	if (val < 0)
+		return val;
+
+	if (val == ZIIRAVE_STATE_INITIAL)
+		ziirave_wdt_stop(&w_priv->wdd);
+
+	ret = ziirave_wdt_init_duration(client);
+	if (ret)
+		return ret;
+
+	ret = ziirave_wdt_revision(client, &w_priv->firmware_rev,
+				   ZIIRAVE_WDT_FIRM_VER_MAJOR);
+	if (ret)
+		return ret;
+
+	ret = ziirave_wdt_revision(client, &w_priv->bootloader_rev,
+				   ZIIRAVE_WDT_BOOT_VER_MAJOR);
+	if (ret)
+		return ret;
+
+	w_priv->reset_reason = i2c_smbus_read_byte_data(client,
+						ZIIRAVE_WDT_RESET_REASON);
+	if (w_priv->reset_reason < 0)
+		return w_priv->reset_reason;
+
+	if (w_priv->reset_reason >= ARRAY_SIZE(ziirave_reasons) ||
+	    !ziirave_reasons[w_priv->reset_reason])
+		return -ENODEV;
+
+	ret = watchdog_register_device(&w_priv->wdd);
+
+	return ret;
+}
+
+static int ziirave_wdt_remove(struct i2c_client *client)
+{
+	struct ziirave_wdt_data *w_priv = i2c_get_clientdata(client);
+
+	watchdog_unregister_device(&w_priv->wdd);
+
+	return 0;
+}
+
+static struct i2c_device_id ziirave_wdt_id[] = {
+	{ "ziirave-wdt", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, ziirave_wdt_id);
+
+static const struct of_device_id zrv_wdt_of_match[] = {
+	{ .compatible = "zii,rave-wdt", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, zrv_wdt_of_match);
+
+static struct i2c_driver ziirave_wdt_driver = {
+	.driver = {
+		.name = "ziirave_wdt",
+		.of_match_table = zrv_wdt_of_match,
+	},
+	.probe = ziirave_wdt_probe,
+	.remove = ziirave_wdt_remove,
+	.id_table = ziirave_wdt_id,
+};
+
+module_i2c_driver(ziirave_wdt_driver);
+
+MODULE_AUTHOR("Martyn Welch <martyn.welch@collabora.co.uk");
+MODULE_DESCRIPTION("Zodiac Aerospace RAVE Switch Watchdog Processor Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index aa8a7f71f310..9b7a35c9e51d 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -1,6 +1,6 @@
 obj-$(CONFIG_HOTPLUG_CPU)		+= cpu_hotplug.o
 obj-$(CONFIG_X86)			+= fallback.o
-obj-y	+= grant-table.o features.o balloon.o manage.o preempt.o
+obj-y	+= grant-table.o features.o balloon.o manage.o preempt.o time.o
 obj-y	+= events/
 obj-y	+= xenbus/
 
diff --git a/drivers/xen/acpi.c b/drivers/xen/acpi.c
index 90307c0b630c..6893c79fd2a1 100644
--- a/drivers/xen/acpi.c
+++ b/drivers/xen/acpi.c
@@ -58,7 +58,7 @@ static int xen_acpi_notify_hypervisor_state(u8 sleep_state,
 		 bits, val_a, val_b))
 		return -1;
 
-	HYPERVISOR_dom0_op(&op);
+	HYPERVISOR_platform_op(&op);
 	return 1;
 }
 
diff --git a/drivers/xen/efi.c b/drivers/xen/efi.c
index f745db270171..be7e56a338e8 100644
--- a/drivers/xen/efi.c
+++ b/drivers/xen/efi.c
@@ -42,7 +42,7 @@ static efi_status_t xen_efi_get_time(efi_time_t *tm, efi_time_cap_t *tc)
 {
 	struct xen_platform_op op = INIT_EFI_OP(get_time);
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	if (tm) {
@@ -67,7 +67,7 @@ static efi_status_t xen_efi_set_time(efi_time_t *tm)
 	BUILD_BUG_ON(sizeof(*tm) != sizeof(efi_data(op).u.set_time));
 	memcpy(&efi_data(op).u.set_time, tm, sizeof(*tm));
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	return efi_data(op).status;
@@ -79,7 +79,7 @@ static efi_status_t xen_efi_get_wakeup_time(efi_bool_t *enabled,
 {
 	struct xen_platform_op op = INIT_EFI_OP(get_wakeup_time);
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	if (tm) {
@@ -108,7 +108,7 @@ static efi_status_t xen_efi_set_wakeup_time(efi_bool_t enabled, efi_time_t *tm)
 	else
 		efi_data(op).misc |= XEN_EFI_SET_WAKEUP_TIME_ENABLE_ONLY;
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	return efi_data(op).status;
@@ -129,7 +129,7 @@ static efi_status_t xen_efi_get_variable(efi_char16_t *name,
 	efi_data(op).u.get_variable.size = *data_size;
 	set_xen_guest_handle(efi_data(op).u.get_variable.data, data);
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	*data_size = efi_data(op).u.get_variable.size;
@@ -152,7 +152,7 @@ static efi_status_t xen_efi_get_next_variable(unsigned long *name_size,
 	memcpy(&efi_data(op).u.get_next_variable_name.vendor_guid, vendor,
 	       sizeof(*vendor));
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	*name_size = efi_data(op).u.get_next_variable_name.size;
@@ -178,7 +178,7 @@ static efi_status_t xen_efi_set_variable(efi_char16_t *name,
 	efi_data(op).u.set_variable.size = data_size;
 	set_xen_guest_handle(efi_data(op).u.set_variable.data, data);
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	return efi_data(op).status;
@@ -196,7 +196,7 @@ static efi_status_t xen_efi_query_variable_info(u32 attr,
 
 	efi_data(op).u.query_variable_info.attr = attr;
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	*storage_space = efi_data(op).u.query_variable_info.max_store_size;
@@ -210,7 +210,7 @@ static efi_status_t xen_efi_get_next_high_mono_count(u32 *count)
 {
 	struct xen_platform_op op = INIT_EFI_OP(get_next_high_monotonic_count);
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	*count = efi_data(op).misc;
@@ -232,7 +232,7 @@ static efi_status_t xen_efi_update_capsule(efi_capsule_header_t **capsules,
 	efi_data(op).u.update_capsule.capsule_count = count;
 	efi_data(op).u.update_capsule.sg_list = sg_list;
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	return efi_data(op).status;
@@ -252,7 +252,7 @@ static efi_status_t xen_efi_query_capsule_caps(efi_capsule_header_t **capsules,
 					capsules);
 	efi_data(op).u.query_capsule_capabilities.capsule_count = count;
 
-	if (HYPERVISOR_dom0_op(&op) < 0)
+	if (HYPERVISOR_platform_op(&op) < 0)
 		return EFI_UNSUPPORTED;
 
 	*max_size = efi_data(op).u.query_capsule_capabilities.max_capsule_size;
@@ -331,7 +331,7 @@ efi_system_table_t __init *xen_efi_probe(void)
 	};
 	union xenpf_efi_info *info = &op.u.firmware_info.u.efi_info;
 
-	if (!xen_initial_domain() || HYPERVISOR_dom0_op(&op) < 0)
+	if (!xen_initial_domain() || HYPERVISOR_platform_op(&op) < 0)
 		return NULL;
 
 	/* Here we know that Xen runs on EFI platform. */
@@ -347,7 +347,7 @@ efi_system_table_t __init *xen_efi_probe(void)
 	info->vendor.bufsz = sizeof(vendor);
 	set_xen_guest_handle(info->vendor.name, vendor);
 
-	if (HYPERVISOR_dom0_op(&op) == 0) {
+	if (HYPERVISOR_platform_op(&op) == 0) {
 		efi_systab_xen.fw_vendor = __pa_symbol(vendor);
 		efi_systab_xen.fw_revision = info->vendor.revision;
 	} else
@@ -357,14 +357,14 @@ efi_system_table_t __init *xen_efi_probe(void)
 	op.u.firmware_info.type = XEN_FW_EFI_INFO;
 	op.u.firmware_info.index = XEN_FW_EFI_VERSION;
 
-	if (HYPERVISOR_dom0_op(&op) == 0)
+	if (HYPERVISOR_platform_op(&op) == 0)
 		efi_systab_xen.hdr.revision = info->version;
 
 	op.cmd = XENPF_firmware_info;
 	op.u.firmware_info.type = XEN_FW_EFI_INFO;
 	op.u.firmware_info.index = XEN_FW_EFI_RT_VERSION;
 
-	if (HYPERVISOR_dom0_op(&op) == 0)
+	if (HYPERVISOR_platform_op(&op) == 0)
 		efi.runtime_version = info->version;
 
 	return &efi_systab_xen;
diff --git a/drivers/xen/events/events_fifo.c b/drivers/xen/events/events_fifo.c
index 96a1b8da5371..eff2b88003d9 100644
--- a/drivers/xen/events/events_fifo.c
+++ b/drivers/xen/events/events_fifo.c
@@ -41,6 +41,7 @@
 #include <linux/percpu.h>
 #include <linux/cpu.h>
 
+#include <asm/barrier.h>
 #include <asm/sync_bitops.h>
 #include <asm/xen/hypercall.h>
 #include <asm/xen/hypervisor.h>
@@ -296,7 +297,7 @@ static void consume_one_event(unsigned cpu,
 	 * control block.
 	 */
 	if (head == 0) {
-		rmb(); /* Ensure word is up-to-date before reading head. */
+		virt_rmb(); /* Ensure word is up-to-date before reading head. */
 		head = control_block->head[priority];
 	}
 
diff --git a/drivers/xen/gntdev.c b/drivers/xen/gntdev.c
index 1be5dd048622..dc495383ad73 100644
--- a/drivers/xen/gntdev.c
+++ b/drivers/xen/gntdev.c
@@ -518,7 +518,7 @@ static void mn_release(struct mmu_notifier *mn,
 	mutex_unlock(&priv->lock);
 }
 
-static struct mmu_notifier_ops gntdev_mmu_ops = {
+static const struct mmu_notifier_ops gntdev_mmu_ops = {
 	.release                = mn_release,
 	.invalidate_page        = mn_invl_page,
 	.invalidate_range_start = mn_invl_range_start,
@@ -748,6 +748,206 @@ static long gntdev_ioctl_notify(struct gntdev_priv *priv, void __user *u)
 	return rc;
 }
 
+#define GNTDEV_COPY_BATCH 24
+
+struct gntdev_copy_batch {
+	struct gnttab_copy ops[GNTDEV_COPY_BATCH];
+	struct page *pages[GNTDEV_COPY_BATCH];
+	s16 __user *status[GNTDEV_COPY_BATCH];
+	unsigned int nr_ops;
+	unsigned int nr_pages;
+};
+
+static int gntdev_get_page(struct gntdev_copy_batch *batch, void __user *virt,
+			   bool writeable, unsigned long *gfn)
+{
+	unsigned long addr = (unsigned long)virt;
+	struct page *page;
+	unsigned long xen_pfn;
+	int ret;
+
+	ret = get_user_pages_fast(addr, 1, writeable, &page);
+	if (ret < 0)
+		return ret;
+
+	batch->pages[batch->nr_pages++] = page;
+
+	xen_pfn = page_to_xen_pfn(page) + XEN_PFN_DOWN(addr & ~PAGE_MASK);
+	*gfn = pfn_to_gfn(xen_pfn);
+
+	return 0;
+}
+
+static void gntdev_put_pages(struct gntdev_copy_batch *batch)
+{
+	unsigned int i;
+
+	for (i = 0; i < batch->nr_pages; i++)
+		put_page(batch->pages[i]);
+	batch->nr_pages = 0;
+}
+
+static int gntdev_copy(struct gntdev_copy_batch *batch)
+{
+	unsigned int i;
+
+	gnttab_batch_copy(batch->ops, batch->nr_ops);
+	gntdev_put_pages(batch);
+
+	/*
+	 * For each completed op, update the status if the op failed
+	 * and all previous ops for the segment were successful.
+	 */
+	for (i = 0; i < batch->nr_ops; i++) {
+		s16 status = batch->ops[i].status;
+		s16 old_status;
+
+		if (status == GNTST_okay)
+			continue;
+
+		if (__get_user(old_status, batch->status[i]))
+			return -EFAULT;
+
+		if (old_status != GNTST_okay)
+			continue;
+
+		if (__put_user(status, batch->status[i]))
+			return -EFAULT;
+	}
+
+	batch->nr_ops = 0;
+	return 0;
+}
+
+static int gntdev_grant_copy_seg(struct gntdev_copy_batch *batch,
+				 struct gntdev_grant_copy_segment *seg,
+				 s16 __user *status)
+{
+	uint16_t copied = 0;
+
+	/*
+	 * Disallow local -> local copies since there is only space in
+	 * batch->pages for one page per-op and this would be a very
+	 * expensive memcpy().
+	 */
+	if (!(seg->flags & (GNTCOPY_source_gref | GNTCOPY_dest_gref)))
+		return -EINVAL;
+
+	/* Can't cross page if source/dest is a grant ref. */
+	if (seg->flags & GNTCOPY_source_gref) {
+		if (seg->source.foreign.offset + seg->len > XEN_PAGE_SIZE)
+			return -EINVAL;
+	}
+	if (seg->flags & GNTCOPY_dest_gref) {
+		if (seg->dest.foreign.offset + seg->len > XEN_PAGE_SIZE)
+			return -EINVAL;
+	}
+
+	if (put_user(GNTST_okay, status))
+		return -EFAULT;
+
+	while (copied < seg->len) {
+		struct gnttab_copy *op;
+		void __user *virt;
+		size_t len, off;
+		unsigned long gfn;
+		int ret;
+
+		if (batch->nr_ops >= GNTDEV_COPY_BATCH) {
+			ret = gntdev_copy(batch);
+			if (ret < 0)
+				return ret;
+		}
+
+		len = seg->len - copied;
+
+		op = &batch->ops[batch->nr_ops];
+		op->flags = 0;
+
+		if (seg->flags & GNTCOPY_source_gref) {
+			op->source.u.ref = seg->source.foreign.ref;
+			op->source.domid = seg->source.foreign.domid;
+			op->source.offset = seg->source.foreign.offset + copied;
+			op->flags |= GNTCOPY_source_gref;
+		} else {
+			virt = seg->source.virt + copied;
+			off = (unsigned long)virt & ~XEN_PAGE_MASK;
+			len = min(len, (size_t)XEN_PAGE_SIZE - off);
+
+			ret = gntdev_get_page(batch, virt, false, &gfn);
+			if (ret < 0)
+				return ret;
+
+			op->source.u.gmfn = gfn;
+			op->source.domid = DOMID_SELF;
+			op->source.offset = off;
+		}
+
+		if (seg->flags & GNTCOPY_dest_gref) {
+			op->dest.u.ref = seg->dest.foreign.ref;
+			op->dest.domid = seg->dest.foreign.domid;
+			op->dest.offset = seg->dest.foreign.offset + copied;
+			op->flags |= GNTCOPY_dest_gref;
+		} else {
+			virt = seg->dest.virt + copied;
+			off = (unsigned long)virt & ~XEN_PAGE_MASK;
+			len = min(len, (size_t)XEN_PAGE_SIZE - off);
+
+			ret = gntdev_get_page(batch, virt, true, &gfn);
+			if (ret < 0)
+				return ret;
+
+			op->dest.u.gmfn = gfn;
+			op->dest.domid = DOMID_SELF;
+			op->dest.offset = off;
+		}
+
+		op->len = len;
+		copied += len;
+
+		batch->status[batch->nr_ops] = status;
+		batch->nr_ops++;
+	}
+
+	return 0;
+}
+
+static long gntdev_ioctl_grant_copy(struct gntdev_priv *priv, void __user *u)
+{
+	struct ioctl_gntdev_grant_copy copy;
+	struct gntdev_copy_batch batch;
+	unsigned int i;
+	int ret = 0;
+
+	if (copy_from_user(&copy, u, sizeof(copy)))
+		return -EFAULT;
+
+	batch.nr_ops = 0;
+	batch.nr_pages = 0;
+
+	for (i = 0; i < copy.count; i++) {
+		struct gntdev_grant_copy_segment seg;
+
+		if (copy_from_user(&seg, &copy.segments[i], sizeof(seg))) {
+			ret = -EFAULT;
+			goto out;
+		}
+
+		ret = gntdev_grant_copy_seg(&batch, &seg, &copy.segments[i].status);
+		if (ret < 0)
+			goto out;
+
+		cond_resched();
+	}
+	if (batch.nr_ops)
+		ret = gntdev_copy(&batch);
+	return ret;
+
+  out:
+	gntdev_put_pages(&batch);
+	return ret;
+}
+
 static long gntdev_ioctl(struct file *flip,
 			 unsigned int cmd, unsigned long arg)
 {
@@ -767,6 +967,9 @@ static long gntdev_ioctl(struct file *flip,
 	case IOCTL_GNTDEV_SET_UNMAP_NOTIFY:
 		return gntdev_ioctl_notify(priv, ptr);
 
+	case IOCTL_GNTDEV_GRANT_COPY:
+		return gntdev_ioctl_grant_copy(priv, ptr);
+
 	default:
 		pr_debug("priv %p, unknown cmd %x\n", priv, cmd);
 		return -ENOIOCTLCMD;
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index c49f79ed58c5..effbaf91791f 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -128,7 +128,7 @@ struct unmap_refs_callback_data {
 	int result;
 };
 
-static struct gnttab_ops *gnttab_interface;
+static const struct gnttab_ops *gnttab_interface;
 
 static int grant_table_version;
 static int grefs_per_grant_frame;
@@ -1013,7 +1013,7 @@ static int gnttab_map(unsigned int start_idx, unsigned int end_idx)
 	return rc;
 }
 
-static struct gnttab_ops gnttab_v1_ops = {
+static const struct gnttab_ops gnttab_v1_ops = {
 	.map_frames			= gnttab_map_frames_v1,
 	.unmap_frames			= gnttab_unmap_frames_v1,
 	.update_entry			= gnttab_update_entry_v1,
diff --git a/drivers/xen/pcpu.c b/drivers/xen/pcpu.c
index 49e88f2ce7a1..cdc6daa7a9f6 100644
--- a/drivers/xen/pcpu.c
+++ b/drivers/xen/pcpu.c
@@ -78,7 +78,7 @@ static int xen_pcpu_down(uint32_t cpu_id)
 		.u.cpu_ol.cpuid		= cpu_id,
 	};
 
-	return HYPERVISOR_dom0_op(&op);
+	return HYPERVISOR_platform_op(&op);
 }
 
 static int xen_pcpu_up(uint32_t cpu_id)
@@ -89,7 +89,7 @@ static int xen_pcpu_up(uint32_t cpu_id)
 		.u.cpu_ol.cpuid		= cpu_id,
 	};
 
-	return HYPERVISOR_dom0_op(&op);
+	return HYPERVISOR_platform_op(&op);
 }
 
 static ssize_t show_online(struct device *dev,
@@ -277,7 +277,7 @@ static int sync_pcpu(uint32_t cpu, uint32_t *max_cpu)
 		.u.pcpu_info.xen_cpuid = cpu,
 	};
 
-	ret = HYPERVISOR_dom0_op(&op);
+	ret = HYPERVISOR_platform_op(&op);
 	if (ret)
 		return ret;
 
@@ -364,7 +364,7 @@ int xen_pcpu_id(uint32_t acpi_id)
 	op.cmd = XENPF_get_cpuinfo;
 	while (cpu_id <= max_id) {
 		op.u.pcpu_info.xen_cpuid = cpu_id;
-		if (HYPERVISOR_dom0_op(&op)) {
+		if (HYPERVISOR_platform_op(&op)) {
 			cpu_id++;
 			continue;
 		}
diff --git a/drivers/xen/time.c b/drivers/xen/time.c
new file mode 100644
index 000000000000..71078425c9ea
--- /dev/null
+++ b/drivers/xen/time.c
@@ -0,0 +1,88 @@
+/*
+ * Xen stolen ticks accounting.
+ */
+#include <linux/kernel.h>
+#include <linux/kernel_stat.h>
+#include <linux/math64.h>
+#include <linux/gfp.h>
+
+#include <asm/xen/hypervisor.h>
+#include <asm/xen/hypercall.h>
+
+#include <xen/events.h>
+#include <xen/features.h>
+#include <xen/interface/xen.h>
+#include <xen/interface/vcpu.h>
+#include <xen/xen-ops.h>
+
+/* runstate info updated by Xen */
+static DEFINE_PER_CPU(struct vcpu_runstate_info, xen_runstate);
+
+/* return an consistent snapshot of 64-bit time/counter value */
+static u64 get64(const u64 *p)
+{
+	u64 ret;
+
+	if (BITS_PER_LONG < 64) {
+		u32 *p32 = (u32 *)p;
+		u32 h, l, h2;
+
+		/*
+		 * Read high then low, and then make sure high is
+		 * still the same; this will only loop if low wraps
+		 * and carries into high.
+		 * XXX some clean way to make this endian-proof?
+		 */
+		do {
+			h = READ_ONCE(p32[1]);
+			l = READ_ONCE(p32[0]);
+			h2 = READ_ONCE(p32[1]);
+		} while(h2 != h);
+
+		ret = (((u64)h) << 32) | l;
+	} else
+		ret = READ_ONCE(*p);
+
+	return ret;
+}
+
+/*
+ * Runstate accounting
+ */
+void xen_get_runstate_snapshot(struct vcpu_runstate_info *res)
+{
+	u64 state_time;
+	struct vcpu_runstate_info *state;
+
+	BUG_ON(preemptible());
+
+	state = this_cpu_ptr(&xen_runstate);
+
+	/*
+	 * The runstate info is always updated by the hypervisor on
+	 * the current CPU, so there's no need to use anything
+	 * stronger than a compiler barrier when fetching it.
+	 */
+	do {
+		state_time = get64(&state->state_entry_time);
+		*res = READ_ONCE(*state);
+	} while (get64(&state->state_entry_time) != state_time);
+}
+
+/* return true when a vcpu could run but has no real cpu to run on */
+bool xen_vcpu_stolen(int vcpu)
+{
+	return per_cpu(xen_runstate, vcpu).state == RUNSTATE_runnable;
+}
+
+void xen_setup_runstate_info(int cpu)
+{
+	struct vcpu_register_runstate_memory_area area;
+
+	area.addr.v = &per_cpu(xen_runstate, cpu);
+
+	if (HYPERVISOR_vcpu_op(VCPUOP_register_runstate_memory_area,
+			       cpu, &area))
+		BUG();
+}
+
diff --git a/drivers/xen/tmem.c b/drivers/xen/tmem.c
index 945fc4327201..4ac2ca8a7656 100644
--- a/drivers/xen/tmem.c
+++ b/drivers/xen/tmem.c
@@ -242,7 +242,7 @@ static int tmem_cleancache_init_shared_fs(char *uuid, size_t pagesize)
 	return xen_tmem_new_pool(shared_uuid, TMEM_POOL_SHARED, pagesize);
 }
 
-static struct cleancache_ops tmem_cleancache_ops = {
+static const struct cleancache_ops tmem_cleancache_ops = {
 	.put_page = tmem_cleancache_put_page,
 	.get_page = tmem_cleancache_get_page,
 	.invalidate_page = tmem_cleancache_flush_page,
diff --git a/drivers/xen/xen-acpi-cpuhotplug.c b/drivers/xen/xen-acpi-cpuhotplug.c
index f4a369429553..fdc9e67b842d 100644
--- a/drivers/xen/xen-acpi-cpuhotplug.c
+++ b/drivers/xen/xen-acpi-cpuhotplug.c
@@ -206,7 +206,7 @@ static int xen_hotadd_cpu(struct acpi_processor *pr)
 	op.u.cpu_add.acpi_id = pr->acpi_id;
 	op.u.cpu_add.pxm = pxm;
 
-	cpu_id = HYPERVISOR_dom0_op(&op);
+	cpu_id = HYPERVISOR_platform_op(&op);
 	if (cpu_id < 0)
 		pr_err(PREFIX "Failed to hotadd CPU for acpi_id %d\n",
 				pr->acpi_id);
diff --git a/drivers/xen/xen-acpi-pad.c b/drivers/xen/xen-acpi-pad.c
index f83b754505f8..23d1808fe027 100644
--- a/drivers/xen/xen-acpi-pad.c
+++ b/drivers/xen/xen-acpi-pad.c
@@ -36,7 +36,7 @@ static int xen_acpi_pad_idle_cpus(unsigned int idle_nums)
 	op.u.core_parking.type = XEN_CORE_PARKING_SET;
 	op.u.core_parking.idle_nums = idle_nums;
 
-	return HYPERVISOR_dom0_op(&op);
+	return HYPERVISOR_platform_op(&op);
 }
 
 static int xen_acpi_pad_idle_cpus_num(void)
@@ -46,7 +46,7 @@ static int xen_acpi_pad_idle_cpus_num(void)
 	op.cmd = XENPF_core_parking;
 	op.u.core_parking.type = XEN_CORE_PARKING_GET;
 
-	return HYPERVISOR_dom0_op(&op)
+	return HYPERVISOR_platform_op(&op)
 	       ?: op.u.core_parking.idle_nums;
 }
 
diff --git a/drivers/xen/xen-acpi-processor.c b/drivers/xen/xen-acpi-processor.c
index 70fa438000af..076970a54f89 100644
--- a/drivers/xen/xen-acpi-processor.c
+++ b/drivers/xen/xen-acpi-processor.c
@@ -116,7 +116,7 @@ static int push_cxx_to_hypervisor(struct acpi_processor *_pr)
 	set_xen_guest_handle(op.u.set_pminfo.power.states, dst_cx_states);
 
 	if (!no_hypercall)
-		ret = HYPERVISOR_dom0_op(&op);
+		ret = HYPERVISOR_platform_op(&op);
 
 	if (!ret) {
 		pr_debug("ACPI CPU%u - C-states uploaded.\n", _pr->acpi_id);
@@ -244,7 +244,7 @@ static int push_pxx_to_hypervisor(struct acpi_processor *_pr)
 	}
 
 	if (!no_hypercall)
-		ret = HYPERVISOR_dom0_op(&op);
+		ret = HYPERVISOR_platform_op(&op);
 
 	if (!ret) {
 		struct acpi_processor_performance *perf;
@@ -302,7 +302,7 @@ static unsigned int __init get_max_acpi_id(void)
 	info = &op.u.pcpu_info;
 	info->xen_cpuid = 0;
 
-	ret = HYPERVISOR_dom0_op(&op);
+	ret = HYPERVISOR_platform_op(&op);
 	if (ret)
 		return NR_CPUS;
 
@@ -310,7 +310,7 @@ static unsigned int __init get_max_acpi_id(void)
 	last_cpu = op.u.pcpu_info.max_present;
 	for (i = 0; i <= last_cpu; i++) {
 		info->xen_cpuid = i;
-		ret = HYPERVISOR_dom0_op(&op);
+		ret = HYPERVISOR_platform_op(&op);
 		if (ret)
 			continue;
 		max_acpi_id = max(info->acpi_id, max_acpi_id);
diff --git a/drivers/xen/xen-pciback/pciback_ops.c b/drivers/xen/xen-pciback/pciback_ops.c
index 73dafdc494aa..fb0221434f81 100644
--- a/drivers/xen/xen-pciback/pciback_ops.c
+++ b/drivers/xen/xen-pciback/pciback_ops.c
@@ -227,8 +227,9 @@ int xen_pcibk_enable_msix(struct xen_pcibk_device *pdev,
 	/*
 	 * PCI_COMMAND_MEMORY must be enabled, otherwise we may not be able
 	 * to access the BARs where the MSI-X entries reside.
+	 * But VF devices are unique in which the PF needs to be checked.
 	 */
-	pci_read_config_word(dev, PCI_COMMAND, &cmd);
+	pci_read_config_word(pci_physfn(dev), PCI_COMMAND, &cmd);
 	if (dev->msi_enabled || !(cmd & PCI_COMMAND_MEMORY))
 		return -ENXIO;
 
@@ -332,6 +333,9 @@ void xen_pcibk_do_op(struct work_struct *data)
 	struct xen_pcibk_dev_data *dev_data = NULL;
 	struct xen_pci_op *op = &pdev->op;
 	int test_intx = 0;
+#ifdef CONFIG_PCI_MSI
+	unsigned int nr = 0;
+#endif
 
 	*op = pdev->sh_info->op;
 	barrier();
@@ -360,6 +364,7 @@ void xen_pcibk_do_op(struct work_struct *data)
 			op->err = xen_pcibk_disable_msi(pdev, dev, op);
 			break;
 		case XEN_PCI_OP_enable_msix:
+			nr = op->value;
 			op->err = xen_pcibk_enable_msix(pdev, dev, op);
 			break;
 		case XEN_PCI_OP_disable_msix:
@@ -382,7 +387,7 @@ void xen_pcibk_do_op(struct work_struct *data)
 	if (op->cmd == XEN_PCI_OP_enable_msix && op->err == 0) {
 		unsigned int i;
 
-		for (i = 0; i < op->value; i++)
+		for (i = 0; i < nr; i++)
 			pdev->sh_info->op.msix_entries[i].vector =
 				op->msix_entries[i].vector;
 	}
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
index ad4eb1024d1f..c46ee189466f 100644
--- a/drivers/xen/xen-scsiback.c
+++ b/drivers/xen/xen-scsiback.c
@@ -849,15 +849,31 @@ static int scsiback_map(struct vscsibk_info *info)
 }
 
 /*
+  Check for a translation entry being present
+*/
+static struct v2p_entry *scsiback_chk_translation_entry(
+	struct vscsibk_info *info, struct ids_tuple *v)
+{
+	struct list_head *head = &(info->v2p_entry_lists);
+	struct v2p_entry *entry;
+
+	list_for_each_entry(entry, head, l)
+		if ((entry->v.chn == v->chn) &&
+		    (entry->v.tgt == v->tgt) &&
+		    (entry->v.lun == v->lun))
+			return entry;
+
+	return NULL;
+}
+
+/*
   Add a new translation entry
 */
 static int scsiback_add_translation_entry(struct vscsibk_info *info,
 					  char *phy, struct ids_tuple *v)
 {
 	int err = 0;
-	struct v2p_entry *entry;
 	struct v2p_entry *new;
-	struct list_head *head = &(info->v2p_entry_lists);
 	unsigned long flags;
 	char *lunp;
 	unsigned long long unpacked_lun;
@@ -917,15 +933,10 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	spin_lock_irqsave(&info->v2p_lock, flags);
 
 	/* Check double assignment to identical virtual ID */
-	list_for_each_entry(entry, head, l) {
-		if ((entry->v.chn == v->chn) &&
-		    (entry->v.tgt == v->tgt) &&
-		    (entry->v.lun == v->lun)) {
-			pr_warn("Virtual ID is already used. Assignment was not performed.\n");
-			err = -EEXIST;
-			goto out;
-		}
-
+	if (scsiback_chk_translation_entry(info, v)) {
+		pr_warn("Virtual ID is already used. Assignment was not performed.\n");
+		err = -EEXIST;
+		goto out;
 	}
 
 	/* Create a new translation entry and add to the list */
@@ -933,18 +944,18 @@ static int scsiback_add_translation_entry(struct vscsibk_info *info,
 	new->v = *v;
 	new->tpg = tpg;
 	new->lun = unpacked_lun;
-	list_add_tail(&new->l, head);
+	list_add_tail(&new->l, &info->v2p_entry_lists);
 
 out:
 	spin_unlock_irqrestore(&info->v2p_lock, flags);
 
 out_free:
-	mutex_lock(&tpg->tv_tpg_mutex);
-	tpg->tv_tpg_fe_count--;
-	mutex_unlock(&tpg->tv_tpg_mutex);
-
-	if (err)
+	if (err) {
+		mutex_lock(&tpg->tv_tpg_mutex);
+		tpg->tv_tpg_fe_count--;
+		mutex_unlock(&tpg->tv_tpg_mutex);
 		kfree(new);
+	}
 
 	return err;
 }
@@ -956,39 +967,40 @@ static void __scsiback_del_translation_entry(struct v2p_entry *entry)
 }
 
 /*
-  Delete the translation entry specfied
+  Delete the translation entry specified
 */
 static int scsiback_del_translation_entry(struct vscsibk_info *info,
 					  struct ids_tuple *v)
 {
 	struct v2p_entry *entry;
-	struct list_head *head = &(info->v2p_entry_lists);
 	unsigned long flags;
+	int ret = 0;
 
 	spin_lock_irqsave(&info->v2p_lock, flags);
 	/* Find out the translation entry specified */
-	list_for_each_entry(entry, head, l) {
-		if ((entry->v.chn == v->chn) &&
-		    (entry->v.tgt == v->tgt) &&
-		    (entry->v.lun == v->lun)) {
-			goto found;
-		}
-	}
-
-	spin_unlock_irqrestore(&info->v2p_lock, flags);
-	return 1;
-
-found:
-	/* Delete the translation entry specfied */
-	__scsiback_del_translation_entry(entry);
+	entry = scsiback_chk_translation_entry(info, v);
+	if (entry)
+		__scsiback_del_translation_entry(entry);
+	else
+		ret = -ENOENT;
 
 	spin_unlock_irqrestore(&info->v2p_lock, flags);
-	return 0;
+	return ret;
 }
 
 static void scsiback_do_add_lun(struct vscsibk_info *info, const char *state,
 				char *phy, struct ids_tuple *vir, int try)
 {
+	struct v2p_entry *entry;
+	unsigned long flags;
+
+	if (try) {
+		spin_lock_irqsave(&info->v2p_lock, flags);
+		entry = scsiback_chk_translation_entry(info, vir);
+		spin_unlock_irqrestore(&info->v2p_lock, flags);
+		if (entry)
+			return;
+	}
 	if (!scsiback_add_translation_entry(info, phy, vir)) {
 		if (xenbus_printf(XBT_NIL, info->dev->nodename, state,
 				  "%d", XenbusStateInitialised)) {
diff --git a/drivers/xen/xenbus/xenbus_comms.c b/drivers/xen/xenbus/xenbus_comms.c
index fdb0f339d0a7..ecdecce80a6c 100644
--- a/drivers/xen/xenbus/xenbus_comms.c
+++ b/drivers/xen/xenbus/xenbus_comms.c
@@ -123,14 +123,14 @@ int xb_write(const void *data, unsigned len)
 			avail = len;
 
 		/* Must write data /after/ reading the consumer index. */
-		mb();
+		virt_mb();
 
 		memcpy(dst, data, avail);
 		data += avail;
 		len -= avail;
 
 		/* Other side must not see new producer until data is there. */
-		wmb();
+		virt_wmb();
 		intf->req_prod += avail;
 
 		/* Implies mb(): other side will see the updated producer. */
@@ -180,14 +180,14 @@ int xb_read(void *data, unsigned len)
 			avail = len;
 
 		/* Must read data /after/ reading the producer index. */
-		rmb();
+		virt_rmb();
 
 		memcpy(data, src, avail);
 		data += avail;
 		len -= avail;
 
 		/* Other side must not see free space until we've copied out */
-		mb();
+		virt_mb();
 		intf->rsp_cons += avail;
 
 		pr_debug("Finished read of %i bytes (%i to go)\n", avail, len);
diff --git a/drivers/xen/xenbus/xenbus_dev_frontend.c b/drivers/xen/xenbus/xenbus_dev_frontend.c
index 9433e46518c8..912b64edb42b 100644
--- a/drivers/xen/xenbus/xenbus_dev_frontend.c
+++ b/drivers/xen/xenbus/xenbus_dev_frontend.c
@@ -188,6 +188,8 @@ static int queue_reply(struct list_head *queue, const void *data, size_t len)
 
 	if (len == 0)
 		return 0;
+	if (len > XENSTORE_PAYLOAD_MAX)
+		return -EINVAL;
 
 	rb = kmalloc(sizeof(*rb) + len, GFP_KERNEL);
 	if (rb == NULL)
diff --git a/drivers/xen/xenfs/xensyms.c b/drivers/xen/xenfs/xensyms.c
index f8b12856753f..a03f261b12d8 100644
--- a/drivers/xen/xenfs/xensyms.c
+++ b/drivers/xen/xenfs/xensyms.c
@@ -31,7 +31,7 @@ static int xensyms_next_sym(struct xensyms *xs)
 
 	symnum = symdata->symnum;
 
-	ret = HYPERVISOR_dom0_op(&xs->op);
+	ret = HYPERVISOR_platform_op(&xs->op);
 	if (ret < 0)
 		return ret;
 
@@ -50,7 +50,7 @@ static int xensyms_next_sym(struct xensyms *xs)
 		set_xen_guest_handle(symdata->name, xs->name);
 		symdata->symnum--; /* Rewind */
 
-		ret = HYPERVISOR_dom0_op(&xs->op);
+		ret = HYPERVISOR_platform_op(&xs->op);
 		if (ret < 0)
 			return ret;
 	}